4264 files changed, 206837 insertions, 151044 deletions

diff --git a/.mailmap b/.mailmap
index 658003aa944..df1baba43a6 100644
--- a/.mailmap
+++ b/.mailmap
@@ -99,6 +99,7 @@ Sachin P Sant <ssant@in.ibm.com>
 Sam Ravnborg <sam@mars.ravnborg.org>
 Sascha Hauer <s.hauer@pengutronix.de>
 S.Çağlar Onur <caglar@pardus.org.tr>
+Shiraz Hashim <shiraz.linux.kernel@gmail.com> <shiraz.hashim@st.com>
 Simon Kelley <simon@thekelleys.org.uk>
 Stéphane Witzmann <stephane.witzmann@ubpmes.univ-bpclermont.fr>
 Stephen Hemminger <shemminger@osdl.org>
diff --git a/Documentation/ABI/testing/configfs-usb-gadget b/Documentation/ABI/testing/configfs-usb-gadget
index 37559a06393..95a36589a66 100644
--- a/Documentation/ABI/testing/configfs-usb-gadget
+++ b/Documentation/ABI/testing/configfs-usb-gadget
@@ -62,6 +62,40 @@ KernelVersion:	3.11
 Description:
 		This group contains functions available to this USB gadget.
 
+What:		/config/usb-gadget/gadget/functions/<func>.<inst>/interface.<n>
+Date:		May 2014
+KernelVersion:	3.16
+Description:
+		This group contains "Feature Descriptors" specific for one
+		gadget's USB interface or one interface group described
+		by an IAD.
+
+		The attributes:
+
+		compatible_id		- 8-byte string for "Compatible ID"
+		sub_compatible_id	- 8-byte string for "Sub Compatible ID"
+
+What:		/config/usb-gadget/gadget/functions/<func>.<inst>/interface.<n>/<property>
+Date:		May 2014
+KernelVersion:	3.16
+Description:
+		This group contains "Extended Property Descriptors" specific for one
+		gadget's USB interface or one interface group described
+		by an IAD.
+
+		The attributes:
+
+		type		- value 1..7 for interpreting the data
+				1: unicode string
+				2: unicode string with environment variable
+				3: binary
+				4: little-endian 32-bit
+				5: big-endian 32-bit
+				6: unicode string with a symbolic link
+				7: multiple unicode strings
+		data		- blob of data to be interpreted depending on
+				type
+
 What:		/config/usb-gadget/gadget/strings
 Date:		Jun 2013
 KernelVersion:	3.11
@@ -79,3 +113,14 @@ Description:
 		product		- gadget's product description
 		manufacturer	- gadget's manufacturer description
 
+What:		/config/usb-gadget/gadget/os_desc
+Date:		May 2014
+KernelVersion:	3.16
+Description:
+		This group contains "OS String" extension handling attributes.
+
+		use		- flag turning "OS Desctiptors" support on/off
+		b_vendor_code	- one-byte value used for custom per-device and
+				per-interface requests
+		qw_sign		- an identifier to be reported as "OS String"
+				proper
diff --git a/Documentation/ABI/testing/sysfs-bus-iio b/Documentation/ABI/testing/sysfs-bus-iio
index 6e02c502915..a9757dcf2e8 100644
--- a/Documentation/ABI/testing/sysfs-bus-iio
+++ b/Documentation/ABI/testing/sysfs-bus-iio
@@ -114,14 +114,17 @@ What:		/sys/bus/iio/devices/iio:deviceX/in_temp_raw
 What:		/sys/bus/iio/devices/iio:deviceX/in_tempX_raw
 What:		/sys/bus/iio/devices/iio:deviceX/in_temp_x_raw
 What:		/sys/bus/iio/devices/iio:deviceX/in_temp_y_raw
-What:		/sys/bus/iio/devices/iio:deviceX/in_temp_z_raw
+What:		/sys/bus/iio/devices/iio:deviceX/in_temp_ambient_raw
+What:		/sys/bus/iio/devices/iio:deviceX/in_temp_object_raw
 KernelVersion:	2.6.35
 Contact:	linux-iio@vger.kernel.org
 Description:
 		Raw (unscaled no bias removal etc.) temperature measurement.
 		If an axis is specified it generally means that the temperature
 		sensor is associated with one part of a compound device (e.g.
-		a gyroscope axis). Units after application of scale and offset
+		a gyroscope axis). The ambient and object modifiers distinguish
+		between ambient (reference) and distant temperature for contact-
+		less measurements. Units after application of scale and offset
 		are milli degrees Celsius.
 
 What:		/sys/bus/iio/devices/iio:deviceX/in_tempX_input
@@ -210,6 +213,14 @@ Contact:	linux-iio@vger.kernel.org
 Description:
 		Scaled humidity measurement in milli percent.
 
+What:		/sys/bus/iio/devices/iio:deviceX/in_X_mean_raw
+KernelVersion:	3.5
+Contact:	linux-iio@vger.kernel.org
+Description:
+		Averaged raw measurement from channel X. The number of values
+		used for averaging is device specific. The converting rules for
+		normal raw values also applies to the averaged raw values.
+
 What:		/sys/bus/iio/devices/iio:deviceX/in_accel_offset
 What:		/sys/bus/iio/devices/iio:deviceX/in_accel_x_offset
 What:		/sys/bus/iio/devices/iio:deviceX/in_accel_y_offset
@@ -784,6 +795,7 @@ What:		/sys/.../iio:deviceX/scan_elements/in_incli_x_en
 What:		/sys/.../iio:deviceX/scan_elements/in_incli_y_en
 What:		/sys/.../iio:deviceX/scan_elements/in_pressureY_en
 What:		/sys/.../iio:deviceX/scan_elements/in_pressure_en
+What:		/sys/.../iio:deviceX/scan_elements/in_rot_quaternion_en
 KernelVersion:	2.6.37
 Contact:	linux-iio@vger.kernel.org
 Description:
@@ -799,6 +811,7 @@ What:		/sys/.../iio:deviceX/scan_elements/in_voltageY_supply_type
 What:		/sys/.../iio:deviceX/scan_elements/in_timestamp_type
 What:		/sys/.../iio:deviceX/scan_elements/in_pressureY_type
 What:		/sys/.../iio:deviceX/scan_elements/in_pressure_type
+What:		/sys/.../iio:deviceX/scan_elements/in_rot_quaternion_type
 KernelVersion:	2.6.37
 Contact:	linux-iio@vger.kernel.org
 Description:
@@ -845,6 +858,7 @@ What:		/sys/.../iio:deviceX/scan_elements/in_incli_y_index
 What:		/sys/.../iio:deviceX/scan_elements/in_timestamp_index
 What:		/sys/.../iio:deviceX/scan_elements/in_pressureY_index
 What:		/sys/.../iio:deviceX/scan_elements/in_pressure_index
+What:		/sys/.../iio:deviceX/scan_elements/in_rot_quaternion_index
 KernelVersion:	2.6.37
 Contact:	linux-iio@vger.kernel.org
 Description:
@@ -881,6 +895,25 @@ Description:
 		on-chip EEPROM. After power-up or chip reset the device will
 		automatically load the saved configuration.
 
+What:		/sys/.../iio:deviceX/in_illuminanceY_input
+What:		/sys/.../iio:deviceX/in_illuminanceY_raw
+What:		/sys/.../iio:deviceX/in_illuminanceY_mean_raw
+KernelVersion:	3.4
+Contact:	linux-iio@vger.kernel.org
+Description:
+		Illuminance measurement, units after application of scale
+		and offset are lux.
+
+What:		/sys/.../iio:deviceX/in_intensityY_raw
+What:		/sys/.../iio:deviceX/in_intensityY_ir_raw
+What:		/sys/.../iio:deviceX/in_intensityY_both_raw
+KernelVersion:	3.4
+Contact:	linux-iio@vger.kernel.org
+Description:
+		Unit-less light intensity. Modifiers both and ir indicate
+		that measurements contains visible and infrared light
+		components or just infrared light, respectively.
+
 What:		/sys/.../iio:deviceX/in_intensity_red_integration_time
 What:		/sys/.../iio:deviceX/in_intensity_green_integration_time
 What:		/sys/.../iio:deviceX/in_intensity_blue_integration_time
@@ -891,3 +924,12 @@ Contact:	linux-iio@vger.kernel.org
 Description:
 		This attribute is used to get/set the integration time in
 		seconds.
+
+What:		/sys/bus/iio/devices/iio:deviceX/in_rot_quaternion_raw
+KernelVersion:	3.15
+Contact:	linux-iio@vger.kernel.org
+Description:
+		Raw value of quaternion components using a format
+		x y z w. Here x, y, and z component represents the axis about
+		which a rotation will occur and w component represents the
+		amount of rotation.
diff --git a/Documentation/ABI/testing/sysfs-bus-iio-proximity-as3935 b/Documentation/ABI/testing/sysfs-bus-iio-proximity-as3935
new file mode 100644
index 00000000000..6708c5e264a
--- /dev/null
+++ b/Documentation/ABI/testing/sysfs-bus-iio-proximity-as3935
@@ -0,0 +1,16 @@
+What		/sys/bus/iio/devices/iio:deviceX/in_proximity_raw
+Date:		March 2014
+KernelVersion:	3.15
+Contact:	Matt Ranostay <mranostay@gmail.com>
+Description:
+		Get the current distance in meters of storm (1km steps)
+		1000-40000 = distance in meters
+
+What		/sys/bus/iio/devices/iio:deviceX/sensor_sensitivity
+Date:		March 2014
+KernelVersion:	3.15
+Contact:	Matt Ranostay <mranostay@gmail.com>
+Description:
+		Show or set the gain boost of the amp, from 0-31 range.
+		18 = indoors (default)
+		14 = outdoors
diff --git a/Documentation/ABI/testing/sysfs-bus-pci b/Documentation/ABI/testing/sysfs-bus-pci
index a3c5a668503..6615fda0abf 100644
--- a/Documentation/ABI/testing/sysfs-bus-pci
+++ b/Documentation/ABI/testing/sysfs-bus-pci
@@ -117,7 +117,7 @@ Description:
 
 What:		/sys/bus/pci/devices/.../vpd
 Date:		February 2008
-Contact:	Ben Hutchings <bhutchings@solarflare.com>
+Contact:	Ben Hutchings <bwh@kernel.org>
 Description:
 		A file named vpd in a device directory will be a
 		binary file containing the Vital Product Data for the
@@ -250,3 +250,24 @@ Description:
 		valid.  For example, writing a 2 to this file when sriov_numvfs
 		is not 0 and not 2 already will return an error. Writing a 10
 		when the value of sriov_totalvfs is 8 will return an error.
+
+What:		/sys/bus/pci/devices/.../driver_override
+Date:		April 2014
+Contact:	Alex Williamson <alex.williamson@redhat.com>
+Description:
+		This file allows the driver for a device to be specified which
+		will override standard static and dynamic ID matching.  When
+		specified, only a driver with a name matching the value written
+		to driver_override will have an opportunity to bind to the
+		device.  The override is specified by writing a string to the
+		driver_override file (echo pci-stub > driver_override) and
+		may be cleared with an empty string (echo > driver_override).
+		This returns the device to standard matching rules binding.
+		Writing to driver_override does not automatically unbind the
+		device from its current driver or make any attempt to
+		automatically load the specified driver.  If no driver with a
+		matching name is currently loaded in the kernel, the device
+		will not bind to any driver.  This also allows devices to
+		opt-out of driver binding using a driver_override name such as
+		"none".  Only a single driver may be specified in the override,
+		there is no support for parsing delimiters.
diff --git a/Documentation/ABI/testing/sysfs-platform-brcmstb-gisb-arb b/Documentation/ABI/testing/sysfs-platform-brcmstb-gisb-arb
new file mode 100644
index 00000000000..f1bad92bbe2
--- /dev/null
+++ b/Documentation/ABI/testing/sysfs-platform-brcmstb-gisb-arb
@@ -0,0 +1,8 @@
+What:		/sys/devices/../../gisb_arb_timeout
+Date:		May 2014
+KernelVersion:	3.17
+Contact:	Florian Fainelli <f.fainelli@gmail.com>
+Description:
+		Returns the currently configured raw timeout value of the
+		Broadcom Set Top Box internal GISB bus arbiter. Minimum value
+		is 1, and maximum value is 0xffffffff.
diff --git a/Documentation/ABI/testing/sysfs-platform-chipidea-usb-otg b/Documentation/ABI/testing/sysfs-platform-chipidea-usb-otg
new file mode 100644
index 00000000000..151c59578db
--- /dev/null
+++ b/Documentation/ABI/testing/sysfs-platform-chipidea-usb-otg
@@ -0,0 +1,56 @@
+What:		/sys/bus/platform/devices/ci_hdrc.0/inputs/a_bus_req
+Date:		Feb 2014
+Contact:	Li Jun <b47624@freescale.com>
+Description:
+		Can be set and read.
+		Set a_bus_req(A-device bus request) input to be 1 if
+		the application running on the A-device wants to use the bus,
+		and to be 0 when the application no longer wants to use
+		the bus(or wants to work as peripheral). a_bus_req can also
+		be set to 1 by kernel in response to remote wakeup signaling
+		from the B-device, the A-device should decide to resume the bus.
+
+		Valid values are "1" and "0".
+
+		Reading: returns 1 if the application running on the A-device
+		is using the bus as host role, otherwise 0.
+
+What:		/sys/bus/platform/devices/ci_hdrc.0/inputs/a_bus_drop
+Date:		Feb 2014
+Contact:	Li Jun <b47624@freescale.com>
+Description:
+		Can be set and read
+		The a_bus_drop(A-device bus drop) input is 1 when the
+		application running on the A-device wants to power down
+		the bus, and is 0 otherwise, When a_bus_drop is 1, then
+		the a_bus_req shall be 0.
+
+		Valid values are "1" and "0".
+
+		Reading: returns 1 if the bus is off(vbus is turned off) by
+			 A-device, otherwise 0.
+
+What:		/sys/bus/platform/devices/ci_hdrc.0/inputs/b_bus_req
+Date:		Feb 2014
+Contact:	Li Jun <b47624@freescale.com>
+Description:
+		Can be set and read.
+		The b_bus_req(B-device bus request) input is 1 during the time
+		that the application running on the B-device wants to use the
+		bus as host, and is 0 when the application no longer wants to
+		work as host and decides to switch back to be peripheral.
+
+		Valid values are "1" and "0".
+
+		Reading: returns if the application running on the B device
+		is using the bus as host role, otherwise 0.
+
+What:		/sys/bus/platform/devices/ci_hdrc.0/inputs/a_clr_err
+Date:		Feb 2014
+Contact:	Li Jun <b47624@freescale.com>
+Description:
+		Only can be set.
+		The a_clr_err(A-device Vbus error clear) input is used to clear
+		vbus error, then A-device will power down the bus.
+
+		Valid value is "1"
diff --git a/Documentation/DMA-API-HOWTO.txt b/Documentation/DMA-API-HOWTO.txt
index 5e983031cc1..dcbbe3602d7 100644
--- a/Documentation/DMA-API-HOWTO.txt
+++ b/Documentation/DMA-API-HOWTO.txt
@@ -9,16 +9,76 @@ This is a guide to device driver writers on how to use the DMA API
 with example pseudo-code.  For a concise description of the API, see
 DMA-API.txt.
 
-Most of the 64bit platforms have special hardware that translates bus
-addresses (DMA addresses) into physical addresses.  This is similar to
-how page tables and/or a TLB translates virtual addresses to physical
-addresses on a CPU.  This is needed so that e.g. PCI devices can
-access with a Single Address Cycle (32bit DMA address) any page in the
-64bit physical address space.  Previously in Linux those 64bit
-platforms had to set artificial limits on the maximum RAM size in the
-system, so that the virt_to_bus() static scheme works (the DMA address
-translation tables were simply filled on bootup to map each bus
-address to the physical page __pa(bus_to_virt())).
+                       CPU and DMA addresses
+
+There are several kinds of addresses involved in the DMA API, and it's
+important to understand the differences.
+
+The kernel normally uses virtual addresses.  Any address returned by
+kmalloc(), vmalloc(), and similar interfaces is a virtual address and can
+be stored in a "void *".
+
+The virtual memory system (TLB, page tables, etc.) translates virtual
+addresses to CPU physical addresses, which are stored as "phys_addr_t" or
+"resource_size_t".  The kernel manages device resources like registers as
+physical addresses.  These are the addresses in /proc/iomem.  The physical
+address is not directly useful to a driver; it must use ioremap() to map
+the space and produce a virtual address.
+
+I/O devices use a third kind of address: a "bus address" or "DMA address".
+If a device has registers at an MMIO address, or if it performs DMA to read
+or write system memory, the addresses used by the device are bus addresses.
+In some systems, bus addresses are identical to CPU physical addresses, but
+in general they are not.  IOMMUs and host bridges can produce arbitrary
+mappings between physical and bus addresses.
+
+Here's a picture and some examples:
+
+               CPU                  CPU                  Bus
+             Virtual              Physical             Address
+             Address              Address               Space
+              Space                Space
+
+            +-------+             +------+             +------+
+            |       |             |MMIO  |   Offset    |      |
+            |       |  Virtual    |Space |   applied   |      |
+          C +-------+ --------> B +------+ ----------> +------+ A
+            |       |  mapping    |      |   by host   |      |
+  +-----+   |       |             |      |   bridge    |      |   +--------+
+  |     |   |       |             +------+             |      |   |        |
+  | CPU |   |       |             | RAM  |             |      |   | Device |
+  |     |   |       |             |      |             |      |   |        |
+  +-----+   +-------+             +------+             +------+   +--------+
+            |       |  Virtual    |Buffer|   Mapping   |      |
+          X +-------+ --------> Y +------+ <---------- +------+ Z
+            |       |  mapping    | RAM  |   by IOMMU
+            |       |             |      |
+            |       |             |      |
+            +-------+             +------+
+
+During the enumeration process, the kernel learns about I/O devices and
+their MMIO space and the host bridges that connect them to the system.  For
+example, if a PCI device has a BAR, the kernel reads the bus address (A)
+from the BAR and converts it to a CPU physical address (B).  The address B
+is stored in a struct resource and usually exposed via /proc/iomem.  When a
+driver claims a device, it typically uses ioremap() to map physical address
+B at a virtual address (C).  It can then use, e.g., ioread32(C), to access
+the device registers at bus address A.
+
+If the device supports DMA, the driver sets up a buffer using kmalloc() or
+a similar interface, which returns a virtual address (X).  The virtual
+memory system maps X to a physical address (Y) in system RAM.  The driver
+can use virtual address X to access the buffer, but the device itself
+cannot because DMA doesn't go through the CPU virtual memory system.
+
+In some simple systems, the device can do DMA directly to physical address
+Y.  But in many others, there is IOMMU hardware that translates bus
+addresses to physical addresses, e.g., it translates Z to Y.  This is part
+of the reason for the DMA API: the driver can give a virtual address X to
+an interface like dma_map_single(), which sets up any required IOMMU
+mapping and returns the bus address Z.  The driver then tells the device to
+do DMA to Z, and the IOMMU maps it to the buffer at address Y in system
+RAM.
 
 So that Linux can use the dynamic DMA mapping, it needs some help from the
 drivers, namely it has to take into account that DMA addresses should be
@@ -29,17 +89,17 @@ The following API will work of course even on platforms where no such
 hardware exists.
 
 Note that the DMA API works with any bus independent of the underlying
-microprocessor architecture. You should use the DMA API rather than
-the bus specific DMA API (e.g. pci_dma_*).
+microprocessor architecture. You should use the DMA API rather than the
+bus-specific DMA API, i.e., use the dma_map_*() interfaces rather than the
+pci_map_*() interfaces.
 
 First of all, you should make sure
 
 #include <linux/dma-mapping.h>
 
-is in your driver. This file will obtain for you the definition of the
-dma_addr_t (which can hold any valid DMA address for the platform)
-type which should be used everywhere you hold a DMA (bus) address
-returned from the DMA mapping functions.
+is in your driver, which provides the definition of dma_addr_t.  This type
+can hold any valid DMA or bus address for the platform and should be used
+everywhere you hold a DMA address returned from the DMA mapping functions.
 
 			 What memory is DMA'able?
 
@@ -123,9 +183,9 @@ Here, dev is a pointer to the device struct of your device, and mask
 is a bit mask describing which bits of an address your device
 supports.  It returns zero if your card can perform DMA properly on
 the machine given the address mask you provided.  In general, the
-device struct of your device is embedded in the bus specific device
-struct of your device.  For example, a pointer to the device struct of
-your PCI device is pdev->dev (pdev is a pointer to the PCI device
+device struct of your device is embedded in the bus-specific device
+struct of your device.  For example, &pdev->dev is a pointer to the
+device struct of a PCI device (pdev is a pointer to the PCI device
 struct of your device).
 
 If it returns non-zero, your device cannot perform DMA properly on
@@ -147,8 +207,7 @@ exactly why.
 The standard 32-bit addressing device would do something like this:
 
 	if (dma_set_mask_and_coherent(dev, DMA_BIT_MASK(32))) {
-		printk(KERN_WARNING
-		       "mydev: No suitable DMA available.\n");
+		dev_warn(dev, "mydev: No suitable DMA available\n");
 		goto ignore_this_device;
 	}
 
@@ -170,8 +229,7 @@ all 64-bits when accessing streaming DMA:
 	} else if (!dma_set_mask(dev, DMA_BIT_MASK(32))) {
 		using_dac = 0;
 	} else {
-		printk(KERN_WARNING
-		       "mydev: No suitable DMA available.\n");
+		dev_warn(dev, "mydev: No suitable DMA available\n");
 		goto ignore_this_device;
 	}
 
@@ -187,22 +245,20 @@ the case would look like this:
 		using_dac = 0;
 		consistent_using_dac = 0;
 	} else {
-		printk(KERN_WARNING
-		       "mydev: No suitable DMA available.\n");
+		dev_warn(dev, "mydev: No suitable DMA available\n");
 		goto ignore_this_device;
 	}
 
-The coherent coherent mask will always be able to set the same or a
-smaller mask as the streaming mask. However for the rare case that a
-device driver only uses consistent allocations, one would have to
-check the return value from dma_set_coherent_mask().
+The coherent mask will always be able to set the same or a smaller mask as
+the streaming mask. However for the rare case that a device driver only
+uses consistent allocations, one would have to check the return value from
+dma_set_coherent_mask().
 
 Finally, if your device can only drive the low 24-bits of
 address you might do something like:
 
 	if (dma_set_mask(dev, DMA_BIT_MASK(24))) {
-		printk(KERN_WARNING
-		       "mydev: 24-bit DMA addressing not available.\n");
+		dev_warn(dev, "mydev: 24-bit DMA addressing not available\n");
 		goto ignore_this_device;
 	}
 
@@ -232,14 +288,14 @@ Here is pseudo-code showing how this might be done:
 		card->playback_enabled = 1;
 	} else {
 		card->playback_enabled = 0;
-		printk(KERN_WARNING "%s: Playback disabled due to DMA limitations.\n",
+		dev_warn(dev, "%s: Playback disabled due to DMA limitations\n",
 		       card->name);
 	}
 	if (!dma_set_mask(dev, RECORD_ADDRESS_BITS)) {
 		card->record_enabled = 1;
 	} else {
 		card->record_enabled = 0;
-		printk(KERN_WARNING "%s: Record disabled due to DMA limitations.\n",
+		dev_warn(dev, "%s: Record disabled due to DMA limitations\n",
 		       card->name);
 	}
 
@@ -331,7 +387,7 @@ context with the GFP_ATOMIC flag.
 Size is the length of the region you want to allocate, in bytes.
 
 This routine will allocate RAM for that region, so it acts similarly to
-__get_free_pages (but takes size instead of a page order).  If your
+__get_free_pages() (but takes size instead of a page order).  If your
 driver needs regions sized smaller than a page, you may prefer using
 the dma_pool interface, described below.
 
@@ -343,11 +399,11 @@ the consistent DMA mask has been explicitly changed via
 dma_set_coherent_mask().  This is true of the dma_pool interface as
 well.
 
-dma_alloc_coherent returns two values: the virtual address which you
+dma_alloc_coherent() returns two values: the virtual address which you
 can use to access it from the CPU and dma_handle which you pass to the
 card.
 
-The cpu return address and the DMA bus master address are both
+The CPU virtual address and the DMA bus address are both
 guaranteed to be aligned to the smallest PAGE_SIZE order which
 is greater than or equal to the requested size.  This invariant
 exists (for example) to guarantee that if you allocate a chunk
@@ -359,13 +415,13 @@ To unmap and free such a DMA region, you call:
 	dma_free_coherent(dev, size, cpu_addr, dma_handle);
 
 where dev, size are the same as in the above call and cpu_addr and
-dma_handle are the values dma_alloc_coherent returned to you.
+dma_handle are the values dma_alloc_coherent() returned to you.
 This function may not be called in interrupt context.
 
 If your driver needs lots of smaller memory regions, you can write
-custom code to subdivide pages returned by dma_alloc_coherent,
+custom code to subdivide pages returned by dma_alloc_coherent(),
 or you can use the dma_pool API to do that.  A dma_pool is like
-a kmem_cache, but it uses dma_alloc_coherent not __get_free_pages.
+a kmem_cache, but it uses dma_alloc_coherent(), not __get_free_pages().
 Also, it understands common hardware constraints for alignment,
 like queue heads needing to be aligned on N byte boundaries.
 
@@ -373,37 +429,37 @@ Create a dma_pool like this:
 
 	struct dma_pool *pool;
 
-	pool = dma_pool_create(name, dev, size, align, alloc);
+	pool = dma_pool_create(name, dev, size, align, boundary);
 
 The "name" is for diagnostics (like a kmem_cache name); dev and size
 are as above.  The device's hardware alignment requirement for this
 type of data is "align" (which is expressed in bytes, and must be a
 power of two).  If your device has no boundary crossing restrictions,
-pass 0 for alloc; passing 4096 says memory allocated from this pool
+pass 0 for boundary; passing 4096 says memory allocated from this pool
 must not cross 4KByte boundaries (but at that time it may be better to
-go for dma_alloc_coherent directly instead).
+use dma_alloc_coherent() directly instead).
 
-Allocate memory from a dma pool like this:
+Allocate memory from a DMA pool like this:
 
 	cpu_addr = dma_pool_alloc(pool, flags, &dma_handle);
 
-flags are SLAB_KERNEL if blocking is permitted (not in_interrupt nor
-holding SMP locks), SLAB_ATOMIC otherwise.  Like dma_alloc_coherent,
+flags are GFP_KERNEL if blocking is permitted (not in_interrupt nor
+holding SMP locks), GFP_ATOMIC otherwise.  Like dma_alloc_coherent(),
 this returns two values, cpu_addr and dma_handle.
 
 Free memory that was allocated from a dma_pool like this:
 
 	dma_pool_free(pool, cpu_addr, dma_handle);
 
-where pool is what you passed to dma_pool_alloc, and cpu_addr and
-dma_handle are the values dma_pool_alloc returned. This function
+where pool is what you passed to dma_pool_alloc(), and cpu_addr and
+dma_handle are the values dma_pool_alloc() returned. This function
 may be called in interrupt context.
 
 Destroy a dma_pool by calling:
 
 	dma_pool_destroy(pool);
 
-Make sure you've called dma_pool_free for all memory allocated
+Make sure you've called dma_pool_free() for all memory allocated
 from a pool before you destroy the pool. This function may not
 be called in interrupt context.
 
@@ -418,7 +474,7 @@ one of the following values:
  DMA_FROM_DEVICE
  DMA_NONE
 
-One should provide the exact DMA direction if you know it.
+You should provide the exact DMA direction if you know it.
 
 DMA_TO_DEVICE means "from main memory to the device"
 DMA_FROM_DEVICE means "from the device to main memory"
@@ -489,14 +545,14 @@ and to unmap it:
 	dma_unmap_single(dev, dma_handle, size, direction);
 
 You should call dma_mapping_error() as dma_map_single() could fail and return
-error. Not all dma implementations support dma_mapping_error() interface.
+error. Not all DMA implementations support the dma_mapping_error() interface.
 However, it is a good practice to call dma_mapping_error() interface, which
 will invoke the generic mapping error check interface. Doing so will ensure
-that the mapping code will work correctly on all dma implementations without
+that the mapping code will work correctly on all DMA implementations without
 any dependency on the specifics of the underlying implementation. Using the
 returned address without checking for errors could result in failures ranging
 from panics to silent data corruption. A couple of examples of incorrect ways
-to check for errors that make assumptions about the underlying dma
+to check for errors that make assumptions about the underlying DMA
 implementation are as follows and these are applicable to dma_map_page() as
 well.
 
@@ -516,13 +572,13 @@ Incorrect example 2:
 		goto map_error;
 	}
 
-You should call dma_unmap_single when the DMA activity is finished, e.g.
+You should call dma_unmap_single() when the DMA activity is finished, e.g.,
 from the interrupt which told you that the DMA transfer is done.
 
-Using cpu pointers like this for single mappings has a disadvantage,
+Using CPU pointers like this for single mappings has a disadvantage:
 you cannot reference HIGHMEM memory in this way.  Thus, there is a
-map/unmap interface pair akin to dma_{map,unmap}_single.  These
-interfaces deal with page/offset pairs instead of cpu pointers.
+map/unmap interface pair akin to dma_{map,unmap}_single().  These
+interfaces deal with page/offset pairs instead of CPU pointers.
 Specifically:
 
 	struct device *dev = &my_dev->dev;
@@ -550,7 +606,7 @@ Here, "offset" means byte offset within the given page.
 You should call dma_mapping_error() as dma_map_page() could fail and return
 error as outlined under the dma_map_single() discussion.
 
-You should call dma_unmap_page when the DMA activity is finished, e.g.
+You should call dma_unmap_page() when the DMA activity is finished, e.g.,
 from the interrupt which told you that the DMA transfer is done.
 
 With scatterlists, you map a region gathered from several regions by:
@@ -588,18 +644,16 @@ PLEASE NOTE:  The 'nents' argument to the dma_unmap_sg call must be
 	      it should _NOT_ be the 'count' value _returned_ from the
               dma_map_sg call.
 
-Every dma_map_{single,sg} call should have its dma_unmap_{single,sg}
-counterpart, because the bus address space is a shared resource (although
-in some ports the mapping is per each BUS so less devices contend for the
-same bus address space) and you could render the machine unusable by eating
-all bus addresses.
+Every dma_map_{single,sg}() call should have its dma_unmap_{single,sg}()
+counterpart, because the bus address space is a shared resource and
+you could render the machine unusable by consuming all bus addresses.
 
 If you need to use the same streaming DMA region multiple times and touch
 the data in between the DMA transfers, the buffer needs to be synced
-properly in order for the cpu and device to see the most uptodate and
+properly in order for the CPU and device to see the most up-to-date and
 correct copy of the DMA buffer.
 
-So, firstly, just map it with dma_map_{single,sg}, and after each DMA
+So, firstly, just map it with dma_map_{single,sg}(), and after each DMA
 transfer call either:
 
 	dma_sync_single_for_cpu(dev, dma_handle, size, direction);
@@ -611,7 +665,7 @@ or:
 as appropriate.
 
 Then, if you wish to let the device get at the DMA area again,
-finish accessing the data with the cpu, and then before actually
+finish accessing the data with the CPU, and then before actually
 giving the buffer to the hardware call either:
 
 	dma_sync_single_for_device(dev, dma_handle, size, direction);
@@ -623,9 +677,9 @@ or:
 as appropriate.
 
 After the last DMA transfer call one of the DMA unmap routines
-dma_unmap_{single,sg}. If you don't touch the data from the first dma_map_*
-call till dma_unmap_*, then you don't have to call the dma_sync_*
-routines at all.
+dma_unmap_{single,sg}(). If you don't touch the data from the first
+dma_map_*() call till dma_unmap_*(), then you don't have to call the
+dma_sync_*() routines at all.
 
 Here is pseudo code which shows a situation in which you would need
 to use the dma_sync_*() interfaces.
@@ -690,12 +744,12 @@ to use the dma_sync_*() interfaces.
 		}
 	}
 
-Drivers converted fully to this interface should not use virt_to_bus any
-longer, nor should they use bus_to_virt. Some drivers have to be changed a
-little bit, because there is no longer an equivalent to bus_to_virt in the
+Drivers converted fully to this interface should not use virt_to_bus() any
+longer, nor should they use bus_to_virt(). Some drivers have to be changed a
+little bit, because there is no longer an equivalent to bus_to_virt() in the
 dynamic DMA mapping scheme - you have to always store the DMA addresses
-returned by the dma_alloc_coherent, dma_pool_alloc, and dma_map_single
-calls (dma_map_sg stores them in the scatterlist itself if the platform
+returned by the dma_alloc_coherent(), dma_pool_alloc(), and dma_map_single()
+calls (dma_map_sg() stores them in the scatterlist itself if the platform
 supports dynamic DMA mapping in hardware) in your driver structures and/or
 in the card registers.
 
@@ -709,9 +763,9 @@ as it is impossible to correctly support them.
 DMA address space is limited on some architectures and an allocation
 failure can be determined by:
 
-- checking if dma_alloc_coherent returns NULL or dma_map_sg returns 0
+- checking if dma_alloc_coherent() returns NULL or dma_map_sg returns 0
 
-- checking the returned dma_addr_t of dma_map_single and dma_map_page
+- checking the dma_addr_t returned from dma_map_single() and dma_map_page()
   by using dma_mapping_error():
 
 	dma_addr_t dma_handle;
@@ -794,7 +848,7 @@ Example 2: (if buffers are allocated in a loop, unmap all mapped buffers when
 		dma_unmap_single(array[i].dma_addr);
 	}
 
-Networking drivers must call dev_kfree_skb to free the socket buffer
+Networking drivers must call dev_kfree_skb() to free the socket buffer
 and return NETDEV_TX_OK if the DMA mapping fails on the transmit hook
 (ndo_start_xmit). This means that the socket buffer is just dropped in
 the failure case.
@@ -831,7 +885,7 @@ transform some example code.
 		DEFINE_DMA_UNMAP_LEN(len);
 	};
 
-2) Use dma_unmap_{addr,len}_set to set these values.
+2) Use dma_unmap_{addr,len}_set() to set these values.
    Example, before:
 
 	ringp->mapping = FOO;
@@ -842,7 +896,7 @@ transform some example code.
 	dma_unmap_addr_set(ringp, mapping, FOO);
 	dma_unmap_len_set(ringp, len, BAR);
 
-3) Use dma_unmap_{addr,len} to access these values.
+3) Use dma_unmap_{addr,len}() to access these values.
    Example, before:
 
 	dma_unmap_single(dev, ringp->mapping, ringp->len,
diff --git a/Documentation/DMA-API.txt b/Documentation/DMA-API.txt
index e865279cec5..52088408668 100644
--- a/Documentation/DMA-API.txt
+++ b/Documentation/DMA-API.txt
@@ -4,22 +4,26 @@
         James E.J. Bottomley <James.Bottomley@HansenPartnership.com>
 
 This document describes the DMA API.  For a more gentle introduction
-of the API (and actual examples) see
-Documentation/DMA-API-HOWTO.txt.
+of the API (and actual examples), see Documentation/DMA-API-HOWTO.txt.
 
-This API is split into two pieces.  Part I describes the API.  Part II
-describes the extensions to the API for supporting non-consistent
-memory machines.  Unless you know that your driver absolutely has to
-support non-consistent platforms (this is usually only legacy
-platforms) you should only use the API described in part I.
+This API is split into two pieces.  Part I describes the basic API.
+Part II describes extensions for supporting non-consistent memory
+machines.  Unless you know that your driver absolutely has to support
+non-consistent platforms (this is usually only legacy platforms) you
+should only use the API described in part I.
 
 Part I - dma_ API
 -------------------------------------
 
-To get the dma_ API, you must #include <linux/dma-mapping.h>
+To get the dma_ API, you must #include <linux/dma-mapping.h>.  This
+provides dma_addr_t and the interfaces described below.
 
+A dma_addr_t can hold any valid DMA or bus address for the platform.  It
+can be given to a device to use as a DMA source or target.  A CPU cannot
+reference a dma_addr_t directly because there may be translation between
+its physical address space and the bus address space.
 
-Part Ia - Using large dma-coherent buffers
+Part Ia - Using large DMA-coherent buffers
 ------------------------------------------
 
 void *
@@ -33,20 +37,21 @@ to make sure to flush the processor's write buffers before telling
 devices to read that memory.)
 
 This routine allocates a region of <size> bytes of consistent memory.
-It also returns a <dma_handle> which may be cast to an unsigned
-integer the same width as the bus and used as the physical address
-base of the region.
 
-Returns: a pointer to the allocated region (in the processor's virtual
+It returns a pointer to the allocated region (in the processor's virtual
 address space) or NULL if the allocation failed.
 
+It also returns a <dma_handle> which may be cast to an unsigned integer the
+same width as the bus and given to the device as the bus address base of
+the region.
+
 Note: consistent memory can be expensive on some platforms, and the
 minimum allocation length may be as big as a page, so you should
 consolidate your requests for consistent memory as much as possible.
 The simplest way to do that is to use the dma_pool calls (see below).
 
-The flag parameter (dma_alloc_coherent only) allows the caller to
-specify the GFP_ flags (see kmalloc) for the allocation (the
+The flag parameter (dma_alloc_coherent() only) allows the caller to
+specify the GFP_ flags (see kmalloc()) for the allocation (the
 implementation may choose to ignore flags that affect the location of
 the returned memory, like GFP_DMA).
 
@@ -61,24 +66,24 @@ void
 dma_free_coherent(struct device *dev, size_t size, void *cpu_addr,
 			   dma_addr_t dma_handle)
 
-Free the region of consistent memory you previously allocated.  dev,
-size and dma_handle must all be the same as those passed into the
-consistent allocate.  cpu_addr must be the virtual address returned by
-the consistent allocate.
+Free a region of consistent memory you previously allocated.  dev,
+size and dma_handle must all be the same as those passed into
+dma_alloc_coherent().  cpu_addr must be the virtual address returned by
+the dma_alloc_coherent().
 
 Note that unlike their sibling allocation calls, these routines
 may only be called with IRQs enabled.
 
 
-Part Ib - Using small dma-coherent buffers
+Part Ib - Using small DMA-coherent buffers
 ------------------------------------------
 
 To get this part of the dma_ API, you must #include <linux/dmapool.h>
 
-Many drivers need lots of small dma-coherent memory regions for DMA
+Many drivers need lots of small DMA-coherent memory regions for DMA
 descriptors or I/O buffers.  Rather than allocating in units of a page
 or more using dma_alloc_coherent(), you can use DMA pools.  These work
-much like a struct kmem_cache, except that they use the dma-coherent allocator,
+much like a struct kmem_cache, except that they use the DMA-coherent allocator,
 not __get_free_pages().  Also, they understand common hardware constraints
 for alignment, like queue heads needing to be aligned on N-byte boundaries.
 
@@ -87,7 +92,7 @@ for alignment, like queue heads needing to be aligned on N-byte boundaries.
 	dma_pool_create(const char *name, struct device *dev,
 			size_t size, size_t align, size_t alloc);
 
-The pool create() routines initialize a pool of dma-coherent buffers
+dma_pool_create() initializes a pool of DMA-coherent buffers
 for use with a given device.  It must be called in a context which
 can sleep.
 
@@ -102,25 +107,26 @@ from this pool must not cross 4KByte boundaries.
 	void *dma_pool_alloc(struct dma_pool *pool, gfp_t gfp_flags,
 			dma_addr_t *dma_handle);
 
-This allocates memory from the pool; the returned memory will meet the size
-and alignment requirements specified at creation time.  Pass GFP_ATOMIC to
-prevent blocking, or if it's permitted (not in_interrupt, not holding SMP locks),
-pass GFP_KERNEL to allow blocking.  Like dma_alloc_coherent(), this returns
-two values:  an address usable by the cpu, and the dma address usable by the
-pool's device.
+This allocates memory from the pool; the returned memory will meet the
+size and alignment requirements specified at creation time.  Pass
+GFP_ATOMIC to prevent blocking, or if it's permitted (not
+in_interrupt, not holding SMP locks), pass GFP_KERNEL to allow
+blocking.  Like dma_alloc_coherent(), this returns two values:  an
+address usable by the CPU, and the DMA address usable by the pool's
+device.
 
 
 	void dma_pool_free(struct dma_pool *pool, void *vaddr,
 			dma_addr_t addr);
 
 This puts memory back into the pool.  The pool is what was passed to
-the pool allocation routine; the cpu (vaddr) and dma addresses are what
+dma_pool_alloc(); the CPU (vaddr) and DMA addresses are what
 were returned when that routine allocated the memory being freed.
 
 
 	void dma_pool_destroy(struct dma_pool *pool);
 
-The pool destroy() routines free the resources of the pool.  They must be
+dma_pool_destroy() frees the resources of the pool.  It must be
 called in a context which can sleep.  Make sure you've freed all allocated
 memory back to the pool before you destroy it.
 
@@ -187,9 +193,9 @@ dma_map_single(struct device *dev, void *cpu_addr, size_t size,
 		      enum dma_data_direction direction)
 
 Maps a piece of processor virtual memory so it can be accessed by the
-device and returns the physical handle of the memory.
+device and returns the bus address of the memory.
 
-The direction for both api's may be converted freely by casting.
+The direction for both APIs may be converted freely by casting.
 However the dma_ API uses a strongly typed enumerator for its
 direction:
 
@@ -198,31 +204,30 @@ DMA_TO_DEVICE		data is going from the memory to the device
 DMA_FROM_DEVICE		data is coming from the device to the memory
 DMA_BIDIRECTIONAL	direction isn't known
 
-Notes:  Not all memory regions in a machine can be mapped by this
-API.  Further, regions that appear to be physically contiguous in
-kernel virtual space may not be contiguous as physical memory.  Since
-this API does not provide any scatter/gather capability, it will fail
-if the user tries to map a non-physically contiguous piece of memory.
-For this reason, it is recommended that memory mapped by this API be
-obtained only from sources which guarantee it to be physically contiguous
-(like kmalloc).
-
-Further, the physical address of the memory must be within the
-dma_mask of the device (the dma_mask represents a bit mask of the
-addressable region for the device.  I.e., if the physical address of
-the memory anded with the dma_mask is still equal to the physical
-address, then the device can perform DMA to the memory).  In order to
+Notes:  Not all memory regions in a machine can be mapped by this API.
+Further, contiguous kernel virtual space may not be contiguous as
+physical memory.  Since this API does not provide any scatter/gather
+capability, it will fail if the user tries to map a non-physically
+contiguous piece of memory.  For this reason, memory to be mapped by
+this API should be obtained from sources which guarantee it to be
+physically contiguous (like kmalloc).
+
+Further, the bus address of the memory must be within the
+dma_mask of the device (the dma_mask is a bit mask of the
+addressable region for the device, i.e., if the bus address of
+the memory ANDed with the dma_mask is still equal to the bus
+address, then the device can perform DMA to the memory).  To
 ensure that the memory allocated by kmalloc is within the dma_mask,
 the driver may specify various platform-dependent flags to restrict
-the physical memory range of the allocation (e.g. on x86, GFP_DMA
-guarantees to be within the first 16Mb of available physical memory,
+the bus address range of the allocation (e.g., on x86, GFP_DMA
+guarantees to be within the first 16MB of available bus addresses,
 as required by ISA devices).
 
 Note also that the above constraints on physical contiguity and
 dma_mask may not apply if the platform has an IOMMU (a device which
-supplies a physical to virtual mapping between the I/O memory bus and
-the device).  However, to be portable, device driver writers may *not*
-assume that such an IOMMU exists.
+maps an I/O bus address to a physical memory address).  However, to be
+portable, device driver writers may *not* assume that such an IOMMU
+exists.
 
 Warnings:  Memory coherency operates at a granularity called the cache
 line width.  In order for memory mapped by this API to operate
@@ -281,9 +286,9 @@ cache width is.
 int
 dma_mapping_error(struct device *dev, dma_addr_t dma_addr)
 
-In some circumstances dma_map_single and dma_map_page will fail to create
+In some circumstances dma_map_single() and dma_map_page() will fail to create
 a mapping. A driver can check for these errors by testing the returned
-dma address with dma_mapping_error(). A non-zero return value means the mapping
+DMA address with dma_mapping_error(). A non-zero return value means the mapping
 could not be created and the driver should take appropriate action (e.g.
 reduce current DMA mapping usage or delay and try again later).
 
@@ -291,7 +296,7 @@ reduce current DMA mapping usage or delay and try again later).
 	dma_map_sg(struct device *dev, struct scatterlist *sg,
 		int nents, enum dma_data_direction direction)
 
-Returns: the number of physical segments mapped (this may be shorter
+Returns: the number of bus address segments mapped (this may be shorter
 than <nents> passed in if some elements of the scatter/gather list are
 physically or virtually adjacent and an IOMMU maps them with a single
 entry).
@@ -299,7 +304,7 @@ entry).
 Please note that the sg cannot be mapped again if it has been mapped once.
 The mapping process is allowed to destroy information in the sg.
 
-As with the other mapping interfaces, dma_map_sg can fail. When it
+As with the other mapping interfaces, dma_map_sg() can fail. When it
 does, 0 is returned and a driver must take appropriate action. It is
 critical that the driver do something, in the case of a block driver
 aborting the request or even oopsing is better than doing nothing and
@@ -335,7 +340,7 @@ must be the same as those and passed in to the scatter/gather mapping
 API.
 
 Note: <nents> must be the number you passed in, *not* the number of
-physical entries returned.
+bus address entries returned.
 
 void
 dma_sync_single_for_cpu(struct device *dev, dma_addr_t dma_handle, size_t size,
@@ -350,7 +355,7 @@ void
 dma_sync_sg_for_device(struct device *dev, struct scatterlist *sg, int nelems,
 		       enum dma_data_direction direction)
 
-Synchronise a single contiguous or scatter/gather mapping for the cpu
+Synchronise a single contiguous or scatter/gather mapping for the CPU
 and device. With the sync_sg API, all the parameters must be the same
 as those passed into the single mapping API. With the sync_single API,
 you can use dma_handle and size parameters that aren't identical to
@@ -391,10 +396,10 @@ The four functions above are just like the counterpart functions
 without the _attrs suffixes, except that they pass an optional
 struct dma_attrs*.
 
-struct dma_attrs encapsulates a set of "dma attributes". For the
+struct dma_attrs encapsulates a set of "DMA attributes". For the
 definition of struct dma_attrs see linux/dma-attrs.h.
 
-The interpretation of dma attributes is architecture-specific, and
+The interpretation of DMA attributes is architecture-specific, and
 each attribute should be documented in Documentation/DMA-attributes.txt.
 
 If struct dma_attrs* is NULL, the semantics of each of these
@@ -458,7 +463,7 @@ Note: where the platform can return consistent memory, it will
 guarantee that the sync points become nops.
 
 Warning:  Handling non-consistent memory is a real pain.  You should
-only ever use this API if you positively know your driver will be
+only use this API if you positively know your driver will be
 required to work on one of the rare (usually non-PCI) architectures
 that simply cannot make consistent memory.
 
@@ -492,30 +497,29 @@ continuing on for size.  Again, you *must* observe the cache line
 boundaries when doing this.
 
 int
-dma_declare_coherent_memory(struct device *dev, dma_addr_t bus_addr,
+dma_declare_coherent_memory(struct device *dev, phys_addr_t phys_addr,
 			    dma_addr_t device_addr, size_t size, int
 			    flags)
 
-Declare region of memory to be handed out by dma_alloc_coherent when
+Declare region of memory to be handed out by dma_alloc_coherent() when
 it's asked for coherent memory for this device.
 
-bus_addr is the physical address to which the memory is currently
-assigned in the bus responding region (this will be used by the
-platform to perform the mapping).
+phys_addr is the CPU physical address to which the memory is currently
+assigned (this will be ioremapped so the CPU can access the region).
 
-device_addr is the physical address the device needs to be programmed
-with actually to address this memory (this will be handed out as the
+device_addr is the bus address the device needs to be programmed
+with to actually address this memory (this will be handed out as the
 dma_addr_t in dma_alloc_coherent()).
 
 size is the size of the area (must be multiples of PAGE_SIZE).
 
-flags can be or'd together and are:
+flags can be ORed together and are:
 
 DMA_MEMORY_MAP - request that the memory returned from
 dma_alloc_coherent() be directly writable.
 
 DMA_MEMORY_IO - request that the memory returned from
-dma_alloc_coherent() be addressable using read/write/memcpy_toio etc.
+dma_alloc_coherent() be addressable using read()/write()/memcpy_toio() etc.
 
 One or both of these flags must be present.
 
@@ -572,7 +576,7 @@ region is occupied.
 Part III - Debug drivers use of the DMA-API
 -------------------------------------------
 
-The DMA-API as described above as some constraints. DMA addresses must be
+The DMA-API as described above has some constraints. DMA addresses must be
 released with the corresponding function with the same size for example. With
 the advent of hardware IOMMUs it becomes more and more important that drivers
 do not violate those constraints. In the worst case such a violation can
@@ -690,11 +694,11 @@ architectural default.
 void debug_dmap_mapping_error(struct device *dev, dma_addr_t dma_addr);
 
 dma-debug interface debug_dma_mapping_error() to debug drivers that fail
-to check dma mapping errors on addresses returned by dma_map_single() and
+to check DMA mapping errors on addresses returned by dma_map_single() and
 dma_map_page() interfaces. This interface clears a flag set by
 debug_dma_map_page() to indicate that dma_mapping_error() has been called by
 the driver. When driver does unmap, debug_dma_unmap() checks the flag and if
 this flag is still set, prints warning message that includes call trace that
 leads up to the unmap. This interface can be called from dma_mapping_error()
-routines to enable dma mapping error check debugging.
+routines to enable DMA mapping error check debugging.
 
diff --git a/Documentation/DMA-ISA-LPC.txt b/Documentation/DMA-ISA-LPC.txt
index e767805b418..b1a19835e90 100644
--- a/Documentation/DMA-ISA-LPC.txt
+++ b/Documentation/DMA-ISA-LPC.txt
@@ -16,7 +16,7 @@ To do ISA style DMA you need to include two headers:
 #include <asm/dma.h>
 
 The first is the generic DMA API used to convert virtual addresses to
-physical addresses (see Documentation/DMA-API.txt for details).
+bus addresses (see Documentation/DMA-API.txt for details).
 
 The second contains the routines specific to ISA DMA transfers. Since
 this is not present on all platforms make sure you construct your
@@ -50,7 +50,7 @@ early as possible and not release it until the driver is unloaded.)
 Part III - Address translation
 ------------------------------
 
-To translate the virtual address to a physical use the normal DMA
+To translate the virtual address to a bus address, use the normal DMA
 API. Do _not_ use isa_virt_to_phys() even though it does the same
 thing. The reason for this is that the function isa_virt_to_phys()
 will require a Kconfig dependency to ISA, not just ISA_DMA_API which
diff --git a/Documentation/DocBook/Makefile b/Documentation/DocBook/Makefile
index b444f2e8fe3..bec06659e0e 100644
--- a/Documentation/DocBook/Makefile
+++ b/Documentation/DocBook/Makefile
@@ -14,7 +14,8 @@ DOCBOOKS := z8530book.xml device-drivers.xml \
 	    genericirq.xml s390-drivers.xml uio-howto.xml scsi.xml \
 	    80211.xml debugobjects.xml sh.xml regulator.xml \
 	    alsa-driver-api.xml writing-an-alsa-driver.xml \
-	    tracepoint.xml drm.xml media_api.xml w1.xml
+	    tracepoint.xml drm.xml media_api.xml w1.xml \
+	    writing_musb_glue_layer.xml
 
 include Documentation/DocBook/media/Makefile
 
diff --git a/Documentation/DocBook/drm.tmpl b/Documentation/DocBook/drm.tmpl
index 702c4474919..ba60d93c185 100644
--- a/Documentation/DocBook/drm.tmpl
+++ b/Documentation/DocBook/drm.tmpl
@@ -79,7 +79,7 @@
   <partintro>
     <para>
       This first part of the DRM Developer's Guide documents core DRM code,
-      helper libraries for writting drivers and generic userspace interfaces
+      helper libraries for writing drivers and generic userspace interfaces
       exposed by DRM drivers.
     </para>
   </partintro>
@@ -459,7 +459,7 @@ char *date;</synopsis>
       providing a solution to every graphics memory-related problems, GEM
       identified common code between drivers and created a support library to
       share it. GEM has simpler initialization and execution requirements than
-      TTM, but has no video RAM management capabitilies and is thus limited to
+      TTM, but has no video RAM management capabilities and is thus limited to
       UMA devices.
     </para>
     <sect2>
@@ -889,7 +889,7 @@ int (*prime_fd_to_handle)(struct drm_device *dev,
 	    vice versa. Drivers must use the kernel dma-buf buffer sharing framework
 	    to manage the PRIME file descriptors. Similar to the mode setting
 	    API PRIME is agnostic to the underlying buffer object manager, as
-	    long as handles are 32bit unsinged integers.
+	    long as handles are 32bit unsigned integers.
 	  </para>
 	  <para>
 	    While non-GEM drivers must implement the operations themselves, GEM
@@ -2287,6 +2287,11 @@ void intel_crt_init(struct drm_device *dev)
 !Edrivers/gpu/drm/drm_crtc_helper.c
     </sect2>
     <sect2>
+      <title>Output Probing Helper Functions Reference</title>
+!Pdrivers/gpu/drm/drm_probe_helper.c output probing helper overview
+!Edrivers/gpu/drm/drm_probe_helper.c
+    </sect2>
+    <sect2>
       <title>fbdev Helper Functions Reference</title>
 !Pdrivers/gpu/drm/drm_fb_helper.c fbdev helpers
 !Edrivers/gpu/drm/drm_fb_helper.c
@@ -2351,7 +2356,7 @@ void intel_crt_init(struct drm_device *dev)
       first create properties and then create and associate individual instances
       of those properties to objects. A property can be instantiated multiple
       times and associated with different objects. Values are stored in property
-      instances, and all other property information are stored in the propery
+      instances, and all other property information are stored in the property
       and shared between all instances of the property.
     </para>
     <para>
@@ -2692,10 +2697,10 @@ int num_ioctls;</synopsis>
   <sect1>
     <title>Legacy Support Code</title>
     <para>
-      The section very brievely covers some of the old legacy support code which
+      The section very briefly covers some of the old legacy support code which
       is only used by old DRM drivers which have done a so-called shadow-attach
       to the underlying device instead of registering as a real driver. This
-      also includes some of the old generic buffer mangement and command
+      also includes some of the old generic buffer management and command
       submission code. Do not use any of this in new and modern drivers.
     </para>
 
diff --git a/Documentation/DocBook/filesystems.tmpl b/Documentation/DocBook/filesystems.tmpl
index 4f676838da0..bcdfdb9a927 100644
--- a/Documentation/DocBook/filesystems.tmpl
+++ b/Documentation/DocBook/filesystems.tmpl
@@ -62,7 +62,7 @@
 !Efs/mpage.c
 !Efs/namei.c
 !Efs/buffer.c
-!Efs/bio.c
+!Eblock/bio.c
 !Efs/seq_file.c
 !Efs/filesystems.c
 !Efs/fs-writeback.c
diff --git a/Documentation/DocBook/media/Makefile b/Documentation/DocBook/media/Makefile
index f9fd615427f..1d27f0a1abd 100644
--- a/Documentation/DocBook/media/Makefile
+++ b/Documentation/DocBook/media/Makefile
@@ -195,7 +195,7 @@ DVB_DOCUMENTED = \
 #
 
 install_media_images = \
-	$(Q)cp $(OBJIMGFILES) $(MEDIA_SRC_DIR)/v4l/*.svg $(MEDIA_OBJ_DIR)/media_api
+	$(Q)-cp $(OBJIMGFILES) $(MEDIA_SRC_DIR)/v4l/*.svg $(MEDIA_OBJ_DIR)/media_api
 
 $(MEDIA_OBJ_DIR)/%: $(MEDIA_SRC_DIR)/%.b64
 	$(Q)base64 -d $< >$@
diff --git a/Documentation/DocBook/writing_musb_glue_layer.tmpl b/Documentation/DocBook/writing_musb_glue_layer.tmpl
new file mode 100644
index 00000000000..837eca77f27
--- /dev/null
+++ b/Documentation/DocBook/writing_musb_glue_layer.tmpl
@@ -0,0 +1,873 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE book PUBLIC "-//OASIS//DTD DocBook XML V4.1.2//EN"
+	"http://www.oasis-open.org/docbook/xml/4.1.2/docbookx.dtd" []>
+
+<book id="Writing-MUSB-Glue-Layer">
+ <bookinfo>
+  <title>Writing an MUSB Glue Layer</title>
+
+  <authorgroup>
+   <author>
+    <firstname>Apelete</firstname>
+    <surname>Seketeli</surname>
+    <affiliation>
+     <address>
+      <email>apelete at seketeli.net</email>
+     </address>
+    </affiliation>
+   </author>
+  </authorgroup>
+
+  <copyright>
+   <year>2014</year>
+   <holder>Apelete Seketeli</holder>
+  </copyright>
+
+  <legalnotice>
+   <para>
+     This documentation 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.
+   </para>
+
+   <para>
+     This documentation 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.
+   </para>
+
+   <para>
+     You should have received a copy of the GNU General Public License
+     along with this documentation; if not, write to the Free Software
+     Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
+     02111-1307 USA
+   </para>
+
+   <para>
+     For more details see the file COPYING in the Linux kernel source
+     tree.
+   </para>
+  </legalnotice>
+ </bookinfo>
+
+<toc></toc>
+
+  <chapter id="introduction">
+    <title>Introduction</title>
+    <para>
+      The Linux MUSB subsystem is part of the larger Linux USB
+      subsystem. It provides support for embedded USB Device Controllers
+      (UDC) that do not use Universal Host Controller Interface (UHCI)
+      or Open Host Controller Interface (OHCI).
+    </para>
+    <para>
+      Instead, these embedded UDC rely on the USB On-the-Go (OTG)
+      specification which they implement at least partially. The silicon
+      reference design used in most cases is the Multipoint USB
+      Highspeed Dual-Role Controller (MUSB HDRC) found in the Mentor
+      Graphics Inventraâ„¢ design.
+    </para>
+    <para>
+      As a self-taught exercise I have written an MUSB glue layer for
+      the Ingenic JZ4740 SoC, modelled after the many MUSB glue layers
+      in the kernel source tree. This layer can be found at
+      drivers/usb/musb/jz4740.c. In this documentation I will walk
+      through the basics of the jz4740.c glue layer, explaining the
+      different pieces and what needs to be done in order to write your
+      own device glue layer.
+    </para>
+  </chapter>
+
+  <chapter id="linux-musb-basics">
+    <title>Linux MUSB Basics</title>
+    <para>
+      To get started on the topic, please read USB On-the-Go Basics (see
+      Resources) which provides an introduction of USB OTG operation at
+      the hardware level. A couple of wiki pages by Texas Instruments
+      and Analog Devices also provide an overview of the Linux kernel
+      MUSB configuration, albeit focused on some specific devices
+      provided by these companies. Finally, getting acquainted with the
+      USB specification at USB home page may come in handy, with
+      practical instance provided through the Writing USB Device Drivers
+      documentation (again, see Resources).
+    </para>
+    <para>
+      Linux USB stack is a layered architecture in which the MUSB
+      controller hardware sits at the lowest. The MUSB controller driver
+      abstract the MUSB controller hardware to the Linux USB stack.
+    </para>
+    <programlisting>
+      ------------------------
+      |                      | &lt;------- drivers/usb/gadget
+      | Linux USB Core Stack | &lt;------- drivers/usb/host
+      |                      | &lt;------- drivers/usb/core
+      ------------------------
+                 �
+     --------------------------
+     |                        | &lt;------ drivers/usb/musb/musb_gadget.c
+     | MUSB Controller driver | &lt;------ drivers/usb/musb/musb_host.c
+     |                        | &lt;------ drivers/usb/musb/musb_core.c
+     --------------------------
+                 �
+  ---------------------------------
+  | MUSB Platform Specific Driver |
+  |                               | &lt;-- drivers/usb/musb/jz4740.c
+  |       aka &quot;Glue Layer&quot;        |
+  ---------------------------------
+                 �
+  ---------------------------------
+  |   MUSB Controller Hardware    |
+  ---------------------------------
+    </programlisting>
+    <para>
+      As outlined above, the glue layer is actually the platform
+      specific code sitting in between the controller driver and the
+      controller hardware.
+    </para>
+    <para>
+      Just like a Linux USB driver needs to register itself with the
+      Linux USB subsystem, the MUSB glue layer needs first to register
+      itself with the MUSB controller driver. This will allow the
+      controller driver to know about which device the glue layer
+      supports and which functions to call when a supported device is
+      detected or released; remember we are talking about an embedded
+      controller chip here, so no insertion or removal at run-time.
+    </para>
+    <para>
+      All of this information is passed to the MUSB controller driver
+      through a platform_driver structure defined in the glue layer as:
+    </para>
+    <programlisting linenumbering="numbered">
+static struct platform_driver jz4740_driver = {
+	.probe		= jz4740_probe,
+	.remove		= jz4740_remove,
+	.driver		= {
+		.name	= "musb-jz4740",
+	},
+};
+    </programlisting>
+    <para>
+      The probe and remove function pointers are called when a matching
+      device is detected and, respectively, released. The name string
+      describes the device supported by this glue layer. In the current
+      case it matches a platform_device structure declared in
+      arch/mips/jz4740/platform.c. Note that we are not using device
+      tree bindings here.
+    </para>
+    <para>
+      In order to register itself to the controller driver, the glue
+      layer goes through a few steps, basically allocating the
+      controller hardware resources and initialising a couple of
+      circuits. To do so, it needs to keep track of the information used
+      throughout these steps. This is done by defining a private
+      jz4740_glue structure:
+    </para>
+    <programlisting linenumbering="numbered">
+struct jz4740_glue {
+	struct device           *dev;
+	struct platform_device  *musb;
+	struct clk		*clk;
+};
+    </programlisting>
+    <para>
+      The dev and musb members are both device structure variables. The
+      first one holds generic information about the device, since it's
+      the basic device structure, and the latter holds information more
+      closely related to the subsystem the device is registered to. The
+      clk variable keeps information related to the device clock
+      operation.
+    </para>
+    <para>
+      Let's go through the steps of the probe function that leads the
+      glue layer to register itself to the controller driver.
+    </para>
+    <para>
+      N.B.: For the sake of readability each function will be split in
+      logical parts, each part being shown as if it was independent from
+      the others.
+    </para>
+    <programlisting linenumbering="numbered">
+static int jz4740_probe(struct platform_device *pdev)
+{
+	struct platform_device		*musb;
+	struct jz4740_glue		*glue;
+	struct clk                      *clk;
+	int				ret;
+
+	glue = devm_kzalloc(&amp;pdev->dev, sizeof(*glue), GFP_KERNEL);
+	if (!glue)
+		return -ENOMEM;
+
+	musb = platform_device_alloc("musb-hdrc", PLATFORM_DEVID_AUTO);
+	if (!musb) {
+		dev_err(&amp;pdev->dev, "failed to allocate musb device\n");
+		return -ENOMEM;
+	}
+
+	clk = devm_clk_get(&amp;pdev->dev, "udc");
+	if (IS_ERR(clk)) {
+		dev_err(&amp;pdev->dev, "failed to get clock\n");
+		ret = PTR_ERR(clk);
+		goto err_platform_device_put;
+	}
+
+	ret = clk_prepare_enable(clk);
+	if (ret) {
+		dev_err(&amp;pdev->dev, "failed to enable clock\n");
+		goto err_platform_device_put;
+	}
+
+	musb->dev.parent		= &amp;pdev->dev;
+
+	glue->dev			= &amp;pdev->dev;
+	glue->musb			= musb;
+	glue->clk			= clk;
+
+	return 0;
+
+err_platform_device_put:
+	platform_device_put(musb);
+	return ret;
+}
+    </programlisting>
+    <para>
+      The first few lines of the probe function allocate and assign the
+      glue, musb and clk variables. The GFP_KERNEL flag (line 8) allows
+      the allocation process to sleep and wait for memory, thus being
+      usable in a blocking situation. The PLATFORM_DEVID_AUTO flag (line
+      12) allows automatic allocation and management of device IDs in
+      order to avoid device namespace collisions with explicit IDs. With
+      devm_clk_get() (line 18) the glue layer allocates the clock -- the
+      <literal>devm_</literal> prefix indicates that clk_get() is
+      managed: it automatically frees the allocated clock resource data
+      when the device is released -- and enable it.
+    </para>
+    <para>
+      Then comes the registration steps:
+    </para>
+    <programlisting linenumbering="numbered">
+static int jz4740_probe(struct platform_device *pdev)
+{
+	struct musb_hdrc_platform_data	*pdata = &amp;jz4740_musb_platform_data;
+
+	pdata->platform_ops		= &amp;jz4740_musb_ops;
+
+	platform_set_drvdata(pdev, glue);
+
+	ret = platform_device_add_resources(musb, pdev->resource,
+					    pdev->num_resources);
+	if (ret) {
+		dev_err(&amp;pdev->dev, "failed to add resources\n");
+		goto err_clk_disable;
+	}
+
+	ret = platform_device_add_data(musb, pdata, sizeof(*pdata));
+	if (ret) {
+		dev_err(&amp;pdev->dev, "failed to add platform_data\n");
+		goto err_clk_disable;
+	}
+
+	return 0;
+
+err_clk_disable:
+	clk_disable_unprepare(clk);
+err_platform_device_put:
+	platform_device_put(musb);
+	return ret;
+}
+    </programlisting>
+    <para>
+      The first step is to pass the device data privately held by the
+      glue layer on to the controller driver through
+      platform_set_drvdata() (line 7). Next is passing on the device
+      resources information, also privately held at that point, through
+      platform_device_add_resources() (line 9).
+    </para>
+    <para>
+      Finally comes passing on the platform specific data to the
+      controller driver (line 16). Platform data will be discussed in
+      <link linkend="device-platform-data">Chapter 4</link>, but here
+      we are looking at the platform_ops function pointer (line 5) in
+      musb_hdrc_platform_data structure (line 3).  This function
+      pointer allows the MUSB controller driver to know which function
+      to call for device operation:
+    </para>
+    <programlisting linenumbering="numbered">
+static const struct musb_platform_ops jz4740_musb_ops = {
+	.init		= jz4740_musb_init,
+	.exit		= jz4740_musb_exit,
+};
+    </programlisting>
+    <para>
+      Here we have the minimal case where only init and exit functions
+      are called by the controller driver when needed. Fact is the
+      JZ4740 MUSB controller is a basic controller, lacking some
+      features found in other controllers, otherwise we may also have
+      pointers to a few other functions like a power management function
+      or a function to switch between OTG and non-OTG modes, for
+      instance.
+    </para>
+    <para>
+      At that point of the registration process, the controller driver
+      actually calls the init function:
+    </para>
+    <programlisting linenumbering="numbered">
+static int jz4740_musb_init(struct musb *musb)
+{
+	musb->xceiv = usb_get_phy(USB_PHY_TYPE_USB2);
+	if (!musb->xceiv) {
+		pr_err("HS UDC: no transceiver configured\n");
+		return -ENODEV;
+	}
+
+	/* Silicon does not implement ConfigData register.
+	 * Set dyn_fifo to avoid reading EP config from hardware.
+	 */
+	musb->dyn_fifo = true;
+
+	musb->isr = jz4740_musb_interrupt;
+
+	return 0;
+}
+    </programlisting>
+    <para>
+      The goal of jz4740_musb_init() is to get hold of the transceiver
+      driver data of the MUSB controller hardware and pass it on to the
+      MUSB controller driver, as usual. The transceiver is the circuitry
+      inside the controller hardware responsible for sending/receiving
+      the USB data. Since it is an implementation of the physical layer
+      of the OSI model, the transceiver is also referred to as PHY.
+    </para>
+    <para>
+      Getting hold of the MUSB PHY driver data is done with
+      usb_get_phy() which returns a pointer to the structure
+      containing the driver instance data. The next couple of
+      instructions (line 12 and 14) are used as a quirk and to setup
+      IRQ handling respectively. Quirks and IRQ handling will be
+      discussed later in <link linkend="device-quirks">Chapter
+      5</link> and <link linkend="handling-irqs">Chapter 3</link>.
+    </para>
+    <programlisting linenumbering="numbered">
+static int jz4740_musb_exit(struct musb *musb)
+{
+	usb_put_phy(musb->xceiv);
+
+	return 0;
+}
+    </programlisting>
+    <para>
+      Acting as the counterpart of init, the exit function releases the
+      MUSB PHY driver when the controller hardware itself is about to be
+      released.
+    </para>
+    <para>
+      Again, note that init and exit are fairly simple in this case due
+      to the basic set of features of the JZ4740 controller hardware.
+      When writing an musb glue layer for a more complex controller
+      hardware, you might need to take care of more processing in those
+      two functions.
+    </para>
+    <para>
+      Returning from the init function, the MUSB controller driver jumps
+      back into the probe function:
+    </para>
+    <programlisting linenumbering="numbered">
+static int jz4740_probe(struct platform_device *pdev)
+{
+	ret = platform_device_add(musb);
+	if (ret) {
+		dev_err(&amp;pdev->dev, "failed to register musb device\n");
+		goto err_clk_disable;
+	}
+
+	return 0;
+
+err_clk_disable:
+	clk_disable_unprepare(clk);
+err_platform_device_put:
+	platform_device_put(musb);
+	return ret;
+}
+    </programlisting>
+    <para>
+      This is the last part of the device registration process where the
+      glue layer adds the controller hardware device to Linux kernel
+      device hierarchy: at this stage, all known information about the
+      device is passed on to the Linux USB core stack.
+    </para>
+    <programlisting linenumbering="numbered">
+static int jz4740_remove(struct platform_device *pdev)
+{
+	struct jz4740_glue	*glue = platform_get_drvdata(pdev);
+
+	platform_device_unregister(glue->musb);
+	clk_disable_unprepare(glue->clk);
+
+	return 0;
+}
+    </programlisting>
+    <para>
+      Acting as the counterpart of probe, the remove function unregister
+      the MUSB controller hardware (line 5) and disable the clock (line
+      6), allowing it to be gated.
+    </para>
+  </chapter>
+
+  <chapter id="handling-irqs">
+    <title>Handling IRQs</title>
+    <para>
+      Additionally to the MUSB controller hardware basic setup and
+      registration, the glue layer is also responsible for handling the
+      IRQs:
+    </para>
+    <programlisting linenumbering="numbered">
+static irqreturn_t jz4740_musb_interrupt(int irq, void *__hci)
+{
+	unsigned long   flags;
+	irqreturn_t     retval = IRQ_NONE;
+	struct musb     *musb = __hci;
+
+	spin_lock_irqsave(&amp;musb->lock, flags);
+
+	musb->int_usb = musb_readb(musb->mregs, MUSB_INTRUSB);
+	musb->int_tx = musb_readw(musb->mregs, MUSB_INTRTX);
+	musb->int_rx = musb_readw(musb->mregs, MUSB_INTRRX);
+
+	/*
+	 * The controller is gadget only, the state of the host mode IRQ bits is
+	 * undefined. Mask them to make sure that the musb driver core will
+	 * never see them set
+	 */
+	musb->int_usb &amp;= MUSB_INTR_SUSPEND | MUSB_INTR_RESUME |
+	    MUSB_INTR_RESET | MUSB_INTR_SOF;
+
+	if (musb->int_usb || musb->int_tx || musb->int_rx)
+		retval = musb_interrupt(musb);
+
+	spin_unlock_irqrestore(&amp;musb->lock, flags);
+
+	return retval;
+}
+    </programlisting>
+    <para>
+      Here the glue layer mostly has to read the relevant hardware
+      registers and pass their values on to the controller driver which
+      will handle the actual event that triggered the IRQ.
+    </para>
+    <para>
+      The interrupt handler critical section is protected by the
+      spin_lock_irqsave() and counterpart spin_unlock_irqrestore()
+      functions (line 7 and 24 respectively), which prevent the
+      interrupt handler code to be run by two different threads at the
+      same time.
+    </para>
+    <para>
+      Then the relevant interrupt registers are read (line 9 to 11):
+    </para>
+    <itemizedlist>
+      <listitem>
+        <para>
+          MUSB_INTRUSB: indicates which USB interrupts are currently
+          active,
+        </para>
+      </listitem>
+      <listitem>
+        <para>
+          MUSB_INTRTX: indicates which of the interrupts for TX
+          endpoints are currently active,
+        </para>
+      </listitem>
+      <listitem>
+        <para>
+          MUSB_INTRRX: indicates which of the interrupts for TX
+          endpoints are currently active.
+        </para>
+      </listitem>
+    </itemizedlist>
+    <para>
+      Note that musb_readb() is used to read 8-bit registers at most,
+      while musb_readw() allows us to read at most 16-bit registers.
+      There are other functions that can be used depending on the size
+      of your device registers. See musb_io.h for more information.
+    </para>
+    <para>
+      Instruction on line 18 is another quirk specific to the JZ4740
+      USB device controller, which will be discussed later in <link
+      linkend="device-quirks">Chapter 5</link>.
+    </para>
+    <para>
+      The glue layer still needs to register the IRQ handler though.
+      Remember the instruction on line 14 of the init function:
+    </para>
+    <programlisting linenumbering="numbered">
+static int jz4740_musb_init(struct musb *musb)
+{
+	musb->isr = jz4740_musb_interrupt;
+
+	return 0;
+}
+    </programlisting>
+    <para>
+      This instruction sets a pointer to the glue layer IRQ handler
+      function, in order for the controller hardware to call the handler
+      back when an IRQ comes from the controller hardware. The interrupt
+      handler is now implemented and registered.
+    </para>
+  </chapter>
+
+  <chapter id="device-platform-data">
+    <title>Device Platform Data</title>
+    <para>
+      In order to write an MUSB glue layer, you need to have some data
+      describing the hardware capabilities of your controller hardware,
+      which is called the platform data.
+    </para>
+    <para>
+      Platform data is specific to your hardware, though it may cover a
+      broad range of devices, and is generally found somewhere in the
+      arch/ directory, depending on your device architecture.
+    </para>
+    <para>
+      For instance, platform data for the JZ4740 SoC is found in
+      arch/mips/jz4740/platform.c. In the platform.c file each device of
+      the JZ4740 SoC is described through a set of structures.
+    </para>
+    <para>
+      Here is the part of arch/mips/jz4740/platform.c that covers the
+      USB Device Controller (UDC):
+    </para>
+    <programlisting linenumbering="numbered">
+/* USB Device Controller */
+struct platform_device jz4740_udc_xceiv_device = {
+	.name = "usb_phy_gen_xceiv",
+	.id   = 0,
+};
+
+static struct resource jz4740_udc_resources[] = {
+	[0] = {
+		.start = JZ4740_UDC_BASE_ADDR,
+		.end   = JZ4740_UDC_BASE_ADDR + 0x10000 - 1,
+		.flags = IORESOURCE_MEM,
+	},
+	[1] = {
+		.start = JZ4740_IRQ_UDC,
+		.end   = JZ4740_IRQ_UDC,
+		.flags = IORESOURCE_IRQ,
+		.name  = "mc",
+	},
+};
+
+struct platform_device jz4740_udc_device = {
+	.name = "musb-jz4740",
+	.id   = -1,
+	.dev  = {
+		.dma_mask          = &amp;jz4740_udc_device.dev.coherent_dma_mask,
+		.coherent_dma_mask = DMA_BIT_MASK(32),
+	},
+	.num_resources = ARRAY_SIZE(jz4740_udc_resources),
+	.resource      = jz4740_udc_resources,
+};
+    </programlisting>
+    <para>
+      The jz4740_udc_xceiv_device platform device structure (line 2)
+      describes the UDC transceiver with a name and id number.
+    </para>
+    <para>
+      At the time of this writing, note that
+      &quot;usb_phy_gen_xceiv&quot; is the specific name to be used for
+      all transceivers that are either built-in with reference USB IP or
+      autonomous and doesn't require any PHY programming. You will need
+      to set CONFIG_NOP_USB_XCEIV=y in the kernel configuration to make
+      use of the corresponding transceiver driver. The id field could be
+      set to -1 (equivalent to PLATFORM_DEVID_NONE), -2 (equivalent to
+      PLATFORM_DEVID_AUTO) or start with 0 for the first device of this
+      kind if we want a specific id number.
+    </para>
+    <para>
+      The jz4740_udc_resources resource structure (line 7) defines the
+      UDC registers base addresses.
+    </para>
+    <para>
+      The first array (line 9 to 11) defines the UDC registers base
+      memory addresses: start points to the first register memory
+      address, end points to the last register memory address and the
+      flags member defines the type of resource we are dealing with. So
+      IORESOURCE_MEM is used to define the registers memory addresses.
+      The second array (line 14 to 17) defines the UDC IRQ registers
+      addresses. Since there is only one IRQ register available for the
+      JZ4740 UDC, start and end point at the same address. The
+      IORESOURCE_IRQ flag tells that we are dealing with IRQ resources,
+      and the name &quot;mc&quot; is in fact hard-coded in the MUSB core
+      in order for the controller driver to retrieve this IRQ resource
+      by querying it by its name.
+    </para>
+    <para>
+      Finally, the jz4740_udc_device platform device structure (line 21)
+      describes the UDC itself.
+    </para>
+    <para>
+      The &quot;musb-jz4740&quot; name (line 22) defines the MUSB
+      driver that is used for this device; remember this is in fact
+      the name that we used in the jz4740_driver platform driver
+      structure in <link linkend="linux-musb-basics">Chapter
+      2</link>. The id field (line 23) is set to -1 (equivalent to
+      PLATFORM_DEVID_NONE) since we do not need an id for the device:
+      the MUSB controller driver was already set to allocate an
+      automatic id in <link linkend="linux-musb-basics">Chapter
+      2</link>. In the dev field we care for DMA related information
+      here. The dma_mask field (line 25) defines the width of the DMA
+      mask that is going to be used, and coherent_dma_mask (line 26)
+      has the same purpose but for the alloc_coherent DMA mappings: in
+      both cases we are using a 32 bits mask. Then the resource field
+      (line 29) is simply a pointer to the resource structure defined
+      before, while the num_resources field (line 28) keeps track of
+      the number of arrays defined in the resource structure (in this
+      case there were two resource arrays defined before).
+    </para>
+    <para>
+      With this quick overview of the UDC platform data at the arch/
+      level now done, let's get back to the MUSB glue layer specific
+      platform data in drivers/usb/musb/jz4740.c:
+    </para>
+    <programlisting linenumbering="numbered">
+static struct musb_hdrc_config jz4740_musb_config = {
+	/* Silicon does not implement USB OTG. */
+	.multipoint = 0,
+	/* Max EPs scanned, driver will decide which EP can be used. */
+	.num_eps    = 4,
+	/* RAMbits needed to configure EPs from table */
+	.ram_bits   = 9,
+	.fifo_cfg = jz4740_musb_fifo_cfg,
+	.fifo_cfg_size = ARRAY_SIZE(jz4740_musb_fifo_cfg),
+};
+
+static struct musb_hdrc_platform_data jz4740_musb_platform_data = {
+	.mode   = MUSB_PERIPHERAL,
+	.config = &amp;jz4740_musb_config,
+};
+    </programlisting>
+    <para>
+      First the glue layer configures some aspects of the controller
+      driver operation related to the controller hardware specifics.
+      This is done through the jz4740_musb_config musb_hdrc_config
+      structure.
+    </para>
+    <para>
+      Defining the OTG capability of the controller hardware, the
+      multipoint member (line 3) is set to 0 (equivalent to false)
+      since the JZ4740 UDC is not OTG compatible. Then num_eps (line
+      5) defines the number of USB endpoints of the controller
+      hardware, including endpoint 0: here we have 3 endpoints +
+      endpoint 0. Next is ram_bits (line 7) which is the width of the
+      RAM address bus for the MUSB controller hardware. This
+      information is needed when the controller driver cannot
+      automatically configure endpoints by reading the relevant
+      controller hardware registers. This issue will be discussed when
+      we get to device quirks in <link linkend="device-quirks">Chapter
+      5</link>. Last two fields (line 8 and 9) are also about device
+      quirks: fifo_cfg points to the USB endpoints configuration table
+      and fifo_cfg_size keeps track of the size of the number of
+      entries in that configuration table. More on that later in <link
+      linkend="device-quirks">Chapter 5</link>.
+    </para>
+    <para>
+      Then this configuration is embedded inside
+      jz4740_musb_platform_data musb_hdrc_platform_data structure (line
+      11): config is a pointer to the configuration structure itself,
+      and mode tells the controller driver if the controller hardware
+      may be used as MUSB_HOST only, MUSB_PERIPHERAL only or MUSB_OTG
+      which is a dual mode.
+    </para>
+    <para>
+      Remember that jz4740_musb_platform_data is then used to convey
+      platform data information as we have seen in the probe function
+      in <link linkend="linux-musb-basics">Chapter 2</link>
+    </para>
+  </chapter>
+
+  <chapter id="device-quirks">
+    <title>Device Quirks</title>
+    <para>
+      Completing the platform data specific to your device, you may also
+      need to write some code in the glue layer to work around some
+      device specific limitations. These quirks may be due to some
+      hardware bugs, or simply be the result of an incomplete
+      implementation of the USB On-the-Go specification.
+    </para>
+    <para>
+      The JZ4740 UDC exhibits such quirks, some of which we will discuss
+      here for the sake of insight even though these might not be found
+      in the controller hardware you are working on.
+    </para>
+    <para>
+      Let's get back to the init function first:
+    </para>
+    <programlisting linenumbering="numbered">
+static int jz4740_musb_init(struct musb *musb)
+{
+	musb->xceiv = usb_get_phy(USB_PHY_TYPE_USB2);
+	if (!musb->xceiv) {
+		pr_err("HS UDC: no transceiver configured\n");
+		return -ENODEV;
+	}
+
+	/* Silicon does not implement ConfigData register.
+	 * Set dyn_fifo to avoid reading EP config from hardware.
+	 */
+	musb->dyn_fifo = true;
+
+	musb->isr = jz4740_musb_interrupt;
+
+	return 0;
+}
+    </programlisting>
+    <para>
+      Instruction on line 12 helps the MUSB controller driver to work
+      around the fact that the controller hardware is missing registers
+      that are used for USB endpoints configuration.
+    </para>
+    <para>
+      Without these registers, the controller driver is unable to read
+      the endpoints configuration from the hardware, so we use line 12
+      instruction to bypass reading the configuration from silicon, and
+      rely on a hard-coded table that describes the endpoints
+      configuration instead:
+    </para>
+    <programlisting linenumbering="numbered">
+static struct musb_fifo_cfg jz4740_musb_fifo_cfg[] = {
+{ .hw_ep_num = 1, .style = FIFO_TX, .maxpacket = 512, },
+{ .hw_ep_num = 1, .style = FIFO_RX, .maxpacket = 512, },
+{ .hw_ep_num = 2, .style = FIFO_TX, .maxpacket = 64, },
+};
+    </programlisting>
+    <para>
+      Looking at the configuration table above, we see that each
+      endpoints is described by three fields: hw_ep_num is the endpoint
+      number, style is its direction (either FIFO_TX for the controller
+      driver to send packets in the controller hardware, or FIFO_RX to
+      receive packets from hardware), and maxpacket defines the maximum
+      size of each data packet that can be transmitted over that
+      endpoint. Reading from the table, the controller driver knows that
+      endpoint 1 can be used to send and receive USB data packets of 512
+      bytes at once (this is in fact a bulk in/out endpoint), and
+      endpoint 2 can be used to send data packets of 64 bytes at once
+      (this is in fact an interrupt endpoint).
+    </para>
+    <para>
+      Note that there is no information about endpoint 0 here: that one
+      is implemented by default in every silicon design, with a
+      predefined configuration according to the USB specification. For
+      more examples of endpoint configuration tables, see musb_core.c.
+    </para>
+    <para>
+      Let's now get back to the interrupt handler function:
+    </para>
+    <programlisting linenumbering="numbered">
+static irqreturn_t jz4740_musb_interrupt(int irq, void *__hci)
+{
+	unsigned long   flags;
+	irqreturn_t     retval = IRQ_NONE;
+	struct musb     *musb = __hci;
+
+	spin_lock_irqsave(&amp;musb->lock, flags);
+
+	musb->int_usb = musb_readb(musb->mregs, MUSB_INTRUSB);
+	musb->int_tx = musb_readw(musb->mregs, MUSB_INTRTX);
+	musb->int_rx = musb_readw(musb->mregs, MUSB_INTRRX);
+
+	/*
+	 * The controller is gadget only, the state of the host mode IRQ bits is
+	 * undefined. Mask them to make sure that the musb driver core will
+	 * never see them set
+	 */
+	musb->int_usb &amp;= MUSB_INTR_SUSPEND | MUSB_INTR_RESUME |
+	    MUSB_INTR_RESET | MUSB_INTR_SOF;
+
+	if (musb->int_usb || musb->int_tx || musb->int_rx)
+		retval = musb_interrupt(musb);
+
+	spin_unlock_irqrestore(&amp;musb->lock, flags);
+
+	return retval;
+}
+    </programlisting>
+    <para>
+      Instruction on line 18 above is a way for the controller driver to
+      work around the fact that some interrupt bits used for USB host
+      mode operation are missing in the MUSB_INTRUSB register, thus left
+      in an undefined hardware state, since this MUSB controller
+      hardware is used in peripheral mode only. As a consequence, the
+      glue layer masks these missing bits out to avoid parasite
+      interrupts by doing a logical AND operation between the value read
+      from MUSB_INTRUSB and the bits that are actually implemented in
+      the register.
+    </para>
+    <para>
+      These are only a couple of the quirks found in the JZ4740 USB
+      device controller. Some others were directly addressed in the MUSB
+      core since the fixes were generic enough to provide a better
+      handling of the issues for others controller hardware eventually.
+    </para>
+  </chapter>
+
+  <chapter id="conclusion">
+    <title>Conclusion</title>
+    <para>
+      Writing a Linux MUSB glue layer should be a more accessible task,
+      as this documentation tries to show the ins and outs of this
+      exercise.
+    </para>
+    <para>
+      The JZ4740 USB device controller being fairly simple, I hope its
+      glue layer serves as a good example for the curious mind. Used
+      with the current MUSB glue layers, this documentation should
+      provide enough guidance to get started; should anything gets out
+      of hand, the linux-usb mailing list archive is another helpful
+      resource to browse through.
+    </para>
+  </chapter>
+
+  <chapter id="acknowledgements">
+    <title>Acknowledgements</title>
+    <para>
+      Many thanks to Lars-Peter Clausen and Maarten ter Huurne for
+      answering my questions while I was writing the JZ4740 glue layer
+      and for helping me out getting the code in good shape.
+    </para>
+    <para>
+      I would also like to thank the Qi-Hardware community at large for
+      its cheerful guidance and support.
+    </para>
+  </chapter>
+
+  <chapter id="resources">
+    <title>Resources</title>
+    <para>
+      USB Home Page:
+      <ulink url="http://www.usb.org">http://www.usb.org</ulink>
+    </para>
+    <para>
+      linux-usb Mailing List Archives:
+      <ulink url="http://marc.info/?l=linux-usb">http://marc.info/?l=linux-usb</ulink>
+    </para>
+    <para>
+      USB On-the-Go Basics:
+      <ulink url="http://www.maximintegrated.com/app-notes/index.mvp/id/1822">http://www.maximintegrated.com/app-notes/index.mvp/id/1822</ulink>
+    </para>
+    <para>
+      Writing USB Device Drivers:
+      <ulink url="https://www.kernel.org/doc/htmldocs/writing_usb_driver/index.html">https://www.kernel.org/doc/htmldocs/writing_usb_driver/index.html</ulink>
+    </para>
+    <para>
+      Texas Instruments USB Configuration Wiki Page:
+      <ulink url="http://processors.wiki.ti.com/index.php/Usbgeneralpage">http://processors.wiki.ti.com/index.php/Usbgeneralpage</ulink>
+    </para>
+    <para>
+      Analog Devices Blackfin MUSB Configuration:
+      <ulink url="http://docs.blackfin.uclinux.org/doku.php?id=linux-kernel:drivers:musb">http://docs.blackfin.uclinux.org/doku.php?id=linux-kernel:drivers:musb</ulink>
+    </para>
+  </chapter>
+
+</book>
diff --git a/Documentation/RCU/00-INDEX b/Documentation/RCU/00-INDEX
index fa57139f50b..f773a264ae0 100644
--- a/Documentation/RCU/00-INDEX
+++ b/Documentation/RCU/00-INDEX
@@ -12,6 +12,8 @@ lockdep-splat.txt
 	- RCU Lockdep splats explained.
 NMI-RCU.txt
 	- Using RCU to Protect Dynamic NMI Handlers
+rcu_dereference.txt
+	- Proper care and feeding of return values from rcu_dereference()
 rcubarrier.txt
 	- RCU and Unloadable Modules
 rculist_nulls.txt
diff --git a/Documentation/RCU/checklist.txt b/Documentation/RCU/checklist.txt
index 9d10d1db16a..877947130eb 100644
--- a/Documentation/RCU/checklist.txt
+++ b/Documentation/RCU/checklist.txt
@@ -114,12 +114,16 @@ over a rather long period of time, but improvements are always welcome!
 			http://www.openvms.compaq.com/wizard/wiz_2637.html
 
 		The rcu_dereference() primitive is also an excellent
-		documentation aid, letting the person reading the code
-		know exactly which pointers are protected by RCU.
+		documentation aid, letting the person reading the
+		code know exactly which pointers are protected by RCU.
 		Please note that compilers can also reorder code, and
 		they are becoming increasingly aggressive about doing
-		just that.  The rcu_dereference() primitive therefore
-		also prevents destructive compiler optimizations.
+		just that.  The rcu_dereference() primitive therefore also
+		prevents destructive compiler optimizations.  However,
+		with a bit of devious creativity, it is possible to
+		mishandle the return value from rcu_dereference().
+		Please see rcu_dereference.txt in this directory for
+		more information.
 
 		The rcu_dereference() primitive is used by the
 		various "_rcu()" list-traversal primitives, such
diff --git a/Documentation/RCU/rcu_dereference.txt b/Documentation/RCU/rcu_dereference.txt
new file mode 100644
index 00000000000..ceb05da5a5a
--- /dev/null
+++ b/Documentation/RCU/rcu_dereference.txt
@@ -0,0 +1,371 @@
+PROPER CARE AND FEEDING OF RETURN VALUES FROM rcu_dereference()
+
+Most of the time, you can use values from rcu_dereference() or one of
+the similar primitives without worries.  Dereferencing (prefix "*"),
+field selection ("->"), assignment ("="), address-of ("&"), addition and
+subtraction of constants, and casts all work quite naturally and safely.
+
+It is nevertheless possible to get into trouble with other operations.
+Follow these rules to keep your RCU code working properly:
+
+o	You must use one of the rcu_dereference() family of primitives
+	to load an RCU-protected pointer, otherwise CONFIG_PROVE_RCU
+	will complain.  Worse yet, your code can see random memory-corruption
+	bugs due to games that compilers and DEC Alpha can play.
+	Without one of the rcu_dereference() primitives, compilers
+	can reload the value, and won't your code have fun with two
+	different values for a single pointer!  Without rcu_dereference(),
+	DEC Alpha can load a pointer, dereference that pointer, and
+	return data preceding initialization that preceded the store of
+	the pointer.
+
+	In addition, the volatile cast in rcu_dereference() prevents the
+	compiler from deducing the resulting pointer value.  Please see
+	the section entitled "EXAMPLE WHERE THE COMPILER KNOWS TOO MUCH"
+	for an example where the compiler can in fact deduce the exact
+	value of the pointer, and thus cause misordering.
+
+o	Do not use single-element RCU-protected arrays.  The compiler
+	is within its right to assume that the value of an index into
+	such an array must necessarily evaluate to zero.  The compiler
+	could then substitute the constant zero for the computation, so
+	that the array index no longer depended on the value returned
+	by rcu_dereference().  If the array index no longer depends
+	on rcu_dereference(), then both the compiler and the CPU
+	are within their rights to order the array access before the
+	rcu_dereference(), which can cause the array access to return
+	garbage.
+
+o	Avoid cancellation when using the "+" and "-" infix arithmetic
+	operators.  For example, for a given variable "x", avoid
+	"(x-x)".  There are similar arithmetic pitfalls from other
+	arithmetic operatiors, such as "(x*0)", "(x/(x+1))" or "(x%1)".
+	The compiler is within its rights to substitute zero for all of
+	these expressions, so that subsequent accesses no longer depend
+	on the rcu_dereference(), again possibly resulting in bugs due
+	to misordering.
+
+	Of course, if "p" is a pointer from rcu_dereference(), and "a"
+	and "b" are integers that happen to be equal, the expression
+	"p+a-b" is safe because its value still necessarily depends on
+	the rcu_dereference(), thus maintaining proper ordering.
+
+o	Avoid all-zero operands to the bitwise "&" operator, and
+	similarly avoid all-ones operands to the bitwise "|" operator.
+	If the compiler is able to deduce the value of such operands,
+	it is within its rights to substitute the corresponding constant
+	for the bitwise operation.  Once again, this causes subsequent
+	accesses to no longer depend on the rcu_dereference(), causing
+	bugs due to misordering.
+
+	Please note that single-bit operands to bitwise "&" can also
+	be dangerous.  At this point, the compiler knows that the
+	resulting value can only take on one of two possible values.
+	Therefore, a very small amount of additional information will
+	allow the compiler to deduce the exact value, which again can
+	result in misordering.
+
+o	If you are using RCU to protect JITed functions, so that the
+	"()" function-invocation operator is applied to a value obtained
+	(directly or indirectly) from rcu_dereference(), you may need to
+	interact directly with the hardware to flush instruction caches.
+	This issue arises on some systems when a newly JITed function is
+	using the same memory that was used by an earlier JITed function.
+
+o	Do not use the results from the boolean "&&" and "||" when
+	dereferencing.	For example, the following (rather improbable)
+	code is buggy:
+
+		int a[2];
+		int index;
+		int force_zero_index = 1;
+
+		...
+
+		r1 = rcu_dereference(i1)
+		r2 = a[r1 && force_zero_index];  /* BUGGY!!! */
+
+	The reason this is buggy is that "&&" and "||" are often compiled
+	using branches.  While weak-memory machines such as ARM or PowerPC
+	do order stores after such branches, they can speculate loads,
+	which can result in misordering bugs.
+
+o	Do not use the results from relational operators ("==", "!=",
+	">", ">=", "<", or "<=") when dereferencing.  For example,
+	the following (quite strange) code is buggy:
+
+		int a[2];
+		int index;
+		int flip_index = 0;
+
+		...
+
+		r1 = rcu_dereference(i1)
+		r2 = a[r1 != flip_index];  /* BUGGY!!! */
+
+	As before, the reason this is buggy is that relational operators
+	are often compiled using branches.  And as before, although
+	weak-memory machines such as ARM or PowerPC do order stores
+	after such branches, but can speculate loads, which can again
+	result in misordering bugs.
+
+o	Be very careful about comparing pointers obtained from
+	rcu_dereference() against non-NULL values.  As Linus Torvalds
+	explained, if the two pointers are equal, the compiler could
+	substitute the pointer you are comparing against for the pointer
+	obtained from rcu_dereference().  For example:
+
+		p = rcu_dereference(gp);
+		if (p == &default_struct)
+			do_default(p->a);
+
+	Because the compiler now knows that the value of "p" is exactly
+	the address of the variable "default_struct", it is free to
+	transform this code into the following:
+
+		p = rcu_dereference(gp);
+		if (p == &default_struct)
+			do_default(default_struct.a);
+
+	On ARM and Power hardware, the load from "default_struct.a"
+	can now be speculated, such that it might happen before the
+	rcu_dereference().  This could result in bugs due to misordering.
+
+	However, comparisons are OK in the following cases:
+
+	o	The comparison was against the NULL pointer.  If the
+		compiler knows that the pointer is NULL, you had better
+		not be dereferencing it anyway.  If the comparison is
+		non-equal, the compiler is none the wiser.  Therefore,
+		it is safe to compare pointers from rcu_dereference()
+		against NULL pointers.
+
+	o	The pointer is never dereferenced after being compared.
+		Since there are no subsequent dereferences, the compiler
+		cannot use anything it learned from the comparison
+		to reorder the non-existent subsequent dereferences.
+		This sort of comparison occurs frequently when scanning
+		RCU-protected circular linked lists.
+
+	o	The comparison is against a pointer that references memory
+		that was initialized "a long time ago."  The reason
+		this is safe is that even if misordering occurs, the
+		misordering will not affect the accesses that follow
+		the comparison.  So exactly how long ago is "a long
+		time ago"?  Here are some possibilities:
+
+		o	Compile time.
+
+		o	Boot time.
+
+		o	Module-init time for module code.
+
+		o	Prior to kthread creation for kthread code.
+
+		o	During some prior acquisition of the lock that
+			we now hold.
+
+		o	Before mod_timer() time for a timer handler.
+
+		There are many other possibilities involving the Linux
+		kernel's wide array of primitives that cause code to
+		be invoked at a later time.
+
+	o	The pointer being compared against also came from
+		rcu_dereference().  In this case, both pointers depend
+		on one rcu_dereference() or another, so you get proper
+		ordering either way.
+
+		That said, this situation can make certain RCU usage
+		bugs more likely to happen.  Which can be a good thing,
+		at least if they happen during testing.  An example
+		of such an RCU usage bug is shown in the section titled
+		"EXAMPLE OF AMPLIFIED RCU-USAGE BUG".
+
+	o	All of the accesses following the comparison are stores,
+		so that a control dependency preserves the needed ordering.
+		That said, it is easy to get control dependencies wrong.
+		Please see the "CONTROL DEPENDENCIES" section of
+		Documentation/memory-barriers.txt for more details.
+
+	o	The pointers are not equal -and- the compiler does
+		not have enough information to deduce the value of the
+		pointer.  Note that the volatile cast in rcu_dereference()
+		will normally prevent the compiler from knowing too much.
+
+o	Disable any value-speculation optimizations that your compiler
+	might provide, especially if you are making use of feedback-based
+	optimizations that take data collected from prior runs.  Such
+	value-speculation optimizations reorder operations by design.
+
+	There is one exception to this rule:  Value-speculation
+	optimizations that leverage the branch-prediction hardware are
+	safe on strongly ordered systems (such as x86), but not on weakly
+	ordered systems (such as ARM or Power).  Choose your compiler
+	command-line options wisely!
+
+
+EXAMPLE OF AMPLIFIED RCU-USAGE BUG
+
+Because updaters can run concurrently with RCU readers, RCU readers can
+see stale and/or inconsistent values.  If RCU readers need fresh or
+consistent values, which they sometimes do, they need to take proper
+precautions.  To see this, consider the following code fragment:
+
+	struct foo {
+		int a;
+		int b;
+		int c;
+	};
+	struct foo *gp1;
+	struct foo *gp2;
+
+	void updater(void)
+	{
+		struct foo *p;
+
+		p = kmalloc(...);
+		if (p == NULL)
+			deal_with_it();
+		p->a = 42;  /* Each field in its own cache line. */
+		p->b = 43;
+		p->c = 44;
+		rcu_assign_pointer(gp1, p);
+		p->b = 143;
+		p->c = 144;
+		rcu_assign_pointer(gp2, p);
+	}
+
+	void reader(void)
+	{
+		struct foo *p;
+		struct foo *q;
+		int r1, r2;
+
+		p = rcu_dereference(gp2);
+		if (p == NULL)
+			return;
+		r1 = p->b;  /* Guaranteed to get 143. */
+		q = rcu_dereference(gp1);  /* Guaranteed non-NULL. */
+		if (p == q) {
+			/* The compiler decides that q->c is same as p->c. */
+			r2 = p->c; /* Could get 44 on weakly order system. */
+		}
+		do_something_with(r1, r2);
+	}
+
+You might be surprised that the outcome (r1 == 143 && r2 == 44) is possible,
+but you should not be.  After all, the updater might have been invoked
+a second time between the time reader() loaded into "r1" and the time
+that it loaded into "r2".  The fact that this same result can occur due
+to some reordering from the compiler and CPUs is beside the point.
+
+But suppose that the reader needs a consistent view?
+
+Then one approach is to use locking, for example, as follows:
+
+	struct foo {
+		int a;
+		int b;
+		int c;
+		spinlock_t lock;
+	};
+	struct foo *gp1;
+	struct foo *gp2;
+
+	void updater(void)
+	{
+		struct foo *p;
+
+		p = kmalloc(...);
+		if (p == NULL)
+			deal_with_it();
+		spin_lock(&p->lock);
+		p->a = 42;  /* Each field in its own cache line. */
+		p->b = 43;
+		p->c = 44;
+		spin_unlock(&p->lock);
+		rcu_assign_pointer(gp1, p);
+		spin_lock(&p->lock);
+		p->b = 143;
+		p->c = 144;
+		spin_unlock(&p->lock);
+		rcu_assign_pointer(gp2, p);
+	}
+
+	void reader(void)
+	{
+		struct foo *p;
+		struct foo *q;
+		int r1, r2;
+
+		p = rcu_dereference(gp2);
+		if (p == NULL)
+			return;
+		spin_lock(&p->lock);
+		r1 = p->b;  /* Guaranteed to get 143. */
+		q = rcu_dereference(gp1);  /* Guaranteed non-NULL. */
+		if (p == q) {
+			/* The compiler decides that q->c is same as p->c. */
+			r2 = p->c; /* Locking guarantees r2 == 144. */
+		}
+		spin_unlock(&p->lock);
+		do_something_with(r1, r2);
+	}
+
+As always, use the right tool for the job!
+
+
+EXAMPLE WHERE THE COMPILER KNOWS TOO MUCH
+
+If a pointer obtained from rcu_dereference() compares not-equal to some
+other pointer, the compiler normally has no clue what the value of the
+first pointer might be.  This lack of knowledge prevents the compiler
+from carrying out optimizations that otherwise might destroy the ordering
+guarantees that RCU depends on.  And the volatile cast in rcu_dereference()
+should prevent the compiler from guessing the value.
+
+But without rcu_dereference(), the compiler knows more than you might
+expect.  Consider the following code fragment:
+
+	struct foo {
+		int a;
+		int b;
+	};
+	static struct foo variable1;
+	static struct foo variable2;
+	static struct foo *gp = &variable1;
+
+	void updater(void)
+	{
+		initialize_foo(&variable2);
+		rcu_assign_pointer(gp, &variable2);
+		/*
+		 * The above is the only store to gp in this translation unit,
+		 * and the address of gp is not exported in any way.
+		 */
+	}
+
+	int reader(void)
+	{
+		struct foo *p;
+
+		p = gp;
+		barrier();
+		if (p == &variable1)
+			return p->a; /* Must be variable1.a. */
+		else
+			return p->b; /* Must be variable2.b. */
+	}
+
+Because the compiler can see all stores to "gp", it knows that the only
+possible values of "gp" are "variable1" on the one hand and "variable2"
+on the other.  The comparison in reader() therefore tells the compiler
+the exact value of "p" even in the not-equals case.  This allows the
+compiler to make the return values independent of the load from "gp",
+in turn destroying the ordering between this load and the loads of the
+return values.  This can result in "p->b" returning pre-initialization
+garbage values.
+
+In short, rcu_dereference() is -not- optional when you are going to
+dereference the resulting pointer.
diff --git a/Documentation/RCU/stallwarn.txt b/Documentation/RCU/stallwarn.txt
index 6f3a0057548..68fe3ad2701 100644
--- a/Documentation/RCU/stallwarn.txt
+++ b/Documentation/RCU/stallwarn.txt
@@ -24,7 +24,7 @@ CONFIG_RCU_CPU_STALL_TIMEOUT
 	timing of the next warning for the current stall.
 
 	Stall-warning messages may be enabled and disabled completely via
-	/sys/module/rcutree/parameters/rcu_cpu_stall_suppress.
+	/sys/module/rcupdate/parameters/rcu_cpu_stall_suppress.
 
 CONFIG_RCU_CPU_STALL_VERBOSE
 
diff --git a/Documentation/RCU/whatisRCU.txt b/Documentation/RCU/whatisRCU.txt
index 0f0fb7c432c..49b8551a3b6 100644
--- a/Documentation/RCU/whatisRCU.txt
+++ b/Documentation/RCU/whatisRCU.txt
@@ -326,11 +326,11 @@ used as follows:
 a.	synchronize_rcu()	rcu_read_lock() / rcu_read_unlock()
 	call_rcu()		rcu_dereference()
 
-b.	call_rcu_bh()		rcu_read_lock_bh() / rcu_read_unlock_bh()
-				rcu_dereference_bh()
+b.	synchronize_rcu_bh()	rcu_read_lock_bh() / rcu_read_unlock_bh()
+	call_rcu_bh()		rcu_dereference_bh()
 
 c.	synchronize_sched()	rcu_read_lock_sched() / rcu_read_unlock_sched()
-				preempt_disable() / preempt_enable()
+	call_rcu_sched()	preempt_disable() / preempt_enable()
 				local_irq_save() / local_irq_restore()
 				hardirq enter / hardirq exit
 				NMI enter / NMI exit
@@ -794,10 +794,22 @@ in docbook.  Here is the list, by category.
 
 RCU list traversal:
 
+	list_entry_rcu
+	list_first_entry_rcu
+	list_next_rcu
 	list_for_each_entry_rcu
+	list_for_each_entry_continue_rcu
+	hlist_first_rcu
+	hlist_next_rcu
+	hlist_pprev_rcu
 	hlist_for_each_entry_rcu
+	hlist_for_each_entry_rcu_bh
+	hlist_for_each_entry_continue_rcu
+	hlist_for_each_entry_continue_rcu_bh
+	hlist_nulls_first_rcu
 	hlist_nulls_for_each_entry_rcu
-	list_for_each_entry_continue_rcu
+	hlist_bl_first_rcu
+	hlist_bl_for_each_entry_rcu
 
 RCU pointer/list update:
 
@@ -806,28 +818,38 @@ RCU pointer/list update:
 	list_add_tail_rcu
 	list_del_rcu
 	list_replace_rcu
-	hlist_del_rcu
 	hlist_add_after_rcu
 	hlist_add_before_rcu
 	hlist_add_head_rcu
+	hlist_del_rcu
+	hlist_del_init_rcu
 	hlist_replace_rcu
 	list_splice_init_rcu()
+	hlist_nulls_del_init_rcu
+	hlist_nulls_del_rcu
+	hlist_nulls_add_head_rcu
+	hlist_bl_add_head_rcu
+	hlist_bl_del_init_rcu
+	hlist_bl_del_rcu
+	hlist_bl_set_first_rcu
 
 RCU:	Critical sections	Grace period		Barrier
 
 	rcu_read_lock		synchronize_net		rcu_barrier
 	rcu_read_unlock		synchronize_rcu
 	rcu_dereference		synchronize_rcu_expedited
-				call_rcu
-				kfree_rcu
-
+	rcu_read_lock_held	call_rcu
+	rcu_dereference_check	kfree_rcu
+	rcu_dereference_protected
 
 bh:	Critical sections	Grace period		Barrier
 
 	rcu_read_lock_bh	call_rcu_bh		rcu_barrier_bh
 	rcu_read_unlock_bh	synchronize_rcu_bh
 	rcu_dereference_bh	synchronize_rcu_bh_expedited
-
+	rcu_dereference_bh_check
+	rcu_dereference_bh_protected
+	rcu_read_lock_bh_held
 
 sched:	Critical sections	Grace period		Barrier
 
@@ -835,7 +857,12 @@ sched:	Critical sections	Grace period		Barrier
 	rcu_read_unlock_sched	call_rcu_sched
 	[preempt_disable]	synchronize_sched_expedited
 	[and friends]
+	rcu_read_lock_sched_notrace
+	rcu_read_unlock_sched_notrace
 	rcu_dereference_sched
+	rcu_dereference_sched_check
+	rcu_dereference_sched_protected
+	rcu_read_lock_sched_held
 
 
 SRCU:	Critical sections	Grace period		Barrier
@@ -843,6 +870,8 @@ SRCU:	Critical sections	Grace period		Barrier
 	srcu_read_lock		synchronize_srcu	srcu_barrier
 	srcu_read_unlock	call_srcu
 	srcu_dereference	synchronize_srcu_expedited
+	srcu_dereference_check
+	srcu_read_lock_held
 
 SRCU:	Initialization/cleanup
 	init_srcu_struct
@@ -850,9 +879,13 @@ SRCU:	Initialization/cleanup
 
 All:  lockdep-checked RCU-protected pointer access
 
-	rcu_dereference_check
-	rcu_dereference_protected
+	rcu_access_index
 	rcu_access_pointer
+	rcu_dereference_index_check
+	rcu_dereference_raw
+	rcu_lockdep_assert
+	rcu_sleep_check
+	RCU_NONIDLE
 
 See the comment headers in the source code (or the docbook generated
 from them) for more information.
diff --git a/Documentation/acpi/enumeration.txt b/Documentation/acpi/enumeration.txt
index 2a1519b8717..fd786ea13a1 100644
--- a/Documentation/acpi/enumeration.txt
+++ b/Documentation/acpi/enumeration.txt
@@ -296,7 +296,7 @@ specifies the path to the controller. In order to use these GPIOs in Linux
 we need to translate them to the corresponding Linux GPIO descriptors.
 
 There is a standard GPIO API for that and is documented in
-Documentation/gpio.txt.
+Documentation/gpio/.
 
 In the above example we can get the corresponding two GPIO descriptors with
 a code like this:
diff --git a/Documentation/arm/Marvell/README b/Documentation/arm/Marvell/README
index 963ec445e15..2cce5401e32 100644
--- a/Documentation/arm/Marvell/README
+++ b/Documentation/arm/Marvell/README
@@ -234,6 +234,11 @@ Berlin family (Digital Entertainment)
 		Core:		Marvell PJ4B (ARMv7), Tauros3 L2CC
 		Homepage:	http://www.marvell.com/digital-entertainment/armada-1500/
 		Product Brief:	http://www.marvell.com/digital-entertainment/armada-1500/assets/Marvell-ARMADA-1500-Product-Brief.pdf
+	88DE3114, Armada 1500 Pro
+		Design name:	BG2-Q
+		Core:		Quad Core ARM Cortex-A9, PL310 L2CC
+		Homepage:	http://www.marvell.com/digital-entertainment/armada-1500-pro/
+		Product Brief:	http://www.marvell.com/digital-entertainment/armada-1500-pro/assets/Marvell_ARMADA_1500_PRO-01_product_brief.pdf
 	88DE????
 		Design name:	BG3
 		Core:		ARM Cortex-A15, CA15 integrated L2CC
diff --git a/Documentation/arm/sti/stih407-overview.txt b/Documentation/arm/sti/stih407-overview.txt
new file mode 100644
index 00000000000..3343f32f58b
--- /dev/null
+++ b/Documentation/arm/sti/stih407-overview.txt
@@ -0,0 +1,18 @@
+			STiH407 Overview
+			================
+
+Introduction
+------------
+
+    The STiH407 is the new generation of SoC for Multi-HD, AVC set-top boxes
+    and server/connected client application for satellite, cable, terrestrial
+    and IP-STB markets.
+
+    Features
+    - ARM Cortex-A9 1.5 GHz dual core CPU (28nm)
+    - SATA2, USB 3.0, PCIe, Gbit Ethernet
+
+  Document Author
+  ---------------
+
+  Maxime Coquelin <maxime.coquelin@st.com>, (c) 2014 ST Microelectronics
diff --git a/Documentation/connector/connector.txt b/Documentation/connector/connector.txt
index e5c5f5e6ab7..f6215f95149 100644
--- a/Documentation/connector/connector.txt
+++ b/Documentation/connector/connector.txt
@@ -24,7 +24,8 @@ netlink based networking for inter-process communication in a significantly
 easier way:
 
 int cn_add_callback(struct cb_id *id, char *name, void (*callback) (struct cn_msg *, struct netlink_skb_parms *));
-void cn_netlink_send(struct cn_msg *msg, u32 __group, int gfp_mask);
+void cn_netlink_send_multi(struct cn_msg *msg, u16 len, u32 portid, u32 __group, int gfp_mask);
+void cn_netlink_send(struct cn_msg *msg, u32 portid, u32 __group, int gfp_mask);
 
 struct cb_id
 {
@@ -71,15 +72,21 @@ void cn_del_callback(struct cb_id *id);
  struct cb_id *id		- unique connector's user identifier.
 
 
-int cn_netlink_send(struct cn_msg *msg, u32 __groups, int gfp_mask);
+int cn_netlink_send_multi(struct cn_msg *msg, u16 len, u32 portid, u32 __groups, int gfp_mask);
+int cn_netlink_send(struct cn_msg *msg, u32 portid, u32 __groups, int gfp_mask);
 
  Sends message to the specified groups.  It can be safely called from
  softirq context, but may silently fail under strong memory pressure.
  If there are no listeners for given group -ESRCH can be returned.
 
  struct cn_msg *		- message header(with attached data).
+ u16 len			- for *_multi multiple cn_msg messages can be sent
+ u32 port			- destination port.
+ 				  If non-zero the message will be sent to the
+				  given port, which should be set to the
+				  original sender.
  u32 __group			- destination group.
-				  If __group is zero, then appropriate group will
+				  If port and __group is zero, then appropriate group will
 				  be searched through all registered connector users,
 				  and message will be delivered to the group which was
 				  created for user with the same ID as in msg.
@@ -111,7 +118,7 @@ acknowledge number MUST be the same + 1.
 If we receive a message and its sequence number is not equal to one we
 are expecting, then it is a new message.  If we receive a message and
 its sequence number is the same as one we are expecting, but its
-acknowledge is not equal to the acknowledge number in the original
+acknowledge is not equal to the sequence number in the original
 message + 1, then it is a new message.
 
 Obviously, the protocol header contains the above id.
diff --git a/Documentation/debugging-via-ohci1394.txt b/Documentation/debugging-via-ohci1394.txt
index fa0151a712f..5c9a567b3fa 100644
--- a/Documentation/debugging-via-ohci1394.txt
+++ b/Documentation/debugging-via-ohci1394.txt
@@ -25,9 +25,11 @@ using data transfer rates in the order of 10MB/s or more.
 With most FireWire controllers, memory access is limited to the low 4 GB
 of physical address space.  This can be a problem on IA64 machines where
 memory is located mostly above that limit, but it is rarely a problem on
-more common hardware such as x86, x86-64 and PowerPC.  However, at least
-Agere/LSI FW643e and FW643e2 controllers are known to support access to
-physical addresses above 4 GB.
+more common hardware such as x86, x86-64 and PowerPC.
+
+At least LSI FW643e and FW643e2 controllers are known to support access to
+physical addresses above 4 GB, but this feature is currently not enabled by
+Linux.
 
 Together with a early initialization of the OHCI-1394 controller for debugging,
 this facility proved most useful for examining long debugs logs in the printk
@@ -101,8 +103,9 @@ Step-by-step instructions for using firescope with early OHCI initialization:
    compliant, they are based on TI PCILynx chips and require drivers for Win-
    dows operating systems.
 
-   The mentioned kernel log message contains ">4 GB phys DMA" in case of
-   OHCI-1394 controllers which support accesses above this limit.
+   The mentioned kernel log message contains the string "physUB" if the
+   controller implements a writable Physical Upper Bound register.  This is
+   required for physical DMA above 4 GB (but not utilized by Linux yet).
 
 2) Establish a working FireWire cable connection:
 
diff --git a/Documentation/device-mapper/thin-provisioning.txt b/Documentation/device-mapper/thin-provisioning.txt
index 05a27e9442b..2f5173500bd 100644
--- a/Documentation/device-mapper/thin-provisioning.txt
+++ b/Documentation/device-mapper/thin-provisioning.txt
@@ -309,7 +309,10 @@ ii) Status
     error_if_no_space|queue_if_no_space
 	If the pool runs out of data or metadata space, the pool will
 	either queue or error the IO destined to the data device.  The
-	default is to queue the IO until more space is added.
+	default is to queue the IO until more space is added or the
+	'no_space_timeout' expires.  The 'no_space_timeout' dm-thin-pool
+	module parameter can be used to change this timeout -- it
+	defaults to 60 seconds but may be disabled using a value of 0.
 
 iii) Messages
 
diff --git a/Documentation/devicetree/bindings/arm/arch_timer.txt b/Documentation/devicetree/bindings/arm/arch_timer.txt
index 06fc7602593..37b2cafa4e5 100644
--- a/Documentation/devicetree/bindings/arm/arch_timer.txt
+++ b/Documentation/devicetree/bindings/arm/arch_timer.txt
@@ -19,6 +19,9 @@ to deliver its interrupts via SPIs.
 
 - clock-frequency : The frequency of the main counter, in Hz. Optional.
 
+- always-on : a boolean property. If present, the timer is powered through an
+  always-on power domain, therefore it never loses context.
+
 Example:
 
 	timer {
diff --git a/Documentation/devicetree/bindings/arm/armada-370-xp-pmsu.txt b/Documentation/devicetree/bindings/arm/armada-370-xp-pmsu.txt
index 926b4d6aae7..26799ef562d 100644
--- a/Documentation/devicetree/bindings/arm/armada-370-xp-pmsu.txt
+++ b/Documentation/devicetree/bindings/arm/armada-370-xp-pmsu.txt
@@ -1,20 +1,21 @@
 Power Management Service Unit(PMSU)
 -----------------------------------
-Available on Marvell SOCs: Armada 370 and Armada XP
+Available on Marvell SOCs: Armada 370, Armada 38x and Armada XP
 
 Required properties:
 
-- compatible: "marvell,armada-370-xp-pmsu"
+- compatible: should be one of:
+  - "marvell,armada-370-pmsu" for Armada 370 or Armada XP
+  - "marvell,armada-380-pmsu" for Armada 38x
+  - "marvell,armada-370-xp-pmsu" was used for Armada 370/XP but is now
+    deprecated and will be removed
 
-- reg: Should contain PMSU registers location and length. First pair
-  for the per-CPU SW Reset Control registers, second pair for the
-  Power Management Service Unit.
+- reg: Should contain PMSU registers location and length.
 
 Example:
 
-armada-370-xp-pmsu@d0022000 {
-	compatible = "marvell,armada-370-xp-pmsu";
-	reg = <0xd0022100 0x430>,
-	      <0xd0020800 0x20>;
+armada-370-xp-pmsu@22000 {
+	compatible = "marvell,armada-370-pmsu";
+	reg = <0x22000 0x1000>;
 };
 
diff --git a/Documentation/devicetree/bindings/arm/armada-cpu-reset.txt b/Documentation/devicetree/bindings/arm/armada-cpu-reset.txt
new file mode 100644
index 00000000000..b63a7b6ab99
--- /dev/null
+++ b/Documentation/devicetree/bindings/arm/armada-cpu-reset.txt
@@ -0,0 +1,14 @@
+Marvell Armada CPU reset controller
+===================================
+
+Required properties:
+
+- compatible: Should be "marvell,armada-370-cpu-reset".
+
+- reg: should be register base and length as documented in the
+  datasheet for the CPU reset registers
+
+cpurst: cpurst@20800 {
+       compatible = "marvell,armada-370-cpu-reset";
+       reg = <0x20800 0x20>;
+};
diff --git a/Documentation/devicetree/bindings/arm/axxia.txt b/Documentation/devicetree/bindings/arm/axxia.txt
new file mode 100644
index 00000000000..7b4ef9c0769
--- /dev/null
+++ b/Documentation/devicetree/bindings/arm/axxia.txt
@@ -0,0 +1,12 @@
+Axxia AXM55xx device tree bindings
+
+Boards using the AXM55xx SoC need to have the following properties:
+
+Required root node property:
+
+  - compatible = "lsi,axm5516"
+
+Boards:
+
+  LSI AXM5516 Validation board (Amarillo)
+	compatible = "lsi,axm5516-amarillo", "lsi,axm5516"
diff --git a/Documentation/devicetree/bindings/arm/coherency-fabric.txt b/Documentation/devicetree/bindings/arm/coherency-fabric.txt
index 17d8cd10755..8dd46617c88 100644
--- a/Documentation/devicetree/bindings/arm/coherency-fabric.txt
+++ b/Documentation/devicetree/bindings/arm/coherency-fabric.txt
@@ -1,16 +1,33 @@
 Coherency fabric
 ----------------
-Available on Marvell SOCs: Armada 370 and Armada XP
+Available on Marvell SOCs: Armada 370, Armada 375, Armada 38x and Armada XP
 
 Required properties:
 
-- compatible: "marvell,coherency-fabric"
+- compatible: the possible values are:
+
+ * "marvell,coherency-fabric", to be used for the coherency fabric of
+   the Armada 370 and Armada XP.
+
+ * "marvell,armada-375-coherency-fabric", for the Armada 375 coherency
+   fabric.
+
+ * "marvell,armada-380-coherency-fabric", for the Armada 38x coherency
+   fabric.
 
 - reg: Should contain coherency fabric registers location and
-  length. First pair for the coherency fabric registers, second pair
-  for the per-CPU fabric registers registers.
+  length.
+
+ * For "marvell,coherency-fabric", the first pair for the coherency
+   fabric registers, second pair for the per-CPU fabric registers.
 
-Example:
+ * For "marvell,armada-375-coherency-fabric", only one pair is needed
+   for the per-CPU fabric registers.
+
+ * For "marvell,armada-380-coherency-fabric", only one pair is needed
+   for the per-CPU fabric registers.
+
+Examples:
 
 coherency-fabric@d0020200 {
 	compatible = "marvell,coherency-fabric";
@@ -19,3 +36,8 @@ coherency-fabric@d0020200 {
 
 };
 
+coherency-fabric@21810 {
+	compatible = "marvell,armada-375-coherency-fabric";
+	reg = <0x21810 0x1c>;
+};
+
diff --git a/Documentation/devicetree/bindings/arm/cpus.txt b/Documentation/devicetree/bindings/arm/cpus.txt
index 333f4aea302..1fe72a0778c 100644
--- a/Documentation/devicetree/bindings/arm/cpus.txt
+++ b/Documentation/devicetree/bindings/arm/cpus.txt
@@ -178,13 +178,19 @@ nodes to be present and contain the properties described below.
 		Usage and definition depend on ARM architecture version.
 			# On ARM v8 64-bit this property is required and must
 			  be one of:
-			     "spin-table"
 			     "psci"
+			     "spin-table"
 			# On ARM 32-bit systems this property is optional and
 			  can be one of:
+			    "allwinner,sun6i-a31"
+			    "arm,psci"
+			    "marvell,armada-375-smp"
+			    "marvell,armada-380-smp"
+			    "marvell,armada-xp-smp"
 			    "qcom,gcc-msm8660"
 			    "qcom,kpss-acc-v1"
 			    "qcom,kpss-acc-v2"
+			    "rockchip,rk3066-smp"
 
 	- cpu-release-addr
 		Usage: required for systems that have an "enable-method"
diff --git a/Documentation/devicetree/bindings/arm/exynos/smp-sysram.txt b/Documentation/devicetree/bindings/arm/exynos/smp-sysram.txt
new file mode 100644
index 00000000000..4a0a4f70a0c
--- /dev/null
+++ b/Documentation/devicetree/bindings/arm/exynos/smp-sysram.txt
@@ -0,0 +1,38 @@
+Samsung Exynos SYSRAM for SMP bringup:
+------------------------------------
+
+Samsung SMP-capable Exynos SoCs use part of the SYSRAM for the bringup
+of the secondary cores. Once the core gets powered up it executes the
+code that is residing at some specific location of the SYSRAM.
+
+Therefore reserved section sub-nodes have to be added to the mmio-sram
+declaration. These nodes are of two types depending upon secure or
+non-secure execution environment.
+
+Required sub-node properties:
+- compatible : depending upon boot mode, should be
+		"samsung,exynos4210-sysram" : for Secure SYSRAM
+		"samsung,exynos4210-sysram-ns" : for Non-secure SYSRAM
+
+The rest of the properties should follow the generic mmio-sram discription
+found in ../../misc/sysram.txt
+
+Example:
+
+	sysram@02020000 {
+		compatible = "mmio-sram";
+		reg = <0x02020000 0x54000>;
+		#address-cells = <1>;
+		#size-cells = <1>;
+		ranges = <0 0x02020000 0x54000>;
+
+		smp-sysram@0 {
+			compatible = "samsung,exynos4210-sysram";
+			reg = <0x0 0x1000>;
+		};
+
+		smp-sysram@53000 {
+			compatible = "samsung,exynos4210-sysram-ns";
+			reg = <0x53000 0x1000>;
+		};
+	};
diff --git a/Documentation/devicetree/bindings/arm/marvell,berlin.txt b/Documentation/devicetree/bindings/arm/marvell,berlin.txt
index 737afa5f814..94013a9a876 100644
--- a/Documentation/devicetree/bindings/arm/marvell,berlin.txt
+++ b/Documentation/devicetree/bindings/arm/marvell,berlin.txt
@@ -12,6 +12,7 @@ SoC and board used. Currently known SoC compatibles are:
     "marvell,berlin2"      for Marvell Armada 1500 (BG2, 88DE3100),
     "marvell,berlin2cd"    for Marvell Armada 1500-mini (BG2CD, 88DE3005)
     "marvell,berlin2ct"    for Marvell Armada ? (BG2CT, 88DE????)
+    "marvell,berlin2q"     for Marvell Armada 1500-pro (BG2Q, 88DE3114)
     "marvell,berlin3"      for Marvell Armada ? (BG3, 88DE????)
 
 * Example:
@@ -22,3 +23,104 @@ SoC and board used. Currently known SoC compatibles are:
 
 	...
 }
+
+* Marvell Berlin2 chip control binding
+
+Marvell Berlin SoCs have a chip control register set providing several
+individual registers dealing with pinmux, padmux, clock, reset, and secondary
+CPU boot address. Unfortunately, the individual registers are spread among the
+chip control registers, so there should be a single DT node only providing the
+different functions which are described below.
+
+Required properties:
+- compatible: shall be one of
+	"marvell,berlin2-chip-ctrl" for BG2
+	"marvell,berlin2cd-chip-ctrl" for BG2CD
+	"marvell,berlin2q-chip-ctrl" for BG2Q
+- reg: address and length of following register sets for
+  BG2/BG2CD: chip control register set
+  BG2Q: chip control register set and cpu pll registers
+
+* Marvell Berlin2 system control binding
+
+Marvell Berlin SoCs have a system control register set providing several
+individual registers dealing with pinmux, padmux, and reset.
+
+Required properties:
+- compatible: should be one of
+	"marvell,berlin2-system-ctrl" for BG2
+	"marvell,berlin2cd-system-ctrl" for BG2CD
+	"marvell,berlin2q-system-ctrl" for BG2Q
+- reg: address and length of the system control register set
+
+* Clock provider binding
+
+As clock related registers are spread among the chip control registers, the
+chip control node also provides the clocks. Marvell Berlin2 (BG2, BG2CD, BG2Q)
+SoCs share the same IP for PLLs and clocks, with some minor differences in
+features and register layout.
+
+Required properties:
+- #clock-cells: shall be set to 1
+- clocks: clock specifiers referencing the core clock input clocks
+- clock-names: array of strings describing the input clock specifiers above.
+    Allowed clock-names for the reference clocks are
+      "refclk" for the SoCs osciallator input on all SoCs,
+    and SoC-specific input clocks for
+      BG2/BG2CD: "video_ext0" for the external video clock input
+
+Clocks provided by core clocks shall be referenced by a clock specifier
+indexing one of the provided clocks. Refer to dt-bindings/clock/berlin<soc>.h
+for the corresponding index mapping.
+
+* Pin controller binding
+
+Pin control registers are part of both register sets, chip control and system
+control. The pins controlled are organized in groups, so no actual pin
+information is needed.
+
+A pin-controller node should contain subnodes representing the pin group
+configurations, one per function. Each subnode has the group name and the muxing
+function used.
+
+Be aware the Marvell Berlin datasheets use the keyword 'mode' for what is called
+a 'function' in the pin-controller subsystem.
+
+Required subnode-properties:
+- groups: a list of strings describing the group names.
+- function: a string describing the function used to mux the groups.
+
+Example:
+
+chip: chip-control@ea0000 {
+	compatible = "marvell,berlin2-chip-ctrl";
+	#clock-cells = <1>;
+	reg = <0xea0000 0x400>;
+	clocks = <&refclk>, <&externaldev 0>;
+	clock-names = "refclk", "video_ext0";
+
+	spi1_pmux: spi1-pmux {
+		groups = "G0";
+		function = "spi1";
+	};
+};
+
+sysctrl: system-controller@d000 {
+	compatible = "marvell,berlin2-system-ctrl";
+	reg = <0xd000 0x100>;
+
+	uart0_pmux: uart0-pmux {
+		groups = "GSM4";
+		function = "uart0";
+	};
+
+	uart1_pmux: uart1-pmux {
+		groups = "GSM5";
+		function = "uart1";
+	};
+
+	uart2_pmux: uart2-pmux {
+		groups = "GSM3";
+		function = "uart2";
+	};
+};
diff --git a/Documentation/devicetree/bindings/arm/marvell,kirkwood.txt b/Documentation/devicetree/bindings/arm/marvell,kirkwood.txt
new file mode 100644
index 00000000000..925ecbf6e7b
--- /dev/null
+++ b/Documentation/devicetree/bindings/arm/marvell,kirkwood.txt
@@ -0,0 +1,97 @@
+Marvell Kirkwood SoC Family Device Tree Bindings
+------------------------------------------------
+
+Boards with a SoC of the Marvell Kirkwook family, eg 88f6281
+
+* Required root node properties:
+compatible: must contain "marvell,kirkwood"
+
+In addition, the above compatible shall be extended with the specific
+SoC. Currently known SoC compatibles are:
+
+"marvell,kirkwood-88f6192"
+"marvell,kirkwood-88f6281"
+"marvell,kirkwood-88f6282"
+"marvell,kirkwood-88f6283"
+"marvell,kirkwood-88f6702"
+"marvell,kirkwood-98DX4122"
+
+And in addition, the compatible shall be extended with the specific
+board. Currently known boards are:
+
+"buffalo,lschlv2"
+"buffalo,lsxhl"
+"buffalo,lsxl"
+"dlink,dns-320"
+"dlink,dns-320-a1"
+"dlink,dns-325"
+"dlink,dns-325-a1"
+"dlink,dns-kirkwood"
+"excito,b3"
+"globalscale,dreamplug-003-ds2001"
+"globalscale,guruplug"
+"globalscale,guruplug-server-plus"
+"globalscale,sheevaplug"
+"globalscale,sheevaplug"
+"globalscale,sheevaplug-esata"
+"globalscale,sheevaplug-esata-rev13"
+"iom,iconnect"
+"iom,iconnect-1.1"
+"iom,ix2-200"
+"keymile,km_kirkwood"
+"lacie,cloudbox"
+"lacie,inetspace_v2"
+"lacie,laplug"
+"lacie,netspace_lite_v2"
+"lacie,netspace_max_v2"
+"lacie,netspace_mini_v2"
+"lacie,netspace_v2"
+"marvell,db-88f6281-bp"
+"marvell,db-88f6282-bp"
+"marvell,mv88f6281gtw-ge"
+"marvell,rd88f6281"
+"marvell,rd88f6281"
+"marvell,rd88f6281-a0"
+"marvell,rd88f6281-a1"
+"mpl,cec4"
+"mpl,cec4-10"
+"netgear,readynas"
+"netgear,readynas"
+"netgear,readynas-duo-v2"
+"netgear,readynas-nv+-v2"
+"plathome,openblocks-a6"
+"plathome,openblocks-a7"
+"raidsonic,ib-nas6210"
+"raidsonic,ib-nas6210-b"
+"raidsonic,ib-nas6220"
+"raidsonic,ib-nas6220-b"
+"raidsonic,ib-nas62x0"
+"seagate,dockstar"
+"seagate,goflexnet"
+"synology,ds109"
+"synology,ds110jv10"
+"synology,ds110jv20"
+"synology,ds110jv30"
+"synology,ds111"
+"synology,ds209"
+"synology,ds210jv10"
+"synology,ds210jv20"
+"synology,ds212"
+"synology,ds212jv10"
+"synology,ds212jv20"
+"synology,ds212pv10"
+"synology,ds409"
+"synology,ds409slim"
+"synology,ds410j"
+"synology,ds411"
+"synology,ds411j"
+"synology,ds411slim"
+"synology,ds413jv10"
+"synology,rs212"
+"synology,rs409"
+"synology,rs411"
+"synology,rs812"
+"usi,topkick"
+"usi,topkick-1281P2"
+"zyxel,nsa310"
+"zyxel,nsa310a"
diff --git a/Documentation/devicetree/bindings/arm/omap/l3-noc.txt b/Documentation/devicetree/bindings/arm/omap/l3-noc.txt
index c0105de55cb..974624ea68f 100644
--- a/Documentation/devicetree/bindings/arm/omap/l3-noc.txt
+++ b/Documentation/devicetree/bindings/arm/omap/l3-noc.txt
@@ -6,6 +6,8 @@ provided by Arteris.
 Required properties:
 - compatible : Should be "ti,omap3-l3-smx" for OMAP3 family
                Should be "ti,omap4-l3-noc" for OMAP4 family
+	       Should be "ti,dra7-l3-noc" for DRA7 family
+               Should be "ti,am4372-l3-noc" for AM43 family
 - reg:	Contains L3 register address range for each noc domain.
 - ti,hwmods: "l3_main_1", ... One hwmod for each noc domain.
 
diff --git a/Documentation/devicetree/bindings/arm/omap/omap.txt b/Documentation/devicetree/bindings/arm/omap/omap.txt
index 36ede19a163..189baba40cd 100644
--- a/Documentation/devicetree/bindings/arm/omap/omap.txt
+++ b/Documentation/devicetree/bindings/arm/omap/omap.txt
@@ -80,7 +80,10 @@ SoCs:
   compatible = "ti,omap5432", "ti,omap5"
 
 - DRA742
-  compatible = "ti,dra7xx", "ti,dra7"
+  compatible = "ti,dra742", "ti,dra74", "ti,dra7"
+
+- DRA722
+  compatible = "ti,dra722", "ti,dra72", "ti,dra7"
 
 - AM4372
   compatible = "ti,am4372", "ti,am43"
@@ -102,6 +105,12 @@ Boards:
 - OMAP4 DuoVero with Parlor : Commercial expansion board with daughter board
   compatible = "gumstix,omap4-duovero-parlor", "gumstix,omap4-duovero", "ti,omap4430", "ti,omap4";
 
+- OMAP4 VAR-STK-OM44 : Commercial dev kit with VAR-OM44CustomBoard and VAR-SOM-OM44 w/WLAN
+  compatible = "variscite,var-stk-om44", "variscite,var-som-om44", "ti,omap4460", "ti,omap4";
+
+- OMAP4 VAR-DVK-OM44 : Commercial dev kit with VAR-OM44CustomBoard, VAR-SOM-OM44 w/WLAN and LCD touchscreen
+  compatible = "variscite,var-dvk-om44", "variscite,var-som-om44", "ti,omap4460", "ti,omap4";
+
 - OMAP3 EVM : Software Development Board for OMAP35x, AM/DM37x
   compatible = "ti,omap3-evm", "ti,omap3"
 
@@ -120,5 +129,8 @@ Boards:
 - AM437x GP EVM
   compatible = "ti,am437x-gp-evm", "ti,am4372", "ti,am43"
 
-- DRA7 EVM:  Software Developement Board for DRA7XX
-  compatible = "ti,dra7-evm", "ti,dra7"
+- DRA742 EVM:  Software Developement Board for DRA742
+  compatible = "ti,dra7-evm", "ti,dra742", "ti,dra74", "ti,dra7"
+
+- DRA722 EVM: Software Development Board for DRA722
+  compatible = "ti,dra72-evm", "ti,dra722", "ti,dra72", "ti,dra7"
diff --git a/Documentation/devicetree/bindings/arm/rockchip.txt b/Documentation/devicetree/bindings/arm/rockchip.txt
new file mode 100644
index 00000000000..857f12636eb
--- /dev/null
+++ b/Documentation/devicetree/bindings/arm/rockchip.txt
@@ -0,0 +1,10 @@
+Rockchip platforms device tree bindings
+---------------------------------------
+
+- bq Curie 2 tablet:
+    Required root node properties:
+      - compatible = "mundoreader,bq-curie2", "rockchip,rk3066a";
+
+- Radxa Rock board:
+    Required root node properties:
+      - compatible = "radxa,rock", "rockchip,rk3188";
diff --git a/Documentation/devicetree/bindings/arm/samsung/pmu.txt b/Documentation/devicetree/bindings/arm/samsung/pmu.txt
index f1f155255f2..2a4ab046a8a 100644
--- a/Documentation/devicetree/bindings/arm/samsung/pmu.txt
+++ b/Documentation/devicetree/bindings/arm/samsung/pmu.txt
@@ -2,6 +2,10 @@ SAMSUNG Exynos SoC series PMU Registers
 
 Properties:
  - compatible : should contain two values. First value must be one from following list:
+		   - "samsung,exynos3250-pmu" - for Exynos3250 SoC,
+		   - "samsung,exynos4210-pmu" - for Exynos4210 SoC,
+		   - "samsung,exynos4212-pmu" - for Exynos4212 SoC,
+		   - "samsung,exynos4412-pmu" - for Exynos4412 SoC,
 		   - "samsung,exynos5250-pmu" - for Exynos5250 SoC,
 		   - "samsung,exynos5420-pmu" - for Exynos5420 SoC.
 		second value must be always "syscon".
diff --git a/Documentation/devicetree/bindings/arm/samsung/sysreg.txt b/Documentation/devicetree/bindings/arm/samsung/sysreg.txt
index 0ab3251a6ec..4fced6e9d5e 100644
--- a/Documentation/devicetree/bindings/arm/samsung/sysreg.txt
+++ b/Documentation/devicetree/bindings/arm/samsung/sysreg.txt
@@ -1,8 +1,10 @@
 SAMSUNG S5P/Exynos SoC series System Registers (SYSREG)
 
 Properties:
- - compatible : should contain "samsung,<chip name>-sysreg", "syscon";
-   For Exynos4 SoC series it should be "samsung,exynos4-sysreg", "syscon";
+ - compatible : should contain two values. First value must be one from following list:
+		- "samsung,exynos4-sysreg" - for Exynos4 based SoCs,
+		- "samsung,exynos5-sysreg" - for Exynos5 based SoCs.
+		second value must be always "syscon".
  - reg : offset and length of the register set.
 
 Example:
@@ -10,3 +12,8 @@ Example:
 		compatible = "samsung,exynos4-sysreg", "syscon";
 		reg = <0x10010000 0x400>;
 	};
+
+	syscon@10050000 {
+		compatible = "samsung,exynos5-sysreg", "syscon";
+		reg = <0x10050000 0x5000>;
+	};
diff --git a/Documentation/devicetree/bindings/arm/sti.txt b/Documentation/devicetree/bindings/arm/sti.txt
new file mode 100644
index 00000000000..92f16c78bb6
--- /dev/null
+++ b/Documentation/devicetree/bindings/arm/sti.txt
@@ -0,0 +1,15 @@
+ST STi Platforms Device Tree Bindings
+---------------------------------------
+
+Boards with the ST STiH415 SoC shall have the following properties:
+Required root node property:
+compatible = "st,stih415";
+
+Boards with the ST STiH416 SoC shall have the following properties:
+Required root node property:
+compatible = "st,stih416";
+
+Boards with the ST STiH407 SoC shall have the following properties:
+Required root node property:
+compatible = "st,stih407";
+
diff --git a/Documentation/devicetree/bindings/arm/vexpress-sysreg.txt b/Documentation/devicetree/bindings/arm/vexpress-sysreg.txt
index 5580e9c4bd8..00318d083c9 100644
--- a/Documentation/devicetree/bindings/arm/vexpress-sysreg.txt
+++ b/Documentation/devicetree/bindings/arm/vexpress-sysreg.txt
@@ -8,6 +8,8 @@ interrupt generation, MMC and NOR Flash control etc.
 Required node properties:
 - compatible value : = "arm,vexpress,sysreg";
 - reg : physical base address and the size of the registers window
+
+Deprecated properties, replaced by GPIO subnodes (see below):
 - gpio-controller : specifies that the node is a GPIO controller
 - #gpio-cells : size of the GPIO specifier, should be 2:
   - first cell is the pseudo-GPIO line number:
@@ -16,35 +18,86 @@ Required node properties:
     2 - NOR FLASH WPn
   - second cell can take standard GPIO flags (currently ignored).
 
+Control registers providing pseudo-GPIO lines must be represented
+by subnodes, each of them requiring the following properties:
+- compatible value : one of
+			"arm,vexpress-sysreg,sys_led"
+			"arm,vexpress-sysreg,sys_mci"
+			"arm,vexpress-sysreg,sys_flash"
+- gpio-controller : makes the node a GPIO controller
+- #gpio-cells : size of the GPIO specifier, must be 2:
+  - first cell is the function number:
+    - for sys_led : 0..7 = LED 0..7
+    - for sys_mci : 0 = MMC CARDIN, 1 = MMC WPROT
+    - for sys_flash : 0 = NOR FLASH WPn
+  - second cell can take standard GPIO flags (currently ignored).
+
 Example:
 	v2m_sysreg: sysreg@10000000 {
  		compatible = "arm,vexpress-sysreg";
  		reg = <0x10000000 0x1000>;
-		gpio-controller;
-		#gpio-cells = <2>;
+
+		v2m_led_gpios: sys_led@08 {
+			compatible = "arm,vexpress-sysreg,sys_led";
+			gpio-controller;
+			#gpio-cells = <2>;
+		};
+
+		v2m_mmc_gpios: sys_mci@48 {
+			compatible = "arm,vexpress-sysreg,sys_mci";
+			gpio-controller;
+			#gpio-cells = <2>;
+		};
+
+		v2m_flash_gpios: sys_flash@4c {
+			compatible = "arm,vexpress-sysreg,sys_flash";
+			gpio-controller;
+			#gpio-cells = <2>;
+		};
  	};
 
 This block also can also act a bridge to the platform's configuration
 bus via "system control" interface, addressing devices with site number,
 position in the board stack, config controller, function and device
-numbers - see motherboard's TRM for more details.
-
-The node describing a config device must refer to the sysreg node via
-"arm,vexpress,config-bridge" phandle (can be also defined in the node's
-parent) and relies on the board topology properties - see main vexpress
-node documentation for more details. It must also define the following
-property:
-- arm,vexpress-sysreg,func : must contain two cells:
-  - first cell defines function number (eg. 1 for clock generator,
-    2 for voltage regulators etc.)
-  - device number (eg. osc 0, osc 1 etc.)
+numbers - see motherboard's TRM for more details. All configuration
+controller accessible via this interface must reference the sysreg
+node via "arm,vexpress,config-bridge" phandle and define appropriate
+topology properties - see main vexpress node documentation for more
+details. Each child of such node describes one function and must
+define the following properties:
+- compatible value : must be one of (corresponding to the TRM):
+	"arm,vexpress-amp"
+	"arm,vexpress-dvimode"
+	"arm,vexpress-energy"
+	"arm,vexpress-muxfpga"
+	"arm,vexpress-osc"
+	"arm,vexpress-power"
+	"arm,vexpress-reboot"
+	"arm,vexpress-reset"
+	"arm,vexpress-scc"
+	"arm,vexpress-shutdown"
+	"arm,vexpress-temp"
+	"arm,vexpress-volt"
+- arm,vexpress-sysreg,func : must contain a set of two cells long groups:
+  - first cell of each group defines the function number
+    (eg. 1 for clock generator, 2 for voltage regulators etc.)
+  - second cell of each group defines device number (eg. osc 0,
+    osc 1 etc.)
+  - some functions (eg. energy meter, with its 64 bit long counter)
+    are using more than one function/device number pair
 
 Example:
 	mcc {
+		compatible = "arm,vexpress,config-bus";
 		arm,vexpress,config-bridge = <&v2m_sysreg>;
 
 		osc@0 {
 			compatible = "arm,vexpress-osc";
 			arm,vexpress-sysreg,func = <1 0>;
 		};
+
+		energy@0 {
+			compatible = "arm,vexpress-energy";
+			arm,vexpress-sysreg,func = <13 0>, <13 1>;
+		};
 	};
diff --git a/Documentation/devicetree/bindings/arm/vexpress.txt b/Documentation/devicetree/bindings/arm/vexpress.txt
index ae49161e478..39844cd0bcc 100644
--- a/Documentation/devicetree/bindings/arm/vexpress.txt
+++ b/Documentation/devicetree/bindings/arm/vexpress.txt
@@ -80,12 +80,17 @@ but also control clock generators, voltage regulators, gather
 environmental data like temperature, power consumption etc. Even
 the video output switch (FPGA) is controlled that way.
 
-Nodes describing devices controlled by this infrastructure should
-point at the bridge device node:
+The controllers are not mapped into normal memory address space
+and must be accessed through bridges - other devices capable
+of generating transactions on the configuration bus.
+
+The nodes describing configuration controllers must define
+the following properties:
+- compatible value:
+	compatible = "arm,vexpress,config-bus";
 - bridge phandle:
 	arm,vexpress,config-bridge = <phandle>;
-This property can be also defined in a parent node (eg. for a DCC)
-and is effective for all children.
+and children describing available functions.
 
 
 Platform topology
@@ -197,7 +202,7 @@ Example of a VE tile description (simplified)
 	};
 
 	dcc {
-		compatible = "simple-bus";
+		compatible = "arm,vexpress,config-bus";
 		arm,vexpress,config-bridge = <&v2m_sysreg>;
 
 		osc@0 {
diff --git a/Documentation/devicetree/bindings/ata/apm-xgene.txt b/Documentation/devicetree/bindings/ata/apm-xgene.txt
index 7bcfbf59810..a668f0e7d00 100644
--- a/Documentation/devicetree/bindings/ata/apm-xgene.txt
+++ b/Documentation/devicetree/bindings/ata/apm-xgene.txt
@@ -24,6 +24,7 @@ Required properties:
   * "sata-phy" for the SATA 6.0Gbps PHY
 
 Optional properties:
+- dma-coherent		: Present if dma operations are coherent
 - status		: Shall be "ok" if enabled or "disabled" if disabled.
 			  Default is "ok".
 
@@ -55,6 +56,7 @@ Example:
 			      <0x0 0x1f22e000 0x0 0x1000>,
 			      <0x0 0x1f227000 0x0 0x1000>;
 			interrupts = <0x0 0x87 0x4>;
+			dma-coherent;
 			status = "ok";
 			clocks = <&sataclk 0>;
 			phys = <&phy2 0>;
@@ -69,6 +71,7 @@ Example:
 			      <0x0 0x1f23e000 0x0 0x1000>,
 			      <0x0 0x1f237000 0x0 0x1000>;
 			interrupts = <0x0 0x88 0x4>;
+			dma-coherent;
 			status = "ok";
 			clocks = <&sataclk 0>;
 			phys = <&phy3 0>;
diff --git a/Documentation/devicetree/bindings/bus/brcm,gisb-arb.txt b/Documentation/devicetree/bindings/bus/brcm,gisb-arb.txt
new file mode 100644
index 00000000000..e2d501d20c9
--- /dev/null
+++ b/Documentation/devicetree/bindings/bus/brcm,gisb-arb.txt
@@ -0,0 +1,30 @@
+Broadcom GISB bus Arbiter controller
+
+Required properties:
+
+- compatible: should be "brcm,gisb-arb"
+- reg: specifies the base physical address and size of the registers
+- interrupt-parent: specifies the phandle to the parent interrupt controller
+  this arbiter gets interrupt line from
+- interrupts: specifies the two interrupts (timeout and TEA) to be used from
+  the parent interrupt controller
+
+Optional properties:
+
+- brcm,gisb-arb-master-mask: 32-bits wide bitmask used to specify which GISB
+  masters are valid at the system level
+- brcm,gisb-arb-master-names: string list of the litteral name of the GISB
+  masters. Should match the number of bits set in brcm,gisb-master-mask and
+  the order in which they appear
+
+Example:
+
+gisb-arb@f0400000 {
+	compatible = "brcm,gisb-arb";
+	reg = <0xf0400000 0x800>;
+	interrupts = <0>, <2>;
+	interrupt-parent = <&sun_l2_intc>;
+
+	brcm,gisb-arb-master-mask = <0x7>;
+	brcm,gisb-arb-master-names = "bsp_0", "scpu_0", "cpu_0";
+};
diff --git a/Documentation/devicetree/bindings/clock/altr_socfpga.txt b/Documentation/devicetree/bindings/clock/altr_socfpga.txt
index 5dfd145d3cc..f72e80e0dad 100644
--- a/Documentation/devicetree/bindings/clock/altr_socfpga.txt
+++ b/Documentation/devicetree/bindings/clock/altr_socfpga.txt
@@ -21,8 +21,8 @@ Optional properties:
 - fixed-divider : If clocks have a fixed divider value, use this property.
 - clk-gate : For "socfpga-gate-clk", clk-gate contains the gating register
         and the bit index.
-- div-reg : For "socfpga-gate-clk", div-reg contains the divider register, bit shift,
-        and width.
+- div-reg : For "socfpga-gate-clk" and "socfpga-periph-clock", div-reg contains
+	the divider register, bit shift, and width.
 - clk-phase : For the sdmmc_clk, contains the value of the clock phase that controls
 	the SDMMC CIU clock. The first value is the clk_sample(smpsel), and the second
 	value is the cclk_in_drv(drvsel). The clk-phase is used to enable the correct
diff --git a/Documentation/devicetree/bindings/clock/at91-clock.txt b/Documentation/devicetree/bindings/clock/at91-clock.txt
index cd5e2391288..b3d544ca522 100644
--- a/Documentation/devicetree/bindings/clock/at91-clock.txt
+++ b/Documentation/devicetree/bindings/clock/at91-clock.txt
@@ -6,6 +6,16 @@ This binding uses the common clock binding[1].
 
 Required properties:
 - compatible : shall be one of the following:
+	"atmel,at91sam9x5-sckc":
+		at91 SCKC (Slow Clock Controller)
+		This node contains the slow clock definitions.
+
+	"atmel,at91sam9x5-clk-slow-osc":
+		at91 slow oscillator
+
+	"atmel,at91sam9x5-clk-slow-rc-osc":
+		at91 internal slow RC oscillator
+
 	"atmel,at91rm9200-pmc" or
 	"atmel,at91sam9g45-pmc" or
 	"atmel,at91sam9n12-pmc" or
@@ -15,8 +25,18 @@ Required properties:
 		All at91 specific clocks (clocks defined below) must be child
 		node of the PMC node.
 
+	"atmel,at91sam9x5-clk-slow" (under sckc node)
+	or
+	"atmel,at91sam9260-clk-slow" (under pmc node):
+		at91 slow clk
+
+	"atmel,at91rm9200-clk-main-osc"
+	"atmel,at91sam9x5-clk-main-rc-osc"
+		at91 main clk sources
+
+	"atmel,at91sam9x5-clk-main"
 	"atmel,at91rm9200-clk-main":
-		at91 main oscillator
+		at91 main clock
 
 	"atmel,at91rm9200-clk-master" or
 	"atmel,at91sam9x5-clk-master":
@@ -54,6 +74,63 @@ Required properties:
 	"atmel,at91sam9x5-clk-utmi":
 		at91 utmi clock
 
+Required properties for SCKC node:
+- reg : defines the IO memory reserved for the SCKC.
+- #size-cells : shall be 0 (reg is used to encode clk id).
+- #address-cells : shall be 1 (reg is used to encode clk id).
+
+
+For example:
+	sckc: sckc@fffffe50 {
+		compatible = "atmel,sama5d3-pmc";
+		reg = <0xfffffe50 0x4>
+		#size-cells = <0>;
+		#address-cells = <1>;
+
+		/* put at91 slow clocks here */
+	};
+
+
+Required properties for internal slow RC oscillator:
+- #clock-cells : from common clock binding; shall be set to 0.
+- clock-frequency : define the internal RC oscillator frequency.
+
+Optional properties:
+- clock-accuracy : define the internal RC oscillator accuracy.
+
+For example:
+	slow_rc_osc: slow_rc_osc {
+		compatible = "atmel,at91sam9x5-clk-slow-rc-osc";
+		clock-frequency = <32768>;
+		clock-accuracy = <50000000>;
+	};
+
+Required properties for slow oscillator:
+- #clock-cells : from common clock binding; shall be set to 0.
+- clocks : shall encode the main osc source clk sources (see atmel datasheet).
+
+Optional properties:
+- atmel,osc-bypass : boolean property. Set this when a clock signal is directly
+  provided on XIN.
+
+For example:
+	slow_osc: slow_osc {
+		compatible = "atmel,at91rm9200-clk-slow-osc";
+		#clock-cells = <0>;
+		clocks = <&slow_xtal>;
+	};
+
+Required properties for slow clock:
+- #clock-cells : from common clock binding; shall be set to 0.
+- clocks : shall encode the slow clk sources (see atmel datasheet).
+
+For example:
+	clk32k: slck {
+		compatible = "atmel,at91sam9x5-clk-slow";
+		#clock-cells = <0>;
+		clocks = <&slow_rc_osc &slow_osc>;
+	};
+
 Required properties for PMC node:
 - reg : defines the IO memory reserved for the PMC.
 - #size-cells : shall be 0 (reg is used to encode clk id).
@@ -62,7 +139,7 @@ Required properties for PMC node:
 - interrupt-controller : tell that the PMC is an interrupt controller.
 - #interrupt-cells : must be set to 1. The first cell encodes the interrupt id,
 	and reflect the bit position in the PMC_ER/DR/SR registers.
-	You can use the dt macros defined in dt-bindings/clk/at91.h.
+	You can use the dt macros defined in dt-bindings/clock/at91.h.
 	0 (AT91_PMC_MOSCS) -> main oscillator ready
 	1 (AT91_PMC_LOCKA) -> PLL A ready
 	2 (AT91_PMC_LOCKB) -> PLL B ready
@@ -85,24 +162,57 @@ For example:
 		/* put at91 clocks here */
 	};
 
+Required properties for main clock internal RC oscillator:
+- interrupt-parent : must reference the PMC node.
+- interrupts : shall be set to "<0>".
+- clock-frequency : define the internal RC oscillator frequency.
+
+Optional properties:
+- clock-accuracy : define the internal RC oscillator accuracy.
+
+For example:
+	main_rc_osc: main_rc_osc {
+		compatible = "atmel,at91sam9x5-clk-main-rc-osc";
+		interrupt-parent = <&pmc>;
+		interrupts = <0>;
+		clock-frequency = <12000000>;
+		clock-accuracy = <50000000>;
+	};
+
+Required properties for main clock oscillator:
+- interrupt-parent : must reference the PMC node.
+- interrupts : shall be set to "<0>".
+- #clock-cells : from common clock binding; shall be set to 0.
+- clocks : shall encode the main osc source clk sources (see atmel datasheet).
+
+Optional properties:
+- atmel,osc-bypass : boolean property. Specified if a clock signal is provided
+  on XIN.
+
+  clock signal is directly provided on XIN pin.
+
+For example:
+	main_osc: main_osc {
+		compatible = "atmel,at91rm9200-clk-main-osc";
+		interrupt-parent = <&pmc>;
+		interrupts = <0>;
+		#clock-cells = <0>;
+		clocks = <&main_xtal>;
+	};
+
 Required properties for main clock:
 - interrupt-parent : must reference the PMC node.
 - interrupts : shall be set to "<0>".
 - #clock-cells : from common clock binding; shall be set to 0.
-- clocks (optional if clock-frequency is provided) : shall be the slow clock
-	phandle. This clock is used to calculate the main clock rate if
-	"clock-frequency" is not provided.
-- clock-frequency : the main oscillator frequency.Prefer the use of
-	"clock-frequency" over automatic clock rate calculation.
+- clocks : shall encode the main clk sources (see atmel datasheet).
 
 For example:
 	main: mainck {
-		compatible = "atmel,at91rm9200-clk-main";
+		compatible = "atmel,at91sam9x5-clk-main";
 		interrupt-parent = <&pmc>;
 		interrupts = <0>;
 		#clock-cells = <0>;
-		clocks = <&ck32k>;
-		clock-frequency = <18432000>;
+		clocks = <&main_rc_osc &main_osc>;
 	};
 
 Required properties for master clock:
diff --git a/Documentation/devicetree/bindings/clock/exynos3250-clock.txt b/Documentation/devicetree/bindings/clock/exynos3250-clock.txt
new file mode 100644
index 00000000000..aadc9c59e2d
--- /dev/null
+++ b/Documentation/devicetree/bindings/clock/exynos3250-clock.txt
@@ -0,0 +1,41 @@
+* Samsung Exynos3250 Clock Controller
+
+The Exynos3250 clock controller generates and supplies clock to various
+controllers within the Exynos3250 SoC.
+
+Required Properties:
+
+- compatible: should be one of the following.
+  - "samsung,exynos3250-cmu" - controller compatible with Exynos3250 SoC.
+
+- reg: physical base address of the controller and length of memory mapped
+  region.
+
+- #clock-cells: should be 1.
+
+Each clock is assigned an identifier and client nodes can use this identifier
+to specify the clock which they consume.
+
+All available clocks are defined as preprocessor macros in
+dt-bindings/clock/exynos3250.h header and can be used in device
+tree sources.
+
+Example 1: An example of a clock controller node is listed below.
+
+	cmu: clock-controller@10030000 {
+		compatible = "samsung,exynos3250-cmu";
+		reg = <0x10030000 0x20000>;
+		#clock-cells = <1>;
+	};
+
+Example 2: UART controller node that consumes the clock generated by the clock
+	   controller. Refer to the standard clock bindings for information
+	   about 'clocks' and 'clock-names' property.
+
+	serial@13800000 {
+		compatible = "samsung,exynos4210-uart";
+		reg = <0x13800000 0x100>;
+		interrupts = <0 109 0>;
+		clocks = <&cmu CLK_UART0>, <&cmu CLK_SCLK_UART0>;
+		clock-names = "uart", "clk_uart_baud0";
+	};
diff --git a/Documentation/devicetree/bindings/clock/exynos5260-clock.txt b/Documentation/devicetree/bindings/clock/exynos5260-clock.txt
new file mode 100644
index 00000000000..5496b2fac48
--- /dev/null
+++ b/Documentation/devicetree/bindings/clock/exynos5260-clock.txt
@@ -0,0 +1,190 @@
+* Samsung Exynos5260 Clock Controller
+
+Exynos5260 has 13 clock controllers which are instantiated
+independently from the device-tree. These clock controllers
+generate and supply clocks to various hardware blocks within
+the SoC.
+
+Each clock is assigned an identifier and client nodes can use
+this identifier to specify the clock which they consume. All
+available clocks are defined as preprocessor macros in
+dt-bindings/clock/exynos5260-clk.h header and can be used in
+device tree sources.
+
+External clocks:
+
+There are several clocks that are generated outside the SoC. It
+is expected that they are defined using standard clock bindings
+with following clock-output-names:
+
+ - "fin_pll" - PLL input clock from XXTI
+ - "xrtcxti" - input clock from XRTCXTI
+ - "ioclk_pcm_extclk" - pcm external operation clock
+ - "ioclk_spdif_extclk" - spdif external operation clock
+ - "ioclk_i2s_cdclk" - i2s0 codec clock
+
+Phy clocks:
+
+There are several clocks which are generated by specific PHYs.
+These clocks are fed into the clock controller and then routed to
+the hardware blocks. These clocks are defined as fixed clocks in the
+driver with following names:
+
+ - "phyclk_dptx_phy_ch3_txd_clk" - dp phy clock for channel 3
+ - "phyclk_dptx_phy_ch2_txd_clk" - dp phy clock for channel 2
+ - "phyclk_dptx_phy_ch1_txd_clk" - dp phy clock for channel 1
+ - "phyclk_dptx_phy_ch0_txd_clk" - dp phy clock for channel 0
+ - "phyclk_hdmi_phy_tmds_clko" - hdmi phy tmds clock
+ - "phyclk_hdmi_phy_pixel_clko" - hdmi phy pixel clock
+ - "phyclk_hdmi_link_o_tmds_clkhi" - hdmi phy for hdmi link
+ - "phyclk_dptx_phy_o_ref_clk_24m" - dp phy reference clock
+ - "phyclk_dptx_phy_clk_div2"
+ - "phyclk_mipi_dphy_4l_m_rxclkesc0"
+ - "phyclk_usbhost20_phy_phyclock" - usb 2.0 phy clock
+ - "phyclk_usbhost20_phy_freeclk"
+ - "phyclk_usbhost20_phy_clk48mohci"
+ - "phyclk_usbdrd30_udrd30_pipe_pclk"
+ - "phyclk_usbdrd30_udrd30_phyclock" - usb 3.0 phy clock
+
+Required Properties for Clock Controller:
+
+ - compatible: should be one of the following.
+	1) "samsung,exynos5260-clock-top"
+	2) "samsung,exynos5260-clock-peri"
+	3) "samsung,exynos5260-clock-egl"
+	4) "samsung,exynos5260-clock-kfc"
+	5) "samsung,exynos5260-clock-g2d"
+	6) "samsung,exynos5260-clock-mif"
+	7) "samsung,exynos5260-clock-mfc"
+	8) "samsung,exynos5260-clock-g3d"
+	9) "samsung,exynos5260-clock-fsys"
+	10) "samsung,exynos5260-clock-aud"
+	11) "samsung,exynos5260-clock-isp"
+	12) "samsung,exynos5260-clock-gscl"
+	13) "samsung,exynos5260-clock-disp"
+
+ - reg: physical base address of the controller and the length of
+	memory mapped region.
+
+ - #clock-cells: should be 1.
+
+ - clocks: list of clock identifiers which are fed as the input to
+	the given clock controller. Please refer the next section to find
+	the input clocks for a given controller.
+
+ - clock-names: list of names of clocks which are fed as the input
+	to the given clock controller.
+
+Input clocks for top clock controller:
+	- fin_pll
+	- dout_mem_pll
+	- dout_bus_pll
+	- dout_media_pll
+
+Input clocks for peri clock controller:
+	- fin_pll
+	- ioclk_pcm_extclk
+	- ioclk_i2s_cdclk
+	- ioclk_spdif_extclk
+	- phyclk_hdmi_phy_ref_cko
+	- dout_aclk_peri_66
+	- dout_sclk_peri_uart0
+	- dout_sclk_peri_uart1
+	- dout_sclk_peri_uart2
+	- dout_sclk_peri_spi0_b
+	- dout_sclk_peri_spi1_b
+	- dout_sclk_peri_spi2_b
+	- dout_aclk_peri_aud
+	- dout_sclk_peri_spi0_b
+
+Input clocks for egl clock controller:
+	- fin_pll
+	- dout_bus_pll
+
+Input clocks for kfc clock controller:
+	- fin_pll
+	- dout_media_pll
+
+Input clocks for g2d clock controller:
+	- fin_pll
+	- dout_aclk_g2d_333
+
+Input clocks for mif clock controller:
+	- fin_pll
+
+Input clocks for mfc clock controller:
+	- fin_pll
+	- dout_aclk_mfc_333
+
+Input clocks for g3d clock controller:
+	- fin_pll
+
+Input clocks for fsys clock controller:
+	- fin_pll
+	- phyclk_usbhost20_phy_phyclock
+	- phyclk_usbhost20_phy_freeclk
+	- phyclk_usbhost20_phy_clk48mohci
+	- phyclk_usbdrd30_udrd30_pipe_pclk
+	- phyclk_usbdrd30_udrd30_phyclock
+	- dout_aclk_fsys_200
+
+Input clocks for aud clock controller:
+	- fin_pll
+	- fout_aud_pll
+	- ioclk_i2s_cdclk
+	- ioclk_pcm_extclk
+
+Input clocks for isp clock controller:
+	- fin_pll
+	- dout_aclk_isp1_266
+	- dout_aclk_isp1_400
+	- mout_aclk_isp1_266
+
+Input clocks for gscl clock controller:
+	- fin_pll
+	- dout_aclk_gscl_400
+	- dout_aclk_gscl_333
+
+Input clocks for disp clock controller:
+	- fin_pll
+	- phyclk_dptx_phy_ch3_txd_clk
+	- phyclk_dptx_phy_ch2_txd_clk
+	- phyclk_dptx_phy_ch1_txd_clk
+	- phyclk_dptx_phy_ch0_txd_clk
+	- phyclk_hdmi_phy_tmds_clko
+	- phyclk_hdmi_phy_ref_clko
+	- phyclk_hdmi_phy_pixel_clko
+	- phyclk_hdmi_link_o_tmds_clkhi
+	- phyclk_mipi_dphy_4l_m_txbyte_clkhs
+	- phyclk_dptx_phy_o_ref_clk_24m
+	- phyclk_dptx_phy_clk_div2
+	- phyclk_mipi_dphy_4l_m_rxclkesc0
+	- phyclk_hdmi_phy_ref_cko
+	- ioclk_spdif_extclk
+	- dout_aclk_peri_aud
+	- dout_aclk_disp_222
+	- dout_sclk_disp_pixel
+	- dout_aclk_disp_333
+
+Example 1: An example of a clock controller node is listed below.
+
+	clock_mfc: clock-controller@11090000 {
+		compatible = "samsung,exynos5260-clock-mfc";
+		clock = <&fin_pll>, <&clock_top TOP_DOUT_ACLK_MFC_333>;
+		clock-names = "fin_pll", "dout_aclk_mfc_333";
+		reg = <0x11090000 0x10000>;
+		#clock-cells = <1>;
+	};
+
+Example 2: UART controller node that consumes the clock generated by the
+		peri clock controller. Refer to the standard clock bindings for
+		information about 'clocks' and 'clock-names' property.
+
+	serial@12C00000 {
+		compatible = "samsung,exynos4210-uart";
+		reg = <0x12C00000 0x100>;
+		interrupts = <0 146 0>;
+		clocks = <&clock_peri PERI_PCLK_UART0>, <&clock_peri PERI_SCLK_UART0>;
+		clock-names = "uart", "clk_uart_baud0";
+	};
+
diff --git a/Documentation/devicetree/bindings/clock/exynos5410-clock.txt b/Documentation/devicetree/bindings/clock/exynos5410-clock.txt
new file mode 100644
index 00000000000..aeab635b07b
--- /dev/null
+++ b/Documentation/devicetree/bindings/clock/exynos5410-clock.txt
@@ -0,0 +1,45 @@
+* Samsung Exynos5410 Clock Controller
+
+The Exynos5410 clock controller generates and supplies clock to various
+controllers within the Exynos5410 SoC.
+
+Required Properties:
+
+- compatible: should be "samsung,exynos5410-clock"
+
+- reg: physical base address of the controller and length of memory mapped
+  region.
+
+- #clock-cells: should be 1.
+
+All available clocks are defined as preprocessor macros in
+dt-bindings/clock/exynos5410.h header and can be used in device
+tree sources.
+
+External clock:
+
+There is clock that is generated outside the SoC. It
+is expected that it is defined using standard clock bindings
+with following clock-output-name:
+
+ - "fin_pll" - PLL input clock from XXTI
+
+Example 1: An example of a clock controller node is listed below.
+
+	clock: clock-controller@0x10010000 {
+		compatible = "samsung,exynos5410-clock";
+		reg = <0x10010000 0x30000>;
+		#clock-cells = <1>;
+	};
+
+Example 2: UART controller node that consumes the clock generated by the clock
+	   controller. Refer to the standard clock bindings for information
+	   about 'clocks' and 'clock-names' property.
+
+	serial@12C20000 {
+		compatible = "samsung,exynos4210-uart";
+		reg = <0x12C00000 0x100>;
+		interrupts = <0 51 0>;
+		clocks = <&clock CLK_UART0>, <&clock CLK_SCLK_UART0>;
+		clock-names = "uart", "clk_uart_baud0";
+	};
diff --git a/Documentation/devicetree/bindings/clock/exynos5420-clock.txt b/Documentation/devicetree/bindings/clock/exynos5420-clock.txt
index ca88c97a856..d54f42cf044 100644
--- a/Documentation/devicetree/bindings/clock/exynos5420-clock.txt
+++ b/Documentation/devicetree/bindings/clock/exynos5420-clock.txt
@@ -1,12 +1,13 @@
 * Samsung Exynos5420 Clock Controller
 
 The Exynos5420 clock controller generates and supplies clock to various
-controllers within the Exynos5420 SoC.
+controllers within the Exynos5420 SoC and for the Exynos5800 SoC.
 
 Required Properties:
 
 - compatible: should be one of the following.
   - "samsung,exynos5420-clock" - controller compatible with Exynos5420 SoC.
+  - "samsung,exynos5800-clock" - controller compatible with Exynos5800 SoC.
 
 - reg: physical base address of the controller and length of memory mapped
   region.
diff --git a/Documentation/devicetree/bindings/clock/imx25-clock.txt b/Documentation/devicetree/bindings/clock/imx25-clock.txt
index db4f2f05c4d..ba6b312ff8a 100644
--- a/Documentation/devicetree/bindings/clock/imx25-clock.txt
+++ b/Documentation/devicetree/bindings/clock/imx25-clock.txt
@@ -139,6 +139,9 @@ clocks and IDs.
 	uart5_ipg		124
 	reserved		125
 	wdt_ipg			126
+	cko_div			127
+	cko_sel			128
+	cko			129
 
 Examples:
 
diff --git a/Documentation/devicetree/bindings/clock/imx27-clock.txt b/Documentation/devicetree/bindings/clock/imx27-clock.txt
index 7a207039373..6bc9fd2c663 100644
--- a/Documentation/devicetree/bindings/clock/imx27-clock.txt
+++ b/Documentation/devicetree/bindings/clock/imx27-clock.txt
@@ -98,7 +98,12 @@ clocks and IDs.
 	fpm                  83
 	mpll_osc_sel         84
 	mpll_sel             85
-	spll_gate	     86
+	spll_gate            86
+	mshc_div             87
+	rtic_ipg_gate        88
+	mshc_ipg_gate        89
+	rtic_ahb_gate        90
+	mshc_baud_gate       91
 
 Examples:
 
diff --git a/Documentation/devicetree/bindings/clock/imx6q-clock.txt b/Documentation/devicetree/bindings/clock/imx6q-clock.txt
index 6aab72bf67e..90ec91fe5ce 100644
--- a/Documentation/devicetree/bindings/clock/imx6q-clock.txt
+++ b/Documentation/devicetree/bindings/clock/imx6q-clock.txt
@@ -220,6 +220,7 @@ clocks and IDs.
 	lvds2_sel		205
 	lvds1_gate		206
 	lvds2_gate		207
+	esai_ahb		208
 
 Examples:
 
diff --git a/Documentation/devicetree/bindings/clock/imx6sx-clock.txt b/Documentation/devicetree/bindings/clock/imx6sx-clock.txt
new file mode 100644
index 00000000000..22362b9b7ba
--- /dev/null
+++ b/Documentation/devicetree/bindings/clock/imx6sx-clock.txt
@@ -0,0 +1,13 @@
+* Clock bindings for Freescale i.MX6 SoloX
+
+Required properties:
+- compatible: Should be "fsl,imx6sx-ccm"
+- reg: Address and length of the register set
+- #clock-cells: Should be <1>
+- clocks: list of clock specifiers, must contain an entry for each required
+  entry in clock-names
+- clock-names: should include entries "ckil", "osc", "ipp_di0" and "ipp_di1"
+
+The clock consumer should specify the desired clock by having the clock
+ID in its "clocks" phandle cell.  See include/dt-bindings/clock/imx6sx-clock.h
+for the full list of i.MX6 SoloX clock IDs.
diff --git a/Documentation/devicetree/bindings/clock/renesas,cpg-mstp-clocks.txt b/Documentation/devicetree/bindings/clock/renesas,cpg-mstp-clocks.txt
index 5992dceec7a..6c3c0847e4f 100644
--- a/Documentation/devicetree/bindings/clock/renesas,cpg-mstp-clocks.txt
+++ b/Documentation/devicetree/bindings/clock/renesas,cpg-mstp-clocks.txt
@@ -10,6 +10,7 @@ index in the group, from 0 to 31.
 Required Properties:
 
   - compatible: Must be one of the following
+    - "renesas,r7s72100-mstp-clocks" for R7S72100 (RZ) MSTP gate clocks
     - "renesas,r8a7790-mstp-clocks" for R8A7790 (R-Car H2) MSTP gate clocks
     - "renesas,r8a7791-mstp-clocks" for R8A7791 (R-Car M2) MSTP gate clocks
     - "renesas,cpg-mstp-clock" for generic MSTP gate clocks
@@ -43,7 +44,7 @@ Example
 		clock-output-names =
 			"tpu0", "mmcif1", "sdhi3", "sdhi2",
 			 "sdhi1", "sdhi0", "mmcif0";
-		renesas,clock-indices = <
+		clock-indices = <
 			R8A7790_CLK_TPU0 R8A7790_CLK_MMCIF1 R8A7790_CLK_SDHI3
 			R8A7790_CLK_SDHI2 R8A7790_CLK_SDHI1 R8A7790_CLK_SDHI0
 			R8A7790_CLK_MMCIF0
diff --git a/Documentation/devicetree/bindings/clock/samsung,s3c2410-clock.txt b/Documentation/devicetree/bindings/clock/samsung,s3c2410-clock.txt
new file mode 100644
index 00000000000..822505e715a
--- /dev/null
+++ b/Documentation/devicetree/bindings/clock/samsung,s3c2410-clock.txt
@@ -0,0 +1,50 @@
+* Samsung S3C2410 Clock Controller
+
+The S3C2410 clock controller generates and supplies clock to various controllers
+within the SoC. The clock binding described here is applicable to the s3c2410,
+s3c2440 and s3c2442 SoCs in the s3c24x family.
+
+Required Properties:
+
+- compatible: should be one of the following.
+  - "samsung,s3c2410-clock" - controller compatible with S3C2410 SoC.
+  - "samsung,s3c2440-clock" - controller compatible with S3C2440 SoC.
+  - "samsung,s3c2442-clock" - controller compatible with S3C2442 SoC.
+- reg: physical base address of the controller and length of memory mapped
+  region.
+- #clock-cells: should be 1.
+
+Each clock is assigned an identifier and client nodes can use this identifier
+to specify the clock which they consume. Some of the clocks are available only
+on a particular SoC.
+
+All available clocks are defined as preprocessor macros in
+dt-bindings/clock/s3c2410.h header and can be used in device
+tree sources.
+
+External clocks:
+
+The xti clock used as input for the plls is generated outside the SoC. It is
+expected that is are defined using standard clock bindings with a
+clock-output-names value of "xti".
+
+Example: Clock controller node:
+
+	clocks: clock-controller@4c000000 {
+		compatible = "samsung,s3c2410-clock";
+		reg = <0x4c000000 0x20>;
+		#clock-cells = <1>;
+	};
+
+Example: UART controller node that consumes the clock generated by the clock
+  controller (refer to the standard clock bindings for information about
+  "clocks" and "clock-names" properties):
+
+	serial@50004000 {
+		compatible = "samsung,s3c2440-uart";
+		reg = <0x50004000 0x4000>;
+		interrupts = <1 23 3 4>, <1 23 4 4>;
+		clock-names = "uart", "clk_uart_baud2";
+		clocks = <&clocks PCLK_UART0>, <&clocks PCLK_UART0>;
+		status = "disabled";
+	};
diff --git a/Documentation/devicetree/bindings/clock/samsung,s3c2412-clock.txt b/Documentation/devicetree/bindings/clock/samsung,s3c2412-clock.txt
new file mode 100644
index 00000000000..2b430960ba4
--- /dev/null
+++ b/Documentation/devicetree/bindings/clock/samsung,s3c2412-clock.txt
@@ -0,0 +1,50 @@
+* Samsung S3C2412 Clock Controller
+
+The S3C2412 clock controller generates and supplies clock to various controllers
+within the SoC. The clock binding described here is applicable to the s3c2412
+and s3c2413 SoCs in the s3c24x family.
+
+Required Properties:
+
+- compatible: should be "samsung,s3c2412-clock"
+- reg: physical base address of the controller and length of memory mapped
+  region.
+- #clock-cells: should be 1.
+
+Each clock is assigned an identifier and client nodes can use this identifier
+to specify the clock which they consume. Some of the clocks are available only
+on a particular SoC.
+
+All available clocks are defined as preprocessor macros in
+dt-bindings/clock/s3c2412.h header and can be used in device
+tree sources.
+
+External clocks:
+
+There are several clocks that are generated outside the SoC. It is expected
+that they are defined using standard clock bindings with following
+clock-output-names:
+ - "xti" - crystal input - required,
+ - "ext" - external clock source - optional,
+
+Example: Clock controller node:
+
+	clocks: clock-controller@4c000000 {
+		compatible = "samsung,s3c2412-clock";
+		reg = <0x4c000000 0x20>;
+		#clock-cells = <1>;
+	};
+
+Example: UART controller node that consumes the clock generated by the clock
+  controller (refer to the standard clock bindings for information about
+  "clocks" and "clock-names" properties):
+
+	serial@50004000 {
+		compatible = "samsung,s3c2412-uart";
+		reg = <0x50004000 0x4000>;
+		interrupts = <1 23 3 4>, <1 23 4 4>;
+		clock-names = "uart", "clk_uart_baud2", "clk_uart_baud3";
+		clocks = <&clocks PCLK_UART0>, <&clocks PCLK_UART0>,
+			 <&clocks SCLK_UART>;
+		status = "disabled";
+	};
diff --git a/Documentation/devicetree/bindings/clock/samsung,s3c2443-clock.txt b/Documentation/devicetree/bindings/clock/samsung,s3c2443-clock.txt
new file mode 100644
index 00000000000..e67bb05478a
--- /dev/null
+++ b/Documentation/devicetree/bindings/clock/samsung,s3c2443-clock.txt
@@ -0,0 +1,56 @@
+* Samsung S3C2443 Clock Controller
+
+The S3C2443 clock controller generates and supplies clock to various controllers
+within the SoC. The clock binding described here is applicable to all SoCs in
+the s3c24x family starting with the s3c2443.
+
+Required Properties:
+
+- compatible: should be one of the following.
+  - "samsung,s3c2416-clock" - controller compatible with S3C2416 SoC.
+  - "samsung,s3c2443-clock" - controller compatible with S3C2443 SoC.
+  - "samsung,s3c2450-clock" - controller compatible with S3C2450 SoC.
+- reg: physical base address of the controller and length of memory mapped
+  region.
+- #clock-cells: should be 1.
+
+Each clock is assigned an identifier and client nodes can use this identifier
+to specify the clock which they consume. Some of the clocks are available only
+on a particular SoC.
+
+All available clocks are defined as preprocessor macros in
+dt-bindings/clock/s3c2443.h header and can be used in device
+tree sources.
+
+External clocks:
+
+There are several clocks that are generated outside the SoC. It is expected
+that they are defined using standard clock bindings with following
+clock-output-names:
+ - "xti" - crystal input - required,
+ - "ext" - external clock source - optional,
+ - "ext_i2s" - external I2S clock - optional,
+ - "ext_uart" - external uart clock - optional,
+
+Example: Clock controller node:
+
+	clocks: clock-controller@4c000000 {
+		compatible = "samsung,s3c2416-clock";
+		reg = <0x4c000000 0x40>;
+		#clock-cells = <1>;
+	};
+
+Example: UART controller node that consumes the clock generated by the clock
+  controller (refer to the standard clock bindings for information about
+  "clocks" and "clock-names" properties):
+
+	serial@50004000 {
+		compatible = "samsung,s3c2440-uart";
+		reg = <0x50004000 0x4000>;
+		interrupts = <1 23 3 4>, <1 23 4 4>;
+		clock-names = "uart", "clk_uart_baud2",
+				"clk_uart_baud3";
+		clocks = <&clocks PCLK_UART0>, <&clocks PCLK_UART0>,
+				<&clocks SCLK_UART>;
+		status = "disabled";
+	};
diff --git a/Documentation/devicetree/bindings/clock/ti-keystone-pllctrl.txt b/Documentation/devicetree/bindings/clock/ti-keystone-pllctrl.txt
new file mode 100644
index 00000000000..3e6a81e9980
--- /dev/null
+++ b/Documentation/devicetree/bindings/clock/ti-keystone-pllctrl.txt
@@ -0,0 +1,20 @@
+* Device tree bindings for Texas Instruments keystone pll controller
+
+The main pll controller used to drive theC66x CorePacs, the switch fabric,
+and a majority of the peripheral clocks (all but the ARM CorePacs, DDR3 and
+the NETCP modules) requires a PLL Controller to manage the various clock
+divisions, gating, and synchronization.
+
+Required properties:
+
+- compatible:		"ti,keystone-pllctrl", "syscon"
+
+- reg:			contains offset/length value for pll controller
+			registers space.
+
+Example:
+
+pllctrl: pll-controller@0x02310000 {
+	compatible = "ti,keystone-pllctrl", "syscon";
+	reg = <0x02310000 0x200>;
+};
diff --git a/Documentation/devicetree/bindings/dma/ti-edma.txt b/Documentation/devicetree/bindings/dma/ti-edma.txt
index 9fbbdb783a7..5ba525a1003 100644
--- a/Documentation/devicetree/bindings/dma/ti-edma.txt
+++ b/Documentation/devicetree/bindings/dma/ti-edma.txt
@@ -2,11 +2,8 @@ TI EDMA
 
 Required properties:
 - compatible : "ti,edma3"
-- ti,edma-regions: Number of regions
-- ti,edma-slots: Number of slots
 - #dma-cells: Should be set to <1>
               Clients should use a single channel number per DMA request.
-- dma-channels: Specify total DMA channels per CC
 - reg: Memory map for accessing module
 - interrupt-parent: Interrupt controller the interrupt is routed through
 - interrupts: Exactly 3 interrupts need to be specified in the order:
@@ -17,6 +14,13 @@ Optional properties:
 - ti,hwmods: Name of the hwmods associated to the EDMA
 - ti,edma-xbar-event-map: Crossbar event to channel map
 
+Deprecated properties:
+Listed here in case one wants to boot an old kernel with new DTB. These
+properties might need to be added to the new DTS files.
+- ti,edma-regions: Number of regions
+- ti,edma-slots: Number of slots
+- dma-channels: Specify total DMA channels per CC
+
 Example:
 
 edma: edma@49000000 {
@@ -26,9 +30,6 @@ edma: edma@49000000 {
 	compatible = "ti,edma3";
 	ti,hwmods = "tpcc", "tptc0", "tptc1", "tptc2";
 	#dma-cells = <1>;
-	dma-channels = <64>;
-	ti,edma-regions = <4>;
-	ti,edma-slots = <256>;
-	ti,edma-xbar-event-map = <1 12
-				  2 13>;
+	ti,edma-xbar-event-map = /bits/ 16 <1 12
+					    2 13>;
 };
diff --git a/Documentation/devicetree/bindings/gpio/renesas,gpio-rcar.txt b/Documentation/devicetree/bindings/gpio/renesas,gpio-rcar.txt
index f61cef74a21..941a26aa432 100644
--- a/Documentation/devicetree/bindings/gpio/renesas,gpio-rcar.txt
+++ b/Documentation/devicetree/bindings/gpio/renesas,gpio-rcar.txt
@@ -21,6 +21,12 @@ Required Properties:
     GPIO_ACTIVE_HIGH and GPIO_ACTIVE_LOW flags are supported.
   - gpio-ranges: Range of pins managed by the GPIO controller.
 
+Optional properties:
+
+  - clocks: Must contain a reference to the functional clock.  The property is
+    mandatory if the hardware implements a controllable functional clock for
+    the GPIO instance.
+
 Please refer to gpio.txt in this directory for details of gpio-ranges property
 and the common GPIO bindings used by client devices.
 
diff --git a/Documentation/devicetree/bindings/hsi/client-devices.txt b/Documentation/devicetree/bindings/hsi/client-devices.txt
new file mode 100644
index 00000000000..104c9a3e57a
--- /dev/null
+++ b/Documentation/devicetree/bindings/hsi/client-devices.txt
@@ -0,0 +1,44 @@
+Each HSI port is supposed to have one child node, which
+symbols the remote device connected to the HSI port. The
+following properties are standardized for HSI clients:
+
+Required HSI configuration properties:
+
+- hsi-channel-ids:	A list of channel ids
+
+- hsi-rx-mode:		Receiver Bit transmission mode ("stream" or "frame")
+- hsi-tx-mode:		Transmitter Bit transmission mode ("stream" or "frame")
+- hsi-mode:		May be used instead hsi-rx-mode and hsi-tx-mode if
+			the transmission mode is the same for receiver and
+			transmitter
+- hsi-speed-kbps:	Max bit transmission speed in kbit/s
+- hsi-flow:		RX flow type ("synchronized" or "pipeline")
+- hsi-arb-mode:		Arbitration mode for TX frame ("round-robin", "priority")
+
+Optional HSI configuration properties:
+
+- hsi-channel-names:	A list with one name per channel specified in the
+			hsi-channel-ids property
+
+
+Device Tree node example for an HSI client:
+
+hsi-controller {
+	hsi-port {
+		modem: hsi-client {
+			compatible = "nokia,n900-modem";
+
+			hsi-channel-ids = <0>, <1>, <2>, <3>;
+			hsi-channel-names = "mcsaab-control",
+					    "speech-control",
+					    "speech-data",
+					    "mcsaab-data";
+			hsi-speed-kbps = <55000>;
+			hsi-mode = "frame";
+			hsi-flow = "synchronized";
+			hsi-arb-mode = "round-robin";
+
+			/* more client specific properties */
+		};
+	};
+};
diff --git a/Documentation/devicetree/bindings/hsi/nokia-modem.txt b/Documentation/devicetree/bindings/hsi/nokia-modem.txt
new file mode 100644
index 00000000000..8a979780452
--- /dev/null
+++ b/Documentation/devicetree/bindings/hsi/nokia-modem.txt
@@ -0,0 +1,57 @@
+Nokia modem client bindings
+
+The Nokia modem HSI client follows the common HSI client binding
+and inherits all required properties. The following additional
+properties are needed by the Nokia modem HSI client:
+
+Required properties:
+- compatible:		Should be one of
+      "nokia,n900-modem"
+- hsi-channel-names:	Should contain the following strings
+      "mcsaab-control"
+      "speech-control"
+      "speech-data"
+      "mcsaab-data"
+- gpios:		Should provide a GPIO handler for each GPIO listed in
+                        gpio-names
+- gpio-names:		Should contain the following strings
+      "cmt_apeslpx"
+      "cmt_rst_rq"
+      "cmt_en"
+      "cmt_rst"
+      "cmt_bsi"
+- interrupts:		Should be IRQ handle for modem's reset indication
+
+Example:
+
+&ssi_port {
+	modem: hsi-client {
+		compatible = "nokia,n900-modem";
+
+		pinctrl-names = "default";
+		pinctrl-0 = <&modem_pins>;
+
+		hsi-channel-ids = <0>, <1>, <2>, <3>;
+		hsi-channel-names = "mcsaab-control",
+				    "speech-control",
+				    "speech-data",
+				    "mcsaab-data";
+		hsi-speed-kbps = <55000>;
+		hsi-mode = "frame";
+		hsi-flow = "synchronized";
+		hsi-arb-mode = "round-robin";
+
+		interrupts-extended = <&gpio3 8 IRQ_TYPE_EDGE_FALLING>; /* 72 */
+
+		gpios = <&gpio3  6 GPIO_ACTIVE_HIGH>, /* 70 */
+			<&gpio3  9 GPIO_ACTIVE_HIGH>, /* 73 */
+			<&gpio3 10 GPIO_ACTIVE_HIGH>, /* 74 */
+			<&gpio3 11 GPIO_ACTIVE_HIGH>, /* 75 */
+			<&gpio5 29 GPIO_ACTIVE_HIGH>; /* 157 */
+		gpio-names = "cmt_apeslpx",
+			     "cmt_rst_rq",
+			     "cmt_en",
+			     "cmt_rst",
+			     "cmt_bsi";
+	};
+};
diff --git a/Documentation/devicetree/bindings/hsi/omap-ssi.txt b/Documentation/devicetree/bindings/hsi/omap-ssi.txt
new file mode 100644
index 00000000000..f26625e4269
--- /dev/null
+++ b/Documentation/devicetree/bindings/hsi/omap-ssi.txt
@@ -0,0 +1,97 @@
+OMAP SSI controller bindings
+
+OMAP Synchronous Serial Interface (SSI) controller implements a legacy
+variant of MIPI's High Speed Synchronous Serial Interface (HSI).
+
+Required properties:
+- compatible:		Should include "ti,omap3-ssi".
+- reg-names:		Contains the values "sys" and "gdd" (in this order).
+- reg:			Contains a matching register specifier for each entry
+			in reg-names.
+- interrupt-names:	Contains the value "gdd_mpu".
+- interrupts: 		Contains matching interrupt information for each entry
+			in interrupt-names.
+- ranges:		Represents the bus address mapping between the main
+			controller node and the child nodes below.
+- clock-names:		Must include the following entries:
+  "ssi_ssr_fck": The OMAP clock of that name
+  "ssi_sst_fck": The OMAP clock of that name
+  "ssi_ick": The OMAP clock of that name
+- clocks:		Contains a matching clock specifier for each entry in
+			clock-names.
+- #address-cells:	Should be set to <1>
+- #size-cells:		Should be set to <1>
+
+Each port is represented as a sub-node of the ti,omap3-ssi device.
+
+Required Port sub-node properties:
+- compatible:		Should be set to the following value
+			ti,omap3-ssi-port (applicable to OMAP34xx devices)
+- reg-names:		Contains the values "tx" and "rx" (in this order).
+- reg:			Contains a matching register specifier for each entry
+			in reg-names.
+- interrupt-parent	Should be a phandle for the interrupt controller
+- interrupts:		Should contain interrupt specifiers for mpu interrupts
+			0 and 1 (in this order).
+- ti,ssi-cawake-gpio:	Defines which GPIO pin is used to signify CAWAKE
+			events for the port. This is an optional board-specific
+			property. If it's missing the port will not be
+			enabled.
+
+Example for Nokia N900:
+
+ssi-controller@48058000 {
+	compatible = "ti,omap3-ssi";
+
+	/* needed until hwmod is updated to use the compatible string */
+	ti,hwmods = "ssi";
+
+	reg = <0x48058000 0x1000>,
+	      <0x48059000 0x1000>;
+	reg-names = "sys",
+		    "gdd";
+
+	interrupts = <55>;
+	interrupt-names = "gdd_mpu";
+
+	clocks = <&ssi_ssr_fck>,
+		 <&ssi_sst_fck>,
+		 <&ssi_ick>;
+	clock-names = "ssi_ssr_fck",
+		      "ssi_sst_fck",
+		      "ssi_ick";
+
+	#address-cells = <1>;
+	#size-cells = <1>;
+	ranges;
+
+	ssi-port@4805a000 {
+		compatible = "ti,omap3-ssi-port";
+
+		reg = <0x4805a000 0x800>,
+		      <0x4805a800 0x800>;
+		reg-names = "tx",
+			    "rx";
+
+		interrupt-parent = <&intc>;
+		interrupts = <67>,
+			     <68>;
+
+		ti,ssi-cawake-gpio = <&gpio5 23 GPIO_ACTIVE_HIGH>; /* 151 */
+	}
+
+	ssi-port@4805a000 {
+		compatible = "ti,omap3-ssi-port";
+
+		reg = <0x4805b000 0x800>,
+		      <0x4805b800 0x800>;
+		reg-names = "tx",
+			    "rx";
+
+		interrupt-parent = <&intc>;
+		interrupts = <69>,
+			     <70>;
+
+		status = "disabled"; /* second port is not used on N900 */
+	}
+}
diff --git a/Documentation/devicetree/bindings/i2c/trivial-devices.txt b/Documentation/devicetree/bindings/i2c/trivial-devices.txt
index 71724d026ff..bef86e57c38 100644
--- a/Documentation/devicetree/bindings/i2c/trivial-devices.txt
+++ b/Documentation/devicetree/bindings/i2c/trivial-devices.txt
@@ -13,8 +13,22 @@ ad,ad7414		SMBus/I2C Digital Temperature Sensor in 6-Pin SOT with SMBus Alert an
 ad,adm9240		ADM9240:  Complete System Hardware Monitor for uProcessor-Based Systems
 adi,adt7461		+/-1C TDM Extended Temp Range I.C
 adt7461			+/-1C TDM Extended Temp Range I.C
+adi,adt7473		+/-1C TDM Extended Temp Range I.C
+adi,adt7475		+/-1C TDM Extended Temp Range I.C
+adi,adt7476		+/-1C TDM Extended Temp Range I.C
+adi,adt7490		+/-1C TDM Extended Temp Range I.C
 at,24c08		i2c serial eeprom  (24cxx)
+atmel,24c00		i2c serial eeprom  (24cxx)
+atmel,24c01		i2c serial eeprom  (24cxx)
 atmel,24c02		i2c serial eeprom  (24cxx)
+atmel,24c04		i2c serial eeprom  (24cxx)
+atmel,24c16		i2c serial eeprom  (24cxx)
+atmel,24c32		i2c serial eeprom  (24cxx)
+atmel,24c64		i2c serial eeprom  (24cxx)
+atmel,24c128		i2c serial eeprom  (24cxx)
+atmel,24c256		i2c serial eeprom  (24cxx)
+atmel,24c512		i2c serial eeprom  (24cxx)
+atmel,24c1024		i2c serial eeprom  (24cxx)
 atmel,at97sc3204t	i2c trusted platform module (TPM)
 capella,cm32181		CM32181: Ambient Light Sensor
 catalyst,24c32		i2c serial eeprom
@@ -46,8 +60,10 @@ maxim,ds1050		5 Bit Programmable, Pulse-Width Modulator
 maxim,max1237		Low-Power, 4-/12-Channel, 2-Wire Serial, 12-Bit ADCs
 maxim,max6625		9-Bit/12-Bit Temperature Sensors with I²C-Compatible Serial Interface
 mc,rv3029c2		Real Time Clock Module with I2C-Bus
+national,lm63		Temperature sensor with integrated fan control
 national,lm75		I2C TEMP SENSOR
 national,lm80		Serial Interface ACPI-Compatible Microprocessor System Hardware Monitor
+national,lm85		Temperature sensor with integrated fan control
 national,lm92		±0.33°C Accurate, 12-Bit + Sign Temperature Sensor and Thermal Window Comparator with Two-Wire Interface
 nuvoton,npct501		i2c trusted platform module (TPM)
 nxp,pca9556		Octal SMBus and I2C registered interface
diff --git a/Documentation/devicetree/bindings/iio/proximity/as3935.txt b/Documentation/devicetree/bindings/iio/proximity/as3935.txt
new file mode 100644
index 00000000000..ae23dd8da73
--- /dev/null
+++ b/Documentation/devicetree/bindings/iio/proximity/as3935.txt
@@ -0,0 +1,28 @@
+Austrian Microsystems AS3935 Franklin lightning sensor device driver
+
+Required properties:
+	- compatible: must be "ams,as3935"
+	- reg: SPI chip select number for the device
+	- spi-cpha: SPI Mode 1. Refer to spi/spi-bus.txt for generic SPI
+	slave node bindings.
+	- interrupt-parent : should be the phandle for the interrupt controller
+	- interrupts : the sole interrupt generated by the device
+
+	Refer to interrupt-controller/interrupts.txt for generic
+	interrupt client node bindings.
+
+Optional properties:
+	- ams,tuning-capacitor-pf: Calibration tuning capacitor stepping
+	  value 0 - 120pF. This will require using the calibration data from
+	  the manufacturer.
+
+Example:
+
+as3935@0 {
+	compatible = "ams,as3935";
+	reg = <0>;
+	spi-cpha;
+	interrupt-parent = <&gpio1>;
+	interrupts = <16 1>;
+	ams,tuning-capacitor-pf = <80>;
+};
diff --git a/Documentation/devicetree/bindings/memory-controllers/mvebu-devbus.txt b/Documentation/devicetree/bindings/memory-controllers/mvebu-devbus.txt
index 653c90c34a7..1ee3bc09f31 100644
--- a/Documentation/devicetree/bindings/memory-controllers/mvebu-devbus.txt
+++ b/Documentation/devicetree/bindings/memory-controllers/mvebu-devbus.txt
@@ -6,10 +6,11 @@ The actual devices are instantiated from the child nodes of a Device Bus node.
 
 Required properties:
 
- - compatible:          Currently only Armada 370/XP SoC are supported,
-                        with this compatible string:
+ - compatible:          Armada 370/XP SoC are supported using the
+                        "marvell,mvebu-devbus" compatible string.
 
-                        marvell,mvebu-devbus
+                        Orion5x SoC are supported using the
+                        "marvell,orion-devbus" compatible string.
 
  - reg:                 A resource specifier for the register space.
                         This is the base address of a chip select within
@@ -22,7 +23,14 @@ Required properties:
                         integer values for each chip-select line in use:
                         0 <physical address of mapping> <size>
 
-Mandatory timing properties for child nodes:
+Optional properties:
+
+ - devbus,keep-config   This property can optionally be used to keep
+                        using the timing parameters set by the
+                        bootloader. It makes all the timing properties
+                        described below unused.
+
+Timing properties for child nodes:
 
 Read parameters:
 
@@ -30,21 +38,26 @@ Read parameters:
                         drive the AD bus after the completion of a device read.
                         This prevents contentions on the Device Bus after a read
                         cycle from a slow device.
+                        Mandatory, except if devbus,keep-config is used.
 
- - devbus,bus-width:    Defines the bus width (e.g. <16>)
+ - devbus,bus-width:    Defines the bus width, in bits (e.g. <16>).
+                        Mandatory, except if devbus,keep-config is used.
 
  - devbus,badr-skew-ps: Defines the time delay from from A[2:0] toggle,
                         to read data sample. This parameter is useful for
                         synchronous pipelined devices, where the address
                         precedes the read data by one or two cycles.
+                        Mandatory, except if devbus,keep-config is used.
 
  - devbus,acc-first-ps: Defines the time delay from the negation of
                         ALE[0] to the cycle that the first read data is sampled
                         by the controller.
+                        Mandatory, except if devbus,keep-config is used.
 
  - devbus,acc-next-ps:  Defines the time delay between the cycle that
                         samples data N and the cycle that samples data N+1
                         (in burst accesses).
+                        Mandatory, except if devbus,keep-config is used.
 
  - devbus,rd-setup-ps:  Defines the time delay between DEV_CSn assertion to
 			DEV_OEn assertion. If set to 0 (default),
@@ -52,6 +65,8 @@ Read parameters:
                         This parameter has no affect on <acc-first-ps> parameter
                         (no affect on first data sample). Set <rd-setup-ps>
                         to a value smaller than <acc-first-ps>.
+                        Mandatory for "marvell,mvebu-devbus" compatible string,
+                        except if devbus,keep-config is used.
 
  - devbus,rd-hold-ps:   Defines the time between the last data sample to the
 			de-assertion of DEV_CSn. If set to 0 (default),
@@ -62,16 +77,20 @@ Read parameters:
                         last data sampled. Also this parameter has no
                         affect on <turn-off-ps> parameter.
                         Set <rd-hold-ps> to a value smaller than <turn-off-ps>.
+                        Mandatory for "marvell,mvebu-devbus" compatible string,
+                        except if devbus,keep-config is used.
 
 Write parameters:
 
  - devbus,ale-wr-ps:    Defines the time delay from the ALE[0] negation cycle
 			to the DEV_WEn assertion.
+                        Mandatory.
 
  - devbus,wr-low-ps:    Defines the time during which DEV_WEn is active.
                         A[2:0] and Data are kept valid as long as DEV_WEn
                         is active. This parameter defines the setup time of
                         address and data to DEV_WEn rise.
+                        Mandatory.
 
  - devbus,wr-high-ps:   Defines the time during which DEV_WEn is kept
                         inactive (high) between data beats of a burst write.
@@ -79,10 +98,13 @@ Write parameters:
                         <wr-high-ps> - <tick> ps.
 			This parameter defines the hold time of address and
 			data after DEV_WEn rise.
+                        Mandatory.
 
  - devbus,sync-enable: Synchronous device enable.
                        1: True
                        0: False
+                       Mandatory for "marvell,mvebu-devbus" compatible string,
+                       except if devbus,keep-config is used.
 
 An example for an Armada XP GP board, with a 16 MiB NOR device as child
 is showed below. Note that the Device Bus driver is in charge of allocating
diff --git a/Documentation/devicetree/bindings/mfd/bcm590xx.txt b/Documentation/devicetree/bindings/mfd/bcm590xx.txt
index 1fe30e2b10d..be51a15e05f 100644
--- a/Documentation/devicetree/bindings/mfd/bcm590xx.txt
+++ b/Documentation/devicetree/bindings/mfd/bcm590xx.txt
@@ -19,7 +19,9 @@ Optional child nodes:
   The valid regulator node names for BCM59056 are:
   	rfldo, camldo1, camldo2, simldo1, simldo2, sdldo, sdxldo,
 	mmcldo1, mmcldo2, audldo, micldo, usbldo, vibldo,
-	csr, iosr1, iosr2, msr, sdsr1, sdsr2, vsr
+	csr, iosr1, iosr2, msr, sdsr1, sdsr2, vsr,
+	gpldo1, gpldo2, gpldo3, gpldo4, gpldo5, gpldo6,
+	vbus
 
 Example:
 	pmu: bcm59056@8 {
diff --git a/Documentation/devicetree/bindings/mfd/s2mps11.txt b/Documentation/devicetree/bindings/mfd/s2mps11.txt
index 802e839b082..d81ba30c0d8 100644
--- a/Documentation/devicetree/bindings/mfd/s2mps11.txt
+++ b/Documentation/devicetree/bindings/mfd/s2mps11.txt
@@ -56,6 +56,20 @@ for a particular group of BUCKs. So provide same regulator-ramp-delay<value>.
 Grouping of BUCKs sharing ramp rate setting is as follow : BUCK[1, 6],
 BUCK[3, 4], and BUCK[7, 8, 10]
 
+On S2MPS14 the LDO10, LDO11 and LDO12 can be configured to external control
+over GPIO. To turn this feature on this property must be added to the regulator
+sub-node:
+	- samsung,ext-control-gpios: GPIO specifier for one GPIO
+		controlling this regulator (enable/disable);
+Example:
+	LDO12 {
+		regulator-name = "V_EMMC_2.8V";
+		regulator-min-microvolt = <2800000>;
+		regulator-max-microvolt = <2800000>;
+		samsung,ext-control-gpios = <&gpk0 2 0>;
+	};
+
+
 The regulator constraints inside the regulator nodes use the standard regulator
 bindings which are documented elsewhere.
 
diff --git a/Documentation/devicetree/bindings/misc/arm-charlcd.txt b/Documentation/devicetree/bindings/misc/arm-charlcd.txt
new file mode 100644
index 00000000000..e28e2aac47f
--- /dev/null
+++ b/Documentation/devicetree/bindings/misc/arm-charlcd.txt
@@ -0,0 +1,18 @@
+ARM Versatile Character LCD
+-----------------------------------------------------
+This binding defines the character LCD interface found on ARM Versatile AB
+and PB reference platforms.
+
+Required properties:
+- compatible : "arm,versatile-clcd"
+- reg : Location and size of character LCD registers
+
+Optional properties:
+- interrupts - single interrupt for character LCD. The character LCD can
+  operate in polled mode without an interrupt.
+
+Example:
+	lcd@10008000 {
+		compatible = "arm,versatile-lcd";
+		reg = <0x10008000 0x1000>;
+	};
diff --git a/Documentation/devicetree/bindings/mmc/mmci.txt b/Documentation/devicetree/bindings/mmc/mmci.txt
index 2b584cae352..03796cf2d3e 100644
--- a/Documentation/devicetree/bindings/mmc/mmci.txt
+++ b/Documentation/devicetree/bindings/mmc/mmci.txt
@@ -4,12 +4,58 @@ The ARM PrimeCell MMCI PL180 and PL181 provides an interface for
 reading and writing to MultiMedia and SD cards alike.
 
 This file documents differences between the core properties described
-by mmc.txt and the properties used by the mmci driver.
+by mmc.txt and the properties used by the mmci driver. Using "st" as
+the prefix for a property, indicates support by the ST Micro variant.
 
 Required properties:
 - compatible             : contains "arm,pl18x", "arm,primecell".
-- arm,primecell-periphid : contains the PrimeCell Peripheral ID.
+- vmmc-supply            : phandle to the regulator device tree node, mentioned
+                           as the VCC/VDD supply in the eMMC/SD specs.
 
 Optional properties:
-- mmc-cap-mmc-highspeed  : indicates whether MMC is high speed capable
-- mmc-cap-sd-highspeed   : indicates whether SD is high speed capable
+- arm,primecell-periphid : contains the PrimeCell Peripheral ID, it overrides
+                           the ID provided by the HW
+- vqmmc-supply           : phandle to the regulator device tree node, mentioned
+                           as the VCCQ/VDD_IO supply in the eMMC/SD specs.
+- st,sig-dir-dat0        : bus signal direction pin used for DAT[0].
+- st,sig-dir-dat2        : bus signal direction pin used for DAT[2].
+- st,sig-dir-dat31       : bus signal direction pin used for DAT[3] and DAT[1].
+- st,sig-dir-dat74       : bus signal direction pin used for DAT[4] to DAT[7].
+- st,sig-dir-cmd         : cmd signal direction pin used for CMD.
+- st,sig-pin-fbclk       : feedback clock signal pin used.
+
+Deprecated properties:
+- mmc-cap-mmc-highspeed  : indicates whether MMC is high speed capable.
+- mmc-cap-sd-highspeed   : indicates whether SD is high speed capable.
+
+Example:
+
+sdi0_per1@80126000 {
+	compatible = "arm,pl18x", "arm,primecell";
+	reg = <0x80126000 0x1000>;
+	interrupts = <0 60 IRQ_TYPE_LEVEL_HIGH>;
+
+	dmas = <&dma 29 0 0x2>, /* Logical - DevToMem */
+	       <&dma 29 0 0x0>; /* Logical - MemToDev */
+	dma-names = "rx", "tx";
+
+	clocks = <&prcc_kclk 1 5>, <&prcc_pclk 1 5>;
+	clock-names = "sdi", "apb_pclk";
+
+	max-frequency = <100000000>;
+	bus-width = <4>;
+	cap-sd-highspeed;
+	cap-mmc-highspeed;
+	cd-gpios  = <&gpio2 31 0x4>; // 95
+	st,sig-dir-dat0;
+	st,sig-dir-dat2;
+	st,sig-dir-cmd;
+	st,sig-pin-fbclk;
+
+	vmmc-supply = <&ab8500_ldo_aux3_reg>;
+	vqmmc-supply = <&vmmci>;
+
+	pinctrl-names = "default", "sleep";
+	pinctrl-0 = <&sdi0_default_mode>;
+	pinctrl-1 = <&sdi0_sleep_mode>;
+};
diff --git a/Documentation/devicetree/bindings/net/arc_emac.txt b/Documentation/devicetree/bindings/net/arc_emac.txt
index 7fbb027218a..a1d71eb43b2 100644
--- a/Documentation/devicetree/bindings/net/arc_emac.txt
+++ b/Documentation/devicetree/bindings/net/arc_emac.txt
@@ -4,11 +4,15 @@ Required properties:
 - compatible: Should be "snps,arc-emac"
 - reg: Address and length of the register set for the device
 - interrupts: Should contain the EMAC interrupts
-- clock-frequency: CPU frequency. It is needed to calculate and set polling
-period of EMAC.
 - max-speed: see ethernet.txt file in the same directory.
 - phy: see ethernet.txt file in the same directory.
 
+Clock handling:
+The clock frequency is needed to calculate and set polling period of EMAC.
+It must be provided by one of:
+- clock-frequency: CPU frequency.
+- clocks: reference to the clock supplying the EMAC.
+
 Child nodes of the driver are the individual PHY devices connected to the
 MDIO bus. They must have a "reg" property given the PHY address on the MDIO bus.
 
@@ -19,7 +23,11 @@ Examples:
 		reg = <0xc0fc2000 0x3c>;
 		interrupts = <6>;
 		mac-address = [ 00 11 22 33 44 55 ];
+
 		clock-frequency = <80000000>;
+		/* or */
+		clocks = <&emac_clock>;
+
 		max-speed = <100>;
 		phy = <&phy0>;
 
diff --git a/Documentation/devicetree/bindings/net/ethernet.txt b/Documentation/devicetree/bindings/net/ethernet.txt
index 9ecd43d8792..3fc360523bc 100644
--- a/Documentation/devicetree/bindings/net/ethernet.txt
+++ b/Documentation/devicetree/bindings/net/ethernet.txt
@@ -10,7 +10,7 @@ The following properties are common to the Ethernet controllers:
 - max-frame-size: number, maximum transfer unit (IEEE defined MTU), rather than
   the maximum frame size (there's contradiction in ePAPR).
 - phy-mode: string, operation mode of the PHY interface; supported values are
-  "mii", "gmii", "sgmii", "tbi", "rev-mii", "rmii", "rgmii", "rgmii-id",
+  "mii", "gmii", "sgmii", "qsgmii", "tbi", "rev-mii", "rmii", "rgmii", "rgmii-id",
   "rgmii-rxid", "rgmii-txid", "rtbi", "smii", "xgmii"; this is now a de-facto
   standard property;
 - phy-connection-type: the same as "phy-mode" property but described in ePAPR;
diff --git a/Documentation/devicetree/bindings/net/mdio-gpio.txt b/Documentation/devicetree/bindings/net/mdio-gpio.txt
index c79bab02536..8dbcf8295c6 100644
--- a/Documentation/devicetree/bindings/net/mdio-gpio.txt
+++ b/Documentation/devicetree/bindings/net/mdio-gpio.txt
@@ -14,7 +14,7 @@ node.
 Example:
 
 aliases {
-	mdio-gpio0 = <&mdio0>;
+	mdio-gpio0 = &mdio0;
 };
 
 mdio0: mdio {
diff --git a/Documentation/devicetree/bindings/net/socfpga-dwmac.txt b/Documentation/devicetree/bindings/net/socfpga-dwmac.txt
index 636f0ac4e22..2a60cd3e8d5 100644
--- a/Documentation/devicetree/bindings/net/socfpga-dwmac.txt
+++ b/Documentation/devicetree/bindings/net/socfpga-dwmac.txt
@@ -23,5 +23,5 @@ gmac0: ethernet@ff700000 {
 	interrupt-names = "macirq";
 	mac-address = [00 00 00 00 00 00];/* Filled in by U-Boot */
 	clocks = <&emac_0_clk>;
-	clocks-names = "stmmaceth";
+	clock-names = "stmmaceth";
 };
diff --git a/Documentation/devicetree/bindings/net/stmmac.txt b/Documentation/devicetree/bindings/net/stmmac.txt
index 80c1fb8bfbb..a2acd2b26ba 100644
--- a/Documentation/devicetree/bindings/net/stmmac.txt
+++ b/Documentation/devicetree/bindings/net/stmmac.txt
@@ -33,7 +33,7 @@ Optional properties:
 - max-frame-size: See ethernet.txt file in the same directory
 - clocks: If present, the first clock should be the GMAC main clock,
   further clocks may be specified in derived bindings.
-- clocks-names: One name for each entry in the clocks property, the
+- clock-names: One name for each entry in the clocks property, the
   first one should be "stmmaceth".
 
 Examples:
diff --git a/Documentation/devicetree/bindings/pci/host-generic-pci.txt b/Documentation/devicetree/bindings/pci/host-generic-pci.txt
new file mode 100644
index 00000000000..f0b0436807b
--- /dev/null
+++ b/Documentation/devicetree/bindings/pci/host-generic-pci.txt
@@ -0,0 +1,100 @@
+* Generic PCI host controller
+
+Firmware-initialised PCI host controllers and PCI emulations, such as the
+virtio-pci implementations found in kvmtool and other para-virtualised
+systems, do not require driver support for complexities such as regulator
+and clock management. In fact, the controller may not even require the
+configuration of a control interface by the operating system, instead
+presenting a set of fixed windows describing a subset of IO, Memory and
+Configuration Spaces.
+
+Such a controller can be described purely in terms of the standardized device
+tree bindings communicated in pci.txt:
+
+
+Properties of the host controller node:
+
+- compatible     : Must be "pci-host-cam-generic" or "pci-host-ecam-generic"
+                   depending on the layout of configuration space (CAM vs
+                   ECAM respectively).
+
+- device_type    : Must be "pci".
+
+- ranges         : As described in IEEE Std 1275-1994, but must provide
+                   at least a definition of non-prefetchable memory. One
+                   or both of prefetchable Memory and IO Space may also
+                   be provided.
+
+- bus-range      : Optional property (also described in IEEE Std 1275-1994)
+                   to indicate the range of bus numbers for this controller.
+                   If absent, defaults to <0 255> (i.e. all buses).
+
+- #address-cells : Must be 3.
+
+- #size-cells    : Must be 2.
+
+- reg            : The Configuration Space base address and size, as accessed
+                   from the parent bus.
+
+
+Properties of the /chosen node:
+
+- linux,pci-probe-only
+                 : Optional property which takes a single-cell argument.
+                   If '0', then Linux will assign devices in its usual manner,
+                   otherwise it will not try to assign devices and instead use
+                   them as they are configured already.
+
+Configuration Space is assumed to be memory-mapped (as opposed to being
+accessed via an ioport) and laid out with a direct correspondence to the
+geography of a PCI bus address by concatenating the various components to
+form an offset.
+
+For CAM, this 24-bit offset is:
+
+        cfg_offset(bus, device, function, register) =
+                   bus << 16 | device << 11 | function << 8 | register
+
+Whilst ECAM extends this by 4 bits to accomodate 4k of function space:
+
+        cfg_offset(bus, device, function, register) =
+                   bus << 20 | device << 15 | function << 12 | register
+
+Interrupt mapping is exactly as described in `Open Firmware Recommended
+Practice: Interrupt Mapping' and requires the following properties:
+
+- #interrupt-cells   : Must be 1
+
+- interrupt-map      : <see aforementioned specification>
+
+- interrupt-map-mask : <see aforementioned specification>
+
+
+Example:
+
+pci {
+    compatible = "pci-host-cam-generic"
+    device_type = "pci";
+    #address-cells = <3>;
+    #size-cells = <2>;
+    bus-range = <0x0 0x1>;
+
+    // CPU_PHYSICAL(2)  SIZE(2)
+    reg = <0x0 0x40000000  0x0 0x1000000>;
+
+    // BUS_ADDRESS(3)  CPU_PHYSICAL(2)  SIZE(2)
+    ranges = <0x01000000 0x0 0x01000000  0x0 0x01000000  0x0 0x00010000>,
+             <0x02000000 0x0 0x41000000  0x0 0x41000000  0x0 0x3f000000>;
+
+
+    #interrupt-cells = <0x1>;
+
+    // PCI_DEVICE(3)  INT#(1)  CONTROLLER(PHANDLE)  CONTROLLER_DATA(3)
+    interrupt-map = <  0x0 0x0 0x0  0x1  &gic  0x0 0x4 0x1
+                     0x800 0x0 0x0  0x1  &gic  0x0 0x5 0x1
+                    0x1000 0x0 0x0  0x1  &gic  0x0 0x6 0x1
+                    0x1800 0x0 0x0  0x1  &gic  0x0 0x7 0x1>;
+
+    // PCI_DEVICE(3)  INT#(1)
+    interrupt-map-mask = <0xf800 0x0 0x0  0x7>;
+}
diff --git a/Documentation/devicetree/bindings/pci/pci-rcar-gen2.txt b/Documentation/devicetree/bindings/pci/pci-rcar-gen2.txt
new file mode 100644
index 00000000000..d8ef5bf50f1
--- /dev/null
+++ b/Documentation/devicetree/bindings/pci/pci-rcar-gen2.txt
@@ -0,0 +1,66 @@
+Renesas AHB to PCI bridge
+-------------------------
+
+This is the bridge used internally to connect the USB controllers to the
+AHB. There is one bridge instance per USB port connected to the internal
+OHCI and EHCI controllers.
+
+Required properties:
+- compatible: "renesas,pci-r8a7790" for the R8A7790 SoC;
+	      "renesas,pci-r8a7791" for the R8A7791 SoC.
+- reg:	A list of physical regions to access the device: the first is
+	the operational registers for the OHCI/EHCI controllers and the
+	second is for the bridge configuration and control registers.
+- interrupts: interrupt for the device.
+- clocks: The reference to the device clock.
+- bus-range: The PCI bus number range; as this is a single bus, the range
+	     should be specified as the same value twice.
+- #address-cells: must be 3.
+- #size-cells: must be 2.
+- #interrupt-cells: must be 1.
+- interrupt-map: standard property used to define the mapping of the PCI
+  interrupts to the GIC interrupts.
+- interrupt-map-mask: standard property that helps to define the interrupt
+  mapping.
+
+Example SoC configuration:
+
+	pci0: pci@ee090000  {
+		compatible = "renesas,pci-r8a7790";
+		clocks = <&mstp7_clks R8A7790_CLK_EHCI>;
+		reg = <0x0 0xee090000 0x0 0xc00>,
+		      <0x0 0xee080000 0x0 0x1100>;
+		interrupts = <0 108 IRQ_TYPE_LEVEL_HIGH>;
+		status = "disabled";
+
+		bus-range = <0 0>;
+		#address-cells = <3>;
+		#size-cells = <2>;
+		#interrupt-cells = <1>;
+		interrupt-map-mask = <0xff00 0 0 0x7>;
+		interrupt-map = <0x0000 0 0 1 &gic 0 108 IRQ_TYPE_LEVEL_HIGH
+				 0x0800 0 0 1 &gic 0 108 IRQ_TYPE_LEVEL_HIGH
+				 0x1000 0 0 2 &gic 0 108 IRQ_TYPE_LEVEL_HIGH>;
+
+		pci@0,1 {
+			reg = <0x800 0 0 0 0>;
+			device_type = "pci";
+			phys = <&usbphy 0 0>;
+			phy-names = "usb";
+		};
+
+		pci@0,2 {
+			reg = <0x1000 0 0 0 0>;
+			device_type = "pci";
+			phys = <&usbphy 0 0>;
+			phy-names = "usb";
+		};
+	};
+
+Example board setup:
+
+&pci0 {
+	status = "okay";
+	pinctrl-0 = <&usb0_pins>;
+	pinctrl-names = "default";
+};
diff --git a/Documentation/devicetree/bindings/pci/rcar-pci.txt b/Documentation/devicetree/bindings/pci/rcar-pci.txt
new file mode 100644
index 00000000000..29d3b989d3b
--- /dev/null
+++ b/Documentation/devicetree/bindings/pci/rcar-pci.txt
@@ -0,0 +1,47 @@
+* Renesas RCar PCIe interface
+
+Required properties:
+- compatible: should contain one of the following
+	"renesas,pcie-r8a7779", "renesas,pcie-r8a7790", "renesas,pcie-r8a7791"
+- reg: base address and length of the pcie controller registers.
+- #address-cells: set to <3>
+- #size-cells: set to <2>
+- bus-range: PCI bus numbers covered
+- device_type: set to "pci"
+- ranges: ranges for the PCI memory and I/O regions.
+- dma-ranges: ranges for the inbound memory regions.
+- interrupts: two interrupt sources for MSI interrupts, followed by interrupt
+	source for hardware related interrupts (e.g. link speed change).
+- #interrupt-cells: set to <1>
+- interrupt-map-mask and interrupt-map: standard PCI properties
+	to define the mapping of the PCIe interface to interrupt
+	numbers.
+- clocks: from common clock binding: clock specifiers for the PCIe controller
+	and PCIe bus clocks.
+- clock-names: from common clock binding: should be "pcie" and "pcie_bus".
+
+Example:
+
+SoC specific DT Entry:
+
+	pcie: pcie@fe000000 {
+		compatible = "renesas,pcie-r8a7791";
+		reg = <0 0xfe000000 0 0x80000>;
+		#address-cells = <3>;
+		#size-cells = <2>;
+		bus-range = <0x00 0xff>;
+		device_type = "pci";
+		ranges = <0x01000000 0 0x00000000 0 0xfe100000 0 0x00100000
+			  0x02000000 0 0xfe200000 0 0xfe200000 0 0x00200000
+			  0x02000000 0 0x30000000 0 0x30000000 0 0x08000000
+			  0x42000000 0 0x38000000 0 0x38000000 0 0x08000000>;
+		dma-ranges = <0x42000000 0 0x40000000 0 0x40000000 0 0x40000000
+			      0x42000000 2 0x00000000 2 0x00000000 0 0x40000000>;
+		interrupts = <0 116 4>, <0 117 4>, <0 118 4>;
+		#interrupt-cells = <1>;
+		interrupt-map-mask = <0 0 0 0>;
+		interrupt-map = <0 0 0 0 &gic 0 116 4>;
+		clocks = <&mstp3_clks R8A7791_CLK_PCIE>, <&pcie_bus_clk>;
+		clock-names = "pcie", "pcie_bus";
+		status = "disabled";
+	};
diff --git a/Documentation/devicetree/bindings/phy/samsung-phy.txt b/Documentation/devicetree/bindings/phy/samsung-phy.txt
index b422e38946d..2049261d8c3 100644
--- a/Documentation/devicetree/bindings/phy/samsung-phy.txt
+++ b/Documentation/devicetree/bindings/phy/samsung-phy.txt
@@ -114,3 +114,50 @@ Example:
 		compatible = "samsung,exynos-sataphy-i2c";
 		reg = <0x38>;
 	};
+
+Samsung Exynos5 SoC series USB DRD PHY controller
+--------------------------------------------------
+
+Required properties:
+- compatible : Should be set to one of the following supported values:
+	- "samsung,exynos5250-usbdrd-phy" - for exynos5250 SoC,
+	- "samsung,exynos5420-usbdrd-phy" - for exynos5420 SoC.
+- reg : Register offset and length of USB DRD PHY register set;
+- clocks: Clock IDs array as required by the controller
+- clock-names: names of clocks correseponding to IDs in the clock property;
+	       Required clocks:
+	- phy: main PHY clock (same as USB DRD controller i.e. DWC3 IP clock),
+	       used for register access.
+	- ref: PHY's reference clock (usually crystal clock), used for
+	       PHY operations, associated by phy name. It is used to
+	       determine bit values for clock settings register.
+	       For Exynos5420 this is given as 'sclk_usbphy30' in CMU.
+- samsung,pmu-syscon: phandle for PMU system controller interface, used to
+		      control pmu registers for power isolation.
+- #phy-cells : from the generic PHY bindings, must be 1;
+
+For "samsung,exynos5250-usbdrd-phy" and "samsung,exynos5420-usbdrd-phy"
+compatible PHYs, the second cell in the PHY specifier identifies the
+PHY id, which is interpreted as follows:
+  0 - UTMI+ type phy,
+  1 - PIPE3 type phy,
+
+Example:
+	usbdrd_phy: usbphy@12100000 {
+		compatible = "samsung,exynos5250-usbdrd-phy";
+		reg = <0x12100000 0x100>;
+		clocks = <&clock 286>, <&clock 1>;
+		clock-names = "phy", "ref";
+		samsung,pmu-syscon = <&pmu_system_controller>;
+		#phy-cells = <1>;
+	};
+
+- aliases: For SoCs like Exynos5420 having multiple USB 3.0 DRD PHY controllers,
+	   'usbdrd_phy' nodes should have numbered alias in the aliases node,
+	   in the form of usbdrdphyN, N = 0, 1... (depending on number of
+	   controllers).
+Example:
+	aliases {
+		usbdrdphy0 = &usb3_phy0;
+		usbdrdphy1 = &usb3_phy1;
+	};
diff --git a/Documentation/devicetree/bindings/phy/sun4i-usb-phy.txt b/Documentation/devicetree/bindings/phy/sun4i-usb-phy.txt
index a82361b6201..16528b9eb56 100644
--- a/Documentation/devicetree/bindings/phy/sun4i-usb-phy.txt
+++ b/Documentation/devicetree/bindings/phy/sun4i-usb-phy.txt
@@ -2,15 +2,26 @@ Allwinner sun4i USB PHY
 -----------------------
 
 Required properties:
-- compatible : should be one of "allwinner,sun4i-a10-usb-phy",
-  "allwinner,sun5i-a13-usb-phy" or "allwinner,sun7i-a20-usb-phy"
+- compatible : should be one of
+  * allwinner,sun4i-a10-usb-phy
+  * allwinner,sun5i-a13-usb-phy
+  * allwinner,sun6i-a31-usb-phy
+  * allwinner,sun7i-a20-usb-phy
 - reg : a list of offset + length pairs
-- reg-names : "phy_ctrl", "pmu1" and for sun4i or sun7i "pmu2"
+- reg-names :
+  * "phy_ctrl"
+  * "pmu1"
+  * "pmu2" for sun4i, sun6i or sun7i
 - #phy-cells : from the generic phy bindings, must be 1
-- clocks : phandle + clock specifier for the phy clock
-- clock-names : "usb_phy"
+- clocks : phandle + clock specifier for the phy clocks
+- clock-names :
+  * "usb_phy" for sun4i, sun5i or sun7i
+  * "usb0_phy", "usb1_phy" and "usb2_phy" for sun6i
 - resets : a list of phandle + reset specifier pairs
-- reset-names : "usb0_reset", "usb1_reset" and for sun4i or sun7i "usb2_reset"
+- reset-names :
+  * "usb0_reset"
+  * "usb1_reset"
+  * "usb2_reset" for sun4i, sun6i or sun7i
 
 Example:
 	usbphy: phy@0x01c13400 {
diff --git a/Documentation/devicetree/bindings/phy/ti-phy.txt b/Documentation/devicetree/bindings/phy/ti-phy.txt
index 788fb0fa376..9ce458f3294 100644
--- a/Documentation/devicetree/bindings/phy/ti-phy.txt
+++ b/Documentation/devicetree/bindings/phy/ti-phy.txt
@@ -32,6 +32,11 @@ Required properties:
  - reg : Address and length of the register set for the device.
  - #phy-cells: determine the number of cells that should be given in the
    phandle while referencing this phy.
+ - clocks: a list of phandles and clock-specifier pairs, one for each entry in
+   clock-names.
+ - clock-names: should include:
+   * "wkupclk" - wakeup clock.
+   * "refclk" - reference clock (optional).
 
 Optional properties:
  - ctrl-module : phandle of the control module used by PHY driver to power on
@@ -44,6 +49,8 @@ usb2phy@4a0ad080 {
 	reg = <0x4a0ad080 0x58>;
 	ctrl-module = <&omap_control_usb>;
 	#phy-cells = <0>;
+	clocks = <&usb_phy_cm_clk32k>, <&usb_otg_ss_refclk960m>;
+	clock-names = "wkupclk", "refclk";
 };
 
 TI PIPE3 PHY
diff --git a/Documentation/devicetree/bindings/pinctrl/allwinner,sunxi-pinctrl.txt b/Documentation/devicetree/bindings/pinctrl/allwinner,sunxi-pinctrl.txt
index dff0e5f995e..d8d065608ec 100644
--- a/Documentation/devicetree/bindings/pinctrl/allwinner,sunxi-pinctrl.txt
+++ b/Documentation/devicetree/bindings/pinctrl/allwinner,sunxi-pinctrl.txt
@@ -6,8 +6,13 @@ the first two functions being GPIO in and out. The configuration on
 the pins includes drive strength and pull-up.
 
 Required properties:
-- compatible: "allwinner,<soc>-pinctrl". Supported SoCs for now are:
-  sun5i-a13.
+- compatible: Should be one of the followings (depending on you SoC):
+  "allwinner,sun4i-a10-pinctrl"
+  "allwinner,sun5i-a10s-pinctrl"
+  "allwinner,sun5i-a13-pinctrl"
+  "allwinner,sun6i-a31-pinctrl"
+  "allwinner,sun6i-a31-r-pinctrl"
+  "allwinner,sun7i-a20-pinctrl"
 - reg: Should contain the register physical address and length for the
   pin controller.
 
diff --git a/Documentation/devicetree/bindings/pinctrl/fsl,imx6sx-pinctrl.txt b/Documentation/devicetree/bindings/pinctrl/fsl,imx6sx-pinctrl.txt
new file mode 100644
index 00000000000..b1b595220f1
--- /dev/null
+++ b/Documentation/devicetree/bindings/pinctrl/fsl,imx6sx-pinctrl.txt
@@ -0,0 +1,36 @@
+* Freescale i.MX6 SoloX IOMUX Controller
+
+Please refer to fsl,imx-pinctrl.txt in this directory for common binding part
+and usage.
+
+Required properties:
+- compatible: "fsl,imx6sx-iomuxc"
+- fsl,pins: each entry consists of 6 integers and represents the mux and config
+  setting for one pin.  The first 5 integers <mux_reg conf_reg input_reg mux_val
+  input_val> are specified using a PIN_FUNC_ID macro, which can be found in
+  imx6sx-pinfunc.h under device tree source folder.  The last integer CONFIG is
+  the pad setting value like pull-up on this pin.  Please refer to i.MX6 SoloX
+  Reference Manual for detailed CONFIG settings.
+
+CONFIG bits definition:
+PAD_CTL_HYS                     (1 << 16)
+PAD_CTL_PUS_100K_DOWN           (0 << 14)
+PAD_CTL_PUS_47K_UP              (1 << 14)
+PAD_CTL_PUS_100K_UP             (2 << 14)
+PAD_CTL_PUS_22K_UP              (3 << 14)
+PAD_CTL_PUE                     (1 << 13)
+PAD_CTL_PKE                     (1 << 12)
+PAD_CTL_ODE                     (1 << 11)
+PAD_CTL_SPEED_LOW               (0 << 6)
+PAD_CTL_SPEED_MED               (1 << 6)
+PAD_CTL_SPEED_HIGH              (3 << 6)
+PAD_CTL_DSE_DISABLE             (0 << 3)
+PAD_CTL_DSE_260ohm              (1 << 3)
+PAD_CTL_DSE_130ohm              (2 << 3)
+PAD_CTL_DSE_87ohm               (3 << 3)
+PAD_CTL_DSE_65ohm               (4 << 3)
+PAD_CTL_DSE_52ohm               (5 << 3)
+PAD_CTL_DSE_43ohm               (6 << 3)
+PAD_CTL_DSE_37ohm               (7 << 3)
+PAD_CTL_SRE_FAST                (1 << 0)
+PAD_CTL_SRE_SLOW                (0 << 0)
diff --git a/Documentation/devicetree/bindings/pinctrl/marvell,orion-pinctrl.txt b/Documentation/devicetree/bindings/pinctrl/marvell,orion-pinctrl.txt
new file mode 100644
index 00000000000..27570a3a174
--- /dev/null
+++ b/Documentation/devicetree/bindings/pinctrl/marvell,orion-pinctrl.txt
@@ -0,0 +1,91 @@
+* Marvell Orion SoC pinctrl driver for mpp
+
+Please refer to marvell,mvebu-pinctrl.txt in this directory for common binding
+part and usage.
+
+Required properties:
+- compatible: "marvell,88f5181l-pinctrl", "marvell,88f5182-pinctrl",
+              "marvell,88f5281-pinctrl"
+
+- reg: two register areas, the first one describing the first two
+  contiguous MPP registers, and the second one describing the single
+  final MPP register, separated from the previous one.
+
+Available mpp pins/groups and functions:
+Note: brackets (x) are not part of the mpp name for marvell,function and given
+only for more detailed description in this document.
+
+* Marvell Orion 88f5181l
+
+name          pins     functions
+================================================================================
+mpp0          0        pcie(rstout), pci(req2), gpio
+mpp1          1        gpio, pci(gnt2)
+mpp2          2        gpio, pci(req3), pci-1(pme)
+mpp3          3        gpio, pci(gnt3)
+mpp4          4        gpio, pci(req4)
+mpp5          5        gpio, pci(gnt4)
+mpp6          6        gpio, pci(req5), pci-1(clk)
+mpp7          7        gpio, pci(gnt5), pci-1(clk)
+mpp8          8        gpio, ge(col)
+mpp9          9        gpio, ge(rxerr)
+mpp10         10       gpio, ge(crs)
+mpp11         11       gpio, ge(txerr)
+mpp12         12       gpio, ge(txd4)
+mpp13         13       gpio, ge(txd5)
+mpp14         14       gpio, ge(txd6)
+mpp15         15       gpio, ge(txd7)
+mpp16         16       ge(rxd4)
+mpp17         17       ge(rxd5)
+mpp18         18       ge(rxd6)
+mpp19         19       ge(rxd7)
+
+* Marvell Orion 88f5182
+
+name          pins     functions
+================================================================================
+mpp0          0        pcie(rstout), pci(req2), gpio
+mpp1          1        gpio, pci(gnt2)
+mpp2          2        gpio, pci(req3), pci-1(pme)
+mpp3          3        gpio, pci(gnt3)
+mpp4          4        gpio, pci(req4), bootnand(re), sata0(prsnt)
+mpp5          5        gpio, pci(gnt4), bootnand(we), sata1(prsnt)
+mpp6          6        gpio, pci(req5), nand(re0), sata0(act)
+mpp7          7        gpio, pci(gnt5), nand(we0), sata1(act)
+mpp8          8        gpio, ge(col)
+mpp9          9        gpio, ge(rxerr)
+mpp10         10       gpio, ge(crs)
+mpp11         11       gpio, ge(txerr)
+mpp12         12       gpio, ge(txd4), nand(re1), sata0(ledprsnt)
+mpp13         13       gpio, ge(txd5), nand(we1), sata1(ledprsnt)
+mpp14         14       gpio, ge(txd6), nand(re2), sata0(ledact)
+mpp15         15       gpio, ge(txd7), nand(we2), sata1(ledact)
+mpp16         16       uart1(rxd), ge(rxd4), gpio
+mpp17         17       uart1(txd), ge(rxd5), gpio
+mpp18         18       uart1(cts), ge(rxd6), gpio
+mpp19         19       uart1(rts), ge(rxd7), gpio
+
+* Marvell Orion 88f5281
+
+name          pins     functions
+================================================================================
+mpp0          0        pcie(rstout), pci(req2), gpio
+mpp1          1        gpio, pci(gnt2)
+mpp2          2        gpio, pci(req3), pci(pme)
+mpp3          3        gpio, pci(gnt3)
+mpp4          4        gpio, pci(req4), bootnand(re)
+mpp5          5        gpio, pci(gnt4), bootnand(we)
+mpp6          6        gpio, pci(req5), nand(re0)
+mpp7          7        gpio, pci(gnt5), nand(we0)
+mpp8          8        gpio, ge(col)
+mpp9          9        gpio, ge(rxerr)
+mpp10         10       gpio, ge(crs)
+mpp11         11       gpio, ge(txerr)
+mpp12         12       gpio, ge(txd4), nand(re1)
+mpp13         13       gpio, ge(txd5), nand(we1)
+mpp14         14       gpio, ge(txd6), nand(re2)
+mpp15         15       gpio, ge(txd7), nand(we2)
+mpp16         16       uart1(rxd), ge(rxd4)
+mpp17         17       uart1(txd), ge(rxd5)
+mpp18         18       uart1(cts), ge(rxd6)
+mpp19         19       uart1(rts), ge(rxd7)
diff --git a/Documentation/devicetree/bindings/pinctrl/pinctrl-bindings.txt b/Documentation/devicetree/bindings/pinctrl/pinctrl-bindings.txt
index 4414163e76d..fa40a177164 100644
--- a/Documentation/devicetree/bindings/pinctrl/pinctrl-bindings.txt
+++ b/Documentation/devicetree/bindings/pinctrl/pinctrl-bindings.txt
@@ -156,6 +156,7 @@ input-disable		- disable input on pin (no effect on output)
 input-schmitt-enable	- enable schmitt-trigger mode
 input-schmitt-disable	- disable schmitt-trigger mode
 input-debounce		- debounce mode with debound time X
+power-source		- select between different power supplies
 low-power-enable	- enable low power mode
 low-power-disable	- disable low power mode
 output-low		- set the pin to output mode with low level
diff --git a/Documentation/devicetree/bindings/pinctrl/pinctrl-st.txt b/Documentation/devicetree/bindings/pinctrl/pinctrl-st.txt
index 4bd5be0e5e7..26bcb18f4e6 100644
--- a/Documentation/devicetree/bindings/pinctrl/pinctrl-st.txt
+++ b/Documentation/devicetree/bindings/pinctrl/pinctrl-st.txt
@@ -83,7 +83,7 @@ Example:
 		reg 		= <0xfe61f080 0x4>;
 		reg-names	= "irqmux";
 		interrupts 	= <GIC_SPI 180 IRQ_TYPE_LEVEL_HIGH>;
-		interrupts-names = "irqmux";
+		interrupt-names	= "irqmux";
 		ranges 		= <0 0xfe610000 0x5000>;
 
 		PIO0: gpio@fe610000 {
@@ -165,7 +165,7 @@ sdhci0:sdhci@fe810000{
 	interrupt-parent = <&PIO3>;
 	#interrupt-cells = <2>;
 	interrupts = <3 IRQ_TYPE_LEVEL_HIGH>; /* Interrupt line via PIO3-3 */
-	interrupts-names = "card-detect";
+	interrupt-names = "card-detect";
 	pinctrl-names = "default";
 	pinctrl-0	= <&pinctrl_mmc>;
 };
diff --git a/Documentation/devicetree/bindings/pinctrl/qcom,apq8064-pinctrl.txt b/Documentation/devicetree/bindings/pinctrl/qcom,apq8064-pinctrl.txt
new file mode 100644
index 00000000000..7181f925aca
--- /dev/null
+++ b/Documentation/devicetree/bindings/pinctrl/qcom,apq8064-pinctrl.txt
@@ -0,0 +1,88 @@
+Qualcomm APQ8064 TLMM block
+
+Required properties:
+- compatible: "qcom,apq8064-pinctrl"
+- reg: Should be the base address and length of the TLMM block.
+- interrupts: Should be the parent IRQ of the TLMM block.
+- interrupt-controller: Marks the device node as an interrupt controller.
+- #interrupt-cells: Should be two.
+- gpio-controller: Marks the device node as a GPIO controller.
+- #gpio-cells : Should be two.
+                The first cell is the gpio pin number and the
+                second cell is used for optional parameters.
+
+Please refer to ../gpio/gpio.txt and ../interrupt-controller/interrupts.txt for
+a general description of GPIO and interrupt bindings.
+
+Please refer to pinctrl-bindings.txt in this directory for details of the
+common pinctrl bindings used by client devices, including the meaning of the
+phrase "pin configuration node".
+
+Qualcomm's pin configuration nodes act as a container for an abitrary number of
+subnodes. Each of these subnodes represents some desired configuration for a
+pin, a group, or a list of pins or groups. This configuration can include the
+mux function to select on those pin(s)/group(s), and various pin configuration
+parameters, such as pull-up, drive strength, etc.
+
+The name of each subnode is not important; all subnodes should be enumerated
+and processed purely based on their content.
+
+Each subnode only affects those parameters that are explicitly listed. In
+other words, a subnode that lists a mux function but no pin configuration
+parameters implies no information about any pin configuration parameters.
+Similarly, a pin subnode that describes a pullup parameter implies no
+information about e.g. the mux function.
+
+
+The following generic properties as defined in pinctrl-bindings.txt are valid
+to specify in a pin configuration subnode:
+
+ pins, function, bias-disable, bias-pull-down, bias-pull,up, drive-strength,
+ output-low, output-high.
+
+Non-empty subnodes must specify the 'pins' property.
+
+Valid values for pins are:
+  gpio0-gpio89
+
+Valid values for function are:
+  cam_mclk, codec_mic_i2s, codec_spkr_i2s, gsbi1, gsbi2, gsbi3, gsbi4,
+  gsbi4_cam_i2c, gsbi5, gsbi5_spi_cs1, gsbi5_spi_cs2, gsbi5_spi_cs3, gsbi6,
+  gsbi6_spi_cs1, gsbi6_spi_cs2, gsbi6_spi_cs3, gsbi7, gsbi7_spi_cs1,
+  gsbi7_spi_cs2, gsbi7_spi_cs3, gsbi_cam_i2c, hdmi, mi2s, riva_bt, riva_fm,
+  riva_wlan, sdc2, sdc4, slimbus, spkr_i2s, tsif1, tsif2, usb2_hsic,
+
+Example:
+
+	msmgpio: pinctrl@800000 {
+		compatible = "qcom,apq8064-pinctrl";
+		reg = <0x800000 0x4000>;
+
+		gpio-controller;
+		#gpio-cells = <2>;
+		interrupt-controller;
+		#interrupt-cells = <2>;
+		interrupts = <0 32 0x4>;
+
+		pinctrl-names = "default";
+		pinctrl-0 = <&gsbi5_uart_default>;
+
+		gsbi5_uart_default: gsbi5_uart_default {
+			mux {
+				pins = "gpio51", "gpio52";
+				function = "gsbi5";
+			};
+
+			tx {
+				pins = "gpio51";
+				drive-strength = <4>;
+				bias-disable;
+			};
+
+			rx {
+				pins = "gpio52";
+				drive-strength = <2>;
+				bias-pull-up;
+			};
+		};
+	};
diff --git a/Documentation/devicetree/bindings/pinctrl/qcom,ipq8064-pinctrl.txt b/Documentation/devicetree/bindings/pinctrl/qcom,ipq8064-pinctrl.txt
new file mode 100644
index 00000000000..e0d35a40981
--- /dev/null
+++ b/Documentation/devicetree/bindings/pinctrl/qcom,ipq8064-pinctrl.txt
@@ -0,0 +1,95 @@
+Qualcomm IPQ8064 TLMM block
+
+Required properties:
+- compatible: "qcom,ipq8064-pinctrl"
+- reg: Should be the base address and length of the TLMM block.
+- interrupts: Should be the parent IRQ of the TLMM block.
+- interrupt-controller: Marks the device node as an interrupt controller.
+- #interrupt-cells: Should be two.
+- gpio-controller: Marks the device node as a GPIO controller.
+- #gpio-cells : Should be two.
+                The first cell is the gpio pin number and the
+                second cell is used for optional parameters.
+
+Please refer to ../gpio/gpio.txt and ../interrupt-controller/interrupts.txt for
+a general description of GPIO and interrupt bindings.
+
+Please refer to pinctrl-bindings.txt in this directory for details of the
+common pinctrl bindings used by client devices, including the meaning of the
+phrase "pin configuration node".
+
+Qualcomm's pin configuration nodes act as a container for an abitrary number of
+subnodes. Each of these subnodes represents some desired configuration for a
+pin, a group, or a list of pins or groups. This configuration can include the
+mux function to select on those pin(s)/group(s), and various pin configuration
+parameters, such as pull-up, drive strength, etc.
+
+The name of each subnode is not important; all subnodes should be enumerated
+and processed purely based on their content.
+
+Each subnode only affects those parameters that are explicitly listed. In
+other words, a subnode that lists a mux function but no pin configuration
+parameters implies no information about any pin configuration parameters.
+Similarly, a pin subnode that describes a pullup parameter implies no
+information about e.g. the mux function.
+
+
+The following generic properties as defined in pinctrl-bindings.txt are valid
+to specify in a pin configuration subnode:
+
+ pins, function, bias-disable, bias-pull-down, bias-pull,up, drive-strength,
+ output-low, output-high.
+
+Non-empty subnodes must specify the 'pins' property.
+
+Valid values for qcom,pins are:
+  gpio0-gpio68
+   Supports mux, bias, and drive-strength
+
+  sdc3_clk, sdc3_cmd, sdc3_data
+   Supports bias and drive-strength
+
+
+Valid values for function are:
+  mdio, mi2s, pdm, ssbi, spmi, audio_pcm, gsbi1, gsbi2, gsbi4, gsbi5,
+  gsbi5_spi_cs1, gsbi5_spi_cs2, gsbi5_spi_cs3, gsbi6, gsbi7, nss_spi, sdc1,
+  spdif, nand, tsif1, tsif2, usb_fs_n, usb_fs, usb2_hsic, rgmii2, sata,
+  pcie1_rst, pcie1_prsnt, pcie1_pwren_n, pcie1_pwren, pcie1_pwrflt,
+  pcie1_clk_req, pcie2_rst, pcie2_prsnt, pcie2_pwren_n, pcie2_pwren,
+  pcie2_pwrflt, pcie2_clk_req, pcie3_rst, pcie3_prsnt, pcie3_pwren_n,
+  pcie3_pwren, pcie3_pwrflt, pcie3_clk_req, ps_hold
+
+Example:
+
+	pinmux: pinctrl@800000 {
+		compatible = "qcom,ipq8064-pinctrl";
+		reg = <0x800000 0x4000>;
+
+		gpio-controller;
+		#gpio-cells = <2>;
+		interrupt-controller;
+		#interrupt-cells = <2>;
+		interrupts = <0 32 0x4>;
+
+		pinctrl-names = "default";
+		pinctrl-0 = <&gsbi5_uart_default>;
+
+		gsbi5_uart_default: gsbi5_uart_default {
+			mux {
+				pins = "gpio18", "gpio19";
+				function = "gsbi5";
+			};
+
+			tx {
+				pins = "gpio18";
+				drive-strength = <4>;
+				bias-disable;
+			};
+
+			rx {
+				pins = "gpio19";
+				drive-strength = <2>;
+				bias-pull-up;
+			};
+		};
+	};
diff --git a/Documentation/devicetree/bindings/pinctrl/qcom,msm8974-pinctrl.txt b/Documentation/devicetree/bindings/pinctrl/qcom,msm8974-pinctrl.txt
index 9fb89e3f61e..73262b575df 100644
--- a/Documentation/devicetree/bindings/pinctrl/qcom,msm8974-pinctrl.txt
+++ b/Documentation/devicetree/bindings/pinctrl/qcom,msm8974-pinctrl.txt
@@ -50,7 +50,27 @@ Valid values for pins are:
     Supports bias and drive-strength
 
 Valid values for function are:
-  blsp_i2c2, blsp_i2c6, blsp_i2c11, blsp_spi1, blsp_uart2, blsp_uart8, slimbus
+  cci_i2c0, cci_i2c1, uim1, uim2, uim_batt_alarm,
+  blsp_uim1, blsp_uart1, blsp_i2c1, blsp_spi1,
+  blsp_uim2, blsp_uart2, blsp_i2c2, blsp_spi2,
+  blsp_uim3, blsp_uart3, blsp_i2c3, blsp_spi3,
+  blsp_uim4, blsp_uart4, blsp_i2c4, blsp_spi4,
+  blsp_uim5, blsp_uart5, blsp_i2c5, blsp_spi5,
+  blsp_uim6, blsp_uart6, blsp_i2c6, blsp_spi6,
+  blsp_uim7, blsp_uart7, blsp_i2c7, blsp_spi7,
+  blsp_uim8, blsp_uart8, blsp_i2c8, blsp_spi8,
+  blsp_uim9, blsp_uart9, blsp_i2c9, blsp_spi9,
+  blsp_uim10, blsp_uart10, blsp_i2c10, blsp_spi10,
+  blsp_uim11, blsp_uart11, blsp_i2c11, blsp_spi11,
+  blsp_uim12, blsp_uart12, blsp_i2c12, blsp_spi12,
+  blsp_spi1_cs1, blsp_spi2_cs2, blsp_spi_cs3, blsp_spi2_cs1, blsp_spi2_cs2
+  blsp_spi2_cs3, blsp_spi10_cs1, blsp_spi10_cs2, blsp_spi10_cs3,
+  sdc3, sdc4, gcc_gp_clk1, gcc_gp_clk2, gcc_gp_clk3, cci_timer0, cci_timer1,
+  cci_timer2, cci_timer3, cci_async_in0, cci_async_in1, cci_async_in2,
+  cam_mckl0, cam_mclk1, cam_mclk2, cam_mclk3, mdp_vsync, hdmi_cec, hdmi_ddc,
+  hdmi_hpd, edp_hpd, gp_pdm0, gp_pdm1, gp_pdm2, gp_pdm3, gp0_clk, gp1_clk,
+  gp_mn, tsif1, tsif2, hsic, grfc, audio_ref_clk, qua_mi2s, pri_mi2s, spkr_mi2s,
+  ter_mi2s, sec_mi2s, bt, fm, wlan, slimbus
 
   (Note that this is not yet the complete list of functions)
 
diff --git a/Documentation/devicetree/bindings/pinctrl/rockchip,pinctrl.txt b/Documentation/devicetree/bindings/pinctrl/rockchip,pinctrl.txt
index f378d342aae..cefef741a40 100644
--- a/Documentation/devicetree/bindings/pinctrl/rockchip,pinctrl.txt
+++ b/Documentation/devicetree/bindings/pinctrl/rockchip,pinctrl.txt
@@ -21,13 +21,23 @@ defined as gpio sub-nodes of the pinmux controller.
 Required properties for iomux controller:
   - compatible: one of "rockchip,rk2928-pinctrl", "rockchip,rk3066a-pinctrl"
 		       "rockchip,rk3066b-pinctrl", "rockchip,rk3188-pinctrl"
+  - rockchip,grf: phandle referencing a syscon providing the
+	 "general register files"
+
+Optional properties for iomux controller:
+  - rockchip,pmu: phandle referencing a syscon providing the pmu registers
+	 as some SoCs carry parts of the iomux controller registers there.
+	 Required for at least rk3188 and rk3288.
+
+Deprecated properties for iomux controller:
   - reg: first element is the general register space of the iomux controller
-	 second element is the separate pull register space of the rk3188
+	 It should be large enough to contain also separate pull registers.
+	 second element is the separate pull register space of the rk3188.
+	 Use rockchip,grf and rockchip,pmu described above instead.
 
 Required properties for gpio sub nodes:
   - compatible: "rockchip,gpio-bank", "rockchip,rk3188-gpio-bank0"
   - reg: register of the gpio bank (different than the iomux registerset)
-         second element: separate pull register for rk3188 bank0
   - interrupts: base interrupt of the gpio bank in the interrupt controller
   - clocks: clock that drives this bank
   - gpio-controller: identifies the node as a gpio controller and pin bank.
@@ -39,6 +49,10 @@ Required properties for gpio sub nodes:
     cells should use the standard two-cell scheme described in
     bindings/interrupt-controller/interrupts.txt
 
+Deprecated properties for gpio sub nodes:
+  - reg: second element: separate pull register for rk3188 bank0, use
+	 rockchip,pmu described above instead
+
 Required properties for pin configuration node:
   - rockchip,pins: 3 integers array, represents a group of pins mux and config
     setting. The format is rockchip,pins = <PIN_BANK PIN_BANK_IDX MUX &phandle>.
@@ -54,7 +68,8 @@ Examples:
 
 pinctrl@20008000 {
 	compatible = "rockchip,rk3066a-pinctrl";
-	reg = <0x20008000 0x150>;
+	rockchip,grf = <&grf>;
+
 	#address-cells = <1>;
 	#size-cells = <1>;
 	ranges;
@@ -103,16 +118,15 @@ Example for rk3188:
 
 	pinctrl@20008000 {
 		compatible = "rockchip,rk3188-pinctrl";
-		reg = <0x20008000 0xa0>,
-		      <0x20008164 0x1a0>;
+		rockchip,grf = <&grf>;
+		rockchip,pmu = <&pmu>;
 		#address-cells = <1>;
 		#size-cells = <1>;
 		ranges;
 
 		gpio0: gpio0@0x2000a000 {
 			compatible = "rockchip,rk3188-gpio-bank0";
-			reg = <0x2000a000 0x100>,
-			      <0x20004064 0x8>;
+			reg = <0x2000a000 0x100>;
 			interrupts = <GIC_SPI 54 IRQ_TYPE_LEVEL_HIGH>;
 			clocks = <&clk_gates8 9>;
 
diff --git a/Documentation/devicetree/bindings/power/reset/keystone-reset.txt b/Documentation/devicetree/bindings/power/reset/keystone-reset.txt
new file mode 100644
index 00000000000..c82f12e2d85
--- /dev/null
+++ b/Documentation/devicetree/bindings/power/reset/keystone-reset.txt
@@ -0,0 +1,67 @@
+* Device tree bindings for Texas Instruments keystone reset
+
+This node is intended to allow SoC reset in case of software reset
+of selected watchdogs.
+
+The Keystone SoCs can contain up to 4 watchdog timers to reset
+SoC. Each watchdog timer event input is connected to the Reset Mux
+block. The Reset Mux block can be configured to cause reset or not.
+
+Additionally soft or hard reset can be configured.
+
+Required properties:
+
+- compatible:		ti,keystone-reset
+
+- ti,syscon-pll:	phandle/offset pair. The phandle to syscon used to
+			access pll controller registers and the offset to use
+			reset control registers.
+
+- ti,syscon-dev:	phandle/offset pair. The phandle to syscon used to
+			access device state control registers and the offset
+			in order to use mux block registers for all watchdogs.
+
+Optional properties:
+
+- ti,soft-reset:	Boolean option indicating soft reset.
+			By default hard reset is used.
+
+- ti,wdt-list:		WDT list that can cause SoC reset. It's not related
+			to WDT driver, it's just needed to enable a SoC related
+			reset that's triggered by one of WDTs. The list is
+			in format: <0>, <2>; It can be in random order and
+			begins from 0 to 3, as keystone can contain up to 4 SoC
+			reset watchdogs and can be in random order.
+
+Example 1:
+Setup keystone reset so that in case software reset or
+WDT0 is triggered it issues hard reset for SoC.
+
+pllctrl: pll-controller@02310000 {
+	compatible = "ti,keystone-pllctrl", "syscon";
+	reg = <0x02310000 0x200>;
+};
+
+devctrl: device-state-control@02620000 {
+	compatible = "ti,keystone-devctrl", "syscon";
+	reg = <0x02620000 0x1000>;
+};
+
+rstctrl: reset-controller {
+	compatible = "ti,keystone-reset";
+	ti,syscon-pll = <&pllctrl 0xe4>;
+	ti,syscon-dev = <&devctrl 0x328>;
+	ti,wdt-list = <0>;
+};
+
+Example 2:
+Setup keystone reset so that in case of software reset or
+WDT0 or WDT2 is triggered it issues soft reset for SoC.
+
+rstctrl: reset-controller {
+	compatible = "ti,keystone-reset";
+	ti,syscon-pll = <&pllctrl 0xe4>;
+	ti,syscon-dev = <&devctrl 0x328>;
+	ti,wdt-list = <0>, <2>;
+	ti,soft-reset;
+};
diff --git a/Documentation/devicetree/bindings/power_supply/axxia-reset.txt b/Documentation/devicetree/bindings/power_supply/axxia-reset.txt
new file mode 100644
index 00000000000..47e720d249d
--- /dev/null
+++ b/Documentation/devicetree/bindings/power_supply/axxia-reset.txt
@@ -0,0 +1,20 @@
+Axxia Restart Driver
+
+This driver can do reset of the Axxia SoC. It uses the registers in the syscon
+block to initiate a chip reset.
+
+Required Properties:
+  -compatible: "lsi,axm55xx-reset"
+  -syscon: phandle to the syscon node.
+
+Example:
+
+	syscon: syscon@2010030000 {
+		compatible = "lsi,axxia-syscon", "syscon";
+		reg = <0x20 0x10030000 0 0x2000>;
+	};
+
+	reset: reset@2010031000 {
+		compatible = "lsi,axm55xx-reset";
+		syscon = <&syscon>;
+	};
diff --git a/Documentation/devicetree/bindings/regulator/ltc3589.txt b/Documentation/devicetree/bindings/regulator/ltc3589.txt
new file mode 100644
index 00000000000..80105303614
--- /dev/null
+++ b/Documentation/devicetree/bindings/regulator/ltc3589.txt
@@ -0,0 +1,99 @@
+Linear Technology LTC3589, LTC3589-1, and LTC3589-2 8-output regulators
+
+Required properties:
+- compatible: "lltc,ltc3589", "lltc,ltc3589-1" or "lltc,ltc3589-2"
+- reg: I2C slave address
+
+Required child node:
+- regulators: Contains eight regulator child nodes sw1, sw2, sw3, bb-out,
+  ldo1, ldo2, ldo3, and ldo4, specifying the initialization data as
+  documented in Documentation/devicetree/bindings/regulator/regulator.txt.
+
+Each regulator is defined using the standard binding for regulators. The
+nodes for sw1, sw2, sw3, bb-out, ldo1, and ldo2 additionally need to specify
+the resistor values of their external feedback voltage dividers:
+
+Required properties (not on ldo3, ldo4):
+- lltc,fb-voltage-divider: An array of two integers containing the resistor
+  values R1 and R2 of the feedback voltage divider in ohms.
+
+Regulators sw1, sw2, sw3, and ldo2 can regulate the feedback reference from
+0.3625 V to 0.75 V in 12.5 mV steps. The output voltage thus ranges between
+0.3625 * (1 + R1/R2) V and 0.75 * (1 + R1/R2) V. Regulators bb-out and ldo1
+have a fixed 0.8 V reference and thus output 0.8 * (1 + R1/R2) V. The ldo3
+regulator is fixed to 1.8 V on LTC3589 and to 2.8 V on LTC3589-1,2. The ldo4
+regulator can output between 1.8 V and 3.3 V on LTC3589 and between 1.2 V
+and 3.2 V on LTC3589-1,2 in four steps. The ldo1 standby regulator can not
+be disabled and thus should have the regulator-always-on property set.
+
+Example:
+
+	ltc3589: pmic@34 {
+		compatible = "lltc,ltc3589-1";
+		reg = <0x34>;
+
+		regulators {
+			sw1_reg: sw1 {
+				regulator-min-microvolt = <591930>;
+				regulator-max-microvolt = <1224671>;
+				lltc,fb-voltage-divider = <100000 158000>;
+				regulator-ramp-delay = <7000>;
+				regulator-boot-on;
+				regulator-always-on;
+			};
+
+			sw2_reg: sw2 {
+				regulator-min-microvolt = <704123>;
+				regulator-max-microvolt = <1456803>;
+				lltc,fb-voltage-divider = <180000 191000>;
+				regulator-ramp-delay = <7000>;
+				regulator-boot-on;
+				regulator-always-on;
+			};
+
+			sw3_reg: sw3 {
+				regulator-min-microvolt = <1341250>;
+				regulator-max-microvolt = <2775000>;
+				lltc,fb-voltage-divider = <270000 100000>;
+				regulator-ramp-delay = <7000>;
+				regulator-boot-on;
+				regulator-always-on;
+			};
+
+			bb_out_reg: bb-out {
+				regulator-min-microvolt = <3387341>;
+				regulator-max-microvolt = <3387341>;
+				lltc,fb-voltage-divider = <511000 158000>;
+				regulator-boot-on;
+				regulator-always-on;
+			};
+
+			ldo1_reg: ldo1 {
+				regulator-min-microvolt = <1306329>;
+				regulator-max-microvolt = <1306329>;
+				lltc,fb-voltage-divider = <100000 158000>;
+				regulator-boot-on;
+				regulator-always-on;
+			};
+
+			ldo2_reg: ldo2 {
+				regulator-min-microvolt = <704123>;
+				regulator-max-microvolt = <1456806>;
+				lltc,fb-voltage-divider = <180000 191000>;
+				regulator-ramp-delay = <7000>;
+				regulator-boot-on;
+				regulator-always-on;
+			};
+
+			ldo3_reg: ldo3 {
+				regulator-min-microvolt = <2800000>;
+				regulator-max-microvolt = <2800000>;
+				regulator-boot-on;
+			};
+
+			ldo4_reg: ldo4 {
+				regulator-min-microvolt = <1200000>;
+				regulator-max-microvolt = <3200000>;
+			};
+		};
+	};
diff --git a/Documentation/devicetree/bindings/regulator/tps65090.txt b/Documentation/devicetree/bindings/regulator/tps65090.txt
index 313a60ba61d..340980239ea 100644
--- a/Documentation/devicetree/bindings/regulator/tps65090.txt
+++ b/Documentation/devicetree/bindings/regulator/tps65090.txt
@@ -21,6 +21,10 @@ Optional properties:
   number should be provided. If it is externally controlled and no GPIO
   entry then driver will just configure this rails as external control
   and will not provide any enable/disable APIs.
+- ti,overcurrent-wait: This is applicable to FET registers, which have a
+  poorly defined "overcurrent wait" field.  If this property is present it
+  should be between 0 - 3.  If this property isn't present we won't touch the
+  "overcurrent wait" field and we'll leave it to the BIOS/EC to deal with.
 
 Each regulator is defined using the standard binding for regulators.
 
diff --git a/Documentation/devicetree/bindings/reset/allwinner,sunxi-clock-reset.txt b/Documentation/devicetree/bindings/reset/allwinner,sunxi-clock-reset.txt
new file mode 100644
index 00000000000..c8f77571488
--- /dev/null
+++ b/Documentation/devicetree/bindings/reset/allwinner,sunxi-clock-reset.txt
@@ -0,0 +1,21 @@
+Allwinner sunxi Peripheral Reset Controller
+===========================================
+
+Please also refer to reset.txt in this directory for common reset
+controller binding usage.
+
+Required properties:
+- compatible: Should be one of the following:
+  "allwinner,sun6i-a31-ahb1-reset"
+  "allwinner,sun6i-a31-clock-reset"
+- reg: should be register base and length as documented in the
+  datasheet
+- #reset-cells: 1, see below
+
+example:
+
+ahb1_rst: reset@01c202c0 {
+	#reset-cells = <1>;
+	compatible = "allwinner,sun6i-a31-ahb1-reset";
+	reg = <0x01c202c0 0xc>;
+};
diff --git a/Documentation/devicetree/bindings/arm/altera/socfpga-reset.txt b/Documentation/devicetree/bindings/reset/socfpga-reset.txt
index ecdb57d69db..32c1c8bfd5d 100644
--- a/Documentation/devicetree/bindings/arm/altera/socfpga-reset.txt
+++ b/Documentation/devicetree/bindings/reset/socfpga-reset.txt
@@ -3,9 +3,11 @@ Altera SOCFPGA Reset Manager
 Required properties:
 - compatible : "altr,rst-mgr"
 - reg : Should contain 1 register ranges(address and length)
+- #reset-cells: 1
 
 Example:
 	 rstmgr@ffd05000 {
+		#reset-cells = <1>;
 		compatible = "altr,rst-mgr";
 		reg = <0xffd05000 0x1000>;
 	};
diff --git a/Documentation/devicetree/bindings/serial/atmel-usart.txt b/Documentation/devicetree/bindings/serial/atmel-usart.txt
index 17c1042b2df..a6391e70a8f 100644
--- a/Documentation/devicetree/bindings/serial/atmel-usart.txt
+++ b/Documentation/devicetree/bindings/serial/atmel-usart.txt
@@ -13,8 +13,9 @@ Required properties:
 Optional properties:
 - atmel,use-dma-rx: use of PDC or DMA for receiving data
 - atmel,use-dma-tx: use of PDC or DMA for transmitting data
-- rts-gpios: specify a GPIO for RTS line. It will use specified PIO instead of the peripheral
-  function pin for the USART RTS feature. If unsure, don't specify this property.
+- {rts,cts,dtr,dsr,rng,dcd}-gpios: specify a GPIO for RTS/CTS/DTR/DSR/RI/DCD line respectively.
+  It will use specified PIO instead of the peripheral function pin for the USART feature.
+  If unsure, don't specify this property.
 - add dma bindings for dma transfer:
 	- dmas: DMA specifier, consisting of a phandle to DMA controller node,
 		memory peripheral interface and USART DMA channel ID, FIFO configuration.
@@ -35,7 +36,12 @@ Example:
 		clock-names = "usart";
 		atmel,use-dma-rx;
 		atmel,use-dma-tx;
-		rts-gpios = <&pioD 15 0>;
+		rts-gpios = <&pioD 15 GPIO_ACTIVE_LOW>;
+		cts-gpios = <&pioD 16 GPIO_ACTIVE_LOW>;
+		dtr-gpios = <&pioD 17 GPIO_ACTIVE_LOW>;
+		dsr-gpios = <&pioD 18 GPIO_ACTIVE_LOW>;
+		dcd-gpios = <&pioD 20 GPIO_ACTIVE_LOW>;
+		rng-gpios = <&pioD 19 GPIO_ACTIVE_LOW>;
 	};
 
 - use DMA:
diff --git a/Documentation/devicetree/bindings/serial/efm32-uart.txt b/Documentation/devicetree/bindings/serial/efm32-uart.txt
index 1984bdfbd54..3ca01336b83 100644
--- a/Documentation/devicetree/bindings/serial/efm32-uart.txt
+++ b/Documentation/devicetree/bindings/serial/efm32-uart.txt
@@ -1,7 +1,7 @@
 * Energymicro efm32 UART
 
 Required properties:
-- compatible : Should be "efm32,uart"
+- compatible : Should be "energymicro,efm32-uart"
 - reg : Address and length of the register set
 - interrupts : Should contain uart interrupt
 
@@ -13,7 +13,7 @@ Optional properties:
 Example:
 
 uart@0x4000c400 {
-	compatible = "efm32,uart";
+	compatible = "energymicro,efm32-uart";
 	reg = <0x4000c400 0x400>;
 	interrupts = <15>;
 	efm32,location = <0>;
diff --git a/Documentation/devicetree/bindings/serial/nxp,sc16is7xx.txt b/Documentation/devicetree/bindings/serial/nxp,sc16is7xx.txt
new file mode 100644
index 00000000000..246c795668d
--- /dev/null
+++ b/Documentation/devicetree/bindings/serial/nxp,sc16is7xx.txt
@@ -0,0 +1,33 @@
+* NXP SC16IS7xx advanced Universal Asynchronous Receiver-Transmitter (UART)
+
+Required properties:
+- compatible: Should be one of the following:
+  - "nxp,sc16is740" for NXP SC16IS740,
+  - "nxp,sc16is741" for NXP SC16IS741,
+  - "nxp,sc16is750" for NXP SC16IS750,
+  - "nxp,sc16is752" for NXP SC16IS752,
+  - "nxp,sc16is760" for NXP SC16IS760,
+  - "nxp,sc16is762" for NXP SC16IS762.
+- reg: I2C address of the SC16IS7xx device.
+- interrupt-parent: The phandle for the interrupt controller that
+  services interrupts for this IC.
+- interrupts: Should contain the UART interrupt
+- clocks: Reference to the IC source clock.
+
+Optional properties:
+- gpio-controller: Marks the device node as a GPIO controller.
+- #gpio-cells: Should be two. The first cell is the GPIO number and
+  the second cell is used to specify the GPIO polarity:
+    0 = active high,
+    1 = active low.
+
+Example:
+        sc16is750: sc16is750@51 {
+                compatible = "nxp,sc16is750";
+                reg = <0x51>;
+                clocks = <&clk20m>;
+                interrupt-parent = <&gpio3>;
+                interrupts = <7 IRQ_TYPE_EDGE_FALLING>;
+                gpio-controller;
+                #gpio-cells = <2>;
+        };
diff --git a/Documentation/devicetree/bindings/serial/of-serial.txt b/Documentation/devicetree/bindings/serial/of-serial.txt
index 1928a3e83cd..77054772a8f 100644
--- a/Documentation/devicetree/bindings/serial/of-serial.txt
+++ b/Documentation/devicetree/bindings/serial/of-serial.txt
@@ -37,6 +37,7 @@ Optional properties:
 - 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.
+- has-hw-flow-control: the hardware has flow control capability.
 
 Example:
 
diff --git a/Documentation/devicetree/bindings/serial/renesas,sci-serial.txt b/Documentation/devicetree/bindings/serial/renesas,sci-serial.txt
index 53e6c175db6..64fd7dec1bb 100644
--- a/Documentation/devicetree/bindings/serial/renesas,sci-serial.txt
+++ b/Documentation/devicetree/bindings/serial/renesas,sci-serial.txt
@@ -4,6 +4,7 @@ Required properties:
 
   - compatible: Must contain one of the following:
 
+    - "renesas,scif-r8a7779" for R8A7779 (R-Car H1) SCIF compatible UART.
     - "renesas,scif-r8a7790" for R8A7790 (R-Car H2) SCIF compatible UART.
     - "renesas,scifa-r8a7790" for R8A7790 (R-Car H2) SCIFA compatible UART.
     - "renesas,scifb-r8a7790" for R8A7790 (R-Car H2) SCIFB compatible UART.
diff --git a/Documentation/devicetree/bindings/soc/qcom/qcom,gsbi.txt b/Documentation/devicetree/bindings/soc/qcom/qcom,gsbi.txt
new file mode 100644
index 00000000000..4ce24d425bf
--- /dev/null
+++ b/Documentation/devicetree/bindings/soc/qcom/qcom,gsbi.txt
@@ -0,0 +1,78 @@
+QCOM GSBI (General Serial Bus Interface) Driver
+
+The GSBI controller is modeled as a node with zero or more child nodes, each
+representing a serial sub-node device that is mux'd as part of the GSBI
+configuration settings.  The mode setting will govern the input/output mode of
+the 4 GSBI IOs.
+
+Required properties:
+- compatible: must contain "qcom,gsbi-v1.0.0" for APQ8064/IPQ8064
+- reg: Address range for GSBI registers
+- clocks: required clock
+- clock-names: must contain "iface" entry
+- qcom,mode : indicates MUX value for configuration of the serial interface.
+  Please reference dt-bindings/soc/qcom,gsbi.h for valid mux values.
+
+Optional properties:
+- qcom,crci : indicates CRCI MUX value for QUP CRCI ports.  Please reference
+  dt-bindings/soc/qcom,gsbi.h for valid CRCI mux values.
+
+Required properties if child node exists:
+- #address-cells: Must be 1
+- #size-cells: Must be 1
+- ranges: Must be present
+
+Properties for children:
+
+A GSBI controller node can contain 0 or more child nodes representing serial
+devices.  These serial devices can be a QCOM UART, I2C controller, spi
+controller, or some combination of aforementioned devices.
+
+See the following for child node definitions:
+Documentation/devicetree/bindings/i2c/qcom,i2c-qup.txt
+Documentation/devicetree/bindings/spi/qcom,spi-qup.txt
+Documentation/devicetree/bindings/serial/qcom,msm-uartdm.txt
+
+Example for APQ8064:
+
+#include <dt-bindings/soc/qcom,gsbi.h>
+
+	gsbi4@16300000 {
+		compatible = "qcom,gsbi-v1.0.0";
+		reg = <0x16300000 0x100>;
+		clocks = <&gcc GSBI4_H_CLK>;
+		clock-names = "iface";
+		#address-cells = <1>;
+		#size-cells = <1>;
+		ranges;
+		qcom,mode = <GSBI_PROT_I2C_UART>;
+		qcom,crci = <GSBI_CRCI_QUP>;
+
+		/* child nodes go under here */
+
+		i2c_qup4: i2c@16380000 {
+		        compatible = "qcom,i2c-qup-v1.1.1";
+		        reg = <0x16380000 0x1000>;
+		        interrupts = <0 153 0>;
+
+		        clocks = <&gcc GSBI4_QUP_CLK>, <&gcc GSBI4_H_CLK>;
+		        clock-names = "core", "iface";
+
+		        clock-frequency = <200000>;
+
+		        #address-cells = <1>;
+		        #size-cells = <0>;
+
+		 };
+
+		uart4:	serial@16340000 {
+			compatible = "qcom,msm-uartdm-v1.3", "qcom,msm-uartdm";
+			reg = <0x16340000 0x1000>,
+				<0x16300000 0x1000>;
+			interrupts = <0 152 0x0>;
+			clocks = <&gcc GSBI4_UART_CLK>, <&gcc GSBI4_H_CLK>;
+			clock-names = "core", "iface";
+			status = "ok";
+		};
+	};
+
diff --git a/Documentation/devicetree/bindings/sound/davinci-mcasp-audio.txt b/Documentation/devicetree/bindings/sound/davinci-mcasp-audio.txt
index 569b26c4a81..60ca0799645 100644
--- a/Documentation/devicetree/bindings/sound/davinci-mcasp-audio.txt
+++ b/Documentation/devicetree/bindings/sound/davinci-mcasp-audio.txt
@@ -47,7 +47,7 @@ mcasp0: mcasp0@1d00000 {
 	reg = <0x100000 0x3000>;
 	reg-names "mpu";
 	interrupts = <82>, <83>;
-	interrupts-names = "tx", "rx";
+	interrupt-names = "tx", "rx";
 	op-mode = <0>;		/* MCASP_IIS_MODE */
 	tdm-slots = <2>;
 	serial-dir = <
diff --git a/Documentation/devicetree/bindings/sound/tlv320aic31xx.txt b/Documentation/devicetree/bindings/sound/tlv320aic31xx.txt
index 74c66dee3e1..eff12be5e78 100644
--- a/Documentation/devicetree/bindings/sound/tlv320aic31xx.txt
+++ b/Documentation/devicetree/bindings/sound/tlv320aic31xx.txt
@@ -13,6 +13,9 @@ Required properties:
     "ti,tlv320aic3111" - TLV320AIC3111 (stereo speaker amp, MiniDSP)
 
 - reg - <int> -  I2C slave address
+- HPVDD-supply, SPRVDD-supply, SPLVDD-supply, AVDD-supply, IOVDD-supply,
+  DVDD-supply : power supplies for the device as covered in
+  Documentation/devicetree/bindings/regulator/regulator.txt
 
 
 Optional properties:
@@ -24,9 +27,6 @@ Optional properties:
         3 or MICBIAS_AVDD - MICBIAS output is connected to AVDD
 	If this node is not mentioned or if the value is unknown, then
 	micbias	is set to 2.0V.
-- HPVDD-supply, SPRVDD-supply, SPLVDD-supply, AVDD-supply, IOVDD-supply,
-  DVDD-supply : power supplies for the device as covered in
-  Documentation/devicetree/bindings/regulator/regulator.txt
 
 CODEC output pins:
   * HPL
diff --git a/Documentation/devicetree/bindings/spi/fsl-spi.txt b/Documentation/devicetree/bindings/spi/fsl-spi.txt
index b032dd76e9d..a2331372068 100644
--- a/Documentation/devicetree/bindings/spi/fsl-spi.txt
+++ b/Documentation/devicetree/bindings/spi/fsl-spi.txt
@@ -42,6 +42,10 @@ Required properties:
 - interrupts : should contain eSPI interrupt, the device has one interrupt.
 - fsl,espi-num-chipselects : the number of the chipselect signals.
 
+Optional properties:
+- fsl,csbef: chip select assertion time in bits before frame starts
+- fsl,csaft: chip select negation time in bits after frame ends
+
 Example:
 	spi@110000 {
 		#address-cells = <1>;
@@ -51,4 +55,6 @@ Example:
 		interrupts = <53 0x2>;
 		interrupt-parent = <&mpic>;
 		fsl,espi-num-chipselects = <4>;
+		fsl,csbef = <1>;
+		fsl,csaft = <1>;
 	};
diff --git a/Documentation/devicetree/bindings/spi/spi-bus.txt b/Documentation/devicetree/bindings/spi/spi-bus.txt
index e5a4d1b4acf..22d57404fdc 100644
--- a/Documentation/devicetree/bindings/spi/spi-bus.txt
+++ b/Documentation/devicetree/bindings/spi/spi-bus.txt
@@ -55,6 +55,8 @@ contain the following properties.
     		chip select active high
 - spi-3wire       - (optional) Empty property indicating device requires
     		    3-wire mode.
+- spi-lsb-first   - (optional) Empty property indicating device requires
+		LSB first mode.
 - spi-tx-bus-width - (optional) The bus width(number of data wires) that
                       used for MOSI. Defaults to 1 if not present.
 - spi-rx-bus-width - (optional) The bus width(number of data wires) that
diff --git a/Documentation/devicetree/bindings/spi/spi-cadence.txt b/Documentation/devicetree/bindings/spi/spi-cadence.txt
new file mode 100644
index 00000000000..94f09141a4f
--- /dev/null
+++ b/Documentation/devicetree/bindings/spi/spi-cadence.txt
@@ -0,0 +1,31 @@
+Cadence SPI controller Device Tree Bindings
+-------------------------------------------
+
+Required properties:
+- compatible		: Should be "cdns,spi-r1p6" or "xlnx,zynq-spi-r1p6".
+- reg			: Physical base address and size of SPI registers map.
+- interrupts		: Property with a value describing the interrupt
+			  number.
+- interrupt-parent	: Must be core interrupt controller
+- clock-names		: List of input clock names - "ref_clk", "pclk"
+			  (See clock bindings for details).
+- clocks		: Clock phandles (see clock bindings for details).
+
+Optional properties:
+- num-cs		: Number of chip selects used.
+			  If a decoder is used, this will be the number of
+			  chip selects after the decoder.
+- is-decoded-cs		: Flag to indicate whether decoder is used or not.
+
+Example:
+
+	spi@e0007000 {
+		compatible = "xlnx,zynq-spi-r1p6";
+		clock-names = "ref_clk", "pclk";
+		clocks = <&clkc 26>, <&clkc 35>;
+		interrupt-parent = <&intc>;
+		interrupts = <0 49 4>;
+		num-cs = <4>;
+		is-decoded-cs = <0>;
+		reg = <0xe0007000 0x1000>;
+	} ;
diff --git a/Documentation/devicetree/bindings/spi/spi-dw.txt b/Documentation/devicetree/bindings/spi/spi-dw.txt
new file mode 100644
index 00000000000..7b63ed60199
--- /dev/null
+++ b/Documentation/devicetree/bindings/spi/spi-dw.txt
@@ -0,0 +1,24 @@
+Synopsys DesignWare SPI master
+
+Required properties:
+- compatible: should be "snps,designware-spi"
+- #address-cells: see spi-bus.txt
+- #size-cells: see spi-bus.txt
+- reg: address and length of the spi master registers
+- interrupts: should contain one interrupt
+- clocks: spi clock phandle
+- num-cs: see spi-bus.txt
+
+Optional properties:
+- cs-gpios: see spi-bus.txt
+
+Example:
+
+spi: spi@4020a000 {
+	compatible = "snps,designware-spi";
+	interrupts = <11 1>;
+	reg = <0x4020a000 0x1000>;
+	clocks = <&pclk>;
+	num-cs = <2>;
+	cs-gpios = <&banka 0 0>;
+};
diff --git a/Documentation/devicetree/bindings/spmi/spmi.txt b/Documentation/devicetree/bindings/spmi/spmi.txt
index 462a42fb3a1..4bb10d161a2 100644
--- a/Documentation/devicetree/bindings/spmi/spmi.txt
+++ b/Documentation/devicetree/bindings/spmi/spmi.txt
@@ -26,7 +26,7 @@ Each child node must have one and only one 'reg' entry of type SPMI_USID.
 		reg = <...>;
 
 		#address-cells = <2>;
-		#size-cells <0>;
+		#size-cells = <0>;
 
 		child@0 {
 			compatible = "...";
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 3be5ce7a965..e75f0e549ff 100644
--- a/Documentation/devicetree/bindings/staging/imx-drm/fsl-imx-drm.txt
+++ b/Documentation/devicetree/bindings/staging/imx-drm/fsl-imx-drm.txt
@@ -61,6 +61,7 @@ Required properties:
 Optional properties:
 - interface_pix_fmt: How this display is connected to the
   display interface. Currently supported types: "rgb24", "rgb565", "bgr666"
+  and "lvds666".
 - 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/usb/ci-hdrc-qcom.txt b/Documentation/devicetree/bindings/usb/ci-hdrc-qcom.txt
new file mode 100644
index 00000000000..f2899b55093
--- /dev/null
+++ b/Documentation/devicetree/bindings/usb/ci-hdrc-qcom.txt
@@ -0,0 +1,17 @@
+Qualcomm CI13xxx (Chipidea) USB controllers
+
+Required properties:
+- compatible:   should contain "qcom,ci-hdrc"
+- reg:          offset and length of the register set in the memory map
+- interrupts:   interrupt-specifier for the controller interrupt.
+- usb-phy:      phandle for the PHY device
+- dr_mode:      Should be "peripheral"
+
+Examples:
+	gadget@f9a55000 {
+		compatible = "qcom,ci-hdrc";
+		reg = <0xf9a55000 0x400>;
+		dr_mode = "peripheral";
+		interrupts = <0 134 0>;
+		usb-phy = <&usbphy0>;
+	};
diff --git a/Documentation/devicetree/bindings/usb/ehci-orion.txt b/Documentation/devicetree/bindings/usb/ehci-orion.txt
index 6bc09ec14c4..17c3bc858b8 100644
--- a/Documentation/devicetree/bindings/usb/ehci-orion.txt
+++ b/Documentation/devicetree/bindings/usb/ehci-orion.txt
@@ -6,6 +6,11 @@ Required properties:
   region.
 - interrupts: The EHCI interrupt
 
+Optional properties:
+- clocks: reference to the clock
+- phys: reference to the USB PHY
+- phy-names: name of the USB PHY, should be "usb"
+
 Example:
 
 	ehci@50000 {
diff --git a/Documentation/devicetree/bindings/usb/exynos-usb.txt b/Documentation/devicetree/bindings/usb/exynos-usb.txt
index d967ba16de6..a3b5990d0f2 100644
--- a/Documentation/devicetree/bindings/usb/exynos-usb.txt
+++ b/Documentation/devicetree/bindings/usb/exynos-usb.txt
@@ -12,6 +12,13 @@ Required properties:
  - interrupts: interrupt number to the cpu.
  - clocks: from common clock binding: handle to usb clock.
  - clock-names: from common clock binding: Shall be "usbhost".
+ - port: if in the SoC there are EHCI phys, they should be listed here.
+   One phy per port. Each port should have following entries:
+	- reg: port number on EHCI controller, e.g
+	       On Exynos5250, port 0 is USB2.0 otg phy
+			      port 1 is HSIC phy0
+			      port 2 is HSIC phy1
+	- phys: from the *Generic PHY* bindings; specifying phy used by port.
 
 Optional properties:
  - samsung,vbus-gpio:  if present, specifies the GPIO that
@@ -27,6 +34,14 @@ Example:
 
 		clocks = <&clock 285>;
 		clock-names = "usbhost";
+
+		#address-cells = <1>;
+		#size-cells = <0>;
+		port@0 {
+		    reg = <0>;
+		    phys = <&usb2phy 1>;
+		    status = "disabled";
+		};
 	};
 
 OHCI
@@ -38,6 +53,13 @@ Required properties:
  - interrupts: interrupt number to the cpu.
  - clocks: from common clock binding: handle to usb clock.
  - clock-names: from common clock binding: Shall be "usbhost".
+ - port: if in the SoC there are OHCI phys, they should be listed here.
+   One phy per port. Each port should have following entries:
+	- reg: port number on OHCI controller, e.g
+	       On Exynos5250, port 0 is USB2.0 otg phy
+			      port 1 is HSIC phy0
+			      port 2 is HSIC phy1
+	- phys: from the *Generic PHY* bindings, specifying phy used by port.
 
 Example:
 	usb@12120000 {
@@ -47,6 +69,15 @@ Example:
 
 		clocks = <&clock 285>;
 		clock-names = "usbhost";
+
+		#address-cells = <1>;
+		#size-cells = <0>;
+		port@0 {
+		    reg = <0>;
+		    phys = <&usb2phy 1>;
+		    status = "disabled";
+		};
+
 	};
 
 DWC3
diff --git a/Documentation/devicetree/bindings/usb/gr-udc.txt b/Documentation/devicetree/bindings/usb/gr-udc.txt
index 0c5118f7a91..e9445224fab 100644
--- a/Documentation/devicetree/bindings/usb/gr-udc.txt
+++ b/Documentation/devicetree/bindings/usb/gr-udc.txt
@@ -12,17 +12,23 @@ Required properties:
 
 - reg : Address and length of the register set for the device
 
-- interrupts : Interrupt numbers for this device
+- interrupts : Interrupt numbers for this device. Either one interrupt number
+	for all interrupts, or one for status related interrupts, one for IN
+	endpoint related interrupts and one for OUT endpoint related interrupts.
 
 Optional properties:
 
-- epobufsizes : An array of buffer sizes for OUT endpoints. If the property is
-	not present, or for endpoints outside of the array, 1024 is assumed by
-	the driver.
-
-- epibufsizes : An array of buffer sizes for IN endpoints. If the property is
-	not present, or for endpoints outside of the array, 1024 is assumed by
-	the driver.
+- epobufsizes : Array of buffer sizes for OUT endpoints when they differ
+	from the default size of 1024. The array is indexed by the OUT endpoint
+	number. If the property is present it typically contains one entry for
+	each OUT endpoint of the core. Fewer entries overrides the default sizes
+	only for as many endpoints as the array contains.
+
+- epibufsizes : Array of buffer sizes for IN endpoints when they differ
+	from the default size of 1024. The array is indexed by the IN endpoint
+	number. If the property is present it typically contains one entry for
+	each IN endpoint of the core. Fewer entries overrides the default sizes
+	only for as many endpoints as the array contains.
 
 For further information look in the documentation for the GLIB IP core library:
 http://www.gaisler.com/products/grlib/grip.pdf
diff --git a/Documentation/devicetree/bindings/usb/msm-hsusb.txt b/Documentation/devicetree/bindings/usb/msm-hsusb.txt
index 5ea26c631e3..2826f2af503 100644
--- a/Documentation/devicetree/bindings/usb/msm-hsusb.txt
+++ b/Documentation/devicetree/bindings/usb/msm-hsusb.txt
@@ -15,3 +15,81 @@ Example EHCI controller device node:
 		usb-phy = <&usb_otg>;
 	};
 
+USB PHY with optional OTG:
+
+Required properties:
+- compatible:   Should contain:
+  "qcom,usb-otg-ci" for chipsets with ChipIdea 45nm PHY
+  "qcom,usb-otg-snps" for chipsets with Synopsys 28nm PHY
+
+- regs:         Offset and length of the register set in the memory map
+- interrupts:   interrupt-specifier for the OTG interrupt.
+
+- clocks:       A list of phandle + clock-specifier pairs for the
+                clocks listed in clock-names
+- clock-names:  Should contain the following:
+  "phy"         USB PHY reference clock
+  "core"        Protocol engine clock
+  "iface"       Interface bus clock
+  "alt_core"    Protocol engine clock for targets with asynchronous
+                reset methodology. (optional)
+
+- vdccx-supply: phandle to the regulator for the vdd supply for
+                digital circuit operation.
+- v1p8-supply:  phandle to the regulator for the 1.8V supply
+- v3p3-supply:  phandle to the regulator for the 3.3V supply
+
+- resets:       A list of phandle + reset-specifier pairs for the
+                resets listed in reset-names
+- reset-names:  Should contain the following:
+  "phy"         USB PHY controller reset
+  "link"        USB LINK controller reset
+
+- qcom,otg-control: OTG control (VBUS and ID notifications) can be one of
+                1 - PHY control
+                2 - PMIC control
+
+Optional properties:
+- dr_mode:      One of "host", "peripheral" or "otg". Defaults to "otg"
+
+- qcom,phy-init-sequence: PHY configuration sequence values. This is related to Device
+                Mode Eye Diagram test. Start address at which these values will be
+                written is ULPI_EXT_VENDOR_SPECIFIC. Value of -1 is reserved as
+                "do not overwrite default value at this address".
+                For example: qcom,phy-init-sequence = < -1 0x63 >;
+                Will update only value at address ULPI_EXT_VENDOR_SPECIFIC + 1.
+
+- qcom,phy-num: Select number of pyco-phy to use, can be one of
+                0 - PHY one, default
+                1 - Second PHY
+                Some platforms may have configuration to allow USB
+                controller work with any of the two HSPHYs present.
+
+- qcom,vdd-levels: This property must be a list of three integer values
+                (no, min, max) where each value represents either a voltage
+                in microvolts or a value corresponding to voltage corner.
+
+Example HSUSB OTG controller device node:
+
+    usb@f9a55000 {
+        compatible = "qcom,usb-otg-snps";
+        reg = <0xf9a55000 0x400>;
+        interrupts = <0 134 0>;
+        dr_mode = "peripheral";
+
+        clocks = <&gcc GCC_XO_CLK>, <&gcc GCC_USB_HS_SYSTEM_CLK>,
+                <&gcc GCC_USB_HS_AHB_CLK>;
+
+        clock-names = "phy", "core", "iface";
+
+        vddcx-supply = <&pm8841_s2_corner>;
+        v1p8-supply = <&pm8941_l6>;
+        v3p3-supply = <&pm8941_l24>;
+
+        resets = <&gcc GCC_USB2A_PHY_BCR>, <&gcc GCC_USB_HS_BCR>;
+        reset-names = "phy", "link";
+
+        qcom,otg-control = <1>;
+        qcom,phy-init-sequence = < -1 0x63 >;
+        qcom,vdd-levels = <1 5 7>;
+	};
diff --git a/Documentation/devicetree/bindings/usb/omap-usb.txt b/Documentation/devicetree/bindings/usb/omap-usb.txt
index 38b2faec419..38d9bb8507c 100644
--- a/Documentation/devicetree/bindings/usb/omap-usb.txt
+++ b/Documentation/devicetree/bindings/usb/omap-usb.txt
@@ -44,7 +44,9 @@ Board specific device node entry
 };
 
 OMAP DWC3 GLUE
- - compatible : Should be "ti,dwc3"
+ - compatible : Should be
+	* "ti,dwc3" for OMAP5 and DRA7
+	* "ti,am437x-dwc3" for AM437x
  - 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
diff --git a/Documentation/devicetree/bindings/usb/usb-ehci.txt b/Documentation/devicetree/bindings/usb/usb-ehci.txt
index ff151ec084c..43c1a4e0676 100644
--- a/Documentation/devicetree/bindings/usb/usb-ehci.txt
+++ b/Documentation/devicetree/bindings/usb/usb-ehci.txt
@@ -15,6 +15,7 @@ Optional properties:
  - clocks : a list of phandle + clock specifier pairs
  - phys : phandle + phy specifier pair
  - phy-names : "usb"
+ - resets : phandle + reset specifier pair
 
 Example (Sequoia 440EPx):
     ehci@e0000300 {
diff --git a/Documentation/devicetree/bindings/usb/usb-ohci.txt b/Documentation/devicetree/bindings/usb/usb-ohci.txt
index 45f67d91e88..b968a1aea99 100644
--- a/Documentation/devicetree/bindings/usb/usb-ohci.txt
+++ b/Documentation/devicetree/bindings/usb/usb-ohci.txt
@@ -12,6 +12,7 @@ Optional properties:
 - clocks : a list of phandle + clock specifier pairs
 - phys : phandle + phy specifier pair
 - phy-names : "usb"
+- resets : phandle + reset specifier pair
 
 Example:
 
diff --git a/Documentation/devicetree/bindings/usb/usb-xhci.txt b/Documentation/devicetree/bindings/usb/usb-xhci.txt
index 90f8f607d12..5a79377c6a9 100644
--- a/Documentation/devicetree/bindings/usb/usb-xhci.txt
+++ b/Documentation/devicetree/bindings/usb/usb-xhci.txt
@@ -1,11 +1,17 @@
 USB xHCI controllers
 
 Required properties:
-  - compatible: should be "generic-xhci" (deprecated: "xhci-platform").
+  - compatible: should be one of "generic-xhci",
+    "marvell,armada-375-xhci", "marvell,armada-380-xhci",
+    "renesas,xhci-r8a7790", "renesas,xhci-r8a7791" (deprecated:
+    "xhci-platform").
   - reg: should contain address and length of the standard XHCI
     register set for the device.
   - interrupts: one XHCI interrupt should be described here.
 
+Optional property:
+  - clocks: reference to a clock
+
 Example:
 	usb@f0931000 {
 		compatible = "generic-xhci";
diff --git a/Documentation/devicetree/bindings/usb/usb3503.txt b/Documentation/devicetree/bindings/usb/usb3503.txt
index a018da4a7ad..221ac0dbc67 100644
--- a/Documentation/devicetree/bindings/usb/usb3503.txt
+++ b/Documentation/devicetree/bindings/usb/usb3503.txt
@@ -15,6 +15,14 @@ Optional properties:
 - reset-gpios: Should specify GPIO for reset.
 - initial-mode: Should specify initial mode.
                 (1 for HUB mode, 2 for STANDBY mode)
+- refclk: Clock used for driving REFCLK signal (optional, if not provided
+	the driver assumes that clock signal is always available, its
+	rate is specified by REF_SEL pins and a value from the primary
+	reference clock frequencies table is used)
+- refclk-frequency: Frequency of the REFCLK signal as defined by REF_SEL
+	pins (optional, if not provided, driver will not set rate of the
+	REFCLK signal and assume that a value from the primary reference
+	clock frequencies table is used)
 
 Examples:
 	usb3503@08 {
diff --git a/Documentation/devicetree/bindings/vendor-prefixes.txt b/Documentation/devicetree/bindings/vendor-prefixes.txt
index 0f01c9bf19c..941980474e1 100644
--- a/Documentation/devicetree/bindings/vendor-prefixes.txt
+++ b/Documentation/devicetree/bindings/vendor-prefixes.txt
@@ -13,6 +13,7 @@ allwinner	Allwinner Technology Co., Ltd.
 altr	Altera Corp.
 amcc	Applied Micro Circuits Corporation (APM, formally AMCC)
 amd	Advanced Micro Devices (AMD), Inc.
+ams	AMS AG
 amstaos	AMS-Taos Inc.
 apm	Applied Micro Circuits Corporation (APM)
 arm	ARM Ltd.
@@ -22,6 +23,7 @@ auo	AU Optronics Corporation
 avago	Avago Technologies
 bosch	Bosch Sensortec GmbH
 brcm	Broadcom Corporation
+buffalo	Buffalo, Inc.
 calxeda	Calxeda
 capella	Capella Microsystems, Inc
 cavium	Cavium, Inc.
@@ -33,15 +35,18 @@ cortina	Cortina Systems, Inc.
 crystalfontz	Crystalfontz America, Inc.
 dallas	Maxim Integrated Products (formerly Dallas Semiconductor)
 davicom	DAVICOM Semiconductor, Inc.
-dlink	D-Link Systems, Inc.
 denx	Denx Software Engineering
+digi	Digi International Inc.
+dlink	D-Link Corporation
 dmo	Data Modul AG
+ebv	EBV Elektronik
 edt	Emerging Display Technologies
 emmicro	EM Microelectronic
 epfl	Ecole Polytechnique Fédérale de Lausanne
 epson	Seiko Epson Corp.
 est	ESTeem Wireless Modems
 eukrea  Eukréa Electromatique
+excito	Excito
 fsl	Freescale Semiconductor
 GEFanuc	GE Fanuc Intelligent Platforms Embedded Systems, Inc.
 gef	GE Fanuc Intelligent Platforms Embedded Systems, Inc.
@@ -53,26 +58,35 @@ haoyu	Haoyu Microelectronic Co. Ltd.
 hisilicon	Hisilicon Limited.
 honeywell	Honeywell
 hp	Hewlett Packard
+i2se	I2SE GmbH
 ibm	International Business Machines (IBM)
 idt	Integrated Device Technologies, Inc.
+iom	Iomega Corporation
 img	Imagination Technologies Ltd.
 intel	Intel Corporation
 intercontrol	Inter Control Group
+isee	ISEE 2007 S.L.
 isl	Intersil
 karo	Ka-Ro electronics GmbH
+keymile	Keymile GmbH
 lacie	LaCie
 lantiq	Lantiq Semiconductor
 lg	LG Corporation
 linux	Linux-specific binding
 lsi	LSI Corp. (LSI Logic)
+lltc	Linear Technology Corporation
 marvell	Marvell Technology Group Ltd.
 maxim	Maxim Integrated Products
 microchip	Microchip Technology Inc.
 mosaixtech	Mosaix Technologies, Inc.
 moxa	Moxa
+mpl	MPL AG
+mundoreader	Mundo Reader S.L.
+mxicy	Macronix International Co., Ltd.
 national	National Semiconductor
 neonode		Neonode Inc.
 netgear	NETGEAR
+newhaven	Newhaven Display International
 nintendo	Nintendo
 nokia	Nokia
 nvidia	NVIDIA
@@ -82,10 +96,13 @@ opencores	OpenCores.org
 panasonic	Panasonic Corporation
 phytec	PHYTEC Messtechnik GmbH
 picochip	Picochip Ltd
+plathome	Plat'Home Co., Ltd.
 powervr	PowerVR (deprecated, use img)
 qca	Qualcomm Atheros, Inc.
 qcom	Qualcomm Technologies, Inc
 qnap	QNAP Systems, Inc.
+radxa	Radxa
+raidsonic	RaidSonic Technology GmbH
 ralink	Mediatek/Ralink Technology Corp.
 ramtron	Ramtron International
 realtek Realtek Semiconductor Corp.
@@ -95,6 +112,7 @@ rockchip	Fuzhou Rockchip Electronics Co., Ltd
 samsung	Samsung Semiconductor
 sbs	Smart Battery System
 schindler	Schindler
+seagate	Seagate Technology PLC
 sil	Silicon Image
 silabs	Silicon Laboratories
 simtek
@@ -109,9 +127,12 @@ stericsson	ST-Ericsson
 synology	Synology, Inc.
 ti	Texas Instruments
 tlm	Trusted Logic Mobility
+toradex	Toradex AG
 toshiba	Toshiba Corporation
 toumaz	Toumaz
+usi	Universal Scientifc Industrial Co., Ltd.
 v3	V3 Semiconductor
+variscite	Variscite Ltd.
 via	VIA Technologies, Inc.
 voipac	Voipac Technologies s.r.o.
 winbond Winbond Electronics corp.
@@ -119,3 +140,4 @@ wlf	Wolfson Microelectronics
 wm	Wondermedia Technologies, Inc.
 xes	Extreme Engineering Solutions (X-ES)
 xlnx	Xilinx
+zyxel	ZyXEL Communications Corp.
diff --git a/Documentation/driver-model/devres.txt b/Documentation/driver-model/devres.txt
index 4f7897e99cb..89472558011 100644
--- a/Documentation/driver-model/devres.txt
+++ b/Documentation/driver-model/devres.txt
@@ -236,6 +236,9 @@ certainly invest a bit more effort into libata core layer).
 MEM
   devm_kzalloc()
   devm_kfree()
+  devm_kmemdup()
+  devm_get_free_pages()
+  devm_free_pages()
 
 IIO
   devm_iio_device_alloc()
@@ -308,3 +311,10 @@ SLAVE DMA ENGINE
 
 SPI
   devm_spi_register_master()
+
+GPIO
+  devm_gpiod_get()
+  devm_gpiod_get_index()
+  devm_gpiod_get_optional()
+  devm_gpiod_get_index_optional()
+  devm_gpiod_put()
diff --git a/Documentation/email-clients.txt b/Documentation/email-clients.txt
index e9f5daccbd0..4e30ebaa9e5 100644
--- a/Documentation/email-clients.txt
+++ b/Documentation/email-clients.txt
@@ -201,20 +201,15 @@ To beat some sense out of the internal editor, do this:
 
 - Edit your Thunderbird config settings so that it won't use format=flowed.
   Go to "edit->preferences->advanced->config editor" to bring up the
-  thunderbird's registry editor, and set "mailnews.send_plaintext_flowed" to
-  "false".
+  thunderbird's registry editor.
 
-- Disable HTML Format: Set "mail.identity.id1.compose_html" to "false".
+- Set "mailnews.send_plaintext_flowed" to "false"
 
-- Enable "preformat" mode: Set "editor.quotesPreformatted" to "true".
+- Set "mailnews.wraplength" from "72" to "0"
 
-- Enable UTF8: Set "prefs.converted-to-utf8" to "true".
+- "View" > "Message Body As" > "Plain Text"
 
-- Install the "toggle wordwrap" extension.  Download the file from:
-    https://addons.mozilla.org/thunderbird/addon/2351/
-  Then go to "tools->add ons", select "install" at the bottom of the screen,
-  and browse to where you saved the .xul file.  This adds an "Enable
-  Wordwrap" entry under the Options menu of the message composer.
+- "View" > "Character Encoding" > "Unicode (UTF-8)"
 
 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 TkRat (GUI)
diff --git a/Documentation/filesystems/proc.txt b/Documentation/filesystems/proc.txt
index 8b9cd8eb3f9..264bcde0c51 100644
--- a/Documentation/filesystems/proc.txt
+++ b/Documentation/filesystems/proc.txt
@@ -1245,8 +1245,9 @@ second).  The meanings of the columns are as follows, from left to right:
 
 The "intr" line gives counts of interrupts  serviced since boot time, for each
 of the  possible system interrupts.   The first  column  is the  total of  all
-interrupts serviced; each  subsequent column is the  total for that particular
-interrupt.
+interrupts serviced  including  unnumbered  architecture specific  interrupts;
+each  subsequent column is the  total for that particular numbered interrupt.
+Unnumbered interrupts are not shown, only summed into the total.
 
 The "ctxt" line gives the total number of context switches across all CPUs.
 
diff --git a/Documentation/gpio/driver.txt b/Documentation/gpio/driver.txt
index f73cc7b5dc8..fa9a0a8b373 100644
--- a/Documentation/gpio/driver.txt
+++ b/Documentation/gpio/driver.txt
@@ -73,6 +73,65 @@ The IRQ portions of the GPIO block are implemented using an irqchip, using
 the header <linux/irq.h>. So basically such a driver is utilizing two sub-
 systems simultaneously: gpio and irq.
 
+GPIO irqchips usually fall in one of two categories:
+
+* CHAINED GPIO irqchips: these are usually the type that is embedded on
+  an SoC. This means that there is a fast IRQ handler for the GPIOs that
+  gets called in a chain from the parent IRQ handler, most typically the
+  system interrupt controller. This means the GPIO irqchip is registered
+  using irq_set_chained_handler() or the corresponding
+  gpiochip_set_chained_irqchip() helper function, and the GPIO irqchip
+  handler will be called immediately from the parent irqchip, while
+  holding the IRQs disabled. The GPIO irqchip will then end up calling
+  something like this sequence in its interrupt handler:
+
+  static irqreturn_t tc3589x_gpio_irq(int irq, void *data)
+      chained_irq_enter(...);
+      generic_handle_irq(...);
+      chained_irq_exit(...);
+
+  Chained GPIO irqchips typically can NOT set the .can_sleep flag on
+  struct gpio_chip, as everything happens directly in the callbacks.
+
+* NESTED THREADED GPIO irqchips: these are off-chip GPIO expanders and any
+  other GPIO irqchip residing on the other side of a sleeping bus. Of course
+  such drivers that need slow bus traffic to read out IRQ status and similar,
+  traffic which may in turn incur other IRQs to happen, cannot be handled
+  in a quick IRQ handler with IRQs disabled. Instead they need to spawn a
+  thread and then mask the parent IRQ line until the interrupt is handled
+  by the driver. The hallmark of this driver is to call something like
+  this in its interrupt handler:
+
+  static irqreturn_t tc3589x_gpio_irq(int irq, void *data)
+      ...
+      handle_nested_irq(irq);
+
+  The hallmark of threaded GPIO irqchips is that they set the .can_sleep
+  flag on struct gpio_chip to true, indicating that this chip may sleep
+  when accessing the GPIOs.
+
+To help out in handling the set-up and management of GPIO irqchips and the
+associated irqdomain and resource allocation callbacks, the gpiolib has
+some helpers that can be enabled by selecting the GPIOLIB_IRQCHIP Kconfig
+symbol:
+
+* gpiochip_irqchip_add(): adds an irqchip to a gpiochip. It will pass
+  the struct gpio_chip* for the chip to all IRQ callbacks, so the callbacks
+  need to embed the gpio_chip in its state container and obtain a pointer
+  to the container using container_of().
+  (See Documentation/driver-model/design-patterns.txt)
+
+* gpiochip_set_chained_irqchip(): sets up a chained irq handler for a
+  gpio_chip from a parent IRQ and passes the struct gpio_chip* as handler
+  data. (Notice handler data, since the irqchip data is likely used by the
+  parent irqchip!) This is for the chained type of chip.
+
+To use the helpers please keep the following in mind:
+
+- Make sure to assign all relevant members of the struct gpio_chip so that
+  the irqchip can initialize. E.g. .dev and .can_sleep shall be set up
+  properly.
+
 It is legal for any IRQ consumer to request an IRQ from any irqchip no matter
 if that is a combined GPIO+IRQ driver. The basic premise is that gpio_chip and
 irq_chip are orthogonal, and offering their services independent of each
diff --git a/Documentation/hsi.txt b/Documentation/hsi.txt
new file mode 100644
index 00000000000..6ac6cd51852
--- /dev/null
+++ b/Documentation/hsi.txt
@@ -0,0 +1,75 @@
+HSI - High-speed Synchronous Serial Interface
+
+1. Introduction
+~~~~~~~~~~~~~~~
+
+High Speed Syncronous Interface (HSI) is a fullduplex, low latency protocol,
+that is optimized for die-level interconnect between an Application Processor
+and a Baseband chipset. It has been specified by the MIPI alliance in 2003 and
+implemented by multiple vendors since then.
+
+The HSI interface supports full duplex communication over multiple channels
+(typically 8) and is capable of reaching speeds up to 200 Mbit/s.
+
+The serial protocol uses two signals, DATA and FLAG as combined data and clock
+signals and an additional READY signal for flow control. An additional WAKE
+signal can be used to wakeup the chips from standby modes. The signals are
+commonly prefixed by AC for signals going from the application die to the
+cellular die and CA for signals going the other way around.
+
++------------+                                 +---------------+
+|  Cellular  |                                 |  Application  |
+|    Die     |                                 |      Die      |
+|            | - - - - - - CAWAKE - - - - - - >|               |
+|           T|------------ CADATA ------------>|R              |
+|           X|------------ CAFLAG ------------>|X              |
+|            |<----------- ACREADY ------------|               |
+|            |                                 |               |
+|            |                                 |               |
+|            |< - - - - -  ACWAKE - - - - - - -|               |
+|           R|<----------- ACDATA -------------|T              |
+|           X|<----------- ACFLAG -------------|X              |
+|            |------------ CAREADY ----------->|               |
+|            |                                 |               |
+|            |                                 |               |
++------------+                                 +---------------+
+
+2. HSI Subsystem in Linux
+~~~~~~~~~~~~~~~~~~~~~~~~~
+
+In the Linux kernel the hsi subsystem is supposed to be used for HSI devices.
+The hsi subsystem contains drivers for hsi controllers including support for
+multi-port controllers and provides a generic API for using the HSI ports.
+
+It also contains HSI client drivers, which make use of the generic API to
+implement a protocol used on the HSI interface. These client drivers can
+use an arbitrary number of channels.
+
+3. hsi-char Device
+~~~~~~~~~~~~~~~~~~
+
+Each port automatically registers a generic client driver called hsi_char,
+which provides a charecter device for userspace representing the HSI port.
+It can be used to communicate via HSI from userspace. Userspace may
+configure the hsi_char device using the following ioctl commands:
+
+* HSC_RESET:
+ - flush the HSI port
+
+* HSC_SET_PM
+ - enable or disable the client.
+
+* HSC_SEND_BREAK
+ - send break
+
+* HSC_SET_RX
+ - set RX configuration
+
+* HSC_GET_RX
+ - get RX configuration
+
+* HSC_SET_TX
+ - set TX configuration
+
+* HSC_GET_TX
+ - get TX configuration
diff --git a/Documentation/hwmon/emc1403 b/Documentation/hwmon/emc1403
new file mode 100644
index 00000000000..a869b0ef6a9
--- /dev/null
+++ b/Documentation/hwmon/emc1403
@@ -0,0 +1,59 @@
+Kernel driver emc1403
+=====================
+
+Supported chips:
+  * SMSC / Microchip EMC1402, EMC1412
+    Addresses scanned: I2C 0x18, 0x1c, 0x29, 0x4c, 0x4d, 0x5c
+    Prefix: 'emc1402'
+    Datasheets:
+	http://ww1.microchip.com/downloads/en/DeviceDoc/1412.pdf
+	http://ww1.microchip.com/downloads/en/DeviceDoc/1402.pdf
+  * SMSC / Microchip EMC1403, EMC1404, EMC1413, EMC1414
+    Addresses scanned: I2C 0x18, 0x29, 0x4c, 0x4d
+    Prefix: 'emc1403', 'emc1404'
+    Datasheets:
+	http://ww1.microchip.com/downloads/en/DeviceDoc/1403_1404.pdf
+	http://ww1.microchip.com/downloads/en/DeviceDoc/1413_1414.pdf
+  * SMSC / Microchip EMC1422
+    Addresses scanned: I2C 0x4c
+    Prefix: 'emc1422'
+    Datasheet:
+	http://ww1.microchip.com/downloads/en/DeviceDoc/1422.pdf
+  * SMSC / Microchip EMC1423, EMC1424
+    Addresses scanned: I2C 0x4c
+    Prefix: 'emc1423', 'emc1424'
+    Datasheet:
+	http://ww1.microchip.com/downloads/en/DeviceDoc/1423_1424.pdf
+
+Author:
+    Kalhan Trisal <kalhan.trisal@intel.com
+
+
+Description
+-----------
+
+The Standard Microsystems Corporation (SMSC) / Microchip EMC14xx chips
+contain up to four temperature sensors. EMC14x2 support two sensors
+(one internal, one external). EMC14x3 support three sensors (one internal,
+two external), and EMC14x4 support four sensors (one internal, three
+external).
+
+The chips implement three limits for each sensor: low (tempX_min), high
+(tempX_max) and critical (tempX_crit.) The chips also implement an
+hysteresis mechanism which applies to all limits. The relative difference
+is stored in a single register on the chip, which means that the relative
+difference between the limit and its hysteresis is always the same for
+all three limits.
+
+This implementation detail implies the following:
+* When setting a limit, its hysteresis will automatically follow, the
+  difference staying unchanged. For example, if the old critical limit
+  was 80 degrees C, and the hysteresis was 75 degrees C, and you change
+  the critical limit to 90 degrees C, then the hysteresis will
+  automatically change to 85 degrees C.
+* The hysteresis values can't be set independently. We decided to make
+  only temp1_crit_hyst writable, while all other hysteresis attributes
+  are read-only. Setting temp1_crit_hyst writes the difference between
+  temp1_crit_hyst and temp1_crit into the chip, and the same relative
+  hysteresis applies automatically to all other limits.
+* The limits should be set before the hysteresis.
diff --git a/Documentation/hwmon/hwmon-kernel-api.txt b/Documentation/hwmon/hwmon-kernel-api.txt
new file mode 100644
index 00000000000..2ecdbfc85ec
--- /dev/null
+++ b/Documentation/hwmon/hwmon-kernel-api.txt
@@ -0,0 +1,107 @@
+The Linux Hardware Monitoring kernel API.
+=========================================
+
+Guenter Roeck
+
+Introduction
+------------
+
+This document describes the API that can be used by hardware monitoring
+drivers that want to use the hardware monitoring framework.
+
+This document does not describe what a hardware monitoring (hwmon) Driver or
+Device is. It also does not describe the API which can be used by user space
+to communicate with a hardware monitoring device. If you want to know this
+then please read the following file: Documentation/hwmon/sysfs-interface.
+
+For additional guidelines on how to write and improve hwmon drivers, please
+also read Documentation/hwmon/submitting-patches.
+
+The API
+-------
+Each hardware monitoring driver must #include <linux/hwmon.h> and, in most
+cases, <linux/hwmon-sysfs.h>. linux/hwmon.h declares the following
+register/unregister functions:
+
+struct device *hwmon_device_register(struct device *dev);
+struct device *
+hwmon_device_register_with_groups(struct device *dev, const char *name,
+				  void *drvdata,
+				  const struct attribute_group **groups);
+
+struct device *
+devm_hwmon_device_register_with_groups(struct device *dev,
+				       const char *name, void *drvdata,
+				       const struct attribute_group **groups);
+
+void hwmon_device_unregister(struct device *dev);
+void devm_hwmon_device_unregister(struct device *dev);
+
+hwmon_device_register registers a hardware monitoring device. The parameter
+of this function is a pointer to the parent device.
+This function returns a pointer to the newly created hardware monitoring device
+or PTR_ERR for failure. If this registration function is used, hardware
+monitoring sysfs attributes are expected to have been created and attached to
+the parent device prior to calling hwmon_device_register. A name attribute must
+have been created by the caller.
+
+hwmon_device_register_with_groups is similar to hwmon_device_register. However,
+it has additional parameters. The name parameter is a pointer to the hwmon
+device name. The registration function wil create a name sysfs attribute
+pointing to this name. The drvdata parameter is the pointer to the local
+driver data.  hwmon_device_register_with_groups will attach this pointer
+to the newly allocated hwmon device. The pointer can be retrieved by the driver
+using dev_get_drvdata() on the hwmon device pointer. The groups parameter is
+a pointer to a list of sysfs attribute groups. The list must be NULL terminated.
+hwmon_device_register_with_groups creates the hwmon device with name attribute
+as well as all sysfs attributes attached to the hwmon device.
+
+devm_hwmon_device_register_with_groups is similar to
+hwmon_device_register_with_groups. However, it is device managed, meaning the
+hwmon device does not have to be removed explicitly by the removal function.
+
+hwmon_device_unregister deregisters a registered hardware monitoring device.
+The parameter of this function is the pointer to the registered hardware
+monitoring device structure. This function must be called from the driver
+remove function if the hardware monitoring device was registered with
+hwmon_device_register or with hwmon_device_register_with_groups.
+
+devm_hwmon_device_unregister does not normally have to be called. It is only
+needed for error handling, and only needed if the driver probe fails after
+the call to devm_hwmon_device_register_with_groups.
+
+The header file linux/hwmon-sysfs.h provides a number of useful macros to
+declare and use hardware monitoring sysfs attributes.
+
+In many cases, you can use the exsting define DEVICE_ATTR to declare such
+attributes. This is feasible if an attribute has no additional context. However,
+in many cases there will be additional information such as a sensor index which
+will need to be passed to the sysfs attribute handling function.
+
+SENSOR_DEVICE_ATTR and SENSOR_DEVICE_ATTR_2 can be used to define attributes
+which need such additional context information. SENSOR_DEVICE_ATTR requires
+one additional argument, SENSOR_DEVICE_ATTR_2 requires two.
+
+SENSOR_DEVICE_ATTR defines a struct sensor_device_attribute variable.
+This structure has the following fields.
+
+struct sensor_device_attribute {
+	struct device_attribute dev_attr;
+	int index;
+};
+
+You can use to_sensor_dev_attr to get the pointer to this structure from the
+attribute read or write function. Its parameter is the device to which the
+attribute is attached.
+
+SENSOR_DEVICE_ATTR_2 defines a struct sensor_device_attribute_2 variable,
+which is defined as follows.
+
+struct sensor_device_attribute_2 {
+	struct device_attribute dev_attr;
+	u8 index;
+	u8 nr;
+};
+
+Use to_sensor_dev_attr_2 to get the pointer to this structure. Its parameter
+is the device to which the attribute is attached.
diff --git a/Documentation/hwmon/jc42 b/Documentation/hwmon/jc42
index 868d74d6b77..f3893f7440d 100644
--- a/Documentation/hwmon/jc42
+++ b/Documentation/hwmon/jc42
@@ -5,9 +5,12 @@ Supported chips:
   * Analog Devices ADT7408
     Datasheets:
 	http://www.analog.com/static/imported-files/data_sheets/ADT7408.pdf
-  * Atmel AT30TS00
+  * Atmel AT30TS00, AT30TS002A/B, AT30TSE004A
     Datasheets:
 	http://www.atmel.com/Images/doc8585.pdf
+	http://www.atmel.com/Images/doc8711.pdf
+	http://www.atmel.com/Images/Atmel-8852-SEEPROM-AT30TSE002A-Datasheet.pdf
+	http://www.atmel.com/Images/Atmel-8868-DTS-AT30TSE004A-Datasheet.pdf
   * IDT TSE2002B3, TSE2002GB2, TS3000B3, TS3000GB2
     Datasheets:
 	http://www.idt.com/sites/default/files/documents/IDT_TSE2002B3C_DST_20100512_120303152056.pdf
@@ -34,12 +37,13 @@ Supported chips:
     Datasheet:
 	http://www.onsemi.com/pub_link/Collateral/CAT34TS02-D.PDF
 	http://www.onsemi.com/pub/Collateral/CAT6095-D.PDF
-  * ST Microelectronics STTS424, STTS424E02, STTS2002, STTS3000
+  * ST Microelectronics STTS424, STTS424E02, STTS2002, STTS2004, STTS3000
     Datasheets:
-	http://www.st.com/internet/com/TECHNICAL_RESOURCES/TECHNICAL_LITERATURE/DATASHEET/CD00157556.pdf
-	http://www.st.com/internet/com/TECHNICAL_RESOURCES/TECHNICAL_LITERATURE/DATASHEET/CD00157558.pdf
-	http://www.st.com/internet/com/TECHNICAL_RESOURCES/TECHNICAL_LITERATURE/DATASHEET/CD00225278.pdf
-	http://www.st.com/internet/com/TECHNICAL_RESOURCES/TECHNICAL_LITERATURE/DATA_BRIEF/CD00270920.pdf
+	http://www.st.com/web/en/resource/technical/document/datasheet/CD00157556.pdf
+	http://www.st.com/web/en/resource/technical/document/datasheet/CD00157558.pdf
+	http://www.st.com/web/en/resource/technical/document/datasheet/CD00266638.pdf
+	http://www.st.com/web/en/resource/technical/document/datasheet/CD00225278.pdf
+	http://www.st.com/web/en/resource/technical/document/datasheet/DM00076709.pdf
   * JEDEC JC 42.4 compliant temperature sensor chips
     Datasheet:
 	http://www.jedec.org/sites/default/files/docs/4_01_04R19.pdf
diff --git a/Documentation/hwmon/lm77 b/Documentation/hwmon/lm77
index 57c3a46d637..bfc915fe363 100644
--- a/Documentation/hwmon/lm77
+++ b/Documentation/hwmon/lm77
@@ -18,5 +18,21 @@ sensor incorporates a band-gap type temperature sensor,
 10-bit ADC, and a digital comparator with user-programmable upper
 and lower limit values.
 
-Limits can be set through the Overtemperature Shutdown register and
-Hysteresis register.
+The LM77 implements 3 limits: low (temp1_min), high (temp1_max) and
+critical (temp1_crit.) It also implements an hysteresis mechanism which
+applies to all 3 limits. The relative difference is stored in a single
+register on the chip, which means that the relative difference between
+the limit and its hysteresis is always the same for all 3 limits.
+
+This implementation detail implies the following:
+* When setting a limit, its hysteresis will automatically follow, the
+  difference staying unchanged. For example, if the old critical limit
+  was 80 degrees C, and the hysteresis was 75 degrees C, and you change
+  the critical limit to 90 degrees C, then the hysteresis will
+  automatically change to 85 degrees C.
+* All 3 hysteresis can't be set independently. We decided to make
+  temp1_crit_hyst writable, while temp1_min_hyst and temp1_max_hyst are
+  read-only. Setting temp1_crit_hyst writes the difference between
+  temp1_crit_hyst and temp1_crit into the chip, and the same relative
+  hysteresis applies automatically to the low and high limits.
+* The limits should be set before the hysteresis.
diff --git a/Documentation/hwmon/nct6683 b/Documentation/hwmon/nct6683
new file mode 100644
index 00000000000..c1301d4300c
--- /dev/null
+++ b/Documentation/hwmon/nct6683
@@ -0,0 +1,57 @@
+Kernel driver nct6683
+=====================
+
+Supported chips:
+  * Nuvoton NCT6683D
+    Prefix: 'nct6683'
+    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 NCT6683D eSIO chip.
+
+The chips implement up to shared 32 temperature and voltage sensors.
+It supports up to 16 fan rotation sensors and up to 8 fan control engines.
+
+Temperatures are measured in degrees Celsius. Measurement resolution is
+0.5 degrees C.
+
+Voltage sensors (also known as IN sensors) report their values in millivolts.
+
+Fan rotation speeds are reported in RPM (rotations per minute).
+
+Usage Note
+----------
+
+Limit register locations on Intel boards with EC firmware version 1.0
+build date 04/03/13 do not match the register locations in the Nuvoton
+datasheet. Nuvoton confirms that Intel uses a special firmware version
+with different register addresses. The specification describing the Intel
+firmware is held under NDA by Nuvoton and Intel and not available
+to the public.
+
+Some of the register locations can be reverse engineered; others are too
+well hidden. Given this, writing any values from the operating system is
+considered too risky with this firmware and has been disabled. All limits
+must all be written from the BIOS.
+
+The driver has only been tested with the Intel firmware, and by default
+only instantiates on Intel boards. To enable it on non-Intel boards,
+set the 'force' module parameter to 1.
+
+Tested Boards and Firmware Versions
+-----------------------------------
+
+The driver has been reported to work with the following boards and
+firmware versions.
+
+Board		Firmware version
+---------------------------------------------------------------
+Intel DH87RL	NCT6683D EC firmware version 1.0 build 04/03/13
+Intel DH87MC	NCT6683D EC firmware version 1.0 build 04/03/13
+Intel DB85FL	NCT6683D EC firmware version 1.0 build 04/03/13
diff --git a/Documentation/hwmon/sysfs-interface b/Documentation/hwmon/sysfs-interface
index 79f8257dd79..2cc95ad4660 100644
--- a/Documentation/hwmon/sysfs-interface
+++ b/Documentation/hwmon/sysfs-interface
@@ -327,6 +327,13 @@ temp[1-*]_max_hyst
 		from the max value.
 		RW
 
+temp[1-*]_min_hyst
+		Temperature hysteresis value for min limit.
+		Unit: millidegree Celsius
+		Must be reported as an absolute temperature, NOT a delta
+		from the min value.
+		RW
+
 temp[1-*]_input Temperature input value.
 		Unit: millidegree Celsius
 		RO
@@ -362,6 +369,13 @@ temp[1-*]_lcrit	Temperature critical min value, typically lower than
 		Unit: millidegree Celsius
 		RW
 
+temp[1-*]_lcrit_hyst
+		Temperature hysteresis value for critical min limit.
+		Unit: millidegree Celsius
+		Must be reported as an absolute temperature, NOT a delta
+		from the critical min value.
+		RW
+
 temp[1-*]_offset
 		Temperature offset which is added to the temperature reading
 		by the chip.
diff --git a/Documentation/input/elantech.txt b/Documentation/input/elantech.txt
index 5602eb71ad5..e1ae127ed09 100644
--- a/Documentation/input/elantech.txt
+++ b/Documentation/input/elantech.txt
@@ -504,9 +504,12 @@ byte 5:
 * reg_10
 
    bit   7   6   5   4   3   2   1   0
-         0   0   0   0   0   0   0   A
+         0   0   0   0   R   F   T   A
 
          A: 1 = enable absolute tracking
+         T: 1 = enable two finger mode auto correct
+         F: 1 = disable ABS Position Filter
+         R: 1 = enable real hardware resolution
 
 6.2 Native absolute mode 6 byte packet format
     ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
diff --git a/Documentation/ja_JP/HOWTO b/Documentation/ja_JP/HOWTO
index 0091a8215ac..b61885c35ce 100644
--- a/Documentation/ja_JP/HOWTO
+++ b/Documentation/ja_JP/HOWTO
@@ -315,7 +315,7 @@ Andrew Morton ã�Œ Linux-kernel メーリングリストã�«ã‚«ãƒ¼ãƒ�ルリリーã
 も��3.x.y カー�ル�存在���場����番��一番大�� 3.x �
 最新�安定版カー�ル��。
 
-3.x.y � "stable" �ーム <stable@kernel.org> �メンテ�れ��り�必
+3.x.y � "stable" �ーム <stable@vger.kernel.org> �メンテ�れ��り�必
 ��応��リリース�れ��。通常�リリース期間� 2週間毎����差�迫�
 ��題���れ�も�少�長��る��も�り��。セキュリティ関連��題
 �場���れ�対�������場�����リリース��れ��。
diff --git a/Documentation/ja_JP/stable_kernel_rules.txt b/Documentation/ja_JP/stable_kernel_rules.txt
index 14265837c4c..9dbda9b5d21 100644
--- a/Documentation/ja_JP/stable_kernel_rules.txt
+++ b/Documentation/ja_JP/stable_kernel_rules.txt
@@ -50,16 +50,16 @@ linux-2.6.29/Documentation/stable_kernel_rules.txt
 
 -stable ツリー�パッ�を�付�る手続�-
 
- - 上記��則�従���る�を確���後��stable@kernel.org �パッ�
+ - 上記��則�従���る�を確���後��stable@vger.kernel.org �パッ�
    を�る。
  - �信者�パッ��キュー���付�られ�際�� ACK を��下�れ�場�
    �� NAK を���る。���応�開発者���スケジュール�よ���数
    日��る場���る。
  - も����られ�ら�パッ��他�開発者���関連�るサブシステム�
    メンテナー�よるレビュー���� -stable キュー�追加�れる。
- - パッ�� stable@kernel.org �アドレス�付加�れ��る������れ
+ - パッ�� stable@vger.kernel.org �アドレス�付加�れ��る������れ
    � Linus �ツリー�入る時�自動的� stable �ーム� email �れる。
- - セキュリティパッ����エイリアス (stable@kernel.org) ��られる�
+ - セキュリティパッ����エイリアス (stable@vger.kernel.org) ��られる�
    ������代�り� security@kernel.org �アドレス��られる。
 
 レビューサイクル-
diff --git a/Documentation/java.txt b/Documentation/java.txt
index e6a72328154..418020584cc 100644
--- a/Documentation/java.txt
+++ b/Documentation/java.txt
@@ -188,6 +188,9 @@ shift
 #define CP_METHODREF 10
 #define CP_INTERFACEMETHODREF 11
 #define CP_NAMEANDTYPE 12
+#define CP_METHODHANDLE 15
+#define CP_METHODTYPE 16
+#define CP_INVOKEDYNAMIC 18
 
 /* Define some commonly used error messages */
 
@@ -242,14 +245,19 @@ void skip_constant(FILE *classfile, u_int16_t *cur)
 		break;
 	case CP_CLASS:
 	case CP_STRING:
+	case CP_METHODTYPE:
 		seekerr = fseek(classfile, 2, SEEK_CUR);
 		break;
+	case CP_METHODHANDLE:
+		seekerr = fseek(classfile, 3, SEEK_CUR);
+		break;
 	case CP_INTEGER:
 	case CP_FLOAT:
 	case CP_FIELDREF:
 	case CP_METHODREF:
 	case CP_INTERFACEMETHODREF:
 	case CP_NAMEANDTYPE:
+	case CP_INVOKEDYNAMIC:
 		seekerr = fseek(classfile, 4, SEEK_CUR);
 		break;
 	case CP_LONG:
diff --git a/Documentation/kernel-parameters.txt b/Documentation/kernel-parameters.txt
index 03e50b4883a..4ddcbf94969 100644
--- a/Documentation/kernel-parameters.txt
+++ b/Documentation/kernel-parameters.txt
@@ -804,13 +804,6 @@ bytes respectively. Such letter suffixes can also be entirely omitted.
 	dhash_entries=	[KNL]
 			Set number of hash buckets for dentry cache.
 
-	digi=		[HW,SERIAL]
-			IO parameters + enable/disable command.
-
-	digiepca=	[HW,SERIAL]
-			See drivers/char/README.epca and
-			Documentation/serial/digiepca.txt.
-
 	disable=	[IPV6]
 			See Documentation/networking/ipv6.txt.
 
@@ -890,6 +883,7 @@ bytes respectively. Such letter suffixes can also be entirely omitted.
 			which are not unmapped.
 
 	earlycon=	[KNL] Output early console device and options.
+
 		uart[8250],io,<addr>[,options]
 		uart[8250],mmio,<addr>[,options]
 		uart[8250],mmio32,<addr>[,options]
@@ -899,7 +893,15 @@ 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,ARM]
+		pl011,<addr>
+			Start an early, polled-mode console on a pl011 serial
+			port at the specified address. The pl011 serial port
+			must already be setup and configured. Options are not
+			yet supported.
+
+		smh	Use ARM semihosting calls for early console.
+
+	earlyprintk=	[X86,SH,BLACKFIN,ARM,M68k]
 			earlyprintk=vga
 			earlyprintk=efi
 			earlyprintk=xen
@@ -2225,10 +2227,10 @@ bytes respectively. Such letter suffixes can also be entirely omitted.
 	noreplace-smp	[X86-32,SMP] Don't replace SMP instructions
 			with UP alternatives
 
-	nordrand	[X86] Disable the direct use of the RDRAND
-			instruction even if it is supported by the
-			processor.  RDRAND is still available to user
-			space applications.
+	nordrand	[X86] Disable kernel use of the RDRAND and
+			RDSEED instructions even if they are supported
+			by the processor.  RDRAND and RDSEED are still
+			available to user space applications.
 
 	noresume	[SWSUSP] Disables resume and restores original swap
 			space.
@@ -2939,9 +2941,6 @@ bytes respectively. Such letter suffixes can also be entirely omitted.
 	rhash_entries=	[KNL,NET]
 			Set number of hash buckets for route cache
 
-	riscom8=	[HW,SERIAL]
-			Format: <io_board1>[,<io_board2>[,...<io_boardN>]]
-
 	ro		[KNL] Mount root device read-only on boot
 
 	root=		[KNL] Root filesystem
@@ -3083,9 +3082,6 @@ bytes respectively. Such letter suffixes can also be entirely omitted.
 	sonypi.*=	[HW] Sony Programmable I/O Control Device driver
 			See Documentation/laptops/sonypi.txt
 
-	specialix=	[HW,SERIAL] Specialix multi-serial port adapter
-			See Documentation/serial/specialix.txt.
-
 	spia_io_base=	[HW,MTD]
 	spia_fio_base=
 	spia_pedr=
diff --git a/Documentation/magic-number.txt b/Documentation/magic-number.txt
index 76d80a64bbe..4c8e142db2e 100644
--- a/Documentation/magic-number.txt
+++ b/Documentation/magic-number.txt
@@ -63,8 +63,6 @@ Magic Name            Number      Structure            File
 PG_MAGIC              'P'         pg_{read,write}_hdr include/linux/pg.h
 CMAGIC                0x0111      user              include/linux/a.out.h
 MKISS_DRIVER_MAGIC    0x04bf      mkiss_channel     drivers/net/mkiss.h
-RISCOM8_MAGIC         0x0907      riscom_port       drivers/char/riscom8.h
-SPECIALIX_MAGIC       0x0907      specialix_port    drivers/char/specialix_io8.h
 HDLC_MAGIC            0x239e      n_hdlc            drivers/char/n_hdlc.c
 APM_BIOS_MAGIC        0x4101      apm_user          arch/x86/kernel/apm_32.c
 CYCLADES_MAGIC        0x4359      cyclades_port     include/linux/cyclades.h
@@ -82,7 +80,6 @@ STRIP_MAGIC           0x5303      strip             drivers/net/strip.c
 X25_ASY_MAGIC         0x5303      x25_asy           drivers/net/x25_asy.h
 SIXPACK_MAGIC         0x5304      sixpack           drivers/net/hamradio/6pack.h
 AX25_MAGIC            0x5316      ax_disp           drivers/net/mkiss.h
-ESP_MAGIC             0x53ee      esp_struct        drivers/char/esp.h
 TTY_MAGIC             0x5401      tty_struct        include/linux/tty.h
 MGSL_MAGIC            0x5401      mgsl_info         drivers/char/synclink.c
 TTY_DRIVER_MAGIC      0x5402      tty_driver        include/linux/tty_driver.h
@@ -94,13 +91,10 @@ USB_BLUETOOTH_MAGIC   0x6d02      usb_bluetooth     drivers/usb/class/bluetty.c
 RFCOMM_TTY_MAGIC      0x6d02                        net/bluetooth/rfcomm/tty.c
 USB_SERIAL_PORT_MAGIC 0x7301      usb_serial_port   drivers/usb/serial/usb-serial.h
 CG_MAGIC              0x00090255  ufs_cylinder_group include/linux/ufs_fs.h
-A2232_MAGIC           0x000a2232  gs_port           drivers/char/ser_a2232.h
 RPORT_MAGIC           0x00525001  r_port            drivers/char/rocket_int.h
 LSEMAGIC              0x05091998  lse               drivers/fc4/fc.c
 GDTIOCTL_MAGIC        0x06030f07  gdth_iowr_str     drivers/scsi/gdth_ioctl.h
 RIEBL_MAGIC           0x09051990                    drivers/net/atarilance.c
-RIO_MAGIC             0x12345678  gs_port           drivers/char/rio/rio_linux.c
-SX_MAGIC              0x12345678  gs_port           drivers/char/sx.h
 NBD_REQUEST_MAGIC     0x12560953  nbd_request       include/linux/nbd.h
 RED_MAGIC2            0x170fc2a5  (any)             mm/slab.c
 BAYCOM_MAGIC          0x19730510  baycom_state      drivers/net/baycom_epp.c
@@ -116,7 +110,6 @@ ISDN_ASYNC_MAGIC      0x49344C01  modem_info        include/linux/isdn.h
 CTC_ASYNC_MAGIC       0x49344C01  ctc_tty_info      drivers/s390/net/ctctty.c
 ISDN_NET_MAGIC        0x49344C02  isdn_net_local_s  drivers/isdn/i4l/isdn_net_lib.h
 SAVEKMSG_MAGIC2       0x4B4D5347  savekmsg          arch/*/amiga/config.c
-STLI_BOARDMAGIC       0x4bc6c825  stlibrd           include/linux/istallion.h
 CS_STATE_MAGIC        0x4c4f4749  cs_state          sound/oss/cs46xx.c
 SLAB_C_MAGIC          0x4f17a36d  kmem_cache        mm/slab.c
 COW_MAGIC             0x4f4f4f4d  cow_header_v1     arch/um/drivers/ubd_user.c
@@ -127,10 +120,8 @@ SCC_MAGIC             0x52696368  gs_port           drivers/char/scc.h
 SAVEKMSG_MAGIC1       0x53415645  savekmsg          arch/*/amiga/config.c
 GDA_MAGIC             0x58464552  gda               arch/mips/include/asm/sn/gda.h
 RED_MAGIC1            0x5a2cf071  (any)             mm/slab.c
-STL_PORTMAGIC         0x5a7182c9  stlport           include/linux/stallion.h
 EEPROM_MAGIC_VALUE    0x5ab478d2  lanai_dev         drivers/atm/lanai.c
 HDLCDRV_MAGIC         0x5ac6e778  hdlcdrv_state     include/linux/hdlcdrv.h
-EPCA_MAGIC            0x5c6df104  channel           include/linux/epca.h
 PCXX_MAGIC            0x5c6df104  channel           drivers/char/pcxx.h
 KV_MAGIC              0x5f4b565f  kernel_vars_s     arch/mips/include/asm/sn/klkernvars.h
 I810_STATE_MAGIC      0x63657373  i810_state        sound/oss/i810_audio.c
@@ -142,17 +133,14 @@ SLOT_MAGIC            0x67267322  slot              drivers/hotplug/acpiphp.h
 LO_MAGIC              0x68797548  nbd_device        include/linux/nbd.h
 OPROFILE_MAGIC        0x6f70726f  super_block       drivers/oprofile/oprofilefs.h
 M3_STATE_MAGIC        0x734d724d  m3_state          sound/oss/maestro3.c
-STL_PANELMAGIC        0x7ef621a1  stlpanel          include/linux/stallion.h
 VMALLOC_MAGIC         0x87654320  snd_alloc_track   sound/core/memory.c
 KMALLOC_MAGIC         0x87654321  snd_alloc_track   sound/core/memory.c
 PWC_MAGIC             0x89DC10AB  pwc_device        drivers/usb/media/pwc.h
 NBD_REPLY_MAGIC       0x96744668  nbd_reply         include/linux/nbd.h
-STL_BOARDMAGIC        0xa2267f52  stlbrd            include/linux/stallion.h
 ENI155_MAGIC          0xa54b872d  midway_eprom	    drivers/atm/eni.h
 SCI_MAGIC             0xbabeface  gs_port           drivers/char/sh-sci.h
 CODA_MAGIC            0xC0DAC0DA  coda_file_info    fs/coda/coda_fs_i.h
 DPMEM_MAGIC           0xc0ffee11  gdt_pci_sram      drivers/scsi/gdth.h
-STLI_PORTMAGIC        0xe671c7a1  stliport          include/linux/istallion.h
 YAM_MAGIC             0xF10A7654  yam_port          drivers/net/hamradio/yam.c
 CCB_MAGIC             0xf2691ad2  ccb               drivers/scsi/ncr53c8xx.c
 QUEUE_MAGIC_FREE      0xf7e1c9a3  queue_entry       drivers/scsi/arm/queue.c
diff --git a/Documentation/networking/filter.txt b/Documentation/networking/filter.txt
index 81f940f4e88..e3ba753cb71 100644
--- a/Documentation/networking/filter.txt
+++ b/Documentation/networking/filter.txt
@@ -277,7 +277,7 @@ Possible BPF extensions are shown in the following table:
   mark                                  skb->mark
   queue                                 skb->queue_mapping
   hatype                                skb->dev->type
-  rxhash                                skb->rxhash
+  rxhash                                skb->hash
   cpu                                   raw_smp_processor_id()
   vlan_tci                              vlan_tx_tag_get(skb)
   vlan_pr                               vlan_tx_tag_present(skb)
diff --git a/Documentation/networking/packet_mmap.txt b/Documentation/networking/packet_mmap.txt
index 6fea79efb4c..38112d512f4 100644
--- a/Documentation/networking/packet_mmap.txt
+++ b/Documentation/networking/packet_mmap.txt
@@ -578,7 +578,7 @@ processes. This also works in combination with mmap(2) on packet sockets.
 
 Currently implemented fanout policies are:
 
-  - PACKET_FANOUT_HASH: schedule to socket by skb's rxhash
+  - PACKET_FANOUT_HASH: schedule to socket by skb's packet hash
   - PACKET_FANOUT_LB: schedule to socket by round-robin
   - PACKET_FANOUT_CPU: schedule to socket by CPU packet arrives on
   - PACKET_FANOUT_RND: schedule to socket by random selection
diff --git a/Documentation/networking/scaling.txt b/Documentation/networking/scaling.txt
index ca6977f5b2e..99ca40e8e81 100644
--- a/Documentation/networking/scaling.txt
+++ b/Documentation/networking/scaling.txt
@@ -429,7 +429,7 @@ RPS and RFS were introduced in kernel 2.6.35. XPS was incorporated into
 (therbert@google.com)
 
 Accelerated RFS was introduced in 2.6.35. Original patches were
-submitted by Ben Hutchings (bhutchings@solarflare.com)
+submitted by Ben Hutchings (bwh@kernel.org)
 
 Authors:
 Tom Herbert (therbert@google.com)
diff --git a/Documentation/s390/zfcpdump.txt b/Documentation/s390/zfcpdump.txt
index cf45d27c460..dc929be9601 100644
--- a/Documentation/s390/zfcpdump.txt
+++ b/Documentation/s390/zfcpdump.txt
@@ -1,15 +1,15 @@
-s390 SCSI dump tool (zfcpdump)
+The s390 SCSI dump tool (zfcpdump)
 
 System z machines (z900 or higher) provide hardware support for creating system
 dumps on SCSI disks. The dump process is initiated by booting a dump tool, which
 has to create a dump of the current (probably crashed) Linux image. In order to
 not overwrite memory of the crashed Linux with data of the dump tool, the
-hardware saves some memory plus the register sets of the boot cpu before the
+hardware saves some memory plus the register sets of the boot CPU before the
 dump tool is loaded. There exists an SCLP hardware interface to obtain the saved
 memory afterwards. Currently 32 MB are saved.
 
 This zfcpdump implementation consists of a Linux dump kernel together with
-a userspace dump tool, which are loaded together into the saved memory region
+a user space dump tool, which are loaded together into the saved memory region
 below 32 MB. zfcpdump is installed on a SCSI disk using zipl (as contained in
 the s390-tools package) to make the device bootable. The operator of a Linux
 system can then trigger a SCSI dump by booting the SCSI disk, where zfcpdump
@@ -19,68 +19,33 @@ The kernel part of zfcpdump is implemented as a debugfs file under "zcore/mem",
 which exports memory and registers of the crashed Linux in an s390
 standalone dump format. It can be used in the same way as e.g. /dev/mem. The
 dump format defines a 4K header followed by plain uncompressed memory. The
-register sets are stored in the prefix pages of the respective cpus. To build a
+register sets are stored in the prefix pages of the respective CPUs. To build a
 dump enabled kernel with the zcore driver, the kernel config option
-CONFIG_ZFCPDUMP has to be set. When reading from "zcore/mem", the part of
+CONFIG_CRASH_DUMP has to be set. When reading from "zcore/mem", the part of
 memory, which has been saved by hardware is read by the driver via the SCLP
 hardware interface. The second part is just copied from the non overwritten real
 memory.
 
-The userspace application of zfcpdump can reside e.g. in an intitramfs or an
-initrd. It reads from zcore/mem and writes the system dump to a file on a
-SCSI disk.
+Since kernel version 3.12 also the /proc/vmcore file can also be used to access
+the dump.
 
-To build a zfcpdump kernel use the following settings in your kernel
-configuration:
- * CONFIG_ZFCPDUMP=y
- * Enable ZFCP driver
- * Enable SCSI driver
- * Enable ext2 and ext3 filesystems
- * Disable as many features as possible to keep the kernel small.
-   E.g. network support is not needed at all.
+To get a valid zfcpdump kernel configuration use "make zfcpdump_defconfig".
 
-To use the zfcpdump userspace application in an initramfs you have to do the
-following:
+The s390 zipl tool looks for the zfcpdump kernel and optional initrd/initramfs
+under the following locations:
 
- * Copy the zfcpdump executable somewhere into your Linux tree.
-   E.g. to "arch/s390/boot/zfcpdump. If you do not want to include
-   shared libraries, compile the tool with the "-static" gcc option.
- * If you want to include e2fsck, add it to your source tree, too. The zfcpdump
-   application attempts to start /sbin/e2fsck from the ramdisk.
- * Use an initramfs config file like the following:
+* kernel:  <zfcpdump directory>/zfcpdump.image
+* ramdisk: <zfcpdump directory>/zfcpdump.rd
 
-   dir /dev 755 0 0
-   nod /dev/console 644 0 0 c 5 1
-   nod /dev/null 644 0 0 c 1 3
-   nod /dev/sda1 644 0 0 b 8 1
-   nod /dev/sda2 644 0 0 b 8 2
-   nod /dev/sda3 644 0 0 b 8 3
-   nod /dev/sda4 644 0 0 b 8 4
-   nod /dev/sda5 644 0 0 b 8 5
-   nod /dev/sda6 644 0 0 b 8 6
-   nod /dev/sda7 644 0 0 b 8 7
-   nod /dev/sda8 644 0 0 b 8 8
-   nod /dev/sda9 644 0 0 b 8 9
-   nod /dev/sda10 644 0 0 b 8 10
-   nod /dev/sda11 644 0 0 b 8 11
-   nod /dev/sda12 644 0 0 b 8 12
-   nod /dev/sda13 644 0 0 b 8 13
-   nod /dev/sda14 644 0 0 b 8 14
-   nod /dev/sda15 644 0 0 b 8 15
-   file /init arch/s390/boot/zfcpdump 755 0 0
-   file /sbin/e2fsck arch/s390/boot/e2fsck 755 0 0
-   dir /proc 755 0 0
-   dir /sys 755 0 0
-   dir /mnt 755 0 0
-   dir /sbin 755 0 0
+The zfcpdump directory is defined in the s390-tools package.
 
- * Issue "make image" to build the zfcpdump image with initramfs.
+The user space application of zfcpdump can reside in an intitramfs or an
+initrd. It can also be included in a built-in kernel initramfs. The application
+reads from /proc/vmcore or zcore/mem and writes the system dump to a SCSI disk.
 
-In a Linux distribution the zfcpdump enabled kernel image must be copied to
-/usr/share/zfcpdump/zfcpdump.image, where the s390 zipl tool is looking for the
-dump kernel when preparing a SCSI dump disk.
-
-If you use a ramdisk copy it to "/usr/share/zfcpdump/zfcpdump.rd".
+The s390-tools package version 1.24.0 and above builds an external zfcpdump
+initramfs with a user space application that writes the dump to a SCSI
+partition.
 
 For more information on how to use zfcpdump refer to the s390 'Using the Dump
 Tools book', which is available from
diff --git a/Documentation/serial/00-INDEX b/Documentation/serial/00-INDEX
index f9c6b5ed03e..8021a9f29fc 100644
--- a/Documentation/serial/00-INDEX
+++ b/Documentation/serial/00-INDEX
@@ -2,23 +2,15 @@
 	- this file.
 README.cycladesZ
 	- info on Cyclades-Z firmware loading.
-digiepca.txt
-	- info on Digi Intl. {PC,PCI,EISA}Xx and Xem series cards.
 driver
 	- intro to the low level serial driver.
 moxa-smartio
 	- file with info on installing/using Moxa multiport serial driver.
 n_gsm.txt
 	- GSM 0710 tty multiplexer howto.
-riscom8.txt
-	- notes on using the RISCom/8 multi-port serial driver.
 rocket.txt
 	- info on the Comtrol RocketPort multiport serial driver.
 serial-rs485.txt
 	- info about RS485 structures and support in the kernel.
-specialix.txt
-	- info on hardware/driver for specialix IO8+ multiport serial card.
-sx.txt
-	- info on the Specialix SX/SI multiport serial driver.
 tty.txt
 	- guide to the locking policies of the tty layer.
diff --git a/Documentation/serial/digiepca.txt b/Documentation/serial/digiepca.txt
deleted file mode 100644
index f2560e22f2c..00000000000
--- a/Documentation/serial/digiepca.txt
+++ /dev/null
@@ -1,98 +0,0 @@
-NOTE:  This driver is obsolete.  Digi provides a 2.6 driver (dgdm) at
-http://www.digi.com for PCI cards.  They no longer maintain this driver,
-and have no 2.6 driver for ISA cards.
-
-This driver requires a number of user-space tools.  They can be acquired from
-http://www.digi.com, but only works with 2.4 kernels.
-
-
-The Digi Intl. epca driver. 
-----------------------------
-The Digi Intl. epca driver for Linux supports the following boards:
-
-Digi PC/Xem, PC/Xr, PC/Xe, PC/Xi, PC/Xeve 
-Digi EISA/Xem, PCI/Xem, PCI/Xr 
-
-Limitations:
-------------
-Currently the driver only autoprobes for supported PCI boards. 
-
-The Linux MAKEDEV command does not support generating the Digiboard
-Devices.  Users executing digiConfig to setup EISA and PC series cards
-will have their device nodes automatically constructed (cud?? for ~CLOCAL,
-and ttyD?? for CLOCAL).  Users wishing to boot their board from the LILO
-prompt, or those users booting PCI cards may use buildDIGI to construct 
-the necessary nodes. 
-
-Notes:
-------
-This driver may be configured via LILO.  For users who have already configured
-their driver using digiConfig, configuring from LILO will override previous 
-settings.  Multiple boards may be configured by issuing multiple LILO command 
-lines.  For examples see the bottom of this document.
-
-Device names start at 0 and continue up.  Beware of this as previous Digi 
-drivers started device names with 1.
-
-PCI boards are auto-detected and configured by the driver.  PCI boards will
-be allocated device numbers (internally) beginning with the lowest PCI slot
-first.  In other words a PCI card in slot 3 will always have higher device
-nodes than a PCI card in slot 1. 
-
-LILO config examples:
----------------------
-Using LILO's APPEND command, a string of comma separated identifiers or 
-integers can be used to configure supported boards.  The six values in order 
-are:
-
-   Enable/Disable this card or Override,
-   Type of card: PC/Xe (AccelePort) (0), PC/Xeve (1), PC/Xem or PC/Xr (2), 
-                 EISA/Xem (3), PC/64Xe (4), PC/Xi (5), 
-   Enable/Disable alternate pin arrangement,
-   Number of ports on this card,
-   I/O Port where card is configured (in HEX if using string identifiers),
-   Base of memory window (in HEX if using string identifiers), 
-
-NOTE : PCI boards are auto-detected and configured.  Do not attempt to 
-configure PCI boards with the LILO append command.  If you wish to override
-previous configuration data (As set by digiConfig), but you do not wish to
-configure any specific card (Example if there are PCI cards in the system) 
-the following override command will accomplish this:
--> append="digi=2"
-
-Samples:
-   append="digiepca=E,PC/Xe,D,16,200,D0000"
-                  or
-   append="digi=1,0,0,16,512,851968"
-
-Supporting Tools:
------------------
-Supporting tools include digiDload, digiConfig, buildPCI, and ditty.  See
-drivers/char/README.epca for more details.  Note,
-this driver REQUIRES that digiDload be executed prior to it being used. 
-Failure to do this will result in an ENODEV error.
-
-Documentation:
---------------
-Complete documentation for this product may be found in the tool package. 
-
-Sources of information and support:
------------------------------------
-Digi Intl. support site for this product:
-
-->  http://www.digi.com
-
-Acknowledgments:
-----------------
-Much of this work (And even text) was derived from a similar document 
-supporting the original public domain DigiBoard driver Copyright (C)
-1994,1995 Troy De Jongh.  Many thanks to Christoph Lameter 
-(christoph@lameter.com) and Mike McLagan (mike.mclagan@linux.org) who authored 
-and contributed to the original document. 
-
-Changelog:
-----------
-10-29-04:	Update status of driver, remove dead links in document
-		James Nelson <james4765@gmail.com>
-
-2000 (?)	Original Document
diff --git a/Documentation/serial/driver b/Documentation/serial/driver
index c3a7689a90e..3bba1aeb799 100644
--- a/Documentation/serial/driver
+++ b/Documentation/serial/driver
@@ -429,3 +429,28 @@ thus:
 		struct uart_port	port;
 		int			my_stuff;
 	};
+
+Modem control lines via GPIO
+----------------------------
+
+Some helpers are provided in order to set/get modem control lines via GPIO.
+
+mctrl_gpio_init(dev, idx):
+	This will get the {cts,rts,...}-gpios from device tree if they are
+	present and request them, set direction etc, and return an
+	allocated structure. devm_* functions are used, so there's no need
+	to call mctrl_gpio_free().
+
+mctrl_gpio_free(dev, gpios):
+	This will free the requested gpios in mctrl_gpio_init().
+	As devm_* function are used, there's generally no need to call
+	this function.
+
+mctrl_gpio_to_gpiod(gpios, gidx)
+	This returns the gpio structure associated to the modem line index.
+
+mctrl_gpio_set(gpios, mctrl):
+	This will sets the gpios according to the mctrl state.
+
+mctrl_gpio_get(gpios, mctrl):
+	This will update mctrl with the gpios values.
diff --git a/Documentation/serial/riscom8.txt b/Documentation/serial/riscom8.txt
deleted file mode 100644
index 14f61fdad7c..00000000000
--- a/Documentation/serial/riscom8.txt
+++ /dev/null
@@ -1,36 +0,0 @@
-* NOTE - this is an unmaintained driver.  The original author cannot be located.
-
-SDL Communications is now SBS Technologies, and does not have any
-information on these ancient ISA cards on their website.
-
-James Nelson <james4765@gmail.com> - 12-12-2004
-
-	This is the README for RISCom/8 multi-port serial driver
-	(C) 1994-1996 D.Gorodchanin
-	See file LICENSE for terms and conditions.
-
-NOTE: English is not my native language. 
-      I'm sorry for any mistakes in this text.
-
-Misc. notes for RISCom/8 serial driver, in no particular order :)
-
-1) This driver can support up to 4 boards at time.
-   Use string "riscom8=0xXXX,0xXXX,0xXXX,0xXXX" at LILO prompt, for
-   setting I/O base addresses for boards. If you compile driver
-   as module use modprobe options "iobase=0xXXX iobase1=0xXXX iobase2=..."
-
-2) The driver partially supports famous 'setserial' program, you can use almost
-   any of its options, excluding port & irq settings.
-
-3) There are some misc. defines at the beginning of riscom8.c, please read the 
-   comments and try to change some of them in case of problems.
-
-4) I consider the current state of the driver as BETA.
-
-5) SDL Communications WWW page is http://www.sdlcomm.com.
-
-6) You can use the MAKEDEV program to create RISCom/8 /dev/ttyL* entries.
-
-7) Minor numbers for first board are 0-7, for second 8-15, etc.
-
-22 Apr 1996.
diff --git a/Documentation/serial/specialix.txt b/Documentation/serial/specialix.txt
deleted file mode 100644
index 6eb6f3a3331..00000000000
--- a/Documentation/serial/specialix.txt
+++ /dev/null
@@ -1,383 +0,0 @@
-
-      specialix.txt  -- specialix IO8+ multiport serial driver readme.
-
-
-
-      Copyright (C) 1997  Roger Wolff (R.E.Wolff@BitWizard.nl)
-
-      Specialix pays for the development and support of this driver.
-      Please DO contact io8-linux@specialix.co.uk if you require
-      support.
-
-      This driver was developed in the BitWizard linux device
-      driver service. If you require a linux device driver for your
-      product, please contact devices@BitWizard.nl for a quote.
-
-      This code is firmly based on the riscom/8 serial driver,
-      written by Dmitry Gorodchanin. The specialix IO8+ card
-      programming information was obtained from the CL-CD1865 Data
-      Book, and Specialix document number 6200059: IO8+ Hardware
-      Functional Specification, augmented by document number 6200088:
-      Merak Hardware Functional Specification. (IO8+/PCI is also 
-      called Merak)
-
-
-      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.
-
-
-Intro
-=====
-
- 
-This file contains some random information, that I like to have online
-instead of in a manual that can get lost. Ever misplace your Linux
-kernel sources?  And the manual of one of the boards in your computer?
-
-
-Addresses and interrupts
-========================
-
-Address dip switch settings:
-The dip switch sets bits 2-9 of the IO address. 
-
-       9 8 7 6 5 4 3 2 
-     +-----------------+
-   0 | X   X X X X X X |
-     |                 |    =   IoBase = 0x100 
-   1 |   X             |
-     +-----------------+          ------ RS232 connectors ---->
-         
-         |    |    |
-       edge connector
-         |    |    |
-         V    V    V
-
-Base address 0x100 caused a conflict in one of my computers once.  I
-haven't the foggiest why. My Specialix card is now at 0x180.  My
-other computer runs just fine with the Specialix card at 0x100....
-The card occupies 4 addresses, but actually only two are really used.
-
-The PCI version doesn't have any dip switches. The BIOS assigns
-an IO address. 
-
-The driver now still autoprobes at 0x100, 0x180, 0x250 and 0x260.  If
-that causes trouble for you, please report that. I'll remove
-autoprobing then.
-
-The driver will tell the card what IRQ to use, so you don't have to
-change any jumpers to change the IRQ. Just use a command line
-argument (irq=xx) to the insmod program to set the interrupt.
-
-The BIOS assigns the IRQ on the PCI version. You have no say in what
-IRQ to use in that case. 
-
-If your specialix cards are not at the default locations, you can use
-the kernel command line argument "specialix=io0,irq0,io1,irq1...".
-Here "io0" is the io address for the first card, and "irq0" is the
-irq line that the first card should use. And so on. 
-
-Examples. 
-
-You use the driver as a module and have three cards at 0x100, 0x250
-and 0x180. And some way or another you want them detected in that
-order. Moreover irq 12 is taken (e.g. by your PS/2 mouse).
-
-  insmod specialix.o iobase=0x100,0x250,0x180 irq=9,11,15
-
-The same three cards, but now in the kernel would require you to
-add 
-
-   specialix=0x100,9,0x250,11,0x180,15
-
-to the command line. This would become 
-
-   append="specialix=0x100,9,0x250,11,0x180,15" 
-
-in your /etc/lilo.conf file if you use lilo. 
-
-The Specialix driver is slightly odd: It allows you to have the second
-or third card detected without having a first card. This has
-advantages and disadvantages. A slot that isn't filled by an ISA card,
-might be filled if a PCI card is detected. Thus if you have an ISA
-card at 0x250 and a PCI card, you would get:
-
-sx0: specialix IO8+ Board at 0x100 not found.
-sx1: specialix IO8+ Board at 0x180 not found.
-sx2: specialix IO8+ board detected at 0x250, IRQ 12, CD1865 Rev. B.
-sx3: specialix IO8+ Board at 0x260 not found.
-sx0: specialix IO8+ board detected at 0xd800, IRQ 9, CD1865 Rev. B.
-
-This would happen if you don't give any probe hints to the driver. 
-If you would specify:
-
-   specialix=0x250,11
-
-you'd get the following messages:
-
-sx0: specialix IO8+ board detected at 0x250, IRQ 11, CD1865 Rev. B.
-sx1: specialix IO8+ board detected at 0xd800, IRQ 9, CD1865 Rev. B.
-
-ISA probing is aborted after the IO address you gave is exhausted, and
-the PCI card is now detected as the second card. The ISA card is now
-also forced to IRQ11....
-
-
-Baud rates
-==========
-
-The rev 1.2 and below boards use a CL-CD1864. These chips can only 
-do 64kbit. The rev 1.3 and newer boards use a CL-CD1865. These chips
-are officially capable of 115k2.
-
-The Specialix card uses a 25MHz crystal (in times two mode, which in
-fact is a divided by two mode). This is not enough to reach the rated
-115k2 on all ports at the same time. With this clock rate you can only
-do 37% of this rate. This means that at 115k2 on all ports you are
-going to lose characters (The chip cannot handle that many incoming
-bits at this clock rate.) (Yes, you read that correctly: there is a
-limit to the number of -=bits=- per second that the chip can handle.)
-
-If you near the "limit" you will first start to see a graceful
-degradation in that the chip cannot keep the transmitter busy at all
-times. However with a central clock this slow, you can also get it to
-miss incoming characters. The driver will print a warning message when
-you are outside the official specs. The messages usually show up in
-the file /var/log/messages .
-
-The specialix card cannot reliably do 115k2. If you use it, you have
-to do "extensive testing" (*) to verify if it actually works.
-
-When "mgetty" communicates with my modem at 115k2 it reports:
-got: +++[0d]ATQ0V1H0[0d][0d][8a]O[cb][0d][8a]
-                            ^^^^ ^^^^    ^^^^ 
-
-The three characters that have the "^^^" under them have suffered a
-bit error in the highest bit. In conclusion: I've tested it, and found
-that it simply DOESN'T work for me. I also suspect that this is also
-caused by the baud rate being just a little bit out of tune. 
-
-I upgraded the crystal to 66Mhz on one of my Specialix cards. Works
-great! Contact me for details. (Voids warranty, requires a steady hand
-and more such restrictions....)
-
-
-(*) Cirrus logic CD1864 databook, page 40.
-
-
-Cables for the Specialix IO8+
-=============================
-
-The pinout of the connectors on the IO8+ is:
-
-     pin    short    direction    long name
-            name
-    Pin 1   DCD      input        Data Carrier Detect
-    Pin 2   RXD      input        Receive
-    Pin 3   DTR/RTS  output       Data Terminal Ready/Ready To Send
-    Pin 4   GND      -            Ground
-    Pin 5   TXD      output       Transmit
-    Pin 6   CTS      input        Clear To Send
-        
-    
-             -- 6  5  4  3  2  1 --
-             |                    |
-             |                    |
-             |                    |
-             |                    |
-             +-----          -----+
-                  |__________|
-                      clip
-    
-    Front view of an RJ12 connector. Cable moves "into" the paper.
-    (the plug is ready to plug into your mouth this way...)
-
-    
-    NULL cable. I don't know who is going to use these except for
-    testing purposes, but I tested the cards with this cable. (It 
-    took quite a while to figure out, so I'm not going to delete
-    it. So there! :-)
-    
-    
-    This end goes               This end needs
-    straight into the           some twists in
-    RJ12 plug.                  the wiring.
-       IO8+ RJ12                   IO8+ RJ12
-        1  DCD       white          -
-        -             -             1 DCD
-        2  RXD       black          5 TXD
-        3  DTR/RTS   red            6 CTS
-        4  GND       green          4 GND
-        5  TXD       yellow         2 RXD
-        6  CTS       blue           3 DTR/RTS
-    
-
-    Same NULL cable, but now sorted on the second column.
- 
-        1  DCD       white          -
-        -             -             1 DCD
-        5  TXD       yellow         2 RXD
-        6  CTS       blue           3 DTR/RTS
-        4  GND       green          4 GND
-        2  RXD       black          5 TXD
-        3  DTR/RTS   red            6 CTS
-    
-    
-    
-    This is a modem cable usable for hardware handshaking:
-        RJ12                        DB25           DB9
-        1   DCD      white          8 DCD          1 DCD
-        2   RXD      black          3 RXD          2 RXD
-        3   DTR/RTS  red            4 RTS          7 RTS
-        4   GND      green          7 GND          5 GND
-        5   TXD      yellow         2 TXD          3 TXD
-        6   CTS      blue           5 CTS          8 CTS
-                            +----   6 DSR          6 DSR
-                            +----  20 DTR          4 DTR
-
-    This is a modem cable usable for software handshaking:
-    It allows you to reset the modem using the DTR ioctls.
-    I (REW) have never tested this, "but xxxxxxxxxxxxx
-    says that it works." If you test this, please
-    tell me and I'll fill in your name on the xxx's.
-
-        RJ12                        DB25           DB9
-        1   DCD      white          8 DCD          1 DCD
-        2   RXD      black          3 RXD          2 RXD
-        3   DTR/RTS  red           20 DTR          4 DTR
-        4   GND      green          7 GND          5 GND
-        5   TXD      yellow         2 TXD          3 TXD
-        6   CTS      blue           5 CTS          8 CTS
-                            +----   6 DSR          6 DSR
-                            +----   4 RTS          7 RTS
-
-   I bought a 6 wire flat cable. It was colored as indicated.
-   Check that yours is the same before you trust me on this.
-   
- 
-Hardware handshaking issues.
-============================
-
-The driver can be told to operate in two different ways. The default
-behaviour is specialix.sx_rtscts = 0 where the pin behaves as DTR when
-hardware handshaking is off. It behaves as the RTS hardware
-handshaking signal when hardware handshaking is selected.
-
-When you use this, you have to use the appropriate cable. The
-cable will either be compatible with hardware handshaking or with
-software handshaking. So switching on the fly is not really an
-option.
-
-I actually prefer to use the "specialix.sx_rtscts=1" option.
-This makes the DTR/RTS pin always an RTS pin, and ioctls to
-change DTR are always ignored. I have a cable that is configured
-for this. 
-
-
-Ports and devices
-=================
-
-Port 0 is the one furthest from the card-edge connector.
-
-Devices:
-
-You should make the devices as follows:
-
-bash
-cd /dev
-for i in  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
-do
-  echo -n "$i "
-  mknod /dev/ttyW$i c 75 $i
-  mknod /dev/cuw$i c 76 $i
-done
-echo ""
-
-If your system doesn't come with these devices preinstalled, bug your
-linux-vendor about this. They have had ample time to get this
-implemented by now.
-
-You cannot have more than 4 boards in one computer. The card only
-supports 4 different interrupts. If you really want this, contact me
-about this and I'll give you a few tips (requires soldering iron)....
-
-If you have enough PCI slots, you can probably use more than 4 PCI
-versions of the card though.... 
-
-The PCI version of the card cannot adhere to the mechanical part of
-the PCI spec because the 8 serial connectors are simply too large. If
-it doesn't fit in your computer, bring back the card.
-
-
-------------------------------------------------------------------------
-
-
-  Fixed bugs and restrictions:
-       - During initialization, interrupts are blindly turned on.
-            Having a shadow variable would cause an extra memory
-            access on every IO instruction. 
-       - The interrupt (on the card) should be disabled when we
-         don't allocate the Linux end of the interrupt. This allows 
-         a different driver/card to use it while all ports are not in
-         use..... (a la standard serial port)
-       == An extra _off variant of the sx_in and sx_out macros are
-          now available. They don't set the interrupt enable bit.
-          These are used during initialization. Normal operation uses
-          the old variant which enables the interrupt line.
-       - RTS/DTR issue needs to be implemented according to 
-         specialix' spec.
-            I kind of like the "determinism" of the current 
-            implementation. Compile time flag?
-       == Ok. Compile time flag! Default is how Specialix likes it.
-       == Now a config time flag! Gets saved in your config file. Neat!
-       - Can you set the IO address from the lilo command line?
-            If you need this, bug me about it, I'll make it. 
-       == Hah! No bugging needed. Fixed! :-)
-       - Cirrus logic hasn't gotten back to me yet why the CD1865 can
-            and the CD1864 can't do 115k2. I suspect that this is
-            because the CD1864 is not rated for 33MHz operation.
-            Therefore the CD1864 versions of the card can't do 115k2 on 
-            all ports just like the CD1865 versions. The driver does
-            not block 115k2 on CD1864 cards. 
-        == I called the Cirrus Logic representative here in Holland.
-           The CD1864 databook is identical to the CD1865 databook, 
-           except for an extra warning at the end. Similar Bit errors
-           have been observed in testing at 115k2 on both an 1865 and
-           a 1864 chip. I see no reason why I would prohibit 115k2 on
-           1864 chips and not do it on 1865 chips. Actually there is
-           reason to prohibit it on BOTH chips. I print a warning.
-           If you use 115k2, you're on your own. 
-       - A spiky CD may send spurious HUPs. Also in CLOCAL???
-         -- A fix for this turned out to be counter productive. 
-            Different fix? Current behaviour is acceptable?
-         -- Maybe the current implementation is correct. If anybody
-            gets bitten by this, please report, and it will get fixed.
-
-         -- Testing revealed that when in CLOCAL, the problem doesn't
-            occur. As warned for in the CD1865 manual, the chip may
-            send modem intr's on a spike. We could filter those out,
-            but that would be a cludge anyway (You'd still risk getting
-            a spurious HUP when two spikes occur.).....
- 
-
-
-  Bugs & restrictions:
-       - This is a difficult card to autoprobe.
-            You have to WRITE to the address register to even 
-            read-probe a CD186x register. Disable autodetection?
-         -- Specialix: any suggestions?
-
-
diff --git a/Documentation/serial/sx.txt b/Documentation/serial/sx.txt
deleted file mode 100644
index cb4efa0fb5c..00000000000
--- a/Documentation/serial/sx.txt
+++ /dev/null
@@ -1,294 +0,0 @@
-
-      sx.txt  -- specialix SX/SI multiport serial driver readme.
-
-
-
-      Copyright (C) 1997  Roger Wolff (R.E.Wolff@BitWizard.nl)
-
-      Specialix pays for the development and support of this driver.
-      Please DO contact support@specialix.co.uk if you require
-      support.
-
-      This driver was developed in the BitWizard linux device
-      driver service. If you require a linux device driver for your
-      product, please contact devices@BitWizard.nl for a quote.
-
-      (History)
-      There used to be an SI driver by Simon Allan. This is a complete 
-      rewrite  from scratch. Just a few lines-of-code have been snatched.
-
-      (Sources)
-      Specialix document number 6210028: SX Host Card and Download Code
-      Software Functional Specification.
-
-      (Copying)
-      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.
-      
-      (Addendum)
-      I'd appreciate it that if you have fixes, that you send them
-      to me first. 
-
-
-Introduction
-============
-
-This file contains some random information, that I like to have online
-instead of in a manual that can get lost. Ever misplace your Linux
-kernel sources?  And the manual of one of the boards in your computer?
-
-
-Theory of operation
-===================
-
-An important thing to know is that the driver itself doesn't have the
-firmware for the card. This means that you need the separate package
-"sx_firmware". For now you can get the source at
-
-	ftp://ftp.bitwizard.nl/specialix/sx_firmware_<version>.tgz
-
-The firmware load needs a "misc" device, so you'll need to enable the
-"Support for user misc device modules" in your kernel configuration.
-The misc device needs to be called "/dev/specialix_sxctl". It needs
-misc major 10, and minor number 167 (assigned by HPA). The section
-on creating device files below also creates this device. 
-
-After loading the sx.o module into your kernel, the driver will report
-the number of cards detected, but because it doesn't have any
-firmware, it will not be able to determine the number of ports. Only
-when you then run "sx_firmware" will the firmware be downloaded and
-the rest of the driver initialized. At that time the sx_firmware
-program will report the number of ports installed.
-
-In contrast with many other multi port serial cards, some of the data
-structures are only allocated when the card knows the number of ports
-that are connected. This means we won't waste memory for 120 port
-descriptor structures when you only have 8 ports. If you experience
-problems due to this, please report them: I haven't seen any.
-
-
-Interrupts
-==========
-
-A multi port serial card, would generate a horrendous amount of
-interrupts if it would interrupt the CPU for every received
-character. Even more than 10 years ago, the trick not to use
-interrupts but to poll the serial cards was invented.
-
-The SX card allow us to do this two ways. First the card limits its
-own interrupt rate to a rate that won't overwhelm the CPU. Secondly,
-we could forget about the cards interrupt completely and use the
-internal timer for this purpose.
-
-Polling the card can take up to a few percent of your CPU. Using the
-interrupts would be better if you have most of the ports idle. Using
-timer-based polling is better if your card almost always has work to
-do. You save the separate interrupt in that case.
-
-In any case, it doesn't really matter all that much. 
-
-The most common problem with interrupts is that for ISA cards in a PCI
-system the BIOS has to be told to configure that interrupt as "legacy
-ISA". Otherwise the card can pull on the interrupt line all it wants
-but the CPU won't see this.
-
-If you can't get the interrupt to work, remember that polling mode is
-more efficient (provided you actually use the card intensively).
-
-
-Allowed Configurations
-======================
-
-Some configurations are disallowed. Even though at a glance they might
-seem to work, they are known to lockup the bus between the host card
-and the device concentrators. You should respect the drivers decision
-not to support certain configurations. It's there for a reason.
-
-Warning: Seriously technical stuff ahead. Executive summary: Don't use
-SX cards except configured at a 64k boundary. Skip the next paragraph.
-
-The SX cards can theoretically be placed at a 32k boundary. So for
-instance you can put an SX card at 0xc8000-0xd7fff. This is not a
-"recommended configuration". ISA cards have to tell the bus controller
-how they like their timing. Due to timing issues they have to do this
-based on which 64k window the address falls into. This means that the
-32k window below and above the SX card have to use exactly the same
-timing as the SX card. That reportedly works for other SX cards. But
-you're still left with two useless 32k windows that should not be used
-by anybody else.
-
-
-Configuring the driver
-======================
-
-PCI cards are always detected. The driver auto-probes for ISA cards at
-some sensible addresses. Please report if the auto-probe causes trouble
-in your system, or when a card isn't detected.
-
-I'm afraid I haven't implemented "kernel command line parameters" yet.
-This means that if the default doesn't work for you, you shouldn't use
-the compiled-into-the-kernel version of the driver. Use a module
-instead.  If you convince me that you need this, I'll make it for
-you. Deal?
-
-I'm afraid that the module parameters are a bit clumsy. If you have a
-better idea, please tell me.
-
-You can specify several parameters:
-
-	sx_poll: number of jiffies between timer-based polls.
-
-		Set this to "0" to disable timer based polls. 
-                Initialization of cards without a working interrupt
-                will fail.
-
-		Set this to "1" if you want a polling driver. 
-		(on Intel: 100 polls per second). If you don't use
-                fast baud rates, you might consider a value like "5". 
-                (If you don't know how to do the math, use 1).
-
-	sx_slowpoll: Number of jiffies between timer-based polls. 
- 		Set this to "100" to poll once a second. 
-		This should get the card out of a stall if the driver
-                ever misses an interrupt. I've never seen this happen,
-                and if it does, that's a bug. Tell me. 
-
-	sx_maxints: Number of interrupts to request from the card. 
-		The card normally limits interrupts to about 100 per
-		second to offload the host CPU. You can increase this
-		number to reduce latency on the card a little.
-		Note that if you give a very high number you can overload
-		your CPU as well as the CPU on the host card. This setting 
-		is inaccurate and not recommended for SI cards (But it 
-		works). 
-
-	sx_irqmask: The mask of allowable IRQs to use. I suggest you set 
-		this to 0 (disable IRQs all together) and use polling if 
-		the assignment of IRQs becomes problematic. This is defined
-		as the sum of (1 << irq) 's that you want to allow. So 
-		sx_irqmask of 8 (1 << 3) specifies that only irq 3 may
-		be used by the SX driver. If you want to specify to the 
-		driver: "Either irq 11 or 12 is ok for you to use", then
-		specify (1 << 11) | (1 << 12) = 0x1800 . 
-
-	sx_debug: You can enable different sorts of debug traces with this. 
-		At "-1" all debugging traces are active. You'll get several
-		times more debugging output than you'll get characters 
-		transmitted. 
-
-
-Baud rates
-==========
-
-Theoretically new SXDCs should be capable of more than 460k
-baud. However the line drivers usually give up before that.  Also the
-CPU on the card may not be able to handle 8 channels going at full
-blast at that speed. Moreover, the buffers are not large enough to
-allow operation with 100 interrupts per second. You'll have to realize
-that the card has a 256 byte buffer, so you'll have to increase the
-number of interrupts per second if you have more than 256*100 bytes
-per second to transmit.  If you do any performance testing in this
-area, I'd be glad to hear from you...
-
-(Psst Linux users..... I think the Linux driver is more efficient than
-the driver for other OSes. If you can and want to benchmark them
-against each other, be my guest, and report your findings...... :-)
-
-
-Ports and devices
-=================
-
-Port 0 is the top connector on the module closest to the host
-card. Oh, the ports on the SXDCs and TAs are labelled from 1 to 8
-instead of from 0 to 7, as they are numbered by linux. I'm stubborn in
-this: I know for sure that I wouldn't be able to calculate which port
-is which anymore if I would change that....
-
-
-Devices:
-
-You should make the device files as follows:
-
-#!/bin/sh
-# (I recommend that you cut-and-paste this into a file and run that)
-cd /dev
-t=0
-mknod specialix_sxctl c 10 167
-while [ $t -lt 64 ]
-  do 
-  echo -n "$t "
-  mknod ttyX$t c 32 $t
-  mknod cux$t  c 33 $t
-  t=`expr $t + 1`
-done
-echo ""
-rm /etc/psdevtab
-ps > /dev/null
-
-
-This creates 64 devices. If you have more, increase the constant on
-the line with "while". The devices start at 0, as is customary on
-Linux. Specialix seems to like starting the numbering at 1. 
-
-If your system doesn't come with these devices pre-installed, bug your
-linux-vendor about this. They should have these devices
-"pre-installed" before the new millennium. The "ps" stuff at the end
-is to "tell" ps that the new devices exist.
-
-Officially the maximum number of cards per computer is 4. This driver
-however supports as many cards in one machine as you want. You'll run
-out of interrupts after a few, but you can switch to polled operation
-then. At about 256 ports (More than 8 cards), we run out of minor
-device numbers. Sorry. I suggest you buy a second computer.... (Or
-switch to RIO).
-
-------------------------------------------------------------------------
-
-
-  Fixed bugs and restrictions:
-	- Hangup processing.  
-	  -- Done.
-
-	- the write path in generic_serial (lockup / oops). 
-	  -- Done (Ugly: not the way I want it. Copied from serial.c).
-
-        - write buffer isn't flushed at close.
-	  -- Done. I still seem to lose a few chars at close. 
-	     Sorry. I think that this is a firmware issue. (-> Specialix)
-
-	- drain hardware before  changing termios
-	- Change debug on the fly. 
-	- ISA free irq -1. (no firmware loaded).
-	- adding c8000 as a probe address. Added warning. 
-	- Add a RAMtest for the RAM on the card.c
-        - Crash when opening a port "way" of the number of allowed ports. 
-          (for example opening port 60 when there are only 24 ports attached)
-	- Sometimes the use-count strays a bit. After a few hours of
-          testing the use count is sometimes "3". If you are not like
-          me and can remember what you did to get it that way, I'd
-          appreciate an Email. Possibly fixed. Tell me if anyone still
-          sees this.
-	- TAs don't work right if you don't connect all the modem control
-	  signals. SXDCs do. T225 firmware problem -> Specialix. 
-	  (Mostly fixed now, I think. Tell me if you encounter this!)
-
-  Bugs & restrictions:
-
-	- Arbitrary baud rates. Requires firmware update. (-> Specialix)
-
-	- Low latency (mostly firmware, -> Specialix)
-
-
-
diff --git a/Documentation/stable_kernel_rules.txt b/Documentation/stable_kernel_rules.txt
index b0714d8f678..cbc2f03056b 100644
--- a/Documentation/stable_kernel_rules.txt
+++ b/Documentation/stable_kernel_rules.txt
@@ -39,7 +39,7 @@ Procedure for submitting patches to the -stable tree:
    the stable tree without anything else needing to be done by the author
    or subsystem maintainer.
  - If the patch requires other patches as prerequisites which can be
-   cherry-picked than this can be specified in the following format in
+   cherry-picked, then this can be specified in the following format in
    the sign-off area:
 
      Cc: <stable@vger.kernel.org> # 3.3.x: a1f84a3: sched: Check for idle
diff --git a/Documentation/usb/chipidea.txt b/Documentation/usb/chipidea.txt
new file mode 100644
index 00000000000..995c8bca40e
--- /dev/null
+++ b/Documentation/usb/chipidea.txt
@@ -0,0 +1,71 @@
+1. How to test OTG FSM(HNP and SRP)
+-----------------------------------
+To show how to demo OTG HNP and SRP functions via sys input files
+with 2 Freescale i.MX6Q sabre SD boards.
+
+1.1 How to enable OTG FSM in menuconfig
+---------------------------------------
+Select CONFIG_USB_OTG_FSM, rebuild kernel Image and modules.
+If you want to check some internal variables for otg fsm,
+select CONFIG_USB_CHIPIDEA_DEBUG, there are 2 files which
+can show otg fsm variables and some controller registers value:
+cat /sys/kernel/debug/ci_hdrc.0/otg
+cat /sys/kernel/debug/ci_hdrc.0/registers
+
+1.2 Test operations
+-------------------
+1) Power up 2 Freescale i.MX6Q sabre SD boards with gadget class driver loaded
+   (e.g. g_mass_storage).
+
+2) Connect 2 boards with usb cable with one end is micro A plug, the other end
+   is micro B plug.
+
+   The A-device(with micro A plug inserted) should enumrate B-device.
+
+3) Role switch
+   On B-device:
+   echo 1 > /sys/bus/platform/devices/ci_hdrc.0/inputs/b_bus_req
+
+   if HNP polling is not supported, also need:
+   On A-device:
+   echo 0 > /sys/bus/platform/devices/ci_hdrc.0/inputs/a_bus_req
+
+   B-device should take host role and enumrate A-device.
+
+4) A-device switch back to host.
+   On B-device:
+   echo 0 > /sys/bus/platform/devices/ci_hdrc.0/inputs/b_bus_req
+
+   A-device should switch back to host and enumrate B-device.
+
+5) Remove B-device(unplug micro B plug) and insert again in 10 seconds,
+   A-device should enumrate B-device again.
+
+6) Remove B-device(unplug micro B plug) and insert again after 10 seconds,
+   A-device should NOT enumrate B-device.
+
+   if A-device wants to use bus:
+   On A-device:
+   echo 0 > /sys/bus/platform/devices/ci_hdrc.0/inputs/a_bus_drop
+   echo 1 > /sys/bus/platform/devices/ci_hdrc.0/inputs/a_bus_req
+
+   if B-device wants to use bus:
+   On B-device:
+   echo 1 > /sys/bus/platform/devices/ci_hdrc.0/inputs/b_bus_req
+
+7) A-device power down the bus.
+   On A-device:
+   echo 1 > /sys/bus/platform/devices/ci_hdrc.0/inputs/a_bus_drop
+
+   A-device should disconnect with B-device and power down the bus.
+
+8) B-device does data pulse for SRP.
+   On B-device:
+   echo 1 > /sys/bus/platform/devices/ci_hdrc.0/inputs/b_bus_req
+
+   A-device should resume usb bus and enumrate B-device.
+
+1.3 Reference document
+----------------------
+"On-The-Go and Embedded Host Supplement to the USB Revision 2.0 Specification
+July 27, 2012 Revision 2.0 version 1.1a"
diff --git a/Documentation/virtual/kvm/api.txt b/Documentation/virtual/kvm/api.txt
index a9380ba54c8..b4f53653c10 100644
--- a/Documentation/virtual/kvm/api.txt
+++ b/Documentation/virtual/kvm/api.txt
@@ -2126,7 +2126,7 @@ into the hash PTE second double word).
 4.75 KVM_IRQFD
 
 Capability: KVM_CAP_IRQFD
-Architectures: x86
+Architectures: x86 s390
 Type: vm ioctl
 Parameters: struct kvm_irqfd (in)
 Returns: 0 on success, -1 on error
diff --git a/Documentation/vm/numa_memory_policy.txt b/Documentation/vm/numa_memory_policy.txt
index 4e7da654342..badb0507608 100644
--- a/Documentation/vm/numa_memory_policy.txt
+++ b/Documentation/vm/numa_memory_policy.txt
@@ -174,7 +174,6 @@ Components of Memory Policies
 	allocation fails, the kernel will search other nodes, in order of
 	increasing distance from the preferred node based on information
 	provided by the platform firmware.
-	containing the cpu where the allocation takes place.
 
 	    Internally, the Preferred policy uses a single node--the
 	    preferred_node member of struct mempolicy.  When the internal
@@ -275,9 +274,9 @@ Components of Memory Policies
 	    For example, consider a task that is attached to a cpuset with
 	    mems 2-5 that sets an Interleave policy over the same set with
 	    MPOL_F_RELATIVE_NODES.  If the cpuset's mems change to 3-7, the
-	    interleave now occurs over nodes 3,5-6.  If the cpuset's mems
+	    interleave now occurs over nodes 3,5-7.  If the cpuset's mems
 	    then change to 0,2-3,5, then the interleave occurs over nodes
-	    0,3,5.
+	    0,2-3,5.
 
 	    Thanks to the consistent remapping, applications preparing
 	    nodemasks to specify memory policies using this flag should
diff --git a/Documentation/w1/w1.generic b/Documentation/w1/w1.generic
index a31c5a24297..b2033c64c7d 100644
--- a/Documentation/w1/w1.generic
+++ b/Documentation/w1/w1.generic
@@ -82,7 +82,7 @@ driver             - (standard) symlink to the w1 driver
 w1_master_add      - Manually register a slave device
 w1_master_attempts - the number of times a search was attempted
 w1_master_max_slave_count
-                   - the maximum slaves that may be attached to a master
+                   - maximum number of slaves to search for at a time
 w1_master_name     - the name of the device (w1_bus_masterX)
 w1_master_pullup   - 5V strong pullup 0 enabled, 1 disabled
 w1_master_remove   - Manually remove a slave device
diff --git a/Documentation/w1/w1.netlink b/Documentation/w1/w1.netlink
index 927a52cc051..ef2727192d6 100644
--- a/Documentation/w1/w1.netlink
+++ b/Documentation/w1/w1.netlink
@@ -30,7 +30,7 @@ Protocol.
 			W1_SLAVE_CMD
 				userspace command for slave device
 				(read/write/touch)
-	__u8 res	- reserved
+	__u8 status	- error indication from kernel
 	__u16 len	- size of data attached to this header data
 	union {
 		__u8 id[8];			 - slave unique device id
@@ -44,10 +44,14 @@ Protocol.
 	__u8 cmd	- command opcode.
 			W1_CMD_READ 	- read command
 			W1_CMD_WRITE	- write command
-			W1_CMD_TOUCH	- touch command
-				(write and sample data back to userspace)
 			W1_CMD_SEARCH	- search command
 			W1_CMD_ALARM_SEARCH - alarm search command
+			W1_CMD_TOUCH	- touch command
+				(write and sample data back to userspace)
+			W1_CMD_RESET	- send bus reset
+			W1_CMD_SLAVE_ADD	- add slave to kernel list
+			W1_CMD_SLAVE_REMOVE	- remove slave from kernel list
+			W1_CMD_LIST_SLAVES	- get slaves list from kernel
 	__u8 res	- reserved
 	__u16 len	- length of data for this command
 		For read command data must be allocated like for write command
@@ -87,8 +91,7 @@ format:
 	id0 ... idN
 
 	Each message is at most 4k in size, so if number of master devices
-	exceeds this, it will be split into several messages,
-	cn.seq will be increased for each one.
+	exceeds this, it will be split into several messages.
 
 W1 search and alarm search commands.
 request:
diff --git a/Documentation/zh_CN/HOWTO b/Documentation/zh_CN/HOWTO
index 6c914aa87e7..54ea24ff63c 100644
--- a/Documentation/zh_CN/HOWTO
+++ b/Documentation/zh_CN/HOWTO
@@ -237,7 +237,7 @@ kernel.org网站的pub/linux/kernel/v2.6/目录下找到它。它的开��循
 如果没有2.6.x.y版本内核存在，那么最新的2.6.x版本内核就相当于是当�的稳定
 版内核。
 
-2.6.x.y版本由“稳定版��组（邮件地�<stable@kernel.org>）维护，一般隔周�
+2.6.x.y版本由“稳定版��组（邮件地�<stable@vger.kernel.org>）维护，一般隔周�
 布新版本。
 
 内核��中的Documentation/stable_kernel_rules.txt文件具体�述了�被稳定
diff --git a/Documentation/zh_CN/io_ordering.txt b/Documentation/zh_CN/io_ordering.txt
new file mode 100644
index 00000000000..e592daf4e01
--- /dev/null
+++ b/Documentation/zh_CN/io_ordering.txt
@@ -0,0 +1,67 @@
+Chinese translated version of Documentation/io_orderings.txt
+
+If you have any comment or update to the content, please contact the
+original document maintainer directly.  However, if you have a problem
+communicating in English you can also ask the Chinese maintainer for
+help.  Contact the Chinese maintainer if this translation is outdated
+or if there is a problem with the translation.
+
+Chinese maintainer: Lin Yongting <linyongting@gmail.com>
+---------------------------------------------------------------------
+Documentation/io_ordering.txt 的中文翻译
+
+如果想评论或更新本文的内容，请直接�系原文档的维护者。如果你使用英文
+交�有困难的�，也�以�中文版维护者求助。如果本翻译更新��时或者翻
+译存在问题，请�系中文版维护者。
+
+中文版维护者： 林永� Lin Yongting <linyongting@gmail.com>
+中文版翻译者： 林永� Lin Yongting <linyongting@gmail.com>
+中文版校译者： 林永� Lin Yongting <linyongting@gmail.com>
+
+
+以下为正文
+---------------------------------------------------------------------
+
+在�些平�上，所谓的内存映射I/O是弱顺�。在这些平�上，驱动开�者有责任
+��I/O内存映射地�的写�作按程�图�的顺�达到设备。通常读�一个“安全�
+设备寄存器或桥寄存器，触�IO芯片清刷未处�的写�作到达设备��处�读�作，
+而达到��目的。驱动程�通常在spinlock�护的临界区退出之�使用这�技术。
+这也�以����的写�作�在��的写�作之�到达设备（这�常类似于内存
+�障�作，mb()，�过仅适用于I/O）。
+
+�设一个设备驱动程的具体例�：
+
+        ...
+CPU A:  spin_lock_irqsave(&dev_lock, flags)
+CPU A:  val = readl(my_status);
+CPU A:  ...
+CPU A:  writel(newval, ring_ptr);
+CPU A:  spin_unlock_irqrestore(&dev_lock, flags)
+        ...
+CPU B:  spin_lock_irqsave(&dev_lock, flags)
+CPU B:  val = readl(my_status);
+CPU B:  ...
+CPU B:  writel(newval2, ring_ptr);
+CPU B:  spin_unlock_irqrestore(&dev_lock, flags)
+        ...
+
+上述例�中，设备�能会先接收到newval2的值，然�接收到newval的值，问题就
+�生了。�过很容易通过下�方法�修�：
+
+        ...
+CPU A:  spin_lock_irqsave(&dev_lock, flags)
+CPU A:  val = readl(my_status);
+CPU A:  ...
+CPU A:  writel(newval, ring_ptr);
+CPU A:  (void)readl(safe_register); /* �置寄存器？*/
+CPU A:  spin_unlock_irqrestore(&dev_lock, flags)
+        ...
+CPU B:  spin_lock_irqsave(&dev_lock, flags)
+CPU B:  val = readl(my_status);
+CPU B:  ...
+CPU B:  writel(newval2, ring_ptr);
+CPU B:  (void)readl(safe_register); /* �置寄存器？*/
+CPU B:  spin_unlock_irqrestore(&dev_lock, flags)
+
+在解决方案中，读�safe_register寄存器，触�IO芯片清刷未处�的写�作，
+�处���的读�作，防止引�数��一致问题。
diff --git a/Documentation/zh_CN/magic-number.txt b/Documentation/zh_CN/magic-number.txt
index 2ebe539f545..dfb72a5c63e 100644
--- a/Documentation/zh_CN/magic-number.txt
+++ b/Documentation/zh_CN/magic-number.txt
@@ -63,8 +63,6 @@ struct tty_ldisc {
 PG_MAGIC              'P'         pg_{read,write}_hdr include/linux/pg.h
 CMAGIC                0x0111      user              include/linux/a.out.h
 MKISS_DRIVER_MAGIC    0x04bf      mkiss_channel     drivers/net/mkiss.h
-RISCOM8_MAGIC         0x0907      riscom_port       drivers/char/riscom8.h
-SPECIALIX_MAGIC       0x0907      specialix_port    drivers/char/specialix_io8.h
 HDLC_MAGIC            0x239e      n_hdlc            drivers/char/n_hdlc.c
 APM_BIOS_MAGIC        0x4101      apm_user          arch/x86/kernel/apm_32.c
 CYCLADES_MAGIC        0x4359      cyclades_port     include/linux/cyclades.h
@@ -82,7 +80,6 @@ STRIP_MAGIC           0x5303      strip             drivers/net/strip.c
 X25_ASY_MAGIC         0x5303      x25_asy           drivers/net/x25_asy.h
 SIXPACK_MAGIC         0x5304      sixpack           drivers/net/hamradio/6pack.h
 AX25_MAGIC            0x5316      ax_disp           drivers/net/mkiss.h
-ESP_MAGIC             0x53ee      esp_struct        drivers/char/esp.h
 TTY_MAGIC             0x5401      tty_struct        include/linux/tty.h
 MGSL_MAGIC            0x5401      mgsl_info         drivers/char/synclink.c
 TTY_DRIVER_MAGIC      0x5402      tty_driver        include/linux/tty_driver.h
@@ -94,13 +91,10 @@ USB_BLUETOOTH_MAGIC   0x6d02      usb_bluetooth     drivers/usb/class/bluetty.c
 RFCOMM_TTY_MAGIC      0x6d02                        net/bluetooth/rfcomm/tty.c
 USB_SERIAL_PORT_MAGIC 0x7301      usb_serial_port   drivers/usb/serial/usb-serial.h
 CG_MAGIC              0x00090255  ufs_cylinder_group include/linux/ufs_fs.h
-A2232_MAGIC           0x000a2232  gs_port           drivers/char/ser_a2232.h
 RPORT_MAGIC           0x00525001  r_port            drivers/char/rocket_int.h
 LSEMAGIC              0x05091998  lse               drivers/fc4/fc.c
 GDTIOCTL_MAGIC        0x06030f07  gdth_iowr_str     drivers/scsi/gdth_ioctl.h
 RIEBL_MAGIC           0x09051990                    drivers/net/atarilance.c
-RIO_MAGIC             0x12345678  gs_port           drivers/char/rio/rio_linux.c
-SX_MAGIC              0x12345678  gs_port           drivers/char/sx.h
 NBD_REQUEST_MAGIC     0x12560953  nbd_request       include/linux/nbd.h
 RED_MAGIC2            0x170fc2a5  (any)             mm/slab.c
 BAYCOM_MAGIC          0x19730510  baycom_state      drivers/net/baycom_epp.c
@@ -116,7 +110,6 @@ ISDN_ASYNC_MAGIC      0x49344C01  modem_info        include/linux/isdn.h
 CTC_ASYNC_MAGIC       0x49344C01  ctc_tty_info      drivers/s390/net/ctctty.c
 ISDN_NET_MAGIC        0x49344C02  isdn_net_local_s  drivers/isdn/i4l/isdn_net_lib.h
 SAVEKMSG_MAGIC2       0x4B4D5347  savekmsg          arch/*/amiga/config.c
-STLI_BOARDMAGIC       0x4bc6c825  stlibrd           include/linux/istallion.h
 CS_STATE_MAGIC        0x4c4f4749  cs_state          sound/oss/cs46xx.c
 SLAB_C_MAGIC          0x4f17a36d  kmem_cache        mm/slab.c
 COW_MAGIC             0x4f4f4f4d  cow_header_v1     arch/um/drivers/ubd_user.c
@@ -127,10 +120,8 @@ SCC_MAGIC             0x52696368  gs_port           drivers/char/scc.h
 SAVEKMSG_MAGIC1       0x53415645  savekmsg          arch/*/amiga/config.c
 GDA_MAGIC             0x58464552  gda               arch/mips/include/asm/sn/gda.h
 RED_MAGIC1            0x5a2cf071  (any)             mm/slab.c
-STL_PORTMAGIC         0x5a7182c9  stlport           include/linux/stallion.h
 EEPROM_MAGIC_VALUE    0x5ab478d2  lanai_dev         drivers/atm/lanai.c
 HDLCDRV_MAGIC         0x5ac6e778  hdlcdrv_state     include/linux/hdlcdrv.h
-EPCA_MAGIC            0x5c6df104  channel           include/linux/epca.h
 PCXX_MAGIC            0x5c6df104  channel           drivers/char/pcxx.h
 KV_MAGIC              0x5f4b565f  kernel_vars_s     arch/mips/include/asm/sn/klkernvars.h
 I810_STATE_MAGIC      0x63657373  i810_state        sound/oss/i810_audio.c
@@ -142,17 +133,14 @@ SLOT_MAGIC            0x67267322  slot              drivers/hotplug/acpiphp.h
 LO_MAGIC              0x68797548  nbd_device        include/linux/nbd.h
 OPROFILE_MAGIC        0x6f70726f  super_block       drivers/oprofile/oprofilefs.h
 M3_STATE_MAGIC        0x734d724d  m3_state          sound/oss/maestro3.c
-STL_PANELMAGIC        0x7ef621a1  stlpanel          include/linux/stallion.h
 VMALLOC_MAGIC         0x87654320  snd_alloc_track   sound/core/memory.c
 KMALLOC_MAGIC         0x87654321  snd_alloc_track   sound/core/memory.c
 PWC_MAGIC             0x89DC10AB  pwc_device        drivers/usb/media/pwc.h
 NBD_REPLY_MAGIC       0x96744668  nbd_reply         include/linux/nbd.h
-STL_BOARDMAGIC        0xa2267f52  stlbrd            include/linux/stallion.h
 ENI155_MAGIC          0xa54b872d  midway_eprom	    drivers/atm/eni.h
 SCI_MAGIC             0xbabeface  gs_port           drivers/char/sh-sci.h
 CODA_MAGIC            0xC0DAC0DA  coda_file_info    include/linux/coda_fs_i.h
 DPMEM_MAGIC           0xc0ffee11  gdt_pci_sram      drivers/scsi/gdth.h
-STLI_PORTMAGIC        0xe671c7a1  stliport          include/linux/istallion.h
 YAM_MAGIC             0xF10A7654  yam_port          drivers/net/hamradio/yam.c
 CCB_MAGIC             0xf2691ad2  ccb               drivers/scsi/ncr53c8xx.c
 QUEUE_MAGIC_FREE      0xf7e1c9a3  queue_entry       drivers/scsi/arm/queue.c
diff --git a/Documentation/zh_CN/stable_kernel_rules.txt b/Documentation/zh_CN/stable_kernel_rules.txt
index b5b9b0ab02f..26ea5ed7cd9 100644
--- a/Documentation/zh_CN/stable_kernel_rules.txt
+++ b/Documentation/zh_CN/stable_kernel_rules.txt
@@ -42,7 +42,7 @@ Documentation/stable_kernel_rules.txt 的中文翻译
 
 �稳定版代�树�交补�的过程：
 
-  - 在确认了补�符�以上的规则�，将补���到stable@kernel.org。
+  - 在确认了补�符�以上的规则�，将补���到stable@vger.kernel.org。
   - 如果补�被接�到队列里，��者会收到一个ACK回�，如果没有被接�，收
     到的是NAK回�。回�需�几天的时间，这�决于开�者的时间安排。
   - 被接�的补�会被加到稳定版本队列里，等待其他开�者的审查。
diff --git a/MAINTAINERS b/MAINTAINERS
index 80399fff805..53feaaf1cf1 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -355,7 +355,7 @@ F:	Documentation/hwmon/adm1025
 F:	drivers/hwmon/adm1025.c
 
 ADM1029 HARDWARE MONITOR DRIVER
-M:	Corentin Labbe <corentin.labbe@geomatys.fr>
+M:	Corentin Labbe <clabbe.montjoie@gmail.com>
 L:	lm-sensors@lm-sensors.org
 S:	Maintained
 F:	drivers/hwmon/adm1029.c
@@ -537,7 +537,7 @@ L:	linux-alpha@vger.kernel.org
 F:	arch/alpha/
 
 ALTERA TRIPLE SPEED ETHERNET DRIVER
-M:	Vince Bridgers <vbridgers2013@gmail.com
+M:	Vince Bridgers <vbridgers2013@gmail.com>
 L:	netdev@vger.kernel.org
 L:	nios2-dev@lists.rocketboards.org (moderated for non-subscribers)
 S:	Maintained
@@ -1617,12 +1617,6 @@ S:	Supported
 F:	drivers/misc/atmel_tclib.c
 F:	drivers/clocksource/tcb_clksrc.c
 
-ATMEL TSADCC DRIVER
-M:	Josh Wu <josh.wu@atmel.com>
-L:	linux-input@vger.kernel.org
-S:	Supported
-F:	drivers/input/touchscreen/atmel_tsadcc.c
-
 ATMEL USBA UDC DRIVER
 M:	Nicolas Ferre <nicolas.ferre@atmel.com>
 L:	linux-arm-kernel@lists.infradead.org (moderated for non-subscribers)
@@ -1893,14 +1887,15 @@ L:	netdev@vger.kernel.org
 S:	Supported
 F:	drivers/net/ethernet/broadcom/bnx2x/
 
-BROADCOM BCM281XX/BCM11XXX ARM ARCHITECTURE
+BROADCOM BCM281XX/BCM11XXX/BCM216XX ARM ARCHITECTURE
 M:	Christian Daudt <bcm@fixthebug.org>
 M:	Matt Porter <mporter@linaro.org>
 L:	bcm-kernel-feedback-list@broadcom.com
-T:	git git://git.github.com/broadcom/bcm11351
+T:	git git://github.com/broadcom/mach-bcm
 S:	Maintained
 F:	arch/arm/mach-bcm/
 F:	arch/arm/boot/dts/bcm113*
+F:	arch/arm/boot/dts/bcm216*
 F:	arch/arm/boot/dts/bcm281*
 F:	arch/arm/configs/bcm_defconfig
 F:	drivers/mmc/host/sdhci_bcm_kona.c
@@ -2193,6 +2188,7 @@ T:	git git://git.kernel.org/pub/scm/linux/kernel/git/gregkh/char-misc.git
 S:	Supported
 F:	drivers/char/*
 F:	drivers/misc/*
+F:	include/linux/miscdevice.h
 
 CHECKPATCH
 M:	Andy Whitcroft <apw@canonical.com>
@@ -2245,12 +2241,6 @@ L:	linux-usb@vger.kernel.org
 S:	Maintained
 F:	drivers/usb/host/ohci-ep93xx.c
 
-CIRRUS LOGIC CS4270 SOUND DRIVER
-M:	Timur Tabi <timur@tabi.org>
-L:	alsa-devel@alsa-project.org (moderated for non-subscribers)
-S:	Odd Fixes
-F:	sound/soc/codecs/cs4270*
-
 CIRRUS LOGIC AUDIO CODEC DRIVERS
 M:	Brian Austin <brian.austin@cirrus.com>
 M:	Paul Handrigan <Paul.Handrigan@cirrus.com>
@@ -2789,12 +2779,14 @@ F:	sound/soc/codecs/da[79]*.[ch]
 
 DIGI NEO AND CLASSIC PCI PRODUCTS
 M:	Lidza Louina <lidza.louina@gmail.com>
+M:	Mark Hounschell <markh@compro.net>
 L:	driverdev-devel@linuxdriverproject.org
 S:	Maintained
 F:	drivers/staging/dgnc/
 
 DIGI EPCA PCI PRODUCTS
 M:	Lidza Louina <lidza.louina@gmail.com>
+M:	Mark Hounschell <markh@compro.net>
 L:	driverdev-devel@linuxdriverproject.org
 S:	Maintained
 F:	drivers/staging/dgap/
@@ -3485,6 +3477,12 @@ S:	Maintained
 F:	drivers/extcon/
 F:	Documentation/extcon/
 
+EXYNOS DP DRIVER
+M:	Jingoo Han <jg1.han@samsung.com>
+L:	dri-devel@lists.freedesktop.org
+S:	Maintained
+F:	drivers/gpu/drm/exynos/exynos_dp*
+
 EXYNOS MIPI DISPLAY DRIVERS
 M:	Inki Dae <inki.dae@samsung.com>
 M:	Donghwa Lee <dh09.lee@samsung.com>
@@ -3550,7 +3548,7 @@ F:	include/scsi/libfcoe.h
 F:	include/uapi/scsi/fc/
 
 FILE LOCKING (flock() and fcntl()/lockf())
-M:	Jeff Layton <jlayton@redhat.com>
+M:	Jeff Layton <jlayton@poochiereds.net>
 M:	J. Bruce Fields <bfields@fieldses.org>
 L:	linux-fsdevel@vger.kernel.org
 S:	Maintained
@@ -4203,9 +4201,11 @@ S:	Maintained
 F:	fs/hpfs/
 
 HSI SUBSYSTEM
-M:	Sebastian Reichel <sre@debian.org>
+M:	Sebastian Reichel <sre@kernel.org>
+T:	git git://git.kernel.org/pub/scm/linux/kernel/git/sre/linux-hsi.git
 S:	Maintained
 F:	Documentation/ABI/testing/sysfs-bus-hsi
+F:	Documentation/hsi.txt
 F:	drivers/hsi/
 F:	include/linux/hsi/
 F:	include/uapi/linux/hsi/
@@ -4812,6 +4812,14 @@ L:	linux-kernel@vger.kernel.org
 S:	Maintained
 T:	git git://git.kernel.org/pub/scm/linux/kernel/git/tip/tip.git irq/core
 F:	kernel/irq/
+
+IRQCHIP DRIVERS
+M:	Thomas Gleixner <tglx@linutronix.de>
+M:	Jason Cooper <jason@lakedaemon.net>
+L:	linux-kernel@vger.kernel.org
+S:	Maintained
+T:	git git://git.kernel.org/pub/scm/linux/kernel/git/tip/tip.git irq/core
+T:	git git://git.infradead.org/users/jcooper/linux.git irqchip/core
 F:	drivers/irqchip/
 
 IRQ DOMAINS (IRQ NUMBER MAPPING LIBRARY)
@@ -5108,14 +5116,19 @@ F:	drivers/s390/kvm/
 
 KERNEL VIRTUAL MACHINE (KVM) FOR ARM
 M:	Christoffer Dall <christoffer.dall@linaro.org>
+M:	Marc Zyngier <marc.zyngier@arm.com>
+L:	linux-arm-kernel@lists.infradead.org (moderated for non-subscribers)
 L:	kvmarm@lists.cs.columbia.edu
 W:	http://systems.cs.columbia.edu/projects/kvm-arm
 S:	Supported
 F:	arch/arm/include/uapi/asm/kvm*
 F:	arch/arm/include/asm/kvm*
 F:	arch/arm/kvm/
+F:	virt/kvm/arm/
+F:	include/kvm/arm_*
 
 KERNEL VIRTUAL MACHINE FOR ARM64 (KVM/arm64)
+M:	Christoffer Dall <christoffer.dall@linaro.org>
 M:	Marc Zyngier <marc.zyngier@arm.com>
 L:	linux-arm-kernel@lists.infradead.org (moderated for non-subscribers)
 L:	kvmarm@lists.cs.columbia.edu
@@ -5479,15 +5492,15 @@ F:	Documentation/hwmon/ltc4261
 F:	drivers/hwmon/ltc4261.c
 
 LTP (Linux Test Project)
-M:	Shubham Goyal <shubham@linux.vnet.ibm.com>
 M:	Mike Frysinger <vapier@gentoo.org>
 M:	Cyril Hrubis <chrubis@suse.cz>
-M:	Caspar Zhang <caspar@casparzhang.com>
 M:	Wanlong Gao <gaowanlong@cn.fujitsu.com>
+M:	Jan Stancek <jstancek@redhat.com>
+M:	Stanislav Kholmanskikh <stanislav.kholmanskikh@oracle.com>
+M:	Alexey Kodanev <alexey.kodanev@oracle.com>
 L:	ltp-list@lists.sourceforge.net (subscribers-only)
-W:	http://ltp.sourceforge.net/
+W:	http://linux-test-project.github.io/
 T:	git git://github.com/linux-test-project/ltp.git
-T:	git git://ltp.git.sourceforge.net/gitroot/ltp/ltp-dev
 S:	Maintained
 
 M32R ARCHITECTURE
@@ -6404,6 +6417,7 @@ F:	drivers/usb/*/*omap*
 F:	arch/arm/*omap*/usb*
 
 OMAP GPIO DRIVER
+M:	Javier Martinez Canillas <javier@dowhile0.org>
 M:	Santosh Shilimkar <santosh.shilimkar@ti.com>
 M:	Kevin Hilman <khilman@deeprootsystems.com>
 L:	linux-omap@vger.kernel.org
@@ -6500,10 +6514,10 @@ T:	git git://openrisc.net/~jonas/linux
 F:	arch/openrisc/
 
 OPENVSWITCH
-M:	Jesse Gross <jesse@nicira.com>
+M:	Pravin Shelar <pshelar@nicira.com>
 L:	dev@openvswitch.org
 W:	http://openvswitch.org
-T:	git git://git.kernel.org/pub/scm/linux/kernel/git/jesse/openvswitch.git
+T:	git git://git.kernel.org/pub/scm/linux/kernel/git/pshelar/openvswitch.git
 S:	Maintained
 F:	net/openvswitch/
 
@@ -6693,6 +6707,7 @@ F:	Documentation/PCI/
 F:	drivers/pci/
 F:	include/linux/pci*
 F:	arch/x86/pci/
+F:	arch/x86/kernel/quirks.c
 
 PCI DRIVER FOR IMX6
 M:	Richard Zhu <r65037@freescale.com>
@@ -6740,6 +6755,14 @@ L:	linux-pci@vger.kernel.org
 S:	Maintained
 F:	drivers/pci/host/*designware*
 
+PCI DRIVER FOR GENERIC OF HOSTS
+M:	Will Deacon <will.deacon@arm.com>
+L:	linux-pci@vger.kernel.org
+L:	linux-arm-kernel@lists.infradead.org (moderated for non-subscribers)
+S:	Maintained
+F:	Documentation/devicetree/bindings/pci/host-generic-pci.txt
+F:	drivers/pci/host/pci-host-generic.c
+
 PCMCIA SUBSYSTEM
 P:	Linux PCMCIA Team
 L:	linux-pcmcia@lists.infradead.org
@@ -7277,7 +7300,6 @@ F:	drivers/video/aty/aty128fb.c
 RALINK RT2X00 WIRELESS LAN DRIVER
 P:	rt2x00 project
 M:	Ivo van Doorn <IvDoorn@gmail.com>
-M:	Gertjan van Wingerde <gwingerde@gmail.com>
 M:	Helmut Schaa <helmut.schaa@googlemail.com>
 L:	linux-wireless@vger.kernel.org
 L:	users@rt2x00.serialmonkey.com (moderated for non-subscribers)
@@ -7293,7 +7315,7 @@ F:	Documentation/blockdev/ramdisk.txt
 F:	drivers/block/brd.c
 
 RANDOM NUMBER DRIVER
-M:	Theodore Ts'o" <tytso@mit.edu>
+M:	"Theodore Ts'o" <tytso@mit.edu>
 S:	Maintained
 F:	drivers/char/random.c
 
@@ -7392,6 +7414,14 @@ F:	drivers/rpmsg/
 F:	Documentation/rpmsg.txt
 F:	include/linux/rpmsg.h
 
+RESET CONTROLLER FRAMEWORK
+M:	Philipp Zabel <p.zabel@pengutronix.de>
+S:	Maintained
+F:	drivers/reset/
+F:	Documentation/devicetree/bindings/reset/
+F:	include/linux/reset.h
+F:	include/linux/reset-controller.h
+
 RFKILL
 M:	Johannes Berg <johannes@sipsolutions.net>
 L:	linux-wireless@vger.kernel.org
@@ -7674,7 +7704,6 @@ F:	drivers/clk/samsung/
 SAMSUNG SXGBE DRIVERS
 M:	Byungho An <bh74.an@samsung.com>
 M:	Girish K S <ks.giri@samsung.com>
-M:	Siva Reddy Kallam <siva.kallam@samsung.com>
 M:	Vipul Pandya <vipul.pandya@samsung.com>
 S:	Supported
 L:	netdev@vger.kernel.org
@@ -8315,7 +8344,7 @@ F:	include/linux/compiler.h
 
 SPEAR PLATFORM SUPPORT
 M:	Viresh Kumar <viresh.linux@gmail.com>
-M:	Shiraz Hashim <shiraz.hashim@st.com>
+M:	Shiraz Hashim <shiraz.linux.kernel@gmail.com>
 L:	spear-devel@list.st.com
 L:	linux-arm-kernel@lists.infradead.org (moderated for non-subscribers)
 W:	http://www.st.com/spear
@@ -8464,7 +8493,7 @@ S:	Maintained
 F:	drivers/staging/olpc_dcon/
 
 STAGING - OZMO DEVICES USB OVER WIFI DRIVER
-M:	Rupesh Gujare <rupesh.gujare@atmel.com>
+M:	Shigekatsu Tateno <shigekatsu.tateno@atmel.com>
 S:	Maintained
 F:	drivers/staging/ozwpan/
 
@@ -8479,6 +8508,13 @@ M:	Florian Schilhabel <florian.c.schilhabel@googlemail.com>.
 S:	Odd Fixes
 F:	drivers/staging/rtl8712/
 
+STAGING - REALTEK RTL8723U WIRELESS DRIVER
+M:	Larry Finger <Larry.Finger@lwfinger.net>
+M:	Jes Sorensen <Jes.Sorensen@redhat.com>
+L:	linux-wireless@vger.kernel.org
+S:	Maintained
+F:	drivers/staging/rtl8723au/
+
 STAGING - SILICON MOTION SM7XX FRAME BUFFER DRIVER
 M:	Teddy Wang <teddy.wang@siliconmotion.com.cn>
 S:	Odd Fixes
@@ -9098,6 +9134,9 @@ F:	arch/um/os-Linux/drivers/
 
 TURBOCHANNEL SUBSYSTEM
 M:	"Maciej W. Rozycki" <macro@linux-mips.org>
+M:	Ralf Baechle <ralf@linux-mips.org>
+L:	linux-mips@linux-mips.org
+Q:	http://patchwork.linux-mips.org/project/linux-mips/list/
 S:	Maintained
 F:	drivers/tc/
 F:	include/linux/tc.h
@@ -9951,7 +9990,7 @@ F:	drivers/net/hamradio/*scc.c
 F:	drivers/net/hamradio/z8530.h
 
 ZBUD COMPRESSED PAGE ALLOCATOR
-M:	Seth Jennings <sjenning@linux.vnet.ibm.com>
+M:	Seth Jennings <sjennings@variantweb.net>
 L:	linux-mm@kvack.org
 S:	Maintained
 F:	mm/zbud.c
@@ -9996,7 +10035,7 @@ F:	mm/zsmalloc.c
 F:	include/linux/zsmalloc.h
 
 ZSWAP COMPRESSED SWAP CACHING
-M:	Seth Jennings <sjenning@linux.vnet.ibm.com>
+M:	Seth Jennings <sjennings@variantweb.net>
 L:	linux-mm@kvack.org
 S:	Maintained
 F:	mm/zswap.c
diff --git a/Makefile b/Makefile
index 60ccbfe750a..cdaa5b6a1c4 100644
--- a/Makefile
+++ b/Makefile
@@ -1,7 +1,7 @@
 VERSION = 3
 PATCHLEVEL = 15
 SUBLEVEL = 0
-EXTRAVERSION = -rc1
+EXTRAVERSION = -rc8
 NAME = Shuffling Zombie Juror
 
 # *DOCUMENTATION*
diff --git a/arch/alpha/include/asm/pci.h b/arch/alpha/include/asm/pci.h
index d01afb78919..f7f680f7457 100644
--- a/arch/alpha/include/asm/pci.h
+++ b/arch/alpha/include/asm/pci.h
@@ -59,11 +59,6 @@ struct pci_controller {
 
 extern void pcibios_set_master(struct pci_dev *dev);
 
-extern inline void pcibios_penalize_isa_irq(int irq, int active)
-{
-	/* We don't do dynamic PCI IRQ allocation */
-}
-
 /* IOMMU controls.  */
 
 /* The PCI address space does not equal the physical memory address space.
diff --git a/arch/arc/kernel/entry.S b/arch/arc/kernel/entry.S
index 819dd5f7eb0..29b82adbf0b 100644
--- a/arch/arc/kernel/entry.S
+++ b/arch/arc/kernel/entry.S
@@ -614,11 +614,13 @@ resume_user_mode_begin:
 
 resume_kernel_mode:
 
-#ifdef CONFIG_PREEMPT
-
-	; This is a must for preempt_schedule_irq()
+	; Disable Interrupts from this point on
+	; CONFIG_PREEMPT: This is a must for preempt_schedule_irq()
+	; !CONFIG_PREEMPT: To ensure restore_regs is intr safe
 	IRQ_DISABLE	r9
 
+#ifdef CONFIG_PREEMPT
+
 	; Can't preempt if preemption disabled
 	GET_CURR_THR_INFO_FROM_SP   r10
 	ld  r8, [r10, THREAD_INFO_PREEMPT_COUNT]
diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
index ab438cb5af5..071dce78959 100644
--- a/arch/arm/Kconfig
+++ b/arch/arm/Kconfig
@@ -30,9 +30,9 @@ config ARM
 	select HAVE_ARCH_SECCOMP_FILTER if (AEABI && !OABI_COMPAT)
 	select HAVE_ARCH_TRACEHOOK
 	select HAVE_BPF_JIT
+	select HAVE_CC_STACKPROTECTOR
 	select HAVE_CONTEXT_TRACKING
 	select HAVE_C_RECORDMCOUNT
-	select HAVE_CC_STACKPROTECTOR
 	select HAVE_DEBUG_KMEMLEAK
 	select HAVE_DMA_API_DEBUG
 	select HAVE_DMA_ATTRS
@@ -311,8 +311,10 @@ config ARCH_MULTIPLATFORM
 	select ARM_HAS_SG_CHAIN
 	select ARM_PATCH_PHYS_VIRT
 	select AUTO_ZRELADDR
+	select CLKSRC_OF
 	select COMMON_CLK
 	select GENERIC_CLOCKEVENTS
+	select MIGHT_HAVE_PCI
 	select MULTI_IRQ_HANDLER
 	select SPARSE_IRQ
 	select USE_OF
@@ -375,7 +377,6 @@ config ARCH_AT91
 	select ARCH_REQUIRE_GPIOLIB
 	select CLKDEV_LOOKUP
 	select IRQ_DOMAIN
-	select NEED_MACH_GPIO_H
 	select NEED_MACH_IO_H if PCCARD
 	select PINCTRL
 	select PINCTRL_AT91 if USE_OF
@@ -422,8 +423,8 @@ config ARCH_EFM32
 	bool "Energy Micro efm32"
 	depends on !MMU
 	select ARCH_REQUIRE_GPIOLIB
-	select AUTO_ZRELADDR
 	select ARM_NVIC
+	select AUTO_ZRELADDR
 	select CLKSRC_OF
 	select COMMON_CLK
 	select CPU_V7M
@@ -511,8 +512,8 @@ config ARCH_IXP4XX
 	bool "IXP4xx-based"
 	depends on MMU
 	select ARCH_HAS_DMA_SET_COHERENT_MASK
-	select ARCH_SUPPORTS_BIG_ENDIAN
 	select ARCH_REQUIRE_GPIOLIB
+	select ARCH_SUPPORTS_BIG_ENDIAN
 	select CLKSRC_MMIO
 	select CPU_XSCALE
 	select DMABOUNCE if PCI
@@ -754,7 +755,7 @@ config ARCH_S3C64XX
 	select ATAGS
 	select CLKDEV_LOOKUP
 	select CLKSRC_SAMSUNG_PWM
-	select COMMON_CLK
+	select COMMON_CLK_SAMSUNG
 	select CPU_V6K
 	select GENERIC_CLOCKEVENTS
 	select GPIO_SAMSUNG
@@ -828,25 +829,6 @@ config ARCH_S5PV210
 	help
 	  Samsung S5PV210/S5PC110 series based systems
 
-config ARCH_EXYNOS
-	bool "Samsung EXYNOS"
-	select ARCH_HAS_CPUFREQ
-	select ARCH_HAS_HOLES_MEMORYMODEL
-	select ARCH_REQUIRE_GPIOLIB
-	select ARCH_SPARSEMEM_ENABLE
-	select ARM_GIC
-	select COMMON_CLK
-	select CPU_V7
-	select GENERIC_CLOCKEVENTS
-	select HAVE_S3C2410_I2C if I2C
-	select HAVE_S3C2410_WATCHDOG if WATCHDOG
-	select HAVE_S3C_RTC if RTC_CLASS
-	select NEED_MACH_MEMORY_H
-	select SPARSE_IRQ
-	select USE_OF
-	help
-	  Support for SAMSUNG's EXYNOS SoCs (EXYNOS4/5)
-
 config ARCH_DAVINCI
 	bool "TI DaVinci"
 	select ARCH_HAS_HOLES_MEMORYMODEL
@@ -950,6 +932,8 @@ source "arch/arm/mach-mvebu/Kconfig"
 
 source "arch/arm/mach-at91/Kconfig"
 
+source "arch/arm/mach-axxia/Kconfig"
+
 source "arch/arm/mach-bcm/Kconfig"
 
 source "arch/arm/mach-berlin/Kconfig"
@@ -1110,9 +1094,9 @@ config ARM_NR_BANKS
 	default 8
 
 config IWMMXT
-	bool "Enable iWMMXt support" if !CPU_PJ4
-	depends on CPU_XSCALE || CPU_XSC3 || CPU_MOHAWK || CPU_PJ4
-	default y if PXA27x || PXA3xx || ARCH_MMP || CPU_PJ4
+	bool "Enable iWMMXt support"
+	depends on CPU_XSCALE || CPU_XSC3 || CPU_MOHAWK || CPU_PJ4 || CPU_PJ4B
+	default y if PXA27x || PXA3xx || ARCH_MMP || CPU_PJ4 || CPU_PJ4B
 	help
 	  Enable support for iWMMXt context switching at run time if
 	  running on a CPU that supports it.
@@ -1575,8 +1559,8 @@ config BIG_LITTLE
 config BL_SWITCHER
 	bool "big.LITTLE switcher support"
 	depends on BIG_LITTLE && MCPM && HOTPLUG_CPU
-	select CPU_PM
 	select ARM_CPU_SUSPEND
+	select CPU_PM
 	help
 	  The big.LITTLE "switcher" provides the core functionality to
 	  transparently handle transition between a cluster of A15's
@@ -1645,9 +1629,9 @@ config ARCH_NR_GPIO
 	int
 	default 1024 if ARCH_SHMOBILE || ARCH_TEGRA
 	default 512 if ARCH_EXYNOS || ARCH_KEYSTONE || SOC_OMAP5 || SOC_DRA7XX || ARCH_S3C24XX || ARCH_S3C64XX
+	default 416 if ARCH_SUNXI
 	default 392 if ARCH_U8500
 	default 352 if ARCH_VT8500
-	default 288 if ARCH_SUNXI
 	default 264 if MACH_H4700
 	default 0
 	help
@@ -1920,9 +1904,9 @@ config XEN
 	depends on CPU_V7 && !CPU_V6
 	depends on !GENERIC_ATOMIC64
 	depends on MMU
+	select ARCH_DMA_ADDR_T_64BIT
 	select ARM_PSCI
 	select SWIOTLB_XEN
-	select ARCH_DMA_ADDR_T_64BIT
 	help
 	  Say Y if you want to run Linux in a Virtual Machine on Xen on ARM.
 
diff --git a/arch/arm/Kconfig.debug b/arch/arm/Kconfig.debug
index 4a2fc0bf6fc..8f90595069a 100644
--- a/arch/arm/Kconfig.debug
+++ b/arch/arm/Kconfig.debug
@@ -317,6 +317,13 @@ choice
 		  Say Y here if you want kernel low-level debugging support
 		  on i.MX6SL.
 
+	config DEBUG_IMX6SX_UART
+		bool "i.MX6SX Debug UART"
+		depends on SOC_IMX6SX
+		help
+		  Say Y here if you want kernel low-level debugging support
+		  on i.MX6SX.
+
 	config DEBUG_KEYSTONE_UART0
 		bool "Kernel low-level debugging on KEYSTONE2 using UART0"
 		depends on ARCH_KEYSTONE
@@ -349,56 +356,40 @@ choice
 		  Say Y here if you want kernel low-level debugging support
 		  on MMP UART3.
 
-	config DEBUG_MSM_UART1
-		bool "Kernel low-level debugging messages via MSM UART1"
-		depends on ARCH_MSM7X00A || ARCH_MSM7X30 || ARCH_QSD8X50
-		select DEBUG_MSM_UART
+	config DEBUG_MSM_UART
+		bool "Kernel low-level debugging messages via MSM UART"
+		depends on ARCH_MSM
 		help
 		  Say Y here if you want the debug print routines to direct
-		  their output to the first serial port on MSM devices.
+		  their output to the serial port on MSM devices.
 
-	config DEBUG_MSM_UART2
-		bool "Kernel low-level debugging messages via MSM UART2"
-		depends on ARCH_MSM7X00A || ARCH_MSM7X30 || ARCH_QSD8X50
-		select DEBUG_MSM_UART
-		help
-		  Say Y here if you want the debug print routines to direct
-		  their output to the second serial port on MSM devices.
+		  ARCH                DEBUG_UART_PHYS   DEBUG_UART_BASE   #
+		  MSM7X00A, QSD8X50   0xa9a00000        0xe1000000        UART1
+		  MSM7X00A, QSD8X50   0xa9b00000        0xe1000000        UART2
+		  MSM7X00A, QSD8X50   0xa9c00000        0xe1000000        UART3
 
-	config DEBUG_MSM_UART3
-		bool "Kernel low-level debugging messages via MSM UART3"
-		depends on ARCH_MSM7X00A || ARCH_MSM7X30 || ARCH_QSD8X50
-		select DEBUG_MSM_UART
-		help
-		  Say Y here if you want the debug print routines to direct
-		  their output to the third serial port on MSM devices.
+		  MSM7X30             0xaca00000        0xe1000000        UART1
+		  MSM7X30             0xacb00000        0xe1000000        UART2
+		  MSM7X30             0xacc00000        0xe1000000        UART3
 
-	config DEBUG_MSM8660_UART
-		bool "Kernel low-level debugging messages via MSM 8660 UART"
-		depends on ARCH_MSM8X60
-		select MSM_HAS_DEBUG_UART_HS
-		select DEBUG_MSM_UART
-		help
-		  Say Y here if you want the debug print routines to direct
-		  their output to the serial port on MSM 8660 devices.
+		  Please adjust DEBUG_UART_PHYS and DEBUG_UART_BASE configuration
+		  options based on your needs.
 
-	config DEBUG_MSM8960_UART
-		bool "Kernel low-level debugging messages via MSM 8960 UART"
-		depends on ARCH_MSM8960
-		select MSM_HAS_DEBUG_UART_HS
-		select DEBUG_MSM_UART
+	config DEBUG_QCOM_UARTDM
+		bool "Kernel low-level debugging messages via QCOM UARTDM"
+		depends on ARCH_QCOM
 		help
 		  Say Y here if you want the debug print routines to direct
-		  their output to the serial port on MSM 8960 devices.
+		  their output to the serial port on Qualcomm devices.
 
-	config DEBUG_MSM8974_UART
-		bool "Kernel low-level debugging messages via MSM 8974 UART"
-		depends on ARCH_MSM8974
-		select MSM_HAS_DEBUG_UART_HS
-		select DEBUG_MSM_UART
-		help
-		  Say Y here if you want the debug print routines to direct
-		  their output to the serial port on MSM 8974 devices.
+		  ARCH      DEBUG_UART_PHYS   DEBUG_UART_BASE
+		  APQ8084   0xf995e000        0xfa75e000
+		  MSM8X60   0x19c40000        0xf0040000
+		  MSM8960   0x16440000        0xf0040000
+		  MSM8974   0xf991e000        0xfa71e000
+
+		  Please adjust DEBUG_UART_PHYS and DEBUG_UART_BASE configuration
+		  options based on your needs.
 
 	config DEBUG_MVEBU_UART
 		bool "Kernel low-level debugging messages via MVEBU UART (old bootloaders)"
@@ -625,6 +616,7 @@ choice
 	config DEBUG_S3C_UART0
 		depends on PLAT_SAMSUNG
 		select DEBUG_EXYNOS_UART if ARCH_EXYNOS
+		select DEBUG_S3C24XX_UART if ARCH_S3C24XX
 		bool "Use S3C UART 0 for low-level debug"
 		help
 		  Say Y here if you want the debug print routines to direct
@@ -637,6 +629,7 @@ choice
 	config DEBUG_S3C_UART1
 		depends on PLAT_SAMSUNG
 		select DEBUG_EXYNOS_UART if ARCH_EXYNOS
+		select DEBUG_S3C24XX_UART if ARCH_S3C24XX
 		bool "Use S3C UART 1 for low-level debug"
 		help
 		  Say Y here if you want the debug print routines to direct
@@ -649,6 +642,7 @@ choice
 	config DEBUG_S3C_UART2
 		depends on PLAT_SAMSUNG
 		select DEBUG_EXYNOS_UART if ARCH_EXYNOS
+		select DEBUG_S3C24XX_UART if ARCH_S3C24XX
 		bool "Use S3C UART 2 for low-level debug"
 		help
 		  Say Y here if you want the debug print routines to direct
@@ -670,6 +664,33 @@ choice
 		  The uncompressor code port configuration is now handled
 		  by CONFIG_S3C_LOWLEVEL_UART_PORT.
 
+	config DEBUG_S3C2410_UART0
+		depends on ARCH_S3C24XX
+		select DEBUG_S3C2410_UART
+		bool "Use S3C2410/S3C2412 UART 0 for low-level debug"
+		help
+		  Say Y here if you want the debug print routines to direct
+		  their output to UART 0. The port must have been initialised
+		  by the boot-loader before use.
+
+	config DEBUG_S3C2410_UART1
+		depends on ARCH_S3C24XX
+		select DEBUG_S3C2410_UART
+		bool "Use S3C2410/S3C2412 UART 1 for low-level debug"
+		help
+		  Say Y here if you want the debug print routines to direct
+		  their output to UART 1. The port must have been initialised
+		  by the boot-loader before use.
+
+	config DEBUG_S3C2410_UART2
+		depends on ARCH_S3C24XX
+		select DEBUG_S3C2410_UART
+		bool "Use S3C2410/S3C2412 UART 2 for low-level debug"
+		help
+		  Say Y here if you want the debug print routines to direct
+		  their output to UART 2. The port must have been initialised
+		  by the boot-loader before use.
+
 	config DEBUG_SOCFPGA_UART
 		depends on ARCH_SOCFPGA
 		bool "Use SOCFPGA UART for low-level debug"
@@ -921,6 +942,13 @@ endchoice
 config DEBUG_EXYNOS_UART
 	bool
 
+config DEBUG_S3C2410_UART
+	bool
+	select DEBUG_S3C24XX_UART
+
+config DEBUG_S3C24XX_UART
+	bool
+
 config DEBUG_OMAP2PLUS_UART
 	bool
 	depends on ARCH_OMAP2PLUS
@@ -935,13 +963,23 @@ config DEBUG_IMX_UART_PORT
 						DEBUG_IMX51_UART || \
 						DEBUG_IMX53_UART || \
 						DEBUG_IMX6Q_UART || \
-						DEBUG_IMX6SL_UART
+						DEBUG_IMX6SL_UART || \
+						DEBUG_IMX6SX_UART
 	default 1
 	depends on ARCH_MXC
 	help
 	  Choose UART port on which kernel low-level debug messages
 	  should be output.
 
+config DEBUG_VF_UART_PORT
+	int "Vybrid Debug UART Port Selection" if DEBUG_VF_UART
+	default 1
+	range 0 3
+	depends on SOC_VF610
+	help
+	  Choose UART port on which kernel low-level debug messages
+	  should be output.
+
 config DEBUG_TEGRA_UART
 	bool
 	depends on ARCH_TEGRA
@@ -950,10 +988,6 @@ config DEBUG_STI_UART
 	bool
 	depends on ARCH_STI
 
-config DEBUG_MSM_UART
-	bool
-	depends on ARCH_MSM || ARCH_QCOM
-
 config DEBUG_LL_INCLUDE
 	string
 	default "debug/8250.S" if DEBUG_LL_UART_8250 || DEBUG_UART_8250
@@ -970,9 +1004,11 @@ config DEBUG_LL_INCLUDE
 				 DEBUG_IMX51_UART || \
 				 DEBUG_IMX53_UART ||\
 				 DEBUG_IMX6Q_UART || \
-				 DEBUG_IMX6SL_UART
-	default "debug/msm.S" if DEBUG_MSM_UART
+				 DEBUG_IMX6SL_UART || \
+				 DEBUG_IMX6SX_UART
+	default "debug/msm.S" if DEBUG_MSM_UART || DEBUG_QCOM_UARTDM
 	default "debug/omap2plus.S" if DEBUG_OMAP2PLUS_UART
+	default "debug/s3c24xx.S" if DEBUG_S3C24XX_UART
 	default "debug/sirf.S" if DEBUG_SIRFPRIMA2_UART1 || DEBUG_SIRFMARCO_UART1
 	default "debug/sti.S" if DEBUG_STI_UART
 	default "debug/tegra.S" if DEBUG_TEGRA_UART
@@ -1029,12 +1065,19 @@ config DEBUG_UART_PHYS
 	default 0x40090000 if ARCH_LPC32XX
 	default 0x40100000 if DEBUG_PXA_UART1
 	default 0x42000000 if ARCH_GEMINI
+	default 0x50000000 if DEBUG_S3C24XX_UART && (DEBUG_S3C_UART0 || \
+				DEBUG_S3C2410_UART0)
+	default 0x50004000 if DEBUG_S3C24XX_UART && (DEBUG_S3C_UART1 || \
+				DEBUG_S3C2410_UART1)
+	default 0x50008000 if DEBUG_S3C24XX_UART && (DEBUG_S3C_UART2 || \
+				DEBUG_S3C2410_UART2)
 	default 0x7c0003f8 if FOOTBRIDGE
-	default 0x80230000 if DEBUG_PICOXCELL_UART
 	default 0x80070000 if DEBUG_IMX23_UART
 	default 0x80074000 if DEBUG_IMX28_UART
+	default 0x80230000 if DEBUG_PICOXCELL_UART
 	default 0x808c0000 if ARCH_EP93XX
 	default 0x90020000 if DEBUG_NSPIRE_CLASSIC_UART || DEBUG_NSPIRE_CX_UART
+	default 0xa9a00000 if DEBUG_MSM_UART
 	default 0xb0090000 if DEBUG_VEXPRESS_UART0_CRX
 	default 0xc0013000 if DEBUG_U300_UART
 	default 0xc8000000 if ARCH_IXP4XX && !CPU_BIG_ENDIAN
@@ -1050,6 +1093,7 @@ config DEBUG_UART_PHYS
 				ARCH_ORION5X
 	default 0xf7fc9000 if DEBUG_BERLIN_UART
 	default 0xf8b00000 if DEBUG_HI3716_UART
+	default 0xf991e000 if DEBUG_QCOM_UARTDM
 	default 0xfcb00000 if DEBUG_HI3620_UART
 	default 0xfe800000 if ARCH_IOP32X
 	default 0xffc02000 if DEBUG_SOCFPGA_UART
@@ -1058,11 +1102,13 @@ config DEBUG_UART_PHYS
 	default 0xfffff700 if ARCH_IOP33X
 	depends on DEBUG_LL_UART_8250 || DEBUG_LL_UART_PL01X || \
 		DEBUG_LL_UART_EFM32 || \
-		DEBUG_UART_8250 || DEBUG_UART_PL01X
+		DEBUG_UART_8250 || DEBUG_UART_PL01X || \
+		DEBUG_MSM_UART || DEBUG_QCOM_UARTDM || DEBUG_S3C24XX_UART
 
 config DEBUG_UART_VIRT
 	hex "Virtual base address of debug UART"
 	default 0xe0010fe0 if ARCH_RPC
+	default 0xe1000000 if DEBUG_MSM_UART
 	default 0xf0000be0 if ARCH_EBSA110
 	default 0xf0009000 if DEBUG_CNS3XXX
 	default 0xf01fb000 if DEBUG_NOMADIK_UART
@@ -1075,9 +1121,16 @@ config DEBUG_UART_VIRT
 	default 0xf2100000 if DEBUG_PXA_UART1
 	default 0xf4090000 if ARCH_LPC32XX
 	default 0xf4200000 if ARCH_GEMINI
+	default 0xf7000000 if DEBUG_S3C24XX_UART && (DEBUG_S3C_UART0 || \
+				DEBUG_S3C2410_UART0)
+	default 0xf7004000 if DEBUG_S3C24XX_UART && (DEBUG_S3C_UART1 || \
+				DEBUG_S3C2410_UART1)
+	default 0xf7008000 if DEBUG_S3C24XX_UART && (DEBUG_S3C_UART2 || \
+				DEBUG_S3C2410_UART2)
 	default 0xf7fc9000 if DEBUG_BERLIN_UART
 	default 0xf8009000 if DEBUG_VEXPRESS_UART0_CA9
 	default 0xf8090000 if DEBUG_VEXPRESS_UART0_RS1
+	default 0xfa71e000 if DEBUG_QCOM_UARTDM
 	default 0xfb009000 if DEBUG_REALVIEW_STD_PORT
 	default 0xfb10c000 if DEBUG_REALVIEW_PB1176_PORT
 	default 0xfd000000 if ARCH_SPEAR3XX || ARCH_SPEAR6XX
@@ -1096,27 +1149,28 @@ config DEBUG_UART_VIRT
 	default 0xfeb26000 if DEBUG_RK3X_UART1
 	default 0xfeb30c00 if DEBUG_KEYSTONE_UART0
 	default 0xfeb31000 if DEBUG_KEYSTONE_UART1
-	default 0xfec12000 if DEBUG_MVEBU_UART || DEBUG_MVEBU_UART_ALTERNATE
-	default 0xfed60000 if DEBUG_RK29_UART0
-	default 0xfed64000 if DEBUG_RK29_UART1 || DEBUG_RK3X_UART2
-	default 0xfed68000 if DEBUG_RK29_UART2 || DEBUG_RK3X_UART3
 	default 0xfec02000 if DEBUG_SOCFPGA_UART
+	default 0xfec12000 if DEBUG_MVEBU_UART || DEBUG_MVEBU_UART_ALTERNATE
 	default 0xfec20000 if DEBUG_DAVINCI_DMx_UART0
 	default 0xfed0c000 if DEBUG_DAVINCI_DA8XX_UART1
 	default 0xfed0d000 if DEBUG_DAVINCI_DA8XX_UART2
 	default 0xfed12000 if ARCH_KIRKWOOD
+	default 0xfed60000 if DEBUG_RK29_UART0
+	default 0xfed64000 if DEBUG_RK29_UART1 || DEBUG_RK3X_UART2
+	default 0xfed68000 if DEBUG_RK29_UART2 || DEBUG_RK3X_UART3
 	default 0xfedc0000 if ARCH_EP93XX
 	default 0xfee003f8 if FOOTBRIDGE
 	default 0xfee20000 if DEBUG_NSPIRE_CLASSIC_UART || DEBUG_NSPIRE_CX_UART
-	default 0xfef36000 if DEBUG_HIGHBANK_UART
 	default 0xfee82340 if ARCH_IOP13XX
 	default 0xfef00000 if ARCH_IXP4XX && !CPU_BIG_ENDIAN
 	default 0xfef00003 if ARCH_IXP4XX && CPU_BIG_ENDIAN
+	default 0xfef36000 if DEBUG_HIGHBANK_UART
 	default 0xfefff700 if ARCH_IOP33X
 	default 0xff003000 if DEBUG_U300_UART
 	default DEBUG_UART_PHYS if !MMU
 	depends on DEBUG_LL_UART_8250 || DEBUG_LL_UART_PL01X || \
-		DEBUG_UART_8250 || DEBUG_UART_PL01X
+		DEBUG_UART_8250 || DEBUG_UART_PL01X || \
+		DEBUG_MSM_UART || DEBUG_QCOM_UARTDM || DEBUG_S3C24XX_UART
 
 config DEBUG_UART_8250_SHIFT
 	int "Register offset shift for the 8250 debug UART"
diff --git a/arch/arm/Makefile b/arch/arm/Makefile
index 41c1931f015..6721fab1373 100644
--- a/arch/arm/Makefile
+++ b/arch/arm/Makefile
@@ -138,10 +138,12 @@ endif
 textofs-$(CONFIG_ARCH_MSM7X30) := 0x00208000
 textofs-$(CONFIG_ARCH_MSM8X60) := 0x00208000
 textofs-$(CONFIG_ARCH_MSM8960) := 0x00208000
+textofs-$(CONFIG_ARCH_AXXIA) := 0x00308000
 
 # Machine directory name.  This list is sorted alphanumerically
 # by CONFIG_* macro name.
 machine-$(CONFIG_ARCH_AT91)		+= at91
+machine-$(CONFIG_ARCH_AXXIA)		+= axxia
 machine-$(CONFIG_ARCH_BCM)		+= bcm
 machine-$(CONFIG_ARCH_BERLIN)		+= berlin
 machine-$(CONFIG_ARCH_CLPS711X)		+= clps711x
diff --git a/arch/arm/boot/compressed/head.S b/arch/arm/boot/compressed/head.S
index 066b03480b6..3a8b32df6b3 100644
--- a/arch/arm/boot/compressed/head.S
+++ b/arch/arm/boot/compressed/head.S
@@ -60,11 +60,6 @@
 		add	\rb, \rb, #0x00010000	@ Ser1
 #endif
 		.endm
-#elif defined(CONFIG_ARCH_S3C24XX)
-		.macro loadsp, rb, tmp
-		mov	\rb, #0x50000000
-		add	\rb, \rb, #0x4000 * CONFIG_S3C_LOWLEVEL_UART_PORT
-		.endm
 #else
 		.macro	loadsp,	rb, tmp
 		addruart \rb, \tmp
diff --git a/arch/arm/boot/dts/Makefile b/arch/arm/boot/dts/Makefile
index 35c146f31e4..5986ff63b90 100644
--- a/arch/arm/boot/dts/Makefile
+++ b/arch/arm/boot/dts/Makefile
@@ -50,14 +50,15 @@ dtb-$(CONFIG_ARCH_AT91)	+= sama5d35ek.dtb
 dtb-$(CONFIG_ARCH_AT91)	+= sama5d36ek.dtb
 
 dtb-$(CONFIG_ARCH_ATLAS6) += atlas6-evb.dtb
+dtb-$(CONFIG_ARCH_AXXIA) += axm5516-amarillo.dtb
 dtb-$(CONFIG_ARCH_BCM2835) += bcm2835-rpi-b.dtb
+dtb-$(CONFIG_ARCH_BCM_5301X) += bcm4708-netgear-r6250.dtb
 dtb-$(CONFIG_ARCH_BCM_MOBILE) += bcm28155-ap.dtb \
 	bcm21664-garnet.dtb
-dtb-$(CONFIG_ARCH_BCM2835) += bcm2835-rpi-b.dtb
-dtb-$(CONFIG_ARCH_BCM_5301X) += bcm4708-netgear-r6250.dtb
 dtb-$(CONFIG_ARCH_BERLIN) += \
 	berlin2-sony-nsz-gs7.dtb	\
-	berlin2cd-google-chromecast.dtb
+	berlin2cd-google-chromecast.dtb	\
+	berlin2q-marvell-dmp.dtb
 dtb-$(CONFIG_ARCH_DAVINCI) += da850-enbw-cmc.dtb \
 	da850-evm.dtb
 dtb-$(CONFIG_ARCH_EFM32) += efm32gg-dk3750.dtb
@@ -73,10 +74,14 @@ dtb-$(CONFIG_ARCH_EXYNOS) += exynos4210-origen.dtb \
 	exynos5250-arndale.dtb \
 	exynos5250-smdk5250.dtb \
 	exynos5250-snow.dtb \
+	exynos5260-xyref5260.dtb \
+	exynos5410-smdk5410.dtb \
 	exynos5420-arndale-octa.dtb \
+	exynos5420-peach-pit.dtb \
 	exynos5420-smdk5420.dtb \
 	exynos5440-sd5v1.dtb \
-	exynos5440-ssdk5440.dtb
+	exynos5440-ssdk5440.dtb \
+	exynos5800-peach-pi.dtb
 dtb-$(CONFIG_ARCH_HI3xxx) += hi3620-hi4511.dtb
 dtb-$(CONFIG_ARCH_HIGHBANK) += highbank.dtb \
 	ecx-2000.dtb
@@ -128,6 +133,9 @@ kirkwood := \
 	kirkwood-nsa310a.dtb \
 	kirkwood-openblocks_a6.dtb \
 	kirkwood-openblocks_a7.dtb \
+	kirkwood-openrd-base.dtb \
+	kirkwood-openrd-client.dtb \
+	kirkwood-openrd-ultimate.dtb \
 	kirkwood-rd88f6192.dtb \
 	kirkwood-rd88f6281-a0.dtb \
 	kirkwood-rd88f6281-a1.dtb \
@@ -158,10 +166,12 @@ dtb-$(CONFIG_ARCH_MXC) += \
 	imx27-phytec-phycard-s-rdk.dtb \
 	imx31-bug.dtb \
 	imx35-eukrea-mbimxsd35-baseboard.dtb \
+	imx35-pdk.dtb \
 	imx50-evk.dtb \
 	imx51-apf51.dtb \
 	imx51-apf51dev.dtb \
 	imx51-babbage.dtb \
+	imx51-digi-connectcore-jsk.dtb \
 	imx51-eukrea-mbimxsd51-baseboard.dtb \
 	imx53-ard.dtb \
 	imx53-m53evk.dtb \
@@ -180,6 +190,8 @@ dtb-$(CONFIG_ARCH_MXC) += \
 	imx6dl-gw54xx.dtb \
 	imx6dl-hummingboard.dtb \
 	imx6dl-nitrogen6x.dtb \
+	imx6dl-phytec-pbab01.dtb \
+	imx6dl-riotboard.dtb \
 	imx6dl-sabreauto.dtb \
 	imx6dl-sabrelite.dtb \
 	imx6dl-sabresd.dtb \
@@ -204,6 +216,7 @@ dtb-$(CONFIG_ARCH_MXC) += \
 	imx6q-udoo.dtb \
 	imx6q-wandboard.dtb \
 	imx6sl-evk.dtb \
+	vf610-colibri.dtb \
 	vf610-cosmic.dtb \
 	vf610-twr.dtb
 dtb-$(CONFIG_ARCH_MXS) += imx23-evk.dtb \
@@ -231,82 +244,84 @@ dtb-$(CONFIG_ARCH_NOMADIK) += ste-nomadik-s8815.dtb
 dtb-$(CONFIG_ARCH_NSPIRE) += nspire-cx.dtb \
 	nspire-tp.dtb \
 	nspire-clp.dtb
-dtb-$(CONFIG_ARCH_OMAP2PLUS) += omap2420-h4.dtb \
-	omap2430-sdp.dtb \
+dtb-$(CONFIG_ARCH_OMAP2) += omap2420-h4.dtb \
 	omap2420-n800.dtb \
 	omap2420-n810.dtb \
 	omap2420-n810-wimax.dtb \
+	omap2430-sdp.dtb
+dtb-$(CONFIG_ARCH_OMAP3) += am3517-craneboard.dtb \
+	am3517-evm.dtb \
+	am3517_mt_ventoux.dtb \
 	omap3430-sdp.dtb \
 	omap3-beagle.dtb \
+	omap3-beagle-xm.dtb \
+	omap3-beagle-xm-ab.dtb \
 	omap3-cm-t3517.dtb \
-	omap3-sbc-t3517.dtb \
 	omap3-cm-t3530.dtb \
-	omap3-sbc-t3530.dtb \
 	omap3-cm-t3730.dtb \
-	omap3-sbc-t3730.dtb \
 	omap3-devkit8000.dtb \
-	omap3-beagle-xm.dtb \
 	omap3-evm.dtb \
 	omap3-evm-37xx.dtb \
+	omap3-gta04.dtb \
+	omap3-igep0020.dtb \
+	omap3-igep0030.dtb \
 	omap3-ldp.dtb \
+	omap3-lilly-dbb056.dtb \
 	omap3-n900.dtb \
 	omap3-n9.dtb \
 	omap3-n950.dtb \
 	omap3-overo-alto35.dtb \
-	omap3-overo-storm-alto35.dtb \
 	omap3-overo-chestnut43.dtb \
-	omap3-overo-storm-chestnut43.dtb \
 	omap3-overo-gallop43.dtb \
-	omap3-overo-storm-gallop43.dtb \
 	omap3-overo-palo43.dtb \
+	omap3-overo-storm-alto35.dtb \
+	omap3-overo-storm-chestnut43.dtb \
+	omap3-overo-storm-gallop43.dtb \
 	omap3-overo-storm-palo43.dtb \
-	omap3-overo-summit.dtb \
 	omap3-overo-storm-summit.dtb \
-	omap3-overo-tobi.dtb \
 	omap3-overo-storm-tobi.dtb \
-	omap3-gta04.dtb \
-	omap3-igep0020.dtb \
-	omap3-igep0030.dtb \
-	omap3-lilly-dbb056.dtb \
-	omap3-zoom3.dtb \
-	omap4-duovero-parlor.dtb \
+	omap3-overo-summit.dtb \
+	omap3-overo-tobi.dtb \
+	omap3-sbc-t3517.dtb \
+	omap3-sbc-t3530.dtb \
+	omap3-sbc-t3730.dtb \
+	omap3-zoom3.dtb
+dtb-$(CONFIG_SOC_AM33XX) += am335x-base0033.dtb \
+	am335x-bone.dtb \
+	am335x-boneblack.dtb \
+	am335x-evm.dtb \
+	am335x-evmsk.dtb \
+	am335x-nano.dtb
+dtb-$(CONFIG_ARCH_OMAP4) += omap4-duovero-parlor.dtb \
 	omap4-panda.dtb \
 	omap4-panda-a4.dtb \
 	omap4-panda-es.dtb \
-	omap4-var-som.dtb \
 	omap4-sdp.dtb \
 	omap4-sdp-es23plus.dtb \
-	omap5-uevm.dtb \
-	am335x-evm.dtb \
-	am335x-evmsk.dtb \
-	am335x-bone.dtb \
-	am335x-boneblack.dtb \
-	am335x-nano.dtb \
-	am335x-base0033.dtb \
-	am3517-craneboard.dtb \
-	am3517-evm.dtb \
-	am3517_mt_ventoux.dtb \
-	am43x-epos-evm.dtb \
-	am437x-gp-evm.dtb \
-	dra7-evm.dtb
-dtb-$(CONFIG_ARCH_ORION5X) += orion5x-lacie-ethernet-disk-mini-v2.dtb
+	omap4-var-dvk-om44.dtb \
+	omap4-var-stk-om44.dtb
+dtb-$(CONFIG_SOC_AM43XX) += am43x-epos-evm.dtb \
+	am437x-gp-evm.dtb
+dtb-$(CONFIG_SOC_OMAP5) += omap5-cm-t54.dtb \
+	omap5-sbc-t54.dtb \
+	omap5-uevm.dtb
+dtb-$(CONFIG_SOC_DRA7XX) += dra7-evm.dtb \
+	dra72-evm.dtb
+dtb-$(CONFIG_ARCH_ORION5X) += orion5x-lacie-d2-network.dtb \
+	orion5x-lacie-ethernet-disk-mini-v2.dtb \
+	orion5x-maxtor-shared-storage-2.dtb \
+	orion5x-rd88f5182-nas.dtb
 dtb-$(CONFIG_ARCH_PRIMA2) += prima2-evb.dtb
-dtb-$(CONFIG_ARCH_QCOM) += qcom-msm8660-surf.dtb \
-	qcom-msm8960-cdp.dtb \
-	qcom-apq8074-dragonboard.dtb
-dtb-$(CONFIG_ARCH_U8500) += ste-snowball.dtb \
-	ste-hrefprev60-stuib.dtb \
-	ste-hrefprev60-tvk.dtb \
-	ste-hrefv60plus-stuib.dtb \
-	ste-hrefv60plus-tvk.dtb \
-	ste-ccu8540.dtb \
-	ste-ccu9540.dtb
+dtb-$(CONFIG_ARCH_QCOM) += \
+	qcom-apq8064-ifc6410.dtb \
+	qcom-apq8074-dragonboard.dtb \
+	qcom-apq8084-mtp.dtb \
+	qcom-msm8660-surf.dtb \
+	qcom-msm8960-cdp.dtb
 dtb-$(CONFIG_ARCH_S3C24XX) += s3c2416-smdk2416.dtb
 dtb-$(CONFIG_ARCH_S3C64XX) += s3c6410-mini6410.dtb \
 	s3c6410-smdk6410.dtb
-dtb-$(CONFIG_ARCH_SHMOBILE_LEGACY) += emev2-kzm9d.dtb \
-	r7s72100-genmai.dtb \
-	r7s72100-genmai-reference.dtb \
+dtb-$(CONFIG_ARCH_SHMOBILE_LEGACY) += r7s72100-genmai.dtb \
 	r8a7740-armadillo800eva.dtb \
 	r8a7778-bockw.dtb \
 	r8a7778-bockw-reference.dtb \
@@ -321,12 +336,14 @@ dtb-$(CONFIG_ARCH_SHMOBILE_LEGACY) += emev2-kzm9d.dtb \
 	r8a73a4-ape6evm-reference.dtb \
 	sh7372-mackerel.dtb
 dtb-$(CONFIG_ARCH_SHMOBILE_MULTI) += emev2-kzm9d.dtb \
-	r7s72100-genmai-reference.dtb \
+	r7s72100-genmai.dtb \
+	r8a7791-henninger.dtb \
 	r8a7791-koelsch.dtb \
 	r8a7790-lager.dtb
 dtb-$(CONFIG_ARCH_SOCFPGA) += socfpga_arria5_socdk.dtb \
 	socfpga_cyclone5_socdk.dtb \
 	socfpga_cyclone5_sockit.dtb \
+	socfpga_cyclone5_socrates.dtb \
 	socfpga_vt.dtb
 dtb-$(CONFIG_ARCH_SPEAR13XX) += spear1310-evb.dtb \
 	spear1340-evb.dtb
@@ -335,24 +352,33 @@ dtb-$(CONFIG_ARCH_SPEAR3XX)+= spear300-evb.dtb \
 	spear320-evb.dtb \
 	spear320-hmi.dtb
 dtb-$(CONFIG_ARCH_SPEAR6XX)+= spear600-evb.dtb
-dtb-$(CONFIG_ARCH_STI)+= stih415-b2000.dtb \
-	stih416-b2000.dtb \
+dtb-$(CONFIG_ARCH_STI)+= stih407-b2120.dtb \
+	stih415-b2000.dtb \
 	stih415-b2020.dtb \
-	stih416-b2020.dtb
-dtb-$(CONFIG_ARCH_SUNXI) += \
+	stih416-b2000.dtb \
+	stih416-b2020.dtb \
+	stih416-b2020-revE.dtb
+dtb-$(CONFIG_MACH_SUN4I) += \
 	sun4i-a10-a1000.dtb \
 	sun4i-a10-cubieboard.dtb \
 	sun4i-a10-mini-xplus.dtb \
 	sun4i-a10-hackberry.dtb \
 	sun4i-a10-inet97fv2.dtb \
 	sun4i-a10-olinuxino-lime.dtb \
-	sun4i-a10-pcduino.dtb \
+	sun4i-a10-pcduino.dtb
+dtb-$(CONFIG_MACH_SUN5I) += \
 	sun5i-a10s-olinuxino-micro.dtb \
+	sun5i-a10s-r7-tv-dongle.dtb \
 	sun5i-a13-olinuxino.dtb \
-	sun5i-a13-olinuxino-micro.dtb \
+	sun5i-a13-olinuxino-micro.dtb
+dtb-$(CONFIG_MACH_SUN6I) += \
+	sun6i-a31-app4-evb1.dtb \
 	sun6i-a31-colombus.dtb \
+	sun6i-a31-m9.dtb
+dtb-$(CONFIG_MACH_SUN7I) += \
 	sun7i-a20-cubieboard2.dtb \
 	sun7i-a20-cubietruck.dtb \
+	sun7i-a20-i12-tvbox.dtb \
 	sun7i-a20-olinuxino-micro.dtb
 dtb-$(CONFIG_ARCH_TEGRA) += tegra20-harmony.dtb \
 	tegra20-iris-512.dtb \
@@ -367,11 +393,22 @@ dtb-$(CONFIG_ARCH_TEGRA) += tegra20-harmony.dtb \
 	tegra30-beaver.dtb \
 	tegra30-cardhu-a02.dtb \
 	tegra30-cardhu-a04.dtb \
+	tegra30-colibri-eval-v3.dtb \
 	tegra114-dalmore.dtb \
+	tegra114-roth.dtb \
+	tegra114-tn7.dtb \
+	tegra124-jetson-tk1.dtb \
 	tegra124-venice2.dtb
+dtb-$(CONFIG_ARCH_U300) += ste-u300.dtb
+dtb-$(CONFIG_ARCH_U8500) += ste-snowball.dtb \
+	ste-hrefprev60-stuib.dtb \
+	ste-hrefprev60-tvk.dtb \
+	ste-hrefv60plus-stuib.dtb \
+	ste-hrefv60plus-tvk.dtb \
+	ste-ccu8540.dtb \
+	ste-ccu9540.dtb
 dtb-$(CONFIG_ARCH_VERSATILE) += versatile-ab.dtb \
 	versatile-pb.dtb
-dtb-$(CONFIG_ARCH_U300) += ste-u300.dtb
 dtb-$(CONFIG_ARCH_VEXPRESS) += vexpress-v2p-ca5s.dtb \
 	vexpress-v2p-ca9.dtb \
 	vexpress-v2p-ca15-tc1.dtb \
diff --git a/arch/arm/boot/dts/am335x-bone-common.dtsi b/arch/arm/boot/dts/am335x-bone-common.dtsi
index e3f27ec3171..bde1777b62b 100644
--- a/arch/arm/boot/dts/am335x-bone-common.dtsi
+++ b/arch/arm/boot/dts/am335x-bone-common.dtsi
@@ -182,31 +182,31 @@
 
 &usb {
 	status = "okay";
+};
 
-	control@44e10000 {
-		status = "okay";
-	};
+&usb_ctrl_mod {
+	status = "okay";
+};
 
-	usb-phy@47401300 {
-		status = "okay";
-	};
+&usb0_phy {
+	status = "okay";
+};
 
-	usb-phy@47401b00 {
-		status = "okay";
-	};
+&usb1_phy {
+	status = "okay";
+};
 
-	usb@47401000 {
-		status = "okay";
-	};
+&usb0 {
+	status = "okay";
+};
 
-	usb@47401800 {
-		status = "okay";
-		dr_mode = "host";
-	};
+&usb1 {
+	status = "okay";
+	dr_mode = "host";
+};
 
-	dma-controller@07402000  {
-		status = "okay";
-	};
+&cppi41dma  {
+	status = "okay";
 };
 
 &i2c0 {
@@ -280,13 +280,14 @@
 	pinctrl-names = "default", "sleep";
 	pinctrl-0 = <&cpsw_default>;
 	pinctrl-1 = <&cpsw_sleep>;
-
+	status = "okay";
 };
 
 &davinci_mdio {
 	pinctrl-names = "default", "sleep";
 	pinctrl-0 = <&davinci_mdio_default>;
 	pinctrl-1 = <&davinci_mdio_sleep>;
+	status = "okay";
 };
 
 &mmc1 {
diff --git a/arch/arm/boot/dts/am335x-boneblack.dts b/arch/arm/boot/dts/am335x-boneblack.dts
index 6b71ad95a5c..305975d3f53 100644
--- a/arch/arm/boot/dts/am335x-boneblack.dts
+++ b/arch/arm/boot/dts/am335x-boneblack.dts
@@ -26,7 +26,6 @@
 	pinctrl-0 = <&emmc_pins>;
 	bus-width = <8>;
 	status = "okay";
-	ti,vcc-aux-disable-is-sleep;
 };
 
 &am33xx_pinmux {
diff --git a/arch/arm/boot/dts/am335x-evm.dts b/arch/arm/boot/dts/am335x-evm.dts
index 28ae040e7c3..ecb267767cf 100644
--- a/arch/arm/boot/dts/am335x-evm.dts
+++ b/arch/arm/boot/dts/am335x-evm.dts
@@ -268,41 +268,41 @@
 
 	lcd_pins_s0: lcd_pins_s0 {
 		pinctrl-single,pins = <
-			0x20 0x01	/* gpmc_ad8.lcd_data16, OUTPUT | MODE1 */
-			0x24 0x01	/* gpmc_ad9.lcd_data17, OUTPUT | MODE1 */
-			0x28 0x01	/* gpmc_ad10.lcd_data18, OUTPUT | MODE1 */
-			0x2c 0x01	/* gpmc_ad11.lcd_data19, OUTPUT | MODE1 */
-			0x30 0x01	/* gpmc_ad12.lcd_data20, OUTPUT | MODE1 */
-			0x34 0x01	/* gpmc_ad13.lcd_data21, OUTPUT | MODE1 */
-			0x38 0x01	/* gpmc_ad14.lcd_data22, OUTPUT | MODE1 */
-			0x3c 0x01	/* gpmc_ad15.lcd_data23, OUTPUT | MODE1 */
-			0xa0 0x00	/* lcd_data0.lcd_data0, OUTPUT | MODE0 */
-			0xa4 0x00	/* lcd_data1.lcd_data1, OUTPUT | MODE0 */
-			0xa8 0x00	/* lcd_data2.lcd_data2, OUTPUT | MODE0 */
-			0xac 0x00	/* lcd_data3.lcd_data3, OUTPUT | MODE0 */
-			0xb0 0x00	/* lcd_data4.lcd_data4, OUTPUT | MODE0 */
-			0xb4 0x00	/* lcd_data5.lcd_data5, OUTPUT | MODE0 */
-			0xb8 0x00	/* lcd_data6.lcd_data6, OUTPUT | MODE0 */
-			0xbc 0x00	/* lcd_data7.lcd_data7, OUTPUT | MODE0 */
-			0xc0 0x00	/* lcd_data8.lcd_data8, OUTPUT | MODE0 */
-			0xc4 0x00	/* lcd_data9.lcd_data9, OUTPUT | MODE0 */
-			0xc8 0x00	/* lcd_data10.lcd_data10, OUTPUT | MODE0 */
-			0xcc 0x00	/* lcd_data11.lcd_data11, OUTPUT | MODE0 */
-			0xd0 0x00	/* lcd_data12.lcd_data12, OUTPUT | MODE0 */
-			0xd4 0x00	/* lcd_data13.lcd_data13, OUTPUT | MODE0 */
-			0xd8 0x00	/* lcd_data14.lcd_data14, OUTPUT | MODE0 */
-			0xdc 0x00	/* lcd_data15.lcd_data15, OUTPUT | MODE0 */
-			0xe0 0x00	/* lcd_vsync.lcd_vsync, OUTPUT | MODE0 */
-			0xe4 0x00	/* lcd_hsync.lcd_hsync, OUTPUT | MODE0 */
-			0xe8 0x00	/* lcd_pclk.lcd_pclk, OUTPUT | MODE0 */
-			0xec 0x00	/* lcd_ac_bias_en.lcd_ac_bias_en, OUTPUT | MODE0 */
+			0x20 (PIN_OUTPUT | MUX_MODE1)		/* gpmc_ad8.lcd_data23 */
+			0x24 (PIN_OUTPUT | MUX_MODE1)		/* gpmc_ad9.lcd_data22 */
+			0x28 (PIN_OUTPUT | MUX_MODE1)		/* gpmc_ad10.lcd_data21 */
+			0x2c (PIN_OUTPUT | MUX_MODE1)		/* gpmc_ad11.lcd_data20 */
+			0x30 (PIN_OUTPUT | MUX_MODE1)		/* gpmc_ad12.lcd_data19 */
+			0x34 (PIN_OUTPUT | MUX_MODE1)		/* gpmc_ad13.lcd_data18 */
+			0x38 (PIN_OUTPUT | MUX_MODE1)		/* gpmc_ad14.lcd_data17 */
+			0x3c (PIN_OUTPUT | MUX_MODE1)		/* gpmc_ad15.lcd_data16 */
+			0xa0 (PIN_OUTPUT | MUX_MODE0)		/* lcd_data0.lcd_data0 */
+			0xa4 (PIN_OUTPUT | MUX_MODE0)		/* lcd_data1.lcd_data1 */
+			0xa8 (PIN_OUTPUT | MUX_MODE0)		/* lcd_data2.lcd_data2 */
+			0xac (PIN_OUTPUT | MUX_MODE0)		/* lcd_data3.lcd_data3 */
+			0xb0 (PIN_OUTPUT | MUX_MODE0)		/* lcd_data4.lcd_data4 */
+			0xb4 (PIN_OUTPUT | MUX_MODE0)		/* lcd_data5.lcd_data5 */
+			0xb8 (PIN_OUTPUT | MUX_MODE0)		/* lcd_data6.lcd_data6 */
+			0xbc (PIN_OUTPUT | MUX_MODE0)		/* lcd_data7.lcd_data7 */
+			0xc0 (PIN_OUTPUT | MUX_MODE0)		/* lcd_data8.lcd_data8 */
+			0xc4 (PIN_OUTPUT | MUX_MODE0)		/* lcd_data9.lcd_data9 */
+			0xc8 (PIN_OUTPUT | MUX_MODE0)		/* lcd_data10.lcd_data10 */
+			0xcc (PIN_OUTPUT | MUX_MODE0)		/* lcd_data11.lcd_data11 */
+			0xd0 (PIN_OUTPUT | MUX_MODE0)		/* lcd_data12.lcd_data12 */
+			0xd4 (PIN_OUTPUT | MUX_MODE0)		/* lcd_data13.lcd_data13 */
+			0xd8 (PIN_OUTPUT | MUX_MODE0)		/* lcd_data14.lcd_data14 */
+			0xdc (PIN_OUTPUT | MUX_MODE0)		/* lcd_data15.lcd_data15 */
+			0xe0 (PIN_OUTPUT | MUX_MODE0)		/* lcd_vsync.lcd_vsync */
+			0xe4 (PIN_OUTPUT | MUX_MODE0)		/* lcd_hsync.lcd_hsync */
+			0xe8 (PIN_OUTPUT | MUX_MODE0)		/* lcd_pclk.lcd_pclk */
+			0xec (PIN_OUTPUT | MUX_MODE0)		/* lcd_ac_bias_en.lcd_ac_bias_en */
 		>;
 	};
 
 	am335x_evm_audio_pins: am335x_evm_audio_pins {
 		pinctrl-single,pins = <
-			0x10c (PIN_INPUT_PULLDOWN | MUX_MODE4) /* mii1_rx_dv.mcasp1_aclkx */
-			0x110 (PIN_INPUT_PULLDOWN | MUX_MODE4) /* mii1_txd3.mcasp1_fsx */
+			0x10c (PIN_INPUT_PULLDOWN | MUX_MODE4) /* mii1_crs.mcasp1_aclkx */
+			0x110 (PIN_INPUT_PULLDOWN | MUX_MODE4) /* mii1_rxerr.mcasp1_fsx */
 			0x108 (PIN_OUTPUT_PULLDOWN | MUX_MODE4) /* mii1_col.mcasp1_axr2 */
 			0x144 (PIN_INPUT_PULLDOWN | MUX_MODE4) /* rmii1_ref_clk.mcasp1_axr3 */
 		>;
@@ -330,31 +330,31 @@
 
 &usb {
 	status = "okay";
+};
 
-	control@44e10000 {
-		status = "okay";
-	};
+&usb_ctrl_mod {
+	status = "okay";
+};
 
-	usb-phy@47401300 {
-		status = "okay";
-	};
+&usb0_phy {
+	status = "okay";
+};
 
-	usb-phy@47401b00 {
-		status = "okay";
-	};
+&usb1_phy {
+	status = "okay";
+};
 
-	usb@47401000 {
-		status = "okay";
-	};
+&usb0 {
+	status = "okay";
+};
 
-	usb@47401800 {
-		status = "okay";
-		dr_mode = "host";
-	};
+&usb1 {
+	status = "okay";
+	dr_mode = "host";
+};
 
-	dma-controller@07402000  {
-		status = "okay";
-	};
+&cppi41dma  {
+	status = "okay";
 };
 
 &i2c1 {
@@ -614,12 +614,14 @@
 	pinctrl-names = "default", "sleep";
 	pinctrl-0 = <&cpsw_default>;
 	pinctrl-1 = <&cpsw_sleep>;
+	status = "okay";
 };
 
 &davinci_mdio {
 	pinctrl-names = "default", "sleep";
 	pinctrl-0 = <&davinci_mdio_default>;
 	pinctrl-1 = <&davinci_mdio_sleep>;
+	status = "okay";
 };
 
 &cpsw_emac0 {
diff --git a/arch/arm/boot/dts/am335x-evmsk.dts b/arch/arm/boot/dts/am335x-evmsk.dts
index ec08f6f677c..ab9a34ce524 100644
--- a/arch/arm/boot/dts/am335x-evmsk.dts
+++ b/arch/arm/boot/dts/am335x-evmsk.dts
@@ -57,6 +57,17 @@
 		enable-active-high;
 	};
 
+	vtt_fixed: fixedregulator@3 {
+		compatible = "regulator-fixed";
+		regulator-name = "vtt";
+		regulator-min-microvolt = <1500000>;
+		regulator-max-microvolt = <1500000>;
+		gpio = <&gpio0 7 GPIO_ACTIVE_HIGH>;
+		regulator-always-on;
+		regulator-boot-on;
+		enable-active-high;
+	};
+
 	leds {
 		pinctrl-names = "default";
 		pinctrl-0 = <&user_leds_s0>;
@@ -363,31 +374,31 @@
 
 &usb {
 	status = "okay";
+};
 
-	control@44e10000 {
-		status = "okay";
-	};
+&usb_ctrl_mod {
+	status = "okay";
+};
 
-	usb-phy@47401300 {
-		status = "okay";
-	};
+&usb0_phy {
+	status = "okay";
+};
 
-	usb-phy@47401b00 {
-		status = "okay";
-	};
+&usb1_phy {
+	status = "okay";
+};
 
-	usb@47401000 {
-		status = "okay";
-	};
+&usb0 {
+	status = "okay";
+};
 
-	usb@47401800 {
-		status = "okay";
-		dr_mode = "host";
-	};
+&usb1 {
+	status = "okay";
+	dr_mode = "host";
+};
 
-	dma-controller@07402000  {
-		status = "okay";
-	};
+&cppi41dma  {
+	status = "okay";
 };
 
 &epwmss2 {
@@ -484,12 +495,14 @@
 	pinctrl-0 = <&cpsw_default>;
 	pinctrl-1 = <&cpsw_sleep>;
 	dual_emac = <1>;
+	status = "okay";
 };
 
 &davinci_mdio {
 	pinctrl-names = "default", "sleep";
 	pinctrl-0 = <&davinci_mdio_default>;
 	pinctrl-1 = <&davinci_mdio_sleep>;
+	status = "okay";
 };
 
 &cpsw_emac0 {
diff --git a/arch/arm/boot/dts/am335x-igep0033.dtsi b/arch/arm/boot/dts/am335x-igep0033.dtsi
index 7063311a58d..8a0a72dc7dd 100644
--- a/arch/arm/boot/dts/am335x-igep0033.dtsi
+++ b/arch/arm/boot/dts/am335x-igep0033.dtsi
@@ -95,6 +95,14 @@
 	};
 };
 
+&mac {
+	status = "okay";
+};
+
+&davinci_mdio {
+	status = "okay";
+};
+
 &cpsw_emac0 {
 	phy_id = <&davinci_mdio>, <0>;
 };
@@ -118,7 +126,6 @@
 		reg = <0 0 0>; /* CS0, offset 0 */
 		nand-bus-width = <8>;
 		ti,nand-ecc-opt = "bch8";
-		gpmc,device-nand = "true";
 		gpmc,device-width = <1>;
 		gpmc,sync-clk-ps = <0>;
 		gpmc,cs-on-ns = <0>;
@@ -201,31 +208,31 @@
 
 &usb {
 	status = "okay";
+};
 
-	control@44e10000 {
-		status = "okay";
-	};
+&usb_ctrl_mod {
+	status = "okay";
+};
 
-	usb-phy@47401300 {
-		status = "okay";
-	};
+&usb0_phy {
+	status = "okay";
+};
 
-	usb-phy@47401b00 {
-		status = "okay";
-	};
+&usb1_phy {
+	status = "okay";
+};
 
-	usb@47401000 {
-		status = "okay";
-	};
+&usb0 {
+	status = "okay";
+};
 
-	usb@47401800 {
-		status = "okay";
-		dr_mode = "host";
-	};
+&usb1 {
+	status = "okay";
+	dr_mode = "host";
+};
 
-	dma-controller@07402000  {
-		status = "okay";
-	};
+&cppi41dma  {
+	status = "okay";
 };
 
 #include "tps65910.dtsi"
diff --git a/arch/arm/boot/dts/am335x-nano.dts b/arch/arm/boot/dts/am335x-nano.dts
index 9907b494b99..a3466455b17 100644
--- a/arch/arm/boot/dts/am335x-nano.dts
+++ b/arch/arm/boot/dts/am335x-nano.dts
@@ -344,6 +344,11 @@
 
 &mac {
 	dual_emac = <1>;
+	status = "okay";
+};
+
+&davinci_mdio {
+	status = "okay";
 };
 
 &cpsw_emac0 {
diff --git a/arch/arm/boot/dts/am33xx-clocks.dtsi b/arch/arm/boot/dts/am33xx-clocks.dtsi
index 9ccfe508dea..712edce7d6f 100644
--- a/arch/arm/boot/dts/am33xx-clocks.dtsi
+++ b/arch/arm/boot/dts/am33xx-clocks.dtsi
@@ -96,47 +96,29 @@
 		clock-div = <1>;
 	};
 
-	ehrpwm0_gate_tbclk: ehrpwm0_gate_tbclk {
+	ehrpwm0_tbclk: ehrpwm0_tbclk@44e10664 {
 		#clock-cells = <0>;
-		compatible = "ti,composite-no-wait-gate-clock";
+		compatible = "ti,gate-clock";
 		clocks = <&dpll_per_m2_ck>;
 		ti,bit-shift = <0>;
 		reg = <0x0664>;
 	};
 
-	ehrpwm0_tbclk: ehrpwm0_tbclk {
-		#clock-cells = <0>;
-		compatible = "ti,composite-clock";
-		clocks = <&ehrpwm0_gate_tbclk>;
-	};
-
-	ehrpwm1_gate_tbclk: ehrpwm1_gate_tbclk {
+	ehrpwm1_tbclk: ehrpwm1_tbclk@44e10664 {
 		#clock-cells = <0>;
-		compatible = "ti,composite-no-wait-gate-clock";
+		compatible = "ti,gate-clock";
 		clocks = <&dpll_per_m2_ck>;
 		ti,bit-shift = <1>;
 		reg = <0x0664>;
 	};
 
-	ehrpwm1_tbclk: ehrpwm1_tbclk {
-		#clock-cells = <0>;
-		compatible = "ti,composite-clock";
-		clocks = <&ehrpwm1_gate_tbclk>;
-	};
-
-	ehrpwm2_gate_tbclk: ehrpwm2_gate_tbclk {
+	ehrpwm2_tbclk: ehrpwm2_tbclk@44e10664 {
 		#clock-cells = <0>;
-		compatible = "ti,composite-no-wait-gate-clock";
+		compatible = "ti,gate-clock";
 		clocks = <&dpll_per_m2_ck>;
 		ti,bit-shift = <2>;
 		reg = <0x0664>;
 	};
-
-	ehrpwm2_tbclk: ehrpwm2_tbclk {
-		#clock-cells = <0>;
-		compatible = "ti,composite-clock";
-		clocks = <&ehrpwm2_gate_tbclk>;
-	};
 };
 &prcm_clocks {
 	clk_32768_ck: clk_32768_ck {
diff --git a/arch/arm/boot/dts/am33xx.dtsi b/arch/arm/boot/dts/am33xx.dtsi
index 9770e35f253..9f53e824b03 100644
--- a/arch/arm/boot/dts/am33xx.dtsi
+++ b/arch/arm/boot/dts/am33xx.dtsi
@@ -72,7 +72,7 @@
 	};
 
 	/*
-	 * The soc node represents the soc top level view. It is uses for IPs
+	 * The soc node represents the soc top level view. It is used for IPs
 	 * that are not memory mapped in the MPU view or for the MPU itself.
 	 */
 	soc {
@@ -94,8 +94,8 @@
 
 	/*
 	 * XXX: Use a flat representation of the AM33XX interconnect.
-	 * The real AM33XX interconnect network is quite complex.Since
-	 * that will not bring real advantage to represent that in DT
+	 * The real AM33XX interconnect network is quite complex. Since
+	 * it will not bring real advantage to represent that in DT
 	 * for the moment, just use a fake OCP bus entry to represent
 	 * the whole bus hierarchy.
 	 */
@@ -144,12 +144,9 @@
 			compatible = "ti,edma3";
 			ti,hwmods = "tpcc", "tptc0", "tptc1", "tptc2";
 			reg =	<0x49000000 0x10000>,
-				<0x44e10f90 0x10>;
+				<0x44e10f90 0x40>;
 			interrupts = <12 13 14>;
 			#dma-cells = <1>;
-			dma-channels = <64>;
-			ti,edma-regions = <4>;
-			ti,edma-slots = <256>;
 		};
 
 		gpio0: gpio@44e07000 {
@@ -688,6 +685,7 @@
 			 */
 			interrupts = <40 41 42 43>;
 			ranges;
+			status = "disabled";
 
 			davinci_mdio: mdio@4a101000 {
 				compatible = "ti,davinci_mdio";
@@ -696,6 +694,7 @@
 				ti,hwmods = "davinci_mdio";
 				bus_freq = <1000000>;
 				reg = <0x4a101000 0x100>;
+				status = "disabled";
 			};
 
 			cpsw_emac0: slave@4a100200 {
@@ -802,7 +801,7 @@
 			      <0x46000000 0x400000>;
 			reg-names = "mpu", "dat";
 			interrupts = <80>, <81>;
-			interrupts-names = "tx", "rx";
+			interrupt-names = "tx", "rx";
 			status = "disabled";
 			dmas = <&edma 8>,
 				<&edma 9>;
@@ -816,7 +815,7 @@
 			      <0x46400000 0x400000>;
 			reg-names = "mpu", "dat";
 			interrupts = <82>, <83>;
-			interrupts-names = "tx", "rx";
+			interrupt-names = "tx", "rx";
 			status = "disabled";
 			dmas = <&edma 10>,
 				<&edma 11>;
diff --git a/arch/arm/boot/dts/am3517.dtsi b/arch/arm/boot/dts/am3517.dtsi
index 788391f9168..5a452fdd7c5 100644
--- a/arch/arm/boot/dts/am3517.dtsi
+++ b/arch/arm/boot/dts/am3517.dtsi
@@ -62,5 +62,21 @@
 	};
 };
 
+&iva {
+	status = "disabled";
+};
+
+&mailbox {
+	status = "disabled";
+};
+
+&mmu_isp {
+	status = "disabled";
+};
+
+&smartreflex_mpu_iva {
+	status = "disabled";
+};
+
 /include/ "am35xx-clocks.dtsi"
 /include/ "omap36xx-am35xx-omap3430es2plus-clocks.dtsi"
diff --git a/arch/arm/boot/dts/am4372.dtsi b/arch/arm/boot/dts/am4372.dtsi
index 36d523a2683..794c73e5c4e 100644
--- a/arch/arm/boot/dts/am4372.dtsi
+++ b/arch/arm/boot/dts/am4372.dtsi
@@ -67,11 +67,15 @@
 	};
 
 	ocp {
-		compatible = "simple-bus";
+		compatible = "ti,am4372-l3-noc", "simple-bus";
 		#address-cells = <1>;
 		#size-cells = <1>;
 		ranges;
 		ti,hwmods = "l3_main";
+		reg = <0x44000000 0x400000
+		       0x44800000 0x400000>;
+		interrupts = <GIC_SPI 9 IRQ_TYPE_LEVEL_HIGH>,
+			     <GIC_SPI 10 IRQ_TYPE_LEVEL_HIGH>;
 
 		prcm: prcm@44df0000 {
 			compatible = "ti,am4-prcm";
@@ -108,9 +112,6 @@
 					<GIC_SPI 13 IRQ_TYPE_LEVEL_HIGH>,
 					<GIC_SPI 14 IRQ_TYPE_LEVEL_HIGH>;
 			#dma-cells = <1>;
-			dma-channels = <64>;
-			ti,edma-regions = <4>;
-			ti,edma-slots = <256>;
 		};
 
 		uart0: serial@44e09000 {
@@ -521,6 +522,12 @@
 				/* Filled in by U-Boot */
 				mac-address = [ 00 00 00 00 00 00 ];
 			};
+
+			phy_sel: cpsw-phy-sel@44e10650 {
+				compatible = "ti,am43xx-cpsw-phy-sel";
+				reg= <0x44e10650 0x4>;
+				reg-names = "gmii-sel";
+			};
 		};
 
 		epwmss0: epwmss@48300000 {
@@ -691,7 +698,7 @@
 			      <0x46000000 0x400000>;
 			reg-names = "mpu", "dat";
 			interrupts = <80>, <81>;
-			interrupts-names = "tx", "rx";
+			interrupt-names = "tx", "rx";
 			status = "disabled";
 			dmas = <&edma 8>,
 			       <&edma 9>;
@@ -705,7 +712,7 @@
 			      <0x46400000 0x400000>;
 			reg-names = "mpu", "dat";
 			interrupts = <82>, <83>;
-			interrupts-names = "tx", "rx";
+			interrupt-names = "tx", "rx";
 			status = "disabled";
 			dmas = <&edma 10>,
 			       <&edma 11>;
@@ -735,6 +742,121 @@
 			#size-cells = <1>;
 			status = "disabled";
 		};
+
+		am43xx_control_usb2phy1: control-phy@44e10620 {
+			compatible = "ti,control-phy-usb2-am437";
+			reg = <0x44e10620 0x4>;
+			reg-names = "power";
+		};
+
+		am43xx_control_usb2phy2: control-phy@0x44e10628 {
+			compatible = "ti,control-phy-usb2-am437";
+			reg = <0x44e10628 0x4>;
+			reg-names = "power";
+		};
+
+		ocp2scp0: ocp2scp@483a8000 {
+			compatible = "ti,omap-ocp2scp";
+			#address-cells = <1>;
+			#size-cells = <1>;
+			ranges;
+			ti,hwmods = "ocp2scp0";
+
+			usb2_phy1: phy@483a8000 {
+				compatible = "ti,am437x-usb2";
+				reg = <0x483a8000 0x8000>;
+				ctrl-module = <&am43xx_control_usb2phy1>;
+				clocks = <&usb_phy0_always_on_clk32k>,
+					 <&usb_otg_ss0_refclk960m>;
+				clock-names = "wkupclk", "refclk";
+				#phy-cells = <0>;
+				status = "disabled";
+			};
+		};
+
+		ocp2scp1: ocp2scp@483e8000 {
+			compatible = "ti,omap-ocp2scp";
+			#address-cells = <1>;
+			#size-cells = <1>;
+			ranges;
+			ti,hwmods = "ocp2scp1";
+
+			usb2_phy2: phy@483e8000 {
+				compatible = "ti,am437x-usb2";
+				reg = <0x483e8000 0x8000>;
+				ctrl-module = <&am43xx_control_usb2phy2>;
+				clocks = <&usb_phy1_always_on_clk32k>,
+					 <&usb_otg_ss1_refclk960m>;
+				clock-names = "wkupclk", "refclk";
+				#phy-cells = <0>;
+				status = "disabled";
+			};
+		};
+
+		dwc3_1: omap_dwc3@48380000 {
+			compatible = "ti,am437x-dwc3";
+			ti,hwmods = "usb_otg_ss0";
+			reg = <0x48380000 0x10000>;
+			interrupts = <GIC_SPI 172 IRQ_TYPE_LEVEL_HIGH>;
+			#address-cells = <1>;
+			#size-cells = <1>;
+			utmi-mode = <1>;
+			ranges;
+
+			usb1: usb@48390000 {
+				compatible = "synopsys,dwc3";
+				reg = <0x48390000 0x17000>;
+				interrupts = <GIC_SPI 168 IRQ_TYPE_LEVEL_HIGH>;
+				phys = <&usb2_phy1>;
+				phy-names = "usb2-phy";
+				maximum-speed = "high-speed";
+				dr_mode = "otg";
+				status = "disabled";
+			};
+		};
+
+		dwc3_2: omap_dwc3@483c0000 {
+			compatible = "ti,am437x-dwc3";
+			ti,hwmods = "usb_otg_ss1";
+			reg = <0x483c0000 0x10000>;
+			interrupts = <GIC_SPI 178 IRQ_TYPE_LEVEL_HIGH>;
+			#address-cells = <1>;
+			#size-cells = <1>;
+			utmi-mode = <1>;
+			ranges;
+
+			usb2: usb@483d0000 {
+				compatible = "synopsys,dwc3";
+				reg = <0x483d0000 0x17000>;
+				interrupts = <GIC_SPI 174 IRQ_TYPE_LEVEL_HIGH>;
+				phys = <&usb2_phy2>;
+				phy-names = "usb2-phy";
+				maximum-speed = "high-speed";
+				dr_mode = "otg";
+				status = "disabled";
+			};
+		};
+
+		qspi: qspi@47900000 {
+			compatible = "ti,am4372-qspi";
+			reg = <0x47900000 0x100>;
+			#address-cells = <1>;
+			#size-cells = <0>;
+			ti,hwmods = "qspi";
+			interrupts = <0 138 0x4>;
+			num-cs = <4>;
+			status = "disabled";
+		};
+
+		hdq: hdq@48347000 {
+			compatible = "ti,am43xx-hdq";
+			reg = <0x48347000 0x1000>;
+			interrupts = <GIC_SPI 139 IRQ_TYPE_LEVEL_HIGH>;
+			clocks = <&func_12m_clk>;
+			clock-names = "fck";
+			ti,hwmods = "hdq1w";
+			status = "disabled";
+		};
 	};
 };
 
diff --git a/arch/arm/boot/dts/am437x-gp-evm.dts b/arch/arm/boot/dts/am437x-gp-evm.dts
index df8798e8bd2..c25d15837ce 100644
--- a/arch/arm/boot/dts/am437x-gp-evm.dts
+++ b/arch/arm/boot/dts/am437x-gp-evm.dts
@@ -27,6 +27,17 @@
 		enable-active-high;
 	};
 
+	vtt_fixed: fixedregulator-vtt {
+		compatible = "regulator-fixed";
+		regulator-name = "vtt_fixed";
+		regulator-min-microvolt = <1500000>;
+		regulator-max-microvolt = <1500000>;
+		regulator-always-on;
+		regulator-boot-on;
+		enable-active-high;
+		gpio = <&gpio5 7 GPIO_ACTIVE_HIGH>;
+	};
+
 	backlight {
 		compatible = "pwm-backlight";
 		pwms = <&ecap0 0 50000 PWM_POLARITY_INVERTED>;
@@ -81,6 +92,85 @@
 			0x164 MUX_MODE0       /* eCAP0_in_PWM0_out.eCAP0_in_PWM0_out MODE0 */
 		>;
 	};
+
+	pixcir_ts_pins: pixcir_ts_pins {
+		pinctrl-single,pins = <
+			0x264 (PIN_INPUT_PULLUP | MUX_MODE7)  /* spi2_d0.gpio3_22 */
+		>;
+	};
+
+	cpsw_default: cpsw_default {
+		pinctrl-single,pins = <
+			/* Slave 1 */
+			0x114 (PIN_OUTPUT_PULLDOWN | MUX_MODE2)	/* mii1_txen.rgmii1_txen */
+			0x118 (PIN_INPUT_PULLDOWN | MUX_MODE2)	/* mii1_rxdv.rgmii1_rxctl */
+			0x11c (PIN_OUTPUT_PULLDOWN | MUX_MODE2)	/* mii1_txd1.rgmii1_txd3 */
+			0x120 (PIN_OUTPUT_PULLDOWN | MUX_MODE2)	/* mii1_txd0.rgmii1_txd2 */
+			0x124 (PIN_OUTPUT_PULLDOWN | MUX_MODE2)	/* mii1_txd1.rgmii1_txd1 */
+			0x128 (PIN_OUTPUT_PULLDOWN | MUX_MODE2)	/* mii1_txd0.rgmii1_txd0 */
+			0x12c (PIN_OUTPUT_PULLDOWN | MUX_MODE2)	/* mii1_txclk.rmii1_tclk */
+			0x130 (PIN_INPUT_PULLDOWN | MUX_MODE2)	/* mii1_rxclk.rmii1_rclk */
+			0x134 (PIN_INPUT_PULLDOWN | MUX_MODE2)	/* mii1_rxd1.rgmii1_rxd3 */
+			0x138 (PIN_INPUT_PULLDOWN | MUX_MODE2)	/* mii1_rxd0.rgmii1_rxd2 */
+			0x13c (PIN_INPUT_PULLDOWN | MUX_MODE2)	/* mii1_rxd1.rgmii1_rxd1 */
+			0x140 (PIN_INPUT_PULLDOWN | MUX_MODE2)	/* mii1_rxd0.rgmii1_rxd0 */
+		>;
+	};
+
+	cpsw_sleep: cpsw_sleep {
+		pinctrl-single,pins = <
+			/* Slave 1 reset value */
+			0x114 (PIN_INPUT_PULLDOWN | MUX_MODE7)
+			0x118 (PIN_INPUT_PULLDOWN | MUX_MODE7)
+			0x11c (PIN_INPUT_PULLDOWN | MUX_MODE7)
+			0x120 (PIN_INPUT_PULLDOWN | MUX_MODE7)
+			0x124 (PIN_INPUT_PULLDOWN | MUX_MODE7)
+			0x128 (PIN_INPUT_PULLDOWN | MUX_MODE7)
+			0x12c (PIN_INPUT_PULLDOWN | MUX_MODE7)
+			0x130 (PIN_INPUT_PULLDOWN | MUX_MODE7)
+			0x134 (PIN_INPUT_PULLDOWN | MUX_MODE7)
+			0x138 (PIN_INPUT_PULLDOWN | MUX_MODE7)
+			0x13c (PIN_INPUT_PULLDOWN | MUX_MODE7)
+			0x140 (PIN_INPUT_PULLDOWN | MUX_MODE7)
+		>;
+	};
+
+	davinci_mdio_default: davinci_mdio_default {
+		pinctrl-single,pins = <
+			/* MDIO */
+			0x148 (PIN_INPUT_PULLUP | SLEWCTRL_FAST | MUX_MODE0)	/* mdio_data.mdio_data */
+			0x14c (PIN_OUTPUT_PULLUP | MUX_MODE0)			/* mdio_clk.mdio_clk */
+		>;
+	};
+
+	davinci_mdio_sleep: davinci_mdio_sleep {
+		pinctrl-single,pins = <
+			/* MDIO reset value */
+			0x148 (PIN_INPUT_PULLDOWN | MUX_MODE7)
+			0x14c (PIN_INPUT_PULLDOWN | MUX_MODE7)
+		>;
+	};
+
+	nand_flash_x8: nand_flash_x8 {
+		pinctrl-single,pins = <
+			0x26c(PIN_OUTPUT_PULLDOWN | MUX_MODE7)	/* spi2_cs0.gpio/eMMCorNANDsel */
+			0x0  (PIN_INPUT  | MUX_MODE0)	/* gpmc_ad0.gpmc_ad0 */
+			0x4  (PIN_INPUT  | MUX_MODE0)	/* gpmc_ad1.gpmc_ad1 */
+			0x8  (PIN_INPUT  | MUX_MODE0)	/* gpmc_ad2.gpmc_ad2 */
+			0xc  (PIN_INPUT  | MUX_MODE0)	/* gpmc_ad3.gpmc_ad3 */
+			0x10 (PIN_INPUT  | MUX_MODE0)	/* gpmc_ad4.gpmc_ad4 */
+			0x14 (PIN_INPUT  | MUX_MODE0)	/* gpmc_ad5.gpmc_ad5 */
+			0x18 (PIN_INPUT  | MUX_MODE0)	/* gpmc_ad6.gpmc_ad6 */
+			0x1c (PIN_INPUT  | MUX_MODE0)	/* gpmc_ad7.gpmc_ad7 */
+			0x70 (PIN_INPUT_PULLUP | MUX_MODE0)	/* gpmc_wait0.gpmc_wait0 */
+			0x74 (PIN_OUTPUT_PULLUP | MUX_MODE7)	/* gpmc_wpn.gpmc_wpn */
+			0x7c (PIN_OUTPUT | MUX_MODE0)		/* gpmc_csn0.gpmc_csn0  */
+			0x90 (PIN_OUTPUT | MUX_MODE0)		/* gpmc_advn_ale.gpmc_advn_ale */
+			0x94 (PIN_OUTPUT | MUX_MODE0)		/* gpmc_oen_ren.gpmc_oen_ren */
+			0x98 (PIN_OUTPUT | MUX_MODE0)		/* gpmc_wen.gpmc_wen */
+			0x9c (PIN_OUTPUT | MUX_MODE0)		/* gpmc_be0n_cle.gpmc_be0n_cle */
+		>;
+	};
 };
 
 &i2c0 {
@@ -93,6 +183,20 @@
         status = "okay";
         pinctrl-names = "default";
         pinctrl-0 = <&i2c1_pins>;
+
+	pixcir_ts@5c {
+		compatible = "pixcir,pixcir_tangoc";
+		pinctrl-names = "default";
+		pinctrl-0 = <&pixcir_ts_pins>;
+		reg = <0x5c>;
+		interrupt-parent = <&gpio3>;
+		interrupts = <22 0>;
+
+		attb-gpio = <&gpio3 22 GPIO_ACTIVE_HIGH>;
+
+		x-size = <1024>;
+		y-size = <600>;
+	};
 };
 
 &epwmss0 {
@@ -117,6 +221,11 @@
 	status = "okay";
 };
 
+&gpio5 {
+	status = "okay";
+	ti,no-reset-on-init;
+};
+
 &mmc1 {
 	status = "okay";
 	vmmc-supply = <&vmmcsd_fixed>;
@@ -125,3 +234,128 @@
 	pinctrl-0 = <&mmc1_pins>;
 	cd-gpios = <&gpio0 6 GPIO_ACTIVE_HIGH>;
 };
+
+&usb2_phy1 {
+	status = "okay";
+};
+
+&usb1 {
+	dr_mode = "peripheral";
+	status = "okay";
+};
+
+&usb2_phy2 {
+	status = "okay";
+};
+
+&usb2 {
+	dr_mode = "host";
+	status = "okay";
+};
+
+&mac {
+	slaves = <1>;
+	pinctrl-names = "default", "sleep";
+	pinctrl-0 = <&cpsw_default>;
+	pinctrl-1 = <&cpsw_sleep>;
+	status = "okay";
+};
+
+&davinci_mdio {
+	pinctrl-names = "default", "sleep";
+	pinctrl-0 = <&davinci_mdio_default>;
+	pinctrl-1 = <&davinci_mdio_sleep>;
+	status = "okay";
+};
+
+&cpsw_emac0 {
+	phy_id = <&davinci_mdio>, <0>;
+	phy-mode = "rgmii";
+};
+
+&elm {
+	status = "okay";
+};
+
+&gpmc {
+	status = "okay";
+	pinctrl-names = "default";
+	pinctrl-0 = <&nand_flash_x8>;
+	ranges = <0 0 0 0x01000000>;	/* minimum GPMC partition = 16MB */
+	nand@0,0 {
+		reg = <0 0 4>;		/* device IO registers */
+		ti,nand-ecc-opt = "bch8";
+		ti,elm-id = <&elm>;
+		nand-bus-width = <8>;
+		gpmc,device-width = <1>;
+		gpmc,sync-clk-ps = <0>;
+		gpmc,cs-on-ns = <0>;
+		gpmc,cs-rd-off-ns = <40>;
+		gpmc,cs-wr-off-ns = <40>;
+		gpmc,adv-on-ns = <0>;
+		gpmc,adv-rd-off-ns = <25>;
+		gpmc,adv-wr-off-ns = <25>;
+		gpmc,we-on-ns = <0>;
+		gpmc,we-off-ns = <20>;
+		gpmc,oe-on-ns = <3>;
+		gpmc,oe-off-ns = <30>;
+		gpmc,access-ns = <30>;
+		gpmc,rd-cycle-ns = <40>;
+		gpmc,wr-cycle-ns = <40>;
+		gpmc,wait-pin = <0>;
+		gpmc,wait-on-read;
+		gpmc,wait-on-write;
+		gpmc,bus-turnaround-ns = <0>;
+		gpmc,cycle2cycle-delay-ns = <0>;
+		gpmc,clk-activation-ns = <0>;
+		gpmc,wait-monitoring-ns = <0>;
+		gpmc,wr-access-ns = <40>;
+		gpmc,wr-data-mux-bus-ns = <0>;
+		/* MTD partition table */
+		/* All SPL-* partitions are sized to minimal length
+		 * which can be independently programmable. For
+		 * NAND flash this is equal to size of erase-block */
+		#address-cells = <1>;
+		#size-cells = <1>;
+		partition@0 {
+			label = "NAND.SPL";
+			reg = <0x00000000 0x00040000>;
+		};
+		partition@1 {
+			label = "NAND.SPL.backup1";
+			reg = <0x00040000 0x00040000>;
+		};
+		partition@2 {
+			label = "NAND.SPL.backup2";
+			reg = <0x00080000 0x00040000>;
+		};
+		partition@3 {
+			label = "NAND.SPL.backup3";
+			reg = <0x000c0000 0x00040000>;
+		};
+		partition@4 {
+			label = "NAND.u-boot-spl-os";
+			reg = <0x00100000 0x00080000>;
+		};
+		partition@5 {
+			label = "NAND.u-boot";
+			reg = <0x00180000 0x00100000>;
+		};
+		partition@6 {
+			label = "NAND.u-boot-env";
+			reg = <0x00280000 0x00040000>;
+		};
+		partition@7 {
+			label = "NAND.u-boot-env.backup1";
+			reg = <0x002c0000 0x00040000>;
+		};
+		partition@8 {
+			label = "NAND.kernel";
+			reg = <0x00300000 0x00700000>;
+		};
+		partition@9 {
+			label = "NAND.file-system";
+			reg = <0x00a00000 0x1f600000>;
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/am43x-epos-evm.dts b/arch/arm/boot/dts/am43x-epos-evm.dts
index 167dbc8494d..ad362c50e32 100644
--- a/arch/arm/boot/dts/am43x-epos-evm.dts
+++ b/arch/arm/boot/dts/am43x-epos-evm.dts
@@ -138,6 +138,29 @@
 				0x160 (PIN_INPUT | MUX_MODE7) /* spi0_cs1.gpio0_6 */
 			>;
 		};
+
+		qspi1_default: qspi1_default {
+			pinctrl-single,pins = <
+				0x7c (PIN_INPUT_PULLUP | MUX_MODE3)
+				0x88 (PIN_INPUT_PULLUP | MUX_MODE2)
+				0x90 (PIN_INPUT_PULLUP | MUX_MODE3)
+				0x94 (PIN_INPUT_PULLUP | MUX_MODE3)
+				0x98 (PIN_INPUT_PULLUP | MUX_MODE3)
+				0x9c (PIN_INPUT_PULLUP | MUX_MODE3)
+			>;
+		};
+
+		pixcir_ts_pins: pixcir_ts_pins {
+			pinctrl-single,pins = <
+				0x44 (PIN_INPUT_PULLUP | MUX_MODE7)	/* gpmc_a1.gpio1_17 */
+			>;
+		};
+
+		hdq_pins: pinmux_hdq_pins {
+			pinctrl-single,pins = <
+				0x234 (PIN_INPUT_PULLUP | MUX_MODE1)    /* cam1_wen.hdq_gpio */
+			>;
+		};
 	};
 
 	matrix_keypad: matrix_keypad@0 {
@@ -226,7 +249,9 @@
 	};
 
 	pixcir_ts@5c {
-		compatible = "pixcir,pixcir_ts";
+		compatible = "pixcir,pixcir_tangoc";
+		pinctrl-names = "default";
+		pinctrl-0 = <&pixcir_ts_pins>;
 		reg = <0x5c>;
 		interrupt-parent = <&gpio1>;
 		interrupts = <17 0>;
@@ -234,7 +259,7 @@
 		attb-gpio = <&gpio1 17 GPIO_ACTIVE_HIGH>;
 
 		x-size = <1024>;
-		y-size = <768>;
+		y-size = <600>;
 	};
 };
 
@@ -341,7 +366,7 @@
 		};
 		partition@9 {
 			label = "NAND.file-system";
-			reg = <0x00800000 0x1F600000>;
+			reg = <0x00a00000 0x1f600000>;
 		};
 	};
 };
@@ -367,3 +392,79 @@
 	pinctrl-0 = <&spi1_pins>;
 	status = "okay";
 };
+
+&usb2_phy1 {
+	status = "okay";
+};
+
+&usb1 {
+	dr_mode = "peripheral";
+	status = "okay";
+};
+
+&usb2_phy2 {
+	status = "okay";
+};
+
+&usb2 {
+	dr_mode = "host";
+	status = "okay";
+};
+
+&qspi {
+	status = "okay";
+	pinctrl-names = "default";
+	pinctrl-0 = <&qspi1_default>;
+
+	spi-max-frequency = <48000000>;
+	m25p80@0 {
+		compatible = "mx66l51235l";
+		spi-max-frequency = <48000000>;
+		reg = <0>;
+		spi-cpol;
+		spi-cpha;
+		spi-tx-bus-width = <1>;
+		spi-rx-bus-width = <4>;
+		#address-cells = <1>;
+		#size-cells = <1>;
+
+		/* MTD partition table.
+		 * The ROM checks the first 512KiB
+		 * for a valid file to boot(XIP).
+		 */
+		partition@0 {
+			label = "QSPI.U_BOOT";
+			reg = <0x00000000 0x000080000>;
+		};
+		partition@1 {
+			label = "QSPI.U_BOOT.backup";
+			reg = <0x00080000 0x00080000>;
+		};
+		partition@2 {
+			label = "QSPI.U-BOOT-SPL_OS";
+			reg = <0x00100000 0x00010000>;
+		};
+		partition@3 {
+			label = "QSPI.U_BOOT_ENV";
+			reg = <0x00110000 0x00010000>;
+		};
+		partition@4 {
+			label = "QSPI.U-BOOT-ENV.backup";
+			reg = <0x00120000 0x00010000>;
+		};
+		partition@5 {
+			label = "QSPI.KERNEL";
+			reg = <0x00130000 0x0800000>;
+		};
+		partition@6 {
+			label = "QSPI.FILESYSTEM";
+			reg = <0x00930000 0x36D0000>;
+		};
+	};
+};
+
+&hdq {
+	status = "okay";
+	pinctrl-names = "default";
+	pinctrl-0 = <&hdq_pins>;
+};
diff --git a/arch/arm/boot/dts/am43xx-clocks.dtsi b/arch/arm/boot/dts/am43xx-clocks.dtsi
index 142009cc933..c7dc9dab93a 100644
--- a/arch/arm/boot/dts/am43xx-clocks.dtsi
+++ b/arch/arm/boot/dts/am43xx-clocks.dtsi
@@ -11,6 +11,22 @@
 	sys_clkin_ck: sys_clkin_ck {
 		#clock-cells = <0>;
 		compatible = "ti,mux-clock";
+		clocks = <&sysboot_freq_sel_ck>, <&crystal_freq_sel_ck>;
+		ti,bit-shift = <31>;
+		reg = <0x0040>;
+	};
+
+	crystal_freq_sel_ck: crystal_freq_sel_ck {
+		#clock-cells = <0>;
+		compatible = "ti,mux-clock";
+		clocks = <&virt_19200000_ck>, <&virt_24000000_ck>, <&virt_25000000_ck>, <&virt_26000000_ck>;
+		ti,bit-shift = <29>;
+		reg = <0x0040>;
+	};
+
+	sysboot_freq_sel_ck: sysboot_freq_sel_ck@44e10040 {
+		#clock-cells = <0>;
+		compatible = "ti,mux-clock";
 		clocks = <&virt_19200000_ck>, <&virt_24000000_ck>, <&virt_25000000_ck>, <&virt_26000000_ck>;
 		ti,bit-shift = <22>;
 		reg = <0x0040>;
@@ -87,6 +103,54 @@
 		clock-mult = <1>;
 		clock-div = <1>;
 	};
+
+	ehrpwm0_tbclk: ehrpwm0_tbclk {
+		#clock-cells = <0>;
+		compatible = "ti,gate-clock";
+		clocks = <&dpll_per_m2_ck>;
+		ti,bit-shift = <0>;
+		reg = <0x0664>;
+	};
+
+	ehrpwm1_tbclk: ehrpwm1_tbclk {
+		#clock-cells = <0>;
+		compatible = "ti,gate-clock";
+		clocks = <&dpll_per_m2_ck>;
+		ti,bit-shift = <1>;
+		reg = <0x0664>;
+	};
+
+	ehrpwm2_tbclk: ehrpwm2_tbclk {
+		#clock-cells = <0>;
+		compatible = "ti,gate-clock";
+		clocks = <&dpll_per_m2_ck>;
+		ti,bit-shift = <2>;
+		reg = <0x0664>;
+	};
+
+	ehrpwm3_tbclk: ehrpwm3_tbclk {
+		#clock-cells = <0>;
+		compatible = "ti,gate-clock";
+		clocks = <&dpll_per_m2_ck>;
+		ti,bit-shift = <4>;
+		reg = <0x0664>;
+	};
+
+	ehrpwm4_tbclk: ehrpwm4_tbclk {
+		#clock-cells = <0>;
+		compatible = "ti,gate-clock";
+		clocks = <&dpll_per_m2_ck>;
+		ti,bit-shift = <5>;
+		reg = <0x0664>;
+	};
+
+	ehrpwm5_tbclk: ehrpwm5_tbclk {
+		#clock-cells = <0>;
+		compatible = "ti,gate-clock";
+		clocks = <&dpll_per_m2_ck>;
+		ti,bit-shift = <6>;
+		reg = <0x0664>;
+	};
 };
 &prcm_clocks {
 	clk_32768_ck: clk_32768_ck {
@@ -229,6 +293,7 @@
 		reg = <0x2e30>;
 		ti,index-starts-at-one;
 		ti,invert-autoidle-bit;
+		ti,set-rate-parent;
 	};
 
 	dpll_per_ck: dpll_per_ck {
@@ -511,6 +576,7 @@
 		compatible = "ti,mux-clock";
 		clocks = <&dpll_disp_m2_ck>, <&dpll_core_m5_ck>, <&dpll_per_m2_ck>;
 		reg = <0x4244>;
+		ti,set-rate-parent;
 	};
 
 	dpll_extdev_ck: dpll_extdev_ck {
@@ -609,10 +675,13 @@
 
 	dpll_per_clkdcoldo: dpll_per_clkdcoldo {
 		#clock-cells = <0>;
-		compatible = "fixed-factor-clock";
+		compatible = "ti,fixed-factor-clock";
 		clocks = <&dpll_per_ck>;
-		clock-mult = <1>;
-		clock-div = <1>;
+		ti,clock-mult = <1>;
+		ti,clock-div = <1>;
+		ti,autoidle-shift = <8>;
+		reg = <0x2e14>;
+		ti,invert-autoidle-bit;
 	};
 
 	dll_aging_clk_div: dll_aging_clk_div {
@@ -653,4 +722,36 @@
 		clocks = <&clk_32768_ck>, <&clk_32k_tpm_ck>;
 		reg = <0x4260>;
 	};
+
+	usb_phy0_always_on_clk32k: usb_phy0_always_on_clk32k {
+		#clock-cells = <0>;
+		compatible = "ti,gate-clock";
+		clocks = <&usbphy_32khz_clkmux>;
+		ti,bit-shift = <8>;
+		reg = <0x2a40>;
+	};
+
+	usb_phy1_always_on_clk32k: usb_phy1_always_on_clk32k {
+		#clock-cells = <0>;
+		compatible = "ti,gate-clock";
+		clocks = <&usbphy_32khz_clkmux>;
+		ti,bit-shift = <8>;
+		reg = <0x2a48>;
+	};
+
+	usb_otg_ss0_refclk960m: usb_otg_ss0_refclk960m {
+		#clock-cells = <0>;
+		compatible = "ti,gate-clock";
+		clocks = <&dpll_per_clkdcoldo>;
+		ti,bit-shift = <8>;
+		reg = <0x8a60>;
+	};
+
+	usb_otg_ss1_refclk960m: usb_otg_ss1_refclk960m {
+		#clock-cells = <0>;
+		compatible = "ti,gate-clock";
+		clocks = <&dpll_per_clkdcoldo>;
+		ti,bit-shift = <8>;
+		reg = <0x8a68>;
+	};
 };
diff --git a/arch/arm/boot/dts/armada-370-db.dts b/arch/arm/boot/dts/armada-370-db.dts
index 82f238a9063..416f4e5a69c 100644
--- a/arch/arm/boot/dts/armada-370-db.dts
+++ b/arch/arm/boot/dts/armada-370-db.dts
@@ -35,7 +35,6 @@
 
 		internal-regs {
 			serial@12000 {
-				clock-frequency = <200000000>;
 				status = "okay";
 			};
 			sata@a0000 {
@@ -67,6 +66,7 @@
 			i2c@11000 {
 				pinctrl-0 = <&i2c0_pins>;
 				pinctrl-names = "default";
+				clock-frequency = <100000>;
 				status = "okay";
 				audio_codec: audio-codec@4a {
 					compatible = "cirrus,cs42l51";
diff --git a/arch/arm/boot/dts/armada-370-mirabox.dts b/arch/arm/boot/dts/armada-370-mirabox.dts
index 2354fe023ee..097df7d8f0f 100644
--- a/arch/arm/boot/dts/armada-370-mirabox.dts
+++ b/arch/arm/boot/dts/armada-370-mirabox.dts
@@ -47,7 +47,6 @@
 
 		internal-regs {
 			serial@12000 {
-				clock-frequency = <200000000>;
 				status = "okay";
 			};
 			timer@20300 {
diff --git a/arch/arm/boot/dts/armada-370-netgear-rn102.dts b/arch/arm/boot/dts/armada-370-netgear-rn102.dts
index 651aeb5ef43..d6d572e5af3 100644
--- a/arch/arm/boot/dts/armada-370-netgear-rn102.dts
+++ b/arch/arm/boot/dts/armada-370-netgear-rn102.dts
@@ -50,7 +50,6 @@
 
 		internal-regs {
 			serial@12000 {
-				clock-frequency = <200000000>;
 				status = "okay";
 			};
 
diff --git a/arch/arm/boot/dts/armada-370-netgear-rn104.dts b/arch/arm/boot/dts/armada-370-netgear-rn104.dts
index 4e27587667b..c5fe8b5dcdc 100644
--- a/arch/arm/boot/dts/armada-370-netgear-rn104.dts
+++ b/arch/arm/boot/dts/armada-370-netgear-rn104.dts
@@ -50,7 +50,6 @@
 
 		internal-regs {
 			serial@12000 {
-				clock-frequency = <200000000>;
 				status = "okay";
 			};
 
diff --git a/arch/arm/boot/dts/armada-370-rd.dts b/arch/arm/boot/dts/armada-370-rd.dts
index 3e2c857d600..4169f4096ea 100644
--- a/arch/arm/boot/dts/armada-370-rd.dts
+++ b/arch/arm/boot/dts/armada-370-rd.dts
@@ -51,7 +51,6 @@
 
 		internal-regs {
 			serial@12000 {
-				clock-frequency = <200000000>;
 				status = "okay";
 			};
 			sata@a0000 {
diff --git a/arch/arm/boot/dts/armada-370-xp.dtsi b/arch/arm/boot/dts/armada-370-xp.dtsi
index bbb40f62037..23227e0027e 100644
--- a/arch/arm/boot/dts/armada-370-xp.dtsi
+++ b/arch/arm/boot/dts/armada-370-xp.dtsi
@@ -157,6 +157,7 @@
 				reg-shift = <2>;
 				interrupts = <41>;
 				reg-io-width = <1>;
+				clocks = <&coreclk 0>;
 				status = "disabled";
 			};
 			serial@12100 {
@@ -165,6 +166,7 @@
 				reg-shift = <2>;
 				interrupts = <42>;
 				reg-io-width = <1>;
+				clocks = <&coreclk 0>;
 				status = "disabled";
 			};
 
@@ -203,6 +205,11 @@
 				reg = <0x20300 0x34>, <0x20704 0x4>;
 			};
 
+			pmsu@22000 {
+				compatible = "marvell,armada-370-pmsu";
+				reg = <0x22000 0x1000>;
+			};
+
 			usb@50000 {
 				compatible = "marvell,orion-ehci";
 				reg = <0x50000 0x500>;
@@ -230,6 +237,7 @@
 				#size-cells = <0>;
 				compatible = "marvell,orion-mdio";
 				reg = <0x72004 0x4>;
+				clocks = <&gateclk 4>;
 			};
 
 			eth1: ethernet@74000 {
diff --git a/arch/arm/boot/dts/armada-370.dtsi b/arch/arm/boot/dts/armada-370.dtsi
index af1f11e9e5a..21b588b6f6b 100644
--- a/arch/arm/boot/dts/armada-370.dtsi
+++ b/arch/arm/boot/dts/armada-370.dtsi
@@ -220,6 +220,11 @@
 				clocks = <&coreclk 2>;
 			};
 
+			cpurst@20800 {
+				compatible = "marvell,armada-370-cpu-reset";
+				reg = <0x20800 0x8>;
+			};
+
 			audio_controller: audio-controller@30000 {
 				compatible = "marvell,armada370-audio";
 				reg = <0x30000 0x4000>;
diff --git a/arch/arm/boot/dts/armada-375-db.dts b/arch/arm/boot/dts/armada-375-db.dts
index 9378d3136b4..772fec2d26c 100644
--- a/arch/arm/boot/dts/armada-375-db.dts
+++ b/arch/arm/boot/dts/armada-375-db.dts
@@ -68,7 +68,6 @@
 			};
 
 			serial@12000 {
-				clock-frequency = <200000000>;
 				status = "okay";
 			};
 
@@ -79,6 +78,11 @@
 				};
 			};
 
+			sata@a0000 {
+				status = "okay";
+				nr-ports = <2>;
+			};
+
 			nand: nand@d0000 {
 				pinctrl-0 = <&nand_pins>;
 				pinctrl-names = "default";
@@ -102,6 +106,14 @@
 				};
 			};
 
+			usb@54000 {
+				status = "okay";
+			};
+
+			usb3@58000 {
+				status = "okay";
+			};
+
 			mvsdio@d4000 {
 				pinctrl-0 = <&sdio_pins &sdio_st_pins>;
 				pinctrl-names = "default";
diff --git a/arch/arm/boot/dts/armada-375.dtsi b/arch/arm/boot/dts/armada-375.dtsi
index 3877693fb2d..fb92551a1e7 100644
--- a/arch/arm/boot/dts/armada-375.dtsi
+++ b/arch/arm/boot/dts/armada-375.dtsi
@@ -39,6 +39,8 @@
 	cpus {
 		#address-cells = <1>;
 		#size-cells = <0>;
+		enable-method = "marvell,armada-375-smp";
+
 		cpu@0 {
 			device_type = "cpu";
 			compatible = "arm,cortex-a9";
@@ -128,6 +130,11 @@
 				cache-level = <2>;
 			};
 
+			scu@c000 {
+				compatible = "arm,cortex-a9-scu";
+				reg = <0xc000 0x58>;
+			};
+
 			timer@c600 {
 				compatible = "arm,cortex-a9-twd-timer";
 				reg = <0xc600 0x20>;
@@ -194,6 +201,7 @@
 				reg-shift = <2>;
 				interrupts = <GIC_SPI 12 IRQ_TYPE_LEVEL_HIGH>;
 				reg-io-width = <1>;
+				clocks = <&coreclk 0>;
 				status = "disabled";
 			};
 
@@ -203,6 +211,7 @@
 				reg-shift = <2>;
 				interrupts = <GIC_SPI 13 IRQ_TYPE_LEVEL_HIGH>;
 				reg-io-width = <1>;
+				clocks = <&coreclk 0>;
 				status = "disabled";
 			};
 
@@ -320,6 +329,46 @@
 				clocks = <&coreclk 0>;
 			};
 
+			watchdog@20300 {
+				compatible = "marvell,armada-375-wdt";
+				reg = <0x20300 0x34>, <0x20704 0x4>, <0x18254 0x4>;
+				clocks = <&coreclk 0>;
+			};
+
+			cpurst@20800 {
+				compatible = "marvell,armada-370-cpu-reset";
+				reg = <0x20800 0x10>;
+			};
+
+			coherency-fabric@21010 {
+				compatible = "marvell,armada-375-coherency-fabric";
+				reg = <0x21010 0x1c>;
+			};
+
+			usb@50000 {
+				compatible = "marvell,orion-ehci";
+				reg = <0x50000 0x500>;
+				interrupts = <GIC_SPI 17 IRQ_TYPE_LEVEL_HIGH>;
+				clocks = <&gateclk 18>;
+				status = "disabled";
+			};
+
+			usb@54000 {
+				compatible = "marvell,orion-ehci";
+				reg = <0x54000 0x500>;
+				interrupts = <GIC_SPI 18 IRQ_TYPE_LEVEL_HIGH>;
+				clocks = <&gateclk 26>;
+				status = "disabled";
+			};
+
+			usb3@58000 {
+				compatible = "marvell,armada-375-xhci";
+				reg = <0x58000 0x20000>,<0x5b880 0x80>;
+				interrupts = <GIC_SPI 16 IRQ_TYPE_LEVEL_HIGH>;
+				clocks = <&gateclk 16>;
+				status = "disabled";
+			};
+
 			xor@60800 {
 				compatible = "marvell,orion-xor";
 				reg = <0x60800 0x100
@@ -391,6 +440,12 @@
 				status = "disabled";
 			};
 
+			thermal@e8078 {
+				compatible = "marvell,armada375-thermal";
+				reg = <0xe8078 0x4>, <0xe807c 0x8>;
+				status = "okay";
+			};
+
 			coreclk: mvebu-sar@e8204 {
 				compatible = "marvell,armada-375-core-clock";
 				reg = <0xe8204 0x04>;
diff --git a/arch/arm/boot/dts/armada-380.dtsi b/arch/arm/boot/dts/armada-380.dtsi
index 068031f0f26..e69bc6759c3 100644
--- a/arch/arm/boot/dts/armada-380.dtsi
+++ b/arch/arm/boot/dts/armada-380.dtsi
@@ -21,6 +21,8 @@
 	cpus {
 		#address-cells = <1>;
 		#size-cells = <0>;
+		enable-method = "marvell,armada-380-smp";
+
 		cpu@0 {
 			device_type = "cpu";
 			compatible = "arm,cortex-a9";
@@ -99,7 +101,7 @@
 			pcie@3,0 {
 				device_type = "pci";
 				assigned-addresses = <0x82000800 0 0x44000 0 0x2000>;
-				reg = <0x1000 0 0 0 0>;
+				reg = <0x1800 0 0 0 0>;
 				#address-cells = <3>;
 				#size-cells = <2>;
 				#interrupt-cells = <1>;
diff --git a/arch/arm/boot/dts/armada-385-db.dts b/arch/arm/boot/dts/armada-385-db.dts
index 6828d77696a..ff9637dd8d0 100644
--- a/arch/arm/boot/dts/armada-385-db.dts
+++ b/arch/arm/boot/dts/armada-385-db.dts
@@ -55,7 +55,6 @@
 			};
 
 			serial@12000 {
-				clock-frequency = <200000000>;
 				status = "okay";
 			};
 
@@ -65,6 +64,10 @@
 				phy-mode = "rgmii-id";
 			};
 
+			usb@50000 {
+				status = "ok";
+			};
+
 			ethernet@70000 {
 				status = "okay";
 				phy = <&phy0>;
@@ -81,6 +84,14 @@
 				};
 			};
 
+			sata@a8000 {
+				status = "okay";
+			};
+
+			sata@e0000 {
+				status = "okay";
+			};
+
 			flash@d0000 {
 				status = "okay";
 				num-cs = <1>;
@@ -101,6 +112,22 @@
 					reg = <0x1000000 0x3f000000>;
 				};
 			};
+
+			sdhci@d8000 {
+				clock-frequency = <200000000>;
+				broken-cd;
+				wp-inverted;
+				bus-width = <8>;
+				status = "okay";
+			};
+
+			usb3@f0000 {
+				status = "okay";
+			};
+
+			usb3@f8000 {
+				status = "okay";
+			};
 		};
 
 		pcie-controller {
diff --git a/arch/arm/boot/dts/armada-385-rd.dts b/arch/arm/boot/dts/armada-385-rd.dts
index 45250c88814..40893255a3f 100644
--- a/arch/arm/boot/dts/armada-385-rd.dts
+++ b/arch/arm/boot/dts/armada-385-rd.dts
@@ -51,7 +51,6 @@
 			};
 
 			serial@12000 {
-				clock-frequency = <200000000>;
 				status = "okay";
 			};
 
@@ -77,6 +76,10 @@
 					reg = <1>;
 				};
 			};
+
+			usb3@f0000 {
+				status = "okay";
+			};
 		};
 
 		pcie-controller {
diff --git a/arch/arm/boot/dts/armada-385.dtsi b/arch/arm/boot/dts/armada-385.dtsi
index e2919f02e1d..f011009bf4c 100644
--- a/arch/arm/boot/dts/armada-385.dtsi
+++ b/arch/arm/boot/dts/armada-385.dtsi
@@ -21,6 +21,8 @@
 	cpus {
 		#address-cells = <1>;
 		#size-cells = <0>;
+		enable-method = "marvell,armada-380-smp";
+
 		cpu@0 {
 			device_type = "cpu";
 			compatible = "arm,cortex-a9";
@@ -110,7 +112,7 @@
 			pcie@3,0 {
 				device_type = "pci";
 				assigned-addresses = <0x82000800 0 0x44000 0 0x2000>;
-				reg = <0x1000 0 0 0 0>;
+				reg = <0x1800 0 0 0 0>;
 				#address-cells = <3>;
 				#size-cells = <2>;
 				#interrupt-cells = <1>;
@@ -131,7 +133,7 @@
 			pcie@4,0 {
 				device_type = "pci";
 				assigned-addresses = <0x82000800 0 0x48000 0 0x2000>;
-				reg = <0x1000 0 0 0 0>;
+				reg = <0x2000 0 0 0 0>;
 				#address-cells = <3>;
 				#size-cells = <2>;
 				#interrupt-cells = <1>;
diff --git a/arch/arm/boot/dts/armada-38x.dtsi b/arch/arm/boot/dts/armada-38x.dtsi
index a064f59da02..3de364e81b5 100644
--- a/arch/arm/boot/dts/armada-38x.dtsi
+++ b/arch/arm/boot/dts/armada-38x.dtsi
@@ -108,6 +108,11 @@
 				cache-level = <2>;
 			};
 
+			scu@c000 {
+				compatible = "arm,cortex-a9-scu";
+				reg = <0xc000 0x58>;
+			};
+
 			timer@c600 {
 				compatible = "arm,cortex-a9-twd-timer";
 				reg = <0xc600 0x20>;
@@ -174,6 +179,7 @@
 				reg-shift = <2>;
 				interrupts = <GIC_SPI 12 IRQ_TYPE_LEVEL_HIGH>;
 				reg-io-width = <1>;
+				clocks = <&coreclk 0>;
 				status = "disabled";
 			};
 
@@ -183,6 +189,7 @@
 				reg-shift = <2>;
 				interrupts = <GIC_SPI 13 IRQ_TYPE_LEVEL_HIGH>;
 				reg-io-width = <1>;
+				clocks = <&coreclk 0>;
 				status = "disabled";
 			};
 
@@ -267,6 +274,28 @@
 				clock-names = "nbclk", "fixed";
 			};
 
+			watchdog@20300 {
+				compatible = "marvell,armada-380-wdt";
+				reg = <0x20300 0x34>, <0x20704 0x4>, <0x18260 0x4>;
+				clocks = <&coreclk 2>, <&refclk>;
+				clock-names = "nbclk", "fixed";
+			};
+
+			cpurst@20800 {
+				compatible = "marvell,armada-370-cpu-reset";
+				reg = <0x20800 0x10>;
+			};
+
+			coherency-fabric@21010 {
+				compatible = "marvell,armada-380-coherency-fabric";
+				reg = <0x21010 0x1c>;
+			};
+
+			pmsu@22000 {
+				compatible = "marvell,armada-380-pmsu";
+				reg = <0x22000 0x1000>;
+			};
+
 			eth1: ethernet@30000 {
 				compatible = "marvell,armada-370-neta";
 				reg = <0x30000 0x4000>;
@@ -283,6 +312,14 @@
 				status = "disabled";
 			};
 
+			usb@50000 {
+				compatible = "marvell,orion-ehci";
+				reg = <0x58000 0x500>;
+				interrupts = <GIC_SPI 18 IRQ_TYPE_LEVEL_HIGH>;
+				clocks = <&gateclk 18>;
+				status = "disabled";
+			};
+
 			xor@60800 {
 				compatible = "marvell,orion-xor";
 				reg = <0x60800 0x100
@@ -336,6 +373,23 @@
 				#size-cells = <0>;
 				compatible = "marvell,orion-mdio";
 				reg = <0x72004 0x4>;
+				clocks = <&gateclk 4>;
+			};
+
+			sata@a8000 {
+				compatible = "marvell,armada-380-ahci";
+				reg = <0xa8000 0x2000>;
+				interrupts = <GIC_SPI 26 IRQ_TYPE_LEVEL_HIGH>;
+				clocks = <&gateclk 15>;
+				status = "disabled";
+			};
+
+			sata@e0000 {
+				compatible = "marvell,armada-380-ahci";
+				reg = <0xe0000 0x2000>;
+				interrupts = <GIC_SPI 28 IRQ_TYPE_LEVEL_HIGH>;
+				clocks = <&gateclk 30>;
+				status = "disabled";
 			};
 
 			coredivclk: clock@e4250 {
@@ -346,6 +400,12 @@
 				clock-output-names = "nand";
 			};
 
+			thermal@e8078 {
+				compatible = "marvell,armada380-thermal";
+				reg = <0xe4078 0x4>, <0xe4074 0x4>;
+				status = "okay";
+			};
+
 			flash@d0000 {
 				compatible = "marvell,armada370-nand";
 				reg = <0xd0000 0x54>;
@@ -355,6 +415,31 @@
 				clocks = <&coredivclk 0>;
 				status = "disabled";
 			};
+
+			sdhci@d8000 {
+				compatible = "marvell,armada-380-sdhci";
+				reg = <0xd8000 0x1000>, <0xdc000 0x100>;
+				interrupts = <0 25 0x4>;
+				clocks = <&gateclk 17>;
+				mrvl,clk-delay-cycles = <0x1F>;
+				status = "disabled";
+			};
+
+			usb3@f0000 {
+				compatible = "marvell,armada-380-xhci";
+				reg = <0xf0000 0x4000>,<0xf4000 0x4000>;
+				interrupts = <GIC_SPI 16 IRQ_TYPE_LEVEL_HIGH>;
+				clocks = <&gateclk 9>;
+				status = "disabled";
+			};
+
+			usb3@f8000 {
+				compatible = "marvell,armada-380-xhci";
+				reg = <0xf8000 0x4000>,<0xfc000 0x4000>;
+				interrupts = <GIC_SPI 17 IRQ_TYPE_LEVEL_HIGH>;
+				clocks = <&gateclk 10>;
+				status = "disabled";
+			};
 		};
 	};
 
diff --git a/arch/arm/boot/dts/armada-xp-axpwifiap.dts b/arch/arm/boot/dts/armada-xp-axpwifiap.dts
index d83d7d69ac0..a55a97a7050 100644
--- a/arch/arm/boot/dts/armada-xp-axpwifiap.dts
+++ b/arch/arm/boot/dts/armada-xp-axpwifiap.dts
@@ -95,12 +95,10 @@
 			};
 
 			serial@12000 {
-				clock-frequency = <250000000>;
 				status = "okay";
 			};
 
 			serial@12100 {
-				clock-frequency = <250000000>;
 				status = "okay";
 			};
 
diff --git a/arch/arm/boot/dts/armada-xp-db.dts b/arch/arm/boot/dts/armada-xp-db.dts
index 448373c4b0e..42ddb286436 100644
--- a/arch/arm/boot/dts/armada-xp-db.dts
+++ b/arch/arm/boot/dts/armada-xp-db.dts
@@ -49,7 +49,7 @@
 			/* Device Bus parameters are required */
 
 			/* Read parameters */
-			devbus,bus-width    = <8>;
+			devbus,bus-width    = <16>;
 			devbus,turn-off-ps  = <60000>;
 			devbus,badr-skew-ps = <0>;
 			devbus,acc-first-ps = <124000>;
@@ -106,19 +106,15 @@
 
 		internal-regs {
 			serial@12000 {
-				clock-frequency = <250000000>;
 				status = "okay";
 			};
 			serial@12100 {
-				clock-frequency = <250000000>;
 				status = "okay";
 			};
 			serial@12200 {
-				clock-frequency = <250000000>;
 				status = "okay";
 			};
 			serial@12300 {
-				clock-frequency = <250000000>;
 				status = "okay";
 			};
 
diff --git a/arch/arm/boot/dts/armada-xp-gp.dts b/arch/arm/boot/dts/armada-xp-gp.dts
index 61bda687f78..0478c55ca65 100644
--- a/arch/arm/boot/dts/armada-xp-gp.dts
+++ b/arch/arm/boot/dts/armada-xp-gp.dts
@@ -59,7 +59,7 @@
 			/* Device Bus parameters are required */
 
 			/* Read parameters */
-			devbus,bus-width    = <8>;
+			devbus,bus-width    = <16>;
 			devbus,turn-off-ps  = <60000>;
 			devbus,badr-skew-ps = <0>;
 			devbus,acc-first-ps = <124000>;
@@ -104,19 +104,15 @@
 
 		internal-regs {
 			serial@12000 {
-				clock-frequency = <250000000>;
 				status = "okay";
 			};
 			serial@12100 {
-				clock-frequency = <250000000>;
 				status = "okay";
 			};
 			serial@12200 {
-				clock-frequency = <250000000>;
 				status = "okay";
 			};
 			serial@12300 {
-				clock-frequency = <250000000>;
 				status = "okay";
 			};
 
@@ -146,22 +142,22 @@
 			ethernet@70000 {
 				status = "okay";
 				phy = <&phy0>;
-				phy-mode = "rgmii-id";
+				phy-mode = "qsgmii";
 			};
 			ethernet@74000 {
 				status = "okay";
 				phy = <&phy1>;
-				phy-mode = "rgmii-id";
+				phy-mode = "qsgmii";
 			};
 			ethernet@30000 {
 				status = "okay";
 				phy = <&phy2>;
-				phy-mode = "rgmii-id";
+				phy-mode = "qsgmii";
 			};
 			ethernet@34000 {
 				status = "okay";
 				phy = <&phy3>;
-				phy-mode = "rgmii-id";
+				phy-mode = "qsgmii";
 			};
 
 			/* Front-side USB slot */
diff --git a/arch/arm/boot/dts/armada-xp-matrix.dts b/arch/arm/boot/dts/armada-xp-matrix.dts
index c2242745b9b..25674fe81f7 100644
--- a/arch/arm/boot/dts/armada-xp-matrix.dts
+++ b/arch/arm/boot/dts/armada-xp-matrix.dts
@@ -37,19 +37,15 @@
 
 		internal-regs {
 			serial@12000 {
-				clock-frequency = <250000000>;
 				status = "okay";
 			};
 			serial@12100 {
-				clock-frequency = <250000000>;
 				status = "okay";
 			};
 			serial@12200 {
-				clock-frequency = <250000000>;
 				status = "okay";
 			};
 			serial@12300 {
-				clock-frequency = <250000000>;
 				status = "okay";
 			};
 
diff --git a/arch/arm/boot/dts/armada-xp-mv78230.dtsi b/arch/arm/boot/dts/armada-xp-mv78230.dtsi
index 98335fb34b7..1257ff1ed27 100644
--- a/arch/arm/boot/dts/armada-xp-mv78230.dtsi
+++ b/arch/arm/boot/dts/armada-xp-mv78230.dtsi
@@ -27,6 +27,7 @@
 	cpus {
 		#address-cells = <1>;
 		#size-cells = <0>;
+		enable-method = "marvell,armada-xp-smp";
 
 		cpu@0 {
 			device_type = "cpu";
diff --git a/arch/arm/boot/dts/armada-xp-mv78260.dtsi b/arch/arm/boot/dts/armada-xp-mv78260.dtsi
index 9480cf891f8..3396b25b39e 100644
--- a/arch/arm/boot/dts/armada-xp-mv78260.dtsi
+++ b/arch/arm/boot/dts/armada-xp-mv78260.dtsi
@@ -29,6 +29,7 @@
 	cpus {
 		#address-cells = <1>;
 		#size-cells = <0>;
+		enable-method = "marvell,armada-xp-smp";
 
 		cpu@0 {
 			device_type = "cpu";
diff --git a/arch/arm/boot/dts/armada-xp-mv78460.dtsi b/arch/arm/boot/dts/armada-xp-mv78460.dtsi
index 31ba6d8fbad..6da84bf40aa 100644
--- a/arch/arm/boot/dts/armada-xp-mv78460.dtsi
+++ b/arch/arm/boot/dts/armada-xp-mv78460.dtsi
@@ -30,6 +30,7 @@
 	cpus {
 		#address-cells = <1>;
 		#size-cells = <0>;
+		enable-method = "marvell,armada-xp-smp";
 
 		cpu@0 {
 			device_type = "cpu";
diff --git a/arch/arm/boot/dts/armada-xp-netgear-rn2120.dts b/arch/arm/boot/dts/armada-xp-netgear-rn2120.dts
index ff049ee862e..0cf999abc4e 100644
--- a/arch/arm/boot/dts/armada-xp-netgear-rn2120.dts
+++ b/arch/arm/boot/dts/armada-xp-netgear-rn2120.dts
@@ -138,7 +138,6 @@
 			};
 
 			serial@12000 {
-				clocks = <&coreclk 0>;
 				status = "okay";
 			};
 
diff --git a/arch/arm/boot/dts/armada-xp-openblocks-ax3-4.dts b/arch/arm/boot/dts/armada-xp-openblocks-ax3-4.dts
index 985948ce67b..e5c6a0492ca 100644
--- a/arch/arm/boot/dts/armada-xp-openblocks-ax3-4.dts
+++ b/arch/arm/boot/dts/armada-xp-openblocks-ax3-4.dts
@@ -39,7 +39,7 @@
 			/* Device Bus parameters are required */
 
 			/* Read parameters */
-			devbus,bus-width    = <8>;
+			devbus,bus-width    = <16>;
 			devbus,turn-off-ps  = <60000>;
 			devbus,badr-skew-ps = <0>;
 			devbus,acc-first-ps = <124000>;
@@ -72,11 +72,9 @@
 
 		internal-regs {
 			serial@12000 {
-				clock-frequency = <250000000>;
 				status = "okay";
 			};
 			serial@12100 {
-				clock-frequency = <250000000>;
 				status = "okay";
 			};
 			pinctrl {
diff --git a/arch/arm/boot/dts/armada-xp.dtsi b/arch/arm/boot/dts/armada-xp.dtsi
index abb9f9dcc52..5902e8359c9 100644
--- a/arch/arm/boot/dts/armada-xp.dtsi
+++ b/arch/arm/boot/dts/armada-xp.dtsi
@@ -58,6 +58,7 @@
 				reg-shift = <2>;
 				interrupts = <43>;
 				reg-io-width = <1>;
+				clocks = <&coreclk 0>;
 				status = "disabled";
 			};
 			serial@12300 {
@@ -66,6 +67,7 @@
 				reg-shift = <2>;
 				interrupts = <44>;
 				reg-io-width = <1>;
+				clocks = <&coreclk 0>;
 				status = "disabled";
 			};
 
@@ -117,9 +119,9 @@
 				clock-names = "nbclk", "fixed";
 			};
 
-			armada-370-xp-pmsu@22000 {
-				compatible = "marvell,armada-370-xp-pmsu";
-				reg = <0x22100 0x400>, <0x20800 0x20>;
+			cpurst@20800 {
+				compatible = "marvell,armada-370-cpu-reset";
+				reg = <0x20800 0x20>;
 			};
 
 			eth2: ethernet@30000 {
diff --git a/arch/arm/boot/dts/at91-cosino_mega2560.dts b/arch/arm/boot/dts/at91-cosino_mega2560.dts
index a542d5837a1..27ebb0f722f 100644
--- a/arch/arm/boot/dts/at91-cosino_mega2560.dts
+++ b/arch/arm/boot/dts/at91-cosino_mega2560.dts
@@ -32,11 +32,6 @@
 				status = "okay";
 			};
 
-
-			tsadcc: tsadcc@f804c000 {
-				status = "okay";
-			};
-
 			rtc@fffffeb0 {
 				status = "okay";
 			};
diff --git a/arch/arm/boot/dts/at91-sama5d3_xplained.dts b/arch/arm/boot/dts/at91-sama5d3_xplained.dts
index ce1375595e5..5b8e40400be 100644
--- a/arch/arm/boot/dts/at91-sama5d3_xplained.dts
+++ b/arch/arm/boot/dts/at91-sama5d3_xplained.dts
@@ -21,6 +21,14 @@
 		reg = <0x20000000 0x10000000>;
 	};
 
+	slow_xtal {
+		clock-frequency = <32768>;
+	};
+
+	main_xtal {
+		clock-frequency = <12000000>;
+	};
+
 	ahb {
 		apb {
 			mmc0: mmc@f0000000 {
@@ -34,7 +42,7 @@
 			};
 
 			spi0: spi@f0004000 {
-				cs-gpios = <&pioD 13 0>;
+				cs-gpios = <&pioD 13 0>, <0>, <0>, <&pioD 16 0>;
 				status = "okay";
 			};
 
@@ -43,11 +51,54 @@
 			};
 
 			i2c0: i2c@f0014000 {
+				pinctrl-0 = <&pinctrl_i2c0_pu>;
 				status = "okay";
 			};
 
 			i2c1: i2c@f0018000 {
 				status = "okay";
+
+				pmic: act8865@5b {
+					compatible = "active-semi,act8865";
+					reg = <0x5b>;
+					status = "okay";
+
+					regulators {
+						vcc_1v8_reg: DCDC_REG1 {
+							regulator-name = "VCC_1V8";
+							regulator-min-microvolt = <1800000>;
+							regulator-max-microvolt = <1800000>;
+							regulator-always-on;
+						};
+
+						vcc_1v2_reg: DCDC_REG2 {
+							regulator-name = "VCC_1V2";
+							regulator-min-microvolt = <1200000>;
+							regulator-max-microvolt = <1200000>;
+							regulator-always-on;
+						};
+
+						vcc_3v3_reg: DCDC_REG3 {
+							regulator-name = "VCC_3V3";
+							regulator-min-microvolt = <3300000>;
+							regulator-max-microvolt = <3300000>;
+							regulator-always-on;
+						};
+
+						vddfuse_reg: LDO_REG1 {
+							regulator-name = "FUSE_2V5";
+							regulator-min-microvolt = <2500000>;
+							regulator-max-microvolt = <2500000>;
+						};
+
+						vddana_reg: LDO_REG2 {
+							regulator-name = "VDDANA";
+							regulator-min-microvolt = <3300000>;
+							regulator-max-microvolt = <3300000>;
+							regulator-always-on;
+						};
+					};
+				};
 			};
 
 			macb0: ethernet@f0028000 {
@@ -55,6 +106,12 @@
 				status = "okay";
 			};
 
+			pwm0: pwm@f002c000 {
+				pinctrl-names = "default";
+				pinctrl-0 = <&pinctrl_pwm0_pwmh0_0 &pinctrl_pwm0_pwmh1_0>;
+				status = "okay";
+			};
+
 			usart0: serial@f001c000 {
 				status = "okay";
 			};
@@ -79,7 +136,7 @@
 			};
 
 			spi1: spi@f8008000 {
-				cs-gpios = <&pioC 25 0>, <0>, <0>, <&pioD 16 0>;
+				cs-gpios = <&pioC 25 0>;
 				status = "okay";
 			};
 
@@ -102,6 +159,7 @@
 
 			i2c2: i2c@f801c000 {
 				dmas = <0>, <0>;	/* Do not use DMA for i2c2 */
+				pinctrl-0 = <&pinctrl_i2c2_pu>;
 				status = "okay";
 			};
 
@@ -116,6 +174,18 @@
 
 			pinctrl@fffff200 {
 				board {
+					pinctrl_i2c0_pu: i2c0_pu {
+						atmel,pins =
+							<AT91_PIOA 30 AT91_PERIPH_A AT91_PINCTRL_PULL_UP>,
+							<AT91_PIOA 31 AT91_PERIPH_A AT91_PINCTRL_PULL_UP>;
+					};
+
+					pinctrl_i2c2_pu: i2c2_pu {
+						atmel,pins =
+							<AT91_PIOA 18 AT91_PERIPH_B AT91_PINCTRL_PULL_UP>,
+							<AT91_PIOA 19 AT91_PERIPH_B AT91_PINCTRL_PULL_UP>;
+					};
+
 					pinctrl_mmc0_cd: mmc0_cd {
 						atmel,pins =
 							<AT91_PIOE 0 AT91_PERIPH_GPIO AT91_PINCTRL_PULL_UP_DEGLITCH>;
diff --git a/arch/arm/boot/dts/at91sam9260.dtsi b/arch/arm/boot/dts/at91sam9260.dtsi
index 366fc2cbcd6..c0e0eae16a2 100644
--- a/arch/arm/boot/dts/at91sam9260.dtsi
+++ b/arch/arm/boot/dts/at91sam9260.dtsi
@@ -641,7 +641,7 @@
 				trigger@3 {
 					reg = <3>;
 					trigger-name = "external";
-					trigger-value = <0x13>;
+					trigger-value = <0xd>;
 					trigger-external;
 				};
 			};
diff --git a/arch/arm/boot/dts/at91sam9261.dtsi b/arch/arm/boot/dts/at91sam9261.dtsi
index e21dda0e898..b309c1c6e84 100644
--- a/arch/arm/boot/dts/at91sam9261.dtsi
+++ b/arch/arm/boot/dts/at91sam9261.dtsi
@@ -10,7 +10,7 @@
 #include <dt-bindings/pinctrl/at91.h>
 #include <dt-bindings/interrupt-controller/irq.h>
 #include <dt-bindings/gpio/gpio.h>
-#include <dt-bindings/clk/at91.h>
+#include <dt-bindings/clock/at91.h>
 
 / {
 	model = "Atmel AT91SAM9261 family SoC";
@@ -29,6 +29,7 @@
 		i2c0 = &i2c0;
 		ssc0 = &ssc0;
 		ssc1 = &ssc1;
+		ssc2 = &ssc2;
 	};
 
 	cpus {
@@ -45,6 +46,18 @@
 		reg = <0x20000000 0x08000000>;
 	};
 
+	main_xtal: main_xtal {
+		compatible = "fixed-clock";
+		#clock-cells = <0>;
+		clock-frequency = <0>;
+	};
+
+	slow_xtal: slow_xtal {
+		compatible = "fixed-clock";
+		#clock-cells = <0>;
+		clock-frequency = <0>;
+	};
+
 	ahb {
 		compatible = "simple-bus";
 		#address-cells = <1>;
@@ -182,6 +195,8 @@
 				interrupts = <14 IRQ_TYPE_LEVEL_HIGH 5>;
 				pinctrl-names = "default";
 				pinctrl-0 = <&pinctrl_ssc0_tx &pinctrl_ssc0_rx>;
+				clocks = <&ssc0_clk>;
+				clock-names = "pclk";
 				status = "disabled";
 			};
 
@@ -191,6 +206,19 @@
 				interrupts = <15 IRQ_TYPE_LEVEL_HIGH 5>;
 				pinctrl-names = "default";
 				pinctrl-0 = <&pinctrl_ssc1_tx &pinctrl_ssc1_rx>;
+				clocks = <&ssc1_clk>;
+				clock-names = "pclk";
+				status = "disabled";
+			};
+
+			ssc2: ssc@fffc4000 {
+				compatible = "atmel,at91rm9200-ssc";
+				reg = <0xfffc4000 0x4000>;
+				interrupts = <16 IRQ_TYPE_LEVEL_HIGH 5>;
+				pinctrl-names = "default";
+				pinctrl-0 = <&pinctrl_ssc2_tx &pinctrl_ssc2_rx>;
+				clocks = <&ssc2_clk>;
+				clock-names = "pclk";
 				status = "disabled";
 			};
 
@@ -385,6 +413,22 @@
 					};
 				};
 
+				ssc2 {
+					pinctrl_ssc2_tx: ssc2_tx-0 {
+						atmel,pins =
+							<AT91_PIOC 25 AT91_PERIPH_B AT91_PINCTRL_NONE>,
+							<AT91_PIOC 26 AT91_PERIPH_B AT91_PINCTRL_NONE>,
+							<AT91_PIOC 27 AT91_PERIPH_B AT91_PINCTRL_NONE>;
+					};
+
+					pinctrl_ssc2_rx: ssc2_rx-0 {
+						atmel,pins =
+							<AT91_PIOC 28 AT91_PERIPH_B AT91_PINCTRL_NONE>,
+							<AT91_PIOC 29 AT91_PERIPH_B AT91_PINCTRL_NONE>,
+							<AT91_PIOC 30 AT91_PERIPH_B AT91_PINCTRL_NONE>;
+					};
+				};
+
 				spi0 {
 					pinctrl_spi0: spi0-0 {
 						atmel,pins =
@@ -524,17 +568,24 @@
 				#size-cells = <0>;
 				#interrupt-cells = <1>;
 
-				clk32k: slck {
+				slow_rc_osc: slow_rc_osc {
 					compatible = "fixed-clock";
 					#clock-cells = <0>;
 					clock-frequency = <32768>;
+					clock-accuracy = <50000000>;
+				};
+
+				clk32k: slck {
+					compatible = "atmel,at91sam9260-clk-slow";
+					#clock-cells = <0>;
+					clocks = <&slow_rc_osc &slow_xtal>;
 				};
 
 				main: mainck {
 					compatible = "atmel,at91rm9200-clk-main";
 					#clock-cells = <0>;
 					interrupts-extended = <&pmc AT91_PMC_MOSCS>;
-					clocks = <&clk32k>;
+					clocks = <&main_xtal>;
 				};
 
 				plla: pllack {
@@ -545,7 +596,8 @@
 					reg = <0>;
 					atmel,clk-input-range = <1000000 32000000>;
 					#atmel,pll-clk-output-range-cells = <4>;
-					atmel,pll-clk-output-ranges = <80000000 200000000 190000000 240000000>;
+					atmel,pll-clk-output-ranges = <80000000 200000000 0 1>,
+								<190000000 240000000 2 1>;
 				};
 
 				pllb: pllbck {
@@ -554,9 +606,9 @@
 					interrupts-extended = <&pmc AT91_PMC_LOCKB>;
 					clocks = <&main>;
 					reg = <1>;
-					atmel,clk-input-range = <1000000 32000000>;
+					atmel,clk-input-range = <1000000 5000000>;
 					#atmel,pll-clk-output-range-cells = <4>;
-					atmel,pll-clk-output-ranges = <80000000 200000000 190000000 240000000>;
+					atmel,pll-clk-output-ranges = <70000000 130000000 1 1>;
 				};
 
 				mck: masterck {
@@ -565,16 +617,48 @@
 					interrupts-extended = <&pmc AT91_PMC_MCKRDY>;
 					clocks = <&clk32k>, <&main>, <&plla>, <&pllb>;
 					atmel,clk-output-range = <0 94000000>;
-					atmel,clk-divisors = <1 2 4 3>;
+					atmel,clk-divisors = <1 2 4 0>;
 				};
 
 				usb: usbck {
 					compatible = "atmel,at91rm9200-clk-usb";
 					#clock-cells = <0>;
-					atmel,clk-divisors = <1 2 4 3>;
+					atmel,clk-divisors = <1 2 4 0>;
 					clocks = <&pllb>;
 				};
 
+				prog: progck {
+					compatible = "atmel,at91rm9200-clk-programmable";
+					#address-cells = <1>;
+					#size-cells = <0>;
+					interrupt-parent = <&pmc>;
+					clocks = <&clk32k>, <&main>, <&plla>, <&pllb>;
+
+					prog0: prog0 {
+						#clock-cells = <0>;
+						reg = <0>;
+						interrupts = <AT91_PMC_PCKRDY(0)>;
+					};
+
+					prog1: prog1 {
+						#clock-cells = <0>;
+						reg = <1>;
+						interrupts = <AT91_PMC_PCKRDY(1)>;
+					};
+
+					prog2: prog2 {
+						#clock-cells = <0>;
+						reg = <2>;
+						interrupts = <AT91_PMC_PCKRDY(2)>;
+					};
+
+					prog3: prog3 {
+						#clock-cells = <0>;
+						reg = <3>;
+						interrupts = <AT91_PMC_PCKRDY(3)>;
+					};
+				};
+
 				systemck {
 					compatible = "atmel,at91rm9200-clk-system";
 					#address-cells = <1>;
@@ -592,6 +676,30 @@
 						clocks = <&usb>;
 					};
 
+					pck0: pck0 {
+						#clock-cells = <0>;
+						reg = <8>;
+						clocks = <&prog0>;
+					};
+
+					pck1: pck1 {
+						#clock-cells = <0>;
+						reg = <9>;
+						clocks = <&prog1>;
+					};
+
+					pck2: pck2 {
+						#clock-cells = <0>;
+						reg = <10>;
+						clocks = <&prog2>;
+					};
+
+					pck3: pck3 {
+						#clock-cells = <0>;
+						reg = <11>;
+						clocks = <&prog3>;
+					};
+
 					hclk0: hclk0 {
 						#clock-cells = <0>;
 						reg = <16>;
@@ -666,6 +774,21 @@
 						reg = <13>;
 					};
 
+					ssc0_clk: ssc0_clk {
+						#clock-cells = <0>;
+						reg = <14>;
+					};
+
+					ssc1_clk: ssc1_clk {
+						#clock-cells = <0>;
+						reg = <15>;
+					};
+
+					ssc2_clk: ssc2_clk {
+						#clock-cells = <0>;
+						reg = <16>;
+					};
+
 					tc0_clk: tc0_clk {
 						#clock-cells = <0>;
 						reg = <17>;
diff --git a/arch/arm/boot/dts/at91sam9261ek.dts b/arch/arm/boot/dts/at91sam9261ek.dts
index 2ce527e70c7..c6683ea8b74 100644
--- a/arch/arm/boot/dts/at91sam9261ek.dts
+++ b/arch/arm/boot/dts/at91sam9261ek.dts
@@ -20,6 +20,10 @@
 		reg = <0x20000000 0x4000000>;
 	};
 
+	main_xtal {
+		clock-frequency = <18432000>;
+	};
+
 	clocks {
 		#address-cells = <1>;
 		#size-cells = <1>;
diff --git a/arch/arm/boot/dts/at91sam9g45.dtsi b/arch/arm/boot/dts/at91sam9g45.dtsi
index 9cdaecff13b..ace6bf197b7 100644
--- a/arch/arm/boot/dts/at91sam9g45.dtsi
+++ b/arch/arm/boot/dts/at91sam9g45.dtsi
@@ -136,6 +136,36 @@
 				      >;
 
 				/* shared pinctrl settings */
+				adc0 {
+					pinctrl_adc0_adtrg: adc0_adtrg {
+						atmel,pins = <AT91_PIOD 28 AT91_PERIPH_A AT91_PINCTRL_NONE>;
+					};
+					pinctrl_adc0_ad0: adc0_ad0 {
+						atmel,pins = <AT91_PIOD 20 AT91_PERIPH_GPIO AT91_PINCTRL_NONE>;
+					};
+					pinctrl_adc0_ad1: adc0_ad1 {
+						atmel,pins = <AT91_PIOD 21 AT91_PERIPH_GPIO AT91_PINCTRL_NONE>;
+					};
+					pinctrl_adc0_ad2: adc0_ad2 {
+						atmel,pins = <AT91_PIOD 22 AT91_PERIPH_GPIO AT91_PINCTRL_NONE>;
+					};
+					pinctrl_adc0_ad3: adc0_ad3 {
+						atmel,pins = <AT91_PIOD 23 AT91_PERIPH_GPIO AT91_PINCTRL_NONE>;
+					};
+					pinctrl_adc0_ad4: adc0_ad4 {
+						atmel,pins = <AT91_PIOD 24 AT91_PERIPH_GPIO AT91_PINCTRL_NONE>;
+					};
+					pinctrl_adc0_ad5: adc0_ad5 {
+						atmel,pins = <AT91_PIOD 25 AT91_PERIPH_GPIO AT91_PINCTRL_NONE>;
+					};
+					pinctrl_adc0_ad6: adc0_ad6 {
+						atmel,pins = <AT91_PIOD 26 AT91_PERIPH_GPIO AT91_PINCTRL_NONE>;
+					};
+					pinctrl_adc0_ad7: adc0_ad7 {
+						atmel,pins = <AT91_PIOD 27 AT91_PERIPH_GPIO AT91_PINCTRL_NONE>;
+					};
+				};
+
 				dbgu {
 					pinctrl_dbgu: dbgu-0 {
 						atmel,pins =
@@ -634,10 +664,9 @@
 			adc0: adc@fffb0000 {
 				#address-cells = <1>;
 				#size-cells = <0>;
-				compatible = "atmel,at91sam9260-adc";
+				compatible = "atmel,at91sam9g45-adc";
 				reg = <0xfffb0000 0x100>;
 				interrupts = <20 IRQ_TYPE_LEVEL_HIGH 0>;
-				atmel,adc-use-external-triggers;
 				atmel,adc-channels-used = <0xff>;
 				atmel,adc-vref = <3300>;
 				atmel,adc-startup-time = <40>;
diff --git a/arch/arm/boot/dts/at91sam9m10g45ek.dts b/arch/arm/boot/dts/at91sam9m10g45ek.dts
index 7ff665a8c70..9f5b0a67499 100644
--- a/arch/arm/boot/dts/at91sam9m10g45ek.dts
+++ b/arch/arm/boot/dts/at91sam9m10g45ek.dts
@@ -8,6 +8,7 @@
  */
 /dts-v1/;
 #include "at91sam9g45.dtsi"
+#include <dt-bindings/pwm/pwm.h>
 
 / {
 	model = "Atmel AT91SAM9M10G45-EK";
@@ -130,6 +131,21 @@
 				status = "okay";
 			};
 
+			adc0: adc@fffb0000 {
+				pinctrl-names = "default";
+				pinctrl-0 = <
+					&pinctrl_adc0_ad0
+					&pinctrl_adc0_ad1
+					&pinctrl_adc0_ad2
+					&pinctrl_adc0_ad3
+					&pinctrl_adc0_ad4
+					&pinctrl_adc0_ad5
+					&pinctrl_adc0_ad6
+					&pinctrl_adc0_ad7>;
+				atmel,adc-ts-wires = <4>;
+				status = "okay";
+			};
+
 			pwm0: pwm@fffb8000 {
 				status = "okay";
 
@@ -216,14 +232,14 @@
 
 		d6 {
 			label = "d6";
-			pwms = <&pwm0 3 5000 0>;
+			pwms = <&pwm0 3 5000 PWM_POLARITY_INVERTED>;
 			max-brightness = <255>;
 			linux,default-trigger = "nand-disk";
 		};
 
 		d7 {
 			label = "d7";
-			pwms = <&pwm0 1 5000 0>;
+			pwms = <&pwm0 1 5000 PWM_POLARITY_INVERTED>;
 			max-brightness = <255>;
 			linux,default-trigger = "mmc0";
 		};
diff --git a/arch/arm/boot/dts/at91sam9n12.dtsi b/arch/arm/boot/dts/at91sam9n12.dtsi
index 9f04808fc69..d1b82e6635d 100644
--- a/arch/arm/boot/dts/at91sam9n12.dtsi
+++ b/arch/arm/boot/dts/at91sam9n12.dtsi
@@ -12,6 +12,7 @@
 #include <dt-bindings/pinctrl/at91.h>
 #include <dt-bindings/interrupt-controller/irq.h>
 #include <dt-bindings/gpio/gpio.h>
+#include <dt-bindings/clock/at91.h>
 
 / {
 	model = "Atmel AT91SAM9N12 SoC";
@@ -49,6 +50,18 @@
 		reg = <0x20000000 0x10000000>;
 	};
 
+	slow_xtal: slow_xtal {
+		compatible = "fixed-clock";
+		#clock-cells = <0>;
+		clock-frequency = <0>;
+	};
+
+	main_xtal: main_xtal {
+		compatible = "fixed-clock";
+		#clock-cells = <0>;
+		clock-frequency = <0>;
+	};
+
 	ahb {
 		compatible = "simple-bus";
 		#address-cells = <1>;
@@ -75,8 +88,280 @@
 			};
 
 			pmc: pmc@fffffc00 {
-				compatible = "atmel,at91rm9200-pmc";
-				reg = <0xfffffc00 0x100>;
+				compatible = "atmel,at91sam9n12-pmc";
+				reg = <0xfffffc00 0x200>;
+				interrupts = <1 IRQ_TYPE_LEVEL_HIGH 7>;
+				interrupt-controller;
+				#address-cells = <1>;
+				#size-cells = <0>;
+				#interrupt-cells = <1>;
+
+				main_rc_osc: main_rc_osc {
+					compatible = "atmel,at91sam9x5-clk-main-rc-osc";
+					#clock-cells = <0>;
+					interrupts-extended = <&pmc AT91_PMC_MOSCRCS>;
+					clock-frequency = <12000000>;
+					clock-accuracy = <50000000>;
+				};
+
+				main_osc: main_osc {
+					compatible = "atmel,at91rm9200-clk-main-osc";
+					#clock-cells = <0>;
+					interrupts-extended = <&pmc AT91_PMC_MOSCS>;
+					clocks = <&main_xtal>;
+				};
+
+				main: mainck {
+					compatible = "atmel,at91sam9x5-clk-main";
+					#clock-cells = <0>;
+					interrupts-extended = <&pmc AT91_PMC_MOSCSELS>;
+					clocks = <&main_rc_osc>, <&main_osc>;
+				};
+
+				plla: pllack {
+					compatible = "atmel,at91rm9200-clk-pll";
+					#clock-cells = <0>;
+					interrupts-extended = <&pmc AT91_PMC_LOCKA>;
+					clocks = <&main>;
+					reg = <0>;
+					atmel,clk-input-range = <2000000 32000000>;
+					#atmel,pll-clk-output-range-cells = <4>;
+					atmel,pll-clk-output-ranges = <745000000 800000000 0 0>,
+								      <695000000 750000000 1 0>,
+								      <645000000 700000000 2 0>,
+								      <595000000 650000000 3 0>,
+								      <545000000 600000000 0 1>,
+								      <495000000 555000000 1 1>,
+								      <445000000 500000000 1 2>,
+								      <400000000 450000000 1 3>;
+				};
+
+				plladiv: plladivck {
+					compatible = "atmel,at91sam9x5-clk-plldiv";
+					#clock-cells = <0>;
+					clocks = <&plla>;
+				};
+
+				pllb: pllbck {
+					compatible = "atmel,at91rm9200-clk-pll";
+					#clock-cells = <0>;
+					interrupts-extended = <&pmc AT91_PMC_LOCKB>;
+					clocks = <&main>;
+					reg = <1>;
+					atmel,clk-input-range = <2000000 32000000>;
+					#atmel,pll-clk-output-range-cells = <3>;
+					atmel,pll-clk-output-ranges = <30000000 100000000 0>;
+				};
+
+				mck: masterck {
+					compatible = "atmel,at91sam9x5-clk-master";
+					#clock-cells = <0>;
+					interrupts-extended = <&pmc AT91_PMC_MCKRDY>;
+					clocks = <&clk32k>, <&main>, <&plladiv>, <&pllb>;
+					atmel,clk-output-range = <0 133333333>;
+					atmel,clk-divisors = <1 2 4 3>;
+					atmel,master-clk-have-div3-pres;
+				};
+
+				usb: usbck {
+					compatible = "atmel,at91sam9n12-clk-usb";
+					#clock-cells = <0>;
+					clocks = <&pllb>;
+				};
+
+				prog: progck {
+					compatible = "atmel,at91sam9x5-clk-programmable";
+					#address-cells = <1>;
+					#size-cells = <0>;
+					interrupt-parent = <&pmc>;
+					clocks = <&clk32k>, <&main>, <&plladiv>, <&pllb>, <&mck>;
+
+					prog0: prog0 {
+						#clock-cells = <0>;
+						reg = <0>;
+						interrupts = <AT91_PMC_PCKRDY(0)>;
+					};
+
+					prog1: prog1 {
+						#clock-cells = <0>;
+						reg = <1>;
+						interrupts = <AT91_PMC_PCKRDY(1)>;
+					};
+				};
+
+				systemck {
+					compatible = "atmel,at91rm9200-clk-system";
+					#address-cells = <1>;
+					#size-cells = <0>;
+
+					ddrck: ddrck {
+						#clock-cells = <0>;
+						reg = <2>;
+						clocks = <&mck>;
+					};
+
+					lcdck: lcdck {
+						#clock-cells = <0>;
+						reg = <3>;
+						clocks = <&mck>;
+					};
+
+					uhpck: uhpck {
+						#clock-cells = <0>;
+						reg = <6>;
+						clocks = <&usb>;
+					};
+
+					udpck: udpck {
+						#clock-cells = <0>;
+						reg = <7>;
+						clocks = <&usb>;
+					};
+
+					pck0: pck0 {
+						#clock-cells = <0>;
+						reg = <8>;
+						clocks = <&prog0>;
+					};
+
+					pck1: pck1 {
+						#clock-cells = <0>;
+						reg = <9>;
+						clocks = <&prog1>;
+					};
+				};
+
+				periphck {
+					compatible = "atmel,at91sam9x5-clk-peripheral";
+					#address-cells = <1>;
+					#size-cells = <0>;
+					clocks = <&mck>;
+
+					pioAB_clk: pioAB_clk {
+						#clock-cells = <0>;
+						reg = <2>;
+					};
+
+					pioCD_clk: pioCD_clk {
+						#clock-cells = <0>;
+						reg = <3>;
+					};
+
+					fuse_clk: fuse_clk {
+						#clock-cells = <0>;
+						reg = <4>;
+					};
+
+					usart0_clk: usart0_clk {
+						#clock-cells = <0>;
+						reg = <5>;
+					};
+
+					usart1_clk: usart1_clk {
+						#clock-cells = <0>;
+						reg = <6>;
+					};
+
+					usart2_clk: usart2_clk {
+						#clock-cells = <0>;
+						reg = <7>;
+					};
+
+					usart3_clk: usart3_clk {
+						#clock-cells = <0>;
+						reg = <8>;
+					};
+
+					twi0_clk: twi0_clk {
+						reg = <9>;
+						#clock-cells = <0>;
+					};
+
+					twi1_clk: twi1_clk {
+						#clock-cells = <0>;
+						reg = <10>;
+					};
+
+					mci0_clk: mci0_clk {
+						#clock-cells = <0>;
+						reg = <12>;
+					};
+
+					spi0_clk: spi0_clk {
+						#clock-cells = <0>;
+						reg = <13>;
+					};
+
+					spi1_clk: spi1_clk {
+						#clock-cells = <0>;
+						reg = <14>;
+					};
+
+					uart0_clk: uart0_clk {
+						#clock-cells = <0>;
+						reg = <15>;
+					};
+
+					uart1_clk: uart1_clk {
+						#clock-cells = <0>;
+						reg = <16>;
+					};
+
+					tcb_clk: tcb_clk {
+						#clock-cells = <0>;
+						reg = <17>;
+					};
+
+					pwm_clk: pwm_clk {
+						#clock-cells = <0>;
+						reg = <18>;
+					};
+
+					adc_clk: adc_clk {
+						#clock-cells = <0>;
+						reg = <19>;
+					};
+
+					dma0_clk: dma0_clk {
+						#clock-cells = <0>;
+						reg = <20>;
+					};
+
+					uhphs_clk: uhphs_clk {
+						#clock-cells = <0>;
+						reg = <22>;
+					};
+
+					udphs_clk: udphs_clk {
+						#clock-cells = <0>;
+						reg = <23>;
+					};
+
+					lcdc_clk: lcdc_clk {
+						#clock-cells = <0>;
+						reg = <25>;
+					};
+
+					sha_clk: sha_clk {
+						#clock-cells = <0>;
+						reg = <27>;
+					};
+
+					ssc0_clk: ssc0_clk {
+						#clock-cells = <0>;
+						reg = <28>;
+					};
+
+					aes_clk: aes_clk {
+						#clock-cells = <0>;
+						reg = <29>;
+					};
+
+					trng_clk: trng_clk {
+						#clock-cells = <0>;
+						reg = <30>;
+					};
+				};
 			};
 
 			rstc@fffffe00 {
@@ -88,6 +373,7 @@
 				compatible = "atmel,at91sam9260-pit";
 				reg = <0xfffffe30 0xf>;
 				interrupts = <1 IRQ_TYPE_LEVEL_HIGH 7>;
+				clocks = <&mck>;
 			};
 
 			shdwc@fffffe10 {
@@ -95,12 +381,38 @@
 				reg = <0xfffffe10 0x10>;
 			};
 
+			sckc@fffffe50 {
+				compatible = "atmel,at91sam9x5-sckc";
+				reg = <0xfffffe50 0x4>;
+
+				slow_osc: slow_osc {
+					compatible = "atmel,at91sam9x5-clk-slow-osc";
+					#clock-cells = <0>;
+					clocks = <&slow_xtal>;
+				};
+
+				slow_rc_osc: slow_rc_osc {
+					compatible = "atmel,at91sam9x5-clk-slow-rc-osc";
+					#clock-cells = <0>;
+					clock-frequency = <32768>;
+					clock-accuracy = <50000000>;
+				};
+
+				clk32k: slck {
+					compatible = "atmel,at91sam9x5-clk-slow";
+					#clock-cells = <0>;
+					clocks = <&slow_rc_osc>, <&slow_osc>;
+				};
+			};
+
 			mmc0: mmc@f0008000 {
 				compatible = "atmel,hsmci";
 				reg = <0xf0008000 0x600>;
 				interrupts = <12 IRQ_TYPE_LEVEL_HIGH 0>;
 				dmas = <&dma 1 AT91_DMA_CFG_PER_ID(0)>;
 				dma-names = "rxtx";
+				clocks = <&mci0_clk>;
+				clock-names = "mci_clk";
 				#address-cells = <1>;
 				#size-cells = <0>;
 				status = "disabled";
@@ -110,12 +422,16 @@
 				compatible = "atmel,at91sam9x5-tcb";
 				reg = <0xf8008000 0x100>;
 				interrupts = <17 IRQ_TYPE_LEVEL_HIGH 0>;
+				clocks = <&tcb_clk>;
+				clock-names = "t0_clk";
 			};
 
 			tcb1: timer@f800c000 {
 				compatible = "atmel,at91sam9x5-tcb";
 				reg = <0xf800c000 0x100>;
 				interrupts = <17 IRQ_TYPE_LEVEL_HIGH 0>;
+				clocks = <&tcb_clk>;
+				clock-names = "t0_clk";
 			};
 
 			dma: dma-controller@ffffec00 {
@@ -123,6 +439,8 @@
 				reg = <0xffffec00 0x200>;
 				interrupts = <20 IRQ_TYPE_LEVEL_HIGH 0>;
 				#dma-cells = <2>;
+				clocks = <&dma0_clk>;
+				clock-names = "dma_clk";
 			};
 
 			pinctrl@fffff400 {
@@ -392,6 +710,7 @@
 					gpio-controller;
 					interrupt-controller;
 					#interrupt-cells = <2>;
+					clocks = <&pioAB_clk>;
 				};
 
 				pioB: gpio@fffff600 {
@@ -402,6 +721,7 @@
 					gpio-controller;
 					interrupt-controller;
 					#interrupt-cells = <2>;
+					clocks = <&pioAB_clk>;
 				};
 
 				pioC: gpio@fffff800 {
@@ -412,6 +732,7 @@
 					gpio-controller;
 					interrupt-controller;
 					#interrupt-cells = <2>;
+					clocks = <&pioCD_clk>;
 				};
 
 				pioD: gpio@fffffa00 {
@@ -422,6 +743,7 @@
 					gpio-controller;
 					interrupt-controller;
 					#interrupt-cells = <2>;
+					clocks = <&pioCD_clk>;
 				};
 			};
 
@@ -431,6 +753,8 @@
 				interrupts = <1 IRQ_TYPE_LEVEL_HIGH 7>;
 				pinctrl-names = "default";
 				pinctrl-0 = <&pinctrl_dbgu>;
+				clocks = <&mck>;
+				clock-names = "usart";
 				status = "disabled";
 			};
 
@@ -443,6 +767,8 @@
 				dma-names = "tx", "rx";
 				pinctrl-names = "default";
 				pinctrl-0 = <&pinctrl_ssc0_tx &pinctrl_ssc0_rx>;
+				clocks = <&ssc0_clk>;
+				clock-names = "pclk";
 				status = "disabled";
 			};
 
@@ -452,6 +778,8 @@
 				interrupts = <5 IRQ_TYPE_LEVEL_HIGH 5>;
 				pinctrl-names = "default";
 				pinctrl-0 = <&pinctrl_usart0>;
+				clocks = <&usart0_clk>;
+				clock-names = "usart";
 				status = "disabled";
 			};
 
@@ -461,6 +789,8 @@
 				interrupts = <6 IRQ_TYPE_LEVEL_HIGH 5>;
 				pinctrl-names = "default";
 				pinctrl-0 = <&pinctrl_usart1>;
+				clocks = <&usart1_clk>;
+				clock-names = "usart";
 				status = "disabled";
 			};
 
@@ -470,6 +800,8 @@
 				interrupts = <7 IRQ_TYPE_LEVEL_HIGH 5>;
 				pinctrl-names = "default";
 				pinctrl-0 = <&pinctrl_usart2>;
+				clocks = <&usart2_clk>;
+				clock-names = "usart";
 				status = "disabled";
 			};
 
@@ -479,6 +811,8 @@
 				interrupts = <8 IRQ_TYPE_LEVEL_HIGH 5>;
 				pinctrl-names = "default";
 				pinctrl-0 = <&pinctrl_usart3>;
+				clocks = <&usart3_clk>;
+				clock-names = "usart";
 				status = "disabled";
 			};
 
@@ -493,6 +827,7 @@
 				#size-cells = <0>;
 				pinctrl-names = "default";
 				pinctrl-0 = <&pinctrl_i2c0>;
+				clocks = <&twi0_clk>;
 				status = "disabled";
 			};
 
@@ -507,6 +842,7 @@
 				#size-cells = <0>;
 				pinctrl-names = "default";
 				pinctrl-0 = <&pinctrl_i2c1>;
+				clocks = <&twi1_clk>;
 				status = "disabled";
 			};
 
@@ -521,6 +857,8 @@
 				dma-names = "tx", "rx";
 				pinctrl-names = "default";
 				pinctrl-0 = <&pinctrl_spi0>;
+				clocks = <&spi0_clk>;
+				clock-names = "spi_clk";
 				status = "disabled";
 			};
 
@@ -535,6 +873,8 @@
 				dma-names = "tx", "rx";
 				pinctrl-names = "default";
 				pinctrl-0 = <&pinctrl_spi1>;
+				clocks = <&spi1_clk>;
+				clock-names = "spi_clk";
 				status = "disabled";
 			};
 
@@ -554,6 +894,7 @@
 				reg = <0xf8034000 0x300>;
 				interrupts = <18 IRQ_TYPE_LEVEL_HIGH 4>;
 				#pwm-cells = <3>;
+				clocks = <&pwm_clk>;
 				status = "disabled";
 			};
 		};
@@ -584,6 +925,9 @@
 			compatible = "atmel,at91rm9200-ohci", "usb-ohci";
 			reg = <0x00500000 0x00100000>;
 			interrupts = <22 IRQ_TYPE_LEVEL_HIGH 2>;
+			clocks = <&usb>, <&uhphs_clk>, <&udphs_clk>,
+				 <&uhpck>;
+			clock-names = "usb_clk", "ohci_clk", "hclk", "uhpck";
 			status = "disabled";
 		};
 	};
diff --git a/arch/arm/boot/dts/at91sam9n12ek.dts b/arch/arm/boot/dts/at91sam9n12ek.dts
index 924a6a6ffd0..64bbe46e4f9 100644
--- a/arch/arm/boot/dts/at91sam9n12ek.dts
+++ b/arch/arm/boot/dts/at91sam9n12ek.dts
@@ -21,6 +21,14 @@
 		reg = <0x20000000 0x8000000>;
 	};
 
+	slow_xtal {
+		clock-frequency = <32768>;
+	};
+
+	main_xtal {
+		clock-frequency = <16000000>;
+	};
+
 	clocks {
 		#address-cells = <1>;
 		#size-cells = <1>;
diff --git a/arch/arm/boot/dts/at91sam9rl.dtsi b/arch/arm/boot/dts/at91sam9rl.dtsi
index 63e1784d272..1da183155ee 100644
--- a/arch/arm/boot/dts/at91sam9rl.dtsi
+++ b/arch/arm/boot/dts/at91sam9rl.dtsi
@@ -8,9 +8,10 @@
 
 #include "skeleton.dtsi"
 #include <dt-bindings/pinctrl/at91.h>
-#include <dt-bindings/clk/at91.h>
+#include <dt-bindings/clock/at91.h>
 #include <dt-bindings/interrupt-controller/irq.h>
 #include <dt-bindings/gpio/gpio.h>
+#include <dt-bindings/pwm/pwm.h>
 
 / {
 	model = "Atmel AT91SAM9RL family SoC";
@@ -32,6 +33,7 @@
 		i2c1 = &i2c1;
 		ssc0 = &ssc0;
 		ssc1 = &ssc1;
+		pwm0 = &pwm0;
 	};
 
 	cpus {
@@ -48,12 +50,43 @@
 		reg = <0x20000000 0x04000000>;
 	};
 
+	slow_xtal: slow_xtal {
+		compatible = "fixed-clock";
+		#clock-cells = <0>;
+		clock-frequency = <0>;
+	};
+
+	main_xtal: main_xtal {
+		compatible = "fixed-clock";
+		#clock-cells = <0>;
+		clock-frequency = <0>;
+	};
+
+	clocks {
+		adc_op_clk: adc_op_clk{
+			compatible = "fixed-clock";
+			#clock-cells = <0>;
+			clock-frequency = <1000000>;
+		};
+	};
+
 	ahb {
 		compatible = "simple-bus";
 		#address-cells = <1>;
 		#size-cells = <1>;
 		ranges;
 
+		fb0: fb@00500000 {
+			compatible = "atmel,at91sam9rl-lcdc";
+			reg = <0x00500000 0x1000>;
+			interrupts = <23 IRQ_TYPE_LEVEL_HIGH 3>;
+			pinctrl-names = "default";
+			pinctrl-0 = <&pinctrl_fb>;
+			clocks = <&lcd_clk>, <&lcd_clk>;
+			clock-names = "hclk", "lcdc_clk";
+			status = "disabled";
+		};
+
 		nand0: nand@40000000 {
 			compatible = "atmel,at91rm9200-nand";
 			#address-cells = <1>;
@@ -187,6 +220,16 @@
 				status = "disabled";
 			};
 
+			pwm0: pwm@fffc8000 {
+				compatible = "atmel,at91sam9rl-pwm";
+				reg = <0xfffc8000 0x300>;
+				interrupts = <19 IRQ_TYPE_LEVEL_HIGH 4>;
+				#pwm-cells = <3>;
+				clocks = <&pwm_clk>;
+				clock-names = "pwm_clk";
+				status = "disabled";
+			};
+
 			spi0: spi@fffcc000 {
 				#address-cells = <1>;
 				#size-cells = <0>;
@@ -200,6 +243,111 @@
 				status = "disabled";
 			};
 
+			adc0: adc@fffd0000 {
+				#address-cells = <1>;
+				#size-cells = <0>;
+				compatible = "atmel,at91sam9rl-adc";
+				reg = <0xfffd0000 0x100>;
+				interrupts = <20 IRQ_TYPE_LEVEL_HIGH 0>;
+				clocks = <&adc_clk>, <&adc_op_clk>;
+				clock-names = "adc_clk", "adc_op_clk";
+				atmel,adc-use-external-triggers;
+				atmel,adc-channels-used = <0x3f>;
+				atmel,adc-vref = <3300>;
+				atmel,adc-startup-time = <40>;
+				atmel,adc-res = <8 10>;
+				atmel,adc-res-names = "lowres", "highres";
+				atmel,adc-use-res = "highres";
+
+				trigger@0 {
+					reg = <0>;
+					trigger-name = "timer-counter-0";
+					trigger-value = <0x1>;
+				};
+				trigger@1 {
+					reg = <1>;
+					trigger-name = "timer-counter-1";
+					trigger-value = <0x3>;
+				};
+
+				trigger@2 {
+					reg = <2>;
+					trigger-name = "timer-counter-2";
+					trigger-value = <0x5>;
+				};
+
+				trigger@3 {
+					reg = <3>;
+					trigger-name = "external";
+					trigger-value = <0x13>;
+					trigger-external;
+				};
+			};
+
+			usb0: gadget@fffd4000 {
+				#address-cells = <1>;
+				#size-cells = <0>;
+				compatible = "atmel,at91sam9rl-udc";
+				reg = <0x00600000 0x100000>,
+				      <0xfffd4000 0x4000>;
+				interrupts = <22 IRQ_TYPE_LEVEL_HIGH 2>;
+				clocks = <&udphs_clk>, <&utmi>;
+				clock-names = "pclk", "hclk";
+				status = "disabled";
+
+				ep0 {
+					reg = <0>;
+					atmel,fifo-size = <64>;
+					atmel,nb-banks = <1>;
+				};
+
+				ep1 {
+					reg = <1>;
+					atmel,fifo-size = <1024>;
+					atmel,nb-banks = <2>;
+					atmel,can-dma;
+					atmel,can-isoc;
+				};
+
+				ep2 {
+					reg = <2>;
+					atmel,fifo-size = <1024>;
+					atmel,nb-banks = <2>;
+					atmel,can-dma;
+					atmel,can-isoc;
+				};
+
+				ep3 {
+					reg = <3>;
+					atmel,fifo-size = <1024>;
+					atmel,nb-banks = <3>;
+					atmel,can-dma;
+				};
+
+				ep4 {
+					reg = <4>;
+					atmel,fifo-size = <1024>;
+					atmel,nb-banks = <3>;
+					atmel,can-dma;
+				};
+
+				ep5 {
+					reg = <5>;
+					atmel,fifo-size = <1024>;
+					atmel,nb-banks = <3>;
+					atmel,can-dma;
+					atmel,can-isoc;
+				};
+
+				ep6 {
+					reg = <6>;
+					atmel,fifo-size = <1024>;
+					atmel,nb-banks = <3>;
+					atmel,can-dma;
+					atmel,can-isoc;
+				};
+			};
+
 			ramc0: ramc@ffffea00 {
 				compatible = "atmel,at91sam9260-sdramc";
 				reg = <0xffffea00 0x200>;
@@ -238,6 +386,44 @@
 					<0x003fffff 0x0001ff3c>;  /* pioD */
 
 				/* shared pinctrl settings */
+				adc0 {
+					pinctrl_adc0_ts: adc0_ts-0 {
+						atmel,pins =
+							<AT91_PIOA 17 AT91_PERIPH_A AT91_PINCTRL_NONE>,
+							<AT91_PIOA 18 AT91_PERIPH_A AT91_PINCTRL_NONE>,
+							<AT91_PIOA 19 AT91_PERIPH_A AT91_PINCTRL_NONE>,
+							<AT91_PIOA 20 AT91_PERIPH_A AT91_PINCTRL_NONE>;
+					};
+
+					pinctrl_adc0_ad0: adc0_ad0-0 {
+						atmel,pins = <AT91_PIOA 17 AT91_PERIPH_A AT91_PINCTRL_NONE>;
+					};
+
+					pinctrl_adc0_ad1: adc0_ad1-0 {
+						atmel,pins = <AT91_PIOA 18 AT91_PERIPH_A AT91_PINCTRL_NONE>;
+					};
+
+					pinctrl_adc0_ad2: adc0_ad2-0 {
+						atmel,pins = <AT91_PIOA 19 AT91_PERIPH_A AT91_PINCTRL_NONE>;
+					};
+
+					pinctrl_adc0_ad3: adc0_ad3-0 {
+						atmel,pins = <AT91_PIOA 20 AT91_PERIPH_A AT91_PINCTRL_NONE>;
+					};
+
+					pinctrl_adc0_ad4: adc0_ad4-0 {
+						atmel,pins = <AT91_PIOD 6 AT91_PERIPH_A AT91_PINCTRL_NONE>;
+					};
+
+					pinctrl_adc0_ad5: adc0_ad5-0 {
+						atmel,pins = <AT91_PIOD 7 AT91_PERIPH_A AT91_PINCTRL_NONE>;
+					};
+
+					pinctrl_adc0_adtrg: adc0_adtrg-0 {
+						atmel,pins = <AT91_PIOB 15 AT91_PERIPH_A AT91_PINCTRL_NONE>;
+					};
+				};
+
 				dbgu {
 					pinctrl_dbgu: dbgu-0 {
 						atmel,pins =
@@ -246,6 +432,33 @@
 					};
 				};
 
+				fb {
+					pinctrl_fb: fb-0 {
+						atmel,pins =
+							<AT91_PIOC 1 AT91_PERIPH_B AT91_PINCTRL_NONE>,
+							<AT91_PIOC 3 AT91_PERIPH_A AT91_PINCTRL_NONE>,
+							<AT91_PIOC 5 AT91_PERIPH_A AT91_PINCTRL_NONE>,
+							<AT91_PIOC 6 AT91_PERIPH_A AT91_PINCTRL_NONE>,
+							<AT91_PIOC 7 AT91_PERIPH_A AT91_PINCTRL_NONE>,
+							<AT91_PIOC 9 AT91_PERIPH_B AT91_PINCTRL_NONE>,
+							<AT91_PIOC 10 AT91_PERIPH_B AT91_PINCTRL_NONE>,
+							<AT91_PIOC 11 AT91_PERIPH_B AT91_PINCTRL_NONE>,
+							<AT91_PIOC 12 AT91_PERIPH_B AT91_PINCTRL_NONE>,
+							<AT91_PIOC 13 AT91_PERIPH_B AT91_PINCTRL_NONE>,
+							<AT91_PIOC 15 AT91_PERIPH_B AT91_PINCTRL_NONE>,
+							<AT91_PIOC 16 AT91_PERIPH_B AT91_PINCTRL_NONE>,
+							<AT91_PIOC 17 AT91_PERIPH_B AT91_PINCTRL_NONE>,
+							<AT91_PIOC 18 AT91_PERIPH_B AT91_PINCTRL_NONE>,
+							<AT91_PIOC 19 AT91_PERIPH_B AT91_PINCTRL_NONE>,
+							<AT91_PIOC 20 AT91_PERIPH_B AT91_PINCTRL_NONE>,
+							<AT91_PIOC 21 AT91_PERIPH_B AT91_PINCTRL_NONE>,
+							<AT91_PIOC 22 AT91_PERIPH_B AT91_PINCTRL_NONE>,
+							<AT91_PIOC 23 AT91_PERIPH_B AT91_PINCTRL_NONE>,
+							<AT91_PIOC 24 AT91_PERIPH_B AT91_PINCTRL_NONE>,
+							<AT91_PIOC 25 AT91_PERIPH_B AT91_PINCTRL_NONE>;
+					};
+				};
+
 				i2c_gpio0 {
 					pinctrl_i2c_gpio0: i2c_gpio0-0 {
 						atmel,pins =
@@ -307,6 +520,61 @@
 					};
 				};
 
+				pwm0 {
+					pinctrl_pwm0_pwm0_0: pwm0_pwm0-0 {
+						atmel,pins = <AT91_PIOB 8 AT91_PERIPH_B AT91_PINCTRL_NONE>;
+					};
+
+					pinctrl_pwm0_pwm0_1: pwm0_pwm0-1 {
+						atmel,pins = <AT91_PIOC 2 AT91_PERIPH_B AT91_PINCTRL_NONE>;
+					};
+
+					pinctrl_pwm0_pwm0_2: pwm0_pwm0-2 {
+						atmel,pins = <AT91_PIOD 14 AT91_PERIPH_B AT91_PINCTRL_NONE>;
+					};
+
+					pinctrl_pwm0_pwm1_0: pwm0_pwm1-0 {
+						atmel,pins = <AT91_PIOB 9 AT91_PERIPH_B AT91_PINCTRL_NONE>;
+					};
+
+					pinctrl_pwm0_pwm1_1: pwm0_pwm1-1 {
+						atmel,pins = <AT91_PIOC 3 AT91_PERIPH_B AT91_PINCTRL_NONE>;
+					};
+
+					pinctrl_pwm0_pwm1_2: pwm0_pwm1-2 {
+						atmel,pins = <AT91_PIOD 15 AT91_PERIPH_B AT91_PINCTRL_NONE>;
+					};
+
+					pinctrl_pwm0_pwm2_0: pwm0_pwm2-0 {
+						atmel,pins = <AT91_PIOD 5 AT91_PERIPH_B AT91_PINCTRL_NONE>;
+					};
+
+					pinctrl_pwm0_pwm2_1: pwm0_pwm2-1 {
+						atmel,pins = <AT91_PIOD 12 AT91_PERIPH_A AT91_PINCTRL_NONE>;
+					};
+
+					pinctrl_pwm0_pwm2_2: pwm0_pwm2-2 {
+						atmel,pins = <AT91_PIOD 16 AT91_PERIPH_B AT91_PINCTRL_NONE>;
+					};
+
+					pinctrl_pwm0_pwm3_0: pwm0_pwm3-0 {
+						atmel,pins = <AT91_PIOD 8 AT91_PERIPH_B AT91_PINCTRL_NONE>;
+					};
+
+					pinctrl_pwm0_pwm3_1: pwm0_pwm3-1 {
+						atmel,pins = <AT91_PIOD 18 AT91_PERIPH_A AT91_PINCTRL_NONE>;
+					};
+				};
+
+				spi0 {
+					pinctrl_spi0: spi0-0 {
+						atmel,pins =
+							<AT91_PIOA 25 AT91_PERIPH_A AT91_PINCTRL_NONE>,
+							<AT91_PIOA 26 AT91_PERIPH_A AT91_PINCTRL_NONE>,
+							<AT91_PIOA 27 AT91_PERIPH_A AT91_PINCTRL_NONE>;
+					};
+				};
+
 				ssc0 {
 					pinctrl_ssc0_tx: ssc0_tx-0 {
 						atmel,pins =
@@ -339,15 +607,6 @@
 					};
 				};
 
-				spi0 {
-					pinctrl_spi0: spi0-0 {
-						atmel,pins =
-							<AT91_PIOA 25 AT91_PERIPH_A AT91_PINCTRL_NONE>,
-							<AT91_PIOA 26 AT91_PERIPH_A AT91_PINCTRL_NONE>,
-							<AT91_PIOA 27 AT91_PERIPH_A AT91_PINCTRL_NONE>;
-					};
-				};
-
 				tcb0 {
 					pinctrl_tcb0_tclk0: tcb0_tclk0-0 {
 						atmel,pins = <AT91_PIOA 3 AT91_PERIPH_B AT91_PINCTRL_NONE>;
@@ -548,17 +807,11 @@
 				#size-cells = <0>;
 				#interrupt-cells = <1>;
 
-				clk32k: slck {
-					compatible = "fixed-clock";
-					#clock-cells = <0>;
-					clock-frequency = <32768>;
-				};
-
 				main: mainck {
 					compatible = "atmel,at91rm9200-clk-main";
 					#clock-cells = <0>;
 					interrupts-extended = <&pmc AT91_PMC_MOSCS>;
-					clocks = <&clk32k>;
+					clocks = <&main_xtal>;
 				};
 
 				plla: pllack {
@@ -568,8 +821,9 @@
 					clocks = <&main>;
 					reg = <0>;
 					atmel,clk-input-range = <1000000 32000000>;
-					#atmel,pll-clk-output-range-cells = <4>;
-					atmel,pll-clk-output-ranges = <80000000 200000000 190000000 240000000>;
+					#atmel,pll-clk-output-range-cells = <3>;
+					atmel,pll-clk-output-ranges = <80000000 200000000 0>,
+								<190000000 240000000 2>;
 				};
 
 				utmi: utmick {
@@ -586,7 +840,7 @@
 					interrupts-extended = <&pmc AT91_PMC_MCKRDY>;
 					clocks = <&clk32k>, <&main>, <&plla>, <&utmi>;
 					atmel,clk-output-range = <0 94000000>;
-					atmel,clk-divisors = <1 2 4 3>;
+					atmel,clk-divisors = <1 2 4 0>;
 				};
 
 				prog: progck {
@@ -769,6 +1023,32 @@
 				interrupts = <1 IRQ_TYPE_LEVEL_HIGH 7>;
 				status = "disabled";
 			};
+
+			sckc@fffffd50 {
+				compatible = "atmel,at91sam9x5-sckc";
+				reg = <0xfffffd50 0x4>;
+
+				slow_osc: slow_osc {
+					compatible = "atmel,at91sam9x5-clk-slow-osc";
+					#clock-cells = <0>;
+					atmel,startup-time-usec = <1200000>;
+					clocks = <&slow_xtal>;
+				};
+
+				slow_rc_osc: slow_rc_osc {
+					compatible = "atmel,at91sam9x5-clk-slow-rc-osc";
+					#clock-cells = <0>;
+					atmel,startup-time-usec = <75>;
+					clock-frequency = <32768>;
+					clock-accuracy = <50000000>;
+				};
+
+				clk32k: slck {
+					compatible = "atmel,at91sam9x5-clk-slow";
+					#clock-cells = <0>;
+					clocks = <&slow_rc_osc &slow_osc>;
+				};
+			};
 		};
 	};
 
diff --git a/arch/arm/boot/dts/at91sam9rlek.dts b/arch/arm/boot/dts/at91sam9rlek.dts
index cddb37825fa..d4a010e40fe 100644
--- a/arch/arm/boot/dts/at91sam9rlek.dts
+++ b/arch/arm/boot/dts/at91sam9rlek.dts
@@ -20,6 +20,15 @@
 		reg = <0x20000000 0x4000000>;
 	};
 
+
+	slow_xtal {
+		clock-frequency = <32768>;
+	};
+
+	main_xtal {
+		clock-frequency = <12000000>;
+	};
+
 	clocks {
 		#address-cells = <1>;
 		#size-cells = <1>;
@@ -32,6 +41,37 @@
 	};
 
 	ahb {
+		fb0: fb@00500000 {
+			display = <&display0>;
+			status = "okay";
+
+			display0: display {
+				bits-per-pixel = <16>;
+				atmel,lcdcon-backlight;
+				atmel,dmacon = <0x1>;
+				atmel,lcdcon2 = <0x80008002>;
+				atmel,guard-time = <1>;
+				atmel,lcd-wiring-mode = "RGB";
+
+				display-timings {
+					native-mode = <&timing0>;
+					timing0: timing0 {
+						clock-frequency = <4965000>;
+						hactive = <240>;
+						vactive = <320>;
+						hback-porch = <1>;
+						hfront-porch = <33>;
+						vback-porch = <1>;
+						vfront-porch = <0>;
+						hsync-len = <5>;
+						vsync-len = <1>;
+						hsync-active = <1>;
+						vsync-active = <1>;
+					};
+				};
+			};
+		};
+
 		nand0: nand@40000000 {
 			nand-bus-width = <8>;
 			nand-ecc-mode = "soft";
@@ -92,6 +132,43 @@
 				status = "okay";
 			};
 
+			adc0: adc@fffd0000 {
+				pinctrl-names = "default";
+				pinctrl-0 = <
+					&pinctrl_adc0_ad0
+					&pinctrl_adc0_ad1
+					&pinctrl_adc0_ad2
+					&pinctrl_adc0_ad3
+					&pinctrl_adc0_ad4
+					&pinctrl_adc0_ad5
+					&pinctrl_adc0_adtrg>;
+				atmel,adc-ts-wires = <4>;
+				status = "okay";
+			};
+
+			usb0: gadget@fffd4000 {
+				atmel,vbus-gpio = <&pioA 8 GPIO_ACTIVE_HIGH>;
+				status = "okay";
+			};
+
+			spi0: spi@fffcc000 {
+				status = "okay";
+				cs-gpios = <&pioA 28 0>, <0>, <0>, <0>;
+				mtd_dataflash@0 {
+					compatible = "atmel,at45", "atmel,dataflash";
+					spi-max-frequency = <15000000>;
+					reg = <0>;
+				};
+			};
+
+			pwm0: pwm@fffc8000 {
+				status = "okay";
+
+				pinctrl-names = "default";
+				pinctrl-0 = <&pinctrl_pwm0_pwm1_2>,
+					<&pinctrl_pwm0_pwm2_2>;
+			};
+
 			dbgu: serial@fffff200 {
 				status = "okay";
 			};
@@ -117,18 +194,24 @@
 		};
 	};
 
-	leds {
-		compatible = "gpio-leds";
+	pwmleds {
+		compatible = "pwm-leds";
 
 		ds1 {
 			label = "ds1";
-			gpios = <&pioD 15 GPIO_ACTIVE_LOW>;
+			pwms = <&pwm0 1 5000 PWM_POLARITY_INVERTED>;
+			max-brightness = <255>;
 		};
 
 		ds2 {
 			label = "ds2";
-			gpios = <&pioD 16 GPIO_ACTIVE_LOW>;
+			pwms = <&pwm0 2 5000 PWM_POLARITY_INVERTED>;
+			max-brightness = <255>;
 		};
+	};
+
+	leds {
+		compatible = "gpio-leds";
 
 		ds3 {
 			label = "ds3";
@@ -154,4 +237,12 @@
 			gpio-key,wakeup;
 		};
 	};
+
+	i2c@0 {
+		status = "okay";
+	};
+
+	i2c@1 {
+		status = "okay";
+	};
 };
diff --git a/arch/arm/boot/dts/at91sam9x5.dtsi b/arch/arm/boot/dts/at91sam9x5.dtsi
index fc13c9240da..1a57298636a 100644
--- a/arch/arm/boot/dts/at91sam9x5.dtsi
+++ b/arch/arm/boot/dts/at91sam9x5.dtsi
@@ -14,6 +14,7 @@
 #include <dt-bindings/pinctrl/at91.h>
 #include <dt-bindings/interrupt-controller/irq.h>
 #include <dt-bindings/gpio/gpio.h>
+#include <dt-bindings/clock/at91.h>
 
 / {
 	model = "Atmel AT91SAM9x5 family SoC";
@@ -51,6 +52,24 @@
 		reg = <0x20000000 0x10000000>;
 	};
 
+	slow_xtal: slow_xtal {
+		compatible = "fixed-clock";
+		#clock-cells = <0>;
+		clock-frequency = <0>;
+	};
+
+	main_xtal: main_xtal {
+		compatible = "fixed-clock";
+		#clock-cells = <0>;
+		clock-frequency = <0>;
+	};
+
+	adc_op_clk: adc_op_clk{
+		compatible = "fixed-clock";
+		#clock-cells = <0>;
+		clock-frequency = <5000000>;
+	};
+
 	ahb {
 		compatible = "simple-bus";
 		#address-cells = <1>;
@@ -77,8 +96,272 @@
 			};
 
 			pmc: pmc@fffffc00 {
-				compatible = "atmel,at91rm9200-pmc";
+				compatible = "atmel,at91sam9x5-pmc";
 				reg = <0xfffffc00 0x100>;
+				interrupts = <1 IRQ_TYPE_LEVEL_HIGH 7>;
+				interrupt-controller;
+				#address-cells = <1>;
+				#size-cells = <0>;
+				#interrupt-cells = <1>;
+
+				main_rc_osc: main_rc_osc {
+					compatible = "atmel,at91sam9x5-clk-main-rc-osc";
+					#clock-cells = <0>;
+					interrupts-extended = <&pmc AT91_PMC_MOSCRCS>;
+					clock-frequency = <12000000>;
+					clock-accuracy = <50000000>;
+				};
+
+				main_osc: main_osc {
+					compatible = "atmel,at91rm9200-clk-main-osc";
+					#clock-cells = <0>;
+					interrupts-extended = <&pmc AT91_PMC_MOSCS>;
+					clocks = <&main_xtal>;
+				};
+
+				main: mainck {
+					compatible = "atmel,at91sam9x5-clk-main";
+					#clock-cells = <0>;
+					interrupts-extended = <&pmc AT91_PMC_MOSCSELS>;
+					clocks = <&main_rc_osc>, <&main_osc>;
+				};
+
+				plla: pllack {
+					compatible = "atmel,at91rm9200-clk-pll";
+					#clock-cells = <0>;
+					interrupts-extended = <&pmc AT91_PMC_LOCKA>;
+					clocks = <&main>;
+					reg = <0>;
+					atmel,clk-input-range = <2000000 32000000>;
+					#atmel,pll-clk-output-range-cells = <4>;
+					atmel,pll-clk-output-ranges = <745000000 800000000 0 0
+								       695000000 750000000 1 0
+								       645000000 700000000 2 0
+								       595000000 650000000 3 0
+								       545000000 600000000 0 1
+								       495000000 555000000 1 1
+								       445000000 500000000 1 2
+								       400000000 450000000 1 3>;
+				};
+
+				plladiv: plladivck {
+					compatible = "atmel,at91sam9x5-clk-plldiv";
+					#clock-cells = <0>;
+					clocks = <&plla>;
+				};
+
+				utmi: utmick {
+					compatible = "atmel,at91sam9x5-clk-utmi";
+					#clock-cells = <0>;
+					interrupts-extended = <&pmc AT91_PMC_LOCKU>;
+					clocks = <&main>;
+				};
+
+				mck: masterck {
+					compatible = "atmel,at91sam9x5-clk-master";
+					#clock-cells = <0>;
+					interrupts-extended = <&pmc AT91_PMC_MCKRDY>;
+					clocks = <&clk32k>, <&main>, <&plladiv>, <&utmi>;
+					atmel,clk-output-range = <0 133333333>;
+					atmel,clk-divisors = <1 2 4 3>;
+					atmel,master-clk-have-div3-pres;
+				};
+
+				usb: usbck {
+					compatible = "atmel,at91sam9x5-clk-usb";
+					#clock-cells = <0>;
+					clocks = <&plladiv>, <&utmi>;
+				};
+
+				prog: progck {
+					compatible = "atmel,at91sam9x5-clk-programmable";
+					#address-cells = <1>;
+					#size-cells = <0>;
+					interrupt-parent = <&pmc>;
+					clocks = <&clk32k>, <&main>, <&plladiv>, <&utmi>, <&mck>;
+
+					prog0: prog0 {
+						#clock-cells = <0>;
+						reg = <0>;
+						interrupts = <AT91_PMC_PCKRDY(0)>;
+					};
+
+					prog1: prog1 {
+						#clock-cells = <0>;
+						reg = <1>;
+						interrupts = <AT91_PMC_PCKRDY(1)>;
+					};
+				};
+
+				smd: smdclk {
+					compatible = "atmel,at91sam9x5-clk-smd";
+					#clock-cells = <0>;
+					clocks = <&plladiv>, <&utmi>;
+				};
+
+				systemck {
+					compatible = "atmel,at91rm9200-clk-system";
+					#address-cells = <1>;
+					#size-cells = <0>;
+
+					ddrck: ddrck {
+						#clock-cells = <0>;
+						reg = <2>;
+						clocks = <&mck>;
+					};
+
+					smdck: smdck {
+						#clock-cells = <0>;
+						reg = <4>;
+						clocks = <&smd>;
+					};
+
+					uhpck: uhpck {
+						#clock-cells = <0>;
+						reg = <6>;
+						clocks = <&usb>;
+					};
+
+					udpck: udpck {
+						#clock-cells = <0>;
+						reg = <7>;
+						clocks = <&usb>;
+					};
+
+					pck0: pck0 {
+						#clock-cells = <0>;
+						reg = <8>;
+						clocks = <&prog0>;
+					};
+
+					pck1: pck1 {
+						#clock-cells = <0>;
+						reg = <9>;
+						clocks = <&prog1>;
+					};
+				};
+
+				periphck {
+					compatible = "atmel,at91sam9x5-clk-peripheral";
+					#address-cells = <1>;
+					#size-cells = <0>;
+					clocks = <&mck>;
+
+					pioAB_clk: pioAB_clk {
+						#clock-cells = <0>;
+						reg = <2>;
+					};
+
+					pioCD_clk: pioCD_clk {
+						#clock-cells = <0>;
+						reg = <3>;
+					};
+
+					smd_clk: smd_clk {
+						#clock-cells = <0>;
+						reg = <4>;
+					};
+
+					usart0_clk: usart0_clk {
+						#clock-cells = <0>;
+						reg = <5>;
+					};
+
+					usart1_clk: usart1_clk {
+						#clock-cells = <0>;
+						reg = <6>;
+					};
+
+					usart2_clk: usart2_clk {
+						#clock-cells = <0>;
+						reg = <7>;
+					};
+
+					twi0_clk: twi0_clk {
+						reg = <9>;
+						#clock-cells = <0>;
+					};
+
+					twi1_clk: twi1_clk {
+						#clock-cells = <0>;
+						reg = <10>;
+					};
+
+					twi2_clk: twi2_clk {
+						#clock-cells = <0>;
+						reg = <11>;
+					};
+
+					mci0_clk: mci0_clk {
+						#clock-cells = <0>;
+						reg = <12>;
+					};
+
+					spi0_clk: spi0_clk {
+						#clock-cells = <0>;
+						reg = <13>;
+					};
+
+					spi1_clk: spi1_clk {
+						#clock-cells = <0>;
+						reg = <14>;
+					};
+
+					uart0_clk: uart0_clk {
+						#clock-cells = <0>;
+						reg = <15>;
+					};
+
+					uart1_clk: uart1_clk {
+						#clock-cells = <0>;
+						reg = <16>;
+					};
+
+					tcb0_clk: tcb0_clk {
+						#clock-cells = <0>;
+						reg = <17>;
+					};
+
+					pwm_clk: pwm_clk {
+						#clock-cells = <0>;
+						reg = <18>;
+					};
+
+					adc_clk: adc_clk {
+						#clock-cells = <0>;
+						reg = <19>;
+					};
+
+					dma0_clk: dma0_clk {
+						#clock-cells = <0>;
+						reg = <20>;
+					};
+
+					dma1_clk: dma1_clk {
+						#clock-cells = <0>;
+						reg = <21>;
+					};
+
+					uhphs_clk: uhphs_clk {
+						#clock-cells = <0>;
+						reg = <22>;
+					};
+
+					udphs_clk: udphs_clk {
+						#clock-cells = <0>;
+						reg = <23>;
+					};
+
+					mci1_clk: mci1_clk {
+						#clock-cells = <0>;
+						reg = <26>;
+					};
+
+					ssc0_clk: ssc0_clk {
+						#clock-cells = <0>;
+						reg = <28>;
+					};
+				};
 			};
 
 			rstc@fffffe00 {
@@ -95,18 +378,47 @@
 				compatible = "atmel,at91sam9260-pit";
 				reg = <0xfffffe30 0xf>;
 				interrupts = <1 IRQ_TYPE_LEVEL_HIGH 7>;
+				clocks = <&mck>;
+			};
+
+			sckc@fffffe50 {
+				compatible = "atmel,at91sam9x5-sckc";
+				reg = <0xfffffe50 0x4>;
+
+				slow_osc: slow_osc {
+					compatible = "atmel,at91sam9x5-clk-slow-osc";
+					#clock-cells = <0>;
+					clocks = <&slow_xtal>;
+				};
+
+				slow_rc_osc: slow_rc_osc {
+					compatible = "atmel,at91sam9x5-clk-slow-rc-osc";
+					#clock-cells = <0>;
+					clock-frequency = <32768>;
+					clock-accuracy = <50000000>;
+				};
+
+				clk32k: slck {
+					compatible = "atmel,at91sam9x5-clk-slow";
+					#clock-cells = <0>;
+					clocks = <&slow_rc_osc>, <&slow_osc>;
+				};
 			};
 
 			tcb0: timer@f8008000 {
 				compatible = "atmel,at91sam9x5-tcb";
 				reg = <0xf8008000 0x100>;
 				interrupts = <17 IRQ_TYPE_LEVEL_HIGH 0>;
+				clocks = <&tcb0_clk>;
+				clock-names = "t0_clk";
 			};
 
 			tcb1: timer@f800c000 {
 				compatible = "atmel,at91sam9x5-tcb";
 				reg = <0xf800c000 0x100>;
 				interrupts = <17 IRQ_TYPE_LEVEL_HIGH 0>;
+				clocks = <&tcb0_clk>;
+				clock-names = "t0_clk";
 			};
 
 			dma0: dma-controller@ffffec00 {
@@ -114,6 +426,8 @@
 				reg = <0xffffec00 0x200>;
 				interrupts = <20 IRQ_TYPE_LEVEL_HIGH 0>;
 				#dma-cells = <2>;
+				clocks = <&dma0_clk>;
+				clock-names = "dma_clk";
 			};
 
 			dma1: dma-controller@ffffee00 {
@@ -121,6 +435,8 @@
 				reg = <0xffffee00 0x200>;
 				interrupts = <21 IRQ_TYPE_LEVEL_HIGH 0>;
 				#dma-cells = <2>;
+				clocks = <&dma1_clk>;
+				clock-names = "dma_clk";
 			};
 
 			pinctrl@fffff400 {
@@ -453,6 +769,7 @@
 					gpio-controller;
 					interrupt-controller;
 					#interrupt-cells = <2>;
+					clocks = <&pioAB_clk>;
 				};
 
 				pioB: gpio@fffff600 {
@@ -464,6 +781,7 @@
 					#gpio-lines = <19>;
 					interrupt-controller;
 					#interrupt-cells = <2>;
+					clocks = <&pioAB_clk>;
 				};
 
 				pioC: gpio@fffff800 {
@@ -474,6 +792,7 @@
 					gpio-controller;
 					interrupt-controller;
 					#interrupt-cells = <2>;
+					clocks = <&pioCD_clk>;
 				};
 
 				pioD: gpio@fffffa00 {
@@ -485,6 +804,7 @@
 					#gpio-lines = <22>;
 					interrupt-controller;
 					#interrupt-cells = <2>;
+					clocks = <&pioCD_clk>;
 				};
 			};
 
@@ -497,6 +817,8 @@
 				dma-names = "tx", "rx";
 				pinctrl-names = "default";
 				pinctrl-0 = <&pinctrl_ssc0_tx &pinctrl_ssc0_rx>;
+				clocks = <&ssc0_clk>;
+				clock-names = "pclk";
 				status = "disabled";
 			};
 
@@ -507,6 +829,8 @@
 				dmas = <&dma0 1 AT91_DMA_CFG_PER_ID(0)>;
 				dma-names = "rxtx";
 				pinctrl-names = "default";
+				clocks = <&mci0_clk>;
+				clock-names = "mci_clk";
 				#address-cells = <1>;
 				#size-cells = <0>;
 				status = "disabled";
@@ -519,6 +843,8 @@
 				dmas = <&dma1 1 AT91_DMA_CFG_PER_ID(0)>;
 				dma-names = "rxtx";
 				pinctrl-names = "default";
+				clocks = <&mci1_clk>;
+				clock-names = "mci_clk";
 				#address-cells = <1>;
 				#size-cells = <0>;
 				status = "disabled";
@@ -530,6 +856,8 @@
 				interrupts = <1 IRQ_TYPE_LEVEL_HIGH 7>;
 				pinctrl-names = "default";
 				pinctrl-0 = <&pinctrl_dbgu>;
+				clocks = <&mck>;
+				clock-names = "usart";
 				status = "disabled";
 			};
 
@@ -539,6 +867,8 @@
 				interrupts = <5 IRQ_TYPE_LEVEL_HIGH 5>;
 				pinctrl-names = "default";
 				pinctrl-0 = <&pinctrl_usart0>;
+				clocks = <&usart0_clk>;
+				clock-names = "usart";
 				status = "disabled";
 			};
 
@@ -548,6 +878,8 @@
 				interrupts = <6 IRQ_TYPE_LEVEL_HIGH 5>;
 				pinctrl-names = "default";
 				pinctrl-0 = <&pinctrl_usart1>;
+				clocks = <&usart1_clk>;
+				clock-names = "usart";
 				status = "disabled";
 			};
 
@@ -557,6 +889,8 @@
 				interrupts = <7 IRQ_TYPE_LEVEL_HIGH 5>;
 				pinctrl-names = "default";
 				pinctrl-0 = <&pinctrl_usart2>;
+				clocks = <&usart2_clk>;
+				clock-names = "usart";
 				status = "disabled";
 			};
 
@@ -571,6 +905,7 @@
 				#size-cells = <0>;
 				pinctrl-names = "default";
 				pinctrl-0 = <&pinctrl_i2c0>;
+				clocks = <&twi0_clk>;
 				status = "disabled";
 			};
 
@@ -585,6 +920,7 @@
 				#size-cells = <0>;
 				pinctrl-names = "default";
 				pinctrl-0 = <&pinctrl_i2c1>;
+				clocks = <&twi1_clk>;
 				status = "disabled";
 			};
 
@@ -599,6 +935,7 @@
 				#size-cells = <0>;
 				pinctrl-names = "default";
 				pinctrl-0 = <&pinctrl_i2c2>;
+				clocks = <&twi2_clk>;
 				status = "disabled";
 			};
 
@@ -608,6 +945,8 @@
 				interrupts = <15 IRQ_TYPE_LEVEL_HIGH 5>;
 				pinctrl-names = "default";
 				pinctrl-0 = <&pinctrl_uart0>;
+				clocks = <&uart0_clk>;
+				clock-names = "usart";
 				status = "disabled";
 			};
 
@@ -617,6 +956,8 @@
 				interrupts = <16 IRQ_TYPE_LEVEL_HIGH 5>;
 				pinctrl-names = "default";
 				pinctrl-0 = <&pinctrl_uart1>;
+				clocks = <&uart1_clk>;
+				clock-names = "usart";
 				status = "disabled";
 			};
 
@@ -626,6 +967,9 @@
 				compatible = "atmel,at91sam9260-adc";
 				reg = <0xf804c000 0x100>;
 				interrupts = <19 IRQ_TYPE_LEVEL_HIGH 0>;
+				clocks = <&adc_clk>,
+					 <&adc_op_clk>;
+				clock-names = "adc_clk", "adc_op_clk";
 				atmel,adc-use-external-triggers;
 				atmel,adc-channels-used = <0xffff>;
 				atmel,adc-vref = <3300>;
@@ -673,6 +1017,8 @@
 				dma-names = "tx", "rx";
 				pinctrl-names = "default";
 				pinctrl-0 = <&pinctrl_spi0>;
+				clocks = <&spi0_clk>;
+				clock-names = "spi_clk";
 				status = "disabled";
 			};
 
@@ -687,6 +1033,8 @@
 				dma-names = "tx", "rx";
 				pinctrl-names = "default";
 				pinctrl-0 = <&pinctrl_spi1>;
+				clocks = <&spi1_clk>;
+				clock-names = "spi_clk";
 				status = "disabled";
 			};
 
@@ -805,6 +1153,9 @@
 			compatible = "atmel,at91rm9200-ohci", "usb-ohci";
 			reg = <0x00600000 0x100000>;
 			interrupts = <22 IRQ_TYPE_LEVEL_HIGH 2>;
+			clocks = <&usb>, <&uhphs_clk>, <&udphs_clk>,
+				 <&uhpck>;
+			clock-names = "usb_clk", "ohci_clk", "hclk", "uhpck";
 			status = "disabled";
 		};
 
@@ -812,6 +1163,8 @@
 			compatible = "atmel,at91sam9g45-ehci", "usb-ehci";
 			reg = <0x00700000 0x100000>;
 			interrupts = <22 IRQ_TYPE_LEVEL_HIGH 2>;
+			clocks = <&usb>, <&uhphs_clk>, <&uhpck>;
+			clock-names = "usb_clk", "ehci_clk", "uhpck";
 			status = "disabled";
 		};
 	};
diff --git a/arch/arm/boot/dts/at91sam9x5_can.dtsi b/arch/arm/boot/dts/at91sam9x5_can.dtsi
new file mode 100644
index 00000000000..f44ab7702a1
--- /dev/null
+++ b/arch/arm/boot/dts/at91sam9x5_can.dtsi
@@ -0,0 +1,31 @@
+/*
+ * at91sam9x5_macb0.dtsi - Device Tree Include file for AT91SAM9x5 SoC with 1
+ * Ethernet interface.
+ *
+ * Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com>
+ *
+ * Licensed under GPLv2.
+ */
+
+#include <dt-bindings/pinctrl/at91.h>
+#include <dt-bindings/interrupt-controller/irq.h>
+
+/ {
+	ahb {
+		apb {
+			pmc: pmc@fffffc00 {
+				periphck {
+					can0_clk: can0_clk {
+						#clock-cells = <0>;
+						reg = <29>;
+					};
+
+                                        can1_clk: can1_clk {
+                                                #clock-cells = <0>;
+                                                reg = <30>;
+                                        };
+				};
+			};
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/at91sam9x5_isi.dtsi b/arch/arm/boot/dts/at91sam9x5_isi.dtsi
new file mode 100644
index 00000000000..98bc877a68e
--- /dev/null
+++ b/arch/arm/boot/dts/at91sam9x5_isi.dtsi
@@ -0,0 +1,26 @@
+/*
+ * at91sam9x5_isi.dtsi - Device Tree Include file for AT91SAM9x5 SoC with an
+ * Image Sensor Interface.
+ *
+ * Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com>
+ *
+ * Licensed under GPLv2.
+ */
+
+#include <dt-bindings/pinctrl/at91.h>
+#include <dt-bindings/interrupt-controller/irq.h>
+
+/ {
+	ahb {
+		apb {
+			pmc: pmc@fffffc00 {
+				periphck {
+					isi_clk: isi_clk {
+						#clock-cells = <0>;
+						reg = <25>;
+					};
+				};
+			};
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/at91sam9x5_lcd.dtsi b/arch/arm/boot/dts/at91sam9x5_lcd.dtsi
new file mode 100644
index 00000000000..485302e8233
--- /dev/null
+++ b/arch/arm/boot/dts/at91sam9x5_lcd.dtsi
@@ -0,0 +1,26 @@
+/*
+ * at91sam9x5_lcd.dtsi - Device Tree Include file for AT91SAM9x5 SoC with an
+ * LCD controller.
+ *
+ * Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com>
+ *
+ * Licensed under GPLv2.
+ */
+
+#include <dt-bindings/pinctrl/at91.h>
+#include <dt-bindings/interrupt-controller/irq.h>
+
+/ {
+	ahb {
+		apb {
+			pmc: pmc@fffffc00 {
+				periphck {
+					lcdc_clk: lcdc_clk {
+						#clock-cells = <0>;
+						reg = <25>;
+					};
+				};
+			};
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/at91sam9x5_macb0.dtsi b/arch/arm/boot/dts/at91sam9x5_macb0.dtsi
index 55731ffba76..57e89d1d032 100644
--- a/arch/arm/boot/dts/at91sam9x5_macb0.dtsi
+++ b/arch/arm/boot/dts/at91sam9x5_macb0.dtsi
@@ -43,12 +43,23 @@
 				};
 			};
 
+			pmc: pmc@fffffc00 {
+				periphck {
+					macb0_clk: macb0_clk {
+						#clock-cells = <0>;
+						reg = <24>;
+					};
+				};
+			};
+
 			macb0: ethernet@f802c000 {
 				compatible = "cdns,at32ap7000-macb", "cdns,macb";
 				reg = <0xf802c000 0x100>;
 				interrupts = <24 IRQ_TYPE_LEVEL_HIGH 3>;
 				pinctrl-names = "default";
 				pinctrl-0 = <&pinctrl_macb0_rmii>;
+				clocks = <&macb0_clk>, <&macb0_clk>;
+				clock-names = "hclk", "pclk";
 				status = "disabled";
 			};
 		};
diff --git a/arch/arm/boot/dts/at91sam9x5_macb1.dtsi b/arch/arm/boot/dts/at91sam9x5_macb1.dtsi
index 77425a627a9..663676c0286 100644
--- a/arch/arm/boot/dts/at91sam9x5_macb1.dtsi
+++ b/arch/arm/boot/dts/at91sam9x5_macb1.dtsi
@@ -31,12 +31,23 @@
 				};
 			};
 
+			pmc: pmc@fffffc00 {
+				periphck {
+					macb1_clk: macb1_clk {
+						#clock-cells = <0>;
+						reg = <27>;
+					};
+				};
+			};
+
 			macb1: ethernet@f8030000 {
 				compatible = "cdns,at32ap7000-macb", "cdns,macb";
 				reg = <0xf8030000 0x100>;
 				interrupts = <27 IRQ_TYPE_LEVEL_HIGH 3>;
 				pinctrl-names = "default";
 				pinctrl-0 = <&pinctrl_macb1_rmii>;
+				clocks = <&macb1_clk>, <&macb1_clk>;
+				clock-names = "hclk", "pclk";
 				status = "disabled";
 			};
 		};
diff --git a/arch/arm/boot/dts/at91sam9x5_usart3.dtsi b/arch/arm/boot/dts/at91sam9x5_usart3.dtsi
index 6801106fa1f..140217a5438 100644
--- a/arch/arm/boot/dts/at91sam9x5_usart3.dtsi
+++ b/arch/arm/boot/dts/at91sam9x5_usart3.dtsi
@@ -42,12 +42,23 @@
 				};
 			};
 
+			pmc: pmc@fffffc00 {
+				periphck {
+					usart3_clk: usart3_clk {
+						#clock-cells = <0>;
+						reg = <8>;
+					};
+				};
+			};
+
 			usart3: serial@f8028000 {
 				compatible = "atmel,at91sam9260-usart";
 				reg = <0xf8028000 0x200>;
 				interrupts = <8 IRQ_TYPE_LEVEL_HIGH 5>;
 				pinctrl-names = "default";
 				pinctrl-0 = <&pinctrl_usart3>;
+				clocks = <&usart3_clk>;
+				clock-names = "usart";
 				status = "disabled";
 			};
 		};
diff --git a/arch/arm/boot/dts/at91sam9x5cm.dtsi b/arch/arm/boot/dts/at91sam9x5cm.dtsi
index 4a5ee5cc115..8413e21192e 100644
--- a/arch/arm/boot/dts/at91sam9x5cm.dtsi
+++ b/arch/arm/boot/dts/at91sam9x5cm.dtsi
@@ -23,6 +23,14 @@
 		};
 	};
 
+	slow_xtal {
+		clock-frequency = <32768>;
+	};
+
+	main_xtal {
+		clock-frequency = <12000000>;
+	};
+
 	ahb {
 		apb {
 			pinctrl@fffff400 {
diff --git a/arch/arm/boot/dts/atlas6.dtsi b/arch/arm/boot/dts/atlas6.dtsi
index 9d72674049d..bb22842a082 100644
--- a/arch/arm/boot/dts/atlas6.dtsi
+++ b/arch/arm/boot/dts/atlas6.dtsi
@@ -39,6 +39,11 @@
 		};
 	};
 
+	arm-pmu {
+		compatible = "arm,cortex-a9-pmu";
+		interrupts = <29>;
+	};
+
 	axi {
 		compatible = "simple-bus";
 		#address-cells = <1>;
@@ -167,6 +172,7 @@
 				compatible = "sirf,prima2-dspif";
 				reg = <0xa8000000 0x10000>;
 				interrupts = <9>;
+				resets = <&rstc 1>;
 			};
 
 			gps@a8010000 {
@@ -174,6 +180,7 @@
 				reg = <0xa8010000 0x10000>;
 				interrupts = <7>;
 				clocks = <&clks 9>;
+				resets = <&rstc 2>;
 			};
 
 			dsp@a9000000 {
@@ -181,6 +188,7 @@
 				reg = <0xa9000000 0x1000000>;
 				interrupts = <8>;
 				clocks = <&clks 8>;
+				resets = <&rstc 0>;
 			};
 		};
 
@@ -195,6 +203,7 @@
 				compatible = "sirf,prima2-tick";
 				reg = <0xb0020000 0x1000>;
 				interrupts = <0>;
+				clocks = <&clks 11>;
 			};
 
 			nand@b0030000 {
@@ -297,9 +306,9 @@
 				reg = <0xb00d0000 0x10000>;
 				interrupts = <15>;
 				sirf,spi-num-chipselects = <1>;
-				cs-gpios = <&gpio 0 0>;
-				sirf,spi-dma-rx-channel = <25>;
-				sirf,spi-dma-tx-channel = <20>;
+				dmas = <&dmac1 9>,
+				     <&dmac1 4>;
+				dma-names = "rx", "tx";
 				#address-cells = <1>;
 				#size-cells = <0>;
 				clocks = <&clks 19>;
@@ -312,8 +321,9 @@
 				reg = <0xb0170000 0x10000>;
 				interrupts = <16>;
 				sirf,spi-num-chipselects = <1>;
-				sirf,spi-dma-rx-channel = <12>;
-				sirf,spi-dma-tx-channel = <13>;
+				dmas = <&dmac0 12>,
+				     <&dmac0 13>;
+				dma-names = "rx", "tx";
 				#address-cells = <1>;
 				#size-cells = <0>;
 				clocks = <&clks 20>;
@@ -554,6 +564,18 @@
                                                 sirf,function = "usp0_uart_nostreamctrl";
                                         };
                                 };
+				usp0_only_utfs_pins_a: usp0@2 {
+					usp0 {
+						sirf,pins = "usp0_only_utfs_grp";
+						sirf,function = "usp0_only_utfs";
+					};
+				};
+				usp0_only_urfs_pins_a: usp0@3 {
+					usp0 {
+						sirf,pins = "usp0_only_urfs_grp";
+						sirf,function = "usp0_only_urfs";
+					};
+				};
                                 usp1_pins_a: usp1@0 {
                                         usp1 {
                                                 sirf,pins = "usp1grp";
diff --git a/arch/arm/boot/dts/axm5516-amarillo.dts b/arch/arm/boot/dts/axm5516-amarillo.dts
new file mode 100644
index 00000000000..a9d60471d9f
--- /dev/null
+++ b/arch/arm/boot/dts/axm5516-amarillo.dts
@@ -0,0 +1,51 @@
+/*
+ * arch/arm/boot/dts/axm5516-amarillo.dts
+ *
+ * Copyright (C) 2013 LSI
+ *
+ * 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.
+ */
+
+/dts-v1/;
+
+/memreserve/ 0x00000000 0x00400000;
+
+#include "axm55xx.dtsi"
+#include "axm5516-cpus.dtsi"
+
+/ {
+	model = "Amarillo AXM5516";
+	compatible = "lsi,axm5516-amarillo", "lsi,axm5516";
+
+	memory {
+		device_type = "memory";
+		reg = <0 0x00000000 0x02 0x00000000>;
+	};
+};
+
+&serial0 {
+	status = "okay";
+};
+
+&serial1 {
+	status = "okay";
+};
+
+&serial2 {
+	status = "okay";
+};
+
+&serial3 {
+	status = "okay";
+};
+
+&gpio0 {
+	status = "okay";
+};
+
+&gpio1 {
+	status = "okay";
+};
diff --git a/arch/arm/boot/dts/axm5516-cpus.dtsi b/arch/arm/boot/dts/axm5516-cpus.dtsi
new file mode 100644
index 00000000000..b85f360cb12
--- /dev/null
+++ b/arch/arm/boot/dts/axm5516-cpus.dtsi
@@ -0,0 +1,204 @@
+/*
+ * arch/arm/boot/dts/axm5516-cpus.dtsi
+ *
+ * Copyright (C) 2013 LSI
+ *
+ * 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.
+ */
+
+/ {
+	cpus {
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		cpu-map {
+			cluster0 {
+				core0 {
+					cpu = <&CPU0>;
+				};
+				core1 {
+					cpu = <&CPU1>;
+				};
+				core2 {
+					cpu = <&CPU2>;
+				};
+				core3 {
+					cpu = <&CPU3>;
+				};
+			};
+			cluster1 {
+				core0 {
+					cpu = <&CPU4>;
+				};
+				core1 {
+					cpu = <&CPU5>;
+				};
+				core2 {
+					cpu = <&CPU6>;
+				};
+				core3 {
+					cpu = <&CPU7>;
+				};
+			};
+			cluster2 {
+				core0 {
+					cpu = <&CPU8>;
+				};
+				core1 {
+					cpu = <&CPU9>;
+				};
+				core2 {
+					cpu = <&CPU10>;
+				};
+				core3 {
+					cpu = <&CPU11>;
+				};
+			};
+			cluster3 {
+				core0 {
+					cpu = <&CPU12>;
+				};
+				core1 {
+					cpu = <&CPU13>;
+				};
+				core2 {
+					cpu = <&CPU14>;
+				};
+				core3 {
+					cpu = <&CPU15>;
+				};
+			};
+		};
+
+		CPU0: cpu@0 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a15";
+			reg = <0x00>;
+			clock-frequency= <1400000000>;
+			cpu-release-addr = <0>; // Fixed by the boot loader
+		};
+
+		CPU1: cpu@1 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a15";
+			reg = <0x01>;
+			clock-frequency= <1400000000>;
+			cpu-release-addr = <0>; // Fixed by the boot loader
+		};
+
+		CPU2: cpu@2 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a15";
+			reg = <0x02>;
+			clock-frequency= <1400000000>;
+			cpu-release-addr = <0>; // Fixed by the boot loader
+		};
+
+		CPU3: cpu@3 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a15";
+			reg = <0x03>;
+			clock-frequency= <1400000000>;
+			cpu-release-addr = <0>; // Fixed by the boot loader
+		};
+
+		CPU4: cpu@100 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a15";
+			reg = <0x100>;
+			clock-frequency= <1400000000>;
+			cpu-release-addr = <0>; // Fixed by the boot loader
+		};
+
+		CPU5: cpu@101 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a15";
+			reg = <0x101>;
+			clock-frequency= <1400000000>;
+			cpu-release-addr = <0>; // Fixed by the boot loader
+		};
+
+		CPU6: cpu@102 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a15";
+			reg = <0x102>;
+			clock-frequency= <1400000000>;
+			cpu-release-addr = <0>; // Fixed by the boot loader
+		};
+
+		CPU7: cpu@103 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a15";
+			reg = <0x103>;
+			clock-frequency= <1400000000>;
+			cpu-release-addr = <0>; // Fixed by the boot loader
+		};
+
+		CPU8: cpu@200 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a15";
+			reg = <0x200>;
+			clock-frequency= <1400000000>;
+			cpu-release-addr = <0>; // Fixed by the boot loader
+		};
+
+		CPU9: cpu@201 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a15";
+			reg = <0x201>;
+			clock-frequency= <1400000000>;
+			cpu-release-addr = <0>; // Fixed by the boot loader
+		};
+
+		CPU10: cpu@202 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a15";
+			reg = <0x202>;
+			clock-frequency= <1400000000>;
+			cpu-release-addr = <0>; // Fixed by the boot loader
+		};
+
+		CPU11: cpu@203 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a15";
+			reg = <0x203>;
+			clock-frequency= <1400000000>;
+			cpu-release-addr = <0>; // Fixed by the boot loader
+		};
+
+		CPU12: cpu@300 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a15";
+			reg = <0x300>;
+			clock-frequency= <1400000000>;
+			cpu-release-addr = <0>; // Fixed by the boot loader
+		};
+
+		CPU13: cpu@301 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a15";
+			reg = <0x301>;
+			clock-frequency= <1400000000>;
+			cpu-release-addr = <0>; // Fixed by the boot loader
+		};
+
+		CPU14: cpu@302 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a15";
+			reg = <0x302>;
+			clock-frequency= <1400000000>;
+			cpu-release-addr = <0>; // Fixed by the boot loader
+		};
+
+		CPU15: cpu@303 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a15";
+			reg = <0x303>;
+			clock-frequency= <1400000000>;
+			cpu-release-addr = <0>; // Fixed by the boot loader
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/axm55xx.dtsi b/arch/arm/boot/dts/axm55xx.dtsi
new file mode 100644
index 00000000000..ea288f0a1d3
--- /dev/null
+++ b/arch/arm/boot/dts/axm55xx.dtsi
@@ -0,0 +1,204 @@
+/*
+ * arch/arm/boot/dts/axm55xx.dtsi
+ *
+ * Copyright (C) 2013 LSI
+ *
+ * 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 <dt-bindings/interrupt-controller/arm-gic.h>
+#include <dt-bindings/clock/lsi,axm5516-clks.h>
+
+#include "skeleton64.dtsi"
+
+/ {
+	interrupt-parent = <&gic>;
+
+	aliases {
+		serial0	  = &serial0;
+		serial1   = &serial1;
+		serial2	  = &serial2;
+		serial3	  = &serial3;
+		timer	  = &timer0;
+	};
+
+	clocks {
+		compatible = "simple-bus";
+		#address-cells = <2>;
+		#size-cells = <2>;
+		ranges;
+
+		clk_ref0: clk_ref0 {
+			compatible = "fixed-clock";
+			#clock-cells = <0>;
+			clock-frequency = <125000000>;
+		};
+
+		clk_ref1: clk_ref1 {
+			compatible = "fixed-clock";
+			#clock-cells = <0>;
+			clock-frequency = <125000000>;
+		};
+
+		clk_ref2: clk_ref2 {
+			compatible = "fixed-clock";
+			#clock-cells = <0>;
+			clock-frequency = <125000000>;
+		};
+
+		clks: clock-controller@2010020000 {
+			compatible = "lsi,axm5516-clks";
+			#clock-cells = <1>;
+			reg = <0x20 0x10020000 0 0x20000>;
+		};
+	};
+
+	gic: interrupt-controller@2001001000 {
+		compatible = "arm,cortex-a15-gic";
+		#interrupt-cells = <3>;
+		#address-cells = <0>;
+		interrupt-controller;
+		reg = <0x20 0x01001000 0 0x1000>,
+		      <0x20 0x01002000 0 0x1000>,
+		      <0x20 0x01004000 0 0x2000>,
+		      <0x20 0x01006000 0 0x2000>;
+		interrupts = <GIC_PPI 9 (GIC_CPU_MASK_SIMPLE(4) |
+				IRQ_TYPE_LEVEL_HIGH)>;
+	};
+
+	timer {
+		compatible = "arm,armv7-timer";
+		interrupts =
+			<GIC_PPI 13
+				(GIC_CPU_MASK_SIMPLE(4) | IRQ_TYPE_LEVEL_LOW)>,
+			<GIC_PPI 14
+				(GIC_CPU_MASK_SIMPLE(4) | IRQ_TYPE_LEVEL_LOW)>,
+			<GIC_PPI 11
+				(GIC_CPU_MASK_SIMPLE(4) | IRQ_TYPE_LEVEL_LOW)>,
+			<GIC_PPI 10
+				(GIC_CPU_MASK_SIMPLE(4) | IRQ_TYPE_LEVEL_LOW)>;
+	};
+
+
+	pmu {
+		compatible = "arm,cortex-a15-pmu";
+		interrupts = <GIC_SPI 190 IRQ_TYPE_LEVEL_HIGH>;
+	};
+
+	soc {
+		compatible = "simple-bus";
+		device_type = "soc";
+		#address-cells = <2>;
+		#size-cells = <2>;
+		interrupt-parent = <&gic>;
+		ranges;
+
+		syscon: syscon@2010030000 {
+			compatible = "lsi,axxia-syscon", "syscon";
+			reg = <0x20 0x10030000 0 0x2000>;
+		};
+
+		reset: reset@2010031000 {
+			compatible = "lsi,axm55xx-reset";
+			syscon = <&syscon>;
+		};
+
+		amba {
+			compatible = "arm,amba-bus";
+			#address-cells = <2>;
+			#size-cells = <2>;
+			ranges;
+
+			serial0: uart@2010080000 {
+				compatible = "arm,pl011", "arm,primecell";
+				reg = <0x20 0x10080000 0 0x1000>;
+				interrupts = <GIC_SPI 56 IRQ_TYPE_LEVEL_HIGH>;
+				clocks = <&clks AXXIA_CLK_PER>;
+				clock-names = "apb_pclk";
+				status = "disabled";
+			};
+
+			serial1: uart@2010081000 {
+				compatible = "arm,pl011", "arm,primecell";
+				reg = <0x20 0x10081000 0 0x1000>;
+				interrupts = <GIC_SPI 57 IRQ_TYPE_LEVEL_HIGH>;
+				clocks = <&clks AXXIA_CLK_PER>;
+				clock-names = "apb_pclk";
+				status = "disabled";
+			};
+
+			serial2: uart@2010082000 {
+				compatible = "arm,pl011", "arm,primecell";
+				reg = <0x20 0x10082000 0 0x1000>;
+				interrupts = <GIC_SPI 58 IRQ_TYPE_LEVEL_HIGH>;
+				clocks = <&clks AXXIA_CLK_PER>;
+				clock-names = "apb_pclk";
+				status = "disabled";
+			};
+
+			serial3: uart@2010083000 {
+				compatible = "arm,pl011", "arm,primecell";
+				reg = <0x20 0x10083000 0 0x1000>;
+				interrupts = <GIC_SPI 59 IRQ_TYPE_LEVEL_HIGH>;
+				clocks = <&clks AXXIA_CLK_PER>;
+				clock-names = "apb_pclk";
+				status = "disabled";
+			};
+
+			timer0: timer@2010091000 {
+				compatible = "arm,sp804", "arm,primecell";
+				reg = <0x20 0x10091000 0 0x1000>;
+				interrupts = <GIC_SPI 46 IRQ_TYPE_LEVEL_HIGH>,
+					     <GIC_SPI 46 IRQ_TYPE_LEVEL_HIGH>,
+					     <GIC_SPI 47 IRQ_TYPE_LEVEL_HIGH>,
+					     <GIC_SPI 48 IRQ_TYPE_LEVEL_HIGH>,
+					     <GIC_SPI 49 IRQ_TYPE_LEVEL_HIGH>,
+					     <GIC_SPI 50 IRQ_TYPE_LEVEL_HIGH>,
+					     <GIC_SPI 51 IRQ_TYPE_LEVEL_HIGH>,
+					     <GIC_SPI 52 IRQ_TYPE_LEVEL_HIGH>,
+					     <GIC_SPI 53 IRQ_TYPE_LEVEL_HIGH>;
+				clocks = <&clks AXXIA_CLK_PER>;
+				clock-names = "apb_pclk";
+				status = "okay";
+			};
+
+			gpio0: gpio@2010092000 {
+				#gpio-cells = <2>;
+				compatible = "arm,pl061", "arm,primecell";
+				gpio-controller;
+				reg = <0x20 0x10092000 0x00 0x1000>;
+				interrupts = <GIC_SPI 10 IRQ_TYPE_LEVEL_HIGH>,
+					     <GIC_SPI 11 IRQ_TYPE_LEVEL_HIGH>,
+					     <GIC_SPI 12 IRQ_TYPE_LEVEL_HIGH>,
+					     <GIC_SPI 13 IRQ_TYPE_LEVEL_HIGH>,
+					     <GIC_SPI 14 IRQ_TYPE_LEVEL_HIGH>,
+					     <GIC_SPI 15 IRQ_TYPE_LEVEL_HIGH>,
+					     <GIC_SPI 16 IRQ_TYPE_LEVEL_HIGH>,
+					     <GIC_SPI 17 IRQ_TYPE_LEVEL_HIGH>;
+				clocks = <&clks AXXIA_CLK_PER>;
+				clock-names = "apb_pclk";
+				status = "disabled";
+			};
+
+			gpio1: gpio@2010093000 {
+				#gpio-cells = <2>;
+				compatible = "arm,pl061", "arm,primecell";
+				gpio-controller;
+				reg = <0x20 0x10093000 0x00 0x1000>;
+				interrupts = <GIC_SPI 18 IRQ_TYPE_LEVEL_HIGH>;
+				clocks = <&clks AXXIA_CLK_PER>;
+				clock-names = "apb_pclk";
+				status = "disabled";
+			};
+		};
+	};
+};
+
+/*
+  Local Variables:
+  mode: C
+  End:
+*/
diff --git a/arch/arm/boot/dts/bcm11351.dtsi b/arch/arm/boot/dts/bcm11351.dtsi
index 64d069bcc40..6b05ae6d476 100644
--- a/arch/arm/boot/dts/bcm11351.dtsi
+++ b/arch/arm/boot/dts/bcm11351.dtsi
@@ -193,6 +193,14 @@
 		status = "disabled";
 	};
 
+	pwm: pwm@3e01a000 {
+		compatible = "brcm,bcm11351-pwm", "brcm,kona-pwm";
+		reg = <0x3e01a000 0xcc>;
+		clocks = <&slave_ccu BCM281XX_SLAVE_CCU_PWM>;
+		#pwm-cells = <3>;
+		status = "disabled";
+	};
+
 	clocks {
 		#address-cells = <1>;
 		#size-cells = <1>;
diff --git a/arch/arm/boot/dts/bcm28155-ap.dts b/arch/arm/boot/dts/bcm28155-ap.dts
index af3da55eef4..9ce91dd60cb 100644
--- a/arch/arm/boot/dts/bcm28155-ap.dts
+++ b/arch/arm/boot/dts/bcm28155-ap.dts
@@ -69,6 +69,10 @@
 		status = "okay";
 	};
 
+	pwm: pwm@3e01a000 {
+		status = "okay";
+	};
+
 	usbotg: usb@3f120000 {
 		vusb_d-supply = <&usbldo_reg>;
 		vusb_a-supply = <&iosr1_reg>;
diff --git a/arch/arm/boot/dts/bcm59056.dtsi b/arch/arm/boot/dts/bcm59056.dtsi
index dfadaaa89b0..066adfb10bd 100644
--- a/arch/arm/boot/dts/bcm59056.dtsi
+++ b/arch/arm/boot/dts/bcm59056.dtsi
@@ -70,5 +70,26 @@
 
 		vsr_reg: vsr {
 		};
+
+		gpldo1_reg: gpldo1 {
+		};
+
+		gpldo2_reg: gpldo2 {
+		};
+
+		gpldo3_reg: gpldo3 {
+		};
+
+		gpldo4_reg: gpldo4 {
+		};
+
+		gpldo5_reg: gpldo5 {
+		};
+
+		gpldo6_reg: gpldo6 {
+		};
+
+		vbus_reg: vbus {
+		};
 	};
 };
diff --git a/arch/arm/boot/dts/berlin2.dtsi b/arch/arm/boot/dts/berlin2.dtsi
index 56a1af2f105..2477dac4d64 100644
--- a/arch/arm/boot/dts/berlin2.dtsi
+++ b/arch/arm/boot/dts/berlin2.dtsi
@@ -12,6 +12,7 @@
  */
 
 #include "skeleton.dtsi"
+#include <dt-bindings/clock/berlin2.h>
 #include <dt-bindings/interrupt-controller/arm-gic.h>
 
 / {
@@ -37,24 +38,10 @@
 		};
 	};
 
-	clocks {
-		smclk: sysmgr-clock {
-			compatible = "fixed-clock";
-			#clock-cells = <0>;
-			clock-frequency = <25000000>;
-		};
-
-		cfgclk: cfg-clock {
-			compatible = "fixed-clock";
-			#clock-cells = <0>;
-			clock-frequency = <100000000>;
-		};
-
-		sysclk: system-clock {
-			compatible = "fixed-clock";
-			#clock-cells = <0>;
-			clock-frequency = <400000000>;
-		};
+	refclk: oscillator {
+		compatible = "fixed-clock";
+		#clock-cells = <0>;
+		clock-frequency = <25000000>;
 	};
 
 	soc {
@@ -72,6 +59,11 @@
 			cache-level = <2>;
 		};
 
+		scu: snoop-control-unit@ad0000 {
+			compatible = "arm,cortex-a9-scu";
+			reg = <0xad0000 0x58>;
+		};
+
 		gic: interrupt-controller@ad1000 {
 			compatible = "arm,cortex-a9-gic";
 			reg = <0xad1000 0x1000>, <0xad0100 0x0100>;
@@ -83,7 +75,7 @@
 			compatible = "arm,cortex-a9-twd-timer";
 			reg = <0xad0600 0x20>;
 			interrupts = <GIC_PPI 13 IRQ_TYPE_LEVEL_HIGH>;
-			clocks = <&sysclk>;
+			clocks = <&chip CLKID_TWD>;
 		};
 
 		apb@e80000 {
@@ -94,11 +86,83 @@
 			ranges = <0 0xe80000 0x10000>;
 			interrupt-parent = <&aic>;
 
+			gpio0: gpio@0400 {
+				compatible = "snps,dw-apb-gpio";
+				reg = <0x0400 0x400>;
+				#address-cells = <1>;
+				#size-cells = <0>;
+
+				porta: gpio-port@0 {
+					compatible = "snps,dw-apb-gpio-port";
+					gpio-controller;
+					#gpio-cells = <2>;
+					snps,nr-gpios = <8>;
+					reg = <0>;
+					interrupt-controller;
+					#interrupt-cells = <2>;
+					interrupts = <0>;
+				};
+			};
+
+			gpio1: gpio@0800 {
+				compatible = "snps,dw-apb-gpio";
+				reg = <0x0800 0x400>;
+				#address-cells = <1>;
+				#size-cells = <0>;
+
+				portb: gpio-port@1 {
+					compatible = "snps,dw-apb-gpio-port";
+					gpio-controller;
+					#gpio-cells = <2>;
+					snps,nr-gpios = <8>;
+					reg = <0>;
+					interrupt-controller;
+					#interrupt-cells = <2>;
+					interrupts = <1>;
+				};
+			};
+
+			gpio2: gpio@0c00 {
+				compatible = "snps,dw-apb-gpio";
+				reg = <0x0c00 0x400>;
+				#address-cells = <1>;
+				#size-cells = <0>;
+
+				portc: gpio-port@2 {
+					compatible = "snps,dw-apb-gpio-port";
+					gpio-controller;
+					#gpio-cells = <2>;
+					snps,nr-gpios = <8>;
+					reg = <0>;
+					interrupt-controller;
+					#interrupt-cells = <2>;
+					interrupts = <2>;
+				};
+			};
+
+			gpio3: gpio@1000 {
+				compatible = "snps,dw-apb-gpio";
+				reg = <0x1000 0x400>;
+				#address-cells = <1>;
+				#size-cells = <0>;
+
+				portd: gpio-port@3 {
+					compatible = "snps,dw-apb-gpio-port";
+					gpio-controller;
+					#gpio-cells = <2>;
+					snps,nr-gpios = <8>;
+					reg = <0>;
+					interrupt-controller;
+					#interrupt-cells = <2>;
+					interrupts = <3>;
+				};
+			};
+
 			timer0: timer@2c00 {
 				compatible = "snps,dw-apb-timer";
 				reg = <0x2c00 0x14>;
 				interrupts = <8>;
-				clocks = <&cfgclk>;
+				clocks = <&chip CLKID_CFG>;
 				clock-names = "timer";
 				status = "okay";
 			};
@@ -107,7 +171,7 @@
 				compatible = "snps,dw-apb-timer";
 				reg = <0x2c14 0x14>;
 				interrupts = <9>;
-				clocks = <&cfgclk>;
+				clocks = <&chip CLKID_CFG>;
 				clock-names = "timer";
 				status = "okay";
 			};
@@ -116,7 +180,7 @@
 				compatible = "snps,dw-apb-timer";
 				reg = <0x2c28 0x14>;
 				interrupts = <10>;
-				clocks = <&cfgclk>;
+				clocks = <&chip CLKID_CFG>;
 				clock-names = "timer";
 				status = "disabled";
 			};
@@ -125,7 +189,7 @@
 				compatible = "snps,dw-apb-timer";
 				reg = <0x2c3c 0x14>;
 				interrupts = <11>;
-				clocks = <&cfgclk>;
+				clocks = <&chip CLKID_CFG>;
 				clock-names = "timer";
 				status = "disabled";
 			};
@@ -134,7 +198,7 @@
 				compatible = "snps,dw-apb-timer";
 				reg = <0x2c50 0x14>;
 				interrupts = <12>;
-				clocks = <&cfgclk>;
+				clocks = <&chip CLKID_CFG>;
 				clock-names = "timer";
 				status = "disabled";
 			};
@@ -143,7 +207,7 @@
 				compatible = "snps,dw-apb-timer";
 				reg = <0x2c64 0x14>;
 				interrupts = <13>;
-				clocks = <&cfgclk>;
+				clocks = <&chip CLKID_CFG>;
 				clock-names = "timer";
 				status = "disabled";
 			};
@@ -152,7 +216,7 @@
 				compatible = "snps,dw-apb-timer";
 				reg = <0x2c78 0x14>;
 				interrupts = <14>;
-				clocks = <&cfgclk>;
+				clocks = <&chip CLKID_CFG>;
 				clock-names = "timer";
 				status = "disabled";
 			};
@@ -161,7 +225,7 @@
 				compatible = "snps,dw-apb-timer";
 				reg = <0x2c8c 0x14>;
 				interrupts = <15>;
-				clocks = <&cfgclk>;
+				clocks = <&chip CLKID_CFG>;
 				clock-names = "timer";
 				status = "disabled";
 			};
@@ -176,6 +240,14 @@
 			};
 		};
 
+		chip: chip-control@ea0000 {
+			compatible = "marvell,berlin2-chip-ctrl";
+			#clock-cells = <1>;
+			reg = <0xea0000 0x400>;
+			clocks = <&refclk>;
+			clock-names = "refclk";
+		};
+
 		apb@fc0000 {
 			compatible = "simple-bus";
 			#address-cells = <1>;
@@ -184,13 +256,48 @@
 			ranges = <0 0xfc0000 0x10000>;
 			interrupt-parent = <&sic>;
 
+			sm_gpio1: gpio@5000 {
+				compatible = "snps,dw-apb-gpio";
+				reg = <0x5000 0x400>;
+				#address-cells = <1>;
+				#size-cells = <0>;
+
+				portf: gpio-port@5 {
+					compatible = "snps,dw-apb-gpio-port";
+					gpio-controller;
+					#gpio-cells = <2>;
+					snps,nr-gpios = <8>;
+					reg = <0>;
+				};
+			};
+
+			sm_gpio0: gpio@c000 {
+				compatible = "snps,dw-apb-gpio";
+				reg = <0xc000 0x400>;
+				#address-cells = <1>;
+				#size-cells = <0>;
+
+				porte: gpio-port@4 {
+					compatible = "snps,dw-apb-gpio-port";
+					gpio-controller;
+					#gpio-cells = <2>;
+					snps,nr-gpios = <8>;
+					reg = <0>;
+					interrupt-controller;
+					#interrupt-cells = <2>;
+					interrupts = <11>;
+				};
+			};
+
 			uart0: serial@9000 {
 				compatible = "snps,dw-apb-uart";
 				reg = <0x9000 0x100>;
 				reg-shift = <2>;
 				reg-io-width = <1>;
 				interrupts = <8>;
-				clocks = <&smclk>;
+				clocks = <&refclk>;
+				pinctrl-0 = <&uart0_pmux>;
+				pinctrl-names = "default";
 				status = "disabled";
 			};
 
@@ -200,7 +307,9 @@
 				reg-shift = <2>;
 				reg-io-width = <1>;
 				interrupts = <9>;
-				clocks = <&smclk>;
+				clocks = <&refclk>;
+				pinctrl-0 = <&uart1_pmux>;
+				pinctrl-names = "default";
 				status = "disabled";
 			};
 
@@ -210,10 +319,32 @@
 				reg-shift = <2>;
 				reg-io-width = <1>;
 				interrupts = <10>;
-				clocks = <&smclk>;
+				clocks = <&refclk>;
+				pinctrl-0 = <&uart2_pmux>;
+				pinctrl-names = "default";
 				status = "disabled";
 			};
 
+			sysctrl: system-controller@d000 {
+				compatible = "marvell,berlin2-system-ctrl";
+				reg = <0xd000 0x100>;
+
+				uart0_pmux: uart0-pmux {
+					groups = "GSM4";
+					function = "uart0";
+				};
+
+				uart1_pmux: uart1-pmux {
+					groups = "GSM5";
+					function = "uart1";
+				};
+
+				uart2_pmux: uart2-pmux {
+					groups = "GSM3";
+					function = "uart2";
+				};
+			};
+
 			sic: interrupt-controller@e000 {
 				compatible = "snps,dw-apb-ictl";
 				reg = <0xe000 0x400>;
diff --git a/arch/arm/boot/dts/berlin2cd.dtsi b/arch/arm/boot/dts/berlin2cd.dtsi
index 094968c2753..cc1df65da50 100644
--- a/arch/arm/boot/dts/berlin2cd.dtsi
+++ b/arch/arm/boot/dts/berlin2cd.dtsi
@@ -12,6 +12,7 @@
  */
 
 #include "skeleton.dtsi"
+#include <dt-bindings/clock/berlin2.h>
 #include <dt-bindings/interrupt-controller/arm-gic.h>
 
 / {
@@ -30,24 +31,10 @@
 		};
 	};
 
-	clocks {
-		smclk: sysmgr-clock {
-			compatible = "fixed-clock";
-			#clock-cells = <0>;
-			clock-frequency = <25000000>;
-		};
-
-		cfgclk: cfg-clock {
-			compatible = "fixed-clock";
-			#clock-cells = <0>;
-			clock-frequency = <75000000>;
-		};
-
-		sysclk: system-clock {
-			compatible = "fixed-clock";
-			#clock-cells = <0>;
-			clock-frequency = <300000000>;
-		};
+	refclk: oscillator {
+		compatible = "fixed-clock";
+		#clock-cells = <0>;
+		clock-frequency = <25000000>;
 	};
 
 	soc {
@@ -76,7 +63,7 @@
 			compatible = "arm,cortex-a9-twd-timer";
 			reg = <0xad0600 0x20>;
 			interrupts = <GIC_PPI 13 IRQ_TYPE_LEVEL_HIGH>;
-			clocks = <&sysclk>;
+			clocks = <&chip CLKID_TWD>;
 		};
 
 		apb@e80000 {
@@ -87,11 +74,83 @@
 			ranges = <0 0xe80000 0x10000>;
 			interrupt-parent = <&aic>;
 
+			gpio0: gpio@0400 {
+				compatible = "snps,dw-apb-gpio";
+				reg = <0x0400 0x400>;
+				#address-cells = <1>;
+				#size-cells = <0>;
+
+				porta: gpio-port@0 {
+					compatible = "snps,dw-apb-gpio-port";
+					gpio-controller;
+					#gpio-cells = <2>;
+					snps,nr-gpios = <8>;
+					reg = <0>;
+					interrupt-controller;
+					#interrupt-cells = <2>;
+					interrupts = <0>;
+				};
+			};
+
+			gpio1: gpio@0800 {
+				compatible = "snps,dw-apb-gpio";
+				reg = <0x0800 0x400>;
+				#address-cells = <1>;
+				#size-cells = <0>;
+
+				portb: gpio-port@1 {
+					compatible = "snps,dw-apb-gpio-port";
+					gpio-controller;
+					#gpio-cells = <2>;
+					snps,nr-gpios = <8>;
+					reg = <0>;
+					interrupt-controller;
+					#interrupt-cells = <2>;
+					interrupts = <1>;
+				};
+			};
+
+			gpio2: gpio@0c00 {
+				compatible = "snps,dw-apb-gpio";
+				reg = <0x0c00 0x400>;
+				#address-cells = <1>;
+				#size-cells = <0>;
+
+				portc: gpio-port@2 {
+					compatible = "snps,dw-apb-gpio-port";
+					gpio-controller;
+					#gpio-cells = <2>;
+					snps,nr-gpios = <8>;
+					reg = <0>;
+					interrupt-controller;
+					#interrupt-cells = <2>;
+					interrupts = <2>;
+				};
+			};
+
+			gpio3: gpio@1000 {
+				compatible = "snps,dw-apb-gpio";
+				reg = <0x1000 0x400>;
+				#address-cells = <1>;
+				#size-cells = <0>;
+
+				portd: gpio-port@3 {
+					compatible = "snps,dw-apb-gpio-port";
+					gpio-controller;
+					#gpio-cells = <2>;
+					snps,nr-gpios = <8>;
+					reg = <0>;
+					interrupt-controller;
+					#interrupt-cells = <2>;
+					interrupts = <3>;
+				};
+			};
+
 			timer0: timer@2c00 {
 				compatible = "snps,dw-apb-timer";
 				reg = <0x2c00 0x14>;
 				interrupts = <8>;
-				clocks = <&cfgclk>;
+				clocks = <&chip CLKID_CFG>;
 				clock-names = "timer";
 				status = "okay";
 			};
@@ -100,7 +159,7 @@
 				compatible = "snps,dw-apb-timer";
 				reg = <0x2c14 0x14>;
 				interrupts = <9>;
-				clocks = <&cfgclk>;
+				clocks = <&chip CLKID_CFG>;
 				clock-names = "timer";
 				status = "okay";
 			};
@@ -109,7 +168,7 @@
 				compatible = "snps,dw-apb-timer";
 				reg = <0x2c28 0x14>;
 				interrupts = <10>;
-				clocks = <&cfgclk>;
+				clocks = <&chip CLKID_CFG>;
 				clock-names = "timer";
 				status = "disabled";
 			};
@@ -118,7 +177,7 @@
 				compatible = "snps,dw-apb-timer";
 				reg = <0x2c3c 0x14>;
 				interrupts = <11>;
-				clocks = <&cfgclk>;
+				clocks = <&chip CLKID_CFG>;
 				clock-names = "timer";
 				status = "disabled";
 			};
@@ -127,7 +186,7 @@
 				compatible = "snps,dw-apb-timer";
 				reg = <0x2c50 0x14>;
 				interrupts = <12>;
-				clocks = <&cfgclk>;
+				clocks = <&chip CLKID_CFG>;
 				clock-names = "timer";
 				status = "disabled";
 			};
@@ -136,7 +195,7 @@
 				compatible = "snps,dw-apb-timer";
 				reg = <0x2c64 0x14>;
 				interrupts = <13>;
-				clocks = <&cfgclk>;
+				clocks = <&chip CLKID_CFG>;
 				clock-names = "timer";
 				status = "disabled";
 			};
@@ -145,7 +204,7 @@
 				compatible = "snps,dw-apb-timer";
 				reg = <0x2c78 0x14>;
 				interrupts = <14>;
-				clocks = <&cfgclk>;
+				clocks = <&chip CLKID_CFG>;
 				clock-names = "timer";
 				status = "disabled";
 			};
@@ -154,7 +213,7 @@
 				compatible = "snps,dw-apb-timer";
 				reg = <0x2c8c 0x14>;
 				interrupts = <15>;
-				clocks = <&cfgclk>;
+				clocks = <&chip CLKID_CFG>;
 				clock-names = "timer";
 				status = "disabled";
 			};
@@ -169,6 +228,19 @@
 			};
 		};
 
+		chip: chip-control@ea0000 {
+			compatible = "marvell,berlin2cd-chip-ctrl";
+			#clock-cells = <1>;
+			reg = <0xea0000 0x400>;
+			clocks = <&refclk>;
+			clock-names = "refclk";
+
+			uart0_pmux: uart0-pmux {
+				groups = "G6";
+				function = "uart0";
+			};
+		};
+
 		apb@fc0000 {
 			compatible = "simple-bus";
 			#address-cells = <1>;
@@ -177,13 +249,45 @@
 			ranges = <0 0xfc0000 0x10000>;
 			interrupt-parent = <&sic>;
 
+			sm_gpio1: gpio@5000 {
+				compatible = "snps,dw-apb-gpio";
+				reg = <0x5000 0x400>;
+				#address-cells = <1>;
+				#size-cells = <0>;
+
+				portf: gpio-port@5 {
+					compatible = "snps,dw-apb-gpio-port";
+					gpio-controller;
+					#gpio-cells = <2>;
+					snps,nr-gpios = <8>;
+					reg = <0>;
+				};
+			};
+
+			sm_gpio0: gpio@c000 {
+				compatible = "snps,dw-apb-gpio";
+				reg = <0xc000 0x400>;
+				#address-cells = <1>;
+				#size-cells = <0>;
+
+				porte: gpio-port@4 {
+					compatible = "snps,dw-apb-gpio-port";
+					gpio-controller;
+					#gpio-cells = <2>;
+					snps,nr-gpios = <8>;
+					reg = <0>;
+				};
+			};
+
 			uart0: serial@9000 {
 				compatible = "snps,dw-apb-uart";
 				reg = <0x9000 0x100>;
 				reg-shift = <2>;
 				reg-io-width = <1>;
 				interrupts = <8>;
-				clocks = <&smclk>;
+				clocks = <&refclk>;
+				pinctrl-0 = <&uart0_pmux>;
+				pinctrl-names = "default";
 				status = "disabled";
 			};
 
@@ -193,10 +297,15 @@
 				reg-shift = <2>;
 				reg-io-width = <1>;
 				interrupts = <9>;
-				clocks = <&smclk>;
+				clocks = <&refclk>;
 				status = "disabled";
 			};
 
+			sysctrl: system-controller@d000 {
+				compatible = "marvell,berlin2cd-system-ctrl";
+				reg = <0xd000 0x100>;
+			};
+
 			sic: interrupt-controller@e000 {
 				compatible = "snps,dw-apb-ictl";
 				reg = <0xe000 0x400>;
diff --git a/arch/arm/boot/dts/berlin2q-marvell-dmp.dts b/arch/arm/boot/dts/berlin2q-marvell-dmp.dts
new file mode 100644
index 00000000000..995150f9379
--- /dev/null
+++ b/arch/arm/boot/dts/berlin2q-marvell-dmp.dts
@@ -0,0 +1,39 @@
+/*
+ * Copyright (C) 2014 Antoine Ténart <antoine.tenart@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.
+ */
+
+/dts-v1/;
+#include "berlin2q.dtsi"
+
+/ {
+	model = "Marvell BG2-Q DMP";
+	compatible = "marvell,berlin2q-dmp", "marvell,berlin2q", "marvell,berlin";
+
+	memory {
+		device_type = "memory";
+		reg = <0x00000000 0x80000000>;
+	};
+
+	choosen {
+		bootargs = "console=ttyS0,115200 earlyprintk";
+	};
+};
+
+&sdhci1 {
+	broken-cd;
+	sdhci,wp-inverted;
+	status = "okay";
+};
+
+&sdhci2 {
+	non-removable;
+	status = "okay";
+};
+
+&uart0 {
+	status = "okay";
+};
diff --git a/arch/arm/boot/dts/berlin2q.dtsi b/arch/arm/boot/dts/berlin2q.dtsi
new file mode 100644
index 00000000000..635a16a64cb
--- /dev/null
+++ b/arch/arm/boot/dts/berlin2q.dtsi
@@ -0,0 +1,363 @@
+/*
+ * Copyright (C) 2014 Antoine Ténart <antoine.tenart@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.
+ */
+
+#include <dt-bindings/clock/berlin2q.h>
+#include <dt-bindings/interrupt-controller/arm-gic.h>
+
+#include "skeleton.dtsi"
+
+/ {
+	model = "Marvell Armada 1500 pro (BG2-Q) SoC";
+	compatible = "marvell,berlin2q", "marvell,berlin";
+
+	cpus {
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		cpu@0 {
+			compatible = "arm,cortex-a9";
+			device_type = "cpu";
+			next-level-cache = <&l2>;
+			reg = <0>;
+		};
+
+		cpu@1 {
+			compatible = "arm,cortex-a9";
+			device_type = "cpu";
+			next-level-cache = <&l2>;
+			reg = <1>;
+		};
+
+		cpu@2 {
+			compatible = "arm,cortex-a9";
+			device_type = "cpu";
+			next-level-cache = <&l2>;
+			reg = <2>;
+		};
+
+		cpu@3 {
+			compatible = "arm,cortex-a9";
+			device_type = "cpu";
+			next-level-cache = <&l2>;
+			reg = <3>;
+		};
+	};
+
+	refclk: oscillator {
+		compatible = "fixed-clock";
+		#clock-cells = <0>;
+		clock-frequency = <25000000>;
+	};
+
+	soc {
+		compatible = "simple-bus";
+		#address-cells = <1>;
+		#size-cells = <1>;
+
+		ranges = <0 0xf7000000 0x1000000>;
+		interrupt-parent = <&gic>;
+
+		sdhci0: sdhci@ab0000 {
+			compatible = "mrvl,pxav3-mmc";
+			reg = <0xab0000 0x200>;
+			clocks = <&chip CLKID_SDIO1XIN>;
+			interrupts = <GIC_SPI 17 IRQ_TYPE_LEVEL_HIGH>;
+			status = "disabled";
+		};
+
+		sdhci1: sdhci@ab0800 {
+			compatible = "mrvl,pxav3-mmc";
+			reg = <0xab0800 0x200>;
+			clocks = <&chip CLKID_SDIO1XIN>;
+			interrupts = <GIC_SPI 20 IRQ_TYPE_LEVEL_HIGH>;
+			status = "disabled";
+		};
+
+		sdhci2: sdhci@ab1000 {
+			compatible = "mrvl,pxav3-mmc";
+			reg = <0xab1000 0x200>;
+			interrupts = <GIC_SPI 28 IRQ_TYPE_LEVEL_HIGH>;
+			clocks = <&chip CLKID_SDIO1XIN>;
+			status = "disabled";
+		};
+
+		l2: l2-cache-controller@ac0000 {
+			compatible = "arm,pl310-cache";
+			reg = <0xac0000 0x1000>;
+			cache-level = <2>;
+		};
+
+		scu: snoop-control-unit@ad0000 {
+			compatible = "arm,cortex-a9-scu";
+			reg = <0xad0000 0x58>;
+		};
+
+		local-timer@ad0600 {
+			compatible = "arm,cortex-a9-twd-timer";
+			reg = <0xad0600 0x20>;
+			clocks = <&chip CLKID_TWD>;
+			interrupts = <GIC_PPI 13 IRQ_TYPE_LEVEL_HIGH>;
+		};
+
+		gic: interrupt-controller@ad1000 {
+			compatible = "arm,cortex-a9-gic";
+			reg = <0xad1000 0x1000>, <0xad0100 0x100>;
+			interrupt-controller;
+			#interrupt-cells = <3>;
+		};
+
+		apb@e80000 {
+			compatible = "simple-bus";
+			#address-cells = <1>;
+			#size-cells = <1>;
+
+			ranges = <0 0xe80000 0x10000>;
+			interrupt-parent = <&aic>;
+
+			gpio0: gpio@0400 {
+				compatible = "snps,dw-apb-gpio";
+				reg = <0x0400 0x400>;
+				#address-cells = <1>;
+				#size-cells = <0>;
+
+				porta: gpio-port@0 {
+					compatible = "snps,dw-apb-gpio-port";
+					gpio-controller;
+					#gpio-cells = <2>;
+					snps,nr-gpios = <32>;
+					reg = <0>;
+					interrupt-controller;
+					#interrupt-cells = <2>;
+					interrupts = <0>;
+				};
+			};
+
+			gpio1: gpio@0800 {
+				compatible = "snps,dw-apb-gpio";
+				reg = <0x0800 0x400>;
+				#address-cells = <1>;
+				#size-cells = <0>;
+
+				portb: gpio-port@1 {
+					compatible = "snps,dw-apb-gpio-port";
+					gpio-controller;
+					#gpio-cells = <2>;
+					snps,nr-gpios = <32>;
+					reg = <0>;
+					interrupt-controller;
+					#interrupt-cells = <2>;
+					interrupts = <1>;
+				};
+			};
+
+			gpio2: gpio@0c00 {
+				compatible = "snps,dw-apb-gpio";
+				reg = <0x0c00 0x400>;
+				#address-cells = <1>;
+				#size-cells = <0>;
+
+				portc: gpio-port@2 {
+					compatible = "snps,dw-apb-gpio-port";
+					gpio-controller;
+					#gpio-cells = <2>;
+					snps,nr-gpios = <32>;
+					reg = <0>;
+					interrupt-controller;
+					#interrupt-cells = <2>;
+					interrupts = <2>;
+				};
+			};
+
+			gpio3: gpio@1000 {
+				compatible = "snps,dw-apb-gpio";
+				reg = <0x1000 0x400>;
+				#address-cells = <1>;
+				#size-cells = <0>;
+
+				portd: gpio-port@3 {
+					compatible = "snps,dw-apb-gpio-port";
+					gpio-controller;
+					#gpio-cells = <2>;
+					snps,nr-gpios = <32>;
+					reg = <0>;
+					interrupt-controller;
+					#interrupt-cells = <2>;
+					interrupts = <3>;
+				};
+			};
+
+			timer0: timer@2c00 {
+				compatible = "snps,dw-apb-timer";
+				reg = <0x2c00 0x14>;
+				clocks = <&chip CLKID_CFG>;
+				clock-names = "timer";
+				interrupts = <8>;
+			};
+
+			timer1: timer@2c14 {
+				compatible = "snps,dw-apb-timer";
+				reg = <0x2c14 0x14>;
+				clocks = <&chip CLKID_CFG>;
+				clock-names = "timer";
+				status = "disabled";
+			};
+
+			timer2: timer@2c28 {
+				compatible = "snps,dw-apb-timer";
+				reg = <0x2c28 0x14>;
+				clocks = <&chip CLKID_CFG>;
+				clock-names = "timer";
+				status = "disabled";
+			};
+
+			timer3: timer@2c3c {
+				compatible = "snps,dw-apb-timer";
+				reg = <0x2c3c 0x14>;
+				clocks = <&chip CLKID_CFG>;
+				clock-names = "timer";
+				status = "disabled";
+			};
+
+			timer4: timer@2c50 {
+				compatible = "snps,dw-apb-timer";
+				reg = <0x2c50 0x14>;
+				clocks = <&chip CLKID_CFG>;
+				clock-names = "timer";
+				status = "disabled";
+			};
+
+			timer5: timer@2c64 {
+				compatible = "snps,dw-apb-timer";
+				reg = <0x2c64 0x14>;
+				clocks = <&chip CLKID_CFG>;
+				clock-names = "timer";
+				status = "disabled";
+			};
+
+			timer6: timer@2c78 {
+				compatible = "snps,dw-apb-timer";
+				reg = <0x2c78 0x14>;
+				clocks = <&chip CLKID_CFG>;
+				clock-names = "timer";
+				status = "disabled";
+			};
+
+			timer7: timer@2c8c {
+				compatible = "snps,dw-apb-timer";
+				reg = <0x2c8c 0x14>;
+				clocks = <&chip CLKID_CFG>;
+				clock-names = "timer";
+				status = "disabled";
+			};
+
+			aic: interrupt-controller@3800 {
+				compatible = "snps,dw-apb-ictl";
+				reg = <0x3800 0x30>;
+				interrupt-controller;
+				#interrupt-cells = <1>;
+				interrupt-parent = <&gic>;
+				interrupts = <GIC_SPI 3 IRQ_TYPE_LEVEL_HIGH>;
+			};
+
+			gpio4: gpio@5000 {
+				compatible = "snps,dw-apb-gpio";
+				reg = <0x5000 0x400>;
+				#address-cells = <1>;
+				#size-cells = <0>;
+
+				porte: gpio-port@4 {
+					compatible = "snps,dw-apb-gpio-port";
+					gpio-controller;
+					#gpio-cells = <2>;
+					snps,nr-gpios = <32>;
+					reg = <0>;
+				};
+			};
+
+			gpio5: gpio@c000 {
+				compatible = "snps,dw-apb-gpio";
+				reg = <0xc000 0x400>;
+				#address-cells = <1>;
+				#size-cells = <0>;
+
+				portf: gpio-port@5 {
+					compatible = "snps,dw-apb-gpio-port";
+					gpio-controller;
+					#gpio-cells = <2>;
+					snps,nr-gpios = <32>;
+					reg = <0>;
+				};
+			};
+		};
+
+		chip: chip-control@ea0000 {
+			compatible = "marvell,berlin2q-chip-ctrl";
+			#clock-cells = <1>;
+			reg = <0xea0000 0x400>, <0xdd0170 0x10>;
+			clocks = <&refclk>;
+			clock-names = "refclk";
+		};
+
+		apb@fc0000 {
+			compatible = "simple-bus";
+			#address-cells = <1>;
+			#size-cells = <1>;
+
+			ranges = <0 0xfc0000 0x10000>;
+			interrupt-parent = <&sic>;
+
+			uart0: uart@9000 {
+				compatible = "snps,dw-apb-uart";
+				reg = <0x9000 0x100>;
+				interrupt-parent = <&sic>;
+				interrupts = <8>;
+				clocks = <&refclk>;
+				reg-shift = <2>;
+				pinctrl-0 = <&uart0_pmux>;
+				pinctrl-names = "default";
+				status = "disabled";
+			};
+
+			uart1: uart@a000 {
+				compatible = "snps,dw-apb-uart";
+				reg = <0xa000 0x100>;
+				interrupt-parent = <&sic>;
+				interrupts = <9>;
+				clocks = <&refclk>;
+				reg-shift = <2>;
+				pinctrl-0 = <&uart1_pmux>;
+				pinctrl-names = "default";
+				status = "disabled";
+			};
+
+			sysctrl: pin-controller@d000 {
+				compatible = "marvell,berlin2q-system-ctrl";
+				reg = <0xd000 0x100>;
+
+				uart0_pmux: uart0-pmux {
+					groups = "GSM12";
+					function = "uart0";
+				};
+
+				uart1_pmux: uart1-pmux {
+					groups = "GSM14";
+					function = "uart1";
+				};
+			};
+
+			sic: interrupt-controller@e000 {
+				compatible = "snps,dw-apb-ictl";
+				reg = <0xe000 0x30>;
+				interrupt-controller;
+				#interrupt-cells = <1>;
+				interrupt-parent = <&gic>;
+				interrupts = <GIC_SPI 15 IRQ_TYPE_LEVEL_HIGH>;
+			};
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/dra7-evm.dts b/arch/arm/boot/dts/dra7-evm.dts
index 5babba0a3a7..4adc28039c3 100644
--- a/arch/arm/boot/dts/dra7-evm.dts
+++ b/arch/arm/boot/dts/dra7-evm.dts
@@ -7,11 +7,11 @@
  */
 /dts-v1/;
 
-#include "dra7.dtsi"
+#include "dra74x.dtsi"
 
 / {
-	model = "TI DRA7";
-	compatible = "ti,dra7-evm", "ti,dra752", "ti,dra7";
+	model = "TI DRA742";
+	compatible = "ti,dra7-evm", "ti,dra742", "ti,dra74", "ti,dra7";
 
 	memory {
 		device_type = "memory";
@@ -93,6 +93,64 @@
 			0x24c (PIN_INPUT_SLEW | MUX_MODE0) /* uart3_txd */
 		>;
 	};
+
+	qspi1_pins: pinmux_qspi1_pins {
+		pinctrl-single,pins = <
+			0x4c (PIN_INPUT | MUX_MODE1)  /* gpmc_a3.qspi1_cs2 */
+			0x50 (PIN_INPUT | MUX_MODE1)  /* gpmc_a4.qspi1_cs3 */
+			0x74 (PIN_INPUT | MUX_MODE1)  /* gpmc_a13.qspi1_rtclk */
+			0x78 (PIN_INPUT | MUX_MODE1)  /* gpmc_a14.qspi1_d3 */
+			0x7c (PIN_INPUT | MUX_MODE1)  /* gpmc_a15.qspi1_d2 */
+			0x80 (PIN_INPUT | MUX_MODE1) /* gpmc_a16.qspi1_d1 */
+			0x84 (PIN_INPUT | MUX_MODE1)  /* gpmc_a17.qspi1_d0 */
+			0x88 (PIN_INPUT | MUX_MODE1)  /* qpmc_a18.qspi1_sclk */
+			0xb8 (PIN_INPUT_PULLUP | MUX_MODE1)  /* gpmc_cs2.qspi1_cs0 */
+			0xbc (PIN_INPUT_PULLUP | MUX_MODE1)  /* gpmc_cs3.qspi1_cs1 */
+		>;
+	};
+
+	usb1_pins: pinmux_usb1_pins {
+                pinctrl-single,pins = <
+			0x280 (PIN_INPUT_SLEW | MUX_MODE0) /* usb1_drvvbus */
+                >;
+        };
+
+	usb2_pins: pinmux_usb2_pins {
+                pinctrl-single,pins = <
+			0x284 (PIN_INPUT_SLEW | MUX_MODE0) /* usb2_drvvbus */
+                >;
+        };
+
+	nand_flash_x16: nand_flash_x16 {
+		/* On DRA7 EVM, GPMC_WPN and NAND_BOOTn comes from DIP switch
+		 * So NAND flash requires following switch settings:
+		 * SW5.9 (GPMC_WPN) = LOW
+		 * SW5.1 (NAND_BOOTn) = HIGH */
+		pinctrl-single,pins = <
+			0x0 	(PIN_INPUT  | MUX_MODE0)	/* gpmc_ad0	*/
+			0x4 	(PIN_INPUT  | MUX_MODE0)	/* gpmc_ad1	*/
+			0x8 	(PIN_INPUT  | MUX_MODE0)	/* gpmc_ad2	*/
+			0xc 	(PIN_INPUT  | MUX_MODE0)	/* gpmc_ad3	*/
+			0x10	(PIN_INPUT  | MUX_MODE0)	/* gpmc_ad4	*/
+			0x14	(PIN_INPUT  | MUX_MODE0)	/* gpmc_ad5	*/
+			0x18	(PIN_INPUT  | MUX_MODE0)	/* gpmc_ad6	*/
+			0x1c	(PIN_INPUT  | MUX_MODE0)	/* gpmc_ad7	*/
+			0x20	(PIN_INPUT  | MUX_MODE0)	/* gpmc_ad8	*/
+			0x24	(PIN_INPUT  | MUX_MODE0)	/* gpmc_ad9	*/
+			0x28	(PIN_INPUT  | MUX_MODE0)	/* gpmc_ad10	*/
+			0x2c	(PIN_INPUT  | MUX_MODE0)	/* gpmc_ad11	*/
+			0x30	(PIN_INPUT  | MUX_MODE0)	/* gpmc_ad12	*/
+			0x34	(PIN_INPUT  | MUX_MODE0)	/* gpmc_ad13	*/
+			0x38	(PIN_INPUT  | MUX_MODE0)	/* gpmc_ad14	*/
+			0x3c	(PIN_INPUT  | MUX_MODE0)	/* gpmc_ad15	*/
+			0xd8	(PIN_INPUT_PULLUP  | MUX_MODE0)	/* gpmc_wait0	*/
+			0xcc	(PIN_OUTPUT | MUX_MODE0)	/* gpmc_wen	*/
+			0xb4	(PIN_OUTPUT_PULLUP | MUX_MODE0)	/* gpmc_csn0	*/
+			0xc4	(PIN_OUTPUT | MUX_MODE0)	/* gpmc_advn_ale */
+			0xc8	(PIN_OUTPUT | MUX_MODE0)	/* gpmc_oen_ren	 */
+			0xd0	(PIN_OUTPUT | MUX_MODE0)	/* gpmc_be0n_cle */
+		>;
+	};
 };
 
 &i2c1 {
@@ -273,3 +331,167 @@
 &cpu0 {
 	cpu0-supply = <&smps123_reg>;
 };
+
+&qspi {
+	status = "okay";
+	pinctrl-names = "default";
+	pinctrl-0 = <&qspi1_pins>;
+
+	spi-max-frequency = <48000000>;
+	m25p80@0 {
+		compatible = "s25fl256s1";
+		spi-max-frequency = <48000000>;
+		reg = <0>;
+		spi-tx-bus-width = <1>;
+		spi-rx-bus-width = <4>;
+		spi-cpol;
+		spi-cpha;
+		#address-cells = <1>;
+		#size-cells = <1>;
+
+		/* MTD partition table.
+		 * The ROM checks the first four physical blocks
+		 * for a valid file to boot and the flash here is
+		 * 64KiB block size.
+		 */
+		partition@0 {
+			label = "QSPI.SPL";
+			reg = <0x00000000 0x000010000>;
+		};
+		partition@1 {
+			label = "QSPI.SPL.backup1";
+			reg = <0x00010000 0x00010000>;
+		};
+		partition@2 {
+			label = "QSPI.SPL.backup2";
+			reg = <0x00020000 0x00010000>;
+		};
+		partition@3 {
+			label = "QSPI.SPL.backup3";
+			reg = <0x00030000 0x00010000>;
+		};
+		partition@4 {
+			label = "QSPI.u-boot";
+			reg = <0x00040000 0x00100000>;
+		};
+		partition@5 {
+			label = "QSPI.u-boot-spl-os";
+			reg = <0x00140000 0x00010000>;
+		};
+		partition@6 {
+			label = "QSPI.u-boot-env";
+			reg = <0x00150000 0x00010000>;
+		};
+		partition@7 {
+			label = "QSPI.u-boot-env.backup1";
+			reg = <0x00160000 0x0010000>;
+		};
+		partition@8 {
+			label = "QSPI.kernel";
+			reg = <0x00170000 0x0800000>;
+		};
+		partition@9 {
+			label = "QSPI.file-system";
+			reg = <0x00970000 0x01690000>;
+		};
+	};
+};
+
+&usb1 {
+	dr_mode = "peripheral";
+	pinctrl-names = "default";
+	pinctrl-0 = <&usb1_pins>;
+};
+
+&usb2 {
+	dr_mode = "host";
+	pinctrl-names = "default";
+	pinctrl-0 = <&usb2_pins>;
+};
+
+&elm {
+	status = "okay";
+};
+
+&gpmc {
+	status = "okay";
+	pinctrl-names = "default";
+	pinctrl-0 = <&nand_flash_x16>;
+	ranges = <0 0 0 0x01000000>;	/* minimum GPMC partition = 16MB */
+	nand@0,0 {
+		reg = <0 0 4>;		/* device IO registers */
+		ti,nand-ecc-opt = "bch8";
+		ti,elm-id = <&elm>;
+		nand-bus-width = <16>;
+		gpmc,device-width = <2>;
+		gpmc,sync-clk-ps = <0>;
+		gpmc,cs-on-ns = <0>;
+		gpmc,cs-rd-off-ns = <40>;
+		gpmc,cs-wr-off-ns = <40>;
+		gpmc,adv-on-ns = <0>;
+		gpmc,adv-rd-off-ns = <30>;
+		gpmc,adv-wr-off-ns = <30>;
+		gpmc,we-on-ns = <5>;
+		gpmc,we-off-ns = <25>;
+		gpmc,oe-on-ns = <2>;
+		gpmc,oe-off-ns = <20>;
+		gpmc,access-ns = <20>;
+		gpmc,wr-access-ns = <40>;
+		gpmc,rd-cycle-ns = <40>;
+		gpmc,wr-cycle-ns = <40>;
+		gpmc,wait-pin = <0>;
+		gpmc,wait-on-read;
+		gpmc,wait-on-write;
+		gpmc,bus-turnaround-ns = <0>;
+		gpmc,cycle2cycle-delay-ns = <0>;
+		gpmc,clk-activation-ns = <0>;
+		gpmc,wait-monitoring-ns = <0>;
+		gpmc,wr-data-mux-bus-ns = <0>;
+		/* MTD partition table */
+		/* All SPL-* partitions are sized to minimal length
+		 * which can be independently programmable. For
+		 * NAND flash this is equal to size of erase-block */
+		#address-cells = <1>;
+		#size-cells = <1>;
+		partition@0 {
+			label = "NAND.SPL";
+			reg = <0x00000000 0x000020000>;
+		};
+		partition@1 {
+			label = "NAND.SPL.backup1";
+			reg = <0x00020000 0x00020000>;
+		};
+		partition@2 {
+			label = "NAND.SPL.backup2";
+			reg = <0x00040000 0x00020000>;
+		};
+		partition@3 {
+			label = "NAND.SPL.backup3";
+			reg = <0x00060000 0x00020000>;
+		};
+		partition@4 {
+			label = "NAND.u-boot-spl-os";
+			reg = <0x00080000 0x00040000>;
+		};
+		partition@5 {
+			label = "NAND.u-boot";
+			reg = <0x000c0000 0x00100000>;
+		};
+		partition@6 {
+			label = "NAND.u-boot-env";
+			reg = <0x001c0000 0x00020000>;
+		};
+		partition@7 {
+			label = "NAND.u-boot-env";
+			reg = <0x001e0000 0x00020000>;
+		};
+		partition@8 {
+			label = "NAND.kernel";
+			reg = <0x00200000 0x00800000>;
+		};
+		partition@9 {
+			label = "NAND.file-system";
+			reg = <0x00a00000 0x0f600000>;
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/dra7.dtsi b/arch/arm/boot/dts/dra7.dtsi
index 1c0f8e1893a..c29945e07c5 100644
--- a/arch/arm/boot/dts/dra7.dtsi
+++ b/arch/arm/boot/dts/dra7.dtsi
@@ -33,33 +33,6 @@
 		serial5 = &uart6;
 	};
 
-	cpus {
-		#address-cells = <1>;
-		#size-cells = <0>;
-
-		cpu0: cpu@0 {
-			device_type = "cpu";
-			compatible = "arm,cortex-a15";
-			reg = <0>;
-
-			operating-points = <
-				/* kHz    uV */
-				1000000	1060000
-				1176000	1160000
-				>;
-
-			clocks = <&dpll_mpu_ck>;
-			clock-names = "cpu";
-
-			clock-latency = <300000>; /* From omap-cpufreq driver */
-		};
-		cpu@1 {
-			device_type = "cpu";
-			compatible = "arm,cortex-a15";
-			reg = <1>;
-		};
-	};
-
 	timer {
 		compatible = "arm,armv7-timer";
 		interrupts = <GIC_PPI 13 (GIC_CPU_MASK_SIMPLE(2) | IRQ_TYPE_LEVEL_LOW)>,
@@ -80,7 +53,7 @@
 	};
 
 	/*
-	 * The soc node represents the soc top level view. It is uses for IPs
+	 * The soc node represents the soc top level view. It is used for IPs
 	 * that are not memory mapped in the MPU view or for the MPU itself.
 	 */
 	soc {
@@ -94,18 +67,18 @@
 	/*
 	 * XXX: Use a flat representation of the SOC interconnect.
 	 * The real OMAP interconnect network is quite complex.
-	 * Since that will not bring real advantage to represent that in DT for
+	 * Since it will not bring real advantage to represent that in DT for
 	 * the moment, just use a fake OCP bus entry to represent the whole bus
 	 * hierarchy.
 	 */
 	ocp {
-		compatible = "ti,omap4-l3-noc", "simple-bus";
+		compatible = "ti,dra7-l3-noc", "simple-bus";
 		#address-cells = <1>;
 		#size-cells = <1>;
 		ranges;
 		ti,hwmods = "l3_main_1", "l3_main_2";
-		reg = <0x44000000 0x2000>,
-		      <0x44800000 0x3000>;
+		reg = <0x44000000 0x1000000>,
+		      <0x45000000 0x1000>;
 		interrupts = <GIC_SPI 9 IRQ_TYPE_LEVEL_HIGH>,
 			     <GIC_SPI 10 IRQ_TYPE_LEVEL_HIGH>;
 
@@ -789,6 +762,228 @@
 			dma-names = "tx0", "rx0";
 			status = "disabled";
 		};
+
+		qspi: qspi@4b300000 {
+			compatible = "ti,dra7xxx-qspi";
+			reg = <0x4b300000 0x100>;
+			reg-names = "qspi_base";
+			#address-cells = <1>;
+			#size-cells = <0>;
+			ti,hwmods = "qspi";
+			clocks = <&qspi_gfclk_div>;
+			clock-names = "fck";
+			num-cs = <4>;
+			interrupts = <0 343 0x4>;
+			status = "disabled";
+		};
+
+		omap_control_sata: control-phy@4a002374 {
+			compatible = "ti,control-phy-pipe3";
+			reg = <0x4a002374 0x4>;
+			reg-names = "power";
+			clocks = <&sys_clkin1>;
+			clock-names = "sysclk";
+		};
+
+		/* OCP2SCP3 */
+		ocp2scp@4a090000 {
+			compatible = "ti,omap-ocp2scp";
+			#address-cells = <1>;
+			#size-cells = <1>;
+			ranges;
+			reg = <0x4a090000 0x20>;
+			ti,hwmods = "ocp2scp3";
+			sata_phy: phy@4A096000 {
+				compatible = "ti,phy-pipe3-sata";
+				reg = <0x4A096000 0x80>, /* phy_rx */
+				      <0x4A096400 0x64>, /* phy_tx */
+				      <0x4A096800 0x40>; /* pll_ctrl */
+				reg-names = "phy_rx", "phy_tx", "pll_ctrl";
+				ctrl-module = <&omap_control_sata>;
+				clocks = <&sys_clkin1>;
+				clock-names = "sysclk";
+				#phy-cells = <0>;
+			};
+		};
+
+		sata: sata@4a141100 {
+			compatible = "snps,dwc-ahci";
+			reg = <0x4a140000 0x1100>, <0x4a141100 0x7>;
+			interrupts = <GIC_SPI 54 IRQ_TYPE_LEVEL_HIGH>;
+			phys = <&sata_phy>;
+			phy-names = "sata-phy";
+			clocks = <&sata_ref_clk>;
+			ti,hwmods = "sata";
+		};
+
+		omap_control_usb2phy1: control-phy@4a002300 {
+			compatible = "ti,control-phy-usb2";
+			reg = <0x4a002300 0x4>;
+			reg-names = "power";
+		};
+
+		omap_control_usb3phy1: control-phy@4a002370 {
+			compatible = "ti,control-phy-pipe3";
+			reg = <0x4a002370 0x4>;
+			reg-names = "power";
+		};
+
+		omap_control_usb2phy2: control-phy@0x4a002e74 {
+			compatible = "ti,control-phy-usb2-dra7";
+			reg = <0x4a002e74 0x4>;
+			reg-names = "power";
+		};
+
+		/* OCP2SCP1 */
+		ocp2scp@4a080000 {
+			compatible = "ti,omap-ocp2scp";
+			#address-cells = <1>;
+			#size-cells = <1>;
+			ranges;
+			reg = <0x4a080000 0x20>;
+			ti,hwmods = "ocp2scp1";
+
+			usb2_phy1: phy@4a084000 {
+				compatible = "ti,omap-usb2";
+				reg = <0x4a084000 0x400>;
+				ctrl-module = <&omap_control_usb2phy1>;
+				clocks = <&usb_phy1_always_on_clk32k>,
+					 <&usb_otg_ss1_refclk960m>;
+				clock-names =	"wkupclk",
+						"refclk";
+				#phy-cells = <0>;
+			};
+
+			usb2_phy2: phy@4a085000 {
+				compatible = "ti,omap-usb2";
+				reg = <0x4a085000 0x400>;
+				ctrl-module = <&omap_control_usb2phy2>;
+				clocks = <&usb_phy2_always_on_clk32k>,
+					 <&usb_otg_ss2_refclk960m>;
+				clock-names =	"wkupclk",
+						"refclk";
+				#phy-cells = <0>;
+			};
+
+			usb3_phy1: phy@4a084400 {
+				compatible = "ti,omap-usb3";
+				reg = <0x4a084400 0x80>,
+				      <0x4a084800 0x64>,
+				      <0x4a084c00 0x40>;
+				reg-names = "phy_rx", "phy_tx", "pll_ctrl";
+				ctrl-module = <&omap_control_usb3phy1>;
+				clocks = <&usb_phy3_always_on_clk32k>,
+					 <&sys_clkin1>,
+					 <&usb_otg_ss1_refclk960m>;
+				clock-names =	"wkupclk",
+						"sysclk",
+						"refclk";
+				#phy-cells = <0>;
+			};
+		};
+
+		omap_dwc3_1@48880000 {
+			compatible = "ti,dwc3";
+			ti,hwmods = "usb_otg_ss1";
+			reg = <0x48880000 0x10000>;
+			interrupts = <0 77 4>;
+			#address-cells = <1>;
+			#size-cells = <1>;
+			utmi-mode = <2>;
+			ranges;
+			usb1: usb@48890000 {
+				compatible = "snps,dwc3";
+				reg = <0x48890000 0x17000>;
+				interrupts = <0 76 4>;
+				phys = <&usb2_phy1>, <&usb3_phy1>;
+				phy-names = "usb2-phy", "usb3-phy";
+				tx-fifo-resize;
+				maximum-speed = "super-speed";
+				dr_mode = "otg";
+			};
+		};
+
+		omap_dwc3_2@488c0000 {
+			compatible = "ti,dwc3";
+			ti,hwmods = "usb_otg_ss2";
+			reg = <0x488c0000 0x10000>;
+			interrupts = <0 92 4>;
+			#address-cells = <1>;
+			#size-cells = <1>;
+			utmi-mode = <2>;
+			ranges;
+			usb2: usb@488d0000 {
+				compatible = "snps,dwc3";
+				reg = <0x488d0000 0x17000>;
+				interrupts = <0 78 4>;
+				phys = <&usb2_phy2>;
+				phy-names = "usb2-phy";
+				tx-fifo-resize;
+				maximum-speed = "high-speed";
+				dr_mode = "otg";
+			};
+		};
+
+		/* IRQ for DWC3_3 and DWC3_4 need IRQ crossbar */
+		omap_dwc3_3@48900000 {
+			compatible = "ti,dwc3";
+			ti,hwmods = "usb_otg_ss3";
+			reg = <0x48900000 0x10000>;
+		/*	interrupts = <0 TBD 4>; */
+			#address-cells = <1>;
+			#size-cells = <1>;
+			utmi-mode = <2>;
+			ranges;
+			status = "disabled";
+			usb3: usb@48910000 {
+				compatible = "snps,dwc3";
+				reg = <0x48910000 0x17000>;
+		/*		interrupts = <0 93 4>; */
+				tx-fifo-resize;
+				maximum-speed = "high-speed";
+				dr_mode = "otg";
+			};
+		};
+
+		omap_dwc3_4@48940000 {
+			compatible = "ti,dwc3";
+			ti,hwmods = "usb_otg_ss4";
+			reg = <0x48940000 0x10000>;
+		/*	interrupts = <0 TBD 4>; */
+			#address-cells = <1>;
+			#size-cells = <1>;
+			utmi-mode = <2>;
+			ranges;
+			status = "disabled";
+			usb4: usb@48950000 {
+				compatible = "snps,dwc3";
+				reg = <0x48950000 0x17000>;
+		/*		interrupts = <0 TBD 4>; */
+				tx-fifo-resize;
+				maximum-speed = "high-speed";
+				dr_mode = "otg";
+			};
+		};
+
+		elm: elm@48078000 {
+			compatible = "ti,am3352-elm";
+			reg = <0x48078000 0xfc0>;      /* device IO registers */
+			interrupts = <GIC_SPI 4 IRQ_TYPE_LEVEL_HIGH>;
+			ti,hwmods = "elm";
+			status = "disabled";
+		};
+
+		gpmc: gpmc@50000000 {
+			compatible = "ti,am3352-gpmc";
+			ti,hwmods = "gpmc";
+			reg = <0x50000000 0x37c>;      /* device IO registers */
+			interrupts = <GIC_SPI 20 IRQ_TYPE_LEVEL_HIGH>;
+			gpmc,num-cs = <8>;
+			gpmc,num-waitpins = <2>;
+			#address-cells = <2>;
+			#size-cells = <1>;
+			status = "disabled";
+		};
 	};
 };
 
diff --git a/arch/arm/boot/dts/dra72-evm.dts b/arch/arm/boot/dts/dra72-evm.dts
new file mode 100644
index 00000000000..51470234881
--- /dev/null
+++ b/arch/arm/boot/dts/dra72-evm.dts
@@ -0,0 +1,24 @@
+/*
+ * Copyright (C) 2014 Texas Instruments Incorporated - http://www.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.
+ */
+/dts-v1/;
+
+#include "dra72x.dtsi"
+
+/ {
+	model = "TI DRA722";
+	compatible = "ti,dra72-evm", "ti,dra722", "ti,dra72", "ti,dra7";
+
+	memory {
+		device_type = "memory";
+		reg = <0x80000000 0x40000000>; /* 1024 MB */
+	};
+};
+
+&uart1 {
+	status = "okay";
+};
diff --git a/arch/arm/boot/dts/dra72x.dtsi b/arch/arm/boot/dts/dra72x.dtsi
new file mode 100644
index 00000000000..f1ec22f6ebf
--- /dev/null
+++ b/arch/arm/boot/dts/dra72x.dtsi
@@ -0,0 +1,25 @@
+/*
+ * Copyright (C) 2014 Texas Instruments Incorporated - http://www.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.
+ * Based on "omap4.dtsi"
+ */
+
+#include "dra7.dtsi"
+
+/ {
+	compatible = "ti,dra722", "ti,dra72", "ti,dra7";
+
+	cpus {
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		cpu0: cpu@0 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a15";
+			reg = <0>;
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/dra74x.dtsi b/arch/arm/boot/dts/dra74x.dtsi
new file mode 100644
index 00000000000..a4e8bb9f95c
--- /dev/null
+++ b/arch/arm/boot/dts/dra74x.dtsi
@@ -0,0 +1,41 @@
+/*
+ * Copyright (C) 2014 Texas Instruments Incorporated - http://www.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.
+ * Based on "omap4.dtsi"
+ */
+
+#include "dra7.dtsi"
+
+/ {
+	compatible = "ti,dra742", "ti,dra74", "ti,dra7";
+
+	cpus {
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		cpu0: cpu@0 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a15";
+			reg = <0>;
+
+			operating-points = <
+				/* kHz    uV */
+				1000000	1060000
+				1176000	1160000
+				>;
+
+			clocks = <&dpll_mpu_ck>;
+			clock-names = "cpu";
+
+			clock-latency = <300000>; /* From omap-cpufreq driver */
+		};
+		cpu@1 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a15";
+			reg = <1>;
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/dra7xx-clocks.dtsi b/arch/arm/boot/dts/dra7xx-clocks.dtsi
index e96da9a898a..c7676871d9c 100644
--- a/arch/arm/boot/dts/dra7xx-clocks.dtsi
+++ b/arch/arm/boot/dts/dra7xx-clocks.dtsi
@@ -1386,6 +1386,14 @@
 		ti,dividers = <1>, <8>;
 	};
 
+	l3init_960m_gfclk: l3init_960m_gfclk {
+		#clock-cells = <0>;
+		compatible = "ti,gate-clock";
+		clocks = <&dpll_usb_clkdcoldo>;
+		ti,bit-shift = <8>;
+		reg = <0x06c0>;
+	};
+
 	dss_32khz_clk: dss_32khz_clk {
 		#clock-cells = <0>;
 		compatible = "ti,gate-clock";
@@ -1533,7 +1541,7 @@
 	usb_otg_ss1_refclk960m: usb_otg_ss1_refclk960m {
 		#clock-cells = <0>;
 		compatible = "ti,gate-clock";
-		clocks = <&dpll_usb_clkdcoldo>;
+		clocks = <&l3init_960m_gfclk>;
 		ti,bit-shift = <8>;
 		reg = <0x13f0>;
 	};
@@ -1541,7 +1549,7 @@
 	usb_otg_ss2_refclk960m: usb_otg_ss2_refclk960m {
 		#clock-cells = <0>;
 		compatible = "ti,gate-clock";
-		clocks = <&dpll_usb_clkdcoldo>;
+		clocks = <&l3init_960m_gfclk>;
 		ti,bit-shift = <8>;
 		reg = <0x1340>;
 	};
@@ -1640,7 +1648,7 @@
 		#clock-cells = <0>;
 		compatible = "ti,mux-clock";
 		clocks = <&abe_24m_fclk>, <&abe_sys_clk_div>, <&func_24m_clk>, <&atlclkin3_ck>, <&atl_clkin2_ck>, <&atl_clkin1_ck>, <&atl_clkin0_ck>, <&sys_clkin2>, <&ref_clkin0_ck>, <&ref_clkin1_ck>, <&ref_clkin2_ck>, <&ref_clkin3_ck>, <&mlb_clk>, <&mlbp_clk>;
-		ti,bit-shift = <28>;
+		ti,bit-shift = <24>;
 		reg = <0x1860>;
 	};
 
diff --git a/arch/arm/boot/dts/exynos3250-pinctrl.dtsi b/arch/arm/boot/dts/exynos3250-pinctrl.dtsi
new file mode 100644
index 00000000000..47b92c150f4
--- /dev/null
+++ b/arch/arm/boot/dts/exynos3250-pinctrl.dtsi
@@ -0,0 +1,475 @@
+/*
+ * Samsung's Exynos3250 SoCs pin-mux and pin-config device tree source
+ *
+ * Copyright (c) 2014 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * Samsung's Exynos3250 SoCs pin-mux and pin-config optiosn are listed as device
+ * tree nodes are listed in this file.
+ *
+ * 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.
+*/
+
+&pinctrl_0 {
+	gpa0: gpa0 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpa1: gpa1 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpb: gpb {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpc0: gpc0 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpc1: gpc1 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpd0: gpd0 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpd1: gpd1 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	uart0_data: uart0-data {
+		samsung,pins = "gpa0-0", "gpa0-1";
+		samsung,pin-function = <0x2>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <0>;
+	};
+
+	uart0_fctl: uart0-fctl {
+		samsung,pins = "gpa0-2", "gpa0-3";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <0>;
+	};
+
+	uart1_data: uart1-data {
+		samsung,pins = "gpa0-4", "gpa0-5";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <0>;
+	};
+
+	uart1_fctl: uart1-fctl {
+		samsung,pins = "gpa0-6", "gpa0-7";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <0>;
+	};
+
+	i2c2_bus: i2c2-bus {
+		samsung,pins = "gpa0-6", "gpa0-7";
+		samsung,pin-function = <3>;
+		samsung,pin-pud = <3>;
+		samsung,pin-drv = <0>;
+	};
+
+	i2c3_bus: i2c3-bus {
+		samsung,pins = "gpa1-2", "gpa1-3";
+		samsung,pin-function = <3>;
+		samsung,pin-pud = <3>;
+		samsung,pin-drv = <0>;
+	};
+
+	spi0_bus: spi0-bus {
+		samsung,pins = "gpb-0", "gpb-2", "gpb-3";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <3>;
+		samsung,pin-drv = <0>;
+	};
+
+	i2c4_bus: i2c4-bus {
+		samsung,pins = "gpb-0", "gpb-1";
+		samsung,pin-function = <3>;
+		samsung,pin-pud = <3>;
+		samsung,pin-drv = <0>;
+	};
+
+	spi1_bus: spi1-bus {
+		samsung,pins = "gpb-4", "gpb-6", "gpb-7";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <3>;
+		samsung,pin-drv = <0>;
+	};
+
+	i2c5_bus: i2c5-bus {
+		samsung,pins = "gpb-2", "gpb-3";
+		samsung,pin-function = <3>;
+		samsung,pin-pud = <3>;
+		samsung,pin-drv = <0>;
+	};
+
+	i2s2_bus: i2s2-bus {
+		samsung,pins = "gpc1-0", "gpc1-1", "gpc1-2", "gpc1-3",
+				"gpc1-4";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <0>;
+	};
+
+	pcm2_bus: pcm2-bus {
+		samsung,pins = "gpc1-0", "gpc1-1", "gpc1-2", "gpc1-3",
+				"gpc1-4";
+		samsung,pin-function = <3>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <0>;
+	};
+
+	i2c6_bus: i2c6-bus {
+		samsung,pins = "gpc1-3", "gpc1-4";
+		samsung,pin-function = <4>;
+		samsung,pin-pud = <3>;
+		samsung,pin-drv = <0>;
+	};
+
+	pwm0_out: pwm0-out {
+		samsung,pins = "gpd0-0";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <0>;
+	};
+
+	pwm1_out: pwm1-out {
+		samsung,pins = "gpd0-1";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <0>;
+	};
+
+	i2c7_bus: i2c7-bus {
+		samsung,pins = "gpd0-2", "gpd0-3";
+		samsung,pin-function = <3>;
+		samsung,pin-pud = <3>;
+		samsung,pin-drv = <0>;
+	};
+
+	pwm2_out: pwm2-out {
+		samsung,pins = "gpd0-2";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <0>;
+	};
+
+	pwm3_out: pwm3-out {
+		samsung,pins = "gpd0-3";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <0>;
+	};
+
+	i2c0_bus: i2c0-bus {
+		samsung,pins = "gpd1-0", "gpd1-1";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <3>;
+		samsung,pin-drv = <0>;
+	};
+
+	mipi0_clk: mipi0-clk {
+		samsung,pins = "gpd1-0", "gpd1-1";
+		samsung,pin-function = <3>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <0>;
+	};
+
+	i2c1_bus: i2c1-bus {
+		samsung,pins = "gpd1-2", "gpd1-3";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <3>;
+		samsung,pin-drv = <0>;
+	};
+};
+
+&pinctrl_1 {
+	gpe0: gpe0 {
+		gpio-controller;
+		#gpio-cells = <2>;
+	};
+
+	gpe1: gpe1 {
+		gpio-controller;
+		#gpio-cells = <2>;
+	};
+
+	gpe2: gpe2 {
+		gpio-controller;
+		#gpio-cells = <2>;
+	};
+
+	gpk0: gpk0 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpk1: gpk1 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpk2: gpk2 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpl0: gpl0 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpm0: gpm0 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpm1: gpm1 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpm2: gpm2 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpm3: gpm3 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpm4: gpm4 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpx0: gpx0 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		interrupt-parent = <&gic>;
+		interrupts = <0 32 0>, <0 33 0>, <0 34 0>, <0 35 0>,
+				<0 36 0>, <0 37 0>, <0 38 0>, <0 39 0>;
+		#interrupt-cells = <2>;
+	};
+
+	gpx1: gpx1 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		interrupt-parent = <&gic>;
+		interrupts = <0 40 0>, <0 41 0>, <0 42 0>, <0 43 0>,
+				<0 44 0>, <0 45 0>, <0 46 0>, <0 47 0>;
+		#interrupt-cells = <2>;
+	};
+
+	gpx2: gpx2 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpx3: gpx3 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	sd0_clk: sd0-clk {
+		samsung,pins = "gpk0-0";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <3>;
+	};
+
+	sd0_cmd: sd0-cmd {
+		samsung,pins = "gpk0-1";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <3>;
+	};
+
+	sd0_cd: sd0-cd {
+		samsung,pins = "gpk0-2";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <3>;
+		samsung,pin-drv = <3>;
+	};
+
+	sd0_rdqs: sd0-rdqs {
+		samsung,pins = "gpk0-7";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <3>;
+	};
+
+	sd0_bus1: sd0-bus-width1 {
+		samsung,pins = "gpk0-3";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <3>;
+		samsung,pin-drv = <3>;
+	};
+
+	sd0_bus4: sd0-bus-width4 {
+		samsung,pins = "gpk0-4", "gpk0-5", "gpk0-6";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <3>;
+		samsung,pin-drv = <3>;
+	};
+
+	sd0_bus8: sd0-bus-width8 {
+		samsung,pins = "gpl0-0", "gpl0-1", "gpl0-2", "gpl0-3";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <3>;
+		samsung,pin-drv = <3>;
+	};
+
+	sd1_clk: sd1-clk {
+		samsung,pins = "gpk1-0";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <3>;
+	};
+
+	sd1_cmd: sd1-cmd {
+		samsung,pins = "gpk1-1";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <3>;
+	};
+
+	sd1_cd: sd1-cd {
+		samsung,pins = "gpk1-2";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <3>;
+		samsung,pin-drv = <3>;
+	};
+
+	sd1_bus1: sd1-bus-width1 {
+		samsung,pins = "gpk1-3";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <3>;
+		samsung,pin-drv = <3>;
+	};
+
+	sd1_bus4: sd1-bus-width4 {
+		samsung,pins = "gpk1-4", "gpk1-5", "gpk1-6";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <3>;
+		samsung,pin-drv = <3>;
+	};
+
+	cam_port_b_io: cam-port-b-io {
+		samsung,pins = "gpm0-0", "gpm0-1", "gpm0-2", "gpm0-3",
+				"gpm0-4", "gpm0-5", "gpm0-6", "gpm0-7",
+				"gpm1-0", "gpm1-1", "gpm2-0", "gpm2-1";
+		samsung,pin-function = <3>;
+		samsung,pin-pud = <3>;
+		samsung,pin-drv = <0>;
+	};
+
+	cam_port_b_clk_active: cam-port-b-clk-active {
+		samsung,pins = "gpm2-2";
+		samsung,pin-function = <3>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <3>;
+	};
+
+	cam_port_b_clk_idle: cam-port-b-clk-idle {
+		samsung,pins = "gpm2-2";
+		samsung,pin-function = <0>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <0>;
+	};
+
+	fimc_is_i2c0: fimc-is-i2c0 {
+		samsung,pins = "gpm4-0", "gpm4-1";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <0>;
+	};
+
+	fimc_is_i2c1: fimc-is-i2c1 {
+		samsung,pins = "gpm4-2", "gpm4-3";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <0>;
+	};
+
+	fimc_is_uart: fimc-is-uart {
+		samsung,pins = "gpm3-5", "gpm3-7";
+		samsung,pin-function = <3>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <0>;
+	};
+};
diff --git a/arch/arm/boot/dts/exynos3250.dtsi b/arch/arm/boot/dts/exynos3250.dtsi
new file mode 100644
index 00000000000..3e678fa335b
--- /dev/null
+++ b/arch/arm/boot/dts/exynos3250.dtsi
@@ -0,0 +1,444 @@
+/*
+ * Samsung's Exynos3250 SoC device tree source
+ *
+ * Copyright (c) 2014 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * Samsung's Exynos3250 SoC device nodes are listed in this file. Exynos3250
+ * based board files can include this file and provide values for board specfic
+ * bindings.
+ *
+ * Note: This file does not include device nodes for all the controllers in
+ * Exynos3250 SoC. As device tree coverage for Exynos3250 increases, additional
+ * nodes can be added to this file.
+ *
+ * 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 "skeleton.dtsi"
+#include <dt-bindings/clock/exynos3250.h>
+
+/ {
+	compatible = "samsung,exynos3250";
+	interrupt-parent = <&gic>;
+
+	aliases {
+		pinctrl0 = &pinctrl_0;
+		pinctrl1 = &pinctrl_1;
+		mshc0 = &mshc_0;
+		mshc1 = &mshc_1;
+		spi0 = &spi_0;
+		spi1 = &spi_1;
+		i2c0 = &i2c_0;
+		i2c1 = &i2c_1;
+		i2c2 = &i2c_2;
+		i2c3 = &i2c_3;
+		i2c4 = &i2c_4;
+		i2c5 = &i2c_5;
+		i2c6 = &i2c_6;
+		i2c7 = &i2c_7;
+	};
+
+	cpus {
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		cpu0: cpu@0 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a7";
+			reg = <0>;
+			clock-frequency = <1000000000>;
+		};
+
+		cpu1: cpu@1 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a7";
+			reg = <1>;
+			clock-frequency = <1000000000>;
+		};
+	};
+
+	soc: soc {
+		compatible = "simple-bus";
+		#address-cells = <1>;
+		#size-cells = <1>;
+		ranges;
+
+		fixed-rate-clocks {
+			#address-cells = <1>;
+			#size-cells = <0>;
+
+			xusbxti: clock@0 {
+				compatible = "fixed-clock";
+				#address-cells = <1>;
+				#size-cells = <0>;
+				reg = <0>;
+				clock-frequency = <0>;
+				#clock-cells = <0>;
+				clock-output-names = "xusbxti";
+			};
+
+			xxti: clock@1 {
+				compatible = "fixed-clock";
+				reg = <1>;
+				clock-frequency = <0>;
+				#clock-cells = <0>;
+				clock-output-names = "xxti";
+			};
+
+			xtcxo: clock@2 {
+				compatible = "fixed-clock";
+				reg = <2>;
+				clock-frequency = <0>;
+				#clock-cells = <0>;
+				clock-output-names = "xtcxo";
+			};
+		};
+
+		sysram@02020000 {
+			compatible = "mmio-sram";
+			reg = <0x02020000 0x40000>;
+			#address-cells = <1>;
+			#size-cells = <1>;
+			ranges = <0 0x02020000 0x40000>;
+
+			smp-sysram@0 {
+				compatible = "samsung,exynos4210-sysram";
+				reg = <0x0 0x1000>;
+			};
+
+			smp-sysram@3f000 {
+				compatible = "samsung,exynos4210-sysram-ns";
+				reg = <0x3f000 0x1000>;
+			};
+		};
+
+		chipid@10000000 {
+			compatible = "samsung,exynos4210-chipid";
+			reg = <0x10000000 0x100>;
+		};
+
+		sys_reg: syscon@10010000 {
+			compatible = "samsung,exynos3-sysreg", "syscon";
+			reg = <0x10010000 0x400>;
+		};
+
+		pmu_system_controller: system-controller@10020000 {
+			compatible = "samsung,exynos3250-pmu", "syscon";
+			reg = <0x10020000 0x4000>;
+		};
+
+		pd_cam: cam-power-domain@10023C00 {
+			compatible = "samsung,exynos4210-pd";
+			reg = <0x10023C00 0x20>;
+		};
+
+		pd_mfc: mfc-power-domain@10023C40 {
+			compatible = "samsung,exynos4210-pd";
+			reg = <0x10023C40 0x20>;
+		};
+
+		pd_g3d: g3d-power-domain@10023C60 {
+			compatible = "samsung,exynos4210-pd";
+			reg = <0x10023C60 0x20>;
+		};
+
+		pd_lcd0: lcd0-power-domain@10023C80 {
+			compatible = "samsung,exynos4210-pd";
+			reg = <0x10023C80 0x20>;
+		};
+
+		pd_isp: isp-power-domain@10023CA0 {
+			compatible = "samsung,exynos4210-pd";
+			reg = <0x10023CA0 0x20>;
+		};
+
+		cmu: clock-controller@10030000 {
+			compatible = "samsung,exynos3250-cmu";
+			reg = <0x10030000 0x20000>;
+			#clock-cells = <1>;
+		};
+
+		rtc: rtc@10070000 {
+			compatible = "samsung,s3c6410-rtc";
+			reg = <0x10070000 0x100>;
+			interrupts = <0 73 0>, <0 74 0>;
+			status = "disabled";
+		};
+
+		gic: interrupt-controller@10481000 {
+			compatible = "arm,cortex-a15-gic";
+			#interrupt-cells = <3>;
+			interrupt-controller;
+			reg = <0x10481000 0x1000>,
+			      <0x10482000 0x1000>,
+			      <0x10484000 0x2000>,
+			      <0x10486000 0x2000>;
+			interrupts = <1 9 0xf04>;
+		};
+
+		mct@10050000 {
+			compatible = "samsung,exynos4210-mct";
+			reg = <0x10050000 0x800>;
+			interrupts = <0 218 0>, <0 219 0>, <0 220 0>, <0 221 0>,
+				     <0 223 0>, <0 226 0>, <0 227 0>, <0 228 0>;
+			clocks = <&cmu CLK_FIN_PLL>, <&cmu CLK_MCT>;
+			clock-names = "fin_pll", "mct";
+		};
+
+		pinctrl_1: pinctrl@11000000 {
+			compatible = "samsung,exynos3250-pinctrl";
+			reg = <0x11000000 0x1000>;
+			interrupts = <0 225 0>;
+
+			wakeup-interrupt-controller {
+				compatible = "samsung,exynos4210-wakeup-eint";
+				interrupt-parent = <&gic>;
+				interrupts = <0 48 0>;
+			};
+		};
+
+		pinctrl_0: pinctrl@11400000 {
+			compatible = "samsung,exynos3250-pinctrl";
+			reg = <0x11400000 0x1000>;
+			interrupts = <0 240 0>;
+		};
+
+		mshc_0: mshc@12510000 {
+			compatible = "samsung,exynos5250-dw-mshc";
+			reg = <0x12510000 0x1000>;
+			interrupts = <0 142 0>;
+			clocks = <&cmu CLK_SDMMC0>, <&cmu CLK_SCLK_MMC0>;
+			clock-names = "biu", "ciu";
+			fifo-depth = <0x80>;
+			#address-cells = <1>;
+			#size-cells = <0>;
+			status = "disabled";
+		};
+
+		mshc_1: mshc@12520000 {
+			compatible = "samsung,exynos5250-dw-mshc";
+			reg = <0x12520000 0x1000>;
+			interrupts = <0 143 0>;
+			clocks = <&cmu CLK_SDMMC1>, <&cmu CLK_SCLK_MMC1>;
+			clock-names = "biu", "ciu";
+			fifo-depth = <0x80>;
+			#address-cells = <1>;
+			#size-cells = <0>;
+			status = "disabled";
+		};
+
+		amba {
+			compatible = "arm,amba-bus";
+			#address-cells = <1>;
+			#size-cells = <1>;
+			interrupt-parent = <&gic>;
+			ranges;
+
+			pdma0: pdma@12680000 {
+				compatible = "arm,pl330", "arm,primecell";
+				reg = <0x12680000 0x1000>;
+				interrupts = <0 138 0>;
+				clocks = <&cmu CLK_PDMA0>;
+				clock-names = "apb_pclk";
+				#dma-cells = <1>;
+				#dma-channels = <8>;
+				#dma-requests = <32>;
+			};
+
+			pdma1: pdma@12690000 {
+				compatible = "arm,pl330", "arm,primecell";
+				reg = <0x12690000 0x1000>;
+				interrupts = <0 139 0>;
+				clocks = <&cmu CLK_PDMA1>;
+				clock-names = "apb_pclk";
+				#dma-cells = <1>;
+				#dma-channels = <8>;
+				#dma-requests = <32>;
+			};
+		};
+
+		adc: adc@126C0000 {
+			compatible = "samsung,exynos-adc-v3";
+			reg = <0x126C0000 0x100>, <0x10020718 0x4>;
+			interrupts = <0 137 0>;
+			clock-names = "adc", "sclk_tsadc";
+			clocks = <&cmu CLK_TSADC>, <&cmu CLK_SCLK_TSADC>;
+			#io-channel-cells = <1>;
+			io-channel-ranges;
+			status = "disabled";
+		};
+
+		serial_0: serial@13800000 {
+			compatible = "samsung,exynos4210-uart";
+			reg = <0x13800000 0x100>;
+			interrupts = <0 109 0>;
+			clocks = <&cmu CLK_UART0>, <&cmu CLK_SCLK_UART0>;
+			clock-names = "uart", "clk_uart_baud0";
+			status = "disabled";
+		};
+
+		serial_1: serial@13810000 {
+			compatible = "samsung,exynos4210-uart";
+			reg = <0x13810000 0x100>;
+			interrupts = <0 110 0>;
+			clocks = <&cmu CLK_UART1>, <&cmu CLK_SCLK_UART1>;
+			clock-names = "uart", "clk_uart_baud0";
+			status = "disabled";
+		};
+
+		i2c_0: i2c@13860000 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			compatible = "samsung,s3c2440-i2c";
+			reg = <0x13860000 0x100>;
+			interrupts = <0 113 0>;
+			clocks = <&cmu CLK_I2C0>;
+			clock-names = "i2c";
+			pinctrl-names = "default";
+			pinctrl-0 = <&i2c0_bus>;
+			status = "disabled";
+		};
+
+		i2c_1: i2c@13870000 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			compatible = "samsung,s3c2440-i2c";
+			reg = <0x13870000 0x100>;
+			interrupts = <0 114 0>;
+			clocks = <&cmu CLK_I2C1>;
+			clock-names = "i2c";
+			pinctrl-names = "default";
+			pinctrl-0 = <&i2c1_bus>;
+			status = "disabled";
+		};
+
+		i2c_2: i2c@13880000 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			compatible = "samsung,s3c2440-i2c";
+			reg = <0x13880000 0x100>;
+			interrupts = <0 115 0>;
+			clocks = <&cmu CLK_I2C2>;
+			clock-names = "i2c";
+			pinctrl-names = "default";
+			pinctrl-0 = <&i2c2_bus>;
+			status = "disabled";
+		};
+
+		i2c_3: i2c@13890000 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			compatible = "samsung,s3c2440-i2c";
+			reg = <0x13890000 0x100>;
+			interrupts = <0 116 0>;
+			clocks = <&cmu CLK_I2C3>;
+			clock-names = "i2c";
+			pinctrl-names = "default";
+			pinctrl-0 = <&i2c3_bus>;
+			status = "disabled";
+		};
+
+		i2c_4: i2c@138A0000 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			compatible = "samsung,s3c2440-i2c";
+			reg = <0x138A0000 0x100>;
+			interrupts = <0 117 0>;
+			clocks = <&cmu CLK_I2C4>;
+			clock-names = "i2c";
+			pinctrl-names = "default";
+			pinctrl-0 = <&i2c4_bus>;
+			status = "disabled";
+		};
+
+		i2c_5: i2c@138B0000 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			compatible = "samsung,s3c2440-i2c";
+			reg = <0x138B0000 0x100>;
+			interrupts = <0 118 0>;
+			clocks = <&cmu CLK_I2C5>;
+			clock-names = "i2c";
+			pinctrl-names = "default";
+			pinctrl-0 = <&i2c5_bus>;
+			status = "disabled";
+		};
+
+		i2c_6: i2c@138C0000 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			compatible = "samsung,s3c2440-i2c";
+			reg = <0x138C0000 0x100>;
+			interrupts = <0 119 0>;
+			clocks = <&cmu CLK_I2C6>;
+			clock-names = "i2c";
+			pinctrl-names = "default";
+			pinctrl-0 = <&i2c6_bus>;
+			status = "disabled";
+		};
+
+		i2c_7: i2c@138D0000 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			compatible = "samsung,s3c2440-i2c";
+			reg = <0x138D0000 0x100>;
+			interrupts = <0 120 0>;
+			clocks = <&cmu CLK_I2C7>;
+			clock-names = "i2c";
+			pinctrl-names = "default";
+			pinctrl-0 = <&i2c7_bus>;
+			status = "disabled";
+		};
+
+		spi_0: spi@13920000 {
+			compatible = "samsung,exynos4210-spi";
+			reg = <0x13920000 0x100>;
+			interrupts = <0 121 0>;
+			dmas = <&pdma0 7>, <&pdma0 6>;
+			dma-names = "tx", "rx";
+			#address-cells = <1>;
+			#size-cells = <0>;
+			clocks = <&cmu CLK_SPI0>, <&cmu CLK_SCLK_SPI0>;
+			clock-names = "spi", "spi_busclk0";
+			samsung,spi-src-clk = <0>;
+			pinctrl-names = "default";
+			pinctrl-0 = <&spi0_bus>;
+			status = "disabled";
+		};
+
+		spi_1: spi@13930000 {
+			compatible = "samsung,exynos4210-spi";
+			reg = <0x13930000 0x100>;
+			interrupts = <0 122 0>;
+			dmas = <&pdma1 7>, <&pdma1 6>;
+			dma-names = "tx", "rx";
+			#address-cells = <1>;
+			#size-cells = <0>;
+			clocks = <&cmu CLK_SPI1>, <&cmu CLK_SCLK_SPI1>;
+			clock-names = "spi", "spi_busclk0";
+			samsung,spi-src-clk = <0>;
+			pinctrl-names = "default";
+			pinctrl-0 = <&spi1_bus>;
+			status = "disabled";
+		};
+
+		pwm: pwm@139D0000 {
+			compatible = "samsung,exynos4210-pwm";
+			reg = <0x139D0000 0x1000>;
+			interrupts = <0 104 0>, <0 105 0>, <0 106 0>,
+				     <0 107 0>, <0 108 0>;
+			#pwm-cells = <3>;
+			status = "disabled";
+		};
+
+		pmu {
+			compatible = "arm,cortex-a7-pmu";
+			interrupts = <0 18 0>, <0 19 0>;
+		};
+	};
+};
+
+#include "exynos3250-pinctrl.dtsi"
diff --git a/arch/arm/boot/dts/exynos4.dtsi b/arch/arm/boot/dts/exynos4.dtsi
index 2f8bcd068d1..b8ece4be41c 100644
--- a/arch/arm/boot/dts/exynos4.dtsi
+++ b/arch/arm/boot/dts/exynos4.dtsi
@@ -20,6 +20,7 @@
  */
 
 #include <dt-bindings/clock/exynos4.h>
+#include <dt-bindings/clock/exynos-audss-clk.h>
 #include "skeleton.dtsi"
 
 / {
@@ -45,6 +46,23 @@
 		fimc3 = &fimc_3;
 	};
 
+	clock_audss: clock-controller@03810000 {
+		compatible = "samsung,exynos4210-audss-clock";
+		reg = <0x03810000 0x0C>;
+		#clock-cells = <1>;
+	};
+
+	i2s0: i2s@03830000 {
+		compatible = "samsung,s5pv210-i2s";
+		reg = <0x03830000 0x100>;
+		clocks = <&clock_audss EXYNOS_I2S_BUS>;
+		clock-names = "iis";
+		dmas = <&pdma0 12>, <&pdma0 11>, <&pdma0 10>;
+		dma-names = "tx", "rx", "tx-sec";
+		samsung,idma-addr = <0x03000000>;
+		status = "disabled";
+	};
+
 	chipid@10000000 {
 		compatible = "samsung,exynos4210-chipid";
 		reg = <0x10000000 0x100>;
@@ -110,6 +128,11 @@
 		reg = <0x10010000 0x400>;
 	};
 
+	pmu_system_controller: system-controller@10020000 {
+		compatible = "samsung,exynos4210-pmu", "syscon";
+		reg = <0x10020000 0x4000>;
+	};
+
 	dsi_0: dsi@11C80000 {
 		compatible = "samsung,exynos4210-mipi-dsi";
 		reg = <0x11C80000 0x10000>;
@@ -117,7 +140,7 @@
 		samsung,power-domain = <&pd_lcd0>;
 		phys = <&mipi_phy 1>;
 		phy-names = "dsim";
-		clocks = <&clock 286>, <&clock 143>;
+		clocks = <&clock CLK_DSIM0>, <&clock CLK_SCLK_MIPI0>;
 		clock-names = "bus_clk", "pll_clk";
 		status = "disabled";
 		#address-cells = <1>;
@@ -129,12 +152,10 @@
 		status = "disabled";
 		#address-cells = <1>;
 		#size-cells = <1>;
+		#clock-cells = <1>;
+		clock-output-names = "cam_a_clkout", "cam_b_clkout";
 		ranges;
 
-		clock_cam: clock-controller {
-			 #clock-cells = <1>;
-		};
-
 		fimc_0: fimc@11800000 {
 			compatible = "samsung,exynos4210-fimc";
 			reg = <0x11800000 0x1000>;
@@ -273,6 +294,27 @@
 		status = "disabled";
 	};
 
+	exynos_usbphy: exynos-usbphy@125B0000 {
+		compatible = "samsung,exynos4210-usb2-phy";
+		reg = <0x125B0000 0x100>;
+		samsung,pmureg-phandle = <&pmu_system_controller>;
+		clocks = <&clock CLK_USB_DEVICE>, <&clock CLK_XUSBXTI>;
+		clock-names = "phy", "ref";
+		#phy-cells = <1>;
+		status = "disabled";
+	};
+
+	hsotg@12480000 {
+		compatible = "samsung,s3c6400-hsotg";
+		reg = <0x12480000 0x20000>;
+		interrupts = <0 71 0>;
+		clocks = <&clock CLK_USB_DEVICE>;
+		clock-names = "otg";
+		phys = <&exynos_usbphy 0>;
+		phy-names = "usb2-phy";
+		status = "disabled";
+	};
+
 	ehci@12580000 {
 		compatible = "samsung,exynos4210-ehci";
 		reg = <0x12580000 0x100>;
@@ -291,6 +333,26 @@
 		status = "disabled";
 	};
 
+	i2s1: i2s@13960000 {
+		compatible = "samsung,s5pv210-i2s";
+		reg = <0x13960000 0x100>;
+		clocks = <&clock CLK_I2S1>;
+		clock-names = "iis";
+		dmas = <&pdma1 12>, <&pdma1 11>;
+		dma-names = "tx", "rx";
+		status = "disabled";
+	};
+
+	i2s2: i2s@13970000 {
+		compatible = "samsung,s5pv210-i2s";
+		reg = <0x13970000 0x100>;
+		clocks = <&clock CLK_I2S2>;
+		clock-names = "iis";
+		dmas = <&pdma0 14>, <&pdma0 13>;
+		dma-names = "tx", "rx";
+		status = "disabled";
+	};
+
 	mfc: codec@13400000 {
 		compatible = "samsung,mfc-v5";
 		reg = <0x13400000 0x10000>;
@@ -371,6 +433,8 @@
 		interrupts = <0 60 0>;
 		clocks = <&clock CLK_I2C2>;
 		clock-names = "i2c";
+		pinctrl-names = "default";
+		pinctrl-0 = <&i2c2_bus>;
 		status = "disabled";
 	};
 
@@ -382,6 +446,8 @@
 		interrupts = <0 61 0>;
 		clocks = <&clock CLK_I2C3>;
 		clock-names = "i2c";
+		pinctrl-names = "default";
+		pinctrl-0 = <&i2c3_bus>;
 		status = "disabled";
 	};
 
@@ -393,6 +459,8 @@
 		interrupts = <0 62 0>;
 		clocks = <&clock CLK_I2C4>;
 		clock-names = "i2c";
+		pinctrl-names = "default";
+		pinctrl-0 = <&i2c4_bus>;
 		status = "disabled";
 	};
 
@@ -404,6 +472,8 @@
 		interrupts = <0 63 0>;
 		clocks = <&clock CLK_I2C5>;
 		clock-names = "i2c";
+		pinctrl-names = "default";
+		pinctrl-0 = <&i2c5_bus>;
 		status = "disabled";
 	};
 
@@ -415,6 +485,8 @@
 		interrupts = <0 64 0>;
 		clocks = <&clock CLK_I2C6>;
 		clock-names = "i2c";
+		pinctrl-names = "default";
+		pinctrl-0 = <&i2c6_bus>;
 		status = "disabled";
 	};
 
@@ -426,6 +498,8 @@
 		interrupts = <0 65 0>;
 		clocks = <&clock CLK_I2C7>;
 		clock-names = "i2c";
+		pinctrl-names = "default";
+		pinctrl-0 = <&i2c7_bus>;
 		status = "disabled";
 	};
 
diff --git a/arch/arm/boot/dts/exynos4210-origen.dts b/arch/arm/boot/dts/exynos4210-origen.dts
index 72fb11f7ea2..f767c425d0b 100644
--- a/arch/arm/boot/dts/exynos4210-origen.dts
+++ b/arch/arm/boot/dts/exynos4210-origen.dts
@@ -16,6 +16,7 @@
 
 /dts-v1/;
 #include "exynos4210.dtsi"
+#include <dt-bindings/input/input.h>
 
 / {
 	model = "Insignal Origen evaluation board based on Exynos4210";
@@ -48,6 +49,14 @@
 		};
 	};
 
+	watchdog@10060000 {
+		status = "okay";
+	};
+
+	rtc@10070000 {
+		status = "okay";
+	};
+
 	tmu@100C0000 {
 		status = "okay";
 	};
@@ -251,35 +260,35 @@
 		up {
 			label = "Up";
 			gpios = <&gpx2 0 1>;
-			linux,code = <103>;
+			linux,code = <KEY_UP>;
 			gpio-key,wakeup;
 		};
 
 		down {
 			label = "Down";
 			gpios = <&gpx2 1 1>;
-			linux,code = <108>;
+			linux,code = <KEY_DOWN>;
 			gpio-key,wakeup;
 		};
 
 		back {
 			label = "Back";
 			gpios = <&gpx1 7 1>;
-			linux,code = <158>;
+			linux,code = <KEY_BACK>;
 			gpio-key,wakeup;
 		};
 
 		home {
 			label = "Home";
 			gpios = <&gpx1 6 1>;
-			linux,code = <102>;
+			linux,code = <KEY_HOME>;
 			gpio-key,wakeup;
 		};
 
 		menu {
 			label = "Menu";
 			gpios = <&gpx1 5 1>;
-			linux,code = <139>;
+			linux,code = <KEY_MENU>;
 			gpio-key,wakeup;
 		};
 	};
diff --git a/arch/arm/boot/dts/exynos4210-trats.dts b/arch/arm/boot/dts/exynos4210-trats.dts
index 63aa2bb24a4..f516da9e8b3 100644
--- a/arch/arm/boot/dts/exynos4210-trats.dts
+++ b/arch/arm/boot/dts/exynos4210-trats.dts
@@ -88,6 +88,12 @@
 		};
 	};
 
+	hsotg@12480000 {
+		vusb_d-supply = <&vusb_reg>;
+		vusb_a-supply = <&vusbdac_reg>;
+		status = "okay";
+	};
+
 	sdhci_emmc: sdhci@12510000 {
 		bus-width = <8>;
 		non-removable;
@@ -97,6 +103,10 @@
 		status = "okay";
 	};
 
+	exynos-usbphy@125B0000 {
+		status = "okay";
+	};
+
 	serial@13800000 {
 		status = "okay";
 	};
diff --git a/arch/arm/boot/dts/exynos4210-universal_c210.dts b/arch/arm/boot/dts/exynos4210-universal_c210.dts
index 63e34b24b04..d50eb3aa708 100644
--- a/arch/arm/boot/dts/exynos4210-universal_c210.dts
+++ b/arch/arm/boot/dts/exynos4210-universal_c210.dts
@@ -28,6 +28,21 @@
 		bootargs = "console=ttySAC2,115200N8 root=/dev/mmcblk0p5 rw rootwait earlyprintk panic=5 maxcpus=1";
 	};
 
+	sysram@02020000 {
+		smp-sysram@0 {
+			status = "disabled";
+		};
+
+		smp-sysram@5000 {
+			compatible = "samsung,exynos4210-sysram";
+			reg = <0x5000 0x1000>;
+		};
+
+		smp-sysram@1f000 {
+			status = "disabled";
+		};
+	};
+
 	mct@10050000 {
 		compatible = "none";
 	};
@@ -53,6 +68,12 @@
 		enable-active-high;
 	};
 
+	hsotg@12480000 {
+		vusb_d-supply = <&ldo3_reg>;
+		vusb_a-supply = <&ldo8_reg>;
+		status = "okay";
+	};
+
 	sdhci_emmc: sdhci@12510000 {
 		bus-width = <8>;
 		non-removable;
@@ -62,6 +83,34 @@
 		status = "okay";
 	};
 
+	sdhci_sd: sdhci@12530000 {
+		bus-width = <4>;
+		pinctrl-0 = <&sd2_clk &sd2_cmd &sd2_bus4>;
+		pinctrl-names = "default";
+		vmmc-supply = <&ldo5_reg>;
+		cd-gpios = <&gpx3 4 0>;
+		cd-inverted;
+		status = "okay";
+	};
+
+	ehci@12580000 {
+		status = "okay";
+		port@0 {
+			status = "okay";
+		};
+	};
+
+	ohci@12590000 {
+		status = "okay";
+		port@0 {
+			status = "okay";
+		};
+	};
+
+	exynos-usbphy@125B0000 {
+		status = "okay";
+	};
+
 	serial@13800000 {
 		status = "okay";
 	};
@@ -201,6 +250,7 @@
 					regulator-name = "VUSB+MIPI_1.1V";
 					regulator-min-microvolt = <1100000>;
 					regulator-max-microvolt = <1100000>;
+					regulator-always-on;
 				};
 
 				ldo4_reg: LDO4 {
@@ -231,6 +281,7 @@
 					regulator-name = "VUSB+VDAC_3.3V";
 					regulator-min-microvolt = <3300000>;
 					regulator-max-microvolt = <3300000>;
+					regulator-always-on;
 				};
 
 				ldo9_reg: LDO9 {
@@ -413,6 +464,29 @@
 		compatible = "samsung,s5p6440-pwm";
 		status = "okay";
 	};
+
+	camera {
+		status = "okay";
+
+		pinctrl-names = "default";
+		pinctrl-0 = <>;
+
+		fimc_0: fimc@11800000 {
+			status = "okay";
+		};
+
+		fimc_1: fimc@11810000 {
+			status = "okay";
+		};
+
+		fimc_2: fimc@11820000 {
+			status = "okay";
+		};
+
+		fimc_3: fimc@11830000 {
+			status = "okay";
+		};
+	};
 };
 
 &mdma1 {
diff --git a/arch/arm/boot/dts/exynos4210.dtsi b/arch/arm/boot/dts/exynos4210.dtsi
index cacf6140dd2..ee3001f3882 100644
--- a/arch/arm/boot/dts/exynos4210.dtsi
+++ b/arch/arm/boot/dts/exynos4210.dtsi
@@ -31,6 +31,24 @@
 		pinctrl2 = &pinctrl_2;
 	};
 
+	sysram@02020000 {
+		compatible = "mmio-sram";
+		reg = <0x02020000 0x20000>;
+		#address-cells = <1>;
+		#size-cells = <1>;
+		ranges = <0 0x02020000 0x20000>;
+
+		smp-sysram@0 {
+			compatible = "samsung,exynos4210-sysram";
+			reg = <0x0 0x1000>;
+		};
+
+		smp-sysram@1f000 {
+			compatible = "samsung,exynos4210-sysram-ns";
+			reg = <0x1f000 0x1000>;
+		};
+	};
+
 	pd_lcd1: lcd1-power-domain@10023CA0 {
 		compatible = "samsung,exynos4210-pd";
 		reg = <0x10023CA0 0x20>;
diff --git a/arch/arm/boot/dts/exynos4412-origen.dts b/arch/arm/boot/dts/exynos4412-origen.dts
index e2c0dcab4d8..e925c9fbfb0 100644
--- a/arch/arm/boot/dts/exynos4412-origen.dts
+++ b/arch/arm/boot/dts/exynos4412-origen.dts
@@ -14,6 +14,7 @@
 
 /dts-v1/;
 #include "exynos4412.dtsi"
+#include <dt-bindings/input/input.h>
 
 / {
 	model = "Insignal Origen evaluation board based on Exynos4412";
@@ -48,6 +49,14 @@
 		};
 	};
 
+	watchdog@10060000 {
+		status = "okay";
+	};
+
+	rtc@10070000 {
+		status = "okay";
+	};
+
 	pinctrl@11000000 {
 		keypad_rows: keypad-rows {
 			samsung,pins = "gpx2-0", "gpx2-1", "gpx2-2";
@@ -76,37 +85,37 @@
 		key_home {
 			keypad,row = <0>;
 			keypad,column = <0>;
-			linux,code = <102>;
+			linux,code = <KEY_HOME>;
 		};
 
 		key_down {
 			keypad,row = <0>;
 			keypad,column = <1>;
-			linux,code = <108>;
+			linux,code = <KEY_DOWN>;
 		};
 
 		key_up {
 			keypad,row = <1>;
 			keypad,column = <0>;
-			linux,code = <103>;
+			linux,code = <KEY_UP>;
 		};
 
 		key_menu {
 			keypad,row = <1>;
 			keypad,column = <1>;
-			linux,code = <139>;
+			linux,code = <KEY_MENU>;
 		};
 
 		key_back {
 			keypad,row = <2>;
 			keypad,column = <0>;
-			linux,code = <158>;
+			linux,code = <KEY_BACK>;
 		};
 
 		key_enter {
 			keypad,row = <2>;
 			keypad,column = <1>;
-			linux,code = <28>;
+			linux,code = <KEY_ENTER>;
 		};
 	};
 
diff --git a/arch/arm/boot/dts/exynos4412-trats2.dts b/arch/arm/boot/dts/exynos4412-trats2.dts
index 9583563dd0e..77878447b31 100644
--- a/arch/arm/boot/dts/exynos4412-trats2.dts
+++ b/arch/arm/boot/dts/exynos4412-trats2.dts
@@ -20,7 +20,8 @@
 	compatible = "samsung,trats2", "samsung,exynos4412", "samsung,exynos4";
 
 	aliases {
-		i2c8 = &i2c_ak8975;
+		i2c9 = &i2c_ak8975;
+		i2c10 = &i2c_cm36651;
 	};
 
 	memory {
@@ -80,39 +81,67 @@
 			enable-active-high;
 		};
 
-		/* More to come */
+		cam_af_reg: voltage-regulator-3 {
+			compatible = "regulator-fixed";
+			regulator-name = "CAM_AF";
+			regulator-min-microvolt = <2800000>;
+			regulator-max-microvolt = <2800000>;
+			gpio = <&gpm0 4 0>;
+			enable-active-high;
+		};
+
+		cam_isp_core_reg: voltage-regulator-4 {
+			compatible = "regulator-fixed";
+			regulator-name = "CAM_ISP_CORE_1.2V_EN";
+			regulator-min-microvolt = <1200000>;
+			regulator-max-microvolt = <1200000>;
+			gpio = <&gpm0 3 0>;
+			enable-active-high;
+			regulator-always-on;
+		};
+
+		ps_als_reg: voltage-regulator-5 {
+			compatible = "regulator-fixed";
+			regulator-name = "LED_A_3.0V";
+			regulator-min-microvolt = <3000000>;
+			regulator-max-microvolt = <3000000>;
+			gpio = <&gpj0 5 0>;
+			enable-active-high;
+		};
 	};
 
 	gpio-keys {
 		compatible = "gpio-keys";
 
 		key-down {
-			interrupt-parent = <&gpj1>;
-			interrupts = <2 0>;
-			gpios = <&gpj1 2 1>;
+			gpios = <&gpx3 3 1>;
 			linux,code = <114>;
 			label = "volume down";
 			debounce-interval = <10>;
 		};
 
 		key-up {
-			interrupt-parent = <&gpj1>;
-			interrupts = <1 0>;
-			gpios = <&gpj1 1 1>;
+			gpios = <&gpx2 2 1>;
 			linux,code = <115>;
 			label = "volume up";
 			debounce-interval = <10>;
 		};
 
 		key-power {
-			interrupt-parent = <&gpx2>;
-			interrupts = <7 0>;
 			gpios = <&gpx2 7 1>;
 			linux,code = <116>;
 			label = "power";
 			debounce-interval = <10>;
 			gpio-key,wakeup;
 		};
+
+		key-ok {
+			gpios = <&gpx0 1 1>;
+			linux,code = <139>;
+			label = "ok";
+			debounce-inteval = <10>;
+			gpio-key,wakeup;
+		};
 	};
 
 	adc: adc@126C0000 {
@@ -140,6 +169,38 @@
 		};
 	};
 
+	i2c_0: i2c@13860000 {
+		samsung,i2c-sda-delay = <100>;
+		samsung,i2c-slave-addr = <0x10>;
+		samsung,i2c-max-bus-freq = <400000>;
+		pinctrl-0 = <&i2c0_bus>;
+		pinctrl-names = "default";
+		status = "okay";
+
+		s5c73m3@3c {
+			compatible = "samsung,s5c73m3";
+			reg = <0x3c>;
+			standby-gpios = <&gpm0 1 1>;   /* ISP_STANDBY */
+			xshutdown-gpios = <&gpf1 3 1>; /* ISP_RESET */
+			vdd-int-supply = <&buck9_reg>;
+			vddio-cis-supply = <&ldo9_reg>;
+			vdda-supply = <&ldo17_reg>;
+			vddio-host-supply = <&ldo18_reg>;
+			vdd-af-supply = <&cam_af_reg>;
+			vdd-reg-supply = <&cam_io_reg>;
+			clock-frequency = <24000000>;
+			/* CAM_A_CLKOUT */
+			clocks = <&camera 0>;
+			clock-names = "cis_extclk";
+			port {
+				s5c73m3_ep: endpoint {
+					remote-endpoint = <&csis0_ep>;
+					data-lanes = <1 2 3 4>;
+				};
+			};
+		};
+	};
+
 	i2c@138D0000 {
 		samsung,i2c-sda-delay = <100>;
 		samsung,i2c-slave-addr = <0x10>;
@@ -503,12 +564,28 @@
 		status = "okay";
 
 		ak8975@0c {
-			compatible = "ak,ak8975";
+			compatible = "asahi-kasei,ak8975";
 			reg = <0x0c>;
 			gpios = <&gpj0 7 0>;
 		};
 	};
 
+	i2c_cm36651: i2c-gpio-2 {
+		compatible = "i2c-gpio";
+		gpios = <&gpf0 0 1>, <&gpf0 1 1>;
+		i2c-gpio,delay-us = <2>;
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		cm36651@18 {
+			compatible = "capella,cm36651";
+			reg = <0x18>;
+			interrupt-parent = <&gpx0>;
+			interrupts = <2 2>;
+			vled-supply = <&ps_als_reg>;
+		};
+	};
+
 	spi_1: spi@13930000 {
 		pinctrl-names = "default";
 		pinctrl-0 = <&spi1_bus>;
@@ -586,8 +663,8 @@
 		status = "okay";
 	};
 
-	camera {
-		pinctrl-0 = <&cam_port_b_clk_active>;
+	camera: camera {
+		pinctrl-0 = <&cam_port_a_clk_active &cam_port_b_clk_active>;
 		pinctrl-names = "default";
 		status = "okay";
 
@@ -607,6 +684,23 @@
 			status = "okay";
 		};
 
+		csis_0: csis@11880000 {
+			status = "okay";
+			vddcore-supply = <&ldo8_reg>;
+			vddio-supply = <&ldo10_reg>;
+			clock-frequency = <176000000>;
+
+			/* Camera C (3) MIPI CSI-2 (CSIS0) */
+			port@3 {
+				reg = <3>;
+				csis0_ep: endpoint {
+					remote-endpoint = <&s5c73m3_ep>;
+					data-lanes = <1 2 3 4>;
+					samsung,csis-hs-settle = <12>;
+				};
+			};
+		};
+
 		csis_1: csis@11890000 {
 			vddcore-supply = <&ldo8_reg>;
 			vddio-supply = <&ldo10_reg>;
@@ -647,10 +741,11 @@
 					reg = <0x10>;
 					svdda-supply = <&cam_io_reg>;
 					svddio-supply = <&ldo19_reg>;
+					afvdd-supply = <&ldo19_reg>;
 					clock-frequency = <24000000>;
 					/* CAM_B_CLKOUT */
-					clocks = <&clock_cam 1>;
-					clock-names = "mclk";
+					clocks = <&camera 1>;
+					clock-names = "extclk";
 					samsung,camclk-out = <1>;
 					gpios = <&gpm1 6 0>;
 
@@ -665,6 +760,16 @@
 		};
 	};
 
+	exynos-usbphy@125B0000 {
+		status = "okay";
+	};
+
+	hsotg@12480000 {
+		vusb_d-supply = <&ldo15_reg>;
+		vusb_a-supply = <&ldo12_reg>;
+		status = "okay";
+	};
+
 	thermistor-ap@0 {
 		compatible = "ntc,ncp15wb473";
 		pullup-uv = <1800000>;	 /* VCC_1.8V_AP */
diff --git a/arch/arm/boot/dts/exynos4412.dtsi b/arch/arm/boot/dts/exynos4412.dtsi
index 15d3c0ac2f5..c42a3e196cd 100644
--- a/arch/arm/boot/dts/exynos4412.dtsi
+++ b/arch/arm/boot/dts/exynos4412.dtsi
@@ -29,4 +29,8 @@
 	gic: interrupt-controller@10490000 {
 		cpu-offset = <0x4000>;
 	};
+
+	pmu_system_controller: system-controller@10020000 {
+		compatible = "samsung,exynos4412-pmu", "syscon";
+	};
 };
diff --git a/arch/arm/boot/dts/exynos4x12.dtsi b/arch/arm/boot/dts/exynos4x12.dtsi
index c4a9306f852..c5a943df1cd 100644
--- a/arch/arm/boot/dts/exynos4x12.dtsi
+++ b/arch/arm/boot/dts/exynos4x12.dtsi
@@ -37,6 +37,24 @@
 		interrupts = <2 2>, <3 2>, <18 2>, <19 2>;
 	};
 
+	sysram@02020000 {
+		compatible = "mmio-sram";
+		reg = <0x02020000 0x40000>;
+		#address-cells = <1>;
+		#size-cells = <1>;
+		ranges = <0 0x02020000 0x40000>;
+
+		smp-sysram@0 {
+			compatible = "samsung,exynos4210-sysram";
+			reg = <0x0 0x1000>;
+		};
+
+		smp-sysram@2f000 {
+			compatible = "samsung,exynos4210-sysram-ns";
+			reg = <0x2f000 0x1000>;
+		};
+	};
+
 	pd_isp: isp-power-domain@10023CA0 {
 		compatible = "samsung,exynos4210-pd";
 		reg = <0x10023CA0 0x20>;
@@ -119,6 +137,10 @@
 		interrupts = <0 72 0>;
 	};
 
+	pmu_system_controller: system-controller@10020000 {
+		compatible = "samsung,exynos4212-pmu", "syscon";
+	};
+
 	g2d@10800000 {
 		compatible = "samsung,exynos4212-g2d";
 		reg = <0x10800000 0x1000>;
@@ -243,4 +265,9 @@
 		clock-names = "biu", "ciu";
 		status = "disabled";
 	};
+
+	exynos-usbphy@125B0000 {
+		compatible = "samsung,exynos4x12-usb2-phy";
+		samsung,sysreg-phandle = <&sys_reg>;
+	};
 };
diff --git a/arch/arm/boot/dts/exynos5250-arndale.dts b/arch/arm/boot/dts/exynos5250-arndale.dts
index 090f9830b12..d0de1f50d15 100644
--- a/arch/arm/boot/dts/exynos5250-arndale.dts
+++ b/arch/arm/boot/dts/exynos5250-arndale.dts
@@ -12,6 +12,7 @@
 /dts-v1/;
 #include "exynos5250.dtsi"
 #include <dt-bindings/interrupt-controller/irq.h>
+#include <dt-bindings/input/input.h>
 
 / {
 	model = "Insignal Arndale evaluation board based on EXYNOS5250";
@@ -107,6 +108,7 @@
 					regulator-name = "VDD_IOPERI_1.8V";
 					regulator-min-microvolt = <1800000>;
 					regulator-max-microvolt = <1800000>;
+					regulator-always-on;
 					op_mode = <1>;
 				};
 
@@ -444,42 +446,42 @@
 		menu {
 			label = "SW-TACT2";
 			gpios = <&gpx1 4 1>;
-			linux,code = <139>;
+			linux,code = <KEY_MENU>;
 			gpio-key,wakeup;
 		};
 
 		home {
 			label = "SW-TACT3";
 			gpios = <&gpx1 5 1>;
-			linux,code = <102>;
+			linux,code = <KEY_HOME>;
 			gpio-key,wakeup;
 		};
 
 		up {
 			label = "SW-TACT4";
 			gpios = <&gpx1 6 1>;
-			linux,code = <103>;
+			linux,code = <KEY_UP>;
 			gpio-key,wakeup;
 		};
 
 		down {
 			label = "SW-TACT5";
 			gpios = <&gpx1 7 1>;
-			linux,code = <108>;
+			linux,code = <KEY_DOWN>;
 			gpio-key,wakeup;
 		};
 
 		back {
 			label = "SW-TACT6";
 			gpios = <&gpx2 0 1>;
-			linux,code = <158>;
+			linux,code = <KEY_BACK>;
 			gpio-key,wakeup;
 		};
 
 		wakeup {
 			label = "SW-TACT7";
 			gpios = <&gpx2 1 1>;
-			linux,code = <143>;
+			linux,code = <KEY_WAKEUP>;
 			gpio-key,wakeup;
 		};
 	};
diff --git a/arch/arm/boot/dts/exynos5250-cros-common.dtsi b/arch/arm/boot/dts/exynos5250-cros-common.dtsi
index 2c1560d52f1..89ac90f59e2 100644
--- a/arch/arm/boot/dts/exynos5250-cros-common.dtsi
+++ b/arch/arm/boot/dts/exynos5250-cros-common.dtsi
@@ -240,7 +240,7 @@
 		samsung,i2c-sda-delay = <100>;
 		samsung,i2c-max-bus-freq = <378000>;
 
-		hdmiphy@38 {
+		hdmiphy: hdmiphy@38 {
 			compatible = "samsung,exynos4212-hdmiphy";
 			reg = <0x38>;
 		};
@@ -304,6 +304,10 @@
 
 	hdmi {
 		hpd-gpio = <&gpx3 7 0>;
+		pinctrl-names = "default";
+		pinctrl-0 = <&hdmi_hpd_irq>;
+		phy = <&hdmiphy>;
+		ddc = <&i2c_2>;
 	};
 
 	gpio-keys {
diff --git a/arch/arm/boot/dts/exynos5250-pinctrl.dtsi b/arch/arm/boot/dts/exynos5250-pinctrl.dtsi
index 9a49e6804ae..886cfca044a 100644
--- a/arch/arm/boot/dts/exynos5250-pinctrl.dtsi
+++ b/arch/arm/boot/dts/exynos5250-pinctrl.dtsi
@@ -351,6 +351,34 @@
 			samsung,pin-drv = <0>;
 		};
 
+		pwm0_out: pwm0-out {
+			samsung,pins = "gpb2-0";
+			samsung,pin-function = <2>;
+			samsung,pin-pud = <0>;
+			samsung,pin-drv = <0>;
+		};
+
+		pwm1_out: pwm1-out {
+			samsung,pins = "gpb2-1";
+			samsung,pin-function = <2>;
+			samsung,pin-pud = <0>;
+			samsung,pin-drv = <0>;
+		};
+
+		pwm2_out: pwm2-out {
+			samsung,pins = "gpb2-2";
+			samsung,pin-function = <2>;
+			samsung,pin-pud = <0>;
+			samsung,pin-drv = <0>;
+		};
+
+		pwm3_out: pwm3-out {
+			samsung,pins = "gpb2-3";
+			samsung,pin-function = <2>;
+			samsung,pin-pud = <0>;
+			samsung,pin-drv = <0>;
+		};
+
 		i2c7_bus: i2c7-bus {
 			samsung,pins = "gpb2-2", "gpb2-3";
 			samsung,pin-function = <3>;
diff --git a/arch/arm/boot/dts/exynos5250-snow.dts b/arch/arm/boot/dts/exynos5250-snow.dts
index 1ce1088a00f..079fdf9e3f1 100644
--- a/arch/arm/boot/dts/exynos5250-snow.dts
+++ b/arch/arm/boot/dts/exynos5250-snow.dts
@@ -25,6 +25,13 @@
 	};
 
 	pinctrl@11400000 {
+		ec_irq: ec-irq {
+			samsung,pins = "gpx1-6";
+			samsung,pin-function = <0>;
+			samsung,pin-pud = <0>;
+			samsung,pin-drv = <0>;
+		};
+
 		sd3_clk: sd3-clk {
 			samsung,pin-drv = <0>;
 		};
@@ -37,6 +44,50 @@
 		sd3_bus4: sd3-bus-width4 {
 			samsung,pin-drv = <0>;
 		};
+
+		max98095_en: max98095-en {
+			samsung,pins = "gpx1-7";
+			samsung,pin-function = <0>;
+			samsung,pin-pud = <3>;
+			samsung,pin-drv = <0>;
+		};
+
+		tps65090_irq: tps65090-irq {
+			samsung,pins = "gpx2-6";
+			samsung,pin-function = <0>;
+			samsung,pin-pud = <0>;
+			samsung,pin-drv = <0>;
+		};
+
+		usb3_vbus_en: usb3-vbus-en {
+			samsung,pins = "gpx2-7";
+			samsung,pin-function = <1>;
+			samsung,pin-pud = <0>;
+			samsung,pin-drv = <0>;
+		};
+
+		hdmi_hpd_irq: hdmi-hpd-irq {
+			samsung,pins = "gpx3-7";
+			samsung,pin-function = <0>;
+			samsung,pin-pud = <1>;
+			samsung,pin-drv = <0>;
+		};
+	};
+
+	pinctrl@13400000 {
+		arb_their_claim: arb-their-claim {
+			samsung,pins = "gpe0-4";
+			samsung,pin-function = <0>;
+			samsung,pin-pud = <3>;
+			samsung,pin-drv = <0>;
+		};
+
+		arb_our_claim: arb-our-claim {
+			samsung,pins = "gpf0-3";
+			samsung,pin-function = <1>;
+			samsung,pin-pud = <0>;
+			samsung,pin-drv = <0>;
+		};
 	};
 
 	gpio-keys {
@@ -52,6 +103,12 @@
 		};
 	};
 
+	vbat: vbat-fixed-regulator {
+		compatible = "regulator-fixed";
+		regulator-name = "vbat-supply";
+		regulator-boot-on;
+	};
+
 	i2c-arbitrator {
 		compatible = "i2c-arb-gpio-challenge";
 		#address-cells = <1>;
@@ -65,6 +122,9 @@
 		wait-retry-us = <3000>;
 		wait-free-us = <50000>;
 
+		pinctrl-names = "default";
+		pinctrl-0 = <&arb_our_claim &arb_their_claim>;
+
 		/* Use ID 104 as a hint that we're on physical bus 4 */
 		i2c_104: i2c@0 {
 			reg = <0>;
@@ -82,6 +142,8 @@
 				reg = <0x1e>;
 				interrupts = <6 0>;
 				interrupt-parent = <&gpx1>;
+				pinctrl-names = "default";
+				pinctrl-0 = <&ec_irq>;
 				wakeup-source;
 
 				keyboard-controller {
@@ -173,6 +235,83 @@
 							0x070c0069>;	/* LEFT */
 				};
 			};
+
+			power-regulator {
+				compatible = "ti,tps65090";
+				reg = <0x48>;
+
+				/*
+				 * Config irq to disable internal pulls
+				 * even though we run in polling mode.
+				 */
+				pinctrl-names = "default";
+				pinctrl-0 = <&tps65090_irq>;
+
+				vsys1-supply = <&vbat>;
+				vsys2-supply = <&vbat>;
+				vsys3-supply = <&vbat>;
+				infet1-supply = <&vbat>;
+				infet2-supply = <&vbat>;
+				infet3-supply = <&vbat>;
+				infet4-supply = <&vbat>;
+				infet5-supply = <&vbat>;
+				infet6-supply = <&vbat>;
+				infet7-supply = <&vbat>;
+				vsys-l1-supply = <&vbat>;
+				vsys-l2-supply = <&vbat>;
+
+				regulators {
+					dcdc1 {
+						ti,enable-ext-control;
+					};
+					dcdc2 {
+						ti,enable-ext-control;
+					};
+					dcdc3 {
+						ti,enable-ext-control;
+					};
+					fet1 {
+						regulator-name = "vcd_led";
+						ti,overcurrent-wait = <3>;
+					};
+					tps65090_fet2: fet2 {
+						regulator-name = "video_mid";
+						regulator-always-on;
+						ti,overcurrent-wait = <3>;
+					};
+					fet3 {
+						regulator-name = "wwan_r";
+						regulator-always-on;
+						ti,overcurrent-wait = <3>;
+					};
+					fet4 {
+						regulator-name = "sdcard";
+						ti,overcurrent-wait = <3>;
+					};
+					fet5 {
+						regulator-name = "camout";
+						regulator-always-on;
+						ti,overcurrent-wait = <3>;
+					};
+					fet6 {
+						regulator-name = "lcd_vdd";
+						ti,overcurrent-wait = <3>;
+					};
+					tps65090_fet7: fet7 {
+						regulator-name = "video_mid_1a";
+						regulator-always-on;
+						ti,overcurrent-wait = <3>;
+					};
+					ldo1 {
+					};
+					ldo2 {
+					};
+				};
+
+				charger {
+					compatible = "ti,tps65090-charger";
+				};
+			};
 		};
 	};
 
@@ -196,6 +335,41 @@
 		};
 	};
 
+	i2c@12CD0000 {
+		max98095: codec@11 {
+			compatible = "maxim,max98095";
+			reg = <0x11>;
+			pinctrl-0 = <&max98095_en>;
+			pinctrl-names = "default";
+		};
+	};
+
+	i2s0: i2s@03830000 {
+		status = "okay";
+	};
+
+	sound {
+		compatible = "google,snow-audio-max98095";
+
+		samsung,i2s-controller = <&i2s0>;
+		samsung,audio-codec = <&max98095>;
+	};
+
+	usb3_vbus_reg: regulator-usb3 {
+		compatible = "regulator-fixed";
+		regulator-name = "P5.0V_USB3CON";
+		regulator-min-microvolt = <5000000>;
+		regulator-max-microvolt = <5000000>;
+		gpio = <&gpx2 7 0>;
+		pinctrl-names = "default";
+		pinctrl-0 = <&usb3_vbus_en>;
+		enable-active-high;
+	};
+
+	phy@12100000 {
+		vbus-supply = <&usb3_vbus_reg>;
+	};
+
 	usb@12110000 {
 		samsung,vbus-gpio = <&gpx1 1 0>;
 	};
@@ -206,4 +380,54 @@
 			clock-frequency = <24000000>;
 		};
 	};
+
+	hdmi {
+		hdmi-en-supply = <&tps65090_fet7>;
+		vdd-supply = <&ldo8_reg>;
+		vdd_osc-supply = <&ldo10_reg>;
+		vdd_pll-supply = <&ldo8_reg>;
+	};
+
+	backlight {
+		compatible = "pwm-backlight";
+		pwms = <&pwm 0 1000000 0>;
+		brightness-levels = <0 100 500 1000 1500 2000 2500 2800>;
+		default-brightness-level = <7>;
+		pinctrl-0 = <&pwm0_out>;
+		pinctrl-names = "default";
+	};
+
+	fimd@14400000 {
+		status = "okay";
+		samsung,invert-vclk;
+	};
+
+	dp-controller@145B0000 {
+		status = "okay";
+		pinctrl-names = "default";
+		pinctrl-0 = <&dp_hpd>;
+		samsung,color-space = <0>;
+		samsung,dynamic-range = <0>;
+		samsung,ycbcr-coeff = <0>;
+		samsung,color-depth = <1>;
+		samsung,link-rate = <0x0a>;
+		samsung,lane-count = <2>;
+		samsung,hpd-gpio = <&gpx0 7 0>;
+
+		display-timings {
+			native-mode = <&timing1>;
+
+			timing1: timing@1 {
+				clock-frequency = <70589280>;
+				hactive = <1366>;
+				vactive = <768>;
+				hfront-porch = <40>;
+				hback-porch = <40>;
+				hsync-len = <32>;
+				vback-porch = <10>;
+				vfront-porch = <12>;
+				vsync-len = <6>;
+			};
+		};
+	};
 };
diff --git a/arch/arm/boot/dts/exynos5250.dtsi b/arch/arm/boot/dts/exynos5250.dtsi
index 37423314a02..834fb5a5306 100644
--- a/arch/arm/boot/dts/exynos5250.dtsi
+++ b/arch/arm/boot/dts/exynos5250.dtsi
@@ -72,6 +72,24 @@
 		};
 	};
 
+	sysram@02020000 {
+		compatible = "mmio-sram";
+		reg = <0x02020000 0x30000>;
+		#address-cells = <1>;
+		#size-cells = <1>;
+		ranges = <0 0x02020000 0x30000>;
+
+		smp-sysram@0 {
+			compatible = "samsung,exynos4210-sysram";
+			reg = <0x0 0x1000>;
+		};
+
+		smp-sysram@2f000 {
+			compatible = "samsung,exynos4210-sysram-ns";
+			reg = <0x2f000 0x1000>;
+		};
+	};
+
 	pd_gsc: gsc-power-domain@10044000 {
 		compatible = "samsung,exynos4210-pd";
 		reg = <0x10044000 0x20>;
@@ -175,6 +193,11 @@
 		reg = <0x10040000 0x5000>;
 	};
 
+	sysreg_system_controller: syscon@10050000 {
+		compatible = "samsung,exynos5-sysreg", "syscon";
+		reg = <0x10050000 0x5000>;
+	};
+
 	watchdog@101D0000 {
 		compatible = "samsung,exynos5250-wdt";
 		reg = <0x101D0000 0x100>;
@@ -250,7 +273,7 @@
 	sata_phy: sata-phy@12170000 {
 		compatible = "samsung,exynos5250-sata-phy";
 		reg = <0x12170000 0x1ff>;
-		clocks = <&clock 287>;
+		clocks = <&clock CLK_SATA_PHYCTRL>;
 		clock-names = "sata_phyctrl";
 		#phy-cells = <0>;
 		samsung,syscon-phandle = <&pmu_system_controller>;
@@ -533,22 +556,18 @@
 			compatible = "synopsys,dwc3";
 			reg = <0x12000000 0x10000>;
 			interrupts = <0 72 0>;
-			usb-phy = <&usb2_phy &usb3_phy>;
+			phys = <&usbdrd_phy 0>, <&usbdrd_phy 1>;
+			phy-names = "usb2-phy", "usb3-phy";
 		};
 	};
 
-	usb3_phy: usbphy@12100000 {
-		compatible = "samsung,exynos5250-usb3phy";
+	usbdrd_phy: phy@12100000 {
+		compatible = "samsung,exynos5250-usbdrd-phy";
 		reg = <0x12100000 0x100>;
-		clocks = <&clock CLK_FIN_PLL>, <&clock CLK_USB3>;
-		clock-names = "ext_xtal", "usbdrd30";
-		#address-cells = <1>;
-		#size-cells = <1>;
-		ranges;
-
-		usbphy-sys {
-			reg = <0x10040704 0x8>;
-		};
+		clocks = <&clock CLK_USB3>, <&clock CLK_FIN_PLL>;
+		clock-names = "phy", "ref";
+		samsung,pmu-syscon = <&pmu_system_controller>;
+		#phy-cells = <1>;
 	};
 
 	usb@12110000 {
@@ -558,6 +577,12 @@
 
 		clocks = <&clock CLK_USB2>;
 		clock-names = "usbhost";
+		#address-cells = <1>;
+		#size-cells = <0>;
+		port@0 {
+			reg = <0>;
+			phys = <&usb2_phy_gen 1>;
+		};
 	};
 
 	usb@12120000 {
@@ -567,6 +592,12 @@
 
 		clocks = <&clock CLK_USB2>;
 		clock-names = "usbhost";
+		#address-cells = <1>;
+		#size-cells = <0>;
+		port@0 {
+			reg = <0>;
+			phys = <&usb2_phy_gen 1>;
+		};
 	};
 
 	usb2_phy: usbphy@12130000 {
@@ -584,6 +615,16 @@
 		};
 	};
 
+	usb2_phy_gen: phy@12130000 {
+		compatible = "samsung,exynos5250-usb2-phy";
+		reg = <0x12130000 0x100>;
+		clocks = <&clock CLK_USB2>, <&clock CLK_FIN_PLL>;
+		clock-names = "phy", "ref";
+		#phy-cells = <1>;
+		samsung,sysreg-phandle = <&sysreg_system_controller>;
+		samsung,pmureg-phandle = <&pmu_system_controller>;
+	};
+
 	pwm: pwm@12dd0000 {
 		compatible = "samsung,exynos4210-pwm";
 		reg = <0x12dd0000 0x100>;
@@ -690,6 +731,7 @@
 			 <&clock CLK_MOUT_HDMI>;
 		clock-names = "hdmi", "sclk_hdmi", "sclk_pixel",
 				"sclk_hdmiphy", "mout_hdmi";
+		samsung,syscon-phandle = <&pmu_system_controller>;
 	};
 
 	mixer {
@@ -733,7 +775,7 @@
 		compatible = "samsung,exynos4210-secss";
 		reg = <0x10830000 0x10000>;
 		interrupts = <0 112 0>;
-		clocks = <&clock 348>;
+		clocks = <&clock CLK_SSS>;
 		clock-names = "secss";
 	};
 };
diff --git a/arch/arm/boot/dts/exynos5260-pinctrl.dtsi b/arch/arm/boot/dts/exynos5260-pinctrl.dtsi
new file mode 100644
index 00000000000..f6ee55ea070
--- /dev/null
+++ b/arch/arm/boot/dts/exynos5260-pinctrl.dtsi
@@ -0,0 +1,574 @@
+/*
+ * Samsung's Exynos5260 SoC pin-mux and pin-config device tree source
+ *
+ * Copyright (c) 2013 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * Samsung's Exynos5260 SoC pin-mux and pin-config options are listed as device
+ * tree nodes are listed in this file.
+ *
+ * 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 PIN_PULL_NONE	0
+#define PIN_PULL_DOWN	1
+#define PIN_PULL_UP	3
+
+&pinctrl_0 {
+	gpa0: gpa0 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpa1: gpa1 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpa2: gpa2 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpb0: gpb0 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpb1: gpb1 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpb2: gpb2 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpb3: gpb3 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpb4: gpb4 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpb5: gpb5 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpd0: gpd0 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpd1: gpd1 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpd2: gpd2 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpe0: gpe0 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpe1: gpe1 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpf0: gpf0 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpf1: gpf1 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpk0: gpk0 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpx0: gpx0 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpx1: gpx1 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpx2: gpx2 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpx3: gpx3 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	uart0_data: uart0-data {
+		samsung,pins = "gpa0-0", "gpa0-1";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_NONE>;
+		samsung,pin-drv = <0>;
+	};
+
+	uart0_fctl: uart0-fctl {
+		samsung,pins = "gpa0-2", "gpa0-3";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_NONE>;
+		samsung,pin-drv = <0>;
+	};
+
+	uart1_data: uart1-data {
+		samsung,pins = "gpa1-0", "gpa1-1";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_NONE>;
+		samsung,pin-drv = <0>;
+	};
+
+	uart1_fctl: uart1-fctl {
+		samsung,pins = "gpa1-2", "gpa1-3";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_NONE>;
+		samsung,pin-drv = <0>;
+	};
+
+	uart2_data: uart2-data {
+		samsung,pins = "gpa1-4", "gpa1-5";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_NONE>;
+		samsung,pin-drv = <0>;
+	};
+
+	spi0_bus: spi0-bus {
+		samsung,pins = "gpa2-0", "gpa2-2", "gpa2-3";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_UP>;
+		samsung,pin-drv = <0>;
+	};
+
+	spi1_bus: spi1-bus {
+		samsung,pins = "gpa2-4", "gpa2-6", "gpa2-7";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_UP>;
+		samsung,pin-drv = <0>;
+	};
+
+	usb3_vbus0_en: usb3-vbus0-en {
+		samsung,pins = "gpa2-4";
+		samsung,pin-function = <1>;
+		samsung,pin-pud = <PIN_PULL_NONE>;
+		samsung,pin-drv = <0>;
+	};
+
+	i2s1_bus: i2s1-bus {
+		samsung,pins = "gpb0-0", "gpb0-1", "gpb0-2", "gpb0-3",
+				"gpb0-4";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_NONE>;
+		samsung,pin-drv = <0>;
+	};
+
+	pcm1_bus: pcm1-bus {
+		samsung,pins = "gpb0-0", "gpb0-1", "gpb0-2", "gpb0-3",
+				"gpb0-4";
+		samsung,pin-function = <3>;
+		samsung,pin-pud = <PIN_PULL_NONE>;
+		samsung,pin-drv = <0>;
+	};
+
+	spdif1_bus: spdif1-bus {
+		samsung,pins = "gpb0-0", "gpb0-1", "gpb0-2";
+		samsung,pin-function = <4>;
+		samsung,pin-pud = <PIN_PULL_NONE>;
+		samsung,pin-drv = <0>;
+	};
+
+	spi2_bus: spi2-bus {
+		samsung,pins = "gpb1-0", "gpb1-2", "gpb1-3";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_UP>;
+		samsung,pin-drv = <0>;
+	};
+
+	i2c0_hs_bus: i2c0-hs-bus {
+		samsung,pins = "gpb3-0", "gpb3-1";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_UP>;
+		samsung,pin-drv = <0>;
+	};
+
+	i2c1_hs_bus: i2c1-hs-bus {
+		samsung,pins = "gpb3-2", "gpb3-3";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_UP>;
+		samsung,pin-drv = <0>;
+	};
+
+	i2c2_hs_bus: i2c2-hs-bus {
+		samsung,pins = "gpb3-4", "gpb3-5";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_UP>;
+		samsung,pin-drv = <0>;
+	};
+
+	i2c3_hs_bus: i2c3-hs-bus {
+		samsung,pins = "gpb3-6", "gpb3-7";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_UP>;
+		samsung,pin-drv = <0>;
+	};
+
+	i2c4_bus: i2c4-bus {
+		samsung,pins = "gpb4-0", "gpb4-1";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_UP>;
+		samsung,pin-drv = <0>;
+	};
+
+	i2c5_bus: i2c5-bus {
+		samsung,pins = "gpb4-2", "gpb4-3";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_UP>;
+		samsung,pin-drv = <0>;
+	};
+
+	i2c6_bus: i2c6-bus {
+		samsung,pins = "gpb4-4", "gpb4-5";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_UP>;
+		samsung,pin-drv = <0>;
+	};
+
+	i2c7_bus: i2c7-bus {
+		samsung,pins = "gpb4-6", "gpb4-7";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_UP>;
+		samsung,pin-drv = <0>;
+	};
+
+	i2c8_bus: i2c8-bus {
+		samsung,pins = "gpb5-0", "gpb5-1";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_UP>;
+		samsung,pin-drv = <0>;
+	};
+
+	i2c9_bus: i2c9-bus {
+		samsung,pins = "gpb5-2", "gpb5-3";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_UP>;
+		samsung,pin-drv = <0>;
+	};
+
+	i2c10_bus: i2c10-bus {
+		samsung,pins = "gpb5-4", "gpb5-5";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_UP>;
+		samsung,pin-drv = <0>;
+	};
+
+	i2c11_bus: i2c11-bus {
+		samsung,pins = "gpb5-6", "gpb5-7";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_UP>;
+		samsung,pin-drv = <0>;
+	};
+
+	cam_gpio_a: cam-gpio-a {
+		samsung,pins = "gpe0-0", "gpe0-1", "gpe0-2", "gpe0-3",
+			"gpe0-4", "gpe0-5", "gpe0-6", "gpe0-7",
+			"gpe1-0", "gpe1-1";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_NONE>;
+		samsung,pin-drv = <0>;
+	};
+
+	cam_gpio_b: cam-gpio-b {
+		samsung,pins = "gpf0-0", "gpf0-1", "gpf0-2", "gpf0-3",
+			"gpf1-0", "gpf1-1", "gpf1-2", "gpf1-3";
+		samsung,pin-function = <3>;
+		samsung,pin-pud = <PIN_PULL_NONE>;
+		samsung,pin-drv = <0>;
+	};
+
+	cam_i2c1_bus: cam-i2c1-bus {
+		samsung,pins = "gpf0-2", "gpf0-3";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_UP>;
+		samsung,pin-drv = <0>;
+	};
+
+	cam_i2c0_bus: cam-i2c0-bus {
+		samsung,pins = "gpf0-0", "gpf0-1";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_UP>;
+		samsung,pin-drv = <0>;
+	};
+
+	cam_spi0_bus: cam-spi0-bus {
+		samsung,pins = "gpf1-0", "gpf1-1", "gpf1-2", "gpf1-3";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_NONE>;
+		samsung,pin-drv = <0>;
+	};
+
+	cam_spi1_bus: cam-spi1-bus {
+		samsung,pins = "gpf1-4", "gpf1-5", "gpf1-6", "gpf1-7";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_NONE>;
+		samsung,pin-drv = <0>;
+	};
+};
+
+&pinctrl_1 {
+	gpc0: gpc0 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpc1: gpc1 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpc2: gpc2 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpc3: gpc3 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpc4: gpc4 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	sd0_clk: sd0-clk {
+		samsung,pins = "gpc0-0";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_NONE>;
+		samsung,pin-drv = <3>;
+	};
+
+	sd0_cmd: sd0-cmd {
+		samsung,pins = "gpc0-1";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_NONE>;
+		samsung,pin-drv = <3>;
+	};
+
+	sd0_bus1: sd0-bus-width1 {
+		samsung,pins = "gpc0-2";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_UP>;
+		samsung,pin-drv = <3>;
+	};
+
+	sd0_bus4: sd0-bus-width4 {
+		samsung,pins = "gpc0-3", "gpc0-4", "gpc0-5";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_UP>;
+		samsung,pin-drv = <3>;
+	};
+
+	sd0_bus8: sd0-bus-width8 {
+		samsung,pins = "gpc3-0", "gpc3-1", "gpc3-2", "gpc3-3";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_UP>;
+		samsung,pin-drv = <3>;
+	};
+
+	sd0_rdqs: sd0-rdqs {
+		samsung,pins = "gpc0-6";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_UP>;
+		samsung,pin-drv = <3>;
+	};
+
+	sd1_clk: sd1-clk {
+		samsung,pins = "gpc1-0";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_NONE>;
+		samsung,pin-drv = <3>;
+	};
+
+	sd1_cmd: sd1-cmd {
+		samsung,pins = "gpc1-1";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_NONE>;
+		samsung,pin-drv = <3>;
+	};
+
+	sd1_bus1: sd1-bus-width1 {
+		samsung,pins = "gpc1-2";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_UP>;
+		samsung,pin-drv = <3>;
+	};
+
+	sd1_bus4: sd1-bus-width4 {
+		samsung,pins = "gpc1-3", "gpc1-4", "gpc1-5";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_UP>;
+		samsung,pin-drv = <3>;
+	};
+
+	sd1_bus8: sd1-bus-width8 {
+		samsung,pins = "gpc4-0", "gpc4-1", "gpc4-2", "gpc4-3";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_UP>;
+		samsung,pin-drv = <3>;
+	};
+
+	sd2_clk: sd2-clk {
+		samsung,pins = "gpc2-0";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_NONE>;
+		samsung,pin-drv = <3>;
+	};
+
+	sd2_cmd: sd2-cmd {
+		samsung,pins = "gpc2-1";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_NONE>;
+		samsung,pin-drv = <3>;
+	};
+
+	sd2_cd: sd2-cd {
+		samsung,pins = "gpc2-2";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_UP>;
+		samsung,pin-drv = <3>;
+	};
+
+	sd2_bus1: sd2-bus-width1 {
+		samsung,pins = "gpc2-3";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_UP>;
+		samsung,pin-drv = <3>;
+	};
+
+	sd2_bus4: sd2-bus-width4 {
+		samsung,pins = "gpc2-4", "gpc2-5", "gpc2-6";
+		samsung,pin-function = <2>;
+		samsung,pin-pud = <PIN_PULL_UP>;
+		samsung,pin-drv = <3>;
+	};
+};
+
+&pinctrl_2 {
+	gpz0: gpz0 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+
+	gpz1: gpz1 {
+		gpio-controller;
+		#gpio-cells = <2>;
+
+		interrupt-controller;
+		#interrupt-cells = <2>;
+	};
+};
diff --git a/arch/arm/boot/dts/exynos5260-xyref5260.dts b/arch/arm/boot/dts/exynos5260-xyref5260.dts
new file mode 100644
index 00000000000..8c84ab27c19
--- /dev/null
+++ b/arch/arm/boot/dts/exynos5260-xyref5260.dts
@@ -0,0 +1,103 @@
+/*
+ * SAMSUNG XYREF5260 board device tree source
+ *
+ * Copyright (c) 2013 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+
+/dts-v1/;
+#include "exynos5260.dtsi"
+
+/ {
+	model = "SAMSUNG XYREF5260 board based on EXYNOS5260";
+	compatible = "samsung,xyref5260", "samsung,exynos5260", "samsung,exynos5";
+
+	memory {
+		reg = <0x20000000 0x80000000>;
+	};
+
+	chosen {
+		bootargs = "console=ttySAC2,115200";
+	};
+
+	fin_pll: xxti {
+		compatible = "fixed-clock";
+		clock-frequency = <24000000>;
+		clock-output-names = "fin_pll";
+		#clock-cells = <0>;
+	};
+
+	xrtcxti: xrtcxti {
+		compatible = "fixed-clock";
+		clock-frequency = <32768>;
+		clock-output-names = "xrtcxti";
+		#clock-cells = <0>;
+	};
+};
+
+&pinctrl_0 {
+	hdmi_hpd_irq: hdmi-hpd-irq {
+		samsung,pins = "gpx3-7";
+		samsung,pin-function = <0>;
+		samsung,pin-pud = <1>;
+		samsung,pin-drv = <0>;
+	};
+};
+
+&uart0 {
+	status = "okay";
+};
+
+&uart1 {
+	status = "okay";
+};
+
+&uart2 {
+	status = "okay";
+};
+
+&uart3 {
+	status = "okay";
+};
+
+&mmc_0 {
+	status = "okay";
+	num-slots = <1>;
+	broken-cd;
+	bypass-smu;
+	supports-highspeed;
+	supports-hs200-mode; /* 200 Mhz */
+	card-detect-delay = <200>;
+	samsung,dw-mshc-ciu-div = <3>;
+	samsung,dw-mshc-sdr-timing = <0 4>;
+	samsung,dw-mshc-ddr-timing = <0 2>;
+	pinctrl-names = "default";
+	pinctrl-0 = <&sd0_rdqs &sd0_clk &sd0_cmd &sd0_bus1 &sd0_bus4 &sd0_bus8>;
+
+	slot@0 {
+		reg = <0>;
+		bus-width = <8>;
+	};
+};
+
+&mmc_2 {
+	status = "okay";
+	num-slots = <1>;
+	supports-highspeed;
+	card-detect-delay = <200>;
+	samsung,dw-mshc-ciu-div = <3>;
+	samsung,dw-mshc-sdr-timing = <2 3>;
+	samsung,dw-mshc-ddr-timing = <1 2>;
+	pinctrl-names = "default";
+	pinctrl-0 = <&sd2_clk &sd2_cmd &sd2_cd &sd2_bus1 &sd2_bus4>;
+
+	slot@0 {
+		reg = <0>;
+		bus-width = <4>;
+		disable-wp;
+	};
+};
diff --git a/arch/arm/boot/dts/exynos5260.dtsi b/arch/arm/boot/dts/exynos5260.dtsi
new file mode 100644
index 00000000000..5398a60207c
--- /dev/null
+++ b/arch/arm/boot/dts/exynos5260.dtsi
@@ -0,0 +1,304 @@
+/*
+ * SAMSUNG EXYNOS5260 SoC device tree source
+ *
+ * Copyright (c) 2013 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+
+#include "skeleton.dtsi"
+
+#include <dt-bindings/clock/exynos5260-clk.h>
+
+/ {
+	compatible = "samsung,exynos5260", "samsung,exynos5";
+	interrupt-parent = <&gic>;
+
+	aliases {
+		pinctrl0 = &pinctrl_0;
+		pinctrl1 = &pinctrl_1;
+		pinctrl2 = &pinctrl_2;
+	};
+
+	cpus {
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		cpu@0 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a15";
+			reg = <0x0>;
+			cci-control-port = <&cci_control1>;
+		};
+
+		cpu@1 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a15";
+			reg = <0x1>;
+			cci-control-port = <&cci_control1>;
+		};
+
+		cpu@100 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a7";
+			reg = <0x100>;
+			cci-control-port = <&cci_control0>;
+		};
+
+		cpu@101 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a7";
+			reg = <0x101>;
+			cci-control-port = <&cci_control0>;
+		};
+
+		cpu@102 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a7";
+			reg = <0x102>;
+			cci-control-port = <&cci_control0>;
+		};
+
+		cpu@103 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a7";
+			reg = <0x103>;
+			cci-control-port = <&cci_control0>;
+		};
+	};
+
+	soc: soc {
+		compatible = "simple-bus";
+		#address-cells = <1>;
+		#size-cells = <1>;
+		ranges;
+
+		clock_top: clock-controller@10010000 {
+			compatible = "samsung,exynos5260-clock-top";
+			reg = <0x10010000 0x10000>;
+			#clock-cells = <1>;
+		};
+
+		clock_peri: clock-controller@10200000 {
+			compatible = "samsung,exynos5260-clock-peri";
+			reg = <0x10200000 0x10000>;
+			#clock-cells = <1>;
+		};
+
+		clock_egl: clock-controller@10600000 {
+			compatible = "samsung,exynos5260-clock-egl";
+			reg = <0x10600000 0x10000>;
+			#clock-cells = <1>;
+		};
+
+		clock_kfc: clock-controller@10700000 {
+			compatible = "samsung,exynos5260-clock-kfc";
+			reg = <0x10700000 0x10000>;
+			#clock-cells = <1>;
+		};
+
+		clock_g2d: clock-controller@10A00000 {
+			compatible = "samsung,exynos5260-clock-g2d";
+			reg = <0x10A00000 0x10000>;
+			#clock-cells = <1>;
+		};
+
+		clock_mif: clock-controller@10CE0000 {
+			compatible = "samsung,exynos5260-clock-mif";
+			reg = <0x10CE0000 0x10000>;
+			#clock-cells = <1>;
+		};
+
+		clock_mfc: clock-controller@11090000 {
+			compatible = "samsung,exynos5260-clock-mfc";
+			reg = <0x11090000 0x10000>;
+			#clock-cells = <1>;
+		};
+
+		clock_g3d: clock-controller@11830000 {
+			compatible = "samsung,exynos5260-clock-g3d";
+			reg = <0x11830000 0x10000>;
+			#clock-cells = <1>;
+		};
+
+		clock_fsys: clock-controller@122E0000 {
+			compatible = "samsung,exynos5260-clock-fsys";
+			reg = <0x122E0000 0x10000>;
+			#clock-cells = <1>;
+		};
+
+		clock_aud: clock-controller@128C0000 {
+			compatible = "samsung,exynos5260-clock-aud";
+			reg = <0x128C0000 0x10000>;
+			#clock-cells = <1>;
+		};
+
+		clock_isp: clock-controller@133C0000 {
+			compatible = "samsung,exynos5260-clock-isp";
+			reg = <0x133C0000 0x10000>;
+			#clock-cells = <1>;
+		};
+
+		clock_gscl: clock-controller@13F00000 {
+			compatible = "samsung,exynos5260-clock-gscl";
+			reg = <0x13F00000 0x10000>;
+			#clock-cells = <1>;
+		};
+
+		clock_disp: clock-controller@14550000 {
+			compatible = "samsung,exynos5260-clock-disp";
+			reg = <0x14550000 0x10000>;
+			#clock-cells = <1>;
+		};
+
+		gic: interrupt-controller@10481000 {
+			compatible = "arm,cortex-a15-gic", "arm,cortex-a9-gic";
+			#interrupt-cells = <3>;
+			#address-cells = <0>;
+			#size-cells = <0>;
+			interrupt-controller;
+			reg = <0x10481000 0x1000>,
+				<0x10482000 0x1000>,
+				<0x10484000 0x2000>,
+				<0x10486000 0x2000>;
+			interrupts = <1 9 0xf04>;
+		};
+
+		chipid: chipid@10000000 {
+			compatible = "samsung,exynos4210-chipid";
+			reg = <0x10000000 0x100>;
+		};
+
+		mct: mct@100B0000 {
+			compatible = "samsung,exynos4210-mct";
+			reg = <0x100B0000 0x1000>;
+			clocks = <&fin_pll>, <&clock_peri PERI_CLK_MCT>;
+			clock-names = "fin_pll", "mct";
+			interrupts = <0 104 0>, <0 105 0>, <0 106 0>,
+					<0 107 0>, <0 122 0>, <0 123 0>,
+					<0 124 0>, <0 125 0>, <0 126 0>,
+					<0 127 0>, <0 128 0>, <0 129 0>;
+		};
+
+		cci: cci@10F00000 {
+			compatible = "arm,cci-400";
+			#address-cells = <1>;
+			#size-cells = <1>;
+			reg = <0x10F00000 0x1000>;
+			ranges = <0x0 0x10F00000 0x6000>;
+
+			cci_control0: slave-if@4000 {
+				compatible = "arm,cci-400-ctrl-if";
+				interface-type = "ace";
+				reg = <0x4000 0x1000>;
+			};
+
+			cci_control1: slave-if@5000 {
+				compatible = "arm,cci-400-ctrl-if";
+				interface-type = "ace";
+				reg = <0x5000 0x1000>;
+			};
+		};
+
+		pinctrl_0: pinctrl@11600000 {
+			compatible = "samsung,exynos5260-pinctrl";
+			reg = <0x11600000 0x1000>;
+			interrupts = <0 79 0>;
+
+			wakeup-interrupt-controller {
+				compatible = "samsung,exynos4210-wakeup-eint";
+				interrupt-parent = <&gic>;
+				interrupts = <0 32 0>;
+			};
+		};
+
+		pinctrl_1: pinctrl@12290000 {
+			compatible = "samsung,exynos5260-pinctrl";
+			reg = <0x12290000 0x1000>;
+			interrupts = <0 157 0>;
+		};
+
+		pinctrl_2: pinctrl@128B0000 {
+			compatible = "samsung,exynos5260-pinctrl";
+			reg = <0x128B0000 0x1000>;
+			interrupts = <0 243 0>;
+		};
+
+		uart0: serial@12C00000 {
+			compatible = "samsung,exynos4210-uart";
+			reg = <0x12C00000 0x100>;
+			interrupts = <0 146 0>;
+			clocks = <&clock_peri PERI_CLK_UART0>, <&clock_peri PERI_SCLK_UART0>;
+			clock-names = "uart", "clk_uart_baud0";
+			status = "disabled";
+		};
+
+		uart1: serial@12C10000 {
+			compatible = "samsung,exynos4210-uart";
+			reg = <0x12C10000 0x100>;
+			interrupts = <0 147 0>;
+			clocks = <&clock_peri PERI_CLK_UART1>, <&clock_peri PERI_SCLK_UART1>;
+			clock-names = "uart", "clk_uart_baud0";
+			status = "disabled";
+		};
+
+		uart2: serial@12C20000 {
+			compatible = "samsung,exynos4210-uart";
+			reg = <0x12C20000 0x100>;
+			interrupts = <0 148 0>;
+			clocks = <&clock_peri PERI_CLK_UART2>, <&clock_peri PERI_SCLK_UART2>;
+			clock-names = "uart", "clk_uart_baud0";
+			status = "disabled";
+		};
+
+		uart3: serial@12860000 {
+			compatible = "samsung,exynos4210-uart";
+			reg = <0x12860000 0x100>;
+			interrupts = <0 145 0>;
+			clocks = <&clock_aud AUD_CLK_AUD_UART>, <&clock_aud AUD_SCLK_AUD_UART>;
+			clock-names = "uart", "clk_uart_baud0";
+			status = "disabled";
+		};
+
+		mmc_0: mmc@12140000 {
+			compatible = "samsung,exynos5250-dw-mshc";
+			reg = <0x12140000 0x2000>;
+			interrupts = <0 156 0>;
+			#address-cells = <1>;
+			#size-cells = <0>;
+			clocks = <&clock_fsys FSYS_CLK_MMC0>, <&clock_top TOP_SCLK_MMC0>;
+			clock-names = "biu", "ciu";
+			fifo-depth = <64>;
+			status = "disabled";
+		};
+
+		mmc_1: mmc@12150000 {
+			compatible = "samsung,exynos5250-dw-mshc";
+			reg = <0x12150000 0x2000>;
+			interrupts = <0 158 0>;
+			#address-cells = <1>;
+			#size-cells = <0>;
+			clocks = <&clock_fsys FSYS_CLK_MMC1>, <&clock_top TOP_SCLK_MMC1>;
+			clock-names = "biu", "ciu";
+			fifo-depth = <64>;
+			status = "disabled";
+		};
+
+		mmc_2: mmc@12160000 {
+			compatible = "samsung,exynos5250-dw-mshc";
+			reg = <0x12160000 0x2000>;
+			interrupts = <0 159 0>;
+			#address-cells = <1>;
+			#size-cells = <0>;
+			clocks = <&clock_fsys FSYS_CLK_MMC2>, <&clock_top TOP_SCLK_MMC2>;
+			clock-names = "biu", "ciu";
+			fifo-depth = <64>;
+			status = "disabled";
+		};
+	};
+};
+
+#include "exynos5260-pinctrl.dtsi"
diff --git a/arch/arm/boot/dts/exynos5410-smdk5410.dts b/arch/arm/boot/dts/exynos5410-smdk5410.dts
new file mode 100644
index 00000000000..7275bbd6fc4
--- /dev/null
+++ b/arch/arm/boot/dts/exynos5410-smdk5410.dts
@@ -0,0 +1,82 @@
+/*
+ * SAMSUNG SMDK5410 board device tree source
+ *
+ * Copyright (c) 2013 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+
+/dts-v1/;
+#include "exynos5410.dtsi"
+/ {
+	model = "Samsung SMDK5410 board based on EXYNOS5410";
+	compatible = "samsung,smdk5410", "samsung,exynos5410", "samsung,exynos5";
+
+	memory {
+		reg = <0x40000000 0x80000000>;
+	};
+
+	chosen {
+		bootargs = "console=ttySAC2,115200";
+	};
+
+	fin_pll: xxti {
+		compatible = "fixed-clock";
+		clock-frequency = <24000000>;
+		clock-output-names = "fin_pll";
+		#clock-cells = <0>;
+	};
+
+	firmware@02037000 {
+		compatible = "samsung,secure-firmware";
+		reg = <0x02037000 0x1000>;
+	};
+
+};
+
+&mmc_0 {
+	status = "okay";
+	num-slots = <1>;
+	supports-highspeed;
+	broken-cd;
+	card-detect-delay = <200>;
+	samsung,dw-mshc-ciu-div = <3>;
+	samsung,dw-mshc-sdr-timing = <2 3>;
+	samsung,dw-mshc-ddr-timing = <1 2>;
+
+	slot@0 {
+		reg = <0>;
+		bus-width = <8>;
+	};
+};
+
+&mmc_2 {
+	status = "okay";
+	num-slots = <1>;
+	supports-highspeed;
+	card-detect-delay = <200>;
+	samsung,dw-mshc-ciu-div = <3>;
+	samsung,dw-mshc-sdr-timing = <2 3>;
+	samsung,dw-mshc-ddr-timing = <1 2>;
+
+	slot@0 {
+		reg = <0>;
+		bus-width = <4>;
+		disable-wp;
+	};
+};
+
+&uart0 {
+		status = "okay";
+};
+
+&uart1 {
+		status = "okay";
+};
+
+&uart2 {
+		status = "okay";
+};
diff --git a/arch/arm/boot/dts/exynos5410.dtsi b/arch/arm/boot/dts/exynos5410.dtsi
new file mode 100644
index 00000000000..3839c26f467
--- /dev/null
+++ b/arch/arm/boot/dts/exynos5410.dtsi
@@ -0,0 +1,206 @@
+/*
+ * SAMSUNG EXYNOS5410 SoC device tree source
+ *
+ * Copyright (c) 2013 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * SAMSUNG EXYNOS5410 SoC device nodes are listed in this file.
+ * EXYNOS5410 based board files can include this file and provide
+ * values for board specfic bindings.
+ *
+ * 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 "skeleton.dtsi"
+#include <dt-bindings/clock/exynos5410.h>
+
+/ {
+	compatible = "samsung,exynos5410", "samsung,exynos5";
+	interrupt-parent = <&gic>;
+
+	cpus {
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		CPU0: cpu@0 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a15";
+			reg = <0x0>;
+		};
+
+		CPU1: cpu@1 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a15";
+			reg = <0x1>;
+		};
+
+		CPU2: cpu@2 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a15";
+			reg = <0x2>;
+		};
+
+		CPU3: cpu@3 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a15";
+			reg = <0x3>;
+		};
+	};
+
+	soc: soc {
+		compatible = "simple-bus";
+		#address-cells = <1>;
+		#size-cells = <1>;
+		ranges;
+
+		combiner: interrupt-controller@10440000 {
+			compatible = "samsung,exynos4210-combiner";
+			#interrupt-cells = <2>;
+			interrupt-controller;
+			samsung,combiner-nr = <32>;
+			reg = <0x10440000 0x1000>;
+			interrupts =	<0 0 0>, <0 1 0>, <0 2 0>, <0 3 0>,
+					<0 4 0>, <0 5 0>, <0 6 0>, <0 7 0>,
+					<0 8 0>, <0 9 0>, <0 10 0>, <0 11 0>,
+					<0 12 0>, <0 13 0>, <0 14 0>, <0 15 0>,
+					<0 16 0>, <0 17 0>, <0 18 0>, <0 19 0>,
+					<0 20 0>, <0 21 0>, <0 22 0>, <0 23 0>,
+					<0 24 0>, <0 25 0>, <0 26 0>, <0 27 0>,
+					<0 28 0>, <0 29 0>, <0 30 0>, <0 31 0>;
+		};
+
+		gic: interrupt-controller@10481000 {
+			compatible = "arm,cortex-a15-gic", "arm,cortex-a9-gic";
+			#interrupt-cells = <3>;
+			interrupt-controller;
+			reg =	<0x10481000 0x1000>,
+				<0x10482000 0x1000>,
+				<0x10484000 0x2000>,
+				<0x10486000 0x2000>;
+			interrupts = <1 9 0xf04>;
+		};
+
+		chipid@10000000 {
+			compatible = "samsung,exynos4210-chipid";
+			reg = <0x10000000 0x100>;
+		};
+
+		mct: mct@101C0000 {
+			compatible = "samsung,exynos4210-mct";
+			reg = <0x101C0000 0xB00>;
+			interrupt-parent = <&interrupt_map>;
+			interrupts = <0>, <1>, <2>, <3>,
+				<4>, <5>, <6>, <7>,
+				<8>, <9>, <10>, <11>;
+			clocks = <&fin_pll>, <&clock CLK_MCT>;
+			clock-names = "fin_pll", "mct";
+
+			interrupt_map: interrupt-map {
+				#interrupt-cells = <1>;
+				#address-cells = <0>;
+				#size-cells = <0>;
+				interrupt-map = <0 &combiner 23 3>,
+						<1 &combiner 23 4>,
+						<2 &combiner 25 2>,
+						<3 &combiner 25 3>,
+						<4 &gic 0 120 0>,
+						<5 &gic 0 121 0>,
+						<6 &gic 0 122 0>,
+						<7 &gic 0 123 0>,
+						<8 &gic 0 128 0>,
+						<9 &gic 0 129 0>,
+						<10 &gic 0 130 0>,
+						<11 &gic 0 131 0>;
+			};
+		};
+
+		sysram@02020000 {
+			compatible = "mmio-sram";
+			reg = <0x02020000 0x54000>;
+			#address-cells = <1>;
+			#size-cells = <1>;
+			ranges = <0 0x02020000 0x54000>;
+
+			smp-sysram@0 {
+				compatible = "samsung,exynos4210-sysram";
+				reg = <0x0 0x1000>;
+			};
+
+			smp-sysram@53000 {
+				compatible = "samsung,exynos4210-sysram-ns";
+				reg = <0x53000 0x1000>;
+			};
+		};
+
+		clock: clock-controller@10010000 {
+			compatible = "samsung,exynos5410-clock";
+			reg = <0x10010000 0x30000>;
+			#clock-cells = <1>;
+		};
+
+		mmc_0: mmc@12200000 {
+			compatible = "samsung,exynos5250-dw-mshc";
+			reg = <0x12200000 0x1000>;
+			interrupts = <0 75 0>;
+			#address-cells = <1>;
+			#size-cells = <0>;
+			clocks = <&clock CLK_MMC0>, <&clock CLK_SCLK_MMC0>;
+			clock-names = "biu", "ciu";
+			fifo-depth = <0x80>;
+			status = "disabled";
+		};
+
+		mmc_1: mmc@12210000 {
+			compatible = "samsung,exynos5250-dw-mshc";
+			reg = <0x12210000 0x1000>;
+			interrupts = <0 76 0>;
+			#address-cells = <1>;
+			#size-cells = <0>;
+			clocks = <&clock CLK_MMC1>, <&clock CLK_SCLK_MMC1>;
+			clock-names = "biu", "ciu";
+			fifo-depth = <0x80>;
+			status = "disabled";
+		};
+
+		mmc_2: mmc@12220000 {
+			compatible = "samsung,exynos5250-dw-mshc";
+			reg = <0x12220000 0x1000>;
+			interrupts = <0 77 0>;
+			#address-cells = <1>;
+			#size-cells = <0>;
+			clocks = <&clock CLK_MMC2>, <&clock CLK_SCLK_MMC2>;
+			clock-names = "biu", "ciu";
+			fifo-depth = <0x80>;
+			status = "disabled";
+		};
+
+		uart0: serial@12C00000 {
+			compatible = "samsung,exynos4210-uart";
+			reg = <0x12C00000 0x100>;
+			interrupts = <0 51 0>;
+			clocks = <&clock CLK_UART0>, <&clock CLK_SCLK_UART0>;
+			clock-names = "uart", "clk_uart_baud0";
+			status = "disabled";
+		};
+
+		uart1: serial@12C10000 {
+			compatible = "samsung,exynos4210-uart";
+			reg = <0x12C10000 0x100>;
+			interrupts = <0 52 0>;
+			clocks = <&clock CLK_UART1>, <&clock CLK_SCLK_UART1>;
+			clock-names = "uart", "clk_uart_baud0";
+			status = "disabled";
+		};
+
+		uart2: serial@12C20000 {
+			compatible = "samsung,exynos4210-uart";
+			reg = <0x12C20000 0x100>;
+			interrupts = <0 53 0>;
+			clocks = <&clock CLK_UART2>, <&clock CLK_SCLK_UART2>;
+			clock-names = "uart", "clk_uart_baud0";
+			status = "disabled";
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/exynos5420-arndale-octa.dts b/arch/arm/boot/dts/exynos5420-arndale-octa.dts
index 80a3bf4c598..434fd9d3e09 100644
--- a/arch/arm/boot/dts/exynos5420-arndale-octa.dts
+++ b/arch/arm/boot/dts/exynos5420-arndale-octa.dts
@@ -26,6 +26,11 @@
 		bootargs = "console=ttySAC3,115200";
 	};
 
+	firmware@02073000 {
+		compatible = "samsung,secure-firmware";
+		reg = <0x02073000 0x1000>;
+	};
+
 	fixed-rate-clocks {
 		oscclk {
 			compatible = "samsung,exynos5420-oscclk";
@@ -37,6 +42,11 @@
 		status = "okay";
 	};
 
+	codec@11000000 {
+		samsung,mfc-r = <0x43000000 0x800000>;
+		samsung,mfc-l = <0x51000000 0x800000>;
+	};
+
 	mmc@12200000 {
 		status = "okay";
 		broken-cd;
@@ -364,16 +374,4 @@
 			gpio-key,wakeup;
 		};
 	};
-
-	amba {
-		mdma1: mdma@11C10000 {
-			/*
-			 * MDMA1 can support both secure and non-secure
-			 * AXI transactions. When this is enabled in the kernel
-			 * for boards that run in secure mode, we are getting
-			 * imprecise external aborts causing the kernel to oops.
-			 */
-			status = "disabled";
-		};
-	};
 };
diff --git a/arch/arm/boot/dts/exynos5420-peach-pit.dts b/arch/arm/boot/dts/exynos5420-peach-pit.dts
new file mode 100644
index 00000000000..1c5b8f9f4a3
--- /dev/null
+++ b/arch/arm/boot/dts/exynos5420-peach-pit.dts
@@ -0,0 +1,287 @@
+/*
+ * Google Peach Pit Rev 6+ board device tree source
+ *
+ * Copyright (c) 2014 Google, 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.
+ */
+
+/dts-v1/;
+#include <dt-bindings/input/input.h>
+#include <dt-bindings/gpio/gpio.h>
+#include "exynos5420.dtsi"
+
+/ {
+	model = "Google Peach Pit Rev 6+";
+
+	compatible = "google,pit-rev16",
+		"google,pit-rev15", "google,pit-rev14",
+		"google,pit-rev13", "google,pit-rev12",
+		"google,pit-rev11", "google,pit-rev10",
+		"google,pit-rev9", "google,pit-rev8",
+		"google,pit-rev7", "google,pit-rev6",
+		"google,pit", "google,peach","samsung,exynos5420",
+		"samsung,exynos5";
+
+	memory {
+		reg = <0x20000000 0x80000000>;
+	};
+
+	fixed-rate-clocks {
+		oscclk {
+			compatible = "samsung,exynos5420-oscclk";
+			clock-frequency = <24000000>;
+		};
+	};
+
+	gpio-keys {
+		compatible = "gpio-keys";
+
+		pinctrl-names = "default";
+		pinctrl-0 = <&power_key_irq>;
+
+		power {
+			label = "Power";
+			gpios = <&gpx1 2 GPIO_ACTIVE_LOW>;
+			linux,code = <KEY_POWER>;
+			gpio-key,wakeup;
+		};
+	};
+
+	backlight {
+		compatible = "pwm-backlight";
+		pwms = <&pwm 0 1000000 0>;
+		brightness-levels = <0 100 500 1000 1500 2000 2500 2800>;
+		default-brightness-level = <7>;
+		pinctrl-0 = <&pwm0_out>;
+		pinctrl-names = "default";
+	};
+
+	sound {
+		compatible = "google,snow-audio-max98090";
+
+		samsung,i2s-controller = <&i2s0>;
+		samsung,audio-codec = <&max98090>;
+	};
+
+	usb300_vbus_reg: regulator-usb300 {
+		compatible = "regulator-fixed";
+		regulator-name = "P5.0V_USB3CON0";
+		regulator-min-microvolt = <5000000>;
+		regulator-max-microvolt = <5000000>;
+		gpio = <&gph0 0 0>;
+		pinctrl-names = "default";
+		pinctrl-0 = <&usb300_vbus_en>;
+		enable-active-high;
+	};
+
+	usb301_vbus_reg: regulator-usb301 {
+		compatible = "regulator-fixed";
+		regulator-name = "P5.0V_USB3CON1";
+		regulator-min-microvolt = <5000000>;
+		regulator-max-microvolt = <5000000>;
+		gpio = <&gph0 1 0>;
+		pinctrl-names = "default";
+		pinctrl-0 = <&usb301_vbus_en>;
+		enable-active-high;
+	};
+};
+
+&pinctrl_0 {
+	max98090_irq: max98090-irq {
+		samsung,pins = "gpx0-2";
+		samsung,pin-function = <0>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <0>;
+	};
+
+	tpm_irq: tpm-irq {
+		samsung,pins = "gpx1-0";
+		samsung,pin-function = <0>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <0>;
+	};
+
+	power_key_irq: power-key-irq {
+		samsung,pins = "gpx1-2";
+		samsung,pin-function = <0>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <0>;
+	};
+
+	hdmi_hpd_irq: hdmi-hpd-irq {
+		samsung,pins = "gpx3-7";
+		samsung,pin-function = <0>;
+		samsung,pin-pud = <1>;
+		samsung,pin-drv = <0>;
+	};
+
+	dp_hpd_gpio: dp_hpd_gpio {
+		samsung,pins = "gpx2-6";
+		samsung,pin-function = <0>;
+		samsung,pin-pud = <3>;
+		samsung,pin-drv = <0>;
+	};
+};
+
+&pinctrl_3 {
+	usb300_vbus_en: usb300-vbus-en {
+		samsung,pins = "gph0-0";
+		samsung,pin-function = <1>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <0>;
+	};
+
+	usb301_vbus_en: usb301-vbus-en {
+		samsung,pins = "gph0-1";
+		samsung,pin-function = <1>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <0>;
+	};
+};
+
+&rtc {
+	status = "okay";
+};
+
+&uart_3 {
+	status = "okay";
+};
+
+&mmc_0 {
+	status = "okay";
+	num-slots = <1>;
+	broken-cd;
+	caps2-mmc-hs200-1_8v;
+	supports-highspeed;
+	non-removable;
+	card-detect-delay = <200>;
+	clock-frequency = <400000000>;
+	samsung,dw-mshc-ciu-div = <3>;
+	samsung,dw-mshc-sdr-timing = <0 4>;
+	samsung,dw-mshc-ddr-timing = <0 2>;
+	pinctrl-names = "default";
+	pinctrl-0 = <&sd0_clk &sd0_cmd &sd0_bus4 &sd0_bus8>;
+
+	slot@0 {
+		reg = <0>;
+		bus-width = <8>;
+	};
+};
+
+&mmc_2 {
+	status = "okay";
+	num-slots = <1>;
+	supports-highspeed;
+	card-detect-delay = <200>;
+	clock-frequency = <400000000>;
+	samsung,dw-mshc-ciu-div = <3>;
+	samsung,dw-mshc-sdr-timing = <2 3>;
+	samsung,dw-mshc-ddr-timing = <1 2>;
+	pinctrl-names = "default";
+	pinctrl-0 = <&sd2_clk &sd2_cmd &sd2_cd &sd2_bus4>;
+
+	slot@0 {
+		reg = <0>;
+		bus-width = <4>;
+	};
+};
+
+&hsi2c_7 {
+	status = "okay";
+
+	max98090: codec@10 {
+		compatible = "maxim,max98090";
+		reg = <0x10>;
+		interrupts = <2 0>;
+		interrupt-parent = <&gpx0>;
+		pinctrl-names = "default";
+		pinctrl-0 = <&max98090_irq>;
+	};
+};
+
+&hsi2c_9 {
+	status = "okay";
+	clock-frequency = <400000>;
+
+	tpm@20 {
+		compatible = "infineon,slb9645tt";
+		reg = <0x20>;
+
+		/* Unused irq; but still need to configure the pins */
+		pinctrl-names = "default";
+		pinctrl-0 = <&tpm_irq>;
+	};
+};
+
+&i2c_2	{
+	status = "okay";
+	samsung,i2c-sda-delay = <100>;
+	samsung,i2c-max-bus-freq = <66000>;
+	samsung,i2c-slave-addr = <0x50>;
+};
+
+&hdmi {
+	status = "okay";
+	hpd-gpio = <&gpx3 7 GPIO_ACTIVE_HIGH>;
+	pinctrl-names = "default";
+	pinctrl-0 = <&hdmi_hpd_irq>;
+	ddc = <&i2c_2>;
+};
+
+&usbdrd_phy0 {
+	vbus-supply = <&usb300_vbus_reg>;
+};
+
+&usbdrd_phy1 {
+	vbus-supply = <&usb301_vbus_reg>;
+};
+
+/*
+ * Use longest HW watchdog in SoC (32 seconds) since the hardware
+ * watchdog provides no debugging information (compared to soft/hard
+ * lockup detectors) and so should be last resort.
+ */
+&watchdog {
+	timeout-sec = <32>;
+};
+
+&i2s0 {
+	status = "okay";
+};
+
+&fimd {
+	status = "okay";
+	samsung,invert-vclk;
+};
+
+&dp {
+	status = "okay";
+	pinctrl-names = "default";
+	pinctrl-0 = <&dp_hpd_gpio>;
+	samsung,color-space = <0>;
+	samsung,dynamic-range = <0>;
+	samsung,ycbcr-coeff = <0>;
+	samsung,color-depth = <1>;
+	samsung,link-rate = <0x06>;
+	samsung,lane-count = <2>;
+	samsung,hpd-gpio = <&gpx2 6 0>;
+
+	display-timings {
+		native-mode = <&timing1>;
+
+		timing1: timing@1 {
+			clock-frequency = <70589280>;
+			hactive = <1366>;
+			vactive = <768>;
+			hfront-porch = <40>;
+			hback-porch = <40>;
+			hsync-len = <32>;
+			vback-porch = <10>;
+			vfront-porch = <12>;
+			vsync-len = <6>;
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/exynos5420-pinctrl.dtsi b/arch/arm/boot/dts/exynos5420-pinctrl.dtsi
index e62c8eb5743..ba686e40eac 100644
--- a/arch/arm/boot/dts/exynos5420-pinctrl.dtsi
+++ b/arch/arm/boot/dts/exynos5420-pinctrl.dtsi
@@ -624,6 +624,34 @@
 			samsung,pin-drv = <0>;
 		};
 
+		pwm0_out: pwm0-out {
+			samsung,pins = "gpb2-0";
+			samsung,pin-function = <2>;
+			samsung,pin-pud = <0>;
+			samsung,pin-drv = <0>;
+		};
+
+		pwm1_out: pwm1-out {
+			samsung,pins = "gpb2-1";
+			samsung,pin-function = <2>;
+			samsung,pin-pud = <0>;
+			samsung,pin-drv = <0>;
+		};
+
+		pwm2_out: pwm2-out {
+			samsung,pins = "gpb2-2";
+			samsung,pin-function = <2>;
+			samsung,pin-pud = <0>;
+			samsung,pin-drv = <0>;
+		};
+
+		pwm3_out: pwm3-out {
+			samsung,pins = "gpb2-3";
+			samsung,pin-function = <2>;
+			samsung,pin-pud = <0>;
+			samsung,pin-drv = <0>;
+		};
+
 		i2c7_hs_bus: i2c7-hs-bus {
 			samsung,pins = "gpb2-2", "gpb2-3";
 			samsung,pin-function = <3>;
diff --git a/arch/arm/boot/dts/exynos5420-smdk5420.dts b/arch/arm/boot/dts/exynos5420-smdk5420.dts
index 69104850eb5..6052aa9c565 100644
--- a/arch/arm/boot/dts/exynos5420-smdk5420.dts
+++ b/arch/arm/boot/dts/exynos5420-smdk5420.dts
@@ -68,6 +68,11 @@
 		status = "okay";
 	};
 
+	codec@11000000 {
+		samsung,mfc-r = <0x43000000 0x800000>;
+		samsung,mfc-l = <0x51000000 0x800000>;
+	};
+
 	mmc@12200000 {
 		status = "okay";
 		broken-cd;
@@ -140,6 +145,22 @@
 		};
 	};
 
+	pinctrl@14000000 {
+		usb300_vbus_en: usb300-vbus-en {
+			samsung,pins = "gpg0-5";
+			samsung,pin-function = <1>;
+			samsung,pin-pud = <0>;
+			samsung,pin-drv = <0>;
+		};
+
+		usb301_vbus_en: usb301-vbus-en {
+			samsung,pins = "gpg1-4";
+			samsung,pin-function = <1>;
+			samsung,pin-pud = <0>;
+			samsung,pin-drv = <0>;
+		};
+	};
+
 	hdmi@14530000 {
 		status = "okay";
 		hpd-gpio = <&gpx3 7 0>;
@@ -147,6 +168,36 @@
 		pinctrl-0 = <&hdmi_hpd_irq>;
 	};
 
+	usb300_vbus_reg: regulator-usb300 {
+		compatible = "regulator-fixed";
+		regulator-name = "VBUS0";
+		regulator-min-microvolt = <5000000>;
+		regulator-max-microvolt = <5000000>;
+		gpio = <&gpg0 5 0>;
+		pinctrl-names = "default";
+		pinctrl-0 = <&usb300_vbus_en>;
+		enable-active-high;
+	};
+
+	usb301_vbus_reg: regulator-usb301 {
+		compatible = "regulator-fixed";
+		regulator-name = "VBUS1";
+		regulator-min-microvolt = <5000000>;
+		regulator-max-microvolt = <5000000>;
+		gpio = <&gpg1 4 0>;
+		pinctrl-names = "default";
+		pinctrl-0 = <&usb301_vbus_en>;
+		enable-active-high;
+	};
+
+	phy@12100000 {
+		vbus-supply = <&usb300_vbus_reg>;
+	};
+
+	phy@12500000 {
+		vbus-supply = <&usb301_vbus_reg>;
+	};
+
 	i2c_2: i2c@12C80000 {
 		samsung,i2c-sda-delay = <100>;
 		samsung,i2c-max-bus-freq = <66000>;
diff --git a/arch/arm/boot/dts/exynos5420.dtsi b/arch/arm/boot/dts/exynos5420.dtsi
index c3a9a66c576..e38532271ef 100644
--- a/arch/arm/boot/dts/exynos5420.dtsi
+++ b/arch/arm/boot/dts/exynos5420.dtsi
@@ -47,6 +47,8 @@
 		spi0 = &spi_0;
 		spi1 = &spi_1;
 		spi2 = &spi_2;
+		usbdrdphy0 = &usbdrd_phy0;
+		usbdrdphy1 = &usbdrd_phy1;
 	};
 
 	cpus {
@@ -58,6 +60,7 @@
 			compatible = "arm,cortex-a15";
 			reg = <0x0>;
 			clock-frequency = <1800000000>;
+			cci-control-port = <&cci_control1>;
 		};
 
 		cpu1: cpu@1 {
@@ -65,6 +68,7 @@
 			compatible = "arm,cortex-a15";
 			reg = <0x1>;
 			clock-frequency = <1800000000>;
+			cci-control-port = <&cci_control1>;
 		};
 
 		cpu2: cpu@2 {
@@ -72,6 +76,7 @@
 			compatible = "arm,cortex-a15";
 			reg = <0x2>;
 			clock-frequency = <1800000000>;
+			cci-control-port = <&cci_control1>;
 		};
 
 		cpu3: cpu@3 {
@@ -79,6 +84,7 @@
 			compatible = "arm,cortex-a15";
 			reg = <0x3>;
 			clock-frequency = <1800000000>;
+			cci-control-port = <&cci_control1>;
 		};
 
 		cpu4: cpu@100 {
@@ -86,6 +92,7 @@
 			compatible = "arm,cortex-a7";
 			reg = <0x100>;
 			clock-frequency = <1000000000>;
+			cci-control-port = <&cci_control0>;
 		};
 
 		cpu5: cpu@101 {
@@ -93,6 +100,7 @@
 			compatible = "arm,cortex-a7";
 			reg = <0x101>;
 			clock-frequency = <1000000000>;
+			cci-control-port = <&cci_control0>;
 		};
 
 		cpu6: cpu@102 {
@@ -100,6 +108,7 @@
 			compatible = "arm,cortex-a7";
 			reg = <0x102>;
 			clock-frequency = <1000000000>;
+			cci-control-port = <&cci_control0>;
 		};
 
 		cpu7: cpu@103 {
@@ -107,6 +116,44 @@
 			compatible = "arm,cortex-a7";
 			reg = <0x103>;
 			clock-frequency = <1000000000>;
+			cci-control-port = <&cci_control0>;
+		};
+	};
+
+	cci@10d20000 {
+		compatible = "arm,cci-400";
+		#address-cells = <1>;
+		#size-cells = <1>;
+		reg = <0x10d20000 0x1000>;
+		ranges = <0x0 0x10d20000 0x6000>;
+
+		cci_control0: slave-if@4000 {
+			compatible = "arm,cci-400-ctrl-if";
+			interface-type = "ace";
+			reg = <0x4000 0x1000>;
+		};
+		cci_control1: slave-if@5000 {
+			compatible = "arm,cci-400-ctrl-if";
+			interface-type = "ace";
+			reg = <0x5000 0x1000>;
+		};
+	};
+
+	sysram@02020000 {
+		compatible = "mmio-sram";
+		reg = <0x02020000 0x54000>;
+		#address-cells = <1>;
+		#size-cells = <1>;
+		ranges = <0 0x02020000 0x54000>;
+
+		smp-sysram@0 {
+			compatible = "samsung,exynos4210-sysram";
+			reg = <0x0 0x1000>;
+		};
+
+		smp-sysram@53000 {
+			compatible = "samsung,exynos4210-sysram-ns";
+			reg = <0x53000 0x1000>;
 		};
 	};
 
@@ -125,12 +172,13 @@
 		clock-names = "pll_ref", "pll_in", "sclk_audio", "sclk_pcm_in";
 	};
 
-	codec@11000000 {
+	mfc: codec@11000000 {
 		compatible = "samsung,mfc-v7";
 		reg = <0x11000000 0x10000>;
 		interrupts = <0 96 0>;
 		clocks = <&clock CLK_MFC>;
 		clock-names = "mfc";
+		samsung,power-domain = <&mfc_pd>;
 	};
 
 	mmc_0: mmc@12200000 {
@@ -169,7 +217,7 @@
 		status = "disabled";
 	};
 
-	mct@101C0000 {
+	mct: mct@101C0000 {
 		compatible = "samsung,exynos4210-mct";
 		reg = <0x101C0000 0x800>;
 		interrupt-controller;
@@ -219,16 +267,6 @@
 		reg = <0x100440C0 0x20>;
 	};
 
-	mau_pd: power-domain@100440E0 {
-		compatible = "samsung,exynos4210-pd";
-		reg = <0x100440E0 0x20>;
-	};
-
-	g2d_pd: power-domain@10044100 {
-		compatible = "samsung,exynos4210-pd";
-		reg = <0x10044100 0x20>;
-	};
-
 	msc_pd: power-domain@10044120 {
 		compatible = "samsung,exynos4210-pd";
 		reg = <0x10044120 0x20>;
@@ -270,7 +308,7 @@
 		interrupts = <0 47 0>;
 	};
 
-	rtc@101E0000 {
+	rtc: rtc@101E0000 {
 		clocks = <&clock CLK_RTC>;
 		clock-names = "rtc";
 		status = "disabled";
@@ -336,6 +374,13 @@
 			#dma-cells = <1>;
 			#dma-channels = <8>;
 			#dma-requests = <1>;
+			/*
+			 * MDMA1 can support both secure and non-secure
+			 * AXI transactions. When this is enabled in the kernel
+			 * for boards that run in secure mode, we are getting
+			 * imprecise external aborts causing the kernel to oops.
+			 */
+			status = "disabled";
 		};
 	};
 
@@ -385,7 +430,7 @@
 	spi_0: spi@12d20000 {
 		compatible = "samsung,exynos4210-spi";
 		reg = <0x12d20000 0x100>;
-		interrupts = <0 66 0>;
+		interrupts = <0 68 0>;
 		dmas = <&pdma0 5
 			&pdma0 4>;
 		dma-names = "tx", "rx";
@@ -401,7 +446,7 @@
 	spi_1: spi@12d30000 {
 		compatible = "samsung,exynos4210-spi";
 		reg = <0x12d30000 0x100>;
-		interrupts = <0 67 0>;
+		interrupts = <0 69 0>;
 		dmas = <&pdma1 5
 			&pdma1 4>;
 		dma-names = "tx", "rx";
@@ -417,7 +462,7 @@
 	spi_2: spi@12d40000 {
 		compatible = "samsung,exynos4210-spi";
 		reg = <0x12d40000 0x100>;
-		interrupts = <0 68 0>;
+		interrupts = <0 70 0>;
 		dmas = <&pdma0 7
 			&pdma0 6>;
 		dma-names = "tx", "rx";
@@ -430,22 +475,22 @@
 		status = "disabled";
 	};
 
-	serial@12C00000 {
+	uart_0: serial@12C00000 {
 		clocks = <&clock CLK_UART0>, <&clock CLK_SCLK_UART0>;
 		clock-names = "uart", "clk_uart_baud0";
 	};
 
-	serial@12C10000 {
+	uart_1: serial@12C10000 {
 		clocks = <&clock CLK_UART1>, <&clock CLK_SCLK_UART1>;
 		clock-names = "uart", "clk_uart_baud0";
 	};
 
-	serial@12C20000 {
+	uart_2: serial@12C20000 {
 		clocks = <&clock CLK_UART2>, <&clock CLK_SCLK_UART2>;
 		clock-names = "uart", "clk_uart_baud0";
 	};
 
-	serial@12C30000 {
+	uart_3: serial@12C30000 {
 		clocks = <&clock CLK_UART3>, <&clock CLK_SCLK_UART3>;
 		clock-names = "uart", "clk_uart_baud0";
 	};
@@ -465,14 +510,14 @@
 		#phy-cells = <0>;
 	};
 
-	dp-controller@145B0000 {
+	dp: dp-controller@145B0000 {
 		clocks = <&clock CLK_DP1>;
 		clock-names = "dp";
 		phys = <&dp_phy>;
 		phy-names = "dp";
 	};
 
-	fimd@14400000 {
+	fimd: fimd@14400000 {
 		samsung,power-domain = <&disp_pd>;
 		clocks = <&clock CLK_SCLK_FIMD1>, <&clock CLK_FIMD1>;
 		clock-names = "sclk_fimd", "fimd";
@@ -549,7 +594,7 @@
 		#size-cells = <0>;
 		pinctrl-names = "default";
 		pinctrl-0 = <&i2c4_hs_bus>;
-		clocks = <&clock CLK_I2C4>;
+		clocks = <&clock CLK_USI0>;
 		clock-names = "hsi2c";
 		status = "disabled";
 	};
@@ -562,7 +607,7 @@
 		#size-cells = <0>;
 		pinctrl-names = "default";
 		pinctrl-0 = <&i2c5_hs_bus>;
-		clocks = <&clock CLK_I2C5>;
+		clocks = <&clock CLK_USI1>;
 		clock-names = "hsi2c";
 		status = "disabled";
 	};
@@ -575,7 +620,7 @@
 		#size-cells = <0>;
 		pinctrl-names = "default";
 		pinctrl-0 = <&i2c6_hs_bus>;
-		clocks = <&clock CLK_I2C6>;
+		clocks = <&clock CLK_USI2>;
 		clock-names = "hsi2c";
 		status = "disabled";
 	};
@@ -588,7 +633,7 @@
 		#size-cells = <0>;
 		pinctrl-names = "default";
 		pinctrl-0 = <&i2c7_hs_bus>;
-		clocks = <&clock CLK_I2C7>;
+		clocks = <&clock CLK_USI3>;
 		clock-names = "hsi2c";
 		status = "disabled";
 	};
@@ -601,7 +646,7 @@
 		#size-cells = <0>;
 		pinctrl-names = "default";
 		pinctrl-0 = <&i2c8_hs_bus>;
-		clocks = <&clock CLK_I2C8>;
+		clocks = <&clock CLK_USI4>;
 		clock-names = "hsi2c";
 		status = "disabled";
 	};
@@ -614,7 +659,7 @@
 		#size-cells = <0>;
 		pinctrl-names = "default";
 		pinctrl-0 = <&i2c9_hs_bus>;
-		clocks = <&clock CLK_I2C9>;
+		clocks = <&clock CLK_USI5>;
 		clock-names = "hsi2c";
 		status = "disabled";
 	};
@@ -627,13 +672,13 @@
 		#size-cells = <0>;
 		pinctrl-names = "default";
 		pinctrl-0 = <&i2c10_hs_bus>;
-		clocks = <&clock CLK_I2C10>;
+		clocks = <&clock CLK_USI6>;
 		clock-names = "hsi2c";
 		status = "disabled";
 	};
 
-	hdmi@14530000 {
-		compatible = "samsung,exynos4212-hdmi";
+	hdmi: hdmi@14530000 {
+		compatible = "samsung,exynos5420-hdmi";
 		reg = <0x14530000 0x70000>;
 		interrupts = <0 95 0>;
 		clocks = <&clock CLK_HDMI>, <&clock CLK_SCLK_HDMI>,
@@ -641,10 +686,16 @@
 			 <&clock CLK_MOUT_HDMI>;
 		clock-names = "hdmi", "sclk_hdmi", "sclk_pixel",
 			"sclk_hdmiphy", "mout_hdmi";
+		phy = <&hdmiphy>;
+		samsung,syscon-phandle = <&pmu_system_controller>;
 		status = "disabled";
 	};
 
-	mixer@14450000 {
+	hdmiphy: hdmiphy@145D0000 {
+		reg = <0x145D0000 0x20>;
+	};
+
+	mixer: mixer@14450000 {
 		compatible = "samsung,exynos5420-mixer";
 		reg = <0x14450000 0x10000>;
 		interrupts = <0 94 0>;
@@ -675,6 +726,11 @@
 		reg = <0x10040000 0x5000>;
 	};
 
+	sysreg_system_controller: syscon@10050000 {
+		compatible = "samsung,exynos5-sysreg", "syscon";
+		reg = <0x10050000 0x5000>;
+	};
+
 	tmu_cpu0: tmu@10060000 {
 		compatible = "samsung,exynos5420-tmu";
 		reg = <0x10060000 0x100>;
@@ -715,7 +771,7 @@
 		clock-names = "tmu_apbif", "tmu_triminfo_apbif";
 	};
 
-        watchdog@101D0000 {
+        watchdog: watchdog@101D0000 {
 		compatible = "samsung,exynos5420-wdt";
 		reg = <0x101D0000 0x100>;
 		interrupts = <0 42 0>;
@@ -724,12 +780,103 @@
 		samsung,syscon-phandle = <&pmu_system_controller>;
         };
 
-	sss@10830000 {
+	sss: sss@10830000 {
 		compatible = "samsung,exynos4210-secss";
 		reg = <0x10830000 0x10000>;
 		interrupts = <0 112 0>;
-		clocks = <&clock 471>;
+		clocks = <&clock CLK_SSS>;
 		clock-names = "secss";
-		samsung,power-domain = <&g2d_pd>;
+	};
+
+	usbdrd3_0: usb@12000000 {
+		compatible = "samsung,exynos5250-dwusb3";
+		clocks = <&clock CLK_USBD300>;
+		clock-names = "usbdrd30";
+		#address-cells = <1>;
+		#size-cells = <1>;
+		ranges;
+
+		dwc3 {
+			compatible = "snps,dwc3";
+			reg = <0x12000000 0x10000>;
+			interrupts = <0 72 0>;
+			phys = <&usbdrd_phy0 0>, <&usbdrd_phy0 1>;
+			phy-names = "usb2-phy", "usb3-phy";
+		};
+	};
+
+	usbdrd_phy0: phy@12100000 {
+		compatible = "samsung,exynos5420-usbdrd-phy";
+		reg = <0x12100000 0x100>;
+		clocks = <&clock CLK_USBD300>, <&clock CLK_SCLK_USBPHY300>;
+		clock-names = "phy", "ref";
+		samsung,pmu-syscon = <&pmu_system_controller>;
+		#phy-cells = <1>;
+	};
+
+	usbdrd3_1: usb@12400000 {
+		compatible = "samsung,exynos5250-dwusb3";
+		clocks = <&clock CLK_USBD301>;
+		clock-names = "usbdrd30";
+		#address-cells = <1>;
+		#size-cells = <1>;
+		ranges;
+
+		dwc3 {
+			compatible = "snps,dwc3";
+			reg = <0x12400000 0x10000>;
+			interrupts = <0 73 0>;
+			phys = <&usbdrd_phy1 0>, <&usbdrd_phy1 1>;
+			phy-names = "usb2-phy", "usb3-phy";
+		};
+	};
+
+	usbdrd_phy1: phy@12500000 {
+		compatible = "samsung,exynos5420-usbdrd-phy";
+		reg = <0x12500000 0x100>;
+		clocks = <&clock CLK_USBD301>, <&clock CLK_SCLK_USBPHY301>;
+		clock-names = "phy", "ref";
+		samsung,pmu-syscon = <&pmu_system_controller>;
+		#phy-cells = <1>;
+	};
+
+	usbhost2: usb@12110000 {
+		compatible = "samsung,exynos4210-ehci";
+		reg = <0x12110000 0x100>;
+		interrupts = <0 71 0>;
+
+		clocks = <&clock CLK_USBH20>;
+		clock-names = "usbhost";
+		#address-cells = <1>;
+		#size-cells = <0>;
+		port@0 {
+			reg = <0>;
+			phys = <&usb2_phy 1>;
+		};
+	};
+
+	usbhost1: usb@12120000 {
+		compatible = "samsung,exynos4210-ohci";
+		reg = <0x12120000 0x100>;
+		interrupts = <0 71 0>;
+
+		clocks = <&clock CLK_USBH20>;
+		clock-names = "usbhost";
+		#address-cells = <1>;
+		#size-cells = <0>;
+		port@0 {
+			reg = <0>;
+			phys = <&usb2_phy 1>;
+		};
+	};
+
+	usb2_phy: phy@12130000 {
+		compatible = "samsung,exynos5250-usb2-phy";
+		reg = <0x12130000 0x100>;
+		clocks = <&clock CLK_USBH20>, <&clock CLK_SCLK_USBPHY300>;
+		clock-names = "phy", "ref";
+		#phy-cells = <1>;
+		samsung,sysreg-phandle = <&sysreg_system_controller>;
+		samsung,pmureg-phandle = <&pmu_system_controller>;
 	};
 };
diff --git a/arch/arm/boot/dts/exynos5440.dtsi b/arch/arm/boot/dts/exynos5440.dtsi
index 84f77c2fe4d..ae3a17c791f 100644
--- a/arch/arm/boot/dts/exynos5440.dtsi
+++ b/arch/arm/boot/dts/exynos5440.dtsi
@@ -176,7 +176,7 @@
 		clock-names = "i2c";
 	};
 
-	watchdog {
+	watchdog@110000 {
 		compatible = "samsung,s3c2410-wdt";
 		reg = <0x110000 0x1000>;
 		interrupts = <0 1 0>;
diff --git a/arch/arm/boot/dts/exynos5800-peach-pi.dts b/arch/arm/boot/dts/exynos5800-peach-pi.dts
new file mode 100644
index 00000000000..f3af2079a06
--- /dev/null
+++ b/arch/arm/boot/dts/exynos5800-peach-pi.dts
@@ -0,0 +1,253 @@
+/*
+ * Google Peach Pi Rev 10+ board device tree source
+ *
+ * Copyright (c) 2014 Google, 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.
+ */
+
+/dts-v1/;
+#include <dt-bindings/input/input.h>
+#include <dt-bindings/gpio/gpio.h>
+#include "exynos5800.dtsi"
+
+/ {
+	model = "Google Peach Pi Rev 10+";
+
+	compatible = "google,pi-rev16",
+		"google,pi-rev15", "google,pi-rev14",
+		"google,pi-rev13", "google,pi-rev12",
+		"google,pi-rev11", "google,pi-rev10",
+		"google,pi", "google,peach", "samsung,exynos5800",
+		"samsung,exynos5";
+
+	memory {
+		reg = <0x20000000 0x80000000>;
+	};
+
+	fixed-rate-clocks {
+		oscclk {
+			compatible = "samsung,exynos5420-oscclk";
+			clock-frequency = <24000000>;
+		};
+	};
+
+	gpio-keys {
+		compatible = "gpio-keys";
+
+		pinctrl-names = "default";
+		pinctrl-0 = <&power_key_irq>;
+
+		power {
+			label = "Power";
+			gpios = <&gpx1 2 GPIO_ACTIVE_LOW>;
+			linux,code = <KEY_POWER>;
+			gpio-key,wakeup;
+		};
+	};
+
+	backlight {
+		compatible = "pwm-backlight";
+		pwms = <&pwm 0 1000000 0>;
+		brightness-levels = <0 100 500 1000 1500 2000 2500 2800>;
+		default-brightness-level = <7>;
+		pinctrl-0 = <&pwm0_out>;
+		pinctrl-names = "default";
+	};
+
+	usb300_vbus_reg: regulator-usb300 {
+		compatible = "regulator-fixed";
+		regulator-name = "P5.0V_USB3CON0";
+		regulator-min-microvolt = <5000000>;
+		regulator-max-microvolt = <5000000>;
+		gpio = <&gph0 0 0>;
+		pinctrl-names = "default";
+		pinctrl-0 = <&usb300_vbus_en>;
+		enable-active-high;
+	};
+
+	usb301_vbus_reg: regulator-usb301 {
+		compatible = "regulator-fixed";
+		regulator-name = "P5.0V_USB3CON1";
+		regulator-min-microvolt = <5000000>;
+		regulator-max-microvolt = <5000000>;
+		gpio = <&gph0 1 0>;
+		pinctrl-names = "default";
+		pinctrl-0 = <&usb301_vbus_en>;
+		enable-active-high;
+	};
+};
+
+&pinctrl_0 {
+	tpm_irq: tpm-irq {
+		samsung,pins = "gpx1-0";
+		samsung,pin-function = <0>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <0>;
+	};
+
+	power_key_irq: power-key-irq {
+		samsung,pins = "gpx1-2";
+		samsung,pin-function = <0>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <0>;
+	};
+
+	dp_hpd_gpio: dp_hpd_gpio {
+		samsung,pins = "gpx2-6";
+		samsung,pin-function = <0>;
+		samsung,pin-pud = <3>;
+		samsung,pin-drv = <0>;
+	};
+
+	hdmi_hpd_irq: hdmi-hpd-irq {
+		samsung,pins = "gpx3-7";
+		samsung,pin-function = <0>;
+		samsung,pin-pud = <1>;
+		samsung,pin-drv = <0>;
+	};
+};
+
+&pinctrl_3 {
+	usb300_vbus_en: usb300-vbus-en {
+		samsung,pins = "gph0-0";
+		samsung,pin-function = <1>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <0>;
+	};
+
+	usb301_vbus_en: usb301-vbus-en {
+		samsung,pins = "gph0-1";
+		samsung,pin-function = <1>;
+		samsung,pin-pud = <0>;
+		samsung,pin-drv = <0>;
+	};
+};
+
+&rtc {
+	status = "okay";
+};
+
+&uart_3 {
+	status = "okay";
+};
+
+&mmc_0 {
+	status = "okay";
+	num-slots = <1>;
+	broken-cd;
+	caps2-mmc-hs200-1_8v;
+	supports-highspeed;
+	non-removable;
+	card-detect-delay = <200>;
+	clock-frequency = <400000000>;
+	samsung,dw-mshc-ciu-div = <3>;
+	samsung,dw-mshc-sdr-timing = <0 4>;
+	samsung,dw-mshc-ddr-timing = <0 2>;
+	pinctrl-names = "default";
+	pinctrl-0 = <&sd0_clk &sd0_cmd &sd0_bus4 &sd0_bus8>;
+
+	slot@0 {
+		reg = <0>;
+		bus-width = <8>;
+	};
+};
+
+&mmc_2 {
+	status = "okay";
+	num-slots = <1>;
+	supports-highspeed;
+	card-detect-delay = <200>;
+	clock-frequency = <400000000>;
+	samsung,dw-mshc-ciu-div = <3>;
+	samsung,dw-mshc-sdr-timing = <2 3>;
+	samsung,dw-mshc-ddr-timing = <1 2>;
+	pinctrl-names = "default";
+	pinctrl-0 = <&sd2_clk &sd2_cmd &sd2_cd &sd2_bus4>;
+
+	slot@0 {
+		reg = <0>;
+		bus-width = <4>;
+	};
+};
+
+&dp {
+	status = "okay";
+	pinctrl-names = "default";
+	pinctrl-0 = <&dp_hpd_gpio>;
+	samsung,color-space = <0>;
+	samsung,dynamic-range = <0>;
+	samsung,ycbcr-coeff = <0>;
+	samsung,color-depth = <1>;
+	samsung,link-rate = <0x0a>;
+	samsung,lane-count = <2>;
+	samsung,hpd-gpio = <&gpx2 6 0>;
+
+	display-timings {
+		native-mode = <&timing1>;
+
+		timing1: timing@1 {
+			clock-frequency = <150660000>;
+			hactive = <1920>;
+			vactive = <1080>;
+			hfront-porch = <60>;
+			hback-porch = <172>;
+			hsync-len = <80>;
+			vback-porch = <25>;
+			vfront-porch = <10>;
+			vsync-len = <10>;
+		};
+	};
+};
+
+&fimd {
+	status = "okay";
+	samsung,invert-vclk;
+};
+
+&hsi2c_9 {
+	status = "okay";
+	clock-frequency = <400000>;
+
+	tpm@20 {
+		compatible = "infineon,slb9645tt";
+		reg = <0x20>;
+		/* Unused irq; but still need to configure the pins */
+		pinctrl-names = "default";
+		pinctrl-0 = <&tpm_irq>;
+	};
+};
+
+&i2c_2 {
+	status = "okay";
+	samsung,i2c-sda-delay = <100>;
+	samsung,i2c-max-bus-freq = <66000>;
+	samsung,i2c-slave-addr = <0x50>;
+};
+
+&hdmi {
+	status = "okay";
+	hpd-gpio = <&gpx3 7 GPIO_ACTIVE_HIGH>;
+	pinctrl-names = "default";
+	pinctrl-0 = <&hdmi_hpd_irq>;
+	ddc = <&i2c_2>;
+};
+
+&usbdrd_phy0 {
+	vbus-supply = <&usb300_vbus_reg>;
+};
+
+&usbdrd_phy1 {
+	vbus-supply = <&usb301_vbus_reg>;
+};
+
+/*
+ * Use longest HW watchdog in SoC (32 seconds) since the hardware
+ * watchdog provides no debugging information (compared to soft/hard
+ * lockup detectors) and so should be last resort.
+ */
+&watchdog {
+	timeout-sec = <32>;
+};
diff --git a/arch/arm/boot/dts/exynos5800.dtsi b/arch/arm/boot/dts/exynos5800.dtsi
new file mode 100644
index 00000000000..c0bb3563cac
--- /dev/null
+++ b/arch/arm/boot/dts/exynos5800.dtsi
@@ -0,0 +1,28 @@
+/*
+ * SAMSUNG EXYNOS5800 SoC device tree source
+ *
+ * Copyright (c) 2014 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * SAMSUNG EXYNOS5800 SoC device nodes are listed in this file.
+ * EXYNOS5800 based board files can include this file and provide
+ * values for board specfic bindings.
+ *
+ * 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 "exynos5420.dtsi"
+
+/ {
+	compatible = "samsung,exynos5800", "samsung,exynos5";
+};
+
+&clock {
+	compatible = "samsung,exynos5800-clock";
+};
+
+&mfc {
+	compatible = "samsung,mfc-v8";
+};
diff --git a/arch/arm/boot/dts/imx25-eukrea-mbimxsd25-baseboard.dts b/arch/arm/boot/dts/imx25-eukrea-mbimxsd25-baseboard.dts
index 62fb3da50bd..ad12da38fc9 100644
--- a/arch/arm/boot/dts/imx25-eukrea-mbimxsd25-baseboard.dts
+++ b/arch/arm/boot/dts/imx25-eukrea-mbimxsd25-baseboard.dts
@@ -172,3 +172,16 @@
 	fsl,uart-has-rtscts;
 	status = "okay";
 };
+
+&usbhost1 {
+	phy_type = "serial";
+	dr_mode = "host";
+	status = "okay";
+};
+
+&usbotg {
+	phy_type = "utmi";
+	dr_mode = "otg";
+	external-vbus-divider;
+	status = "okay";
+};
diff --git a/arch/arm/boot/dts/imx25-karo-tx25.dts b/arch/arm/boot/dts/imx25-karo-tx25.dts
index f8db366c46f..9b31faa9637 100644
--- a/arch/arm/boot/dts/imx25-karo-tx25.dts
+++ b/arch/arm/boot/dts/imx25-karo-tx25.dts
@@ -16,21 +16,98 @@
 	model = "Ka-Ro TX25";
 	compatible = "karo,imx25-tx25", "fsl,imx25";
 
+	chosen {
+		stdout-path = &uart1;
+	};
+
+	regulators {
+		compatible = "simple-bus";
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		reg_fec_phy: regulator@0 {
+			compatible = "regulator-fixed";
+			reg = <0>;
+			regulator-name = "fec-phy";
+			regulator-min-microvolt = <3300000>;
+			regulator-max-microvolt = <3300000>;
+			gpio = <&gpio4 9 0>;
+			enable-active-high;
+		};
+	};
+
 	memory {
 		reg = <0x80000000 0x02000000 0x90000000 0x02000000>;
 	};
 };
 
+&iomuxc {
+	pinctrl_uart1: uart1grp {
+		fsl,pins = <
+			MX25_PAD_UART1_TXD__UART1_TXD 0x80000000
+			MX25_PAD_UART1_RXD__UART1_RXD 0x80000000
+			MX25_PAD_UART1_CTS__UART1_CTS 0x80000000
+			MX25_PAD_UART1_RTS__UART1_RTS 0x80000000
+		>;
+	};
+
+	pinctrl_fec: fecgrp {
+		fsl,pins = <
+			MX25_PAD_D11__GPIO_4_9		0x80000000 /* FEC PHY power on pin */
+			MX25_PAD_D13__GPIO_4_7		0x80000000 /* FEC reset */
+			MX25_PAD_FEC_MDC__FEC_MDC	0x80000000
+			MX25_PAD_FEC_MDIO__FEC_MDIO	0x80000000
+			MX25_PAD_FEC_TDATA0__FEC_TDATA0	0x80000000
+			MX25_PAD_FEC_TDATA1__FEC_TDATA1	0x80000000
+			MX25_PAD_FEC_TX_EN__FEC_TX_EN	0x80000000
+			MX25_PAD_FEC_RDATA0__FEC_RDATA0	0x80000000
+			MX25_PAD_FEC_RDATA1__FEC_RDATA1	0x80000000
+			MX25_PAD_FEC_RX_DV__FEC_RX_DV	0x80000000
+			MX25_PAD_FEC_TX_CLK__FEC_TX_CLK	0x80000000
+		>;
+	};
+
+	pinctrl_nfc: nfcgrp {
+		fsl,pins = <
+			MX25_PAD_NF_CE0__NF_CE0		0x80000000
+			MX25_PAD_NFWE_B__NFWE_B		0x80000000
+			MX25_PAD_NFRE_B__NFRE_B		0x80000000
+			MX25_PAD_NFALE__NFALE		0x80000000
+			MX25_PAD_NFCLE__NFCLE		0x80000000
+			MX25_PAD_NFWP_B__NFWP_B		0x80000000
+			MX25_PAD_NFRB__NFRB		0x80000000
+			MX25_PAD_D7__D7			0x80000000
+			MX25_PAD_D6__D6			0x80000000
+			MX25_PAD_D5__D5			0x80000000
+			MX25_PAD_D4__D4			0x80000000
+			MX25_PAD_D3__D3			0x80000000
+			MX25_PAD_D2__D2			0x80000000
+			MX25_PAD_D1__D1			0x80000000
+			MX25_PAD_D0__D0			0x80000000
+		>;
+	};
+};
+
 &uart1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_uart1>;
 	status = "okay";
 };
 
 &fec {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_fec>;
+	phy-reset-gpios = <&gpio3 7 0>;
 	phy-mode = "rmii";
+	phy-supply = <&reg_fec_phy>;
 	status = "okay";
 };
 
 &nfc {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_nfc>;
 	nand-on-flash-bbt;
+	nand-ecc-mode = "hw";
+	nand-bus-width = <8>;
 	status = "okay";
 };
diff --git a/arch/arm/boot/dts/imx25-pdk.dts b/arch/arm/boot/dts/imx25-pdk.dts
index f607ce520ed..c608942b8a3 100644
--- a/arch/arm/boot/dts/imx25-pdk.dts
+++ b/arch/arm/boot/dts/imx25-pdk.dts
@@ -10,6 +10,7 @@
  */
 
 /dts-v1/;
+#include <dt-bindings/input/input.h>
 #include "imx25.dtsi"
 
 / {
@@ -19,18 +20,232 @@
 	memory {
 		reg = <0x80000000 0x4000000>;
 	};
+
+	regulators {
+		compatible = "simple-bus";
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		reg_fec_3v3: regulator@0 {
+			compatible = "regulator-fixed";
+			reg = <0>;
+			regulator-name = "fec-3v3";
+			regulator-min-microvolt = <3300000>;
+			regulator-max-microvolt = <3300000>;
+			gpio = <&gpio2 3 0>;
+			enable-active-high;
+		};
+
+		reg_2p5v: regulator@1 {
+			compatible = "regulator-fixed";
+			reg = <1>;
+			regulator-name = "2P5V";
+			regulator-min-microvolt = <2500000>;
+			regulator-max-microvolt = <2500000>;
+		};
+
+		reg_3p3v: regulator@2 {
+			compatible = "regulator-fixed";
+			reg = <2>;
+			regulator-name = "3P3V";
+			regulator-min-microvolt = <3300000>;
+			regulator-max-microvolt = <3300000>;
+		};
+
+		reg_can_3v3: regulator@3 {
+			compatible = "regulator-fixed";
+			reg = <3>;
+			regulator-name = "can-3v3";
+			regulator-min-microvolt = <3300000>;
+			regulator-max-microvolt = <3300000>;
+			gpio = <&gpio4 6 0>;
+		};
+	};
+
+	sound {
+		compatible = "fsl,imx25-pdk-sgtl5000",
+			     "fsl,imx-audio-sgtl5000";
+		model = "imx25-pdk-sgtl5000";
+		ssi-controller = <&ssi1>;
+		audio-codec = <&codec>;
+		audio-routing =
+			"MIC_IN", "Mic Jack",
+			"Mic Jack", "Mic Bias",
+			"Headphone Jack", "HP_OUT";
+		mux-int-port = <1>;
+		mux-ext-port = <4>;
+	};
 };
 
-&uart1 {
+&audmux {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_audmux>;
+	status = "okay";
+};
+
+&can1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_can1>;
+	xceiver-supply = <&reg_can_3v3>;
+	status = "okay";
+};
+
+&esdhc1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_esdhc1>;
+	cd-gpios = <&gpio2 1 0>;
+	wp-gpios = <&gpio2 0 0>;
 	status = "okay";
 };
 
 &fec {
 	phy-mode = "rmii";
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_fec>;
+	phy-supply = <&reg_fec_3v3>;
+	phy-reset-gpios = <&gpio4 8 0>;
 	status = "okay";
 };
 
+&i2c1 {
+	clock-frequency = <100000>;
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_i2c1>;
+	status = "okay";
+
+	codec: sgtl5000@0a {
+		compatible = "fsl,sgtl5000";
+		reg = <0x0a>;
+		clocks = <&clks 129>;
+		VDDA-supply = <&reg_2p5v>;
+		VDDIO-supply = <&reg_3p3v>;
+	};
+};
+
+&iomuxc {
+	imx25-pdk {
+		pinctrl_audmux: audmuxgrp {
+			fsl,pins = <
+				MX25_PAD_RW__AUD4_TXFS			0xe0
+				MX25_PAD_OE__AUD4_TXC			0xe0
+				MX25_PAD_EB0__AUD4_TXD			0xe0
+				MX25_PAD_EB1__AUD4_RXD			0xe0
+			>;
+		};
+
+		pinctrl_can1: can1grp {
+			fsl,pins = <
+				MX25_PAD_GPIO_A__CAN1_TX		0x0
+				MX25_PAD_GPIO_B__CAN1_RX		0x0
+				MX25_PAD_D14__GPIO_4_6 			0x80000000
+			>;
+		};
+
+		pinctrl_esdhc1: esdhc1grp {
+			fsl,pins = <
+				MX25_PAD_SD1_CMD__SD1_CMD		0x80000000
+				MX25_PAD_SD1_CLK__SD1_CLK		0x80000000
+				MX25_PAD_SD1_DATA0__SD1_DATA0		0x80000000
+				MX25_PAD_SD1_DATA1__SD1_DATA1		0x80000000
+				MX25_PAD_SD1_DATA2__SD1_DATA2		0x80000000
+				MX25_PAD_SD1_DATA3__SD1_DATA3		0x80000000
+				MX25_PAD_A14__GPIO_2_0			0x80000000
+				MX25_PAD_A15__GPIO_2_1			0x80000000
+			>;
+		};
+
+		pinctrl_fec: fecgrp {
+			fsl,pins = <
+				MX25_PAD_FEC_MDC__FEC_MDC		0x80000000
+				MX25_PAD_FEC_MDIO__FEC_MDIO		0x400001e0
+				MX25_PAD_FEC_TDATA0__FEC_TDATA0		0x80000000
+				MX25_PAD_FEC_TDATA1__FEC_TDATA1		0x80000000
+				MX25_PAD_FEC_TX_EN__FEC_TX_EN		0x80000000
+				MX25_PAD_FEC_RDATA0__FEC_RDATA0		0x80000000
+				MX25_PAD_FEC_RDATA1__FEC_RDATA1		0x80000000
+				MX25_PAD_FEC_RX_DV__FEC_RX_DV		0x80000000
+				MX25_PAD_FEC_TX_CLK__FEC_TX_CLK		0x1c0
+				MX25_PAD_A17__GPIO_2_3			0x80000000
+				MX25_PAD_D12__GPIO_4_8			0x80000000
+			>;
+		};
+
+		pinctrl_i2c1: i2c1grp {
+			fsl,pins = <
+				MX25_PAD_I2C1_CLK__I2C1_CLK		0x80000000
+				MX25_PAD_I2C1_DAT__I2C1_DAT		0x80000000
+			>;
+		};
+
+		pinctrl_kpp: kppgrp {
+			fsl,pins = <
+				MX25_PAD_KPP_ROW0__KPP_ROW0	0x80000000
+				MX25_PAD_KPP_ROW1__KPP_ROW1	0x80000000
+				MX25_PAD_KPP_ROW2__KPP_ROW2	0x80000000
+				MX25_PAD_KPP_ROW3__KPP_ROW3	0x80000000
+				MX25_PAD_KPP_COL0__KPP_COL0	0x80000000
+				MX25_PAD_KPP_COL1__KPP_COL1	0x80000000
+				MX25_PAD_KPP_COL2__KPP_COL2	0x80000000
+				MX25_PAD_KPP_COL3__KPP_COL3	0x80000000
+			>;
+		};
+
+
+		pinctrl_uart1: uart1grp {
+			fsl,pins = <
+				MX25_PAD_UART1_RTS__UART1_RTS		0xe0
+				MX25_PAD_UART1_CTS__UART1_CTS		0xe0
+				MX25_PAD_UART1_TXD__UART1_TXD		0x80000000
+				MX25_PAD_UART1_RXD__UART1_RXD		0xc0
+			>;
+		};
+	};
+};
+
 &nfc {
 	nand-on-flash-bbt;
 	status = "okay";
 };
+
+&kpp {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_kpp>;
+	linux,keymap = <
+			MATRIX_KEY(0x0, 0x0, KEY_UP)
+			MATRIX_KEY(0x0, 0x1, KEY_DOWN)
+			MATRIX_KEY(0x0, 0x2, KEY_VOLUMEDOWN)
+			MATRIX_KEY(0x0, 0x3, KEY_HOME)
+			MATRIX_KEY(0x1, 0x0, KEY_RIGHT)
+			MATRIX_KEY(0x1, 0x1, KEY_LEFT)
+			MATRIX_KEY(0x1, 0x2, KEY_ENTER)
+			MATRIX_KEY(0x1, 0x3, KEY_VOLUMEUP)
+			MATRIX_KEY(0x2, 0x0, KEY_F6)
+			MATRIX_KEY(0x2, 0x1, KEY_F8)
+			MATRIX_KEY(0x2, 0x2, KEY_F9)
+			MATRIX_KEY(0x2, 0x3, KEY_F10)
+			MATRIX_KEY(0x3, 0x0, KEY_F1)
+			MATRIX_KEY(0x3, 0x1, KEY_F2)
+			MATRIX_KEY(0x3, 0x2, KEY_F3)
+			MATRIX_KEY(0x3, 0x2, KEY_POWER)
+	>;
+	status = "okay";
+};
+
+&ssi1 {
+	codec-handle = <&codec>;
+	fsl,mode = "i2s-slave";
+	status = "okay";
+};
+
+&uart1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_uart1>;
+	fsl,uart-has-rtscts;
+	status = "okay";
+};
+
+&usbhost1 {
+	phy_type = "serial";
+	dr_mode = "host";
+	status = "okay";
+};
diff --git a/arch/arm/boot/dts/imx25.dtsi b/arch/arm/boot/dts/imx25.dtsi
index 32f760e2489..bb74d9582b7 100644
--- a/arch/arm/boot/dts/imx25.dtsi
+++ b/arch/arm/boot/dts/imx25.dtsi
@@ -14,6 +14,7 @@
 
 / {
 	aliases {
+		ethernet0 = &fec;
 		gpio0 = &gpio1;
 		gpio1 = &gpio2;
 		gpio2 = &gpio3;
@@ -21,6 +22,8 @@
 		i2c0 = &i2c1;
 		i2c1 = &i2c2;
 		i2c2 = &i2c3;
+		mmc0 = &esdhc1;
+		mmc1 = &esdhc2;
 		serial0 = &uart1;
 		serial1 = &uart2;
 		serial2 = &uart3;
@@ -56,6 +59,7 @@
 
 		osc {
 			compatible = "fsl,imx-osc", "fixed-clock";
+			#clock-cells = <0>;
 			clock-frequency = <24000000>;
 		};
 	};
@@ -164,9 +168,10 @@
 				status = "disabled";
 			};
 
-			kpp@43fa8000 {
+			kpp: kpp@43fa8000 {
 				#address-cells = <1>;
 				#size-cells = <0>;
+				compatible = "fsl,imx25-kpp", "fsl,imx21-kpp";
 				reg = <0x43fa8000 0x4000>;
 				clocks = <&clks 102>;
 				clock-names = "";
@@ -481,23 +486,13 @@
 				clocks = <&clks 99>;
 			};
 
-			usbphy1: usbphy@1 {
-				compatible = "nop-usbphy";
-				status = "disabled";
-			};
-
-			usbphy2: usbphy@2 {
-				compatible = "nop-usbphy";
-				status = "disabled";
-			};
-
 			usbotg: usb@53ff4000 {
 				compatible = "fsl,imx25-usb", "fsl,imx27-usb";
 				reg = <0x53ff4000 0x0200>;
 				interrupts = <37>;
-				clocks = <&clks 9>, <&clks 70>, <&clks 8>;
-				clock-names = "ipg", "ahb", "per";
+				clocks = <&clks 70>;
 				fsl,usbmisc = <&usbmisc 0>;
+				fsl,usbphy = <&usbphy0>;
 				status = "disabled";
 			};
 
@@ -505,9 +500,9 @@
 				compatible = "fsl,imx25-usb", "fsl,imx27-usb";
 				reg = <0x53ff4400 0x0200>;
 				interrupts = <35>;
-				clocks = <&clks 9>, <&clks 70>, <&clks 8>;
-				clock-names = "ipg", "ahb", "per";
+				clocks = <&clks 70>;
 				fsl,usbmisc = <&usbmisc 1>;
+				fsl,usbphy = <&usbphy1>;
 				status = "disabled";
 			};
 
@@ -517,7 +512,6 @@
 				clocks = <&clks 9>, <&clks 70>, <&clks 8>;
 				clock-names = "ipg", "ahb", "per";
 				reg = <0x53ff4600 0x00f>;
-				status = "disabled";
 			};
 
 			dryice@53ffc000 {
@@ -529,6 +523,11 @@
 			};
 		};
 
+		iram: sram@78000000 {
+			compatible = "mmio-sram";
+			reg = <0x78000000 0x20000>;
+		};
+
 		emi@80000000 {
 			compatible = "fsl,emi-bus", "simple-bus";
 			#address-cells = <1>;
@@ -549,4 +548,20 @@
 			};
 		};
 	};
+
+	usbphy {
+		compatible = "simple-bus";
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		usbphy0: usb-phy@0 {
+			reg = <0>;
+			compatible = "usb-nop-xceiv";
+		};
+
+		usbphy1: usb-phy@1 {
+			reg = <1>;
+			compatible = "usb-nop-xceiv";
+		};
+	};
 };
diff --git a/arch/arm/boot/dts/imx27-apf27.dts b/arch/arm/boot/dts/imx27-apf27.dts
index 09f57b39e3e..73aae4f5e53 100644
--- a/arch/arm/boot/dts/imx27-apf27.dts
+++ b/arch/arm/boot/dts/imx27-apf27.dts
@@ -29,6 +29,7 @@
 
 		osc26m {
 			compatible = "fsl,imx-osc26m", "fixed-clock";
+			#clock-cells = <0>;
 			clock-frequency = <0>;
 		};
 	};
diff --git a/arch/arm/boot/dts/imx27-pdk.dts b/arch/arm/boot/dts/imx27-pdk.dts
index 5ce89aa275d..4c317716b51 100644
--- a/arch/arm/boot/dts/imx27-pdk.dts
+++ b/arch/arm/boot/dts/imx27-pdk.dts
@@ -17,15 +17,181 @@
 	compatible = "fsl,imx27-pdk", "fsl,imx27";
 
 	memory {
-		reg = <0x0 0x0>;
+		reg = <0xa0000000 0x08000000>;
 	};
+
+	usbphy {
+		compatible = "simple-bus";
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		usbphy0: usbphy@0 {
+			compatible = "usb-nop-xceiv";
+			reg = <0>;
+			clocks = <&clks 0>;
+			clock-names = "main_clk";
+		};
+	};
+};
+
+&cspi2 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_cspi2>;
+	fsl,spi-num-chipselects = <1>;
+	cs-gpios = <&gpio4 21 GPIO_ACTIVE_HIGH>;
+	status = "okay";
+
+	pmic: mc13783@0 {
+		compatible = "fsl,mc13783";
+		reg = <0>;
+		spi-cs-high;
+		spi-max-frequency = <1000000>;
+		interrupt-parent = <&gpio3>;
+		interrupts = <14 IRQ_TYPE_LEVEL_HIGH>;
+
+		regulators {
+			vgen_reg: vgen {
+				regulator-min-microvolt = <1500000>;
+				regulator-max-microvolt = <1500000>;
+				regulator-always-on;
+				regulator-boot-on;
+			};
+
+			vmmc1_reg: vmmc1 {
+				regulator-min-microvolt = <1600000>;
+				regulator-max-microvolt = <3000000>;
+			};
+
+			gpo1_reg: gpo1 {
+				regulator-always-on;
+				regulator-boot-on;
+			};
+
+			gpo3_reg: gpo3 {
+				regulator-always-on;
+				regulator-boot-on;
+			};
+		};
+	};
+};
+
+&fec {
+	phy-mode = "mii";
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_fec>;
+	status = "okay";
+};
+
+&kpp {
+	linux,keymap = <
+		MATRIX_KEY(0, 0, KEY_UP)
+		MATRIX_KEY(0, 1, KEY_DOWN)
+		MATRIX_KEY(1, 0, KEY_RIGHT)
+		MATRIX_KEY(1, 1, KEY_LEFT)
+		MATRIX_KEY(1, 2, KEY_ENTER)
+		MATRIX_KEY(2, 0, KEY_F6)
+		MATRIX_KEY(2, 1, KEY_F8)
+		MATRIX_KEY(2, 2, KEY_F9)
+		MATRIX_KEY(2, 3, KEY_F10)
+	>;
+	status = "okay";
+};
+
+&nfc {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_nand>;
+	nand-ecc-mode = "hw";
+	nand-on-flash-bbt;
+	status = "okay";
 };
 
 &uart1 {
 	fsl,uart-has-rtscts;
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_uart1>;
 	status = "okay";
 };
 
-&fec {
+&usbotg {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_usbotg>;
+	dr_mode = "otg";
+	fsl,usbphy = <&usbphy0>;
+	phy_type = "ulpi";
 	status = "okay";
 };
+
+&iomuxc {
+	imx27-pdk {
+		pinctrl_cspi2: cspi2grp {
+			fsl,pins = <
+				MX27_PAD_CSPI2_MISO__CSPI2_MISO 0x0
+				MX27_PAD_CSPI2_MOSI__CSPI2_MOSI 0x0
+				MX27_PAD_CSPI2_SCLK__CSPI2_SCLK 0x0
+				MX27_PAD_CSPI2_SS0__GPIO4_21	0x0 /* SPI2 CS0 */
+				MX27_PAD_TOUT__GPIO3_14		0x0 /* PMIC IRQ */
+			>;
+		};
+
+		pinctrl_fec: fecgrp {
+			fsl,pins = <
+				MX27_PAD_SD3_CMD__FEC_TXD0 0x0
+				MX27_PAD_SD3_CLK__FEC_TXD1 0x0
+				MX27_PAD_ATA_DATA0__FEC_TXD2 0x0
+				MX27_PAD_ATA_DATA1__FEC_TXD3 0x0
+				MX27_PAD_ATA_DATA2__FEC_RX_ER 0x0
+				MX27_PAD_ATA_DATA3__FEC_RXD1 0x0
+				MX27_PAD_ATA_DATA4__FEC_RXD2 0x0
+				MX27_PAD_ATA_DATA5__FEC_RXD3 0x0
+				MX27_PAD_ATA_DATA6__FEC_MDIO 0x0
+				MX27_PAD_ATA_DATA7__FEC_MDC 0x0
+				MX27_PAD_ATA_DATA8__FEC_CRS 0x0
+				MX27_PAD_ATA_DATA9__FEC_TX_CLK 0x0
+				MX27_PAD_ATA_DATA10__FEC_RXD0 0x0
+				MX27_PAD_ATA_DATA11__FEC_RX_DV 0x0
+				MX27_PAD_ATA_DATA12__FEC_RX_CLK 0x0
+				MX27_PAD_ATA_DATA13__FEC_COL 0x0
+				MX27_PAD_ATA_DATA14__FEC_TX_ER 0x0
+				MX27_PAD_ATA_DATA15__FEC_TX_EN 0x0
+			>;
+		};
+
+		pinctrl_nand: nandgrp {
+			fsl,pins = <
+				MX27_PAD_NFRB__NFRB	0x0
+				MX27_PAD_NFCLE__NFCLE	0x0
+				MX27_PAD_NFWP_B__NFWP_B	0x0
+				MX27_PAD_NFCE_B__NFCE_B	0x0
+				MX27_PAD_NFALE__NFALE	0x0
+				MX27_PAD_NFRE_B__NFRE_B	0x0
+				MX27_PAD_NFWE_B__NFWE_B	0x0
+			>;
+		};
+
+		pinctrl_uart1: uart1grp {
+			fsl,pins = <
+				MX27_PAD_UART1_TXD__UART1_TXD 0x0
+				MX27_PAD_UART1_RXD__UART1_RXD 0x0
+				MX27_PAD_UART1_CTS__UART1_CTS 0x0
+				MX27_PAD_UART1_RTS__UART1_RTS 0x0
+			>;
+		};
+
+		pinctrl_usbotg: usbotggrp {
+			fsl,pins = <
+				MX27_PAD_USBOTG_NXT__USBOTG_NXT 0x0
+				MX27_PAD_USBOTG_STP__USBOTG_STP 0x0
+				MX27_PAD_USBOTG_DIR__USBOTG_DIR 0x0
+				MX27_PAD_USBOTG_CLK__USBOTG_CLK 0x0
+				MX27_PAD_USBOTG_DATA0__USBOTG_DATA0 0x0
+				MX27_PAD_USBOTG_DATA1__USBOTG_DATA1 0x0
+				MX27_PAD_USBOTG_DATA2__USBOTG_DATA2 0x0
+				MX27_PAD_USBOTG_DATA3__USBOTG_DATA3 0x0
+				MX27_PAD_USBOTG_DATA4__USBOTG_DATA4 0x0
+				MX27_PAD_USBOTG_DATA5__USBOTG_DATA5 0x0
+				MX27_PAD_USBOTG_DATA6__USBOTG_DATA6 0x0
+				MX27_PAD_USBOTG_DATA7__USBOTG_DATA7 0x0
+			>;
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/imx27-phytec-phycard-s-rdk.dts b/arch/arm/boot/dts/imx27-phytec-phycard-s-rdk.dts
index 3c3964a9963..7c869fe3c30 100644
--- a/arch/arm/boot/dts/imx27-phytec-phycard-s-rdk.dts
+++ b/arch/arm/boot/dts/imx27-phytec-phycard-s-rdk.dts
@@ -15,6 +15,10 @@
 	model = "Phytec pca100 rapid development kit";
 	compatible = "phytec,imx27-pca100-rdk", "phytec,imx27-pca100", "fsl,imx27";
 
+	chosen {
+		stdout-path = &uart1;
+	};
+
 	display: display {
 		model = "Primeview-PD050VL1";
 		native-mode = <&timing0>;
diff --git a/arch/arm/boot/dts/imx27-phytec-phycore-rdk.dts b/arch/arm/boot/dts/imx27-phytec-phycore-rdk.dts
index df3b2e73183..fe02bc7a24f 100644
--- a/arch/arm/boot/dts/imx27-phytec-phycore-rdk.dts
+++ b/arch/arm/boot/dts/imx27-phytec-phycore-rdk.dts
@@ -12,14 +12,79 @@
 / {
 	model = "Phytec pcm970";
 	compatible = "phytec,imx27-pcm970", "phytec,imx27-pcm038", "fsl,imx27";
+
+	chosen {
+		stdout-path = &uart1;
+	};
+
+	display0: LQ035Q7 {
+		model = "Sharp-LQ035Q7";
+		native-mode = <&timing0>;
+		bits-per-pixel = <16>;
+		fsl,pcr = <0xf00080c0>;
+
+		display-timings {
+			timing0: 240x320 {
+				clock-frequency = <5500000>;
+				hactive = <240>;
+				vactive = <320>;
+				hback-porch = <5>;
+				hsync-len = <7>;
+				hfront-porch = <16>;
+				vback-porch = <7>;
+				vsync-len = <1>;
+				vfront-porch = <9>;
+				pixelclk-active = <1>;
+				hsync-active = <1>;
+				vsync-active = <1>;
+				de-active = <0>;
+			};
+		};
+	};
+
+	regulators {
+		regulator@2 {
+			compatible = "regulator-fixed";
+			pinctrl-names = "default";
+			pinctrl-0 = <&pinctrl_csien>;
+			reg = <2>;
+			regulator-name = "CSI_EN";
+			regulator-min-microvolt = <3300000>;
+			regulator-max-microvolt = <3300000>;
+			gpio = <&gpio2 24 GPIO_ACTIVE_LOW>;
+			regulator-always-on;
+		};
+	};
+
+	usbphy {
+		usbphy2: usbphy@2 {
+			compatible = "usb-nop-xceiv";
+			reg = <2>;
+			vcc-supply = <&reg_5v0>;
+			clocks = <&clks 0>;
+			clock-names = "main_clk";
+		};
+	};
 };
 
 &cspi1 {
+	pinctrl-0 = <&pinctrl_cspi1>, <&pinctrl_cspi1cs1>;
 	fsl,spi-num-chipselects = <2>;
 	cs-gpios = <&gpio4 28 GPIO_ACTIVE_HIGH>,
 		   <&gpio4 27 GPIO_ACTIVE_LOW>;
 };
 
+&fb {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_imxfb1>;
+	display = <&display0>;
+	lcd-supply = <&reg_5v0>;
+	fsl,dmacr = <0x00020010>;
+	fsl,lscr1 = <0x00120300>;
+	fsl,lpccr = <0x00a903ff>;
+	status = "okay";
+};
+
 &i2c1 {
 	clock-frequency = <400000>;
 	pinctrl-names = "default";
@@ -36,6 +101,50 @@
 
 &iomuxc {
 	imx27_phycore_rdk {
+		pinctrl_csien: csiengrp {
+			fsl,pins = <
+				MX27_PAD_USB_OC_B__GPIO2_24 0x0
+			>;
+		};
+
+		pinctrl_cspi1cs1: cspi1cs1grp {
+			fsl,pins = <
+				MX27_PAD_CSPI1_SS1__GPIO4_27 0x0
+			>;
+		};
+
+		pinctrl_imxfb1: imxfbgrp {
+			fsl,pins = <
+				MX27_PAD_LD0__LD0 0x0
+				MX27_PAD_LD1__LD1 0x0
+				MX27_PAD_LD2__LD2 0x0
+				MX27_PAD_LD3__LD3 0x0
+				MX27_PAD_LD4__LD4 0x0
+				MX27_PAD_LD5__LD5 0x0
+				MX27_PAD_LD6__LD6 0x0
+				MX27_PAD_LD7__LD7 0x0
+				MX27_PAD_LD8__LD8 0x0
+				MX27_PAD_LD9__LD9 0x0
+				MX27_PAD_LD10__LD10 0x0
+				MX27_PAD_LD11__LD11 0x0
+				MX27_PAD_LD12__LD12 0x0
+				MX27_PAD_LD13__LD13 0x0
+				MX27_PAD_LD14__LD14 0x0
+				MX27_PAD_LD15__LD15 0x0
+				MX27_PAD_LD16__LD16 0x0
+				MX27_PAD_LD17__LD17 0x0
+				MX27_PAD_CLS__CLS 0x0
+				MX27_PAD_CONTRAST__CONTRAST 0x0
+				MX27_PAD_LSCLK__LSCLK 0x0
+				MX27_PAD_OE_ACD__OE_ACD 0x0
+				MX27_PAD_PS__PS 0x0
+				MX27_PAD_REV__REV 0x0
+				MX27_PAD_SPL_SPR__SPL_SPR 0x0
+				MX27_PAD_HSYNC__HSYNC 0x0
+				MX27_PAD_VSYNC__VSYNC 0x0
+			>;
+		};
+
 		pinctrl_i2c1: i2c1grp {
 			/* Add pullup to DATA line */
 			fsl,pins = <
@@ -193,19 +302,16 @@
 	dr_mode = "host";
 	phy_type = "ulpi";
 	vbus-supply = <&reg_5v0>;
+	fsl,usbphy = <&usbphy2>;
 	disable-over-current;
 	status = "okay";
 };
 
-&usbphy2 {
-	vcc-supply = <&reg_5v0>;
-};
-
 &weim {
 	pinctrl-names = "default";
 	pinctrl-0 = <&pinctrl_weim>;
 
-	can@d4000000 {
+	can@4,0 {
 		compatible = "nxp,sja1000";
 		reg = <4 0x00000000 0x00000100>;
 		interrupt-parent = <&gpio5>;
diff --git a/arch/arm/boot/dts/imx27-phytec-phycore-som.dtsi b/arch/arm/boot/dts/imx27-phytec-phycore-som.dtsi
index cefaa699462..31e9f7049f7 100644
--- a/arch/arm/boot/dts/imx27-phytec-phycore-som.dtsi
+++ b/arch/arm/boot/dts/imx27-phytec-phycore-som.dtsi
@@ -41,6 +41,20 @@
 			regulator-max-microvolt = <5000000>;
 		};
 	};
+
+	usbphy {
+		compatible = "simple-bus";
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		usbphy0: usbphy@0 {
+			compatible = "usb-nop-xceiv";
+			reg = <0>;
+			vcc-supply = <&sw3_reg>;
+			clocks = <&clks 0>;
+			clock-names = "main_clk";
+		};
+	};
 };
 
 &audmux {
@@ -66,9 +80,9 @@
 	status = "okay";
 
 	pmic: mc13783@0 {
-		#address-cells = <1>;
-		#size-cells = <0>;
 		compatible = "fsl,mc13783";
+		pinctrl-names = "default";
+		pinctrl-0 = <&pinctrl_pmic>;
 		reg = <0>;
 		spi-cs-high;
 		spi-max-frequency = <20000000>;
@@ -166,7 +180,7 @@
 
 &fec {
 	phy-mode = "mii";
-	phy-reset-gpios = <&gpio3 30 GPIO_ACTIVE_HIGH>;
+	phy-reset-gpios = <&gpio3 30 GPIO_ACTIVE_LOW>;
 	phy-supply = <&reg_3v3>;
 	pinctrl-names = "default";
 	pinctrl-0 = <&pinctrl_fec1>;
@@ -204,7 +218,6 @@
 				MX27_PAD_CSPI1_MOSI__CSPI1_MOSI 0x0
 				MX27_PAD_CSPI1_SCLK__CSPI1_SCLK 0x0
 				MX27_PAD_CSPI1_SS0__GPIO4_28	0x0 /* SPI1 CS0 */
-				MX27_PAD_USB_PWR__GPIO2_23	0x0 /* PMIC IRQ */
 			>;
 		};
 
@@ -251,6 +264,21 @@
 			>;
 		};
 
+		pinctrl_pmic: pmicgrp {
+			fsl,pins = <
+				MX27_PAD_USB_PWR__GPIO2_23	0x0 /* PMIC IRQ */
+			>;
+		};
+
+		pinctrl_ssi1: ssi1grp {
+			fsl,pins = <
+				MX27_PAD_SSI1_FS__SSI1_FS 0x0
+				MX27_PAD_SSI1_RXDAT__SSI1_RXDAT 0x0
+				MX27_PAD_SSI1_TXDAT__SSI1_TXDAT 0x0
+				MX27_PAD_SSI1_CLK__SSI1_CLK 0x0
+			>;
+		};
+
 		pinctrl_usbotg: usbotggrp {
 			fsl,pins = <
 				MX27_PAD_USBOTG_CLK__USBOTG_CLK 0x0
@@ -279,23 +307,28 @@
 	status = "okay";
 };
 
+&ssi1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_ssi1>;
+	fsl,mode = "i2s-slave";
+	status = "okay";
+};
+
 &usbotg {
 	pinctrl-names = "default";
 	pinctrl-0 = <&pinctrl_usbotg>;
 	dr_mode = "otg";
 	phy_type = "ulpi";
+	fsl,usbphy = <&usbphy0>;
 	vbus-supply = <&sw3_reg>;
+	disable-over-current;
 	status = "okay";
 };
 
-&usbphy0 {
-	vcc-supply = <&sw3_reg>;
-};
-
 &weim {
 	status = "okay";
 
-	nor: nor@c0000000 {
+	nor: nor@0,0 {
 		compatible = "cfi-flash";
 		reg = <0 0x00000000 0x02000000>;
 		bank-width = <2>;
@@ -305,7 +338,7 @@
 		#size-cells = <1>;
 	};
 
-	sram: sram@c8000000 {
+	sram: sram@1,0 {
 		compatible = "mtd-ram";
 		reg = <1 0x00000000 0x00800000>;
 		bank-width = <2>;
diff --git a/arch/arm/boot/dts/imx27.dtsi b/arch/arm/boot/dts/imx27.dtsi
index 6279e0b4f76..a75555c3953 100644
--- a/arch/arm/boot/dts/imx27.dtsi
+++ b/arch/arm/boot/dts/imx27.dtsi
@@ -11,11 +11,13 @@
 
 #include "skeleton.dtsi"
 #include "imx27-pinfunc.h"
+#include <dt-bindings/input/input.h>
 #include <dt-bindings/interrupt-controller/irq.h>
 #include <dt-bindings/gpio/gpio.h>
 
 / {
 	aliases {
+		ethernet0 = &fec;
 		gpio0 = &gpio1;
 		gpio1 = &gpio2;
 		gpio2 = &gpio3;
@@ -48,6 +50,7 @@
 
 		osc26m {
 			compatible = "fsl,imx-osc26m", "fixed-clock";
+			#clock-cells = <0>;
 			clock-frequency = <26000000>;
 		};
 	};
@@ -70,26 +73,6 @@
 		};
 	};
 
-	usbphy {
-		compatible = "simple-bus";
-		#address-cells = <1>;
-		#size-cells = <0>;
-
-		usbphy0: usbphy@0 {
-			compatible = "usb-nop-xceiv";
-			reg = <0>;
-			clocks = <&clks 75>;
-			clock-names = "main_clk";
-		};
-
-		usbphy2: usbphy@2 {
-			compatible = "usb-nop-xceiv";
-			reg = <2>;
-			clocks = <&clks 75>;
-			clock-names = "main_clk";
-		};
-	};
-
 	soc {
 		#address-cells = <1>;
 		#size-cells = <1>;
@@ -463,9 +446,8 @@
 				compatible = "fsl,imx27-usb";
 				reg = <0x10024000 0x200>;
 				interrupts = <56>;
-				clocks = <&clks 15>;
+				clocks = <&clks 75>;
 				fsl,usbmisc = <&usbmisc 0>;
-				fsl,usbphy = <&usbphy0>;
 				status = "disabled";
 			};
 
@@ -473,7 +455,7 @@
 				compatible = "fsl,imx27-usb";
 				reg = <0x10024200 0x200>;
 				interrupts = <54>;
-				clocks = <&clks 15>;
+				clocks = <&clks 75>;
 				fsl,usbmisc = <&usbmisc 1>;
 				status = "disabled";
 			};
@@ -482,9 +464,8 @@
 				compatible = "fsl,imx27-usb";
 				reg = <0x10024400 0x200>;
 				interrupts = <55>;
-				clocks = <&clks 15>;
+				clocks = <&clks 75>;
 				fsl,usbmisc = <&usbmisc 2>;
-				fsl,usbphy = <&usbphy2>;
 				status = "disabled";
 			};
 
diff --git a/arch/arm/boot/dts/imx28-duckbill.dts b/arch/arm/boot/dts/imx28-duckbill.dts
index 5f326c1c185..ce1a7effba3 100644
--- a/arch/arm/boot/dts/imx28-duckbill.dts
+++ b/arch/arm/boot/dts/imx28-duckbill.dts
@@ -25,9 +25,9 @@
 			ssp0: ssp@80010000 {
 				compatible = "fsl,imx28-mmc";
 				pinctrl-names = "default";
-				pinctrl-0 = <&mmc0_8bit_pins_a
+				pinctrl-0 = <&mmc0_4bit_pins_a
 					&mmc0_cd_cfg &mmc0_sck_cfg>;
-				bus-width = <8>;
+				bus-width = <4>;
 				vmmc-supply = <&reg_3p3v>;
 				status = "okay";
 			};
@@ -39,7 +39,7 @@
 				hog_pins_a: hog@0 {
 					reg = <0>;
 					fsl,pinmux-ids = <
-						MX28_PAD_ENET0_RX_CLK__GPIO_4_13 /* PHY Reset */
+						MX28_PAD_SSP0_DATA7__GPIO_2_7 /* PHY Reset */
 					>;
 					fsl,drive-strength = <MXS_DRIVE_4mA>;
 					fsl,voltage = <MXS_VOLTAGE_HIGH>;
@@ -82,7 +82,7 @@
 			pinctrl-names = "default";
 			pinctrl-0 = <&mac0_pins_a>;
 			phy-supply = <&reg_3p3v>;
-			phy-reset-gpios = <&gpio4 13 0>;
+			phy-reset-gpios = <&gpio2 7 GPIO_ACTIVE_LOW>;
 			phy-reset-duration = <100>;
 			status = "okay";
 		};
@@ -110,12 +110,12 @@
 
 		status {
 			label = "duckbill:green:status";
-			gpios = <&gpio3 5 0>;
+			gpios = <&gpio3 5 GPIO_ACTIVE_HIGH>;
 		};
 
 		failure {
 			label = "duckbill:red:status";
-			gpios = <&gpio3 4 0>;
+			gpios = <&gpio3 4 GPIO_ACTIVE_HIGH>;
 		};
 	};
 };
diff --git a/arch/arm/boot/dts/imx28.dtsi b/arch/arm/boot/dts/imx28.dtsi
index 90a579532b8..a95cc5358ff 100644
--- a/arch/arm/boot/dts/imx28.dtsi
+++ b/arch/arm/boot/dts/imx28.dtsi
@@ -9,6 +9,7 @@
  * http://www.gnu.org/copyleft/gpl.html
  */
 
+#include <dt-bindings/gpio/gpio.h>
 #include "skeleton.dtsi"
 #include "imx28-pinfunc.h"
 
diff --git a/arch/arm/boot/dts/imx35-eukrea-cpuimx35.dtsi b/arch/arm/boot/dts/imx35-eukrea-cpuimx35.dtsi
index 906ae937b01..9c2b715ab8b 100644
--- a/arch/arm/boot/dts/imx35-eukrea-cpuimx35.dtsi
+++ b/arch/arm/boot/dts/imx35-eukrea-cpuimx35.dtsi
@@ -37,6 +37,17 @@
 		compatible = "nxp,pcf8563";
 		reg = <0x51>;
 	};
+
+	tsc2007: tsc2007@48 {
+		compatible = "ti,tsc2007";
+		gpios = <&gpio3 2 0>;
+		interrupt-parent = <&gpio3>;
+		interrupts = <0x2 0x8>;
+		pinctrl-names = "default";
+		pinctrl-0 = <&pinctrl_tsc2007_1>;
+		reg = <0x48>;
+		ti,x-plate-ohms = <180>;
+	};
 };
 
 &iomuxc {
@@ -70,6 +81,10 @@
 				MX35_PAD_I2C1_DAT__I2C1_SDA		0x80000000
 			>;
 		};
+
+		pinctrl_tsc2007_1: tsc2007grp-1 {
+			fsl,pins = <MX35_PAD_ATA_DA2__GPIO3_2 0x80000000>;
+		};
 	};
 };
 
diff --git a/arch/arm/boot/dts/imx35-eukrea-mbimxsd35-baseboard.dts b/arch/arm/boot/dts/imx35-eukrea-mbimxsd35-baseboard.dts
index 1bdec21f453..f04ae91eea8 100644
--- a/arch/arm/boot/dts/imx35-eukrea-mbimxsd35-baseboard.dts
+++ b/arch/arm/boot/dts/imx35-eukrea-mbimxsd35-baseboard.dts
@@ -46,6 +46,14 @@
 			linux,default-trigger = "heartbeat";
 		};
 	};
+
+	sound {
+		compatible = "eukrea,asoc-tlv320";
+		eukrea,model = "imx35-eukrea-tlv320aic23";
+		ssi-controller = <&ssi1>;
+		fsl,mux-int-port = <1>;
+		fsl,mux-ext-port = <4>;
+	};
 };
 
 &audmux {
@@ -124,6 +132,7 @@
 };
 
 &ssi1 {
+	codec-handle = <&tlv320aic23>;
 	fsl,mode = "i2s-slave";
 	status = "okay";
 };
@@ -141,3 +150,16 @@
 	fsl,uart-has-rtscts;
 	status = "okay";
 };
+
+&usbhost1 {
+	phy_type = "serial";
+	dr_mode = "host";
+	status = "okay";
+};
+
+&usbotg {
+	phy_type = "utmi";
+	dr_mode = "otg";
+	external-vbus-divider;
+	status = "okay";
+};
diff --git a/arch/arm/boot/dts/imx35-pdk.dts b/arch/arm/boot/dts/imx35-pdk.dts
new file mode 100644
index 00000000000..8d715523708
--- /dev/null
+++ b/arch/arm/boot/dts/imx35-pdk.dts
@@ -0,0 +1,68 @@
+/*
+ * Copyright 2013 Eukréa Electromatique <denis@eukrea.com>
+ * Copyright 2014 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
+ */
+
+/dts-v1/;
+#include "imx35.dtsi"
+
+/ {
+	model = "Freescale i.MX35 Product Development Kit";
+	compatible = "fsl,imx35-pdk", "fsl,imx35";
+
+	memory {
+		reg = <0x80000000 0x8000000>,
+		      <0x90000000 0x8000000>;
+	};
+};
+
+&esdhc1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_esdhc1>;
+	status = "okay";
+};
+
+&iomuxc {
+	imx35-pdk {
+		pinctrl_esdhc1: esdhc1grp {
+			fsl,pins = <
+				MX35_PAD_SD1_CMD__ESDHC1_CMD		0x80000000
+				MX35_PAD_SD1_CLK__ESDHC1_CLK		0x80000000
+				MX35_PAD_SD1_DATA0__ESDHC1_DAT0		0x80000000
+				MX35_PAD_SD1_DATA1__ESDHC1_DAT1		0x80000000
+				MX35_PAD_SD1_DATA2__ESDHC1_DAT2		0x80000000
+				MX35_PAD_SD1_DATA3__ESDHC1_DAT3		0x80000000
+			>;
+		};
+
+		pinctrl_uart1: uart1grp {
+			fsl,pins = <
+				MX35_PAD_TXD1__UART1_TXD_MUX		0x1c5
+				MX35_PAD_RXD1__UART1_RXD_MUX		0x1c5
+				MX35_PAD_CTS1__UART1_CTS		0x1c5
+				MX35_PAD_RTS1__UART1_RTS		0x1c5
+			>;
+		};
+	};
+};
+
+&nfc {
+	nand-bus-width = <16>;
+	nand-ecc-mode = "hw";
+	nand-on-flash-bbt;
+	status = "okay";
+};
+
+&uart1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_uart1>;
+	fsl,uart-has-rtscts;
+	status = "okay";
+};
diff --git a/arch/arm/boot/dts/imx35.dtsi b/arch/arm/boot/dts/imx35.dtsi
index 88b218f8f81..4759abb4943 100644
--- a/arch/arm/boot/dts/imx35.dtsi
+++ b/arch/arm/boot/dts/imx35.dtsi
@@ -13,6 +13,7 @@
 
 / {
 	aliases {
+		ethernet0 = &fec;
 		gpio0 = &gpio1;
 		gpio1 = &gpio2;
 		gpio2 = &gpio3;
@@ -295,9 +296,9 @@
 				compatible = "fsl,imx35-usb", "fsl,imx27-usb";
 				reg = <0x53ff4000 0x0200>;
 				interrupts = <37>;
-				clocks = <&clks 9>, <&clks 73>, <&clks 28>;
-				clock-names = "ipg", "ahb", "per";
+				clocks = <&clks 73>;
 				fsl,usbmisc = <&usbmisc 0>;
+				fsl,usbphy = <&usbphy0>;
 				status = "disabled";
 			};
 
@@ -305,9 +306,9 @@
 				compatible = "fsl,imx35-usb", "fsl,imx27-usb";
 				reg = <0x53ff4400 0x0200>;
 				interrupts = <35>;
-				clocks = <&clks 9>, <&clks 73>, <&clks 28>;
-				clock-names = "ipg", "ahb", "per";
+				clocks = <&clks 73>;
 				fsl,usbmisc = <&usbmisc 1>;
+				fsl,usbphy = <&usbphy1>;
 				status = "disabled";
 			};
 
@@ -356,4 +357,20 @@
 			};
 		};
 	};
+
+	usbphy {
+		compatible = "simple-bus";
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		usbphy0: usb-phy@0 {
+			reg = <0>;
+			compatible = "usb-nop-xceiv";
+		};
+
+		usbphy1: usb-phy@1 {
+			reg = <1>;
+			compatible = "usb-nop-xceiv";
+		};
+	};
 };
diff --git a/arch/arm/boot/dts/imx50.dtsi b/arch/arm/boot/dts/imx50.dtsi
index 0c75fe3deb3..6a201cf5436 100644
--- a/arch/arm/boot/dts/imx50.dtsi
+++ b/arch/arm/boot/dts/imx50.dtsi
@@ -17,6 +17,7 @@
 
 / {
 	aliases {
+		ethernet0 = &fec;
 		gpio0 = &gpio1;
 		gpio1 = &gpio2;
 		gpio2 = &gpio3;
@@ -53,21 +54,25 @@
 
 		ckil {
 			compatible = "fsl,imx-ckil", "fixed-clock";
+			#clock-cells = <0>;
 			clock-frequency = <32768>;
 		};
 
 		ckih1 {
 			compatible = "fsl,imx-ckih1", "fixed-clock";
+			#clock-cells = <0>;
 			clock-frequency = <22579200>;
 		};
 
 		ckih2 {
 			compatible = "fsl,imx-ckih2", "fixed-clock";
+			#clock-cells = <0>;
 			clock-frequency = <0>;
 		};
 
 		osc {
 			compatible = "fsl,imx-osc", "fixed-clock";
+			#clock-cells = <0>;
 			clock-frequency = <24000000>;
 		};
 	};
diff --git a/arch/arm/boot/dts/imx51-babbage.dts b/arch/arm/boot/dts/imx51-babbage.dts
index 9e9deb244b7..6bc3243a80d 100644
--- a/arch/arm/boot/dts/imx51-babbage.dts
+++ b/arch/arm/boot/dts/imx51-babbage.dts
@@ -17,10 +17,28 @@
 	model = "Freescale i.MX51 Babbage Board";
 	compatible = "fsl,imx51-babbage", "fsl,imx51";
 
+	chosen {
+		stdout-path = &uart1;
+	};
+
 	memory {
 		reg = <0x90000000 0x20000000>;
 	};
 
+	clocks {
+		ckih1 {
+			clock-frequency = <22579200>;
+		};
+
+		clk_26M: codec_clock {
+			compatible = "fixed-clock";
+			reg=<0>;
+			#clock-cells = <0>;
+			clock-frequency = <26000000>;
+			gpios = <&gpio4 26 GPIO_ACTIVE_LOW>;
+		};
+	};
+
 	display0: display@di0 {
 		compatible = "fsl,imx-parallel-display";
 		interface-pix-fmt = "rgb24";
@@ -82,11 +100,13 @@
 
 	gpio-keys {
 		compatible = "gpio-keys";
+		pinctrl-names = "default";
+		pinctrl-0 = <&pinctrl_gpio_keys>;
 
 		power {
 			label = "Power Button";
 			gpios = <&gpio2 21 GPIO_ACTIVE_HIGH>;
-			linux,code = <116>; /* KEY_POWER */
+			linux,code = <KEY_POWER>;
 			gpio-key,wakeup;
 		};
 	};
@@ -102,6 +122,36 @@
 		};
 	};
 
+	regulators {
+		compatible = "simple-bus";
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		reg_usbh1_vbus: regulator@0 {
+			compatible = "regulator-fixed";
+			pinctrl-names = "default";
+			pinctrl-0 = <&pinctrl_usbh1reg>;
+			reg = <0>;
+			regulator-name = "usbh1_vbus";
+			regulator-min-microvolt = <5000000>;
+			regulator-max-microvolt = <5000000>;
+			gpio = <&gpio2 5 GPIO_ACTIVE_HIGH>;
+			enable-active-high;
+		};
+
+		reg_usbotg_vbus: regulator@1 {
+			compatible = "regulator-fixed";
+			pinctrl-names = "default";
+			pinctrl-0 = <&pinctrl_usbotgreg>;
+			reg = <1>;
+			regulator-name = "usbotg_vbus";
+			regulator-min-microvolt = <5000000>;
+			regulator-max-microvolt = <5000000>;
+			gpio = <&gpio1 7 GPIO_ACTIVE_HIGH>;
+			enable-active-high;
+		};
+	};
+
 	sound {
 		compatible = "fsl,imx51-babbage-sgtl5000",
 			     "fsl,imx-audio-sgtl5000";
@@ -116,41 +166,23 @@
 		mux-ext-port = <3>;
 	};
 
-	clocks {
-		ckih1 {
-			clock-frequency = <22579200>;
-		};
+	usbphy {
+		#address-cells = <1>;
+		#size-cells = <0>;
+		compatible = "simple-bus";
 
-		clk_26M: codec_clock {
-			compatible = "fixed-clock";
-			reg=<0>;
-			#clock-cells = <0>;
-			clock-frequency = <26000000>;
-			gpios = <&gpio4 26 GPIO_ACTIVE_LOW>;
+		usbh1phy: usbh1phy@0 {
+			compatible = "usb-nop-xceiv";
+			reg = <0>;
+			clocks = <&clks IMX5_CLK_DUMMY>;
+			clock-names = "main_clk";
 		};
 	};
 };
 
-&esdhc1 {
-	pinctrl-names = "default";
-	pinctrl-0 = <&pinctrl_esdhc1>;
-	fsl,cd-controller;
-	fsl,wp-controller;
-	status = "okay";
-};
-
-&esdhc2 {
-	pinctrl-names = "default";
-	pinctrl-0 = <&pinctrl_esdhc2>;
-	cd-gpios = <&gpio1 6 GPIO_ACTIVE_HIGH>;
-	wp-gpios = <&gpio1 5 GPIO_ACTIVE_HIGH>;
-	status = "okay";
-};
-
-&uart3 {
+&audmux {
 	pinctrl-names = "default";
-	pinctrl-0 = <&pinctrl_uart3>;
-	fsl,uart-has-rtscts;
+	pinctrl-0 = <&pinctrl_audmux>;
 	status = "okay";
 };
 
@@ -163,9 +195,9 @@
 	status = "okay";
 
 	pmic: mc13892@0 {
-		#address-cells = <1>;
-		#size-cells = <0>;
 		compatible = "fsl,mc13892";
+		pinctrl-names = "default";
+		pinctrl-0 = <&pinctrl_pmic>;
 		spi-max-frequency = <6000000>;
 		spi-cs-high;
 		reg = <0>;
@@ -280,6 +312,53 @@
 	};
 };
 
+&esdhc1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_esdhc1>;
+	fsl,cd-controller;
+	fsl,wp-controller;
+	status = "okay";
+};
+
+&esdhc2 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_esdhc2>;
+	cd-gpios = <&gpio1 6 GPIO_ACTIVE_HIGH>;
+	wp-gpios = <&gpio1 5 GPIO_ACTIVE_HIGH>;
+	status = "okay";
+};
+
+&fec {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_fec>;
+	phy-mode = "mii";
+	phy-reset-gpios = <&gpio2 14 GPIO_ACTIVE_LOW>;
+	phy-reset-duration = <1>;
+	status = "okay";
+};
+
+&i2c1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_i2c1>;
+	status = "okay";
+};
+
+&i2c2 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_i2c2>;
+	status = "okay";
+
+	sgtl5000: codec@0a {
+		compatible = "fsl,sgtl5000";
+		pinctrl-names = "default";
+		pinctrl-0 = <&pinctrl_clkcodec>;
+		reg = <0x0a>;
+		clocks = <&clk_26M>;
+		VDDA-supply = <&vdig_reg>;
+		VDDIO-supply = <&vvideo_reg>;
+	};
+};
+
 &ipu_di0_disp0 {
 	remote-endpoint = <&display0_in>;
 };
@@ -288,29 +367,74 @@
 	remote-endpoint = <&display1_in>;
 };
 
+&kpp {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_kpp>;
+	linux,keymap = <
+		MATRIX_KEY(0, 0, KEY_UP)
+		MATRIX_KEY(0, 1, KEY_DOWN)
+		MATRIX_KEY(0, 2, KEY_VOLUMEDOWN)
+		MATRIX_KEY(0, 3, KEY_HOME)
+		MATRIX_KEY(1, 0, KEY_RIGHT)
+		MATRIX_KEY(1, 1, KEY_LEFT)
+		MATRIX_KEY(1, 2, KEY_ENTER)
+		MATRIX_KEY(1, 3, KEY_VOLUMEUP)
+		MATRIX_KEY(2, 0, KEY_F6)
+		MATRIX_KEY(2, 1, KEY_F8)
+		MATRIX_KEY(2, 2, KEY_F9)
+		MATRIX_KEY(2, 3, KEY_F10)
+		MATRIX_KEY(3, 0, KEY_F1)
+		MATRIX_KEY(3, 1, KEY_F2)
+		MATRIX_KEY(3, 2, KEY_F3)
+		MATRIX_KEY(3, 3, KEY_POWER)
+	>;
+	status = "okay";
+};
+
 &ssi2 {
 	fsl,mode = "i2s-slave";
 	status = "okay";
 };
 
-&iomuxc {
+&uart1 {
 	pinctrl-names = "default";
-	pinctrl-0 = <&pinctrl_hog>;
+	pinctrl-0 = <&pinctrl_uart1>;
+	fsl,uart-has-rtscts;
+	status = "okay";
+};
 
-	imx51-babbage {
-		pinctrl_hog: hoggrp {
-			fsl,pins = <
-				MX51_PAD_GPIO1_0__SD1_CD     0x20d5
-				MX51_PAD_GPIO1_1__SD1_WP     0x20d5
-				MX51_PAD_GPIO1_5__GPIO1_5    0x100
-				MX51_PAD_GPIO1_6__GPIO1_6    0x100
-				MX51_PAD_EIM_A27__GPIO2_21   0x5
-				MX51_PAD_CSPI1_SS0__GPIO4_24 0x85
-				MX51_PAD_CSPI1_SS1__GPIO4_25 0x85
-				MX51_PAD_CSPI1_RDY__GPIO4_26 0x80000000
-			>;
-		};
+&uart2 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_uart2>;
+	status = "okay";
+};
+
+&uart3 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_uart3>;
+	fsl,uart-has-rtscts;
+	status = "okay";
+};
+
+&usbh1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_usbh1>;
+	vbus-supply = <&reg_usbh1_vbus>;
+	fsl,usbphy = <&usbh1phy>;
+	phy_type = "ulpi";
+	status = "okay";
+};
 
+&usbotg {
+	dr_mode = "otg";
+	disable-over-current;
+	phy_type = "utmi_wide";
+	vbus-supply = <&reg_usbotg_vbus>;
+	status = "okay";
+};
+
+&iomuxc {
+	imx51-babbage {
 		pinctrl_audmux: audmuxgrp {
 			fsl,pins = <
 				MX51_PAD_AUD3_BB_TXD__AUD3_TXD		0x80000000
@@ -320,11 +444,19 @@
 			>;
 		};
 
+		pinctrl_clkcodec: clkcodecgrp {
+			fsl,pins = <
+				MX51_PAD_CSPI1_RDY__GPIO4_26		0x80000000
+			>;
+		};
+
 		pinctrl_ecspi1: ecspi1grp {
 			fsl,pins = <
 				MX51_PAD_CSPI1_MISO__ECSPI1_MISO	0x185
 				MX51_PAD_CSPI1_MOSI__ECSPI1_MOSI	0x185
 				MX51_PAD_CSPI1_SCLK__ECSPI1_SCLK	0x185
+				MX51_PAD_CSPI1_SS0__GPIO4_24		0x85 /* CS0 */
+				MX51_PAD_CSPI1_SS1__GPIO4_25		0x85 /* CS1 */
 			>;
 		};
 
@@ -336,6 +468,8 @@
 				MX51_PAD_SD1_DATA1__SD1_DATA1		0x20d5
 				MX51_PAD_SD1_DATA2__SD1_DATA2		0x20d5
 				MX51_PAD_SD1_DATA3__SD1_DATA3		0x20d5
+				MX51_PAD_GPIO1_0__SD1_CD		0x20d5
+				MX51_PAD_GPIO1_1__SD1_WP		0x20d5
 			>;
 		};
 
@@ -347,29 +481,38 @@
 				MX51_PAD_SD2_DATA1__SD2_DATA1		0x20d5
 				MX51_PAD_SD2_DATA2__SD2_DATA2		0x20d5
 				MX51_PAD_SD2_DATA3__SD2_DATA3		0x20d5
+				MX51_PAD_GPIO1_5__GPIO1_5		0x100 /* WP */
+				MX51_PAD_GPIO1_6__GPIO1_6		0x100 /* CD */
 			>;
 		};
 
 		pinctrl_fec: fecgrp {
 			fsl,pins = <
-				MX51_PAD_EIM_EB2__FEC_MDIO		0x80000000
-				MX51_PAD_EIM_EB3__FEC_RDATA1		0x80000000
-				MX51_PAD_EIM_CS2__FEC_RDATA2		0x80000000
-				MX51_PAD_EIM_CS3__FEC_RDATA3		0x80000000
-				MX51_PAD_EIM_CS4__FEC_RX_ER		0x80000000
-				MX51_PAD_EIM_CS5__FEC_CRS		0x80000000
-				MX51_PAD_NANDF_RB2__FEC_COL		0x80000000
-				MX51_PAD_NANDF_RB3__FEC_RX_CLK		0x80000000
-				MX51_PAD_NANDF_D9__FEC_RDATA0		0x80000000
-				MX51_PAD_NANDF_D8__FEC_TDATA0		0x80000000
-				MX51_PAD_NANDF_CS2__FEC_TX_ER		0x80000000
-				MX51_PAD_NANDF_CS3__FEC_MDC		0x80000000
-				MX51_PAD_NANDF_CS4__FEC_TDATA1		0x80000000
-				MX51_PAD_NANDF_CS5__FEC_TDATA2		0x80000000
-				MX51_PAD_NANDF_CS6__FEC_TDATA3		0x80000000
-				MX51_PAD_NANDF_CS7__FEC_TX_EN		0x80000000
-				MX51_PAD_NANDF_RDY_INT__FEC_TX_CLK	0x80000000
-				MX51_PAD_EIM_A20__GPIO2_14 0x85 /* Reset */
+				MX51_PAD_EIM_EB2__FEC_MDIO		0x000001f5
+				MX51_PAD_EIM_EB3__FEC_RDATA1		0x00000085
+				MX51_PAD_EIM_CS2__FEC_RDATA2		0x00000085
+				MX51_PAD_EIM_CS3__FEC_RDATA3		0x00000085
+				MX51_PAD_EIM_CS4__FEC_RX_ER		0x00000180
+				MX51_PAD_EIM_CS5__FEC_CRS		0x00000180
+				MX51_PAD_NANDF_RB2__FEC_COL		0x00000180
+				MX51_PAD_NANDF_RB3__FEC_RX_CLK		0x00000180
+				MX51_PAD_NANDF_D9__FEC_RDATA0		0x00002180
+				MX51_PAD_NANDF_D8__FEC_TDATA0		0x00002004
+				MX51_PAD_NANDF_CS2__FEC_TX_ER		0x00002004
+				MX51_PAD_NANDF_CS3__FEC_MDC		0x00002004
+				MX51_PAD_NANDF_CS4__FEC_TDATA1		0x00002004
+				MX51_PAD_NANDF_CS5__FEC_TDATA2		0x00002004
+				MX51_PAD_NANDF_CS6__FEC_TDATA3		0x00002004
+				MX51_PAD_NANDF_CS7__FEC_TX_EN		0x00002004
+				MX51_PAD_NANDF_RDY_INT__FEC_TX_CLK	0x00002180
+				MX51_PAD_NANDF_D11__FEC_RX_DV		0x000020a4
+				MX51_PAD_EIM_A20__GPIO2_14		0x00000085 /* Phy Reset */
+			>;
+		};
+
+		pinctrl_gpio_keys: gpiokeysgrp {
+			fsl,pins = <
+				MX51_PAD_EIM_A27__GPIO2_21		0x5
 			>;
 		};
 
@@ -379,6 +522,13 @@
 			>;
 		};
 
+		pinctrl_i2c1: i2c1grp {
+			fsl,pins = <
+				MX51_PAD_EIM_D19__I2C1_SCL		0x400001ed
+				MX51_PAD_EIM_D16__I2C1_SDA		0x400001ed
+			>;
+		};
+
 		pinctrl_i2c2: i2c2grp {
 			fsl,pins = <
 				MX51_PAD_KEY_COL4__I2C2_SCL		0x400001ed
@@ -455,6 +605,12 @@
 			>;
 		};
 
+		pinctrl_pmic: pmicgrp {
+			fsl,pins = <
+				MX51_PAD_GPIO1_8__GPIO1_8		0xe5 /* IRQ */
+			>;
+		};
+
 		pinctrl_uart1: uart1grp {
 			fsl,pins = <
 				MX51_PAD_UART1_RXD__UART1_RXD		0x1c5
@@ -479,71 +635,33 @@
 				MX51_PAD_EIM_D24__UART3_CTS		0x1c5
 			>;
 		};
-	};
-};
 
-&uart1 {
-	pinctrl-names = "default";
-	pinctrl-0 = <&pinctrl_uart1>;
-	fsl,uart-has-rtscts;
-	status = "okay";
-};
-
-&uart2 {
-	pinctrl-names = "default";
-	pinctrl-0 = <&pinctrl_uart2>;
-	status = "okay";
-};
+		pinctrl_usbh1: usbh1grp {
+			fsl,pins = <
+				MX51_PAD_USBH1_CLK__USBH1_CLK		0x80000000
+				MX51_PAD_USBH1_DIR__USBH1_DIR		0x80000000
+				MX51_PAD_USBH1_NXT__USBH1_NXT		0x80000000
+				MX51_PAD_USBH1_DATA0__USBH1_DATA0	0x80000000
+				MX51_PAD_USBH1_DATA1__USBH1_DATA1	0x80000000
+				MX51_PAD_USBH1_DATA2__USBH1_DATA2	0x80000000
+				MX51_PAD_USBH1_DATA3__USBH1_DATA3	0x80000000
+				MX51_PAD_USBH1_DATA4__USBH1_DATA4	0x80000000
+				MX51_PAD_USBH1_DATA5__USBH1_DATA5	0x80000000
+				MX51_PAD_USBH1_DATA6__USBH1_DATA6	0x80000000
+				MX51_PAD_USBH1_DATA7__USBH1_DATA7	0x80000000
+			>;
+		};
 
-&i2c2 {
-	pinctrl-names = "default";
-	pinctrl-0 = <&pinctrl_i2c2>;
-	status = "okay";
+		pinctrl_usbh1reg: usbh1reggrp {
+			fsl,pins = <
+				MX51_PAD_EIM_D21__GPIO2_5		0x85
+			>;
+		};
 
-	sgtl5000: codec@0a {
-		compatible = "fsl,sgtl5000";
-		reg = <0x0a>;
-		clocks = <&clk_26M>;
-		VDDA-supply = <&vdig_reg>;
-		VDDIO-supply = <&vvideo_reg>;
+		pinctrl_usbotgreg: usbotgreggrp {
+			fsl,pins = <
+				MX51_PAD_GPIO1_7__GPIO1_7		0x85
+			>;
+		};
 	};
 };
-
-&audmux {
-	pinctrl-names = "default";
-	pinctrl-0 = <&pinctrl_audmux>;
-	status = "okay";
-};
-
-&fec {
-	pinctrl-names = "default";
-	pinctrl-0 = <&pinctrl_fec>;
-	phy-mode = "mii";
-	phy-reset-gpios = <&gpio2 14 GPIO_ACTIVE_LOW>;
-	phy-reset-duration = <1>;
-	status = "okay";
-};
-
-&kpp {
-	pinctrl-names = "default";
-	pinctrl-0 = <&pinctrl_kpp>;
-	linux,keymap = <
-		MATRIX_KEY(0, 0, KEY_UP)
-		MATRIX_KEY(0, 1, KEY_DOWN)
-		MATRIX_KEY(0, 2, KEY_VOLUMEDOWN)
-		MATRIX_KEY(0, 3, KEY_HOME)
-		MATRIX_KEY(1, 0, KEY_RIGHT)
-		MATRIX_KEY(1, 1, KEY_LEFT)
-		MATRIX_KEY(1, 2, KEY_ENTER)
-		MATRIX_KEY(1, 3, KEY_VOLUMEUP)
-		MATRIX_KEY(2, 0, KEY_F6)
-		MATRIX_KEY(2, 1, KEY_F8)
-		MATRIX_KEY(2, 2, KEY_F9)
-		MATRIX_KEY(2, 3, KEY_F10)
-		MATRIX_KEY(3, 0, KEY_F1)
-		MATRIX_KEY(3, 1, KEY_F2)
-		MATRIX_KEY(3, 2, KEY_F3)
-		MATRIX_KEY(3, 3, KEY_POWER)
-		>;
-	status = "okay";
-};
diff --git a/arch/arm/boot/dts/imx51-digi-connectcore-jsk.dts b/arch/arm/boot/dts/imx51-digi-connectcore-jsk.dts
new file mode 100644
index 00000000000..1db517d3d49
--- /dev/null
+++ b/arch/arm/boot/dts/imx51-digi-connectcore-jsk.dts
@@ -0,0 +1,108 @@
+/*
+ * Copyright (C) 2014 Alexander Shiyan <shc_work@mail.ru>
+ *
+ * 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 "imx51-digi-connectcore-som.dtsi"
+
+/ {
+	model = "Digi ConnectCore CC(W)-MX51 JSK";
+	compatible = "digi,connectcore-ccxmx51-jsk",
+		     "digi,connectcore-ccxmx51-som", "fsl,imx51";
+
+	chosen {
+		linux,stdout-path = &uart1;
+	};
+};
+
+&owire {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_owire>;
+	status = "okay";
+};
+
+&uart1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_uart1>;
+	status = "okay";
+};
+
+&uart2 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_uart2>;
+	status = "okay";
+};
+
+&uart3 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_uart3>;
+	status = "okay";
+};
+
+&usbotg {
+	dr_mode = "otg";
+	status = "okay";
+};
+
+&usbh1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_usbh1>;
+	dr_mode = "host";
+	phy_type = "ulpi";
+	disable-over-current;
+	status = "okay";
+};
+
+&iomuxc {
+	imx51-digi-connectcore-jsk {
+		pinctrl_owire: owiregrp {
+			fsl,pins = <
+				MX51_PAD_OWIRE_LINE__OWIRE_LINE		0x40000000
+			>;
+		};
+
+		pinctrl_uart1: uart1grp {
+			fsl,pins = <
+				MX51_PAD_UART1_RXD__UART1_RXD		0x1c5
+				MX51_PAD_UART1_TXD__UART1_TXD		0x1c5
+			>;
+		};
+
+		pinctrl_uart2: uart2grp {
+			fsl,pins = <
+				MX51_PAD_UART2_RXD__UART2_RXD		0x1c5
+				MX51_PAD_UART2_TXD__UART2_TXD		0x1c5
+			>;
+		};
+
+		pinctrl_uart3: uart3grp {
+			fsl,pins = <
+				MX51_PAD_UART3_RXD__UART3_RXD		0x1c5
+				MX51_PAD_UART3_TXD__UART3_TXD		0x1c5
+			>;
+		};
+
+		pinctrl_usbh1: usbh1grp {
+			fsl,pins = <
+				MX51_PAD_USBH1_DATA0__USBH1_DATA0	0x1e5
+				MX51_PAD_USBH1_DATA1__USBH1_DATA1	0x1e5
+				MX51_PAD_USBH1_DATA2__USBH1_DATA2	0x1e5
+				MX51_PAD_USBH1_DATA3__USBH1_DATA3	0x1e5
+				MX51_PAD_USBH1_DATA4__USBH1_DATA4	0x1e5
+				MX51_PAD_USBH1_DATA5__USBH1_DATA5	0x1e5
+				MX51_PAD_USBH1_DATA6__USBH1_DATA6	0x1e5
+				MX51_PAD_USBH1_DATA7__USBH1_DATA7	0x1e5
+				MX51_PAD_USBH1_CLK__USBH1_CLK		0x1e5
+				MX51_PAD_USBH1_DIR__USBH1_DIR		0x1e5
+				MX51_PAD_USBH1_NXT__USBH1_NXT		0x1e5
+				MX51_PAD_USBH1_STP__USBH1_STP		0x1e5
+			>;
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/imx51-digi-connectcore-som.dtsi b/arch/arm/boot/dts/imx51-digi-connectcore-som.dtsi
new file mode 100644
index 00000000000..321662f53e3
--- /dev/null
+++ b/arch/arm/boot/dts/imx51-digi-connectcore-som.dtsi
@@ -0,0 +1,377 @@
+/*
+ * Copyright (C) 2014 Alexander Shiyan <shc_work@mail.ru>
+ *
+ * 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
+ */
+
+/dts-v1/;
+#include "imx51.dtsi"
+
+/ {
+	model = "Digi ConnectCore CC(W)-MX51";
+	compatible = "digi,connectcore-ccxmx51-som", "fsl,imx51";
+
+	memory {
+		reg = <0x90000000 0x08000000>;
+	};
+};
+
+&ecspi1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_ecspi1>;
+	fsl,spi-num-chipselects = <1>;
+	cs-gpios = <&gpio4 24 GPIO_ACTIVE_HIGH>;
+	status = "okay";
+
+	pmic: mc13892@0 {
+		pinctrl-names = "default";
+		pinctrl-0 = <&pinctrl_mc13892>;
+		compatible = "fsl,mc13892";
+		spi-max-frequency = <16000000>;
+		spi-cs-high;
+		reg = <0>;
+		interrupt-parent = <&gpio1>;
+		interrupts = <5 IRQ_TYPE_LEVEL_HIGH>;
+		fsl,mc13xxx-uses-rtc;
+
+		regulators {
+			sw1_reg: sw1 {
+				regulator-min-microvolt = <1000000>;
+				regulator-max-microvolt = <1100000>;
+				regulator-boot-on;
+				regulator-always-on;
+			};
+
+			sw2_reg: sw2 {
+				regulator-min-microvolt = <1225000>;
+				regulator-max-microvolt = <1225000>;
+				regulator-boot-on;
+				regulator-always-on;
+			};
+
+			sw3_reg: sw3 {
+				regulator-min-microvolt = <1200000>;
+				regulator-max-microvolt = <1200000>;
+				regulator-boot-on;
+				regulator-always-on;
+			};
+
+			swbst_reg: swbst { };
+
+			viohi_reg: viohi {
+				regulator-always-on;
+			};
+
+			vpll_reg: vpll {
+				regulator-min-microvolt = <1800000>;
+				regulator-max-microvolt = <1800000>;
+				regulator-always-on;
+			};
+
+			vdig_reg: vdig {
+				regulator-min-microvolt = <1250000>;
+				regulator-max-microvolt = <1250000>;
+				regulator-always-on;
+			};
+
+			vsd_reg: vsd {
+				regulator-min-microvolt = <3150000>;
+				regulator-max-microvolt = <3150000>;
+				regulator-always-on;
+			};
+
+			vusb2_reg: vusb2 {
+				regulator-min-microvolt = <2600000>;
+				regulator-max-microvolt = <2600000>;
+				regulator-always-on;
+			};
+
+			vvideo_reg: vvideo {
+				regulator-min-microvolt = <2775000>;
+				regulator-max-microvolt = <2775000>;
+				regulator-always-on;
+			};
+
+			vaudio_reg: vaudio {
+				regulator-min-microvolt = <3000000>;
+				regulator-max-microvolt = <3000000>;
+				regulator-always-on;
+			};
+
+			vcam_reg: vcam {
+				regulator-min-microvolt = <2750000>;
+				regulator-max-microvolt = <2750000>;
+				regulator-always-on;
+			};
+
+			vgen1_reg: vgen1 {
+				regulator-min-microvolt = <1200000>;
+				regulator-max-microvolt = <1200000>;
+				regulator-always-on;
+			};
+
+			vgen2_reg: vgen2 {
+				regulator-min-microvolt = <3150000>;
+				regulator-max-microvolt = <3150000>;
+				regulator-always-on;
+			};
+
+			vgen3_reg: vgen3 {
+				regulator-min-microvolt = <1800000>;
+				regulator-max-microvolt = <1800000>;
+				regulator-always-on;
+			};
+
+			vusb_reg: vusb {
+				regulator-always-on;
+			};
+
+			gpo1_reg: gpo1 { };
+
+			gpo2_reg: gpo2 { };
+
+			gpo3_reg: gpo3 { };
+
+			gpo4_reg: gpo4 { };
+
+			pwgt2spi_reg: pwgt2spi {
+				regulator-always-on;
+			};
+
+			vcoincell_reg: vcoincell {
+				regulator-min-microvolt = <3000000>;
+				regulator-max-microvolt = <3000000>;
+				regulator-always-on;
+			};
+		};
+	};
+};
+
+&esdhc2 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_esdhc2>;
+	cap-sdio-irq;
+	enable-sdio-wakeup;
+	keep-power-in-suspend;
+	max-frequency = <50000000>;
+	no-1-8-v;
+	non-removable;
+	vmmc-supply = <&gpo4_reg>;
+	status = "okay";
+};
+
+&fec {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_fec>;
+	phy-mode = "mii";
+	phy-supply = <&gpo3_reg>;
+	/* Pins shared with LCD2, keep status disabled */
+};
+
+&i2c2 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_i2c2>;
+	clock-frequency = <400000>;
+	status = "okay";
+
+	mma7455l@1d {
+		pinctrl-names = "default";
+		pinctrl-0 = <&pinctrl_mma7455l>;
+		compatible = "fsl,mma7455l";
+		reg = <0x1d>;
+		interrupt-parent = <&gpio1>;
+		interrupts = <7 IRQ_TYPE_LEVEL_HIGH>, <6 IRQ_TYPE_LEVEL_HIGH>;
+	};
+};
+
+&nfc {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_nfc>;
+	nand-bus-width = <8>;
+	nand-ecc-mode = "hw";
+	nand-on-flash-bbt;
+	status = "okay";
+};
+
+&usbotg {
+	phy_type = "utmi_wide";
+	disable-over-current;
+	/* Device role is not known, keep status disabled */
+};
+
+&weim {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_weim>;
+	status = "okay";
+
+	lan9221: lan9221@5,0 {
+		pinctrl-names = "default";
+		pinctrl-0 = <&pinctrl_lan9221>;
+		compatible = "smsc,lan9221", "smsc,lan9115";
+		reg = <5 0x00000000 0x1000>;
+		fsl,weim-cs-timing = <
+			0x00420081 0x00000000
+			0x32260000 0x00000000
+			0x72080f00 0x00000000
+		>;
+		clocks = <&clks IMX5_CLK_DUMMY>;
+		interrupt-parent = <&gpio1>;
+		interrupts = <9 IRQ_TYPE_LEVEL_LOW>;
+		phy-mode = "mii";
+		reg-io-width = <2>;
+		smsc,irq-push-pull;
+		vdd33a-supply = <&gpo2_reg>;
+		vddvario-supply = <&gpo2_reg>;
+	};
+};
+
+&iomuxc {
+	imx51-digi-connectcore-som {
+		pinctrl_ecspi1: ecspi1grp {
+			fsl,pins = <
+				MX51_PAD_CSPI1_MISO__ECSPI1_MISO	0x185
+				MX51_PAD_CSPI1_MOSI__ECSPI1_MOSI	0x185
+				MX51_PAD_CSPI1_SCLK__ECSPI1_SCLK	0x185
+				MX51_PAD_CSPI1_SS0__GPIO4_24		0x85 /* CS0 */
+			>;
+		};
+
+		pinctrl_esdhc2: esdhc2grp {
+			fsl,pins = <
+				MX51_PAD_SD2_CMD__SD2_CMD		0x400020d5
+				MX51_PAD_SD2_CLK__SD2_CLK		0x20d5
+				MX51_PAD_SD2_DATA0__SD2_DATA0		0x20d5
+				MX51_PAD_SD2_DATA1__SD2_DATA1		0x20d5
+				MX51_PAD_SD2_DATA2__SD2_DATA2		0x20d5
+				MX51_PAD_SD2_DATA3__SD2_DATA3		0x20d5
+			>;
+		};
+
+		pinctrl_fec: fecgrp {
+			fsl,pins = <
+				MX51_PAD_DI_GP3__FEC_TX_ER		0x80000000
+				MX51_PAD_DI2_PIN4__FEC_CRS		0x80000000
+				MX51_PAD_DI2_PIN2__FEC_MDC		0x80000000
+				MX51_PAD_DI2_PIN3__FEC_MDIO		0x80000000
+				MX51_PAD_DI2_DISP_CLK__FEC_RDATA1	0x80000000
+				MX51_PAD_DI_GP4__FEC_RDATA2		0x80000000
+				MX51_PAD_DISP2_DAT0__FEC_RDATA3		0x80000000
+				MX51_PAD_DISP2_DAT1__FEC_RX_ER		0x80000000
+				MX51_PAD_DISP2_DAT6__FEC_TDATA1		0x80000000
+				MX51_PAD_DISP2_DAT7__FEC_TDATA2		0x80000000
+				MX51_PAD_DISP2_DAT8__FEC_TDATA3		0x80000000
+				MX51_PAD_DISP2_DAT9__FEC_TX_EN		0x80000000
+				MX51_PAD_DISP2_DAT10__FEC_COL		0x80000000
+				MX51_PAD_DISP2_DAT11__FEC_RX_CLK	0x80000000
+				MX51_PAD_DISP2_DAT12__FEC_RX_DV		0x80000000
+				MX51_PAD_DISP2_DAT13__FEC_TX_CLK	0x80000000
+				MX51_PAD_DISP2_DAT14__FEC_RDATA0	0x80000000
+				MX51_PAD_DISP2_DAT15__FEC_TDATA0	0x80000000
+			>;
+		};
+
+		pinctrl_i2c2: i2c2grp {
+			fsl,pins = <
+				MX51_PAD_GPIO1_2__I2C2_SCL		0x400001ed
+				MX51_PAD_GPIO1_3__I2C2_SDA		0x400001ed
+			>;
+		};
+
+		pinctrl_nfc: nfcgrp {
+			fsl,pins = <
+				MX51_PAD_NANDF_D0__NANDF_D0		0x80000000
+				MX51_PAD_NANDF_D1__NANDF_D1		0x80000000
+				MX51_PAD_NANDF_D2__NANDF_D2		0x80000000
+				MX51_PAD_NANDF_D3__NANDF_D3		0x80000000
+				MX51_PAD_NANDF_D4__NANDF_D4		0x80000000
+				MX51_PAD_NANDF_D5__NANDF_D5		0x80000000
+				MX51_PAD_NANDF_D6__NANDF_D6		0x80000000
+				MX51_PAD_NANDF_D7__NANDF_D7		0x80000000
+				MX51_PAD_NANDF_ALE__NANDF_ALE		0x80000000
+				MX51_PAD_NANDF_CLE__NANDF_CLE		0x80000000
+				MX51_PAD_NANDF_RE_B__NANDF_RE_B		0x80000000
+				MX51_PAD_NANDF_WE_B__NANDF_WE_B		0x80000000
+				MX51_PAD_NANDF_WP_B__NANDF_WP_B		0x80000000
+				MX51_PAD_NANDF_CS0__NANDF_CS0		0x80000000
+				MX51_PAD_NANDF_RB0__NANDF_RB0		0x80000000
+			>;
+		};
+
+		pinctrl_lan9221: lan9221grp {
+			fsl,pins = <
+				MX51_PAD_GPIO1_9__GPIO1_9		0xe5 /* IRQ */
+			>;
+		};
+
+		pinctrl_mc13892: mc13892grp {
+			fsl,pins = <
+				MX51_PAD_GPIO1_5__GPIO1_5		0xe5 /* IRQ */
+			>;
+		};
+
+		pinctrl_mma7455l: mma7455lgrp {
+			fsl,pins = <
+				MX51_PAD_GPIO1_7__GPIO1_7		0xe5 /* IRQ1 */
+				MX51_PAD_GPIO1_6__GPIO1_6		0xe5 /* IRQ2 */
+			>;
+		};
+
+		pinctrl_weim: weimgrp {
+			fsl,pins = <
+				MX51_PAD_EIM_DA0__EIM_DA0		0x80000000
+				MX51_PAD_EIM_DA1__EIM_DA1		0x80000000
+				MX51_PAD_EIM_DA2__EIM_DA2		0x80000000
+				MX51_PAD_EIM_DA3__EIM_DA3		0x80000000
+				MX51_PAD_EIM_DA4__EIM_DA4		0x80000000
+				MX51_PAD_EIM_DA5__EIM_DA5		0x80000000
+				MX51_PAD_EIM_DA6__EIM_DA6		0x80000000
+				MX51_PAD_EIM_DA7__EIM_DA7		0x80000000
+				MX51_PAD_EIM_DA8__EIM_DA8		0x80000000
+				MX51_PAD_EIM_DA9__EIM_DA9		0x80000000
+				MX51_PAD_EIM_DA10__EIM_DA10		0x80000000
+				MX51_PAD_EIM_DA11__EIM_DA11		0x80000000
+				MX51_PAD_EIM_DA12__EIM_DA12		0x80000000
+				MX51_PAD_EIM_DA13__EIM_DA13		0x80000000
+				MX51_PAD_EIM_DA14__EIM_DA14		0x80000000
+				MX51_PAD_EIM_DA15__EIM_DA15		0x80000000
+				MX51_PAD_EIM_A16__EIM_A16		0x80000000
+				MX51_PAD_EIM_A17__EIM_A17		0x80000000
+				MX51_PAD_EIM_A18__EIM_A18		0x80000000
+				MX51_PAD_EIM_A19__EIM_A19		0x80000000
+				MX51_PAD_EIM_A20__EIM_A20		0x80000000
+				MX51_PAD_EIM_A21__EIM_A21		0x80000000
+				MX51_PAD_EIM_A22__EIM_A22		0x80000000
+				MX51_PAD_EIM_A23__EIM_A23		0x80000000
+				MX51_PAD_EIM_A24__EIM_A24		0x80000000
+				MX51_PAD_EIM_A25__EIM_A25		0x80000000
+				MX51_PAD_EIM_A26__EIM_A26		0x80000000
+				MX51_PAD_EIM_A27__EIM_A27		0x80000000
+				MX51_PAD_EIM_D16__EIM_D16		0x80000000
+				MX51_PAD_EIM_D17__EIM_D17		0x80000000
+				MX51_PAD_EIM_D18__EIM_D18		0x80000000
+				MX51_PAD_EIM_D19__EIM_D19		0x80000000
+				MX51_PAD_EIM_D20__EIM_D20		0x80000000
+				MX51_PAD_EIM_D21__EIM_D21		0x80000000
+				MX51_PAD_EIM_D22__EIM_D22		0x80000000
+				MX51_PAD_EIM_D23__EIM_D23		0x80000000
+				MX51_PAD_EIM_D24__EIM_D24		0x80000000
+				MX51_PAD_EIM_D25__EIM_D25		0x80000000
+				MX51_PAD_EIM_D26__EIM_D26		0x80000000
+				MX51_PAD_EIM_D27__EIM_D27		0x80000000
+				MX51_PAD_EIM_D28__EIM_D28		0x80000000
+				MX51_PAD_EIM_D29__EIM_D29		0x80000000
+				MX51_PAD_EIM_D30__EIM_D30		0x80000000
+				MX51_PAD_EIM_D31__EIM_D31		0x80000000
+				MX51_PAD_EIM_OE__EIM_OE			0x80000000
+				MX51_PAD_EIM_DTACK__EIM_DTACK		0x80000000
+				MX51_PAD_EIM_LBA__EIM_LBA		0x80000000
+				MX51_PAD_EIM_CS5__EIM_CS5		0x80000000 /* CS5 */
+			>;
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/imx51-eukrea-cpuimx51.dtsi b/arch/arm/boot/dts/imx51-eukrea-cpuimx51.dtsi
index 9b3acf6e428..63164266af8 100644
--- a/arch/arm/boot/dts/imx51-eukrea-cpuimx51.dtsi
+++ b/arch/arm/boot/dts/imx51-eukrea-cpuimx51.dtsi
@@ -42,6 +42,17 @@
 		compatible = "nxp,pcf8563";
 		reg = <0x51>;
 	};
+
+	tsc2007: tsc2007@49 {
+		compatible = "ti,tsc2007";
+		gpios = <&gpio4 0 1>;
+		interrupt-parent = <&gpio4>;
+		interrupts = <0x0 0x8>;
+		pinctrl-names = "default";
+		pinctrl-0 = <&pinctrl_tsc2007_1>;
+		reg = <0x49>;
+		ti,x-plate-ohms = <180>;
+	};
 };
 
 &iomuxc {
diff --git a/arch/arm/boot/dts/imx51-eukrea-mbimxsd51-baseboard.dts b/arch/arm/boot/dts/imx51-eukrea-mbimxsd51-baseboard.dts
index 5cec4f32209..75e66c9c614 100644
--- a/arch/arm/boot/dts/imx51-eukrea-mbimxsd51-baseboard.dts
+++ b/arch/arm/boot/dts/imx51-eukrea-mbimxsd51-baseboard.dts
@@ -24,6 +24,14 @@
 	model = "Eukrea CPUIMX51";
 	compatible = "eukrea,mbimxsd51","eukrea,cpuimx51", "fsl,imx51";
 
+	clocks {
+		clk24M: can_clock {
+			compatible = "fixed-clock";
+			#clock-cells = <0>;
+			clock-frequency = <24000000>;
+		};
+	};
+
 	gpio_keys {
 		compatible = "gpio-keys";
 		pinctrl-names = "default";
@@ -50,6 +58,23 @@
 		};
 	};
 
+	regulators {
+		compatible = "simple-bus";
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		reg_can: regulator@0 {
+			compatible = "regulator-fixed";
+			reg = <0>;
+			regulator-name = "CAN_RST";
+			regulator-min-microvolt = <3300000>;
+			regulator-max-microvolt = <3300000>;
+			gpio = <&gpio4 15 GPIO_ACTIVE_HIGH>;
+			startup-delay-us = <20000>;
+			enable-active-high;
+		};
+	};
+
 	sound {
 		compatible = "eukrea,asoc-tlv320";
 		eukrea,model = "imx51-eukrea-tlv320aic23";
@@ -57,6 +82,20 @@
 		fsl,mux-int-port = <2>;
 		fsl,mux-ext-port = <3>;
 	};
+
+	usbphy {
+		#address-cells = <1>;
+		#size-cells = <0>;
+		compatible = "simple-bus";
+
+		usbh1phy: usbh1phy@0 {
+			compatible = "usb-nop-xceiv";
+			reg = <0>;
+			clocks = <&clks IMX5_CLK_USB_PHY_GATE>;
+			clock-names = "main_clk";
+			clock-frequency = <19200000>;
+		};
+	};
 };
 
 &audmux {
@@ -72,6 +111,26 @@
 	status = "okay";
 };
 
+&ecspi1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_ecspi1>;
+	fsl,spi-num-chipselects = <1>;
+	cs-gpios = <&gpio4 24 GPIO_ACTIVE_LOW>;
+	status = "okay";
+
+	can0: can@0 {
+		pinctrl-names = "default";
+		pinctrl-0 = <&pinctrl_can>;
+		compatible = "microchip,mcp2515";
+		reg = <0>;
+		clocks = <&clk24M>;
+		spi-max-frequency = <10000000>;
+		interrupt-parent = <&gpio1>;
+		interrupts = <1 IRQ_TYPE_EDGE_FALLING>;
+		vdd-supply = <&reg_can>;
+	};
+};
+
 &i2c1 {
 	tlv320aic23: codec@1a {
 		compatible = "ti,tlv320aic23";
@@ -90,6 +149,23 @@
 			>;
 		};
 
+
+		pinctrl_can: cangrp {
+			fsl,pins = <
+				MX51_PAD_CSI2_PIXCLK__GPIO4_15		0x80000000	/* nReset */
+				MX51_PAD_GPIO1_1__GPIO1_1		0x80000000	/* IRQ */
+			>;
+		};
+
+		pinctrl_ecspi1: ecspi1grp {
+			fsl,pins = <
+				MX51_PAD_CSPI1_MISO__ECSPI1_MISO	0x185
+				MX51_PAD_CSPI1_MOSI__ECSPI1_MOSI	0x185
+				MX51_PAD_CSPI1_SCLK__ECSPI1_SCLK	0x185
+				MX51_PAD_CSPI1_SS0__GPIO4_24		0x80000000 	/* CS0 */
+			>;
+		};
+
 		pinctrl_esdhc1: esdhc1grp {
 			fsl,pins = <
 				MX51_PAD_SD1_CMD__SD1_CMD		0x400020d5
@@ -151,6 +227,29 @@
 				MX51_PAD_CSI1_D9__GPIO3_13 0x1f5
 			>;
 		};
+
+		pinctrl_usbh1: usbh1grp {
+			fsl,pins = <
+				MX51_PAD_USBH1_CLK__USBH1_CLK     0x1e5
+				MX51_PAD_USBH1_DIR__USBH1_DIR     0x1e5
+				MX51_PAD_USBH1_NXT__USBH1_NXT     0x1e5
+				MX51_PAD_USBH1_DATA0__USBH1_DATA0 0x1e5
+				MX51_PAD_USBH1_DATA1__USBH1_DATA1 0x1e5
+				MX51_PAD_USBH1_DATA2__USBH1_DATA2 0x1e5
+				MX51_PAD_USBH1_DATA3__USBH1_DATA3 0x1e5
+				MX51_PAD_USBH1_DATA4__USBH1_DATA4 0x1e5
+				MX51_PAD_USBH1_DATA5__USBH1_DATA5 0x1e5
+				MX51_PAD_USBH1_DATA6__USBH1_DATA6 0x1e5
+				MX51_PAD_USBH1_DATA7__USBH1_DATA7 0x1e5
+				MX51_PAD_USBH1_STP__USBH1_STP     0x1e5
+			>;
+		};
+
+		pinctrl_usbh1_vbus: usbh1-vbusgrp {
+			fsl,pins = <
+				MX51_PAD_EIM_CS3__GPIO2_28 0x1f5
+			>;
+		};
 	};
 };
 
@@ -173,3 +272,24 @@
 	fsl,uart-has-rtscts;
 	status = "okay";
 };
+
+&usbh1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_usbh1>;
+	fsl,usbphy = <&usbh1phy>;
+	dr_mode = "host";
+	phy_type = "ulpi";
+	status = "okay";
+};
+
+&usbotg {
+	dr_mode = "otg";
+	phy_type = "utmi_wide";
+	status = "okay";
+};
+
+&usbphy0 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_usbh1_vbus>;
+	reset-gpios = <&gpio2 28 GPIO_ACTIVE_LOW>;
+};
diff --git a/arch/arm/boot/dts/imx51.dtsi b/arch/arm/boot/dts/imx51.dtsi
index 5f8216d08f6..bebbf3ba0d5 100644
--- a/arch/arm/boot/dts/imx51.dtsi
+++ b/arch/arm/boot/dts/imx51.dtsi
@@ -19,6 +19,7 @@
 
 / {
 	aliases {
+		ethernet0 = &fec;
 		gpio0 = &gpio1;
 		gpio1 = &gpio2;
 		gpio2 = &gpio3;
@@ -50,21 +51,25 @@
 
 		ckil {
 			compatible = "fsl,imx-ckil", "fixed-clock";
+			#clock-cells = <0>;
 			clock-frequency = <32768>;
 		};
 
 		ckih1 {
 			compatible = "fsl,imx-ckih1", "fixed-clock";
+			#clock-cells = <0>;
 			clock-frequency = <0>;
 		};
 
 		ckih2 {
 			compatible = "fsl,imx-ckih2", "fixed-clock";
+			#clock-cells = <0>;
 			clock-frequency = <0>;
 		};
 
 		osc {
 			compatible = "fsl,imx-osc", "fixed-clock";
+			#clock-cells = <0>;
 			clock-frequency = <24000000>;
 		};
 	};
@@ -533,6 +538,8 @@
 			};
 
 			nfc: nand@83fdb000 {
+				#address-cells = <1>;
+				#size-cells = <1>;
 				compatible = "fsl,imx51-nand";
 				reg = <0x83fdb000 0x1000 0xcfff0000 0x10000>;
 				interrupts = <8>;
diff --git a/arch/arm/boot/dts/imx53-m53evk.dts b/arch/arm/boot/dts/imx53-m53evk.dts
index f6d3ac3e558..d5d146a8b14 100644
--- a/arch/arm/boot/dts/imx53-m53evk.dts
+++ b/arch/arm/boot/dts/imx53-m53evk.dts
@@ -17,7 +17,8 @@
 	compatible = "denx,imx53-m53evk", "fsl,imx53";
 
 	memory {
-		reg = <0x70000000 0x20000000>;
+		reg = <0x70000000 0x20000000>,
+		      <0xb0000000 0x20000000>;
 	};
 
 	soc {
@@ -193,17 +194,17 @@
 		irq-trigger = <0x1>;
 
 		stmpe_touchscreen {
-			compatible = "stmpe,ts";
+			compatible = "st,stmpe-ts";
 			reg = <0>;
-			ts,sample-time = <4>;
-			ts,mod-12b = <1>;
-			ts,ref-sel = <0>;
-			ts,adc-freq = <1>;
-			ts,ave-ctrl = <3>;
-			ts,touch-det-delay = <3>;
-			ts,settling = <4>;
-			ts,fraction-z = <7>;
-			ts,i-drive = <1>;
+			st,sample-time = <4>;
+			st,mod-12b = <1>;
+			st,ref-sel = <0>;
+			st,adc-freq = <1>;
+			st,ave-ctrl = <3>;
+			st,touch-det-delay = <3>;
+			st,settling = <4>;
+			st,fraction-z = <7>;
+			st,i-drive = <1>;
 		};
 	};
 
diff --git a/arch/arm/boot/dts/imx53-mba53.dts b/arch/arm/boot/dts/imx53-mba53.dts
index 7c8c1296989..3e3f17aa93a 100644
--- a/arch/arm/boot/dts/imx53-mba53.dts
+++ b/arch/arm/boot/dts/imx53-mba53.dts
@@ -17,6 +17,10 @@
 	model = "TQ MBa53 starter kit";
 	compatible = "tq,mba53", "tq,tqma53", "fsl,imx53";
 
+	chosen {
+		stdout-path = &uart2;
+	};
+
 	backlight {
 		compatible = "pwm-backlight";
 		pwms = <&pwm2 0 50000>;
@@ -244,7 +248,7 @@
 &tve {
 	pinctrl-names = "default";
 	pinctrl-0 = <&pinctrl_vga_sync_1>;
-	i2c-ddc-bus = <&i2c3>;
+	ddc-i2c-bus = <&i2c3>;
 	fsl,tve-mode = "vga";
 	fsl,hsync-pin = <4>;
 	fsl,vsync-pin = <6>;
diff --git a/arch/arm/boot/dts/imx53-qsb-common.dtsi b/arch/arm/boot/dts/imx53-qsb-common.dtsi
index 3f825a6813d..fd8c60dde7d 100644
--- a/arch/arm/boot/dts/imx53-qsb-common.dtsi
+++ b/arch/arm/boot/dts/imx53-qsb-common.dtsi
@@ -13,8 +13,13 @@
 #include "imx53.dtsi"
 
 / {
+	chosen {
+		stdout-path = &uart1;
+	};
+
 	memory {
-		reg = <0x70000000 0x40000000>;
+		reg = <0x70000000 0x20000000>,
+		      <0xb0000000 0x20000000>;
 	};
 
 	display0: display@di0 {
@@ -271,6 +276,14 @@
 			>;
 		};
 
+		pinctrl_vga_sync: vgasync-grp {
+			fsl,pins = <
+				/* VGA_HSYNC, VSYNC with max drive strength */
+				MX53_PAD_EIM_OE__IPU_DI1_PIN7 0xe6
+				MX53_PAD_EIM_RW__IPU_DI1_PIN8 0xe6
+			>;
+		};
+
 		pinctrl_uart1: uart1grp {
 			fsl,pins = <
 				MX53_PAD_CSI0_DAT10__UART1_TXD_MUX	0x1e4
@@ -280,6 +293,15 @@
 	};
 };
 
+&tve {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_vga_sync>;
+	fsl,tve-mode = "vga";
+	fsl,hsync-pin = <4>;
+	fsl,vsync-pin = <6>;
+	status = "okay";
+};
+
 &uart1 {
 	pinctrl-names = "default";
 	pinctrl-0 = <&pinctrl_uart1>;
diff --git a/arch/arm/boot/dts/imx53-tx53-x03x.dts b/arch/arm/boot/dts/imx53-tx53-x03x.dts
index 0217dde3b36..3b73e81dc3f 100644
--- a/arch/arm/boot/dts/imx53-tx53-x03x.dts
+++ b/arch/arm/boot/dts/imx53-tx53-x03x.dts
@@ -25,12 +25,17 @@
 	soc {
 		display: display@di0 {
 			compatible = "fsl,imx-parallel-display";
-			crtcs = <&ipu 0>;
 			interface-pix-fmt = "rgb24";
 			pinctrl-names = "default";
 			pinctrl-0 = <&pinctrl_rgb24_vga1>;
 			status = "okay";
 
+			port {
+				display0_in: endpoint {
+					remote-endpoint = <&ipu_di0_disp0>;
+				};
+			};
+
 			display-timings {
 				VGA {
 					clock-frequency = <25200000>;
@@ -293,6 +298,10 @@
 	};
 };
 
+&ipu_di0_disp0 {
+	remote-endpoint = <&display0_in>;
+};
+
 &kpp {
 	pinctrl-names = "default";
 	pinctrl-0 = <&pinctrl_kpp>;
diff --git a/arch/arm/boot/dts/imx53.dtsi b/arch/arm/boot/dts/imx53.dtsi
index b57ab57740f..6456a008438 100644
--- a/arch/arm/boot/dts/imx53.dtsi
+++ b/arch/arm/boot/dts/imx53.dtsi
@@ -18,6 +18,7 @@
 
 / {
 	aliases {
+		ethernet0 = &fec;
 		gpio0 = &gpio1;
 		gpio1 = &gpio2;
 		gpio2 = &gpio3;
@@ -70,21 +71,25 @@
 
 		ckil {
 			compatible = "fsl,imx-ckil", "fixed-clock";
+			#clock-cells = <0>;
 			clock-frequency = <32768>;
 		};
 
 		ckih1 {
 			compatible = "fsl,imx-ckih1", "fixed-clock";
+			#clock-cells = <0>;
 			clock-frequency = <22579200>;
 		};
 
 		ckih2 {
 			compatible = "fsl,imx-ckih2", "fixed-clock";
+			#clock-cells = <0>;
 			clock-frequency = <0>;
 		};
 
 		osc {
 			compatible = "fsl,imx-osc", "fixed-clock";
+			#clock-cells = <0>;
 			clock-frequency = <24000000>;
 		};
 	};
@@ -111,7 +116,7 @@
 			#address-cells = <1>;
 			#size-cells = <0>;
 			compatible = "fsl,imx53-ipu";
-			reg = <0x18000000 0x080000000>;
+			reg = <0x18000000 0x08000000>;
 			interrupts = <11 10>;
 			clocks = <&clks IMX5_CLK_IPU_GATE>,
 			         <&clks IMX5_CLK_IPU_DI0_GATE>,
@@ -430,7 +435,7 @@
 
 					port {
 						lvds1_in: endpoint {
-							remote-endpoint = <&ipu_di0_lvds0>;
+							remote-endpoint = <&ipu_di1_lvds1>;
 						};
 					};
 				};
@@ -722,8 +727,8 @@
 				clocks = <&clks IMX5_CLK_VPU_GATE>,
 				         <&clks IMX5_CLK_VPU_GATE>;
 				clock-names = "per", "ahb";
+				resets = <&src 1>;
 				iram = <&ocram>;
-				status = "disabled";
 			};
 		};
 
diff --git a/arch/arm/boot/dts/imx6dl-hummingboard.dts b/arch/arm/boot/dts/imx6dl-hummingboard.dts
index 5bfae54fb78..5373a5f2782 100644
--- a/arch/arm/boot/dts/imx6dl-hummingboard.dts
+++ b/arch/arm/boot/dts/imx6dl-hummingboard.dts
@@ -11,6 +11,10 @@
 	model = "SolidRun HummingBoard DL/Solo";
 	compatible = "solidrun,hummingboard", "fsl,imx6dl";
 
+	chosen {
+		stdout-path = &uart1;
+	};
+
 	ir_recv: ir-receiver {
 		compatible = "gpio-ir-receiver";
 		gpios = <&gpio1 2 1>;
@@ -67,6 +71,13 @@
 	status = "okay";
 };
 
+&hdmi {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_hummingboard_hdmi>;
+	ddc-i2c-bus = <&i2c2>;
+	status = "okay";
+};
+
 &i2c1 {
 	pinctrl-names = "default";
 	pinctrl-0 = <&pinctrl_hummingboard_i2c1>;
@@ -82,6 +93,13 @@
 	 */
 };
 
+&i2c2 {
+	clock-frequency = <100000>;
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_hummingboard_i2c2>;
+	status = "okay";
+};
+
 &iomuxc {
 	hummingboard {
 		pinctrl_hummingboard_flexcan1: hummingboard-flexcan1 {
@@ -97,6 +115,12 @@
 			>;
 		};
 
+		pinctrl_hummingboard_hdmi: hummingboard-hdmi {
+			fsl,pins = <
+				MX6QDL_PAD_KEY_ROW2__HDMI_TX_CEC_LINE 0x1f8b0
+			>;
+		};
+
 		pinctrl_hummingboard_i2c1: hummingboard-i2c1 {
 			fsl,pins = <
 				MX6QDL_PAD_EIM_D21__I2C1_SCL 0x4001b8b1
@@ -104,6 +128,13 @@
 			>;
 		};
 
+		pinctrl_hummingboard_i2c2: hummingboard-i2c2 {
+			fsl,pins = <
+				MX6QDL_PAD_KEY_COL3__I2C2_SCL 0x4001b8b1
+				MX6QDL_PAD_KEY_ROW3__I2C2_SDA 0x4001b8b1
+			>;
+		};
+
 		pinctrl_hummingboard_spdif: hummingboard-spdif {
 			fsl,pins = <MX6QDL_PAD_GPIO_17__SPDIF_OUT 0x13091>;
 		};
diff --git a/arch/arm/boot/dts/imx6dl-phytec-pbab01.dts b/arch/arm/boot/dts/imx6dl-phytec-pbab01.dts
new file mode 100644
index 00000000000..08e97801494
--- /dev/null
+++ b/arch/arm/boot/dts/imx6dl-phytec-pbab01.dts
@@ -0,0 +1,19 @@
+/*
+ * Copyright 2013 Christian Hemp, Phytec Messtechnik GmbH
+ *
+ * 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
+ */
+
+/dts-v1/;
+#include "imx6dl-phytec-pfla02.dtsi"
+#include "imx6qdl-phytec-pbab01.dtsi"
+
+/ {
+	model = "Phytec phyFLEX-i.MX6 DualLite/Solo Carrier-Board";
+	compatible = "phytec,imx6dl-pbab01", "phytec,imx6dl-pfla02", "fsl,imx6dl";
+};
diff --git a/arch/arm/boot/dts/imx6dl-phytec-pfla02.dtsi b/arch/arm/boot/dts/imx6dl-phytec-pfla02.dtsi
new file mode 100644
index 00000000000..964bc2ad3c5
--- /dev/null
+++ b/arch/arm/boot/dts/imx6dl-phytec-pfla02.dtsi
@@ -0,0 +1,22 @@
+/*
+ * Copyright 2013 Christian Hemp, Phytec Messtechnik GmbH
+ *
+ * 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 "imx6dl.dtsi"
+#include "imx6qdl-phytec-pfla02.dtsi"
+
+/ {
+	model = "Phytec phyFLEX-i.MX6 DualLite/Solo";
+	compatible = "phytec,imx6dl-pfla02", "fsl,imx6dl";
+
+	memory {
+		reg = <0x10000000 0x20000000>;
+	};
+};
diff --git a/arch/arm/boot/dts/imx6dl-riotboard.dts b/arch/arm/boot/dts/imx6dl-riotboard.dts
new file mode 100644
index 00000000000..909fafc0b65
--- /dev/null
+++ b/arch/arm/boot/dts/imx6dl-riotboard.dts
@@ -0,0 +1,539 @@
+/*
+ * Copyright 2014 Iain Paton <ipaton0@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ */
+
+/dts-v1/;
+#include "imx6dl.dtsi"
+#include <dt-bindings/gpio/gpio.h>
+
+/ {
+	model = "RIoTboard i.MX6S";
+	compatible = "riot,imx6s-riotboard", "fsl,imx6dl";
+
+	memory {
+		reg = <0x10000000 0x40000000>;
+	};
+
+	regulators {
+		compatible = "simple-bus";
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		reg_2p5v: regulator@0 {
+			compatible = "regulator-fixed";
+			reg = <0>;
+			regulator-name = "2P5V";
+			regulator-min-microvolt = <2500000>;
+			regulator-max-microvolt = <2500000>;
+		};
+
+		reg_3p3v: regulator@1 {
+			compatible = "regulator-fixed";
+			reg = <1>;
+			regulator-name = "3P3V";
+			regulator-min-microvolt = <3300000>;
+			regulator-max-microvolt = <3300000>;
+		};
+
+		reg_usb_otg_vbus: regulator@2 {
+			compatible = "regulator-fixed";
+			reg = <2>;
+			regulator-name = "usb_otg_vbus";
+			regulator-min-microvolt = <5000000>;
+			regulator-max-microvolt = <5000000>;
+			gpio = <&gpio3 22 0>;
+			enable-active-high;
+		};
+	};
+
+	leds {
+		compatible = "gpio-leds";
+		pinctrl-names = "default";
+		pinctrl-0 = <&pinctrl_led>;
+
+		led0: user1 {
+			label = "user1";
+			gpios = <&gpio5 2 GPIO_ACTIVE_LOW>;
+			default-state = "on";
+			linux,default-trigger = "heartbeat";
+		};
+
+		led1: user2 {
+			label = "user2";
+			gpios = <&gpio3 28 GPIO_ACTIVE_LOW>;
+			default-state = "off";
+		};
+	};
+
+	sound {
+		compatible = "fsl,imx-audio-sgtl5000";
+		model = "imx6-riotboard-sgtl5000";
+		ssi-controller = <&ssi1>;
+		audio-codec = <&codec>;
+		audio-routing =
+			"MIC_IN", "Mic Jack",
+			"Mic Jack", "Mic Bias",
+			"Headphone Jack", "HP_OUT";
+			mux-int-port = <1>;
+			mux-ext-port = <3>;
+	};
+};
+
+&audmux {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_audmux>;
+	status = "okay";
+};
+
+&fec {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_enet>;
+	phy-mode = "rgmii";
+	phy-reset-gpios = <&gpio3 31 0>;
+	interrupts-extended = <&gpio1 6 IRQ_TYPE_LEVEL_HIGH>,
+			      <&intc 0 119 IRQ_TYPE_LEVEL_HIGH>;
+	status = "okay";
+};
+
+&hdmi {
+	ddc-i2c-bus = <&i2c2>;
+	status = "okay";
+};
+
+&i2c1 {
+	clock-frequency = <100000>;
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_i2c1>;
+	status = "okay";
+
+	codec: sgtl5000@0a {
+		compatible = "fsl,sgtl5000";
+		reg = <0x0a>;
+		clocks = <&clks 201>;
+		VDDA-supply = <&reg_2p5v>;
+		VDDIO-supply = <&reg_3p3v>;
+	};
+
+	pmic: pf0100@08 {
+		compatible = "fsl,pfuze100";
+		reg = <0x08>;
+		interrupt-parent = <&gpio5>;
+		interrupts = <16 8>;
+
+		regulators {
+			reg_vddcore: sw1ab {				/* VDDARM_IN */
+				regulator-min-microvolt = <300000>;
+				regulator-max-microvolt = <1875000>;
+				regulator-always-on;
+			};
+
+			reg_vddsoc: sw1c {				/* VDDSOC_IN */
+				regulator-min-microvolt = <300000>;
+				regulator-max-microvolt = <1875000>;
+				regulator-always-on;
+			};
+
+			reg_gen_3v3: sw2 {				/* VDDHIGH_IN */
+				regulator-min-microvolt = <800000>;
+				regulator-max-microvolt = <3300000>;
+				regulator-always-on;
+			};
+
+			reg_ddr_1v5a: sw3a {				/* NVCC_DRAM, NVCC_RGMII */
+				regulator-min-microvolt = <400000>;
+				regulator-max-microvolt = <1975000>;
+				regulator-always-on;
+			};
+
+			reg_ddr_1v5b: sw3b {				/* NVCC_DRAM, NVCC_RGMII */
+				regulator-min-microvolt = <400000>;
+				regulator-max-microvolt = <1975000>;
+				regulator-always-on;
+			};
+
+			reg_ddr_vtt: sw4 {				/* MIPI conn */
+				regulator-min-microvolt = <400000>;
+				regulator-max-microvolt = <1975000>;
+				regulator-always-on;
+			};
+
+			reg_5v_600mA: swbst {				/* not used */
+				regulator-min-microvolt = <5000000>;
+				regulator-max-microvolt = <5150000>;
+			};
+
+			reg_snvs_3v: vsnvs {				/* VDD_SNVS_IN */
+				regulator-min-microvolt = <1500000>;
+				regulator-max-microvolt = <3000000>;
+				regulator-always-on;
+			};
+
+			vref_reg: vrefddr {				/* VREF_DDR */
+				regulator-boot-on;
+				regulator-always-on;
+			};
+
+			reg_vgen1_1v5: vgen1 {				/* not used */
+				regulator-min-microvolt = <800000>;
+				regulator-max-microvolt = <1550000>;
+			};
+
+			reg_vgen2_1v2_eth: vgen2 {			/* pcie ? */
+				regulator-min-microvolt = <800000>;
+				regulator-max-microvolt = <1550000>;
+				regulator-always-on;
+			};
+
+			reg_vgen3_2v8: vgen3 {				/* not used */
+				regulator-min-microvolt = <1800000>;
+				regulator-max-microvolt = <3300000>;
+			};
+			reg_vgen4_1v8: vgen4 {				/* NVCC_SD3 */
+				regulator-min-microvolt = <1800000>;
+				regulator-max-microvolt = <3300000>;
+				regulator-always-on;
+			};
+
+			reg_vgen5_2v5_sgtl: vgen5 {			/* Pwr LED & 5V0_delayed enable */
+				regulator-min-microvolt = <1800000>;
+				regulator-max-microvolt = <3300000>;
+				regulator-always-on;
+			};
+
+			reg_vgen6_3v3: vgen6 {				/* #V#_DELAYED enable, MIPI */
+				regulator-min-microvolt = <1800000>;
+				regulator-max-microvolt = <3300000>;
+				regulator-always-on;
+			};
+		};
+	};
+};
+
+&i2c2 {
+	clock-frequency = <100000>;
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_i2c2>;
+	status = "okay";
+};
+
+&i2c4 {
+	clock-frequency = <100000>;
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_i2c4>;
+	clocks = <&clks 116>;
+	status = "okay";
+};
+
+&pwm1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_pwm1>;
+	status = "okay";
+};
+
+&pwm2 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_pwm2>;
+	status = "okay";
+};
+
+&pwm3 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_pwm3>;
+	status = "okay";
+};
+
+&pwm4 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_pwm4>;
+	status = "okay";
+};
+
+&ssi1 {
+	fsl,mode = "i2s-slave";
+	status = "okay";
+};
+
+&uart1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_uart1>;
+	status = "okay";
+};
+
+&uart2 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_uart2>;
+	status = "okay";
+};
+
+&uart3 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_uart3>;
+	status = "okay";
+};
+
+&uart4 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_uart4>;
+	status = "okay";
+};
+
+&uart5 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_uart5>;
+	status = "okay";
+};
+
+&usbh1 {
+	dr_mode = "host";
+	disable-over-current;
+	status = "okay";
+};
+
+&usbotg {
+	vbus-supply = <&reg_usb_otg_vbus>;
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_usbotg>;
+	disable-over-current;
+	dr_mode = "otg";
+	status = "okay";
+};
+
+&usdhc2 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_usdhc2>;
+	cd-gpios = <&gpio1 4 0>;
+	wp-gpios = <&gpio1 2 0>;
+	vmmc-supply = <&reg_3p3v>;
+	status = "okay";
+};
+
+&usdhc3 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_usdhc3>;
+	cd-gpios = <&gpio7 0 0>;
+	wp-gpios = <&gpio7 1 0>;
+	vmmc-supply = <&reg_3p3v>;
+	status = "okay";
+};
+
+&usdhc4 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_usdhc4>;
+	vmmc-supply = <&reg_3p3v>;
+	non-removable;
+	status = "okay";
+};
+
+&iomuxc {
+	pinctrl-names = "default";
+
+	imx6-riotboard {
+		pinctrl_audmux: audmuxgrp {
+			fsl,pins = <
+				MX6QDL_PAD_CSI0_DAT7__AUD3_RXD		0x8000000
+				MX6QDL_PAD_CSI0_DAT4__AUD3_TXC		0x8000000
+				MX6QDL_PAD_CSI0_DAT5__AUD3_TXD		0x8000000
+				MX6QDL_PAD_CSI0_DAT6__AUD3_TXFS		0x8000000
+				MX6QDL_PAD_GPIO_0__CCM_CLKO1		0x130b0		/* CAM_MCLK */
+			>;
+		};
+
+		pinctrl_ecspi1: ecspi1grp {
+			fsl,pins = <
+				MX6QDL_PAD_EIM_D16__ECSPI1_SCLK		0x100b1
+				MX6QDL_PAD_EIM_D17__ECSPI1_MISO		0x100b1
+				MX6QDL_PAD_EIM_D18__ECSPI1_MOSI		0x100b1
+				MX6QDL_PAD_DISP0_DAT23__GPIO5_IO17	0x000b1		/* CS0 */
+			>;
+		};
+
+		pinctrl_ecspi2: ecspi2grp {
+			fsl,pins = <
+				MX6QDL_PAD_DISP0_DAT15__GPIO5_IO09	0x000b1		/* CS1 */
+				MX6QDL_PAD_DISP0_DAT16__ECSPI2_MOSI	0x100b1
+				MX6QDL_PAD_DISP0_DAT17__ECSPI2_MISO	0x100b1
+				MX6QDL_PAD_DISP0_DAT18__GPIO5_IO12	0x000b1		/* CS0 */
+				MX6QDL_PAD_DISP0_DAT19__ECSPI2_SCLK	0x100b1
+			>;
+		};
+
+		pinctrl_ecspi3: ecspi3grp {
+			fsl,pins = <
+				MX6QDL_PAD_DISP0_DAT0__ECSPI3_SCLK	0x100b1
+				MX6QDL_PAD_DISP0_DAT1__ECSPI3_MOSI	0x100b1
+				MX6QDL_PAD_DISP0_DAT2__ECSPI3_MISO	0x100b1
+				MX6QDL_PAD_DISP0_DAT3__GPIO4_IO24	0x000b1		/* CS0 */
+				MX6QDL_PAD_DISP0_DAT4__GPIO4_IO25	0x000b1		/* CS1 */
+			>;
+		};
+
+		pinctrl_enet: enetgrp {
+			fsl,pins = <
+				MX6QDL_PAD_ENET_MDIO__ENET_MDIO		0x1b0b0
+				MX6QDL_PAD_ENET_MDC__ENET_MDC		0x1b0b0
+				MX6QDL_PAD_RGMII_TXC__RGMII_TXC		0x80000000
+				MX6QDL_PAD_RGMII_TD0__RGMII_TD0		0x1b0b0
+				MX6QDL_PAD_RGMII_TD1__RGMII_TD1		0x1b0b0
+				MX6QDL_PAD_RGMII_TD2__RGMII_TD2		0x1b0b0
+				MX6QDL_PAD_RGMII_TD3__RGMII_TD3		0x1b0b0
+				MX6QDL_PAD_RGMII_TX_CTL__RGMII_TX_CTL	0x1b0b0
+				MX6QDL_PAD_ENET_REF_CLK__ENET_TX_CLK	0x0a0b1		/* AR8035 CLK_25M --> ENET_REF_CLK (V22) */
+				MX6QDL_PAD_RGMII_RXC__RGMII_RXC		0x1b0b0		/* AR8035 pin strapping: IO voltage: pull up */
+				MX6QDL_PAD_RGMII_RD0__RGMII_RD0		0x130b0		/* AR8035 pin strapping: PHYADDR#0: pull down */
+				MX6QDL_PAD_RGMII_RD1__RGMII_RD1		0x130b0		/* AR8035 pin strapping: PHYADDR#1: pull down */
+				MX6QDL_PAD_RGMII_RD2__RGMII_RD2		0x1b0b0		/* AR8035 pin strapping: MODE#1: pull up */
+				MX6QDL_PAD_RGMII_RD3__RGMII_RD3		0x1b0b0		/* AR8035 pin strapping: MODE#3: pull up */
+				MX6QDL_PAD_RGMII_RX_CTL__RGMII_RX_CTL	0x130b0		/* AR8035 pin strapping: MODE#0: pull down */
+				MX6QDL_PAD_GPIO_16__ENET_REF_CLK	0xc0000000	/* GPIO16 -> AR8035 25MHz */
+			        MX6QDL_PAD_EIM_D31__GPIO3_IO31		0x130b0		/* RGMII_nRST */
+				MX6QDL_PAD_ENET_TX_EN__GPIO1_IO28	0x80000000	/* AR8035 interrupt */
+				MX6QDL_PAD_GPIO_6__ENET_IRQ		0x000b1
+			>;
+		};
+
+		pinctrl_i2c1: i2c1grp {
+			fsl,pins = <
+				MX6QDL_PAD_CSI0_DAT8__I2C1_SDA		0x4001b8b1
+				MX6QDL_PAD_CSI0_DAT9__I2C1_SCL		0x4001b8b1
+			>;
+		};
+
+		pinctrl_i2c2: i2c2grp {
+			fsl,pins = <
+				MX6QDL_PAD_KEY_COL3__I2C2_SCL		0x4001b8b1
+				MX6QDL_PAD_KEY_ROW3__I2C2_SDA		0x4001b8b1
+			>;
+		};
+
+		pinctrl_i2c3: i2c3grp {
+			fsl,pins = <
+				MX6QDL_PAD_GPIO_5__I2C3_SCL		0x4001b8b1
+				MX6QDL_PAD_GPIO_6__I2C3_SDA		0x4001b8b1
+			>;
+		};
+
+		pinctrl_i2c4: i2c4grp {
+			fsl,pins = <
+				MX6QDL_PAD_GPIO_7__I2C4_SCL             0x4001b8b1
+				MX6QDL_PAD_GPIO_8__I2C4_SDA             0x4001b8b1
+			>;
+		};
+
+		pinctrl_led: ledgrp {
+			fsl,pins = <
+				MX6QDL_PAD_EIM_A25__GPIO5_IO02		0x80000000	/* user led0 */
+				MX6QDL_PAD_EIM_D28__GPIO3_IO28		0x80000000	/* user led1 */
+			>;
+		};
+
+		pinctrl_pwm1: pwm1grp {
+			fsl,pins = <
+				MX6QDL_PAD_DISP0_DAT8__PWM1_OUT		0x1b0b1
+			>;
+		};
+
+		pinctrl_pwm2: pwm2grp {
+			fsl,pins = <
+				MX6QDL_PAD_DISP0_DAT9__PWM2_OUT		0x1b0b1
+			>;
+		};
+
+		pinctrl_pwm3: pwm3grp {
+			fsl,pins = <
+				MX6QDL_PAD_SD1_DAT1__PWM3_OUT		0x1b0b1
+			>;
+		};
+
+		pinctrl_pwm4: pwm4grp {
+			fsl,pins = <
+				MX6QDL_PAD_SD1_CMD__PWM4_OUT		0x1b0b1
+			>;
+		};
+
+		pinctrl_uart1: uart1grp {
+			fsl,pins = <
+				MX6QDL_PAD_CSI0_DAT10__UART1_TX_DATA	0x1b0b1
+				MX6QDL_PAD_CSI0_DAT11__UART1_RX_DATA	0x1b0b1
+			>;
+		};
+
+		pinctrl_uart2: uart2grp {
+			fsl,pins = <
+				MX6QDL_PAD_EIM_D26__UART2_TX_DATA	0x1b0b1
+				MX6QDL_PAD_EIM_D27__UART2_RX_DATA	0x1b0b1
+			>;
+		};
+
+		pinctrl_uart3: uart3grp {
+			fsl,pins = <
+				MX6QDL_PAD_EIM_D24__UART3_TX_DATA	0x1b0b1
+				MX6QDL_PAD_EIM_D25__UART3_RX_DATA	0x1b0b1
+			>;
+		};
+
+		pinctrl_uart4: uart4grp {
+			fsl,pins = <
+				MX6QDL_PAD_KEY_COL0__UART4_TX_DATA	0x1b0b1
+				MX6QDL_PAD_KEY_ROW0__UART4_RX_DATA	0x1b0b1
+			>;
+		};
+
+		pinctrl_uart5: uart5grp {
+			fsl,pins = <
+				MX6QDL_PAD_KEY_COL1__UART5_TX_DATA	0x1b0b1
+				MX6QDL_PAD_KEY_ROW1__UART5_RX_DATA	0x1b0b1
+			>;
+		};
+
+		pinctrl_usbotg: usbotggrp {
+			fsl,pins = <
+				MX6QDL_PAD_ENET_RX_ER__USB_OTG_ID	0x17059
+				MX6QDL_PAD_EIM_D22__GPIO3_IO22		0x80000000	/* MX6QDL_PAD_EIM_D22__USB_OTG_PWR */
+				MX6QDL_PAD_EIM_D21__USB_OTG_OC		0x80000000
+			>;
+		};
+
+		pinctrl_usdhc2: usdhc2grp {
+			fsl,pins = <
+				MX6QDL_PAD_SD2_CMD__SD2_CMD		0x17059
+				MX6QDL_PAD_SD2_CLK__SD2_CLK		0x10059
+				MX6QDL_PAD_SD2_DAT0__SD2_DATA0		0x17059
+				MX6QDL_PAD_SD2_DAT1__SD2_DATA1		0x17059
+				MX6QDL_PAD_SD2_DAT2__SD2_DATA2		0x17059
+				MX6QDL_PAD_SD2_DAT3__SD2_DATA3		0x17059
+				MX6QDL_PAD_GPIO_4__GPIO1_IO04		0x80000000	/* SD2 CD */
+				MX6QDL_PAD_GPIO_2__GPIO1_IO02		0x80000000	/* SD2 WP */
+			>;
+		};
+
+		pinctrl_usdhc3: usdhc3grp {
+			fsl,pins = <
+				MX6QDL_PAD_SD3_CMD__SD3_CMD		0x17059
+				MX6QDL_PAD_SD3_CLK__SD3_CLK		0x10059
+				MX6QDL_PAD_SD3_DAT0__SD3_DATA0		0x17059
+				MX6QDL_PAD_SD3_DAT1__SD3_DATA1		0x17059
+				MX6QDL_PAD_SD3_DAT2__SD3_DATA2		0x17059
+				MX6QDL_PAD_SD3_DAT3__SD3_DATA3		0x17059
+				MX6QDL_PAD_SD3_DAT5__GPIO7_IO00		0x80000000	/* SD3 CD */
+				MX6QDL_PAD_SD3_DAT4__GPIO7_IO01		0x80000000	/* SD3 WP */
+			>;
+		};
+
+		pinctrl_usdhc4: usdhc4grp {
+			fsl,pins = <
+				MX6QDL_PAD_SD4_CMD__SD4_CMD		0x17059
+				MX6QDL_PAD_SD4_CLK__SD4_CLK		0x10059
+				MX6QDL_PAD_SD4_DAT0__SD4_DATA0		0x17059
+				MX6QDL_PAD_SD4_DAT1__SD4_DATA1		0x17059
+				MX6QDL_PAD_SD4_DAT2__SD4_DATA2		0x17059
+				MX6QDL_PAD_SD4_DAT3__SD4_DATA3		0x17059
+				MX6QDL_PAD_NANDF_ALE__GPIO6_IO08	0x80000000	/* SD4 RST (eMMC) */
+			>;
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/imx6dl.dtsi b/arch/arm/boot/dts/imx6dl.dtsi
index 5c5f574330f..0a9c49d69d4 100644
--- a/arch/arm/boot/dts/imx6dl.dtsi
+++ b/arch/arm/boot/dts/imx6dl.dtsi
@@ -84,9 +84,10 @@
 			i2c4: i2c@021f8000 {
 				#address-cells = <1>;
 				#size-cells = <0>;
-				compatible = "fsl,imx1-i2c";
+				compatible = "fsl,imx6q-i2c", "fsl,imx21-i2c";
 				reg = <0x021f8000 0x4000>;
 				interrupts = <0 35 IRQ_TYPE_LEVEL_HIGH>;
+				clocks = <&clks 116>;
 				status = "disabled";
 			};
 		};
diff --git a/arch/arm/boot/dts/imx6q-dmo-edmqmx6.dts b/arch/arm/boot/dts/imx6q-dmo-edmqmx6.dts
index a63bbb3d46b..e0302636aff 100644
--- a/arch/arm/boot/dts/imx6q-dmo-edmqmx6.dts
+++ b/arch/arm/boot/dts/imx6q-dmo-edmqmx6.dts
@@ -18,8 +18,15 @@
 	model = "Data Modul eDM-QMX6 Board";
 	compatible = "dmo,imx6q-edmqmx6", "fsl,imx6q";
 
+	chosen {
+		stdout-path = &uart2;
+	};
+
 	aliases {
-		gpio7 = &stmpe_gpio;
+		gpio7 = &stmpe_gpio1;
+		gpio8 = &stmpe_gpio2;
+		stmpe-i2c0 = &stmpe1;
+		stmpe-i2c1 = &stmpe2;
 	};
 
 	memory {
@@ -40,13 +47,15 @@
 			regulator-always-on;
 		};
 
-		reg_usb_otg_vbus: regulator@1 {
+		reg_usb_otg_switch: regulator@1 {
 			compatible = "regulator-fixed";
 			reg = <1>;
-			regulator-name = "usb_otg_vbus";
+			regulator-name = "usb_otg_switch";
 			regulator-min-microvolt = <5000000>;
 			regulator-max-microvolt = <5000000>;
 			gpio = <&gpio7 12 0>;
+			regulator-boot-on;
+			regulator-always-on;
 		};
 
 		reg_usb_host1: regulator@2 {
@@ -65,27 +74,41 @@
 
 		led-blue {
 			label = "blue";
-			gpios = <&stmpe_gpio 8 GPIO_ACTIVE_HIGH>;
+			gpios = <&stmpe_gpio1 8 GPIO_ACTIVE_HIGH>;
 			linux,default-trigger = "heartbeat";
 		};
 
 		led-green {
 			label = "green";
-			gpios = <&stmpe_gpio 9 GPIO_ACTIVE_HIGH>;
+			gpios = <&stmpe_gpio1 9 GPIO_ACTIVE_HIGH>;
 		};
 
 		led-pink {
 			label = "pink";
-			gpios = <&stmpe_gpio 10 GPIO_ACTIVE_HIGH>;
+			gpios = <&stmpe_gpio1 10 GPIO_ACTIVE_HIGH>;
 		};
 
 		led-red {
 			label = "red";
-			gpios = <&stmpe_gpio 11 GPIO_ACTIVE_HIGH>;
+			gpios = <&stmpe_gpio1 11 GPIO_ACTIVE_HIGH>;
 		};
 	};
 };
 
+&ecspi5 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_ecspi5>;
+	fsl,spi-num-chipselects = <1>;
+	cs-gpios = <&gpio1 12 0>;
+	status = "okay";
+
+	flash: m25p80@0 {
+		compatible = "m25p80";
+		spi-max-frequency = <40000000>;
+		reg = <0>;
+	};
+};
+
 &fec {
 	pinctrl-names = "default";
 	pinctrl-0 = <&pinctrl_enet>;
@@ -99,7 +122,9 @@
 	clock-frequency = <100000>;
 	pinctrl-names = "default";
 	pinctrl-0 = <&pinctrl_i2c2
-		     &pinctrl_stmpe>;
+		     &pinctrl_stmpe1
+		     &pinctrl_stmpe2
+		     &pinctrl_pfuze>;
 	status = "okay";
 
 	pmic: pfuze100@08 {
@@ -205,13 +230,29 @@
 		};
 	};
 
-	stmpe: stmpe1601@40 {
+	stmpe1: stmpe1601@40 {
 		compatible = "st,stmpe1601";
 		reg = <0x40>;
 		interrupts = <30 0>;
 		interrupt-parent = <&gpio3>;
+		vcc-supply = <&sw2_reg>;
+		vio-supply = <&sw2_reg>;
+
+		stmpe_gpio1: stmpe_gpio {
+			#gpio-cells = <2>;
+			compatible = "st,stmpe-gpio";
+		};
+	};
+
+	stmpe2: stmpe1601@44 {
+		compatible = "st,stmpe1601";
+		reg = <0x44>;
+		interrupts = <2 0>;
+		interrupt-parent = <&gpio5>;
+		vcc-supply = <&sw2_reg>;
+		vio-supply = <&sw2_reg>;
 
-		stmpe_gpio: stmpe_gpio {
+		stmpe_gpio2: stmpe_gpio {
 			#gpio-cells = <2>;
 			compatible = "st,stmpe-gpio";
 		};
@@ -245,6 +286,15 @@
 			>;
 		};
 
+		pinctrl_ecspi5: ecspi5rp-1 {
+			fsl,pins = <
+				MX6QDL_PAD_SD1_DAT0__ECSPI5_MISO	0x80000000
+				MX6QDL_PAD_SD1_CMD__ECSPI5_MOSI		0x80000000
+				MX6QDL_PAD_SD1_CLK__ECSPI5_SCLK		0x80000000
+				MX6QDL_PAD_SD2_DAT3__GPIO1_IO12		0x80000000
+			>;
+		};
+
 		pinctrl_enet: enetgrp {
 			fsl,pins = <
 				MX6QDL_PAD_RGMII_RXC__RGMII_RXC		0x1b0b0
@@ -273,10 +323,20 @@
 			>;
 		};
 
-		pinctrl_stmpe: stmpegrp {
+		pinctrl_pfuze: pfuze100grp1 {
+			fsl,pins = <
+				MX6QDL_PAD_EIM_D20__GPIO3_IO20		0x80000000
+			>;
+		};
+
+		pinctrl_stmpe1: stmpe1grp {
 			fsl,pins = <MX6QDL_PAD_EIM_D30__GPIO3_IO30 0x80000000>;
 		};
 
+		pinctrl_stmpe2: stmpe2grp {
+			fsl,pins = <MX6QDL_PAD_EIM_A25__GPIO5_IO02 0x80000000>;
+		};
+
 		pinctrl_uart1: uart1grp {
 			fsl,pins = <
 				MX6QDL_PAD_SD3_DAT7__UART1_TX_DATA	0x1b0b1
@@ -293,7 +353,7 @@
 
 		pinctrl_usbotg: usbotggrp {
 			fsl,pins = <
-				MX6QDL_PAD_GPIO_1__USB_OTG_ID		0x17059
+				MX6QDL_PAD_ENET_RX_ER__USB_OTG_ID	0x17059
 			>;
 		};
 
@@ -344,11 +404,11 @@
 &usbh1 {
 	vbus-supply = <&reg_usb_host1>;
 	disable-over-current;
+	dr_mode = "host";
 	status = "okay";
 };
 
 &usbotg {
-	vbus-supply = <&reg_usb_otg_vbus>;
 	pinctrl-names = "default";
 	pinctrl-0 = <&pinctrl_usbotg>;
 	disable-over-current;
diff --git a/arch/arm/boot/dts/imx6q-gk802.dts b/arch/arm/boot/dts/imx6q-gk802.dts
index 4a9b4dc9afc..703539cf36d 100644
--- a/arch/arm/boot/dts/imx6q-gk802.dts
+++ b/arch/arm/boot/dts/imx6q-gk802.dts
@@ -14,7 +14,7 @@
 	compatible = "zealz,imx6q-gk802", "fsl,imx6q";
 
 	chosen {
-		linux,stdout-path = &uart4;
+		stdout-path = &uart4;
 	};
 
 	memory {
@@ -48,6 +48,11 @@
 	};
 };
 
+&hdmi {
+	ddc-i2c-bus = <&i2c3>;
+	status = "okay";
+};
+
 /* Internal I2C */
 &i2c2 {
 	pinctrl-names = "default";
diff --git a/arch/arm/boot/dts/imx6q-gw5400-a.dts b/arch/arm/boot/dts/imx6q-gw5400-a.dts
index 902f9831048..3689eaa5882 100644
--- a/arch/arm/boot/dts/imx6q-gw5400-a.dts
+++ b/arch/arm/boot/dts/imx6q-gw5400-a.dts
@@ -157,6 +157,11 @@
 	status = "okay";
 };
 
+&hdmi {
+	ddc-i2c-bus = <&i2c3>;
+	status = "okay";
+};
+
 &i2c1 {
 	clock-frequency = <100000>;
 	pinctrl-names = "default";
@@ -487,9 +492,6 @@
 
 &ldb {
 	status = "okay";
-	lvds-channel@0 {
-		crtcs = <&ipu1 0>, <&ipu1 1>, <&ipu2 0>, <&ipu2 1>;
-	};
 };
 
 &pcie {
diff --git a/arch/arm/boot/dts/imx6q-phytec-pbab01.dts b/arch/arm/boot/dts/imx6q-phytec-pbab01.dts
index 5607c331fca..c139ac0ebe1 100644
--- a/arch/arm/boot/dts/imx6q-phytec-pbab01.dts
+++ b/arch/arm/boot/dts/imx6q-phytec-pbab01.dts
@@ -11,40 +11,17 @@
 
 /dts-v1/;
 #include "imx6q-phytec-pfla02.dtsi"
+#include "imx6qdl-phytec-pbab01.dtsi"
 
 / {
 	model = "Phytec phyFLEX-i.MX6 Quad Carrier-Board";
 	compatible = "phytec,imx6q-pbab01", "phytec,imx6q-pfla02", "fsl,imx6q";
-};
-
-&fec {
-	status = "okay";
-};
 
-&gpmi {
-	status = "okay";
+	chosen {
+		stdout-path = &uart4;
+	};
 };
 
 &sata {
-	status = "okay";
-};
-
-&uart4 {
-	status = "okay";
-};
-
-&usbh1 {
-	status = "okay";
-};
-
-&usbotg {
-	status = "okay";
-};
-
-&usdhc2 {
-	status = "okay";
-};
-
-&usdhc3 {
-	status = "okay";
+        status = "okay";
 };
diff --git a/arch/arm/boot/dts/imx6q-phytec-pfla02.dtsi b/arch/arm/boot/dts/imx6q-phytec-pfla02.dtsi
index 324f1550976..cd20d0a948d 100644
--- a/arch/arm/boot/dts/imx6q-phytec-pfla02.dtsi
+++ b/arch/arm/boot/dts/imx6q-phytec-pfla02.dtsi
@@ -10,316 +10,13 @@
  */
 
 #include "imx6q.dtsi"
+#include "imx6qdl-phytec-pfla02.dtsi"
 
 / {
-	model = "Phytec phyFLEX-i.MX6 Ouad";
+	model = "Phytec phyFLEX-i.MX6 Quad";
 	compatible = "phytec,imx6q-pfla02", "fsl,imx6q";
 
 	memory {
 		reg = <0x10000000 0x80000000>;
 	};
-
-	regulators {
-		compatible = "simple-bus";
-		#address-cells = <1>;
-		#size-cells = <0>;
-
-		reg_usb_otg_vbus: regulator@0 {
-			compatible = "regulator-fixed";
-			reg = <0>;
-			regulator-name = "usb_otg_vbus";
-			regulator-min-microvolt = <5000000>;
-			regulator-max-microvolt = <5000000>;
-			gpio = <&gpio4 15 0>;
-		};
-
-		reg_usb_h1_vbus: regulator@1 {
-			compatible = "regulator-fixed";
-			reg = <1>;
-			regulator-name = "usb_h1_vbus";
-			regulator-min-microvolt = <5000000>;
-			regulator-max-microvolt = <5000000>;
-			gpio = <&gpio1 0 0>;
-		};
-	};
-};
-
-&ecspi3 {
-	pinctrl-names = "default";
-	pinctrl-0 = <&pinctrl_ecspi3>;
-	status = "okay";
-	fsl,spi-num-chipselects = <1>;
-	cs-gpios = <&gpio4 24 0>;
-
-	flash@0 {
-		compatible = "m25p80";
-		spi-max-frequency = <20000000>;
-		reg = <0>;
-	};
-};
-
-&i2c1 {
-	pinctrl-names = "default";
-	pinctrl-0 = <&pinctrl_i2c1>;
-	status = "okay";
-
-	eeprom@50 {
-		compatible = "atmel,24c32";
-		reg = <0x50>;
-	};
-
-	pmic@58 {
-		compatible = "dialog,da9063";
-		reg = <0x58>;
-		interrupt-parent = <&gpio4>;
-		interrupts = <17 0x8>; /* active-low GPIO4_17 */
-
-		regulators {
-			vddcore_reg: bcore1 {
-				regulator-min-microvolt = <730000>;
-				regulator-max-microvolt = <1380000>;
-				regulator-always-on;
-			};
-
-			vddsoc_reg: bcore2 {
-				regulator-min-microvolt = <730000>;
-				regulator-max-microvolt = <1380000>;
-				regulator-always-on;
-			};
-
-			vdd_ddr3_reg: bpro {
-				regulator-min-microvolt = <1500000>;
-				regulator-max-microvolt = <1500000>;
-				regulator-always-on;
-			};
-
-			vdd_3v3_reg: bperi {
-				regulator-min-microvolt = <3300000>;
-				regulator-max-microvolt = <3300000>;
-				regulator-always-on;
-			};
-
-			vdd_buckmem_reg: bmem {
-				regulator-min-microvolt = <3300000>;
-				regulator-max-microvolt = <3300000>;
-				regulator-always-on;
-			};
-
-			vdd_eth_reg: bio {
-				regulator-min-microvolt = <1200000>;
-				regulator-max-microvolt = <1200000>;
-				regulator-always-on;
-			};
-
-			vdd_eth_io_reg: ldo4 {
-				regulator-min-microvolt = <2500000>;
-				regulator-max-microvolt = <2500000>;
-				regulator-always-on;
-			};
-
-			vdd_mx6_snvs_reg: ldo5 {
-				regulator-min-microvolt = <3000000>;
-				regulator-max-microvolt = <3000000>;
-				regulator-always-on;
-			};
-
-			vdd_3v3_pmic_io_reg: ldo6 {
-				regulator-min-microvolt = <3300000>;
-				regulator-max-microvolt = <3300000>;
-				regulator-always-on;
-			};
-
-			vdd_sd0_reg: ldo9 {
-				regulator-min-microvolt = <3300000>;
-				regulator-max-microvolt = <3300000>;
-			};
-
-			vdd_sd1_reg: ldo10 {
-				regulator-min-microvolt = <3300000>;
-				regulator-max-microvolt = <3300000>;
-			};
-
-			vdd_mx6_high_reg: ldo11 {
-				regulator-min-microvolt = <3000000>;
-				regulator-max-microvolt = <3000000>;
-				regulator-always-on;
-			};
-		};
-	};
-};
-
-&iomuxc {
-	pinctrl-names = "default";
-	pinctrl-0 = <&pinctrl_hog>;
-
-	imx6q-phytec-pfla02 {
-		pinctrl_hog: hoggrp {
-			fsl,pins = <
-				MX6QDL_PAD_EIM_D23__GPIO3_IO23 0x80000000
-				MX6QDL_PAD_DISP0_DAT3__GPIO4_IO24 0x80000000 /* SPI NOR chipselect */
-				MX6QDL_PAD_DI0_PIN15__GPIO4_IO17  0x80000000 /* PMIC interrupt */
-			>;
-		};
-
-		pinctrl_ecspi3: ecspi3grp {
-			fsl,pins = <
-				MX6QDL_PAD_DISP0_DAT2__ECSPI3_MISO	0x100b1
-				MX6QDL_PAD_DISP0_DAT1__ECSPI3_MOSI	0x100b1
-				MX6QDL_PAD_DISP0_DAT0__ECSPI3_SCLK	0x100b1
-			>;
-		};
-
-		pinctrl_enet: enetgrp {
-			fsl,pins = <
-				MX6QDL_PAD_ENET_MDIO__ENET_MDIO		0x1b0b0
-				MX6QDL_PAD_ENET_MDC__ENET_MDC		0x1b0b0
-				MX6QDL_PAD_RGMII_TXC__RGMII_TXC		0x1b0b0
-				MX6QDL_PAD_RGMII_TD0__RGMII_TD0		0x1b0b0
-				MX6QDL_PAD_RGMII_TD1__RGMII_TD1		0x1b0b0
-				MX6QDL_PAD_RGMII_TD2__RGMII_TD2		0x1b0b0
-				MX6QDL_PAD_RGMII_TD3__RGMII_TD3		0x1b0b0
-				MX6QDL_PAD_RGMII_TX_CTL__RGMII_TX_CTL	0x1b0b0
-				MX6QDL_PAD_ENET_REF_CLK__ENET_TX_CLK	0x1b0b0
-				MX6QDL_PAD_RGMII_RXC__RGMII_RXC		0x1b0b0
-				MX6QDL_PAD_RGMII_RD0__RGMII_RD0		0x1b0b0
-				MX6QDL_PAD_RGMII_RD1__RGMII_RD1		0x1b0b0
-				MX6QDL_PAD_RGMII_RD2__RGMII_RD2		0x1b0b0
-				MX6QDL_PAD_RGMII_RD3__RGMII_RD3		0x1b0b0
-				MX6QDL_PAD_RGMII_RX_CTL__RGMII_RX_CTL	0x1b0b0
-				MX6QDL_PAD_ENET_TX_EN__ENET_TX_EN	0x1b0b0
-			>;
-		};
-
-		pinctrl_gpmi_nand: gpminandgrp {
-			fsl,pins = <
-				MX6QDL_PAD_NANDF_CLE__NAND_CLE		0xb0b1
-				MX6QDL_PAD_NANDF_ALE__NAND_ALE		0xb0b1
-				MX6QDL_PAD_NANDF_WP_B__NAND_WP_B	0xb0b1
-				MX6QDL_PAD_NANDF_RB0__NAND_READY_B	0xb000
-				MX6QDL_PAD_NANDF_CS0__NAND_CE0_B	0xb0b1
-				MX6QDL_PAD_NANDF_CS1__NAND_CE1_B	0xb0b1
-				MX6QDL_PAD_SD4_CMD__NAND_RE_B		0xb0b1
-				MX6QDL_PAD_SD4_CLK__NAND_WE_B		0xb0b1
-				MX6QDL_PAD_NANDF_D0__NAND_DATA00	0xb0b1
-				MX6QDL_PAD_NANDF_D1__NAND_DATA01	0xb0b1
-				MX6QDL_PAD_NANDF_D2__NAND_DATA02	0xb0b1
-				MX6QDL_PAD_NANDF_D3__NAND_DATA03	0xb0b1
-				MX6QDL_PAD_NANDF_D4__NAND_DATA04	0xb0b1
-				MX6QDL_PAD_NANDF_D5__NAND_DATA05	0xb0b1
-				MX6QDL_PAD_NANDF_D6__NAND_DATA06	0xb0b1
-				MX6QDL_PAD_NANDF_D7__NAND_DATA07	0xb0b1
-				MX6QDL_PAD_SD4_DAT0__NAND_DQS		0x00b1
-			>;
-		};
-
-		pinctrl_i2c1: i2c1grp {
-			fsl,pins = <
-				MX6QDL_PAD_EIM_D21__I2C1_SCL		0x4001b8b1
-				MX6QDL_PAD_EIM_D28__I2C1_SDA		0x4001b8b1
-			>;
-		};
-
-		pinctrl_uart4: uart4grp {
-			fsl,pins = <
-				MX6QDL_PAD_KEY_COL0__UART4_TX_DATA	0x1b0b1
-				MX6QDL_PAD_KEY_ROW0__UART4_RX_DATA	0x1b0b1
-			>;
-		};
-
-		pinctrl_usbh1: usbh1grp {
-			fsl,pins = <
-				MX6QDL_PAD_GPIO_0__USB_H1_PWR		0x80000000
-			>;
-		};
-
-		pinctrl_usbotg: usbotggrp {
-			fsl,pins = <
-				MX6QDL_PAD_GPIO_1__USB_OTG_ID		0x17059
-				MX6QDL_PAD_KEY_COL4__USB_OTG_OC		0x1b0b0
-				MX6QDL_PAD_KEY_ROW4__GPIO4_IO15		0x80000000
-			>;
-		};
-
-		pinctrl_usdhc2: usdhc2grp {
-			fsl,pins = <
-				MX6QDL_PAD_SD2_CMD__SD2_CMD		0x17059
-				MX6QDL_PAD_SD2_CLK__SD2_CLK		0x10059
-				MX6QDL_PAD_SD2_DAT0__SD2_DATA0		0x17059
-				MX6QDL_PAD_SD2_DAT1__SD2_DATA1		0x17059
-				MX6QDL_PAD_SD2_DAT2__SD2_DATA2		0x17059
-				MX6QDL_PAD_SD2_DAT3__SD2_DATA3		0x17059
-			>;
-		};
-
-		pinctrl_usdhc3: usdhc3grp {
-			fsl,pins = <
-				MX6QDL_PAD_SD3_CMD__SD3_CMD		0x17059
-				MX6QDL_PAD_SD3_CLK__SD3_CLK		0x10059
-				MX6QDL_PAD_SD3_DAT0__SD3_DATA0		0x17059
-				MX6QDL_PAD_SD3_DAT1__SD3_DATA1		0x17059
-				MX6QDL_PAD_SD3_DAT2__SD3_DATA2		0x17059
-				MX6QDL_PAD_SD3_DAT3__SD3_DATA3		0x17059
-			>;
-		};
-
-		pinctrl_usdhc3_cdwp: usdhc3cdwp {
-			fsl,pins = <
-				MX6QDL_PAD_ENET_RXD0__GPIO1_IO27 0x80000000
-				MX6QDL_PAD_ENET_TXD1__GPIO1_IO29 0x80000000
-			>;
-		};
-	};
-};
-
-&fec {
-	pinctrl-names = "default";
-	pinctrl-0 = <&pinctrl_enet>;
-	phy-mode = "rgmii";
-	phy-reset-gpios = <&gpio3 23 0>;
-	status = "disabled";
-};
-
-&gpmi {
-	pinctrl-names = "default";
-	pinctrl-0 = <&pinctrl_gpmi_nand>;
-	nand-on-flash-bbt;
-	status = "disabled";
-};
-
-&uart4 {
-	pinctrl-names = "default";
-	pinctrl-0 = <&pinctrl_uart4>;
-	status = "disabled";
-};
-
-&usbh1 {
-	vbus-supply = <&reg_usb_h1_vbus>;
-	pinctrl-names = "default";
-	pinctrl-0 = <&pinctrl_usbh1>;
-	status = "disabled";
-};
-
-&usbotg {
-	vbus-supply = <&reg_usb_otg_vbus>;
-	pinctrl-names = "default";
-	pinctrl-0 = <&pinctrl_usbotg>;
-	disable-over-current;
-	status = "disabled";
-};
-
-&usdhc2 {
-	pinctrl-names = "default";
-	pinctrl-0 = <&pinctrl_usdhc2>;
-	cd-gpios = <&gpio1 4 0>;
-	wp-gpios = <&gpio1 2 0>;
-	status = "disabled";
-};
-
-&usdhc3 {
-        pinctrl-names = "default";
-        pinctrl-0 = <&pinctrl_usdhc3
-		     &pinctrl_usdhc3_cdwp>;
-        cd-gpios = <&gpio1 27 0>;
-        wp-gpios = <&gpio1 29 0>;
-        status = "disabled";
 };
diff --git a/arch/arm/boot/dts/imx6q-udoo.dts b/arch/arm/boot/dts/imx6q-udoo.dts
index ed397d149ab..6c561060bf5 100644
--- a/arch/arm/boot/dts/imx6q-udoo.dts
+++ b/arch/arm/boot/dts/imx6q-udoo.dts
@@ -16,6 +16,10 @@
 	model = "Udoo i.MX6 Quad Board";
 	compatible = "udoo,imx6q-udoo", "fsl,imx6q";
 
+	chosen {
+		stdout-path = &uart2;
+	};
+
 	memory {
 		reg = <0x10000000 0x40000000>;
 	};
@@ -28,6 +32,18 @@
 	status = "okay";
 };
 
+&hdmi {
+	ddc-i2c-bus = <&i2c2>;
+	status = "okay";
+};
+
+&i2c2 {
+	clock-frequency = <100000>;
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_i2c2>;
+	status = "okay";
+};
+
 &iomuxc {
 	imx6q-udoo {
 		pinctrl_enet: enetgrp {
@@ -51,6 +67,13 @@
 			>;
 		};
 
+		pinctrl_i2c2: i2c2grp {
+			fsl,pins = <
+				MX6QDL_PAD_KEY_COL3__I2C2_SCL		0x4001b8b1
+				MX6QDL_PAD_KEY_ROW3__I2C2_SDA		0x4001b8b1
+			>;
+		};
+
 		pinctrl_uart2: uart2grp {
 			fsl,pins = <
 				MX6QDL_PAD_EIM_D26__UART2_TX_DATA	0x1b0b1
diff --git a/arch/arm/boot/dts/imx6qdl-cubox-i.dtsi b/arch/arm/boot/dts/imx6qdl-cubox-i.dtsi
index c2a24888a27..25da82a0311 100644
--- a/arch/arm/boot/dts/imx6qdl-cubox-i.dtsi
+++ b/arch/arm/boot/dts/imx6qdl-cubox-i.dtsi
@@ -55,6 +55,20 @@
 	};
 };
 
+&hdmi {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_cubox_i_hdmi>;
+	ddc-i2c-bus = <&i2c2>;
+	status = "okay";
+};
+
+&i2c2 {
+	clock-frequency = <100000>;
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_cubox_i_i2c2>;
+	status = "okay";
+};
+
 &i2c3 {
 	pinctrl-names = "default";
 	pinctrl-0 = <&pinctrl_cubox_i_i2c3>;
@@ -69,6 +83,19 @@
 
 &iomuxc {
 	cubox_i {
+		pinctrl_cubox_i_hdmi: cubox-i-hdmi {
+			fsl,pins = <
+				MX6QDL_PAD_KEY_ROW2__HDMI_TX_CEC_LINE 0x1f8b0
+			>;
+		};
+
+		pinctrl_cubox_i_i2c2: cubox-i-i2c2 {
+			fsl,pins = <
+				MX6QDL_PAD_KEY_COL3__I2C2_SCL 0x4001b8b1
+				MX6QDL_PAD_KEY_ROW3__I2C2_SDA 0x4001b8b1
+			>;
+		};
+
 		pinctrl_cubox_i_i2c3: cubox-i-i2c3 {
 			fsl,pins = <
 				MX6QDL_PAD_EIM_D17__I2C3_SCL 0x4001b8b1
diff --git a/arch/arm/boot/dts/imx6qdl-dfi-fs700-m60.dtsi b/arch/arm/boot/dts/imx6qdl-dfi-fs700-m60.dtsi
index 25cf035dd36..2c253d6d20b 100644
--- a/arch/arm/boot/dts/imx6qdl-dfi-fs700-m60.dtsi
+++ b/arch/arm/boot/dts/imx6qdl-dfi-fs700-m60.dtsi
@@ -22,7 +22,7 @@
 	};
 
 	chosen {
-		linux,stdout-path = &uart1;
+		stdout-path = &uart1;
 	};
 };
 
diff --git a/arch/arm/boot/dts/imx6qdl-gw51xx.dtsi b/arch/arm/boot/dts/imx6qdl-gw51xx.dtsi
index 98a422153ce..31665adcbf3 100644
--- a/arch/arm/boot/dts/imx6qdl-gw51xx.dtsi
+++ b/arch/arm/boot/dts/imx6qdl-gw51xx.dtsi
@@ -101,6 +101,11 @@
 	status = "okay";
 };
 
+&hdmi {
+	ddc-i2c-bus = <&i2c3>;
+	status = "okay";
+};
+
 &i2c1 {
 	clock-frequency = <100000>;
 	pinctrl-names = "default";
diff --git a/arch/arm/boot/dts/imx6qdl-gw52xx.dtsi b/arch/arm/boot/dts/imx6qdl-gw52xx.dtsi
index 8e99c9a9bc7..367af3ec943 100644
--- a/arch/arm/boot/dts/imx6qdl-gw52xx.dtsi
+++ b/arch/arm/boot/dts/imx6qdl-gw52xx.dtsi
@@ -27,6 +27,13 @@
 		bootargs = "console=ttymxc1,115200";
 	};
 
+	backlight {
+		compatible = "pwm-backlight";
+		pwms = <&pwm4 0 5000000>;
+		brightness-levels = <0 4 8 16 32 64 128 255>;
+		default-brightness-level = <7>;
+	};
+
 	leds {
 		compatible = "gpio-leds";
 
@@ -148,6 +155,11 @@
 	status = "okay";
 };
 
+&hdmi {
+	ddc-i2c-bus = <&i2c3>;
+	status = "okay";
+};
+
 &i2c1 {
 	clock-frequency = <100000>;
 	pinctrl-names = "default";
@@ -394,6 +406,12 @@
 			>;
 		};
 
+		pinctrl_pwm4: pwm4grp {
+			fsl,pins = <
+				MX6QDL_PAD_SD1_CMD__PWM4_OUT		0x1b0b1
+			>;
+		};
+
 		pinctrl_uart1: uart1grp {
 			fsl,pins = <
 				MX6QDL_PAD_SD3_DAT7__UART1_TX_DATA	0x1b0b1
@@ -436,8 +454,26 @@
 
 &ldb {
 	status = "okay";
+
 	lvds-channel@0 {
-		crtcs = <&ipu1 0>, <&ipu1 1>;
+		fsl,data-mapping = "spwg";
+		fsl,data-width = <18>;
+		status = "okay";
+
+		display-timings {
+			native-mode = <&timing0>;
+			timing0: hsd100pxn1 {
+				clock-frequency = <65000000>;
+				hactive = <1024>;
+				vactive = <768>;
+				hback-porch = <220>;
+				hfront-porch = <40>;
+				vback-porch = <21>;
+				vfront-porch = <7>;
+				hsync-len = <60>;
+				vsync-len = <10>;
+			};
+		};
 	};
 };
 
@@ -446,6 +482,12 @@
 	status = "okay";
 };
 
+&pwm4 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_pwm4>;
+	status = "okay";
+};
+
 &ssi1 {
 	fsl,mode = "i2s-slave";
 	status = "okay";
diff --git a/arch/arm/boot/dts/imx6qdl-gw53xx.dtsi b/arch/arm/boot/dts/imx6qdl-gw53xx.dtsi
index c8e5ae06dea..c91b5a6c769 100644
--- a/arch/arm/boot/dts/imx6qdl-gw53xx.dtsi
+++ b/arch/arm/boot/dts/imx6qdl-gw53xx.dtsi
@@ -30,6 +30,13 @@
 		bootargs = "console=ttymxc1,115200";
 	};
 
+	backlight {
+		compatible = "pwm-backlight";
+		pwms = <&pwm4 0 5000000>;
+		brightness-levels = <0 4 8 16 32 64 128 255>;
+		default-brightness-level = <7>;
+	};
+
 	leds {
 		compatible = "gpio-leds";
 
@@ -157,6 +164,11 @@
 	status = "okay";
 };
 
+&hdmi {
+	ddc-i2c-bus = <&i2c3>;
+	status = "okay";
+};
+
 &i2c1 {
 	clock-frequency = <100000>;
 	pinctrl-names = "default";
@@ -434,6 +446,12 @@
 			>;
 		};
 
+		pinctrl_pwm4: pwm4grp {
+			fsl,pins = <
+				MX6QDL_PAD_SD1_CMD__PWM4_OUT		0x1b0b1
+			>;
+		};
+
 		pinctrl_uart1: uart1grp {
 			fsl,pins = <
 				MX6QDL_PAD_SD3_DAT7__UART1_TX_DATA	0x1b0b1
@@ -508,6 +526,12 @@
 	};
 };
 
+&pwm4 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_pwm4>;
+	status = "okay";
+};
+
 &ssi1 {
 	fsl,mode = "i2s-slave";
 	status = "okay";
diff --git a/arch/arm/boot/dts/imx6qdl-gw54xx.dtsi b/arch/arm/boot/dts/imx6qdl-gw54xx.dtsi
index 2795dfc8c92..698d3063b29 100644
--- a/arch/arm/boot/dts/imx6qdl-gw54xx.dtsi
+++ b/arch/arm/boot/dts/imx6qdl-gw54xx.dtsi
@@ -30,6 +30,13 @@
 		bootargs = "console=ttymxc1,115200";
 	};
 
+	backlight {
+		compatible = "pwm-backlight";
+		pwms = <&pwm4 0 5000000>;
+		brightness-levels = <0 4 8 16 32 64 128 255>;
+		default-brightness-level = <7>;
+	};
+
 	leds {
 		compatible = "gpio-leds";
 
@@ -147,6 +154,11 @@
 	status = "okay";
 };
 
+&hdmi {
+	ddc-i2c-bus = <&i2c3>;
+	status = "okay";
+};
+
 &i2c1 {
 	clock-frequency = <100000>;
 	pinctrl-names = "default";
@@ -456,6 +468,12 @@
 			>;
 		};
 
+		pinctrl_pwm4: pwm4grp {
+			fsl,pins = <
+				MX6QDL_PAD_SD1_CMD__PWM4_OUT		0x1b0b1
+			>;
+		};
+
 		pinctrl_uart1: uart1grp {
 			fsl,pins = <
 				MX6QDL_PAD_SD3_DAT7__UART1_TX_DATA	0x1b0b1
@@ -530,6 +548,12 @@
 	};
 };
 
+&pwm4 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_pwm4>;
+	status = "okay";
+};
+
 &ssi1 {
 	fsl,mode = "i2s-slave";
 	status = "okay";
diff --git a/arch/arm/boot/dts/imx6qdl-microsom-ar8035.dtsi b/arch/arm/boot/dts/imx6qdl-microsom-ar8035.dtsi
index a3cb2fff8f6..d16066608e2 100644
--- a/arch/arm/boot/dts/imx6qdl-microsom-ar8035.dtsi
+++ b/arch/arm/boot/dts/imx6qdl-microsom-ar8035.dtsi
@@ -26,25 +26,25 @@
 				/* GPIO16 -> AR8035 25MHz */
 				MX6QDL_PAD_GPIO_16__ENET_REF_CLK	0xc0000000
 				MX6QDL_PAD_RGMII_TXC__RGMII_TXC		0x80000000
-				MX6QDL_PAD_RGMII_TD0__RGMII_TD0		0x1b0b0
-				MX6QDL_PAD_RGMII_TD1__RGMII_TD1		0x1b0b0
-				MX6QDL_PAD_RGMII_TD2__RGMII_TD2		0x1b0b0
-				MX6QDL_PAD_RGMII_TD3__RGMII_TD3		0x1b0b0
-				MX6QDL_PAD_RGMII_TX_CTL__RGMII_TX_CTL	0x1b0b0
+				MX6QDL_PAD_RGMII_TD0__RGMII_TD0		0x1b030
+				MX6QDL_PAD_RGMII_TD1__RGMII_TD1		0x1b030
+				MX6QDL_PAD_RGMII_TD2__RGMII_TD2		0x1b030
+				MX6QDL_PAD_RGMII_TD3__RGMII_TD3		0x1b030
+				MX6QDL_PAD_RGMII_TX_CTL__RGMII_TX_CTL	0x1b030
 				/* AR8035 CLK_25M --> ENET_REF_CLK (V22) */
 				MX6QDL_PAD_ENET_REF_CLK__ENET_TX_CLK	0x0a0b1
 				/* AR8035 pin strapping: IO voltage: pull up */
-				MX6QDL_PAD_RGMII_RXC__RGMII_RXC		0x1b0b0
+				MX6QDL_PAD_RGMII_RXC__RGMII_RXC		0x1b030
 				/* AR8035 pin strapping: PHYADDR#0: pull down */
-				MX6QDL_PAD_RGMII_RD0__RGMII_RD0		0x130b0
+				MX6QDL_PAD_RGMII_RD0__RGMII_RD0		0x13030
 				/* AR8035 pin strapping: PHYADDR#1: pull down */
-				MX6QDL_PAD_RGMII_RD1__RGMII_RD1		0x130b0
+				MX6QDL_PAD_RGMII_RD1__RGMII_RD1		0x13030
 				/* AR8035 pin strapping: MODE#1: pull up */
-				MX6QDL_PAD_RGMII_RD2__RGMII_RD2		0x1b0b0
+				MX6QDL_PAD_RGMII_RD2__RGMII_RD2		0x1b030
 				/* AR8035 pin strapping: MODE#3: pull up */
-				MX6QDL_PAD_RGMII_RD3__RGMII_RD3		0x1b0b0
+				MX6QDL_PAD_RGMII_RD3__RGMII_RD3		0x1b030
 				/* AR8035 pin strapping: MODE#0: pull down */
-				MX6QDL_PAD_RGMII_RX_CTL__RGMII_RX_CTL	0x130b0
+				MX6QDL_PAD_RGMII_RX_CTL__RGMII_RX_CTL	0x13030
 
 				/*
 				 * As the RMII pins are also connected to RGMII
diff --git a/arch/arm/boot/dts/imx6qdl-nitrogen6x.dtsi b/arch/arm/boot/dts/imx6qdl-nitrogen6x.dtsi
index 99be301b523..4c4b17596c8 100644
--- a/arch/arm/boot/dts/imx6qdl-nitrogen6x.dtsi
+++ b/arch/arm/boot/dts/imx6qdl-nitrogen6x.dtsi
@@ -14,6 +14,10 @@
 #include <dt-bindings/input/input.h>
 
 / {
+	chosen {
+		stdout-path = &uart2;
+	};
+
 	memory {
 		reg = <0x10000000 0x40000000>;
 	};
diff --git a/arch/arm/boot/dts/imx6qdl-phytec-pbab01.dtsi b/arch/arm/boot/dts/imx6qdl-phytec-pbab01.dtsi
new file mode 100644
index 00000000000..58472126412
--- /dev/null
+++ b/arch/arm/boot/dts/imx6qdl-phytec-pbab01.dtsi
@@ -0,0 +1,102 @@
+/*
+ * Copyright 2013 Christian Hemp, Phytec Messtechnik GmbH
+ *
+ * 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
+ */
+
+/ {
+	chosen {
+		linux,stdout-path = &uart4;
+	};
+};
+
+&fec {
+	status = "okay";
+};
+
+&gpmi {
+	status = "okay";
+};
+
+&hdmi {
+	status = "okay";
+};
+
+&i2c2 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_i2c2>;
+	clock-frequency = <100000>;
+	status = "okay";
+
+	tlv320@18 {
+		compatible = "ti,tlv320aic3x";
+		reg = <0x18>;
+	};
+
+	stmpe@41 {
+		compatible = "st,stmpe811";
+		reg = <0x41>;
+	};
+
+	rtc@51 {
+		compatible = "nxp,rtc8564";
+		reg = <0x51>;
+	};
+
+	adc@64 {
+		compatible = "maxim,max1037";
+		reg = <0x64>;
+	};
+};
+
+&i2c3 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_i2c3>;
+	clock-frequency = <100000>;
+	status = "okay";
+};
+
+&uart3 {
+	status = "okay";
+};
+
+&uart4 {
+	status = "okay";
+};
+
+&usbh1 {
+	status = "okay";
+};
+
+&usbotg {
+	status = "okay";
+};
+
+&usdhc2 {
+	status = "okay";
+};
+
+&usdhc3 {
+	status = "okay";
+};
+
+&iomuxc {
+	pinctrl_i2c2: i2c2grp {
+		fsl,pins = <
+			MX6QDL_PAD_EIM_EB2__I2C2_SCL		0x4001b8b1
+			MX6QDL_PAD_EIM_D16__I2C2_SDA		0x4001b8b1
+		>;
+	};
+
+	pinctrl_i2c3: i2c3grp {
+		fsl,pins = <
+			MX6QDL_PAD_EIM_D17__I2C3_SCL		0x4001b8b1
+			MX6QDL_PAD_EIM_D18__I2C3_SDA		0x4001b8b1
+		>;
+	};
+};
diff --git a/arch/arm/boot/dts/imx6qdl-phytec-pfla02.dtsi b/arch/arm/boot/dts/imx6qdl-phytec-pfla02.dtsi
new file mode 100644
index 00000000000..faa3494a69d
--- /dev/null
+++ b/arch/arm/boot/dts/imx6qdl-phytec-pfla02.dtsi
@@ -0,0 +1,356 @@
+/*
+ * Copyright 2013 Christian Hemp, Phytec Messtechnik GmbH
+ *
+ * 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 <dt-bindings/gpio/gpio.h>
+
+/ {
+	model = "Phytec phyFLEX-i.MX6 Ouad";
+	compatible = "phytec,imx6q-pfla02", "fsl,imx6q";
+
+	memory {
+		reg = <0x10000000 0x80000000>;
+	};
+
+	regulators {
+		compatible = "simple-bus";
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		reg_usb_otg_vbus: regulator@0 {
+			compatible = "regulator-fixed";
+			reg = <0>;
+			regulator-name = "usb_otg_vbus";
+			regulator-min-microvolt = <5000000>;
+			regulator-max-microvolt = <5000000>;
+			gpio = <&gpio4 15 0>;
+		};
+
+		reg_usb_h1_vbus: regulator@1 {
+			compatible = "regulator-fixed";
+			reg = <1>;
+			regulator-name = "usb_h1_vbus";
+			regulator-min-microvolt = <5000000>;
+			regulator-max-microvolt = <5000000>;
+			gpio = <&gpio1 0 0>;
+		};
+	};
+
+	gpio_leds: leds {
+		compatible = "gpio-leds";
+
+		green {
+			label = "phyflex:green";
+			gpios = <&gpio1 30 0>;
+		};
+
+		red {
+			label = "phyflex:red";
+			gpios = <&gpio2 31 0>;
+		};
+	};
+};
+
+&ecspi3 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_ecspi3>;
+	status = "okay";
+	fsl,spi-num-chipselects = <1>;
+	cs-gpios = <&gpio4 24 0>;
+
+	flash@0 {
+		compatible = "m25p80";
+		spi-max-frequency = <20000000>;
+		reg = <0>;
+	};
+};
+
+&i2c1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_i2c1>;
+	status = "okay";
+
+	eeprom@50 {
+		compatible = "atmel,24c32";
+		reg = <0x50>;
+	};
+
+	pmic@58 {
+		compatible = "dialog,da9063";
+		reg = <0x58>;
+		interrupt-parent = <&gpio4>;
+		interrupts = <17 0x8>; /* active-low GPIO4_17 */
+
+		regulators {
+			vddcore_reg: bcore1 {
+				regulator-min-microvolt = <730000>;
+				regulator-max-microvolt = <1380000>;
+				regulator-always-on;
+			};
+
+			vddsoc_reg: bcore2 {
+				regulator-min-microvolt = <730000>;
+				regulator-max-microvolt = <1380000>;
+				regulator-always-on;
+			};
+
+			vdd_ddr3_reg: bpro {
+				regulator-min-microvolt = <1500000>;
+				regulator-max-microvolt = <1500000>;
+				regulator-always-on;
+			};
+
+			vdd_3v3_reg: bperi {
+				regulator-min-microvolt = <3300000>;
+				regulator-max-microvolt = <3300000>;
+				regulator-always-on;
+			};
+
+			vdd_buckmem_reg: bmem {
+				regulator-min-microvolt = <3300000>;
+				regulator-max-microvolt = <3300000>;
+				regulator-always-on;
+			};
+
+			vdd_eth_reg: bio {
+				regulator-min-microvolt = <1200000>;
+				regulator-max-microvolt = <1200000>;
+				regulator-always-on;
+			};
+
+			vdd_eth_io_reg: ldo4 {
+				regulator-min-microvolt = <2500000>;
+				regulator-max-microvolt = <2500000>;
+				regulator-always-on;
+			};
+
+			vdd_mx6_snvs_reg: ldo5 {
+				regulator-min-microvolt = <3000000>;
+				regulator-max-microvolt = <3000000>;
+				regulator-always-on;
+			};
+
+			vdd_3v3_pmic_io_reg: ldo6 {
+				regulator-min-microvolt = <3300000>;
+				regulator-max-microvolt = <3300000>;
+				regulator-always-on;
+			};
+
+			vdd_sd0_reg: ldo9 {
+				regulator-min-microvolt = <3300000>;
+				regulator-max-microvolt = <3300000>;
+			};
+
+			vdd_sd1_reg: ldo10 {
+				regulator-min-microvolt = <3300000>;
+				regulator-max-microvolt = <3300000>;
+			};
+
+			vdd_mx6_high_reg: ldo11 {
+				regulator-min-microvolt = <3000000>;
+				regulator-max-microvolt = <3000000>;
+				regulator-always-on;
+			};
+		};
+	};
+};
+
+&iomuxc {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_hog>;
+
+	imx6q-phytec-pfla02 {
+		pinctrl_hog: hoggrp {
+			fsl,pins = <
+				MX6QDL_PAD_EIM_D23__GPIO3_IO23 0x80000000
+				MX6QDL_PAD_DISP0_DAT3__GPIO4_IO24 0x80000000 /* SPI NOR chipselect */
+				MX6QDL_PAD_DI0_PIN15__GPIO4_IO17  0x80000000 /* PMIC interrupt */
+				MX6QDL_PAD_ENET_TXD0__GPIO1_IO30 0x80000000 /* Green LED */
+				MX6QDL_PAD_EIM_EB3__GPIO2_IO31 0x80000000 /* Red LED */
+			>;
+		};
+
+		pinctrl_ecspi3: ecspi3grp {
+			fsl,pins = <
+				MX6QDL_PAD_DISP0_DAT2__ECSPI3_MISO	0x100b1
+				MX6QDL_PAD_DISP0_DAT1__ECSPI3_MOSI	0x100b1
+				MX6QDL_PAD_DISP0_DAT0__ECSPI3_SCLK	0x100b1
+			>;
+		};
+
+		pinctrl_enet: enetgrp {
+			fsl,pins = <
+				MX6QDL_PAD_ENET_MDIO__ENET_MDIO		0x1b0b0
+				MX6QDL_PAD_ENET_MDC__ENET_MDC		0x1b0b0
+				MX6QDL_PAD_RGMII_TXC__RGMII_TXC		0x1b0b0
+				MX6QDL_PAD_RGMII_TD0__RGMII_TD0		0x1b0b0
+				MX6QDL_PAD_RGMII_TD1__RGMII_TD1		0x1b0b0
+				MX6QDL_PAD_RGMII_TD2__RGMII_TD2		0x1b0b0
+				MX6QDL_PAD_RGMII_TD3__RGMII_TD3		0x1b0b0
+				MX6QDL_PAD_RGMII_TX_CTL__RGMII_TX_CTL	0x1b0b0
+				MX6QDL_PAD_ENET_REF_CLK__ENET_TX_CLK	0x1b0b0
+				MX6QDL_PAD_RGMII_RXC__RGMII_RXC		0x1b0b0
+				MX6QDL_PAD_RGMII_RD0__RGMII_RD0		0x1b0b0
+				MX6QDL_PAD_RGMII_RD1__RGMII_RD1		0x1b0b0
+				MX6QDL_PAD_RGMII_RD2__RGMII_RD2		0x1b0b0
+				MX6QDL_PAD_RGMII_RD3__RGMII_RD3		0x1b0b0
+				MX6QDL_PAD_RGMII_RX_CTL__RGMII_RX_CTL	0x1b0b0
+				MX6QDL_PAD_ENET_TX_EN__ENET_TX_EN	0x1b0b0
+			>;
+		};
+
+		pinctrl_gpmi_nand: gpminandgrp {
+			fsl,pins = <
+				MX6QDL_PAD_NANDF_CLE__NAND_CLE		0xb0b1
+				MX6QDL_PAD_NANDF_ALE__NAND_ALE		0xb0b1
+				MX6QDL_PAD_NANDF_WP_B__NAND_WP_B	0xb0b1
+				MX6QDL_PAD_NANDF_RB0__NAND_READY_B	0xb000
+				MX6QDL_PAD_NANDF_CS0__NAND_CE0_B	0xb0b1
+				MX6QDL_PAD_NANDF_CS1__NAND_CE1_B	0xb0b1
+				MX6QDL_PAD_SD4_CMD__NAND_RE_B		0xb0b1
+				MX6QDL_PAD_SD4_CLK__NAND_WE_B		0xb0b1
+				MX6QDL_PAD_NANDF_D0__NAND_DATA00	0xb0b1
+				MX6QDL_PAD_NANDF_D1__NAND_DATA01	0xb0b1
+				MX6QDL_PAD_NANDF_D2__NAND_DATA02	0xb0b1
+				MX6QDL_PAD_NANDF_D3__NAND_DATA03	0xb0b1
+				MX6QDL_PAD_NANDF_D4__NAND_DATA04	0xb0b1
+				MX6QDL_PAD_NANDF_D5__NAND_DATA05	0xb0b1
+				MX6QDL_PAD_NANDF_D6__NAND_DATA06	0xb0b1
+				MX6QDL_PAD_NANDF_D7__NAND_DATA07	0xb0b1
+				MX6QDL_PAD_SD4_DAT0__NAND_DQS		0x00b1
+			>;
+		};
+
+		pinctrl_i2c1: i2c1grp {
+			fsl,pins = <
+				MX6QDL_PAD_EIM_D21__I2C1_SCL		0x4001b8b1
+				MX6QDL_PAD_EIM_D28__I2C1_SDA		0x4001b8b1
+			>;
+		};
+
+		pinctrl_uart3: uart3grp {
+			fsl,pins = <
+				MX6QDL_PAD_EIM_D24__UART3_TX_DATA	0x1b0b1
+				MX6QDL_PAD_EIM_D25__UART3_RX_DATA	0x1b0b1
+				MX6QDL_PAD_EIM_D30__UART3_RTS_B		0x1b0b1
+				MX6QDL_PAD_EIM_D31__UART3_CTS_B		0x1b0b1
+			>;
+		};
+
+		pinctrl_uart4: uart4grp {
+			fsl,pins = <
+				MX6QDL_PAD_KEY_COL0__UART4_TX_DATA	0x1b0b1
+				MX6QDL_PAD_KEY_ROW0__UART4_RX_DATA	0x1b0b1
+			>;
+		};
+
+		pinctrl_usbh1: usbh1grp {
+			fsl,pins = <
+				MX6QDL_PAD_GPIO_0__USB_H1_PWR		0x80000000
+			>;
+		};
+
+		pinctrl_usbotg: usbotggrp {
+			fsl,pins = <
+				MX6QDL_PAD_GPIO_1__USB_OTG_ID		0x17059
+				MX6QDL_PAD_KEY_COL4__USB_OTG_OC		0x1b0b0
+				MX6QDL_PAD_KEY_ROW4__GPIO4_IO15		0x80000000
+			>;
+		};
+
+		pinctrl_usdhc2: usdhc2grp {
+			fsl,pins = <
+				MX6QDL_PAD_SD2_CMD__SD2_CMD		0x17059
+				MX6QDL_PAD_SD2_CLK__SD2_CLK		0x10059
+				MX6QDL_PAD_SD2_DAT0__SD2_DATA0		0x17059
+				MX6QDL_PAD_SD2_DAT1__SD2_DATA1		0x17059
+				MX6QDL_PAD_SD2_DAT2__SD2_DATA2		0x17059
+				MX6QDL_PAD_SD2_DAT3__SD2_DATA3		0x17059
+			>;
+		};
+
+		pinctrl_usdhc3: usdhc3grp {
+			fsl,pins = <
+				MX6QDL_PAD_SD3_CMD__SD3_CMD		0x17059
+				MX6QDL_PAD_SD3_CLK__SD3_CLK		0x10059
+				MX6QDL_PAD_SD3_DAT0__SD3_DATA0		0x17059
+				MX6QDL_PAD_SD3_DAT1__SD3_DATA1		0x17059
+				MX6QDL_PAD_SD3_DAT2__SD3_DATA2		0x17059
+				MX6QDL_PAD_SD3_DAT3__SD3_DATA3		0x17059
+			>;
+		};
+
+		pinctrl_usdhc3_cdwp: usdhc3cdwp {
+			fsl,pins = <
+				MX6QDL_PAD_ENET_RXD0__GPIO1_IO27 0x80000000
+				MX6QDL_PAD_ENET_TXD1__GPIO1_IO29 0x80000000
+			>;
+		};
+	};
+};
+
+&fec {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_enet>;
+	phy-mode = "rgmii";
+	phy-reset-gpios = <&gpio3 23 GPIO_ACTIVE_LOW>;
+	status = "disabled";
+};
+
+&gpmi {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_gpmi_nand>;
+	nand-on-flash-bbt;
+	status = "disabled";
+};
+
+&uart3 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_uart3>;
+	status = "disabled";
+};
+
+&uart4 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_uart4>;
+	status = "disabled";
+};
+
+&usbh1 {
+	vbus-supply = <&reg_usb_h1_vbus>;
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_usbh1>;
+	status = "disabled";
+};
+
+&usbotg {
+	vbus-supply = <&reg_usb_otg_vbus>;
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_usbotg>;
+	disable-over-current;
+	status = "disabled";
+};
+
+&usdhc2 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_usdhc2>;
+	cd-gpios = <&gpio1 4 0>;
+	wp-gpios = <&gpio1 2 0>;
+	status = "disabled";
+};
+
+&usdhc3 {
+        pinctrl-names = "default";
+        pinctrl-0 = <&pinctrl_usdhc3
+		     &pinctrl_usdhc3_cdwp>;
+        cd-gpios = <&gpio1 27 0>;
+        wp-gpios = <&gpio1 29 0>;
+        status = "disabled";
+};
diff --git a/arch/arm/boot/dts/imx6qdl-sabrelite.dtsi b/arch/arm/boot/dts/imx6qdl-sabrelite.dtsi
index 3bec128c797..6df6127bf83 100644
--- a/arch/arm/boot/dts/imx6qdl-sabrelite.dtsi
+++ b/arch/arm/boot/dts/imx6qdl-sabrelite.dtsi
@@ -13,6 +13,10 @@
 #include <dt-bindings/input/input.h>
 
 / {
+	chosen {
+		stdout-path = &uart2;
+	};
+
 	memory {
 		reg = <0x10000000 0x40000000>;
 	};
diff --git a/arch/arm/boot/dts/imx6qdl-sabresd.dtsi b/arch/arm/boot/dts/imx6qdl-sabresd.dtsi
index 0d816d3be4b..40ea3653464 100644
--- a/arch/arm/boot/dts/imx6qdl-sabresd.dtsi
+++ b/arch/arm/boot/dts/imx6qdl-sabresd.dtsi
@@ -14,6 +14,10 @@
 #include <dt-bindings/input/input.h>
 
 / {
+	chosen {
+		stdout-path = &uart1;
+	};
+
 	memory {
 		reg = <0x10000000 0x40000000>;
 	};
@@ -105,6 +109,17 @@
 		default-brightness-level = <7>;
 		status = "okay";
 	};
+
+	leds {
+		compatible = "gpio-leds";
+		pinctrl-names = "default";
+		pinctrl-0 = <&pinctrl_gpio_leds>;
+
+		red {
+		        gpios = <&gpio1 2 0>;
+		        default-state = "on";
+		};
+	};
 };
 
 &audmux {
@@ -137,6 +152,11 @@
 	status = "okay";
 };
 
+&hdmi {
+	ddc-i2c-bus = <&i2c2>;
+	status = "okay";
+};
+
 &i2c1 {
 	clock-frequency = <100000>;
 	pinctrl-names = "default";
@@ -373,6 +393,12 @@
 			>;
 		};
 
+		pinctrl_pcie: pciegrp {
+			fsl,pins = <
+				MX6QDL_PAD_GPIO_17__GPIO7_IO12	0x80000000
+			>;
+		};
+
 		pinctrl_pwm1: pwm1grp {
 			fsl,pins = <
 				MX6QDL_PAD_SD1_DAT3__PWM1_OUT		0x1b0b1
@@ -421,6 +447,29 @@
 				MX6QDL_PAD_SD3_DAT7__SD3_DATA7		0x17059
 			>;
 		};
+
+		pinctrl_usdhc4: usdhc4grp {
+			fsl,pins = <
+				MX6QDL_PAD_SD4_CMD__SD4_CMD		0x17059
+				MX6QDL_PAD_SD4_CLK__SD4_CLK		0x10059
+				MX6QDL_PAD_SD4_DAT0__SD4_DATA0		0x17059
+				MX6QDL_PAD_SD4_DAT1__SD4_DATA1		0x17059
+				MX6QDL_PAD_SD4_DAT2__SD4_DATA2		0x17059
+				MX6QDL_PAD_SD4_DAT3__SD4_DATA3		0x17059
+				MX6QDL_PAD_SD4_DAT4__SD4_DATA4		0x17059
+				MX6QDL_PAD_SD4_DAT5__SD4_DATA5		0x17059
+				MX6QDL_PAD_SD4_DAT6__SD4_DATA6		0x17059
+				MX6QDL_PAD_SD4_DAT7__SD4_DATA7		0x17059
+			>;
+		};
+	};
+
+	gpio_leds {
+		pinctrl_gpio_leds: gpioledsgrp {
+			fsl,pins = <
+				MX6QDL_PAD_GPIO_2__GPIO1_IO02 0x80000000
+			>;
+		};
 	};
 };
 
@@ -449,6 +498,13 @@
 	};
 };
 
+&pcie {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_pcie>;
+	reset-gpio = <&gpio7 12 0>;
+	status = "okay";
+};
+
 &pwm1 {
 	pinctrl-names = "default";
 	pinctrl-0 = <&pinctrl_pwm1>;
@@ -496,3 +552,12 @@
 	wp-gpios = <&gpio2 1 0>;
 	status = "okay";
 };
+
+&usdhc4 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_usdhc4>;
+	bus-width = <8>;
+	non-removable;
+	no-1-8-v;
+	status = "okay";
+};
diff --git a/arch/arm/boot/dts/imx6qdl-wandboard.dtsi b/arch/arm/boot/dts/imx6qdl-wandboard.dtsi
index bdfdf89d405..5c6f10c43f6 100644
--- a/arch/arm/boot/dts/imx6qdl-wandboard.dtsi
+++ b/arch/arm/boot/dts/imx6qdl-wandboard.dtsi
@@ -62,6 +62,18 @@
 	status = "okay";
 };
 
+&hdmi {
+	ddc-i2c-bus = <&i2c1>;
+	status = "okay";
+};
+
+&i2c1 {
+	clock-frequency = <100000>;
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_i2c1>;
+	status = "okay";
+};
+
 &i2c2 {
 	clock-frequency = <100000>;
 	pinctrl-names = "default";
@@ -127,6 +139,13 @@
 			>;
 		};
 
+		pinctrl_i2c1: i2c1grp {
+			fsl,pins = <
+				MX6QDL_PAD_EIM_D21__I2C1_SCL 		0x4001b8b1
+				MX6QDL_PAD_EIM_D28__I2C1_SDA 		0x4001b8b1
+			>;
+		};
+
 		pinctrl_i2c2: i2c2grp {
 			fsl,pins = <
 				MX6QDL_PAD_KEY_COL3__I2C2_SCL		0x4001b8b1
diff --git a/arch/arm/boot/dts/imx6qdl.dtsi b/arch/arm/boot/dts/imx6qdl.dtsi
index 55cb926fa3f..ce0599134a6 100644
--- a/arch/arm/boot/dts/imx6qdl.dtsi
+++ b/arch/arm/boot/dts/imx6qdl.dtsi
@@ -10,10 +10,13 @@
  * http://www.gnu.org/copyleft/gpl.html
  */
 
+#include <dt-bindings/interrupt-controller/arm-gic.h>
+
 #include "skeleton.dtsi"
 
 / {
 	aliases {
+		ethernet0 = &fec;
 		can0 = &can1;
 		can1 = &can2;
 		gpio0 = &gpio1;
@@ -46,8 +49,6 @@
 	intc: interrupt-controller@00a01000 {
 		compatible = "arm,cortex-a9-gic";
 		#interrupt-cells = <3>;
-		#address-cells = <1>;
-		#size-cells = <1>;
 		interrupt-controller;
 		reg = <0x00a01000 0x1000>,
 		      <0x00a00100 0x100>;
@@ -59,16 +60,19 @@
 
 		ckil {
 			compatible = "fsl,imx-ckil", "fixed-clock";
+			#clock-cells = <0>;
 			clock-frequency = <32768>;
 		};
 
 		ckih1 {
 			compatible = "fsl,imx-ckih1", "fixed-clock";
+			#clock-cells = <0>;
 			clock-frequency = <0>;
 		};
 
 		osc {
 			compatible = "fsl,imx-osc", "fixed-clock";
+			#clock-cells = <0>;
 			clock-frequency = <24000000>;
 		};
 	};
@@ -137,9 +141,16 @@
 				  0x81000000 0 0          0x01f80000 0 0x00010000 /* downstream I/O */
 				  0x82000000 0 0x01000000 0x01000000 0 0x00f00000>; /* non-prefetchable memory */
 			num-lanes = <1>;
-			interrupts = <0 123 IRQ_TYPE_LEVEL_HIGH>;
-			clocks = <&clks 189>, <&clks 187>, <&clks 206>, <&clks 144>;
-			clock-names = "pcie_ref_125m", "sata_ref_100m", "lvds_gate", "pcie_axi";
+			interrupts = <GIC_SPI 120 IRQ_TYPE_LEVEL_HIGH>;
+			interrupt-names = "msi";
+			#interrupt-cells = <1>;
+			interrupt-map-mask = <0 0 0 0x7>;
+			interrupt-map = <0 0 0 1 &intc GIC_SPI 123 IRQ_TYPE_LEVEL_HIGH>,
+			                <0 0 0 2 &intc GIC_SPI 122 IRQ_TYPE_LEVEL_HIGH>,
+			                <0 0 0 3 &intc GIC_SPI 121 IRQ_TYPE_LEVEL_HIGH>,
+			                <0 0 0 4 &intc GIC_SPI 120 IRQ_TYPE_LEVEL_HIGH>;
+			clocks = <&clks 144>, <&clks 206>, <&clks 189>;
+			clock-names = "pcie", "pcie_bus", "pcie_phy";
 			status = "disabled";
 		};
 
diff --git a/arch/arm/boot/dts/imx6sl-evk.dts b/arch/arm/boot/dts/imx6sl-evk.dts
index 864d8dfb51c..a8d9a93fab8 100644
--- a/arch/arm/boot/dts/imx6sl-evk.dts
+++ b/arch/arm/boot/dts/imx6sl-evk.dts
@@ -282,6 +282,7 @@
 				MX6SL_PAD_ECSPI1_MISO__ECSPI1_MISO	0x100b1
 				MX6SL_PAD_ECSPI1_MOSI__ECSPI1_MOSI	0x100b1
 				MX6SL_PAD_ECSPI1_SCLK__ECSPI1_SCLK	0x100b1
+				MX6SL_PAD_ECSPI1_SS0__GPIO4_IO11	0x80000000
 			>;
 		};
 
diff --git a/arch/arm/boot/dts/imx6sl.dtsi b/arch/arm/boot/dts/imx6sl.dtsi
index 3cb4941afee..2d4e5285f3f 100644
--- a/arch/arm/boot/dts/imx6sl.dtsi
+++ b/arch/arm/boot/dts/imx6sl.dtsi
@@ -14,6 +14,7 @@
 
 / {
 	aliases {
+		ethernet0 = &fec;
 		gpio0 = &gpio1;
 		gpio1 = &gpio2;
 		gpio2 = &gpio3;
@@ -68,8 +69,6 @@
 	intc: interrupt-controller@00a01000 {
 		compatible = "arm,cortex-a9-gic";
 		#interrupt-cells = <3>;
-		#address-cells = <1>;
-		#size-cells = <1>;
 		interrupt-controller;
 		reg = <0x00a01000 0x1000>,
 		      <0x00a00100 0x100>;
@@ -81,11 +80,13 @@
 
 		ckil {
 			compatible = "fixed-clock";
+			#clock-cells = <0>;
 			clock-frequency = <32768>;
 		};
 
 		osc {
 			compatible = "fixed-clock";
+			#clock-cells = <0>;
 			clock-frequency = <24000000>;
 		};
 	};
diff --git a/arch/arm/boot/dts/k2e-evm.dts b/arch/arm/boot/dts/k2e-evm.dts
index 74b3b63e94c..c568f067604 100644
--- a/arch/arm/boot/dts/k2e-evm.dts
+++ b/arch/arm/boot/dts/k2e-evm.dts
@@ -58,3 +58,84 @@
 &usb1 {
 	status = "okay";
 };
+
+&i2c0 {
+	dtt@50 {
+		compatible = "at,24c1024";
+		reg = <0x50>;
+	};
+};
+
+&aemif {
+	cs0 {
+		#address-cells = <2>;
+		#size-cells = <1>;
+		clock-ranges;
+		ranges;
+
+		ti,cs-chipselect = <0>;
+		/* all timings in nanoseconds */
+		ti,cs-min-turnaround-ns = <12>;
+		ti,cs-read-hold-ns = <6>;
+		ti,cs-read-strobe-ns = <23>;
+		ti,cs-read-setup-ns = <9>;
+		ti,cs-write-hold-ns = <8>;
+		ti,cs-write-strobe-ns = <23>;
+		ti,cs-write-setup-ns = <8>;
+
+		nand@0,0 {
+			compatible = "ti,keystone-nand","ti,davinci-nand";
+			#address-cells = <1>;
+			#size-cells = <1>;
+			reg = <0 0 0x4000000
+			       1 0 0x0000100>;
+
+			ti,davinci-chipselect = <0>;
+			ti,davinci-mask-ale = <0x2000>;
+			ti,davinci-mask-cle = <0x4000>;
+			ti,davinci-mask-chipsel = <0>;
+			nand-ecc-mode = "hw";
+			ti,davinci-ecc-bits = <4>;
+			nand-on-flash-bbt;
+
+			partition@0 {
+				label = "u-boot";
+				reg = <0x0 0x100000>;
+				read-only;
+			};
+
+			partition@100000 {
+				label = "params";
+				reg = <0x100000 0x80000>;
+				read-only;
+			};
+
+			partition@180000 {
+				label = "ubifs";
+				reg = <0x180000 0x1FE80000>;
+			};
+		};
+	};
+};
+
+&spi0 {
+	nor_flash: n25q128a11@0 {
+		#address-cells = <1>;
+		#size-cells = <1>;
+		compatible = "Micron,n25q128a11";
+		spi-max-frequency = <54000000>;
+		m25p,fast-read;
+		reg = <0>;
+
+		partition@0 {
+			label = "u-boot-spl";
+			reg = <0x0 0x80000>;
+			read-only;
+		};
+
+		partition@1 {
+			label = "misc";
+			reg = <0x80000 0xf80000>;
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/k2hk-evm.dts b/arch/arm/boot/dts/k2hk-evm.dts
index c93d06f9f2a..1f90cbf27fd 100644
--- a/arch/arm/boot/dts/k2hk-evm.dts
+++ b/arch/arm/boot/dts/k2hk-evm.dts
@@ -138,3 +138,32 @@
 		};
 	};
 };
+
+&i2c0 {
+	dtt@50 {
+		compatible = "at,24c1024";
+		reg = <0x50>;
+	};
+};
+
+&spi0 {
+	nor_flash: n25q128a11@0 {
+		#address-cells = <1>;
+		#size-cells = <1>;
+		compatible = "Micron,n25q128a11";
+		spi-max-frequency = <54000000>;
+		m25p,fast-read;
+		reg = <0>;
+
+		partition@0 {
+			label = "u-boot-spl";
+			reg = <0x0 0x80000>;
+			read-only;
+		};
+
+		partition@1 {
+			label = "misc";
+			reg = <0x80000 0xf80000>;
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/k2l-evm.dts b/arch/arm/boot/dts/k2l-evm.dts
index 50a70132ac9..fec43128a2e 100644
--- a/arch/arm/boot/dts/k2l-evm.dts
+++ b/arch/arm/boot/dts/k2l-evm.dts
@@ -35,3 +35,84 @@
 &usb {
 	status = "okay";
 };
+
+&i2c0 {
+	dtt@50 {
+		compatible = "at,24c1024";
+		reg = <0x50>;
+	};
+};
+
+&aemif {
+	cs0 {
+		#address-cells = <2>;
+		#size-cells = <1>;
+		clock-ranges;
+		ranges;
+
+		ti,cs-chipselect = <0>;
+		/* all timings in nanoseconds */
+		ti,cs-min-turnaround-ns = <12>;
+		ti,cs-read-hold-ns = <6>;
+		ti,cs-read-strobe-ns = <23>;
+		ti,cs-read-setup-ns = <9>;
+		ti,cs-write-hold-ns = <8>;
+		ti,cs-write-strobe-ns = <23>;
+		ti,cs-write-setup-ns = <8>;
+
+		nand@0,0 {
+			compatible = "ti,keystone-nand","ti,davinci-nand";
+			#address-cells = <1>;
+			#size-cells = <1>;
+			reg = <0 0 0x4000000
+			       1 0 0x0000100>;
+
+			ti,davinci-chipselect = <0>;
+			ti,davinci-mask-ale = <0x2000>;
+			ti,davinci-mask-cle = <0x4000>;
+			ti,davinci-mask-chipsel = <0>;
+			nand-ecc-mode = "hw";
+			ti,davinci-ecc-bits = <4>;
+			nand-on-flash-bbt;
+
+			partition@0 {
+				label = "u-boot";
+				reg = <0x0 0x100000>;
+				read-only;
+			};
+
+			partition@100000 {
+				label = "params";
+				reg = <0x100000 0x80000>;
+				read-only;
+			};
+
+			partition@180000 {
+				label = "ubifs";
+				reg = <0x180000 0x7FE80000>;
+			};
+		};
+	};
+};
+
+&spi0 {
+	nor_flash: n25q128a11@0 {
+		#address-cells = <1>;
+		#size-cells = <1>;
+		compatible = "Micron,n25q128a11";
+		spi-max-frequency = <54000000>;
+		m25p,fast-read;
+		reg = <0>;
+
+		partition@0 {
+			label = "u-boot-spl";
+			reg = <0x0 0x80000>;
+			read-only;
+		};
+
+		partition@1 {
+			label = "misc";
+			reg = <0x80000 0xf80000>;
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/keystone.dtsi b/arch/arm/boot/dts/keystone.dtsi
index 90823eb90c1..d9f99e7deb8 100644
--- a/arch/arm/boot/dts/keystone.dtsi
+++ b/arch/arm/boot/dts/keystone.dtsi
@@ -28,8 +28,6 @@
 	gic: interrupt-controller {
 		compatible = "arm,cortex-a15-gic";
 		#interrupt-cells = <3>;
-		#size-cells = <0>;
-		#address-cells = <1>;
 		interrupt-controller;
 		reg = <0x0 0x02561000 0x0 0x1000>,
 		      <0x0 0x02562000 0x0 0x2000>,
@@ -66,6 +64,7 @@
 		compatible = "ti,keystone","simple-bus";
 		interrupt-parent = <&gic>;
 		ranges = <0x0 0x0 0x0 0xc0000000>;
+		dma-ranges = <0x80000000 0x8 0x00000000 0x80000000>;
 
 		rstctrl: reset-controller {
 			compatible = "ti,keystone-reset";
@@ -102,11 +101,6 @@
 			interrupts = <GIC_SPI 283 IRQ_TYPE_EDGE_RISING>;
 			#address-cells = <1>;
 			#size-cells = <0>;
-
-			dtt@50 {
-				compatible = "at,24c1024";
-				reg = <0x50>;
-			};
 		};
 
 		i2c1: i2c@2530400 {
@@ -115,6 +109,8 @@
 			clock-frequency = <100000>;
 			clocks = <&clki2c>;
 			interrupts = <GIC_SPI 286 IRQ_TYPE_EDGE_RISING>;
+			#address-cells = <1>;
+			#size-cells = <0>;
 		};
 
 		i2c2: i2c@2530800 {
@@ -123,6 +119,8 @@
 			clock-frequency = <100000>;
 			clocks = <&clki2c>;
 			interrupts = <GIC_SPI 289 IRQ_TYPE_EDGE_RISING>;
+			#address-cells = <1>;
+			#size-cells = <0>;
 		};
 
 		spi0: spi@21000400 {
@@ -132,6 +130,8 @@
 			ti,davinci-spi-intr-line = <0>;
 			interrupts = <GIC_SPI 292 IRQ_TYPE_EDGE_RISING>;
 			clocks = <&clkspi>;
+			#address-cells = <1>;
+			#size-cells = <0>;
 		};
 
 		spi1: spi@21000600 {
@@ -141,6 +141,8 @@
 			ti,davinci-spi-intr-line = <0>;
 			interrupts = <GIC_SPI 296 IRQ_TYPE_EDGE_RISING>;
 			clocks = <&clkspi>;
+			#address-cells = <1>;
+			#size-cells = <0>;
 		};
 
 		spi2: spi@21000800 {
@@ -150,6 +152,8 @@
 			ti,davinci-spi-intr-line = <0>;
 			interrupts = <GIC_SPI 300 IRQ_TYPE_EDGE_RISING>;
 			clocks = <&clkspi>;
+			#address-cells = <1>;
+			#size-cells = <0>;
 		};
 
 		usb_phy: usb_phy@2620738 {
@@ -169,6 +173,8 @@
 			clock-names = "usb";
 			interrupts = <GIC_SPI 393 IRQ_TYPE_EDGE_RISING>;
 			ranges;
+			dma-coherent;
+			dma-ranges;
 			status = "disabled";
 
 			dwc3@2690000 {
diff --git a/arch/arm/boot/dts/kirkwood-6192.dtsi b/arch/arm/boot/dts/kirkwood-6192.dtsi
index 3916937d681..dd81508b919 100644
--- a/arch/arm/boot/dts/kirkwood-6192.dtsi
+++ b/arch/arm/boot/dts/kirkwood-6192.dtsi
@@ -1,6 +1,6 @@
 / {
 	mbus {
-		pcie-controller {
+		pciec: pcie-controller {
 			compatible = "marvell,kirkwood-pcie";
 			status = "disabled";
 			device_type = "pci";
@@ -15,7 +15,7 @@
 				0x82000000 0x1 0     MBUS_ID(0x04, 0xe8) 0       1 0 /* Port 0.0 MEM */
 				0x81000000 0x1 0     MBUS_ID(0x04, 0xe0) 0       1 0 /* Port 0.0 IO  */>;
 
-			pcie@1,0 {
+			pcie0: pcie@1,0 {
 				device_type = "pci";
 				assigned-addresses = <0x82000800 0 0x00040000 0 0x2000>;
 				reg = <0x0800 0 0 0 0>;
@@ -35,16 +35,9 @@
 	};
 
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 			compatible = "marvell,88f6192-pinctrl";
-			reg = <0x10000 0x20>;
 
-			pmx_nand: pmx-nand {
-				marvell,pins = "mpp0", "mpp1", "mpp2", "mpp3",
-					       "mpp4", "mpp5", "mpp18",
-					       "mpp19";
-				marvell,function = "nand";
-			};
 			pmx_sata0: pmx-sata0 {
 				marvell,pins = "mpp5", "mpp21", "mpp23";
 				marvell,function = "sata0";
@@ -53,22 +46,6 @@
 				marvell,pins = "mpp4", "mpp20", "mpp22";
 				marvell,function = "sata1";
 			};
-			pmx_spi: pmx-spi {
-				marvell,pins = "mpp0", "mpp1", "mpp2", "mpp3";
-				marvell,function = "spi";
-			};
-			pmx_twsi0: pmx-twsi0 {
-				marvell,pins = "mpp8", "mpp9";
-				marvell,function = "twsi0";
-			};
-			pmx_uart0: pmx-uart0 {
-				marvell,pins = "mpp10", "mpp11";
-				marvell,function = "uart0";
-			};
-			pmx_uart1: pmx-uart1 {
-				marvell,pins = "mpp13", "mpp14";
-				marvell,function = "uart1";
-			};
 			pmx_sdio: pmx-sdio {
 				marvell,pins = "mpp12", "mpp13", "mpp14",
 					       "mpp15", "mpp16", "mpp17";
@@ -76,14 +53,14 @@
 			};
 		};
 
-		rtc@10300 {
+		rtc: rtc@10300 {
 			compatible = "marvell,kirkwood-rtc", "marvell,orion-rtc";
 			reg = <0x10300 0x20>;
 			interrupts = <53>;
 			clocks = <&gate_clk 7>;
 		};
 
-		sata@80000 {
+		sata: sata@80000 {
 			compatible = "marvell,orion-sata";
 			reg = <0x80000 0x5000>;
 			interrupts = <21>;
@@ -92,7 +69,7 @@
 			status = "disabled";
 		};
 
-		mvsdio@90000 {
+		sdio: mvsdio@90000 {
 			compatible = "marvell,orion-sdio";
 			reg = <0x90000 0x200>;
 			interrupts = <28>;
diff --git a/arch/arm/boot/dts/kirkwood-6281.dtsi b/arch/arm/boot/dts/kirkwood-6281.dtsi
index 416d96e1302..7dc7d6782e8 100644
--- a/arch/arm/boot/dts/kirkwood-6281.dtsi
+++ b/arch/arm/boot/dts/kirkwood-6281.dtsi
@@ -1,6 +1,6 @@
 / {
 	mbus {
-		pcie-controller {
+		pciec: pcie-controller {
 			compatible = "marvell,kirkwood-pcie";
 			status = "disabled";
 			device_type = "pci";
@@ -15,7 +15,7 @@
 				0x82000000 0x1 0     MBUS_ID(0x04, 0xe8) 0       1 0 /* Port 0.0 MEM */
 				0x81000000 0x1 0     MBUS_ID(0x04, 0xe0) 0       1 0 /* Port 0.0 IO  */>;
 
-			pcie@1,0 {
+			pcie0: pcie@1,0 {
 				device_type = "pci";
 				assigned-addresses = <0x82000800 0 0x00040000 0 0x2000>;
 				reg = <0x0800 0 0 0 0>;
@@ -35,16 +35,9 @@
 	};
 
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 			compatible = "marvell,88f6281-pinctrl";
-			reg = <0x10000 0x20>;
 
-			pmx_nand: pmx-nand {
-				marvell,pins = "mpp0", "mpp1", "mpp2", "mpp3",
-					       "mpp4", "mpp5", "mpp18",
-					       "mpp19";
-				marvell,function = "nand";
-			};
 			pmx_sata0: pmx-sata0 {
 				marvell,pins = "mpp5", "mpp21", "mpp23";
 				marvell,function = "sata0";
@@ -53,22 +46,6 @@
 				marvell,pins = "mpp4", "mpp20", "mpp22";
 				marvell,function = "sata1";
 			};
-			pmx_spi: pmx-spi {
-				marvell,pins = "mpp0", "mpp1", "mpp2", "mpp3";
-				marvell,function = "spi";
-			};
-			pmx_twsi0: pmx-twsi0 {
-				marvell,pins = "mpp8", "mpp9";
-				marvell,function = "twsi0";
-			};
-			pmx_uart0: pmx-uart0 {
-				marvell,pins = "mpp10", "mpp11";
-				marvell,function = "uart0";
-			};
-			pmx_uart1: pmx-uart1 {
-				marvell,pins = "mpp13", "mpp14";
-				marvell,function = "uart1";
-			};
 			pmx_sdio: pmx-sdio {
 				marvell,pins = "mpp12", "mpp13", "mpp14",
 					       "mpp15", "mpp16", "mpp17";
@@ -76,14 +53,14 @@
 			};
 		};
 
-		rtc@10300 {
+		rtc: rtc@10300 {
 			compatible = "marvell,kirkwood-rtc", "marvell,orion-rtc";
 			reg = <0x10300 0x20>;
 			interrupts = <53>;
 			clocks = <&gate_clk 7>;
 		};
 
-		sata@80000 {
+		sata: sata@80000 {
 			compatible = "marvell,orion-sata";
 			reg = <0x80000 0x5000>;
 			interrupts = <21>;
@@ -94,7 +71,7 @@
 			status = "disabled";
 		};
 
-		mvsdio@90000 {
+		sdio: mvsdio@90000 {
 			compatible = "marvell,orion-sdio";
 			reg = <0x90000 0x200>;
 			interrupts = <28>;
diff --git a/arch/arm/boot/dts/kirkwood-6282.dtsi b/arch/arm/boot/dts/kirkwood-6282.dtsi
index 2902e0d7971..4680eec990f 100644
--- a/arch/arm/boot/dts/kirkwood-6282.dtsi
+++ b/arch/arm/boot/dts/kirkwood-6282.dtsi
@@ -1,6 +1,6 @@
 / {
 	mbus {
-		pcie-controller {
+		pciec: pcie-controller {
 			compatible = "marvell,kirkwood-pcie";
 			status = "disabled";
 			device_type = "pci";
@@ -19,7 +19,7 @@
 				0x82000000 0x2 0     MBUS_ID(0x04, 0xd8) 0       1 0 /* Port 1.0 MEM */
 				0x81000000 0x2 0     MBUS_ID(0x04, 0xd0) 0       1 0 /* Port 1.0 IO  */>;
 
-			pcie@1,0 {
+			pcie0: pcie@1,0 {
 				device_type = "pci";
 				assigned-addresses = <0x82000800 0 0x00040000 0 0x2000>;
 				reg = <0x0800 0 0 0 0>;
@@ -36,7 +36,7 @@
 				status = "disabled";
 			};
 
-			pcie@2,0 {
+			pcie1: pcie@2,0 {
 				device_type = "pci";
 				assigned-addresses = <0x82001000 0 0x00044000 0 0x2000>;
 				reg = <0x1000 0 0 0 0>;
@@ -56,15 +56,8 @@
 	};
 	ocp@f1000000 {
 
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 			compatible = "marvell,88f6282-pinctrl";
-			reg = <0x10000 0x20>;
-
-			pmx_nand: pmx-nand {
-				marvell,pins = "mpp0", "mpp1", "mpp2", "mpp3",
-							"mpp4", "mpp5", "mpp18", "mpp19";
-				marvell,function = "nand";
-			};
 
 			pmx_sata0: pmx-sata0 {
 				marvell,pins = "mpp5", "mpp21", "mpp23";
@@ -74,29 +67,16 @@
 				marvell,pins = "mpp4", "mpp20", "mpp22";
 				marvell,function = "sata1";
 			};
-			pmx_spi: pmx-spi {
-				marvell,pins = "mpp0", "mpp1", "mpp2", "mpp3";
-				marvell,function = "spi";
-			};
-			pmx_twsi0: pmx-twsi0 {
-				marvell,pins = "mpp8", "mpp9";
-				marvell,function = "twsi0";
-			};
 
+			/*
+			 * Default I2C1 pinctrl setting on mpp36/mpp37,
+			 * overwrite marvell,pins on board level if required.
+			 */
 			pmx_twsi1: pmx-twsi1 {
 				marvell,pins = "mpp36", "mpp37";
 				marvell,function = "twsi1";
 			};
 
-			pmx_uart0: pmx-uart0 {
-				marvell,pins = "mpp10", "mpp11";
-				marvell,function = "uart0";
-			};
-
-			pmx_uart1: pmx-uart1 {
-				marvell,pins = "mpp13", "mpp14";
-				marvell,function = "uart1";
-			};
 			pmx_sdio: pmx-sdio {
 				marvell,pins = "mpp12", "mpp13", "mpp14",
 					       "mpp15", "mpp16", "mpp17";
@@ -104,20 +84,20 @@
 			};
 		};
 
-		thermal@10078 {
+		thermal: thermal@10078 {
 			compatible = "marvell,kirkwood-thermal";
 			reg = <0x10078 0x4>;
 			status = "okay";
 		};
 
-		rtc@10300 {
+		rtc: rtc@10300 {
 			compatible = "marvell,kirkwood-rtc", "marvell,orion-rtc";
 			reg = <0x10300 0x20>;
 			interrupts = <53>;
 			clocks = <&gate_clk 7>;
 		};
 
-		i2c@11100 {
+		i2c1: i2c@11100 {
 			compatible = "marvell,mv64xxx-i2c";
 			reg = <0x11100 0x20>;
 			#address-cells = <1>;
@@ -125,10 +105,12 @@
 			interrupts = <32>;
 			clock-frequency = <100000>;
 			clocks = <&gate_clk 7>;
+			pinctrl-0 = <&pmx_twsi1>;
+			pinctrl-names = "default";
 			status = "disabled";
 		};
 
-		sata@80000 {
+		sata: sata@80000 {
 			compatible = "marvell,orion-sata";
 			reg = <0x80000 0x5000>;
 			interrupts = <21>;
@@ -139,7 +121,7 @@
 			status = "disabled";
 		};
 
-		mvsdio@90000 {
+		sdio: mvsdio@90000 {
 			compatible = "marvell,orion-sdio";
 			reg = <0x90000 0x200>;
 			interrupts = <28>;
diff --git a/arch/arm/boot/dts/kirkwood-98dx4122.dtsi b/arch/arm/boot/dts/kirkwood-98dx4122.dtsi
index 3271e4c8ea0..9e1f741d74f 100644
--- a/arch/arm/boot/dts/kirkwood-98dx4122.dtsi
+++ b/arch/arm/boot/dts/kirkwood-98dx4122.dtsi
@@ -1,31 +1,51 @@
 / {
+	mbus {
+		pciec: pcie-controller {
+			compatible = "marvell,kirkwood-pcie";
+			status = "disabled";
+			device_type = "pci";
+
+			#address-cells = <3>;
+			#size-cells = <2>;
+
+			bus-range = <0x00 0xff>;
+
+			ranges =
+			       <0x82000000 0 0x40000 MBUS_ID(0xf0, 0x01) 0x40000 0 0x00002000
+				0x82000000 0x1 0     MBUS_ID(0x04, 0xe8) 0       1 0 /* Port 0.0 MEM */
+				0x81000000 0x1 0     MBUS_ID(0x04, 0xe0) 0       1 0 /* Port 0.0 IO  */>;
+
+			pcie0: pcie@1,0 {
+				device_type = "pci";
+				assigned-addresses = <0x82000800 0 0x00040000 0 0x2000>;
+				reg = <0x0800 0 0 0 0>;
+				#address-cells = <3>;
+				#size-cells = <2>;
+				#interrupt-cells = <1>;
+				ranges = <0x82000000 0 0 0x82000000 0x1 0 1 0
+					  0x81000000 0 0 0x81000000 0x1 0 1 0>;
+				interrupt-map-mask = <0 0 0 0>;
+				interrupt-map = <0 0 0 0 &intc 9>;
+				marvell,pcie-port = <0>;
+				marvell,pcie-lane = <0>;
+				clocks = <&gate_clk 2>;
+				status = "disabled";
+			};
+		};
+	};
+
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 			compatible = "marvell,98dx4122-pinctrl";
-			reg = <0x10000 0x20>;
 
-			pmx_nand: pmx-nand {
-				marvell,pins = "mpp0", "mpp1", "mpp2", "mpp3",
-					       "mpp4", "mpp5", "mpp18",
-					       "mpp19";
-				marvell,function = "nand";
-			};
-			pmx_spi: pmx-spi {
-				marvell,pins = "mpp0", "mpp1", "mpp2", "mpp3";
-				marvell,function = "spi";
-			};
-			pmx_twsi0: pmx-twsi0 {
-				marvell,pins = "mpp8", "mpp9";
-				marvell,function = "twsi0";
-			};
-			pmx_uart0: pmx-uart0 {
-				marvell,pins = "mpp10", "mpp11";
-				marvell,function = "uart0";
-			};
-			pmx_uart1: pmx-uart1 {
-				marvell,pins = "mpp13", "mpp14";
-				marvell,function = "uart1";
-			};
 		};
 	};
 };
+
+&sata_phy0 {
+	status = "disabled";
+};
+
+&sata_phy1 {
+	status = "disabled";
+};
diff --git a/arch/arm/boot/dts/kirkwood-b3.dts b/arch/arm/boot/dts/kirkwood-b3.dts
index 40791053106..c9247f8672a 100644
--- a/arch/arm/boot/dts/kirkwood-b3.dts
+++ b/arch/arm/boot/dts/kirkwood-b3.dts
@@ -30,6 +30,7 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8 earlyprintk";
+		stdout-path = &uart0;
 	};
 
 	mbus {
@@ -44,7 +45,7 @@
 	};
 
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 			pmx_button_power: pmx-button-power {
 				marvell,pins = "mpp39";
 				marvell,function = "gpio";
@@ -69,13 +70,11 @@
 
 		spi@10600 {
 			status = "okay";
-			pinctrl-0 = <&pmx_spi>;
-			pinctrl-names = "default";
 
 			m25p16@0 {
 				#address-cells = <1>;
 				#size-cells = <1>;
-				compatible = "m25p16";
+				compatible = "st,m25p16";
 				reg = <0>;
 				spi-max-frequency = <40000000>;
 				mode = <0>;
@@ -113,8 +112,6 @@
 			 * UART0_TX = Testpoint 66
 			 * See the Excito Wiki for more details.
 		 	 */
-			pinctrl-0 = <&pmx_uart0>;
-			pinctrl-names = "default";
 			status = "okay";
 		};
 
diff --git a/arch/arm/boot/dts/kirkwood-cloudbox.dts b/arch/arm/boot/dts/kirkwood-cloudbox.dts
index 0e06fd3cee4..ab6ab4933e6 100644
--- a/arch/arm/boot/dts/kirkwood-cloudbox.dts
+++ b/arch/arm/boot/dts/kirkwood-cloudbox.dts
@@ -14,10 +14,11 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8";
+		stdout-path = &uart0;
 	};
 
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 			pmx_cloudbox_sata0: pmx-cloudbox-sata0 {
 				marvell,pins = "mpp15";
 				marvell,function = "sata0";
@@ -25,9 +26,6 @@
 		};
 
 		serial@12000 {
-			pinctrl-0 = <&pmx_uart0>;
-			pinctrl-names = "default";
-			clock-frequency = <166666667>;
 			status = "okay";
 		};
 
@@ -39,14 +37,12 @@
 		};
 
 		spi@10600 {
-			pinctrl-0 = <&pmx_spi>;
-			pinctrl-names = "default";
 			status = "okay";
 
 			flash@0 {
 				#address-cells = <1>;
 				#size-cells = <1>;
-				compatible = "mx25l4005a";
+				compatible = "mxicy,mx25l4005a";
 				reg = <0>;
 				spi-max-frequency = <20000000>;
 				mode = <0>;
diff --git a/arch/arm/boot/dts/kirkwood-db.dtsi b/arch/arm/boot/dts/kirkwood-db.dtsi
index 02d1225ef99..812df691ae3 100644
--- a/arch/arm/boot/dts/kirkwood-db.dtsi
+++ b/arch/arm/boot/dts/kirkwood-db.dtsi
@@ -22,10 +22,11 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8 earlyprintk";
+		stdout-path = &uart0;
 	};
 
 	ocp@f1000000 {
-		pinctrl@10000 {
+		pin-controller@10000 {
 			pmx_sdio_gpios: pmx-sdio-gpios {
 				marvell,pins = "mpp37", "mpp38";
 				marvell,function = "gpio";
@@ -33,10 +34,7 @@
 		};
 
 		serial@12000 {
-			pinctrl-0 = <&pmx_uart0>;
-			pinctrl-names = "default";
-			clock-frequency = <200000000>;
-			status = "ok";
+			status = "okay";
 		};
 
 		sata@80000 {
@@ -59,8 +57,6 @@
 };
 
 &nand {
-	pinctrl-0 = <&pmx_nand>;
-	pinctrl-names = "default";
 	chip-delay = <25>;
 	status = "okay";
 
diff --git a/arch/arm/boot/dts/kirkwood-dns320.dts b/arch/arm/boot/dts/kirkwood-dns320.dts
index bf7fe8ab88f..d85ef0a91b5 100644
--- a/arch/arm/boot/dts/kirkwood-dns320.dts
+++ b/arch/arm/boot/dts/kirkwood-dns320.dts
@@ -13,6 +13,7 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8 earlyprintk";
+		stdout-path = &uart0;
 	};
 
 	gpio-leds {
@@ -51,8 +52,6 @@
 		};
 
 		serial@12100 {
-			pinctrl-0 = <&pmx_uart1>;
-			pinctrl-names = "default";
 			status = "okay";
 		};
 	};
diff --git a/arch/arm/boot/dts/kirkwood-dns325.dts b/arch/arm/boot/dts/kirkwood-dns325.dts
index cb9978c652f..5e586ed04c5 100644
--- a/arch/arm/boot/dts/kirkwood-dns325.dts
+++ b/arch/arm/boot/dts/kirkwood-dns325.dts
@@ -13,6 +13,7 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8 earlyprintk";
+		stdout-path = &uart0;
 	};
 
 	gpio-leds {
diff --git a/arch/arm/boot/dts/kirkwood-dnskw.dtsi b/arch/arm/boot/dts/kirkwood-dnskw.dtsi
index d5aa9564a28..113dcf056dc 100644
--- a/arch/arm/boot/dts/kirkwood-dnskw.dtsi
+++ b/arch/arm/boot/dts/kirkwood-dnskw.dtsi
@@ -50,7 +50,7 @@
 	};
 
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 
 			pinctrl-0 = <&pmx_power_back_on &pmx_present_sata0
 				     &pmx_present_sata1 &pmx_fan_tacho
@@ -183,8 +183,6 @@
 };
 
 &nand {
-	pinctrl-0 = <&pmx_nand>;
-	pinctrl-names = "default";
 	status = "okay";
 	chip-delay = <35>;
 
diff --git a/arch/arm/boot/dts/kirkwood-dockstar.dts b/arch/arm/boot/dts/kirkwood-dockstar.dts
index f31312ebd0d..84973634951 100644
--- a/arch/arm/boot/dts/kirkwood-dockstar.dts
+++ b/arch/arm/boot/dts/kirkwood-dockstar.dts
@@ -14,10 +14,11 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8 earlyprintk root=/dev/sda1 rootdelay=10";
+		stdout-path = &uart0;
 	};
 
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 			pmx_usb_power_enable: pmx-usb-power-enable {
 				marvell,pins = "mpp29";
 				marvell,function = "gpio";
diff --git a/arch/arm/boot/dts/kirkwood-dreamplug.dts b/arch/arm/boot/dts/kirkwood-dreamplug.dts
index ef3463e0ae1..6467c792419 100644
--- a/arch/arm/boot/dts/kirkwood-dreamplug.dts
+++ b/arch/arm/boot/dts/kirkwood-dreamplug.dts
@@ -14,10 +14,11 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8 earlyprintk";
+		stdout-path = &uart0;
 	};
 
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 			pmx_led_bluetooth: pmx-led-bluetooth {
 				marvell,pins = "mpp47";
 				marvell,function = "gpio";
@@ -37,13 +38,11 @@
 
 		spi@10600 {
 			status = "okay";
-			pinctrl-0 = <&pmx_spi>;
-			pinctrl-names = "default";
 
 			m25p40@0 {
 				#address-cells = <1>;
 				#size-cells = <1>;
-				compatible = "mx25l1606e";
+				compatible = "mxicy,mx25l1606e";
 				reg = <0>;
 				spi-max-frequency = <50000000>;
 				mode = <0>;
diff --git a/arch/arm/boot/dts/kirkwood-ds109.dts b/arch/arm/boot/dts/kirkwood-ds109.dts
index 772092c94ca..d4bcc1c7f6b 100644
--- a/arch/arm/boot/dts/kirkwood-ds109.dts
+++ b/arch/arm/boot/dts/kirkwood-ds109.dts
@@ -25,6 +25,7 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8";
+		stdout-path = &uart0;
 	};
 
 	gpio-fan-150-32-35 {
diff --git a/arch/arm/boot/dts/kirkwood-ds110jv10.dts b/arch/arm/boot/dts/kirkwood-ds110jv10.dts
index aabafbe0da4..95bf83b91b4 100644
--- a/arch/arm/boot/dts/kirkwood-ds110jv10.dts
+++ b/arch/arm/boot/dts/kirkwood-ds110jv10.dts
@@ -25,6 +25,7 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8";
+		stdout-path = &uart0;
 	};
 
 	gpio-fan-150-32-35 {
diff --git a/arch/arm/boot/dts/kirkwood-ds111.dts b/arch/arm/boot/dts/kirkwood-ds111.dts
index 16ec7fbab57..61f47fbe44d 100644
--- a/arch/arm/boot/dts/kirkwood-ds111.dts
+++ b/arch/arm/boot/dts/kirkwood-ds111.dts
@@ -24,6 +24,7 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8";
+		stdout-path = &uart0;
 	};
 
 	gpio-fan-100-15-35-1 {
diff --git a/arch/arm/boot/dts/kirkwood-ds112.dts b/arch/arm/boot/dts/kirkwood-ds112.dts
index cff1b238876..bf4143c6cb8 100644
--- a/arch/arm/boot/dts/kirkwood-ds112.dts
+++ b/arch/arm/boot/dts/kirkwood-ds112.dts
@@ -24,6 +24,7 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8";
+		stdout-path = &uart0;
 	};
 
 	gpio-fan-100-15-35-1 {
diff --git a/arch/arm/boot/dts/kirkwood-ds209.dts b/arch/arm/boot/dts/kirkwood-ds209.dts
index 330411993d3..6d25093a9ac 100644
--- a/arch/arm/boot/dts/kirkwood-ds209.dts
+++ b/arch/arm/boot/dts/kirkwood-ds209.dts
@@ -24,6 +24,7 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8";
+		stdout-path = &uart0;
 	};
 
 	gpio-fan-150-32-35 {
diff --git a/arch/arm/boot/dts/kirkwood-ds210.dts b/arch/arm/boot/dts/kirkwood-ds210.dts
index 6052eaa37d4..2f1933efcac 100644
--- a/arch/arm/boot/dts/kirkwood-ds210.dts
+++ b/arch/arm/boot/dts/kirkwood-ds210.dts
@@ -26,6 +26,7 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8";
+		stdout-path = &uart0;
 	};
 
 	gpio-fan-150-32-35 {
diff --git a/arch/arm/boot/dts/kirkwood-ds212.dts b/arch/arm/boot/dts/kirkwood-ds212.dts
index 7f76cd30e84..99afd462f95 100644
--- a/arch/arm/boot/dts/kirkwood-ds212.dts
+++ b/arch/arm/boot/dts/kirkwood-ds212.dts
@@ -27,6 +27,7 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8";
+		stdout-path = &uart0;
 	};
 
 	gpio-fan-100-15-35-1 {
diff --git a/arch/arm/boot/dts/kirkwood-ds212j.dts b/arch/arm/boot/dts/kirkwood-ds212j.dts
index 1f83a00f1f7..f5c4213fc67 100644
--- a/arch/arm/boot/dts/kirkwood-ds212j.dts
+++ b/arch/arm/boot/dts/kirkwood-ds212j.dts
@@ -25,6 +25,7 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8";
+		stdout-path = &uart0;
 	};
 
 	gpio-fan-100-32-35 {
diff --git a/arch/arm/boot/dts/kirkwood-ds409.dts b/arch/arm/boot/dts/kirkwood-ds409.dts
index 0a573add44a..e80a962ebba 100644
--- a/arch/arm/boot/dts/kirkwood-ds409.dts
+++ b/arch/arm/boot/dts/kirkwood-ds409.dts
@@ -24,6 +24,7 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8";
+		stdout-path = &uart0;
 	};
 
 	gpio-fan-150-15-18 {
diff --git a/arch/arm/boot/dts/kirkwood-ds409slim.dts b/arch/arm/boot/dts/kirkwood-ds409slim.dts
index 1848a6245fd..cae5af4b88b 100644
--- a/arch/arm/boot/dts/kirkwood-ds409slim.dts
+++ b/arch/arm/boot/dts/kirkwood-ds409slim.dts
@@ -24,6 +24,7 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8";
+		stdout-path = &uart0;
 	};
 
 	gpio-fan-150-32-35 {
diff --git a/arch/arm/boot/dts/kirkwood-ds411.dts b/arch/arm/boot/dts/kirkwood-ds411.dts
index a1737b4311c..623cd4a37d7 100644
--- a/arch/arm/boot/dts/kirkwood-ds411.dts
+++ b/arch/arm/boot/dts/kirkwood-ds411.dts
@@ -24,6 +24,7 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8";
+		stdout-path = &uart0;
 	};
 
 	gpio-fan-100-15-35-1 {
diff --git a/arch/arm/boot/dts/kirkwood-ds411j.dts b/arch/arm/boot/dts/kirkwood-ds411j.dts
index 0cde914ecea..3348e330f07 100644
--- a/arch/arm/boot/dts/kirkwood-ds411j.dts
+++ b/arch/arm/boot/dts/kirkwood-ds411j.dts
@@ -24,6 +24,7 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8";
+		stdout-path = &uart0;
 	};
 
 	gpio-fan-150-15-18 {
diff --git a/arch/arm/boot/dts/kirkwood-ds411slim.dts b/arch/arm/boot/dts/kirkwood-ds411slim.dts
index aef0cadc2c7..a0a1fad8b4d 100644
--- a/arch/arm/boot/dts/kirkwood-ds411slim.dts
+++ b/arch/arm/boot/dts/kirkwood-ds411slim.dts
@@ -24,6 +24,7 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8";
+		stdout-path = &uart0;
 	};
 
 	gpio-fan-100-15-35-1 {
diff --git a/arch/arm/boot/dts/kirkwood-goflexnet.dts b/arch/arm/boot/dts/kirkwood-goflexnet.dts
index eb932942010..aa60a0b049a 100644
--- a/arch/arm/boot/dts/kirkwood-goflexnet.dts
+++ b/arch/arm/boot/dts/kirkwood-goflexnet.dts
@@ -14,10 +14,11 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8 earlyprintk root=/dev/sda1 rootdelay=10";
+		stdout-path = &uart0;
 	};
 
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 			pmx_usb_power_enable: pmx-usb-power-enable {
 				marvell,pins = "mpp29";
 				marvell,function = "gpio";
diff --git a/arch/arm/boot/dts/kirkwood-guruplug-server-plus.dts b/arch/arm/boot/dts/kirkwood-guruplug-server-plus.dts
index 2d51fce74a5..c5a1fc75c7a 100644
--- a/arch/arm/boot/dts/kirkwood-guruplug-server-plus.dts
+++ b/arch/arm/boot/dts/kirkwood-guruplug-server-plus.dts
@@ -14,10 +14,11 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8 earlyprintk";
+		stdout-path = &uart0;
 	};
 
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 			pmx_led_health_r: pmx-led-health-r {
 				marvell,pins = "mpp46";
 				marvell,function = "gpio";
@@ -36,7 +37,6 @@
 			};
 		};
 		serial@12000 {
-			clock-frequency = <200000000>;
 			status = "ok";
 		};
 
@@ -101,13 +101,19 @@
 	status = "okay";
 
 	ethphy0: ethernet-phy@0 {
-		compatible = "marvell,88e1121";
+		/* Marvell 88E1121R */
+		compatible = "ethernet-phy-id0141.0cb0",
+		             "ethernet-phy-ieee802.3-c22";
 		reg = <0>;
+		phy-connection-type = "rgmii-id";
 	};
 
 	ethphy1: ethernet-phy@1 {
-		compatible = "marvell,88e1121";
+		/* Marvell 88E1121R */
+		compatible = "ethernet-phy-id0141.0cb0",
+		             "ethernet-phy-ieee802.3-c22";
 		reg = <1>;
+		phy-connection-type = "rgmii-id";
 	};
 };
 
diff --git a/arch/arm/boot/dts/kirkwood-ib62x0.dts b/arch/arm/boot/dts/kirkwood-ib62x0.dts
index a1add3f215e..bfa5edde179 100644
--- a/arch/arm/boot/dts/kirkwood-ib62x0.dts
+++ b/arch/arm/boot/dts/kirkwood-ib62x0.dts
@@ -14,10 +14,11 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8 earlyprintk";
+		stdout-path = &uart0;
 	};
 
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 			pmx_led_os_red: pmx-led-os-red {
 				marvell,pins = "mpp22";
 				marvell,function = "gpio";
@@ -104,8 +105,6 @@
 
 &nand {
 	status = "okay";
-	pinctrl-0 = <&pmx_nand>;
-	pinctrl-names = "default";
 
 	partition@0 {
 		label = "u-boot";
diff --git a/arch/arm/boot/dts/kirkwood-iconnect.dts b/arch/arm/boot/dts/kirkwood-iconnect.dts
index 8d8c80e3656..38e31d15a62 100644
--- a/arch/arm/boot/dts/kirkwood-iconnect.dts
+++ b/arch/arm/boot/dts/kirkwood-iconnect.dts
@@ -14,6 +14,7 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8 earlyprintk";
+		stdout-path = &uart0;
 		linux,initrd-start = <0x4500040>;
 		linux,initrd-end   = <0x4800000>;
 	};
@@ -29,7 +30,7 @@
 	};
 
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 			pmx_button_reset: pmx-button-reset {
 				marvell,pins = "mpp12";
 				marvell,function = "gpio";
diff --git a/arch/arm/boot/dts/kirkwood-iomega_ix2_200.dts b/arch/arm/boot/dts/kirkwood-iomega_ix2_200.dts
index 59e7a5adeed..05291f3990d 100644
--- a/arch/arm/boot/dts/kirkwood-iomega_ix2_200.dts
+++ b/arch/arm/boot/dts/kirkwood-iomega_ix2_200.dts
@@ -14,10 +14,11 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8 earlyprintk";
+		stdout-path = &uart0;
 	};
 
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 			pinctrl-0 = < &pmx_led_sata_brt_ctrl_1
 				      &pmx_led_sata_brt_ctrl_2
 				      &pmx_led_backup_brt_ctrl_1
diff --git a/arch/arm/boot/dts/kirkwood-km_common.dtsi b/arch/arm/boot/dts/kirkwood-km_common.dtsi
new file mode 100644
index 00000000000..8367c772c76
--- /dev/null
+++ b/arch/arm/boot/dts/kirkwood-km_common.dtsi
@@ -0,0 +1,48 @@
+/ {
+	chosen {
+		bootargs = "console=ttyS0,115200n8 earlyprintk";
+		stdout-path = &uart0;
+	};
+
+	mbus {
+		pcie-controller {
+			status = "okay";
+
+			pcie@1,0 {
+				status = "okay";
+			};
+		};
+	};
+
+	ocp@f1000000 {
+		pinctrl: pin-controller@10000 {
+			pinctrl-0 = < &pmx_i2c_gpio_sda &pmx_i2c_gpio_scl >;
+			pinctrl-names = "default";
+
+			pmx_i2c_gpio_sda: pmx-gpio-sda {
+				marvell,pins = "mpp8";
+				marvell,function = "gpio";
+			};
+			pmx_i2c_gpio_scl: pmx-gpio-scl {
+				marvell,pins = "mpp9";
+				marvell,function = "gpio";
+			};
+		};
+
+		serial@12000 {
+			status = "okay";
+		};
+	};
+
+	i2c@0 {
+		compatible = "i2c-gpio";
+		gpios = < &gpio0 8 GPIO_ACTIVE_HIGH		/* sda */
+			  &gpio0 9 GPIO_ACTIVE_HIGH>;		/* scl */
+		i2c-gpio,delay-us = <2>;	/* ~100 kHz */
+	};
+};
+
+&nand {
+	status = "okay";
+	chip-delay = <25>;
+};
diff --git a/arch/arm/boot/dts/kirkwood-km_fixedeth.dts b/arch/arm/boot/dts/kirkwood-km_fixedeth.dts
new file mode 100644
index 00000000000..9895f2b10f8
--- /dev/null
+++ b/arch/arm/boot/dts/kirkwood-km_fixedeth.dts
@@ -0,0 +1,23 @@
+/dts-v1/;
+
+#include "kirkwood.dtsi"
+#include "kirkwood-98dx4122.dtsi"
+#include "kirkwood-km_common.dtsi"
+
+/ {
+	model = "Keymile Kirkwood Fixed Eth";
+	compatible = "keymile,km_fixedeth", "marvell,kirkwood-98DX4122", "marvell,kirkwood";
+
+	memory {
+		device_type = "memory";
+		reg = <0x00000000 0x10000000>;
+	};
+};
+
+&eth0 {
+	status = "okay";
+	ethernet0-port@0 {
+		speed = <1000>;  /* <SPEED_1000> */
+		duplex = <1>; /* <DUPLEX_FULL> */
+	};
+};
diff --git a/arch/arm/boot/dts/kirkwood-km_kirkwood.dts b/arch/arm/boot/dts/kirkwood-km_kirkwood.dts
index 04a1e44541b..235bf382fff 100644
--- a/arch/arm/boot/dts/kirkwood-km_kirkwood.dts
+++ b/arch/arm/boot/dts/kirkwood-km_kirkwood.dts
@@ -2,6 +2,7 @@
 
 #include "kirkwood.dtsi"
 #include "kirkwood-98dx4122.dtsi"
+#include "kirkwood-km_common.dtsi"
 
 / {
 	model = "Keymile Kirkwood Reference Design";
@@ -11,44 +12,6 @@
 		device_type = "memory";
 		reg = <0x00000000 0x08000000>;
 	};
-
-	chosen {
-		bootargs = "console=ttyS0,115200n8 earlyprintk";
-	};
-
-	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
-			pinctrl-0 = < &pmx_i2c_gpio_sda &pmx_i2c_gpio_scl >;
-			pinctrl-names = "default";
-
-			pmx_i2c_gpio_sda: pmx-gpio-sda {
-				marvell,pins = "mpp8";
-				marvell,function = "gpio";
-			};
-			pmx_i2c_gpio_scl: pmx-gpio-scl {
-				marvell,pins = "mpp9";
-				marvell,function = "gpio";
-			};
-		};
-
-		serial@12000 {
-			status = "ok";
-		};
-	};
-
-	i2c@0 {
-		compatible = "i2c-gpio";
-		gpios = < &gpio0 8 GPIO_ACTIVE_HIGH		/* sda */
-			  &gpio0 9 GPIO_ACTIVE_HIGH>;		/* scl */
-		i2c-gpio,delay-us = <2>;	/* ~100 kHz */
-	};
-};
-
-&nand {
-	pinctrl-0 = <&pmx_nand>;
-	pinctrl-names = "default";
-	status = "ok";
-	chip-delay = <25>;
 };
 
 &mdio {
diff --git a/arch/arm/boot/dts/kirkwood-laplug.dts b/arch/arm/boot/dts/kirkwood-laplug.dts
index c9e82eff9bf..24425660e97 100644
--- a/arch/arm/boot/dts/kirkwood-laplug.dts
+++ b/arch/arm/boot/dts/kirkwood-laplug.dts
@@ -24,6 +24,7 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8 earlyprintk";
+		stdout-path = &uart0;
 	};
 
 	mbus {
@@ -37,24 +38,20 @@
 
 	ocp@f1000000 {
 		serial@12000 {
-			pinctrl-0 = <&pmx_uart0>;
-			pinctrl-names = "default";
 			status = "okay";
 		};
 
 		i2c@11000 {
-			pinctrl-0 = <&pmx_twsi0>;
-			pinctrl-names = "default";
 			status = "okay";
 
 			eeprom@50 {
-				compatible = "at,24c04";
+				compatible = "atmel,24c04";
 				pagesize = <16>;
 				reg = <0x50>;
 			};
 		};
 
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 			pmx_usb_power_enable: pmx-usb-power-enable {
 				marvell,pins = "mpp14";
 				marvell,function = "gpio";
@@ -139,7 +136,6 @@
 &nand {
 	/* Total size : 512MB */
 	status = "okay";
-	pinctrl-0 = <&pmx_nand>;
 
 	partition@0 {
 		label = "u-boot";
diff --git a/arch/arm/boot/dts/kirkwood-lsxl.dtsi b/arch/arm/boot/dts/kirkwood-lsxl.dtsi
index 1656653d339..53484474df1 100644
--- a/arch/arm/boot/dts/kirkwood-lsxl.dtsi
+++ b/arch/arm/boot/dts/kirkwood-lsxl.dtsi
@@ -4,10 +4,11 @@
 / {
 	chosen {
 		bootargs = "console=ttyS0,115200n8 earlyprintk";
+		stdout-path = &uart0;
 	};
 
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 			pmx_power_hdd: pmx-power-hdd {
 				marvell,pins = "mpp10";
 				marvell,function = "gpo";
diff --git a/arch/arm/boot/dts/kirkwood-mplcec4.dts b/arch/arm/boot/dts/kirkwood-mplcec4.dts
index 73722c06750..f3a99183751 100644
--- a/arch/arm/boot/dts/kirkwood-mplcec4.dts
+++ b/arch/arm/boot/dts/kirkwood-mplcec4.dts
@@ -12,9 +12,10 @@
                 reg = <0x00000000 0x20000000>;
         };
 
-        chosen {
-                bootargs = "console=ttyS0,115200n8 earlyprintk";
-        };
+	chosen {
+		bootargs = "console=ttyS0,115200n8 earlyprintk";
+		stdout-path = &uart0;
+	};
 
 	mbus {
 		pcie-controller {
@@ -27,7 +28,7 @@
 	};
 
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 			pmx_led_health: pmx-led-health {
 				marvell,pins = "mpp7";
 				marvell,function = "gpo";
@@ -89,11 +90,9 @@
 
                 };
 
-                serial@12000 {
-                        status = "ok";
-                        pinctrl-0 = <&pmx_uart0>;
-                        pinctrl-names = "default";
-                };
+		serial@12000 {
+			status = "okay";
+		};
 
 		rtc@10300 {
 			status = "disabled";
@@ -163,8 +162,6 @@
 };
 
 &nand {
-	pinctrl-0 = <&pmx_nand>;
-	pinctrl-names = "default";
 	status = "okay";
 
 	partition@0 {
diff --git a/arch/arm/boot/dts/kirkwood-mv88f6281gtw-ge.dts b/arch/arm/boot/dts/kirkwood-mv88f6281gtw-ge.dts
index 2cb0dc52916..8f76d28759a 100644
--- a/arch/arm/boot/dts/kirkwood-mv88f6281gtw-ge.dts
+++ b/arch/arm/boot/dts/kirkwood-mv88f6281gtw-ge.dts
@@ -28,10 +28,21 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8 earlyprintk";
+		stdout-path = &uart0;
 	};
 
+	mbus {
+		pcie-controller {
+			status = "okay";
+
+			pcie@1,0 {
+				status = "okay";
+			};
+		};
+        };
+
 	ocp@f1000000 {
-		pinctrl@10000 {
+		pin-controller@10000 {
 			pmx_usb_led: pmx-usb-led {
 				marvell,pins = "mpp12";
 				marvell,function = "gpo";
@@ -49,14 +60,12 @@
 		};
 
 		spi@10600 {
-			pinctrl-0 = <&pmx_spi>;
-			pinctrl-names = "default";
 			status = "okay";
 
 			flash@0 {
 				#address-cells = <1>;
 				#size-cells = <1>;
-				compatible = "mx25l12805d";
+				compatible = "mxicy,mx25l12805d";
 				reg = <0>;
 				spi-max-frequency = <50000000>;
 				mode = <0>;
@@ -64,22 +73,11 @@
 		};
 
 		serial@12000 {
-			pinctrl-0 = <&pmx_uart0>;
-			pinctrl-names = "default";
-			clock-frequency = <200000000>;
-			status = "ok";
-		};
-
-		ehci@50000 {
 			status = "okay";
 		};
 
-		pcie-controller {
+		ehci@50000 {
 			status = "okay";
-
-			pcie@1,0 {
-				status = "okay";
-			};
 		};
 	};
 
diff --git a/arch/arm/boot/dts/kirkwood-netgear_readynas_duo_v2.dts b/arch/arm/boot/dts/kirkwood-netgear_readynas_duo_v2.dts
index 4838478019c..fd733c63bc2 100644
--- a/arch/arm/boot/dts/kirkwood-netgear_readynas_duo_v2.dts
+++ b/arch/arm/boot/dts/kirkwood-netgear_readynas_duo_v2.dts
@@ -25,6 +25,7 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8 earlyprintk";
+		stdout-path = &uart0;
 	};
 
 	mbus {
@@ -38,7 +39,7 @@
 	};
 
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 			pmx_button_power: pmx-button-power {
 				marvell,pins = "mpp47";
 				marvell,function = "gpio";
@@ -112,8 +113,6 @@
 		};
 
 		serial@12000 {
-			pinctrl-0 = <&pmx_uart0>;
-			pinctrl-names = "default";
 			status = "okay";
 		};
 
diff --git a/arch/arm/boot/dts/kirkwood-netgear_readynas_nv+_v2.dts b/arch/arm/boot/dts/kirkwood-netgear_readynas_nv+_v2.dts
index 7c8a0d9d8d1..b514d643fb6 100644
--- a/arch/arm/boot/dts/kirkwood-netgear_readynas_nv+_v2.dts
+++ b/arch/arm/boot/dts/kirkwood-netgear_readynas_nv+_v2.dts
@@ -25,6 +25,7 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8 earlyprintk";
+		stdout-path = &uart0;
 	};
 
 	mbus {
@@ -40,7 +41,7 @@
 	};
 
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 			pmx_button_power: pmx-button-power {
 				marvell,pins = "mpp47";
 				marvell,function = "gpio";
@@ -119,8 +120,6 @@
 		};
 
 		serial@12000 {
-			pinctrl-0 = <&pmx_uart0>;
-			pinctrl-names = "default";
 			status = "okay";
 		};
 
diff --git a/arch/arm/boot/dts/kirkwood-ns2-common.dtsi b/arch/arm/boot/dts/kirkwood-ns2-common.dtsi
index 743152f31a8..fe6c0246db1 100644
--- a/arch/arm/boot/dts/kirkwood-ns2-common.dtsi
+++ b/arch/arm/boot/dts/kirkwood-ns2-common.dtsi
@@ -4,10 +4,11 @@
 / {
 	chosen {
 		bootargs = "console=ttyS0,115200n8";
+		stdout-path = &uart0;
 	};
 
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 			pmx_ns2_sata0: pmx-ns2-sata0 {
 				marvell,pins = "mpp21";
 				marvell,function = "sata0";
@@ -19,20 +20,16 @@
 		};
 
 		serial@12000 {
-			pinctrl-0 = <&pmx_uart0>;
-			pinctrl-names = "default";
 			status = "okay";
 		};
 
 		spi@10600 {
-			pinctrl-0 = <&pmx_spi>;
-			pinctrl-names = "default";
 			status = "okay";
 
 			flash@0 {
 				#address-cells = <1>;
 				#size-cells = <1>;
-				compatible = "mx25l4005a";
+				compatible = "mxicy,mx25l4005a";
 				reg = <0>;
 				spi-max-frequency = <20000000>;
 				mode = <0>;
@@ -45,12 +42,10 @@
 		};
 
 		i2c@11000 {
-			pinctrl-0 = <&pmx_twsi0>;
-			pinctrl-names = "default";
 			status = "okay";
 
 			eeprom@50 {
-				compatible = "at,24c04";
+				compatible = "atmel,24c04";
 				pagesize = <16>;
 				reg = <0x50>;
 			};
diff --git a/arch/arm/boot/dts/kirkwood-nsa310.dts b/arch/arm/boot/dts/kirkwood-nsa310.dts
index 03fa24cf334..6139df0f376 100644
--- a/arch/arm/boot/dts/kirkwood-nsa310.dts
+++ b/arch/arm/boot/dts/kirkwood-nsa310.dts
@@ -1,6 +1,6 @@
 /dts-v1/;
 
-#include "kirkwood-nsa310-common.dtsi"
+#include "kirkwood-nsa3x0-common.dtsi"
 
 / {
 	compatible = "zyxel,nsa310", "marvell,kirkwood-88f6281", "marvell,kirkwood";
@@ -12,6 +12,7 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200";
+		stdout-path = &uart0;
 	};
 
 	mbus {
@@ -25,7 +26,7 @@
 	};
 
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 			pinctrl-0 = <&pmx_unknown>;
 			pinctrl-names = "default";
 
@@ -59,26 +60,6 @@
 				marvell,function = "gpio";
 			};
 
-			pmx_btn_reset: pmx-btn-reset {
-				marvell,pins = "mpp36";
-				marvell,function = "gpio";
-			};
-
-			pmx_btn_copy: pmx-btn-copy {
-				marvell,pins = "mpp37";
-				marvell,function = "gpio";
-			};
-
-			pmx_led_copy_green: pmx-led-copy-green {
-				marvell,pins = "mpp39";
-				marvell,function = "gpio";
-			};
-
-			pmx_led_copy_red: pmx-led-copy-red {
-				marvell,pins = "mpp40";
-				marvell,function = "gpio";
-			};
-
 			pmx_led_hdd_green: pmx-led-hdd-green {
 				marvell,pins = "mpp41";
 				marvell,function = "gpio";
@@ -94,46 +75,18 @@
 				marvell,function = "gpio";
 			};
 
-			pmx_btn_power: pmx-btn-power {
-				marvell,pins = "mpp46";
-				marvell,function = "gpio";
-			};
 		};
 
 		i2c@11000 {
 			status = "okay";
 
 			adt7476: adt7476a@2e {
-				compatible = "adt7476";
+				compatible = "adi,adt7476";
 				reg = <0x2e>;
 			};
 		};
 	};
 
-	gpio_keys {
-		compatible = "gpio-keys";
-		#address-cells = <1>;
-		#size-cells = <0>;
-		pinctrl-0 = <&pmx_btn_reset &pmx_btn_copy &pmx_btn_power>;
-		pinctrl-names = "default";
-
-		button@1 {
-			label = "Power Button";
-			linux,code = <KEY_POWER>;
-			gpios = <&gpio1 14 GPIO_ACTIVE_HIGH>;
-		};
-		button@2 {
-			label = "Copy Button";
-			linux,code = <KEY_COPY>;
-			gpios = <&gpio1 5 GPIO_ACTIVE_LOW>;
-		};
-		button@3 {
-			label = "Reset Button";
-			linux,code = <KEY_RESTART>;
-			gpios = <&gpio1 4 GPIO_ACTIVE_LOW>;
-		};
-	};
-
 	gpio-leds {
 		compatible = "gpio-leds";
 		pinctrl-0 = <&pmx_led_esata_green &pmx_led_esata_red
diff --git a/arch/arm/boot/dts/kirkwood-nsa310a.dts b/arch/arm/boot/dts/kirkwood-nsa310a.dts
index a5e77945286..3d2b3d494c1 100644
--- a/arch/arm/boot/dts/kirkwood-nsa310a.dts
+++ b/arch/arm/boot/dts/kirkwood-nsa310a.dts
@@ -1,6 +1,6 @@
 /dts-v1/;
 
-#include "kirkwood-nsa310-common.dtsi"
+#include "kirkwood-nsa3x0-common.dtsi"
 
 /*
  * There are at least two different NSA310 designs. This variant does
@@ -17,10 +17,11 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200";
+		stdout-path = &uart0;
 	};
 
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 			pinctrl-names = "default";
 
 			pmx_led_esata_green: pmx-led-esata-green {
@@ -38,11 +39,6 @@
 				marvell,function = "gpio";
 			};
 
-			pmx_usb_power_off: pmx-usb-power-off {
-				marvell,pins = "mpp21";
-				marvell,function = "gpio";
-			};
-
 			pmx_led_sys_green: pmx-led-sys-green {
 				marvell,pins = "mpp28";
 				marvell,function = "gpio";
@@ -53,26 +49,6 @@
 				marvell,function = "gpio";
 			};
 
-			pmx_btn_reset: pmx-btn-reset {
-				marvell,pins = "mpp36";
-				marvell,function = "gpio";
-			};
-
-			pmx_btn_copy: pmx-btn-copy {
-				marvell,pins = "mpp37";
-				marvell,function = "gpio";
-			};
-
-			pmx_led_copy_green: pmx-led-copy-green {
-				marvell,pins = "mpp39";
-				marvell,function = "gpio";
-			};
-
-			pmx_led_copy_red: pmx-led-copy-red {
-				marvell,pins = "mpp40";
-				marvell,function = "gpio";
-			};
-
 			pmx_led_hdd_green: pmx-led-hdd-green {
 				marvell,pins = "mpp41";
 				marvell,function = "gpio";
@@ -83,45 +59,18 @@
 				marvell,function = "gpio";
 			};
 
-			pmx_btn_power: pmx-btn-power {
-				marvell,pins = "mpp46";
-				marvell,function = "gpio";
-			};
-
 		};
 
 		i2c@11000 {
 			status = "okay";
 
 			lm85: lm85@2e {
-				compatible = "lm85";
+				compatible = "national,lm85";
 				reg = <0x2e>;
 			};
 		};
 	};
 
-	gpio_keys {
-		compatible = "gpio-keys";
-		#address-cells = <1>;
-		#size-cells = <0>;
-
-		button@1 {
-			label = "Power Button";
-			linux,code = <KEY_POWER>;
-			gpios = <&gpio1 14 GPIO_ACTIVE_HIGH>;
-		};
-		button@2 {
-			label = "Copy Button";
-			linux,code = <KEY_COPY>;
-			gpios = <&gpio1 5 GPIO_ACTIVE_LOW>;
-		};
-		button@3 {
-			label = "Reset Button";
-			linux,code = <KEY_RESTART>;
-			gpios = <&gpio1 4 GPIO_ACTIVE_LOW>;
-		};
-	};
-
 	gpio-leds {
 		compatible = "gpio-leds";
 
diff --git a/arch/arm/boot/dts/kirkwood-nsa320.dts b/arch/arm/boot/dts/kirkwood-nsa320.dts
new file mode 100644
index 00000000000..24f686d1044
--- /dev/null
+++ b/arch/arm/boot/dts/kirkwood-nsa320.dts
@@ -0,0 +1,215 @@
+/* Device tree file for the Zyxel NSA 320 NAS box.
+ *
+ * Copyright (c) 2014, Adam Baker <linux@baker-net.org.uk>
+ *
+ * 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.
+ *
+ * Based upon the board setup file created by Peter Schildmann */
+
+/dts-v1/;
+
+#include "kirkwood-nsa3x0-common.dtsi"
+
+/ {
+	model = "Zyxel NSA320";
+	compatible = "zyxel,nsa320", "marvell,kirkwood-88f6281", "marvell,kirkwood";
+
+	memory {
+		device_type = "memory";
+		reg = <0x00000000 0x20000000>;
+	};
+
+	chosen {
+		bootargs = "console=ttyS0,115200";
+		stdout-path = &uart0;
+	};
+
+	mbus {
+		pcie-controller {
+			status = "okay";
+
+			pcie@1,0 {
+				status = "okay";
+			};
+		};
+	};
+
+	ocp@f1000000 {
+		pinctrl: pin-controller@10000 {
+			pinctrl-names = "default";
+
+			/* SATA Activity and Present pins are not connected */
+			pmx_sata0: pmx-sata0 {
+				marvell,pins ;
+				marvell,function = "sata0";
+			};
+
+			pmx_sata1: pmx-sata1 {
+				marvell,pins ;
+				marvell,function = "sata1";
+			};
+
+			pmx_led_hdd2_green: pmx-led-hdd2-green {
+				marvell,pins = "mpp12";
+				marvell,function = "gpio";
+			};
+
+			pmx_led_hdd2_red: pmx-led-hdd2-red {
+				marvell,pins = "mpp13";
+				marvell,function = "gpio";
+			};
+
+			pmx_mcu_data: pmx-mcu-data {
+				marvell,pins = "mpp14";
+				marvell,function = "gpio";
+			};
+
+			pmx_led_usb_green: pmx-led-usb-green {
+				marvell,pins = "mpp15";
+				marvell,function = "gpio";
+			};
+
+			pmx_mcu_clk: pmx-mcu-clk {
+				marvell,pins = "mpp16";
+				marvell,function = "gpio";
+			};
+
+			pmx_mcu_act: pmx-mcu-act {
+				marvell,pins = "mpp17";
+				marvell,function = "gpio";
+			};
+
+			pmx_led_sys_green: pmx-led-sys-green {
+				marvell,pins = "mpp28";
+				marvell,function = "gpio";
+			};
+
+			pmx_led_sys_orange: pmx-led-sys-orange {
+				marvell,pins = "mpp29";
+				marvell,function = "gpio";
+			};
+
+			pmx_led_hdd1_green: pmx-led-hdd1-green {
+				marvell,pins = "mpp41";
+				marvell,function = "gpio";
+			};
+
+			pmx_led_hdd1_red: pmx-led-hdd1-red {
+				marvell,pins = "mpp42";
+				marvell,function = "gpio";
+			};
+
+			pmx_htp: pmx-htp {
+				marvell,pins = "mpp43";
+				marvell,function = "gpio";
+			};
+
+			/* Buzzer needs to be switched at around 1kHz so is
+			   not compatible with the gpio-beeper driver. */
+			pmx_buzzer: pmx-buzzer {
+				marvell,pins = "mpp44";
+				marvell,function = "gpio";
+			};
+
+			pmx_vid_b1: pmx-vid-b1 {
+				marvell,pins = "mpp45";
+				marvell,function = "gpio";
+			};
+
+			pmx_power_resume_data: pmx-power-resume-data {
+				marvell,pins = "mpp47";
+				marvell,function = "gpio";
+			};
+
+			pmx_power_resume_clk: pmx-power-resume-clk {
+				marvell,pins = "mpp49";
+				marvell,function = "gpio";
+			};
+		};
+
+		i2c@11000 {
+			status = "okay";
+
+			pcf8563: pcf8563@51 {
+				compatible = "nxp,pcf8563";
+				reg = <0x51>;
+			};
+		};
+	};
+
+	regulators {
+		usb0_power: regulator@1 {
+			enable-active-high;
+		};
+	};
+
+	gpio-leds {
+		compatible = "gpio-leds";
+		pinctrl-0 = <&pmx_led_hdd2_green &pmx_led_hdd2_red
+			     &pmx_led_usb_green
+			     &pmx_led_sys_green &pmx_led_sys_orange
+			     &pmx_led_copy_green &pmx_led_copy_red
+			     &pmx_led_hdd1_green &pmx_led_hdd1_red>;
+		pinctrl-names = "default";
+
+		green-sys {
+			label = "nsa320:green:sys";
+			gpios = <&gpio0 28 GPIO_ACTIVE_HIGH>;
+		};
+		orange-sys {
+			label = "nsa320:orange:sys";
+			gpios = <&gpio0 29 GPIO_ACTIVE_HIGH>;
+		};
+		green-hdd1 {
+			label = "nsa320:green:hdd1";
+			gpios = <&gpio1 9 GPIO_ACTIVE_HIGH>;
+		};
+		red-hdd1 {
+			label = "nsa320:red:hdd1";
+			gpios = <&gpio1 10 GPIO_ACTIVE_HIGH>;
+		};
+		green-hdd2 {
+			label = "nsa320:green:hdd2";
+			gpios = <&gpio0 12 GPIO_ACTIVE_HIGH>;
+		};
+		red-hdd2 {
+			label = "nsa320:red:hdd2";
+			gpios = <&gpio0 13 GPIO_ACTIVE_HIGH>;
+		};
+		green-usb {
+			label = "nsa320:green:usb";
+			gpios = <&gpio0 15 GPIO_ACTIVE_HIGH>;
+		};
+		green-copy {
+			label = "nsa320:green:copy";
+			gpios = <&gpio1 7 GPIO_ACTIVE_HIGH>;
+		};
+		red-copy {
+			label = "nsa320:red:copy";
+			gpios = <&gpio1 8 GPIO_ACTIVE_HIGH>;
+		};
+	};
+
+	/* The following pins are currently not assigned to a driver,
+	   some of them should be configured as inputs.
+	pinctrl-0 = <&pmx_mcu_data &pmx_mcu_clk &pmx_mcu_act
+		     &pmx_htp &pmx_vid_b1
+		     &pmx_power_resume_data &pmx_power_resume_clk>; */
+};
+
+&mdio {
+	status = "okay";
+	ethphy0: ethernet-phy@1 {
+		reg = <1>;
+	};
+};
+
+&eth0 {
+	status = "okay";
+	ethernet0-port@0 {
+		phy-handle = <&ethphy0>;
+	};
+};
diff --git a/arch/arm/boot/dts/kirkwood-nsa310-common.dtsi b/arch/arm/boot/dts/kirkwood-nsa3x0-common.dtsi
index aa78c2d11fe..2075a2e828f 100644
--- a/arch/arm/boot/dts/kirkwood-nsa310-common.dtsi
+++ b/arch/arm/boot/dts/kirkwood-nsa3x0-common.dtsi
@@ -4,18 +4,53 @@
 / {
 	model = "ZyXEL NSA310";
 
+	mbus {
+		pcie-controller {
+			status = "okay";
+
+			pcie@1,0 {
+				status = "okay";
+			};
+		};
+	};
+
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 
-			pmx_usb_power_off: pmx-usb-power-off {
+			pmx_usb_power: pmx-usb-power {
 				marvell,pins = "mpp21";
 				marvell,function = "gpio";
 			};
+
 			pmx_pwr_off: pmx-pwr-off {
 				marvell,pins = "mpp48";
 				marvell,function = "gpio";
 			};
 
+			pmx_btn_reset: pmx-btn-reset {
+				marvell,pins = "mpp36";
+				marvell,function = "gpio";
+			};
+
+			pmx_btn_copy: pmx-btn-copy {
+				marvell,pins = "mpp37";
+				marvell,function = "gpio";
+			};
+
+			pmx_btn_power: pmx-btn-power {
+				marvell,pins = "mpp46";
+				marvell,function = "gpio";
+			};
+
+			pmx_led_copy_green: pmx-led-copy-green {
+				marvell,pins = "mpp39";
+				marvell,function = "gpio";
+			};
+
+			pmx_led_copy_red: pmx-led-copy-red {
+				marvell,pins = "mpp40";
+				marvell,function = "gpio";
+			};
 		};
 
 		serial@12000 {
@@ -26,14 +61,6 @@
 			status = "okay";
 			nr-ports = <2>;
 		};
-
-		pcie-controller {
-			status = "okay";
-
-			pcie@1,0 {
-				status = "okay";
-			};
-		};
 	};
 
 	gpio_poweroff {
@@ -43,17 +70,42 @@
 		gpios = <&gpio1 16 GPIO_ACTIVE_HIGH>;
 	};
 
+	gpio_keys {
+		compatible = "gpio-keys";
+		#address-cells = <1>;
+		#size-cells = <0>;
+		pinctrl-0 = <&pmx_btn_reset &pmx_btn_copy &pmx_btn_power>;
+		pinctrl-names = "default";
+
+		button@1 {
+			label = "Power Button";
+			linux,code = <KEY_POWER>;
+			gpios = <&gpio1 14 GPIO_ACTIVE_HIGH>;
+		};
+		button@2 {
+			label = "Copy Button";
+			linux,code = <KEY_COPY>;
+			gpios = <&gpio1 5 GPIO_ACTIVE_LOW>;
+		};
+		button@3 {
+			label = "Reset Button";
+			linux,code = <KEY_RESTART>;
+			gpios = <&gpio1 4 GPIO_ACTIVE_LOW>;
+		};
+	};
+
+
 	regulators {
 		compatible = "simple-bus";
 		#address-cells = <1>;
 		#size-cells = <0>;
-		pinctrl-0 = <&pmx_usb_power_off>;
+		pinctrl-0 = <&pmx_usb_power>;
 		pinctrl-names = "default";
 
-		usb0_power_off: regulator@1 {
+		usb0_power: regulator@1 {
 			compatible = "regulator-fixed";
 			reg = <1>;
-			regulator-name = "USB Power Off";
+			regulator-name = "USB Power";
 			regulator-min-microvolt = <5000000>;
 			regulator-max-microvolt = <5000000>;
 			regulator-always-on;
diff --git a/arch/arm/boot/dts/kirkwood-openblocks_a6.dts b/arch/arm/boot/dts/kirkwood-openblocks_a6.dts
index b88da9392c3..fb9dc227255 100644
--- a/arch/arm/boot/dts/kirkwood-openblocks_a6.dts
+++ b/arch/arm/boot/dts/kirkwood-openblocks_a6.dts
@@ -14,19 +14,16 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8 earlyprintk";
+		stdout-path = &uart0;
 	};
 
 	ocp@f1000000 {
 		serial@12000 {
-			status = "ok";
-			pinctrl-0 = <&pmx_uart0>;
-			pinctrl-names = "default";
+			status = "okay";
 		};
 
 		serial@12100 {
-			status = "ok";
-			pinctrl-0 = <&pmx_uart1>;
-			pinctrl-names = "default";
+			status = "okay";
 		};
 
 		sata@80000 {
@@ -36,16 +33,14 @@
 
 		i2c@11100 {
 			status = "okay";
-			pinctrl-0 = <&pmx_twsi1>;
-			pinctrl-names = "default";
 
 			s35390a: s35390a@30 {
-				compatible = "s35390a";
+				compatible = "sii,s35390a";
 				reg = <0x30>;
 			};
 		};
 
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 			pinctrl-0 = <&pmx_dip_switches &pmx_gpio_header>;
 			pinctrl-names = "default";
 
@@ -133,8 +128,6 @@
 &nand {
 	chip-delay = <25>;
 	status = "okay";
-	pinctrl-0 = <&pmx_nand>;
-	pinctrl-names = "default";
 
 	partition@0 {
 		label = "uboot";
diff --git a/arch/arm/boot/dts/kirkwood-openblocks_a7.dts b/arch/arm/boot/dts/kirkwood-openblocks_a7.dts
index b2f7cae0683..d5e3bc51896 100644
--- a/arch/arm/boot/dts/kirkwood-openblocks_a7.dts
+++ b/arch/arm/boot/dts/kirkwood-openblocks_a7.dts
@@ -26,19 +26,16 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8 earlyprintk";
+		stdout-path = &uart0;
 	};
 
 	ocp@f1000000 {
 		serial@12000 {
-			status = "ok";
-			pinctrl-0 = <&pmx_uart0>;
-			pinctrl-names = "default";
+			status = "okay";
 		};
 
 		serial@12100 {
-			status = "ok";
-			pinctrl-0 = <&pmx_uart1>;
-			pinctrl-names = "default";
+			status = "okay";
 		};
 
 		sata@80000 {
@@ -48,16 +45,14 @@
 
 		i2c@11100 {
 			status = "okay";
-			pinctrl-0 = <&pmx_twsi1>;
-			pinctrl-names = "default";
 
 			s24c02: s24c02@50 {
-				compatible = "24c02";
+				compatible = "atmel,24c02";
 				reg = <0x50>;
 			};
 		};
 
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 			pinctrl-0 = <&pmx_dip_switches &pmx_gpio_header>;
 			pinctrl-names = "default";
 
@@ -109,13 +104,6 @@
 				marvell,pins = "mpp41", "mpp42", "mpp43";
 				marvell,function = "gpio";
 			};
-
-			pmx_ge1: pmx-ge1 {
-				marvell,pins = "mpp20", "mpp21", "mpp22", "mpp23",
-					       "mpp24", "mpp25", "mpp26", "mpp27",
-					       "mpp30", "mpp31", "mpp32", "mpp33";
-				marvell,function = "ge1";
-			};
 		};
 	};
 
@@ -158,8 +146,6 @@
 &nand {
 	chip-delay = <25>;
 	status = "okay";
-	pinctrl-0 = <&pmx_nand>;
-	pinctrl-names = "default";
 
 	partition@0 {
 		label = "uboot";
@@ -213,8 +199,6 @@
 
 &eth1 {
 	status = "okay";
-	pinctrl-0 = <&pmx_ge1>;
-	pinctrl-names = "default";
 	ethernet1-port@0 {
 		phy-handle = <&ethphy1>;
 	};
diff --git a/arch/arm/boot/dts/kirkwood-openrd-base.dts b/arch/arm/boot/dts/kirkwood-openrd-base.dts
new file mode 100644
index 00000000000..8af58999606
--- /dev/null
+++ b/arch/arm/boot/dts/kirkwood-openrd-base.dts
@@ -0,0 +1,42 @@
+/*
+ * Marvell OpenRD Base Board Description
+ *
+ * Andrew Lunn <andrew@lunn.ch>
+ *
+ * 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.
+ *
+ * This file contains the definitions that are specific to OpenRD
+ * base variant of the Marvell Kirkwood Development Board.
+ */
+
+/dts-v1/;
+
+#include "kirkwood-openrd.dtsi"
+
+/ {
+	model = "OpenRD Base";
+	compatible = "marvell,openrd-base", "marvell,openrd", "marvell,kirkwood-88f6281", "marvell,kirkwood";
+
+	ocp@f1000000 {
+		serial@12100 {
+			status = "okay";
+		};
+	};
+};
+
+&mdio {
+	status = "okay";
+
+	ethphy0: ethernet-phy@8 {
+		reg = <8>;
+	};
+};
+
+&eth0 {
+	status = "okay";
+	ethernet0-port@0 {
+		phy-handle = <&ethphy0>;
+	};
+};
diff --git a/arch/arm/boot/dts/kirkwood-openrd-client.dts b/arch/arm/boot/dts/kirkwood-openrd-client.dts
new file mode 100644
index 00000000000..887b9c1fee4
--- /dev/null
+++ b/arch/arm/boot/dts/kirkwood-openrd-client.dts
@@ -0,0 +1,73 @@
+/*
+ * Marvell OpenRD Client Board Description
+ *
+ * Andrew Lunn <andrew@lunn.ch>
+ *
+ * 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.
+ *
+ * This file contains the definitions that are specific to OpenRD
+ * client variant of the Marvell Kirkwood Development Board.
+ */
+
+/dts-v1/;
+
+#include "kirkwood-openrd.dtsi"
+
+/ {
+	model = "OpenRD Client";
+	compatible = "marvell,openrd-client", "marvell,openrd", "marvell,kirkwood-88f6281", "marvell,kirkwood";
+
+	ocp@f1000000 {
+		i2c@11000 {
+			status = "okay";
+			clock-frequency = <400000>;
+
+			cs42l51: cs42l51@4a {
+				compatible = "cirrus,cs42l51";
+				reg = <0x4a>;
+			};
+		};
+	};
+
+	sound {
+		compatible = "simple-audio-card";
+		simple-audio-card,format = "i2s";
+		simple-audio-card,mclk-fs = <256>;
+
+		simple-audio-card,cpu {
+			sound-dai = <&audio0>;
+		};
+
+		simple-audio-card,codec {
+			sound-dai = <&cs42l51>;
+		};
+	};
+};
+
+&mdio {
+	status = "okay";
+
+	ethphy0: ethernet-phy@8 {
+		reg = <8>;
+	};
+	ethphy1: ethernet-phy@24 {
+		reg = <24>;
+	};
+};
+
+&eth0 {
+	status = "okay";
+	ethernet0-port@0 {
+		phy-handle = <&ethphy0>;
+	};
+};
+
+&eth1 {
+	status = "okay";
+	ethernet1-port@0 {
+		phy-handle = <&ethphy1>;
+	};
+};
+
diff --git a/arch/arm/boot/dts/kirkwood-openrd-ultimate.dts b/arch/arm/boot/dts/kirkwood-openrd-ultimate.dts
new file mode 100644
index 00000000000..9f12f8b53e2
--- /dev/null
+++ b/arch/arm/boot/dts/kirkwood-openrd-ultimate.dts
@@ -0,0 +1,58 @@
+/*
+ * Marvell OpenRD Ultimate Board Description
+ *
+ * Andrew Lunn <andrew@lunn.ch>
+ *
+ * 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.
+ *
+ * This file contains the definitions that are specific to OpenRD
+ * ultimate variant of the Marvell Kirkwood Development Board.
+ */
+
+/dts-v1/;
+
+#include "kirkwood-openrd.dtsi"
+
+/ {
+	model = "OpenRD Ultimate";
+	compatible = "marvell,openrd-ultimate", "marvell,openrd", "marvell,kirkwood-88f6281", "marvell,kirkwood";
+
+	ocp@f1000000 {
+		i2c@11000 {
+			status = "okay";
+			clock-frequency = <400000>;
+
+			cs42l51: cs42l51@4a {
+				compatible = "cirrus,cs42l51";
+				reg = <0x4a>;
+			};
+		};
+	};
+};
+
+&mdio {
+	status = "okay";
+
+	ethphy0: ethernet-phy@0 {
+		reg = <0>;
+	};
+	ethphy1: ethernet-phy@1 {
+		reg = <1>;
+	};
+};
+
+&eth0 {
+	status = "okay";
+	ethernet0-port@0 {
+		phy-handle = <&ethphy0>;
+	};
+};
+
+&eth1 {
+	status = "okay";
+	ethernet1-port@0 {
+		phy-handle = <&ethphy1>;
+	};
+};
diff --git a/arch/arm/boot/dts/kirkwood-openrd.dtsi b/arch/arm/boot/dts/kirkwood-openrd.dtsi
new file mode 100644
index 00000000000..d3330dadf7e
--- /dev/null
+++ b/arch/arm/boot/dts/kirkwood-openrd.dtsi
@@ -0,0 +1,90 @@
+/*
+ * Marvell OpenRD (Base|Client|Ultimate) Board Description
+ *
+ * Andrew Lunn <andrew@lunn.ch>
+ *
+ * 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.
+ *
+ * This file contains the definitions that are common between the three
+ * variants of the Marvell Kirkwood Development Board.
+ */
+
+#include "kirkwood.dtsi"
+#include "kirkwood-6281.dtsi"
+
+/ {
+	memory {
+		device_type = "memory";
+		reg = <0x00000000 0x20000000>;
+	};
+
+	chosen {
+		bootargs = "console=ttyS0,115200n8";
+		stdout-path = &uart0;
+	};
+
+	mbus {
+		pcie-controller {
+			status = "okay";
+
+			pcie@1,0 {
+				status = "okay";
+			};
+		};
+	};
+
+	ocp@f1000000 {
+		pinctrl: pin-controller@10000 {
+			pinctrl-0 = <&pmx_select28 &pmx_sdio_cd &pmx_select34>;
+			pinctrl-names = "default";
+
+			pmx_select28: pmx-select-uart-sd {
+				marvell,pins = "mpp28";
+				marvell,function = "gpio";
+			};
+			pmx_sdio_cd: pmx-sdio-cd {
+				marvell,pins = "mpp29";
+				marvell,function = "gpio";
+			};
+			pmx_select34: pmx-select-rs232-rs484 {
+				marvell,pins = "mpp34";
+				marvell,function = "gpio";
+			};
+		};
+		serial@12000 {
+			status = "okay";
+
+		};
+		sata@80000 {
+			status = "okay";
+			nr-ports = <2>;
+		};
+		mvsdio@90000 {
+			status = "okay";
+			cd-gpios = <&gpio0 29 9>;
+		};
+	};
+};
+
+&nand {
+	status = "okay";
+	pinctrl-0 = <&pmx_nand>;
+	pinctrl-names = "default";
+
+	partition@0 {
+		label = "u-boot";
+		reg = <0x0000000 0x100000>;
+	};
+
+	partition@100000 {
+		label = "uImage";
+		reg = <0x0100000 0x400000>;
+	};
+
+	partition@600000 {
+		label = "root";
+		reg = <0x0600000 0x1FA00000>;
+	};
+};
diff --git a/arch/arm/boot/dts/kirkwood-rd88f6192.dts b/arch/arm/boot/dts/kirkwood-rd88f6192.dts
index e9dd8504929..35a29dee8dd 100644
--- a/arch/arm/boot/dts/kirkwood-rd88f6192.dts
+++ b/arch/arm/boot/dts/kirkwood-rd88f6192.dts
@@ -26,6 +26,7 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8";
+		stdout-path = &uart0;
 	};
 
 	mbus {
@@ -39,7 +40,7 @@
 	};
 
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 			pinctrl-0 = <&pmx_usb_power>;
 			pinctrl-names = "default";
 
@@ -56,8 +57,6 @@
 
 		spi@10600 {
 			status = "okay";
-			pinctrl-0 = <&pmx_spi>;
-			pinctrl-names = "default";
 
 			m25p128@0 {
 				#address-cells = <1>;
diff --git a/arch/arm/boot/dts/kirkwood-rd88f6281.dtsi b/arch/arm/boot/dts/kirkwood-rd88f6281.dtsi
index d6368c39102..26cf0e0ccef 100644
--- a/arch/arm/boot/dts/kirkwood-rd88f6281.dtsi
+++ b/arch/arm/boot/dts/kirkwood-rd88f6281.dtsi
@@ -22,6 +22,7 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8";
+		stdout-path = &uart0;
 	};
 
 	mbus {
@@ -35,7 +36,7 @@
 	};
 
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 			pinctrl-0 = <&pmx_sdio_cd>;
 			pinctrl-names = "default";
 
diff --git a/arch/arm/boot/dts/kirkwood-rs212.dts b/arch/arm/boot/dts/kirkwood-rs212.dts
index 93ec3d00c6a..3b19f1fd4ca 100644
--- a/arch/arm/boot/dts/kirkwood-rs212.dts
+++ b/arch/arm/boot/dts/kirkwood-rs212.dts
@@ -24,6 +24,7 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8";
+		stdout-path = &uart0;
 	};
 
 	gpio-fan-100-15-35-3 {
diff --git a/arch/arm/boot/dts/kirkwood-rs409.dts b/arch/arm/boot/dts/kirkwood-rs409.dts
index 311df4e5aa2..921ca49e85a 100644
--- a/arch/arm/boot/dts/kirkwood-rs409.dts
+++ b/arch/arm/boot/dts/kirkwood-rs409.dts
@@ -24,6 +24,7 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8";
+		stdout-path = &uart0;
 	};
 
 	gpio-fan-150-15-18 {
diff --git a/arch/arm/boot/dts/kirkwood-rs411.dts b/arch/arm/boot/dts/kirkwood-rs411.dts
index f90da850bb3..02852b0c809 100644
--- a/arch/arm/boot/dts/kirkwood-rs411.dts
+++ b/arch/arm/boot/dts/kirkwood-rs411.dts
@@ -24,6 +24,7 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8";
+		stdout-path = &uart0;
 	};
 
 	gpio-fan-100-15-35-3 {
diff --git a/arch/arm/boot/dts/kirkwood-sheevaplug-common.dtsi b/arch/arm/boot/dts/kirkwood-sheevaplug-common.dtsi
index 1ff848d570a..7196c7f3e10 100644
--- a/arch/arm/boot/dts/kirkwood-sheevaplug-common.dtsi
+++ b/arch/arm/boot/dts/kirkwood-sheevaplug-common.dtsi
@@ -17,10 +17,11 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8 earlyprintk";
+		stdout-path = &uart0;
 	};
 
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 
 			pmx_usb_power_enable: pmx-usb-power-enable {
 				marvell,pins = "mpp29";
@@ -44,8 +45,6 @@
 			};
 		};
 		serial@12000 {
-			pinctrl-0 = <&pmx_uart0>;
-			pinctrl-names = "default";
 			status = "okay";
 		};
 	};
@@ -72,8 +71,6 @@
 };
 
 &nand {
-	pinctrl-0 = <&pmx_nand>;
-	pinctrl-names = "default";
 	status = "okay";
 
 	partition@0 {
diff --git a/arch/arm/boot/dts/kirkwood-synology.dtsi b/arch/arm/boot/dts/kirkwood-synology.dtsi
index 4227c974729..811e0971fc5 100644
--- a/arch/arm/boot/dts/kirkwood-synology.dtsi
+++ b/arch/arm/boot/dts/kirkwood-synology.dtsi
@@ -25,7 +25,7 @@
 	};
 
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 			pmx_alarmled_12: pmx-alarmled-12 {
 				marvell,pins = "mpp12";
 				marvell,function = "gpio";
@@ -213,8 +213,6 @@
 
 		spi@10600 {
 			status = "okay";
-			pinctrl-0 = <&pmx_spi>;
-			pinctrl-names = "default";
 
 			m25p80@0 {
 				#address-cells = <1>;
@@ -259,8 +257,6 @@
 		i2c@11000 {
 			status = "okay";
 			clock-frequency = <400000>;
-			pinctrl-0 = <&pmx_twsi0>;
-			pinctrl-names = "default";
 
 			rs5c372: rs5c372@32 {
 				status = "disabled";
@@ -277,14 +273,10 @@
 
 		serial@12000 {
 			status = "okay";
-			pinctrl-0 = <&pmx_uart0>;
-			pinctrl-names = "default";
 		};
 
 		serial@12100 {
 			status = "okay";
-			pinctrl-0 = <&pmx_uart1>;
-			pinctrl-names = "default";
 		};
 
 		poweroff@12100 {
diff --git a/arch/arm/boot/dts/kirkwood-t5325.dts b/arch/arm/boot/dts/kirkwood-t5325.dts
index 7d1c7677a18..610ec0f9585 100644
--- a/arch/arm/boot/dts/kirkwood-t5325.dts
+++ b/arch/arm/boot/dts/kirkwood-t5325.dts
@@ -27,6 +27,7 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8";
+		stdout-path = &uart0;
 	};
 
 	mbus {
@@ -40,7 +41,7 @@
 	};
 
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 			pinctrl-0 = <&pmx_i2s &pmx_sysrst>;
 			pinctrl-names = "default";
 
@@ -64,10 +65,6 @@
 				marvell,function = "gpio";
 			};
 
-			/*
-			 * Redefined from kirkwood-6281.dtsi, because
-			 * we don't use SPI CS on MPP0, but on MPP7.
-			 */
 			pmx_spi: pmx-spi {
 				marvell,pins = "mpp1", "mpp2", "mpp3", "mpp7";
 				marvell,function = "spi";
@@ -86,8 +83,6 @@
 		};
 
 		spi@10600 {
-			pinctrl-0 = <&pmx_spi>;
-			pinctrl-names = "default";
 			status = "okay";
 
 			flash@0 {
@@ -131,6 +126,9 @@
 			alc5621: alc5621@1a {
 				compatible = "realtek,alc5621";
 				reg = <0x1a>;
+				#sound-dai-cells = <0>;
+				add-ctrl = <0x3700>;
+				jack-det-ctrl = <0x4810>;
 			};
 		};
 
@@ -189,6 +187,31 @@
 		gpios = <&gpio1 17 GPIO_ACTIVE_HIGH>;
 	};
 
+	sound {
+		compatible = "simple-audio-card";
+		simple-audio-card,format = "i2s";
+		simple-audio-card,routing =
+			"Headphone Jack", "HPL",
+			"Headphone Jack", "HPR",
+			"Speaker", "SPKOUT",
+			"Speaker", "SPKOUTN",
+			"MIC1", "Mic Jack",
+			"MIC2", "Mic Jack";
+		simple-audio-card,widgets =
+			"Headphone", "Headphone Jack",
+			"Speaker", "Speaker",
+			"Microphone", "Mic Jack";
+
+		simple-audio-card,mclk-fs = <256>;
+
+		simple-audio-card,cpu {
+			sound-dai = <&audio>;
+		};
+
+		simple-audio-card,codec {
+			sound-dai = <&alc5621>;
+		};
+	};
 };
 
 &mdio {
diff --git a/arch/arm/boot/dts/kirkwood-topkick.dts b/arch/arm/boot/dts/kirkwood-topkick.dts
index 5fc817c2cb8..f5c8c0dd41d 100644
--- a/arch/arm/boot/dts/kirkwood-topkick.dts
+++ b/arch/arm/boot/dts/kirkwood-topkick.dts
@@ -14,10 +14,11 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8 earlyprintk";
+		stdout-path = &uart0;
 	};
 
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 			/*
 			 * Switch positions
 			 *
@@ -85,9 +86,7 @@
 		};
 
 		serial@12000 {
-			status = "ok";
-			pinctrl-0 = <&pmx_uart0>;
-			pinctrl-names = "default";
+			status = "okay";
 		};
 
 		sata@80000 {
@@ -96,9 +95,7 @@
 		};
 
 		i2c@11000 {
-			status = "ok";
-			pinctrl-0 = <&pmx_twsi0>;
-			pinctrl-names = "default";
+			status = "okay";
 		};
 
 		mvsdio@90000 {
@@ -175,8 +172,6 @@
 
 &nand {
 	status = "okay";
-	pinctrl-0 = <&pmx_nand>;
-	pinctrl-names = "default";
 
 	partition@0 {
 		label = "u-boot";
diff --git a/arch/arm/boot/dts/kirkwood-ts219-6281.dts b/arch/arm/boot/dts/kirkwood-ts219-6281.dts
index c17ae45e19b..9767d73f385 100644
--- a/arch/arm/boot/dts/kirkwood-ts219-6281.dts
+++ b/arch/arm/boot/dts/kirkwood-ts219-6281.dts
@@ -6,7 +6,7 @@
 
 / {
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 
 			pinctrl-0 = <&pmx_ram_size &pmx_board_id>;
 			pinctrl-names = "default";
diff --git a/arch/arm/boot/dts/kirkwood-ts219-6282.dts b/arch/arm/boot/dts/kirkwood-ts219-6282.dts
index 0713d072758..bfc1a32d4e4 100644
--- a/arch/arm/boot/dts/kirkwood-ts219-6282.dts
+++ b/arch/arm/boot/dts/kirkwood-ts219-6282.dts
@@ -16,7 +16,7 @@
 	};
 
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 
 			pinctrl-0 = <&pmx_ram_size &pmx_board_id>;
 			pinctrl-names = "default";
diff --git a/arch/arm/boot/dts/kirkwood-ts219.dtsi b/arch/arm/boot/dts/kirkwood-ts219.dtsi
index 911f3a8cee2..df7f1527657 100644
--- a/arch/arm/boot/dts/kirkwood-ts219.dtsi
+++ b/arch/arm/boot/dts/kirkwood-ts219.dtsi
@@ -9,6 +9,7 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8";
+		stdout-path = &uart0;
 	};
 
 	mbus {
@@ -25,8 +26,6 @@
 		i2c@11000 {
 			status = "okay";
 			clock-frequency = <400000>;
-			pinctrl-0 = <&pmx_twsi0>;
-			pinctrl-names = "default";
 
 			s35390a: s35390a@30 {
 				compatible = "s35390a";
@@ -34,16 +33,10 @@
 			};
 		};
 		serial@12000 {
-			clock-frequency = <200000000>;
 			status = "okay";
-			pinctrl-0 = <&pmx_uart0>;
-			pinctrl-names = "default";
 		};
 		serial@12100 {
-			clock-frequency = <200000000>;
 			status = "okay";
-			pinctrl-0 = <&pmx_uart1>;
-			pinctrl-names = "default";
 		};
 		poweroff@12100 {
 			compatible = "qnap,power-off";
@@ -52,8 +45,6 @@
 		};
 		spi@10600 {
 			status = "okay";
-			pinctrl-0 = <&pmx_spi>;
-			pinctrl-names = "default";
 
 			m25p128@0 {
 				#address-cells = <1>;
diff --git a/arch/arm/boot/dts/kirkwood-ts419.dtsi b/arch/arm/boot/dts/kirkwood-ts419.dtsi
index 1a9c624c7a9..30ab93bfb1e 100644
--- a/arch/arm/boot/dts/kirkwood-ts419.dtsi
+++ b/arch/arm/boot/dts/kirkwood-ts419.dtsi
@@ -14,7 +14,7 @@
 	compatible = "qnap,ts419", "marvell,kirkwood";
 
 	ocp@f1000000 {
-		pinctrl: pinctrl@10000 {
+		pinctrl: pin-controller@10000 {
 			pinctrl-names = "default";
 
 			pmx_USB_copy_button: pmx-USB-copy-button {
diff --git a/arch/arm/boot/dts/kirkwood.dtsi b/arch/arm/boot/dts/kirkwood.dtsi
index 90384587c27..afc640cd80c 100644
--- a/arch/arm/boot/dts/kirkwood.dtsi
+++ b/arch/arm/boot/dts/kirkwood.dtsi
@@ -40,7 +40,7 @@
 		pcie-mem-aperture = <0xe0000000 0x10000000>; /* 256 MiB memory space */
 		pcie-io-aperture  = <0xf2000000 0x100000>;   /*   1 MiB    I/O space */
 
-		crypto@0301 {
+		cesa: crypto@0301 {
 			compatible = "marvell,orion-crypto";
 			reg = <MBUS_ID(0xf0, 0x01) 0x30000 0x10000>,
 			      <MBUS_ID(0x03, 0x01) 0 0x800>;
@@ -61,6 +61,8 @@
 			chip-delay = <25>;
 			/* set partition map and/or chip-delay in board dts */
 			clocks = <&gate_clk 7>;
+			pinctrl-0 = <&pmx_nand>;
+			pinctrl-names = "default";
 			status = "disabled";
 		};
 	};
@@ -71,13 +73,59 @@
 		#address-cells = <1>;
 		#size-cells = <1>;
 
+		pinctrl: pin-controller@10000 {
+			/* set compatible property in SoC file */
+			reg = <0x10000 0x20>;
+
+			pmx_ge1: pmx-ge1 {
+				marvell,pins = "mpp20", "mpp21", "mpp22", "mpp23",
+					       "mpp24", "mpp25", "mpp26", "mpp27",
+					       "mpp30", "mpp31", "mpp32", "mpp33";
+				marvell,function = "ge1";
+			};
+
+			pmx_nand: pmx-nand {
+				marvell,pins = "mpp0", "mpp1", "mpp2", "mpp3",
+					       "mpp4", "mpp5", "mpp18", "mpp19";
+				marvell,function = "nand";
+			};
+
+			/*
+			 * Default SPI0 pinctrl setting with CSn on mpp0,
+			 * overwrite marvell,pins on board level if required.
+			 */
+			pmx_spi: pmx-spi {
+				marvell,pins = "mpp0", "mpp1", "mpp2", "mpp3";
+				marvell,function = "spi";
+			};
+
+			pmx_twsi0: pmx-twsi0 {
+				marvell,pins = "mpp8", "mpp9";
+				marvell,function = "twsi0";
+			};
+
+			/*
+			 * Default UART pinctrl setting without RTS/CTS,
+			 * overwrite marvell,pins on board level if required.
+			 */
+			pmx_uart0: pmx-uart0 {
+				marvell,pins = "mpp10", "mpp11";
+				marvell,function = "uart0";
+			};
+
+			pmx_uart1: pmx-uart1 {
+				marvell,pins = "mpp13", "mpp14";
+				marvell,function = "uart1";
+			};
+		};
+
 		core_clk: core-clocks@10030 {
 			compatible = "marvell,kirkwood-core-clock";
 			reg = <0x10030 0x4>;
 			#clock-cells = <1>;
 		};
 
-		spi@10600 {
+		spi0: spi@10600 {
 			compatible = "marvell,orion-spi";
 			#address-cells = <1>;
 			#size-cells = <0>;
@@ -85,6 +133,8 @@
 			interrupts = <23>;
 			reg = <0x10600 0x28>;
 			clocks = <&gate_clk 7>;
+			pinctrl-0 = <&pmx_spi>;
+			pinctrl-names = "default";
 			status = "disabled";
 		};
 
@@ -120,24 +170,30 @@
 			interrupts = <29>;
 			clock-frequency = <100000>;
 			clocks = <&gate_clk 7>;
+			pinctrl-0 = <&pmx_twsi0>;
+			pinctrl-names = "default";
 			status = "disabled";
 		};
 
-		serial@12000 {
+		uart0: serial@12000 {
 			compatible = "ns16550a";
 			reg = <0x12000 0x100>;
 			reg-shift = <2>;
 			interrupts = <33>;
 			clocks = <&gate_clk 7>;
+			pinctrl-0 = <&pmx_uart0>;
+			pinctrl-names = "default";
 			status = "disabled";
 		};
 
-		serial@12100 {
+		uart1: serial@12100 {
 			compatible = "ns16550a";
 			reg = <0x12100 0x100>;
 			reg-shift = <2>;
 			interrupts = <34>;
 			clocks = <&gate_clk 7>;
+			pinctrl-0 = <&pmx_uart1>;
+			pinctrl-names = "default";
 			status = "disabled";
 		};
 
@@ -146,7 +202,7 @@
 			reg = <0x20000 0x80>, <0x1500 0x20>;
 		};
 
-		system-controller@20000 {
+		sysc: system-controller@20000 {
 			compatible = "marvell,orion-system-controller";
 			reg = <0x20000 0x120>;
 		};
@@ -196,7 +252,7 @@
 			status = "okay";
 		};
 
-		ehci@50000 {
+		usb0: ehci@50000 {
 			compatible = "marvell,orion-ehci";
 			reg = <0x50000 0x1000>;
 			interrupts = <19>;
@@ -204,7 +260,7 @@
 			status = "okay";
 		};
 
-		xor@60800 {
+		dma0: xor@60800 {
 			compatible = "marvell,orion-xor";
 			reg = <0x60800 0x100
 			       0x60A00 0x100>;
@@ -224,7 +280,7 @@
 			};
 		};
 
-		xor@60900 {
+		dma1: xor@60900 {
 			compatible = "marvell,orion-xor";
 			reg = <0x60900 0x100
 			       0x60B00 0x100>;
@@ -282,6 +338,8 @@
 			reg = <0x76000 0x4000>;
 			clocks = <&gate_clk 19>;
 			marvell,tx-checksum-limit = <1600>;
+			pinctrl-0 = <&pmx_ge1>;
+			pinctrl-names = "default";
 			status = "disabled";
 
 			ethernet1-port@0 {
@@ -314,6 +372,7 @@
 
 		audio0: audio-controller@a0000 {
 			compatible = "marvell,kirkwood-audio";
+			#sound-dai-cells = <0>;
 			reg = <0xa0000 0x2210>;
 			interrupts = <24>;
 			clocks = <&gate_clk 9>;
diff --git a/arch/arm/boot/dts/omap-gpmc-smsc911x.dtsi b/arch/arm/boot/dts/omap-gpmc-smsc911x.dtsi
index f577b7df9a2..521c587acae 100644
--- a/arch/arm/boot/dts/omap-gpmc-smsc911x.dtsi
+++ b/arch/arm/boot/dts/omap-gpmc-smsc911x.dtsi
@@ -24,11 +24,10 @@
 		compatible = "smsc,lan9221", "smsc,lan9115";
 		bank-width = <2>;
 		gpmc,mux-add-data;
-		gpmc,cs-on-ns = <0>;
-		gpmc,cs-rd-off-ns = <186>;
-		gpmc,cs-wr-off-ns = <186>;
-		gpmc,adv-on-ns = <12>;
-		gpmc,adv-rd-off-ns = <48>;
+		gpmc,cs-on-ns = <1>;
+		gpmc,cs-rd-off-ns = <180>;
+		gpmc,cs-wr-off-ns = <180>;
+		gpmc,adv-rd-off-ns = <18>;
 		gpmc,adv-wr-off-ns = <48>;
 		gpmc,oe-on-ns = <54>;
 		gpmc,oe-off-ns = <168>;
@@ -36,12 +35,10 @@
 		gpmc,we-off-ns = <168>;
 		gpmc,rd-cycle-ns = <186>;
 		gpmc,wr-cycle-ns = <186>;
-		gpmc,access-ns = <114>;
-		gpmc,page-burst-access-ns = <6>;
-		gpmc,bus-turnaround-ns = <12>;
-		gpmc,cycle2cycle-delay-ns = <18>;
-		gpmc,wr-data-mux-bus-ns = <90>;
-		gpmc,wr-access-ns = <186>;
+		gpmc,access-ns = <144>;
+		gpmc,page-burst-access-ns = <24>;
+		gpmc,bus-turnaround-ns = <90>;
+		gpmc,cycle2cycle-delay-ns = <90>;
 		gpmc,cycle2cycle-samecsen;
 		gpmc,cycle2cycle-diffcsen;
 		vddvario-supply = <&vddvario>;
diff --git a/arch/arm/boot/dts/omap2.dtsi b/arch/arm/boot/dts/omap2.dtsi
index 22f35ea142c..8f8c07da4ac 100644
--- a/arch/arm/boot/dts/omap2.dtsi
+++ b/arch/arm/boot/dts/omap2.dtsi
@@ -71,13 +71,6 @@
 			interrupts = <58>;
 		};
 
-		mailbox: mailbox@48094000 {
-			compatible = "ti,omap2-mailbox";
-			ti,hwmods = "mailbox";
-			reg = <0x48094000 0x200>;
-			interrupts = <26>;
-		};
-
 		intc: interrupt-controller@1 {
 			compatible = "ti,omap2-intc";
 			interrupt-controller;
diff --git a/arch/arm/boot/dts/omap2420-clocks.dtsi b/arch/arm/boot/dts/omap2420-clocks.dtsi
new file mode 100644
index 00000000000..ce8c742d7e9
--- /dev/null
+++ b/arch/arm/boot/dts/omap2420-clocks.dtsi
@@ -0,0 +1,270 @@
+/*
+ * Device Tree Source for OMAP2420 clock data
+ *
+ * Copyright (C) 2014 Texas Instruments, 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.
+ */
+
+&prcm_clocks {
+	sys_clkout2_src_gate: sys_clkout2_src_gate {
+		#clock-cells = <0>;
+		compatible = "ti,composite-no-wait-gate-clock";
+		clocks = <&core_ck>;
+		ti,bit-shift = <15>;
+		reg = <0x0070>;
+	};
+
+	sys_clkout2_src_mux: sys_clkout2_src_mux {
+		#clock-cells = <0>;
+		compatible = "ti,composite-mux-clock";
+		clocks = <&core_ck>, <&sys_ck>, <&func_96m_ck>, <&func_54m_ck>;
+		ti,bit-shift = <8>;
+		reg = <0x0070>;
+	};
+
+	sys_clkout2_src: sys_clkout2_src {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&sys_clkout2_src_gate>, <&sys_clkout2_src_mux>;
+	};
+
+	sys_clkout2: sys_clkout2 {
+		#clock-cells = <0>;
+		compatible = "ti,divider-clock";
+		clocks = <&sys_clkout2_src>;
+		ti,bit-shift = <11>;
+		ti,max-div = <64>;
+		reg = <0x0070>;
+		ti,index-power-of-two;
+	};
+
+	dsp_gate_ick: dsp_gate_ick {
+		#clock-cells = <0>;
+		compatible = "ti,composite-interface-clock";
+		clocks = <&dsp_fck>;
+		ti,bit-shift = <1>;
+		reg = <0x0810>;
+	};
+
+	dsp_div_ick: dsp_div_ick {
+		#clock-cells = <0>;
+		compatible = "ti,composite-divider-clock";
+		clocks = <&dsp_fck>;
+		ti,bit-shift = <5>;
+		ti,max-div = <3>;
+		reg = <0x0840>;
+		ti,index-starts-at-one;
+	};
+
+	dsp_ick: dsp_ick {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&dsp_gate_ick>, <&dsp_div_ick>;
+	};
+
+	iva1_gate_ifck: iva1_gate_ifck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-gate-clock";
+		clocks = <&core_ck>;
+		ti,bit-shift = <10>;
+		reg = <0x0800>;
+	};
+
+	iva1_div_ifck: iva1_div_ifck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-divider-clock";
+		clocks = <&core_ck>;
+		ti,bit-shift = <8>;
+		reg = <0x0840>;
+		ti,dividers = <0>, <1>, <2>, <3>, <4>, <0>, <6>, <0>, <8>, <0>, <0>, <0>, <12>;
+	};
+
+	iva1_ifck: iva1_ifck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&iva1_gate_ifck>, <&iva1_div_ifck>;
+	};
+
+	iva1_ifck_div: iva1_ifck_div {
+		#clock-cells = <0>;
+		compatible = "fixed-factor-clock";
+		clocks = <&iva1_ifck>;
+		clock-mult = <1>;
+		clock-div = <2>;
+	};
+
+	iva1_mpu_int_ifck: iva1_mpu_int_ifck {
+		#clock-cells = <0>;
+		compatible = "ti,wait-gate-clock";
+		clocks = <&iva1_ifck_div>;
+		ti,bit-shift = <8>;
+		reg = <0x0800>;
+	};
+
+	wdt3_ick: wdt3_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <28>;
+		reg = <0x0210>;
+	};
+
+	wdt3_fck: wdt3_fck {
+		#clock-cells = <0>;
+		compatible = "ti,wait-gate-clock";
+		clocks = <&func_32k_ck>;
+		ti,bit-shift = <28>;
+		reg = <0x0200>;
+	};
+
+	mmc_ick: mmc_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <26>;
+		reg = <0x0210>;
+	};
+
+	mmc_fck: mmc_fck {
+		#clock-cells = <0>;
+		compatible = "ti,wait-gate-clock";
+		clocks = <&func_96m_ck>;
+		ti,bit-shift = <26>;
+		reg = <0x0200>;
+	};
+
+	eac_ick: eac_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <24>;
+		reg = <0x0210>;
+	};
+
+	eac_fck: eac_fck {
+		#clock-cells = <0>;
+		compatible = "ti,wait-gate-clock";
+		clocks = <&func_96m_ck>;
+		ti,bit-shift = <24>;
+		reg = <0x0200>;
+	};
+
+	i2c1_fck: i2c1_fck {
+		#clock-cells = <0>;
+		compatible = "ti,wait-gate-clock";
+		clocks = <&func_12m_ck>;
+		ti,bit-shift = <19>;
+		reg = <0x0200>;
+	};
+
+	i2c2_fck: i2c2_fck {
+		#clock-cells = <0>;
+		compatible = "ti,wait-gate-clock";
+		clocks = <&func_12m_ck>;
+		ti,bit-shift = <20>;
+		reg = <0x0200>;
+	};
+
+	vlynq_ick: vlynq_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&core_l3_ck>;
+		ti,bit-shift = <3>;
+		reg = <0x0210>;
+	};
+
+	vlynq_gate_fck: vlynq_gate_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-gate-clock";
+		clocks = <&core_ck>;
+		ti,bit-shift = <3>;
+		reg = <0x0200>;
+	};
+
+	core_d18_ck: core_d18_ck {
+		#clock-cells = <0>;
+		compatible = "fixed-factor-clock";
+		clocks = <&core_ck>;
+		clock-mult = <1>;
+		clock-div = <18>;
+	};
+
+	vlynq_mux_fck: vlynq_mux_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-mux-clock";
+		clocks = <&func_96m_ck>, <&core_ck>, <&core_d2_ck>, <&core_d3_ck>, <&core_d4_ck>, <&dummy_ck>, <&core_d6_ck>, <&dummy_ck>, <&core_d8_ck>, <&core_d9_ck>, <&dummy_ck>, <&dummy_ck>, <&core_d12_ck>, <&dummy_ck>, <&dummy_ck>, <&dummy_ck>, <&core_d16_ck>, <&dummy_ck>, <&core_d18_ck>;
+		ti,bit-shift = <15>;
+		reg = <0x0240>;
+	};
+
+	vlynq_fck: vlynq_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&vlynq_gate_fck>, <&vlynq_mux_fck>;
+	};
+};
+
+&prcm_clockdomains {
+	gfx_clkdm: gfx_clkdm {
+		compatible = "ti,clockdomain";
+		clocks = <&gfx_ick>;
+	};
+
+	core_l3_clkdm: core_l3_clkdm {
+		compatible = "ti,clockdomain";
+		clocks = <&cam_fck>, <&vlynq_ick>, <&usb_fck>;
+	};
+
+	wkup_clkdm: wkup_clkdm {
+		compatible = "ti,clockdomain";
+		clocks = <&dpll_ck>, <&emul_ck>, <&gpt1_ick>, <&gpios_ick>,
+			 <&gpios_fck>, <&mpu_wdt_ick>, <&mpu_wdt_fck>,
+			 <&sync_32k_ick>, <&wdt1_ick>, <&omapctrl_ick>;
+	};
+
+	iva1_clkdm: iva1_clkdm {
+		compatible = "ti,clockdomain";
+		clocks = <&iva1_mpu_int_ifck>;
+	};
+
+	dss_clkdm: dss_clkdm {
+		compatible = "ti,clockdomain";
+		clocks = <&dss_ick>, <&dss_54m_fck>;
+	};
+
+	core_l4_clkdm: core_l4_clkdm {
+		compatible = "ti,clockdomain";
+		clocks = <&ssi_l4_ick>, <&gpt2_ick>, <&gpt3_ick>, <&gpt4_ick>,
+			 <&gpt5_ick>, <&gpt6_ick>, <&gpt7_ick>, <&gpt8_ick>,
+			 <&gpt9_ick>, <&gpt10_ick>, <&gpt11_ick>, <&gpt12_ick>,
+			 <&mcbsp1_ick>, <&mcbsp2_ick>, <&mcspi1_ick>,
+			 <&mcspi1_fck>, <&mcspi2_ick>, <&mcspi2_fck>,
+			 <&uart1_ick>, <&uart1_fck>, <&uart2_ick>, <&uart2_fck>,
+			 <&uart3_ick>, <&uart3_fck>, <&cam_ick>,
+			 <&mailboxes_ick>, <&wdt4_ick>, <&wdt4_fck>,
+			 <&wdt3_ick>, <&wdt3_fck>, <&mspro_ick>, <&mspro_fck>,
+			 <&mmc_ick>, <&mmc_fck>, <&fac_ick>, <&fac_fck>,
+			 <&eac_ick>, <&eac_fck>, <&hdq_ick>, <&hdq_fck>,
+			 <&i2c1_ick>, <&i2c1_fck>, <&i2c2_ick>, <&i2c2_fck>,
+			 <&des_ick>, <&sha_ick>, <&rng_ick>, <&aes_ick>,
+			 <&pka_ick>;
+	};
+};
+
+&func_96m_ck {
+	compatible = "fixed-factor-clock";
+	clocks = <&apll96_ck>;
+	clock-mult = <1>;
+	clock-div = <1>;
+};
+
+&dsp_div_fck {
+	ti,dividers = <0>, <1>, <2>, <3>, <4>, <0>, <6>, <0>, <8>, <0>, <0>, <0>, <12>;
+};
+
+&ssi_ssr_sst_div_fck {
+	ti,dividers = <0>, <1>, <2>, <3>, <4>, <0>, <6>, <0>, <8>;
+};
diff --git a/arch/arm/boot/dts/omap2420.dtsi b/arch/arm/boot/dts/omap2420.dtsi
index 85b1fb014c4..e83b0468080 100644
--- a/arch/arm/boot/dts/omap2420.dtsi
+++ b/arch/arm/boot/dts/omap2420.dtsi
@@ -14,6 +14,32 @@
 	compatible = "ti,omap2420", "ti,omap2";
 
 	ocp {
+		prcm: prcm@48008000 {
+			compatible = "ti,omap2-prcm";
+			reg = <0x48008000 0x1000>;
+
+			prcm_clocks: clocks {
+				#address-cells = <1>;
+				#size-cells = <0>;
+			};
+
+			prcm_clockdomains: clockdomains {
+			};
+		};
+
+		scrm: scrm@48000000 {
+			compatible = "ti,omap2-scrm";
+			reg = <0x48000000 0x1000>;
+
+			scrm_clocks: clocks {
+				#address-cells = <1>;
+				#size-cells = <0>;
+			};
+
+			scrm_clockdomains: clockdomains {
+			};
+		};
+
 		counter32k: counter@48004000 {
 			compatible = "ti,omap-counter32k";
 			reg = <0x48004000 0x20>;
@@ -125,6 +151,14 @@
 			dma-names = "tx", "rx";
 		};
 
+		mailbox: mailbox@48094000 {
+			compatible = "ti,omap2-mailbox";
+			reg = <0x48094000 0x200>;
+			interrupts = <26>, <34>;
+			interrupt-names = "dsp", "iva";
+			ti,hwmods = "mailbox";
+		};
+
 		timer1: timer@48028000 {
 			compatible = "ti,omap2420-timer";
 			reg = <0x48028000 0x400>;
diff --git a/arch/arm/boot/dts/omap2430-clocks.dtsi b/arch/arm/boot/dts/omap2430-clocks.dtsi
new file mode 100644
index 00000000000..805f75df1cf
--- /dev/null
+++ b/arch/arm/boot/dts/omap2430-clocks.dtsi
@@ -0,0 +1,344 @@
+/*
+ * Device Tree Source for OMAP2430 clock data
+ *
+ * Copyright (C) 2014 Texas Instruments, 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.
+ */
+
+&scrm_clocks {
+	mcbsp3_mux_fck: mcbsp3_mux_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-mux-clock";
+		clocks = <&func_96m_ck>, <&mcbsp_clks>;
+		reg = <0x02e8>;
+	};
+
+	mcbsp3_fck: mcbsp3_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&mcbsp3_gate_fck>, <&mcbsp3_mux_fck>;
+	};
+
+	mcbsp4_mux_fck: mcbsp4_mux_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-mux-clock";
+		clocks = <&func_96m_ck>, <&mcbsp_clks>;
+		ti,bit-shift = <2>;
+		reg = <0x02e8>;
+	};
+
+	mcbsp4_fck: mcbsp4_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&mcbsp4_gate_fck>, <&mcbsp4_mux_fck>;
+	};
+
+	mcbsp5_mux_fck: mcbsp5_mux_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-mux-clock";
+		clocks = <&func_96m_ck>, <&mcbsp_clks>;
+		ti,bit-shift = <4>;
+		reg = <0x02e8>;
+	};
+
+	mcbsp5_fck: mcbsp5_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&mcbsp5_gate_fck>, <&mcbsp5_mux_fck>;
+	};
+};
+
+&prcm_clocks {
+	iva2_1_gate_ick: iva2_1_gate_ick {
+		#clock-cells = <0>;
+		compatible = "ti,composite-gate-clock";
+		clocks = <&dsp_fck>;
+		ti,bit-shift = <0>;
+		reg = <0x0800>;
+	};
+
+	iva2_1_div_ick: iva2_1_div_ick {
+		#clock-cells = <0>;
+		compatible = "ti,composite-divider-clock";
+		clocks = <&dsp_fck>;
+		ti,bit-shift = <5>;
+		ti,max-div = <3>;
+		reg = <0x0840>;
+		ti,index-starts-at-one;
+	};
+
+	iva2_1_ick: iva2_1_ick {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&iva2_1_gate_ick>, <&iva2_1_div_ick>;
+	};
+
+	mdm_gate_ick: mdm_gate_ick {
+		#clock-cells = <0>;
+		compatible = "ti,composite-interface-clock";
+		clocks = <&core_ck>;
+		ti,bit-shift = <0>;
+		reg = <0x0c10>;
+	};
+
+	mdm_div_ick: mdm_div_ick {
+		#clock-cells = <0>;
+		compatible = "ti,composite-divider-clock";
+		clocks = <&core_ck>;
+		reg = <0x0c40>;
+		ti,dividers = <0>, <1>, <0>, <0>, <4>, <0>, <6>, <0>, <0>, <9>;
+	};
+
+	mdm_ick: mdm_ick {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&mdm_gate_ick>, <&mdm_div_ick>;
+	};
+
+	mdm_osc_ck: mdm_osc_ck {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&osc_ck>;
+		ti,bit-shift = <1>;
+		reg = <0x0c00>;
+	};
+
+	mcbsp3_ick: mcbsp3_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <3>;
+		reg = <0x0214>;
+	};
+
+	mcbsp3_gate_fck: mcbsp3_gate_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-gate-clock";
+		clocks = <&mcbsp_clks>;
+		ti,bit-shift = <3>;
+		reg = <0x0204>;
+	};
+
+	mcbsp4_ick: mcbsp4_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <4>;
+		reg = <0x0214>;
+	};
+
+	mcbsp4_gate_fck: mcbsp4_gate_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-gate-clock";
+		clocks = <&mcbsp_clks>;
+		ti,bit-shift = <4>;
+		reg = <0x0204>;
+	};
+
+	mcbsp5_ick: mcbsp5_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <5>;
+		reg = <0x0214>;
+	};
+
+	mcbsp5_gate_fck: mcbsp5_gate_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-gate-clock";
+		clocks = <&mcbsp_clks>;
+		ti,bit-shift = <5>;
+		reg = <0x0204>;
+	};
+
+	mcspi3_ick: mcspi3_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <9>;
+		reg = <0x0214>;
+	};
+
+	mcspi3_fck: mcspi3_fck {
+		#clock-cells = <0>;
+		compatible = "ti,wait-gate-clock";
+		clocks = <&func_48m_ck>;
+		ti,bit-shift = <9>;
+		reg = <0x0204>;
+	};
+
+	icr_ick: icr_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&sys_ck>;
+		ti,bit-shift = <6>;
+		reg = <0x0410>;
+	};
+
+	i2chs1_fck: i2chs1_fck {
+		#clock-cells = <0>;
+		compatible = "ti,omap2430-interface-clock";
+		clocks = <&func_96m_ck>;
+		ti,bit-shift = <19>;
+		reg = <0x0204>;
+	};
+
+	i2chs2_fck: i2chs2_fck {
+		#clock-cells = <0>;
+		compatible = "ti,omap2430-interface-clock";
+		clocks = <&func_96m_ck>;
+		ti,bit-shift = <20>;
+		reg = <0x0204>;
+	};
+
+	usbhs_ick: usbhs_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&core_l3_ck>;
+		ti,bit-shift = <6>;
+		reg = <0x0214>;
+	};
+
+	mmchs1_ick: mmchs1_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <7>;
+		reg = <0x0214>;
+	};
+
+	mmchs1_fck: mmchs1_fck {
+		#clock-cells = <0>;
+		compatible = "ti,wait-gate-clock";
+		clocks = <&func_96m_ck>;
+		ti,bit-shift = <7>;
+		reg = <0x0204>;
+	};
+
+	mmchs2_ick: mmchs2_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <8>;
+		reg = <0x0214>;
+	};
+
+	mmchs2_fck: mmchs2_fck {
+		#clock-cells = <0>;
+		compatible = "ti,wait-gate-clock";
+		clocks = <&func_96m_ck>;
+		ti,bit-shift = <8>;
+		reg = <0x0204>;
+	};
+
+	gpio5_ick: gpio5_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <10>;
+		reg = <0x0214>;
+	};
+
+	gpio5_fck: gpio5_fck {
+		#clock-cells = <0>;
+		compatible = "ti,wait-gate-clock";
+		clocks = <&func_32k_ck>;
+		ti,bit-shift = <10>;
+		reg = <0x0204>;
+	};
+
+	mdm_intc_ick: mdm_intc_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <11>;
+		reg = <0x0214>;
+	};
+
+	mmchsdb1_fck: mmchsdb1_fck {
+		#clock-cells = <0>;
+		compatible = "ti,wait-gate-clock";
+		clocks = <&func_32k_ck>;
+		ti,bit-shift = <16>;
+		reg = <0x0204>;
+	};
+
+	mmchsdb2_fck: mmchsdb2_fck {
+		#clock-cells = <0>;
+		compatible = "ti,wait-gate-clock";
+		clocks = <&func_32k_ck>;
+		ti,bit-shift = <17>;
+		reg = <0x0204>;
+	};
+};
+
+&prcm_clockdomains {
+	gfx_clkdm: gfx_clkdm {
+		compatible = "ti,clockdomain";
+		clocks = <&gfx_ick>;
+	};
+
+	core_l3_clkdm: core_l3_clkdm {
+		compatible = "ti,clockdomain";
+		clocks = <&cam_fck>, <&usb_fck>, <&usbhs_ick>;
+	};
+
+	wkup_clkdm: wkup_clkdm {
+		compatible = "ti,clockdomain";
+		clocks = <&dpll_ck>, <&emul_ck>, <&gpt1_ick>, <&gpios_ick>,
+			 <&gpios_fck>, <&mpu_wdt_ick>, <&mpu_wdt_fck>,
+			 <&sync_32k_ick>, <&wdt1_ick>, <&omapctrl_ick>,
+			 <&icr_ick>;
+	};
+
+	dss_clkdm: dss_clkdm {
+		compatible = "ti,clockdomain";
+		clocks = <&dss_ick>, <&dss_54m_fck>;
+	};
+
+	core_l4_clkdm: core_l4_clkdm {
+		compatible = "ti,clockdomain";
+		clocks = <&ssi_l4_ick>, <&gpt2_ick>, <&gpt3_ick>, <&gpt4_ick>,
+			 <&gpt5_ick>, <&gpt6_ick>, <&gpt7_ick>, <&gpt8_ick>,
+			 <&gpt9_ick>, <&gpt10_ick>, <&gpt11_ick>, <&gpt12_ick>,
+			 <&mcbsp1_ick>, <&mcbsp2_ick>, <&mcbsp3_ick>,
+			 <&mcbsp4_ick>, <&mcbsp5_ick>, <&mcspi1_ick>,
+			 <&mcspi1_fck>, <&mcspi2_ick>, <&mcspi2_fck>,
+			 <&mcspi3_ick>, <&mcspi3_fck>, <&uart1_ick>,
+			 <&uart1_fck>, <&uart2_ick>, <&uart2_fck>, <&uart3_ick>,
+			 <&uart3_fck>, <&cam_ick>, <&mailboxes_ick>,
+			 <&wdt4_ick>, <&wdt4_fck>, <&mspro_ick>, <&mspro_fck>,
+			 <&fac_ick>, <&fac_fck>, <&hdq_ick>, <&hdq_fck>,
+			 <&i2c1_ick>, <&i2chs1_fck>, <&i2c2_ick>, <&i2chs2_fck>,
+			 <&des_ick>, <&sha_ick>, <&rng_ick>, <&aes_ick>,
+			 <&pka_ick>, <&mmchs1_ick>, <&mmchs1_fck>,
+			 <&mmchs2_ick>, <&mmchs2_fck>, <&gpio5_ick>,
+			 <&gpio5_fck>, <&mdm_intc_ick>, <&mmchsdb1_fck>,
+			 <&mmchsdb2_fck>;
+	};
+
+	mdm_clkdm: mdm_clkdm {
+		compatible = "ti,clockdomain";
+		clocks = <&mdm_osc_ck>;
+	};
+};
+
+&func_96m_ck {
+	compatible = "ti,mux-clock";
+	clocks = <&apll96_ck>, <&alt_ck>;
+	ti,bit-shift = <4>;
+	reg = <0x0540>;
+};
+
+&dsp_div_fck {
+	ti,max-div = <4>;
+	ti,index-starts-at-one;
+};
+
+&ssi_ssr_sst_div_fck {
+	ti,max-div = <5>;
+	ti,index-starts-at-one;
+};
diff --git a/arch/arm/boot/dts/omap2430.dtsi b/arch/arm/boot/dts/omap2430.dtsi
index d09697dab55..c4e8013801e 100644
--- a/arch/arm/boot/dts/omap2430.dtsi
+++ b/arch/arm/boot/dts/omap2430.dtsi
@@ -14,6 +14,32 @@
 	compatible = "ti,omap2430", "ti,omap2";
 
 	ocp {
+		prcm: prcm@49006000 {
+			compatible = "ti,omap2-prcm";
+			reg = <0x49006000 0x1000>;
+
+			prcm_clocks: clocks {
+				#address-cells = <1>;
+				#size-cells = <0>;
+			};
+
+			prcm_clockdomains: clockdomains {
+			};
+		};
+
+		scrm: scrm@49002000 {
+			compatible = "ti,omap2-scrm";
+			reg = <0x49002000 0x1000>;
+
+			scrm_clocks: clocks {
+				#address-cells = <1>;
+				#size-cells = <0>;
+			};
+
+			scrm_clockdomains: clockdomains {
+			};
+		};
+
 		counter32k: counter@49020000 {
 			compatible = "ti,omap-counter32k";
 			reg = <0x49020000 0x20>;
@@ -216,6 +242,13 @@
 			dma-names = "tx", "rx";
 		};
 
+		mailbox: mailbox@48094000 {
+			compatible = "ti,omap2-mailbox";
+			reg = <0x48094000 0x200>;
+			interrupts = <26>;
+			ti,hwmods = "mailbox";
+		};
+
 		timer1: timer@49018000 {
 			compatible = "ti,omap2420-timer";
 			reg = <0x49018000 0x400>;
diff --git a/arch/arm/boot/dts/omap24xx-clocks.dtsi b/arch/arm/boot/dts/omap24xx-clocks.dtsi
new file mode 100644
index 00000000000..a1365ca926e
--- /dev/null
+++ b/arch/arm/boot/dts/omap24xx-clocks.dtsi
@@ -0,0 +1,1244 @@
+/*
+ * Device Tree Source for OMAP24xx clock data
+ *
+ * Copyright (C) 2014 Texas Instruments, 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.
+ */
+&scrm_clocks {
+	mcbsp1_mux_fck: mcbsp1_mux_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-mux-clock";
+		clocks = <&func_96m_ck>, <&mcbsp_clks>;
+		ti,bit-shift = <2>;
+		reg = <0x0274>;
+	};
+
+	mcbsp1_fck: mcbsp1_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&mcbsp1_gate_fck>, <&mcbsp1_mux_fck>;
+	};
+
+	mcbsp2_mux_fck: mcbsp2_mux_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-mux-clock";
+		clocks = <&func_96m_ck>, <&mcbsp_clks>;
+		ti,bit-shift = <6>;
+		reg = <0x0274>;
+	};
+
+	mcbsp2_fck: mcbsp2_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&mcbsp2_gate_fck>, <&mcbsp2_mux_fck>;
+	};
+};
+
+&prcm_clocks {
+	func_32k_ck: func_32k_ck {
+		#clock-cells = <0>;
+		compatible = "fixed-clock";
+		clock-frequency = <32768>;
+	};
+
+	secure_32k_ck: secure_32k_ck {
+		#clock-cells = <0>;
+		compatible = "fixed-clock";
+		clock-frequency = <32768>;
+	};
+
+	virt_12m_ck: virt_12m_ck {
+		#clock-cells = <0>;
+		compatible = "fixed-clock";
+		clock-frequency = <12000000>;
+	};
+
+	virt_13m_ck: virt_13m_ck {
+		#clock-cells = <0>;
+		compatible = "fixed-clock";
+		clock-frequency = <13000000>;
+	};
+
+	virt_19200000_ck: virt_19200000_ck {
+		#clock-cells = <0>;
+		compatible = "fixed-clock";
+		clock-frequency = <19200000>;
+	};
+
+	virt_26m_ck: virt_26m_ck {
+		#clock-cells = <0>;
+		compatible = "fixed-clock";
+		clock-frequency = <26000000>;
+	};
+
+	aplls_clkin_ck: aplls_clkin_ck {
+		#clock-cells = <0>;
+		compatible = "ti,mux-clock";
+		clocks = <&virt_19200000_ck>, <&virt_26m_ck>, <&virt_13m_ck>, <&virt_12m_ck>;
+		ti,bit-shift = <23>;
+		reg = <0x0540>;
+	};
+
+	aplls_clkin_x2_ck: aplls_clkin_x2_ck {
+		#clock-cells = <0>;
+		compatible = "fixed-factor-clock";
+		clocks = <&aplls_clkin_ck>;
+		clock-mult = <2>;
+		clock-div = <1>;
+	};
+
+	osc_ck: osc_ck {
+		#clock-cells = <0>;
+		compatible = "ti,mux-clock";
+		clocks = <&aplls_clkin_ck>, <&aplls_clkin_x2_ck>;
+		ti,bit-shift = <6>;
+		reg = <0x0060>;
+		ti,index-starts-at-one;
+	};
+
+	sys_ck: sys_ck {
+		#clock-cells = <0>;
+		compatible = "ti,divider-clock";
+		clocks = <&osc_ck>;
+		ti,bit-shift = <6>;
+		ti,max-div = <3>;
+		reg = <0x0060>;
+		ti,index-starts-at-one;
+	};
+
+	alt_ck: alt_ck {
+		#clock-cells = <0>;
+		compatible = "fixed-clock";
+		clock-frequency = <54000000>;
+	};
+
+	mcbsp_clks: mcbsp_clks {
+		#clock-cells = <0>;
+		compatible = "fixed-clock";
+		clock-frequency = <0x0>;
+	};
+
+	dpll_ck: dpll_ck {
+		#clock-cells = <0>;
+		compatible = "ti,omap2-dpll-core-clock";
+		clocks = <&sys_ck>, <&sys_ck>;
+		reg = <0x0500>, <0x0540>;
+	};
+
+	apll96_ck: apll96_ck {
+		#clock-cells = <0>;
+		compatible = "ti,omap2-apll-clock";
+		clocks = <&sys_ck>;
+		ti,bit-shift = <2>;
+		ti,idlest-shift = <8>;
+		ti,clock-frequency = <96000000>;
+		reg = <0x0500>, <0x0530>, <0x0520>;
+	};
+
+	apll54_ck: apll54_ck {
+		#clock-cells = <0>;
+		compatible = "ti,omap2-apll-clock";
+		clocks = <&sys_ck>;
+		ti,bit-shift = <6>;
+		ti,idlest-shift = <9>;
+		ti,clock-frequency = <54000000>;
+		reg = <0x0500>, <0x0530>, <0x0520>;
+	};
+
+	func_54m_ck: func_54m_ck {
+		#clock-cells = <0>;
+		compatible = "ti,mux-clock";
+		clocks = <&apll54_ck>, <&alt_ck>;
+		ti,bit-shift = <5>;
+		reg = <0x0540>;
+	};
+
+	core_ck: core_ck {
+		#clock-cells = <0>;
+		compatible = "fixed-factor-clock";
+		clocks = <&dpll_ck>;
+		clock-mult = <1>;
+		clock-div = <1>;
+	};
+
+	func_96m_ck: func_96m_ck {
+		#clock-cells = <0>;
+	};
+
+	apll96_d2_ck: apll96_d2_ck {
+		#clock-cells = <0>;
+		compatible = "fixed-factor-clock";
+		clocks = <&apll96_ck>;
+		clock-mult = <1>;
+		clock-div = <2>;
+	};
+
+	func_48m_ck: func_48m_ck {
+		#clock-cells = <0>;
+		compatible = "ti,mux-clock";
+		clocks = <&apll96_d2_ck>, <&alt_ck>;
+		ti,bit-shift = <3>;
+		reg = <0x0540>;
+	};
+
+	func_12m_ck: func_12m_ck {
+		#clock-cells = <0>;
+		compatible = "fixed-factor-clock";
+		clocks = <&func_48m_ck>;
+		clock-mult = <1>;
+		clock-div = <4>;
+	};
+
+	sys_clkout_src_gate: sys_clkout_src_gate {
+		#clock-cells = <0>;
+		compatible = "ti,composite-no-wait-gate-clock";
+		clocks = <&core_ck>;
+		ti,bit-shift = <7>;
+		reg = <0x0070>;
+	};
+
+	sys_clkout_src_mux: sys_clkout_src_mux {
+		#clock-cells = <0>;
+		compatible = "ti,composite-mux-clock";
+		clocks = <&core_ck>, <&sys_ck>, <&func_96m_ck>, <&func_54m_ck>;
+		reg = <0x0070>;
+	};
+
+	sys_clkout_src: sys_clkout_src {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&sys_clkout_src_gate>, <&sys_clkout_src_mux>;
+	};
+
+	sys_clkout: sys_clkout {
+		#clock-cells = <0>;
+		compatible = "ti,divider-clock";
+		clocks = <&sys_clkout_src>;
+		ti,bit-shift = <3>;
+		ti,max-div = <64>;
+		reg = <0x0070>;
+		ti,index-power-of-two;
+	};
+
+	emul_ck: emul_ck {
+		#clock-cells = <0>;
+		compatible = "ti,gate-clock";
+		clocks = <&func_54m_ck>;
+		ti,bit-shift = <0>;
+		reg = <0x0078>;
+	};
+
+	mpu_ck: mpu_ck {
+		#clock-cells = <0>;
+		compatible = "ti,divider-clock";
+		clocks = <&core_ck>;
+		ti,max-div = <31>;
+		reg = <0x0140>;
+		ti,index-starts-at-one;
+	};
+
+	dsp_gate_fck: dsp_gate_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-gate-clock";
+		clocks = <&core_ck>;
+		ti,bit-shift = <0>;
+		reg = <0x0800>;
+	};
+
+	dsp_div_fck: dsp_div_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-divider-clock";
+		clocks = <&core_ck>;
+		reg = <0x0840>;
+	};
+
+	dsp_fck: dsp_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&dsp_gate_fck>, <&dsp_div_fck>;
+	};
+
+	core_l3_ck: core_l3_ck {
+		#clock-cells = <0>;
+		compatible = "ti,divider-clock";
+		clocks = <&core_ck>;
+		ti,max-div = <31>;
+		reg = <0x0240>;
+		ti,index-starts-at-one;
+	};
+
+	gfx_3d_gate_fck: gfx_3d_gate_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-gate-clock";
+		clocks = <&core_l3_ck>;
+		ti,bit-shift = <2>;
+		reg = <0x0300>;
+	};
+
+	gfx_3d_div_fck: gfx_3d_div_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-divider-clock";
+		clocks = <&core_l3_ck>;
+		ti,max-div = <4>;
+		reg = <0x0340>;
+		ti,index-starts-at-one;
+	};
+
+	gfx_3d_fck: gfx_3d_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&gfx_3d_gate_fck>, <&gfx_3d_div_fck>;
+	};
+
+	gfx_2d_gate_fck: gfx_2d_gate_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-gate-clock";
+		clocks = <&core_l3_ck>;
+		ti,bit-shift = <1>;
+		reg = <0x0300>;
+	};
+
+	gfx_2d_div_fck: gfx_2d_div_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-divider-clock";
+		clocks = <&core_l3_ck>;
+		ti,max-div = <4>;
+		reg = <0x0340>;
+		ti,index-starts-at-one;
+	};
+
+	gfx_2d_fck: gfx_2d_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&gfx_2d_gate_fck>, <&gfx_2d_div_fck>;
+	};
+
+	gfx_ick: gfx_ick {
+		#clock-cells = <0>;
+		compatible = "ti,wait-gate-clock";
+		clocks = <&core_l3_ck>;
+		ti,bit-shift = <0>;
+		reg = <0x0310>;
+	};
+
+	l4_ck: l4_ck {
+		#clock-cells = <0>;
+		compatible = "ti,divider-clock";
+		clocks = <&core_l3_ck>;
+		ti,bit-shift = <5>;
+		ti,max-div = <3>;
+		reg = <0x0240>;
+		ti,index-starts-at-one;
+	};
+
+	dss_ick: dss_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-no-wait-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <0>;
+		reg = <0x0210>;
+	};
+
+	dss1_gate_fck: dss1_gate_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-no-wait-gate-clock";
+		clocks = <&core_ck>;
+		ti,bit-shift = <0>;
+		reg = <0x0200>;
+	};
+
+	core_d2_ck: core_d2_ck {
+		#clock-cells = <0>;
+		compatible = "fixed-factor-clock";
+		clocks = <&core_ck>;
+		clock-mult = <1>;
+		clock-div = <2>;
+	};
+
+	core_d3_ck: core_d3_ck {
+		#clock-cells = <0>;
+		compatible = "fixed-factor-clock";
+		clocks = <&core_ck>;
+		clock-mult = <1>;
+		clock-div = <3>;
+	};
+
+	core_d4_ck: core_d4_ck {
+		#clock-cells = <0>;
+		compatible = "fixed-factor-clock";
+		clocks = <&core_ck>;
+		clock-mult = <1>;
+		clock-div = <4>;
+	};
+
+	core_d5_ck: core_d5_ck {
+		#clock-cells = <0>;
+		compatible = "fixed-factor-clock";
+		clocks = <&core_ck>;
+		clock-mult = <1>;
+		clock-div = <5>;
+	};
+
+	core_d6_ck: core_d6_ck {
+		#clock-cells = <0>;
+		compatible = "fixed-factor-clock";
+		clocks = <&core_ck>;
+		clock-mult = <1>;
+		clock-div = <6>;
+	};
+
+	dummy_ck: dummy_ck {
+		#clock-cells = <0>;
+		compatible = "fixed-clock";
+		clock-frequency = <0>;
+	};
+
+	core_d8_ck: core_d8_ck {
+		#clock-cells = <0>;
+		compatible = "fixed-factor-clock";
+		clocks = <&core_ck>;
+		clock-mult = <1>;
+		clock-div = <8>;
+	};
+
+	core_d9_ck: core_d9_ck {
+		#clock-cells = <0>;
+		compatible = "fixed-factor-clock";
+		clocks = <&core_ck>;
+		clock-mult = <1>;
+		clock-div = <9>;
+	};
+
+	core_d12_ck: core_d12_ck {
+		#clock-cells = <0>;
+		compatible = "fixed-factor-clock";
+		clocks = <&core_ck>;
+		clock-mult = <1>;
+		clock-div = <12>;
+	};
+
+	core_d16_ck: core_d16_ck {
+		#clock-cells = <0>;
+		compatible = "fixed-factor-clock";
+		clocks = <&core_ck>;
+		clock-mult = <1>;
+		clock-div = <16>;
+	};
+
+	dss1_mux_fck: dss1_mux_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-mux-clock";
+		clocks = <&sys_ck>, <&core_ck>, <&core_d2_ck>, <&core_d3_ck>, <&core_d4_ck>, <&core_d5_ck>, <&core_d6_ck>, <&core_d8_ck>, <&core_d9_ck>, <&core_d12_ck>, <&core_d16_ck>;
+		ti,bit-shift = <8>;
+		reg = <0x0240>;
+	};
+
+	dss1_fck: dss1_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&dss1_gate_fck>, <&dss1_mux_fck>;
+	};
+
+	dss2_gate_fck: dss2_gate_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-no-wait-gate-clock";
+		clocks = <&func_48m_ck>;
+		ti,bit-shift = <1>;
+		reg = <0x0200>;
+	};
+
+	dss2_mux_fck: dss2_mux_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-mux-clock";
+		clocks = <&sys_ck>, <&func_48m_ck>;
+		ti,bit-shift = <13>;
+		reg = <0x0240>;
+	};
+
+	dss2_fck: dss2_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&dss2_gate_fck>, <&dss2_mux_fck>;
+	};
+
+	dss_54m_fck: dss_54m_fck {
+		#clock-cells = <0>;
+		compatible = "ti,wait-gate-clock";
+		clocks = <&func_54m_ck>;
+		ti,bit-shift = <2>;
+		reg = <0x0200>;
+	};
+
+	ssi_ssr_sst_gate_fck: ssi_ssr_sst_gate_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-gate-clock";
+		clocks = <&core_ck>;
+		ti,bit-shift = <1>;
+		reg = <0x0204>;
+	};
+
+	ssi_ssr_sst_div_fck: ssi_ssr_sst_div_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-divider-clock";
+		clocks = <&core_ck>;
+		ti,bit-shift = <20>;
+		reg = <0x0240>;
+	};
+
+	ssi_ssr_sst_fck: ssi_ssr_sst_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&ssi_ssr_sst_gate_fck>, <&ssi_ssr_sst_div_fck>;
+	};
+
+	usb_l4_gate_ick: usb_l4_gate_ick {
+		#clock-cells = <0>;
+		compatible = "ti,composite-interface-clock";
+		clocks = <&core_l3_ck>;
+		ti,bit-shift = <0>;
+		reg = <0x0214>;
+	};
+
+	usb_l4_div_ick: usb_l4_div_ick {
+		#clock-cells = <0>;
+		compatible = "ti,composite-divider-clock";
+		clocks = <&core_l3_ck>;
+		ti,bit-shift = <25>;
+		reg = <0x0240>;
+		ti,dividers = <0>, <1>, <2>, <0>, <4>;
+	};
+
+	usb_l4_ick: usb_l4_ick {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&usb_l4_gate_ick>, <&usb_l4_div_ick>;
+	};
+
+	ssi_l4_ick: ssi_l4_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <1>;
+		reg = <0x0214>;
+	};
+
+	gpt1_ick: gpt1_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&sys_ck>;
+		ti,bit-shift = <0>;
+		reg = <0x0410>;
+	};
+
+	gpt1_gate_fck: gpt1_gate_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-gate-clock";
+		clocks = <&func_32k_ck>;
+		ti,bit-shift = <0>;
+		reg = <0x0400>;
+	};
+
+	gpt1_mux_fck: gpt1_mux_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-mux-clock";
+		clocks = <&func_32k_ck>, <&sys_ck>, <&alt_ck>;
+		reg = <0x0440>;
+	};
+
+	gpt1_fck: gpt1_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&gpt1_gate_fck>, <&gpt1_mux_fck>;
+	};
+
+	gpt2_ick: gpt2_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <4>;
+		reg = <0x0210>;
+	};
+
+	gpt2_gate_fck: gpt2_gate_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-gate-clock";
+		clocks = <&func_32k_ck>;
+		ti,bit-shift = <4>;
+		reg = <0x0200>;
+	};
+
+	gpt2_mux_fck: gpt2_mux_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-mux-clock";
+		clocks = <&func_32k_ck>, <&sys_ck>, <&alt_ck>;
+		ti,bit-shift = <2>;
+		reg = <0x0244>;
+	};
+
+	gpt2_fck: gpt2_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&gpt2_gate_fck>, <&gpt2_mux_fck>;
+	};
+
+	gpt3_ick: gpt3_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <5>;
+		reg = <0x0210>;
+	};
+
+	gpt3_gate_fck: gpt3_gate_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-gate-clock";
+		clocks = <&func_32k_ck>;
+		ti,bit-shift = <5>;
+		reg = <0x0200>;
+	};
+
+	gpt3_mux_fck: gpt3_mux_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-mux-clock";
+		clocks = <&func_32k_ck>, <&sys_ck>, <&alt_ck>;
+		ti,bit-shift = <4>;
+		reg = <0x0244>;
+	};
+
+	gpt3_fck: gpt3_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&gpt3_gate_fck>, <&gpt3_mux_fck>;
+	};
+
+	gpt4_ick: gpt4_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <6>;
+		reg = <0x0210>;
+	};
+
+	gpt4_gate_fck: gpt4_gate_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-gate-clock";
+		clocks = <&func_32k_ck>;
+		ti,bit-shift = <6>;
+		reg = <0x0200>;
+	};
+
+	gpt4_mux_fck: gpt4_mux_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-mux-clock";
+		clocks = <&func_32k_ck>, <&sys_ck>, <&alt_ck>;
+		ti,bit-shift = <6>;
+		reg = <0x0244>;
+	};
+
+	gpt4_fck: gpt4_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&gpt4_gate_fck>, <&gpt4_mux_fck>;
+	};
+
+	gpt5_ick: gpt5_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <7>;
+		reg = <0x0210>;
+	};
+
+	gpt5_gate_fck: gpt5_gate_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-gate-clock";
+		clocks = <&func_32k_ck>;
+		ti,bit-shift = <7>;
+		reg = <0x0200>;
+	};
+
+	gpt5_mux_fck: gpt5_mux_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-mux-clock";
+		clocks = <&func_32k_ck>, <&sys_ck>, <&alt_ck>;
+		ti,bit-shift = <8>;
+		reg = <0x0244>;
+	};
+
+	gpt5_fck: gpt5_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&gpt5_gate_fck>, <&gpt5_mux_fck>;
+	};
+
+	gpt6_ick: gpt6_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <8>;
+		reg = <0x0210>;
+	};
+
+	gpt6_gate_fck: gpt6_gate_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-gate-clock";
+		clocks = <&func_32k_ck>;
+		ti,bit-shift = <8>;
+		reg = <0x0200>;
+	};
+
+	gpt6_mux_fck: gpt6_mux_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-mux-clock";
+		clocks = <&func_32k_ck>, <&sys_ck>, <&alt_ck>;
+		ti,bit-shift = <10>;
+		reg = <0x0244>;
+	};
+
+	gpt6_fck: gpt6_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&gpt6_gate_fck>, <&gpt6_mux_fck>;
+	};
+
+	gpt7_ick: gpt7_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <9>;
+		reg = <0x0210>;
+	};
+
+	gpt7_gate_fck: gpt7_gate_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-gate-clock";
+		clocks = <&func_32k_ck>;
+		ti,bit-shift = <9>;
+		reg = <0x0200>;
+	};
+
+	gpt7_mux_fck: gpt7_mux_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-mux-clock";
+		clocks = <&func_32k_ck>, <&sys_ck>, <&alt_ck>;
+		ti,bit-shift = <12>;
+		reg = <0x0244>;
+	};
+
+	gpt7_fck: gpt7_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&gpt7_gate_fck>, <&gpt7_mux_fck>;
+	};
+
+	gpt8_ick: gpt8_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <10>;
+		reg = <0x0210>;
+	};
+
+	gpt8_gate_fck: gpt8_gate_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-gate-clock";
+		clocks = <&func_32k_ck>;
+		ti,bit-shift = <10>;
+		reg = <0x0200>;
+	};
+
+	gpt8_mux_fck: gpt8_mux_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-mux-clock";
+		clocks = <&func_32k_ck>, <&sys_ck>, <&alt_ck>;
+		ti,bit-shift = <14>;
+		reg = <0x0244>;
+	};
+
+	gpt8_fck: gpt8_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&gpt8_gate_fck>, <&gpt8_mux_fck>;
+	};
+
+	gpt9_ick: gpt9_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <11>;
+		reg = <0x0210>;
+	};
+
+	gpt9_gate_fck: gpt9_gate_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-gate-clock";
+		clocks = <&func_32k_ck>;
+		ti,bit-shift = <11>;
+		reg = <0x0200>;
+	};
+
+	gpt9_mux_fck: gpt9_mux_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-mux-clock";
+		clocks = <&func_32k_ck>, <&sys_ck>, <&alt_ck>;
+		ti,bit-shift = <16>;
+		reg = <0x0244>;
+	};
+
+	gpt9_fck: gpt9_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&gpt9_gate_fck>, <&gpt9_mux_fck>;
+	};
+
+	gpt10_ick: gpt10_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <12>;
+		reg = <0x0210>;
+	};
+
+	gpt10_gate_fck: gpt10_gate_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-gate-clock";
+		clocks = <&func_32k_ck>;
+		ti,bit-shift = <12>;
+		reg = <0x0200>;
+	};
+
+	gpt10_mux_fck: gpt10_mux_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-mux-clock";
+		clocks = <&func_32k_ck>, <&sys_ck>, <&alt_ck>;
+		ti,bit-shift = <18>;
+		reg = <0x0244>;
+	};
+
+	gpt10_fck: gpt10_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&gpt10_gate_fck>, <&gpt10_mux_fck>;
+	};
+
+	gpt11_ick: gpt11_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <13>;
+		reg = <0x0210>;
+	};
+
+	gpt11_gate_fck: gpt11_gate_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-gate-clock";
+		clocks = <&func_32k_ck>;
+		ti,bit-shift = <13>;
+		reg = <0x0200>;
+	};
+
+	gpt11_mux_fck: gpt11_mux_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-mux-clock";
+		clocks = <&func_32k_ck>, <&sys_ck>, <&alt_ck>;
+		ti,bit-shift = <20>;
+		reg = <0x0244>;
+	};
+
+	gpt11_fck: gpt11_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&gpt11_gate_fck>, <&gpt11_mux_fck>;
+	};
+
+	gpt12_ick: gpt12_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <14>;
+		reg = <0x0210>;
+	};
+
+	gpt12_gate_fck: gpt12_gate_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-gate-clock";
+		clocks = <&func_32k_ck>;
+		ti,bit-shift = <14>;
+		reg = <0x0200>;
+	};
+
+	gpt12_mux_fck: gpt12_mux_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-mux-clock";
+		clocks = <&func_32k_ck>, <&sys_ck>, <&alt_ck>;
+		ti,bit-shift = <22>;
+		reg = <0x0244>;
+	};
+
+	gpt12_fck: gpt12_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-clock";
+		clocks = <&gpt12_gate_fck>, <&gpt12_mux_fck>;
+	};
+
+	mcbsp1_ick: mcbsp1_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <15>;
+		reg = <0x0210>;
+	};
+
+	mcbsp1_gate_fck: mcbsp1_gate_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-gate-clock";
+		clocks = <&mcbsp_clks>;
+		ti,bit-shift = <15>;
+		reg = <0x0200>;
+	};
+
+	mcbsp2_ick: mcbsp2_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <16>;
+		reg = <0x0210>;
+	};
+
+	mcbsp2_gate_fck: mcbsp2_gate_fck {
+		#clock-cells = <0>;
+		compatible = "ti,composite-gate-clock";
+		clocks = <&mcbsp_clks>;
+		ti,bit-shift = <16>;
+		reg = <0x0200>;
+	};
+
+	mcspi1_ick: mcspi1_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <17>;
+		reg = <0x0210>;
+	};
+
+	mcspi1_fck: mcspi1_fck {
+		#clock-cells = <0>;
+		compatible = "ti,wait-gate-clock";
+		clocks = <&func_48m_ck>;
+		ti,bit-shift = <17>;
+		reg = <0x0200>;
+	};
+
+	mcspi2_ick: mcspi2_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <18>;
+		reg = <0x0210>;
+	};
+
+	mcspi2_fck: mcspi2_fck {
+		#clock-cells = <0>;
+		compatible = "ti,wait-gate-clock";
+		clocks = <&func_48m_ck>;
+		ti,bit-shift = <18>;
+		reg = <0x0200>;
+	};
+
+	uart1_ick: uart1_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <21>;
+		reg = <0x0210>;
+	};
+
+	uart1_fck: uart1_fck {
+		#clock-cells = <0>;
+		compatible = "ti,wait-gate-clock";
+		clocks = <&func_48m_ck>;
+		ti,bit-shift = <21>;
+		reg = <0x0200>;
+	};
+
+	uart2_ick: uart2_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <22>;
+		reg = <0x0210>;
+	};
+
+	uart2_fck: uart2_fck {
+		#clock-cells = <0>;
+		compatible = "ti,wait-gate-clock";
+		clocks = <&func_48m_ck>;
+		ti,bit-shift = <22>;
+		reg = <0x0200>;
+	};
+
+	uart3_ick: uart3_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <2>;
+		reg = <0x0214>;
+	};
+
+	uart3_fck: uart3_fck {
+		#clock-cells = <0>;
+		compatible = "ti,wait-gate-clock";
+		clocks = <&func_48m_ck>;
+		ti,bit-shift = <2>;
+		reg = <0x0204>;
+	};
+
+	gpios_ick: gpios_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&sys_ck>;
+		ti,bit-shift = <2>;
+		reg = <0x0410>;
+	};
+
+	gpios_fck: gpios_fck {
+		#clock-cells = <0>;
+		compatible = "ti,wait-gate-clock";
+		clocks = <&func_32k_ck>;
+		ti,bit-shift = <2>;
+		reg = <0x0400>;
+	};
+
+	mpu_wdt_ick: mpu_wdt_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&sys_ck>;
+		ti,bit-shift = <3>;
+		reg = <0x0410>;
+	};
+
+	mpu_wdt_fck: mpu_wdt_fck {
+		#clock-cells = <0>;
+		compatible = "ti,wait-gate-clock";
+		clocks = <&func_32k_ck>;
+		ti,bit-shift = <3>;
+		reg = <0x0400>;
+	};
+
+	sync_32k_ick: sync_32k_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&sys_ck>;
+		ti,bit-shift = <1>;
+		reg = <0x0410>;
+	};
+
+	wdt1_ick: wdt1_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&sys_ck>;
+		ti,bit-shift = <4>;
+		reg = <0x0410>;
+	};
+
+	omapctrl_ick: omapctrl_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&sys_ck>;
+		ti,bit-shift = <5>;
+		reg = <0x0410>;
+	};
+
+	cam_fck: cam_fck {
+		#clock-cells = <0>;
+		compatible = "ti,gate-clock";
+		clocks = <&func_96m_ck>;
+		ti,bit-shift = <31>;
+		reg = <0x0200>;
+	};
+
+	cam_ick: cam_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-no-wait-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <31>;
+		reg = <0x0210>;
+	};
+
+	mailboxes_ick: mailboxes_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <30>;
+		reg = <0x0210>;
+	};
+
+	wdt4_ick: wdt4_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <29>;
+		reg = <0x0210>;
+	};
+
+	wdt4_fck: wdt4_fck {
+		#clock-cells = <0>;
+		compatible = "ti,wait-gate-clock";
+		clocks = <&func_32k_ck>;
+		ti,bit-shift = <29>;
+		reg = <0x0200>;
+	};
+
+	mspro_ick: mspro_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <27>;
+		reg = <0x0210>;
+	};
+
+	mspro_fck: mspro_fck {
+		#clock-cells = <0>;
+		compatible = "ti,wait-gate-clock";
+		clocks = <&func_96m_ck>;
+		ti,bit-shift = <27>;
+		reg = <0x0200>;
+	};
+
+	fac_ick: fac_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <25>;
+		reg = <0x0210>;
+	};
+
+	fac_fck: fac_fck {
+		#clock-cells = <0>;
+		compatible = "ti,wait-gate-clock";
+		clocks = <&func_12m_ck>;
+		ti,bit-shift = <25>;
+		reg = <0x0200>;
+	};
+
+	hdq_ick: hdq_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <23>;
+		reg = <0x0210>;
+	};
+
+	hdq_fck: hdq_fck {
+		#clock-cells = <0>;
+		compatible = "ti,wait-gate-clock";
+		clocks = <&func_12m_ck>;
+		ti,bit-shift = <23>;
+		reg = <0x0200>;
+	};
+
+	i2c1_ick: i2c1_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <19>;
+		reg = <0x0210>;
+	};
+
+	i2c2_ick: i2c2_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <20>;
+		reg = <0x0210>;
+	};
+
+	gpmc_fck: gpmc_fck {
+		#clock-cells = <0>;
+		compatible = "ti,fixed-factor-clock";
+		clocks = <&core_l3_ck>;
+		ti,clock-div = <1>;
+		ti,autoidle-shift = <1>;
+		reg = <0x0238>;
+		ti,clock-mult = <1>;
+	};
+
+	sdma_fck: sdma_fck {
+		#clock-cells = <0>;
+		compatible = "fixed-factor-clock";
+		clocks = <&core_l3_ck>;
+		clock-mult = <1>;
+		clock-div = <1>;
+	};
+
+	sdma_ick: sdma_ick {
+		#clock-cells = <0>;
+		compatible = "ti,fixed-factor-clock";
+		clocks = <&core_l3_ck>;
+		ti,clock-div = <1>;
+		ti,autoidle-shift = <0>;
+		reg = <0x0238>;
+		ti,clock-mult = <1>;
+	};
+
+	sdrc_ick: sdrc_ick {
+		#clock-cells = <0>;
+		compatible = "ti,fixed-factor-clock";
+		clocks = <&core_l3_ck>;
+		ti,clock-div = <1>;
+		ti,autoidle-shift = <2>;
+		reg = <0x0238>;
+		ti,clock-mult = <1>;
+	};
+
+	des_ick: des_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <0>;
+		reg = <0x021c>;
+	};
+
+	sha_ick: sha_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <1>;
+		reg = <0x021c>;
+	};
+
+	rng_ick: rng_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <2>;
+		reg = <0x021c>;
+	};
+
+	aes_ick: aes_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <3>;
+		reg = <0x021c>;
+	};
+
+	pka_ick: pka_ick {
+		#clock-cells = <0>;
+		compatible = "ti,omap3-interface-clock";
+		clocks = <&l4_ck>;
+		ti,bit-shift = <4>;
+		reg = <0x021c>;
+	};
+
+	usb_fck: usb_fck {
+		#clock-cells = <0>;
+		compatible = "ti,wait-gate-clock";
+		clocks = <&func_48m_ck>;
+		ti,bit-shift = <0>;
+		reg = <0x0204>;
+	};
+};
diff --git a/arch/arm/boot/dts/omap3-beagle-xm-ab.dts b/arch/arm/boot/dts/omap3-beagle-xm-ab.dts
new file mode 100644
index 00000000000..7ac3bcf59d5
--- /dev/null
+++ b/arch/arm/boot/dts/omap3-beagle-xm-ab.dts
@@ -0,0 +1,16 @@
+/*
+ * Copyright (C) 2011 Texas Instruments Incorporated - http://www.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.
+ */
+
+#include "omap3-beagle-xm.dts"
+
+/ {
+	/* HS USB Port 2 Power enable was inverted with the xM C */
+	hsusb2_power: hsusb2_power_reg {
+		enable-active-high;
+	};
+};
diff --git a/arch/arm/boot/dts/omap3-cm-t3x30.dtsi b/arch/arm/boot/dts/omap3-cm-t3x30.dtsi
index d00055809e3..25ba08331d8 100644
--- a/arch/arm/boot/dts/omap3-cm-t3x30.dtsi
+++ b/arch/arm/boot/dts/omap3-cm-t3x30.dtsi
@@ -10,18 +10,6 @@
 			cpu0-supply = <&vcc>;
 		};
 	};
-
-	vddvario: regulator-vddvario {
-		compatible = "regulator-fixed";
-		regulator-name = "vddvario";
-		regulator-always-on;
-	};
-
-	vdd33a: regulator-vdd33a {
-		compatible = "regulator-fixed";
-		regulator-name = "vdd33a";
-		regulator-always-on;
-	};
 };
 
 &omap3_pmx_core {
@@ -35,58 +23,34 @@
 
  	hsusb0_pins: pinmux_hsusb0_pins {
 		pinctrl-single,pins = <
-			OMAP3_CORE1_IOPAD(0x21a0, PIN_OUTPUT | MUX_MODE0)		/* hsusb0_clk.hsusb0_clk */
-			OMAP3_CORE1_IOPAD(0x21a2, PIN_OUTPUT | MUX_MODE0)		/* hsusb0_stp.hsusb0_stp */
-			OMAP3_CORE1_IOPAD(0x21a4, PIN_INPUT_PULLDOWN | MUX_MODE0)	/* hsusb0_dir.hsusb0_dir */
-			OMAP3_CORE1_IOPAD(0x21a6, PIN_INPUT_PULLDOWN | MUX_MODE0)	/* hsusb0_nxt.hsusb0_nxt */
-			OMAP3_CORE1_IOPAD(0x21a8, PIN_INPUT_PULLDOWN | MUX_MODE0)	/* hsusb0_data0.hsusb2_data0 */
-			OMAP3_CORE1_IOPAD(0x21aa, PIN_INPUT_PULLDOWN | MUX_MODE0)	/* hsusb0_data1.hsusb0_data1 */
-			OMAP3_CORE1_IOPAD(0x21ac, PIN_INPUT_PULLDOWN | MUX_MODE0)	/* hsusb0_data2.hsusb0_data2 */
-			OMAP3_CORE1_IOPAD(0x21ae, PIN_INPUT_PULLDOWN | MUX_MODE0)	/* hsusb0_data7.hsusb0_data3 */
-			OMAP3_CORE1_IOPAD(0x21b0, PIN_INPUT_PULLDOWN | MUX_MODE0)	/* hsusb0_data7.hsusb0_data4 */
-			OMAP3_CORE1_IOPAD(0x21b2, PIN_INPUT_PULLDOWN | MUX_MODE0)	/* hsusb0_data7.hsusb0_data5 */
-			OMAP3_CORE1_IOPAD(0x21b4, PIN_INPUT_PULLDOWN | MUX_MODE0)	/* hsusb0_data7.hsusb0_data6 */
-			OMAP3_CORE1_IOPAD(0x21b6, PIN_INPUT_PULLDOWN | MUX_MODE0)	/* hsusb0_data7.hsusb0_data7 */
+			OMAP3_CORE1_IOPAD(0x21a2, PIN_OUTPUT | MUX_MODE0)		/* hsusb0_clk.hsusb0_clk */
+			OMAP3_CORE1_IOPAD(0x21a4, PIN_OUTPUT | MUX_MODE0)		/* hsusb0_stp.hsusb0_stp */
+			OMAP3_CORE1_IOPAD(0x21a6, PIN_INPUT_PULLDOWN | MUX_MODE0)	/* hsusb0_dir.hsusb0_dir */
+			OMAP3_CORE1_IOPAD(0x21a8, PIN_INPUT_PULLDOWN | MUX_MODE0)	/* hsusb0_nxt.hsusb0_nxt */
+			OMAP3_CORE1_IOPAD(0x21aa, PIN_INPUT_PULLDOWN | MUX_MODE0)	/* hsusb0_data0.hsusb2_data0 */
+			OMAP3_CORE1_IOPAD(0x21ac, PIN_INPUT_PULLDOWN | MUX_MODE0)	/* hsusb0_data1.hsusb0_data1 */
+			OMAP3_CORE1_IOPAD(0x21ae, PIN_INPUT_PULLDOWN | MUX_MODE0)	/* hsusb0_data2.hsusb0_data2 */
+			OMAP3_CORE1_IOPAD(0x21b0, PIN_INPUT_PULLDOWN | MUX_MODE0)	/* hsusb0_data7.hsusb0_data3 */
+			OMAP3_CORE1_IOPAD(0x21b2, PIN_INPUT_PULLDOWN | MUX_MODE0)	/* hsusb0_data7.hsusb0_data4 */
+			OMAP3_CORE1_IOPAD(0x21b4, PIN_INPUT_PULLDOWN | MUX_MODE0)	/* hsusb0_data7.hsusb0_data5 */
+			OMAP3_CORE1_IOPAD(0x21b6, PIN_INPUT_PULLDOWN | MUX_MODE0)	/* hsusb0_data7.hsusb0_data6 */
+			OMAP3_CORE1_IOPAD(0x21b8, PIN_INPUT_PULLDOWN | MUX_MODE0)	/* hsusb0_data7.hsusb0_data7 */
 		>;
 	};
 };
 
+#include "omap-gpmc-smsc911x.dtsi"
+
 &gpmc {
 	ranges = <5 0 0x2c000000 0x01000000>;
 
-	smsc1: ethernet@5,0 {
+	smsc1: ethernet@gpmc {
 		compatible = "smsc,lan9221", "smsc,lan9115";
 		pinctrl-names = "default";
 		pinctrl-0 = <&smsc1_pins>;
 		interrupt-parent = <&gpio6>;
 		interrupts = <3 IRQ_TYPE_LEVEL_LOW>;
 		reg = <5 0 0xff>;
-		bank-width = <2>;
-		gpmc,mux-add-data;
-		gpmc,cs-on-ns = <0>;
-		gpmc,cs-rd-off-ns = <186>;
-		gpmc,cs-wr-off-ns = <186>;
-		gpmc,adv-on-ns = <12>;
-		gpmc,adv-rd-off-ns = <48>;
-		gpmc,adv-wr-off-ns = <48>;
-		gpmc,oe-on-ns = <54>;
-		gpmc,oe-off-ns = <168>;
-		gpmc,we-on-ns = <54>;
-		gpmc,we-off-ns = <168>;
-		gpmc,rd-cycle-ns = <186>;
-		gpmc,wr-cycle-ns = <186>;
-		gpmc,access-ns = <114>;
-		gpmc,page-burst-access-ns = <6>;
-		gpmc,bus-turnaround-ns = <12>;
-		gpmc,cycle2cycle-delay-ns = <18>;
-		gpmc,wr-data-mux-bus-ns = <90>;
-		gpmc,wr-access-ns = <186>;
-		gpmc,cycle2cycle-samecsen;
-		gpmc,cycle2cycle-diffcsen;
-		vddvario-supply = <&vddvario>;
-		vdd33a-supply = <&vdd33a>;
-		reg-io-width = <4>;
-		smsc,save-mac-address;
 	};
 };
 
diff --git a/arch/arm/boot/dts/omap3-devkit8000.dts b/arch/arm/boot/dts/omap3-devkit8000.dts
index bf5a515a324..da402f0fdab 100644
--- a/arch/arm/boot/dts/omap3-devkit8000.dts
+++ b/arch/arm/boot/dts/omap3-devkit8000.dts
@@ -112,7 +112,6 @@
 		reg = <0 0 0>; /* CS0, offset 0 */
 		nand-bus-width = <16>;
 
-		gpmc,device-nand;
 		gpmc,sync-clk-ps = <0>;
 		gpmc,cs-on-ns = <0>;
 		gpmc,cs-rd-off-ns = <44>;
diff --git a/arch/arm/boot/dts/omap3-evm-37xx.dts b/arch/arm/boot/dts/omap3-evm-37xx.dts
index 4df68ad3736..9cba94bed7a 100644
--- a/arch/arm/boot/dts/omap3-evm-37xx.dts
+++ b/arch/arm/boot/dts/omap3-evm-37xx.dts
@@ -89,7 +89,16 @@
 	status = "disabled";
 };
 
+&uart1 {
+	interrupts-extended = <&intc 72 &omap3_pmx_core OMAP3_UART1_RX>;
+};
+
+&uart2 {
+	interrupts-extended = <&intc 73 &omap3_pmx_core OMAP3_UART2_RX>;
+};
+
 &uart3 {
+	interrupts-extended = <&intc 74 &omap3_pmx_core OMAP3_UART3_RX>;
 	pinctrl-names = "default";
 	pinctrl-0 = <&uart3_pins>;
 };
diff --git a/arch/arm/boot/dts/omap3-igep.dtsi b/arch/arm/boot/dts/omap3-igep.dtsi
index b97736d98a6..e2d163bf061 100644
--- a/arch/arm/boot/dts/omap3-igep.dtsi
+++ b/arch/arm/boot/dts/omap3-igep.dtsi
@@ -107,7 +107,7 @@
 		>;
 	};
 
-	smsc911x_pins: pinmux_smsc911x_pins {
+	smsc9221_pins: pinmux_smsc9221_pins {
 		pinctrl-single,pins = <
 			0x1a2 (PIN_INPUT | MUX_MODE4)		/* mcspi1_cs2.gpio_176 */
 		>;
diff --git a/arch/arm/boot/dts/omap3-igep0020.dts b/arch/arm/boot/dts/omap3-igep0020.dts
index 7abd64f6ae2..b22caaaf774 100644
--- a/arch/arm/boot/dts/omap3-igep0020.dts
+++ b/arch/arm/boot/dts/omap3-igep0020.dts
@@ -10,7 +10,7 @@
  */
 
 #include "omap3-igep.dtsi"
-#include "omap-gpmc-smsc911x.dtsi"
+#include "omap-gpmc-smsc9221.dtsi"
 
 / {
 	model = "IGEPv2 (TI OMAP AM/DM37x)";
@@ -248,7 +248,7 @@
 
 	ethernet@gpmc {
 		pinctrl-names = "default";
-		pinctrl-0 = <&smsc911x_pins>;
+		pinctrl-0 = <&smsc9221_pins>;
 		reg = <5 0 0xff>;
 		interrupt-parent = <&gpio6>;
 		interrupts = <16 IRQ_TYPE_LEVEL_LOW>;
diff --git a/arch/arm/boot/dts/omap3-ldp.dts b/arch/arm/boot/dts/omap3-ldp.dts
index 0abe986a4ec..476ff158ddb 100644
--- a/arch/arm/boot/dts/omap3-ldp.dts
+++ b/arch/arm/boot/dts/omap3-ldp.dts
@@ -234,6 +234,10 @@
 	};
 };
 
+&uart3 {
+	interrupts-extended = <&intc 74 &omap3_pmx_core OMAP3_UART3_RX>;
+};
+
 &usb_otg_hs {
 	pinctrl-names = "default";
 	pinctrl-0 = <&musb_pins>;
diff --git a/arch/arm/boot/dts/omap3-lilly-a83x.dtsi b/arch/arm/boot/dts/omap3-lilly-a83x.dtsi
index 6369d9f43ca..d97308896f0 100644
--- a/arch/arm/boot/dts/omap3-lilly-a83x.dtsi
+++ b/arch/arm/boot/dts/omap3-lilly-a83x.dtsi
@@ -176,9 +176,6 @@
 
 &omap3_pmx_core2 {
 	pinctrl-names = "default";
-	pinctrl-0 = <
-			&hsusb1_2_pins
-	>;
 
 	hsusb1_2_pins: pinmux_hsusb1_2_pins {
 		pinctrl-single,pins = <
@@ -357,6 +354,10 @@
 	power = <50>;
 };
 
+&mcbsp2 {
+	status = "okay";
+};
+
 &gpmc {
 	ranges = <0 0 0x30000000 0x1000000>,
 		<7 0 0x15000000 0x01000000>;
@@ -368,7 +369,6 @@
 		/* no elm on omap3 */
 
 		gpmc,mux-add-data = <0>;
-		gpmc,device-nand;
 		gpmc,device-width = <2>;
 		gpmc,wait-pin = <0>;
 		gpmc,wait-monitoring-ns = <0>;
diff --git a/arch/arm/boot/dts/omap3-n900.dts b/arch/arm/boot/dts/omap3-n900.dts
index 1a57b61f5e2..059a8ff1e6a 100644
--- a/arch/arm/boot/dts/omap3-n900.dts
+++ b/arch/arm/boot/dts/omap3-n900.dts
@@ -10,6 +10,7 @@
 /dts-v1/;
 
 #include "omap34xx-hs.dtsi"
+#include <dt-bindings/input/input.h>
 
 / {
 	model = "Nokia N900";
@@ -21,6 +22,17 @@
 		};
 	};
 
+	leds {
+		compatible = "gpio-leds";
+		heartbeat {
+			label = "debug::sleep";
+			gpios = <&gpio6 2 GPIO_ACTIVE_HIGH>;  /* gpio162 */
+			linux,default-trigger = "default-on";
+			pinctrl-names = "default";
+			pinctrl-0 = <&debug_leds>;
+		};
+	};
+
 	memory {
 		device_type = "memory";
 		reg = <0x80000000 0x10000000>; /* 256 MB */
@@ -90,6 +102,19 @@
 			};
 		};
 	};
+
+	sound: n900-audio {
+		compatible = "nokia,n900-audio";
+
+		nokia,cpu-dai = <&mcbsp2>;
+		nokia,audio-codec = <&tlv320aic3x>, <&tlv320aic3x_aux>;
+		nokia,headphone-amplifier = <&tpa6130a2>;
+
+		tvout-selection-gpios = <&gpio2 8 GPIO_ACTIVE_HIGH>; /* 40 */
+		jack-detection-gpios = <&gpio6 17 GPIO_ACTIVE_HIGH>; /* 177 */
+		eci-switch-gpios = <&gpio6 22 GPIO_ACTIVE_HIGH>; /* 182 */
+		speaker-amplifier-gpios = <&twl_gpio 7 GPIO_ACTIVE_HIGH>;
+	};
 };
 
 &omap3_pmx_core {
@@ -130,6 +155,21 @@
 		>;
 	};
 
+	debug_leds: pinmux_debug_led_pins {
+		pinctrl-single,pins = <
+			OMAP3_CORE1_IOPAD(0x2198, PIN_OUTPUT | MUX_MODE4)	/* mcbsp1_clkx.gpio_162 */
+		>;
+	};
+
+	mcspi4_pins: pinmux_mcspi4_pins {
+		pinctrl-single,pins = <
+			0x15c (PIN_INPUT_PULLDOWN | MUX_MODE1) /* mcspi4_clk */
+			0x162 (PIN_INPUT_PULLDOWN | MUX_MODE1) /* mcspi4_somi */
+			0x160 (PIN_OUTPUT | MUX_MODE1) /* mcspi4_simo */
+			0x166 (PIN_OUTPUT | MUX_MODE1) /* mcspi4_cs0 */
+		>;
+	};
+
 	mmc1_pins: pinmux_mmc1_pins {
 		pinctrl-single,pins = <
 			0x114 (PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmmc1_clk */
@@ -173,6 +213,37 @@
 			0x0da (PIN_OUTPUT | MUX_MODE1)   /* dss_data23.sdi_clkn */
 		>;
 	};
+
+	wl1251_pins: pinmux_wl1251 {
+		pinctrl-single,pins = <
+			0x0ce (PIN_OUTPUT | MUX_MODE4)		/* gpio 87 => wl1251 enable */
+			0x05a (PIN_INPUT | MUX_MODE4)		/* gpio 42 => wl1251 irq */
+		>;
+	};
+
+	ssi_pins: pinmux_ssi {
+		pinctrl-single,pins = <
+			0x150 (PIN_INPUT_PULLUP | MUX_MODE1)	/* ssi1_rdy_tx */
+			0x14e (PIN_OUTPUT | MUX_MODE1)		/* ssi1_flag_tx */
+			0x152 (PIN_INPUT | WAKEUP_EN | MUX_MODE4) /* ssi1_wake_tx (cawake) */
+			0x14c (PIN_OUTPUT | MUX_MODE1)		/* ssi1_dat_tx */
+			0x154 (PIN_INPUT | MUX_MODE1)		/* ssi1_dat_rx */
+			0x156 (PIN_INPUT | MUX_MODE1)		/* ssi1_flag_rx */
+			0x158 (PIN_OUTPUT | MUX_MODE1)		/* ssi1_rdy_rx */
+			0x15a (PIN_OUTPUT | MUX_MODE1)		/* ssi1_wake */
+		>;
+	};
+
+	modem_pins: pinmux_modem {
+		pinctrl-single,pins = <
+			0x0ac (PIN_OUTPUT | MUX_MODE4)		/* gpio 70 => cmt_apeslpx */
+			0x0b0 (PIN_INPUT | WAKEUP_EN | MUX_MODE4) /* gpio 72 => ape_rst_rq */
+			0x0b2 (PIN_OUTPUT | MUX_MODE4)		/* gpio 73 => cmt_rst_rq */
+			0x0b4 (PIN_OUTPUT | MUX_MODE4)		/* gpio 74 => cmt_en */
+			0x0b6 (PIN_OUTPUT | MUX_MODE4)		/* gpio 75 => cmt_rst */
+			0x15e (PIN_OUTPUT | MUX_MODE4)		/* gpio 157 => cmt_bsi */
+		>;
+	};
 };
 
 &i2c1 {
@@ -283,57 +354,57 @@
 };
 
 &twl_keypad {
-	linux,keymap = < 0x00000010 /* KEY_Q */
-			 0x00010018 /* KEY_O */
-			 0x00020019 /* KEY_P */
-			 0x00030033 /* KEY_COMMA */
-			 0x0004000e /* KEY_BACKSPACE */
-			 0x0006001e /* KEY_A */
-			 0x0007001f /* KEY_S */
-
-			 0x01000011 /* KEY_W */
-			 0x01010020 /* KEY_D */
-			 0x01020021 /* KEY_F */
-			 0x01030022 /* KEY_G */
-			 0x01040023 /* KEY_H */
-			 0x01050024 /* KEY_J */
-			 0x01060025 /* KEY_K */
-			 0x01070026 /* KEY_L */
-
-			 0x02000012 /* KEY_E */
-			 0x02010034 /* KEY_DOT */
-			 0x02020067 /* KEY_UP */
-			 0x0203001c /* KEY_ENTER */
-			 0x0205002c /* KEY_Z */
-			 0x0206002d /* KEY_X */
-			 0x0207002e /* KEY_C */
-			 0x02080043 /* KEY_F9 */
-
-			 0x03000013 /* KEY_R */
-			 0x0301002f /* KEY_V */
-			 0x03020030 /* KEY_B */
-			 0x03030031 /* KEY_N */
-			 0x03040032 /* KEY_M */
-			 0x03050039 /* KEY_SPACE */
-			 0x03060039 /* KEY_SPACE */
-			 0x03070069 /* KEY_LEFT */
-
-			 0x04000014 /* KEY_T */
-			 0x0401006c /* KEY_DOWN */
-			 0x0402006a /* KEY_RIGHT */
-			 0x0404001d /* KEY_LEFTCTRL */
-			 0x04050064 /* KEY_RIGHTALT */
-			 0x0406002a /* KEY_LEFTSHIFT */
-			 0x04080044 /* KEY_F10 */
-
-			 0x05000015 /* KEY_Y */
-			 0x05080057 /* KEY_F11 */
-
-			 0x06000016 /* KEY_U */
-
-			 0x07000017 /* KEY_I */
-			 0x07010041 /* KEY_F7 */
-			 0x07020042 /* KEY_F8 */
+	linux,keymap = < MATRIX_KEY(0x00, 0x00, KEY_Q)
+			 MATRIX_KEY(0x00, 0x01, KEY_O)
+			 MATRIX_KEY(0x00, 0x02, KEY_P)
+			 MATRIX_KEY(0x00, 0x03, KEY_COMMA)
+			 MATRIX_KEY(0x00, 0x04, KEY_BACKSPACE)
+			 MATRIX_KEY(0x00, 0x06, KEY_A)
+			 MATRIX_KEY(0x00, 0x07, KEY_S)
+
+			 MATRIX_KEY(0x01, 0x00, KEY_W)
+			 MATRIX_KEY(0x01, 0x01, KEY_D)
+			 MATRIX_KEY(0x01, 0x02, KEY_F)
+			 MATRIX_KEY(0x01, 0x03, KEY_G)
+			 MATRIX_KEY(0x01, 0x04, KEY_H)
+			 MATRIX_KEY(0x01, 0x05, KEY_J)
+			 MATRIX_KEY(0x01, 0x06, KEY_K)
+			 MATRIX_KEY(0x01, 0x07, KEY_L)
+
+			 MATRIX_KEY(0x02, 0x00, KEY_E)
+			 MATRIX_KEY(0x02, 0x01, KEY_DOT)
+			 MATRIX_KEY(0x02, 0x02, KEY_UP)
+			 MATRIX_KEY(0x02, 0x03, KEY_ENTER)
+			 MATRIX_KEY(0x02, 0x05, KEY_Z)
+			 MATRIX_KEY(0x02, 0x06, KEY_X)
+			 MATRIX_KEY(0x02, 0x07, KEY_C)
+			 MATRIX_KEY(0x02, 0x08, KEY_F9)
+
+			 MATRIX_KEY(0x03, 0x00, KEY_R)
+			 MATRIX_KEY(0x03, 0x01, KEY_V)
+			 MATRIX_KEY(0x03, 0x02, KEY_B)
+			 MATRIX_KEY(0x03, 0x03, KEY_N)
+			 MATRIX_KEY(0x03, 0x04, KEY_M)
+			 MATRIX_KEY(0x03, 0x05, KEY_SPACE)
+			 MATRIX_KEY(0x03, 0x06, KEY_SPACE)
+			 MATRIX_KEY(0x03, 0x07, KEY_LEFT)
+
+			 MATRIX_KEY(0x04, 0x00, KEY_T)
+			 MATRIX_KEY(0x04, 0x01, KEY_DOWN)
+			 MATRIX_KEY(0x04, 0x02, KEY_RIGHT)
+			 MATRIX_KEY(0x04, 0x04, KEY_LEFTCTRL)
+			 MATRIX_KEY(0x04, 0x05, KEY_RIGHTALT)
+			 MATRIX_KEY(0x04, 0x06, KEY_LEFTSHIFT)
+			 MATRIX_KEY(0x04, 0x08, KEY_F10)
+
+			 MATRIX_KEY(0x05, 0x00, KEY_Y)
+			 MATRIX_KEY(0x05, 0x08, KEY_F11)
+
+			 MATRIX_KEY(0x06, 0x00, KEY_U)
+
+			 MATRIX_KEY(0x07, 0x00, KEY_I)
+			 MATRIX_KEY(0x07, 0x01, KEY_F7)
+			 MATRIX_KEY(0x07, 0x02, KEY_F8)
 			 >;
 };
 
@@ -604,6 +675,30 @@
 	};
 };
 
+&mcspi4 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&mcspi4_pins>;
+
+	wl1251@0 {
+		pinctrl-names = "default";
+		pinctrl-0 = <&wl1251_pins>;
+
+		vio-supply = <&vio>;
+
+		compatible = "ti,wl1251";
+		reg = <0>;
+		spi-max-frequency = <48000000>;
+
+		spi-cpol;
+		spi-cpha;
+
+		ti,power-gpio = <&gpio3 23 GPIO_ACTIVE_HIGH>; /* 87 */
+
+		interrupt-parent = <&gpio2>;
+		interrupts = <10 IRQ_TYPE_NONE>; /* gpio line 42 */
+	};
+};
+
 &usb_otg_hs {
 	interface-type = <0>;
 	usb-phy = <&usb2_phy>;
@@ -618,11 +713,13 @@
 };
 
 &uart2 {
+	interrupts-extended = <&intc 73 &omap3_pmx_core OMAP3_UART2_RX>;
 	pinctrl-names = "default";
 	pinctrl-0 = <&uart2_pins>;
 };
 
 &uart3 {
+	interrupts-extended = <&intc 74 &omap3_pmx_core OMAP3_UART3_RX>;
 	pinctrl-names = "default";
 	pinctrl-0 = <&uart3_pins>;
 };
@@ -662,3 +759,48 @@
 		};
 	};
 };
+
+&mcbsp2 {
+	status = "ok";
+};
+
+&ssi_port1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&ssi_pins>;
+
+	ti,ssi-cawake-gpio = <&gpio5 23 GPIO_ACTIVE_HIGH>; /* 151 */
+
+	modem: hsi-client {
+		compatible = "nokia,n900-modem";
+
+		pinctrl-names = "default";
+		pinctrl-0 = <&modem_pins>;
+
+		hsi-channel-ids = <0>, <1>, <2>, <3>;
+		hsi-channel-names = "mcsaab-control",
+				    "speech-control",
+				    "speech-data",
+				    "mcsaab-data";
+		hsi-speed-kbps = <55000>;
+		hsi-mode = "frame";
+		hsi-flow = "synchronized";
+		hsi-arb-mode = "round-robin";
+
+		interrupts-extended = <&gpio3 8 IRQ_TYPE_EDGE_FALLING>; /* 72 */
+
+		gpios = <&gpio3  6 GPIO_ACTIVE_HIGH>, /* 70 */
+			<&gpio3  9 GPIO_ACTIVE_HIGH>, /* 73 */
+			<&gpio3 10 GPIO_ACTIVE_HIGH>, /* 74 */
+			<&gpio3 11 GPIO_ACTIVE_HIGH>, /* 75 */
+			<&gpio5 29 GPIO_ACTIVE_HIGH>; /* 157 */
+		gpio-names = "cmt_apeslpx",
+			     "cmt_rst_rq",
+			     "cmt_en",
+			     "cmt_rst",
+			     "cmt_bsi";
+	};
+};
+
+&ssi_port2 {
+	status = "disabled";
+};
diff --git a/arch/arm/boot/dts/omap3-n950-n9.dtsi b/arch/arm/boot/dts/omap3-n950-n9.dtsi
index 5c26c184f2c..70addcba37c 100644
--- a/arch/arm/boot/dts/omap3-n950-n9.dtsi
+++ b/arch/arm/boot/dts/omap3-n950-n9.dtsi
@@ -67,6 +67,20 @@
 	ti,pulldowns	= <0x008106>; /* BIT(1) | BIT(2) | BIT(8) | BIT(15) */
 };
 
+/* CSI-2 receiver */
+&vaux2 {
+	regulator-name = "vaux2";
+	regulator-min-microvolt = <1800000>;
+	regulator-max-microvolt = <1800000>;
+};
+
+/* Cameras */
+&vaux3 {
+	regulator-name = "vaux3";
+	regulator-min-microvolt = <2800000>;
+	regulator-max-microvolt = <2800000>;
+};
+
 &i2c2 {
 	clock-frequency = <400000>;
 };
diff --git a/arch/arm/boot/dts/omap3-sb-t35.dtsi b/arch/arm/boot/dts/omap3-sb-t35.dtsi
index 7909c51b05a..d59e3de1441 100644
--- a/arch/arm/boot/dts/omap3-sb-t35.dtsi
+++ b/arch/arm/boot/dts/omap3-sb-t35.dtsi
@@ -2,20 +2,6 @@
  * Common support for CompuLab SB-T35 used on SBC-T3530, SBC-T3517 and SBC-T3730
  */
 
-/ {
-	vddvario_sb_t35: regulator-vddvario-sb-t35 {
-		compatible = "regulator-fixed";
-		regulator-name = "vddvario";
-		regulator-always-on;
-	};
-
-	vdd33a_sb_t35: regulator-vdd33a-sb-t35 {
-		compatible = "regulator-fixed";
-		regulator-name = "vdd33a";
-		regulator-always-on;
-	};
-};
-
 &omap3_pmx_core {
 	smsc2_pins: pinmux_smsc2_pins {
 		pinctrl-single,pins = <
@@ -37,11 +23,10 @@
 		reg = <4 0 0xff>;
 		bank-width = <2>;
 		gpmc,mux-add-data;
-		gpmc,cs-on-ns = <0>;
-		gpmc,cs-rd-off-ns = <186>;
-		gpmc,cs-wr-off-ns = <186>;
-		gpmc,adv-on-ns = <12>;
-		gpmc,adv-rd-off-ns = <48>;
+		gpmc,cs-on-ns = <1>;
+		gpmc,cs-rd-off-ns = <180>;
+		gpmc,cs-wr-off-ns = <180>;
+		gpmc,adv-rd-off-ns = <18>;
 		gpmc,adv-wr-off-ns = <48>;
 		gpmc,oe-on-ns = <54>;
 		gpmc,oe-off-ns = <168>;
@@ -49,16 +34,14 @@
 		gpmc,we-off-ns = <168>;
 		gpmc,rd-cycle-ns = <186>;
 		gpmc,wr-cycle-ns = <186>;
-		gpmc,access-ns = <114>;
-		gpmc,page-burst-access-ns = <6>;
-		gpmc,bus-turnaround-ns = <12>;
-		gpmc,cycle2cycle-delay-ns = <18>;
-		gpmc,wr-data-mux-bus-ns = <90>;
-		gpmc,wr-access-ns = <186>;
+		gpmc,access-ns = <144>;
+		gpmc,page-burst-access-ns = <24>;
+		gpmc,bus-turnaround-ns = <90>;
+		gpmc,cycle2cycle-delay-ns = <90>;
 		gpmc,cycle2cycle-samecsen;
 		gpmc,cycle2cycle-diffcsen;
-		vddvario-supply = <&vddvario_sb_t35>;
-		vdd33a-supply = <&vdd33a_sb_t35>;
+		vddvario-supply = <&vddvario>;
+		vdd33a-supply = <&vdd33a>;
 		reg-io-width = <4>;
 		smsc,save-mac-address;
 	};
diff --git a/arch/arm/boot/dts/omap3-sbc-t3517.dts b/arch/arm/boot/dts/omap3-sbc-t3517.dts
index 024c9c6c682..42189b65d39 100644
--- a/arch/arm/boot/dts/omap3-sbc-t3517.dts
+++ b/arch/arm/boot/dts/omap3-sbc-t3517.dts
@@ -8,6 +8,19 @@
 / {
 	model = "CompuLab SBC-T3517 with CM-T3517";
 	compatible = "compulab,omap3-sbc-t3517", "compulab,omap3-cm-t3517", "ti,am3517", "ti,omap3";
+
+	/* Only one GPMC smsc9220 on SBC-T3517, CM-T3517 uses am35x Ethernet */
+	vddvario: regulator-vddvario-sb-t35 {
+		compatible = "regulator-fixed";
+		regulator-name = "vddvario";
+		regulator-always-on;
+	};
+
+	vdd33a: regulator-vdd33a-sb-t35 {
+		compatible = "regulator-fixed";
+		regulator-name = "vdd33a";
+		regulator-always-on;
+	};
 };
 
 &omap3_pmx_core {
diff --git a/arch/arm/boot/dts/omap3.dtsi b/arch/arm/boot/dts/omap3.dtsi
index 5e5790f631e..b2891a9a697 100644
--- a/arch/arm/boot/dts/omap3.dtsi
+++ b/arch/arm/boot/dts/omap3.dtsi
@@ -61,7 +61,7 @@
 			ti,hwmods = "mpu";
 		};
 
-		iva {
+		iva: iva {
 			compatible = "ti,iva2.2";
 			ti,hwmods = "iva";
 
@@ -74,7 +74,7 @@
 	/*
 	 * XXX: Use a flat representation of the OMAP3 interconnect.
 	 * The real OMAP interconnect network is quite complex.
-	 * Since that will not bring real advantage to represent that in DT for
+	 * Since it will not bring real advantage to represent that in DT for
 	 * the moment, just use a fake OCP bus entry to represent the whole bus
 	 * hierarchy.
 	 */
@@ -267,7 +267,7 @@
 		uart1: serial@4806a000 {
 			compatible = "ti,omap3-uart";
 			reg = <0x4806a000 0x2000>;
-			interrupts = <72>;
+			interrupts-extended = <&intc 72>;
 			dmas = <&sdma 49 &sdma 50>;
 			dma-names = "tx", "rx";
 			ti,hwmods = "uart1";
@@ -277,7 +277,7 @@
 		uart2: serial@4806c000 {
 			compatible = "ti,omap3-uart";
 			reg = <0x4806c000 0x400>;
-			interrupts = <73>;
+			interrupts-extended = <&intc 73>;
 			dmas = <&sdma 51 &sdma 52>;
 			dma-names = "tx", "rx";
 			ti,hwmods = "uart2";
@@ -287,7 +287,7 @@
 		uart3: serial@49020000 {
 			compatible = "ti,omap3-uart";
 			reg = <0x49020000 0x400>;
-			interrupts = <74>;
+			interrupts-extended = <&intc 74>;
 			dmas = <&sdma 53 &sdma 54>;
 			dma-names = "tx", "rx";
 			ti,hwmods = "uart3";
@@ -757,6 +757,51 @@
 				clock-names = "fck";
 			};
 		};
+
+		ssi: ssi-controller@48058000 {
+			compatible = "ti,omap3-ssi";
+			ti,hwmods = "ssi";
+
+			status = "disabled";
+
+			reg = <0x48058000 0x1000>,
+			      <0x48059000 0x1000>;
+			reg-names = "sys",
+				    "gdd";
+
+			interrupts = <71>;
+			interrupt-names = "gdd_mpu";
+
+			#address-cells = <1>;
+			#size-cells = <1>;
+			ranges;
+
+			ssi_port1: ssi-port@4805a000 {
+				compatible = "ti,omap3-ssi-port";
+
+				reg = <0x4805a000 0x800>,
+				      <0x4805a800 0x800>;
+				reg-names = "tx",
+					    "rx";
+
+				interrupt-parent = <&intc>;
+				interrupts = <67>,
+					     <68>;
+			};
+
+			ssi_port2: ssi-port@4805b000 {
+				compatible = "ti,omap3-ssi-port";
+
+				reg = <0x4805b000 0x800>,
+				      <0x4805b800 0x800>;
+				reg-names = "tx",
+					    "rx";
+
+				interrupt-parent = <&intc>;
+				interrupts = <69>,
+					     <70>;
+			};
+		};
 	};
 };
 
diff --git a/arch/arm/boot/dts/omap34xx.dtsi b/arch/arm/boot/dts/omap34xx.dtsi
index 2e92360da1f..3819c1e9159 100644
--- a/arch/arm/boot/dts/omap34xx.dtsi
+++ b/arch/arm/boot/dts/omap34xx.dtsi
@@ -40,6 +40,17 @@
 	};
 };
 
+&ssi {
+	status = "ok";
+
+	clocks = <&ssi_ssr_fck>,
+		 <&ssi_sst_fck>,
+		 <&ssi_ick>;
+	clock-names = "ssi_ssr_fck",
+		      "ssi_sst_fck",
+		      "ssi_ick";
+};
+
 /include/ "omap34xx-omap36xx-clocks.dtsi"
 /include/ "omap36xx-omap3430es2plus-clocks.dtsi"
 /include/ "omap36xx-am35xx-omap3430es2plus-clocks.dtsi"
diff --git a/arch/arm/boot/dts/omap36xx-clocks.dtsi b/arch/arm/boot/dts/omap36xx-clocks.dtsi
index 6b5280d04a0..200ae3a5cbb 100644
--- a/arch/arm/boot/dts/omap36xx-clocks.dtsi
+++ b/arch/arm/boot/dts/omap36xx-clocks.dtsi
@@ -83,7 +83,7 @@
 };
 
 &dpll4_m5x2_mul_ck {
-	clock-mult = <1>;
+	ti,clock-mult = <1>;
 };
 
 &dpll4_m6x2_mul_ck {
diff --git a/arch/arm/boot/dts/omap36xx.dtsi b/arch/arm/boot/dts/omap36xx.dtsi
index 22cf4647087..541704a59a5 100644
--- a/arch/arm/boot/dts/omap36xx.dtsi
+++ b/arch/arm/boot/dts/omap36xx.dtsi
@@ -78,6 +78,17 @@
 	clock-names = "fck", "tv_dac_clk";
 };
 
+&ssi {
+	status = "ok";
+
+	clocks = <&ssi_ssr_fck>,
+		 <&ssi_sst_fck>,
+		 <&ssi_ick>;
+	clock-names = "ssi_ssr_fck",
+		      "ssi_sst_fck",
+		      "ssi_ick";
+};
+
 /include/ "omap34xx-omap36xx-clocks.dtsi"
 /include/ "omap36xx-omap3430es2plus-clocks.dtsi"
 /include/ "omap36xx-am35xx-omap3430es2plus-clocks.dtsi"
diff --git a/arch/arm/boot/dts/omap3xxx-clocks.dtsi b/arch/arm/boot/dts/omap3xxx-clocks.dtsi
index 12be2b35dae..e47ff69dcf7 100644
--- a/arch/arm/boot/dts/omap3xxx-clocks.dtsi
+++ b/arch/arm/boot/dts/omap3xxx-clocks.dtsi
@@ -453,10 +453,11 @@
 
 	dpll4_m5x2_mul_ck: dpll4_m5x2_mul_ck {
 		#clock-cells = <0>;
-		compatible = "fixed-factor-clock";
+		compatible = "ti,fixed-factor-clock";
 		clocks = <&dpll4_m5_ck>;
-		clock-mult = <2>;
-		clock-div = <1>;
+		ti,clock-mult = <2>;
+		ti,clock-div = <1>;
+		ti,set-rate-parent;
 	};
 
 	dpll4_m5x2_ck: dpll4_m5x2_ck {
diff --git a/arch/arm/boot/dts/omap4-duovero-parlor.dts b/arch/arm/boot/dts/omap4-duovero-parlor.dts
index 96f51d87081..cd53a64d8f2 100644
--- a/arch/arm/boot/dts/omap4-duovero-parlor.dts
+++ b/arch/arm/boot/dts/omap4-duovero-parlor.dts
@@ -46,35 +46,35 @@
 
 	led_pins: pinmux_led_pins {
 		pinctrl-single,pins = <
-			0xd6 (PIN_OUTPUT | MUX_MODE3)		/* abe_dmic_din3.gpio_122 */
+			OMAP4_IOPAD(0x116, PIN_OUTPUT | MUX_MODE3)		/* abe_dmic_din3.gpio_122 */
 		>;
 	};
 
 	button_pins: pinmux_button_pins {
 		pinctrl-single,pins = <
-			0xd4 (PIN_INPUT_PULLUP | MUX_MODE3)	/* abe_dmic_din2.gpio_121 */
+			OMAP4_IOPAD(0x114, PIN_INPUT_PULLUP | MUX_MODE3)	/* abe_dmic_din2.gpio_121 */
 		>;
 	};
 
 	i2c2_pins: pinmux_i2c2_pins {
 		pinctrl-single,pins = <
-			0xe6 (PIN_INPUT_PULLUP | MUX_MODE0)	/* i2c2_scl */
-			0xe8 (PIN_INPUT_PULLUP | MUX_MODE0)	/* i2c2_sda */
+			OMAP4_IOPAD(0x126, PIN_INPUT_PULLUP | MUX_MODE0)	/* i2c2_scl */
+			OMAP4_IOPAD(0x128, PIN_INPUT_PULLUP | MUX_MODE0)	/* i2c2_sda */
 		>;
 	};
 
 	i2c3_pins: pinmux_i2c3_pins {
 		pinctrl-single,pins = <
-			0xea (PIN_INPUT_PULLUP | MUX_MODE0)	/* i2c3_scl */
-			0xec (PIN_INPUT_PULLUP | MUX_MODE0)	/* i2c3_sda */
+			OMAP4_IOPAD(0x12a, PIN_INPUT_PULLUP | MUX_MODE0)	/* i2c3_scl */
+			OMAP4_IOPAD(0x12c, PIN_INPUT_PULLUP | MUX_MODE0)	/* i2c3_sda */
 		>;
 	};
 
 	smsc_pins: pinmux_smsc_pins {
 		pinctrl-single,pins = <
-			0x28 (PIN_INPUT | MUX_MODE3)		/* gpmc_a20.gpio_44: IRQ */
-			0x2a (PIN_INPUT_PULLUP | MUX_MODE3)	/* gpmc_a21.gpio_45: nReset */
-			0x30 (PIN_INPUT_PULLUP | MUX_MODE3)	/* gpmc_a24.gpio_48: amdix enabled */
+			OMAP4_IOPAD(0x068, PIN_INPUT | MUX_MODE3)		/* gpmc_a20.gpio_44: IRQ */
+			OMAP4_IOPAD(0x06a, PIN_INPUT_PULLUP | MUX_MODE3)	/* gpmc_a21.gpio_45: nReset */
+			OMAP4_IOPAD(0x070, PIN_INPUT_PULLUP | MUX_MODE3)	/* gpmc_a24.gpio_48: amdix enabled */
 		>;
 	};
 };
diff --git a/arch/arm/boot/dts/omap4-duovero.dtsi b/arch/arm/boot/dts/omap4-duovero.dtsi
index a514791154e..e860ccd9d09 100644
--- a/arch/arm/boot/dts/omap4-duovero.dtsi
+++ b/arch/arm/boot/dts/omap4-duovero.dtsi
@@ -67,100 +67,98 @@
 	pinctrl-names = "default";
 	pinctrl-0 = <
 			&twl6040_pins
-			&mcpdm_pins
-			&mcbsp1_pins
 			&hsusbb1_pins
 	>;
 
 	twl6040_pins: pinmux_twl6040_pins {
 		pinctrl-single,pins = <
-			0x126 (PIN_OUTPUT | MUX_MODE3)		/* usbb2_ulpitll_nxt.gpio_160 */
-			0x160 (PIN_INPUT | MUX_MODE0)		/* sys_nirq2.sys_nirq2 */
+			OMAP4_IOPAD(0x166, PIN_OUTPUT | MUX_MODE3)		/* usbb2_ulpitll_nxt.gpio_160 */
+			OMAP4_IOPAD(0x1a0, PIN_INPUT | MUX_MODE0)		/* sys_nirq2.sys_nirq2 */
 		>;
 	};
 
 	mcpdm_pins: pinmux_mcpdm_pins {
 		pinctrl-single,pins = <
-			0xc6 (PIN_INPUT_PULLDOWN | MUX_MODE0)	/* abe_pdm_ul_data.abe_pdm_ul_data */
-			0xc8 (PIN_INPUT_PULLDOWN | MUX_MODE0)	/* abe_pdm_dl_data.abe_pdm_dl_data */
-			0xca (PIN_INPUT_PULLUP   | MUX_MODE0)	/* abe_pdm_frame.abe_pdm_frame */
-			0xcc (PIN_INPUT_PULLDOWN | MUX_MODE0)	/* abe_pdm_lb_clk.abe_pdm_lb_clk */
-			0xce (PIN_INPUT_PULLDOWN | MUX_MODE0)	/* abe_clks.abe_clks */
+			OMAP4_IOPAD(0x106, PIN_INPUT_PULLDOWN | MUX_MODE0)	/* abe_pdm_ul_data.abe_pdm_ul_data */
+			OMAP4_IOPAD(0x108, PIN_INPUT_PULLDOWN | MUX_MODE0)	/* abe_pdm_dl_data.abe_pdm_dl_data */
+			OMAP4_IOPAD(0x10a, PIN_INPUT_PULLUP   | MUX_MODE0)	/* abe_pdm_frame.abe_pdm_frame */
+			OMAP4_IOPAD(0x10c, PIN_INPUT_PULLDOWN | MUX_MODE0)	/* abe_pdm_lb_clk.abe_pdm_lb_clk */
+			OMAP4_IOPAD(0x10e, PIN_INPUT_PULLDOWN | MUX_MODE0)	/* abe_clks.abe_clks */
 		>;
 	};
 
 	mcbsp1_pins: pinmux_mcbsp1_pins {
 		pinctrl-single,pins = <
-			0xbe (PIN_INPUT | MUX_MODE0)		/* abe_mcbsp1_clkx.abe_mcbsp1_clkx */
-			0xc0 (PIN_INPUT_PULLDOWN | MUX_MODE0)	/* abe_mcbsp1_dr.abe_mcbsp1_dr */
-			0xc2 (PIN_OUTPUT_PULLDOWN | MUX_MODE0)	/* abe_mcbsp1_dx.abe_mcbsp1_dx */
-			0xc4 (PIN_INPUT | MUX_MODE0)		/* abe_mcbsp1_fsx.abe_mcbsp1_fsx */
+			OMAP4_IOPAD(0x0fe, PIN_INPUT | MUX_MODE0)		/* abe_mcbsp1_clkx.abe_mcbsp1_clkx */
+			OMAP4_IOPAD(0x100, PIN_INPUT_PULLDOWN | MUX_MODE0)	/* abe_mcbsp1_dr.abe_mcbsp1_dr */
+			OMAP4_IOPAD(0x102, PIN_OUTPUT_PULLDOWN | MUX_MODE0)	/* abe_mcbsp1_dx.abe_mcbsp1_dx */
+			OMAP4_IOPAD(0x104, PIN_INPUT | MUX_MODE0)		/* abe_mcbsp1_fsx.abe_mcbsp1_fsx */
 		>;
 	};
 
 	hsusbb1_pins: pinmux_hsusbb1_pins {
 		pinctrl-single,pins = <
-			0x82 (PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_clk.usbb1_ulpiphy_clk */
-			0x84 (PIN_OUTPUT | MUX_MODE4)		/* usbb1_ulpitll_stp.usbb1_ulpiphy_stp */
-			0x86 (PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_dir.usbb1_ulpiphy_dir */
-			0x88 (PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_nxt.usbb1_ulpiphy_nxt */
-			0x8a (PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_dat0.usbb1_ulpiphy_dat0 */
-			0x8c (PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_dat1.usbb1_ulpiphy_dat1 */
-			0x8e (PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_dat2.usbb1_ulpiphy_dat2 */
-			0x90 (PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_dat3.usbb1_ulpiphy_dat3 */
-			0x92 (PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_dat4.usbb1_ulpiphy_dat4 */
-			0x94 (PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_dat5.usbb1_ulpiphy_dat5 */
-			0x96 (PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_dat6.usbb1_ulpiphy_dat6 */
-			0x98 (PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_dat7.usbb1_ulpiphy_dat7 */
+			OMAP4_IOPAD(0x0c2, PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_clk.usbb1_ulpiphy_clk */
+			OMAP4_IOPAD(0x0c4, PIN_OUTPUT | MUX_MODE4)		/* usbb1_ulpitll_stp.usbb1_ulpiphy_stp */
+			OMAP4_IOPAD(0x0c6, PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_dir.usbb1_ulpiphy_dir */
+			OMAP4_IOPAD(0x0c8, PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_nxt.usbb1_ulpiphy_nxt */
+			OMAP4_IOPAD(0x0ca, PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_dat0.usbb1_ulpiphy_dat0 */
+			OMAP4_IOPAD(0x0cc, PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_dat1.usbb1_ulpiphy_dat1 */
+			OMAP4_IOPAD(0x0ce, PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_dat2.usbb1_ulpiphy_dat2 */
+			OMAP4_IOPAD(0x0d0, PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_dat3.usbb1_ulpiphy_dat3 */
+			OMAP4_IOPAD(0x0d2, PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_dat4.usbb1_ulpiphy_dat4 */
+			OMAP4_IOPAD(0x0d4, PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_dat5.usbb1_ulpiphy_dat5 */
+			OMAP4_IOPAD(0x0d6, PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_dat6.usbb1_ulpiphy_dat6 */
+			OMAP4_IOPAD(0x0d8, PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_dat7.usbb1_ulpiphy_dat7 */
 		>;
 	};
 
 	hsusb1phy_pins: pinmux_hsusb1phy_pins {
 		pinctrl-single,pins = <
-			0x4c (PIN_OUTPUT | MUX_MODE3)		/* gpmc_wait1.gpio_62 */
+			OMAP4_IOPAD(0x08c, PIN_OUTPUT | MUX_MODE3)		/* gpmc_wait1.gpio_62 */
 		>;
 	};
 
 	w2cbw0015_pins: pinmux_w2cbw0015_pins {
 		pinctrl-single,pins = <
-			0x26 (PIN_OUTPUT | MUX_MODE3)		/* gpmc_a19.gpio_43 */
-			0x3a (PIN_INPUT | MUX_MODE3)		/* gpmc_ncs3.gpio_53 */
+			OMAP4_IOPAD(0x066, PIN_OUTPUT | MUX_MODE3)		/* gpmc_a19.gpio_43 */
+			OMAP4_IOPAD(0x07a, PIN_INPUT | MUX_MODE3)		/* gpmc_ncs3.gpio_53 */
 		>;
 	};
 
 	i2c1_pins: pinmux_i2c1_pins {
 		pinctrl-single,pins = <
-			0xe2 (PIN_INPUT_PULLUP | MUX_MODE0)	/* i2c1_scl */
-			0xe4 (PIN_INPUT_PULLUP | MUX_MODE0)	/* i2c1_sda */
+			OMAP4_IOPAD(0x122, PIN_INPUT_PULLUP | MUX_MODE0)	/* i2c1_scl */
+			OMAP4_IOPAD(0x124, PIN_INPUT_PULLUP | MUX_MODE0)	/* i2c1_sda */
 		>;
 	};
 
 	i2c4_pins: pinmux_i2c4_pins {
 		pinctrl-single,pins = <
-			0xee (PIN_INPUT_PULLUP | MUX_MODE0)	/* i2c4_scl */
-			0xf0 (PIN_INPUT_PULLUP | MUX_MODE0)	/* i2c4_sda */
+			OMAP4_IOPAD(0x12e, PIN_INPUT_PULLUP | MUX_MODE0)	/* i2c4_scl */
+			OMAP4_IOPAD(0x130, PIN_INPUT_PULLUP | MUX_MODE0)	/* i2c4_sda */
 		>;
 	};
 
 	mmc1_pins: pinmux_mmc1_pins {
 		pinctrl-single,pins = <
-			0xa2 (PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmmc1_clk */
-			0xa4 (PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmcc1_cmd */
-			0xa6 (PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmcc1_dat0 */
-			0xa8 (PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmmc1_dat1 */
-			0xaa (PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmmc1_dat2 */
-			0xac (PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmmc1_dat3 */
+			OMAP4_IOPAD(0x0e2, PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmmc1_clk */
+			OMAP4_IOPAD(0x0e4, PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmcc1_cmd */
+			OMAP4_IOPAD(0x0e6, PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmcc1_dat0 */
+			OMAP4_IOPAD(0x0e8, PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmmc1_dat1 */
+			OMAP4_IOPAD(0x0ea, PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmmc1_dat2 */
+			OMAP4_IOPAD(0x0ec, PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmmc1_dat3 */
 		>;
 	};
 
 	mmc5_pins: pinmux_mmc5_pins {
 		pinctrl-single,pins = <
-			0x108 (PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmmc5_clk */
-			0x10a (PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmcc5_cmd */
-			0x10c (PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmcc5_dat0 */
-			0x10e (PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmmc5_dat1 */
-			0x110 (PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmmc5_dat2 */
-			0x112 (PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmmc5_dat3 */
+			OMAP4_IOPAD(0x148, PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmmc5_clk */
+			OMAP4_IOPAD(0x14a, PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmcc5_cmd */
+			OMAP4_IOPAD(0x14c, PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmcc5_dat0 */
+			OMAP4_IOPAD(0x14e, PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmmc5_dat1 */
+			OMAP4_IOPAD(0x150, PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmmc5_dat2 */
+			OMAP4_IOPAD(0x152, PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmmc5_dat3 */
 		>;
 	};
 };
@@ -202,6 +200,18 @@
 	clock-frequency = <400000>;
 };
 
+&mcbsp1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&mcbsp1_pins>;
+	status = "okay";
+};
+
+&mcpdm {
+	pinctrl-names = "default";
+	pinctrl-0 = <&mcpdm_pins>;
+	status = "okay";
+};
+
 &mmc1 {
 	pinctrl-names = "default";
 	pinctrl-0 = <&mmc1_pins>;
diff --git a/arch/arm/boot/dts/omap4-panda-common.dtsi b/arch/arm/boot/dts/omap4-panda-common.dtsi
index d2c45bfaaa2..8cfa3c8a72b 100644
--- a/arch/arm/boot/dts/omap4-panda-common.dtsi
+++ b/arch/arm/boot/dts/omap4-panda-common.dtsi
@@ -481,6 +481,21 @@
 	usb-supply = <&vusb>;
 };
 
+&uart2 {
+	interrupts-extended = <&gic GIC_SPI 73 IRQ_TYPE_LEVEL_HIGH
+			       &omap4_pmx_core OMAP4_UART2_RX>;
+};
+
+&uart3 {
+	interrupts-extended = <&gic GIC_SPI 74 IRQ_TYPE_LEVEL_HIGH
+			       &omap4_pmx_core OMAP4_UART3_RX>;
+};
+
+&uart4 {
+	interrupts-extended = <&gic GIC_SPI 70 IRQ_TYPE_LEVEL_HIGH
+			       &omap4_pmx_core OMAP4_UART4_RX>;
+};
+
 &usb_otg_hs {
 	interface-type = <1>;
 	mode = <3>;
diff --git a/arch/arm/boot/dts/omap4-sdp.dts b/arch/arm/boot/dts/omap4-sdp.dts
index 48983c8d56c..3e1da43068f 100644
--- a/arch/arm/boot/dts/omap4-sdp.dts
+++ b/arch/arm/boot/dts/omap4-sdp.dts
@@ -570,16 +570,22 @@
 };
 
 &uart2 {
+	interrupts-extended = <&gic GIC_SPI 73 IRQ_TYPE_LEVEL_HIGH
+			       &omap4_pmx_core OMAP4_UART2_RX>;
 	pinctrl-names = "default";
 	pinctrl-0 = <&uart2_pins>;
 };
 
 &uart3 {
+	interrupts-extended = <&gic GIC_SPI 74 IRQ_TYPE_LEVEL_HIGH
+			       &omap4_pmx_core OMAP4_UART3_RX>;
 	pinctrl-names = "default";
 	pinctrl-0 = <&uart3_pins>;
 };
 
 &uart4 {
+	interrupts-extended = <&gic GIC_SPI 70 IRQ_TYPE_LEVEL_HIGH
+			       &omap4_pmx_core OMAP4_UART4_RX>;
 	pinctrl-names = "default";
 	pinctrl-0 = <&uart4_pins>;
 };
diff --git a/arch/arm/boot/dts/omap4-var-dvk-om44.dts b/arch/arm/boot/dts/omap4-var-dvk-om44.dts
new file mode 100644
index 00000000000..458d79fa378
--- /dev/null
+++ b/arch/arm/boot/dts/omap4-var-dvk-om44.dts
@@ -0,0 +1,71 @@
+/*
+ * Copyright (C) 2014 Joachim Eastwood <manabian@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+/dts-v1/;
+
+#include "omap4-var-som-om44.dtsi"
+#include "omap4-var-som-om44-wlan.dtsi"
+#include "omap4-var-om44customboard.dtsi"
+
+/ {
+	model = "Variscite VAR-DVK-OM44";
+	compatible = "variscite,var-dvk-om44", "variscite,var-som-om44", "ti,omap4460", "ti,omap4";
+
+	aliases {
+		display0 = &lcd0;
+		display1 = &hdmi0;
+	};
+
+	lcd0: display {
+		compatible = "innolux,at070tn83", "panel-dpi";
+		label = "lcd";
+		panel-timing {
+			clock-frequency = <33333333>;
+
+			hback-porch = <40>;
+			hactive = <800>;
+			hfront-porch = <40>;
+			hsync-len = <48>;
+
+			vback-porch = <29>;
+			vactive = <480>;
+			vfront-porch = <13>;
+			vsync-len = <3>;
+		};
+
+		port {
+			lcd_in: endpoint {
+				remote-endpoint = <&dpi_out>;
+			};
+		};
+	};
+
+	backlight {
+		compatible = "gpio-backlight";
+		pinctrl-names = "default";
+		pinctrl-0 = <&backlight_pins>;
+
+		gpios = <&gpio4 26 GPIO_ACTIVE_HIGH>; /* gpio 122 */
+	};
+};
+
+&dss {
+	pinctrl-names = "default";
+	pinctrl-0 = <&dss_dpi_pins>;
+
+	port {
+		dpi_out: endpoint {
+			remote-endpoint = <&lcd_in>;
+			data-lines = <24>;
+		};
+	};
+};
+
+&dsi2 {
+	status = "okay";
+	vdd-supply = <&vcxio>;
+};
diff --git a/arch/arm/boot/dts/omap4-var-om44customboard.dtsi b/arch/arm/boot/dts/omap4-var-om44customboard.dtsi
new file mode 100644
index 00000000000..f2d2fdb7562
--- /dev/null
+++ b/arch/arm/boot/dts/omap4-var-om44customboard.dtsi
@@ -0,0 +1,235 @@
+/*
+ * Copyright (C) 2014 Joachim Eastwood <manabian@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <dt-bindings/input/input.h>
+
+/ {
+	aliases {
+		display0 = &hdmi0;
+	};
+
+	leds {
+		compatible = "gpio-leds";
+		pinctrl-names = "default";
+		pinctrl-0 = <&gpio_led_pins>;
+
+		led0 {
+			label = "var:green:led0";
+			gpios = <&gpio6 13 GPIO_ACTIVE_HIGH>; /* gpio 173 */
+			linux,default-trigger = "heartbeat";
+		};
+
+		led1 {
+			label = "var:green:led1";
+			gpios = <&gpio6 12 GPIO_ACTIVE_HIGH>; /* gpio 172 */
+		};
+	};
+
+	gpio-keys {
+		compatible = "gpio-keys";
+		pinctrl-names = "default";
+		pinctrl-0 = <&gpio_key_pins>;
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		user-key@184 {
+			label = "user";
+			gpios = <&gpio6 24 GPIO_ACTIVE_HIGH>; /* gpio 184 */
+			linux,code = <BTN_EXTRA>;
+			gpio-key,wakeup;
+		};
+	};
+
+	hdmi0: connector@0 {
+		compatible = "hdmi-connector";
+		pinctrl-names = "default";
+		pinctrl-0 = <&hdmi_hpd_pins>;
+		label = "hdmi";
+		type = "a";
+
+		hpd-gpios = <&gpio2 31 GPIO_ACTIVE_HIGH>; /* gpio_63 */
+
+		port {
+			hdmi_connector_in: endpoint {
+				remote-endpoint = <&hdmi_out>;
+			};
+		};
+	};
+};
+
+&omap4_pmx_core {
+	uart1_pins: pinmux_uart1_pins {
+		pinctrl-single,pins = <
+			OMAP4_IOPAD(0x13c, PIN_INPUT_PULLUP | MUX_MODE1)	/* mcspi1_cs2.uart1_cts */
+			OMAP4_IOPAD(0x13e, PIN_OUTPUT | MUX_MODE1)		/* mcspi1_cs3.uart1_rts */
+			OMAP4_IOPAD(0x126, PIN_INPUT_PULLUP | MUX_MODE1)	/* i2c2_scl.uart1_rx */
+			OMAP4_IOPAD(0x128, PIN_OUTPUT | MUX_MODE1)		/* i2c2_sda.uart1_tx */
+		>;
+	};
+
+	mcspi1_pins: pinmux_mcspi1_pins {
+		pinctrl-single,pins = <
+			OMAP4_IOPAD(0x132, PIN_INPUT | MUX_MODE0)		/*  mcspi1_clk.mcspi1_clk */
+			OMAP4_IOPAD(0x134, PIN_INPUT | MUX_MODE0)		/*  mcspi1_somi.mcspi1_somi */
+			OMAP4_IOPAD(0x136, PIN_INPUT | MUX_MODE0)		/*  mcspi1_simo.mcspi1_simo */
+			OMAP4_IOPAD(0x138, PIN_INPUT | MUX_MODE0)		/*  mcspi1_cs0.mcspi1_cs0 */
+		>;
+	};
+
+	mcasp_pins: pinmux_mcsasp_pins {
+		pinctrl-single,pins = <
+			OMAP4_IOPAD(0x0f8, PIN_OUTPUT | MUX_MODE2)		/*  mcbsp2_dr.abe_mcasp_axr */
+		>;
+	};
+
+	dss_dpi_pins: pinmux_dss_dpi_pins {
+		pinctrl-single,pins = <
+			OMAP4_IOPAD(0x162, PIN_OUTPUT | MUX_MODE5)		/* dispc2_data23 */
+			OMAP4_IOPAD(0x164, PIN_OUTPUT | MUX_MODE5)		/* dispc2_data22 */
+			OMAP4_IOPAD(0x166, PIN_OUTPUT | MUX_MODE5)		/* dispc2_data21 */
+			OMAP4_IOPAD(0x168, PIN_OUTPUT | MUX_MODE5)		/* dispc2_data20 */
+			OMAP4_IOPAD(0x16a, PIN_OUTPUT | MUX_MODE5)		/* dispc2_data19 */
+			OMAP4_IOPAD(0x16c, PIN_OUTPUT | MUX_MODE5)		/* dispc2_data18 */
+			OMAP4_IOPAD(0x16e, PIN_OUTPUT | MUX_MODE5)		/* dispc2_data15 */
+			OMAP4_IOPAD(0x170, PIN_OUTPUT | MUX_MODE5)		/* dispc2_data14 */
+			OMAP4_IOPAD(0x172, PIN_OUTPUT | MUX_MODE5)		/* dispc2_data13 */
+			OMAP4_IOPAD(0x174, PIN_OUTPUT | MUX_MODE5)		/* dispc2_data12 */
+			OMAP4_IOPAD(0x176, PIN_OUTPUT | MUX_MODE5)		/* dispc2_data11 */
+			OMAP4_IOPAD(0x1b4, PIN_OUTPUT | MUX_MODE5)		/* dispc2_data10 */
+			OMAP4_IOPAD(0x1b6, PIN_OUTPUT | MUX_MODE5)		/* dispc2_data9 */
+			OMAP4_IOPAD(0x1b8, PIN_OUTPUT | MUX_MODE5)		/* dispc2_data16 */
+			OMAP4_IOPAD(0x1ba, PIN_OUTPUT | MUX_MODE5)		/* dispc2_data17 */
+			OMAP4_IOPAD(0x1bc, PIN_OUTPUT | MUX_MODE5)		/* dispc2_hsync */
+			OMAP4_IOPAD(0x1be, PIN_OUTPUT | MUX_MODE5)		/* dispc2_pclk */
+			OMAP4_IOPAD(0x1c0, PIN_OUTPUT | MUX_MODE5)		/* dispc2_vsync */
+			OMAP4_IOPAD(0x1c2, PIN_OUTPUT | MUX_MODE5)		/* dispc2_de */
+			OMAP4_IOPAD(0x1c4, PIN_OUTPUT | MUX_MODE5)		/* dispc2_data8 */
+			OMAP4_IOPAD(0x1c6, PIN_OUTPUT | MUX_MODE5)		/* dispc2_data7 */
+			OMAP4_IOPAD(0x1c8, PIN_OUTPUT | MUX_MODE5)		/* dispc2_data6 */
+			OMAP4_IOPAD(0x1ca, PIN_OUTPUT | MUX_MODE5)		/* dispc2_data5 */
+			OMAP4_IOPAD(0x1cc, PIN_OUTPUT | MUX_MODE5)		/* dispc2_data4 */
+			OMAP4_IOPAD(0x1ce, PIN_OUTPUT | MUX_MODE5)		/* dispc2_data3 */
+			OMAP4_IOPAD(0x1d0, PIN_OUTPUT | MUX_MODE5)		/* dispc2_data2 */
+			OMAP4_IOPAD(0x1d2, PIN_OUTPUT | MUX_MODE5)		/* dispc2_data1 */
+			OMAP4_IOPAD(0x1d4, PIN_OUTPUT | MUX_MODE5)		/* dispc2_data0 */
+		>;
+	};
+
+	dss_hdmi_pins: pinmux_dss_hdmi_pins {
+		pinctrl-single,pins = <
+			OMAP4_IOPAD(0x09a, PIN_INPUT_PULLUP | MUX_MODE0)	/* hdmi_cec.hdmi_cec */
+			OMAP4_IOPAD(0x09c, PIN_INPUT_PULLUP | MUX_MODE0)	/* hdmi_scl.hdmi_scl */
+			OMAP4_IOPAD(0x09e, PIN_INPUT_PULLUP | MUX_MODE0)	/* hdmi_sda.hdmi_sda */
+		>;
+	};
+
+	i2c4_pins: pinmux_i2c4_pins {
+		pinctrl-single,pins = <
+			OMAP4_IOPAD(0x12e, PIN_INPUT_PULLUP | MUX_MODE0)	/* i2c4_scl */
+			OMAP4_IOPAD(0x130, PIN_INPUT_PULLUP | MUX_MODE0)	/* i2c4_sda */
+		>;
+	};
+
+	mmc5_pins: pinmux_mmc5_pins {
+		pinctrl-single,pins = <
+			OMAP4_IOPAD(0x0f6, PIN_INPUT | MUX_MODE3)		/* abe_mcbsp2_clkx.gpio_110 */
+			OMAP4_IOPAD(0x148, PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmmc5_clk.sdmmc5_clk */
+			OMAP4_IOPAD(0x14a, PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmmc5_cmd.sdmmc5_cmd */
+			OMAP4_IOPAD(0x14c, PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmmc5_dat0.sdmmc5_dat0 */
+			OMAP4_IOPAD(0x14e, PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmmc5_dat1.sdmmc5_dat1 */
+			OMAP4_IOPAD(0x150, PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmmc5_dat2.sdmmc5_dat2 */
+			OMAP4_IOPAD(0x152, PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmmc5_dat3.sdmmc5_dat3 */
+		>;
+	};
+
+	gpio_led_pins: pinmux_gpio_led_pins {
+		pinctrl-single,pins = <
+			OMAP4_IOPAD(0x17e, PIN_OUTPUT | MUX_MODE3)		/* kpd_col4.gpio_172 */
+			OMAP4_IOPAD(0x180, PIN_OUTPUT | MUX_MODE3)		/* kpd_col5.gpio_173 */
+		>;
+	};
+
+	gpio_key_pins: pinmux_gpio_key_pins {
+		pinctrl-single,pins = <
+			OMAP4_IOPAD(0x1a2, PIN_INPUT | MUX_MODE3)		/* sys_boot0.gpio_184 */
+		>;
+	};
+
+	ks8851_irq_pins: pinmux_ks8851_irq_pins {
+		pinctrl-single,pins = <
+			OMAP4_IOPAD(0x17c, PIN_INPUT_PULLUP | MUX_MODE3)	/* kpd_col3.gpio_171 */
+		>;
+	};
+
+	hdmi_hpd_pins: pinmux_hdmi_hpd_pins {
+		pinctrl-single,pins = <
+			OMAP4_IOPAD(0x098, PIN_INPUT_PULLDOWN | MUX_MODE3)	/* hdmi_hpd.gpio_63 */
+		>;
+	};
+
+	backlight_pins: pinmux_backlight_pins {
+		pinctrl-single,pins = <
+			OMAP4_IOPAD(0x116, PIN_OUTPUT | MUX_MODE3)		/* abe_dmic_din3.gpio_122 */
+		>;
+	};
+};
+
+&i2c4 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&i2c4_pins>;
+	clock-frequency = <400000>;
+	status = "okay";
+};
+
+&uart1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&uart1_pins>;
+	status = "okay";
+};
+
+&mcspi1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&mcspi1_pins>;
+	status = "okay";
+
+	eth@0 {
+		compatible = "ks8851";
+		pinctrl-names = "default";
+		pinctrl-0 = <&ks8851_irq_pins>;
+		spi-max-frequency = <24000000>;
+		reg = <0>;
+		interrupt-parent = <&gpio6>;
+		interrupts = <11 IRQ_TYPE_LEVEL_LOW>; /* gpio 171 */
+	};
+};
+
+&mmc5 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&mmc5_pins>;
+	vmmc-supply = <&vbat>;
+	bus-width = <4>;
+	cd-gpios = <&gpio4 14 GPIO_ACTIVE_HIGH>; /* gpio 110 */
+	status = "okay";
+};
+
+&dss {
+	status = "okay";
+};
+
+&hdmi {
+	status = "okay";
+	pinctrl-names = "default";
+	pinctrl-0 = <&dss_hdmi_pins>;
+	vdda-supply = <&vdac>;
+
+	port {
+		hdmi_out: endpoint {
+			remote-endpoint = <&hdmi_connector_in>;
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/omap4-var-som-om44-wlan.dtsi b/arch/arm/boot/dts/omap4-var-som-om44-wlan.dtsi
new file mode 100644
index 00000000000..cc66af41923
--- /dev/null
+++ b/arch/arm/boot/dts/omap4-var-som-om44-wlan.dtsi
@@ -0,0 +1,68 @@
+/*
+ * Copyright (C) 2014 Joachim Eastwood <manabian@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+/ {
+	/* regulator for wl12xx on sdio4 */
+	wl12xx_vmmc: wl12xx_vmmc {
+		pinctrl-names = "default";
+		pinctrl-0 = <&wl12xx_ctrl_pins>;
+		compatible = "regulator-fixed";
+		regulator-name = "vwl1271";
+		regulator-min-microvolt = <1800000>;
+		regulator-max-microvolt = <1800000>;
+		gpio = <&gpio2 11 0>;	/* gpio 43 */
+		startup-delay-us = <70000>;
+		enable-active-high;
+	};
+};
+
+&omap4_pmx_core {
+	uart2_pins: pinmux_uart2_pins {
+		pinctrl-single,pins = <
+			OMAP4_IOPAD(0x118, PIN_INPUT_PULLUP | MUX_MODE0)	/* uart2_cts.uart2_cts */
+			OMAP4_IOPAD(0x11a, PIN_OUTPUT | MUX_MODE0)		/* uart2_rts.uart2_rts */
+			OMAP4_IOPAD(0x11c, PIN_INPUT_PULLUP | MUX_MODE0)	/* uart2_rx.uart2_rx */
+			OMAP4_IOPAD(0x11e, PIN_OUTPUT | MUX_MODE0)		/* uart2_tx.uart2_tx */
+		>;
+	};
+
+	wl12xx_ctrl_pins: pinmux_wl12xx_ctrl_pins {
+		pinctrl-single,pins = <
+			OMAP4_IOPAD(0x062, PIN_INPUT_PULLUP | MUX_MODE3)	/* gpmc_a17.gpio_41 (WLAN_IRQ) */
+			OMAP4_IOPAD(0x064, PIN_OUTPUT | MUX_MODE3)		/* gpmc_a18.gpio_42 (BT_EN) */
+			OMAP4_IOPAD(0x066, PIN_OUTPUT | MUX_MODE3)		/* gpmc_a19.gpio_43 (WLAN_EN) */
+		>;
+	};
+
+	mmc4_pins: pinmux_mmc4_pins {
+		pinctrl-single,pins = <
+			OMAP4_IOPAD(0x154, PIN_INPUT_PULLUP | MUX_MODE1)	/* mcspi4_clk.sdmmc4_clk */
+			OMAP4_IOPAD(0x156, PIN_INPUT_PULLUP | MUX_MODE1)	/* mcspi4_simo.sdmmc4_cmd */
+			OMAP4_IOPAD(0x158, PIN_INPUT_PULLUP | MUX_MODE1)	/* mcspi4_somi.sdmmc4_dat0 */
+			OMAP4_IOPAD(0x15e, PIN_INPUT_PULLUP | MUX_MODE1)	/* uart4_tx.sdmmc4_dat1 */
+			OMAP4_IOPAD(0x15c, PIN_INPUT_PULLUP | MUX_MODE1)	/* uart4_rx.sdmmc4_dat2 */
+			OMAP4_IOPAD(0x15a, PIN_INPUT_PULLUP | MUX_MODE1)	/* mcspi4_cs0.sdmmc4_dat3 */
+		>;
+	};
+};
+
+&uart2 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&uart2_pins>;
+	status = "okay";
+};
+
+&mmc4 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&mmc4_pins>;
+	vmmc-supply = <&wl12xx_vmmc>;
+	non-removable;
+	bus-width = <4>;
+	cap-power-off-card;
+	status = "okay";
+};
diff --git a/arch/arm/boot/dts/omap4-var-som-om44.dtsi b/arch/arm/boot/dts/omap4-var-som-om44.dtsi
new file mode 100644
index 00000000000..062701e1a89
--- /dev/null
+++ b/arch/arm/boot/dts/omap4-var-som-om44.dtsi
@@ -0,0 +1,343 @@
+/*
+ * Copyright (C) 2014 Joachim Eastwood <manabian@gmail.com>
+ * Copyright (C) 2012 Variscite Ltd. - http://www.variscite.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 "omap4460.dtsi"
+
+/ {
+	model = "Variscite VAR-SOM-OM44";
+	compatible = "variscite,var-som-om44", "ti,omap4460", "ti,omap4";
+
+	memory {
+		device_type = "memory";
+		reg = <0x80000000 0x40000000>; /* 1 GB */
+	};
+
+	sound: sound@0 {
+		compatible = "ti,abe-twl6040";
+		ti,model = "VAR-SOM-OM44";
+
+		ti,mclk-freq = <38400000>;
+		ti,mcpdm = <&mcpdm>;
+		ti,twl6040 = <&twl6040>;
+
+		/* Audio routing */
+		ti,audio-routing =
+			"Headset Stereophone", "HSOL",
+			"Headset Stereophone", "HSOR",
+			"AFML", "Line In",
+			"AFMR", "Line In";
+	};
+
+	/* HS USB Host PHY on PORT 1 */
+	hsusb1_phy: hsusb1_phy {
+		compatible = "usb-nop-xceiv";
+		pinctrl-names = "default";
+		pinctrl-0 = <
+			&hsusbb1_phy_clk_pins
+			&hsusbb1_phy_rst_pins
+		>;
+
+		reset-gpios = <&gpio6 17 GPIO_ACTIVE_LOW>; /* gpio 177 */
+		vcc-supply = <&vbat>;
+
+		clocks = <&auxclk3_ck>;
+		clock-names = "main_clk";
+		clock-frequency = <19200000>;
+	};
+
+	vbat: fixedregulator-vbat {
+		compatible = "regulator-fixed";
+		regulator-name = "VBAT";
+		regulator-min-microvolt = <3300000>;
+		regulator-max-microvolt = <3300000>;
+		regulator-always-on;
+		regulator-boot-on;
+	};
+};
+
+&omap4_pmx_core {
+	pinctrl-names = "default";
+	pinctrl-0 = <
+			&hsusbb1_pins
+	>;
+
+	twl6040_pins: pinmux_twl6040_pins {
+		pinctrl-single,pins = <
+			OMAP4_IOPAD(0x19c, PIN_OUTPUT | MUX_MODE3)		/* fref_clk2_out.gpio_182 */
+			OMAP4_IOPAD(0x1a0, PIN_INPUT | MUX_MODE0)		/* sys_nirq2.sys_nirq2 */
+		>;
+	};
+
+	mcpdm_pins: pinmux_mcpdm_pins {
+		pinctrl-single,pins = <
+			OMAP4_IOPAD(0x106, PIN_INPUT_PULLDOWN | MUX_MODE0)	/* abe_pdm_ul_data.abe_pdm_ul_data */
+			OMAP4_IOPAD(0x108, PIN_INPUT_PULLDOWN | MUX_MODE0)	/* abe_pdm_dl_data.abe_pdm_dl_data */
+			OMAP4_IOPAD(0x10a, PIN_INPUT_PULLUP | MUX_MODE0)	/* abe_pdm_frame.abe_pdm_frame */
+			OMAP4_IOPAD(0x10c, PIN_INPUT_PULLDOWN | MUX_MODE0)	/* abe_pdm_lb_clk.abe_pdm_lb_clk */
+			OMAP4_IOPAD(0x10e, PIN_INPUT_PULLDOWN | MUX_MODE0)	/* abe_clks.abe_clks */
+		>;
+	};
+
+	tsc2004_pins: pinmux_tsc2004_pins {
+		pinctrl-single,pins = <
+			OMAP4_IOPAD(0x090, PIN_INPUT | MUX_MODE3)		/* gpmc_ncs4.gpio_101 (irq) */
+			OMAP4_IOPAD(0x092, PIN_OUTPUT | MUX_MODE3)		/* gpmc_ncs5.gpio_102 (rst) */
+		>;
+	};
+
+	uart3_pins: pinmux_uart3_pins {
+		pinctrl-single,pins = <
+			OMAP4_IOPAD(0x140, PIN_INPUT_PULLUP | MUX_MODE0)	/* uart3_cts_rctx.uart3_cts_rctx */
+			OMAP4_IOPAD(0x142, PIN_OUTPUT | MUX_MODE0)		/* uart3_rts_sd.uart3_rts_sd */
+			OMAP4_IOPAD(0x144, PIN_INPUT | MUX_MODE0)		/* uart3_rx_irrx.uart3_rx_irrx */
+			OMAP4_IOPAD(0x146, PIN_OUTPUT | MUX_MODE0)		/* uart3_tx_irtx.uart3_tx_irtx */
+		>;
+	};
+
+	hsusbb1_pins: pinmux_hsusbb1_pins {
+		pinctrl-single,pins = <
+			OMAP4_IOPAD(0x0c2, PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_clk.usbb1_ulpiphy_clk */
+			OMAP4_IOPAD(0x0c4, PIN_OUTPUT | MUX_MODE4)		/* usbb1_ulpitll_stp.usbb1_ulpiphy_stp */
+			OMAP4_IOPAD(0x0c6, PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_dir.usbb1_ulpiphy_dir */
+			OMAP4_IOPAD(0x0c8, PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_nxt.usbb1_ulpiphy_nxt */
+			OMAP4_IOPAD(0x0ca, PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_dat0.usbb1_ulpiphy_dat0 */
+			OMAP4_IOPAD(0x0cc, PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_dat1.usbb1_ulpiphy_dat1 */
+			OMAP4_IOPAD(0x0ce, PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_dat2.usbb1_ulpiphy_dat2 */
+			OMAP4_IOPAD(0x0d0, PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_dat3.usbb1_ulpiphy_dat3 */
+			OMAP4_IOPAD(0x0d2, PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_dat4.usbb1_ulpiphy_dat4 */
+			OMAP4_IOPAD(0x0d4, PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_dat5.usbb1_ulpiphy_dat5 */
+			OMAP4_IOPAD(0x0d6, PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_dat6.usbb1_ulpiphy_dat6 */
+			OMAP4_IOPAD(0x0d8, PIN_INPUT_PULLDOWN | MUX_MODE4)	/* usbb1_ulpitll_dat7.usbb1_ulpiphy_dat7 */
+		>;
+	};
+
+	hsusbb1_phy_rst_pins: pinmux_hsusbb1_phy_rst_pins {
+		pinctrl-single,pins = <
+			OMAP4_IOPAD(0x18c, PIN_OUTPUT | MUX_MODE3)		/* kpd_row2.gpio_177 */
+		>;
+	};
+
+	i2c1_pins: pinmux_i2c1_pins {
+		pinctrl-single,pins = <
+			OMAP4_IOPAD(0x122, PIN_INPUT_PULLUP | MUX_MODE0)	/* i2c1_scl */
+			OMAP4_IOPAD(0x124, PIN_INPUT_PULLUP | MUX_MODE0)	/* i2c1_sda */
+		>;
+	};
+
+	i2c3_pins: pinmux_i2c3_pins {
+		pinctrl-single,pins = <
+			OMAP4_IOPAD(0x12a, PIN_INPUT_PULLUP | MUX_MODE0)	/* i2c3_scl */
+			OMAP4_IOPAD(0x12c, PIN_INPUT_PULLUP | MUX_MODE0)	/* i2c3_sda */
+		>;
+	};
+
+	mmc1_pins: pinmux_mmc1_pins {
+		pinctrl-single,pins = <
+			OMAP4_IOPAD(0x0e2, PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmmc1_clk.sdmmc1_clk */
+			OMAP4_IOPAD(0x0e4, PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmmc1_cmd.sdmmc1_cmd */
+			OMAP4_IOPAD(0x0e6, PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmmc1_dat0.sdmmc1_dat0 */
+			OMAP4_IOPAD(0x0e8, PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmmc1_dat1.sdmmc1_dat1 */
+			OMAP4_IOPAD(0x0ea, PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmmc1_dat2.sdmmc1_dat2 */
+			OMAP4_IOPAD(0x0ec, PIN_INPUT_PULLUP | MUX_MODE0)	/* sdmmc1_dat3.sdmmc1_dat3 */
+		>;
+	};
+};
+
+&omap4_pmx_wkup {
+	pinctrl-names = "default";
+	pinctrl-0 = <
+		&hsusbb1_hub_rst_pins
+		&lan7500_rst_pins
+	>;
+
+	hsusbb1_phy_clk_pins: pinmux_hsusbb1_phy_clk_pins {
+		pinctrl-single,pins = <
+			OMAP4_IOPAD(0x058, PIN_OUTPUT | MUX_MODE0)	/* fref_clk3_out */
+		>;
+	};
+
+	hsusbb1_hub_rst_pins: pinmux_hsusbb1_hub_rst_pins {
+		pinctrl-single,pins = <
+			OMAP4_IOPAD(0x042, PIN_OUTPUT | MUX_MODE3)	/* gpio_wk1 */
+		>;
+	};
+
+	lan7500_rst_pins: pinmux_lan7500_rst_pins {
+		pinctrl-single,pins = <
+			OMAP4_IOPAD(0x040, PIN_OUTPUT | MUX_MODE3)	/* gpio_wk0 */
+		>;
+	};
+};
+
+&i2c1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&i2c1_pins>;
+	status = "okay";
+
+	clock-frequency = <400000>;
+
+	twl: twl@48 {
+		reg = <0x48>;
+		/* SPI = 0, IRQ# = 7, 4 = active high level-sensitive */
+		interrupts = <GIC_SPI 7 IRQ_TYPE_LEVEL_HIGH>; /* IRQ_SYS_1N cascaded to gic */
+		interrupt-parent = <&gic>;
+	};
+
+	twl6040: twl@4b {
+		compatible = "ti,twl6040";
+		reg = <0x4b>;
+
+		pinctrl-names = "default";
+		pinctrl-0 = <&twl6040_pins>;
+
+		/* SPI = 0, IRQ# = 119, 4 = active high level-sensitive */
+		interrupts = <GIC_SPI 119 IRQ_TYPE_LEVEL_HIGH>; /* IRQ_SYS_2N cascaded to gic */
+		interrupt-parent = <&gic>;
+		ti,audpwron-gpio = <&gpio6 22 0>; /* gpio 182 */
+
+		vio-supply = <&v1v8>;
+		v2v1-supply = <&v2v1>;
+		enable-active-high;
+	};
+};
+
+#include "twl6030.dtsi"
+#include "twl6030_omap4.dtsi"
+
+&vusim {
+	regulator-min-microvolt = <3000000>;
+	regulator-max-microvolt = <3000000>;
+	regulator-always-on;
+};
+
+&i2c2 {
+	status = "disabled";
+};
+
+&i2c3 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&i2c3_pins>;
+	status = "okay";
+
+	clock-frequency = <400000>;
+
+	touchscreen: tsc2004@48 {
+		compatible = "ti,tsc2004";
+		reg = <0x48>;
+		pinctrl-names = "default";
+		pinctrl-0 = <&tsc2004_pins>;
+		interrupt-parent = <&gpio4>;
+		interrupts = <5 IRQ_TYPE_LEVEL_LOW>; /* gpio 101 */
+		status = "disabled";
+	};
+
+	tmp105@49 {
+		compatible = "ti,tmp105";
+		reg = <0x49>;
+	};
+
+	eeprom@50 {
+		compatible = "microchip,24c32";
+		reg = <0x50>;
+	};
+};
+
+&i2c4 {
+	status = "disabled";
+};
+
+&mcpdm {
+	pinctrl-names = "default";
+	pinctrl-0 = <&mcpdm_pins>;
+	status = "okay";
+};
+
+&gpmc {
+	status = "disabled";
+};
+
+&mcspi1 {
+	status = "disabled";
+};
+
+&mcspi2 {
+	status = "disabled";
+};
+
+&mcspi3 {
+	status = "disabled";
+};
+
+&mcspi4 {
+	status = "disabled";
+};
+
+&mmc1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&mmc1_pins>;
+	vmmc-supply = <&vmmc>;
+	bus-width = <4>;
+	ti,non-removable;
+	status = "okay";
+};
+
+&mmc2 {
+	status = "disabled";
+};
+
+&mmc3 {
+	status = "disabled";
+};
+
+&mmc4 {
+	status = "disabled";
+};
+
+&mmc5 {
+	status = "disabled";
+};
+
+&uart1 {
+	status = "disabled";
+};
+
+&uart2 {
+	status = "disabled";
+};
+
+&uart3 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&uart3_pins>;
+	status = "okay";
+};
+
+&uart4 {
+	status = "disabled";
+};
+
+&keypad {
+	status = "disabled";
+};
+
+&twl_usb_comparator {
+	usb-supply = <&vusb>;
+};
+
+&usb_otg_hs {
+	interface-type = <1>;
+	mode = <3>;
+	power = <50>;
+};
+
+&usbhshost {
+	port1-mode = "ehci-phy";
+};
+
+&usbhsehci {
+	phys = <&hsusb1_phy>;
+};
diff --git a/arch/arm/boot/dts/omap4-var-som.dts b/arch/arm/boot/dts/omap4-var-som.dts
deleted file mode 100644
index b41269e871d..00000000000
--- a/arch/arm/boot/dts/omap4-var-som.dts
+++ /dev/null
@@ -1,96 +0,0 @@
-/*
- * Copyright (C) 2012 Variscite Ltd. - http://www.variscite.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.
- */
-/dts-v1/;
-
-#include "omap443x.dtsi"
-
-/ {
-	model = "Variscite OMAP4 SOM";
-	compatible = "var,omap4-var_som", "ti,omap4430", "ti,omap4";
-
-	memory {
-		device_type = "memory";
-		reg = <0x80000000 0x40000000>; /* 1 GB */
-	};
-
-	vdd_eth: fixedregulator@0 {
-		compatible = "regulator-fixed";
-		regulator-name = "VDD_ETH";
-		regulator-min-microvolt = <3300000>;
-		regulator-max-microvolt = <3300000>;
-		enable-active-high;
-		regulator-boot-on;
-	};
-};
-
-&i2c1 {
-	clock-frequency = <400000>;
-
-	twl: twl@48 {
-		reg = <0x48>;
-		/* SPI = 0, IRQ# = 7, 4 = active high level-sensitive */
-		interrupts = <GIC_SPI 7 IRQ_TYPE_LEVEL_HIGH>; /* IRQ_SYS_1N cascaded to gic */
-		interrupt-parent = <&gic>;
-	};
-};
-
-#include "twl6030.dtsi"
-
-&i2c2 {
-	clock-frequency = <400000>;
-};
-
-&i2c3 {
-	clock-frequency = <400000>;
-
-	/*
-	 * Temperature Sensor
-	 * http://www.ti.com/lit/ds/symlink/tmp105.pdf
-	 */
-	tmp105@49 {
-		compatible = "ti,tmp105";
-		reg = <0x49>;
-	};
-};
-
-&i2c4 {
-	clock-frequency = <400000>;
-};
-
-&mcspi1 {
-	eth@0 {
-		compatible = "ks8851";
-		spi-max-frequency = <24000000>;
-		reg = <0>;
-		interrupt-parent = <&gpio6>;
-		interrupts = <11 IRQ_TYPE_LEVEL_LOW>; /* gpio line 171 */
-		vdd-supply = <&vdd_eth>;
-	};
-};
-
-&mmc1 {
-	vmmc-supply = <&vmmc>;
-	ti,bus-width = <8>;
-	ti,non-removable;
-};
-
-&mmc2 {
-	status = "disabled";
-};
-
-&mmc3 {
-	status = "disabled";
-};
-
-&mmc4 {
-	status = "disabled";
-};
-
-&mmc5 {
-	ti,bus-width = <4>;
-};
diff --git a/arch/arm/boot/dts/omap4-var-stk-om44.dts b/arch/arm/boot/dts/omap4-var-stk-om44.dts
new file mode 100644
index 00000000000..56b64e61860
--- /dev/null
+++ b/arch/arm/boot/dts/omap4-var-stk-om44.dts
@@ -0,0 +1,17 @@
+/*
+ * Copyright (C) 2014 Joachim Eastwood <manabian@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+/dts-v1/;
+
+#include "omap4-var-som-om44.dtsi"
+#include "omap4-var-som-om44-wlan.dtsi"
+#include "omap4-var-om44customboard.dtsi"
+
+/ {
+	model = "Variscite VAR-STK-OM44";
+	compatible = "variscite,var-stk-om44", "variscite,var-som-om44", "ti,omap4460", "ti,omap4";
+};
diff --git a/arch/arm/boot/dts/omap4.dtsi b/arch/arm/boot/dts/omap4.dtsi
index 27fcac87474..43a587e097d 100644
--- a/arch/arm/boot/dts/omap4.dtsi
+++ b/arch/arm/boot/dts/omap4.dtsi
@@ -67,12 +67,13 @@
 
 	local-timer@48240600 {
 		compatible = "arm,cortex-a9-twd-timer";
+		clocks = <&mpu_periphclk>;
 		reg = <0x48240600 0x20>;
 		interrupts = <GIC_PPI 13 (GIC_CPU_MASK_RAW(3) | IRQ_TYPE_LEVEL_HIGH)>;
 	};
 
 	/*
-	 * The soc node represents the soc top level view. It is uses for IPs
+	 * The soc node represents the soc top level view. It is used for IPs
 	 * that are not memory mapped in the MPU view or for the MPU itself.
 	 */
 	soc {
@@ -96,7 +97,7 @@
 	/*
 	 * XXX: Use a flat representation of the OMAP4 interconnect.
 	 * The real OMAP interconnect network is quite complex.
-	 * Since that will not bring real advantage to represent that in DT for
+	 * Since it will not bring real advantage to represent that in DT for
 	 * the moment, just use a fake OCP bus entry to represent the whole bus
 	 * hierarchy.
 	 */
@@ -311,7 +312,7 @@
 		uart2: serial@4806c000 {
 			compatible = "ti,omap4-uart";
 			reg = <0x4806c000 0x100>;
-			interrupts = <GIC_SPI 73 IRQ_TYPE_LEVEL_HIGH>;
+			interrupts-extended = <&gic GIC_SPI 73 IRQ_TYPE_LEVEL_HIGH>;
 			ti,hwmods = "uart2";
 			clock-frequency = <48000000>;
 		};
@@ -319,7 +320,7 @@
 		uart3: serial@48020000 {
 			compatible = "ti,omap4-uart";
 			reg = <0x48020000 0x100>;
-			interrupts = <GIC_SPI 74 IRQ_TYPE_LEVEL_HIGH>;
+			interrupts-extended = <&gic GIC_SPI 74 IRQ_TYPE_LEVEL_HIGH>;
 			ti,hwmods = "uart3";
 			clock-frequency = <48000000>;
 		};
@@ -327,7 +328,7 @@
 		uart4: serial@4806e000 {
 			compatible = "ti,omap4-uart";
 			reg = <0x4806e000 0x100>;
-			interrupts = <GIC_SPI 70 IRQ_TYPE_LEVEL_HIGH>;
+			interrupts-extended = <&gic GIC_SPI 70 IRQ_TYPE_LEVEL_HIGH>;
 			ti,hwmods = "uart4";
 			clock-frequency = <48000000>;
 		};
@@ -642,6 +643,8 @@
 				compatible = "ti,omap-usb2";
 				reg = <0x4a0ad080 0x58>;
 				ctrl-module = <&omap_control_usb2phy>;
+				clocks = <&usb_phy_cm_clk32k>;
+				clock-names = "wkupclk";
 				#phy-cells = <0>;
 			};
 		};
diff --git a/arch/arm/boot/dts/omap5-cm-t54.dts b/arch/arm/boot/dts/omap5-cm-t54.dts
new file mode 100644
index 00000000000..b8698ca6864
--- /dev/null
+++ b/arch/arm/boot/dts/omap5-cm-t54.dts
@@ -0,0 +1,413 @@
+/*
+ * Support for CompuLab CM-T54
+ */
+/dts-v1/;
+
+#include "omap5.dtsi"
+#include <dt-bindings/interrupt-controller/irq.h>
+#include <dt-bindings/interrupt-controller/arm-gic.h>
+
+/ {
+	model = "CompuLab CM-T54";
+	compatible = "compulab,omap5-cm-t54", "ti,omap5";
+
+	memory {
+		device_type = "memory";
+		reg = <0x80000000 0x7F000000>; /* 2048 MB */
+	};
+
+	vmmcsd_fixed: fixed-regulator-mmcsd {
+		compatible = "regulator-fixed";
+		regulator-name = "vmmcsd_fixed";
+		regulator-min-microvolt = <3300000>;
+		regulator-max-microvolt = <3300000>;
+	};
+
+	vwlan_pdn_fixed: fixed-regulator-vwlan-pdn {
+		compatible = "regulator-fixed";
+		regulator-name = "vwlan_pdn_fixed";
+		regulator-min-microvolt = <3300000>;
+		regulator-max-microvolt = <3300000>;
+		vin-supply = <&ldo2_reg>;
+		gpio = <&gpio4 13 GPIO_ACTIVE_HIGH>;   /* gpio4_109 */
+		startup-delay-us = <1000>;
+		enable-active-high;
+	};
+
+	vwlan_fixed: fixed-regulator-vwlan {
+		compatible = "regulator-fixed";
+		regulator-name = "vwlan_fixed";
+		regulator-min-microvolt = <3300000>;
+		regulator-max-microvolt = <3300000>;
+		vin-supply = <&vwlan_pdn_fixed>;
+		gpio = <&gpio4 14 GPIO_ACTIVE_HIGH>;   /* gpio4_110 */
+		startup-delay-us = <1000>;
+		enable-active-high;
+	};
+
+	/* HS USB Host PHY on PORT 2 */
+	hsusb2_phy: hsusb2_phy {
+		compatible = "usb-nop-xceiv";
+		reset-gpios = <&gpio3 12 GPIO_ACTIVE_LOW>; /* gpio3_76 HUB_RESET */
+	};
+
+	/* HS USB Host PHY on PORT 3 */
+	hsusb3_phy: hsusb3_phy {
+		compatible = "usb-nop-xceiv";
+		reset-gpios = <&gpio3 19 GPIO_ACTIVE_LOW>; /* gpio3_83 ETH_RESET */
+	};
+
+	leds {
+		compatible = "gpio-leds";
+		led@1 {
+			label = "Heartbeat";
+			gpios = <&gpio3 16 GPIO_ACTIVE_HIGH>; /* gpio3_80 ACT_LED */
+			linux,default-trigger = "heartbeat";
+			default-state = "off";
+		};
+	};
+};
+
+&omap5_pmx_core {
+	pinctrl-names = "default";
+	pinctrl-0 = <
+			&led_gpio_pins
+			&usbhost_pins
+	>;
+
+	led_gpio_pins: pinmux_led_gpio_pins {
+		pinctrl-single,pins = <
+			OMAP5_IOPAD(0x00b0, PIN_OUTPUT | MUX_MODE6) /* hsi2_caflag.gpio3_80 */
+		>;
+	};
+
+	i2c1_pins: pinmux_i2c1_pins {
+		pinctrl-single,pins = <
+			OMAP5_IOPAD(0x01f2, PIN_INPUT_PULLUP | MUX_MODE0) /* i2c1_pmic_scl */
+			OMAP5_IOPAD(0x01f4, PIN_INPUT_PULLUP | MUX_MODE0) /* i2c1_pmic_sda */
+		>;
+	};
+
+	mmc1_pins: pinmux_mmc1_pins {
+		pinctrl-single,pins = <
+			OMAP5_IOPAD(0x01e2, PIN_INPUT_PULLUP | MUX_MODE0) /* sdcard_clk */
+			OMAP5_IOPAD(0x01e4, PIN_INPUT_PULLUP | MUX_MODE0) /* sdcard_cmd */
+			OMAP5_IOPAD(0x01e6, PIN_INPUT_PULLUP | MUX_MODE0) /* sdcard_data2 */
+			OMAP5_IOPAD(0x01e8, PIN_INPUT_PULLUP | MUX_MODE0) /* sdcard_data3 */
+			OMAP5_IOPAD(0x01ea, PIN_INPUT_PULLUP | MUX_MODE0) /* sdcard_data0 */
+			OMAP5_IOPAD(0x01ec, PIN_INPUT_PULLUP | MUX_MODE0) /* sdcard_data1 */
+		>;
+	};
+
+	mmc2_pins: pinmux_mmc2_pins {
+		pinctrl-single,pins = <
+			OMAP5_IOPAD(0x0040, PIN_INPUT_PULLUP | MUX_MODE0) /* emmc_clk */
+			OMAP5_IOPAD(0x0042, PIN_INPUT_PULLUP | MUX_MODE0) /* emmc_cmd */
+			OMAP5_IOPAD(0x0044, PIN_INPUT_PULLUP | MUX_MODE0) /* emmc_data0 */
+			OMAP5_IOPAD(0x0046, PIN_INPUT_PULLUP | MUX_MODE0) /* emmc_data1 */
+			OMAP5_IOPAD(0x0048, PIN_INPUT_PULLUP | MUX_MODE0) /* emmc_data2 */
+			OMAP5_IOPAD(0x004a, PIN_INPUT_PULLUP | MUX_MODE0) /* emmc_data3 */
+			OMAP5_IOPAD(0x004c, PIN_INPUT_PULLUP | MUX_MODE0) /* emmc_data4 */
+			OMAP5_IOPAD(0x004e, PIN_INPUT_PULLUP | MUX_MODE0) /* emmc_data5 */
+			OMAP5_IOPAD(0x0050, PIN_INPUT_PULLUP | MUX_MODE0) /* emmc_data6 */
+			OMAP5_IOPAD(0x0052, PIN_INPUT_PULLUP | MUX_MODE0) /* emmc_data7 */
+		>;
+	};
+
+	mmc3_pins: pinmux_mmc3_pins {
+		pinctrl-single,pins = <
+			OMAP5_IOPAD(0x01a4, PIN_INPUT_PULLUP | MUX_MODE0) /* wlsdio_clk */
+			OMAP5_IOPAD(0x01a6, PIN_INPUT_PULLUP | MUX_MODE0) /* wlsdio_cmd */
+			OMAP5_IOPAD(0x01a8, PIN_INPUT_PULLUP | MUX_MODE0) /* wlsdio_data0 */
+			OMAP5_IOPAD(0x01aa, PIN_INPUT_PULLUP | MUX_MODE0) /* wlsdio_data1 */
+			OMAP5_IOPAD(0x01ac, PIN_INPUT_PULLUP | MUX_MODE0) /* wlsdio_data2 */
+			OMAP5_IOPAD(0x01ae, PIN_INPUT_PULLUP | MUX_MODE0) /* wlsdio_data3 */
+		>;
+	};
+
+	wlan_gpios_pins: pinmux_wlan_gpios_pins {
+		pinctrl-single,pins = <
+			OMAP5_IOPAD(0x019c, PIN_OUTPUT_PULLDOWN | MUX_MODE6) /* gpio4_109 */
+			OMAP5_IOPAD(0x019e, PIN_OUTPUT_PULLDOWN | MUX_MODE6) /* gpio4_110 */
+		>;
+	};
+
+	usbhost_pins: pinmux_usbhost_pins {
+		pinctrl-single,pins = <
+			OMAP5_IOPAD(0x00c4, PIN_INPUT | MUX_MODE0)  /* usbb2_hsic_strobe */
+			OMAP5_IOPAD(0x00c6, PIN_INPUT | MUX_MODE0)  /* usbb2_hsic_data */
+
+			OMAP5_IOPAD(0x01dc, PIN_INPUT | MUX_MODE0)  /* usbb3_hsic_strobe */
+			OMAP5_IOPAD(0x01de, PIN_INPUT | MUX_MODE0)  /* usbb3_hsic_data */
+
+			OMAP5_IOPAD(0x00a8, PIN_OUTPUT | MUX_MODE6) /* hsi2_caready.gpio3_76 */
+			OMAP5_IOPAD(0x00b6, PIN_OUTPUT | MUX_MODE6) /* hsi2_acdata.gpio3_83 */
+		>;
+	};
+};
+
+&mmc1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&mmc1_pins>;
+	vmmc-supply = <&ldo9_reg>;
+	bus-width = <4>;
+};
+
+&mmc2 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&mmc2_pins>;
+	vmmc-supply = <&vmmcsd_fixed>;
+	bus-width = <8>;
+	ti,non-removable;
+};
+
+&mmc3 {
+	pinctrl-names = "default";
+	pinctrl-0 = <
+		&mmc3_pins
+		&wlan_gpios_pins
+	>;
+	vmmc-supply = <&vwlan_fixed>;
+	bus-width = <4>;
+	ti,non-removable;
+};
+
+&mmc4 {
+	status = "disabled";
+};
+
+&mmc5 {
+	status = "disabled";
+};
+
+&i2c1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&i2c1_pins>;
+
+	clock-frequency = <400000>;
+
+	at24@50 {
+		compatible = "at24,24c02";
+		pagesize = <16>;
+		reg = <0x50>;
+	};
+
+	palmas: palmas@48 {
+		compatible = "ti,palmas";
+		interrupts = <GIC_SPI 7 IRQ_TYPE_NONE>; /* IRQ_SYS_1N */
+		interrupt-parent = <&gic>;
+		reg = <0x48>;
+		interrupt-controller;
+		#interrupt-cells = <2>;
+		ti,system-power-controller;
+
+		extcon_usb3: palmas_usb {
+			compatible = "ti,palmas-usb-vid";
+			ti,enable-vbus-detection;
+			ti,enable-id-detection;
+			ti,wakeup;
+		};
+
+		rtc {
+			compatible = "ti,palmas-rtc";
+			interrupt-parent = <&palmas>;
+			interrupts = <8 IRQ_TYPE_NONE>;
+		};
+
+		palmas_pmic {
+			compatible = "ti,palmas-pmic";
+			interrupt-parent = <&palmas>;
+			interrupts = <14 IRQ_TYPE_NONE>;
+			interrupt-name = "short-irq";
+
+			ti,ldo6-vibrator;
+
+			regulators {
+				smps123_reg: smps123 {
+					/* VDD_OPP_MPU */
+					regulator-name = "smps123";
+					regulator-min-microvolt = < 600000>;
+					regulator-max-microvolt = <1500000>;
+					regulator-always-on;
+					regulator-boot-on;
+				};
+
+				smps45_reg: smps45 {
+					/* VDD_OPP_MM */
+					regulator-name = "smps45";
+					regulator-min-microvolt = < 600000>;
+					regulator-max-microvolt = <1310000>;
+					regulator-always-on;
+					regulator-boot-on;
+				};
+
+				smps6_reg: smps6 {
+					/* VDD_DDR3 - over VDD_SMPS6 */
+					regulator-name = "smps6";
+					regulator-min-microvolt = <1500000>;
+					regulator-max-microvolt = <1500000>;
+					regulator-always-on;
+					regulator-boot-on;
+				};
+
+				smps7_reg: smps7 {
+					/* VDDS_1v8_OMAP over VDDS_1v8_MAIN */
+					regulator-name = "smps7";
+					regulator-min-microvolt = <1800000>;
+					regulator-max-microvolt = <1800000>;
+					regulator-always-on;
+					regulator-boot-on;
+				};
+
+				smps8_reg: smps8 {
+					/* VDD_OPP_CORE */
+					regulator-name = "smps8";
+					regulator-min-microvolt = < 600000>;
+					regulator-max-microvolt = <1310000>;
+					regulator-always-on;
+					regulator-boot-on;
+				};
+
+				smps9_reg: smps9 {
+					/* VDDA_2v1_AUD over VDD_2v1 */
+					regulator-name = "smps9";
+					regulator-min-microvolt = <3300000>;
+					regulator-max-microvolt = <3300000>;
+					ti,smps-range = <0x80>;
+					regulator-always-on;
+					regulator-boot-on;
+				};
+
+				smps10_out2_reg: smps10_out2 {
+					/* VBUS_5V_OTG */
+					regulator-name = "smps10_out2";
+					regulator-min-microvolt = <5000000>;
+					regulator-max-microvolt = <5000000>;
+					regulator-always-on;
+					regulator-boot-on;
+				};
+
+				smps10_out1_reg: smps10_out1 {
+					/* VBUS_5V_OTG */
+					regulator-name = "smps10_out1";
+					regulator-min-microvolt = <5000000>;
+					regulator-max-microvolt = <5000000>;
+				};
+
+				ldo1_reg: ldo1 {
+					/* VDDAPHY_CAM: vdda_csiport */
+					regulator-name = "ldo1";
+					regulator-min-microvolt = <1500000>;
+					regulator-max-microvolt = <1800000>;
+				};
+
+				ldo2_reg: ldo2 {
+					/* VDD_3V3_WLAN */
+					regulator-name = "ldo2";
+					regulator-min-microvolt = <3300000>;
+					regulator-max-microvolt = <3300000>;
+					startup-delay-us = <1000>;
+				};
+
+				ldo3_reg: ldo3 {
+					/* VCC_1V5_AUD */
+					regulator-name = "ldo3";
+					regulator-min-microvolt = <1500000>;
+					regulator-max-microvolt = <1500000>;
+					regulator-always-on;
+					regulator-boot-on;
+				};
+
+				ldo4_reg: ldo4 {
+					/* VDDAPHY_DISP: vdda_dsiport/hdmi */
+					regulator-name = "ldo4";
+					regulator-min-microvolt = <1500000>;
+					regulator-max-microvolt = <1800000>;
+				};
+
+				ldo5_reg: ldo5 {
+					/* VDDA_1V8_PHY: usb/sata/hdmi.. */
+					regulator-name = "ldo5";
+					regulator-min-microvolt = <1800000>;
+					regulator-max-microvolt = <1800000>;
+					regulator-always-on;
+					regulator-boot-on;
+				};
+
+				ldo6_reg: ldo6 {
+					/* VDDS_1V2_WKUP: hsic/ldo_emu_wkup */
+					regulator-name = "ldo6";
+					regulator-min-microvolt = <1200000>;
+					regulator-max-microvolt = <1200000>;
+					regulator-always-on;
+					regulator-boot-on;
+				};
+
+				ldo7_reg: ldo7 {
+					/* VDD_VPP: vpp1 */
+					regulator-name = "ldo7";
+					regulator-min-microvolt = <2000000>;
+					regulator-max-microvolt = <2000000>;
+					/* Only for efuse reprograming! */
+					status = "disabled";
+				};
+
+				ldo8_reg: ldo8 {
+					/* VDD_3v0: Does not go anywhere */
+					regulator-name = "ldo8";
+					regulator-min-microvolt = <3000000>;
+					regulator-max-microvolt = <3000000>;
+					regulator-boot-on;
+					/* Unused */
+					status = "disabled";
+				};
+
+				ldo9_reg: ldo9 {
+					/* VCC_DV_SDIO: vdds_sdcard */
+					regulator-name = "ldo9";
+					regulator-min-microvolt = <1800000>;
+					regulator-max-microvolt = <3000000>;
+					regulator-boot-on;
+				};
+
+				ldoln_reg: ldoln {
+					/* VDDA_1v8_REF: vdds_osc/mm_l4per.. */
+					regulator-name = "ldoln";
+					regulator-min-microvolt = <1800000>;
+					regulator-max-microvolt = <1800000>;
+					regulator-always-on;
+					regulator-boot-on;
+				};
+
+				ldousb_reg: ldousb {
+					/* VDDA_3V_USB: VDDA_USBHS33 */
+					regulator-name = "ldousb";
+					regulator-min-microvolt = <3250000>;
+					regulator-max-microvolt = <3250000>;
+					regulator-always-on;
+					regulator-boot-on;
+				};
+
+				regen3_reg: regen3 {
+					/* REGEN3 controls LDO9 supply to card */
+					regulator-name = "regen3";
+					regulator-always-on;
+					regulator-boot-on;
+				};
+			};
+		};
+	};
+};
+
+&usbhshost {
+	port2-mode = "ehci-hsic";
+	port3-mode = "ehci-hsic";
+};
+
+&usbhsehci {
+	phys = <0 &hsusb2_phy &hsusb3_phy>;
+};
+
+&cpu0 {
+	cpu0-supply = <&smps123_reg>;
+};
diff --git a/arch/arm/boot/dts/omap5-sbc-t54.dts b/arch/arm/boot/dts/omap5-sbc-t54.dts
new file mode 100644
index 00000000000..aa98fea3f2b
--- /dev/null
+++ b/arch/arm/boot/dts/omap5-sbc-t54.dts
@@ -0,0 +1,51 @@
+/*
+ * Suppport for CompuLab SBC-T54 with CM-T54
+ */
+
+#include "omap5-cm-t54.dts"
+
+/ {
+	model = "CompuLab SBC-T54 with CM-T54";
+	compatible = "compulab,omap5-sbc-t54", "compulab,omap5-cm-t54", "ti,omap5";
+};
+
+&omap5_pmx_core {
+	i2c4_pins: pinmux_i2c4_pins {
+		pinctrl-single,pins = <
+			OMAP5_IOPAD(0x00f8, PIN_INPUT_PULLUP | MUX_MODE0) /* i2c4_scl */
+			OMAP5_IOPAD(0x00fa, PIN_INPUT_PULLUP | MUX_MODE0) /* i2c4_sda */
+		>;
+	};
+
+	mmc1_aux_pins: pinmux_mmc1_aux_pins {
+		pinctrl-single,pins = <
+			OMAP5_IOPAD(0x0174, PIN_INPUT_PULLUP | MUX_MODE6) /* gpio8_228 */
+			OMAP5_IOPAD(0x0176, PIN_INPUT_PULLUP | MUX_MODE6) /* gpio8_229 */
+		>;
+	};
+};
+
+&mmc1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <
+		&mmc1_pins
+		&mmc1_aux_pins
+	>;
+	cd-inverted;
+	wp-inverted;
+	cd-gpios = <&gpio8 4 GPIO_ACTIVE_LOW>; /* gpio8_228 */
+	wp-gpios = <&gpio8 5 GPIO_ACTIVE_LOW>; /* gpio8_229 */
+};
+
+&i2c4 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&i2c4_pins>;
+
+	clock-frequency = <400000>;
+
+	at24@50 {
+		compatible = "at24,24c02";
+		pagesize = <16>;
+		reg = <0x50>;
+	};
+};
diff --git a/arch/arm/boot/dts/omap5.dtsi b/arch/arm/boot/dts/omap5.dtsi
index 6f3de22fb26..e58be57984a 100644
--- a/arch/arm/boot/dts/omap5.dtsi
+++ b/arch/arm/boot/dts/omap5.dtsi
@@ -82,6 +82,12 @@
 			     <GIC_PPI 10 (GIC_CPU_MASK_RAW(3) | IRQ_TYPE_LEVEL_LOW)>;
 	};
 
+	pmu {
+		compatible = "arm,cortex-a15-pmu";
+		interrupts = <GIC_SPI 131 IRQ_TYPE_LEVEL_HIGH>,
+			     <GIC_SPI 132 IRQ_TYPE_LEVEL_HIGH>;
+	};
+
 	gic: interrupt-controller@48211000 {
 		compatible = "arm,cortex-a15-gic";
 		interrupt-controller;
@@ -93,7 +99,7 @@
 	};
 
 	/*
-	 * The soc node represents the soc top level view. It is uses for IPs
+	 * The soc node represents the soc top level view. It is used for IPs
 	 * that are not memory mapped in the MPU view or for the MPU itself.
 	 */
 	soc {
@@ -107,7 +113,7 @@
 	/*
 	 * XXX: Use a flat representation of the OMAP3 interconnect.
 	 * The real OMAP interconnect network is quite complex.
-	 * Since that will not bring real advantage to represent that in DT for
+	 * Since it will not bring real advantage to represent that in DT for
 	 * the moment, just use a fake OCP bus entry to represent the whole bus
 	 * hierarchy.
 	 */
@@ -630,6 +636,13 @@
 			status = "disabled";
 		};
 
+		mailbox: mailbox@4a0f4000 {
+			compatible = "ti,omap4-mailbox";
+			reg = <0x4a0f4000 0x200>;
+			interrupts = <GIC_SPI 26 IRQ_TYPE_LEVEL_HIGH>;
+			ti,hwmods = "mailbox";
+		};
+
 		timer1: timer@4ae18000 {
 			compatible = "ti,omap5430-timer";
 			reg = <0x4ae18000 0x80>;
@@ -803,6 +816,8 @@
 				compatible = "ti,omap-usb2";
 				reg = <0x4a084000 0x7c>;
 				ctrl-module = <&omap_control_usb2phy>;
+				clocks = <&usb_phy_cm_clk32k>, <&usb_otg_ss_refclk960m>;
+				clock-names = "wkupclk", "refclk";
 				#phy-cells = <0>;
 			};
 
@@ -813,6 +828,12 @@
 				      <0x4a084c00 0x40>;
 				reg-names = "phy_rx", "phy_tx", "pll_ctrl";
 				ctrl-module = <&omap_control_usb3phy>;
+				clocks = <&usb_phy_cm_clk32k>,
+					 <&sys_clkin>,
+					 <&usb_otg_ss_refclk960m>;
+				clock-names =	"wkupclk",
+						"sysclk",
+						"refclk";
 				#phy-cells = <0>;
 			};
 		};
@@ -863,6 +884,46 @@
 
 			#thermal-sensor-cells = <1>;
 		};
+
+		omap_control_sata: control-phy@4a002374 {
+			compatible = "ti,control-phy-pipe3";
+			reg = <0x4a002374 0x4>;
+			reg-names = "power";
+			clocks = <&sys_clkin>;
+			clock-names = "sysclk";
+		};
+
+		/* OCP2SCP3 */
+		ocp2scp@4a090000 {
+			compatible = "ti,omap-ocp2scp";
+			#address-cells = <1>;
+			#size-cells = <1>;
+			reg = <0x4a090000 0x20>;
+			ranges;
+			ti,hwmods = "ocp2scp3";
+			sata_phy: phy@4a096000 {
+				compatible = "ti,phy-pipe3-sata";
+				reg = <0x4A096000 0x80>, /* phy_rx */
+				      <0x4A096400 0x64>, /* phy_tx */
+				      <0x4A096800 0x40>; /* pll_ctrl */
+				reg-names = "phy_rx", "phy_tx", "pll_ctrl";
+				ctrl-module = <&omap_control_sata>;
+				clocks = <&sys_clkin>;
+				clock-names = "sysclk";
+				#phy-cells = <0>;
+			};
+		};
+
+		sata: sata@4a141100 {
+			compatible = "snps,dwc-ahci";
+			reg = <0x4a140000 0x1100>, <0x4a141100 0x7>;
+			interrupts = <GIC_SPI 54 IRQ_TYPE_LEVEL_HIGH>;
+			phys = <&sata_phy>;
+			phy-names = "sata-phy";
+			clocks = <&sata_ref_clk>;
+			ti,hwmods = "sata";
+		};
+
 	};
 };
 
diff --git a/arch/arm/boot/dts/omap54xx-clocks.dtsi b/arch/arm/boot/dts/omap54xx-clocks.dtsi
index d487fdab392..aeb142ce8e9 100644
--- a/arch/arm/boot/dts/omap54xx-clocks.dtsi
+++ b/arch/arm/boot/dts/omap54xx-clocks.dtsi
@@ -120,10 +120,8 @@
 		compatible = "ti,divider-clock";
 		clocks = <&dpll_abe_x2_ck>;
 		ti,max-div = <31>;
-		ti,autoidle-shift = <8>;
 		reg = <0x01f0>;
 		ti,index-starts-at-one;
-		ti,invert-autoidle-bit;
 	};
 
 	abe_24m_fclk: abe_24m_fclk {
@@ -145,10 +143,11 @@
 
 	abe_iclk: abe_iclk {
 		#clock-cells = <0>;
-		compatible = "fixed-factor-clock";
-		clocks = <&abe_clk>;
-		clock-mult = <1>;
-		clock-div = <2>;
+		compatible = "ti,divider-clock";
+		clocks = <&aess_fclk>;
+		ti,bit-shift = <24>;
+		reg = <0x0528>;
+		ti,dividers = <2>, <1>;
 	};
 
 	abe_lp_clk_div: abe_lp_clk_div {
@@ -164,10 +163,8 @@
 		compatible = "ti,divider-clock";
 		clocks = <&dpll_abe_x2_ck>;
 		ti,max-div = <31>;
-		ti,autoidle-shift = <8>;
 		reg = <0x01f4>;
 		ti,index-starts-at-one;
-		ti,invert-autoidle-bit;
 	};
 
 	dpll_core_ck: dpll_core_ck {
@@ -188,10 +185,8 @@
 		compatible = "ti,divider-clock";
 		clocks = <&dpll_core_x2_ck>;
 		ti,max-div = <63>;
-		ti,autoidle-shift = <8>;
 		reg = <0x0150>;
 		ti,index-starts-at-one;
-		ti,invert-autoidle-bit;
 	};
 
 	c2c_fclk: c2c_fclk {
@@ -215,10 +210,8 @@
 		compatible = "ti,divider-clock";
 		clocks = <&dpll_core_x2_ck>;
 		ti,max-div = <63>;
-		ti,autoidle-shift = <8>;
 		reg = <0x0138>;
 		ti,index-starts-at-one;
-		ti,invert-autoidle-bit;
 	};
 
 	dpll_core_h12x2_ck: dpll_core_h12x2_ck {
@@ -226,10 +219,8 @@
 		compatible = "ti,divider-clock";
 		clocks = <&dpll_core_x2_ck>;
 		ti,max-div = <63>;
-		ti,autoidle-shift = <8>;
 		reg = <0x013c>;
 		ti,index-starts-at-one;
-		ti,invert-autoidle-bit;
 	};
 
 	dpll_core_h13x2_ck: dpll_core_h13x2_ck {
@@ -237,10 +228,8 @@
 		compatible = "ti,divider-clock";
 		clocks = <&dpll_core_x2_ck>;
 		ti,max-div = <63>;
-		ti,autoidle-shift = <8>;
 		reg = <0x0140>;
 		ti,index-starts-at-one;
-		ti,invert-autoidle-bit;
 	};
 
 	dpll_core_h14x2_ck: dpll_core_h14x2_ck {
@@ -248,10 +237,8 @@
 		compatible = "ti,divider-clock";
 		clocks = <&dpll_core_x2_ck>;
 		ti,max-div = <63>;
-		ti,autoidle-shift = <8>;
 		reg = <0x0144>;
 		ti,index-starts-at-one;
-		ti,invert-autoidle-bit;
 	};
 
 	dpll_core_h22x2_ck: dpll_core_h22x2_ck {
@@ -259,10 +246,8 @@
 		compatible = "ti,divider-clock";
 		clocks = <&dpll_core_x2_ck>;
 		ti,max-div = <63>;
-		ti,autoidle-shift = <8>;
 		reg = <0x0154>;
 		ti,index-starts-at-one;
-		ti,invert-autoidle-bit;
 	};
 
 	dpll_core_h23x2_ck: dpll_core_h23x2_ck {
@@ -270,10 +255,8 @@
 		compatible = "ti,divider-clock";
 		clocks = <&dpll_core_x2_ck>;
 		ti,max-div = <63>;
-		ti,autoidle-shift = <8>;
 		reg = <0x0158>;
 		ti,index-starts-at-one;
-		ti,invert-autoidle-bit;
 	};
 
 	dpll_core_h24x2_ck: dpll_core_h24x2_ck {
@@ -281,10 +264,8 @@
 		compatible = "ti,divider-clock";
 		clocks = <&dpll_core_x2_ck>;
 		ti,max-div = <63>;
-		ti,autoidle-shift = <8>;
 		reg = <0x015c>;
 		ti,index-starts-at-one;
-		ti,invert-autoidle-bit;
 	};
 
 	dpll_core_m2_ck: dpll_core_m2_ck {
@@ -292,10 +273,8 @@
 		compatible = "ti,divider-clock";
 		clocks = <&dpll_core_ck>;
 		ti,max-div = <31>;
-		ti,autoidle-shift = <8>;
 		reg = <0x0130>;
 		ti,index-starts-at-one;
-		ti,invert-autoidle-bit;
 	};
 
 	dpll_core_m3x2_ck: dpll_core_m3x2_ck {
@@ -303,10 +282,8 @@
 		compatible = "ti,divider-clock";
 		clocks = <&dpll_core_x2_ck>;
 		ti,max-div = <31>;
-		ti,autoidle-shift = <8>;
 		reg = <0x0134>;
 		ti,index-starts-at-one;
-		ti,invert-autoidle-bit;
 	};
 
 	iva_dpll_hs_clk_div: iva_dpll_hs_clk_div {
@@ -335,10 +312,8 @@
 		compatible = "ti,divider-clock";
 		clocks = <&dpll_iva_x2_ck>;
 		ti,max-div = <63>;
-		ti,autoidle-shift = <8>;
 		reg = <0x01b8>;
 		ti,index-starts-at-one;
-		ti,invert-autoidle-bit;
 	};
 
 	dpll_iva_h12x2_ck: dpll_iva_h12x2_ck {
@@ -346,10 +321,8 @@
 		compatible = "ti,divider-clock";
 		clocks = <&dpll_iva_x2_ck>;
 		ti,max-div = <63>;
-		ti,autoidle-shift = <8>;
 		reg = <0x01bc>;
 		ti,index-starts-at-one;
-		ti,invert-autoidle-bit;
 	};
 
 	mpu_dpll_hs_clk_div: mpu_dpll_hs_clk_div {
@@ -372,10 +345,8 @@
 		compatible = "ti,divider-clock";
 		clocks = <&dpll_mpu_ck>;
 		ti,max-div = <31>;
-		ti,autoidle-shift = <8>;
 		reg = <0x0170>;
 		ti,index-starts-at-one;
-		ti,invert-autoidle-bit;
 	};
 
 	per_dpll_hs_clk_div: per_dpll_hs_clk_div {
@@ -642,10 +613,8 @@
 		compatible = "ti,divider-clock";
 		clocks = <&dpll_per_x2_ck>;
 		ti,max-div = <63>;
-		ti,autoidle-shift = <8>;
 		reg = <0x0158>;
 		ti,index-starts-at-one;
-		ti,invert-autoidle-bit;
 	};
 
 	dpll_per_h12x2_ck: dpll_per_h12x2_ck {
@@ -653,10 +622,8 @@
 		compatible = "ti,divider-clock";
 		clocks = <&dpll_per_x2_ck>;
 		ti,max-div = <63>;
-		ti,autoidle-shift = <8>;
 		reg = <0x015c>;
 		ti,index-starts-at-one;
-		ti,invert-autoidle-bit;
 	};
 
 	dpll_per_h14x2_ck: dpll_per_h14x2_ck {
@@ -664,10 +631,8 @@
 		compatible = "ti,divider-clock";
 		clocks = <&dpll_per_x2_ck>;
 		ti,max-div = <63>;
-		ti,autoidle-shift = <8>;
 		reg = <0x0164>;
 		ti,index-starts-at-one;
-		ti,invert-autoidle-bit;
 	};
 
 	dpll_per_m2_ck: dpll_per_m2_ck {
@@ -675,10 +640,8 @@
 		compatible = "ti,divider-clock";
 		clocks = <&dpll_per_ck>;
 		ti,max-div = <31>;
-		ti,autoidle-shift = <8>;
 		reg = <0x0150>;
 		ti,index-starts-at-one;
-		ti,invert-autoidle-bit;
 	};
 
 	dpll_per_m2x2_ck: dpll_per_m2x2_ck {
@@ -686,10 +649,8 @@
 		compatible = "ti,divider-clock";
 		clocks = <&dpll_per_x2_ck>;
 		ti,max-div = <31>;
-		ti,autoidle-shift = <8>;
 		reg = <0x0150>;
 		ti,index-starts-at-one;
-		ti,invert-autoidle-bit;
 	};
 
 	dpll_per_m3x2_ck: dpll_per_m3x2_ck {
@@ -697,10 +658,8 @@
 		compatible = "ti,divider-clock";
 		clocks = <&dpll_per_x2_ck>;
 		ti,max-div = <31>;
-		ti,autoidle-shift = <8>;
 		reg = <0x0154>;
 		ti,index-starts-at-one;
-		ti,invert-autoidle-bit;
 	};
 
 	dpll_unipro1_ck: dpll_unipro1_ck {
@@ -723,10 +682,8 @@
 		compatible = "ti,divider-clock";
 		clocks = <&dpll_unipro1_ck>;
 		ti,max-div = <127>;
-		ti,autoidle-shift = <8>;
 		reg = <0x0210>;
 		ti,index-starts-at-one;
-		ti,invert-autoidle-bit;
 	};
 
 	dpll_unipro2_ck: dpll_unipro2_ck {
@@ -749,10 +706,8 @@
 		compatible = "ti,divider-clock";
 		clocks = <&dpll_unipro2_ck>;
 		ti,max-div = <127>;
-		ti,autoidle-shift = <8>;
 		reg = <0x01d0>;
 		ti,index-starts-at-one;
-		ti,invert-autoidle-bit;
 	};
 
 	dpll_usb_ck: dpll_usb_ck {
@@ -775,10 +730,8 @@
 		compatible = "ti,divider-clock";
 		clocks = <&dpll_usb_ck>;
 		ti,max-div = <127>;
-		ti,autoidle-shift = <8>;
 		reg = <0x0190>;
 		ti,index-starts-at-one;
-		ti,invert-autoidle-bit;
 	};
 
 	func_128m_clk: func_128m_clk {
@@ -851,6 +804,7 @@
 		clocks = <&dpll_per_h12x2_ck>;
 		ti,bit-shift = <8>;
 		reg = <0x1420>;
+		ti,set-rate-parent;
 	};
 
 	dss_sys_clk: dss_sys_clk {
diff --git a/arch/arm/boot/dts/orion5x-lacie-d2-network.dts b/arch/arm/boot/dts/orion5x-lacie-d2-network.dts
new file mode 100644
index 00000000000..c701e8d16bb
--- /dev/null
+++ b/arch/arm/boot/dts/orion5x-lacie-d2-network.dts
@@ -0,0 +1,236 @@
+/*
+ * Copyright (C) 2014 Thomas Petazzoni <thomas.petazzoni@free-electrons.com>
+ * Copyright (C) 2009 Simon Guinot <sguinot@lacie.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.
+ */
+
+/dts-v1/;
+
+#include <dt-bindings/gpio/gpio.h>
+#include <dt-bindings/input/input.h>
+#include <dt-bindings/interrupt-controller/irq.h>
+#include "orion5x-mv88f5182.dtsi"
+
+/ {
+	model = "LaCie d2 Network";
+	compatible = "lacie,d2-network", "marvell,orion5x-88f5182", "marvell,orion5x";
+
+	memory {
+		reg = <0x00000000 0x4000000>; /* 64 MB */
+	};
+
+	chosen {
+		bootargs = "console=ttyS0,115200n8 earlyprintk";
+		linux,stdout-path = &uart0;
+	};
+
+	soc {
+		ranges = <MBUS_ID(0xf0, 0x01) 0 0xf1000000 0x100000>,
+			 <MBUS_ID(0x09, 0x00) 0 0xf2200000 0x800>,
+			 <MBUS_ID(0x01, 0x0f) 0 0xfff80000 0x80000>;
+	};
+
+	gpio-keys {
+		compatible = "gpio-keys";
+		pinctrl-0 = <&pmx_buttons>;
+		pinctrl-names = "default";
+		#address-cells = <1>;
+		#size-cells = <0>;
+		front_button {
+			label = "Front Push Button";
+			linux,code = <KEY_POWER>;
+			gpios = <&gpio0 18 GPIO_ACTIVE_HIGH>;
+		};
+
+		power_rocker_sw_on {
+			label = "Power rocker switch (on|auto)";
+			linux,input-type = <5>; /* EV_SW */
+			linux,code = <1>; /* D2NET_SWITCH_POWER_ON */
+			gpios = <&gpio0 8 GPIO_ACTIVE_HIGH>;
+		};
+
+		power_rocker_sw_off {
+			label = "Power rocker switch (auto|off)";
+			linux,input-type = <5>; /* EV_SW */
+			linux,code = <2>; /* D2NET_SWITCH_POWER_OFF */
+			gpios = <&gpio0 9 GPIO_ACTIVE_HIGH>;
+		};
+	};
+
+	regulators {
+		compatible = "simple-bus";
+		#address-cells = <1>;
+		#size-cells = <0>;
+		pinctrl-0 = <&pmx_sata0_power &pmx_sata1_power>;
+		pinctrl-names = "default";
+
+		sata0_power: regulator@0 {
+			compatible = "regulator-fixed";
+			reg = <0>;
+			regulator-name = "SATA0 Power";
+			regulator-min-microvolt = <5000000>;
+			regulator-max-microvolt = <5000000>;
+			enable-active-high;
+			regulator-always-on;
+			regulator-boot-on;
+			gpio = <&gpio0 3 GPIO_ACTIVE_HIGH>;
+		};
+
+		sata1_power: regulator@1 {
+			compatible = "regulator-fixed";
+			reg = <1>;
+			regulator-name = "SATA1 Power";
+			regulator-min-microvolt = <5000000>;
+			regulator-max-microvolt = <5000000>;
+			enable-active-high;
+			regulator-always-on;
+			regulator-boot-on;
+			gpio = <&gpio0 12 GPIO_ACTIVE_HIGH>;
+		};
+	};
+};
+
+&devbus_bootcs {
+	status = "okay";
+
+	devbus,keep-config;
+
+	/*
+	 * Currently the MTD code does not recognize the MX29LV400CBCT
+	 * as a bottom-type device. This could cause risks of
+	 * accidentally erasing critical flash sectors. We thus define
+	 * a single, write-protected partition covering the whole
+	 * flash.  TODO: once the flash part TOP/BOTTOM detection
+	 * issue is sorted out in the MTD code, break this into at
+	 * least three partitions: 'u-boot code', 'u-boot environment'
+	 * and 'whatever is left'.
+	 */
+	flash@0 {
+		compatible = "cfi-flash";
+		reg = <0 0x80000>;
+		bank-width = <1>;
+                #address-cells = <1>;
+		#size-cells = <1>;
+
+		partition@0 {
+			label = "Full512Kb";
+			reg = <0 0x80000>;
+			read-only;
+		};
+	};
+};
+
+&mdio {
+	status = "okay";
+
+	ethphy: ethernet-phy {
+		reg = <8>;
+	};
+};
+
+&ehci0 {
+	status = "okay";
+};
+
+&eth {
+	status = "okay";
+
+	ethernet-port@0 {
+		phy-handle = <&ethphy>;
+	};
+};
+
+&i2c {
+	status = "okay";
+	clock-frequency = <100000>;
+	#address-cells = <1>;
+
+	rtc@32 {
+		compatible = "ricoh,rs5c372b";
+		reg = <0x32>;
+	};
+
+	fan@3e {
+		compatible = "gmt,g762";
+		reg = <0x3e>;
+
+		/* Not enough HW info */
+		status = "disabled";
+	};
+
+	eeprom@50 {
+		compatible = "atmel,24c08";
+		reg = <0x50>;
+	};
+};
+
+&pinctrl {
+	pinctrl-0 = <&pmx_leds &pmx_board_id &pmx_fan_fail>;
+	pinctrl-names = "default";
+
+	pmx_board_id: pmx-board-id {
+		marvell,pins = "mpp0", "mpp1", "mpp2";
+		marvell,function = "gpio";
+	};
+
+	pmx_buttons: pmx-buttons {
+		marvell,pins = "mpp8", "mpp9", "mpp18";
+		marvell,function = "gpio";
+	};
+
+	pmx_fan_fail: pmx-fan-fail {
+		marvell,pins = "mpp5";
+		marvell,function = "gpio";
+	};
+
+	/*
+	 * MPP6: Red front LED
+	 * MPP16: Blue front LED blink control
+	 */
+	pmx_leds: pmx-leds {
+		marvell,pins = "mpp6", "mpp16";
+		marvell,function = "gpio";
+	};
+
+	pmx_sata0_led_active: pmx-sata0-led-active {
+		marvell,pins = "mpp14";
+		marvell,function = "sata0";
+	};
+
+	pmx_sata0_power: pmx-sata0-power {
+		marvell,pins = "mpp3";
+		marvell,function = "gpio";
+	};
+
+	pmx_sata1_led_active: pmx-sata1-led-active {
+		marvell,pins = "mpp15";
+		marvell,function = "sata1";
+	};
+
+	pmx_sata1_power: pmx-sata1-power {
+		marvell,pins = "mpp12";
+		marvell,function = "gpio";
+	};
+
+	/*
+	 * Non MPP GPIOs:
+	 *  GPIO 22: USB port 1 fuse (0 = Fail, 1 = Ok)
+	 *  GPIO 23: Blue front LED off
+	 *  GPIO 24: Inhibit board power off (0 = Disabled, 1 = Enabled)
+	 */
+};
+
+&sata {
+	pinctrl-0 = <&pmx_sata0_led_active
+		     &pmx_sata1_led_active>;
+	pinctrl-names = "default";
+	status = "okay";
+	nr-ports = <2>;
+};
+
+&uart0 {
+	status = "okay";
+};
diff --git a/arch/arm/boot/dts/orion5x-lacie-ethernet-disk-mini-v2.dts b/arch/arm/boot/dts/orion5x-lacie-ethernet-disk-mini-v2.dts
index 5ed6c137690..89ff404a528 100644
--- a/arch/arm/boot/dts/orion5x-lacie-ethernet-disk-mini-v2.dts
+++ b/arch/arm/boot/dts/orion5x-lacie-ethernet-disk-mini-v2.dts
@@ -6,8 +6,19 @@
  * warranty of any kind, whether express or implied.
  */
 
+/*
+ * TODO: add Orion USB device port init when kernel.org support is added.
+ * TODO: add flash write support: see below.
+ * TODO: add power-off support.
+ * TODO: add I2C EEPROM support.
+ */
+
 /dts-v1/;
-/include/ "orion5x.dtsi"
+
+#include <dt-bindings/gpio/gpio.h>
+#include <dt-bindings/input/input.h>
+#include <dt-bindings/interrupt-controller/irq.h>
+#include "orion5x-mv88f5182.dtsi"
 
 / {
 	model = "LaCie Ethernet Disk mini V2";
@@ -19,49 +30,84 @@
 
 	chosen {
 		bootargs = "console=ttyS0,115200n8 earlyprintk";
+		linux,stdout-path = &uart0;
 	};
 
-	ocp@f1000000 {
-		serial@12000 {
-			clock-frequency = <166666667>;
-			status = "okay";
-		};
-
-		sata@80000 {
-			status = "okay";
-			nr-ports = <2>;
-		};
+	soc {
+		ranges = <MBUS_ID(0xf0, 0x01) 0 0xf1000000 0x100000>,
+			 <MBUS_ID(0x09, 0x00) 0 0xf2200000 0x800>,
+			 <MBUS_ID(0x01, 0x0f) 0 0xfff80000 0x80000>;
 	};
 
-	gpio_keys {
+	gpio-keys {
 		compatible = "gpio-keys";
+		pinctrl-0 = <&pmx_power_button>;
+		pinctrl-names = "default";
 		#address-cells = <1>;
 		#size-cells = <0>;
 		button@1 {
 			label = "Power-on Switch";
-			linux,code = <116>; /* KEY_POWER */
-			gpios = <&gpio0 18 0>;
+			linux,code = <KEY_POWER>;
+			gpios = <&gpio0 18 GPIO_ACTIVE_HIGH>;
 		};
 	};
 
-	gpio_leds {
+	gpio-leds {
 		compatible = "gpio-leds";
+		pinctrl-0 = <&pmx_power_led>;
+		pinctrl-names = "default";
 
 		led@1 {
 			label = "power:blue";
-			gpios = <&gpio0 16 1>;
+			gpios = <&gpio0 16 GPIO_ACTIVE_LOW>;
 		};
 	};
 };
 
-&mdio {
+&devbus_bootcs {
 	status = "okay";
 
-	ethphy: ethernet-phy {
-		reg = <8>;
+	/* Read parameters */
+	devbus,bus-width    = <8>;
+	devbus,turn-off-ps  = <90000>;
+	devbus,badr-skew-ps = <0>;
+	devbus,acc-first-ps = <186000>;
+	devbus,acc-next-ps  = <186000>;
+
+	/* Write parameters */
+	devbus,wr-high-ps  = <90000>;
+	devbus,wr-low-ps   = <90000>;
+	devbus,ale-wr-ps   = <90000>;
+
+	/*
+	 * Currently the MTD code does not recognize the MX29LV400CBCT
+	 * as a bottom-type device. This could cause risks of
+	 * accidentally erasing critical flash sectors. We thus define
+	 * a single, write-protected partition covering the whole
+	 * flash.  TODO: once the flash part TOP/BOTTOM detection
+	 * issue is sorted out in the MTD code, break this into at
+	 * least three partitions: 'u-boot code', 'u-boot environment'
+	 * and 'whatever is left'.
+	 */
+	flash@0 {
+		compatible = "cfi-flash";
+		reg = <0 0x80000>;
+		bank-width = <1>;
+		#address-cells = <1>;
+		#size-cells = <1>;
+
+		partition@0 {
+			label = "Full512Kb";
+			reg = <0 0x80000>;
+			read-only;
+		};
 	};
 };
 
+&ehci0 {
+	status = "okay";
+};
+
 &eth {
 	status = "okay";
 
@@ -69,3 +115,60 @@
 		phy-handle = <&ethphy>;
 	};
 };
+
+&i2c {
+	status = "okay";
+	clock-frequency = <100000>;
+	#address-cells = <1>;
+
+	rtc@32 {
+		compatible = "ricoh,rs5c372a";
+		reg = <0x32>;
+		interrupt-parent = <&gpio0>;
+		interrupts = <3 IRQ_TYPE_LEVEL_LOW>;
+	};
+};
+
+&mdio {
+	status = "okay";
+
+	ethphy: ethernet-phy {
+		reg = <8>;
+	};
+};
+
+&pinctrl {
+	pinctrl-0 = <&pmx_rtc &pmx_power_led_ctrl>;
+	pinctrl-names = "default";
+
+	pmx_power_button: pmx-power-button {
+		marvell,pins = "mpp18";
+		marvell,function = "gpio";
+	};
+
+	pmx_power_led: pmx-power-led {
+		marvell,pins = "mpp16";
+		marvell,function = "gpio";
+	};
+
+	pmx_power_led_ctrl: pmx-power-led-ctrl {
+		marvell,pins = "mpp17";
+		marvell,function = "gpio";
+	};
+
+	pmx_rtc: pmx-rtc {
+		marvell,pins = "mpp3";
+		marvell,function = "gpio";
+	};
+};
+
+&sata {
+	pinctrl-0 = <&pmx_sata0 &pmx_sata1>;
+	pinctrl-names = "default";
+	status = "okay";
+	nr-ports = <2>;
+};
+
+&uart0 {
+	status = "okay";
+};
diff --git a/arch/arm/boot/dts/orion5x-maxtor-shared-storage-2.dts b/arch/arm/boot/dts/orion5x-maxtor-shared-storage-2.dts
new file mode 100644
index 00000000000..ff348490429
--- /dev/null
+++ b/arch/arm/boot/dts/orion5x-maxtor-shared-storage-2.dts
@@ -0,0 +1,178 @@
+/*
+ * Copyright (C) 2014 Thomas Petazzoni <thomas.petazzoni@free-electrons.com>
+ * Copyright (C) Sylver Bruneau <sylver.bruneau@googlemail.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.
+ */
+
+/dts-v1/;
+
+#include <dt-bindings/gpio/gpio.h>
+#include <dt-bindings/input/input.h>
+#include <dt-bindings/interrupt-controller/irq.h>
+#include "orion5x-mv88f5182.dtsi"
+
+/ {
+	model = "Maxtor Shared Storage II";
+	compatible = "maxtor,shared-storage-2", "marvell,orion5x-88f5182", "marvell,orion5x";
+
+	memory {
+		reg = <0x00000000 0x4000000>; /* 64 MB */
+	};
+
+	chosen {
+		bootargs = "console=ttyS0,115200n8 earlyprintk";
+		linux,stdout-path = &uart0;
+	};
+
+	soc {
+		ranges = <MBUS_ID(0xf0, 0x01) 0 0xf1000000 0x100000>,
+			 <MBUS_ID(0x09, 0x00) 0 0xf2200000 0x800>,
+			 <MBUS_ID(0x01, 0x0f) 0 0xff800000 0x40000>;
+	};
+
+	gpio-keys {
+		compatible = "gpio-keys";
+		pinctrl-0 = <&pmx_buttons>;
+		pinctrl-names = "default";
+		#address-cells = <1>;
+		#size-cells = <0>;
+		power {
+			label = "Power";
+			linux,code = <KEY_POWER>;
+			gpios = <&gpio0 11 GPIO_ACTIVE_LOW>;
+		};
+
+		reset {
+			label = "Reset";
+			linux,code = <KEY_RESTART>;
+			gpios = <&gpio0 12 GPIO_ACTIVE_LOW>;
+		};
+	};
+};
+
+&devbus_bootcs {
+	status = "okay";
+
+	devbus,keep-config;
+
+	/*
+	 * Currently the MTD code does not recognize the MX29LV400CBCT
+	 * as a bottom-type device. This could cause risks of
+	 * accidentally erasing critical flash sectors. We thus define
+	 * a single, write-protected partition covering the whole
+	 * flash.  TODO: once the flash part TOP/BOTTOM detection
+	 * issue is sorted out in the MTD code, break this into at
+	 * least three partitions: 'u-boot code', 'u-boot environment'
+	 * and 'whatever is left'.
+	 */
+	flash@0 {
+		compatible = "cfi-flash";
+		reg = <0 0x40000>;
+		bank-width = <1>;
+                #address-cells = <1>;
+		#size-cells = <1>;
+	};
+};
+
+&mdio {
+	status = "okay";
+
+	ethphy: ethernet-phy {
+		reg = <8>;
+	};
+};
+
+&ehci0 {
+	status = "okay";
+};
+
+&eth {
+	status = "okay";
+
+	ethernet-port@0 {
+		phy-handle = <&ethphy>;
+	};
+};
+
+&i2c {
+	status = "okay";
+	clock-frequency = <100000>;
+	#address-cells = <1>;
+
+	rtc@68 {
+		compatible = "st,m41t81";
+		reg = <0x68>;
+		pinctrl-0 = <&pmx_rtc>;
+		pinctrl-names = "default";
+		interrupt-parent = <&gpio0>;
+		interrupts = <3 IRQ_TYPE_LEVEL_LOW>;
+	};
+};
+
+&pinctrl {
+	pinctrl-0 = <&pmx_leds &pmx_misc>;
+	pinctrl-names = "default";
+
+	pmx_buttons: pmx-buttons {
+		marvell,pins = "mpp11", "mpp12";
+		marvell,function = "gpio";
+	};
+
+	/*
+	 * MPP0: Power LED
+	 * MPP1: Error LED
+	 */
+	pmx_leds: pmx-leds {
+		marvell,pins = "mpp0", "mpp1";
+		marvell,function = "gpio";
+	};
+
+	/*
+	 * MPP4: HDD ind. (Single/Dual)
+	 * MPP5: HD0 5V control
+	 * MPP6: HD0 12V control
+	 * MPP7: HD1 5V control
+	 * MPP8: HD1 12V control
+	 */
+	pmx_misc: pmx-misc {
+		marvell,pins = "mpp4", "mpp5", "mpp6", "mpp7", "mpp8", "mpp10";
+		marvell,function = "gpio";
+	};
+
+	pmx_rtc: pmx-rtc {
+		marvell,pins = "mpp3";
+		marvell,function = "gpio";
+	};
+
+	pmx_sata0_led_active: pmx-sata0-led-active {
+		marvell,pins = "mpp14";
+		marvell,function = "sata0";
+	};
+
+	pmx_sata1_led_active: pmx-sata1-led-active {
+		marvell,pins = "mpp15";
+		marvell,function = "sata1";
+	};
+
+	/*
+	 * Non MPP GPIOs:
+	 *  GPIO 22: USB port 1 fuse (0 = Fail, 1 = Ok)
+	 *  GPIO 23: Blue front LED off
+	 *  GPIO 24: Inhibit board power off (0 = Disabled, 1 = Enabled)
+	 */
+};
+
+&sata {
+	pinctrl-0 = <&pmx_sata0_led_active
+		     &pmx_sata1_led_active>;
+	pinctrl-names = "default";
+	status = "okay";
+	nr-ports = <2>;
+};
+
+&uart0 {
+	status = "okay";
+};
diff --git a/arch/arm/boot/dts/orion5x-mv88f5182.dtsi b/arch/arm/boot/dts/orion5x-mv88f5182.dtsi
new file mode 100644
index 00000000000..d1ed71c6020
--- /dev/null
+++ b/arch/arm/boot/dts/orion5x-mv88f5182.dtsi
@@ -0,0 +1,45 @@
+/*
+ * Copyright (C) 2014 Thomas Petazzoni <thomas.petazzoni@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.
+ */
+
+#include "orion5x.dtsi"
+
+/ {
+	compatible = "marvell,orion5x-88f5182", "marvell,orion5x";
+
+	soc {
+		compatible = "marvell,orion5x-88f5182-mbus", "simple-bus";
+
+		internal-regs {
+			pinctrl: pinctrl@10000 {
+				compatible = "marvell,88f5182-pinctrl";
+				reg = <0x10000 0x8>, <0x10050 0x4>;
+
+				pmx_sata0: pmx-sata0 {
+					marvell,pins = "mpp12", "mpp14";
+					marvell,function = "sata0";
+				};
+
+				pmx_sata1: pmx-sata1 {
+					marvell,pins = "mpp13", "mpp15";
+					marvell,function = "sata1";
+				};
+			};
+
+			core_clk: core-clocks@10030 {
+				compatible = "marvell,mv88f5182-core-clock";
+				reg = <0x10010 0x4>;
+				#clock-cells = <1>;
+			};
+
+			mbusc: mbus-controller@20000 {
+				compatible = "marvell,mbus-controller";
+				reg = <0x20000 0x100>, <0x1500 0x20>;
+			};
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/orion5x-rd88f5182-nas.dts b/arch/arm/boot/dts/orion5x-rd88f5182-nas.dts
new file mode 100644
index 00000000000..6fb052507b3
--- /dev/null
+++ b/arch/arm/boot/dts/orion5x-rd88f5182-nas.dts
@@ -0,0 +1,177 @@
+/*
+ * Copyright (C) 2014 Thomas Petazzoni <thomas.petazzoni@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.
+ */
+
+/dts-v1/;
+
+#include <dt-bindings/gpio/gpio.h>
+#include "orion5x-mv88f5182.dtsi"
+
+/ {
+	model = "Marvell Reference Design 88F5182 NAS";
+	compatible = "marvell,rd-88f5182-nas", "marvell,orion5x-88f5182", "marvell,orion5x";
+
+	memory {
+		reg = <0x00000000 0x4000000>; /* 64 MB */
+	};
+
+	chosen {
+		bootargs = "console=ttyS0,115200n8 earlyprintk";
+		linux,stdout-path = &uart0;
+	};
+
+	soc {
+		ranges = <MBUS_ID(0xf0, 0x01) 0 0xf1000000 0x100000>,
+		         <MBUS_ID(0x09, 0x00) 0 0xf2200000 0x800>,
+			 <MBUS_ID(0x01, 0x0f) 0 0xf4000000 0x80000>,
+			 <MBUS_ID(0x01, 0x1d) 0 0xfc000000 0x1000000>;
+	};
+
+	gpio-leds {
+		compatible = "gpio-leds";
+		pinctrl-0 = <&pmx_debug_led>;
+		pinctrl-names = "default";
+
+		led@0 {
+			label = "rd88f5182:cpu";
+			linux,default-trigger = "heartbeat";
+			gpios = <&gpio0 0 GPIO_ACTIVE_HIGH>;
+		};
+	};
+};
+
+&devbus_bootcs {
+	status = "okay";
+
+	/* Read parameters */
+	devbus,bus-width    = <8>;
+	devbus,turn-off-ps  = <90000>;
+	devbus,badr-skew-ps = <0>;
+	devbus,acc-first-ps = <186000>;
+	devbus,acc-next-ps  = <186000>;
+
+	/* Write parameters */
+	devbus,wr-high-ps  = <90000>;
+	devbus,wr-low-ps   = <90000>;
+	devbus,ale-wr-ps   = <90000>;
+
+	flash@0 {
+		compatible = "cfi-flash";
+		reg = <0 0x80000>;
+		bank-width = <1>;
+	};
+};
+
+&devbus_cs1 {
+	status = "okay";
+
+	/* Read parameters */
+	devbus,bus-width    = <8>;
+	devbus,turn-off-ps  = <90000>;
+	devbus,badr-skew-ps = <0>;
+	devbus,acc-first-ps = <186000>;
+	devbus,acc-next-ps  = <186000>;
+
+	/* Write parameters */
+	devbus,wr-high-ps  = <90000>;
+	devbus,wr-low-ps   = <90000>;
+	devbus,ale-wr-ps   = <90000>;
+
+	flash@0 {
+		compatible = "cfi-flash";
+		reg = <0 0x1000000>;
+		bank-width = <1>;
+	};
+};
+
+&ehci0 {
+	status = "okay";
+};
+
+&ehci1 {
+	status = "okay";
+};
+
+&eth {
+	status = "okay";
+
+	ethernet-port@0 {
+		phy-handle = <&ethphy>;
+	};
+};
+
+&i2c {
+	status = "okay";
+	clock-frequency = <100000>;
+	#address-cells = <1>;
+
+	rtc@68 {
+		pinctrl-0 = <&pmx_rtc>;
+		pinctrl-names = "default";
+		compatible = "dallas,ds1338";
+		reg = <0x68>;
+	};
+};
+
+&mdio {
+	status = "okay";
+
+	ethphy: ethernet-phy {
+		reg = <8>;
+	};
+};
+
+&pinctrl {
+	pinctrl-0 = <&pmx_reset_switch &pmx_misc_gpios
+		&pmx_pci_gpios>;
+	pinctrl-names = "default";
+
+	/*
+	 * MPP[20] PCI Clock to MV88F5182
+	 * MPP[21] PCI Clock to mini PCI CON11
+	 * MPP[22] USB 0 over current indication
+	 * MPP[23] USB 1 over current indication
+	 * MPP[24] USB 1 over current enable
+	 * MPP[25] USB 0 over current enable
+	 */
+
+	pmx_debug_led: pmx-debug_led {
+		marvell,pins = "mpp0";
+		marvell,function = "gpio";
+	};
+
+	pmx_reset_switch: pmx-reset-switch {
+		marvell,pins = "mpp1";
+		marvell,function = "gpio";
+	};
+
+	pmx_rtc: pmx-rtc {
+		marvell,pins = "mpp3";
+		marvell,function = "gpio";
+	};
+
+	pmx_misc_gpios: pmx-misc-gpios {
+		marvell,pins = "mpp4", "mpp5";
+		marvell,function = "gpio";
+	};
+
+	pmx_pci_gpios: pmx-pci-gpios {
+		marvell,pins = "mpp6", "mpp7";
+		marvell,function = "gpio";
+	};
+};
+
+&sata {
+	pinctrl-0 = <&pmx_sata0 &pmx_sata1>;
+	pinctrl-names = "default";
+	status = "okay";
+	nr-ports = <2>;
+};
+
+&uart0 {
+	status = "okay";
+};
diff --git a/arch/arm/boot/dts/orion5x.dtsi b/arch/arm/boot/dts/orion5x.dtsi
index 174d89241f7..75cd01bd602 100644
--- a/arch/arm/boot/dts/orion5x.dtsi
+++ b/arch/arm/boot/dts/orion5x.dtsi
@@ -6,7 +6,9 @@
  * warranty of any kind, whether express or implied.
  */
 
-/include/ "skeleton.dtsi"
+#include "skeleton.dtsi"
+
+#define MBUS_ID(target,attributes) (((target) << 24) | ((attributes) << 16))
 
 / {
 	model = "Marvell Orion5x SoC";
@@ -17,149 +19,214 @@
 		gpio0 = &gpio0;
 	};
 
-	intc: interrupt-controller {
-		compatible = "marvell,orion-intc";
-		interrupt-controller;
-		#interrupt-cells = <1>;
-		reg = <0xf1020200 0x08>;
-	};
-
-	ocp@f1000000 {
-		compatible = "simple-bus";
-		ranges = <0x00000000 0xf1000000 0x4000000
-		          0xf2200000 0xf2200000 0x0000800>;
-		#address-cells = <1>;
+	soc {
+		#address-cells = <2>;
 		#size-cells = <1>;
+		controller = <&mbusc>;
 
-		gpio0: gpio@10100 {
-			compatible = "marvell,orion-gpio";
-			#gpio-cells = <2>;
-			gpio-controller;
-			reg = <0x10100 0x40>;
-			ngpios = <32>;
-			interrupt-controller;
-			#interrupt-cells = <2>;
-			interrupts = <6>, <7>, <8>, <9>;
-		};
-
-		spi@10600 {
-			compatible = "marvell,orion-spi";
+		devbus_bootcs: devbus-bootcs {
+			compatible = "marvell,orion-devbus";
+			reg = <MBUS_ID(0xf0, 0x01) 0x1046C 0x4>;
+			ranges = <0 MBUS_ID(0x01, 0x0f) 0 0xffffffff>;
 			#address-cells = <1>;
-			#size-cells = <0>;
-			cell-index = <0>;
-			reg = <0x10600 0x28>;
+			#size-cells = <1>;
+			clocks = <&core_clk 0>;
 			status = "disabled";
 		};
 
-		i2c@11000 {
-			compatible = "marvell,mv64xxx-i2c";
-			reg = <0x11000 0x20>;
+		devbus_cs0: devbus-cs0 {
+			compatible = "marvell,orion-devbus";
+			reg = <MBUS_ID(0xf0, 0x01) 0x1045C 0x4>;
+			ranges = <0 MBUS_ID(0x01, 0x1e) 0 0xffffffff>;
 			#address-cells = <1>;
-			#size-cells = <0>;
-			interrupts = <5>;
-			clock-frequency = <100000>;
+			#size-cells = <1>;
+			clocks = <&core_clk 0>;
 			status = "disabled";
 		};
 
-		serial@12000 {
-			compatible = "ns16550a";
-			reg = <0x12000 0x100>;
-			reg-shift = <2>;
-			interrupts = <3>;
-			/* set clock-frequency in board dts */
+		devbus_cs1: devbus-cs1 {
+			compatible = "marvell,orion-devbus";
+			reg = <MBUS_ID(0xf0, 0x01) 0x10460 0x4>;
+			ranges = <0 MBUS_ID(0x01, 0x1d) 0 0xffffffff>;
+			#address-cells = <1>;
+			#size-cells = <1>;
+			clocks = <&core_clk 0>;
 			status = "disabled";
 		};
 
-		serial@12100 {
-			compatible = "ns16550a";
-			reg = <0x12100 0x100>;
-			reg-shift = <2>;
-			interrupts = <4>;
-			/* set clock-frequency in board dts */
+		devbus_cs2: devbus-cs2 {
+			compatible = "marvell,orion-devbus";
+			reg = <MBUS_ID(0xf0, 0x01) 0x10464 0x4>;
+			ranges = <0 MBUS_ID(0x01, 0x1b) 0 0xffffffff>;
+			#address-cells = <1>;
+			#size-cells = <1>;
+			clocks = <&core_clk 0>;
 			status = "disabled";
 		};
 
-		wdt@20300 {
-			compatible = "marvell,orion-wdt";
-			reg = <0x20300 0x28>;
-			status = "okay";
-		};
+		internal-regs {
+			compatible = "simple-bus";
+			#address-cells = <1>;
+			#size-cells = <1>;
+			ranges = <0 MBUS_ID(0xf0, 0x01) 0 0x100000>;
+
+			gpio0: gpio@10100 {
+				compatible = "marvell,orion-gpio";
+				#gpio-cells = <2>;
+				gpio-controller;
+				reg = <0x10100 0x40>;
+				ngpios = <32>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				interrupts = <6>, <7>, <8>, <9>;
+			};
 
-		ehci@50000 {
-			compatible = "marvell,orion-ehci";
-			reg = <0x50000 0x1000>;
-			interrupts = <17>;
-			status = "disabled";
-		};
+			spi: spi@10600 {
+				compatible = "marvell,orion-spi";
+				#address-cells = <1>;
+				#size-cells = <0>;
+				cell-index = <0>;
+				reg = <0x10600 0x28>;
+				status = "disabled";
+			};
 
-		xor@60900 {
-			compatible = "marvell,orion-xor";
-			reg = <0x60900 0x100
-			       0x60b00 0x100>;
-			status = "okay";
+			i2c: i2c@11000 {
+				compatible = "marvell,mv64xxx-i2c";
+				reg = <0x11000 0x20>;
+				#address-cells = <1>;
+				#size-cells = <0>;
+				interrupts = <5>;
+				clocks = <&core_clk 0>;
+				status = "disabled";
+			};
 
-			xor00 {
-			      interrupts = <30>;
-			      dmacap,memcpy;
-			      dmacap,xor;
+			uart0: serial@12000 {
+				compatible = "ns16550a";
+				reg = <0x12000 0x100>;
+				reg-shift = <2>;
+				interrupts = <3>;
+				clocks = <&core_clk 0>;
+				status = "disabled";
 			};
-			xor01 {
-			      interrupts = <31>;
-			      dmacap,memcpy;
-			      dmacap,xor;
-			      dmacap,memset;
+
+			uart1: serial@12100 {
+				compatible = "ns16550a";
+				reg = <0x12100 0x100>;
+				reg-shift = <2>;
+				interrupts = <4>;
+				clocks = <&core_clk 0>;
+				status = "disabled";
 			};
-		};
 
-		eth: ethernet-controller@72000 {
-			compatible = "marvell,orion-eth";
-			#address-cells = <1>;
-			#size-cells = <0>;
-			reg = <0x72000 0x4000>;
-			marvell,tx-checksum-limit = <1600>;
-			status = "disabled";
+			bridge_intc: bridge-interrupt-ctrl@20110 {
+				compatible = "marvell,orion-bridge-intc";
+				interrupt-controller;
+				#interrupt-cells = <1>;
+				reg = <0x20110 0x8>;
+				interrupts = <0>;
+				marvell,#interrupts = <4>;
+			};
 
-			ethernet-port@0 {
-				compatible = "marvell,orion-eth-port";
-				reg = <0>;
-				/* overwrite MAC address in bootloader */
-				local-mac-address = [00 00 00 00 00 00];
-				/* set phy-handle property in board file */
+			intc: interrupt-controller@20200 {
+				compatible = "marvell,orion-intc";
+				interrupt-controller;
+				#interrupt-cells = <1>;
+				reg = <0x20200 0x08>;
 			};
-		};
 
-		mdio: mdio-bus@72004 {
-			compatible = "marvell,orion-mdio";
-			#address-cells = <1>;
-			#size-cells = <0>;
-			reg = <0x72004 0x84>;
-			interrupts = <22>;
-			status = "disabled";
+			timer: timer@20300 {
+				compatible = "marvell,orion-timer";
+				reg = <0x20300 0x20>;
+				interrupt-parent = <&bridge_intc>;
+				interrupts = <1>, <2>;
+				clocks = <&core_clk 0>;
+			};
 
-			/* add phy nodes in board file */
-		};
+			wdt: wdt@20300 {
+				compatible = "marvell,orion-wdt";
+				reg = <0x20300 0x28>;
+				interrupt-parent = <&bridge_intc>;
+				interrupts = <3>;
+				status = "okay";
+			};
 
-		sata@80000 {
-			compatible = "marvell,orion-sata";
-			reg = <0x80000 0x5000>;
-			interrupts = <29>;
-			status = "disabled";
+			ehci0: ehci@50000 {
+				compatible = "marvell,orion-ehci";
+				reg = <0x50000 0x1000>;
+				interrupts = <17>;
+				status = "disabled";
+			};
+
+			xor: dma-controller@60900 {
+				compatible = "marvell,orion-xor";
+				reg = <0x60900 0x100
+				       0x60b00 0x100>;
+				status = "okay";
+
+				xor00 {
+				      interrupts = <30>;
+				      dmacap,memcpy;
+				      dmacap,xor;
+				};
+				xor01 {
+				      interrupts = <31>;
+				      dmacap,memcpy;
+				      dmacap,xor;
+				      dmacap,memset;
+				};
+			};
+
+			eth: ethernet-controller@72000 {
+				compatible = "marvell,orion-eth";
+				#address-cells = <1>;
+				#size-cells = <0>;
+				reg = <0x72000 0x4000>;
+				marvell,tx-checksum-limit = <1600>;
+				status = "disabled";
+
+				ethport: ethernet-port@0 {
+					compatible = "marvell,orion-eth-port";
+					reg = <0>;
+					interrupts = <21>;
+					/* overwrite MAC address in bootloader */
+					local-mac-address = [00 00 00 00 00 00];
+					/* set phy-handle property in board file */
+				};
+			};
+
+			mdio: mdio-bus@72004 {
+				compatible = "marvell,orion-mdio";
+				#address-cells = <1>;
+				#size-cells = <0>;
+				reg = <0x72004 0x84>;
+				interrupts = <22>;
+				status = "disabled";
+
+				/* add phy nodes in board file */
+			};
+
+			sata: sata@80000 {
+				compatible = "marvell,orion-sata";
+				reg = <0x80000 0x5000>;
+				interrupts = <29>;
+				status = "disabled";
+			};
+
+			ehci1: ehci@a0000 {
+				compatible = "marvell,orion-ehci";
+				reg = <0xa0000 0x1000>;
+				interrupts = <12>;
+				status = "disabled";
+			};
 		};
 
-		crypto@90000 {
+		cesa: crypto@90000 {
 			compatible = "marvell,orion-crypto";
-			reg = <0x90000 0x10000>,
-			      <0xf2200000 0x800>;
+			reg = <MBUS_ID(0xf0, 0x01) 0x90000 0x10000>,
+			      <MBUS_ID(0x09, 0x00) 0x0 0x800>;
 			reg-names = "regs", "sram";
 			interrupts = <28>;
 			status = "okay";
 		};
-
-		ehci@a0000 {
-			compatible = "marvell,orion-ehci";
-			reg = <0xa0000 0x1000>;
-			interrupts = <12>;
-			status = "disabled";
-		};
 	};
 };
diff --git a/arch/arm/boot/dts/prima2.dtsi b/arch/arm/boot/dts/prima2.dtsi
index 1e82571d682..3df7ba86028 100644
--- a/arch/arm/boot/dts/prima2.dtsi
+++ b/arch/arm/boot/dts/prima2.dtsi
@@ -201,6 +201,7 @@
 				compatible = "sirf,prima2-tick";
 				reg = <0xb0020000 0x1000>;
 				interrupts = <0>;
+				clocks = <&clks 11>;
 			};
 
 			nand@b0030000 {
@@ -313,8 +314,9 @@
 				reg = <0xb00d0000 0x10000>;
 				interrupts = <15>;
 				sirf,spi-num-chipselects = <1>;
-				sirf,spi-dma-rx-channel = <25>;
-				sirf,spi-dma-tx-channel = <20>;
+				dmas = <&dmac1 9>,
+				     <&dmac1 4>;
+				dma-names = "rx", "tx";
 				#address-cells = <1>;
 				#size-cells = <0>;
 				clocks = <&clks 19>;
@@ -327,8 +329,9 @@
 				reg = <0xb0170000 0x10000>;
 				interrupts = <16>;
 				sirf,spi-num-chipselects = <1>;
-				sirf,spi-dma-rx-channel = <12>;
-				sirf,spi-dma-tx-channel = <13>;
+				dmas = <&dmac0 12>,
+				     <&dmac0 13>;
+				dma-names = "rx", "tx";
 				#address-cells = <1>;
 				#size-cells = <0>;
 				clocks = <&clks 20>;
diff --git a/arch/arm/boot/dts/qcom-apq8064-ifc6410.dts b/arch/arm/boot/dts/qcom-apq8064-ifc6410.dts
new file mode 100644
index 00000000000..7c2441d526b
--- /dev/null
+++ b/arch/arm/boot/dts/qcom-apq8064-ifc6410.dts
@@ -0,0 +1,16 @@
+#include "qcom-apq8064-v2.0.dtsi"
+
+/ {
+	model = "Qualcomm APQ8064/IFC6410";
+	compatible = "qcom,apq8064-ifc6410", "qcom,apq8064";
+
+	soc {
+		gsbi@16600000 {
+			status = "ok";
+			qcom,mode = <GSBI_PROT_I2C_UART>;
+			serial@16640000 {
+				status = "ok";
+			};
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/qcom-apq8064-v2.0.dtsi b/arch/arm/boot/dts/qcom-apq8064-v2.0.dtsi
new file mode 100644
index 00000000000..935c3945fc5
--- /dev/null
+++ b/arch/arm/boot/dts/qcom-apq8064-v2.0.dtsi
@@ -0,0 +1 @@
+#include "qcom-apq8064.dtsi"
diff --git a/arch/arm/boot/dts/qcom-apq8064.dtsi b/arch/arm/boot/dts/qcom-apq8064.dtsi
new file mode 100644
index 00000000000..92bf793622c
--- /dev/null
+++ b/arch/arm/boot/dts/qcom-apq8064.dtsi
@@ -0,0 +1,170 @@
+/dts-v1/;
+
+#include "skeleton.dtsi"
+#include <dt-bindings/clock/qcom,gcc-msm8960.h>
+#include <dt-bindings/soc/qcom,gsbi.h>
+
+/ {
+	model = "Qualcomm APQ8064";
+	compatible = "qcom,apq8064";
+	interrupt-parent = <&intc>;
+
+	cpus {
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		cpu@0 {
+			compatible = "qcom,krait";
+			enable-method = "qcom,kpss-acc-v1";
+			device_type = "cpu";
+			reg = <0>;
+			next-level-cache = <&L2>;
+			qcom,acc = <&acc0>;
+			qcom,saw = <&saw0>;
+		};
+
+		cpu@1 {
+			compatible = "qcom,krait";
+			enable-method = "qcom,kpss-acc-v1";
+			device_type = "cpu";
+			reg = <1>;
+			next-level-cache = <&L2>;
+			qcom,acc = <&acc1>;
+			qcom,saw = <&saw1>;
+		};
+
+		cpu@2 {
+			compatible = "qcom,krait";
+			enable-method = "qcom,kpss-acc-v1";
+			device_type = "cpu";
+			reg = <2>;
+			next-level-cache = <&L2>;
+			qcom,acc = <&acc2>;
+			qcom,saw = <&saw2>;
+		};
+
+		cpu@3 {
+			compatible = "qcom,krait";
+			enable-method = "qcom,kpss-acc-v1";
+			device_type = "cpu";
+			reg = <3>;
+			next-level-cache = <&L2>;
+			qcom,acc = <&acc3>;
+			qcom,saw = <&saw3>;
+		};
+
+		L2: l2-cache {
+			compatible = "cache";
+			cache-level = <2>;
+		};
+	};
+
+	cpu-pmu {
+		compatible = "qcom,krait-pmu";
+		interrupts = <1 10 0x304>;
+	};
+
+	soc: soc {
+		#address-cells = <1>;
+		#size-cells = <1>;
+		ranges;
+		compatible = "simple-bus";
+
+		intc: interrupt-controller@2000000 {
+			compatible = "qcom,msm-qgic2";
+			interrupt-controller;
+			#interrupt-cells = <3>;
+			reg = <0x02000000 0x1000>,
+			      <0x02002000 0x1000>;
+		};
+
+		timer@200a000 {
+			compatible = "qcom,kpss-timer", "qcom,msm-timer";
+			interrupts = <1 1 0x301>,
+				     <1 2 0x301>,
+				     <1 3 0x301>;
+			reg = <0x0200a000 0x100>;
+			clock-frequency = <27000000>,
+					  <32768>;
+			cpu-offset = <0x80000>;
+		};
+
+		acc0: clock-controller@2088000 {
+			compatible = "qcom,kpss-acc-v1";
+			reg = <0x02088000 0x1000>, <0x02008000 0x1000>;
+		};
+
+		acc1: clock-controller@2098000 {
+			compatible = "qcom,kpss-acc-v1";
+			reg = <0x02098000 0x1000>, <0x02008000 0x1000>;
+		};
+
+		acc2: clock-controller@20a8000 {
+			compatible = "qcom,kpss-acc-v1";
+			reg = <0x020a8000 0x1000>, <0x02008000 0x1000>;
+		};
+
+		acc3: clock-controller@20b8000 {
+			compatible = "qcom,kpss-acc-v1";
+			reg = <0x020b8000 0x1000>, <0x02008000 0x1000>;
+		};
+
+		saw0: regulator@2089000 {
+			compatible = "qcom,saw2";
+			reg = <0x02089000 0x1000>, <0x02009000 0x1000>;
+			regulator;
+		};
+
+		saw1: regulator@2099000 {
+			compatible = "qcom,saw2";
+			reg = <0x02099000 0x1000>, <0x02009000 0x1000>;
+			regulator;
+		};
+
+		saw2: regulator@20a9000 {
+			compatible = "qcom,saw2";
+			reg = <0x020a9000 0x1000>, <0x02009000 0x1000>;
+			regulator;
+		};
+
+		saw3: regulator@20b9000 {
+			compatible = "qcom,saw2";
+			reg = <0x020b9000 0x1000>, <0x02009000 0x1000>;
+			regulator;
+		};
+
+		gsbi7: gsbi@16600000 {
+			status = "disabled";
+			compatible = "qcom,gsbi-v1.0.0";
+			reg = <0x16600000 0x100>;
+			clocks = <&gcc GSBI7_H_CLK>;
+			clock-names = "iface";
+			#address-cells = <1>;
+			#size-cells = <1>;
+			ranges;
+
+			serial@16640000 {
+				compatible = "qcom,msm-uartdm-v1.3", "qcom,msm-uartdm";
+				reg = <0x16640000 0x1000>,
+				      <0x16600000 0x1000>;
+				interrupts = <0 158 0x0>;
+				clocks = <&gcc GSBI7_UART_CLK>, <&gcc GSBI7_H_CLK>;
+				clock-names = "core", "iface";
+				status = "disabled";
+			};
+		};
+
+		qcom,ssbi@500000 {
+			compatible = "qcom,ssbi";
+			reg = <0x00500000 0x1000>;
+			qcom,controller-type = "pmic-arbiter";
+		};
+
+		gcc: clock-controller@900000 {
+			compatible = "qcom,gcc-apq8064";
+			reg = <0x00900000 0x4000>;
+			#clock-cells = <1>;
+			#reset-cells = <1>;
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/qcom-apq8074-dragonboard.dts b/arch/arm/boot/dts/qcom-apq8074-dragonboard.dts
index 13ac3e22249..b4dfb01fe6f 100644
--- a/arch/arm/boot/dts/qcom-apq8074-dragonboard.dts
+++ b/arch/arm/boot/dts/qcom-apq8074-dragonboard.dts
@@ -3,4 +3,43 @@
 / {
 	model = "Qualcomm APQ8074 Dragonboard";
 	compatible = "qcom,apq8074-dragonboard", "qcom,apq8074";
+
+	soc {
+		serial@f991e000 {
+			status = "ok";
+		};
+
+		sdhci@f9824900 {
+			bus-width = <8>;
+			non-removable;
+			status = "ok";
+		};
+
+		sdhci@f98a4900 {
+			cd-gpios = <&msmgpio 62 0x1>;
+			bus-width = <4>;
+		};
+
+
+		pinctrl@fd510000 {
+			spi8_default: spi8_default {
+				mosi {
+					pins = "gpio45";
+					function = "blsp_spi8";
+				};
+				miso {
+					pins = "gpio46";
+					function = "blsp_spi8";
+				};
+				cs {
+					pins = "gpio47";
+					function = "blsp_spi8";
+				};
+				clk {
+					pins = "gpio48";
+					function = "blsp_spi8";
+				};
+			};
+		};
+	};
 };
diff --git a/arch/arm/boot/dts/qcom-apq8084-mtp.dts b/arch/arm/boot/dts/qcom-apq8084-mtp.dts
new file mode 100644
index 00000000000..9dae3878b71
--- /dev/null
+++ b/arch/arm/boot/dts/qcom-apq8084-mtp.dts
@@ -0,0 +1,6 @@
+#include "qcom-apq8084.dtsi"
+
+/ {
+	model = "Qualcomm APQ 8084-MTP";
+	compatible = "qcom,apq8084-mtp", "qcom,apq8084";
+};
diff --git a/arch/arm/boot/dts/qcom-apq8084.dtsi b/arch/arm/boot/dts/qcom-apq8084.dtsi
new file mode 100644
index 00000000000..e3e009a5912
--- /dev/null
+++ b/arch/arm/boot/dts/qcom-apq8084.dtsi
@@ -0,0 +1,179 @@
+/dts-v1/;
+
+#include "skeleton.dtsi"
+
+/ {
+	model = "Qualcomm APQ 8084";
+	compatible = "qcom,apq8084";
+	interrupt-parent = <&intc>;
+
+	cpus {
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		cpu@0 {
+			device_type = "cpu";
+			compatible = "qcom,krait";
+			reg = <0>;
+			enable-method = "qcom,kpss-acc-v2";
+			next-level-cache = <&L2>;
+			qcom,acc = <&acc0>;
+		};
+
+		cpu@1 {
+			device_type = "cpu";
+			compatible = "qcom,krait";
+			reg = <1>;
+			enable-method = "qcom,kpss-acc-v2";
+			next-level-cache = <&L2>;
+			qcom,acc = <&acc1>;
+		};
+
+		cpu@2 {
+			device_type = "cpu";
+			compatible = "qcom,krait";
+			reg = <2>;
+			enable-method = "qcom,kpss-acc-v2";
+			next-level-cache = <&L2>;
+			qcom,acc = <&acc2>;
+		};
+
+		cpu@3 {
+			device_type = "cpu";
+			compatible = "qcom,krait";
+			reg = <3>;
+			enable-method = "qcom,kpss-acc-v2";
+			next-level-cache = <&L2>;
+			qcom,acc = <&acc3>;
+		};
+
+		L2: l2-cache {
+			compatible = "qcom,arch-cache";
+			cache-level = <2>;
+			qcom,saw = <&saw_l2>;
+		};
+	};
+
+	cpu-pmu {
+		compatible = "qcom,krait-pmu";
+		interrupts = <1 7 0xf04>;
+	};
+
+	timer {
+		compatible = "arm,armv7-timer";
+		interrupts = <1 2 0xf08>,
+			     <1 3 0xf08>,
+			     <1 4 0xf08>,
+			     <1 1 0xf08>;
+		clock-frequency = <19200000>;
+	};
+
+	soc: soc {
+		#address-cells = <1>;
+		#size-cells = <1>;
+		ranges;
+		compatible = "simple-bus";
+
+		intc: interrupt-controller@f9000000 {
+			compatible = "qcom,msm-qgic2";
+			interrupt-controller;
+			#interrupt-cells = <3>;
+			reg = <0xf9000000 0x1000>,
+			      <0xf9002000 0x1000>;
+		};
+
+		timer@f9020000 {
+			#address-cells = <1>;
+			#size-cells = <1>;
+			ranges;
+			compatible = "arm,armv7-timer-mem";
+			reg = <0xf9020000 0x1000>;
+			clock-frequency = <19200000>;
+
+			frame@f9021000 {
+				frame-number = <0>;
+				interrupts = <0 8 0x4>,
+					     <0 7 0x4>;
+				reg = <0xf9021000 0x1000>,
+				      <0xf9022000 0x1000>;
+			};
+
+			frame@f9023000 {
+				frame-number = <1>;
+				interrupts = <0 9 0x4>;
+				reg = <0xf9023000 0x1000>;
+				status = "disabled";
+			};
+
+			frame@f9024000 {
+				frame-number = <2>;
+				interrupts = <0 10 0x4>;
+				reg = <0xf9024000 0x1000>;
+				status = "disabled";
+			};
+
+			frame@f9025000 {
+				frame-number = <3>;
+				interrupts = <0 11 0x4>;
+				reg = <0xf9025000 0x1000>;
+				status = "disabled";
+			};
+
+			frame@f9026000 {
+				frame-number = <4>;
+				interrupts = <0 12 0x4>;
+				reg = <0xf9026000 0x1000>;
+				status = "disabled";
+			};
+
+			frame@f9027000 {
+				frame-number = <5>;
+				interrupts = <0 13 0x4>;
+				reg = <0xf9027000 0x1000>;
+				status = "disabled";
+			};
+
+			frame@f9028000 {
+				frame-number = <6>;
+				interrupts = <0 14 0x4>;
+				reg = <0xf9028000 0x1000>;
+				status = "disabled";
+			};
+		};
+
+		saw_l2: regulator@f9012000 {
+			compatible = "qcom,saw2";
+			reg = <0xf9012000 0x1000>;
+			regulator;
+		};
+
+		acc0: clock-controller@f9088000 {
+			compatible = "qcom,kpss-acc-v2";
+			reg = <0xf9088000 0x1000>,
+			      <0xf9008000 0x1000>;
+		};
+
+		acc1: clock-controller@f9098000 {
+			compatible = "qcom,kpss-acc-v2";
+			reg = <0xf9098000 0x1000>,
+			      <0xf9008000 0x1000>;
+		};
+
+		acc2: clock-controller@f90a8000 {
+			compatible = "qcom,kpss-acc-v2";
+			reg = <0xf90a8000 0x1000>,
+			      <0xf9008000 0x1000>;
+		};
+
+		acc3: clock-controller@f90b8000 {
+			compatible = "qcom,kpss-acc-v2";
+			reg = <0xf90b8000 0x1000>,
+			      <0xf9008000 0x1000>;
+		};
+
+		restart@fc4ab000 {
+			compatible = "qcom,pshold";
+			reg = <0xfc4ab000 0x4>;
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/qcom-msm8660-surf.dts b/arch/arm/boot/dts/qcom-msm8660-surf.dts
index 169bad90dac..45180adfadf 100644
--- a/arch/arm/boot/dts/qcom-msm8660-surf.dts
+++ b/arch/arm/boot/dts/qcom-msm8660-surf.dts
@@ -3,4 +3,14 @@
 / {
 	model = "Qualcomm MSM8660 SURF";
 	compatible = "qcom,msm8660-surf", "qcom,msm8660";
+
+	soc {
+		gsbi@19c00000 {
+			status = "ok";
+			qcom,mode = <GSBI_PROT_I2C_UART>;
+			serial@19c40000 {
+				status = "ok";
+			};
+		};
+	};
 };
diff --git a/arch/arm/boot/dts/qcom-msm8660.dtsi b/arch/arm/boot/dts/qcom-msm8660.dtsi
index c52a9e964a4..53837aaa2f7 100644
--- a/arch/arm/boot/dts/qcom-msm8660.dtsi
+++ b/arch/arm/boot/dts/qcom-msm8660.dtsi
@@ -3,6 +3,7 @@
 /include/ "skeleton.dtsi"
 
 #include <dt-bindings/clock/qcom,gcc-msm8660.h>
+#include <dt-bindings/soc/qcom,gsbi.h>
 
 / {
 	model = "Qualcomm MSM8660";
@@ -12,16 +13,18 @@
 	cpus {
 		#address-cells = <1>;
 		#size-cells = <0>;
-		compatible = "qcom,scorpion";
-		enable-method = "qcom,gcc-msm8660";
 
 		cpu@0 {
+			compatible = "qcom,scorpion";
+			enable-method = "qcom,gcc-msm8660";
 			device_type = "cpu";
 			reg = <0>;
 			next-level-cache = <&L2>;
 		};
 
 		cpu@1 {
+			compatible = "qcom,scorpion";
+			enable-method = "qcom,gcc-msm8660";
 			device_type = "cpu";
 			reg = <1>;
 			next-level-cache = <&L2>;
@@ -33,55 +36,73 @@
 		};
 	};
 
-	intc: interrupt-controller@2080000 {
-		compatible = "qcom,msm-8660-qgic";
-		interrupt-controller;
-		#interrupt-cells = <3>;
-		reg = < 0x02080000 0x1000 >,
-		      < 0x02081000 0x1000 >;
-	};
+	soc: soc {
+		#address-cells = <1>;
+		#size-cells = <1>;
+		ranges;
+		compatible = "simple-bus";
 
-	timer@2000000 {
-		compatible = "qcom,scss-timer", "qcom,msm-timer";
-		interrupts = <1 0 0x301>,
-			     <1 1 0x301>,
-			     <1 2 0x301>;
-		reg = <0x02000000 0x100>;
-		clock-frequency = <27000000>,
-				  <32768>;
-		cpu-offset = <0x40000>;
-	};
+		intc: interrupt-controller@2080000 {
+			compatible = "qcom,msm-8660-qgic";
+			interrupt-controller;
+			#interrupt-cells = <3>;
+			reg = < 0x02080000 0x1000 >,
+			      < 0x02081000 0x1000 >;
+		};
 
-	msmgpio: gpio@800000 {
-		compatible = "qcom,msm-gpio";
-		reg = <0x00800000 0x4000>;
-		gpio-controller;
-		#gpio-cells = <2>;
-		ngpio = <173>;
-		interrupts = <0 16 0x4>;
-		interrupt-controller;
-		#interrupt-cells = <2>;
-	};
+		timer@2000000 {
+			compatible = "qcom,scss-timer", "qcom,msm-timer";
+			interrupts = <1 0 0x301>,
+				     <1 1 0x301>,
+				     <1 2 0x301>;
+			reg = <0x02000000 0x100>;
+			clock-frequency = <27000000>,
+					  <32768>;
+			cpu-offset = <0x40000>;
+		};
 
-	gcc: clock-controller@900000 {
-		compatible = "qcom,gcc-msm8660";
-		#clock-cells = <1>;
-		#reset-cells = <1>;
-		reg = <0x900000 0x4000>;
-	};
+		msmgpio: gpio@800000 {
+			compatible = "qcom,msm-gpio";
+			reg = <0x00800000 0x4000>;
+			gpio-controller;
+			#gpio-cells = <2>;
+			ngpio = <173>;
+			interrupts = <0 16 0x4>;
+			interrupt-controller;
+			#interrupt-cells = <2>;
+		};
 
-	serial@19c40000 {
-		compatible = "qcom,msm-uartdm-v1.3", "qcom,msm-uartdm";
-		reg = <0x19c40000 0x1000>,
-		      <0x19c00000 0x1000>;
-		interrupts = <0 195 0x0>;
-		clocks = <&gcc GSBI12_UART_CLK>, <&gcc GSBI12_H_CLK>;
-		clock-names = "core", "iface";
-	};
+		gcc: clock-controller@900000 {
+			compatible = "qcom,gcc-msm8660";
+			#clock-cells = <1>;
+			#reset-cells = <1>;
+			reg = <0x900000 0x4000>;
+		};
+
+		gsbi12: gsbi@19c00000 {
+			compatible = "qcom,gsbi-v1.0.0";
+			reg = <0x19c00000 0x100>;
+			clocks = <&gcc GSBI12_H_CLK>;
+			clock-names = "iface";
+			#address-cells = <1>;
+			#size-cells = <1>;
+			ranges;
 
-	qcom,ssbi@500000 {
-		compatible = "qcom,ssbi";
-		reg = <0x500000 0x1000>;
-		qcom,controller-type = "pmic-arbiter";
+			serial@19c40000 {
+				compatible = "qcom,msm-uartdm-v1.3", "qcom,msm-uartdm";
+				reg = <0x19c40000 0x1000>,
+				      <0x19c00000 0x1000>;
+				interrupts = <0 195 0x0>;
+				clocks = <&gcc GSBI12_UART_CLK>, <&gcc GSBI12_H_CLK>;
+				clock-names = "core", "iface";
+				status = "disabled";
+			};
+		};
+
+		qcom,ssbi@500000 {
+			compatible = "qcom,ssbi";
+			reg = <0x500000 0x1000>;
+			qcom,controller-type = "pmic-arbiter";
+		};
 	};
 };
diff --git a/arch/arm/boot/dts/qcom-msm8960-cdp.dts b/arch/arm/boot/dts/qcom-msm8960-cdp.dts
index a58fb88315f..8f75cc4c834 100644
--- a/arch/arm/boot/dts/qcom-msm8960-cdp.dts
+++ b/arch/arm/boot/dts/qcom-msm8960-cdp.dts
@@ -3,4 +3,14 @@
 / {
 	model = "Qualcomm MSM8960 CDP";
 	compatible = "qcom,msm8960-cdp", "qcom,msm8960";
+
+	soc {
+		gsbi@16400000 {
+			status = "ok";
+			qcom,mode = <GSBI_PROT_I2C_UART>;
+			serial@16440000 {
+				status = "ok";
+			};
+		};
+	};
 };
diff --git a/arch/arm/boot/dts/qcom-msm8960.dtsi b/arch/arm/boot/dts/qcom-msm8960.dtsi
index 997b7b94e11..5303e53e34d 100644
--- a/arch/arm/boot/dts/qcom-msm8960.dtsi
+++ b/arch/arm/boot/dts/qcom-msm8960.dtsi
@@ -3,6 +3,7 @@
 /include/ "skeleton.dtsi"
 
 #include <dt-bindings/clock/qcom,gcc-msm8960.h>
+#include <dt-bindings/soc/qcom,gsbi.h>
 
 / {
 	model = "Qualcomm MSM8960";
@@ -13,10 +14,10 @@
 		#address-cells = <1>;
 		#size-cells = <0>;
 		interrupts = <1 14 0x304>;
-		compatible = "qcom,krait";
-		enable-method = "qcom,kpss-acc-v1";
 
 		cpu@0 {
+			compatible = "qcom,krait";
+			enable-method = "qcom,kpss-acc-v1";
 			device_type = "cpu";
 			reg = <0>;
 			next-level-cache = <&L2>;
@@ -25,6 +26,8 @@
 		};
 
 		cpu@1 {
+			compatible = "qcom,krait";
+			enable-method = "qcom,kpss-acc-v1";
 			device_type = "cpu";
 			reg = <1>;
 			next-level-cache = <&L2>;
@@ -35,7 +38,6 @@
 		L2: l2-cache {
 			compatible = "cache";
 			cache-level = <2>;
-			interrupts = <0 2 0x4>;
 		};
 	};
 
@@ -45,91 +47,109 @@
 		qcom,no-pc-write;
 	};
 
-	intc: interrupt-controller@2000000 {
-		compatible = "qcom,msm-qgic2";
-		interrupt-controller;
-		#interrupt-cells = <3>;
-		reg = < 0x02000000 0x1000 >,
-		      < 0x02002000 0x1000 >;
-	};
+	soc: soc {
+		#address-cells = <1>;
+		#size-cells = <1>;
+		ranges;
+		compatible = "simple-bus";
+
+		intc: interrupt-controller@2000000 {
+			compatible = "qcom,msm-qgic2";
+			interrupt-controller;
+			#interrupt-cells = <3>;
+			reg = <0x02000000 0x1000>,
+			      <0x02002000 0x1000>;
+		};
 
-	timer@200a000 {
-		compatible = "qcom,kpss-timer", "qcom,msm-timer";
-		interrupts = <1 1 0x301>,
-			     <1 2 0x301>,
-			     <1 3 0x301>;
-		reg = <0x0200a000 0x100>;
-		clock-frequency = <27000000>,
-				  <32768>;
-		cpu-offset = <0x80000>;
-	};
+		timer@200a000 {
+			compatible = "qcom,kpss-timer", "qcom,msm-timer";
+			interrupts = <1 1 0x301>,
+				     <1 2 0x301>,
+				     <1 3 0x301>;
+			reg = <0x0200a000 0x100>;
+			clock-frequency = <27000000>,
+					  <32768>;
+			cpu-offset = <0x80000>;
+		};
 
-	msmgpio: gpio@800000 {
-		compatible = "qcom,msm-gpio";
-		gpio-controller;
-		#gpio-cells = <2>;
-		ngpio = <150>;
-		interrupts = <0 16 0x4>;
-		interrupt-controller;
-		#interrupt-cells = <2>;
-		reg = <0x800000 0x4000>;
-	};
+		msmgpio: gpio@800000 {
+			compatible = "qcom,msm-gpio";
+			gpio-controller;
+			#gpio-cells = <2>;
+			ngpio = <150>;
+			interrupts = <0 16 0x4>;
+			interrupt-controller;
+			#interrupt-cells = <2>;
+			reg = <0x800000 0x4000>;
+		};
 
-	gcc: clock-controller@900000 {
-		compatible = "qcom,gcc-msm8960";
-		#clock-cells = <1>;
-		#reset-cells = <1>;
-		reg = <0x900000 0x4000>;
-	};
+		gcc: clock-controller@900000 {
+			compatible = "qcom,gcc-msm8960";
+			#clock-cells = <1>;
+			#reset-cells = <1>;
+			reg = <0x900000 0x4000>;
+		};
 
-	clock-controller@4000000 {
-		compatible = "qcom,mmcc-msm8960";
-		reg = <0x4000000 0x1000>;
-		#clock-cells = <1>;
-		#reset-cells = <1>;
-	};
+		clock-controller@4000000 {
+			compatible = "qcom,mmcc-msm8960";
+			reg = <0x4000000 0x1000>;
+			#clock-cells = <1>;
+			#reset-cells = <1>;
+		};
 
-	acc0: clock-controller@2088000 {
-		compatible = "qcom,kpss-acc-v1";
-		reg = <0x02088000 0x1000>, <0x02008000 0x1000>;
-	};
+		acc0: clock-controller@2088000 {
+			compatible = "qcom,kpss-acc-v1";
+			reg = <0x02088000 0x1000>, <0x02008000 0x1000>;
+		};
 
-	acc1: clock-controller@2098000 {
-		compatible = "qcom,kpss-acc-v1";
-		reg = <0x02098000 0x1000>, <0x02008000 0x1000>;
-	};
+		acc1: clock-controller@2098000 {
+			compatible = "qcom,kpss-acc-v1";
+			reg = <0x02098000 0x1000>, <0x02008000 0x1000>;
+		};
 
-	saw0: regulator@2089000 {
-		compatible = "qcom,saw2";
-		reg = <0x02089000 0x1000>, <0x02009000 0x1000>;
-		regulator;
-	};
+		saw0: regulator@2089000 {
+			compatible = "qcom,saw2";
+			reg = <0x02089000 0x1000>, <0x02009000 0x1000>;
+			regulator;
+		};
 
-	saw1: regulator@2099000 {
-		compatible = "qcom,saw2";
-		reg = <0x02099000 0x1000>, <0x02009000 0x1000>;
-		regulator;
-	};
+		saw1: regulator@2099000 {
+			compatible = "qcom,saw2";
+			reg = <0x02099000 0x1000>, <0x02009000 0x1000>;
+			regulator;
+		};
 
-	serial@16440000 {
-		compatible = "qcom,msm-uartdm-v1.3", "qcom,msm-uartdm";
-		reg = <0x16440000 0x1000>,
-		      <0x16400000 0x1000>;
-		interrupts = <0 154 0x0>;
-		clocks = <&gcc GSBI5_UART_CLK>, <&gcc GSBI5_H_CLK>;
-		clock-names = "core", "iface";
-	};
+		gsbi5: gsbi@16400000 {
+			compatible = "qcom,gsbi-v1.0.0";
+			reg = <0x16400000 0x100>;
+			clocks = <&gcc GSBI5_H_CLK>;
+			clock-names = "iface";
+			#address-cells = <1>;
+			#size-cells = <1>;
+			ranges;
+
+			serial@16440000 {
+				compatible = "qcom,msm-uartdm-v1.3", "qcom,msm-uartdm";
+				reg = <0x16440000 0x1000>,
+				      <0x16400000 0x1000>;
+				interrupts = <0 154 0x0>;
+				clocks = <&gcc GSBI5_UART_CLK>, <&gcc GSBI5_H_CLK>;
+				clock-names = "core", "iface";
+				status = "disabled";
+			};
+		};
 
-	qcom,ssbi@500000 {
-		compatible = "qcom,ssbi";
-		reg = <0x500000 0x1000>;
-		qcom,controller-type = "pmic-arbiter";
-	};
+		qcom,ssbi@500000 {
+			compatible = "qcom,ssbi";
+			reg = <0x500000 0x1000>;
+			qcom,controller-type = "pmic-arbiter";
+		};
 
-	rng@1a500000 {
-		compatible = "qcom,prng";
-		reg = <0x1a500000 0x200>;
-		clocks = <&gcc PRNG_CLK>;
-		clock-names = "core";
+		rng@1a500000 {
+			compatible = "qcom,prng";
+			reg = <0x1a500000 0x200>;
+			clocks = <&gcc PRNG_CLK>;
+			clock-names = "core";
+		};
 	};
 };
diff --git a/arch/arm/boot/dts/qcom-msm8974.dtsi b/arch/arm/boot/dts/qcom-msm8974.dtsi
index f68723918b3..69dca2aca25 100644
--- a/arch/arm/boot/dts/qcom-msm8974.dtsi
+++ b/arch/arm/boot/dts/qcom-msm8974.dtsi
@@ -13,10 +13,10 @@
 		#address-cells = <1>;
 		#size-cells = <0>;
 		interrupts = <1 9 0xf04>;
-		compatible = "qcom,krait";
-		enable-method = "qcom,kpss-acc-v2";
 
 		cpu@0 {
+			compatible = "qcom,krait";
+			enable-method = "qcom,kpss-acc-v2";
 			device_type = "cpu";
 			reg = <0>;
 			next-level-cache = <&L2>;
@@ -24,6 +24,8 @@
 		};
 
 		cpu@1 {
+			compatible = "qcom,krait";
+			enable-method = "qcom,kpss-acc-v2";
 			device_type = "cpu";
 			reg = <1>;
 			next-level-cache = <&L2>;
@@ -31,6 +33,8 @@
 		};
 
 		cpu@2 {
+			compatible = "qcom,krait";
+			enable-method = "qcom,kpss-acc-v2";
 			device_type = "cpu";
 			reg = <2>;
 			next-level-cache = <&L2>;
@@ -38,6 +42,8 @@
 		};
 
 		cpu@3 {
+			compatible = "qcom,krait";
+			enable-method = "qcom,kpss-acc-v2";
 			device_type = "cpu";
 			reg = <3>;
 			next-level-cache = <&L2>;
@@ -47,7 +53,6 @@
 		L2: l2-cache {
 			compatible = "cache";
 			cache-level = <2>;
-			interrupts = <0 2 0x4>;
 			qcom,saw = <&saw_l2>;
 		};
 	};
@@ -57,6 +62,15 @@
 		interrupts = <1 7 0xf04>;
 	};
 
+	timer {
+		compatible = "arm,armv7-timer";
+		interrupts = <1 2 0xf08>,
+			     <1 3 0xf08>,
+			     <1 4 0xf08>,
+			     <1 1 0xf08>;
+		clock-frequency = <19200000>;
+	};
+
 	soc: soc {
 		#address-cells = <1>;
 		#size-cells = <1>;
@@ -71,15 +85,6 @@
 			      <0xf9002000 0x1000>;
 		};
 
-		timer {
-			compatible = "arm,armv7-timer";
-			interrupts = <1 2 0xf08>,
-				     <1 3 0xf08>,
-				     <1 4 0xf08>,
-				     <1 1 0xf08>;
-			clock-frequency = <19200000>;
-		};
-
 		timer@f9020000 {
 			#address-cells = <1>;
 			#size-cells = <1>;
@@ -190,6 +195,29 @@
 			interrupts = <0 108 0x0>;
 			clocks = <&gcc GCC_BLSP1_UART2_APPS_CLK>, <&gcc GCC_BLSP1_AHB_CLK>;
 			clock-names = "core", "iface";
+			status = "disabled";
+		};
+
+		sdhci@f9824900 {
+			compatible = "qcom,sdhci-msm-v4";
+			reg = <0xf9824900 0x11c>, <0xf9824000 0x800>;
+			reg-names = "hc_mem", "core_mem";
+			interrupts = <0 123 0>, <0 138 0>;
+			interrupt-names = "hc_irq", "pwr_irq";
+			clocks = <&gcc GCC_SDCC1_APPS_CLK>, <&gcc GCC_SDCC1_AHB_CLK>;
+			clock-names = "core", "iface";
+			status = "disabled";
+		};
+
+		sdhci@f98a4900 {
+			compatible = "qcom,sdhci-msm-v4";
+			reg = <0xf98a4900 0x11c>, <0xf98a4000 0x800>;
+			reg-names = "hc_mem", "core_mem";
+			interrupts = <0 125 0>, <0 221 0>;
+			interrupt-names = "hc_irq", "pwr_irq";
+			clocks = <&gcc GCC_SDCC2_APPS_CLK>, <&gcc GCC_SDCC2_AHB_CLK>;
+			clock-names = "core", "iface";
+			status = "disabled";
 		};
 
 		rng@f9bff000 {
@@ -198,5 +226,15 @@
 			clocks = <&gcc GCC_PRNG_AHB_CLK>;
 			clock-names = "core";
 		};
+
+		msmgpio: pinctrl@fd510000 {
+			compatible = "qcom,msm8974-pinctrl";
+			reg = <0xfd510000 0x4000>;
+			gpio-controller;
+			#gpio-cells = <2>;
+			interrupt-controller;
+			#interrupt-cells = <2>;
+			interrupts = <0 208 0>;
+		};
 	};
 };
diff --git a/arch/arm/boot/dts/r7s72100-genmai-reference.dts b/arch/arm/boot/dts/r7s72100-genmai-reference.dts
deleted file mode 100644
index e664611a47c..00000000000
--- a/arch/arm/boot/dts/r7s72100-genmai-reference.dts
+++ /dev/null
@@ -1,42 +0,0 @@
-/*
- * Device Tree Source for the Genmai board
- *
- * Copyright (C) 2013 Renesas Solutions Corp.
- *
- * 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.
- */
-
-/dts-v1/;
-#include "r7s72100.dtsi"
-
-/ {
-	model = "Genmai";
-	compatible = "renesas,genmai-reference", "renesas,r7s72100";
-
-	chosen {
-		bootargs = "console=ttySC2,115200 ignore_loglevel rw root=/dev/nfs ip=dhcp";
-	};
-
-	memory {
-		device_type = "memory";
-		reg = <0x08000000 0x08000000>;
-	};
-
-	lbsc {
-		#address-cells = <1>;
-		#size-cells = <1>;
-	};
-};
-
-&i2c2 {
-	status = "okay";
-	clock-frequency = <400000>;
-
-	eeprom@50 {
-		compatible = "renesas,24c128";
-		reg = <0x50>;
-		pagesize = <64>;
-	};
-};
diff --git a/arch/arm/boot/dts/r7s72100-genmai.dts b/arch/arm/boot/dts/r7s72100-genmai.dts
index b1deaf7e2e0..56849b55e1c 100644
--- a/arch/arm/boot/dts/r7s72100-genmai.dts
+++ b/arch/arm/boot/dts/r7s72100-genmai.dts
@@ -1,7 +1,8 @@
 /*
  * Device Tree Source for the Genmai board
  *
- * Copyright (C) 2013 Renesas Solutions Corp.
+ * Copyright (C) 2013-14 Renesas Solutions Corp.
+ * Copyright (C) 2014 Wolfram Sang, Sang Engineering <wsa@sang-engineering.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
@@ -15,6 +16,10 @@
 	model = "Genmai";
 	compatible = "renesas,genmai", "renesas,r7s72100";
 
+	aliases {
+		serial2 = &scif2;
+	};
+
 	chosen {
 		bootargs = "console=ttySC2,115200 ignore_loglevel rw root=/dev/nfs ip=dhcp";
 	};
@@ -29,3 +34,26 @@
 		#size-cells = <1>;
 	};
 };
+
+&extal_clk {
+	clock-frequency = <13330000>;
+};
+
+&usb_x1_clk {
+	clock-frequency = <48000000>;
+};
+
+&i2c2 {
+	status = "okay";
+	clock-frequency = <400000>;
+
+	eeprom@50 {
+		compatible = "renesas,24c128";
+		reg = <0x50>;
+		pagesize = <64>;
+	};
+};
+
+&scif2 {
+	status = "okay";
+};
diff --git a/arch/arm/boot/dts/r7s72100.dtsi b/arch/arm/boot/dts/r7s72100.dtsi
index ee700717a34..f50fbc8f3bd 100644
--- a/arch/arm/boot/dts/r7s72100.dtsi
+++ b/arch/arm/boot/dts/r7s72100.dtsi
@@ -1,13 +1,15 @@
 /*
  * Device Tree Source for the r7s72100 SoC
  *
- * Copyright (C) 2013 Renesas Solutions Corp.
+ * Copyright (C) 2013-14 Renesas Solutions Corp.
+ * Copyright (C) 2014 Wolfram Sang, Sang Engineering <wsa@sang-engineering.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.
  */
 
+#include <dt-bindings/clock/r7s72100-clock.h>
 #include <dt-bindings/interrupt-controller/irq.h>
 
 / {
@@ -28,6 +30,112 @@
 		spi4 = &spi4;
 	};
 
+	clocks {
+		ranges;
+		#address-cells = <1>;
+		#size-cells = <1>;
+
+		/* External clocks */
+		extal_clk: extal_clk {
+			#clock-cells = <0>;
+			compatible = "fixed-clock";
+			/* If clk present, value must be set by board */
+			clock-frequency = <0>;
+			clock-output-names = "extal";
+		};
+
+		usb_x1_clk: usb_x1_clk {
+			#clock-cells = <0>;
+			compatible = "fixed-clock";
+			/* If clk present, value must be set by board */
+			clock-frequency = <0>;
+			clock-output-names = "usb_x1";
+		};
+
+		/* Special CPG clocks */
+		cpg_clocks: cpg_clocks@fcfe0000 {
+			#clock-cells = <1>;
+			compatible = "renesas,r7s72100-cpg-clocks",
+				     "renesas,rz-cpg-clocks";
+			reg = <0xfcfe0000 0x18>;
+			clocks = <&extal_clk>, <&usb_x1_clk>;
+			clock-output-names = "pll", "i", "g";
+		};
+
+		/* Fixed factor clocks */
+		b_clk: b_clk {
+			#clock-cells = <0>;
+			compatible = "fixed-factor-clock";
+			clocks = <&cpg_clocks R7S72100_CLK_PLL>;
+			clock-mult = <1>;
+			clock-div = <3>;
+			clock-output-names = "b";
+		};
+		p1_clk: p1_clk {
+			#clock-cells = <0>;
+			compatible = "fixed-factor-clock";
+			clocks = <&cpg_clocks R7S72100_CLK_PLL>;
+			clock-mult = <1>;
+			clock-div = <6>;
+			clock-output-names = "p1";
+		};
+		p0_clk: p0_clk {
+			#clock-cells = <0>;
+			compatible = "fixed-factor-clock";
+			clocks = <&cpg_clocks R7S72100_CLK_PLL>;
+			clock-mult = <1>;
+			clock-div = <12>;
+			clock-output-names = "p0";
+		};
+
+		/* MSTP clocks */
+		mstp3_clks: mstp3_clks@fcfe0420 {
+			#clock-cells = <1>;
+			compatible = "renesas,r7s72100-mstp-clocks", "renesas,cpg-mstp-clocks";
+			reg = <0xfcfe0420 4>;
+			clocks = <&p0_clk>;
+			clock-indices = <R7S72100_CLK_MTU2>;
+			clock-output-names = "mtu2";
+		};
+
+		mstp4_clks: mstp4_clks@fcfe0424 {
+			#clock-cells = <1>;
+			compatible = "renesas,r7s72100-mstp-clocks", "renesas,cpg-mstp-clocks";
+			reg = <0xfcfe0424 4>;
+			clocks = <&p1_clk>, <&p1_clk>, <&p1_clk>, <&p1_clk>,
+				 <&p1_clk>, <&p1_clk>, <&p1_clk>, <&p1_clk>;
+			clock-indices = <
+				R7S72100_CLK_SCIF0 R7S72100_CLK_SCIF1 R7S72100_CLK_SCIF2 R7S72100_CLK_SCIF3
+				R7S72100_CLK_SCIF4 R7S72100_CLK_SCIF5 R7S72100_CLK_SCIF6 R7S72100_CLK_SCIF7
+			>;
+			clock-output-names = "scif0", "scif1", "scif2", "scif3", "scif4", "scif5", "scif6", "scif7";
+		};
+
+		mstp9_clks: mstp9_clks@fcfe0438 {
+			#clock-cells = <1>;
+			compatible = "renesas,r7s72100-mstp-clocks", "renesas,cpg-mstp-clocks";
+			reg = <0xfcfe0438 4>;
+			clocks = <&p0_clk>, <&p0_clk>, <&p0_clk>, <&p0_clk>;
+			clock-indices = <
+				R7S72100_CLK_I2C0 R7S72100_CLK_I2C1 R7S72100_CLK_I2C2 R7S72100_CLK_I2C3
+			>;
+			clock-output-names = "i2c0", "i2c1", "i2c2", "i2c3";
+		};
+
+		mstp10_clks: mstp10_clks@fcfe043c {
+			#clock-cells = <1>;
+			compatible = "renesas,r7s72100-mstp-clocks", "renesas,cpg-mstp-clocks";
+			reg = <0xfcfe043c 4>;
+			clocks = <&p1_clk>, <&p1_clk>, <&p1_clk>, <&p1_clk>,
+				 <&p1_clk>;
+			clock-indices = <
+				R7S72100_CLK_SPI0 R7S72100_CLK_SPI1 R7S72100_CLK_SPI2 R7S72100_CLK_SPI3
+				R7S72100_CLK_SPI4
+			>;
+			clock-output-names = "spi0", "spi1", "spi2", "spi3", "spi4";
+		};
+	};
+
 	cpus {
 		#address-cells = <1>;
 		#size-cells = <0>;
@@ -61,6 +169,7 @@
 			     <0 162 IRQ_TYPE_LEVEL_HIGH>,
 			     <0 163 IRQ_TYPE_LEVEL_HIGH>,
 			     <0 164 IRQ_TYPE_LEVEL_HIGH>;
+		clocks = <&mstp9_clks R7S72100_CLK_I2C0>;
 		clock-frequency = <100000>;
 		status = "disabled";
 	};
@@ -78,6 +187,7 @@
 			     <0 170 IRQ_TYPE_LEVEL_HIGH>,
 			     <0 171 IRQ_TYPE_LEVEL_HIGH>,
 			     <0 172 IRQ_TYPE_LEVEL_HIGH>;
+		clocks = <&mstp9_clks R7S72100_CLK_I2C1>;
 		clock-frequency = <100000>;
 		status = "disabled";
 	};
@@ -95,6 +205,7 @@
 			     <0 178 IRQ_TYPE_LEVEL_HIGH>,
 			     <0 179 IRQ_TYPE_LEVEL_HIGH>,
 			     <0 180 IRQ_TYPE_LEVEL_HIGH>;
+		clocks = <&mstp9_clks R7S72100_CLK_I2C2>;
 		clock-frequency = <100000>;
 		status = "disabled";
 	};
@@ -112,10 +223,107 @@
 			     <0 186 IRQ_TYPE_LEVEL_HIGH>,
 			     <0 187 IRQ_TYPE_LEVEL_HIGH>,
 			     <0 188 IRQ_TYPE_LEVEL_HIGH>;
+		clocks = <&mstp9_clks R7S72100_CLK_I2C3>;
 		clock-frequency = <100000>;
 		status = "disabled";
 	};
 
+	scif0: serial@e8007000 {
+		compatible = "renesas,scif-r7s72100", "renesas,scif";
+		reg = <0xe8007000 64>;
+		interrupts = <0 190 IRQ_TYPE_LEVEL_HIGH>,
+			     <0 191 IRQ_TYPE_LEVEL_HIGH>,
+			     <0 192 IRQ_TYPE_LEVEL_HIGH>,
+			     <0 189 IRQ_TYPE_LEVEL_HIGH>;
+		clocks = <&mstp4_clks R7S72100_CLK_SCIF0>;
+		clock-names = "sci_ick";
+		status = "disabled";
+	};
+
+	scif1: serial@e8007800 {
+		compatible = "renesas,scif-r7s72100", "renesas,scif";
+		reg = <0xe8007800 64>;
+		interrupts = <0 194 IRQ_TYPE_LEVEL_HIGH>,
+			     <0 195 IRQ_TYPE_LEVEL_HIGH>,
+			     <0 196 IRQ_TYPE_LEVEL_HIGH>,
+			     <0 193 IRQ_TYPE_LEVEL_HIGH>;
+		clocks = <&mstp4_clks R7S72100_CLK_SCIF1>;
+		clock-names = "sci_ick";
+		status = "disabled";
+	};
+
+	scif2: serial@e8008000 {
+		compatible = "renesas,scif-r7s72100", "renesas,scif";
+		reg = <0xe8008000 64>;
+		interrupts = <0 198 IRQ_TYPE_LEVEL_HIGH>,
+			     <0 199 IRQ_TYPE_LEVEL_HIGH>,
+			     <0 200 IRQ_TYPE_LEVEL_HIGH>,
+			     <0 197 IRQ_TYPE_LEVEL_HIGH>;
+		clocks = <&mstp4_clks R7S72100_CLK_SCIF2>;
+		clock-names = "sci_ick";
+		status = "disabled";
+	};
+
+	scif3: serial@e8008800 {
+		compatible = "renesas,scif-r7s72100", "renesas,scif";
+		reg = <0xe8008800 64>;
+		interrupts = <0 202 IRQ_TYPE_LEVEL_HIGH>,
+			     <0 203 IRQ_TYPE_LEVEL_HIGH>,
+			     <0 204 IRQ_TYPE_LEVEL_HIGH>,
+			     <0 201 IRQ_TYPE_LEVEL_HIGH>;
+		clocks = <&mstp4_clks R7S72100_CLK_SCIF3>;
+		clock-names = "sci_ick";
+		status = "disabled";
+	};
+
+	scif4: serial@e8009000 {
+		compatible = "renesas,scif-r7s72100", "renesas,scif";
+		reg = <0xe8009000 64>;
+		interrupts = <0 206 IRQ_TYPE_LEVEL_HIGH>,
+			     <0 207 IRQ_TYPE_LEVEL_HIGH>,
+			     <0 208 IRQ_TYPE_LEVEL_HIGH>,
+			     <0 205 IRQ_TYPE_LEVEL_HIGH>;
+		clocks = <&mstp4_clks R7S72100_CLK_SCIF4>;
+		clock-names = "sci_ick";
+		status = "disabled";
+	};
+
+	scif5: serial@e8009800 {
+		compatible = "renesas,scif-r7s72100", "renesas,scif";
+		reg = <0xe8009800 64>;
+		interrupts = <0 210 IRQ_TYPE_LEVEL_HIGH>,
+			     <0 211 IRQ_TYPE_LEVEL_HIGH>,
+			     <0 212 IRQ_TYPE_LEVEL_HIGH>,
+			     <0 209 IRQ_TYPE_LEVEL_HIGH>;
+		clocks = <&mstp4_clks R7S72100_CLK_SCIF5>;
+		clock-names = "sci_ick";
+		status = "disabled";
+	};
+
+	scif6: serial@e800a000 {
+		compatible = "renesas,scif-r7s72100", "renesas,scif";
+		reg = <0xe800a000 64>;
+		interrupts = <0 214 IRQ_TYPE_LEVEL_HIGH>,
+			     <0 215 IRQ_TYPE_LEVEL_HIGH>,
+			     <0 216 IRQ_TYPE_LEVEL_HIGH>,
+			     <0 213 IRQ_TYPE_LEVEL_HIGH>;
+		clocks = <&mstp4_clks R7S72100_CLK_SCIF6>;
+		clock-names = "sci_ick";
+		status = "disabled";
+	};
+
+	scif7: serial@e800a800 {
+		compatible = "renesas,scif-r7s72100", "renesas,scif";
+		reg = <0xe800a800 64>;
+		interrupts = <0 218 IRQ_TYPE_LEVEL_HIGH>,
+			     <0 219 IRQ_TYPE_LEVEL_HIGH>,
+			     <0 220 IRQ_TYPE_LEVEL_HIGH>,
+			     <0 217 IRQ_TYPE_LEVEL_HIGH>;
+		clocks = <&mstp4_clks R7S72100_CLK_SCIF7>;
+		clock-names = "sci_ick";
+		status = "disabled";
+	};
+
 	spi0: spi@e800c800 {
 		compatible = "renesas,rspi-r7s72100", "renesas,rspi-rz";
 		reg = <0xe800c800 0x24>;
@@ -123,6 +331,7 @@
 			     <0 239 IRQ_TYPE_LEVEL_HIGH>,
 			     <0 240 IRQ_TYPE_LEVEL_HIGH>;
 		interrupt-names = "error", "rx", "tx";
+		clocks = <&mstp10_clks R7S72100_CLK_SPI0>;
 		num-cs = <1>;
 		#address-cells = <1>;
 		#size-cells = <0>;
@@ -136,6 +345,7 @@
 			     <0 242 IRQ_TYPE_LEVEL_HIGH>,
 			     <0 243 IRQ_TYPE_LEVEL_HIGH>;
 		interrupt-names = "error", "rx", "tx";
+		clocks = <&mstp10_clks R7S72100_CLK_SPI1>;
 		num-cs = <1>;
 		#address-cells = <1>;
 		#size-cells = <0>;
@@ -149,6 +359,7 @@
 			     <0 245 IRQ_TYPE_LEVEL_HIGH>,
 			     <0 246 IRQ_TYPE_LEVEL_HIGH>;
 		interrupt-names = "error", "rx", "tx";
+		clocks = <&mstp10_clks R7S72100_CLK_SPI2>;
 		num-cs = <1>;
 		#address-cells = <1>;
 		#size-cells = <0>;
@@ -162,6 +373,7 @@
 			     <0 248 IRQ_TYPE_LEVEL_HIGH>,
 			     <0 249 IRQ_TYPE_LEVEL_HIGH>;
 		interrupt-names = "error", "rx", "tx";
+		clocks = <&mstp10_clks R7S72100_CLK_SPI3>;
 		num-cs = <1>;
 		#address-cells = <1>;
 		#size-cells = <0>;
@@ -175,6 +387,7 @@
 			     <0 251 IRQ_TYPE_LEVEL_HIGH>,
 			     <0 252 IRQ_TYPE_LEVEL_HIGH>;
 		interrupt-names = "error", "rx", "tx";
+		clocks = <&mstp10_clks R7S72100_CLK_SPI4>;
 		num-cs = <1>;
 		#address-cells = <1>;
 		#size-cells = <0>;
diff --git a/arch/arm/boot/dts/r8a73a4.dtsi b/arch/arm/boot/dts/r8a73a4.dtsi
index 62d0211bd19..82c5ac82538 100644
--- a/arch/arm/boot/dts/r8a73a4.dtsi
+++ b/arch/arm/boot/dts/r8a73a4.dtsi
@@ -55,7 +55,6 @@
 		#interrupt-cells = <2>;
 		interrupt-controller;
 		reg = <0 0xe61c0000 0 0x200>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 0 IRQ_TYPE_LEVEL_HIGH>,
 			     <0 1 IRQ_TYPE_LEVEL_HIGH>,
 			     <0 2 IRQ_TYPE_LEVEL_HIGH>,
@@ -95,7 +94,6 @@
 		#interrupt-cells = <2>;
 		interrupt-controller;
 		reg = <0 0xe61c0200 0 0x200>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 32 IRQ_TYPE_LEVEL_HIGH>,
 			     <0 33 IRQ_TYPE_LEVEL_HIGH>,
 			     <0 34 IRQ_TYPE_LEVEL_HIGH>,
@@ -136,7 +134,6 @@
 		dma0: dma-controller@e6700020 {
 			compatible = "renesas,shdma-r8a73a4";
 			reg = <0 0xe6700020 0 0x89e0>;
-			interrupt-parent = <&gic>;
 			interrupts = <0 220 IRQ_TYPE_LEVEL_HIGH
 					0 200 IRQ_TYPE_LEVEL_HIGH
 					0 201 IRQ_TYPE_LEVEL_HIGH
@@ -171,7 +168,6 @@
 		compatible = "renesas,rcar-thermal";
 		reg = <0 0xe61f0000 0 0x14>, <0 0xe61f0100 0 0x38>,
 			 <0 0xe61f0200 0 0x38>, <0 0xe61f0300 0 0x38>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 69 IRQ_TYPE_LEVEL_HIGH>;
 	};
 
@@ -180,7 +176,6 @@
 		#size-cells = <0>;
 		compatible = "renesas,rmobile-iic";
 		reg = <0 0xe6500000 0 0x428>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 174 IRQ_TYPE_LEVEL_HIGH>;
 		status = "disabled";
 	};
@@ -190,7 +185,6 @@
 		#size-cells = <0>;
 		compatible = "renesas,rmobile-iic";
 		reg = <0 0xe6510000 0 0x428>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 175 IRQ_TYPE_LEVEL_HIGH>;
 		status = "disabled";
 	};
@@ -200,7 +194,6 @@
 		#size-cells = <0>;
 		compatible = "renesas,rmobile-iic";
 		reg = <0 0xe6520000 0 0x428>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 176 IRQ_TYPE_LEVEL_HIGH>;
 		status = "disabled";
 	};
@@ -210,7 +203,6 @@
 		#size-cells = <0>;
 		compatible = "renesas,rmobile-iic";
 		reg = <0 0xe6530000 0 0x428>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 177 IRQ_TYPE_LEVEL_HIGH>;
 		status = "disabled";
 	};
@@ -220,7 +212,6 @@
 		#size-cells = <0>;
 		compatible = "renesas,rmobile-iic";
 		reg = <0 0xe6540000 0 0x428>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 178 IRQ_TYPE_LEVEL_HIGH>;
 		status = "disabled";
 	};
@@ -230,7 +221,6 @@
 		#size-cells = <0>;
 		compatible = "renesas,rmobile-iic";
 		reg = <0 0xe60b0000 0 0x428>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 179 IRQ_TYPE_LEVEL_HIGH>;
 		status = "disabled";
 	};
@@ -240,7 +230,6 @@
 		#size-cells = <0>;
 		compatible = "renesas,rmobile-iic";
 		reg = <0 0xe6550000 0 0x428>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 184 IRQ_TYPE_LEVEL_HIGH>;
 		status = "disabled";
 	};
@@ -250,7 +239,6 @@
 		#size-cells = <0>;
 		compatible = "renesas,rmobile-iic";
 		reg = <0 0xe6560000 0 0x428>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 185 IRQ_TYPE_LEVEL_HIGH>;
 		status = "disabled";
 	};
@@ -260,7 +248,6 @@
 		#size-cells = <0>;
 		compatible = "renesas,rmobile-iic";
 		reg = <0 0xe6570000 0 0x428>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 173 IRQ_TYPE_LEVEL_HIGH>;
 		status = "disabled";
 	};
@@ -268,7 +255,6 @@
 	mmcif0: mmc@ee200000 {
 		compatible = "renesas,sh-mmcif";
 		reg = <0 0xee200000 0 0x80>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 169 IRQ_TYPE_LEVEL_HIGH>;
 		reg-io-width = <4>;
 		status = "disabled";
@@ -277,7 +263,6 @@
 	mmcif1: mmc@ee220000 {
 		compatible = "renesas,sh-mmcif";
 		reg = <0 0xee220000 0 0x80>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 170 IRQ_TYPE_LEVEL_HIGH>;
 		reg-io-width = <4>;
 		status = "disabled";
@@ -309,7 +294,6 @@
 	sdhi0: sd@ee100000 {
 		compatible = "renesas,sdhi-r8a73a4";
 		reg = <0 0xee100000 0 0x100>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 165 IRQ_TYPE_LEVEL_HIGH>;
 		cap-sd-highspeed;
 		status = "disabled";
@@ -318,7 +302,6 @@
 	sdhi1: sd@ee120000 {
 		compatible = "renesas,sdhi-r8a73a4";
 		reg = <0 0xee120000 0 0x100>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 166 IRQ_TYPE_LEVEL_HIGH>;
 		cap-sd-highspeed;
 		status = "disabled";
@@ -327,7 +310,6 @@
 	sdhi2: sd@ee140000 {
 		compatible = "renesas,sdhi-r8a73a4";
 		reg = <0 0xee140000 0 0x100>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 167 IRQ_TYPE_LEVEL_HIGH>;
 		cap-sd-highspeed;
 		status = "disabled";
diff --git a/arch/arm/boot/dts/r8a7740-armadillo800eva-reference.dts b/arch/arm/boot/dts/r8a7740-armadillo800eva-reference.dts
index 95a849bf921..486007d7ffe 100644
--- a/arch/arm/boot/dts/r8a7740-armadillo800eva-reference.dts
+++ b/arch/arm/boot/dts/r8a7740-armadillo800eva-reference.dts
@@ -11,6 +11,7 @@
 /dts-v1/;
 #include "r8a7740.dtsi"
 #include <dt-bindings/gpio/gpio.h>
+#include <dt-bindings/input/input.h>
 #include <dt-bindings/interrupt-controller/irq.h>
 #include <dt-bindings/pwm/pwm.h>
 
@@ -77,26 +78,26 @@
 
 		power-key {
 			gpios = <&pfc 99 GPIO_ACTIVE_LOW>;
-			linux,code = <116>;
+			linux,code = <KEY_POWER>;
 			label = "SW3";
 			gpio-key,wakeup;
 		};
 
 		back-key {
 			gpios = <&pfc 100 GPIO_ACTIVE_LOW>;
-			linux,code = <158>;
+			linux,code = <KEY_BACK>;
 			label = "SW4";
 		};
 
 		menu-key {
 			gpios = <&pfc 97 GPIO_ACTIVE_LOW>;
-			linux,code = <139>;
+			linux,code = <KEY_MENU>;
 			label = "SW5";
 		};
 
 		home-key {
 			gpios = <&pfc 98 GPIO_ACTIVE_LOW>;
-			linux,code = <102>;
+			linux,code = <KEY_HOME>;
 			label = "SW6";
 		};
 	};
@@ -117,6 +118,16 @@
 		};
 	};
 
+	i2c2: i2c@2 {
+		#address-cells = <1>;
+		#size-cells = <0>;
+		compatible = "i2c-gpio";
+		gpios = <&pfc 208 GPIO_ACTIVE_HIGH /* sda */
+			 &pfc 91 GPIO_ACTIVE_HIGH /* scl */
+			>;
+		i2c-gpio,delay-us = <5>;
+	};
+
 	backlight {
 		compatible = "pwm-backlight";
 		pwms = <&tpu 2 33333 PWM_POLARITY_INVERTED>;
@@ -147,6 +158,18 @@
 	};
 };
 
+&ether {
+	pinctrl-0 = <&ether_pins>;
+	pinctrl-names = "default";
+
+	phy-handle = <&phy0>;
+	status = "ok";
+
+	phy0: ethernet-phy@0 {
+		reg = <0>;
+	};
+};
+
 &i2c0 {
 	status = "okay";
 	touchscreen@55 {
@@ -166,10 +189,23 @@
 	};
 };
 
+&i2c2 {
+	status = "okay";
+	rtc@30 {
+		compatible = "sii,s35390a";
+		reg = <0x30>;
+	};
+};
+
 &pfc {
 	pinctrl-0 = <&scifa1_pins>;
 	pinctrl-names = "default";
 
+	ether_pins: ether {
+		renesas,groups = "gether_mii", "gether_int";
+		renesas,function = "gether";
+	};
+
 	scifa1_pins: serial1 {
 		renesas,groups = "scifa1_data";
 		renesas,function = "scifa1";
diff --git a/arch/arm/boot/dts/r8a7740.dtsi b/arch/arm/boot/dts/r8a7740.dtsi
index 8280884bfa5..55d29f4d2ed 100644
--- a/arch/arm/boot/dts/r8a7740.dtsi
+++ b/arch/arm/boot/dts/r8a7740.dtsi
@@ -14,6 +14,7 @@
 
 / {
 	compatible = "renesas,r8a7740";
+	interrupt-parent = <&gic>;
 
 	cpus {
 		#address-cells = <1>;
@@ -22,13 +23,13 @@
 			compatible = "arm,cortex-a9";
 			device_type = "cpu";
 			reg = <0x0>;
+			clock-frequency = <800000000>;
 		};
 	};
 
 	gic: interrupt-controller@c2800000 {
 		compatible = "arm,cortex-a9-gic";
 		#interrupt-cells = <3>;
-		#address-cells = <1>;
 		interrupt-controller;
 		reg = <0xc2800000 0x1000>,
 		      <0xc2000000 0x1000>;
@@ -49,7 +50,6 @@
 			<0xe6900020 1>,
 			<0xe6900040 1>,
 			<0xe6900060 1>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 149 IRQ_TYPE_LEVEL_HIGH
 			      0 149 IRQ_TYPE_LEVEL_HIGH
 			      0 149 IRQ_TYPE_LEVEL_HIGH
@@ -70,7 +70,6 @@
 			<0xe6900024 1>,
 			<0xe6900044 1>,
 			<0xe6900064 1>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 149 IRQ_TYPE_LEVEL_HIGH
 			      0 149 IRQ_TYPE_LEVEL_HIGH
 			      0 149 IRQ_TYPE_LEVEL_HIGH
@@ -91,7 +90,6 @@
 			<0xe6900028 1>,
 			<0xe6900048 1>,
 			<0xe6900068 1>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 149 IRQ_TYPE_LEVEL_HIGH
 			      0 149 IRQ_TYPE_LEVEL_HIGH
 			      0 149 IRQ_TYPE_LEVEL_HIGH
@@ -112,7 +110,6 @@
 			<0xe690002c 1>,
 			<0xe690004c 1>,
 			<0xe690006c 1>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 149 IRQ_TYPE_LEVEL_HIGH
 			      0 149 IRQ_TYPE_LEVEL_HIGH
 			      0 149 IRQ_TYPE_LEVEL_HIGH
@@ -123,12 +120,23 @@
 			      0 149 IRQ_TYPE_LEVEL_HIGH>;
 	};
 
+	ether: ethernet@e9a00000 {
+		compatible = "renesas,gether-r8a7740";
+		reg = <0xe9a00000 0x800>,
+		      <0xe9a01800 0x800>;
+		interrupts = <0 110 IRQ_TYPE_LEVEL_HIGH>;
+		/* clocks = <&mstp3_clks R8A7740_CLK_GETHER>; */
+		phy-mode = "mii";
+		#address-cells = <1>;
+		#size-cells = <0>;
+		status = "disabled";
+	};
+
 	i2c0: i2c@fff20000 {
 		#address-cells = <1>;
 		#size-cells = <0>;
-		compatible = "renesas,rmobile-iic";
+		compatible = "renesas,iic-r8a7740", "renesas,rmobile-iic";
 		reg = <0xfff20000 0x425>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 201 IRQ_TYPE_LEVEL_HIGH
 			      0 202 IRQ_TYPE_LEVEL_HIGH
 			      0 203 IRQ_TYPE_LEVEL_HIGH
@@ -139,9 +147,8 @@
 	i2c1: i2c@e6c20000 {
 		#address-cells = <1>;
 		#size-cells = <0>;
-		compatible = "renesas,rmobile-iic";
+		compatible = "renesas,iic-r8a7740", "renesas,rmobile-iic";
 		reg = <0xe6c20000 0x425>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 70 IRQ_TYPE_LEVEL_HIGH
 			      0 71 IRQ_TYPE_LEVEL_HIGH
 			      0 72 IRQ_TYPE_LEVEL_HIGH
@@ -174,9 +181,8 @@
 	};
 
 	mmcif0: mmc@e6bd0000 {
-		compatible = "renesas,sh-mmcif";
+		compatible = "renesas,mmcif-r8a7740", "renesas,sh-mmcif";
 		reg = <0xe6bd0000 0x100>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 56 IRQ_TYPE_LEVEL_HIGH
 			      0 57 IRQ_TYPE_LEVEL_HIGH>;
 		status = "disabled";
@@ -185,7 +191,6 @@
 	sdhi0: sd@e6850000 {
 		compatible = "renesas,sdhi-r8a7740";
 		reg = <0xe6850000 0x100>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 117 IRQ_TYPE_LEVEL_HIGH
 			      0 118 IRQ_TYPE_LEVEL_HIGH
 			      0 119 IRQ_TYPE_LEVEL_HIGH>;
@@ -197,7 +202,6 @@
 	sdhi1: sd@e6860000 {
 		compatible = "renesas,sdhi-r8a7740";
 		reg = <0xe6860000 0x100>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 121 IRQ_TYPE_LEVEL_HIGH
 			      0 122 IRQ_TYPE_LEVEL_HIGH
 			      0 123 IRQ_TYPE_LEVEL_HIGH>;
@@ -209,7 +213,6 @@
 	sdhi2: sd@e6870000 {
 		compatible = "renesas,sdhi-r8a7740";
 		reg = <0xe6870000 0x100>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 125 IRQ_TYPE_LEVEL_HIGH
 			      0 126 IRQ_TYPE_LEVEL_HIGH
 			      0 127 IRQ_TYPE_LEVEL_HIGH>;
@@ -220,9 +223,8 @@
 
 	sh_fsi2: sound@fe1f0000 {
 		#sound-dai-cells = <1>;
-		compatible = "renesas,sh_fsi2";
+		compatible = "renesas,fsi2-r8a7740", "renesas,sh_fsi2";
 		reg = <0xfe1f0000 0x400>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 9 0x4>;
 		status = "disabled";
 	};
diff --git a/arch/arm/boot/dts/r8a7778-bockw-reference.dts b/arch/arm/boot/dts/r8a7778-bockw-reference.dts
index 06cda19dac6..f76f6ec01e1 100644
--- a/arch/arm/boot/dts/r8a7778-bockw-reference.dts
+++ b/arch/arm/boot/dts/r8a7778-bockw-reference.dts
@@ -109,4 +109,18 @@
 	pinctrl-0 = <&hspi0_pins>;
 	pinctrl-names = "default";
 	status = "okay";
+
+	flash: flash@0 {
+		#address-cells = <1>;
+		#size-cells = <1>;
+		compatible = "spansion,s25fl008k";
+		reg = <0>;
+		spi-max-frequency = <104000000>;
+		m25p,fast-read;
+
+		partition@0 {
+			label = "data(spi)";
+			reg = <0x00000000 0x00100000>;
+		};
+	};
 };
diff --git a/arch/arm/boot/dts/r8a7778.dtsi b/arch/arm/boot/dts/r8a7778.dtsi
index 85c5b3b99f5..3af0a218749 100644
--- a/arch/arm/boot/dts/r8a7778.dtsi
+++ b/arch/arm/boot/dts/r8a7778.dtsi
@@ -20,6 +20,7 @@
 
 / {
 	compatible = "renesas,r8a7778";
+	interrupt-parent = <&gic>;
 
 	cpus {
 		cpu@0 {
@@ -52,7 +53,6 @@
 			<0xfe780024 4>,
 			<0xfe780044 4>,
 			<0xfe780064 4>;
-		interrupt-parent = <&gic>;
 		interrupts =   <0 27 IRQ_TYPE_LEVEL_HIGH
 				0 28 IRQ_TYPE_LEVEL_HIGH
 				0 29 IRQ_TYPE_LEVEL_HIGH
@@ -63,7 +63,6 @@
 	gpio0: gpio@ffc40000 {
 		compatible = "renesas,gpio-r8a7778", "renesas,gpio-rcar";
 		reg = <0xffc40000 0x2c>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 103 IRQ_TYPE_LEVEL_HIGH>;
 		#gpio-cells = <2>;
 		gpio-controller;
@@ -75,7 +74,6 @@
 	gpio1: gpio@ffc41000 {
 		compatible = "renesas,gpio-r8a7778", "renesas,gpio-rcar";
 		reg = <0xffc41000 0x2c>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 103 IRQ_TYPE_LEVEL_HIGH>;
 		#gpio-cells = <2>;
 		gpio-controller;
@@ -87,7 +85,6 @@
 	gpio2: gpio@ffc42000 {
 		compatible = "renesas,gpio-r8a7778", "renesas,gpio-rcar";
 		reg = <0xffc42000 0x2c>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 103 IRQ_TYPE_LEVEL_HIGH>;
 		#gpio-cells = <2>;
 		gpio-controller;
@@ -99,7 +96,6 @@
 	gpio3: gpio@ffc43000 {
 		compatible = "renesas,gpio-r8a7778", "renesas,gpio-rcar";
 		reg = <0xffc43000 0x2c>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 103 IRQ_TYPE_LEVEL_HIGH>;
 		#gpio-cells = <2>;
 		gpio-controller;
@@ -111,7 +107,6 @@
 	gpio4: gpio@ffc44000 {
 		compatible = "renesas,gpio-r8a7778", "renesas,gpio-rcar";
 		reg = <0xffc44000 0x2c>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 103 IRQ_TYPE_LEVEL_HIGH>;
 		#gpio-cells = <2>;
 		gpio-controller;
@@ -130,7 +125,6 @@
 		#size-cells = <0>;
 		compatible = "renesas,i2c-r8a7778";
 		reg = <0xffc70000 0x1000>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 67 IRQ_TYPE_LEVEL_HIGH>;
 		status = "disabled";
 	};
@@ -140,7 +134,6 @@
 		#size-cells = <0>;
 		compatible = "renesas,i2c-r8a7778";
 		reg = <0xffc71000 0x1000>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 78 IRQ_TYPE_LEVEL_HIGH>;
 		status = "disabled";
 	};
@@ -150,7 +143,6 @@
 		#size-cells = <0>;
 		compatible = "renesas,i2c-r8a7778";
 		reg = <0xffc72000 0x1000>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 76 IRQ_TYPE_LEVEL_HIGH>;
 		status = "disabled";
 	};
@@ -160,7 +152,6 @@
 		#size-cells = <0>;
 		compatible = "renesas,i2c-r8a7778";
 		reg = <0xffc73000 0x1000>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 77 IRQ_TYPE_LEVEL_HIGH>;
 		status = "disabled";
 	};
@@ -168,7 +159,6 @@
 	mmcif: mmc@ffe4e000 {
 		compatible = "renesas,sh-mmcif";
 		reg = <0xffe4e000 0x100>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 61 IRQ_TYPE_LEVEL_HIGH>;
 		status = "disabled";
 	};
@@ -176,7 +166,6 @@
 	sdhi0: sd@ffe4c000 {
 		compatible = "renesas,sdhi-r8a7778";
 		reg = <0xffe4c000 0x100>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 87 IRQ_TYPE_LEVEL_HIGH>;
 		cap-sd-highspeed;
 		cap-sdio-irq;
@@ -186,7 +175,6 @@
 	sdhi1: sd@ffe4d000 {
 		compatible = "renesas,sdhi-r8a7778";
 		reg = <0xffe4d000 0x100>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 88 IRQ_TYPE_LEVEL_HIGH>;
 		cap-sd-highspeed;
 		cap-sdio-irq;
@@ -196,7 +184,6 @@
 	sdhi2: sd@ffe4f000 {
 		compatible = "renesas,sdhi-r8a7778";
 		reg = <0xffe4f000 0x100>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 86 IRQ_TYPE_LEVEL_HIGH>;
 		cap-sd-highspeed;
 		cap-sdio-irq;
@@ -204,26 +191,29 @@
 	};
 
 	hspi0: spi@fffc7000 {
-		compatible = "renesas,hspi";
+		compatible = "renesas,hspi-r8a7778", "renesas,hspi";
 		reg = <0xfffc7000 0x18>;
-		interrupt-controller = <&gic>;
 		interrupts = <0 63 IRQ_TYPE_LEVEL_HIGH>;
+		#address-cells = <1>;
+		#size-cells = <0>;
 		status = "disabled";
 	};
 
 	hspi1: spi@fffc8000 {
-		compatible = "renesas,hspi";
+		compatible = "renesas,hspi-r8a7778", "renesas,hspi";
 		reg = <0xfffc8000 0x18>;
-		interrupt-controller = <&gic>;
 		interrupts = <0 84 IRQ_TYPE_LEVEL_HIGH>;
+		#address-cells = <1>;
+		#size-cells = <0>;
 		status = "disabled";
 	};
 
 	hspi2: spi@fffc6000 {
-		compatible = "renesas,hspi";
+		compatible = "renesas,hspi-r8a7778", "renesas,hspi";
 		reg = <0xfffc6000 0x18>;
-		interrupt-controller = <&gic>;
 		interrupts = <0 85 IRQ_TYPE_LEVEL_HIGH>;
+		#address-cells = <1>;
+		#size-cells = <0>;
 		status = "disabled";
 	};
 };
diff --git a/arch/arm/boot/dts/r8a7779-marzen-reference.dts b/arch/arm/boot/dts/r8a7779-marzen-reference.dts
index 76f5eef7d1c..b27c6373ff4 100644
--- a/arch/arm/boot/dts/r8a7779-marzen-reference.dts
+++ b/arch/arm/boot/dts/r8a7779-marzen-reference.dts
@@ -45,6 +45,7 @@
 		phy-mode = "mii";
 		interrupt-parent = <&irqpin0>;
 		interrupts = <1 IRQ_TYPE_EDGE_FALLING>;
+		smsc,irq-push-pull;
 		reg-io-width = <4>;
 		vddvario-supply = <&fixedregulator3v3>;
 		vdd33a-supply = <&fixedregulator3v3>;
diff --git a/arch/arm/boot/dts/r8a7779.dtsi b/arch/arm/boot/dts/r8a7779.dtsi
index d0561d4c7c4..b517c8e6b42 100644
--- a/arch/arm/boot/dts/r8a7779.dtsi
+++ b/arch/arm/boot/dts/r8a7779.dtsi
@@ -15,6 +15,7 @@
 
 / {
 	compatible = "renesas,r8a7779";
+	interrupt-parent = <&gic>;
 
 	cpus {
 		#address-cells = <1>;
@@ -59,7 +60,6 @@
 	gpio0: gpio@ffc40000 {
 		compatible = "renesas,gpio-r8a7779", "renesas,gpio-rcar";
 		reg = <0xffc40000 0x2c>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 141 IRQ_TYPE_LEVEL_HIGH>;
 		#gpio-cells = <2>;
 		gpio-controller;
@@ -71,7 +71,6 @@
 	gpio1: gpio@ffc41000 {
 		compatible = "renesas,gpio-r8a7779", "renesas,gpio-rcar";
 		reg = <0xffc41000 0x2c>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 142 IRQ_TYPE_LEVEL_HIGH>;
 		#gpio-cells = <2>;
 		gpio-controller;
@@ -83,7 +82,6 @@
 	gpio2: gpio@ffc42000 {
 		compatible = "renesas,gpio-r8a7779", "renesas,gpio-rcar";
 		reg = <0xffc42000 0x2c>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 143 IRQ_TYPE_LEVEL_HIGH>;
 		#gpio-cells = <2>;
 		gpio-controller;
@@ -95,7 +93,6 @@
 	gpio3: gpio@ffc43000 {
 		compatible = "renesas,gpio-r8a7779", "renesas,gpio-rcar";
 		reg = <0xffc43000 0x2c>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 144 IRQ_TYPE_LEVEL_HIGH>;
 		#gpio-cells = <2>;
 		gpio-controller;
@@ -107,7 +104,6 @@
 	gpio4: gpio@ffc44000 {
 		compatible = "renesas,gpio-r8a7779", "renesas,gpio-rcar";
 		reg = <0xffc44000 0x2c>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 145 IRQ_TYPE_LEVEL_HIGH>;
 		#gpio-cells = <2>;
 		gpio-controller;
@@ -119,7 +115,6 @@
 	gpio5: gpio@ffc45000 {
 		compatible = "renesas,gpio-r8a7779", "renesas,gpio-rcar";
 		reg = <0xffc45000 0x2c>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 146 IRQ_TYPE_LEVEL_HIGH>;
 		#gpio-cells = <2>;
 		gpio-controller;
@@ -131,7 +126,6 @@
 	gpio6: gpio@ffc46000 {
 		compatible = "renesas,gpio-r8a7779", "renesas,gpio-rcar";
 		reg = <0xffc46000 0x2c>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 147 IRQ_TYPE_LEVEL_HIGH>;
 		#gpio-cells = <2>;
 		gpio-controller;
@@ -150,7 +144,6 @@
 			<0xfe780024 4>,
 			<0xfe780044 4>,
 			<0xfe780064 4>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 27 IRQ_TYPE_LEVEL_HIGH
 			      0 28 IRQ_TYPE_LEVEL_HIGH
 			      0 29 IRQ_TYPE_LEVEL_HIGH
@@ -163,7 +156,6 @@
 		#size-cells = <0>;
 		compatible = "renesas,i2c-r8a7779";
 		reg = <0xffc70000 0x1000>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 79 IRQ_TYPE_LEVEL_HIGH>;
 		status = "disabled";
 	};
@@ -173,7 +165,6 @@
 		#size-cells = <0>;
 		compatible = "renesas,i2c-r8a7779";
 		reg = <0xffc71000 0x1000>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 82 IRQ_TYPE_LEVEL_HIGH>;
 		status = "disabled";
 	};
@@ -183,7 +174,6 @@
 		#size-cells = <0>;
 		compatible = "renesas,i2c-r8a7779";
 		reg = <0xffc72000 0x1000>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 80 IRQ_TYPE_LEVEL_HIGH>;
 		status = "disabled";
 	};
@@ -193,7 +183,6 @@
 		#size-cells = <0>;
 		compatible = "renesas,i2c-r8a7779";
 		reg = <0xffc73000 0x1000>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 81 IRQ_TYPE_LEVEL_HIGH>;
 		status = "disabled";
 	};
@@ -211,14 +200,12 @@
 	sata: sata@fc600000 {
 		compatible = "renesas,rcar-sata";
 		reg = <0xfc600000 0x2000>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 100 IRQ_TYPE_LEVEL_HIGH>;
 	};
 
 	sdhi0: sd@ffe4c000 {
 		compatible = "renesas,sdhi-r8a7779";
 		reg = <0xffe4c000 0x100>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 104 IRQ_TYPE_LEVEL_HIGH>;
 		cap-sd-highspeed;
 		cap-sdio-irq;
@@ -228,7 +215,6 @@
 	sdhi1: sd@ffe4d000 {
 		compatible = "renesas,sdhi-r8a7779";
 		reg = <0xffe4d000 0x100>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 105 IRQ_TYPE_LEVEL_HIGH>;
 		cap-sd-highspeed;
 		cap-sdio-irq;
@@ -238,7 +224,6 @@
 	sdhi2: sd@ffe4e000 {
 		compatible = "renesas,sdhi-r8a7779";
 		reg = <0xffe4e000 0x100>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 107 IRQ_TYPE_LEVEL_HIGH>;
 		cap-sd-highspeed;
 		cap-sdio-irq;
@@ -248,7 +233,6 @@
 	sdhi3: sd@ffe4f000 {
 		compatible = "renesas,sdhi-r8a7779";
 		reg = <0xffe4f000 0x100>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 106 IRQ_TYPE_LEVEL_HIGH>;
 		cap-sd-highspeed;
 		cap-sdio-irq;
@@ -256,26 +240,29 @@
 	};
 
 	hspi0: spi@fffc7000 {
-		compatible = "renesas,hspi";
+		compatible = "renesas,hspi-r8a7779", "renesas,hspi";
 		reg = <0xfffc7000 0x18>;
-		interrupt-controller = <&gic>;
 		interrupts = <0 73 IRQ_TYPE_LEVEL_HIGH>;
+		#address-cells = <1>;
+		#size-cells = <0>;
 		status = "disabled";
 	};
 
 	hspi1: spi@fffc8000 {
-		compatible = "renesas,hspi";
+		compatible = "renesas,hspi-r8a7779", "renesas,hspi";
 		reg = <0xfffc8000 0x18>;
-		interrupt-controller = <&gic>;
 		interrupts = <0 74 IRQ_TYPE_LEVEL_HIGH>;
+		#address-cells = <1>;
+		#size-cells = <0>;
 		status = "disabled";
 	};
 
 	hspi2: spi@fffc6000 {
-		compatible = "renesas,hspi";
+		compatible = "renesas,hspi-r8a7779", "renesas,hspi";
 		reg = <0xfffc6000 0x18>;
-		interrupt-controller = <&gic>;
 		interrupts = <0 75 IRQ_TYPE_LEVEL_HIGH>;
+		#address-cells = <1>;
+		#size-cells = <0>;
 		status = "disabled";
 	};
 };
diff --git a/arch/arm/boot/dts/r8a7790-lager.dts b/arch/arm/boot/dts/r8a7790-lager.dts
index 6e99eb2df07..dd2fe46073f 100644
--- a/arch/arm/boot/dts/r8a7790-lager.dts
+++ b/arch/arm/boot/dts/r8a7790-lager.dts
@@ -12,11 +12,17 @@
 /dts-v1/;
 #include "r8a7790.dtsi"
 #include <dt-bindings/gpio/gpio.h>
+#include <dt-bindings/input/input.h>
 
 / {
 	model = "Lager";
 	compatible = "renesas,lager", "renesas,r8a7790";
 
+	aliases {
+		serial6 = &scif0;
+		serial7 = &scif1;
+	};
+
 	chosen {
 		bootargs = "console=ttySC6,115200 ignore_loglevel rw root=/dev/nfs ip=dhcp";
 	};
@@ -36,6 +42,39 @@
 		#size-cells = <1>;
 	};
 
+	gpio_keys {
+		compatible = "gpio-keys";
+
+		button@1 {
+			linux,code = <KEY_1>;
+			label = "SW2-1";
+			gpio-key,wakeup;
+			debounce-interval = <20>;
+			gpios = <&gpio1 14 GPIO_ACTIVE_LOW>;
+		};
+		button@2 {
+			linux,code = <KEY_2>;
+			label = "SW2-2";
+			gpio-key,wakeup;
+			debounce-interval = <20>;
+			gpios = <&gpio1 24 GPIO_ACTIVE_LOW>;
+		};
+		button@3 {
+			linux,code = <KEY_3>;
+			label = "SW2-3";
+			gpio-key,wakeup;
+			debounce-interval = <20>;
+			gpios = <&gpio1 26 GPIO_ACTIVE_LOW>;
+		};
+		button@4 {
+			linux,code = <KEY_4>;
+			label = "SW2-4";
+			gpio-key,wakeup;
+			debounce-interval = <20>;
+			gpios = <&gpio1 28 GPIO_ACTIVE_LOW>;
+		};
+	};
+
 	leds {
 		compatible = "gpio-leds";
 		led6 {
@@ -112,7 +151,7 @@
 };
 
 &pfc {
-	pinctrl-0 = <&du_pins &scif0_pins &scif1_pins>;
+	pinctrl-0 = <&du_pins>;
 	pinctrl-names = "default";
 
 	du_pins: du {
@@ -141,12 +180,12 @@
 	};
 
 	sdhi0_pins: sd0 {
-		renesas,gpios = "sdhi0_data4", "sdhi0_ctrl";
+		renesas,groups = "sdhi0_data4", "sdhi0_ctrl";
 		renesas,function = "sdhi0";
 	};
 
 	sdhi2_pins: sd2 {
-		renesas,gpios = "sdhi2_data4", "sdhi2_ctrl";
+		renesas,groups = "sdhi2_data4", "sdhi2_ctrl";
 		renesas,function = "sdhi2";
 	};
 
@@ -155,10 +194,16 @@
 		renesas,function = "mmc1";
 	};
 
-	qspi_pins: spi {
+	qspi_pins: spi0 {
 		renesas,groups = "qspi_ctrl", "qspi_data4";
 		renesas,function = "qspi";
 	};
+
+	msiof1_pins: spi2 {
+		renesas,groups = "msiof1_clk", "msiof1_sync", "msiof1_rx",
+				 "msiof1_tx";
+		renesas,function = "msiof1";
+	};
 };
 
 &ether {
@@ -173,6 +218,7 @@
 		reg = <1>;
 		interrupt-parent = <&irqc0>;
 		interrupts = <0 IRQ_TYPE_LEVEL_LOW>;
+		micrel,led-mode = <1>;
 	};
 };
 
@@ -190,7 +236,7 @@
 	status = "okay";
 };
 
-&spi {
+&qspi {
 	pinctrl-0 = <&qspi_pins>;
 	pinctrl-names = "default";
 
@@ -202,6 +248,8 @@
 		compatible = "spansion,s25fl512s";
 		reg = <0>;
 		spi-max-frequency = <30000000>;
+		spi-tx-bus-width = <4>;
+		spi-rx-bus-width = <4>;
 		m25p,fast-read;
 
 		partition@0 {
@@ -221,6 +269,35 @@
 	};
 };
 
+&scif0 {
+	pinctrl-0 = <&scif0_pins>;
+	pinctrl-names = "default";
+
+	status = "okay";
+};
+
+&scif1 {
+	pinctrl-0 = <&scif1_pins>;
+	pinctrl-names = "default";
+
+	status = "okay";
+};
+
+&msiof1 {
+	pinctrl-0 = <&msiof1_pins>;
+	pinctrl-names = "default";
+
+	status = "okay";
+
+	pmic: pmic@0 {
+		compatible = "renesas,r2a11302ft";
+		reg = <0>;
+		spi-max-frequency = <6000000>;
+		spi-cpol;
+		spi-cpha;
+	};
+};
+
 &sdhi0 {
 	pinctrl-0 = <&sdhi0_pins>;
 	pinctrl-names = "default";
diff --git a/arch/arm/boot/dts/r8a7790.dtsi b/arch/arm/boot/dts/r8a7790.dtsi
index 618e5b537ea..7ff29601f96 100644
--- a/arch/arm/boot/dts/r8a7790.dtsi
+++ b/arch/arm/boot/dts/r8a7790.dtsi
@@ -24,6 +24,15 @@
 		i2c1 = &i2c1;
 		i2c2 = &i2c2;
 		i2c3 = &i2c3;
+		i2c4 = &iic0;
+		i2c5 = &iic1;
+		i2c6 = &iic2;
+		i2c7 = &iic3;
+		spi0 = &qspi;
+		spi1 = &msiof0;
+		spi2 = &msiof1;
+		spi3 = &msiof2;
+		spi4 = &msiof3;
 	};
 
 	cpus {
@@ -108,6 +117,7 @@
 		gpio-ranges = <&pfc 0 0 32>;
 		#interrupt-cells = <2>;
 		interrupt-controller;
+		clocks = <&mstp9_clks R8A7790_CLK_GPIO0>;
 	};
 
 	gpio1: gpio@e6051000 {
@@ -119,6 +129,7 @@
 		gpio-ranges = <&pfc 0 32 32>;
 		#interrupt-cells = <2>;
 		interrupt-controller;
+		clocks = <&mstp9_clks R8A7790_CLK_GPIO1>;
 	};
 
 	gpio2: gpio@e6052000 {
@@ -130,6 +141,7 @@
 		gpio-ranges = <&pfc 0 64 32>;
 		#interrupt-cells = <2>;
 		interrupt-controller;
+		clocks = <&mstp9_clks R8A7790_CLK_GPIO2>;
 	};
 
 	gpio3: gpio@e6053000 {
@@ -141,6 +153,7 @@
 		gpio-ranges = <&pfc 0 96 32>;
 		#interrupt-cells = <2>;
 		interrupt-controller;
+		clocks = <&mstp9_clks R8A7790_CLK_GPIO3>;
 	};
 
 	gpio4: gpio@e6054000 {
@@ -152,6 +165,7 @@
 		gpio-ranges = <&pfc 0 128 32>;
 		#interrupt-cells = <2>;
 		interrupt-controller;
+		clocks = <&mstp9_clks R8A7790_CLK_GPIO4>;
 	};
 
 	gpio5: gpio@e6055000 {
@@ -163,6 +177,7 @@
 		gpio-ranges = <&pfc 0 160 32>;
 		#interrupt-cells = <2>;
 		interrupt-controller;
+		clocks = <&mstp9_clks R8A7790_CLK_GPIO5>;
 	};
 
 	thermal@e61f0000 {
@@ -231,6 +246,46 @@
 		status = "disabled";
 	};
 
+	iic0: i2c@e6500000 {
+		#address-cells = <1>;
+		#size-cells = <0>;
+		compatible = "renesas,iic-r8a7790", "renesas,rmobile-iic";
+		reg = <0 0xe6500000 0 0x425>;
+		interrupts = <0 174 IRQ_TYPE_LEVEL_HIGH>;
+		clocks = <&mstp3_clks R8A7790_CLK_IIC0>;
+		status = "disabled";
+	};
+
+	iic1: i2c@e6510000 {
+		#address-cells = <1>;
+		#size-cells = <0>;
+		compatible = "renesas,iic-r8a7790", "renesas,rmobile-iic";
+		reg = <0 0xe6510000 0 0x425>;
+		interrupts = <0 175 IRQ_TYPE_LEVEL_HIGH>;
+		clocks = <&mstp3_clks R8A7790_CLK_IIC1>;
+		status = "disabled";
+	};
+
+	iic2: i2c@e6520000 {
+		#address-cells = <1>;
+		#size-cells = <0>;
+		compatible = "renesas,iic-r8a7790", "renesas,rmobile-iic";
+		reg = <0 0xe6520000 0 0x425>;
+		interrupts = <0 176 IRQ_TYPE_LEVEL_HIGH>;
+		clocks = <&mstp3_clks R8A7790_CLK_IIC2>;
+		status = "disabled";
+	};
+
+	iic3: i2c@e60b0000 {
+		#address-cells = <1>;
+		#size-cells = <0>;
+		compatible = "renesas,iic-r8a7790", "renesas,rmobile-iic";
+		reg = <0 0xe60b0000 0 0x425>;
+		interrupts = <0 173 IRQ_TYPE_LEVEL_HIGH>;
+		clocks = <&mstp9_clks R8A7790_CLK_IICDVFS>;
+		status = "disabled";
+	};
+
 	mmcif0: mmcif@ee200000 {
 		compatible = "renesas,mmcif-r8a7790", "renesas,sh-mmcif";
 		reg = <0 0xee200000 0 0x80>;
@@ -673,7 +728,7 @@
 			renesas,clock-indices = <
 				R8A7790_CLK_TMU1 R8A7790_CLK_TMU3 R8A7790_CLK_TMU2
 				R8A7790_CLK_CMT0 R8A7790_CLK_TMU0 R8A7790_CLK_VSP1_DU1
-				R8A7790_CLK_VSP1_DU0 R8A7790_CLK_VSP1_RT R8A7790_CLK_VSP1_SY
+				R8A7790_CLK_VSP1_DU0 R8A7790_CLK_VSP1_R R8A7790_CLK_VSP1_S
 			>;
 			clock-output-names =
 				"tmu1", "tmu3", "tmu2", "cmt0", "tmu0", "vsp1-du1",
@@ -697,18 +752,19 @@
 		mstp3_clks: mstp3_clks@e615013c {
 			compatible = "renesas,r8a7790-mstp-clocks", "renesas,cpg-mstp-clocks";
 			reg = <0 0xe615013c 0 4>, <0 0xe6150048 0 4>;
-			clocks = <&cp_clk>, <&mmc1_clk>, <&sd3_clk>, <&sd2_clk>,
-				 <&cpg_clocks R8A7790_CLK_SD1>, <&cpg_clocks R8A7790_CLK_SD0>,
-				 <&mmc0_clk>, <&rclk_clk>;
+			clocks = <&hp_clk>, <&cp_clk>, <&mmc1_clk>, <&sd3_clk>,
+				 <&sd2_clk>, <&cpg_clocks R8A7790_CLK_SD1>, <&cpg_clocks R8A7790_CLK_SD0>, <&mmc0_clk>,
+				 <&hp_clk>, <&hp_clk>, <&rclk_clk>;
 			#clock-cells = <1>;
 			renesas,clock-indices = <
-				R8A7790_CLK_TPU0 R8A7790_CLK_MMCIF1 R8A7790_CLK_SDHI3
-				R8A7790_CLK_SDHI2 R8A7790_CLK_SDHI1 R8A7790_CLK_SDHI0
-				R8A7790_CLK_MMCIF0 R8A7790_CLK_CMT1
+				R8A7790_CLK_IIC2 R8A7790_CLK_TPU0 R8A7790_CLK_MMCIF1 R8A7790_CLK_SDHI3
+				R8A7790_CLK_SDHI2 R8A7790_CLK_SDHI1 R8A7790_CLK_SDHI0 R8A7790_CLK_MMCIF0
+				R8A7790_CLK_IIC0 R8A7790_CLK_IIC1 R8A7790_CLK_CMT1
 			>;
 			clock-output-names =
-				"tpu0", "mmcif1", "sdhi3", "sdhi2",
-				"sdhi1", "sdhi0", "mmcif0", "cmt1";
+				"iic2", "tpu0", "mmcif1", "sdhi3",
+				"sdhi2", "sdhi1", "sdhi0", "mmcif0",
+				"iic0", "iic1", "cmt1";
 		};
 		mstp5_clks: mstp5_clks@e6150144 {
 			compatible = "renesas,r8a7790-mstp-clocks", "renesas,cpg-mstp-clocks";
@@ -752,20 +808,25 @@
 		mstp9_clks: mstp9_clks@e6150994 {
 			compatible = "renesas,r8a7790-mstp-clocks", "renesas,cpg-mstp-clocks";
 			reg = <0 0xe6150994 0 4>, <0 0xe61509a4 0 4>;
-			clocks = <&p_clk>, <&p_clk>, <&cpg_clocks R8A7790_CLK_QSPI>,
-				 <&p_clk>, <&p_clk>, <&p_clk>, <&p_clk>;
+			clocks = <&cp_clk>, <&cp_clk>, <&cp_clk>,
+				 <&cp_clk>, <&cp_clk>, <&cp_clk>,
+				 <&p_clk>, <&p_clk>, <&cpg_clocks R8A7790_CLK_QSPI>, <&cp_clk>,
+				 <&hp_clk>, <&hp_clk>, <&hp_clk>, <&hp_clk>;
 			#clock-cells = <1>;
 			renesas,clock-indices = <
-				R8A7790_CLK_RCAN1 R8A7790_CLK_RCAN0 R8A7790_CLK_QSPI_MOD
-				R8A7790_CLK_I2C3 R8A7790_CLK_I2C2 R8A7790_CLK_I2C1
-				R8A7790_CLK_I2C0
+				R8A7790_CLK_GPIO5 R8A7790_CLK_GPIO4 R8A7790_CLK_GPIO3
+				R8A7790_CLK_GPIO2 R8A7790_CLK_GPIO1 R8A7790_CLK_GPIO0
+				R8A7790_CLK_RCAN1 R8A7790_CLK_RCAN0 R8A7790_CLK_QSPI_MOD R8A7790_CLK_IICDVFS
+				R8A7790_CLK_I2C3 R8A7790_CLK_I2C2 R8A7790_CLK_I2C1 R8A7790_CLK_I2C0
 			>;
 			clock-output-names =
-				"rcan1", "rcan0", "qspi_mod", "i2c3", "i2c2", "i2c1", "i2c0";
+				"gpio5", "gpio4", "gpio3", "gpio2", "gpio1", "gpio0",
+				"rcan1", "rcan0", "qspi_mod", "iic3",
+				"i2c3", "i2c2", "i2c1", "i2c0";
 		};
 	};
 
-	spi: spi@e6b10000 {
+	qspi: spi@e6b10000 {
 		compatible = "renesas,qspi-r8a7790", "renesas,qspi";
 		reg = <0 0xe6b10000 0 0x2c>;
 		interrupts = <0 184 IRQ_TYPE_LEVEL_HIGH>;
@@ -775,4 +836,44 @@
 		#size-cells = <0>;
 		status = "disabled";
 	};
+
+	msiof0: spi@e6e20000 {
+		compatible = "renesas,msiof-r8a7790";
+		reg = <0 0xe6e20000 0 0x0064>;
+		interrupts = <0 156 IRQ_TYPE_LEVEL_HIGH>;
+		clocks = <&mstp0_clks R8A7790_CLK_MSIOF0>;
+		#address-cells = <1>;
+		#size-cells = <0>;
+		status = "disabled";
+	};
+
+	msiof1: spi@e6e10000 {
+		compatible = "renesas,msiof-r8a7790";
+		reg = <0 0xe6e10000 0 0x0064>;
+		interrupts = <0 157 IRQ_TYPE_LEVEL_HIGH>;
+		clocks = <&mstp2_clks R8A7790_CLK_MSIOF1>;
+		#address-cells = <1>;
+		#size-cells = <0>;
+		status = "disabled";
+	};
+
+	msiof2: spi@e6e00000 {
+		compatible = "renesas,msiof-r8a7790";
+		reg = <0 0xe6e00000 0 0x0064>;
+		interrupts = <0 158 IRQ_TYPE_LEVEL_HIGH>;
+		clocks = <&mstp2_clks R8A7790_CLK_MSIOF2>;
+		#address-cells = <1>;
+		#size-cells = <0>;
+		status = "disabled";
+	};
+
+	msiof3: spi@e6c90000 {
+		compatible = "renesas,msiof-r8a7790";
+		reg = <0 0xe6c90000 0 0x0064>;
+		interrupts = <0 159 IRQ_TYPE_LEVEL_HIGH>;
+		clocks = <&mstp2_clks R8A7790_CLK_MSIOF3>;
+		#address-cells = <1>;
+		#size-cells = <0>;
+		status = "disabled";
+	};
 };
diff --git a/arch/arm/boot/dts/r8a7791-henninger.dts b/arch/arm/boot/dts/r8a7791-henninger.dts
new file mode 100644
index 00000000000..cc6d992e8db
--- /dev/null
+++ b/arch/arm/boot/dts/r8a7791-henninger.dts
@@ -0,0 +1,219 @@
+/*
+ * Device Tree Source for the Henninger board
+ *
+ * Copyright (C) 2014 Renesas Solutions Corp.
+ * Copyright (C) 2014 Cogent Embedded, Inc.
+ *
+ * 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.
+ */
+
+/dts-v1/;
+#include "r8a7791.dtsi"
+#include <dt-bindings/gpio/gpio.h>
+
+/ {
+	model = "Henninger";
+	compatible = "renesas,henninger", "renesas,r8a7791";
+
+	aliases {
+		serial0 = &scif0;
+	};
+
+	chosen {
+		bootargs = "console=ttySC0,38400 ignore_loglevel rw root=/dev/nfs ip=dhcp";
+	};
+
+	memory@40000000 {
+		device_type = "memory";
+		reg = <0 0x40000000 0 0x40000000>;
+	};
+
+	memory@200000000 {
+		device_type = "memory";
+		reg = <2 0x00000000 0 0x40000000>;
+	};
+
+	vcc_sdhi0: regulator@0 {
+		compatible = "regulator-fixed";
+
+		regulator-name = "SDHI0 Vcc";
+		regulator-min-microvolt = <3300000>;
+		regulator-max-microvolt = <3300000>;
+		regulator-always-on;
+	};
+
+	vccq_sdhi0: regulator@1 {
+		compatible = "regulator-gpio";
+
+		regulator-name = "SDHI0 VccQ";
+		regulator-min-microvolt = <1800000>;
+		regulator-max-microvolt = <3300000>;
+
+		gpios = <&gpio2 12 GPIO_ACTIVE_HIGH>;
+		gpios-states = <1>;
+		states = <3300000 1
+			  1800000 0>;
+	};
+
+	vcc_sdhi2: regulator@2 {
+		compatible = "regulator-fixed";
+
+		regulator-name = "SDHI2 Vcc";
+		regulator-min-microvolt = <3300000>;
+		regulator-max-microvolt = <3300000>;
+		regulator-always-on;
+	};
+
+	vccq_sdhi2: regulator@3 {
+		compatible = "regulator-gpio";
+
+		regulator-name = "SDHI2 VccQ";
+		regulator-min-microvolt = <1800000>;
+		regulator-max-microvolt = <3300000>;
+
+		gpios = <&gpio2 26 GPIO_ACTIVE_HIGH>;
+		gpios-states = <1>;
+		states = <3300000 1
+			  1800000 0>;
+	};
+};
+
+&extal_clk {
+	clock-frequency = <20000000>;
+};
+
+&pfc {
+	scif0_pins: serial0 {
+		renesas,groups = "scif0_data_d";
+		renesas,function = "scif0";
+	};
+
+	ether_pins: ether {
+		renesas,groups = "eth_link", "eth_mdio", "eth_rmii";
+		renesas,function = "eth";
+	};
+
+	phy1_pins: phy1 {
+		renesas,groups = "intc_irq0";
+		renesas,function = "intc";
+	};
+
+	sdhi0_pins: sd0 {
+		renesas,groups = "sdhi0_data4", "sdhi0_ctrl";
+		renesas,function = "sdhi0";
+	};
+
+	sdhi2_pins: sd2 {
+		renesas,groups = "sdhi2_data4", "sdhi2_ctrl";
+		renesas,function = "sdhi2";
+	};
+
+	qspi_pins: spi0 {
+		renesas,groups = "qspi_ctrl", "qspi_data4";
+		renesas,function = "qspi";
+	};
+
+	msiof0_pins: spi1 {
+		renesas,groups = "msiof0_clk", "msiof0_sync", "msiof0_rx",
+				 "msiof0_tx";
+		renesas,function = "msiof0";
+	};
+};
+
+&scif0 {
+	pinctrl-0 = <&scif0_pins>;
+	pinctrl-names = "default";
+
+	status = "okay";
+};
+
+&ether {
+	pinctrl-0 = <&ether_pins &phy1_pins>;
+	pinctrl-names = "default";
+
+	phy-handle = <&phy1>;
+	renesas,ether-link-active-low;
+	status = "ok";
+
+	phy1: ethernet-phy@1 {
+		reg = <1>;
+		interrupt-parent = <&irqc0>;
+		interrupts = <0 IRQ_TYPE_LEVEL_LOW>;
+		micrel,led-mode = <1>;
+	};
+};
+
+&sata0 {
+       status = "okay";
+};
+
+&sdhi0 {
+	pinctrl-0 = <&sdhi0_pins>;
+	pinctrl-names = "default";
+
+	vmmc-supply = <&vcc_sdhi0>;
+	vqmmc-supply = <&vccq_sdhi0>;
+	cd-gpios = <&gpio6 6 GPIO_ACTIVE_LOW>;
+	wp-gpios = <&gpio6 7 GPIO_ACTIVE_HIGH>;
+	status = "okay";
+};
+
+&sdhi2 {
+	pinctrl-0 = <&sdhi2_pins>;
+	pinctrl-names = "default";
+
+	vmmc-supply = <&vcc_sdhi2>;
+	vqmmc-supply = <&vccq_sdhi2>;
+	cd-gpios = <&gpio6 22 GPIO_ACTIVE_LOW>;
+	status = "okay";
+};
+
+&qspi {
+	pinctrl-0 = <&qspi_pins>;
+	pinctrl-names = "default";
+
+	status = "okay";
+
+	flash@0 {
+		#address-cells = <1>;
+		#size-cells = <1>;
+		compatible = "spansion,s25fl512s";
+		reg = <0>;
+		spi-max-frequency = <30000000>;
+		spi-tx-bus-width = <4>;
+		spi-rx-bus-width = <4>;
+		m25p,fast-read;
+
+		partition@0 {
+			label = "loader_prg";
+			reg = <0x00000000 0x00040000>;
+			read-only;
+		};
+		partition@40000 {
+			label = "user_prg";
+			reg = <0x00040000 0x00400000>;
+			read-only;
+		};
+		partition@440000 {
+			label = "flash_fs";
+			reg = <0x00440000 0x03bc0000>;
+		};
+	};
+};
+
+&msiof0 {
+	pinctrl-0 = <&msiof0_pins>;
+	pinctrl-names = "default";
+
+	status = "okay";
+
+	pmic@0 {
+		compatible = "renesas,r2a11302ft";
+		reg = <0>;
+		spi-max-frequency = <6000000>;
+		spi-cpol;
+		spi-cpha;
+	};
+};
diff --git a/arch/arm/boot/dts/r8a7791-koelsch.dts b/arch/arm/boot/dts/r8a7791-koelsch.dts
index bdd73e6657b..05d44f9b202 100644
--- a/arch/arm/boot/dts/r8a7791-koelsch.dts
+++ b/arch/arm/boot/dts/r8a7791-koelsch.dts
@@ -13,11 +13,17 @@
 /dts-v1/;
 #include "r8a7791.dtsi"
 #include <dt-bindings/gpio/gpio.h>
+#include <dt-bindings/input/input.h>
 
 / {
 	model = "Koelsch";
 	compatible = "renesas,koelsch", "renesas,r8a7791";
 
+	aliases {
+		serial6 = &scif0;
+		serial7 = &scif1;
+	};
+
 	chosen {
 		bootargs = "console=ttySC6,115200 ignore_loglevel rw root=/dev/nfs ip=dhcp";
 	};
@@ -40,51 +46,79 @@
 	gpio-keys {
 		compatible = "gpio-keys";
 
+		key-1 {
+			gpios = <&gpio5 0 GPIO_ACTIVE_LOW>;
+			linux,code = <KEY_1>;
+			label = "SW2-1";
+			gpio-key,wakeup;
+			debounce-interval = <20>;
+		};
+		key-2 {
+			gpios = <&gpio5 1 GPIO_ACTIVE_LOW>;
+			linux,code = <KEY_2>;
+			label = "SW2-2";
+			gpio-key,wakeup;
+			debounce-interval = <20>;
+		};
+		key-3 {
+			gpios = <&gpio5 2 GPIO_ACTIVE_LOW>;
+			linux,code = <KEY_3>;
+			label = "SW2-3";
+			gpio-key,wakeup;
+			debounce-interval = <20>;
+		};
+		key-4 {
+			gpios = <&gpio5 3 GPIO_ACTIVE_LOW>;
+			linux,code = <KEY_4>;
+			label = "SW2-4";
+			gpio-key,wakeup;
+			debounce-interval = <20>;
+		};
 		key-a {
 			gpios = <&gpio7 0 GPIO_ACTIVE_LOW>;
-			linux,code = <30>;
+			linux,code = <KEY_A>;
 			label = "SW30";
 			gpio-key,wakeup;
 			debounce-interval = <20>;
 		};
 		key-b {
 			gpios = <&gpio7 1 GPIO_ACTIVE_LOW>;
-			linux,code = <48>;
+			linux,code = <KEY_B>;
 			label = "SW31";
 			gpio-key,wakeup;
 			debounce-interval = <20>;
 		};
 		key-c {
 			gpios = <&gpio7 2 GPIO_ACTIVE_LOW>;
-			linux,code = <46>;
+			linux,code = <KEY_C>;
 			label = "SW32";
 			gpio-key,wakeup;
 			debounce-interval = <20>;
 		};
 		key-d {
 			gpios = <&gpio7 3 GPIO_ACTIVE_LOW>;
-			linux,code = <32>;
+			linux,code = <KEY_D>;
 			label = "SW33";
 			gpio-key,wakeup;
 			debounce-interval = <20>;
 		};
 		key-e {
 			gpios = <&gpio7 4 GPIO_ACTIVE_LOW>;
-			linux,code = <18>;
+			linux,code = <KEY_E>;
 			label = "SW34";
 			gpio-key,wakeup;
 			debounce-interval = <20>;
 		};
 		key-f {
 			gpios = <&gpio7 5 GPIO_ACTIVE_LOW>;
-			linux,code = <33>;
+			linux,code = <KEY_F>;
 			label = "SW35";
 			gpio-key,wakeup;
 			debounce-interval = <20>;
 		};
 		key-g {
 			gpios = <&gpio7 6 GPIO_ACTIVE_LOW>;
-			linux,code = <34>;
+			linux,code = <KEY_G>;
 			label = "SW36";
 			gpio-key,wakeup;
 			debounce-interval = <20>;
@@ -195,11 +229,16 @@
 	};
 };
 
+&i2c6 {
+	status = "okay";
+	clock-frequency = <100000>;
+};
+
 &pfc {
-	pinctrl-0 = <&du_pins &scif0_pins &scif1_pins>;
+	pinctrl-0 = <&du_pins>;
 	pinctrl-names = "default";
 
-	i2c2_pins: i2c {
+	i2c2_pins: i2c2 {
 		renesas,groups = "i2c2";
 		renesas,function = "i2c2";
 	};
@@ -230,24 +269,30 @@
 	};
 
 	sdhi0_pins: sd0 {
-		renesas,gpios = "sdhi0_data4", "sdhi0_ctrl";
+		renesas,groups = "sdhi0_data4", "sdhi0_ctrl";
 		renesas,function = "sdhi0";
 	};
 
 	sdhi1_pins: sd1 {
-		renesas,gpios = "sdhi1_data4", "sdhi1_ctrl";
+		renesas,groups = "sdhi1_data4", "sdhi1_ctrl";
 		renesas,function = "sdhi1";
 	};
 
 	sdhi2_pins: sd2 {
-		renesas,gpios = "sdhi2_data4", "sdhi2_ctrl";
+		renesas,groups = "sdhi2_data4", "sdhi2_ctrl";
 		renesas,function = "sdhi2";
 	};
 
-	qspi_pins: spi {
+	qspi_pins: spi0 {
 		renesas,groups = "qspi_ctrl", "qspi_data4";
 		renesas,function = "qspi";
 	};
+
+	msiof0_pins: spi1 {
+		renesas,groups = "msiof0_clk", "msiof0_sync", "msiof0_rx",
+				 "msiof0_tx";
+		renesas,function = "msiof0";
+	};
 };
 
 &ether {
@@ -262,6 +307,7 @@
 		reg = <1>;
 		interrupt-parent = <&irqc0>;
 		interrupts = <0 IRQ_TYPE_LEVEL_LOW>;
+		micrel,led-mode = <1>;
 	};
 };
 
@@ -269,6 +315,20 @@
 	status = "okay";
 };
 
+&scif0 {
+	pinctrl-0 = <&scif0_pins>;
+	pinctrl-names = "default";
+
+	status = "okay";
+};
+
+&scif1 {
+	pinctrl-0 = <&scif1_pins>;
+	pinctrl-names = "default";
+
+	status = "okay";
+};
+
 &sdhi0 {
 	pinctrl-0 = <&sdhi0_pins>;
 	pinctrl-names = "default";
@@ -301,7 +361,7 @@
 	status = "okay";
 };
 
-&spi {
+&qspi {
 	pinctrl-0 = <&qspi_pins>;
 	pinctrl-names = "default";
 
@@ -313,6 +373,8 @@
 		compatible = "spansion,s25fl512s";
 		reg = <0>;
 		spi-max-frequency = <30000000>;
+		spi-tx-bus-width = <4>;
+		spi-rx-bus-width = <4>;
 		m25p,fast-read;
 
 		partition@0 {
@@ -331,3 +393,18 @@
 		};
 	};
 };
+
+&msiof0 {
+	pinctrl-0 = <&msiof0_pins>;
+	pinctrl-names = "default";
+
+	status = "okay";
+
+	pmic: pmic@0 {
+		compatible = "renesas,r2a11302ft";
+		reg = <0>;
+		spi-max-frequency = <6000000>;
+		spi-cpol;
+		spi-cpha;
+	};
+};
diff --git a/arch/arm/boot/dts/r8a7791.dtsi b/arch/arm/boot/dts/r8a7791.dtsi
index 46181708e59..8d7ffaeff6e 100644
--- a/arch/arm/boot/dts/r8a7791.dtsi
+++ b/arch/arm/boot/dts/r8a7791.dtsi
@@ -27,6 +27,13 @@
 		i2c3 = &i2c3;
 		i2c4 = &i2c4;
 		i2c5 = &i2c5;
+		i2c6 = &i2c6;
+		i2c7 = &i2c7;
+		i2c8 = &i2c8;
+		spi0 = &qspi;
+		spi1 = &msiof0;
+		spi2 = &msiof1;
+		spi3 = &msiof2;
 	};
 
 	cpus {
@@ -37,14 +44,14 @@
 			device_type = "cpu";
 			compatible = "arm,cortex-a15";
 			reg = <0>;
-			clock-frequency = <1300000000>;
+			clock-frequency = <1500000000>;
 		};
 
 		cpu1: cpu@1 {
 			device_type = "cpu";
 			compatible = "arm,cortex-a15";
 			reg = <1>;
-			clock-frequency = <1300000000>;
+			clock-frequency = <1500000000>;
 		};
 	};
 
@@ -69,6 +76,7 @@
 		gpio-ranges = <&pfc 0 0 32>;
 		#interrupt-cells = <2>;
 		interrupt-controller;
+		clocks = <&mstp9_clks R8A7791_CLK_GPIO0>;
 	};
 
 	gpio1: gpio@e6051000 {
@@ -80,6 +88,7 @@
 		gpio-ranges = <&pfc 0 32 32>;
 		#interrupt-cells = <2>;
 		interrupt-controller;
+		clocks = <&mstp9_clks R8A7791_CLK_GPIO1>;
 	};
 
 	gpio2: gpio@e6052000 {
@@ -91,6 +100,7 @@
 		gpio-ranges = <&pfc 0 64 32>;
 		#interrupt-cells = <2>;
 		interrupt-controller;
+		clocks = <&mstp9_clks R8A7791_CLK_GPIO2>;
 	};
 
 	gpio3: gpio@e6053000 {
@@ -102,6 +112,7 @@
 		gpio-ranges = <&pfc 0 96 32>;
 		#interrupt-cells = <2>;
 		interrupt-controller;
+		clocks = <&mstp9_clks R8A7791_CLK_GPIO3>;
 	};
 
 	gpio4: gpio@e6054000 {
@@ -113,6 +124,7 @@
 		gpio-ranges = <&pfc 0 128 32>;
 		#interrupt-cells = <2>;
 		interrupt-controller;
+		clocks = <&mstp9_clks R8A7791_CLK_GPIO4>;
 	};
 
 	gpio5: gpio@e6055000 {
@@ -124,6 +136,7 @@
 		gpio-ranges = <&pfc 0 160 32>;
 		#interrupt-cells = <2>;
 		interrupt-controller;
+		clocks = <&mstp9_clks R8A7791_CLK_GPIO5>;
 	};
 
 	gpio6: gpio@e6055400 {
@@ -135,6 +148,7 @@
 		gpio-ranges = <&pfc 0 192 32>;
 		#interrupt-cells = <2>;
 		interrupt-controller;
+		clocks = <&mstp9_clks R8A7791_CLK_GPIO6>;
 	};
 
 	gpio7: gpio@e6055800 {
@@ -146,6 +160,7 @@
 		gpio-ranges = <&pfc 0 224 26>;
 		#interrupt-cells = <2>;
 		interrupt-controller;
+		clocks = <&mstp9_clks R8A7791_CLK_GPIO7>;
 	};
 
 	thermal@e61f0000 {
@@ -180,6 +195,7 @@
 			     <0 17 IRQ_TYPE_LEVEL_HIGH>;
 	};
 
+	/* The memory map in the User's Manual maps the cores to bus numbers */
 	i2c0: i2c@e6508000 {
 		#address-cells = <1>;
 		#size-cells = <0>;
@@ -231,6 +247,7 @@
 	};
 
 	i2c5: i2c@e6528000 {
+		/* doesn't need pinmux */
 		#address-cells = <1>;
 		#size-cells = <0>;
 		compatible = "renesas,i2c-r8a7791";
@@ -240,6 +257,37 @@
 		status = "disabled";
 	};
 
+	i2c6: i2c@e60b0000 {
+		/* doesn't need pinmux */
+		#address-cells = <1>;
+		#size-cells = <0>;
+		compatible = "renesas,iic-r8a7791", "renesas,rmobile-iic";
+		reg = <0 0xe60b0000 0 0x425>;
+		interrupts = <0 173 IRQ_TYPE_LEVEL_HIGH>;
+		clocks = <&mstp9_clks R8A7791_CLK_IICDVFS>;
+		status = "disabled";
+	};
+
+	i2c7: i2c@e6500000 {
+		#address-cells = <1>;
+		#size-cells = <0>;
+		compatible = "renesas,iic-r8a7791", "renesas,rmobile-iic";
+		reg = <0 0xe6500000 0 0x425>;
+		interrupts = <0 174 IRQ_TYPE_LEVEL_HIGH>;
+		clocks = <&mstp3_clks R8A7791_CLK_IIC0>;
+		status = "disabled";
+	};
+
+	i2c8: i2c@e6510000 {
+		#address-cells = <1>;
+		#size-cells = <0>;
+		compatible = "renesas,iic-r8a7791", "renesas,rmobile-iic";
+		reg = <0 0xe6510000 0 0x425>;
+		interrupts = <0 175 IRQ_TYPE_LEVEL_HIGH>;
+		clocks = <&mstp3_clks R8A7791_CLK_IIC1>;
+		status = "disabled";
+	};
+
 	pfc: pfc@e6060000 {
 		compatible = "renesas,pfc-r8a7791";
 		reg = <0 0xe6060000 0 0x250>;
@@ -249,7 +297,6 @@
 	sdhi0: sd@ee100000 {
 		compatible = "renesas,sdhi-r8a7791";
 		reg = <0 0xee100000 0 0x200>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 165 IRQ_TYPE_LEVEL_HIGH>;
 		clocks = <&mstp3_clks R8A7791_CLK_SDHI0>;
 		status = "disabled";
@@ -258,7 +305,6 @@
 	sdhi1: sd@ee140000 {
 		compatible = "renesas,sdhi-r8a7791";
 		reg = <0 0xee140000 0 0x100>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 167 IRQ_TYPE_LEVEL_HIGH>;
 		clocks = <&mstp3_clks R8A7791_CLK_SDHI1>;
 		status = "disabled";
@@ -267,7 +313,6 @@
 	sdhi2: sd@ee160000 {
 		compatible = "renesas,sdhi-r8a7791";
 		reg = <0 0xee160000 0 0x100>;
-		interrupt-parent = <&gic>;
 		interrupts = <0 168 IRQ_TYPE_LEVEL_HIGH>;
 		clocks = <&mstp3_clks R8A7791_CLK_SDHI2>;
 		status = "disabled";
@@ -688,7 +733,7 @@
 			renesas,clock-indices = <
 				R8A7791_CLK_TMU1 R8A7791_CLK_TMU3 R8A7791_CLK_TMU2
 				R8A7791_CLK_CMT0 R8A7791_CLK_TMU0 R8A7791_CLK_VSP1_DU1
-				R8A7791_CLK_VSP1_DU0 R8A7791_CLK_VSP1_SY
+				R8A7791_CLK_VSP1_DU0 R8A7791_CLK_VSP1_S
 			>;
 			clock-output-names =
 				"tmu1", "tmu3", "tmu2", "cmt0", "tmu0", "vsp1-du1",
@@ -712,15 +757,16 @@
 		mstp3_clks: mstp3_clks@e615013c {
 			compatible = "renesas,r8a7791-mstp-clocks", "renesas,cpg-mstp-clocks";
 			reg = <0 0xe615013c 0 4>, <0 0xe6150048 0 4>;
-			clocks = <&cp_clk>, <&sd2_clk>, <&sd1_clk>,
-				<&cpg_clocks R8A7791_CLK_SD0>, <&mmc0_clk>, <&rclk_clk>;
+			clocks = <&cp_clk>, <&sd2_clk>, <&sd1_clk>, <&cpg_clocks R8A7791_CLK_SD0>,
+				 <&mmc0_clk>, <&hp_clk>, <&hp_clk>, <&rclk_clk>;
 			#clock-cells = <1>;
 			renesas,clock-indices = <
-				R8A7791_CLK_TPU0 R8A7791_CLK_SDHI2 R8A7791_CLK_SDHI1
-				R8A7791_CLK_SDHI0 R8A7791_CLK_MMCIF0 R8A7791_CLK_CMT1
+				R8A7791_CLK_TPU0 R8A7791_CLK_SDHI2 R8A7791_CLK_SDHI1 R8A7791_CLK_SDHI0
+				R8A7791_CLK_MMCIF0 R8A7791_CLK_IIC0 R8A7791_CLK_IIC1 R8A7791_CLK_CMT1
 			>;
 			clock-output-names =
-				"tpu0", "sdhi2", "sdhi1", "sdhi0", "mmcif0", "cmt1";
+				"tpu0", "sdhi2", "sdhi1", "sdhi0",
+				"mmcif0", "i2c7", "i2c8", "cmt1";
 		};
 		mstp5_clks: mstp5_clks@e6150144 {
 			compatible = "renesas,r8a7791-mstp-clocks", "renesas,cpg-mstp-clocks";
@@ -733,19 +779,19 @@
 		mstp7_clks: mstp7_clks@e615014c {
 			compatible = "renesas,r8a7791-mstp-clocks", "renesas,cpg-mstp-clocks";
 			reg = <0 0xe615014c 0 4>, <0 0xe61501c4 0 4>;
-			clocks = <&mp_clk>, <&zs_clk>, <&p_clk>, <&p_clk>, <&zs_clk>,
+			clocks = <&mp_clk>,  <&mp_clk>, <&zs_clk>, <&p_clk>, <&p_clk>, <&zs_clk>,
 				 <&zs_clk>, <&p_clk>, <&p_clk>, <&p_clk>, <&p_clk>,
 				 <&zx_clk>, <&zx_clk>, <&zx_clk>;
 			#clock-cells = <1>;
 			renesas,clock-indices = <
-				R8A7791_CLK_HSUSB R8A7791_CLK_HSCIF2 R8A7791_CLK_SCIF5
+				R8A7791_CLK_EHCI R8A7791_CLK_HSUSB R8A7791_CLK_HSCIF2 R8A7791_CLK_SCIF5
 				R8A7791_CLK_SCIF4 R8A7791_CLK_HSCIF1 R8A7791_CLK_HSCIF0
 				R8A7791_CLK_SCIF3 R8A7791_CLK_SCIF2 R8A7791_CLK_SCIF1
 				R8A7791_CLK_SCIF0 R8A7791_CLK_DU1 R8A7791_CLK_DU0
 				R8A7791_CLK_LVDS0
 			>;
 			clock-output-names =
-				"hsusb", "hscif2", "scif5", "scif4", "hscif1", "hscif0",
+				"ehci", "hsusb", "hscif2", "scif5", "scif4", "hscif1", "hscif0",
 				"scif3", "scif2", "scif1", "scif0", "du1", "du0", "lvds0";
 		};
 		mstp8_clks: mstp8_clks@e6150990 {
@@ -764,18 +810,23 @@
 		mstp9_clks: mstp9_clks@e6150994 {
 			compatible = "renesas,r8a7791-mstp-clocks", "renesas,cpg-mstp-clocks";
 			reg = <0 0xe6150994 0 4>, <0 0xe61509a4 0 4>;
-			clocks = <&p_clk>, <&p_clk>, <&cpg_clocks R8A7791_CLK_QSPI>,
-				 <&p_clk>, <&p_clk>, <&p_clk>, <&p_clk>, <&p_clk>,
-				 <&p_clk>;
+			clocks = <&cp_clk>, <&cp_clk>, <&cp_clk>, <&cp_clk>,
+				 <&cp_clk>, <&cp_clk>, <&cp_clk>, <&cp_clk>,
+				 <&p_clk>, <&p_clk>, <&cpg_clocks R8A7791_CLK_QSPI>, <&hp_clk>,
+				 <&cp_clk>, <&hp_clk>, <&hp_clk>, <&hp_clk>,
+				 <&hp_clk>, <&hp_clk>;
 			#clock-cells = <1>;
 			renesas,clock-indices = <
-				R8A7791_CLK_RCAN1 R8A7791_CLK_RCAN0 R8A7791_CLK_QSPI_MOD
-				R8A7791_CLK_I2C5 R8A7791_CLK_I2C4 R8A7791_CLK_I2C3
-				R8A7791_CLK_I2C2 R8A7791_CLK_I2C1 R8A7791_CLK_I2C0
+				R8A7791_CLK_GPIO7 R8A7791_CLK_GPIO6 R8A7791_CLK_GPIO5 R8A7791_CLK_GPIO4
+				R8A7791_CLK_GPIO3 R8A7791_CLK_GPIO2 R8A7791_CLK_GPIO1 R8A7791_CLK_GPIO0
+				R8A7791_CLK_RCAN1 R8A7791_CLK_RCAN0 R8A7791_CLK_QSPI_MOD R8A7791_CLK_I2C5
+				R8A7791_CLK_IICDVFS R8A7791_CLK_I2C4 R8A7791_CLK_I2C3 R8A7791_CLK_I2C2
+				R8A7791_CLK_I2C1 R8A7791_CLK_I2C0
 			>;
 			clock-output-names =
-				"rcan1", "rcan0", "qspi_mod", "i2c5", "i2c4", "i2c3",
-				"i2c2", "i2c1", "i2c0";
+				"gpio7", "gpio6", "gpio5", "gpio4", "gpio3", "gpio2", "gpio1", "gpio0",
+				"rcan1", "rcan0", "qspi_mod", "i2c5", "i2c6", "i2c4", "i2c3", "i2c2",
+				"i2c1", "i2c0";
 		};
 		mstp11_clks: mstp11_clks@e615099c {
 			compatible = "renesas,r8a7791-mstp-clocks", "renesas,cpg-mstp-clocks";
@@ -789,7 +840,7 @@
 		};
 	};
 
-	spi: spi@e6b10000 {
+	qspi: spi@e6b10000 {
 		compatible = "renesas,qspi-r8a7791", "renesas,qspi";
 		reg = <0 0xe6b10000 0 0x2c>;
 		interrupts = <0 184 IRQ_TYPE_LEVEL_HIGH>;
@@ -799,4 +850,34 @@
 		#size-cells = <0>;
 		status = "disabled";
 	};
+
+	msiof0: spi@e6e20000 {
+		compatible = "renesas,msiof-r8a7791";
+		reg = <0 0xe6e20000 0 0x0064>;
+		interrupts = <0 156 IRQ_TYPE_LEVEL_HIGH>;
+		clocks = <&mstp0_clks R8A7791_CLK_MSIOF0>;
+		#address-cells = <1>;
+		#size-cells = <0>;
+		status = "disabled";
+	};
+
+	msiof1: spi@e6e10000 {
+		compatible = "renesas,msiof-r8a7791";
+		reg = <0 0xe6e10000 0 0x0064>;
+		interrupts = <0 157 IRQ_TYPE_LEVEL_HIGH>;
+		clocks = <&mstp2_clks R8A7791_CLK_MSIOF1>;
+		#address-cells = <1>;
+		#size-cells = <0>;
+		status = "disabled";
+	};
+
+	msiof2: spi@e6e00000 {
+		compatible = "renesas,msiof-r8a7791";
+		reg = <0 0xe6e00000 0 0x0064>;
+		interrupts = <0 158 IRQ_TYPE_LEVEL_HIGH>;
+		clocks = <&mstp2_clks R8A7791_CLK_MSIOF2>;
+		#address-cells = <1>;
+		#size-cells = <0>;
+		status = "disabled";
+	};
 };
diff --git a/arch/arm/boot/dts/rk3066a-bqcurie2.dts b/arch/arm/boot/dts/rk3066a-bqcurie2.dts
index 035df4053c2..afb327322a4 100644
--- a/arch/arm/boot/dts/rk3066a-bqcurie2.dts
+++ b/arch/arm/boot/dts/rk3066a-bqcurie2.dts
@@ -18,6 +18,7 @@
 
 / {
 	model = "bq Curie 2";
+	compatible = "mundoreader,bq-curie2", "rockchip,rk3066a";
 
 	memory {
 		reg = <0x60000000 0x40000000>;
diff --git a/arch/arm/boot/dts/rk3066a.dtsi b/arch/arm/boot/dts/rk3066a.dtsi
index 4d4dfbb59f4..4387cfd420b 100644
--- a/arch/arm/boot/dts/rk3066a.dtsi
+++ b/arch/arm/boot/dts/rk3066a.dtsi
@@ -24,6 +24,7 @@
 	cpus {
 		#address-cells = <1>;
 		#size-cells = <0>;
+		enable-method = "rockchip,rk3066-smp";
 
 		cpu@0 {
 			device_type = "cpu";
@@ -79,7 +80,7 @@
 
 		pinctrl@20008000 {
 			compatible = "rockchip,rk3066a-pinctrl";
-			reg = <0x20008000 0x150>;
+			rockchip,grf = <&grf>;
 			#address-cells = <1>;
 			#size-cells = <1>;
 			ranges;
diff --git a/arch/arm/boot/dts/rk3188-radxarock.dts b/arch/arm/boot/dts/rk3188-radxarock.dts
index 3ba1968a70a..a5eee55079c 100644
--- a/arch/arm/boot/dts/rk3188-radxarock.dts
+++ b/arch/arm/boot/dts/rk3188-radxarock.dts
@@ -17,6 +17,7 @@
 
 / {
 	model = "Radxa Rock";
+	compatible = "radxa,rock", "rockchip,rk3188";
 
 	memory {
 		reg = <0x60000000 0x80000000>;
diff --git a/arch/arm/boot/dts/rk3188.dtsi b/arch/arm/boot/dts/rk3188.dtsi
index bb36596ea20..238c996d4a7 100644
--- a/arch/arm/boot/dts/rk3188.dtsi
+++ b/arch/arm/boot/dts/rk3188.dtsi
@@ -24,6 +24,7 @@
 	cpus {
 		#address-cells = <1>;
 		#size-cells = <0>;
+		enable-method = "rockchip,rk3066-smp";
 
 		cpu@0 {
 			device_type = "cpu";
@@ -75,17 +76,16 @@
 
 		pinctrl@20008000 {
 			compatible = "rockchip,rk3188-pinctrl";
-			reg = <0x20008000 0xa0>,
-			      <0x20008164 0x1a0>;
-			reg-names = "base", "pull";
+			rockchip,grf = <&grf>;
+			rockchip,pmu = <&pmu>;
+
 			#address-cells = <1>;
 			#size-cells = <1>;
 			ranges;
 
 			gpio0: gpio0@0x2000a000 {
 				compatible = "rockchip,rk3188-gpio-bank0";
-				reg = <0x2000a000 0x100>,
-				      <0x20004064 0x8>;
+				reg = <0x2000a000 0x100>;
 				interrupts = <GIC_SPI 54 IRQ_TYPE_LEVEL_HIGH>;
 				clocks = <&clk_gates8 9>;
 
@@ -149,7 +149,7 @@
 
 			uart0 {
 				uart0_xfer: uart0-xfer {
-					rockchip,pins = <RK_GPIO1 0 RK_FUNC_1 &pcfg_pull_none>,
+					rockchip,pins = <RK_GPIO1 0 RK_FUNC_1 &pcfg_pull_up>,
 							<RK_GPIO1 1 RK_FUNC_1 &pcfg_pull_none>;
 				};
 
@@ -164,7 +164,7 @@
 
 			uart1 {
 				uart1_xfer: uart1-xfer {
-					rockchip,pins = <RK_GPIO1 4 RK_FUNC_1 &pcfg_pull_none>,
+					rockchip,pins = <RK_GPIO1 4 RK_FUNC_1 &pcfg_pull_up>,
 							<RK_GPIO1 5 RK_FUNC_1 &pcfg_pull_none>;
 				};
 
@@ -179,7 +179,7 @@
 
 			uart2 {
 				uart2_xfer: uart2-xfer {
-					rockchip,pins = <RK_GPIO1 8 RK_FUNC_1 &pcfg_pull_none>,
+					rockchip,pins = <RK_GPIO1 8 RK_FUNC_1 &pcfg_pull_up>,
 							<RK_GPIO1 9 RK_FUNC_1 &pcfg_pull_none>;
 				};
 				/* no rts / cts for uart2 */
@@ -187,7 +187,7 @@
 
 			uart3 {
 				uart3_xfer: uart3-xfer {
-					rockchip,pins = <RK_GPIO1 10 RK_FUNC_1 &pcfg_pull_none>,
+					rockchip,pins = <RK_GPIO1 10 RK_FUNC_1 &pcfg_pull_up>,
 							<RK_GPIO1 11 RK_FUNC_1 &pcfg_pull_none>;
 				};
 
diff --git a/arch/arm/boot/dts/rk3xxx.dtsi b/arch/arm/boot/dts/rk3xxx.dtsi
index 26e5a968d49..2adf1cc9e85 100644
--- a/arch/arm/boot/dts/rk3xxx.dtsi
+++ b/arch/arm/boot/dts/rk3xxx.dtsi
@@ -31,11 +31,16 @@
 			reg = <0x1013c000 0x100>;
 		};
 
-		pmu@20004000 {
-			compatible = "rockchip,rk3066-pmu";
+		pmu: pmu@20004000 {
+			compatible = "rockchip,rk3066-pmu", "syscon";
 			reg = <0x20004000 0x100>;
 		};
 
+		grf: grf@20008000 {
+			compatible = "syscon";
+			reg = <0x20008000 0x200>;
+		};
+
 		gic: interrupt-controller@1013d000 {
 			compatible = "arm,cortex-a9-gic";
 			interrupt-controller;
diff --git a/arch/arm/boot/dts/s3c2416-smdk2416.dts b/arch/arm/boot/dts/s3c2416-smdk2416.dts
index 59594cf1599..ea92fd69529 100644
--- a/arch/arm/boot/dts/s3c2416-smdk2416.dts
+++ b/arch/arm/boot/dts/s3c2416-smdk2416.dts
@@ -19,6 +19,19 @@
 		reg =  <0x30000000 0x4000000>;
 	};
 
+	clocks {
+		compatible = "simple-bus";
+		#address-cells = <1>;
+		#size-cells = <1>;
+
+		xti: xti {
+			compatible = "fixed-clock";
+			clock-frequency = <12000000>;
+			clock-output-names = "xti";
+			#clock-cells = <0>;
+		};
+	};
+
 	serial@50000000 {
 		status = "okay";
 		pinctrl-names = "default";
diff --git a/arch/arm/boot/dts/s3c2416.dtsi b/arch/arm/boot/dts/s3c2416.dtsi
index e6555bdd81b..955e4a4f8c3 100644
--- a/arch/arm/boot/dts/s3c2416.dtsi
+++ b/arch/arm/boot/dts/s3c2416.dtsi
@@ -8,6 +8,7 @@
  * published by the Free Software Foundation.
  */
 
+#include <dt-bindings/clock/s3c2443.h>
 #include "s3c24xx.dtsi"
 #include "s3c2416-pinctrl.dtsi"
 
@@ -28,26 +29,53 @@
 		compatible = "samsung,s3c2416-irq";
 	};
 
+	clocks: clock-controller@0x4c000000 {
+		compatible = "samsung,s3c2416-clock";
+		reg = <0x4c000000 0x40>;
+		#clock-cells = <1>;
+	};
+
 	pinctrl@56000000 {
 		compatible = "samsung,s3c2416-pinctrl";
 	};
 
+	timer@51000000 {
+		clocks = <&clocks PCLK_PWM>;
+		clock-names = "timers";
+	};
+
 	serial@50000000 {
 		compatible = "samsung,s3c2440-uart";
+		clock-names = "uart", "clk_uart_baud2",
+				"clk_uart_baud3";
+		clocks = <&clocks PCLK_UART0>, <&clocks PCLK_UART0>,
+				<&clocks SCLK_UART>;
 	};
 
 	serial@50004000 {
 		compatible = "samsung,s3c2440-uart";
+		clock-names = "uart", "clk_uart_baud2",
+				"clk_uart_baud3";
+		clocks = <&clocks PCLK_UART1>, <&clocks PCLK_UART1>,
+				<&clocks SCLK_UART>;
 	};
 
 	serial@50008000 {
 		compatible = "samsung,s3c2440-uart";
+		clock-names = "uart", "clk_uart_baud2",
+				"clk_uart_baud3";
+		clocks = <&clocks PCLK_UART2>, <&clocks PCLK_UART2>,
+				<&clocks SCLK_UART>;
 	};
 
 	serial@5000C000 {
 		compatible = "samsung,s3c2440-uart";
 		reg = <0x5000C000 0x4000>;
 		interrupts = <1 18 24 4>, <1 18 25 4>;
+		clock-names = "uart", "clk_uart_baud2",
+				"clk_uart_baud3";
+		clocks = <&clocks PCLK_UART3>, <&clocks PCLK_UART3>,
+				<&clocks SCLK_UART>;
 		status = "disabled";
 	};
 
@@ -55,6 +83,10 @@
 		compatible = "samsung,s3c6410-sdhci";
 		reg = <0x4AC00000 0x100>;
 		interrupts = <0 0 21 3>;
+		clock-names = "hsmmc", "mmc_busclk.0",
+				"mmc_busclk.2";
+		clocks = <&clocks HCLK_HSMMC0>, <&clocks HCLK_HSMMC0>,
+				<&clocks MUX_HSMMC0>;
 		status = "disabled";
 	};
 
@@ -62,18 +94,28 @@
 		compatible = "samsung,s3c6410-sdhci";
 		reg = <0x4A800000 0x100>;
 		interrupts = <0 0 20 3>;
+		clock-names = "hsmmc", "mmc_busclk.0",
+				"mmc_busclk.2";
+		clocks = <&clocks HCLK_HSMMC1>, <&clocks HCLK_HSMMC1>,
+				<&clocks MUX_HSMMC1>;
 		status = "disabled";
 	};
 
 	watchdog@53000000 {
 		interrupts = <1 9 27 3>;
+		clocks = <&clocks PCLK_WDT>;
+		clock-names = "watchdog";
 	};
 
 	rtc@57000000 {
 		compatible = "samsung,s3c2416-rtc";
+		clocks = <&clocks PCLK_RTC>;
+		clock-names = "rtc";
 	};
 
 	i2c@54000000 {
 		compatible = "samsung,s3c2440-i2c";
+		clocks = <&clocks PCLK_I2C0>;
+		clock-names = "i2c";
 	};
 };
diff --git a/arch/arm/boot/dts/sama5d3.dtsi b/arch/arm/boot/dts/sama5d3.dtsi
index eabcfdbb403..e0b15a6e889 100644
--- a/arch/arm/boot/dts/sama5d3.dtsi
+++ b/arch/arm/boot/dts/sama5d3.dtsi
@@ -13,7 +13,7 @@
 #include <dt-bindings/pinctrl/at91.h>
 #include <dt-bindings/interrupt-controller/irq.h>
 #include <dt-bindings/gpio/gpio.h>
-#include <dt-bindings/clk/at91.h>
+#include <dt-bindings/clock/at91.h>
 
 / {
 	model = "Atmel SAMA5D3 family SoC";
@@ -58,6 +58,18 @@
 		reg = <0x20000000 0x8000000>;
 	};
 
+	slow_xtal: slow_xtal {
+		compatible = "fixed-clock";
+		#clock-cells = <0>;
+		clock-frequency = <0>;
+	};
+
+	main_xtal: main_xtal {
+		compatible = "fixed-clock";
+		#clock-cells = <0>;
+		clock-frequency = <0>;
+	};
+
 	clocks {
 		adc_op_clk: adc_op_clk{
 			compatible = "fixed-clock";
@@ -113,6 +125,9 @@
 				compatible = "atmel,at91sam9g45-ssc";
 				reg = <0xf0008000 0x4000>;
 				interrupts = <38 IRQ_TYPE_LEVEL_HIGH 4>;
+				dmas = <&dma0 2 AT91_DMA_CFG_PER_ID(13)>,
+				       <&dma0 2 AT91_DMA_CFG_PER_ID(14)>;
+				dma-names = "tx", "rx";
 				pinctrl-names = "default";
 				pinctrl-0 = <&pinctrl_ssc0_tx &pinctrl_ssc0_rx>;
 				clocks = <&ssc0_clk>;
@@ -231,6 +246,9 @@
 				compatible = "atmel,at91sam9g45-ssc";
 				reg = <0xf800c000 0x4000>;
 				interrupts = <39 IRQ_TYPE_LEVEL_HIGH 4>;
+				dmas = <&dma1 2 AT91_DMA_CFG_PER_ID(3)>,
+				       <&dma1 2 AT91_DMA_CFG_PER_ID(4)>;
+				dma-names = "tx", "rx";
 				pinctrl-names = "default";
 				pinctrl-0 = <&pinctrl_ssc1_tx &pinctrl_ssc1_rx>;
 				clocks = <&ssc1_clk>;
@@ -577,6 +595,84 @@
 					};
 				};
 
+				pwm0 {
+					pinctrl_pwm0_pwmh0_0: pwm0_pwmh0-0 {
+						atmel,pins =
+							<AT91_PIOA 20 AT91_PERIPH_B AT91_PINCTRL_NONE>;	/* conflicts with ISI_D4 and LCDDAT20 */
+					};
+					pinctrl_pwm0_pwmh0_1: pwm0_pwmh0-1 {
+						atmel,pins =
+							<AT91_PIOB 0 AT91_PERIPH_B AT91_PINCTRL_NONE>;	/* conflicts with GTX0 */
+					};
+					pinctrl_pwm0_pwml0_0: pwm0_pwml0-0 {
+						atmel,pins =
+							<AT91_PIOA 21 AT91_PERIPH_B AT91_PINCTRL_NONE>;	/* conflicts with ISI_D5 and LCDDAT21 */
+					};
+					pinctrl_pwm0_pwml0_1: pwm0_pwml0-1 {
+						atmel,pins =
+							<AT91_PIOB 1 AT91_PERIPH_B AT91_PINCTRL_NONE>;	/* conflicts with GTX1 */
+					};
+
+					pinctrl_pwm0_pwmh1_0: pwm0_pwmh1-0 {
+						atmel,pins =
+							<AT91_PIOA 22 AT91_PERIPH_B AT91_PINCTRL_NONE>;	/* conflicts with ISI_D6 and LCDDAT22 */
+					};
+					pinctrl_pwm0_pwmh1_1: pwm0_pwmh1-1 {
+						atmel,pins =
+							<AT91_PIOB 4 AT91_PERIPH_B AT91_PINCTRL_NONE>;	/* conflicts with GRX0 */
+					};
+					pinctrl_pwm0_pwmh1_2: pwm0_pwmh1-2 {
+						atmel,pins =
+							<AT91_PIOB 27 AT91_PERIPH_C AT91_PINCTRL_NONE>;	/* conflicts with G125CKO and RTS1 */
+					};
+					pinctrl_pwm0_pwml1_0: pwm0_pwml1-0 {
+						atmel,pins =
+							<AT91_PIOA 23 AT91_PERIPH_B AT91_PINCTRL_NONE>;	/* conflicts with ISI_D7 and LCDDAT23 */
+					};
+					pinctrl_pwm0_pwml1_1: pwm0_pwml1-1 {
+						atmel,pins =
+							<AT91_PIOB 5 AT91_PERIPH_B AT91_PINCTRL_NONE>;	/* conflicts with GRX1 */
+					};
+					pinctrl_pwm0_pwml1_2: pwm0_pwml1-2 {
+						atmel,pins =
+							<AT91_PIOE 31 AT91_PERIPH_B AT91_PINCTRL_NONE>;	/* conflicts with IRQ */
+					};
+
+					pinctrl_pwm0_pwmh2_0: pwm0_pwmh2-0 {
+						atmel,pins =
+							<AT91_PIOB 8 AT91_PERIPH_B AT91_PINCTRL_NONE>;	/* conflicts with GTXCK */
+					};
+					pinctrl_pwm0_pwmh2_1: pwm0_pwmh2-1 {
+						atmel,pins =
+							<AT91_PIOD 5 AT91_PERIPH_C AT91_PINCTRL_NONE>;	/* conflicts with MCI0_DA4 and TIOA0 */
+					};
+					pinctrl_pwm0_pwml2_0: pwm0_pwml2-0 {
+						atmel,pins =
+							<AT91_PIOB 9 AT91_PERIPH_B AT91_PINCTRL_NONE>;	/* conflicts with GTXEN */
+					};
+					pinctrl_pwm0_pwml2_1: pwm0_pwml2-1 {
+						atmel,pins =
+							<AT91_PIOD 6 AT91_PERIPH_C AT91_PINCTRL_NONE>;	/* conflicts with MCI0_DA5 and TIOB0 */
+					};
+
+					pinctrl_pwm0_pwmh3_0: pwm0_pwmh3-0 {
+						atmel,pins =
+							<AT91_PIOB 12 AT91_PERIPH_B AT91_PINCTRL_NONE>;	/* conflicts with GRXDV */
+					};
+					pinctrl_pwm0_pwmh3_1: pwm0_pwmh3-1 {
+						atmel,pins =
+							<AT91_PIOD 7 AT91_PERIPH_C AT91_PINCTRL_NONE>;	/* conflicts with MCI0_DA6 and TCLK0 */
+					};
+					pinctrl_pwm0_pwml3_0: pwm0_pwml3-0 {
+						atmel,pins =
+							<AT91_PIOB 13 AT91_PERIPH_B AT91_PINCTRL_NONE>;	/* conflicts with GRXER */
+					};
+					pinctrl_pwm0_pwml3_1: pwm0_pwml3-1 {
+						atmel,pins =
+							<AT91_PIOD 8 AT91_PERIPH_C AT91_PINCTRL_NONE>;	/* conflicts with MCI0_DA7 */
+					};
+				};
+
 				spi0 {
 					pinctrl_spi0: spi0-0 {
 						atmel,pins =
@@ -749,18 +845,29 @@
 				#size-cells = <0>;
 				#interrupt-cells = <1>;
 
-				clk32k: slck {
-					compatible = "fixed-clock";
+				main_rc_osc: main_rc_osc {
+					compatible = "atmel,at91sam9x5-clk-main-rc-osc";
 					#clock-cells = <0>;
-					clock-frequency = <32768>;
+					interrupt-parent = <&pmc>;
+					interrupts = <AT91_PMC_MOSCRCS>;
+					clock-frequency = <12000000>;
+					clock-accuracy = <50000000>;
 				};
 
-				main: mainck {
-					compatible = "atmel,at91rm9200-clk-main";
+				main_osc: main_osc {
+					compatible = "atmel,at91rm9200-clk-main-osc";
 					#clock-cells = <0>;
 					interrupt-parent = <&pmc>;
 					interrupts = <AT91_PMC_MOSCS>;
-					clocks = <&clk32k>;
+					clocks = <&main_xtal>;
+				};
+
+				main: mainck {
+					compatible = "atmel,at91sam9x5-clk-main";
+					#clock-cells = <0>;
+					interrupt-parent = <&pmc>;
+					interrupts = <AT91_PMC_MOSCSELS>;
+					clocks = <&main_rc_osc &main_osc>;
 				};
 
 				plla: pllack {
@@ -1089,6 +1196,32 @@
 				status = "disabled";
 			};
 
+			sckc@fffffe50 {
+				compatible = "atmel,at91sam9x5-sckc";
+				reg = <0xfffffe50 0x4>;
+
+				slow_rc_osc: slow_rc_osc {
+					compatible = "atmel,at91sam9x5-clk-slow-rc-osc";
+					#clock-cells = <0>;
+					clock-frequency = <32768>;
+					clock-accuracy = <50000000>;
+					atmel,startup-time-usec = <75>;
+				};
+
+				slow_osc: slow_osc {
+					compatible = "atmel,at91sam9x5-clk-slow-osc";
+					#clock-cells = <0>;
+					clocks = <&slow_xtal>;
+					atmel,startup-time-usec = <1200000>;
+				};
+
+				clk32k: slowck {
+					compatible = "atmel,at91sam9x5-clk-slow";
+					#clock-cells = <0>;
+					clocks = <&slow_rc_osc &slow_osc>;
+				};
+			};
+
 			rtc@fffffeb0 {
 				compatible = "atmel,at91rm9200-rtc";
 				reg = <0xfffffeb0 0x30>;
diff --git a/arch/arm/boot/dts/sama5d3_mci2.dtsi b/arch/arm/boot/dts/sama5d3_mci2.dtsi
index b029fe7ef17..1b02208ea6f 100644
--- a/arch/arm/boot/dts/sama5d3_mci2.dtsi
+++ b/arch/arm/boot/dts/sama5d3_mci2.dtsi
@@ -9,7 +9,7 @@
 
 #include <dt-bindings/pinctrl/at91.h>
 #include <dt-bindings/interrupt-controller/irq.h>
-#include <dt-bindings/clk/at91.h>
+#include <dt-bindings/clock/at91.h>
 
 / {
 	ahb {
diff --git a/arch/arm/boot/dts/sama5d3_tcb1.dtsi b/arch/arm/boot/dts/sama5d3_tcb1.dtsi
index 382b04431f6..02848453ca0 100644
--- a/arch/arm/boot/dts/sama5d3_tcb1.dtsi
+++ b/arch/arm/boot/dts/sama5d3_tcb1.dtsi
@@ -9,7 +9,7 @@
 
 #include <dt-bindings/pinctrl/at91.h>
 #include <dt-bindings/interrupt-controller/irq.h>
-#include <dt-bindings/clk/at91.h>
+#include <dt-bindings/clock/at91.h>
 
 / {
 	aliases {
diff --git a/arch/arm/boot/dts/sama5d3_uart.dtsi b/arch/arm/boot/dts/sama5d3_uart.dtsi
index a9fa75e4165..7a8d4c6115f 100644
--- a/arch/arm/boot/dts/sama5d3_uart.dtsi
+++ b/arch/arm/boot/dts/sama5d3_uart.dtsi
@@ -9,7 +9,7 @@
 
 #include <dt-bindings/pinctrl/at91.h>
 #include <dt-bindings/interrupt-controller/irq.h>
-#include <dt-bindings/clk/at91.h>
+#include <dt-bindings/clock/at91.h>
 
 / {
 	aliases {
diff --git a/arch/arm/boot/dts/sama5d3xcm.dtsi b/arch/arm/boot/dts/sama5d3xcm.dtsi
index f55ed072c8e..b0b1331c197 100644
--- a/arch/arm/boot/dts/sama5d3xcm.dtsi
+++ b/arch/arm/boot/dts/sama5d3xcm.dtsi
@@ -18,6 +18,14 @@
 		reg = <0x20000000 0x20000000>;
 	};
 
+	slow_xtal {
+		clock-frequency = <32768>;
+	};
+
+	main_xtal {
+		clock-frequency = <12000000>;
+	};
+
 	ahb {
 		apb {
 			spi0: spi@f0004000 {
diff --git a/arch/arm/boot/dts/sama5d3xmb.dtsi b/arch/arm/boot/dts/sama5d3xmb.dtsi
index dba739b6ef3..306eef0f97e 100644
--- a/arch/arm/boot/dts/sama5d3xmb.dtsi
+++ b/arch/arm/boot/dts/sama5d3xmb.dtsi
@@ -32,6 +32,10 @@
 				};
 			};
 
+			ssc0: ssc@f0008000 {
+				atmel,clk-from-rk-pin;
+			};
+
 			/*
 			 * i2c0 conflicts with ISI:
 			 * disable it to allow the use of ISI
@@ -156,7 +160,7 @@
 	};
 
 	sound {
-		compatible = "atmel,sama5d3ek-wm8904";
+		compatible = "atmel,asoc-wm8904";
 		pinctrl-names = "default";
 		pinctrl-0 = <&pinctrl_pck0_as_audio_mck>;
 
@@ -166,9 +170,12 @@
 			"Headphone Jack", "HPOUTR",
 			"IN2L", "Line In Jack",
 			"IN2R", "Line In Jack",
+			"MICBIAS", "IN1L",
 			"IN1L", "Mic";
 
 		atmel,ssc-controller = <&ssc0>;
 		atmel,audio-codec = <&wm8904>;
+
+		status = "disabled";
 	};
 };
diff --git a/arch/arm/boot/dts/sh73a0-kzm9g-reference.dts b/arch/arm/boot/dts/sh73a0-kzm9g-reference.dts
index eb8886b535e..a99171c8a78 100644
--- a/arch/arm/boot/dts/sh73a0-kzm9g-reference.dts
+++ b/arch/arm/boot/dts/sh73a0-kzm9g-reference.dts
@@ -14,6 +14,7 @@
 /dts-v1/;
 #include "sh73a0.dtsi"
 #include <dt-bindings/gpio/gpio.h>
+#include <dt-bindings/input/input.h>
 #include <dt-bindings/interrupt-controller/irq.h>
 
 / {
@@ -112,43 +113,43 @@
 
 		back-key {
 			gpios = <&pcf8575 8 GPIO_ACTIVE_LOW>;
-			linux,code = <158>;
+			linux,code = <KEY_BACK>;
 			label = "SW3";
 		};
 
 		right-key {
 			gpios = <&pcf8575 9 GPIO_ACTIVE_LOW>;
-			linux,code = <106>;
+			linux,code = <KEY_RIGHT>;
 			label = "SW2-R";
 		};
 
 		left-key {
 			gpios = <&pcf8575 10 GPIO_ACTIVE_LOW>;
-			linux,code = <105>;
+			linux,code = <KEY_LEFT>;
 			label = "SW2-L";
 		};
 
 		enter-key {
 			gpios = <&pcf8575 11 GPIO_ACTIVE_LOW>;
-			linux,code = <28>;
+			linux,code = <KEY_ENTER>;
 			label = "SW2-P";
 		};
 
 		up-key {
 			gpios = <&pcf8575 12 GPIO_ACTIVE_LOW>;
-			linux,code = <103>;
+			linux,code = <KEY_UP>;
 			label = "SW2-U";
 		};
 
 		down-key {
 			gpios = <&pcf8575 13 GPIO_ACTIVE_LOW>;
-			linux,code = <108>;
+			linux,code = <KEY_DOWN>;
 			label = "SW2-D";
 		};
 
 		home-key {
 			gpios = <&pcf8575 14 GPIO_ACTIVE_LOW>;
-			linux,code = <102>;
+			linux,code = <KEY_HOME>;
 			label = "SW1";
 		};
 	};
diff --git a/arch/arm/boot/dts/sh73a0.dtsi b/arch/arm/boot/dts/sh73a0.dtsi
index b7bd3b9a675..5ecf552e1c0 100644
--- a/arch/arm/boot/dts/sh73a0.dtsi
+++ b/arch/arm/boot/dts/sh73a0.dtsi
@@ -34,7 +34,6 @@
 	gic: interrupt-controller@f0001000 {
 		compatible = "arm,cortex-a9-gic";
 		#interrupt-cells = <3>;
-		#address-cells = <1>;
 		interrupt-controller;
 		reg = <0xf0001000 0x1000>,
 		      <0xf0000100 0x100>;
diff --git a/arch/arm/boot/dts/socfpga.dtsi b/arch/arm/boot/dts/socfpga.dtsi
index 56fc214e6d2..4676f25e87a 100644
--- a/arch/arm/boot/dts/socfpga.dtsi
+++ b/arch/arm/boot/dts/socfpga.dtsi
@@ -15,7 +15,8 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-/include/ "skeleton.dtsi"
+#include "skeleton.dtsi"
+#include <dt-bindings/reset/altr,rst-mgr.h>
 
 / {
 	#address-cells = <1>;
@@ -75,7 +76,14 @@
 			pdma: pdma@ffe01000 {
 				compatible = "arm,pl330", "arm,primecell";
 				reg = <0xffe01000 0x1000>;
-				interrupts = <0 180 4>;
+				interrupts = <0 104 4>,
+					     <0 105 4>,
+					     <0 106 4>,
+					     <0 107 4>,
+					     <0 108 4>,
+					     <0 109 4>,
+					     <0 110 4>,
+					     <0 111 4>;
 				#dma-cells = <1>;
 				#dma-channels = <8>;
 				#dma-requests = <32>;
@@ -84,6 +92,22 @@
 			};
 		};
 
+		can0: can@ffc00000 {
+			compatible = "bosch,d_can";
+			reg = <0xffc00000 0x1000>;
+			interrupts = <0 131 4>, <0 132 4>, <0 133 4>, <0 134 4>;
+			clocks = <&can0_clk>;
+			status = "disabled";
+		};
+
+		can1: can@ffc01000 {
+			compatible = "bosch,d_can";
+			reg = <0xffc01000 0x1000>;
+			interrupts = <0 135 4>, <0 136 4>, <0 137 4>, <0 138 4>;
+			clocks = <&can1_clk>;
+			status = "disabled";
+		};
+
 		clkmgr@ffd04000 {
 				compatible = "altr,clk-mgr";
 				reg = <0xffd04000 0x1000>;
@@ -124,7 +148,7 @@
 							#clock-cells = <0>;
 							compatible = "altr,socfpga-perip-clk";
 							clocks = <&main_pll>;
-							fixed-divider = <2>;
+							div-reg = <0xe0 0 9>;
 							reg = <0x48>;
 						};
 
@@ -132,7 +156,7 @@
 							#clock-cells = <0>;
 							compatible = "altr,socfpga-perip-clk";
 							clocks = <&main_pll>;
-							fixed-divider = <4>;
+							div-reg = <0xe4 0 9>;
 							reg = <0x4C>;
 						};
 
@@ -140,7 +164,7 @@
 							#clock-cells = <0>;
 							compatible = "altr,socfpga-perip-clk";
 							clocks = <&main_pll>;
-							fixed-divider = <4>;
+							div-reg = <0xe8 0 9>;
 							reg = <0x50>;
 						};
 
@@ -460,6 +484,8 @@
 			mac-address = [00 00 00 00 00 00];/* Filled in by U-Boot */
 			clocks = <&emac0_clk>;
 			clock-names = "stmmaceth";
+			resets = <&rst EMAC0_RESET>;
+			reset-names = "stmmaceth";
 			status = "disabled";
 		};
 
@@ -472,9 +498,111 @@
 			mac-address = [00 00 00 00 00 00];/* Filled in by U-Boot */
 			clocks = <&emac1_clk>;
 			clock-names = "stmmaceth";
+			resets = <&rst EMAC1_RESET>;
+			reset-names = "stmmaceth";
+			status = "disabled";
+		};
+
+		i2c0: i2c@ffc04000 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			compatible = "snps,designware-i2c";
+			reg = <0xffc04000 0x1000>;
+			clocks = <&l4_sp_clk>;
+			interrupts = <0 158 0x4>;
+			status = "disabled";
+		};
+
+		i2c1: i2c@ffc05000 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			compatible = "snps,designware-i2c";
+			reg = <0xffc05000 0x1000>;
+			clocks = <&l4_sp_clk>;
+			interrupts = <0 159 0x4>;
 			status = "disabled";
 		};
 
+		i2c2: i2c@ffc06000 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			compatible = "snps,designware-i2c";
+			reg = <0xffc06000 0x1000>;
+			clocks = <&l4_sp_clk>;
+			interrupts = <0 160 0x4>;
+			status = "disabled";
+		};
+
+		i2c3: i2c@ffc07000 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			compatible = "snps,designware-i2c";
+			reg = <0xffc07000 0x1000>;
+			clocks = <&l4_sp_clk>;
+			interrupts = <0 161 0x4>;
+			status = "disabled";
+		};
+
+		gpio@ff708000 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			compatible = "snps,dw-apb-gpio";
+			reg = <0xff708000 0x1000>;
+			clocks = <&per_base_clk>;
+			status = "disabled";
+
+			gpio0: gpio-controller@0 {
+				compatible = "snps,dw-apb-gpio-port";
+				gpio-controller;
+				#gpio-cells = <2>;
+				snps,nr-gpios = <29>;
+				reg = <0>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				interrupts = <0 164 4>;
+			};
+		};
+
+		gpio@ff709000 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			compatible = "snps,dw-apb-gpio";
+			reg = <0xff709000 0x1000>;
+			clocks = <&per_base_clk>;
+			status = "disabled";
+
+			gpio1: gpio-controller@0 {
+				compatible = "snps,dw-apb-gpio-port";
+				gpio-controller;
+				#gpio-cells = <2>;
+				snps,nr-gpios = <29>;
+				reg = <0>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				interrupts = <0 165 4>;
+			};
+		};
+
+		gpio@ff70a000 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			compatible = "snps,dw-apb-gpio";
+			reg = <0xff70a000 0x1000>;
+			clocks = <&per_base_clk>;
+			status = "disabled";
+
+			gpio2: gpio-controller@0 {
+				compatible = "snps,dw-apb-gpio-port";
+				gpio-controller;
+				#gpio-cells = <2>;
+				snps,nr-gpios = <27>;
+				reg = <0>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				interrupts = <0 166 4>;
+			};
+		};
+
 		L2: l2-cache@fffef000 {
 			compatible = "arm,pl310-cache";
 			reg = <0xfffef000 0x1000>;
@@ -508,24 +636,32 @@
 			compatible = "snps,dw-apb-timer";
 			interrupts = <0 167 4>;
 			reg = <0xffc08000 0x1000>;
+			clocks = <&l4_sp_clk>;
+			clock-names = "timer";
 		};
 
 		timer1: timer1@ffc09000 {
 			compatible = "snps,dw-apb-timer";
 			interrupts = <0 168 4>;
 			reg = <0xffc09000 0x1000>;
+			clocks = <&l4_sp_clk>;
+			clock-names = "timer";
 		};
 
 		timer2: timer2@ffd00000 {
 			compatible = "snps,dw-apb-timer";
 			interrupts = <0 169 4>;
 			reg = <0xffd00000 0x1000>;
+			clocks = <&osc1>;
+			clock-names = "timer";
 		};
 
 		timer3: timer3@ffd01000 {
 			compatible = "snps,dw-apb-timer";
 			interrupts = <0 170 4>;
 			reg = <0xffd01000 0x1000>;
+			clocks = <&osc1>;
+			clock-names = "timer";
 		};
 
 		uart0: serial0@ffc02000 {
@@ -534,6 +670,7 @@
 			interrupts = <0 162 4>;
 			reg-shift = <2>;
 			reg-io-width = <4>;
+			clocks = <&l4_sp_clk>;
 		};
 
 		uart1: serial1@ffc03000 {
@@ -542,13 +679,58 @@
 			interrupts = <0 163 4>;
 			reg-shift = <2>;
 			reg-io-width = <4>;
+			clocks = <&l4_sp_clk>;
 		};
 
-		rstmgr@ffd05000 {
+		rst: rstmgr@ffd05000 {
 			compatible = "altr,rst-mgr";
 			reg = <0xffd05000 0x1000>;
 		};
 
+		usbphy0: usbphy@0 {
+			#phy-cells = <0>;
+			compatible = "usb-nop-xceiv";
+			status = "okay";
+		};
+
+		usb0: usb@ffb00000 {
+			compatible = "snps,dwc2";
+			reg = <0xffb00000 0xffff>;
+			interrupts = <0 125 4>;
+			clocks = <&usb_mp_clk>;
+			clock-names = "otg";
+			phys = <&usbphy0>;
+			phy-names = "usb2-phy";
+			status = "disabled";
+		};
+
+		usb1: usb@ffb40000 {
+			compatible = "snps,dwc2";
+			reg = <0xffb40000 0xffff>;
+			interrupts = <0 128 4>;
+			clocks = <&usb_mp_clk>;
+			clock-names = "otg";
+			phys = <&usbphy0>;
+			phy-names = "usb2-phy";
+			status = "disabled";
+		};
+
+		watchdog0: watchdog@ffd02000 {
+			compatible = "snps,dw-wdt";
+			reg = <0xffd02000 0x1000>;
+			interrupts = <0 171 4>;
+			clocks = <&osc1>;
+			status = "disabled";
+		};
+
+		watchdog1: watchdog@ffd03000 {
+			compatible = "snps,dw-wdt";
+			reg = <0xffd03000 0x1000>;
+			interrupts = <0 172 4>;
+			clocks = <&osc1>;
+			status = "disabled";
+		};
+
 		sysmgr: sysmgr@ffd08000 {
 			compatible = "altr,sys-mgr", "syscon";
 			reg = <0xffd08000 0x4000>;
diff --git a/arch/arm/boot/dts/socfpga_arria5.dtsi b/arch/arm/boot/dts/socfpga_arria5.dtsi
index 6c87b7070ca..12d1c2ccaf5 100644
--- a/arch/arm/boot/dts/socfpga_arria5.dtsi
+++ b/arch/arm/boot/dts/socfpga_arria5.dtsi
@@ -15,7 +15,7 @@
  */
 
 /dts-v1/;
-/include/ "socfpga.dtsi"
+#include "socfpga.dtsi"
 
 / {
 	soc {
@@ -38,32 +38,8 @@
 			};
 		};
 
-		serial0@ffc02000 {
-			clock-frequency = <100000000>;
-		};
-
-		serial1@ffc03000 {
-			clock-frequency = <100000000>;
-		};
-
 		sysmgr@ffd08000 {
 			cpu1-start-addr = <0xffd080c4>;
 		};
-
-		timer0@ffc08000 {
-			clock-frequency = <100000000>;
-		};
-
-		timer1@ffc09000 {
-			clock-frequency = <100000000>;
-		};
-
-		timer2@ffd00000 {
-			clock-frequency = <25000000>;
-		};
-
-		timer3@ffd01000 {
-			clock-frequency = <25000000>;
-		};
 	};
 };
diff --git a/arch/arm/boot/dts/socfpga_arria5_socdk.dts b/arch/arm/boot/dts/socfpga_arria5_socdk.dts
index a87ee1c0766..d532d171e39 100644
--- a/arch/arm/boot/dts/socfpga_arria5_socdk.dts
+++ b/arch/arm/boot/dts/socfpga_arria5_socdk.dts
@@ -15,7 +15,7 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-/include/ "socfpga_arria5.dtsi"
+#include "socfpga_arria5.dtsi"
 
 / {
 	model = "Altera SOCFPGA Arria V SoC Development Kit";
@@ -59,3 +59,22 @@
 	rxdv-skew-ps = <0>;
 	rxc-skew-ps = <2000>;
 };
+
+&i2c0 {
+	status = "okay";
+
+	eeprom@51 {
+		compatible = "atmel,24c32";
+		reg = <0x51>;
+		pagesize = <32>;
+	};
+
+	rtc@68 {
+		compatible = "dallas,ds1339";
+		reg = <0x68>;
+	};
+};
+
+&usb1 {
+	status = "okay";
+};
diff --git a/arch/arm/boot/dts/socfpga_cyclone5.dtsi b/arch/arm/boot/dts/socfpga_cyclone5.dtsi
index ca41b0ebf46..bf511828729 100644
--- a/arch/arm/boot/dts/socfpga_cyclone5.dtsi
+++ b/arch/arm/boot/dts/socfpga_cyclone5.dtsi
@@ -16,7 +16,7 @@
  */
 
 /dts-v1/;
-/include/ "socfpga.dtsi"
+#include "socfpga.dtsi"
 
 / {
 	soc {
@@ -45,30 +45,6 @@
 			status = "okay";
 		};
 
-		timer0@ffc08000 {
-			clock-frequency = <100000000>;
-		};
-
-		timer1@ffc09000 {
-			clock-frequency = <100000000>;
-		};
-
-		timer2@ffd00000 {
-			clock-frequency = <25000000>;
-		};
-
-		timer3@ffd01000 {
-			clock-frequency = <25000000>;
-		};
-
-		serial0@ffc02000 {
-			clock-frequency = <100000000>;
-		};
-
-		serial1@ffc03000 {
-			clock-frequency = <100000000>;
-		};
-
 		sysmgr@ffd08000 {
 			cpu1-start-addr = <0xffd080c4>;
 		};
diff --git a/arch/arm/boot/dts/socfpga_cyclone5_socdk.dts b/arch/arm/boot/dts/socfpga_cyclone5_socdk.dts
index ae16d975196..45de1514af0 100644
--- a/arch/arm/boot/dts/socfpga_cyclone5_socdk.dts
+++ b/arch/arm/boot/dts/socfpga_cyclone5_socdk.dts
@@ -15,7 +15,7 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-/include/ "socfpga_cyclone5.dtsi"
+#include "socfpga_cyclone5.dtsi"
 
 / {
 	model = "Altera SOCFPGA Cyclone V SoC Development Kit";
@@ -52,3 +52,22 @@
 	rxdv-skew-ps = <0>;
 	rxc-skew-ps = <2000>;
 };
+
+&i2c0 {
+	status = "okay";
+
+	eeprom@51 {
+		compatible = "atmel,24c32";
+		reg = <0x51>;
+		pagesize = <32>;
+	};
+
+	rtc@68 {
+		compatible = "dallas,ds1339";
+		reg = <0x68>;
+	};
+};
+
+&usb1 {
+	status = "okay";
+};
diff --git a/arch/arm/boot/dts/socfpga_cyclone5_sockit.dts b/arch/arm/boot/dts/socfpga_cyclone5_sockit.dts
index b79e2a2bf17..d26f155f5fd 100644
--- a/arch/arm/boot/dts/socfpga_cyclone5_sockit.dts
+++ b/arch/arm/boot/dts/socfpga_cyclone5_sockit.dts
@@ -15,7 +15,7 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-/include/ "socfpga_cyclone5.dtsi"
+#include "socfpga_cyclone5.dtsi"
 
 / {
 	model = "Terasic SoCkit";
@@ -52,3 +52,7 @@
 	rxdv-skew-ps = <0>;
 	rxc-skew-ps = <2000>;
 };
+
+&usb1 {
+	status = "okay";
+};
diff --git a/arch/arm/boot/dts/socfpga_cyclone5_socrates.dts b/arch/arm/boot/dts/socfpga_cyclone5_socrates.dts
new file mode 100644
index 00000000000..a1814b45745
--- /dev/null
+++ b/arch/arm/boot/dts/socfpga_cyclone5_socrates.dts
@@ -0,0 +1,50 @@
+/*
+ *  Copyright (C) 2014 Steffen Trumtrar <s.trumtrar@pengutronix.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, 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 "socfpga_cyclone5.dtsi"
+
+/ {
+	model = "EBV SOCrates";
+	compatible = "ebv,socrates", "altr,socfpga-cyclone5", "altr,socfpga";
+
+	chosen {
+		bootargs = "console=ttyS0,115200";
+	};
+
+	memory {
+		name = "memory";
+		device_type = "memory";
+		reg = <0x0 0x40000000>; /* 1GB */
+	};
+};
+
+&gmac1 {
+	status = "okay";
+};
+
+&i2c0 {
+	status = "okay";
+
+	rtc: rtc@68 {
+		compatible = "stm,m41t82";
+		reg = <0x68>;
+	};
+};
+
+&mmc {
+	status = "okay";
+};
diff --git a/arch/arm/boot/dts/socfpga_vt.dts b/arch/arm/boot/dts/socfpga_vt.dts
index 87d6f759a9c..09792b41111 100644
--- a/arch/arm/boot/dts/socfpga_vt.dts
+++ b/arch/arm/boot/dts/socfpga_vt.dts
@@ -16,7 +16,7 @@
  */
 
 /dts-v1/;
-/include/ "socfpga.dtsi"
+#include "socfpga.dtsi"
 
 / {
 	model = "Altera SOCFPGA VT";
diff --git a/arch/arm/boot/dts/spear320-hmi.dts b/arch/arm/boot/dts/spear320-hmi.dts
index 3075d2d3a8b..0aa6fef5ce2 100644
--- a/arch/arm/boot/dts/spear320-hmi.dts
+++ b/arch/arm/boot/dts/spear320-hmi.dts
@@ -1,7 +1,7 @@
 /*
  * DTS file for SPEAr320 Evaluation Baord
  *
- * Copyright 2012 Shiraz Hashim <shiraz.hashim@st.com>
+ * Copyright 2012 Shiraz Hashim <shiraz.linux.kernel@gmail.com>
  *
  * The code contained herein is licensed under the GNU General Public
  * License. You may obtain a copy of the GNU General Public License
diff --git a/arch/arm/boot/dts/ste-ccu8540.dts b/arch/arm/boot/dts/ste-ccu8540.dts
index 7f3baf51a3a..32dd55e5f4e 100644
--- a/arch/arm/boot/dts/ste-ccu8540.dts
+++ b/arch/arm/boot/dts/ste-ccu8540.dts
@@ -18,6 +18,7 @@
 	compatible = "st-ericsson,ccu8540", "st-ericsson,u8540";
 
 	memory@0 {
+		device_type = "memory";
 		reg = <0x20000000 0x1f000000>, <0xc0000000 0x3f000000>;
 	};
 
diff --git a/arch/arm/boot/dts/ste-ccu9540.dts b/arch/arm/boot/dts/ste-ccu9540.dts
index 22950875089..651c56d400a 100644
--- a/arch/arm/boot/dts/ste-ccu9540.dts
+++ b/arch/arm/boot/dts/ste-ccu9540.dts
@@ -38,8 +38,8 @@
 			arm,primecell-periphid = <0x10480180>;
 			max-frequency = <100000000>;
 			bus-width = <4>;
-			mmc-cap-sd-highspeed;
-			mmc-cap-mmc-highspeed;
+			cap-sd-highspeed;
+			cap-mmc-highspeed;
 			vmmc-supply = <&ab8500_ldo_aux3_reg>;
 
 			cd-gpios  = <&gpio7 6 0x4>; // 230
@@ -63,7 +63,7 @@
 			arm,primecell-periphid = <0x10480180>;
 		        max-frequency = <100000000>;
 			bus-width = <8>;
-			mmc-cap-mmc-highspeed;
+			cap-mmc-highspeed;
 			vmmc-supply = <&ab8500_ldo_aux2_reg>;
 
 			status = "okay";
diff --git a/arch/arm/boot/dts/ste-href.dtsi b/arch/arm/boot/dts/ste-href.dtsi
index 6cb9b68e218..bf8f0eddc2c 100644
--- a/arch/arm/boot/dts/ste-href.dtsi
+++ b/arch/arm/boot/dts/ste-href.dtsi
@@ -116,8 +116,15 @@
 			arm,primecell-periphid = <0x10480180>;
 			max-frequency = <100000000>;
 			bus-width = <4>;
-			mmc-cap-sd-highspeed;
-			mmc-cap-mmc-highspeed;
+			cap-sd-highspeed;
+			cap-mmc-highspeed;
+			sd-uhs-sdr12;
+			sd-uhs-sdr25;
+			full-pwr-cycle;
+			st,sig-dir-dat0;
+			st,sig-dir-dat2;
+			st,sig-dir-cmd;
+			st,sig-pin-fbclk;
 			vmmc-supply = <&ab8500_ldo_aux3_reg>;
 			vqmmc-supply = <&vmmci>;
 			pinctrl-names = "default", "sleep";
@@ -132,6 +139,7 @@
 			arm,primecell-periphid = <0x10480180>;
 			max-frequency = <100000000>;
 			bus-width = <4>;
+			non-removable;
 			pinctrl-names = "default", "sleep";
 			pinctrl-0 = <&sdi1_default_mode>;
 			pinctrl-1 = <&sdi1_sleep_mode>;
@@ -144,7 +152,9 @@
 			arm,primecell-periphid = <0x10480180>;
 			max-frequency = <100000000>;
 			bus-width = <8>;
-			mmc-cap-mmc-highspeed;
+			cap-mmc-highspeed;
+			non-removable;
+			vmmc-supply = <&db8500_vsmps2_reg>;
 			pinctrl-names = "default", "sleep";
 			pinctrl-0 = <&sdi2_default_mode>;
 			pinctrl-1 = <&sdi2_sleep_mode>;
@@ -157,7 +167,8 @@
 			arm,primecell-periphid = <0x10480180>;
 		        max-frequency = <100000000>;
 			bus-width = <8>;
-			mmc-cap-mmc-highspeed;
+			cap-mmc-highspeed;
+			non-removable;
 			vmmc-supply = <&ab8500_ldo_aux2_reg>;
 			pinctrl-names = "default", "sleep";
 			pinctrl-0 = <&sdi4_default_mode>;
diff --git a/arch/arm/boot/dts/ste-nomadik-stn8815.dtsi b/arch/arm/boot/dts/ste-nomadik-stn8815.dtsi
index 5acc0449676..d316c955bd5 100644
--- a/arch/arm/boot/dts/ste-nomadik-stn8815.dtsi
+++ b/arch/arm/boot/dts/ste-nomadik-stn8815.dtsi
@@ -840,8 +840,8 @@
 			interrupts = <22>;
 			max-frequency = <48000000>;
 			bus-width = <4>;
-			mmc-cap-mmc-highspeed;
-			mmc-cap-sd-highspeed;
+			cap-mmc-highspeed;
+			cap-sd-highspeed;
 			cd-gpios = <&gpio3 15 0x1>;
 			cd-inverted;
 			pinctrl-names = "default";
diff --git a/arch/arm/boot/dts/ste-snowball.dts b/arch/arm/boot/dts/ste-snowball.dts
index a2f632d0be2..474ef83229c 100644
--- a/arch/arm/boot/dts/ste-snowball.dts
+++ b/arch/arm/boot/dts/ste-snowball.dts
@@ -156,7 +156,7 @@
 			arm,primecell-periphid = <0x10480180>;
 			max-frequency = <100000000>;
 			bus-width = <4>;
-			mmc-cap-mmc-highspeed;
+			cap-mmc-highspeed;
 			vmmc-supply = <&ab8500_ldo_aux3_reg>;
 			vqmmc-supply = <&vmmci>;
 			pinctrl-names = "default", "sleep";
@@ -195,7 +195,7 @@
 			arm,primecell-periphid = <0x10480180>;
 		        max-frequency = <100000000>;
 			bus-width = <8>;
-			mmc-cap-mmc-highspeed;
+			cap-mmc-highspeed;
 			vmmc-supply = <&ab8500_ldo_aux2_reg>;
 			pinctrl-names = "default", "sleep";
 			pinctrl-0 = <&sdi4_default_mode>;
diff --git a/arch/arm/boot/dts/ste-u300.dts b/arch/arm/boot/dts/ste-u300.dts
index 6fe688e9e4d..82a661677e9 100644
--- a/arch/arm/boot/dts/ste-u300.dts
+++ b/arch/arm/boot/dts/ste-u300.dts
@@ -442,8 +442,8 @@
 			clock-names = "apb_pclk", "mclk";
 			max-frequency = <24000000>;
 			bus-width = <4>; // SD-card slot
-			mmc-cap-mmc-highspeed;
-			mmc-cap-sd-highspeed;
+			cap-mmc-highspeed;
+			cap-sd-highspeed;
 			cd-gpios = <&gpio 12 0x4>;
 			cd-inverted;
 			vmmc-supply = <&ab3100_ldo_g_reg>;
diff --git a/arch/arm/boot/dts/stih407-b2120.dts b/arch/arm/boot/dts/stih407-b2120.dts
new file mode 100644
index 00000000000..fe69f92e5f8
--- /dev/null
+++ b/arch/arm/boot/dts/stih407-b2120.dts
@@ -0,0 +1,78 @@
+/*
+ * Copyright (C) 2014 STMicroelectronics (R&D) Limited.
+ * Author: Giuseppe Cavallaro <peppe.cavallaro@st.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.
+ */
+/dts-v1/;
+#include "stih407.dtsi"
+/ {
+	model = "STiH407 B2120";
+	compatible = "st,stih407-b2120", "st,stih407";
+
+	chosen {
+		bootargs = "console=ttyAS0,115200";
+		linux,stdout-path = &sbc_serial0;
+	};
+
+	memory {
+		device_type = "memory";
+		reg = <0x40000000 0x80000000>;
+	};
+
+	aliases {
+		ttyAS0 = &sbc_serial0;
+	};
+
+	soc {
+		sbc_serial0: serial@9530000 {
+			status = "okay";
+		};
+
+		leds {
+			compatible = "gpio-leds";
+			red {
+				#gpio-cells = <2>;
+				label = "Front Panel LED";
+				gpios = <&pio4 1 0>;
+				linux,default-trigger = "heartbeat";
+			};
+			green {
+				#gpio-cells = <2>;
+				gpios = <&pio1 3 0>;
+				default-state = "off";
+			};
+		};
+
+		i2c@9842000 {
+			status = "okay";
+		};
+
+		i2c@9843000 {
+			status = "okay";
+		};
+
+		i2c@9844000 {
+			status = "okay";
+		};
+
+		i2c@9845000 {
+			status = "okay";
+		};
+
+		i2c@9540000 {
+			status = "okay";
+		};
+
+		/* SSC11 to HDMI */
+		i2c@9541000 {
+			status = "okay";
+			/* HDMI V1.3a supports Standard mode only */
+			clock-frequency = <100000>;
+			st,i2c-min-scl-pulse-width-us = <0>;
+			st,i2c-min-sda-pulse-width-us = <5>;
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/stih407-clock.dtsi b/arch/arm/boot/dts/stih407-clock.dtsi
new file mode 100644
index 00000000000..800f46f009f
--- /dev/null
+++ b/arch/arm/boot/dts/stih407-clock.dtsi
@@ -0,0 +1,39 @@
+/*
+ * Copyright (C) 2014 STMicroelectronics R&D Limited
+ *
+ * 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.
+ */
+/ {
+	clocks {
+		/*
+		 * Fixed 30MHz oscillator inputs to SoC
+		 */
+		clk_sysin: clk-sysin {
+			#clock-cells = <0>;
+			compatible = "fixed-clock";
+			clock-frequency = <30000000>;
+		};
+
+		/*
+		 * ARM Peripheral clock for timers
+		 */
+		arm_periph_clk: arm-periph-clk {
+			#clock-cells = <0>;
+			compatible = "fixed-clock";
+			clock-frequency = <600000000>;
+		};
+
+		/*
+		 * Bootloader initialized system infrastructure clock for
+		 * serial devices.
+		 */
+		clk_ext2f_a9: clockgen-c0@13 {
+			#clock-cells = <0>;
+			compatible = "fixed-clock";
+			clock-frequency = <200000000>;
+			clock-output-names = "clk-s-icn-reg-0";
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/stih407-pinctrl.dtsi b/arch/arm/boot/dts/stih407-pinctrl.dtsi
new file mode 100644
index 00000000000..402844cb315
--- /dev/null
+++ b/arch/arm/boot/dts/stih407-pinctrl.dtsi
@@ -0,0 +1,615 @@
+/*
+ * Copyright (C) 2014 STMicroelectronics Limited.
+ * Author: Giuseppe Cavallaro <peppe.cavallaro@st.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
+ * publishhed by the Free Software Foundation.
+ */
+#include "st-pincfg.h"
+#include <dt-bindings/interrupt-controller/arm-gic.h>
+/ {
+
+	aliases {
+		/* 0-5: PIO_SBC */
+		gpio0 = &pio0;
+		gpio1 = &pio1;
+		gpio2 = &pio2;
+		gpio3 = &pio3;
+		gpio4 = &pio4;
+		gpio5 = &pio5;
+		/* 10-19: PIO_FRONT0 */
+		gpio6 = &pio10;
+		gpio7 = &pio11;
+		gpio8 = &pio12;
+		gpio9 = &pio13;
+		gpio10 = &pio14;
+		gpio11 = &pio15;
+		gpio12 = &pio16;
+		gpio13 = &pio17;
+		gpio14 = &pio18;
+		gpio15 = &pio19;
+		/* 20: PIO_FRONT1 */
+		gpio16 = &pio20;
+		/* 30-35: PIO_REAR */
+		gpio17 = &pio30;
+		gpio18 = &pio31;
+		gpio19 = &pio32;
+		gpio20 = &pio33;
+		gpio21 = &pio34;
+		gpio22 = &pio35;
+		/* 40-42: PIO_FLASH */
+		gpio23 = &pio40;
+		gpio24 = &pio41;
+		gpio25 = &pio42;
+	};
+
+	soc {
+		pin-controller-sbc {
+			#address-cells = <1>;
+			#size-cells = <1>;
+			compatible = "st,stih407-sbc-pinctrl";
+			st,syscfg = <&syscfg_sbc>;
+			reg = <0x0961f080 0x4>;
+			reg-names = "irqmux";
+			interrupts = <GIC_SPI 188 IRQ_TYPE_NONE>;
+			interrupts-names = "irqmux";
+			ranges = <0 0x09610000 0x6000>;
+
+			pio0: gpio@09610000 {
+				gpio-controller;
+				#gpio-cells = <1>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x0 0x100>;
+				st,bank-name = "PIO0";
+			};
+			pio1: gpio@09611000 {
+				gpio-controller;
+				#gpio-cells = <1>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x1000 0x100>;
+				st,bank-name = "PIO1";
+			};
+			pio2: gpio@09612000 {
+				gpio-controller;
+				#gpio-cells = <1>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x2000 0x100>;
+				st,bank-name = "PIO2";
+			};
+			pio3: gpio@09613000 {
+				gpio-controller;
+				#gpio-cells = <1>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x3000 0x100>;
+				st,bank-name = "PIO3";
+			};
+			pio4: gpio@09614000 {
+				gpio-controller;
+				#gpio-cells = <1>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x4000 0x100>;
+				st,bank-name = "PIO4";
+			};
+
+			pio5: gpio@09615000 {
+				gpio-controller;
+				#gpio-cells = <1>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x5000 0x100>;
+				st,bank-name = "PIO5";
+			};
+
+			rc {
+				pinctrl_ir: ir0 {
+					st,pins {
+						ir = <&pio4 0 ALT2 IN>;
+					};
+				};
+			};
+
+			/* SBC_ASC0 - UART10 */
+			sbc_serial0 {
+				pinctrl_sbc_serial0: sbc_serial0-0 {
+					st,pins {
+						tx = <&pio3 4 ALT1 OUT>;
+						rx = <&pio3 5 ALT1 IN>;
+					};
+				};
+			};
+			/* SBC_ASC1 - UART11 */
+			sbc_serial1 {
+				pinctrl_sbc_serial1: sbc_serial1-0 {
+					st,pins {
+						tx = <&pio2 6 ALT3 OUT>;
+						rx = <&pio2 7 ALT3 IN>;
+					};
+				};
+			};
+
+			i2c10 {
+				pinctrl_i2c10_default: i2c10-default {
+					st,pins {
+						sda = <&pio4 6 ALT1 BIDIR>;
+						scl = <&pio4 5 ALT1 BIDIR>;
+					};
+				};
+			};
+
+			i2c11 {
+				pinctrl_i2c11_default: i2c11-default {
+					st,pins {
+						sda = <&pio5 1 ALT1 BIDIR>;
+						scl = <&pio5 0 ALT1 BIDIR>;
+					};
+				};
+			};
+
+			keyscan {
+				pinctrl_keyscan: keyscan {
+					st,pins {
+						keyin0 = <&pio4 0 ALT6 IN>;
+						keyin1 = <&pio4 5 ALT4 IN>;
+						keyin2 = <&pio0 4 ALT2 IN>;
+						keyin3 = <&pio2 6 ALT2 IN>;
+
+						keyout0 = <&pio4 6 ALT4 OUT>;
+						keyout1 = <&pio1 7 ALT2 OUT>;
+						keyout2 = <&pio0 6 ALT2 OUT>;
+						keyout3 = <&pio2 7 ALT2 OUT>;
+					};
+				};
+			};
+
+			gmac1 {
+				/*
+				 * Almost all the boards based on STiH407 SoC have an embedded
+				 * switch where the mdio/mdc have been used for managing the SMI
+				 * iface via I2C. For this reason these lines can be allocated
+				 * by using dedicated configuration (in case of there will be a
+				 * standard PHY transceiver on-board).
+				 */
+				pinctrl_rgmii1: rgmii1-0 {
+					st,pins {
+
+						txd0 = <&pio0 0 ALT1 OUT DE_IO 0 CLK_A>;
+						txd1 = <&pio0 1 ALT1 OUT DE_IO 0 CLK_A>;
+						txd2 = <&pio0 2 ALT1 OUT DE_IO 0 CLK_A>;
+						txd3 = <&pio0 3 ALT1 OUT DE_IO 0 CLK_A>;
+						txen = <&pio0 5 ALT1 OUT DE_IO 0 CLK_A>;
+						txclk = <&pio0 6 ALT1 IN NICLK 0 CLK_A>;
+						rxd0 = <&pio1 4 ALT1 IN DE_IO 0 CLK_A>;
+						rxd1 = <&pio1 5 ALT1 IN DE_IO 0 CLK_A>;
+						rxd2 = <&pio1 6 ALT1 IN DE_IO 0 CLK_A>;
+						rxd3 = <&pio1 7 ALT1 IN DE_IO 0 CLK_A>;
+						rxdv = <&pio2 0 ALT1 IN DE_IO 0 CLK_A>;
+						rxclk = <&pio2 2 ALT1 IN NICLK 500 CLK_A>;
+						clk125 = <&pio3 7 ALT4 IN NICLK 0 CLK_A>;
+						phyclk = <&pio2 3 ALT4 OUT NICLK 1750 CLK_B>;
+					};
+				};
+
+				pinctrl_rgmii1_mdio: rgmii1-mdio {
+					st,pins {
+						mdio = <&pio1 0 ALT1 OUT BYPASS 0>;
+						mdc = <&pio1 1 ALT1 OUT NICLK 0 CLK_A>;
+						mdint = <&pio1 3 ALT1 IN BYPASS 0>;
+					};
+				};
+
+				pinctrl_mii1: mii1 {
+					st,pins {
+						txd0 = <&pio0 0 ALT1 OUT SE_NICLK_IO 0 CLK_A>;
+						txd1 = <&pio0 1 ALT1 OUT SE_NICLK_IO 0 CLK_A>;
+						txd2 = <&pio0 2 ALT1 OUT SE_NICLK_IO 0 CLK_A>;
+						txd3 = <&pio0 3 ALT1 OUT SE_NICLK_IO 0 CLK_A>;
+						txer = <&pio0 4 ALT1 OUT SE_NICLK_IO 0 CLK_A>;
+						txen = <&pio0 5 ALT1 OUT SE_NICLK_IO 0 CLK_A>;
+						txclk = <&pio0 6 ALT1 IN NICLK 0 CLK_A>;
+						col = <&pio0 7 ALT1 IN BYPASS 1000>;
+
+						mdio = <&pio1 0 ALT1 OUT BYPASS 1500>;
+						mdc = <&pio1 1 ALT1 OUT NICLK 0 CLK_A>;
+						crs = <&pio1 2 ALT1 IN BYPASS 1000>;
+						mdint = <&pio1 3 ALT1 IN BYPASS 0>;
+						rxd0 = <&pio1 4 ALT1 IN SE_NICLK_IO 0 CLK_A>;
+						rxd1 = <&pio1 5 ALT1 IN SE_NICLK_IO 0 CLK_A>;
+						rxd2 = <&pio1 6 ALT1 IN SE_NICLK_IO 0 CLK_A>;
+						rxd3 = <&pio1 7 ALT1 IN SE_NICLK_IO 0 CLK_A>;
+
+						rxdv = <&pio2 0 ALT1 IN SE_NICLK_IO 0 CLK_A>;
+						rx_er = <&pio2 1 ALT1 IN SE_NICLK_IO 0 CLK_A>;
+						rxclk = <&pio2 2 ALT1 IN NICLK 0 CLK_A>;
+						phyclk = <&pio2 3 ALT1 OUT NICLK 0 CLK_A>;
+					};
+				};
+			};
+
+			pwm1 {
+				pinctrl_pwm1_chan0_default: pwm1-0-default {
+					st,pins {
+						pwm-out = <&pio3 0 ALT1 OUT>;
+					};
+				};
+				pinctrl_pwm1_chan1_default: pwm1-1-default {
+					st,pins {
+						pwm-out = <&pio4 4 ALT1 OUT>;
+					};
+				};
+				pinctrl_pwm1_chan2_default: pwm1-2-default {
+					st,pins {
+						pwm-out = <&pio4 6 ALT3 OUT>;
+					};
+				};
+				pinctrl_pwm1_chan3_default: pwm1-3-default {
+					st,pins {
+						pwm-out = <&pio4 7 ALT3 OUT>;
+					};
+				};
+			};
+		};
+
+		pin-controller-front0 {
+			#address-cells = <1>;
+			#size-cells = <1>;
+			compatible = "st,stih407-front-pinctrl";
+			st,syscfg = <&syscfg_front>;
+			reg = <0x0920f080 0x4>;
+			reg-names = "irqmux";
+			interrupts = <GIC_SPI 189 IRQ_TYPE_NONE>;
+			interrupts-names = "irqmux";
+			ranges = <0 0x09200000 0x10000>;
+
+			pio10: pio@09200000 {
+				gpio-controller;
+				#gpio-cells = <1>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x0 0x100>;
+				st,bank-name = "PIO10";
+			};
+			pio11: pio@09201000 {
+				gpio-controller;
+				#gpio-cells = <1>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x1000 0x100>;
+				st,bank-name = "PIO11";
+			};
+			pio12: pio@09202000 {
+				gpio-controller;
+				#gpio-cells = <1>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x2000 0x100>;
+				st,bank-name = "PIO12";
+			};
+			pio13: pio@09203000 {
+				gpio-controller;
+				#gpio-cells = <1>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x3000 0x100>;
+				st,bank-name = "PIO13";
+			};
+			pio14: pio@09204000 {
+				gpio-controller;
+				#gpio-cells = <1>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x4000 0x100>;
+				st,bank-name = "PIO14";
+			};
+			pio15: pio@09205000 {
+				gpio-controller;
+				#gpio-cells = <1>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x5000 0x100>;
+				st,bank-name = "PIO15";
+			};
+			pio16: pio@09206000 {
+				gpio-controller;
+				#gpio-cells = <1>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x6000 0x100>;
+				st,bank-name = "PIO16";
+			};
+			pio17: pio@09207000 {
+				gpio-controller;
+				#gpio-cells = <1>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x7000 0x100>;
+				st,bank-name = "PIO17";
+			};
+			pio18: pio@09208000 {
+				gpio-controller;
+				#gpio-cells = <1>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x8000 0x100>;
+				st,bank-name = "PIO18";
+			};
+			pio19: pio@09209000 {
+				gpio-controller;
+				#gpio-cells = <1>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x9000 0x100>;
+				st,bank-name = "PIO19";
+			};
+
+			/* Comms */
+			serial0 {
+				pinctrl_serial0: serial0-0 {
+					st,pins {
+						tx = <&pio17 0 ALT1 OUT>;
+						rx = <&pio17 1 ALT1 IN>;
+					};
+				};
+			};
+
+			serial1 {
+				pinctrl_serial1: serial1-0 {
+					st,pins {
+						tx = <&pio16 0 ALT1 OUT>;
+						rx = <&pio16 1 ALT1 IN>;
+					};
+				};
+			};
+
+			serial2 {
+				pinctrl_serial2: serial2-0 {
+					st,pins {
+						tx = <&pio15 0 ALT1 OUT>;
+						rx = <&pio15 1 ALT1 IN>;
+					};
+				};
+			};
+
+			mmc1 {
+				pinctrl_sd1: sd1-0 {
+					st,pins {
+						sd_clk = <&pio19 3 ALT5 BIDIR NICLK 0 CLK_B>;
+						sd_cmd = <&pio19 2 ALT5 BIDIR_PU BYPASS 0>;
+						sd_dat0 = <&pio19 4 ALT5 BIDIR_PU BYPASS 0>;
+						sd_dat1 = <&pio19 5 ALT5 BIDIR_PU BYPASS 0>;
+						sd_dat2 = <&pio19 6 ALT5 BIDIR_PU BYPASS 0>;
+						sd_dat3 = <&pio19 7 ALT5 BIDIR_PU BYPASS 0>;
+						sd_led = <&pio16 6 ALT6 OUT>;
+						sd_pwren = <&pio16 7 ALT6 OUT>;
+						sd_cd = <&pio19 0 ALT6 IN>;
+						sd_wp = <&pio19 1 ALT6 IN>;
+					};
+				};
+			};
+
+
+			i2c0 {
+				pinctrl_i2c0_default: i2c0-default {
+					st,pins {
+						sda = <&pio10 6 ALT2 BIDIR>;
+						scl = <&pio10 5 ALT2 BIDIR>;
+					};
+				};
+			};
+
+			i2c1 {
+				pinctrl_i2c1_default: i2c1-default {
+					st,pins {
+						sda = <&pio11 1 ALT2 BIDIR>;
+						scl = <&pio11 0 ALT2 BIDIR>;
+					};
+				};
+			};
+
+			i2c2 {
+				pinctrl_i2c2_default: i2c2-default {
+					st,pins {
+						sda = <&pio15 6 ALT2 BIDIR>;
+						scl = <&pio15 5 ALT2 BIDIR>;
+					};
+				};
+			};
+
+			i2c3 {
+				pinctrl_i2c3_default: i2c3-default {
+					st,pins {
+						sda = <&pio18 6 ALT1 BIDIR>;
+						scl = <&pio18 5 ALT1 BIDIR>;
+					};
+				};
+			};
+
+			spi0 {
+				pinctrl_spi0_default: spi0-default {
+					st,pins {
+						mtsr = <&pio12 6 ALT2 BIDIR>;
+						mrst = <&pio12 7 ALT2 BIDIR>;
+						scl = <&pio12 5 ALT2 BIDIR>;
+					};
+				};
+			};
+		};
+
+		pin-controller-front1 {
+			#address-cells = <1>;
+			#size-cells = <1>;
+			compatible = "st,stih407-front-pinctrl";
+			st,syscfg = <&syscfg_front>;
+			reg = <0x0921f080 0x4>;
+			reg-names = "irqmux";
+			interrupts = <GIC_SPI 190 IRQ_TYPE_NONE>;
+			interrupts-names = "irqmux";
+			ranges = <0 0x09210000 0x10000>;
+
+			pio20: pio@09210000 {
+				gpio-controller;
+				#gpio-cells = <1>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x0 0x100>;
+				st,bank-name = "PIO20";
+			};
+		};
+
+		pin-controller-rear {
+			#address-cells = <1>;
+			#size-cells = <1>;
+			compatible = "st,stih407-rear-pinctrl";
+			st,syscfg = <&syscfg_rear>;
+			reg = <0x0922f080 0x4>;
+			reg-names = "irqmux";
+			interrupts = <GIC_SPI 191 IRQ_TYPE_NONE>;
+			interrupts-names = "irqmux";
+			ranges = <0 0x09220000 0x6000>;
+
+			pio30: gpio@09220000 {
+				gpio-controller;
+				#gpio-cells = <1>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x0 0x100>;
+				st,bank-name = "PIO30";
+			};
+			pio31: gpio@09221000 {
+				gpio-controller;
+				#gpio-cells = <1>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x1000 0x100>;
+				st,bank-name = "PIO31";
+			};
+			pio32: gpio@09222000 {
+				gpio-controller;
+				#gpio-cells = <1>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x2000 0x100>;
+				st,bank-name = "PIO32";
+			};
+			pio33: gpio@09223000 {
+				gpio-controller;
+				#gpio-cells = <1>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x3000 0x100>;
+				st,bank-name = "PIO33";
+			};
+			pio34: gpio@09224000 {
+				gpio-controller;
+				#gpio-cells = <1>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x4000 0x100>;
+				st,bank-name = "PIO34";
+			};
+			pio35: gpio@09225000 {
+				gpio-controller;
+				#gpio-cells = <1>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x5000 0x100>;
+				st,bank-name = "PIO35";
+			};
+
+			i2c4 {
+				pinctrl_i2c4_default: i2c4-default {
+					st,pins {
+						sda = <&pio30 1 ALT1 BIDIR>;
+						scl = <&pio30 0 ALT1 BIDIR>;
+					};
+				};
+			};
+
+			i2c5 {
+				pinctrl_i2c5_default: i2c5-default {
+					st,pins {
+						sda = <&pio34 4 ALT1 BIDIR>;
+						scl = <&pio34 3 ALT1 BIDIR>;
+					};
+				};
+			};
+
+			usb3 {
+				pinctrl_usb3: usb3-2 {
+					st,pins {
+						usb-oc-detect = <&pio35 4 ALT1 IN>;
+						usb-pwr-enable = <&pio35 5 ALT1 OUT>;
+						usb-vbus-valid = <&pio35 6 ALT1 IN>;
+					};
+				};
+			};
+
+			pwm0 {
+				pinctrl_pwm0_chan0_default: pwm0-0-default {
+					st,pins {
+						pwm-out = <&pio31 1 ALT1 OUT>;
+					};
+				};
+			};
+		};
+
+		pin-controller-flash {
+			#address-cells = <1>;
+			#size-cells = <1>;
+			compatible = "st,stih407-flash-pinctrl";
+			st,syscfg = <&syscfg_flash>;
+			reg = <0x0923f080 0x4>;
+			reg-names = "irqmux";
+			interrupts = <GIC_SPI 192 IRQ_TYPE_NONE>;
+			interrupts-names = "irqmux";
+			ranges = <0 0x09230000 0x3000>;
+
+			pio40: gpio@09230000 {
+				gpio-controller;
+				#gpio-cells = <1>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0 0x100>;
+				st,bank-name = "PIO40";
+			};
+			pio41: gpio@09231000 {
+				gpio-controller;
+				#gpio-cells = <1>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x1000 0x100>;
+				st,bank-name = "PIO41";
+			};
+			pio42: gpio@09232000 {
+				gpio-controller;
+				#gpio-cells = <1>;
+				interrupt-controller;
+				#interrupt-cells = <2>;
+				reg = <0x2000 0x100>;
+				st,bank-name = "PIO42";
+			};
+
+			mmc0 {
+				pinctrl_mmc0: mmc0-0 {
+					st,pins {
+						emmc_clk = <&pio40 6 ALT1 BIDIR>;
+						emmc_cmd = <&pio40 7 ALT1 BIDIR_PU>;
+						emmc_d0 = <&pio41 0 ALT1 BIDIR_PU>;
+						emmc_d1 = <&pio41 1 ALT1 BIDIR_PU>;
+						emmc_d2 = <&pio41 2 ALT1 BIDIR_PU>;
+						emmc_d3 = <&pio41 3 ALT1 BIDIR_PU>;
+						emmc_d4 = <&pio41 4 ALT1 BIDIR_PU>;
+						emmc_d5 = <&pio41 5 ALT1 BIDIR_PU>;
+						emmc_d6 = <&pio41 6 ALT1 BIDIR_PU>;
+						emmc_d7 = <&pio41 7 ALT1 BIDIR_PU>;
+					};
+				};
+			};
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/stih407.dtsi b/arch/arm/boot/dts/stih407.dtsi
new file mode 100644
index 00000000000..4f9024f1986
--- /dev/null
+++ b/arch/arm/boot/dts/stih407.dtsi
@@ -0,0 +1,263 @@
+/*
+ * Copyright (C) 2014 STMicroelectronics Limited.
+ * Author: Giuseppe Cavallaro <peppe.cavallaro@st.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
+ * publishhed by the Free Software Foundation.
+ */
+#include "stih407-clock.dtsi"
+#include "stih407-pinctrl.dtsi"
+/ {
+	#address-cells = <1>;
+	#size-cells = <1>;
+
+	cpus {
+		#address-cells = <1>;
+		#size-cells = <0>;
+		cpu@0 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a9";
+			reg = <0>;
+		};
+		cpu@1 {
+			device_type = "cpu";
+			compatible = "arm,cortex-a9";
+			reg = <1>;
+		};
+	};
+
+	intc: interrupt-controller@08761000 {
+		compatible = "arm,cortex-a9-gic";
+		#interrupt-cells = <3>;
+		interrupt-controller;
+		reg = <0x08761000 0x1000>, <0x08760100 0x100>;
+	};
+
+	scu@08760000 {
+		compatible = "arm,cortex-a9-scu";
+		reg = <0x08760000 0x1000>;
+	};
+
+	timer@08760200 {
+		interrupt-parent = <&intc>;
+		compatible = "arm,cortex-a9-global-timer";
+		reg = <0x08760200 0x100>;
+		interrupts = <GIC_PPI 11 IRQ_TYPE_LEVEL_HIGH>;
+		clocks = <&arm_periph_clk>;
+	};
+
+	l2: cache-controller {
+		compatible = "arm,pl310-cache";
+		reg = <0x08762000 0x1000>;
+		arm,data-latency = <3 3 3>;
+		arm,tag-latency = <2 2 2>;
+		cache-unified;
+		cache-level = <2>;
+	};
+
+	soc {
+		#address-cells = <1>;
+		#size-cells = <1>;
+		interrupt-parent = <&intc>;
+		ranges;
+		compatible = "simple-bus";
+
+		syscfg_sbc: sbc-syscfg@9620000 {
+			compatible = "st,stih407-sbc-syscfg", "syscon";
+			reg = <0x9620000 0x1000>;
+		};
+
+		syscfg_front: front-syscfg@9280000 {
+			compatible = "st,stih407-front-syscfg", "syscon";
+			reg = <0x9280000 0x1000>;
+		};
+
+		syscfg_rear: rear-syscfg@9290000 {
+			compatible = "st,stih407-rear-syscfg", "syscon";
+			reg = <0x9290000 0x1000>;
+		};
+
+		syscfg_flash: flash-syscfg@92a0000 {
+			compatible = "st,stih407-flash-syscfg", "syscon";
+			reg = <0x92a0000 0x1000>;
+		};
+
+		syscfg_sbc_reg: fvdp-lite-syscfg@9600000 {
+			compatible = "st,stih407-sbc-reg-syscfg", "syscon";
+			reg = <0x9600000 0x1000>;
+		};
+
+		syscfg_core: core-syscfg@92b0000 {
+			compatible = "st,stih407-core-syscfg", "syscon";
+			reg = <0x92b0000 0x1000>;
+		};
+
+		syscfg_lpm: lpm-syscfg@94b5100 {
+			compatible = "st,stih407-lpm-syscfg", "syscon";
+			reg = <0x94b5100 0x1000>;
+		};
+
+		serial@9830000 {
+			compatible = "st,asc";
+			reg = <0x9830000 0x2c>;
+			interrupts = <GIC_SPI 122 IRQ_TYPE_NONE>;
+			pinctrl-names = "default";
+			pinctrl-0 = <&pinctrl_serial0>;
+			clocks = <&clk_ext2f_a9>;
+
+			status = "disabled";
+		};
+
+		serial@9831000 {
+			compatible = "st,asc";
+			reg = <0x9831000 0x2c>;
+			interrupts = <GIC_SPI 123 IRQ_TYPE_NONE>;
+			pinctrl-names = "default";
+			pinctrl-0 = <&pinctrl_serial1>;
+			clocks = <&clk_ext2f_a9>;
+
+			status = "disabled";
+		};
+
+		serial@9832000 {
+			compatible = "st,asc";
+			reg = <0x9832000 0x2c>;
+			interrupts = <GIC_SPI 124 IRQ_TYPE_NONE>;
+			pinctrl-names = "default";
+			pinctrl-0 = <&pinctrl_serial2>;
+			clocks = <&clk_ext2f_a9>;
+
+			status = "disabled";
+		};
+
+		/* SBC_ASC0 - UART10 */
+		sbc_serial0: serial@9530000 {
+			compatible = "st,asc";
+			reg = <0x9530000 0x2c>;
+			interrupts = <GIC_SPI 138 IRQ_TYPE_NONE>;
+			pinctrl-names = "default";
+			pinctrl-0 = <&pinctrl_sbc_serial0>;
+			clocks = <&clk_sysin>;
+
+			status = "disabled";
+		};
+
+		serial@9531000 {
+			compatible = "st,asc";
+			reg = <0x9531000 0x2c>;
+			interrupts = <GIC_SPI 139 IRQ_TYPE_NONE>;
+			pinctrl-names = "default";
+			pinctrl-0 = <&pinctrl_sbc_serial1>;
+			clocks = <&clk_sysin>;
+
+			status = "disabled";
+		};
+
+		i2c@9840000 {
+			compatible = "st,comms-ssc4-i2c";
+			interrupts = <GIC_SPI 112 IRQ_TYPE_LEVEL_HIGH>;
+			reg = <0x9840000 0x110>;
+			clocks = <&clk_ext2f_a9>;
+			clock-names = "ssc";
+			clock-frequency = <400000>;
+			pinctrl-names = "default";
+			pinctrl-0 = <&pinctrl_i2c0_default>;
+
+			status = "disabled";
+		};
+
+		i2c@9841000 {
+			compatible = "st,comms-ssc4-i2c";
+			reg = <0x9841000 0x110>;
+			interrupts = <GIC_SPI 113 IRQ_TYPE_LEVEL_HIGH>;
+			clocks = <&clk_ext2f_a9>;
+			clock-names = "ssc";
+			clock-frequency = <400000>;
+			pinctrl-names = "default";
+			pinctrl-0 = <&pinctrl_i2c1_default>;
+
+			status = "disabled";
+		};
+
+		i2c@9842000 {
+			compatible = "st,comms-ssc4-i2c";
+			reg = <0x9842000 0x110>;
+			interrupts = <GIC_SPI 114 IRQ_TYPE_LEVEL_HIGH>;
+			clocks = <&clk_ext2f_a9>;
+			clock-names = "ssc";
+			clock-frequency = <400000>;
+			pinctrl-names = "default";
+			pinctrl-0 = <&pinctrl_i2c2_default>;
+
+			status = "disabled";
+		};
+
+		i2c@9843000 {
+			compatible = "st,comms-ssc4-i2c";
+			reg = <0x9843000 0x110>;
+			interrupts = <GIC_SPI 115 IRQ_TYPE_LEVEL_HIGH>;
+			clocks = <&clk_ext2f_a9>;
+			clock-names = "ssc";
+			clock-frequency = <400000>;
+			pinctrl-names = "default";
+			pinctrl-0 = <&pinctrl_i2c3_default>;
+
+			status = "disabled";
+		};
+
+		i2c@9844000 {
+			compatible = "st,comms-ssc4-i2c";
+			reg = <0x9844000 0x110>;
+			interrupts = <GIC_SPI 116 IRQ_TYPE_LEVEL_HIGH>;
+			clocks = <&clk_ext2f_a9>;
+			clock-names = "ssc";
+			clock-frequency = <400000>;
+			pinctrl-names = "default";
+			pinctrl-0 = <&pinctrl_i2c4_default>;
+
+			status = "disabled";
+		};
+
+		i2c@9845000 {
+			compatible = "st,comms-ssc4-i2c";
+			reg = <0x9845000 0x110>;
+			interrupts = <GIC_SPI 117 IRQ_TYPE_LEVEL_HIGH>;
+			clocks = <&clk_ext2f_a9>;
+			clock-names = "ssc";
+			clock-frequency = <400000>;
+			pinctrl-names = "default";
+			pinctrl-0 = <&pinctrl_i2c5_default>;
+
+			status = "disabled";
+		};
+
+
+		/* SSCs on SBC */
+		i2c@9540000 {
+			compatible = "st,comms-ssc4-i2c";
+			reg = <0x9540000 0x110>;
+			interrupts = <GIC_SPI 135 IRQ_TYPE_LEVEL_HIGH>;
+			clocks = <&clk_sysin>;
+			clock-names = "ssc";
+			clock-frequency = <400000>;
+			pinctrl-names = "default";
+			pinctrl-0 = <&pinctrl_i2c10_default>;
+
+			status = "disabled";
+		};
+
+		i2c@9541000 {
+			compatible = "st,comms-ssc4-i2c";
+			reg = <0x9541000 0x110>;
+			interrupts = <GIC_SPI 136 IRQ_TYPE_LEVEL_HIGH>;
+			clocks = <&clk_sysin>;
+			clock-names = "ssc";
+			clock-frequency = <400000>;
+			pinctrl-names = "default";
+			pinctrl-0 = <&pinctrl_i2c11_default>;
+
+			status = "disabled";
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/stih415-b2000.dts b/arch/arm/boot/dts/stih415-b2000.dts
index d4af5316043..bdfbd3765db 100644
--- a/arch/arm/boot/dts/stih415-b2000.dts
+++ b/arch/arm/boot/dts/stih415-b2000.dts
@@ -11,5 +11,5 @@
 #include "stih41x-b2000.dtsi"
 / {
 	model = "STiH415 B2000 Board";
-	compatible = "st,stih415", "st,stih415-b2000";
+	compatible = "st,stih415-b2000", "st,stih415";
 };
diff --git a/arch/arm/boot/dts/stih415-b2020.dts b/arch/arm/boot/dts/stih415-b2020.dts
index 442b019e9a3..71903a87bd3 100644
--- a/arch/arm/boot/dts/stih415-b2020.dts
+++ b/arch/arm/boot/dts/stih415-b2020.dts
@@ -11,5 +11,5 @@
 #include "stih41x-b2020.dtsi"
 / {
 	model = "STiH415 B2020 Board";
-	compatible = "st,stih415", "st,stih415-b2020";
+	compatible = "st,stih415-b2020", "st,stih415";
 };
diff --git a/arch/arm/boot/dts/stih415-clock.dtsi b/arch/arm/boot/dts/stih415-clock.dtsi
index d047dbc28d6..3ee34514bc4 100644
--- a/arch/arm/boot/dts/stih415-clock.dtsi
+++ b/arch/arm/boot/dts/stih415-clock.dtsi
@@ -5,48 +5,529 @@
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
  */
+
+#include <dt-bindings/clock/stih415-clks.h>
+
 / {
 	clocks {
+		#address-cells = <1>;
+		#size-cells = <1>;
+		ranges;
+
 		/*
 		 * Fixed 30MHz oscillator input to SoC
 		 */
-		CLK_SYSIN: CLK_SYSIN {
+		clk_sysin: clk-sysin {
 			#clock-cells = <0>;
 			compatible = "fixed-clock";
 			clock-frequency = <30000000>;
 		};
 
 		/*
-		 * ARM Peripheral clock for timers
+		 * ClockGenAs on SASG1
 		 */
-		arm_periph_clk: arm_periph_clk {
-			#clock-cells = <0>;
-			compatible = "fixed-clock";
-			clock-frequency = <500000000>;
+		clockgen-a@fee62000 {
+			reg = <0xfee62000 0xb48>;
+
+			clk_s_a0_pll: clk-s-a0-pll {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-plls-c65";
+
+				clocks = <&clk_sysin>;
+
+				clock-output-names = "clk-s-a0-pll0-hs",
+						     "clk-s-a0-pll0-ls",
+						     "clk-s-a0-pll1";
+			};
+
+			clk_s_a0_osc_prediv: clk-s-a0-osc-prediv {
+				#clock-cells = <0>;
+				compatible = "st,clkgena-prediv-c65",
+					     "st,clkgena-prediv";
+
+				clocks = <&clk_sysin>;
+
+				clock-output-names = "clk-s-a0-osc-prediv";
+			};
+
+			clk_s_a0_hs: clk-s-a0-hs {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c65-hs",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_s_a0_osc_prediv>,
+					 <&clk_s_a0_pll 0>, /* PLL0 HS */
+					 <&clk_s_a0_pll 2>; /* PLL1 */
+
+				clock-output-names = "clk-s-fdma-0",
+						     "clk-s-fdma-1",
+						     ""; /* clk-s-jit-sense */
+						     /* Fourth output unused */
+			};
+
+			clk_s_a0_ls: clk-s-a0-ls {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c65-ls",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_s_a0_osc_prediv>,
+					 <&clk_s_a0_pll 1>, /* PLL0 LS */
+					 <&clk_s_a0_pll 2>; /* PLL1 */
+
+				clock-output-names = "clk-s-icn-reg-0",
+						     "clk-s-icn-if-0",
+						     "clk-s-icn-reg-lp-0",
+						     "clk-s-emiss",
+						     "clk-s-eth1-phy",
+						     "clk-s-mii-ref-out";
+						 /* Remaining outputs unused */
+			};
+		};
+
+		clockgen-a@fee81000 {
+			reg = <0xfee81000 0xb48>;
+
+			clk_s_a1_pll: clk-s-a1-pll {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-plls-c65";
+
+				clocks = <&clk_sysin>;
+
+				clock-output-names = "clk-s-a1-pll0-hs",
+						     "clk-s-a1-pll0-ls",
+						     "clk-s-a1-pll1";
+			};
+
+			clk_s_a1_osc_prediv: clk-s-a1-osc-prediv {
+				#clock-cells = <0>;
+				compatible = "st,clkgena-prediv-c65",
+					     "st,clkgena-prediv";
+
+				clocks = <&clk_sysin>;
+
+				clock-output-names = "clk-s-a1-osc-prediv";
+			};
+
+			clk_s_a1_hs: clk-s-a1-hs {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c65-hs",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_s_a1_osc_prediv>,
+					 <&clk_s_a1_pll 0>, /* PLL0 HS */
+					 <&clk_s_a1_pll 2>; /* PLL1 */
+
+				clock-output-names = "", /* Reserved */
+						     "", /* Reserved */
+						     "clk-s-stac-phy",
+						     "clk-s-vtac-tx-phy";
+			};
+
+			clk_s_a1_ls: clk-s-a1-ls {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c65-ls",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_s_a1_osc_prediv>,
+					 <&clk_s_a1_pll 1>, /* PLL0 LS */
+					 <&clk_s_a1_pll 2>; /* PLL1 */
+
+				clock-output-names = "clk-s-icn-if-2",
+						     "clk-s-card-mmc",
+						     "clk-s-icn-if-1",
+						     "clk-s-gmac0-phy",
+						     "clk-s-nand-ctrl",
+						     "", /* Reserved */
+						     "clk-s-mii0-ref-out",
+						     ""; /* clk-s-stac-sys */
+						 /* Remaining outputs unused */
+			};
 		};
 
 		/*
-		 * Bootloader initialized system infrastructure clock for
-		 * serial devices.
+		 * ClockGenAs on MPE41
 		 */
-		CLKS_ICN_REG_0: CLKS_ICN_REG_0 {
+		clockgen-a@fde12000 {
+			reg = <0xfde12000 0xb50>;
+
+			clk_m_a0_pll0: clk-m-a0-pll0 {
+				#clock-cells = <1>;
+				compatible = "st,plls-c32-a1x-0", "st,clkgen-plls-c32";
+
+				clocks = <&clk_sysin>;
+
+				clock-output-names = "clk-m-a0-pll0-phi0",
+						     "clk-m-a0-pll0-phi1",
+						     "clk-m-a0-pll0-phi2",
+						     "clk-m-a0-pll0-phi3";
+			};
+
+			clk_m_a0_pll1: clk-m-a0-pll1 {
+				#clock-cells = <1>;
+				compatible = "st,plls-c32-a1x-1", "st,clkgen-plls-c32";
+
+				clocks = <&clk_sysin>;
+
+				clock-output-names = "clk-m-a0-pll1-phi0",
+						     "clk-m-a0-pll1-phi1",
+						     "clk-m-a0-pll1-phi2",
+						     "clk-m-a0-pll1-phi3";
+			};
+
+			clk_m_a0_osc_prediv: clk-m-a0-osc-prediv {
+				#clock-cells = <0>;
+				compatible = "st,clkgena-prediv-c32",
+					     "st,clkgena-prediv";
+
+				clocks = <&clk_sysin>;
+
+				clock-output-names = "clk-m-a0-osc-prediv";
+			};
+
+			clk_m_a0_div0: clk-m-a0-div0 {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c32-odf0",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_m_a0_osc_prediv>,
+					 <&clk_m_a0_pll0 0>, /* PLL0 PHI0 */
+					 <&clk_m_a0_pll1 0>; /* PLL1 PHI0 */
+
+				clock-output-names = "clk-m-apb-pm", /* Unused */
+						     "", /* Unused */
+						     "", /* Unused */
+						     "", /* Unused */
+						     "clk-m-pp-dmu-0",
+						     "clk-m-pp-dmu-1",
+						     "clk-m-icm-disp",
+						     ""; /* Unused */
+			};
+
+			clk_m_a0_div1: clk-m-a0-div1 {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c32-odf1",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_m_a0_osc_prediv>,
+					 <&clk_m_a0_pll0 1>, /* PLL0 PHI1 */
+					 <&clk_m_a0_pll1 1>; /* PLL1 PHI1 */
+
+				clock-output-names = "", /* Unused */
+						     "", /* Unused */
+						     "clk-m-a9-ext2f",
+						     "clk-m-st40rt",
+						     "clk-m-st231-dmu-0",
+						     "clk-m-st231-dmu-1",
+						     "clk-m-st231-aud",
+						     "clk-m-st231-gp-0";
+			};
+
+			clk_m_a0_div2: clk-m-a0-div2 {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c32-odf2",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_m_a0_osc_prediv>,
+					 <&clk_m_a0_pll0 2>, /* PLL0 PHI2 */
+					 <&clk_m_a0_pll1 2>; /* PLL1 PHI2 */
+
+				clock-output-names = "clk-m-st231-gp-1",
+						     "clk-m-icn-cpu",
+						     "clk-m-icn-stac",
+						     "clk-m-icn-dmu-0",
+						     "clk-m-icn-dmu-1",
+						     "", /* Unused */
+						     "", /* Unused */
+						     ""; /* Unused */
+			};
+
+			clk_m_a0_div3: clk-m-a0-div3 {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c32-odf3",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_m_a0_osc_prediv>,
+					 <&clk_m_a0_pll0 3>, /* PLL0 PHI3 */
+					 <&clk_m_a0_pll1 3>; /* PLL1 PHI3 */
+
+				clock-output-names = "", /* Unused */
+						     "", /* Unused */
+						     "", /* Unused */
+						     "", /* Unused */
+						     "", /* Unused */
+						     "", /* Unused */
+						     "clk-m-icn-eram",
+						     "clk-m-a9-trace";
+			};
+		};
+
+		clockgen-a@fd6db000 {
+			reg = <0xfd6db000 0xb50>;
+
+			clk_m_a1_pll0: clk-m-a1-pll0 {
+				#clock-cells = <1>;
+				compatible = "st,plls-c32-a1x-0", "st,clkgen-plls-c32";
+
+				clocks = <&clk_sysin>;
+
+				clock-output-names = "clk-m-a1-pll0-phi0",
+						     "clk-m-a1-pll0-phi1",
+						     "clk-m-a1-pll0-phi2",
+						     "clk-m-a1-pll0-phi3";
+			};
+
+			clk_m_a1_pll1: clk-m-a1-pll1 {
+				#clock-cells = <1>;
+				compatible = "st,plls-c32-a1x-1", "st,clkgen-plls-c32";
+
+				clocks = <&clk_sysin>;
+
+				clock-output-names = "clk-m-a1-pll1-phi0",
+						     "clk-m-a1-pll1-phi1",
+						     "clk-m-a1-pll1-phi2",
+						     "clk-m-a1-pll1-phi3";
+			};
+
+			clk_m_a1_osc_prediv: clk-m-a1-osc-prediv {
+				#clock-cells = <0>;
+				compatible = "st,clkgena-prediv-c32",
+					     "st,clkgena-prediv";
+
+				clocks = <&clk_sysin>;
+
+				clock-output-names = "clk-m-a1-osc-prediv";
+			};
+
+			clk_m_a1_div0: clk-m-a1-div0 {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c32-odf0",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_m_a1_osc_prediv>,
+					 <&clk_m_a1_pll0 0>, /* PLL0 PHI0 */
+					 <&clk_m_a1_pll1 0>; /* PLL1 PHI0 */
+
+				clock-output-names = "clk-m-fdma-12",
+						     "clk-m-fdma-10",
+						     "clk-m-fdma-11",
+						     "clk-m-hva-lmi",
+						     "clk-m-proc-sc",
+						     "clk-m-tp",
+						     "clk-m-icn-gpu",
+						     "clk-m-icn-vdp-0";
+			};
+
+			clk_m_a1_div1: clk-m-a1-div1 {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c32-odf1",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_m_a1_osc_prediv>,
+					 <&clk_m_a1_pll0 1>, /* PLL0 PHI1 */
+					 <&clk_m_a1_pll1 1>; /* PLL1 PHI1 */
+
+				clock-output-names = "clk-m-icn-vdp-1",
+						     "clk-m-icn-vdp-2",
+						     "clk-m-icn-vdp-3",
+						     "clk-m-prv-t1-bus",
+						     "clk-m-icn-vdp-4",
+						     "clk-m-icn-reg-10",
+						     "", /* Unused */
+						     ""; /* clk-m-icn-st231 */
+			};
+
+			clk_m_a1_div2: clk-m-a1-div2 {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c32-odf2",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_m_a1_osc_prediv>,
+					 <&clk_m_a1_pll0 2>, /* PLL0 PHI2 */
+					 <&clk_m_a1_pll1 2>; /* PLL1 PHI2 */
+
+				clock-output-names = "clk-m-fvdp-proc-alt",
+						     "", /* Unused */
+						     "", /* Unused */
+						     "", /* Unused */
+						     "", /* Unused */
+						     "", /* Unused */
+						     "", /* Unused */
+						     ""; /* Unused */
+			};
+
+			clk_m_a1_div3: clk-m-a1-div3 {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c32-odf3",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_m_a1_osc_prediv>,
+					 <&clk_m_a1_pll0 3>, /* PLL0 PHI3 */
+					 <&clk_m_a1_pll1 3>; /* PLL1 PHI3 */
+
+				clock-output-names = "", /* Unused */
+						     "", /* Unused */
+						     "", /* Unused */
+						     "", /* Unused */
+						     "", /* Unused */
+						     "", /* Unused */
+						     "", /* Unused */
+						     ""; /* Unused */
+			};
+		};
+
+		clk_m_a9_ext2f_div2: clk-m-a9-ext2f-div2 {
 			#clock-cells = <0>;
-			compatible = "fixed-clock";
-			clock-frequency = <100000000>;
+			compatible = "fixed-factor-clock";
+			clocks = <&clk_m_a0_div1 2>;
+			clock-div = <2>;
+			clock-mult = <1>;
 		};
 
-		CLKS_GMAC0_PHY: clockgenA1@7 {
+		clockgen-a@fd345000 {
+			reg = <0xfd345000 0xb50>;
+
+			clk_m_a2_pll0: clk-m-a2-pll0 {
+				#clock-cells = <1>;
+				compatible = "st,plls-c32-a1x-0", "st,clkgen-plls-c32";
+
+				clocks = <&clk_sysin>;
+
+				clock-output-names = "clk-m-a2-pll0-phi0",
+						     "clk-m-a2-pll0-phi1",
+						     "clk-m-a2-pll0-phi2",
+						     "clk-m-a2-pll0-phi3";
+			};
+
+			clk_m_a2_pll1: clk-m-a2-pll1 {
+				#clock-cells = <1>;
+				compatible = "st,plls-c32-a1x-1", "st,clkgen-plls-c32";
+
+				clocks = <&clk_sysin>;
+
+				clock-output-names = "clk-m-a2-pll1-phi0",
+						     "clk-m-a2-pll1-phi1",
+						     "clk-m-a2-pll1-phi2",
+						     "clk-m-a2-pll1-phi3";
+			};
+
+			clk_m_a2_osc_prediv: clk-m-a2-osc-prediv {
+				#clock-cells = <0>;
+				compatible = "st,clkgena-prediv-c32",
+					     "st,clkgena-prediv";
+
+				clocks = <&clk_sysin>;
+
+				clock-output-names = "clk-m-a2-osc-prediv";
+			};
+
+			clk_m_a2_div0: clk-m-a2-div0 {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c32-odf0",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_m_a2_osc_prediv>,
+					 <&clk_m_a2_pll0 0>, /* PLL0 PHI0 */
+					 <&clk_m_a2_pll1 0>; /* PLL1 PHI0 */
+
+				clock-output-names = "clk-m-vtac-main-phy",
+						     "clk-m-vtac-aux-phy",
+						     "clk-m-stac-phy",
+						     "clk-m-stac-sys",
+						     "", /* clk-m-mpestac-pg */
+						     "", /* clk-m-mpestac-wc */
+						     "", /* clk-m-mpevtacaux-pg*/
+						     ""; /* clk-m-mpevtacmain-pg*/
+			};
+
+			clk_m_a2_div1: clk-m-a2-div1 {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c32-odf1",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_m_a2_osc_prediv>,
+					 <&clk_m_a2_pll0 1>, /* PLL0 PHI1 */
+					 <&clk_m_a2_pll1 1>; /* PLL1 PHI1 */
+
+				clock-output-names = "", /* clk-m-mpevtacrx0-wc */
+						     "", /* clk-m-mpevtacrx1-wc */
+						     "clk-m-compo-main",
+						     "clk-m-compo-aux",
+						     "clk-m-bdisp-0",
+						     "clk-m-bdisp-1",
+						     "clk-m-icn-bdisp-0",
+						     "clk-m-icn-bdisp-1";
+			};
+
+			clk_m_a2_div2: clk-m-a2-div2 {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c32-odf2",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_m_a2_osc_prediv>,
+					 <&clk_m_a2_pll0 2>, /* PLL0 PHI2 */
+					 <&clk_m_a2_pll1 2>; /* PLL1 PHI2 */
+
+				clock-output-names = "", /* clk-m-icn-hqvdp0 */
+						     "", /* clk-m-icn-hqvdp1 */
+						     "clk-m-icn-compo",
+						     "", /* clk-m-icn-vdpaux */
+						     "clk-m-icn-ts",
+						     "clk-m-icn-reg-lp-10",
+						     "clk-m-dcephy-impctrl",
+						     ""; /* Unused */
+			};
+
+			clk_m_a2_div3: clk-m-a2-div3 {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c32-odf3",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_m_a2_osc_prediv>,
+					 <&clk_m_a2_pll0 3>, /* PLL0 PHI3 */
+					 <&clk_m_a2_pll1 3>; /* PLL1 PHI3 */
+
+				clock-output-names = ""; /* Unused */
+						/* Remaining outputs unused */
+			};
+		};
+
+		/*
+		 * A9 PLL
+		 */
+		clockgen-a9@fdde00d8 {
+			reg = <0xfdde00d8 0x70>;
+
+			clockgen_a9_pll: clockgen-a9-pll {
+				#clock-cells = <1>;
+				compatible = "st,stih415-plls-c32-a9", "st,clkgen-plls-c32";
+
+				clocks = <&clk_sysin>;
+				clock-output-names = "clockgen-a9-pll-odf";
+			};
+		};
+
+		/*
+		 * ARM CPU related clocks
+		 */
+		clk_m_a9: clk-m-a9@fdde00d8 {
 			#clock-cells = <0>;
-			compatible = "fixed-clock";
-			clock-frequency = <25000000>;
-			clock-output-names = "CLKS_GMAC0_PHY";
+			compatible = "st,stih415-clkgen-a9-mux", "st,clkgen-mux";
+			reg = <0xfdde00d8 0x4>;
+			clocks = <&clockgen_a9_pll 0>,
+				 <&clockgen_a9_pll 0>,
+				 <&clk_m_a0_div1 2>,
+				 <&clk_m_a9_ext2f_div2>;
 		};
 
-		CLKS_ETH1_PHY: clockgenA0@7 {
+		/*
+		 * ARM Peripheral clock for timers
+		 */
+		arm_periph_clk: clk-m-a9-periphs {
 			#clock-cells = <0>;
-			compatible = "fixed-clock";
-			clock-frequency = <25000000>;
-			clock-output-names = "CLKS_ETH1_PHY";
+			compatible = "fixed-factor-clock";
+			clocks = <&clk_m_a9>;
+			clock-div = <2>;
+			clock-mult = <1>;
 		};
 	};
 };
diff --git a/arch/arm/boot/dts/stih415-pinctrl.dtsi b/arch/arm/boot/dts/stih415-pinctrl.dtsi
index f09fb10a379..8509a037ae2 100644
--- a/arch/arm/boot/dts/stih415-pinctrl.dtsi
+++ b/arch/arm/boot/dts/stih415-pinctrl.dtsi
@@ -49,7 +49,7 @@
 			reg 		= <0xfe61f080 0x4>;
 			reg-names	= "irqmux";
 			interrupts 	= <GIC_SPI 180 IRQ_TYPE_LEVEL_HIGH>;
-			interrupts-names = "irqmux";
+			interrupt-names	= "irqmux";
 			ranges 		= <0 0xfe610000 0x5000>;
 
 			PIO0: gpio@fe610000 {
@@ -102,6 +102,22 @@
 				};
 			};
 
+			keyscan {
+				pinctrl_keyscan: keyscan {
+					st,pins {
+						keyin0 = <&PIO0 2 ALT2 IN>;
+						keyin1 = <&PIO0 3 ALT2 IN>;
+						keyin2 = <&PIO0 4 ALT2 IN>;
+						keyin3 = <&PIO2 6 ALT2 IN>;
+
+						keyout0 = <&PIO1 6 ALT2 OUT>;
+						keyout1 = <&PIO1 7 ALT2 OUT>;
+						keyout2 = <&PIO0 6 ALT2 OUT>;
+						keyout3 = <&PIO2 7 ALT2 OUT>;
+					};
+				};
+			};
+
 			sbc_i2c0 {
 				pinctrl_sbc_i2c0_default: sbc_i2c0-default {
 					st,pins {
@@ -187,7 +203,7 @@
 			reg 		= <0xfee0f080 0x4>;
 			reg-names	= "irqmux";
 			interrupts 	= <GIC_SPI 181 IRQ_TYPE_LEVEL_HIGH>;
-			interrupts-names = "irqmux";
+			interrupt-names	= "irqmux";
 			ranges		= <0 0xfee00000 0x8000>;
 
 			PIO5: gpio@fee00000 {
@@ -282,7 +298,7 @@
 			reg 		= <0xfe82f080 0x4>;
 			reg-names	= "irqmux";
 			interrupts 	= <GIC_SPI 182 IRQ_TYPE_LEVEL_HIGH>;
-			interrupts-names = "irqmux";
+			interrupt-names	= "irqmux";
 			ranges		= <0 0xfe820000 0x8000>;
 
 			PIO13: gpio@fe820000 {
@@ -423,7 +439,7 @@
 			reg 		= <0xfd6bf080 0x4>;
 			reg-names	= "irqmux";
 			interrupts 	= <GIC_SPI 113 IRQ_TYPE_LEVEL_HIGH>;
-			interrupts-names = "irqmux";
+			interrupt-names	= "irqmux";
 			ranges		= <0 0xfd6b0000 0x3000>;
 
 			PIO100: gpio@fd6b0000 {
@@ -460,7 +476,7 @@
 			reg 		= <0xfd33f080 0x4>;
 			reg-names	= "irqmux";
 			interrupts 	= <GIC_SPI 114 IRQ_TYPE_LEVEL_HIGH>;
-			interrupts-names = "irqmux";
+			interrupt-names	= "irqmux";
 			ranges		= <0 0xfd330000 0x5000>;
 
 			PIO103: gpio@fd330000 {
diff --git a/arch/arm/boot/dts/stih415.dtsi b/arch/arm/boot/dts/stih415.dtsi
index d89064c20c8..d6f254f302f 100644
--- a/arch/arm/boot/dts/stih415.dtsi
+++ b/arch/arm/boot/dts/stih415.dtsi
@@ -82,7 +82,7 @@
 			interrupts	= <0 197 0>;
 			pinctrl-names 	= "default";
 			pinctrl-0 	= <&pinctrl_serial2>;
-			clocks		= <&CLKS_ICN_REG_0>;
+			clocks		= <&clk_s_a0_ls CLK_ICN_REG>;
 		};
 
 		/* SBC comms block ASCs in SASG1 */
@@ -91,7 +91,7 @@
 			status 		= "disabled";
 			reg		= <0xfe531000 0x2c>;
 			interrupts	= <0 210 0>;
-			clocks		= <&CLK_SYSIN>;
+			clocks		= <&clk_sysin>;
 			pinctrl-names 	= "default";
 			pinctrl-0	= <&pinctrl_sbc_serial1>;
 		};
@@ -100,7 +100,7 @@
 			compatible	= "st,comms-ssc4-i2c";
 			reg		= <0xfed40000 0x110>;
 			interrupts	= <GIC_SPI 187 IRQ_TYPE_LEVEL_HIGH>;
-			clocks		= <&CLKS_ICN_REG_0>;
+			clocks		= <&clk_s_a0_ls CLK_ICN_REG>;
 			clock-names	= "ssc";
 			clock-frequency = <400000>;
 			pinctrl-names	= "default";
@@ -113,7 +113,7 @@
 			compatible	= "st,comms-ssc4-i2c";
 			reg		= <0xfed41000 0x110>;
 			interrupts	= <GIC_SPI 188 IRQ_TYPE_LEVEL_HIGH>;
-			clocks		= <&CLKS_ICN_REG_0>;
+			clocks		= <&clk_s_a0_ls CLK_ICN_REG>;
 			clock-names	= "ssc";
 			clock-frequency = <400000>;
 			pinctrl-names	= "default";
@@ -126,7 +126,7 @@
 			compatible	= "st,comms-ssc4-i2c";
 			reg		= <0xfe540000 0x110>;
 			interrupts	= <GIC_SPI 206 IRQ_TYPE_LEVEL_HIGH>;
-			clocks		= <&CLK_SYSIN>;
+			clocks		= <&clk_sysin>;
 			clock-names	= "ssc";
 			clock-frequency = <400000>;
 			pinctrl-names	= "default";
@@ -139,7 +139,7 @@
 			compatible	= "st,comms-ssc4-i2c";
 			reg		= <0xfe541000 0x110>;
 			interrupts	= <GIC_SPI 207 IRQ_TYPE_LEVEL_HIGH>;
-			clocks		= <&CLK_SYSIN>;
+			clocks		= <&clk_sysin>;
 			clock-names	= "ssc";
 			clock-frequency = <400000>;
 			pinctrl-names	= "default";
@@ -170,7 +170,7 @@
 			pinctrl-names 	= "default";
 			pinctrl-0	= <&pinctrl_mii0>;
 			clock-names	= "stmmaceth";
-			clocks		= <&CLKS_GMAC0_PHY>;
+			clocks		= <&clk_s_a1_ls CLK_GMAC0_PHY>;
 		};
 
 		ethernet1: dwmac@fef08000 {
@@ -193,18 +193,30 @@
 			pinctrl-names 	= "default";
 			pinctrl-0	= <&pinctrl_mii1>;
 			clock-names	= "stmmaceth";
-			clocks		= <&CLKS_ETH1_PHY>;
+			clocks		= <&clk_s_a0_ls CLK_ETH1_PHY>;
 		};
 
 		rc: rc@fe518000 {
 			compatible	= "st,comms-irb";
 			reg		= <0xfe518000 0x234>;
 			interrupts	=  <0 203 0>;
-			clocks		= <&CLK_SYSIN>;
+			clocks		= <&clk_sysin>;
 			rx-mode		= "infrared";
 			pinctrl-names 	= "default";
 			pinctrl-0	= <&pinctrl_ir>;
 			resets		= <&softreset STIH415_IRB_SOFTRESET>;
 		};
+
+		keyscan: keyscan@fe4b0000 {
+			compatible = "st,sti-keyscan";
+			status = "disabled";
+			reg = <0xfe4b0000 0x2000>;
+			interrupts = <GIC_SPI 212 IRQ_TYPE_NONE>;
+			clocks = <&clk_sysin>;
+			pinctrl-names = "default";
+			pinctrl-0 = <&pinctrl_keyscan>;
+			resets	= <&powerdown STIH415_KEYSCAN_POWERDOWN>,
+				  <&softreset STIH415_KEYSCAN_SOFTRESET>;
+		};
 	};
 };
diff --git a/arch/arm/boot/dts/stih416-b2000.dts b/arch/arm/boot/dts/stih416-b2000.dts
index a5eb6eee10b..488e80a5d69 100644
--- a/arch/arm/boot/dts/stih416-b2000.dts
+++ b/arch/arm/boot/dts/stih416-b2000.dts
@@ -9,8 +9,7 @@
 /dts-v1/;
 #include "stih416.dtsi"
 #include "stih41x-b2000.dtsi"
-
 / {
-	compatible = "st,stih416", "st,stih416-b2000";
 	model = "STiH416 B2000";
+	compatible = "st,stih416-b2000", "st,stih416";
 };
diff --git a/arch/arm/boot/dts/stih416-b2020-revE.dts b/arch/arm/boot/dts/stih416-b2020-revE.dts
new file mode 100644
index 00000000000..ba0fa2caaf1
--- /dev/null
+++ b/arch/arm/boot/dts/stih416-b2020-revE.dts
@@ -0,0 +1,35 @@
+/*
+ * Copyright (C) 2014 STMicroelectronics (R&D) Limited.
+ * Author: Lee Jones <lee.jones@linaro.org>
+ *
+ * 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.
+ */
+/dts-v1/;
+#include "stih416.dtsi"
+#include "stih41x-b2020.dtsi"
+/ {
+	model = "STiH416 B2020 REV-E";
+	compatible = "st,stih416-b2020", "st,stih416";
+
+	soc {
+		leds {
+			compatible = "gpio-leds";
+			red {
+				#gpio-cells		= <1>;
+				label			= "Front Panel LED";
+				gpios			= <&PIO4 1>;
+				linux,default-trigger	= "heartbeat";
+			};
+			green {
+				gpios			= <&PIO1 3>;
+				default-state 		= "off";
+			};
+		};
+
+		ethernet1: dwmac@fef08000 {
+			snps,reset-gpio = <&PIO0 7>;
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/stih416-b2020.dts b/arch/arm/boot/dts/stih416-b2020.dts
index 276f28da573..4e2df66b99e 100644
--- a/arch/arm/boot/dts/stih416-b2020.dts
+++ b/arch/arm/boot/dts/stih416-b2020.dts
@@ -11,6 +11,5 @@
 #include "stih41x-b2020.dtsi"
 / {
 	model = "STiH416 B2020";
-	compatible = "st,stih416", "st,stih416-b2020";
-
+	compatible = "st,stih416-b2020", "st,stih416";
 };
diff --git a/arch/arm/boot/dts/stih416-clock.dtsi b/arch/arm/boot/dts/stih416-clock.dtsi
index a6942c75cbb..5b4fb838cdd 100644
--- a/arch/arm/boot/dts/stih416-clock.dtsi
+++ b/arch/arm/boot/dts/stih416-clock.dtsi
@@ -6,50 +6,751 @@
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
  */
+
+#include <dt-bindings/clock/stih416-clks.h>
+
 / {
 	clocks {
+		#address-cells = <1>;
+		#size-cells = <1>;
+		ranges;
+
 		/*
 		 * Fixed 30MHz oscillator inputs to SoC
 		 */
-		CLK_SYSIN: CLK_SYSIN {
+		clk_sysin: clk-sysin {
 			#clock-cells = <0>;
 			compatible = "fixed-clock";
 			clock-frequency = <30000000>;
-			clock-output-names = "CLK_SYSIN";
+		};
+
+		/*
+		 * ClockGenAs on SASG2
+		 */
+		clockgen-a@fee62000 {
+			reg = <0xfee62000 0xb48>;
+
+			clk_s_a0_pll: clk-s-a0-pll {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-plls-c65";
+
+				clocks = <&clk_sysin>;
+
+				clock-output-names = "clk-s-a0-pll0-hs",
+						     "clk-s-a0-pll0-ls",
+						     "clk-s-a0-pll1";
+			};
+
+			clk_s_a0_osc_prediv: clk-s-a0-osc-prediv {
+				#clock-cells = <0>;
+				compatible = "st,clkgena-prediv-c65",
+					     "st,clkgena-prediv";
+
+				clocks = <&clk_sysin>;
+
+				clock-output-names = "clk-s-a0-osc-prediv";
+			};
+
+			clk_s_a0_hs: clk-s-a0-hs {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c65-hs",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_s_a0_osc_prediv>,
+					 <&clk_s_a0_pll 0>, /* PLL0 HS */
+					 <&clk_s_a0_pll 2>; /* PLL1 */
+
+				clock-output-names = "clk-s-fdma-0",
+						     "clk-s-fdma-1",
+						     ""; /* clk-s-jit-sense */
+						     /* Fourth output unused */
+			};
+
+			clk_s_a0_ls: clk-s-a0-ls {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c65-ls",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_s_a0_osc_prediv>,
+					 <&clk_s_a0_pll 1>, /* PLL0 LS */
+					 <&clk_s_a0_pll 2>; /* PLL1 */
+
+				clock-output-names = "clk-s-icn-reg-0",
+						     "clk-s-icn-if-0",
+						     "clk-s-icn-reg-lp-0",
+						     "clk-s-emiss",
+						     "clk-s-eth1-phy",
+						     "clk-s-mii-ref-out";
+						     /* Remaining outputs unused */
+			};
+		};
+
+		clockgen-a@fee81000 {
+			reg = <0xfee81000 0xb48>;
+
+			clk_s_a1_pll: clk-s-a1-pll {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-plls-c65";
+
+				clocks = <&clk_sysin>;
+
+				clock-output-names = "clk-s-a1-pll0-hs",
+						     "clk-s-a1-pll0-ls",
+						     "clk-s-a1-pll1";
+			};
+
+			clk_s_a1_osc_prediv: clk-s-a1-osc-prediv {
+				#clock-cells = <0>;
+				compatible = "st,clkgena-prediv-c65",
+					     "st,clkgena-prediv";
+
+				clocks = <&clk_sysin>;
+
+				clock-output-names = "clk-s-a1-osc-prediv";
+			};
+
+			clk_s_a1_hs: clk-s-a1-hs {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c65-hs",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_s_a1_osc_prediv>,
+					 <&clk_s_a1_pll 0>, /* PLL0 HS */
+					 <&clk_s_a1_pll 2>; /* PLL1 */
+
+				clock-output-names = "", /* Reserved */
+						     "", /* Reserved */
+						     "clk-s-stac-phy",
+						     "clk-s-vtac-tx-phy";
+			};
+
+			clk_s_a1_ls: clk-s-a1-ls {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c65-ls",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_s_a1_osc_prediv>,
+					 <&clk_s_a1_pll 1>, /* PLL0 LS */
+					 <&clk_s_a1_pll 2>; /* PLL1 */
+
+				clock-output-names = "clk-s-icn-if-2",
+						     "clk-s-card-mmc-0",
+						     "clk-s-icn-if-1",
+						     "clk-s-gmac0-phy",
+						     "clk-s-nand-ctrl",
+						     "", /* Reserved */
+						     "clk-s-mii0-ref-out",
+						     "clk-s-stac-sys",
+						     "clk-s-card-mmc-1";
+						     /* Remaining outputs unused */
+			};
+		};
+
+		/*
+		 * ClockGenAs on MPE42
+		 */
+		clockgen-a@fde12000 {
+			reg = <0xfde12000 0xb50>;
+
+			clk_m_a0_pll0: clk-m-a0-pll0 {
+				#clock-cells = <1>;
+				compatible = "st,plls-c32-a1x-0", "st,clkgen-plls-c32";
+
+				clocks = <&clk_sysin>;
+
+				clock-output-names = "clk-m-a0-pll0-phi0",
+						     "clk-m-a0-pll0-phi1",
+						     "clk-m-a0-pll0-phi2",
+						     "clk-m-a0-pll0-phi3";
+			};
+
+			clk_m_a0_pll1: clk-m-a0-pll1 {
+				#clock-cells = <1>;
+				compatible = "st,plls-c32-a1x-1", "st,clkgen-plls-c32";
+
+				clocks = <&clk_sysin>;
+
+				clock-output-names = "clk-m-a0-pll1-phi0",
+						     "clk-m-a0-pll1-phi1",
+						     "clk-m-a0-pll1-phi2",
+						     "clk-m-a0-pll1-phi3";
+			};
+
+			clk_m_a0_osc_prediv: clk-m-a0-osc-prediv {
+				#clock-cells = <0>;
+				compatible = "st,clkgena-prediv-c32",
+					     "st,clkgena-prediv";
+
+				clocks = <&clk_sysin>;
+
+				clock-output-names = "clk-m-a0-osc-prediv";
+			};
+
+			clk_m_a0_div0: clk-m-a0-div0 {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c32-odf0",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_m_a0_osc_prediv>,
+					 <&clk_m_a0_pll0 0>, /* PLL0 PHI0 */
+					 <&clk_m_a0_pll1 0>; /* PLL1 PHI0 */
+
+				clock-output-names = "", /* Unused */
+						     "", /* Unused */
+						     "clk-m-fdma-12",
+						     "", /* Unused */
+						     "clk-m-pp-dmu-0",
+						     "clk-m-pp-dmu-1",
+						     "clk-m-icm-lmi",
+						     "clk-m-vid-dmu-0";
+			};
+
+			clk_m_a0_div1: clk-m-a0-div1 {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c32-odf1",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_m_a0_osc_prediv>,
+					 <&clk_m_a0_pll0 1>, /* PLL0 PHI1 */
+					 <&clk_m_a0_pll1 1>; /* PLL1 PHI1 */
+
+				clock-output-names = "clk-m-vid-dmu-1",
+						     "", /* Unused */
+						     "clk-m-a9-ext2f",
+						     "clk-m-st40rt",
+						     "clk-m-st231-dmu-0",
+						     "clk-m-st231-dmu-1",
+						     "clk-m-st231-aud",
+						     "clk-m-st231-gp-0";
+			};
+
+			clk_m_a0_div2: clk-m-a0-div2 {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c32-odf2",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_m_a0_osc_prediv>,
+					 <&clk_m_a0_pll0 2>, /* PLL0 PHI2 */
+					 <&clk_m_a0_pll1 2>; /* PLL1 PHI2 */
+
+				clock-output-names = "clk-m-st231-gp-1",
+						     "clk-m-icn-cpu",
+						     "clk-m-icn-stac",
+						     "clk-m-tx-icn-dmu-0",
+						     "clk-m-tx-icn-dmu-1",
+						     "clk-m-tx-icn-ts",
+						     "clk-m-icn-vdp-0",
+						     "clk-m-icn-vdp-1";
+			};
+
+			clk_m_a0_div3: clk-m-a0-div3 {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c32-odf3",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_m_a0_osc_prediv>,
+					 <&clk_m_a0_pll0 3>, /* PLL0 PHI3 */
+					 <&clk_m_a0_pll1 3>; /* PLL1 PHI3 */
+
+				clock-output-names = "", /* Unused */
+						     "", /* Unused */
+						     "", /* Unused */
+						     "", /* Unused */
+						     "clk-m-icn-vp8",
+						     "", /* Unused */
+						     "clk-m-icn-reg-11",
+						     "clk-m-a9-trace";
+			};
+		};
+
+		clockgen-a@fd6db000 {
+			reg = <0xfd6db000 0xb50>;
+
+			clk_m_a1_pll0: clk-m-a1-pll0 {
+				#clock-cells = <1>;
+				compatible = "st,plls-c32-a1x-0", "st,clkgen-plls-c32";
+
+				clocks = <&clk_sysin>;
+
+				clock-output-names = "clk-m-a1-pll0-phi0",
+						     "clk-m-a1-pll0-phi1",
+						     "clk-m-a1-pll0-phi2",
+						     "clk-m-a1-pll0-phi3";
+			};
+
+			clk_m_a1_pll1: clk-m-a1-pll1 {
+				#clock-cells = <1>;
+				compatible = "st,plls-c32-a1x-1", "st,clkgen-plls-c32";
+
+				clocks = <&clk_sysin>;
+
+				clock-output-names = "clk-m-a1-pll1-phi0",
+						     "clk-m-a1-pll1-phi1",
+						     "clk-m-a1-pll1-phi2",
+						     "clk-m-a1-pll1-phi3";
+			};
+
+			clk_m_a1_osc_prediv: clk-m-a1-osc-prediv {
+				#clock-cells = <0>;
+				compatible = "st,clkgena-prediv-c32",
+					     "st,clkgena-prediv";
+
+				clocks = <&clk_sysin>;
+
+				clock-output-names = "clk-m-a1-osc-prediv";
+			};
+
+			clk_m_a1_div0: clk-m-a1-div0 {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c32-odf0",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_m_a1_osc_prediv>,
+					 <&clk_m_a1_pll0 0>, /* PLL0 PHI0 */
+					 <&clk_m_a1_pll1 0>; /* PLL1 PHI0 */
+
+				clock-output-names = "", /* Unused */
+						     "clk-m-fdma-10",
+						     "clk-m-fdma-11",
+						     "clk-m-hva-alt",
+						     "clk-m-proc-sc",
+						     "clk-m-tp",
+						     "clk-m-rx-icn-dmu-0",
+						     "clk-m-rx-icn-dmu-1";
+			};
+
+			clk_m_a1_div1: clk-m-a1-div1 {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c32-odf1",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_m_a1_osc_prediv>,
+					 <&clk_m_a1_pll0 1>, /* PLL0 PHI1 */
+					 <&clk_m_a1_pll1 1>; /* PLL1 PHI1 */
+
+				clock-output-names = "clk-m-rx-icn-ts",
+						     "clk-m-rx-icn-vdp-0",
+						     "", /* Unused */
+						     "clk-m-prv-t1-bus",
+						     "clk-m-icn-reg-12",
+						     "clk-m-icn-reg-10",
+						     "", /* Unused */
+						     "clk-m-icn-st231";
+			};
+
+			clk_m_a1_div2: clk-m-a1-div2 {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c32-odf2",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_m_a1_osc_prediv>,
+					 <&clk_m_a1_pll0 2>, /* PLL0 PHI2 */
+					 <&clk_m_a1_pll1 2>; /* PLL1 PHI2 */
+
+				clock-output-names = "clk-m-fvdp-proc-alt",
+						     "clk-m-icn-reg-13",
+						     "clk-m-tx-icn-gpu",
+						     "clk-m-rx-icn-gpu",
+						     "", /* Unused */
+						     "", /* Unused */
+						     "", /* clk-m-apb-pm-12 */
+						     ""; /* Unused */
+			};
+
+			clk_m_a1_div3: clk-m-a1-div3 {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c32-odf3",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_m_a1_osc_prediv>,
+					 <&clk_m_a1_pll0 3>, /* PLL0 PHI3 */
+					 <&clk_m_a1_pll1 3>; /* PLL1 PHI3 */
+
+				clock-output-names = "", /* Unused */
+						     "", /* Unused */
+						     "", /* Unused */
+						     "", /* Unused */
+						     "", /* Unused */
+						     "", /* Unused */
+						     "", /* Unused */
+						     ""; /* clk-m-gpu-alt */
+			};
+		};
+
+		clk_m_a9_ext2f_div2: clk-m-a9-ext2f-div2 {
+			#clock-cells = <0>;
+			compatible = "fixed-factor-clock";
+			clocks = <&clk_m_a0_div1 2>;
+			clock-div = <2>;
+			clock-mult = <1>;
+		};
+
+		clockgen-a@fd345000 {
+			reg = <0xfd345000 0xb50>;
+
+			clk_m_a2_pll0: clk-m-a2-pll0 {
+				#clock-cells = <1>;
+				compatible = "st,plls-c32-a1x-0", "st,clkgen-plls-c32";
+
+				clocks = <&clk_sysin>;
+
+				clock-output-names = "clk-m-a2-pll0-phi0",
+						     "clk-m-a2-pll0-phi1",
+						     "clk-m-a2-pll0-phi2",
+						     "clk-m-a2-pll0-phi3";
+			};
+
+			clk_m_a2_pll1: clk-m-a2-pll1 {
+				#clock-cells = <1>;
+				compatible = "st,plls-c32-a1x-1", "st,clkgen-plls-c32";
+
+				clocks = <&clk_sysin>;
+
+				clock-output-names = "clk-m-a2-pll1-phi0",
+						     "clk-m-a2-pll1-phi1",
+						     "clk-m-a2-pll1-phi2",
+						     "clk-m-a2-pll1-phi3";
+			};
+
+			clk_m_a2_osc_prediv: clk-m-a2-osc-prediv {
+				#clock-cells = <0>;
+				compatible = "st,clkgena-prediv-c32",
+					     "st,clkgena-prediv";
+
+				clocks = <&clk_sysin>;
+
+				clock-output-names = "clk-m-a2-osc-prediv";
+			};
+
+			clk_m_a2_div0: clk-m-a2-div0 {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c32-odf0",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_m_a2_osc_prediv>,
+					 <&clk_m_a2_pll0 0>, /* PLL0 PHI0 */
+					 <&clk_m_a2_pll1 0>; /* PLL1 PHI0 */
+
+				clock-output-names = "clk-m-vtac-main-phy",
+						     "clk-m-vtac-aux-phy",
+						     "clk-m-stac-phy",
+						     "clk-m-stac-sys",
+						     "", /* clk-m-mpestac-pg */
+						     "", /* clk-m-mpestac-wc */
+						     "", /* clk-m-mpevtacaux-pg*/
+						     ""; /* clk-m-mpevtacmain-pg*/
+			};
+
+			clk_m_a2_div1: clk-m-a2-div1 {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c32-odf1",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_m_a2_osc_prediv>,
+					 <&clk_m_a2_pll0 1>, /* PLL0 PHI1 */
+					 <&clk_m_a2_pll1 1>; /* PLL1 PHI1 */
+
+				clock-output-names = "", /* clk-m-mpevtacrx0-wc */
+						     "", /* clk-m-mpevtacrx1-wc */
+						     "clk-m-compo-main",
+						     "clk-m-compo-aux",
+						     "clk-m-bdisp-0",
+						     "clk-m-bdisp-1",
+						     "clk-m-icn-bdisp",
+						     "clk-m-icn-compo";
+			};
+
+			clk_m_a2_div2: clk-m-a2-div2 {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c32-odf2",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_m_a2_osc_prediv>,
+					 <&clk_m_a2_pll0 2>, /* PLL0 PHI2 */
+					 <&clk_m_a2_pll1 2>; /* PLL1 PHI2 */
+
+				clock-output-names = "clk-m-icn-vdp-2",
+						     "", /* Unused */
+						     "clk-m-icn-reg-14",
+						     "clk-m-mdtp",
+						     "clk-m-jpegdec",
+						     "", /* Unused */
+						     "clk-m-dcephy-impctrl",
+						     ""; /* Unused */
+			};
+
+			clk_m_a2_div3: clk-m-a2-div3 {
+				#clock-cells = <1>;
+				compatible = "st,clkgena-divmux-c32-odf3",
+					     "st,clkgena-divmux";
+
+				clocks = <&clk_m_a2_osc_prediv>,
+					 <&clk_m_a2_pll0 3>, /* PLL0 PHI3 */
+					 <&clk_m_a2_pll1 3>; /* PLL1 PHI3 */
+
+				clock-output-names = "", /* Unused */
+						     ""; /* clk-m-apb-pm-11 */
+						     /* Remaining outputs unused */
+			};
+		};
+
+		/*
+		 * A9 PLL
+		 */
+		clockgen-a9@fdde08b0 {
+			reg = <0xfdde08b0 0x70>;
+
+			clockgen_a9_pll: clockgen-a9-pll {
+				#clock-cells = <1>;
+				compatible = "st,stih416-plls-c32-a9", "st,clkgen-plls-c32";
+
+				clocks = <&clk_sysin>;
+				clock-output-names = "clockgen-a9-pll-odf";
+			};
+		};
+
+		/*
+		 * ARM CPU related clocks
+		 */
+		clk_m_a9: clk-m-a9@fdde08ac {
+			#clock-cells = <0>;
+			compatible = "st,stih416-clkgen-a9-mux", "st,clkgen-mux";
+			reg = <0xfdde08ac 0x4>;
+			clocks = <&clockgen_a9_pll 0>,
+				 <&clockgen_a9_pll 0>,
+				 <&clk_m_a0_div1 2>,
+				 <&clk_m_a9_ext2f_div2>;
 		};
 
 		/*
 		 * ARM Peripheral clock for timers
 		 */
-		arm_periph_clk: arm_periph_clk {
+		arm_periph_clk: clk-m-a9-periphs {
 			#clock-cells = <0>;
-			compatible = "fixed-clock";
-			clock-frequency = <600000000>;
+			compatible = "fixed-factor-clock";
+			clocks = <&clk_m_a9>;
+			clock-div = <2>;
+			clock-mult = <1>;
 		};
 
 		/*
-		 * Bootloader initialized system infrastructure clock for
-		 * serial devices.
+		 * Frequency synthesizers on the SASG2
 		 */
-		CLK_S_ICN_REG_0: clockgenA0@4 {
+		clockgen_b0: clockgen-b0@fee108b4 {
+			#clock-cells = <1>;
+			compatible = "st,stih416-quadfs216", "st,quadfs";
+			reg = <0xfee108b4 0x44>;
+
+			clocks = <&clk_sysin>;
+			clock-output-names = "clk-s-usb48",
+					     "clk-s-dss",
+					     "clk-s-stfe-frc-2",
+					     "clk-s-thsens-scard";
+		};
+
+		clockgen_b1: clockgen-b1@fe8308c4 {
+			#clock-cells = <1>;
+			compatible = "st,stih416-quadfs216", "st,quadfs";
+			reg = <0xfe8308c4 0x44>;
+
+			clocks = <&clk_sysin>;
+			clock-output-names = "clk-s-pcm-0",
+					     "clk-s-pcm-1",
+					     "clk-s-pcm-2",
+					     "clk-s-pcm-3";
+		};
+
+		clockgen_c: clockgen-c@fe8307d0 {
+			#clock-cells = <1>;
+			compatible = "st,stih416-quadfs432", "st,quadfs";
+			reg = <0xfe8307d0 0x44>;
+
+			clocks = <&clk_sysin>;
+			clock-output-names = "clk-s-c-fs0-ch0",
+					     "clk-s-c-vcc-sd",
+					     "clk-s-c-fs0-ch2";
+		};
+
+		clk_s_vcc_hd: clk-s-vcc-hd@fe8308b8 {
 			#clock-cells = <0>;
-			compatible = "fixed-clock";
-			clock-frequency = <100000000>;
-			clock-output-names = "CLK_S_ICN_REG_0";
+			compatible = "st,stih416-clkgenc-vcc-hd", "st,clkgen-mux";
+			reg = <0xfe8308b8 0x4>; /* SYSCFG2558 */
+
+			clocks = <&clk_sysin>,
+				 <&clockgen_c 0>;
 		};
 
-		CLK_S_GMAC0_PHY: clockgenA1@7 {
+		/*
+		 * Add a dummy clock for the HDMI PHY for the VCC input mux
+		 */
+		clk_s_tmds_fromphy: clk-s-tmds-fromphy {
 			#clock-cells = <0>;
 			compatible = "fixed-clock";
-			clock-frequency = <25000000>;
-			clock-output-names = "CLK_S_GMAC0_PHY";
+			clock-frequency = <0>;
+		};
+
+		clockgen_c_vcc: clockgen-c-vcc@fe8308ac {
+			#clock-cells = <1>;
+			compatible = "st,stih416-clkgenc", "st,clkgen-vcc";
+			reg = <0xfe8308ac 0xc>; /* SYSCFG2555,2556,2557 */
+
+			clocks = <&clk_s_vcc_hd>,
+				 <&clockgen_c 1>,
+				 <&clk_s_tmds_fromphy>,
+				 <&clockgen_c 2>;
+
+			clock-output-names  = "clk-s-pix-hdmi",
+					      "clk-s-pix-dvo",
+					      "clk-s-out-dvo",
+					      "clk-s-pix-hd",
+					      "clk-s-hddac",
+					      "clk-s-denc",
+					      "clk-s-sddac",
+					      "clk-s-pix-main",
+					      "clk-s-pix-aux",
+					      "clk-s-stfe-frc-0",
+					      "clk-s-ref-mcru",
+					      "clk-s-slave-mcru",
+					      "clk-s-tmds-hdmi",
+					      "clk-s-hdmi-reject-pll",
+					      "clk-s-thsens";
 		};
 
-		CLK_S_ETH1_PHY: clockgenA0@7 {
+		clockgen_d: clockgen-d@fee107e0 {
+			#clock-cells = <1>;
+			compatible = "st,stih416-quadfs216", "st,quadfs";
+			reg = <0xfee107e0 0x44>;
+
+			clocks = <&clk_sysin>;
+			clock-output-names = "clk-s-ccsc",
+					     "clk-s-stfe-frc-1",
+					     "clk-s-tsout-1",
+					     "clk-s-mchi";
+		};
+
+		/*
+		 * Frequency synthesizers on the MPE42
+		 */
+		clockgen_e: clockgen-e@fd3208bc {
+			#clock-cells = <1>;
+			compatible = "st,stih416-quadfs660-E", "st,quadfs";
+			reg = <0xfd3208bc 0xb0>;
+
+			clocks = <&clk_sysin>;
+			clock-output-names = "clk-m-pix-mdtp-0",
+					     "clk-m-pix-mdtp-1",
+					     "clk-m-pix-mdtp-2",
+					     "clk-m-mpelpc";
+		};
+
+		clockgen_f: clockgen-f@fd320878 {
+			#clock-cells = <1>;
+			compatible = "st,stih416-quadfs660-F", "st,quadfs";
+			reg = <0xfd320878 0xf0>;
+
+			clocks = <&clk_sysin>;
+			clock-output-names = "clk-m-main-vidfs",
+					     "clk-m-hva-fs",
+					     "clk-m-fvdp-vcpu",
+					     "clk-m-fvdp-proc-fs";
+		};
+
+		clk_m_fvdp_proc: clk-m-fvdp-proc@fd320910 {
+			#clock-cells = <0>;
+			compatible = "st,stih416-clkgenf-vcc-fvdp", "st,clkgen-mux";
+			reg = <0xfd320910 0x4>; /* SYSCFG8580 */
+
+			clocks = <&clk_m_a1_div2 0>,
+				 <&clockgen_f 3>;
+		};
+
+		clk_m_hva: clk-m-hva@fd690868 {
+			#clock-cells = <0>;
+			compatible = "st,stih416-clkgenf-vcc-hva", "st,clkgen-mux";
+			reg = <0xfd690868 0x4>; /* SYSCFG9538 */
+
+			clocks = <&clockgen_f 1>,
+				 <&clk_m_a1_div0 3>;
+		};
+
+		clk_m_f_vcc_hd: clk-m-f-vcc-hd@fd32086c {
+			#clock-cells = <0>;
+			compatible = "st,stih416-clkgenf-vcc-hd", "st,clkgen-mux";
+			reg = <0xfd32086c 0x4>; /* SYSCFG8539 */
+
+			clocks = <&clockgen_c_vcc 7>,
+				 <&clockgen_f 0>;
+		};
+
+		clk_m_f_vcc_sd: clk-m-f-vcc-sd@fd32086c {
+			#clock-cells = <0>;
+			compatible = "st,stih416-clkgenf-vcc-sd", "st,clkgen-mux";
+			reg = <0xfd32086c 0x4>; /* SYSCFG8539 */
+
+			clocks = <&clockgen_c_vcc 8>,
+				 <&clockgen_f 1>;
+		};
+
+		/*
+		 * Add a dummy clock for the HDMIRx external signal clock
+		 */
+		clk_m_pix_hdmirx_sas: clk-m-pix-hdmirx-sas {
 			#clock-cells = <0>;
 			compatible = "fixed-clock";
-			clock-frequency = <25000000>;
-			clock-output-names = "CLK_S_ETH1_PHY";
+			clock-frequency = <0>;
+		};
+
+		clockgen_f_vcc: clockgen-f-vcc@fd32086c {
+			#clock-cells = <1>;
+			compatible = "st,stih416-clkgenf", "st,clkgen-vcc";
+			reg = <0xfd32086c 0xc>; /* SYSCFG8539,8540,8541 */
+
+			clocks = <&clk_m_f_vcc_hd>,
+				 <&clk_m_f_vcc_sd>,
+				 <&clockgen_f 0>,
+				 <&clk_m_pix_hdmirx_sas>;
+
+			clock-output-names  = "clk-m-pix-main-pipe",
+					      "clk-m-pix-aux-pipe",
+					      "clk-m-pix-main-cru",
+					      "clk-m-pix-aux-cru",
+					      "clk-m-xfer-be-compo",
+					      "clk-m-xfer-pip-compo",
+					      "clk-m-xfer-aux-compo",
+					      "clk-m-vsens",
+					      "clk-m-pix-hdmirx-0",
+					      "clk-m-pix-hdmirx-1";
+		};
+
+		/*
+		 * DDR PLL
+		 */
+		clockgen-ddr@0xfdde07d8 {
+			reg = <0xfdde07d8 0x110>;
+
+			clockgen_ddr_pll: clockgen-ddr-pll {
+				#clock-cells = <1>;
+				compatible = "st,stih416-plls-c32-ddr", "st,clkgen-plls-c32";
+
+				clocks = <&clk_sysin>;
+				clock-output-names = "clockgen-ddr0",
+						     "clockgen-ddr1";
+			};
+		};
+
+		/*
+		 * GPU PLL
+		 */
+		clockgen-gpu@fd68ff00 {
+			reg = <0xfd68ff00 0x910>;
+
+			clockgen_gpu_pll: clockgen-gpu-pll {
+				#clock-cells = <1>;
+				compatible = "st,stih416-gpu-pll-c32", "st,clkgengpu-pll-c32";
+
+				clocks = <&clk_sysin>;
+				clock-output-names = "clockgen-gpu-pll";
+			};
 		};
 	};
 };
diff --git a/arch/arm/boot/dts/stih416-pinctrl.dtsi b/arch/arm/boot/dts/stih416-pinctrl.dtsi
index aeea304086e..ee6c119e261 100644
--- a/arch/arm/boot/dts/stih416-pinctrl.dtsi
+++ b/arch/arm/boot/dts/stih416-pinctrl.dtsi
@@ -53,7 +53,7 @@
 			reg 		= <0xfe61f080 0x4>;
 			reg-names	= "irqmux";
 			interrupts 	= <GIC_SPI 182 IRQ_TYPE_LEVEL_HIGH>;
-			interrupts-names = "irqmux";
+			interrupt-names	= "irqmux";
 			ranges		= <0 0xfe610000 0x6000>;
 
 			PIO0: gpio@fe610000 {
@@ -122,6 +122,22 @@
 				};
 			};
 
+			keyscan {
+				pinctrl_keyscan: keyscan {
+					st,pins {
+						keyin0 = <&PIO0 2 ALT2 IN>;
+						keyin1 = <&PIO0 3 ALT2 IN>;
+						keyin2 = <&PIO0 4 ALT2 IN>;
+						keyin3 = <&PIO2 6 ALT2 IN>;
+
+						keyout0 = <&PIO1 6 ALT2 OUT>;
+						keyout1 = <&PIO1 7 ALT2 OUT>;
+						keyout2 = <&PIO0 6 ALT2 OUT>;
+						keyout3 = <&PIO2 7 ALT2 OUT>;
+					};
+				};
+			};
+
 			sbc_i2c0 {
 				pinctrl_sbc_i2c0_default: sbc_i2c0-default {
 					st,pins {
@@ -201,7 +217,7 @@
 			reg 		= <0xfee0f080 0x4>;
 			reg-names	= "irqmux";
 			interrupts 	= <GIC_SPI 183 IRQ_TYPE_LEVEL_HIGH>;
-			interrupts-names = "irqmux";
+			interrupt-names	= "irqmux";
 			ranges		= <0 0xfee00000 0x10000>;
 
 			PIO5: gpio@fee00000 {
@@ -333,7 +349,7 @@
 			reg 		= <0xfe82f080 0x4>;
 			reg-names	= "irqmux";
 			interrupts 	= <GIC_SPI 184 IRQ_TYPE_LEVEL_HIGH>;
-			interrupts-names = "irqmux";
+			interrupt-names	= "irqmux";
 			ranges 		= <0 0xfe820000 0x6000>;
 
 			PIO13: gpio@fe820000 {
@@ -461,7 +477,7 @@
 			reg 		= <0xfd6bf080 0x4>;
 			reg-names	= "irqmux";
 			interrupts 	= <GIC_SPI 113 IRQ_TYPE_LEVEL_HIGH>;
-			interrupts-names = "irqmux";
+			interrupt-names	= "irqmux";
 			ranges		= <0 0xfd6b0000 0x3000>;
 
 			PIO100: gpio@fd6b0000 {
@@ -498,7 +514,7 @@
 			reg 		= <0xfd33f080 0x4>;
 			reg-names	= "irqmux";
 			interrupts 	= <GIC_SPI 114 IRQ_TYPE_LEVEL_HIGH>;
-			interrupts-names = "irqmux";
+			interrupt-names	= "irqmux";
 			ranges			= <0 0xfd330000 0x5000>;
 
 			PIO103: gpio@fd330000 {
diff --git a/arch/arm/boot/dts/stih416.dtsi b/arch/arm/boot/dts/stih416.dtsi
index 78746d20382..06473c5d9ea 100644
--- a/arch/arm/boot/dts/stih416.dtsi
+++ b/arch/arm/boot/dts/stih416.dtsi
@@ -89,7 +89,7 @@
 			status 		= "disabled";
 			reg		= <0xfed32000 0x2c>;
 			interrupts	= <0 197 0>;
-			clocks          = <&CLK_S_ICN_REG_0>;
+			clocks 		= <&clk_s_a0_ls CLK_ICN_REG>;
 			pinctrl-names 	= "default";
 			pinctrl-0 	= <&pinctrl_serial2 &pinctrl_serial2_oe>;
 		};
@@ -102,14 +102,14 @@
 			interrupts	= <0 210 0>;
 			pinctrl-names 	= "default";
 			pinctrl-0 	= <&pinctrl_sbc_serial1>;
-			clocks          = <&CLK_SYSIN>;
+			clocks          = <&clk_sysin>;
 		};
 
 		i2c@fed40000 {
 			compatible	= "st,comms-ssc4-i2c";
 			reg		= <0xfed40000 0x110>;
 			interrupts	= <GIC_SPI 187 IRQ_TYPE_LEVEL_HIGH>;
-			clocks		= <&CLK_S_ICN_REG_0>;
+			clocks 		= <&clk_s_a0_ls CLK_ICN_REG>;
 			clock-names	= "ssc";
 			clock-frequency = <400000>;
 			pinctrl-names	= "default";
@@ -122,7 +122,7 @@
 			compatible	= "st,comms-ssc4-i2c";
 			reg		= <0xfed41000 0x110>;
 			interrupts	= <GIC_SPI 188 IRQ_TYPE_LEVEL_HIGH>;
-			clocks		= <&CLK_S_ICN_REG_0>;
+			clocks 		= <&clk_s_a0_ls CLK_ICN_REG>;
 			clock-names	= "ssc";
 			clock-frequency = <400000>;
 			pinctrl-names	= "default";
@@ -135,7 +135,7 @@
 			compatible	= "st,comms-ssc4-i2c";
 			reg		= <0xfe540000 0x110>;
 			interrupts	= <GIC_SPI 206 IRQ_TYPE_LEVEL_HIGH>;
-			clocks		= <&CLK_SYSIN>;
+			clocks		= <&clk_sysin>;
 			clock-names	= "ssc";
 			clock-frequency = <400000>;
 			pinctrl-names	= "default";
@@ -148,7 +148,7 @@
 			compatible	= "st,comms-ssc4-i2c";
 			reg		= <0xfe541000 0x110>;
 			interrupts	= <GIC_SPI 207 IRQ_TYPE_LEVEL_HIGH>;
-			clocks		= <&CLK_SYSIN>;
+			clocks		= <&clk_sysin>;
 			clock-names	= "ssc";
 			clock-frequency = <400000>;
 			pinctrl-names	= "default";
@@ -176,7 +176,7 @@
 			pinctrl-names 	= "default";
 			pinctrl-0	= <&pinctrl_mii0>;
 			clock-names	= "stmmaceth";
-			clocks		= <&CLK_S_GMAC0_PHY>;
+			clocks		= <&clk_s_a1_ls CLK_GMAC0_PHY>;
 		};
 
 		ethernet1: dwmac@fef08000 {
@@ -198,7 +198,7 @@
 			pinctrl-names 	= "default";
 			pinctrl-0	= <&pinctrl_mii1>;
 			clock-names	= "stmmaceth";
-			clocks		= <&CLK_S_ETH1_PHY>;
+			clocks		= <&clk_s_a0_ls CLK_ETH1_PHY>;
 		};
 
 		rc: rc@fe518000 {
@@ -206,7 +206,7 @@
 			reg		= <0xfe518000 0x234>;
 			interrupts	=  <0 203 0>;
 			rx-mode         = "infrared";
-			clocks		= <&CLK_SYSIN>;
+			clocks		= <&clk_sysin>;
 			pinctrl-names 	= "default";
 			pinctrl-0	= <&pinctrl_ir>;
 			resets		= <&softreset STIH416_IRB_SOFTRESET>;
@@ -224,5 +224,17 @@
 
 			status = "disabled";
 		};
+
+		keyscan: keyscan@fe4b0000 {
+			compatible = "st,sti-keyscan";
+			status = "disabled";
+			reg = <0xfe4b0000 0x2000>;
+			interrupts = <GIC_SPI 212 IRQ_TYPE_NONE>;
+			clocks = <&clk_sysin>;
+			pinctrl-names = "default";
+			pinctrl-0 = <&pinctrl_keyscan>;
+			resets	= <&powerdown STIH416_KEYSCAN_POWERDOWN>,
+				  <&softreset STIH416_KEYSCAN_SOFTRESET>;
+		};
 	};
 };
diff --git a/arch/arm/boot/dts/stih41x-b2000.dtsi b/arch/arm/boot/dts/stih41x-b2000.dtsi
index bf65c49095a..b3dd6ca5c2a 100644
--- a/arch/arm/boot/dts/stih41x-b2000.dtsi
+++ b/arch/arm/boot/dts/stih41x-b2000.dtsi
@@ -6,6 +6,7 @@
  * it under the terms of the GNU General Public License version 2 as
  * publishhed by the Free Software Foundation.
  */
+#include <dt-bindings/input/input.h>
 / {
 
 	memory{
@@ -14,7 +15,7 @@
 	};
 
 	chosen {
-		bootargs = "console=ttyAS0,115200";
+		bootargs = "console=ttyAS0,115200 clk_ignore_unused";
 		linux,stdout-path = &serial2;
 	};
 
@@ -68,5 +69,27 @@
 			snps,reset-active-low;
 			snps,reset-delays-us 	= <0 10000 10000>;
 		};
+
+		keyscan: keyscan@fe4b0000 {
+			keypad,num-rows = <4>;
+			keypad,num-columns = <4>;
+			st,debounce-us = <5000>;
+			linux,keymap = < MATRIX_KEY(0x00, 0x00, KEY_F13)
+					 MATRIX_KEY(0x00, 0x01, KEY_F9)
+					 MATRIX_KEY(0x00, 0x02, KEY_F5)
+					 MATRIX_KEY(0x00, 0x03, KEY_F1)
+					 MATRIX_KEY(0x01, 0x00, KEY_F14)
+					 MATRIX_KEY(0x01, 0x01, KEY_F10)
+					 MATRIX_KEY(0x01, 0x02, KEY_F6)
+					 MATRIX_KEY(0x01, 0x03, KEY_F2)
+					 MATRIX_KEY(0x02, 0x00, KEY_F15)
+					 MATRIX_KEY(0x02, 0x01, KEY_F11)
+					 MATRIX_KEY(0x02, 0x02, KEY_F7)
+					 MATRIX_KEY(0x02, 0x03, KEY_F3)
+					 MATRIX_KEY(0x03, 0x00, KEY_F16)
+					 MATRIX_KEY(0x03, 0x01, KEY_F12)
+					 MATRIX_KEY(0x03, 0x02, KEY_F8)
+					 MATRIX_KEY(0x03, 0x03, KEY_F4) >;
+		};
 	};
 };
diff --git a/arch/arm/boot/dts/stih41x-b2020.dtsi b/arch/arm/boot/dts/stih41x-b2020.dtsi
index 838513f9ddc..d8a84295c32 100644
--- a/arch/arm/boot/dts/stih41x-b2020.dtsi
+++ b/arch/arm/boot/dts/stih41x-b2020.dtsi
@@ -14,7 +14,7 @@
 	};
 
 	chosen {
-		bootargs = "console=ttyAS0,115200";
+		bootargs = "console=ttyAS0,115200 clk_ignore_unused";
 		linux,stdout-path = &sbc_serial1;
 	};
 
diff --git a/arch/arm/boot/dts/stih41x.dtsi b/arch/arm/boot/dts/stih41x.dtsi
index f5b9898d9c6..5cb0e63376b 100644
--- a/arch/arm/boot/dts/stih41x.dtsi
+++ b/arch/arm/boot/dts/stih41x.dtsi
@@ -1,3 +1,10 @@
+/*
+ * Copyright (C) 2014 STMicroelectronics Limited.
+ *
+ * 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.
+ */
 / {
 	#address-cells = <1>;
 	#size-cells = <1>;
diff --git a/arch/arm/boot/dts/sun4i-a10-a1000.dts b/arch/arm/boot/dts/sun4i-a10-a1000.dts
index fa746aea5e6..0b97c071dd5 100644
--- a/arch/arm/boot/dts/sun4i-a10-a1000.dts
+++ b/arch/arm/boot/dts/sun4i-a10-a1000.dts
@@ -36,6 +36,16 @@
 			};
 		};
 
+		mmc0: mmc@01c0f000 {
+			pinctrl-names = "default";
+			pinctrl-0 = <&mmc0_pins_a>, <&mmc0_cd_pin_reference_design>;
+			vmmc-supply = <&reg_vcc3v3>;
+			bus-width = <4>;
+			cd-gpios = <&pio 7 1 0>; /* PH1 */
+			cd-inverted;
+			status = "okay";
+		};
+
 		usbphy: phy@01c13400 {
 			usb1_vbus-supply = <&reg_usb1_vbus>;
 			usb2_vbus-supply = <&reg_usb2_vbus>;
diff --git a/arch/arm/boot/dts/sun4i-a10-cubieboard.dts b/arch/arm/boot/dts/sun4i-a10-cubieboard.dts
index 4684cbe6843..c200eacc66e 100644
--- a/arch/arm/boot/dts/sun4i-a10-cubieboard.dts
+++ b/arch/arm/boot/dts/sun4i-a10-cubieboard.dts
@@ -34,6 +34,16 @@
 			};
 		};
 
+		mmc0: mmc@01c0f000 {
+			pinctrl-names = "default";
+			pinctrl-0 = <&mmc0_pins_a>, <&mmc0_cd_pin_reference_design>;
+			vmmc-supply = <&reg_vcc3v3>;
+			bus-width = <4>;
+			cd-gpios = <&pio 7 1 0>; /* PH1 */
+			cd-inverted;
+			status = "okay";
+		};
+
 		usbphy: phy@01c13400 {
 			usb1_vbus-supply = <&reg_usb1_vbus>;
 			usb2_vbus-supply = <&reg_usb2_vbus>;
diff --git a/arch/arm/boot/dts/sun4i-a10-hackberry.dts b/arch/arm/boot/dts/sun4i-a10-hackberry.dts
index d7c17e46ce2..547fadcb984 100644
--- a/arch/arm/boot/dts/sun4i-a10-hackberry.dts
+++ b/arch/arm/boot/dts/sun4i-a10-hackberry.dts
@@ -36,6 +36,16 @@
 			};
 		};
 
+		mmc0: mmc@01c0f000 {
+			pinctrl-names = "default";
+			pinctrl-0 = <&mmc0_pins_a>, <&mmc0_cd_pin_reference_design>;
+			vmmc-supply = <&reg_vcc3v3>;
+			bus-width = <4>;
+			cd-gpios = <&pio 7 1 0>; /* PH1 */
+			cd-inverted;
+			status = "okay";
+		};
+
 		usbphy: phy@01c13400 {
 			usb1_vbus-supply = <&reg_usb1_vbus>;
 			usb2_vbus-supply = <&reg_usb2_vbus>;
diff --git a/arch/arm/boot/dts/sun4i-a10-inet97fv2.dts b/arch/arm/boot/dts/sun4i-a10-inet97fv2.dts
index fe9272ee55c..f13723e18b8 100644
--- a/arch/arm/boot/dts/sun4i-a10-inet97fv2.dts
+++ b/arch/arm/boot/dts/sun4i-a10-inet97fv2.dts
@@ -24,6 +24,16 @@
 	};
 
 	soc@01c00000 {
+		mmc0: mmc@01c0f000 {
+			pinctrl-names = "default";
+			pinctrl-0 = <&mmc0_pins_a>, <&mmc0_cd_pin_reference_design>;
+			vmmc-supply = <&reg_vcc3v3>;
+			bus-width = <4>;
+			cd-gpios = <&pio 7 1 0>; /* PH1 */
+			cd-inverted;
+			status = "okay";
+		};
+
 		uart0: serial@01c28000 {
 			pinctrl-names = "default";
 			pinctrl-0 = <&uart0_pins_a>;
diff --git a/arch/arm/boot/dts/sun4i-a10-mini-xplus.dts b/arch/arm/boot/dts/sun4i-a10-mini-xplus.dts
index dd84a9e313b..c01cea50cf0 100644
--- a/arch/arm/boot/dts/sun4i-a10-mini-xplus.dts
+++ b/arch/arm/boot/dts/sun4i-a10-mini-xplus.dts
@@ -20,6 +20,16 @@
 	compatible = "pineriver,mini-xplus", "allwinner,sun4i-a10";
 
 	soc@01c00000 {
+		mmc0: mmc@01c0f000 {
+			pinctrl-names = "default";
+			pinctrl-0 = <&mmc0_pins_a>, <&mmc0_cd_pin_reference_design>;
+			vmmc-supply = <&reg_vcc3v3>;
+			bus-width = <4>;
+			cd-gpios = <&pio 7 1 0>; /* PH1 */
+			cd-inverted;
+			status = "okay";
+		};
+
 		usbphy: phy@01c13400 {
 			usb1_vbus-supply = <&reg_usb1_vbus>;
 			usb2_vbus-supply = <&reg_usb2_vbus>;
diff --git a/arch/arm/boot/dts/sun4i-a10-olinuxino-lime.dts b/arch/arm/boot/dts/sun4i-a10-olinuxino-lime.dts
index 66cf0c7cf5b..d46a7dbecef 100644
--- a/arch/arm/boot/dts/sun4i-a10-olinuxino-lime.dts
+++ b/arch/arm/boot/dts/sun4i-a10-olinuxino-lime.dts
@@ -33,6 +33,16 @@
 			};
 		};
 
+		mmc0: mmc@01c0f000 {
+			pinctrl-names = "default";
+			pinctrl-0 = <&mmc0_pins_a>, <&mmc0_cd_pin_reference_design>;
+			vmmc-supply = <&reg_vcc3v3>;
+			bus-width = <4>;
+			cd-gpios = <&pio 7 1 0>; /* PH1 */
+			cd-inverted;
+			status = "okay";
+		};
+
 		usbphy: phy@01c13400 {
 			usb1_vbus-supply = <&reg_usb1_vbus>;
 			usb2_vbus-supply = <&reg_usb2_vbus>;
diff --git a/arch/arm/boot/dts/sun4i-a10-pcduino.dts b/arch/arm/boot/dts/sun4i-a10-pcduino.dts
index 255b47e7019..fb03bccb78d 100644
--- a/arch/arm/boot/dts/sun4i-a10-pcduino.dts
+++ b/arch/arm/boot/dts/sun4i-a10-pcduino.dts
@@ -34,6 +34,16 @@
 			};
 		};
 
+		mmc0: mmc@01c0f000 {
+			pinctrl-names = "default";
+			pinctrl-0 = <&mmc0_pins_a>, <&mmc0_cd_pin_reference_design>;
+			vmmc-supply = <&reg_vcc3v3>;
+			bus-width = <4>;
+			cd-gpios = <&pio 7 1 0>; /* PH1 */
+			cd-inverted;
+			status = "okay";
+		};
+
 		usbphy: phy@01c13400 {
 			usb1_vbus-supply = <&reg_usb1_vbus>;
 			usb2_vbus-supply = <&reg_usb2_vbus>;
diff --git a/arch/arm/boot/dts/sun4i-a10.dtsi b/arch/arm/boot/dts/sun4i-a10.dtsi
index 9174724571e..c7b794ed337 100644
--- a/arch/arm/boot/dts/sun4i-a10.dtsi
+++ b/arch/arm/boot/dts/sun4i-a10.dtsi
@@ -377,6 +377,42 @@
 			#size-cells = <0>;
 		};
 
+		mmc0: mmc@01c0f000 {
+			compatible = "allwinner,sun4i-a10-mmc";
+			reg = <0x01c0f000 0x1000>;
+			clocks = <&ahb_gates 8>, <&mmc0_clk>;
+			clock-names = "ahb", "mmc";
+			interrupts = <32>;
+			status = "disabled";
+		};
+
+		mmc1: mmc@01c10000 {
+			compatible = "allwinner,sun4i-a10-mmc";
+			reg = <0x01c10000 0x1000>;
+			clocks = <&ahb_gates 9>, <&mmc1_clk>;
+			clock-names = "ahb", "mmc";
+			interrupts = <33>;
+			status = "disabled";
+		};
+
+		mmc2: mmc@01c11000 {
+			compatible = "allwinner,sun4i-a10-mmc";
+			reg = <0x01c11000 0x1000>;
+			clocks = <&ahb_gates 10>, <&mmc2_clk>;
+			clock-names = "ahb", "mmc";
+			interrupts = <34>;
+			status = "disabled";
+		};
+
+		mmc3: mmc@01c12000 {
+			compatible = "allwinner,sun4i-a10-mmc";
+			reg = <0x01c12000 0x1000>;
+			clocks = <&ahb_gates 11>, <&mmc3_clk>;
+			clock-names = "ahb", "mmc";
+			interrupts = <35>;
+			status = "disabled";
+		};
+
 		usbphy: phy@01c13400 {
 			#phy-cells = <1>;
 			compatible = "allwinner,sun4i-a10-usb-phy";
@@ -477,6 +513,20 @@
 			#size-cells = <0>;
 			#gpio-cells = <3>;
 
+			pwm0_pins_a: pwm0@0 {
+				allwinner,pins = "PB2";
+				allwinner,function = "pwm";
+				allwinner,drive = <0>;
+				allwinner,pull = <0>;
+			};
+
+			pwm1_pins_a: pwm1@0 {
+				allwinner,pins = "PI3";
+				allwinner,function = "pwm";
+				allwinner,drive = <0>;
+				allwinner,pull = <0>;
+			};
+
 			uart0_pins_a: uart0@0 {
 				allwinner,pins = "PB22", "PB23";
 				allwinner,function = "uart0";
@@ -529,6 +579,20 @@
 				allwinner,drive = <0>;
 				allwinner,pull = <0>;
 			};
+
+			mmc0_pins_a: mmc0@0 {
+				allwinner,pins = "PF0","PF1","PF2","PF3","PF4","PF5";
+				allwinner,function = "mmc0";
+				allwinner,drive = <2>;
+				allwinner,pull = <0>;
+			};
+
+			mmc0_cd_pin_reference_design: mmc0_cd_pin@0 {
+				allwinner,pins = "PH1";
+				allwinner,function = "gpio_in";
+				allwinner,drive = <0>;
+				allwinner,pull = <1>;
+			};
 		};
 
 		timer@01c20c00 {
@@ -549,6 +613,14 @@
 			interrupts = <24>;
 		};
 
+		pwm: pwm@01c20e00 {
+			compatible = "allwinner,sun4i-a10-pwm";
+			reg = <0x01c20e00 0xc>;
+			clocks = <&osc24M>;
+			#pwm-cells = <3>;
+			status = "disabled";
+		};
+
 		sid: eeprom@01c23800 {
 			compatible = "allwinner,sun4i-a10-sid";
 			reg = <0x01c23800 0x10>;
@@ -647,6 +719,8 @@
 			clocks = <&apb1_gates 0>;
 			clock-frequency = <100000>;
 			status = "disabled";
+			#address-cells = <1>;
+			#size-cells = <0>;
 		};
 
 		i2c1: i2c@01c2b000 {
@@ -656,6 +730,8 @@
 			clocks = <&apb1_gates 1>;
 			clock-frequency = <100000>;
 			status = "disabled";
+			#address-cells = <1>;
+			#size-cells = <0>;
 		};
 
 		i2c2: i2c@01c2b400 {
@@ -665,6 +741,8 @@
 			clocks = <&apb1_gates 2>;
 			clock-frequency = <100000>;
 			status = "disabled";
+			#address-cells = <1>;
+			#size-cells = <0>;
 		};
 	};
 };
diff --git a/arch/arm/boot/dts/sun5i-a10s-olinuxino-micro.dts b/arch/arm/boot/dts/sun5i-a10s-olinuxino-micro.dts
index 23611b71d3a..ea9519da576 100644
--- a/arch/arm/boot/dts/sun5i-a10s-olinuxino-micro.dts
+++ b/arch/arm/boot/dts/sun5i-a10s-olinuxino-micro.dts
@@ -35,6 +35,26 @@
 			};
 		};
 
+		mmc0: mmc@01c0f000 {
+			pinctrl-names = "default";
+			pinctrl-0 = <&mmc0_pins_a>, <&mmc0_cd_pin_olinuxino_micro>;
+			vmmc-supply = <&reg_vcc3v3>;
+			bus-width = <4>;
+			cd-gpios = <&pio 6 1 0>; /* PG1 */
+			cd-inverted;
+			status = "okay";
+		};
+
+		mmc1: mmc@01c10000 {
+			pinctrl-names = "default";
+			pinctrl-0 = <&mmc1_pins_a>, <&mmc1_cd_pin_olinuxino_micro>;
+			vmmc-supply = <&reg_vcc3v3>;
+			bus-width = <4>;
+			cd-gpios = <&pio 6 13 0>; /* PG13 */
+			cd-inverted;
+			status = "okay";
+		};
+
 		usbphy: phy@01c13400 {
 			usb1_vbus-supply = <&reg_usb1_vbus>;
 			status = "okay";
@@ -49,6 +69,20 @@
 		};
 
 		pinctrl@01c20800 {
+			mmc0_cd_pin_olinuxino_micro: mmc0_cd_pin@0 {
+				allwinner,pins = "PG1";
+				allwinner,function = "gpio_in";
+				allwinner,drive = <0>;
+				allwinner,pull = <1>;
+			};
+
+			mmc1_cd_pin_olinuxino_micro: mmc1_cd_pin@0 {
+				allwinner,pins = "PG13";
+				allwinner,function = "gpio_in";
+				allwinner,drive = <0>;
+				allwinner,pull = <1>;
+			};
+
 			led_pins_olinuxino: led_pins@0 {
 				allwinner,pins = "PE3";
 				allwinner,function = "gpio_out";
diff --git a/arch/arm/boot/dts/sun5i-a10s-r7-tv-dongle.dts b/arch/arm/boot/dts/sun5i-a10s-r7-tv-dongle.dts
new file mode 100644
index 00000000000..43a93762d4f
--- /dev/null
+++ b/arch/arm/boot/dts/sun5i-a10s-r7-tv-dongle.dts
@@ -0,0 +1,100 @@
+/*
+ * Copyright 2014 Hans de Goede <hdegoede@redhat.com>
+ *
+ * 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
+ */
+
+/dts-v1/;
+/include/ "sun5i-a10s.dtsi"
+/include/ "sunxi-common-regulators.dtsi"
+
+/ {
+	model = "R7 A10s hdmi tv-stick";
+	compatible = "allwinner,r7-tv-dongle", "allwinner,sun5i-a10s";
+
+	soc@01c00000 {
+		mmc0: mmc@01c0f000 {
+			pinctrl-names = "default";
+			pinctrl-0 = <&mmc0_pins_a>, <&mmc0_cd_pin_r7>;
+			vmmc-supply = <&reg_vcc3v3>;
+			bus-width = <4>;
+			cd-gpios = <&pio 6 1 0>; /* PG1 */
+			cd-inverted;
+			status = "okay";
+		};
+
+		mmc1: mmc@01c10000 {
+			pinctrl-names = "default";
+			pinctrl-0 = <&mmc1_pins_a>;
+			vmmc-supply = <&reg_vcc3v3>;
+			bus-width = <4>;
+			non-removable;
+			status = "okay";
+		};
+
+		usbphy: phy@01c13400 {
+			usb1_vbus-supply = <&reg_usb1_vbus>;
+			status = "okay";
+		};
+
+		ehci0: usb@01c14000 {
+			status = "okay";
+		};
+
+		ohci0: usb@01c14400 {
+			status = "okay";
+		};
+
+		pinctrl@01c20800 {
+			mmc0_cd_pin_r7: mmc0_cd_pin@0 {
+				allwinner,pins = "PG1";
+				allwinner,function = "gpio_in";
+				allwinner,drive = <0>;
+				allwinner,pull = <1>;
+			};
+
+			led_pins_r7: led_pins@0 {
+				allwinner,pins = "PB2";
+				allwinner,function = "gpio_out";
+				allwinner,drive = <1>;
+				allwinner,pull = <0>;
+			};
+
+			usb1_vbus_pin_r7: usb1_vbus_pin@0 {
+				allwinner,pins = "PG13";
+				allwinner,function = "gpio_out";
+				allwinner,drive = <0>;
+				allwinner,pull = <0>;
+			};
+		};
+
+		uart0: serial@01c28000 {
+			pinctrl-names = "default";
+			pinctrl-0 = <&uart0_pins_a>;
+			status = "okay";
+		};
+	};
+
+	leds {
+		compatible = "gpio-leds";
+		pinctrl-names = "default";
+		pinctrl-0 = <&led_pins_r7>;
+
+		green {
+			label = "r7-tv-dongle:green:usr";
+			gpios = <&pio 1 2 0>;
+			default-state = "on";
+		};
+	};
+
+	reg_usb1_vbus: usb1-vbus {
+		pinctrl-0 = <&usb1_vbus_pin_r7>;
+		gpio = <&pio 6 13 0>;
+		status = "okay";
+	};
+};
diff --git a/arch/arm/boot/dts/sun5i-a10s.dtsi b/arch/arm/boot/dts/sun5i-a10s.dtsi
index 79989ed5658..aa1dd59dc34 100644
--- a/arch/arm/boot/dts/sun5i-a10s.dtsi
+++ b/arch/arm/boot/dts/sun5i-a10s.dtsi
@@ -338,6 +338,33 @@
 			#size-cells = <0>;
 		};
 
+		mmc0: mmc@01c0f000 {
+			compatible = "allwinner,sun5i-a13-mmc";
+			reg = <0x01c0f000 0x1000>;
+			clocks = <&ahb_gates 8>, <&mmc0_clk>;
+			clock-names = "ahb", "mmc";
+			interrupts = <32>;
+			status = "disabled";
+		};
+
+		mmc1: mmc@01c10000 {
+			compatible = "allwinner,sun5i-a13-mmc";
+			reg = <0x01c10000 0x1000>;
+			clocks = <&ahb_gates 9>, <&mmc1_clk>;
+			clock-names = "ahb", "mmc";
+			interrupts = <33>;
+			status = "disabled";
+		};
+
+		mmc2: mmc@01c11000 {
+			compatible = "allwinner,sun5i-a13-mmc";
+			reg = <0x01c11000 0x1000>;
+			clocks = <&ahb_gates 10>, <&mmc2_clk>;
+			clock-names = "ahb", "mmc";
+			interrupts = <34>;
+			status = "disabled";
+		};
+
 		usbphy: phy@01c13400 {
 			#phy-cells = <1>;
 			compatible = "allwinner,sun5i-a13-usb-phy";
@@ -451,6 +478,20 @@
 				allwinner,drive = <0>;
 				allwinner,pull = <0>;
 			};
+
+			mmc0_pins_a: mmc0@0 {
+				allwinner,pins = "PF0","PF1","PF2","PF3","PF4","PF5";
+				allwinner,function = "mmc0";
+				allwinner,drive = <2>;
+				allwinner,pull = <0>;
+			};
+
+			mmc1_pins_a: mmc1@0 {
+				allwinner,pins = "PG3","PG4","PG5","PG6","PG7","PG8";
+				allwinner,function = "mmc1";
+				allwinner,drive = <2>;
+				allwinner,pull = <0>;
+			};
 		};
 
 		timer@01c20c00 {
diff --git a/arch/arm/boot/dts/sun5i-a13-olinuxino-micro.dts b/arch/arm/boot/dts/sun5i-a13-olinuxino-micro.dts
index 11169d5b5b8..fa44b026483 100644
--- a/arch/arm/boot/dts/sun5i-a13-olinuxino-micro.dts
+++ b/arch/arm/boot/dts/sun5i-a13-olinuxino-micro.dts
@@ -21,6 +21,16 @@
 	compatible = "olimex,a13-olinuxino-micro", "allwinner,sun5i-a13";
 
 	soc@01c00000 {
+		mmc0: mmc@01c0f000 {
+			pinctrl-names = "default";
+			pinctrl-0 = <&mmc0_pins_a>, <&mmc0_cd_pin_olinuxinom>;
+			vmmc-supply = <&reg_vcc3v3>;
+			bus-width = <4>;
+			cd-gpios = <&pio 6 0 0>; /* PG0 */
+			cd-inverted;
+			status = "okay";
+		};
+
 		usbphy: phy@01c13400 {
 			usb1_vbus-supply = <&reg_usb1_vbus>;
 			status = "okay";
@@ -35,6 +45,13 @@
 		};
 
 		pinctrl@01c20800 {
+			mmc0_cd_pin_olinuxinom: mmc0_cd_pin@0 {
+				allwinner,pins = "PG0";
+				allwinner,function = "gpio_in";
+				allwinner,drive = <0>;
+				allwinner,pull = <1>;
+			};
+
 			led_pins_olinuxinom: led_pins@0 {
 				allwinner,pins = "PG9";
 				allwinner,function = "gpio_out";
diff --git a/arch/arm/boot/dts/sun5i-a13-olinuxino.dts b/arch/arm/boot/dts/sun5i-a13-olinuxino.dts
index 7a9187bbeb2..429994e1943 100644
--- a/arch/arm/boot/dts/sun5i-a13-olinuxino.dts
+++ b/arch/arm/boot/dts/sun5i-a13-olinuxino.dts
@@ -20,6 +20,16 @@
 	compatible = "olimex,a13-olinuxino", "allwinner,sun5i-a13";
 
 	soc@01c00000 {
+		mmc0: mmc@01c0f000 {
+			pinctrl-names = "default";
+			pinctrl-0 = <&mmc0_pins_a>, <&mmc0_cd_pin_olinuxino>;
+			vmmc-supply = <&reg_vcc3v3>;
+			bus-width = <4>;
+			cd-gpios = <&pio 6 0 0>; /* PG0 */
+			cd-inverted;
+			status = "okay";
+		};
+
 		usbphy: phy@01c13400 {
 			usb1_vbus-supply = <&reg_usb1_vbus>;
 			status = "okay";
@@ -34,6 +44,13 @@
 		};
 
 		pinctrl@01c20800 {
+			mmc0_cd_pin_olinuxino: mmc0_cd_pin@0 {
+				allwinner,pins = "PG0";
+				allwinner,function = "gpio_in";
+				allwinner,drive = <0>;
+				allwinner,pull = <1>;
+			};
+
 			led_pins_olinuxino: led_pins@0 {
 				allwinner,pins = "PG9";
 				allwinner,function = "gpio_out";
diff --git a/arch/arm/boot/dts/sun5i-a13.dtsi b/arch/arm/boot/dts/sun5i-a13.dtsi
index f01c315bdc4..c9fdb7b3ecd 100644
--- a/arch/arm/boot/dts/sun5i-a13.dtsi
+++ b/arch/arm/boot/dts/sun5i-a13.dtsi
@@ -320,6 +320,24 @@
 			#size-cells = <0>;
 		};
 
+		mmc0: mmc@01c0f000 {
+			compatible = "allwinner,sun5i-a13-mmc";
+			reg = <0x01c0f000 0x1000>;
+			clocks = <&ahb_gates 8>, <&mmc0_clk>;
+			clock-names = "ahb", "mmc";
+			interrupts = <32>;
+			status = "disabled";
+		};
+
+		mmc2: mmc@01c11000 {
+			compatible = "allwinner,sun5i-a13-mmc";
+			reg = <0x01c11000 0x1000>;
+			clocks = <&ahb_gates 10>, <&mmc2_clk>;
+			clock-names = "ahb", "mmc";
+			interrupts = <34>;
+			status = "disabled";
+		};
+
 		usbphy: phy@01c13400 {
 			#phy-cells = <1>;
 			compatible = "allwinner,sun5i-a13-usb-phy";
@@ -415,6 +433,13 @@
 				allwinner,drive = <0>;
 				allwinner,pull = <0>;
 			};
+
+			mmc0_pins_a: mmc0@0 {
+				allwinner,pins = "PF0","PF1","PF2","PF3","PF4","PF5";
+				allwinner,function = "mmc0";
+				allwinner,drive = <2>;
+				allwinner,pull = <0>;
+			};
 		};
 
 		timer@01c20c00 {
@@ -467,6 +492,8 @@
 			clocks = <&apb1_gates 0>;
 			clock-frequency = <100000>;
 			status = "disabled";
+			#address-cells = <1>;
+			#size-cells = <0>;
 		};
 
 		i2c1: i2c@01c2b000 {
@@ -476,6 +503,8 @@
 			clocks = <&apb1_gates 1>;
 			clock-frequency = <100000>;
 			status = "disabled";
+			#address-cells = <1>;
+			#size-cells = <0>;
 		};
 
 		i2c2: i2c@01c2b400 {
@@ -485,6 +514,8 @@
 			clocks = <&apb1_gates 2>;
 			clock-frequency = <100000>;
 			status = "disabled";
+			#address-cells = <1>;
+			#size-cells = <0>;
 		};
 
 		timer@01c60000 {
diff --git a/arch/arm/boot/dts/sun6i-a31-app4-evb1.dts b/arch/arm/boot/dts/sun6i-a31-app4-evb1.dts
new file mode 100644
index 00000000000..2bbf8867362
--- /dev/null
+++ b/arch/arm/boot/dts/sun6i-a31-app4-evb1.dts
@@ -0,0 +1,57 @@
+/*
+ * Copyright 2014 Boris Brezillon
+ *
+ * Boris Brezillon <boris.brezillon@free-electrons.com>
+ *
+ * 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
+ */
+
+/dts-v1/;
+/include/ "sun6i-a31.dtsi"
+/include/ "sunxi-common-regulators.dtsi"
+
+/ {
+	model = "Allwinner A31 APP4 EVB1 Evaluation Board";
+	compatible = "allwinner,app4-evb1", "allwinner,sun6i-a31";
+
+	chosen {
+		bootargs = "earlyprintk console=ttyS0,115200";
+	};
+
+	soc@01c00000 {
+		pio: pinctrl@01c20800 {
+			usb1_vbus_pin_a: usb1_vbus_pin@0 {
+				allwinner,pins = "PH27";
+				allwinner,function = "gpio_out";
+				allwinner,drive = <0>;
+				allwinner,pull = <0>;
+			};
+		};
+
+		usbphy: phy@01c19400 {
+			usb1_vbus-supply = <&reg_usb1_vbus>;
+			status = "okay";
+		};
+
+		ehci0: usb@01c1a000 {
+			status = "okay";
+		};
+
+		uart0: serial@01c28000 {
+			pinctrl-names = "default";
+			pinctrl-0 = <&uart0_pins_a>;
+			status = "okay";
+		};
+	};
+
+	reg_usb1_vbus: usb1-vbus {
+		pinctrl-0 = <&usb1_vbus_pin_a>;
+		gpio = <&pio 7 27 0>;
+		status = "okay";
+	};
+};
diff --git a/arch/arm/boot/dts/sun6i-a31-colombus.dts b/arch/arm/boot/dts/sun6i-a31-colombus.dts
index 3898a7bce83..546cf6eff5c 100644
--- a/arch/arm/boot/dts/sun6i-a31-colombus.dts
+++ b/arch/arm/boot/dts/sun6i-a31-colombus.dts
@@ -13,6 +13,7 @@
 
 /dts-v1/;
 /include/ "sun6i-a31.dtsi"
+/include/ "sunxi-common-regulators.dtsi"
 
 / {
 	model = "WITS A31 Colombus Evaluation Board";
@@ -23,6 +24,45 @@
 	};
 
 	soc@01c00000 {
+		mmc0: mmc@01c0f000 {
+			pinctrl-names = "default";
+			pinctrl-0 = <&mmc0_pins_a>, <&mmc0_cd_pin_colombus>;
+			vmmc-supply = <&reg_vcc3v0>;
+			bus-width = <4>;
+			cd-gpios = <&pio 0 8 0>; /* PA8 */
+			cd-inverted;
+			status = "okay";
+		};
+
+		usbphy: phy@01c19400 {
+			usb2_vbus-supply = <&reg_usb2_vbus>;
+			status = "okay";
+		};
+
+		ehci1: usb@01c1b000 {
+			status = "okay";
+		};
+
+		pio: pinctrl@01c20800 {
+			mmc0_pins_a: mmc0@0 {
+				allwinner,pull = <1>;
+			};
+
+			mmc0_cd_pin_colombus: mmc0_cd_pin@0 {
+				allwinner,pins = "PA8";
+				allwinner,function = "gpio_in";
+				allwinner,drive = <0>;
+				allwinner,pull = <1>;
+			};
+
+			usb2_vbus_pin_colombus: usb2_vbus_pin@0 {
+				allwinner,pins = "PH24";
+				allwinner,function = "gpio_out";
+				allwinner,drive = <0>;
+				allwinner,pull = <0>;
+			};
+		};
+
 		uart0: serial@01c28000 {
 			pinctrl-names = "default";
 			pinctrl-0 = <&uart0_pins_a>;
@@ -47,4 +87,11 @@
 			status = "okay";
 		};
 	};
+
+	reg_usb2_vbus: usb2-vbus {
+		pinctrl-names = "default";
+		pinctrl-0 = <&usb2_vbus_pin_colombus>;
+		gpio = <&pio 7 24 0>;
+		status = "okay";
+	};
 };
diff --git a/arch/arm/boot/dts/sun6i-a31-m9.dts b/arch/arm/boot/dts/sun6i-a31-m9.dts
new file mode 100644
index 00000000000..bc6115da5ae
--- /dev/null
+++ b/arch/arm/boot/dts/sun6i-a31-m9.dts
@@ -0,0 +1,50 @@
+/*
+ * Copyright 2014 Hans de Goede <hdegoede@redhat.com>
+ *
+ * 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
+ */
+
+/dts-v1/;
+/include/ "sun6i-a31.dtsi"
+/include/ "sunxi-common-regulators.dtsi"
+
+/ {
+	model = "Mele M9 / A1000G Quad top set box";
+	compatible = "mele,m9", "allwinner,sun6i-a31";
+
+	chosen {
+		bootargs = "earlyprintk console=ttyS0,115200";
+	};
+
+	soc@01c00000 {
+		mmc0: mmc@01c0f000 {
+			pinctrl-names = "default";
+			pinctrl-0 = <&mmc0_pins_a>, <&mmc0_cd_pin_m9>;
+			vmmc-supply = <&reg_vcc3v3>;
+			bus-width = <4>;
+			cd-gpios = <&pio 7 22 0>; /* PH22 */
+			cd-inverted;
+			status = "okay";
+		};
+
+		pio: pinctrl@01c20800 {
+			mmc0_cd_pin_m9: mmc0_cd_pin@0 {
+				allwinner,pins = "PH22";
+				allwinner,function = "gpio_in";
+				allwinner,drive = <0>;
+				allwinner,pull = <1>;
+			};
+		};
+
+		uart0: serial@01c28000 {
+			pinctrl-names = "default";
+			pinctrl-0 = <&uart0_pins_a>;
+			status = "okay";
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/sun6i-a31.dtsi b/arch/arm/boot/dts/sun6i-a31.dtsi
index d45efa74827..0f4ea4990d2 100644
--- a/arch/arm/boot/dts/sun6i-a31.dtsi
+++ b/arch/arm/boot/dts/sun6i-a31.dtsi
@@ -27,6 +27,7 @@
 
 
 	cpus {
+		enable-method = "allwinner,sun6i-a31";
 		#address-cells = <1>;
 		#size-cells = <0>;
 
@@ -59,6 +60,14 @@
 		reg = <0x40000000 0x80000000>;
 	};
 
+	pmu {
+		compatible = "arm,cortex-a7-pmu", "arm,cortex-a15-pmu";
+		interrupts = <0 120 4>,
+			     <0 121 4>,
+			     <0 122 4>,
+			     <0 123 4>;
+	};
+
 	clocks {
 		#address-cells = <1>;
 		#size-cells = <1>;
@@ -198,6 +207,38 @@
 					"apb2_uart4", "apb2_uart5";
 		};
 
+		mmc0_clk: clk@01c20088 {
+			#clock-cells = <0>;
+			compatible = "allwinner,sun4i-a10-mod0-clk";
+			reg = <0x01c20088 0x4>;
+			clocks = <&osc24M>, <&pll6>;
+			clock-output-names = "mmc0";
+		};
+
+		mmc1_clk: clk@01c2008c {
+			#clock-cells = <0>;
+			compatible = "allwinner,sun4i-a10-mod0-clk";
+			reg = <0x01c2008c 0x4>;
+			clocks = <&osc24M>, <&pll6>;
+			clock-output-names = "mmc1";
+		};
+
+		mmc2_clk: clk@01c20090 {
+			#clock-cells = <0>;
+			compatible = "allwinner,sun4i-a10-mod0-clk";
+			reg = <0x01c20090 0x4>;
+			clocks = <&osc24M>, <&pll6>;
+			clock-output-names = "mmc2";
+		};
+
+		mmc3_clk: clk@01c20094 {
+			#clock-cells = <0>;
+			compatible = "allwinner,sun4i-a10-mod0-clk";
+			reg = <0x01c20094 0x4>;
+			clocks = <&osc24M>, <&pll6>;
+			clock-output-names = "mmc3";
+		};
+
 		spi0_clk: clk@01c200a0 {
 			#clock-cells = <0>;
 			compatible = "allwinner,sun4i-a10-mod0-clk";
@@ -229,6 +270,17 @@
 			clocks = <&osc24M>, <&pll6>;
 			clock-output-names = "spi3";
 		};
+
+		usb_clk: clk@01c200cc {
+			#clock-cells = <1>;
+		        #reset-cells = <1>;
+			compatible = "allwinner,sun6i-a31-usb-clk";
+			reg = <0x01c200cc 0x4>;
+			clocks = <&osc24M>;
+			clock-output-names = "usb_phy0", "usb_phy1", "usb_phy2",
+					     "usb_ohci0", "usb_ohci1",
+					     "usb_ohci2";
+		};
 	};
 
 	soc@01c00000 {
@@ -237,12 +289,134 @@
 		#size-cells = <1>;
 		ranges;
 
-		nmi_intc: interrupt-controller@01f00c0c {
-			compatible = "allwinner,sun6i-a31-sc-nmi";
-			interrupt-controller;
-			#interrupt-cells = <2>;
-			reg = <0x01f00c0c 0x38>;
-			interrupts = <0 32 4>;
+		dma: dma-controller@01c02000 {
+			compatible = "allwinner,sun6i-a31-dma";
+			reg = <0x01c02000 0x1000>;
+			interrupts = <0 50 4>;
+			clocks = <&ahb1_gates 6>;
+			resets = <&ahb1_rst 6>;
+			#dma-cells = <1>;
+		};
+
+		mmc0: mmc@01c0f000 {
+			compatible = "allwinner,sun5i-a13-mmc";
+			reg = <0x01c0f000 0x1000>;
+			clocks = <&ahb1_gates 8>, <&mmc0_clk>;
+			clock-names = "ahb", "mmc";
+			resets = <&ahb1_rst 8>;
+			reset-names = "ahb";
+			interrupts = <0 60 4>;
+			status = "disabled";
+		};
+
+		mmc1: mmc@01c10000 {
+			compatible = "allwinner,sun5i-a13-mmc";
+			reg = <0x01c10000 0x1000>;
+			clocks = <&ahb1_gates 9>, <&mmc1_clk>;
+			clock-names = "ahb", "mmc";
+			resets = <&ahb1_rst 9>;
+			reset-names = "ahb";
+			interrupts = <0 61 4>;
+			status = "disabled";
+		};
+
+		mmc2: mmc@01c11000 {
+			compatible = "allwinner,sun5i-a13-mmc";
+			reg = <0x01c11000 0x1000>;
+			clocks = <&ahb1_gates 10>, <&mmc2_clk>;
+			clock-names = "ahb", "mmc";
+			resets = <&ahb1_rst 10>;
+			reset-names = "ahb";
+			interrupts = <0 62 4>;
+			status = "disabled";
+		};
+
+		mmc3: mmc@01c12000 {
+			compatible = "allwinner,sun5i-a13-mmc";
+			reg = <0x01c12000 0x1000>;
+			clocks = <&ahb1_gates 11>, <&mmc3_clk>;
+			clock-names = "ahb", "mmc";
+			resets = <&ahb1_rst 11>;
+			reset-names = "ahb";
+			interrupts = <0 63 4>;
+			status = "disabled";
+		};
+
+		usbphy: phy@01c19400 {
+			compatible = "allwinner,sun6i-a31-usb-phy";
+			reg = <0x01c19400 0x10>,
+			      <0x01c1a800 0x4>,
+			      <0x01c1b800 0x4>;
+			reg-names = "phy_ctrl",
+				    "pmu1",
+				    "pmu2";
+			clocks = <&usb_clk 8>,
+				 <&usb_clk 9>,
+				 <&usb_clk 10>;
+			clock-names = "usb0_phy",
+				      "usb1_phy",
+				      "usb2_phy";
+			resets = <&usb_clk 0>,
+				 <&usb_clk 1>,
+				 <&usb_clk 2>;
+			reset-names = "usb0_reset",
+				      "usb1_reset",
+				      "usb2_reset";
+			status = "disabled";
+			#phy-cells = <1>;
+		};
+
+		ehci0: usb@01c1a000 {
+			compatible = "allwinner,sun6i-a31-ehci", "generic-ehci";
+			reg = <0x01c1a000 0x100>;
+			interrupts = <0 72 4>;
+			clocks = <&ahb1_gates 26>;
+			resets = <&ahb1_rst 26>;
+			phys = <&usbphy 1>;
+			phy-names = "usb";
+			status = "disabled";
+		};
+
+		ohci0: usb@01c1a400 {
+			compatible = "allwinner,sun6i-a31-ohci", "generic-ohci";
+			reg = <0x01c1a400 0x100>;
+			interrupts = <0 73 4>;
+			clocks = <&ahb1_gates 29>, <&usb_clk 16>;
+			resets = <&ahb1_rst 29>;
+			phys = <&usbphy 1>;
+			phy-names = "usb";
+			status = "disabled";
+		};
+
+		ehci1: usb@01c1b000 {
+			compatible = "allwinner,sun6i-a31-ehci", "generic-ehci";
+			reg = <0x01c1b000 0x100>;
+			interrupts = <0 74 4>;
+			clocks = <&ahb1_gates 27>;
+			resets = <&ahb1_rst 27>;
+			phys = <&usbphy 2>;
+			phy-names = "usb";
+			status = "disabled";
+		};
+
+		ohci1: usb@01c1b400 {
+			compatible = "allwinner,sun6i-a31-ohci", "generic-ohci";
+			reg = <0x01c1b400 0x100>;
+			interrupts = <0 75 4>;
+			clocks = <&ahb1_gates 30>, <&usb_clk 17>;
+			resets = <&ahb1_rst 30>;
+			phys = <&usbphy 2>;
+			phy-names = "usb";
+			status = "disabled";
+		};
+
+		ohci2: usb@01c1c400 {
+			compatible = "allwinner,sun6i-a31-ohci", "generic-ohci";
+			reg = <0x01c1c400 0x100>;
+			interrupts = <0 77 4>;
+			clocks = <&ahb1_gates 31>, <&usb_clk 18>;
+			resets = <&ahb1_rst 31>;
+			status = "disabled";
 		};
 
 		pio: pinctrl@01c20800 {
@@ -286,6 +460,13 @@
 				allwinner,drive = <0>;
 				allwinner,pull = <0>;
 			};
+
+			mmc0_pins_a: mmc0@0 {
+				allwinner,pins = "PF0","PF1","PF2","PF3","PF4","PF5";
+				allwinner,function = "mmc0";
+				allwinner,drive = <2>;
+				allwinner,pull = <0>;
+			};
 		};
 
 		ahb1_rst: reset@01c202c0 {
@@ -330,6 +511,8 @@
 			reg-io-width = <4>;
 			clocks = <&apb2_gates 16>;
 			resets = <&apb2_rst 16>;
+			dmas = <&dma 6>, <&dma 6>;
+			dma-names = "rx", "tx";
 			status = "disabled";
 		};
 
@@ -341,6 +524,8 @@
 			reg-io-width = <4>;
 			clocks = <&apb2_gates 17>;
 			resets = <&apb2_rst 17>;
+			dmas = <&dma 7>, <&dma 7>;
+			dma-names = "rx", "tx";
 			status = "disabled";
 		};
 
@@ -352,6 +537,8 @@
 			reg-io-width = <4>;
 			clocks = <&apb2_gates 18>;
 			resets = <&apb2_rst 18>;
+			dmas = <&dma 8>, <&dma 8>;
+			dma-names = "rx", "tx";
 			status = "disabled";
 		};
 
@@ -363,6 +550,8 @@
 			reg-io-width = <4>;
 			clocks = <&apb2_gates 19>;
 			resets = <&apb2_rst 19>;
+			dmas = <&dma 9>, <&dma 9>;
+			dma-names = "rx", "tx";
 			status = "disabled";
 		};
 
@@ -374,6 +563,8 @@
 			reg-io-width = <4>;
 			clocks = <&apb2_gates 20>;
 			resets = <&apb2_rst 20>;
+			dmas = <&dma 10>, <&dma 10>;
+			dma-names = "rx", "tx";
 			status = "disabled";
 		};
 
@@ -385,6 +576,8 @@
 			reg-io-width = <4>;
 			clocks = <&apb2_gates 21>;
 			resets = <&apb2_rst 21>;
+			dmas = <&dma 22>, <&dma 22>;
+			dma-names = "rx", "tx";
 			status = "disabled";
 		};
 
@@ -434,6 +627,8 @@
 			interrupts = <0 65 4>;
 			clocks = <&ahb1_gates 20>, <&spi0_clk>;
 			clock-names = "ahb", "mod";
+			dmas = <&dma 23>, <&dma 23>;
+			dma-names = "rx", "tx";
 			resets = <&ahb1_rst 20>;
 			status = "disabled";
 		};
@@ -444,6 +639,8 @@
 			interrupts = <0 66 4>;
 			clocks = <&ahb1_gates 21>, <&spi1_clk>;
 			clock-names = "ahb", "mod";
+			dmas = <&dma 24>, <&dma 24>;
+			dma-names = "rx", "tx";
 			resets = <&ahb1_rst 21>;
 			status = "disabled";
 		};
@@ -454,6 +651,8 @@
 			interrupts = <0 67 4>;
 			clocks = <&ahb1_gates 22>, <&spi2_clk>;
 			clock-names = "ahb", "mod";
+			dmas = <&dma 25>, <&dma 25>;
+			dma-names = "rx", "tx";
 			resets = <&ahb1_rst 22>;
 			status = "disabled";
 		};
@@ -464,6 +663,8 @@
 			interrupts = <0 68 4>;
 			clocks = <&ahb1_gates 23>, <&spi3_clk>;
 			clock-names = "ahb", "mod";
+			dmas = <&dma 26>, <&dma 26>;
+			dma-names = "rx", "tx";
 			resets = <&ahb1_rst 23>;
 			status = "disabled";
 		};
@@ -479,14 +680,74 @@
 			interrupts = <1 9 0xf04>;
 		};
 
+		nmi_intc: interrupt-controller@01f00c0c {
+			compatible = "allwinner,sun6i-a31-sc-nmi";
+			interrupt-controller;
+			#interrupt-cells = <2>;
+			reg = <0x01f00c0c 0x38>;
+			interrupts = <0 32 4>;
+		};
+
+		prcm@01f01400 {
+			compatible = "allwinner,sun6i-a31-prcm";
+			reg = <0x01f01400 0x200>;
+
+			ar100: ar100_clk {
+				compatible = "allwinner,sun6i-a31-ar100-clk";
+				#clock-cells = <0>;
+				clocks = <&osc32k>, <&osc24M>, <&pll6>, <&pll6>;
+				clock-output-names = "ar100";
+			};
+
+			ahb0: ahb0_clk {
+				compatible = "fixed-factor-clock";
+				#clock-cells = <0>;
+				clock-div = <1>;
+				clock-mult = <1>;
+				clocks = <&ar100>;
+				clock-output-names = "ahb0";
+			};
+
+			apb0: apb0_clk {
+				compatible = "allwinner,sun6i-a31-apb0-clk";
+				#clock-cells = <0>;
+				clocks = <&ahb0>;
+				clock-output-names = "apb0";
+			};
+
+			apb0_gates: apb0_gates_clk {
+				compatible = "allwinner,sun6i-a31-apb0-gates-clk";
+				#clock-cells = <1>;
+				clocks = <&apb0>;
+				clock-output-names = "apb0_pio", "apb0_ir",
+						"apb0_timer", "apb0_p2wi",
+						"apb0_uart", "apb0_1wire",
+						"apb0_i2c";
+			};
+
+			apb0_rst: apb0_rst {
+				compatible = "allwinner,sun6i-a31-clock-reset";
+				#reset-cells = <1>;
+			};
+		};
+
 		cpucfg@01f01c00 {
 			compatible = "allwinner,sun6i-a31-cpuconfig";
 			reg = <0x01f01c00 0x300>;
 		};
 
-		prcm@01f01c00 {
-			compatible = "allwinner,sun6i-a31-prcm";
-			reg = <0x01f01400 0x200>;
+		r_pio: pinctrl@01f02c00 {
+			compatible = "allwinner,sun6i-a31-r-pinctrl";
+			reg = <0x01f02c00 0x400>;
+			interrupts = <0 45 4>,
+				     <0 46 4>;
+			clocks = <&apb0_gates 0>;
+			resets = <&apb0_rst 0>;
+			gpio-controller;
+			interrupt-controller;
+			#address-cells = <1>;
+			#size-cells = <0>;
+			#gpio-cells = <3>;
 		};
 	};
 };
diff --git a/arch/arm/boot/dts/sun7i-a20-cubieboard2.dts b/arch/arm/boot/dts/sun7i-a20-cubieboard2.dts
index 68de89ffbdf..a5ad945197e 100644
--- a/arch/arm/boot/dts/sun7i-a20-cubieboard2.dts
+++ b/arch/arm/boot/dts/sun7i-a20-cubieboard2.dts
@@ -20,6 +20,16 @@
 	compatible = "cubietech,cubieboard2", "allwinner,sun7i-a20";
 
 	soc@01c00000 {
+		mmc0: mmc@01c0f000 {
+			pinctrl-names = "default";
+			pinctrl-0 = <&mmc0_pins_a>, <&mmc0_cd_pin_reference_design>;
+			vmmc-supply = <&reg_vcc3v3>;
+			bus-width = <4>;
+			cd-gpios = <&pio 7 1 0>; /* PH1 */
+			cd-inverted;
+			status = "okay";
+		};
+
 		usbphy: phy@01c13400 {
 			usb1_vbus-supply = <&reg_usb1_vbus>;
 			usb2_vbus-supply = <&reg_usb2_vbus>;
diff --git a/arch/arm/boot/dts/sun7i-a20-cubietruck.dts b/arch/arm/boot/dts/sun7i-a20-cubietruck.dts
index cb25d3c8da5..b87fea90148 100644
--- a/arch/arm/boot/dts/sun7i-a20-cubietruck.dts
+++ b/arch/arm/boot/dts/sun7i-a20-cubietruck.dts
@@ -20,6 +20,25 @@
 	compatible = "cubietech,cubietruck", "allwinner,sun7i-a20";
 
 	soc@01c00000 {
+		mmc0: mmc@01c0f000 {
+			pinctrl-names = "default";
+			pinctrl-0 = <&mmc0_pins_a>, <&mmc0_cd_pin_reference_design>;
+			vmmc-supply = <&reg_vcc3v3>;
+			bus-width = <4>;
+			cd-gpios = <&pio 7 1 0>; /* PH1 */
+			cd-inverted;
+			status = "okay";
+		};
+
+		mmc3: mmc@01c12000 {
+			pinctrl-names = "default";
+			pinctrl-0 = <&mmc3_pins_a>;
+			vmmc-supply = <&reg_vmmc3>;
+			bus-width = <4>;
+			non-removable;
+			status = "okay";
+		};
+
 		usbphy: phy@01c13400 {
 			usb1_vbus-supply = <&reg_usb1_vbus>;
 			usb2_vbus-supply = <&reg_usb2_vbus>;
@@ -48,6 +67,18 @@
 		};
 
 		pinctrl@01c20800 {
+			mmc3_pins_a: mmc3@0 {
+				/* AP6210 requires pull-up */
+				allwinner,pull = <1>;
+			};
+
+			vmmc3_pin_cubietruck: vmmc3_pin@0 {
+				allwinner,pins = "PH9";
+				allwinner,function = "gpio_out";
+				allwinner,drive = <0>;
+				allwinner,pull = <0>;
+			};
+
 			ahci_pwr_pin_cubietruck: ahci_pwr_pin@1 {
 				allwinner,pins = "PH12";
 				allwinner,function = "gpio_out";
@@ -63,6 +94,12 @@
 			};
 		};
 
+		pwm: pwm@01c20e00 {
+			pinctrl-names = "default";
+			pinctrl-0 = <&pwm0_pins_a>, <&pwm1_pins_a>;
+			status = "okay";
+		};
+
 		uart0: serial@01c28000 {
 			pinctrl-names = "default";
 			pinctrl-0 = <&uart0_pins_a>;
@@ -139,4 +176,15 @@
 	reg_usb2_vbus: usb2-vbus {
 		status = "okay";
 	};
+
+	reg_vmmc3: vmmc3 {
+		compatible = "regulator-fixed";
+		pinctrl-names = "default";
+		pinctrl-0 = <&vmmc3_pin_cubietruck>;
+		regulator-name = "vmmc3";
+		regulator-min-microvolt = <3300000>;
+		regulator-max-microvolt = <3300000>;
+		enable-active-high;
+		gpio = <&pio 7 9 0>;
+	};
 };
diff --git a/arch/arm/boot/dts/sun7i-a20-i12-tvbox.dts b/arch/arm/boot/dts/sun7i-a20-i12-tvbox.dts
new file mode 100644
index 00000000000..b77308e9019
--- /dev/null
+++ b/arch/arm/boot/dts/sun7i-a20-i12-tvbox.dts
@@ -0,0 +1,176 @@
+/*
+ * Copyright 2014 Hans de Goede <hdegoede@redhat.com>
+ *
+ * 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
+ */
+
+/dts-v1/;
+/include/ "sun7i-a20.dtsi"
+/include/ "sunxi-common-regulators.dtsi"
+
+/ {
+	model = "I12 / Q5 / QT840A A20 tvbox";
+	compatible = "allwinner,i12-tvbox", "allwinner,sun7i-a20";
+
+	soc@01c00000 {
+		mmc0: mmc@01c0f000 {
+			pinctrl-names = "default";
+			pinctrl-0 = <&mmc0_pins_a>, <&mmc0_cd_pin_reference_design>;
+			vmmc-supply = <&reg_vcc3v3>;
+			bus-width = <4>;
+			cd-gpios = <&pio 7 1 0>; /* PH1 */
+			cd-inverted;
+			status = "okay";
+		};
+
+		mmc3: mmc@01c12000 {
+			pinctrl-names = "default";
+			pinctrl-0 = <&mmc3_pins_a>;
+			vmmc-supply = <&reg_vmmc3>;
+			bus-width = <4>;
+			non-removable;
+			status = "okay";
+		};
+
+		usbphy: phy@01c13400 {
+			usb1_vbus-supply = <&reg_usb1_vbus>;
+			usb2_vbus-supply = <&reg_usb2_vbus>;
+			status = "okay";
+		};
+
+		ehci0: usb@01c14000 {
+			status = "okay";
+		};
+
+		ohci0: usb@01c14400 {
+			status = "okay";
+		};
+
+		ehci1: usb@01c1c000 {
+			status = "okay";
+		};
+
+		ohci1: usb@01c1c400 {
+			status = "okay";
+		};
+
+		pinctrl@01c20800 {
+			mmc3_pins_a: mmc3@0 {
+				/* AP6210 / AP6330 requires pull-up */
+				allwinner,pull = <1>;
+			};
+
+			vmmc3_pin_i12_tvbox: vmmc3_pin@0 {
+				allwinner,pins = "PH2";
+				allwinner,function = "gpio_out";
+				allwinner,drive = <0>;
+				allwinner,pull = <0>;
+			};
+
+			vmmc3_io_pin_i12_tvbox: vmmc3_io_pin@0 {
+				allwinner,pins = "PH12";
+				allwinner,function = "gpio_out";
+				allwinner,drive = <0>;
+				allwinner,pull = <0>;
+			};
+
+			gmac_power_pin_i12_tvbox: gmac_power_pin@0 {
+				allwinner,pins = "PH21";
+				allwinner,function = "gpio_out";
+				allwinner,drive = <0>;
+				allwinner,pull = <0>;
+			};
+
+			led_pins_i12_tvbox: led_pins@0 {
+				allwinner,pins = "PH9", "PH20";
+				allwinner,function = "gpio_out";
+				allwinner,drive = <0>;
+				allwinner,pull = <0>;
+			};
+		};
+
+		uart0: serial@01c28000 {
+			pinctrl-names = "default";
+			pinctrl-0 = <&uart0_pins_a>;
+			status = "okay";
+		};
+
+		gmac: ethernet@01c50000 {
+			pinctrl-names = "default";
+			pinctrl-0 = <&gmac_pins_mii_a>;
+			phy = <&phy1>;
+			phy-mode = "mii";
+			phy-supply = <&reg_gmac_3v3>;
+			status = "okay";
+
+			phy1: ethernet-phy@1 {
+				reg = <1>;
+			};
+		};
+	};
+
+	leds {
+		compatible = "gpio-leds";
+		pinctrl-names = "default";
+		pinctrl-0 = <&led_pins_i12_tvbox>;
+
+		red {
+			label = "i12_tvbox:red:usr";
+			gpios = <&pio 7 9 1>;
+		};
+
+		blue {
+			label = "i12_tvbox:blue:usr";
+			gpios = <&pio 7 20 0>;
+		};
+	};
+
+	reg_usb1_vbus: usb1-vbus {
+		status = "okay";
+	};
+
+	reg_usb2_vbus: usb2-vbus {
+		status = "okay";
+	};
+
+	reg_vmmc3: vmmc3 {
+		compatible = "regulator-fixed";
+		pinctrl-names = "default";
+		pinctrl-0 = <&vmmc3_pin_i12_tvbox>;
+		regulator-name = "vmmc3";
+		regulator-min-microvolt = <3300000>;
+		regulator-max-microvolt = <3300000>;
+		enable-active-high;
+		gpio = <&pio 7 2 0>;
+	};
+
+	reg_vmmc3_io: vmmc3-io {
+		compatible = "regulator-fixed";
+		pinctrl-names = "default";
+		pinctrl-0 = <&vmmc3_io_pin_i12_tvbox>;
+		regulator-name = "vmmc3-io";
+		regulator-min-microvolt = <3300000>;
+		regulator-max-microvolt = <3300000>;
+		/* This controls VCC-PI, must be always on! */
+		regulator-always-on;
+		enable-active-high;
+		gpio = <&pio 7 12 0>;
+	};
+
+	reg_gmac_3v3: gmac-3v3 {
+		compatible = "regulator-fixed";
+		pinctrl-names = "default";
+		pinctrl-0 = <&gmac_power_pin_i12_tvbox>;
+		regulator-name = "gmac-3v3";
+		regulator-min-microvolt = <3300000>;
+		regulator-max-microvolt = <3300000>;
+		startup-delay-us = <50000>;
+		enable-active-high;
+		gpio = <&pio 7 21 0>;
+	};
+};
diff --git a/arch/arm/boot/dts/sun7i-a20-olinuxino-micro.dts b/arch/arm/boot/dts/sun7i-a20-olinuxino-micro.dts
index eeadf76362f..b759630bc9a 100644
--- a/arch/arm/boot/dts/sun7i-a20-olinuxino-micro.dts
+++ b/arch/arm/boot/dts/sun7i-a20-olinuxino-micro.dts
@@ -31,6 +31,26 @@
 			status = "okay";
 		};
 
+		mmc0: mmc@01c0f000 {
+			pinctrl-names = "default";
+			pinctrl-0 = <&mmc0_pins_a>, <&mmc0_cd_pin_reference_design>;
+			vmmc-supply = <&reg_vcc3v3>;
+			bus-width = <4>;
+			cd-gpios = <&pio 7 1 0>; /* PH1 */
+			cd-inverted;
+			status = "okay";
+		};
+
+		mmc3: mmc@01c12000 {
+			pinctrl-names = "default";
+			pinctrl-0 = <&mmc3_pins_a>, <&mmc3_cd_pin_olinuxinom>;
+			vmmc-supply = <&reg_vcc3v3>;
+			bus-width = <4>;
+			cd-gpios = <&pio 7 11 0>; /* PH11 */
+			cd-inverted;
+			status = "okay";
+		};
+
 		usbphy: phy@01c13400 {
 			usb1_vbus-supply = <&reg_usb1_vbus>;
 			usb2_vbus-supply = <&reg_usb2_vbus>;
@@ -65,6 +85,13 @@
 		};
 
 		pinctrl@01c20800 {
+			mmc3_cd_pin_olinuxinom: mmc3_cd_pin@0 {
+				allwinner,pins = "PH11";
+				allwinner,function = "gpio_in";
+				allwinner,drive = <0>;
+				allwinner,pull = <1>;
+			};
+
 			led_pins_olinuxino: led_pins@0 {
 				allwinner,pins = "PH2";
 				allwinner,function = "gpio_out";
diff --git a/arch/arm/boot/dts/sun7i-a20.dtsi b/arch/arm/boot/dts/sun7i-a20.dtsi
index 32efc105df8..385933bac11 100644
--- a/arch/arm/boot/dts/sun7i-a20.dtsi
+++ b/arch/arm/boot/dts/sun7i-a20.dtsi
@@ -57,6 +57,12 @@
 			     <1 10 0xf08>;
 	};
 
+	pmu {
+		compatible = "arm,cortex-a7-pmu", "arm,cortex-a15-pmu";
+		interrupts = <0 120 4>,
+			     <0 121 4>;
+	};
+
 	clocks {
 		#address-cells = <1>;
 		#size-cells = <1>;
@@ -87,7 +93,7 @@
 
 		pll4: clk@01c20018 {
 			#clock-cells = <0>;
-			compatible = "allwinner,sun4i-a10-pll1-clk";
+			compatible = "allwinner,sun7i-a20-pll4-clk";
 			reg = <0x01c20018 0x4>;
 			clocks = <&osc24M>;
 			clock-output-names = "pll4";
@@ -109,6 +115,14 @@
 			clock-output-names = "pll6_sata", "pll6_other", "pll6";
 		};
 
+		pll8: clk@01c20040 {
+			#clock-cells = <0>;
+			compatible = "allwinner,sun7i-a20-pll4-clk";
+			reg = <0x01c20040 0x4>;
+			clocks = <&osc24M>;
+			clock-output-names = "pll8";
+		};
+
 		cpu: cpu@01c20054 {
 			#clock-cells = <0>;
 			compatible = "allwinner,sun4i-a10-cpu-clk";
@@ -447,6 +461,42 @@
 			#size-cells = <0>;
 		};
 
+		mmc0: mmc@01c0f000 {
+			compatible = "allwinner,sun5i-a13-mmc";
+			reg = <0x01c0f000 0x1000>;
+			clocks = <&ahb_gates 8>, <&mmc0_clk>;
+			clock-names = "ahb", "mmc";
+			interrupts = <0 32 4>;
+			status = "disabled";
+		};
+
+		mmc1: mmc@01c10000 {
+			compatible = "allwinner,sun5i-a13-mmc";
+			reg = <0x01c10000 0x1000>;
+			clocks = <&ahb_gates 9>, <&mmc1_clk>;
+			clock-names = "ahb", "mmc";
+			interrupts = <0 33 4>;
+			status = "disabled";
+		};
+
+		mmc2: mmc@01c11000 {
+			compatible = "allwinner,sun5i-a13-mmc";
+			reg = <0x01c11000 0x1000>;
+			clocks = <&ahb_gates 10>, <&mmc2_clk>;
+			clock-names = "ahb", "mmc";
+			interrupts = <0 34 4>;
+			status = "disabled";
+		};
+
+		mmc3: mmc@01c12000 {
+			compatible = "allwinner,sun5i-a13-mmc";
+			reg = <0x01c12000 0x1000>;
+			clocks = <&ahb_gates 11>, <&mmc3_clk>;
+			clock-names = "ahb", "mmc";
+			interrupts = <0 35 4>;
+			status = "disabled";
+		};
+
 		usbphy: phy@01c13400 {
 			#phy-cells = <1>;
 			compatible = "allwinner,sun7i-a20-usb-phy";
@@ -540,6 +590,20 @@
 			#size-cells = <0>;
 			#gpio-cells = <3>;
 
+			pwm0_pins_a: pwm0@0 {
+				allwinner,pins = "PB2";
+				allwinner,function = "pwm";
+				allwinner,drive = <0>;
+				allwinner,pull = <0>;
+			};
+
+			pwm1_pins_a: pwm1@0 {
+				allwinner,pins = "PI3";
+				allwinner,function = "pwm";
+				allwinner,drive = <0>;
+				allwinner,pull = <0>;
+			};
+
 			uart0_pins_a: uart0@0 {
 				allwinner,pins = "PB22", "PB23";
 				allwinner,function = "uart0";
@@ -653,6 +717,27 @@
 				allwinner,drive = <0>;
 				allwinner,pull = <0>;
 			};
+
+			mmc0_pins_a: mmc0@0 {
+				allwinner,pins = "PF0","PF1","PF2","PF3","PF4","PF5";
+				allwinner,function = "mmc0";
+				allwinner,drive = <2>;
+				allwinner,pull = <0>;
+			};
+
+			mmc0_cd_pin_reference_design: mmc0_cd_pin@0 {
+				allwinner,pins = "PH1";
+				allwinner,function = "gpio_in";
+				allwinner,drive = <0>;
+				allwinner,pull = <1>;
+			};
+
+			mmc3_pins_a: mmc3@0 {
+				allwinner,pins = "PI4","PI5","PI6","PI7","PI8","PI9";
+				allwinner,function = "mmc3";
+				allwinner,drive = <2>;
+				allwinner,pull = <0>;
+			};
 		};
 
 		timer@01c20c00 {
@@ -678,6 +763,14 @@
 			interrupts = <0 24 4>;
 		};
 
+		pwm: pwm@01c20e00 {
+			compatible = "allwinner,sun7i-a20-pwm";
+			reg = <0x01c20e00 0xc>;
+			clocks = <&osc24M>;
+			#pwm-cells = <3>;
+			status = "disabled";
+		};
+
 		sid: eeprom@01c23800 {
 			compatible = "allwinner,sun7i-a20-sid";
 			reg = <0x01c23800 0x200>;
@@ -776,6 +869,8 @@
 			clocks = <&apb1_gates 0>;
 			clock-frequency = <100000>;
 			status = "disabled";
+			#address-cells = <1>;
+			#size-cells = <0>;
 		};
 
 		i2c1: i2c@01c2b000 {
@@ -785,6 +880,8 @@
 			clocks = <&apb1_gates 1>;
 			clock-frequency = <100000>;
 			status = "disabled";
+			#address-cells = <1>;
+			#size-cells = <0>;
 		};
 
 		i2c2: i2c@01c2b400 {
@@ -794,6 +891,8 @@
 			clocks = <&apb1_gates 2>;
 			clock-frequency = <100000>;
 			status = "disabled";
+			#address-cells = <1>;
+			#size-cells = <0>;
 		};
 
 		i2c3: i2c@01c2b800 {
@@ -803,15 +902,19 @@
 			clocks = <&apb1_gates 3>;
 			clock-frequency = <100000>;
 			status = "disabled";
+			#address-cells = <1>;
+			#size-cells = <0>;
 		};
 
-		i2c4: i2c@01c2bc00 {
+		i2c4: i2c@01c2c000 {
 			compatible = "allwinner,sun4i-i2c";
-			reg = <0x01c2bc00 0x400>;
+			reg = <0x01c2c000 0x400>;
 			interrupts = <0 89 4>;
 			clocks = <&apb1_gates 15>;
 			clock-frequency = <100000>;
 			status = "disabled";
+			#address-cells = <1>;
+			#size-cells = <0>;
 		};
 
 		gmac: ethernet@01c50000 {
diff --git a/arch/arm/boot/dts/sunxi-common-regulators.dtsi b/arch/arm/boot/dts/sunxi-common-regulators.dtsi
index 18eeac0670b..3d021efd1a3 100644
--- a/arch/arm/boot/dts/sunxi-common-regulators.dtsi
+++ b/arch/arm/boot/dts/sunxi-common-regulators.dtsi
@@ -72,4 +72,18 @@
 		gpio = <&pio 7 3 0>;
 		status = "disabled";
 	};
+
+	reg_vcc3v0: vcc3v0 {
+		compatible = "regulator-fixed";
+		regulator-name = "vcc3v0";
+		regulator-min-microvolt = <3000000>;
+		regulator-max-microvolt = <3000000>;
+	};
+
+	reg_vcc3v3: vcc3v3 {
+		compatible = "regulator-fixed";
+		regulator-name = "vcc3v3";
+		regulator-min-microvolt = <3300000>;
+		regulator-max-microvolt = <3300000>;
+	};
 };
diff --git a/arch/arm/boot/dts/tegra114-dalmore.dts b/arch/arm/boot/dts/tegra114-dalmore.dts
index a288a12823e..5c21d216515 100644
--- a/arch/arm/boot/dts/tegra114-dalmore.dts
+++ b/arch/arm/boot/dts/tegra114-dalmore.dts
@@ -25,6 +25,7 @@
 		hdmi@54280000 {
 			status = "okay";
 
+			hdmi-supply = <&vdd_5v0_hdmi>;
 			vdd-supply = <&vdd_hdmi_reg>;
 			pll-supply = <&palmas_smps3_reg>;
 
@@ -36,6 +37,8 @@
 		dsi@54300000 {
 			status = "okay";
 
+			avdd-dsi-csi-supply = <&avdd_1v2_reg>;
+
 			panel@0 {
 				compatible = "panasonic,vvx10f004b00",
 					     "simple-panel";
@@ -982,12 +985,10 @@
 						regulator-max-microvolt = <2800000>;
 					};
 
-					ldo3 {
+					avdd_1v2_reg: ldo3 {
 						regulator-name = "avdd-dsi-csi";
 						regulator-min-microvolt = <1200000>;
 						regulator-max-microvolt = <1200000>;
-						regulator-always-on;
-						regulator-boot-on;
 					};
 
 					ldo4 {
@@ -1105,6 +1106,7 @@
 
 	sdhci@78000400 {
 		cd-gpios = <&gpio TEGRA_GPIO(V, 2) GPIO_ACTIVE_LOW>;
+		wp-gpios = <&gpio TEGRA_GPIO(Q, 4) GPIO_ACTIVE_HIGH>;
 		bus-width = <4>;
 		status = "okay";
 	};
@@ -1231,8 +1233,6 @@
 			regulator-name = "vdd_hdmi_5v0";
 			regulator-min-microvolt = <5000000>;
 			regulator-max-microvolt = <5000000>;
-			enable-active-high;
-			gpio = <&gpio TEGRA_GPIO(K, 1) GPIO_ACTIVE_HIGH>;
 			vin-supply = <&tps65090_dcdc1_reg>;
 		};
 
@@ -1245,6 +1245,17 @@
 			enable-active-high;
 			gpio = <&palmas_gpio 6 0>;
 		};
+
+		vdd_5v0_hdmi: regulator@7 {
+			compatible = "regulator-fixed";
+			reg = <7>;
+			regulator-name = "VDD_5V0_HDMI_CON";
+			regulator-min-microvolt = <5000000>;
+			regulator-max-microvolt = <5000000>;
+			gpio = <&gpio TEGRA_GPIO(K, 1) GPIO_ACTIVE_HIGH>;
+			enable-active-high;
+			vin-supply = <&tps65090_dcdc1_reg>;
+		};
 	};
 
 	sound {
diff --git a/arch/arm/boot/dts/tegra114-roth.dts b/arch/arm/boot/dts/tegra114-roth.dts
new file mode 100644
index 00000000000..0b0e8e07d96
--- /dev/null
+++ b/arch/arm/boot/dts/tegra114-roth.dts
@@ -0,0 +1,1113 @@
+/dts-v1/;
+
+#include <dt-bindings/input/input.h>
+#include "tegra114.dtsi"
+
+/ {
+	model = "NVIDIA SHIELD";
+	compatible = "nvidia,roth", "nvidia,tegra114";
+
+	chosen {
+		/* SHIELD's bootloader's arguments need to be overridden */
+		bootargs = "console=ttyS0,115200n8 console=tty1 gpt fbcon=rotate:1";
+		/* SHIELD's bootloader will place initrd at this address */
+		linux,initrd-start = <0x82000000>;
+		linux,initrd-end = <0x82800000>;
+	};
+
+	firmware {
+		trusted-foundations {
+			compatible = "tlm,trusted-foundations";
+			tlm,version-major = <2>;
+			tlm,version-minor = <8>;
+		};
+	};
+
+	memory {
+		/* memory >= 0x79600000 is reserved for firmware usage */
+		reg = <0x80000000 0x79600000>;
+	};
+
+	pinmux@70000868 {
+		pinctrl-names = "default";
+		pinctrl-0 = <&state_default>;
+
+		state_default: pinmux {
+			clk1_out_pw4 {
+				nvidia,pins = "clk1_out_pw4";
+				nvidia,function = "extperiph1";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			dap1_din_pn1 {
+				nvidia,pins = "dap1_din_pn1";
+				nvidia,function = "i2s0";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_ENABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			dap1_dout_pn2 {
+				nvidia,pins = "dap1_dout_pn2",
+						"dap1_fs_pn0",
+						"dap1_sclk_pn3";
+				nvidia,function = "i2s0";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			dap2_din_pa4 {
+				nvidia,pins = "dap2_din_pa4";
+				nvidia,function = "i2s1";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_ENABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			dap2_dout_pa5 {
+				nvidia,pins = "dap2_dout_pa5",
+						"dap2_fs_pa2",
+						"dap2_sclk_pa3";
+				nvidia,function = "i2s1";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			dap4_din_pp5 {
+				nvidia,pins = "dap4_din_pp5",
+						"dap4_dout_pp6",
+						"dap4_fs_pp4",
+						"dap4_sclk_pp7";
+				nvidia,function = "i2s3";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			dvfs_pwm_px0 {
+				nvidia,pins = "dvfs_pwm_px0",
+						"dvfs_clk_px2";
+				nvidia,function = "cldvfs";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			ulpi_clk_py0 {
+				nvidia,pins = "ulpi_clk_py0",
+						"ulpi_data0_po1",
+						"ulpi_data1_po2",
+						"ulpi_data2_po3",
+						"ulpi_data3_po4",
+						"ulpi_data4_po5",
+						"ulpi_data5_po6",
+						"ulpi_data6_po7",
+						"ulpi_data7_po0";
+				nvidia,function = "ulpi";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			ulpi_dir_py1 {
+				nvidia,pins = "ulpi_dir_py1",
+						"ulpi_nxt_py2";
+				nvidia,function = "ulpi";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_ENABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			ulpi_stp_py3 {
+				nvidia,pins = "ulpi_stp_py3";
+				nvidia,function = "ulpi";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			cam_i2c_scl_pbb1 {
+				nvidia,pins = "cam_i2c_scl_pbb1",
+						"cam_i2c_sda_pbb2";
+				nvidia,function = "i2c3";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+				nvidia,lock = <TEGRA_PIN_DISABLE>;
+				nvidia,open-drain = <TEGRA_PIN_DISABLE>;
+			};
+			cam_mclk_pcc0 {
+				nvidia,pins = "cam_mclk_pcc0",
+						"pbb0";
+				nvidia,function = "vi_alt3";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+				nvidia,lock = <TEGRA_PIN_DISABLE>;
+			};
+			pbb4 {
+				nvidia,pins = "pbb4";
+				nvidia,function = "vgp4";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+				nvidia,lock = <TEGRA_PIN_DISABLE>;
+			};
+			gen2_i2c_scl_pt5 {
+				nvidia,pins = "gen2_i2c_scl_pt5",
+						"gen2_i2c_sda_pt6";
+				nvidia,function = "i2c2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+				nvidia,lock = <TEGRA_PIN_DISABLE>;
+				nvidia,open-drain = <TEGRA_PIN_DISABLE>;
+			};
+			gmi_a16_pj7 {
+				nvidia,pins = "gmi_a16_pj7",
+						"gmi_a19_pk7";
+				nvidia,function = "uartd";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			gmi_a17_pb0 {
+				nvidia,pins = "gmi_a17_pb0",
+						"gmi_a18_pb1";
+				nvidia,function = "uartd";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_ENABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			gmi_ad5_pg5 {
+				nvidia,pins = "gmi_ad5_pg5",
+						"gmi_wr_n_pi0";
+				nvidia,function = "spi4";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			gmi_ad6_pg6 {
+				nvidia,pins = "gmi_ad6_pg6",
+						"gmi_ad7_pg7";
+				nvidia,function = "spi4";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			gmi_ad12_ph4 {
+				nvidia,pins = "gmi_ad12_ph4";
+				nvidia,function = "rsvd4";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			gmi_cs6_n_pi13 {
+				nvidia,pins = "gmi_cs6_n_pi3";
+				nvidia,function = "nand";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_ENABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			gmi_ad9_ph1 {
+				nvidia,pins = "gmi_ad9_ph1";
+				nvidia,function = "pwm1";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			gmi_cs1_n_pj2 {
+				nvidia,pins = "gmi_cs1_n_pj2",
+						"gmi_oe_n_pi1";
+				nvidia,function = "soc";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_ENABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			gmi_rst_n_pi4 {
+				nvidia,pins = "gmi_rst_n_pi4";
+				nvidia,function = "gmi";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			gmi_iordy_pi5 {
+				nvidia,pins = "gmi_iordy_pi5";
+				nvidia,function = "gmi";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_ENABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			clk2_out_pw5 {
+				nvidia,pins = "clk2_out_pw5";
+				nvidia,function = "extperiph2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			sdmmc1_clk_pz0 {
+				nvidia,pins = "sdmmc1_clk_pz0";
+				nvidia,function = "sdmmc1";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			sdmmc1_cmd_pz1 {
+				nvidia,pins = "sdmmc1_cmd_pz1",
+						"sdmmc1_dat0_py7",
+						"sdmmc1_dat1_py6",
+						"sdmmc1_dat2_py5",
+						"sdmmc1_dat3_py4";
+				nvidia,function = "sdmmc1";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			sdmmc3_clk_pa6 {
+				nvidia,pins = "sdmmc3_clk_pa6";
+				nvidia,function = "sdmmc3";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			sdmmc3_cmd_pa7 {
+				nvidia,pins = "sdmmc3_cmd_pa7",
+						"sdmmc3_dat0_pb7",
+						"sdmmc3_dat1_pb6",
+						"sdmmc3_dat2_pb5",
+						"sdmmc3_dat3_pb4",
+						"sdmmc3_cd_n_pv2",
+						"sdmmc3_clk_lb_out_pee4",
+						"sdmmc3_clk_lb_in_pee5";
+				nvidia,function = "sdmmc3";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			kb_col4_pq4 {
+				nvidia,pins = "kb_col4_pq4";
+				nvidia,function = "sdmmc3";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_ENABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			sdmmc4_clk_pcc4 {
+				nvidia,pins = "sdmmc4_clk_pcc4";
+				nvidia,function = "sdmmc4";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			sdmmc4_cmd_pt7 {
+				nvidia,pins = "sdmmc4_cmd_pt7",
+						"sdmmc4_dat0_paa0",
+						"sdmmc4_dat1_paa1",
+						"sdmmc4_dat2_paa2",
+						"sdmmc4_dat3_paa3",
+						"sdmmc4_dat4_paa4",
+						"sdmmc4_dat5_paa5",
+						"sdmmc4_dat6_paa6",
+						"sdmmc4_dat7_paa7";
+				nvidia,function = "sdmmc4";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			clk_32k_out_pa0 {
+				nvidia,pins = "clk_32k_out_pa0";
+				nvidia,function = "blink";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			kb_col0_pq0 {
+				nvidia,pins = "kb_col0_pq0",
+						"kb_col1_pq1",
+						"kb_col2_pq2",
+						"kb_row0_pr0",
+						"kb_row1_pr1",
+						"kb_row2_pr2",
+						"kb_row8_ps0";
+				nvidia,function = "kbc";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			kb_row7_pr7 {
+				nvidia,pins = "kb_row7_pr7";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			kb_row10_ps2 {
+				nvidia,pins = "kb_row10_ps2";
+				nvidia,function = "uarta";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_ENABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			kb_row9_ps1 {
+				nvidia,pins = "kb_row9_ps1";
+				nvidia,function = "uarta";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pwr_i2c_scl_pz6 {
+				nvidia,pins = "pwr_i2c_scl_pz6",
+						"pwr_i2c_sda_pz7";
+				nvidia,function = "i2cpwr";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+				nvidia,lock = <TEGRA_PIN_DISABLE>;
+				nvidia,open-drain = <TEGRA_PIN_DISABLE>;
+			};
+			sys_clk_req_pz5 {
+				nvidia,pins = "sys_clk_req_pz5";
+				nvidia,function = "sysclk";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			core_pwr_req {
+				nvidia,pins = "core_pwr_req";
+				nvidia,function = "pwron";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			cpu_pwr_req {
+				nvidia,pins = "cpu_pwr_req";
+				nvidia,function = "cpu";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pwr_int_n {
+				nvidia,pins = "pwr_int_n";
+				nvidia,function = "pmi";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_ENABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			reset_out_n {
+				nvidia,pins = "reset_out_n";
+				nvidia,function = "reset_out_n";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			clk3_out_pee0 {
+				nvidia,pins = "clk3_out_pee0";
+				nvidia,function = "extperiph3";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			gen1_i2c_scl_pc4 {
+				nvidia,pins = "gen1_i2c_scl_pc4",
+						"gen1_i2c_sda_pc5";
+				nvidia,function = "i2c1";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+				nvidia,lock = <TEGRA_PIN_DISABLE>;
+				nvidia,open-drain = <TEGRA_PIN_DISABLE>;
+			};
+			uart2_cts_n_pj5 {
+				nvidia,pins = "uart2_cts_n_pj5";
+				nvidia,function = "uartb";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_ENABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			uart2_rts_n_pj6 {
+				nvidia,pins = "uart2_rts_n_pj6";
+				nvidia,function = "uartb";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			uart2_rxd_pc3 {
+				nvidia,pins = "uart2_rxd_pc3";
+				nvidia,function = "irda";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_ENABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			uart2_txd_pc2 {
+				nvidia,pins = "uart2_txd_pc2";
+				nvidia,function = "irda";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			uart3_cts_n_pa1 {
+				nvidia,pins = "uart3_cts_n_pa1",
+						"uart3_rxd_pw7";
+				nvidia,function = "uartc";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_ENABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			uart3_rts_n_pc0 {
+				nvidia,pins = "uart3_rts_n_pc0",
+						"uart3_txd_pw6";
+				nvidia,function = "uartc";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			owr {
+				nvidia,pins = "owr";
+				nvidia,function = "owr";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			hdmi_cec_pee3 {
+				nvidia,pins = "hdmi_cec_pee3";
+				nvidia,function = "cec";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+				nvidia,lock = <TEGRA_PIN_DISABLE>;
+				nvidia,open-drain = <TEGRA_PIN_DISABLE>;
+			};
+			ddc_scl_pv4 {
+				nvidia,pins = "ddc_scl_pv4",
+						"ddc_sda_pv5";
+				nvidia,function = "i2c4";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+				nvidia,lock = <TEGRA_PIN_DISABLE>;
+				nvidia,rcv-sel = <TEGRA_PIN_ENABLE>;
+			};
+			spdif_in_pk6 {
+				nvidia,pins = "spdif_in_pk6";
+				nvidia,function = "usb";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+				nvidia,lock = <TEGRA_PIN_DISABLE>;
+			};
+			usb_vbus_en0_pn4 {
+				nvidia,pins = "usb_vbus_en0_pn4";
+				nvidia,function = "usb";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+				nvidia,lock = <TEGRA_PIN_DISABLE>;
+				nvidia,open-drain = <TEGRA_PIN_ENABLE>;
+			};
+			gpio_x6_aud_px6 {
+				nvidia,pins = "gpio_x6_aud_px6";
+				nvidia,function = "spi6";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_ENABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			gpio_x1_aud_px1 {
+				nvidia,pins = "gpio_x1_aud_px1";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			gpio_x7_aud_px7 {
+				nvidia,pins = "gpio_x7_aud_px7";
+				nvidia,function = "rsvd1";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			gmi_adv_n_pk0 {
+				nvidia,pins = "gmi_adv_n_pk0";
+				nvidia,function = "gmi";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_ENABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			gmi_cs0_n_pj0 {
+				nvidia,pins = "gmi_cs0_n_pj0";
+				nvidia,function = "gmi";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			pu3 {
+				nvidia,pins = "pu3";
+				nvidia,function = "pwm0";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			gpio_x4_aud_px4 {
+				nvidia,pins = "gpio_x4_aud_px4",
+						"gpio_x5_aud_px5";
+				nvidia,function = "rsvd1";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			gpio_x3_aud_px3 {
+				nvidia,pins = "gpio_x3_aud_px3";
+				nvidia,function = "rsvd4";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			gpio_w2_aud_pw2 {
+				nvidia,pins = "gpio_w2_aud_pw2";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			gpio_w3_aud_pw3 {
+				nvidia,pins = "gpio_w3_aud_pw3";
+				nvidia,function = "spi6";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			dap3_fs_pp0 {
+				nvidia,pins = "dap3_fs_pp0",
+						"dap3_din_pp1",
+						"dap3_dout_pp2",
+						"dap3_sclk_pp3";
+				nvidia,function = "i2s2";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pv0 {
+				nvidia,pins = "pv0";
+				nvidia,function = "rsvd4";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			pv1 {
+				nvidia,pins = "pv1";
+				nvidia,function = "rsvd1";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			pbb3 {
+				nvidia,pins = "pbb3",
+						"pbb5",
+						"pbb6",
+						"pbb7";
+				nvidia,function = "rsvd4";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pcc1 {
+				nvidia,pins = "pcc1",
+						"pcc2";
+				nvidia,function = "rsvd4";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			gmi_ad0_pg0 {
+				nvidia,pins = "gmi_ad0_pg0",
+						"gmi_ad1_pg1";
+				nvidia,function = "gmi";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			gmi_ad10_ph2 {
+				nvidia,pins = "gmi_ad10_ph2",
+						"gmi_ad12_ph4",
+						"gmi_ad15_ph7",
+						"gmi_cs3_n_pk4";
+				nvidia,function = "gmi";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			gmi_ad11_ph3 {
+				nvidia,pins = "gmi_ad11_ph3",
+						"gmi_ad13_ph5",
+						"gmi_ad8_ph0",
+						"gmi_clk_pk1",
+						"gmi_cs2_n_pk3";
+				nvidia,function = "gmi";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			gmi_ad14_ph6 {
+				nvidia,pins = "gmi_ad14_ph6",
+						"gmi_cs0_n_pj0",
+						"gmi_cs4_n_pk2",
+						"gmi_cs7_n_pi6",
+						"gmi_dqs_p_pj3",
+						"gmi_wp_n_pc7";
+				nvidia,function = "gmi";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			gmi_ad2_pg2 {
+				nvidia,pins = "gmi_ad2_pg2",
+						"gmi_ad3_pg3";
+				nvidia,function = "gmi";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			sdmmc1_wp_n_pv3 {
+				nvidia,pins = "sdmmc1_wp_n_pv3";
+				nvidia,function = "spi4";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			clk2_req_pcc5 {
+				nvidia,pins = "clk2_req_pcc5";
+				nvidia,function = "rsvd4";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			kb_col3_pq3 {
+				nvidia,pins = "kb_col3_pq3";
+				nvidia,function = "pwm2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			kb_col5_pq5 {
+				nvidia,pins = "kb_col5_pq5";
+				nvidia,function = "kbc";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			kb_col6_pq6 {
+				nvidia,pins = "kb_col6_pq6",
+						"kb_col7_pq7";
+				nvidia,function = "kbc";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			kb_row3_pr3 {
+				nvidia,pins = "kb_row3_pr3",
+						"kb_row4_pr4",
+						"kb_row6_pr6";
+				nvidia,function = "kbc";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			clk3_req_pee1 {
+				nvidia,pins = "clk3_req_pee1";
+				nvidia,function = "rsvd4";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pu2 {
+				nvidia,pins = "pu2";
+				nvidia,function = "rsvd1";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			hdmi_int_pn7 {
+				nvidia,pins = "hdmi_int_pn7";
+				nvidia,function = "rsvd1";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+
+			drive_sdio1 {
+				nvidia,pins = "drive_sdio1";
+				nvidia,high-speed-mode = <TEGRA_PIN_ENABLE>;
+				nvidia,schmitt = <TEGRA_PIN_DISABLE>;
+				nvidia,low-power-mode = <TEGRA_PIN_LP_DRIVE_DIV_1>;
+				nvidia,pull-down-strength = <36>;
+				nvidia,pull-up-strength = <20>;
+				nvidia,slew-rate-rising = <TEGRA_PIN_SLEW_RATE_SLOW>;
+				nvidia,slew-rate-falling = <TEGRA_PIN_SLEW_RATE_SLOW>;
+			};
+			drive_sdio3 {
+				nvidia,pins = "drive_sdio3";
+				nvidia,high-speed-mode = <TEGRA_PIN_ENABLE>;
+				nvidia,schmitt = <TEGRA_PIN_DISABLE>;
+				nvidia,low-power-mode = <TEGRA_PIN_LP_DRIVE_DIV_1>;
+				nvidia,pull-down-strength = <36>;
+				nvidia,pull-up-strength = <20>;
+				nvidia,slew-rate-rising = <TEGRA_PIN_SLEW_RATE_FASTEST>;
+				nvidia,slew-rate-falling = <TEGRA_PIN_SLEW_RATE_FASTEST>;
+			};
+			drive_gma {
+				nvidia,pins = "drive_gma";
+				nvidia,high-speed-mode = <TEGRA_PIN_ENABLE>;
+				nvidia,schmitt = <TEGRA_PIN_DISABLE>;
+				nvidia,low-power-mode = <TEGRA_PIN_LP_DRIVE_DIV_1>;
+				nvidia,pull-down-strength = <2>;
+				nvidia,pull-up-strength = <2>;
+				nvidia,slew-rate-rising = <TEGRA_PIN_SLEW_RATE_FASTEST>;
+				nvidia,slew-rate-falling = <TEGRA_PIN_SLEW_RATE_FASTEST>;
+				nvidia,drive-type = <1>;
+			};
+		};
+	};
+
+	/* Usable on reworked devices only */
+	serial@70006300 {
+		status = "okay";
+	};
+
+	pwm@7000a000 {
+		status = "okay";
+	};
+
+	i2c@7000d000 {
+		status = "okay";
+		clock-frequency = <400000>;
+
+		regulator@43 {
+			compatible = "ti,tps51632";
+			reg = <0x43>;
+			regulator-name = "vdd-cpu";
+			regulator-min-microvolt = <500000>;
+			regulator-max-microvolt = <1520000>;
+			regulator-always-on;
+			regulator-boot-on;
+		};
+
+		palmas: pmic@58 {
+			compatible = "ti,palmas";
+			reg = <0x58>;
+			interrupts = <GIC_SPI 86 IRQ_TYPE_LEVEL_LOW>;
+
+			#interrupt-cells = <2>;
+			interrupt-controller;
+
+			ti,system-power-controller;
+
+			palmas_gpio: gpio {
+				compatible = "ti,palmas-gpio";
+				gpio-controller;
+				#gpio-cells = <2>;
+			};
+
+			pmic {
+				compatible = "ti,tps65913-pmic", "ti,palmas-pmic";
+
+				regulators {
+					smps12 {
+						regulator-name = "vdd-ddr";
+						regulator-min-microvolt = <1200000>;
+						regulator-max-microvolt = <1500000>;
+						regulator-always-on;
+						regulator-boot-on;
+					};
+
+					vdd_1v8: smps3 {
+						regulator-name = "vdd-1v8";
+						regulator-min-microvolt = <1800000>;
+						regulator-max-microvolt = <1800000>;
+						regulator-always-on;
+						regulator-boot-on;
+					};
+
+					smps457 {
+						regulator-name = "vdd-soc";
+						regulator-min-microvolt = <900000>;
+						regulator-max-microvolt = <1400000>;
+						regulator-always-on;
+						regulator-boot-on;
+					};
+
+					smps8 {
+						regulator-name = "avdd-pll-1v05";
+						regulator-min-microvolt = <1050000>;
+						regulator-max-microvolt = <1050000>;
+						regulator-always-on;
+						regulator-boot-on;
+					};
+
+					smps9 {
+						regulator-name = "vdd-2v85-emmc";
+						regulator-min-microvolt = <2800000>;
+						regulator-max-microvolt = <2800000>;
+						regulator-always-on;
+					};
+
+					smps10_out1 {
+						regulator-name = "vdd-fan";
+						regulator-min-microvolt = <5000000>;
+						regulator-max-microvolt = <5000000>;
+						regulator-always-on;
+						regulator-boot-on;
+					};
+
+					smps10_out2 {
+						regulator-name = "vdd-5v0-sys";
+						regulator-min-microvolt = <5000000>;
+						regulator-max-microvolt = <5000000>;
+						regulator-always-on;
+						regulator-boot-on;
+					};
+
+					ldo2 {
+						regulator-name = "vdd-2v8-display";
+						regulator-min-microvolt = <2800000>;
+						regulator-max-microvolt = <2800000>;
+						regulator-boot-on;
+					};
+
+					ldo3 {
+						regulator-name = "avdd-1v2";
+						regulator-min-microvolt = <1200000>;
+						regulator-max-microvolt = <1200000>;
+						regulator-always-on;
+						regulator-boot-on;
+					};
+
+					ldo4 {
+						regulator-name = "vpp-fuse";
+						regulator-min-microvolt = <1800000>;
+						regulator-max-microvolt = <1800000>;
+					};
+
+					ldo5 {
+						regulator-name = "avdd-hdmi-pll";
+						regulator-min-microvolt = <1200000>;
+						regulator-max-microvolt = <1200000>;
+					};
+
+					ldo6 {
+						regulator-name = "vdd-sensor-2v8";
+						regulator-min-microvolt = <2850000>;
+						regulator-max-microvolt = <2850000>;
+					};
+
+					ldo8 {
+						regulator-name = "vdd-rtc";
+						regulator-min-microvolt = <1100000>;
+						regulator-max-microvolt = <1100000>;
+						regulator-always-on;
+						regulator-boot-on;
+						ti,enable-ldo8-tracking;
+					};
+
+					vddio_sdmmc3: ldo9 {
+						regulator-name = "vddio-sdmmc3";
+						regulator-min-microvolt = <1800000>;
+						regulator-max-microvolt = <3300000>;
+						regulator-always-on;
+						regulator-boot-on;
+					};
+
+					ldousb {
+						regulator-name = "avdd-usb-hdmi";
+						regulator-min-microvolt = <3300000>;
+						regulator-max-microvolt = <3300000>;
+						regulator-always-on;
+						regulator-boot-on;
+					};
+
+					vdd_3v3_sys: regen1 {
+						regulator-name = "rail-3v3";
+						regulator-max-microvolt = <3300000>;
+						regulator-always-on;
+						regulator-boot-on;
+					};
+
+					regen2 {
+						regulator-name = "rail-5v0";
+						regulator-max-microvolt = <5000000>;
+						regulator-always-on;
+						regulator-boot-on;
+					};
+
+				};
+			};
+
+			rtc {
+				compatible = "ti,palmas-rtc";
+				interrupt-parent = <&palmas>;
+				interrupts = <8 0>;
+			};
+
+		};
+	};
+
+	pmc@7000e400 {
+		nvidia,invert-interrupt;
+	};
+
+	/* SD card */
+	sdhci@78000400 {
+		status = "okay";
+		bus-width = <4>;
+		vmmc-supply = <&vddio_sdmmc3>;
+		cd-gpios = <&gpio TEGRA_GPIO(V, 2) GPIO_ACTIVE_LOW>;
+		power-gpios = <&gpio TEGRA_GPIO(H, 0) GPIO_ACTIVE_HIGH>;
+	};
+
+	/* eMMC */
+	sdhci@78000600 {
+		status = "okay";
+		bus-width = <8>;
+		vmmc-supply = <&vdd_1v8>;
+		non-removable;
+	};
+
+	/* External USB port (must be powered) */
+	usb@7d000000 {
+		status = "okay";
+	};
+
+	usb-phy@7d000000 {
+		status = "okay";
+		nvidia,xcvr-setup = <7>;
+		nvidia,xcvr-lsfslew = <2>;
+		nvidia,xcvr-lsrslew = <2>;
+		interrupts = <GIC_SPI 20 IRQ_TYPE_LEVEL_HIGH>;
+		/* Should be changed to "otg" once we have vbus_supply */
+		/* As of now, USB devices need to be powered externally */
+		dr_mode = "host";
+	};
+
+	/* SHIELD controller */
+	usb@7d008000 {
+		status = "okay";
+	};
+
+	usb-phy@7d008000 {
+		status = "okay";
+		nvidia,xcvr-setup = <7>;
+		nvidia,xcvr-lsfslew = <2>;
+		nvidia,xcvr-lsrslew = <2>;
+	};
+
+	backlight: backlight {
+		compatible = "pwm-backlight";
+		pwms = <&pwm 1 40000>;
+
+		brightness-levels = <0 4 8 16 32 64 128 255>;
+		default-brightness-level = <6>;
+
+		power-supply = <&lcd_bl_en>;
+		enable-gpios = <&gpio TEGRA_GPIO(H, 2) GPIO_ACTIVE_HIGH>;
+	};
+
+	clocks {
+		compatible = "simple-bus";
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		clk32k_in: clock {
+			compatible = "fixed-clock";
+			reg=<0>;
+			#clock-cells = <0>;
+			clock-frequency = <32768>;
+		};
+	};
+
+	gpio-keys {
+		compatible = "gpio-keys";
+
+		back {
+			label = "Back";
+			gpios = <&gpio TEGRA_GPIO(R, 2) GPIO_ACTIVE_LOW>;
+			linux,code = <KEY_BACK>;
+		};
+
+		home {
+			label = "Home";
+			gpios = <&gpio TEGRA_GPIO(R, 1) GPIO_ACTIVE_LOW>;
+			linux,code = <KEY_HOME>;
+		};
+
+		power {
+			label = "Power";
+			gpios = <&gpio TEGRA_GPIO(Q, 0) GPIO_ACTIVE_LOW>;
+			linux,code = <KEY_POWER>;
+			gpio-key,wakeup;
+		};
+	};
+
+	regulators {
+		compatible = "simple-bus";
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		lcd_bl_en: regulator@0 {
+			compatible = "regulator-fixed";
+			reg = <0>;
+			regulator-name = "lcd_bl_en";
+			regulator-min-microvolt = <5000000>;
+			regulator-max-microvolt = <5000000>;
+			regulator-boot-on;
+		};
+
+		regulator@1 {
+			compatible = "regulator-fixed";
+			reg = <1>;
+			regulator-name = "vdd_lcd_1v8";
+			regulator-min-microvolt = <1800000>;
+			regulator-max-microvolt = <1800000>;
+			vin-supply = <&vdd_1v8>;
+			enable-active-high;
+			gpio = <&gpio TEGRA_GPIO(U, 4) GPIO_ACTIVE_HIGH>;
+			regulator-boot-on;
+		};
+
+		regulator@2 {
+			compatible = "regulator-fixed";
+			reg = <2>;
+			regulator-name = "vdd_1v8_ts";
+			regulator-min-microvolt = <1800000>;
+			regulator-max-microvolt = <1800000>;
+			gpio = <&gpio TEGRA_GPIO(K, 3) GPIO_ACTIVE_LOW>;
+			regulator-boot-on;
+		};
+
+		regulator@3 {
+			compatible = "regulator-fixed";
+			reg = <3>;
+			regulator-name = "vdd_3v3_ts";
+			regulator-min-microvolt = <3300000>;
+			regulator-max-microvolt = <3300000>;
+			enable-active-high;
+			gpio = <&gpio TEGRA_GPIO(H, 5) GPIO_ACTIVE_HIGH>;
+			regulator-boot-on;
+		};
+
+		regulator@4 {
+			compatible = "regulator-fixed";
+			reg = <4>;
+			regulator-name = "vdd_1v8_com";
+			regulator-min-microvolt = <1800000>;
+			regulator-max-microvolt = <1800000>;
+			vin-supply = <&vdd_1v8>;
+			enable-active-high;
+			gpio = <&gpio TEGRA_GPIO(X, 1) GPIO_ACTIVE_HIGH>;
+			regulator-boot-on;
+		};
+
+		regulator@5 {
+			compatible = "regulator-fixed";
+			reg = <5>;
+			regulator-name = "vdd_3v3_com";
+			regulator-min-microvolt = <3300000>;
+			regulator-max-microvolt = <3300000>;
+			vin-supply = <&vdd_3v3_sys>;
+			enable-active-high;
+			gpio = <&gpio TEGRA_GPIO(X, 7) GPIO_ACTIVE_HIGH>;
+			regulator-always-on;
+			regulator-boot-on;
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/tegra114-tn7.dts b/arch/arm/boot/dts/tegra114-tn7.dts
new file mode 100644
index 00000000000..96366214563
--- /dev/null
+++ b/arch/arm/boot/dts/tegra114-tn7.dts
@@ -0,0 +1,348 @@
+/dts-v1/;
+
+#include <dt-bindings/input/input.h>
+#include "tegra114.dtsi"
+
+/ {
+	model = "Tegra Note 7";
+	compatible = "nvidia,tn7", "nvidia,tegra114";
+
+	chosen {
+		/* TN7's bootloader's arguments need to be overridden */
+		bootargs = "console=ttyS0,115200n8 console=tty1 gpt fbcon=rotate:2";
+		/* TN7's bootloader will place initrd at this address */
+		linux,initrd-start = <0x82000000>;
+		linux,initrd-end = <0x82800000>;
+	};
+
+	firmware {
+		trusted-foundations {
+			compatible = "tlm,trusted-foundations";
+			tlm,version-major = <2>;
+			tlm,version-minor = <8>;
+		};
+	};
+
+	memory {
+		/* memory >= 0x37e00000 is reserved for firmware usage */
+		reg = <0x80000000 0x37e00000>;
+	};
+
+	host1x@50000000 {
+		dsi@54300000 {
+			status = "okay";
+
+			vdd-supply = <&vdd_1v2_ap>;
+
+			panel@0 {
+				compatible = "lg,ld070wx3-sl01";
+				reg = <0>;
+
+				power-supply = <&vdd_lcd>;
+				backlight = <&backlight>;
+			};
+		};
+	};
+
+	serial@70006300 {
+		status = "okay";
+	};
+
+	pwm@7000a000 {
+		status = "okay";
+	};
+
+	i2c@7000d000 {
+		status = "okay";
+		clock-frequency = <400000>;
+
+		palmas: pmic@58 {
+			compatible = "ti,palmas";
+			reg = <0x58>;
+			interrupts = <GIC_SPI 86 IRQ_TYPE_LEVEL_LOW>;
+
+			#interrupt-cells = <2>;
+			interrupt-controller;
+
+			ti,system-power-controller;
+
+			palmas_gpio: gpio {
+				compatible = "ti,palmas-gpio";
+				gpio-controller;
+				#gpio-cells = <2>;
+			};
+
+			pmic {
+				compatible = "ti,tps65913-pmic", "ti,palmas-pmic";
+
+				ldoln-in-supply = <&vdd_smps10_out2>;
+
+				regulators {
+					smps123 {
+						regulator-name = "vd-cpu";
+						regulator-min-microvolt = <1000000>;
+						regulator-max-microvolt = <1000000>;
+						regulator-always-on;
+						regulator-boot-on;
+					};
+
+					smps45 {
+						regulator-name = "vd-soc";
+						regulator-min-microvolt = <1100000>;
+						regulator-max-microvolt = <1100000>;
+						regulator-always-on;
+						regulator-boot-on;
+					};
+
+					smps6 {
+						regulator-name = "va-lcd-hv";
+						regulator-min-microvolt = <3000000>;
+						regulator-max-microvolt = <3000000>;
+						regulator-always-on;
+						regulator-boot-on;
+					};
+
+					smps7 {
+						regulator-name = "vd-ddr";
+						regulator-min-microvolt = <1350000>;
+						regulator-max-microvolt = <1350000>;
+						regulator-always-on;
+						regulator-boot-on;
+					};
+
+					vdd_1v8: smps8 {
+						regulator-name = "vs-pmu-1v8";
+						regulator-min-microvolt = <1800000>;
+						regulator-max-microvolt = <1800000>;
+						regulator-always-on;
+						regulator-boot-on;
+					};
+
+					vdd_2v9_sys: smps9 {
+						regulator-name = "vs-sys-2v9";
+						regulator-min-microvolt = <2900000>;
+						regulator-max-microvolt = <2900000>;
+						regulator-always-on;
+						regulator-boot-on;
+					};
+
+					vdd_smps10_out1: smps10_out1 {
+						regulator-name = "vd-smps10-out1";
+						regulator-min-microvolt = <5000000>;
+						regulator-max-microvolt = <5000000>;
+						regulator-always-on;
+						regulator-boot-on;
+					};
+
+					vdd_smps10_out2: smps10_out2 {
+						regulator-name = "vd-smps10-out2";
+						regulator-min-microvolt = <5000000>;
+						regulator-max-microvolt = <5000000>;
+						regulator-always-on;
+						regulator-boot-on;
+					};
+
+					ldo1 {
+						regulator-name = "va-pllx";
+						regulator-min-microvolt = <1050000>;
+						regulator-max-microvolt = <1050000>;
+						regulator-always-on;
+						regulator-boot-on;
+					};
+
+					vdd_1v2_ap: ldo2 {
+						regulator-name = "va-ap-1v2";
+						regulator-min-microvolt = <1200000>;
+						regulator-max-microvolt = <1200000>;
+						regulator-always-on;
+						regulator-boot-on;
+					};
+
+					ldo3 {
+						regulator-name = "vd-fuse";
+						regulator-min-microvolt = <1800000>;
+						regulator-max-microvolt = <1800000>;
+						regulator-always-on;
+						regulator-boot-on;
+					};
+
+					ldo4 {
+						regulator-name = "vd-ts-hv";
+						regulator-min-microvolt = <3200000>;
+						regulator-max-microvolt = <3200000>;
+						regulator-always-on;
+						regulator-boot-on;
+					};
+
+					ldo5 {
+						regulator-name = "va-cam2-hv";
+						regulator-min-microvolt = <2700000>;
+						regulator-max-microvolt = <2700000>;
+					};
+
+					ldo6 {
+						regulator-name = "va-sns-hv";
+						regulator-min-microvolt = <2850000>;
+						regulator-max-microvolt = <2850000>;
+					};
+
+					ldo7 {
+						regulator-name = "va-cam1-hv";
+						regulator-min-microvolt = <2700000>;
+						regulator-max-microvolt = <2700000>;
+					};
+
+					ldo8 {
+						regulator-name = "va-ap-rtc";
+						regulator-min-microvolt = <1100000>;
+						regulator-max-microvolt = <1100000>;
+						ti,enable-ldo8-tracking;
+						regulator-always-on;
+						regulator-boot-on;
+					};
+
+					ldo9 {
+						regulator-name = "vi-sdcard";
+						regulator-min-microvolt = <2900000>;
+						regulator-max-microvolt = <2900000>;
+					};
+
+					ldousb {
+						regulator-name = "avdd-usb";
+						regulator-min-microvolt = <3300000>;
+						regulator-max-microvolt = <3300000>;
+						regulator-always-on;
+						regulator-boot-on;
+					};
+
+					ldoln {
+						regulator-name = "va-hdmi";
+						regulator-min-microvolt = <3300000>;
+						regulator-max-microvolt = <3300000>;
+					};
+				};
+			};
+
+			rtc {
+				compatible = "ti,palmas-rtc";
+				interrupt-parent = <&palmas>;
+				interrupts = <8 0>;
+			};
+
+		};
+	};
+
+	pmc@7000e400 {
+		nvidia,invert-interrupt;
+	};
+
+	/* eMMC */
+	sdhci@78000600 {
+		status = "okay";
+		bus-width = <8>;
+		vmmc-supply = <&vdd_1v8>;
+		non-removable;
+	};
+
+	usb@7d000000 {
+		status = "okay";
+	};
+
+	usb-phy@7d000000 {
+		status = "okay";
+		nvidia,xcvr-setup = <7>;
+		nvidia,xcvr-lsfslew = <2>;
+		nvidia,xcvr-lsrslew = <2>;
+		interrupts = <GIC_SPI 20 IRQ_TYPE_LEVEL_HIGH>;
+		/* Should be changed to "otg" once we have vbus_supply */
+		/* As of now, USB devices need to be powered externally */
+		dr_mode = "host";
+	};
+
+	backlight: backlight {
+		compatible = "pwm-backlight";
+		pwms = <&pwm 1 40000>;
+
+		brightness-levels = <0 4 8 16 32 64 128 255>;
+		default-brightness-level = <6>;
+
+		power-supply = <&lcd_bl_en>;
+	};
+
+	clocks {
+		compatible = "simple-bus";
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		clk32k_in: clock {
+			compatible = "fixed-clock";
+			reg = <0>;
+			#clock-cells = <0>;
+			clock-frequency = <32768>;
+		};
+	};
+
+	gpio-keys {
+		compatible = "gpio-keys";
+
+		power {
+			label = "Power";
+			gpios = <&gpio TEGRA_GPIO(Q, 0) GPIO_ACTIVE_LOW>;
+			linux,code = <KEY_POWER>;
+			gpio-key,wakeup;
+		};
+
+		volume_down {
+			label = "Volume Down";
+			gpios = <&gpio TEGRA_GPIO(Q, 2) GPIO_ACTIVE_LOW>;
+			linux,code = <KEY_VOLUMEDOWN>;
+		};
+
+		volume_up {
+			label = "Volume Up";
+			gpios = <&gpio TEGRA_GPIO(R, 2) GPIO_ACTIVE_LOW>;
+			linux,code = <KEY_VOLUMEUP>;
+		};
+	};
+
+	regulators {
+		compatible = "simple-bus";
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		/* FIXME: output of BQ24192 */
+		vs_sys: regulator@0 {
+			compatible = "regulator-fixed";
+			reg = <0>;
+			regulator-name = "VS_SYS";
+			regulator-min-microvolt = <4200000>;
+			regulator-max-microvolt = <4200000>;
+			regulator-always-on;
+			regulator-boot-on;
+		};
+
+		lcd_bl_en: regulator@1 {
+			compatible = "regulator-fixed";
+			reg = <1>;
+			regulator-name = "VDD_LCD_BL";
+			regulator-min-microvolt = <16500000>;
+			regulator-max-microvolt = <16500000>;
+			gpio = <&gpio TEGRA_GPIO(H, 2) GPIO_ACTIVE_HIGH>;
+			enable-active-high;
+			vin-supply = <&vs_sys>;
+			regulator-boot-on;
+		};
+
+		vdd_lcd: regulator@2 {
+			compatible = "regulator-fixed";
+			reg = <2>;
+			regulator-name = "VD_LCD_1V8";
+			regulator-min-microvolt = <1800000>;
+			regulator-max-microvolt = <1800000>;
+			gpio = <&palmas_gpio 4 GPIO_ACTIVE_HIGH>;
+			enable-active-high;
+			vin-supply = <&vdd_1v8>;
+			regulator-boot-on;
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/tegra124-jetson-tk1.dts b/arch/arm/boot/dts/tegra124-jetson-tk1.dts
new file mode 100644
index 00000000000..e31fb61a81d
--- /dev/null
+++ b/arch/arm/boot/dts/tegra124-jetson-tk1.dts
@@ -0,0 +1,1827 @@
+/dts-v1/;
+
+#include <dt-bindings/input/input.h>
+#include "tegra124.dtsi"
+
+/ {
+	model = "NVIDIA Tegra124 Jetson TK1";
+	compatible = "nvidia,jetson-tk1", "nvidia,tegra124";
+
+	aliases {
+		rtc0 = "/i2c@0,7000d000/pmic@40";
+		rtc1 = "/rtc@0,7000e000";
+	};
+
+	memory {
+		reg = <0x0 0x80000000 0x0 0x80000000>;
+	};
+
+	host1x@0,50000000 {
+		hdmi@0,54280000 {
+			status = "okay";
+
+			hdmi-supply = <&vdd_5v0_hdmi>;
+			pll-supply = <&vdd_hdmi_pll>;
+			vdd-supply = <&vdd_3v3_hdmi>;
+
+			nvidia,ddc-i2c-bus = <&hdmi_ddc>;
+			nvidia,hpd-gpio =
+				<&gpio TEGRA_GPIO(N, 7) GPIO_ACTIVE_HIGH>;
+		};
+	};
+
+	pinmux: pinmux@0,70000868 {
+		pinctrl-names = "default";
+		pinctrl-0 = <&state_default>;
+
+		state_default: pinmux {
+			clk_32k_out_pa0 {
+				nvidia,pins = "clk_32k_out_pa0";
+				nvidia,function = "soc";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			uart3_cts_n_pa1 {
+				nvidia,pins = "uart3_cts_n_pa1";
+				nvidia,function = "uartc";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			dap2_fs_pa2 {
+				nvidia,pins = "dap2_fs_pa2";
+				nvidia,function = "i2s1";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			dap2_sclk_pa3 {
+				nvidia,pins = "dap2_sclk_pa3";
+				nvidia,function = "i2s1";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			dap2_din_pa4 {
+				nvidia,pins = "dap2_din_pa4";
+				nvidia,function = "i2s1";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			dap2_dout_pa5 {
+				nvidia,pins = "dap2_dout_pa5";
+				nvidia,function = "i2s1";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			sdmmc3_clk_pa6 {
+				nvidia,pins = "sdmmc3_clk_pa6";
+				nvidia,function = "sdmmc3";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			sdmmc3_cmd_pa7 {
+				nvidia,pins = "sdmmc3_cmd_pa7";
+				nvidia,function = "sdmmc3";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			pb0 {
+				nvidia,pins = "pb0";
+				nvidia,function = "uartd";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			pb1 {
+				nvidia,pins = "pb1";
+				nvidia,function = "uartd";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			sdmmc3_dat3_pb4 {
+				nvidia,pins = "sdmmc3_dat3_pb4";
+				nvidia,function = "sdmmc3";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			sdmmc3_dat2_pb5 {
+				nvidia,pins = "sdmmc3_dat2_pb5";
+				nvidia,function = "sdmmc3";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			sdmmc3_dat1_pb6 {
+				nvidia,pins = "sdmmc3_dat1_pb6";
+				nvidia,function = "sdmmc3";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			sdmmc3_dat0_pb7 {
+				nvidia,pins = "sdmmc3_dat0_pb7";
+				nvidia,function = "sdmmc3";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			uart3_rts_n_pc0 {
+				nvidia,pins = "uart3_rts_n_pc0";
+				nvidia,function = "uartc";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			uart2_txd_pc2 {
+				nvidia,pins = "uart2_txd_pc2";
+				nvidia,function = "irda";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			uart2_rxd_pc3 {
+				nvidia,pins = "uart2_rxd_pc3";
+				nvidia,function = "irda";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			gen1_i2c_scl_pc4 {
+				nvidia,pins = "gen1_i2c_scl_pc4";
+				nvidia,function = "i2c1";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+				nvidia,open-drain = <TEGRA_PIN_ENABLE>;
+			};
+			gen1_i2c_sda_pc5 {
+				nvidia,pins = "gen1_i2c_sda_pc5";
+				nvidia,function = "i2c1";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+				nvidia,open-drain = <TEGRA_PIN_ENABLE>;
+			};
+			pc7 {
+				nvidia,pins = "pc7";
+				nvidia,function = "rsvd1";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			pg0 {
+				nvidia,pins = "pg0";
+				nvidia,function = "rsvd1";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pg1 {
+				nvidia,pins = "pg1";
+				nvidia,function = "rsvd1";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pg2 {
+				nvidia,pins = "pg2";
+				nvidia,function = "rsvd1";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			pg3 {
+				nvidia,pins = "pg3";
+				nvidia,function = "rsvd1";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			pg4 {
+				nvidia,pins = "pg4";
+				nvidia,function = "spi4";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pg5 {
+				nvidia,pins = "pg5";
+				nvidia,function = "spi4";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pg6 {
+				nvidia,pins = "pg6";
+				nvidia,function = "spi4";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pg7 {
+				nvidia,pins = "pg7";
+				nvidia,function = "spi4";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			ph0 {
+				nvidia,pins = "ph0";
+				nvidia,function = "gmi";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_ENABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			ph1 {
+				nvidia,pins = "ph1";
+				nvidia,function = "pwm1";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			ph2 {
+				nvidia,pins = "ph2";
+				nvidia,function = "gmi";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			ph3 {
+				nvidia,pins = "ph3";
+				nvidia,function = "gmi";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			ph4 {
+				nvidia,pins = "ph4";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			ph5 {
+				nvidia,pins = "ph5";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			ph6 {
+				nvidia,pins = "ph6";
+				nvidia,function = "gmi";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			ph7 {
+				nvidia,pins = "ph7";
+				nvidia,function = "gmi";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pi0 {
+				nvidia,pins = "pi0";
+				nvidia,function = "rsvd1";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pi1 {
+				nvidia,pins = "pi1";
+				nvidia,function = "rsvd1";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_ENABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pi2 {
+				nvidia,pins = "pi2";
+				nvidia,function = "rsvd4";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pi3 {
+				nvidia,pins = "pi3";
+				nvidia,function = "spi4";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pi4 {
+				nvidia,pins = "pi4";
+				nvidia,function = "gmi";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pi5 {
+				nvidia,pins = "pi5";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			pi6 {
+				nvidia,pins = "pi6";
+				nvidia,function = "rsvd1";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			pi7 {
+				nvidia,pins = "pi7";
+				nvidia,function = "rsvd1";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_ENABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pj0 {
+				nvidia,pins = "pj0";
+				nvidia,function = "rsvd1";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			pj2 {
+				nvidia,pins = "pj2";
+				nvidia,function = "rsvd1";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			uart2_cts_n_pj5 {
+				nvidia,pins = "uart2_cts_n_pj5";
+				nvidia,function = "uartb";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			uart2_rts_n_pj6 {
+				nvidia,pins = "uart2_rts_n_pj6";
+				nvidia,function = "uartb";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pj7 {
+				nvidia,pins = "pj7";
+				nvidia,function = "uartd";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pk0 {
+				nvidia,pins = "pk0";
+				nvidia,function = "soc";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			pk1 {
+				nvidia,pins = "pk1";
+				nvidia,function = "rsvd4";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pk2 {
+				nvidia,pins = "pk2";
+				nvidia,function = "rsvd1";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			pk3 {
+				nvidia,pins = "pk3";
+				nvidia,function = "gmi";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			pk4 {
+				nvidia,pins = "pk4";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			spdif_out_pk5 {
+				nvidia,pins = "spdif_out_pk5";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			spdif_in_pk6 {
+				nvidia,pins = "spdif_in_pk6";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pk7 {
+				nvidia,pins = "pk7";
+				nvidia,function = "uartd";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			dap1_fs_pn0 {
+				nvidia,pins = "dap1_fs_pn0";
+				nvidia,function = "i2s0";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			dap1_din_pn1 {
+				nvidia,pins = "dap1_din_pn1";
+				nvidia,function = "i2s0";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			dap1_dout_pn2 {
+				nvidia,pins = "dap1_dout_pn2";
+				nvidia,function = "sata";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			dap1_sclk_pn3 {
+				nvidia,pins = "dap1_sclk_pn3";
+				nvidia,function = "i2s0";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			usb_vbus_en0_pn4 {
+				nvidia,pins = "usb_vbus_en0_pn4";
+				nvidia,function = "usb";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+				nvidia,open-drain = <TEGRA_PIN_ENABLE>;
+			};
+			usb_vbus_en1_pn5 {
+				nvidia,pins = "usb_vbus_en1_pn5";
+				nvidia,function = "usb";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+				nvidia,open-drain = <TEGRA_PIN_ENABLE>;
+			};
+			hdmi_int_pn7 {
+				nvidia,pins = "hdmi_int_pn7";
+				nvidia,function = "rsvd1";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+				nvidia,rcv-sel = <TEGRA_PIN_DISABLE>;
+			};
+			ulpi_data7_po0 {
+				nvidia,pins = "ulpi_data7_po0";
+				nvidia,function = "ulpi";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			ulpi_data0_po1 {
+				nvidia,pins = "ulpi_data0_po1";
+				nvidia,function = "ulpi";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			ulpi_data1_po2 {
+				nvidia,pins = "ulpi_data1_po2";
+				nvidia,function = "ulpi";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			ulpi_data2_po3 {
+				nvidia,pins = "ulpi_data2_po3";
+				nvidia,function = "ulpi";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			ulpi_data3_po4 {
+				nvidia,pins = "ulpi_data3_po4";
+				nvidia,function = "ulpi";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			ulpi_data4_po5 {
+				nvidia,pins = "ulpi_data4_po5";
+				nvidia,function = "ulpi";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			ulpi_data5_po6 {
+				nvidia,pins = "ulpi_data5_po6";
+				nvidia,function = "ulpi";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			ulpi_data6_po7 {
+				nvidia,pins = "ulpi_data6_po7";
+				nvidia,function = "ulpi";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			dap3_fs_pp0 {
+				nvidia,pins = "dap3_fs_pp0";
+				nvidia,function = "i2s2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			dap3_din_pp1 {
+				nvidia,pins = "dap3_din_pp1";
+				nvidia,function = "i2s2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			dap3_dout_pp2 {
+				nvidia,pins = "dap3_dout_pp2";
+				nvidia,function = "rsvd4";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			dap3_sclk_pp3 {
+				nvidia,pins = "dap3_sclk_pp3";
+				nvidia,function = "rsvd3";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_ENABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			dap4_fs_pp4 {
+				nvidia,pins = "dap4_fs_pp4";
+				nvidia,function = "i2s3";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			dap4_din_pp5 {
+				nvidia,pins = "dap4_din_pp5";
+				nvidia,function = "i2s3";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			dap4_dout_pp6 {
+				nvidia,pins = "dap4_dout_pp6";
+				nvidia,function = "i2s3";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			dap4_sclk_pp7 {
+				nvidia,pins = "dap4_sclk_pp7";
+				nvidia,function = "i2s3";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			kb_col0_pq0 {
+				nvidia,pins = "kb_col0_pq0";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			kb_col1_pq1 {
+				nvidia,pins = "kb_col1_pq1";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			kb_col2_pq2 {
+				nvidia,pins = "kb_col2_pq2";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			kb_col3_pq3 {
+				nvidia,pins = "kb_col3_pq3";
+				nvidia,function = "kbc";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_ENABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			kb_col4_pq4 {
+				nvidia,pins = "kb_col4_pq4";
+				nvidia,function = "sdmmc3";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			kb_col5_pq5 {
+				nvidia,pins = "kb_col5_pq5";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			kb_col6_pq6 {
+				nvidia,pins = "kb_col6_pq6";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			kb_col7_pq7 {
+				nvidia,pins = "kb_col7_pq7";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			kb_row0_pr0 {
+				nvidia,pins = "kb_row0_pr0";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			kb_row1_pr1 {
+				nvidia,pins = "kb_row1_pr1";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			kb_row2_pr2 {
+				nvidia,pins = "kb_row2_pr2";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			kb_row3_pr3 {
+				nvidia,pins = "kb_row3_pr3";
+				nvidia,function = "sys";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			kb_row4_pr4 {
+				nvidia,pins = "kb_row4_pr4";
+				nvidia,function = "rsvd3";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			kb_row5_pr5 {
+				nvidia,pins = "kb_row5_pr5";
+				nvidia,function = "rsvd3";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			kb_row6_pr6 {
+				nvidia,pins = "kb_row6_pr6";
+				nvidia,function = "displaya_alt";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			kb_row7_pr7 {
+				nvidia,pins = "kb_row7_pr7";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			kb_row8_ps0 {
+				nvidia,pins = "kb_row8_ps0";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			kb_row9_ps1 {
+				nvidia,pins = "kb_row9_ps1";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			kb_row10_ps2 {
+				nvidia,pins = "kb_row10_ps2";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			kb_row11_ps3 {
+				nvidia,pins = "kb_row11_ps3";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			kb_row12_ps4 {
+				nvidia,pins = "kb_row12_ps4";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			kb_row13_ps5 {
+				nvidia,pins = "kb_row13_ps5";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			kb_row14_ps6 {
+				nvidia,pins = "kb_row14_ps6";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			kb_row15_ps7 {
+				nvidia,pins = "kb_row15_ps7";
+				nvidia,function = "soc";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			kb_row16_pt0 {
+				nvidia,pins = "kb_row16_pt0";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			kb_row17_pt1 {
+				nvidia,pins = "kb_row17_pt1";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			gen2_i2c_scl_pt5 {
+				nvidia,pins = "gen2_i2c_scl_pt5";
+				nvidia,function = "i2c2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+				nvidia,open-drain = <TEGRA_PIN_ENABLE>;
+			};
+			gen2_i2c_sda_pt6 {
+				nvidia,pins = "gen2_i2c_sda_pt6";
+				nvidia,function = "i2c2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+				nvidia,open-drain = <TEGRA_PIN_ENABLE>;
+			};
+			sdmmc4_cmd_pt7 {
+				nvidia,pins = "sdmmc4_cmd_pt7";
+				nvidia,function = "sdmmc4";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			pu0 {
+				nvidia,pins = "pu0";
+				nvidia,function = "rsvd4";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pu1 {
+				nvidia,pins = "pu1";
+				nvidia,function = "rsvd1";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			pu2 {
+				nvidia,pins = "pu2";
+				nvidia,function = "rsvd1";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			pu3 {
+				nvidia,pins = "pu3";
+				nvidia,function = "gmi";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pu4 {
+				nvidia,pins = "pu4";
+				nvidia,function = "gmi";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pu5 {
+				nvidia,pins = "pu5";
+				nvidia,function = "gmi";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			pu6 {
+				nvidia,pins = "pu6";
+				nvidia,function = "rsvd3";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			pv0 {
+				nvidia,pins = "pv0";
+				nvidia,function = "rsvd1";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			pv1 {
+				nvidia,pins = "pv1";
+				nvidia,function = "rsvd1";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			sdmmc3_cd_n_pv2 {
+				nvidia,pins = "sdmmc3_cd_n_pv2";
+				nvidia,function = "sdmmc3";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			sdmmc1_wp_n_pv3 {
+				nvidia,pins = "sdmmc1_wp_n_pv3";
+				nvidia,function = "sdmmc1";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_ENABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			ddc_scl_pv4 {
+				nvidia,pins = "ddc_scl_pv4";
+				nvidia,function = "i2c4";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+				nvidia,rcv-sel = <TEGRA_PIN_DISABLE>;
+			};
+			ddc_sda_pv5 {
+				nvidia,pins = "ddc_sda_pv5";
+				nvidia,function = "i2c4";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+				nvidia,rcv-sel = <TEGRA_PIN_DISABLE>;
+			};
+			gpio_w2_aud_pw2 {
+				nvidia,pins = "gpio_w2_aud_pw2";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			gpio_w3_aud_pw3 {
+				nvidia,pins = "gpio_w3_aud_pw3";
+				nvidia,function = "spi6";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			dap_mclk1_pw4 {
+				nvidia,pins = "dap_mclk1_pw4";
+				nvidia,function = "extperiph1";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			clk2_out_pw5 {
+				nvidia,pins = "clk2_out_pw5";
+				nvidia,function = "extperiph2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			uart3_txd_pw6 {
+				nvidia,pins = "uart3_txd_pw6";
+				nvidia,function = "uartc";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			uart3_rxd_pw7 {
+				nvidia,pins = "uart3_rxd_pw7";
+				nvidia,function = "uartc";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			dvfs_pwm_px0 {
+				nvidia,pins = "dvfs_pwm_px0";
+				nvidia,function = "cldvfs";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			gpio_x1_aud_px1 {
+				nvidia,pins = "gpio_x1_aud_px1";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			dvfs_clk_px2 {
+				nvidia,pins = "dvfs_clk_px2";
+				nvidia,function = "cldvfs";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			gpio_x3_aud_px3 {
+				nvidia,pins = "gpio_x3_aud_px3";
+				nvidia,function = "rsvd4";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			gpio_x4_aud_px4 {
+				nvidia,pins = "gpio_x4_aud_px4";
+				nvidia,function = "gmi";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			gpio_x5_aud_px5 {
+				nvidia,pins = "gpio_x5_aud_px5";
+				nvidia,function = "rsvd4";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			gpio_x6_aud_px6 {
+				nvidia,pins = "gpio_x6_aud_px6";
+				nvidia,function = "gmi";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			gpio_x7_aud_px7 {
+				nvidia,pins = "gpio_x7_aud_px7";
+				nvidia,function = "rsvd1";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			ulpi_clk_py0 {
+				nvidia,pins = "ulpi_clk_py0";
+				nvidia,function = "spi1";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			ulpi_dir_py1 {
+				nvidia,pins = "ulpi_dir_py1";
+				nvidia,function = "spi1";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			ulpi_nxt_py2 {
+				nvidia,pins = "ulpi_nxt_py2";
+				nvidia,function = "spi1";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			ulpi_stp_py3 {
+				nvidia,pins = "ulpi_stp_py3";
+				nvidia,function = "spi1";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			sdmmc1_dat3_py4 {
+				nvidia,pins = "sdmmc1_dat3_py4";
+				nvidia,function = "sdmmc1";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			sdmmc1_dat2_py5 {
+				nvidia,pins = "sdmmc1_dat2_py5";
+				nvidia,function = "sdmmc1";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			sdmmc1_dat1_py6 {
+				nvidia,pins = "sdmmc1_dat1_py6";
+				nvidia,function = "sdmmc1";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			sdmmc1_dat0_py7 {
+				nvidia,pins = "sdmmc1_dat0_py7";
+				nvidia,function = "sdmmc1";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			sdmmc1_clk_pz0 {
+				nvidia,pins = "sdmmc1_clk_pz0";
+				nvidia,function = "sdmmc1";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			sdmmc1_cmd_pz1 {
+				nvidia,pins = "sdmmc1_cmd_pz1";
+				nvidia,function = "sdmmc1";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			pwr_i2c_scl_pz6 {
+				nvidia,pins = "pwr_i2c_scl_pz6";
+				nvidia,function = "i2cpwr";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+				nvidia,open-drain = <TEGRA_PIN_ENABLE>;
+			};
+			pwr_i2c_sda_pz7 {
+				nvidia,pins = "pwr_i2c_sda_pz7";
+				nvidia,function = "i2cpwr";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+				nvidia,open-drain = <TEGRA_PIN_ENABLE>;
+			};
+			sdmmc4_dat0_paa0 {
+				nvidia,pins = "sdmmc4_dat0_paa0";
+				nvidia,function = "sdmmc4";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			sdmmc4_dat1_paa1 {
+				nvidia,pins = "sdmmc4_dat1_paa1";
+				nvidia,function = "sdmmc4";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			sdmmc4_dat2_paa2 {
+				nvidia,pins = "sdmmc4_dat2_paa2";
+				nvidia,function = "sdmmc4";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			sdmmc4_dat3_paa3 {
+				nvidia,pins = "sdmmc4_dat3_paa3";
+				nvidia,function = "sdmmc4";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			sdmmc4_dat4_paa4 {
+				nvidia,pins = "sdmmc4_dat4_paa4";
+				nvidia,function = "sdmmc4";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			sdmmc4_dat5_paa5 {
+				nvidia,pins = "sdmmc4_dat5_paa5";
+				nvidia,function = "sdmmc4";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			sdmmc4_dat6_paa6 {
+				nvidia,pins = "sdmmc4_dat6_paa6";
+				nvidia,function = "sdmmc4";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			sdmmc4_dat7_paa7 {
+				nvidia,pins = "sdmmc4_dat7_paa7";
+				nvidia,function = "sdmmc4";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			pbb0 {
+				nvidia,pins = "pbb0";
+				nvidia,function = "vimclk2_alt";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			cam_i2c_scl_pbb1 {
+				nvidia,pins = "cam_i2c_scl_pbb1";
+				nvidia,function = "i2c3";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+				nvidia,open-drain = <TEGRA_PIN_ENABLE>;
+			};
+			cam_i2c_sda_pbb2 {
+				nvidia,pins = "cam_i2c_sda_pbb2";
+				nvidia,function = "i2c3";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+				nvidia,open-drain = <TEGRA_PIN_ENABLE>;
+			};
+			pbb3 {
+				nvidia,pins = "pbb3";
+				nvidia,function = "vgp3";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pbb4 {
+				nvidia,pins = "pbb4";
+				nvidia,function = "vgp4";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pbb5 {
+				nvidia,pins = "pbb5";
+				nvidia,function = "rsvd3";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pbb6 {
+				nvidia,pins = "pbb6";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pbb7 {
+				nvidia,pins = "pbb7";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			cam_mclk_pcc0 {
+				nvidia,pins = "cam_mclk_pcc0";
+				nvidia,function = "vi_alt3";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pcc1 {
+				nvidia,pins = "pcc1";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			pcc2 {
+				nvidia,pins = "pcc2";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			sdmmc4_clk_pcc4 {
+				nvidia,pins = "sdmmc4_clk_pcc4";
+				nvidia,function = "sdmmc4";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			clk2_req_pcc5 {
+				nvidia,pins = "clk2_req_pcc5";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			clk3_out_pee0 {
+				nvidia,pins = "clk3_out_pee0";
+				nvidia,function = "extperiph3";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			clk3_req_pee1 {
+				nvidia,pins = "clk3_req_pee1";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			dap_mclk1_req_pee2 {
+				nvidia,pins = "dap_mclk1_req_pee2";
+				nvidia,function = "sata";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			hdmi_cec_pee3 {
+				nvidia,pins = "hdmi_cec_pee3";
+				nvidia,function = "cec";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+				nvidia,open-drain = <TEGRA_PIN_ENABLE>;
+			};
+			sdmmc3_clk_lb_out_pee4 {
+				nvidia,pins = "sdmmc3_clk_lb_out_pee4";
+				nvidia,function = "sdmmc3";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			sdmmc3_clk_lb_in_pee5 {
+				nvidia,pins = "sdmmc3_clk_lb_in_pee5";
+				nvidia,function = "sdmmc3";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			dp_hpd_pff0 {
+				nvidia,pins = "dp_hpd_pff0";
+				nvidia,function = "dp";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			usb_vbus_en2_pff1 {
+				nvidia,pins = "usb_vbus_en2_pff1";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+				nvidia,open-drain = <TEGRA_PIN_DISABLE>;
+			};
+			pff2 {
+				nvidia,pins = "pff2";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+				nvidia,open-drain = <TEGRA_PIN_DISABLE>;
+			};
+			core_pwr_req {
+				nvidia,pins = "core_pwr_req";
+				nvidia,function = "pwron";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			cpu_pwr_req {
+				nvidia,pins = "cpu_pwr_req";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			pwr_int_n {
+				nvidia,pins = "pwr_int_n";
+				nvidia,function = "pmi";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			reset_out_n {
+				nvidia,pins = "reset_out_n";
+				nvidia,function = "reset_out_n";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+			owr {
+				nvidia,pins = "owr";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_DOWN>;
+				nvidia,tristate = <TEGRA_PIN_ENABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+				nvidia,rcv-sel = <TEGRA_PIN_DISABLE>;
+			};
+			clk_32k_in {
+				nvidia,pins = "clk_32k_in";
+				nvidia,function = "rsvd2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+			jtag_rtck {
+				nvidia,pins = "jtag_rtck";
+				nvidia,function = "rtck";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_DISABLE>;
+			};
+		};
+	};
+
+	/* DB9 serial port */
+	serial@0,70006300 {
+		status = "okay";
+	};
+
+	/* Expansion GEN1_I2C_*, mini-PCIe I2C, on-board components */
+	i2c@0,7000c000 {
+		status = "okay";
+		clock-frequency = <100000>;
+
+		rt5639: audio-codec@1c {
+			compatible = "realtek,rt5639";
+			reg = <0x1c>;
+			interrupt-parent = <&gpio>;
+			interrupts = <TEGRA_GPIO(H, 4) GPIO_ACTIVE_HIGH>;
+			realtek,ldo1-en-gpios =
+				<&gpio TEGRA_GPIO(R, 2) GPIO_ACTIVE_HIGH>;
+		};
+
+		temperature-sensor@4c {
+			compatible = "ti,tmp451";
+			reg = <0x4c>;
+			interrupt-parent = <&gpio>;
+			interrupts = <TEGRA_GPIO(I, 6) IRQ_TYPE_LEVEL_LOW>;
+		};
+
+		eeprom@56 {
+			compatible = "atmel,24c02";
+			reg = <0x56>;
+			pagesize = <8>;
+		};
+	};
+
+	/* Expansion GEN2_I2C_* */
+	i2c@0,7000c400 {
+		status = "okay";
+		clock-frequency = <100000>;
+	};
+
+	/* Expansion CAM_I2C_* */
+	i2c@0,7000c500 {
+		status = "okay";
+		clock-frequency = <100000>;
+	};
+
+	/* HDMI DDC */
+	hdmi_ddc: i2c@0,7000c700 {
+		status = "okay";
+		clock-frequency = <100000>;
+	};
+
+	/* Expansion PWR_I2C_*, on-board components */
+	i2c@0,7000d000 {
+		status = "okay";
+		clock-frequency = <400000>;
+
+		pmic: pmic@40 {
+			compatible = "ams,as3722";
+			reg = <0x40>;
+			interrupts = <0 86 IRQ_TYPE_LEVEL_HIGH>;
+
+			ams,system-power-controller;
+
+			#interrupt-cells = <2>;
+			interrupt-controller;
+
+			gpio-controller;
+			#gpio-cells = <2>;
+
+			pinctrl-names = "default";
+			pinctrl-0 = <&as3722_default>;
+
+			as3722_default: pinmux {
+				gpio0 {
+					pins = "gpio0";
+					function = "gpio";
+					bias-pull-down;
+				};
+
+				gpio1_2_4_7 {
+					pins = "gpio1", "gpio2", "gpio4", "gpio7";
+					function = "gpio";
+					bias-pull-up;
+				};
+
+				gpio3_5_6 {
+					pins = "gpio3", "gpio5", "gpio6";
+					bias-high-impedance;
+				};
+			};
+
+			regulators {
+				vsup-sd2-supply = <&vdd_5v0_sys>;
+				vsup-sd3-supply = <&vdd_5v0_sys>;
+				vsup-sd4-supply = <&vdd_5v0_sys>;
+				vsup-sd5-supply = <&vdd_5v0_sys>;
+				vin-ldo0-supply = <&vdd_1v35_lp0>;
+				vin-ldo1-6-supply = <&vdd_3v3_run>;
+				vin-ldo2-5-7-supply = <&vddio_1v8>;
+				vin-ldo3-4-supply = <&vdd_3v3_sys>;
+				vin-ldo9-10-supply = <&vdd_5v0_sys>;
+				vin-ldo11-supply = <&vdd_3v3_run>;
+
+				sd0 {
+					regulator-name = "+VDD_CPU_AP";
+					regulator-min-microvolt = <700000>;
+					regulator-max-microvolt = <1400000>;
+					regulator-min-microamp = <3500000>;
+					regulator-max-microamp = <3500000>;
+					regulator-always-on;
+					regulator-boot-on;
+					ams,external-control = <2>;
+				};
+
+				sd1 {
+					regulator-name = "+VDD_CORE";
+					regulator-min-microvolt = <700000>;
+					regulator-max-microvolt = <1350000>;
+					regulator-min-microamp = <2500000>;
+					regulator-max-microamp = <2500000>;
+					regulator-always-on;
+					regulator-boot-on;
+					ams,external-control = <1>;
+				};
+
+				vdd_1v35_lp0: sd2 {
+					regulator-name = "+1.35V_LP0(sd2)";
+					regulator-min-microvolt = <1350000>;
+					regulator-max-microvolt = <1350000>;
+					regulator-always-on;
+					regulator-boot-on;
+				};
+
+				sd3 {
+					regulator-name = "+1.35V_LP0(sd3)";
+					regulator-min-microvolt = <1350000>;
+					regulator-max-microvolt = <1350000>;
+					regulator-always-on;
+					regulator-boot-on;
+				};
+
+				vdd_1v05_run: sd4 {
+					regulator-name = "+1.05V_RUN";
+					regulator-min-microvolt = <1050000>;
+					regulator-max-microvolt = <1050000>;
+				};
+
+				vddio_1v8: sd5 {
+					regulator-name = "+1.8V_VDDIO";
+					regulator-min-microvolt = <1800000>;
+					regulator-max-microvolt = <1800000>;
+					regulator-boot-on;
+					regulator-always-on;
+				};
+
+				sd6 {
+					regulator-name = "+VDD_GPU_AP";
+					regulator-min-microvolt = <650000>;
+					regulator-max-microvolt = <1200000>;
+					regulator-min-microamp = <3500000>;
+					regulator-max-microamp = <3500000>;
+					regulator-boot-on;
+					regulator-always-on;
+				};
+
+				ldo0 {
+					regulator-name = "+1.05V_RUN_AVDD";
+					regulator-min-microvolt = <1050000>;
+					regulator-max-microvolt = <1050000>;
+					regulator-boot-on;
+					regulator-always-on;
+					ams,external-control = <1>;
+				};
+
+				ldo1 {
+					regulator-name = "+1.8V_RUN_CAM";
+					regulator-min-microvolt = <1800000>;
+					regulator-max-microvolt = <1800000>;
+				};
+
+				ldo2 {
+					regulator-name = "+1.2V_GEN_AVDD";
+					regulator-min-microvolt = <1200000>;
+					regulator-max-microvolt = <1200000>;
+					regulator-boot-on;
+					regulator-always-on;
+				};
+
+				ldo3 {
+					regulator-name = "+1.05V_LP0_VDD_RTC";
+					regulator-min-microvolt = <1000000>;
+					regulator-max-microvolt = <1000000>;
+					regulator-boot-on;
+					regulator-always-on;
+					ams,enable-tracking;
+				};
+
+				ldo4 {
+					regulator-name = "+2.8V_RUN_CAM";
+					regulator-min-microvolt = <2800000>;
+					regulator-max-microvolt = <2800000>;
+				};
+
+				ldo5 {
+					regulator-name = "+1.2V_RUN_CAM_FRONT";
+					regulator-min-microvolt = <1200000>;
+					regulator-max-microvolt = <1200000>;
+				};
+
+				vddio_sdmmc3: ldo6 {
+					regulator-name = "+VDDIO_SDMMC3";
+					regulator-min-microvolt = <1800000>;
+					regulator-max-microvolt = <3300000>;
+				};
+
+				ldo7 {
+					regulator-name = "+1.05V_RUN_CAM_REAR";
+					regulator-min-microvolt = <1050000>;
+					regulator-max-microvolt = <1050000>;
+				};
+
+				ldo9 {
+					regulator-name = "+3.3V_RUN_TOUCH";
+					regulator-min-microvolt = <2800000>;
+					regulator-max-microvolt = <2800000>;
+				};
+
+				ldo10 {
+					regulator-name = "+2.8V_RUN_CAM_AF";
+					regulator-min-microvolt = <2800000>;
+					regulator-max-microvolt = <2800000>;
+				};
+
+				ldo11 {
+					regulator-name = "+1.8V_RUN_VPP_FUSE";
+					regulator-min-microvolt = <1800000>;
+					regulator-max-microvolt = <1800000>;
+				};
+			};
+		};
+	};
+
+	/* Expansion TS_SPI_* */
+	spi@0,7000d400 {
+		status = "okay";
+	};
+
+	/* Internal SPI */
+	spi@0,7000da00 {
+		status = "okay";
+		spi-max-frequency = <25000000>;
+		spi-flash@0 {
+			compatible = "winbond,w25q32dw";
+			reg = <0>;
+			spi-max-frequency = <20000000>;
+		};
+	};
+
+	pmc@0,7000e400 {
+		nvidia,invert-interrupt;
+		nvidia,suspend-mode = <1>;
+		nvidia,cpu-pwr-good-time = <500>;
+		nvidia,cpu-pwr-off-time = <300>;
+		nvidia,core-pwr-good-time = <641 3845>;
+		nvidia,core-pwr-off-time = <61036>;
+		nvidia,core-power-req-active-high;
+		nvidia,sys-clock-req-active-high;
+	};
+
+	/* SD card */
+	sdhci@0,700b0400 {
+		status = "okay";
+		cd-gpios = <&gpio TEGRA_GPIO(V, 2) GPIO_ACTIVE_LOW>;
+		power-gpios = <&gpio TEGRA_GPIO(R, 0) GPIO_ACTIVE_HIGH>;
+		wp-gpios = <&gpio TEGRA_GPIO(Q, 4) GPIO_ACTIVE_HIGH>;
+		bus-width = <4>;
+		vqmmc-supply = <&vddio_sdmmc3>;
+	};
+
+	/* eMMC */
+	sdhci@0,700b0600 {
+		status = "okay";
+		bus-width = <8>;
+	};
+
+	ahub@0,70300000 {
+		i2s@0,70301100 {
+			status = "okay";
+		};
+	};
+
+	/* mini-PCIe USB */
+	usb@0,7d004000 {
+		status = "okay";
+	};
+
+	usb-phy@0,7d004000 {
+		status = "okay";
+	};
+
+	/* USB A connector */
+	usb@0,7d008000 {
+		status = "okay";
+	};
+
+	usb-phy@0,7d008000 {
+		status = "okay";
+		vbus-supply = <&vdd_usb3_vbus>;
+	};
+
+	clocks {
+		compatible = "simple-bus";
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		clk32k_in: clock@0 {
+			compatible = "fixed-clock";
+			reg = <0>;
+			#clock-cells = <0>;
+			clock-frequency = <32768>;
+		};
+	};
+
+	gpio-keys {
+		compatible = "gpio-keys";
+
+		power {
+			label = "Power";
+			gpios = <&gpio TEGRA_GPIO(Q, 0) GPIO_ACTIVE_LOW>;
+			linux,code = <KEY_POWER>;
+			debounce-interval = <10>;
+			gpio-key,wakeup;
+		};
+	};
+
+	regulators {
+		compatible = "simple-bus";
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		vdd_mux: regulator@0 {
+			compatible = "regulator-fixed";
+			reg = <0>;
+			regulator-name = "+VDD_MUX";
+			regulator-min-microvolt = <12000000>;
+			regulator-max-microvolt = <12000000>;
+			regulator-always-on;
+			regulator-boot-on;
+		};
+
+		vdd_5v0_sys: regulator@1 {
+			compatible = "regulator-fixed";
+			reg = <1>;
+			regulator-name = "+5V_SYS";
+			regulator-min-microvolt = <5000000>;
+			regulator-max-microvolt = <5000000>;
+			regulator-always-on;
+			regulator-boot-on;
+			vin-supply = <&vdd_mux>;
+		};
+
+		vdd_3v3_sys: regulator@2 {
+			compatible = "regulator-fixed";
+			reg = <2>;
+			regulator-name = "+3.3V_SYS";
+			regulator-min-microvolt = <3300000>;
+			regulator-max-microvolt = <3300000>;
+			regulator-always-on;
+			regulator-boot-on;
+			vin-supply = <&vdd_mux>;
+		};
+
+		vdd_3v3_run: regulator@3 {
+			compatible = "regulator-fixed";
+			reg = <3>;
+			regulator-name = "+3.3V_RUN";
+			regulator-min-microvolt = <3300000>;
+			regulator-max-microvolt = <3300000>;
+			regulator-always-on;
+			regulator-boot-on;
+			gpio = <&pmic 1 GPIO_ACTIVE_HIGH>;
+			enable-active-high;
+			vin-supply = <&vdd_3v3_sys>;
+		};
+
+		vdd_3v3_hdmi: regulator@4 {
+			compatible = "regulator-fixed";
+			reg = <4>;
+			regulator-name = "+3.3V_AVDD_HDMI_AP_GATED";
+			regulator-min-microvolt = <3300000>;
+			regulator-max-microvolt = <3300000>;
+			vin-supply = <&vdd_3v3_run>;
+		};
+
+		vdd_usb1_vbus: regulator@7 {
+			compatible = "regulator-fixed";
+			reg = <7>;
+			regulator-name = "+USB0_VBUS_SW";
+			regulator-min-microvolt = <5000000>;
+			regulator-max-microvolt = <5000000>;
+			gpio = <&gpio TEGRA_GPIO(N, 4) GPIO_ACTIVE_HIGH>;
+			enable-active-high;
+			gpio-open-drain;
+			vin-supply = <&vdd_5v0_sys>;
+		};
+
+		vdd_usb3_vbus: regulator@8 {
+			compatible = "regulator-fixed";
+			reg = <8>;
+			regulator-name = "+5V_USB_HS";
+			regulator-min-microvolt = <5000000>;
+			regulator-max-microvolt = <5000000>;
+			gpio = <&gpio TEGRA_GPIO(N, 5) GPIO_ACTIVE_HIGH>;
+			enable-active-high;
+			gpio-open-drain;
+			vin-supply = <&vdd_5v0_sys>;
+		};
+
+		vdd_3v3_lp0: regulator@10 {
+			compatible = "regulator-fixed";
+			reg = <10>;
+			regulator-name = "+3.3V_LP0";
+			regulator-min-microvolt = <3300000>;
+			regulator-max-microvolt = <3300000>;
+			regulator-always-on;
+			regulator-boot-on;
+			gpio = <&pmic 2 GPIO_ACTIVE_HIGH>;
+			enable-active-high;
+			vin-supply = <&vdd_3v3_sys>;
+		};
+
+		vdd_hdmi_pll: regulator@11 {
+			compatible = "regulator-fixed";
+			reg = <11>;
+			regulator-name = "+1.05V_RUN_AVDD_HDMI_PLL";
+			regulator-min-microvolt = <1050000>;
+			regulator-max-microvolt = <1050000>;
+			gpio = <&gpio TEGRA_GPIO(H, 7) GPIO_ACTIVE_LOW>;
+			vin-supply = <&vdd_1v05_run>;
+		};
+
+		vdd_5v0_hdmi: regulator@12 {
+			compatible = "regulator-fixed";
+			reg = <12>;
+			regulator-name = "+5V_HDMI_CON";
+			regulator-min-microvolt = <5000000>;
+			regulator-max-microvolt = <5000000>;
+			gpio = <&gpio TEGRA_GPIO(K, 6) GPIO_ACTIVE_HIGH>;
+			enable-active-high;
+			vin-supply = <&vdd_5v0_sys>;
+		};
+	};
+
+	sound {
+		compatible = "nvidia,tegra-audio-rt5640-jetson-tk1",
+			     "nvidia,tegra-audio-rt5640";
+		nvidia,model = "NVIDIA Tegra Jetson TK1";
+
+		nvidia,audio-routing =
+			"Headphones", "HPOR",
+			"Headphones", "HPOL",
+			"Mic Jack", "MICBIAS1",
+			"IN2P", "Mic Jack";
+
+		nvidia,i2s-controller = <&tegra_i2s1>;
+		nvidia,audio-codec = <&rt5639>;
+
+		nvidia,hp-det-gpios = <&gpio TEGRA_GPIO(R, 7) GPIO_ACTIVE_LOW>;
+
+		clocks = <&tegra_car TEGRA124_CLK_PLL_A>,
+			 <&tegra_car TEGRA124_CLK_PLL_A_OUT0>,
+			 <&tegra_car TEGRA124_CLK_EXTERN1>;
+		clock-names = "pll_a", "pll_a_out0", "mclk";
+	};
+};
diff --git a/arch/arm/boot/dts/tegra124-venice2.dts b/arch/arm/boot/dts/tegra124-venice2.dts
index c17283c0459..f0bb8424402 100644
--- a/arch/arm/boot/dts/tegra124-venice2.dts
+++ b/arch/arm/boot/dts/tegra124-venice2.dts
@@ -17,6 +17,18 @@
 	};
 
 	host1x@0,50000000 {
+		hdmi@0,54280000 {
+			status = "okay";
+
+			vdd-supply = <&vdd_3v3_hdmi>;
+			pll-supply = <&vdd_hdmi_pll>;
+			hdmi-supply = <&vdd_5v0_hdmi>;
+
+			nvidia,ddc-i2c-bus = <&hdmi_ddc>;
+			nvidia,hpd-gpio =
+				<&gpio TEGRA_GPIO(N, 7) GPIO_ACTIVE_HIGH>;
+		};
+
 		sor@0,54540000 {
 			status = "okay";
 
@@ -601,7 +613,7 @@
 		clock-frequency = <100000>;
 	};
 
-	i2c@0,7000c700 {
+	hdmi_ddc: i2c@0,7000c700 {
 		status = "okay";
 		clock-frequency = <100000>;
 	};
@@ -700,7 +712,7 @@
 					regulator-boot-on;
 				};
 
-				sd4 {
+				vdd_1v05_run: sd4 {
 					regulator-name = "+1.05V_RUN";
 					regulator-min-microvolt = <1050000>;
 					regulator-max-microvolt = <1050000>;
@@ -931,9 +943,10 @@
 	sdhci@0,700b0400 {
 		cd-gpios = <&gpio TEGRA_GPIO(V, 2) GPIO_ACTIVE_HIGH>;
 		power-gpios = <&gpio TEGRA_GPIO(R, 0) GPIO_ACTIVE_HIGH>;
+		wp-gpios = <&gpio TEGRA_GPIO(Q, 4) GPIO_ACTIVE_LOW>;
 		status = "okay";
 		bus-width = <4>;
-		vmmc-supply = <&vddio_sdmmc3>;
+		vqmmc-supply = <&vddio_sdmmc3>;
 	};
 
 	sdhci@0,700b0600 {
@@ -1060,6 +1073,8 @@
 			regulator-name = "+3.3V_RUN";
 			regulator-min-microvolt = <3300000>;
 			regulator-max-microvolt = <3300000>;
+			regulator-always-on;
+			regulator-boot-on;
 			gpio = <&pmic 1 GPIO_ACTIVE_HIGH>;
 			enable-active-high;
 			vin-supply = <&vdd_3v3_sys>;
@@ -1145,6 +1160,27 @@
 			enable-active-high;
 			vin-supply = <&vdd_3v3_sys>;
 		};
+
+		vdd_hdmi_pll: regulator@11 {
+			compatible = "regulator-fixed";
+			reg = <11>;
+			regulator-name = "+1.05V_RUN_AVDD_HDMI_PLL";
+			regulator-min-microvolt = <1050000>;
+			regulator-max-microvolt = <1050000>;
+			gpio = <&gpio TEGRA_GPIO(H, 7) GPIO_ACTIVE_LOW>;
+			vin-supply = <&vdd_1v05_run>;
+		};
+
+		vdd_5v0_hdmi: regulator@12 {
+			compatible = "regulator-fixed";
+			reg = <12>;
+			regulator-name = "+5V_HDMI_CON";
+			regulator-min-microvolt = <5000000>;
+			regulator-max-microvolt = <5000000>;
+			gpio = <&gpio TEGRA_GPIO(K, 6) GPIO_ACTIVE_HIGH>;
+			enable-active-high;
+			vin-supply = <&vdd_5v0_sys>;
+		};
 	};
 
 	sound {
diff --git a/arch/arm/boot/dts/tegra124.dtsi b/arch/arm/boot/dts/tegra124.dtsi
index cf45a1a3948..6e6bc4e8185 100644
--- a/arch/arm/boot/dts/tegra124.dtsi
+++ b/arch/arm/boot/dts/tegra124.dtsi
@@ -51,6 +51,18 @@
 			nvidia,head = <1>;
 		};
 
+		hdmi@0,54280000 {
+			compatible = "nvidia,tegra124-hdmi";
+			reg = <0x0 0x54280000 0x0 0x00040000>;
+			interrupts = <GIC_SPI 75 IRQ_TYPE_LEVEL_HIGH>;
+			clocks = <&tegra_car TEGRA124_CLK_HDMI>,
+				 <&tegra_car TEGRA124_CLK_PLL_D2_OUT0>;
+			clock-names = "hdmi", "parent";
+			resets = <&tegra_car 51>;
+			reset-names = "hdmi";
+			status = "disabled";
+		};
+
 		sor@0,54540000 {
 			compatible = "nvidia,tegra124-sor";
 			reg = <0x0 0x54540000 0x0 0x00040000>;
@@ -233,19 +245,6 @@
 		status = "disabled";
 	};
 
-	serial@0,70006400 {
-		compatible = "nvidia,tegra124-uart", "nvidia,tegra20-uart";
-		reg = <0x0 0x70006400 0x0 0x40>;
-		reg-shift = <2>;
-		interrupts = <GIC_SPI 91 IRQ_TYPE_LEVEL_HIGH>;
-		clocks = <&tegra_car TEGRA124_CLK_UARTE>;
-		resets = <&tegra_car 66>;
-		reset-names = "serial";
-		dmas = <&apbdma 20>, <&apbdma 20>;
-		dma-names = "rx", "tx";
-		status = "disabled";
-	};
-
 	pwm@0,7000a000 {
 		compatible = "nvidia,tegra124-pwm", "nvidia,tegra20-pwm";
 		reg = <0x0 0x7000a000 0x0 0x100>;
diff --git a/arch/arm/boot/dts/tegra20-harmony.dts b/arch/arm/boot/dts/tegra20-harmony.dts
index 3fb1f50f6d4..f45aad688d9 100644
--- a/arch/arm/boot/dts/tegra20-harmony.dts
+++ b/arch/arm/boot/dts/tegra20-harmony.dts
@@ -28,6 +28,7 @@
 		hdmi@54280000 {
 			status = "okay";
 
+			hdmi-supply = <&vdd_5v0_hdmi>;
 			vdd-supply = <&hdmi_vdd_reg>;
 			pll-supply = <&hdmi_pll_reg>;
 
@@ -724,6 +725,17 @@
 			gpio = <&gpio TEGRA_GPIO(W, 0) GPIO_ACTIVE_HIGH>;
 			enable-active-high;
 		};
+
+		vdd_5v0_hdmi: regulator@6 {
+			compatible = "regulator-fixed";
+			reg = <6>;
+			regulator-name = "VDDIO_HDMI";
+			regulator-min-microvolt = <5000000>;
+			regulator-max-microvolt = <5000000>;
+			gpio = <&gpio TEGRA_GPIO(T, 2) GPIO_ACTIVE_HIGH>;
+			enable-active-high;
+			vin-supply = <&vdd_5v0_reg>;
+		};
 	};
 
 	sound {
diff --git a/arch/arm/boot/dts/tegra30-beaver.dts b/arch/arm/boot/dts/tegra30-beaver.dts
index e93fe45b780..3189791a928 100644
--- a/arch/arm/boot/dts/tegra30-beaver.dts
+++ b/arch/arm/boot/dts/tegra30-beaver.dts
@@ -40,6 +40,7 @@
 		hdmi@54280000 {
 			status = "okay";
 
+			hdmi-supply = <&vdd_5v0_hdmi>;
 			vdd-supply = <&sys_3v3_reg>;
 			pll-supply = <&vio_reg>;
 
@@ -478,6 +479,17 @@
 			gpio = <&gpio TEGRA_GPIO(L, 7) GPIO_ACTIVE_HIGH>;
 			vin-supply = <&sys_3v3_reg>;
 		};
+
+		vdd_5v0_hdmi: regulator@8 {
+			compatible = "regulator-fixed";
+			reg = <8>;
+			regulator-name = "+VDD_5V_HDMI";
+			regulator-min-microvolt = <5000000>;
+			regulator-max-microvolt = <5000000>;
+			regulator-always-on;
+			regulator-boot-on;
+			vin-supply = <&sys_3v3_reg>;
+		};
 	};
 
 	sound {
diff --git a/arch/arm/boot/dts/tegra30-colibri-eval-v3.dts b/arch/arm/boot/dts/tegra30-colibri-eval-v3.dts
new file mode 100644
index 00000000000..7793abd5bef
--- /dev/null
+++ b/arch/arm/boot/dts/tegra30-colibri-eval-v3.dts
@@ -0,0 +1,205 @@
+/dts-v1/;
+
+#include "tegra30-colibri.dtsi"
+
+/ {
+	model = "Toradex Colibri T30 on Colibri Evaluation Board";
+	compatible = "toradex,colibri_t30-eval-v3", "toradex,colibri_t30", "nvidia,tegra30";
+
+	aliases {
+		rtc0 = "/i2c@7000c000/rtc@68";
+		rtc1 = "/i2c@7000d000/tps65911@2d";
+		rtc2 = "/rtc@7000e000";
+	};
+
+	host1x@50000000 {
+		dc@54200000 {
+			rgb {
+				status = "okay";
+				nvidia,panel = <&panel>;
+			};
+		};
+		hdmi@54280000 {
+			status = "okay";
+		};
+	};
+
+	serial@70006000 {
+		status = "okay";
+	};
+
+	serial@70006040 {
+		compatible = "nvidia,tegra30-hsuart";
+		status = "okay";
+	};
+
+	serial@70006300 {
+		compatible = "nvidia,tegra30-hsuart";
+		status = "okay";
+	};
+
+	pwm@7000a000 {
+		status = "okay";
+	};
+
+	/*
+	 * GEN1_I2C: I2C_SDA/SCL on SODIMM pin 194/196 (e.g. RTC on carrier
+	 * board)
+	 */
+	i2c@7000c000 {
+		status = "okay";
+		clock-frequency = <100000>;
+
+		/* M41T0M6 real time clock on carrier board */
+		rtc@68 {
+			compatible = "stm,m41t00";
+			reg = <0x68>;
+		};
+	};
+
+	/* DDC_CLOCK/DATA on X3 pin 15/16 (e.g. display EDID) */
+	hdmiddc: i2c@7000c700 {
+		status = "okay";
+	};
+
+	/* SPI1: Colibri SSP */
+	spi@7000d400 {
+		status = "okay";
+		spi-max-frequency = <25000000>;
+		can0: can@0 {
+			compatible = "microchip,mcp2515";
+			reg = <0>;
+			clocks = <&clk16m>;
+			interrupt-parent = <&gpio>;
+			interrupts = <TEGRA_GPIO(S, 0) GPIO_ACTIVE_LOW>;
+			spi-max-frequency = <10000000>;
+		};
+		spidev0: spi@1 {
+			compatible = "spidev";
+			reg = <1>;
+			spi-max-frequency = <25000000>;
+		};
+	};
+
+	sdhci@78000200 {
+		status = "okay";
+		bus-width = <4>;
+		cd-gpios = <&gpio TEGRA_GPIO(C, 7) GPIO_ACTIVE_LOW>;
+		no-1-8-v;
+	};
+
+	/* EHCI instance 0: USB1_DP/N -> USBC_P/N */
+	usb@7d000000 {
+		status = "okay";
+	};
+
+	usb-phy@7d000000 {
+		status = "okay";
+		dr_mode = "otg";
+		vbus-supply = <&usbc_vbus_reg>;
+	};
+
+	/* EHCI instance 2: USB3_DP/N -> USBH_P/N */
+	usb@7d008000 {
+		status = "okay";
+	};
+
+	usb-phy@7d008000 {
+		status = "okay";
+		vbus-supply = <&usbh_vbus_reg>;
+	};
+
+	backlight: backlight {
+		compatible = "pwm-backlight";
+
+		/* PWM<A> */
+		pwms = <&pwm 0 5000000>;
+		brightness-levels = <255 128 64 32 16 8 4 0>;
+		default-brightness-level = <6>;
+		/* BL_ON */
+		enable-gpios = <&gpio TEGRA_GPIO(V, 2) GPIO_ACTIVE_HIGH>;
+	};
+
+	clocks {
+		clk16m: clk@1 {
+			compatible = "fixed-clock";
+			reg=<1>;
+			#clock-cells = <0>;
+			clock-frequency = <16000000>;
+			clock-output-names = "clk16m";
+		};
+	};
+
+	gpio-keys {
+		compatible = "gpio-keys";
+
+		power {
+			label = "Power";
+			gpios = <&gpio TEGRA_GPIO(V, 1) GPIO_ACTIVE_HIGH>;
+			linux,code = <KEY_POWER>;
+			debounce-interval = <10>;
+			gpio-key,wakeup;
+		};
+	};
+
+	panel: panel {
+		/*
+		 * edt,et057090dhu: EDT 5.7" LCD TFT
+		 * edt,et070080dh6: EDT 7.0" LCD TFT
+		 */
+		compatible = "edt,et057090dhu", "simple-panel";
+
+		backlight = <&backlight>;
+	};
+
+	pwmleds {
+		compatible = "pwm-leds";
+
+		pwmb {
+			label = "PWM<B>";
+			pwms = <&pwm 1 19600>;
+			max-brightness = <255>;
+		};
+		pwmc {
+			label = "PWM<C>";
+			pwms = <&pwm 2 19600>;
+			max-brightness = <255>;
+		};
+		pwmd {
+			label = "PWM<D>";
+			pwms = <&pwm 3 19600>;
+			max-brightness = <255>;
+		};
+	};
+
+	regulators {
+		sys_5v0_reg: regulator@1 {
+			compatible = "regulator-fixed";
+			reg = <1>;
+			regulator-name = "5v0";
+			regulator-min-microvolt = <5000000>;
+			regulator-max-microvolt = <5000000>;
+			regulator-always-on;
+		};
+
+		usbc_vbus_reg: regulator@2 {
+			compatible = "regulator-fixed";
+			reg = <2>;
+			regulator-name = "usbc_vbus";
+			regulator-min-microvolt = <5000000>;
+			regulator-max-microvolt = <5000000>;
+			vin-supply = <&sys_5v0_reg>;
+		};
+
+		/* USBH_PEN */
+		usbh_vbus_reg: regulator@3 {
+			compatible = "regulator-fixed";
+			reg = <3>;
+			regulator-name = "usbh_vbus";
+			regulator-min-microvolt = <5000000>;
+			regulator-max-microvolt = <5000000>;
+			gpio = <&gpio TEGRA_GPIO(W, 2) GPIO_ACTIVE_LOW>;
+			vin-supply = <&sys_5v0_reg>;
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/tegra30-colibri.dtsi b/arch/arm/boot/dts/tegra30-colibri.dtsi
new file mode 100644
index 00000000000..bf16f8e6562
--- /dev/null
+++ b/arch/arm/boot/dts/tegra30-colibri.dtsi
@@ -0,0 +1,377 @@
+#include <dt-bindings/input/input.h>
+#include "tegra30.dtsi"
+
+/*
+ * Toradex Colibri T30 Device Tree
+ * Compatible for Revisions 1.1B/1.1C/1.1D
+ */
+/ {
+	model = "Toradex Colibri T30";
+	compatible = "toradex,colibri_t30", "nvidia,tegra30";
+
+	memory {
+		reg = <0x80000000 0x40000000>;
+	};
+
+	host1x@50000000 {
+		hdmi@54280000 {
+			vdd-supply = <&sys_3v3_reg>;
+			pll-supply = <&vio_reg>;
+
+			nvidia,hpd-gpio =
+				<&gpio TEGRA_GPIO(N, 7) GPIO_ACTIVE_HIGH>;
+			nvidia,ddc-i2c-bus = <&hdmiddc>;
+		};
+	};
+
+	pinmux@70000868 {
+		pinctrl-names = "default";
+		pinctrl-0 = <&state_default>;
+
+		state_default: pinmux {
+			/* Colibri BL_ON */
+			pv2 {
+				nvidia,pins = "pv2";
+				nvidia,function = "rsvd4";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+			};
+
+			/* Colibri Backlight PWM<A> */
+			sdmmc3_dat3_pb4 {
+				nvidia,pins =	"sdmmc3_dat3_pb4";
+				nvidia,function = "pwm0";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+			};
+
+			/* Colibri CAN_INT */
+			kb_row8_ps0 {
+				nvidia,pins = "kb_row8_ps0";
+				nvidia,function = "kbc";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+
+			/*
+			 * Colibri L_BIAS, LCD_M1 is muxed with LCD_DE
+			 * todays display need DE, disable LCD_M1
+			 */
+			lcd_m1_pw1 {
+				nvidia,pins = "lcd_m1_pw1";
+				nvidia,function = "rsvd3";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+
+			/* Thermal alert, need to be disabled */
+			lcd_dc1_pd2 {
+				nvidia,pins = "lcd_dc1_pd2";
+				nvidia,function = "rsvd3";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+				nvidia,enable-input = <TEGRA_PIN_ENABLE>;
+			};
+
+			/* Colibri MMC */
+			kb_row10_ps2 {
+				nvidia,pins = "kb_row10_ps2";
+				nvidia,function = "sdmmc2";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+			};
+			kb_row11_ps3 {
+				nvidia,pins =	"kb_row11_ps3",
+						"kb_row12_ps4",
+						"kb_row13_ps5",
+						"kb_row14_ps6",
+						"kb_row15_ps7";
+				nvidia,function = "sdmmc2";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+			};
+
+			/* Colibri SSP */
+			ulpi_clk_py0 {
+				nvidia,pins =   "ulpi_clk_py0",
+						"ulpi_dir_py1",
+						"ulpi_nxt_py2",
+						"ulpi_stp_py3";
+				nvidia,function = "spi1";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+			};
+			sdmmc3_dat6_pd3 {
+				nvidia,pins =	"sdmmc3_dat6_pd3",
+						"sdmmc3_dat7_pd4";
+				nvidia,function = "spdif";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_ENABLE>;
+			};
+
+			/* Colibri UART_A */
+			ulpi_data0 {
+				nvidia,pins =   "ulpi_data0_po1",
+						"ulpi_data1_po2",
+						"ulpi_data2_po3",
+						"ulpi_data3_po4",
+						"ulpi_data4_po5",
+						"ulpi_data5_po6",
+						"ulpi_data6_po7",
+						"ulpi_data7_po0";
+				nvidia,function = "uarta";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+			};
+
+			/* Colibri UART_B */
+			gmi_a16_pj7 {
+				nvidia,pins =   "gmi_a16_pj7",
+						"gmi_a17_pb0",
+						"gmi_a18_pb1",
+						"gmi_a19_pk7";
+				nvidia,function = "uartd";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+			};
+
+			/* Colibri UART_C */
+			uart2_rxd {
+				nvidia,pins =   "uart2_rxd_pc3",
+						"uart2_txd_pc2";
+				nvidia,function = "uartb";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+			};
+
+			/* eMMC */
+			sdmmc4_clk_pcc4 {
+				nvidia,pins =	"sdmmc4_clk_pcc4",
+						"sdmmc4_rst_n_pcc3";
+				nvidia,function = "sdmmc4";
+				nvidia,pull = <TEGRA_PIN_PULL_NONE>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+			};
+			sdmmc4_dat0_paa0 {
+				nvidia,pins =	"sdmmc4_dat0_paa0",
+						"sdmmc4_dat1_paa1",
+						"sdmmc4_dat2_paa2",
+						"sdmmc4_dat3_paa3",
+						"sdmmc4_dat4_paa4",
+						"sdmmc4_dat5_paa5",
+						"sdmmc4_dat6_paa6",
+						"sdmmc4_dat7_paa7";
+				nvidia,function = "sdmmc4";
+				nvidia,pull = <TEGRA_PIN_PULL_UP>;
+				nvidia,tristate = <TEGRA_PIN_DISABLE>;
+			};
+		};
+	};
+
+	hdmiddc: i2c@7000c700 {
+		clock-frequency = <100000>;
+	};
+
+	/*
+	 * PWR_I2C: power I2C to audio codec, PMIC, temperature sensor and
+	 * touch screen controller
+	 */
+	i2c@7000d000 {
+		status = "okay";
+		clock-frequency = <100000>;
+
+		pmic: tps65911@2d {
+			compatible = "ti,tps65911";
+			reg = <0x2d>;
+
+			interrupts = <GIC_SPI 86 IRQ_TYPE_LEVEL_HIGH>;
+			#interrupt-cells = <2>;
+			interrupt-controller;
+
+			ti,system-power-controller;
+
+			#gpio-cells = <2>;
+			gpio-controller;
+
+			vcc1-supply = <&sys_3v3_reg>;
+			vcc2-supply = <&sys_3v3_reg>;
+			vcc3-supply = <&vio_reg>;
+			vcc4-supply = <&sys_3v3_reg>;
+			vcc5-supply = <&sys_3v3_reg>;
+			vcc6-supply = <&vio_reg>;
+			vcc7-supply = <&sys_5v0_reg>;
+			vccio-supply = <&sys_3v3_reg>;
+
+			regulators {
+				/* SW1: +V1.35_VDDIO_DDR */
+				vdd1_reg: vdd1 {
+					regulator-name = "vddio_ddr_1v35";
+					regulator-min-microvolt = <1350000>;
+					regulator-max-microvolt = <1350000>;
+					regulator-always-on;
+				};
+
+				/* SW2: unused */
+
+				/* SW CTRL: +V1.0_VDD_CPU */
+				vddctrl_reg: vddctrl {
+					regulator-name = "vdd_cpu,vdd_sys";
+					regulator-min-microvolt = <1150000>;
+					regulator-max-microvolt = <1150000>;
+					regulator-always-on;
+				};
+
+				/* SWIO: +V1.8 */
+				vio_reg: vio {
+					regulator-name = "vdd_1v8_gen";
+					regulator-min-microvolt = <1800000>;
+					regulator-max-microvolt = <1800000>;
+					regulator-always-on;
+				};
+
+				/* LDO1: unused */
+
+				/*
+				 * EN_+V3.3 switching via FET:
+				 * +V3.3_AUDIO_AVDD_S, +V3.3 and +V1.8_VDD_LAN
+				 * see also v3_3 fixed supply
+				 */
+				ldo2_reg: ldo2 {
+					regulator-name = "en_3v3";
+					regulator-min-microvolt = <3300000>;
+					regulator-max-microvolt = <3300000>;
+					regulator-always-on;
+				};
+
+				/* LDO3: unused */
+
+				/* +V1.2_VDD_RTC */
+				ldo4_reg: ldo4 {
+					regulator-name = "vdd_rtc";
+					regulator-min-microvolt = <1200000>;
+					regulator-max-microvolt = <1200000>;
+					regulator-always-on;
+				};
+
+				/*
+				 * +V2.8_AVDD_VDAC:
+				 * only required for analog RGB
+				 */
+				ldo5_reg: ldo5 {
+					regulator-name = "avdd_vdac";
+					regulator-min-microvolt = <2800000>;
+					regulator-max-microvolt = <2800000>;
+					regulator-always-on;
+				};
+
+				/*
+				 * +V1.05_AVDD_PLLE: avdd_plle should be 1.05V
+				 * but LDO6 can't set voltage in 50mV
+				 * granularity
+				 */
+				ldo6_reg: ldo6 {
+					regulator-name = "avdd_plle";
+					regulator-min-microvolt = <1100000>;
+					regulator-max-microvolt = <1100000>;
+				};
+
+				/* +V1.2_AVDD_PLL */
+				ldo7_reg: ldo7 {
+					regulator-name = "avdd_pll";
+					regulator-min-microvolt = <1200000>;
+					regulator-max-microvolt = <1200000>;
+					regulator-always-on;
+				};
+
+				/* +V1.0_VDD_DDR_HS */
+				ldo8_reg: ldo8 {
+					regulator-name = "vdd_ddr_hs";
+					regulator-min-microvolt = <1000000>;
+					regulator-max-microvolt = <1000000>;
+					regulator-always-on;
+				};
+			};
+		};
+
+		/*
+		 * LM95245 temperature sensor
+		 * Note: OVERT_N directly connected to PMIC PWRDN
+		 */
+		temp-sensor@4c {
+			compatible = "national,lm95245";
+			reg = <0x4c>;
+		};
+
+		/* SW: +V1.2_VDD_CORE */
+		tps62362@60 {
+			compatible = "ti,tps62362";
+			reg = <0x60>;
+
+			regulator-name = "tps62362-vout";
+			regulator-min-microvolt = <900000>;
+			regulator-max-microvolt = <1400000>;
+			regulator-boot-on;
+			regulator-always-on;
+			ti,vsel0-state-low;
+			/* VSEL1: EN_CORE_DVFS_N low for DVFS */
+			ti,vsel1-state-low;
+		};
+	};
+
+	pmc@7000e400 {
+		nvidia,invert-interrupt;
+		nvidia,suspend-mode = <1>;
+		nvidia,cpu-pwr-good-time = <5000>;
+		nvidia,cpu-pwr-off-time = <5000>;
+		nvidia,core-pwr-good-time = <3845 3845>;
+		nvidia,core-pwr-off-time = <0>;
+		nvidia,core-power-req-active-high;
+		nvidia,sys-clock-req-active-high;
+	};
+
+	emmc: sdhci@78000600 {
+		status = "okay";
+		bus-width = <8>;
+		non-removable;
+	};
+
+	/* EHCI instance 1: USB2_DP/N -> AX88772B */
+	usb@7d004000 {
+		status = "okay";
+	};
+
+	usb-phy@7d004000 {
+		status = "okay";
+		nvidia,is-wired = <1>;
+	};
+
+	clocks {
+		compatible = "simple-bus";
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		clk32k_in: clk@0 {
+			compatible = "fixed-clock";
+			reg=<0>;
+			#clock-cells = <0>;
+			clock-frequency = <32768>;
+		};
+	};
+
+	regulators {
+		compatible = "simple-bus";
+		#address-cells = <1>;
+		#size-cells = <0>;
+
+		sys_3v3_reg: regulator@100 {
+			compatible = "regulator-fixed";
+			reg = <100>;
+			regulator-name = "3v3";
+			regulator-min-microvolt = <3300000>;
+			regulator-max-microvolt = <3300000>;
+			regulator-always-on;
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/twl4030.dtsi b/arch/arm/boot/dts/twl4030.dtsi
index 86cfc7d15ca..36ae9160b55 100644
--- a/arch/arm/boot/dts/twl4030.dtsi
+++ b/arch/arm/boot/dts/twl4030.dtsi
@@ -152,4 +152,10 @@
 		keypad,num-rows = <8>;
 		keypad,num-columns = <8>;
 	};
+
+	twl_madc: madc {
+		compatible = "ti,twl4030-madc";
+		interrupts = <3>;
+		#io-channel-cells = <1>;
+	};
 };
diff --git a/arch/arm/boot/dts/twl4030_omap3.dtsi b/arch/arm/boot/dts/twl4030_omap3.dtsi
index c353ef0a6ac..3537ae5b214 100644
--- a/arch/arm/boot/dts/twl4030_omap3.dtsi
+++ b/arch/arm/boot/dts/twl4030_omap3.dtsi
@@ -8,7 +8,7 @@
 
 &twl {
 	pinctrl-names = "default";
-	pinctrl-0 = <&twl4030_pins>;
+	pinctrl-0 = <&twl4030_pins &twl4030_vpins>;
 };
 
 &omap3_pmx_core {
@@ -23,3 +23,20 @@
 		>;
 	};
 };
+
+/*
+ * If your board is not using the I2C4 pins with twl4030, then don't include
+ * this file. For proper idle mode signaling with sys_clkreq and sys_off_mode
+ * pins we need to configure I2C4, or else use the legacy sys_nvmode1 and
+ * sys_nvmode2 signaling.
+ */
+&omap3_pmx_wkup {
+	twl4030_vpins: pinmux_twl4030_vpins {
+		pinctrl-single,pins = <
+			OMAP3_WKUP_IOPAD(0x2a00, PIN_INPUT | MUX_MODE0)		/* i2c4_scl.i2c4_scl */
+			OMAP3_WKUP_IOPAD(0x2a02, PIN_INPUT | MUX_MODE0)		/* i2c4_sda.i2c4_sda */
+			OMAP3_WKUP_IOPAD(0x2a06, PIN_OUTPUT | MUX_MODE0)	/* sys_clkreq.sys_clkreq */
+			OMAP3_WKUP_IOPAD(0x2a18, PIN_OUTPUT | MUX_MODE0)	/* sys_off_mode.sys_off_mode */
+		>;
+	};
+};
diff --git a/arch/arm/boot/dts/vexpress-v2m-rs1.dtsi b/arch/arm/boot/dts/vexpress-v2m-rs1.dtsi
index ac870fb3fa0..756c986995a 100644
--- a/arch/arm/boot/dts/vexpress-v2m-rs1.dtsi
+++ b/arch/arm/boot/dts/vexpress-v2m-rs1.dtsi
@@ -74,8 +74,24 @@
 			v2m_sysreg: sysreg@010000 {
 				compatible = "arm,vexpress-sysreg";
 				reg = <0x010000 0x1000>;
-				gpio-controller;
-				#gpio-cells = <2>;
+
+				v2m_led_gpios: sys_led@08 {
+					compatible = "arm,vexpress-sysreg,sys_led";
+					gpio-controller;
+					#gpio-cells = <2>;
+				};
+
+				v2m_mmc_gpios: sys_mci@48 {
+					compatible = "arm,vexpress-sysreg,sys_mci";
+					gpio-controller;
+					#gpio-cells = <2>;
+				};
+
+				v2m_flash_gpios: sys_flash@4c {
+					compatible = "arm,vexpress-sysreg,sys_flash";
+					gpio-controller;
+					#gpio-cells = <2>;
+				};
 			};
 
 			v2m_sysctl: sysctl@020000 {
@@ -113,8 +129,8 @@
 				compatible = "arm,pl180", "arm,primecell";
 				reg = <0x050000 0x1000>;
 				interrupts = <9 10>;
-				cd-gpios = <&v2m_sysreg 0 0>;
-				wp-gpios = <&v2m_sysreg 1 0>;
+				cd-gpios = <&v2m_mmc_gpios 0 0>;
+				wp-gpios = <&v2m_mmc_gpios 1 0>;
 				max-frequency = <12000000>;
 				vmmc-supply = <&v2m_fixed_3v3>;
 				clocks = <&v2m_clk24mhz>, <&smbclk>;
@@ -265,6 +281,58 @@
 			clock-output-names = "v2m:refclk32khz";
 		};
 
+		leds {
+			compatible = "gpio-leds";
+
+			user@1 {
+				label = "v2m:green:user1";
+				gpios = <&v2m_led_gpios 0 0>;
+				linux,default-trigger = "heartbeat";
+			};
+
+			user@2 {
+				label = "v2m:green:user2";
+				gpios = <&v2m_led_gpios 1 0>;
+				linux,default-trigger = "mmc0";
+			};
+
+			user@3 {
+				label = "v2m:green:user3";
+				gpios = <&v2m_led_gpios 2 0>;
+				linux,default-trigger = "cpu0";
+			};
+
+			user@4 {
+				label = "v2m:green:user4";
+				gpios = <&v2m_led_gpios 3 0>;
+				linux,default-trigger = "cpu1";
+			};
+
+			user@5 {
+				label = "v2m:green:user5";
+				gpios = <&v2m_led_gpios 4 0>;
+				linux,default-trigger = "cpu2";
+			};
+
+			user@6 {
+				label = "v2m:green:user6";
+				gpios = <&v2m_led_gpios 5 0>;
+				linux,default-trigger = "cpu3";
+			};
+
+			user@7 {
+				label = "v2m:green:user7";
+				gpios = <&v2m_led_gpios 6 0>;
+				linux,default-trigger = "cpu4";
+			};
+
+			user@8 {
+				label = "v2m:green:user8";
+				gpios = <&v2m_led_gpios 7 0>;
+				linux,default-trigger = "cpu5";
+			};
+		};
+
 		mcc {
 			compatible = "arm,vexpress,config-bus";
 			arm,vexpress,config-bridge = <&v2m_sysreg>;
diff --git a/arch/arm/boot/dts/vexpress-v2m.dtsi b/arch/arm/boot/dts/vexpress-v2m.dtsi
index f1420368355..ba856d604fb 100644
--- a/arch/arm/boot/dts/vexpress-v2m.dtsi
+++ b/arch/arm/boot/dts/vexpress-v2m.dtsi
@@ -73,8 +73,24 @@
 			v2m_sysreg: sysreg@00000 {
 				compatible = "arm,vexpress-sysreg";
 				reg = <0x00000 0x1000>;
-				gpio-controller;
-				#gpio-cells = <2>;
+
+				v2m_led_gpios: sys_led@08 {
+					compatible = "arm,vexpress-sysreg,sys_led";
+					gpio-controller;
+					#gpio-cells = <2>;
+				};
+
+				v2m_mmc_gpios: sys_mci@48 {
+					compatible = "arm,vexpress-sysreg,sys_mci";
+					gpio-controller;
+					#gpio-cells = <2>;
+				};
+
+				v2m_flash_gpios: sys_flash@4c {
+					compatible = "arm,vexpress-sysreg,sys_flash";
+					gpio-controller;
+					#gpio-cells = <2>;
+				};
 			};
 
 			v2m_sysctl: sysctl@01000 {
@@ -112,8 +128,8 @@
 				compatible = "arm,pl180", "arm,primecell";
 				reg = <0x05000 0x1000>;
 				interrupts = <9 10>;
-				cd-gpios = <&v2m_sysreg 0 0>;
-				wp-gpios = <&v2m_sysreg 1 0>;
+				cd-gpios = <&v2m_mmc_gpios 0 0>;
+				wp-gpios = <&v2m_mmc_gpios 1 0>;
 				max-frequency = <12000000>;
 				vmmc-supply = <&v2m_fixed_3v3>;
 				clocks = <&v2m_clk24mhz>, <&smbclk>;
@@ -264,6 +280,58 @@
 			clock-output-names = "v2m:refclk32khz";
 		};
 
+		leds {
+			compatible = "gpio-leds";
+
+			user@1 {
+				label = "v2m:green:user1";
+				gpios = <&v2m_led_gpios 0 0>;
+				linux,default-trigger = "heartbeat";
+			};
+
+			user@2 {
+				label = "v2m:green:user2";
+				gpios = <&v2m_led_gpios 1 0>;
+				linux,default-trigger = "mmc0";
+			};
+
+			user@3 {
+				label = "v2m:green:user3";
+				gpios = <&v2m_led_gpios 2 0>;
+				linux,default-trigger = "cpu0";
+			};
+
+			user@4 {
+				label = "v2m:green:user4";
+				gpios = <&v2m_led_gpios 3 0>;
+				linux,default-trigger = "cpu1";
+			};
+
+			user@5 {
+				label = "v2m:green:user5";
+				gpios = <&v2m_led_gpios 4 0>;
+				linux,default-trigger = "cpu2";
+			};
+
+			user@6 {
+				label = "v2m:green:user6";
+				gpios = <&v2m_led_gpios 5 0>;
+				linux,default-trigger = "cpu3";
+			};
+
+			user@7 {
+				label = "v2m:green:user7";
+				gpios = <&v2m_led_gpios 6 0>;
+				linux,default-trigger = "cpu4";
+			};
+
+			user@8 {
+				label = "v2m:green:user8";
+				gpios = <&v2m_led_gpios 7 0>;
+				linux,default-trigger = "cpu5";
+			};
+		};
+
 		mcc {
 			compatible = "arm,vexpress,config-bus";
 			arm,vexpress,config-bridge = <&v2m_sysreg>;
diff --git a/arch/arm/boot/dts/vexpress-v2p-ca15_a7.dts b/arch/arm/boot/dts/vexpress-v2p-ca15_a7.dts
index 15f98cbcb75..a25c262326d 100644
--- a/arch/arm/boot/dts/vexpress-v2p-ca15_a7.dts
+++ b/arch/arm/boot/dts/vexpress-v2p-ca15_a7.dts
@@ -312,6 +312,7 @@
 			arm,vexpress-sysreg,func = <12 0>;
 			label = "A15 Pcore";
 		};
+
 		power@1 {
 			/* Total power for the three A7 cores */
 			compatible = "arm,vexpress-power";
@@ -322,14 +323,14 @@
 		energy@0 {
 			/* Total energy for the two A15 cores */
 			compatible = "arm,vexpress-energy";
-			arm,vexpress-sysreg,func = <13 0>;
+			arm,vexpress-sysreg,func = <13 0>, <13 1>;
 			label = "A15 Jcore";
 		};
 
 		energy@2 {
 			/* Total energy for the three A7 cores */
 			compatible = "arm,vexpress-energy";
-			arm,vexpress-sysreg,func = <13 2>;
+			arm,vexpress-sysreg,func = <13 2>, <13 3>;
 			label = "A7 Jcore";
 		};
 	};
diff --git a/arch/arm/boot/dts/vf610-colibri.dts b/arch/arm/boot/dts/vf610-colibri.dts
new file mode 100644
index 00000000000..aecc7dbc65e
--- /dev/null
+++ b/arch/arm/boot/dts/vf610-colibri.dts
@@ -0,0 +1,123 @@
+/*
+ * Copyright 2014 Toradex AG
+ *
+ * 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.
+ */
+
+/dts-v1/;
+#include "vf610.dtsi"
+
+/ {
+	model = "Toradex Colibri VF61 COM";
+	compatible = "toradex,vf610-colibri", "fsl,vf610";
+
+	chosen {
+		bootargs = "console=ttyLP0,115200";
+	};
+
+	memory {
+		reg = <0x80000000 0x10000000>;
+	};
+
+	clocks {
+		enet_ext {
+			compatible = "fixed-clock";
+			#clock-cells = <0>;
+			clock-frequency = <50000000>;
+		};
+	};
+
+};
+
+&esdhc1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_esdhc1>;
+	bus-width = <4>;
+	status = "okay";
+};
+
+&fec1 {
+	phy-mode = "rmii";
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_fec1>;
+	status = "okay";
+};
+
+&L2 {
+	arm,data-latency = <2 1 2>;
+	arm,tag-latency = <3 2 3>;
+};
+
+&uart0 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_uart0>;
+	status = "okay";
+};
+
+&uart1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_uart1>;
+	status = "okay";
+};
+
+&uart2 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_uart2>;
+	status = "okay";
+};
+
+&iomuxc {
+	vf610-colibri {
+		pinctrl_esdhc1: esdhc1grp {
+			fsl,fsl,pins = <
+				VF610_PAD_PTA24__ESDHC1_CLK	0x31ef
+				VF610_PAD_PTA25__ESDHC1_CMD	0x31ef
+				VF610_PAD_PTA26__ESDHC1_DAT0	0x31ef
+				VF610_PAD_PTA27__ESDHC1_DAT1	0x31ef
+				VF610_PAD_PTA28__ESDHC1_DATA2	0x31ef
+				VF610_PAD_PTA29__ESDHC1_DAT3	0x31ef
+				VF610_PAD_PTB20__GPIO_42	0x219d
+			>;
+		};
+
+		pinctrl_fec1: fec1grp {
+			fsl,pins = <
+				VF610_PAD_PTC9__ENET_RMII1_MDC		0x30d2
+				VF610_PAD_PTC10__ENET_RMII1_MDIO	0x30d3
+				VF610_PAD_PTC11__ENET_RMII1_CRS		0x30d1
+				VF610_PAD_PTC12__ENET_RMII_RXD1		0x30d1
+				VF610_PAD_PTC13__ENET_RMII1_RXD0	0x30d1
+				VF610_PAD_PTC14__ENET_RMII1_RXER	0x30d1
+				VF610_PAD_PTC15__ENET_RMII1_TXD1	0x30d2
+				VF610_PAD_PTC16__ENET_RMII1_TXD0	0x30d2
+				VF610_PAD_PTC17__ENET_RMII1_TXEN	0x30d2
+			>;
+		};
+
+		pinctrl_uart0: uart0grp {
+			fsl,pins = <
+				VF610_PAD_PTB10__UART0_TX		0x21a2
+				VF610_PAD_PTB11__UART0_RX		0x21a1
+			>;
+		};
+
+		pinctrl_uart1: uart1grp {
+			fsl,pins = <
+				VF610_PAD_PTB4__UART1_TX		0x21a2
+				VF610_PAD_PTB5__UART1_RX		0x21a1
+			>;
+		};
+
+		pinctrl_uart2: uart2grp {
+			fsl,pins = <
+				VF610_PAD_PTD0__UART2_TX		0x21a2
+				VF610_PAD_PTD1__UART2_RX		0x21a1
+				VF610_PAD_PTD2__UART2_RTS		0x21a2
+				VF610_PAD_PTD3__UART2_CTS		0x21a1
+			>;
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/vf610-twr.dts b/arch/arm/boot/dts/vf610-twr.dts
index 7dd1d6ede52..11d733406c7 100644
--- a/arch/arm/boot/dts/vf610-twr.dts
+++ b/arch/arm/boot/dts/vf610-twr.dts
@@ -25,11 +25,13 @@
 	clocks {
 		audio_ext {
 			compatible = "fixed-clock";
+			#clock-cells = <0>;
 			clock-frequency = <24576000>;
 		};
 
 		enet_ext {
 			compatible = "fixed-clock";
+			#clock-cells = <0>;
 			clock-frequency = <50000000>;
 		};
 	};
@@ -111,6 +113,13 @@
 	};
 };
 
+&esdhc1 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_esdhc1>;
+	bus-width = <4>;
+	status = "okay";
+};
+
 &fec0 {
 	phy-mode = "rmii";
 	pinctrl-names = "default";
@@ -158,6 +167,18 @@
 			>;
 		};
 
+		pinctrl_esdhc1: esdhc1grp {
+			fsl,fsl,pins = <
+				VF610_PAD_PTA24__ESDHC1_CLK	0x31ef
+				VF610_PAD_PTA25__ESDHC1_CMD	0x31ef
+				VF610_PAD_PTA26__ESDHC1_DAT0	0x31ef
+				VF610_PAD_PTA27__ESDHC1_DAT1	0x31ef
+				VF610_PAD_PTA28__ESDHC1_DATA2	0x31ef
+				VF610_PAD_PTA29__ESDHC1_DAT3	0x31ef
+				VF610_PAD_PTA7__GPIO_134	0x219d
+			>;
+		};
+
 		pinctrl_fec0: fec0grp {
 			fsl,pins = <
 				VF610_PAD_PTA6__RMII_CLKIN		0x30d1
@@ -194,6 +215,17 @@
 			>;
 		};
 
+		pinctrl_pwm0: pwm0grp {
+			fsl,pins = <
+				VF610_PAD_PTB0__FTM0_CH0		0x1582
+				VF610_PAD_PTB1__FTM0_CH1		0x1582
+				VF610_PAD_PTB2__FTM0_CH2		0x1582
+				VF610_PAD_PTB3__FTM0_CH3		0x1582
+				VF610_PAD_PTB6__FTM0_CH6		0x1582
+				VF610_PAD_PTB7__FTM0_CH7		0x1582
+			>;
+		};
+
 		pinctrl_sai2: sai2grp {
 			fsl,pins = <
 				VF610_PAD_PTA16__SAI2_TX_BCLK		0x02ed
@@ -215,6 +247,12 @@
 	};
 };
 
+&pwm0 {
+	pinctrl-names = "default";
+	pinctrl-0 = <&pinctrl_pwm0>;
+	status = "okay";
+};
+
 &sai2 {
 	#sound-dai-cells = <0>;
 	pinctrl-names = "default";
diff --git a/arch/arm/boot/dts/vf610.dtsi b/arch/arm/boot/dts/vf610.dtsi
index 80487336766..73355ddc518 100644
--- a/arch/arm/boot/dts/vf610.dtsi
+++ b/arch/arm/boot/dts/vf610.dtsi
@@ -45,11 +45,13 @@
 
 		sxosc {
 			compatible = "fixed-clock";
+			#clock-cells = <0>;
 			clock-frequency = <32768>;
 		};
 
 		fxosc {
 			compatible = "fixed-clock";
+			#clock-cells = <0>;
 			clock-frequency = <24000000>;
 		};
 	};
@@ -72,8 +74,6 @@
 			intc: interrupt-controller@40002000 {
 				compatible = "arm,cortex-a9-gic";
 				#interrupt-cells = <3>;
-				#address-cells = <1>;
-				#size-cells = <1>;
 				interrupt-controller;
 				reg = <0x40003000 0x1000>,
 				      <0x40002100 0x100>;
@@ -183,6 +183,19 @@
 				clock-names = "pit";
 			};
 
+			pwm0: pwm@40038000 {
+				compatible = "fsl,vf610-ftm-pwm";
+				#pwm-cells = <3>;
+				reg = <0x40038000 0x1000>;
+				clock-names = "ftm_sys", "ftm_ext",
+					      "ftm_fix", "ftm_cnt_clk_en";
+				clocks = <&clks VF610_CLK_FTM0>,
+					<&clks VF610_CLK_FTM0_EXT_SEL>,
+					<&clks VF610_CLK_FTM0_FIX_SEL>,
+					<&clks VF610_CLK_FTM0_EXT_FIX_EN>;
+				status = "disabled";
+			};
+
 			adc0: adc@4003b000 {
 				compatible = "fsl,vf610-adc";
 				reg = <0x4003b000 0x1000>;
@@ -347,6 +360,17 @@
 				status = "disabled";
 			};
 
+			esdhc1: esdhc@400b2000 {
+				compatible = "fsl,imx53-esdhc";
+				reg = <0x400b2000 0x4000>;
+				interrupts = <0 28 0x04>;
+				clocks = <&clks VF610_CLK_IPG_BUS>,
+					<&clks VF610_CLK_PLATFORM_BUS>,
+					<&clks VF610_CLK_ESDHC1>;
+				clock-names = "ipg", "ahb", "per";
+				status = "disabled";
+			};
+
 			fec0: ethernet@400d0000 {
 				compatible = "fsl,mvf600-fec";
 				reg = <0x400d0000 0x1000>;
diff --git a/arch/arm/boot/dts/zynq-7000.dtsi b/arch/arm/boot/dts/zynq-7000.dtsi
index 511180769af..760bbc463c5 100644
--- a/arch/arm/boot/dts/zynq-7000.dtsi
+++ b/arch/arm/boot/dts/zynq-7000.dtsi
@@ -1,5 +1,5 @@
 /*
- *  Copyright (C) 2011 Xilinx
+ *  Copyright (C) 2011 - 2014 Xilinx
  *
  * This software is licensed under the terms of the GNU General Public
  * License version 2, as published by the Free Software Foundation, and
@@ -24,6 +24,8 @@
 			device_type = "cpu";
 			reg = <0>;
 			clocks = <&clkc 3>;
+			clock-latency = <1000>;
+			cpu0-supply = <&regulator_vccpint>;
 			operating-points = <
 				/* kHz    uV */
 				666667  1000000
@@ -47,6 +49,15 @@
 		reg = < 0xf8891000 0x1000 0xf8893000 0x1000 >;
 	};
 
+	regulator_vccpint: fixedregulator@0 {
+		compatible = "regulator-fixed";
+		regulator-name = "VCCPINT";
+		regulator-min-microvolt = <1000000>;
+		regulator-max-microvolt = <1000000>;
+		regulator-boot-on;
+		regulator-always-on;
+	};
+
 	amba {
 		compatible = "simple-bus";
 		#address-cells = <1>;
@@ -54,10 +65,31 @@
 		interrupt-parent = <&intc>;
 		ranges;
 
+		i2c0: i2c@e0004000 {
+			compatible = "cdns,i2c-r1p10";
+			status = "disabled";
+			clocks = <&clkc 38>;
+			interrupt-parent = <&intc>;
+			interrupts = <0 25 4>;
+			reg = <0xe0004000 0x1000>;
+			#address-cells = <1>;
+			#size-cells = <0>;
+		};
+
+		i2c1: i2c@e0005000 {
+			compatible = "cdns,i2c-r1p10";
+			status = "disabled";
+			clocks = <&clkc 39>;
+			interrupt-parent = <&intc>;
+			interrupts = <0 48 4>;
+			reg = <0xe0005000 0x1000>;
+			#address-cells = <1>;
+			#size-cells = <0>;
+		};
+
 		intc: interrupt-controller@f8f01000 {
 			compatible = "arm,cortex-a9-gic";
 			#interrupt-cells = <3>;
-			#address-cells = <1>;
 			interrupt-controller;
 			reg = <0xF8F01000 0x1000>,
 			      <0xF8F00100 0x100>;
@@ -72,7 +104,7 @@
 			cache-level = <2>;
 		};
 
-		uart0: uart@e0000000 {
+		uart0: serial@e0000000 {
 			compatible = "xlnx,xuartps";
 			status = "disabled";
 			clocks = <&clkc 23>, <&clkc 40>;
@@ -81,7 +113,7 @@
 			interrupts = <0 27 4>;
 		};
 
-		uart1: uart@e0001000 {
+		uart1: serial@e0001000 {
 			compatible = "xlnx,xuartps";
 			status = "disabled";
 			clocks = <&clkc 24>, <&clkc 41>;
@@ -108,7 +140,7 @@
 			clock-names = "pclk", "hclk", "tx_clk";
 		};
 
-		sdhci0: ps7-sdhci@e0100000 {
+		sdhci0: sdhci@e0100000 {
 			compatible = "arasan,sdhci-8.9a";
 			status = "disabled";
 			clock-names = "clk_xin", "clk_ahb";
@@ -118,7 +150,7 @@
 			reg = <0xe0100000 0x1000>;
 		} ;
 
-		sdhci1: ps7-sdhci@e0101000 {
+		sdhci1: sdhci@e0101000 {
 			compatible = "arasan,sdhci-8.9a";
 			status = "disabled";
 			clock-names = "clk_xin", "clk_ahb";
@@ -154,6 +186,11 @@
 			};
 		};
 
+		devcfg: devcfg@f8007000 {
+			compatible = "xlnx,zynq-devcfg-1.0";
+			reg = <0xf8007000 0x100>;
+		} ;
+
 		global_timer: timer@f8f00200 {
 			compatible = "arm,cortex-a9-global-timer";
 			reg = <0xf8f00200 0x20>;
@@ -162,26 +199,27 @@
 			clocks = <&clkc 4>;
 		};
 
-		ttc0: ttc0@f8001000 {
+		ttc0: timer@f8001000 {
 			interrupt-parent = <&intc>;
-			interrupts = < 0 10 4 0 11 4 0 12 4 >;
+			interrupts = <0 10 4>, <0 11 4>, <0 12 4>;
 			compatible = "cdns,ttc";
 			clocks = <&clkc 6>;
 			reg = <0xF8001000 0x1000>;
 		};
 
-		ttc1: ttc1@f8002000 {
+		ttc1: timer@f8002000 {
 			interrupt-parent = <&intc>;
-			interrupts = < 0 37 4 0 38 4 0 39 4 >;
+			interrupts = <0 37 4>, <0 38 4>, <0 39 4>;
 			compatible = "cdns,ttc";
 			clocks = <&clkc 6>;
 			reg = <0xF8002000 0x1000>;
 		};
-		scutimer: scutimer@f8f00600 {
+
+		scutimer: timer@f8f00600 {
 			interrupt-parent = <&intc>;
-			interrupts = < 1 13 0x301 >;
+			interrupts = <1 13 0x301>;
 			compatible = "arm,cortex-a9-twd-timer";
-			reg = < 0xf8f00600 0x20 >;
+			reg = <0xf8f00600 0x20>;
 			clocks = <&clkc 4>;
 		} ;
 	};
diff --git a/arch/arm/boot/dts/zynq-zc702.dts b/arch/arm/boot/dts/zynq-zc702.dts
index c913f77a21e..5e09cee33d4 100644
--- a/arch/arm/boot/dts/zynq-zc702.dts
+++ b/arch/arm/boot/dts/zynq-zc702.dts
@@ -34,6 +34,82 @@
 	phy-mode = "rgmii";
 };
 
+&i2c0 {
+	status = "okay";
+	clock-frequency = <400000>;
+
+	i2cswitch@74 {
+		compatible = "nxp,pca9548";
+		#address-cells = <1>;
+		#size-cells = <0>;
+		reg = <0x74>;
+
+		i2c@0 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			reg = <0>;
+			si570: clock-generator@5d {
+				#clock-cells = <0>;
+				compatible = "silabs,si570";
+				temperature-stability = <50>;
+				reg = <0x5d>;
+				factory-fout = <156250000>;
+				clock-frequency = <148500000>;
+			};
+		};
+
+		i2c@2 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			reg = <2>;
+			eeprom@54 {
+				compatible = "at,24c08";
+				reg = <0x54>;
+			};
+		};
+
+		i2c@3 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			reg = <3>;
+			gpio@21 {
+				compatible = "ti,tca6416";
+				reg = <0x21>;
+				gpio-controller;
+				#gpio-cells = <2>;
+			};
+		};
+
+		i2c@4 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			reg = <4>;
+			rtc@51 {
+				compatible = "nxp,pcf8563";
+				reg = <0x51>;
+			};
+		};
+
+		i2c@7 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			reg = <7>;
+			hwmon@52 {
+				compatible = "ti,ucd9248";
+				reg = <52>;
+			};
+			hwmon@53 {
+				compatible = "ti,ucd9248";
+				reg = <53>;
+			};
+			hwmon@54 {
+				compatible = "ti,ucd9248";
+				reg = <54>;
+			};
+		};
+	};
+};
+
 &sdhci0 {
 	status = "okay";
 };
diff --git a/arch/arm/boot/dts/zynq-zc706.dts b/arch/arm/boot/dts/zynq-zc706.dts
index 88f62c50382..4cc9913078c 100644
--- a/arch/arm/boot/dts/zynq-zc706.dts
+++ b/arch/arm/boot/dts/zynq-zc706.dts
@@ -35,6 +35,74 @@
 	phy-mode = "rgmii";
 };
 
+&i2c0 {
+	status = "okay";
+	clock-frequency = <400000>;
+
+	i2cswitch@74 {
+		compatible = "nxp,pca9548";
+		#address-cells = <1>;
+		#size-cells = <0>;
+		reg = <0x74>;
+
+		i2c@0 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			reg = <0>;
+			si570: clock-generator@5d {
+				#clock-cells = <0>;
+				compatible = "silabs,si570";
+				temperature-stability = <50>;
+				reg = <0x5d>;
+				factory-fout = <156250000>;
+				clock-frequency = <148500000>;
+			};
+		};
+
+		i2c@2 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			reg = <2>;
+			eeprom@54 {
+				compatible = "at,24c08";
+				reg = <0x54>;
+			};
+		};
+
+		i2c@3 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			reg = <3>;
+			gpio@21 {
+				compatible = "ti,tca6416";
+				reg = <0x21>;
+				gpio-controller;
+				#gpio-cells = <2>;
+			};
+		};
+
+		i2c@4 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			reg = <4>;
+			rtc@51 {
+				compatible = "nxp,pcf8563";
+				reg = <0x51>;
+			};
+		};
+
+		i2c@7 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			reg = <7>;
+			ucd90120@65 {
+				compatible = "ti,ucd90120";
+				reg = <0x65>;
+			};
+		};
+	};
+};
+
 &sdhci0 {
 	status = "okay";
 };
diff --git a/arch/arm/common/bL_switcher.c b/arch/arm/common/bL_switcher.c
index 5774b6ea7ad..490f3dced74 100644
--- a/arch/arm/common/bL_switcher.c
+++ b/arch/arm/common/bL_switcher.c
@@ -433,8 +433,12 @@ static void bL_switcher_restore_cpus(void)
 {
 	int i;
 
-	for_each_cpu(i, &bL_switcher_removed_logical_cpus)
-		cpu_up(i);
+	for_each_cpu(i, &bL_switcher_removed_logical_cpus) {
+		struct device *cpu_dev = get_cpu_device(i);
+		int ret = device_online(cpu_dev);
+		if (ret)
+			dev_err(cpu_dev, "switcher: unable to restore CPU\n");
+	}
 }
 
 static int bL_switcher_halve_cpus(void)
@@ -521,7 +525,7 @@ static int bL_switcher_halve_cpus(void)
 			continue;
 		}
 
-		ret = cpu_down(i);
+		ret = device_offline(get_cpu_device(i));
 		if (ret) {
 			bL_switcher_restore_cpus();
 			return ret;
@@ -797,10 +801,8 @@ static int __init bL_switcher_init(void)
 {
 	int ret;
 
-	if (MAX_NR_CLUSTERS != 2) {
-		pr_err("%s: only dual cluster systems are supported\n", __func__);
-		return -EINVAL;
-	}
+	if (!mcpm_is_available())
+		return -ENODEV;
 
 	cpu_notifier(bL_switcher_hotplug_callback, 0);
 
diff --git a/arch/arm/common/edma.c b/arch/arm/common/edma.c
index 41bca32409f..485be42519b 100644
--- a/arch/arm/common/edma.c
+++ b/arch/arm/common/edma.c
@@ -102,7 +102,13 @@
 #define PARM_OFFSET(param_no)	(EDMA_PARM + ((param_no) << 5))
 
 #define EDMA_DCHMAP	0x0100  /* 64 registers */
-#define CHMAP_EXIST	BIT(24)
+
+/* CCCFG register */
+#define GET_NUM_DMACH(x)	(x & 0x7) /* bits 0-2 */
+#define GET_NUM_PAENTRY(x)	((x & 0x7000) >> 12) /* bits 12-14 */
+#define GET_NUM_EVQUE(x)	((x & 0x70000) >> 16) /* bits 16-18 */
+#define GET_NUM_REGN(x)		((x & 0x300000) >> 20) /* bits 20-21 */
+#define CHMAP_EXIST		BIT(24)
 
 #define EDMA_MAX_DMACH           64
 #define EDMA_MAX_PARAMENTRY     512
@@ -233,7 +239,6 @@ struct edma {
 	unsigned	num_region;
 	unsigned	num_slots;
 	unsigned	num_tc;
-	unsigned	num_cc;
 	enum dma_event_q 	default_queue;
 
 	/* list of channels with no even trigger; terminated by "-1" */
@@ -290,12 +295,6 @@ static void map_dmach_queue(unsigned ctlr, unsigned ch_no,
 			~(0x7 << bit), queue_no << bit);
 }
 
-static void __init map_queue_tc(unsigned ctlr, int queue_no, int tc_no)
-{
-	int bit = queue_no * 4;
-	edma_modify(ctlr, EDMA_QUETCMAP, ~(0x7 << bit), ((tc_no & 0x7) << bit));
-}
-
 static void __init assign_priority_to_queue(unsigned ctlr, int queue_no,
 		int priority)
 {
@@ -994,29 +993,23 @@ void edma_set_dest(unsigned slot, dma_addr_t dest_port,
 EXPORT_SYMBOL(edma_set_dest);
 
 /**
- * edma_get_position - returns the current transfer points
+ * edma_get_position - returns the current transfer point
  * @slot: parameter RAM slot being examined
- * @src: pointer to source port position
- * @dst: pointer to destination port position
+ * @dst:  true selects the dest position, false the source
  *
- * Returns current source and destination addresses for a particular
- * parameter RAM slot.  Its channel should not be active when this is called.
+ * Returns the position of the current active slot
  */
-void edma_get_position(unsigned slot, dma_addr_t *src, dma_addr_t *dst)
+dma_addr_t edma_get_position(unsigned slot, bool dst)
 {
-	struct edmacc_param temp;
-	unsigned ctlr;
+	u32 offs, ctlr = EDMA_CTLR(slot);
 
-	ctlr = EDMA_CTLR(slot);
 	slot = EDMA_CHAN_SLOT(slot);
 
-	edma_read_slot(EDMA_CTLR_CHAN(ctlr, slot), &temp);
-	if (src != NULL)
-		*src = temp.src;
-	if (dst != NULL)
-		*dst = temp.dst;
+	offs = PARM_OFFSET(slot);
+	offs += dst ? PARM_DST : PARM_SRC;
+
+	return edma_read(ctlr, offs);
 }
-EXPORT_SYMBOL(edma_get_position);
 
 /**
  * edma_set_src_index - configure DMA source address indexing
@@ -1421,57 +1414,101 @@ void edma_clear_event(unsigned channel)
 }
 EXPORT_SYMBOL(edma_clear_event);
 
-#if IS_ENABLED(CONFIG_OF) && IS_ENABLED(CONFIG_DMADEVICES)
-
-static int edma_of_read_u32_to_s16_array(const struct device_node *np,
-					 const char *propname, s16 *out_values,
-					 size_t sz)
+static int edma_setup_from_hw(struct device *dev, struct edma_soc_info *pdata,
+			      struct edma *edma_cc)
 {
-	int ret;
+	int i;
+	u32 value, cccfg;
+	s8 (*queue_priority_map)[2];
 
-	ret = of_property_read_u16_array(np, propname, out_values, sz);
-	if (ret)
-		return ret;
+	/* Decode the eDMA3 configuration from CCCFG register */
+	cccfg = edma_read(0, EDMA_CCCFG);
+
+	value = GET_NUM_REGN(cccfg);
+	edma_cc->num_region = BIT(value);
+
+	value = GET_NUM_DMACH(cccfg);
+	edma_cc->num_channels = BIT(value + 1);
+
+	value = GET_NUM_PAENTRY(cccfg);
+	edma_cc->num_slots = BIT(value + 4);
 
-	/* Terminate it */
-	*out_values++ = -1;
-	*out_values++ = -1;
+	value = GET_NUM_EVQUE(cccfg);
+	edma_cc->num_tc = value + 1;
+
+	dev_dbg(dev, "eDMA3 HW configuration (cccfg: 0x%08x):\n", cccfg);
+	dev_dbg(dev, "num_region: %u\n", edma_cc->num_region);
+	dev_dbg(dev, "num_channel: %u\n", edma_cc->num_channels);
+	dev_dbg(dev, "num_slot: %u\n", edma_cc->num_slots);
+	dev_dbg(dev, "num_tc: %u\n", edma_cc->num_tc);
+
+	/* Nothing need to be done if queue priority is provided */
+	if (pdata->queue_priority_mapping)
+		return 0;
+
+	/*
+	 * Configure TC/queue priority as follows:
+	 * Q0 - priority 0
+	 * Q1 - priority 1
+	 * Q2 - priority 2
+	 * ...
+	 * The meaning of priority numbers: 0 highest priority, 7 lowest
+	 * priority. So Q0 is the highest priority queue and the last queue has
+	 * the lowest priority.
+	 */
+	queue_priority_map = devm_kzalloc(dev,
+					  (edma_cc->num_tc + 1) * sizeof(s8),
+					  GFP_KERNEL);
+	if (!queue_priority_map)
+		return -ENOMEM;
+
+	for (i = 0; i < edma_cc->num_tc; i++) {
+		queue_priority_map[i][0] = i;
+		queue_priority_map[i][1] = i;
+	}
+	queue_priority_map[i][0] = -1;
+	queue_priority_map[i][1] = -1;
+
+	pdata->queue_priority_mapping = queue_priority_map;
+	pdata->default_queue = 0;
 
 	return 0;
 }
 
-static int edma_xbar_event_map(struct device *dev,
-			       struct device_node *node,
-			       struct edma_soc_info *pdata, int len)
+#if IS_ENABLED(CONFIG_OF) && IS_ENABLED(CONFIG_DMADEVICES)
+
+static int edma_xbar_event_map(struct device *dev, struct device_node *node,
+			       struct edma_soc_info *pdata, size_t sz)
 {
-	int ret, i;
+	const char pname[] = "ti,edma-xbar-event-map";
 	struct resource res;
 	void __iomem *xbar;
-	const s16 (*xbar_chans)[2];
+	s16 (*xbar_chans)[2];
+	size_t nelm = sz / sizeof(s16);
 	u32 shift, offset, mux;
+	int ret, i;
 
-	xbar_chans = devm_kzalloc(dev,
-				  len/sizeof(s16) + 2*sizeof(s16),
-				  GFP_KERNEL);
+	xbar_chans = devm_kzalloc(dev, (nelm + 2) * sizeof(s16), GFP_KERNEL);
 	if (!xbar_chans)
 		return -ENOMEM;
 
 	ret = of_address_to_resource(node, 1, &res);
 	if (ret)
-		return -EIO;
+		return -ENOMEM;
 
 	xbar = devm_ioremap(dev, res.start, resource_size(&res));
 	if (!xbar)
 		return -ENOMEM;
 
-	ret = edma_of_read_u32_to_s16_array(node,
-					    "ti,edma-xbar-event-map",
-					    (s16 *)xbar_chans,
-					    len/sizeof(u32));
+	ret = of_property_read_u16_array(node, pname, (u16 *)xbar_chans, nelm);
 	if (ret)
 		return -EIO;
 
-	for (i = 0; xbar_chans[i][0] != -1; i++) {
+	/* Invalidate last entry for the other user of this mess */
+	nelm >>= 1;
+	xbar_chans[nelm][0] = xbar_chans[nelm][1] = -1;
+
+	for (i = 0; i < nelm; i++) {
 		shift = (xbar_chans[i][1] & 0x03) << 3;
 		offset = xbar_chans[i][1] & 0xfffffffc;
 		mux = readl(xbar + offset);
@@ -1480,8 +1517,7 @@ static int edma_xbar_event_map(struct device *dev,
 		writel(mux, (xbar + offset));
 	}
 
-	pdata->xbar_chans = xbar_chans;
-
+	pdata->xbar_chans = (const s16 (*)[2]) xbar_chans;
 	return 0;
 }
 
@@ -1489,65 +1525,16 @@ static int edma_of_parse_dt(struct device *dev,
 			    struct device_node *node,
 			    struct edma_soc_info *pdata)
 {
-	int ret = 0, i;
-	u32 value;
+	int ret = 0;
 	struct property *prop;
 	size_t sz;
 	struct edma_rsv_info *rsv_info;
-	s8 (*queue_tc_map)[2], (*queue_priority_map)[2];
-
-	memset(pdata, 0, sizeof(struct edma_soc_info));
-
-	ret = of_property_read_u32(node, "dma-channels", &value);
-	if (ret < 0)
-		return ret;
-	pdata->n_channel = value;
-
-	ret = of_property_read_u32(node, "ti,edma-regions", &value);
-	if (ret < 0)
-		return ret;
-	pdata->n_region = value;
-
-	ret = of_property_read_u32(node, "ti,edma-slots", &value);
-	if (ret < 0)
-		return ret;
-	pdata->n_slot = value;
-
-	pdata->n_cc = 1;
 
 	rsv_info = devm_kzalloc(dev, sizeof(struct edma_rsv_info), GFP_KERNEL);
 	if (!rsv_info)
 		return -ENOMEM;
 	pdata->rsv = rsv_info;
 
-	queue_tc_map = devm_kzalloc(dev, 8*sizeof(s8), GFP_KERNEL);
-	if (!queue_tc_map)
-		return -ENOMEM;
-
-	for (i = 0; i < 3; i++) {
-		queue_tc_map[i][0] = i;
-		queue_tc_map[i][1] = i;
-	}
-	queue_tc_map[i][0] = -1;
-	queue_tc_map[i][1] = -1;
-
-	pdata->queue_tc_mapping = queue_tc_map;
-
-	queue_priority_map = devm_kzalloc(dev, 8*sizeof(s8), GFP_KERNEL);
-	if (!queue_priority_map)
-		return -ENOMEM;
-
-	for (i = 0; i < 3; i++) {
-		queue_priority_map[i][0] = i;
-		queue_priority_map[i][1] = i;
-	}
-	queue_priority_map[i][0] = -1;
-	queue_priority_map[i][1] = -1;
-
-	pdata->queue_priority_mapping = queue_priority_map;
-
-	pdata->default_queue = 0;
-
 	prop = of_find_property(node, "ti,edma-xbar-event-map", &sz);
 	if (prop)
 		ret = edma_xbar_event_map(dev, node, pdata, sz);
@@ -1574,6 +1561,7 @@ static struct edma_soc_info *edma_setup_info_from_dt(struct device *dev,
 		return ERR_PTR(ret);
 
 	dma_cap_set(DMA_SLAVE, edma_filter_info.dma_cap);
+	dma_cap_set(DMA_CYCLIC, edma_filter_info.dma_cap);
 	of_dma_controller_register(dev->of_node, of_dma_simple_xlate,
 				   &edma_filter_info);
 
@@ -1592,7 +1580,6 @@ static int edma_probe(struct platform_device *pdev)
 	struct edma_soc_info	**info = pdev->dev.platform_data;
 	struct edma_soc_info    *ninfo[EDMA_MAX_CC] = {NULL};
 	s8		(*queue_priority_mapping)[2];
-	s8		(*queue_tc_mapping)[2];
 	int			i, j, off, ln, found = 0;
 	int			status = -1;
 	const s16		(*rsv_chans)[2];
@@ -1603,7 +1590,6 @@ static int edma_probe(struct platform_device *pdev)
 	struct resource		*r[EDMA_MAX_CC] = {NULL};
 	struct resource		res[EDMA_MAX_CC];
 	char			res_name[10];
-	char			irq_name[10];
 	struct device_node	*node = pdev->dev.of_node;
 	struct device		*dev = &pdev->dev;
 	int			ret;
@@ -1668,12 +1654,10 @@ static int edma_probe(struct platform_device *pdev)
 		if (!edma_cc[j])
 			return -ENOMEM;
 
-		edma_cc[j]->num_channels = min_t(unsigned, info[j]->n_channel,
-							EDMA_MAX_DMACH);
-		edma_cc[j]->num_slots = min_t(unsigned, info[j]->n_slot,
-							EDMA_MAX_PARAMENTRY);
-		edma_cc[j]->num_cc = min_t(unsigned, info[j]->n_cc,
-							EDMA_MAX_CC);
+		/* Get eDMA3 configuration from IP */
+		ret = edma_setup_from_hw(dev, info[j], edma_cc[j]);
+		if (ret)
+			return ret;
 
 		edma_cc[j]->default_queue = info[j]->default_queue;
 
@@ -1725,14 +1709,21 @@ static int edma_probe(struct platform_device *pdev)
 
 		if (node) {
 			irq[j] = irq_of_parse_and_map(node, 0);
+			err_irq[j] = irq_of_parse_and_map(node, 2);
 		} else {
+			char irq_name[10];
+
 			sprintf(irq_name, "edma%d", j);
 			irq[j] = platform_get_irq_byname(pdev, irq_name);
+
+			sprintf(irq_name, "edma%d_err", j);
+			err_irq[j] = platform_get_irq_byname(pdev, irq_name);
 		}
 		edma_cc[j]->irq_res_start = irq[j];
-		status = devm_request_irq(&pdev->dev, irq[j],
-					  dma_irq_handler, 0, "edma",
-					  &pdev->dev);
+		edma_cc[j]->irq_res_end = err_irq[j];
+
+		status = devm_request_irq(dev, irq[j], dma_irq_handler, 0,
+					  "edma", dev);
 		if (status < 0) {
 			dev_dbg(&pdev->dev,
 				"devm_request_irq %d failed --> %d\n",
@@ -1740,16 +1731,8 @@ static int edma_probe(struct platform_device *pdev)
 			return status;
 		}
 
-		if (node) {
-			err_irq[j] = irq_of_parse_and_map(node, 2);
-		} else {
-			sprintf(irq_name, "edma%d_err", j);
-			err_irq[j] = platform_get_irq_byname(pdev, irq_name);
-		}
-		edma_cc[j]->irq_res_end = err_irq[j];
-		status = devm_request_irq(&pdev->dev, err_irq[j],
-					  dma_ccerr_handler, 0,
-					  "edma_error", &pdev->dev);
+		status = devm_request_irq(dev, err_irq[j], dma_ccerr_handler, 0,
+					  "edma_error", dev);
 		if (status < 0) {
 			dev_dbg(&pdev->dev,
 				"devm_request_irq %d failed --> %d\n",
@@ -1760,14 +1743,8 @@ static int edma_probe(struct platform_device *pdev)
 		for (i = 0; i < edma_cc[j]->num_channels; i++)
 			map_dmach_queue(j, i, info[j]->default_queue);
 
-		queue_tc_mapping = info[j]->queue_tc_mapping;
 		queue_priority_mapping = info[j]->queue_priority_mapping;
 
-		/* Event queue to TC mapping */
-		for (i = 0; queue_tc_mapping[i][0] != -1; i++)
-			map_queue_tc(j, queue_tc_mapping[i][0],
-					queue_tc_mapping[i][1]);
-
 		/* Event queue priority mapping */
 		for (i = 0; queue_priority_mapping[i][0] != -1; i++)
 			assign_priority_to_queue(j,
@@ -1780,7 +1757,7 @@ static int edma_probe(struct platform_device *pdev)
 		if (edma_read(j, EDMA_CCCFG) & CHMAP_EXIST)
 			map_dmach_param(j);
 
-		for (i = 0; i < info[j]->n_region; i++) {
+		for (i = 0; i < edma_cc[j]->num_region; i++) {
 			edma_write_array2(j, EDMA_DRAE, i, 0, 0x0);
 			edma_write_array2(j, EDMA_DRAE, i, 1, 0x0);
 			edma_write_array(j, EDMA_QRAE, i, 0x0);
diff --git a/arch/arm/common/mcpm_entry.c b/arch/arm/common/mcpm_entry.c
index 1e361abc29e..86fd60fefbc 100644
--- a/arch/arm/common/mcpm_entry.c
+++ b/arch/arm/common/mcpm_entry.c
@@ -48,6 +48,11 @@ int __init mcpm_platform_register(const struct mcpm_platform_ops *ops)
 	return 0;
 }
 
+bool mcpm_is_available(void)
+{
+	return (platform_ops) ? true : false;
+}
+
 int mcpm_cpu_power_up(unsigned int cpu, unsigned int cluster)
 {
 	if (!platform_ops)
diff --git a/arch/arm/configs/at91sam9g45_defconfig b/arch/arm/configs/at91sam9g45_defconfig
index e181a50fd65..c6661a60025 100644
--- a/arch/arm/configs/at91sam9g45_defconfig
+++ b/arch/arm/configs/at91sam9g45_defconfig
@@ -83,7 +83,6 @@ CONFIG_KEYBOARD_GPIO=y
 # CONFIG_INPUT_MOUSE is not set
 CONFIG_INPUT_TOUCHSCREEN=y
 CONFIG_TOUCHSCREEN_ATMEL_MXT=m
-CONFIG_TOUCHSCREEN_ATMEL_TSADCC=y
 # CONFIG_SERIO is not set
 # CONFIG_LEGACY_PTYS is not set
 CONFIG_SERIAL_ATMEL=y
@@ -146,6 +145,8 @@ CONFIG_DMADEVICES=y
 CONFIG_AT_HDMAC=y
 CONFIG_DMATEST=m
 # CONFIG_IOMMU_SUPPORT is not set
+CONFIG_IIO=y
+CONFIG_AT91_ADC=y
 CONFIG_EXT4_FS=y
 CONFIG_FANOTIFY=y
 CONFIG_VFAT_FS=y
diff --git a/arch/arm/configs/at91sam9rl_defconfig b/arch/arm/configs/at91sam9rl_defconfig
index 85f846ae9ff..5d7797d43d2 100644
--- a/arch/arm/configs/at91sam9rl_defconfig
+++ b/arch/arm/configs/at91sam9rl_defconfig
@@ -45,7 +45,6 @@ CONFIG_INPUT_EVDEV=y
 # CONFIG_INPUT_KEYBOARD is not set
 # CONFIG_INPUT_MOUSE is not set
 CONFIG_INPUT_TOUCHSCREEN=y
-CONFIG_TOUCHSCREEN_ATMEL_TSADCC=y
 # CONFIG_SERIO is not set
 CONFIG_SERIAL_ATMEL=y
 CONFIG_SERIAL_ATMEL_CONSOLE=y
@@ -65,6 +64,8 @@ CONFIG_MMC=y
 CONFIG_MMC_ATMELMCI=m
 CONFIG_RTC_CLASS=y
 CONFIG_RTC_DRV_AT91SAM9=y
+CONFIG_IIO=y
+CONFIG_AT91_ADC=y
 CONFIG_EXT2_FS=y
 CONFIG_MSDOS_FS=y
 CONFIG_VFAT_FS=y
diff --git a/arch/arm/configs/axm55xx_defconfig b/arch/arm/configs/axm55xx_defconfig
new file mode 100644
index 00000000000..d3260d7d5af
--- /dev/null
+++ b/arch/arm/configs/axm55xx_defconfig
@@ -0,0 +1,248 @@
+CONFIG_SYSVIPC=y
+CONFIG_POSIX_MQUEUE=y
+CONFIG_FHANDLE=y
+CONFIG_AUDIT=y
+CONFIG_HIGH_RES_TIMERS=y
+CONFIG_BSD_PROCESS_ACCT=y
+CONFIG_BSD_PROCESS_ACCT_V3=y
+CONFIG_TASKSTATS=y
+CONFIG_TASK_DELAY_ACCT=y
+CONFIG_TASK_XACCT=y
+CONFIG_TASK_IO_ACCOUNTING=y
+CONFIG_IKCONFIG=y
+CONFIG_IKCONFIG_PROC=y
+CONFIG_LOG_BUF_SHIFT=16
+CONFIG_NAMESPACES=y
+# CONFIG_UTS_NS is not set
+# CONFIG_IPC_NS is not set
+# CONFIG_PID_NS is not set
+# CONFIG_NET_NS is not set
+CONFIG_SCHED_AUTOGROUP=y
+CONFIG_RELAY=y
+CONFIG_BLK_DEV_INITRD=y
+CONFIG_SYSCTL_SYSCALL=y
+CONFIG_EMBEDDED=y
+# CONFIG_COMPAT_BRK is not set
+CONFIG_PROFILING=y
+CONFIG_MODULES=y
+CONFIG_MODULE_UNLOAD=y
+# CONFIG_IOSCHED_DEADLINE is not set
+CONFIG_ARCH_AXXIA=y
+CONFIG_GPIO_PCA953X=y
+CONFIG_ARM_LPAE=y
+CONFIG_ARM_THUMBEE=y
+CONFIG_ARM_ERRATA_430973=y
+CONFIG_ARM_ERRATA_643719=y
+CONFIG_ARM_ERRATA_720789=y
+CONFIG_ARM_ERRATA_754322=y
+CONFIG_ARM_ERRATA_754327=y
+CONFIG_ARM_ERRATA_764369=y
+CONFIG_ARM_ERRATA_775420=y
+CONFIG_ARM_ERRATA_798181=y
+CONFIG_PCI=y
+CONFIG_PCI_MSI=y
+CONFIG_PCIE_AXXIA=y
+CONFIG_SMP=y
+CONFIG_NR_CPUS=16
+CONFIG_HOTPLUG_CPU=y
+CONFIG_PREEMPT=y
+CONFIG_AEABI=y
+CONFIG_OABI_COMPAT=y
+CONFIG_HIGHMEM=y
+CONFIG_KSM=y
+CONFIG_ZBOOT_ROM_TEXT=0x0
+CONFIG_ZBOOT_ROM_BSS=0x0
+CONFIG_ARM_APPENDED_DTB=y
+CONFIG_ARM_ATAG_DTB_COMPAT=y
+CONFIG_VFP=y
+CONFIG_NEON=y
+# CONFIG_CORE_DUMP_DEFAULT_ELF_HEADERS is not set
+CONFIG_BINFMT_MISC=y
+# CONFIG_SUSPEND is not set
+CONFIG_NET=y
+CONFIG_PACKET=y
+CONFIG_UNIX=y
+CONFIG_XFRM_USER=y
+CONFIG_XFRM_SUB_POLICY=y
+CONFIG_XFRM_MIGRATE=y
+CONFIG_XFRM_STATISTICS=y
+CONFIG_NET_KEY=y
+CONFIG_INET=y
+CONFIG_IP_PNP=y
+CONFIG_IP_PNP_DHCP=y
+CONFIG_IP_PNP_BOOTP=y
+CONFIG_INET_AH=y
+CONFIG_INET_ESP=y
+CONFIG_INET_IPCOMP=y
+# CONFIG_INET_LRO is not set
+# CONFIG_IPV6 is not set
+CONFIG_NETWORK_PHY_TIMESTAMPING=y
+CONFIG_BRIDGE=y
+# CONFIG_WIRELESS is not set
+CONFIG_UEVENT_HELPER_PATH="/sbin/hotplug"
+CONFIG_DEVTMPFS=y
+CONFIG_DEVTMPFS_MOUNT=y
+CONFIG_MTD=y
+CONFIG_MTD_CMDLINE_PARTS=y
+CONFIG_MTD_AFS_PARTS=y
+CONFIG_MTD_BLOCK=y
+CONFIG_MTD_CFI=y
+CONFIG_MTD_CFI_INTELEXT=y
+CONFIG_MTD_CFI_AMDSTD=y
+CONFIG_MTD_CFI_STAA=y
+CONFIG_MTD_PHYSMAP=y
+CONFIG_MTD_PHYSMAP_OF=y
+CONFIG_MTD_M25P80=y
+CONFIG_PROC_DEVICETREE=y
+CONFIG_BLK_DEV_LOOP=y
+CONFIG_BLK_DEV_RAM=y
+CONFIG_EEPROM_AT24=y
+CONFIG_EEPROM_AT25=y
+CONFIG_BLK_DEV_SD=y
+CONFIG_CHR_DEV_SG=y
+CONFIG_ATA=y
+CONFIG_PATA_PLATFORM=y
+CONFIG_PATA_OF_PLATFORM=y
+CONFIG_MD=y
+CONFIG_BLK_DEV_DM=y
+CONFIG_DM_UEVENT=y
+CONFIG_NETDEVICES=y
+CONFIG_TUN=y
+CONFIG_VETH=y
+CONFIG_VIRTIO_NET=y
+# CONFIG_NET_CADENCE is not set
+# CONFIG_NET_VENDOR_BROADCOM is not set
+# CONFIG_NET_VENDOR_CIRRUS is not set
+# CONFIG_NET_VENDOR_FARADAY is not set
+# CONFIG_NET_VENDOR_INTEL is not set
+# CONFIG_NET_VENDOR_MARVELL is not set
+# CONFIG_NET_VENDOR_MICREL is not set
+# CONFIG_NET_VENDOR_NATSEMI is not set
+# CONFIG_NET_VENDOR_SEEQ is not set
+# CONFIG_NET_VENDOR_SMSC is not set
+# CONFIG_NET_VENDOR_STMICRO is not set
+# CONFIG_NET_VENDOR_VIA is not set
+# CONFIG_NET_VENDOR_WIZNET is not set
+CONFIG_BROADCOM_PHY=y
+# CONFIG_WLAN is not set
+# CONFIG_MOUSE_PS2_ALPS is not set
+# CONFIG_MOUSE_PS2_LOGIPS2PP is not set
+# CONFIG_MOUSE_PS2_SYNAPTICS is not set
+# CONFIG_MOUSE_PS2_TRACKPOINT is not set
+# CONFIG_SERIO_SERPORT is not set
+CONFIG_SERIO_AMBAKMI=y
+CONFIG_LEGACY_PTY_COUNT=16
+CONFIG_SERIAL_AMBA_PL011=y
+CONFIG_SERIAL_AMBA_PL011_CONSOLE=y
+CONFIG_VIRTIO_CONSOLE=y
+# CONFIG_HW_RANDOM is not set
+CONFIG_I2C=y
+CONFIG_I2C_CHARDEV=y
+CONFIG_I2C_MUX=y
+CONFIG_I2C_AXXIA=y
+CONFIG_SPI=y
+CONFIG_SPI_PL022=y
+CONFIG_DP83640_PHY=y
+CONFIG_GPIOLIB=y
+CONFIG_GPIO_SYSFS=y
+CONFIG_GPIO_PL061=y
+CONFIG_POWER_SUPPLY=y
+CONFIG_POWER_RESET=y
+CONFIG_POWER_RESET_AXXIA=y
+CONFIG_SENSORS_ADT7475=y
+CONFIG_SENSORS_JC42=y
+CONFIG_SENSORS_LM75=y
+CONFIG_PMBUS=y
+CONFIG_SENSORS_LTC2978=y
+CONFIG_WATCHDOG=y
+CONFIG_ARM_SP805_WATCHDOG=y
+CONFIG_FB=y
+CONFIG_FB_ARMCLCD=y
+CONFIG_FRAMEBUFFER_CONSOLE=y
+CONFIG_LOGO=y
+# CONFIG_LOGO_LINUX_MONO is not set
+# CONFIG_LOGO_LINUX_VGA16 is not set
+CONFIG_HID_A4TECH=y
+CONFIG_HID_APPLE=y
+CONFIG_HID_BELKIN=y
+CONFIG_HID_CHERRY=y
+CONFIG_HID_CHICONY=y
+CONFIG_HID_CYPRESS=y
+CONFIG_HID_DRAGONRISE=y
+CONFIG_HID_EZKEY=y
+CONFIG_HID_KYE=y
+CONFIG_HID_GYRATION=y
+CONFIG_HID_TWINHAN=y
+CONFIG_HID_KENSINGTON=y
+CONFIG_HID_LOGITECH=y
+CONFIG_HID_MICROSOFT=y
+CONFIG_HID_MONTEREY=y
+CONFIG_HID_NTRIG=y
+CONFIG_HID_ORTEK=y
+CONFIG_HID_PANTHERLORD=y
+CONFIG_HID_PETALYNX=y
+CONFIG_HID_SAMSUNG=y
+CONFIG_HID_SUNPLUS=y
+CONFIG_HID_GREENASIA=y
+CONFIG_HID_SMARTJOYPLUS=y
+CONFIG_HID_TOPSEED=y
+CONFIG_HID_THRUSTMASTER=y
+CONFIG_HID_ZEROPLUS=y
+CONFIG_USB=y
+CONFIG_USB_ANNOUNCE_NEW_DEVICES=y
+CONFIG_USB_EHCI_HCD=y
+# CONFIG_USB_EHCI_TT_NEWSCHED is not set
+CONFIG_USB_EHCI_HCD_AXXIA=y
+CONFIG_USB_STORAGE=y
+CONFIG_MMC=y
+CONFIG_MMC_ARMMMCI=y
+CONFIG_DMADEVICES=y
+CONFIG_PL330_DMA=y
+CONFIG_VIRT_DRIVERS=y
+CONFIG_VIRTIO_MMIO=y
+CONFIG_MAILBOX=y
+CONFIG_PL320_MBOX=y
+# CONFIG_IOMMU_SUPPORT is not set
+CONFIG_EXT2_FS=y
+CONFIG_EXT3_FS=y
+# CONFIG_EXT3_DEFAULTS_TO_ORDERED is not set
+CONFIG_EXT4_FS=y
+CONFIG_AUTOFS4_FS=y
+CONFIG_FUSE_FS=y
+CONFIG_CUSE=y
+CONFIG_FSCACHE=y
+CONFIG_FSCACHE_STATS=y
+CONFIG_FSCACHE_HISTOGRAM=y
+CONFIG_FSCACHE_DEBUG=y
+CONFIG_FSCACHE_OBJECT_LIST=y
+CONFIG_CACHEFILES=y
+CONFIG_CACHEFILES_HISTOGRAM=y
+CONFIG_ISO9660_FS=y
+CONFIG_UDF_FS=y
+CONFIG_MSDOS_FS=y
+CONFIG_VFAT_FS=y
+CONFIG_NTFS_FS=y
+CONFIG_TMPFS=y
+CONFIG_JFFS2_FS=y
+CONFIG_CRAMFS=y
+CONFIG_NFS_FS=y
+CONFIG_NFS_V4=y
+CONFIG_ROOT_NFS=y
+CONFIG_NFS_FSCACHE=y
+CONFIG_SUNRPC_DEBUG=y
+CONFIG_NLS_CODEPAGE_437=y
+CONFIG_NLS_ISO8859_1=y
+CONFIG_PRINTK_TIME=y
+CONFIG_DEBUG_INFO=y
+CONFIG_DEBUG_FS=y
+CONFIG_MAGIC_SYSRQ=y
+# CONFIG_SCHED_DEBUG is not set
+CONFIG_RCU_CPU_STALL_TIMEOUT=60
+# CONFIG_FTRACE is not set
+CONFIG_DEBUG_USER=y
+CONFIG_CRYPTO_GCM=y
+CONFIG_CRYPTO_XCBC=y
+CONFIG_CRYPTO_SHA256=y
+# CONFIG_CRYPTO_ANSI_CPRNG is not set
+CONFIG_VIRTUALIZATION=y
+CONFIG_KVM=y
diff --git a/arch/arm/configs/badge4_defconfig b/arch/arm/configs/badge4_defconfig
index 5b54abbeb0b..0494c8f229a 100644
--- a/arch/arm/configs/badge4_defconfig
+++ b/arch/arm/configs/badge4_defconfig
@@ -73,8 +73,6 @@ CONFIG_SA1100_WATCHDOG=m
 CONFIG_SOUND=y
 CONFIG_SOUND_PRIME=y
 CONFIG_USB=y
-CONFIG_USB_DEBUG=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=y
 CONFIG_USB_ACM=m
 CONFIG_USB_PRINTER=m
diff --git a/arch/arm/configs/bcm_defconfig b/arch/arm/configs/bcm_defconfig
index 3df3f3a79ef..9d13dae9912 100644
--- a/arch/arm/configs/bcm_defconfig
+++ b/arch/arm/configs/bcm_defconfig
@@ -91,6 +91,7 @@ CONFIG_FB=y
 CONFIG_BACKLIGHT_LCD_SUPPORT=y
 CONFIG_LCD_CLASS_DEVICE=y
 CONFIG_BACKLIGHT_CLASS_DEVICE=y
+CONFIG_BACKLIGHT_PWM=y
 # CONFIG_USB_SUPPORT is not set
 CONFIG_MMC=y
 CONFIG_MMC_UNSAFE_RESUME=y
@@ -104,6 +105,8 @@ CONFIG_LEDS_TRIGGERS=y
 CONFIG_LEDS_TRIGGER_TIMER=y
 CONFIG_LEDS_TRIGGER_HEARTBEAT=y
 CONFIG_LEDS_TRIGGER_DEFAULT_ON=y
+CONFIG_PWM=y
+CONFIG_PWM_BCM_KONA=y
 CONFIG_EXT4_FS=y
 CONFIG_EXT4_FS_POSIX_ACL=y
 CONFIG_EXT4_FS_SECURITY=y
diff --git a/arch/arm/configs/cm_x2xx_defconfig b/arch/arm/configs/cm_x2xx_defconfig
index a93ff8da5ba..dc01c049a52 100644
--- a/arch/arm/configs/cm_x2xx_defconfig
+++ b/arch/arm/configs/cm_x2xx_defconfig
@@ -144,7 +144,6 @@ CONFIG_HID_SAMSUNG=y
 CONFIG_HID_SONY=y
 CONFIG_HID_SUNPLUS=y
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_MON=y
 CONFIG_USB_OHCI_HCD=y
diff --git a/arch/arm/configs/cm_x300_defconfig b/arch/arm/configs/cm_x300_defconfig
index f4b767256f9..7df040e91c1 100644
--- a/arch/arm/configs/cm_x300_defconfig
+++ b/arch/arm/configs/cm_x300_defconfig
@@ -129,7 +129,6 @@ CONFIG_HID_TOPSEED=y
 CONFIG_HID_THRUSTMASTER=y
 CONFIG_HID_ZEROPLUS=y
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_MON=y
 CONFIG_USB_OHCI_HCD=y
diff --git a/arch/arm/configs/colibri_pxa270_defconfig b/arch/arm/configs/colibri_pxa270_defconfig
index 2ef2c5e8aae..18c311ae111 100644
--- a/arch/arm/configs/colibri_pxa270_defconfig
+++ b/arch/arm/configs/colibri_pxa270_defconfig
@@ -124,7 +124,6 @@ CONFIG_FONT_8x16=y
 CONFIG_LOGO=y
 # CONFIG_USB_HID is not set
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_SERIAL=m
 CONFIG_USB_GADGET=m
diff --git a/arch/arm/configs/colibri_pxa300_defconfig b/arch/arm/configs/colibri_pxa300_defconfig
index b985334e42d..be02fe2b14c 100644
--- a/arch/arm/configs/colibri_pxa300_defconfig
+++ b/arch/arm/configs/colibri_pxa300_defconfig
@@ -47,9 +47,7 @@ CONFIG_FRAMEBUFFER_CONSOLE=y
 CONFIG_LOGO=y
 # CONFIG_HID_SUPPORT is not set
 CONFIG_USB=y
-CONFIG_USB_DEBUG=y
 CONFIG_USB_ANNOUNCE_NEW_DEVICES=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=y
 CONFIG_USB_STORAGE=y
 CONFIG_MMC=y
diff --git a/arch/arm/configs/corgi_defconfig b/arch/arm/configs/corgi_defconfig
index 1fd1d1de322..c1470a00f55 100644
--- a/arch/arm/configs/corgi_defconfig
+++ b/arch/arm/configs/corgi_defconfig
@@ -172,7 +172,6 @@ CONFIG_HID_SAMSUNG=m
 CONFIG_HID_SONY=m
 CONFIG_HID_SUNPLUS=m
 CONFIG_USB=m
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=m
 CONFIG_USB_SL811_HCD=m
 CONFIG_USB_SL811_CS=m
diff --git a/arch/arm/configs/davinci_all_defconfig b/arch/arm/configs/davinci_all_defconfig
index 2a282c051cf..f95f72d62db 100644
--- a/arch/arm/configs/davinci_all_defconfig
+++ b/arch/arm/configs/davinci_all_defconfig
@@ -157,10 +157,8 @@ CONFIG_HID_SAMSUNG=m
 CONFIG_HID_SONY=m
 CONFIG_HID_SUNPLUS=m
 CONFIG_USB=m
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=m
 CONFIG_USB_MUSB_HDRC=m
-CONFIG_USB_MUSB_PERIPHERAL=y
 CONFIG_USB_GADGET_MUSB_HDRC=y
 CONFIG_MUSB_PIO_ONLY=y
 CONFIG_USB_STORAGE=m
diff --git a/arch/arm/configs/dove_defconfig b/arch/arm/configs/dove_defconfig
index f1595514417..701677f9248 100644
--- a/arch/arm/configs/dove_defconfig
+++ b/arch/arm/configs/dove_defconfig
@@ -37,7 +37,6 @@ CONFIG_DEVTMPFS=y
 CONFIG_DEVTMPFS_MOUNT=y
 CONFIG_MTD=y
 CONFIG_MTD_CMDLINE_PARTS=y
-CONFIG_MTD_CHAR=y
 CONFIG_MTD_BLOCK=y
 CONFIG_MTD_CFI=y
 CONFIG_MTD_JEDECPROBE=y
@@ -48,6 +47,7 @@ CONFIG_MTD_CFI_INTELEXT=y
 CONFIG_MTD_CFI_STAA=y
 CONFIG_MTD_PHYSMAP=y
 CONFIG_MTD_M25P80=y
+CONFIG_MTD_SPI_NOR=y
 CONFIG_BLK_DEV_LOOP=y
 CONFIG_BLK_DEV_RAM=y
 CONFIG_BLK_DEV_RAM_COUNT=1
diff --git a/arch/arm/configs/em_x270_defconfig b/arch/arm/configs/em_x270_defconfig
index 60a21e01eb7..4560c9ca663 100644
--- a/arch/arm/configs/em_x270_defconfig
+++ b/arch/arm/configs/em_x270_defconfig
@@ -144,7 +144,6 @@ CONFIG_HID_SAMSUNG=y
 CONFIG_HID_SONY=y
 CONFIG_HID_SUNPLUS=y
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_MON=y
 CONFIG_USB_OHCI_HCD=y
diff --git a/arch/arm/configs/ep93xx_defconfig b/arch/arm/configs/ep93xx_defconfig
index 6ac5ea73bd0..1b650c85bdd 100644
--- a/arch/arm/configs/ep93xx_defconfig
+++ b/arch/arm/configs/ep93xx_defconfig
@@ -80,7 +80,6 @@ CONFIG_I2C_DEBUG_BUS=y
 CONFIG_WATCHDOG=y
 CONFIG_EP93XX_WATCHDOG=y
 CONFIG_USB=y
-CONFIG_USB_DEBUG=y
 CONFIG_USB_DYNAMIC_MINORS=y
 CONFIG_USB_OHCI_HCD=y
 CONFIG_USB_OHCI_HCD_PLATFORM=y
diff --git a/arch/arm/configs/exynos_defconfig b/arch/arm/configs/exynos_defconfig
index 4ce7b70ea90..e07a227ec0d 100644
--- a/arch/arm/configs/exynos_defconfig
+++ b/arch/arm/configs/exynos_defconfig
@@ -65,6 +65,7 @@ CONFIG_TCG_TIS_I2C_INFINEON=y
 CONFIG_I2C=y
 CONFIG_I2C_MUX=y
 CONFIG_I2C_ARB_GPIO_CHALLENGE=y
+CONFIG_I2C_EXYNOS5=y
 CONFIG_I2C_S3C2410=y
 CONFIG_DEBUG_GPIO=y
 # CONFIG_HWMON is not set
diff --git a/arch/arm/configs/footbridge_defconfig b/arch/arm/configs/footbridge_defconfig
index 038518ab39a..87e020f303a 100644
--- a/arch/arm/configs/footbridge_defconfig
+++ b/arch/arm/configs/footbridge_defconfig
@@ -100,8 +100,6 @@ CONFIG_FB_CYBER2000=y
 CONFIG_SOUND=m
 # CONFIG_USB_HID is not set
 CONFIG_USB=m
-CONFIG_USB_DEBUG=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=m
 CONFIG_USB_PRINTER=m
 CONFIG_EXT2_FS=y
diff --git a/arch/arm/configs/imx_v4_v5_defconfig b/arch/arm/configs/imx_v4_v5_defconfig
index f1aeb7d7271..bada59d93b6 100644
--- a/arch/arm/configs/imx_v4_v5_defconfig
+++ b/arch/arm/configs/imx_v4_v5_defconfig
@@ -80,6 +80,7 @@ CONFIG_MTD_UBI=y
 CONFIG_EEPROM_AT24=y
 CONFIG_EEPROM_AT25=y
 CONFIG_ATA=y
+CONFIG_BLK_DEV_SD=y
 CONFIG_PATA_IMX=y
 CONFIG_NETDEVICES=y
 CONFIG_CS89x0=y
@@ -153,8 +154,12 @@ CONFIG_USB_HID=m
 CONFIG_USB=y
 CONFIG_USB_EHCI_HCD=y
 CONFIG_USB_EHCI_MXC=y
+CONFIG_USB_STORAGE=y
+CONFIG_USB_CHIPIDEA=y
+CONFIG_USB_CHIPIDEA_UDC=y
+CONFIG_USB_CHIPIDEA_HOST=y
+CONFIG_NOP_USB_XCEIV=y
 CONFIG_MMC=y
-CONFIG_MMC_UNSAFE_RESUME=y
 CONFIG_MMC_SDHCI=y
 CONFIG_MMC_SDHCI_PLTFM=y
 CONFIG_MMC_SDHCI_ESDHC_IMX=y
@@ -177,7 +182,6 @@ CONFIG_RTC_DRV_MXC=y
 CONFIG_DMADEVICES=y
 CONFIG_IMX_SDMA=y
 CONFIG_IMX_DMA=y
-CONFIG_COMMON_CLK_DEBUG=y
 # CONFIG_IOMMU_SUPPORT is not set
 CONFIG_EXT2_FS=y
 CONFIG_EXT3_FS=y
diff --git a/arch/arm/configs/imx_v6_v7_defconfig b/arch/arm/configs/imx_v6_v7_defconfig
index 09e974392fa..ef8815327e5 100644
--- a/arch/arm/configs/imx_v6_v7_defconfig
+++ b/arch/arm/configs/imx_v6_v7_defconfig
@@ -1,4 +1,3 @@
-# CONFIG_LOCALVERSION_AUTO is not set
 CONFIG_KERNEL_LZO=y
 CONFIG_SYSVIPC=y
 CONFIG_NO_HZ=y
@@ -33,7 +32,6 @@ CONFIG_MACH_PCM043=y
 CONFIG_MACH_MX35_3DS=y
 CONFIG_MACH_VPR200=y
 CONFIG_MACH_IMX51_DT=y
-CONFIG_MACH_EUKREA_CPUIMX51SD=y
 CONFIG_SOC_IMX50=y
 CONFIG_SOC_IMX53=y
 CONFIG_SOC_IMX6Q=y
@@ -46,7 +44,11 @@ CONFIG_VMSPLIT_2G=y
 CONFIG_PREEMPT_VOLUNTARY=y
 CONFIG_AEABI=y
 CONFIG_HIGHMEM=y
+CONFIG_CMA=y
 CONFIG_CMDLINE="noinitrd console=ttymxc0,115200"
+CONFIG_CPU_FREQ=y
+CONFIG_CPU_FREQ_DEFAULT_GOV_ONDEMAND=y
+CONFIG_ARM_IMX6Q_CPUFREQ=y
 CONFIG_VFP=y
 CONFIG_NEON=y
 CONFIG_BINFMT_MISC=m
@@ -72,6 +74,7 @@ CONFIG_RFKILL_INPUT=y
 CONFIG_DEVTMPFS=y
 CONFIG_DEVTMPFS_MOUNT=y
 # CONFIG_STANDALONE is not set
+CONFIG_DMA_CMA=y
 CONFIG_IMX_WEIM=y
 CONFIG_CONNECTOR=y
 CONFIG_MTD=y
@@ -89,6 +92,7 @@ CONFIG_MTD_SST25L=y
 CONFIG_MTD_NAND=y
 CONFIG_MTD_NAND_GPMI_NAND=y
 CONFIG_MTD_NAND_MXC=y
+CONFIG_MTD_SPI_NOR=y
 CONFIG_MTD_UBI=y
 CONFIG_BLK_DEV_LOOP=y
 CONFIG_BLK_DEV_RAM=y
@@ -183,6 +187,7 @@ CONFIG_V4L_MEM2MEM_DRIVERS=y
 CONFIG_VIDEO_CODA=y
 CONFIG_SOC_CAMERA_OV2640=y
 CONFIG_DRM=y
+CONFIG_DRM_PANEL_SIMPLE=y
 CONFIG_BACKLIGHT_LCD_SUPPORT=y
 CONFIG_LCD_CLASS_DEVICE=y
 CONFIG_LCD_L4F00242T03=y
@@ -215,7 +220,6 @@ CONFIG_USB_GADGET=y
 CONFIG_USB_ETH=m
 CONFIG_USB_MASS_STORAGE=m
 CONFIG_MMC=y
-CONFIG_MMC_UNSAFE_RESUME=y
 CONFIG_MMC_SDHCI=y
 CONFIG_MMC_SDHCI_PLTFM=y
 CONFIG_MMC_SDHCI_ESDHC_IMX=y
@@ -245,7 +249,7 @@ CONFIG_DRM_IMX_TVE=y
 CONFIG_DRM_IMX_LDB=y
 CONFIG_DRM_IMX_IPUV3_CORE=y
 CONFIG_DRM_IMX_IPUV3=y
-CONFIG_COMMON_CLK_DEBUG=y
+CONFIG_DRM_IMX_HDMI=y
 # CONFIG_IOMMU_SUPPORT is not set
 CONFIG_PWM=y
 CONFIG_PWM_IMX=y
diff --git a/arch/arm/configs/ixp4xx_defconfig b/arch/arm/configs/ixp4xx_defconfig
index 063e2ab2c8f..1af665e847d 100644
--- a/arch/arm/configs/ixp4xx_defconfig
+++ b/arch/arm/configs/ixp4xx_defconfig
@@ -169,7 +169,6 @@ CONFIG_SENSORS_W83781D=y
 CONFIG_WATCHDOG=y
 CONFIG_IXP4XX_WATCHDOG=y
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_EHCI_HCD=y
 CONFIG_USB_OHCI_HCD=y
diff --git a/arch/arm/configs/keystone_defconfig b/arch/arm/configs/keystone_defconfig
index ec9a41d5068..095bb52671f 100644
--- a/arch/arm/configs/keystone_defconfig
+++ b/arch/arm/configs/keystone_defconfig
@@ -135,7 +135,6 @@ CONFIG_WATCHDOG=y
 CONFIG_WATCHDOG_CORE=y
 CONFIG_DAVINCI_WATCHDOG=y
 CONFIG_USB=y
-CONFIG_USB_DEBUG=y
 CONFIG_USB_ANNOUNCE_NEW_DEVICES=y
 CONFIG_USB_MON=y
 CONFIG_USB_XHCI_HCD=y
diff --git a/arch/arm/configs/kirkwood_defconfig b/arch/arm/configs/kirkwood_defconfig
index 2e762d94e94..b9e480c10b1 100644
--- a/arch/arm/configs/kirkwood_defconfig
+++ b/arch/arm/configs/kirkwood_defconfig
@@ -61,6 +61,7 @@ CONFIG_MTD_PHYSMAP=y
 CONFIG_MTD_M25P80=y
 CONFIG_MTD_NAND=y
 CONFIG_MTD_NAND_ORION=y
+CONFIG_MTD_SPI_NOR=y
 CONFIG_BLK_DEV_LOOP=y
 CONFIG_EEPROM_AT24=y
 # CONFIG_SCSI_PROC_FS is not set
diff --git a/arch/arm/configs/kzm9g_defconfig b/arch/arm/configs/kzm9g_defconfig
index 12bd1f63c39..bd097d455f8 100644
--- a/arch/arm/configs/kzm9g_defconfig
+++ b/arch/arm/configs/kzm9g_defconfig
@@ -106,7 +106,6 @@ CONFIG_SND_SOC=y
 CONFIG_SND_SOC_SH4_FSI=y
 # CONFIG_HID_SUPPORT is not set
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_R8A66597_HCD=y
 CONFIG_USB_RENESAS_USBHS=y
 CONFIG_USB_STORAGE=y
diff --git a/arch/arm/configs/mini2440_defconfig b/arch/arm/configs/mini2440_defconfig
index a07948a87ca..9c93f565524 100644
--- a/arch/arm/configs/mini2440_defconfig
+++ b/arch/arm/configs/mini2440_defconfig
@@ -217,7 +217,6 @@ CONFIG_HID_SONY=y
 CONFIG_HID_SUNPLUS=y
 CONFIG_HID_TOPSEED=y
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_OHCI_HCD=y
 CONFIG_USB_ACM=m
diff --git a/arch/arm/configs/msm_defconfig b/arch/arm/configs/msm_defconfig
index c5858b9eb51..7f52dad97f5 100644
--- a/arch/arm/configs/msm_defconfig
+++ b/arch/arm/configs/msm_defconfig
@@ -17,21 +17,14 @@ CONFIG_MODULE_UNLOAD=y
 CONFIG_MODULE_FORCE_UNLOAD=y
 CONFIG_MODVERSIONS=y
 CONFIG_PARTITION_ADVANCED=y
-CONFIG_ARCH_MSM_DT=y
-CONFIG_ARCH_MSM8X60=y
-CONFIG_ARCH_MSM8960=y
-CONFIG_ARCH_MSM8974=y
-CONFIG_SMP=y
+CONFIG_ARCH_MSM=y
 CONFIG_PREEMPT=y
 CONFIG_AEABI=y
 CONFIG_HIGHMEM=y
 CONFIG_HIGHPTE=y
 CONFIG_CLEANCACHE=y
-CONFIG_CC_STACKPROTECTOR=y
-CONFIG_ARM_APPENDED_DTB=y
-CONFIG_ARM_ATAG_DTB_COMPAT=y
+CONFIG_AUTO_ZRELADDR=y
 CONFIG_VFP=y
-CONFIG_NEON=y
 # CONFIG_CORE_DUMP_DEFAULT_ELF_HEADERS is not set
 CONFIG_NET=y
 CONFIG_PACKET=y
@@ -79,16 +72,12 @@ CONFIG_SERIO_LIBPS2=y
 # CONFIG_LEGACY_PTYS is not set
 CONFIG_SERIAL_MSM=y
 CONFIG_SERIAL_MSM_CONSOLE=y
-CONFIG_HW_RANDOM=y
-CONFIG_HW_RANDOM_MSM=y
+# CONFIG_HW_RANDOM is not set
 CONFIG_I2C=y
 CONFIG_I2C_CHARDEV=y
 CONFIG_SPI=y
 CONFIG_DEBUG_GPIO=y
 CONFIG_GPIO_SYSFS=y
-CONFIG_POWER_SUPPLY=y
-CONFIG_POWER_RESET=y
-CONFIG_POWER_RESET_MSM=y
 CONFIG_THERMAL=y
 CONFIG_REGULATOR=y
 CONFIG_MEDIA_SUPPORT=y
@@ -100,25 +89,17 @@ CONFIG_SND_DYNAMIC_MINORS=y
 # CONFIG_SND_SPI is not set
 # CONFIG_SND_USB is not set
 CONFIG_SND_SOC=y
-CONFIG_HID_BATTERY_STRENGTH=y
 CONFIG_USB=y
 CONFIG_USB_ANNOUNCE_NEW_DEVICES=y
 CONFIG_USB_MON=y
 CONFIG_USB_EHCI_HCD=y
-CONFIG_USB_EHCI_MSM=y
 CONFIG_USB_ACM=y
 CONFIG_USB_SERIAL=y
 CONFIG_USB_GADGET=y
 CONFIG_USB_GADGET_DEBUG_FILES=y
 CONFIG_USB_GADGET_VBUS_DRAW=500
-CONFIG_NEW_LEDS=y
 CONFIG_RTC_CLASS=y
 CONFIG_STAGING=y
-CONFIG_COMMON_CLK_QCOM=y
-CONFIG_MSM_GCC_8660=y
-CONFIG_MSM_MMCC_8960=y
-CONFIG_MSM_MMCC_8974=y
-CONFIG_MSM_IOMMU=y
 CONFIG_EXT2_FS=y
 CONFIG_EXT2_FS_XATTR=y
 CONFIG_EXT3_FS=y
diff --git a/arch/arm/configs/multi_v5_defconfig b/arch/arm/configs/multi_v5_defconfig
index aa3dfb084fe..5ebfa8bf850 100644
--- a/arch/arm/configs/multi_v5_defconfig
+++ b/arch/arm/configs/multi_v5_defconfig
@@ -11,7 +11,6 @@ CONFIG_MODULE_UNLOAD=y
 # CONFIG_ARCH_MULTI_V7 is not set
 CONFIG_ARCH_MVEBU=y
 CONFIG_MACH_KIRKWOOD=y
-CONFIG_MACH_T5325=y
 CONFIG_ARCH_MXC=y
 CONFIG_MACH_IMX25_DT=y
 CONFIG_MACH_IMX27_DT=y
@@ -108,6 +107,8 @@ CONFIG_SND=y
 CONFIG_SND_SOC=y
 CONFIG_SND_KIRKWOOD_SOC=y
 CONFIG_SND_KIRKWOOD_SOC_T5325=y
+CONFIG_SND_SOC_ALC5623=y
+CONFIG_SND_SIMPLE_CARD=y
 # CONFIG_ABX500_CORE is not set
 CONFIG_REGULATOR=y
 CONFIG_REGULATOR_FIXED_VOLTAGE=y
diff --git a/arch/arm/configs/multi_v7_defconfig b/arch/arm/configs/multi_v7_defconfig
index d4e8a47a2f7..e2d62048e19 100644
--- a/arch/arm/configs/multi_v7_defconfig
+++ b/arch/arm/configs/multi_v7_defconfig
@@ -5,9 +5,11 @@ CONFIG_NO_HZ=y
 CONFIG_HIGH_RES_TIMERS=y
 CONFIG_BLK_DEV_INITRD=y
 CONFIG_EMBEDDED=y
+CONFIG_PERF_EVENTS=y
 CONFIG_MODULES=y
 CONFIG_MODULE_UNLOAD=y
 CONFIG_PARTITION_ADVANCED=y
+CONFIG_ARCH_VIRT=y
 CONFIG_ARCH_MVEBU=y
 CONFIG_MACH_ARMADA_370=y
 CONFIG_MACH_ARMADA_375=y
@@ -15,12 +17,12 @@ CONFIG_MACH_ARMADA_38X=y
 CONFIG_MACH_ARMADA_XP=y
 CONFIG_MACH_DOVE=y
 CONFIG_ARCH_BCM=y
-CONFIG_ARCH_BCM_5301X=y
 CONFIG_ARCH_BCM_MOBILE=y
+CONFIG_ARCH_BCM_5301X=y
 CONFIG_ARCH_BERLIN=y
 CONFIG_MACH_BERLIN_BG2=y
 CONFIG_MACH_BERLIN_BG2CD=y
-CONFIG_GPIO_PCA953X=y
+CONFIG_MACH_BERLIN_BG2Q=y
 CONFIG_ARCH_HIGHBANK=y
 CONFIG_ARCH_HI3xxx=y
 CONFIG_ARCH_KEYSTONE=y
@@ -34,8 +36,8 @@ CONFIG_ARCH_OMAP3=y
 CONFIG_ARCH_OMAP4=y
 CONFIG_SOC_OMAP5=y
 CONFIG_SOC_AM33XX=y
-CONFIG_SOC_DRA7XX=y
 CONFIG_SOC_AM43XX=y
+CONFIG_SOC_DRA7XX=y
 CONFIG_ARCH_QCOM=y
 CONFIG_ARCH_MSM8X60=y
 CONFIG_ARCH_MSM8960=y
@@ -47,6 +49,7 @@ CONFIG_ARCH_SPEAR13XX=y
 CONFIG_MACH_SPEAR1310=y
 CONFIG_MACH_SPEAR1340=y
 CONFIG_ARCH_STI=y
+CONFIG_ARCH_EXYNOS=y
 CONFIG_ARCH_SUNXI=y
 CONFIG_ARCH_SIRF=y
 CONFIG_ARCH_TEGRA=y
@@ -61,7 +64,6 @@ CONFIG_MACH_SNOWBALL=y
 CONFIG_MACH_UX500_DT=y
 CONFIG_ARCH_VEXPRESS=y
 CONFIG_ARCH_VEXPRESS_CA9X4=y
-CONFIG_ARCH_VIRT=y
 CONFIG_ARCH_WM8850=y
 CONFIG_ARCH_ZYNQ=y
 CONFIG_NEON=y
@@ -71,6 +73,7 @@ CONFIG_PCI_MSI=y
 CONFIG_PCI_MVEBU=y
 CONFIG_PCI_TEGRA=y
 CONFIG_SMP=y
+CONFIG_NR_CPUS=8
 CONFIG_HIGHPTE=y
 CONFIG_CMA=y
 CONFIG_ARM_APPENDED_DTB=y
@@ -96,6 +99,11 @@ CONFIG_INET6_IPCOMP=m
 CONFIG_IPV6_MIP6=m
 CONFIG_IPV6_TUNNEL=m
 CONFIG_IPV6_MULTIPLE_TABLES=y
+CONFIG_CAN=y
+CONFIG_CAN_RAW=y
+CONFIG_CAN_BCM=y
+CONFIG_CAN_DEV=y
+CONFIG_CAN_MCP251X=y
 CONFIG_CFG80211=m
 CONFIG_MAC80211=m
 CONFIG_RFKILL=y
@@ -112,15 +120,19 @@ CONFIG_BLK_DEV_LOOP=y
 CONFIG_ICS932S401=y
 CONFIG_APDS9802ALS=y
 CONFIG_ISL29003=y
+CONFIG_EEPROM_AT24=y
+CONFIG_EEPROM_SUNXI_SID=y
 CONFIG_BLK_DEV_SD=y
 CONFIG_BLK_DEV_SR=y
 CONFIG_SCSI_MULTI_LUN=y
 CONFIG_ATA=y
 CONFIG_SATA_AHCI_PLATFORM=y
+CONFIG_AHCI_SUNXI=y
 CONFIG_SATA_HIGHBANK=y
 CONFIG_SATA_MV=y
 CONFIG_NETDEVICES=y
 CONFIG_SUN4I_EMAC=y
+CONFIG_MACB=y
 CONFIG_NET_CALXEDA_XGMAC=y
 CONFIG_MV643XX_ETH=y
 CONFIG_MVNETA=y
@@ -153,6 +165,8 @@ CONFIG_SERIAL_8250_CONSOLE=y
 CONFIG_SERIAL_8250_DW=y
 CONFIG_SERIAL_AMBA_PL011=y
 CONFIG_SERIAL_AMBA_PL011_CONSOLE=y
+CONFIG_SERIAL_SAMSUNG=y
+CONFIG_SERIAL_SAMSUNG_CONSOLE=y
 CONFIG_SERIAL_SIRFSOC=y
 CONFIG_SERIAL_SIRFSOC_CONSOLE=y
 CONFIG_SERIAL_TEGRA=y
@@ -175,7 +189,9 @@ CONFIG_I2C_CHARDEV=y
 CONFIG_I2C_MUX=y
 CONFIG_I2C_MUX_PCA954x=y
 CONFIG_I2C_MUX_PINCTRL=y
+CONFIG_I2C_CADENCE=y
 CONFIG_I2C_DESIGNWARE_PLATFORM=y
+CONFIG_I2C_EXYNOS5=y
 CONFIG_I2C_MV64XXX=y
 CONFIG_I2C_SIRF=y
 CONFIG_I2C_TEGRA=y
@@ -184,6 +200,8 @@ CONFIG_SPI_OMAP24XX=y
 CONFIG_SPI_ORION=y
 CONFIG_SPI_PL022=y
 CONFIG_SPI_SIRF=y
+CONFIG_SPI_SUN4I=y
+CONFIG_SPI_SUN6I=y
 CONFIG_SPI_TEGRA114=y
 CONFIG_SPI_TEGRA20_SFLASH=y
 CONFIG_SPI_TEGRA20_SLINK=y
@@ -191,6 +209,8 @@ CONFIG_PINCTRL_AS3722=y
 CONFIG_PINCTRL_PALMAS=y
 CONFIG_GPIO_SYSFS=y
 CONFIG_GPIO_GENERIC_PLATFORM=y
+CONFIG_GPIO_DWAPB=y
+CONFIG_GPIO_PCA953X=y
 CONFIG_GPIO_PCA953X_IRQ=y
 CONFIG_GPIO_TWL4030=y
 CONFIG_GPIO_PALMAS=y
@@ -200,16 +220,19 @@ CONFIG_BATTERY_SBS=y
 CONFIG_CHARGER_TPS65090=y
 CONFIG_POWER_RESET_AS3722=y
 CONFIG_POWER_RESET_GPIO=y
+CONFIG_POWER_RESET_SUN6I=y
 CONFIG_SENSORS_LM90=y
 CONFIG_THERMAL=y
 CONFIG_DOVE_THERMAL=y
 CONFIG_ARMADA_THERMAL=y
 CONFIG_WATCHDOG=y
 CONFIG_ORION_WATCHDOG=y
+CONFIG_SUNXI_WATCHDOG=y
 CONFIG_MFD_AS3722=y
 CONFIG_MFD_CROS_EC=y
 CONFIG_MFD_CROS_EC_SPI=y
 CONFIG_MFD_MAX8907=y
+CONFIG_MFD_SEC_CORE=y
 CONFIG_MFD_PALMAS=y
 CONFIG_MFD_TPS65090=y
 CONFIG_MFD_TPS6586X=y
@@ -220,6 +243,8 @@ CONFIG_REGULATOR_AS3722=y
 CONFIG_REGULATOR_GPIO=y
 CONFIG_REGULATOR_MAX8907=y
 CONFIG_REGULATOR_PALMAS=y
+CONFIG_REGULATOR_S2MPS11=y
+CONFIG_REGULATOR_S5M8767=y
 CONFIG_REGULATOR_TPS51632=y
 CONFIG_REGULATOR_TPS62360=y
 CONFIG_REGULATOR_TPS65090=y
@@ -254,10 +279,13 @@ CONFIG_SND_SOC_TEGRA_ALC5632=y
 CONFIG_SND_SOC_TEGRA_MAX98090=y
 CONFIG_USB=y
 CONFIG_USB_XHCI_HCD=y
+CONFIG_USB_XHCI_MVEBU=y
 CONFIG_USB_EHCI_HCD=y
 CONFIG_USB_EHCI_TEGRA=y
 CONFIG_USB_EHCI_HCD_PLATFORM=y
 CONFIG_USB_ISP1760_HCD=y
+CONFIG_USB_OHCI_HCD=y
+CONFIG_USB_OHCI_HCD_PLATFORM=y
 CONFIG_USB_STORAGE=y
 CONFIG_USB_CHIPIDEA=y
 CONFIG_USB_CHIPIDEA_HOST=y
@@ -272,20 +300,28 @@ CONFIG_MMC=y
 CONFIG_MMC_BLOCK_MINORS=16
 CONFIG_MMC_ARMMMCI=y
 CONFIG_MMC_SDHCI=y
+CONFIG_MMC_SDHCI_OF_ARASAN=y
 CONFIG_MMC_SDHCI_ESDHC_IMX=y
-CONFIG_MMC_SDHCI_TEGRA=y
 CONFIG_MMC_SDHCI_DOVE=y
+CONFIG_MMC_SDHCI_TEGRA=y
+CONFIG_MMC_SDHCI_PXAV3=y
 CONFIG_MMC_SDHCI_SPEAR=y
+CONFIG_MMC_SDHCI_S3C=y
+CONFIG_MMC_SDHCI_S3C_DMA=y
 CONFIG_MMC_SDHCI_BCM_KONA=y
 CONFIG_MMC_OMAP=y
 CONFIG_MMC_OMAP_HS=y
 CONFIG_MMC_MVSDIO=y
+CONFIG_MMC_SUNXI=y
+CONFIG_MMC_DW=y
+CONFIG_MMC_DW_EXYNOS=y
 CONFIG_EDAC=y
 CONFIG_EDAC_MM_EDAC=y
 CONFIG_EDAC_HIGHBANK_MC=y
 CONFIG_EDAC_HIGHBANK_L2=y
 CONFIG_RTC_CLASS=y
 CONFIG_RTC_DRV_AS3722=y
+CONFIG_RTC_DRV_DS1307=y
 CONFIG_RTC_DRV_MAX8907=y
 CONFIG_RTC_DRV_PALMAS=y
 CONFIG_RTC_DRV_TWL4030=y
@@ -294,6 +330,7 @@ CONFIG_RTC_DRV_TPS65910=y
 CONFIG_RTC_DRV_EM3027=y
 CONFIG_RTC_DRV_PL031=y
 CONFIG_RTC_DRV_VT8500=y
+CONFIG_RTC_DRV_SUNXI=y
 CONFIG_RTC_DRV_MV=y
 CONFIG_RTC_DRV_TEGRA=y
 CONFIG_DMADEVICES=y
@@ -328,6 +365,7 @@ CONFIG_PWM=y
 CONFIG_PWM_TEGRA=y
 CONFIG_PWM_VT8500=y
 CONFIG_OMAP_USB2=y
+CONFIG_PHY_SUN4I_USB=y
 CONFIG_EXT4_FS=y
 CONFIG_VFAT_FS=y
 CONFIG_TMPFS=y
diff --git a/arch/arm/configs/mv78xx0_defconfig b/arch/arm/configs/mv78xx0_defconfig
index 1f08219c1b3..0dae1c1f007 100644
--- a/arch/arm/configs/mv78xx0_defconfig
+++ b/arch/arm/configs/mv78xx0_defconfig
@@ -80,7 +80,6 @@ CONFIG_I2C=y
 CONFIG_I2C_CHARDEV=y
 CONFIG_I2C_MV64XXX=y
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_EHCI_HCD=y
 CONFIG_USB_EHCI_ROOT_HUB_TT=y
 CONFIG_USB_EHCI_TT_NEWSCHED=y
diff --git a/arch/arm/configs/mvebu_v5_defconfig b/arch/arm/configs/mvebu_v5_defconfig
index 36484a37a1c..27c732fdf21 100644
--- a/arch/arm/configs/mvebu_v5_defconfig
+++ b/arch/arm/configs/mvebu_v5_defconfig
@@ -1,4 +1,5 @@
 CONFIG_SYSVIPC=y
+CONFIG_FHANDLE=y
 CONFIG_NO_HZ=y
 CONFIG_HIGH_RES_TIMERS=y
 CONFIG_LOG_BUF_SHIFT=19
@@ -11,7 +12,6 @@ CONFIG_MODULE_UNLOAD=y
 # CONFIG_ARCH_MULTI_V7 is not set
 CONFIG_ARCH_MVEBU=y
 CONFIG_MACH_KIRKWOOD=y
-CONFIG_MACH_T5325=y
 # CONFIG_CPU_FEROCEON_OLD_ID is not set
 CONFIG_PCI_MVEBU=y
 CONFIG_PREEMPT=y
@@ -50,6 +50,7 @@ CONFIG_MTD_PHYSMAP=y
 CONFIG_MTD_M25P80=y
 CONFIG_MTD_NAND=y
 CONFIG_MTD_NAND_ORION=y
+CONFIG_MTD_SPI_NOR=y
 CONFIG_BLK_DEV_LOOP=y
 CONFIG_EEPROM_AT24=y
 # CONFIG_SCSI_PROC_FS is not set
@@ -100,6 +101,8 @@ CONFIG_SND=y
 CONFIG_SND_SOC=y
 CONFIG_SND_KIRKWOOD_SOC=y
 CONFIG_SND_KIRKWOOD_SOC_T5325=y
+CONFIG_SND_SOC_ALC5623=y
+CONFIG_SND_SIMPLE_CARD=y
 CONFIG_REGULATOR=y
 CONFIG_REGULATOR_FIXED_VOLTAGE=y
 CONFIG_HID_DRAGONRISE=y
diff --git a/arch/arm/configs/mvebu_v7_defconfig b/arch/arm/configs/mvebu_v7_defconfig
index a34713d8db9..e11170e3744 100644
--- a/arch/arm/configs/mvebu_v7_defconfig
+++ b/arch/arm/configs/mvebu_v7_defconfig
@@ -1,5 +1,6 @@
 CONFIG_EXPERIMENTAL=y
 CONFIG_SYSVIPC=y
+CONFIG_FHANDLE=y
 CONFIG_IRQ_DOMAIN_DEBUG=y
 CONFIG_HIGH_RES_TIMERS=y
 CONFIG_LOG_BUF_SHIFT=14
@@ -17,6 +18,7 @@ CONFIG_NEON=y
 # CONFIG_CACHE_L2X0 is not set
 # CONFIG_SWP_EMULATE is not set
 CONFIG_PCI=y
+CONFIG_PCI_MSI=y
 CONFIG_PCI_MVEBU=y
 CONFIG_SMP=y
 CONFIG_AEABI=y
@@ -29,6 +31,9 @@ CONFIG_ARM_ATAG_DTB_COMPAT=y
 CONFIG_VFP=y
 CONFIG_NET=y
 CONFIG_INET=y
+CONFIG_IP_PNP=y
+CONFIG_IP_PNP_DHCP=y
+CONFIG_IP_PNP_BOOTP=y
 CONFIG_BT=y
 CONFIG_BT_MRVL=y
 CONFIG_BT_MRVL_SDIO=y
@@ -36,6 +41,7 @@ CONFIG_CFG80211=y
 CONFIG_UEVENT_HELPER_PATH="/sbin/hotplug"
 CONFIG_BLK_DEV_SD=y
 CONFIG_ATA=y
+CONFIG_AHCI_MVEBU=y
 CONFIG_SATA_MV=y
 CONFIG_NETDEVICES=y
 CONFIG_MVNETA=y
@@ -53,6 +59,7 @@ CONFIG_I2C_MV64XXX=y
 CONFIG_MTD=y
 CONFIG_MTD_CHAR=y
 CONFIG_MTD_M25P80=y
+CONFIG_MTD_SPI_NOR=y
 CONFIG_MTD_CFI=y
 CONFIG_MTD_CFI_INTELEXT=y
 CONFIG_MTD_CFI_AMDSTD=y
@@ -78,7 +85,9 @@ CONFIG_USB_EHCI_HCD=y
 CONFIG_USB_EHCI_ROOT_HUB_TT=y
 CONFIG_USB_STORAGE=y
 CONFIG_USB_XHCI_HCD=y
+CONFIG_USB_XHCI_MVEBU=y
 CONFIG_MMC=y
+CONFIG_MMC_SDHCI_PXAV3=y
 CONFIG_MMC_MVSDIO=y
 CONFIG_NEW_LEDS=y
 CONFIG_LEDS_GPIO=y
@@ -103,6 +112,8 @@ CONFIG_UDF_FS=m
 CONFIG_MSDOS_FS=y
 CONFIG_VFAT_FS=y
 CONFIG_TMPFS=y
+CONFIG_NFS_FS=y
+CONFIG_ROOT_NFS=y
 CONFIG_NLS_CODEPAGE_437=y
 CONFIG_NLS_CODEPAGE_850=y
 CONFIG_NLS_ISO8859_1=y
diff --git a/arch/arm/configs/mxs_defconfig b/arch/arm/configs/mxs_defconfig
index 6150108e15d..a9f992335eb 100644
--- a/arch/arm/configs/mxs_defconfig
+++ b/arch/arm/configs/mxs_defconfig
@@ -26,7 +26,6 @@ CONFIG_ARCH_MXS=y
 # CONFIG_ARM_THUMB is not set
 CONFIG_PREEMPT_VOLUNTARY=y
 CONFIG_AEABI=y
-CONFIG_FPE_NWFPE=y
 CONFIG_NET=y
 CONFIG_PACKET=y
 CONFIG_UNIX=y
@@ -51,10 +50,10 @@ CONFIG_MTD_CMDLINE_PARTS=y
 CONFIG_MTD_BLOCK=y
 CONFIG_MTD_DATAFLASH=y
 CONFIG_MTD_M25P80=y
-# CONFIG_M25PXX_USE_FAST_READ is not set
 CONFIG_MTD_SST25L=y
 CONFIG_MTD_NAND=y
 CONFIG_MTD_NAND_GPMI_NAND=y
+CONFIG_MTD_SPI_NOR=y
 CONFIG_MTD_UBI=y
 # CONFIG_BLK_DEV is not set
 CONFIG_EEPROM_AT24=y
@@ -120,7 +119,6 @@ CONFIG_USB_GADGET=y
 CONFIG_USB_ETH=m
 CONFIG_USB_MASS_STORAGE=m
 CONFIG_MMC=y
-CONFIG_MMC_UNSAFE_RESUME=y
 CONFIG_MMC_MXS=y
 CONFIG_NEW_LEDS=y
 CONFIG_LEDS_CLASS=y
@@ -138,7 +136,6 @@ CONFIG_DMADEVICES=y
 CONFIG_MXS_DMA=y
 CONFIG_STAGING=y
 CONFIG_MXS_LRADC=y
-CONFIG_COMMON_CLK_DEBUG=y
 CONFIG_IIO=y
 CONFIG_IIO_SYSFS_TRIGGER=y
 CONFIG_PWM=y
@@ -180,7 +177,7 @@ CONFIG_BLK_DEV_IO_TRACE=y
 CONFIG_STRICT_DEVMEM=y
 CONFIG_DEBUG_USER=y
 # CONFIG_CRYPTO_ANSI_CPRNG is not set
-# CONFIG_CRYPTO_HW is not set
+CONFIG_CRYPTO_DEV_MXS_DCP=y
 CONFIG_CRC_ITU_T=m
 CONFIG_CRC7=m
 CONFIG_FONTS=y
diff --git a/arch/arm/configs/neponset_defconfig b/arch/arm/configs/neponset_defconfig
index d7dc9922cff..460dca4a4f9 100644
--- a/arch/arm/configs/neponset_defconfig
+++ b/arch/arm/configs/neponset_defconfig
@@ -68,8 +68,6 @@ CONFIG_SOUND=y
 CONFIG_SOUND_PRIME=y
 # CONFIG_USB_HID is not set
 CONFIG_USB=m
-CONFIG_USB_DEBUG=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=m
 CONFIG_USB_OHCI_HCD=m
 CONFIG_USB_STORAGE=m
diff --git a/arch/arm/configs/omap1_defconfig b/arch/arm/configs/omap1_defconfig
index d74edbad18f..ce541bb3c2d 100644
--- a/arch/arm/configs/omap1_defconfig
+++ b/arch/arm/configs/omap1_defconfig
@@ -197,8 +197,6 @@ 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
 CONFIG_USB_MON=y
 CONFIG_USB_OHCI_HCD=y
diff --git a/arch/arm/configs/omap2plus_defconfig b/arch/arm/configs/omap2plus_defconfig
index a9667957b75..59066cf0271 100644
--- a/arch/arm/configs/omap2plus_defconfig
+++ b/arch/arm/configs/omap2plus_defconfig
@@ -21,6 +21,8 @@ CONFIG_MODULE_SRCVERSION_ALL=y
 # CONFIG_BLK_DEV_BSG is not set
 CONFIG_PARTITION_ADVANCED=y
 CONFIG_ARCH_MULTI_V6=y
+CONFIG_POWER_AVS_OMAP=y
+CONFIG_POWER_AVS_OMAP_CLASS3=y
 CONFIG_OMAP_RESET_CLOCKS=y
 CONFIG_OMAP_MUX_DEBUG=y
 CONFIG_ARCH_OMAP2=y
@@ -42,6 +44,7 @@ CONFIG_ARM_ATAG_DTB_COMPAT=y
 CONFIG_CMDLINE="root=/dev/mmcblk0p2 rootwait console=ttyO2,115200"
 CONFIG_KEXEC=y
 CONFIG_FPE_NWFPE=y
+CONFIG_CPU_IDLE=y
 CONFIG_BINFMT_MISC=y
 CONFIG_PM_DEBUG=y
 CONFIG_NET=y
@@ -159,11 +162,14 @@ CONFIG_GPIO_SYSFS=y
 CONFIG_GPIO_TWL4030=y
 CONFIG_W1=y
 CONFIG_POWER_SUPPLY=y
+CONFIG_POWER_AVS=y
 CONFIG_SENSORS_LM75=m
 CONFIG_THERMAL=y
 CONFIG_THERMAL_GOV_FAIR_SHARE=y
 CONFIG_THERMAL_GOV_USER_SPACE=y
+CONFIG_CPU_THERMAL=y
 CONFIG_TI_SOC_THERMAL=y
+CONFIG_TI_THERMAL=y
 CONFIG_OMAP4_THERMAL=y
 CONFIG_OMAP5_THERMAL=y
 CONFIG_DRA752_THERMAL=y
@@ -177,6 +183,7 @@ CONFIG_MFD_TPS65910=y
 CONFIG_TWL6040_CORE=y
 CONFIG_REGULATOR_FIXED_VOLTAGE=y
 CONFIG_REGULATOR_PALMAS=y
+CONFIG_REGULATOR_TI_ABB=y
 CONFIG_REGULATOR_TPS65023=y
 CONFIG_REGULATOR_TPS6507X=y
 CONFIG_REGULATOR_TPS65217=y
@@ -217,7 +224,6 @@ CONFIG_SND_OMAP_SOC_OMAP_TWL4030=m
 CONFIG_SND_OMAP_SOC_OMAP_ABE_TWL6040=m
 CONFIG_SND_OMAP_SOC_OMAP3_PANDORA=m
 CONFIG_USB=y
-CONFIG_USB_DEBUG=y
 CONFIG_USB_ANNOUNCE_NEW_DEVICES=y
 CONFIG_USB_MON=y
 CONFIG_USB_WDM=y
@@ -226,7 +232,7 @@ CONFIG_USB_DWC3=m
 CONFIG_USB_TEST=y
 CONFIG_NOP_USB_XCEIV=y
 CONFIG_OMAP_USB2=y
-CONFIG_OMAP_USB3=y
+CONFIG_TI_PIPE3=y
 CONFIG_AM335X_PHY_USB=y
 CONFIG_USB_GADGET=y
 CONFIG_USB_GADGET_DEBUG=y
@@ -239,6 +245,7 @@ CONFIG_SDIO_UART=y
 CONFIG_MMC_OMAP=y
 CONFIG_MMC_OMAP_HS=y
 CONFIG_NEW_LEDS=y
+CONFIG_LEDS_CLASS=y
 CONFIG_LEDS_GPIO=y
 CONFIG_LEDS_TRIGGERS=y
 CONFIG_LEDS_TRIGGER_TIMER=y
diff --git a/arch/arm/configs/pcm027_defconfig b/arch/arm/configs/pcm027_defconfig
index 2f136c30a98..0a847d04ddc 100644
--- a/arch/arm/configs/pcm027_defconfig
+++ b/arch/arm/configs/pcm027_defconfig
@@ -76,7 +76,6 @@ CONFIG_SND_PCM_OSS=y
 CONFIG_SND_PXA2XX_AC97=y
 # CONFIG_HID_SUPPORT is not set
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_OHCI_HCD=y
 CONFIG_USB_STORAGE=y
 CONFIG_MMC=y
diff --git a/arch/arm/configs/qcom_defconfig b/arch/arm/configs/qcom_defconfig
new file mode 100644
index 00000000000..42ebd72799e
--- /dev/null
+++ b/arch/arm/configs/qcom_defconfig
@@ -0,0 +1,165 @@
+CONFIG_SYSVIPC=y
+CONFIG_NO_HZ=y
+CONFIG_HIGH_RES_TIMERS=y
+CONFIG_IKCONFIG=y
+CONFIG_IKCONFIG_PROC=y
+CONFIG_BLK_DEV_INITRD=y
+CONFIG_SYSCTL_SYSCALL=y
+CONFIG_KALLSYMS_ALL=y
+CONFIG_EMBEDDED=y
+# CONFIG_SLUB_DEBUG is not set
+# CONFIG_COMPAT_BRK is not set
+CONFIG_PROFILING=y
+CONFIG_OPROFILE=y
+CONFIG_KPROBES=y
+CONFIG_MODULES=y
+CONFIG_MODULE_UNLOAD=y
+CONFIG_MODULE_FORCE_UNLOAD=y
+CONFIG_MODVERSIONS=y
+CONFIG_PARTITION_ADVANCED=y
+CONFIG_ARCH_QCOM=y
+CONFIG_ARCH_MSM8X60=y
+CONFIG_ARCH_MSM8960=y
+CONFIG_ARCH_MSM8974=y
+CONFIG_SMP=y
+CONFIG_PREEMPT=y
+CONFIG_AEABI=y
+CONFIG_HIGHMEM=y
+CONFIG_HIGHPTE=y
+CONFIG_CLEANCACHE=y
+CONFIG_ARM_APPENDED_DTB=y
+CONFIG_ARM_ATAG_DTB_COMPAT=y
+CONFIG_VFP=y
+CONFIG_NEON=y
+# CONFIG_CORE_DUMP_DEFAULT_ELF_HEADERS is not set
+CONFIG_NET=y
+CONFIG_PACKET=y
+CONFIG_UNIX=y
+CONFIG_INET=y
+CONFIG_IP_ADVANCED_ROUTER=y
+CONFIG_IP_MULTIPLE_TABLES=y
+CONFIG_IP_ROUTE_VERBOSE=y
+CONFIG_IP_PNP=y
+CONFIG_IP_PNP_DHCP=y
+# CONFIG_INET_XFRM_MODE_TRANSPORT is not set
+# CONFIG_INET_XFRM_MODE_TUNNEL is not set
+# CONFIG_INET_XFRM_MODE_BEET is not set
+# CONFIG_INET_LRO is not set
+# CONFIG_IPV6 is not set
+CONFIG_CFG80211=y
+CONFIG_RFKILL=y
+CONFIG_DEVTMPFS=y
+CONFIG_DEVTMPFS_MOUNT=y
+CONFIG_MTD=y
+CONFIG_MTD_BLOCK=y
+CONFIG_MTD_M25P80=y
+CONFIG_BLK_DEV_LOOP=y
+CONFIG_BLK_DEV_RAM=y
+CONFIG_SCSI=y
+CONFIG_SCSI_TGT=y
+CONFIG_BLK_DEV_SD=y
+CONFIG_CHR_DEV_SG=y
+CONFIG_CHR_DEV_SCH=y
+CONFIG_SCSI_MULTI_LUN=y
+CONFIG_SCSI_CONSTANTS=y
+CONFIG_SCSI_LOGGING=y
+CONFIG_SCSI_SCAN_ASYNC=y
+CONFIG_NETDEVICES=y
+CONFIG_DUMMY=y
+CONFIG_MDIO_BITBANG=y
+CONFIG_MDIO_GPIO=y
+CONFIG_SLIP=y
+CONFIG_SLIP_COMPRESSED=y
+CONFIG_SLIP_MODE_SLIP6=y
+CONFIG_USB_USBNET=y
+# CONFIG_USB_NET_AX8817X is not set
+# CONFIG_USB_NET_ZAURUS is not set
+CONFIG_INPUT_EVDEV=y
+# CONFIG_KEYBOARD_ATKBD is not set
+# CONFIG_MOUSE_PS2 is not set
+CONFIG_INPUT_JOYSTICK=y
+CONFIG_INPUT_TOUCHSCREEN=y
+CONFIG_INPUT_MISC=y
+CONFIG_INPUT_UINPUT=y
+CONFIG_SERIO_LIBPS2=y
+# CONFIG_LEGACY_PTYS is not set
+CONFIG_SERIAL_MSM=y
+CONFIG_SERIAL_MSM_CONSOLE=y
+CONFIG_HW_RANDOM=y
+CONFIG_HW_RANDOM_MSM=y
+CONFIG_I2C=y
+CONFIG_I2C_CHARDEV=y
+CONFIG_I2C_QUP=y
+CONFIG_SPI=y
+CONFIG_SPI_QUP=y
+CONFIG_SPMI=y
+CONFIG_PINCTRL_APQ8064=y
+CONFIG_PINCTRL_IPQ8064=y
+CONFIG_PINCTRL_MSM8X74=y
+CONFIG_DEBUG_GPIO=y
+CONFIG_GPIO_SYSFS=y
+CONFIG_POWER_SUPPLY=y
+CONFIG_POWER_RESET=y
+CONFIG_POWER_RESET_MSM=y
+CONFIG_THERMAL=y
+CONFIG_REGULATOR=y
+CONFIG_MEDIA_SUPPORT=y
+CONFIG_FB=y
+CONFIG_SOUND=y
+CONFIG_SND=y
+CONFIG_SND_DYNAMIC_MINORS=y
+# CONFIG_SND_ARM is not set
+# CONFIG_SND_SPI is not set
+# CONFIG_SND_USB is not set
+CONFIG_SND_SOC=y
+CONFIG_HID_BATTERY_STRENGTH=y
+CONFIG_USB=y
+CONFIG_USB_ANNOUNCE_NEW_DEVICES=y
+CONFIG_USB_MON=y
+CONFIG_USB_EHCI_HCD=y
+CONFIG_USB_ACM=y
+CONFIG_USB_SERIAL=y
+CONFIG_USB_GADGET=y
+CONFIG_USB_GADGET_DEBUG_FILES=y
+CONFIG_USB_GADGET_VBUS_DRAW=500
+CONFIG_MMC=y
+CONFIG_MMC_BLOCK_MINORS=16
+CONFIG_MMC_SDHCI=y
+CONFIG_MMC_SDHCI_PLTFM=y
+CONFIG_MMC_SDHCI_MSM=y
+CONFIG_RTC_CLASS=y
+CONFIG_DMADEVICES=y
+CONFIG_QCOM_BAM_DMA=y
+CONFIG_STAGING=y
+CONFIG_QCOM_GSBI=y
+CONFIG_COMMON_CLK_QCOM=y
+CONFIG_MSM_GCC_8660=y
+CONFIG_MSM_MMCC_8960=y
+CONFIG_MSM_MMCC_8974=y
+CONFIG_MSM_IOMMU=y
+CONFIG_GENERIC_PHY=y
+CONFIG_EXT2_FS=y
+CONFIG_EXT2_FS_XATTR=y
+CONFIG_EXT3_FS=y
+# CONFIG_EXT3_DEFAULTS_TO_ORDERED is not set
+CONFIG_EXT4_FS=y
+CONFIG_FUSE_FS=y
+CONFIG_VFAT_FS=y
+CONFIG_TMPFS=y
+CONFIG_JFFS2_FS=y
+CONFIG_NFS_FS=y
+CONFIG_NFS_V3_ACL=y
+CONFIG_NFS_V4=y
+CONFIG_CIFS=y
+CONFIG_NLS_CODEPAGE_437=y
+CONFIG_NLS_ASCII=y
+CONFIG_NLS_ISO8859_1=y
+CONFIG_NLS_UTF8=y
+CONFIG_PRINTK_TIME=y
+CONFIG_DYNAMIC_DEBUG=y
+CONFIG_DEBUG_INFO=y
+CONFIG_MAGIC_SYSRQ=y
+CONFIG_LOCKUP_DETECTOR=y
+# CONFIG_DETECT_HUNG_TASK is not set
+# CONFIG_SCHED_DEBUG is not set
+CONFIG_TIMER_STATS=y
diff --git a/arch/arm/configs/raumfeld_defconfig b/arch/arm/configs/raumfeld_defconfig
index f7caa909b40..3d833aea545 100644
--- a/arch/arm/configs/raumfeld_defconfig
+++ b/arch/arm/configs/raumfeld_defconfig
@@ -122,7 +122,6 @@ CONFIG_HID_TOPSEED=y
 CONFIG_HID_THRUSTMASTER=y
 CONFIG_HID_ZEROPLUS=y
 CONFIG_USB=y
-CONFIG_USB_DEBUG=y
 CONFIG_USB_ANNOUNCE_NEW_DEVICES=y
 CONFIG_USB_MON=y
 CONFIG_USB_OHCI_HCD=y
diff --git a/arch/arm/configs/realview-smp_defconfig b/arch/arm/configs/realview-smp_defconfig
index abe61bf379d..1da5d9e4822 100644
--- a/arch/arm/configs/realview-smp_defconfig
+++ b/arch/arm/configs/realview-smp_defconfig
@@ -76,8 +76,10 @@ CONFIG_MMC=y
 CONFIG_MMC_ARMMMCI=y
 CONFIG_NEW_LEDS=y
 CONFIG_LEDS_CLASS=y
+CONFIG_LEDS_VERSATILE=y
 CONFIG_LEDS_TRIGGERS=y
 CONFIG_LEDS_TRIGGER_HEARTBEAT=y
+CONFIG_LEDS_TRIGGER_CPU=y
 CONFIG_RTC_CLASS=y
 CONFIG_RTC_DRV_DS1307=y
 CONFIG_RTC_DRV_PL031=y
diff --git a/arch/arm/configs/realview_defconfig b/arch/arm/configs/realview_defconfig
index 7079cbe898a..d02e9d911bb 100644
--- a/arch/arm/configs/realview_defconfig
+++ b/arch/arm/configs/realview_defconfig
@@ -75,8 +75,10 @@ CONFIG_MMC=y
 CONFIG_MMC_ARMMMCI=y
 CONFIG_NEW_LEDS=y
 CONFIG_LEDS_CLASS=y
+CONFIG_LEDS_VERSATILE=y
 CONFIG_LEDS_TRIGGERS=y
 CONFIG_LEDS_TRIGGER_HEARTBEAT=y
+CONFIG_LEDS_TRIGGER_CPU=y
 CONFIG_RTC_CLASS=y
 CONFIG_RTC_DRV_DS1307=y
 CONFIG_RTC_DRV_PL031=y
diff --git a/arch/arm/configs/s3c2410_defconfig b/arch/arm/configs/s3c2410_defconfig
index 193448f3128..eb4d204bff4 100644
--- a/arch/arm/configs/s3c2410_defconfig
+++ b/arch/arm/configs/s3c2410_defconfig
@@ -324,7 +324,6 @@ CONFIG_SND_USB_CAIAQ=m
 CONFIG_SND_SOC=y
 # CONFIG_USB_HID is not set
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=y
 CONFIG_USB_OHCI_HCD=y
 CONFIG_USB_ACM=m
diff --git a/arch/arm/configs/s3c6400_defconfig b/arch/arm/configs/s3c6400_defconfig
index 3a186d653da..e2f9fa5bb54 100644
--- a/arch/arm/configs/s3c6400_defconfig
+++ b/arch/arm/configs/s3c6400_defconfig
@@ -56,7 +56,6 @@ CONFIG_SND_S3C24XX_SOC=m
 CONFIG_SND_SOC_SMDK_WM9713=m
 CONFIG_USB=y
 CONFIG_USB_ANNOUNCE_NEW_DEVICES=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_OHCI_HCD=y
 CONFIG_USB_ACM=m
 CONFIG_USB_PRINTER=m
diff --git a/arch/arm/configs/sama5_defconfig b/arch/arm/configs/sama5_defconfig
index dc3881e0763..4414990521d 100644
--- a/arch/arm/configs/sama5_defconfig
+++ b/arch/arm/configs/sama5_defconfig
@@ -122,7 +122,6 @@ CONFIG_KEYBOARD_GPIO=y
 # CONFIG_INPUT_MOUSE is not set
 CONFIG_INPUT_TOUCHSCREEN=y
 CONFIG_TOUCHSCREEN_ATMEL_MXT=y
-CONFIG_TOUCHSCREEN_ATMEL_TSADCC=y
 # CONFIG_SERIO is not set
 CONFIG_LEGACY_PTY_COUNT=4
 CONFIG_SERIAL_ATMEL=y
@@ -138,6 +137,8 @@ CONFIG_SPI_GPIO=y
 CONFIG_GPIO_SYSFS=y
 # CONFIG_HWMON is not set
 CONFIG_SSB=m
+CONFIG_REGULATOR=y
+CONFIG_REGULATOR_ACT8865=y
 CONFIG_FB=y
 CONFIG_BACKLIGHT_LCD_SUPPORT=y
 # CONFIG_LCD_CLASS_DEVICE is not set
diff --git a/arch/arm/configs/shmobile_defconfig b/arch/arm/configs/shmobile_defconfig
index 83b07258a38..6d6437cbbc5 100644
--- a/arch/arm/configs/shmobile_defconfig
+++ b/arch/arm/configs/shmobile_defconfig
@@ -25,6 +25,7 @@ CONFIG_SCHED_MC=y
 CONFIG_HAVE_ARM_ARCH_TIMER=y
 CONFIG_NR_CPUS=8
 CONFIG_AEABI=y
+CONFIG_HIGHMEM=y
 CONFIG_ZBOOT_ROM_TEXT=0x0
 CONFIG_ZBOOT_ROM_BSS=0x0
 CONFIG_ARM_APPENDED_DTB=y
@@ -43,6 +44,7 @@ CONFIG_DEVTMPFS=y
 CONFIG_DEVTMPFS_MOUNT=y
 CONFIG_MTD=y
 CONFIG_MTD_M25P80=y
+CONFIG_EEPROM_AT24=y
 CONFIG_BLK_DEV_SD=y
 CONFIG_ATA=y
 CONFIG_SATA_RCAR=y
@@ -75,9 +77,11 @@ CONFIG_SERIAL_SH_SCI=y
 CONFIG_SERIAL_SH_SCI_NR_UARTS=20
 CONFIG_SERIAL_SH_SCI_CONSOLE=y
 CONFIG_I2C_GPIO=y
+CONFIG_I2C_SH_MOBILE=y
 CONFIG_I2C_RCAR=y
 CONFIG_SPI=y
 CONFIG_SPI_RSPI=y
+CONFIG_SPI_SH_MSIOF=y
 CONFIG_GPIO_EM=y
 CONFIG_GPIO_RCAR=y
 # CONFIG_HWMON is not set
@@ -88,10 +92,14 @@ CONFIG_REGULATOR_FIXED_VOLTAGE=y
 CONFIG_REGULATOR_GPIO=y
 CONFIG_MEDIA_SUPPORT=y
 CONFIG_MEDIA_CAMERA_SUPPORT=y
+CONFIG_MEDIA_CONTROLLER=y
+CONFIG_VIDEO_V4L2_SUBDEV_API=y
 CONFIG_V4L_PLATFORM_DRIVERS=y
 CONFIG_SOC_CAMERA=y
 CONFIG_SOC_CAMERA_PLATFORM=y
 CONFIG_VIDEO_RCAR_VIN=y
+CONFIG_V4L_MEM2MEM_DRIVERS=y
+CONFIG_VIDEO_RENESAS_VSP1=y
 # CONFIG_MEDIA_SUBDRV_AUTOSELECT is not set
 CONFIG_VIDEO_ADV7180=y
 CONFIG_DRM=y
@@ -100,7 +108,13 @@ CONFIG_SOUND=y
 CONFIG_SND=y
 CONFIG_SND_SOC=y
 CONFIG_SND_SOC_RCAR=y
+CONFIG_USB=y
 CONFIG_USB_RCAR_GEN2_PHY=y
+CONFIG_USB_EHCI_HCD=y
+CONFIG_USB_OHCI_HCD=y
+CONFIG_USB_RENESAS_USBHS=y
+CONFIG_USB_GADGET=y
+CONFIG_USB_RENESAS_USBHS_UDC=y
 CONFIG_MMC=y
 CONFIG_MMC_SDHI=y
 CONFIG_MMC_SH_MMCIF=y
diff --git a/arch/arm/configs/spitz_defconfig b/arch/arm/configs/spitz_defconfig
index 2e0419d1b96..a1ede1966ba 100644
--- a/arch/arm/configs/spitz_defconfig
+++ b/arch/arm/configs/spitz_defconfig
@@ -166,7 +166,6 @@ CONFIG_HID_SAMSUNG=m
 CONFIG_HID_SONY=m
 CONFIG_HID_SUNPLUS=m
 CONFIG_USB=m
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=m
 CONFIG_USB_OHCI_HCD=m
 CONFIG_USB_SL811_HCD=m
diff --git a/arch/arm/configs/sunxi_defconfig b/arch/arm/configs/sunxi_defconfig
index b5df4a511b0..7209bfd6207 100644
--- a/arch/arm/configs/sunxi_defconfig
+++ b/arch/arm/configs/sunxi_defconfig
@@ -1,13 +1,17 @@
 CONFIG_NO_HZ=y
 CONFIG_HIGH_RES_TIMERS=y
 CONFIG_BLK_DEV_INITRD=y
+CONFIG_PERF_EVENTS=y
 CONFIG_ARCH_SUNXI=y
 CONFIG_SMP=y
 CONFIG_AEABI=y
 CONFIG_HIGHMEM=y
 CONFIG_HIGHPTE=y
+CONFIG_ARM_APPENDED_DTB=y
+CONFIG_ARM_ATAG_DTB_COMPAT=y
 CONFIG_VFP=y
 CONFIG_NEON=y
+CONFIG_PM_RUNTIME=y
 CONFIG_NET=y
 CONFIG_PACKET=y
 CONFIG_UNIX=y
@@ -25,8 +29,12 @@ CONFIG_IP_PNP_BOOTP=y
 CONFIG_DEVTMPFS=y
 CONFIG_DEVTMPFS_MOUNT=y
 CONFIG_EEPROM_SUNXI_SID=y
+CONFIG_BLK_DEV_SD=y
+CONFIG_ATA=y
+CONFIG_AHCI_SUNXI=y
 CONFIG_NETDEVICES=y
 CONFIG_SUN4I_EMAC=y
+# CONFIG_NET_VENDOR_ARC is not set
 # CONFIG_NET_CADENCE is not set
 # CONFIG_NET_VENDOR_BROADCOM is not set
 # CONFIG_NET_VENDOR_CIRRUS is not set
@@ -34,38 +42,66 @@ CONFIG_SUN4I_EMAC=y
 # CONFIG_NET_VENDOR_INTEL is not set
 # CONFIG_NET_VENDOR_MARVELL is not set
 # CONFIG_NET_VENDOR_MICREL is not set
+# CONFIG_NET_VENDOR_MICROCHIP is not set
 # CONFIG_NET_VENDOR_NATSEMI is not set
+# CONFIG_NET_VENDOR_SAMSUNG is not set
 # CONFIG_NET_VENDOR_SEEQ is not set
 # CONFIG_NET_VENDOR_SMSC is not set
-# CONFIG_NET_VENDOR_STMICRO is not set
+CONFIG_STMMAC_ETH=y
+# CONFIG_NET_VENDOR_VIA is not set
 # CONFIG_NET_VENDOR_WIZNET is not set
 # CONFIG_WLAN is not set
+# CONFIG_INPUT_MOUSEDEV is not set
+# CONFIG_INPUT_KEYBOARD is not set
+# CONFIG_INPUT_MOUSE is not set
 CONFIG_SERIAL_8250=y
 CONFIG_SERIAL_8250_CONSOLE=y
 CONFIG_SERIAL_8250_NR_UARTS=8
 CONFIG_SERIAL_8250_RUNTIME_UARTS=8
 CONFIG_SERIAL_8250_DW=y
+CONFIG_SERIAL_OF_PLATFORM=y
+# CONFIG_HW_RANDOM is not set
 CONFIG_I2C=y
-# CONFIG_I2C_COMPAT is not set
 CONFIG_I2C_CHARDEV=y
 CONFIG_I2C_MV64XXX=y
 CONFIG_SPI=y
+CONFIG_SPI_SUN4I=y
 CONFIG_SPI_SUN6I=y
 CONFIG_GPIO_SYSFS=y
+CONFIG_POWER_SUPPLY=y
+CONFIG_POWER_RESET=y
+CONFIG_POWER_RESET_SUN6I=y
 # CONFIG_HWMON is not set
 CONFIG_WATCHDOG=y
 CONFIG_SUNXI_WATCHDOG=y
-# CONFIG_USB_SUPPORT is not set
+CONFIG_MFD_AXP20X=y
+CONFIG_REGULATOR_VIRTUAL_CONSUMER=y
+CONFIG_REGULATOR_GPIO=y
+CONFIG_USB=y
+CONFIG_USB_EHCI_HCD=y
+CONFIG_USB_EHCI_HCD_PLATFORM=y
+CONFIG_USB_OHCI_HCD=y
+CONFIG_USB_OHCI_HCD_PLATFORM=y
+CONFIG_MMC=y
+CONFIG_MMC_SUNXI=y
 CONFIG_NEW_LEDS=y
 CONFIG_LEDS_CLASS=y
 CONFIG_LEDS_GPIO=y
 CONFIG_LEDS_TRIGGERS=y
 CONFIG_LEDS_TRIGGER_HEARTBEAT=y
 CONFIG_LEDS_TRIGGER_DEFAULT_ON=y
-CONFIG_COMMON_CLK_DEBUG=y
+CONFIG_RTC_CLASS=y
+# CONFIG_RTC_INTF_SYSFS is not set
+# CONFIG_RTC_INTF_PROC is not set
+CONFIG_RTC_DRV_SUNXI=y
 # CONFIG_IOMMU_SUPPORT is not set
+CONFIG_PHY_SUN4I_USB=y
+CONFIG_EXT4_FS=y
+CONFIG_VFAT_FS=y
 CONFIG_TMPFS=y
 CONFIG_NFS_FS=y
+CONFIG_NFS_V3_ACL=y
+CONFIG_NFS_V4=y
 CONFIG_ROOT_NFS=y
-CONFIG_NLS=y
 CONFIG_PRINTK_TIME=y
+CONFIG_DEBUG_FS=y
diff --git a/arch/arm/configs/tct_hammer_defconfig b/arch/arm/configs/tct_hammer_defconfig
index 71277a1591b..7209a2caefc 100644
--- a/arch/arm/configs/tct_hammer_defconfig
+++ b/arch/arm/configs/tct_hammer_defconfig
@@ -47,7 +47,6 @@ CONFIG_BLK_DEV_RAM_SIZE=10240
 # CONFIG_VGA_CONSOLE is not set
 # CONFIG_HID_SUPPORT is not set
 CONFIG_USB=y
-CONFIG_USB_DEBUG=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_MON=y
 CONFIG_USB_OHCI_HCD=y
diff --git a/arch/arm/configs/tegra_defconfig b/arch/arm/configs/tegra_defconfig
index 2926281368a..fb25e2982f6 100644
--- a/arch/arm/configs/tegra_defconfig
+++ b/arch/arm/configs/tegra_defconfig
@@ -73,6 +73,11 @@ CONFIG_INET6_IPCOMP=y
 CONFIG_IPV6_MIP6=y
 CONFIG_IPV6_TUNNEL=y
 CONFIG_IPV6_MULTIPLE_TABLES=y
+CONFIG_CAN=y
+CONFIG_CAN_RAW=y
+CONFIG_CAN_BCM=y
+CONFIG_CAN_DEV=y
+CONFIG_CAN_MCP251X=y
 CONFIG_BT=y
 CONFIG_BT_RFCOMM=y
 CONFIG_BT_BNEP=y
@@ -90,6 +95,7 @@ CONFIG_DMA_CMA=y
 CONFIG_CMA_SIZE_MBYTES=64
 CONFIG_MTD=y
 CONFIG_MTD_M25P80=y
+CONFIG_MTD_SPI_NOR=y
 CONFIG_PROC_DEVICETREE=y
 CONFIG_BLK_DEV_LOOP=y
 CONFIG_AD525X_DPOT=y
@@ -97,6 +103,7 @@ CONFIG_AD525X_DPOT_I2C=y
 CONFIG_ICS932S401=y
 CONFIG_APDS9802ALS=y
 CONFIG_ISL29003=y
+CONFIG_EEPROM_AT24=y
 CONFIG_SCSI=y
 CONFIG_BLK_DEV_SD=y
 CONFIG_BLK_DEV_SR=y
@@ -112,6 +119,7 @@ CONFIG_USB_NET_SMSC95XX=y
 CONFIG_BRCMFMAC=m
 CONFIG_RT2X00=y
 CONFIG_RT2800USB=m
+CONFIG_INPUT_JOYDEV=y
 CONFIG_INPUT_EVDEV=y
 CONFIG_KEYBOARD_GPIO=y
 CONFIG_KEYBOARD_TEGRA=y
@@ -181,6 +189,7 @@ CONFIG_BACKLIGHT_CLASS_DEVICE=y
 # CONFIG_BACKLIGHT_GENERIC is not set
 CONFIG_BACKLIGHT_PWM=y
 CONFIG_FRAMEBUFFER_CONSOLE=y
+CONFIG_FRAMEBUFFER_CONSOLE_ROTATION=y
 CONFIG_LOGO=y
 CONFIG_SOUND=y
 CONFIG_SND=y
@@ -222,6 +231,7 @@ CONFIG_LEDS_TRIGGER_TRANSIENT=y
 CONFIG_LEDS_TRIGGER_CAMERA=y
 CONFIG_RTC_CLASS=y
 CONFIG_RTC_DRV_AS3722=y
+CONFIG_RTC_DRV_DS1307=y
 CONFIG_RTC_DRV_MAX8907=y
 CONFIG_RTC_DRV_PALMAS=y
 CONFIG_RTC_DRV_TPS6586X=y
diff --git a/arch/arm/configs/trizeps4_defconfig b/arch/arm/configs/trizeps4_defconfig
index 3162173fa75..932ee4e4a13 100644
--- a/arch/arm/configs/trizeps4_defconfig
+++ b/arch/arm/configs/trizeps4_defconfig
@@ -165,7 +165,6 @@ CONFIG_SND_PXA2XX_AC97=y
 CONFIG_SND_USB_AUDIO=m
 # CONFIG_USB_HID is not set
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_OHCI_HCD=y
 CONFIG_USB_STORAGE=m
diff --git a/arch/arm/configs/u300_defconfig b/arch/arm/configs/u300_defconfig
index fd81a1b99cc..aaa95ab606a 100644
--- a/arch/arm/configs/u300_defconfig
+++ b/arch/arm/configs/u300_defconfig
@@ -11,6 +11,7 @@ CONFIG_MODULES=y
 CONFIG_MODULE_UNLOAD=y
 # CONFIG_LBDAF is not set
 # CONFIG_BLK_DEV_BSG is not set
+CONFIG_PARTITION_ADVANCED=y
 # CONFIG_IOSCHED_CFQ is not set
 # CONFIG_ARCH_MULTI_V7 is not set
 CONFIG_ARCH_U300=y
@@ -21,7 +22,6 @@ CONFIG_ZBOOT_ROM_TEXT=0x0
 CONFIG_ZBOOT_ROM_BSS=0x0
 CONFIG_CMDLINE="root=/dev/ram0 rw rootfstype=rootfs console=ttyAMA0,115200n8 lpj=515072"
 CONFIG_CPU_IDLE=y
-CONFIG_FPE_NWFPE=y
 # CONFIG_SUSPEND is not set
 CONFIG_UEVENT_HELPER_PATH="/sbin/hotplug"
 # CONFIG_PREVENT_FIRMWARE_BUILD is not set
@@ -64,8 +64,8 @@ CONFIG_TMPFS=y
 CONFIG_NLS_CODEPAGE_437=y
 CONFIG_NLS_ISO8859_1=y
 CONFIG_PRINTK_TIME=y
+CONFIG_DEBUG_INFO=y
 CONFIG_DEBUG_FS=y
 # CONFIG_SCHED_DEBUG is not set
 CONFIG_TIMER_STATS=y
 # CONFIG_DEBUG_PREEMPT is not set
-CONFIG_DEBUG_INFO=y
diff --git a/arch/arm/configs/u8500_defconfig b/arch/arm/configs/u8500_defconfig
index 65f77885c16..d219d6a4323 100644
--- a/arch/arm/configs/u8500_defconfig
+++ b/arch/arm/configs/u8500_defconfig
@@ -1,16 +1,16 @@
 # CONFIG_SWAP is not set
 CONFIG_SYSVIPC=y
-CONFIG_NO_HZ=y
+CONFIG_NO_HZ_IDLE=y
 CONFIG_HIGH_RES_TIMERS=y
 CONFIG_BLK_DEV_INITRD=y
 CONFIG_KALLSYMS_ALL=y
 CONFIG_MODULES=y
 CONFIG_MODULE_UNLOAD=y
 # CONFIG_BLK_DEV_BSG is not set
+CONFIG_PARTITION_ADVANCED=y
 CONFIG_ARCH_U8500=y
 CONFIG_MACH_HREFV60=y
 CONFIG_MACH_SNOWBALL=y
-CONFIG_MACH_UX500_DT=y
 CONFIG_SMP=y
 CONFIG_NR_CPUS=2
 CONFIG_PREEMPT=y
@@ -34,16 +34,22 @@ CONFIG_IP_PNP=y
 CONFIG_IP_PNP_DHCP=y
 CONFIG_NETFILTER=y
 CONFIG_PHONET=y
-# CONFIG_WIRELESS is not set
+CONFIG_CFG80211=y
+CONFIG_CFG80211_DEBUGFS=y
+CONFIG_MAC80211=y
+CONFIG_MAC80211_LEDS=y
 CONFIG_CAIF=y
 CONFIG_UEVENT_HELPER_PATH="/sbin/hotplug"
+CONFIG_DEVTMPFS=y
+CONFIG_DEVTMPFS_MOUNT=y
 CONFIG_BLK_DEV_RAM=y
 CONFIG_BLK_DEV_RAM_SIZE=65536
 CONFIG_SENSORS_BH1780=y
 CONFIG_NETDEVICES=y
 CONFIG_SMSC911X=y
 CONFIG_SMSC_PHY=y
-# CONFIG_WLAN is not set
+CONFIG_CW1200=y
+CONFIG_CW1200_WLAN_SDIO=y
 # CONFIG_INPUT_MOUSEDEV_PSAUX is not set
 CONFIG_INPUT_EVDEV=y
 # CONFIG_KEYBOARD_ATKBD is not set
@@ -85,15 +91,12 @@ CONFIG_AB8500_USB=y
 CONFIG_USB_GADGET=y
 CONFIG_USB_ETH=m
 CONFIG_MMC=y
-CONFIG_MMC_UNSAFE_RESUME=y
-# CONFIG_MMC_BLOCK_BOUNCE is not set
 CONFIG_MMC_ARMMMCI=y
 CONFIG_NEW_LEDS=y
 CONFIG_LEDS_CLASS=y
 CONFIG_LEDS_LM3530=y
 CONFIG_LEDS_GPIO=y
 CONFIG_LEDS_LP5521=y
-CONFIG_LEDS_TRIGGERS=y
 CONFIG_LEDS_TRIGGER_HEARTBEAT=y
 CONFIG_RTC_CLASS=y
 CONFIG_RTC_DRV_AB8500=y
@@ -103,6 +106,11 @@ CONFIG_STE_DMA40=y
 CONFIG_STAGING=y
 CONFIG_TOUCHSCREEN_SYNAPTICS_I2C_RMI4=y
 CONFIG_HSEM_U8500=y
+CONFIG_IIO=y
+CONFIG_IIO_ST_ACCEL_3AXIS=y
+CONFIG_IIO_ST_GYRO_3AXIS=y
+CONFIG_IIO_ST_MAGN_3AXIS=y
+CONFIG_IIO_ST_PRESS=y
 CONFIG_EXT2_FS=y
 CONFIG_EXT2_FS_XATTR=y
 CONFIG_EXT2_FS_POSIX_ACL=y
@@ -110,8 +118,6 @@ CONFIG_EXT2_FS_SECURITY=y
 CONFIG_EXT3_FS=y
 CONFIG_EXT4_FS=y
 CONFIG_VFAT_FS=y
-CONFIG_DEVTMPFS=y
-CONFIG_DEVTMPFS_MOUNT=y
 CONFIG_TMPFS=y
 CONFIG_TMPFS_POSIX_ACL=y
 # CONFIG_MISC_FILESYSTEMS is not set
diff --git a/arch/arm/configs/versatile_defconfig b/arch/arm/configs/versatile_defconfig
index 073541a50e2..d52b4ffe201 100644
--- a/arch/arm/configs/versatile_defconfig
+++ b/arch/arm/configs/versatile_defconfig
@@ -61,6 +61,9 @@ CONFIG_SND_ARMAACI=m
 CONFIG_MMC=y
 CONFIG_MMC_ARMMMCI=m
 CONFIG_NEW_LEDS=y
+CONFIG_LEDS_CLASS=y
+CONFIG_LEDS_VERSATILE=y
+CONFIG_LEDS_TRIGGERS=y
 CONFIG_LEDS_TRIGGER_HEARTBEAT=y
 CONFIG_LEDS_TRIGGER_CPU=y
 CONFIG_EXT2_FS=y
diff --git a/arch/arm/configs/viper_defconfig b/arch/arm/configs/viper_defconfig
index d36e0d3c86e..0d717a5eff2 100644
--- a/arch/arm/configs/viper_defconfig
+++ b/arch/arm/configs/viper_defconfig
@@ -127,7 +127,6 @@ CONFIG_SND_MIXER_OSS=m
 CONFIG_SND_PCM_OSS=m
 CONFIG_SND_PXA2XX_AC97=m
 CONFIG_USB=m
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_ISP116X_HCD=m
 CONFIG_USB_SL811_HCD=m
 CONFIG_USB_R8A66597_HCD=m
diff --git a/arch/arm/configs/zeus_defconfig b/arch/arm/configs/zeus_defconfig
index 731d4f98531..cd11da8b512 100644
--- a/arch/arm/configs/zeus_defconfig
+++ b/arch/arm/configs/zeus_defconfig
@@ -132,7 +132,6 @@ CONFIG_SND_SOC=m
 CONFIG_SND_PXA2XX_SOC=m
 # CONFIG_HID_SUPPORT is not set
 CONFIG_USB=m
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_OHCI_HCD=m
 CONFIG_USB_ACM=m
 CONFIG_USB_STORAGE=m
diff --git a/arch/arm/include/asm/cputype.h b/arch/arm/include/asm/cputype.h
index c651e3b26ec..4764344367d 100644
--- a/arch/arm/include/asm/cputype.h
+++ b/arch/arm/include/asm/cputype.h
@@ -222,22 +222,22 @@ static inline int cpu_is_xsc3(void)
 #endif
 
 /*
- * Marvell's PJ4 core is based on V7 version. It has some modification
- * for coprocessor setting. For this reason, we need a way to distinguish
- * it.
+ * Marvell's PJ4 and PJ4B cores are based on V7 version,
+ * but require a specical sequence for enabling coprocessors.
+ * For this reason, we need a way to distinguish them.
  */
-#ifndef CONFIG_CPU_PJ4
-#define cpu_is_pj4()	0
-#else
+#if defined(CONFIG_CPU_PJ4) || defined(CONFIG_CPU_PJ4B)
 static inline int cpu_is_pj4(void)
 {
 	unsigned int id;
 
 	id = read_cpuid_id();
-	if ((id & 0xfffffff0) == 0x562f5840)
+	if ((id & 0xff0fff00) == 0x560f5800)
 		return 1;
 
 	return 0;
 }
+#else
+#define cpu_is_pj4()	0
 #endif
 #endif
diff --git a/arch/arm/include/asm/dcc.h b/arch/arm/include/asm/dcc.h
new file mode 100644
index 00000000000..b74899de077
--- /dev/null
+++ b/arch/arm/include/asm/dcc.h
@@ -0,0 +1,41 @@
+/* Copyright (c) 2010, 2014 The Linux Foundation. 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 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.
+ */
+
+#include <asm/barrier.h>
+
+static inline u32 __dcc_getstatus(void)
+{
+	u32 __ret;
+	asm volatile("mrc p14, 0, %0, c0, c1, 0	@ read comms ctrl reg"
+		: "=r" (__ret) : : "cc");
+
+	return __ret;
+}
+
+static inline char __dcc_getchar(void)
+{
+	char __c;
+
+	asm volatile("mrc p14, 0, %0, c0, c5, 0	@ read comms data reg"
+		: "=r" (__c));
+	isb();
+
+	return __c;
+}
+
+static inline void __dcc_putchar(char c)
+{
+	asm volatile("mcr p14, 0, %0, c0, c5, 0	@ write a char"
+		: /* no output register */
+		: "r" (c));
+	isb();
+}
diff --git a/arch/arm/include/asm/div64.h b/arch/arm/include/asm/div64.h
index 191ada6e4d2..662c7bd0610 100644
--- a/arch/arm/include/asm/div64.h
+++ b/arch/arm/include/asm/div64.h
@@ -156,7 +156,7 @@
 		/* Select the best insn combination to perform the   */	\
 		/* actual __m * __n / (__p << 64) operation.         */	\
 		if (!__c) {						\
-			asm (	"umull	%Q0, %R0, %1, %Q2\n\t"		\
+			asm (	"umull	%Q0, %R0, %Q1, %Q2\n\t"		\
 				"mov	%Q0, #0"			\
 				: "=&r" (__res)				\
 				: "r" (__m), "r" (__n)			\
diff --git a/arch/arm/include/asm/dma-iommu.h b/arch/arm/include/asm/dma-iommu.h
index eec0a12c5c1..8e3fcb924db 100644
--- a/arch/arm/include/asm/dma-iommu.h
+++ b/arch/arm/include/asm/dma-iommu.h
@@ -18,7 +18,6 @@ struct dma_iommu_mapping {
 	unsigned int		extensions;
 	size_t			bitmap_size;	/* size of a single bitmap */
 	size_t			bits;		/* per bitmap */
-	unsigned int		size;		/* per bitmap */
 	dma_addr_t		base;
 
 	spinlock_t		lock;
diff --git a/arch/arm/include/asm/mcpm.h b/arch/arm/include/asm/mcpm.h
index 608516ebabf..a5ff410dcdb 100644
--- a/arch/arm/include/asm/mcpm.h
+++ b/arch/arm/include/asm/mcpm.h
@@ -54,6 +54,13 @@ void mcpm_set_early_poke(unsigned cpu, unsigned cluster,
  */
 
 /**
+ * mcpm_is_available - returns whether MCPM is initialized and available
+ *
+ * This returns true or false accordingly.
+ */
+bool mcpm_is_available(void);
+
+/**
  * mcpm_cpu_power_up - make given CPU in given cluster runable
  *
  * @cpu: CPU number within given cluster
diff --git a/arch/arm/include/asm/pci.h b/arch/arm/include/asm/pci.h
index 680a83e9446..7e95d8535e2 100644
--- a/arch/arm/include/asm/pci.h
+++ b/arch/arm/include/asm/pci.h
@@ -31,11 +31,6 @@ static inline int pci_proc_domain(struct pci_bus *bus)
 }
 #endif /* CONFIG_PCI_DOMAINS */
 
-static inline void pcibios_penalize_isa_irq(int irq, int active)
-{
-	/* We don't do dynamic PCI IRQ allocation */
-}
-
 /*
  * The PCI address space does equal the physical memory address space.
  * The networking and block device layers use this boolean for bounce
diff --git a/arch/arm/include/asm/tlb.h b/arch/arm/include/asm/tlb.h
index 0baf7f0d939..f1a0dace3ef 100644
--- a/arch/arm/include/asm/tlb.h
+++ b/arch/arm/include/asm/tlb.h
@@ -98,15 +98,25 @@ static inline void __tlb_alloc_page(struct mmu_gather *tlb)
 	}
 }
 
-static inline void tlb_flush_mmu(struct mmu_gather *tlb)
+static inline void tlb_flush_mmu_tlbonly(struct mmu_gather *tlb)
 {
 	tlb_flush(tlb);
+}
+
+static inline void tlb_flush_mmu_free(struct mmu_gather *tlb)
+{
 	free_pages_and_swap_cache(tlb->pages, tlb->nr);
 	tlb->nr = 0;
 	if (tlb->pages == tlb->local)
 		__tlb_alloc_page(tlb);
 }
 
+static inline void tlb_flush_mmu(struct mmu_gather *tlb)
+{
+	tlb_flush_mmu_tlbonly(tlb);
+	tlb_flush_mmu_free(tlb);
+}
+
 static inline void
 tlb_gather_mmu(struct mmu_gather *tlb, struct mm_struct *mm, unsigned long start, unsigned long end)
 {
diff --git a/arch/arm/include/asm/trusted_foundations.h b/arch/arm/include/asm/trusted_foundations.h
index b5f7705abcb..624e1d436c6 100644
--- a/arch/arm/include/asm/trusted_foundations.h
+++ b/arch/arm/include/asm/trusted_foundations.h
@@ -54,7 +54,9 @@ static inline void register_trusted_foundations(
 	 */
 	pr_err("No support for Trusted Foundations, continuing in degraded mode.\n");
 	pr_err("Secondary processors as well as CPU PM will be disabled.\n");
+#if IS_ENABLED(CONFIG_SMP)
 	setup_max_cpus = 0;
+#endif
 	cpu_idle_poll_ctrl(true);
 }
 
diff --git a/arch/arm/include/asm/uaccess.h b/arch/arm/include/asm/uaccess.h
index 12c3a5decc6..75d95799b6e 100644
--- a/arch/arm/include/asm/uaccess.h
+++ b/arch/arm/include/asm/uaccess.h
@@ -171,8 +171,9 @@ extern int __put_user_8(void *, unsigned long long);
 #define __put_user_check(x,p)							\
 	({								\
 		unsigned long __limit = current_thread_info()->addr_limit - 1; \
+		const typeof(*(p)) __user *__tmp_p = (p);		\
 		register const typeof(*(p)) __r2 asm("r2") = (x);	\
-		register const typeof(*(p)) __user *__p asm("r0") = (p);\
+		register const typeof(*(p)) __user *__p asm("r0") = __tmp_p; \
 		register unsigned long __l asm("r1") = __limit;		\
 		register int __e asm("r0");				\
 		switch (sizeof(*(__p))) {				\
diff --git a/arch/arm/include/asm/xen/hypercall.h b/arch/arm/include/asm/xen/hypercall.h
index 7704e28c348..712b50e0a6d 100644
--- a/arch/arm/include/asm/xen/hypercall.h
+++ b/arch/arm/include/asm/xen/hypercall.h
@@ -34,6 +34,7 @@
 #define _ASM_ARM_XEN_HYPERCALL_H
 
 #include <xen/interface/xen.h>
+#include <xen/interface/sched.h>
 
 long privcmd_call(unsigned call, unsigned long a1,
 		unsigned long a2, unsigned long a3,
@@ -48,6 +49,16 @@ int HYPERVISOR_memory_op(unsigned int cmd, void *arg);
 int HYPERVISOR_physdev_op(int cmd, void *arg);
 int HYPERVISOR_vcpu_op(int cmd, int vcpuid, void *extra_args);
 int HYPERVISOR_tmem_op(void *arg);
+int HYPERVISOR_multicall(struct multicall_entry *calls, uint32_t nr);
+
+static inline int
+HYPERVISOR_suspend(unsigned long start_info_mfn)
+{
+	struct sched_shutdown r = { .reason = SHUTDOWN_suspend };
+
+	/* start_info_mfn is unused on ARM */
+	return HYPERVISOR_sched_op(SCHEDOP_shutdown, &r);
+}
 
 static inline void
 MULTI_update_va_mapping(struct multicall_entry *mcl, unsigned long va,
@@ -63,9 +74,4 @@ MULTI_mmu_update(struct multicall_entry *mcl, struct mmu_update *req,
 	BUG();
 }
 
-static inline int
-HYPERVISOR_multicall(void *call_list, int nr_calls)
-{
-	BUG();
-}
 #endif /* _ASM_ARM_XEN_HYPERCALL_H */
diff --git a/arch/arm/include/asm/xen/interface.h b/arch/arm/include/asm/xen/interface.h
index 1151188bcd8..50066006e6b 100644
--- a/arch/arm/include/asm/xen/interface.h
+++ b/arch/arm/include/asm/xen/interface.h
@@ -40,6 +40,8 @@ typedef uint64_t xen_pfn_t;
 #define PRI_xen_pfn "llx"
 typedef uint64_t xen_ulong_t;
 #define PRI_xen_ulong "llx"
+typedef int64_t xen_long_t;
+#define PRI_xen_long "llx"
 /* Guest handles for primitive C types. */
 __DEFINE_GUEST_HANDLE(uchar, unsigned char);
 __DEFINE_GUEST_HANDLE(uint,  unsigned int);
diff --git a/arch/arm/include/asm/xen/page.h b/arch/arm/include/asm/xen/page.h
index cf4f3e86739..ded062f9b35 100644
--- a/arch/arm/include/asm/xen/page.h
+++ b/arch/arm/include/asm/xen/page.h
@@ -77,7 +77,6 @@ static inline xpaddr_t machine_to_phys(xmaddr_t machine)
 }
 /* VIRT <-> MACHINE conversion */
 #define virt_to_machine(v)	(phys_to_machine(XPADDR(__pa(v))))
-#define virt_to_pfn(v)          (PFN_DOWN(__pa(v)))
 #define virt_to_mfn(v)		(pfn_to_mfn(virt_to_pfn(v)))
 #define mfn_to_virt(m)		(__va(mfn_to_pfn(m) << PAGE_SHIFT))
 
diff --git a/arch/arm/include/debug/imx-uart.h b/arch/arm/include/debug/imx-uart.h
index 42b823cd2d2..032a316eb80 100644
--- a/arch/arm/include/debug/imx-uart.h
+++ b/arch/arm/include/debug/imx-uart.h
@@ -81,6 +81,15 @@
 #define IMX6SL_UART_BASE_ADDR(n) IMX6SL_UART##n##_BASE_ADDR
 #define IMX6SL_UART_BASE(n)	IMX6SL_UART_BASE_ADDR(n)
 
+#define IMX6SX_UART1_BASE_ADDR	0x02020000
+#define IMX6SX_UART2_BASE_ADDR	0x021e8000
+#define IMX6SX_UART3_BASE_ADDR	0x021ec000
+#define IMX6SX_UART4_BASE_ADDR	0x021f0000
+#define IMX6SX_UART5_BASE_ADDR	0x021f4000
+#define IMX6SX_UART6_BASE_ADDR	0x022a0000
+#define IMX6SX_UART_BASE_ADDR(n) IMX6SX_UART##n##_BASE_ADDR
+#define IMX6SX_UART_BASE(n)	IMX6SX_UART_BASE_ADDR(n)
+
 #define IMX_DEBUG_UART_BASE(soc) soc##_UART_BASE(CONFIG_DEBUG_IMX_UART_PORT)
 
 #ifdef CONFIG_DEBUG_IMX1_UART
@@ -103,6 +112,8 @@
 #define UART_PADDR	IMX_DEBUG_UART_BASE(IMX6Q)
 #elif defined(CONFIG_DEBUG_IMX6SL_UART)
 #define UART_PADDR	IMX_DEBUG_UART_BASE(IMX6SL)
+#elif defined(CONFIG_DEBUG_IMX6SX_UART)
+#define UART_PADDR	IMX_DEBUG_UART_BASE(IMX6SX)
 #endif
 
 #endif /* __DEBUG_IMX_UART_H */
diff --git a/arch/arm/include/debug/msm.S b/arch/arm/include/debug/msm.S
index 9d653d47590..9ef57612811 100644
--- a/arch/arm/include/debug/msm.S
+++ b/arch/arm/include/debug/msm.S
@@ -15,51 +15,15 @@
  *
  */
 
-#if defined(CONFIG_ARCH_MSM7X00A) || defined(CONFIG_ARCH_QSD8X50)
-#define MSM_UART1_PHYS        0xA9A00000
-#define MSM_UART2_PHYS        0xA9B00000
-#define MSM_UART3_PHYS        0xA9C00000
-#elif defined(CONFIG_ARCH_MSM7X30)
-#define MSM_UART1_PHYS        0xACA00000
-#define MSM_UART2_PHYS        0xACB00000
-#define MSM_UART3_PHYS        0xACC00000
-#endif
-
-#if defined(CONFIG_DEBUG_MSM_UART1)
-#define MSM_DEBUG_UART_BASE	0xE1000000
-#define MSM_DEBUG_UART_PHYS	MSM_UART1_PHYS
-#elif defined(CONFIG_DEBUG_MSM_UART2)
-#define MSM_DEBUG_UART_BASE	0xE1000000
-#define MSM_DEBUG_UART_PHYS	MSM_UART2_PHYS
-#elif defined(CONFIG_DEBUG_MSM_UART3)
-#define MSM_DEBUG_UART_BASE	0xE1000000
-#define MSM_DEBUG_UART_PHYS	MSM_UART3_PHYS
-#endif
-
-#ifdef CONFIG_DEBUG_MSM8660_UART
-#define MSM_DEBUG_UART_BASE	0xF0040000
-#define MSM_DEBUG_UART_PHYS	0x19C40000
-#endif
-
-#ifdef CONFIG_DEBUG_MSM8960_UART
-#define MSM_DEBUG_UART_BASE	0xF0040000
-#define MSM_DEBUG_UART_PHYS	0x16440000
-#endif
-
-#ifdef CONFIG_DEBUG_MSM8974_UART
-#define MSM_DEBUG_UART_BASE	0xFA71E000
-#define MSM_DEBUG_UART_PHYS	0xF991E000
-#endif
-
 	.macro	addruart, rp, rv, tmp
-#ifdef MSM_DEBUG_UART_PHYS
-	ldr	\rp, =MSM_DEBUG_UART_PHYS
-	ldr	\rv, =MSM_DEBUG_UART_BASE
+#ifdef CONFIG_DEBUG_UART_PHYS
+	ldr	\rp, =CONFIG_DEBUG_UART_PHYS
+	ldr	\rv, =CONFIG_DEBUG_UART_VIRT
 #endif
 	.endm
 
 	.macro	senduart, rd, rx
-#ifdef CONFIG_MSM_HAS_DEBUG_UART_HS
+#ifdef CONFIG_DEBUG_QCOM_UARTDM
 	@ Write the 1 character to UARTDM_TF
 	str	\rd, [\rx, #0x70]
 #else
@@ -68,7 +32,7 @@
 	.endm
 
 	.macro	waituart, rd, rx
-#ifdef CONFIG_MSM_HAS_DEBUG_UART_HS
+#ifdef CONFIG_DEBUG_QCOM_UARTDM
 	@ check for TX_EMT in UARTDM_SR
 	ldr	\rd, [\rx, #0x08]
 	tst	\rd, #0x08
diff --git a/arch/arm/include/debug/s3c24xx.S b/arch/arm/include/debug/s3c24xx.S
new file mode 100644
index 00000000000..b1f54dc4888
--- /dev/null
+++ b/arch/arm/include/debug/s3c24xx.S
@@ -0,0 +1,46 @@
+/* arch/arm/mach-s3c2410/include/mach/debug-macro.S
+ *
+ * Debugging macro include header
+ *
+ *  Copyright (C) 1994-1999 Russell King
+ *  Copyright (C) 2005 Simtec Electronics
+ *
+ *  Moved from linux/arch/arm/kernel/debug.S by Ben Dooks
+ *
+ * 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/serial_s3c.h>
+
+#define S3C2410_UART1_OFF (0x4000)
+
+	.macro addruart, rp, rv, tmp
+		ldr	\rp, = CONFIG_DEBUG_UART_PHYS
+		ldr	\rv, = CONFIG_DEBUG_UART_VIRT
+	.endm
+
+	.macro  fifo_full_s3c2410 rd, rx
+		ldr	\rd, [\rx, # S3C2410_UFSTAT]
+		tst	\rd, #S3C2410_UFSTAT_TXFULL
+	.endm
+
+	.macro fifo_level_s3c2410 rd, rx
+		ldr	\rd, [\rx, # S3C2410_UFSTAT]
+		and	\rd, \rd, #S3C2410_UFSTAT_TXMASK
+	.endm
+
+/* Select the correct implementation depending on the configuration. The
+ * S3C2440 will get selected by default, as these are the most widely
+ * used variants of these
+*/
+
+#if defined(CONFIG_DEBUG_S3C2410_UART)
+#define fifo_full  fifo_full_s3c2410
+#define fifo_level fifo_level_s3c2410
+#endif
+
+/* include the reset of the code which will do the work */
+
+#include <debug/samsung.S>
diff --git a/arch/arm/include/debug/vf.S b/arch/arm/include/debug/vf.S
index ba12cc44b2c..b88933849a1 100644
--- a/arch/arm/include/debug/vf.S
+++ b/arch/arm/include/debug/vf.S
@@ -7,9 +7,20 @@
  *
  */
 
+#define VF_UART0_BASE_ADDR	0x40027000
+#define VF_UART1_BASE_ADDR	0x40028000
+#define VF_UART2_BASE_ADDR	0x40029000
+#define VF_UART3_BASE_ADDR	0x4002a000
+#define VF_UART_BASE_ADDR(n)	VF_UART##n##_BASE_ADDR
+#define VF_UART_BASE(n)		VF_UART_BASE_ADDR(n)
+#define VF_UART_PHYSICAL_BASE	VF_UART_BASE(CONFIG_DEBUG_VF_UART_PORT)
+
+#define VF_UART_VIRTUAL_BASE	0xfe000000
+
 	.macro	addruart, rp, rv, tmp
-	ldr	\rp, =0x40028000	@ physical
-	ldr	\rv, =0xfe028000	@ virtual
+	ldr	\rp, =VF_UART_PHYSICAL_BASE 	@ physical
+	and	\rv, \rp, #0xffffff		@ offset within 16MB section
+	add	\rv, \rv, #VF_UART_VIRTUAL_BASE
 	.endm
 
 	.macro	senduart, rd, rx
diff --git a/arch/arm/include/debug/zynq.S b/arch/arm/include/debug/zynq.S
index 0b762fafa75..bd13dedbdef 100644
--- a/arch/arm/include/debug/zynq.S
+++ b/arch/arm/include/debug/zynq.S
@@ -20,18 +20,18 @@
 #define UART_SR_TXEMPTY		0x00000008	/* TX FIFO empty */
 
 #define UART0_PHYS		0xE0000000
+#define UART0_VIRT		0xF0000000
 #define UART1_PHYS		0xE0001000
-#define UART_SIZE		SZ_4K
-#define UART_VIRT		0xF0001000
+#define UART1_VIRT		0xF0001000
 
 #if IS_ENABLED(CONFIG_DEBUG_ZYNQ_UART1)
 # define LL_UART_PADDR		UART1_PHYS
+# define LL_UART_VADDR		UART1_VIRT
 #else
 # define LL_UART_PADDR		UART0_PHYS
+# define LL_UART_VADDR		UART0_VIRT
 #endif
 
-#define LL_UART_VADDR		UART_VIRT
-
 		.macro	addruart, rp, rv, tmp
 		ldr	\rp, =LL_UART_PADDR	@ physical
 		ldr	\rv, =LL_UART_VADDR	@ virtual
@@ -43,12 +43,14 @@
 
 		.macro	waituart,rd,rx
 1001:		ldr	\rd, [\rx, #UART_SR_OFFSET]
+ARM_BE8(	rev	\rd, \rd )
 		tst	\rd, #UART_SR_TXEMPTY
 		beq	1001b
 		.endm
 
 		.macro	busyuart,rd,rx
 1002:		ldr	\rd, [\rx, #UART_SR_OFFSET]	@ get status register
+ARM_BE8(	rev	\rd, \rd )
 		tst	\rd, #UART_SR_TXFULL		@
 		bne	1002b			@ wait if FIFO is full
 		.endm
diff --git a/arch/arm/include/uapi/asm/unistd.h b/arch/arm/include/uapi/asm/unistd.h
index fb5584d0cc0..ba94446c72d 100644
--- a/arch/arm/include/uapi/asm/unistd.h
+++ b/arch/arm/include/uapi/asm/unistd.h
@@ -408,6 +408,7 @@
 #define __NR_finit_module		(__NR_SYSCALL_BASE+379)
 #define __NR_sched_setattr		(__NR_SYSCALL_BASE+380)
 #define __NR_sched_getattr		(__NR_SYSCALL_BASE+381)
+#define __NR_renameat2			(__NR_SYSCALL_BASE+382)
 
 /*
  * This may need to be greater than __NR_last_syscall+1 in order to
diff --git a/arch/arm/kernel/Makefile b/arch/arm/kernel/Makefile
index a766bcbaf8a..040619c32d6 100644
--- a/arch/arm/kernel/Makefile
+++ b/arch/arm/kernel/Makefile
@@ -79,6 +79,7 @@ obj-$(CONFIG_CPU_XSCALE)	+= xscale-cp0.o
 obj-$(CONFIG_CPU_XSC3)		+= xscale-cp0.o
 obj-$(CONFIG_CPU_MOHAWK)	+= xscale-cp0.o
 obj-$(CONFIG_CPU_PJ4)		+= pj4-cp0.o
+obj-$(CONFIG_CPU_PJ4B)		+= pj4-cp0.o
 obj-$(CONFIG_IWMMXT)		+= iwmmxt.o
 obj-$(CONFIG_PERF_EVENTS)	+= perf_regs.o
 obj-$(CONFIG_HW_PERF_EVENTS)	+= perf_event.o perf_event_cpu.o
diff --git a/arch/arm/kernel/bios32.c b/arch/arm/kernel/bios32.c
index 16d43cd4561..17a26c17f7f 100644
--- a/arch/arm/kernel/bios32.c
+++ b/arch/arm/kernel/bios32.c
@@ -545,6 +545,18 @@ void pci_common_init_dev(struct device *parent, struct hw_pci *hw)
 		 */
 		pci_bus_add_devices(bus);
 	}
+
+	list_for_each_entry(sys, &head, node) {
+		struct pci_bus *bus = sys->bus;
+
+		/* Configure PCI Express settings */
+		if (bus && !pci_has_flag(PCI_PROBE_ONLY)) {
+			struct pci_bus *child;
+
+			list_for_each_entry(child, &bus->children, node)
+				pcie_bus_configure_settings(child);
+		}
+	}
 }
 
 #ifndef CONFIG_PCI_HOST_ITE8152
diff --git a/arch/arm/kernel/calls.S b/arch/arm/kernel/calls.S
index 166e945de83..8f51bdcdacb 100644
--- a/arch/arm/kernel/calls.S
+++ b/arch/arm/kernel/calls.S
@@ -391,6 +391,7 @@
 		CALL(sys_finit_module)
 /* 380 */	CALL(sys_sched_setattr)
 		CALL(sys_sched_getattr)
+		CALL(sys_renameat2)
 #ifndef syscalls_counted
 .equ syscalls_padding, ((NR_syscalls + 3) & ~3) - NR_syscalls
 #define syscalls_counted
diff --git a/arch/arm/kernel/entry-header.S b/arch/arm/kernel/entry-header.S
index 1420725142c..efb208de75e 100644
--- a/arch/arm/kernel/entry-header.S
+++ b/arch/arm/kernel/entry-header.S
@@ -132,6 +132,10 @@
 	orrne	r5, V7M_xPSR_FRAMEPTRALIGN
 	biceq	r5, V7M_xPSR_FRAMEPTRALIGN
 
+	@ ensure bit 0 is cleared in the PC, otherwise behaviour is
+	@ unpredictable
+	bic	r4, #1
+
 	@ write basic exception frame
 	stmdb	r2!, {r1, r3-r5}
 	ldmia	sp, {r1, r3-r5}
diff --git a/arch/arm/kernel/head.S b/arch/arm/kernel/head.S
index f8c08839edf..591d6e4a649 100644
--- a/arch/arm/kernel/head.S
+++ b/arch/arm/kernel/head.S
@@ -587,7 +587,7 @@ __fixup_pv_table:
 	add	r6, r6, r3	@ adjust __pv_phys_pfn_offset address
 	add	r7, r7, r3	@ adjust __pv_offset address
 	mov	r0, r8, lsr #12	@ convert to PFN
-	str	r0, [r6, #LOW_OFFSET]	@ save computed PHYS_OFFSET to __pv_phys_pfn_offset
+	str	r0, [r6]	@ save computed PHYS_OFFSET to __pv_phys_pfn_offset
 	strcc	ip, [r7, #HIGH_OFFSET]	@ save to __pv_offset high bits
 	mov	r6, r3, lsr #24	@ constant for add/sub instructions
 	teq	r3, r6, lsl #24 @ must be 16MiB aligned
diff --git a/arch/arm/kernel/iwmmxt.S b/arch/arm/kernel/iwmmxt.S
index a08783823b3..2452dd1bef5 100644
--- a/arch/arm/kernel/iwmmxt.S
+++ b/arch/arm/kernel/iwmmxt.S
@@ -19,12 +19,16 @@
 #include <asm/thread_info.h>
 #include <asm/asm-offsets.h>
 
-#if defined(CONFIG_CPU_PJ4)
+#if defined(CONFIG_CPU_PJ4) || defined(CONFIG_CPU_PJ4B)
 #define PJ4(code...)		code
 #define XSC(code...)
-#else
+#elif defined(CONFIG_CPU_MOHAWK) || \
+	defined(CONFIG_CPU_XSC3) || \
+	defined(CONFIG_CPU_XSCALE)
 #define PJ4(code...)
 #define XSC(code...)		code
+#else
+#error "Unsupported iWMMXt architecture"
 #endif
 
 #define MMX_WR0		 	(0x00)
diff --git a/arch/arm/kernel/machine_kexec.c b/arch/arm/kernel/machine_kexec.c
index f0d180d8b29..8cf0996aa1a 100644
--- a/arch/arm/kernel/machine_kexec.c
+++ b/arch/arm/kernel/machine_kexec.c
@@ -184,3 +184,10 @@ void machine_kexec(struct kimage *image)
 
 	soft_restart(reboot_entry_phys);
 }
+
+void arch_crash_save_vmcoreinfo(void)
+{
+#ifdef CONFIG_ARM_LPAE
+	VMCOREINFO_CONFIG(ARM_LPAE);
+#endif
+}
diff --git a/arch/arm/kernel/pj4-cp0.c b/arch/arm/kernel/pj4-cp0.c
index fc720863628..8153e36b249 100644
--- a/arch/arm/kernel/pj4-cp0.c
+++ b/arch/arm/kernel/pj4-cp0.c
@@ -45,7 +45,7 @@ static int iwmmxt_do(struct notifier_block *self, unsigned long cmd, void *t)
 	return NOTIFY_DONE;
 }
 
-static struct notifier_block iwmmxt_notifier_block = {
+static struct notifier_block __maybe_unused iwmmxt_notifier_block = {
 	.notifier_call	= iwmmxt_do,
 };
 
@@ -72,6 +72,33 @@ static void __init pj4_cp_access_write(u32 value)
 		: "=r" (temp) : "r" (value));
 }
 
+static int __init pj4_get_iwmmxt_version(void)
+{
+	u32 cp_access, wcid;
+
+	cp_access = pj4_cp_access_read();
+	pj4_cp_access_write(cp_access | 0xf);
+
+	/* check if coprocessor 0 and 1 are available */
+	if ((pj4_cp_access_read() & 0xf) != 0xf) {
+		pj4_cp_access_write(cp_access);
+		return -ENODEV;
+	}
+
+	/* read iWMMXt coprocessor id register p1, c0 */
+	__asm__ __volatile__ ("mrc    p1, 0, %0, c0, c0, 0\n" : "=r" (wcid));
+
+	pj4_cp_access_write(cp_access);
+
+	/* iWMMXt v1 */
+	if ((wcid & 0xffffff00) == 0x56051000)
+		return 1;
+	/* iWMMXt v2 */
+	if ((wcid & 0xffffff00) == 0x56052000)
+		return 2;
+
+	return -EINVAL;
+}
 
 /*
  * Disable CP0/CP1 on boot, and let call_fpe() and the iWMMXt lazy
@@ -79,17 +106,26 @@ static void __init pj4_cp_access_write(u32 value)
  */
 static int __init pj4_cp0_init(void)
 {
-	u32 cp_access;
+	u32 __maybe_unused cp_access;
+	int vers;
 
 	if (!cpu_is_pj4())
 		return 0;
 
+	vers = pj4_get_iwmmxt_version();
+	if (vers < 0)
+		return 0;
+
+#ifndef CONFIG_IWMMXT
+	pr_info("PJ4 iWMMXt coprocessor detected, but kernel support is missing.\n");
+#else
 	cp_access = pj4_cp_access_read() & ~0xf;
 	pj4_cp_access_write(cp_access);
 
-	printk(KERN_INFO "PJ4 iWMMXt coprocessor enabled.\n");
+	pr_info("PJ4 iWMMXt v%d coprocessor enabled.\n", vers);
 	elf_hwcap |= HWCAP_IWMMXT;
 	thread_register_notifier(&iwmmxt_notifier_block);
+#endif
 
 	return 0;
 }
diff --git a/arch/arm/kernel/sys_oabi-compat.c b/arch/arm/kernel/sys_oabi-compat.c
index 702bd329d9d..e90a3148f38 100644
--- a/arch/arm/kernel/sys_oabi-compat.c
+++ b/arch/arm/kernel/sys_oabi-compat.c
@@ -203,9 +203,9 @@ asmlinkage long sys_oabi_fcntl64(unsigned int fd, unsigned int cmd,
 	int ret;
 
 	switch (cmd) {
-	case F_GETLKP:
-	case F_SETLKP:
-	case F_SETLKPW:
+	case F_OFD_GETLK:
+	case F_OFD_SETLK:
+	case F_OFD_SETLKW:
 	case F_GETLK64:
 	case F_SETLK64:
 	case F_SETLKW64:
diff --git a/arch/arm/kernel/unwind.c b/arch/arm/kernel/unwind.c
index 3c217694ebe..cb791ac6a00 100644
--- a/arch/arm/kernel/unwind.c
+++ b/arch/arm/kernel/unwind.c
@@ -285,7 +285,7 @@ static int unwind_exec_pop_r4_to_rN(struct unwind_ctrl_block *ctrl,
 		if (unwind_pop_register(ctrl, &vsp, reg))
 				return -URC_FAILURE;
 
-	if (insn & 0x80)
+	if (insn & 0x8)
 		if (unwind_pop_register(ctrl, &vsp, 14))
 				return -URC_FAILURE;
 
diff --git a/arch/arm/kvm/Kconfig b/arch/arm/kvm/Kconfig
index 466bd299b1a..4be5bb150bd 100644
--- a/arch/arm/kvm/Kconfig
+++ b/arch/arm/kvm/Kconfig
@@ -23,7 +23,7 @@ config KVM
 	select HAVE_KVM_CPU_RELAX_INTERCEPT
 	select KVM_MMIO
 	select KVM_ARM_HOST
-	depends on ARM_VIRT_EXT && ARM_LPAE
+	depends on ARM_VIRT_EXT && ARM_LPAE && !CPU_BIG_ENDIAN
 	---help---
 	  Support hosting virtualized guest machines. You will also
 	  need to select one or more of the processor modules below.
diff --git a/arch/arm/kvm/mmu.c b/arch/arm/kvm/mmu.c
index 80bb1e6c2c2..16f804938b8 100644
--- a/arch/arm/kvm/mmu.c
+++ b/arch/arm/kvm/mmu.c
@@ -42,6 +42,8 @@ static unsigned long hyp_idmap_start;
 static unsigned long hyp_idmap_end;
 static phys_addr_t hyp_idmap_vector;
 
+#define pgd_order get_order(PTRS_PER_PGD * sizeof(pgd_t))
+
 #define kvm_pmd_huge(_x)	(pmd_huge(_x) || pmd_trans_huge(_x))
 
 static void kvm_tlb_flush_vmid_ipa(struct kvm *kvm, phys_addr_t ipa)
@@ -293,14 +295,14 @@ void free_boot_hyp_pgd(void)
 	if (boot_hyp_pgd) {
 		unmap_range(NULL, boot_hyp_pgd, hyp_idmap_start, PAGE_SIZE);
 		unmap_range(NULL, boot_hyp_pgd, TRAMPOLINE_VA, PAGE_SIZE);
-		kfree(boot_hyp_pgd);
+		free_pages((unsigned long)boot_hyp_pgd, pgd_order);
 		boot_hyp_pgd = NULL;
 	}
 
 	if (hyp_pgd)
 		unmap_range(NULL, hyp_pgd, TRAMPOLINE_VA, PAGE_SIZE);
 
-	kfree(init_bounce_page);
+	free_page((unsigned long)init_bounce_page);
 	init_bounce_page = NULL;
 
 	mutex_unlock(&kvm_hyp_pgd_mutex);
@@ -330,7 +332,7 @@ void free_hyp_pgds(void)
 		for (addr = VMALLOC_START; is_vmalloc_addr((void*)addr); addr += PGDIR_SIZE)
 			unmap_range(NULL, hyp_pgd, KERN_TO_HYP(addr), PGDIR_SIZE);
 
-		kfree(hyp_pgd);
+		free_pages((unsigned long)hyp_pgd, pgd_order);
 		hyp_pgd = NULL;
 	}
 
@@ -1024,7 +1026,7 @@ int kvm_mmu_init(void)
 		size_t len = __hyp_idmap_text_end - __hyp_idmap_text_start;
 		phys_addr_t phys_base;
 
-		init_bounce_page = kmalloc(PAGE_SIZE, GFP_KERNEL);
+		init_bounce_page = (void *)__get_free_page(GFP_KERNEL);
 		if (!init_bounce_page) {
 			kvm_err("Couldn't allocate HYP init bounce page\n");
 			err = -ENOMEM;
@@ -1050,8 +1052,9 @@ int kvm_mmu_init(void)
 			 (unsigned long)phys_base);
 	}
 
-	hyp_pgd = kzalloc(PTRS_PER_PGD * sizeof(pgd_t), GFP_KERNEL);
-	boot_hyp_pgd = kzalloc(PTRS_PER_PGD * sizeof(pgd_t), GFP_KERNEL);
+	hyp_pgd = (pgd_t *)__get_free_pages(GFP_KERNEL | __GFP_ZERO, pgd_order);
+	boot_hyp_pgd = (pgd_t *)__get_free_pages(GFP_KERNEL | __GFP_ZERO, pgd_order);
+
 	if (!hyp_pgd || !boot_hyp_pgd) {
 		kvm_err("Hyp mode PGD not allocated\n");
 		err = -ENOMEM;
diff --git a/arch/arm/mach-at91/Kconfig b/arch/arm/mach-at91/Kconfig
index b2d2cf4dc05..45b55e0f0db 100644
--- a/arch/arm/mach-at91/Kconfig
+++ b/arch/arm/mach-at91/Kconfig
@@ -167,7 +167,6 @@ config SOC_AT91SAM9X5
 	select HAVE_AT91_DBGU0
 	select HAVE_FB_ATMEL
 	select SOC_AT91SAM9
-	select AT91_USE_OLD_CLK
 	select HAVE_AT91_UTMI
 	select HAVE_AT91_SMD
 	select HAVE_AT91_USB_CLK
@@ -183,7 +182,6 @@ config SOC_AT91SAM9N12
 	select HAVE_AT91_DBGU0
 	select HAVE_FB_ATMEL
 	select SOC_AT91SAM9
-	select AT91_USE_OLD_CLK
 	select HAVE_AT91_USB_CLK
 	help
 	  Select this if you are using Atmel's AT91SAM9N12 SoC.
diff --git a/arch/arm/mach-at91/at91rm9200_devices.c b/arch/arm/mach-at91/at91rm9200_devices.c
index f3f19f21352..3f4bb58aea5 100644
--- a/arch/arm/mach-at91/at91rm9200_devices.c
+++ b/arch/arm/mach-at91/at91rm9200_devices.c
@@ -15,6 +15,7 @@
 
 #include <linux/dma-mapping.h>
 #include <linux/gpio.h>
+#include <linux/gpio/driver.h>
 #include <linux/platform_device.h>
 #include <linux/i2c-gpio.h>
 
@@ -25,6 +26,7 @@
 
 #include "board.h"
 #include "generic.h"
+#include "gpio.h"
 
 
 /* --------------------------------------------------------------------
@@ -923,7 +925,6 @@ static struct resource dbgu_resources[] = {
 static struct atmel_uart_data dbgu_data = {
 	.use_dma_tx	= 0,
 	.use_dma_rx	= 0,		/* DBGU not capable of receive DMA */
-	.rts_gpio	= -EINVAL,
 };
 
 static u64 dbgu_dmamask = DMA_BIT_MASK(32);
@@ -962,7 +963,14 @@ static struct resource uart0_resources[] = {
 static struct atmel_uart_data uart0_data = {
 	.use_dma_tx	= 1,
 	.use_dma_rx	= 1,
-	.rts_gpio	= -EINVAL,
+};
+
+static struct gpiod_lookup_table uart0_gpios_table = {
+	.dev_id = "atmel_usart",
+	.table = {
+		GPIO_LOOKUP("pioA", 21, "rts", GPIO_ACTIVE_LOW),
+		{ },
+	},
 };
 
 static u64 uart0_dmamask = DMA_BIT_MASK(32);
@@ -993,7 +1001,7 @@ static inline void configure_usart0_pins(unsigned pins)
 		 * We need to drive the pin manually. The serial driver will driver
 		 * this to high when initializing.
 		 */
-		uart0_data.rts_gpio = AT91_PIN_PA21;
+		gpiod_add_lookup_table(&uart0_gpios_table);
 	}
 }
 
@@ -1013,7 +1021,6 @@ static struct resource uart1_resources[] = {
 static struct atmel_uart_data uart1_data = {
 	.use_dma_tx	= 1,
 	.use_dma_rx	= 1,
-	.rts_gpio	= -EINVAL,
 };
 
 static u64 uart1_dmamask = DMA_BIT_MASK(32);
@@ -1065,7 +1072,6 @@ static struct resource uart2_resources[] = {
 static struct atmel_uart_data uart2_data = {
 	.use_dma_tx	= 1,
 	.use_dma_rx	= 1,
-	.rts_gpio	= -EINVAL,
 };
 
 static u64 uart2_dmamask = DMA_BIT_MASK(32);
@@ -1109,7 +1115,6 @@ static struct resource uart3_resources[] = {
 static struct atmel_uart_data uart3_data = {
 	.use_dma_tx	= 1,
 	.use_dma_rx	= 1,
-	.rts_gpio	= -EINVAL,
 };
 
 static u64 uart3_dmamask = DMA_BIT_MASK(32);
diff --git a/arch/arm/mach-at91/at91sam9260_devices.c b/arch/arm/mach-at91/at91sam9260_devices.c
index 8b1b0a87002..ef88e0fe4e8 100644
--- a/arch/arm/mach-at91/at91sam9260_devices.c
+++ b/arch/arm/mach-at91/at91sam9260_devices.c
@@ -24,12 +24,11 @@
 #include <mach/at91sam9260_matrix.h>
 #include <mach/at91_matrix.h>
 #include <mach/at91sam9_smc.h>
-#include <mach/at91_adc.h>
 #include <mach/hardware.h>
 
 #include "board.h"
 #include "generic.h"
-
+#include "gpio.h"
 
 /* --------------------------------------------------------------------
  *  USB Host
@@ -820,7 +819,6 @@ static struct resource dbgu_resources[] = {
 static struct atmel_uart_data dbgu_data = {
 	.use_dma_tx	= 0,
 	.use_dma_rx	= 0,		/* DBGU not capable of receive DMA */
-	.rts_gpio	= -EINVAL,
 };
 
 static u64 dbgu_dmamask = DMA_BIT_MASK(32);
@@ -859,7 +857,6 @@ static struct resource uart0_resources[] = {
 static struct atmel_uart_data uart0_data = {
 	.use_dma_tx	= 1,
 	.use_dma_rx	= 1,
-	.rts_gpio	= -EINVAL,
 };
 
 static u64 uart0_dmamask = DMA_BIT_MASK(32);
@@ -911,7 +908,6 @@ static struct resource uart1_resources[] = {
 static struct atmel_uart_data uart1_data = {
 	.use_dma_tx	= 1,
 	.use_dma_rx	= 1,
-	.rts_gpio	= -EINVAL,
 };
 
 static u64 uart1_dmamask = DMA_BIT_MASK(32);
@@ -955,7 +951,6 @@ static struct resource uart2_resources[] = {
 static struct atmel_uart_data uart2_data = {
 	.use_dma_tx	= 1,
 	.use_dma_rx	= 1,
-	.rts_gpio	= -EINVAL,
 };
 
 static u64 uart2_dmamask = DMA_BIT_MASK(32);
@@ -999,7 +994,6 @@ static struct resource uart3_resources[] = {
 static struct atmel_uart_data uart3_data = {
 	.use_dma_tx	= 1,
 	.use_dma_rx	= 1,
-	.rts_gpio	= -EINVAL,
 };
 
 static u64 uart3_dmamask = DMA_BIT_MASK(32);
@@ -1043,7 +1037,6 @@ static struct resource uart4_resources[] = {
 static struct atmel_uart_data uart4_data = {
 	.use_dma_tx	= 1,
 	.use_dma_rx	= 1,
-	.rts_gpio	= -EINVAL,
 };
 
 static u64 uart4_dmamask = DMA_BIT_MASK(32);
@@ -1082,7 +1075,6 @@ static struct resource uart5_resources[] = {
 static struct atmel_uart_data uart5_data = {
 	.use_dma_tx	= 1,
 	.use_dma_rx	= 1,
-	.rts_gpio	= -EINVAL,
 };
 
 static u64 uart5_dmamask = DMA_BIT_MASK(32);
@@ -1296,7 +1288,7 @@ static struct resource adc_resources[] = {
 };
 
 static struct platform_device at91_adc_device = {
-	.name		= "at91_adc",
+	.name		= "at91sam9260-adc",
 	.id		= -1,
 	.dev		= {
 				.platform_data		= &adc_data,
@@ -1308,30 +1300,23 @@ static struct platform_device at91_adc_device = {
 static struct at91_adc_trigger at91_adc_triggers[] = {
 	[0] = {
 		.name = "timer-counter-0",
-		.value = AT91_ADC_TRGSEL_TC0 | AT91_ADC_TRGEN,
+		.value = 0x1,
 	},
 	[1] = {
 		.name = "timer-counter-1",
-		.value = AT91_ADC_TRGSEL_TC1 | AT91_ADC_TRGEN,
+		.value = 0x3,
 	},
 	[2] = {
 		.name = "timer-counter-2",
-		.value = AT91_ADC_TRGSEL_TC2 | AT91_ADC_TRGEN,
+		.value = 0x5,
 	},
 	[3] = {
 		.name = "external",
-		.value = AT91_ADC_TRGSEL_EXTERNAL | AT91_ADC_TRGEN,
+		.value = 0xd,
 		.is_external = true,
 	},
 };
 
-static struct at91_adc_reg_desc at91_adc_register_g20 = {
-	.channel_base = AT91_ADC_CHR(0),
-	.drdy_mask = AT91_ADC_DRDY,
-	.status_register = AT91_ADC_SR,
-	.trigger_register = AT91_ADC_MR,
-};
-
 void __init at91_add_device_adc(struct at91_adc_data *data)
 {
 	if (!data)
@@ -1349,9 +1334,7 @@ void __init at91_add_device_adc(struct at91_adc_data *data)
 	if (data->use_external_triggers)
 		at91_set_A_periph(AT91_PIN_PA22, 0);
 
-	data->num_channels = 4;
 	data->startup_time = 10;
-	data->registers = &at91_adc_register_g20;
 	data->trigger_number = 4;
 	data->trigger_list = at91_adc_triggers;
 
diff --git a/arch/arm/mach-at91/at91sam9261_devices.c b/arch/arm/mach-at91/at91sam9261_devices.c
index 80e35895d28..29baacb5c35 100644
--- a/arch/arm/mach-at91/at91sam9261_devices.c
+++ b/arch/arm/mach-at91/at91sam9261_devices.c
@@ -29,7 +29,7 @@
 
 #include "board.h"
 #include "generic.h"
-
+#include "gpio.h"
 
 /* --------------------------------------------------------------------
  *  USB Host
@@ -881,7 +881,6 @@ static struct resource dbgu_resources[] = {
 static struct atmel_uart_data dbgu_data = {
 	.use_dma_tx	= 0,
 	.use_dma_rx	= 0,		/* DBGU not capable of receive DMA */
-	.rts_gpio	= -EINVAL,
 };
 
 static u64 dbgu_dmamask = DMA_BIT_MASK(32);
@@ -920,7 +919,6 @@ static struct resource uart0_resources[] = {
 static struct atmel_uart_data uart0_data = {
 	.use_dma_tx	= 1,
 	.use_dma_rx	= 1,
-	.rts_gpio	= -EINVAL,
 };
 
 static u64 uart0_dmamask = DMA_BIT_MASK(32);
@@ -964,7 +962,6 @@ static struct resource uart1_resources[] = {
 static struct atmel_uart_data uart1_data = {
 	.use_dma_tx	= 1,
 	.use_dma_rx	= 1,
-	.rts_gpio	= -EINVAL,
 };
 
 static u64 uart1_dmamask = DMA_BIT_MASK(32);
@@ -1008,7 +1005,6 @@ static struct resource uart2_resources[] = {
 static struct atmel_uart_data uart2_data = {
 	.use_dma_tx	= 1,
 	.use_dma_rx	= 1,
-	.rts_gpio	= -EINVAL,
 };
 
 static u64 uart2_dmamask = DMA_BIT_MASK(32);
diff --git a/arch/arm/mach-at91/at91sam9263_devices.c b/arch/arm/mach-at91/at91sam9263_devices.c
index 43d53d6156d..309390d8e2f 100644
--- a/arch/arm/mach-at91/at91sam9263_devices.c
+++ b/arch/arm/mach-at91/at91sam9263_devices.c
@@ -28,6 +28,7 @@
 
 #include "board.h"
 #include "generic.h"
+#include "gpio.h"
 
 
 /* --------------------------------------------------------------------
@@ -1325,7 +1326,6 @@ static struct resource dbgu_resources[] = {
 static struct atmel_uart_data dbgu_data = {
 	.use_dma_tx	= 0,
 	.use_dma_rx	= 0,		/* DBGU not capable of receive DMA */
-	.rts_gpio	= -EINVAL,
 };
 
 static u64 dbgu_dmamask = DMA_BIT_MASK(32);
@@ -1364,7 +1364,6 @@ static struct resource uart0_resources[] = {
 static struct atmel_uart_data uart0_data = {
 	.use_dma_tx	= 1,
 	.use_dma_rx	= 1,
-	.rts_gpio	= -EINVAL,
 };
 
 static u64 uart0_dmamask = DMA_BIT_MASK(32);
@@ -1408,7 +1407,6 @@ static struct resource uart1_resources[] = {
 static struct atmel_uart_data uart1_data = {
 	.use_dma_tx	= 1,
 	.use_dma_rx	= 1,
-	.rts_gpio	= -EINVAL,
 };
 
 static u64 uart1_dmamask = DMA_BIT_MASK(32);
@@ -1452,7 +1450,6 @@ static struct resource uart2_resources[] = {
 static struct atmel_uart_data uart2_data = {
 	.use_dma_tx	= 1,
 	.use_dma_rx	= 1,
-	.rts_gpio	= -EINVAL,
 };
 
 static u64 uart2_dmamask = DMA_BIT_MASK(32);
diff --git a/arch/arm/mach-at91/at91sam9g45.c b/arch/arm/mach-at91/at91sam9g45.c
index 5e6f498db0a..9d3d544ac19 100644
--- a/arch/arm/mach-at91/at91sam9g45.c
+++ b/arch/arm/mach-at91/at91sam9g45.c
@@ -182,7 +182,7 @@ static struct clk vdec_clk = {
 static struct clk adc_op_clk = {
 	.name		= "adc_op_clk",
 	.type		= CLK_TYPE_PERIPHERAL,
-	.rate_hz	= 13200000,
+	.rate_hz	= 300000,
 };
 
 /* AES/TDES/SHA clock - Only for sam9m11/sam9g56 */
diff --git a/arch/arm/mach-at91/at91sam9g45_devices.c b/arch/arm/mach-at91/at91sam9g45_devices.c
index 77b04c2edd7..391ab6bb536 100644
--- a/arch/arm/mach-at91/at91sam9g45_devices.c
+++ b/arch/arm/mach-at91/at91sam9g45_devices.c
@@ -25,7 +25,6 @@
 #include <linux/fb.h>
 #include <video/atmel_lcdc.h>
 
-#include <mach/at91_adc.h>
 #include <mach/at91sam9g45.h>
 #include <mach/at91sam9g45_matrix.h>
 #include <mach/at91_matrix.h>
@@ -39,6 +38,7 @@
 #include "board.h"
 #include "generic.h"
 #include "clock.h"
+#include "gpio.h"
 
 
 /* --------------------------------------------------------------------
@@ -1133,58 +1133,7 @@ static void __init at91_add_device_rtc(void) {}
 
 
 /* --------------------------------------------------------------------
- *  Touchscreen
- * -------------------------------------------------------------------- */
-
-#if defined(CONFIG_TOUCHSCREEN_ATMEL_TSADCC) || defined(CONFIG_TOUCHSCREEN_ATMEL_TSADCC_MODULE)
-static u64 tsadcc_dmamask = DMA_BIT_MASK(32);
-static struct at91_tsadcc_data tsadcc_data;
-
-static struct resource tsadcc_resources[] = {
-	[0] = {
-		.start	= AT91SAM9G45_BASE_TSC,
-		.end	= AT91SAM9G45_BASE_TSC + SZ_16K - 1,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= NR_IRQS_LEGACY + AT91SAM9G45_ID_TSC,
-		.end	= NR_IRQS_LEGACY + AT91SAM9G45_ID_TSC,
-		.flags	= IORESOURCE_IRQ,
-	}
-};
-
-static struct platform_device at91sam9g45_tsadcc_device = {
-	.name		= "atmel_tsadcc",
-	.id		= -1,
-	.dev		= {
-				.dma_mask		= &tsadcc_dmamask,
-				.coherent_dma_mask	= DMA_BIT_MASK(32),
-				.platform_data		= &tsadcc_data,
-	},
-	.resource	= tsadcc_resources,
-	.num_resources	= ARRAY_SIZE(tsadcc_resources),
-};
-
-void __init at91_add_device_tsadcc(struct at91_tsadcc_data *data)
-{
-	if (!data)
-		return;
-
-	at91_set_gpio_input(AT91_PIN_PD20, 0);	/* AD0_XR */
-	at91_set_gpio_input(AT91_PIN_PD21, 0);	/* AD1_XL */
-	at91_set_gpio_input(AT91_PIN_PD22, 0);	/* AD2_YT */
-	at91_set_gpio_input(AT91_PIN_PD23, 0);	/* AD3_TB */
-
-	tsadcc_data = *data;
-	platform_device_register(&at91sam9g45_tsadcc_device);
-}
-#else
-void __init at91_add_device_tsadcc(struct at91_tsadcc_data *data) {}
-#endif
-
-
-/* --------------------------------------------------------------------
- *  ADC
+ *  ADC and touchscreen
  * -------------------------------------------------------------------- */
 
 #if IS_ENABLED(CONFIG_AT91_ADC)
@@ -1204,7 +1153,7 @@ static struct resource adc_resources[] = {
 };
 
 static struct platform_device at91_adc_device = {
-	.name		= "at91_adc",
+	.name		= "at91sam9g45-adc",
 	.id		= -1,
 	.dev		= {
 				.platform_data	= &adc_data,
@@ -1236,13 +1185,6 @@ static struct at91_adc_trigger at91_adc_triggers[] = {
 	},
 };
 
-static struct at91_adc_reg_desc at91_adc_register_g45 = {
-	.channel_base = AT91_ADC_CHR(0),
-	.drdy_mask = AT91_ADC_DRDY,
-	.status_register = AT91_ADC_SR,
-	.trigger_register = 0x08,
-};
-
 void __init at91_add_device_adc(struct at91_adc_data *data)
 {
 	if (!data)
@@ -1268,9 +1210,7 @@ void __init at91_add_device_adc(struct at91_adc_data *data)
 	if (data->use_external_triggers)
 		at91_set_A_periph(AT91_PIN_PD28, 0);
 
-	data->num_channels = 8;
 	data->startup_time = 40;
-	data->registers = &at91_adc_register_g45;
 	data->trigger_number = 4;
 	data->trigger_list = at91_adc_triggers;
 
@@ -1588,7 +1528,6 @@ static struct resource dbgu_resources[] = {
 static struct atmel_uart_data dbgu_data = {
 	.use_dma_tx	= 0,
 	.use_dma_rx	= 0,
-	.rts_gpio	= -EINVAL,
 };
 
 static u64 dbgu_dmamask = DMA_BIT_MASK(32);
@@ -1627,7 +1566,6 @@ static struct resource uart0_resources[] = {
 static struct atmel_uart_data uart0_data = {
 	.use_dma_tx	= 1,
 	.use_dma_rx	= 1,
-	.rts_gpio	= -EINVAL,
 };
 
 static u64 uart0_dmamask = DMA_BIT_MASK(32);
@@ -1671,7 +1609,6 @@ static struct resource uart1_resources[] = {
 static struct atmel_uart_data uart1_data = {
 	.use_dma_tx	= 1,
 	.use_dma_rx	= 1,
-	.rts_gpio	= -EINVAL,
 };
 
 static u64 uart1_dmamask = DMA_BIT_MASK(32);
@@ -1715,7 +1652,6 @@ static struct resource uart2_resources[] = {
 static struct atmel_uart_data uart2_data = {
 	.use_dma_tx	= 1,
 	.use_dma_rx	= 1,
-	.rts_gpio	= -EINVAL,
 };
 
 static u64 uart2_dmamask = DMA_BIT_MASK(32);
@@ -1759,7 +1695,6 @@ static struct resource uart3_resources[] = {
 static struct atmel_uart_data uart3_data = {
 	.use_dma_tx	= 1,
 	.use_dma_rx	= 1,
-	.rts_gpio	= -EINVAL,
 };
 
 static u64 uart3_dmamask = DMA_BIT_MASK(32);
diff --git a/arch/arm/mach-at91/at91sam9n12.c b/arch/arm/mach-at91/at91sam9n12.c
index f2ea7b0a02d..c8988fe5ff7 100644
--- a/arch/arm/mach-at91/at91sam9n12.c
+++ b/arch/arm/mach-at91/at91sam9n12.c
@@ -19,9 +19,10 @@
 #include "board.h"
 #include "soc.h"
 #include "generic.h"
-#include "clock.h"
 #include "sam9_smc.h"
 
+#if defined(CONFIG_OLD_CLK_AT91)
+#include "clock.h"
 /* --------------------------------------------------------------------
  *  Clocks
  * -------------------------------------------------------------------- */
@@ -215,6 +216,9 @@ static void __init at91sam9n12_register_clocks(void)
 			 ARRAY_SIZE(periph_clocks_lookups));
 
 }
+#else
+#define at91sam9n12_register_clocks NULL
+#endif
 
 /* --------------------------------------------------------------------
  *  AT91SAM9N12 processor initialization
diff --git a/arch/arm/mach-at91/at91sam9rl.c b/arch/arm/mach-at91/at91sam9rl.c
index 57f12d86c0e..a79960f57e6 100644
--- a/arch/arm/mach-at91/at91sam9rl.c
+++ b/arch/arm/mach-at91/at91sam9rl.c
@@ -153,6 +153,11 @@ static struct clk ac97_clk = {
 	.pmc_mask	= 1 << AT91SAM9RL_ID_AC97C,
 	.type		= CLK_TYPE_PERIPHERAL,
 };
+static struct clk adc_op_clk = {
+	.name		= "adc_op_clk",
+	.type		= CLK_TYPE_PERIPHERAL,
+	.rate_hz	= 1000000,
+};
 
 static struct clk *periph_clocks[] __initdata = {
 	&pioA_clk,
@@ -178,6 +183,7 @@ static struct clk *periph_clocks[] __initdata = {
 	&udphs_clk,
 	&lcdc_clk,
 	&ac97_clk,
+	&adc_op_clk,
 	// irq0
 };
 
@@ -216,6 +222,7 @@ static struct clk_lookup periph_clocks_lookups[] = {
 	CLKDEV_CON_DEV_ID(NULL, "fffff600.gpio", &pioB_clk),
 	CLKDEV_CON_DEV_ID(NULL, "fffff800.gpio", &pioC_clk),
 	CLKDEV_CON_DEV_ID(NULL, "fffffa00.gpio", &pioD_clk),
+	CLKDEV_CON_ID("adc_clk", &tsc_clk),
 };
 
 static struct clk_lookup usart_clocks_lookups[] = {
diff --git a/arch/arm/mach-at91/at91sam9rl_devices.c b/arch/arm/mach-at91/at91sam9rl_devices.c
index 428fc412aaf..0b1d71a7d9b 100644
--- a/arch/arm/mach-at91/at91sam9rl_devices.c
+++ b/arch/arm/mach-at91/at91sam9rl_devices.c
@@ -23,9 +23,11 @@
 #include <mach/at91sam9_smc.h>
 #include <mach/hardware.h>
 #include <linux/platform_data/dma-atmel.h>
+#include <linux/platform_data/at91_adc.h>
 
 #include "board.h"
 #include "generic.h"
+#include "gpio.h"
 
 
 /* --------------------------------------------------------------------
@@ -608,14 +610,13 @@ static void __init at91_add_device_tc(void) { }
 
 
 /* --------------------------------------------------------------------
- *  Touchscreen
+ *  ADC and Touchscreen
  * -------------------------------------------------------------------- */
 
-#if defined(CONFIG_TOUCHSCREEN_ATMEL_TSADCC) || defined(CONFIG_TOUCHSCREEN_ATMEL_TSADCC_MODULE)
-static u64 tsadcc_dmamask = DMA_BIT_MASK(32);
-static struct at91_tsadcc_data tsadcc_data;
+#if IS_ENABLED(CONFIG_AT91_ADC)
+static struct at91_adc_data adc_data;
 
-static struct resource tsadcc_resources[] = {
+static struct resource adc_resources[] = {
 	[0] = {
 		.start	= AT91SAM9RL_BASE_TSC,
 		.end	= AT91SAM9RL_BASE_TSC + SZ_16K - 1,
@@ -628,36 +629,71 @@ static struct resource tsadcc_resources[] = {
 	}
 };
 
-static struct platform_device at91sam9rl_tsadcc_device = {
-	.name		= "atmel_tsadcc",
-	.id		= -1,
-	.dev		= {
-				.dma_mask		= &tsadcc_dmamask,
-				.coherent_dma_mask	= DMA_BIT_MASK(32),
-				.platform_data		= &tsadcc_data,
+static struct platform_device at91_adc_device = {
+	.name           = "at91sam9rl-adc",
+	.id             = -1,
+	.dev            = {
+		.platform_data  = &adc_data,
 	},
-	.resource	= tsadcc_resources,
-	.num_resources	= ARRAY_SIZE(tsadcc_resources),
+	.resource       = adc_resources,
+	.num_resources  = ARRAY_SIZE(adc_resources),
 };
 
-void __init at91_add_device_tsadcc(struct at91_tsadcc_data *data)
+static struct at91_adc_trigger at91_adc_triggers[] = {
+	[0] = {
+		.name = "external-rising",
+		.value = 1,
+		.is_external = true,
+	},
+	[1] = {
+		.name = "external-falling",
+		.value = 2,
+		.is_external = true,
+	},
+	[2] = {
+		.name = "external-any",
+		.value = 3,
+		.is_external = true,
+	},
+	[3] = {
+		.name = "continuous",
+		.value = 6,
+		.is_external = false,
+	},
+};
+
+void __init at91_add_device_adc(struct at91_adc_data *data)
 {
 	if (!data)
 		return;
 
-	at91_set_A_periph(AT91_PIN_PA17, 0);	/* AD0_XR */
-	at91_set_A_periph(AT91_PIN_PA18, 0);	/* AD1_XL */
-	at91_set_A_periph(AT91_PIN_PA19, 0);	/* AD2_YT */
-	at91_set_A_periph(AT91_PIN_PA20, 0);	/* AD3_TB */
-
-	tsadcc_data = *data;
-	platform_device_register(&at91sam9rl_tsadcc_device);
+	if (test_bit(0, &data->channels_used))
+		at91_set_A_periph(AT91_PIN_PA17, 0);
+	if (test_bit(1, &data->channels_used))
+		at91_set_A_periph(AT91_PIN_PA18, 0);
+	if (test_bit(2, &data->channels_used))
+		at91_set_A_periph(AT91_PIN_PA19, 0);
+	if (test_bit(3, &data->channels_used))
+		at91_set_A_periph(AT91_PIN_PA20, 0);
+	if (test_bit(4, &data->channels_used))
+		at91_set_A_periph(AT91_PIN_PD6, 0);
+	if (test_bit(5, &data->channels_used))
+		at91_set_A_periph(AT91_PIN_PD7, 0);
+
+	if (data->use_external_triggers)
+		at91_set_A_periph(AT91_PIN_PB15, 0);
+
+	data->startup_time = 40;
+	data->trigger_number = 4;
+	data->trigger_list = at91_adc_triggers;
+
+	adc_data = *data;
+	platform_device_register(&at91_adc_device);
 }
 #else
-void __init at91_add_device_tsadcc(struct at91_tsadcc_data *data) {}
+void __init at91_add_device_adc(struct at91_adc_data *data) {}
 #endif
 
-
 /* --------------------------------------------------------------------
  *  RTC
  * -------------------------------------------------------------------- */
@@ -957,7 +993,6 @@ static struct resource dbgu_resources[] = {
 static struct atmel_uart_data dbgu_data = {
 	.use_dma_tx	= 0,
 	.use_dma_rx	= 0,		/* DBGU not capable of receive DMA */
-	.rts_gpio	= -EINVAL,
 };
 
 static u64 dbgu_dmamask = DMA_BIT_MASK(32);
@@ -996,7 +1031,6 @@ static struct resource uart0_resources[] = {
 static struct atmel_uart_data uart0_data = {
 	.use_dma_tx	= 1,
 	.use_dma_rx	= 1,
-	.rts_gpio	= -EINVAL,
 };
 
 static u64 uart0_dmamask = DMA_BIT_MASK(32);
@@ -1048,7 +1082,6 @@ static struct resource uart1_resources[] = {
 static struct atmel_uart_data uart1_data = {
 	.use_dma_tx	= 1,
 	.use_dma_rx	= 1,
-	.rts_gpio	= -EINVAL,
 };
 
 static u64 uart1_dmamask = DMA_BIT_MASK(32);
@@ -1092,7 +1125,6 @@ static struct resource uart2_resources[] = {
 static struct atmel_uart_data uart2_data = {
 	.use_dma_tx	= 1,
 	.use_dma_rx	= 1,
-	.rts_gpio	= -EINVAL,
 };
 
 static u64 uart2_dmamask = DMA_BIT_MASK(32);
@@ -1136,7 +1168,6 @@ static struct resource uart3_resources[] = {
 static struct atmel_uart_data uart3_data = {
 	.use_dma_tx	= 1,
 	.use_dma_rx	= 1,
-	.rts_gpio	= -EINVAL,
 };
 
 static u64 uart3_dmamask = DMA_BIT_MASK(32);
diff --git a/arch/arm/mach-at91/at91sam9x5.c b/arch/arm/mach-at91/at91sam9x5.c
index 9ad781d5ee7..028268ff372 100644
--- a/arch/arm/mach-at91/at91sam9x5.c
+++ b/arch/arm/mach-at91/at91sam9x5.c
@@ -19,9 +19,10 @@
 #include "board.h"
 #include "soc.h"
 #include "generic.h"
-#include "clock.h"
 #include "sam9_smc.h"
 
+#if defined(CONFIG_OLD_CLK_AT91)
+#include "clock.h"
 /* --------------------------------------------------------------------
  *  Clocks
  * -------------------------------------------------------------------- */
@@ -313,6 +314,9 @@ static void __init at91sam9x5_register_clocks(void)
 	clk_register(&pck0);
 	clk_register(&pck1);
 }
+#else
+#define at91sam9x5_register_clocks	NULL
+#endif
 
 /* --------------------------------------------------------------------
  *  AT91SAM9x5 processor initialization
diff --git a/arch/arm/mach-at91/board-1arm.c b/arch/arm/mach-at91/board-1arm.c
index 35ab632bbf6..3f6dbcc3402 100644
--- a/arch/arm/mach-at91/board-1arm.c
+++ b/arch/arm/mach-at91/board-1arm.c
@@ -39,7 +39,7 @@
 #include "at91_aic.h"
 #include "board.h"
 #include "generic.h"
-
+#include "gpio.h"
 
 static void __init onearm_init_early(void)
 {
diff --git a/arch/arm/mach-at91/board-afeb-9260v1.c b/arch/arm/mach-at91/board-afeb-9260v1.c
index f95e31cda4b..597c649170a 100644
--- a/arch/arm/mach-at91/board-afeb-9260v1.c
+++ b/arch/arm/mach-at91/board-afeb-9260v1.c
@@ -46,6 +46,7 @@
 #include "at91_aic.h"
 #include "board.h"
 #include "generic.h"
+#include "gpio.h"
 
 
 static void __init afeb9260_init_early(void)
diff --git a/arch/arm/mach-at91/board-cam60.c b/arch/arm/mach-at91/board-cam60.c
index 112e867c4ab..a30502c8d37 100644
--- a/arch/arm/mach-at91/board-cam60.c
+++ b/arch/arm/mach-at91/board-cam60.c
@@ -44,6 +44,7 @@
 #include "board.h"
 #include "sam9_smc.h"
 #include "generic.h"
+#include "gpio.h"
 
 
 static void __init cam60_init_early(void)
diff --git a/arch/arm/mach-at91/board-carmeva.c b/arch/arm/mach-at91/board-carmeva.c
index 92983050a9b..47313d3ee03 100644
--- a/arch/arm/mach-at91/board-carmeva.c
+++ b/arch/arm/mach-at91/board-carmeva.c
@@ -39,6 +39,7 @@
 #include "at91_aic.h"
 #include "board.h"
 #include "generic.h"
+#include "gpio.h"
 
 
 static void __init carmeva_init_early(void)
diff --git a/arch/arm/mach-at91/board-cpu9krea.c b/arch/arm/mach-at91/board-cpu9krea.c
index 008527efdbc..2037f78c84e 100644
--- a/arch/arm/mach-at91/board-cpu9krea.c
+++ b/arch/arm/mach-at91/board-cpu9krea.c
@@ -48,6 +48,7 @@
 #include "board.h"
 #include "sam9_smc.h"
 #include "generic.h"
+#include "gpio.h"
 
 static void __init cpu9krea_init_early(void)
 {
diff --git a/arch/arm/mach-at91/board-cpuat91.c b/arch/arm/mach-at91/board-cpuat91.c
index 42f1353a4ba..c094350c931 100644
--- a/arch/arm/mach-at91/board-cpuat91.c
+++ b/arch/arm/mach-at91/board-cpuat91.c
@@ -43,6 +43,8 @@
 #include "at91_aic.h"
 #include "board.h"
 #include "generic.h"
+#include "gpio.h"
+
 
 static struct gpio_led cpuat91_leds[] = {
 	{
diff --git a/arch/arm/mach-at91/board-csb337.c b/arch/arm/mach-at91/board-csb337.c
index e5fde215225..0e35a45cf8d 100644
--- a/arch/arm/mach-at91/board-csb337.c
+++ b/arch/arm/mach-at91/board-csb337.c
@@ -42,7 +42,7 @@
 #include "at91_aic.h"
 #include "board.h"
 #include "generic.h"
-
+#include "gpio.h"
 
 static void __init csb337_init_early(void)
 {
diff --git a/arch/arm/mach-at91/board-csb637.c b/arch/arm/mach-at91/board-csb637.c
index fdf11061c57..18d027f529a 100644
--- a/arch/arm/mach-at91/board-csb637.c
+++ b/arch/arm/mach-at91/board-csb637.c
@@ -39,6 +39,7 @@
 #include "at91_aic.h"
 #include "board.h"
 #include "generic.h"
+#include "gpio.h"
 
 
 static void __init csb637_init_early(void)
diff --git a/arch/arm/mach-at91/board-eb9200.c b/arch/arm/mach-at91/board-eb9200.c
index f9be8161bbf..aa457a8b22f 100644
--- a/arch/arm/mach-at91/board-eb9200.c
+++ b/arch/arm/mach-at91/board-eb9200.c
@@ -38,6 +38,7 @@
 #include "at91_aic.h"
 #include "board.h"
 #include "generic.h"
+#include "gpio.h"
 
 
 static void __init eb9200_init_early(void)
diff --git a/arch/arm/mach-at91/board-ecbat91.c b/arch/arm/mach-at91/board-ecbat91.c
index b2fcd71262b..ede1373ccab 100644
--- a/arch/arm/mach-at91/board-ecbat91.c
+++ b/arch/arm/mach-at91/board-ecbat91.c
@@ -42,6 +42,7 @@
 #include "at91_aic.h"
 #include "board.h"
 #include "generic.h"
+#include "gpio.h"
 
 
 static void __init ecb_at91init_early(void)
diff --git a/arch/arm/mach-at91/board-eco920.c b/arch/arm/mach-at91/board-eco920.c
index 77de410efc9..4e75321a8f2 100644
--- a/arch/arm/mach-at91/board-eco920.c
+++ b/arch/arm/mach-at91/board-eco920.c
@@ -31,6 +31,8 @@
 #include "at91_aic.h"
 #include "board.h"
 #include "generic.h"
+#include "gpio.h"
+
 
 static void __init eco920_init_early(void)
 {
diff --git a/arch/arm/mach-at91/board-flexibity.c b/arch/arm/mach-at91/board-flexibity.c
index 737c0856362..68f1ab6bd08 100644
--- a/arch/arm/mach-at91/board-flexibity.c
+++ b/arch/arm/mach-at91/board-flexibity.c
@@ -37,6 +37,7 @@
 #include "at91_aic.h"
 #include "board.h"
 #include "generic.h"
+#include "gpio.h"
 
 static void __init flexibity_init_early(void)
 {
diff --git a/arch/arm/mach-at91/board-foxg20.c b/arch/arm/mach-at91/board-foxg20.c
index c20a870ea9c..8b22c60bb23 100644
--- a/arch/arm/mach-at91/board-foxg20.c
+++ b/arch/arm/mach-at91/board-foxg20.c
@@ -47,6 +47,7 @@
 #include "board.h"
 #include "sam9_smc.h"
 #include "generic.h"
+#include "gpio.h"
 
 /*
  * The FOX Board G20 hardware comes as the "Netus G20" board with
diff --git a/arch/arm/mach-at91/board-gsia18s.c b/arch/arm/mach-at91/board-gsia18s.c
index 416bae8435e..b729dd1271b 100644
--- a/arch/arm/mach-at91/board-gsia18s.c
+++ b/arch/arm/mach-at91/board-gsia18s.c
@@ -39,6 +39,7 @@
 #include "generic.h"
 #include "gsia18s.h"
 #include "stamp9g20.h"
+#include "gpio.h"
 
 static void __init gsia18s_init_early(void)
 {
diff --git a/arch/arm/mach-at91/board-kafa.c b/arch/arm/mach-at91/board-kafa.c
index 88e2f5d2d16..93b1df42f63 100644
--- a/arch/arm/mach-at91/board-kafa.c
+++ b/arch/arm/mach-at91/board-kafa.c
@@ -39,6 +39,7 @@
 #include "at91_aic.h"
 #include "board.h"
 #include "generic.h"
+#include "gpio.h"
 
 
 static void __init kafa_init_early(void)
diff --git a/arch/arm/mach-at91/board-kb9202.c b/arch/arm/mach-at91/board-kb9202.c
index 0c519d9ebff..d58d36225e0 100644
--- a/arch/arm/mach-at91/board-kb9202.c
+++ b/arch/arm/mach-at91/board-kb9202.c
@@ -42,6 +42,7 @@
 #include "at91_aic.h"
 #include "board.h"
 #include "generic.h"
+#include "gpio.h"
 
 
 static void __init kb9202_init_early(void)
diff --git a/arch/arm/mach-at91/board-pcontrol-g20.c b/arch/arm/mach-at91/board-pcontrol-g20.c
index 5f25fa54eb9..b48d95ec515 100644
--- a/arch/arm/mach-at91/board-pcontrol-g20.c
+++ b/arch/arm/mach-at91/board-pcontrol-g20.c
@@ -37,6 +37,7 @@
 #include "sam9_smc.h"
 #include "generic.h"
 #include "stamp9g20.h"
+#include "gpio.h"
 
 
 static void __init pcontrol_g20_init_early(void)
diff --git a/arch/arm/mach-at91/board-picotux200.c b/arch/arm/mach-at91/board-picotux200.c
index ab2b2ec36c1..2c0f2d554d8 100644
--- a/arch/arm/mach-at91/board-picotux200.c
+++ b/arch/arm/mach-at91/board-picotux200.c
@@ -43,6 +43,7 @@
 #include "at91_aic.h"
 #include "board.h"
 #include "generic.h"
+#include "gpio.h"
 
 
 static void __init picotux200_init_early(void)
diff --git a/arch/arm/mach-at91/board-rm9200ek.c b/arch/arm/mach-at91/board-rm9200ek.c
index 8b17dadc1ab..953cea41675 100644
--- a/arch/arm/mach-at91/board-rm9200ek.c
+++ b/arch/arm/mach-at91/board-rm9200ek.c
@@ -45,6 +45,7 @@
 #include "at91_aic.h"
 #include "board.h"
 #include "generic.h"
+#include "gpio.h"
 
 
 static void __init ek_init_early(void)
diff --git a/arch/arm/mach-at91/board-rsi-ews.c b/arch/arm/mach-at91/board-rsi-ews.c
index f6d7f1958c7..f28e8b74df4 100644
--- a/arch/arm/mach-at91/board-rsi-ews.c
+++ b/arch/arm/mach-at91/board-rsi-ews.c
@@ -31,6 +31,7 @@
 #include "at91_aic.h"
 #include "board.h"
 #include "generic.h"
+#include "gpio.h"
 
 static void __init rsi_ews_init_early(void)
 {
diff --git a/arch/arm/mach-at91/board-sam9-l9260.c b/arch/arm/mach-at91/board-sam9-l9260.c
index 43ee4dc43b5..d24dda67e2d 100644
--- a/arch/arm/mach-at91/board-sam9-l9260.c
+++ b/arch/arm/mach-at91/board-sam9-l9260.c
@@ -43,6 +43,7 @@
 #include "board.h"
 #include "sam9_smc.h"
 #include "generic.h"
+#include "gpio.h"
 
 
 static void __init ek_init_early(void)
diff --git a/arch/arm/mach-at91/board-sam9260ek.c b/arch/arm/mach-at91/board-sam9260ek.c
index f4f8735315d..65dea12d685 100644
--- a/arch/arm/mach-at91/board-sam9260ek.c
+++ b/arch/arm/mach-at91/board-sam9260ek.c
@@ -49,6 +49,7 @@
 #include "board.h"
 #include "sam9_smc.h"
 #include "generic.h"
+#include "gpio.h"
 
 
 static void __init ek_init_early(void)
diff --git a/arch/arm/mach-at91/board-sam9261ek.c b/arch/arm/mach-at91/board-sam9261ek.c
index 473546b9408..4637432de08 100644
--- a/arch/arm/mach-at91/board-sam9261ek.c
+++ b/arch/arm/mach-at91/board-sam9261ek.c
@@ -53,6 +53,7 @@
 #include "board.h"
 #include "sam9_smc.h"
 #include "generic.h"
+#include "gpio.h"
 
 
 static void __init ek_init_early(void)
diff --git a/arch/arm/mach-at91/board-sam9263ek.c b/arch/arm/mach-at91/board-sam9263ek.c
index 2f931915c80..cd2726ee5ad 100644
--- a/arch/arm/mach-at91/board-sam9263ek.c
+++ b/arch/arm/mach-at91/board-sam9263ek.c
@@ -52,6 +52,7 @@
 #include "board.h"
 #include "sam9_smc.h"
 #include "generic.h"
+#include "gpio.h"
 
 
 static void __init ek_init_early(void)
diff --git a/arch/arm/mach-at91/board-sam9g20ek.c b/arch/arm/mach-at91/board-sam9g20ek.c
index f9cd1f2c714..e1be6e25b38 100644
--- a/arch/arm/mach-at91/board-sam9g20ek.c
+++ b/arch/arm/mach-at91/board-sam9g20ek.c
@@ -50,6 +50,7 @@
 #include "board.h"
 #include "sam9_smc.h"
 #include "generic.h"
+#include "gpio.h"
 
 /*
  * board revision encoding
diff --git a/arch/arm/mach-at91/board-sam9m10g45ek.c b/arch/arm/mach-at91/board-sam9m10g45ek.c
index ef39078c8ce..1ea61328f30 100644
--- a/arch/arm/mach-at91/board-sam9m10g45ek.c
+++ b/arch/arm/mach-at91/board-sam9m10g45ek.c
@@ -50,6 +50,7 @@
 #include "board.h"
 #include "sam9_smc.h"
 #include "generic.h"
+#include "gpio.h"
 
 
 static void __init ek_init_early(void)
@@ -300,21 +301,13 @@ static struct atmel_lcdfb_pdata __initdata ek_lcdc_data;
 
 
 /*
- * Touchscreen
- */
-static struct at91_tsadcc_data ek_tsadcc_data = {
-	.adc_clock		= 300000,
-	.pendet_debounce	= 0x0d,
-	.ts_sample_hold_time	= 0x0a,
-};
-
-/*
- * ADCs
+ * ADCs and touchscreen
  */
 static struct at91_adc_data ek_adc_data = {
 	.channels_used = BIT(0) | BIT(1) | BIT(2) | BIT(3) | BIT(4) | BIT(5) | BIT(6) | BIT(7),
 	.use_external_triggers = true,
 	.vref = 3300,
+	.touchscreen_type = ATMEL_ADC_TOUCHSCREEN_4WIRE,
 };
 
 /*
@@ -485,9 +478,7 @@ static void __init ek_board_init(void)
 	at91_add_device_isi(&isi_data, true);
 	/* LCD Controller */
 	at91_add_device_lcdc(&ek_lcdc_data);
-	/* Touch Screen */
-	at91_add_device_tsadcc(&ek_tsadcc_data);
-	/* ADC */
+	/* ADC and touchscreen */
 	at91_add_device_adc(&ek_adc_data);
 	/* Push Buttons */
 	ek_add_device_buttons();
diff --git a/arch/arm/mach-at91/board-sam9rlek.c b/arch/arm/mach-at91/board-sam9rlek.c
index 604eecf6cd7..b64648b4a1f 100644
--- a/arch/arm/mach-at91/board-sam9rlek.c
+++ b/arch/arm/mach-at91/board-sam9rlek.c
@@ -18,6 +18,7 @@
 #include <linux/clk.h>
 #include <linux/input.h>
 #include <linux/gpio_keys.h>
+#include <linux/platform_data/at91_adc.h>
 
 #include <video/atmel_lcdc.h>
 
@@ -38,6 +39,7 @@
 #include "board.h"
 #include "sam9_smc.h"
 #include "generic.h"
+#include "gpio.h"
 
 
 static void __init ek_init_early(void)
@@ -229,12 +231,13 @@ static struct gpio_led ek_leds[] = {
 
 
 /*
- * Touchscreen
+ * ADC + Touchscreen
  */
-static struct at91_tsadcc_data ek_tsadcc_data = {
-	.adc_clock		= 1000000,
-	.pendet_debounce	= 0x0f,
-	.ts_sample_hold_time	= 0x03,
+static struct at91_adc_data ek_adc_data = {
+	.channels_used = BIT(0) | BIT(1) | BIT(2) | BIT(3) | BIT(4) | BIT(5),
+	.use_external_triggers = true,
+	.vref = 3300,
+	.touchscreen_type = ATMEL_ADC_TOUCHSCREEN_4WIRE,
 };
 
 
@@ -310,8 +313,8 @@ static void __init ek_board_init(void)
 	at91_add_device_lcdc(&ek_lcdc_data);
 	/* AC97 */
 	at91_add_device_ac97(&ek_ac97_data);
-	/* Touch Screen Controller */
-	at91_add_device_tsadcc(&ek_tsadcc_data);
+	/* Touch Screen Controller + ADC */
+	at91_add_device_adc(&ek_adc_data);
 	/* LEDs */
 	at91_gpio_leds(ek_leds, ARRAY_SIZE(ek_leds));
 	/* Push Buttons */
diff --git a/arch/arm/mach-at91/board-snapper9260.c b/arch/arm/mach-at91/board-snapper9260.c
index f1d49e929cc..1b870e6def0 100644
--- a/arch/arm/mach-at91/board-snapper9260.c
+++ b/arch/arm/mach-at91/board-snapper9260.c
@@ -38,6 +38,7 @@
 #include "board.h"
 #include "sam9_smc.h"
 #include "generic.h"
+#include "gpio.h"
 
 #define SNAPPER9260_IO_EXP_GPIO(x)	(NR_BUILTIN_GPIO + (x))
 
diff --git a/arch/arm/mach-at91/board-stamp9g20.c b/arch/arm/mach-at91/board-stamp9g20.c
index e4a5ac17cdb..3b575036ff9 100644
--- a/arch/arm/mach-at91/board-stamp9g20.c
+++ b/arch/arm/mach-at91/board-stamp9g20.c
@@ -32,6 +32,7 @@
 #include "board.h"
 #include "sam9_smc.h"
 #include "generic.h"
+#include "gpio.h"
 
 
 void __init stamp9g20_init_early(void)
diff --git a/arch/arm/mach-at91/board-yl-9200.c b/arch/arm/mach-at91/board-yl-9200.c
index be083771df2..46fdb0c68a6 100644
--- a/arch/arm/mach-at91/board-yl-9200.c
+++ b/arch/arm/mach-at91/board-yl-9200.c
@@ -50,6 +50,7 @@
 #include "at91_aic.h"
 #include "board.h"
 #include "generic.h"
+#include "gpio.h"
 
 
 static void __init yl9200_init_early(void)
diff --git a/arch/arm/mach-at91/board.h b/arch/arm/mach-at91/board.h
index 6c08b341167..4e773b55bc2 100644
--- a/arch/arm/mach-at91/board.h
+++ b/arch/arm/mach-at91/board.h
@@ -118,9 +118,6 @@ struct isi_platform_data;
 extern void __init at91_add_device_isi(struct isi_platform_data *data,
 		bool use_pck_as_mck);
 
- /* Touchscreen Controller */
-extern void __init at91_add_device_tsadcc(struct at91_tsadcc_data *data);
-
 /* CAN */
 extern void __init at91_add_device_can(struct at91_can_data *data);
 
diff --git a/arch/arm/mach-at91/gpio.c b/arch/arm/mach-at91/gpio.c
index a5afcf76550..d3f05aaad8b 100644
--- a/arch/arm/mach-at91/gpio.c
+++ b/arch/arm/mach-at91/gpio.c
@@ -29,6 +29,7 @@
 #include <mach/at91_pio.h>
 
 #include "generic.h"
+#include "gpio.h"
 
 #define MAX_NB_GPIO_PER_BANK	32
 
@@ -49,6 +50,7 @@ static int at91_gpiolib_request(struct gpio_chip *chip, unsigned offset);
 static void at91_gpiolib_dbg_show(struct seq_file *s, struct gpio_chip *chip);
 static void at91_gpiolib_set(struct gpio_chip *chip, unsigned offset, int val);
 static int at91_gpiolib_get(struct gpio_chip *chip, unsigned offset);
+static int at91_gpiolib_get_direction(struct gpio_chip *chip, unsigned offset);
 static int at91_gpiolib_direction_output(struct gpio_chip *chip,
 					 unsigned offset, int val);
 static int at91_gpiolib_direction_input(struct gpio_chip *chip,
@@ -60,6 +62,7 @@ static int at91_gpiolib_to_irq(struct gpio_chip *chip, unsigned offset);
 		.chip = {						\
 			.label		  = name,			\
 			.request	  = at91_gpiolib_request,	\
+			.get_direction    = at91_gpiolib_get_direction, \
 			.direction_input  = at91_gpiolib_direction_input, \
 			.direction_output = at91_gpiolib_direction_output, \
 			.get		  = at91_gpiolib_get,		\
@@ -799,6 +802,17 @@ static int at91_gpiolib_request(struct gpio_chip *chip, unsigned offset)
 	return 0;
 }
 
+static int at91_gpiolib_get_direction(struct gpio_chip *chip, unsigned offset)
+{
+	struct at91_gpio_chip *at91_gpio = to_at91_gpio_chip(chip);
+	void __iomem *pio = at91_gpio->regbase;
+	unsigned mask = 1 << offset;
+	u32 osr;
+
+	osr = __raw_readl(pio + PIO_OSR);
+	return !(osr & mask);
+}
+
 static int at91_gpiolib_direction_input(struct gpio_chip *chip,
 					unsigned offset)
 {
diff --git a/arch/arm/mach-at91/include/mach/gpio.h b/arch/arm/mach-at91/gpio.h
index 5fc23771c15..eed465ab0dd 100644
--- a/arch/arm/mach-at91/include/mach/gpio.h
+++ b/arch/arm/mach-at91/gpio.h
@@ -209,14 +209,6 @@ extern int at91_get_gpio_value(unsigned pin);
 extern void at91_gpio_suspend(void);
 extern void at91_gpio_resume(void);
 
-#ifdef CONFIG_PINCTRL_AT91
-extern void at91_pinctrl_gpio_suspend(void);
-extern void at91_pinctrl_gpio_resume(void);
-#else
-static inline void at91_pinctrl_gpio_suspend(void) {}
-static inline void at91_pinctrl_gpio_resume(void) {}
-#endif
-
 #endif	/* __ASSEMBLY__ */
 
 #endif
diff --git a/arch/arm/mach-at91/include/mach/at91_adc.h b/arch/arm/mach-at91/include/mach/at91_adc.h
deleted file mode 100644
index c287307b9a3..00000000000
--- a/arch/arm/mach-at91/include/mach/at91_adc.h
+++ /dev/null
@@ -1,107 +0,0 @@
-/*
- * arch/arm/mach-at91/include/mach/at91_adc.h
- *
- * Copyright (C) SAN People
- *
- * Analog-to-Digital Converter (ADC) registers.
- * Based on AT91SAM9260 datasheet revision D.
- *
- * 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 AT91_ADC_H
-#define AT91_ADC_H
-
-#define AT91_ADC_CR		0x00		/* Control Register */
-#define		AT91_ADC_SWRST		(1 << 0)	/* Software Reset */
-#define		AT91_ADC_START		(1 << 1)	/* Start Conversion */
-
-#define AT91_ADC_MR		0x04		/* Mode Register */
-#define		AT91_ADC_TRGEN		(1 << 0)	/* Trigger Enable */
-#define		AT91_ADC_TRGSEL		(7 << 1)	/* Trigger Selection */
-#define			AT91_ADC_TRGSEL_TC0		(0 << 1)
-#define			AT91_ADC_TRGSEL_TC1		(1 << 1)
-#define			AT91_ADC_TRGSEL_TC2		(2 << 1)
-#define			AT91_ADC_TRGSEL_EXTERNAL	(6 << 1)
-#define		AT91_ADC_LOWRES		(1 << 4)	/* Low Resolution */
-#define		AT91_ADC_SLEEP		(1 << 5)	/* Sleep Mode */
-#define		AT91_ADC_PRESCAL_9260	(0x3f << 8)	/* Prescalar Rate Selection */
-#define		AT91_ADC_PRESCAL_9G45	(0xff << 8)
-#define			AT91_ADC_PRESCAL_(x)	((x) << 8)
-#define		AT91_ADC_STARTUP_9260	(0x1f << 16)	/* Startup Up Time */
-#define		AT91_ADC_STARTUP_9G45	(0x7f << 16)
-#define		AT91_ADC_STARTUP_9X5	(0xf << 16)
-#define			AT91_ADC_STARTUP_(x)	((x) << 16)
-#define		AT91_ADC_SHTIM		(0xf  << 24)	/* Sample & Hold Time */
-#define			AT91_ADC_SHTIM_(x)	((x) << 24)
-
-#define AT91_ADC_CHER		0x10		/* Channel Enable Register */
-#define AT91_ADC_CHDR		0x14		/* Channel Disable Register */
-#define AT91_ADC_CHSR		0x18		/* Channel Status Register */
-#define		AT91_ADC_CH(n)		(1 << (n))	/* Channel Number */
-
-#define AT91_ADC_SR		0x1C		/* Status Register */
-#define		AT91_ADC_EOC(n)		(1 << (n))	/* End of Conversion on Channel N */
-#define		AT91_ADC_OVRE(n)	(1 << ((n) + 8))/* Overrun Error on Channel N */
-#define		AT91_ADC_DRDY		(1 << 16)	/* Data Ready */
-#define		AT91_ADC_GOVRE		(1 << 17)	/* General Overrun Error */
-#define		AT91_ADC_ENDRX		(1 << 18)	/* End of RX Buffer */
-#define		AT91_ADC_RXFUFF		(1 << 19)	/* RX Buffer Full */
-
-#define AT91_ADC_SR_9X5		0x30		/* Status Register for 9x5 */
-#define		AT91_ADC_SR_DRDY_9X5	(1 << 24)	/* Data Ready */
-
-#define AT91_ADC_LCDR		0x20		/* Last Converted Data Register */
-#define		AT91_ADC_LDATA		(0x3ff)
-
-#define AT91_ADC_IER		0x24		/* Interrupt Enable Register */
-#define AT91_ADC_IDR		0x28		/* Interrupt Disable Register */
-#define AT91_ADC_IMR		0x2C		/* Interrupt Mask Register */
-#define		AT91_ADC_IER_PEN	(1 << 29)
-#define		AT91_ADC_IER_NOPEN	(1 << 30)
-#define		AT91_ADC_IER_XRDY	(1 << 20)
-#define		AT91_ADC_IER_YRDY	(1 << 21)
-#define		AT91_ADC_IER_PRDY	(1 << 22)
-#define		AT91_ADC_ISR_PENS	(1 << 31)
-
-#define AT91_ADC_CHR(n)		(0x30 + ((n) * 4))	/* Channel Data Register N */
-#define		AT91_ADC_DATA		(0x3ff)
-
-#define AT91_ADC_CDR0_9X5	(0x50)			/* Channel Data Register 0 for 9X5 */
-
-#define AT91_ADC_ACR		0x94	/* Analog Control Register */
-#define		AT91_ADC_ACR_PENDETSENS	(0x3 << 0)	/* pull-up resistor */
-
-#define AT91_ADC_TSMR		0xB0
-#define		AT91_ADC_TSMR_TSMODE	(3 << 0)	/* Touch Screen Mode */
-#define			AT91_ADC_TSMR_TSMODE_NONE		(0 << 0)
-#define			AT91_ADC_TSMR_TSMODE_4WIRE_NO_PRESS	(1 << 0)
-#define			AT91_ADC_TSMR_TSMODE_4WIRE_PRESS	(2 << 0)
-#define			AT91_ADC_TSMR_TSMODE_5WIRE		(3 << 0)
-#define		AT91_ADC_TSMR_TSAV	(3 << 4)	/* Averages samples */
-#define			AT91_ADC_TSMR_TSAV_(x)		((x) << 4)
-#define		AT91_ADC_TSMR_SCTIM	(0x0f << 16)	/* Switch closure time */
-#define		AT91_ADC_TSMR_PENDBC	(0x0f << 28)	/* Pen Debounce time */
-#define			AT91_ADC_TSMR_PENDBC_(x)	((x) << 28)
-#define		AT91_ADC_TSMR_NOTSDMA	(1 << 22)	/* No Touchscreen DMA */
-#define		AT91_ADC_TSMR_PENDET_DIS	(0 << 24)	/* Pen contact detection disable */
-#define		AT91_ADC_TSMR_PENDET_ENA	(1 << 24)	/* Pen contact detection enable */
-
-#define AT91_ADC_TSXPOSR	0xB4
-#define AT91_ADC_TSYPOSR	0xB8
-#define AT91_ADC_TSPRESSR	0xBC
-
-#define AT91_ADC_TRGR_9260	AT91_ADC_MR
-#define AT91_ADC_TRGR_9G45	0x08
-#define AT91_ADC_TRGR_9X5	0xC0
-
-/* Trigger Register bit field */
-#define		AT91_ADC_TRGR_TRGPER	(0xffff << 16)
-#define			AT91_ADC_TRGR_TRGPER_(x)	((x) << 16)
-#define		AT91_ADC_TRGR_TRGMOD	(0x7 << 0)
-#define			AT91_ADC_TRGR_MOD_PERIOD_TRIG	(5 << 0)
-
-#endif
diff --git a/arch/arm/mach-at91/include/mach/hardware.h b/arch/arm/mach-at91/include/mach/hardware.h
index f17aa315001..56338245653 100644
--- a/arch/arm/mach-at91/include/mach/hardware.h
+++ b/arch/arm/mach-at91/include/mach/hardware.h
@@ -104,5 +104,20 @@
 /* Clocks */
 #define AT91_SLOW_CLOCK		32768		/* slow clock */
 
+/*
+ * FIXME: this is needed to communicate between the pinctrl driver and
+ * the PM implementation in the machine. Possibly part of the PM
+ * implementation should be moved down into the pinctrl driver and get
+ * called as part of the generic suspend/resume path.
+ */
+#ifndef __ASSEMBLY__
+#ifdef CONFIG_PINCTRL_AT91
+extern void at91_pinctrl_gpio_suspend(void);
+extern void at91_pinctrl_gpio_resume(void);
+#else
+static inline void at91_pinctrl_gpio_suspend(void) {}
+static inline void at91_pinctrl_gpio_resume(void) {}
+#endif
+#endif
 
 #endif
diff --git a/arch/arm/mach-at91/leds.c b/arch/arm/mach-at91/leds.c
index 3e22978b554..77c4d8fd03f 100644
--- a/arch/arm/mach-at91/leds.c
+++ b/arch/arm/mach-at91/leds.c
@@ -16,6 +16,7 @@
 #include <linux/platform_device.h>
 
 #include "board.h"
+#include "gpio.h"
 
 
 /* ------------------------------------------------------------------------- */
diff --git a/arch/arm/mach-at91/pm.c b/arch/arm/mach-at91/pm.c
index 8bda1cefdf9..e9555453298 100644
--- a/arch/arm/mach-at91/pm.c
+++ b/arch/arm/mach-at91/pm.c
@@ -32,6 +32,7 @@
 #include "at91_aic.h"
 #include "generic.h"
 #include "pm.h"
+#include "gpio.h"
 
 /*
  * Show the reason for the previous system reset.
diff --git a/arch/arm/mach-axxia/Kconfig b/arch/arm/mach-axxia/Kconfig
new file mode 100644
index 00000000000..8be7e0ae192
--- /dev/null
+++ b/arch/arm/mach-axxia/Kconfig
@@ -0,0 +1,16 @@
+config ARCH_AXXIA
+	bool "LSI Axxia platforms" if (ARCH_MULTI_V7 && ARM_LPAE)
+	select ARCH_DMA_ADDR_T_64BIT
+	select ARM_AMBA
+	select ARM_GIC
+	select ARM_TIMER_SP804
+	select HAVE_ARM_ARCH_TIMER
+	select MFD_SYSCON
+	select MIGHT_HAVE_PCI
+	select PCI_DOMAINS if PCI
+	select ZONE_DMA
+	help
+	  This enables support for the LSI Axxia devices.
+
+	  The LSI Axxia platforms require a Flattened Device Tree to be passed
+	  to the kernel.
diff --git a/arch/arm/mach-axxia/Makefile b/arch/arm/mach-axxia/Makefile
new file mode 100644
index 00000000000..ec4f68b460c
--- /dev/null
+++ b/arch/arm/mach-axxia/Makefile
@@ -0,0 +1,2 @@
+obj-y				+= axxia.o
+obj-$(CONFIG_SMP)		+= platsmp.o
diff --git a/arch/arm/mach-axxia/axxia.c b/arch/arm/mach-axxia/axxia.c
new file mode 100644
index 00000000000..19e5a1d9539
--- /dev/null
+++ b/arch/arm/mach-axxia/axxia.c
@@ -0,0 +1,28 @@
+/*
+ * Support for the LSI Axxia SoC devices based on ARM cores.
+ *
+ * Copyright (C) 2012 LSI
+ *
+ * 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/init.h>
+#include <asm/mach/arch.h>
+
+static const char *axxia_dt_match[] __initconst = {
+	"lsi,axm5516",
+	"lsi,axm5516-sim",
+	"lsi,axm5516-emu",
+	NULL
+};
+
+DT_MACHINE_START(AXXIA_DT, "LSI Axxia AXM55XX")
+	.dt_compat = axxia_dt_match,
+MACHINE_END
diff --git a/arch/arm/mach-axxia/platsmp.c b/arch/arm/mach-axxia/platsmp.c
new file mode 100644
index 00000000000..959d4df3d2b
--- /dev/null
+++ b/arch/arm/mach-axxia/platsmp.c
@@ -0,0 +1,89 @@
+/*
+ * linux/arch/arm/mach-axxia/platsmp.c
+ *
+ * Copyright (C) 2012 LSI Corporation
+ *
+ * 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/init.h>
+#include <linux/io.h>
+#include <linux/smp.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <asm/cacheflush.h>
+
+/* Syscon register offsets for releasing cores from reset */
+#define SC_CRIT_WRITE_KEY	0x1000
+#define SC_RST_CPU_HOLD		0x1010
+
+/*
+ * Write the kernel entry point for secondary CPUs to the specified address
+ */
+static void write_release_addr(u32 release_phys)
+{
+	u32 *virt = (u32 *) phys_to_virt(release_phys);
+	writel_relaxed(virt_to_phys(secondary_startup), virt);
+	/* Make sure this store is visible to other CPUs */
+	smp_wmb();
+	__cpuc_flush_dcache_area(virt, sizeof(u32));
+}
+
+static int axxia_boot_secondary(unsigned int cpu, struct task_struct *idle)
+{
+	struct device_node *syscon_np;
+	void __iomem *syscon;
+	u32 tmp;
+
+	syscon_np = of_find_compatible_node(NULL, NULL, "lsi,axxia-syscon");
+	if (!syscon_np)
+		return -ENOENT;
+
+	syscon = of_iomap(syscon_np, 0);
+	if (!syscon)
+		return -ENOMEM;
+
+	tmp = readl(syscon + SC_RST_CPU_HOLD);
+	writel(0xab, syscon + SC_CRIT_WRITE_KEY);
+	tmp &= ~(1 << cpu);
+	writel(tmp, syscon + SC_RST_CPU_HOLD);
+
+	return 0;
+}
+
+static void __init axxia_smp_prepare_cpus(unsigned int max_cpus)
+{
+	int cpu_count = 0;
+	int cpu;
+
+	/*
+	 * Initialise the present map, which describes the set of CPUs actually
+	 * populated at the present time.
+	 */
+	for_each_possible_cpu(cpu) {
+		struct device_node *np;
+		u32 release_phys;
+
+		np = of_get_cpu_node(cpu, NULL);
+		if (!np)
+			continue;
+		if (of_property_read_u32(np, "cpu-release-addr", &release_phys))
+			continue;
+
+		if (cpu_count < max_cpus) {
+			set_cpu_present(cpu, true);
+			cpu_count++;
+		}
+
+		if (release_phys != 0)
+			write_release_addr(release_phys);
+	}
+}
+
+static struct smp_operations axxia_smp_ops __initdata = {
+	.smp_prepare_cpus	= axxia_smp_prepare_cpus,
+	.smp_boot_secondary	= axxia_boot_secondary,
+};
+CPU_METHOD_OF_DECLARE(axxia_smp, "lsi,syscon-release", &axxia_smp_ops);
diff --git a/arch/arm/mach-bcm/Kconfig b/arch/arm/mach-bcm/Kconfig
index 49c914cd9c7..9bc6db1c134 100644
--- a/arch/arm/mach-bcm/Kconfig
+++ b/arch/arm/mach-bcm/Kconfig
@@ -1,31 +1,58 @@
 config ARCH_BCM
-	bool "Broadcom SoC Support"
-	depends on ARCH_MULTIPLATFORM
+	bool "Broadcom SoC Support" if ARCH_MULTI_V6_V7
 	help
-	  This enables support for Broadcom ARM based SoC
-          chips
-
-if ARCH_BCM
+	  This enables support for Broadcom ARM based SoC chips
 
 menu "Broadcom SoC Selection"
+	depends on ARCH_BCM
 
 config ARCH_BCM_MOBILE
-	bool "Broadcom Mobile SoC" if ARCH_MULTI_V7
-	depends on MMU
+	bool "Broadcom Mobile SoC Support" if ARCH_MULTI_V7
 	select ARCH_REQUIRE_GPIOLIB
 	select ARM_ERRATA_754322
 	select ARM_ERRATA_764369 if SMP
+	select ARM_ERRATA_775420
 	select ARM_GIC
 	select GPIO_BCM_KONA
 	select TICK_ONESHOT
-	select CACHE_L2X0
 	select HAVE_ARM_ARCH_TIMER
 	select PINCTRL
 	help
 	  This enables support for systems based on Broadcom mobile SoCs.
-	  It currently supports the 'BCM281XX' family, which includes
-	  BCM11130, BCM11140, BCM11351, BCM28145 and
-	  BCM28155 variants.
+
+if ARCH_BCM_MOBILE
+
+menu "Broadcom Mobile SoC Selection"
+
+config ARCH_BCM_281XX
+	bool "Broadcom BCM281XX SoC family"
+	default y
+	help
+	  Enable support for the the BCM281XX family, which includes
+	  BCM11130, BCM11140, BCM11351, BCM28145 and BCM28155
+	  variants.
+
+config ARCH_BCM_21664
+	bool "Broadcom BCM21664 SoC family"
+	default y
+	help
+	  Enable support for the the BCM21664 family, which includes
+	  BCM21663 and BCM21664 variants.
+
+config ARCH_BCM_MOBILE_L2_CACHE
+	bool "Broadcom mobile SoC level 2 cache support"
+	depends on (ARCH_BCM_281XX || ARCH_BCM_21664)
+	default y
+	select CACHE_L2X0
+	select ARCH_BCM_MOBILE_SMC
+
+config ARCH_BCM_MOBILE_SMC
+	bool
+	depends on ARCH_BCM_281XX || ARCH_BCM_21664
+
+endmenu
+
+endif
 
 config ARCH_BCM2835
 	bool "Broadcom BCM2835 family" if ARCH_MULTI_V6
@@ -33,10 +60,7 @@ config ARCH_BCM2835
 	select ARM_AMBA
 	select ARM_ERRATA_411920
 	select ARM_TIMER_SP804
-	select CLKDEV_LOOKUP
 	select CLKSRC_OF
-	select CPU_V6
-	select GENERIC_CLOCKEVENTS
 	select PINCTRL
 	select PINCTRL_BCM2835
 	help
@@ -45,17 +69,12 @@ config ARCH_BCM2835
 
 config ARCH_BCM_5301X
 	bool "Broadcom BCM470X / BCM5301X ARM SoC" if ARCH_MULTI_V7
-	depends on MMU
 	select ARM_GIC
 	select CACHE_L2X0
 	select HAVE_ARM_SCU if SMP
 	select HAVE_ARM_TWD if SMP
-	select HAVE_SMP
-	select COMMON_CLK
-	select GENERIC_CLOCKEVENTS
 	select ARM_GLOBAL_TIMER
 	select CLKSRC_ARM_GLOBAL_TIMER_SCHED_CLOCK
-	select MIGHT_HAVE_PCI
 	help
 	  Support for Broadcom BCM470X and BCM5301X SoCs with ARM CPU cores.
 
@@ -70,5 +89,3 @@ config ARCH_BCM_5301X
 	  network SoC using a MIPS CPU, they are supported by arch/mips/bcm47xx
 
 endmenu
-
-endif
diff --git a/arch/arm/mach-bcm/Makefile b/arch/arm/mach-bcm/Makefile
index a326b28c440..73129211497 100644
--- a/arch/arm/mach-bcm/Makefile
+++ b/arch/arm/mach-bcm/Makefile
@@ -10,10 +10,23 @@
 # of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 # GNU General Public License for more details.
 
-obj-$(CONFIG_ARCH_BCM_MOBILE)	:= board_bcm281xx.o board_bcm21664.o \
-				bcm_kona_smc.o bcm_kona_smc_asm.o kona.o
+# BCM281XX
+obj-$(CONFIG_ARCH_BCM_281XX)	+= board_bcm281xx.o
+
+# BCM21664
+obj-$(CONFIG_ARCH_BCM_21664)	+= board_bcm21664.o
+
+# BCM281XX and BCM21664 L2 cache control
+obj-$(CONFIG_ARCH_BCM_MOBILE_L2_CACHE) += kona_l2_cache.o
+
+# Support for secure monitor traps
+obj-$(CONFIG_ARCH_BCM_MOBILE_SMC) += bcm_kona_smc.o
+ifeq ($(call as-instr,.arch_extension sec,as_has_sec),as_has_sec)
+CFLAGS_bcm_kona_smc.o		+= -Wa,-march=armv7-a+sec -DREQUIRES_SEC
+endif
+
+# BCM2835
 obj-$(CONFIG_ARCH_BCM2835)	+= board_bcm2835.o
 
-plus_sec := $(call as-instr,.arch_extension sec,+sec)
-AFLAGS_bcm_kona_smc_asm.o	:=-Wa,-march=armv7-a$(plus_sec)
+# BCM5301X
 obj-$(CONFIG_ARCH_BCM_5301X)	+= bcm_5301x.o
diff --git a/arch/arm/mach-bcm/bcm_kona_smc.c b/arch/arm/mach-bcm/bcm_kona_smc.c
index 5e31e918f32..a55a7ecf146 100644
--- a/arch/arm/mach-bcm/bcm_kona_smc.c
+++ b/arch/arm/mach-bcm/bcm_kona_smc.c
@@ -21,11 +21,8 @@
 
 #include "bcm_kona_smc.h"
 
-struct secure_bridge_data {
-	void __iomem *bounce;		/* virtual address */
-	u32 __iomem buffer_addr;	/* physical address */
-	int initialized;
-} bridge_data;
+static u32		bcm_smc_buffer_phys;	/* physical address */
+static void __iomem	*bcm_smc_buffer;	/* virtual address */
 
 struct bcm_kona_smc_data {
 	unsigned service_id;
@@ -33,6 +30,7 @@ struct bcm_kona_smc_data {
 	unsigned arg1;
 	unsigned arg2;
 	unsigned arg3;
+	unsigned result;
 };
 
 static const struct of_device_id bcm_kona_smc_ids[] __initconst = {
@@ -41,59 +39,125 @@ static const struct of_device_id bcm_kona_smc_ids[] __initconst = {
 	{},
 };
 
-/* Map in the bounce area */
+/* Map in the args buffer area */
 int __init bcm_kona_smc_init(void)
 {
 	struct device_node *node;
+	const __be32 *prop_val;
+	u64 prop_size = 0;
+	unsigned long buffer_size;
+	u32 buffer_phys;
 
 	/* Read buffer addr and size from the device tree node */
 	node = of_find_matching_node(NULL, bcm_kona_smc_ids);
 	if (!node)
 		return -ENODEV;
 
-	/* Don't care about size or flags of the DT node */
-	bridge_data.buffer_addr =
-		be32_to_cpu(*of_get_address(node, 0, NULL, NULL));
-	BUG_ON(!bridge_data.buffer_addr);
+	prop_val = of_get_address(node, 0, &prop_size, NULL);
+	if (!prop_val)
+		return -EINVAL;
 
-	bridge_data.bounce = of_iomap(node, 0);
-	BUG_ON(!bridge_data.bounce);
+	/* We assume space for four 32-bit arguments */
+	if (prop_size < 4 * sizeof(u32) || prop_size > (u64)ULONG_MAX)
+		return -EINVAL;
+	buffer_size = (unsigned long)prop_size;
 
-	bridge_data.initialized = 1;
+	buffer_phys = be32_to_cpup(prop_val);
+	if (!buffer_phys)
+		return -EINVAL;
+
+	bcm_smc_buffer = ioremap(buffer_phys, buffer_size);
+	if (!bcm_smc_buffer)
+		return -ENOMEM;
+	bcm_smc_buffer_phys = buffer_phys;
 
 	pr_info("Kona Secure API initialized\n");
 
 	return 0;
 }
 
+/*
+ * int bcm_kona_do_smc(u32 service_id, u32 buffer_addr)
+ *
+ * Only core 0 can run the secure monitor code.  If an "smc" request
+ * is initiated on a different core it must be redirected to core 0
+ * for execution.  We rely on the caller to handle this.
+ *
+ * Each "smc" request supplies a service id and the address of a
+ * buffer containing parameters related to the service to be
+ * performed.  A flags value defines the behavior of the level 2
+ * cache and interrupt handling while the secure monitor executes.
+ *
+ * Parameters to the "smc" request are passed in r4-r6 as follows:
+ *     r4	service id
+ *     r5	flags (SEC_ROM_*)
+ *     r6	physical address of buffer with other parameters
+ *
+ * Execution of an "smc" request produces two distinct results.
+ *
+ * First, the secure monitor call itself (regardless of the specific
+ * service request) can succeed, or can produce an error.  When an
+ * "smc" request completes this value is found in r12; it should
+ * always be SEC_EXIT_NORMAL.
+ *
+ * In addition, the particular service performed produces a result.
+ * The values that should be expected depend on the service.  We
+ * therefore return this value to the caller, so it can handle the
+ * request result appropriately.  This result value is found in r0
+ * when the "smc" request completes.
+ */
+static int bcm_kona_do_smc(u32 service_id, u32 buffer_phys)
+{
+	register u32 ip asm("ip");	/* Also called r12 */
+	register u32 r0 asm("r0");
+	register u32 r4 asm("r4");
+	register u32 r5 asm("r5");
+	register u32 r6 asm("r6");
+
+	r4 = service_id;
+	r5 = 0x3;		/* Keep IRQ and FIQ off in SM */
+	r6 = buffer_phys;
+
+	asm volatile (
+		/* Make sure we got the registers we want */
+		__asmeq("%0", "ip")
+		__asmeq("%1", "r0")
+		__asmeq("%2", "r4")
+		__asmeq("%3", "r5")
+		__asmeq("%4", "r6")
+#ifdef REQUIRES_SEC
+		".arch_extension sec\n"
+#endif
+		"	smc    #0\n"
+		: "=r" (ip), "=r" (r0)
+		: "r" (r4), "r" (r5), "r" (r6)
+		: "r1", "r2", "r3", "r7", "lr");
+
+	BUG_ON(ip != SEC_EXIT_NORMAL);
+
+	return r0;
+}
+
 /* __bcm_kona_smc() should only run on CPU 0, with pre-emption disabled */
 static void __bcm_kona_smc(void *info)
 {
 	struct bcm_kona_smc_data *data = info;
-	u32 *args = bridge_data.bounce;
-	int rc = 0;
+	u32 *args = bcm_smc_buffer;
 
-	/* Must run on CPU 0 */
 	BUG_ON(smp_processor_id() != 0);
+	BUG_ON(!args);
 
-	/* Check map in the bounce area */
-	BUG_ON(!bridge_data.initialized);
-
-	/* Copy one 32 bit word into the bounce area */
-	args[0] = data->arg0;
-	args[1] = data->arg1;
-	args[2] = data->arg2;
-	args[3] = data->arg3;
+	/* Copy the four 32 bit argument values into the bounce area */
+	writel_relaxed(data->arg0, args++);
+	writel_relaxed(data->arg1, args++);
+	writel_relaxed(data->arg2, args++);
+	writel(data->arg3, args);
 
 	/* Flush caches for input data passed to Secure Monitor */
-	if (data->service_id != SSAPI_BRCM_START_VC_CORE)
-		flush_cache_all();
-
-	/* Trap into Secure Monitor */
-	rc = bcm_kona_smc_asm(data->service_id, bridge_data.buffer_addr);
+	flush_cache_all();
 
-	if (rc != SEC_ROM_RET_OK)
-		pr_err("Secure Monitor call failed (0x%x)!\n", rc);
+	/* Trap into Secure Monitor and record the request result */
+	data->result = bcm_kona_do_smc(data->service_id, bcm_smc_buffer_phys);
 }
 
 unsigned bcm_kona_smc(unsigned service_id, unsigned arg0, unsigned arg1,
@@ -106,17 +170,13 @@ unsigned bcm_kona_smc(unsigned service_id, unsigned arg0, unsigned arg1,
 	data.arg1 = arg1;
 	data.arg2 = arg2;
 	data.arg3 = arg3;
+	data.result = 0;
 
 	/*
 	 * Due to a limitation of the secure monitor, we must use the SMP
 	 * infrastructure to forward all secure monitor calls to Core 0.
 	 */
-	if (get_cpu() != 0)
-		smp_call_function_single(0, __bcm_kona_smc, (void *)&data, 1);
-	else
-		__bcm_kona_smc(&data);
+	smp_call_function_single(0, __bcm_kona_smc, &data, 1);
 
-	put_cpu();
-
-	return 0;
+	return data.result;
 }
diff --git a/arch/arm/mach-bcm/bcm_kona_smc.h b/arch/arm/mach-bcm/bcm_kona_smc.h
index d098a7e7674..2e29ec67e41 100644
--- a/arch/arm/mach-bcm/bcm_kona_smc.h
+++ b/arch/arm/mach-bcm/bcm_kona_smc.h
@@ -15,55 +15,12 @@
 #define BCM_KONA_SMC_H
 
 #include <linux/types.h>
-#define FLAGS	(SEC_ROM_ICACHE_ENABLE_MASK | SEC_ROM_DCACHE_ENABLE_MASK | \
-			SEC_ROM_IRQ_ENABLE_MASK | SEC_ROM_FIQ_ENABLE_MASK)
 
-/*!
- * Definitions for IRQ & FIQ Mask for ARM
- */
-
-#define FIQ_IRQ_MASK						0xC0
-#define FIQ_MASK						0x40
-#define IRQ_MASK						0x80
-
-/*!
- * Secure Mode FLAGs
- */
-
-/* When set, enables ICache within the secure mode */
-#define SEC_ROM_ICACHE_ENABLE_MASK                        0x00000001
-
-/* When set, enables DCache within the secure mode */
-#define SEC_ROM_DCACHE_ENABLE_MASK                        0x00000002
-
-/* When set, enables IRQ within the secure mode */
-#define SEC_ROM_IRQ_ENABLE_MASK                           0x00000004
-
-/* When set, enables FIQ within the secure mode */
-#define SEC_ROM_FIQ_ENABLE_MASK                           0x00000008
-
-/* When set, enables Unified L2 cache within the secure mode */
-#define SEC_ROM_UL2_CACHE_ENABLE_MASK                     0x00000010
-
-/* Broadcom Secure Service API Service IDs */
-#define SSAPI_DORMANT_ENTRY_SERV                          0x01000000
-#define SSAPI_PUBLIC_OTP_SERV                             0x01000001
-#define SSAPI_ENABLE_L2_CACHE                             0x01000002
-#define SSAPI_DISABLE_L2_CACHE                            0x01000003
-#define SSAPI_WRITE_SCU_STATUS                            0x01000004
-#define SSAPI_WRITE_PWR_GATE                              0x01000005
-
-/* Broadcom Secure Service API Return Codes */
+/* Broadcom Secure Service API service IDs, return codes, and exit codes */
+#define SSAPI_ENABLE_L2_CACHE		0x01000002
 #define SEC_ROM_RET_OK			0x00000001
-#define SEC_ROM_RET_FAIL		0x00000009
-
-#define SSAPI_RET_FROM_INT_SERV		0x4
 #define SEC_EXIT_NORMAL			0x1
 
-#define SSAPI_ROW_AES			0x0E000006
-#define SSAPI_BRCM_START_VC_CORE	0x0E000008
-
-#ifndef	__ASSEMBLY__
 extern int __init bcm_kona_smc_init(void);
 
 extern unsigned bcm_kona_smc(unsigned service_id,
@@ -72,9 +29,4 @@ extern unsigned bcm_kona_smc(unsigned service_id,
 			     unsigned arg2,
 			     unsigned arg3);
 
-extern int bcm_kona_smc_asm(u32 service_id,
-			    u32 buffer_addr);
-
-#endif	/* __ASSEMBLY__ */
-
 #endif /* BCM_KONA_SMC_H */
diff --git a/arch/arm/mach-bcm/bcm_kona_smc_asm.S b/arch/arm/mach-bcm/bcm_kona_smc_asm.S
deleted file mode 100644
index a1608480d60..00000000000
--- a/arch/arm/mach-bcm/bcm_kona_smc_asm.S
+++ /dev/null
@@ -1,41 +0,0 @@
-/*
- * Copyright (C) 2013 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/linkage.h>
-#include "bcm_kona_smc.h"
-
-/*
- * int bcm_kona_smc_asm(u32 service_id, u32 buffer_addr)
- */
-
-ENTRY(bcm_kona_smc_asm)
-	stmfd	sp!, {r4-r12, lr}
-	mov	r4, r0		@ service_id
-	mov	r5, #3		@ Keep IRQ and FIQ off in SM
-	/*
-	 * Since interrupts are disabled in the open mode, we must keep
-	 * interrupts disabled in secure mode by setting R5=0x3. If interrupts
-	 * are enabled in open mode, we can set R5=0x0 to allow interrupts in
-	 * secure mode.  If we did this, the secure monitor would return back
-	 * control to the open mode to handle the interrupt prior to completing
-	 * the secure service. If this happened, R12 would not be
-	 * SEC_EXIT_NORMAL and we would need to call SMC again after resetting
-	 * R5 (it gets clobbered by the secure monitor) and setting R4 to
-	 * SSAPI_RET_FROM_INT_SERV to indicate that we want the secure monitor
-	 * to finish up the previous uncompleted secure service.
-	 */
-	mov	r6, r1		@ buffer_addr
-	smc	#0
-	/* Check r12 for SEC_EXIT_NORMAL here if interrupts are enabled */
-	ldmfd	sp!, {r4-r12, pc}
-ENDPROC(bcm_kona_smc_asm)
diff --git a/arch/arm/mach-bcm/board_bcm21664.c b/arch/arm/mach-bcm/board_bcm21664.c
index acc1573fd00..f0521cc0640 100644
--- a/arch/arm/mach-bcm/board_bcm21664.c
+++ b/arch/arm/mach-bcm/board_bcm21664.c
@@ -11,14 +11,13 @@
  * GNU General Public License for more details.
  */
 
-#include <linux/clocksource.h>
 #include <linux/of_address.h>
 #include <linux/of_platform.h>
+#include <linux/io.h>
 
 #include <asm/mach/arch.h>
 
-#include "bcm_kona_smc.h"
-#include "kona.h"
+#include "kona_l2_cache.h"
 
 #define RSTMGR_DT_STRING		"brcm,bcm21664-resetmgr"
 
diff --git a/arch/arm/mach-bcm/board_bcm281xx.c b/arch/arm/mach-bcm/board_bcm281xx.c
index 6be54c10f8c..1ac59fc0cb1 100644
--- a/arch/arm/mach-bcm/board_bcm281xx.c
+++ b/arch/arm/mach-bcm/board_bcm281xx.c
@@ -17,7 +17,7 @@
 
 #include <asm/mach/arch.h>
 
-#include "kona.h"
+#include "kona_l2_cache.h"
 
 #define SECWDOG_OFFSET			0x00000000
 #define SECWDOG_RESERVED_MASK		0xe2000000
diff --git a/arch/arm/mach-bcm/kona.c b/arch/arm/mach-bcm/kona_l2_cache.c
index 768bc2837bf..b31970377c2 100644
--- a/arch/arm/mach-bcm/kona.c
+++ b/arch/arm/mach-bcm/kona_l2_cache.c
@@ -11,19 +11,18 @@
  * GNU General Public License for more details.
  */
 
-#include <linux/of_platform.h>
+
+#include <linux/init.h>
+#include <linux/printk.h>
 #include <asm/hardware/cache-l2x0.h>
 
 #include "bcm_kona_smc.h"
-#include "kona.h"
 
 void __init kona_l2_cache_init(void)
 {
+	unsigned int result;
 	int ret;
 
-	if (!IS_ENABLED(CONFIG_CACHE_L2X0))
-		return;
-
 	ret = bcm_kona_smc_init();
 	if (ret) {
 		pr_info("Secure API not available (%d). Skipping L2 init.\n",
@@ -31,7 +30,12 @@ void __init kona_l2_cache_init(void)
 		return;
 	}
 
-	bcm_kona_smc(SSAPI_ENABLE_L2_CACHE, 0, 0, 0, 0);
+	result = bcm_kona_smc(SSAPI_ENABLE_L2_CACHE, 0, 0, 0, 0);
+	if (result != SEC_ROM_RET_OK) {
+		pr_err("Secure Monitor call failed (%u)! Skipping L2 init.\n",
+			result);
+		return;
+	}
 
 	/*
 	 * The aux_val and aux_mask have no effect since L2 cache is already
diff --git a/arch/arm/mach-bcm/kona.h b/arch/arm/mach-bcm/kona_l2_cache.h
index 3a7a017c29c..46f84a95ab1 100644
--- a/arch/arm/mach-bcm/kona.h
+++ b/arch/arm/mach-bcm/kona_l2_cache.h
@@ -11,4 +11,8 @@
  * GNU General Public License for more details.
  */
 
-void __init kona_l2_cache_init(void);
+#ifdef CONFIG_ARCH_BCM_MOBILE_L2_CACHE
+void	kona_l2_cache_init(void);
+#else
+#define kona_l2_cache_init() ((void)0)
+#endif
diff --git a/arch/arm/mach-berlin/Kconfig b/arch/arm/mach-berlin/Kconfig
index b0cb0722acd..101e0f35673 100644
--- a/arch/arm/mach-berlin/Kconfig
+++ b/arch/arm/mach-berlin/Kconfig
@@ -1,9 +1,11 @@
 config ARCH_BERLIN
 	bool "Marvell Berlin SoCs" if ARCH_MULTI_V7
+	select ARCH_REQUIRE_GPIOLIB
 	select ARM_GIC
 	select GENERIC_IRQ_CHIP
 	select DW_APB_ICTL
 	select DW_APB_TIMER_OF
+	select PINCTRL
 
 if ARCH_BERLIN
 
@@ -14,11 +16,19 @@ config MACH_BERLIN_BG2
 	select CACHE_L2X0
 	select CPU_PJ4B
 	select HAVE_ARM_TWD if SMP
+	select PINCTRL_BERLIN_BG2
 
 config MACH_BERLIN_BG2CD
 	bool "Marvell Armada 1500-mini (BG2CD)"
 	select CACHE_L2X0
 	select HAVE_ARM_TWD if SMP
+	select PINCTRL_BERLIN_BG2CD
+
+config MACH_BERLIN_BG2Q
+	bool "Marvell Armada 1500 Pro (BG2-Q)"
+	select CACHE_L2X0
+	select HAVE_ARM_TWD if SMP
+	select PINCTRL_BERLIN_BG2Q
 
 endmenu
 
diff --git a/arch/arm/mach-cns3xxx/Kconfig b/arch/arm/mach-cns3xxx/Kconfig
index dce8decd5d4..66838f42037 100644
--- a/arch/arm/mach-cns3xxx/Kconfig
+++ b/arch/arm/mach-cns3xxx/Kconfig
@@ -1,7 +1,6 @@
 config ARCH_CNS3XXX
 	bool "Cavium Networks CNS3XXX family" if ARCH_MULTI_V6
 	select ARM_GIC
-	select MIGHT_HAVE_PCI
 	select PCI_DOMAINS if PCI
 	help
 	  Support for Cavium Networks CNS3XXX platform.
diff --git a/arch/arm/mach-davinci/board-dm355-evm.c b/arch/arm/mach-davinci/board-dm355-evm.c
index ecdc7d44fa7..06d63d5651f 100644
--- a/arch/arm/mach-davinci/board-dm355-evm.c
+++ b/arch/arm/mach-davinci/board-dm355-evm.c
@@ -350,11 +350,7 @@ static struct davinci_mmc_config dm355evm_mmc_config = {
  * you have proper Mini-B or Mini-A cables (or Mini-A adapters)
  * the ID pin won't need any help.
  */
-#ifdef CONFIG_USB_MUSB_PERIPHERAL
-#define USB_ID_VALUE	0	/* ID pulled high; *should* float */
-#else
 #define USB_ID_VALUE	1	/* ID pulled low */
-#endif
 
 static struct spi_eeprom at25640a = {
 	.byte_len	= SZ_64K / 8,
diff --git a/arch/arm/mach-davinci/board-dm355-leopard.c b/arch/arm/mach-davinci/board-dm355-leopard.c
index 43bacbf1531..680a7a2d910 100644
--- a/arch/arm/mach-davinci/board-dm355-leopard.c
+++ b/arch/arm/mach-davinci/board-dm355-leopard.c
@@ -208,11 +208,7 @@ static struct davinci_mmc_config dm355leopard_mmc_config = {
  * you have proper Mini-B or Mini-A cables (or Mini-A adapters)
  * the ID pin won't need any help.
  */
-#ifdef CONFIG_USB_MUSB_PERIPHERAL
-#define USB_ID_VALUE	0	/* ID pulled high; *should* float */
-#else
 #define USB_ID_VALUE	1	/* ID pulled low */
-#endif
 
 static struct spi_eeprom at25640a = {
 	.byte_len	= SZ_64K / 8,
diff --git a/arch/arm/mach-davinci/devices-da8xx.c b/arch/arm/mach-davinci/devices-da8xx.c
index 56ea41d5f84..b85b781b05f 100644
--- a/arch/arm/mach-davinci/devices-da8xx.c
+++ b/arch/arm/mach-davinci/devices-da8xx.c
@@ -134,13 +134,6 @@ struct platform_device da8xx_serial_device[] = {
 	}
 };
 
-static s8 da8xx_queue_tc_mapping[][2] = {
-	/* {event queue no, TC no} */
-	{0, 0},
-	{1, 1},
-	{-1, -1}
-};
-
 static s8 da8xx_queue_priority_mapping[][2] = {
 	/* {event queue no, Priority} */
 	{0, 3},
@@ -148,12 +141,6 @@ static s8 da8xx_queue_priority_mapping[][2] = {
 	{-1, -1}
 };
 
-static s8 da850_queue_tc_mapping[][2] = {
-	/* {event queue no, TC no} */
-	{0, 0},
-	{-1, -1}
-};
-
 static s8 da850_queue_priority_mapping[][2] = {
 	/* {event queue no, Priority} */
 	{0, 3},
@@ -161,12 +148,6 @@ static s8 da850_queue_priority_mapping[][2] = {
 };
 
 static struct edma_soc_info da830_edma_cc0_info = {
-	.n_channel		= 32,
-	.n_region		= 4,
-	.n_slot			= 128,
-	.n_tc			= 2,
-	.n_cc			= 1,
-	.queue_tc_mapping	= da8xx_queue_tc_mapping,
 	.queue_priority_mapping	= da8xx_queue_priority_mapping,
 	.default_queue		= EVENTQ_1,
 };
@@ -177,22 +158,10 @@ static struct edma_soc_info *da830_edma_info[EDMA_MAX_CC] = {
 
 static struct edma_soc_info da850_edma_cc_info[] = {
 	{
-		.n_channel		= 32,
-		.n_region		= 4,
-		.n_slot			= 128,
-		.n_tc			= 2,
-		.n_cc			= 1,
-		.queue_tc_mapping	= da8xx_queue_tc_mapping,
 		.queue_priority_mapping	= da8xx_queue_priority_mapping,
 		.default_queue		= EVENTQ_1,
 	},
 	{
-		.n_channel		= 32,
-		.n_region		= 4,
-		.n_slot			= 128,
-		.n_tc			= 1,
-		.n_cc			= 1,
-		.queue_tc_mapping	= da850_queue_tc_mapping,
 		.queue_priority_mapping	= da850_queue_priority_mapping,
 		.default_queue		= EVENTQ_0,
 	},
diff --git a/arch/arm/mach-davinci/dm355.c b/arch/arm/mach-davinci/dm355.c
index 07381d8cea6..2f3ed3a58d5 100644
--- a/arch/arm/mach-davinci/dm355.c
+++ b/arch/arm/mach-davinci/dm355.c
@@ -569,14 +569,6 @@ static u8 dm355_default_priorities[DAVINCI_N_AINTC_IRQ] = {
 /*----------------------------------------------------------------------*/
 
 static s8
-queue_tc_mapping[][2] = {
-	/* {event queue no, TC no} */
-	{0, 0},
-	{1, 1},
-	{-1, -1},
-};
-
-static s8
 queue_priority_mapping[][2] = {
 	/* {event queue no, Priority} */
 	{0, 3},
@@ -585,12 +577,6 @@ queue_priority_mapping[][2] = {
 };
 
 static struct edma_soc_info edma_cc0_info = {
-	.n_channel		= 64,
-	.n_region		= 4,
-	.n_slot			= 128,
-	.n_tc			= 2,
-	.n_cc			= 1,
-	.queue_tc_mapping	= queue_tc_mapping,
 	.queue_priority_mapping	= queue_priority_mapping,
 	.default_queue		= EVENTQ_1,
 };
diff --git a/arch/arm/mach-davinci/dm365.c b/arch/arm/mach-davinci/dm365.c
index 08a61b93833..0ae8114f5cc 100644
--- a/arch/arm/mach-davinci/dm365.c
+++ b/arch/arm/mach-davinci/dm365.c
@@ -853,16 +853,6 @@ static u8 dm365_default_priorities[DAVINCI_N_AINTC_IRQ] = {
 
 /* Four Transfer Controllers on DM365 */
 static s8
-dm365_queue_tc_mapping[][2] = {
-	/* {event queue no, TC no} */
-	{0, 0},
-	{1, 1},
-	{2, 2},
-	{3, 3},
-	{-1, -1},
-};
-
-static s8
 dm365_queue_priority_mapping[][2] = {
 	/* {event queue no, Priority} */
 	{0, 7},
@@ -873,12 +863,6 @@ dm365_queue_priority_mapping[][2] = {
 };
 
 static struct edma_soc_info edma_cc0_info = {
-	.n_channel		= 64,
-	.n_region		= 4,
-	.n_slot			= 256,
-	.n_tc			= 4,
-	.n_cc			= 1,
-	.queue_tc_mapping	= dm365_queue_tc_mapping,
 	.queue_priority_mapping	= dm365_queue_priority_mapping,
 	.default_queue		= EVENTQ_3,
 };
diff --git a/arch/arm/mach-davinci/dm644x.c b/arch/arm/mach-davinci/dm644x.c
index 5debffba4b2..dc52657909c 100644
--- a/arch/arm/mach-davinci/dm644x.c
+++ b/arch/arm/mach-davinci/dm644x.c
@@ -499,14 +499,6 @@ static u8 dm644x_default_priorities[DAVINCI_N_AINTC_IRQ] = {
 /*----------------------------------------------------------------------*/
 
 static s8
-queue_tc_mapping[][2] = {
-	/* {event queue no, TC no} */
-	{0, 0},
-	{1, 1},
-	{-1, -1},
-};
-
-static s8
 queue_priority_mapping[][2] = {
 	/* {event queue no, Priority} */
 	{0, 3},
@@ -515,12 +507,6 @@ queue_priority_mapping[][2] = {
 };
 
 static struct edma_soc_info edma_cc0_info = {
-	.n_channel		= 64,
-	.n_region		= 4,
-	.n_slot			= 128,
-	.n_tc			= 2,
-	.n_cc			= 1,
-	.queue_tc_mapping	= queue_tc_mapping,
 	.queue_priority_mapping	= queue_priority_mapping,
 	.default_queue		= EVENTQ_1,
 };
diff --git a/arch/arm/mach-davinci/dm646x.c b/arch/arm/mach-davinci/dm646x.c
index 332d00d24dc..6c3bbea7d77 100644
--- a/arch/arm/mach-davinci/dm646x.c
+++ b/arch/arm/mach-davinci/dm646x.c
@@ -533,16 +533,6 @@ static u8 dm646x_default_priorities[DAVINCI_N_AINTC_IRQ] = {
 
 /* Four Transfer Controllers on DM646x */
 static s8
-dm646x_queue_tc_mapping[][2] = {
-	/* {event queue no, TC no} */
-	{0, 0},
-	{1, 1},
-	{2, 2},
-	{3, 3},
-	{-1, -1},
-};
-
-static s8
 dm646x_queue_priority_mapping[][2] = {
 	/* {event queue no, Priority} */
 	{0, 4},
@@ -553,12 +543,6 @@ dm646x_queue_priority_mapping[][2] = {
 };
 
 static struct edma_soc_info edma_cc0_info = {
-	.n_channel		= 64,
-	.n_region		= 6,	/* 0-1, 4-7 */
-	.n_slot			= 512,
-	.n_tc			= 4,
-	.n_cc			= 1,
-	.queue_tc_mapping	= dm646x_queue_tc_mapping,
 	.queue_priority_mapping	= dm646x_queue_priority_mapping,
 	.default_queue		= EVENTQ_1,
 };
diff --git a/arch/arm/mach-dove/irq.c b/arch/arm/mach-dove/irq.c
index bc4344aa100..4a5a7aedcb7 100644
--- a/arch/arm/mach-dove/irq.c
+++ b/arch/arm/mach-dove/irq.c
@@ -108,6 +108,38 @@ static int __initdata gpio2_irqs[4] = {
 	0,
 };
 
+#ifdef CONFIG_MULTI_IRQ_HANDLER
+/*
+ * Compiling with both non-DT and DT support enabled, will
+ * break asm irq handler used by non-DT boards. Therefore,
+ * we provide a C-style irq handler even for non-DT boards,
+ * if MULTI_IRQ_HANDLER is set.
+ */
+
+static void __iomem *dove_irq_base = IRQ_VIRT_BASE;
+
+static asmlinkage void
+__exception_irq_entry dove_legacy_handle_irq(struct pt_regs *regs)
+{
+	u32 stat;
+
+	stat = readl_relaxed(dove_irq_base + IRQ_CAUSE_LOW_OFF);
+	stat &= readl_relaxed(dove_irq_base + IRQ_MASK_LOW_OFF);
+	if (stat) {
+		unsigned int hwirq = __fls(stat);
+		handle_IRQ(hwirq, regs);
+		return;
+	}
+	stat = readl_relaxed(dove_irq_base + IRQ_CAUSE_HIGH_OFF);
+	stat &= readl_relaxed(dove_irq_base + IRQ_MASK_HIGH_OFF);
+	if (stat) {
+		unsigned int hwirq = 32 + __fls(stat);
+		handle_IRQ(hwirq, regs);
+		return;
+	}
+}
+#endif
+
 void __init dove_init_irq(void)
 {
 	int i;
@@ -115,6 +147,10 @@ void __init dove_init_irq(void)
 	orion_irq_init(0, IRQ_VIRT_BASE + IRQ_MASK_LOW_OFF);
 	orion_irq_init(32, IRQ_VIRT_BASE + IRQ_MASK_HIGH_OFF);
 
+#ifdef CONFIG_MULTI_IRQ_HANDLER
+	set_handle_irq(dove_legacy_handle_irq);
+#endif
+
 	/*
 	 * Initialize gpiolib for GPIOs 0-71.
 	 */
diff --git a/arch/arm/mach-exynos/Kconfig b/arch/arm/mach-exynos/Kconfig
index fc8bf18e222..d58995c9a95 100644
--- a/arch/arm/mach-exynos/Kconfig
+++ b/arch/arm/mach-exynos/Kconfig
@@ -7,97 +7,102 @@
 
 # Configuration options for the EXYNOS4
 
+config ARCH_EXYNOS
+	bool "Samsung EXYNOS" if ARCH_MULTI_V7
+	select ARCH_HAS_BANDGAP
+	select ARCH_HAS_CPUFREQ
+	select ARCH_HAS_HOLES_MEMORYMODEL
+	select ARCH_REQUIRE_GPIOLIB
+	select ARM_AMBA
+	select ARM_GIC
+	select COMMON_CLK_SAMSUNG
+	select HAVE_ARM_SCU if SMP
+	select HAVE_S3C2410_I2C if I2C
+	select HAVE_S3C2410_WATCHDOG if WATCHDOG
+	select HAVE_S3C_RTC if RTC_CLASS
+	select PINCTRL
+	select PINCTRL_EXYNOS
+	select PM_GENERIC_DOMAINS if PM_RUNTIME
+	select S5P_DEV_MFC
+	select SRAM
+	help
+	  Support for SAMSUNG EXYNOS SoCs (EXYNOS4/5)
+
 if ARCH_EXYNOS
 
 menu "SAMSUNG EXYNOS SoCs Support"
 
+config ARCH_EXYNOS3
+	bool "SAMSUNG EXYNOS3"
+	select ARM_CPU_SUSPEND if PM
+	help
+	  Samsung EXYNOS3 (Crotex-A7) SoC based systems
+
 config ARCH_EXYNOS4
 	bool "SAMSUNG EXYNOS4"
 	default y
-	select ARM_AMBA
-	select CLKSRC_OF
+	select ARM_CPU_SUSPEND if PM_SLEEP
 	select CLKSRC_SAMSUNG_PWM if CPU_EXYNOS4210
 	select CPU_EXYNOS4210
 	select GIC_NON_BANKED
 	select KEYBOARD_SAMSUNG if INPUT_KEYBOARD
-	select HAVE_ARM_SCU if SMP
-	select HAVE_SMP
 	select MIGHT_HAVE_CACHE_L2X0
-	select PINCTRL
-	select PM_GENERIC_DOMAINS if PM_RUNTIME
-	select S5P_DEV_MFC
 	help
-	  Samsung EXYNOS4 SoCs based systems
+	  Samsung EXYNOS4 (Cortex-A9) SoC based systems
 
 config ARCH_EXYNOS5
 	bool "SAMSUNG EXYNOS5"
-	select ARM_AMBA
-	select CLKSRC_OF
-	select HAVE_ARM_SCU if SMP
-	select HAVE_SMP
-	select PINCTRL
+	default y
 	help
-	  Samsung EXYNOS5 (Cortex-A15) SoC based systems
+	  Samsung EXYNOS5 (Cortex-A15/A7) SoC based systems
 
 comment "EXYNOS SoCs"
 
+config SOC_EXYNOS3250
+	bool "SAMSUNG EXYNOS3250"
+	default y
+	depends on ARCH_EXYNOS3
+
 config CPU_EXYNOS4210
 	bool "SAMSUNG EXYNOS4210"
 	default y
 	depends on ARCH_EXYNOS4
-	select ARCH_HAS_BANDGAP
-	select ARM_CPU_SUSPEND if PM_SLEEP
-	select PINCTRL_EXYNOS
-	select SAMSUNG_DMADEV
-	help
-	  Enable EXYNOS4210 CPU support
 
 config SOC_EXYNOS4212
 	bool "SAMSUNG EXYNOS4212"
 	default y
 	depends on ARCH_EXYNOS4
-	select ARCH_HAS_BANDGAP
-	select PINCTRL_EXYNOS
-	select SAMSUNG_DMADEV
-	help
-	  Enable EXYNOS4212 SoC support
 
 config SOC_EXYNOS4412
 	bool "SAMSUNG EXYNOS4412"
 	default y
 	depends on ARCH_EXYNOS4
-	select ARCH_HAS_BANDGAP
-	select PINCTRL_EXYNOS
-	select SAMSUNG_DMADEV
-	help
-	  Enable EXYNOS4412 SoC support
 
 config SOC_EXYNOS5250
 	bool "SAMSUNG EXYNOS5250"
 	default y
 	depends on ARCH_EXYNOS5
-	select ARCH_HAS_BANDGAP
-	select PINCTRL_EXYNOS
-	select PM_GENERIC_DOMAINS if PM_RUNTIME
-	select S5P_DEV_MFC
-	select SAMSUNG_DMADEV
-	help
-	  Enable EXYNOS5250 SoC support
+
+config SOC_EXYNOS5260
+	bool "SAMSUNG EXYNOS5260"
+	default y
+	depends on ARCH_EXYNOS5
+
+config SOC_EXYNOS5410
+	bool "SAMSUNG EXYNOS5410"
+	default y
+	depends on ARCH_EXYNOS5
 
 config SOC_EXYNOS5420
 	bool "SAMSUNG EXYNOS5420"
 	default y
 	depends on ARCH_EXYNOS5
-	select PM_GENERIC_DOMAINS if PM_RUNTIME
-	help
-	  Enable EXYNOS5420 SoC support
 
 config SOC_EXYNOS5440
 	bool "SAMSUNG EXYNOS5440"
 	default y
 	depends on ARCH_EXYNOS5
 	select ARCH_DMA_ADDR_T_64BIT if ARM_LPAE
-	select ARCH_HAS_BANDGAP
 	select ARCH_HAS_OPP
 	select HAVE_ARM_ARCH_TIMER
 	select AUTO_ZRELADDR
@@ -108,6 +113,19 @@ config SOC_EXYNOS5440
 	help
 	  Enable EXYNOS5440 SoC support
 
+config SOC_EXYNOS5800
+	bool "SAMSUNG EXYNOS5800"
+	default y
+	depends on SOC_EXYNOS5420
+
 endmenu
 
+config EXYNOS5420_MCPM
+	bool "Exynos5420 Multi-Cluster PM support"
+	depends on MCPM && SOC_EXYNOS5420
+	select ARM_CCI
+	help
+	  This is needed to provide CPU and cluster power management
+	  on Exynos5420 implementing big.LITTLE.
+
 endif
diff --git a/arch/arm/mach-exynos/Makefile b/arch/arm/mach-exynos/Makefile
index a656dbe3b78..788f26d2114 100644
--- a/arch/arm/mach-exynos/Makefile
+++ b/arch/arm/mach-exynos/Makefile
@@ -5,6 +5,8 @@
 #
 # Licensed under GPLv2
 
+ccflags-$(CONFIG_ARCH_MULTIPLATFORM) += -I$(srctree)/$(src)/include -I$(srctree)/arch/arm/plat-samsung/include
+
 obj-y				:=
 obj-m				:=
 obj-n				:=
@@ -12,20 +14,18 @@ obj-				:=
 
 # Core
 
-obj-$(CONFIG_ARCH_EXYNOS)	+= exynos.o
+obj-$(CONFIG_ARCH_EXYNOS)	+= exynos.o pmu.o exynos-smc.o firmware.o
 
 obj-$(CONFIG_PM_SLEEP)		+= pm.o sleep.o
 obj-$(CONFIG_PM_GENERIC_DOMAINS) += pm_domains.o
-obj-$(CONFIG_CPU_IDLE)		+= cpuidle.o
-
-obj-$(CONFIG_ARCH_EXYNOS)	+= pmu.o
 
 obj-$(CONFIG_SMP)		+= platsmp.o headsmp.o
 
 obj-$(CONFIG_HOTPLUG_CPU)	+= hotplug.o
-
-obj-$(CONFIG_ARCH_EXYNOS)	+= exynos-smc.o
-obj-$(CONFIG_ARCH_EXYNOS)	+= firmware.o
+CFLAGS_hotplug.o		+= -march=armv7-a
 
 plus_sec := $(call as-instr,.arch_extension sec,+sec)
 AFLAGS_exynos-smc.o		:=-Wa,-march=armv7-a$(plus_sec)
+
+obj-$(CONFIG_EXYNOS5420_MCPM)	+= mcpm-exynos.o
+CFLAGS_mcpm-exynos.o		+= -march=armv7-a
diff --git a/arch/arm/mach-exynos/common.h b/arch/arm/mach-exynos/common.h
index 9ef3f83efaf..80b90e346ca 100644
--- a/arch/arm/mach-exynos/common.h
+++ b/arch/arm/mach-exynos/common.h
@@ -15,9 +15,107 @@
 #include <linux/reboot.h>
 #include <linux/of.h>
 
+#define EXYNOS3250_SOC_ID	0xE3472000
+#define EXYNOS3_SOC_MASK	0xFFFFF000
+
+#define EXYNOS4210_CPU_ID	0x43210000
+#define EXYNOS4212_CPU_ID	0x43220000
+#define EXYNOS4412_CPU_ID	0xE4412200
+#define EXYNOS4_CPU_MASK	0xFFFE0000
+
+#define EXYNOS5250_SOC_ID	0x43520000
+#define EXYNOS5410_SOC_ID	0xE5410000
+#define EXYNOS5420_SOC_ID	0xE5420000
+#define EXYNOS5440_SOC_ID	0xE5440000
+#define EXYNOS5800_SOC_ID	0xE5422000
+#define EXYNOS5_SOC_MASK	0xFFFFF000
+
+extern unsigned long samsung_cpu_id;
+
+#define IS_SAMSUNG_CPU(name, id, mask)		\
+static inline int is_samsung_##name(void)	\
+{						\
+	return ((samsung_cpu_id & mask) == (id & mask));	\
+}
+
+IS_SAMSUNG_CPU(exynos3250, EXYNOS3250_SOC_ID, EXYNOS3_SOC_MASK)
+IS_SAMSUNG_CPU(exynos4210, EXYNOS4210_CPU_ID, EXYNOS4_CPU_MASK)
+IS_SAMSUNG_CPU(exynos4212, EXYNOS4212_CPU_ID, EXYNOS4_CPU_MASK)
+IS_SAMSUNG_CPU(exynos4412, EXYNOS4412_CPU_ID, EXYNOS4_CPU_MASK)
+IS_SAMSUNG_CPU(exynos5250, EXYNOS5250_SOC_ID, EXYNOS5_SOC_MASK)
+IS_SAMSUNG_CPU(exynos5410, EXYNOS5410_SOC_ID, EXYNOS5_SOC_MASK)
+IS_SAMSUNG_CPU(exynos5420, EXYNOS5420_SOC_ID, EXYNOS5_SOC_MASK)
+IS_SAMSUNG_CPU(exynos5440, EXYNOS5440_SOC_ID, EXYNOS5_SOC_MASK)
+IS_SAMSUNG_CPU(exynos5800, EXYNOS5800_SOC_ID, EXYNOS5_SOC_MASK)
+
+#if defined(CONFIG_SOC_EXYNOS3250)
+# define soc_is_exynos3250()	is_samsung_exynos3250()
+#else
+# define soc_is_exynos3250()	0
+#endif
+
+#if defined(CONFIG_CPU_EXYNOS4210)
+# define soc_is_exynos4210()	is_samsung_exynos4210()
+#else
+# define soc_is_exynos4210()	0
+#endif
+
+#if defined(CONFIG_SOC_EXYNOS4212)
+# define soc_is_exynos4212()	is_samsung_exynos4212()
+#else
+# define soc_is_exynos4212()	0
+#endif
+
+#if defined(CONFIG_SOC_EXYNOS4412)
+# define soc_is_exynos4412()	is_samsung_exynos4412()
+#else
+# define soc_is_exynos4412()	0
+#endif
+
+#define EXYNOS4210_REV_0	(0x0)
+#define EXYNOS4210_REV_1_0	(0x10)
+#define EXYNOS4210_REV_1_1	(0x11)
+
+#if defined(CONFIG_SOC_EXYNOS5250)
+# define soc_is_exynos5250()	is_samsung_exynos5250()
+#else
+# define soc_is_exynos5250()	0
+#endif
+
+#if defined(CONFIG_SOC_EXYNOS5410)
+# define soc_is_exynos5410()	is_samsung_exynos5410()
+#else
+# define soc_is_exynos5410()	0
+#endif
+
+#if defined(CONFIG_SOC_EXYNOS5420)
+# define soc_is_exynos5420()	is_samsung_exynos5420()
+#else
+# define soc_is_exynos5420()	0
+#endif
+
+#if defined(CONFIG_SOC_EXYNOS5440)
+# define soc_is_exynos5440()	is_samsung_exynos5440()
+#else
+# define soc_is_exynos5440()	0
+#endif
+
+#if defined(CONFIG_SOC_EXYNOS5800)
+# define soc_is_exynos5800()	is_samsung_exynos5800()
+#else
+# define soc_is_exynos5800()	0
+#endif
+
+#define soc_is_exynos4() (soc_is_exynos4210() || soc_is_exynos4212() || \
+			  soc_is_exynos4412())
+#define soc_is_exynos5() (soc_is_exynos5250() || soc_is_exynos5410() || \
+			  soc_is_exynos5420() || soc_is_exynos5800())
+
 void mct_init(void __iomem *base, int irq_g0, int irq_l0, int irq_l1);
 
 struct map_desc;
+extern void __iomem *sysram_ns_base_addr;
+extern void __iomem *sysram_base_addr;
 void exynos_init_io(void);
 void exynos_restart(enum reboot_mode mode, const char *cmd);
 void exynos_cpuidle_init(void);
@@ -62,5 +160,15 @@ struct exynos_pmu_conf {
 };
 
 extern void exynos_sys_powerdown_conf(enum sys_powerdown mode);
+extern void exynos_cpu_power_down(int cpu);
+extern void exynos_cpu_power_up(int cpu);
+extern int  exynos_cpu_power_state(int cpu);
+extern void exynos_cluster_power_down(int cluster);
+extern void exynos_cluster_power_up(int cluster);
+extern int  exynos_cluster_power_state(int cluster);
+extern void exynos_enter_aftr(void);
+
+extern void s5p_init_cpu(void __iomem *cpuid_addr);
+extern unsigned int samsung_rev(void);
 
 #endif /* __ARCH_ARM_MACH_EXYNOS_COMMON_H */
diff --git a/arch/arm/mach-exynos/cpuidle.c b/arch/arm/mach-exynos/cpuidle.c
deleted file mode 100644
index c57cae0e877..00000000000
--- a/arch/arm/mach-exynos/cpuidle.c
+++ /dev/null
@@ -1,256 +0,0 @@
-/* linux/arch/arm/mach-exynos4/cpuidle.c
- *
- * Copyright (c) 2011 Samsung Electronics Co., Ltd.
- *		http://www.samsung.com
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
-*/
-
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/cpuidle.h>
-#include <linux/cpu_pm.h>
-#include <linux/io.h>
-#include <linux/export.h>
-#include <linux/module.h>
-#include <linux/time.h>
-#include <linux/platform_device.h>
-
-#include <asm/proc-fns.h>
-#include <asm/smp_scu.h>
-#include <asm/suspend.h>
-#include <asm/unified.h>
-#include <asm/cpuidle.h>
-
-#include <plat/cpu.h>
-#include <plat/pm.h>
-
-#include <mach/map.h>
-
-#include "common.h"
-#include "regs-pmu.h"
-
-#define REG_DIRECTGO_ADDR	(samsung_rev() == EXYNOS4210_REV_1_1 ? \
-			S5P_INFORM7 : (samsung_rev() == EXYNOS4210_REV_1_0 ? \
-			(S5P_VA_SYSRAM + 0x24) : S5P_INFORM0))
-#define REG_DIRECTGO_FLAG	(samsung_rev() == EXYNOS4210_REV_1_1 ? \
-			S5P_INFORM6 : (samsung_rev() == EXYNOS4210_REV_1_0 ? \
-			(S5P_VA_SYSRAM + 0x20) : S5P_INFORM1))
-
-#define S5P_CHECK_AFTR		0xFCBA0D10
-
-#define EXYNOS5_PWR_CTRL1			(S5P_VA_CMU + 0x01020)
-#define EXYNOS5_PWR_CTRL2			(S5P_VA_CMU + 0x01024)
-
-#define PWR_CTRL1_CORE2_DOWN_RATIO		(7 << 28)
-#define PWR_CTRL1_CORE1_DOWN_RATIO		(7 << 16)
-#define PWR_CTRL1_DIV2_DOWN_EN			(1 << 9)
-#define PWR_CTRL1_DIV1_DOWN_EN			(1 << 8)
-#define PWR_CTRL1_USE_CORE1_WFE			(1 << 5)
-#define PWR_CTRL1_USE_CORE0_WFE			(1 << 4)
-#define PWR_CTRL1_USE_CORE1_WFI			(1 << 1)
-#define PWR_CTRL1_USE_CORE0_WFI			(1 << 0)
-
-#define PWR_CTRL2_DIV2_UP_EN			(1 << 25)
-#define PWR_CTRL2_DIV1_UP_EN			(1 << 24)
-#define PWR_CTRL2_DUR_STANDBY2_VAL		(1 << 16)
-#define PWR_CTRL2_DUR_STANDBY1_VAL		(1 << 8)
-#define PWR_CTRL2_CORE2_UP_RATIO		(1 << 4)
-#define PWR_CTRL2_CORE1_UP_RATIO		(1 << 0)
-
-static int exynos4_enter_lowpower(struct cpuidle_device *dev,
-				struct cpuidle_driver *drv,
-				int index);
-
-static DEFINE_PER_CPU(struct cpuidle_device, exynos4_cpuidle_device);
-
-static struct cpuidle_driver exynos4_idle_driver = {
-	.name			= "exynos4_idle",
-	.owner			= THIS_MODULE,
-	.states = {
-		[0] = ARM_CPUIDLE_WFI_STATE,
-		[1] = {
-			.enter			= exynos4_enter_lowpower,
-			.exit_latency		= 300,
-			.target_residency	= 100000,
-			.flags			= CPUIDLE_FLAG_TIME_VALID,
-			.name			= "C1",
-			.desc			= "ARM power down",
-		},
-	},
-	.state_count = 2,
-	.safe_state_index = 0,
-};
-
-/* Ext-GIC nIRQ/nFIQ is the only wakeup source in AFTR */
-static void exynos4_set_wakeupmask(void)
-{
-	__raw_writel(0x0000ff3e, S5P_WAKEUP_MASK);
-}
-
-static unsigned int g_pwr_ctrl, g_diag_reg;
-
-static void save_cpu_arch_register(void)
-{
-	/*read power control register*/
-	asm("mrc p15, 0, %0, c15, c0, 0" : "=r"(g_pwr_ctrl) : : "cc");
-	/*read diagnostic register*/
-	asm("mrc p15, 0, %0, c15, c0, 1" : "=r"(g_diag_reg) : : "cc");
-	return;
-}
-
-static void restore_cpu_arch_register(void)
-{
-	/*write power control register*/
-	asm("mcr p15, 0, %0, c15, c0, 0" : : "r"(g_pwr_ctrl) : "cc");
-	/*write diagnostic register*/
-	asm("mcr p15, 0, %0, c15, c0, 1" : : "r"(g_diag_reg) : "cc");
-	return;
-}
-
-static int idle_finisher(unsigned long flags)
-{
-	cpu_do_idle();
-	return 1;
-}
-
-static int exynos4_enter_core0_aftr(struct cpuidle_device *dev,
-				struct cpuidle_driver *drv,
-				int index)
-{
-	unsigned long tmp;
-
-	exynos4_set_wakeupmask();
-
-	/* Set value of power down register for aftr mode */
-	exynos_sys_powerdown_conf(SYS_AFTR);
-
-	__raw_writel(virt_to_phys(exynos_cpu_resume), REG_DIRECTGO_ADDR);
-	__raw_writel(S5P_CHECK_AFTR, REG_DIRECTGO_FLAG);
-
-	save_cpu_arch_register();
-
-	/* Setting Central Sequence Register for power down mode */
-	tmp = __raw_readl(S5P_CENTRAL_SEQ_CONFIGURATION);
-	tmp &= ~S5P_CENTRAL_LOWPWR_CFG;
-	__raw_writel(tmp, S5P_CENTRAL_SEQ_CONFIGURATION);
-
-	cpu_pm_enter();
-	cpu_suspend(0, idle_finisher);
-
-#ifdef CONFIG_SMP
-	if (!soc_is_exynos5250())
-		scu_enable(S5P_VA_SCU);
-#endif
-	cpu_pm_exit();
-
-	restore_cpu_arch_register();
-
-	/*
-	 * If PMU failed while entering sleep mode, WFI will be
-	 * ignored by PMU and then exiting cpu_do_idle().
-	 * S5P_CENTRAL_LOWPWR_CFG bit will not be set automatically
-	 * in this situation.
-	 */
-	tmp = __raw_readl(S5P_CENTRAL_SEQ_CONFIGURATION);
-	if (!(tmp & S5P_CENTRAL_LOWPWR_CFG)) {
-		tmp |= S5P_CENTRAL_LOWPWR_CFG;
-		__raw_writel(tmp, S5P_CENTRAL_SEQ_CONFIGURATION);
-	}
-
-	/* Clear wakeup state register */
-	__raw_writel(0x0, S5P_WAKEUP_STAT);
-
-	return index;
-}
-
-static int exynos4_enter_lowpower(struct cpuidle_device *dev,
-				struct cpuidle_driver *drv,
-				int index)
-{
-	int new_index = index;
-
-	/* AFTR can only be entered when cores other than CPU0 are offline */
-	if (num_online_cpus() > 1 || dev->cpu != 0)
-		new_index = drv->safe_state_index;
-
-	if (new_index == 0)
-		return arm_cpuidle_simple_enter(dev, drv, new_index);
-	else
-		return exynos4_enter_core0_aftr(dev, drv, new_index);
-}
-
-static void __init exynos5_core_down_clk(void)
-{
-	unsigned int tmp;
-
-	/*
-	 * Enable arm clock down (in idle) and set arm divider
-	 * ratios in WFI/WFE state.
-	 */
-	tmp = PWR_CTRL1_CORE2_DOWN_RATIO | \
-	      PWR_CTRL1_CORE1_DOWN_RATIO | \
-	      PWR_CTRL1_DIV2_DOWN_EN	 | \
-	      PWR_CTRL1_DIV1_DOWN_EN	 | \
-	      PWR_CTRL1_USE_CORE1_WFE	 | \
-	      PWR_CTRL1_USE_CORE0_WFE	 | \
-	      PWR_CTRL1_USE_CORE1_WFI	 | \
-	      PWR_CTRL1_USE_CORE0_WFI;
-	__raw_writel(tmp, EXYNOS5_PWR_CTRL1);
-
-	/*
-	 * Enable arm clock up (on exiting idle). Set arm divider
-	 * ratios when not in idle along with the standby duration
-	 * ratios.
-	 */
-	tmp = PWR_CTRL2_DIV2_UP_EN	 | \
-	      PWR_CTRL2_DIV1_UP_EN	 | \
-	      PWR_CTRL2_DUR_STANDBY2_VAL | \
-	      PWR_CTRL2_DUR_STANDBY1_VAL | \
-	      PWR_CTRL2_CORE2_UP_RATIO	 | \
-	      PWR_CTRL2_CORE1_UP_RATIO;
-	__raw_writel(tmp, EXYNOS5_PWR_CTRL2);
-}
-
-static int exynos_cpuidle_probe(struct platform_device *pdev)
-{
-	int cpu_id, ret;
-	struct cpuidle_device *device;
-
-	if (soc_is_exynos5250())
-		exynos5_core_down_clk();
-
-	if (soc_is_exynos5440())
-		exynos4_idle_driver.state_count = 1;
-
-	ret = cpuidle_register_driver(&exynos4_idle_driver);
-	if (ret) {
-		dev_err(&pdev->dev, "failed to register cpuidle driver\n");
-		return ret;
-	}
-
-	for_each_online_cpu(cpu_id) {
-		device = &per_cpu(exynos4_cpuidle_device, cpu_id);
-		device->cpu = cpu_id;
-
-		ret = cpuidle_register_device(device);
-		if (ret) {
-			dev_err(&pdev->dev, "failed to register cpuidle device\n");
-			return ret;
-		}
-	}
-
-	return 0;
-}
-
-static struct platform_driver exynos_cpuidle_driver = {
-	.probe	= exynos_cpuidle_probe,
-	.driver = {
-		.name = "exynos_cpuidle",
-		.owner = THIS_MODULE,
-	},
-};
-
-module_platform_driver(exynos_cpuidle_driver);
diff --git a/arch/arm/mach-exynos/exynos.c b/arch/arm/mach-exynos/exynos.c
index b32a907d021..bc43e22693b 100644
--- a/arch/arm/mach-exynos/exynos.c
+++ b/arch/arm/mach-exynos/exynos.c
@@ -26,8 +26,6 @@
 #include <asm/mach/map.h>
 #include <asm/memory.h>
 
-#include <plat/cpu.h>
-
 #include "common.h"
 #include "mfc.h"
 #include "regs-pmu.h"
@@ -114,51 +112,6 @@ static struct map_desc exynos4_iodesc[] __initdata = {
 	},
 };
 
-static struct map_desc exynos4_iodesc0[] __initdata = {
-	{
-		.virtual	= (unsigned long)S5P_VA_SYSRAM,
-		.pfn		= __phys_to_pfn(EXYNOS4_PA_SYSRAM0),
-		.length		= SZ_4K,
-		.type		= MT_DEVICE,
-	},
-};
-
-static struct map_desc exynos4_iodesc1[] __initdata = {
-	{
-		.virtual	= (unsigned long)S5P_VA_SYSRAM,
-		.pfn		= __phys_to_pfn(EXYNOS4_PA_SYSRAM1),
-		.length		= SZ_4K,
-		.type		= MT_DEVICE,
-	},
-};
-
-static struct map_desc exynos4210_iodesc[] __initdata = {
-	{
-		.virtual	= (unsigned long)S5P_VA_SYSRAM_NS,
-		.pfn		= __phys_to_pfn(EXYNOS4210_PA_SYSRAM_NS),
-		.length		= SZ_4K,
-		.type		= MT_DEVICE,
-	},
-};
-
-static struct map_desc exynos4x12_iodesc[] __initdata = {
-	{
-		.virtual	= (unsigned long)S5P_VA_SYSRAM_NS,
-		.pfn		= __phys_to_pfn(EXYNOS4x12_PA_SYSRAM_NS),
-		.length		= SZ_4K,
-		.type		= MT_DEVICE,
-	},
-};
-
-static struct map_desc exynos5250_iodesc[] __initdata = {
-	{
-		.virtual	= (unsigned long)S5P_VA_SYSRAM_NS,
-		.pfn		= __phys_to_pfn(EXYNOS5250_PA_SYSRAM_NS),
-		.length		= SZ_4K,
-		.type		= MT_DEVICE,
-	},
-};
-
 static struct map_desc exynos5_iodesc[] __initdata = {
 	{
 		.virtual	= (unsigned long)S3C_VA_SYS,
@@ -181,11 +134,6 @@ static struct map_desc exynos5_iodesc[] __initdata = {
 		.length		= SZ_4K,
 		.type		= MT_DEVICE,
 	}, {
-		.virtual	= (unsigned long)S5P_VA_SYSRAM,
-		.pfn		= __phys_to_pfn(EXYNOS5_PA_SYSRAM),
-		.length		= SZ_4K,
-		.type		= MT_DEVICE,
-	}, {
 		.virtual	= (unsigned long)S5P_VA_CMU,
 		.pfn		= __phys_to_pfn(EXYNOS5_PA_CMU),
 		.length		= 144 * SZ_1K,
@@ -221,12 +169,16 @@ void exynos_restart(enum reboot_mode mode, const char *cmd)
 }
 
 static struct platform_device exynos_cpuidle = {
-	.name		= "exynos_cpuidle",
-	.id		= -1,
+	.name              = "exynos_cpuidle",
+	.dev.platform_data = exynos_enter_aftr,
+	.id                = -1,
 };
 
 void __init exynos_cpuidle_init(void)
 {
+	if (soc_is_exynos5440())
+		return;
+
 	platform_device_register(&exynos_cpuidle);
 }
 
@@ -280,20 +232,6 @@ static void __init exynos_map_io(void)
 
 	if (soc_is_exynos5())
 		iotable_init(exynos5_iodesc, ARRAY_SIZE(exynos5_iodesc));
-
-	if (soc_is_exynos4210()) {
-		if (samsung_rev() == EXYNOS4210_REV_0)
-			iotable_init(exynos4_iodesc0,
-						ARRAY_SIZE(exynos4_iodesc0));
-		else
-			iotable_init(exynos4_iodesc1,
-						ARRAY_SIZE(exynos4_iodesc1));
-		iotable_init(exynos4210_iodesc, ARRAY_SIZE(exynos4210_iodesc));
-	}
-	if (soc_is_exynos4212() || soc_is_exynos4412())
-		iotable_init(exynos4x12_iodesc, ARRAY_SIZE(exynos4x12_iodesc));
-	if (soc_is_exynos5250())
-		iotable_init(exynos5250_iodesc, ARRAY_SIZE(exynos5250_iodesc));
 }
 
 void __init exynos_init_io(void)
@@ -308,21 +246,13 @@ void __init exynos_init_io(void)
 	exynos_map_io();
 }
 
-struct bus_type exynos_subsys = {
-	.name		= "exynos-core",
-	.dev_name	= "exynos-core",
-};
-
-static int __init exynos_core_init(void)
-{
-	return subsys_system_register(&exynos_subsys, NULL);
-}
-core_initcall(exynos_core_init);
-
 static int __init exynos4_l2x0_cache_init(void)
 {
 	int ret;
 
+	if (!soc_is_exynos4())
+		return 0;
+
 	ret = l2x0_of_init(L2_AUX_VAL, L2_AUX_MASK);
 	if (ret)
 		return ret;
@@ -370,12 +300,15 @@ static void __init exynos_dt_machine_init(void)
 }
 
 static char const *exynos_dt_compat[] __initconst = {
+	"samsung,exynos3",
+	"samsung,exynos3250",
 	"samsung,exynos4",
 	"samsung,exynos4210",
 	"samsung,exynos4212",
 	"samsung,exynos4412",
 	"samsung,exynos5",
 	"samsung,exynos5250",
+	"samsung,exynos5260",
 	"samsung,exynos5420",
 	"samsung,exynos5440",
 	NULL
diff --git a/arch/arm/mach-exynos/firmware.c b/arch/arm/mach-exynos/firmware.c
index 932129ef26c..eb91d2350f8 100644
--- a/arch/arm/mach-exynos/firmware.c
+++ b/arch/arm/mach-exynos/firmware.c
@@ -18,6 +18,7 @@
 
 #include <mach/map.h>
 
+#include "common.h"
 #include "smc.h"
 
 static int exynos_do_idle(void)
@@ -28,13 +29,36 @@ static int exynos_do_idle(void)
 
 static int exynos_cpu_boot(int cpu)
 {
+	/*
+	 * Exynos3250 doesn't need to send smc command for secondary CPU boot
+	 * because Exynos3250 removes WFE in secure mode.
+	 */
+	if (soc_is_exynos3250())
+		return 0;
+
+	/*
+	 * The second parameter of SMC_CMD_CPU1BOOT command means CPU id.
+	 * But, Exynos4212 has only one secondary CPU so second parameter
+	 * isn't used for informing secure firmware about CPU id.
+	 */
+	if (soc_is_exynos4212())
+		cpu = 0;
+
 	exynos_smc(SMC_CMD_CPU1BOOT, cpu, 0, 0);
 	return 0;
 }
 
 static int exynos_set_cpu_boot_addr(int cpu, unsigned long boot_addr)
 {
-	void __iomem *boot_reg = S5P_VA_SYSRAM_NS + 0x1c + 4*cpu;
+	void __iomem *boot_reg;
+
+	if (!sysram_ns_base_addr)
+		return -ENODEV;
+
+	boot_reg = sysram_ns_base_addr + 0x1c;
+
+	if (!soc_is_exynos4212() && !soc_is_exynos3250())
+		boot_reg += 4*cpu;
 
 	__raw_writel(boot_addr, boot_reg);
 	return 0;
diff --git a/arch/arm/mach-exynos/hotplug.c b/arch/arm/mach-exynos/hotplug.c
index 5eead530c6f..69fa4839739 100644
--- a/arch/arm/mach-exynos/hotplug.c
+++ b/arch/arm/mach-exynos/hotplug.c
@@ -19,61 +19,9 @@
 #include <asm/cp15.h>
 #include <asm/smp_plat.h>
 
-#include <plat/cpu.h>
-
 #include "common.h"
 #include "regs-pmu.h"
 
-static inline void cpu_enter_lowpower_a9(void)
-{
-	unsigned int v;
-
-	asm volatile(
-	"	mcr	p15, 0, %1, c7, c5, 0\n"
-	"	mcr	p15, 0, %1, c7, c10, 4\n"
-	/*
-	 * Turn off coherency
-	 */
-	"	mrc	p15, 0, %0, c1, c0, 1\n"
-	"	bic	%0, %0, %3\n"
-	"	mcr	p15, 0, %0, c1, c0, 1\n"
-	"	mrc	p15, 0, %0, c1, c0, 0\n"
-	"	bic	%0, %0, %2\n"
-	"	mcr	p15, 0, %0, c1, c0, 0\n"
-	  : "=&r" (v)
-	  : "r" (0), "Ir" (CR_C), "Ir" (0x40)
-	  : "cc");
-}
-
-static inline void cpu_enter_lowpower_a15(void)
-{
-	unsigned int v;
-
-	asm volatile(
-	"	mrc	p15, 0, %0, c1, c0, 0\n"
-	"	bic	%0, %0, %1\n"
-	"	mcr	p15, 0, %0, c1, c0, 0\n"
-	  : "=&r" (v)
-	  : "Ir" (CR_C)
-	  : "cc");
-
-	flush_cache_louis();
-
-	asm volatile(
-	/*
-	* Turn off coherency
-	*/
-	"	mrc	p15, 0, %0, c1, c0, 1\n"
-	"	bic	%0, %0, %1\n"
-	"	mcr	p15, 0, %0, c1, c0, 1\n"
-	: "=&r" (v)
-	: "Ir" (0x40)
-	: "cc");
-
-	isb();
-	dsb();
-}
-
 static inline void cpu_leave_lowpower(void)
 {
 	unsigned int v;
@@ -96,7 +44,7 @@ static inline void platform_do_lowpower(unsigned int cpu, int *spurious)
 
 		/* make cpu1 to be turned off at next WFI command */
 		if (cpu == 1)
-			__raw_writel(0, S5P_ARM_CORE1_CONFIGURATION);
+			exynos_cpu_power_down(cpu);
 
 		/*
 		 * here's the WFI
@@ -132,19 +80,8 @@ static inline void platform_do_lowpower(unsigned int cpu, int *spurious)
 void __ref exynos_cpu_die(unsigned int cpu)
 {
 	int spurious = 0;
-	int primary_part = 0;
 
-	/*
-	 * we're ready for shutdown now, so do it.
-	 * Exynos4 is A9 based while Exynos5 is A15; check the CPU part
-	 * number by reading the Main ID register and then perform the
-	 * appropriate sequence for entering low power.
-	 */
-	asm("mrc p15, 0, %0, c0, c0, 0" : "=r"(primary_part) : : "cc");
-	if ((primary_part & 0xfff0) == 0xc0f0)
-		cpu_enter_lowpower_a15();
-	else
-		cpu_enter_lowpower_a9();
+	v7_exit_coherency_flush(louis);
 
 	platform_do_lowpower(cpu, &spurious);
 
diff --git a/arch/arm/mach-exynos/include/mach/map.h b/arch/arm/mach-exynos/include/mach/map.h
index 7b046b59d9e..548269a6063 100644
--- a/arch/arm/mach-exynos/include/mach/map.h
+++ b/arch/arm/mach-exynos/include/mach/map.h
@@ -23,13 +23,6 @@
 
 #include <plat/map-s5p.h>
 
-#define EXYNOS4_PA_SYSRAM0		0x02025000
-#define EXYNOS4_PA_SYSRAM1		0x02020000
-#define EXYNOS5_PA_SYSRAM		0x02020000
-#define EXYNOS4210_PA_SYSRAM_NS		0x0203F000
-#define EXYNOS4x12_PA_SYSRAM_NS		0x0204F000
-#define EXYNOS5250_PA_SYSRAM_NS		0x0204F000
-
 #define EXYNOS_PA_CHIPID		0x10000000
 
 #define EXYNOS4_PA_SYSCON		0x10010000
diff --git a/arch/arm/mach-exynos/mcpm-exynos.c b/arch/arm/mach-exynos/mcpm-exynos.c
new file mode 100644
index 00000000000..0498d0b887e
--- /dev/null
+++ b/arch/arm/mach-exynos/mcpm-exynos.c
@@ -0,0 +1,357 @@
+/*
+ * Copyright (c) 2014 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * arch/arm/mach-exynos/mcpm-exynos.c
+ *
+ * Based on arch/arm/mach-vexpress/dcscb.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
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/arm-cci.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/of_address.h>
+
+#include <asm/cputype.h>
+#include <asm/cp15.h>
+#include <asm/mcpm.h>
+
+#include "regs-pmu.h"
+#include "common.h"
+
+#define EXYNOS5420_CPUS_PER_CLUSTER	4
+#define EXYNOS5420_NR_CLUSTERS		2
+#define MCPM_BOOT_ADDR_OFFSET		0x1c
+
+/*
+ * The common v7_exit_coherency_flush API could not be used because of the
+ * Erratum 799270 workaround. This macro is the same as the common one (in
+ * arch/arm/include/asm/cacheflush.h) except for the erratum handling.
+ */
+#define exynos_v7_exit_coherency_flush(level) \
+	asm volatile( \
+	"stmfd	sp!, {fp, ip}\n\t"\
+	"mrc	p15, 0, r0, c1, c0, 0	@ get SCTLR\n\t" \
+	"bic	r0, r0, #"__stringify(CR_C)"\n\t" \
+	"mcr	p15, 0, r0, c1, c0, 0	@ set SCTLR\n\t" \
+	"isb\n\t"\
+	"bl	v7_flush_dcache_"__stringify(level)"\n\t" \
+	"clrex\n\t"\
+	"mrc	p15, 0, r0, c1, c0, 1	@ get ACTLR\n\t" \
+	"bic	r0, r0, #(1 << 6)	@ disable local coherency\n\t" \
+	/* Dummy Load of a device register to avoid Erratum 799270 */ \
+	"ldr	r4, [%0]\n\t" \
+	"and	r4, r4, #0\n\t" \
+	"orr	r0, r0, r4\n\t" \
+	"mcr	p15, 0, r0, c1, c0, 1	@ set ACTLR\n\t" \
+	"isb\n\t" \
+	"dsb\n\t" \
+	"ldmfd	sp!, {fp, ip}" \
+	: \
+	: "Ir" (S5P_INFORM0) \
+	: "r0", "r1", "r2", "r3", "r4", "r5", "r6", "r7", \
+	  "r9", "r10", "lr", "memory")
+
+/*
+ * We can't use regular spinlocks. In the switcher case, it is possible
+ * for an outbound CPU to call power_down() after its inbound counterpart
+ * is already live using the same logical CPU number which trips lockdep
+ * debugging.
+ */
+static arch_spinlock_t exynos_mcpm_lock = __ARCH_SPIN_LOCK_UNLOCKED;
+static int
+cpu_use_count[EXYNOS5420_CPUS_PER_CLUSTER][EXYNOS5420_NR_CLUSTERS];
+
+#define exynos_cluster_usecnt(cluster) \
+	(cpu_use_count[0][cluster] +   \
+	 cpu_use_count[1][cluster] +   \
+	 cpu_use_count[2][cluster] +   \
+	 cpu_use_count[3][cluster])
+
+#define exynos_cluster_unused(cluster) !exynos_cluster_usecnt(cluster)
+
+static int exynos_cluster_power_control(unsigned int cluster, int enable)
+{
+	unsigned int tries = 100;
+	unsigned int val;
+
+	if (enable) {
+		exynos_cluster_power_up(cluster);
+		val = S5P_CORE_LOCAL_PWR_EN;
+	} else {
+		exynos_cluster_power_down(cluster);
+		val = 0;
+	}
+
+	/* Wait until cluster power control is applied */
+	while (tries--) {
+		if (exynos_cluster_power_state(cluster) == val)
+			return 0;
+
+		cpu_relax();
+	}
+	pr_debug("timed out waiting for cluster %u to power %s\n", cluster,
+		enable ? "on" : "off");
+
+	return -ETIMEDOUT;
+}
+
+static int exynos_power_up(unsigned int cpu, unsigned int cluster)
+{
+	unsigned int cpunr = cpu + (cluster * EXYNOS5420_CPUS_PER_CLUSTER);
+	int err = 0;
+
+	pr_debug("%s: cpu %u cluster %u\n", __func__, cpu, cluster);
+	if (cpu >= EXYNOS5420_CPUS_PER_CLUSTER ||
+		cluster >= EXYNOS5420_NR_CLUSTERS)
+		return -EINVAL;
+
+	/*
+	 * Since this is called with IRQs enabled, and no arch_spin_lock_irq
+	 * variant exists, we need to disable IRQs manually here.
+	 */
+	local_irq_disable();
+	arch_spin_lock(&exynos_mcpm_lock);
+
+	cpu_use_count[cpu][cluster]++;
+	if (cpu_use_count[cpu][cluster] == 1) {
+		bool was_cluster_down =
+			(exynos_cluster_usecnt(cluster) == 1);
+
+		/*
+		 * Turn on the cluster (L2/COMMON) and then power on the
+		 * cores.
+		 */
+		if (was_cluster_down)
+			err = exynos_cluster_power_control(cluster, 1);
+
+		if (!err)
+			exynos_cpu_power_up(cpunr);
+		else
+			exynos_cluster_power_control(cluster, 0);
+	} else if (cpu_use_count[cpu][cluster] != 2) {
+		/*
+		 * The only possible values are:
+		 * 0 = CPU down
+		 * 1 = CPU (still) up
+		 * 2 = CPU requested to be up before it had a chance
+		 *     to actually make itself down.
+		 * Any other value is a bug.
+		 */
+		BUG();
+	}
+
+	arch_spin_unlock(&exynos_mcpm_lock);
+	local_irq_enable();
+
+	return err;
+}
+
+/*
+ * NOTE: This function requires the stack data to be visible through power down
+ * and can only be executed on processors like A15 and A7 that hit the cache
+ * with the C bit clear in the SCTLR register.
+ */
+static void exynos_power_down(void)
+{
+	unsigned int mpidr, cpu, cluster;
+	bool last_man = false, skip_wfi = false;
+	unsigned int cpunr;
+
+	mpidr = read_cpuid_mpidr();
+	cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0);
+	cluster = MPIDR_AFFINITY_LEVEL(mpidr, 1);
+	cpunr =  cpu + (cluster * EXYNOS5420_CPUS_PER_CLUSTER);
+
+	pr_debug("%s: cpu %u cluster %u\n", __func__, cpu, cluster);
+	BUG_ON(cpu >= EXYNOS5420_CPUS_PER_CLUSTER ||
+			cluster >= EXYNOS5420_NR_CLUSTERS);
+
+	__mcpm_cpu_going_down(cpu, cluster);
+
+	arch_spin_lock(&exynos_mcpm_lock);
+	BUG_ON(__mcpm_cluster_state(cluster) != CLUSTER_UP);
+	cpu_use_count[cpu][cluster]--;
+	if (cpu_use_count[cpu][cluster] == 0) {
+		exynos_cpu_power_down(cpunr);
+
+		if (exynos_cluster_unused(cluster))
+			/* TODO: Turn off the cluster here to save power. */
+			last_man = true;
+	} else if (cpu_use_count[cpu][cluster] == 1) {
+		/*
+		 * A power_up request went ahead of us.
+		 * Even if we do not want to shut this CPU down,
+		 * the caller expects a certain state as if the WFI
+		 * was aborted.  So let's continue with cache cleaning.
+		 */
+		skip_wfi = true;
+	} else {
+		BUG();
+	}
+
+	if (last_man && __mcpm_outbound_enter_critical(cpu, cluster)) {
+		arch_spin_unlock(&exynos_mcpm_lock);
+
+		if (read_cpuid_part_number() == ARM_CPU_PART_CORTEX_A15) {
+			/*
+			 * On the Cortex-A15 we need to disable
+			 * L2 prefetching before flushing the cache.
+			 */
+			asm volatile(
+			"mcr	p15, 1, %0, c15, c0, 3\n\t"
+			"isb\n\t"
+			"dsb"
+			: : "r" (0x400));
+		}
+
+		/* Flush all cache levels for this cluster. */
+		exynos_v7_exit_coherency_flush(all);
+
+		/*
+		 * Disable cluster-level coherency by masking
+		 * incoming snoops and DVM messages:
+		 */
+		cci_disable_port_by_cpu(mpidr);
+
+		__mcpm_outbound_leave_critical(cluster, CLUSTER_DOWN);
+	} else {
+		arch_spin_unlock(&exynos_mcpm_lock);
+
+		/* Disable and flush the local CPU cache. */
+		exynos_v7_exit_coherency_flush(louis);
+	}
+
+	__mcpm_cpu_down(cpu, cluster);
+
+	/* Now we are prepared for power-down, do it: */
+	if (!skip_wfi)
+		wfi();
+
+	/* Not dead at this point?  Let our caller cope. */
+}
+
+static int exynos_wait_for_powerdown(unsigned int cpu, unsigned int cluster)
+{
+	unsigned int tries = 100;
+	unsigned int cpunr = cpu + (cluster * EXYNOS5420_CPUS_PER_CLUSTER);
+
+	pr_debug("%s: cpu %u cluster %u\n", __func__, cpu, cluster);
+	BUG_ON(cpu >= EXYNOS5420_CPUS_PER_CLUSTER ||
+			cluster >= EXYNOS5420_NR_CLUSTERS);
+
+	/* Wait for the core state to be OFF */
+	while (tries--) {
+		if (ACCESS_ONCE(cpu_use_count[cpu][cluster]) == 0) {
+			if ((exynos_cpu_power_state(cpunr) == 0))
+				return 0; /* success: the CPU is halted */
+		}
+
+		/* Otherwise, wait and retry: */
+		msleep(1);
+	}
+
+	return -ETIMEDOUT; /* timeout */
+}
+
+static const struct mcpm_platform_ops exynos_power_ops = {
+	.power_up		= exynos_power_up,
+	.power_down		= exynos_power_down,
+	.wait_for_powerdown	= exynos_wait_for_powerdown,
+};
+
+static void __init exynos_mcpm_usage_count_init(void)
+{
+	unsigned int mpidr, cpu, cluster;
+
+	mpidr = read_cpuid_mpidr();
+	cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0);
+	cluster = MPIDR_AFFINITY_LEVEL(mpidr, 1);
+
+	pr_debug("%s: cpu %u cluster %u\n", __func__, cpu, cluster);
+	BUG_ON(cpu >= EXYNOS5420_CPUS_PER_CLUSTER  ||
+			cluster >= EXYNOS5420_NR_CLUSTERS);
+
+	cpu_use_count[cpu][cluster] = 1;
+}
+
+/*
+ * Enable cluster-level coherency, in preparation for turning on the MMU.
+ */
+static void __naked exynos_pm_power_up_setup(unsigned int affinity_level)
+{
+	asm volatile ("\n"
+	"cmp	r0, #1\n"
+	"bxne	lr\n"
+	"b	cci_enable_port_for_self");
+}
+
+static const struct of_device_id exynos_dt_mcpm_match[] = {
+	{ .compatible = "samsung,exynos5420" },
+	{ .compatible = "samsung,exynos5800" },
+	{},
+};
+
+static int __init exynos_mcpm_init(void)
+{
+	struct device_node *node;
+	void __iomem *ns_sram_base_addr;
+	int ret;
+
+	node = of_find_matching_node(NULL, exynos_dt_mcpm_match);
+	if (!node)
+		return -ENODEV;
+	of_node_put(node);
+
+	if (!cci_probed())
+		return -ENODEV;
+
+	node = of_find_compatible_node(NULL, NULL,
+			"samsung,exynos4210-sysram-ns");
+	if (!node)
+		return -ENODEV;
+
+	ns_sram_base_addr = of_iomap(node, 0);
+	of_node_put(node);
+	if (!ns_sram_base_addr) {
+		pr_err("failed to map non-secure iRAM base address\n");
+		return -ENOMEM;
+	}
+
+	/*
+	 * To increase the stability of KFC reset we need to program
+	 * the PMU SPARE3 register
+	 */
+	__raw_writel(EXYNOS5420_SWRESET_KFC_SEL, S5P_PMU_SPARE3);
+
+	exynos_mcpm_usage_count_init();
+
+	ret = mcpm_platform_register(&exynos_power_ops);
+	if (!ret)
+		ret = mcpm_sync_init(exynos_pm_power_up_setup);
+	if (ret) {
+		iounmap(ns_sram_base_addr);
+		return ret;
+	}
+
+	mcpm_smp_set_ops();
+
+	pr_info("Exynos MCPM support installed\n");
+
+	/*
+	 * Future entries into the kernel can now go
+	 * through the cluster entry vectors.
+	 */
+	__raw_writel(virt_to_phys(mcpm_entry_point),
+			ns_sram_base_addr + MCPM_BOOT_ADDR_OFFSET);
+
+	iounmap(ns_sram_base_addr);
+
+	return ret;
+}
+
+early_initcall(exynos_mcpm_init);
diff --git a/arch/arm/mach-exynos/platsmp.c b/arch/arm/mach-exynos/platsmp.c
index 03e5e9f9470..ec02422e849 100644
--- a/arch/arm/mach-exynos/platsmp.c
+++ b/arch/arm/mach-exynos/platsmp.c
@@ -20,24 +20,45 @@
 #include <linux/jiffies.h>
 #include <linux/smp.h>
 #include <linux/io.h>
+#include <linux/of_address.h>
 
 #include <asm/cacheflush.h>
 #include <asm/smp_plat.h>
 #include <asm/smp_scu.h>
 #include <asm/firmware.h>
 
-#include <plat/cpu.h>
-
 #include "common.h"
 #include "regs-pmu.h"
 
 extern void exynos4_secondary_startup(void);
 
+void __iomem *sysram_base_addr;
+void __iomem *sysram_ns_base_addr;
+
+static void __init exynos_smp_prepare_sysram(void)
+{
+	struct device_node *node;
+
+	for_each_compatible_node(node, NULL, "samsung,exynos4210-sysram") {
+		if (!of_device_is_available(node))
+			continue;
+		sysram_base_addr = of_iomap(node, 0);
+		break;
+	}
+
+	for_each_compatible_node(node, NULL, "samsung,exynos4210-sysram-ns") {
+		if (!of_device_is_available(node))
+			continue;
+		sysram_ns_base_addr = of_iomap(node, 0);
+		break;
+	}
+}
+
 static inline void __iomem *cpu_boot_reg_base(void)
 {
 	if (soc_is_exynos4210() && samsung_rev() == EXYNOS4210_REV_1_1)
 		return S5P_INFORM5;
-	return S5P_VA_SYSRAM;
+	return sysram_base_addr;
 }
 
 static inline void __iomem *cpu_boot_reg(int cpu)
@@ -45,9 +66,11 @@ static inline void __iomem *cpu_boot_reg(int cpu)
 	void __iomem *boot_reg;
 
 	boot_reg = cpu_boot_reg_base();
+	if (!boot_reg)
+		return ERR_PTR(-ENODEV);
 	if (soc_is_exynos4412())
 		boot_reg += 4*cpu;
-	else if (soc_is_exynos5420())
+	else if (soc_is_exynos5420() || soc_is_exynos5800())
 		boot_reg += 4;
 	return boot_reg;
 }
@@ -90,6 +113,7 @@ static int exynos_boot_secondary(unsigned int cpu, struct task_struct *idle)
 {
 	unsigned long timeout;
 	unsigned long phys_cpu = cpu_logical_map(cpu);
+	int ret = -ENOSYS;
 
 	/*
 	 * Set synchronisation state between this boot processor
@@ -107,15 +131,12 @@ static int exynos_boot_secondary(unsigned int cpu, struct task_struct *idle)
 	 */
 	write_pen_release(phys_cpu);
 
-	if (!(__raw_readl(S5P_ARM_CORE1_STATUS) & S5P_CORE_LOCAL_PWR_EN)) {
-		__raw_writel(S5P_CORE_LOCAL_PWR_EN,
-			     S5P_ARM_CORE1_CONFIGURATION);
-
+	if (!exynos_cpu_power_state(cpu)) {
+		exynos_cpu_power_up(cpu);
 		timeout = 10;
 
 		/* wait max 10 ms until cpu1 is on */
-		while ((__raw_readl(S5P_ARM_CORE1_STATUS)
-			& S5P_CORE_LOCAL_PWR_EN) != S5P_CORE_LOCAL_PWR_EN) {
+		while (exynos_cpu_power_state(cpu) != S5P_CORE_LOCAL_PWR_EN) {
 			if (timeout-- == 0)
 				break;
 
@@ -146,8 +167,18 @@ static int exynos_boot_secondary(unsigned int cpu, struct task_struct *idle)
 		 * Try to set boot address using firmware first
 		 * and fall back to boot register if it fails.
 		 */
-		if (call_firmware_op(set_cpu_boot_addr, phys_cpu, boot_addr))
+		ret = call_firmware_op(set_cpu_boot_addr, phys_cpu, boot_addr);
+		if (ret && ret != -ENOSYS)
+			goto fail;
+		if (ret == -ENOSYS) {
+			void __iomem *boot_reg = cpu_boot_reg(phys_cpu);
+
+			if (IS_ERR(boot_reg)) {
+				ret = PTR_ERR(boot_reg);
+				goto fail;
+			}
 			__raw_writel(boot_addr, cpu_boot_reg(phys_cpu));
+		}
 
 		call_firmware_op(cpu_boot, phys_cpu);
 
@@ -163,9 +194,10 @@ static int exynos_boot_secondary(unsigned int cpu, struct task_struct *idle)
 	 * now the secondary core is starting up let it run its
 	 * calibrations, then wait for it to finish
 	 */
+fail:
 	spin_unlock(&boot_lock);
 
-	return pen_release != -1 ? -ENOSYS : 0;
+	return pen_release != -1 ? ret : 0;
 }
 
 /*
@@ -205,6 +237,8 @@ static void __init exynos_smp_prepare_cpus(unsigned int max_cpus)
 	if (read_cpuid_part_number() == ARM_CPU_PART_CORTEX_A9)
 		scu_enable(scu_base_addr());
 
+	exynos_smp_prepare_sysram();
+
 	/*
 	 * Write the address of secondary startup into the
 	 * system-wide flags register. The boot monitor waits
@@ -217,12 +251,21 @@ static void __init exynos_smp_prepare_cpus(unsigned int max_cpus)
 	for (i = 1; i < max_cpus; ++i) {
 		unsigned long phys_cpu;
 		unsigned long boot_addr;
+		int ret;
 
 		phys_cpu = cpu_logical_map(i);
 		boot_addr = virt_to_phys(exynos4_secondary_startup);
 
-		if (call_firmware_op(set_cpu_boot_addr, phys_cpu, boot_addr))
+		ret = call_firmware_op(set_cpu_boot_addr, phys_cpu, boot_addr);
+		if (ret && ret != -ENOSYS)
+			break;
+		if (ret == -ENOSYS) {
+			void __iomem *boot_reg = cpu_boot_reg(phys_cpu);
+
+			if (IS_ERR(boot_reg))
+				break;
 			__raw_writel(boot_addr, cpu_boot_reg(phys_cpu));
+		}
 	}
 }
 
diff --git a/arch/arm/mach-exynos/pm.c b/arch/arm/mach-exynos/pm.c
index 15af0ceb0a6..87c0d34c7fb 100644
--- a/arch/arm/mach-exynos/pm.c
+++ b/arch/arm/mach-exynos/pm.c
@@ -16,6 +16,7 @@
 #include <linux/init.h>
 #include <linux/suspend.h>
 #include <linux/syscore_ops.h>
+#include <linux/cpu_pm.h>
 #include <linux/io.h>
 #include <linux/irqchip/arm-gic.h>
 #include <linux/err.h>
@@ -26,7 +27,6 @@
 #include <asm/smp_scu.h>
 #include <asm/suspend.h>
 
-#include <plat/cpu.h>
 #include <plat/pm-common.h>
 #include <plat/pll.h>
 #include <plat/regs-srom.h>
@@ -100,9 +100,141 @@ static int exynos_irq_set_wake(struct irq_data *data, unsigned int state)
 	return -ENOENT;
 }
 
+/**
+ * exynos_core_power_down : power down the specified cpu
+ * @cpu : the cpu to power down
+ *
+ * Power down the specified cpu. The sequence must be finished by a
+ * call to cpu_do_idle()
+ *
+ */
+void exynos_cpu_power_down(int cpu)
+{
+	__raw_writel(0, EXYNOS_ARM_CORE_CONFIGURATION(cpu));
+}
+
+/**
+ * exynos_cpu_power_up : power up the specified cpu
+ * @cpu : the cpu to power up
+ *
+ * Power up the specified cpu
+ */
+void exynos_cpu_power_up(int cpu)
+{
+	__raw_writel(S5P_CORE_LOCAL_PWR_EN,
+		     EXYNOS_ARM_CORE_CONFIGURATION(cpu));
+}
+
+/**
+ * exynos_cpu_power_state : returns the power state of the cpu
+ * @cpu : the cpu to retrieve the power state from
+ *
+ */
+int exynos_cpu_power_state(int cpu)
+{
+	return (__raw_readl(EXYNOS_ARM_CORE_STATUS(cpu)) &
+			S5P_CORE_LOCAL_PWR_EN);
+}
+
+/**
+ * exynos_cluster_power_down : power down the specified cluster
+ * @cluster : the cluster to power down
+ */
+void exynos_cluster_power_down(int cluster)
+{
+	__raw_writel(0, EXYNOS_COMMON_CONFIGURATION(cluster));
+}
+
+/**
+ * exynos_cluster_power_up : power up the specified cluster
+ * @cluster : the cluster to power up
+ */
+void exynos_cluster_power_up(int cluster)
+{
+	__raw_writel(S5P_CORE_LOCAL_PWR_EN,
+		     EXYNOS_COMMON_CONFIGURATION(cluster));
+}
+
+/**
+ * exynos_cluster_power_state : returns the power state of the cluster
+ * @cluster : the cluster to retrieve the power state from
+ *
+ */
+int exynos_cluster_power_state(int cluster)
+{
+	return (__raw_readl(EXYNOS_COMMON_STATUS(cluster)) &
+			S5P_CORE_LOCAL_PWR_EN);
+}
+
+#define EXYNOS_BOOT_VECTOR_ADDR	(samsung_rev() == EXYNOS4210_REV_1_1 ? \
+			S5P_INFORM7 : (samsung_rev() == EXYNOS4210_REV_1_0 ? \
+			(sysram_base_addr + 0x24) : S5P_INFORM0))
+#define EXYNOS_BOOT_VECTOR_FLAG	(samsung_rev() == EXYNOS4210_REV_1_1 ? \
+			S5P_INFORM6 : (samsung_rev() == EXYNOS4210_REV_1_0 ? \
+			(sysram_base_addr + 0x20) : S5P_INFORM1))
+
+#define S5P_CHECK_AFTR  0xFCBA0D10
+#define S5P_CHECK_SLEEP 0x00000BAD
+
+/* Ext-GIC nIRQ/nFIQ is the only wakeup source in AFTR */
+static void exynos_set_wakeupmask(long mask)
+{
+	__raw_writel(mask, S5P_WAKEUP_MASK);
+}
+
+static void exynos_cpu_set_boot_vector(long flags)
+{
+	__raw_writel(virt_to_phys(exynos_cpu_resume), EXYNOS_BOOT_VECTOR_ADDR);
+	__raw_writel(flags, EXYNOS_BOOT_VECTOR_FLAG);
+}
+
+void exynos_enter_aftr(void)
+{
+	exynos_set_wakeupmask(0x0000ff3e);
+	exynos_cpu_set_boot_vector(S5P_CHECK_AFTR);
+	/* Set value of power down register for aftr mode */
+	exynos_sys_powerdown_conf(SYS_AFTR);
+}
+
 /* For Cortex-A9 Diagnostic and Power control register */
 static unsigned int save_arm_register[2];
 
+static void exynos_cpu_save_register(void)
+{
+	unsigned long tmp;
+
+	/* Save Power control register */
+	asm ("mrc p15, 0, %0, c15, c0, 0"
+	     : "=r" (tmp) : : "cc");
+
+	save_arm_register[0] = tmp;
+
+	/* Save Diagnostic register */
+	asm ("mrc p15, 0, %0, c15, c0, 1"
+	     : "=r" (tmp) : : "cc");
+
+	save_arm_register[1] = tmp;
+}
+
+static void exynos_cpu_restore_register(void)
+{
+	unsigned long tmp;
+
+	/* Restore Power control register */
+	tmp = save_arm_register[0];
+
+	asm volatile ("mcr p15, 0, %0, c15, c0, 0"
+		      : : "r" (tmp)
+		      : "cc");
+
+	/* Restore Diagnostic register */
+	tmp = save_arm_register[1];
+
+	asm volatile ("mcr p15, 0, %0, c15, c0, 1"
+		      : : "r" (tmp)
+		      : "cc");
+}
+
 static int exynos_cpu_suspend(unsigned long arg)
 {
 #ifdef CONFIG_CACHE_L2X0
@@ -147,37 +279,34 @@ static void exynos_pm_prepare(void)
 	__raw_writel(virt_to_phys(exynos_cpu_resume), S5P_INFORM0);
 }
 
-static int exynos_pm_suspend(void)
+static void exynos_pm_central_suspend(void)
 {
 	unsigned long tmp;
 
 	/* Setting Central Sequence Register for power down mode */
-
 	tmp = __raw_readl(S5P_CENTRAL_SEQ_CONFIGURATION);
 	tmp &= ~S5P_CENTRAL_LOWPWR_CFG;
 	__raw_writel(tmp, S5P_CENTRAL_SEQ_CONFIGURATION);
+}
+
+static int exynos_pm_suspend(void)
+{
+	unsigned long tmp;
+
+	exynos_pm_central_suspend();
 
 	/* Setting SEQ_OPTION register */
 
 	tmp = (S5P_USE_STANDBY_WFI0 | S5P_USE_STANDBY_WFE0);
 	__raw_writel(tmp, S5P_CENTRAL_SEQ_OPTION);
 
-	if (!soc_is_exynos5250()) {
-		/* Save Power control register */
-		asm ("mrc p15, 0, %0, c15, c0, 0"
-		     : "=r" (tmp) : : "cc");
-		save_arm_register[0] = tmp;
-
-		/* Save Diagnostic register */
-		asm ("mrc p15, 0, %0, c15, c0, 1"
-		     : "=r" (tmp) : : "cc");
-		save_arm_register[1] = tmp;
-	}
+	if (!soc_is_exynos5250())
+		exynos_cpu_save_register();
 
 	return 0;
 }
 
-static void exynos_pm_resume(void)
+static int exynos_pm_central_resume(void)
 {
 	unsigned long tmp;
 
@@ -194,22 +323,20 @@ static void exynos_pm_resume(void)
 		/* clear the wakeup state register */
 		__raw_writel(0x0, S5P_WAKEUP_STAT);
 		/* No need to perform below restore code */
-		goto early_wakeup;
-	}
-	if (!soc_is_exynos5250()) {
-		/* Restore Power control register */
-		tmp = save_arm_register[0];
-		asm volatile ("mcr p15, 0, %0, c15, c0, 0"
-			      : : "r" (tmp)
-			      : "cc");
-
-		/* Restore Diagnostic register */
-		tmp = save_arm_register[1];
-		asm volatile ("mcr p15, 0, %0, c15, c0, 1"
-			      : : "r" (tmp)
-			      : "cc");
+		return -1;
 	}
 
+	return 0;
+}
+
+static void exynos_pm_resume(void)
+{
+	if (exynos_pm_central_resume())
+		goto early_wakeup;
+
+	if (!soc_is_exynos5250())
+		exynos_cpu_restore_register();
+
 	/* For release retention */
 
 	__raw_writel((1 << 28), S5P_PAD_RET_MAUDIO_OPTION);
@@ -226,7 +353,7 @@ static void exynos_pm_resume(void)
 
 	s3c_pm_do_restore_core(exynos_core_save, ARRAY_SIZE(exynos_core_save));
 
-	if (IS_ENABLED(CONFIG_SMP) && !soc_is_exynos5250())
+	if (!soc_is_exynos5250())
 		scu_enable(S5P_VA_SCU);
 
 early_wakeup:
@@ -304,10 +431,42 @@ static const struct platform_suspend_ops exynos_suspend_ops = {
 	.valid		= suspend_valid_only_mem,
 };
 
+static int exynos_cpu_pm_notifier(struct notifier_block *self,
+				  unsigned long cmd, void *v)
+{
+	int cpu = smp_processor_id();
+
+	switch (cmd) {
+	case CPU_PM_ENTER:
+		if (cpu == 0) {
+			exynos_pm_central_suspend();
+			exynos_cpu_save_register();
+		}
+		break;
+
+	case CPU_PM_EXIT:
+		if (cpu == 0) {
+			if (!soc_is_exynos5250())
+				scu_enable(S5P_VA_SCU);
+			exynos_cpu_restore_register();
+			exynos_pm_central_resume();
+		}
+		break;
+	}
+
+	return NOTIFY_OK;
+}
+
+static struct notifier_block exynos_cpu_pm_notifier_block = {
+	.notifier_call = exynos_cpu_pm_notifier,
+};
+
 void __init exynos_pm_init(void)
 {
 	u32 tmp;
 
+	cpu_pm_register_notifier(&exynos_cpu_pm_notifier_block);
+
 	/* Platform-specific GIC callback */
 	gic_arch_extn.irq_set_wake = exynos_irq_set_wake;
 
diff --git a/arch/arm/mach-exynos/pmu.c b/arch/arm/mach-exynos/pmu.c
index 05c7ce15322..fb0deda3b3a 100644
--- a/arch/arm/mach-exynos/pmu.c
+++ b/arch/arm/mach-exynos/pmu.c
@@ -13,8 +13,6 @@
 #include <linux/kernel.h>
 #include <linux/bug.h>
 
-#include <plat/cpu.h>
-
 #include "common.h"
 #include "regs-pmu.h"
 
diff --git a/arch/arm/mach-exynos/regs-pmu.h b/arch/arm/mach-exynos/regs-pmu.h
index 4f6a2560d02..1d13b08708f 100644
--- a/arch/arm/mach-exynos/regs-pmu.h
+++ b/arch/arm/mach-exynos/regs-pmu.h
@@ -38,6 +38,7 @@
 #define S5P_INFORM5				S5P_PMUREG(0x0814)
 #define S5P_INFORM6				S5P_PMUREG(0x0818)
 #define S5P_INFORM7				S5P_PMUREG(0x081C)
+#define S5P_PMU_SPARE3				S5P_PMUREG(0x090C)
 
 #define S5P_ARM_CORE0_LOWPWR			S5P_PMUREG(0x1000)
 #define S5P_DIS_IRQ_CORE0			S5P_PMUREG(0x1004)
@@ -105,8 +106,17 @@
 #define S5P_GPS_LOWPWR				S5P_PMUREG(0x139C)
 #define S5P_GPS_ALIVE_LOWPWR			S5P_PMUREG(0x13A0)
 
-#define S5P_ARM_CORE1_CONFIGURATION		S5P_PMUREG(0x2080)
-#define S5P_ARM_CORE1_STATUS			S5P_PMUREG(0x2084)
+#define EXYNOS_ARM_CORE0_CONFIGURATION		S5P_PMUREG(0x2000)
+#define EXYNOS_ARM_CORE_CONFIGURATION(_nr)	\
+			(EXYNOS_ARM_CORE0_CONFIGURATION + (0x80 * (_nr)))
+#define EXYNOS_ARM_CORE_STATUS(_nr)		\
+			(EXYNOS_ARM_CORE_CONFIGURATION(_nr) + 0x4)
+
+#define EXYNOS_ARM_COMMON_CONFIGURATION		S5P_PMUREG(0x2500)
+#define EXYNOS_COMMON_CONFIGURATION(_nr)	\
+			(EXYNOS_ARM_COMMON_CONFIGURATION + (0x80 * (_nr)))
+#define EXYNOS_COMMON_STATUS(_nr)		\
+			(EXYNOS_COMMON_CONFIGURATION(_nr) + 0x4)
 
 #define S5P_PAD_RET_MAUDIO_OPTION		S5P_PMUREG(0x3028)
 #define S5P_PAD_RET_GPIO_OPTION			S5P_PMUREG(0x3108)
@@ -119,8 +129,6 @@
 #define S5P_CORE_LOCAL_PWR_EN			0x3
 #define S5P_INT_LOCAL_PWR_EN			0x7
 
-#define S5P_CHECK_SLEEP				0x00000BAD
-
 /* Only for EXYNOS4210 */
 #define S5P_CMU_CLKSTOP_LCD1_LOWPWR	S5P_PMUREG(0x1154)
 #define S5P_CMU_RESET_LCD1_LOWPWR	S5P_PMUREG(0x1174)
@@ -313,4 +321,6 @@
 
 #define EXYNOS5_OPTION_USE_RETENTION				(1 << 4)
 
+#define EXYNOS5420_SWRESET_KFC_SEL				0x3
+
 #endif /* __ASM_ARCH_REGS_PMU_H */
diff --git a/arch/arm/mach-imx/Kconfig b/arch/arm/mach-imx/Kconfig
index 5740296dc42..8d42eab76d5 100644
--- a/arch/arm/mach-imx/Kconfig
+++ b/arch/arm/mach-imx/Kconfig
@@ -702,61 +702,6 @@ endif
 
 if ARCH_MULTI_V7
 
-comment "i.MX51 machines:"
-
-config MACH_IMX51_DT
-	bool "Support i.MX51 platforms from device tree"
-	select SOC_IMX51
-	help
-	  Include support for Freescale i.MX51 based platforms
-	  using the device tree for discovery
-
-config MACH_MX51_BABBAGE
-	bool "Support MX51 BABBAGE platforms"
-	select IMX_HAVE_PLATFORM_FSL_USB2_UDC
-	select IMX_HAVE_PLATFORM_IMX2_WDT
-	select IMX_HAVE_PLATFORM_IMX_I2C
-	select IMX_HAVE_PLATFORM_IMX_UART
-	select IMX_HAVE_PLATFORM_MXC_EHCI
-	select IMX_HAVE_PLATFORM_SDHCI_ESDHC_IMX
-	select IMX_HAVE_PLATFORM_SPI_IMX
-	select SOC_IMX51
-	help
-	  Include support for MX51 Babbage platform, also known as MX51EVK in
-	  u-boot. This includes specific configurations for the board and its
-	  peripherals.
-
-config MACH_EUKREA_CPUIMX51SD
-	bool "Support Eukrea CPUIMX51SD module"
-	select IMX_HAVE_PLATFORM_FSL_USB2_UDC
-	select IMX_HAVE_PLATFORM_IMX2_WDT
-	select IMX_HAVE_PLATFORM_IMX_I2C
-	select IMX_HAVE_PLATFORM_IMX_UART
-	select IMX_HAVE_PLATFORM_MXC_EHCI
-	select IMX_HAVE_PLATFORM_MXC_NAND
-	select IMX_HAVE_PLATFORM_SPI_IMX
-	select SOC_IMX51
-	help
-	  Include support for Eukrea CPUIMX51SD platform. This includes
-	  specific configurations for the module and its peripherals.
-
-choice
-	prompt "Baseboard"
-	depends on MACH_EUKREA_CPUIMX51SD
-	default MACH_EUKREA_MBIMXSD51_BASEBOARD
-
-config MACH_EUKREA_MBIMXSD51_BASEBOARD
-	prompt "Eukrea MBIMXSD development board"
-	bool
-	select IMX_HAVE_PLATFORM_IMX_SSI
-	select IMX_HAVE_PLATFORM_SDHCI_ESDHC_IMX
-	select LEDS_GPIO_REGISTER
-	help
-	  This adds board specific devices that can be found on Eukrea's
-	  MBIMXSD evaluation board.
-
-endchoice
-
 comment "Device tree only"
 
 config	SOC_IMX50
@@ -768,6 +713,12 @@ config	SOC_IMX50
 	help
 	  This enables support for Freescale i.MX50 processor.
 
+config MACH_IMX51_DT
+	bool "i.MX51 support"
+	select SOC_IMX51
+	help
+	  This enables support for Freescale i.MX51 processor
+
 config	SOC_IMX53
 	bool "i.MX53 support"
 	select HAVE_IMX_SRC
@@ -796,7 +747,6 @@ config SOC_IMX6Q
 	select ARM_ERRATA_764369 if SMP
 	select HAVE_ARM_SCU if SMP
 	select HAVE_ARM_TWD if SMP
-	select MIGHT_HAVE_PCI
 	select PCI_DOMAINS if PCI
 	select PINCTRL_IMX6Q
 	select SOC_IMX6
@@ -812,6 +762,14 @@ config SOC_IMX6SL
 	help
 	  This enables support for Freescale i.MX6 SoloLite processor.
 
+config SOC_IMX6SX
+	bool "i.MX6 SoloX support"
+	select PINCTRL_IMX6SX
+	select SOC_IMX6
+
+	help
+	  This enables support for Freescale i.MX6 SoloX processor.
+
 config SOC_VF610
 	bool "Vybrid Family VF610 support"
 	select ARM_GIC
diff --git a/arch/arm/mach-imx/Makefile b/arch/arm/mach-imx/Makefile
index f4ed83032dd..bbe93bbfd00 100644
--- a/arch/arm/mach-imx/Makefile
+++ b/arch/arm/mach-imx/Makefile
@@ -101,6 +101,7 @@ obj-$(CONFIG_SMP) += headsmp.o platsmp.o
 obj-$(CONFIG_HOTPLUG_CPU) += hotplug.o
 obj-$(CONFIG_SOC_IMX6Q) += clk-imx6q.o mach-imx6q.o
 obj-$(CONFIG_SOC_IMX6SL) += clk-imx6sl.o mach-imx6sl.o
+obj-$(CONFIG_SOC_IMX6SX) += clk-imx6sx.o mach-imx6sx.o
 
 ifeq ($(CONFIG_SUSPEND),y)
 AFLAGS_suspend-imx6.o :=-Wa,-march=armv7-a
@@ -108,11 +109,6 @@ obj-$(CONFIG_SOC_IMX6) += suspend-imx6.o
 endif
 obj-$(CONFIG_SOC_IMX6) += pm-imx6.o
 
-# i.MX5 based machines
-obj-$(CONFIG_MACH_MX51_BABBAGE) += mach-mx51_babbage.o
-obj-$(CONFIG_MACH_EUKREA_CPUIMX51SD) += mach-cpuimx51sd.o
-obj-$(CONFIG_MACH_EUKREA_MBIMXSD51_BASEBOARD) += eukrea_mbimxsd51-baseboard.o
-
 obj-$(CONFIG_MACH_IMX51_DT) += imx51-dt.o
 obj-$(CONFIG_SOC_IMX50) += mach-imx50.o
 obj-$(CONFIG_SOC_IMX53) += mach-imx53.o
diff --git a/arch/arm/mach-imx/avic.c b/arch/arm/mach-imx/avic.c
index 8d1df2e4b7a..24b103c67f8 100644
--- a/arch/arm/mach-imx/avic.c
+++ b/arch/arm/mach-imx/avic.c
@@ -135,7 +135,7 @@ static __init void avic_init_gc(int idx, unsigned int irq_start)
 	irq_setup_generic_chip(gc, IRQ_MSK(32), 0, IRQ_NOREQUEST, 0);
 }
 
-asmlinkage void __exception_irq_entry avic_handle_irq(struct pt_regs *regs)
+static void __exception_irq_entry avic_handle_irq(struct pt_regs *regs)
 {
 	u32 nivector;
 
@@ -190,6 +190,8 @@ void __init mxc_init_irq(void __iomem *irqbase)
 	for (i = 0; i < 8; i++)
 		__raw_writel(0, avic_base + AVIC_NIPRIORITY(i));
 
+	set_handle_irq(avic_handle_irq);
+
 #ifdef CONFIG_FIQ
 	/* Initialize FIQ */
 	init_FIQ(FIQ_START);
diff --git a/arch/arm/mach-imx/clk-gate2.c b/arch/arm/mach-imx/clk-gate2.c
index a2ecc006b32..4ba587da89d 100644
--- a/arch/arm/mach-imx/clk-gate2.c
+++ b/arch/arm/mach-imx/clk-gate2.c
@@ -27,48 +27,61 @@
  * parent - fixed parent.  No clk_set_parent support
  */
 
-#define to_clk_gate(_hw) container_of(_hw, struct clk_gate, hw)
+struct clk_gate2 {
+	struct clk_hw hw;
+	void __iomem	*reg;
+	u8		bit_idx;
+	u8		flags;
+	spinlock_t	*lock;
+	unsigned int	*share_count;
+};
+
+#define to_clk_gate2(_hw) container_of(_hw, struct clk_gate2, hw)
 
 static int clk_gate2_enable(struct clk_hw *hw)
 {
-	struct clk_gate *gate = to_clk_gate(hw);
+	struct clk_gate2 *gate = to_clk_gate2(hw);
 	u32 reg;
 	unsigned long flags = 0;
 
-	if (gate->lock)
-		spin_lock_irqsave(gate->lock, flags);
+	spin_lock_irqsave(gate->lock, flags);
+
+	if (gate->share_count && (*gate->share_count)++ > 0)
+		goto out;
 
 	reg = readl(gate->reg);
 	reg |= 3 << gate->bit_idx;
 	writel(reg, gate->reg);
 
-	if (gate->lock)
-		spin_unlock_irqrestore(gate->lock, flags);
+out:
+	spin_unlock_irqrestore(gate->lock, flags);
 
 	return 0;
 }
 
 static void clk_gate2_disable(struct clk_hw *hw)
 {
-	struct clk_gate *gate = to_clk_gate(hw);
+	struct clk_gate2 *gate = to_clk_gate2(hw);
 	u32 reg;
 	unsigned long flags = 0;
 
-	if (gate->lock)
-		spin_lock_irqsave(gate->lock, flags);
+	spin_lock_irqsave(gate->lock, flags);
+
+	if (gate->share_count && --(*gate->share_count) > 0)
+		goto out;
 
 	reg = readl(gate->reg);
 	reg &= ~(3 << gate->bit_idx);
 	writel(reg, gate->reg);
 
-	if (gate->lock)
-		spin_unlock_irqrestore(gate->lock, flags);
+out:
+	spin_unlock_irqrestore(gate->lock, flags);
 }
 
 static int clk_gate2_is_enabled(struct clk_hw *hw)
 {
 	u32 reg;
-	struct clk_gate *gate = to_clk_gate(hw);
+	struct clk_gate2 *gate = to_clk_gate2(hw);
 
 	reg = readl(gate->reg);
 
@@ -87,21 +100,23 @@ static struct clk_ops clk_gate2_ops = {
 struct clk *clk_register_gate2(struct device *dev, const char *name,
 		const char *parent_name, unsigned long flags,
 		void __iomem *reg, u8 bit_idx,
-		u8 clk_gate2_flags, spinlock_t *lock)
+		u8 clk_gate2_flags, spinlock_t *lock,
+		unsigned int *share_count)
 {
-	struct clk_gate *gate;
+	struct clk_gate2 *gate;
 	struct clk *clk;
 	struct clk_init_data init;
 
-	gate = kzalloc(sizeof(struct clk_gate), GFP_KERNEL);
+	gate = kzalloc(sizeof(struct clk_gate2), GFP_KERNEL);
 	if (!gate)
 		return ERR_PTR(-ENOMEM);
 
-	/* struct clk_gate assignments */
+	/* struct clk_gate2 assignments */
 	gate->reg = reg;
 	gate->bit_idx = bit_idx;
 	gate->flags = clk_gate2_flags;
 	gate->lock = lock;
+	gate->share_count = share_count;
 
 	init.name = name;
 	init.ops = &clk_gate2_ops;
diff --git a/arch/arm/mach-imx/clk-imx1.c b/arch/arm/mach-imx/clk-imx1.c
index 15f9d223cf0..7f739be3de2 100644
--- a/arch/arm/mach-imx/clk-imx1.c
+++ b/arch/arm/mach-imx/clk-imx1.c
@@ -40,12 +40,14 @@
 #define SCM_GCCR	IO_ADDR_SCM(0xc)
 
 static const char *prem_sel_clks[] = { "clk32_premult", "clk16m", };
-static const char *clko_sel_clks[] = { "per1", "hclk", "clk48m", "clk16m", "prem",
-				"fclk", };
+static const char *clko_sel_clks[] = { "per1", "hclk", "clk48m", "clk16m",
+				       "prem", "fclk", };
+
 enum imx1_clks {
-	dummy, clk32, clk16m_ext, clk16m, clk32_premult, prem, mpll, spll, mcu,
-	fclk, hclk, clk48m, per1, per2, per3, clko, dma_gate, csi_gate,
-	mma_gate, usbd_gate, clk_max
+	dummy, clk32, clk16m_ext, clk16m, clk32_premult, prem, mpll, mpll_gate,
+	spll, spll_gate, mcu, fclk, hclk, clk48m, per1, per2, per3, clko,
+	uart3_gate, ssi2_gate, brom_gate, dma_gate, csi_gate, mma_gate,
+	usbd_gate, clk_max
 };
 
 static struct clk *clk[clk_max];
@@ -62,17 +64,22 @@ int __init mx1_clocks_init(unsigned long fref)
 	clk[prem] = imx_clk_mux("prem", CCM_CSCR, 16, 1, prem_sel_clks,
 			ARRAY_SIZE(prem_sel_clks));
 	clk[mpll] = imx_clk_pllv1("mpll", "clk32_premult", CCM_MPCTL0);
+	clk[mpll_gate] = imx_clk_gate("mpll_gate", "mpll", CCM_CSCR, 0);
 	clk[spll] = imx_clk_pllv1("spll", "prem", CCM_SPCTL0);
+	clk[spll_gate] = imx_clk_gate("spll_gate", "spll", CCM_CSCR, 1);
 	clk[mcu] = imx_clk_divider("mcu", "clk32_premult", CCM_CSCR, 15, 1);
-	clk[fclk] = imx_clk_divider("fclk", "mpll", CCM_CSCR, 15, 1);
-	clk[hclk] = imx_clk_divider("hclk", "spll", CCM_CSCR, 10, 4);
-	clk[clk48m] = imx_clk_divider("clk48m", "spll", CCM_CSCR, 26, 3);
-	clk[per1] = imx_clk_divider("per1", "spll", CCM_PCDR, 0, 4);
-	clk[per2] = imx_clk_divider("per2", "spll", CCM_PCDR, 4, 4);
-	clk[per3] = imx_clk_divider("per3", "spll", CCM_PCDR, 16, 7);
+	clk[fclk] = imx_clk_divider("fclk", "mpll_gate", CCM_CSCR, 15, 1);
+	clk[hclk] = imx_clk_divider("hclk", "spll_gate", CCM_CSCR, 10, 4);
+	clk[clk48m] = imx_clk_divider("clk48m", "spll_gate", CCM_CSCR, 26, 3);
+	clk[per1] = imx_clk_divider("per1", "spll_gate", CCM_PCDR, 0, 4);
+	clk[per2] = imx_clk_divider("per2", "spll_gate", CCM_PCDR, 4, 4);
+	clk[per3] = imx_clk_divider("per3", "spll_gate", CCM_PCDR, 16, 7);
 	clk[clko] = imx_clk_mux("clko", CCM_CSCR, 29, 3, clko_sel_clks,
 			ARRAY_SIZE(clko_sel_clks));
-	clk[dma_gate] = imx_clk_gate("dma_gate", "hclk", SCM_GCCR, 4);
+	clk[uart3_gate] = imx_clk_gate("uart3_gate", "hclk", SCM_GCCR, 6);
+	clk[ssi2_gate] = imx_clk_gate("ssi2_gate", "hclk", SCM_GCCR, 5);
+	clk[brom_gate] = imx_clk_gate("brom_gate", "hclk", SCM_GCCR, 4);
+	clk[dma_gate] = imx_clk_gate("dma_gate", "hclk", SCM_GCCR, 3);
 	clk[csi_gate] = imx_clk_gate("csi_gate", "hclk", SCM_GCCR, 2);
 	clk[mma_gate] = imx_clk_gate("mma_gate", "hclk", SCM_GCCR, 1);
 	clk[usbd_gate] = imx_clk_gate("usbd_gate", "clk48m", SCM_GCCR, 0);
@@ -84,9 +91,6 @@ int __init mx1_clocks_init(unsigned long fref)
 
 	clk_register_clkdev(clk[dma_gate], "ahb", "imx1-dma");
 	clk_register_clkdev(clk[hclk], "ipg", "imx1-dma");
-	clk_register_clkdev(clk[csi_gate], NULL, "mx1-camera.0");
-	clk_register_clkdev(clk[mma_gate], "mma", NULL);
-	clk_register_clkdev(clk[usbd_gate], NULL, "imx_udc.0");
 	clk_register_clkdev(clk[per1], "per", "imx-gpt.0");
 	clk_register_clkdev(clk[hclk], "ipg", "imx-gpt.0");
 	clk_register_clkdev(clk[per1], "per", "imx1-uart.0");
@@ -94,20 +98,15 @@ int __init mx1_clocks_init(unsigned long fref)
 	clk_register_clkdev(clk[per1], "per", "imx1-uart.1");
 	clk_register_clkdev(clk[hclk], "ipg", "imx1-uart.1");
 	clk_register_clkdev(clk[per1], "per", "imx1-uart.2");
-	clk_register_clkdev(clk[hclk], "ipg", "imx1-uart.2");
+	clk_register_clkdev(clk[uart3_gate], "ipg", "imx1-uart.2");
 	clk_register_clkdev(clk[hclk], NULL, "imx1-i2c.0");
 	clk_register_clkdev(clk[per2], "per", "imx1-cspi.0");
 	clk_register_clkdev(clk[dummy], "ipg", "imx1-cspi.0");
 	clk_register_clkdev(clk[per2], "per", "imx1-cspi.1");
 	clk_register_clkdev(clk[dummy], "ipg", "imx1-cspi.1");
-	clk_register_clkdev(clk[per2], NULL, "imx-mmc.0");
 	clk_register_clkdev(clk[per2], "per", "imx1-fb.0");
 	clk_register_clkdev(clk[dummy], "ipg", "imx1-fb.0");
 	clk_register_clkdev(clk[dummy], "ahb", "imx1-fb.0");
-	clk_register_clkdev(clk[hclk], "mshc", NULL);
-	clk_register_clkdev(clk[per3], "ssi", NULL);
-	clk_register_clkdev(clk[clk32], NULL, "imx1-rtc.0");
-	clk_register_clkdev(clk[clko], "clko", NULL);
 
 	mxc_timer_init(MX1_IO_ADDRESS(MX1_TIM1_BASE_ADDR), MX1_TIM1_INT);
 
diff --git a/arch/arm/mach-imx/clk-imx25.c b/arch/arm/mach-imx/clk-imx25.c
index dc36e6c2f1d..ae578c096ad 100644
--- a/arch/arm/mach-imx/clk-imx25.c
+++ b/arch/arm/mach-imx/clk-imx25.c
@@ -62,6 +62,10 @@ static struct clk_onecell_data clk_data;
 
 static const char *cpu_sel_clks[] = { "mpll", "mpll_cpu_3_4", };
 static const char *per_sel_clks[] = { "ahb", "upll", };
+static const char *cko_sel_clks[] = { "dummy", "osc", "cpu", "ahb",
+				      "ipg", "dummy", "dummy", "dummy",
+				      "dummy", "dummy", "per0", "per2",
+				      "per13", "per14", "usbotg_ahb", "dummy",};
 
 enum mx25_clks {
 	dummy, osc, mpll, upll, mpll_cpu_3_4, cpu_sel, cpu, ahb, usb_div, ipg,
@@ -82,7 +86,7 @@ enum mx25_clks {
 	pwm2_ipg, pwm3_ipg, pwm4_ipg, rngb_ipg, reserved16, scc_ipg, sdma_ipg,
 	sim1_ipg, sim2_ipg, slcdc_ipg, spba_ipg, ssi1_ipg, ssi2_ipg, tsc_ipg,
 	uart1_ipg, uart2_ipg, uart3_ipg, uart4_ipg, uart5_ipg, reserved17,
-	wdt_ipg, clk_max
+	wdt_ipg, cko_div, cko_sel, cko, clk_max
 };
 
 static struct clk *clk[clk_max];
@@ -117,6 +121,9 @@ static int __init __mx25_clocks_init(unsigned long osc_rate)
 	clk[per13_sel] = imx_clk_mux("per13_sel", ccm(CCM_MCR), 13, 1, per_sel_clks, ARRAY_SIZE(per_sel_clks));
 	clk[per14_sel] = imx_clk_mux("per14_sel", ccm(CCM_MCR), 14, 1, per_sel_clks, ARRAY_SIZE(per_sel_clks));
 	clk[per15_sel] = imx_clk_mux("per15_sel", ccm(CCM_MCR), 15, 1, per_sel_clks, ARRAY_SIZE(per_sel_clks));
+	clk[cko_div] = imx_clk_divider("cko_div", "cko_sel", ccm(CCM_MCR), 24, 6);
+	clk[cko_sel] = imx_clk_mux("cko_sel", ccm(CCM_MCR), 20, 4, cko_sel_clks, ARRAY_SIZE(cko_sel_clks));
+	clk[cko] = imx_clk_gate("cko", "cko_div", ccm(CCM_MCR),  30);
 	clk[per0] = imx_clk_divider("per0", "per0_sel", ccm(CCM_PCDR0), 0, 6);
 	clk[per1] = imx_clk_divider("per1", "per1_sel", ccm(CCM_PCDR0), 8, 6);
 	clk[per2] = imx_clk_divider("per2", "per2_sel", ccm(CCM_PCDR0), 16, 6);
@@ -230,6 +237,12 @@ static int __init __mx25_clocks_init(unsigned long osc_rate)
 	clk_register_clkdev(clk[ipg], "ipg", "imx-gpt.0");
 	clk_register_clkdev(clk[gpt_ipg_per], "per", "imx-gpt.0");
 
+	/*
+	 * Let's initially set up CLKO parent as ipg, since this configuration
+	 * is used on some imx25 board designs to clock the audio codec.
+	 */
+	clk_set_parent(clk[cko_sel], clk[ipg]);
+
 	return 0;
 }
 
@@ -304,8 +317,6 @@ int __init mx25_clocks_init(void)
 int __init mx25_clocks_init_dt(void)
 {
 	struct device_node *np;
-	void __iomem *base;
-	int irq;
 	unsigned long osc_rate = 24000000;
 
 	/* retrieve the freqency of fixed clocks from device tree */
@@ -325,12 +336,7 @@ int __init mx25_clocks_init_dt(void)
 
 	__mx25_clocks_init(osc_rate);
 
-	np = of_find_compatible_node(NULL, NULL, "fsl,imx25-gpt");
-	base = of_iomap(np, 0);
-	WARN_ON(!base);
-	irq = irq_of_parse_and_map(np, 0);
-
-	mxc_timer_init(base, irq);
+	mxc_timer_init_dt(of_find_compatible_node(NULL, NULL, "fsl,imx25-gpt"));
 
 	return 0;
 }
diff --git a/arch/arm/mach-imx/clk-imx27.c b/arch/arm/mach-imx/clk-imx27.c
index d2da8908b26..317a662626d 100644
--- a/arch/arm/mach-imx/clk-imx27.c
+++ b/arch/arm/mach-imx/clk-imx27.c
@@ -82,7 +82,8 @@ enum mx27_clks {
 	csi_ahb_gate, brom_ahb_gate, ata_ahb_gate, wdog_ipg_gate, usb_ipg_gate,
 	uart6_ipg_gate, uart5_ipg_gate, uart4_ipg_gate, uart3_ipg_gate,
 	uart2_ipg_gate, uart1_ipg_gate, ckih_div1p5, fpm, mpll_osc_sel,
-	mpll_sel, spll_gate, clk_max
+	mpll_sel, spll_gate, mshc_div, rtic_ipg_gate, mshc_ipg_gate,
+	rtic_ahb_gate, mshc_baud_gate, clk_max
 };
 
 static struct clk *clk[clk_max];
@@ -117,6 +118,7 @@ int __init mx27_clocks_init(unsigned long fref)
 		clk[ipg] = imx_clk_divider("ipg", "ahb", CCM_CSCR, 8, 1);
 	}
 
+	clk[mshc_div] = imx_clk_divider("mshc_div", "ahb", CCM_PCDR0, 0, 6);
 	clk[nfc_div] = imx_clk_divider("nfc_div", "ahb", CCM_PCDR0, 6, 4);
 	clk[per1_div] = imx_clk_divider("per1_div", "mpll_main2", CCM_PCDR1, 0, 6);
 	clk[per2_div] = imx_clk_divider("per2_div", "mpll_main2", CCM_PCDR1, 8, 6);
@@ -145,9 +147,11 @@ int __init mx27_clocks_init(unsigned long fref)
 	clk[sdhc1_ipg_gate] = imx_clk_gate("sdhc1_ipg_gate", "ipg", CCM_PCCR0, 5);
 	clk[scc_ipg_gate] = imx_clk_gate("scc_ipg_gate", "ipg", CCM_PCCR0, 6);
 	clk[sahara_ipg_gate] = imx_clk_gate("sahara_ipg_gate", "ipg", CCM_PCCR0, 7);
+	clk[rtic_ipg_gate] = imx_clk_gate("rtic_ipg_gate", "ipg", CCM_PCCR0, 8);
 	clk[rtc_ipg_gate] = imx_clk_gate("rtc_ipg_gate", "ipg", CCM_PCCR0, 9);
 	clk[pwm_ipg_gate] = imx_clk_gate("pwm_ipg_gate", "ipg", CCM_PCCR0, 11);
 	clk[owire_ipg_gate] = imx_clk_gate("owire_ipg_gate", "ipg", CCM_PCCR0, 12);
+	clk[mshc_ipg_gate] = imx_clk_gate("mshc_ipg_gate", "ipg", CCM_PCCR0, 13);
 	clk[lcdc_ipg_gate] = imx_clk_gate("lcdc_ipg_gate", "ipg", CCM_PCCR0, 14);
 	clk[kpp_ipg_gate] = imx_clk_gate("kpp_ipg_gate", "ipg", CCM_PCCR0, 15);
 	clk[iim_ipg_gate] = imx_clk_gate("iim_ipg_gate", "ipg", CCM_PCCR0, 16);
@@ -166,6 +170,7 @@ int __init mx27_clocks_init(unsigned long fref)
 	clk[cspi3_ipg_gate] = imx_clk_gate("cspi3_ipg_gate", "ipg", CCM_PCCR0, 29);
 	clk[cspi2_ipg_gate] = imx_clk_gate("cspi2_ipg_gate", "ipg", CCM_PCCR0, 30);
 	clk[cspi1_ipg_gate] = imx_clk_gate("cspi1_ipg_gate", "ipg", CCM_PCCR0, 31);
+	clk[mshc_baud_gate] = imx_clk_gate("mshc_baud_gate", "mshc_div", CCM_PCCR1, 2);
 	clk[nfc_baud_gate] = imx_clk_gate("nfc_baud_gate", "nfc_div", CCM_PCCR1,  3);
 	clk[ssi2_baud_gate] = imx_clk_gate("ssi2_baud_gate", "ssi2_div", CCM_PCCR1,  4);
 	clk[ssi1_baud_gate] = imx_clk_gate("ssi1_baud_gate", "ssi1_div", CCM_PCCR1,  5);
@@ -177,6 +182,7 @@ int __init mx27_clocks_init(unsigned long fref)
 	clk[usb_ahb_gate] = imx_clk_gate("usb_ahb_gate", "ahb", CCM_PCCR1, 11);
 	clk[slcdc_ahb_gate] = imx_clk_gate("slcdc_ahb_gate", "ahb", CCM_PCCR1, 12);
 	clk[sahara_ahb_gate] = imx_clk_gate("sahara_ahb_gate", "ahb", CCM_PCCR1, 13);
+	clk[rtic_ahb_gate] = imx_clk_gate("rtic_ahb_gate", "ahb", CCM_PCCR1, 14);
 	clk[lcdc_ahb_gate] = imx_clk_gate("lcdc_ahb_gate", "ahb", CCM_PCCR1, 15);
 	clk[vpu_ahb_gate] = imx_clk_gate("vpu_ahb_gate", "ahb", CCM_PCCR1, 16);
 	clk[fec_ahb_gate] = imx_clk_gate("fec_ahb_gate", "ahb", CCM_PCCR1, 17);
@@ -221,16 +227,6 @@ int __init mx27_clocks_init(unsigned long fref)
 	clk_register_clkdev(clk[per1_gate], "per", "imx21-uart.5");
 	clk_register_clkdev(clk[gpt1_ipg_gate], "ipg", "imx-gpt.0");
 	clk_register_clkdev(clk[per1_gate], "per", "imx-gpt.0");
-	clk_register_clkdev(clk[gpt2_ipg_gate], "ipg", "imx-gpt.1");
-	clk_register_clkdev(clk[per1_gate], "per", "imx-gpt.1");
-	clk_register_clkdev(clk[gpt3_ipg_gate], "ipg", "imx-gpt.2");
-	clk_register_clkdev(clk[per1_gate], "per", "imx-gpt.2");
-	clk_register_clkdev(clk[gpt4_ipg_gate], "ipg", "imx-gpt.3");
-	clk_register_clkdev(clk[per1_gate], "per", "imx-gpt.3");
-	clk_register_clkdev(clk[gpt5_ipg_gate], "ipg", "imx-gpt.4");
-	clk_register_clkdev(clk[per1_gate], "per", "imx-gpt.4");
-	clk_register_clkdev(clk[gpt6_ipg_gate], "ipg", "imx-gpt.5");
-	clk_register_clkdev(clk[per1_gate], "per", "imx-gpt.5");
 	clk_register_clkdev(clk[per2_gate], "per", "imx21-mmc.0");
 	clk_register_clkdev(clk[sdhc1_ipg_gate], "ipg", "imx21-mmc.0");
 	clk_register_clkdev(clk[per2_gate], "per", "imx21-mmc.1");
@@ -278,14 +274,7 @@ int __init mx27_clocks_init(unsigned long fref)
 	clk_register_clkdev(clk[emma_ipg_gate], "emma-ipg", "imx27-camera.0");
 	clk_register_clkdev(clk[emma_ahb_gate], "ahb", "m2m-emmaprp.0");
 	clk_register_clkdev(clk[emma_ipg_gate], "ipg", "m2m-emmaprp.0");
-	clk_register_clkdev(clk[iim_ipg_gate], "iim", NULL);
-	clk_register_clkdev(clk[gpio_ipg_gate], "gpio", NULL);
-	clk_register_clkdev(clk[brom_ahb_gate], "brom", NULL);
-	clk_register_clkdev(clk[ata_ahb_gate], "ata", NULL);
-	clk_register_clkdev(clk[rtc_ipg_gate], NULL, "imx21-rtc");
-	clk_register_clkdev(clk[scc_ipg_gate], "scc", NULL);
 	clk_register_clkdev(clk[cpu_div], NULL, "cpu0");
-	clk_register_clkdev(clk[emi_ahb_gate], "emi_ahb" , NULL);
 
 	mxc_timer_init(MX27_IO_ADDRESS(MX27_GPT1_BASE_ADDR), MX27_INT_GPT1);
 
@@ -296,7 +285,6 @@ int __init mx27_clocks_init(unsigned long fref)
 	return 0;
 }
 
-#ifdef CONFIG_OF
 int __init mx27_clocks_init_dt(void)
 {
 	struct device_node *np;
@@ -312,4 +300,3 @@ int __init mx27_clocks_init_dt(void)
 
 	return mx27_clocks_init(fref);
 }
-#endif
diff --git a/arch/arm/mach-imx/clk-imx31.c b/arch/arm/mach-imx/clk-imx31.c
index b5b65f3efaf..4a9de0835eb 100644
--- a/arch/arm/mach-imx/clk-imx31.c
+++ b/arch/arm/mach-imx/clk-imx31.c
@@ -191,7 +191,6 @@ int __init mx31_clocks_init(unsigned long fref)
 	return 0;
 }
 
-#ifdef CONFIG_OF
 int __init mx31_clocks_init_dt(void)
 {
 	struct device_node *np;
@@ -207,4 +206,3 @@ int __init mx31_clocks_init_dt(void)
 
 	return mx31_clocks_init(fref);
 }
-#endif
diff --git a/arch/arm/mach-imx/clk-imx51-imx53.c b/arch/arm/mach-imx/clk-imx51-imx53.c
index 568ef0a4de8..21d2b111c83 100644
--- a/arch/arm/mach-imx/clk-imx51-imx53.c
+++ b/arch/arm/mach-imx/clk-imx51-imx53.c
@@ -322,9 +322,8 @@ static void __init mx5_clocks_common_init(unsigned long rate_ckil,
 
 static void __init mx50_clocks_init(struct device_node *np)
 {
-	void __iomem *base;
 	unsigned long r;
-	int i, irq;
+	int i;
 
 	clk[IMX5_CLK_PLL1_SW]		= imx_clk_pllv2("pll1_sw", "osc", MX53_DPLL1_BASE);
 	clk[IMX5_CLK_PLL2_SW]		= imx_clk_pllv2("pll2_sw", "osc", MX53_DPLL2_BASE);
@@ -372,11 +371,7 @@ static void __init mx50_clocks_init(struct device_node *np)
 	r = clk_round_rate(clk[IMX5_CLK_USBOH3_PER_GATE], 54000000);
 	clk_set_rate(clk[IMX5_CLK_USBOH3_PER_GATE], r);
 
-	np = of_find_compatible_node(NULL, NULL, "fsl,imx50-gpt");
-	base = of_iomap(np, 0);
-	WARN_ON(!base);
-	irq = irq_of_parse_and_map(np, 0);
-	mxc_timer_init(base, irq);
+	mxc_timer_init_dt(of_find_compatible_node(NULL, NULL, "fsl,imx50-gpt"));
 }
 CLK_OF_DECLARE(imx50_ccm, "fsl,imx50-ccm", mx50_clocks_init);
 
@@ -436,7 +431,6 @@ int __init mx51_clocks_init(unsigned long rate_ckil, unsigned long rate_osc,
 
 	clk_register_clkdev(clk[IMX5_CLK_HSI2C_GATE], NULL, "imx21-i2c.2");
 	clk_register_clkdev(clk[IMX5_CLK_MX51_MIPI], "mipi_hsp", NULL);
-	clk_register_clkdev(clk[IMX5_CLK_VPU_GATE], NULL, "imx51-vpu.0");
 	clk_register_clkdev(clk[IMX5_CLK_FEC_GATE], NULL, "imx27-fec.0");
 	clk_register_clkdev(clk[IMX5_CLK_USB_PHY_GATE], "phy", "mxc-ehci.0");
 	clk_register_clkdev(clk[IMX5_CLK_ESDHC1_IPG_GATE], "ipg", "sdhci-esdhc-imx51.0");
@@ -492,9 +486,8 @@ CLK_OF_DECLARE(imx51_ccm, "fsl,imx51-ccm", mx51_clocks_init_dt);
 
 static void __init mx53_clocks_init(struct device_node *np)
 {
-	int i, irq;
+	int i;
 	unsigned long r;
-	void __iomem *base;
 
 	clk[IMX5_CLK_PLL1_SW]		= imx_clk_pllv2("pll1_sw", "osc", MX53_DPLL1_BASE);
 	clk[IMX5_CLK_PLL2_SW]		= imx_clk_pllv2("pll2_sw", "osc", MX53_DPLL2_BASE);
@@ -561,7 +554,6 @@ static void __init mx53_clocks_init(struct device_node *np)
 
 	mx5_clocks_common_init(0, 0, 0, 0);
 
-	clk_register_clkdev(clk[IMX5_CLK_VPU_GATE], NULL, "imx53-vpu.0");
 	clk_register_clkdev(clk[IMX5_CLK_I2C3_GATE], NULL, "imx21-i2c.2");
 	clk_register_clkdev(clk[IMX5_CLK_FEC_GATE], NULL, "imx25-fec.0");
 	clk_register_clkdev(clk[IMX5_CLK_USB_PHY1_GATE], "usb_phy1", "mxc-ehci.0");
@@ -592,10 +584,6 @@ static void __init mx53_clocks_init(struct device_node *np)
 	r = clk_round_rate(clk[IMX5_CLK_USBOH3_PER_GATE], 54000000);
 	clk_set_rate(clk[IMX5_CLK_USBOH3_PER_GATE], r);
 
-	np = of_find_compatible_node(NULL, NULL, "fsl,imx53-gpt");
-	base = of_iomap(np, 0);
-	WARN_ON(!base);
-	irq = irq_of_parse_and_map(np, 0);
-	mxc_timer_init(base, irq);
+	mxc_timer_init_dt(of_find_compatible_node(NULL, NULL, "fsl,imx53-gpt"));
 }
 CLK_OF_DECLARE(imx53_ccm, "fsl,imx53-ccm", mx53_clocks_init);
diff --git a/arch/arm/mach-imx/clk-imx6q.c b/arch/arm/mach-imx/clk-imx6q.c
index b0e7f9d2c24..8e795dea02e 100644
--- a/arch/arm/mach-imx/clk-imx6q.c
+++ b/arch/arm/mach-imx/clk-imx6q.c
@@ -107,7 +107,7 @@ enum mx6q_clks {
 	sata_ref, sata_ref_100m, pcie_ref, pcie_ref_125m, enet_ref, usbphy1_gate,
 	usbphy2_gate, pll4_post_div, pll5_post_div, pll5_video_div, eim_slow,
 	spdif, cko2_sel, cko2_podf, cko2, cko, vdoa, pll4_audio_div,
-	lvds1_sel, lvds2_sel, lvds1_gate, lvds2_gate, clk_max
+	lvds1_sel, lvds2_sel, lvds1_gate, lvds2_gate, esai_ahb, clk_max
 };
 
 static struct clk *clk[clk_max];
@@ -140,11 +140,13 @@ static struct clk_div_table video_div_table[] = {
 	{ /* sentinel */ }
 };
 
+static unsigned int share_count_esai;
+
 static void __init imx6q_clocks_init(struct device_node *ccm_node)
 {
 	struct device_node *np;
 	void __iomem *base;
-	int i, irq;
+	int i;
 	int ret;
 
 	clk[dummy] = imx_clk_fixed("dummy", 0);
@@ -208,8 +210,8 @@ static void __init imx6q_clocks_init(struct device_node *ccm_node)
 	 * the "output_enable" bit as a gate, even though it's really just
 	 * enabling clock output.
 	 */
-	clk[lvds1_gate] = imx_clk_gate("lvds1_gate", "dummy", base + 0x160, 10);
-	clk[lvds2_gate] = imx_clk_gate("lvds2_gate", "dummy", base + 0x160, 11);
+	clk[lvds1_gate] = imx_clk_gate("lvds1_gate", "lvds1_sel", base + 0x160, 10);
+	clk[lvds2_gate] = imx_clk_gate("lvds2_gate", "lvds2_sel", base + 0x160, 11);
 
 	/*                                name              parent_name        reg       idx */
 	clk[pll2_pfd0_352m] = imx_clk_pfd("pll2_pfd0_352m", "pll2_bus",     base + 0x100, 0);
@@ -258,14 +260,14 @@ static void __init imx6q_clocks_init(struct device_node *ccm_node)
 	clk[ipu2_sel]         = imx_clk_mux("ipu2_sel",         base + 0x3c, 14, 2, ipu_sels,          ARRAY_SIZE(ipu_sels));
 	clk[ldb_di0_sel]      = imx_clk_mux_flags("ldb_di0_sel", base + 0x2c, 9,  3, ldb_di_sels,      ARRAY_SIZE(ldb_di_sels), CLK_SET_RATE_PARENT);
 	clk[ldb_di1_sel]      = imx_clk_mux_flags("ldb_di1_sel", base + 0x2c, 12, 3, ldb_di_sels,      ARRAY_SIZE(ldb_di_sels), CLK_SET_RATE_PARENT);
-	clk[ipu1_di0_pre_sel] = imx_clk_mux("ipu1_di0_pre_sel", base + 0x34, 6,  3, ipu_di_pre_sels,   ARRAY_SIZE(ipu_di_pre_sels));
-	clk[ipu1_di1_pre_sel] = imx_clk_mux("ipu1_di1_pre_sel", base + 0x34, 15, 3, ipu_di_pre_sels,   ARRAY_SIZE(ipu_di_pre_sels));
-	clk[ipu2_di0_pre_sel] = imx_clk_mux("ipu2_di0_pre_sel", base + 0x38, 6,  3, ipu_di_pre_sels,   ARRAY_SIZE(ipu_di_pre_sels));
-	clk[ipu2_di1_pre_sel] = imx_clk_mux("ipu2_di1_pre_sel", base + 0x38, 15, 3, ipu_di_pre_sels,   ARRAY_SIZE(ipu_di_pre_sels));
-	clk[ipu1_di0_sel]     = imx_clk_mux("ipu1_di0_sel",     base + 0x34, 0,  3, ipu1_di0_sels,     ARRAY_SIZE(ipu1_di0_sels));
-	clk[ipu1_di1_sel]     = imx_clk_mux("ipu1_di1_sel",     base + 0x34, 9,  3, ipu1_di1_sels,     ARRAY_SIZE(ipu1_di1_sels));
-	clk[ipu2_di0_sel]     = imx_clk_mux("ipu2_di0_sel",     base + 0x38, 0,  3, ipu2_di0_sels,     ARRAY_SIZE(ipu2_di0_sels));
-	clk[ipu2_di1_sel]     = imx_clk_mux("ipu2_di1_sel",     base + 0x38, 9,  3, ipu2_di1_sels,     ARRAY_SIZE(ipu2_di1_sels));
+	clk[ipu1_di0_pre_sel] = imx_clk_mux_flags("ipu1_di0_pre_sel", base + 0x34, 6,  3, ipu_di_pre_sels,   ARRAY_SIZE(ipu_di_pre_sels), CLK_SET_RATE_PARENT);
+	clk[ipu1_di1_pre_sel] = imx_clk_mux_flags("ipu1_di1_pre_sel", base + 0x34, 15, 3, ipu_di_pre_sels,   ARRAY_SIZE(ipu_di_pre_sels), CLK_SET_RATE_PARENT);
+	clk[ipu2_di0_pre_sel] = imx_clk_mux_flags("ipu2_di0_pre_sel", base + 0x38, 6,  3, ipu_di_pre_sels,   ARRAY_SIZE(ipu_di_pre_sels), CLK_SET_RATE_PARENT);
+	clk[ipu2_di1_pre_sel] = imx_clk_mux_flags("ipu2_di1_pre_sel", base + 0x38, 15, 3, ipu_di_pre_sels,   ARRAY_SIZE(ipu_di_pre_sels), CLK_SET_RATE_PARENT);
+	clk[ipu1_di0_sel]     = imx_clk_mux_flags("ipu1_di0_sel",     base + 0x34, 0,  3, ipu1_di0_sels,     ARRAY_SIZE(ipu1_di0_sels), CLK_SET_RATE_PARENT);
+	clk[ipu1_di1_sel]     = imx_clk_mux_flags("ipu1_di1_sel",     base + 0x34, 9,  3, ipu1_di1_sels,     ARRAY_SIZE(ipu1_di1_sels), CLK_SET_RATE_PARENT);
+	clk[ipu2_di0_sel]     = imx_clk_mux_flags("ipu2_di0_sel",     base + 0x38, 0,  3, ipu2_di0_sels,     ARRAY_SIZE(ipu2_di0_sels), CLK_SET_RATE_PARENT);
+	clk[ipu2_di1_sel]     = imx_clk_mux_flags("ipu2_di1_sel",     base + 0x38, 9,  3, ipu2_di1_sels,     ARRAY_SIZE(ipu2_di1_sels), CLK_SET_RATE_PARENT);
 	clk[hsi_tx_sel]       = imx_clk_mux("hsi_tx_sel",       base + 0x30, 28, 1, hsi_tx_sels,       ARRAY_SIZE(hsi_tx_sels));
 	clk[pcie_axi_sel]     = imx_clk_mux("pcie_axi_sel",     base + 0x18, 10, 1, pcie_axi_sels,     ARRAY_SIZE(pcie_axi_sels));
 	clk[ssi1_sel]         = imx_clk_fixup_mux("ssi1_sel",   base + 0x1c, 10, 2, ssi_sels,          ARRAY_SIZE(ssi_sels),          imx_cscmr1_fixup);
@@ -352,9 +354,14 @@ static void __init imx6q_clocks_init(struct device_node *ccm_node)
 	clk[ecspi2]       = imx_clk_gate2("ecspi2",        "ecspi_root",        base + 0x6c, 2);
 	clk[ecspi3]       = imx_clk_gate2("ecspi3",        "ecspi_root",        base + 0x6c, 4);
 	clk[ecspi4]       = imx_clk_gate2("ecspi4",        "ecspi_root",        base + 0x6c, 6);
-	clk[ecspi5]       = imx_clk_gate2("ecspi5",        "ecspi_root",        base + 0x6c, 8);
+	if (cpu_is_imx6dl())
+		/* ecspi5 is replaced with i2c4 on imx6dl & imx6s */
+		clk[ecspi5] = imx_clk_gate2("i2c4",        "ipg_per",           base + 0x6c, 8);
+	else
+		clk[ecspi5] = imx_clk_gate2("ecspi5",      "ecspi_root",        base + 0x6c, 8);
 	clk[enet]         = imx_clk_gate2("enet",          "ipg",               base + 0x6c, 10);
-	clk[esai]         = imx_clk_gate2("esai",          "esai_podf",         base + 0x6c, 16);
+	clk[esai]         = imx_clk_gate2_shared("esai",   "esai_podf",         base + 0x6c, 16, &share_count_esai);
+	clk[esai_ahb]     = imx_clk_gate2_shared("esai_ahb", "ahb",             base + 0x6c, 16, &share_count_esai);
 	clk[gpt_ipg]      = imx_clk_gate2("gpt_ipg",       "ipg",               base + 0x6c, 20);
 	clk[gpt_ipg_per]  = imx_clk_gate2("gpt_ipg_per",   "ipg_per",           base + 0x6c, 22);
 	if (cpu_is_imx6dl())
@@ -445,6 +452,15 @@ static void __init imx6q_clocks_init(struct device_node *ccm_node)
 		clk_set_parent(clk[ldb_di1_sel], clk[pll5_video_div]);
 	}
 
+	clk_set_parent(clk[ipu1_di0_pre_sel], clk[pll5_video_div]);
+	clk_set_parent(clk[ipu1_di1_pre_sel], clk[pll5_video_div]);
+	clk_set_parent(clk[ipu2_di0_pre_sel], clk[pll5_video_div]);
+	clk_set_parent(clk[ipu2_di1_pre_sel], clk[pll5_video_div]);
+	clk_set_parent(clk[ipu1_di0_sel], clk[ipu1_di0_pre]);
+	clk_set_parent(clk[ipu1_di1_sel], clk[ipu1_di1_pre]);
+	clk_set_parent(clk[ipu2_di0_sel], clk[ipu2_di0_pre]);
+	clk_set_parent(clk[ipu2_di1_sel], clk[ipu2_di1_pre]);
+
 	/*
 	 * The gpmi needs 100MHz frequency in the EDO/Sync mode,
 	 * We can not get the 100MHz from the pll2_pfd0_352m.
@@ -480,10 +496,6 @@ static void __init imx6q_clocks_init(struct device_node *ccm_node)
 	/* Set initial power mode */
 	imx6q_set_lpm(WAIT_CLOCKED);
 
-	np = of_find_compatible_node(NULL, NULL, "fsl,imx6q-gpt");
-	base = of_iomap(np, 0);
-	WARN_ON(!base);
-	irq = irq_of_parse_and_map(np, 0);
-	mxc_timer_init(base, irq);
+	mxc_timer_init_dt(of_find_compatible_node(NULL, NULL, "fsl,imx6q-gpt"));
 }
 CLK_OF_DECLARE(imx6q, "fsl,imx6q-ccm", imx6q_clocks_init);
diff --git a/arch/arm/mach-imx/clk-imx6sl.c b/arch/arm/mach-imx/clk-imx6sl.c
index f7073c0782f..21cf06cebad 100644
--- a/arch/arm/mach-imx/clk-imx6sl.c
+++ b/arch/arm/mach-imx/clk-imx6sl.c
@@ -169,7 +169,6 @@ static void __init imx6sl_clocks_init(struct device_node *ccm_node)
 {
 	struct device_node *np;
 	void __iomem *base;
-	int irq;
 	int i;
 	int ret;
 
@@ -385,9 +384,6 @@ static void __init imx6sl_clocks_init(struct device_node *ccm_node)
 	imx6q_set_lpm(WAIT_CLOCKED);
 
 	np = of_find_compatible_node(NULL, NULL, "fsl,imx6sl-gpt");
-	base = of_iomap(np, 0);
-	WARN_ON(!base);
-	irq = irq_of_parse_and_map(np, 0);
-	mxc_timer_init(base, irq);
+	mxc_timer_init_dt(np);
 }
 CLK_OF_DECLARE(imx6sl, "fsl,imx6sl-ccm", imx6sl_clocks_init);
diff --git a/arch/arm/mach-imx/clk-imx6sx.c b/arch/arm/mach-imx/clk-imx6sx.c
new file mode 100644
index 00000000000..72f8902235d
--- /dev/null
+++ b/arch/arm/mach-imx/clk-imx6sx.c
@@ -0,0 +1,524 @@
+/*
+ * Copyright (C) 2014 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 <dt-bindings/clock/imx6sx-clock.h>
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/types.h>
+
+#include "clk.h"
+#include "common.h"
+
+#define CCDR    0x4
+#define BM_CCM_CCDR_MMDC_CH0_MASK       (0x2 << 16)
+
+static const char *step_sels[]		= { "osc", "pll2_pfd2_396m", };
+static const char *pll1_sw_sels[]	= { "pll1_sys", "step", };
+static const char *periph_pre_sels[]	= { "pll2_bus", "pll2_pfd2_396m", "pll2_pfd0_352m", "pll2_198m", };
+static const char *periph2_pre_sels[]	= { "pll2_bus", "pll2_pfd2_396m", "pll2_pfd0_352m", "pll4_audio_div", };
+static const char *periph_clk2_sels[]	= { "pll3_usb_otg", "osc", "osc", };
+static const char *periph2_clk2_sels[]	= { "pll3_usb_otg", "osc", };
+static const char *periph_sels[]	= { "periph_pre", "periph_clk2", };
+static const char *periph2_sels[]	= { "periph2_pre", "periph2_clk2", };
+static const char *ocram_sels[]		= { "periph", "pll2_pfd2_396m", "periph", "pll3_pfd1_540m", };
+static const char *audio_sels[]		= { "pll4_audio_div", "pll3_pfd2_508m", "pll5_video_div", "pll3_usb_otg", };
+static const char *gpu_axi_sels[]	= { "pll2_pfd2_396m", "pll3_pfd0_720m", "pll3_pfd1_540m", "pll2_bus", };
+static const char *gpu_core_sels[]	= { "pll3_pfd1_540m", "pll3_pfd0_720m", "pll2_bus", "pll2_pfd2_396m", };
+static const char *ldb_di0_div_sels[]	= { "ldb_di0_div_3_5", "ldb_di0_div_7", };
+static const char *ldb_di1_div_sels[]	= { "ldb_di1_div_3_5", "ldb_di1_div_7", };
+static const char *ldb_di0_sels[]	= { "pll5_video_div", "pll2_pfd0_352m", "pll2_pfd2_396m", "pll2_pfd3_594m", "pll2_pfd1_594m", "pll3_pfd3_454m", };
+static const char *ldb_di1_sels[]	= { "pll3_usb_otg", "pll2_pfd0_352m", "pll2_pfd2_396m", "pll2_bus", "pll3_pfd3_454m", "pll3_pfd2_508m", };
+static const char *pcie_axi_sels[]	= { "axi", "ahb", };
+static const char *ssi_sels[]		= { "pll3_pfd2_508m", "pll5_video_div", "pll4_audio_div", };
+static const char *qspi1_sels[]		= { "pll3_usb_otg", "pll2_pfd0_352m", "pll2_pfd2_396m", "pll2_bus", "pll3_pfd3_454m", "pll3_pfd2_508m", };
+static const char *perclk_sels[]	= { "ipg", "osc", };
+static const char *usdhc_sels[]		= { "pll2_pfd2_396m", "pll2_pfd0_352m", };
+static const char *vid_sels[]		= { "pll3_pfd1_540m", "pll3_usb_otg", "pll3_pfd3_454m", "pll4_audio_div", "pll5_video_div", };
+static const char *can_sels[]		= { "pll3_60m", "osc", "pll3_80m", "dummy", };
+static const char *uart_sels[]		= { "pll3_80m", "osc", };
+static const char *qspi2_sels[]		= { "pll2_pfd0_352m", "pll2_bus", "pll3_usb_otg", "pll2_pfd2_396m", "pll3_pfd3_454m", "dummy", "dummy", "dummy", };
+static const char *enet_pre_sels[]	= { "pll2_bus", "pll3_usb_otg", "pll5_video_div", "pll2_pfd0_352m", "pll2_pfd2_396m", "pll3_pfd2_508m", };
+static const char *enet_sels[]		= { "enet_podf", "ipp_di0", "ipp_di1", "ldb_di0", "ldb_di1", };
+static const char *m4_pre_sels[]	= { "pll2_bus", "pll3_usb_otg", "osc", "pll2_pfd0_352m", "pll2_pfd2_396m", "pll3_pfd3_454m", };
+static const char *m4_sels[]		= { "m4_pre_sel", "ipp_di0", "ipp_di1", "ldb_di0", "ldb_di1", };
+static const char *eim_slow_sels[]	= { "ocram", "pll3_usb_otg", "pll2_pfd2_396m", "pll2_pfd0_352m", };
+static const char *ecspi_sels[]		= { "pll3_60m", "osc", };
+static const char *lcdif1_pre_sels[]	= { "pll2_bus", "pll3_pfd3_454m", "pll5_video_div", "pll2_pfd0_352m", "pll2_pfd1_594m", "pll3_pfd1_540m", };
+static const char *lcdif1_sels[]	= { "lcdif1_podf", "ipp_di0", "ipp_di1", "ldb_di0", "ldb_di1", };
+static const char *lcdif2_pre_sels[]	= { "pll2_bus", "pll3_pfd3_454m", "pll5_video_div", "pll2_pfd0_352m", "pll2_pfd3_594m", "pll3_pfd1_540m", };
+static const char *lcdif2_sels[]	= { "lcdif2_podf", "ipp_di0", "ipp_di1", "ldb_di0", "ldb_di1", };
+static const char *display_sels[]	= { "pll2_bus", "pll2_pfd2_396m", "pll3_usb_otg", "pll3_pfd1_540m", };
+static const char *csi_sels[]		= { "osc", "pll2_pfd2_396m", "pll3_120m", "pll3_pfd1_540m", };
+static const char *cko1_sels[]		= {
+	"pll3_usb_otg", "pll2_bus", "pll1_sys", "pll5_video_div",
+	"dummy", "ocram", "dummy", "pxp_axi", "epdc_axi", "lcdif_pix",
+	"epdc_pix", "ahb", "ipg", "perclk", "ckil", "pll4_audio_div",
+};
+static const char *cko2_sels[]		= {
+	"dummy", "mmdc_p0_fast", "usdhc4", "usdhc1", "dummy", "wrck",
+	"ecspi_root", "dummy", "usdhc3", "pcie", "arm", "csi_core",
+	"lcdif_axi", "dummy", "osc", "dummy", "gpu2d_ovg_core",
+	"usdhc2", "ssi1", "ssi2", "ssi3", "gpu2d_core", "dummy",
+	"dummy", "dummy", "dummy", "esai_extal", "eim_slow", "uart_serial",
+	"spdif", "asrc", "dummy",
+};
+static const char *cko_sels[] = { "cko1", "cko2", };
+static const char *lvds_sels[]	= {
+	"arm", "pll1_sys", "dummy", "dummy", "dummy", "dummy", "dummy", "pll5_video_div",
+	"dummy", "dummy", "pcie_ref_125m", "dummy", "usbphy1", "usbphy2",
+};
+
+static struct clk *clks[IMX6SX_CLK_CLK_END];
+static struct clk_onecell_data clk_data;
+
+static int const clks_init_on[] __initconst = {
+	IMX6SX_CLK_AIPS_TZ1, IMX6SX_CLK_AIPS_TZ2, IMX6SX_CLK_AIPS_TZ3,
+	IMX6SX_CLK_IPMUX1, IMX6SX_CLK_IPMUX2, IMX6SX_CLK_IPMUX3,
+	IMX6SX_CLK_WAKEUP, IMX6SX_CLK_MMDC_P0_FAST, IMX6SX_CLK_MMDC_P0_IPG,
+	IMX6SX_CLK_ROM, IMX6SX_CLK_ARM, IMX6SX_CLK_IPG, IMX6SX_CLK_OCRAM,
+	IMX6SX_CLK_PER2_MAIN, IMX6SX_CLK_PERCLK, IMX6SX_CLK_M4,
+	IMX6SX_CLK_QSPI1, IMX6SX_CLK_QSPI2, IMX6SX_CLK_UART_IPG,
+	IMX6SX_CLK_UART_SERIAL, IMX6SX_CLK_I2C3, IMX6SX_CLK_ECSPI5,
+	IMX6SX_CLK_CAN1_IPG, IMX6SX_CLK_CAN1_SERIAL, IMX6SX_CLK_CAN2_IPG,
+	IMX6SX_CLK_CAN2_SERIAL, IMX6SX_CLK_CANFD, IMX6SX_CLK_EPIT1,
+	IMX6SX_CLK_EPIT2,
+};
+
+static struct clk_div_table clk_enet_ref_table[] = {
+	{ .val = 0, .div = 20, },
+	{ .val = 1, .div = 10, },
+	{ .val = 2, .div = 5, },
+	{ .val = 3, .div = 4, },
+	{ }
+};
+
+static struct clk_div_table post_div_table[] = {
+	{ .val = 2, .div = 1, },
+	{ .val = 1, .div = 2, },
+	{ .val = 0, .div = 4, },
+	{ }
+};
+
+static struct clk_div_table video_div_table[] = {
+	{ .val = 0, .div = 1, },
+	{ .val = 1, .div = 2, },
+	{ .val = 2, .div = 1, },
+	{ .val = 3, .div = 4, },
+	{ }
+};
+
+static u32 share_count_asrc;
+static u32 share_count_audio;
+static u32 share_count_esai;
+
+static void __init imx6sx_clocks_init(struct device_node *ccm_node)
+{
+	struct device_node *np;
+	void __iomem *base;
+	int i;
+
+	clks[IMX6SX_CLK_DUMMY] = imx_clk_fixed("dummy", 0);
+
+	clks[IMX6SX_CLK_CKIL] = of_clk_get_by_name(ccm_node, "ckil");
+	clks[IMX6SX_CLK_OSC] = of_clk_get_by_name(ccm_node, "osc");
+
+	/* ipp_di clock is external input */
+	clks[IMX6SX_CLK_IPP_DI0] = of_clk_get_by_name(ccm_node, "ipp_di0");
+	clks[IMX6SX_CLK_IPP_DI1] = of_clk_get_by_name(ccm_node, "ipp_di1");
+
+	np = of_find_compatible_node(NULL, NULL, "fsl,imx6sx-anatop");
+	base = of_iomap(np, 0);
+	WARN_ON(!base);
+
+	/*                                              type               name             parent_name   base         div_mask */
+	clks[IMX6SX_CLK_PLL1_SYS]       = imx_clk_pllv3(IMX_PLLV3_SYS,     "pll1_sys",      "osc",        base,        0x7f);
+	clks[IMX6SX_CLK_PLL2_BUS]       = imx_clk_pllv3(IMX_PLLV3_GENERIC, "pll2_bus",      "osc",        base + 0x30, 0x1);
+	clks[IMX6SX_CLK_PLL3_USB_OTG]   = imx_clk_pllv3(IMX_PLLV3_USB,     "pll3_usb_otg",  "osc",        base + 0x10, 0x3);
+	clks[IMX6SX_CLK_PLL4_AUDIO]     = imx_clk_pllv3(IMX_PLLV3_AV,      "pll4_audio",    "osc",        base + 0x70, 0x7f);
+	clks[IMX6SX_CLK_PLL5_VIDEO]     = imx_clk_pllv3(IMX_PLLV3_AV,      "pll5_video",    "osc",        base + 0xa0, 0x7f);
+	clks[IMX6SX_CLK_PLL6_ENET]      = imx_clk_pllv3(IMX_PLLV3_ENET,    "pll6_enet",     "osc",        base + 0xe0, 0x3);
+	clks[IMX6SX_CLK_PLL7_USB_HOST]  = imx_clk_pllv3(IMX_PLLV3_USB,     "pll7_usb_host", "osc",        base + 0x20, 0x3);
+
+	/*
+	 * Bit 20 is the reserved and read-only bit, we do this only for:
+	 * - Do nothing for usbphy clk_enable/disable
+	 * - Keep refcount when do usbphy clk_enable/disable, in that case,
+	 * the clk framework may need to enable/disable usbphy's parent
+	 */
+	clks[IMX6SX_CLK_USBPHY1] = imx_clk_gate("usbphy1", "pll3_usb_otg",  base + 0x10, 20);
+	clks[IMX6SX_CLK_USBPHY2] = imx_clk_gate("usbphy2", "pll7_usb_host", base + 0x20, 20);
+
+	/*
+	 * usbphy*_gate needs to be on after system boots up, and software
+	 * never needs to control it anymore.
+	 */
+	clks[IMX6SX_CLK_USBPHY1_GATE] = imx_clk_gate("usbphy1_gate", "dummy", base + 0x10, 6);
+	clks[IMX6SX_CLK_USBPHY2_GATE] = imx_clk_gate("usbphy2_gate", "dummy", base + 0x20, 6);
+
+	/* FIXME 100Mhz is used for pcie ref for all imx6 pcie, excepted imx6q */
+	clks[IMX6SX_CLK_PCIE_REF] = imx_clk_fixed_factor("pcie_ref", "pll6_enet", 1, 5);
+	clks[IMX6SX_CLK_PCIE_REF_125M] = imx_clk_gate("pcie_ref_125m", "pcie_ref", base + 0xe0, 19);
+
+	clks[IMX6SX_CLK_LVDS1_OUT] = imx_clk_gate("lvds1_out", "lvds1_sel", base + 0x160, 10);
+
+	clks[IMX6SX_CLK_ENET_REF] = clk_register_divider_table(NULL, "enet_ref", "pll6_enet", 0,
+			base + 0xe0, 0, 2, 0, clk_enet_ref_table,
+			&imx_ccm_lock);
+	clks[IMX6SX_CLK_ENET2_REF] = clk_register_divider_table(NULL, "enet2_ref", "pll6_enet", 0,
+			base + 0xe0, 2, 2, 0, clk_enet_ref_table,
+			&imx_ccm_lock);
+	clks[IMX6SX_CLK_ENET2_REF_125M] = imx_clk_gate("enet2_ref_125m", "enet2_ref", base + 0xe0, 20);
+
+	clks[IMX6SX_CLK_ENET_PTP_REF] = imx_clk_fixed_factor("enet_ptp_ref", "pll6_enet", 1, 20);
+	clks[IMX6SX_CLK_ENET_PTP] = imx_clk_gate("enet_ptp_25m", "enet_ptp_ref", base + 0xe0, 21);
+
+	/*                                       name              parent_name     reg           idx */
+	clks[IMX6SX_CLK_PLL2_PFD0] = imx_clk_pfd("pll2_pfd0_352m", "pll2_bus",     base + 0x100, 0);
+	clks[IMX6SX_CLK_PLL2_PFD1] = imx_clk_pfd("pll2_pfd1_594m", "pll2_bus",     base + 0x100, 1);
+	clks[IMX6SX_CLK_PLL2_PFD2] = imx_clk_pfd("pll2_pfd2_396m", "pll2_bus",     base + 0x100, 2);
+	clks[IMX6SX_CLK_PLL2_PFD3] = imx_clk_pfd("pll2_pfd3_594m", "pll2_bus",     base + 0x100, 3);
+	clks[IMX6SX_CLK_PLL3_PFD0] = imx_clk_pfd("pll3_pfd0_720m", "pll3_usb_otg", base + 0xf0,  0);
+	clks[IMX6SX_CLK_PLL3_PFD1] = imx_clk_pfd("pll3_pfd1_540m", "pll3_usb_otg", base + 0xf0,  1);
+	clks[IMX6SX_CLK_PLL3_PFD2] = imx_clk_pfd("pll3_pfd2_508m", "pll3_usb_otg", base + 0xf0,  2);
+	clks[IMX6SX_CLK_PLL3_PFD3] = imx_clk_pfd("pll3_pfd3_454m", "pll3_usb_otg", base + 0xf0,  3);
+
+	/*                                                name         parent_name       mult div */
+	clks[IMX6SX_CLK_PLL2_198M] = imx_clk_fixed_factor("pll2_198m", "pll2_pfd2_396m", 1,   2);
+	clks[IMX6SX_CLK_PLL3_120M] = imx_clk_fixed_factor("pll3_120m", "pll3_usb_otg",   1,   4);
+	clks[IMX6SX_CLK_PLL3_80M]  = imx_clk_fixed_factor("pll3_80m",  "pll3_usb_otg",   1,   6);
+	clks[IMX6SX_CLK_PLL3_60M]  = imx_clk_fixed_factor("pll3_60m",  "pll3_usb_otg",   1,   8);
+	clks[IMX6SX_CLK_TWD]       = imx_clk_fixed_factor("twd",       "arm",            1,   2);
+	clks[IMX6SX_CLK_GPT_3M]    = imx_clk_fixed_factor("gpt_3m",    "osc",            1,   8);
+
+	clks[IMX6SX_CLK_PLL4_POST_DIV]  = clk_register_divider_table(NULL, "pll4_post_div", "pll4_audio",
+				CLK_SET_RATE_PARENT, base + 0x70, 19, 2, 0, post_div_table, &imx_ccm_lock);
+	clks[IMX6SX_CLK_PLL4_AUDIO_DIV] = clk_register_divider(NULL, "pll4_audio_div", "pll4_post_div",
+				CLK_SET_RATE_PARENT, base + 0x170, 15, 1, 0, &imx_ccm_lock);
+	clks[IMX6SX_CLK_PLL5_POST_DIV]  = clk_register_divider_table(NULL, "pll5_post_div", "pll5_video",
+				CLK_SET_RATE_PARENT, base + 0xa0, 19, 2, 0, post_div_table, &imx_ccm_lock);
+	clks[IMX6SX_CLK_PLL5_VIDEO_DIV] = clk_register_divider_table(NULL, "pll5_video_div", "pll5_post_div",
+				CLK_SET_RATE_PARENT, base + 0x170, 30, 2, 0, video_div_table, &imx_ccm_lock);
+
+	/*                                                name                reg           shift   width   parent_names       num_parents */
+	clks[IMX6SX_CLK_LVDS1_SEL]          = imx_clk_mux("lvds1_sel",        base + 0x160, 0,      5,      lvds_sels,         ARRAY_SIZE(lvds_sels));
+
+	np = ccm_node;
+	base = of_iomap(np, 0);
+	WARN_ON(!base);
+
+	imx6q_pm_set_ccm_base(base);
+
+	/*                                                name                reg           shift   width   parent_names       num_parents */
+	clks[IMX6SX_CLK_STEP]               = imx_clk_mux("step",             base + 0xc,   8,      1,      step_sels,         ARRAY_SIZE(step_sels));
+	clks[IMX6SX_CLK_PLL1_SW]            = imx_clk_mux("pll1_sw",          base + 0xc,   2,      1,      pll1_sw_sels,      ARRAY_SIZE(pll1_sw_sels));
+	clks[IMX6SX_CLK_OCRAM_SEL]          = imx_clk_mux("ocram_sel",        base + 0x14,  6,      2,      ocram_sels,        ARRAY_SIZE(ocram_sels));
+	clks[IMX6SX_CLK_PERIPH_PRE]         = imx_clk_mux("periph_pre",       base + 0x18,  18,     2,      periph_pre_sels,   ARRAY_SIZE(periph_pre_sels));
+	clks[IMX6SX_CLK_PERIPH2_PRE]        = imx_clk_mux("periph2_pre",      base + 0x18,  21,     2,      periph2_pre_sels,   ARRAY_SIZE(periph2_pre_sels));
+	clks[IMX6SX_CLK_PERIPH_CLK2_SEL]    = imx_clk_mux("periph_clk2_sel",  base + 0x18,  12,     2,      periph_clk2_sels,  ARRAY_SIZE(periph_clk2_sels));
+	clks[IMX6SX_CLK_PERIPH2_CLK2_SEL]   = imx_clk_mux("periph2_clk2_sel", base + 0x18,  20,     1,      periph2_clk2_sels, ARRAY_SIZE(periph2_clk2_sels));
+	clks[IMX6SX_CLK_PCIE_AXI_SEL]       = imx_clk_mux("pcie_axi_sel",     base + 0x18,  10,     1,      pcie_axi_sels,     ARRAY_SIZE(pcie_axi_sels));
+	clks[IMX6SX_CLK_GPU_AXI_SEL]        = imx_clk_mux("gpu_axi_sel",      base + 0x18,  8,      2,      gpu_axi_sels,      ARRAY_SIZE(gpu_axi_sels));
+	clks[IMX6SX_CLK_GPU_CORE_SEL]       = imx_clk_mux("gpu_core_sel",     base + 0x18,  4,      2,      gpu_core_sels,     ARRAY_SIZE(gpu_core_sels));
+	clks[IMX6SX_CLK_EIM_SLOW_SEL]       = imx_clk_mux("eim_slow_sel",     base + 0x1c,  29,     2,      eim_slow_sels,     ARRAY_SIZE(eim_slow_sels));
+	clks[IMX6SX_CLK_USDHC1_SEL]         = imx_clk_mux("usdhc1_sel",       base + 0x1c,  16,     1,      usdhc_sels,        ARRAY_SIZE(usdhc_sels));
+	clks[IMX6SX_CLK_USDHC2_SEL]         = imx_clk_mux("usdhc2_sel",       base + 0x1c,  17,     1,      usdhc_sels,        ARRAY_SIZE(usdhc_sels));
+	clks[IMX6SX_CLK_USDHC3_SEL]         = imx_clk_mux("usdhc3_sel",       base + 0x1c,  18,     1,      usdhc_sels,        ARRAY_SIZE(usdhc_sels));
+	clks[IMX6SX_CLK_USDHC4_SEL]         = imx_clk_mux("usdhc4_sel",       base + 0x1c,  19,     1,      usdhc_sels,        ARRAY_SIZE(usdhc_sels));
+	clks[IMX6SX_CLK_SSI3_SEL]           = imx_clk_mux("ssi3_sel",         base + 0x1c,  14,     2,      ssi_sels,          ARRAY_SIZE(ssi_sels));
+	clks[IMX6SX_CLK_SSI2_SEL]           = imx_clk_mux("ssi2_sel",         base + 0x1c,  12,     2,      ssi_sels,          ARRAY_SIZE(ssi_sels));
+	clks[IMX6SX_CLK_SSI1_SEL]           = imx_clk_mux("ssi1_sel",         base + 0x1c,  10,     2,      ssi_sels,          ARRAY_SIZE(ssi_sels));
+	clks[IMX6SX_CLK_QSPI1_SEL]          = imx_clk_mux_flags("qspi1_sel", base + 0x1c,  7, 3, qspi1_sels, ARRAY_SIZE(qspi1_sels), CLK_SET_RATE_PARENT);
+	clks[IMX6SX_CLK_PERCLK_SEL]         = imx_clk_mux("perclk_sel",       base + 0x1c,  6,      1,      perclk_sels,       ARRAY_SIZE(perclk_sels));
+	clks[IMX6SX_CLK_VID_SEL]            = imx_clk_mux("vid_sel",          base + 0x20,  21,     3,      vid_sels,          ARRAY_SIZE(vid_sels));
+	clks[IMX6SX_CLK_ESAI_SEL]           = imx_clk_mux("esai_sel",         base + 0x20,  19,     2,      audio_sels,        ARRAY_SIZE(audio_sels));
+	clks[IMX6SX_CLK_CAN_SEL]            = imx_clk_mux("can_sel",          base + 0x20,  8,      2,      can_sels,          ARRAY_SIZE(can_sels));
+	clks[IMX6SX_CLK_UART_SEL]           = imx_clk_mux("uart_sel",         base + 0x24,  6,      1,      uart_sels,         ARRAY_SIZE(uart_sels));
+	clks[IMX6SX_CLK_QSPI2_SEL]          = imx_clk_mux_flags("qspi2_sel", base + 0x2c, 15, 3, qspi2_sels, ARRAY_SIZE(qspi2_sels), CLK_SET_RATE_PARENT);
+	clks[IMX6SX_CLK_SPDIF_SEL]          = imx_clk_mux("spdif_sel",        base + 0x30,  20,     2,      audio_sels,        ARRAY_SIZE(audio_sels));
+	clks[IMX6SX_CLK_AUDIO_SEL]          = imx_clk_mux("audio_sel",        base + 0x30,  7,      2,      audio_sels,        ARRAY_SIZE(audio_sels));
+	clks[IMX6SX_CLK_ENET_PRE_SEL]       = imx_clk_mux("enet_pre_sel",     base + 0x34,  15,     3,      enet_pre_sels,     ARRAY_SIZE(enet_pre_sels));
+	clks[IMX6SX_CLK_ENET_SEL]           = imx_clk_mux("enet_sel",         base + 0x34,  9,      3,      enet_sels,         ARRAY_SIZE(enet_sels));
+	clks[IMX6SX_CLK_M4_PRE_SEL]         = imx_clk_mux("m4_pre_sel",       base + 0x34,  6,      3,      m4_pre_sels,       ARRAY_SIZE(m4_pre_sels));
+	clks[IMX6SX_CLK_M4_SEL]             = imx_clk_mux("m4_sel",           base + 0x34,  0,      3,      m4_sels,           ARRAY_SIZE(m4_sels));
+	clks[IMX6SX_CLK_ECSPI_SEL]          = imx_clk_mux("ecspi_sel",        base + 0x38,  18,     1,      ecspi_sels,        ARRAY_SIZE(ecspi_sels));
+	clks[IMX6SX_CLK_LCDIF2_PRE_SEL]     = imx_clk_mux("lcdif2_pre_sel",   base + 0x38,  6,      3,      lcdif2_pre_sels,   ARRAY_SIZE(lcdif2_pre_sels));
+	clks[IMX6SX_CLK_LCDIF2_SEL]         = imx_clk_mux("lcdif2_sel",       base + 0x38,  0,      3,      lcdif2_sels,       ARRAY_SIZE(lcdif2_sels));
+	clks[IMX6SX_CLK_DISPLAY_SEL]        = imx_clk_mux("display_sel",      base + 0x3c,  14,     2,      display_sels,      ARRAY_SIZE(display_sels));
+	clks[IMX6SX_CLK_CSI_SEL]            = imx_clk_mux("csi_sel",          base + 0x3c,  9,      2,      csi_sels,          ARRAY_SIZE(csi_sels));
+	clks[IMX6SX_CLK_CKO1_SEL]           = imx_clk_mux("cko1_sel",         base + 0x60,  0,      4,      cko1_sels,         ARRAY_SIZE(cko1_sels));
+	clks[IMX6SX_CLK_CKO2_SEL]           = imx_clk_mux("cko2_sel",         base + 0x60,  16,     5,      cko2_sels,         ARRAY_SIZE(cko2_sels));
+	clks[IMX6SX_CLK_CKO]                = imx_clk_mux("cko",              base + 0x60,  8,      1,      cko_sels,          ARRAY_SIZE(cko_sels));
+
+	clks[IMX6SX_CLK_LDB_DI1_DIV_SEL]    = imx_clk_mux_flags("ldb_di1_div_sel", base + 0x20, 11, 1, ldb_di1_div_sels, ARRAY_SIZE(ldb_di1_div_sels), CLK_SET_RATE_PARENT);
+	clks[IMX6SX_CLK_LDB_DI0_DIV_SEL]    = imx_clk_mux_flags("ldb_di0_div_sel", base + 0x20, 10, 1, ldb_di0_div_sels, ARRAY_SIZE(ldb_di0_div_sels), CLK_SET_RATE_PARENT);
+	clks[IMX6SX_CLK_LDB_DI1_SEL]        = imx_clk_mux_flags("ldb_di1_sel",     base + 0x2c, 12, 3, ldb_di1_sels,      ARRAY_SIZE(ldb_di1_sels),    CLK_SET_RATE_PARENT);
+	clks[IMX6SX_CLK_LDB_DI0_SEL]        = imx_clk_mux_flags("ldb_di0_sel",     base + 0x2c, 9,  3, ldb_di0_sels,      ARRAY_SIZE(ldb_di0_sels),    CLK_SET_RATE_PARENT);
+	clks[IMX6SX_CLK_LCDIF1_PRE_SEL]     = imx_clk_mux_flags("lcdif1_pre_sel",  base + 0x38, 15, 3, lcdif1_pre_sels,   ARRAY_SIZE(lcdif1_pre_sels), CLK_SET_RATE_PARENT);
+	clks[IMX6SX_CLK_LCDIF1_SEL]         = imx_clk_mux_flags("lcdif1_sel",      base + 0x38, 9,  3, lcdif1_sels,       ARRAY_SIZE(lcdif1_sels),     CLK_SET_RATE_PARENT);
+
+	/*                                                    name              parent_name          reg          shift width */
+	clks[IMX6SX_CLK_PERIPH_CLK2]        = imx_clk_divider("periph_clk2",    "periph_clk2_sel",   base + 0x14, 27,   3);
+	clks[IMX6SX_CLK_PERIPH2_CLK2]       = imx_clk_divider("periph2_clk2",   "periph2_clk2_sel",  base + 0x14, 0,    3);
+	clks[IMX6SX_CLK_IPG]                = imx_clk_divider("ipg",            "ahb",               base + 0x14, 8,    2);
+	clks[IMX6SX_CLK_GPU_CORE_PODF]      = imx_clk_divider("gpu_core_podf",  "gpu_core_sel",      base + 0x18, 29,   3);
+	clks[IMX6SX_CLK_GPU_AXI_PODF]       = imx_clk_divider("gpu_axi_podf",   "gpu_axi_sel",       base + 0x18, 26,   3);
+	clks[IMX6SX_CLK_LCDIF1_PODF]        = imx_clk_divider("lcdif1_podf",    "lcdif1_pred",       base + 0x18, 23,   3);
+	clks[IMX6SX_CLK_QSPI1_PODF]         = imx_clk_divider("qspi1_podf",     "qspi1_sel",         base + 0x1c, 26,   3);
+	clks[IMX6SX_CLK_EIM_SLOW_PODF]      = imx_clk_divider("eim_slow_podf",  "eim_slow_sel",      base + 0x1c, 23,   3);
+	clks[IMX6SX_CLK_LCDIF2_PODF]        = imx_clk_divider("lcdif2_podf",    "lcdif2_pred",       base + 0x1c, 20,   3);
+	clks[IMX6SX_CLK_PERCLK]             = imx_clk_divider("perclk",         "perclk_sel",        base + 0x1c, 0,    6);
+	clks[IMX6SX_CLK_VID_PODF]           = imx_clk_divider("vid_podf",       "vid_sel",           base + 0x20, 24,   2);
+	clks[IMX6SX_CLK_CAN_PODF]           = imx_clk_divider("can_podf",       "can_sel",           base + 0x20, 2,    6);
+	clks[IMX6SX_CLK_USDHC4_PODF]        = imx_clk_divider("usdhc4_podf",    "usdhc4_sel",        base + 0x24, 22,   3);
+	clks[IMX6SX_CLK_USDHC3_PODF]        = imx_clk_divider("usdhc3_podf",    "usdhc3_sel",        base + 0x24, 19,   3);
+	clks[IMX6SX_CLK_USDHC2_PODF]        = imx_clk_divider("usdhc2_podf",    "usdhc2_sel",        base + 0x24, 16,   3);
+	clks[IMX6SX_CLK_USDHC1_PODF]        = imx_clk_divider("usdhc1_podf",    "usdhc1_sel",        base + 0x24, 11,   3);
+	clks[IMX6SX_CLK_UART_PODF]          = imx_clk_divider("uart_podf",      "uart_sel",          base + 0x24, 0,    6);
+	clks[IMX6SX_CLK_ESAI_PRED]          = imx_clk_divider("esai_pred",      "esai_sel",          base + 0x28, 9,    3);
+	clks[IMX6SX_CLK_ESAI_PODF]          = imx_clk_divider("esai_podf",      "esai_pred",         base + 0x28, 25,   3);
+	clks[IMX6SX_CLK_SSI3_PRED]          = imx_clk_divider("ssi3_pred",      "ssi3_sel",          base + 0x28, 22,   3);
+	clks[IMX6SX_CLK_SSI3_PODF]          = imx_clk_divider("ssi3_podf",      "ssi3_pred",         base + 0x28, 16,   6);
+	clks[IMX6SX_CLK_SSI1_PRED]          = imx_clk_divider("ssi1_pred",      "ssi1_sel",          base + 0x28, 6,    3);
+	clks[IMX6SX_CLK_SSI1_PODF]          = imx_clk_divider("ssi1_podf",      "ssi1_pred",         base + 0x28, 0,    6);
+	clks[IMX6SX_CLK_QSPI2_PRED]         = imx_clk_divider("qspi2_pred",     "qspi2_sel",         base + 0x2c, 18,   3);
+	clks[IMX6SX_CLK_QSPI2_PODF]         = imx_clk_divider("qspi2_podf",     "qspi2_pred",        base + 0x2c, 21,   6);
+	clks[IMX6SX_CLK_SSI2_PRED]          = imx_clk_divider("ssi2_pred",      "ssi2_sel",          base + 0x2c, 6,    3);
+	clks[IMX6SX_CLK_SSI2_PODF]          = imx_clk_divider("ssi2_podf",      "ssi2_pred",         base + 0x2c, 0,    6);
+	clks[IMX6SX_CLK_SPDIF_PRED]         = imx_clk_divider("spdif_pred",     "spdif_sel",         base + 0x30, 25,   3);
+	clks[IMX6SX_CLK_SPDIF_PODF]         = imx_clk_divider("spdif_podf",     "spdif_pred",        base + 0x30, 22,   3);
+	clks[IMX6SX_CLK_AUDIO_PRED]         = imx_clk_divider("audio_pred",     "audio_sel",         base + 0x30, 12,   3);
+	clks[IMX6SX_CLK_AUDIO_PODF]         = imx_clk_divider("audio_podf",     "audio_pred",        base + 0x30, 9,    3);
+	clks[IMX6SX_CLK_ENET_PODF]          = imx_clk_divider("enet_podf",      "enet_pre_sel",      base + 0x34, 12,   3);
+	clks[IMX6SX_CLK_M4_PODF]            = imx_clk_divider("m4_podf",        "m4_sel",            base + 0x34, 3,    3);
+	clks[IMX6SX_CLK_ECSPI_PODF]         = imx_clk_divider("ecspi_podf",     "ecspi_sel",         base + 0x38, 19,   6);
+	clks[IMX6SX_CLK_LCDIF1_PRED]        = imx_clk_divider("lcdif1_pred",    "lcdif1_pre_sel",    base + 0x38, 12,   3);
+	clks[IMX6SX_CLK_LCDIF2_PRED]        = imx_clk_divider("lcdif2_pred",    "lcdif2_pre_sel",    base + 0x38, 3,    3);
+	clks[IMX6SX_CLK_DISPLAY_PODF]       = imx_clk_divider("display_podf",   "display_sel",       base + 0x3c, 16,   3);
+	clks[IMX6SX_CLK_CSI_PODF]           = imx_clk_divider("csi_podf",       "csi_sel",           base + 0x3c, 11,   3);
+	clks[IMX6SX_CLK_CKO1_PODF]          = imx_clk_divider("cko1_podf",      "cko1_sel",          base + 0x60, 4,    3);
+	clks[IMX6SX_CLK_CKO2_PODF]          = imx_clk_divider("cko2_podf",      "cko2_sel",          base + 0x60, 21,   3);
+
+	clks[IMX6SX_CLK_LDB_DI0_DIV_3_5]    = imx_clk_fixed_factor("ldb_di0_div_3_5", "ldb_di0_sel", 2, 7);
+	clks[IMX6SX_CLK_LDB_DI0_DIV_7]      = imx_clk_fixed_factor("ldb_di0_div_7",   "ldb_di0_sel", 1, 7);
+	clks[IMX6SX_CLK_LDB_DI1_DIV_3_5]    = imx_clk_fixed_factor("ldb_di1_div_3_5", "ldb_di1_sel", 2, 7);
+	clks[IMX6SX_CLK_LDB_DI1_DIV_7]      = imx_clk_fixed_factor("ldb_di1_div_7",   "ldb_di1_sel", 1, 7);
+
+	/*                                               name        reg          shift width busy: reg,   shift parent_names       num_parents */
+	clks[IMX6SX_CLK_PERIPH]       = imx_clk_busy_mux("periph",   base + 0x14, 25,   1,    base + 0x48, 5,    periph_sels,       ARRAY_SIZE(periph_sels));
+	clks[IMX6SX_CLK_PERIPH2]      = imx_clk_busy_mux("periph2",  base + 0x14, 26,   1,    base + 0x48, 3,    periph2_sels,      ARRAY_SIZE(periph2_sels));
+	/*                                                   name             parent_name    reg          shift width busy: reg,   shift */
+	clks[IMX6SX_CLK_OCRAM_PODF]   = imx_clk_busy_divider("ocram_podf",    "ocram_sel",   base + 0x14, 16,   3,    base + 0x48, 0);
+	clks[IMX6SX_CLK_AHB]          = imx_clk_busy_divider("ahb",           "periph",      base + 0x14, 10,   3,    base + 0x48, 1);
+	clks[IMX6SX_CLK_MMDC_PODF]    = imx_clk_busy_divider("mmdc_podf",     "periph2",     base + 0x14, 3,    3,    base + 0x48, 2);
+	clks[IMX6SX_CLK_ARM]          = imx_clk_busy_divider("arm",           "pll1_sw",     base + 0x10, 0,    3,    base + 0x48, 16);
+
+	/*                                            name             parent_name          reg         shift */
+	/* CCGR0 */
+	clks[IMX6SX_CLK_AIPS_TZ1]     = imx_clk_gate2("aips_tz1",      "ahb",               base + 0x68, 0);
+	clks[IMX6SX_CLK_AIPS_TZ2]     = imx_clk_gate2("aips_tz2",      "ahb",               base + 0x68, 2);
+	clks[IMX6SX_CLK_APBH_DMA]     = imx_clk_gate2("apbh_dma",      "usdhc3",            base + 0x68, 4);
+	clks[IMX6SX_CLK_ASRC_MEM]     = imx_clk_gate2_shared("asrc_mem", "ahb",             base + 0x68, 6, &share_count_asrc);
+	clks[IMX6SX_CLK_ASRC_IPG]     = imx_clk_gate2_shared("asrc_ipg", "ahb",             base + 0x68, 6, &share_count_asrc);
+	clks[IMX6SX_CLK_CAAM_MEM]     = imx_clk_gate2("caam_mem",      "ahb",               base + 0x68, 8);
+	clks[IMX6SX_CLK_CAAM_ACLK]    = imx_clk_gate2("caam_aclk",     "ahb",               base + 0x68, 10);
+	clks[IMX6SX_CLK_CAAM_IPG]     = imx_clk_gate2("caam_ipg",      "ipg",               base + 0x68, 12);
+	clks[IMX6SX_CLK_CAN1_IPG]     = imx_clk_gate2("can1_ipg",      "ipg",               base + 0x68, 14);
+	clks[IMX6SX_CLK_CAN1_SERIAL]  = imx_clk_gate2("can1_serial",   "can_podf",          base + 0x68, 16);
+	clks[IMX6SX_CLK_CAN2_IPG]     = imx_clk_gate2("can2_ipg",      "ipg",               base + 0x68, 18);
+	clks[IMX6SX_CLK_CAN2_SERIAL]  = imx_clk_gate2("can2_serial",   "can_podf",          base + 0x68, 20);
+	clks[IMX6SX_CLK_DCIC1]        = imx_clk_gate2("dcic1",         "display_podf",      base + 0x68, 24);
+	clks[IMX6SX_CLK_DCIC2]        = imx_clk_gate2("dcic2",         "display_podf",      base + 0x68, 26);
+	clks[IMX6SX_CLK_AIPS_TZ3]     = imx_clk_gate2("aips_tz3",      "ahb",               base + 0x68, 30);
+
+	/* CCGR1 */
+	clks[IMX6SX_CLK_ECSPI1]       = imx_clk_gate2("ecspi1",        "ecspi_podf",        base + 0x6c, 0);
+	clks[IMX6SX_CLK_ECSPI2]       = imx_clk_gate2("ecspi2",        "ecspi_podf",        base + 0x6c, 2);
+	clks[IMX6SX_CLK_ECSPI3]       = imx_clk_gate2("ecspi3",        "ecspi_podf",        base + 0x6c, 4);
+	clks[IMX6SX_CLK_ECSPI4]       = imx_clk_gate2("ecspi4",        "ecspi_podf",        base + 0x6c, 6);
+	clks[IMX6SX_CLK_ECSPI5]       = imx_clk_gate2("ecspi5",        "ecspi_podf",        base + 0x6c, 8);
+	clks[IMX6SX_CLK_EPIT1]        = imx_clk_gate2("epit1",         "perclk",            base + 0x6c, 12);
+	clks[IMX6SX_CLK_EPIT2]        = imx_clk_gate2("epit2",         "perclk",            base + 0x6c, 14);
+	clks[IMX6SX_CLK_ESAI_EXTAL]   = imx_clk_gate2_shared("esai_extal", "esai_podf",     base + 0x6c, 16, &share_count_esai);
+	clks[IMX6SX_CLK_ESAI_IPG]     = imx_clk_gate2_shared("esai_ipg",   "ahb",           base + 0x6c, 16, &share_count_esai);
+	clks[IMX6SX_CLK_ESAI_MEM]     = imx_clk_gate2_shared("esai_mem",   "ahb",           base + 0x6c, 16, &share_count_esai);
+	clks[IMX6SX_CLK_WAKEUP]       = imx_clk_gate2("wakeup",        "ipg",               base + 0x6c, 18);
+	clks[IMX6SX_CLK_GPT_BUS]      = imx_clk_gate2("gpt_bus",       "perclk",            base + 0x6c, 20);
+	clks[IMX6SX_CLK_GPT_SERIAL]   = imx_clk_gate2("gpt_serial",    "perclk",            base + 0x6c, 22);
+	clks[IMX6SX_CLK_GPU]          = imx_clk_gate2("gpu",           "gpu_core_podf",     base + 0x6c, 26);
+	clks[IMX6SX_CLK_CANFD]        = imx_clk_gate2("canfd",         "can_podf",          base + 0x6c, 30);
+
+	/* CCGR2 */
+	clks[IMX6SX_CLK_CSI]          = imx_clk_gate2("csi",           "csi_podf",          base + 0x70, 2);
+	clks[IMX6SX_CLK_I2C1]         = imx_clk_gate2("i2c1",          "perclk",            base + 0x70, 6);
+	clks[IMX6SX_CLK_I2C2]         = imx_clk_gate2("i2c2",          "perclk",            base + 0x70, 8);
+	clks[IMX6SX_CLK_I2C3]         = imx_clk_gate2("i2c3",          "perclk",            base + 0x70, 10);
+	clks[IMX6SX_CLK_OCOTP]        = imx_clk_gate2("ocotp",         "ipg",               base + 0x70, 12);
+	clks[IMX6SX_CLK_IOMUXC]       = imx_clk_gate2("iomuxc",        "lcdif1_podf",       base + 0x70, 14);
+	clks[IMX6SX_CLK_IPMUX1]       = imx_clk_gate2("ipmux1",        "ahb",               base + 0x70, 16);
+	clks[IMX6SX_CLK_IPMUX2]       = imx_clk_gate2("ipmux2",        "ahb",               base + 0x70, 18);
+	clks[IMX6SX_CLK_IPMUX3]       = imx_clk_gate2("ipmux3",        "ahb",               base + 0x70, 20);
+	clks[IMX6SX_CLK_TZASC1]       = imx_clk_gate2("tzasc1",        "mmdc_podf",         base + 0x70, 22);
+	clks[IMX6SX_CLK_LCDIF_APB]    = imx_clk_gate2("lcdif_apb",     "display_podf",      base + 0x70, 28);
+	clks[IMX6SX_CLK_PXP_AXI]      = imx_clk_gate2("pxp_axi",       "display_podf",      base + 0x70, 30);
+
+	/* CCGR3 */
+	clks[IMX6SX_CLK_M4]           = imx_clk_gate2("m4",            "m4_podf",           base + 0x74, 2);
+	clks[IMX6SX_CLK_ENET]         = imx_clk_gate2("enet",          "ipg",               base + 0x74, 4);
+	clks[IMX6SX_CLK_ENET_AHB]     = imx_clk_gate2("enet_ahb",      "enet_sel",          base + 0x74, 4);
+	clks[IMX6SX_CLK_DISPLAY_AXI]  = imx_clk_gate2("display_axi",   "display_podf",      base + 0x74, 6);
+	clks[IMX6SX_CLK_LCDIF2_PIX]   = imx_clk_gate2("lcdif2_pix",    "lcdif2_sel",        base + 0x74, 8);
+	clks[IMX6SX_CLK_LCDIF1_PIX]   = imx_clk_gate2("lcdif1_pix",    "lcdif1_sel",        base + 0x74, 10);
+	clks[IMX6SX_CLK_LDB_DI0]      = imx_clk_gate2("ldb_di0",       "ldb_di0_div_sel",   base + 0x74, 12);
+	clks[IMX6SX_CLK_QSPI1]        = imx_clk_gate2("qspi1",         "qspi1_podf",        base + 0x74, 14);
+	clks[IMX6SX_CLK_MLB]          = imx_clk_gate2("mlb",           "ahb",               base + 0x74, 18);
+	clks[IMX6SX_CLK_MMDC_P0_FAST] = imx_clk_gate2("mmdc_p0_fast",  "mmdc_podf",         base + 0x74, 20);
+	clks[IMX6SX_CLK_MMDC_P0_IPG]  = imx_clk_gate2("mmdc_p0_ipg",   "ipg",               base + 0x74, 24);
+	clks[IMX6SX_CLK_OCRAM]        = imx_clk_gate2("ocram",         "ocram_podf",        base + 0x74, 28);
+
+	/* CCGR4 */
+	clks[IMX6SX_CLK_PCIE_AXI]     = imx_clk_gate2("pcie_axi",      "display_podf",      base + 0x78, 0);
+	clks[IMX6SX_CLK_QSPI2]        = imx_clk_gate2("qspi2",         "qspi2_podf",        base + 0x78, 10);
+	clks[IMX6SX_CLK_PER1_BCH]     = imx_clk_gate2("per1_bch",      "usdhc3",            base + 0x78, 12);
+	clks[IMX6SX_CLK_PER2_MAIN]    = imx_clk_gate2("per2_main",     "ahb",               base + 0x78, 14);
+	clks[IMX6SX_CLK_PWM1]         = imx_clk_gate2("pwm1",          "perclk",            base + 0x78, 16);
+	clks[IMX6SX_CLK_PWM2]         = imx_clk_gate2("pwm2",          "perclk",            base + 0x78, 18);
+	clks[IMX6SX_CLK_PWM3]         = imx_clk_gate2("pwm3",          "perclk",            base + 0x78, 20);
+	clks[IMX6SX_CLK_PWM4]         = imx_clk_gate2("pwm4",          "perclk",            base + 0x78, 22);
+	clks[IMX6SX_CLK_GPMI_BCH_APB] = imx_clk_gate2("gpmi_bch_apb",  "usdhc3",            base + 0x78, 24);
+	clks[IMX6SX_CLK_GPMI_BCH]     = imx_clk_gate2("gpmi_bch",      "usdhc4",            base + 0x78, 26);
+	clks[IMX6SX_CLK_GPMI_IO]      = imx_clk_gate2("gpmi_io",       "qspi2_podf",        base + 0x78, 28);
+	clks[IMX6SX_CLK_GPMI_APB]     = imx_clk_gate2("gpmi_apb",      "usdhc3",            base + 0x78, 30);
+
+	/* CCGR5 */
+	clks[IMX6SX_CLK_ROM]          = imx_clk_gate2("rom",           "ahb",               base + 0x7c, 0);
+	clks[IMX6SX_CLK_SDMA]         = imx_clk_gate2("sdma",          "ahb",               base + 0x7c, 6);
+	clks[IMX6SX_CLK_SPBA]         = imx_clk_gate2("spba",          "ipg",               base + 0x7c, 12);
+	clks[IMX6SX_CLK_AUDIO]        = imx_clk_gate2_shared("audio",  "audio_podf",        base + 0x7c, 14, &share_count_audio);
+	clks[IMX6SX_CLK_SPDIF]        = imx_clk_gate2_shared("spdif",  "spdif_podf",        base + 0x7c, 14, &share_count_audio);
+	clks[IMX6SX_CLK_SSI1_IPG]     = imx_clk_gate2("ssi1_ipg",      "ipg",               base + 0x7c, 18);
+	clks[IMX6SX_CLK_SSI2_IPG]     = imx_clk_gate2("ssi2_ipg",      "ipg",               base + 0x7c, 20);
+	clks[IMX6SX_CLK_SSI3_IPG]     = imx_clk_gate2("ssi3_ipg",      "ipg",               base + 0x7c, 22);
+	clks[IMX6SX_CLK_SSI1]         = imx_clk_gate2("ssi1",          "ssi1_podf",         base + 0x7c, 18);
+	clks[IMX6SX_CLK_SSI2]         = imx_clk_gate2("ssi2",          "ssi2_podf",         base + 0x7c, 20);
+	clks[IMX6SX_CLK_SSI3]         = imx_clk_gate2("ssi3",          "ssi3_podf",         base + 0x7c, 22);
+	clks[IMX6SX_CLK_UART_IPG]     = imx_clk_gate2("uart_ipg",      "ipg",               base + 0x7c, 24);
+	clks[IMX6SX_CLK_UART_SERIAL]  = imx_clk_gate2("uart_serial",   "uart_podf",         base + 0x7c, 26);
+	clks[IMX6SX_CLK_SAI1_IPG]     = imx_clk_gate2("sai1_ipg",      "ipg",               base + 0x7c, 28);
+	clks[IMX6SX_CLK_SAI2_IPG]     = imx_clk_gate2("sai2_ipg",      "ipg",               base + 0x7c, 30);
+	clks[IMX6SX_CLK_SAI1]         = imx_clk_gate2("sai1",          "ssi1_podf",         base + 0x7c, 28);
+	clks[IMX6SX_CLK_SAI2]         = imx_clk_gate2("sai2",          "ssi2_podf",         base + 0x7c, 30);
+
+	/* CCGR6 */
+	clks[IMX6SX_CLK_USBOH3]       = imx_clk_gate2("usboh3",        "ipg",               base + 0x80, 0);
+	clks[IMX6SX_CLK_USDHC1]       = imx_clk_gate2("usdhc1",        "usdhc1_podf",       base + 0x80, 2);
+	clks[IMX6SX_CLK_USDHC2]       = imx_clk_gate2("usdhc2",        "usdhc2_podf",       base + 0x80, 4);
+	clks[IMX6SX_CLK_USDHC3]       = imx_clk_gate2("usdhc3",        "usdhc3_podf",       base + 0x80, 6);
+	clks[IMX6SX_CLK_USDHC4]       = imx_clk_gate2("usdhc4",        "usdhc4_podf",       base + 0x80, 8);
+	clks[IMX6SX_CLK_EIM_SLOW]     = imx_clk_gate2("eim_slow",      "eim_slow_podf",     base + 0x80, 10);
+	clks[IMX6SX_CLK_PWM8]         = imx_clk_gate2("pwm8",          "perclk",            base + 0x80, 16);
+	clks[IMX6SX_CLK_VADC]         = imx_clk_gate2("vadc",          "vid_podf",          base + 0x80, 20);
+	clks[IMX6SX_CLK_GIS]          = imx_clk_gate2("gis",           "display_podf",      base + 0x80, 22);
+	clks[IMX6SX_CLK_I2C4]         = imx_clk_gate2("i2c4",          "perclk",            base + 0x80, 24);
+	clks[IMX6SX_CLK_PWM5]         = imx_clk_gate2("pwm5",          "perclk",            base + 0x80, 26);
+	clks[IMX6SX_CLK_PWM6]         = imx_clk_gate2("pwm6",          "perclk",            base + 0x80, 28);
+	clks[IMX6SX_CLK_PWM7]         = imx_clk_gate2("pwm7",          "perclk",            base + 0x80, 30);
+
+	clks[IMX6SX_CLK_CKO1]         = imx_clk_gate("cko1",           "cko1_podf",         base + 0x60, 7);
+	clks[IMX6SX_CLK_CKO2]         = imx_clk_gate("cko2",           "cko2_podf",         base + 0x60, 24);
+
+	/* mask handshake of mmdc */
+	writel_relaxed(BM_CCM_CCDR_MMDC_CH0_MASK, base + CCDR);
+
+	for (i = 0; i < ARRAY_SIZE(clks); i++)
+		if (IS_ERR(clks[i]))
+			pr_err("i.MX6sx clk %d: register failed with %ld\n", i, PTR_ERR(clks[i]));
+
+	clk_data.clks = clks;
+	clk_data.clk_num = ARRAY_SIZE(clks);
+	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
+
+	clk_register_clkdev(clks[IMX6SX_CLK_GPT_BUS], "ipg", "imx-gpt.0");
+	clk_register_clkdev(clks[IMX6SX_CLK_GPT_SERIAL], "per", "imx-gpt.0");
+
+	for (i = 0; i < ARRAY_SIZE(clks_init_on); i++)
+		clk_prepare_enable(clks[clks_init_on[i]]);
+
+	if (IS_ENABLED(CONFIG_USB_MXS_PHY)) {
+		clk_prepare_enable(clks[IMX6SX_CLK_USBPHY1_GATE]);
+		clk_prepare_enable(clks[IMX6SX_CLK_USBPHY2_GATE]);
+	}
+
+	/* Set the default 132MHz for EIM module */
+	clk_set_parent(clks[IMX6SX_CLK_EIM_SLOW_SEL], clks[IMX6SX_CLK_PLL2_PFD2]);
+	clk_set_rate(clks[IMX6SX_CLK_EIM_SLOW], 132000000);
+
+	/* set parent clock for LCDIF1 pixel clock */
+	clk_set_parent(clks[IMX6SX_CLK_LCDIF1_PRE_SEL], clks[IMX6SX_CLK_PLL5_VIDEO_DIV]);
+	clk_set_parent(clks[IMX6SX_CLK_LCDIF1_SEL], clks[IMX6SX_CLK_LCDIF1_PODF]);
+
+	/* Set the parent clks of PCIe lvds1 and pcie_axi to be pcie ref, axi */
+	if (clk_set_parent(clks[IMX6SX_CLK_LVDS1_SEL], clks[IMX6SX_CLK_PCIE_REF_125M]))
+		pr_err("Failed to set pcie bus parent clk.\n");
+	if (clk_set_parent(clks[IMX6SX_CLK_PCIE_AXI_SEL], clks[IMX6SX_CLK_AXI]))
+		pr_err("Failed to set pcie parent clk.\n");
+
+	/*
+	 * Init enet system AHB clock, set to 200Mhz
+	 * pll2_pfd2_396m-> ENET_PODF-> ENET_AHB
+	 */
+	clk_set_parent(clks[IMX6SX_CLK_ENET_PRE_SEL], clks[IMX6SX_CLK_PLL2_PFD2]);
+	clk_set_parent(clks[IMX6SX_CLK_ENET_SEL], clks[IMX6SX_CLK_ENET_PODF]);
+	clk_set_rate(clks[IMX6SX_CLK_ENET_PODF], 200000000);
+	clk_set_rate(clks[IMX6SX_CLK_ENET_REF], 125000000);
+	clk_set_rate(clks[IMX6SX_CLK_ENET2_REF], 125000000);
+
+	/* Audio clocks */
+	clk_set_rate(clks[IMX6SX_CLK_PLL4_AUDIO_DIV], 393216000);
+
+	clk_set_parent(clks[IMX6SX_CLK_SPDIF_SEL], clks[IMX6SX_CLK_PLL4_AUDIO_DIV]);
+	clk_set_rate(clks[IMX6SX_CLK_SPDIF_PODF], 98304000);
+
+	clk_set_parent(clks[IMX6SX_CLK_AUDIO_SEL], clks[IMX6SX_CLK_PLL3_USB_OTG]);
+	clk_set_rate(clks[IMX6SX_CLK_AUDIO_PODF], 24000000);
+
+	clk_set_parent(clks[IMX6SX_CLK_SSI1_SEL], clks[IMX6SX_CLK_PLL4_AUDIO_DIV]);
+	clk_set_parent(clks[IMX6SX_CLK_SSI2_SEL], clks[IMX6SX_CLK_PLL4_AUDIO_DIV]);
+	clk_set_parent(clks[IMX6SX_CLK_SSI3_SEL], clks[IMX6SX_CLK_PLL4_AUDIO_DIV]);
+	clk_set_rate(clks[IMX6SX_CLK_SSI1_PODF], 24576000);
+	clk_set_rate(clks[IMX6SX_CLK_SSI2_PODF], 24576000);
+	clk_set_rate(clks[IMX6SX_CLK_SSI3_PODF], 24576000);
+
+	clk_set_parent(clks[IMX6SX_CLK_ESAI_SEL], clks[IMX6SX_CLK_PLL4_AUDIO_DIV]);
+	clk_set_rate(clks[IMX6SX_CLK_ESAI_PODF], 24576000);
+
+	/* Set parent clock for vadc */
+	clk_set_parent(clks[IMX6SX_CLK_VID_SEL], clks[IMX6SX_CLK_PLL3_USB_OTG]);
+
+	/* default parent of can_sel clock is invalid, manually set it here */
+	clk_set_parent(clks[IMX6SX_CLK_CAN_SEL], clks[IMX6SX_CLK_PLL3_60M]);
+
+	/* Update gpu clock from default 528M to 720M */
+	clk_set_parent(clks[IMX6SX_CLK_GPU_CORE_SEL], clks[IMX6SX_CLK_PLL3_PFD0]);
+	clk_set_parent(clks[IMX6SX_CLK_GPU_AXI_SEL], clks[IMX6SX_CLK_PLL3_PFD0]);
+
+	/* Set initial power mode */
+	imx6q_set_lpm(WAIT_CLOCKED);
+
+	np = of_find_compatible_node(NULL, NULL, "fsl,imx6sx-gpt");
+	mxc_timer_init_dt(np);
+}
+CLK_OF_DECLARE(imx6sx, "fsl,imx6sx-ccm", imx6sx_clocks_init);
diff --git a/arch/arm/mach-imx/clk.h b/arch/arm/mach-imx/clk.h
index 048c5ad8a80..e29f6ebe9f3 100644
--- a/arch/arm/mach-imx/clk.h
+++ b/arch/arm/mach-imx/clk.h
@@ -28,7 +28,8 @@ struct clk *imx_clk_pllv3(enum imx_pllv3_type type, const char *name,
 struct clk *clk_register_gate2(struct device *dev, const char *name,
 		const char *parent_name, unsigned long flags,
 		void __iomem *reg, u8 bit_idx,
-		u8 clk_gate_flags, spinlock_t *lock);
+		u8 clk_gate_flags, spinlock_t *lock,
+		unsigned int *share_count);
 
 struct clk * imx_obtain_fixed_clock(
 			const char *name, unsigned long rate);
@@ -37,7 +38,15 @@ static inline struct clk *imx_clk_gate2(const char *name, const char *parent,
 		void __iomem *reg, u8 shift)
 {
 	return clk_register_gate2(NULL, name, parent, CLK_SET_RATE_PARENT, reg,
-			shift, 0, &imx_ccm_lock);
+			shift, 0, &imx_ccm_lock, NULL);
+}
+
+static inline struct clk *imx_clk_gate2_shared(const char *name,
+		const char *parent, void __iomem *reg, u8 shift,
+		unsigned int *share_count)
+{
+	return clk_register_gate2(NULL, name, parent, CLK_SET_RATE_PARENT, reg,
+			shift, 0, &imx_ccm_lock, share_count);
 }
 
 struct clk *imx_clk_pfd(const char *name, const char *parent_name,
diff --git a/arch/arm/mach-imx/common.h b/arch/arm/mach-imx/common.h
index b5241ea7670..9ab785ce13e 100644
--- a/arch/arm/mach-imx/common.h
+++ b/arch/arm/mach-imx/common.h
@@ -17,6 +17,7 @@ struct irq_data;
 struct platform_device;
 struct pt_regs;
 struct clk;
+struct device_node;
 enum mxc_cpu_pwr_mode;
 
 void mx1_map_io(void);
@@ -56,6 +57,7 @@ void imx51_init_late(void);
 void imx53_init_late(void);
 void epit_timer_init(void __iomem *base, int irq);
 void mxc_timer_init(void __iomem *, int);
+void mxc_timer_init_dt(struct device_node *);
 int mx1_clocks_init(unsigned long fref);
 int mx21_clocks_init(unsigned long lref, unsigned long fref);
 int mx25_clocks_init(void);
@@ -99,19 +101,6 @@ enum mx3_cpu_pwr_mode {
 void mx3_cpu_lp_set(enum mx3_cpu_pwr_mode mode);
 void imx_print_silicon_rev(const char *cpu, int srev);
 
-void avic_handle_irq(struct pt_regs *);
-void tzic_handle_irq(struct pt_regs *);
-
-#define imx1_handle_irq avic_handle_irq
-#define imx21_handle_irq avic_handle_irq
-#define imx25_handle_irq avic_handle_irq
-#define imx27_handle_irq avic_handle_irq
-#define imx31_handle_irq avic_handle_irq
-#define imx35_handle_irq avic_handle_irq
-#define imx50_handle_irq tzic_handle_irq
-#define imx51_handle_irq tzic_handle_irq
-#define imx53_handle_irq tzic_handle_irq
-
 void imx_enable_cpu(int cpu, bool enable);
 void imx_set_cpu_jump(int cpu, void *jump_addr);
 u32 imx_get_cpu_arg(int cpu);
diff --git a/arch/arm/mach-imx/cpu.c b/arch/arm/mach-imx/cpu.c
index ba3b498a67e..bbe8ff1f041 100644
--- a/arch/arm/mach-imx/cpu.c
+++ b/arch/arm/mach-imx/cpu.c
@@ -111,6 +111,9 @@ struct device * __init imx_soc_device_init(void)
 	case MXC_CPU_IMX6DL:
 		soc_id = "i.MX6DL";
 		break;
+	case MXC_CPU_IMX6SX:
+		soc_id = "i.MX6SX";
+		break;
 	case MXC_CPU_IMX6Q:
 		soc_id = "i.MX6Q";
 		break;
diff --git a/arch/arm/mach-imx/devices/platform-ipu-core.c b/arch/arm/mach-imx/devices/platform-ipu-core.c
index fc4dd7cedc1..6bd7c3f37ac 100644
--- a/arch/arm/mach-imx/devices/platform-ipu-core.c
+++ b/arch/arm/mach-imx/devices/platform-ipu-core.c
@@ -77,7 +77,7 @@ struct platform_device *__init imx_alloc_mx3_camera(
 
 	pdev = platform_device_alloc("mx3-camera", 0);
 	if (!pdev)
-		goto err;
+		return ERR_PTR(-ENOMEM);
 
 	pdev->dev.dma_mask = kmalloc(sizeof(*pdev->dev.dma_mask), GFP_KERNEL);
 	if (!pdev->dev.dma_mask)
diff --git a/arch/arm/mach-imx/devices/platform-mx2-emma.c b/arch/arm/mach-imx/devices/platform-mx2-emma.c
index 11bd01d402f..0dc0651825b 100644
--- a/arch/arm/mach-imx/devices/platform-mx2-emma.c
+++ b/arch/arm/mach-imx/devices/platform-mx2-emma.c
@@ -12,7 +12,7 @@
 #define imx_mx2_emmaprp_data_entry_single(soc)				\
 	{								\
 		.iobase = soc ## _EMMAPRP_BASE_ADDR,			\
-		.iosize = SZ_32,					\
+		.iosize = SZ_256,					\
 		.irq = soc ## _INT_EMMAPRP,				\
 	}
 
diff --git a/arch/arm/mach-imx/eukrea_mbimxsd51-baseboard.c b/arch/arm/mach-imx/eukrea_mbimxsd51-baseboard.c
deleted file mode 100644
index 9be6c1e69d6..00000000000
--- a/arch/arm/mach-imx/eukrea_mbimxsd51-baseboard.c
+++ /dev/null
@@ -1,231 +0,0 @@
-/*
- * Copyright (C) 2010 Eric Benard - eric@eukrea.com
- *
- * Based on pcm970-baseboard.c which is :
- * Copyright (C) 2008 Juergen Beisert (kernel@pengutronix.de)
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * as published by the Free Software Foundation; either version 2
- * of the License, 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/types.h>
-#include <linux/init.h>
-
-#include <linux/gpio.h>
-#include <linux/interrupt.h>
-#include <linux/leds.h>
-#include <linux/platform_device.h>
-#include <linux/input.h>
-#include <linux/i2c.h>
-#include <video/platform_lcd.h>
-#include <linux/backlight.h>
-
-#include <asm/mach-types.h>
-#include <asm/mach/arch.h>
-#include <asm/mach/time.h>
-#include <asm/mach/map.h>
-
-#include "common.h"
-#include "devices-imx51.h"
-#include "hardware.h"
-#include "iomux-mx51.h"
-
-static iomux_v3_cfg_t eukrea_mbimxsd51_pads[] = {
-	/* LED */
-	MX51_PAD_NANDF_D10__GPIO3_30,
-	/* SWITCH */
-	NEW_PAD_CTRL(MX51_PAD_NANDF_D9__GPIO3_31, PAD_CTL_PUS_22K_UP |
-			PAD_CTL_PKE | PAD_CTL_SRE_FAST |
-			PAD_CTL_DSE_HIGH | PAD_CTL_PUE | PAD_CTL_HYS),
-	/* UART2 */
-	MX51_PAD_UART2_RXD__UART2_RXD,
-	MX51_PAD_UART2_TXD__UART2_TXD,
-	/* UART 3 */
-	MX51_PAD_UART3_RXD__UART3_RXD,
-	MX51_PAD_UART3_TXD__UART3_TXD,
-	MX51_PAD_KEY_COL4__UART3_RTS,
-	MX51_PAD_KEY_COL5__UART3_CTS,
-	/* SD */
-	MX51_PAD_SD1_CMD__SD1_CMD,
-	MX51_PAD_SD1_CLK__SD1_CLK,
-	MX51_PAD_SD1_DATA0__SD1_DATA0,
-	MX51_PAD_SD1_DATA1__SD1_DATA1,
-	MX51_PAD_SD1_DATA2__SD1_DATA2,
-	MX51_PAD_SD1_DATA3__SD1_DATA3,
-	/* SD1 CD */
-	NEW_PAD_CTRL(MX51_PAD_GPIO1_0__SD1_CD, PAD_CTL_PUS_22K_UP |
-			PAD_CTL_PKE | PAD_CTL_SRE_FAST |
-			PAD_CTL_DSE_HIGH | PAD_CTL_PUE | PAD_CTL_HYS),
-	/* SSI */
-	MX51_PAD_AUD3_BB_TXD__AUD3_TXD,
-	MX51_PAD_AUD3_BB_RXD__AUD3_RXD,
-	MX51_PAD_AUD3_BB_CK__AUD3_TXC,
-	MX51_PAD_AUD3_BB_FS__AUD3_TXFS,
-	/* LCD Backlight */
-	MX51_PAD_DI1_D1_CS__GPIO3_4,
-	/* LCD RST */
-	MX51_PAD_CSI1_D9__GPIO3_13,
-};
-
-#define GPIO_LED1	IMX_GPIO_NR(3, 30)
-#define GPIO_SWITCH1	IMX_GPIO_NR(3, 31)
-#define GPIO_LCDRST	IMX_GPIO_NR(3, 13)
-#define GPIO_LCDBL	IMX_GPIO_NR(3, 4)
-
-static void eukrea_mbimxsd51_lcd_power_set(struct plat_lcd_data *pd,
-				   unsigned int power)
-{
-	if (power)
-		gpio_direction_output(GPIO_LCDRST, 1);
-	else
-		gpio_direction_output(GPIO_LCDRST, 0);
-}
-
-static struct plat_lcd_data eukrea_mbimxsd51_lcd_power_data = {
-	.set_power		= eukrea_mbimxsd51_lcd_power_set,
-};
-
-static struct platform_device eukrea_mbimxsd51_lcd_powerdev = {
-	.name			= "platform-lcd",
-	.dev.platform_data	= &eukrea_mbimxsd51_lcd_power_data,
-};
-
-static void eukrea_mbimxsd51_bl_set_intensity(int intensity)
-{
-	if (intensity)
-		gpio_direction_output(GPIO_LCDBL, 1);
-	else
-		gpio_direction_output(GPIO_LCDBL, 0);
-}
-
-static struct generic_bl_info eukrea_mbimxsd51_bl_info = {
-	.name			= "eukrea_mbimxsd51-bl",
-	.max_intensity		= 0xff,
-	.default_intensity	= 0xff,
-	.set_bl_intensity	= eukrea_mbimxsd51_bl_set_intensity,
-};
-
-static struct platform_device eukrea_mbimxsd51_bl_dev = {
-	.name			= "generic-bl",
-	.id			= 1,
-	.dev = {
-		.platform_data	= &eukrea_mbimxsd51_bl_info,
-	},
-};
-
-static const struct gpio_led eukrea_mbimxsd51_leds[] __initconst = {
-	{
-		.name			= "led1",
-		.default_trigger	= "heartbeat",
-		.active_low		= 1,
-		.gpio			= GPIO_LED1,
-	},
-};
-
-static const struct gpio_led_platform_data
-		eukrea_mbimxsd51_led_info __initconst = {
-	.leds		= eukrea_mbimxsd51_leds,
-	.num_leds	= ARRAY_SIZE(eukrea_mbimxsd51_leds),
-};
-
-static struct gpio_keys_button eukrea_mbimxsd51_gpio_buttons[] = {
-	{
-		.gpio		= GPIO_SWITCH1,
-		.code		= BTN_0,
-		.desc		= "BP1",
-		.active_low	= 1,
-		.wakeup		= 1,
-	},
-};
-
-static const struct gpio_keys_platform_data
-		eukrea_mbimxsd51_button_data __initconst = {
-	.buttons	= eukrea_mbimxsd51_gpio_buttons,
-	.nbuttons	= ARRAY_SIZE(eukrea_mbimxsd51_gpio_buttons),
-};
-
-static const struct imxuart_platform_data uart_pdata __initconst = {
-	.flags = IMXUART_HAVE_RTSCTS,
-};
-
-static struct i2c_board_info eukrea_mbimxsd51_i2c_devices[] = {
-	{
-		I2C_BOARD_INFO("tlv320aic23", 0x1a),
-	},
-};
-
-static const
-struct imx_ssi_platform_data eukrea_mbimxsd51_ssi_pdata __initconst = {
-	.flags = IMX_SSI_SYN | IMX_SSI_NET | IMX_SSI_USE_I2S_SLAVE,
-};
-
-static int screen_type;
-
-static int __init eukrea_mbimxsd51_screen_type(char *options)
-{
-	if (!strcmp(options, "dvi"))
-		screen_type = 1;
-	else if (!strcmp(options, "tft"))
-		screen_type = 0;
-
-	return 0;
-}
-__setup("screen_type=", eukrea_mbimxsd51_screen_type);
-
-/*
- * system init for baseboard usage. Will be called by cpuimx51sd init.
- *
- * Add platform devices present on this baseboard and init
- * them from CPU side as far as required to use them later on
- */
-void __init eukrea_mbimxsd51_baseboard_init(void)
-{
-	if (mxc_iomux_v3_setup_multiple_pads(eukrea_mbimxsd51_pads,
-			ARRAY_SIZE(eukrea_mbimxsd51_pads)))
-		printk(KERN_ERR "error setting mbimxsd pads !\n");
-
-	imx51_add_imx_uart(1, NULL);
-	imx51_add_imx_uart(2, &uart_pdata);
-
-	imx51_add_sdhci_esdhc_imx(0, NULL);
-
-	imx51_add_imx_ssi(0, &eukrea_mbimxsd51_ssi_pdata);
-
-	gpio_request(GPIO_LED1, "LED1");
-	gpio_direction_output(GPIO_LED1, 1);
-	gpio_free(GPIO_LED1);
-
-	gpio_request(GPIO_SWITCH1, "SWITCH1");
-	gpio_direction_input(GPIO_SWITCH1);
-	gpio_free(GPIO_SWITCH1);
-
-	gpio_request(GPIO_LCDRST, "LCDRST");
-	gpio_direction_output(GPIO_LCDRST, 0);
-	gpio_request(GPIO_LCDBL, "LCDBL");
-	gpio_direction_output(GPIO_LCDBL, 0);
-	if (!screen_type) {
-		platform_device_register(&eukrea_mbimxsd51_bl_dev);
-		platform_device_register(&eukrea_mbimxsd51_lcd_powerdev);
-	} else {
-		gpio_free(GPIO_LCDRST);
-		gpio_free(GPIO_LCDBL);
-	}
-
-	i2c_register_board_info(0, eukrea_mbimxsd51_i2c_devices,
-				ARRAY_SIZE(eukrea_mbimxsd51_i2c_devices));
-
-	gpio_led_register_device(-1, &eukrea_mbimxsd51_led_info);
-	imx_add_gpio_keys(&eukrea_mbimxsd51_button_data);
-	imx_add_platform_device("eukrea_tlv320", 0, NULL, 0, NULL, 0);
-}
diff --git a/arch/arm/mach-imx/imx25-dt.c b/arch/arm/mach-imx/imx25-dt.c
index 3e1ec5ffe63..42a65e06744 100644
--- a/arch/arm/mach-imx/imx25-dt.c
+++ b/arch/arm/mach-imx/imx25-dt.c
@@ -38,7 +38,6 @@ DT_MACHINE_START(IMX25_DT, "Freescale i.MX25 (Device Tree Support)")
 	.map_io		= mx25_map_io,
 	.init_early	= imx25_init_early,
 	.init_irq	= mx25_init_irq,
-	.handle_irq	= imx25_handle_irq,
 	.init_time	= imx25_timer_init,
 	.init_machine	= imx25_dt_init,
 	.dt_compat	= imx25_dt_board_compat,
diff --git a/arch/arm/mach-imx/imx27-dt.c b/arch/arm/mach-imx/imx27-dt.c
index 4e235ecb402..17bd4058133 100644
--- a/arch/arm/mach-imx/imx27-dt.c
+++ b/arch/arm/mach-imx/imx27-dt.c
@@ -43,7 +43,6 @@ DT_MACHINE_START(IMX27_DT, "Freescale i.MX27 (Device Tree Support)")
 	.map_io		= mx27_map_io,
 	.init_early	= imx27_init_early,
 	.init_irq	= mx27_init_irq,
-	.handle_irq	= imx27_handle_irq,
 	.init_time	= imx27_timer_init,
 	.init_machine	= imx27_dt_init,
 	.dt_compat	= imx27_dt_board_compat,
diff --git a/arch/arm/mach-imx/imx31-dt.c b/arch/arm/mach-imx/imx31-dt.c
index e1e70ef7bc2..581f4d6c9b8 100644
--- a/arch/arm/mach-imx/imx31-dt.c
+++ b/arch/arm/mach-imx/imx31-dt.c
@@ -39,7 +39,6 @@ DT_MACHINE_START(IMX31_DT, "Freescale i.MX31 (Device Tree Support)")
 	.map_io		= mx31_map_io,
 	.init_early	= imx31_init_early,
 	.init_irq	= mx31_init_irq,
-	.handle_irq	= imx31_handle_irq,
 	.init_time	= imx31_dt_timer_init,
 	.init_machine	= imx31_dt_init,
 	.dt_compat	= imx31_dt_board_compat,
diff --git a/arch/arm/mach-imx/imx35-dt.c b/arch/arm/mach-imx/imx35-dt.c
index 9d48e0065a6..a62854c5924 100644
--- a/arch/arm/mach-imx/imx35-dt.c
+++ b/arch/arm/mach-imx/imx35-dt.c
@@ -43,7 +43,6 @@ DT_MACHINE_START(IMX35_DT, "Freescale i.MX35 (Device Tree Support)")
 	.map_io		= mx35_map_io,
 	.init_early	= imx35_init_early,
 	.init_irq	= imx35_irq_init,
-	.handle_irq	= imx35_handle_irq,
 	.init_machine	= imx35_dt_init,
 	.dt_compat	= imx35_dt_board_compat,
 	.restart	= mxc_restart,
diff --git a/arch/arm/mach-imx/imx51-dt.c b/arch/arm/mach-imx/imx51-dt.c
index 0230d78d141..b8cd968faa5 100644
--- a/arch/arm/mach-imx/imx51-dt.c
+++ b/arch/arm/mach-imx/imx51-dt.c
@@ -38,7 +38,6 @@ DT_MACHINE_START(IMX51_DT, "Freescale i.MX51 (Device Tree Support)")
 	.map_io		= mx51_map_io,
 	.init_early	= imx51_init_early,
 	.init_irq	= mx51_init_irq,
-	.handle_irq	= imx51_handle_irq,
 	.init_machine	= imx51_dt_init,
 	.init_late	= imx51_init_late,
 	.dt_compat	= imx51_dt_board_compat,
diff --git a/arch/arm/mach-imx/mach-apf9328.c b/arch/arm/mach-imx/mach-apf9328.c
index 067580b2969..ebbb5ab6352 100644
--- a/arch/arm/mach-imx/mach-apf9328.c
+++ b/arch/arm/mach-imx/mach-apf9328.c
@@ -142,7 +142,6 @@ MACHINE_START(APF9328, "Armadeus APF9328")
 	.map_io       = mx1_map_io,
 	.init_early   = imx1_init_early,
 	.init_irq     = mx1_init_irq,
-	.handle_irq   = imx1_handle_irq,
 	.init_time	= apf9328_timer_init,
 	.init_machine = apf9328_init,
 	.restart	= mxc_restart,
diff --git a/arch/arm/mach-imx/mach-armadillo5x0.c b/arch/arm/mach-imx/mach-armadillo5x0.c
index 58b864a3fc2..39406b7e322 100644
--- a/arch/arm/mach-imx/mach-armadillo5x0.c
+++ b/arch/arm/mach-imx/mach-armadillo5x0.c
@@ -562,7 +562,6 @@ MACHINE_START(ARMADILLO5X0, "Armadillo-500")
 	.map_io = mx31_map_io,
 	.init_early = imx31_init_early,
 	.init_irq = mx31_init_irq,
-	.handle_irq = imx31_handle_irq,
 	.init_time	= armadillo5x0_timer_init,
 	.init_machine = armadillo5x0_init,
 	.restart	= mxc_restart,
diff --git a/arch/arm/mach-imx/mach-bug.c b/arch/arm/mach-imx/mach-bug.c
index 2d00476f7d2..c97d7cb3913 100644
--- a/arch/arm/mach-imx/mach-bug.c
+++ b/arch/arm/mach-imx/mach-bug.c
@@ -57,7 +57,6 @@ MACHINE_START(BUG, "BugLabs BUGBase")
 	.map_io = mx31_map_io,
 	.init_early = imx31_init_early,
 	.init_irq = mx31_init_irq,
-	.handle_irq = imx31_handle_irq,
 	.init_time	= bug_timer_init,
 	.init_machine = bug_board_init,
 	.restart	= mxc_restart,
diff --git a/arch/arm/mach-imx/mach-cpuimx27.c b/arch/arm/mach-imx/mach-cpuimx27.c
index ea50870bda8..75b7b6aa272 100644
--- a/arch/arm/mach-imx/mach-cpuimx27.c
+++ b/arch/arm/mach-imx/mach-cpuimx27.c
@@ -314,7 +314,6 @@ MACHINE_START(EUKREA_CPUIMX27, "EUKREA CPUIMX27")
 	.map_io = mx27_map_io,
 	.init_early = imx27_init_early,
 	.init_irq = mx27_init_irq,
-	.handle_irq = imx27_handle_irq,
 	.init_time	= eukrea_cpuimx27_timer_init,
 	.init_machine = eukrea_cpuimx27_init,
 	.restart	= mxc_restart,
diff --git a/arch/arm/mach-imx/mach-cpuimx35.c b/arch/arm/mach-imx/mach-cpuimx35.c
index 65e4c53e155..1ffa2716904 100644
--- a/arch/arm/mach-imx/mach-cpuimx35.c
+++ b/arch/arm/mach-imx/mach-cpuimx35.c
@@ -199,7 +199,6 @@ MACHINE_START(EUKREA_CPUIMX35SD, "Eukrea CPUIMX35")
 	.map_io = mx35_map_io,
 	.init_early = imx35_init_early,
 	.init_irq = mx35_init_irq,
-	.handle_irq = imx35_handle_irq,
 	.init_time	= eukrea_cpuimx35_timer_init,
 	.init_machine = eukrea_cpuimx35_init,
 	.restart	= mxc_restart,
diff --git a/arch/arm/mach-imx/mach-cpuimx51sd.c b/arch/arm/mach-imx/mach-cpuimx51sd.c
deleted file mode 100644
index 1fba2b8e983..00000000000
--- a/arch/arm/mach-imx/mach-cpuimx51sd.c
+++ /dev/null
@@ -1,364 +0,0 @@
-/*
- *
- * Copyright (C) 2010 Eric Bénard <eric@eukrea.com>
- *
- * based on board-mx51_babbage.c which is
- * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
- * Copyright (C) 2009-2010 Amit Kucheria <amit.kucheria@canonical.com>
- *
- * 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/init.h>
-#include <linux/platform_device.h>
-#include <linux/i2c.h>
-#include <linux/i2c/tsc2007.h>
-#include <linux/gpio.h>
-#include <linux/delay.h>
-#include <linux/io.h>
-#include <linux/interrupt.h>
-#include <linux/i2c-gpio.h>
-#include <linux/spi/spi.h>
-#include <linux/can/platform/mcp251x.h>
-
-#include <asm/setup.h>
-#include <asm/mach-types.h>
-#include <asm/mach/arch.h>
-#include <asm/mach/time.h>
-
-#include "common.h"
-#include "devices-imx51.h"
-#include "eukrea-baseboards.h"
-#include "hardware.h"
-#include "iomux-mx51.h"
-
-#define USBH1_RST		IMX_GPIO_NR(2, 28)
-#define ETH_RST			IMX_GPIO_NR(2, 31)
-#define TSC2007_IRQGPIO_REV2	IMX_GPIO_NR(3, 12)
-#define TSC2007_IRQGPIO_REV3	IMX_GPIO_NR(4, 0)
-#define CAN_IRQGPIO		IMX_GPIO_NR(1, 1)
-#define CAN_RST			IMX_GPIO_NR(4, 15)
-#define CAN_NCS			IMX_GPIO_NR(4, 24)
-#define CAN_RXOBF_REV2		IMX_GPIO_NR(1, 4)
-#define CAN_RXOBF_REV3		IMX_GPIO_NR(3, 12)
-#define CAN_RX1BF		IMX_GPIO_NR(1, 6)
-#define CAN_TXORTS		IMX_GPIO_NR(1, 7)
-#define CAN_TX1RTS		IMX_GPIO_NR(1, 8)
-#define CAN_TX2RTS		IMX_GPIO_NR(1, 9)
-#define I2C_SCL			IMX_GPIO_NR(4, 16)
-#define I2C_SDA			IMX_GPIO_NR(4, 17)
-
-/* USB_CTRL_1 */
-#define MX51_USB_CTRL_1_OFFSET		0x10
-#define MX51_USB_CTRL_UH1_EXT_CLK_EN	(1 << 25)
-
-#define	MX51_USB_PLLDIV_12_MHZ		0x00
-#define	MX51_USB_PLL_DIV_19_2_MHZ	0x01
-#define	MX51_USB_PLL_DIV_24_MHZ		0x02
-
-static iomux_v3_cfg_t eukrea_cpuimx51sd_pads[] = {
-	/* UART1 */
-	MX51_PAD_UART1_RXD__UART1_RXD,
-	MX51_PAD_UART1_TXD__UART1_TXD,
-	MX51_PAD_UART1_RTS__UART1_RTS,
-	MX51_PAD_UART1_CTS__UART1_CTS,
-
-	/* USB HOST1 */
-	MX51_PAD_USBH1_CLK__USBH1_CLK,
-	MX51_PAD_USBH1_DIR__USBH1_DIR,
-	MX51_PAD_USBH1_NXT__USBH1_NXT,
-	MX51_PAD_USBH1_DATA0__USBH1_DATA0,
-	MX51_PAD_USBH1_DATA1__USBH1_DATA1,
-	MX51_PAD_USBH1_DATA2__USBH1_DATA2,
-	MX51_PAD_USBH1_DATA3__USBH1_DATA3,
-	MX51_PAD_USBH1_DATA4__USBH1_DATA4,
-	MX51_PAD_USBH1_DATA5__USBH1_DATA5,
-	MX51_PAD_USBH1_DATA6__USBH1_DATA6,
-	MX51_PAD_USBH1_DATA7__USBH1_DATA7,
-	MX51_PAD_USBH1_STP__USBH1_STP,
-	MX51_PAD_EIM_CS3__GPIO2_28,		/* PHY nRESET */
-
-	/* FEC */
-	MX51_PAD_EIM_DTACK__GPIO2_31,		/* PHY nRESET */
-
-	/* HSI2C */
-	MX51_PAD_I2C1_CLK__GPIO4_16,
-	MX51_PAD_I2C1_DAT__GPIO4_17,
-
-	/* I2C1 */
-	MX51_PAD_SD2_CMD__I2C1_SCL,
-	MX51_PAD_SD2_CLK__I2C1_SDA,
-
-	/* CAN */
-	MX51_PAD_CSPI1_MOSI__ECSPI1_MOSI,
-	MX51_PAD_CSPI1_MISO__ECSPI1_MISO,
-	MX51_PAD_CSPI1_SCLK__ECSPI1_SCLK,
-	MX51_PAD_CSPI1_SS0__GPIO4_24,		/* nCS */
-	MX51_PAD_CSI2_PIXCLK__GPIO4_15,		/* nReset */
-	MX51_PAD_GPIO1_1__GPIO1_1,		/* IRQ */
-	MX51_PAD_GPIO1_4__GPIO1_4,		/* Control signals */
-	MX51_PAD_GPIO1_6__GPIO1_6,
-	MX51_PAD_GPIO1_7__GPIO1_7,
-	MX51_PAD_GPIO1_8__GPIO1_8,
-	MX51_PAD_GPIO1_9__GPIO1_9,
-
-	/* Touchscreen */
-	/* IRQ */
-	NEW_PAD_CTRL(MX51_PAD_GPIO_NAND__GPIO_NAND, PAD_CTL_PUS_22K_UP |
-			PAD_CTL_PKE | PAD_CTL_SRE_FAST |
-			PAD_CTL_DSE_HIGH | PAD_CTL_PUE | PAD_CTL_HYS),
-	NEW_PAD_CTRL(MX51_PAD_NANDF_D8__GPIO4_0, PAD_CTL_PUS_22K_UP |
-			PAD_CTL_PKE | PAD_CTL_SRE_FAST |
-			PAD_CTL_DSE_HIGH | PAD_CTL_PUE | PAD_CTL_HYS),
-};
-
-static const struct imxuart_platform_data uart_pdata __initconst = {
-	.flags = IMXUART_HAVE_RTSCTS,
-};
-
-static int tsc2007_get_pendown_state(struct device *dev)
-{
-	if (mx51_revision() < IMX_CHIP_REVISION_3_0)
-		return !gpio_get_value(TSC2007_IRQGPIO_REV2);
-	else
-		return !gpio_get_value(TSC2007_IRQGPIO_REV3);
-}
-
-static struct tsc2007_platform_data tsc2007_info = {
-	.model			= 2007,
-	.x_plate_ohms		= 180,
-	.get_pendown_state	= tsc2007_get_pendown_state,
-};
-
-static struct i2c_board_info eukrea_cpuimx51sd_i2c_devices[] = {
-	{
-		I2C_BOARD_INFO("pcf8563", 0x51),
-	}, {
-		I2C_BOARD_INFO("tsc2007", 0x49),
-		.platform_data	= &tsc2007_info,
-	},
-};
-
-static const struct mxc_nand_platform_data
-		eukrea_cpuimx51sd_nand_board_info __initconst = {
-	.width		= 1,
-	.hw_ecc		= 1,
-	.flash_bbt	= 1,
-};
-
-/* This function is board specific as the bit mask for the plldiv will also
-be different for other Freescale SoCs, thus a common bitmask is not
-possible and cannot get place in /plat-mxc/ehci.c.*/
-static int initialize_otg_port(struct platform_device *pdev)
-{
-	u32 v;
-	void __iomem *usb_base;
-	void __iomem *usbother_base;
-
-	usb_base = ioremap(MX51_USB_OTG_BASE_ADDR, SZ_4K);
-	if (!usb_base)
-		return -ENOMEM;
-	usbother_base = usb_base + MX5_USBOTHER_REGS_OFFSET;
-
-	/* Set the PHY clock to 19.2MHz */
-	v = __raw_readl(usbother_base + MXC_USB_PHY_CTR_FUNC2_OFFSET);
-	v &= ~MX5_USB_UTMI_PHYCTRL1_PLLDIV_MASK;
-	v |= MX51_USB_PLL_DIV_19_2_MHZ;
-	__raw_writel(v, usbother_base + MXC_USB_PHY_CTR_FUNC2_OFFSET);
-	iounmap(usb_base);
-
-	mdelay(10);
-
-	return mx51_initialize_usb_hw(0, MXC_EHCI_INTERNAL_PHY);
-}
-
-static int initialize_usbh1_port(struct platform_device *pdev)
-{
-	u32 v;
-	void __iomem *usb_base;
-	void __iomem *usbother_base;
-
-	usb_base = ioremap(MX51_USB_OTG_BASE_ADDR, SZ_4K);
-	if (!usb_base)
-		return -ENOMEM;
-	usbother_base = usb_base + MX5_USBOTHER_REGS_OFFSET;
-
-	/* The clock for the USBH1 ULPI port will come from the PHY. */
-	v = __raw_readl(usbother_base + MX51_USB_CTRL_1_OFFSET);
-	__raw_writel(v | MX51_USB_CTRL_UH1_EXT_CLK_EN,
-			usbother_base + MX51_USB_CTRL_1_OFFSET);
-	iounmap(usb_base);
-
-	mdelay(10);
-
-	return mx51_initialize_usb_hw(1, MXC_EHCI_POWER_PINS_ENABLED |
-			MXC_EHCI_ITC_NO_THRESHOLD);
-}
-
-static const struct mxc_usbh_platform_data dr_utmi_config __initconst = {
-	.init		= initialize_otg_port,
-	.portsc	= MXC_EHCI_UTMI_16BIT,
-};
-
-static const struct fsl_usb2_platform_data usb_pdata __initconst = {
-	.operating_mode	= FSL_USB2_DR_DEVICE,
-	.phy_mode	= FSL_USB2_PHY_UTMI_WIDE,
-};
-
-static const struct mxc_usbh_platform_data usbh1_config __initconst = {
-	.init		= initialize_usbh1_port,
-	.portsc	= MXC_EHCI_MODE_ULPI,
-};
-
-static bool otg_mode_host __initdata;
-
-static int __init eukrea_cpuimx51sd_otg_mode(char *options)
-{
-	if (!strcmp(options, "host"))
-		otg_mode_host = true;
-	else if (!strcmp(options, "device"))
-		otg_mode_host = false;
-	else
-		pr_info("otg_mode neither \"host\" nor \"device\". "
-			"Defaulting to device\n");
-	return 1;
-}
-__setup("otg_mode=", eukrea_cpuimx51sd_otg_mode);
-
-static struct i2c_gpio_platform_data pdata = {
-	.sda_pin		= I2C_SDA,
-	.sda_is_open_drain	= 0,
-	.scl_pin		= I2C_SCL,
-	.scl_is_open_drain	= 0,
-	.udelay			= 2,
-};
-
-static struct platform_device hsi2c_gpio_device = {
-	.name			= "i2c-gpio",
-	.id			= 0,
-	.dev.platform_data	= &pdata,
-};
-
-static struct mcp251x_platform_data mcp251x_info = {
-	.oscillator_frequency = 24E6,
-};
-
-static struct spi_board_info cpuimx51sd_spi_device[] = {
-	{
-		.modalias        = "mcp2515",
-		.max_speed_hz    = 10000000,
-		.bus_num         = 0,
-		.mode		= SPI_MODE_0,
-		.chip_select     = 0,
-		.platform_data   = &mcp251x_info,
-		/* irq number is run-time assigned */
-	},
-};
-
-static int cpuimx51sd_spi1_cs[] = {
-	CAN_NCS,
-};
-
-static const struct spi_imx_master cpuimx51sd_ecspi1_pdata __initconst = {
-	.chipselect	= cpuimx51sd_spi1_cs,
-	.num_chipselect	= ARRAY_SIZE(cpuimx51sd_spi1_cs),
-};
-
-static struct platform_device *rev2_platform_devices[] __initdata = {
-	&hsi2c_gpio_device,
-};
-
-static const struct imxi2c_platform_data cpuimx51sd_i2c_data __initconst = {
-	.bitrate = 100000,
-};
-
-static void __init eukrea_cpuimx51sd_init(void)
-{
-	imx51_soc_init();
-
-	mxc_iomux_v3_setup_multiple_pads(eukrea_cpuimx51sd_pads,
-					ARRAY_SIZE(eukrea_cpuimx51sd_pads));
-
-	imx51_add_imx_uart(0, &uart_pdata);
-	imx51_add_mxc_nand(&eukrea_cpuimx51sd_nand_board_info);
-	imx51_add_imx2_wdt(0);
-
-	gpio_request(ETH_RST, "eth_rst");
-	gpio_set_value(ETH_RST, 1);
-	imx51_add_fec(NULL);
-
-	gpio_request(CAN_IRQGPIO, "can_irq");
-	gpio_direction_input(CAN_IRQGPIO);
-	gpio_free(CAN_IRQGPIO);
-	gpio_request(CAN_NCS, "can_ncs");
-	gpio_direction_output(CAN_NCS, 1);
-	gpio_free(CAN_NCS);
-	gpio_request(CAN_RST, "can_rst");
-	gpio_direction_output(CAN_RST, 0);
-	msleep(20);
-	gpio_set_value(CAN_RST, 1);
-	imx51_add_ecspi(0, &cpuimx51sd_ecspi1_pdata);
-	cpuimx51sd_spi_device[0].irq = gpio_to_irq(CAN_IRQGPIO);
-	spi_register_board_info(cpuimx51sd_spi_device,
-				ARRAY_SIZE(cpuimx51sd_spi_device));
-
-	if (mx51_revision() < IMX_CHIP_REVISION_3_0) {
-		eukrea_cpuimx51sd_i2c_devices[1].irq =
-			gpio_to_irq(TSC2007_IRQGPIO_REV2),
-		platform_add_devices(rev2_platform_devices,
-			ARRAY_SIZE(rev2_platform_devices));
-		gpio_request(TSC2007_IRQGPIO_REV2, "tsc2007_irq");
-		gpio_direction_input(TSC2007_IRQGPIO_REV2);
-		gpio_free(TSC2007_IRQGPIO_REV2);
-	} else {
-		eukrea_cpuimx51sd_i2c_devices[1].irq =
-			gpio_to_irq(TSC2007_IRQGPIO_REV3),
-		imx51_add_imx_i2c(0, &cpuimx51sd_i2c_data);
-		gpio_request(TSC2007_IRQGPIO_REV3, "tsc2007_irq");
-		gpio_direction_input(TSC2007_IRQGPIO_REV3);
-		gpio_free(TSC2007_IRQGPIO_REV3);
-	}
-
-	i2c_register_board_info(0, eukrea_cpuimx51sd_i2c_devices,
-			ARRAY_SIZE(eukrea_cpuimx51sd_i2c_devices));
-
-	if (otg_mode_host)
-		imx51_add_mxc_ehci_otg(&dr_utmi_config);
-	else {
-		initialize_otg_port(NULL);
-		imx51_add_fsl_usb2_udc(&usb_pdata);
-	}
-
-	gpio_request(USBH1_RST, "usb_rst");
-	gpio_direction_output(USBH1_RST, 0);
-	msleep(20);
-	gpio_set_value(USBH1_RST, 1);
-	imx51_add_mxc_ehci_hs(1, &usbh1_config);
-
-#ifdef CONFIG_MACH_EUKREA_MBIMXSD51_BASEBOARD
-	eukrea_mbimxsd51_baseboard_init();
-#endif
-}
-
-static void __init eukrea_cpuimx51sd_timer_init(void)
-{
-	mx51_clocks_init(32768, 24000000, 22579200, 0);
-}
-
-MACHINE_START(EUKREA_CPUIMX51SD, "Eukrea CPUIMX51SD")
-	/* Maintainer: Eric Bénard <eric@eukrea.com> */
-	.atag_offset = 0x100,
-	.map_io = mx51_map_io,
-	.init_early = imx51_init_early,
-	.init_irq = mx51_init_irq,
-	.handle_irq = imx51_handle_irq,
-	.init_time	= eukrea_cpuimx51sd_timer_init,
-	.init_machine = eukrea_cpuimx51sd_init,
-	.init_late	= imx51_init_late,
-	.restart	= mxc_restart,
-MACHINE_END
diff --git a/arch/arm/mach-imx/mach-eukrea_cpuimx25.c b/arch/arm/mach-imx/mach-eukrea_cpuimx25.c
index 4bf45442424..e978dda1434 100644
--- a/arch/arm/mach-imx/mach-eukrea_cpuimx25.c
+++ b/arch/arm/mach-imx/mach-eukrea_cpuimx25.c
@@ -165,7 +165,6 @@ MACHINE_START(EUKREA_CPUIMX25SD, "Eukrea CPUIMX25")
 	.map_io = mx25_map_io,
 	.init_early = imx25_init_early,
 	.init_irq = mx25_init_irq,
-	.handle_irq = imx25_handle_irq,
 	.init_time = eukrea_cpuimx25_timer_init,
 	.init_machine = eukrea_cpuimx25_init,
 	.restart	= mxc_restart,
diff --git a/arch/arm/mach-imx/mach-imx27_visstrim_m10.c b/arch/arm/mach-imx/mach-imx27_visstrim_m10.c
index 97f9c6297fc..b61bd8ed556 100644
--- a/arch/arm/mach-imx/mach-imx27_visstrim_m10.c
+++ b/arch/arm/mach-imx/mach-imx27_visstrim_m10.c
@@ -604,7 +604,6 @@ MACHINE_START(IMX27_VISSTRIM_M10, "Vista Silicon Visstrim_M10")
 	.map_io = mx27_map_io,
 	.init_early = imx27_init_early,
 	.init_irq = mx27_init_irq,
-	.handle_irq = imx27_handle_irq,
 	.init_time	= visstrim_m10_timer_init,
 	.init_machine = visstrim_m10_board_init,
 	.restart	= mxc_restart,
diff --git a/arch/arm/mach-imx/mach-imx27ipcam.c b/arch/arm/mach-imx/mach-imx27ipcam.c
index 1a851aea683..bb3ca042968 100644
--- a/arch/arm/mach-imx/mach-imx27ipcam.c
+++ b/arch/arm/mach-imx/mach-imx27ipcam.c
@@ -71,7 +71,6 @@ MACHINE_START(IMX27IPCAM, "Freescale IMX27IPCAM")
 	.map_io = mx27_map_io,
 	.init_early = imx27_init_early,
 	.init_irq = mx27_init_irq,
-	.handle_irq = imx27_handle_irq,
 	.init_time	= mx27ipcam_timer_init,
 	.init_machine = mx27ipcam_init,
 	.restart	= mxc_restart,
diff --git a/arch/arm/mach-imx/mach-imx27lite.c b/arch/arm/mach-imx/mach-imx27lite.c
index 3da2e3e44ce..9992089d3ad 100644
--- a/arch/arm/mach-imx/mach-imx27lite.c
+++ b/arch/arm/mach-imx/mach-imx27lite.c
@@ -77,7 +77,6 @@ MACHINE_START(IMX27LITE, "LogicPD i.MX27LITE")
 	.map_io = mx27_map_io,
 	.init_early = imx27_init_early,
 	.init_irq = mx27_init_irq,
-	.handle_irq = imx27_handle_irq,
 	.init_time	= mx27lite_timer_init,
 	.init_machine = mx27lite_init,
 	.restart	= mxc_restart,
diff --git a/arch/arm/mach-imx/mach-imx50.c b/arch/arm/mach-imx/mach-imx50.c
index 77b77a92bb5..b899c0b59af 100644
--- a/arch/arm/mach-imx/mach-imx50.c
+++ b/arch/arm/mach-imx/mach-imx50.c
@@ -31,7 +31,6 @@ static const char *imx50_dt_board_compat[] __initconst = {
 DT_MACHINE_START(IMX50_DT, "Freescale i.MX50 (Device Tree Support)")
 	.map_io		= mx53_map_io,
 	.init_irq	= mx53_init_irq,
-	.handle_irq	= imx50_handle_irq,
 	.init_machine	= imx50_dt_init,
 	.dt_compat	= imx50_dt_board_compat,
 	.restart	= mxc_restart,
diff --git a/arch/arm/mach-imx/mach-imx53.c b/arch/arm/mach-imx/mach-imx53.c
index 65850908a4b..2bad387956c 100644
--- a/arch/arm/mach-imx/mach-imx53.c
+++ b/arch/arm/mach-imx/mach-imx53.c
@@ -40,7 +40,6 @@ DT_MACHINE_START(IMX53_DT, "Freescale i.MX53 (Device Tree Support)")
 	.map_io		= mx53_map_io,
 	.init_early	= imx53_init_early,
 	.init_irq	= mx53_init_irq,
-	.handle_irq	= imx53_handle_irq,
 	.init_machine	= imx53_dt_init,
 	.init_late	= imx53_init_late,
 	.dt_compat	= imx53_dt_board_compat,
diff --git a/arch/arm/mach-imx/mach-imx6sx.c b/arch/arm/mach-imx/mach-imx6sx.c
new file mode 100644
index 00000000000..02fccf6033a
--- /dev/null
+++ b/arch/arm/mach-imx/mach-imx6sx.c
@@ -0,0 +1,51 @@
+/*
+ * Copyright 2014 Freescale Semiconductor, 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.
+ */
+
+#include <linux/irqchip.h>
+#include <linux/of_platform.h>
+#include <asm/mach/arch.h>
+#include <asm/mach/map.h>
+
+#include "common.h"
+
+static void __init imx6sx_init_machine(void)
+{
+	struct device *parent;
+
+	mxc_arch_reset_init_dt();
+
+	parent = imx_soc_device_init();
+	if (parent == NULL)
+		pr_warn("failed to initialize soc device\n");
+
+	of_platform_populate(NULL, of_default_bus_match_table, NULL, parent);
+
+	imx_anatop_init();
+}
+
+static void __init imx6sx_init_irq(void)
+{
+	imx_init_revision_from_anatop();
+	imx_init_l2cache();
+	imx_src_init();
+	imx_gpc_init();
+	irqchip_init();
+}
+
+static const char *imx6sx_dt_compat[] __initconst = {
+	"fsl,imx6sx",
+	NULL,
+};
+
+DT_MACHINE_START(IMX6SX, "Freescale i.MX6 SoloX (Device Tree)")
+	.map_io		= debug_ll_io_init,
+	.init_irq	= imx6sx_init_irq,
+	.init_machine	= imx6sx_init_machine,
+	.dt_compat	= imx6sx_dt_compat,
+	.restart	= mxc_restart,
+MACHINE_END
diff --git a/arch/arm/mach-imx/mach-kzm_arm11_01.c b/arch/arm/mach-imx/mach-kzm_arm11_01.c
index c7bc41d6b46..31df4361996 100644
--- a/arch/arm/mach-imx/mach-kzm_arm11_01.c
+++ b/arch/arm/mach-imx/mach-kzm_arm11_01.c
@@ -289,7 +289,6 @@ MACHINE_START(KZM_ARM11_01, "Kyoto Microcomputer Co., Ltd. KZM-ARM11-01")
 	.map_io = kzm_map_io,
 	.init_early = imx31_init_early,
 	.init_irq = mx31_init_irq,
-	.handle_irq = imx31_handle_irq,
 	.init_time	= kzm_timer_init,
 	.init_machine = kzm_board_init,
 	.restart	= mxc_restart,
diff --git a/arch/arm/mach-imx/mach-mx1ads.c b/arch/arm/mach-imx/mach-mx1ads.c
index 9f883e4d6fc..77fda3de429 100644
--- a/arch/arm/mach-imx/mach-mx1ads.c
+++ b/arch/arm/mach-imx/mach-mx1ads.c
@@ -138,7 +138,6 @@ MACHINE_START(MX1ADS, "Freescale MX1ADS")
 	.map_io = mx1_map_io,
 	.init_early = imx1_init_early,
 	.init_irq = mx1_init_irq,
-	.handle_irq = imx1_handle_irq,
 	.init_time	= mx1ads_timer_init,
 	.init_machine = mx1ads_init,
 	.restart	= mxc_restart,
@@ -149,7 +148,6 @@ MACHINE_START(MXLADS, "Freescale MXLADS")
 	.map_io = mx1_map_io,
 	.init_early = imx1_init_early,
 	.init_irq = mx1_init_irq,
-	.handle_irq = imx1_handle_irq,
 	.init_time	= mx1ads_timer_init,
 	.init_machine = mx1ads_init,
 	.restart	= mxc_restart,
diff --git a/arch/arm/mach-imx/mach-mx21ads.c b/arch/arm/mach-imx/mach-mx21ads.c
index a06aa4dc37f..703ce31d737 100644
--- a/arch/arm/mach-imx/mach-mx21ads.c
+++ b/arch/arm/mach-imx/mach-mx21ads.c
@@ -17,51 +17,46 @@
 #include <linux/platform_device.h>
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/physmap.h>
+#include <linux/basic_mmio_gpio.h>
 #include <linux/gpio.h>
+#include <linux/regulator/fixed.h>
+#include <linux/regulator/machine.h>
 #include <asm/mach-types.h>
 #include <asm/mach/arch.h>
-#include <asm/mach/time.h>
-#include <asm/mach/map.h>
 
 #include "common.h"
 #include "devices-imx21.h"
 #include "hardware.h"
 #include "iomux-mx21.h"
 
-/*
- * Memory-mapped I/O on MX21ADS base board
- */
-#define MX21ADS_MMIO_BASE_ADDR   0xf5000000
-#define MX21ADS_MMIO_SIZE        0xc00000
-
-#define MX21ADS_REG_ADDR(offset)    (void __force __iomem *) \
-		(MX21ADS_MMIO_BASE_ADDR + (offset))
+#define MX21ADS_CS8900A_REG		(MX21_CS1_BASE_ADDR + 0x000000)
+#define MX21ADS_ST16C255_IOBASE_REG	(MX21_CS1_BASE_ADDR + 0x200000)
+#define MX21ADS_VERSION_REG		(MX21_CS1_BASE_ADDR + 0x400000)
+#define MX21ADS_IO_REG			(MX21_CS1_BASE_ADDR + 0x800000)
 
-#define MX21ADS_CS8900A_MMIO_SIZE   0x200000
-#define MX21ADS_CS8900A_IRQ_GPIO    IMX_GPIO_NR(5, 11)
-#define MX21ADS_ST16C255_IOBASE_REG MX21ADS_REG_ADDR(0x200000)
-#define MX21ADS_VERSION_REG         MX21ADS_REG_ADDR(0x400000)
-#define MX21ADS_IO_REG              MX21ADS_REG_ADDR(0x800000)
+#define MX21ADS_MMC_CD			IMX_GPIO_NR(4, 25)
+#define MX21ADS_CS8900A_IRQ_GPIO	IMX_GPIO_NR(5, 11)
+#define MX21ADS_MMGPIO_BASE		(6 * 32)
 
 /* MX21ADS_IO_REG bit definitions */
-#define MX21ADS_IO_SD_WP        0x0001 /* read */
-#define MX21ADS_IO_TP6          0x0001 /* write */
-#define MX21ADS_IO_SW_SEL       0x0002 /* read */
-#define MX21ADS_IO_TP7          0x0002 /* write */
-#define MX21ADS_IO_RESET_E_UART 0x0004
-#define MX21ADS_IO_RESET_BASE   0x0008
-#define MX21ADS_IO_CSI_CTL2     0x0010
-#define MX21ADS_IO_CSI_CTL1     0x0020
-#define MX21ADS_IO_CSI_CTL0     0x0040
-#define MX21ADS_IO_UART1_EN     0x0080
-#define MX21ADS_IO_UART4_EN     0x0100
-#define MX21ADS_IO_LCDON        0x0200
-#define MX21ADS_IO_IRDA_EN      0x0400
-#define MX21ADS_IO_IRDA_FIR_SEL 0x0800
-#define MX21ADS_IO_IRDA_MD0_B   0x1000
-#define MX21ADS_IO_IRDA_MD1     0x2000
-#define MX21ADS_IO_LED4_ON      0x4000
-#define MX21ADS_IO_LED3_ON      0x8000
+#define MX21ADS_IO_SD_WP		(MX21ADS_MMGPIO_BASE + 0)
+#define MX21ADS_IO_TP6			(MX21ADS_IO_SD_WP)
+#define MX21ADS_IO_SW_SEL		(MX21ADS_MMGPIO_BASE + 1)
+#define MX21ADS_IO_TP7			(MX21ADS_IO_SW_SEL)
+#define MX21ADS_IO_RESET_E_UART		(MX21ADS_MMGPIO_BASE + 2)
+#define MX21ADS_IO_RESET_BASE		(MX21ADS_MMGPIO_BASE + 3)
+#define MX21ADS_IO_CSI_CTL2		(MX21ADS_MMGPIO_BASE + 4)
+#define MX21ADS_IO_CSI_CTL1		(MX21ADS_MMGPIO_BASE + 5)
+#define MX21ADS_IO_CSI_CTL0		(MX21ADS_MMGPIO_BASE + 6)
+#define MX21ADS_IO_UART1_EN		(MX21ADS_MMGPIO_BASE + 7)
+#define MX21ADS_IO_UART4_EN		(MX21ADS_MMGPIO_BASE + 8)
+#define MX21ADS_IO_LCDON		(MX21ADS_MMGPIO_BASE + 9)
+#define MX21ADS_IO_IRDA_EN		(MX21ADS_MMGPIO_BASE + 10)
+#define MX21ADS_IO_IRDA_FIR_SEL		(MX21ADS_MMGPIO_BASE + 11)
+#define MX21ADS_IO_IRDA_MD0_B		(MX21ADS_MMGPIO_BASE + 12)
+#define MX21ADS_IO_IRDA_MD1		(MX21ADS_MMGPIO_BASE + 13)
+#define MX21ADS_IO_LED4_ON		(MX21ADS_MMGPIO_BASE + 14)
+#define MX21ADS_IO_LED3_ON		(MX21ADS_MMGPIO_BASE + 15)
 
 static const int mx21ads_pins[] __initconst = {
 
@@ -143,11 +138,8 @@ static struct physmap_flash_data mx21ads_flash_data = {
 	.width = 4,
 };
 
-static struct resource mx21ads_flash_resource = {
-	.start = MX21_CS0_BASE_ADDR,
-	.end = MX21_CS0_BASE_ADDR + 0x02000000 - 1,
-	.flags = IORESOURCE_MEM,
-};
+static struct resource mx21ads_flash_resource =
+	DEFINE_RES_MEM(MX21_CS0_BASE_ADDR, SZ_32M);
 
 static struct platform_device mx21ads_nor_mtd_device = {
 	.name = "physmap-flash",
@@ -160,7 +152,7 @@ static struct platform_device mx21ads_nor_mtd_device = {
 };
 
 static struct resource mx21ads_cs8900_resources[] __initdata = {
-	DEFINE_RES_MEM(MX21_CS1_BASE_ADDR, MX21ADS_CS8900A_MMIO_SIZE),
+	DEFINE_RES_MEM(MX21ADS_CS8900A_REG, SZ_1K),
 	/* irq number is run-time assigned */
 	DEFINE_RES_IRQ(-1),
 };
@@ -179,24 +171,50 @@ static const struct imxuart_platform_data uart_pdata_rts __initconst = {
 static const struct imxuart_platform_data uart_pdata_norts __initconst = {
 };
 
-static int mx21ads_fb_init(struct platform_device *pdev)
-{
-	u16 tmp;
+static struct resource mx21ads_mmgpio_resource =
+	DEFINE_RES_MEM_NAMED(MX21ADS_IO_REG, SZ_2, "dat");
 
-	tmp = __raw_readw(MX21ADS_IO_REG);
-	tmp |= MX21ADS_IO_LCDON;
-	__raw_writew(tmp, MX21ADS_IO_REG);
-	return 0;
-}
+static struct bgpio_pdata mx21ads_mmgpio_pdata = {
+	.base	= MX21ADS_MMGPIO_BASE,
+	.ngpio	= 16,
+};
 
-static void mx21ads_fb_exit(struct platform_device *pdev)
-{
-	u16 tmp;
+static struct platform_device mx21ads_mmgpio = {
+	.name = "basic-mmio-gpio",
+	.id = PLATFORM_DEVID_AUTO,
+	.resource = &mx21ads_mmgpio_resource,
+	.num_resources = 1,
+	.dev = {
+		.platform_data = &mx21ads_mmgpio_pdata,
+	},
+};
 
-	tmp = __raw_readw(MX21ADS_IO_REG);
-	tmp &= ~MX21ADS_IO_LCDON;
-	__raw_writew(tmp, MX21ADS_IO_REG);
-}
+static struct regulator_consumer_supply mx21ads_lcd_regulator_consumer =
+	REGULATOR_SUPPLY("lcd", "imx-fb.0");
+
+static struct regulator_init_data mx21ads_lcd_regulator_init_data = {
+	.constraints = {
+		.valid_ops_mask	= REGULATOR_CHANGE_STATUS,
+	},
+	.consumer_supplies	= &mx21ads_lcd_regulator_consumer,
+	.num_consumer_supplies	= 1,
+};
+
+static struct fixed_voltage_config mx21ads_lcd_regulator_pdata = {
+	.supply_name	= "LCD",
+	.microvolts	= 3300000,
+	.gpio		= MX21ADS_IO_LCDON,
+	.enable_high	= 1,
+	.init_data	= &mx21ads_lcd_regulator_init_data,
+};
+
+static struct platform_device mx21ads_lcd_regulator = {
+	.name = "reg-fixed-voltage",
+	.id = PLATFORM_DEVID_AUTO,
+	.dev = {
+		.platform_data = &mx21ads_lcd_regulator_pdata,
+	},
+};
 
 /*
  * Connected is a portrait Sharp-QVGA display
@@ -229,26 +247,30 @@ static const struct imx_fb_platform_data mx21ads_fb_data __initconst = {
 	.pwmr		= 0x00a903ff,
 	.lscr1		= 0x00120300,
 	.dmacr		= 0x00020008,
-
-	.init = mx21ads_fb_init,
-	.exit = mx21ads_fb_exit,
 };
 
 static int mx21ads_sdhc_get_ro(struct device *dev)
 {
-	return (__raw_readw(MX21ADS_IO_REG) & MX21ADS_IO_SD_WP) ? 1 : 0;
+	return gpio_get_value(MX21ADS_IO_SD_WP);
 }
 
 static int mx21ads_sdhc_init(struct device *dev, irq_handler_t detect_irq,
 	void *data)
 {
-	return request_irq(gpio_to_irq(IMX_GPIO_NR(4, 25)), detect_irq,
-		IRQF_TRIGGER_FALLING, "mmc-detect", data);
+	int ret;
+
+	ret = gpio_request(MX21ADS_IO_SD_WP, "mmc-ro");
+	if (ret)
+		return ret;
+
+	return request_irq(gpio_to_irq(MX21ADS_MMC_CD), detect_irq,
+			   IRQF_TRIGGER_FALLING, "mmc-detect", data);
 }
 
 static void mx21ads_sdhc_exit(struct device *dev, void *data)
 {
-	free_irq(gpio_to_irq(IMX_GPIO_NR(4, 25)), data);
+	free_irq(gpio_to_irq(MX21ADS_MMC_CD), data);
+	gpio_free(MX21ADS_IO_SD_WP);
 }
 
 static const struct imxmmc_platform_data mx21ads_sdhc_pdata __initconst = {
@@ -264,29 +286,9 @@ mx21ads_nand_board_info __initconst = {
 	.hw_ecc = 1,
 };
 
-static struct map_desc mx21ads_io_desc[] __initdata = {
-	/*
-	 * Memory-mapped I/O on MX21ADS Base board:
-	 *   - CS8900A Ethernet controller
-	 *   - ST16C2552CJ UART
-	 *   - CPU and Base board version
-	 *   - Base board I/O register
-	 */
-	{
-		.virtual = MX21ADS_MMIO_BASE_ADDR,
-		.pfn = __phys_to_pfn(MX21_CS1_BASE_ADDR),
-		.length = MX21ADS_MMIO_SIZE,
-		.type = MT_DEVICE,
-	},
-};
-
-static void __init mx21ads_map_io(void)
-{
-	mx21_map_io();
-	iotable_init(mx21ads_io_desc, ARRAY_SIZE(mx21ads_io_desc));
-}
-
 static struct platform_device *platform_devices[] __initdata = {
+	&mx21ads_mmgpio,
+	&mx21ads_lcd_regulator,
 	&mx21ads_nor_mtd_device,
 };
 
@@ -300,12 +302,13 @@ static void __init mx21ads_board_init(void)
 	imx21_add_imx_uart0(&uart_pdata_rts);
 	imx21_add_imx_uart2(&uart_pdata_norts);
 	imx21_add_imx_uart3(&uart_pdata_rts);
-	imx21_add_imx_fb(&mx21ads_fb_data);
 	imx21_add_mxc_mmc(0, &mx21ads_sdhc_pdata);
 	imx21_add_mxc_nand(&mx21ads_nand_board_info);
 
 	platform_add_devices(platform_devices, ARRAY_SIZE(platform_devices));
 
+	imx21_add_imx_fb(&mx21ads_fb_data);
+
 	mx21ads_cs8900_resources[1].start =
 			gpio_to_irq(MX21ADS_CS8900A_IRQ_GPIO);
 	mx21ads_cs8900_resources[1].end =
@@ -321,10 +324,9 @@ static void __init mx21ads_timer_init(void)
 MACHINE_START(MX21ADS, "Freescale i.MX21ADS")
 	/* maintainer: Freescale Semiconductor, Inc. */
 	.atag_offset = 0x100,
-	.map_io = mx21ads_map_io,
+	.map_io		= mx21_map_io,
 	.init_early = imx21_init_early,
 	.init_irq = mx21_init_irq,
-	.handle_irq = imx21_handle_irq,
 	.init_time	= mx21ads_timer_init,
 	.init_machine = mx21ads_board_init,
 	.restart	= mxc_restart,
diff --git a/arch/arm/mach-imx/mach-mx25_3ds.c b/arch/arm/mach-imx/mach-mx25_3ds.c
index 13490c20305..ea1fa199c14 100644
--- a/arch/arm/mach-imx/mach-mx25_3ds.c
+++ b/arch/arm/mach-imx/mach-mx25_3ds.c
@@ -263,7 +263,6 @@ MACHINE_START(MX25_3DS, "Freescale MX25PDK (3DS)")
 	.map_io = mx25_map_io,
 	.init_early = imx25_init_early,
 	.init_irq = mx25_init_irq,
-	.handle_irq = imx25_handle_irq,
 	.init_time	= mx25pdk_timer_init,
 	.init_machine = mx25pdk_init,
 	.restart	= mxc_restart,
diff --git a/arch/arm/mach-imx/mach-mx27_3ds.c b/arch/arm/mach-imx/mach-mx27_3ds.c
index 25b3e4c9bc0..435a5428a67 100644
--- a/arch/arm/mach-imx/mach-mx27_3ds.c
+++ b/arch/arm/mach-imx/mach-mx27_3ds.c
@@ -544,7 +544,6 @@ MACHINE_START(MX27_3DS, "Freescale MX27PDK")
 	.map_io = mx27_map_io,
 	.init_early = imx27_init_early,
 	.init_irq = mx27_init_irq,
-	.handle_irq = imx27_handle_irq,
 	.init_time	= mx27pdk_timer_init,
 	.init_machine = mx27pdk_init,
 	.restart	= mxc_restart,
diff --git a/arch/arm/mach-imx/mach-mx27ads.c b/arch/arm/mach-imx/mach-mx27ads.c
index a7a4a9c6761..2f834ce8f39 100644
--- a/arch/arm/mach-imx/mach-mx27ads.c
+++ b/arch/arm/mach-imx/mach-mx27ads.c
@@ -391,7 +391,6 @@ MACHINE_START(MX27ADS, "Freescale i.MX27ADS")
 	.map_io = mx27ads_map_io,
 	.init_early = imx27_init_early,
 	.init_irq = mx27_init_irq,
-	.handle_irq = imx27_handle_irq,
 	.init_time	= mx27ads_timer_init,
 	.init_machine = mx27ads_board_init,
 	.restart	= mxc_restart,
diff --git a/arch/arm/mach-imx/mach-mx31_3ds.c b/arch/arm/mach-imx/mach-mx31_3ds.c
index 50044a21b38..4217871a965 100644
--- a/arch/arm/mach-imx/mach-mx31_3ds.c
+++ b/arch/arm/mach-imx/mach-mx31_3ds.c
@@ -775,7 +775,6 @@ MACHINE_START(MX31_3DS, "Freescale MX31PDK (3DS)")
 	.map_io = mx31_map_io,
 	.init_early = imx31_init_early,
 	.init_irq = mx31_init_irq,
-	.handle_irq = imx31_handle_irq,
 	.init_time	= mx31_3ds_timer_init,
 	.init_machine = mx31_3ds_init,
 	.reserve = mx31_3ds_reserve,
diff --git a/arch/arm/mach-imx/mach-mx31ads.c b/arch/arm/mach-imx/mach-mx31ads.c
index daf8889125c..d08c37c696f 100644
--- a/arch/arm/mach-imx/mach-mx31ads.c
+++ b/arch/arm/mach-imx/mach-mx31ads.c
@@ -582,7 +582,6 @@ MACHINE_START(MX31ADS, "Freescale MX31ADS")
 	.map_io = mx31ads_map_io,
 	.init_early = imx31_init_early,
 	.init_irq = mx31ads_init_irq,
-	.handle_irq = imx31_handle_irq,
 	.init_time	= mx31ads_timer_init,
 	.init_machine = mx31ads_init,
 	.restart	= mxc_restart,
diff --git a/arch/arm/mach-imx/mach-mx31lilly.c b/arch/arm/mach-imx/mach-mx31lilly.c
index 832b1e2f964..eee042fa276 100644
--- a/arch/arm/mach-imx/mach-mx31lilly.c
+++ b/arch/arm/mach-imx/mach-mx31lilly.c
@@ -308,7 +308,6 @@ MACHINE_START(LILLY1131, "INCO startec LILLY-1131")
 	.map_io = mx31_map_io,
 	.init_early = imx31_init_early,
 	.init_irq = mx31_init_irq,
-	.handle_irq = imx31_handle_irq,
 	.init_time	= mx31lilly_timer_init,
 	.init_machine = mx31lilly_board_init,
 	.restart	= mxc_restart,
diff --git a/arch/arm/mach-imx/mach-mx31lite.c b/arch/arm/mach-imx/mach-mx31lite.c
index bea07299b61..fa15d0b6118 100644
--- a/arch/arm/mach-imx/mach-mx31lite.c
+++ b/arch/arm/mach-imx/mach-mx31lite.c
@@ -291,7 +291,6 @@ MACHINE_START(MX31LITE, "LogicPD i.MX31 SOM")
 	.map_io = mx31lite_map_io,
 	.init_early = imx31_init_early,
 	.init_irq = mx31_init_irq,
-	.handle_irq = imx31_handle_irq,
 	.init_time	= mx31lite_timer_init,
 	.init_machine = mx31lite_init,
 	.restart	= mxc_restart,
diff --git a/arch/arm/mach-imx/mach-mx31moboard.c b/arch/arm/mach-imx/mach-mx31moboard.c
index 8f45afe785f..08730f23844 100644
--- a/arch/arm/mach-imx/mach-mx31moboard.c
+++ b/arch/arm/mach-imx/mach-mx31moboard.c
@@ -600,7 +600,6 @@ MACHINE_START(MX31MOBOARD, "EPFL Mobots mx31moboard")
 	.map_io = mx31_map_io,
 	.init_early = imx31_init_early,
 	.init_irq = mx31_init_irq,
-	.handle_irq = imx31_handle_irq,
 	.init_time	= mx31moboard_timer_init,
 	.init_machine = mx31moboard_init,
 	.restart	= mxc_restart,
diff --git a/arch/arm/mach-imx/mach-mx35_3ds.c b/arch/arm/mach-imx/mach-mx35_3ds.c
index a42f4f07051..4e8b184d773 100644
--- a/arch/arm/mach-imx/mach-mx35_3ds.c
+++ b/arch/arm/mach-imx/mach-mx35_3ds.c
@@ -615,7 +615,6 @@ MACHINE_START(MX35_3DS, "Freescale MX35PDK")
 	.map_io = mx35_map_io,
 	.init_early = imx35_init_early,
 	.init_irq = mx35_init_irq,
-	.handle_irq = imx35_handle_irq,
 	.init_time	= mx35pdk_timer_init,
 	.init_machine = mx35_3ds_init,
 	.reserve = mx35_3ds_reserve,
diff --git a/arch/arm/mach-imx/mach-mx51_babbage.c b/arch/arm/mach-imx/mach-mx51_babbage.c
deleted file mode 100644
index f3d264a636f..00000000000
--- a/arch/arm/mach-imx/mach-mx51_babbage.c
+++ /dev/null
@@ -1,428 +0,0 @@
-/*
- * Copyright 2009-2010 Freescale Semiconductor, Inc. All Rights Reserved.
- * Copyright (C) 2009-2010 Amit Kucheria <amit.kucheria@canonical.com>
- *
- * 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/init.h>
-#include <linux/platform_device.h>
-#include <linux/i2c.h>
-#include <linux/gpio.h>
-#include <linux/delay.h>
-#include <linux/io.h>
-#include <linux/input.h>
-#include <linux/spi/flash.h>
-#include <linux/spi/spi.h>
-
-#include <asm/setup.h>
-#include <asm/mach-types.h>
-#include <asm/mach/arch.h>
-#include <asm/mach/time.h>
-
-#include "common.h"
-#include "devices-imx51.h"
-#include "hardware.h"
-#include "iomux-mx51.h"
-
-#define BABBAGE_USB_HUB_RESET	IMX_GPIO_NR(1, 7)
-#define BABBAGE_USBH1_STP	IMX_GPIO_NR(1, 27)
-#define BABBAGE_USB_PHY_RESET	IMX_GPIO_NR(2, 5)
-#define BABBAGE_FEC_PHY_RESET	IMX_GPIO_NR(2, 14)
-#define BABBAGE_POWER_KEY	IMX_GPIO_NR(2, 21)
-#define BABBAGE_ECSPI1_CS0	IMX_GPIO_NR(4, 24)
-#define BABBAGE_ECSPI1_CS1	IMX_GPIO_NR(4, 25)
-#define BABBAGE_SD2_CD		IMX_GPIO_NR(1, 6)
-#define BABBAGE_SD2_WP		IMX_GPIO_NR(1, 5)
-
-/* USB_CTRL_1 */
-#define MX51_USB_CTRL_1_OFFSET			0x10
-#define MX51_USB_CTRL_UH1_EXT_CLK_EN		(1 << 25)
-
-#define	MX51_USB_PLLDIV_12_MHZ		0x00
-#define	MX51_USB_PLL_DIV_19_2_MHZ	0x01
-#define	MX51_USB_PLL_DIV_24_MHZ	0x02
-
-static struct gpio_keys_button babbage_buttons[] = {
-	{
-		.gpio		= BABBAGE_POWER_KEY,
-		.code		= BTN_0,
-		.desc		= "PWR",
-		.active_low	= 1,
-		.wakeup		= 1,
-	},
-};
-
-static const struct gpio_keys_platform_data imx_button_data __initconst = {
-	.buttons	= babbage_buttons,
-	.nbuttons	= ARRAY_SIZE(babbage_buttons),
-};
-
-static iomux_v3_cfg_t mx51babbage_pads[] = {
-	/* UART1 */
-	MX51_PAD_UART1_RXD__UART1_RXD,
-	MX51_PAD_UART1_TXD__UART1_TXD,
-	MX51_PAD_UART1_RTS__UART1_RTS,
-	MX51_PAD_UART1_CTS__UART1_CTS,
-
-	/* UART2 */
-	MX51_PAD_UART2_RXD__UART2_RXD,
-	MX51_PAD_UART2_TXD__UART2_TXD,
-
-	/* UART3 */
-	MX51_PAD_EIM_D25__UART3_RXD,
-	MX51_PAD_EIM_D26__UART3_TXD,
-	MX51_PAD_EIM_D27__UART3_RTS,
-	MX51_PAD_EIM_D24__UART3_CTS,
-
-	/* I2C1 */
-	MX51_PAD_EIM_D16__I2C1_SDA,
-	MX51_PAD_EIM_D19__I2C1_SCL,
-
-	/* I2C2 */
-	MX51_PAD_KEY_COL4__I2C2_SCL,
-	MX51_PAD_KEY_COL5__I2C2_SDA,
-
-	/* HSI2C */
-	MX51_PAD_I2C1_CLK__I2C1_CLK,
-	MX51_PAD_I2C1_DAT__I2C1_DAT,
-
-	/* USB HOST1 */
-	MX51_PAD_USBH1_CLK__USBH1_CLK,
-	MX51_PAD_USBH1_DIR__USBH1_DIR,
-	MX51_PAD_USBH1_NXT__USBH1_NXT,
-	MX51_PAD_USBH1_DATA0__USBH1_DATA0,
-	MX51_PAD_USBH1_DATA1__USBH1_DATA1,
-	MX51_PAD_USBH1_DATA2__USBH1_DATA2,
-	MX51_PAD_USBH1_DATA3__USBH1_DATA3,
-	MX51_PAD_USBH1_DATA4__USBH1_DATA4,
-	MX51_PAD_USBH1_DATA5__USBH1_DATA5,
-	MX51_PAD_USBH1_DATA6__USBH1_DATA6,
-	MX51_PAD_USBH1_DATA7__USBH1_DATA7,
-
-	/* USB HUB reset line*/
-	MX51_PAD_GPIO1_7__GPIO1_7,
-
-	/* USB PHY reset line */
-	MX51_PAD_EIM_D21__GPIO2_5,
-
-	/* FEC */
-	MX51_PAD_EIM_EB2__FEC_MDIO,
-	MX51_PAD_EIM_EB3__FEC_RDATA1,
-	MX51_PAD_EIM_CS2__FEC_RDATA2,
-	MX51_PAD_EIM_CS3__FEC_RDATA3,
-	MX51_PAD_EIM_CS4__FEC_RX_ER,
-	MX51_PAD_EIM_CS5__FEC_CRS,
-	MX51_PAD_NANDF_RB2__FEC_COL,
-	MX51_PAD_NANDF_RB3__FEC_RX_CLK,
-	MX51_PAD_NANDF_D9__FEC_RDATA0,
-	MX51_PAD_NANDF_D8__FEC_TDATA0,
-	MX51_PAD_NANDF_CS2__FEC_TX_ER,
-	MX51_PAD_NANDF_CS3__FEC_MDC,
-	MX51_PAD_NANDF_CS4__FEC_TDATA1,
-	MX51_PAD_NANDF_CS5__FEC_TDATA2,
-	MX51_PAD_NANDF_CS6__FEC_TDATA3,
-	MX51_PAD_NANDF_CS7__FEC_TX_EN,
-	MX51_PAD_NANDF_RDY_INT__FEC_TX_CLK,
-
-	/* FEC PHY reset line */
-	MX51_PAD_EIM_A20__GPIO2_14,
-
-	/* SD 1 */
-	MX51_PAD_SD1_CMD__SD1_CMD,
-	MX51_PAD_SD1_CLK__SD1_CLK,
-	MX51_PAD_SD1_DATA0__SD1_DATA0,
-	MX51_PAD_SD1_DATA1__SD1_DATA1,
-	MX51_PAD_SD1_DATA2__SD1_DATA2,
-	MX51_PAD_SD1_DATA3__SD1_DATA3,
-	/* CD/WP from controller */
-	MX51_PAD_GPIO1_0__SD1_CD,
-	MX51_PAD_GPIO1_1__SD1_WP,
-
-	/* SD 2 */
-	MX51_PAD_SD2_CMD__SD2_CMD,
-	MX51_PAD_SD2_CLK__SD2_CLK,
-	MX51_PAD_SD2_DATA0__SD2_DATA0,
-	MX51_PAD_SD2_DATA1__SD2_DATA1,
-	MX51_PAD_SD2_DATA2__SD2_DATA2,
-	MX51_PAD_SD2_DATA3__SD2_DATA3,
-	/* CD/WP gpio */
-	MX51_PAD_GPIO1_6__GPIO1_6,
-	MX51_PAD_GPIO1_5__GPIO1_5,
-
-	/* eCSPI1 */
-	MX51_PAD_CSPI1_MISO__ECSPI1_MISO,
-	MX51_PAD_CSPI1_MOSI__ECSPI1_MOSI,
-	MX51_PAD_CSPI1_SCLK__ECSPI1_SCLK,
-	MX51_PAD_CSPI1_SS0__GPIO4_24,
-	MX51_PAD_CSPI1_SS1__GPIO4_25,
-
-	/* Audio */
-	MX51_PAD_AUD3_BB_TXD__AUD3_TXD,
-	MX51_PAD_AUD3_BB_RXD__AUD3_RXD,
-	MX51_PAD_AUD3_BB_CK__AUD3_TXC,
-	MX51_PAD_AUD3_BB_FS__AUD3_TXFS,
-};
-
-/* Serial ports */
-static const struct imxuart_platform_data uart_pdata __initconst = {
-	.flags = IMXUART_HAVE_RTSCTS,
-};
-
-static const struct imxi2c_platform_data babbage_i2c_data __initconst = {
-	.bitrate = 100000,
-};
-
-static const struct imxi2c_platform_data babbage_hsi2c_data __initconst = {
-	.bitrate = 400000,
-};
-
-static struct gpio mx51_babbage_usbh1_gpios[] = {
-	{ BABBAGE_USBH1_STP, GPIOF_OUT_INIT_LOW, "usbh1_stp" },
-	{ BABBAGE_USB_PHY_RESET, GPIOF_OUT_INIT_LOW, "usbh1_phy_reset" },
-};
-
-static int gpio_usbh1_active(void)
-{
-	iomux_v3_cfg_t usbh1stp_gpio = MX51_PAD_USBH1_STP__GPIO1_27;
-	int ret;
-
-	/* Set USBH1_STP to GPIO and toggle it */
-	mxc_iomux_v3_setup_pad(usbh1stp_gpio);
-	ret = gpio_request_array(mx51_babbage_usbh1_gpios,
-					ARRAY_SIZE(mx51_babbage_usbh1_gpios));
-
-	if (ret) {
-		pr_debug("failed to get USBH1 pins: %d\n", ret);
-		return ret;
-	}
-
-	msleep(100);
-	gpio_set_value(BABBAGE_USBH1_STP, 1);
-	gpio_set_value(BABBAGE_USB_PHY_RESET, 1);
-	gpio_free_array(mx51_babbage_usbh1_gpios,
-					ARRAY_SIZE(mx51_babbage_usbh1_gpios));
-	return 0;
-}
-
-static inline void babbage_usbhub_reset(void)
-{
-	int ret;
-
-	/* Reset USB hub */
-	ret = gpio_request_one(BABBAGE_USB_HUB_RESET,
-					GPIOF_OUT_INIT_LOW, "GPIO1_7");
-	if (ret) {
-		printk(KERN_ERR"failed to get GPIO_USB_HUB_RESET: %d\n", ret);
-		return;
-	}
-
-	msleep(2);
-	/* Deassert reset */
-	gpio_set_value(BABBAGE_USB_HUB_RESET, 1);
-}
-
-static inline void babbage_fec_reset(void)
-{
-	int ret;
-
-	/* reset FEC PHY */
-	ret = gpio_request_one(BABBAGE_FEC_PHY_RESET,
-					GPIOF_OUT_INIT_LOW, "fec-phy-reset");
-	if (ret) {
-		printk(KERN_ERR"failed to get GPIO_FEC_PHY_RESET: %d\n", ret);
-		return;
-	}
-	msleep(1);
-	gpio_set_value(BABBAGE_FEC_PHY_RESET, 1);
-}
-
-/* This function is board specific as the bit mask for the plldiv will also
-be different for other Freescale SoCs, thus a common bitmask is not
-possible and cannot get place in /plat-mxc/ehci.c.*/
-static int initialize_otg_port(struct platform_device *pdev)
-{
-	u32 v;
-	void __iomem *usb_base;
-	void __iomem *usbother_base;
-
-	usb_base = ioremap(MX51_USB_OTG_BASE_ADDR, SZ_4K);
-	if (!usb_base)
-		return -ENOMEM;
-	usbother_base = usb_base + MX5_USBOTHER_REGS_OFFSET;
-
-	/* Set the PHY clock to 19.2MHz */
-	v = __raw_readl(usbother_base + MXC_USB_PHY_CTR_FUNC2_OFFSET);
-	v &= ~MX5_USB_UTMI_PHYCTRL1_PLLDIV_MASK;
-	v |= MX51_USB_PLL_DIV_19_2_MHZ;
-	__raw_writel(v, usbother_base + MXC_USB_PHY_CTR_FUNC2_OFFSET);
-	iounmap(usb_base);
-
-	mdelay(10);
-
-	return mx51_initialize_usb_hw(0, MXC_EHCI_INTERNAL_PHY);
-}
-
-static int initialize_usbh1_port(struct platform_device *pdev)
-{
-	u32 v;
-	void __iomem *usb_base;
-	void __iomem *usbother_base;
-
-	usb_base = ioremap(MX51_USB_OTG_BASE_ADDR, SZ_4K);
-	if (!usb_base)
-		return -ENOMEM;
-	usbother_base = usb_base + MX5_USBOTHER_REGS_OFFSET;
-
-	/* The clock for the USBH1 ULPI port will come externally from the PHY. */
-	v = __raw_readl(usbother_base + MX51_USB_CTRL_1_OFFSET);
-	__raw_writel(v | MX51_USB_CTRL_UH1_EXT_CLK_EN, usbother_base + MX51_USB_CTRL_1_OFFSET);
-	iounmap(usb_base);
-
-	mdelay(10);
-
-	return mx51_initialize_usb_hw(1, MXC_EHCI_POWER_PINS_ENABLED |
-			MXC_EHCI_ITC_NO_THRESHOLD);
-}
-
-static const struct mxc_usbh_platform_data dr_utmi_config __initconst = {
-	.init		= initialize_otg_port,
-	.portsc	= MXC_EHCI_UTMI_16BIT,
-};
-
-static const struct fsl_usb2_platform_data usb_pdata __initconst = {
-	.operating_mode	= FSL_USB2_DR_DEVICE,
-	.phy_mode	= FSL_USB2_PHY_UTMI_WIDE,
-};
-
-static const struct mxc_usbh_platform_data usbh1_config __initconst = {
-	.init		= initialize_usbh1_port,
-	.portsc	= MXC_EHCI_MODE_ULPI,
-};
-
-static bool otg_mode_host __initdata;
-
-static int __init babbage_otg_mode(char *options)
-{
-	if (!strcmp(options, "host"))
-		otg_mode_host = true;
-	else if (!strcmp(options, "device"))
-		otg_mode_host = false;
-	else
-		pr_info("otg_mode neither \"host\" nor \"device\". "
-			"Defaulting to device\n");
-	return 1;
-}
-__setup("otg_mode=", babbage_otg_mode);
-
-static struct spi_board_info mx51_babbage_spi_board_info[] __initdata = {
-	{
-		.modalias = "mtd_dataflash",
-		.max_speed_hz = 25000000,
-		.bus_num = 0,
-		.chip_select = 1,
-		.mode = SPI_MODE_0,
-		.platform_data = NULL,
-	},
-};
-
-static int mx51_babbage_spi_cs[] = {
-	BABBAGE_ECSPI1_CS0,
-	BABBAGE_ECSPI1_CS1,
-};
-
-static const struct spi_imx_master mx51_babbage_spi_pdata __initconst = {
-	.chipselect     = mx51_babbage_spi_cs,
-	.num_chipselect = ARRAY_SIZE(mx51_babbage_spi_cs),
-};
-
-static const struct esdhc_platform_data mx51_babbage_sd1_data __initconst = {
-	.cd_type = ESDHC_CD_CONTROLLER,
-	.wp_type = ESDHC_WP_CONTROLLER,
-};
-
-static const struct esdhc_platform_data mx51_babbage_sd2_data __initconst = {
-	.cd_gpio = BABBAGE_SD2_CD,
-	.wp_gpio = BABBAGE_SD2_WP,
-	.cd_type = ESDHC_CD_GPIO,
-	.wp_type = ESDHC_WP_GPIO,
-};
-
-void __init imx51_babbage_common_init(void)
-{
-	mxc_iomux_v3_setup_multiple_pads(mx51babbage_pads,
-					 ARRAY_SIZE(mx51babbage_pads));
-}
-
-/*
- * Board specific initialization.
- */
-static void __init mx51_babbage_init(void)
-{
-	iomux_v3_cfg_t usbh1stp = MX51_PAD_USBH1_STP__USBH1_STP;
-	iomux_v3_cfg_t power_key = NEW_PAD_CTRL(MX51_PAD_EIM_A27__GPIO2_21,
-		PAD_CTL_SRE_FAST | PAD_CTL_DSE_HIGH);
-
-	imx51_soc_init();
-
-	imx51_babbage_common_init();
-
-	imx51_add_imx_uart(0, &uart_pdata);
-	imx51_add_imx_uart(1, NULL);
-	imx51_add_imx_uart(2, &uart_pdata);
-
-	babbage_fec_reset();
-	imx51_add_fec(NULL);
-
-	/* Set the PAD settings for the pwr key. */
-	mxc_iomux_v3_setup_pad(power_key);
-	imx_add_gpio_keys(&imx_button_data);
-
-	imx51_add_imx_i2c(0, &babbage_i2c_data);
-	imx51_add_imx_i2c(1, &babbage_i2c_data);
-	imx51_add_hsi2c(&babbage_hsi2c_data);
-
-	if (otg_mode_host)
-		imx51_add_mxc_ehci_otg(&dr_utmi_config);
-	else {
-		initialize_otg_port(NULL);
-		imx51_add_fsl_usb2_udc(&usb_pdata);
-	}
-
-	gpio_usbh1_active();
-	imx51_add_mxc_ehci_hs(1, &usbh1_config);
-	/* setback USBH1_STP to be function */
-	mxc_iomux_v3_setup_pad(usbh1stp);
-	babbage_usbhub_reset();
-
-	imx51_add_sdhci_esdhc_imx(0, &mx51_babbage_sd1_data);
-	imx51_add_sdhci_esdhc_imx(1, &mx51_babbage_sd2_data);
-
-	spi_register_board_info(mx51_babbage_spi_board_info,
-		ARRAY_SIZE(mx51_babbage_spi_board_info));
-	imx51_add_ecspi(0, &mx51_babbage_spi_pdata);
-	imx51_add_imx2_wdt(0);
-}
-
-static void __init mx51_babbage_timer_init(void)
-{
-	mx51_clocks_init(32768, 24000000, 22579200, 0);
-}
-
-MACHINE_START(MX51_BABBAGE, "Freescale MX51 Babbage Board")
-	/* Maintainer: Amit Kucheria <amit.kucheria@canonical.com> */
-	.atag_offset = 0x100,
-	.map_io = mx51_map_io,
-	.init_early = imx51_init_early,
-	.init_irq = mx51_init_irq,
-	.handle_irq = imx51_handle_irq,
-	.init_time	= mx51_babbage_timer_init,
-	.init_machine = mx51_babbage_init,
-	.init_late	= imx51_init_late,
-	.restart	= mxc_restart,
-MACHINE_END
diff --git a/arch/arm/mach-imx/mach-mxt_td60.c b/arch/arm/mach-imx/mach-mxt_td60.c
index c91894003da..0b5d1ca31b9 100644
--- a/arch/arm/mach-imx/mach-mxt_td60.c
+++ b/arch/arm/mach-imx/mach-mxt_td60.c
@@ -267,7 +267,6 @@ MACHINE_START(MXT_TD60, "Maxtrack i-MXT TD60")
 	.map_io = mx27_map_io,
 	.init_early = imx27_init_early,
 	.init_irq = mx27_init_irq,
-	.handle_irq = imx27_handle_irq,
 	.init_time	= mxt_td60_timer_init,
 	.init_machine = mxt_td60_board_init,
 	.restart	= mxc_restart,
diff --git a/arch/arm/mach-imx/mach-pca100.c b/arch/arm/mach-imx/mach-pca100.c
index bf3ac51d5ac..12212378c67 100644
--- a/arch/arm/mach-imx/mach-pca100.c
+++ b/arch/arm/mach-imx/mach-pca100.c
@@ -245,8 +245,7 @@ static int pca100_sdhc2_init(struct device *dev, irq_handler_t detect_irq,
 	int ret;
 
 	ret = request_irq(gpio_to_irq(IMX_GPIO_NR(3, 29)), detect_irq,
-			  IRQF_DISABLED | IRQF_TRIGGER_FALLING,
-			  "imx-mmc-detect", data);
+			  IRQF_TRIGGER_FALLING, "imx-mmc-detect", data);
 	if (ret)
 		printk(KERN_ERR
 			"pca100: Failed to request irq for sd/mmc detection\n");
@@ -421,7 +420,6 @@ MACHINE_START(PCA100, "phyCARD-i.MX27")
 	.map_io = mx27_map_io,
 	.init_early = imx27_init_early,
 	.init_irq = mx27_init_irq,
-	.handle_irq = imx27_handle_irq,
 	.init_machine = pca100_init,
 	.init_time	= pca100_timer_init,
 	.restart	= mxc_restart,
diff --git a/arch/arm/mach-imx/mach-pcm037.c b/arch/arm/mach-imx/mach-pcm037.c
index 639a3dfb009..81b8affb944 100644
--- a/arch/arm/mach-imx/mach-pcm037.c
+++ b/arch/arm/mach-imx/mach-pcm037.c
@@ -703,7 +703,6 @@ MACHINE_START(PCM037, "Phytec Phycore pcm037")
 	.map_io = mx31_map_io,
 	.init_early = imx31_init_early,
 	.init_irq = mx31_init_irq,
-	.handle_irq = imx31_handle_irq,
 	.init_time	= pcm037_timer_init,
 	.init_machine = pcm037_init,
 	.init_late = pcm037_init_late,
diff --git a/arch/arm/mach-imx/mach-pcm038.c b/arch/arm/mach-imx/mach-pcm038.c
index 592ddbe031a..6c56fb5553c 100644
--- a/arch/arm/mach-imx/mach-pcm038.c
+++ b/arch/arm/mach-imx/mach-pcm038.c
@@ -351,7 +351,6 @@ MACHINE_START(PCM038, "phyCORE-i.MX27")
 	.map_io = mx27_map_io,
 	.init_early = imx27_init_early,
 	.init_irq = mx27_init_irq,
-	.handle_irq = imx27_handle_irq,
 	.init_time	= pcm038_timer_init,
 	.init_machine = pcm038_init,
 	.restart	= mxc_restart,
diff --git a/arch/arm/mach-imx/mach-pcm043.c b/arch/arm/mach-imx/mach-pcm043.c
index ac504b67326..c62b5d26134 100644
--- a/arch/arm/mach-imx/mach-pcm043.c
+++ b/arch/arm/mach-imx/mach-pcm043.c
@@ -400,7 +400,6 @@ MACHINE_START(PCM043, "Phytec Phycore pcm043")
 	.map_io = mx35_map_io,
 	.init_early = imx35_init_early,
 	.init_irq = mx35_init_irq,
-	.handle_irq = imx35_handle_irq,
 	.init_time = pcm043_timer_init,
 	.init_machine = pcm043_init,
 	.restart	= mxc_restart,
diff --git a/arch/arm/mach-imx/mach-qong.c b/arch/arm/mach-imx/mach-qong.c
index 22af27ed457..a213e7b9cb1 100644
--- a/arch/arm/mach-imx/mach-qong.c
+++ b/arch/arm/mach-imx/mach-qong.c
@@ -266,7 +266,6 @@ MACHINE_START(QONG, "Dave/DENX QongEVB-LITE")
 	.map_io = mx31_map_io,
 	.init_early = imx31_init_early,
 	.init_irq = mx31_init_irq,
-	.handle_irq = imx31_handle_irq,
 	.init_time	= qong_timer_init,
 	.init_machine = qong_init,
 	.restart	= mxc_restart,
diff --git a/arch/arm/mach-imx/mach-scb9328.c b/arch/arm/mach-imx/mach-scb9328.c
index b0fa10dd79f..1f6bc3f7ae1 100644
--- a/arch/arm/mach-imx/mach-scb9328.c
+++ b/arch/arm/mach-imx/mach-scb9328.c
@@ -137,7 +137,6 @@ MACHINE_START(SCB9328, "Synertronixx scb9328")
 	.map_io = mx1_map_io,
 	.init_early = imx1_init_early,
 	.init_irq = mx1_init_irq,
-	.handle_irq = imx1_handle_irq,
 	.init_time	= scb9328_timer_init,
 	.init_machine = scb9328_init,
 	.restart	= mxc_restart,
diff --git a/arch/arm/mach-imx/mach-vpr200.c b/arch/arm/mach-imx/mach-vpr200.c
index 8825d1217d1..872b3c6ba40 100644
--- a/arch/arm/mach-imx/mach-vpr200.c
+++ b/arch/arm/mach-imx/mach-vpr200.c
@@ -310,7 +310,6 @@ MACHINE_START(VPR200, "VPR200")
 	.map_io = mx35_map_io,
 	.init_early = imx35_init_early,
 	.init_irq = mx35_init_irq,
-	.handle_irq = imx35_handle_irq,
 	.init_time = vpr200_timer_init,
 	.init_machine = vpr200_board_init,
 	.restart	= mxc_restart,
diff --git a/arch/arm/mach-imx/mxc.h b/arch/arm/mach-imx/mxc.h
index b08ab3ad4a6..75d6a37e1ae 100644
--- a/arch/arm/mach-imx/mxc.h
+++ b/arch/arm/mach-imx/mxc.h
@@ -36,6 +36,7 @@
 #define MXC_CPU_MX53		53
 #define MXC_CPU_IMX6SL		0x60
 #define MXC_CPU_IMX6DL		0x61
+#define MXC_CPU_IMX6SX		0x62
 #define MXC_CPU_IMX6Q		0x63
 
 #define IMX_CHIP_REVISION_1_0		0x10
@@ -163,6 +164,11 @@ static inline bool cpu_is_imx6dl(void)
 	return __mxc_cpu_type == MXC_CPU_IMX6DL;
 }
 
+static inline bool cpu_is_imx6sx(void)
+{
+	return __mxc_cpu_type == MXC_CPU_IMX6SX;
+}
+
 static inline bool cpu_is_imx6q(void)
 {
 	return __mxc_cpu_type == MXC_CPU_IMX6Q;
diff --git a/arch/arm/mach-imx/time.c b/arch/arm/mach-imx/time.c
index 65222ea0df6..bed081e5826 100644
--- a/arch/arm/mach-imx/time.c
+++ b/arch/arm/mach-imx/time.c
@@ -28,6 +28,9 @@
 #include <linux/delay.h>
 #include <linux/err.h>
 #include <linux/sched_clock.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
 
 #include <asm/mach/time.h>
 
@@ -328,3 +331,15 @@ void __init mxc_timer_init(void __iomem *base, int irq)
 	/* Make irqs happen */
 	setup_irq(irq, &mxc_timer_irq);
 }
+
+void __init mxc_timer_init_dt(struct device_node *np)
+{
+	void __iomem *base;
+	int irq;
+
+	base = of_iomap(np, 0);
+	WARN_ON(!base);
+	irq = irq_of_parse_and_map(np, 0);
+
+	mxc_timer_init(base, irq);
+}
diff --git a/arch/arm/mach-imx/tzic.c b/arch/arm/mach-imx/tzic.c
index 8183178d5aa..7828af4b202 100644
--- a/arch/arm/mach-imx/tzic.c
+++ b/arch/arm/mach-imx/tzic.c
@@ -125,7 +125,7 @@ static __init void tzic_init_gc(int idx, unsigned int irq_start)
 	irq_setup_generic_chip(gc, IRQ_MSK(32), 0, IRQ_NOREQUEST, 0);
 }
 
-asmlinkage void __exception_irq_entry tzic_handle_irq(struct pt_regs *regs)
+static void __exception_irq_entry tzic_handle_irq(struct pt_regs *regs)
 {
 	u32 stat;
 	int i, irqofs, handled;
@@ -189,6 +189,8 @@ void __init tzic_init_irq(void __iomem *irqbase)
 	for (i = 0; i < 4; i++, irq_base += 32)
 		tzic_init_gc(i, irq_base);
 
+	set_handle_irq(tzic_handle_irq);
+
 #ifdef CONFIG_FIQ
 	/* Initialize FIQ */
 	init_FIQ(FIQ_START);
diff --git a/arch/arm/mach-kirkwood/board-dt.c b/arch/arm/mach-kirkwood/board-dt.c
index 2801da49e2a..ff18ff20f71 100644
--- a/arch/arm/mach-kirkwood/board-dt.c
+++ b/arch/arm/mach-kirkwood/board-dt.c
@@ -195,7 +195,7 @@ static void __init kirkwood_dt_init(void)
 {
 	kirkwood_disable_mbus_error_propagation();
 
-	BUG_ON(mvebu_mbus_dt_init());
+	BUG_ON(mvebu_mbus_dt_init(false));
 
 #ifdef CONFIG_CACHE_FEROCEON_L2
 	feroceon_of_init();
diff --git a/arch/arm/mach-kirkwood/irq.c b/arch/arm/mach-kirkwood/irq.c
index 2a97a2e4163..2c47a8ad0e2 100644
--- a/arch/arm/mach-kirkwood/irq.c
+++ b/arch/arm/mach-kirkwood/irq.c
@@ -7,6 +7,7 @@
  * License version 2.  This program is licensed "as is" without any
  * warranty of any kind, whether express or implied.
  */
+#include <asm/exception.h>
 #include <linux/gpio.h>
 #include <linux/kernel.h>
 #include <linux/irq.h>
@@ -30,11 +31,47 @@ static int __initdata gpio1_irqs[4] = {
 	0,
 };
 
+#ifdef CONFIG_MULTI_IRQ_HANDLER
+/*
+ * Compiling with both non-DT and DT support enabled, will
+ * break asm irq handler used by non-DT boards. Therefore,
+ * we provide a C-style irq handler even for non-DT boards,
+ * if MULTI_IRQ_HANDLER is set.
+ */
+
+static void __iomem *kirkwood_irq_base = IRQ_VIRT_BASE;
+
+asmlinkage void
+__exception_irq_entry kirkwood_legacy_handle_irq(struct pt_regs *regs)
+{
+	u32 stat;
+
+	stat = readl_relaxed(kirkwood_irq_base + IRQ_CAUSE_LOW_OFF);
+	stat &= readl_relaxed(kirkwood_irq_base + IRQ_MASK_LOW_OFF);
+	if (stat) {
+		unsigned int hwirq = __fls(stat);
+		handle_IRQ(hwirq, regs);
+		return;
+	}
+	stat = readl_relaxed(kirkwood_irq_base + IRQ_CAUSE_HIGH_OFF);
+	stat &= readl_relaxed(kirkwood_irq_base + IRQ_MASK_HIGH_OFF);
+	if (stat) {
+		unsigned int hwirq = 32 + __fls(stat);
+		handle_IRQ(hwirq, regs);
+		return;
+	}
+}
+#endif
+
 void __init kirkwood_init_irq(void)
 {
 	orion_irq_init(0, IRQ_VIRT_BASE + IRQ_MASK_LOW_OFF);
 	orion_irq_init(32, IRQ_VIRT_BASE + IRQ_MASK_HIGH_OFF);
 
+#ifdef CONFIG_MULTI_IRQ_HANDLER
+	set_handle_irq(kirkwood_legacy_handle_irq);
+#endif
+
 	/*
 	 * Initialize gpiolib for GPIOs 0-49.
 	 */
diff --git a/arch/arm/mach-lpc32xx/phy3250.c b/arch/arm/mach-lpc32xx/phy3250.c
index 34932e0e31f..7858d5b6f6c 100644
--- a/arch/arm/mach-lpc32xx/phy3250.c
+++ b/arch/arm/mach-lpc32xx/phy3250.c
@@ -202,9 +202,6 @@ static struct mmci_platform_data lpc32xx_mmci_data = {
 	.ocr_mask	= MMC_VDD_30_31 | MMC_VDD_31_32 |
 			  MMC_VDD_32_33 | MMC_VDD_33_34,
 	.ios_handler	= mmc_handle_ios,
-	.dma_filter	= NULL,
-	/* No DMA for now since AMBA PL080 dmaengine driver only does scatter
-	 * gather, and the MMCI driver doesn't do it this way */
 };
 
 static struct lpc32xx_slc_platform_data lpc32xx_slc_data = {
diff --git a/arch/arm/mach-msm/Kconfig b/arch/arm/mach-msm/Kconfig
index a7f959e58c3..9b26976fb08 100644
--- a/arch/arm/mach-msm/Kconfig
+++ b/arch/arm/mach-msm/Kconfig
@@ -42,9 +42,6 @@ config ARCH_QSD8X50
 
 endchoice
 
-config MSM_HAS_DEBUG_UART_HS
-	bool
-
 config MSM_SOC_REV_A
 	bool
 
diff --git a/arch/arm/mach-msm/board-msm7x30.c b/arch/arm/mach-msm/board-msm7x30.c
index 46de789ad3a..0c4c200e122 100644
--- a/arch/arm/mach-msm/board-msm7x30.c
+++ b/arch/arm/mach-msm/board-msm7x30.c
@@ -95,7 +95,7 @@ static int hsusb_phy_clk_reset(struct clk *phy_clk)
 
 static struct msm_otg_platform_data msm_otg_pdata = {
 	.phy_init_seq		= hsusb_phy_init_seq,
-	.mode                   = USB_PERIPHERAL,
+	.mode                   = USB_DR_MODE_PERIPHERAL,
 	.otg_control		= OTG_PHY_CONTROL,
 	.link_clk_reset		= hsusb_link_clk_reset,
 	.phy_clk_reset		= hsusb_phy_clk_reset,
diff --git a/arch/arm/mach-msm/board-qsd8x50.c b/arch/arm/mach-msm/board-qsd8x50.c
index 9169ec324a4..4c748616ef4 100644
--- a/arch/arm/mach-msm/board-qsd8x50.c
+++ b/arch/arm/mach-msm/board-qsd8x50.c
@@ -116,7 +116,7 @@ static int hsusb_phy_clk_reset(struct clk *phy_clk)
 
 static struct msm_otg_platform_data msm_otg_pdata = {
 	.phy_init_seq		= hsusb_phy_init_seq,
-	.mode                   = USB_PERIPHERAL,
+	.mode                   = USB_DR_MODE_PERIPHERAL,
 	.otg_control		= OTG_PHY_CONTROL,
 	.link_clk_reset		= hsusb_link_clk_reset,
 	.phy_clk_reset		= hsusb_phy_clk_reset,
diff --git a/arch/arm/mach-msm/board-trout-gpio.c b/arch/arm/mach-msm/board-trout-gpio.c
index 87e1d01edec..2c25050209c 100644
--- a/arch/arm/mach-msm/board-trout-gpio.c
+++ b/arch/arm/mach-msm/board-trout-gpio.c
@@ -89,7 +89,7 @@ static int trout_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 			.base		  = base_gpio,			\
 			.ngpio		  = 8,				\
 		},							\
-		.reg = (void *) reg_num + TROUT_CPLD_BASE,		\
+		.reg = reg_num + TROUT_CPLD_BASE,			\
 		.shadow = shadow_val,					\
 	}
 
diff --git a/arch/arm/mach-msm/board-trout.c b/arch/arm/mach-msm/board-trout.c
index 015d544aa01..5edfbd904d0 100644
--- a/arch/arm/mach-msm/board-trout.c
+++ b/arch/arm/mach-msm/board-trout.c
@@ -78,7 +78,7 @@ static void __init trout_init(void)
 
 static struct map_desc trout_io_desc[] __initdata = {
 	{
-		.virtual = TROUT_CPLD_BASE,
+		.virtual = (unsigned long)TROUT_CPLD_BASE,
 		.pfn     = __phys_to_pfn(TROUT_CPLD_START),
 		.length  = TROUT_CPLD_SIZE,
 		.type    = MT_DEVICE_NONSHARED
diff --git a/arch/arm/mach-msm/board-trout.h b/arch/arm/mach-msm/board-trout.h
index b2379ede43b..adb757abbb9 100644
--- a/arch/arm/mach-msm/board-trout.h
+++ b/arch/arm/mach-msm/board-trout.h
@@ -58,7 +58,7 @@
 #define TROUT_4_TP_LS_EN    19
 #define TROUT_5_TP_LS_EN    1
 
-#define TROUT_CPLD_BASE   0xE8100000
+#define TROUT_CPLD_BASE   IOMEM(0xE8100000)
 #define TROUT_CPLD_START  0x98000000
 #define TROUT_CPLD_SIZE   SZ_4K
 
diff --git a/arch/arm/mach-mvebu/Kconfig b/arch/arm/mach-mvebu/Kconfig
index 3f73eecbcfb..6090b9eb00c 100644
--- a/arch/arm/mach-mvebu/Kconfig
+++ b/arch/arm/mach-mvebu/Kconfig
@@ -3,15 +3,13 @@ config ARCH_MVEBU
 	select ARCH_SUPPORTS_BIG_ENDIAN
 	select CLKSRC_MMIO
 	select GENERIC_IRQ_CHIP
-	select IRQ_DOMAIN
 	select PINCTRL
 	select PLAT_ORION
+	select SOC_BUS
 	select MVEBU_MBUS
 	select ZONE_DMA if ARM_LPAE
 	select ARCH_REQUIRE_GPIOLIB
-	select MIGHT_HAVE_PCI
 	select PCI_QUIRKS if PCI
-	select OF_ADDRESS_PCI
 
 if ARCH_MVEBU
 
@@ -38,7 +36,9 @@ config MACH_ARMADA_375
 	select ARM_ERRATA_753970
 	select ARM_GIC
 	select ARMADA_375_CLK
-	select CPU_V7
+	select HAVE_ARM_SCU
+	select HAVE_ARM_TWD if SMP
+	select HAVE_SMP
 	select MACH_MVEBU_V7
 	select PINCTRL_ARMADA_375
 	help
@@ -51,7 +51,9 @@ config MACH_ARMADA_38X
 	select ARM_ERRATA_753970
 	select ARM_GIC
 	select ARMADA_38X_CLK
-	select CPU_V7
+	select HAVE_ARM_SCU
+	select HAVE_ARM_TWD if SMP
+	select HAVE_SMP
 	select MACH_MVEBU_V7
 	select PINCTRL_ARMADA_38X
 	help
@@ -86,24 +88,15 @@ config MACH_KIRKWOOD
 	select ARCH_REQUIRE_GPIOLIB
 	select CPU_FEROCEON
 	select KIRKWOOD_CLK
-	select OF_IRQ
 	select ORION_IRQCHIP
 	select ORION_TIMER
 	select PCI
 	select PCI_QUIRKS
 	select PINCTRL_KIRKWOOD
-	select USE_OF
 	help
 	  Say 'Y' here if you want your kernel to support boards based
 	  on the Marvell Kirkwood device tree.
 
-config MACH_T5325
-	bool "HP T5325 thin client"
-	depends on MACH_KIRKWOOD
-	help
-	  Say 'Y' here if you want your kernel to support the
-	  HP T5325 Thin client
-
 endmenu
 
 endif
diff --git a/arch/arm/mach-mvebu/Makefile b/arch/arm/mach-mvebu/Makefile
index a63e43b6b45..2ecb828e4a8 100644
--- a/arch/arm/mach-mvebu/Makefile
+++ b/arch/arm/mach-mvebu/Makefile
@@ -2,12 +2,15 @@ ccflags-$(CONFIG_ARCH_MULTIPLATFORM) := -I$(srctree)/$(src)/include \
 	-I$(srctree)/arch/arm/plat-orion/include
 
 AFLAGS_coherency_ll.o		:= -Wa,-march=armv7-a
+CFLAGS_pmsu.o			:= -march=armv7-a
 
 obj-y				 += system-controller.o mvebu-soc-id.o
-obj-$(CONFIG_MACH_MVEBU_V7)      += board-v7.o
+
+ifeq ($(CONFIG_MACH_MVEBU_V7),y)
+obj-y				 += cpu-reset.o board-v7.o coherency.o coherency_ll.o pmsu.o
+obj-$(CONFIG_SMP)		 += platsmp.o headsmp.o platsmp-a9.o headsmp-a9.o
+obj-$(CONFIG_HOTPLUG_CPU)	 += hotplug.o
+endif
+
 obj-$(CONFIG_MACH_DOVE)		 += dove.o
-obj-$(CONFIG_ARCH_MVEBU)	 += coherency.o coherency_ll.o pmsu.o
-obj-$(CONFIG_SMP)                += platsmp.o headsmp.o
-obj-$(CONFIG_HOTPLUG_CPU)        += hotplug.o
 obj-$(CONFIG_MACH_KIRKWOOD)	 += kirkwood.o kirkwood-pm.o
-obj-$(CONFIG_MACH_T5325)	 += board-t5325.o
diff --git a/arch/arm/mach-mvebu/armada-370-xp.h b/arch/arm/mach-mvebu/armada-370-xp.h
index 237c86b8339..c3465f5b125 100644
--- a/arch/arm/mach-mvebu/armada-370-xp.h
+++ b/arch/arm/mach-mvebu/armada-370-xp.h
@@ -20,8 +20,6 @@
 
 #define ARMADA_XP_MAX_CPUS 4
 
-void armada_mpic_send_doorbell(const struct cpumask *mask, unsigned int irq);
-void armada_xp_mpic_smp_cpu_init(void);
 void armada_xp_secondary_startup(void);
 extern struct smp_operations armada_xp_smp_ops;
 #endif
diff --git a/arch/arm/mach-mvebu/board-t5325.c b/arch/arm/mach-mvebu/board-t5325.c
deleted file mode 100644
index 65ace6db9f2..00000000000
--- a/arch/arm/mach-mvebu/board-t5325.c
+++ /dev/null
@@ -1,41 +0,0 @@
-/*
- * HP T5325 Board Setup
- *
- * Copyright (C) 2014
- *
- * Andrew Lunn <andrew@lunn.ch>
- *
- * 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.
- */
-
-#include <linux/kernel.h>
-#include <linux/i2c.h>
-#include <linux/init.h>
-#include <linux/platform_device.h>
-#include <sound/alc5623.h>
-#include "board.h"
-
-static struct platform_device hp_t5325_audio_device = {
-	.name		= "t5325-audio",
-	.id		= -1,
-};
-
-static struct alc5623_platform_data alc5621_data = {
-	.add_ctrl = 0x3700,
-	.jack_det_ctrl = 0x4810,
-};
-
-static struct i2c_board_info i2c_board_info[] __initdata = {
-	{
-		I2C_BOARD_INFO("alc5621", 0x1a),
-		.platform_data = &alc5621_data,
-	},
-};
-
-void __init t5325_init(void)
-{
-	i2c_register_board_info(0, i2c_board_info, ARRAY_SIZE(i2c_board_info));
-	platform_device_register(&hp_t5325_audio_device);
-}
diff --git a/arch/arm/mach-mvebu/board-v7.c b/arch/arm/mach-mvebu/board-v7.c
index 333fca8fdc4..01cfce6ac20 100644
--- a/arch/arm/mach-mvebu/board-v7.c
+++ b/arch/arm/mach-mvebu/board-v7.c
@@ -27,12 +27,30 @@
 #include <asm/mach/arch.h>
 #include <asm/mach/map.h>
 #include <asm/mach/time.h>
+#include <asm/smp_scu.h>
 #include "armada-370-xp.h"
 #include "common.h"
 #include "coherency.h"
 #include "mvebu-soc-id.h"
 
 /*
+ * Enables the SCU when available. Obviously, this is only useful on
+ * Cortex-A based SOCs, not on PJ4B based ones.
+ */
+static void __init mvebu_scu_enable(void)
+{
+	void __iomem *scu_base;
+
+	struct device_node *np =
+		of_find_compatible_node(NULL, NULL, "arm,cortex-a9-scu");
+	if (np) {
+		scu_base = of_iomap(np, 0);
+		scu_enable(scu_base);
+		of_node_put(np);
+	}
+}
+
+/*
  * Early versions of Armada 375 SoC have a bug where the BootROM
  * leaves an external data abort pending. The kernel is hit by this
  * data abort as soon as it enters userspace, because it unmasks the
@@ -57,11 +75,10 @@ static void __init mvebu_timer_and_clk_init(void)
 {
 	of_clk_init(NULL);
 	clocksource_of_init();
+	mvebu_scu_enable();
 	coherency_init();
-	BUG_ON(mvebu_mbus_dt_init());
-#ifdef CONFIG_CACHE_L2X0
+	BUG_ON(mvebu_mbus_dt_init(coherency_available()));
 	l2x0_of_init(0, ~0UL);
-#endif
 
 	if (of_machine_is_compatible("marvell,armada375"))
 		hook_fault_code(16 + 6, armada_375_external_abort_wa, SIGBUS, 0,
@@ -78,7 +95,7 @@ static void __init i2c_quirk(void)
 	 * mechanism. We can exit only if we are sure that we can
 	 * get the SoC revision and it is more recent than A0.
 	 */
-	if (mvebu_get_soc_id(&rev, &dev) == 0 && dev > MV78XX0_A0_REV)
+	if (mvebu_get_soc_id(&dev, &rev) == 0 && rev > MV78XX0_A0_REV)
 		return;
 
 	for_each_compatible_node(np, NULL, "marvell,mv78230-i2c") {
@@ -96,10 +113,66 @@ static void __init i2c_quirk(void)
 	return;
 }
 
+#define A375_Z1_THERMAL_FIXUP_OFFSET 0xc
+
+static void __init thermal_quirk(void)
+{
+	struct device_node *np;
+	u32 dev, rev;
+
+	if (mvebu_get_soc_id(&dev, &rev) == 0 && rev > ARMADA_375_Z1_REV)
+		return;
+
+	for_each_compatible_node(np, NULL, "marvell,armada375-thermal") {
+		struct property *prop;
+		__be32 newval, *newprop, *oldprop;
+		int len;
+
+		/*
+		 * The register offset is at a wrong location. This quirk
+		 * creates a new reg property as a clone of the previous
+		 * one and corrects the offset.
+		 */
+		oldprop = (__be32 *)of_get_property(np, "reg", &len);
+		if (!oldprop)
+			continue;
+
+		/* Create a duplicate of the 'reg' property */
+		prop = kzalloc(sizeof(*prop), GFP_KERNEL);
+		prop->length = len;
+		prop->name = kstrdup("reg", GFP_KERNEL);
+		prop->value = kzalloc(len, GFP_KERNEL);
+		memcpy(prop->value, oldprop, len);
+
+		/* Fixup the register offset of the second entry */
+		oldprop += 2;
+		newprop = (__be32 *)prop->value + 2;
+		newval = cpu_to_be32(be32_to_cpu(*oldprop) -
+				     A375_Z1_THERMAL_FIXUP_OFFSET);
+		*newprop = newval;
+		of_update_property(np, prop);
+
+		/*
+		 * The thermal controller needs some quirk too, so let's change
+		 * the compatible string to reflect this.
+		 */
+		prop = kzalloc(sizeof(*prop), GFP_KERNEL);
+		prop->name = kstrdup("compatible", GFP_KERNEL);
+		prop->length = sizeof("marvell,armada375-z1-thermal");
+		prop->value = kstrdup("marvell,armada375-z1-thermal",
+						GFP_KERNEL);
+		of_update_property(np, prop);
+	}
+	return;
+}
+
 static void __init mvebu_dt_init(void)
 {
 	if (of_machine_is_compatible("plathome,openblocks-ax3-4"))
 		i2c_quirk();
+	if (of_machine_is_compatible("marvell,a375-db"))
+		thermal_quirk();
+
 	of_platform_populate(NULL, of_default_bus_match_table, NULL, NULL);
 }
 
@@ -123,6 +196,7 @@ static const char * const armada_375_dt_compat[] = {
 
 DT_MACHINE_START(ARMADA_375_DT, "Marvell Armada 375 (Device Tree)")
 	.init_time	= mvebu_timer_and_clk_init,
+	.init_machine	= mvebu_dt_init,
 	.restart	= mvebu_restart,
 	.dt_compat	= armada_375_dt_compat,
 MACHINE_END
diff --git a/arch/arm/mach-mvebu/board.h b/arch/arm/mach-mvebu/board.h
index de7f0a19139..9c7bb4386f8 100644
--- a/arch/arm/mach-mvebu/board.h
+++ b/arch/arm/mach-mvebu/board.h
@@ -13,10 +13,4 @@
 #ifndef __ARCH_MVEBU_BOARD_H
 #define __ARCH_MVEBU_BOARD_H
 
-#ifdef CONFIG_MACH_T5325
-void t5325_init(void);
-#else
-static inline void t5325_init(void) {};
-#endif
-
 #endif
diff --git a/arch/arm/mach-mvebu/coherency.c b/arch/arm/mach-mvebu/coherency.c
index 4e9d58148ca..477202fd39c 100644
--- a/arch/arm/mach-mvebu/coherency.c
+++ b/arch/arm/mach-mvebu/coherency.c
@@ -17,6 +17,8 @@
  * supplies basic routines for configuring and controlling hardware coherency
  */
 
+#define pr_fmt(fmt) "mvebu-coherency: " fmt
+
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/of_address.h>
@@ -24,13 +26,19 @@
 #include <linux/smp.h>
 #include <linux/dma-mapping.h>
 #include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/mbus.h>
+#include <linux/clk.h>
+#include <linux/pci.h>
 #include <asm/smp_plat.h>
 #include <asm/cacheflush.h>
+#include <asm/mach/map.h>
 #include "armada-370-xp.h"
 #include "coherency.h"
+#include "mvebu-soc-id.h"
 
 unsigned long coherency_phys_base;
-static void __iomem *coherency_base;
+void __iomem *coherency_base;
 static void __iomem *coherency_cpu_base;
 
 /* Coherency fabric registers */
@@ -38,27 +46,190 @@ static void __iomem *coherency_cpu_base;
 
 #define IO_SYNC_BARRIER_CTL_OFFSET		   0x0
 
+enum {
+	COHERENCY_FABRIC_TYPE_NONE,
+	COHERENCY_FABRIC_TYPE_ARMADA_370_XP,
+	COHERENCY_FABRIC_TYPE_ARMADA_375,
+	COHERENCY_FABRIC_TYPE_ARMADA_380,
+};
+
 static struct of_device_id of_coherency_table[] = {
-	{.compatible = "marvell,coherency-fabric"},
+	{.compatible = "marvell,coherency-fabric",
+	 .data = (void *) COHERENCY_FABRIC_TYPE_ARMADA_370_XP },
+	{.compatible = "marvell,armada-375-coherency-fabric",
+	 .data = (void *) COHERENCY_FABRIC_TYPE_ARMADA_375 },
+	{.compatible = "marvell,armada-380-coherency-fabric",
+	 .data = (void *) COHERENCY_FABRIC_TYPE_ARMADA_380 },
 	{ /* end of list */ },
 };
 
-/* Function defined in coherency_ll.S */
-int ll_set_cpu_coherent(void __iomem *base_addr, unsigned int hw_cpu_id);
+/* Functions defined in coherency_ll.S */
+int ll_enable_coherency(void);
+void ll_add_cpu_to_smp_group(void);
 
-int set_cpu_coherent(unsigned int hw_cpu_id, int smp_group_id)
+int set_cpu_coherent(void)
 {
 	if (!coherency_base) {
-		pr_warn("Can't make CPU %d cache coherent.\n", hw_cpu_id);
+		pr_warn("Can't make current CPU cache coherent.\n");
 		pr_warn("Coherency fabric is not initialized\n");
 		return 1;
 	}
 
-	return ll_set_cpu_coherent(coherency_base, hw_cpu_id);
+	ll_add_cpu_to_smp_group();
+	return ll_enable_coherency();
+}
+
+/*
+ * The below code implements the I/O coherency workaround on Armada
+ * 375. This workaround consists in using the two channels of the
+ * first XOR engine to trigger a XOR transaction that serves as the
+ * I/O coherency barrier.
+ */
+
+static void __iomem *xor_base, *xor_high_base;
+static dma_addr_t coherency_wa_buf_phys[CONFIG_NR_CPUS];
+static void *coherency_wa_buf[CONFIG_NR_CPUS];
+static bool coherency_wa_enabled;
+
+#define XOR_CONFIG(chan)            (0x10 + (chan * 4))
+#define XOR_ACTIVATION(chan)        (0x20 + (chan * 4))
+#define WINDOW_BAR_ENABLE(chan)     (0x240 + ((chan) << 2))
+#define WINDOW_BASE(w)              (0x250 + ((w) << 2))
+#define WINDOW_SIZE(w)              (0x270 + ((w) << 2))
+#define WINDOW_REMAP_HIGH(w)        (0x290 + ((w) << 2))
+#define WINDOW_OVERRIDE_CTRL(chan)  (0x2A0 + ((chan) << 2))
+#define XOR_DEST_POINTER(chan)      (0x2B0 + (chan * 4))
+#define XOR_BLOCK_SIZE(chan)        (0x2C0 + (chan * 4))
+#define XOR_INIT_VALUE_LOW           0x2E0
+#define XOR_INIT_VALUE_HIGH          0x2E4
+
+static inline void mvebu_hwcc_armada375_sync_io_barrier_wa(void)
+{
+	int idx = smp_processor_id();
+
+	/* Write '1' to the first word of the buffer */
+	writel(0x1, coherency_wa_buf[idx]);
+
+	/* Wait until the engine is idle */
+	while ((readl(xor_base + XOR_ACTIVATION(idx)) >> 4) & 0x3)
+		;
+
+	dmb();
+
+	/* Trigger channel */
+	writel(0x1, xor_base + XOR_ACTIVATION(idx));
+
+	/* Poll the data until it is cleared by the XOR transaction */
+	while (readl(coherency_wa_buf[idx]))
+		;
+}
+
+static void __init armada_375_coherency_init_wa(void)
+{
+	const struct mbus_dram_target_info *dram;
+	struct device_node *xor_node;
+	struct property *xor_status;
+	struct clk *xor_clk;
+	u32 win_enable = 0;
+	int i;
+
+	pr_warn("enabling coherency workaround for Armada 375 Z1, one XOR engine disabled\n");
+
+	/*
+	 * Since the workaround uses one XOR engine, we grab a
+	 * reference to its Device Tree node first.
+	 */
+	xor_node = of_find_compatible_node(NULL, NULL, "marvell,orion-xor");
+	BUG_ON(!xor_node);
+
+	/*
+	 * Then we mark it as disabled so that the real XOR driver
+	 * will not use it.
+	 */
+	xor_status = kzalloc(sizeof(struct property), GFP_KERNEL);
+	BUG_ON(!xor_status);
+
+	xor_status->value = kstrdup("disabled", GFP_KERNEL);
+	BUG_ON(!xor_status->value);
+
+	xor_status->length = 8;
+	xor_status->name = kstrdup("status", GFP_KERNEL);
+	BUG_ON(!xor_status->name);
+
+	of_update_property(xor_node, xor_status);
+
+	/*
+	 * And we remap the registers, get the clock, and do the
+	 * initial configuration of the XOR engine.
+	 */
+	xor_base = of_iomap(xor_node, 0);
+	xor_high_base = of_iomap(xor_node, 1);
+
+	xor_clk = of_clk_get_by_name(xor_node, NULL);
+	BUG_ON(!xor_clk);
+
+	clk_prepare_enable(xor_clk);
+
+	dram = mv_mbus_dram_info();
+
+	for (i = 0; i < 8; i++) {
+		writel(0, xor_base + WINDOW_BASE(i));
+		writel(0, xor_base + WINDOW_SIZE(i));
+		if (i < 4)
+			writel(0, xor_base + WINDOW_REMAP_HIGH(i));
+	}
+
+	for (i = 0; i < dram->num_cs; i++) {
+		const struct mbus_dram_window *cs = dram->cs + i;
+		writel((cs->base & 0xffff0000) |
+		       (cs->mbus_attr << 8) |
+		       dram->mbus_dram_target_id, xor_base + WINDOW_BASE(i));
+		writel((cs->size - 1) & 0xffff0000, xor_base + WINDOW_SIZE(i));
+
+		win_enable |= (1 << i);
+		win_enable |= 3 << (16 + (2 * i));
+	}
+
+	writel(win_enable, xor_base + WINDOW_BAR_ENABLE(0));
+	writel(win_enable, xor_base + WINDOW_BAR_ENABLE(1));
+	writel(0, xor_base + WINDOW_OVERRIDE_CTRL(0));
+	writel(0, xor_base + WINDOW_OVERRIDE_CTRL(1));
+
+	for (i = 0; i < CONFIG_NR_CPUS; i++) {
+		coherency_wa_buf[i] = kzalloc(PAGE_SIZE, GFP_KERNEL);
+		BUG_ON(!coherency_wa_buf[i]);
+
+		/*
+		 * We can't use the DMA mapping API, since we don't
+		 * have a valid 'struct device' pointer
+		 */
+		coherency_wa_buf_phys[i] =
+			virt_to_phys(coherency_wa_buf[i]);
+		BUG_ON(!coherency_wa_buf_phys[i]);
+
+		/*
+		 * Configure the XOR engine for memset operation, with
+		 * a 128 bytes block size
+		 */
+		writel(0x444, xor_base + XOR_CONFIG(i));
+		writel(128, xor_base + XOR_BLOCK_SIZE(i));
+		writel(coherency_wa_buf_phys[i],
+		       xor_base + XOR_DEST_POINTER(i));
+	}
+
+	writel(0x0, xor_base + XOR_INIT_VALUE_LOW);
+	writel(0x0, xor_base + XOR_INIT_VALUE_HIGH);
+
+	coherency_wa_enabled = true;
 }
 
 static inline void mvebu_hwcc_sync_io_barrier(void)
 {
+	if (coherency_wa_enabled) {
+		mvebu_hwcc_armada375_sync_io_barrier_wa();
+		return;
+	}
+
 	writel(0x1, coherency_cpu_base + IO_SYNC_BARRIER_CTL_OFFSET);
 	while (readl(coherency_cpu_base + IO_SYNC_BARRIER_CTL_OFFSET) & 0x1);
 }
@@ -105,8 +276,8 @@ static struct dma_map_ops mvebu_hwcc_dma_ops = {
 	.set_dma_mask		= arm_dma_set_mask,
 };
 
-static int mvebu_hwcc_platform_notifier(struct notifier_block *nb,
-				       unsigned long event, void *__dev)
+static int mvebu_hwcc_notifier(struct notifier_block *nb,
+			       unsigned long event, void *__dev)
 {
 	struct device *dev = __dev;
 
@@ -117,47 +288,148 @@ static int mvebu_hwcc_platform_notifier(struct notifier_block *nb,
 	return NOTIFY_OK;
 }
 
-static struct notifier_block mvebu_hwcc_platform_nb = {
-	.notifier_call = mvebu_hwcc_platform_notifier,
+static struct notifier_block mvebu_hwcc_nb = {
+	.notifier_call = mvebu_hwcc_notifier,
 };
 
-int __init coherency_init(void)
+static void __init armada_370_coherency_init(struct device_node *np)
+{
+	struct resource res;
+
+	of_address_to_resource(np, 0, &res);
+	coherency_phys_base = res.start;
+	/*
+	 * Ensure secondary CPUs will see the updated value,
+	 * which they read before they join the coherency
+	 * fabric, and therefore before they are coherent with
+	 * the boot CPU cache.
+	 */
+	sync_cache_w(&coherency_phys_base);
+	coherency_base = of_iomap(np, 0);
+	coherency_cpu_base = of_iomap(np, 1);
+	set_cpu_coherent();
+}
+
+/*
+ * This ioremap hook is used on Armada 375/38x to ensure that PCIe
+ * memory areas are mapped as MT_UNCACHED instead of MT_DEVICE. This
+ * is needed as a workaround for a deadlock issue between the PCIe
+ * interface and the cache controller.
+ */
+static void __iomem *
+armada_pcie_wa_ioremap_caller(phys_addr_t phys_addr, size_t size,
+			      unsigned int mtype, void *caller)
+{
+	struct resource pcie_mem;
+
+	mvebu_mbus_get_pcie_mem_aperture(&pcie_mem);
+
+	if (pcie_mem.start <= phys_addr && (phys_addr + size) <= pcie_mem.end)
+		mtype = MT_UNCACHED;
+
+	return __arm_ioremap_caller(phys_addr, size, mtype, caller);
+}
+
+static void __init armada_375_380_coherency_init(struct device_node *np)
+{
+	struct device_node *cache_dn;
+
+	coherency_cpu_base = of_iomap(np, 0);
+	arch_ioremap_caller = armada_pcie_wa_ioremap_caller;
+
+	/*
+	 * Add the PL310 property "arm,io-coherent". This makes sure the
+	 * outer sync operation is not used, which allows to
+	 * workaround the system erratum that causes deadlocks when
+	 * doing PCIe in an SMP situation on Armada 375 and Armada
+	 * 38x.
+	 */
+	for_each_compatible_node(cache_dn, NULL, "arm,pl310-cache") {
+		struct property *p;
+
+		p = kzalloc(sizeof(*p), GFP_KERNEL);
+		p->name = kstrdup("arm,io-coherent", GFP_KERNEL);
+		of_add_property(cache_dn, p);
+	}
+}
+
+static int coherency_type(void)
 {
 	struct device_node *np;
+	const struct of_device_id *match;
 
-	np = of_find_matching_node(NULL, of_coherency_table);
+	np = of_find_matching_node_and_match(NULL, of_coherency_table, &match);
 	if (np) {
-		struct resource res;
-		pr_info("Initializing Coherency fabric\n");
-		of_address_to_resource(np, 0, &res);
-		coherency_phys_base = res.start;
-		/*
-		 * Ensure secondary CPUs will see the updated value,
-		 * which they read before they join the coherency
-		 * fabric, and therefore before they are coherent with
-		 * the boot CPU cache.
-		 */
-		sync_cache_w(&coherency_phys_base);
-		coherency_base = of_iomap(np, 0);
-		coherency_cpu_base = of_iomap(np, 1);
-		set_cpu_coherent(cpu_logical_map(smp_processor_id()), 0);
-		of_node_put(np);
+		int type = (int) match->data;
+
+		/* Armada 370/XP coherency works in both UP and SMP */
+		if (type == COHERENCY_FABRIC_TYPE_ARMADA_370_XP)
+			return type;
+
+		/* Armada 375 coherency works only on SMP */
+		else if (type == COHERENCY_FABRIC_TYPE_ARMADA_375 && is_smp())
+			return type;
+
+		/* Armada 380 coherency works only on SMP */
+		else if (type == COHERENCY_FABRIC_TYPE_ARMADA_380 && is_smp())
+			return type;
 	}
 
-	return 0;
+	return COHERENCY_FABRIC_TYPE_NONE;
 }
 
-static int __init coherency_late_init(void)
+int coherency_available(void)
+{
+	return coherency_type() != COHERENCY_FABRIC_TYPE_NONE;
+}
+
+int __init coherency_init(void)
 {
+	int type = coherency_type();
 	struct device_node *np;
 
 	np = of_find_matching_node(NULL, of_coherency_table);
-	if (np) {
-		bus_register_notifier(&platform_bus_type,
-				      &mvebu_hwcc_platform_nb);
-		of_node_put(np);
+
+	if (type == COHERENCY_FABRIC_TYPE_ARMADA_370_XP)
+		armada_370_coherency_init(np);
+	else if (type == COHERENCY_FABRIC_TYPE_ARMADA_375 ||
+		 type == COHERENCY_FABRIC_TYPE_ARMADA_380)
+		armada_375_380_coherency_init(np);
+
+	return 0;
+}
+
+static int __init coherency_late_init(void)
+{
+	int type = coherency_type();
+
+	if (type == COHERENCY_FABRIC_TYPE_NONE)
+		return 0;
+
+	if (type == COHERENCY_FABRIC_TYPE_ARMADA_375) {
+		u32 dev, rev;
+
+		if (mvebu_get_soc_id(&dev, &rev) == 0 &&
+		    rev == ARMADA_375_Z1_REV)
+			armada_375_coherency_init_wa();
 	}
+
+	bus_register_notifier(&platform_bus_type,
+			      &mvebu_hwcc_nb);
+
 	return 0;
 }
 
 postcore_initcall(coherency_late_init);
+
+#if IS_ENABLED(CONFIG_PCI)
+static int __init coherency_pci_init(void)
+{
+	if (coherency_available())
+		bus_register_notifier(&pci_bus_type,
+				       &mvebu_hwcc_nb);
+	return 0;
+}
+
+arch_initcall(coherency_pci_init);
+#endif
diff --git a/arch/arm/mach-mvebu/coherency.h b/arch/arm/mach-mvebu/coherency.h
index 760226c4135..54cb7607b52 100644
--- a/arch/arm/mach-mvebu/coherency.h
+++ b/arch/arm/mach-mvebu/coherency.h
@@ -15,8 +15,9 @@
 #define __MACH_370_XP_COHERENCY_H
 
 extern unsigned long coherency_phys_base;
+int set_cpu_coherent(void);
 
-int set_cpu_coherent(unsigned int cpu_id, int smp_group_id);
 int coherency_init(void);
+int coherency_available(void);
 
 #endif	/* __MACH_370_XP_COHERENCY_H */
diff --git a/arch/arm/mach-mvebu/coherency_ll.S b/arch/arm/mach-mvebu/coherency_ll.S
index ee7598fe75d..510c29e079c 100644
--- a/arch/arm/mach-mvebu/coherency_ll.S
+++ b/arch/arm/mach-mvebu/coherency_ll.S
@@ -21,38 +21,129 @@
 #define ARMADA_XP_CFB_CFG_REG_OFFSET 0x4
 
 #include <asm/assembler.h>
+#include <asm/cp15.h>
 
 	.text
+/* Returns the coherency base address in r1 (r0 is untouched) */
+ENTRY(ll_get_coherency_base)
+	mrc	p15, 0, r1, c1, c0, 0
+	tst	r1, #CR_M @ Check MMU bit enabled
+	bne	1f
+
+	/*
+	 * MMU is disabled, use the physical address of the coherency
+	 * base address.
+	 */
+	adr	r1, 3f
+	ldr	r3, [r1]
+	ldr	r1, [r1, r3]
+	b	2f
+1:
+	/*
+	 * MMU is enabled, use the virtual address of the coherency
+	 * base address.
+	 */
+	ldr	r1, =coherency_base
+	ldr	r1, [r1]
+2:
+	mov	pc, lr
+ENDPROC(ll_get_coherency_base)
+
 /*
- * r0: Coherency fabric base register address
- * r1: HW CPU id
+ * Returns the coherency CPU mask in r3 (r0 is untouched). This
+ * coherency CPU mask can be used with the coherency fabric
+ * configuration and control registers. Note that the mask is already
+ * endian-swapped as appropriate so that the calling functions do not
+ * have to care about endianness issues while accessing the coherency
+ * fabric registers
  */
-ENTRY(ll_set_cpu_coherent)
-	/* Create bit by cpu index */
-	mov	r3, #(1 << 24)
-	lsl	r1, r3, r1
-ARM_BE8(rev	r1, r1)
-
-	/* Add CPU to SMP group - Atomic */
-	add	r3, r0, #ARMADA_XP_CFB_CTL_REG_OFFSET
-1:
-	ldrex	r2, [r3]
-	orr	r2, r2, r1
-	strex 	r0, r2, [r3]
-	cmp	r0, #0
-	bne 1b
-
-	/* Enable coherency on CPU - Atomic */
-	add	r3, r3, #ARMADA_XP_CFB_CFG_REG_OFFSET
+ENTRY(ll_get_coherency_cpumask)
+	mrc	15, 0, r3, cr0, cr0, 5
+	and	r3, r3, #15
+	mov	r2, #(1 << 24)
+	lsl	r3, r2, r3
+ARM_BE8(rev	r3, r3)
+	mov	pc, lr
+ENDPROC(ll_get_coherency_cpumask)
+
+/*
+ * ll_add_cpu_to_smp_group(), ll_enable_coherency() and
+ * ll_disable_coherency() use the strex/ldrex instructions while the
+ * MMU can be disabled. The Armada XP SoC has an exclusive monitor
+ * that tracks transactions to Device and/or SO memory and thanks to
+ * that, exclusive transactions are functional even when the MMU is
+ * disabled.
+ */
+
+ENTRY(ll_add_cpu_to_smp_group)
+	/*
+	 * As r0 is not modified by ll_get_coherency_base() and
+	 * ll_get_coherency_cpumask(), we use it to temporarly save lr
+	 * and avoid it being modified by the branch and link
+	 * calls. This function is used very early in the secondary
+	 * CPU boot, and no stack is available at this point.
+	 */
+	mov 	r0, lr
+	bl	ll_get_coherency_base
+	bl	ll_get_coherency_cpumask
+	mov 	lr, r0
+	add	r0, r1, #ARMADA_XP_CFB_CFG_REG_OFFSET
 1:
-	ldrex	r2, [r3]
-	orr	r2, r2, r1
-	strex	r0, r2, [r3]
-	cmp	r0, #0
-	bne 1b
+	ldrex	r2, [r0]
+	orr	r2, r2, r3
+	strex	r1, r2, [r0]
+	cmp	r1, #0
+	bne	1b
+	mov	pc, lr
+ENDPROC(ll_add_cpu_to_smp_group)
 
+ENTRY(ll_enable_coherency)
+	/*
+	 * As r0 is not modified by ll_get_coherency_base() and
+	 * ll_get_coherency_cpumask(), we use it to temporarly save lr
+	 * and avoid it being modified by the branch and link
+	 * calls. This function is used very early in the secondary
+	 * CPU boot, and no stack is available at this point.
+	 */
+	mov r0, lr
+	bl	ll_get_coherency_base
+	bl	ll_get_coherency_cpumask
+	mov lr, r0
+	add	r0, r1, #ARMADA_XP_CFB_CTL_REG_OFFSET
+1:
+	ldrex	r2, [r0]
+	orr	r2, r2, r3
+	strex	r1, r2, [r0]
+	cmp	r1, #0
+	bne	1b
 	dsb
-
 	mov	r0, #0
 	mov	pc, lr
-ENDPROC(ll_set_cpu_coherent)
+ENDPROC(ll_enable_coherency)
+
+ENTRY(ll_disable_coherency)
+	/*
+	 * As r0 is not modified by ll_get_coherency_base() and
+	 * ll_get_coherency_cpumask(), we use it to temporarly save lr
+	 * and avoid it being modified by the branch and link
+	 * calls. This function is used very early in the secondary
+	 * CPU boot, and no stack is available at this point.
+	 */
+	mov 	r0, lr
+	bl	ll_get_coherency_base
+	bl	ll_get_coherency_cpumask
+	mov 	lr, r0
+	add	r0, r1, #ARMADA_XP_CFB_CTL_REG_OFFSET
+1:
+	ldrex	r2, [r0]
+	bic	r2, r2, r3
+	strex	r1, r2, [r0]
+	cmp	r1, #0
+	bne	1b
+	dsb
+	mov	pc, lr
+ENDPROC(ll_disable_coherency)
+
+	.align 2
+3:
+	.long	coherency_phys_base - .
diff --git a/arch/arm/mach-mvebu/common.h b/arch/arm/mach-mvebu/common.h
index 55449c487c9..b67fb7a10d8 100644
--- a/arch/arm/mach-mvebu/common.h
+++ b/arch/arm/mach-mvebu/common.h
@@ -18,6 +18,9 @@
 #include <linux/reboot.h>
 
 void mvebu_restart(enum reboot_mode mode, const char *cmd);
+int mvebu_cpu_reset_deassert(int cpu);
+void mvebu_pmsu_set_cpu_boot_addr(int hw_cpu, void *boot_addr);
+void mvebu_system_controller_set_cpu_boot_addr(void *boot_addr);
 
 void armada_xp_cpu_die(unsigned int cpu);
 
diff --git a/arch/arm/mach-mvebu/cpu-reset.c b/arch/arm/mach-mvebu/cpu-reset.c
new file mode 100644
index 00000000000..4a8f9eebebe
--- /dev/null
+++ b/arch/arm/mach-mvebu/cpu-reset.c
@@ -0,0 +1,103 @@
+/*
+ * Copyright (C) 2014 Marvell
+ *
+ * Thomas Petazzoni <thomas.petazzoni@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.
+ */
+
+#define pr_fmt(fmt) "mvebu-cpureset: " fmt
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/of_address.h>
+#include <linux/io.h>
+#include <linux/resource.h>
+#include "armada-370-xp.h"
+
+static void __iomem *cpu_reset_base;
+static size_t cpu_reset_size;
+
+#define CPU_RESET_OFFSET(cpu) (cpu * 0x8)
+#define CPU_RESET_ASSERT      BIT(0)
+
+int mvebu_cpu_reset_deassert(int cpu)
+{
+	u32 reg;
+
+	if (!cpu_reset_base)
+		return -ENODEV;
+
+	if (CPU_RESET_OFFSET(cpu) >= cpu_reset_size)
+		return -EINVAL;
+
+	reg = readl(cpu_reset_base + CPU_RESET_OFFSET(cpu));
+	reg &= ~CPU_RESET_ASSERT;
+	writel(reg, cpu_reset_base + CPU_RESET_OFFSET(cpu));
+
+	return 0;
+}
+
+static int mvebu_cpu_reset_map(struct device_node *np, int res_idx)
+{
+	struct resource res;
+
+	if (of_address_to_resource(np, res_idx, &res)) {
+		pr_err("unable to get resource\n");
+		return -ENOENT;
+	}
+
+	if (!request_mem_region(res.start, resource_size(&res),
+				np->full_name)) {
+		pr_err("unable to request region\n");
+		return -EBUSY;
+	}
+
+	cpu_reset_base = ioremap(res.start, resource_size(&res));
+	if (!cpu_reset_base) {
+		pr_err("unable to map registers\n");
+		release_mem_region(res.start, resource_size(&res));
+		return -ENOMEM;
+	}
+
+	cpu_reset_size = resource_size(&res);
+
+	return 0;
+}
+
+int __init mvebu_cpu_reset_init(void)
+{
+	struct device_node *np;
+	int res_idx;
+	int ret;
+
+	np = of_find_compatible_node(NULL, NULL,
+				     "marvell,armada-370-cpu-reset");
+	if (np) {
+		res_idx = 0;
+	} else {
+		/*
+		 * This code is kept for backward compatibility with
+		 * old Device Trees.
+		 */
+		np = of_find_compatible_node(NULL, NULL,
+					     "marvell,armada-370-xp-pmsu");
+		if (np) {
+			pr_warn(FW_WARN "deprecated pmsu binding\n");
+			res_idx = 1;
+		}
+	}
+
+	/* No reset node found */
+	if (!np)
+		return -ENODEV;
+
+	ret = mvebu_cpu_reset_map(np, res_idx);
+	of_node_put(np);
+
+	return ret;
+}
+
+early_initcall(mvebu_cpu_reset_init);
diff --git a/arch/arm/mach-mvebu/dove.c b/arch/arm/mach-mvebu/dove.c
index 5e5a4362423..b50464ec113 100644
--- a/arch/arm/mach-mvebu/dove.c
+++ b/arch/arm/mach-mvebu/dove.c
@@ -23,7 +23,7 @@ static void __init dove_init(void)
 #ifdef CONFIG_CACHE_TAUROS2
 	tauros2_init(0);
 #endif
-	BUG_ON(mvebu_mbus_dt_init());
+	BUG_ON(mvebu_mbus_dt_init(false));
 	of_platform_populate(NULL, of_default_bus_match_table, NULL, NULL);
 }
 
diff --git a/arch/arm/mach-mvebu/headsmp-a9.S b/arch/arm/mach-mvebu/headsmp-a9.S
new file mode 100644
index 00000000000..5925366bc03
--- /dev/null
+++ b/arch/arm/mach-mvebu/headsmp-a9.S
@@ -0,0 +1,34 @@
+/*
+ * SMP support: Entry point for secondary CPUs of Marvell EBU
+ * Cortex-A9 based SOCs (Armada 375 and Armada 38x).
+ *
+ * Copyright (C) 2014 Marvell
+ *
+ * Gregory CLEMENT <gregory.clement@free-electrons.com>
+ * Thomas Petazzoni <thomas.petazzoni@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.
+ */
+
+#include <linux/linkage.h>
+#include <linux/init.h>
+
+	__CPUINIT
+#define CPU_RESUME_ADDR_REG 0xf10182d4
+
+.global armada_375_smp_cpu1_enable_code_start
+.global armada_375_smp_cpu1_enable_code_end
+
+armada_375_smp_cpu1_enable_code_start:
+	ldr     r0, [pc, #4]
+	ldr     r1, [r0]
+	mov     pc, r1
+	.word   CPU_RESUME_ADDR_REG
+armada_375_smp_cpu1_enable_code_end:
+
+ENTRY(mvebu_cortex_a9_secondary_startup)
+	bl      v7_invalidate_l1
+	b	secondary_startup
+ENDPROC(mvebu_cortex_a9_secondary_startup)
diff --git a/arch/arm/mach-mvebu/headsmp.S b/arch/arm/mach-mvebu/headsmp.S
index 3dd80df428f..2c4032e368b 100644
--- a/arch/arm/mach-mvebu/headsmp.S
+++ b/arch/arm/mach-mvebu/headsmp.S
@@ -31,21 +31,10 @@
 ENTRY(armada_xp_secondary_startup)
  ARM_BE8(setend	be )			@ go BE8 if entered LE
 
-	/* Get coherency fabric base physical address */
-	adr	r0, 1f
-	ldr	r1, [r0]
-	ldr	r0, [r0, r1]
+	bl	ll_add_cpu_to_smp_group
 
-	/* Read CPU id */
-	mrc     p15, 0, r1, c0, c0, 5
-	and     r1, r1, #0xF
+	bl	ll_enable_coherency
 
-	/* Add CPU to coherency fabric */
-	bl	ll_set_cpu_coherent
 	b	secondary_startup
 
 ENDPROC(armada_xp_secondary_startup)
-
-	.align 2
-1:
-	.long	coherency_phys_base - .
diff --git a/arch/arm/mach-mvebu/kirkwood.c b/arch/arm/mach-mvebu/kirkwood.c
index 120207fc36f..46f105913c8 100644
--- a/arch/arm/mach-mvebu/kirkwood.c
+++ b/arch/arm/mach-mvebu/kirkwood.c
@@ -169,7 +169,7 @@ static void __init kirkwood_dt_init(void)
 {
 	kirkwood_disable_mbus_error_propagation();
 
-	BUG_ON(mvebu_mbus_dt_init());
+	BUG_ON(mvebu_mbus_dt_init(false));
 
 #ifdef CONFIG_CACHE_FEROCEON_L2
 	feroceon_of_init();
@@ -180,9 +180,6 @@ static void __init kirkwood_dt_init(void)
 	kirkwood_pm_init();
 	kirkwood_dt_eth_fixup();
 
-	if (of_machine_is_compatible("hp,t5325"))
-		t5325_init();
-
 	of_platform_populate(NULL, of_default_bus_match_table, auxdata, NULL);
 }
 
diff --git a/arch/arm/mach-mvebu/mvebu-soc-id.c b/arch/arm/mach-mvebu/mvebu-soc-id.c
index f3d4cf53f74..d0f35b4d4a2 100644
--- a/arch/arm/mach-mvebu/mvebu-soc-id.c
+++ b/arch/arm/mach-mvebu/mvebu-soc-id.c
@@ -23,6 +23,8 @@
 #include <linux/kernel.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
+#include <linux/slab.h>
+#include <linux/sys_soc.h>
 #include "mvebu-soc-id.h"
 
 #define PCIE_DEV_ID_OFF		0x0
@@ -108,7 +110,18 @@ static int __init mvebu_soc_id_init(void)
 	iounmap(pci_base);
 
 res_ioremap:
-	clk_disable_unprepare(clk);
+	/*
+	 * If the PCIe unit is actually enabled and we have PCI
+	 * support in the kernel, we intentionally do not release the
+	 * reference to the clock. We want to keep it running since
+	 * the bootloader does some PCIe link configuration that the
+	 * kernel is for now unable to do, and gating the clock would
+	 * make us loose this precious configuration.
+	 */
+	if (!of_device_is_available(child) || !IS_ENABLED(CONFIG_PCI_MVEBU)) {
+		clk_disable_unprepare(clk);
+		clk_put(clk);
+	}
 
 clk_err:
 	of_node_put(child);
@@ -116,5 +129,33 @@ clk_err:
 
 	return ret;
 }
-core_initcall(mvebu_soc_id_init);
+early_initcall(mvebu_soc_id_init);
+
+static int __init mvebu_soc_device(void)
+{
+	struct soc_device_attribute *soc_dev_attr;
+	struct soc_device *soc_dev;
+
+	/* Also protects against running on non-mvebu systems */
+	if (!is_id_valid)
+		return 0;
+
+	soc_dev_attr = kzalloc(sizeof(*soc_dev_attr), GFP_KERNEL);
+	if (!soc_dev_attr)
+		return -ENOMEM;
+
+	soc_dev_attr->family = kasprintf(GFP_KERNEL, "Marvell");
+	soc_dev_attr->revision = kasprintf(GFP_KERNEL, "%X", soc_rev);
+	soc_dev_attr->soc_id = kasprintf(GFP_KERNEL, "%X", soc_dev_id);
 
+	soc_dev = soc_device_register(soc_dev_attr);
+	if (IS_ERR(soc_dev)) {
+		kfree(soc_dev_attr->family);
+		kfree(soc_dev_attr->revision);
+		kfree(soc_dev_attr->soc_id);
+		kfree(soc_dev_attr);
+	}
+
+	return 0;
+}
+postcore_initcall(mvebu_soc_device);
diff --git a/arch/arm/mach-mvebu/mvebu-soc-id.h b/arch/arm/mach-mvebu/mvebu-soc-id.h
index 31654252fe3..c16bb68ca81 100644
--- a/arch/arm/mach-mvebu/mvebu-soc-id.h
+++ b/arch/arm/mach-mvebu/mvebu-soc-id.h
@@ -20,6 +20,10 @@
 #define MV78XX0_A0_REV	    0x1
 #define MV78XX0_B0_REV	    0x2
 
+/* Armada 375 */
+#define ARMADA_375_Z1_REV   0x0
+#define ARMADA_375_A0_REV   0x3
+
 #ifdef CONFIG_ARCH_MVEBU
 int mvebu_get_soc_id(u32 *dev, u32 *rev);
 #else
diff --git a/arch/arm/mach-mvebu/platsmp-a9.c b/arch/arm/mach-mvebu/platsmp-a9.c
new file mode 100644
index 00000000000..96c2c59e34b
--- /dev/null
+++ b/arch/arm/mach-mvebu/platsmp-a9.c
@@ -0,0 +1,102 @@
+/*
+ * Symmetric Multi Processing (SMP) support for Marvell EBU Cortex-A9
+ * based SOCs (Armada 375/38x).
+ *
+ * Copyright (C) 2014 Marvell
+ *
+ * Gregory CLEMENT <gregory.clement@free-electrons.com>
+ * Thomas Petazzoni <thomas.petazzoni@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.
+ */
+
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/smp.h>
+#include <linux/mbus.h>
+#include <asm/smp_scu.h>
+#include <asm/smp_plat.h>
+#include "common.h"
+#include "mvebu-soc-id.h"
+#include "pmsu.h"
+
+#define CRYPT0_ENG_ID   41
+#define CRYPT0_ENG_ATTR 0x1
+#define SRAM_PHYS_BASE  0xFFFF0000
+
+#define BOOTROM_BASE    0xFFF00000
+#define BOOTROM_SIZE    0x100000
+
+extern unsigned char armada_375_smp_cpu1_enable_code_end;
+extern unsigned char armada_375_smp_cpu1_enable_code_start;
+
+void armada_375_smp_cpu1_enable_wa(void)
+{
+	void __iomem *sram_virt_base;
+
+	mvebu_mbus_del_window(BOOTROM_BASE, BOOTROM_SIZE);
+	mvebu_mbus_add_window_by_id(CRYPT0_ENG_ID, CRYPT0_ENG_ATTR,
+				SRAM_PHYS_BASE, SZ_64K);
+	sram_virt_base = ioremap(SRAM_PHYS_BASE, SZ_64K);
+
+	memcpy(sram_virt_base, &armada_375_smp_cpu1_enable_code_start,
+	       &armada_375_smp_cpu1_enable_code_end
+	       - &armada_375_smp_cpu1_enable_code_start);
+}
+
+extern void mvebu_cortex_a9_secondary_startup(void);
+
+static int __cpuinit mvebu_cortex_a9_boot_secondary(unsigned int cpu,
+						    struct task_struct *idle)
+{
+	int ret, hw_cpu;
+
+	pr_info("Booting CPU %d\n", cpu);
+
+	/*
+	 * Write the address of secondary startup into the system-wide
+	 * flags register. The boot monitor waits until it receives a
+	 * soft interrupt, and then the secondary CPU branches to this
+	 * address.
+	 */
+	hw_cpu = cpu_logical_map(cpu);
+
+	if (of_machine_is_compatible("marvell,armada375")) {
+		u32 dev, rev;
+
+		if (mvebu_get_soc_id(&dev, &rev) == 0 &&
+		    rev == ARMADA_375_Z1_REV)
+			armada_375_smp_cpu1_enable_wa();
+
+		mvebu_system_controller_set_cpu_boot_addr(mvebu_cortex_a9_secondary_startup);
+	}
+	else {
+		mvebu_pmsu_set_cpu_boot_addr(hw_cpu,
+					     mvebu_cortex_a9_secondary_startup);
+	}
+
+	smp_wmb();
+	ret = mvebu_cpu_reset_deassert(hw_cpu);
+	if (ret) {
+		pr_err("Could not start the secondary CPU: %d\n", ret);
+		return ret;
+	}
+	arch_send_wakeup_ipi_mask(cpumask_of(cpu));
+
+	return 0;
+}
+
+static struct smp_operations mvebu_cortex_a9_smp_ops __initdata = {
+	.smp_boot_secondary	= mvebu_cortex_a9_boot_secondary,
+#ifdef CONFIG_HOTPLUG_CPU
+	.cpu_die		= armada_xp_cpu_die,
+#endif
+};
+
+CPU_METHOD_OF_DECLARE(mvebu_armada_375_smp, "marvell,armada-375-smp",
+		      &mvebu_cortex_a9_smp_ops);
+CPU_METHOD_OF_DECLARE(mvebu_armada_380_smp, "marvell,armada-380-smp",
+		      &mvebu_cortex_a9_smp_ops);
diff --git a/arch/arm/mach-mvebu/platsmp.c b/arch/arm/mach-mvebu/platsmp.c
index a6da03f5b24..88b976b3171 100644
--- a/arch/arm/mach-mvebu/platsmp.c
+++ b/arch/arm/mach-mvebu/platsmp.c
@@ -70,16 +70,19 @@ static void __init set_secondary_cpus_clock(void)
 	}
 }
 
-static void armada_xp_secondary_init(unsigned int cpu)
-{
-	armada_xp_mpic_smp_cpu_init();
-}
-
 static int armada_xp_boot_secondary(unsigned int cpu, struct task_struct *idle)
 {
+	int ret, hw_cpu;
+
 	pr_info("Booting CPU %d\n", cpu);
 
-	armada_xp_boot_cpu(cpu, armada_xp_secondary_startup);
+	hw_cpu = cpu_logical_map(cpu);
+	mvebu_pmsu_set_cpu_boot_addr(hw_cpu, armada_xp_secondary_startup);
+	ret = mvebu_cpu_reset_deassert(hw_cpu);
+	if (ret) {
+		pr_warn("unable to boot CPU: %d\n", ret);
+		return ret;
+	}
 
 	return 0;
 }
@@ -90,8 +93,6 @@ static void __init armada_xp_smp_init_cpus(void)
 
 	if (ncores == 0 || ncores > ARMADA_XP_MAX_CPUS)
 		panic("Invalid number of CPUs in DT\n");
-
-	set_smp_cross_call(armada_mpic_send_doorbell);
 }
 
 static void __init armada_xp_smp_prepare_cpus(unsigned int max_cpus)
@@ -102,7 +103,7 @@ static void __init armada_xp_smp_prepare_cpus(unsigned int max_cpus)
 
 	set_secondary_cpus_clock();
 	flush_cache_all();
-	set_cpu_coherent(cpu_logical_map(smp_processor_id()), 0);
+	set_cpu_coherent();
 
 	/*
 	 * In order to boot the secondary CPUs we need to ensure
@@ -124,9 +125,11 @@ static void __init armada_xp_smp_prepare_cpus(unsigned int max_cpus)
 struct smp_operations armada_xp_smp_ops __initdata = {
 	.smp_init_cpus		= armada_xp_smp_init_cpus,
 	.smp_prepare_cpus	= armada_xp_smp_prepare_cpus,
-	.smp_secondary_init	= armada_xp_secondary_init,
 	.smp_boot_secondary	= armada_xp_boot_secondary,
 #ifdef CONFIG_HOTPLUG_CPU
 	.cpu_die		= armada_xp_cpu_die,
 #endif
 };
+
+CPU_METHOD_OF_DECLARE(armada_xp_smp, "marvell,armada-xp-smp",
+		      &armada_xp_smp_ops);
diff --git a/arch/arm/mach-mvebu/pmsu.c b/arch/arm/mach-mvebu/pmsu.c
index d71ef53107c..53a55c8520b 100644
--- a/arch/arm/mach-mvebu/pmsu.c
+++ b/arch/arm/mach-mvebu/pmsu.c
@@ -16,62 +16,283 @@
  * other SOC units
  */
 
+#define pr_fmt(fmt) "mvebu-pmsu: " fmt
+
+#include <linux/cpu_pm.h>
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/of_address.h>
 #include <linux/io.h>
+#include <linux/platform_device.h>
 #include <linux/smp.h>
+#include <linux/resource.h>
+#include <asm/cacheflush.h>
+#include <asm/cp15.h>
 #include <asm/smp_plat.h>
-#include "pmsu.h"
+#include <asm/suspend.h>
+#include <asm/tlbflush.h>
+#include "common.h"
 
 static void __iomem *pmsu_mp_base;
-static void __iomem *pmsu_reset_base;
 
-#define PMSU_BOOT_ADDR_REDIRECT_OFFSET(cpu)	((cpu * 0x100) + 0x24)
-#define PMSU_RESET_CTL_OFFSET(cpu)		(cpu * 0x8)
+#define PMSU_BASE_OFFSET    0x100
+#define PMSU_REG_SIZE	    0x1000
+
+/* PMSU MP registers */
+#define PMSU_CONTROL_AND_CONFIG(cpu)	    ((cpu * 0x100) + 0x104)
+#define PMSU_CONTROL_AND_CONFIG_DFS_REQ		BIT(18)
+#define PMSU_CONTROL_AND_CONFIG_PWDDN_REQ	BIT(16)
+#define PMSU_CONTROL_AND_CONFIG_L2_PWDDN	BIT(20)
+
+#define PMSU_CPU_POWER_DOWN_CONTROL(cpu)    ((cpu * 0x100) + 0x108)
+
+#define PMSU_CPU_POWER_DOWN_DIS_SNP_Q_SKIP	BIT(0)
+
+#define PMSU_STATUS_AND_MASK(cpu)	    ((cpu * 0x100) + 0x10c)
+#define PMSU_STATUS_AND_MASK_CPU_IDLE_WAIT	BIT(16)
+#define PMSU_STATUS_AND_MASK_SNP_Q_EMPTY_WAIT	BIT(17)
+#define PMSU_STATUS_AND_MASK_IRQ_WAKEUP		BIT(20)
+#define PMSU_STATUS_AND_MASK_FIQ_WAKEUP		BIT(21)
+#define PMSU_STATUS_AND_MASK_DBG_WAKEUP		BIT(22)
+#define PMSU_STATUS_AND_MASK_IRQ_MASK		BIT(24)
+#define PMSU_STATUS_AND_MASK_FIQ_MASK		BIT(25)
+
+#define PMSU_BOOT_ADDR_REDIRECT_OFFSET(cpu) ((cpu * 0x100) + 0x124)
+
+/* PMSU fabric registers */
+#define L2C_NFABRIC_PM_CTL		    0x4
+#define L2C_NFABRIC_PM_CTL_PWR_DOWN		BIT(20)
+
+extern void ll_disable_coherency(void);
+extern void ll_enable_coherency(void);
+
+static struct platform_device armada_xp_cpuidle_device = {
+	.name = "cpuidle-armada-370-xp",
+};
 
 static struct of_device_id of_pmsu_table[] = {
-	{.compatible = "marvell,armada-370-xp-pmsu"},
+	{ .compatible = "marvell,armada-370-pmsu", },
+	{ .compatible = "marvell,armada-370-xp-pmsu", },
+	{ .compatible = "marvell,armada-380-pmsu", },
 	{ /* end of list */ },
 };
 
-#ifdef CONFIG_SMP
-int armada_xp_boot_cpu(unsigned int cpu_id, void *boot_addr)
+void mvebu_pmsu_set_cpu_boot_addr(int hw_cpu, void *boot_addr)
 {
-	int reg, hw_cpu;
+	writel(virt_to_phys(boot_addr), pmsu_mp_base +
+		PMSU_BOOT_ADDR_REDIRECT_OFFSET(hw_cpu));
+}
+
+static int __init armada_370_xp_pmsu_init(void)
+{
+	struct device_node *np;
+	struct resource res;
+	int ret = 0;
+
+	np = of_find_matching_node(NULL, of_pmsu_table);
+	if (!np)
+		return 0;
+
+	pr_info("Initializing Power Management Service Unit\n");
 
-	if (!pmsu_mp_base || !pmsu_reset_base) {
-		pr_warn("Can't boot CPU. PMSU is uninitialized\n");
-		return 1;
+	if (of_address_to_resource(np, 0, &res)) {
+		pr_err("unable to get resource\n");
+		ret = -ENOENT;
+		goto out;
 	}
 
-	hw_cpu = cpu_logical_map(cpu_id);
+	if (of_device_is_compatible(np, "marvell,armada-370-xp-pmsu")) {
+		pr_warn(FW_WARN "deprecated pmsu binding\n");
+		res.start = res.start - PMSU_BASE_OFFSET;
+		res.end = res.start + PMSU_REG_SIZE - 1;
+	}
 
-	writel(virt_to_phys(boot_addr), pmsu_mp_base +
-			PMSU_BOOT_ADDR_REDIRECT_OFFSET(hw_cpu));
+	if (!request_mem_region(res.start, resource_size(&res),
+				np->full_name)) {
+		pr_err("unable to request region\n");
+		ret = -EBUSY;
+		goto out;
+	}
+
+	pmsu_mp_base = ioremap(res.start, resource_size(&res));
+	if (!pmsu_mp_base) {
+		pr_err("unable to map registers\n");
+		release_mem_region(res.start, resource_size(&res));
+		ret = -ENOMEM;
+		goto out;
+	}
+
+ out:
+	of_node_put(np);
+	return ret;
+}
+
+static void armada_370_xp_pmsu_enable_l2_powerdown_onidle(void)
+{
+	u32 reg;
+
+	if (pmsu_mp_base == NULL)
+		return;
+
+	/* Enable L2 & Fabric powerdown in Deep-Idle mode - Fabric */
+	reg = readl(pmsu_mp_base + L2C_NFABRIC_PM_CTL);
+	reg |= L2C_NFABRIC_PM_CTL_PWR_DOWN;
+	writel(reg, pmsu_mp_base + L2C_NFABRIC_PM_CTL);
+}
+
+static void armada_370_xp_cpu_resume(void)
+{
+	asm volatile("bl    ll_add_cpu_to_smp_group\n\t"
+		     "bl    ll_enable_coherency\n\t"
+		     "b	    cpu_resume\n\t");
+}
+
+/* No locking is needed because we only access per-CPU registers */
+void armada_370_xp_pmsu_idle_prepare(bool deepidle)
+{
+	unsigned int hw_cpu = cpu_logical_map(smp_processor_id());
+	u32 reg;
+
+	if (pmsu_mp_base == NULL)
+		return;
 
-	/* Release CPU from reset by clearing reset bit*/
-	reg = readl(pmsu_reset_base + PMSU_RESET_CTL_OFFSET(hw_cpu));
-	reg &= (~0x1);
-	writel(reg, pmsu_reset_base + PMSU_RESET_CTL_OFFSET(hw_cpu));
+	/*
+	 * Adjust the PMSU configuration to wait for WFI signal, enable
+	 * IRQ and FIQ as wakeup events, set wait for snoop queue empty
+	 * indication and mask IRQ and FIQ from CPU
+	 */
+	reg = readl(pmsu_mp_base + PMSU_STATUS_AND_MASK(hw_cpu));
+	reg |= PMSU_STATUS_AND_MASK_CPU_IDLE_WAIT    |
+	       PMSU_STATUS_AND_MASK_IRQ_WAKEUP       |
+	       PMSU_STATUS_AND_MASK_FIQ_WAKEUP       |
+	       PMSU_STATUS_AND_MASK_SNP_Q_EMPTY_WAIT |
+	       PMSU_STATUS_AND_MASK_IRQ_MASK         |
+	       PMSU_STATUS_AND_MASK_FIQ_MASK;
+	writel(reg, pmsu_mp_base + PMSU_STATUS_AND_MASK(hw_cpu));
+
+	reg = readl(pmsu_mp_base + PMSU_CONTROL_AND_CONFIG(hw_cpu));
+	/* ask HW to power down the L2 Cache if needed */
+	if (deepidle)
+		reg |= PMSU_CONTROL_AND_CONFIG_L2_PWDDN;
+
+	/* request power down */
+	reg |= PMSU_CONTROL_AND_CONFIG_PWDDN_REQ;
+	writel(reg, pmsu_mp_base + PMSU_CONTROL_AND_CONFIG(hw_cpu));
+
+	/* Disable snoop disable by HW - SW is taking care of it */
+	reg = readl(pmsu_mp_base + PMSU_CPU_POWER_DOWN_CONTROL(hw_cpu));
+	reg |= PMSU_CPU_POWER_DOWN_DIS_SNP_Q_SKIP;
+	writel(reg, pmsu_mp_base + PMSU_CPU_POWER_DOWN_CONTROL(hw_cpu));
+}
+
+static noinline int do_armada_370_xp_cpu_suspend(unsigned long deepidle)
+{
+	armada_370_xp_pmsu_idle_prepare(deepidle);
+
+	v7_exit_coherency_flush(all);
+
+	ll_disable_coherency();
+
+	dsb();
+
+	wfi();
+
+	/* If we are here, wfi failed. As processors run out of
+	 * coherency for some time, tlbs might be stale, so flush them
+	 */
+	local_flush_tlb_all();
+
+	ll_enable_coherency();
+
+	/* Test the CR_C bit and set it if it was cleared */
+	asm volatile(
+	"mrc	p15, 0, %0, c1, c0, 0 \n\t"
+	"tst	%0, #(1 << 2) \n\t"
+	"orreq	%0, %0, #(1 << 2) \n\t"
+	"mcreq	p15, 0, %0, c1, c0, 0 \n\t"
+	"isb	"
+	: : "r" (0));
+
+	pr_warn("Failed to suspend the system\n");
 
 	return 0;
 }
-#endif
 
-static int __init armada_370_xp_pmsu_init(void)
+static int armada_370_xp_cpu_suspend(unsigned long deepidle)
+{
+	return cpu_suspend(deepidle, do_armada_370_xp_cpu_suspend);
+}
+
+/* No locking is needed because we only access per-CPU registers */
+static noinline void armada_370_xp_pmsu_idle_restore(void)
+{
+	unsigned int hw_cpu = cpu_logical_map(smp_processor_id());
+	u32 reg;
+
+	if (pmsu_mp_base == NULL)
+		return;
+
+	/* cancel ask HW to power down the L2 Cache if possible */
+	reg = readl(pmsu_mp_base + PMSU_CONTROL_AND_CONFIG(hw_cpu));
+	reg &= ~PMSU_CONTROL_AND_CONFIG_L2_PWDDN;
+	writel(reg, pmsu_mp_base + PMSU_CONTROL_AND_CONFIG(hw_cpu));
+
+	/* cancel Enable wakeup events and mask interrupts */
+	reg = readl(pmsu_mp_base + PMSU_STATUS_AND_MASK(hw_cpu));
+	reg &= ~(PMSU_STATUS_AND_MASK_IRQ_WAKEUP | PMSU_STATUS_AND_MASK_FIQ_WAKEUP);
+	reg &= ~PMSU_STATUS_AND_MASK_CPU_IDLE_WAIT;
+	reg &= ~PMSU_STATUS_AND_MASK_SNP_Q_EMPTY_WAIT;
+	reg &= ~(PMSU_STATUS_AND_MASK_IRQ_MASK | PMSU_STATUS_AND_MASK_FIQ_MASK);
+	writel(reg, pmsu_mp_base + PMSU_STATUS_AND_MASK(hw_cpu));
+}
+
+static int armada_370_xp_cpu_pm_notify(struct notifier_block *self,
+				    unsigned long action, void *hcpu)
+{
+	if (action == CPU_PM_ENTER) {
+		unsigned int hw_cpu = cpu_logical_map(smp_processor_id());
+		mvebu_pmsu_set_cpu_boot_addr(hw_cpu, armada_370_xp_cpu_resume);
+	} else if (action == CPU_PM_EXIT) {
+		armada_370_xp_pmsu_idle_restore();
+	}
+
+	return NOTIFY_OK;
+}
+
+static struct notifier_block armada_370_xp_cpu_pm_notifier = {
+	.notifier_call = armada_370_xp_cpu_pm_notify,
+};
+
+int __init armada_370_xp_cpu_pm_init(void)
 {
 	struct device_node *np;
 
+	/*
+	 * Check that all the requirements are available to enable
+	 * cpuidle. So far, it is only supported on Armada XP, cpuidle
+	 * needs the coherency fabric and the PMSU enabled
+	 */
+
+	if (!of_machine_is_compatible("marvell,armadaxp"))
+		return 0;
+
+	np = of_find_compatible_node(NULL, NULL, "marvell,coherency-fabric");
+	if (!np)
+		return 0;
+	of_node_put(np);
+
 	np = of_find_matching_node(NULL, of_pmsu_table);
-	if (np) {
-		pr_info("Initializing Power Management Service Unit\n");
-		pmsu_mp_base = of_iomap(np, 0);
-		pmsu_reset_base = of_iomap(np, 1);
-		of_node_put(np);
-	}
+	if (!np)
+		return 0;
+	of_node_put(np);
+
+	armada_370_xp_pmsu_enable_l2_powerdown_onidle();
+	armada_xp_cpuidle_device.dev.platform_data = armada_370_xp_cpu_suspend;
+	platform_device_register(&armada_xp_cpuidle_device);
+	cpu_pm_register_notifier(&armada_370_xp_cpu_pm_notifier);
 
 	return 0;
 }
 
+arch_initcall(armada_370_xp_cpu_pm_init);
 early_initcall(armada_370_xp_pmsu_init);
diff --git a/arch/arm/mach-mvebu/system-controller.c b/arch/arm/mach-mvebu/system-controller.c
index 614ba6832ff..0c5524ac75b 100644
--- a/arch/arm/mach-mvebu/system-controller.c
+++ b/arch/arm/mach-mvebu/system-controller.c
@@ -37,6 +37,8 @@ struct mvebu_system_controller {
 
 	u32 rstoutn_mask_reset_out_en;
 	u32 system_soft_reset;
+
+	u32 resume_boot_addr;
 };
 static struct mvebu_system_controller *mvebu_sc;
 
@@ -52,6 +54,7 @@ static const struct mvebu_system_controller armada_375_system_controller = {
 	.system_soft_reset_offset = 0x58,
 	.rstoutn_mask_reset_out_en = 0x1,
 	.system_soft_reset = 0x1,
+	.resume_boot_addr = 0xd4,
 };
 
 static const struct mvebu_system_controller orion_system_controller = {
@@ -98,6 +101,16 @@ void mvebu_restart(enum reboot_mode mode, const char *cmd)
 		;
 }
 
+#ifdef CONFIG_SMP
+void mvebu_system_controller_set_cpu_boot_addr(void *boot_addr)
+{
+	BUG_ON(system_controller_base == NULL);
+	BUG_ON(mvebu_sc->resume_boot_addr == 0);
+	writel(virt_to_phys(boot_addr), system_controller_base +
+	       mvebu_sc->resume_boot_addr);
+}
+#endif
+
 static int __init mvebu_system_controller_init(void)
 {
 	const struct of_device_id *match;
@@ -114,4 +127,4 @@ static int __init mvebu_system_controller_init(void)
 	return 0;
 }
 
-arch_initcall(mvebu_system_controller_init);
+early_initcall(mvebu_system_controller_init);
diff --git a/arch/arm/mach-omap1/board-h2.c b/arch/arm/mach-omap1/board-h2.c
index 65d2acb3149..5b45d266d83 100644
--- a/arch/arm/mach-omap1/board-h2.c
+++ b/arch/arm/mach-omap1/board-h2.c
@@ -346,7 +346,7 @@ static struct omap_usb_config h2_usb_config __initdata = {
 	/* usb1 has a Mini-AB port and external isp1301 transceiver */
 	.otg		= 2,
 
-#ifdef	CONFIG_USB_GADGET_OMAP
+#if IS_ENABLED(CONFIG_USB_OMAP)
 	.hmc_mode	= 19,	/* 0:host(off) 1:dev|otg 2:disabled */
 	/* .hmc_mode	= 21,*/	/* 0:host(off) 1:dev(loopback) 2:host(loopback) */
 #elif	defined(CONFIG_USB_OHCI_HCD) || defined(CONFIG_USB_OHCI_HCD_MODULE)
diff --git a/arch/arm/mach-omap1/board-h3.c b/arch/arm/mach-omap1/board-h3.c
index 816ecd13f81..bfed4f92866 100644
--- a/arch/arm/mach-omap1/board-h3.c
+++ b/arch/arm/mach-omap1/board-h3.c
@@ -366,7 +366,7 @@ static struct omap_usb_config h3_usb_config __initdata = {
 	/* usb1 has a Mini-AB port and external isp1301 transceiver */
 	.otg	    = 2,
 
-#ifdef CONFIG_USB_GADGET_OMAP
+#if IS_ENABLED(CONFIG_USB_OMAP)
 	.hmc_mode       = 19,   /* 0:host(off) 1:dev|otg 2:disabled */
 #elif  defined(CONFIG_USB_OHCI_HCD) || defined(CONFIG_USB_OHCI_HCD_MODULE)
 	/* NONSTANDARD CABLE NEEDED (B-to-Mini-B) */
diff --git a/arch/arm/mach-omap1/board-innovator.c b/arch/arm/mach-omap1/board-innovator.c
index bd5f02e9c35..c49ce83cc1e 100644
--- a/arch/arm/mach-omap1/board-innovator.c
+++ b/arch/arm/mach-omap1/board-innovator.c
@@ -312,7 +312,7 @@ static struct omap_usb_config h2_usb_config __initdata = {
 	/* usb1 has a Mini-AB port and external isp1301 transceiver */
 	.otg		= 2,
 
-#ifdef	CONFIG_USB_GADGET_OMAP
+#if IS_ENABLED(CONFIG_USB_OMAP)
 	.hmc_mode	= 19,	/* 0:host(off) 1:dev|otg 2:disabled */
 	/* .hmc_mode	= 21,*/	/* 0:host(off) 1:dev(loopback) 2:host(loopback) */
 #elif	defined(CONFIG_USB_OHCI_HCD) || defined(CONFIG_USB_OHCI_HCD_MODULE)
diff --git a/arch/arm/mach-omap1/board-osk.c b/arch/arm/mach-omap1/board-osk.c
index 3a0262156e9..7436d4cf659 100644
--- a/arch/arm/mach-omap1/board-osk.c
+++ b/arch/arm/mach-omap1/board-osk.c
@@ -283,7 +283,7 @@ static struct omap_usb_config osk_usb_config __initdata = {
 	 * be used, with a NONSTANDARD gender-bending cable/dongle, as
 	 * a peripheral.
 	 */
-#ifdef	CONFIG_USB_GADGET_OMAP
+#if IS_ENABLED(CONFIG_USB_OMAP)
 	.register_dev	= 1,
 	.hmc_mode	= 0,
 #else
diff --git a/arch/arm/mach-omap1/board-sx1.c b/arch/arm/mach-omap1/board-sx1.c
index 0a8d3349149..29e526235dc 100644
--- a/arch/arm/mach-omap1/board-sx1.c
+++ b/arch/arm/mach-omap1/board-sx1.c
@@ -266,31 +266,6 @@ static struct physmap_flash_data sx1_flash_data = {
 	.nr_parts	= ARRAY_SIZE(sx1_partitions),
 };
 
-#ifdef CONFIG_SX1_OLD_FLASH
-/* MTD Intel StrataFlash - old flashes */
-static struct resource sx1_old_flash_resource[] = {
-	[0] = {
-		.start	= OMAP_CS0_PHYS,	/* Physical */
-		.end	= OMAP_CS0_PHYS + SZ_16M - 1,,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= OMAP_CS1_PHYS,
-		.end	= OMAP_CS1_PHYS + SZ_8M - 1,
-		.flags	= IORESOURCE_MEM,
-	},
-};
-
-static struct platform_device sx1_flash_device = {
-	.name		= "physmap-flash",
-	.id		= 0,
-	.dev		= {
-		.platform_data	= &sx1_flash_data,
-	},
-	.num_resources	= 2,
-	.resource	= &sx1_old_flash_resource,
-};
-#else
 /* MTD Intel 4000 flash - new flashes */
 static struct resource sx1_new_flash_resource = {
 	.start		= OMAP_CS0_PHYS,
@@ -307,7 +282,6 @@ static struct platform_device sx1_flash_device = {
 	.num_resources	= 1,
 	.resource	= &sx1_new_flash_resource,
 };
-#endif
 
 /*----------- USB -------------------------*/
 
diff --git a/arch/arm/mach-omap1/pm.c b/arch/arm/mach-omap1/pm.c
index dbee729e3b6..34b4c004496 100644
--- a/arch/arm/mach-omap1/pm.c
+++ b/arch/arm/mach-omap1/pm.c
@@ -123,19 +123,8 @@ void omap1_pm_idle(void)
 #warning Enable 32kHz OS timer in order to allow sleep states in idle
 	use_idlect1 = use_idlect1 & ~(1 << 9);
 #else
-
-	while (enable_dyn_sleep) {
-
-#ifdef CONFIG_CBUS_TAHVO_USB
-		extern int vbus_active;
-		/* Clock requirements? */
-		if (vbus_active)
-			break;
-#endif
+	if (enable_dyn_sleep)
 		do_sleep = 1;
-		break;
-	}
-
 #endif
 
 #ifdef CONFIG_OMAP_DM_TIMER
diff --git a/arch/arm/mach-omap2/board-am3517evm.c b/arch/arm/mach-omap2/board-am3517evm.c
index 543d9a882de..4f9383cecf7 100644
--- a/arch/arm/mach-omap2/board-am3517evm.c
+++ b/arch/arm/mach-omap2/board-am3517evm.c
@@ -262,12 +262,7 @@ static struct usbhs_phy_data phy_data[] __initdata = {
 
 static struct usbhs_omap_platform_data usbhs_bdata __initdata = {
 	.port_mode[0] = OMAP_EHCI_PORT_MODE_PHY,
-#if defined(CONFIG_PANEL_SHARP_LQ043T1DG01) || \
-		defined(CONFIG_PANEL_SHARP_LQ043T1DG01_MODULE)
-	.port_mode[1] = OMAP_USBHS_PORT_MODE_UNUSED,
-#else
 	.port_mode[1] = OMAP_EHCI_PORT_MODE_PHY,
-#endif
 };
 
 #ifdef CONFIG_OMAP_MUX
diff --git a/arch/arm/mach-omap2/board-flash.c b/arch/arm/mach-omap2/board-flash.c
index ac82512b9c8..e87f2a83d6b 100644
--- a/arch/arm/mach-omap2/board-flash.c
+++ b/arch/arm/mach-omap2/board-flash.c
@@ -142,7 +142,7 @@ __init board_nand_init(struct mtd_partition *nand_parts, u8 nr_parts, u8 cs,
 	board_nand_data.nr_parts	= nr_parts;
 	board_nand_data.devsize		= nand_type;
 
-	board_nand_data.ecc_opt = OMAP_ECC_BCH8_CODE_HW;
+	board_nand_data.ecc_opt = OMAP_ECC_HAM1_CODE_HW;
 	gpmc_nand_init(&board_nand_data, gpmc_t);
 }
 #endif /* CONFIG_MTD_NAND_OMAP2 || CONFIG_MTD_NAND_OMAP2_MODULE */
@@ -160,13 +160,13 @@ static u8 get_gpmc0_type(void)
 	if (!fpga_map_addr)
 		return -ENOMEM;
 
-	if (!(__raw_readw(fpga_map_addr + REG_FPGA_REV)))
+	if (!(readw_relaxed(fpga_map_addr + REG_FPGA_REV)))
 		/* we dont have an DEBUG FPGA??? */
 		/* Depend on #defines!! default to strata boot return param */
 		goto unmap;
 
 	/* S8-DIP-OFF = 1, S8-DIP-ON = 0 */
-	cs = __raw_readw(fpga_map_addr + REG_FPGA_DIP_SWITCH_INPUT2) & 0xf;
+	cs = readw_relaxed(fpga_map_addr + REG_FPGA_DIP_SWITCH_INPUT2) & 0xf;
 
 	/* ES2.0 SDP's onwards 4 dip switches are provided for CS */
 	if (omap_rev() >= OMAP3430_REV_ES1_0)
diff --git a/arch/arm/mach-omap2/board-generic.c b/arch/arm/mach-omap2/board-generic.c
index b8920b6bc10..9480997ba61 100644
--- a/arch/arm/mach-omap2/board-generic.c
+++ b/arch/arm/mach-omap2/board-generic.c
@@ -43,7 +43,7 @@ static void __init omap_generic_init(void)
 }
 
 #ifdef CONFIG_SOC_OMAP2420
-static const char *omap242x_boards_compat[] __initdata = {
+static const char *omap242x_boards_compat[] __initconst = {
 	"ti,omap2420",
 	NULL,
 };
@@ -62,7 +62,7 @@ MACHINE_END
 #endif
 
 #ifdef CONFIG_SOC_OMAP2430
-static const char *omap243x_boards_compat[] __initdata = {
+static const char *omap243x_boards_compat[] __initconst = {
 	"ti,omap2430",
 	NULL,
 };
@@ -81,7 +81,7 @@ MACHINE_END
 #endif
 
 #ifdef CONFIG_ARCH_OMAP3
-static const char *omap3_boards_compat[] __initdata = {
+static const char *omap3_boards_compat[] __initconst = {
 	"ti,omap3430",
 	"ti,omap3",
 	NULL,
@@ -100,7 +100,7 @@ DT_MACHINE_START(OMAP3_DT, "Generic OMAP3 (Flattened Device Tree)")
 	.restart	= omap3xxx_restart,
 MACHINE_END
 
-static const char *omap36xx_boards_compat[] __initdata = {
+static const char *omap36xx_boards_compat[] __initconst = {
 	"ti,omap36xx",
 	NULL,
 };
@@ -118,7 +118,7 @@ DT_MACHINE_START(OMAP36XX_DT, "Generic OMAP36xx (Flattened Device Tree)")
 	.restart	= omap3xxx_restart,
 MACHINE_END
 
-static const char *omap3_gp_boards_compat[] __initdata = {
+static const char *omap3_gp_boards_compat[] __initconst = {
 	"ti,omap3-beagle",
 	"timll,omap3-devkit8000",
 	NULL,
@@ -137,7 +137,7 @@ DT_MACHINE_START(OMAP3_GP_DT, "Generic OMAP3-GP (Flattened Device Tree)")
 	.restart	= omap3xxx_restart,
 MACHINE_END
 
-static const char *am3517_boards_compat[] __initdata = {
+static const char *am3517_boards_compat[] __initconst = {
 	"ti,am3517",
 	NULL,
 };
@@ -157,7 +157,7 @@ MACHINE_END
 #endif
 
 #ifdef CONFIG_SOC_AM33XX
-static const char *am33xx_boards_compat[] __initdata = {
+static const char *am33xx_boards_compat[] __initconst = {
 	"ti,am33xx",
 	NULL,
 };
@@ -177,7 +177,7 @@ MACHINE_END
 #endif
 
 #ifdef CONFIG_ARCH_OMAP4
-static const char *omap4_boards_compat[] __initdata = {
+static const char *omap4_boards_compat[] __initconst = {
 	"ti,omap4460",
 	"ti,omap4430",
 	"ti,omap4",
@@ -199,7 +199,7 @@ MACHINE_END
 #endif
 
 #ifdef CONFIG_SOC_OMAP5
-static const char *omap5_boards_compat[] __initdata = {
+static const char *omap5_boards_compat[] __initconst = {
 	"ti,omap5432",
 	"ti,omap5430",
 	"ti,omap5",
@@ -221,7 +221,7 @@ MACHINE_END
 #endif
 
 #ifdef CONFIG_SOC_AM43XX
-static const char *am43_boards_compat[] __initdata = {
+static const char *am43_boards_compat[] __initconst = {
 	"ti,am4372",
 	"ti,am43",
 	NULL,
@@ -240,13 +240,13 @@ MACHINE_END
 #endif
 
 #ifdef CONFIG_SOC_DRA7XX
-static const char *dra7xx_boards_compat[] __initdata = {
-	"ti,dra7xx",
+static const char *dra74x_boards_compat[] __initconst = {
+	"ti,dra742",
 	"ti,dra7",
 	NULL,
 };
 
-DT_MACHINE_START(DRA7XX_DT, "Generic DRA7XX (Flattened Device Tree)")
+DT_MACHINE_START(DRA74X_DT, "Generic DRA74X (Flattened Device Tree)")
 	.reserve	= omap_reserve,
 	.smp		= smp_ops(omap4_smp_ops),
 	.map_io		= omap5_map_io,
@@ -255,7 +255,24 @@ DT_MACHINE_START(DRA7XX_DT, "Generic DRA7XX (Flattened Device Tree)")
 	.init_irq	= omap_gic_of_init,
 	.init_machine	= omap_generic_init,
 	.init_time	= omap5_realtime_timer_init,
-	.dt_compat	= dra7xx_boards_compat,
+	.dt_compat	= dra74x_boards_compat,
+	.restart	= omap44xx_restart,
+MACHINE_END
+
+static const char *dra72x_boards_compat[] __initconst = {
+	"ti,dra722",
+	NULL,
+};
+
+DT_MACHINE_START(DRA72X_DT, "Generic DRA72X (Flattened Device Tree)")
+	.reserve	= omap_reserve,
+	.map_io		= omap5_map_io,
+	.init_early	= dra7xx_init_early,
+	.init_late	= dra7xx_init_late,
+	.init_irq	= omap_gic_of_init,
+	.init_machine	= omap_generic_init,
+	.init_time	= omap5_realtime_timer_init,
+	.dt_compat	= dra72x_boards_compat,
 	.restart	= omap44xx_restart,
 MACHINE_END
 #endif
diff --git a/arch/arm/mach-omap2/board-omap3beagle.c b/arch/arm/mach-omap2/board-omap3beagle.c
index d6ed819ff15..660bfc5a70d 100644
--- a/arch/arm/mach-omap2/board-omap3beagle.c
+++ b/arch/arm/mach-omap2/board-omap3beagle.c
@@ -33,7 +33,6 @@
 #include <linux/mtd/nand.h>
 #include <linux/mmc/host.h>
 #include <linux/usb/phy.h>
-#include <linux/usb/usb_phy_gen_xceiv.h>
 
 #include <linux/regulator/machine.h>
 #include <linux/i2c/twl.h>
diff --git a/arch/arm/mach-omap2/board-omap3stalker.c b/arch/arm/mach-omap2/board-omap3stalker.c
index 119efaf5808..a2e035e0792 100644
--- a/arch/arm/mach-omap2/board-omap3stalker.c
+++ b/arch/arm/mach-omap2/board-omap3stalker.c
@@ -121,11 +121,7 @@ static struct platform_device omap3stalker_tfp410_device = {
 static struct connector_atv_platform_data omap3stalker_tv_pdata = {
 	.name = "tv",
 	.source = "venc.0",
-#if defined(CONFIG_OMAP2_VENC_OUT_TYPE_SVIDEO)
-	.connector_type = OMAP_DSS_VENC_TYPE_SVIDEO,
-#elif defined(CONFIG_OMAP2_VENC_OUT_TYPE_COMPOSITE)
 	.connector_type = OMAP_DSS_VENC_TYPE_COMPOSITE,
-#endif
 	.invert_polarity = false,
 };
 
diff --git a/arch/arm/mach-omap2/board-rx51-video.c b/arch/arm/mach-omap2/board-rx51-video.c
index 43a90c8d683..9cfebc5c745 100644
--- a/arch/arm/mach-omap2/board-rx51-video.c
+++ b/arch/arm/mach-omap2/board-rx51-video.c
@@ -48,7 +48,7 @@ static struct omap_dss_board_info rx51_dss_board_info = {
 
 static int __init rx51_video_init(void)
 {
-	if (!machine_is_nokia_rx51() && !of_machine_is_compatible("nokia,omap3-n900"))
+	if (!machine_is_nokia_rx51())
 		return 0;
 
 	if (omap_mux_init_gpio(RX51_LCD_RESET_GPIO, OMAP_PIN_OUTPUT)) {
diff --git a/arch/arm/mach-omap2/cclock3xxx_data.c b/arch/arm/mach-omap2/cclock3xxx_data.c
index 8f5121b8968..eb8c75ec3b1 100644
--- a/arch/arm/mach-omap2/cclock3xxx_data.c
+++ b/arch/arm/mach-omap2/cclock3xxx_data.c
@@ -456,7 +456,8 @@ static struct clk_hw_omap dpll4_m5x2_ck_hw = {
 	.clkdm_name	= "dpll4_clkdm",
 };
 
-DEFINE_STRUCT_CLK(dpll4_m5x2_ck, dpll4_m5x2_ck_parent_names, dpll4_m5x2_ck_ops);
+DEFINE_STRUCT_CLK_FLAGS(dpll4_m5x2_ck, dpll4_m5x2_ck_parent_names,
+			dpll4_m5x2_ck_ops, CLK_SET_RATE_PARENT);
 
 static struct clk dpll4_m5x2_ck_3630 = {
 	.name		= "dpll4_m5x2_ck",
diff --git a/arch/arm/mach-omap2/clkt2xxx_dpllcore.c b/arch/arm/mach-omap2/clkt2xxx_dpllcore.c
index 3ff32543493..59cf310bc1e 100644
--- a/arch/arm/mach-omap2/clkt2xxx_dpllcore.c
+++ b/arch/arm/mach-omap2/clkt2xxx_dpllcore.c
@@ -138,7 +138,7 @@ int omap2_reprogram_dpllcore(struct clk_hw *hw, unsigned long rate,
 		if (!dd)
 			return -EINVAL;
 
-		tmpset.cm_clksel1_pll = __raw_readl(dd->mult_div1_reg);
+		tmpset.cm_clksel1_pll = readl_relaxed(dd->mult_div1_reg);
 		tmpset.cm_clksel1_pll &= ~(dd->mult_mask |
 					   dd->div1_mask);
 		div = ((curr_prcm_set->xtal_speed / 1000000) - 1);
diff --git a/arch/arm/mach-omap2/clkt2xxx_osc.c b/arch/arm/mach-omap2/clkt2xxx_osc.c
index 19f54d43349..0717dff1bc0 100644
--- a/arch/arm/mach-omap2/clkt2xxx_osc.c
+++ b/arch/arm/mach-omap2/clkt2xxx_osc.c
@@ -39,9 +39,9 @@ int omap2_enable_osc_ck(struct clk_hw *clk)
 {
 	u32 pcc;
 
-	pcc = __raw_readl(prcm_clksrc_ctrl);
+	pcc = readl_relaxed(prcm_clksrc_ctrl);
 
-	__raw_writel(pcc & ~OMAP_AUTOEXTCLKMODE_MASK, prcm_clksrc_ctrl);
+	writel_relaxed(pcc & ~OMAP_AUTOEXTCLKMODE_MASK, prcm_clksrc_ctrl);
 
 	return 0;
 }
@@ -57,9 +57,9 @@ void omap2_disable_osc_ck(struct clk_hw *clk)
 {
 	u32 pcc;
 
-	pcc = __raw_readl(prcm_clksrc_ctrl);
+	pcc = readl_relaxed(prcm_clksrc_ctrl);
 
-	__raw_writel(pcc | OMAP_AUTOEXTCLKMODE_MASK, prcm_clksrc_ctrl);
+	writel_relaxed(pcc | OMAP_AUTOEXTCLKMODE_MASK, prcm_clksrc_ctrl);
 }
 
 unsigned long omap2_osc_clk_recalc(struct clk_hw *clk,
diff --git a/arch/arm/mach-omap2/clkt2xxx_sys.c b/arch/arm/mach-omap2/clkt2xxx_sys.c
index f467d072cd0..58dd3a9b726 100644
--- a/arch/arm/mach-omap2/clkt2xxx_sys.c
+++ b/arch/arm/mach-omap2/clkt2xxx_sys.c
@@ -33,7 +33,7 @@ u32 omap2xxx_get_sysclkdiv(void)
 {
 	u32 div;
 
-	div = __raw_readl(prcm_clksrc_ctrl);
+	div = readl_relaxed(prcm_clksrc_ctrl);
 	div &= OMAP_SYSCLKDIV_MASK;
 	div >>= OMAP_SYSCLKDIV_SHIFT;
 
diff --git a/arch/arm/mach-omap2/clkt_dpll.c b/arch/arm/mach-omap2/clkt_dpll.c
index 2649ce44584..332af927f4d 100644
--- a/arch/arm/mach-omap2/clkt_dpll.c
+++ b/arch/arm/mach-omap2/clkt_dpll.c
@@ -209,7 +209,7 @@ u8 omap2_init_dpll_parent(struct clk_hw *hw)
 		if (v == OMAP3XXX_EN_DPLL_LPBYPASS ||
 		    v == OMAP3XXX_EN_DPLL_FRBYPASS)
 			return 1;
-	} else if (soc_is_am33xx() || cpu_is_omap44xx()) {
+	} else if (soc_is_am33xx() || cpu_is_omap44xx() || soc_is_am43xx()) {
 		if (v == OMAP4XXX_EN_DPLL_LPBYPASS ||
 		    v == OMAP4XXX_EN_DPLL_FRBYPASS ||
 		    v == OMAP4XXX_EN_DPLL_MNBYPASS)
@@ -255,7 +255,7 @@ unsigned long omap2_get_dpll_rate(struct clk_hw_omap *clk)
 		if (v == OMAP3XXX_EN_DPLL_LPBYPASS ||
 		    v == OMAP3XXX_EN_DPLL_FRBYPASS)
 			return __clk_get_rate(dd->clk_bypass);
-	} else if (soc_is_am33xx() || cpu_is_omap44xx()) {
+	} else if (soc_is_am33xx() || cpu_is_omap44xx() || soc_is_am43xx()) {
 		if (v == OMAP4XXX_EN_DPLL_LPBYPASS ||
 		    v == OMAP4XXX_EN_DPLL_FRBYPASS ||
 		    v == OMAP4XXX_EN_DPLL_MNBYPASS)
diff --git a/arch/arm/mach-omap2/clockdomain.h b/arch/arm/mach-omap2/clockdomain.h
index f17f00697cc..82c37b1becc 100644
--- a/arch/arm/mach-omap2/clockdomain.h
+++ b/arch/arm/mach-omap2/clockdomain.h
@@ -18,7 +18,6 @@
 
 #include "powerdomain.h"
 #include "clock.h"
-#include "omap_hwmod.h"
 
 /*
  * Clockdomain flags
@@ -98,6 +97,8 @@ struct clkdm_dep {
 /* Possible flags for struct clockdomain._flags */
 #define _CLKDM_FLAG_HWSUP_ENABLED		BIT(0)
 
+struct omap_hwmod;
+
 /**
  * struct clockdomain - OMAP clockdomain
  * @name: clockdomain name
diff --git a/arch/arm/mach-omap2/cm2xxx.c b/arch/arm/mach-omap2/cm2xxx.c
index ce25abbcffa..8be6ea50c09 100644
--- a/arch/arm/mach-omap2/cm2xxx.c
+++ b/arch/arm/mach-omap2/cm2xxx.c
@@ -18,9 +18,6 @@
 #include <linux/err.h>
 #include <linux/io.h>
 
-#include "soc.h"
-#include "iomap.h"
-#include "common.h"
 #include "prm2xxx.h"
 #include "cm.h"
 #include "cm2xxx.h"
@@ -390,7 +387,7 @@ void omap2xxx_cm_set_mod_dividers(u32 mpu, u32 dsp, u32 gfx, u32 core, u32 mdm)
 	tmp = omap2_cm_read_mod_reg(CORE_MOD, CM_CLKSEL1) &
 		OMAP24XX_CLKSEL_DSS2_MASK;
 	omap2_cm_write_mod_reg(core | tmp, CORE_MOD, CM_CLKSEL1);
-	if (cpu_is_omap2430())
+	if (mdm)
 		omap2_cm_write_mod_reg(mdm, OMAP2430_MDM_MOD, CM_CLKSEL);
 }
 
@@ -405,19 +402,11 @@ static struct cm_ll_data omap2xxx_cm_ll_data = {
 
 int __init omap2xxx_cm_init(void)
 {
-	if (!cpu_is_omap24xx())
-		return 0;
-
 	return cm_register(&omap2xxx_cm_ll_data);
 }
 
 static void __exit omap2xxx_cm_exit(void)
 {
-	if (!cpu_is_omap24xx())
-		return;
-
-	/* Should never happen */
-	WARN(cm_unregister(&omap2xxx_cm_ll_data),
-	     "%s: cm_ll_data function pointer mismatch\n", __func__);
+	cm_unregister(&omap2xxx_cm_ll_data);
 }
 __exitcall(omap2xxx_cm_exit);
diff --git a/arch/arm/mach-omap2/cm2xxx_3xxx.h b/arch/arm/mach-omap2/cm2xxx_3xxx.h
index bfbd16fe915..72928a3ce2a 100644
--- a/arch/arm/mach-omap2/cm2xxx_3xxx.h
+++ b/arch/arm/mach-omap2/cm2xxx_3xxx.h
@@ -52,12 +52,12 @@
 
 static inline u32 omap2_cm_read_mod_reg(s16 module, u16 idx)
 {
-	return __raw_readl(cm_base + module + idx);
+	return readl_relaxed(cm_base + module + idx);
 }
 
 static inline void omap2_cm_write_mod_reg(u32 val, s16 module, u16 idx)
 {
-	__raw_writel(val, cm_base + module + idx);
+	writel_relaxed(val, cm_base + module + idx);
 }
 
 /* Read-modify-write a register in a CM module. Caller must lock */
diff --git a/arch/arm/mach-omap2/cm33xx.c b/arch/arm/mach-omap2/cm33xx.c
index 40a22e5649a..b3f99e93def 100644
--- a/arch/arm/mach-omap2/cm33xx.c
+++ b/arch/arm/mach-omap2/cm33xx.c
@@ -50,13 +50,13 @@
 /* Read a register in a CM instance */
 static inline u32 am33xx_cm_read_reg(u16 inst, u16 idx)
 {
-	return __raw_readl(cm_base + inst + idx);
+	return readl_relaxed(cm_base + inst + idx);
 }
 
 /* Write into a register in a CM */
 static inline void am33xx_cm_write_reg(u32 val, u16 inst, u16 idx)
 {
-	__raw_writel(val, cm_base + inst + idx);
+	writel_relaxed(val, cm_base + inst + idx);
 }
 
 /* Read-modify-write a register in CM */
diff --git a/arch/arm/mach-omap2/cm33xx.h b/arch/arm/mach-omap2/cm33xx.h
index cfb8891b0c0..15a778ce770 100644
--- a/arch/arm/mach-omap2/cm33xx.h
+++ b/arch/arm/mach-omap2/cm33xx.h
@@ -17,11 +17,8 @@
 #ifndef __ARCH_ARM_MACH_OMAP2_CM_33XX_H
 #define __ARCH_ARM_MACH_OMAP2_CM_33XX_H
 
-#include "common.h"
-
 #include "cm.h"
 #include "cm-regbits-33xx.h"
-#include "iomap.h"
 
 /* CM base address */
 #define AM33XX_CM_BASE		0x44e00000
diff --git a/arch/arm/mach-omap2/cm3xxx.c b/arch/arm/mach-omap2/cm3xxx.c
index f6f028867bf..129a4e7f6ef 100644
--- a/arch/arm/mach-omap2/cm3xxx.c
+++ b/arch/arm/mach-omap2/cm3xxx.c
@@ -18,9 +18,6 @@
 #include <linux/err.h>
 #include <linux/io.h>
 
-#include "soc.h"
-#include "iomap.h"
-#include "common.h"
 #include "prm2xxx_3xxx.h"
 #include "cm.h"
 #include "cm3xxx.h"
@@ -388,7 +385,8 @@ void omap3_cm_save_context(void)
 		omap2_cm_read_mod_reg(OMAP3430_IVA2_MOD, CM_CLKSEL1);
 	cm_context.iva2_cm_clksel2 =
 		omap2_cm_read_mod_reg(OMAP3430_IVA2_MOD, CM_CLKSEL2);
-	cm_context.cm_sysconfig = __raw_readl(OMAP3430_CM_SYSCONFIG);
+	cm_context.cm_sysconfig =
+		omap2_cm_read_mod_reg(OCP_MOD, OMAP3430_CM_SYSCONFIG);
 	cm_context.sgx_cm_clksel =
 		omap2_cm_read_mod_reg(OMAP3430ES2_SGX_MOD, CM_CLKSEL);
 	cm_context.dss_cm_clksel =
@@ -418,7 +416,8 @@ void omap3_cm_save_context(void)
 		omap2_cm_read_mod_reg(PLL_MOD, OMAP3430ES2_CM_CLKSEL5);
 	cm_context.pll_cm_clken2 =
 		omap2_cm_read_mod_reg(PLL_MOD, OMAP3430ES2_CM_CLKEN2);
-	cm_context.cm_polctrl = __raw_readl(OMAP3430_CM_POLCTRL);
+	cm_context.cm_polctrl =
+		omap2_cm_read_mod_reg(OCP_MOD, OMAP3430_CM_POLCTRL);
 	cm_context.iva2_cm_fclken =
 		omap2_cm_read_mod_reg(OMAP3430_IVA2_MOD, CM_FCLKEN);
 	cm_context.iva2_cm_clken_pll =
@@ -519,7 +518,8 @@ void omap3_cm_restore_context(void)
 			       CM_CLKSEL1);
 	omap2_cm_write_mod_reg(cm_context.iva2_cm_clksel2, OMAP3430_IVA2_MOD,
 			       CM_CLKSEL2);
-	__raw_writel(cm_context.cm_sysconfig, OMAP3430_CM_SYSCONFIG);
+	omap2_cm_write_mod_reg(cm_context.cm_sysconfig, OCP_MOD,
+			       OMAP3430_CM_SYSCONFIG);
 	omap2_cm_write_mod_reg(cm_context.sgx_cm_clksel, OMAP3430ES2_SGX_MOD,
 			       CM_CLKSEL);
 	omap2_cm_write_mod_reg(cm_context.dss_cm_clksel, OMAP3430_DSS_MOD,
@@ -547,7 +547,8 @@ void omap3_cm_restore_context(void)
 			       OMAP3430ES2_CM_CLKSEL5);
 	omap2_cm_write_mod_reg(cm_context.pll_cm_clken2, PLL_MOD,
 			       OMAP3430ES2_CM_CLKEN2);
-	__raw_writel(cm_context.cm_polctrl, OMAP3430_CM_POLCTRL);
+	omap2_cm_write_mod_reg(cm_context.cm_polctrl, OCP_MOD,
+			       OMAP3430_CM_POLCTRL);
 	omap2_cm_write_mod_reg(cm_context.iva2_cm_fclken, OMAP3430_IVA2_MOD,
 			       CM_FCLKEN);
 	omap2_cm_write_mod_reg(cm_context.iva2_cm_clken_pll, OMAP3430_IVA2_MOD,
@@ -669,19 +670,11 @@ static struct cm_ll_data omap3xxx_cm_ll_data = {
 
 int __init omap3xxx_cm_init(void)
 {
-	if (!cpu_is_omap34xx())
-		return 0;
-
 	return cm_register(&omap3xxx_cm_ll_data);
 }
 
 static void __exit omap3xxx_cm_exit(void)
 {
-	if (!cpu_is_omap34xx())
-		return;
-
-	/* Should never happen */
-	WARN(cm_unregister(&omap3xxx_cm_ll_data),
-	     "%s: cm_ll_data function pointer mismatch\n", __func__);
+	cm_unregister(&omap3xxx_cm_ll_data);
 }
 __exitcall(omap3xxx_cm_exit);
diff --git a/arch/arm/mach-omap2/cm3xxx.h b/arch/arm/mach-omap2/cm3xxx.h
index 8224c91b4d7..7a16b559812 100644
--- a/arch/arm/mach-omap2/cm3xxx.h
+++ b/arch/arm/mach-omap2/cm3xxx.h
@@ -29,9 +29,8 @@
  * These registers appear once per CM module.
  */
 
-#define OMAP3430_CM_REVISION		OMAP34XX_CM_REGADDR(OCP_MOD, 0x0000)
-#define OMAP3430_CM_SYSCONFIG		OMAP34XX_CM_REGADDR(OCP_MOD, 0x0010)
-#define OMAP3430_CM_POLCTRL		OMAP34XX_CM_REGADDR(OCP_MOD, 0x009c)
+#define OMAP3430_CM_SYSCONFIG		0x0010
+#define OMAP3430_CM_POLCTRL		0x009c
 
 #define OMAP3_CM_CLKOUT_CTRL_OFFSET	0x0070
 #define OMAP3430_CM_CLKOUT_CTRL		OMAP_CM_REGADDR(OMAP3430_CCR_MOD, 0x0070)
diff --git a/arch/arm/mach-omap2/cm44xx.c b/arch/arm/mach-omap2/cm44xx.c
index 535d66e2822..fe5cc7bae48 100644
--- a/arch/arm/mach-omap2/cm44xx.c
+++ b/arch/arm/mach-omap2/cm44xx.c
@@ -18,35 +18,32 @@
 #include <linux/err.h>
 #include <linux/io.h>
 
-#include "iomap.h"
-#include "common.h"
 #include "cm.h"
 #include "cm1_44xx.h"
 #include "cm2_44xx.h"
-#include "cm-regbits-44xx.h"
 
 /* CM1 hardware module low-level functions */
 
 /* Read a register in CM1 */
 u32 omap4_cm1_read_inst_reg(s16 inst, u16 reg)
 {
-	return __raw_readl(OMAP44XX_CM1_REGADDR(inst, reg));
+	return readl_relaxed(cm_base + inst + reg);
 }
 
 /* Write into a register in CM1 */
 void omap4_cm1_write_inst_reg(u32 val, s16 inst, u16 reg)
 {
-	__raw_writel(val, OMAP44XX_CM1_REGADDR(inst, reg));
+	writel_relaxed(val, cm_base + inst + reg);
 }
 
 /* Read a register in CM2 */
 u32 omap4_cm2_read_inst_reg(s16 inst, u16 reg)
 {
-	return __raw_readl(OMAP44XX_CM2_REGADDR(inst, reg));
+	return readl_relaxed(cm2_base + inst + reg);
 }
 
 /* Write into a register in CM2 */
 void omap4_cm2_write_inst_reg(u32 val, s16 inst, u16 reg)
 {
-	__raw_writel(val, OMAP44XX_CM2_REGADDR(inst, reg));
+	writel_relaxed(val, cm2_base + inst + reg);
 }
diff --git a/arch/arm/mach-omap2/cm_common.c b/arch/arm/mach-omap2/cm_common.c
index 40b3b5a8445..8f6c4710877 100644
--- a/arch/arm/mach-omap2/cm_common.c
+++ b/arch/arm/mach-omap2/cm_common.c
@@ -14,11 +14,11 @@
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/errno.h>
+#include <linux/bug.h>
 
 #include "cm2xxx.h"
 #include "cm3xxx.h"
 #include "cm44xx.h"
-#include "common.h"
 
 /*
  * cm_ll_data: function pointers to SoC-specific implementations of
diff --git a/arch/arm/mach-omap2/cminst44xx.c b/arch/arm/mach-omap2/cminst44xx.c
index f5c4731b6f0..12aca56942c 100644
--- a/arch/arm/mach-omap2/cminst44xx.c
+++ b/arch/arm/mach-omap2/cminst44xx.c
@@ -21,8 +21,6 @@
 #include <linux/err.h>
 #include <linux/io.h>
 
-#include "iomap.h"
-#include "common.h"
 #include "clockdomain.h"
 #include "cm.h"
 #include "cm1_44xx.h"
@@ -30,12 +28,18 @@
 #include "cm44xx.h"
 #include "cminst44xx.h"
 #include "cm-regbits-34xx.h"
-#include "cm-regbits-44xx.h"
 #include "prcm44xx.h"
 #include "prm44xx.h"
 #include "prcm_mpu44xx.h"
 #include "prcm-common.h"
 
+#define OMAP4430_IDLEST_SHIFT		16
+#define OMAP4430_IDLEST_MASK		(0x3 << 16)
+#define OMAP4430_CLKTRCTRL_SHIFT	0
+#define OMAP4430_CLKTRCTRL_MASK		(0x3 << 0)
+#define OMAP4430_MODULEMODE_SHIFT	0
+#define OMAP4430_MODULEMODE_MASK	(0x3 << 0)
+
 /*
  * CLKCTRL_IDLEST_*: possible values for the CM_*_CLKCTRL.IDLEST bitfield:
  *
@@ -116,7 +120,7 @@ u32 omap4_cminst_read_inst_reg(u8 part, u16 inst, u16 idx)
 	BUG_ON(part >= OMAP4_MAX_PRCM_PARTITIONS ||
 	       part == OMAP4430_INVALID_PRCM_PARTITION ||
 	       !_cm_bases[part]);
-	return __raw_readl(_cm_bases[part] + inst + idx);
+	return readl_relaxed(_cm_bases[part] + inst + idx);
 }
 
 /* Write into a register in a CM instance */
@@ -125,7 +129,7 @@ void omap4_cminst_write_inst_reg(u32 val, u8 part, u16 inst, u16 idx)
 	BUG_ON(part >= OMAP4_MAX_PRCM_PARTITIONS ||
 	       part == OMAP4430_INVALID_PRCM_PARTITION ||
 	       !_cm_bases[part]);
-	__raw_writel(val, _cm_bases[part] + inst + idx);
+	writel_relaxed(val, _cm_bases[part] + inst + idx);
 }
 
 /* Read-modify-write a register in CM1. Caller must lock */
diff --git a/arch/arm/mach-omap2/control.c b/arch/arm/mach-omap2/control.c
index 44bb4d544dc..751f3549bf6 100644
--- a/arch/arm/mach-omap2/control.c
+++ b/arch/arm/mach-omap2/control.c
@@ -151,32 +151,32 @@ void __iomem *omap_ctrl_base_get(void)
 
 u8 omap_ctrl_readb(u16 offset)
 {
-	return __raw_readb(OMAP_CTRL_REGADDR(offset));
+	return readb_relaxed(OMAP_CTRL_REGADDR(offset));
 }
 
 u16 omap_ctrl_readw(u16 offset)
 {
-	return __raw_readw(OMAP_CTRL_REGADDR(offset));
+	return readw_relaxed(OMAP_CTRL_REGADDR(offset));
 }
 
 u32 omap_ctrl_readl(u16 offset)
 {
-	return __raw_readl(OMAP_CTRL_REGADDR(offset));
+	return readl_relaxed(OMAP_CTRL_REGADDR(offset));
 }
 
 void omap_ctrl_writeb(u8 val, u16 offset)
 {
-	__raw_writeb(val, OMAP_CTRL_REGADDR(offset));
+	writeb_relaxed(val, OMAP_CTRL_REGADDR(offset));
 }
 
 void omap_ctrl_writew(u16 val, u16 offset)
 {
-	__raw_writew(val, OMAP_CTRL_REGADDR(offset));
+	writew_relaxed(val, OMAP_CTRL_REGADDR(offset));
 }
 
 void omap_ctrl_writel(u32 val, u16 offset)
 {
-	__raw_writel(val, OMAP_CTRL_REGADDR(offset));
+	writel_relaxed(val, OMAP_CTRL_REGADDR(offset));
 }
 
 /*
@@ -188,12 +188,12 @@ void omap_ctrl_writel(u32 val, u16 offset)
 
 u32 omap4_ctrl_pad_readl(u16 offset)
 {
-	return __raw_readl(OMAP4_CTRL_PAD_REGADDR(offset));
+	return readl_relaxed(OMAP4_CTRL_PAD_REGADDR(offset));
 }
 
 void omap4_ctrl_pad_writel(u32 val, u16 offset)
 {
-	__raw_writel(val, OMAP4_CTRL_PAD_REGADDR(offset));
+	writel_relaxed(val, OMAP4_CTRL_PAD_REGADDR(offset));
 }
 
 #ifdef CONFIG_ARCH_OMAP3
@@ -222,7 +222,7 @@ void omap3_ctrl_write_boot_mode(u8 bootmode)
 	 *
 	 * XXX This should use some omap_ctrl_writel()-type function
 	 */
-	__raw_writel(l, OMAP2_L4_IO_ADDRESS(OMAP343X_SCRATCHPAD + 4));
+	writel_relaxed(l, OMAP2_L4_IO_ADDRESS(OMAP343X_SCRATCHPAD + 4));
 }
 
 #endif
@@ -285,7 +285,7 @@ void omap3_clear_scratchpad_contents(void)
 	if (omap2_prm_read_mod_reg(OMAP3430_GR_MOD, OMAP3_PRM_RSTST_OFFSET) &
 	    OMAP3430_GLOBAL_COLD_RST_MASK) {
 		for ( ; offset <= max_offset; offset += 0x4)
-			__raw_writel(0x0, (v_addr + offset));
+			writel_relaxed(0x0, (v_addr + offset));
 		omap2_prm_set_mod_reg_bits(OMAP3430_GLOBAL_COLD_RST_MASK,
 					   OMAP3430_GR_MOD,
 					   OMAP3_PRM_RSTST_OFFSET);
diff --git a/arch/arm/mach-omap2/cpuidle44xx.c b/arch/arm/mach-omap2/cpuidle44xx.c
index 01fc710c818..2498ab025fa 100644
--- a/arch/arm/mach-omap2/cpuidle44xx.c
+++ b/arch/arm/mach-omap2/cpuidle44xx.c
@@ -14,6 +14,7 @@
 #include <linux/cpuidle.h>
 #include <linux/cpu_pm.h>
 #include <linux/export.h>
+#include <linux/clockchips.h>
 
 #include <asm/cpuidle.h>
 #include <asm/proc-fns.h>
@@ -83,6 +84,7 @@ static int omap_enter_idle_coupled(struct cpuidle_device *dev,
 {
 	struct idle_statedata *cx = state_ptr + index;
 	u32 mpuss_can_lose_context = 0;
+	int cpu_id = smp_processor_id();
 
 	/*
 	 * CPU0 has to wait and stay ON until CPU1 is OFF state.
@@ -110,6 +112,8 @@ static int omap_enter_idle_coupled(struct cpuidle_device *dev,
 	mpuss_can_lose_context = (cx->mpu_state == PWRDM_POWER_RET) &&
 				 (cx->mpu_logic_state == PWRDM_POWER_OFF);
 
+	clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_ENTER, &cpu_id);
+
 	/*
 	 * Call idle CPU PM enter notifier chain so that
 	 * VFP and per CPU interrupt context is saved.
@@ -165,6 +169,8 @@ static int omap_enter_idle_coupled(struct cpuidle_device *dev,
 	if (dev->cpu == 0 && mpuss_can_lose_context)
 		cpu_cluster_pm_exit();
 
+	clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_EXIT, &cpu_id);
+
 fail:
 	cpuidle_coupled_parallel_barrier(dev, &abort_barrier);
 	cpu_done[dev->cpu] = false;
@@ -172,6 +178,16 @@ fail:
 	return index;
 }
 
+/*
+ * For each cpu, setup the broadcast timer because local timers
+ * stops for the states above C1.
+ */
+static void omap_setup_broadcast_timer(void *arg)
+{
+	int cpu = smp_processor_id();
+	clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_ON, &cpu);
+}
+
 static struct cpuidle_driver omap4_idle_driver = {
 	.name				= "omap4_idle",
 	.owner				= THIS_MODULE,
@@ -189,8 +205,7 @@ static struct cpuidle_driver omap4_idle_driver = {
 			/* C2 - CPU0 OFF + CPU1 OFF + MPU CSWR */
 			.exit_latency = 328 + 440,
 			.target_residency = 960,
-			.flags = CPUIDLE_FLAG_TIME_VALID | CPUIDLE_FLAG_COUPLED |
-			         CPUIDLE_FLAG_TIMER_STOP,
+			.flags = CPUIDLE_FLAG_TIME_VALID | CPUIDLE_FLAG_COUPLED,
 			.enter = omap_enter_idle_coupled,
 			.name = "C2",
 			.desc = "CPUx OFF, MPUSS CSWR",
@@ -199,8 +214,7 @@ static struct cpuidle_driver omap4_idle_driver = {
 			/* C3 - CPU0 OFF + CPU1 OFF + MPU OSWR */
 			.exit_latency = 460 + 518,
 			.target_residency = 1100,
-			.flags = CPUIDLE_FLAG_TIME_VALID | CPUIDLE_FLAG_COUPLED |
-			         CPUIDLE_FLAG_TIMER_STOP,
+			.flags = CPUIDLE_FLAG_TIME_VALID | CPUIDLE_FLAG_COUPLED,
 			.enter = omap_enter_idle_coupled,
 			.name = "C3",
 			.desc = "CPUx OFF, MPUSS OSWR",
@@ -231,5 +245,8 @@ int __init omap4_idle_init(void)
 	if (!cpu_clkdm[0] || !cpu_clkdm[1])
 		return -ENODEV;
 
+	/* Configure the broadcast timer on each cpu */
+	on_each_cpu(omap_setup_broadcast_timer, NULL, 1);
+
 	return cpuidle_register(&omap4_idle_driver, cpu_online_mask);
 }
diff --git a/arch/arm/mach-omap2/dma.c b/arch/arm/mach-omap2/dma.c
index 5689c88d986..a6d2cf1f8d0 100644
--- a/arch/arm/mach-omap2/dma.c
+++ b/arch/arm/mach-omap2/dma.c
@@ -91,7 +91,7 @@ static inline void dma_write(u32 val, int reg, int lch)
 	addr += reg_map[reg].offset;
 	addr += reg_map[reg].stride * lch;
 
-	__raw_writel(val, addr);
+	writel_relaxed(val, addr);
 }
 
 static inline u32 dma_read(int reg, int lch)
@@ -101,7 +101,7 @@ static inline u32 dma_read(int reg, int lch)
 	addr += reg_map[reg].offset;
 	addr += reg_map[reg].stride * lch;
 
-	return __raw_readl(addr);
+	return readl_relaxed(addr);
 }
 
 static void omap2_clear_dma(int lch)
diff --git a/arch/arm/mach-omap2/gpmc-nand.c b/arch/arm/mach-omap2/gpmc-nand.c
index 4349e82debf..17cd39360af 100644
--- a/arch/arm/mach-omap2/gpmc-nand.c
+++ b/arch/arm/mach-omap2/gpmc-nand.c
@@ -46,7 +46,7 @@ static struct platform_device gpmc_nand_device = {
 static bool gpmc_hwecc_bch_capable(enum omap_ecc ecc_opt)
 {
 	/* platforms which support all ECC schemes */
-	if (soc_is_am33xx() || cpu_is_omap44xx() ||
+	if (soc_is_am33xx() || soc_is_am43xx() || cpu_is_omap44xx() ||
 		 soc_is_omap54xx() || soc_is_dra7xx())
 		return 1;
 
diff --git a/arch/arm/mach-omap2/gpmc.c b/arch/arm/mach-omap2/gpmc.c
index ab43755364f..852b19a367f 100644
--- a/arch/arm/mach-omap2/gpmc.c
+++ b/arch/arm/mach-omap2/gpmc.c
@@ -170,12 +170,12 @@ static irqreturn_t gpmc_handle_irq(int irq, void *dev);
 
 static void gpmc_write_reg(int idx, u32 val)
 {
-	__raw_writel(val, gpmc_base + idx);
+	writel_relaxed(val, gpmc_base + idx);
 }
 
 static u32 gpmc_read_reg(int idx)
 {
-	return __raw_readl(gpmc_base + idx);
+	return readl_relaxed(gpmc_base + idx);
 }
 
 void gpmc_cs_write_reg(int cs, int idx, u32 val)
@@ -183,7 +183,7 @@ void gpmc_cs_write_reg(int cs, int idx, u32 val)
 	void __iomem *reg_addr;
 
 	reg_addr = gpmc_base + GPMC_CS0_OFFSET + (cs * GPMC_CS_SIZE) + idx;
-	__raw_writel(val, reg_addr);
+	writel_relaxed(val, reg_addr);
 }
 
 static u32 gpmc_cs_read_reg(int cs, int idx)
@@ -191,7 +191,7 @@ static u32 gpmc_cs_read_reg(int cs, int idx)
 	void __iomem *reg_addr;
 
 	reg_addr = gpmc_base + GPMC_CS0_OFFSET + (cs * GPMC_CS_SIZE) + idx;
-	return __raw_readl(reg_addr);
+	return readl_relaxed(reg_addr);
 }
 
 /* TODO: Add support for gpmc_fck to clock framework and use it */
@@ -501,7 +501,7 @@ static int gpmc_cs_delete_mem(int cs)
 	int r;
 
 	spin_lock(&gpmc_mem_lock);
-	r = release_resource(&gpmc_cs_mem[cs]);
+	r = release_resource(res);
 	res->start = 0;
 	res->end = 0;
 	spin_unlock(&gpmc_mem_lock);
@@ -527,6 +527,14 @@ static int gpmc_cs_remap(int cs, u32 base)
 		pr_err("%s: requested chip-select is disabled\n", __func__);
 		return -ENODEV;
 	}
+
+	/*
+	 * Make sure we ignore any device offsets from the GPMC partition
+	 * allocated for the chip select and that the new base confirms
+	 * to the GPMC 16MB minimum granularity.
+	 */ 
+	base &= ~(SZ_16M - 1);
+
 	gpmc_cs_get_memconf(cs, &old_base, &size);
 	if (base == old_base)
 		return 0;
@@ -586,6 +594,8 @@ EXPORT_SYMBOL(gpmc_cs_request);
 
 void gpmc_cs_free(int cs)
 {
+	struct resource	*res = &gpmc_cs_mem[cs];
+
 	spin_lock(&gpmc_mem_lock);
 	if (cs >= gpmc_cs_num || cs < 0 || !gpmc_cs_reserved(cs)) {
 		printk(KERN_ERR "Trying to free non-reserved GPMC CS%d\n", cs);
@@ -594,7 +604,8 @@ void gpmc_cs_free(int cs)
 		return;
 	}
 	gpmc_cs_disable_mem(cs);
-	release_resource(&gpmc_cs_mem[cs]);
+	if (res->flags)
+		release_resource(res);
 	gpmc_cs_set_reserved(cs, 0);
 	spin_unlock(&gpmc_mem_lock);
 }
diff --git a/arch/arm/mach-omap2/hdq1w.c b/arch/arm/mach-omap2/hdq1w.c
index cbc8e3c480e..f78b4a16195 100644
--- a/arch/arm/mach-omap2/hdq1w.c
+++ b/arch/arm/mach-omap2/hdq1w.c
@@ -76,6 +76,7 @@ int omap_hdq1w_reset(struct omap_hwmod *oh)
 	return 0;
 }
 
+#ifndef CONFIG_OF
 static int __init omap_init_hdq(void)
 {
 	int id = -1;
@@ -95,3 +96,4 @@ static int __init omap_init_hdq(void)
 	return 0;
 }
 omap_arch_initcall(omap_init_hdq);
+#endif
diff --git a/arch/arm/mach-omap2/id.c b/arch/arm/mach-omap2/id.c
index 157412e4273..43969da5d50 100644
--- a/arch/arm/mach-omap2/id.c
+++ b/arch/arm/mach-omap2/id.c
@@ -94,7 +94,7 @@ EXPORT_SYMBOL(omap_type);
 #define OMAP_TAP_DIE_ID_44XX_2	0x020c
 #define OMAP_TAP_DIE_ID_44XX_3	0x0210
 
-#define read_tap_reg(reg)	__raw_readl(tap_base  + (reg))
+#define read_tap_reg(reg)	readl_relaxed(tap_base  + (reg))
 
 struct omap_id {
 	u16	hawkeye;	/* Silicon type (Hawkeye id) */
@@ -628,6 +628,41 @@ void __init omap5xxx_check_revision(void)
 	pr_info("%s %s\n", soc_name, soc_rev);
 }
 
+void __init dra7xxx_check_revision(void)
+{
+	u32 idcode;
+	u16 hawkeye;
+	u8 rev;
+
+	idcode = read_tap_reg(OMAP_TAP_IDCODE);
+	hawkeye = (idcode >> 12) & 0xffff;
+	rev = (idcode >> 28) & 0xff;
+	switch (hawkeye) {
+	case 0xb990:
+		switch (rev) {
+		case 0:
+			omap_revision = DRA752_REV_ES1_0;
+			break;
+		case 1:
+		default:
+			omap_revision = DRA752_REV_ES1_1;
+		}
+		break;
+
+	default:
+		/* Unknown default to latest silicon rev as default*/
+		pr_warn("%s: unknown idcode=0x%08x (hawkeye=0x%08x,rev=0x%d)\n",
+			__func__, idcode, hawkeye, rev);
+		omap_revision = DRA752_REV_ES1_1;
+	}
+
+	sprintf(soc_name, "DRA%03x", omap_rev() >> 16);
+	sprintf(soc_rev, "ES%d.%d", (omap_rev() >> 12) & 0xf,
+		(omap_rev() >> 8) & 0xf);
+
+	pr_info("%s %s\n", soc_name, soc_rev);
+}
+
 /*
  * Set up things for map_io and processor detection later on. Gets called
  * pretty much first thing from board init. For multi-omap, this gets
@@ -669,6 +704,8 @@ static const char * __init omap_get_family(void)
 		return kasprintf(GFP_KERNEL, "OMAP5");
 	else if (soc_is_am43xx())
 		return kasprintf(GFP_KERNEL, "AM43xx");
+	else if (soc_is_dra7xx())
+		return kasprintf(GFP_KERNEL, "DRA7");
 	else
 		return kasprintf(GFP_KERNEL, "Unknown");
 }
diff --git a/arch/arm/mach-omap2/io.c b/arch/arm/mach-omap2/io.c
index f14f9ac2dca..4ec3b4a9384 100644
--- a/arch/arm/mach-omap2/io.c
+++ b/arch/arm/mach-omap2/io.c
@@ -693,6 +693,7 @@ void __init dra7xx_init_early(void)
 	omap_prm_base_init();
 	omap_cm_base_init();
 	omap44xx_prm_init();
+	dra7xxx_check_revision();
 	dra7xx_powerdomains_init();
 	dra7xx_clockdomains_init();
 	dra7xx_hwmod_init();
diff --git a/arch/arm/mach-omap2/irq.c b/arch/arm/mach-omap2/irq.c
index 6037a9a01ed..35b8590c322 100644
--- a/arch/arm/mach-omap2/irq.c
+++ b/arch/arm/mach-omap2/irq.c
@@ -83,12 +83,12 @@ struct omap3_intc_regs {
 
 static void intc_bank_write_reg(u32 val, struct omap_irq_bank *bank, u16 reg)
 {
-	__raw_writel(val, bank->base_reg + reg);
+	writel_relaxed(val, bank->base_reg + reg);
 }
 
 static u32 intc_bank_read_reg(struct omap_irq_bank *bank, u16 reg)
 {
-	return __raw_readl(bank->base_reg + reg);
+	return readl_relaxed(bank->base_reg + reg);
 }
 
 /* XXX: FIQ and additional INTC support (only MPU at the moment) */
diff --git a/arch/arm/mach-omap2/mux.c b/arch/arm/mach-omap2/mux.c
index 48094b58c88..fd88edeb027 100644
--- a/arch/arm/mach-omap2/mux.c
+++ b/arch/arm/mach-omap2/mux.c
@@ -70,18 +70,18 @@ struct omap_mux_partition *omap_mux_get(const char *name)
 u16 omap_mux_read(struct omap_mux_partition *partition, u16 reg)
 {
 	if (partition->flags & OMAP_MUX_REG_8BIT)
-		return __raw_readb(partition->base + reg);
+		return readb_relaxed(partition->base + reg);
 	else
-		return __raw_readw(partition->base + reg);
+		return readw_relaxed(partition->base + reg);
 }
 
 void omap_mux_write(struct omap_mux_partition *partition, u16 val,
 			   u16 reg)
 {
 	if (partition->flags & OMAP_MUX_REG_8BIT)
-		__raw_writeb(val, partition->base + reg);
+		writeb_relaxed(val, partition->base + reg);
 	else
-		__raw_writew(val, partition->base + reg);
+		writew_relaxed(val, partition->base + reg);
 }
 
 void omap_mux_write_array(struct omap_mux_partition *partition,
diff --git a/arch/arm/mach-omap2/omap-headsmp.S b/arch/arm/mach-omap2/omap-headsmp.S
index 75e92952c18..4993d4bfe9b 100644
--- a/arch/arm/mach-omap2/omap-headsmp.S
+++ b/arch/arm/mach-omap2/omap-headsmp.S
@@ -1,7 +1,7 @@
 /*
  * Secondary CPU startup routine source file.
  *
- * Copyright (C) 2009 Texas Instruments, Inc.
+ * Copyright (C) 2009-2014 Texas Instruments, Inc.
  *
  * Author:
  *      Santosh Shilimkar <santosh.shilimkar@ti.com>
@@ -28,7 +28,7 @@
  * code.  This routine also provides a holding flag into which
  * secondary core is held until we're ready for it to initialise.
  * The primary core will update this flag using a hardware
-+ * register AuxCoreBoot0.
+ * register AuxCoreBoot0.
  */
 ENTRY(omap5_secondary_startup)
 wait:	ldr	r2, =AUX_CORE_BOOT0_PA	@ read from AuxCoreBoot0
@@ -39,7 +39,7 @@ wait:	ldr	r2, =AUX_CORE_BOOT0_PA	@ read from AuxCoreBoot0
 	cmp	r0, r4
 	bne	wait
 	b	secondary_startup
-END(omap5_secondary_startup)
+ENDPROC(omap5_secondary_startup)
 /*
  * OMAP4 specific entry point for secondary CPU to jump from ROM
  * code.  This routine also provides a holding flag into which
diff --git a/arch/arm/mach-omap2/omap-hotplug.c b/arch/arm/mach-omap2/omap-hotplug.c
index 458f72f9dc8..971791fe9a3 100644
--- a/arch/arm/mach-omap2/omap-hotplug.c
+++ b/arch/arm/mach-omap2/omap-hotplug.c
@@ -39,7 +39,7 @@ void __ref omap4_cpu_die(unsigned int cpu)
 		if (omap_modify_auxcoreboot0(0x0, 0x200) != 0x0)
 			pr_err("Secure clear status failed\n");
 	} else {
-		__raw_writel(0, base + OMAP_AUX_CORE_BOOT_0);
+		writel_relaxed(0, base + OMAP_AUX_CORE_BOOT_0);
 	}
 
 
@@ -53,7 +53,7 @@ void __ref omap4_cpu_die(unsigned int cpu)
 			boot_cpu = omap_read_auxcoreboot0();
 		else
 			boot_cpu =
-				__raw_readl(base + OMAP_AUX_CORE_BOOT_0) >> 5;
+				readl_relaxed(base + OMAP_AUX_CORE_BOOT_0) >> 5;
 
 		if (boot_cpu == smp_processor_id()) {
 			/*
diff --git a/arch/arm/mach-omap2/omap-mpuss-lowpower.c b/arch/arm/mach-omap2/omap-mpuss-lowpower.c
index 667915d236f..eb76e47091a 100644
--- a/arch/arm/mach-omap2/omap-mpuss-lowpower.c
+++ b/arch/arm/mach-omap2/omap-mpuss-lowpower.c
@@ -116,7 +116,7 @@ static inline void set_cpu_wakeup_addr(unsigned int cpu_id, u32 addr)
 {
 	struct omap4_cpu_pm_info *pm_info = &per_cpu(omap4_pm_info, cpu_id);
 
-	__raw_writel(addr, pm_info->wkup_sar_addr);
+	writel_relaxed(addr, pm_info->wkup_sar_addr);
 }
 
 /*
@@ -141,7 +141,7 @@ static void scu_pwrst_prepare(unsigned int cpu_id, unsigned int cpu_state)
 		break;
 	}
 
-	__raw_writel(scu_pwr_st, pm_info->scu_sar_addr);
+	writel_relaxed(scu_pwr_st, pm_info->scu_sar_addr);
 }
 
 /* Helper functions for MPUSS OSWR */
@@ -179,7 +179,7 @@ static void l2x0_pwrst_prepare(unsigned int cpu_id, unsigned int save_state)
 {
 	struct omap4_cpu_pm_info *pm_info = &per_cpu(omap4_pm_info, cpu_id);
 
-	__raw_writel(save_state, pm_info->l2x0_sar_addr);
+	writel_relaxed(save_state, pm_info->l2x0_sar_addr);
 }
 
 /*
@@ -192,10 +192,10 @@ static void save_l2x0_context(void)
 	u32 val;
 	void __iomem *l2x0_base = omap4_get_l2cache_base();
 	if (l2x0_base) {
-		val = __raw_readl(l2x0_base + L2X0_AUX_CTRL);
-		__raw_writel(val, sar_base + L2X0_AUXCTRL_OFFSET);
-		val = __raw_readl(l2x0_base + L2X0_PREFETCH_CTRL);
-		__raw_writel(val, sar_base + L2X0_PREFETCH_CTRL_OFFSET);
+		val = readl_relaxed(l2x0_base + L2X0_AUX_CTRL);
+		writel_relaxed(val, sar_base + L2X0_AUXCTRL_OFFSET);
+		val = readl_relaxed(l2x0_base + L2X0_PREFETCH_CTRL);
+		writel_relaxed(val, sar_base + L2X0_PREFETCH_CTRL_OFFSET);
 	}
 }
 #else
@@ -386,9 +386,9 @@ int __init omap4_mpuss_init(void)
 
 	/* Save device type on scratchpad for low level code to use */
 	if (omap_type() != OMAP2_DEVICE_TYPE_GP)
-		__raw_writel(1, sar_base + OMAP_TYPE_OFFSET);
+		writel_relaxed(1, sar_base + OMAP_TYPE_OFFSET);
 	else
-		__raw_writel(0, sar_base + OMAP_TYPE_OFFSET);
+		writel_relaxed(0, sar_base + OMAP_TYPE_OFFSET);
 
 	save_l2x0_context();
 
diff --git a/arch/arm/mach-omap2/omap-smp.c b/arch/arm/mach-omap2/omap-smp.c
index 17550aa39d0..256e84ef0f6 100644
--- a/arch/arm/mach-omap2/omap-smp.c
+++ b/arch/arm/mach-omap2/omap-smp.c
@@ -99,7 +99,7 @@ static int omap4_boot_secondary(unsigned int cpu, struct task_struct *idle)
 	if (omap_secure_apis_support())
 		omap_modify_auxcoreboot0(0x200, 0xfffffdff);
 	else
-		__raw_writel(0x20, base + OMAP_AUX_CORE_BOOT_0);
+		writel_relaxed(0x20, base + OMAP_AUX_CORE_BOOT_0);
 
 	if (!cpu1_clkdm && !cpu1_pwrdm) {
 		cpu1_clkdm = clkdm_lookup("mpu1_clkdm");
@@ -227,8 +227,8 @@ static void __init omap4_smp_prepare_cpus(unsigned int max_cpus)
 	if (omap_secure_apis_support())
 		omap_auxcoreboot_addr(virt_to_phys(startup_addr));
 	else
-		__raw_writel(virt_to_phys(omap5_secondary_startup),
-						base + OMAP_AUX_CORE_BOOT_1);
+		writel_relaxed(virt_to_phys(omap5_secondary_startup),
+			       base + OMAP_AUX_CORE_BOOT_1);
 
 }
 
diff --git a/arch/arm/mach-omap2/omap-wakeupgen.c b/arch/arm/mach-omap2/omap-wakeupgen.c
index 693fe486e91..37843a7d363 100644
--- a/arch/arm/mach-omap2/omap-wakeupgen.c
+++ b/arch/arm/mach-omap2/omap-wakeupgen.c
@@ -60,19 +60,19 @@ static unsigned int omap_secure_apis;
  */
 static inline u32 wakeupgen_readl(u8 idx, u32 cpu)
 {
-	return __raw_readl(wakeupgen_base + OMAP_WKG_ENB_A_0 +
+	return readl_relaxed(wakeupgen_base + OMAP_WKG_ENB_A_0 +
 				(cpu * CPU_ENA_OFFSET) + (idx * 4));
 }
 
 static inline void wakeupgen_writel(u32 val, u8 idx, u32 cpu)
 {
-	__raw_writel(val, wakeupgen_base + OMAP_WKG_ENB_A_0 +
+	writel_relaxed(val, wakeupgen_base + OMAP_WKG_ENB_A_0 +
 				(cpu * CPU_ENA_OFFSET) + (idx * 4));
 }
 
 static inline void sar_writel(u32 val, u32 offset, u8 idx)
 {
-	__raw_writel(val, sar_base + offset + (idx * 4));
+	writel_relaxed(val, sar_base + offset + (idx * 4));
 }
 
 static inline int _wakeupgen_get_irq_info(u32 irq, u32 *bit_posn, u8 *reg_index)
@@ -231,21 +231,21 @@ static inline void omap4_irq_save_context(void)
 	}
 
 	/* Save AuxBoot* registers */
-	val = __raw_readl(wakeupgen_base + OMAP_AUX_CORE_BOOT_0);
-	__raw_writel(val, sar_base + AUXCOREBOOT0_OFFSET);
-	val = __raw_readl(wakeupgen_base + OMAP_AUX_CORE_BOOT_1);
-	__raw_writel(val, sar_base + AUXCOREBOOT1_OFFSET);
+	val = readl_relaxed(wakeupgen_base + OMAP_AUX_CORE_BOOT_0);
+	writel_relaxed(val, sar_base + AUXCOREBOOT0_OFFSET);
+	val = readl_relaxed(wakeupgen_base + OMAP_AUX_CORE_BOOT_1);
+	writel_relaxed(val, sar_base + AUXCOREBOOT1_OFFSET);
 
 	/* Save SyncReq generation logic */
-	val = __raw_readl(wakeupgen_base + OMAP_PTMSYNCREQ_MASK);
-	__raw_writel(val, sar_base + PTMSYNCREQ_MASK_OFFSET);
-	val = __raw_readl(wakeupgen_base + OMAP_PTMSYNCREQ_EN);
-	__raw_writel(val, sar_base + PTMSYNCREQ_EN_OFFSET);
+	val = readl_relaxed(wakeupgen_base + OMAP_PTMSYNCREQ_MASK);
+	writel_relaxed(val, sar_base + PTMSYNCREQ_MASK_OFFSET);
+	val = readl_relaxed(wakeupgen_base + OMAP_PTMSYNCREQ_EN);
+	writel_relaxed(val, sar_base + PTMSYNCREQ_EN_OFFSET);
 
 	/* Set the Backup Bit Mask status */
-	val = __raw_readl(sar_base + SAR_BACKUP_STATUS_OFFSET);
+	val = readl_relaxed(sar_base + SAR_BACKUP_STATUS_OFFSET);
 	val |= SAR_BACKUP_STATUS_WAKEUPGEN;
-	__raw_writel(val, sar_base + SAR_BACKUP_STATUS_OFFSET);
+	writel_relaxed(val, sar_base + SAR_BACKUP_STATUS_OFFSET);
 
 }
 
@@ -264,15 +264,15 @@ static inline void omap5_irq_save_context(void)
 	}
 
 	/* Save AuxBoot* registers */
-	val = __raw_readl(wakeupgen_base + OMAP_AUX_CORE_BOOT_0);
-	__raw_writel(val, sar_base + OMAP5_AUXCOREBOOT0_OFFSET);
-	val = __raw_readl(wakeupgen_base + OMAP_AUX_CORE_BOOT_0);
-	__raw_writel(val, sar_base + OMAP5_AUXCOREBOOT1_OFFSET);
+	val = readl_relaxed(wakeupgen_base + OMAP_AUX_CORE_BOOT_0);
+	writel_relaxed(val, sar_base + OMAP5_AUXCOREBOOT0_OFFSET);
+	val = readl_relaxed(wakeupgen_base + OMAP_AUX_CORE_BOOT_0);
+	writel_relaxed(val, sar_base + OMAP5_AUXCOREBOOT1_OFFSET);
 
 	/* Set the Backup Bit Mask status */
-	val = __raw_readl(sar_base + OMAP5_SAR_BACKUP_STATUS_OFFSET);
+	val = readl_relaxed(sar_base + OMAP5_SAR_BACKUP_STATUS_OFFSET);
 	val |= SAR_BACKUP_STATUS_WAKEUPGEN;
-	__raw_writel(val, sar_base + OMAP5_SAR_BACKUP_STATUS_OFFSET);
+	writel_relaxed(val, sar_base + OMAP5_SAR_BACKUP_STATUS_OFFSET);
 
 }
 
@@ -306,9 +306,9 @@ static void irq_sar_clear(void)
 	if (soc_is_omap54xx())
 		offset = OMAP5_SAR_BACKUP_STATUS_OFFSET;
 
-	val = __raw_readl(sar_base + offset);
+	val = readl_relaxed(sar_base + offset);
 	val &= ~SAR_BACKUP_STATUS_WAKEUPGEN;
-	__raw_writel(val, sar_base + offset);
+	writel_relaxed(val, sar_base + offset);
 }
 
 /*
diff --git a/arch/arm/mach-omap2/omap4-common.c b/arch/arm/mach-omap2/omap4-common.c
index 95e171a055f..99b0154493a 100644
--- a/arch/arm/mach-omap2/omap4-common.c
+++ b/arch/arm/mach-omap2/omap4-common.c
@@ -125,25 +125,25 @@ void __init gic_init_irq(void)
 void gic_dist_disable(void)
 {
 	if (gic_dist_base_addr)
-		__raw_writel(0x0, gic_dist_base_addr + GIC_DIST_CTRL);
+		writel_relaxed(0x0, gic_dist_base_addr + GIC_DIST_CTRL);
 }
 
 void gic_dist_enable(void)
 {
 	if (gic_dist_base_addr)
-		__raw_writel(0x1, gic_dist_base_addr + GIC_DIST_CTRL);
+		writel_relaxed(0x1, gic_dist_base_addr + GIC_DIST_CTRL);
 }
 
 bool gic_dist_disabled(void)
 {
-	return !(__raw_readl(gic_dist_base_addr + GIC_DIST_CTRL) & 0x1);
+	return !(readl_relaxed(gic_dist_base_addr + GIC_DIST_CTRL) & 0x1);
 }
 
 void gic_timer_retrigger(void)
 {
-	u32 twd_int = __raw_readl(twd_base + TWD_TIMER_INTSTAT);
-	u32 gic_int = __raw_readl(gic_dist_base_addr + GIC_DIST_PENDING_SET);
-	u32 twd_ctrl = __raw_readl(twd_base + TWD_TIMER_CONTROL);
+	u32 twd_int = readl_relaxed(twd_base + TWD_TIMER_INTSTAT);
+	u32 gic_int = readl_relaxed(gic_dist_base_addr + GIC_DIST_PENDING_SET);
+	u32 twd_ctrl = readl_relaxed(twd_base + TWD_TIMER_CONTROL);
 
 	if (twd_int && !(gic_int & BIT(IRQ_LOCALTIMER))) {
 		/*
@@ -151,11 +151,11 @@ void gic_timer_retrigger(void)
 		 * disabled.  Ack the pending interrupt, and retrigger it.
 		 */
 		pr_warn("%s: lost localtimer interrupt\n", __func__);
-		__raw_writel(1, twd_base + TWD_TIMER_INTSTAT);
+		writel_relaxed(1, twd_base + TWD_TIMER_INTSTAT);
 		if (!(twd_ctrl & TWD_TIMER_CONTROL_PERIODIC)) {
-			__raw_writel(1, twd_base + TWD_TIMER_COUNTER);
+			writel_relaxed(1, twd_base + TWD_TIMER_COUNTER);
 			twd_ctrl |= TWD_TIMER_CONTROL_ENABLE;
-			__raw_writel(twd_ctrl, twd_base + TWD_TIMER_CONTROL);
+			writel_relaxed(twd_ctrl, twd_base + TWD_TIMER_CONTROL);
 		}
 	}
 }
diff --git a/arch/arm/mach-omap2/omap_hwmod.c b/arch/arm/mach-omap2/omap_hwmod.c
index 1f33f5db10d..f7bb435bb54 100644
--- a/arch/arm/mach-omap2/omap_hwmod.c
+++ b/arch/arm/mach-omap2/omap_hwmod.c
@@ -72,7 +72,7 @@
  *            | (../mach-omap2/omap_hwmod*)   |
  *            +-------------------------------+
  *            | OMAP clock/PRCM/register fns  |
- *            | (__raw_{read,write}l, clk*)   |
+ *            | ({read,write}l_relaxed, clk*) |
  *            +-------------------------------+
  *
  * Device drivers should not contain any OMAP-specific code or data in
@@ -2546,11 +2546,12 @@ static int __init _init(struct omap_hwmod *oh, void *data)
 		return -EINVAL;
 	}
 
-	if (np)
+	if (np) {
 		if (of_find_property(np, "ti,no-reset-on-init", NULL))
 			oh->flags |= HWMOD_INIT_NO_RESET;
 		if (of_find_property(np, "ti,no-idle-on-init", NULL))
 			oh->flags |= HWMOD_INIT_NO_IDLE;
+	}
 
 	oh->_state = _HWMOD_STATE_INITIALIZED;
 
@@ -3229,17 +3230,17 @@ static int _am33xx_is_hardreset_asserted(struct omap_hwmod *oh,
 u32 omap_hwmod_read(struct omap_hwmod *oh, u16 reg_offs)
 {
 	if (oh->flags & HWMOD_16BIT_REG)
-		return __raw_readw(oh->_mpu_rt_va + reg_offs);
+		return readw_relaxed(oh->_mpu_rt_va + reg_offs);
 	else
-		return __raw_readl(oh->_mpu_rt_va + reg_offs);
+		return readl_relaxed(oh->_mpu_rt_va + reg_offs);
 }
 
 void omap_hwmod_write(u32 v, struct omap_hwmod *oh, u16 reg_offs)
 {
 	if (oh->flags & HWMOD_16BIT_REG)
-		__raw_writew(v, oh->_mpu_rt_va + reg_offs);
+		writew_relaxed(v, oh->_mpu_rt_va + reg_offs);
 	else
-		__raw_writel(v, oh->_mpu_rt_va + reg_offs);
+		writel_relaxed(v, oh->_mpu_rt_va + reg_offs);
 }
 
 /**
diff --git a/arch/arm/mach-omap2/omap_hwmod_33xx_43xx_ipblock_data.c b/arch/arm/mach-omap2/omap_hwmod_33xx_43xx_ipblock_data.c
index 0f178623e7d..a579b89ce9b 100644
--- a/arch/arm/mach-omap2/omap_hwmod_33xx_43xx_ipblock_data.c
+++ b/arch/arm/mach-omap2/omap_hwmod_33xx_43xx_ipblock_data.c
@@ -24,6 +24,7 @@
 #include "prm33xx.h"
 #include "omap_hwmod_33xx_43xx_common_data.h"
 #include "prcm43xx.h"
+#include "common.h"
 
 #define CLKCTRL(oh, clkctrl) ((oh).prcm.omap4.clkctrl_offs = (clkctrl))
 #define RSTCTRL(oh, rstctrl) ((oh).prcm.omap4.rstctrl_offs = (rstctrl))
diff --git a/arch/arm/mach-omap2/omap_hwmod_3xxx_data.c b/arch/arm/mach-omap2/omap_hwmod_3xxx_data.c
index a123ff0070b..1cd0cfdc03e 100644
--- a/arch/arm/mach-omap2/omap_hwmod_3xxx_data.c
+++ b/arch/arm/mach-omap2/omap_hwmod_3xxx_data.c
@@ -1964,7 +1964,7 @@ static struct omap_hwmod_irq_info omap3xxx_usb_host_hs_irqs[] = {
 static struct omap_hwmod omap3xxx_usb_host_hs_hwmod = {
 	.name		= "usb_host_hs",
 	.class		= &omap3xxx_usb_host_hs_hwmod_class,
-	.clkdm_name	= "l3_init_clkdm",
+	.clkdm_name	= "usbhost_clkdm",
 	.mpu_irqs	= omap3xxx_usb_host_hs_irqs,
 	.main_clk	= "usbhost_48m_fck",
 	.prcm = {
@@ -2047,7 +2047,7 @@ static struct omap_hwmod_irq_info omap3xxx_usb_tll_hs_irqs[] = {
 static struct omap_hwmod omap3xxx_usb_tll_hs_hwmod = {
 	.name		= "usb_tll_hs",
 	.class		= &omap3xxx_usb_tll_hs_hwmod_class,
-	.clkdm_name	= "l3_init_clkdm",
+	.clkdm_name	= "core_l4_clkdm",
 	.mpu_irqs	= omap3xxx_usb_tll_hs_irqs,
 	.main_clk	= "usbtll_fck",
 	.prcm = {
@@ -3689,12 +3689,9 @@ static struct omap_hwmod_class_sysconfig omap34xx_ssi_sysc = {
 	.rev_offs	= 0x0000,
 	.sysc_offs	= 0x0010,
 	.syss_offs	= 0x0014,
-	.sysc_flags	= (SYSC_HAS_AUTOIDLE | SYSC_HAS_EMUFREE |
-			   SYSC_HAS_MIDLEMODE | SYSC_HAS_SIDLEMODE |
-			   SYSC_HAS_SOFTRESET | SYSS_HAS_RESET_STATUS),
-	.idlemodes	= (SIDLE_FORCE | SIDLE_NO | SIDLE_SMART |
-			   SIDLE_SMART_WKUP | MSTANDBY_FORCE | MSTANDBY_NO |
-			   MSTANDBY_SMART | MSTANDBY_SMART_WKUP),
+	.sysc_flags	= (SYSC_HAS_AUTOIDLE | SYSC_HAS_MIDLEMODE |
+			   SYSC_HAS_SIDLEMODE | SYSC_HAS_SOFTRESET),
+	.idlemodes	= (SIDLE_FORCE | SIDLE_NO | SIDLE_SMART),
 	.sysc_fields	= &omap_hwmod_sysc_type1,
 };
 
diff --git a/arch/arm/mach-omap2/omap_hwmod_44xx_data.c b/arch/arm/mach-omap2/omap_hwmod_44xx_data.c
index 1219280bb97..41e54f75993 100644
--- a/arch/arm/mach-omap2/omap_hwmod_44xx_data.c
+++ b/arch/arm/mach-omap2/omap_hwmod_44xx_data.c
@@ -3635,15 +3635,7 @@ static struct omap_hwmod_ocp_if omap44xx_l4_abe__dmic = {
 	.master		= &omap44xx_l4_abe_hwmod,
 	.slave		= &omap44xx_dmic_hwmod,
 	.clk		= "ocp_abe_iclk",
-	.user		= OCP_USER_MPU,
-};
-
-/* l4_abe -> dmic (dma) */
-static struct omap_hwmod_ocp_if omap44xx_l4_abe__dmic_dma = {
-	.master		= &omap44xx_l4_abe_hwmod,
-	.slave		= &omap44xx_dmic_hwmod,
-	.clk		= "ocp_abe_iclk",
-	.user		= OCP_USER_SDMA,
+	.user		= OCP_USER_MPU | OCP_USER_SDMA,
 };
 
 /* dsp -> iva */
@@ -4209,15 +4201,7 @@ static struct omap_hwmod_ocp_if omap44xx_l4_abe__mcbsp1 = {
 	.master		= &omap44xx_l4_abe_hwmod,
 	.slave		= &omap44xx_mcbsp1_hwmod,
 	.clk		= "ocp_abe_iclk",
-	.user		= OCP_USER_MPU,
-};
-
-/* l4_abe -> mcbsp1 (dma) */
-static struct omap_hwmod_ocp_if omap44xx_l4_abe__mcbsp1_dma = {
-	.master		= &omap44xx_l4_abe_hwmod,
-	.slave		= &omap44xx_mcbsp1_hwmod,
-	.clk		= "ocp_abe_iclk",
-	.user		= OCP_USER_SDMA,
+	.user		= OCP_USER_MPU | OCP_USER_SDMA,
 };
 
 /* l4_abe -> mcbsp2 */
@@ -4225,15 +4209,7 @@ static struct omap_hwmod_ocp_if omap44xx_l4_abe__mcbsp2 = {
 	.master		= &omap44xx_l4_abe_hwmod,
 	.slave		= &omap44xx_mcbsp2_hwmod,
 	.clk		= "ocp_abe_iclk",
-	.user		= OCP_USER_MPU,
-};
-
-/* l4_abe -> mcbsp2 (dma) */
-static struct omap_hwmod_ocp_if omap44xx_l4_abe__mcbsp2_dma = {
-	.master		= &omap44xx_l4_abe_hwmod,
-	.slave		= &omap44xx_mcbsp2_hwmod,
-	.clk		= "ocp_abe_iclk",
-	.user		= OCP_USER_SDMA,
+	.user		= OCP_USER_MPU | OCP_USER_SDMA,
 };
 
 /* l4_abe -> mcbsp3 */
@@ -4241,15 +4217,7 @@ static struct omap_hwmod_ocp_if omap44xx_l4_abe__mcbsp3 = {
 	.master		= &omap44xx_l4_abe_hwmod,
 	.slave		= &omap44xx_mcbsp3_hwmod,
 	.clk		= "ocp_abe_iclk",
-	.user		= OCP_USER_MPU,
-};
-
-/* l4_abe -> mcbsp3 (dma) */
-static struct omap_hwmod_ocp_if omap44xx_l4_abe__mcbsp3_dma = {
-	.master		= &omap44xx_l4_abe_hwmod,
-	.slave		= &omap44xx_mcbsp3_hwmod,
-	.clk		= "ocp_abe_iclk",
-	.user		= OCP_USER_SDMA,
+	.user		= OCP_USER_MPU | OCP_USER_SDMA,
 };
 
 /* l4_per -> mcbsp4 */
@@ -4265,15 +4233,7 @@ static struct omap_hwmod_ocp_if omap44xx_l4_abe__mcpdm = {
 	.master		= &omap44xx_l4_abe_hwmod,
 	.slave		= &omap44xx_mcpdm_hwmod,
 	.clk		= "ocp_abe_iclk",
-	.user		= OCP_USER_MPU,
-};
-
-/* l4_abe -> mcpdm (dma) */
-static struct omap_hwmod_ocp_if omap44xx_l4_abe__mcpdm_dma = {
-	.master		= &omap44xx_l4_abe_hwmod,
-	.slave		= &omap44xx_mcpdm_hwmod,
-	.clk		= "ocp_abe_iclk",
-	.user		= OCP_USER_SDMA,
+	.user		= OCP_USER_MPU | OCP_USER_SDMA,
 };
 
 /* l4_per -> mcspi1 */
@@ -4575,15 +4535,7 @@ static struct omap_hwmod_ocp_if omap44xx_l4_abe__timer5 = {
 	.master		= &omap44xx_l4_abe_hwmod,
 	.slave		= &omap44xx_timer5_hwmod,
 	.clk		= "ocp_abe_iclk",
-	.user		= OCP_USER_MPU,
-};
-
-/* l4_abe -> timer5 (dma) */
-static struct omap_hwmod_ocp_if omap44xx_l4_abe__timer5_dma = {
-	.master		= &omap44xx_l4_abe_hwmod,
-	.slave		= &omap44xx_timer5_hwmod,
-	.clk		= "ocp_abe_iclk",
-	.user		= OCP_USER_SDMA,
+	.user		= OCP_USER_MPU | OCP_USER_SDMA,
 };
 
 /* l4_abe -> timer6 */
@@ -4591,15 +4543,7 @@ static struct omap_hwmod_ocp_if omap44xx_l4_abe__timer6 = {
 	.master		= &omap44xx_l4_abe_hwmod,
 	.slave		= &omap44xx_timer6_hwmod,
 	.clk		= "ocp_abe_iclk",
-	.user		= OCP_USER_MPU,
-};
-
-/* l4_abe -> timer6 (dma) */
-static struct omap_hwmod_ocp_if omap44xx_l4_abe__timer6_dma = {
-	.master		= &omap44xx_l4_abe_hwmod,
-	.slave		= &omap44xx_timer6_hwmod,
-	.clk		= "ocp_abe_iclk",
-	.user		= OCP_USER_SDMA,
+	.user		= OCP_USER_MPU | OCP_USER_SDMA,
 };
 
 /* l4_abe -> timer7 */
@@ -4607,15 +4551,7 @@ static struct omap_hwmod_ocp_if omap44xx_l4_abe__timer7 = {
 	.master		= &omap44xx_l4_abe_hwmod,
 	.slave		= &omap44xx_timer7_hwmod,
 	.clk		= "ocp_abe_iclk",
-	.user		= OCP_USER_MPU,
-};
-
-/* l4_abe -> timer7 (dma) */
-static struct omap_hwmod_ocp_if omap44xx_l4_abe__timer7_dma = {
-	.master		= &omap44xx_l4_abe_hwmod,
-	.slave		= &omap44xx_timer7_hwmod,
-	.clk		= "ocp_abe_iclk",
-	.user		= OCP_USER_SDMA,
+	.user		= OCP_USER_MPU | OCP_USER_SDMA,
 };
 
 /* l4_abe -> timer8 */
@@ -4623,15 +4559,7 @@ static struct omap_hwmod_ocp_if omap44xx_l4_abe__timer8 = {
 	.master		= &omap44xx_l4_abe_hwmod,
 	.slave		= &omap44xx_timer8_hwmod,
 	.clk		= "ocp_abe_iclk",
-	.user		= OCP_USER_MPU,
-};
-
-/* l4_abe -> timer8 (dma) */
-static struct omap_hwmod_ocp_if omap44xx_l4_abe__timer8_dma = {
-	.master		= &omap44xx_l4_abe_hwmod,
-	.slave		= &omap44xx_timer8_hwmod,
-	.clk		= "ocp_abe_iclk",
-	.user		= OCP_USER_SDMA,
+	.user		= OCP_USER_MPU | OCP_USER_SDMA,
 };
 
 /* l4_per -> timer9 */
@@ -4831,7 +4759,6 @@ static struct omap_hwmod_ocp_if *omap44xx_hwmod_ocp_ifs[] __initdata = {
 	&omap44xx_l3_instr__debugss,
 	&omap44xx_l4_cfg__dma_system,
 	&omap44xx_l4_abe__dmic,
-	&omap44xx_l4_abe__dmic_dma,
 	&omap44xx_dsp__iva,
 	/* &omap44xx_dsp__sl2if, */
 	&omap44xx_l4_cfg__dsp,
@@ -4874,14 +4801,10 @@ static struct omap_hwmod_ocp_if *omap44xx_hwmod_ocp_ifs[] __initdata = {
 	&omap44xx_l4_abe__mcasp,
 	&omap44xx_l4_abe__mcasp_dma,
 	&omap44xx_l4_abe__mcbsp1,
-	&omap44xx_l4_abe__mcbsp1_dma,
 	&omap44xx_l4_abe__mcbsp2,
-	&omap44xx_l4_abe__mcbsp2_dma,
 	&omap44xx_l4_abe__mcbsp3,
-	&omap44xx_l4_abe__mcbsp3_dma,
 	&omap44xx_l4_per__mcbsp4,
 	&omap44xx_l4_abe__mcpdm,
-	&omap44xx_l4_abe__mcpdm_dma,
 	&omap44xx_l4_per__mcspi1,
 	&omap44xx_l4_per__mcspi2,
 	&omap44xx_l4_per__mcspi3,
@@ -4913,13 +4836,9 @@ static struct omap_hwmod_ocp_if *omap44xx_hwmod_ocp_ifs[] __initdata = {
 	&omap44xx_l4_per__timer3,
 	&omap44xx_l4_per__timer4,
 	&omap44xx_l4_abe__timer5,
-	&omap44xx_l4_abe__timer5_dma,
 	&omap44xx_l4_abe__timer6,
-	&omap44xx_l4_abe__timer6_dma,
 	&omap44xx_l4_abe__timer7,
-	&omap44xx_l4_abe__timer7_dma,
 	&omap44xx_l4_abe__timer8,
-	&omap44xx_l4_abe__timer8_dma,
 	&omap44xx_l4_per__timer9,
 	&omap44xx_l4_per__timer10,
 	&omap44xx_l4_per__timer11,
diff --git a/arch/arm/mach-omap2/omap_hwmod_54xx_data.c b/arch/arm/mach-omap2/omap_hwmod_54xx_data.c
index 892317294fd..290213f2cbe 100644
--- a/arch/arm/mach-omap2/omap_hwmod_54xx_data.c
+++ b/arch/arm/mach-omap2/omap_hwmod_54xx_data.c
@@ -334,6 +334,235 @@ static struct omap_hwmod omap54xx_dmic_hwmod = {
 };
 
 /*
+ * 'dss' class
+ * display sub-system
+ */
+static struct omap_hwmod_class_sysconfig omap54xx_dss_sysc = {
+	.rev_offs	= 0x0000,
+	.syss_offs	= 0x0014,
+	.sysc_flags	= SYSS_HAS_RESET_STATUS,
+};
+
+static struct omap_hwmod_class omap54xx_dss_hwmod_class = {
+	.name	= "dss",
+	.sysc	= &omap54xx_dss_sysc,
+	.reset	= omap_dss_reset,
+};
+
+/* dss */
+static struct omap_hwmod_opt_clk dss_opt_clks[] = {
+	{ .role = "32khz_clk", .clk = "dss_32khz_clk" },
+	{ .role = "sys_clk", .clk = "dss_sys_clk" },
+	{ .role = "hdmi_clk", .clk = "dss_48mhz_clk" },
+};
+
+static struct omap_hwmod omap54xx_dss_hwmod = {
+	.name		= "dss_core",
+	.class		= &omap54xx_dss_hwmod_class,
+	.clkdm_name	= "dss_clkdm",
+	.flags		= HWMOD_CONTROL_OPT_CLKS_IN_RESET,
+	.main_clk	= "dss_dss_clk",
+	.prcm = {
+		.omap4 = {
+			.clkctrl_offs = OMAP54XX_CM_DSS_DSS_CLKCTRL_OFFSET,
+			.context_offs = OMAP54XX_RM_DSS_DSS_CONTEXT_OFFSET,
+			.modulemode   = MODULEMODE_SWCTRL,
+		},
+	},
+	.opt_clks	= dss_opt_clks,
+	.opt_clks_cnt	= ARRAY_SIZE(dss_opt_clks),
+};
+
+/*
+ * 'dispc' class
+ * display controller
+ */
+
+static struct omap_hwmod_class_sysconfig omap54xx_dispc_sysc = {
+	.rev_offs	= 0x0000,
+	.sysc_offs	= 0x0010,
+	.syss_offs	= 0x0014,
+	.sysc_flags	= (SYSC_HAS_AUTOIDLE | SYSC_HAS_CLOCKACTIVITY |
+			   SYSC_HAS_ENAWAKEUP | SYSC_HAS_MIDLEMODE |
+			   SYSC_HAS_SIDLEMODE | SYSC_HAS_SOFTRESET |
+			   SYSS_HAS_RESET_STATUS),
+	.idlemodes	= (SIDLE_FORCE | SIDLE_NO | SIDLE_SMART |
+			   MSTANDBY_FORCE | MSTANDBY_NO | MSTANDBY_SMART),
+	.sysc_fields	= &omap_hwmod_sysc_type1,
+};
+
+static struct omap_hwmod_class omap54xx_dispc_hwmod_class = {
+	.name	= "dispc",
+	.sysc	= &omap54xx_dispc_sysc,
+};
+
+/* dss_dispc */
+static struct omap_hwmod_opt_clk dss_dispc_opt_clks[] = {
+	{ .role = "sys_clk", .clk = "dss_sys_clk" },
+};
+
+/* dss_dispc dev_attr */
+static struct omap_dss_dispc_dev_attr dss_dispc_dev_attr = {
+	.has_framedonetv_irq	= 1,
+	.manager_count		= 4,
+};
+
+static struct omap_hwmod omap54xx_dss_dispc_hwmod = {
+	.name		= "dss_dispc",
+	.class		= &omap54xx_dispc_hwmod_class,
+	.clkdm_name	= "dss_clkdm",
+	.main_clk	= "dss_dss_clk",
+	.prcm = {
+		.omap4 = {
+			.clkctrl_offs = OMAP54XX_CM_DSS_DSS_CLKCTRL_OFFSET,
+			.flags = HWMOD_OMAP4_NO_CONTEXT_LOSS_BIT,
+		},
+	},
+	.opt_clks	= dss_dispc_opt_clks,
+	.opt_clks_cnt	= ARRAY_SIZE(dss_dispc_opt_clks),
+	.dev_attr	= &dss_dispc_dev_attr,
+};
+
+/*
+ * 'dsi1' class
+ * display serial interface controller
+ */
+
+static struct omap_hwmod_class_sysconfig omap54xx_dsi1_sysc = {
+	.rev_offs	= 0x0000,
+	.sysc_offs	= 0x0010,
+	.syss_offs	= 0x0014,
+	.sysc_flags	= (SYSC_HAS_AUTOIDLE | SYSC_HAS_CLOCKACTIVITY |
+			   SYSC_HAS_ENAWAKEUP | SYSC_HAS_SIDLEMODE |
+			   SYSC_HAS_SOFTRESET | SYSS_HAS_RESET_STATUS),
+	.idlemodes	= (SIDLE_FORCE | SIDLE_NO | SIDLE_SMART),
+	.sysc_fields	= &omap_hwmod_sysc_type1,
+};
+
+static struct omap_hwmod_class omap54xx_dsi1_hwmod_class = {
+	.name	= "dsi1",
+	.sysc	= &omap54xx_dsi1_sysc,
+};
+
+/* dss_dsi1_a */
+static struct omap_hwmod_opt_clk dss_dsi1_a_opt_clks[] = {
+	{ .role = "sys_clk", .clk = "dss_sys_clk" },
+};
+
+static struct omap_hwmod omap54xx_dss_dsi1_a_hwmod = {
+	.name		= "dss_dsi1",
+	.class		= &omap54xx_dsi1_hwmod_class,
+	.clkdm_name	= "dss_clkdm",
+	.main_clk	= "dss_dss_clk",
+	.prcm = {
+		.omap4 = {
+			.clkctrl_offs = OMAP54XX_CM_DSS_DSS_CLKCTRL_OFFSET,
+			.flags = HWMOD_OMAP4_NO_CONTEXT_LOSS_BIT,
+		},
+	},
+	.opt_clks	= dss_dsi1_a_opt_clks,
+	.opt_clks_cnt	= ARRAY_SIZE(dss_dsi1_a_opt_clks),
+};
+
+/* dss_dsi1_c */
+static struct omap_hwmod_opt_clk dss_dsi1_c_opt_clks[] = {
+	{ .role = "sys_clk", .clk = "dss_sys_clk" },
+};
+
+static struct omap_hwmod omap54xx_dss_dsi1_c_hwmod = {
+	.name		= "dss_dsi2",
+	.class		= &omap54xx_dsi1_hwmod_class,
+	.clkdm_name	= "dss_clkdm",
+	.main_clk	= "dss_dss_clk",
+	.prcm = {
+		.omap4 = {
+			.clkctrl_offs = OMAP54XX_CM_DSS_DSS_CLKCTRL_OFFSET,
+			.flags = HWMOD_OMAP4_NO_CONTEXT_LOSS_BIT,
+		},
+	},
+	.opt_clks	= dss_dsi1_c_opt_clks,
+	.opt_clks_cnt	= ARRAY_SIZE(dss_dsi1_c_opt_clks),
+};
+
+/*
+ * 'hdmi' class
+ * hdmi controller
+ */
+
+static struct omap_hwmod_class_sysconfig omap54xx_hdmi_sysc = {
+	.rev_offs	= 0x0000,
+	.sysc_offs	= 0x0010,
+	.sysc_flags	= (SYSC_HAS_RESET_STATUS | SYSC_HAS_SIDLEMODE |
+			   SYSC_HAS_SOFTRESET),
+	.idlemodes	= (SIDLE_FORCE | SIDLE_NO | SIDLE_SMART |
+			   SIDLE_SMART_WKUP),
+	.sysc_fields	= &omap_hwmod_sysc_type2,
+};
+
+static struct omap_hwmod_class omap54xx_hdmi_hwmod_class = {
+	.name	= "hdmi",
+	.sysc	= &omap54xx_hdmi_sysc,
+};
+
+static struct omap_hwmod_opt_clk dss_hdmi_opt_clks[] = {
+	{ .role = "sys_clk", .clk = "dss_sys_clk" },
+};
+
+static struct omap_hwmod omap54xx_dss_hdmi_hwmod = {
+	.name		= "dss_hdmi",
+	.class		= &omap54xx_hdmi_hwmod_class,
+	.clkdm_name	= "dss_clkdm",
+	.main_clk	= "dss_48mhz_clk",
+	.prcm = {
+		.omap4 = {
+			.clkctrl_offs = OMAP54XX_CM_DSS_DSS_CLKCTRL_OFFSET,
+			.flags = HWMOD_OMAP4_NO_CONTEXT_LOSS_BIT,
+		},
+	},
+	.opt_clks	= dss_hdmi_opt_clks,
+	.opt_clks_cnt	= ARRAY_SIZE(dss_hdmi_opt_clks),
+};
+
+/*
+ * 'rfbi' class
+ * remote frame buffer interface
+ */
+
+static struct omap_hwmod_class_sysconfig omap54xx_rfbi_sysc = {
+	.rev_offs	= 0x0000,
+	.sysc_offs	= 0x0010,
+	.syss_offs	= 0x0014,
+	.sysc_flags	= (SYSC_HAS_AUTOIDLE | SYSC_HAS_SIDLEMODE |
+			   SYSC_HAS_SOFTRESET | SYSS_HAS_RESET_STATUS),
+	.idlemodes	= (SIDLE_FORCE | SIDLE_NO | SIDLE_SMART),
+	.sysc_fields	= &omap_hwmod_sysc_type1,
+};
+
+static struct omap_hwmod_class omap54xx_rfbi_hwmod_class = {
+	.name	= "rfbi",
+	.sysc	= &omap54xx_rfbi_sysc,
+};
+
+/* dss_rfbi */
+static struct omap_hwmod_opt_clk dss_rfbi_opt_clks[] = {
+	{ .role = "ick", .clk = "l3_iclk_div" },
+};
+
+static struct omap_hwmod omap54xx_dss_rfbi_hwmod = {
+	.name		= "dss_rfbi",
+	.class		= &omap54xx_rfbi_hwmod_class,
+	.clkdm_name	= "dss_clkdm",
+	.prcm = {
+		.omap4 = {
+			.clkctrl_offs = OMAP54XX_CM_DSS_DSS_CLKCTRL_OFFSET,
+			.flags = HWMOD_OMAP4_NO_CONTEXT_LOSS_BIT,
+		},
+	},
+	.opt_clks	= dss_rfbi_opt_clks,
+	.opt_clks_cnt	= ARRAY_SIZE(dss_rfbi_opt_clks),
+};
+
+/*
  * 'emif' class
  * external memory interface no1 (wrapper)
  */
@@ -895,7 +1124,7 @@ static struct omap_hwmod omap54xx_mcpdm_hwmod = {
 	 * current exception.
 	 */
 
-	.flags		= HWMOD_EXT_OPT_MAIN_CLK,
+	.flags		= HWMOD_EXT_OPT_MAIN_CLK | HWMOD_SWSUP_SIDLE,
 	.main_clk	= "pad_clks_ck",
 	.prcm = {
 		.omap4 = {
@@ -1974,6 +2203,54 @@ static struct omap_hwmod_ocp_if omap54xx_l4_abe__dmic = {
 	.user		= OCP_USER_MPU,
 };
 
+/* l3_main_2 -> dss */
+static struct omap_hwmod_ocp_if omap54xx_l3_main_2__dss = {
+	.master		= &omap54xx_l3_main_2_hwmod,
+	.slave		= &omap54xx_dss_hwmod,
+	.clk		= "l3_iclk_div",
+	.user		= OCP_USER_MPU | OCP_USER_SDMA,
+};
+
+/* l3_main_2 -> dss_dispc */
+static struct omap_hwmod_ocp_if omap54xx_l3_main_2__dss_dispc = {
+	.master		= &omap54xx_l3_main_2_hwmod,
+	.slave		= &omap54xx_dss_dispc_hwmod,
+	.clk		= "l3_iclk_div",
+	.user		= OCP_USER_MPU | OCP_USER_SDMA,
+};
+
+/* l3_main_2 -> dss_dsi1_a */
+static struct omap_hwmod_ocp_if omap54xx_l3_main_2__dss_dsi1_a = {
+	.master		= &omap54xx_l3_main_2_hwmod,
+	.slave		= &omap54xx_dss_dsi1_a_hwmod,
+	.clk		= "l3_iclk_div",
+	.user		= OCP_USER_MPU | OCP_USER_SDMA,
+};
+
+/* l3_main_2 -> dss_dsi1_c */
+static struct omap_hwmod_ocp_if omap54xx_l3_main_2__dss_dsi1_c = {
+	.master		= &omap54xx_l3_main_2_hwmod,
+	.slave		= &omap54xx_dss_dsi1_c_hwmod,
+	.clk		= "l3_iclk_div",
+	.user		= OCP_USER_MPU | OCP_USER_SDMA,
+};
+
+/* l3_main_2 -> dss_hdmi */
+static struct omap_hwmod_ocp_if omap54xx_l3_main_2__dss_hdmi = {
+	.master		= &omap54xx_l3_main_2_hwmod,
+	.slave		= &omap54xx_dss_hdmi_hwmod,
+	.clk		= "l3_iclk_div",
+	.user		= OCP_USER_MPU | OCP_USER_SDMA,
+};
+
+/* l3_main_2 -> dss_rfbi */
+static struct omap_hwmod_ocp_if omap54xx_l3_main_2__dss_rfbi = {
+	.master		= &omap54xx_l3_main_2_hwmod,
+	.slave		= &omap54xx_dss_rfbi_hwmod,
+	.clk		= "l3_iclk_div",
+	.user		= OCP_USER_MPU | OCP_USER_SDMA,
+};
+
 /* mpu -> emif1 */
 static struct omap_hwmod_ocp_if omap54xx_mpu__emif1 = {
 	.master		= &omap54xx_mpu_hwmod,
@@ -2427,6 +2704,12 @@ static struct omap_hwmod_ocp_if *omap54xx_hwmod_ocp_ifs[] __initdata = {
 	&omap54xx_l4_cfg__dma_system,
 	&omap54xx_l4_abe__dmic,
 	&omap54xx_l4_cfg__mmu_dsp,
+	&omap54xx_l3_main_2__dss,
+	&omap54xx_l3_main_2__dss_dispc,
+	&omap54xx_l3_main_2__dss_dsi1_a,
+	&omap54xx_l3_main_2__dss_dsi1_c,
+	&omap54xx_l3_main_2__dss_hdmi,
+	&omap54xx_l3_main_2__dss_rfbi,
 	&omap54xx_mpu__emif1,
 	&omap54xx_mpu__emif2,
 	&omap54xx_l4_wkup__gpio1,
diff --git a/arch/arm/mach-omap2/omap_hwmod_7xx_data.c b/arch/arm/mach-omap2/omap_hwmod_7xx_data.c
index 810c205d668..20b4398cec0 100644
--- a/arch/arm/mach-omap2/omap_hwmod_7xx_data.c
+++ b/arch/arm/mach-omap2/omap_hwmod_7xx_data.c
@@ -2318,21 +2318,11 @@ static struct omap_hwmod_ocp_if dra7xx_l4_cfg__mpu = {
 	.user		= OCP_USER_MPU | OCP_USER_SDMA,
 };
 
-static struct omap_hwmod_addr_space dra7xx_ocp2scp1_addrs[] = {
-	{
-		.pa_start	= 0x4a080000,
-		.pa_end		= 0x4a08001f,
-		.flags		= ADDR_TYPE_RT
-	},
-	{ }
-};
-
 /* l4_cfg -> ocp2scp1 */
 static struct omap_hwmod_ocp_if dra7xx_l4_cfg__ocp2scp1 = {
 	.master		= &dra7xx_l4_cfg_hwmod,
 	.slave		= &dra7xx_ocp2scp1_hwmod,
 	.clk		= "l4_root_clk_div",
-	.addr		= dra7xx_ocp2scp1_addrs,
 	.user		= OCP_USER_MPU | OCP_USER_SDMA,
 };
 
diff --git a/arch/arm/mach-omap2/omap_phy_internal.c b/arch/arm/mach-omap2/omap_phy_internal.c
index eb8a25de67e..50640b38f0b 100644
--- a/arch/arm/mach-omap2/omap_phy_internal.c
+++ b/arch/arm/mach-omap2/omap_phy_internal.c
@@ -57,7 +57,7 @@ static int __init omap4430_phy_power_down(void)
 	}
 
 	/* Power down the phy */
-	__raw_writel(PHY_PD, ctrl_base + CONTROL_DEV_CONF);
+	writel_relaxed(PHY_PD, ctrl_base + CONTROL_DEV_CONF);
 
 	iounmap(ctrl_base);
 
@@ -162,7 +162,7 @@ void ti81xx_musb_phy_power(u8 on)
 		return;
 	}
 
-	usbphycfg = __raw_readl(scm_base + USBCTRL0);
+	usbphycfg = readl_relaxed(scm_base + USBCTRL0);
 
 	if (on) {
 		if (cpu_is_ti816x()) {
@@ -181,7 +181,7 @@ void ti81xx_musb_phy_power(u8 on)
 			usbphycfg |= USBPHY_CM_PWRDN | USBPHY_OTG_PWRDN;
 
 	}
-	__raw_writel(usbphycfg, scm_base + USBCTRL0);
+	writel_relaxed(usbphycfg, scm_base + USBCTRL0);
 
 	iounmap(scm_base);
 }
diff --git a/arch/arm/mach-omap2/omap_twl.c b/arch/arm/mach-omap2/omap_twl.c
index 615e5b1fb02..6bf62670055 100644
--- a/arch/arm/mach-omap2/omap_twl.c
+++ b/arch/arm/mach-omap2/omap_twl.c
@@ -46,15 +46,8 @@
 
 static bool is_offset_valid;
 static u8 smps_offset;
-/*
- * Flag to ensure Smartreflex bit in TWL
- * being cleared in board file is not overwritten.
- */
-static bool __initdata twl_sr_enable_autoinit;
 
-#define TWL4030_DCDC_GLOBAL_CFG        0x06
 #define REG_SMPS_OFFSET         0xE0
-#define SMARTREFLEX_ENABLE     BIT(3)
 
 static unsigned long twl4030_vsel_to_uv(const u8 vsel)
 {
@@ -251,18 +244,6 @@ int __init omap3_twl_init(void)
 	if (!cpu_is_omap34xx())
 		return -ENODEV;
 
-	/*
-	 * The smartreflex bit on twl4030 specifies if the setting of voltage
-	 * is done over the I2C_SR path. Since this setting is independent of
-	 * the actual usage of smartreflex AVS module, we enable TWL SR bit
-	 * by default irrespective of whether smartreflex AVS module is enabled
-	 * on the OMAP side or not. This is because without this bit enabled,
-	 * the voltage scaling through vp forceupdate/bypass mechanism of
-	 * voltage scaling will not function on TWL over I2C_SR.
-	 */
-	if (!twl_sr_enable_autoinit)
-		omap3_twl_set_sr_bit(true);
-
 	voltdm = voltdm_lookup("mpu_iva");
 	omap_voltage_register_pmic(voltdm, &omap3_mpu_pmic);
 
@@ -271,44 +252,3 @@ int __init omap3_twl_init(void)
 
 	return 0;
 }
-
-/**
- * omap3_twl_set_sr_bit() - Set/Clear SR bit on TWL
- * @enable: enable SR mode in twl or not
- *
- * If 'enable' is true, enables Smartreflex bit on TWL 4030 to make sure
- * voltage scaling through OMAP SR works. Else, the smartreflex bit
- * on twl4030 is cleared as there are platforms which use OMAP3 and T2 but
- * use Synchronized Scaling Hardware Strategy (ENABLE_VMODE=1) and Direct
- * Strategy Software Scaling Mode (ENABLE_VMODE=0), for setting the voltages,
- * in those scenarios this bit is to be cleared (enable = false).
- *
- * Returns 0 on success, error is returned if I2C read/write fails.
- */
-int __init omap3_twl_set_sr_bit(bool enable)
-{
-	u8 temp;
-	int ret;
-	if (twl_sr_enable_autoinit)
-		pr_warning("%s: unexpected multiple calls\n", __func__);
-
-	ret = twl_i2c_read_u8(TWL_MODULE_PM_RECEIVER, &temp,
-			      TWL4030_DCDC_GLOBAL_CFG);
-	if (ret)
-		goto err;
-
-	if (enable)
-		temp |= SMARTREFLEX_ENABLE;
-	else
-		temp &= ~SMARTREFLEX_ENABLE;
-
-	ret = twl_i2c_write_u8(TWL_MODULE_PM_RECEIVER, temp,
-			       TWL4030_DCDC_GLOBAL_CFG);
-	if (!ret) {
-		twl_sr_enable_autoinit = true;
-		return 0;
-	}
-err:
-	pr_err("%s: Error access to TWL4030 (%d)\n", __func__, ret);
-	return ret;
-}
diff --git a/arch/arm/mach-omap2/pdata-quirks.c b/arch/arm/mach-omap2/pdata-quirks.c
index c3b73351cb7..90c88d49848 100644
--- a/arch/arm/mach-omap2/pdata-quirks.c
+++ b/arch/arm/mach-omap2/pdata-quirks.c
@@ -226,6 +226,14 @@ static void __init am3517_evm_legacy_init(void)
 	am35xx_emac_reset();
 }
 
+static struct platform_device omap3_rom_rng_device = {
+	.name		= "omap3-rom-rng",
+	.id		= -1,
+	.dev	= {
+		.platform_data	= rx51_secure_rng_call,
+	},
+};
+
 static void __init nokia_n900_legacy_init(void)
 {
 	hsmmc2_internal_input_clk();
@@ -239,6 +247,10 @@ static void __init nokia_n900_legacy_init(void)
 			pr_warning("RX-51: Not enabling ARM errata 430973 workaround\n");
 			pr_warning("Thumb binaries may crash randomly without this workaround\n");
 		}
+
+		pr_info("RX-51: Registring OMAP3 HWRNG device\n");
+		platform_device_register(&omap3_rom_rng_device);
+
 	}
 }
 #endif /* CONFIG_ARCH_OMAP3 */
@@ -254,6 +266,11 @@ static void __init omap4_panda_legacy_init(void)
 {
 	legacy_init_wl12xx(WL12XX_REFCLOCK_38, 0, 53);
 }
+
+static void __init var_som_om44_legacy_init(void)
+{
+	legacy_init_wl12xx(WL12XX_REFCLOCK_38, 0, 41);
+}
 #endif
 
 #if defined(CONFIG_ARCH_OMAP4) || defined(CONFIG_SOC_OMAP5)
@@ -364,6 +381,8 @@ static struct pdata_init pdata_quirks[] __initdata = {
 #ifdef CONFIG_ARCH_OMAP4
 	{ "ti,omap4-sdp", omap4_sdp_legacy_init, },
 	{ "ti,omap4-panda", omap4_panda_legacy_init, },
+	{ "variscite,var-dvk-om44", var_som_om44_legacy_init, },
+	{ "variscite,var-stk-om44", var_som_om44_legacy_init, },
 #endif
 #ifdef CONFIG_SOC_AM33XX
 	{ "ti,am335x-evmsk", am335x_evmsk_legacy_init, },
diff --git a/arch/arm/mach-omap2/pm.c b/arch/arm/mach-omap2/pm.c
index e1b41416fbf..828aee9ea6a 100644
--- a/arch/arm/mach-omap2/pm.c
+++ b/arch/arm/mach-omap2/pm.c
@@ -32,11 +32,13 @@
 #include "pm.h"
 #include "twl-common.h"
 
+#ifdef CONFIG_SUSPEND
 /*
  * omap_pm_suspend: points to a function that does the SoC-specific
  * suspend work
  */
-int (*omap_pm_suspend)(void);
+static int (*omap_pm_suspend)(void);
+#endif
 
 #ifdef CONFIG_PM
 /**
@@ -243,6 +245,15 @@ static const struct platform_suspend_ops omap_pm_ops = {
 	.valid		= suspend_valid_only_mem,
 };
 
+/**
+ * omap_common_suspend_init - Set common suspend routines for OMAP SoCs
+ * @pm_suspend: function pointer to SoC specific suspend function
+ */
+void omap_common_suspend_init(void *pm_suspend)
+{
+	omap_pm_suspend = pm_suspend;
+	suspend_set_ops(&omap_pm_ops);
+}
 #endif /* CONFIG_SUSPEND */
 
 static void __init omap3_init_voltages(void)
@@ -287,32 +298,24 @@ omap_postcore_initcall(omap2_common_pm_init);
 
 int __init omap2_common_pm_late_init(void)
 {
-	/*
-	 * In the case of DT, the PMIC and SR initialization will be done using
-	 * a completely different mechanism.
-	 * Disable this part if a DT blob is available.
-	 */
-	if (!of_have_populated_dt()) {
-
-		/* Init the voltage layer */
-		omap_pmic_late_init();
-		omap_voltage_late_init();
+	if (of_have_populated_dt()) {
+		omap3_twl_init();
+		omap4_twl_init();
+	}
 
-		/* Initialize the voltages */
-		omap3_init_voltages();
-		omap4_init_voltages();
+	/* Init the voltage layer */
+	omap_pmic_late_init();
+	omap_voltage_late_init();
 
-		/* Smartreflex device init */
-		omap_devinit_smartreflex();
+	/* Initialize the voltages */
+	omap3_init_voltages();
+	omap4_init_voltages();
 
-	}
+	/* Smartreflex device init */
+	omap_devinit_smartreflex();
 
 	/* cpufreq dummy device instantiation */
 	omap_init_cpufreq();
 
-#ifdef CONFIG_SUSPEND
-	suspend_set_ops(&omap_pm_ops);
-#endif
-
 	return 0;
 }
diff --git a/arch/arm/mach-omap2/pm.h b/arch/arm/mach-omap2/pm.h
index d4d0fce325c..e150102d6c0 100644
--- a/arch/arm/mach-omap2/pm.h
+++ b/arch/arm/mach-omap2/pm.h
@@ -34,7 +34,6 @@ extern void *omap3_secure_ram_storage;
 extern void omap3_pm_off_mode_enable(int);
 extern void omap_sram_idle(void);
 extern int omap_pm_clkdms_setup(struct clockdomain *clkdm, void *unused);
-extern int (*omap_pm_suspend)(void);
 
 #if defined(CONFIG_PM_OPP)
 extern int omap3_opp_init(void);
@@ -147,4 +146,11 @@ static inline void omap_pm_get_oscillator(u32 *tstart, u32 *tshut) { *tstart = *
 static inline void omap_pm_setup_sr_i2c_pcb_length(u32 mm) { }
 #endif
 
+#ifdef CONFIG_SUSPEND
+void omap_common_suspend_init(void *pm_suspend);
+#else
+static inline void omap_common_suspend_init(void *pm_suspend)
+{
+}
+#endif /* CONFIG_SUSPEND */
 #endif
diff --git a/arch/arm/mach-omap2/pm24xx.c b/arch/arm/mach-omap2/pm24xx.c
index 8c0759496c8..a5ea988ff34 100644
--- a/arch/arm/mach-omap2/pm24xx.c
+++ b/arch/arm/mach-omap2/pm24xx.c
@@ -229,9 +229,7 @@ static void __init prcm_setup_regs(void)
 	clkdm_for_each(omap_pm_clkdms_setup, NULL);
 	clkdm_add_wkdep(mpu_clkdm, wkup_clkdm);
 
-#ifdef CONFIG_SUSPEND
-	omap_pm_suspend = omap2_enter_full_retention;
-#endif
+	omap_common_suspend_init(omap2_enter_full_retention);
 
 	/* REVISIT: Configure number of 32 kHz clock cycles for sys_clk
 	 * stabilisation */
diff --git a/arch/arm/mach-omap2/pm34xx.c b/arch/arm/mach-omap2/pm34xx.c
index 1f3770a8a72..507d8eeaab9 100644
--- a/arch/arm/mach-omap2/pm34xx.c
+++ b/arch/arm/mach-omap2/pm34xx.c
@@ -50,6 +50,7 @@
 #include "sdrc.h"
 #include "sram.h"
 #include "control.h"
+#include "vc.h"
 
 /* pm34xx errata defined in pm.h */
 u16 pm34xx_errata;
@@ -288,6 +289,9 @@ void omap_sram_idle(void)
 		}
 	}
 
+	/* Configure PMIC signaling for I2C4 or sys_off_mode */
+	omap3_vc_set_pmic_signaling(core_next_state);
+
 	omap3_intc_prepare_idle();
 
 	/*
@@ -330,10 +334,6 @@ void omap_sram_idle(void)
 			omap3_sram_restore_context();
 			omap2_sms_restore_context();
 		}
-		if (core_next_state == PWRDM_POWER_OFF)
-			omap2_prm_clear_mod_reg_bits(OMAP3430_AUTO_OFF_MASK,
-					       OMAP3430_GR_MOD,
-					       OMAP3_PRM_VOLTCTRL_OFFSET);
 	}
 	omap3_intc_resume_idle();
 
@@ -395,7 +395,8 @@ restore:
 
 	return ret;
 }
-
+#else
+#define omap3_pm_suspend NULL
 #endif /* CONFIG_SUSPEND */
 
 
@@ -709,9 +710,7 @@ int __init omap3_pm_init(void)
 	per_clkdm = clkdm_lookup("per_clkdm");
 	wkup_clkdm = clkdm_lookup("wkup_clkdm");
 
-#ifdef CONFIG_SUSPEND
-	omap_pm_suspend = omap3_pm_suspend;
-#endif
+	omap_common_suspend_init(omap3_pm_suspend);
 
 	arm_pm_idle = omap3_pm_idle;
 	omap3_idle_init();
diff --git a/arch/arm/mach-omap2/pm44xx.c b/arch/arm/mach-omap2/pm44xx.c
index eefb30cfcab..0dda6cf8b85 100644
--- a/arch/arm/mach-omap2/pm44xx.c
+++ b/arch/arm/mach-omap2/pm44xx.c
@@ -96,6 +96,8 @@ static int omap4_pm_suspend(void)
 
 	return 0;
 }
+#else
+#define omap4_pm_suspend NULL
 #endif /* CONFIG_SUSPEND */
 
 static int __init pwrdms_setup(struct powerdomain *pwrdm, void *unused)
@@ -251,9 +253,7 @@ int __init omap4_pm_init(void)
 
 	(void) clkdm_for_each(omap_pm_clkdms_setup, NULL);
 
-#ifdef CONFIG_SUSPEND
-	omap_pm_suspend = omap4_pm_suspend;
-#endif
+	omap_common_suspend_init(omap4_pm_suspend);
 
 	/* Overwrite the default cpu_do_idle() */
 	arm_pm_idle = omap_default_idle;
diff --git a/arch/arm/mach-omap2/powerdomain-common.c b/arch/arm/mach-omap2/powerdomain-common.c
index c0aeabfcf00..c40e5f00982 100644
--- a/arch/arm/mach-omap2/powerdomain-common.c
+++ b/arch/arm/mach-omap2/powerdomain-common.c
@@ -17,7 +17,6 @@
 #include "pm.h"
 #include "cm.h"
 #include "cm-regbits-34xx.h"
-#include "cm-regbits-44xx.h"
 #include "prm-regbits-34xx.h"
 #include "prm-regbits-44xx.h"
 
diff --git a/arch/arm/mach-omap2/powerdomain.c b/arch/arm/mach-omap2/powerdomain.c
index 93a2a6e4260..faebd5f076a 100644
--- a/arch/arm/mach-omap2/powerdomain.c
+++ b/arch/arm/mach-omap2/powerdomain.c
@@ -32,6 +32,7 @@
 
 #include "powerdomain.h"
 #include "clockdomain.h"
+#include "voltage.h"
 
 #include "soc.h"
 #include "pm.h"
diff --git a/arch/arm/mach-omap2/powerdomain.h b/arch/arm/mach-omap2/powerdomain.h
index da5a59ae77b..f4727117f6c 100644
--- a/arch/arm/mach-omap2/powerdomain.h
+++ b/arch/arm/mach-omap2/powerdomain.h
@@ -21,8 +21,6 @@
 #include <linux/list.h>
 #include <linux/spinlock.h>
 
-#include "voltage.h"
-
 /* Powerdomain basic power states */
 #define PWRDM_POWER_OFF		0x0
 #define PWRDM_POWER_RET		0x1
@@ -75,6 +73,7 @@
 
 struct clockdomain;
 struct powerdomain;
+struct voltagedomain;
 
 /**
  * struct powerdomain - OMAP powerdomain
diff --git a/arch/arm/mach-omap2/prcm-common.h b/arch/arm/mach-omap2/prcm-common.h
index 0e841fd9498..a8e4b582c52 100644
--- a/arch/arm/mach-omap2/prcm-common.h
+++ b/arch/arm/mach-omap2/prcm-common.h
@@ -428,6 +428,28 @@
 #define MAX_IOPAD_LATCH_TIME			100
 # ifndef __ASSEMBLER__
 
+#include <linux/delay.h>
+
+/**
+ * omap_test_timeout - busy-loop, testing a condition
+ * @cond: condition to test until it evaluates to true
+ * @timeout: maximum number of microseconds in the timeout
+ * @index: loop index (integer)
+ *
+ * Loop waiting for @cond to become true or until at least @timeout
+ * microseconds have passed.  To use, define some integer @index in the
+ * calling code.  After running, if @index == @timeout, then the loop has
+ * timed out.
+ */
+#define omap_test_timeout(cond, timeout, index)			\
+({								\
+	for (index = 0; index < timeout; index++) {		\
+		if (cond)					\
+			break;					\
+		udelay(1);					\
+	}							\
+})
+
 /**
  * struct omap_prcm_irq - describes a PRCM interrupt bit
  * @name: a short name describing the interrupt type, e.g. "wkup" or "io"
@@ -458,6 +480,7 @@ struct omap_prcm_irq {
  * @ocp_barrier: fn ptr to force buffered PRM writes to complete
  * @save_and_clear_irqen: fn ptr to save and clear IRQENABLE regs
  * @restore_irqen: fn ptr to save and clear IRQENABLE regs
+ * @reconfigure_io_chain: fn ptr to reconfigure IO chain
  * @saved_mask: IRQENABLE regs are saved here during suspend
  * @priority_mask: 1 bit per IRQ, set to 1 if omap_prcm_irq.priority = true
  * @base_irq: base dynamic IRQ number, returned from irq_alloc_descs() in init
@@ -479,6 +502,7 @@ struct omap_prcm_irq_setup {
 	void (*ocp_barrier)(void);
 	void (*save_and_clear_irqen)(u32 *saved_mask);
 	void (*restore_irqen)(u32 *saved_mask);
+	void (*reconfigure_io_chain)(void);
 	u32 *saved_mask;
 	u32 *priority_mask;
 	int base_irq;
diff --git a/arch/arm/mach-omap2/prcm_mpu44xx.c b/arch/arm/mach-omap2/prcm_mpu44xx.c
index c30e44a7fab..cdbee6326d2 100644
--- a/arch/arm/mach-omap2/prcm_mpu44xx.c
+++ b/arch/arm/mach-omap2/prcm_mpu44xx.c
@@ -30,12 +30,12 @@ void __iomem *prcm_mpu_base;
 
 u32 omap4_prcm_mpu_read_inst_reg(s16 inst, u16 reg)
 {
-	return __raw_readl(OMAP44XX_PRCM_MPU_REGADDR(inst, reg));
+	return readl_relaxed(OMAP44XX_PRCM_MPU_REGADDR(inst, reg));
 }
 
 void omap4_prcm_mpu_write_inst_reg(u32 val, s16 inst, u16 reg)
 {
-	__raw_writel(val, OMAP44XX_PRCM_MPU_REGADDR(inst, reg));
+	writel_relaxed(val, OMAP44XX_PRCM_MPU_REGADDR(inst, reg));
 }
 
 u32 omap4_prcm_mpu_rmw_inst_reg_bits(u32 mask, u32 bits, s16 inst, s16 reg)
diff --git a/arch/arm/mach-omap2/prcm_mpu44xx.h b/arch/arm/mach-omap2/prcm_mpu44xx.h
index 059bd4f4903..ac9cb455023 100644
--- a/arch/arm/mach-omap2/prcm_mpu44xx.h
+++ b/arch/arm/mach-omap2/prcm_mpu44xx.h
@@ -26,7 +26,6 @@
 #define __ARCH_ARM_MACH_OMAP2_PRCM_MPU44XX_H
 
 #include "prcm_mpu_44xx_54xx.h"
-#include "common.h"
 
 #define OMAP4430_PRCM_MPU_BASE			0x48243000
 
diff --git a/arch/arm/mach-omap2/prm-regbits-34xx.h b/arch/arm/mach-omap2/prm-regbits-34xx.h
index cebad565ed3..106132db532 100644
--- a/arch/arm/mach-omap2/prm-regbits-34xx.h
+++ b/arch/arm/mach-omap2/prm-regbits-34xx.h
@@ -123,8 +123,15 @@
 #define OMAP3430_GLOBAL_SW_RST_SHIFT			1
 #define OMAP3430_GLOBAL_COLD_RST_SHIFT			0
 #define OMAP3430_GLOBAL_COLD_RST_MASK			(1 << 0)
-#define OMAP3430_SEL_OFF_MASK				(1 << 3)
-#define OMAP3430_AUTO_OFF_MASK				(1 << 2)
+#define OMAP3430_PRM_VOLTCTRL_SEL_VMODE			(1 << 4)
+#define OMAP3430_PRM_VOLTCTRL_SEL_OFF			(1 << 3)
+#define OMAP3430_PRM_VOLTCTRL_AUTO_OFF			(1 << 2)
+#define OMAP3430_PRM_VOLTCTRL_AUTO_RET			(1 << 1)
+#define OMAP3430_PRM_VOLTCTRL_AUTO_SLEEP		(1 << 0)
 #define OMAP3430_SETUP_TIME2_MASK			(0xffff << 16)
 #define OMAP3430_SETUP_TIME1_MASK			(0xffff << 0)
+#define OMAP3430_PRM_POLCTRL_OFFMODE_POL		(1 << 3)
+#define OMAP3430_PRM_POLCTRL_CLKOUT_POL			(1 << 2)
+#define OMAP3430_PRM_POLCTRL_CLKREQ_POL			(1 << 1)
+#define OMAP3430_PRM_POLCTRL_EXTVOL_POL			(1 << 0)
 #endif
diff --git a/arch/arm/mach-omap2/prm.h b/arch/arm/mach-omap2/prm.h
index 623db40fdbb..48480d557b6 100644
--- a/arch/arm/mach-omap2/prm.h
+++ b/arch/arm/mach-omap2/prm.h
@@ -17,10 +17,18 @@
 
 # ifndef __ASSEMBLER__
 extern void __iomem *prm_base;
+extern u16 prm_features;
 extern void omap2_set_globals_prm(void __iomem *prm);
 int of_prcm_init(void);
 # endif
 
+/*
+ * prm_features flag values
+ *
+ * PRM_HAS_IO_WAKEUP: has IO wakeup capability
+ * PRM_HAS_VOLTAGE: has voltage domains
+ */
+#define PRM_HAS_IO_WAKEUP	(1 << 0)
 
 /*
  * MAX_MODULE_SOFTRESET_WAIT: Maximum microseconds to wait for OMAP
@@ -118,6 +126,7 @@ struct prm_reset_src_map {
  * @read_reset_sources: ptr to the SoC PRM-specific get_reset_source impl
  * @was_any_context_lost_old: ptr to the SoC PRM context loss test fn
  * @clear_context_loss_flags_old: ptr to the SoC PRM context loss flag clear fn
+ * @late_init: ptr to the late init function
  *
  * XXX @was_any_context_lost_old and @clear_context_loss_flags_old are
  * deprecated.
@@ -126,6 +135,7 @@ struct prm_ll_data {
 	u32 (*read_reset_sources)(void);
 	bool (*was_any_context_lost_old)(u8 part, s16 inst, u16 idx);
 	void (*clear_context_loss_flags_old)(u8 part, s16 inst, u16 idx);
+	int (*late_init)(void);
 };
 
 extern int prm_register(struct prm_ll_data *pld);
diff --git a/arch/arm/mach-omap2/prm2xxx.c b/arch/arm/mach-omap2/prm2xxx.c
index 418de9c3b31..a3a3cca2bcc 100644
--- a/arch/arm/mach-omap2/prm2xxx.c
+++ b/arch/arm/mach-omap2/prm2xxx.c
@@ -18,9 +18,6 @@
 #include <linux/io.h>
 #include <linux/irq.h>
 
-#include "soc.h"
-#include "common.h"
-#include "vp.h"
 #include "powerdomain.h"
 #include "clockdomain.h"
 #include "prm2xxx.h"
@@ -201,19 +198,11 @@ static struct prm_ll_data omap2xxx_prm_ll_data = {
 
 int __init omap2xxx_prm_init(void)
 {
-	if (!cpu_is_omap24xx())
-		return 0;
-
 	return prm_register(&omap2xxx_prm_ll_data);
 }
 
 static void __exit omap2xxx_prm_exit(void)
 {
-	if (!cpu_is_omap24xx())
-		return;
-
-	/* Should never happen */
-	WARN(prm_unregister(&omap2xxx_prm_ll_data),
-	     "%s: prm_ll_data function pointer mismatch\n", __func__);
+	prm_unregister(&omap2xxx_prm_ll_data);
 }
 __exitcall(omap2xxx_prm_exit);
diff --git a/arch/arm/mach-omap2/prm2xxx.h b/arch/arm/mach-omap2/prm2xxx.h
index 3194dd87e0e..d2cb6365716 100644
--- a/arch/arm/mach-omap2/prm2xxx.h
+++ b/arch/arm/mach-omap2/prm2xxx.h
@@ -27,7 +27,7 @@
 
 /*
  * OMAP2-specific global PRM registers
- * Use __raw_{read,write}l() with these registers.
+ * Use {read,write}l_relaxed() with these registers.
  *
  * With a few exceptions, these are the register names beginning with
  * PRCM_* on 24xx.  (The exceptions are the IRQSTATUS and IRQENABLE
diff --git a/arch/arm/mach-omap2/prm2xxx_3xxx.c b/arch/arm/mach-omap2/prm2xxx_3xxx.c
index 947f6adfed0..c13b4e293ff 100644
--- a/arch/arm/mach-omap2/prm2xxx_3xxx.c
+++ b/arch/arm/mach-omap2/prm2xxx_3xxx.c
@@ -16,7 +16,6 @@
 #include <linux/err.h>
 #include <linux/io.h>
 
-#include "common.h"
 #include "powerdomain.h"
 #include "prm2xxx_3xxx.h"
 #include "prm-regbits-24xx.h"
diff --git a/arch/arm/mach-omap2/prm2xxx_3xxx.h b/arch/arm/mach-omap2/prm2xxx_3xxx.h
index 9624b40836d..1a3a96392b9 100644
--- a/arch/arm/mach-omap2/prm2xxx_3xxx.h
+++ b/arch/arm/mach-omap2/prm2xxx_3xxx.h
@@ -55,12 +55,12 @@
 /* Power/reset management domain register get/set */
 static inline u32 omap2_prm_read_mod_reg(s16 module, u16 idx)
 {
-	return __raw_readl(prm_base + module + idx);
+	return readl_relaxed(prm_base + module + idx);
 }
 
 static inline void omap2_prm_write_mod_reg(u32 val, s16 module, u16 idx)
 {
-	__raw_writel(val, prm_base + module + idx);
+	writel_relaxed(val, prm_base + module + idx);
 }
 
 /* Read-modify-write a register in a PRM module. Caller must lock */
diff --git a/arch/arm/mach-omap2/prm33xx.c b/arch/arm/mach-omap2/prm33xx.c
index 72044073774..62709cd2f9c 100644
--- a/arch/arm/mach-omap2/prm33xx.c
+++ b/arch/arm/mach-omap2/prm33xx.c
@@ -19,7 +19,6 @@
 #include <linux/err.h>
 #include <linux/io.h>
 
-#include "common.h"
 #include "powerdomain.h"
 #include "prm33xx.h"
 #include "prm-regbits-33xx.h"
@@ -27,13 +26,13 @@
 /* Read a register in a PRM instance */
 u32 am33xx_prm_read_reg(s16 inst, u16 idx)
 {
-	return __raw_readl(prm_base + inst + idx);
+	return readl_relaxed(prm_base + inst + idx);
 }
 
 /* Write into a register in a PRM instance */
 void am33xx_prm_write_reg(u32 val, s16 inst, u16 idx)
 {
-	__raw_writel(val, prm_base + inst + idx);
+	writel_relaxed(val, prm_base + inst + idx);
 }
 
 /* Read-modify-write a register in PRM. Caller must lock */
diff --git a/arch/arm/mach-omap2/prm3xxx.c b/arch/arm/mach-omap2/prm3xxx.c
index 7721990d200..4bd7a2dca8a 100644
--- a/arch/arm/mach-omap2/prm3xxx.c
+++ b/arch/arm/mach-omap2/prm3xxx.c
@@ -43,6 +43,7 @@ static struct omap_prcm_irq_setup omap3_prcm_irq_setup = {
 	.ocp_barrier		= &omap3xxx_prm_ocp_barrier,
 	.save_and_clear_irqen	= &omap3xxx_prm_save_and_clear_irqen,
 	.restore_irqen		= &omap3xxx_prm_restore_irqen,
+	.reconfigure_io_chain	= &omap3xxx_prm_reconfigure_io_chain,
 };
 
 /*
@@ -246,7 +247,7 @@ void omap3xxx_prm_reconfigure_io_chain(void)
  */
 static void __init omap3xxx_prm_enable_io_wakeup(void)
 {
-	if (omap3_has_io_wakeup())
+	if (prm_features & PRM_HAS_IO_WAKEUP)
 		omap2_prm_set_mod_reg_bits(OMAP3430_EN_IO_MASK, WKUP_MOD,
 					   PM_WKEN);
 }
@@ -400,23 +401,26 @@ struct pwrdm_ops omap3_pwrdm_operations = {
  *
  */
 
+static int omap3xxx_prm_late_init(void);
+
 static struct prm_ll_data omap3xxx_prm_ll_data = {
 	.read_reset_sources = &omap3xxx_prm_read_reset_sources,
+	.late_init = &omap3xxx_prm_late_init,
 };
 
 int __init omap3xxx_prm_init(void)
 {
-	if (!cpu_is_omap34xx())
-		return 0;
+	if (omap3_has_io_wakeup())
+		prm_features |= PRM_HAS_IO_WAKEUP;
 
 	return prm_register(&omap3xxx_prm_ll_data);
 }
 
-static int __init omap3xxx_prm_late_init(void)
+static int omap3xxx_prm_late_init(void)
 {
 	int ret;
 
-	if (!cpu_is_omap34xx())
+	if (!(prm_features & PRM_HAS_IO_WAKEUP))
 		return 0;
 
 	omap3xxx_prm_enable_io_wakeup();
@@ -427,15 +431,9 @@ static int __init omap3xxx_prm_late_init(void)
 
 	return ret;
 }
-omap_subsys_initcall(omap3xxx_prm_late_init);
 
 static void __exit omap3xxx_prm_exit(void)
 {
-	if (!cpu_is_omap34xx())
-		return;
-
-	/* Should never happen */
-	WARN(prm_unregister(&omap3xxx_prm_ll_data),
-	     "%s: prm_ll_data function pointer mismatch\n", __func__);
+	prm_unregister(&omap3xxx_prm_ll_data);
 }
 __exitcall(omap3xxx_prm_exit);
diff --git a/arch/arm/mach-omap2/prm3xxx.h b/arch/arm/mach-omap2/prm3xxx.h
index f8eb83323b1..1dacfc5b195 100644
--- a/arch/arm/mach-omap2/prm3xxx.h
+++ b/arch/arm/mach-omap2/prm3xxx.h
@@ -26,7 +26,7 @@
 
 /*
  * OMAP3-specific global PRM registers
- * Use __raw_{read,write}l() with these registers.
+ * Use {read,write}l_relaxed() with these registers.
  *
  * With a few exceptions, these are the register names beginning with
  * PRM_* on 34xx.  (The exceptions are the IRQSTATUS and IRQENABLE
diff --git a/arch/arm/mach-omap2/prm44xx.c b/arch/arm/mach-omap2/prm44xx.c
index 03a603476cf..a7f6ea27180 100644
--- a/arch/arm/mach-omap2/prm44xx.c
+++ b/arch/arm/mach-omap2/prm44xx.c
@@ -47,6 +47,7 @@ static struct omap_prcm_irq_setup omap4_prcm_irq_setup = {
 	.ocp_barrier		= &omap44xx_prm_ocp_barrier,
 	.save_and_clear_irqen	= &omap44xx_prm_save_and_clear_irqen,
 	.restore_irqen		= &omap44xx_prm_restore_irqen,
+	.reconfigure_io_chain	= &omap44xx_prm_reconfigure_io_chain,
 };
 
 /*
@@ -81,13 +82,13 @@ static struct prm_reset_src_map omap44xx_prm_reset_src_map[] = {
 /* Read a register in a CM/PRM instance in the PRM module */
 u32 omap4_prm_read_inst_reg(s16 inst, u16 reg)
 {
-	return __raw_readl(prm_base + inst + reg);
+	return readl_relaxed(prm_base + inst + reg);
 }
 
 /* Write into a register in a CM/PRM instance in the PRM module */
 void omap4_prm_write_inst_reg(u32 val, s16 inst, u16 reg)
 {
-	__raw_writel(val, prm_base + inst + reg);
+	writel_relaxed(val, prm_base + inst + reg);
 }
 
 /* Read-modify-write a register in a PRM module. Caller must lock */
@@ -649,6 +650,8 @@ struct pwrdm_ops omap4_pwrdm_operations = {
 	.pwrdm_has_voltdm	= omap4_check_vcvp,
 };
 
+static int omap44xx_prm_late_init(void);
+
 /*
  * XXX document
  */
@@ -656,34 +659,29 @@ static struct prm_ll_data omap44xx_prm_ll_data = {
 	.read_reset_sources = &omap44xx_prm_read_reset_sources,
 	.was_any_context_lost_old = &omap44xx_prm_was_any_context_lost_old,
 	.clear_context_loss_flags_old = &omap44xx_prm_clear_context_loss_flags_old,
+	.late_init = &omap44xx_prm_late_init,
 };
 
 int __init omap44xx_prm_init(void)
 {
-	if (!cpu_is_omap44xx() && !soc_is_omap54xx() && !soc_is_dra7xx())
-		return 0;
+	if (cpu_is_omap44xx())
+		prm_features |= PRM_HAS_IO_WAKEUP;
 
 	return prm_register(&omap44xx_prm_ll_data);
 }
 
-static int __init omap44xx_prm_late_init(void)
+static int omap44xx_prm_late_init(void)
 {
-	if (!cpu_is_omap44xx())
+	if (!(prm_features & PRM_HAS_IO_WAKEUP))
 		return 0;
 
 	omap44xx_prm_enable_io_wakeup();
 
 	return omap_prcm_register_chain_handler(&omap4_prcm_irq_setup);
 }
-omap_subsys_initcall(omap44xx_prm_late_init);
 
 static void __exit omap44xx_prm_exit(void)
 {
-	if (!cpu_is_omap44xx())
-		return;
-
-	/* Should never happen */
-	WARN(prm_unregister(&omap44xx_prm_ll_data),
-	     "%s: prm_ll_data function pointer mismatch\n", __func__);
+	prm_unregister(&omap44xx_prm_ll_data);
 }
 __exitcall(omap44xx_prm_exit);
diff --git a/arch/arm/mach-omap2/prm_common.c b/arch/arm/mach-omap2/prm_common.c
index b4c4ab9c804..25e8b823211 100644
--- a/arch/arm/mach-omap2/prm_common.c
+++ b/arch/arm/mach-omap2/prm_common.c
@@ -62,6 +62,8 @@ static struct omap_prcm_irq_setup *prcm_irq_setup;
 /* prm_base: base virtual address of the PRM IP block */
 void __iomem *prm_base;
 
+u16 prm_features;
+
 /*
  * prm_ll_data: function pointers to SoC-specific implementations of
  * common PRM functions
@@ -330,12 +332,7 @@ int omap_prcm_register_chain_handler(struct omap_prcm_irq_setup *irq_setup)
 
 	if (of_have_populated_dt()) {
 		int irq = omap_prcm_event_to_irq("io");
-		if (cpu_is_omap34xx())
-			omap_pcs_legacy_init(irq,
-				omap3xxx_prm_reconfigure_io_chain);
-		else
-			omap_pcs_legacy_init(irq,
-				omap44xx_prm_reconfigure_io_chain);
+		omap_pcs_legacy_init(irq, irq_setup->reconfigure_io_chain);
 	}
 
 	return 0;
@@ -530,3 +527,11 @@ int __init of_prcm_init(void)
 
 	return 0;
 }
+
+static int __init prm_late_init(void)
+{
+	if (prm_ll_data->late_init)
+		return prm_ll_data->late_init();
+	return 0;
+}
+subsys_initcall(prm_late_init);
diff --git a/arch/arm/mach-omap2/prminst44xx.c b/arch/arm/mach-omap2/prminst44xx.c
index 05fcf6de44e..69f0dd08629 100644
--- a/arch/arm/mach-omap2/prminst44xx.c
+++ b/arch/arm/mach-omap2/prminst44xx.c
@@ -49,7 +49,7 @@ u32 omap4_prminst_read_inst_reg(u8 part, s16 inst, u16 idx)
 	BUG_ON(part >= OMAP4_MAX_PRCM_PARTITIONS ||
 	       part == OMAP4430_INVALID_PRCM_PARTITION ||
 	       !_prm_bases[part]);
-	return __raw_readl(_prm_bases[part] + inst + idx);
+	return readl_relaxed(_prm_bases[part] + inst + idx);
 }
 
 /* Write into a register in a PRM instance */
@@ -58,7 +58,7 @@ void omap4_prminst_write_inst_reg(u32 val, u8 part, s16 inst, u16 idx)
 	BUG_ON(part >= OMAP4_MAX_PRCM_PARTITIONS ||
 	       part == OMAP4430_INVALID_PRCM_PARTITION ||
 	       !_prm_bases[part]);
-	__raw_writel(val, _prm_bases[part] + inst + idx);
+	writel_relaxed(val, _prm_bases[part] + inst + idx);
 }
 
 /* Read-modify-write a register in PRM. Caller must lock */
diff --git a/arch/arm/mach-omap2/sdrc.h b/arch/arm/mach-omap2/sdrc.h
index 446aa13511f..645a2a46b21 100644
--- a/arch/arm/mach-omap2/sdrc.h
+++ b/arch/arm/mach-omap2/sdrc.h
@@ -31,24 +31,24 @@ extern void __iomem *omap2_sms_base;
 
 static inline void sdrc_write_reg(u32 val, u16 reg)
 {
-	__raw_writel(val, OMAP_SDRC_REGADDR(reg));
+	writel_relaxed(val, OMAP_SDRC_REGADDR(reg));
 }
 
 static inline u32 sdrc_read_reg(u16 reg)
 {
-	return __raw_readl(OMAP_SDRC_REGADDR(reg));
+	return readl_relaxed(OMAP_SDRC_REGADDR(reg));
 }
 
 /* SMS global register get/set */
 
 static inline void sms_write_reg(u32 val, u16 reg)
 {
-	__raw_writel(val, OMAP_SMS_REGADDR(reg));
+	writel_relaxed(val, OMAP_SMS_REGADDR(reg));
 }
 
 static inline u32 sms_read_reg(u16 reg)
 {
-	return __raw_readl(OMAP_SMS_REGADDR(reg));
+	return readl_relaxed(OMAP_SMS_REGADDR(reg));
 }
 
 extern void omap2_set_globals_sdrc(void __iomem *sdrc, void __iomem *sms);
diff --git a/arch/arm/mach-omap2/sdrc2xxx.c b/arch/arm/mach-omap2/sdrc2xxx.c
index 90729171464..ae3f1553158 100644
--- a/arch/arm/mach-omap2/sdrc2xxx.c
+++ b/arch/arm/mach-omap2/sdrc2xxx.c
@@ -103,9 +103,9 @@ u32 omap2xxx_sdrc_reprogram(u32 level, u32 force)
 	 * prm2xxx.c function
 	 */
 	if (cpu_is_omap2420())
-		__raw_writel(0xffff, OMAP2420_PRCM_VOLTSETUP);
+		writel_relaxed(0xffff, OMAP2420_PRCM_VOLTSETUP);
 	else
-		__raw_writel(0xffff, OMAP2430_PRCM_VOLTSETUP);
+		writel_relaxed(0xffff, OMAP2430_PRCM_VOLTSETUP);
 	omap2_sram_reprogram_sdrc(level, dll_ctrl, m_type);
 	curr_perf_level = level;
 	local_irq_restore(flags);
diff --git a/arch/arm/mach-omap2/soc.h b/arch/arm/mach-omap2/soc.h
index 30abcc8b20e..de2a34c423a 100644
--- a/arch/arm/mach-omap2/soc.h
+++ b/arch/arm/mach-omap2/soc.h
@@ -459,10 +459,15 @@ IS_OMAP_TYPE(3430, 0x3430)
 #define OMAP5430_REV_ES2_0	(OMAP54XX_CLASS | (0x30 << 16) | (0x20 << 8))
 #define OMAP5432_REV_ES2_0	(OMAP54XX_CLASS | (0x32 << 16) | (0x20 << 8))
 
+#define DRA7XX_CLASS		0x07000000
+#define DRA752_REV_ES1_0	(DRA7XX_CLASS | (0x52 << 16) | (0x10 << 8))
+#define DRA752_REV_ES1_1	(DRA7XX_CLASS | (0x52 << 16) | (0x11 << 8))
+
 void omap2xxx_check_revision(void);
 void omap3xxx_check_revision(void);
 void omap4xxx_check_revision(void);
 void omap5xxx_check_revision(void);
+void dra7xxx_check_revision(void);
 void omap3xxx_check_features(void);
 void ti81xx_check_features(void);
 void am33xx_check_features(void);
diff --git a/arch/arm/mach-omap2/sr_device.c b/arch/arm/mach-omap2/sr_device.c
index d7bc33f1534..1b91ef0c182 100644
--- a/arch/arm/mach-omap2/sr_device.c
+++ b/arch/arm/mach-omap2/sr_device.c
@@ -57,7 +57,7 @@ static void __init sr_set_nvalues(struct omap_volt_data *volt_data,
 
 		/*
 		 * In OMAP4 the efuse registers are 24 bit aligned.
-		 * A __raw_readl will fail for non-32 bit aligned address
+		 * A readl_relaxed will fail for non-32 bit aligned address
 		 * and hence the 8-bit read and shift.
 		 */
 		if (cpu_is_omap44xx()) {
diff --git a/arch/arm/mach-omap2/sram.c b/arch/arm/mach-omap2/sram.c
index 4bd09683623..ddf1818af22 100644
--- a/arch/arm/mach-omap2/sram.c
+++ b/arch/arm/mach-omap2/sram.c
@@ -70,16 +70,16 @@ static int is_sram_locked(void)
 	if (OMAP2_DEVICE_TYPE_GP == omap_type()) {
 		/* RAMFW: R/W access to all initiators for all qualifier sets */
 		if (cpu_is_omap242x()) {
-			__raw_writel(0xFF, OMAP24XX_VA_REQINFOPERM0); /* all q-vects */
-			__raw_writel(0xCFDE, OMAP24XX_VA_READPERM0);  /* all i-read */
-			__raw_writel(0xCFDE, OMAP24XX_VA_WRITEPERM0); /* all i-write */
+			writel_relaxed(0xFF, OMAP24XX_VA_REQINFOPERM0); /* all q-vects */
+			writel_relaxed(0xCFDE, OMAP24XX_VA_READPERM0);  /* all i-read */
+			writel_relaxed(0xCFDE, OMAP24XX_VA_WRITEPERM0); /* all i-write */
 		}
 		if (cpu_is_omap34xx()) {
-			__raw_writel(0xFFFF, OMAP34XX_VA_REQINFOPERM0); /* all q-vects */
-			__raw_writel(0xFFFF, OMAP34XX_VA_READPERM0);  /* all i-read */
-			__raw_writel(0xFFFF, OMAP34XX_VA_WRITEPERM0); /* all i-write */
-			__raw_writel(0x0, OMAP34XX_VA_ADDR_MATCH2);
-			__raw_writel(0xFFFFFFFF, OMAP34XX_VA_SMS_RG_ATT0);
+			writel_relaxed(0xFFFF, OMAP34XX_VA_REQINFOPERM0); /* all q-vects */
+			writel_relaxed(0xFFFF, OMAP34XX_VA_READPERM0);  /* all i-read */
+			writel_relaxed(0xFFFF, OMAP34XX_VA_WRITEPERM0); /* all i-write */
+			writel_relaxed(0x0, OMAP34XX_VA_ADDR_MATCH2);
+			writel_relaxed(0xFFFFFFFF, OMAP34XX_VA_SMS_RG_ATT0);
 		}
 		return 0;
 	} else
diff --git a/arch/arm/mach-omap2/timer.c b/arch/arm/mach-omap2/timer.c
index b62de9f9d05..43d03fbf4c0 100644
--- a/arch/arm/mach-omap2/timer.c
+++ b/arch/arm/mach-omap2/timer.c
@@ -361,7 +361,7 @@ static void __init omap2_gp_clockevent_init(int gptimer_id,
 
 /* Clocksource code */
 static struct omap_dm_timer clksrc;
-static bool use_gptimer_clksrc;
+static bool use_gptimer_clksrc __initdata;
 
 /*
  * clocksource
@@ -546,15 +546,15 @@ static void __init realtime_counter_init(void)
 	}
 
 	/* Program numerator and denumerator registers */
-	reg = __raw_readl(base + INCREMENTER_NUMERATOR_OFFSET) &
+	reg = readl_relaxed(base + INCREMENTER_NUMERATOR_OFFSET) &
 			NUMERATOR_DENUMERATOR_MASK;
 	reg |= num;
-	__raw_writel(reg, base + INCREMENTER_NUMERATOR_OFFSET);
+	writel_relaxed(reg, base + INCREMENTER_NUMERATOR_OFFSET);
 
-	reg = __raw_readl(base + INCREMENTER_DENUMERATOR_RELOAD_OFFSET) &
+	reg = readl_relaxed(base + INCREMENTER_DENUMERATOR_RELOAD_OFFSET) &
 			NUMERATOR_DENUMERATOR_MASK;
 	reg |= den;
-	__raw_writel(reg, base + INCREMENTER_DENUMERATOR_RELOAD_OFFSET);
+	writel_relaxed(reg, base + INCREMENTER_DENUMERATOR_RELOAD_OFFSET);
 
 	arch_timer_freq = (rate / den) * num;
 	set_cntfreq();
diff --git a/arch/arm/mach-omap2/usb-host.c b/arch/arm/mach-omap2/usb-host.c
index 10855eb4ccc..745367c0c2b 100644
--- a/arch/arm/mach-omap2/usb-host.c
+++ b/arch/arm/mach-omap2/usb-host.c
@@ -28,7 +28,7 @@
 #include <linux/io.h>
 #include <linux/gpio.h>
 #include <linux/usb/phy.h>
-#include <linux/usb/usb_phy_gen_xceiv.h>
+#include <linux/usb/usb_phy_generic.h>
 
 #include "soc.h"
 #include "omap_device.h"
@@ -349,7 +349,7 @@ static struct fixed_voltage_config hsusb_reg_config = {
 	/* .init_data filled later */
 };
 
-static const char *nop_name = "usb_phy_gen_xceiv"; /* NOP PHY driver */
+static const char *nop_name = "usb_phy_generic"; /* NOP PHY driver */
 static const char *reg_name = "reg-fixed-voltage"; /* Regulator driver */
 
 /**
@@ -435,7 +435,7 @@ int usbhs_init_phys(struct usbhs_phy_data *phy, int num_phys)
 	struct platform_device *pdev;
 	char *phy_id;
 	struct platform_device_info pdevinfo;
-	struct usb_phy_gen_xceiv_platform_data nop_pdata;
+	struct usb_phy_generic_platform_data nop_pdata;
 
 	for (i = 0; i < num_phys; i++) {
 
@@ -469,8 +469,8 @@ int usbhs_init_phys(struct usbhs_phy_data *phy, int num_phys)
 		pdevinfo.id = phy->port;
 		pdevinfo.data = &nop_pdata;
 		pdevinfo.size_data =
-			sizeof(struct usb_phy_gen_xceiv_platform_data);
-		scnprintf(phy_id, MAX_STR, "usb_phy_gen_xceiv.%d",
+			sizeof(struct usb_phy_generic_platform_data);
+		scnprintf(phy_id, MAX_STR, "usb_phy_generic.%d",
 					phy->port);
 		pdev = platform_device_register_full(&pdevinfo);
 		if (IS_ERR(pdev)) {
diff --git a/arch/arm/mach-omap2/vc.c b/arch/arm/mach-omap2/vc.c
index 49ac7977e03..a4628a9e760 100644
--- a/arch/arm/mach-omap2/vc.c
+++ b/arch/arm/mach-omap2/vc.c
@@ -220,10 +220,126 @@ static inline u32 omap_usec_to_32k(u32 usec)
 	return DIV_ROUND_UP_ULL(32768ULL * (u64)usec, 1000000ULL);
 }
 
-/* Set oscillator setup time for omap3 */
-static void omap3_set_clksetup(u32 usec, struct voltagedomain *voltdm)
+struct omap3_vc_timings {
+	u32 voltsetup1;
+	u32 voltsetup2;
+};
+
+struct omap3_vc {
+	struct voltagedomain *vd;
+	u32 voltctrl;
+	u32 voltsetup1;
+	u32 voltsetup2;
+	struct omap3_vc_timings timings[2];
+};
+static struct omap3_vc vc;
+
+void omap3_vc_set_pmic_signaling(int core_next_state)
+{
+	struct voltagedomain *vd = vc.vd;
+	struct omap3_vc_timings *c = vc.timings;
+	u32 voltctrl, voltsetup1, voltsetup2;
+
+	voltctrl = vc.voltctrl;
+	voltsetup1 = vc.voltsetup1;
+	voltsetup2 = vc.voltsetup2;
+
+	switch (core_next_state) {
+	case PWRDM_POWER_OFF:
+		voltctrl &= ~(OMAP3430_PRM_VOLTCTRL_AUTO_RET |
+			      OMAP3430_PRM_VOLTCTRL_AUTO_SLEEP);
+		voltctrl |= OMAP3430_PRM_VOLTCTRL_AUTO_OFF;
+		if (voltctrl & OMAP3430_PRM_VOLTCTRL_SEL_OFF)
+			voltsetup2 = c->voltsetup2;
+		else
+			voltsetup1 = c->voltsetup1;
+		break;
+	case PWRDM_POWER_RET:
+	default:
+		c++;
+		voltctrl &= ~(OMAP3430_PRM_VOLTCTRL_AUTO_OFF |
+			      OMAP3430_PRM_VOLTCTRL_AUTO_SLEEP);
+		voltctrl |= OMAP3430_PRM_VOLTCTRL_AUTO_RET;
+		voltsetup1 = c->voltsetup1;
+		break;
+	}
+
+	if (voltctrl != vc.voltctrl) {
+		vd->write(voltctrl, OMAP3_PRM_VOLTCTRL_OFFSET);
+		vc.voltctrl = voltctrl;
+	}
+	if (voltsetup1 != vc.voltsetup1) {
+		vd->write(c->voltsetup1,
+			  OMAP3_PRM_VOLTSETUP1_OFFSET);
+		vc.voltsetup1 = voltsetup1;
+	}
+	if (voltsetup2 != vc.voltsetup2) {
+		vd->write(c->voltsetup2,
+			  OMAP3_PRM_VOLTSETUP2_OFFSET);
+		vc.voltsetup2 = voltsetup2;
+	}
+}
+
+#define PRM_POLCTRL_TWL_MASK	(OMAP3430_PRM_POLCTRL_CLKREQ_POL | \
+					OMAP3430_PRM_POLCTRL_CLKREQ_POL)
+#define PRM_POLCTRL_TWL_VAL	OMAP3430_PRM_POLCTRL_CLKREQ_POL
+
+/*
+ * Configure signal polarity for sys_clkreq and sys_off_mode pins
+ * as the default values are wrong and can cause the system to hang
+ * if any twl4030 scripts are loaded.
+ */
+static void __init omap3_vc_init_pmic_signaling(struct voltagedomain *voltdm)
+{
+	u32 val;
+
+	if (vc.vd)
+		return;
+
+	vc.vd = voltdm;
+
+	val = voltdm->read(OMAP3_PRM_POLCTRL_OFFSET);
+	if (!(val & OMAP3430_PRM_POLCTRL_CLKREQ_POL) ||
+	    (val & OMAP3430_PRM_POLCTRL_CLKREQ_POL)) {
+		val |= OMAP3430_PRM_POLCTRL_CLKREQ_POL;
+		val &= ~OMAP3430_PRM_POLCTRL_OFFMODE_POL;
+		pr_debug("PM: fixing sys_clkreq and sys_off_mode polarity to 0x%x\n",
+			 val);
+		voltdm->write(val, OMAP3_PRM_POLCTRL_OFFSET);
+	}
+
+	/*
+	 * By default let's use I2C4 signaling for retention idle
+	 * and sys_off_mode pin signaling for off idle. This way we
+	 * have sys_clk_req pin go down for retention and both
+	 * sys_clk_req and sys_off_mode pins will go down for off
+	 * idle. And we can also scale voltages to zero for off-idle.
+	 * Note that no actual voltage scaling during off-idle will
+	 * happen unless the board specific twl4030 PMIC scripts are
+	 * loaded.
+	 */
+	val = voltdm->read(OMAP3_PRM_VOLTCTRL_OFFSET);
+	if (!(val & OMAP3430_PRM_VOLTCTRL_SEL_OFF)) {
+		val |= OMAP3430_PRM_VOLTCTRL_SEL_OFF;
+		pr_debug("PM: setting voltctrl sys_off_mode signaling to 0x%x\n",
+			 val);
+		voltdm->write(val, OMAP3_PRM_VOLTCTRL_OFFSET);
+	}
+	vc.voltctrl = val;
+
+	omap3_vc_set_pmic_signaling(PWRDM_POWER_ON);
+}
+
+static void omap3_init_voltsetup1(struct voltagedomain *voltdm,
+				  struct omap3_vc_timings *c, u32 idle)
 {
-	voltdm->write(omap_usec_to_32k(usec), OMAP3_PRM_CLKSETUP_OFFSET);
+	unsigned long val;
+
+	val = (voltdm->vc_param->on - idle) / voltdm->pmic->slew_rate;
+	val *= voltdm->sys_clk.rate / 8 / 1000000 + 1;
+	val <<= __ffs(voltdm->vfsm->voltsetup_mask);
+	c->voltsetup1 &= ~voltdm->vfsm->voltsetup_mask;
+	c->voltsetup1 |= val;
 }
 
 /**
@@ -236,37 +352,21 @@ static void omap3_set_clksetup(u32 usec, struct voltagedomain *voltdm)
  * or retention. Off mode has additionally an option to use sys_off_mode
  * pad, which uses a global signal to program the whole power IC to
  * off-mode.
+ *
+ * Note that pmic is not controlling the voltage scaling during
+ * retention signaled over I2C4, so we can keep voltsetup2 as 0.
+ * And the oscillator is not shut off over I2C4, so no need to
+ * set clksetup.
  */
-static void omap3_set_i2c_timings(struct voltagedomain *voltdm, bool off_mode)
+static void omap3_set_i2c_timings(struct voltagedomain *voltdm)
 {
-	unsigned long voltsetup1;
-	u32 tgt_volt;
-
-	/*
-	 * Oscillator is shut down only if we are using sys_off_mode pad,
-	 * thus we set a minimal setup time here
-	 */
-	omap3_set_clksetup(1, voltdm);
+	struct omap3_vc_timings *c = vc.timings;
 
-	if (off_mode)
-		tgt_volt = voltdm->vc_param->off;
-	else
-		tgt_volt = voltdm->vc_param->ret;
-
-	voltsetup1 = (voltdm->vc_param->on - tgt_volt) /
-			voltdm->pmic->slew_rate;
-
-	voltsetup1 = voltsetup1 * voltdm->sys_clk.rate / 8 / 1000000 + 1;
-
-	voltdm->rmw(voltdm->vfsm->voltsetup_mask,
-		voltsetup1 << __ffs(voltdm->vfsm->voltsetup_mask),
-		voltdm->vfsm->voltsetup_reg);
-
-	/*
-	 * pmic is not controlling the voltage scaling during retention,
-	 * thus set voltsetup2 to 0
-	 */
-	voltdm->write(0, OMAP3_PRM_VOLTSETUP2_OFFSET);
+	/* Configure PRWDM_POWER_OFF over I2C4 */
+	omap3_init_voltsetup1(voltdm, c, voltdm->vc_param->off);
+	c++;
+	/* Configure PRWDM_POWER_RET over I2C4 */
+	omap3_init_voltsetup1(voltdm, c, voltdm->vc_param->ret);
 }
 
 /**
@@ -275,69 +375,49 @@ static void omap3_set_i2c_timings(struct voltagedomain *voltdm, bool off_mode)
  *
  * Calculates and sets up off-mode timings for a channel. Off-mode
  * can use either I2C based voltage scaling, or alternatively
- * sys_off_mode pad can be used to send a global command to power IC.
- * This function first checks which mode is being used, and calls
- * omap3_set_i2c_timings() if the system is using I2C control mode.
+ * sys_off_mode pad can be used to send a global command to power IC.n,
  * sys_off_mode has the additional benefit that voltages can be
  * scaled to zero volt level with TWL4030 / TWL5030, I2C can only
  * scale to 600mV.
+ *
+ * Note that omap is not controlling the voltage scaling during
+ * off idle signaled by sys_off_mode, so we can keep voltsetup1
+ * as 0.
  */
 static void omap3_set_off_timings(struct voltagedomain *voltdm)
 {
-	unsigned long clksetup;
-	unsigned long voltsetup2;
-	unsigned long voltsetup2_old;
-	u32 val;
-	u32 tstart, tshut;
+	struct omap3_vc_timings *c = vc.timings;
+	u32 tstart, tshut, clksetup, voltoffset;
 
-	/* check if sys_off_mode is used to control off-mode voltages */
-	val = voltdm->read(OMAP3_PRM_VOLTCTRL_OFFSET);
-	if (!(val & OMAP3430_SEL_OFF_MASK)) {
-		/* No, omap is controlling them over I2C */
-		omap3_set_i2c_timings(voltdm, true);
+	if (c->voltsetup2)
 		return;
-	}
 
 	omap_pm_get_oscillator(&tstart, &tshut);
-	omap3_set_clksetup(tstart, voltdm);
-
-	clksetup = voltdm->read(OMAP3_PRM_CLKSETUP_OFFSET);
-
-	/* voltsetup 2 in us */
-	voltsetup2 = voltdm->vc_param->on / voltdm->pmic->slew_rate;
-
-	/* convert to 32k clk cycles */
-	voltsetup2 = DIV_ROUND_UP(voltsetup2 * 32768, 1000000);
-
-	voltsetup2_old = voltdm->read(OMAP3_PRM_VOLTSETUP2_OFFSET);
-
-	/*
-	 * Update voltsetup2 if higher than current value (needed because
-	 * we have multiple channels with different ramp times), also
-	 * update voltoffset always to value recommended by TRM
-	 */
-	if (voltsetup2 > voltsetup2_old) {
-		voltdm->write(voltsetup2, OMAP3_PRM_VOLTSETUP2_OFFSET);
-		voltdm->write(clksetup - voltsetup2,
-			OMAP3_PRM_VOLTOFFSET_OFFSET);
-	} else
-		voltdm->write(clksetup - voltsetup2_old,
-			OMAP3_PRM_VOLTOFFSET_OFFSET);
+	if (tstart == ULONG_MAX) {
+		pr_debug("PM: oscillator start-up time not initialized, using 10ms\n");
+		clksetup = omap_usec_to_32k(10000);
+	} else {
+		clksetup = omap_usec_to_32k(tstart);
+	}
 
 	/*
-	 * omap is not controlling voltage scaling during off-mode,
-	 * thus set voltsetup1 to 0
+	 * For twl4030 errata 27, we need to allow minimum ~488.32 us wait to
+	 * switch from HFCLKIN to internal oscillator. That means timings
+	 * have voltoffset fixed to 0xa in rounded up 32 KiHz cycles. And
+	 * that means we can calculate the value based on the oscillator
+	 * start-up time since voltoffset2 = clksetup - voltoffset.
 	 */
-	voltdm->rmw(voltdm->vfsm->voltsetup_mask, 0,
-		voltdm->vfsm->voltsetup_reg);
-
-	/* voltoffset must be clksetup minus voltsetup2 according to TRM */
-	voltdm->write(clksetup - voltsetup2, OMAP3_PRM_VOLTOFFSET_OFFSET);
+	voltoffset = omap_usec_to_32k(488);
+	c->voltsetup2 = clksetup - voltoffset;
+	voltdm->write(clksetup, OMAP3_PRM_CLKSETUP_OFFSET);
+	voltdm->write(voltoffset, OMAP3_PRM_VOLTOFFSET_OFFSET);
 }
 
 static void __init omap3_vc_init_channel(struct voltagedomain *voltdm)
 {
+	omap3_vc_init_pmic_signaling(voltdm);
 	omap3_set_off_timings(voltdm);
+	omap3_set_i2c_timings(voltdm);
 }
 
 /**
@@ -462,7 +542,7 @@ static void omap4_set_timings(struct voltagedomain *voltdm, bool off_mode)
 	val |= omap4_usec_to_val_scrm(tshut, OMAP4_DOWNTIME_SHIFT,
 		OMAP4_DOWNTIME_MASK);
 
-	__raw_writel(val, OMAP4_SCRM_CLKSETUPTIME);
+	writel_relaxed(val, OMAP4_SCRM_CLKSETUPTIME);
 }
 
 /* OMAP4 specific voltage init functions */
@@ -584,7 +664,7 @@ static void __init omap4_vc_i2c_timing_init(struct voltagedomain *voltdm)
 	val = i2c_data->loadbits << 25 | i2c_data->loadbits << 29;
 
 	/* Write to SYSCTRL_PADCONF_WKUP_CTRL_I2C_2 to setup I2C pull */
-	__raw_writel(val, OMAP2_L4_IO_ADDRESS(OMAP4_CTRL_MODULE_PAD_WKUP +
+	writel_relaxed(val, OMAP2_L4_IO_ADDRESS(OMAP4_CTRL_MODULE_PAD_WKUP +
 				OMAP4_CTRL_MODULE_PAD_WKUP_CONTROL_I2C_2));
 
 	/* HSSCLH can always be zero */
diff --git a/arch/arm/mach-omap2/vc.h b/arch/arm/mach-omap2/vc.h
index 91c8d75bf2e..cdbdd78e755 100644
--- a/arch/arm/mach-omap2/vc.h
+++ b/arch/arm/mach-omap2/vc.h
@@ -117,6 +117,9 @@ extern struct omap_vc_param omap4_mpu_vc_data;
 extern struct omap_vc_param omap4_iva_vc_data;
 extern struct omap_vc_param omap4_core_vc_data;
 
+void omap3_vc_set_pmic_signaling(int core_next_state);
+
+
 void omap_vc_init_channel(struct voltagedomain *voltdm);
 int omap_vc_pre_scale(struct voltagedomain *voltdm,
 		      unsigned long target_volt,
diff --git a/arch/arm/mach-omap2/wd_timer.c b/arch/arm/mach-omap2/wd_timer.c
index d15c7bbab8e..97d6607d447 100644
--- a/arch/arm/mach-omap2/wd_timer.c
+++ b/arch/arm/mach-omap2/wd_timer.c
@@ -49,12 +49,12 @@ int omap2_wd_timer_disable(struct omap_hwmod *oh)
 	}
 
 	/* sequence required to disable watchdog */
-	__raw_writel(0xAAAA, base + OMAP_WDT_SPR);
-	while (__raw_readl(base + OMAP_WDT_WPS) & 0x10)
+	writel_relaxed(0xAAAA, base + OMAP_WDT_SPR);
+	while (readl_relaxed(base + OMAP_WDT_WPS) & 0x10)
 		cpu_relax();
 
-	__raw_writel(0x5555, base + OMAP_WDT_SPR);
-	while (__raw_readl(base + OMAP_WDT_WPS) & 0x10)
+	writel_relaxed(0x5555, base + OMAP_WDT_SPR);
+	while (readl_relaxed(base + OMAP_WDT_WPS) & 0x10)
 		cpu_relax();
 
 	return 0;
diff --git a/arch/arm/mach-orion5x/Kconfig b/arch/arm/mach-orion5x/Kconfig
index 14f2cae4109..2412efb6cdd 100644
--- a/arch/arm/mach-orion5x/Kconfig
+++ b/arch/arm/mach-orion5x/Kconfig
@@ -5,6 +5,11 @@ menu "Orion Implementations"
 config ARCH_ORION5X_DT
 	bool "Marvell Orion5x Flattened Device Tree"
 	select USE_OF
+	select ORION_CLK
+	select ORION_IRQCHIP
+	select ORION_TIMER
+	select PINCTRL
+	select PINCTRL_ORION
 	help
 	  Say 'Y' here if you want your kernel to support the
 	  Marvell Orion5x using flattened device tree.
@@ -23,6 +28,14 @@ config MACH_RD88F5182
 	  Say 'Y' here if you want your kernel to support the
 	  Marvell Orion-NAS (88F5182) RD2
 
+config MACH_RD88F5182_DT
+	bool "Marvell Orion-NAS Reference Design (Flattened Device Tree)"
+	select ARCH_ORION5X_DT
+	select I2C_BOARDINFO
+	help
+	  Say 'Y' here if you want your kernel to support the Marvell
+	  Orion-NAS (88F5182) RD2, Flattened Device Tree.
+
 config MACH_KUROBOX_PRO
 	bool "KuroBox Pro"
 	select I2C_BOARDINFO
@@ -102,28 +115,13 @@ config MACH_MV2120
 	  Say 'Y' here if you want your kernel to support the
 	  HP Media Vault mv2120 or mv5100.
 
-config MACH_EDMINI_V2_DT
-	bool "LaCie Ethernet Disk mini V2 (Flattened Device Tree)"
-	select I2C_BOARDINFO
+config MACH_D2NET_DT
+	bool "LaCie d2 Network / Big Disk Network (Flattened Device Tree)"
 	select ARCH_ORION5X_DT
 	help
 	  Say 'Y' here if you want your kernel to support the
-	  LaCie Ethernet Disk mini V2 (Flattened Device Tree).
-
-config MACH_D2NET
-	bool "LaCie d2 Network"
-	select I2C_BOARDINFO
-	help
-	  Say 'Y' here if you want your kernel to support the
 	  LaCie d2 Network NAS.
 
-config MACH_BIGDISK
-	bool "LaCie Big Disk Network"
-	select I2C_BOARDINFO
-	help
-	  Say 'Y' here if you want your kernel to support the
-	  LaCie Big Disk Network NAS.
-
 config MACH_NET2BIG
 	bool "LaCie 2Big Network"
 	select I2C_BOARDINFO
@@ -131,8 +129,9 @@ config MACH_NET2BIG
 	  Say 'Y' here if you want your kernel to support the
 	  LaCie 2Big Network NAS.
 
-config MACH_MSS2
-	bool "Maxtor Shared Storage II"
+config MACH_MSS2_DT
+	bool "Maxtor Shared Storage II (Flattened Device Tree)"
+	select ARCH_ORION5X_DT
 	help
 	  Say 'Y' here if you want your kernel to support the
 	  Maxtor Shared Storage II platform.
diff --git a/arch/arm/mach-orion5x/Makefile b/arch/arm/mach-orion5x/Makefile
index 45da805fb23..a40b5c9a58c 100644
--- a/arch/arm/mach-orion5x/Makefile
+++ b/arch/arm/mach-orion5x/Makefile
@@ -12,10 +12,7 @@ obj-$(CONFIG_MACH_TS409)	+= ts409-setup.o tsx09-common.o
 obj-$(CONFIG_MACH_WRT350N_V2)	+= wrt350n-v2-setup.o
 obj-$(CONFIG_MACH_TS78XX)	+= ts78xx-setup.o
 obj-$(CONFIG_MACH_MV2120)	+= mv2120-setup.o
-obj-$(CONFIG_MACH_D2NET)	+= d2net-setup.o
-obj-$(CONFIG_MACH_BIGDISK)	+= d2net-setup.o
 obj-$(CONFIG_MACH_NET2BIG)	+= net2big-setup.o
-obj-$(CONFIG_MACH_MSS2)		+= mss2-setup.o
 obj-$(CONFIG_MACH_WNR854T)	+= wnr854t-setup.o
 obj-$(CONFIG_MACH_RD88F5181L_GE)	+= rd88f5181l-ge-setup.o
 obj-$(CONFIG_MACH_RD88F5181L_FXO)	+= rd88f5181l-fxo-setup.o
@@ -23,4 +20,6 @@ obj-$(CONFIG_MACH_RD88F6183AP_GE)	+= rd88f6183ap-ge-setup.o
 obj-$(CONFIG_MACH_LINKSTATION_LSCHL)	+= ls-chl-setup.o
 
 obj-$(CONFIG_ARCH_ORION5X_DT)		+= board-dt.o
-obj-$(CONFIG_MACH_EDMINI_V2_DT)	+= edmini_v2-setup.o
+obj-$(CONFIG_MACH_D2NET_DT)	+= board-d2net.o
+obj-$(CONFIG_MACH_MSS2_DT)	+= board-mss2.o
+obj-$(CONFIG_MACH_RD88F5182_DT)	+= board-rd88f5182.o
diff --git a/arch/arm/mach-orion5x/board-d2net.c b/arch/arm/mach-orion5x/board-d2net.c
new file mode 100644
index 00000000000..8a728412415
--- /dev/null
+++ b/arch/arm/mach-orion5x/board-d2net.c
@@ -0,0 +1,109 @@
+/*
+ * arch/arm/mach-orion5x/board-d2net.c
+ *
+ * LaCie d2Network and Big Disk Network NAS setup
+ *
+ * Copyright (C) 2009 Simon Guinot <sguinot@lacie.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.
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/pci.h>
+#include <linux/irq.h>
+#include <linux/leds.h>
+#include <linux/gpio.h>
+#include <asm/mach-types.h>
+#include <asm/mach/arch.h>
+#include <asm/mach/pci.h>
+#include <mach/orion5x.h>
+#include <plat/orion-gpio.h>
+#include "common.h"
+
+/*****************************************************************************
+ * LaCie d2 Network Info
+ ****************************************************************************/
+
+/*****************************************************************************
+ * GPIO LED's
+ ****************************************************************************/
+
+/*
+ * The blue front LED is wired to the CPLD and can blink in relation with the
+ * SATA activity.
+ *
+ * The following array detail the different LED registers and the combination
+ * of their possible values:
+ *
+ * led_off   | blink_ctrl | SATA active | LED state
+ *           |            |             |
+ *    1      |     x      |      x      |  off
+ *    0      |     0      |      0      |  off
+ *    0      |     1      |      0      |  blink (rate 300ms)
+ *    0      |     x      |      1      |  on
+ *
+ * Notes: The blue and the red front LED's can't be on at the same time.
+ *        Red LED have priority.
+ */
+
+#define D2NET_GPIO_RED_LED		6
+#define D2NET_GPIO_BLUE_LED_BLINK_CTRL	16
+#define D2NET_GPIO_BLUE_LED_OFF		23
+
+static struct gpio_led d2net_leds[] = {
+	{
+		.name = "d2net:blue:sata",
+		.default_trigger = "default-on",
+		.gpio = D2NET_GPIO_BLUE_LED_OFF,
+		.active_low = 1,
+	},
+	{
+		.name = "d2net:red:fail",
+		.gpio = D2NET_GPIO_RED_LED,
+	},
+};
+
+static struct gpio_led_platform_data d2net_led_data = {
+	.num_leds = ARRAY_SIZE(d2net_leds),
+	.leds = d2net_leds,
+};
+
+static struct platform_device d2net_gpio_leds = {
+	.name           = "leds-gpio",
+	.id             = -1,
+	.dev            = {
+		.platform_data  = &d2net_led_data,
+	},
+};
+
+static void __init d2net_gpio_leds_init(void)
+{
+	int err;
+
+	/* Configure register blink_ctrl to allow SATA activity LED blinking. */
+	err = gpio_request(D2NET_GPIO_BLUE_LED_BLINK_CTRL, "blue LED blink");
+	if (err == 0) {
+		err = gpio_direction_output(D2NET_GPIO_BLUE_LED_BLINK_CTRL, 1);
+		if (err)
+			gpio_free(D2NET_GPIO_BLUE_LED_BLINK_CTRL);
+	}
+	if (err)
+		pr_err("d2net: failed to configure blue LED blink GPIO\n");
+
+	platform_device_register(&d2net_gpio_leds);
+}
+
+/*****************************************************************************
+ * General Setup
+ ****************************************************************************/
+
+void __init d2net_init(void)
+{
+	d2net_gpio_leds_init();
+
+	pr_notice("d2net: Flash write are not yet supported.\n");
+}
diff --git a/arch/arm/mach-orion5x/board-dt.c b/arch/arm/mach-orion5x/board-dt.c
index c134a826070..35d418faf8f 100644
--- a/arch/arm/mach-orion5x/board-dt.c
+++ b/arch/arm/mach-orion5x/board-dt.c
@@ -15,10 +15,16 @@
 #include <linux/of.h>
 #include <linux/of_platform.h>
 #include <linux/cpu.h>
+#include <linux/mbus.h>
+#include <linux/clk-provider.h>
+#include <linux/clocksource.h>
 #include <asm/system_misc.h>
 #include <asm/mach/arch.h>
+#include <asm/mach/map.h>
 #include <mach/orion5x.h>
+#include <mach/bridge-regs.h>
 #include <plat/irq.h>
+#include <plat/time.h>
 #include "common.h"
 
 static struct of_dev_auxdata orion5x_auxdata_lookup[] __initdata = {
@@ -39,14 +45,13 @@ static void __init orion5x_dt_init(void)
 	orion5x_id(&dev, &rev, &dev_name);
 	printk(KERN_INFO "Orion ID: %s. TCLK=%d.\n", dev_name, orion5x_tclk);
 
+	BUG_ON(mvebu_mbus_dt_init());
+
 	/*
 	 * Setup Orion address map
 	 */
 	orion5x_setup_wins();
 
-	/* Setup root of clk tree */
-	clk_init();
-
 	/*
 	 * Don't issue "Wait for Interrupt" instruction if we are
 	 * running on D0 5281 silicon.
@@ -56,8 +61,8 @@ static void __init orion5x_dt_init(void)
 		cpu_idle_poll_ctrl(true);
 	}
 
-	if (of_machine_is_compatible("lacie,ethernet-disk-mini-v2"))
-		edmini_v2_init();
+	if (of_machine_is_compatible("maxtor,shared-storage-2"))
+		mss2_init();
 
 	of_platform_populate(NULL, of_default_bus_match_table,
 			     orion5x_auxdata_lookup, NULL);
@@ -71,9 +76,6 @@ static const char *orion5x_dt_compat[] = {
 DT_MACHINE_START(ORION5X_DT, "Marvell Orion5x (Flattened Device Tree)")
 	/* Maintainer: Thomas Petazzoni <thomas.petazzoni@free-electrons.com> */
 	.map_io		= orion5x_map_io,
-	.init_early	= orion5x_init_early,
-	.init_irq	= orion_dt_init_irq,
-	.init_time	= orion5x_timer_init,
 	.init_machine	= orion5x_dt_init,
 	.restart	= orion5x_restart,
 	.dt_compat	= orion5x_dt_compat,
diff --git a/arch/arm/mach-orion5x/board-mss2.c b/arch/arm/mach-orion5x/board-mss2.c
new file mode 100644
index 00000000000..66f9c3ba86c
--- /dev/null
+++ b/arch/arm/mach-orion5x/board-mss2.c
@@ -0,0 +1,90 @@
+/*
+ * Maxtor Shared Storage II Board Setup
+ *
+ * Maintainer: Sylver Bruneau <sylver.bruneau@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.
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/pci.h>
+#include <linux/irq.h>
+#include <asm/mach-types.h>
+#include <asm/mach/arch.h>
+#include <asm/mach/pci.h>
+#include <mach/orion5x.h>
+#include <mach/bridge-regs.h>
+#include "common.h"
+
+/*****************************************************************************
+ * Maxtor Shared Storage II Info
+ ****************************************************************************/
+
+/****************************************************************************
+ * PCI setup
+ ****************************************************************************/
+static int __init mss2_pci_map_irq(const struct pci_dev *dev, u8 slot, u8 pin)
+{
+	int irq;
+
+	/*
+	 * Check for devices with hard-wired IRQs.
+	 */
+	irq = orion5x_pci_map_irq(dev, slot, pin);
+	if (irq != -1)
+		return irq;
+
+	return -1;
+}
+
+static struct hw_pci mss2_pci __initdata = {
+	.nr_controllers = 2,
+	.setup		= orion5x_pci_sys_setup,
+	.scan		= orion5x_pci_sys_scan_bus,
+	.map_irq	= mss2_pci_map_irq,
+};
+
+static int __init mss2_pci_init(void)
+{
+	if (machine_is_mss2())
+		pci_common_init(&mss2_pci);
+
+	return 0;
+}
+subsys_initcall(mss2_pci_init);
+
+/*****************************************************************************
+ * MSS2 power off method
+ ****************************************************************************/
+/*
+ * On the Maxtor Shared Storage II, the shutdown process is the following :
+ * - Userland modifies U-boot env to tell U-boot to go idle at next boot
+ * - The board reboots
+ * - U-boot starts and go into an idle mode until the user press "power"
+ */
+static void mss2_power_off(void)
+{
+	u32 reg;
+
+	/*
+	 * Enable and issue soft reset
+	 */
+	reg = readl(RSTOUTn_MASK);
+	reg |= 1 << 2;
+	writel(reg, RSTOUTn_MASK);
+
+	reg = readl(CPU_SOFT_RESET);
+	reg |= 1;
+	writel(reg, CPU_SOFT_RESET);
+}
+
+void __init mss2_init(void)
+{
+	/* register mss2 specific power-off method */
+	pm_power_off = mss2_power_off;
+}
diff --git a/arch/arm/mach-orion5x/board-rd88f5182.c b/arch/arm/mach-orion5x/board-rd88f5182.c
new file mode 100644
index 00000000000..270824b0e50
--- /dev/null
+++ b/arch/arm/mach-orion5x/board-rd88f5182.c
@@ -0,0 +1,116 @@
+/*
+ * arch/arm/mach-orion5x/rd88f5182-setup.c
+ *
+ * Marvell Orion-NAS Reference Design Setup
+ *
+ * Maintainer: Ronen Shitrit <rshitrit@marvell.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.
+ */
+#include <linux/gpio.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/pci.h>
+#include <linux/irq.h>
+#include <asm/mach-types.h>
+#include <asm/mach/arch.h>
+#include <asm/mach/pci.h>
+#include <mach/orion5x.h>
+#include "common.h"
+
+/*****************************************************************************
+ * RD-88F5182 Info
+ ****************************************************************************/
+
+/*
+ * PCI
+ */
+
+#define RD88F5182_PCI_SLOT0_OFFS	7
+#define RD88F5182_PCI_SLOT0_IRQ_A_PIN	7
+#define RD88F5182_PCI_SLOT0_IRQ_B_PIN	6
+
+/*****************************************************************************
+ * PCI
+ ****************************************************************************/
+
+static void __init rd88f5182_pci_preinit(void)
+{
+	int pin;
+
+	/*
+	 * Configure PCI GPIO IRQ pins
+	 */
+	pin = RD88F5182_PCI_SLOT0_IRQ_A_PIN;
+	if (gpio_request(pin, "PCI IntA") == 0) {
+		if (gpio_direction_input(pin) == 0) {
+			irq_set_irq_type(gpio_to_irq(pin), IRQ_TYPE_LEVEL_LOW);
+		} else {
+			printk(KERN_ERR "rd88f5182_pci_preinit failed to "
+					"set_irq_type pin %d\n", pin);
+			gpio_free(pin);
+		}
+	} else {
+		printk(KERN_ERR "rd88f5182_pci_preinit failed to request gpio %d\n", pin);
+	}
+
+	pin = RD88F5182_PCI_SLOT0_IRQ_B_PIN;
+	if (gpio_request(pin, "PCI IntB") == 0) {
+		if (gpio_direction_input(pin) == 0) {
+			irq_set_irq_type(gpio_to_irq(pin), IRQ_TYPE_LEVEL_LOW);
+		} else {
+			printk(KERN_ERR "rd88f5182_pci_preinit failed to "
+					"set_irq_type pin %d\n", pin);
+			gpio_free(pin);
+		}
+	} else {
+		printk(KERN_ERR "rd88f5182_pci_preinit failed to gpio_request %d\n", pin);
+	}
+}
+
+static int __init rd88f5182_pci_map_irq(const struct pci_dev *dev, u8 slot,
+	u8 pin)
+{
+	int irq;
+
+	/*
+	 * Check for devices with hard-wired IRQs.
+	 */
+	irq = orion5x_pci_map_irq(dev, slot, pin);
+	if (irq != -1)
+		return irq;
+
+	/*
+	 * PCI IRQs are connected via GPIOs
+	 */
+	switch (slot - RD88F5182_PCI_SLOT0_OFFS) {
+	case 0:
+		if (pin == 1)
+			return gpio_to_irq(RD88F5182_PCI_SLOT0_IRQ_A_PIN);
+		else
+			return gpio_to_irq(RD88F5182_PCI_SLOT0_IRQ_B_PIN);
+	default:
+		return -1;
+	}
+}
+
+static struct hw_pci rd88f5182_pci __initdata = {
+	.nr_controllers	= 2,
+	.preinit	= rd88f5182_pci_preinit,
+	.setup		= orion5x_pci_sys_setup,
+	.scan		= orion5x_pci_sys_scan_bus,
+	.map_irq	= rd88f5182_pci_map_irq,
+};
+
+static int __init rd88f5182_pci_init(void)
+{
+	if (of_machine_is_compatible("marvell,rd-88f5182-nas"))
+		pci_common_init(&rd88f5182_pci);
+
+	return 0;
+}
+
+subsys_initcall(rd88f5182_pci_init);
diff --git a/arch/arm/mach-orion5x/common.h b/arch/arm/mach-orion5x/common.h
index f565f9944af..26d6f34b602 100644
--- a/arch/arm/mach-orion5x/common.h
+++ b/arch/arm/mach-orion5x/common.h
@@ -21,7 +21,7 @@ struct mv_sata_platform_data;
 #define ORION_MBUS_DEVBUS_BOOT_ATTR   0x0f
 #define ORION_MBUS_DEVBUS_TARGET(cs)  0x01
 #define ORION_MBUS_DEVBUS_ATTR(cs)    (~(1 << cs))
-#define ORION_MBUS_SRAM_TARGET        0x00
+#define ORION_MBUS_SRAM_TARGET        0x09
 #define ORION_MBUS_SRAM_ATTR          0x00
 
 /*
@@ -64,17 +64,16 @@ int orion5x_pci_sys_setup(int nr, struct pci_sys_data *sys);
 struct pci_bus *orion5x_pci_sys_scan_bus(int nr, struct pci_sys_data *sys);
 int orion5x_pci_map_irq(const struct pci_dev *dev, u8 slot, u8 pin);
 
-/* board init functions for boards not fully converted to fdt */
-#ifdef CONFIG_MACH_EDMINI_V2_DT
-void edmini_v2_init(void);
-#else
-static inline void edmini_v2_init(void) {};
-#endif
-
 struct meminfo;
 struct tag;
 extern void __init tag_fixup_mem32(struct tag *, char **, struct meminfo *);
 
+#ifdef CONFIG_MACH_MSS2_DT
+extern void mss2_init(void);
+#else
+static inline void mss2_init(void) {}
+#endif
+
 /*****************************************************************************
  * Helpers to access Orion registers
  ****************************************************************************/
diff --git a/arch/arm/mach-orion5x/d2net-setup.c b/arch/arm/mach-orion5x/d2net-setup.c
deleted file mode 100644
index 8f68b745c1d..00000000000
--- a/arch/arm/mach-orion5x/d2net-setup.c
+++ /dev/null
@@ -1,365 +0,0 @@
-/*
- * arch/arm/mach-orion5x/d2net-setup.c
- *
- * LaCie d2Network and Big Disk Network NAS setup
- *
- * Copyright (C) 2009 Simon Guinot <sguinot@lacie.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.
- */
-
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/platform_device.h>
-#include <linux/pci.h>
-#include <linux/irq.h>
-#include <linux/mtd/physmap.h>
-#include <linux/mv643xx_eth.h>
-#include <linux/leds.h>
-#include <linux/gpio_keys.h>
-#include <linux/input.h>
-#include <linux/i2c.h>
-#include <linux/ata_platform.h>
-#include <linux/gpio.h>
-#include <asm/mach-types.h>
-#include <asm/mach/arch.h>
-#include <asm/mach/pci.h>
-#include <mach/orion5x.h>
-#include <plat/orion-gpio.h>
-#include "common.h"
-#include "mpp.h"
-
-/*****************************************************************************
- * LaCie d2 Network Info
- ****************************************************************************/
-
-/*
- * 512KB NOR flash Device bus boot chip select
- */
-
-#define D2NET_NOR_BOOT_BASE		0xfff80000
-#define D2NET_NOR_BOOT_SIZE		SZ_512K
-
-/*****************************************************************************
- * 512KB NOR Flash on Boot Device
- ****************************************************************************/
-
-/*
- * TODO: Check write support on flash MX29LV400CBTC-70G
- */
-
-static struct mtd_partition d2net_partitions[] = {
-	{
-		.name		= "Full512kb",
-		.size		= MTDPART_SIZ_FULL,
-		.offset		= 0,
-		.mask_flags	= MTD_WRITEABLE,
-	},
-};
-
-static struct physmap_flash_data d2net_nor_flash_data = {
-	.width		= 1,
-	.parts		= d2net_partitions,
-	.nr_parts	= ARRAY_SIZE(d2net_partitions),
-};
-
-static struct resource d2net_nor_flash_resource = {
-	.flags			= IORESOURCE_MEM,
-	.start			= D2NET_NOR_BOOT_BASE,
-	.end			= D2NET_NOR_BOOT_BASE
-					+ D2NET_NOR_BOOT_SIZE - 1,
-};
-
-static struct platform_device d2net_nor_flash = {
-	.name			= "physmap-flash",
-	.id			= 0,
-	.dev		= {
-		.platform_data	= &d2net_nor_flash_data,
-	},
-	.num_resources		= 1,
-	.resource		= &d2net_nor_flash_resource,
-};
-
-/*****************************************************************************
- * Ethernet
- ****************************************************************************/
-
-static struct mv643xx_eth_platform_data d2net_eth_data = {
-	.phy_addr	= MV643XX_ETH_PHY_ADDR(8),
-};
-
-/*****************************************************************************
- * I2C devices
- ****************************************************************************/
-
-/*
- * i2c addr | chip         | description
- * 0x32     | Ricoh 5C372b | RTC
- * 0x3e     | GMT G762     | PWM fan controller
- * 0x50     | HT24LC08     | eeprom (1kB)
- *
- * TODO: Add G762 support to the g760a driver.
- */
-static struct i2c_board_info __initdata d2net_i2c_devices[] = {
-	{
-		I2C_BOARD_INFO("rs5c372b", 0x32),
-	}, {
-		I2C_BOARD_INFO("24c08", 0x50),
-	},
-};
-
-/*****************************************************************************
- * SATA
- ****************************************************************************/
-
-static struct mv_sata_platform_data d2net_sata_data = {
-	.n_ports	= 2,
-};
-
-#define D2NET_GPIO_SATA0_POWER	3
-#define D2NET_GPIO_SATA1_POWER	12
-
-static void __init d2net_sata_power_init(void)
-{
-	int err;
-
-	err = gpio_request(D2NET_GPIO_SATA0_POWER, "SATA0 power");
-	if (err == 0) {
-		err = gpio_direction_output(D2NET_GPIO_SATA0_POWER, 1);
-		if (err)
-			gpio_free(D2NET_GPIO_SATA0_POWER);
-	}
-	if (err)
-		pr_err("d2net: failed to configure SATA0 power GPIO\n");
-
-	err = gpio_request(D2NET_GPIO_SATA1_POWER, "SATA1 power");
-	if (err == 0) {
-		err = gpio_direction_output(D2NET_GPIO_SATA1_POWER, 1);
-		if (err)
-			gpio_free(D2NET_GPIO_SATA1_POWER);
-	}
-	if (err)
-		pr_err("d2net: failed to configure SATA1 power GPIO\n");
-}
-
-/*****************************************************************************
- * GPIO LED's
- ****************************************************************************/
-
-/*
- * The blue front LED is wired to the CPLD and can blink in relation with the
- * SATA activity.
- *
- * The following array detail the different LED registers and the combination
- * of their possible values:
- *
- * led_off   | blink_ctrl | SATA active | LED state
- *           |            |             |
- *    1      |     x      |      x      |  off
- *    0      |     0      |      0      |  off
- *    0      |     1      |      0      |  blink (rate 300ms)
- *    0      |     x      |      1      |  on
- *
- * Notes: The blue and the red front LED's can't be on at the same time.
- *        Red LED have priority.
- */
-
-#define D2NET_GPIO_RED_LED		6
-#define D2NET_GPIO_BLUE_LED_BLINK_CTRL	16
-#define D2NET_GPIO_BLUE_LED_OFF		23
-
-static struct gpio_led d2net_leds[] = {
-	{
-		.name = "d2net:blue:sata",
-		.default_trigger = "default-on",
-		.gpio = D2NET_GPIO_BLUE_LED_OFF,
-		.active_low = 1,
-	},
-	{
-		.name = "d2net:red:fail",
-		.gpio = D2NET_GPIO_RED_LED,
-	},
-};
-
-static struct gpio_led_platform_data d2net_led_data = {
-	.num_leds = ARRAY_SIZE(d2net_leds),
-	.leds = d2net_leds,
-};
-
-static struct platform_device d2net_gpio_leds = {
-	.name           = "leds-gpio",
-	.id             = -1,
-	.dev            = {
-		.platform_data  = &d2net_led_data,
-	},
-};
-
-static void __init d2net_gpio_leds_init(void)
-{
-	int err;
-
-	/* Configure GPIO over MPP max number. */
-	orion_gpio_set_valid(D2NET_GPIO_BLUE_LED_OFF, 1);
-
-	/* Configure register blink_ctrl to allow SATA activity LED blinking. */
-	err = gpio_request(D2NET_GPIO_BLUE_LED_BLINK_CTRL, "blue LED blink");
-	if (err == 0) {
-		err = gpio_direction_output(D2NET_GPIO_BLUE_LED_BLINK_CTRL, 1);
-		if (err)
-			gpio_free(D2NET_GPIO_BLUE_LED_BLINK_CTRL);
-	}
-	if (err)
-		pr_err("d2net: failed to configure blue LED blink GPIO\n");
-
-	platform_device_register(&d2net_gpio_leds);
-}
-
-/****************************************************************************
- * GPIO keys
- ****************************************************************************/
-
-#define D2NET_GPIO_PUSH_BUTTON		18
-#define D2NET_GPIO_POWER_SWITCH_ON	8
-#define D2NET_GPIO_POWER_SWITCH_OFF	9
-
-#define D2NET_SWITCH_POWER_ON		0x1
-#define D2NET_SWITCH_POWER_OFF		0x2
-
-static struct gpio_keys_button d2net_buttons[] = {
-	{
-		.type		= EV_SW,
-		.code		= D2NET_SWITCH_POWER_OFF,
-		.gpio		= D2NET_GPIO_POWER_SWITCH_OFF,
-		.desc		= "Power rocker switch (auto|off)",
-		.active_low	= 0,
-	},
-	{
-		.type		= EV_SW,
-		.code		= D2NET_SWITCH_POWER_ON,
-		.gpio		= D2NET_GPIO_POWER_SWITCH_ON,
-		.desc		= "Power rocker switch (on|auto)",
-		.active_low	= 0,
-	},
-	{
-		.type		= EV_KEY,
-		.code		= KEY_POWER,
-		.gpio		= D2NET_GPIO_PUSH_BUTTON,
-		.desc		= "Front Push Button",
-		.active_low	= 0,
-	},
-};
-
-static struct gpio_keys_platform_data d2net_button_data = {
-	.buttons	= d2net_buttons,
-	.nbuttons	= ARRAY_SIZE(d2net_buttons),
-};
-
-static struct platform_device d2net_gpio_buttons = {
-	.name		= "gpio-keys",
-	.id		= -1,
-	.dev		= {
-		.platform_data	= &d2net_button_data,
-	},
-};
-
-/*****************************************************************************
- * General Setup
- ****************************************************************************/
-
-static unsigned int d2net_mpp_modes[] __initdata = {
-	MPP0_GPIO,	/* Board ID (bit 0) */
-	MPP1_GPIO,	/* Board ID (bit 1) */
-	MPP2_GPIO,	/* Board ID (bit 2) */
-	MPP3_GPIO,	/* SATA 0 power */
-	MPP4_UNUSED,
-	MPP5_GPIO,	/* Fan fail detection */
-	MPP6_GPIO,	/* Red front LED */
-	MPP7_UNUSED,
-	MPP8_GPIO,	/* Rear power switch (on|auto) */
-	MPP9_GPIO,	/* Rear power switch (auto|off) */
-	MPP10_UNUSED,
-	MPP11_UNUSED,
-	MPP12_GPIO,	/* SATA 1 power */
-	MPP13_UNUSED,
-	MPP14_SATA_LED,	/* SATA 0 active */
-	MPP15_SATA_LED,	/* SATA 1 active */
-	MPP16_GPIO,	/* Blue front LED blink control */
-	MPP17_UNUSED,
-	MPP18_GPIO,	/* Front button (0 = Released, 1 = Pushed ) */
-	MPP19_UNUSED,
-	0,
-	/* 22: USB port 1 fuse (0 = Fail, 1 = Ok) */
-	/* 23: Blue front LED off */
-	/* 24: Inhibit board power off (0 = Disabled, 1 = Enabled) */
-};
-
-#define D2NET_GPIO_INHIBIT_POWER_OFF    24
-
-static void __init d2net_init(void)
-{
-	/*
-	 * Setup basic Orion functions. Need to be called early.
-	 */
-	orion5x_init();
-
-	orion5x_mpp_conf(d2net_mpp_modes);
-
-	/*
-	 * Configure peripherals.
-	 */
-	orion5x_ehci0_init();
-	orion5x_eth_init(&d2net_eth_data);
-	orion5x_i2c_init();
-	orion5x_uart0_init();
-
-	d2net_sata_power_init();
-	orion5x_sata_init(&d2net_sata_data);
-
-	mvebu_mbus_add_window_by_id(ORION_MBUS_DEVBUS_BOOT_TARGET,
-				    ORION_MBUS_DEVBUS_BOOT_ATTR,
-				    D2NET_NOR_BOOT_BASE,
-				    D2NET_NOR_BOOT_SIZE);
-	platform_device_register(&d2net_nor_flash);
-
-	platform_device_register(&d2net_gpio_buttons);
-
-	d2net_gpio_leds_init();
-
-	pr_notice("d2net: Flash write are not yet supported.\n");
-
-	i2c_register_board_info(0, d2net_i2c_devices,
-				ARRAY_SIZE(d2net_i2c_devices));
-
-	orion_gpio_set_valid(D2NET_GPIO_INHIBIT_POWER_OFF, 1);
-}
-
-/* Warning: LaCie use a wrong mach-type (0x20e=526) in their bootloader. */
-
-#ifdef CONFIG_MACH_D2NET
-MACHINE_START(D2NET, "LaCie d2 Network")
-	.atag_offset	= 0x100,
-	.init_machine	= d2net_init,
-	.map_io		= orion5x_map_io,
-	.init_early	= orion5x_init_early,
-	.init_irq	= orion5x_init_irq,
-	.init_time	= orion5x_timer_init,
-	.fixup		= tag_fixup_mem32,
-	.restart	= orion5x_restart,
-MACHINE_END
-#endif
-
-#ifdef CONFIG_MACH_BIGDISK
-MACHINE_START(BIGDISK, "LaCie Big Disk Network")
-	.atag_offset	= 0x100,
-	.init_machine	= d2net_init,
-	.map_io		= orion5x_map_io,
-	.init_early	= orion5x_init_early,
-	.init_irq	= orion5x_init_irq,
-	.init_time	= orion5x_timer_init,
-	.fixup		= tag_fixup_mem32,
-	.restart	= orion5x_restart,
-MACHINE_END
-#endif
-
diff --git a/arch/arm/mach-orion5x/edmini_v2-setup.c b/arch/arm/mach-orion5x/edmini_v2-setup.c
deleted file mode 100644
index f66c1b2ee8c..00000000000
--- a/arch/arm/mach-orion5x/edmini_v2-setup.c
+++ /dev/null
@@ -1,169 +0,0 @@
-/*
- * arch/arm/mach-orion5x/edmini_v2-setup.c
- *
- * LaCie Ethernet Disk mini V2 Setup
- *
- * Copyright (C) 2008 Christopher Moore <moore@free.fr>
- * Copyright (C) 2008 Albert Aribaud <albert.aribaud@free.fr>
- *
- * 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.
- */
-
-/*
- * TODO: add Orion USB device port init when kernel.org support is added.
- * TODO: add flash write support: see below.
- * TODO: add power-off support.
- * TODO: add I2C EEPROM support.
- */
-
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/platform_device.h>
-#include <linux/pci.h>
-#include <linux/irq.h>
-#include <linux/mbus.h>
-#include <linux/mtd/physmap.h>
-#include <linux/leds.h>
-#include <linux/gpio_keys.h>
-#include <linux/input.h>
-#include <linux/i2c.h>
-#include <linux/ata_platform.h>
-#include <linux/gpio.h>
-#include <asm/mach-types.h>
-#include <asm/mach/arch.h>
-#include <asm/mach/pci.h>
-#include <mach/orion5x.h>
-#include "common.h"
-#include "mpp.h"
-
-/*****************************************************************************
- * EDMINI_V2 Info
- ****************************************************************************/
-
-/*
- * 512KB NOR flash Device bus boot chip select
- */
-
-#define EDMINI_V2_NOR_BOOT_BASE		0xfff80000
-#define EDMINI_V2_NOR_BOOT_SIZE		SZ_512K
-
-/*****************************************************************************
- * 512KB NOR Flash on BOOT Device
- ****************************************************************************/
-
-/*
- * Currently the MTD code does not recognize the MX29LV400CBCT as a bottom
- * -type device. This could cause risks of accidentally erasing critical
- * flash sectors. We thus define a single, write-protected partition covering
- * the whole flash.
- * TODO: once the flash part TOP/BOTTOM detection issue is sorted out in the MTD
- * code, break this into at least three partitions: 'u-boot code', 'u-boot
- * environment' and 'whatever is left'.
- */
-
-static struct mtd_partition edmini_v2_partitions[] = {
-	{
-		.name		= "Full512kb",
-		.size		= 0x00080000,
-		.offset		= 0x00000000,
-		.mask_flags	= MTD_WRITEABLE,
-	},
-};
-
-static struct physmap_flash_data edmini_v2_nor_flash_data = {
-	.width		= 1,
-	.parts		= edmini_v2_partitions,
-	.nr_parts	= ARRAY_SIZE(edmini_v2_partitions),
-};
-
-static struct resource edmini_v2_nor_flash_resource = {
-	.flags			= IORESOURCE_MEM,
-	.start			= EDMINI_V2_NOR_BOOT_BASE,
-	.end			= EDMINI_V2_NOR_BOOT_BASE
-		+ EDMINI_V2_NOR_BOOT_SIZE - 1,
-};
-
-static struct platform_device edmini_v2_nor_flash = {
-	.name			= "physmap-flash",
-	.id			= 0,
-	.dev		= {
-		.platform_data	= &edmini_v2_nor_flash_data,
-	},
-	.num_resources		= 1,
-	.resource		= &edmini_v2_nor_flash_resource,
-};
-
-/*****************************************************************************
- * RTC 5C372a on I2C bus
- ****************************************************************************/
-
-#define EDMINIV2_RTC_GPIO	3
-
-static struct i2c_board_info __initdata edmini_v2_i2c_rtc = {
-	I2C_BOARD_INFO("rs5c372a", 0x32),
-	.irq = 0,
-};
-
-/*****************************************************************************
- * General Setup
- ****************************************************************************/
-static unsigned int edminiv2_mpp_modes[] __initdata = {
-	MPP0_UNUSED,
-	MPP1_UNUSED,
-	MPP2_UNUSED,
-	MPP3_GPIO,	/* RTC interrupt */
-	MPP4_UNUSED,
-	MPP5_UNUSED,
-	MPP6_UNUSED,
-	MPP7_UNUSED,
-	MPP8_UNUSED,
-	MPP9_UNUSED,
-	MPP10_UNUSED,
-	MPP11_UNUSED,
-	MPP12_SATA_LED,	/* SATA 0 presence */
-	MPP13_SATA_LED,	/* SATA 1 presence */
-	MPP14_SATA_LED,	/* SATA 0 active */
-	MPP15_SATA_LED,	/* SATA 1 active */
-	/* 16: Power LED control (0 = On, 1 = Off) */
-	MPP16_GPIO,
-	/* 17: Power LED control select (0 = CPLD, 1 = GPIO16) */
-	MPP17_GPIO,
-	/* 18: Power button status (0 = Released, 1 = Pressed) */
-	MPP18_GPIO,
-	MPP19_UNUSED,
-	0,
-};
-
-void __init edmini_v2_init(void)
-{
-	orion5x_mpp_conf(edminiv2_mpp_modes);
-
-	/*
-	 * Configure peripherals.
-	 */
-	orion5x_ehci0_init();
-
-	mvebu_mbus_add_window_by_id(ORION_MBUS_DEVBUS_BOOT_TARGET,
-				    ORION_MBUS_DEVBUS_BOOT_ATTR,
-				    EDMINI_V2_NOR_BOOT_BASE,
-				    EDMINI_V2_NOR_BOOT_SIZE);
-	platform_device_register(&edmini_v2_nor_flash);
-
-	pr_notice("edmini_v2: USB device port, flash write and power-off "
-		  "are not yet supported.\n");
-
-	/* Get RTC IRQ and register the chip */
-	if (gpio_request(EDMINIV2_RTC_GPIO, "rtc") == 0) {
-		if (gpio_direction_input(EDMINIV2_RTC_GPIO) == 0)
-			edmini_v2_i2c_rtc.irq = gpio_to_irq(EDMINIV2_RTC_GPIO);
-		else
-			gpio_free(EDMINIV2_RTC_GPIO);
-	}
-
-	if (edmini_v2_i2c_rtc.irq == 0)
-		pr_warning("edmini_v2: failed to get RTC IRQ\n");
-
-	i2c_register_board_info(0, &edmini_v2_i2c_rtc, 1);
-}
diff --git a/arch/arm/mach-orion5x/irq.c b/arch/arm/mach-orion5x/irq.c
index 9654b0cc589..cd4bac4d7e4 100644
--- a/arch/arm/mach-orion5x/irq.c
+++ b/arch/arm/mach-orion5x/irq.c
@@ -16,6 +16,7 @@
 #include <mach/bridge-regs.h>
 #include <plat/orion-gpio.h>
 #include <plat/irq.h>
+#include <asm/exception.h>
 #include "common.h"
 
 static int __initdata gpio0_irqs[4] = {
@@ -25,10 +26,37 @@ static int __initdata gpio0_irqs[4] = {
 	IRQ_ORION5X_GPIO_24_31,
 };
 
+#ifdef CONFIG_MULTI_IRQ_HANDLER
+/*
+ * Compiling with both non-DT and DT support enabled, will
+ * break asm irq handler used by non-DT boards. Therefore,
+ * we provide a C-style irq handler even for non-DT boards,
+ * if MULTI_IRQ_HANDLER is set.
+ */
+
+asmlinkage void
+__exception_irq_entry orion5x_legacy_handle_irq(struct pt_regs *regs)
+{
+	u32 stat;
+
+	stat = readl_relaxed(MAIN_IRQ_CAUSE);
+	stat &= readl_relaxed(MAIN_IRQ_MASK);
+	if (stat) {
+		unsigned int hwirq = __fls(stat);
+		handle_IRQ(hwirq, regs);
+		return;
+	}
+}
+#endif
+
 void __init orion5x_init_irq(void)
 {
 	orion_irq_init(0, MAIN_IRQ_MASK);
 
+#ifdef CONFIG_MULTI_IRQ_HANDLER
+	set_handle_irq(orion5x_legacy_handle_irq);
+#endif
+
 	/*
 	 * Initialize gpiolib for GPIOs 0-31.
 	 */
diff --git a/arch/arm/mach-orion5x/mss2-setup.c b/arch/arm/mach-orion5x/mss2-setup.c
deleted file mode 100644
index e105130ba51..00000000000
--- a/arch/arm/mach-orion5x/mss2-setup.c
+++ /dev/null
@@ -1,274 +0,0 @@
-/*
- * Maxtor Shared Storage II Board Setup
- *
- * Maintainer: Sylver Bruneau <sylver.bruneau@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.
- */
-
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/platform_device.h>
-#include <linux/pci.h>
-#include <linux/irq.h>
-#include <linux/mtd/physmap.h>
-#include <linux/mv643xx_eth.h>
-#include <linux/leds.h>
-#include <linux/gpio_keys.h>
-#include <linux/input.h>
-#include <linux/i2c.h>
-#include <linux/ata_platform.h>
-#include <linux/gpio.h>
-#include <asm/mach-types.h>
-#include <asm/mach/arch.h>
-#include <asm/mach/pci.h>
-#include <mach/orion5x.h>
-#include <mach/bridge-regs.h>
-#include "common.h"
-#include "mpp.h"
-
-#define MSS2_NOR_BOOT_BASE	0xff800000
-#define MSS2_NOR_BOOT_SIZE	SZ_256K
-
-/*****************************************************************************
- * Maxtor Shared Storage II Info
- ****************************************************************************/
-
-/*
- * Maxtor Shared Storage II hardware :
- * - Marvell 88F5182-A2 C500
- * - Marvell 88E1111 Gigabit Ethernet PHY
- * - RTC M41T81 (@0x68) on I2C bus
- * - 256KB NOR flash
- * - 64MB of RAM
- */
-
-/*****************************************************************************
- * 256KB NOR Flash on BOOT Device
- ****************************************************************************/
-
-static struct physmap_flash_data mss2_nor_flash_data = {
-	.width		= 1,
-};
-
-static struct resource mss2_nor_flash_resource = {
-	.flags		= IORESOURCE_MEM,
-	.start		= MSS2_NOR_BOOT_BASE,
-	.end		= MSS2_NOR_BOOT_BASE + MSS2_NOR_BOOT_SIZE - 1,
-};
-
-static struct platform_device mss2_nor_flash = {
-	.name		= "physmap-flash",
-	.id		= 0,
-	.dev		= {
-		.platform_data	= &mss2_nor_flash_data,
-	},
-	.resource	= &mss2_nor_flash_resource,
-	.num_resources	= 1,
-};
-
-/****************************************************************************
- * PCI setup
- ****************************************************************************/
-static int __init mss2_pci_map_irq(const struct pci_dev *dev, u8 slot, u8 pin)
-{
-	int irq;
-
-	/*
-	 * Check for devices with hard-wired IRQs.
-	 */
-	irq = orion5x_pci_map_irq(dev, slot, pin);
-	if (irq != -1)
-		return irq;
-
-	return -1;
-}
-
-static struct hw_pci mss2_pci __initdata = {
-	.nr_controllers = 2,
-	.setup		= orion5x_pci_sys_setup,
-	.scan		= orion5x_pci_sys_scan_bus,
-	.map_irq	= mss2_pci_map_irq,
-};
-
-static int __init mss2_pci_init(void)
-{
-	if (machine_is_mss2())
-		pci_common_init(&mss2_pci);
-
-	return 0;
-}
-subsys_initcall(mss2_pci_init);
-
-
-/*****************************************************************************
- * Ethernet
- ****************************************************************************/
-
-static struct mv643xx_eth_platform_data mss2_eth_data = {
-	.phy_addr	= MV643XX_ETH_PHY_ADDR(8),
-};
-
-/*****************************************************************************
- * SATA
- ****************************************************************************/
-
-static struct mv_sata_platform_data mss2_sata_data = {
-	.n_ports	= 2,
-};
-
-/*****************************************************************************
- * GPIO buttons
- ****************************************************************************/
-
-#define MSS2_GPIO_KEY_RESET	12
-#define MSS2_GPIO_KEY_POWER	11
-
-static struct gpio_keys_button mss2_buttons[] = {
-	{
-		.code		= KEY_POWER,
-		.gpio		= MSS2_GPIO_KEY_POWER,
-		.desc		= "Power",
-		.active_low	= 1,
-	}, {
-		.code		= KEY_RESTART,
-		.gpio		= MSS2_GPIO_KEY_RESET,
-		.desc		= "Reset",
-		.active_low	= 1,
-	},
-};
-
-static struct gpio_keys_platform_data mss2_button_data = {
-	.buttons	= mss2_buttons,
-	.nbuttons	= ARRAY_SIZE(mss2_buttons),
-};
-
-static struct platform_device mss2_button_device = {
-	.name		= "gpio-keys",
-	.id		= -1,
-	.dev		= {
-		.platform_data	= &mss2_button_data,
-	},
-};
-
-/*****************************************************************************
- * RTC m41t81 on I2C bus
- ****************************************************************************/
-
-#define MSS2_GPIO_RTC_IRQ	3
-
-static struct i2c_board_info __initdata mss2_i2c_rtc = {
-	I2C_BOARD_INFO("m41t81", 0x68),
-};
-
-/*****************************************************************************
- * MSS2 power off method
- ****************************************************************************/
-/*
- * On the Maxtor Shared Storage II, the shutdown process is the following :
- * - Userland modifies U-boot env to tell U-boot to go idle at next boot
- * - The board reboots
- * - U-boot starts and go into an idle mode until the user press "power"
- */
-static void mss2_power_off(void)
-{
-	u32 reg;
-
-	/*
-	 * Enable and issue soft reset
-	 */
-	reg = readl(RSTOUTn_MASK);
-	reg |= 1 << 2;
-	writel(reg, RSTOUTn_MASK);
-
-	reg = readl(CPU_SOFT_RESET);
-	reg |= 1;
-	writel(reg, CPU_SOFT_RESET);
-}
-
-/****************************************************************************
- * General Setup
- ****************************************************************************/
-static unsigned int mss2_mpp_modes[] __initdata = {
-	MPP0_GPIO,		/* Power LED */
-	MPP1_GPIO,		/* Error LED */
-	MPP2_UNUSED,
-	MPP3_GPIO,		/* RTC interrupt */
-	MPP4_GPIO,		/* HDD ind. (Single/Dual)*/
-	MPP5_GPIO,		/* HD0 5V control */
-	MPP6_GPIO,		/* HD0 12V control */
-	MPP7_GPIO,		/* HD1 5V control */
-	MPP8_GPIO,		/* HD1 12V control */
-	MPP9_UNUSED,
-	MPP10_GPIO,		/* Fan control */
-	MPP11_GPIO,		/* Power button */
-	MPP12_GPIO,		/* Reset button */
-	MPP13_UNUSED,
-	MPP14_SATA_LED,		/* SATA 0 active */
-	MPP15_SATA_LED,		/* SATA 1 active */
-	MPP16_UNUSED,
-	MPP17_UNUSED,
-	MPP18_UNUSED,
-	MPP19_UNUSED,
-	0,
-};
-
-static void __init mss2_init(void)
-{
-	/* Setup basic Orion functions. Need to be called early. */
-	orion5x_init();
-
-	orion5x_mpp_conf(mss2_mpp_modes);
-
-	/*
-	 * MPP[20] Unused
-	 * MPP[21] PCI clock
-	 * MPP[22] USB 0 over current
-	 * MPP[23] USB 1 over current
-	 */
-
-	/*
-	 * Configure peripherals.
-	 */
-	orion5x_ehci0_init();
-	orion5x_ehci1_init();
-	orion5x_eth_init(&mss2_eth_data);
-	orion5x_i2c_init();
-	orion5x_sata_init(&mss2_sata_data);
-	orion5x_uart0_init();
-	orion5x_xor_init();
-
-	mvebu_mbus_add_window_by_id(ORION_MBUS_DEVBUS_BOOT_TARGET,
-				    ORION_MBUS_DEVBUS_BOOT_ATTR,
-				    MSS2_NOR_BOOT_BASE,
-				    MSS2_NOR_BOOT_SIZE);
-	platform_device_register(&mss2_nor_flash);
-
-	platform_device_register(&mss2_button_device);
-
-	if (gpio_request(MSS2_GPIO_RTC_IRQ, "rtc") == 0) {
-		if (gpio_direction_input(MSS2_GPIO_RTC_IRQ) == 0)
-			mss2_i2c_rtc.irq = gpio_to_irq(MSS2_GPIO_RTC_IRQ);
-		else
-			gpio_free(MSS2_GPIO_RTC_IRQ);
-	}
-	i2c_register_board_info(0, &mss2_i2c_rtc, 1);
-
-	/* register mss2 specific power-off method */
-	pm_power_off = mss2_power_off;
-}
-
-MACHINE_START(MSS2, "Maxtor Shared Storage II")
-	/* Maintainer: Sylver Bruneau <sylver.bruneau@googlemail.com> */
-	.atag_offset	= 0x100,
-	.init_machine	= mss2_init,
-	.map_io		= orion5x_map_io,
-	.init_early	= orion5x_init_early,
-	.init_irq	= orion5x_init_irq,
-	.init_time	= orion5x_timer_init,
-	.fixup		= tag_fixup_mem32,
-	.restart	= orion5x_restart,
-MACHINE_END
diff --git a/arch/arm/mach-prima2/rstc.c b/arch/arm/mach-prima2/rstc.c
index 4887a2a4c69..3dffcb2d714 100644
--- a/arch/arm/mach-prima2/rstc.c
+++ b/arch/arm/mach-prima2/rstc.c
@@ -36,27 +36,33 @@ static int sirfsoc_reset_module(struct reset_controller_dev *rcdev,
 
 	if (of_device_is_compatible(rcdev->of_node, "sirf,prima2-rstc")) {
 		/*
-		 * Writing 1 to this bit resets corresponding block. Writing 0 to this
-		 * bit de-asserts reset signal of the corresponding block.
-		 * datasheet doesn't require explicit delay between the set and clear
-		 * of reset bit. it could be shorter if tests pass.
+		 * Writing 1 to this bit resets corresponding block.
+		 * Writing 0 to this bit de-asserts reset signal of the
+		 * corresponding block. datasheet doesn't require explicit
+		 * delay between the set and clear of reset bit. it could
+		 * be shorter if tests pass.
 		 */
-		writel(readl(sirfsoc_rstc_base + (reset_bit / 32) * 4) | (1 << reset_bit),
+		writel(readl(sirfsoc_rstc_base +
+			(reset_bit / 32) * 4) | (1 << reset_bit),
 			sirfsoc_rstc_base + (reset_bit / 32) * 4);
-		msleep(10);
-		writel(readl(sirfsoc_rstc_base + (reset_bit / 32) * 4) & ~(1 << reset_bit),
+		msleep(20);
+		writel(readl(sirfsoc_rstc_base +
+			(reset_bit / 32) * 4) & ~(1 << reset_bit),
 			sirfsoc_rstc_base + (reset_bit / 32) * 4);
 	} else {
 		/*
 		 * For MARCO and POLO
-		 * Writing 1 to SET register resets corresponding block. Writing 1 to CLEAR
-		 * register de-asserts reset signal of the corresponding block.
-		 * datasheet doesn't require explicit delay between the set and clear
-		 * of reset bit. it could be shorter if tests pass.
+		 * Writing 1 to SET register resets corresponding block.
+		 * Writing 1 to CLEAR register de-asserts reset signal of the
+		 * corresponding block.
+		 * datasheet doesn't require explicit delay between the set and
+		 * clear of reset bit. it could be shorter if tests pass.
 		 */
-		writel(1 << reset_bit, sirfsoc_rstc_base + (reset_bit / 32) * 8);
-		msleep(10);
-		writel(1 << reset_bit, sirfsoc_rstc_base + (reset_bit / 32) * 8 + 4);
+		writel(1 << reset_bit,
+			sirfsoc_rstc_base + (reset_bit / 32) * 8);
+		msleep(20);
+		writel(1 << reset_bit,
+			sirfsoc_rstc_base + (reset_bit / 32) * 8 + 4);
 	}
 
 	mutex_unlock(&rstc_lock);
diff --git a/arch/arm/mach-pxa/include/mach/hx4700.h b/arch/arm/mach-pxa/include/mach/hx4700.h
index 8bc02913517..0e1bb46264f 100644
--- a/arch/arm/mach-pxa/include/mach/hx4700.h
+++ b/arch/arm/mach-pxa/include/mach/hx4700.h
@@ -14,6 +14,7 @@
 
 #include <linux/gpio.h>
 #include <linux/mfd/asic3.h>
+#include "irqs.h" /* PXA_NR_BUILTIN_GPIO */
 
 #define HX4700_ASIC3_GPIO_BASE	PXA_NR_BUILTIN_GPIO
 #define HX4700_EGPIO_BASE	(HX4700_ASIC3_GPIO_BASE + ASIC3_NUM_GPIOS)
diff --git a/arch/arm/mach-pxa/zeus.c b/arch/arm/mach-pxa/zeus.c
index b19d1c361ca..205f9bf3821 100644
--- a/arch/arm/mach-pxa/zeus.c
+++ b/arch/arm/mach-pxa/zeus.c
@@ -413,7 +413,7 @@ static struct fixed_voltage_config can_regulator_pdata = {
 
 static struct platform_device can_regulator_device = {
 	.name	= "reg-fixed-volage",
-	.id	= -1,
+	.id	= 0,
 	.dev	= {
 		.platform_data	= &can_regulator_pdata,
 	},
@@ -510,18 +510,6 @@ struct platform_device zeus_max6369_device = {
 	.num_resources	= 1,
 };
 
-static struct platform_device *zeus_devices[] __initdata = {
-	&zeus_serial_device,
-	&zeus_mtd_devices[0],
-	&zeus_dm9k0_device,
-	&zeus_dm9k1_device,
-	&zeus_sram_device,
-	&zeus_leds_device,
-	&zeus_pcmcia_device,
-	&zeus_max6369_device,
-	&can_regulator_device,
-};
-
 /* AC'97 */
 static pxa2xx_audio_ops_t zeus_ac97_info = {
 	.reset_gpio = 95,
@@ -532,44 +520,50 @@ static pxa2xx_audio_ops_t zeus_ac97_info = {
  * USB host
  */
 
-static int zeus_ohci_init(struct device *dev)
-{
-	int err;
-
-	/* Switch on port 2. */
-	if ((err = gpio_request(ZEUS_USB2_PWREN_GPIO, "USB2_PWREN"))) {
-		dev_err(dev, "Can't request USB2_PWREN\n");
-		return err;
-	}
-
-	if ((err = gpio_direction_output(ZEUS_USB2_PWREN_GPIO, 1))) {
-		gpio_free(ZEUS_USB2_PWREN_GPIO);
-		dev_err(dev, "Can't enable USB2_PWREN\n");
-		return err;
-	}
+static struct regulator_consumer_supply zeus_ohci_regulator_supplies[] = {
+	REGULATOR_SUPPLY("vbus2", "pxa27x-ohci"),
+};
 
-	/* Port 2 is shared between host and client interface. */
-	UP2OCR = UP2OCR_HXOE | UP2OCR_HXS | UP2OCR_DMPDE | UP2OCR_DPPDE;
+static struct regulator_init_data zeus_ohci_regulator_data = {
+	.constraints = {
+		.valid_ops_mask		= REGULATOR_CHANGE_STATUS,
+	},
+	.num_consumer_supplies	= ARRAY_SIZE(zeus_ohci_regulator_supplies),
+	.consumer_supplies	= zeus_ohci_regulator_supplies,
+};
 
-	return 0;
-}
+static struct fixed_voltage_config zeus_ohci_regulator_config = {
+	.supply_name		= "vbus2",
+	.microvolts		= 5000000, /* 5.0V */
+	.gpio			= ZEUS_USB2_PWREN_GPIO,
+	.enable_high		= 1,
+	.startup_delay		= 0,
+	.init_data		= &zeus_ohci_regulator_data,
+};
 
-static void zeus_ohci_exit(struct device *dev)
-{
-	/* Power-off port 2 */
-	gpio_direction_output(ZEUS_USB2_PWREN_GPIO, 0);
-	gpio_free(ZEUS_USB2_PWREN_GPIO);
-}
+static struct platform_device zeus_ohci_regulator_device = {
+	.name		= "reg-fixed-voltage",
+	.id		= 1,
+	.dev = {
+		.platform_data = &zeus_ohci_regulator_config,
+	},
+};
 
 static struct pxaohci_platform_data zeus_ohci_platform_data = {
 	.port_mode	= PMM_NPS_MODE,
 	/* Clear Power Control Polarity Low and set Power Sense
 	 * Polarity Low. Supply power to USB ports. */
 	.flags		= ENABLE_PORT_ALL | POWER_SENSE_LOW,
-	.init		= zeus_ohci_init,
-	.exit		= zeus_ohci_exit,
 };
 
+static void zeus_register_ohci(void)
+{
+	/* Port 2 is shared between host and client interface. */
+	UP2OCR = UP2OCR_HXOE | UP2OCR_HXS | UP2OCR_DMPDE | UP2OCR_DPPDE;
+
+	pxa_set_ohci_info(&zeus_ohci_platform_data);
+}
+
 /*
  * Flat Panel
  */
@@ -677,6 +671,19 @@ static struct pxa2xx_udc_mach_info zeus_udc_info = {
 	.udc_command = zeus_udc_command,
 };
 
+static struct platform_device *zeus_devices[] __initdata = {
+	&zeus_serial_device,
+	&zeus_mtd_devices[0],
+	&zeus_dm9k0_device,
+	&zeus_dm9k1_device,
+	&zeus_sram_device,
+	&zeus_leds_device,
+	&zeus_pcmcia_device,
+	&zeus_max6369_device,
+	&can_regulator_device,
+	&zeus_ohci_regulator_device,
+};
+
 #ifdef CONFIG_PM
 static void zeus_power_off(void)
 {
@@ -847,7 +854,7 @@ static void __init zeus_init(void)
 
 	platform_add_devices(zeus_devices, ARRAY_SIZE(zeus_devices));
 
-	pxa_set_ohci_info(&zeus_ohci_platform_data);
+	zeus_register_ohci();
 
 	if (zeus_setup_fb_gpios())
 		pr_err("Failed to setup fb gpios\n");
diff --git a/arch/arm/mach-qcom/Kconfig b/arch/arm/mach-qcom/Kconfig
index a028be23433..fd2b99dceb8 100644
--- a/arch/arm/mach-qcom/Kconfig
+++ b/arch/arm/mach-qcom/Kconfig
@@ -2,9 +2,9 @@ config ARCH_QCOM
 	bool "Qualcomm Support" if ARCH_MULTI_V7
 	select ARCH_REQUIRE_GPIOLIB
 	select ARM_GIC
+	select ARM_AMBA
 	select CLKSRC_OF
-	select GENERIC_CLOCKEVENTS
-	select HAVE_SMP
+	select PINCTRL
 	select QCOM_SCM if SMP
 	help
 	  Support for Qualcomm's devicetree based systems.
diff --git a/arch/arm/mach-qcom/board.c b/arch/arm/mach-qcom/board.c
index bae617ef0b3..c437a994172 100644
--- a/arch/arm/mach-qcom/board.c
+++ b/arch/arm/mach-qcom/board.c
@@ -15,9 +15,11 @@
 #include <asm/mach/arch.h>
 
 static const char * const qcom_dt_match[] __initconst = {
+	"qcom,apq8064",
+	"qcom,apq8074-dragonboard",
+	"qcom,apq8084",
 	"qcom,msm8660-surf",
 	"qcom,msm8960-cdp",
-	"qcom,apq8074-dragonboard",
 	NULL
 };
 
diff --git a/arch/arm/mach-realview/core.c b/arch/arm/mach-realview/core.c
index 1d5ee5c9a1d..960b8dd78c4 100644
--- a/arch/arm/mach-realview/core.c
+++ b/arch/arm/mach-realview/core.c
@@ -148,6 +148,21 @@ struct platform_device realview_cf_device = {
 	},
 };
 
+static struct resource realview_leds_resources[] = {
+	{
+		.start	= REALVIEW_SYS_BASE + REALVIEW_SYS_LED_OFFSET,
+		.end	= REALVIEW_SYS_BASE + REALVIEW_SYS_LED_OFFSET + 4,
+		.flags	= IORESOURCE_MEM,
+	},
+};
+
+struct platform_device realview_leds_device = {
+	.name		= "versatile-leds",
+	.id		= -1,
+	.num_resources	= ARRAY_SIZE(realview_leds_resources),
+	.resource	= realview_leds_resources,
+};
+
 static struct resource realview_i2c_resource = {
 	.start		= REALVIEW_I2C_BASE,
 	.end		= REALVIEW_I2C_BASE + SZ_4K - 1,
diff --git a/arch/arm/mach-realview/core.h b/arch/arm/mach-realview/core.h
index 602ca5ec52c..13dc830ef46 100644
--- a/arch/arm/mach-realview/core.h
+++ b/arch/arm/mach-realview/core.h
@@ -37,6 +37,7 @@ struct machine_desc;
 
 extern struct platform_device realview_flash_device;
 extern struct platform_device realview_cf_device;
+extern struct platform_device realview_leds_device;
 extern struct platform_device realview_i2c_device;
 extern struct mmci_platform_data realview_mmc0_plat_data;
 extern struct mmci_platform_data realview_mmc1_plat_data;
diff --git a/arch/arm/mach-realview/realview_eb.c b/arch/arm/mach-realview/realview_eb.c
index c85ddb2a0ad..6bb070e8012 100644
--- a/arch/arm/mach-realview/realview_eb.c
+++ b/arch/arm/mach-realview/realview_eb.c
@@ -452,6 +452,7 @@ static void __init realview_eb_init(void)
 	realview_flash_register(&realview_eb_flash_resource, 1);
 	platform_device_register(&realview_i2c_device);
 	platform_device_register(&char_lcd_device);
+	platform_device_register(&realview_leds_device);
 	eth_device_register();
 	realview_usb_register(realview_eb_isp1761_resources);
 
diff --git a/arch/arm/mach-realview/realview_pb1176.c b/arch/arm/mach-realview/realview_pb1176.c
index c5eade76461..173f2c15de4 100644
--- a/arch/arm/mach-realview/realview_pb1176.c
+++ b/arch/arm/mach-realview/realview_pb1176.c
@@ -367,6 +367,7 @@ static void __init realview_pb1176_init(void)
 	realview_usb_register(realview_pb1176_isp1761_resources);
 	platform_device_register(&pmu_device);
 	platform_device_register(&char_lcd_device);
+	platform_device_register(&realview_leds_device);
 
 	for (i = 0; i < ARRAY_SIZE(amba_devs); i++) {
 		struct amba_device *d = amba_devs[i];
diff --git a/arch/arm/mach-realview/realview_pb11mp.c b/arch/arm/mach-realview/realview_pb11mp.c
index f4b0962578f..bde7e6b1fd4 100644
--- a/arch/arm/mach-realview/realview_pb11mp.c
+++ b/arch/arm/mach-realview/realview_pb11mp.c
@@ -347,6 +347,7 @@ static void __init realview_pb11mp_init(void)
 	realview_eth_register(NULL, realview_pb11mp_smsc911x_resources);
 	platform_device_register(&realview_i2c_device);
 	platform_device_register(&realview_cf_device);
+	platform_device_register(&realview_leds_device);
 	realview_usb_register(realview_pb11mp_isp1761_resources);
 	platform_device_register(&pmu_device);
 
diff --git a/arch/arm/mach-realview/realview_pba8.c b/arch/arm/mach-realview/realview_pba8.c
index 10a3e1d7689..4e57a859926 100644
--- a/arch/arm/mach-realview/realview_pba8.c
+++ b/arch/arm/mach-realview/realview_pba8.c
@@ -289,6 +289,7 @@ static void __init realview_pba8_init(void)
 	realview_eth_register(NULL, realview_pba8_smsc911x_resources);
 	platform_device_register(&realview_i2c_device);
 	platform_device_register(&realview_cf_device);
+	platform_device_register(&realview_leds_device);
 	realview_usb_register(realview_pba8_isp1761_resources);
 	platform_device_register(&pmu_device);
 
diff --git a/arch/arm/mach-realview/realview_pbx.c b/arch/arm/mach-realview/realview_pbx.c
index 9d75493e3f0..72c96caebef 100644
--- a/arch/arm/mach-realview/realview_pbx.c
+++ b/arch/arm/mach-realview/realview_pbx.c
@@ -385,6 +385,7 @@ static void __init realview_pbx_init(void)
 	realview_eth_register(NULL, realview_pbx_smsc911x_resources);
 	platform_device_register(&realview_i2c_device);
 	platform_device_register(&realview_cf_device);
+	platform_device_register(&realview_leds_device);
 	realview_usb_register(realview_pbx_isp1761_resources);
 
 	for (i = 0; i < ARRAY_SIZE(amba_devs); i++) {
diff --git a/arch/arm/mach-rockchip/core.h b/arch/arm/mach-rockchip/core.h
index e2e7c9dbb20..39bca96b555 100644
--- a/arch/arm/mach-rockchip/core.h
+++ b/arch/arm/mach-rockchip/core.h
@@ -18,5 +18,3 @@ extern char rockchip_secondary_trampoline_end;
 
 extern unsigned long rockchip_boot_fn;
 extern void rockchip_secondary_startup(void);
-
-extern struct smp_operations rockchip_smp_ops;
diff --git a/arch/arm/mach-rockchip/platsmp.c b/arch/arm/mach-rockchip/platsmp.c
index dbfa5a26cff..910835d4ccf 100644
--- a/arch/arm/mach-rockchip/platsmp.c
+++ b/arch/arm/mach-rockchip/platsmp.c
@@ -152,7 +152,7 @@ static void __init rockchip_smp_prepare_cpus(unsigned int max_cpus)
 
 	node = of_find_compatible_node(NULL, NULL, "rockchip,rk3066-pmu");
 	if (!node) {
-		pr_err("%s: could not find sram dt node\n", __func__);
+		pr_err("%s: could not find pmu dt node\n", __func__);
 		return;
 	}
 
@@ -178,7 +178,8 @@ static void __init rockchip_smp_prepare_cpus(unsigned int max_cpus)
 		pmu_set_power_domain(0 + i, false);
 }
 
-struct smp_operations rockchip_smp_ops __initdata = {
+static struct smp_operations rockchip_smp_ops __initdata = {
 	.smp_prepare_cpus	= rockchip_smp_prepare_cpus,
 	.smp_boot_secondary	= rockchip_boot_secondary,
 };
+CPU_METHOD_OF_DECLARE(rk3066_smp, "rockchip,rk3066-smp", &rockchip_smp_ops);
diff --git a/arch/arm/mach-rockchip/rockchip.c b/arch/arm/mach-rockchip/rockchip.c
index d211d6fa0d9..4499b0a31a2 100644
--- a/arch/arm/mach-rockchip/rockchip.c
+++ b/arch/arm/mach-rockchip/rockchip.c
@@ -39,7 +39,6 @@ static const char * const rockchip_board_dt_compat[] = {
 };
 
 DT_MACHINE_START(ROCKCHIP_DT, "Rockchip Cortex-A9 (Device Tree)")
-	.smp		= smp_ops(rockchip_smp_ops),
 	.init_machine	= rockchip_dt_init,
 	.dt_compat	= rockchip_board_dt_compat,
 MACHINE_END
diff --git a/arch/arm/mach-s3c24xx/Kconfig b/arch/arm/mach-s3c24xx/Kconfig
index 40cf50b9940..04284de7aca 100644
--- a/arch/arm/mach-s3c24xx/Kconfig
+++ b/arch/arm/mach-s3c24xx/Kconfig
@@ -18,6 +18,8 @@ config PLAT_S3C24XX
 	help
 	  Base platform code for any Samsung S3C24XX device
 
+
+
 menu "SAMSUNG S3C24XX SoCs Support"
 
 comment "S3C24XX SoCs"
@@ -26,8 +28,7 @@ config CPU_S3C2410
 	bool "SAMSUNG S3C2410"
 	default y
 	select CPU_ARM920T
-	select CPU_LLSERIAL_S3C2410
-	select S3C2410_CLOCK
+	select S3C2410_COMMON_CLK
 	select S3C2410_DMA if S3C24XX_DMA
 	select ARM_S3C2410_CPUFREQ if ARM_S3C24XX_CPUFREQ
 	select S3C2410_PM if PM
@@ -39,7 +40,7 @@ config CPU_S3C2410
 config CPU_S3C2412
 	bool "SAMSUNG S3C2412"
 	select CPU_ARM926T
-	select CPU_LLSERIAL_S3C2440
+	select S3C2412_COMMON_CLK
 	select S3C2412_DMA if S3C24XX_DMA
 	select S3C2412_PM if PM
 	help
@@ -48,19 +49,16 @@ config CPU_S3C2412
 config CPU_S3C2416
 	bool "SAMSUNG S3C2416/S3C2450"
 	select CPU_ARM926T
-	select CPU_LLSERIAL_S3C2440
 	select S3C2416_PM if PM
-	select S3C2443_COMMON
+	select S3C2443_COMMON_CLK
 	select S3C2443_DMA if S3C24XX_DMA
-	select SAMSUNG_CLKSRC
 	help
 	  Support for the S3C2416 SoC from the S3C24XX line
 
 config CPU_S3C2440
 	bool "SAMSUNG S3C2440"
 	select CPU_ARM920T
-	select CPU_LLSERIAL_S3C2440
-	select S3C2410_CLOCK
+	select S3C2410_COMMON_CLK
 	select S3C2410_PM if PM
 	select S3C2440_DMA if S3C24XX_DMA
 	help
@@ -69,8 +67,7 @@ config CPU_S3C2440
 config CPU_S3C2442
 	bool "SAMSUNG S3C2442"
 	select CPU_ARM920T
-	select CPU_LLSERIAL_S3C2440
-	select S3C2410_CLOCK
+	select S3C2410_COMMON_CLK
 	select S3C2410_DMA if S3C24XX_DMA
 	select S3C2410_PM if PM
 	help
@@ -84,26 +81,13 @@ config CPU_S3C244X
 config CPU_S3C2443
 	bool "SAMSUNG S3C2443"
 	select CPU_ARM920T
-	select CPU_LLSERIAL_S3C2440
-	select S3C2443_COMMON
+	select S3C2443_COMMON_CLK
 	select S3C2443_DMA if S3C24XX_DMA
-	select SAMSUNG_CLKSRC
 	help
 	  Support for the S3C2443 SoC from the S3C24XX line
 
 # common code
 
-config S3C2410_CLOCK
-	bool
-	help
-	  Clock code for the S3C2410, and similar processors which
-	  is currently includes the S3C2410, S3C2440, S3C2442.
-
-config S3C24XX_DCLK
-	bool
-	help
-	  Clock code for supporting DCLK/CLKOUT on S3C24XX architectures
-
 config S3C24XX_SMDK
 	bool
 	help
@@ -158,28 +142,6 @@ config S3C2410_PM
 	help
 	  Power Management code common to S3C2410 and better
 
-# low-level serial option nodes
-
-config CPU_LLSERIAL_S3C2410_ONLY
-	bool
-	default y if CPU_LLSERIAL_S3C2410 && !CPU_LLSERIAL_S3C2440
-
-config CPU_LLSERIAL_S3C2440_ONLY
-	bool
-	default y if CPU_LLSERIAL_S3C2440 && !CPU_LLSERIAL_S3C2410
-
-config CPU_LLSERIAL_S3C2410
-	bool
-	help
-	  Selected if there is an S3C2410 (or register compatible) serial
-	  low-level implementation needed
-
-config CPU_LLSERIAL_S3C2440
-	bool
-	help
-	  Selected if there is an S3C2440 (or register compatible) serial
-	  low-level implementation needed
-
 config S3C24XX_PLL
 	bool "Support CPUfreq changing of PLL frequency (EXPERIMENTAL)"
 	depends on ARM_S3C24XX_CPUFREQ
@@ -258,8 +220,8 @@ config ARCH_BAST
 	bool "Simtec Electronics BAST (EB2410ITX)"
 	select ISA
 	select MACH_BAST_IDE
+	select S3C2410_COMMON_DCLK
 	select S3C2410_IOTIMING if ARM_S3C2410_CPUFREQ
-	select S3C24XX_DCLK
 	select S3C24XX_SIMTEC_NOR
 	select S3C24XX_SIMTEC_PM if PM
 	select S3C24XX_SIMTEC_USB
@@ -340,7 +302,7 @@ config MACH_TCT_HAMMER
 config MACH_VR1000
 	bool "Thorcom VR1000"
 	select MACH_BAST_IDE
-	select S3C24XX_DCLK
+	select S3C2410_COMMON_DCLK
 	select S3C24XX_SIMTEC_NOR
 	select S3C24XX_SIMTEC_PM if PM
 	select S3C24XX_SIMTEC_USB
@@ -519,8 +481,8 @@ comment "S3C2440 Boards"
 config MACH_ANUBIS
 	bool "Simtec Electronics ANUBIS"
 	select HAVE_PATA_PLATFORM
+	select S3C2410_COMMON_DCLK
 	select S3C2440_XTAL_12000000
-	select S3C24XX_DCLK
 	select S3C24XX_SIMTEC_PM if PM
 	select S3C_DEV_USB_HOST
 	help
@@ -558,9 +520,9 @@ config MACH_NEXCODER_2440
 
 config MACH_OSIRIS
 	bool "Simtec IM2440D20 (OSIRIS) module"
+	select S3C2410_COMMON_DCLK
 	select S3C2410_IOTIMING if ARM_S3C2440_CPUFREQ
 	select S3C2440_XTAL_12000000
-	select S3C24XX_DCLK
 	select S3C24XX_SIMTEC_PM if PM
 	select S3C_DEV_NAND
 	select S3C_DEV_USB_HOST
@@ -629,9 +591,9 @@ config MACH_RX1950
 	bool "HP iPAQ rx1950"
 	select I2C
 	select PM_H1940 if PM
+	select S3C2410_COMMON_DCLK
 	select S3C2410_IOTIMING if ARM_S3C2440_CPUFREQ
 	select S3C2440_XTAL_16934400
-	select S3C24XX_DCLK
 	select S3C24XX_PWM
 	select S3C_DEV_NAND
 	help
@@ -641,12 +603,6 @@ endif	# CPU_S3C2442
 
 if CPU_S3C2443 || CPU_S3C2416
 
-config S3C2443_COMMON
-	bool
-	help
-	  Common code for the S3C2443 and similar processors, which includes
-	  the S3C2416 and S3C2450.
-
 config S3C2443_DMA
 	bool
 	help
diff --git a/arch/arm/mach-s3c24xx/Makefile b/arch/arm/mach-s3c24xx/Makefile
index 7f54e5b954c..2235d0d3b38 100644
--- a/arch/arm/mach-s3c24xx/Makefile
+++ b/arch/arm/mach-s3c24xx/Makefile
@@ -21,22 +21,22 @@ obj-$(CONFIG_S3C2410_DMA)	+= dma-s3c2410.o
 obj-$(CONFIG_S3C2410_PLL)	+= pll-s3c2410.o
 obj-$(CONFIG_S3C2410_PM)	+= pm-s3c2410.o sleep-s3c2410.o
 
-obj-$(CONFIG_CPU_S3C2412)	+= s3c2412.o clock-s3c2412.o
+obj-$(CONFIG_CPU_S3C2412)	+= s3c2412.o
 obj-$(CONFIG_S3C2412_DMA)	+= dma-s3c2412.o
 obj-$(CONFIG_S3C2412_PM)	+= pm-s3c2412.o
 obj-$(CONFIG_S3C2412_PM_SLEEP)	+= sleep-s3c2412.o
 
-obj-$(CONFIG_CPU_S3C2416)	+= s3c2416.o clock-s3c2416.o
+obj-$(CONFIG_CPU_S3C2416)	+= s3c2416.o
 obj-$(CONFIG_S3C2416_PM)	+= pm-s3c2416.o
 
-obj-$(CONFIG_CPU_S3C2440)	+= s3c2440.o clock-s3c2440.o
+obj-$(CONFIG_CPU_S3C2440)	+= s3c2440.o
 obj-$(CONFIG_CPU_S3C2442)	+= s3c2442.o
-obj-$(CONFIG_CPU_S3C244X)	+= s3c244x.o clock-s3c244x.o
+obj-$(CONFIG_CPU_S3C244X)	+= s3c244x.o
 obj-$(CONFIG_S3C2440_DMA)	+= dma-s3c2440.o
 obj-$(CONFIG_S3C2440_PLL_12000000) += pll-s3c2440-12000000.o
 obj-$(CONFIG_S3C2440_PLL_16934400) += pll-s3c2440-16934400.o
 
-obj-$(CONFIG_CPU_S3C2443)	+= s3c2443.o clock-s3c2443.o
+obj-$(CONFIG_CPU_S3C2443)	+= s3c2443.o
 
 # PM
 
@@ -44,16 +44,13 @@ obj-$(CONFIG_PM)		+= pm.o irq-pm.o sleep.o
 
 # common code
 
-obj-$(CONFIG_S3C24XX_DCLK)	+= clock-dclk.o
 obj-$(CONFIG_S3C24XX_DMA)	+= dma.o
 
-obj-$(CONFIG_S3C2410_CLOCK)	+= clock-s3c2410.o
 obj-$(CONFIG_S3C2410_CPUFREQ_UTILS) += cpufreq-utils.o
 
 obj-$(CONFIG_S3C2410_IOTIMING)	+= iotiming-s3c2410.o
 obj-$(CONFIG_S3C2412_IOTIMING)	+= iotiming-s3c2412.o
 
-obj-$(CONFIG_S3C2443_COMMON)	+= common-s3c2443.o
 obj-$(CONFIG_S3C2443_DMA)	+= dma-s3c2443.o
 
 #
diff --git a/arch/arm/mach-s3c24xx/clock-dclk.c b/arch/arm/mach-s3c24xx/clock-dclk.c
deleted file mode 100644
index 1edd9b2369c..00000000000
--- a/arch/arm/mach-s3c24xx/clock-dclk.c
+++ /dev/null
@@ -1,195 +0,0 @@
-/*
- * Copyright (c) 2004-2008 Simtec Electronics
- *	Ben Dooks <ben@simtec.co.uk>
- *	http://armlinux.simtec.co.uk/
- *
- * 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.
- *
- * S3C24XX - definitions for DCLK and CLKOUT registers
- */
-
-#include <linux/kernel.h>
-#include <linux/errno.h>
-#include <linux/clk.h>
-#include <linux/io.h>
-
-#include <mach/regs-clock.h>
-#include <mach/regs-gpio.h>
-
-#include <plat/clock.h>
-#include <plat/cpu.h>
-
-/* clocks that could be registered by external code */
-
-static int s3c24xx_dclk_enable(struct clk *clk, int enable)
-{
-	unsigned long dclkcon = __raw_readl(S3C24XX_DCLKCON);
-
-	if (enable)
-		dclkcon |= clk->ctrlbit;
-	else
-		dclkcon &= ~clk->ctrlbit;
-
-	__raw_writel(dclkcon, S3C24XX_DCLKCON);
-
-	return 0;
-}
-
-static int s3c24xx_dclk_setparent(struct clk *clk, struct clk *parent)
-{
-	unsigned long dclkcon;
-	unsigned int uclk;
-
-	if (parent == &clk_upll)
-		uclk = 1;
-	else if (parent == &clk_p)
-		uclk = 0;
-	else
-		return -EINVAL;
-
-	clk->parent = parent;
-
-	dclkcon = __raw_readl(S3C24XX_DCLKCON);
-
-	if (clk->ctrlbit == S3C2410_DCLKCON_DCLK0EN) {
-		if (uclk)
-			dclkcon |= S3C2410_DCLKCON_DCLK0_UCLK;
-		else
-			dclkcon &= ~S3C2410_DCLKCON_DCLK0_UCLK;
-	} else {
-		if (uclk)
-			dclkcon |= S3C2410_DCLKCON_DCLK1_UCLK;
-		else
-			dclkcon &= ~S3C2410_DCLKCON_DCLK1_UCLK;
-	}
-
-	__raw_writel(dclkcon, S3C24XX_DCLKCON);
-
-	return 0;
-}
-static unsigned long s3c24xx_calc_div(struct clk *clk, unsigned long rate)
-{
-	unsigned long div;
-
-	if ((rate == 0) || !clk->parent)
-		return 0;
-
-	div = clk_get_rate(clk->parent) / rate;
-	if (div < 2)
-		div = 2;
-	else if (div > 16)
-		div = 16;
-
-	return div;
-}
-
-static unsigned long s3c24xx_round_dclk_rate(struct clk *clk,
-	unsigned long rate)
-{
-	unsigned long div = s3c24xx_calc_div(clk, rate);
-
-	if (div == 0)
-		return 0;
-
-	return clk_get_rate(clk->parent) / div;
-}
-
-static int s3c24xx_set_dclk_rate(struct clk *clk, unsigned long rate)
-{
-	unsigned long mask, data, div = s3c24xx_calc_div(clk, rate);
-
-	if (div == 0)
-		return -EINVAL;
-
-	if (clk == &s3c24xx_dclk0) {
-		mask = S3C2410_DCLKCON_DCLK0_DIV_MASK |
-			S3C2410_DCLKCON_DCLK0_CMP_MASK;
-		data = S3C2410_DCLKCON_DCLK0_DIV(div) |
-			S3C2410_DCLKCON_DCLK0_CMP((div + 1) / 2);
-	} else if (clk == &s3c24xx_dclk1) {
-		mask = S3C2410_DCLKCON_DCLK1_DIV_MASK |
-			S3C2410_DCLKCON_DCLK1_CMP_MASK;
-		data = S3C2410_DCLKCON_DCLK1_DIV(div) |
-			S3C2410_DCLKCON_DCLK1_CMP((div + 1) / 2);
-	} else
-		return -EINVAL;
-
-	clk->rate = clk_get_rate(clk->parent) / div;
-	__raw_writel(((__raw_readl(S3C24XX_DCLKCON) & ~mask) | data),
-		S3C24XX_DCLKCON);
-	return clk->rate;
-}
-static int s3c24xx_clkout_setparent(struct clk *clk, struct clk *parent)
-{
-	unsigned long mask;
-	unsigned long source;
-
-	/* calculate the MISCCR setting for the clock */
-
-	if (parent == &clk_mpll)
-		source = S3C2410_MISCCR_CLK0_MPLL;
-	else if (parent == &clk_upll)
-		source = S3C2410_MISCCR_CLK0_UPLL;
-	else if (parent == &clk_f)
-		source = S3C2410_MISCCR_CLK0_FCLK;
-	else if (parent == &clk_h)
-		source = S3C2410_MISCCR_CLK0_HCLK;
-	else if (parent == &clk_p)
-		source = S3C2410_MISCCR_CLK0_PCLK;
-	else if (clk == &s3c24xx_clkout0 && parent == &s3c24xx_dclk0)
-		source = S3C2410_MISCCR_CLK0_DCLK0;
-	else if (clk == &s3c24xx_clkout1 && parent == &s3c24xx_dclk1)
-		source = S3C2410_MISCCR_CLK0_DCLK0;
-	else
-		return -EINVAL;
-
-	clk->parent = parent;
-
-	if (clk == &s3c24xx_clkout0)
-		mask = S3C2410_MISCCR_CLK0_MASK;
-	else {
-		source <<= 4;
-		mask = S3C2410_MISCCR_CLK1_MASK;
-	}
-
-	s3c2410_modify_misccr(mask, source);
-	return 0;
-}
-
-/* external clock definitions */
-
-static struct clk_ops dclk_ops = {
-	.set_parent	= s3c24xx_dclk_setparent,
-	.set_rate	= s3c24xx_set_dclk_rate,
-	.round_rate	= s3c24xx_round_dclk_rate,
-};
-
-struct clk s3c24xx_dclk0 = {
-	.name		= "dclk0",
-	.ctrlbit	= S3C2410_DCLKCON_DCLK0EN,
-	.enable	        = s3c24xx_dclk_enable,
-	.ops		= &dclk_ops,
-};
-
-struct clk s3c24xx_dclk1 = {
-	.name		= "dclk1",
-	.ctrlbit	= S3C2410_DCLKCON_DCLK1EN,
-	.enable		= s3c24xx_dclk_enable,
-	.ops		= &dclk_ops,
-};
-
-static struct clk_ops clkout_ops = {
-	.set_parent	= s3c24xx_clkout_setparent,
-};
-
-struct clk s3c24xx_clkout0 = {
-	.name		= "clkout0",
-	.ops		= &clkout_ops,
-};
-
-struct clk s3c24xx_clkout1 = {
-	.name		= "clkout1",
-	.ops		= &clkout_ops,
-};
diff --git a/arch/arm/mach-s3c24xx/clock-s3c2410.c b/arch/arm/mach-s3c24xx/clock-s3c2410.c
deleted file mode 100644
index d1afcf9252d..00000000000
--- a/arch/arm/mach-s3c24xx/clock-s3c2410.c
+++ /dev/null
@@ -1,284 +0,0 @@
-/*
- * Copyright (c) 2006 Simtec Electronics
- *	Ben Dooks <ben@simtec.co.uk>
- *
- * S3C2410,S3C2440,S3C2442 Clock control support
- *
- * 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/init.h>
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/list.h>
-#include <linux/errno.h>
-#include <linux/err.h>
-#include <linux/device.h>
-#include <linux/clk.h>
-#include <linux/mutex.h>
-#include <linux/delay.h>
-#include <linux/serial_core.h>
-#include <linux/serial_s3c.h>
-#include <linux/io.h>
-
-#include <asm/mach/map.h>
-
-#include <mach/hardware.h>
-#include <mach/regs-clock.h>
-#include <mach/regs-gpio.h>
-
-#include <plat/clock.h>
-#include <plat/cpu.h>
-
-int s3c2410_clkcon_enable(struct clk *clk, int enable)
-{
-	unsigned int clocks = clk->ctrlbit;
-	unsigned long clkcon;
-
-	clkcon = __raw_readl(S3C2410_CLKCON);
-
-	if (enable)
-		clkcon |= clocks;
-	else
-		clkcon &= ~clocks;
-
-	/* ensure none of the special function bits set */
-	clkcon &= ~(S3C2410_CLKCON_IDLE|S3C2410_CLKCON_POWER);
-
-	__raw_writel(clkcon, S3C2410_CLKCON);
-
-	return 0;
-}
-
-static int s3c2410_upll_enable(struct clk *clk, int enable)
-{
-	unsigned long clkslow = __raw_readl(S3C2410_CLKSLOW);
-	unsigned long orig = clkslow;
-
-	if (enable)
-		clkslow &= ~S3C2410_CLKSLOW_UCLK_OFF;
-	else
-		clkslow |= S3C2410_CLKSLOW_UCLK_OFF;
-
-	__raw_writel(clkslow, S3C2410_CLKSLOW);
-
-	/* if we started the UPLL, then allow to settle */
-
-	if (enable && (orig & S3C2410_CLKSLOW_UCLK_OFF))
-		udelay(200);
-
-	return 0;
-}
-
-/* standard clock definitions */
-
-static struct clk init_clocks_off[] = {
-	{
-		.name		= "nand",
-		.parent		= &clk_h,
-		.enable		= s3c2410_clkcon_enable,
-		.ctrlbit	= S3C2410_CLKCON_NAND,
-	}, {
-		.name		= "sdi",
-		.parent		= &clk_p,
-		.enable		= s3c2410_clkcon_enable,
-		.ctrlbit	= S3C2410_CLKCON_SDI,
-	}, {
-		.name		= "adc",
-		.parent		= &clk_p,
-		.enable		= s3c2410_clkcon_enable,
-		.ctrlbit	= S3C2410_CLKCON_ADC,
-	}, {
-		.name		= "i2c",
-		.parent		= &clk_p,
-		.enable		= s3c2410_clkcon_enable,
-		.ctrlbit	= S3C2410_CLKCON_IIC,
-	}, {
-		.name		= "iis",
-		.parent		= &clk_p,
-		.enable		= s3c2410_clkcon_enable,
-		.ctrlbit	= S3C2410_CLKCON_IIS,
-	}, {
-		.name		= "spi",
-		.parent		= &clk_p,
-		.enable		= s3c2410_clkcon_enable,
-		.ctrlbit	= S3C2410_CLKCON_SPI,
-	}
-};
-
-static struct clk clk_lcd = {
-	.name		= "lcd",
-	.parent		= &clk_h,
-	.enable		= s3c2410_clkcon_enable,
-	.ctrlbit	= S3C2410_CLKCON_LCDC,
-};
-
-static struct clk clk_gpio = {
-	.name		= "gpio",
-	.parent		= &clk_p,
-	.enable		= s3c2410_clkcon_enable,
-	.ctrlbit	= S3C2410_CLKCON_GPIO,
-};
-
-static struct clk clk_usb_host = {
-	.name		= "usb-host",
-	.parent		= &clk_h,
-	.enable		= s3c2410_clkcon_enable,
-	.ctrlbit	= S3C2410_CLKCON_USBH,
-};
-
-static struct clk clk_usb_device = {
-	.name		= "usb-device",
-	.parent		= &clk_h,
-	.enable		= s3c2410_clkcon_enable,
-	.ctrlbit	= S3C2410_CLKCON_USBD,
-};
-
-static struct clk clk_timers = {
-	.name		= "timers",
-	.parent		= &clk_p,
-	.enable		= s3c2410_clkcon_enable,
-	.ctrlbit	= S3C2410_CLKCON_PWMT,
-};
-
-struct clk s3c24xx_clk_uart0 = {
-	.name		= "uart",
-	.devname	= "s3c2410-uart.0",
-	.parent		= &clk_p,
-	.enable		= s3c2410_clkcon_enable,
-	.ctrlbit	= S3C2410_CLKCON_UART0,
-};
-
-struct clk s3c24xx_clk_uart1 = {
-	.name		= "uart",
-	.devname	= "s3c2410-uart.1",
-	.parent		= &clk_p,
-	.enable		= s3c2410_clkcon_enable,
-	.ctrlbit	= S3C2410_CLKCON_UART1,
-};
-
-struct clk s3c24xx_clk_uart2 = {
-	.name		= "uart",
-	.devname	= "s3c2410-uart.2",
-	.parent		= &clk_p,
-	.enable		= s3c2410_clkcon_enable,
-	.ctrlbit	= S3C2410_CLKCON_UART2,
-};
-
-static struct clk clk_rtc = {
-	.name		= "rtc",
-	.parent		= &clk_p,
-	.enable		= s3c2410_clkcon_enable,
-	.ctrlbit	= S3C2410_CLKCON_RTC,
-};
-
-static struct clk clk_watchdog = {
-	.name		= "watchdog",
-	.parent		= &clk_p,
-	.ctrlbit	= 0,
-};
-
-static struct clk clk_usb_bus_host = {
-	.name		= "usb-bus-host",
-	.parent		= &clk_usb_bus,
-};
-
-static struct clk clk_usb_bus_gadget = {
-	.name		= "usb-bus-gadget",
-	.parent		= &clk_usb_bus,
-};
-
-static struct clk *init_clocks[] = {
-	&clk_lcd,
-	&clk_gpio,
-	&clk_usb_host,
-	&clk_usb_device,
-	&clk_timers,
-	&s3c24xx_clk_uart0,
-	&s3c24xx_clk_uart1,
-	&s3c24xx_clk_uart2,
-	&clk_rtc,
-	&clk_watchdog,
-	&clk_usb_bus_host,
-	&clk_usb_bus_gadget,
-};
-
-/* s3c2410_baseclk_add()
- *
- * Add all the clocks used by the s3c2410 or compatible CPUs
- * such as the S3C2440 and S3C2442.
- *
- * We cannot use a system device as we are needed before any
- * of the init-calls that initialise the devices are actually
- * done.
-*/
-
-int __init s3c2410_baseclk_add(void)
-{
-	unsigned long clkslow = __raw_readl(S3C2410_CLKSLOW);
-	unsigned long clkcon  = __raw_readl(S3C2410_CLKCON);
-	struct clk *xtal;
-	int ret;
-	int ptr;
-
-	clk_upll.enable = s3c2410_upll_enable;
-
-	if (s3c24xx_register_clock(&clk_usb_bus) < 0)
-		printk(KERN_ERR "failed to register usb bus clock\n");
-
-	/* register clocks from clock array */
-
-	for (ptr = 0; ptr < ARRAY_SIZE(init_clocks); ptr++) {
-		struct clk *clkp = init_clocks[ptr];
-
-		/* ensure that we note the clock state */
-
-		clkp->usage = clkcon & clkp->ctrlbit ? 1 : 0;
-
-		ret = s3c24xx_register_clock(clkp);
-		if (ret < 0) {
-			printk(KERN_ERR "Failed to register clock %s (%d)\n",
-			       clkp->name, ret);
-		}
-	}
-
-	/* We must be careful disabling the clocks we are not intending to
-	 * be using at boot time, as subsystems such as the LCD which do
-	 * their own DMA requests to the bus can cause the system to lockup
-	 * if they where in the middle of requesting bus access.
-	 *
-	 * Disabling the LCD clock if the LCD is active is very dangerous,
-	 * and therefore the bootloader should be careful to not enable
-	 * the LCD clock if it is not needed.
-	*/
-
-	/* install (and disable) the clocks we do not need immediately */
-
-	s3c_register_clocks(init_clocks_off, ARRAY_SIZE(init_clocks_off));
-	s3c_disable_clocks(init_clocks_off, ARRAY_SIZE(init_clocks_off));
-
-	/* show the clock-slow value */
-
-	xtal = clk_get(NULL, "xtal");
-
-	printk("CLOCK: Slow mode (%ld.%ld MHz), %s, MPLL %s, UPLL %s\n",
-	       print_mhz(clk_get_rate(xtal) /
-			 ( 2 * S3C2410_CLKSLOW_GET_SLOWVAL(clkslow))),
-	       (clkslow & S3C2410_CLKSLOW_SLOW) ? "slow" : "fast",
-	       (clkslow & S3C2410_CLKSLOW_MPLL_OFF) ? "off" : "on",
-	       (clkslow & S3C2410_CLKSLOW_UCLK_OFF) ? "off" : "on");
-
-	return 0;
-}
diff --git a/arch/arm/mach-s3c24xx/clock-s3c2412.c b/arch/arm/mach-s3c24xx/clock-s3c2412.c
deleted file mode 100644
index 192a5b2550b..00000000000
--- a/arch/arm/mach-s3c24xx/clock-s3c2412.c
+++ /dev/null
@@ -1,760 +0,0 @@
-/* linux/arch/arm/mach-s3c2412/clock.c
- *
- * Copyright (c) 2006 Simtec Electronics
- *	Ben Dooks <ben@simtec.co.uk>
- *
- * S3C2412,S3C2413 Clock control support
- *
- * 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/init.h>
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/list.h>
-#include <linux/errno.h>
-#include <linux/err.h>
-#include <linux/device.h>
-#include <linux/clk.h>
-#include <linux/mutex.h>
-#include <linux/delay.h>
-#include <linux/serial_core.h>
-#include <linux/serial_s3c.h>
-#include <linux/io.h>
-
-#include <asm/mach/map.h>
-
-#include <mach/hardware.h>
-#include <mach/regs-clock.h>
-#include <mach/regs-gpio.h>
-
-#include <plat/clock.h>
-#include <plat/cpu.h>
-
-/* We currently have to assume that the system is running
- * from the XTPll input, and that all ***REFCLKs are being
- * fed from it, as we cannot read the state of OM[4] from
- * software.
- *
- * It would be possible for each board initialisation to
- * set the correct muxing at initialisation
-*/
-
-static int s3c2412_clkcon_enable(struct clk *clk, int enable)
-{
-	unsigned int clocks = clk->ctrlbit;
-	unsigned long clkcon;
-
-	clkcon = __raw_readl(S3C2410_CLKCON);
-
-	if (enable)
-		clkcon |= clocks;
-	else
-		clkcon &= ~clocks;
-
-	__raw_writel(clkcon, S3C2410_CLKCON);
-
-	return 0;
-}
-
-static int s3c2412_upll_enable(struct clk *clk, int enable)
-{
-	unsigned long upllcon = __raw_readl(S3C2410_UPLLCON);
-	unsigned long orig = upllcon;
-
-	if (!enable)
-		upllcon |= S3C2412_PLLCON_OFF;
-	else
-		upllcon &= ~S3C2412_PLLCON_OFF;
-
-	__raw_writel(upllcon, S3C2410_UPLLCON);
-
-	/* allow ~150uS for the PLL to settle and lock */
-
-	if (enable && (orig & S3C2412_PLLCON_OFF))
-		udelay(150);
-
-	return 0;
-}
-
-/* clock selections */
-
-static struct clk clk_erefclk = {
-	.name		= "erefclk",
-};
-
-static struct clk clk_urefclk = {
-	.name		= "urefclk",
-};
-
-static int s3c2412_setparent_usysclk(struct clk *clk, struct clk *parent)
-{
-	unsigned long clksrc = __raw_readl(S3C2412_CLKSRC);
-
-	if (parent == &clk_urefclk)
-		clksrc &= ~S3C2412_CLKSRC_USYSCLK_UPLL;
-	else if (parent == &clk_upll)
-		clksrc |= S3C2412_CLKSRC_USYSCLK_UPLL;
-	else
-		return -EINVAL;
-
-	clk->parent = parent;
-
-	__raw_writel(clksrc, S3C2412_CLKSRC);
-	return 0;
-}
-
-static struct clk clk_usysclk = {
-	.name		= "usysclk",
-	.parent		= &clk_xtal,
-	.ops		= &(struct clk_ops) {
-		.set_parent	= s3c2412_setparent_usysclk,
-	},
-};
-
-static struct clk clk_mrefclk = {
-	.name		= "mrefclk",
-	.parent		= &clk_xtal,
-};
-
-static struct clk clk_mdivclk = {
-	.name		= "mdivclk",
-	.parent		= &clk_xtal,
-};
-
-static int s3c2412_setparent_usbsrc(struct clk *clk, struct clk *parent)
-{
-	unsigned long clksrc = __raw_readl(S3C2412_CLKSRC);
-
-	if (parent == &clk_usysclk)
-		clksrc &= ~S3C2412_CLKSRC_USBCLK_HCLK;
-	else if (parent == &clk_h)
-		clksrc |= S3C2412_CLKSRC_USBCLK_HCLK;
-	else
-		return -EINVAL;
-
-	clk->parent = parent;
-
-	__raw_writel(clksrc, S3C2412_CLKSRC);
-	return 0;
-}
-
-static unsigned long s3c2412_roundrate_usbsrc(struct clk *clk,
-					      unsigned long rate)
-{
-	unsigned long parent_rate = clk_get_rate(clk->parent);
-	int div;
-
-	if (rate > parent_rate)
-		return parent_rate;
-
-	div = parent_rate / rate;
-	if (div > 2)
-		div = 2;
-
-	return parent_rate / div;
-}
-
-static unsigned long s3c2412_getrate_usbsrc(struct clk *clk)
-{
-	unsigned long parent_rate = clk_get_rate(clk->parent);
-	unsigned long div = __raw_readl(S3C2410_CLKDIVN);
-
-	return parent_rate / ((div & S3C2412_CLKDIVN_USB48DIV) ? 2 : 1);
-}
-
-static int s3c2412_setrate_usbsrc(struct clk *clk, unsigned long rate)
-{
-	unsigned long parent_rate = clk_get_rate(clk->parent);
-	unsigned long clkdivn = __raw_readl(S3C2410_CLKDIVN);
-
-	rate = s3c2412_roundrate_usbsrc(clk, rate);
-
-	if ((parent_rate / rate) == 2)
-		clkdivn |= S3C2412_CLKDIVN_USB48DIV;
-	else
-		clkdivn &= ~S3C2412_CLKDIVN_USB48DIV;
-
-	__raw_writel(clkdivn, S3C2410_CLKDIVN);
-	return 0;
-}
-
-static struct clk clk_usbsrc = {
-	.name		= "usbsrc",
-	.ops		= &(struct clk_ops) {
-		.get_rate	= s3c2412_getrate_usbsrc,
-		.set_rate	= s3c2412_setrate_usbsrc,
-		.round_rate	= s3c2412_roundrate_usbsrc,
-		.set_parent	= s3c2412_setparent_usbsrc,
-	},
-};
-
-static int s3c2412_setparent_msysclk(struct clk *clk, struct clk *parent)
-{
-	unsigned long clksrc = __raw_readl(S3C2412_CLKSRC);
-
-	if (parent == &clk_mdivclk)
-		clksrc &= ~S3C2412_CLKSRC_MSYSCLK_MPLL;
-	else if (parent == &clk_mpll)
-		clksrc |= S3C2412_CLKSRC_MSYSCLK_MPLL;
-	else
-		return -EINVAL;
-
-	clk->parent = parent;
-
-	__raw_writel(clksrc, S3C2412_CLKSRC);
-	return 0;
-}
-
-static struct clk clk_msysclk = {
-	.name		= "msysclk",
-	.ops		= &(struct clk_ops) {
-		.set_parent	= s3c2412_setparent_msysclk,
-	},
-};
-
-static int s3c2412_setparent_armclk(struct clk *clk, struct clk *parent)
-{
-	unsigned long flags;
-	unsigned long clkdiv;
-	unsigned long dvs;
-
-	/* Note, we current equate fclk andf msysclk for S3C2412 */
-
-	if (parent == &clk_msysclk || parent == &clk_f)
-		dvs = 0;
-	else if (parent == &clk_h)
-		dvs = S3C2412_CLKDIVN_DVSEN;
-	else
-		return -EINVAL;
-
-	clk->parent = parent;
-
-	/* update this under irq lockdown, clkdivn is not protected
-	 * by the clock system. */
-
-	local_irq_save(flags);
-
-	clkdiv  = __raw_readl(S3C2410_CLKDIVN);
-	clkdiv &= ~S3C2412_CLKDIVN_DVSEN;
-	clkdiv |= dvs;
-	__raw_writel(clkdiv, S3C2410_CLKDIVN);
-
-	local_irq_restore(flags);
-
-	return 0;
-}
-
-static struct clk clk_armclk = {
-	.name		= "armclk",
-	.parent		= &clk_msysclk,
-	.ops		= &(struct clk_ops) {
-		.set_parent	= s3c2412_setparent_armclk,
-	},
-};
-
-/* these next clocks have an divider immediately after them,
- * so we can register them with their divider and leave out the
- * intermediate clock stage
-*/
-static unsigned long s3c2412_roundrate_clksrc(struct clk *clk,
-					      unsigned long rate)
-{
-	unsigned long parent_rate = clk_get_rate(clk->parent);
-	int div;
-
-	if (rate > parent_rate)
-		return parent_rate;
-
-	/* note, we remove the +/- 1 calculations as they cancel out */
-
-	div = (rate / parent_rate);
-
-	if (div < 1)
-		div = 1;
-	else if (div > 16)
-		div = 16;
-
-	return parent_rate / div;
-}
-
-static int s3c2412_setparent_uart(struct clk *clk, struct clk *parent)
-{
-	unsigned long clksrc = __raw_readl(S3C2412_CLKSRC);
-
-	if (parent == &clk_erefclk)
-		clksrc &= ~S3C2412_CLKSRC_UARTCLK_MPLL;
-	else if (parent == &clk_mpll)
-		clksrc |= S3C2412_CLKSRC_UARTCLK_MPLL;
-	else
-		return -EINVAL;
-
-	clk->parent = parent;
-
-	__raw_writel(clksrc, S3C2412_CLKSRC);
-	return 0;
-}
-
-static unsigned long s3c2412_getrate_uart(struct clk *clk)
-{
-	unsigned long parent_rate = clk_get_rate(clk->parent);
-	unsigned long div = __raw_readl(S3C2410_CLKDIVN);
-
-	div &= S3C2412_CLKDIVN_UARTDIV_MASK;
-	div >>= S3C2412_CLKDIVN_UARTDIV_SHIFT;
-
-	return parent_rate / (div + 1);
-}
-
-static int s3c2412_setrate_uart(struct clk *clk, unsigned long rate)
-{
-	unsigned long parent_rate = clk_get_rate(clk->parent);
-	unsigned long clkdivn = __raw_readl(S3C2410_CLKDIVN);
-
-	rate = s3c2412_roundrate_clksrc(clk, rate);
-
-	clkdivn &= ~S3C2412_CLKDIVN_UARTDIV_MASK;
-	clkdivn |= ((parent_rate / rate) - 1) << S3C2412_CLKDIVN_UARTDIV_SHIFT;
-
-	__raw_writel(clkdivn, S3C2410_CLKDIVN);
-	return 0;
-}
-
-static struct clk clk_uart = {
-	.name		= "uartclk",
-	.ops		= &(struct clk_ops) {
-		.get_rate	= s3c2412_getrate_uart,
-		.set_rate	= s3c2412_setrate_uart,
-		.set_parent	= s3c2412_setparent_uart,
-		.round_rate	= s3c2412_roundrate_clksrc,
-	},
-};
-
-static int s3c2412_setparent_i2s(struct clk *clk, struct clk *parent)
-{
-	unsigned long clksrc = __raw_readl(S3C2412_CLKSRC);
-
-	if (parent == &clk_erefclk)
-		clksrc &= ~S3C2412_CLKSRC_I2SCLK_MPLL;
-	else if (parent == &clk_mpll)
-		clksrc |= S3C2412_CLKSRC_I2SCLK_MPLL;
-	else
-		return -EINVAL;
-
-	clk->parent = parent;
-
-	__raw_writel(clksrc, S3C2412_CLKSRC);
-	return 0;
-}
-
-static unsigned long s3c2412_getrate_i2s(struct clk *clk)
-{
-	unsigned long parent_rate = clk_get_rate(clk->parent);
-	unsigned long div = __raw_readl(S3C2410_CLKDIVN);
-
-	div &= S3C2412_CLKDIVN_I2SDIV_MASK;
-	div >>= S3C2412_CLKDIVN_I2SDIV_SHIFT;
-
-	return parent_rate / (div + 1);
-}
-
-static int s3c2412_setrate_i2s(struct clk *clk, unsigned long rate)
-{
-	unsigned long parent_rate = clk_get_rate(clk->parent);
-	unsigned long clkdivn = __raw_readl(S3C2410_CLKDIVN);
-
-	rate = s3c2412_roundrate_clksrc(clk, rate);
-
-	clkdivn &= ~S3C2412_CLKDIVN_I2SDIV_MASK;
-	clkdivn |= ((parent_rate / rate) - 1) << S3C2412_CLKDIVN_I2SDIV_SHIFT;
-
-	__raw_writel(clkdivn, S3C2410_CLKDIVN);
-	return 0;
-}
-
-static struct clk clk_i2s = {
-	.name		= "i2sclk",
-	.ops		= &(struct clk_ops) {
-		.get_rate	= s3c2412_getrate_i2s,
-		.set_rate	= s3c2412_setrate_i2s,
-		.set_parent	= s3c2412_setparent_i2s,
-		.round_rate	= s3c2412_roundrate_clksrc,
-	},
-};
-
-static int s3c2412_setparent_cam(struct clk *clk, struct clk *parent)
-{
-	unsigned long clksrc = __raw_readl(S3C2412_CLKSRC);
-
-	if (parent == &clk_usysclk)
-		clksrc &= ~S3C2412_CLKSRC_CAMCLK_HCLK;
-	else if (parent == &clk_h)
-		clksrc |= S3C2412_CLKSRC_CAMCLK_HCLK;
-	else
-		return -EINVAL;
-
-	clk->parent = parent;
-
-	__raw_writel(clksrc, S3C2412_CLKSRC);
-	return 0;
-}
-static unsigned long s3c2412_getrate_cam(struct clk *clk)
-{
-	unsigned long parent_rate = clk_get_rate(clk->parent);
-	unsigned long div = __raw_readl(S3C2410_CLKDIVN);
-
-	div &= S3C2412_CLKDIVN_CAMDIV_MASK;
-	div >>= S3C2412_CLKDIVN_CAMDIV_SHIFT;
-
-	return parent_rate / (div + 1);
-}
-
-static int s3c2412_setrate_cam(struct clk *clk, unsigned long rate)
-{
-	unsigned long parent_rate = clk_get_rate(clk->parent);
-	unsigned long clkdivn = __raw_readl(S3C2410_CLKDIVN);
-
-	rate = s3c2412_roundrate_clksrc(clk, rate);
-
-	clkdivn &= ~S3C2412_CLKDIVN_CAMDIV_MASK;
-	clkdivn |= ((parent_rate / rate) - 1) << S3C2412_CLKDIVN_CAMDIV_SHIFT;
-
-	__raw_writel(clkdivn, S3C2410_CLKDIVN);
-	return 0;
-}
-
-static struct clk clk_cam = {
-	.name		= "camif-upll",	/* same as 2440 name */
-	.ops		= &(struct clk_ops) {
-		.get_rate	= s3c2412_getrate_cam,
-		.set_rate	= s3c2412_setrate_cam,
-		.set_parent	= s3c2412_setparent_cam,
-		.round_rate	= s3c2412_roundrate_clksrc,
-	},
-};
-
-/* standard clock definitions */
-
-static struct clk init_clocks_disable[] = {
-	{
-		.name		= "nand",
-		.parent		= &clk_h,
-		.enable		= s3c2412_clkcon_enable,
-		.ctrlbit	= S3C2412_CLKCON_NAND,
-	}, {
-		.name		= "sdi",
-		.parent		= &clk_p,
-		.enable		= s3c2412_clkcon_enable,
-		.ctrlbit	= S3C2412_CLKCON_SDI,
-	}, {
-		.name		= "adc",
-		.parent		= &clk_p,
-		.enable		= s3c2412_clkcon_enable,
-		.ctrlbit	= S3C2412_CLKCON_ADC,
-	}, {
-		.name		= "i2c",
-		.parent		= &clk_p,
-		.enable		= s3c2412_clkcon_enable,
-		.ctrlbit	= S3C2412_CLKCON_IIC,
-	}, {
-		.name		= "iis",
-		.parent		= &clk_p,
-		.enable		= s3c2412_clkcon_enable,
-		.ctrlbit	= S3C2412_CLKCON_IIS,
-	}, {
-		.name		= "spi",
-		.parent		= &clk_p,
-		.enable		= s3c2412_clkcon_enable,
-		.ctrlbit	= S3C2412_CLKCON_SPI,
-	}
-};
-
-static struct clk init_clocks[] = {
-	{
-		.name		= "dma.0",
-		.parent		= &clk_h,
-		.enable		= s3c2412_clkcon_enable,
-		.ctrlbit	= S3C2412_CLKCON_DMA0,
-	}, {
-		.name		= "dma.1",
-		.parent		= &clk_h,
-		.enable		= s3c2412_clkcon_enable,
-		.ctrlbit	= S3C2412_CLKCON_DMA1,
-	}, {
-		.name		= "dma.2",
-		.parent		= &clk_h,
-		.enable		= s3c2412_clkcon_enable,
-		.ctrlbit	= S3C2412_CLKCON_DMA2,
-	}, {
-		.name		= "dma.3",
-		.parent		= &clk_h,
-		.enable		= s3c2412_clkcon_enable,
-		.ctrlbit	= S3C2412_CLKCON_DMA3,
-	}, {
-		.name		= "lcd",
-		.parent		= &clk_h,
-		.enable		= s3c2412_clkcon_enable,
-		.ctrlbit	= S3C2412_CLKCON_LCDC,
-	}, {
-		.name		= "gpio",
-		.parent		= &clk_p,
-		.enable		= s3c2412_clkcon_enable,
-		.ctrlbit	= S3C2412_CLKCON_GPIO,
-	}, {
-		.name		= "usb-host",
-		.parent		= &clk_h,
-		.enable		= s3c2412_clkcon_enable,
-		.ctrlbit	= S3C2412_CLKCON_USBH,
-	}, {
-		.name		= "usb-device",
-		.parent		= &clk_h,
-		.enable		= s3c2412_clkcon_enable,
-		.ctrlbit	= S3C2412_CLKCON_USBD,
-	}, {
-		.name		= "timers",
-		.parent		= &clk_p,
-		.enable		= s3c2412_clkcon_enable,
-		.ctrlbit	= S3C2412_CLKCON_PWMT,
-	}, {
-		.name		= "uart",
-		.devname	= "s3c2412-uart.0",
-		.parent		= &clk_p,
-		.enable		= s3c2412_clkcon_enable,
-		.ctrlbit	= S3C2412_CLKCON_UART0,
-	}, {
-		.name		= "uart",
-		.devname	= "s3c2412-uart.1",
-		.parent		= &clk_p,
-		.enable		= s3c2412_clkcon_enable,
-		.ctrlbit	= S3C2412_CLKCON_UART1,
-	}, {
-		.name		= "uart",
-		.devname	= "s3c2412-uart.2",
-		.parent		= &clk_p,
-		.enable		= s3c2412_clkcon_enable,
-		.ctrlbit	= S3C2412_CLKCON_UART2,
-	}, {
-		.name		= "rtc",
-		.parent		= &clk_p,
-		.enable		= s3c2412_clkcon_enable,
-		.ctrlbit	= S3C2412_CLKCON_RTC,
-	}, {
-		.name		= "watchdog",
-		.parent		= &clk_p,
-		.ctrlbit	= 0,
-	}, {
-		.name		= "usb-bus-gadget",
-		.parent		= &clk_usb_bus,
-		.enable		= s3c2412_clkcon_enable,
-		.ctrlbit	= S3C2412_CLKCON_USB_DEV48,
-	}, {
-		.name		= "usb-bus-host",
-		.parent		= &clk_usb_bus,
-		.enable		= s3c2412_clkcon_enable,
-		.ctrlbit	= S3C2412_CLKCON_USB_HOST48,
-	}
-};
-
-/* clocks to add where we need to check their parentage */
-
-struct clk_init {
-	struct clk	*clk;
-	unsigned int	 bit;
-	struct clk	*src_0;
-	struct clk	*src_1;
-};
-
-static struct clk_init clks_src[] __initdata = {
-	{
-		.clk	= &clk_usysclk,
-		.bit	= S3C2412_CLKSRC_USBCLK_HCLK,
-		.src_0	= &clk_urefclk,
-		.src_1	= &clk_upll,
-	}, {
-		.clk	= &clk_i2s,
-		.bit	= S3C2412_CLKSRC_I2SCLK_MPLL,
-		.src_0	= &clk_erefclk,
-		.src_1	= &clk_mpll,
-	}, {
-		.clk	= &clk_cam,
-		.bit	= S3C2412_CLKSRC_CAMCLK_HCLK,
-		.src_0	= &clk_usysclk,
-		.src_1	= &clk_h,
-	}, {
-		.clk	= &clk_msysclk,
-		.bit	= S3C2412_CLKSRC_MSYSCLK_MPLL,
-		.src_0	= &clk_mdivclk,
-		.src_1	= &clk_mpll,
-	}, {
-		.clk	= &clk_uart,
-		.bit	= S3C2412_CLKSRC_UARTCLK_MPLL,
-		.src_0	= &clk_erefclk,
-		.src_1	= &clk_mpll,
-	}, {
-		.clk	= &clk_usbsrc,
-		.bit	= S3C2412_CLKSRC_USBCLK_HCLK,
-		.src_0	= &clk_usysclk,
-		.src_1	= &clk_h,
-	/* here we assume  OM[4] select xtal */
-	}, {
-		.clk	= &clk_erefclk,
-		.bit	= S3C2412_CLKSRC_EREFCLK_EXTCLK,
-		.src_0	= &clk_xtal,
-		.src_1	= &clk_ext,
-	}, {
-		.clk	= &clk_urefclk,
-		.bit	= S3C2412_CLKSRC_UREFCLK_EXTCLK,
-		.src_0	= &clk_xtal,
-		.src_1	= &clk_ext,
-	},
-};
-
-/* s3c2412_clk_initparents
- *
- * Initialise the parents for the clocks that we get at start-time
-*/
-
-static void __init s3c2412_clk_initparents(void)
-{
-	unsigned long clksrc = __raw_readl(S3C2412_CLKSRC);
-	struct clk_init *cip = clks_src;
-	struct clk *src;
-	int ptr;
-	int ret;
-
-	for (ptr = 0; ptr < ARRAY_SIZE(clks_src); ptr++, cip++) {
-		ret = s3c24xx_register_clock(cip->clk);
-		if (ret < 0) {
-			printk(KERN_ERR "Failed to register clock %s (%d)\n",
-			       cip->clk->name, ret);
-		}
-
-		src = (clksrc & cip->bit) ? cip->src_1 : cip->src_0;
-
-		printk(KERN_INFO "%s: parent %s\n", cip->clk->name, src->name);
-		clk_set_parent(cip->clk, src);
-	}
-}
-
-/* clocks to add straight away */
-
-static struct clk *clks[] __initdata = {
-	&clk_ext,
-	&clk_usb_bus,
-	&clk_mrefclk,
-	&clk_armclk,
-};
-
-static struct clk_lookup s3c2412_clk_lookup[] = {
-	CLKDEV_INIT(NULL, "clk_uart_baud1", &s3c24xx_uclk),
-	CLKDEV_INIT(NULL, "clk_uart_baud2", &clk_p),
-	CLKDEV_INIT(NULL, "clk_uart_baud3", &clk_usysclk),
-};
-
-int __init s3c2412_baseclk_add(void)
-{
-	unsigned long clkcon  = __raw_readl(S3C2410_CLKCON);
-	unsigned int dvs;
-	struct clk *clkp;
-	int ret;
-	int ptr;
-
-	clk_upll.enable = s3c2412_upll_enable;
-	clk_usb_bus.parent = &clk_usbsrc;
-	clk_usb_bus.rate = 0x0;
-
-	clk_f.parent = &clk_msysclk;
-
-	s3c2412_clk_initparents();
-
-	for (ptr = 0; ptr < ARRAY_SIZE(clks); ptr++) {
-		clkp = clks[ptr];
-
-		ret = s3c24xx_register_clock(clkp);
-		if (ret < 0) {
-			printk(KERN_ERR "Failed to register clock %s (%d)\n",
-			       clkp->name, ret);
-		}
-	}
-
-	/* set the dvs state according to what we got at boot time */
-
-	dvs = __raw_readl(S3C2410_CLKDIVN) & S3C2412_CLKDIVN_DVSEN;
-
-	if (dvs)
-		clk_armclk.parent = &clk_h;
-
-	printk(KERN_INFO "S3C2412: DVS is %s\n", dvs ? "on" : "off");
-
-	/* ensure usb bus clock is within correct rate of 48MHz */
-
-	if (clk_get_rate(&clk_usb_bus) != (48 * 1000 * 1000)) {
-		printk(KERN_INFO "Warning: USB bus clock not at 48MHz\n");
-
-		/* for the moment, let's use the UPLL, and see if we can
-		 * get 48MHz */
-
-		clk_set_parent(&clk_usysclk, &clk_upll);
-		clk_set_parent(&clk_usbsrc, &clk_usysclk);
-		clk_set_rate(&clk_usbsrc, 48*1000*1000);
-	}
-
-	printk("S3C2412: upll %s, %ld.%03ld MHz, usb-bus %ld.%03ld MHz\n",
-	       (__raw_readl(S3C2410_UPLLCON) & S3C2412_PLLCON_OFF) ? "off":"on",
-	       print_mhz(clk_get_rate(&clk_upll)),
-	       print_mhz(clk_get_rate(&clk_usb_bus)));
-
-	/* register clocks from clock array */
-
-	clkp = init_clocks;
-	for (ptr = 0; ptr < ARRAY_SIZE(init_clocks); ptr++, clkp++) {
-		/* ensure that we note the clock state */
-
-		clkp->usage = clkcon & clkp->ctrlbit ? 1 : 0;
-
-		ret = s3c24xx_register_clock(clkp);
-		if (ret < 0) {
-			printk(KERN_ERR "Failed to register clock %s (%d)\n",
-			       clkp->name, ret);
-		}
-	}
-
-	/* We must be careful disabling the clocks we are not intending to
-	 * be using at boot time, as subsystems such as the LCD which do
-	 * their own DMA requests to the bus can cause the system to lockup
-	 * if they where in the middle of requesting bus access.
-	 *
-	 * Disabling the LCD clock if the LCD is active is very dangerous,
-	 * and therefore the bootloader should be careful to not enable
-	 * the LCD clock if it is not needed.
-	*/
-
-	/* install (and disable) the clocks we do not need immediately */
-
-	clkp = init_clocks_disable;
-	for (ptr = 0; ptr < ARRAY_SIZE(init_clocks_disable); ptr++, clkp++) {
-
-		ret = s3c24xx_register_clock(clkp);
-		if (ret < 0) {
-			printk(KERN_ERR "Failed to register clock %s (%d)\n",
-			       clkp->name, ret);
-		}
-
-		s3c2412_clkcon_enable(clkp, 0);
-	}
-
-	clkdev_add_table(s3c2412_clk_lookup, ARRAY_SIZE(s3c2412_clk_lookup));
-	return 0;
-}
diff --git a/arch/arm/mach-s3c24xx/clock-s3c2416.c b/arch/arm/mach-s3c24xx/clock-s3c2416.c
deleted file mode 100644
index d421a72920a..00000000000
--- a/arch/arm/mach-s3c24xx/clock-s3c2416.c
+++ /dev/null
@@ -1,171 +0,0 @@
-/* linux/arch/arm/mach-s3c2416/clock.c
- *
- * Copyright (c) 2010 Simtec Electronics
- * Copyright (c) 2010 Ben Dooks <ben-linux@fluff.org>
- *
- * S3C2416 Clock control support
- *
- * 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/init.h>
-#include <linux/clk.h>
-
-#include <plat/clock.h>
-#include <plat/clock-clksrc.h>
-#include <plat/cpu.h>
-
-#include <plat/cpu-freq.h>
-#include <plat/pll.h>
-
-#include <asm/mach/map.h>
-
-#include <mach/regs-clock.h>
-#include <mach/regs-s3c2443-clock.h>
-
-/* armdiv
- *
- * this clock is sourced from msysclk and can have a number of
- * divider values applied to it to then be fed into armclk.
- * The real clock definition is done in s3c2443-clock.c,
- * only the armdiv divisor table must be defined here.
-*/
-
-static unsigned int armdiv[8] = {
-	[0] = 1,
-	[1] = 2,
-	[2] = 3,
-	[3] = 4,
-	[5] = 6,
-	[7] = 8,
-};
-
-static struct clksrc_clk hsspi_eplldiv = {
-	.clk = {
-		.name	= "hsspi-eplldiv",
-		.parent	= &clk_esysclk.clk,
-		.ctrlbit = (1 << 14),
-		.enable = s3c2443_clkcon_enable_s,
-	},
-	.reg_div = { .reg = S3C2443_CLKDIV1, .size = 2, .shift = 24 },
-};
-
-static struct clk *hsspi_sources[] = {
-	[0] = &hsspi_eplldiv.clk,
-	[1] = NULL, /* to fix */
-};
-
-static struct clksrc_clk hsspi_mux = {
-	.clk	= {
-		.name	= "hsspi-if",
-	},
-	.sources = &(struct clksrc_sources) {
-		.sources = hsspi_sources,
-		.nr_sources = ARRAY_SIZE(hsspi_sources),
-	},
-	.reg_src = { .reg = S3C2443_CLKSRC, .size = 1, .shift = 18 },
-};
-
-static struct clksrc_clk hsmmc_div[] = {
-	[0] = {
-		.clk = {
-			.name	= "hsmmc-div",
-			.devname	= "s3c-sdhci.0",
-			.parent	= &clk_esysclk.clk,
-		},
-		.reg_div = { .reg = S3C2416_CLKDIV2, .size = 2, .shift = 6 },
-	},
-	[1] = {
-		.clk = {
-			.name	= "hsmmc-div",
-			.devname	= "s3c-sdhci.1",
-			.parent	= &clk_esysclk.clk,
-		},
-		.reg_div = { .reg = S3C2443_CLKDIV1, .size = 2, .shift = 6 },
-	},
-};
-
-static struct clksrc_clk hsmmc_mux0 = {
-	.clk	= {
-		.name		= "hsmmc-if",
-		.devname	= "s3c-sdhci.0",
-		.ctrlbit	= (1 << 6),
-		.enable		= s3c2443_clkcon_enable_s,
-	},
-	.sources	= &(struct clksrc_sources) {
-		.nr_sources	= 2,
-		.sources	= (struct clk * []) {
-			[0]	= &hsmmc_div[0].clk,
-			[1]	= NULL, /* to fix */
-		},
-	},
-	.reg_src = { .reg = S3C2443_CLKSRC, .size = 1, .shift = 16 },
-};
-
-static struct clksrc_clk hsmmc_mux1 = {
-	.clk	= {
-		.name		= "hsmmc-if",
-		.devname	= "s3c-sdhci.1",
-		.ctrlbit	= (1 << 12),
-		.enable		= s3c2443_clkcon_enable_s,
-	},
-	.sources	= &(struct clksrc_sources) {
-		.nr_sources	= 2,
-		.sources	= (struct clk * []) {
-			[0]	= &hsmmc_div[1].clk,
-			[1]	= NULL, /* to fix */
-		},
-	},
-	.reg_src = { .reg = S3C2443_CLKSRC, .size = 1, .shift = 17 },
-};
-
-static struct clk hsmmc0_clk = {
-	.name		= "hsmmc",
-	.devname	= "s3c-sdhci.0",
-	.parent		= &clk_h,
-	.enable		= s3c2443_clkcon_enable_h,
-	.ctrlbit	= S3C2416_HCLKCON_HSMMC0,
-};
-
-static struct clksrc_clk *clksrcs[] __initdata = {
-	&hsspi_eplldiv,
-	&hsspi_mux,
-	&hsmmc_div[0],
-	&hsmmc_div[1],
-	&hsmmc_mux0,
-	&hsmmc_mux1,
-};
-
-static struct clk_lookup s3c2416_clk_lookup[] = {
-	CLKDEV_INIT("s3c-sdhci.0", "mmc_busclk.0", &hsmmc0_clk),
-	CLKDEV_INIT("s3c-sdhci.0", "mmc_busclk.2", &hsmmc_mux0.clk),
-	CLKDEV_INIT("s3c-sdhci.1", "mmc_busclk.2", &hsmmc_mux1.clk),
-	/* s3c2443-spi.0 is used on s3c2416 and s3c2450 as well */
-	CLKDEV_INIT("s3c2443-spi.0", "spi_busclk2", &hsspi_mux.clk),
-};
-
-void __init s3c2416_init_clocks(int xtal)
-{
-	u32 epllcon = __raw_readl(S3C2443_EPLLCON);
-	u32 epllcon1 = __raw_readl(S3C2443_EPLLCON+4);
-	int ptr;
-
-	/* s3c2416 EPLL compatible with s3c64xx */
-	clk_epll.rate = s3c_get_pll6553x(xtal, epllcon, epllcon1);
-
-	clk_epll.parent = &clk_epllref.clk;
-
-	s3c2443_common_init_clocks(xtal, s3c2416_get_pll,
-				   armdiv, ARRAY_SIZE(armdiv),
-				   S3C2416_CLKDIV0_ARMDIV_MASK);
-
-	for (ptr = 0; ptr < ARRAY_SIZE(clksrcs); ptr++)
-		s3c_register_clksrc(clksrcs[ptr], 1);
-
-	s3c24xx_register_clock(&hsmmc0_clk);
-	clkdev_add_table(s3c2416_clk_lookup, ARRAY_SIZE(s3c2416_clk_lookup));
-
-}
diff --git a/arch/arm/mach-s3c24xx/clock-s3c2440.c b/arch/arm/mach-s3c24xx/clock-s3c2440.c
deleted file mode 100644
index 5527226fd61..00000000000
--- a/arch/arm/mach-s3c24xx/clock-s3c2440.c
+++ /dev/null
@@ -1,217 +0,0 @@
-/* linux/arch/arm/mach-s3c2440/clock.c
- *
- * Copyright (c) 2004-2005 Simtec Electronics
- *	http://armlinux.simtec.co.uk/
- *	Ben Dooks <ben@simtec.co.uk>
- *
- * S3C2440 Clock support
- *
- * 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/init.h>
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/list.h>
-#include <linux/errno.h>
-#include <linux/err.h>
-#include <linux/device.h>
-#include <linux/interrupt.h>
-#include <linux/ioport.h>
-#include <linux/mutex.h>
-#include <linux/clk.h>
-#include <linux/io.h>
-#include <linux/serial_core.h>
-#include <linux/serial_s3c.h>
-
-#include <mach/hardware.h>
-#include <linux/atomic.h>
-#include <asm/irq.h>
-
-#include <mach/regs-clock.h>
-
-#include <plat/clock.h>
-#include <plat/cpu.h>
-
-/* S3C2440 extended clock support */
-
-static unsigned long s3c2440_camif_upll_round(struct clk *clk,
-					      unsigned long rate)
-{
-	unsigned long parent_rate = clk_get_rate(clk->parent);
-	int div;
-
-	if (rate > parent_rate)
-		return parent_rate;
-
-	/* note, we remove the +/- 1 calculations for the divisor */
-
-	div = (parent_rate / rate) / 2;
-
-	if (div < 1)
-		div = 1;
-	else if (div > 16)
-		div = 16;
-
-	return parent_rate / (div * 2);
-}
-
-static int s3c2440_camif_upll_setrate(struct clk *clk, unsigned long rate)
-{
-	unsigned long parent_rate = clk_get_rate(clk->parent);
-	unsigned long camdivn =  __raw_readl(S3C2440_CAMDIVN);
-
-	rate = s3c2440_camif_upll_round(clk, rate);
-
-	camdivn &= ~(S3C2440_CAMDIVN_CAMCLK_SEL | S3C2440_CAMDIVN_CAMCLK_MASK);
-
-	if (rate != parent_rate) {
-		camdivn |= S3C2440_CAMDIVN_CAMCLK_SEL;
-		camdivn |= (((parent_rate / rate) / 2) - 1);
-	}
-
-	__raw_writel(camdivn, S3C2440_CAMDIVN);
-
-	return 0;
-}
-
-static unsigned long s3c2440_camif_upll_getrate(struct clk *clk)
-{
-	unsigned long parent_rate = clk_get_rate(clk->parent);
-	unsigned long camdivn =  __raw_readl(S3C2440_CAMDIVN);
-
-	if (!(camdivn & S3C2440_CAMDIVN_CAMCLK_SEL))
-		return parent_rate;
-
-	camdivn &= S3C2440_CAMDIVN_CAMCLK_MASK;
-
-	return parent_rate / (camdivn + 1) / 2;
-}
-
-/* Extra S3C2440 clocks */
-
-static struct clk s3c2440_clk_cam = {
-	.name		= "camif",
-	.enable		= s3c2410_clkcon_enable,
-	.ctrlbit	= S3C2440_CLKCON_CAMERA,
-};
-
-static struct clk s3c2440_clk_cam_upll = {
-	.name		= "camif-upll",
-	.ops		= &(struct clk_ops) {
-		.set_rate	= s3c2440_camif_upll_setrate,
-		.get_rate	= s3c2440_camif_upll_getrate,
-		.round_rate	= s3c2440_camif_upll_round,
-	},
-};
-
-static struct clk s3c2440_clk_ac97 = {
-	.name		= "ac97",
-	.enable		= s3c2410_clkcon_enable,
-	.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;
-
-	/* the fun of calculating the uart divisors on the s3c2440 */
-	ucon0 = __raw_readl(S3C24XX_VA_UART0 + S3C2410_UCON);
-	ucon1 = __raw_readl(S3C24XX_VA_UART1 + S3C2410_UCON);
-	ucon2 = __raw_readl(S3C24XX_VA_UART2 + S3C2410_UCON);
-
-	ucon0 &= S3C2440_UCON0_DIVMASK;
-	ucon1 &= S3C2440_UCON1_DIVMASK;
-	ucon2 &= S3C2440_UCON2_DIVMASK;
-
-	if (ucon0 != 0)
-		divisor = (ucon0 >> S3C2440_UCON_DIVSHIFT) + 6;
-	else if (ucon1 != 0)
-		divisor = (ucon1 >> S3C2440_UCON_DIVSHIFT) + 21;
-	else if (ucon2 != 0)
-		divisor = (ucon2 >> S3C2440_UCON_DIVSHIFT) + 36;
-	else
-		/* manual calims 44, seems to be 9 */
-		divisor = 9;
-
-	return clk_get_rate(clk->parent) / divisor;
-}
-
-static struct clk s3c2440_clk_fclk_n = {
-	.name		= "fclk_n",
-	.parent		= &clk_f,
-	.ops		= &(struct clk_ops) {
-		.get_rate	= s3c2440_fclk_n_getrate,
-	},
-};
-
-static struct clk_lookup s3c2440_clk_lookup[] = {
-	CLKDEV_INIT(NULL, "clk_uart_baud1", &s3c24xx_uclk),
-	CLKDEV_INIT(NULL, "clk_uart_baud2", &clk_p),
-	CLKDEV_INIT(NULL, "clk_uart_baud3", &s3c2440_clk_fclk_n),
-	CLKDEV_INIT("s3c2440-uart.0", "uart", &s3c24xx_clk_uart0),
-	CLKDEV_INIT("s3c2440-uart.1", "uart", &s3c24xx_clk_uart1),
-	CLKDEV_INIT("s3c2440-uart.2", "uart", &s3c24xx_clk_uart2),
-	CLKDEV_INIT("s3c2440-camif", "camera", &s3c2440_clk_cam_upll),
-};
-
-static int __init_refok s3c2440_clk_add(struct device *dev, struct subsys_interface *sif)
-{
-	struct clk *clock_upll;
-	struct clk *clock_h;
-	struct clk *clock_p;
-
-	clock_p = clk_get(NULL, "pclk");
-	clock_h = clk_get(NULL, "hclk");
-	clock_upll = clk_get(NULL, "upll");
-
-	if (IS_ERR(clock_p) || IS_ERR(clock_h) || IS_ERR(clock_upll)) {
-		printk(KERN_ERR "S3C2440: Failed to get parent clocks\n");
-		return -EINVAL;
-	}
-
-	s3c2440_clk_cam.parent = clock_h;
-	s3c2440_clk_ac97.parent = clock_p;
-	s3c2440_clk_cam_upll.parent = clock_upll;
-	s3c24xx_register_clock(&s3c2440_clk_fclk_n);
-
-	s3c24xx_register_clock(&s3c2440_clk_ac97);
-	s3c24xx_register_clock(&s3c2440_clk_cam);
-	s3c24xx_register_clock(&s3c2440_clk_cam_upll);
-	clkdev_add_table(s3c2440_clk_lookup, ARRAY_SIZE(s3c2440_clk_lookup));
-
-	clk_disable(&s3c2440_clk_ac97);
-	clk_disable(&s3c2440_clk_cam);
-
-	return 0;
-}
-
-static struct subsys_interface s3c2440_clk_interface = {
-	.name		= "s3c2440_clk",
-	.subsys		= &s3c2440_subsys,
-	.add_dev	= s3c2440_clk_add,
-};
-
-static __init int s3c24xx_clk_init(void)
-{
-	return subsys_interface_register(&s3c2440_clk_interface);
-}
-
-arch_initcall(s3c24xx_clk_init);
diff --git a/arch/arm/mach-s3c24xx/clock-s3c2443.c b/arch/arm/mach-s3c24xx/clock-s3c2443.c
deleted file mode 100644
index 76cd31f7804..00000000000
--- a/arch/arm/mach-s3c24xx/clock-s3c2443.c
+++ /dev/null
@@ -1,212 +0,0 @@
-/* linux/arch/arm/mach-s3c2443/clock.c
- *
- * Copyright (c) 2007, 2010 Simtec Electronics
- *	Ben Dooks <ben@simtec.co.uk>
- *
- * S3C2443 Clock control support
- *
- * 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/init.h>
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/list.h>
-#include <linux/errno.h>
-#include <linux/err.h>
-#include <linux/device.h>
-#include <linux/clk.h>
-#include <linux/mutex.h>
-#include <linux/serial_core.h>
-#include <linux/io.h>
-
-#include <asm/mach/map.h>
-
-#include <mach/hardware.h>
-
-#include <mach/regs-s3c2443-clock.h>
-
-#include <plat/cpu-freq.h>
-
-#include <plat/clock.h>
-#include <plat/clock-clksrc.h>
-#include <plat/cpu.h>
-
-/* We currently have to assume that the system is running
- * from the XTPll input, and that all ***REFCLKs are being
- * fed from it, as we cannot read the state of OM[4] from
- * software.
- *
- * It would be possible for each board initialisation to
- * set the correct muxing at initialisation
-*/
-
-/* clock selections */
-
-/* armdiv
- *
- * this clock is sourced from msysclk and can have a number of
- * divider values applied to it to then be fed into armclk.
- * The real clock definition is done in s3c2443-clock.c,
- * only the armdiv divisor table must be defined here.
-*/
-
-static unsigned int armdiv[16] = {
-	[S3C2443_CLKDIV0_ARMDIV_1 >> S3C2443_CLKDIV0_ARMDIV_SHIFT]	= 1,
-	[S3C2443_CLKDIV0_ARMDIV_2 >> S3C2443_CLKDIV0_ARMDIV_SHIFT]	= 2,
-	[S3C2443_CLKDIV0_ARMDIV_3 >> S3C2443_CLKDIV0_ARMDIV_SHIFT]	= 3,
-	[S3C2443_CLKDIV0_ARMDIV_4 >> S3C2443_CLKDIV0_ARMDIV_SHIFT]	= 4,
-	[S3C2443_CLKDIV0_ARMDIV_6 >> S3C2443_CLKDIV0_ARMDIV_SHIFT]	= 6,
-	[S3C2443_CLKDIV0_ARMDIV_8 >> S3C2443_CLKDIV0_ARMDIV_SHIFT]	= 8,
-	[S3C2443_CLKDIV0_ARMDIV_12 >> S3C2443_CLKDIV0_ARMDIV_SHIFT]	= 12,
-	[S3C2443_CLKDIV0_ARMDIV_16 >> S3C2443_CLKDIV0_ARMDIV_SHIFT]	= 16,
-};
-
-/* hsspi
- *
- * high-speed spi clock, sourced from esysclk
-*/
-
-static struct clksrc_clk clk_hsspi = {
-	.clk	= {
-		.name		= "hsspi-if",
-		.parent		= &clk_esysclk.clk,
-		.ctrlbit	= S3C2443_SCLKCON_HSSPICLK,
-		.enable		= s3c2443_clkcon_enable_s,
-	},
-	.reg_div = { .reg = S3C2443_CLKDIV1, .size = 2, .shift = 4 },
-};
-
-
-/* clk_hsmcc_div
- *
- * this clock is sourced from epll, and is fed through a divider,
- * to a mux controlled by sclkcon where either it or a extclk can
- * be fed to the hsmmc block
-*/
-
-static struct clksrc_clk clk_hsmmc_div = {
-	.clk	= {
-		.name		= "hsmmc-div",
-		.devname	= "s3c-sdhci.1",
-		.parent		= &clk_esysclk.clk,
-	},
-	.reg_div = { .reg = S3C2443_CLKDIV1, .size = 2, .shift = 6 },
-};
-
-static int s3c2443_setparent_hsmmc(struct clk *clk, struct clk *parent)
-{
-	unsigned long clksrc = __raw_readl(S3C2443_SCLKCON);
-
-	clksrc &= ~(S3C2443_SCLKCON_HSMMCCLK_EXT |
-		    S3C2443_SCLKCON_HSMMCCLK_EPLL);
-
-	if (parent == &clk_epll)
-		clksrc |= S3C2443_SCLKCON_HSMMCCLK_EPLL;
-	else if (parent == &clk_ext)
-		clksrc |= S3C2443_SCLKCON_HSMMCCLK_EXT;
-	else
-		return -EINVAL;
-
-	if (clk->usage > 0) {
-		__raw_writel(clksrc, S3C2443_SCLKCON);
-	}
-
-	clk->parent = parent;
-	return 0;
-}
-
-static int s3c2443_enable_hsmmc(struct clk *clk, int enable)
-{
-	return s3c2443_setparent_hsmmc(clk, clk->parent);
-}
-
-static struct clk clk_hsmmc = {
-	.name		= "hsmmc-if",
-	.devname	= "s3c-sdhci.1",
-	.parent		= &clk_hsmmc_div.clk,
-	.enable		= s3c2443_enable_hsmmc,
-	.ops		= &(struct clk_ops) {
-		.set_parent	= s3c2443_setparent_hsmmc,
-	},
-};
-
-/* standard clock definitions */
-
-static struct clk init_clocks_off[] = {
-	{
-		.name		= "sdi",
-		.parent		= &clk_p,
-		.enable		= s3c2443_clkcon_enable_p,
-		.ctrlbit	= S3C2443_PCLKCON_SDI,
-	}, {
-		.name		= "spi",
-		.devname	= "s3c2410-spi.0",
-		.parent		= &clk_p,
-		.enable		= s3c2443_clkcon_enable_p,
-		.ctrlbit	= S3C2443_PCLKCON_SPI1,
-	}
-};
-
-/* clocks to add straight away */
-
-static struct clksrc_clk *clksrcs[] __initdata = {
-	&clk_hsspi,
-	&clk_hsmmc_div,
-};
-
-static struct clk *clks[] __initdata = {
-	&clk_hsmmc,
-};
-
-static struct clk_lookup s3c2443_clk_lookup[] = {
-	CLKDEV_INIT("s3c-sdhci.1", "mmc_busclk.2", &clk_hsmmc),
-	CLKDEV_INIT("s3c2443-spi.0", "spi_busclk2", &clk_hsspi.clk),
-};
-
-void __init s3c2443_init_clocks(int xtal)
-{
-	unsigned long epllcon = __raw_readl(S3C2443_EPLLCON);
-	int ptr;
-
-	clk_epll.rate = s3c2443_get_epll(epllcon, xtal);
-	clk_epll.parent = &clk_epllref.clk;
-
-	s3c2443_common_init_clocks(xtal, s3c2443_get_mpll,
-				   armdiv, ARRAY_SIZE(armdiv),
-				   S3C2443_CLKDIV0_ARMDIV_MASK);
-
-	s3c24xx_register_clocks(clks, ARRAY_SIZE(clks));
-
-	for (ptr = 0; ptr < ARRAY_SIZE(clksrcs); ptr++)
-		s3c_register_clksrc(clksrcs[ptr], 1);
-
-	/* We must be careful disabling the clocks we are not intending to
-	 * be using at boot time, as subsystems such as the LCD which do
-	 * their own DMA requests to the bus can cause the system to lockup
-	 * if they where in the middle of requesting bus access.
-	 *
-	 * Disabling the LCD clock if the LCD is active is very dangerous,
-	 * and therefore the bootloader should be careful to not enable
-	 * the LCD clock if it is not needed.
-	*/
-
-	/* install (and disable) the clocks we do not need immediately */
-
-	s3c_register_clocks(init_clocks_off, ARRAY_SIZE(init_clocks_off));
-	s3c_disable_clocks(init_clocks_off, ARRAY_SIZE(init_clocks_off));
-	clkdev_add_table(s3c2443_clk_lookup, ARRAY_SIZE(s3c2443_clk_lookup));
-}
diff --git a/arch/arm/mach-s3c24xx/clock-s3c244x.c b/arch/arm/mach-s3c24xx/clock-s3c244x.c
deleted file mode 100644
index 6d9b688c442..00000000000
--- a/arch/arm/mach-s3c24xx/clock-s3c244x.c
+++ /dev/null
@@ -1,141 +0,0 @@
-/* linux/arch/arm/plat-s3c24xx/s3c24xx-clock.c
- *
- * Copyright (c) 2004-2008 Simtec Electronics
- *	http://armlinux.simtec.co.uk/
- *	Ben Dooks <ben@simtec.co.uk>
- *
- * S3C2440/S3C2442 Common clock support
- *
- * 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/init.h>
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/list.h>
-#include <linux/errno.h>
-#include <linux/err.h>
-#include <linux/device.h>
-#include <linux/interrupt.h>
-#include <linux/ioport.h>
-#include <linux/clk.h>
-#include <linux/io.h>
-
-#include <mach/hardware.h>
-#include <linux/atomic.h>
-#include <asm/irq.h>
-
-#include <mach/regs-clock.h>
-
-#include <plat/clock.h>
-#include <plat/cpu.h>
-
-static int s3c2440_setparent_armclk(struct clk *clk, struct clk *parent)
-{
-	unsigned long camdivn;
-	unsigned long dvs;
-
-	if (parent == &clk_f)
-		dvs = 0;
-	else if (parent == &clk_h)
-		dvs = S3C2440_CAMDIVN_DVSEN;
-	else
-		return -EINVAL;
-
-	clk->parent = parent;
-
-	camdivn  = __raw_readl(S3C2440_CAMDIVN);
-	camdivn &= ~S3C2440_CAMDIVN_DVSEN;
-	camdivn |= dvs;
-	__raw_writel(camdivn, S3C2440_CAMDIVN);
-
-	return 0;
-}
-
-static struct clk clk_arm = {
-	.name		= "armclk",
-	.id		= -1,
-	.ops		= &(struct clk_ops) {
-		.set_parent	= s3c2440_setparent_armclk,
-	},
-};
-
-static int s3c244x_clk_add(struct device *dev, struct subsys_interface *sif)
-{
-	unsigned long camdivn = __raw_readl(S3C2440_CAMDIVN);
-	unsigned long clkdivn;
-	struct clk *clock_upll;
-	int ret;
-
-	printk("S3C244X: Clock Support, DVS %s\n",
-	       (camdivn & S3C2440_CAMDIVN_DVSEN) ? "on" : "off");
-
-	clk_arm.parent = (camdivn & S3C2440_CAMDIVN_DVSEN) ? &clk_h : &clk_f;
-
-	ret = s3c24xx_register_clock(&clk_arm);
-	if (ret < 0) {
-		printk(KERN_ERR "S3C24XX: Failed to add armclk (%d)\n", ret);
-		return ret;
-	}
-
-	clock_upll = clk_get(NULL, "upll");
-	if (IS_ERR(clock_upll)) {
-		printk(KERN_ERR "S3C244X: Failed to get upll clock\n");
-		return -ENOENT;
-	}
-
-	/* check rate of UPLL, and if it is near 96MHz, then change
-	 * to using half the UPLL rate for the system */
-
-	if (clk_get_rate(clock_upll) > (94 * MHZ)) {
-		clk_usb_bus.rate = clk_get_rate(clock_upll) / 2;
-
-		spin_lock(&clocks_lock);
-
-		clkdivn = __raw_readl(S3C2410_CLKDIVN);
-		clkdivn |= S3C2440_CLKDIVN_UCLK;
-		__raw_writel(clkdivn, S3C2410_CLKDIVN);
-
-		spin_unlock(&clocks_lock);
-	}
-
-	return 0;
-}
-
-static struct subsys_interface s3c2440_clk_interface = {
-	.name		= "s3c2440_clk",
-	.subsys		= &s3c2440_subsys,
-	.add_dev	= s3c244x_clk_add,
-};
-
-static int s3c2440_clk_init(void)
-{
-	return subsys_interface_register(&s3c2440_clk_interface);
-}
-
-arch_initcall(s3c2440_clk_init);
-
-static struct subsys_interface s3c2442_clk_interface = {
-	.name		= "s3c2442_clk",
-	.subsys		= &s3c2442_subsys,
-	.add_dev	= s3c244x_clk_add,
-};
-
-static int s3c2442_clk_init(void)
-{
-	return subsys_interface_register(&s3c2442_clk_interface);
-}
-
-arch_initcall(s3c2442_clk_init);
diff --git a/arch/arm/mach-s3c24xx/common-s3c2443.c b/arch/arm/mach-s3c24xx/common-s3c2443.c
deleted file mode 100644
index 65d3eef7309..00000000000
--- a/arch/arm/mach-s3c24xx/common-s3c2443.c
+++ /dev/null
@@ -1,675 +0,0 @@
-/*
- * Common code for SoCs starting with the S3C2443
- *
- * Copyright (c) 2007, 2010 Simtec Electronics
- *	Ben Dooks <ben@simtec.co.uk>
- *
- * 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/init.h>
-#include <linux/clk.h>
-#include <linux/io.h>
-
-#include <mach/regs-s3c2443-clock.h>
-
-#include <plat/clock.h>
-#include <plat/clock-clksrc.h>
-#include <plat/cpu.h>
-
-#include <plat/cpu-freq.h>
-
-
-static int s3c2443_gate(void __iomem *reg, struct clk *clk, int enable)
-{
-	u32 ctrlbit = clk->ctrlbit;
-	u32 con = __raw_readl(reg);
-
-	if (enable)
-		con |= ctrlbit;
-	else
-		con &= ~ctrlbit;
-
-	__raw_writel(con, reg);
-	return 0;
-}
-
-int s3c2443_clkcon_enable_h(struct clk *clk, int enable)
-{
-	return s3c2443_gate(S3C2443_HCLKCON, clk, enable);
-}
-
-int s3c2443_clkcon_enable_p(struct clk *clk, int enable)
-{
-	return s3c2443_gate(S3C2443_PCLKCON, clk, enable);
-}
-
-int s3c2443_clkcon_enable_s(struct clk *clk, int enable)
-{
-	return s3c2443_gate(S3C2443_SCLKCON, clk, enable);
-}
-
-/* mpllref is a direct descendant of clk_xtal by default, but it is not
- * elided as the EPLL can be either sourced by the XTAL or EXTCLK and as
- * such directly equating the two source clocks is impossible.
- */
-static struct clk clk_mpllref = {
-	.name		= "mpllref",
-	.parent		= &clk_xtal,
-};
-
-static struct clk *clk_epllref_sources[] = {
-	[0] = &clk_mpllref,
-	[1] = &clk_mpllref,
-	[2] = &clk_xtal,
-	[3] = &clk_ext,
-};
-
-struct clksrc_clk clk_epllref = {
-	.clk	= {
-		.name		= "epllref",
-	},
-	.sources = &(struct clksrc_sources) {
-		.sources = clk_epllref_sources,
-		.nr_sources = ARRAY_SIZE(clk_epllref_sources),
-	},
-	.reg_src = { .reg = S3C2443_CLKSRC, .size = 2, .shift = 7 },
-};
-
-/* esysclk
- *
- * this is sourced from either the EPLL or the EPLLref clock
-*/
-
-static struct clk *clk_sysclk_sources[] = {
-	[0] = &clk_epllref.clk,
-	[1] = &clk_epll,
-};
-
-struct clksrc_clk clk_esysclk = {
-	.clk	= {
-		.name		= "esysclk",
-		.parent		= &clk_epll,
-	},
-	.sources = &(struct clksrc_sources) {
-		.sources = clk_sysclk_sources,
-		.nr_sources = ARRAY_SIZE(clk_sysclk_sources),
-	},
-	.reg_src = { .reg = S3C2443_CLKSRC, .size = 1, .shift = 6 },
-};
-
-static unsigned long s3c2443_getrate_mdivclk(struct clk *clk)
-{
-	unsigned long parent_rate = clk_get_rate(clk->parent);
-	unsigned long div = __raw_readl(S3C2443_CLKDIV0);
-
-	div  &= S3C2443_CLKDIV0_EXTDIV_MASK;
-	div >>= (S3C2443_CLKDIV0_EXTDIV_SHIFT-1);	/* x2 */
-
-	return parent_rate / (div + 1);
-}
-
-static struct clk clk_mdivclk = {
-	.name		= "mdivclk",
-	.parent		= &clk_mpllref,
-	.ops		= &(struct clk_ops) {
-		.get_rate	= s3c2443_getrate_mdivclk,
-	},
-};
-
-static struct clk *clk_msysclk_sources[] = {
-	[0] = &clk_mpllref,
-	[1] = &clk_mpll,
-	[2] = &clk_mdivclk,
-	[3] = &clk_mpllref,
-};
-
-static struct clksrc_clk clk_msysclk = {
-	.clk	= {
-		.name		= "msysclk",
-		.parent		= &clk_xtal,
-	},
-	.sources = &(struct clksrc_sources) {
-		.sources = clk_msysclk_sources,
-		.nr_sources = ARRAY_SIZE(clk_msysclk_sources),
-	},
-	.reg_src = { .reg = S3C2443_CLKSRC, .size = 2, .shift = 3 },
-};
-
-/* prediv
- *
- * this divides the msysclk down to pass to h/p/etc.
- */
-
-static unsigned long s3c2443_prediv_getrate(struct clk *clk)
-{
-	unsigned long rate = clk_get_rate(clk->parent);
-	unsigned long clkdiv0 = __raw_readl(S3C2443_CLKDIV0);
-
-	clkdiv0 &= S3C2443_CLKDIV0_PREDIV_MASK;
-	clkdiv0 >>= S3C2443_CLKDIV0_PREDIV_SHIFT;
-
-	return rate / (clkdiv0 + 1);
-}
-
-static struct clk clk_prediv = {
-	.name		= "prediv",
-	.parent		= &clk_msysclk.clk,
-	.ops		= &(struct clk_ops) {
-		.get_rate	= s3c2443_prediv_getrate,
-	},
-};
-
-/* hclk divider
- *
- * divides the prediv and provides the hclk.
- */
-
-static unsigned long s3c2443_hclkdiv_getrate(struct clk *clk)
-{
-	unsigned long rate = clk_get_rate(clk->parent);
-	unsigned long clkdiv0 = __raw_readl(S3C2443_CLKDIV0);
-
-	clkdiv0 &= S3C2443_CLKDIV0_HCLKDIV_MASK;
-
-	return rate / (clkdiv0 + 1);
-}
-
-static struct clk_ops clk_h_ops = {
-	.get_rate	= s3c2443_hclkdiv_getrate,
-};
-
-/* pclk divider
- *
- * divides the hclk and provides the pclk.
- */
-
-static unsigned long s3c2443_pclkdiv_getrate(struct clk *clk)
-{
-	unsigned long rate = clk_get_rate(clk->parent);
-	unsigned long clkdiv0 = __raw_readl(S3C2443_CLKDIV0);
-
-	clkdiv0 = ((clkdiv0 & S3C2443_CLKDIV0_HALF_PCLK) ? 1 : 0);
-
-	return rate / (clkdiv0 + 1);
-}
-
-static struct clk_ops clk_p_ops = {
-	.get_rate	= s3c2443_pclkdiv_getrate,
-};
-
-/* armdiv
- *
- * this clock is sourced from msysclk and can have a number of
- * divider values applied to it to then be fed into armclk.
-*/
-
-static unsigned int *armdiv;
-static int nr_armdiv;
-static int armdivmask;
-
-static unsigned long s3c2443_armclk_roundrate(struct clk *clk,
-					      unsigned long rate)
-{
-	unsigned long parent = clk_get_rate(clk->parent);
-	unsigned long calc;
-	unsigned best = 256; /* bigger than any value */
-	unsigned div;
-	int ptr;
-
-	if (!nr_armdiv)
-		return -EINVAL;
-
-	for (ptr = 0; ptr < nr_armdiv; ptr++) {
-		div = armdiv[ptr];
-		if (div) {
-			/* cpufreq provides 266mhz as 266666000 not 266666666 */
-			calc = (parent / div / 1000) * 1000;
-			if (calc <= rate && div < best)
-				best = div;
-		}
-	}
-
-	return parent / best;
-}
-
-static unsigned long s3c2443_armclk_getrate(struct clk *clk)
-{
-	unsigned long rate = clk_get_rate(clk->parent);
-	unsigned long clkcon0;
-	int val;
-
-	if (!nr_armdiv || !armdivmask)
-		return -EINVAL;
-
-	clkcon0 = __raw_readl(S3C2443_CLKDIV0);
-	clkcon0 &= armdivmask;
-	val = clkcon0 >> S3C2443_CLKDIV0_ARMDIV_SHIFT;
-
-	return rate / armdiv[val];
-}
-
-static int s3c2443_armclk_setrate(struct clk *clk, unsigned long rate)
-{
-	unsigned long parent = clk_get_rate(clk->parent);
-	unsigned long calc;
-	unsigned div;
-	unsigned best = 256; /* bigger than any value */
-	int ptr;
-	int val = -1;
-
-	if (!nr_armdiv || !armdivmask)
-		return -EINVAL;
-
-	for (ptr = 0; ptr < nr_armdiv; ptr++) {
-		div = armdiv[ptr];
-		if (div) {
-			/* cpufreq provides 266mhz as 266666000 not 266666666 */
-			calc = (parent / div / 1000) * 1000;
-			if (calc <= rate && div < best) {
-				best = div;
-				val = ptr;
-			}
-		}
-	}
-
-	if (val >= 0) {
-		unsigned long clkcon0;
-
-		clkcon0 = __raw_readl(S3C2443_CLKDIV0);
-		clkcon0 &= ~armdivmask;
-		clkcon0 |= val << S3C2443_CLKDIV0_ARMDIV_SHIFT;
-		__raw_writel(clkcon0, S3C2443_CLKDIV0);
-	}
-
-	return (val == -1) ? -EINVAL : 0;
-}
-
-static struct clk clk_armdiv = {
-	.name		= "armdiv",
-	.parent		= &clk_msysclk.clk,
-	.ops		= &(struct clk_ops) {
-		.round_rate = s3c2443_armclk_roundrate,
-		.get_rate = s3c2443_armclk_getrate,
-		.set_rate = s3c2443_armclk_setrate,
-	},
-};
-
-/* armclk
- *
- * this is the clock fed into the ARM core itself, from armdiv or from hclk.
- */
-
-static struct clk *clk_arm_sources[] = {
-	[0] = &clk_armdiv,
-	[1] = &clk_h,
-};
-
-static struct clksrc_clk clk_arm = {
-	.clk	= {
-		.name		= "armclk",
-	},
-	.sources = &(struct clksrc_sources) {
-		.sources = clk_arm_sources,
-		.nr_sources = ARRAY_SIZE(clk_arm_sources),
-	},
-	.reg_src = { .reg = S3C2443_CLKDIV0, .size = 1, .shift = 13 },
-};
-
-/* usbhost
- *
- * usb host bus-clock, usually 48MHz to provide USB bus clock timing
-*/
-
-static struct clksrc_clk clk_usb_bus_host = {
-	.clk	= {
-		.name		= "usb-bus-host-parent",
-		.parent		= &clk_esysclk.clk,
-		.ctrlbit	= S3C2443_SCLKCON_USBHOST,
-		.enable		= s3c2443_clkcon_enable_s,
-	},
-	.reg_div = { .reg = S3C2443_CLKDIV1, .size = 2, .shift = 4 },
-};
-
-/* common clksrc clocks */
-
-static struct clksrc_clk clksrc_clks[] = {
-	{
-		/* camera interface bus-clock, divided down from esysclk */
-		.clk	= {
-			.name		= "camif-upll",	/* same as 2440 name */
-			.parent		= &clk_esysclk.clk,
-			.ctrlbit	= S3C2443_SCLKCON_CAMCLK,
-			.enable		= s3c2443_clkcon_enable_s,
-		},
-		.reg_div = { .reg = S3C2443_CLKDIV1, .size = 4, .shift = 26 },
-	}, {
-		.clk	= {
-			.name		= "display-if",
-			.parent		= &clk_esysclk.clk,
-			.ctrlbit	= S3C2443_SCLKCON_DISPCLK,
-			.enable		= s3c2443_clkcon_enable_s,
-		},
-		.reg_div = { .reg = S3C2443_CLKDIV1, .size = 8, .shift = 16 },
-	},
-};
-
-static struct clksrc_clk clk_esys_uart = {
-	/* ART baud-rate clock sourced from esysclk via a divisor */
-	.clk	= {
-		.name		= "uartclk",
-		.parent		= &clk_esysclk.clk,
-	},
-	.reg_div = { .reg = S3C2443_CLKDIV1, .size = 4, .shift = 8 },
-};
-
-static struct clk clk_i2s_ext = {
-	.name		= "i2s-ext",
-};
-
-/* i2s_eplldiv
- *
- * This clock is the output from the I2S divisor of ESYSCLK, and is separate
- * from the mux that comes after it (cannot merge into one single clock)
-*/
-
-static struct clksrc_clk clk_i2s_eplldiv = {
-	.clk	= {
-		.name		= "i2s-eplldiv",
-		.parent		= &clk_esysclk.clk,
-	},
-	.reg_div = { .reg = S3C2443_CLKDIV1, .size = 4, .shift = 12, },
-};
-
-/* i2s-ref
- *
- * i2s bus reference clock, selectable from external, esysclk or epllref
- *
- * Note, this used to be two clocks, but was compressed into one.
-*/
-
-static struct clk *clk_i2s_srclist[] = {
-	[0] = &clk_i2s_eplldiv.clk,
-	[1] = &clk_i2s_ext,
-	[2] = &clk_epllref.clk,
-	[3] = &clk_epllref.clk,
-};
-
-static struct clksrc_clk clk_i2s = {
-	.clk	= {
-		.name		= "i2s-if",
-		.ctrlbit	= S3C2443_SCLKCON_I2SCLK,
-		.enable		= s3c2443_clkcon_enable_s,
-
-	},
-	.sources = &(struct clksrc_sources) {
-		.sources = clk_i2s_srclist,
-		.nr_sources = ARRAY_SIZE(clk_i2s_srclist),
-	},
-	.reg_src = { .reg = S3C2443_CLKSRC, .size = 2, .shift = 14 },
-};
-
-static struct clk init_clocks_off[] = {
-	{
-		.name		= "iis",
-		.parent		= &clk_p,
-		.enable		= s3c2443_clkcon_enable_p,
-		.ctrlbit	= S3C2443_PCLKCON_IIS,
-	}, {
-		.name		= "adc",
-		.parent		= &clk_p,
-		.enable		= s3c2443_clkcon_enable_p,
-		.ctrlbit	= S3C2443_PCLKCON_ADC,
-	}, {
-		.name		= "i2c",
-		.parent		= &clk_p,
-		.enable		= s3c2443_clkcon_enable_p,
-		.ctrlbit	= S3C2443_PCLKCON_IIC,
-	}
-};
-
-static struct clk init_clocks[] = {
-	{
-		.name		= "dma.0",
-		.parent		= &clk_h,
-		.enable		= s3c2443_clkcon_enable_h,
-		.ctrlbit	= S3C2443_HCLKCON_DMA0,
-	}, {
-		.name		= "dma.1",
-		.parent		= &clk_h,
-		.enable		= s3c2443_clkcon_enable_h,
-		.ctrlbit	= S3C2443_HCLKCON_DMA1,
-	}, {
-		.name		= "dma.2",
-		.parent		= &clk_h,
-		.enable		= s3c2443_clkcon_enable_h,
-		.ctrlbit	= S3C2443_HCLKCON_DMA2,
-	}, {
-		.name		= "dma.3",
-		.parent		= &clk_h,
-		.enable		= s3c2443_clkcon_enable_h,
-		.ctrlbit	= S3C2443_HCLKCON_DMA3,
-	}, {
-		.name		= "dma.4",
-		.parent		= &clk_h,
-		.enable		= s3c2443_clkcon_enable_h,
-		.ctrlbit	= S3C2443_HCLKCON_DMA4,
-	}, {
-		.name		= "dma.5",
-		.parent		= &clk_h,
-		.enable		= s3c2443_clkcon_enable_h,
-		.ctrlbit	= S3C2443_HCLKCON_DMA5,
-	}, {
-		.name		= "gpio",
-		.parent		= &clk_p,
-		.enable		= s3c2443_clkcon_enable_p,
-		.ctrlbit	= S3C2443_PCLKCON_GPIO,
-	}, {
-		.name		= "usb-host",
-		.parent		= &clk_h,
-		.enable		= s3c2443_clkcon_enable_h,
-		.ctrlbit	= S3C2443_HCLKCON_USBH,
-	}, {
-		.name		= "usb-device",
-		.parent		= &clk_h,
-		.enable		= s3c2443_clkcon_enable_h,
-		.ctrlbit	= S3C2443_HCLKCON_USBD,
-	}, {
-		.name		= "lcd",
-		.parent		= &clk_h,
-		.enable		= s3c2443_clkcon_enable_h,
-		.ctrlbit	= S3C2443_HCLKCON_LCDC,
-
-	}, {
-		.name		= "timers",
-		.parent		= &clk_p,
-		.enable		= s3c2443_clkcon_enable_p,
-		.ctrlbit	= S3C2443_PCLKCON_PWMT,
-	}, {
-		.name		= "cfc",
-		.parent		= &clk_h,
-		.enable		= s3c2443_clkcon_enable_h,
-		.ctrlbit	= S3C2443_HCLKCON_CFC,
-	}, {
-		.name		= "ssmc",
-		.parent		= &clk_h,
-		.enable		= s3c2443_clkcon_enable_h,
-		.ctrlbit	= S3C2443_HCLKCON_SSMC,
-	}, {
-		.name		= "uart",
-		.devname	= "s3c2440-uart.0",
-		.parent		= &clk_p,
-		.enable		= s3c2443_clkcon_enable_p,
-		.ctrlbit	= S3C2443_PCLKCON_UART0,
-	}, {
-		.name		= "uart",
-		.devname	= "s3c2440-uart.1",
-		.parent		= &clk_p,
-		.enable		= s3c2443_clkcon_enable_p,
-		.ctrlbit	= S3C2443_PCLKCON_UART1,
-	}, {
-		.name		= "uart",
-		.devname	= "s3c2440-uart.2",
-		.parent		= &clk_p,
-		.enable		= s3c2443_clkcon_enable_p,
-		.ctrlbit	= S3C2443_PCLKCON_UART2,
-	}, {
-		.name		= "uart",
-		.devname	= "s3c2440-uart.3",
-		.parent		= &clk_p,
-		.enable		= s3c2443_clkcon_enable_p,
-		.ctrlbit	= S3C2443_PCLKCON_UART3,
-	}, {
-		.name		= "rtc",
-		.parent		= &clk_p,
-		.enable		= s3c2443_clkcon_enable_p,
-		.ctrlbit	= S3C2443_PCLKCON_RTC,
-	}, {
-		.name		= "watchdog",
-		.parent		= &clk_p,
-		.ctrlbit	= S3C2443_PCLKCON_WDT,
-	}, {
-		.name		= "ac97",
-		.parent		= &clk_p,
-		.ctrlbit	= S3C2443_PCLKCON_AC97,
-	}, {
-		.name		= "nand",
-		.parent		= &clk_h,
-	}, {
-		.name		= "usb-bus-host",
-		.parent		= &clk_usb_bus_host.clk,
-	}
-};
-
-static struct clk hsmmc1_clk = {
-	.name		= "hsmmc",
-	.devname	= "s3c-sdhci.1",
-	.parent		= &clk_h,
-	.enable		= s3c2443_clkcon_enable_h,
-	.ctrlbit	= S3C2443_HCLKCON_HSMMC,
-};
-
-static struct clk hsspi_clk = {
-	.name		= "spi",
-	.devname	= "s3c2443-spi.0",
-	.parent		= &clk_p,
-	.enable		= s3c2443_clkcon_enable_p,
-	.ctrlbit	= S3C2443_PCLKCON_HSSPI,
-};
-
-/* EPLLCON compatible enough to get on/off information */
-
-void __init_or_cpufreq s3c2443_common_setup_clocks(pll_fn get_mpll)
-{
-	unsigned long epllcon = __raw_readl(S3C2443_EPLLCON);
-	unsigned long mpllcon = __raw_readl(S3C2443_MPLLCON);
-	struct clk *xtal_clk;
-	unsigned long xtal;
-	unsigned long pll;
-	int ptr;
-
-	xtal_clk = clk_get(NULL, "xtal");
-	xtal = clk_get_rate(xtal_clk);
-	clk_put(xtal_clk);
-
-	pll = get_mpll(mpllcon, xtal);
-	clk_msysclk.clk.rate = pll;
-	clk_mpll.rate = pll;
-
-	printk("CPU: MPLL %s %ld.%03ld MHz, cpu %ld.%03ld MHz, mem %ld.%03ld MHz, pclk %ld.%03ld MHz\n",
-	       (mpllcon & S3C2443_PLLCON_OFF) ? "off" : "on",
-	       print_mhz(pll), print_mhz(clk_get_rate(&clk_armdiv)),
-	       print_mhz(clk_get_rate(&clk_h)),
-	       print_mhz(clk_get_rate(&clk_p)));
-
-	for (ptr = 0; ptr < ARRAY_SIZE(clksrc_clks); ptr++)
-		s3c_set_clksrc(&clksrc_clks[ptr], true);
-
-	/* ensure usb bus clock is within correct rate of 48MHz */
-
-	if (clk_get_rate(&clk_usb_bus_host.clk) != (48 * 1000 * 1000)) {
-		printk(KERN_INFO "Warning: USB host bus not at 48MHz\n");
-		clk_set_rate(&clk_usb_bus_host.clk, 48*1000*1000);
-	}
-
-	printk("CPU: EPLL %s %ld.%03ld MHz, usb-bus %ld.%03ld MHz\n",
-	       (epllcon & S3C2443_PLLCON_OFF) ? "off" : "on",
-	       print_mhz(clk_get_rate(&clk_epll)),
-	       print_mhz(clk_get_rate(&clk_usb_bus)));
-}
-
-static struct clk *clks[] __initdata = {
-	&clk_prediv,
-	&clk_mpllref,
-	&clk_mdivclk,
-	&clk_ext,
-	&clk_epll,
-	&clk_usb_bus,
-	&clk_armdiv,
-	&hsmmc1_clk,
-	&hsspi_clk,
-};
-
-static struct clksrc_clk *clksrcs[] __initdata = {
-	&clk_i2s_eplldiv,
-	&clk_i2s,
-	&clk_usb_bus_host,
-	&clk_epllref,
-	&clk_esysclk,
-	&clk_msysclk,
-	&clk_arm,
-};
-
-static struct clk_lookup s3c2443_clk_lookup[] = {
-	CLKDEV_INIT(NULL, "clk_uart_baud1", &s3c24xx_uclk),
-	CLKDEV_INIT(NULL, "clk_uart_baud2", &clk_p),
-	CLKDEV_INIT(NULL, "clk_uart_baud3", &clk_esys_uart.clk),
-	CLKDEV_INIT("s3c-sdhci.1", "mmc_busclk.0", &hsmmc1_clk),
-	CLKDEV_INIT("s3c2443-spi.0", "spi_busclk0", &hsspi_clk),
-};
-
-void __init s3c2443_common_init_clocks(int xtal, pll_fn get_mpll,
-				       unsigned int *divs, int nr_divs,
-				       int divmask)
-{
-	int ptr;
-
-	armdiv = divs;
-	nr_armdiv = nr_divs;
-	armdivmask = divmask;
-
-	/* s3c2443 parents h clock from prediv */
-	clk_h.parent = &clk_prediv;
-	clk_h.ops = &clk_h_ops;
-
-	/* and p clock from h clock */
-	clk_p.parent = &clk_h;
-	clk_p.ops = &clk_p_ops;
-
-	clk_usb_bus.parent = &clk_usb_bus_host.clk;
-	clk_epll.parent = &clk_epllref.clk;
-
-	s3c24xx_register_baseclocks(xtal);
-	s3c24xx_register_clocks(clks, ARRAY_SIZE(clks));
-
-	for (ptr = 0; ptr < ARRAY_SIZE(clksrcs); ptr++)
-		s3c_register_clksrc(clksrcs[ptr], 1);
-
-	s3c_register_clksrc(clksrc_clks, ARRAY_SIZE(clksrc_clks));
-	s3c_register_clocks(init_clocks, ARRAY_SIZE(init_clocks));
-
-	/* See s3c2443/etc notes on disabling clocks at init time */
-	s3c_register_clocks(init_clocks_off, ARRAY_SIZE(init_clocks_off));
-	s3c_disable_clocks(init_clocks_off, ARRAY_SIZE(init_clocks_off));
-	clkdev_add_table(s3c2443_clk_lookup, ARRAY_SIZE(s3c2443_clk_lookup));
-
-	s3c2443_common_setup_clocks(get_mpll);
-}
diff --git a/arch/arm/mach-s3c24xx/common.c b/arch/arm/mach-s3c24xx/common.c
index 1bc8e73c94f..c0763b83774 100644
--- a/arch/arm/mach-s3c24xx/common.c
+++ b/arch/arm/mach-s3c24xx/common.c
@@ -53,6 +53,7 @@
 #include <plat/cpu-freq.h>
 #include <plat/pll.h>
 #include <plat/pwm-core.h>
+#include <plat/watchdog-reset.h>
 
 #include "common.h"
 
@@ -73,7 +74,6 @@ static struct cpu_table cpu_ids[] __initdata = {
 		.idcode		= 0x32410000,
 		.idmask		= 0xffffffff,
 		.map_io		= s3c2410_map_io,
-		.init_clocks	= s3c2410_init_clocks,
 		.init_uarts	= s3c2410_init_uarts,
 		.init		= s3c2410_init,
 		.name		= name_s3c2410
@@ -82,7 +82,6 @@ static struct cpu_table cpu_ids[] __initdata = {
 		.idcode		= 0x32410002,
 		.idmask		= 0xffffffff,
 		.map_io		= s3c2410_map_io,
-		.init_clocks	= s3c2410_init_clocks,
 		.init_uarts	= s3c2410_init_uarts,
 		.init		= s3c2410a_init,
 		.name		= name_s3c2410a
@@ -91,7 +90,6 @@ static struct cpu_table cpu_ids[] __initdata = {
 		.idcode		= 0x32440000,
 		.idmask		= 0xffffffff,
 		.map_io		= s3c2440_map_io,
-		.init_clocks	= s3c244x_init_clocks,
 		.init_uarts	= s3c244x_init_uarts,
 		.init		= s3c2440_init,
 		.name		= name_s3c2440
@@ -100,7 +98,6 @@ static struct cpu_table cpu_ids[] __initdata = {
 		.idcode		= 0x32440001,
 		.idmask		= 0xffffffff,
 		.map_io		= s3c2440_map_io,
-		.init_clocks	= s3c244x_init_clocks,
 		.init_uarts	= s3c244x_init_uarts,
 		.init		= s3c2440_init,
 		.name		= name_s3c2440a
@@ -109,7 +106,6 @@ static struct cpu_table cpu_ids[] __initdata = {
 		.idcode		= 0x32440aaa,
 		.idmask		= 0xffffffff,
 		.map_io		= s3c2442_map_io,
-		.init_clocks	= s3c244x_init_clocks,
 		.init_uarts	= s3c244x_init_uarts,
 		.init		= s3c2442_init,
 		.name		= name_s3c2442
@@ -118,7 +114,6 @@ static struct cpu_table cpu_ids[] __initdata = {
 		.idcode		= 0x32440aab,
 		.idmask		= 0xffffffff,
 		.map_io		= s3c2442_map_io,
-		.init_clocks	= s3c244x_init_clocks,
 		.init_uarts	= s3c244x_init_uarts,
 		.init		= s3c2442_init,
 		.name		= name_s3c2442b
@@ -127,7 +122,6 @@ static struct cpu_table cpu_ids[] __initdata = {
 		.idcode		= 0x32412001,
 		.idmask		= 0xffffffff,
 		.map_io		= s3c2412_map_io,
-		.init_clocks	= s3c2412_init_clocks,
 		.init_uarts	= s3c2412_init_uarts,
 		.init		= s3c2412_init,
 		.name		= name_s3c2412,
@@ -136,7 +130,6 @@ static struct cpu_table cpu_ids[] __initdata = {
 		.idcode		= 0x32412003,
 		.idmask		= 0xffffffff,
 		.map_io		= s3c2412_map_io,
-		.init_clocks	= s3c2412_init_clocks,
 		.init_uarts	= s3c2412_init_uarts,
 		.init		= s3c2412_init,
 		.name		= name_s3c2412,
@@ -145,7 +138,6 @@ static struct cpu_table cpu_ids[] __initdata = {
 		.idcode		= 0x32450003,
 		.idmask		= 0xffffffff,
 		.map_io		= s3c2416_map_io,
-		.init_clocks	= s3c2416_init_clocks,
 		.init_uarts	= s3c2416_init_uarts,
 		.init		= s3c2416_init,
 		.name		= name_s3c2416,
@@ -154,7 +146,6 @@ static struct cpu_table cpu_ids[] __initdata = {
 		.idcode		= 0x32443001,
 		.idmask		= 0xffffffff,
 		.map_io		= s3c2443_map_io,
-		.init_clocks	= s3c2443_init_clocks,
 		.init_uarts	= s3c2443_init_uarts,
 		.init		= s3c2443_init,
 		.name		= name_s3c2443,
@@ -316,21 +307,6 @@ struct s3c24xx_uart_resources s3c2410_uart_resources[] __initdata = {
 	},
 };
 
-/* initialise all the clocks */
-
-void __init_or_cpufreq s3c24xx_setup_clocks(unsigned long fclk,
-					   unsigned long hclk,
-					   unsigned long pclk)
-{
-	clk_upll.rate = s3c24xx_get_pll(__raw_readl(S3C2410_UPLLCON),
-					clk_xtal.rate);
-
-	clk_mpll.rate = fclk;
-	clk_h.rate = hclk;
-	clk_p.rate = pclk;
-	clk_f.rate = fclk;
-}
-
 #if defined(CONFIG_CPU_S3C2410) || defined(CONFIG_CPU_S3C2412) || \
 	defined(CONFIG_CPU_S3C2440) || defined(CONFIG_CPU_S3C2442)
 static struct resource s3c2410_dma_resource[] = {
@@ -534,3 +510,62 @@ struct platform_device s3c2443_device_dma = {
 	},
 };
 #endif
+
+#if defined(CONFIG_COMMON_CLK) && defined(CONFIG_CPU_S3C2410)
+void __init s3c2410_init_clocks(int xtal)
+{
+	s3c2410_common_clk_init(NULL, xtal, 0, S3C24XX_VA_CLKPWR);
+	samsung_wdt_reset_init(S3C24XX_VA_WATCHDOG);
+}
+#endif
+
+#ifdef CONFIG_CPU_S3C2412
+void __init s3c2412_init_clocks(int xtal)
+{
+	s3c2412_common_clk_init(NULL, xtal, 0, S3C24XX_VA_CLKPWR);
+}
+#endif
+
+#ifdef CONFIG_CPU_S3C2416
+void __init s3c2416_init_clocks(int xtal)
+{
+	s3c2443_common_clk_init(NULL, xtal, 0, S3C24XX_VA_CLKPWR);
+}
+#endif
+
+#if defined(CONFIG_COMMON_CLK) && defined(CONFIG_CPU_S3C2440)
+void __init s3c2440_init_clocks(int xtal)
+{
+	s3c2410_common_clk_init(NULL, xtal, 1, S3C24XX_VA_CLKPWR);
+	samsung_wdt_reset_init(S3C24XX_VA_WATCHDOG);
+}
+#endif
+
+#if defined(CONFIG_COMMON_CLK) && defined(CONFIG_CPU_S3C2442)
+void __init s3c2442_init_clocks(int xtal)
+{
+	s3c2410_common_clk_init(NULL, xtal, 2, S3C24XX_VA_CLKPWR);
+	samsung_wdt_reset_init(S3C24XX_VA_WATCHDOG);
+}
+#endif
+
+#ifdef CONFIG_CPU_S3C2443
+void __init s3c2443_init_clocks(int xtal)
+{
+	s3c2443_common_clk_init(NULL, xtal, 1, S3C24XX_VA_CLKPWR);
+}
+#endif
+
+#if defined(CONFIG_CPU_S3C2410) || defined(CONFIG_CPU_S3C2440) || \
+	defined(CONFIG_CPU_S3C2442)
+static struct resource s3c2410_dclk_resource[] = {
+	[0] = DEFINE_RES_MEM(0x56000084, 0x4),
+};
+
+struct platform_device s3c2410_device_dclk = {
+	.name		= "s3c2410-dclk",
+	.id		= 0,
+	.num_resources	= ARRAY_SIZE(s3c2410_dclk_resource),
+	.resource	= s3c2410_dclk_resource,
+};
+#endif
diff --git a/arch/arm/mach-s3c24xx/common.h b/arch/arm/mach-s3c24xx/common.h
index e46c1041721..ac3ff12a060 100644
--- a/arch/arm/mach-s3c24xx/common.h
+++ b/arch/arm/mach-s3c24xx/common.h
@@ -67,16 +67,15 @@ extern struct syscore_ops s3c2416_irq_syscore_ops;
 #if defined(CONFIG_CPU_S3C2440) || defined(CONFIG_CPU_S3C2442)
 extern void s3c244x_map_io(void);
 extern void s3c244x_init_uarts(struct s3c2410_uartcfg *cfg, int no);
-extern void s3c244x_init_clocks(int xtal);
 extern void s3c244x_restart(enum reboot_mode mode, const char *cmd);
 #else
-#define s3c244x_init_clocks NULL
 #define s3c244x_init_uarts NULL
 #endif
 
 #ifdef CONFIG_CPU_S3C2440
 extern  int s3c2440_init(void);
 extern void s3c2440_map_io(void);
+extern void s3c2440_init_clocks(int xtal);
 extern void s3c2440_init_irq(void);
 #else
 #define s3c2440_init NULL
@@ -86,6 +85,7 @@ extern void s3c2440_init_irq(void);
 #ifdef CONFIG_CPU_S3C2442
 extern  int s3c2442_init(void);
 extern void s3c2442_map_io(void);
+extern void s3c2442_init_clocks(int xtal);
 extern void s3c2442_init_irq(void);
 #else
 #define s3c2442_init NULL
@@ -114,4 +114,21 @@ extern struct platform_device s3c2412_device_dma;
 extern struct platform_device s3c2440_device_dma;
 extern struct platform_device s3c2443_device_dma;
 
+extern struct platform_device s3c2410_device_dclk;
+
+#ifdef CONFIG_S3C2410_COMMON_CLK
+void __init s3c2410_common_clk_init(struct device_node *np, unsigned long xti_f,
+				    int current_soc,
+				    void __iomem *reg_base);
+#endif
+#ifdef CONFIG_S3C2412_COMMON_CLK
+void __init s3c2412_common_clk_init(struct device_node *np, unsigned long xti_f,
+				unsigned long ext_f, void __iomem *reg_base);
+#endif
+#ifdef CONFIG_S3C2443_COMMON_CLK
+void __init s3c2443_common_clk_init(struct device_node *np, unsigned long xti_f,
+				    int current_soc,
+				    void __iomem *reg_base);
+#endif
+
 #endif /* __ARCH_ARM_MACH_S3C24XX_COMMON_H */
diff --git a/arch/arm/mach-s3c24xx/cpufreq-utils.c b/arch/arm/mach-s3c24xx/cpufreq-utils.c
index 2a0aa5684e7..d4d9514335f 100644
--- a/arch/arm/mach-s3c24xx/cpufreq-utils.c
+++ b/arch/arm/mach-s3c24xx/cpufreq-utils.c
@@ -14,6 +14,7 @@
 #include <linux/errno.h>
 #include <linux/cpufreq.h>
 #include <linux/io.h>
+#include <linux/clk.h>
 
 #include <mach/map.h>
 #include <mach/regs-clock.h>
@@ -60,5 +61,6 @@ void s3c2410_cpufreq_setrefresh(struct s3c_cpufreq_config *cfg)
  */
 void s3c2410_set_fvco(struct s3c_cpufreq_config *cfg)
 {
-	__raw_writel(cfg->pll.driver_data, S3C2410_MPLLCON);
+	if (!IS_ERR(cfg->mpll))
+		clk_set_rate(cfg->mpll, cfg->pll.frequency);
 }
diff --git a/arch/arm/mach-s3c24xx/include/mach/debug-macro.S b/arch/arm/mach-s3c24xx/include/mach/debug-macro.S
deleted file mode 100644
index 2f39737544c..00000000000
--- a/arch/arm/mach-s3c24xx/include/mach/debug-macro.S
+++ /dev/null
@@ -1,101 +0,0 @@
-/* arch/arm/mach-s3c2410/include/mach/debug-macro.S
- *
- * Debugging macro include header
- *
- *  Copyright (C) 1994-1999 Russell King
- *  Copyright (C) 2005 Simtec Electronics
- *
- *  Moved from linux/arch/arm/kernel/debug.S by Ben Dooks
- *
- * 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 <mach/map.h>
-#include <mach/regs-gpio.h>
-#include <linux/serial_s3c.h>
-
-#define S3C2410_UART1_OFF (0x4000)
-#define SHIFT_2440TXF (14-9)
-
-	.macro addruart, rp, rv, tmp
-		ldr	\rp, = S3C24XX_PA_UART
-		ldr	\rv, = S3C24XX_VA_UART
-#if CONFIG_DEBUG_S3C_UART != 0
-		add	\rp, \rp, #(S3C2410_UART1_OFF * CONFIG_DEBUG_S3C_UART)
-		add	\rv, \rv, #(S3C2410_UART1_OFF * CONFIG_DEBUG_S3C_UART)
-#endif
-	.endm
-
-	.macro fifo_full_s3c24xx rd, rx
-		@ check for arm920 vs arm926. currently assume all arm926
-		@ devices have an 64 byte FIFO identical to the s3c2440
-		mrc	p15, 0, \rd, c0, c0
-		and	\rd, \rd, #0xff0
-		teq	\rd, #0x260
-		beq	1004f
-		mrc	p15, 0, \rd, c1, c0
-		tst	\rd, #1
-		addeq	\rd, \rx, #(S3C24XX_PA_GPIO - S3C24XX_PA_UART)
-		addne	\rd, \rx, #(S3C24XX_VA_GPIO - S3C24XX_VA_UART)
-		bic	\rd, \rd, #0xff000
-		ldr	\rd, [\rd, # S3C2410_GSTATUS1 - S3C2410_GPIOREG(0)]
-		and	\rd, \rd, #0x00ff0000
-		teq	\rd, #0x00440000		@ is it 2440?
-1004:
-		ldr	\rd, [\rx, # S3C2410_UFSTAT]
-		moveq	\rd, \rd, lsr #SHIFT_2440TXF
-		tst	\rd, #S3C2410_UFSTAT_TXFULL
-	.endm
-
-	.macro  fifo_full_s3c2410 rd, rx
-		ldr	\rd, [\rx, # S3C2410_UFSTAT]
-		tst	\rd, #S3C2410_UFSTAT_TXFULL
-	.endm
-
-/* fifo level reading */
-
-	.macro fifo_level_s3c24xx rd, rx
-		@ check for arm920 vs arm926. currently assume all arm926
-		@ devices have an 64 byte FIFO identical to the s3c2440
-		mrc	p15, 0, \rd, c0, c0
-		and	\rd, \rd, #0xff0
-		teq	\rd, #0x260
-		beq	10000f
-		mrc	p15, 0, \rd, c1, c0
-		tst	\rd, #1
-		addeq	\rd, \rx, #(S3C24XX_PA_GPIO - S3C24XX_PA_UART)
-		addne	\rd, \rx, #(S3C24XX_VA_GPIO - S3C24XX_VA_UART)
-		bic	\rd, \rd, #0xff000
-		ldr	\rd, [\rd, # S3C2410_GSTATUS1 - S3C2410_GPIOREG(0)]
-		and	\rd, \rd, #0x00ff0000
-		teq	\rd, #0x00440000		@ is it 2440?
-
-10000:
-		ldr	\rd, [\rx, # S3C2410_UFSTAT]
-		andne	\rd, \rd, #S3C2410_UFSTAT_TXMASK
-		andeq	\rd, \rd, #S3C2440_UFSTAT_TXMASK
-	.endm
-
-	.macro fifo_level_s3c2410 rd, rx
-		ldr	\rd, [\rx, # S3C2410_UFSTAT]
-		and	\rd, \rd, #S3C2410_UFSTAT_TXMASK
-	.endm
-
-/* Select the correct implementation depending on the configuration. The
- * S3C2440 will get selected by default, as these are the most widely
- * used variants of these
-*/
-
-#if defined(CONFIG_CPU_LLSERIAL_S3C2410_ONLY)
-#define fifo_full  fifo_full_s3c2410
-#define fifo_level fifo_level_s3c2410
-#elif !defined(CONFIG_CPU_LLSERIAL_S3C2440_ONLY)
-#define fifo_full  fifo_full_s3c24xx
-#define fifo_level fifo_level_s3c24xx
-#endif
-
-/* include the reset of the code which will do the work */
-
-#include <debug/samsung.S>
diff --git a/arch/arm/mach-s3c24xx/include/mach/regs-clock.h b/arch/arm/mach-s3c24xx/include/mach/regs-clock.h
index 3415b60082d..3db6c10de02 100644
--- a/arch/arm/mach-s3c24xx/include/mach/regs-clock.h
+++ b/arch/arm/mach-s3c24xx/include/mach/regs-clock.h
@@ -42,24 +42,6 @@
 #define S3C2410_CLKCON_IIS	     (1<<17)
 #define S3C2410_CLKCON_SPI	     (1<<18)
 
-/* DCLKCON register addresses in gpio.h */
-
-#define S3C2410_DCLKCON_DCLK0EN	     (1<<0)
-#define S3C2410_DCLKCON_DCLK0_PCLK   (0<<1)
-#define S3C2410_DCLKCON_DCLK0_UCLK   (1<<1)
-#define S3C2410_DCLKCON_DCLK0_DIV(x) (((x) - 1 )<<4)
-#define S3C2410_DCLKCON_DCLK0_CMP(x) (((x) - 1 )<<8)
-#define S3C2410_DCLKCON_DCLK0_DIV_MASK ((0xf)<<4)
-#define S3C2410_DCLKCON_DCLK0_CMP_MASK ((0xf)<<8)
-
-#define S3C2410_DCLKCON_DCLK1EN	     (1<<16)
-#define S3C2410_DCLKCON_DCLK1_PCLK   (0<<17)
-#define S3C2410_DCLKCON_DCLK1_UCLK   (1<<17)
-#define S3C2410_DCLKCON_DCLK1_DIV(x) (((x) - 1) <<20)
-#define S3C2410_DCLKCON_DCLK1_CMP(x) (((x) - 1) <<24)
-#define S3C2410_DCLKCON_DCLK1_DIV_MASK ((0xf) <<20)
-#define S3C2410_DCLKCON_DCLK1_CMP_MASK ((0xf) <<24)
-
 #define S3C2410_CLKDIVN_PDIVN	     (1<<0)
 #define S3C2410_CLKDIVN_HDIVN	     (1<<1)
 
diff --git a/arch/arm/mach-s3c24xx/include/mach/regs-gpio.h b/arch/arm/mach-s3c24xx/include/mach/regs-gpio.h
index c2ef016032a..c6583cfa583 100644
--- a/arch/arm/mach-s3c24xx/include/mach/regs-gpio.h
+++ b/arch/arm/mach-s3c24xx/include/mach/regs-gpio.h
@@ -457,9 +457,6 @@
 
 /* miscellaneous control */
 #define S3C2410_MISCCR	   S3C2410_GPIOREG(0x80)
-#define S3C2410_DCLKCON	   S3C2410_GPIOREG(0x84)
-
-#define S3C24XX_DCLKCON	   S3C24XX_GPIOREG2(0x84)
 
 /* see clock.h for dclk definitions */
 
diff --git a/arch/arm/mach-s3c24xx/mach-amlm5900.c b/arch/arm/mach-s3c24xx/mach-amlm5900.c
index 8ac9554aa99..5157e250dd1 100644
--- a/arch/arm/mach-s3c24xx/mach-amlm5900.c
+++ b/arch/arm/mach-s3c24xx/mach-amlm5900.c
@@ -161,11 +161,16 @@ static struct platform_device *amlm5900_devices[] __initdata = {
 static void __init amlm5900_map_io(void)
 {
 	s3c24xx_init_io(amlm5900_iodesc, ARRAY_SIZE(amlm5900_iodesc));
-	s3c24xx_init_clocks(0);
 	s3c24xx_init_uarts(amlm5900_uartcfgs, ARRAY_SIZE(amlm5900_uartcfgs));
 	samsung_set_timer_source(SAMSUNG_PWM3, SAMSUNG_PWM4);
 }
 
+static void __init amlm5900_init_time(void)
+{
+	s3c2410_init_clocks(12000000);
+	samsung_timer_init();
+}
+
 #ifdef CONFIG_FB_S3C2410
 static struct s3c2410fb_display __initdata amlm5900_lcd_info = {
 	.width		= 160,
@@ -241,6 +246,6 @@ MACHINE_START(AML_M5900, "AML_M5900")
 	.map_io		= amlm5900_map_io,
 	.init_irq	= s3c2410_init_irq,
 	.init_machine	= amlm5900_init,
-	.init_time	= samsung_timer_init,
+	.init_time	= amlm5900_init_time,
 	.restart	= s3c2410_restart,
 MACHINE_END
diff --git a/arch/arm/mach-s3c24xx/mach-anubis.c b/arch/arm/mach-s3c24xx/mach-anubis.c
index 81a270af233..e053581cab0 100644
--- a/arch/arm/mach-s3c24xx/mach-anubis.c
+++ b/arch/arm/mach-s3c24xx/mach-anubis.c
@@ -46,7 +46,6 @@
 
 #include <net/ax88796.h>
 
-#include <plat/clock.h>
 #include <plat/devs.h>
 #include <plat/cpu.h>
 #include <linux/platform_data/asoc-s3c24xx_simtec.h>
@@ -352,6 +351,7 @@ static struct platform_device anubis_device_sm501 = {
 /* Standard Anubis devices */
 
 static struct platform_device *anubis_devices[] __initdata = {
+	&s3c2410_device_dclk,
 	&s3c_device_ohci,
 	&s3c_device_wdt,
 	&s3c_device_adc,
@@ -364,14 +364,6 @@ static struct platform_device *anubis_devices[] __initdata = {
 	&anubis_device_sm501,
 };
 
-static struct clk *anubis_clocks[] __initdata = {
-	&s3c24xx_dclk0,
-	&s3c24xx_dclk1,
-	&s3c24xx_clkout0,
-	&s3c24xx_clkout1,
-	&s3c24xx_uclk,
-};
-
 /* I2C devices. */
 
 static struct i2c_board_info anubis_i2c_devs[] __initdata = {
@@ -394,23 +386,7 @@ static struct s3c24xx_audio_simtec_pdata __initdata anubis_audio = {
 
 static void __init anubis_map_io(void)
 {
-	/* initialise the clocks */
-
-	s3c24xx_dclk0.parent = &clk_upll;
-	s3c24xx_dclk0.rate   = 12*1000*1000;
-
-	s3c24xx_dclk1.parent = &clk_upll;
-	s3c24xx_dclk1.rate   = 24*1000*1000;
-
-	s3c24xx_clkout0.parent  = &s3c24xx_dclk0;
-	s3c24xx_clkout1.parent  = &s3c24xx_dclk1;
-
-	s3c24xx_uclk.parent  = &s3c24xx_clkout1;
-
-	s3c24xx_register_clocks(anubis_clocks, ARRAY_SIZE(anubis_clocks));
-
 	s3c24xx_init_io(anubis_iodesc, ARRAY_SIZE(anubis_iodesc));
-	s3c24xx_init_clocks(0);
 	s3c24xx_init_uarts(anubis_uartcfgs, ARRAY_SIZE(anubis_uartcfgs));
 	samsung_set_timer_source(SAMSUNG_PWM3, SAMSUNG_PWM4);
 
@@ -428,6 +404,12 @@ static void __init anubis_map_io(void)
 	}
 }
 
+static void __init anubis_init_time(void)
+{
+	s3c2440_init_clocks(12000000);
+	samsung_timer_init();
+}
+
 static void __init anubis_init(void)
 {
 	s3c_i2c0_set_platdata(NULL);
@@ -447,6 +429,6 @@ MACHINE_START(ANUBIS, "Simtec-Anubis")
 	.map_io		= anubis_map_io,
 	.init_machine	= anubis_init,
 	.init_irq	= s3c2440_init_irq,
-	.init_time	= samsung_timer_init,
+	.init_time	= anubis_init_time,
 	.restart	= s3c244x_restart,
 MACHINE_END
diff --git a/arch/arm/mach-s3c24xx/mach-at2440evb.c b/arch/arm/mach-s3c24xx/mach-at2440evb.c
index d8f6bb1096c..9db768f448a 100644
--- a/arch/arm/mach-s3c24xx/mach-at2440evb.c
+++ b/arch/arm/mach-s3c24xx/mach-at2440evb.c
@@ -45,7 +45,6 @@
 #include <linux/mtd/nand_ecc.h>
 #include <linux/mtd/partitions.h>
 
-#include <plat/clock.h>
 #include <plat/devs.h>
 #include <plat/cpu.h>
 #include <linux/platform_data/mmc-s3cmci.h>
@@ -192,11 +191,16 @@ static struct platform_device *at2440evb_devices[] __initdata = {
 static void __init at2440evb_map_io(void)
 {
 	s3c24xx_init_io(at2440evb_iodesc, ARRAY_SIZE(at2440evb_iodesc));
-	s3c24xx_init_clocks(16934400);
 	s3c24xx_init_uarts(at2440evb_uartcfgs, ARRAY_SIZE(at2440evb_uartcfgs));
 	samsung_set_timer_source(SAMSUNG_PWM3, SAMSUNG_PWM4);
 }
 
+static void __init at2440evb_init_time(void)
+{
+	s3c2440_init_clocks(16934400);
+	samsung_timer_init();
+}
+
 static void __init at2440evb_init(void)
 {
 	s3c24xx_fb_set_platdata(&at2440evb_fb_info);
@@ -213,6 +217,6 @@ MACHINE_START(AT2440EVB, "AT2440EVB")
 	.map_io		= at2440evb_map_io,
 	.init_machine	= at2440evb_init,
 	.init_irq	= s3c2440_init_irq,
-	.init_time	= samsung_timer_init,
+	.init_time	= at2440evb_init_time,
 	.restart	= s3c244x_restart,
 MACHINE_END
diff --git a/arch/arm/mach-s3c24xx/mach-bast.c b/arch/arm/mach-s3c24xx/mach-bast.c
index e371ff53a40..f9112b801a3 100644
--- a/arch/arm/mach-s3c24xx/mach-bast.c
+++ b/arch/arm/mach-s3c24xx/mach-bast.c
@@ -51,7 +51,6 @@
 #include <mach/regs-lcd.h>
 #include <mach/gpio-samsung.h>
 
-#include <plat/clock.h>
 #include <plat/cpu.h>
 #include <plat/cpu-freq.h>
 #include <plat/devs.h>
@@ -523,6 +522,7 @@ static struct s3c_hwmon_pdata bast_hwmon_info = {
 // cat /sys/devices/platform/s3c24xx-adc/s3c-hwmon/in_0
 
 static struct platform_device *bast_devices[] __initdata = {
+	&s3c2410_device_dclk,
 	&s3c_device_ohci,
 	&s3c_device_lcd,
 	&s3c_device_wdt,
@@ -537,14 +537,6 @@ static struct platform_device *bast_devices[] __initdata = {
 	&bast_sio,
 };
 
-static struct clk *bast_clocks[] __initdata = {
-	&s3c24xx_dclk0,
-	&s3c24xx_dclk1,
-	&s3c24xx_clkout0,
-	&s3c24xx_clkout1,
-	&s3c24xx_uclk,
-};
-
 static struct s3c_cpufreq_board __initdata bast_cpufreq = {
 	.refresh	= 7800, /* 7.8usec */
 	.auto_io	= 1,
@@ -558,29 +550,19 @@ static struct s3c24xx_audio_simtec_pdata __initdata bast_audio = {
 
 static void __init bast_map_io(void)
 {
-	/* initialise the clocks */
-
-	s3c24xx_dclk0.parent = &clk_upll;
-	s3c24xx_dclk0.rate   = 12*1000*1000;
-
-	s3c24xx_dclk1.parent = &clk_upll;
-	s3c24xx_dclk1.rate   = 24*1000*1000;
-
-	s3c24xx_clkout0.parent  = &s3c24xx_dclk0;
-	s3c24xx_clkout1.parent  = &s3c24xx_dclk1;
-
-	s3c24xx_uclk.parent  = &s3c24xx_clkout1;
-
-	s3c24xx_register_clocks(bast_clocks, ARRAY_SIZE(bast_clocks));
-
 	s3c_hwmon_set_platdata(&bast_hwmon_info);
 
 	s3c24xx_init_io(bast_iodesc, ARRAY_SIZE(bast_iodesc));
-	s3c24xx_init_clocks(0);
 	s3c24xx_init_uarts(bast_uartcfgs, ARRAY_SIZE(bast_uartcfgs));
 	samsung_set_timer_source(SAMSUNG_PWM3, SAMSUNG_PWM4);
 }
 
+static void __init bast_init_time(void)
+{
+	s3c2410_init_clocks(12000000);
+	samsung_timer_init();
+}
+
 static void __init bast_init(void)
 {
 	register_syscore_ops(&bast_pm_syscore_ops);
@@ -608,6 +590,6 @@ MACHINE_START(BAST, "Simtec-BAST")
 	.map_io		= bast_map_io,
 	.init_irq	= s3c2410_init_irq,
 	.init_machine	= bast_init,
-	.init_time	= samsung_timer_init,
+	.init_time	= bast_init_time,
 	.restart	= s3c2410_restart,
 MACHINE_END
diff --git a/arch/arm/mach-s3c24xx/mach-gta02.c b/arch/arm/mach-s3c24xx/mach-gta02.c
index dc4db849f0f..fc3a08d0cb3 100644
--- a/arch/arm/mach-s3c24xx/mach-gta02.c
+++ b/arch/arm/mach-s3c24xx/mach-gta02.c
@@ -501,7 +501,6 @@ static struct platform_device gta02_buttons_device = {
 static void __init gta02_map_io(void)
 {
 	s3c24xx_init_io(gta02_iodesc, ARRAY_SIZE(gta02_iodesc));
-	s3c24xx_init_clocks(12000000);
 	s3c24xx_init_uarts(gta02_uartcfgs, ARRAY_SIZE(gta02_uartcfgs));
 	samsung_set_timer_source(SAMSUNG_PWM3, SAMSUNG_PWM4);
 }
@@ -585,6 +584,11 @@ static void __init gta02_machine_init(void)
 	regulator_has_full_constraints();
 }
 
+static void __init gta02_init_time(void)
+{
+	s3c2442_init_clocks(12000000);
+	samsung_timer_init();
+}
 
 MACHINE_START(NEO1973_GTA02, "GTA02")
 	/* Maintainer: Nelson Castillo <arhuaco@freaks-unidos.net> */
@@ -592,6 +596,6 @@ MACHINE_START(NEO1973_GTA02, "GTA02")
 	.map_io		= gta02_map_io,
 	.init_irq	= s3c2442_init_irq,
 	.init_machine	= gta02_machine_init,
-	.init_time	= samsung_timer_init,
+	.init_time	= gta02_init_time,
 	.restart	= s3c244x_restart,
 MACHINE_END
diff --git a/arch/arm/mach-s3c24xx/mach-h1940.c b/arch/arm/mach-s3c24xx/mach-h1940.c
index e453acd92cb..fbf5487ae5d 100644
--- a/arch/arm/mach-s3c24xx/mach-h1940.c
+++ b/arch/arm/mach-s3c24xx/mach-h1940.c
@@ -57,7 +57,6 @@
 #include <mach/regs-lcd.h>
 #include <mach/gpio-samsung.h>
 
-#include <plat/clock.h>
 #include <plat/cpu.h>
 #include <plat/devs.h>
 #include <plat/gpio-cfg.h>
@@ -646,7 +645,6 @@ static struct platform_device *h1940_devices[] __initdata = {
 static void __init h1940_map_io(void)
 {
 	s3c24xx_init_io(h1940_iodesc, ARRAY_SIZE(h1940_iodesc));
-	s3c24xx_init_clocks(0);
 	s3c24xx_init_uarts(h1940_uartcfgs, ARRAY_SIZE(h1940_uartcfgs));
 	samsung_set_timer_source(SAMSUNG_PWM3, SAMSUNG_PWM4);
 
@@ -662,6 +660,12 @@ static void __init h1940_map_io(void)
 	WARN_ON(gpiochip_add(&h1940_latch_gpiochip));
 }
 
+static void __init h1940_init_time(void)
+{
+	s3c2410_init_clocks(12000000);
+	samsung_timer_init();
+}
+
 /* H1940 and RX3715 need to reserve this for suspend */
 static void __init h1940_reserve(void)
 {
@@ -739,6 +743,6 @@ MACHINE_START(H1940, "IPAQ-H1940")
 	.reserve	= h1940_reserve,
 	.init_irq	= s3c2410_init_irq,
 	.init_machine	= h1940_init,
-	.init_time	= samsung_timer_init,
+	.init_time	= h1940_init_time,
 	.restart	= s3c2410_restart,
 MACHINE_END
diff --git a/arch/arm/mach-s3c24xx/mach-jive.c b/arch/arm/mach-s3c24xx/mach-jive.c
index 5faa7239e7d..e81ea82c55f 100644
--- a/arch/arm/mach-s3c24xx/mach-jive.c
+++ b/arch/arm/mach-s3c24xx/mach-jive.c
@@ -507,11 +507,16 @@ static struct syscore_ops jive_pm_syscore_ops = {
 static void __init jive_map_io(void)
 {
 	s3c24xx_init_io(jive_iodesc, ARRAY_SIZE(jive_iodesc));
-	s3c24xx_init_clocks(12000000);
 	s3c24xx_init_uarts(jive_uartcfgs, ARRAY_SIZE(jive_uartcfgs));
 	samsung_set_timer_source(SAMSUNG_PWM3, SAMSUNG_PWM4);
 }
 
+static void __init jive_init_time(void)
+{
+	s3c2412_init_clocks(12000000);
+	samsung_timer_init();
+}
+
 static void jive_power_off(void)
 {
 	printk(KERN_INFO "powering system down...\n");
@@ -665,6 +670,6 @@ MACHINE_START(JIVE, "JIVE")
 	.init_irq	= s3c2412_init_irq,
 	.map_io		= jive_map_io,
 	.init_machine	= jive_machine_init,
-	.init_time	= samsung_timer_init,
+	.init_time	= jive_init_time,
 	.restart	= s3c2412_restart,
 MACHINE_END
diff --git a/arch/arm/mach-s3c24xx/mach-mini2440.c b/arch/arm/mach-s3c24xx/mach-mini2440.c
index 9e57fd9f4f3..5cc40ec1d25 100644
--- a/arch/arm/mach-s3c24xx/mach-mini2440.c
+++ b/arch/arm/mach-s3c24xx/mach-mini2440.c
@@ -54,7 +54,6 @@
 #include <linux/mtd/partitions.h>
 
 #include <plat/gpio-cfg.h>
-#include <plat/clock.h>
 #include <plat/devs.h>
 #include <plat/cpu.h>
 #include <plat/samsung-time.h>
@@ -525,11 +524,16 @@ static struct platform_device *mini2440_devices[] __initdata = {
 static void __init mini2440_map_io(void)
 {
 	s3c24xx_init_io(mini2440_iodesc, ARRAY_SIZE(mini2440_iodesc));
-	s3c24xx_init_clocks(12000000);
 	s3c24xx_init_uarts(mini2440_uartcfgs, ARRAY_SIZE(mini2440_uartcfgs));
 	samsung_set_timer_source(SAMSUNG_PWM3, SAMSUNG_PWM4);
 }
 
+static void __init mini2440_init_time(void)
+{
+	s3c2440_init_clocks(12000000);
+	samsung_timer_init();
+}
+
 /*
  * mini2440_features string
  *
@@ -690,6 +694,6 @@ MACHINE_START(MINI2440, "MINI2440")
 	.map_io		= mini2440_map_io,
 	.init_machine	= mini2440_init,
 	.init_irq	= s3c2440_init_irq,
-	.init_time	= samsung_timer_init,
+	.init_time	= mini2440_init_time,
 	.restart	= s3c244x_restart,
 MACHINE_END
diff --git a/arch/arm/mach-s3c24xx/mach-n30.c b/arch/arm/mach-s3c24xx/mach-n30.c
index 4cccaad3484..3ac2a54348d 100644
--- a/arch/arm/mach-s3c24xx/mach-n30.c
+++ b/arch/arm/mach-s3c24xx/mach-n30.c
@@ -45,7 +45,6 @@
 
 #include <linux/platform_data/i2c-s3c2410.h>
 
-#include <plat/clock.h>
 #include <plat/cpu.h>
 #include <plat/devs.h>
 #include <linux/platform_data/mmc-s3cmci.h>
@@ -535,11 +534,16 @@ static void __init n30_map_io(void)
 {
 	s3c24xx_init_io(n30_iodesc, ARRAY_SIZE(n30_iodesc));
 	n30_hwinit();
-	s3c24xx_init_clocks(0);
 	s3c24xx_init_uarts(n30_uartcfgs, ARRAY_SIZE(n30_uartcfgs));
 	samsung_set_timer_source(SAMSUNG_PWM3, SAMSUNG_PWM4);
 }
 
+static void __init n30_init_time(void)
+{
+	s3c2410_init_clocks(12000000);
+	samsung_timer_init();
+}
+
 /* GPB3 is the line that controls the pull-up for the USB D+ line */
 
 static void __init n30_init(void)
@@ -591,7 +595,7 @@ MACHINE_START(N30, "Acer-N30")
 				Ben Dooks <ben-linux@fluff.org>
 	*/
 	.atag_offset	= 0x100,
-	.init_time	= samsung_timer_init,
+	.init_time	= n30_init_time,
 	.init_machine	= n30_init,
 	.init_irq	= s3c2410_init_irq,
 	.map_io		= n30_map_io,
@@ -602,7 +606,7 @@ MACHINE_START(N35, "Acer-N35")
 	/* Maintainer: Christer Weinigel <christer@weinigel.se>
 	*/
 	.atag_offset	= 0x100,
-	.init_time	= samsung_timer_init,
+	.init_time	= n30_init_time,
 	.init_machine	= n30_init,
 	.init_irq	= s3c2410_init_irq,
 	.map_io		= n30_map_io,
diff --git a/arch/arm/mach-s3c24xx/mach-nexcoder.c b/arch/arm/mach-s3c24xx/mach-nexcoder.c
index 3066851f584..c82c281ce35 100644
--- a/arch/arm/mach-s3c24xx/mach-nexcoder.c
+++ b/arch/arm/mach-s3c24xx/mach-nexcoder.c
@@ -42,7 +42,6 @@
 #include <linux/platform_data/i2c-s3c2410.h>
 
 #include <plat/gpio-cfg.h>
-#include <plat/clock.h>
 #include <plat/devs.h>
 #include <plat/cpu.h>
 #include <plat/samsung-time.h>
@@ -135,13 +134,18 @@ static void __init nexcoder_sensorboard_init(void)
 static void __init nexcoder_map_io(void)
 {
 	s3c24xx_init_io(nexcoder_iodesc, ARRAY_SIZE(nexcoder_iodesc));
-	s3c24xx_init_clocks(0);
 	s3c24xx_init_uarts(nexcoder_uartcfgs, ARRAY_SIZE(nexcoder_uartcfgs));
 	samsung_set_timer_source(SAMSUNG_PWM3, SAMSUNG_PWM4);
 
 	nexcoder_sensorboard_init();
 }
 
+static void __init nexcoder_init_time(void)
+{
+	s3c2440_init_clocks(12000000);
+	samsung_timer_init();
+}
+
 static void __init nexcoder_init(void)
 {
 	s3c_i2c0_set_platdata(NULL);
@@ -154,6 +158,6 @@ MACHINE_START(NEXCODER_2440, "NexVision - Nexcoder 2440")
 	.map_io		= nexcoder_map_io,
 	.init_machine	= nexcoder_init,
 	.init_irq	= s3c2440_init_irq,
-	.init_time	= samsung_timer_init,
+	.init_time	= nexcoder_init_time,
 	.restart	= s3c244x_restart,
 MACHINE_END
diff --git a/arch/arm/mach-s3c24xx/mach-osiris.c b/arch/arm/mach-s3c24xx/mach-osiris.c
index a4ae4bb3666..189147b80ec 100644
--- a/arch/arm/mach-s3c24xx/mach-osiris.c
+++ b/arch/arm/mach-s3c24xx/mach-osiris.c
@@ -40,7 +40,6 @@
 #include <linux/mtd/nand_ecc.h>
 #include <linux/mtd/partitions.h>
 
-#include <plat/clock.h>
 #include <plat/cpu.h>
 #include <plat/cpu-freq.h>
 #include <plat/devs.h>
@@ -344,20 +343,13 @@ static struct i2c_board_info osiris_i2c_devs[] __initdata = {
 /* Standard Osiris devices */
 
 static struct platform_device *osiris_devices[] __initdata = {
+	&s3c2410_device_dclk,
 	&s3c_device_i2c0,
 	&s3c_device_wdt,
 	&s3c_device_nand,
 	&osiris_pcmcia,
 };
 
-static struct clk *osiris_clocks[] __initdata = {
-	&s3c24xx_dclk0,
-	&s3c24xx_dclk1,
-	&s3c24xx_clkout0,
-	&s3c24xx_clkout1,
-	&s3c24xx_uclk,
-};
-
 static struct s3c_cpufreq_board __initdata osiris_cpufreq = {
 	.refresh	= 7800, /* refresh period is 7.8usec */
 	.auto_io	= 1,
@@ -368,23 +360,7 @@ static void __init osiris_map_io(void)
 {
 	unsigned long flags;
 
-	/* initialise the clocks */
-
-	s3c24xx_dclk0.parent = &clk_upll;
-	s3c24xx_dclk0.rate   = 12*1000*1000;
-
-	s3c24xx_dclk1.parent = &clk_upll;
-	s3c24xx_dclk1.rate   = 24*1000*1000;
-
-	s3c24xx_clkout0.parent  = &s3c24xx_dclk0;
-	s3c24xx_clkout1.parent  = &s3c24xx_dclk1;
-
-	s3c24xx_uclk.parent  = &s3c24xx_clkout1;
-
-	s3c24xx_register_clocks(osiris_clocks, ARRAY_SIZE(osiris_clocks));
-
 	s3c24xx_init_io(osiris_iodesc, ARRAY_SIZE(osiris_iodesc));
-	s3c24xx_init_clocks(0);
 	s3c24xx_init_uarts(osiris_uartcfgs, ARRAY_SIZE(osiris_uartcfgs));
 	samsung_set_timer_source(SAMSUNG_PWM3, SAMSUNG_PWM4);
 
@@ -408,6 +384,12 @@ static void __init osiris_map_io(void)
 	local_irq_restore(flags);
 }
 
+static void __init osiris_init_time(void)
+{
+	s3c2440_init_clocks(12000000);
+	samsung_timer_init();
+}
+
 static void __init osiris_init(void)
 {
 	register_syscore_ops(&osiris_pm_syscore_ops);
@@ -429,6 +411,6 @@ MACHINE_START(OSIRIS, "Simtec-OSIRIS")
 	.map_io		= osiris_map_io,
 	.init_irq	= s3c2440_init_irq,
 	.init_machine	= osiris_init,
-	.init_time	= samsung_timer_init,
+	.init_time	= osiris_init_time,
 	.restart	= s3c244x_restart,
 MACHINE_END
diff --git a/arch/arm/mach-s3c24xx/mach-otom.c b/arch/arm/mach-s3c24xx/mach-otom.c
index bdb3faac2d9..45833001186 100644
--- a/arch/arm/mach-s3c24xx/mach-otom.c
+++ b/arch/arm/mach-s3c24xx/mach-otom.c
@@ -30,7 +30,6 @@
 #include <mach/hardware.h>
 #include <mach/regs-gpio.h>
 
-#include <plat/clock.h>
 #include <plat/cpu.h>
 #include <plat/devs.h>
 #include <plat/samsung-time.h>
@@ -100,11 +99,16 @@ static struct platform_device *otom11_devices[] __initdata = {
 static void __init otom11_map_io(void)
 {
 	s3c24xx_init_io(otom11_iodesc, ARRAY_SIZE(otom11_iodesc));
-	s3c24xx_init_clocks(0);
 	s3c24xx_init_uarts(otom11_uartcfgs, ARRAY_SIZE(otom11_uartcfgs));
 	samsung_set_timer_source(SAMSUNG_PWM3, SAMSUNG_PWM4);
 }
 
+static void __init otom11_init_time(void)
+{
+	s3c2410_init_clocks(12000000);
+	samsung_timer_init();
+}
+
 static void __init otom11_init(void)
 {
 	s3c_i2c0_set_platdata(NULL);
@@ -117,6 +121,6 @@ MACHINE_START(OTOM, "Nex Vision - Otom 1.1")
 	.map_io		= otom11_map_io,
 	.init_machine	= otom11_init,
 	.init_irq	= s3c2410_init_irq,
-	.init_time	= samsung_timer_init,
+	.init_time	= otom11_init_time,
 	.restart	= s3c2410_restart,
 MACHINE_END
diff --git a/arch/arm/mach-s3c24xx/mach-qt2410.c b/arch/arm/mach-s3c24xx/mach-qt2410.c
index 8c12787a8fd..228c9094519 100644
--- a/arch/arm/mach-s3c24xx/mach-qt2410.c
+++ b/arch/arm/mach-s3c24xx/mach-qt2410.c
@@ -304,11 +304,16 @@ __setup("tft=", qt2410_tft_setup);
 static void __init qt2410_map_io(void)
 {
 	s3c24xx_init_io(qt2410_iodesc, ARRAY_SIZE(qt2410_iodesc));
-	s3c24xx_init_clocks(12*1000*1000);
 	s3c24xx_init_uarts(smdk2410_uartcfgs, ARRAY_SIZE(smdk2410_uartcfgs));
 	samsung_set_timer_source(SAMSUNG_PWM3, SAMSUNG_PWM4);
 }
 
+static void __init qt2410_init_time(void)
+{
+	s3c2410_init_clocks(12000000);
+	samsung_timer_init();
+}
+
 static void __init qt2410_machine_init(void)
 {
 	s3c_nand_set_platdata(&qt2410_nand_info);
@@ -346,6 +351,6 @@ MACHINE_START(QT2410, "QT2410")
 	.map_io		= qt2410_map_io,
 	.init_irq	= s3c2410_init_irq,
 	.init_machine	= qt2410_machine_init,
-	.init_time	= samsung_timer_init,
+	.init_time	= qt2410_init_time,
 	.restart	= s3c2410_restart,
 MACHINE_END
diff --git a/arch/arm/mach-s3c24xx/mach-rx1950.c b/arch/arm/mach-s3c24xx/mach-rx1950.c
index afb784e934c..e2c6541909c 100644
--- a/arch/arm/mach-s3c24xx/mach-rx1950.c
+++ b/arch/arm/mach-s3c24xx/mach-rx1950.c
@@ -54,7 +54,6 @@
 #include <mach/regs-lcd.h>
 #include <mach/gpio-samsung.h>
 
-#include <plat/clock.h>
 #include <plat/cpu.h>
 #include <plat/devs.h>
 #include <plat/pm.h>
@@ -710,6 +709,7 @@ static struct i2c_board_info rx1950_i2c_devices[] = {
 };
 
 static struct platform_device *rx1950_devices[] __initdata = {
+	&s3c2410_device_dclk,
 	&s3c_device_lcd,
 	&s3c_device_wdt,
 	&s3c_device_i2c0,
@@ -728,20 +728,9 @@ static struct platform_device *rx1950_devices[] __initdata = {
 	&rx1950_leds,
 };
 
-static struct clk *rx1950_clocks[] __initdata = {
-	&s3c24xx_clkout0,
-	&s3c24xx_clkout1,
-};
-
 static void __init rx1950_map_io(void)
 {
-	s3c24xx_clkout0.parent  = &clk_h;
-	s3c24xx_clkout1.parent  = &clk_f;
-
-	s3c24xx_register_clocks(rx1950_clocks, ARRAY_SIZE(rx1950_clocks));
-
 	s3c24xx_init_io(rx1950_iodesc, ARRAY_SIZE(rx1950_iodesc));
-	s3c24xx_init_clocks(16934000);
 	s3c24xx_init_uarts(rx1950_uartcfgs, ARRAY_SIZE(rx1950_uartcfgs));
 	samsung_set_timer_source(SAMSUNG_PWM3, SAMSUNG_PWM4);
 
@@ -754,6 +743,12 @@ static void __init rx1950_map_io(void)
 	s3c_pm_init();
 }
 
+static void __init rx1950_init_time(void)
+{
+	s3c2442_init_clocks(16934000);
+	samsung_timer_init();
+}
+
 static void __init rx1950_init_machine(void)
 {
 	int i;
@@ -816,6 +811,6 @@ MACHINE_START(RX1950, "HP iPAQ RX1950")
 	.reserve	= rx1950_reserve,
 	.init_irq	= s3c2442_init_irq,
 	.init_machine = rx1950_init_machine,
-	.init_time	= samsung_timer_init,
+	.init_time	= rx1950_init_time,
 	.restart	= s3c244x_restart,
 MACHINE_END
diff --git a/arch/arm/mach-s3c24xx/mach-rx3715.c b/arch/arm/mach-s3c24xx/mach-rx3715.c
index e6535ce1bc5..6e749ec3a2e 100644
--- a/arch/arm/mach-s3c24xx/mach-rx3715.c
+++ b/arch/arm/mach-s3c24xx/mach-rx3715.c
@@ -46,7 +46,6 @@
 #include <mach/regs-lcd.h>
 #include <mach/gpio-samsung.h>
 
-#include <plat/clock.h>
 #include <plat/cpu.h>
 #include <plat/devs.h>
 #include <plat/pm.h>
@@ -179,11 +178,16 @@ static struct platform_device *rx3715_devices[] __initdata = {
 static void __init rx3715_map_io(void)
 {
 	s3c24xx_init_io(rx3715_iodesc, ARRAY_SIZE(rx3715_iodesc));
-	s3c24xx_init_clocks(16934000);
 	s3c24xx_init_uarts(rx3715_uartcfgs, ARRAY_SIZE(rx3715_uartcfgs));
 	samsung_set_timer_source(SAMSUNG_PWM3, SAMSUNG_PWM4);
 }
 
+static void __init rx3715_init_time(void)
+{
+	s3c2440_init_clocks(16934000);
+	samsung_timer_init();
+}
+
 /* H1940 and RX3715 need to reserve this for suspend */
 static void __init rx3715_reserve(void)
 {
@@ -210,6 +214,6 @@ MACHINE_START(RX3715, "IPAQ-RX3715")
 	.reserve	= rx3715_reserve,
 	.init_irq	= s3c2440_init_irq,
 	.init_machine	= rx3715_init_machine,
-	.init_time	= samsung_timer_init,
+	.init_time	= rx3715_init_time,
 	.restart	= s3c244x_restart,
 MACHINE_END
diff --git a/arch/arm/mach-s3c24xx/mach-s3c2416-dt.c b/arch/arm/mach-s3c24xx/mach-s3c2416-dt.c
index 70f0900d4bc..e4dcb9aa2ca 100644
--- a/arch/arm/mach-s3c24xx/mach-s3c2416-dt.c
+++ b/arch/arm/mach-s3c24xx/mach-s3c2416-dt.c
@@ -18,7 +18,6 @@
 #include <linux/clocksource.h>
 #include <linux/irqchip.h>
 #include <linux/of_platform.h>
-#include <linux/serial_core.h>
 #include <linux/serial_s3c.h>
 
 #include <asm/mach/arch.h>
@@ -29,48 +28,14 @@
 
 #include "common.h"
 
-/*
- * The following lookup table is used to override device names when devices
- * are registered from device tree. This is temporarily added to enable
- * device tree support addition for the S3C2416 architecture.
- *
- * For drivers that require platform data to be provided from the machine
- * file, a platform data pointer can also be supplied along with the
- * devices names. Usually, the platform data elements that cannot be parsed
- * from the device tree by the drivers (example: function pointers) are
- * supplied. But it should be noted that this is a temporary mechanism and
- * at some point, the drivers should be capable of parsing all the platform
- * data from the device tree.
- */
-static const struct of_dev_auxdata s3c2416_auxdata_lookup[] __initconst = {
-	OF_DEV_AUXDATA("samsung,s3c2440-uart", S3C24XX_PA_UART,
-				"s3c2440-uart.0", NULL),
-	OF_DEV_AUXDATA("samsung,s3c2440-uart", S3C24XX_PA_UART + 0x4000,
-				"s3c2440-uart.1", NULL),
-	OF_DEV_AUXDATA("samsung,s3c2440-uart", S3C24XX_PA_UART + 0x8000,
-				"s3c2440-uart.2", NULL),
-	OF_DEV_AUXDATA("samsung,s3c2440-uart", S3C24XX_PA_UART + 0xC000,
-				"s3c2440-uart.3", NULL),
-	OF_DEV_AUXDATA("samsung,s3c6410-sdhci", S3C_PA_HSMMC0,
-				"s3c-sdhci.0", NULL),
-	OF_DEV_AUXDATA("samsung,s3c6410-sdhci", S3C_PA_HSMMC1,
-				"s3c-sdhci.1", NULL),
-	OF_DEV_AUXDATA("samsung,s3c2440-i2c", S3C_PA_IIC,
-				"s3c2440-i2c.0", NULL),
-	{},
-};
-
 static void __init s3c2416_dt_map_io(void)
 {
 	s3c24xx_init_io(NULL, 0);
-	s3c24xx_init_clocks(12000000);
 }
 
 static void __init s3c2416_dt_machine_init(void)
 {
-	of_platform_populate(NULL, of_default_bus_match_table,
-				s3c2416_auxdata_lookup, NULL);
-
+	of_platform_populate(NULL, of_default_bus_match_table, NULL, NULL);
 	s3c_pm_init();
 }
 
@@ -86,6 +51,5 @@ DT_MACHINE_START(S3C2416_DT, "Samsung S3C2416 (Flattened Device Tree)")
 	.map_io		= s3c2416_dt_map_io,
 	.init_irq	= irqchip_init,
 	.init_machine	= s3c2416_dt_machine_init,
-	 .init_time	= clocksource_of_init,
 	.restart	= s3c2416_restart,
 MACHINE_END
diff --git a/arch/arm/mach-s3c24xx/mach-smdk2410.c b/arch/arm/mach-s3c24xx/mach-smdk2410.c
index f32924ee0e9..419fadd6e44 100644
--- a/arch/arm/mach-s3c24xx/mach-smdk2410.c
+++ b/arch/arm/mach-s3c24xx/mach-smdk2410.c
@@ -99,11 +99,16 @@ static struct platform_device *smdk2410_devices[] __initdata = {
 static void __init smdk2410_map_io(void)
 {
 	s3c24xx_init_io(smdk2410_iodesc, ARRAY_SIZE(smdk2410_iodesc));
-	s3c24xx_init_clocks(0);
 	s3c24xx_init_uarts(smdk2410_uartcfgs, ARRAY_SIZE(smdk2410_uartcfgs));
 	samsung_set_timer_source(SAMSUNG_PWM3, SAMSUNG_PWM4);
 }
 
+static void __init smdk2410_init_time(void)
+{
+	s3c2410_init_clocks(12000000);
+	samsung_timer_init();
+}
+
 static void __init smdk2410_init(void)
 {
 	s3c_i2c0_set_platdata(NULL);
@@ -118,6 +123,6 @@ MACHINE_START(SMDK2410, "SMDK2410") /* @TODO: request a new identifier and switc
 	.map_io		= smdk2410_map_io,
 	.init_irq	= s3c2410_init_irq,
 	.init_machine	= smdk2410_init,
-	.init_time	= samsung_timer_init,
+	.init_time	= smdk2410_init_time,
 	.restart	= s3c2410_restart,
 MACHINE_END
diff --git a/arch/arm/mach-s3c24xx/mach-smdk2413.c b/arch/arm/mach-s3c24xx/mach-smdk2413.c
index 233fe52d201..a38f8a049e2 100644
--- a/arch/arm/mach-s3c24xx/mach-smdk2413.c
+++ b/arch/arm/mach-s3c24xx/mach-smdk2413.c
@@ -106,11 +106,16 @@ static void __init smdk2413_fixup(struct tag *tags, char **cmdline,
 static void __init smdk2413_map_io(void)
 {
 	s3c24xx_init_io(smdk2413_iodesc, ARRAY_SIZE(smdk2413_iodesc));
-	s3c24xx_init_clocks(12000000);
 	s3c24xx_init_uarts(smdk2413_uartcfgs, ARRAY_SIZE(smdk2413_uartcfgs));
 	samsung_set_timer_source(SAMSUNG_PWM3, SAMSUNG_PWM4);
 }
 
+static void __init smdk2413_init_time(void)
+{
+	s3c2412_init_clocks(12000000);
+	samsung_timer_init();
+}
+
 static void __init smdk2413_machine_init(void)
 {	/* Turn off suspend on both USB ports, and switch the
 	 * selectable USB port to USB device mode. */
@@ -159,6 +164,6 @@ MACHINE_START(SMDK2413, "SMDK2413")
 	.init_irq	= s3c2412_init_irq,
 	.map_io		= smdk2413_map_io,
 	.init_machine	= smdk2413_machine_init,
-	.init_time	= samsung_timer_init,
+	.init_time	= smdk2413_init_time,
 	.restart	= s3c2412_restart,
 MACHINE_END
diff --git a/arch/arm/mach-s3c24xx/mach-smdk2416.c b/arch/arm/mach-s3c24xx/mach-smdk2416.c
index b3b54d8e141..fa6f30d2360 100644
--- a/arch/arm/mach-s3c24xx/mach-smdk2416.c
+++ b/arch/arm/mach-s3c24xx/mach-smdk2416.c
@@ -219,10 +219,15 @@ static struct platform_device *smdk2416_devices[] __initdata = {
 	&s3c2443_device_dma,
 };
 
+static void __init smdk2416_init_time(void)
+{
+	s3c2416_init_clocks(12000000);
+	samsung_timer_init();
+}
+
 static void __init smdk2416_map_io(void)
 {
 	s3c24xx_init_io(smdk2416_iodesc, ARRAY_SIZE(smdk2416_iodesc));
-	s3c24xx_init_clocks(12000000);
 	s3c24xx_init_uarts(smdk2416_uartcfgs, ARRAY_SIZE(smdk2416_uartcfgs));
 	samsung_set_timer_source(SAMSUNG_PWM3, SAMSUNG_PWM4);
 }
@@ -257,6 +262,6 @@ MACHINE_START(SMDK2416, "SMDK2416")
 	.init_irq	= s3c2416_init_irq,
 	.map_io		= smdk2416_map_io,
 	.init_machine	= smdk2416_machine_init,
-	.init_time	= samsung_timer_init,
+	.init_time	= smdk2416_init_time,
 	.restart	= s3c2416_restart,
 MACHINE_END
diff --git a/arch/arm/mach-s3c24xx/mach-smdk2440.c b/arch/arm/mach-s3c24xx/mach-smdk2440.c
index d071dcfea54..5fb89c0ae17 100644
--- a/arch/arm/mach-s3c24xx/mach-smdk2440.c
+++ b/arch/arm/mach-s3c24xx/mach-smdk2440.c
@@ -38,7 +38,6 @@
 #include <mach/fb.h>
 #include <linux/platform_data/i2c-s3c2410.h>
 
-#include <plat/clock.h>
 #include <plat/devs.h>
 #include <plat/cpu.h>
 #include <plat/samsung-time.h>
@@ -159,11 +158,16 @@ static struct platform_device *smdk2440_devices[] __initdata = {
 static void __init smdk2440_map_io(void)
 {
 	s3c24xx_init_io(smdk2440_iodesc, ARRAY_SIZE(smdk2440_iodesc));
-	s3c24xx_init_clocks(16934400);
 	s3c24xx_init_uarts(smdk2440_uartcfgs, ARRAY_SIZE(smdk2440_uartcfgs));
 	samsung_set_timer_source(SAMSUNG_PWM3, SAMSUNG_PWM4);
 }
 
+static void __init smdk2440_init_time(void)
+{
+	s3c2440_init_clocks(16934400);
+	samsung_timer_init();
+}
+
 static void __init smdk2440_machine_init(void)
 {
 	s3c24xx_fb_set_platdata(&smdk2440_fb_info);
@@ -180,6 +184,6 @@ MACHINE_START(S3C2440, "SMDK2440")
 	.init_irq	= s3c2440_init_irq,
 	.map_io		= smdk2440_map_io,
 	.init_machine	= smdk2440_machine_init,
-	.init_time	= samsung_timer_init,
+	.init_time	= smdk2440_init_time,
 	.restart	= s3c244x_restart,
 MACHINE_END
diff --git a/arch/arm/mach-s3c24xx/mach-smdk2443.c b/arch/arm/mach-s3c24xx/mach-smdk2443.c
index 06c4d77de3a..ef5d5ea3318 100644
--- a/arch/arm/mach-s3c24xx/mach-smdk2443.c
+++ b/arch/arm/mach-s3c24xx/mach-smdk2443.c
@@ -121,11 +121,16 @@ static struct platform_device *smdk2443_devices[] __initdata = {
 static void __init smdk2443_map_io(void)
 {
 	s3c24xx_init_io(smdk2443_iodesc, ARRAY_SIZE(smdk2443_iodesc));
-	s3c24xx_init_clocks(12000000);
 	s3c24xx_init_uarts(smdk2443_uartcfgs, ARRAY_SIZE(smdk2443_uartcfgs));
 	samsung_set_timer_source(SAMSUNG_PWM3, SAMSUNG_PWM4);
 }
 
+static void __init smdk2443_init_time(void)
+{
+	s3c2443_init_clocks(12000000);
+	samsung_timer_init();
+}
+
 static void __init smdk2443_machine_init(void)
 {
 	s3c_i2c0_set_platdata(NULL);
@@ -145,6 +150,6 @@ MACHINE_START(SMDK2443, "SMDK2443")
 	.init_irq	= s3c2443_init_irq,
 	.map_io		= smdk2443_map_io,
 	.init_machine	= smdk2443_machine_init,
-	.init_time	= samsung_timer_init,
+	.init_time	= smdk2443_init_time,
 	.restart	= s3c2443_restart,
 MACHINE_END
diff --git a/arch/arm/mach-s3c24xx/mach-tct_hammer.c b/arch/arm/mach-s3c24xx/mach-tct_hammer.c
index 4108b2f0ced..c616ca2d409 100644
--- a/arch/arm/mach-s3c24xx/mach-tct_hammer.c
+++ b/arch/arm/mach-s3c24xx/mach-tct_hammer.c
@@ -135,11 +135,16 @@ static struct platform_device *tct_hammer_devices[] __initdata = {
 static void __init tct_hammer_map_io(void)
 {
 	s3c24xx_init_io(tct_hammer_iodesc, ARRAY_SIZE(tct_hammer_iodesc));
-	s3c24xx_init_clocks(0);
 	s3c24xx_init_uarts(tct_hammer_uartcfgs, ARRAY_SIZE(tct_hammer_uartcfgs));
 	samsung_set_timer_source(SAMSUNG_PWM3, SAMSUNG_PWM4);
 }
 
+static void __init tct_hammer_init_time(void)
+{
+	s3c2410_init_clocks(12000000);
+	samsung_timer_init();
+}
+
 static void __init tct_hammer_init(void)
 {
 	s3c_i2c0_set_platdata(NULL);
@@ -151,6 +156,6 @@ MACHINE_START(TCT_HAMMER, "TCT_HAMMER")
 	.map_io		= tct_hammer_map_io,
 	.init_irq	= s3c2410_init_irq,
 	.init_machine	= tct_hammer_init,
-	.init_time	= samsung_timer_init,
+	.init_time	= tct_hammer_init_time,
 	.restart	= s3c2410_restart,
 MACHINE_END
diff --git a/arch/arm/mach-s3c24xx/mach-vr1000.c b/arch/arm/mach-s3c24xx/mach-vr1000.c
index 1cc5b1bd51c..f88c584c300 100644
--- a/arch/arm/mach-s3c24xx/mach-vr1000.c
+++ b/arch/arm/mach-s3c24xx/mach-vr1000.c
@@ -43,7 +43,6 @@
 #include <mach/regs-gpio.h>
 #include <mach/gpio-samsung.h>
 
-#include <plat/clock.h>
 #include <plat/cpu.h>
 #include <plat/devs.h>
 #include <plat/samsung-time.h>
@@ -286,6 +285,7 @@ static struct i2c_board_info vr1000_i2c_devs[] __initdata = {
 /* devices for this board */
 
 static struct platform_device *vr1000_devices[] __initdata = {
+	&s3c2410_device_dclk,
 	&s3c_device_ohci,
 	&s3c_device_lcd,
 	&s3c_device_wdt,
@@ -299,14 +299,6 @@ static struct platform_device *vr1000_devices[] __initdata = {
 	&vr1000_led3,
 };
 
-static struct clk *vr1000_clocks[] __initdata = {
-	&s3c24xx_dclk0,
-	&s3c24xx_dclk1,
-	&s3c24xx_clkout0,
-	&s3c24xx_clkout1,
-	&s3c24xx_uclk,
-};
-
 static void vr1000_power_off(void)
 {
 	gpio_direction_output(S3C2410_GPB(9), 1);
@@ -314,29 +306,19 @@ static void vr1000_power_off(void)
 
 static void __init vr1000_map_io(void)
 {
-	/* initialise clock sources */
-
-	s3c24xx_dclk0.parent = &clk_upll;
-	s3c24xx_dclk0.rate   = 12*1000*1000;
-
-	s3c24xx_dclk1.parent = NULL;
-	s3c24xx_dclk1.rate   = 3692307;
-
-	s3c24xx_clkout0.parent  = &s3c24xx_dclk0;
-	s3c24xx_clkout1.parent  = &s3c24xx_dclk1;
-
-	s3c24xx_uclk.parent  = &s3c24xx_clkout1;
-
-	s3c24xx_register_clocks(vr1000_clocks, ARRAY_SIZE(vr1000_clocks));
-
 	pm_power_off = vr1000_power_off;
 
 	s3c24xx_init_io(vr1000_iodesc, ARRAY_SIZE(vr1000_iodesc));
-	s3c24xx_init_clocks(0);
 	s3c24xx_init_uarts(vr1000_uartcfgs, ARRAY_SIZE(vr1000_uartcfgs));
 	samsung_set_timer_source(SAMSUNG_PWM3, SAMSUNG_PWM4);
 }
 
+static void __init vr1000_init_time(void)
+{
+	s3c2410_init_clocks(12000000);
+	samsung_timer_init();
+}
+
 static void __init vr1000_init(void)
 {
 	s3c_i2c0_set_platdata(NULL);
@@ -357,6 +339,6 @@ MACHINE_START(VR1000, "Thorcom-VR1000")
 	.map_io		= vr1000_map_io,
 	.init_machine	= vr1000_init,
 	.init_irq	= s3c2410_init_irq,
-	.init_time	= samsung_timer_init,
+	.init_time	= vr1000_init_time,
 	.restart	= s3c2410_restart,
 MACHINE_END
diff --git a/arch/arm/mach-s3c24xx/mach-vstms.c b/arch/arm/mach-s3c24xx/mach-vstms.c
index 40868c0e0a6..6b706c91538 100644
--- a/arch/arm/mach-s3c24xx/mach-vstms.c
+++ b/arch/arm/mach-s3c24xx/mach-vstms.c
@@ -142,11 +142,16 @@ static void __init vstms_fixup(struct tag *tags, char **cmdline,
 static void __init vstms_map_io(void)
 {
 	s3c24xx_init_io(vstms_iodesc, ARRAY_SIZE(vstms_iodesc));
-	s3c24xx_init_clocks(12000000);
 	s3c24xx_init_uarts(vstms_uartcfgs, ARRAY_SIZE(vstms_uartcfgs));
 	samsung_set_timer_source(SAMSUNG_PWM3, SAMSUNG_PWM4);
 }
 
+static void __init vstms_init_time(void)
+{
+	s3c2412_init_clocks(12000000);
+	samsung_timer_init();
+}
+
 static void __init vstms_init(void)
 {
 	s3c_i2c0_set_platdata(NULL);
@@ -162,6 +167,6 @@ MACHINE_START(VSTMS, "VSTMS")
 	.init_irq	= s3c2412_init_irq,
 	.init_machine	= vstms_init,
 	.map_io		= vstms_map_io,
-	.init_time	= samsung_timer_init,
+	.init_time	= vstms_init_time,
 	.restart	= s3c2412_restart,
 MACHINE_END
diff --git a/arch/arm/mach-s3c24xx/pm.c b/arch/arm/mach-s3c24xx/pm.c
index 68ea5b7e5dc..b19256ec8d4 100644
--- a/arch/arm/mach-s3c24xx/pm.c
+++ b/arch/arm/mach-s3c24xx/pm.c
@@ -51,9 +51,6 @@
 #define PFX "s3c24xx-pm: "
 
 static struct sleep_save core_save[] = {
-	SAVE_ITEM(S3C2410_LOCKTIME),
-	SAVE_ITEM(S3C2410_CLKCON),
-
 	/* we restore the timings here, with the proviso that the board
 	 * brings the system up in an slower, or equal frequency setting
 	 * to the original system.
@@ -69,18 +66,6 @@ static struct sleep_save core_save[] = {
 	SAVE_ITEM(S3C2410_BANKCON3),
 	SAVE_ITEM(S3C2410_BANKCON4),
 	SAVE_ITEM(S3C2410_BANKCON5),
-
-#ifndef CONFIG_CPU_FREQ
-	SAVE_ITEM(S3C2410_CLKDIVN),
-	SAVE_ITEM(S3C2410_MPLLCON),
-	SAVE_ITEM(S3C2410_REFRESH),
-#endif
-	SAVE_ITEM(S3C2410_UPLLCON),
-	SAVE_ITEM(S3C2410_CLKSLOW),
-};
-
-static struct sleep_save misc_save[] = {
-	SAVE_ITEM(S3C2410_DCLKCON),
 };
 
 /* s3c_pm_check_resume_pin
@@ -140,12 +125,10 @@ void s3c_pm_configure_extint(void)
 void s3c_pm_restore_core(void)
 {
 	s3c_pm_do_restore_core(core_save, ARRAY_SIZE(core_save));
-	s3c_pm_do_restore(misc_save, ARRAY_SIZE(misc_save));
 }
 
 void s3c_pm_save_core(void)
 {
-	s3c_pm_do_save(misc_save, ARRAY_SIZE(misc_save));
 	s3c_pm_do_save(core_save, ARRAY_SIZE(core_save));
 }
 
diff --git a/arch/arm/mach-s3c24xx/s3c2410.c b/arch/arm/mach-s3c24xx/s3c2410.c
index 04b58cb4988..7eab8882988 100644
--- a/arch/arm/mach-s3c24xx/s3c2410.c
+++ b/arch/arm/mach-s3c24xx/s3c2410.c
@@ -85,62 +85,6 @@ void __init s3c2410_map_io(void)
 
 void __init_or_cpufreq s3c2410_setup_clocks(void)
 {
-	struct clk *xtal_clk;
-	unsigned long tmp;
-	unsigned long xtal;
-	unsigned long fclk;
-	unsigned long hclk;
-	unsigned long pclk;
-
-	xtal_clk = clk_get(NULL, "xtal");
-	xtal = clk_get_rate(xtal_clk);
-	clk_put(xtal_clk);
-
-	/* now we've got our machine bits initialised, work out what
-	 * clocks we've got */
-
-	fclk = s3c24xx_get_pll(__raw_readl(S3C2410_MPLLCON), xtal);
-
-	tmp = __raw_readl(S3C2410_CLKDIVN);
-
-	/* work out clock scalings */
-
-	hclk = fclk / ((tmp & S3C2410_CLKDIVN_HDIVN) ? 2 : 1);
-	pclk = hclk / ((tmp & S3C2410_CLKDIVN_PDIVN) ? 2 : 1);
-
-	/* print brieft summary of clocks, etc */
-
-	printk("S3C2410: core %ld.%03ld MHz, memory %ld.%03ld MHz, peripheral %ld.%03ld MHz\n",
-	       print_mhz(fclk), print_mhz(hclk), print_mhz(pclk));
-
-	/* initialise the clocks here, to allow other things like the
-	 * console to use them
-	 */
-
-	s3c24xx_setup_clocks(fclk, hclk, pclk);
-}
-
-/* fake ARMCLK for use with cpufreq, etc. */
-
-static struct clk s3c2410_armclk = {
-	.name	= "armclk",
-	.parent	= &clk_f,
-	.id	= -1,
-};
-
-static struct clk_lookup s3c2410_clk_lookup[] = {
-	CLKDEV_INIT(NULL, "clk_uart_baud0", &clk_p),
-	CLKDEV_INIT(NULL, "clk_uart_baud1", &s3c24xx_uclk),
-};
-
-void __init s3c2410_init_clocks(int xtal)
-{
-	s3c24xx_register_baseclocks(xtal);
-	s3c2410_setup_clocks();
-	s3c2410_baseclk_add();
-	s3c24xx_register_clock(&s3c2410_armclk);
-	clkdev_add_table(s3c2410_clk_lookup, ARRAY_SIZE(s3c2410_clk_lookup));
-	samsung_wdt_reset_init(S3C24XX_VA_WATCHDOG);
 }
 
 struct bus_type s3c2410_subsys = {
diff --git a/arch/arm/mach-s3c24xx/s3c2412.c b/arch/arm/mach-s3c24xx/s3c2412.c
index 657cbaca80a..d49f52fbc84 100644
--- a/arch/arm/mach-s3c24xx/s3c2412.c
+++ b/arch/arm/mach-s3c24xx/s3c2412.c
@@ -173,49 +173,6 @@ void __init s3c2412_map_io(void)
 
 void __init_or_cpufreq s3c2412_setup_clocks(void)
 {
-	struct clk *xtal_clk;
-	unsigned long tmp;
-	unsigned long xtal;
-	unsigned long fclk;
-	unsigned long hclk;
-	unsigned long pclk;
-
-	xtal_clk = clk_get(NULL, "xtal");
-	xtal = clk_get_rate(xtal_clk);
-	clk_put(xtal_clk);
-
-	/* now we've got our machine bits initialised, work out what
-	 * clocks we've got */
-
-	fclk = s3c24xx_get_pll(__raw_readl(S3C2410_MPLLCON), xtal * 2);
-
-	clk_mpll.rate = fclk;
-
-	tmp = __raw_readl(S3C2410_CLKDIVN);
-
-	/* work out clock scalings */
-
-	hclk = fclk / ((tmp & S3C2412_CLKDIVN_HDIVN_MASK) + 1);
-	hclk /= ((tmp & S3C2412_CLKDIVN_ARMDIVN) ? 2 : 1);
-	pclk = hclk / ((tmp & S3C2412_CLKDIVN_PDIVN) ? 2 : 1);
-
-	/* print brieft summary of clocks, etc */
-
-	printk("S3C2412: core %ld.%03ld MHz, memory %ld.%03ld MHz, peripheral %ld.%03ld MHz\n",
-	       print_mhz(fclk), print_mhz(hclk), print_mhz(pclk));
-
-	s3c24xx_setup_clocks(fclk, hclk, pclk);
-}
-
-void __init s3c2412_init_clocks(int xtal)
-{
-	/* initialise the clocks here, to allow other things like the
-	 * console to use them
-	 */
-
-	s3c24xx_register_baseclocks(xtal);
-	s3c2412_setup_clocks();
-	s3c2412_baseclk_add();
 }
 
 /* need to register the subsystem before we actually register the device, and
diff --git a/arch/arm/mach-s3c24xx/s3c2442.c b/arch/arm/mach-s3c24xx/s3c2442.c
index 2c8adc02853..fb9da2b603a 100644
--- a/arch/arm/mach-s3c24xx/s3c2442.c
+++ b/arch/arm/mach-s3c24xx/s3c2442.c
@@ -53,117 +53,6 @@
 
 #include "common.h"
 
-/* S3C2442 extended clock support */
-
-static unsigned long s3c2442_camif_upll_round(struct clk *clk,
-					      unsigned long rate)
-{
-	unsigned long parent_rate = clk_get_rate(clk->parent);
-	int div;
-
-	if (rate > parent_rate)
-		return parent_rate;
-
-	div = parent_rate / rate;
-
-	if (div == 3)
-		return parent_rate / 3;
-
-	/* note, we remove the +/- 1 calculations for the divisor */
-
-	div /= 2;
-
-	if (div < 1)
-		div = 1;
-	else if (div > 16)
-		div = 16;
-
-	return parent_rate / (div * 2);
-}
-
-static int s3c2442_camif_upll_setrate(struct clk *clk, unsigned long rate)
-{
-	unsigned long parent_rate = clk_get_rate(clk->parent);
-	unsigned long camdivn =  __raw_readl(S3C2440_CAMDIVN);
-
-	rate = s3c2442_camif_upll_round(clk, rate);
-
-	camdivn &= ~S3C2442_CAMDIVN_CAMCLK_DIV3;
-
-	if (rate == parent_rate) {
-		camdivn &= ~S3C2440_CAMDIVN_CAMCLK_SEL;
-	} else if ((parent_rate / rate) == 3) {
-		camdivn |= S3C2440_CAMDIVN_CAMCLK_SEL;
-		camdivn |= S3C2442_CAMDIVN_CAMCLK_DIV3;
-	} else {
-		camdivn &= ~S3C2440_CAMDIVN_CAMCLK_MASK;
-		camdivn |= S3C2440_CAMDIVN_CAMCLK_SEL;
-		camdivn |= (((parent_rate / rate) / 2) - 1);
-	}
-
-	__raw_writel(camdivn, S3C2440_CAMDIVN);
-
-	return 0;
-}
-
-/* Extra S3C2442 clocks */
-
-static struct clk s3c2442_clk_cam = {
-	.name		= "camif",
-	.id		= -1,
-	.enable		= s3c2410_clkcon_enable,
-	.ctrlbit	= S3C2440_CLKCON_CAMERA,
-};
-
-static struct clk s3c2442_clk_cam_upll = {
-	.name		= "camif-upll",
-	.id		= -1,
-	.ops		= &(struct clk_ops) {
-		.set_rate	= s3c2442_camif_upll_setrate,
-		.round_rate	= s3c2442_camif_upll_round,
-	},
-};
-
-static int s3c2442_clk_add(struct device *dev, struct subsys_interface *sif)
-{
-	struct clk *clock_upll;
-	struct clk *clock_h;
-	struct clk *clock_p;
-
-	clock_p = clk_get(NULL, "pclk");
-	clock_h = clk_get(NULL, "hclk");
-	clock_upll = clk_get(NULL, "upll");
-
-	if (IS_ERR(clock_p) || IS_ERR(clock_h) || IS_ERR(clock_upll)) {
-		printk(KERN_ERR "S3C2442: Failed to get parent clocks\n");
-		return -EINVAL;
-	}
-
-	s3c2442_clk_cam.parent = clock_h;
-	s3c2442_clk_cam_upll.parent = clock_upll;
-
-	s3c24xx_register_clock(&s3c2442_clk_cam);
-	s3c24xx_register_clock(&s3c2442_clk_cam_upll);
-
-	clk_disable(&s3c2442_clk_cam);
-
-	return 0;
-}
-
-static struct subsys_interface s3c2442_clk_interface = {
-	.name		= "s3c2442_clk",
-	.subsys		= &s3c2442_subsys,
-	.add_dev	= s3c2442_clk_add,
-};
-
-static __init int s3c2442_clk_init(void)
-{
-	return subsys_interface_register(&s3c2442_clk_interface);
-}
-
-arch_initcall(s3c2442_clk_init);
-
-
 static struct device s3c2442_dev = {
 	.bus		= &s3c2442_subsys,
 };
diff --git a/arch/arm/mach-s3c24xx/s3c244x.c b/arch/arm/mach-s3c24xx/s3c244x.c
index fe30ebb234d..4a64bcc9eb5 100644
--- a/arch/arm/mach-s3c24xx/s3c244x.c
+++ b/arch/arm/mach-s3c24xx/s3c244x.c
@@ -46,6 +46,7 @@
 #include <plat/nand-core.h>
 #include <plat/watchdog-reset.h>
 
+#include "common.h"
 #include "regs-dsc.h"
 
 static struct map_desc s3c244x_iodesc[] __initdata = {
@@ -74,67 +75,11 @@ void __init s3c244x_map_io(void)
 	s3c_nand_setname("s3c2440-nand");
 	s3c_device_ts.name = "s3c2440-ts";
 	s3c_device_usbgadget.name = "s3c2440-usbgadget";
+	s3c2410_device_dclk.name = "s3c2440-dclk";
 }
 
 void __init_or_cpufreq s3c244x_setup_clocks(void)
 {
-	struct clk *xtal_clk;
-	unsigned long clkdiv;
-	unsigned long camdiv;
-	unsigned long xtal;
-	unsigned long hclk, fclk, pclk;
-	int hdiv = 1;
-
-	xtal_clk = clk_get(NULL, "xtal");
-	xtal = clk_get_rate(xtal_clk);
-	clk_put(xtal_clk);
-
-	fclk = s3c24xx_get_pll(__raw_readl(S3C2410_MPLLCON), xtal) * 2;
-
-	clkdiv = __raw_readl(S3C2410_CLKDIVN);
-	camdiv = __raw_readl(S3C2440_CAMDIVN);
-
-	/* work out clock scalings */
-
-	switch (clkdiv & S3C2440_CLKDIVN_HDIVN_MASK) {
-	case S3C2440_CLKDIVN_HDIVN_1:
-		hdiv = 1;
-		break;
-
-	case S3C2440_CLKDIVN_HDIVN_2:
-		hdiv = 2;
-		break;
-
-	case S3C2440_CLKDIVN_HDIVN_4_8:
-		hdiv = (camdiv & S3C2440_CAMDIVN_HCLK4_HALF) ? 8 : 4;
-		break;
-
-	case S3C2440_CLKDIVN_HDIVN_3_6:
-		hdiv = (camdiv & S3C2440_CAMDIVN_HCLK3_HALF) ? 6 : 3;
-		break;
-	}
-
-	hclk = fclk / hdiv;
-	pclk = hclk / ((clkdiv & S3C2440_CLKDIVN_PDIVN) ? 2 : 1);
-
-	/* print brief summary of clocks, etc */
-
-	printk("S3C244X: core %ld.%03ld MHz, memory %ld.%03ld MHz, peripheral %ld.%03ld MHz\n",
-	       print_mhz(fclk), print_mhz(hclk), print_mhz(pclk));
-
-	s3c24xx_setup_clocks(fclk, hclk, pclk);
-}
-
-void __init s3c244x_init_clocks(int xtal)
-{
-	/* initialise the clocks here, to allow other things like the
-	 * console to use them, and to add new ones after the initialisation
-	 */
-
-	s3c24xx_register_baseclocks(xtal);
-	s3c244x_setup_clocks();
-	s3c2410_baseclk_add();
-	samsung_wdt_reset_init(S3C24XX_VA_WATCHDOG);
 }
 
 /* Since the S3C2442 and S3C2440 share items, put both subsystems here */
diff --git a/arch/arm/mach-shmobile/Kconfig b/arch/arm/mach-shmobile/Kconfig
index 0f92ba8e788..dbd954e61aa 100644
--- a/arch/arm/mach-shmobile/Kconfig
+++ b/arch/arm/mach-shmobile/Kconfig
@@ -8,7 +8,6 @@ config ARCH_SHMOBILE_MULTI
 	select HAVE_ARM_SCU if SMP
 	select HAVE_ARM_TWD if SMP
 	select ARM_GIC
-	select MIGHT_HAVE_PCI
 	select ARCH_DMA_ADDR_T_64BIT if ARM_LPAE
 	select NO_IOPORT_MAP
 	select PINCTRL
@@ -108,6 +107,7 @@ config ARCH_R8A7778
 	select SH_CLK_CPG
 	select ARM_GIC
 	select SYS_SUPPORTS_SH_TMU
+	select RENESAS_INTC_IRQPIN
 
 config ARCH_R8A7779
 	bool "R-Car H1 (R8A77790)"
@@ -140,16 +140,6 @@ config ARCH_R8A7791
 	select SYS_SUPPORTS_SH_CMT
 	select ARCH_DMA_ADDR_T_64BIT if ARM_LPAE
 
-config ARCH_EMEV2
-	bool "Emma Mobile EV2"
-	select ARCH_WANT_OPTIONAL_GPIOLIB
-	select ARM_GIC
-	select CPU_V7
-	select MIGHT_HAVE_PCI
-	select USE_OF
-	select AUTO_ZRELADDR
-	select SYS_SUPPORTS_EM_STI
-
 config ARCH_R7S72100
 	bool "RZ/A1H (R7S72100)"
 	select ARCH_WANT_OPTIONAL_GPIOLIB
@@ -205,8 +195,8 @@ config MACH_ARMADILLO800EVA_REFERENCE
 	select SND_SOC_WM8978 if SND_SIMPLE_CARD
 	select USE_OF
 	---help---
-	   Use reference implementation of Aramdillo800 EVA board support
-	   which makes a greater use of device tree at the expense
+	   Use reference implementation of Armadillo800 EVA board support
+	   which makes greater use of device tree at the expense
 	   of not supporting a number of devices.
 
 	   This is intended to aid developers
@@ -216,7 +206,6 @@ config MACH_BOCKW
 	depends on ARCH_R8A7778
 	select ARCH_REQUIRE_GPIOLIB
 	select REGULATOR_FIXED_VOLTAGE if REGULATOR
-	select RENESAS_INTC_IRQPIN
 	select SND_SOC_AK4554 if SND_SIMPLE_CARD
 	select SND_SOC_AK4642 if SND_SIMPLE_CARD
 	select USE_OF
@@ -225,7 +214,6 @@ config MACH_BOCKW_REFERENCE
 	bool "BOCK-W  - Reference Device Tree Implementation"
 	depends on ARCH_R8A7778
 	select ARCH_REQUIRE_GPIOLIB
-	select RENESAS_INTC_IRQPIN
 	select REGULATOR_FIXED_VOLTAGE if REGULATOR
 	select USE_OF
 	---help---
@@ -240,17 +228,6 @@ config MACH_GENMAI
 	depends on ARCH_R7S72100
 	select USE_OF
 
-config MACH_GENMAI_REFERENCE
-	bool "Genmai board - Reference Device Tree Implementation"
-	depends on ARCH_R7S72100
-	select USE_OF
-	---help---
-	   Use reference implementation of Genmai board support
-	   which makes use of device tree at the expense
-	   of not supporting a number of devices.
-
-	   This is intended to aid developers
-
 config MACH_MARZEN
 	bool "MARZEN board"
 	depends on ARCH_R8A7779
diff --git a/arch/arm/mach-shmobile/Makefile b/arch/arm/mach-shmobile/Makefile
index 4caffc912a8..38d5fe825e9 100644
--- a/arch/arm/mach-shmobile/Makefile
+++ b/arch/arm/mach-shmobile/Makefile
@@ -21,8 +21,8 @@ obj-$(CONFIG_ARCH_EMEV2)	+= setup-emev2.o
 obj-$(CONFIG_ARCH_R7S72100)	+= setup-r7s72100.o
 
 # Clock objects
-ifndef CONFIG_COMMON_CLK
 obj-y				+= clock.o
+ifndef CONFIG_COMMON_CLK
 obj-$(CONFIG_ARCH_SH7372)	+= clock-sh7372.o
 obj-$(CONFIG_ARCH_SH73A0)	+= clock-sh73a0.o
 obj-$(CONFIG_ARCH_R8A73A4)	+= clock-r8a73a4.o
@@ -31,7 +31,6 @@ obj-$(CONFIG_ARCH_R8A7778)	+= clock-r8a7778.o
 obj-$(CONFIG_ARCH_R8A7779)	+= clock-r8a7779.o
 obj-$(CONFIG_ARCH_R8A7790)	+= clock-r8a7790.o
 obj-$(CONFIG_ARCH_R8A7791)	+= clock-r8a7791.o
-obj-$(CONFIG_ARCH_EMEV2)	+= clock-emev2.o
 obj-$(CONFIG_ARCH_R7S72100)	+= clock-r7s72100.o
 endif
 
@@ -67,7 +66,6 @@ obj-$(CONFIG_MACH_MACKEREL)	+= board-mackerel.o
 obj-$(CONFIG_MACH_BOCKW)	+= board-bockw.o
 obj-$(CONFIG_MACH_BOCKW_REFERENCE)	+= board-bockw-reference.o
 obj-$(CONFIG_MACH_GENMAI)	+= board-genmai.o
-obj-$(CONFIG_MACH_GENMAI_REFERENCE)	+= board-genmai-reference.o
 obj-$(CONFIG_MACH_MARZEN)	+= board-marzen.o
 obj-$(CONFIG_MACH_MARZEN_REFERENCE)	+= board-marzen-reference.o
 obj-$(CONFIG_MACH_LAGER)	+= board-lager.o
diff --git a/arch/arm/mach-shmobile/Makefile.boot b/arch/arm/mach-shmobile/Makefile.boot
index 99455ecafa0..918fccffa1b 100644
--- a/arch/arm/mach-shmobile/Makefile.boot
+++ b/arch/arm/mach-shmobile/Makefile.boot
@@ -7,7 +7,6 @@ loadaddr-$(CONFIG_MACH_ARMADILLO800EVA_REFERENCE) += 0x40008000
 loadaddr-$(CONFIG_MACH_BOCKW) += 0x60008000
 loadaddr-$(CONFIG_MACH_BOCKW_REFERENCE) += 0x60008000
 loadaddr-$(CONFIG_MACH_GENMAI) += 0x08008000
-loadaddr-$(CONFIG_MACH_GENMAI_REFERENCE) += 0x08008000
 loadaddr-$(CONFIG_MACH_KOELSCH) += 0x40008000
 loadaddr-$(CONFIG_MACH_KZM9G) += 0x41008000
 loadaddr-$(CONFIG_MACH_KZM9G_REFERENCE) += 0x41008000
diff --git a/arch/arm/mach-shmobile/board-armadillo800eva-reference.c b/arch/arm/mach-shmobile/board-armadillo800eva-reference.c
index 57d1a78367b..57d246eb881 100644
--- a/arch/arm/mach-shmobile/board-armadillo800eva-reference.c
+++ b/arch/arm/mach-shmobile/board-armadillo800eva-reference.c
@@ -187,7 +187,7 @@ static const char *eva_boards_compat_dt[] __initdata = {
 
 DT_MACHINE_START(ARMADILLO800EVA_DT, "armadillo800eva-reference")
 	.map_io		= r8a7740_map_io,
-	.init_early	= r8a7740_init_delay,
+	.init_early	= shmobile_init_delay,
 	.init_irq	= r8a7740_init_irq_of,
 	.init_machine	= eva_init,
 	.init_late	= shmobile_init_late,
diff --git a/arch/arm/mach-shmobile/board-armadillo800eva.c b/arch/arm/mach-shmobile/board-armadillo800eva.c
index 2858f380bea..bc2cf7a8953 100644
--- a/arch/arm/mach-shmobile/board-armadillo800eva.c
+++ b/arch/arm/mach-shmobile/board-armadillo800eva.c
@@ -992,6 +992,7 @@ static struct asoc_simple_card_info fsi_wm8978_info = {
 	.platform	= "sh_fsi2",
 	.daifmt		= SND_SOC_DAIFMT_I2S | SND_SOC_DAIFMT_CBM_CFM,
 	.cpu_dai = {
+		.fmt	= SND_SOC_DAIFMT_IB_NF,
 		.name	= "fsia-dai",
 	},
 	.codec_dai = {
@@ -1016,7 +1017,7 @@ static struct asoc_simple_card_info fsi2_hdmi_info = {
 	.platform	= "sh_fsi2",
 	.cpu_dai = {
 		.name	= "fsib-dai",
-		.fmt	= SND_SOC_DAIFMT_CBM_CFM,
+		.fmt	= SND_SOC_DAIFMT_CBS_CFS,
 	},
 	.codec_dai = {
 		.name = "sh_mobile_hdmi-hifi",
@@ -1299,11 +1300,6 @@ static void __init eva_earlytimer_init(void)
 	eva_clock_init();
 }
 
-static void __init eva_add_early_devices(void)
-{
-	r8a7740_add_early_devices();
-}
-
 #define RESCNT2 IOMEM(0xe6188020)
 static void eva_restart(enum reboot_mode mode, const char *cmd)
 {
@@ -1318,7 +1314,7 @@ static const char *eva_boards_compat_dt[] __initdata = {
 
 DT_MACHINE_START(ARMADILLO800EVA_DT, "armadillo800eva")
 	.map_io		= r8a7740_map_io,
-	.init_early	= eva_add_early_devices,
+	.init_early	= r8a7740_add_early_devices,
 	.init_irq	= r8a7740_init_irq_of,
 	.init_machine	= eva_init,
 	.init_late	= shmobile_init_late,
diff --git a/arch/arm/mach-shmobile/board-bockw.c b/arch/arm/mach-shmobile/board-bockw.c
index b4122f8cb8d..f444be2f241 100644
--- a/arch/arm/mach-shmobile/board-bockw.c
+++ b/arch/arm/mach-shmobile/board-bockw.c
@@ -345,24 +345,39 @@ static struct rsnd_ssi_platform_info rsnd_ssi[] = {
 	RSND_SSI_UNUSED, /* SSI 0 */
 	RSND_SSI_UNUSED, /* SSI 1 */
 	RSND_SSI_UNUSED, /* SSI 2 */
-	RSND_SSI_SET(1, HPBDMA_SLAVE_HPBIF3_TX, gic_iid(0x85), RSND_SSI_PLAY),
-	RSND_SSI_SET(2, HPBDMA_SLAVE_HPBIF4_RX, gic_iid(0x85), RSND_SSI_CLK_PIN_SHARE),
-	RSND_SSI_SET(0, HPBDMA_SLAVE_HPBIF5_TX, gic_iid(0x86), RSND_SSI_PLAY),
-	RSND_SSI_SET(0, HPBDMA_SLAVE_HPBIF6_RX, gic_iid(0x86), 0),
-	RSND_SSI_SET(3, HPBDMA_SLAVE_HPBIF7_TX, gic_iid(0x86), RSND_SSI_PLAY),
-	RSND_SSI_SET(4, HPBDMA_SLAVE_HPBIF8_RX, gic_iid(0x86), RSND_SSI_CLK_PIN_SHARE),
+	RSND_SSI(HPBDMA_SLAVE_HPBIF3_TX, gic_iid(0x85), 0),
+	RSND_SSI(HPBDMA_SLAVE_HPBIF4_RX, gic_iid(0x85), RSND_SSI_CLK_PIN_SHARE),
+	RSND_SSI(HPBDMA_SLAVE_HPBIF5_TX, gic_iid(0x86), 0),
+	RSND_SSI(HPBDMA_SLAVE_HPBIF6_RX, gic_iid(0x86), 0),
+	RSND_SSI(HPBDMA_SLAVE_HPBIF7_TX, gic_iid(0x86), 0),
+	RSND_SSI(HPBDMA_SLAVE_HPBIF8_RX, gic_iid(0x86), RSND_SSI_CLK_PIN_SHARE),
 };
 
-static struct rsnd_scu_platform_info rsnd_scu[9] = {
-	{ .flags = 0, }, /* SRU 0 */
-	{ .flags = 0, }, /* SRU 1 */
-	{ .flags = 0, }, /* SRU 2 */
-	{ .flags = RSND_SCU_USE_HPBIF, },
-	{ .flags = RSND_SCU_USE_HPBIF, },
-	{ .flags = RSND_SCU_USE_HPBIF, },
-	{ .flags = RSND_SCU_USE_HPBIF, },
-	{ .flags = RSND_SCU_USE_HPBIF, },
-	{ .flags = RSND_SCU_USE_HPBIF, },
+static struct rsnd_src_platform_info rsnd_src[9] = {
+	RSND_SRC_UNUSED, /* SRU 0 */
+	RSND_SRC_UNUSED, /* SRU 1 */
+	RSND_SRC_UNUSED, /* SRU 2 */
+	RSND_SRC(0, 0),
+	RSND_SRC(0, 0),
+	RSND_SRC(0, 0),
+	RSND_SRC(0, 0),
+	RSND_SRC(0, 0),
+	RSND_SRC(0, 0),
+};
+
+static struct rsnd_dai_platform_info rsnd_dai[] = {
+	{
+		.playback = { .ssi = &rsnd_ssi[5], .src = &rsnd_src[5] },
+		.capture  = { .ssi = &rsnd_ssi[6], .src = &rsnd_src[6] },
+	}, {
+		.playback = { .ssi = &rsnd_ssi[3], .src = &rsnd_src[3] },
+	}, {
+		.capture  = { .ssi = &rsnd_ssi[4], .src = &rsnd_src[4] },
+	}, {
+		.playback = { .ssi = &rsnd_ssi[7], .src = &rsnd_src[7] },
+	}, {
+		.capture  = { .ssi = &rsnd_ssi[8], .src = &rsnd_src[8] },
+	},
 };
 
 enum {
@@ -437,8 +452,10 @@ static struct rcar_snd_info rsnd_info = {
 	.flags		= RSND_GEN1,
 	.ssi_info	= rsnd_ssi,
 	.ssi_info_nr	= ARRAY_SIZE(rsnd_ssi),
-	.scu_info	= rsnd_scu,
-	.scu_info_nr	= ARRAY_SIZE(rsnd_scu),
+	.src_info	= rsnd_src,
+	.src_info_nr	= ARRAY_SIZE(rsnd_src),
+	.dai_info	= rsnd_dai,
+	.dai_info_nr	= ARRAY_SIZE(rsnd_dai),
 	.start		= rsnd_start,
 	.stop		= rsnd_stop,
 };
@@ -591,6 +608,7 @@ static void __init bockw_init(void)
 {
 	void __iomem *base;
 	struct clk *clk;
+	struct platform_device *pdev;
 	int i;
 
 	r8a7778_clock_init();
@@ -673,9 +691,6 @@ static void __init bockw_init(void)
 	}
 
 	/* for Audio */
-	clk = clk_get(NULL, "audio_clk_b");
-	clk_set_rate(clk, 24576000);
-	clk_put(clk);
 	rsnd_codec_power(5, 1); /* enable ak4642 */
 
 	platform_device_register_simple(
@@ -684,11 +699,15 @@ static void __init bockw_init(void)
 	platform_device_register_simple(
 		"ak4554-adc-dac", 1, NULL, 0);
 
-	platform_device_register_resndata(
+	pdev = platform_device_register_resndata(
 		&platform_bus, "rcar_sound", -1,
 		rsnd_resources, ARRAY_SIZE(rsnd_resources),
 		&rsnd_info, sizeof(rsnd_info));
 
+	clk = clk_get(&pdev->dev, "clk_b");
+	clk_set_rate(clk, 24576000);
+	clk_put(clk);
+
 	for (i = 0; i < ARRAY_SIZE(rsnd_card_info); i++) {
 		struct platform_device_info cardinfo = {
 			.parent         = &platform_bus,
diff --git a/arch/arm/mach-shmobile/board-genmai-reference.c b/arch/arm/mach-shmobile/board-genmai-reference.c
index 7630c1053e3..2ff6ad6e608 100644
--- a/arch/arm/mach-shmobile/board-genmai-reference.c
+++ b/arch/arm/mach-shmobile/board-genmai-reference.c
@@ -18,27 +18,31 @@
  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  */
 
-#include <linux/clk-provider.h>
 #include <linux/kernel.h>
 #include <linux/of_platform.h>
+#include <mach/clock.h>
 #include <mach/common.h>
 #include <mach/r7s72100.h>
 #include <asm/mach-types.h>
 #include <asm/mach/arch.h>
 
+/*
+ * This is a really crude hack to provide clkdev support to platform
+ * devices until they get moved to DT.
+ */
+static const struct clk_name clk_names[] = {
+	{ "mtu2", "fck", "sh-mtu2" },
+};
+
 static void __init genmai_add_standard_devices(void)
 {
-#ifdef CONFIG_COMMON_CLK
-	of_clk_init(NULL);
-#else
-	r7s72100_clock_init();
-#endif
+	shmobile_clk_workaround(clk_names, ARRAY_SIZE(clk_names), true);
 	r7s72100_add_dt_devices();
 	of_platform_populate(NULL, of_default_bus_match_table, NULL, NULL);
 }
 
 static const char * const genmai_boards_compat_dt[] __initconst = {
-	"renesas,genmai-reference",
+	"renesas,genmai",
 	NULL,
 };
 
diff --git a/arch/arm/mach-shmobile/board-genmai.c b/arch/arm/mach-shmobile/board-genmai.c
index 6c328d63b81..c94201ee859 100644
--- a/arch/arm/mach-shmobile/board-genmai.c
+++ b/arch/arm/mach-shmobile/board-genmai.c
@@ -21,6 +21,7 @@
 
 #include <linux/kernel.h>
 #include <linux/platform_device.h>
+#include <linux/serial_sci.h>
 #include <linux/sh_eth.h>
 #include <linux/spi/rspi.h>
 #include <linux/spi/spi.h>
@@ -89,6 +90,40 @@ static const struct spi_board_info spi_info[] __initconst = {
 	},
 };
 
+/* SCIF */
+#define R7S72100_SCIF(index, baseaddr, irq)				\
+static const struct plat_sci_port scif##index##_platform_data = {	\
+	.type		= PORT_SCIF,					\
+	.regtype	= SCIx_SH2_SCIF_FIFODATA_REGTYPE,		\
+	.flags		= UPF_BOOT_AUTOCONF | UPF_IOREMAP,		\
+	.scscr		= SCSCR_RIE | SCSCR_TIE | SCSCR_RE | SCSCR_TE |	\
+			  SCSCR_REIE,					\
+};									\
+									\
+static struct resource scif##index##_resources[] = {			\
+	DEFINE_RES_MEM(baseaddr, 0x100),				\
+	DEFINE_RES_IRQ(irq + 1),					\
+	DEFINE_RES_IRQ(irq + 2),					\
+	DEFINE_RES_IRQ(irq + 3),					\
+	DEFINE_RES_IRQ(irq),						\
+}									\
+
+R7S72100_SCIF(0, 0xe8007000, gic_iid(221));
+R7S72100_SCIF(1, 0xe8007800, gic_iid(225));
+R7S72100_SCIF(2, 0xe8008000, gic_iid(229));
+R7S72100_SCIF(3, 0xe8008800, gic_iid(233));
+R7S72100_SCIF(4, 0xe8009000, gic_iid(237));
+R7S72100_SCIF(5, 0xe8009800, gic_iid(241));
+R7S72100_SCIF(6, 0xe800a000, gic_iid(245));
+R7S72100_SCIF(7, 0xe800a800, gic_iid(249));
+
+#define r7s72100_register_scif(index)					       \
+	platform_device_register_resndata(&platform_bus, "sh-sci", index,      \
+					  scif##index##_resources,	       \
+					  ARRAY_SIZE(scif##index##_resources), \
+					  &scif##index##_platform_data,	       \
+					  sizeof(scif##index##_platform_data))
+
 static void __init genmai_add_standard_devices(void)
 {
 	r7s72100_clock_init();
@@ -102,6 +137,15 @@ static void __init genmai_add_standard_devices(void)
 	r7s72100_register_rspi(3);
 	r7s72100_register_rspi(4);
 	spi_register_board_info(spi_info, ARRAY_SIZE(spi_info));
+
+	r7s72100_register_scif(0);
+	r7s72100_register_scif(1);
+	r7s72100_register_scif(2);
+	r7s72100_register_scif(3);
+	r7s72100_register_scif(4);
+	r7s72100_register_scif(5);
+	r7s72100_register_scif(6);
+	r7s72100_register_scif(7);
 }
 
 static const char * const genmai_boards_compat_dt[] __initconst = {
diff --git a/arch/arm/mach-shmobile/board-koelsch-reference.c b/arch/arm/mach-shmobile/board-koelsch-reference.c
index a3fd30242bd..d322a162b4b 100644
--- a/arch/arm/mach-shmobile/board-koelsch-reference.c
+++ b/arch/arm/mach-shmobile/board-koelsch-reference.c
@@ -19,12 +19,11 @@
  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  */
 
-#include <linux/clk.h>
-#include <linux/clkdev.h>
 #include <linux/dma-mapping.h>
 #include <linux/kernel.h>
 #include <linux/of_platform.h>
 #include <linux/platform_data/rcar-du.h>
+#include <mach/clock.h>
 #include <mach/common.h>
 #include <mach/irqs.h>
 #include <mach/rcar-gen2.h>
@@ -82,49 +81,35 @@ static void __init koelsch_add_du_device(void)
 	platform_device_register_full(&info);
 }
 
-static void __init koelsch_add_standard_devices(void)
-{
-	/*
-	 * This is a really crude hack to provide clkdev support to the CMT and
-	 * DU devices until they get moved to DT.
-	 */
-	static const struct clk_name {
-		const char *clk;
-		const char *con_id;
-		const char *dev_id;
-	} clk_names[] = {
-		{ "cmt0", NULL, "sh_cmt.0" },
-		{ "scifa0", NULL, "sh-sci.0" },
-		{ "scifa1", NULL, "sh-sci.1" },
-		{ "scifb0", NULL, "sh-sci.2" },
-		{ "scifb1", NULL, "sh-sci.3" },
-		{ "scifb2", NULL, "sh-sci.4" },
-		{ "scifa2", NULL, "sh-sci.5" },
-		{ "scif0", NULL, "sh-sci.6" },
-		{ "scif1", NULL, "sh-sci.7" },
-		{ "scif2", NULL, "sh-sci.8" },
-		{ "scif3", NULL, "sh-sci.9" },
-		{ "scif4", NULL, "sh-sci.10" },
-		{ "scif5", NULL, "sh-sci.11" },
-		{ "scifa3", NULL, "sh-sci.12" },
-		{ "scifa4", NULL, "sh-sci.13" },
-		{ "scifa5", NULL, "sh-sci.14" },
-		{ "du0", "du.0", "rcar-du-r8a7791" },
-		{ "du1", "du.1", "rcar-du-r8a7791" },
-		{ "lvds0", "lvds.0", "rcar-du-r8a7791" },
-	};
-	struct clk *clk;
-	unsigned int i;
+/*
+ * This is a really crude hack to provide clkdev support to platform
+ * devices until they get moved to DT.
+ */
+static const struct clk_name clk_names[] __initconst = {
+	{ "cmt0", "fck", "sh-cmt-48-gen2.0" },
+	{ "du0", "du.0", "rcar-du-r8a7791" },
+	{ "du1", "du.1", "rcar-du-r8a7791" },
+	{ "lvds0", "lvds.0", "rcar-du-r8a7791" },
+};
 
-	for (i = 0; i < ARRAY_SIZE(clk_names); ++i) {
-		clk = clk_get(NULL, clk_names[i].clk);
-		if (!IS_ERR(clk)) {
-			clk_register_clkdev(clk, clk_names[i].con_id,
-					    clk_names[i].dev_id);
-			clk_put(clk);
-		}
-	}
+/*
+ * This is a really crude hack to work around core platform clock issues
+ */
+static const struct clk_name clk_enables[] __initconst = {
+	{ "ether", NULL, "ee700000.ethernet" },
+	{ "i2c2", NULL, "e6530000.i2c" },
+	{ "msiof0", NULL, "e6e20000.spi" },
+	{ "qspi_mod", NULL, "e6b10000.spi" },
+	{ "sdhi0", NULL, "ee100000.sd" },
+	{ "sdhi1", NULL, "ee140000.sd" },
+	{ "sdhi2", NULL, "ee160000.sd" },
+	{ "thermal", NULL, "e61f0000.thermal" },
+};
 
+static void __init koelsch_add_standard_devices(void)
+{
+	shmobile_clk_workaround(clk_names, ARRAY_SIZE(clk_names), false);
+	shmobile_clk_workaround(clk_enables, ARRAY_SIZE(clk_enables), true);
 	r8a7791_add_dt_devices();
 	of_platform_populate(NULL, of_default_bus_match_table, NULL, NULL);
 
@@ -139,7 +124,7 @@ static const char * const koelsch_boards_compat_dt[] __initconst = {
 
 DT_MACHINE_START(KOELSCH_DT, "koelsch")
 	.smp		= smp_ops(r8a7791_smp_ops),
-	.init_early	= r8a7791_init_early,
+	.init_early	= shmobile_init_delay,
 	.init_time	= rcar_gen2_timer_init,
 	.init_machine	= koelsch_add_standard_devices,
 	.init_late	= shmobile_init_late,
diff --git a/arch/arm/mach-shmobile/board-koelsch.c b/arch/arm/mach-shmobile/board-koelsch.c
index 5a034ff405d..c6c68892caa 100644
--- a/arch/arm/mach-shmobile/board-koelsch.c
+++ b/arch/arm/mach-shmobile/board-koelsch.c
@@ -216,7 +216,7 @@ static const struct spi_board_info spi_info[] __initconst = {
 	{
 		.modalias	= "m25p80",
 		.platform_data	= &spi_flash_data,
-		.mode		= SPI_MODE_0,
+		.mode		= SPI_MODE_0 | SPI_TX_QUAD | SPI_RX_QUAD,
 		.max_speed_hz	= 30000000,
 		.bus_num	= 0,
 		.chip_select	= 0,
@@ -522,7 +522,7 @@ static const char * const koelsch_boards_compat_dt[] __initconst = {
 
 DT_MACHINE_START(KOELSCH_DT, "koelsch")
 	.smp		= smp_ops(r8a7791_smp_ops),
-	.init_early	= r8a7791_init_early,
+	.init_early	= shmobile_init_delay,
 	.init_time	= rcar_gen2_timer_init,
 	.init_machine	= koelsch_init,
 	.init_late	= shmobile_init_late,
diff --git a/arch/arm/mach-shmobile/board-lager-reference.c b/arch/arm/mach-shmobile/board-lager-reference.c
index 440aac36d69..749832e3f33 100644
--- a/arch/arm/mach-shmobile/board-lager-reference.c
+++ b/arch/arm/mach-shmobile/board-lager-reference.c
@@ -18,12 +18,11 @@
  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  */
 
-#include <linux/clk.h>
-#include <linux/clkdev.h>
 #include <linux/dma-mapping.h>
 #include <linux/init.h>
 #include <linux/of_platform.h>
 #include <linux/platform_data/rcar-du.h>
+#include <mach/clock.h>
 #include <mach/common.h>
 #include <mach/irqs.h>
 #include <mach/rcar-gen2.h>
@@ -86,46 +85,36 @@ static void __init lager_add_du_device(void)
 	platform_device_register_full(&info);
 }
 
-static void __init lager_add_standard_devices(void)
-{
-	/*
-	 * This is a really crude hack to provide clkdev support to platform
-	 * devices until they get moved to DT.
-	 */
-	static const struct clk_name {
-		const char *clk;
-		const char *con_id;
-		const char *dev_id;
-	} clk_names[] = {
-		{ "cmt0", NULL, "sh_cmt.0" },
-		{ "scifa0", NULL, "sh-sci.0" },
-		{ "scifa1", NULL, "sh-sci.1" },
-		{ "scifb0", NULL, "sh-sci.2" },
-		{ "scifb1", NULL, "sh-sci.3" },
-		{ "scifb2", NULL, "sh-sci.4" },
-		{ "scifa2", NULL, "sh-sci.5" },
-		{ "scif0", NULL, "sh-sci.6" },
-		{ "scif1", NULL, "sh-sci.7" },
-		{ "hscif0", NULL, "sh-sci.8" },
-		{ "hscif1", NULL, "sh-sci.9" },
-		{ "du0", "du.0", "rcar-du-r8a7790" },
-		{ "du1", "du.1", "rcar-du-r8a7790" },
-		{ "du2", "du.2", "rcar-du-r8a7790" },
-		{ "lvds0", "lvds.0", "rcar-du-r8a7790" },
-		{ "lvds1", "lvds.1", "rcar-du-r8a7790" },
-	};
-	struct clk *clk;
-	unsigned int i;
+/*
+ * This is a really crude hack to provide clkdev support to platform
+ * devices until they get moved to DT.
+ */
+static const struct clk_name clk_names[] __initconst = {
+	{ "cmt0", "fck", "sh-cmt-48-gen2.0" },
+	{ "du0", "du.0", "rcar-du-r8a7790" },
+	{ "du1", "du.1", "rcar-du-r8a7790" },
+	{ "du2", "du.2", "rcar-du-r8a7790" },
+	{ "lvds0", "lvds.0", "rcar-du-r8a7790" },
+	{ "lvds1", "lvds.1", "rcar-du-r8a7790" },
+};
 
-	for (i = 0; i < ARRAY_SIZE(clk_names); ++i) {
-		clk = clk_get(NULL, clk_names[i].clk);
-		if (!IS_ERR(clk)) {
-			clk_register_clkdev(clk, clk_names[i].con_id,
-					    clk_names[i].dev_id);
-			clk_put(clk);
-		}
-	}
+/*
+ * This is a really crude hack to work around core platform clock issues
+ */
+static const struct clk_name clk_enables[] __initconst = {
+	{ "ether", NULL, "ee700000.ethernet" },
+	{ "msiof1", NULL, "e6e10000.spi" },
+	{ "mmcif1", NULL, "ee220000.mmc" },
+	{ "qspi_mod", NULL, "e6b10000.spi" },
+	{ "sdhi0", NULL, "ee100000.sd" },
+	{ "sdhi2", NULL, "ee140000.sd" },
+	{ "thermal", NULL, "e61f0000.thermal" },
+};
 
+static void __init lager_add_standard_devices(void)
+{
+	shmobile_clk_workaround(clk_names, ARRAY_SIZE(clk_names), false);
+	shmobile_clk_workaround(clk_enables, ARRAY_SIZE(clk_enables), true);
 	r8a7790_add_dt_devices();
 	of_platform_populate(NULL, of_default_bus_match_table, NULL, NULL);
 
diff --git a/arch/arm/mach-shmobile/board-lager.c b/arch/arm/mach-shmobile/board-lager.c
index f0104bfe544..f8b1e05463c 100644
--- a/arch/arm/mach-shmobile/board-lager.c
+++ b/arch/arm/mach-shmobile/board-lager.c
@@ -325,12 +325,12 @@ static const struct rspi_plat_data qspi_pdata __initconst = {
 
 static const struct spi_board_info spi_info[] __initconst = {
 	{
-		.modalias               = "m25p80",
-		.platform_data          = &spi_flash_data,
-		.mode                   = SPI_MODE_0,
-		.max_speed_hz           = 30000000,
-		.bus_num                = 0,
-		.chip_select            = 0,
+		.modalias	= "m25p80",
+		.platform_data	= &spi_flash_data,
+		.mode		= SPI_MODE_0 | SPI_TX_QUAD | SPI_RX_QUAD,
+		.max_speed_hz	= 30000000,
+		.bus_num	= 0,
+		.chip_select	= 0,
 	},
 };
 
@@ -567,20 +567,27 @@ static struct resource rsnd_resources[] __initdata = {
 };
 
 static struct rsnd_ssi_platform_info rsnd_ssi[] = {
-	RSND_SSI_SET(0, 0, gic_spi(370), RSND_SSI_PLAY),
-	RSND_SSI_SET(0, 0, gic_spi(371), RSND_SSI_CLK_PIN_SHARE),
+	RSND_SSI(0, gic_spi(370), 0),
+	RSND_SSI(0, gic_spi(371), RSND_SSI_CLK_PIN_SHARE),
 };
 
-static struct rsnd_scu_platform_info rsnd_scu[2] = {
+static struct rsnd_src_platform_info rsnd_src[2] = {
 	/* no member at this point */
 };
 
+static struct rsnd_dai_platform_info rsnd_dai = {
+	.playback = { .ssi = &rsnd_ssi[0], },
+	.capture  = { .ssi = &rsnd_ssi[1], },
+};
+
 static struct rcar_snd_info rsnd_info = {
 	.flags		= RSND_GEN2,
 	.ssi_info	= rsnd_ssi,
 	.ssi_info_nr	= ARRAY_SIZE(rsnd_ssi),
-	.scu_info	= rsnd_scu,
-	.scu_info_nr	= ARRAY_SIZE(rsnd_scu),
+	.src_info	= rsnd_src,
+	.src_info_nr	= ARRAY_SIZE(rsnd_src),
+	.dai_info	= &rsnd_dai,
+	.dai_info_nr	= 1,
 };
 
 static struct asoc_simple_card_info rsnd_card_info = {
@@ -588,14 +595,12 @@ static struct asoc_simple_card_info rsnd_card_info = {
 	.card		= "SSI01-AK4643",
 	.codec		= "ak4642-codec.2-0012",
 	.platform	= "rcar_sound",
-	.daifmt		= SND_SOC_DAIFMT_LEFT_J,
+	.daifmt		= SND_SOC_DAIFMT_LEFT_J | SND_SOC_DAIFMT_CBM_CFM,
 	.cpu_dai = {
 		.name	= "rcar_sound",
-		.fmt	= SND_SOC_DAIFMT_CBS_CFS,
 	},
 	.codec_dai = {
 		.name	= "ak4642-hifi",
-		.fmt	= SND_SOC_DAIFMT_CBM_CFM,
 		.sysclk	= 11289600,
 	},
 };
diff --git a/arch/arm/mach-shmobile/clock-emev2.c b/arch/arm/mach-shmobile/clock-emev2.c
deleted file mode 100644
index 5ac13ba71d5..00000000000
--- a/arch/arm/mach-shmobile/clock-emev2.c
+++ /dev/null
@@ -1,231 +0,0 @@
-/*
- * Emma Mobile EV2 clock framework support
- *
- * Copyright (C) 2012  Magnus Damm
- *
- * 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.
- *
- * 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/init.h>
-#include <linux/kernel.h>
-#include <linux/io.h>
-#include <linux/sh_clk.h>
-#include <linux/clkdev.h>
-#include <mach/common.h>
-
-#define EMEV2_SMU_BASE 0xe0110000
-
-/* EMEV2 SMU registers */
-#define USIAU0_RSTCTRL 0x094
-#define USIBU1_RSTCTRL 0x0ac
-#define USIBU2_RSTCTRL 0x0b0
-#define USIBU3_RSTCTRL 0x0b4
-#define STI_RSTCTRL 0x124
-#define USIAU0GCLKCTRL 0x4a0
-#define USIBU1GCLKCTRL 0x4b8
-#define USIBU2GCLKCTRL 0x4bc
-#define USIBU3GCLKCTRL 0x04c0
-#define STIGCLKCTRL 0x528
-#define USIAU0SCLKDIV 0x61c
-#define USIB2SCLKDIV 0x65c
-#define USIB3SCLKDIV 0x660
-#define STI_CLKSEL 0x688
-
-/* not pretty, but hey */
-static void __iomem *smu_base;
-
-static void emev2_smu_write(unsigned long value, int offs)
-{
-	BUG_ON(!smu_base || (offs >= PAGE_SIZE));
-	iowrite32(value, smu_base + offs);
-}
-
-static struct clk_mapping smu_mapping = {
-	.phys	= EMEV2_SMU_BASE,
-	.len	= PAGE_SIZE,
-};
-
-/* Fixed 32 KHz root clock from C32K pin */
-static struct clk c32k_clk = {
-	.rate           = 32768,
-	.mapping	= &smu_mapping,
-};
-
-/* PLL3 multiplies C32K with 7000 */
-static unsigned long pll3_recalc(struct clk *clk)
-{
-	return clk->parent->rate * 7000;
-}
-
-static struct sh_clk_ops pll3_clk_ops = {
-	.recalc		= pll3_recalc,
-};
-
-static struct clk pll3_clk = {
-	.ops		= &pll3_clk_ops,
-	.parent		= &c32k_clk,
-};
-
-static struct clk *main_clks[] = {
-	&c32k_clk,
-	&pll3_clk,
-};
-
-enum { SCLKDIV_USIAU0, SCLKDIV_USIBU2, SCLKDIV_USIBU1, SCLKDIV_USIBU3,
-	SCLKDIV_NR };
-
-#define SCLKDIV(_reg, _shift)			\
-{								\
-	.parent		= &pll3_clk,				\
-	.enable_reg	= IOMEM(EMEV2_SMU_BASE + (_reg)),	\
-	.enable_bit	= _shift,				\
-}
-
-static struct clk sclkdiv_clks[SCLKDIV_NR] = {
-	[SCLKDIV_USIAU0] = SCLKDIV(USIAU0SCLKDIV, 0),
-	[SCLKDIV_USIBU2] = SCLKDIV(USIB2SCLKDIV, 16),
-	[SCLKDIV_USIBU1] = SCLKDIV(USIB2SCLKDIV, 0),
-	[SCLKDIV_USIBU3] = SCLKDIV(USIB3SCLKDIV, 0),
-};
-
-enum { GCLK_USIAU0_SCLK, GCLK_USIBU1_SCLK, GCLK_USIBU2_SCLK, GCLK_USIBU3_SCLK,
-	GCLK_STI_SCLK,
-	GCLK_NR };
-
-#define GCLK_SCLK(_parent, _reg) \
-{								\
-	.parent		= _parent,				\
-	.enable_reg	= IOMEM(EMEV2_SMU_BASE + (_reg)),	\
-	.enable_bit	= 1, /* SCLK_GCC */			\
-}
-
-static struct clk gclk_clks[GCLK_NR] = {
-	[GCLK_USIAU0_SCLK] = GCLK_SCLK(&sclkdiv_clks[SCLKDIV_USIAU0],
-				       USIAU0GCLKCTRL),
-	[GCLK_USIBU1_SCLK] = GCLK_SCLK(&sclkdiv_clks[SCLKDIV_USIBU1],
-				       USIBU1GCLKCTRL),
-	[GCLK_USIBU2_SCLK] = GCLK_SCLK(&sclkdiv_clks[SCLKDIV_USIBU2],
-				       USIBU2GCLKCTRL),
-	[GCLK_USIBU3_SCLK] = GCLK_SCLK(&sclkdiv_clks[SCLKDIV_USIBU3],
-				       USIBU3GCLKCTRL),
-	[GCLK_STI_SCLK] = GCLK_SCLK(&c32k_clk, STIGCLKCTRL),
-};
-
-static int emev2_gclk_enable(struct clk *clk)
-{
-	iowrite32(ioread32(clk->mapped_reg) | (1 << clk->enable_bit),
-		  clk->mapped_reg);
-	return 0;
-}
-
-static void emev2_gclk_disable(struct clk *clk)
-{
-	iowrite32(ioread32(clk->mapped_reg) & ~(1 << clk->enable_bit),
-		  clk->mapped_reg);
-}
-
-static struct sh_clk_ops emev2_gclk_clk_ops = {
-	.enable		= emev2_gclk_enable,
-	.disable	= emev2_gclk_disable,
-	.recalc		= followparent_recalc,
-};
-
-static int __init emev2_gclk_register(struct clk *clks, int nr)
-{
-	struct clk *clkp;
-	int ret = 0;
-	int k;
-
-	for (k = 0; !ret && (k < nr); k++) {
-		clkp = clks + k;
-		clkp->ops = &emev2_gclk_clk_ops;
-		ret |= clk_register(clkp);
-	}
-
-	return ret;
-}
-
-static unsigned long emev2_sclkdiv_recalc(struct clk *clk)
-{
-	unsigned int sclk_div;
-
-	sclk_div = (ioread32(clk->mapped_reg) >> clk->enable_bit) & 0xff;
-
-	return clk->parent->rate / (sclk_div + 1);
-}
-
-static struct sh_clk_ops emev2_sclkdiv_clk_ops = {
-	.recalc		= emev2_sclkdiv_recalc,
-};
-
-static int __init emev2_sclkdiv_register(struct clk *clks, int nr)
-{
-	struct clk *clkp;
-	int ret = 0;
-	int k;
-
-	for (k = 0; !ret && (k < nr); k++) {
-		clkp = clks + k;
-		clkp->ops = &emev2_sclkdiv_clk_ops;
-		ret |= clk_register(clkp);
-	}
-
-	return ret;
-}
-
-static struct clk_lookup lookups[] = {
-	CLKDEV_DEV_ID("serial8250-em.0", &gclk_clks[GCLK_USIAU0_SCLK]),
-	CLKDEV_DEV_ID("e1020000.uart", &gclk_clks[GCLK_USIAU0_SCLK]),
-	CLKDEV_DEV_ID("serial8250-em.1", &gclk_clks[GCLK_USIBU1_SCLK]),
-	CLKDEV_DEV_ID("e1030000.uart", &gclk_clks[GCLK_USIBU1_SCLK]),
-	CLKDEV_DEV_ID("serial8250-em.2", &gclk_clks[GCLK_USIBU2_SCLK]),
-	CLKDEV_DEV_ID("e1040000.uart", &gclk_clks[GCLK_USIBU2_SCLK]),
-	CLKDEV_DEV_ID("serial8250-em.3", &gclk_clks[GCLK_USIBU3_SCLK]),
-	CLKDEV_DEV_ID("e1050000.uart", &gclk_clks[GCLK_USIBU3_SCLK]),
-	CLKDEV_DEV_ID("em_sti.0", &gclk_clks[GCLK_STI_SCLK]),
-	CLKDEV_DEV_ID("e0180000.sti", &gclk_clks[GCLK_STI_SCLK]),
-};
-
-void __init emev2_clock_init(void)
-{
-	int k, ret = 0;
-
-	smu_base = ioremap(EMEV2_SMU_BASE, PAGE_SIZE);
-	BUG_ON(!smu_base);
-
-	/* setup STI timer to run on 32.768 kHz and deassert reset */
-	emev2_smu_write(0, STI_CLKSEL);
-	emev2_smu_write(1, STI_RSTCTRL);
-
-	/* deassert reset for UART0->UART3 */
-	emev2_smu_write(2, USIAU0_RSTCTRL);
-	emev2_smu_write(2, USIBU1_RSTCTRL);
-	emev2_smu_write(2, USIBU2_RSTCTRL);
-	emev2_smu_write(2, USIBU3_RSTCTRL);
-
-	for (k = 0; !ret && (k < ARRAY_SIZE(main_clks)); k++)
-		ret = clk_register(main_clks[k]);
-
-	if (!ret)
-		ret = emev2_sclkdiv_register(sclkdiv_clks, SCLKDIV_NR);
-
-	if (!ret)
-		ret = emev2_gclk_register(gclk_clks, GCLK_NR);
-
-	clkdev_add_table(lookups, ARRAY_SIZE(lookups));
-
-	if (!ret)
-		shmobile_clk_init();
-	else
-		panic("failed to setup emev2 clocks\n");
-}
diff --git a/arch/arm/mach-shmobile/clock-r7s72100.c b/arch/arm/mach-shmobile/clock-r7s72100.c
index bee0073c9b6..df187484de5 100644
--- a/arch/arm/mach-shmobile/clock-r7s72100.c
+++ b/arch/arm/mach-shmobile/clock-r7s72100.c
@@ -194,17 +194,7 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_DEV_ID("rspi-rz.2", &mstp_clks[MSTP105]),
 	CLKDEV_DEV_ID("rspi-rz.3", &mstp_clks[MSTP104]),
 	CLKDEV_DEV_ID("rspi-rz.4", &mstp_clks[MSTP103]),
-	CLKDEV_DEV_ID("e800c800.spi", &mstp_clks[MSTP107]),
-	CLKDEV_DEV_ID("e800d000.spi", &mstp_clks[MSTP106]),
-	CLKDEV_DEV_ID("e800d800.spi", &mstp_clks[MSTP105]),
-	CLKDEV_DEV_ID("e800e000.spi", &mstp_clks[MSTP104]),
-	CLKDEV_DEV_ID("e800e800.spi", &mstp_clks[MSTP103]),
-	CLKDEV_DEV_ID("fcfee000.i2c", &mstp_clks[MSTP97]),
-	CLKDEV_DEV_ID("fcfee400.i2c", &mstp_clks[MSTP96]),
-	CLKDEV_DEV_ID("fcfee800.i2c", &mstp_clks[MSTP95]),
-	CLKDEV_DEV_ID("fcfeec00.i2c", &mstp_clks[MSTP94]),
 	CLKDEV_DEV_ID("r7s72100-ether", &mstp_clks[MSTP74]),
-	CLKDEV_CON_ID("mtu2_fck", &mstp_clks[MSTP33]),
 
 	/* ICK */
 	CLKDEV_ICK_ID("sci_fck", "sh-sci.0", &mstp_clks[MSTP47]),
@@ -215,6 +205,7 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_ICK_ID("sci_fck", "sh-sci.5", &mstp_clks[MSTP42]),
 	CLKDEV_ICK_ID("sci_fck", "sh-sci.6", &mstp_clks[MSTP41]),
 	CLKDEV_ICK_ID("sci_fck", "sh-sci.7", &mstp_clks[MSTP40]),
+	CLKDEV_ICK_ID("fck", "sh-mtu2", &mstp_clks[MSTP33]),
 };
 
 void __init r7s72100_clock_init(void)
diff --git a/arch/arm/mach-shmobile/clock-r8a73a4.c b/arch/arm/mach-shmobile/clock-r8a73a4.c
index 7348d58f500..b5bc22c6a85 100644
--- a/arch/arm/mach-shmobile/clock-r8a73a4.c
+++ b/arch/arm/mach-shmobile/clock-r8a73a4.c
@@ -597,7 +597,7 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_DEV_ID("e6560000.i2c", &mstp_clks[MSTP317]),
 	CLKDEV_DEV_ID("e6500000.i2c", &mstp_clks[MSTP318]),
 	CLKDEV_DEV_ID("e6510000.i2c", &mstp_clks[MSTP323]),
-	CLKDEV_DEV_ID("sh_cmt.10", &mstp_clks[MSTP329]),
+	CLKDEV_ICK_ID("fck", "sh-cmt-48-gen2.1", &mstp_clks[MSTP329]),
 	CLKDEV_DEV_ID("e60b0000.i2c", &mstp_clks[MSTP409]),
 	CLKDEV_DEV_ID("e6540000.i2c", &mstp_clks[MSTP410]),
 	CLKDEV_DEV_ID("e6530000.i2c", &mstp_clks[MSTP411]),
diff --git a/arch/arm/mach-shmobile/clock-r8a7740.c b/arch/arm/mach-shmobile/clock-r8a7740.c
index dd989f93498..50931e3c97c 100644
--- a/arch/arm/mach-shmobile/clock-r8a7740.c
+++ b/arch/arm/mach-shmobile/clock-r8a7740.c
@@ -548,15 +548,9 @@ static struct clk_lookup lookups[] = {
 
 	/* MSTP32 clocks */
 	CLKDEV_DEV_ID("sh_mobile_lcdc_fb.0",	&mstp_clks[MSTP100]),
-	CLKDEV_DEV_ID("sh_tmu.3",		&mstp_clks[MSTP111]),
-	CLKDEV_DEV_ID("sh_tmu.4",		&mstp_clks[MSTP111]),
-	CLKDEV_DEV_ID("sh_tmu.5",		&mstp_clks[MSTP111]),
 	CLKDEV_DEV_ID("i2c-sh_mobile.0",	&mstp_clks[MSTP116]),
 	CLKDEV_DEV_ID("fff20000.i2c",		&mstp_clks[MSTP116]),
 	CLKDEV_DEV_ID("sh_mobile_lcdc_fb.1",	&mstp_clks[MSTP117]),
-	CLKDEV_DEV_ID("sh_tmu.0",		&mstp_clks[MSTP125]),
-	CLKDEV_DEV_ID("sh_tmu.1",		&mstp_clks[MSTP125]),
-	CLKDEV_DEV_ID("sh_tmu.2",		&mstp_clks[MSTP125]),
 	CLKDEV_DEV_ID("sh_mobile_ceu.0",	&mstp_clks[MSTP127]),
 	CLKDEV_DEV_ID("sh_mobile_ceu.1",	&mstp_clks[MSTP128]),
 
@@ -583,7 +577,6 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_DEV_ID("sh-sci.6",		&mstp_clks[MSTP230]),
 	CLKDEV_DEV_ID("e6cc0000.sci",		&mstp_clks[MSTP230]),
 
-	CLKDEV_DEV_ID("sh_cmt.10",		&mstp_clks[MSTP329]),
 	CLKDEV_DEV_ID("sh_fsi2",		&mstp_clks[MSTP328]),
 	CLKDEV_DEV_ID("fe1f0000.sound",		&mstp_clks[MSTP328]),
 	CLKDEV_DEV_ID("i2c-sh_mobile.1",	&mstp_clks[MSTP323]),
@@ -596,7 +589,7 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_DEV_ID("sh_mmcif",		&mstp_clks[MSTP312]),
 	CLKDEV_DEV_ID("e6bd0000.mmc",		&mstp_clks[MSTP312]),
 	CLKDEV_DEV_ID("r8a7740-gether",		&mstp_clks[MSTP309]),
-	CLKDEV_DEV_ID("e9a00000.sh-eth",	&mstp_clks[MSTP309]),
+	CLKDEV_DEV_ID("e9a00000.ethernet",	&mstp_clks[MSTP309]),
 	CLKDEV_DEV_ID("renesas-tpu-pwm",	&mstp_clks[MSTP304]),
 	CLKDEV_DEV_ID("e6600000.pwm",		&mstp_clks[MSTP304]),
 
@@ -604,6 +597,9 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_DEV_ID("e6870000.sd",		&mstp_clks[MSTP415]),
 
 	/* ICK */
+	CLKDEV_ICK_ID("fck",	"sh-tmu.1",		&mstp_clks[MSTP111]),
+	CLKDEV_ICK_ID("fck",	"sh-tmu.0",		&mstp_clks[MSTP125]),
+	CLKDEV_ICK_ID("fck",	"sh-cmt-48.1",		&mstp_clks[MSTP329]),
 	CLKDEV_ICK_ID("host",	"renesas_usbhs",	&mstp_clks[MSTP416]),
 	CLKDEV_ICK_ID("func",	"renesas_usbhs",	&mstp_clks[MSTP407]),
 	CLKDEV_ICK_ID("phy",	"renesas_usbhs",	&mstp_clks[MSTP406]),
diff --git a/arch/arm/mach-shmobile/clock-r8a7778.c b/arch/arm/mach-shmobile/clock-r8a7778.c
index 2009a9bc635..13f8f3ab884 100644
--- a/arch/arm/mach-shmobile/clock-r8a7778.c
+++ b/arch/arm/mach-shmobile/clock-r8a7778.c
@@ -170,15 +170,11 @@ static struct clk mstp_clks[MSTP_NR] = {
 	[MSTP010] = SH_CLK_MSTP32(&p_clk, MSTPCR0, 10, 0), /* SSI2 */
 	[MSTP009] = SH_CLK_MSTP32(&p_clk, MSTPCR0,  9, 0), /* SSI3 */
 	[MSTP008] = SH_CLK_MSTP32(&p_clk, MSTPCR0,  8, 0), /* SRU */
-	[MSTP007] = SH_CLK_MSTP32(&p_clk, MSTPCR0,  7, 0), /* HSPI */
+	[MSTP007] = SH_CLK_MSTP32(&s_clk, MSTPCR0,  7, 0), /* HSPI */
 };
 
 static struct clk_lookup lookups[] = {
 	/* main */
-	CLKDEV_CON_ID("audio_clk_a",	&audio_clk_a),
-	CLKDEV_CON_ID("audio_clk_b",	&audio_clk_b),
-	CLKDEV_CON_ID("audio_clk_c",	&audio_clk_c),
-	CLKDEV_CON_ID("audio_clk_internal",	&s1_clk),
 	CLKDEV_CON_ID("shyway_clk",	&s_clk),
 	CLKDEV_CON_ID("peripheral_clk",	&p_clk),
 
@@ -211,8 +207,6 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_DEV_ID("sh-sci.3", &mstp_clks[MSTP023]), /* SCIF3 */
 	CLKDEV_DEV_ID("sh-sci.4", &mstp_clks[MSTP022]), /* SCIF4 */
 	CLKDEV_DEV_ID("sh-sci.5", &mstp_clks[MSTP021]), /* SCIF6 */
-	CLKDEV_DEV_ID("sh_tmu.0", &mstp_clks[MSTP016]), /* TMU00 */
-	CLKDEV_DEV_ID("sh_tmu.1", &mstp_clks[MSTP015]), /* TMU01 */
 	CLKDEV_DEV_ID("sh-hspi.0", &mstp_clks[MSTP007]), /* HSPI0 */
 	CLKDEV_DEV_ID("fffc7000.spi", &mstp_clks[MSTP007]), /* HSPI0 */
 	CLKDEV_DEV_ID("sh-hspi.1", &mstp_clks[MSTP007]), /* HSPI1 */
@@ -234,15 +228,17 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_ICK_ID("ssi.6", "rcar_sound", &mstp_clks[MSTP309]),
 	CLKDEV_ICK_ID("ssi.7", "rcar_sound", &mstp_clks[MSTP308]),
 	CLKDEV_ICK_ID("ssi.8", "rcar_sound", &mstp_clks[MSTP307]),
-	CLKDEV_ICK_ID("scu.0", "rcar_sound", &mstp_clks[MSTP531]),
-	CLKDEV_ICK_ID("scu.1", "rcar_sound", &mstp_clks[MSTP530]),
-	CLKDEV_ICK_ID("scu.2", "rcar_sound", &mstp_clks[MSTP529]),
-	CLKDEV_ICK_ID("scu.3", "rcar_sound", &mstp_clks[MSTP528]),
-	CLKDEV_ICK_ID("scu.4", "rcar_sound", &mstp_clks[MSTP527]),
-	CLKDEV_ICK_ID("scu.5", "rcar_sound", &mstp_clks[MSTP526]),
-	CLKDEV_ICK_ID("scu.6", "rcar_sound", &mstp_clks[MSTP525]),
-	CLKDEV_ICK_ID("scu.7", "rcar_sound", &mstp_clks[MSTP524]),
-	CLKDEV_ICK_ID("scu.8", "rcar_sound", &mstp_clks[MSTP523]),
+	CLKDEV_ICK_ID("src.0", "rcar_sound", &mstp_clks[MSTP531]),
+	CLKDEV_ICK_ID("src.1", "rcar_sound", &mstp_clks[MSTP530]),
+	CLKDEV_ICK_ID("src.2", "rcar_sound", &mstp_clks[MSTP529]),
+	CLKDEV_ICK_ID("src.3", "rcar_sound", &mstp_clks[MSTP528]),
+	CLKDEV_ICK_ID("src.4", "rcar_sound", &mstp_clks[MSTP527]),
+	CLKDEV_ICK_ID("src.5", "rcar_sound", &mstp_clks[MSTP526]),
+	CLKDEV_ICK_ID("src.6", "rcar_sound", &mstp_clks[MSTP525]),
+	CLKDEV_ICK_ID("src.7", "rcar_sound", &mstp_clks[MSTP524]),
+	CLKDEV_ICK_ID("src.8", "rcar_sound", &mstp_clks[MSTP523]),
+	CLKDEV_ICK_ID("fck", "sh-tmu.0", &mstp_clks[MSTP016]),
+	CLKDEV_ICK_ID("fck", "sh-tmu.1", &mstp_clks[MSTP015]),
 };
 
 void __init r8a7778_clock_init(void)
diff --git a/arch/arm/mach-shmobile/clock-r8a7779.c b/arch/arm/mach-shmobile/clock-r8a7779.c
index 8e403ae0c7b..a13298bd37a 100644
--- a/arch/arm/mach-shmobile/clock-r8a7779.c
+++ b/arch/arm/mach-shmobile/clock-r8a7779.c
@@ -173,9 +173,7 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_DEV_ID("ohci-platform.1", &mstp_clks[MSTP101]), /* USB OHCI port2 */
 	CLKDEV_DEV_ID("ehci-platform.0", &mstp_clks[MSTP100]), /* USB EHCI port0/1 */
 	CLKDEV_DEV_ID("ohci-platform.0", &mstp_clks[MSTP100]), /* USB OHCI port0/1 */
-	CLKDEV_DEV_ID("sh_tmu.0", &mstp_clks[MSTP016]), /* TMU00 */
-	CLKDEV_DEV_ID("sh_tmu.1", &mstp_clks[MSTP016]), /* TMU01 */
-	CLKDEV_DEV_ID("sh_tmu.2", &mstp_clks[MSTP016]), /* TMU02 */
+	CLKDEV_ICK_ID("fck", "sh-tmu.0", &mstp_clks[MSTP016]), /* TMU0 */
 	CLKDEV_DEV_ID("i2c-rcar.0", &mstp_clks[MSTP030]), /* I2C0 */
 	CLKDEV_DEV_ID("ffc70000.i2c", &mstp_clks[MSTP030]), /* I2C0 */
 	CLKDEV_DEV_ID("i2c-rcar.1", &mstp_clks[MSTP029]), /* I2C1 */
diff --git a/arch/arm/mach-shmobile/clock-r8a7790.c b/arch/arm/mach-shmobile/clock-r8a7790.c
index 3f93503f5b9..296a057109e 100644
--- a/arch/arm/mach-shmobile/clock-r8a7790.c
+++ b/arch/arm/mach-shmobile/clock-r8a7790.c
@@ -249,10 +249,10 @@ static struct clk mstp_clks[MSTP_NR] = {
 	[MSTP1007] = SH_CLK_MSTP32_STS(&p_clk, SMSTPCR10, 7, MSTPSR10, 0), /* SSI8 */
 	[MSTP1006] = SH_CLK_MSTP32_STS(&p_clk, SMSTPCR10, 6, MSTPSR10, 0), /* SSI9 */
 	[MSTP1005] = SH_CLK_MSTP32_STS(&p_clk, SMSTPCR10, 5, MSTPSR10, 0), /* SSI ALL */
-	[MSTP931] = SH_CLK_MSTP32_STS(&p_clk, SMSTPCR9, 31, MSTPSR9, 0), /* I2C0 */
-	[MSTP930] = SH_CLK_MSTP32_STS(&p_clk, SMSTPCR9, 30, MSTPSR9, 0), /* I2C1 */
-	[MSTP929] = SH_CLK_MSTP32_STS(&p_clk, SMSTPCR9, 29, MSTPSR9, 0), /* I2C2 */
-	[MSTP928] = SH_CLK_MSTP32_STS(&p_clk, SMSTPCR9, 28, MSTPSR9, 0), /* I2C3 */
+	[MSTP931] = SH_CLK_MSTP32_STS(&hp_clk, SMSTPCR9, 31, MSTPSR9, 0), /* I2C0 */
+	[MSTP930] = SH_CLK_MSTP32_STS(&hp_clk, SMSTPCR9, 30, MSTPSR9, 0), /* I2C1 */
+	[MSTP929] = SH_CLK_MSTP32_STS(&hp_clk, SMSTPCR9, 29, MSTPSR9, 0), /* I2C2 */
+	[MSTP928] = SH_CLK_MSTP32_STS(&hp_clk, SMSTPCR9, 28, MSTPSR9, 0), /* I2C3 */
 	[MSTP917] = SH_CLK_MSTP32_STS(&qspi_clk, SMSTPCR9, 17, MSTPSR9, 0), /* QSPI */
 	[MSTP815] = SH_CLK_MSTP32_STS(&zs_clk, SMSTPCR8, 15, MSTPSR8, 0), /* SATA0 */
 	[MSTP814] = SH_CLK_MSTP32_STS(&zs_clk, SMSTPCR8, 14, MSTPSR8, 0), /* SATA1 */
@@ -294,10 +294,6 @@ static struct clk mstp_clks[MSTP_NR] = {
 static struct clk_lookup lookups[] = {
 
 	/* main clocks */
-	CLKDEV_CON_ID("audio_clk_a",	&audio_clk_a),
-	CLKDEV_CON_ID("audio_clk_b",	&audio_clk_b),
-	CLKDEV_CON_ID("audio_clk_c",	&audio_clk_c),
-	CLKDEV_CON_ID("audio_clk_internal",	&m2_clk),
 	CLKDEV_CON_ID("extal",		&extal_clk),
 	CLKDEV_CON_ID("extal_div2",	&extal_div2_clk),
 	CLKDEV_CON_ID("main",		&main_clk),
@@ -361,7 +357,6 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_DEV_ID("sh_mobile_sdhi.2", &mstp_clks[MSTP312]),
 	CLKDEV_DEV_ID("sh_mobile_sdhi.3", &mstp_clks[MSTP311]),
 	CLKDEV_DEV_ID("sh_mmcif.1", &mstp_clks[MSTP305]),
-	CLKDEV_DEV_ID("sh_cmt.0", &mstp_clks[MSTP124]),
 	CLKDEV_DEV_ID("qspi.0", &mstp_clks[MSTP917]),
 	CLKDEV_DEV_ID("renesas_usbhs", &mstp_clks[MSTP704]),
 	CLKDEV_DEV_ID("pci-rcar-gen2.0", &mstp_clks[MSTP703]),
@@ -371,6 +366,7 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_DEV_ID("sata-r8a7790.1", &mstp_clks[MSTP814]),
 
 	/* ICK */
+	CLKDEV_ICK_ID("fck", "sh-cmt-48-gen2.0", &mstp_clks[MSTP124]),
 	CLKDEV_ICK_ID("usbhs", "usb_phy_rcar_gen2", &mstp_clks[MSTP704]),
 	CLKDEV_ICK_ID("lvds.0", "rcar-du-r8a7790", &mstp_clks[MSTP726]),
 	CLKDEV_ICK_ID("lvds.1", "rcar-du-r8a7790", &mstp_clks[MSTP725]),
@@ -381,16 +377,16 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_ICK_ID("clk_b", "rcar_sound", &audio_clk_b),
 	CLKDEV_ICK_ID("clk_c", "rcar_sound", &audio_clk_c),
 	CLKDEV_ICK_ID("clk_i", "rcar_sound", &m2_clk),
-	CLKDEV_ICK_ID("scu.0", "rcar_sound", &mstp_clks[MSTP1031]),
-	CLKDEV_ICK_ID("scu.1", "rcar_sound", &mstp_clks[MSTP1030]),
-	CLKDEV_ICK_ID("scu.2", "rcar_sound", &mstp_clks[MSTP1029]),
-	CLKDEV_ICK_ID("scu.3", "rcar_sound", &mstp_clks[MSTP1028]),
-	CLKDEV_ICK_ID("scu.4", "rcar_sound", &mstp_clks[MSTP1027]),
-	CLKDEV_ICK_ID("scu.5", "rcar_sound", &mstp_clks[MSTP1026]),
-	CLKDEV_ICK_ID("scu.6", "rcar_sound", &mstp_clks[MSTP1025]),
-	CLKDEV_ICK_ID("scu.7", "rcar_sound", &mstp_clks[MSTP1024]),
-	CLKDEV_ICK_ID("scu.8", "rcar_sound", &mstp_clks[MSTP1023]),
-	CLKDEV_ICK_ID("scu.9", "rcar_sound", &mstp_clks[MSTP1022]),
+	CLKDEV_ICK_ID("src.0", "rcar_sound", &mstp_clks[MSTP1031]),
+	CLKDEV_ICK_ID("src.1", "rcar_sound", &mstp_clks[MSTP1030]),
+	CLKDEV_ICK_ID("src.2", "rcar_sound", &mstp_clks[MSTP1029]),
+	CLKDEV_ICK_ID("src.3", "rcar_sound", &mstp_clks[MSTP1028]),
+	CLKDEV_ICK_ID("src.4", "rcar_sound", &mstp_clks[MSTP1027]),
+	CLKDEV_ICK_ID("src.5", "rcar_sound", &mstp_clks[MSTP1026]),
+	CLKDEV_ICK_ID("src.6", "rcar_sound", &mstp_clks[MSTP1025]),
+	CLKDEV_ICK_ID("src.7", "rcar_sound", &mstp_clks[MSTP1024]),
+	CLKDEV_ICK_ID("src.8", "rcar_sound", &mstp_clks[MSTP1023]),
+	CLKDEV_ICK_ID("src.9", "rcar_sound", &mstp_clks[MSTP1022]),
 	CLKDEV_ICK_ID("ssi.0", "rcar_sound", &mstp_clks[MSTP1015]),
 	CLKDEV_ICK_ID("ssi.1", "rcar_sound", &mstp_clks[MSTP1014]),
 	CLKDEV_ICK_ID("ssi.2", "rcar_sound", &mstp_clks[MSTP1013]),
diff --git a/arch/arm/mach-shmobile/clock-r8a7791.c b/arch/arm/mach-shmobile/clock-r8a7791.c
index 701383fe326..e2fdfcc1443 100644
--- a/arch/arm/mach-shmobile/clock-r8a7791.c
+++ b/arch/arm/mach-shmobile/clock-r8a7791.c
@@ -25,6 +25,7 @@
 #include <linux/clkdev.h>
 #include <mach/clock.h>
 #include <mach/common.h>
+#include <mach/rcar-gen2.h>
 
 /*
  *   MD		EXTAL		PLL0	PLL1	PLL3
@@ -43,8 +44,6 @@
  *	see "p1 / 2" on R8A7791_CLOCK_ROOT() below
  */
 
-#define MD(nr)	(1 << nr)
-
 #define CPG_BASE 0xe6150000
 #define CPG_LEN 0x1000
 
@@ -68,7 +67,6 @@
 #define MSTPSR9		IOMEM(0xe61509a4)
 #define MSTPSR11	IOMEM(0xe61509ac)
 
-#define MODEMR		0xE6160060
 #define SDCKCR		0xE6150074
 #define SD1CKCR		0xE6150078
 #define SD2CKCR		0xE615026c
@@ -190,12 +188,12 @@ static struct clk mstp_clks[MSTP_NR] = {
 	[MSTP1108] = SH_CLK_MSTP32_STS(&mp_clk, SMSTPCR11, 8, MSTPSR11, 0), /* SCIFA5 */
 	[MSTP1107] = SH_CLK_MSTP32_STS(&mp_clk, SMSTPCR11, 7, MSTPSR11, 0), /* SCIFA4 */
 	[MSTP1106] = SH_CLK_MSTP32_STS(&mp_clk, SMSTPCR11, 6, MSTPSR11, 0), /* SCIFA3 */
-	[MSTP931] = SH_CLK_MSTP32_STS(&p_clk, SMSTPCR9, 31, MSTPSR9, 0), /* I2C0 */
-	[MSTP930] = SH_CLK_MSTP32_STS(&p_clk, SMSTPCR9, 30, MSTPSR9, 0), /* I2C1 */
-	[MSTP929] = SH_CLK_MSTP32_STS(&p_clk, SMSTPCR9, 29, MSTPSR9, 0), /* I2C2 */
-	[MSTP928] = SH_CLK_MSTP32_STS(&p_clk, SMSTPCR9, 28, MSTPSR9, 0), /* I2C3 */
-	[MSTP927] = SH_CLK_MSTP32_STS(&p_clk, SMSTPCR9, 27, MSTPSR9, 0), /* I2C4 */
-	[MSTP925] = SH_CLK_MSTP32_STS(&p_clk, SMSTPCR9, 25, MSTPSR9, 0), /* I2C5 */
+	[MSTP931] = SH_CLK_MSTP32_STS(&hp_clk, SMSTPCR9, 31, MSTPSR9, 0), /* I2C0 */
+	[MSTP930] = SH_CLK_MSTP32_STS(&hp_clk, SMSTPCR9, 30, MSTPSR9, 0), /* I2C1 */
+	[MSTP929] = SH_CLK_MSTP32_STS(&hp_clk, SMSTPCR9, 29, MSTPSR9, 0), /* I2C2 */
+	[MSTP928] = SH_CLK_MSTP32_STS(&hp_clk, SMSTPCR9, 28, MSTPSR9, 0), /* I2C3 */
+	[MSTP927] = SH_CLK_MSTP32_STS(&hp_clk, SMSTPCR9, 27, MSTPSR9, 0), /* I2C4 */
+	[MSTP925] = SH_CLK_MSTP32_STS(&hp_clk, SMSTPCR9, 25, MSTPSR9, 0), /* I2C5 */
 	[MSTP917] = SH_CLK_MSTP32_STS(&qspi_clk, SMSTPCR9, 17, MSTPSR9, 0), /* QSPI */
 	[MSTP815] = SH_CLK_MSTP32_STS(&zs_clk, SMSTPCR8, 15, MSTPSR8, 0), /* SATA0 */
 	[MSTP814] = SH_CLK_MSTP32_STS(&zs_clk, SMSTPCR8, 14, MSTPSR8, 0), /* SATA1 */
@@ -266,7 +264,7 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_DEV_ID("sh_mobile_sdhi.0", &mstp_clks[MSTP314]),
 	CLKDEV_DEV_ID("sh_mobile_sdhi.1", &mstp_clks[MSTP312]),
 	CLKDEV_DEV_ID("sh_mobile_sdhi.2", &mstp_clks[MSTP311]),
-	CLKDEV_DEV_ID("sh_cmt.0", &mstp_clks[MSTP124]),
+	CLKDEV_ICK_ID("fck", "sh-cmt-48-gen2.0", &mstp_clks[MSTP124]),
 	CLKDEV_DEV_ID("qspi.0", &mstp_clks[MSTP917]),
 	CLKDEV_DEV_ID("rcar_thermal", &mstp_clks[MSTP522]),
 	CLKDEV_DEV_ID("i2c-rcar_gen2.0", &mstp_clks[MSTP931]),
@@ -295,14 +293,9 @@ static struct clk_lookup lookups[] = {
 
 void __init r8a7791_clock_init(void)
 {
-	void __iomem *modemr = ioremap_nocache(MODEMR, PAGE_SIZE);
-	u32 mode;
+	u32 mode = rcar_gen2_read_mode_pins();
 	int k, ret = 0;
 
-	BUG_ON(!modemr);
-	mode = ioread32(modemr);
-	iounmap(modemr);
-
 	switch (mode & (MD(14) | MD(13))) {
 	case 0:
 		R8A7791_CLOCK_ROOT(15, &extal_clk, 172, 208, 106, 88);
diff --git a/arch/arm/mach-shmobile/clock-sh7372.c b/arch/arm/mach-shmobile/clock-sh7372.c
index 28489978b09..d16d9ca7f79 100644
--- a/arch/arm/mach-shmobile/clock-sh7372.c
+++ b/arch/arm/mach-shmobile/clock-sh7372.c
@@ -515,8 +515,6 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_DEV_ID("uio_pdrv_genirq.1", &mstp_clks[MSTP128]), /* VEU0 */
 	CLKDEV_DEV_ID("sh_mobile_ceu.0", &mstp_clks[MSTP127]), /* CEU */
 	CLKDEV_DEV_ID("sh-mobile-csi2.0", &mstp_clks[MSTP126]), /* CSI2 */
-	CLKDEV_DEV_ID("sh_tmu.0", &mstp_clks[MSTP125]), /* TMU00 */
-	CLKDEV_DEV_ID("sh_tmu.1", &mstp_clks[MSTP125]), /* TMU01 */
 	CLKDEV_DEV_ID("sh-mipi-dsi.0", &mstp_clks[MSTP118]), /* DSITX0 */
 	CLKDEV_DEV_ID("sh_mobile_lcdc_fb.1", &mstp_clks[MSTP117]), /* LCDC1 */
 	CLKDEV_DEV_ID("i2c-sh_mobile.0", &mstp_clks[MSTP116]), /* IIC0 */
@@ -565,10 +563,7 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_DEV_ID("r8a66597_hcd.1", &mstp_clks[MSTP406]), /* USB1 */
 	CLKDEV_DEV_ID("r8a66597_udc.1", &mstp_clks[MSTP406]), /* USB1 */
 	CLKDEV_DEV_ID("renesas_usbhs.1", &mstp_clks[MSTP406]), /* USB1 */
-	CLKDEV_DEV_ID("sh_cmt.4", &mstp_clks[MSTP405]), /* CMT4 */
-	CLKDEV_DEV_ID("sh_cmt.3", &mstp_clks[MSTP404]), /* CMT3 */
 	CLKDEV_DEV_ID("sh_keysc.0", &mstp_clks[MSTP403]), /* KEYSC */
-	CLKDEV_DEV_ID("sh_cmt.2", &mstp_clks[MSTP400]), /* CMT2 */
 
 	/* ICK */
 	CLKDEV_ICK_ID("dsit_clk", "sh-mipi-dsi.0", &div6_clks[DIV6_DSIT]),
@@ -580,7 +575,11 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_ICK_ID("ick", "sh-mobile-hdmi", &div6_reparent_clks[DIV6_HDMI]),
 	CLKDEV_ICK_ID("icka", "sh_fsi2", &div6_reparent_clks[DIV6_FSIA]),
 	CLKDEV_ICK_ID("ickb", "sh_fsi2", &div6_reparent_clks[DIV6_FSIB]),
+	CLKDEV_ICK_ID("fck", "sh-tmu.0", &mstp_clks[MSTP125]), /* TMU0 */
 	CLKDEV_ICK_ID("spu2", "sh_fsi2", &mstp_clks[MSTP223]),
+	CLKDEV_ICK_ID("fck", "sh-cmt-32-fast.4", &mstp_clks[MSTP405]), /* CMT4 */
+	CLKDEV_ICK_ID("fck", "sh-cmt-32-fast.3", &mstp_clks[MSTP404]), /* CMT3 */
+	CLKDEV_ICK_ID("fck", "sh-cmt-32-fast.2", &mstp_clks[MSTP400]), /* CMT2 */
 	CLKDEV_ICK_ID("diva", "sh_fsi2", &fsidivs[FSIDIV_A]),
 	CLKDEV_ICK_ID("divb", "sh_fsi2", &fsidivs[FSIDIV_B]),
 	CLKDEV_ICK_ID("xcka", "sh_fsi2", &fsiack_clk),
diff --git a/arch/arm/mach-shmobile/clock-sh73a0.c b/arch/arm/mach-shmobile/clock-sh73a0.c
index 23edf8360c2..0d9cd1fe021 100644
--- a/arch/arm/mach-shmobile/clock-sh73a0.c
+++ b/arch/arm/mach-shmobile/clock-sh73a0.c
@@ -633,8 +633,6 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_DEV_ID("sh-mobile-csi2.1", &mstp_clks[MSTP128]), /* CSI2-RX1 */
 	CLKDEV_DEV_ID("sh_mobile_ceu.0", &mstp_clks[MSTP127]), /* CEU0 */
 	CLKDEV_DEV_ID("sh-mobile-csi2.0", &mstp_clks[MSTP126]), /* CSI2-RX0 */
-	CLKDEV_DEV_ID("sh_tmu.0", &mstp_clks[MSTP125]), /* TMU00 */
-	CLKDEV_DEV_ID("sh_tmu.1", &mstp_clks[MSTP125]), /* TMU01 */
 	CLKDEV_DEV_ID("sh-mipi-dsi.0", &mstp_clks[MSTP118]), /* DSITX */
 	CLKDEV_DEV_ID("i2c-sh_mobile.0", &mstp_clks[MSTP116]), /* I2C0 */
 	CLKDEV_DEV_ID("e6820000.i2c", &mstp_clks[MSTP116]), /* I2C0 */
@@ -650,7 +648,6 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_DEV_ID("sh-sci.3", &mstp_clks[MSTP201]), /* SCIFA3 */
 	CLKDEV_DEV_ID("sh-sci.4", &mstp_clks[MSTP200]), /* SCIFA4 */
 	CLKDEV_DEV_ID("sh-sci.6", &mstp_clks[MSTP331]), /* SCIFA6 */
-	CLKDEV_DEV_ID("sh_cmt.10", &mstp_clks[MSTP329]), /* CMT10 */
 	CLKDEV_DEV_ID("sh_fsi2", &mstp_clks[MSTP328]), /* FSI */
 	CLKDEV_DEV_ID("ec230000.sound", &mstp_clks[MSTP328]), /* FSI */
 	CLKDEV_DEV_ID("sh_irda.0", &mstp_clks[MSTP325]), /* IrDA */
@@ -683,6 +680,8 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_ICK_ID("dsip_clk", "sh-mipi-dsi.1", &div6_clks[DIV6_DSI1P]),
 	CLKDEV_ICK_ID("dsiphy_clk", "sh-mipi-dsi.0", &dsi0phy_clk),
 	CLKDEV_ICK_ID("dsiphy_clk", "sh-mipi-dsi.1", &dsi1phy_clk),
+	CLKDEV_ICK_ID("fck", "sh-cmt-48.1", &mstp_clks[MSTP329]), /* CMT1 */
+	CLKDEV_ICK_ID("fck", "sh-tmu.0", &mstp_clks[MSTP125]), /* TMU0 */
 };
 
 void __init sh73a0_clock_init(void)
diff --git a/arch/arm/mach-shmobile/clock.c b/arch/arm/mach-shmobile/clock.c
index ad7df629d99..e7232a0373b 100644
--- a/arch/arm/mach-shmobile/clock.c
+++ b/arch/arm/mach-shmobile/clock.c
@@ -21,6 +21,32 @@
  */
 #include <linux/kernel.h>
 #include <linux/init.h>
+
+#ifdef CONFIG_COMMON_CLK
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <mach/clock.h>
+
+void __init shmobile_clk_workaround(const struct clk_name *clks,
+				    int nr_clks, bool enable)
+{
+	const struct clk_name *clkn;
+	struct clk *clk;
+	unsigned int i;
+
+	for (i = 0; i < nr_clks; ++i) {
+		clkn = clks + i;
+		clk = clk_get(NULL, clkn->clk);
+		if (!IS_ERR(clk)) {
+			clk_register_clkdev(clk, clkn->con_id, clkn->dev_id);
+			if (enable)
+				clk_prepare_enable(clk);
+			clk_put(clk);
+		}
+	}
+}
+
+#else /* CONFIG_COMMON_CLK */
 #include <linux/sh_clk.h>
 #include <linux/export.h>
 #include <mach/clock.h>
@@ -58,3 +84,5 @@ void __clk_put(struct clk *clk)
 {
 }
 EXPORT_SYMBOL(__clk_put);
+
+#endif /* CONFIG_COMMON_CLK */
diff --git a/arch/arm/mach-shmobile/include/mach/clock.h b/arch/arm/mach-shmobile/include/mach/clock.h
index 03e56074928..31b6417463e 100644
--- a/arch/arm/mach-shmobile/include/mach/clock.h
+++ b/arch/arm/mach-shmobile/include/mach/clock.h
@@ -1,6 +1,22 @@
 #ifndef CLOCK_H
 #define CLOCK_H
 
+#ifdef CONFIG_COMMON_CLK
+/* temporary clock configuration helper for platform devices */
+
+struct clk_name {
+	const char *clk;
+	const char *con_id;
+	const char *dev_id;
+};
+
+void shmobile_clk_workaround(const struct clk_name *clks, int nr_clks,
+			     bool enable);
+
+#else /* CONFIG_COMMON_CLK */
+/* legacy clock implementation */
+
+struct clk;
 unsigned long shmobile_fixed_ratio_clk_recalc(struct clk *clk);
 extern struct sh_clk_ops shmobile_fixed_ratio_clk_ops;
 
@@ -36,4 +52,5 @@ do {			\
 	(p)->div = d;	\
 } while (0)
 
+#endif /* CONFIG_COMMON_CLK */
 #endif
diff --git a/arch/arm/mach-shmobile/include/mach/common.h b/arch/arm/mach-shmobile/include/mach/common.h
index cb8e32deb2a..f7a360edcc3 100644
--- a/arch/arm/mach-shmobile/include/mach/common.h
+++ b/arch/arm/mach-shmobile/include/mach/common.h
@@ -4,6 +4,7 @@
 extern void shmobile_earlytimer_init(void);
 extern void shmobile_setup_delay(unsigned int max_cpu_core_mhz,
 			 unsigned int mult, unsigned int div);
+extern void shmobile_init_delay(void);
 struct twd_local_timer;
 extern void shmobile_setup_console(void);
 extern void shmobile_boot_vector(void);
diff --git a/arch/arm/mach-shmobile/include/mach/emev2.h b/arch/arm/mach-shmobile/include/mach/emev2.h
deleted file mode 100644
index fcb142a14e0..00000000000
--- a/arch/arm/mach-shmobile/include/mach/emev2.h
+++ /dev/null
@@ -1,9 +0,0 @@
-#ifndef __ASM_EMEV2_H__
-#define __ASM_EMEV2_H__
-
-extern void emev2_map_io(void);
-extern void emev2_init_delay(void);
-extern void emev2_clock_init(void);
-extern struct smp_operations emev2_smp_ops;
-
-#endif /* __ASM_EMEV2_H__ */
diff --git a/arch/arm/mach-shmobile/include/mach/r8a7740.h b/arch/arm/mach-shmobile/include/mach/r8a7740.h
index d07932f872b..5e3c9ec0630 100644
--- a/arch/arm/mach-shmobile/include/mach/r8a7740.h
+++ b/arch/arm/mach-shmobile/include/mach/r8a7740.h
@@ -47,7 +47,6 @@ enum {
 };
 
 extern void r8a7740_meram_workaround(void);
-extern void r8a7740_init_delay(void);
 extern void r8a7740_init_irq_of(void);
 extern void r8a7740_map_io(void);
 extern void r8a7740_add_early_devices(void);
diff --git a/arch/arm/mach-shmobile/include/mach/r8a7791.h b/arch/arm/mach-shmobile/include/mach/r8a7791.h
index 200fa699f73..664274cc4b6 100644
--- a/arch/arm/mach-shmobile/include/mach/r8a7791.h
+++ b/arch/arm/mach-shmobile/include/mach/r8a7791.h
@@ -5,7 +5,6 @@ void r8a7791_add_standard_devices(void);
 void r8a7791_add_dt_devices(void);
 void r8a7791_clock_init(void);
 void r8a7791_pinmux_init(void);
-void r8a7791_init_early(void);
 extern struct smp_operations r8a7791_smp_ops;
 
 #endif /* __ASM_R8A7791_H__ */
diff --git a/arch/arm/mach-shmobile/pm-rmobile.c b/arch/arm/mach-shmobile/pm-rmobile.c
index 1fc05d9453d..f710235aff2 100644
--- a/arch/arm/mach-shmobile/pm-rmobile.c
+++ b/arch/arm/mach-shmobile/pm-rmobile.c
@@ -99,39 +99,7 @@ static int rmobile_pd_power_up(struct generic_pm_domain *genpd)
 
 static bool rmobile_pd_active_wakeup(struct device *dev)
 {
-	bool (*active_wakeup)(struct device *dev);
-
-	active_wakeup = dev_gpd_data(dev)->ops.active_wakeup;
-	return active_wakeup ? active_wakeup(dev) : true;
-}
-
-static int rmobile_pd_stop_dev(struct device *dev)
-{
-	int (*stop)(struct device *dev);
-
-	stop = dev_gpd_data(dev)->ops.stop;
-	if (stop) {
-		int ret = stop(dev);
-		if (ret)
-			return ret;
-	}
-	return pm_clk_suspend(dev);
-}
-
-static int rmobile_pd_start_dev(struct device *dev)
-{
-	int (*start)(struct device *dev);
-	int ret;
-
-	ret = pm_clk_resume(dev);
-	if (ret)
-		return ret;
-
-	start = dev_gpd_data(dev)->ops.start;
-	if (start)
-		ret = start(dev);
-
-	return ret;
+	return true;
 }
 
 static void rmobile_init_pm_domain(struct rmobile_pm_domain *rmobile_pd)
@@ -140,8 +108,8 @@ static void rmobile_init_pm_domain(struct rmobile_pm_domain *rmobile_pd)
 	struct dev_power_governor *gov = rmobile_pd->gov;
 
 	pm_genpd_init(genpd, gov ? : &simple_qos_governor, false);
-	genpd->dev_ops.stop		= rmobile_pd_stop_dev;
-	genpd->dev_ops.start		= rmobile_pd_start_dev;
+	genpd->dev_ops.stop		= pm_clk_suspend;
+	genpd->dev_ops.start		= pm_clk_resume;
 	genpd->dev_ops.active_wakeup	= rmobile_pd_active_wakeup;
 	genpd->dev_irq_safe		= true;
 	genpd->power_off		= rmobile_pd_power_down;
diff --git a/arch/arm/mach-shmobile/setup-emev2.c b/arch/arm/mach-shmobile/setup-emev2.c
index c71d667007b..d953ff6e78a 100644
--- a/arch/arm/mach-shmobile/setup-emev2.c
+++ b/arch/arm/mach-shmobile/setup-emev2.c
@@ -21,7 +21,6 @@
 #include <linux/init.h>
 #include <linux/of_platform.h>
 #include <mach/common.h>
-#include <mach/emev2.h>
 #include <asm/mach-types.h>
 #include <asm/mach/arch.h>
 #include <asm/mach/map.h>
@@ -38,23 +37,19 @@ static struct map_desc emev2_io_desc[] __initdata = {
 #endif
 };
 
-void __init emev2_map_io(void)
+static void __init emev2_map_io(void)
 {
 	iotable_init(emev2_io_desc, ARRAY_SIZE(emev2_io_desc));
 }
 
-void __init emev2_init_delay(void)
+static void __init emev2_init_delay(void)
 {
 	shmobile_setup_delay(533, 1, 3); /* Cortex-A9 @ 533MHz */
 }
 
 static void __init emev2_add_standard_devices_dt(void)
 {
-#ifdef CONFIG_COMMON_CLK
 	of_clk_init(NULL);
-#else
-	emev2_clock_init();
-#endif
 	of_platform_populate(NULL, of_default_bus_match_table, NULL, NULL);
 }
 
@@ -63,6 +58,8 @@ static const char *emev2_boards_compat_dt[] __initconst = {
 	NULL,
 };
 
+extern struct smp_operations emev2_smp_ops;
+
 DT_MACHINE_START(EMEV2_DT, "Generic Emma Mobile EV2 (Flattened Device Tree)")
 	.smp		= smp_ops(emev2_smp_ops),
 	.map_io		= emev2_map_io,
diff --git a/arch/arm/mach-shmobile/setup-r7s72100.c b/arch/arm/mach-shmobile/setup-r7s72100.c
index 9c0b3a9d5f7..412e179429c 100644
--- a/arch/arm/mach-shmobile/setup-r7s72100.c
+++ b/arch/arm/mach-shmobile/setup-r7s72100.c
@@ -21,77 +21,26 @@
 #include <linux/irq.h>
 #include <linux/kernel.h>
 #include <linux/of_platform.h>
-#include <linux/serial_sci.h>
 #include <linux/sh_timer.h>
 #include <mach/common.h>
 #include <mach/irqs.h>
 #include <mach/r7s72100.h>
 #include <asm/mach/arch.h>
 
-#define R7S72100_SCIF(index, baseaddr, irq)				\
-static const struct plat_sci_port scif##index##_platform_data = {	\
-	.type		= PORT_SCIF,					\
-	.regtype	= SCIx_SH2_SCIF_FIFODATA_REGTYPE,		\
-	.flags		= UPF_BOOT_AUTOCONF | UPF_IOREMAP,		\
-	.scscr		= SCSCR_RIE | SCSCR_TIE | SCSCR_RE | SCSCR_TE |	\
-			  SCSCR_REIE,					\
-};									\
-									\
-static struct resource scif##index##_resources[] = {			\
-	DEFINE_RES_MEM(baseaddr, 0x100),				\
-	DEFINE_RES_IRQ(irq + 1),					\
-	DEFINE_RES_IRQ(irq + 2),					\
-	DEFINE_RES_IRQ(irq + 3),					\
-	DEFINE_RES_IRQ(irq),						\
-}									\
-
-R7S72100_SCIF(0, 0xe8007000, gic_iid(221));
-R7S72100_SCIF(1, 0xe8007800, gic_iid(225));
-R7S72100_SCIF(2, 0xe8008000, gic_iid(229));
-R7S72100_SCIF(3, 0xe8008800, gic_iid(233));
-R7S72100_SCIF(4, 0xe8009000, gic_iid(237));
-R7S72100_SCIF(5, 0xe8009800, gic_iid(241));
-R7S72100_SCIF(6, 0xe800a000, gic_iid(245));
-R7S72100_SCIF(7, 0xe800a800, gic_iid(249));
-
-#define r7s72100_register_scif(index)					       \
-	platform_device_register_resndata(&platform_bus, "sh-sci", index,      \
-					  scif##index##_resources,	       \
-					  ARRAY_SIZE(scif##index##_resources), \
-					  &scif##index##_platform_data,	       \
-					  sizeof(scif##index##_platform_data))
-
-
-static struct sh_timer_config mtu2_0_platform_data __initdata = {
-	.name = "MTU2_0",
-	.timer_bit = 0,
-	.channel_offset = -0x80,
-	.clockevent_rating = 200,
-};
-
-static struct resource mtu2_0_resources[] __initdata = {
-	DEFINE_RES_MEM(0xfcff0300, 0x27),
-	DEFINE_RES_IRQ(gic_iid(139)), /* MTU2 TGI0A */
+static struct resource mtu2_resources[] __initdata = {
+	DEFINE_RES_MEM(0xfcff0000, 0x400),
+	DEFINE_RES_IRQ_NAMED(gic_iid(139), "tgi0a"),
 };
 
-#define r7s72100_register_mtu2(idx)					\
-	platform_device_register_resndata(&platform_bus, "sh_mtu2",	\
-					  idx, mtu2_##idx##_resources,	\
-					  ARRAY_SIZE(mtu2_##idx##_resources), \
-					  &mtu2_##idx##_platform_data,	\
-					  sizeof(struct sh_timer_config))
+#define r7s72100_register_mtu2()					\
+	platform_device_register_resndata(&platform_bus, "sh-mtu2",	\
+					  -1, mtu2_resources,		\
+					  ARRAY_SIZE(mtu2_resources),	\
+					  NULL, 0)
 
 void __init r7s72100_add_dt_devices(void)
 {
-	r7s72100_register_scif(0);
-	r7s72100_register_scif(1);
-	r7s72100_register_scif(2);
-	r7s72100_register_scif(3);
-	r7s72100_register_scif(4);
-	r7s72100_register_scif(5);
-	r7s72100_register_scif(6);
-	r7s72100_register_scif(7);
-	r7s72100_register_mtu2(0);
+	r7s72100_register_mtu2();
 }
 
 void __init r7s72100_init_early(void)
diff --git a/arch/arm/mach-shmobile/setup-r8a73a4.c b/arch/arm/mach-shmobile/setup-r8a73a4.c
index cd36f807832..9333770cfac 100644
--- a/arch/arm/mach-shmobile/setup-r8a73a4.c
+++ b/arch/arm/mach-shmobile/setup-r8a73a4.c
@@ -169,20 +169,17 @@ static const struct resource thermal0_resources[] = {
 					thermal0_resources,		\
 					ARRAY_SIZE(thermal0_resources))
 
-static struct sh_timer_config cmt10_platform_data = {
-	.name = "CMT10",
-	.timer_bit = 0,
-	.clockevent_rating = 80,
+static struct sh_timer_config cmt1_platform_data = {
+	.channels_mask = 0xff,
 };
 
-static struct resource cmt10_resources[] = {
-	DEFINE_RES_MEM(0xe6130010, 0x0c),
-	DEFINE_RES_MEM(0xe6130000, 0x04),
-	DEFINE_RES_IRQ(gic_spi(120)), /* CMT1_0 */
+static struct resource cmt1_resources[] = {
+	DEFINE_RES_MEM(0xe6130000, 0x1004),
+	DEFINE_RES_IRQ(gic_spi(120)),
 };
 
 #define r8a7790_register_cmt(idx)					\
-	platform_device_register_resndata(&platform_bus, "sh_cmt",	\
+	platform_device_register_resndata(&platform_bus, "sh-cmt-48-gen2", \
 					  idx, cmt##idx##_resources,	\
 					  ARRAY_SIZE(cmt##idx##_resources), \
 					  &cmt##idx##_platform_data,	\
@@ -196,7 +193,7 @@ void __init r8a73a4_add_dt_devices(void)
 	r8a73a4_register_scif(3);
 	r8a73a4_register_scif(4);
 	r8a73a4_register_scif(5);
-	r8a7790_register_cmt(10);
+	r8a7790_register_cmt(1);
 }
 
 /* DMA */
diff --git a/arch/arm/mach-shmobile/setup-r8a7740.c b/arch/arm/mach-shmobile/setup-r8a7740.c
index 8f3c68101d5..35dec233301 100644
--- a/arch/arm/mach-shmobile/setup-r8a7740.c
+++ b/arch/arm/mach-shmobile/setup-r8a7740.c
@@ -237,126 +237,45 @@ R8A7740_SCIF(PORT_SCIFA, 7, 0xe6cd0000, gic_spi(107));
 R8A7740_SCIF(PORT_SCIFB, 8, 0xe6c30000, gic_spi(108));
 
 /* CMT */
-static struct sh_timer_config cmt10_platform_data = {
-	.name = "CMT10",
-	.channel_offset = 0x10,
-	.timer_bit = 0,
-	.clockevent_rating = 125,
-	.clocksource_rating = 125,
+static struct sh_timer_config cmt1_platform_data = {
+	.channels_mask = 0x3f,
 };
 
-static struct resource cmt10_resources[] = {
-	[0] = {
-		.name	= "CMT10",
-		.start	= 0xe6138010,
-		.end	= 0xe613801b,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= gic_spi(58),
-		.flags	= IORESOURCE_IRQ,
-	},
+static struct resource cmt1_resources[] = {
+	DEFINE_RES_MEM(0xe6138000, 0x170),
+	DEFINE_RES_IRQ(gic_spi(58)),
 };
 
-static struct platform_device cmt10_device = {
-	.name		= "sh_cmt",
-	.id		= 10,
+static struct platform_device cmt1_device = {
+	.name		= "sh-cmt-48",
+	.id		= 1,
 	.dev = {
-		.platform_data	= &cmt10_platform_data,
+		.platform_data	= &cmt1_platform_data,
 	},
-	.resource	= cmt10_resources,
-	.num_resources	= ARRAY_SIZE(cmt10_resources),
+	.resource	= cmt1_resources,
+	.num_resources	= ARRAY_SIZE(cmt1_resources),
 };
 
 /* TMU */
-static struct sh_timer_config tmu00_platform_data = {
-	.name = "TMU00",
-	.channel_offset = 0x4,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
+static struct sh_timer_config tmu0_platform_data = {
+	.channels_mask = 7,
 };
 
-static struct resource tmu00_resources[] = {
-	[0] = {
-		.name	= "TMU00",
-		.start	= 0xfff80008,
-		.end	= 0xfff80014 - 1,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= gic_spi(198),
-		.flags	= IORESOURCE_IRQ,
-	},
+static struct resource tmu0_resources[] = {
+	DEFINE_RES_MEM(0xfff80000, 0x2c),
+	DEFINE_RES_IRQ(gic_spi(198)),
+	DEFINE_RES_IRQ(gic_spi(199)),
+	DEFINE_RES_IRQ(gic_spi(200)),
 };
 
-static struct platform_device tmu00_device = {
-	.name		= "sh_tmu",
+static struct platform_device tmu0_device = {
+	.name		= "sh-tmu",
 	.id		= 0,
 	.dev = {
-		.platform_data	= &tmu00_platform_data,
-	},
-	.resource	= tmu00_resources,
-	.num_resources	= ARRAY_SIZE(tmu00_resources),
-};
-
-static struct sh_timer_config tmu01_platform_data = {
-	.name = "TMU01",
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-	.clocksource_rating = 200,
-};
-
-static struct resource tmu01_resources[] = {
-	[0] = {
-		.name	= "TMU01",
-		.start	= 0xfff80014,
-		.end	= 0xfff80020 - 1,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= gic_spi(199),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu01_device = {
-	.name		= "sh_tmu",
-	.id		= 1,
-	.dev = {
-		.platform_data	= &tmu01_platform_data,
+		.platform_data	= &tmu0_platform_data,
 	},
-	.resource	= tmu01_resources,
-	.num_resources	= ARRAY_SIZE(tmu01_resources),
-};
-
-static struct sh_timer_config tmu02_platform_data = {
-	.name = "TMU02",
-	.channel_offset = 0x1C,
-	.timer_bit = 2,
-	.clocksource_rating = 200,
-};
-
-static struct resource tmu02_resources[] = {
-	[0] = {
-		.name	= "TMU02",
-		.start	= 0xfff80020,
-		.end	= 0xfff8002C - 1,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= gic_spi(200),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu02_device = {
-	.name		= "sh_tmu",
-	.id		= 2,
-	.dev = {
-		.platform_data	= &tmu02_platform_data,
-	},
-	.resource	= tmu02_resources,
-	.num_resources	= ARRAY_SIZE(tmu02_resources),
+	.resource	= tmu0_resources,
+	.num_resources	= ARRAY_SIZE(tmu0_resources),
 };
 
 /* IPMMUI (an IPMMU module for ICB/LMB) */
@@ -400,7 +319,7 @@ static struct platform_device *r8a7740_devices_dt[] __initdata = {
 	&scif6_device,
 	&scif7_device,
 	&scif8_device,
-	&cmt10_device,
+	&cmt1_device,
 };
 
 static struct platform_device *r8a7740_early_devices[] __initdata = {
@@ -408,9 +327,7 @@ static struct platform_device *r8a7740_early_devices[] __initdata = {
 	&irqpin1_device,
 	&irqpin2_device,
 	&irqpin3_device,
-	&tmu00_device,
-	&tmu01_device,
-	&tmu02_device,
+	&tmu0_device,
 	&ipmmu_device,
 };
 
@@ -765,7 +682,7 @@ static struct platform_device *r8a7740_late_devices[] __initdata = {
  *	"Media RAM (MERAM)" on r8a7740 documentation
  */
 #define MEBUFCNTR	0xFE950098
-void r8a7740_meram_workaround(void)
+void __init r8a7740_meram_workaround(void)
 {
 	void __iomem *reg;
 
@@ -869,17 +786,6 @@ void __init r8a7740_add_early_devices(void)
 
 #ifdef CONFIG_USE_OF
 
-void __init r8a7740_add_early_devices_dt(void)
-{
-	shmobile_setup_delay(800, 1, 3); /* Cortex-A9 @ 800MHz */
-
-	early_platform_add_devices(r8a7740_early_devices,
-				   ARRAY_SIZE(r8a7740_early_devices));
-
-	/* setup early console here as well */
-	shmobile_setup_console();
-}
-
 void __init r8a7740_add_standard_devices_dt(void)
 {
 	platform_add_devices(r8a7740_devices_dt,
@@ -887,11 +793,6 @@ void __init r8a7740_add_standard_devices_dt(void)
 	of_platform_populate(NULL, of_default_bus_match_table, NULL, NULL);
 }
 
-void __init r8a7740_init_delay(void)
-{
-	shmobile_setup_delay(800, 1, 3); /* Cortex-A9 @ 800MHz */
-};
-
 void __init r8a7740_init_irq_of(void)
 {
 	void __iomem *intc_prio_base = ioremap_nocache(0xe6900010, 0x10);
@@ -935,9 +836,10 @@ static const char *r8a7740_boards_compat_dt[] __initdata = {
 
 DT_MACHINE_START(R8A7740_DT, "Generic R8A7740 (Flattened Device Tree)")
 	.map_io		= r8a7740_map_io,
-	.init_early	= r8a7740_init_delay,
+	.init_early	= shmobile_init_delay,
 	.init_irq	= r8a7740_init_irq_of,
 	.init_machine	= r8a7740_generic_init,
+	.init_late	= shmobile_init_late,
 	.dt_compat	= r8a7740_boards_compat_dt,
 MACHINE_END
 
diff --git a/arch/arm/mach-shmobile/setup-r8a7778.c b/arch/arm/mach-shmobile/setup-r8a7778.c
index 6d694526e4c..8c02e24f248 100644
--- a/arch/arm/mach-shmobile/setup-r8a7778.c
+++ b/arch/arm/mach-shmobile/setup-r8a7778.c
@@ -71,33 +71,20 @@ R8A7778_SCIF(5, 0xffe45000, gic_iid(0x6b));
 					  sizeof(scif##index##_platform_data))
 
 /* TMU */
-static struct resource sh_tmu0_resources[] __initdata = {
-	DEFINE_RES_MEM(0xffd80008, 12),
-	DEFINE_RES_IRQ(gic_iid(0x40)),
-};
-
-static struct sh_timer_config sh_tmu0_platform_data __initdata = {
-	.name			= "TMU00",
-	.channel_offset		= 0x4,
-	.timer_bit		= 0,
-	.clockevent_rating	= 200,
+static struct sh_timer_config sh_tmu0_platform_data = {
+	.channels_mask = 7,
 };
 
-static struct resource sh_tmu1_resources[] __initdata = {
-	DEFINE_RES_MEM(0xffd80014, 12),
+static struct resource sh_tmu0_resources[] = {
+	DEFINE_RES_MEM(0xffd80000, 0x30),
+	DEFINE_RES_IRQ(gic_iid(0x40)),
 	DEFINE_RES_IRQ(gic_iid(0x41)),
-};
-
-static struct sh_timer_config sh_tmu1_platform_data __initdata = {
-	.name			= "TMU01",
-	.channel_offset		= 0x10,
-	.timer_bit		= 1,
-	.clocksource_rating	= 200,
+	DEFINE_RES_IRQ(gic_iid(0x42)),
 };
 
 #define r8a7778_register_tmu(idx)			\
 	platform_device_register_resndata(		\
-		&platform_bus, "sh_tmu", idx,		\
+		&platform_bus, "sh-tmu", idx,		\
 		sh_tmu##idx##_resources,		\
 		ARRAY_SIZE(sh_tmu##idx##_resources),	\
 		&sh_tmu##idx##_platform_data,		\
@@ -312,7 +299,6 @@ void __init r8a7778_add_dt_devices(void)
 	r8a7778_register_scif(4);
 	r8a7778_register_scif(5);
 	r8a7778_register_tmu(0);
-	r8a7778_register_tmu(1);
 }
 
 /* HPB-DMA */
diff --git a/arch/arm/mach-shmobile/setup-r8a7779.c b/arch/arm/mach-shmobile/setup-r8a7779.c
index 8e860b36997..d197b5adc88 100644
--- a/arch/arm/mach-shmobile/setup-r8a7779.c
+++ b/arch/arm/mach-shmobile/setup-r8a7779.c
@@ -219,64 +219,25 @@ R8A7779_SCIF(4, 0xffe44000, gic_iid(0x7c));
 R8A7779_SCIF(5, 0xffe45000, gic_iid(0x7d));
 
 /* TMU */
-static struct sh_timer_config tmu00_platform_data = {
-	.name = "TMU00",
-	.channel_offset = 0x4,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
+static struct sh_timer_config tmu0_platform_data = {
+	.channels_mask = 7,
 };
 
-static struct resource tmu00_resources[] = {
-	[0] = {
-		.name	= "TMU00",
-		.start	= 0xffd80008,
-		.end	= 0xffd80013,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= gic_iid(0x40),
-		.flags	= IORESOURCE_IRQ,
-	},
+static struct resource tmu0_resources[] = {
+	DEFINE_RES_MEM(0xffd80000, 0x30),
+	DEFINE_RES_IRQ(gic_iid(0x40)),
+	DEFINE_RES_IRQ(gic_iid(0x41)),
+	DEFINE_RES_IRQ(gic_iid(0x42)),
 };
 
-static struct platform_device tmu00_device = {
-	.name		= "sh_tmu",
+static struct platform_device tmu0_device = {
+	.name		= "sh-tmu",
 	.id		= 0,
 	.dev = {
-		.platform_data	= &tmu00_platform_data,
-	},
-	.resource	= tmu00_resources,
-	.num_resources	= ARRAY_SIZE(tmu00_resources),
-};
-
-static struct sh_timer_config tmu01_platform_data = {
-	.name = "TMU01",
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-	.clocksource_rating = 200,
-};
-
-static struct resource tmu01_resources[] = {
-	[0] = {
-		.name	= "TMU01",
-		.start	= 0xffd80014,
-		.end	= 0xffd8001f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= gic_iid(0x41),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu01_device = {
-	.name		= "sh_tmu",
-	.id		= 1,
-	.dev = {
-		.platform_data	= &tmu01_platform_data,
+		.platform_data	= &tmu0_platform_data,
 	},
-	.resource	= tmu01_resources,
-	.num_resources	= ARRAY_SIZE(tmu01_resources),
+	.resource	= tmu0_resources,
+	.num_resources	= ARRAY_SIZE(tmu0_resources),
 };
 
 /* I2C */
@@ -685,8 +646,7 @@ static struct platform_device *r8a7779_devices_dt[] __initdata = {
 	&scif3_device,
 	&scif4_device,
 	&scif5_device,
-	&tmu00_device,
-	&tmu01_device,
+	&tmu0_device,
 };
 
 static struct platform_device *r8a7779_standard_devices[] __initdata = {
diff --git a/arch/arm/mach-shmobile/setup-r8a7790.c b/arch/arm/mach-shmobile/setup-r8a7790.c
index c4616f0698c..6bd08b127fa 100644
--- a/arch/arm/mach-shmobile/setup-r8a7790.c
+++ b/arch/arm/mach-shmobile/setup-r8a7790.c
@@ -185,12 +185,6 @@ void __init r8a7790_pinmux_init(void)
 	r8a7790_register_gpio(3);
 	r8a7790_register_gpio(4);
 	r8a7790_register_gpio(5);
-	r8a7790_register_i2c(0);
-	r8a7790_register_i2c(1);
-	r8a7790_register_i2c(2);
-	r8a7790_register_i2c(3);
-	r8a7790_register_audio_dmac(0);
-	r8a7790_register_audio_dmac(1);
 }
 
 #define __R8A7790_SCIF(scif_type, _scscr, index, baseaddr, irq)		\
@@ -269,20 +263,17 @@ static const struct resource thermal_resources[] __initconst = {
 					thermal_resources,		\
 					ARRAY_SIZE(thermal_resources))
 
-static const struct sh_timer_config cmt00_platform_data __initconst = {
-	.name = "CMT00",
-	.timer_bit = 0,
-	.clockevent_rating = 80,
+static struct sh_timer_config cmt0_platform_data = {
+	.channels_mask = 0x60,
 };
 
-static const struct resource cmt00_resources[] __initconst = {
-	DEFINE_RES_MEM(0xffca0510, 0x0c),
-	DEFINE_RES_MEM(0xffca0500, 0x04),
-	DEFINE_RES_IRQ(gic_spi(142)), /* CMT0_0 */
+static struct resource cmt0_resources[] = {
+	DEFINE_RES_MEM(0xffca0000, 0x1004),
+	DEFINE_RES_IRQ(gic_spi(142)),
 };
 
 #define r8a7790_register_cmt(idx)					\
-	platform_device_register_resndata(&platform_bus, "sh_cmt",	\
+	platform_device_register_resndata(&platform_bus, "sh-cmt-48-gen2", \
 					  idx, cmt##idx##_resources,	\
 					  ARRAY_SIZE(cmt##idx##_resources), \
 					  &cmt##idx##_platform_data,	\
@@ -290,6 +281,11 @@ static const struct resource cmt00_resources[] __initconst = {
 
 void __init r8a7790_add_dt_devices(void)
 {
+	r8a7790_register_cmt(0);
+}
+
+void __init r8a7790_add_standard_devices(void)
+{
 	r8a7790_register_scif(0);
 	r8a7790_register_scif(1);
 	r8a7790_register_scif(2);
@@ -300,14 +296,15 @@ void __init r8a7790_add_dt_devices(void)
 	r8a7790_register_scif(7);
 	r8a7790_register_scif(8);
 	r8a7790_register_scif(9);
-	r8a7790_register_cmt(00);
-}
-
-void __init r8a7790_add_standard_devices(void)
-{
 	r8a7790_add_dt_devices();
 	r8a7790_register_irqc(0);
 	r8a7790_register_thermal();
+	r8a7790_register_i2c(0);
+	r8a7790_register_i2c(1);
+	r8a7790_register_i2c(2);
+	r8a7790_register_i2c(3);
+	r8a7790_register_audio_dmac(0);
+	r8a7790_register_audio_dmac(1);
 }
 
 void __init r8a7790_init_early(void)
diff --git a/arch/arm/mach-shmobile/setup-r8a7791.c b/arch/arm/mach-shmobile/setup-r8a7791.c
index e28404e4386..04a96ddb322 100644
--- a/arch/arm/mach-shmobile/setup-r8a7791.c
+++ b/arch/arm/mach-shmobile/setup-r8a7791.c
@@ -128,20 +128,17 @@ R8A7791_SCIFA(14, 0xe6c80000, gic_spi(31)); /* SCIFA5 */
 					  &scif##index##_platform_data,	       \
 					  sizeof(scif##index##_platform_data))
 
-static const struct sh_timer_config cmt00_platform_data __initconst = {
-	.name = "CMT00",
-	.timer_bit = 0,
-	.clockevent_rating = 80,
+static struct sh_timer_config cmt0_platform_data = {
+	.channels_mask = 0x60,
 };
 
-static const struct resource cmt00_resources[] __initconst = {
-	DEFINE_RES_MEM(0xffca0510, 0x0c),
-	DEFINE_RES_MEM(0xffca0500, 0x04),
-	DEFINE_RES_IRQ(gic_spi(142)), /* CMT0_0 */
+static struct resource cmt0_resources[] = {
+	DEFINE_RES_MEM(0xffca0000, 0x1004),
+	DEFINE_RES_IRQ(gic_spi(142)),
 };
 
 #define r8a7791_register_cmt(idx)					\
-	platform_device_register_resndata(&platform_bus, "sh_cmt",	\
+	platform_device_register_resndata(&platform_bus, "sh-cmt-48-gen2", \
 					  idx, cmt##idx##_resources,	\
 					  ARRAY_SIZE(cmt##idx##_resources), \
 					  &cmt##idx##_platform_data,	\
@@ -185,6 +182,11 @@ static const struct resource thermal_resources[] __initconst = {
 
 void __init r8a7791_add_dt_devices(void)
 {
+	r8a7791_register_cmt(0);
+}
+
+void __init r8a7791_add_standard_devices(void)
+{
 	r8a7791_register_scif(0);
 	r8a7791_register_scif(1);
 	r8a7791_register_scif(2);
@@ -200,23 +202,11 @@ void __init r8a7791_add_dt_devices(void)
 	r8a7791_register_scif(12);
 	r8a7791_register_scif(13);
 	r8a7791_register_scif(14);
-	r8a7791_register_cmt(00);
-}
-
-void __init r8a7791_add_standard_devices(void)
-{
 	r8a7791_add_dt_devices();
 	r8a7791_register_irqc(0);
 	r8a7791_register_thermal();
 }
 
-void __init r8a7791_init_early(void)
-{
-#ifndef CONFIG_ARM_ARCH_TIMER
-	shmobile_setup_delay(1300, 2, 4); /* Cortex-A15 @ 1300MHz */
-#endif
-}
-
 #ifdef CONFIG_USE_OF
 static const char *r8a7791_boards_compat_dt[] __initdata = {
 	"renesas,r8a7791",
@@ -225,7 +215,7 @@ static const char *r8a7791_boards_compat_dt[] __initdata = {
 
 DT_MACHINE_START(R8A7791_DT, "Generic R8A7791 (Flattened Device Tree)")
 	.smp		= smp_ops(r8a7791_smp_ops),
-	.init_early	= r8a7791_init_early,
+	.init_early	= shmobile_init_delay,
 	.init_time	= rcar_gen2_timer_init,
 	.dt_compat	= r8a7791_boards_compat_dt,
 MACHINE_END
diff --git a/arch/arm/mach-shmobile/setup-rcar-gen2.c b/arch/arm/mach-shmobile/setup-rcar-gen2.c
index 10604480f32..542c5a47173 100644
--- a/arch/arm/mach-shmobile/setup-rcar-gen2.c
+++ b/arch/arm/mach-shmobile/setup-rcar-gen2.c
@@ -30,12 +30,16 @@
 
 u32 rcar_gen2_read_mode_pins(void)
 {
-	void __iomem *modemr = ioremap_nocache(MODEMR, 4);
-	u32 mode;
-
-	BUG_ON(!modemr);
-	mode = ioread32(modemr);
-	iounmap(modemr);
+	static u32 mode;
+	static bool mode_valid;
+
+	if (!mode_valid) {
+		void __iomem *modemr = ioremap_nocache(MODEMR, 4);
+		BUG_ON(!modemr);
+		mode = ioread32(modemr);
+		iounmap(modemr);
+		mode_valid = true;
+	}
 
 	return mode;
 }
diff --git a/arch/arm/mach-shmobile/setup-sh7372.c b/arch/arm/mach-shmobile/setup-sh7372.c
index 27301278c20..2a8b9f2a2f5 100644
--- a/arch/arm/mach-shmobile/setup-sh7372.c
+++ b/arch/arm/mach-shmobile/setup-sh7372.c
@@ -119,28 +119,16 @@ SH7372_SCIF(PORT_SCIFB, 6, 0xe6c30000, evt2irq(0x0d60));
 
 /* CMT */
 static struct sh_timer_config cmt2_platform_data = {
-	.name = "CMT2",
-	.channel_offset = 0x40,
-	.timer_bit = 5,
-	.clockevent_rating = 125,
-	.clocksource_rating = 125,
+	.channels_mask = 0x20,
 };
 
 static struct resource cmt2_resources[] = {
-	[0] = {
-		.name	= "CMT2",
-		.start	= 0xe6130040,
-		.end	= 0xe613004b,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x0b80), /* CMT2 */
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xe6130000, 0x50),
+	DEFINE_RES_IRQ(evt2irq(0x0b80)),
 };
 
 static struct platform_device cmt2_device = {
-	.name		= "sh_cmt",
+	.name		= "sh-cmt-32-fast",
 	.id		= 2,
 	.dev = {
 		.platform_data	= &cmt2_platform_data,
@@ -150,64 +138,25 @@ static struct platform_device cmt2_device = {
 };
 
 /* TMU */
-static struct sh_timer_config tmu00_platform_data = {
-	.name = "TMU00",
-	.channel_offset = 0x4,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
+static struct sh_timer_config tmu0_platform_data = {
+	.channels_mask = 7,
 };
 
-static struct resource tmu00_resources[] = {
-	[0] = {
-		.name	= "TMU00",
-		.start	= 0xfff60008,
-		.end	= 0xfff60013,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= intcs_evt2irq(0xe80), /* TMU_TUNI0 */
-		.flags	= IORESOURCE_IRQ,
-	},
+static struct resource tmu0_resources[] = {
+	DEFINE_RES_MEM(0xfff60000, 0x2c),
+	DEFINE_RES_IRQ(intcs_evt2irq(0xe80)),
+	DEFINE_RES_IRQ(intcs_evt2irq(0xea0)),
+	DEFINE_RES_IRQ(intcs_evt2irq(0xec0)),
 };
 
-static struct platform_device tmu00_device = {
-	.name		= "sh_tmu",
+static struct platform_device tmu0_device = {
+	.name		= "sh-tmu",
 	.id		= 0,
 	.dev = {
-		.platform_data	= &tmu00_platform_data,
-	},
-	.resource	= tmu00_resources,
-	.num_resources	= ARRAY_SIZE(tmu00_resources),
-};
-
-static struct sh_timer_config tmu01_platform_data = {
-	.name = "TMU01",
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-	.clocksource_rating = 200,
-};
-
-static struct resource tmu01_resources[] = {
-	[0] = {
-		.name	= "TMU01",
-		.start	= 0xfff60014,
-		.end	= 0xfff6001f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= intcs_evt2irq(0xea0), /* TMU_TUNI1 */
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu01_device = {
-	.name		= "sh_tmu",
-	.id		= 1,
-	.dev = {
-		.platform_data	= &tmu01_platform_data,
+		.platform_data	= &tmu0_platform_data,
 	},
-	.resource	= tmu01_resources,
-	.num_resources	= ARRAY_SIZE(tmu01_resources),
+	.resource	= tmu0_resources,
+	.num_resources	= ARRAY_SIZE(tmu0_resources),
 };
 
 /* I2C */
@@ -952,8 +901,7 @@ static struct platform_device *sh7372_early_devices[] __initdata = {
 	&scif5_device,
 	&scif6_device,
 	&cmt2_device,
-	&tmu00_device,
-	&tmu01_device,
+	&tmu0_device,
 	&ipmmu_device,
 };
 
@@ -1000,8 +948,7 @@ void __init sh7372_add_standard_devices(void)
 		{ "A4R", &veu2_device, },
 		{ "A4R", &veu3_device, },
 		{ "A4R", &jpu_device, },
-		{ "A4R", &tmu00_device, },
-		{ "A4R", &tmu01_device, },
+		{ "A4R", &tmu0_device, },
 	};
 
 	sh7372_init_pm_domains();
@@ -1037,11 +984,7 @@ void __init sh7372_add_early_devices_dt(void)
 {
 	shmobile_setup_delay(800, 1, 3); /* Cortex-A8 @ 800MHz */
 
-	early_platform_add_devices(sh7372_early_devices,
-				   ARRAY_SIZE(sh7372_early_devices));
-
-	/* setup early console here as well */
-	shmobile_setup_console();
+	sh7372_add_early_devices();
 }
 
 void __init sh7372_add_standard_devices_dt(void)
diff --git a/arch/arm/mach-shmobile/setup-sh73a0.c b/arch/arm/mach-shmobile/setup-sh73a0.c
index f74ab530c71..ad00724a226 100644
--- a/arch/arm/mach-shmobile/setup-sh73a0.c
+++ b/arch/arm/mach-shmobile/setup-sh73a0.c
@@ -104,86 +104,45 @@ SH73A0_SCIF(PORT_SCIFA, 6, 0xe6cc0000, gic_spi(156));
 SH73A0_SCIF(PORT_SCIFA, 7, 0xe6cd0000, gic_spi(143));
 SH73A0_SCIF(PORT_SCIFB, 8, 0xe6c30000, gic_spi(80));
 
-static struct sh_timer_config cmt10_platform_data = {
-	.name = "CMT10",
-	.channel_offset = 0x10,
-	.timer_bit = 0,
-	.clockevent_rating = 80,
-	.clocksource_rating = 125,
+static struct sh_timer_config cmt1_platform_data = {
+	.channels_mask = 0x3f,
 };
 
-static struct resource cmt10_resources[] = {
-	[0] = {
-		.name	= "CMT10",
-		.start	= 0xe6138010,
-		.end	= 0xe613801b,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= gic_spi(65),
-		.flags	= IORESOURCE_IRQ,
-	},
+static struct resource cmt1_resources[] = {
+	DEFINE_RES_MEM(0xe6138000, 0x200),
+	DEFINE_RES_IRQ(gic_spi(65)),
 };
 
-static struct platform_device cmt10_device = {
-	.name		= "sh_cmt",
-	.id		= 10,
+static struct platform_device cmt1_device = {
+	.name		= "sh-cmt-48",
+	.id		= 1,
 	.dev = {
-		.platform_data	= &cmt10_platform_data,
+		.platform_data	= &cmt1_platform_data,
 	},
-	.resource	= cmt10_resources,
-	.num_resources	= ARRAY_SIZE(cmt10_resources),
+	.resource	= cmt1_resources,
+	.num_resources	= ARRAY_SIZE(cmt1_resources),
 };
 
 /* TMU */
-static struct sh_timer_config tmu00_platform_data = {
-	.name = "TMU00",
-	.channel_offset = 0x4,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
+static struct sh_timer_config tmu0_platform_data = {
+	.channels_mask = 7,
 };
 
-static struct resource tmu00_resources[] = {
-	[0] = DEFINE_RES_MEM(0xfff60008, 0xc),
-	[1] = {
-		.start	= intcs_evt2irq(0x0e80), /* TMU0_TUNI00 */
-		.flags	= IORESOURCE_IRQ,
-	},
+static struct resource tmu0_resources[] = {
+	DEFINE_RES_MEM(0xfff60000, 0x2c),
+	DEFINE_RES_IRQ(intcs_evt2irq(0xe80)),
+	DEFINE_RES_IRQ(intcs_evt2irq(0xea0)),
+	DEFINE_RES_IRQ(intcs_evt2irq(0xec0)),
 };
 
-static struct platform_device tmu00_device = {
-	.name		= "sh_tmu",
+static struct platform_device tmu0_device = {
+	.name		= "sh-tmu",
 	.id		= 0,
 	.dev = {
-		.platform_data	= &tmu00_platform_data,
-	},
-	.resource	= tmu00_resources,
-	.num_resources	= ARRAY_SIZE(tmu00_resources),
-};
-
-static struct sh_timer_config tmu01_platform_data = {
-	.name = "TMU01",
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-	.clocksource_rating = 200,
-};
-
-static struct resource tmu01_resources[] = {
-	[0] = DEFINE_RES_MEM(0xfff60014, 0xc),
-	[1] = {
-		.start	= intcs_evt2irq(0x0ea0), /* TMU0_TUNI01 */
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu01_device = {
-	.name		= "sh_tmu",
-	.id		= 1,
-	.dev = {
-		.platform_data	= &tmu01_platform_data,
+		.platform_data	= &tmu0_platform_data,
 	},
-	.resource	= tmu01_resources,
-	.num_resources	= ARRAY_SIZE(tmu01_resources),
+	.resource	= tmu0_resources,
+	.num_resources	= ARRAY_SIZE(tmu0_resources),
 };
 
 static struct resource i2c0_resources[] = {
@@ -746,12 +705,11 @@ static struct platform_device *sh73a0_devices_dt[] __initdata = {
 	&scif6_device,
 	&scif7_device,
 	&scif8_device,
-	&cmt10_device,
+	&cmt1_device,
 };
 
 static struct platform_device *sh73a0_early_devices[] __initdata = {
-	&tmu00_device,
-	&tmu01_device,
+	&tmu0_device,
 	&ipmmu_device,
 };
 
diff --git a/arch/arm/mach-shmobile/smp-emev2.c b/arch/arm/mach-shmobile/smp-emev2.c
index f2ca92308f7..2dfd748da7f 100644
--- a/arch/arm/mach-shmobile/smp-emev2.c
+++ b/arch/arm/mach-shmobile/smp-emev2.c
@@ -24,7 +24,6 @@
 #include <linux/io.h>
 #include <linux/delay.h>
 #include <mach/common.h>
-#include <mach/emev2.h>
 #include <asm/smp_plat.h>
 #include <asm/smp_scu.h>
 
diff --git a/arch/arm/mach-shmobile/smp-r8a7791.c b/arch/arm/mach-shmobile/smp-r8a7791.c
index 2df5bd190fe..ec979529f30 100644
--- a/arch/arm/mach-shmobile/smp-r8a7791.c
+++ b/arch/arm/mach-shmobile/smp-r8a7791.c
@@ -20,6 +20,7 @@
 #include <asm/smp_plat.h>
 #include <mach/common.h>
 #include <mach/r8a7791.h>
+#include <mach/rcar-gen2.h>
 
 #define RST		0xe6160000
 #define CA15BAR		0x0020
@@ -51,9 +52,21 @@ static void __init r8a7791_smp_prepare_cpus(unsigned int max_cpus)
 	iounmap(p);
 }
 
+static int r8a7791_smp_boot_secondary(unsigned int cpu,
+				      struct task_struct *idle)
+{
+	/* Error out when hardware debug mode is enabled */
+	if (rcar_gen2_read_mode_pins() & BIT(21)) {
+		pr_warn("Unable to boot CPU%u when MD21 is set\n", cpu);
+		return -ENOTSUPP;
+	}
+
+	return shmobile_smp_apmu_boot_secondary(cpu, idle);
+}
+
 struct smp_operations r8a7791_smp_ops __initdata = {
 	.smp_prepare_cpus	= r8a7791_smp_prepare_cpus,
-	.smp_boot_secondary	= shmobile_smp_apmu_boot_secondary,
+	.smp_boot_secondary	= r8a7791_smp_boot_secondary,
 #ifdef CONFIG_HOTPLUG_CPU
 	.cpu_disable		= shmobile_smp_cpu_disable,
 	.cpu_die		= shmobile_smp_apmu_cpu_die,
diff --git a/arch/arm/mach-shmobile/timer.c b/arch/arm/mach-shmobile/timer.c
index 62d7052d6f2..68bc0b82226 100644
--- a/arch/arm/mach-shmobile/timer.c
+++ b/arch/arm/mach-shmobile/timer.c
@@ -21,6 +21,24 @@
 #include <linux/platform_device.h>
 #include <linux/clocksource.h>
 #include <linux/delay.h>
+#include <linux/of_address.h>
+
+void __init shmobile_setup_delay_hz(unsigned int max_cpu_core_hz,
+				    unsigned int mult, unsigned int div)
+{
+	/* calculate a worst-case loops-per-jiffy value
+	 * based on maximum cpu core hz setting and the
+	 * __delay() implementation in arch/arm/lib/delay.S
+	 *
+	 * this will result in a longer delay than expected
+	 * when the cpu core runs on lower frequencies.
+	 */
+
+	unsigned int value = HZ * div / mult;
+
+	if (!preset_lpj)
+		preset_lpj = max_cpu_core_hz / value;
+}
 
 void __init shmobile_setup_delay(unsigned int max_cpu_core_mhz,
 				 unsigned int mult, unsigned int div)
@@ -39,6 +57,33 @@ void __init shmobile_setup_delay(unsigned int max_cpu_core_mhz,
 		preset_lpj = max_cpu_core_mhz * value;
 }
 
+void __init shmobile_init_delay(void)
+{
+	struct device_node *np, *parent;
+	u32 max_freq, freq;
+
+	max_freq = 0;
+
+	parent = of_find_node_by_path("/cpus");
+	if (parent) {
+		for_each_child_of_node(parent, np) {
+			if (!of_property_read_u32(np, "clock-frequency", &freq))
+				max_freq = max(max_freq, freq);
+		}
+		of_node_put(parent);
+	}
+
+	if (max_freq) {
+		if (of_find_compatible_node(NULL, NULL, "arm,cortex-a8"))
+			shmobile_setup_delay_hz(max_freq, 1, 3);
+		else if (of_find_compatible_node(NULL, NULL, "arm,cortex-a9"))
+			shmobile_setup_delay_hz(max_freq, 1, 3);
+		else if (of_find_compatible_node(NULL, NULL, "arm,cortex-a15"))
+			if (!IS_ENABLED(CONFIG_ARM_ARCH_TIMER))
+				shmobile_setup_delay_hz(max_freq, 2, 4);
+	}
+}
+
 static void __init shmobile_late_time_init(void)
 {
 	/*
diff --git a/arch/arm/mach-spear/headsmp.S b/arch/arm/mach-spear/headsmp.S
index ed85473a047..c52192dc3d9 100644
--- a/arch/arm/mach-spear/headsmp.S
+++ b/arch/arm/mach-spear/headsmp.S
@@ -3,7 +3,7 @@
  *
  * Picked from realview
  * Copyright (c) 2012 ST Microelectronics Limited
- * Shiraz Hashim <shiraz.hashim@st.com>
+ * Shiraz Hashim <shiraz.linux.kernel@gmail.com>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
diff --git a/arch/arm/mach-spear/platsmp.c b/arch/arm/mach-spear/platsmp.c
index 5c4a19887b2..c19751fff2c 100644
--- a/arch/arm/mach-spear/platsmp.c
+++ b/arch/arm/mach-spear/platsmp.c
@@ -4,7 +4,7 @@
  * based upon linux/arch/arm/mach-realview/platsmp.c
  *
  * Copyright (C) 2012 ST Microelectronics Ltd.
- * Shiraz Hashim <shiraz.hashim@st.com>
+ * Shiraz Hashim <shiraz.linux.kernel@gmail.com>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
diff --git a/arch/arm/mach-spear/time.c b/arch/arm/mach-spear/time.c
index 218ba5b67d9..26fda4ed4d5 100644
--- a/arch/arm/mach-spear/time.c
+++ b/arch/arm/mach-spear/time.c
@@ -2,7 +2,7 @@
  * arch/arm/plat-spear/time.c
  *
  * Copyright (C) 2010 ST Microelectronics
- * Shiraz Hashim<shiraz.hashim@st.com>
+ * Shiraz Hashim<shiraz.linux.kernel@gmail.com>
  *
  * This file is licensed under the terms of the GNU General Public
  * License version 2. This program is licensed "as is" without any
@@ -71,7 +71,7 @@ static void clockevent_set_mode(enum clock_event_mode mode,
 static int clockevent_next_event(unsigned long evt,
 				 struct clock_event_device *clk_event_dev);
 
-static void spear_clocksource_init(void)
+static void __init spear_clocksource_init(void)
 {
 	u32 tick_rate;
 	u16 val;
diff --git a/arch/arm/mach-sti/board-dt.c b/arch/arm/mach-sti/board-dt.c
index 1217fb598cf..df731f2322f 100644
--- a/arch/arm/mach-sti/board-dt.c
+++ b/arch/arm/mach-sti/board-dt.c
@@ -36,6 +36,7 @@ static void __init stih41x_machine_init(void)
 static const char *stih41x_dt_match[] __initdata = {
 	"st,stih415",
 	"st,stih416",
+	"st,stih407",
 	NULL
 };
 
diff --git a/arch/arm/mach-sunxi/Kconfig b/arch/arm/mach-sunxi/Kconfig
index b57d7d53b9d..0fbd4f156bf 100644
--- a/arch/arm/mach-sunxi/Kconfig
+++ b/arch/arm/mach-sunxi/Kconfig
@@ -1,14 +1,38 @@
-config ARCH_SUNXI
-	bool "Allwinner A1X SOCs" if ARCH_MULTI_V7
-	select ARCH_HAS_RESET_CONTROLLER
+menuconfig ARCH_SUNXI
+	bool "Allwinner SoCs" if ARCH_MULTI_V7
 	select ARCH_REQUIRE_GPIOLIB
-	select ARM_GIC
-	select ARM_PSCI
 	select CLKSRC_MMIO
 	select GENERIC_IRQ_CHIP
-	select HAVE_ARM_ARCH_TIMER
 	select PINCTRL
 	select PINCTRL_SUNXI
-	select RESET_CONTROLLER
 	select SUN4I_TIMER
+
+if ARCH_SUNXI
+
+config MACH_SUN4I
+	bool "Allwinner A10 (sun4i) SoCs support"
+	default ARCH_SUNXI
+
+config MACH_SUN5I
+	bool "Allwinner A10s / A13 (sun5i) SoCs support"
+	default ARCH_SUNXI
+	select SUN5I_HSTIMER
+
+config MACH_SUN6I
+	bool "Allwinner A31 (sun6i) SoCs support"
+	default ARCH_SUNXI
+	select ARCH_HAS_RESET_CONTROLLER
+	select ARM_GIC
+	select MFD_SUN6I_PRCM
+	select RESET_CONTROLLER
+	select SUN5I_HSTIMER
+
+config MACH_SUN7I
+	bool "Allwinner A20 (sun7i) SoCs support"
+	default ARCH_SUNXI
+	select ARM_GIC
+	select ARM_PSCI
+	select HAVE_ARM_ARCH_TIMER
 	select SUN5I_HSTIMER
+
+endif
diff --git a/arch/arm/mach-sunxi/common.h b/arch/arm/mach-sunxi/common.h
deleted file mode 100644
index 9e5ac4756cb..00000000000
--- a/arch/arm/mach-sunxi/common.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*
- * Core functions for Allwinner SoCs
- *
- * Copyright (C) 2013 Maxime Ripard
- *
- * Maxime Ripard <maxime.ripard@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 __ARCH_SUNXI_COMMON_H_
-#define __ARCH_SUNXI_COMMON_H_
-
-void sun6i_secondary_startup(void);
-extern struct smp_operations sun6i_smp_ops;
-
-#endif /* __ARCH_SUNXI_COMMON_H_ */
diff --git a/arch/arm/mach-sunxi/platsmp.c b/arch/arm/mach-sunxi/platsmp.c
index 0c7dbce033c..c53077bb8c3 100644
--- a/arch/arm/mach-sunxi/platsmp.c
+++ b/arch/arm/mach-sunxi/platsmp.c
@@ -21,8 +21,6 @@
 #include <linux/of_address.h>
 #include <linux/smp.h>
 
-#include "common.h"
-
 #define CPUCFG_CPU_PWR_CLAMP_STATUS_REG(cpu)	((cpu) * 0x40 + 0x64)
 #define CPUCFG_CPU_RST_CTRL_REG(cpu)		(((cpu) + 1) * 0x40)
 #define CPUCFG_CPU_CTRL_REG(cpu)		(((cpu) + 1) * 0x40 + 0x04)
@@ -122,3 +120,4 @@ struct smp_operations sun6i_smp_ops __initdata = {
 	.smp_prepare_cpus	= sun6i_smp_prepare_cpus,
 	.smp_boot_secondary	= sun6i_smp_boot_secondary,
 };
+CPU_METHOD_OF_DECLARE(sun6i_smp, "allwinner,sun6i-a31", &sun6i_smp_ops);
diff --git a/arch/arm/mach-sunxi/sunxi.c b/arch/arm/mach-sunxi/sunxi.c
index 460b5a4962e..3f9587bb51f 100644
--- a/arch/arm/mach-sunxi/sunxi.c
+++ b/arch/arm/mach-sunxi/sunxi.c
@@ -12,111 +12,8 @@
 
 #include <linux/clk-provider.h>
 #include <linux/clocksource.h>
-#include <linux/delay.h>
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/of_address.h>
-#include <linux/of_irq.h>
-#include <linux/of_platform.h>
-#include <linux/io.h>
-#include <linux/reboot.h>
 
 #include <asm/mach/arch.h>
-#include <asm/mach/map.h>
-#include <asm/system_misc.h>
-
-#include "common.h"
-
-#define SUN4I_WATCHDOG_CTRL_REG		0x00
-#define SUN4I_WATCHDOG_CTRL_RESTART		BIT(0)
-#define SUN4I_WATCHDOG_MODE_REG		0x04
-#define SUN4I_WATCHDOG_MODE_ENABLE		BIT(0)
-#define SUN4I_WATCHDOG_MODE_RESET_ENABLE	BIT(1)
-
-#define SUN6I_WATCHDOG1_IRQ_REG		0x00
-#define SUN6I_WATCHDOG1_CTRL_REG	0x10
-#define SUN6I_WATCHDOG1_CTRL_RESTART		BIT(0)
-#define SUN6I_WATCHDOG1_CONFIG_REG	0x14
-#define SUN6I_WATCHDOG1_CONFIG_RESTART		BIT(0)
-#define SUN6I_WATCHDOG1_CONFIG_IRQ		BIT(1)
-#define SUN6I_WATCHDOG1_MODE_REG	0x18
-#define SUN6I_WATCHDOG1_MODE_ENABLE		BIT(0)
-
-static void __iomem *wdt_base;
-
-static void sun4i_restart(enum reboot_mode mode, const char *cmd)
-{
-	if (!wdt_base)
-		return;
-
-	/* Enable timer and set reset bit in the watchdog */
-	writel(SUN4I_WATCHDOG_MODE_ENABLE | SUN4I_WATCHDOG_MODE_RESET_ENABLE,
-	       wdt_base + SUN4I_WATCHDOG_MODE_REG);
-
-	/*
-	 * Restart the watchdog. The default (and lowest) interval
-	 * value for the watchdog is 0.5s.
-	 */
-	writel(SUN4I_WATCHDOG_CTRL_RESTART, wdt_base + SUN4I_WATCHDOG_CTRL_REG);
-
-	while (1) {
-		mdelay(5);
-		writel(SUN4I_WATCHDOG_MODE_ENABLE | SUN4I_WATCHDOG_MODE_RESET_ENABLE,
-		       wdt_base + SUN4I_WATCHDOG_MODE_REG);
-	}
-}
-
-static void sun6i_restart(enum reboot_mode mode, const char *cmd)
-{
-	if (!wdt_base)
-		return;
-
-	/* Disable interrupts */
-	writel(0, wdt_base + SUN6I_WATCHDOG1_IRQ_REG);
-
-	/* We want to disable the IRQ and just reset the whole system */
-	writel(SUN6I_WATCHDOG1_CONFIG_RESTART,
-		wdt_base + SUN6I_WATCHDOG1_CONFIG_REG);
-
-	/* Enable timer. The default and lowest interval value is 0.5s */
-	writel(SUN6I_WATCHDOG1_MODE_ENABLE,
-		wdt_base + SUN6I_WATCHDOG1_MODE_REG);
-
-	/* Restart the watchdog. */
-	writel(SUN6I_WATCHDOG1_CTRL_RESTART,
-		wdt_base + SUN6I_WATCHDOG1_CTRL_REG);
-
-	while (1) {
-		mdelay(5);
-		writel(SUN6I_WATCHDOG1_MODE_ENABLE,
-			wdt_base + SUN6I_WATCHDOG1_MODE_REG);
-	}
-}
-
-static struct of_device_id sunxi_restart_ids[] = {
-	{ .compatible = "allwinner,sun4i-a10-wdt" },
-	{ .compatible = "allwinner,sun6i-a31-wdt" },
-	{ /*sentinel*/ }
-};
-
-static void sunxi_setup_restart(void)
-{
-	struct device_node *np;
-
-	np = of_find_matching_node(NULL, sunxi_restart_ids);
-	if (WARN(!np, "unable to setup watchdog restart"))
-		return;
-
-	wdt_base = of_iomap(np, 0);
-	WARN(!wdt_base, "failed to map watchdog base address");
-}
-
-static void __init sunxi_dt_init(void)
-{
-	sunxi_setup_restart();
-
-	of_platform_populate(NULL, of_default_bus_match_table, NULL, NULL);
-}
 
 static const char * const sunxi_board_dt_compat[] = {
 	"allwinner,sun4i-a10",
@@ -126,9 +23,7 @@ static const char * const sunxi_board_dt_compat[] = {
 };
 
 DT_MACHINE_START(SUNXI_DT, "Allwinner A1X (Device Tree)")
-	.init_machine	= sunxi_dt_init,
 	.dt_compat	= sunxi_board_dt_compat,
-	.restart	= sun4i_restart,
 MACHINE_END
 
 static const char * const sun6i_board_dt_compat[] = {
@@ -140,16 +35,14 @@ extern void __init sun6i_reset_init(void);
 static void __init sun6i_timer_init(void)
 {
 	of_clk_init(NULL);
-	sun6i_reset_init();
+	if (IS_ENABLED(CONFIG_RESET_CONTROLLER))
+		sun6i_reset_init();
 	clocksource_of_init();
 }
 
 DT_MACHINE_START(SUN6I_DT, "Allwinner sun6i (A31) Family")
-	.init_machine	= sunxi_dt_init,
 	.init_time	= sun6i_timer_init,
 	.dt_compat	= sun6i_board_dt_compat,
-	.restart	= sun6i_restart,
-	.smp		= smp_ops(sun6i_smp_ops),
 MACHINE_END
 
 static const char * const sun7i_board_dt_compat[] = {
@@ -158,7 +51,5 @@ static const char * const sun7i_board_dt_compat[] = {
 };
 
 DT_MACHINE_START(SUN7I_DT, "Allwinner sun7i (A20) Family")
-	.init_machine	= sunxi_dt_init,
 	.dt_compat	= sun7i_board_dt_compat,
-	.restart	= sun4i_restart,
 MACHINE_END
diff --git a/arch/arm/mach-tegra/Kconfig b/arch/arm/mach-tegra/Kconfig
index 92d660f9610..e16999e5b73 100644
--- a/arch/arm/mach-tegra/Kconfig
+++ b/arch/arm/mach-tegra/Kconfig
@@ -7,7 +7,6 @@ config ARCH_TEGRA
 	select CLKSRC_MMIO
 	select HAVE_ARM_SCU if SMP
 	select HAVE_ARM_TWD if SMP
-	select MIGHT_HAVE_PCI
 	select PINCTRL
 	select ARCH_HAS_RESET_CONTROLLER
 	select RESET_CONTROLLER
@@ -70,7 +69,4 @@ config TEGRA_AHB
 	  which controls AHB bus master arbitration and some
 	  performance parameters(priority, prefech size).
 
-config TEGRA_EMC_SCALING_ENABLE
-	bool "Enable scaling the memory frequency"
-
 endmenu
diff --git a/arch/arm/mach-tegra/pmc.c b/arch/arm/mach-tegra/pmc.c
index fb7920201ab..7c7123e7557 100644
--- a/arch/arm/mach-tegra/pmc.c
+++ b/arch/arm/mach-tegra/pmc.c
@@ -41,6 +41,14 @@
 #define PMC_REMOVE_CLAMPING		0x34
 #define PMC_PWRGATE_STATUS		0x38
 
+#define PMC_SCRATCH0			0x50
+#define PMC_SCRATCH0_MODE_RECOVERY	(1 << 31)
+#define PMC_SCRATCH0_MODE_BOOTLOADER	(1 << 30)
+#define PMC_SCRATCH0_MODE_RCM		(1 << 1)
+#define PMC_SCRATCH0_MODE_MASK		(PMC_SCRATCH0_MODE_RECOVERY | \
+					 PMC_SCRATCH0_MODE_BOOTLOADER | \
+					 PMC_SCRATCH0_MODE_RCM)
+
 #define PMC_CPUPWRGOOD_TIMER	0xc8
 #define PMC_CPUPWROFF_TIMER	0xcc
 
@@ -165,6 +173,22 @@ void tegra_pmc_restart(enum reboot_mode mode, const char *cmd)
 {
 	u32 val;
 
+	val = tegra_pmc_readl(PMC_SCRATCH0);
+	val &= ~PMC_SCRATCH0_MODE_MASK;
+
+	if (cmd) {
+		if (strcmp(cmd, "recovery") == 0)
+			val |= PMC_SCRATCH0_MODE_RECOVERY;
+
+		if (strcmp(cmd, "bootloader") == 0)
+			val |= PMC_SCRATCH0_MODE_BOOTLOADER;
+
+		if (strcmp(cmd, "forced-recovery") == 0)
+			val |= PMC_SCRATCH0_MODE_RCM;
+	}
+
+	tegra_pmc_writel(val, PMC_SCRATCH0);
+
 	val = tegra_pmc_readl(0);
 	val |= 0x10;
 	tegra_pmc_writel(val, 0);
diff --git a/arch/arm/mach-ux500/Makefile b/arch/arm/mach-ux500/Makefile
index de544aabf29..9741de956b3 100644
--- a/arch/arm/mach-ux500/Makefile
+++ b/arch/arm/mach-ux500/Makefile
@@ -5,8 +5,7 @@
 obj-y				:= cpu.o id.o timer.o pm.o
 obj-$(CONFIG_CACHE_L2X0)	+= cache-l2x0.o
 obj-$(CONFIG_UX500_SOC_DB8500)	+= cpu-db8500.o
-obj-$(CONFIG_MACH_MOP500)	+= board-mop500-sdi.o \
-				board-mop500-regulators.o \
+obj-$(CONFIG_MACH_MOP500)	+= board-mop500-regulators.o \
 				board-mop500-audio.o
 obj-$(CONFIG_SMP)		+= platsmp.o headsmp.o
 obj-$(CONFIG_HOTPLUG_CPU)	+= hotplug.o
diff --git a/arch/arm/mach-ux500/board-mop500-sdi.c b/arch/arm/mach-ux500/board-mop500-sdi.c
deleted file mode 100644
index fcbf3a13a53..00000000000
--- a/arch/arm/mach-ux500/board-mop500-sdi.c
+++ /dev/null
@@ -1,166 +0,0 @@
-/*
- * Copyright (C) ST-Ericsson SA 2010
- *
- * Author: Hanumath Prasad <hanumath.prasad@stericsson.com>
- * License terms: GNU General Public License (GPL) version 2
- */
-
-#include <linux/kernel.h>
-#include <linux/gpio.h>
-#include <linux/amba/bus.h>
-#include <linux/amba/mmci.h>
-#include <linux/mmc/host.h>
-#include <linux/platform_device.h>
-#include <linux/platform_data/dma-ste-dma40.h>
-
-#include <asm/mach-types.h>
-
-#include "db8500-regs.h"
-#include "board-mop500.h"
-#include "ste-dma40-db8500.h"
-
-/*
- * v2 has a new version of this block that need to be forced, the number found
- * in hardware is incorrect
- */
-#define U8500_SDI_V2_PERIPHID 0x10480180
-
-/*
- * SDI 0 (MicroSD slot)
- */
-
-#ifdef CONFIG_STE_DMA40
-struct stedma40_chan_cfg mop500_sdi0_dma_cfg_rx = {
-	.mode = STEDMA40_MODE_LOGICAL,
-	.dir = DMA_DEV_TO_MEM,
-	.dev_type = DB8500_DMA_DEV29_SD_MM0,
-};
-
-static struct stedma40_chan_cfg mop500_sdi0_dma_cfg_tx = {
-	.mode = STEDMA40_MODE_LOGICAL,
-	.dir = DMA_MEM_TO_DEV,
-	.dev_type = DB8500_DMA_DEV29_SD_MM0,
-};
-#endif
-
-struct mmci_platform_data mop500_sdi0_data = {
-	.f_max		= 100000000,
-	.capabilities	= MMC_CAP_4_BIT_DATA |
-				MMC_CAP_SD_HIGHSPEED |
-				MMC_CAP_MMC_HIGHSPEED |
-				MMC_CAP_ERASE |
-				MMC_CAP_UHS_SDR12 |
-				MMC_CAP_UHS_SDR25,
-	.gpio_wp	= -1,
-	.sigdir		= MCI_ST_FBCLKEN |
-				MCI_ST_CMDDIREN |
-				MCI_ST_DATA0DIREN |
-				MCI_ST_DATA2DIREN,
-#ifdef CONFIG_STE_DMA40
-	.dma_filter	= stedma40_filter,
-	.dma_rx_param	= &mop500_sdi0_dma_cfg_rx,
-	.dma_tx_param	= &mop500_sdi0_dma_cfg_tx,
-#endif
-};
-
-/*
- * SDI1 (SDIO WLAN)
- */
-#ifdef CONFIG_STE_DMA40
-static struct stedma40_chan_cfg sdi1_dma_cfg_rx = {
-	.mode = STEDMA40_MODE_LOGICAL,
-	.dir = DMA_DEV_TO_MEM,
-	.dev_type = DB8500_DMA_DEV32_SD_MM1,
-};
-
-static struct stedma40_chan_cfg sdi1_dma_cfg_tx = {
-	.mode = STEDMA40_MODE_LOGICAL,
-	.dir = DMA_MEM_TO_DEV,
-	.dev_type = DB8500_DMA_DEV32_SD_MM1,
-};
-#endif
-
-struct mmci_platform_data mop500_sdi1_data = {
-	.ocr_mask	= MMC_VDD_29_30,
-	.f_max		= 100000000,
-	.capabilities	= MMC_CAP_4_BIT_DATA |
-				MMC_CAP_NONREMOVABLE,
-	.gpio_cd	= -1,
-	.gpio_wp	= -1,
-#ifdef CONFIG_STE_DMA40
-	.dma_filter	= stedma40_filter,
-	.dma_rx_param	= &sdi1_dma_cfg_rx,
-	.dma_tx_param	= &sdi1_dma_cfg_tx,
-#endif
-};
-
-/*
- * SDI 2 (POP eMMC, not on DB8500ed)
- */
-
-#ifdef CONFIG_STE_DMA40
-struct stedma40_chan_cfg mop500_sdi2_dma_cfg_rx = {
-	.mode = STEDMA40_MODE_LOGICAL,
-	.dir = DMA_DEV_TO_MEM,
-	.dev_type =  DB8500_DMA_DEV28_SD_MM2,
-};
-
-static struct stedma40_chan_cfg mop500_sdi2_dma_cfg_tx = {
-	.mode = STEDMA40_MODE_LOGICAL,
-	.dir = DMA_MEM_TO_DEV,
-	.dev_type = DB8500_DMA_DEV28_SD_MM2,
-};
-#endif
-
-struct mmci_platform_data mop500_sdi2_data = {
-	.ocr_mask	= MMC_VDD_165_195,
-	.f_max		= 100000000,
-	.capabilities	= MMC_CAP_4_BIT_DATA |
-				MMC_CAP_8_BIT_DATA |
-				MMC_CAP_NONREMOVABLE |
-				MMC_CAP_MMC_HIGHSPEED |
-				MMC_CAP_ERASE |
-				MMC_CAP_CMD23,
-	.gpio_cd	= -1,
-	.gpio_wp	= -1,
-#ifdef CONFIG_STE_DMA40
-	.dma_filter	= stedma40_filter,
-	.dma_rx_param	= &mop500_sdi2_dma_cfg_rx,
-	.dma_tx_param	= &mop500_sdi2_dma_cfg_tx,
-#endif
-};
-
-/*
- * SDI 4 (on-board eMMC)
- */
-
-#ifdef CONFIG_STE_DMA40
-struct stedma40_chan_cfg mop500_sdi4_dma_cfg_rx = {
-	.mode = STEDMA40_MODE_LOGICAL,
-	.dir = DMA_DEV_TO_MEM,
-	.dev_type =  DB8500_DMA_DEV42_SD_MM4,
-};
-
-static struct stedma40_chan_cfg mop500_sdi4_dma_cfg_tx = {
-	.mode = STEDMA40_MODE_LOGICAL,
-	.dir = DMA_MEM_TO_DEV,
-	.dev_type = DB8500_DMA_DEV42_SD_MM4,
-};
-#endif
-
-struct mmci_platform_data mop500_sdi4_data = {
-	.f_max		= 100000000,
-	.capabilities	= MMC_CAP_4_BIT_DATA |
-				MMC_CAP_8_BIT_DATA |
-				MMC_CAP_NONREMOVABLE |
-				MMC_CAP_MMC_HIGHSPEED |
-				MMC_CAP_ERASE |
-				MMC_CAP_CMD23,
-	.gpio_cd	= -1,
-	.gpio_wp	= -1,
-#ifdef CONFIG_STE_DMA40
-	.dma_filter	= stedma40_filter,
-	.dma_rx_param	= &mop500_sdi4_dma_cfg_rx,
-	.dma_tx_param	= &mop500_sdi4_dma_cfg_tx,
-#endif
-};
diff --git a/arch/arm/mach-ux500/board-mop500.h b/arch/arm/mach-ux500/board-mop500.h
index 32cc0d8d8a0..7c7b0adca58 100644
--- a/arch/arm/mach-ux500/board-mop500.h
+++ b/arch/arm/mach-ux500/board-mop500.h
@@ -8,12 +8,7 @@
 #define __BOARD_MOP500_H
 
 #include <linux/platform_data/asoc-ux500-msp.h>
-#include <linux/amba/mmci.h>
 
-extern struct mmci_platform_data mop500_sdi0_data;
-extern struct mmci_platform_data mop500_sdi1_data;
-extern struct mmci_platform_data mop500_sdi2_data;
-extern struct mmci_platform_data mop500_sdi4_data;
 extern struct msp_i2s_platform_data msp0_platform_data;
 extern struct msp_i2s_platform_data msp1_platform_data;
 extern struct msp_i2s_platform_data msp2_platform_data;
diff --git a/arch/arm/mach-ux500/cpu-db8500.c b/arch/arm/mach-ux500/cpu-db8500.c
index 8820f602fcd..fa308f07fae 100644
--- a/arch/arm/mach-ux500/cpu-db8500.c
+++ b/arch/arm/mach-ux500/cpu-db8500.c
@@ -146,10 +146,6 @@ static struct of_dev_auxdata u8500_auxdata_lookup[] __initdata = {
 	/* Requires call-back bindings. */
 	OF_DEV_AUXDATA("arm,cortex-a9-pmu", 0, "arm-pmu", &db8500_pmu_platdata),
 	/* Requires DMA bindings. */
-	OF_DEV_AUXDATA("arm,pl18x", 0x80126000, "sdi0",  &mop500_sdi0_data),
-	OF_DEV_AUXDATA("arm,pl18x", 0x80118000, "sdi1",  &mop500_sdi1_data),
-	OF_DEV_AUXDATA("arm,pl18x", 0x80005000, "sdi2",  &mop500_sdi2_data),
-	OF_DEV_AUXDATA("arm,pl18x", 0x80114000, "sdi4",  &mop500_sdi4_data),
 	OF_DEV_AUXDATA("stericsson,ux500-msp-i2s", 0x80123000,
 		       "ux500-msp-i2s.0", &msp0_platform_data),
 	OF_DEV_AUXDATA("stericsson,ux500-msp-i2s", 0x80124000,
diff --git a/arch/arm/mach-versatile/core.c b/arch/arm/mach-versatile/core.c
index f2c89fb8fca..be83ba25f81 100644
--- a/arch/arm/mach-versatile/core.c
+++ b/arch/arm/mach-versatile/core.c
@@ -310,6 +310,21 @@ static struct platform_device char_lcd_device = {
 	.resource       =       char_lcd_resources,
 };
 
+static struct resource leds_resources[] = {
+	{
+		.start	= VERSATILE_SYS_BASE + VERSATILE_SYS_LED_OFFSET,
+		.end	= VERSATILE_SYS_BASE + VERSATILE_SYS_LED_OFFSET + 4,
+		.flags	= IORESOURCE_MEM,
+	},
+};
+
+static struct platform_device leds_device = {
+	.name		= "versatile-leds",
+	.id		= -1,
+	.num_resources	= ARRAY_SIZE(leds_resources),
+	.resource	= leds_resources,
+};
+
 /*
  * Clock handling
  */
@@ -795,6 +810,7 @@ void __init versatile_init(void)
 	platform_device_register(&versatile_i2c_device);
 	platform_device_register(&smc91x_device);
 	platform_device_register(&char_lcd_device);
+	platform_device_register(&leds_device);
 
 	for (i = 0; i < ARRAY_SIZE(amba_devs); i++) {
 		struct amba_device *d = amba_devs[i];
diff --git a/arch/arm/mach-vexpress/Kconfig b/arch/arm/mach-vexpress/Kconfig
index 657d52d0391..b8ac752fd24 100644
--- a/arch/arm/mach-vexpress/Kconfig
+++ b/arch/arm/mach-vexpress/Kconfig
@@ -18,6 +18,8 @@ config ARCH_VEXPRESS
 	select POWER_SUPPLY
 	select REGULATOR_FIXED_VOLTAGE if REGULATOR
 	select VEXPRESS_CONFIG
+	select VEXPRESS_SYSCFG
+	select MFD_VEXPRESS_SYSREG
 	help
 	  This option enables support for systems using Cortex processor based
 	  ARM core and logic (FPGA) tiles on the Versatile Express motherboard,
diff --git a/arch/arm/mach-vexpress/core.h b/arch/arm/mach-vexpress/core.h
index bde4374ab6d..152fad91b3a 100644
--- a/arch/arm/mach-vexpress/core.h
+++ b/arch/arm/mach-vexpress/core.h
@@ -4,10 +4,9 @@
 /* Tile's peripherals static mappings should start here */
 #define V2T_PERIPH 0xf8200000
 
-void vexpress_dt_smp_map_io(void);
-
 bool vexpress_smp_init_ops(void);
 
 extern struct smp_operations	vexpress_smp_ops;
+extern struct smp_operations	vexpress_smp_dt_ops;
 
 extern void vexpress_cpu_die(unsigned int cpu);
diff --git a/arch/arm/mach-vexpress/ct-ca9x4.c b/arch/arm/mach-vexpress/ct-ca9x4.c
index 6f34497a424..494d70bfdda 100644
--- a/arch/arm/mach-vexpress/ct-ca9x4.c
+++ b/arch/arm/mach-vexpress/ct-ca9x4.c
@@ -128,6 +128,10 @@ static struct platform_device pmu_device = {
 	.resource	= pmu_resources,
 };
 
+static struct clk_lookup osc1_lookup = {
+	.dev_id		= "ct:clcd",
+};
+
 static struct platform_device osc1_device = {
 	.name		= "vexpress-osc",
 	.id		= 1,
@@ -135,6 +139,7 @@ static struct platform_device osc1_device = {
 	.resource	= (struct resource []) {
 		VEXPRESS_RES_FUNC(0xf, 1),
 	},
+	.dev.platform_data = &osc1_lookup,
 };
 
 static void __init ct_ca9x4_init(void)
@@ -155,10 +160,7 @@ static void __init ct_ca9x4_init(void)
 		amba_device_register(ct_ca9x4_amba_devs[i], &iomem_resource);
 
 	platform_device_register(&pmu_device);
-	platform_device_register(&osc1_device);
-
-	WARN_ON(clk_register_clkdev(vexpress_osc_setup(&osc1_device.dev),
-			NULL, "ct:clcd"));
+	vexpress_syscfg_device_register(&osc1_device);
 }
 
 #ifdef CONFIG_SMP
diff --git a/arch/arm/mach-vexpress/dcscb.c b/arch/arm/mach-vexpress/dcscb.c
index 788495d35cf..30b993399ed 100644
--- a/arch/arm/mach-vexpress/dcscb.c
+++ b/arch/arm/mach-vexpress/dcscb.c
@@ -51,12 +51,14 @@ static int dcscb_allcpus_mask[2];
 static int dcscb_power_up(unsigned int cpu, unsigned int cluster)
 {
 	unsigned int rst_hold, cpumask = (1 << cpu);
-	unsigned int all_mask = dcscb_allcpus_mask[cluster];
+	unsigned int all_mask;
 
 	pr_debug("%s: cpu %u cluster %u\n", __func__, cpu, cluster);
 	if (cpu >= 4 || cluster >= 2)
 		return -EINVAL;
 
+	all_mask = dcscb_allcpus_mask[cluster];
+
 	/*
 	 * Since this is called with IRQs enabled, and no arch_spin_lock_irq
 	 * variant exists, we need to disable IRQs manually here.
@@ -101,11 +103,12 @@ static void dcscb_power_down(void)
 	cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0);
 	cluster = MPIDR_AFFINITY_LEVEL(mpidr, 1);
 	cpumask = (1 << cpu);
-	all_mask = dcscb_allcpus_mask[cluster];
 
 	pr_debug("%s: cpu %u cluster %u\n", __func__, cpu, cluster);
 	BUG_ON(cpu >= 4 || cluster >= 2);
 
+	all_mask = dcscb_allcpus_mask[cluster];
+
 	__mcpm_cpu_going_down(cpu, cluster);
 
 	arch_spin_lock(&dcscb_lock);
diff --git a/arch/arm/mach-vexpress/platsmp.c b/arch/arm/mach-vexpress/platsmp.c
index 993c9ae5dc5..a1f3804fd5a 100644
--- a/arch/arm/mach-vexpress/platsmp.c
+++ b/arch/arm/mach-vexpress/platsmp.c
@@ -12,8 +12,7 @@
 #include <linux/errno.h>
 #include <linux/smp.h>
 #include <linux/io.h>
-#include <linux/of.h>
-#include <linux/of_fdt.h>
+#include <linux/of_address.h>
 #include <linux/vexpress.h>
 
 #include <asm/mcpm.h>
@@ -26,154 +25,13 @@
 
 #include "core.h"
 
-#if defined(CONFIG_OF)
-
-static enum {
-	GENERIC_SCU,
-	CORTEX_A9_SCU,
-} vexpress_dt_scu __initdata = GENERIC_SCU;
-
-static struct map_desc vexpress_dt_cortex_a9_scu_map __initdata = {
-	.virtual	= V2T_PERIPH,
-	/* .pfn	set in vexpress_dt_init_cortex_a9_scu() */
-	.length		= SZ_128,
-	.type		= MT_DEVICE,
-};
-
-static void *vexpress_dt_cortex_a9_scu_base __initdata;
-
-const static char *vexpress_dt_cortex_a9_match[] __initconst = {
-	"arm,cortex-a5-scu",
-	"arm,cortex-a9-scu",
-	NULL
-};
-
-static int __init vexpress_dt_find_scu(unsigned long node,
-		const char *uname, int depth, void *data)
-{
-	if (of_flat_dt_match(node, vexpress_dt_cortex_a9_match)) {
-		phys_addr_t phys_addr;
-		__be32 *reg = of_get_flat_dt_prop(node, "reg", NULL);
-
-		if (WARN_ON(!reg))
-			return -EINVAL;
-
-		phys_addr = be32_to_cpup(reg);
-		vexpress_dt_scu = CORTEX_A9_SCU;
-
-		vexpress_dt_cortex_a9_scu_map.pfn = __phys_to_pfn(phys_addr);
-		iotable_init(&vexpress_dt_cortex_a9_scu_map, 1);
-		vexpress_dt_cortex_a9_scu_base = ioremap(phys_addr, SZ_256);
-		if (WARN_ON(!vexpress_dt_cortex_a9_scu_base))
-			return -EFAULT;
-	}
-
-	return 0;
-}
-
-void __init vexpress_dt_smp_map_io(void)
-{
-	if (initial_boot_params)
-		WARN_ON(of_scan_flat_dt(vexpress_dt_find_scu, NULL));
-}
-
-static int __init vexpress_dt_cpus_num(unsigned long node, const char *uname,
-		int depth, void *data)
-{
-	static int prev_depth = -1;
-	static int nr_cpus = -1;
-
-	if (prev_depth > depth && nr_cpus > 0)
-		return nr_cpus;
-
-	if (nr_cpus < 0 && strcmp(uname, "cpus") == 0)
-		nr_cpus = 0;
-
-	if (nr_cpus >= 0) {
-		const char *device_type = of_get_flat_dt_prop(node,
-				"device_type", NULL);
-
-		if (device_type && strcmp(device_type, "cpu") == 0)
-			nr_cpus++;
-	}
-
-	prev_depth = depth;
-
-	return 0;
-}
-
-static void __init vexpress_dt_smp_init_cpus(void)
-{
-	int ncores = 0, i;
-
-	switch (vexpress_dt_scu) {
-	case GENERIC_SCU:
-		ncores = of_scan_flat_dt(vexpress_dt_cpus_num, NULL);
-		break;
-	case CORTEX_A9_SCU:
-		ncores = scu_get_core_count(vexpress_dt_cortex_a9_scu_base);
-		break;
-	default:
-		WARN_ON(1);
-		break;
-	}
-
-	if (ncores < 2)
-		return;
-
-	if (ncores > nr_cpu_ids) {
-		pr_warn("SMP: %u cores greater than maximum (%u), clipping\n",
-				ncores, nr_cpu_ids);
-		ncores = nr_cpu_ids;
-	}
-
-	for (i = 0; i < ncores; ++i)
-		set_cpu_possible(i, true);
-}
-
-static void __init vexpress_dt_smp_prepare_cpus(unsigned int max_cpus)
-{
-	int i;
-
-	switch (vexpress_dt_scu) {
-	case GENERIC_SCU:
-		for (i = 0; i < max_cpus; i++)
-			set_cpu_present(i, true);
-		break;
-	case CORTEX_A9_SCU:
-		scu_enable(vexpress_dt_cortex_a9_scu_base);
-		break;
-	default:
-		WARN_ON(1);
-		break;
-	}
-}
-
-#else
-
-static void __init vexpress_dt_smp_init_cpus(void)
-{
-	WARN_ON(1);
-}
-
-void __init vexpress_dt_smp_prepare_cpus(unsigned int max_cpus)
-{
-	WARN_ON(1);
-}
-
-#endif
-
 /*
  * Initialise the CPU possible map early - this describes the CPUs
  * which may be present or become present in the system.
  */
 static void __init vexpress_smp_init_cpus(void)
 {
-	if (ct_desc)
-		ct_desc->init_cpu_map();
-	else
-		vexpress_dt_smp_init_cpus();
-
+	ct_desc->init_cpu_map();
 }
 
 static void __init vexpress_smp_prepare_cpus(unsigned int max_cpus)
@@ -182,10 +40,7 @@ static void __init vexpress_smp_prepare_cpus(unsigned int max_cpus)
 	 * Initialise the present map, which describes the set of CPUs
 	 * actually populated at the present time.
 	 */
-	if (ct_desc)
-		ct_desc->smp_enable(max_cpus);
-	else
-		vexpress_dt_smp_prepare_cpus(max_cpus);
+	ct_desc->smp_enable(max_cpus);
 
 	/*
 	 * Write the address of secondary startup into the
@@ -223,3 +78,39 @@ bool __init vexpress_smp_init_ops(void)
 #endif
 	return false;
 }
+
+#if defined(CONFIG_OF)
+
+static const struct of_device_id vexpress_smp_dt_scu_match[] __initconst = {
+	{ .compatible = "arm,cortex-a5-scu", },
+	{ .compatible = "arm,cortex-a9-scu", },
+	{}
+};
+
+static void __init vexpress_smp_dt_prepare_cpus(unsigned int max_cpus)
+{
+	struct device_node *scu = of_find_matching_node(NULL,
+			vexpress_smp_dt_scu_match);
+
+	if (scu)
+		scu_enable(of_iomap(scu, 0));
+
+	/*
+	 * Write the address of secondary startup into the
+	 * system-wide flags register. The boot monitor waits
+	 * until it receives a soft interrupt, and then the
+	 * secondary CPU branches to this address.
+	 */
+	vexpress_flags_set(virt_to_phys(versatile_secondary_startup));
+}
+
+struct smp_operations __initdata vexpress_smp_dt_ops = {
+	.smp_prepare_cpus	= vexpress_smp_dt_prepare_cpus,
+	.smp_secondary_init	= versatile_secondary_init,
+	.smp_boot_secondary	= versatile_boot_secondary,
+#ifdef CONFIG_HOTPLUG_CPU
+	.cpu_die		= vexpress_cpu_die,
+#endif
+};
+
+#endif
diff --git a/arch/arm/mach-vexpress/spc.c b/arch/arm/mach-vexpress/spc.c
index c26ef5b92ca..2c2754e79cb 100644
--- a/arch/arm/mach-vexpress/spc.c
+++ b/arch/arm/mach-vexpress/spc.c
@@ -392,7 +392,7 @@ static irqreturn_t ve_spc_irq_handler(int irq, void *data)
  *  +--------------------------+
  *  | 31      20 | 19        0 |
  *  +--------------------------+
- *  |   u_volt   |  freq(kHz)  |
+ *  |   m_volt   |  freq(kHz)  |
  *  +--------------------------+
  */
 #define MULT_FACTOR	20
@@ -414,7 +414,7 @@ static int ve_spc_populate_opps(uint32_t cluster)
 		ret = ve_spc_read_sys_cfg(SYSCFG_SCC, off, &data);
 		if (!ret) {
 			opps->freq = (data & FREQ_MASK) * MULT_FACTOR;
-			opps->u_volt = data >> VOLT_SHIFT;
+			opps->u_volt = (data >> VOLT_SHIFT) * 1000;
 		} else {
 			break;
 		}
diff --git a/arch/arm/mach-vexpress/v2m.c b/arch/arm/mach-vexpress/v2m.c
index 4f8b8cb17ff..38f4f6f3777 100644
--- a/arch/arm/mach-vexpress/v2m.c
+++ b/arch/arm/mach-vexpress/v2m.c
@@ -201,8 +201,9 @@ static struct platform_device v2m_cf_device = {
 
 static struct mmci_platform_data v2m_mmci_data = {
 	.ocr_mask	= MMC_VDD_32_33|MMC_VDD_33_34,
-	.gpio_wp	= VEXPRESS_GPIO_MMC_WPROT,
-	.gpio_cd	= VEXPRESS_GPIO_MMC_CARDIN,
+	.status		= vexpress_get_mci_cardin,
+	.gpio_cd	= -1,
+	.gpio_wp	= -1,
 };
 
 static struct resource v2m_sysreg_resources[] = {
@@ -340,11 +341,6 @@ static void __init v2m_init(void)
 	regulator_register_fixed(0, v2m_eth_supplies,
 			ARRAY_SIZE(v2m_eth_supplies));
 
-	platform_device_register(&v2m_muxfpga_device);
-	platform_device_register(&v2m_shutdown_device);
-	platform_device_register(&v2m_reboot_device);
-	platform_device_register(&v2m_dvimode_device);
-
 	platform_device_register(&v2m_sysreg_device);
 	platform_device_register(&v2m_pcie_i2c_device);
 	platform_device_register(&v2m_ddc_i2c_device);
@@ -356,6 +352,11 @@ static void __init v2m_init(void)
 	for (i = 0; i < ARRAY_SIZE(v2m_amba_devs); i++)
 		amba_device_register(v2m_amba_devs[i], &iomem_resource);
 
+	vexpress_syscfg_device_register(&v2m_muxfpga_device);
+	vexpress_syscfg_device_register(&v2m_shutdown_device);
+	vexpress_syscfg_device_register(&v2m_reboot_device);
+	vexpress_syscfg_device_register(&v2m_dvimode_device);
+
 	ct_desc->init_tile();
 }
 
@@ -369,71 +370,10 @@ MACHINE_START(VEXPRESS, "ARM-Versatile Express")
 	.init_machine	= v2m_init,
 MACHINE_END
 
-static struct map_desc v2m_rs1_io_desc __initdata = {
-	.virtual	= V2M_PERIPH,
-	.pfn		= __phys_to_pfn(0x1c000000),
-	.length		= SZ_2M,
-	.type		= MT_DEVICE,
-};
-
-static int __init v2m_dt_scan_memory_map(unsigned long node, const char *uname,
-		int depth, void *data)
-{
-	const char **map = data;
-
-	if (strcmp(uname, "motherboard") != 0)
-		return 0;
-
-	*map = of_get_flat_dt_prop(node, "arm,v2m-memory-map", NULL);
-
-	return 1;
-}
-
-void __init v2m_dt_map_io(void)
-{
-	const char *map = NULL;
-
-	of_scan_flat_dt(v2m_dt_scan_memory_map, &map);
-
-	if (map && strcmp(map, "rs1") == 0)
-		iotable_init(&v2m_rs1_io_desc, 1);
-	else
-		iotable_init(v2m_io_desc, ARRAY_SIZE(v2m_io_desc));
-
-#if defined(CONFIG_SMP)
-	vexpress_dt_smp_map_io();
-#endif
-}
-
-void __init v2m_dt_init_early(void)
-{
-	u32 dt_hbi;
-
-	vexpress_sysreg_of_early_init();
-
-	/* Confirm board type against DT property, if available */
-	if (of_property_read_u32(of_allnodes, "arm,hbi", &dt_hbi) == 0) {
-		u32 hbi = vexpress_get_hbi(VEXPRESS_SITE_MASTER);
-
-		if (WARN_ON(dt_hbi != hbi))
-			pr_warning("vexpress: DT HBI (%x) is not matching "
-					"hardware (%x)!\n", dt_hbi, hbi);
-	}
-
-	versatile_sched_clock_init(vexpress_get_24mhz_clock_base(), 24000000);
-}
-
-static const struct of_device_id v2m_dt_bus_match[] __initconst = {
-	{ .compatible = "simple-bus", },
-	{ .compatible = "arm,amba-bus", },
-	{ .compatible = "arm,vexpress,config-bus", },
-	{}
-};
-
 static void __init v2m_dt_init(void)
 {
 	l2x0_of_init(0x00400000, 0xfe0fffff);
-	of_platform_populate(NULL, v2m_dt_bus_match, NULL, NULL);
+	of_platform_populate(NULL, of_default_bus_match_table, NULL, NULL);
 }
 
 static const char * const v2m_dt_match[] __initconst = {
@@ -443,9 +383,7 @@ static const char * const v2m_dt_match[] __initconst = {
 
 DT_MACHINE_START(VEXPRESS_DT, "ARM-Versatile Express")
 	.dt_compat	= v2m_dt_match,
-	.smp		= smp_ops(vexpress_smp_ops),
+	.smp		= smp_ops(vexpress_smp_dt_ops),
 	.smp_init	= smp_init_ops(vexpress_smp_init_ops),
-	.map_io		= v2m_dt_map_io,
-	.init_early	= v2m_dt_init_early,
 	.init_machine	= v2m_dt_init,
 MACHINE_END
diff --git a/arch/arm/mach-zynq/Kconfig b/arch/arm/mach-zynq/Kconfig
index 58c2b844e0a..573e0db1d0f 100644
--- a/arch/arm/mach-zynq/Kconfig
+++ b/arch/arm/mach-zynq/Kconfig
@@ -1,14 +1,16 @@
 config ARCH_ZYNQ
 	bool "Xilinx Zynq ARM Cortex A9 Platform" if ARCH_MULTI_V7
-	select ARM_AMBA
-	select ARM_GIC
 	select ARCH_HAS_CPUFREQ
 	select ARCH_HAS_OPP
+	select ARCH_SUPPORTS_BIG_ENDIAN
+	select ARM_AMBA
+	select ARM_GIC
+	select ARM_GLOBAL_TIMER if !CPU_FREQ
+	select CADENCE_TTC_TIMER
 	select HAVE_ARM_SCU if SMP
 	select HAVE_ARM_TWD if SMP
 	select ICST
-	select CADENCE_TTC_TIMER
-	select ARM_GLOBAL_TIMER if !CPU_FREQ
 	select MFD_SYSCON
+	select SOC_BUS
 	help
 	  Support for Xilinx Zynq ARM Cortex A9 Platform
diff --git a/arch/arm/mach-zynq/common.c b/arch/arm/mach-zynq/common.c
index 6fcc584c1a1..edbd9d83f40 100644
--- a/arch/arm/mach-zynq/common.c
+++ b/arch/arm/mach-zynq/common.c
@@ -29,6 +29,8 @@
 #include <linux/memblock.h>
 #include <linux/irqchip.h>
 #include <linux/irqchip/arm-gic.h>
+#include <linux/slab.h>
+#include <linux/sys_soc.h>
 
 #include <asm/mach/arch.h>
 #include <asm/mach/map.h>
@@ -37,10 +39,15 @@
 #include <asm/page.h>
 #include <asm/pgtable.h>
 #include <asm/smp_scu.h>
+#include <asm/system_info.h>
 #include <asm/hardware/cache-l2x0.h>
 
 #include "common.h"
 
+#define ZYNQ_DEVCFG_MCTRL		0x80
+#define ZYNQ_DEVCFG_PS_VERSION_SHIFT	28
+#define ZYNQ_DEVCFG_PS_VERSION_MASK	0xF
+
 void __iomem *zynq_scu_base;
 
 /**
@@ -60,19 +67,81 @@ static struct platform_device zynq_cpuidle_device = {
 };
 
 /**
+ * zynq_get_revision - Get Zynq silicon revision
+ *
+ * Return: Silicon version or -1 otherwise
+ */
+static int __init zynq_get_revision(void)
+{
+	struct device_node *np;
+	void __iomem *zynq_devcfg_base;
+	u32 revision;
+
+	np = of_find_compatible_node(NULL, NULL, "xlnx,zynq-devcfg-1.0");
+	if (!np) {
+		pr_err("%s: no devcfg node found\n", __func__);
+		return -1;
+	}
+
+	zynq_devcfg_base = of_iomap(np, 0);
+	if (!zynq_devcfg_base) {
+		pr_err("%s: Unable to map I/O memory\n", __func__);
+		return -1;
+	}
+
+	revision = readl(zynq_devcfg_base + ZYNQ_DEVCFG_MCTRL);
+	revision >>= ZYNQ_DEVCFG_PS_VERSION_SHIFT;
+	revision &= ZYNQ_DEVCFG_PS_VERSION_MASK;
+
+	iounmap(zynq_devcfg_base);
+
+	return revision;
+}
+
+/**
  * zynq_init_machine - System specific initialization, intended to be
  *		       called from board specific initialization.
  */
 static void __init zynq_init_machine(void)
 {
 	struct platform_device_info devinfo = { .name = "cpufreq-cpu0", };
+	struct soc_device_attribute *soc_dev_attr;
+	struct soc_device *soc_dev;
+	struct device *parent = NULL;
 
 	/*
 	 * 64KB way size, 8-way associativity, parity disabled
 	 */
 	l2x0_of_init(0x02060000, 0xF0F0FFFF);
 
-	of_platform_populate(NULL, of_default_bus_match_table, NULL, NULL);
+	soc_dev_attr = kzalloc(sizeof(*soc_dev_attr), GFP_KERNEL);
+	if (!soc_dev_attr)
+		goto out;
+
+	system_rev = zynq_get_revision();
+
+	soc_dev_attr->family = kasprintf(GFP_KERNEL, "Xilinx Zynq");
+	soc_dev_attr->revision = kasprintf(GFP_KERNEL, "0x%x", system_rev);
+	soc_dev_attr->soc_id = kasprintf(GFP_KERNEL, "0x%x",
+					 zynq_slcr_get_device_id());
+
+	soc_dev = soc_device_register(soc_dev_attr);
+	if (IS_ERR(soc_dev)) {
+		kfree(soc_dev_attr->family);
+		kfree(soc_dev_attr->revision);
+		kfree(soc_dev_attr->soc_id);
+		kfree(soc_dev_attr);
+		goto out;
+	}
+
+	parent = soc_device_to_device(soc_dev);
+
+out:
+	/*
+	 * Finished with the static registrations now; fill in the missing
+	 * devices
+	 */
+	of_platform_populate(NULL, of_default_bus_match_table, NULL, parent);
 
 	platform_device_register(&zynq_cpuidle_device);
 	platform_device_register_full(&devinfo);
diff --git a/arch/arm/mach-zynq/common.h b/arch/arm/mach-zynq/common.h
index b097844d317..f652f0a884a 100644
--- a/arch/arm/mach-zynq/common.h
+++ b/arch/arm/mach-zynq/common.h
@@ -24,6 +24,7 @@ extern int zynq_early_slcr_init(void);
 extern void zynq_slcr_system_reset(void);
 extern void zynq_slcr_cpu_stop(int cpu);
 extern void zynq_slcr_cpu_start(int cpu);
+extern u32 zynq_slcr_get_device_id(void);
 
 #ifdef CONFIG_SMP
 extern void secondary_startup(void);
diff --git a/arch/arm/mach-zynq/headsmp.S b/arch/arm/mach-zynq/headsmp.S
index 57a32869f0a..dd8c071941e 100644
--- a/arch/arm/mach-zynq/headsmp.S
+++ b/arch/arm/mach-zynq/headsmp.S
@@ -8,9 +8,12 @@
  */
 #include <linux/linkage.h>
 #include <linux/init.h>
+#include <asm/assembler.h>
 
 ENTRY(zynq_secondary_trampoline)
-	ldr	r0, [pc]
+ARM_BE8(setend	be)				@ ensure we are in BE8 mode
+	ldr	r0, zynq_secondary_trampoline_jump
+ARM_BE8(rev	r0, r0)
 	bx	r0
 .globl zynq_secondary_trampoline_jump
 zynq_secondary_trampoline_jump:
diff --git a/arch/arm/mach-zynq/slcr.c b/arch/arm/mach-zynq/slcr.c
index a37d49a6e65..c43a2d16e22 100644
--- a/arch/arm/mach-zynq/slcr.c
+++ b/arch/arm/mach-zynq/slcr.c
@@ -26,10 +26,13 @@
 #define SLCR_PS_RST_CTRL_OFFSET		0x200 /* PS Software Reset Control */
 #define SLCR_A9_CPU_RST_CTRL_OFFSET	0x244 /* CPU Software Reset Control */
 #define SLCR_REBOOT_STATUS_OFFSET	0x258 /* PS Reboot Status */
+#define SLCR_PSS_IDCODE			0x530 /* PS IDCODE */
 
 #define SLCR_UNLOCK_MAGIC		0xDF0D
 #define SLCR_A9_CPU_CLKSTOP		0x10
 #define SLCR_A9_CPU_RST			0x1
+#define SLCR_PSS_IDCODE_DEVICE_SHIFT	12
+#define SLCR_PSS_IDCODE_DEVICE_MASK	0x1F
 
 static void __iomem *zynq_slcr_base;
 static struct regmap *zynq_slcr_regmap;
@@ -83,6 +86,22 @@ static inline int zynq_slcr_unlock(void)
 }
 
 /**
+ * zynq_slcr_get_device_id - Read device code id
+ *
+ * Return:	Device code id
+ */
+u32 zynq_slcr_get_device_id(void)
+{
+	u32 val;
+
+	zynq_slcr_read(&val, SLCR_PSS_IDCODE);
+	val >>= SLCR_PSS_IDCODE_DEVICE_SHIFT;
+	val &= SLCR_PSS_IDCODE_DEVICE_MASK;
+
+	return val;
+}
+
+/**
  * zynq_slcr_system_reset - Reset the entire system.
  */
 void zynq_slcr_system_reset(void)
diff --git a/arch/arm/mm/Kconfig b/arch/arm/mm/Kconfig
index f5ad9ee7042..5bf7c3c3b30 100644
--- a/arch/arm/mm/Kconfig
+++ b/arch/arm/mm/Kconfig
@@ -420,29 +420,29 @@ config CPU_32v3
 	bool
 	select CPU_USE_DOMAINS if MMU
 	select NEEDS_SYSCALL_FOR_CMPXCHG if SMP
-	select TLS_REG_EMUL if SMP || !MMU
 	select NEED_KUSER_HELPERS
+	select TLS_REG_EMUL if SMP || !MMU
 
 config CPU_32v4
 	bool
 	select CPU_USE_DOMAINS if MMU
 	select NEEDS_SYSCALL_FOR_CMPXCHG if SMP
-	select TLS_REG_EMUL if SMP || !MMU
 	select NEED_KUSER_HELPERS
+	select TLS_REG_EMUL if SMP || !MMU
 
 config CPU_32v4T
 	bool
 	select CPU_USE_DOMAINS if MMU
 	select NEEDS_SYSCALL_FOR_CMPXCHG if SMP
-	select TLS_REG_EMUL if SMP || !MMU
 	select NEED_KUSER_HELPERS
+	select TLS_REG_EMUL if SMP || !MMU
 
 config CPU_32v5
 	bool
 	select CPU_USE_DOMAINS if MMU
 	select NEEDS_SYSCALL_FOR_CMPXCHG if SMP
-	select TLS_REG_EMUL if SMP || !MMU
 	select NEED_KUSER_HELPERS
+	select TLS_REG_EMUL if SMP || !MMU
 
 config CPU_32v6
 	bool
diff --git a/arch/arm/mm/dma-mapping.c b/arch/arm/mm/dma-mapping.c
index f62aa0677e5..5bef858568e 100644
--- a/arch/arm/mm/dma-mapping.c
+++ b/arch/arm/mm/dma-mapping.c
@@ -390,7 +390,7 @@ static int __init atomic_pool_init(void)
 	if (!pages)
 		goto no_pages;
 
-	if (IS_ENABLED(CONFIG_DMA_CMA))
+	if (dev_get_cma_area(NULL))
 		ptr = __alloc_from_contiguous(NULL, pool->size, prot, &page,
 					      atomic_pool_init);
 	else
@@ -701,7 +701,7 @@ static void *__dma_alloc(struct device *dev, size_t size, dma_addr_t *handle,
 		addr = __alloc_simple_buffer(dev, size, gfp, &page);
 	else if (!(gfp & __GFP_WAIT))
 		addr = __alloc_from_pool(size, &page);
-	else if (!IS_ENABLED(CONFIG_DMA_CMA))
+	else if (!dev_get_cma_area(dev))
 		addr = __alloc_remap_buffer(dev, size, gfp, prot, &page, caller);
 	else
 		addr = __alloc_from_contiguous(dev, size, prot, &page, caller);
@@ -790,7 +790,7 @@ static void __arm_dma_free(struct device *dev, size_t size, void *cpu_addr,
 		__dma_free_buffer(page, size);
 	} else if (__free_from_pool(cpu_addr, size)) {
 		return;
-	} else if (!IS_ENABLED(CONFIG_DMA_CMA)) {
+	} else if (!dev_get_cma_area(dev)) {
 		__dma_free_remap(cpu_addr, size);
 		__dma_free_buffer(page, size);
 	} else {
@@ -1074,6 +1074,7 @@ static inline dma_addr_t __alloc_iova(struct dma_iommu_mapping *mapping,
 	unsigned int order = get_order(size);
 	unsigned int align = 0;
 	unsigned int count, start;
+	size_t mapping_size = mapping->bits << PAGE_SHIFT;
 	unsigned long flags;
 	dma_addr_t iova;
 	int i;
@@ -1119,7 +1120,7 @@ static inline dma_addr_t __alloc_iova(struct dma_iommu_mapping *mapping,
 	}
 	spin_unlock_irqrestore(&mapping->lock, flags);
 
-	iova = mapping->base + (mapping->size * i);
+	iova = mapping->base + (mapping_size * i);
 	iova += start << PAGE_SHIFT;
 
 	return iova;
@@ -1129,6 +1130,7 @@ static inline void __free_iova(struct dma_iommu_mapping *mapping,
 			       dma_addr_t addr, size_t size)
 {
 	unsigned int start, count;
+	size_t mapping_size = mapping->bits << PAGE_SHIFT;
 	unsigned long flags;
 	dma_addr_t bitmap_base;
 	u32 bitmap_index;
@@ -1136,14 +1138,14 @@ static inline void __free_iova(struct dma_iommu_mapping *mapping,
 	if (!size)
 		return;
 
-	bitmap_index = (u32) (addr - mapping->base) / (u32) mapping->size;
+	bitmap_index = (u32) (addr - mapping->base) / (u32) mapping_size;
 	BUG_ON(addr < mapping->base || bitmap_index > mapping->extensions);
 
-	bitmap_base = mapping->base + mapping->size * bitmap_index;
+	bitmap_base = mapping->base + mapping_size * bitmap_index;
 
 	start = (addr - bitmap_base) >>	PAGE_SHIFT;
 
-	if (addr + size > bitmap_base + mapping->size) {
+	if (addr + size > bitmap_base + mapping_size) {
 		/*
 		 * The address range to be freed reaches into the iova
 		 * range of the next bitmap. This should not happen as
@@ -1963,7 +1965,6 @@ arm_iommu_create_mapping(struct bus_type *bus, dma_addr_t base, size_t size)
 	mapping->nr_bitmaps = 1;
 	mapping->extensions = extensions;
 	mapping->base = base;
-	mapping->size = bitmap_size << PAGE_SHIFT;
 	mapping->bits = BITS_PER_BYTE * bitmap_size;
 
 	spin_lock_init(&mapping->lock);
diff --git a/arch/arm/mm/proc-v7m.S b/arch/arm/mm/proc-v7m.S
index 0c93588fcb9..1ca37c72f12 100644
--- a/arch/arm/mm/proc-v7m.S
+++ b/arch/arm/mm/proc-v7m.S
@@ -123,6 +123,11 @@ __v7m_setup:
 	mov	pc, lr
 ENDPROC(__v7m_setup)
 
+	.align 2
+__v7m_setup_stack:
+	.space	4 * 8				@ 8 registers
+__v7m_setup_stack_top:
+
 	define_processor_functions v7m, dabort=nommu_early_abort, pabort=legacy_pabort, nommu=1
 
 	.section ".rodata"
@@ -152,6 +157,3 @@ __v7m_proc_info:
 	.long	nop_cache_fns		@ proc_info_list.cache
 	.size	__v7m_proc_info, . - __v7m_proc_info
 
-__v7m_setup_stack:
-	.space	4 * 8				@ 8 registers
-__v7m_setup_stack_top:
diff --git a/arch/arm/plat-omap/counter_32k.c b/arch/arm/plat-omap/counter_32k.c
index 384a776d8eb..61b4d705c26 100644
--- a/arch/arm/plat-omap/counter_32k.c
+++ b/arch/arm/plat-omap/counter_32k.c
@@ -40,7 +40,7 @@ static void __iomem *sync32k_cnt_reg;
 
 static u64 notrace omap_32k_read_sched_clock(void)
 {
-	return sync32k_cnt_reg ? __raw_readl(sync32k_cnt_reg) : 0;
+	return sync32k_cnt_reg ? readl_relaxed(sync32k_cnt_reg) : 0;
 }
 
 /**
@@ -64,7 +64,7 @@ static void omap_read_persistent_clock(struct timespec *ts)
 	spin_lock_irqsave(&read_persistent_clock_lock, flags);
 
 	last_cycles = cycles;
-	cycles = sync32k_cnt_reg ? __raw_readl(sync32k_cnt_reg) : 0;
+	cycles = sync32k_cnt_reg ? readl_relaxed(sync32k_cnt_reg) : 0;
 
 	nsecs = clocksource_cyc2ns(cycles - last_cycles,
 					persistent_mult, persistent_shift);
@@ -95,7 +95,7 @@ int __init omap_init_clocksource_32k(void __iomem *vbase)
 	 * The 'SCHEME' bits(30-31) of the revision register is used
 	 * to identify the version.
 	 */
-	if (__raw_readl(vbase + OMAP2_32KSYNCNT_REV_OFF) &
+	if (readl_relaxed(vbase + OMAP2_32KSYNCNT_REV_OFF) &
 						OMAP2_32KSYNCNT_REV_SCHEME)
 		sync32k_cnt_reg = vbase + OMAP2_32KSYNCNT_CR_OFF_HIGH;
 	else
diff --git a/arch/arm/plat-omap/debug-leds.c b/arch/arm/plat-omap/debug-leds.c
index aa7ebc6bcd6..48b69de89a5 100644
--- a/arch/arm/plat-omap/debug-leds.c
+++ b/arch/arm/plat-omap/debug-leds.c
@@ -85,12 +85,12 @@ static void dbg_led_set(struct led_classdev *cdev,
 	struct dbg_led *led = container_of(cdev, struct dbg_led, cdev);
 	u16 reg;
 
-	reg = __raw_readw(&fpga->leds);
+	reg = readw_relaxed(&fpga->leds);
 	if (b != LED_OFF)
 		reg |= led->mask;
 	else
 		reg &= ~led->mask;
-	__raw_writew(reg, &fpga->leds);
+	writew_relaxed(reg, &fpga->leds);
 }
 
 static enum led_brightness dbg_led_get(struct led_classdev *cdev)
@@ -98,7 +98,7 @@ static enum led_brightness dbg_led_get(struct led_classdev *cdev)
 	struct dbg_led *led = container_of(cdev, struct dbg_led, cdev);
 	u16 reg;
 
-	reg = __raw_readw(&fpga->leds);
+	reg = readw_relaxed(&fpga->leds);
 	return (reg & led->mask) ? LED_FULL : LED_OFF;
 }
 
@@ -112,7 +112,7 @@ static int fpga_probe(struct platform_device *pdev)
 		return -ENODEV;
 
 	fpga = ioremap(iomem->start, resource_size(iomem));
-	__raw_writew(0xff, &fpga->leds);
+	writew_relaxed(0xff, &fpga->leds);
 
 	for (i = 0; i < ARRAY_SIZE(dbg_leds); i++) {
 		struct dbg_led *led;
@@ -138,15 +138,15 @@ static int fpga_probe(struct platform_device *pdev)
 
 static int fpga_suspend_noirq(struct device *dev)
 {
-	fpga_led_state = __raw_readw(&fpga->leds);
-	__raw_writew(0xff, &fpga->leds);
+	fpga_led_state = readw_relaxed(&fpga->leds);
+	writew_relaxed(0xff, &fpga->leds);
 
 	return 0;
 }
 
 static int fpga_resume_noirq(struct device *dev)
 {
-	__raw_writew(~fpga_led_state, &fpga->leds);
+	writew_relaxed(~fpga_led_state, &fpga->leds);
 	return 0;
 }
 
diff --git a/arch/arm/plat-omap/dma.c b/arch/arm/plat-omap/dma.c
index 5f5b975887f..b5608b1f9fb 100644
--- a/arch/arm/plat-omap/dma.c
+++ b/arch/arm/plat-omap/dma.c
@@ -70,6 +70,7 @@ static u32 errata;
 
 static struct omap_dma_global_context_registers {
 	u32 dma_irqenable_l0;
+	u32 dma_irqenable_l1;
 	u32 dma_ocp_sysconfig;
 	u32 dma_gcr;
 } omap_dma_global_context;
@@ -1973,10 +1974,17 @@ static struct irqaction omap24xx_dma_irq;
 
 /*----------------------------------------------------------------------------*/
 
+/*
+ * Note that we are currently using only IRQENABLE_L0 and L1.
+ * As the DSP may be using IRQENABLE_L2 and L3, let's not
+ * touch those for now.
+ */
 void omap_dma_global_context_save(void)
 {
 	omap_dma_global_context.dma_irqenable_l0 =
 		p->dma_read(IRQENABLE_L0, 0);
+	omap_dma_global_context.dma_irqenable_l1 =
+		p->dma_read(IRQENABLE_L1, 0);
 	omap_dma_global_context.dma_ocp_sysconfig =
 		p->dma_read(OCP_SYSCONFIG, 0);
 	omap_dma_global_context.dma_gcr = p->dma_read(GCR, 0);
@@ -1991,6 +1999,8 @@ void omap_dma_global_context_restore(void)
 		OCP_SYSCONFIG, 0);
 	p->dma_write(omap_dma_global_context.dma_irqenable_l0,
 		IRQENABLE_L0, 0);
+	p->dma_write(omap_dma_global_context.dma_irqenable_l1,
+		IRQENABLE_L1, 0);
 
 	if (IS_DMA_ERRATA(DMA_ROMCODE_BUG))
 		p->dma_write(0x3 , IRQSTATUS_L0, 0);
diff --git a/arch/arm/plat-omap/dmtimer.c b/arch/arm/plat-omap/dmtimer.c
index 869254cebf8..db10169a08d 100644
--- a/arch/arm/plat-omap/dmtimer.c
+++ b/arch/arm/plat-omap/dmtimer.c
@@ -103,7 +103,7 @@ static void omap_timer_restore_context(struct omap_dm_timer *timer)
 				timer->context.tmar);
 	omap_dm_timer_write_reg(timer, OMAP_TIMER_IF_CTRL_REG,
 				timer->context.tsicr);
-	__raw_writel(timer->context.tier, timer->irq_ena);
+	writel_relaxed(timer->context.tier, timer->irq_ena);
 	omap_dm_timer_write_reg(timer, OMAP_TIMER_CTRL_REG,
 				timer->context.tclr);
 }
@@ -699,9 +699,9 @@ int omap_dm_timer_set_int_disable(struct omap_dm_timer *timer, u32 mask)
 	omap_dm_timer_enable(timer);
 
 	if (timer->revision == 1)
-		l = __raw_readl(timer->irq_ena) & ~mask;
+		l = readl_relaxed(timer->irq_ena) & ~mask;
 
-	__raw_writel(l, timer->irq_dis);
+	writel_relaxed(l, timer->irq_dis);
 	l = omap_dm_timer_read_reg(timer, OMAP_TIMER_WAKEUP_EN_REG) & ~mask;
 	omap_dm_timer_write_reg(timer, OMAP_TIMER_WAKEUP_EN_REG, l);
 
@@ -722,7 +722,7 @@ unsigned int omap_dm_timer_read_status(struct omap_dm_timer *timer)
 		return 0;
 	}
 
-	l = __raw_readl(timer->irq_stat);
+	l = readl_relaxed(timer->irq_stat);
 
 	return l;
 }
diff --git a/arch/arm/plat-omap/include/plat/dmtimer.h b/arch/arm/plat-omap/include/plat/dmtimer.h
index 2861b155485..dd79f3005cd 100644
--- a/arch/arm/plat-omap/include/plat/dmtimer.h
+++ b/arch/arm/plat-omap/include/plat/dmtimer.h
@@ -280,20 +280,20 @@ static inline u32 __omap_dm_timer_read(struct omap_dm_timer *timer, u32 reg,
 						int posted)
 {
 	if (posted)
-		while (__raw_readl(timer->pend) & (reg >> WPSHIFT))
+		while (readl_relaxed(timer->pend) & (reg >> WPSHIFT))
 			cpu_relax();
 
-	return __raw_readl(timer->func_base + (reg & 0xff));
+	return readl_relaxed(timer->func_base + (reg & 0xff));
 }
 
 static inline void __omap_dm_timer_write(struct omap_dm_timer *timer,
 					u32 reg, u32 val, int posted)
 {
 	if (posted)
-		while (__raw_readl(timer->pend) & (reg >> WPSHIFT))
+		while (readl_relaxed(timer->pend) & (reg >> WPSHIFT))
 			cpu_relax();
 
-	__raw_writel(val, timer->func_base + (reg & 0xff));
+	writel_relaxed(val, timer->func_base + (reg & 0xff));
 }
 
 static inline void __omap_dm_timer_init_regs(struct omap_dm_timer *timer)
@@ -301,7 +301,7 @@ static inline void __omap_dm_timer_init_regs(struct omap_dm_timer *timer)
 	u32 tidr;
 
 	/* Assume v1 ip if bits [31:16] are zero */
-	tidr = __raw_readl(timer->io_base);
+	tidr = readl_relaxed(timer->io_base);
 	if (!(tidr >> 16)) {
 		timer->revision = 1;
 		timer->irq_stat = timer->io_base + OMAP_TIMER_V1_STAT_OFFSET;
@@ -385,7 +385,7 @@ static inline void __omap_dm_timer_stop(struct omap_dm_timer *timer,
 	}
 
 	/* Ack possibly pending interrupt */
-	__raw_writel(OMAP_TIMER_INT_OVERFLOW, timer->irq_stat);
+	writel_relaxed(OMAP_TIMER_INT_OVERFLOW, timer->irq_stat);
 }
 
 static inline void __omap_dm_timer_load_start(struct omap_dm_timer *timer,
@@ -399,7 +399,7 @@ static inline void __omap_dm_timer_load_start(struct omap_dm_timer *timer,
 static inline void __omap_dm_timer_int_enable(struct omap_dm_timer *timer,
 						unsigned int value)
 {
-	__raw_writel(value, timer->irq_ena);
+	writel_relaxed(value, timer->irq_ena);
 	__omap_dm_timer_write(timer, OMAP_TIMER_WAKEUP_EN_REG, value, 0);
 }
 
@@ -412,7 +412,7 @@ __omap_dm_timer_read_counter(struct omap_dm_timer *timer, int posted)
 static inline void __omap_dm_timer_write_status(struct omap_dm_timer *timer,
 						unsigned int value)
 {
-	__raw_writel(value, timer->irq_stat);
+	writel_relaxed(value, timer->irq_stat);
 }
 
 #endif /* __ASM_ARCH_DMTIMER_H */
diff --git a/arch/arm/plat-orion/gpio.c b/arch/arm/plat-orion/gpio.c
index 6816192a756..b61a3bcc2fa 100644
--- a/arch/arm/plat-orion/gpio.c
+++ b/arch/arm/plat-orion/gpio.c
@@ -597,51 +597,3 @@ void __init orion_gpio_init(struct device_node *np,
 
 	orion_gpio_chip_count++;
 }
-
-#ifdef CONFIG_OF
-static void __init orion_gpio_of_init_one(struct device_node *np,
-					  int irq_gpio_base)
-{
-	int ngpio, gpio_base, mask_offset;
-	void __iomem *base;
-	int ret, i;
-	int irqs[4];
-	int secondary_irq_base;
-
-	ret = of_property_read_u32(np, "ngpio", &ngpio);
-	if (ret)
-		goto out;
-	ret = of_property_read_u32(np, "mask-offset", &mask_offset);
-	if (ret == -EINVAL)
-		mask_offset = 0;
-	else
-		goto out;
-	base = of_iomap(np, 0);
-	if (!base)
-		goto out;
-
-	secondary_irq_base = irq_gpio_base + (32 * orion_gpio_chip_count);
-	gpio_base = 32 * orion_gpio_chip_count;
-
-	/* Get the interrupt numbers. Each chip can have up to 4
-	 * interrupt handlers, with each handler dealing with 8 GPIO
-	 * pins. */
-
-	for (i = 0; i < 4; i++)
-		irqs[i] = irq_of_parse_and_map(np, i);
-
-	orion_gpio_init(np, gpio_base, ngpio, base, mask_offset,
-			secondary_irq_base, irqs);
-	return;
-out:
-	pr_err("%s: %s: missing mandatory property\n", __func__, np->name);
-}
-
-void __init orion_gpio_of_init(int irq_gpio_base)
-{
-	struct device_node *np;
-
-	for_each_compatible_node(np, NULL, "marvell,orion-gpio")
-		orion_gpio_of_init_one(np, irq_gpio_base);
-}
-#endif
diff --git a/arch/arm/plat-orion/include/plat/irq.h b/arch/arm/plat-orion/include/plat/irq.h
index 50547e41793..96be19e9bd9 100644
--- a/arch/arm/plat-orion/include/plat/irq.h
+++ b/arch/arm/plat-orion/include/plat/irq.h
@@ -12,5 +12,4 @@
 #define __PLAT_IRQ_H
 
 void orion_irq_init(unsigned int irq_start, void __iomem *maskaddr);
-void __init orion_dt_init_irq(void);
 #endif
diff --git a/arch/arm/plat-orion/include/plat/orion-gpio.h b/arch/arm/plat-orion/include/plat/orion-gpio.h
index 614dcac9dc5..e763988b04b 100644
--- a/arch/arm/plat-orion/include/plat/orion-gpio.h
+++ b/arch/arm/plat-orion/include/plat/orion-gpio.h
@@ -33,5 +33,4 @@ void __init orion_gpio_init(struct device_node *np,
 			    int secondary_irq_base,
 			    int irq[4]);
 
-void __init orion_gpio_of_init(int irq_gpio_base);
 #endif
diff --git a/arch/arm/plat-orion/irq.c b/arch/arm/plat-orion/irq.c
index 807df142444..8c1fc06007c 100644
--- a/arch/arm/plat-orion/irq.c
+++ b/arch/arm/plat-orion/irq.c
@@ -20,47 +20,6 @@
 #include <plat/orion-gpio.h>
 #include <mach/bridge-regs.h>
 
-#ifdef CONFIG_MULTI_IRQ_HANDLER
-/*
- * Compiling with both non-DT and DT support enabled, will
- * break asm irq handler used by non-DT boards. Therefore,
- * we provide a C-style irq handler even for non-DT boards,
- * if MULTI_IRQ_HANDLER is set.
- *
- * Notes:
- * - this is prepared for Kirkwood and Dove only, update
- *   accordingly if you add Orion5x or MV78x00.
- * - Orion5x uses different macro names and has only one
- *   set of CAUSE/MASK registers.
- * - MV78x00 uses the same macro names but has a third
- *   set of CAUSE/MASK registers.
- *
- */
-
-static void __iomem *orion_irq_base = IRQ_VIRT_BASE;
-
-asmlinkage void
-__exception_irq_entry orion_legacy_handle_irq(struct pt_regs *regs)
-{
-	u32 stat;
-
-	stat = readl_relaxed(orion_irq_base + IRQ_CAUSE_LOW_OFF);
-	stat &= readl_relaxed(orion_irq_base + IRQ_MASK_LOW_OFF);
-	if (stat) {
-		unsigned int hwirq = __fls(stat);
-		handle_IRQ(hwirq, regs);
-		return;
-	}
-	stat = readl_relaxed(orion_irq_base + IRQ_CAUSE_HIGH_OFF);
-	stat &= readl_relaxed(orion_irq_base + IRQ_MASK_HIGH_OFF);
-	if (stat) {
-		unsigned int hwirq = 32 + __fls(stat);
-		handle_IRQ(hwirq, regs);
-		return;
-	}
-}
-#endif
-
 void __init orion_irq_init(unsigned int irq_start, void __iomem *maskaddr)
 {
 	struct irq_chip_generic *gc;
@@ -78,40 +37,4 @@ void __init orion_irq_init(unsigned int irq_start, void __iomem *maskaddr)
 	ct->chip.irq_unmask = irq_gc_mask_set_bit;
 	irq_setup_generic_chip(gc, IRQ_MSK(32), IRQ_GC_INIT_MASK_CACHE,
 			       IRQ_NOREQUEST, IRQ_LEVEL | IRQ_NOPROBE);
-
-#ifdef CONFIG_MULTI_IRQ_HANDLER
-	set_handle_irq(orion_legacy_handle_irq);
-#endif
-}
-
-#ifdef CONFIG_OF
-static int __init orion_add_irq_domain(struct device_node *np,
-				       struct device_node *interrupt_parent)
-{
-	int i = 0;
-	void __iomem *base;
-
-	do {
-		base = of_iomap(np, i);
-		if (base) {
-			orion_irq_init(i * 32, base + 0x04);
-			i++;
-		}
-	} while (base);
-
-	irq_domain_add_legacy(np, i * 32, 0, 0,
-			      &irq_domain_simple_ops, NULL);
-	return 0;
-}
-
-static const struct of_device_id orion_irq_match[] = {
-	{ .compatible = "marvell,orion-intc",
-	  .data = orion_add_irq_domain, },
-	{},
-};
-
-void __init orion_dt_init_irq(void)
-{
-	of_irq_init(orion_irq_match);
 }
-#endif
diff --git a/arch/arm/plat-samsung/Makefile b/arch/arm/plat-samsung/Makefile
index 25c826ed3b6..5e5beaa9ae1 100644
--- a/arch/arm/plat-samsung/Makefile
+++ b/arch/arm/plat-samsung/Makefile
@@ -4,6 +4,9 @@
 #
 # Licensed under GPLv2
 
+ccflags-$(CONFIG_ARCH_MULTI_V7) += -I$(srctree)/$(src)/include
+ccflags-$(CONFIG_ARCH_EXYNOS)	+= -I$(srctree)/arch/arm/mach-exynos/include
+
 obj-y				:=
 obj-m				:=
 obj-n				:= dummy.o
diff --git a/arch/arm/plat-samsung/include/plat/cpu-freq-core.h b/arch/arm/plat-samsung/include/plat/cpu-freq-core.h
index 7231c8e4975..72d4178ad23 100644
--- a/arch/arm/plat-samsung/include/plat/cpu-freq-core.h
+++ b/arch/arm/plat-samsung/include/plat/cpu-freq-core.h
@@ -119,6 +119,7 @@ struct s3c_plltab {
 struct s3c_cpufreq_config {
 	struct s3c_freq		freq;
 	struct s3c_freq		max;
+	struct clk		*mpll;
 	struct cpufreq_frequency_table pll;
 	struct s3c_clkdivs	divs;
 	struct s3c_cpufreq_info *info;	/* for core, not drivers */
diff --git a/arch/arm/plat-samsung/include/plat/cpu.h b/arch/arm/plat-samsung/include/plat/cpu.h
index 5992b8dd9b8..5a237db9f9e 100644
--- a/arch/arm/plat-samsung/include/plat/cpu.h
+++ b/arch/arm/plat-samsung/include/plat/cpu.h
@@ -43,16 +43,6 @@ extern unsigned long samsung_cpu_id;
 #define S5PV210_CPU_ID		0x43110000
 #define S5PV210_CPU_MASK	0xFFFFF000
 
-#define EXYNOS4210_CPU_ID	0x43210000
-#define EXYNOS4212_CPU_ID	0x43220000
-#define EXYNOS4412_CPU_ID	0xE4412200
-#define EXYNOS4_CPU_MASK	0xFFFE0000
-
-#define EXYNOS5250_SOC_ID	0x43520000
-#define EXYNOS5420_SOC_ID	0xE5420000
-#define EXYNOS5440_SOC_ID	0xE5440000
-#define EXYNOS5_SOC_MASK	0xFFFFF000
-
 #define IS_SAMSUNG_CPU(name, id, mask)		\
 static inline int is_samsung_##name(void)	\
 {						\
@@ -68,12 +58,6 @@ IS_SAMSUNG_CPU(s5p6440, S5P6440_CPU_ID, S5P64XX_CPU_MASK)
 IS_SAMSUNG_CPU(s5p6450, S5P6450_CPU_ID, S5P64XX_CPU_MASK)
 IS_SAMSUNG_CPU(s5pc100, S5PC100_CPU_ID, S5PC100_CPU_MASK)
 IS_SAMSUNG_CPU(s5pv210, S5PV210_CPU_ID, S5PV210_CPU_MASK)
-IS_SAMSUNG_CPU(exynos4210, EXYNOS4210_CPU_ID, EXYNOS4_CPU_MASK)
-IS_SAMSUNG_CPU(exynos4212, EXYNOS4212_CPU_ID, EXYNOS4_CPU_MASK)
-IS_SAMSUNG_CPU(exynos4412, EXYNOS4412_CPU_ID, EXYNOS4_CPU_MASK)
-IS_SAMSUNG_CPU(exynos5250, EXYNOS5250_SOC_ID, EXYNOS5_SOC_MASK)
-IS_SAMSUNG_CPU(exynos5420, EXYNOS5420_SOC_ID, EXYNOS5_SOC_MASK)
-IS_SAMSUNG_CPU(exynos5440, EXYNOS5440_SOC_ID, EXYNOS5_SOC_MASK)
 
 #if defined(CONFIG_CPU_S3C2410) || defined(CONFIG_CPU_S3C2412) || \
     defined(CONFIG_CPU_S3C2416) || defined(CONFIG_CPU_S3C2440) || \
@@ -126,50 +110,6 @@ IS_SAMSUNG_CPU(exynos5440, EXYNOS5440_SOC_ID, EXYNOS5_SOC_MASK)
 # define soc_is_s5pv210()	0
 #endif
 
-#if defined(CONFIG_CPU_EXYNOS4210)
-# define soc_is_exynos4210()	is_samsung_exynos4210()
-#else
-# define soc_is_exynos4210()	0
-#endif
-
-#if defined(CONFIG_SOC_EXYNOS4212)
-# define soc_is_exynos4212()	is_samsung_exynos4212()
-#else
-# define soc_is_exynos4212()	0
-#endif
-
-#if defined(CONFIG_SOC_EXYNOS4412)
-# define soc_is_exynos4412()	is_samsung_exynos4412()
-#else
-# define soc_is_exynos4412()	0
-#endif
-
-#define EXYNOS4210_REV_0	(0x0)
-#define EXYNOS4210_REV_1_0	(0x10)
-#define EXYNOS4210_REV_1_1	(0x11)
-
-#if defined(CONFIG_SOC_EXYNOS5250)
-# define soc_is_exynos5250()	is_samsung_exynos5250()
-#else
-# define soc_is_exynos5250()	0
-#endif
-
-#if defined(CONFIG_SOC_EXYNOS5420)
-# define soc_is_exynos5420()	is_samsung_exynos5420()
-#else
-# define soc_is_exynos5420()	0
-#endif
-
-#if defined(CONFIG_SOC_EXYNOS5440)
-# define soc_is_exynos5440()	is_samsung_exynos5440()
-#else
-# define soc_is_exynos5440()	0
-#endif
-
-#define soc_is_exynos4() (soc_is_exynos4210() || soc_is_exynos4212() || \
-			  soc_is_exynos4412())
-#define soc_is_exynos5() (soc_is_exynos5250() || soc_is_exynos5420())
-
 #define IODESC_ENT(x) { (unsigned long)S3C24XX_VA_##x, __phys_to_pfn(S3C24XX_PA_##x), S3C24XX_SZ_##x, MT_DEVICE }
 
 #ifndef KHZ
@@ -239,7 +179,6 @@ extern struct bus_type s3c2443_subsys;
 extern struct bus_type s3c6410_subsys;
 extern struct bus_type s5p64x0_subsys;
 extern struct bus_type s5pv210_subsys;
-extern struct bus_type exynos_subsys;
 
 extern void (*s5pc1xx_idle)(void);
 
diff --git a/arch/arm/plat-versatile/Kconfig b/arch/arm/plat-versatile/Kconfig
index 2c4332b9f94..fce41e93b6a 100644
--- a/arch/arm/plat-versatile/Kconfig
+++ b/arch/arm/plat-versatile/Kconfig
@@ -6,12 +6,6 @@ config PLAT_VERSATILE_CLOCK
 config PLAT_VERSATILE_CLCD
 	bool
 
-config PLAT_VERSATILE_LEDS
-	def_bool y if NEW_LEDS
-	depends on ARCH_REALVIEW || ARCH_VERSATILE
-	select LEDS_CLASS
-	select LEDS_TRIGGERS
-
 config PLAT_VERSATILE_SCHED_CLOCK
 	def_bool y
 
diff --git a/arch/arm/plat-versatile/Makefile b/arch/arm/plat-versatile/Makefile
index f88d448b629..2e0c472958a 100644
--- a/arch/arm/plat-versatile/Makefile
+++ b/arch/arm/plat-versatile/Makefile
@@ -2,6 +2,5 @@ ccflags-$(CONFIG_ARCH_MULTIPLATFORM) := -I$(srctree)/$(src)/include
 
 obj-$(CONFIG_PLAT_VERSATILE_CLOCK) += clock.o
 obj-$(CONFIG_PLAT_VERSATILE_CLCD) += clcd.o
-obj-$(CONFIG_PLAT_VERSATILE_LEDS) += leds.o
 obj-$(CONFIG_PLAT_VERSATILE_SCHED_CLOCK) += sched-clock.o
 obj-$(CONFIG_SMP) += headsmp.o platsmp.o
diff --git a/arch/arm/vfp/vfpdouble.c b/arch/arm/vfp/vfpdouble.c
index 6cac43bd1d8..423f56dd402 100644
--- a/arch/arm/vfp/vfpdouble.c
+++ b/arch/arm/vfp/vfpdouble.c
@@ -866,6 +866,8 @@ vfp_double_multiply_accumulate(int dd, int dn, int dm, u32 fpscr, u32 negate, ch
 		vdp.sign = vfp_sign_negate(vdp.sign);
 
 	vfp_double_unpack(&vdn, vfp_get_double(dd));
+	if (vdn.exponent == 0 && vdn.significand)
+		vfp_double_normalise_denormal(&vdn);
 	if (negate & NEG_SUBTRACT)
 		vdn.sign = vfp_sign_negate(vdn.sign);
 
diff --git a/arch/arm/vfp/vfpsingle.c b/arch/arm/vfp/vfpsingle.c
index b252631b406..4f96c1617aa 100644
--- a/arch/arm/vfp/vfpsingle.c
+++ b/arch/arm/vfp/vfpsingle.c
@@ -915,6 +915,8 @@ vfp_single_multiply_accumulate(int sd, int sn, s32 m, u32 fpscr, u32 negate, cha
 	v = vfp_get_float(sd);
 	pr_debug("VFP: s%u = %08x\n", sd, v);
 	vfp_single_unpack(&vsn, v);
+	if (vsn.exponent == 0 && vsn.significand)
+		vfp_single_normalise_denormal(&vsn);
 	if (negate & NEG_SUBTRACT)
 		vsn.sign = vfp_sign_negate(vsn.sign);
 
diff --git a/arch/arm/xen/enlighten.c b/arch/arm/xen/enlighten.c
index b96723e258a..1e632430570 100644
--- a/arch/arm/xen/enlighten.c
+++ b/arch/arm/xen/enlighten.c
@@ -339,6 +339,14 @@ static int __init xen_pm_init(void)
 }
 late_initcall(xen_pm_init);
 
+
+/* empty stubs */
+void xen_arch_pre_suspend(void) { }
+void xen_arch_post_suspend(int suspend_cancelled) { }
+void xen_timer_resume(void) { }
+void xen_arch_resume(void) { }
+
+
 /* In the hypervisor.S file. */
 EXPORT_SYMBOL_GPL(HYPERVISOR_event_channel_op);
 EXPORT_SYMBOL_GPL(HYPERVISOR_grant_table_op);
@@ -350,4 +358,5 @@ EXPORT_SYMBOL_GPL(HYPERVISOR_memory_op);
 EXPORT_SYMBOL_GPL(HYPERVISOR_physdev_op);
 EXPORT_SYMBOL_GPL(HYPERVISOR_vcpu_op);
 EXPORT_SYMBOL_GPL(HYPERVISOR_tmem_op);
+EXPORT_SYMBOL_GPL(HYPERVISOR_multicall);
 EXPORT_SYMBOL_GPL(privcmd_call);
diff --git a/arch/arm/xen/hypercall.S b/arch/arm/xen/hypercall.S
index d1cf7b7c220..44e3a5f10c4 100644
--- a/arch/arm/xen/hypercall.S
+++ b/arch/arm/xen/hypercall.S
@@ -89,6 +89,7 @@ HYPERCALL2(memory_op);
 HYPERCALL2(physdev_op);
 HYPERCALL3(vcpu_op);
 HYPERCALL1(tmem_op);
+HYPERCALL2(multicall);
 
 ENTRY(privcmd_call)
 	stmdb sp!, {r4}
diff --git a/arch/arm64/Kconfig b/arch/arm64/Kconfig
index e6e4d3749a6..f5f63b715d9 100644
--- a/arch/arm64/Kconfig
+++ b/arch/arm64/Kconfig
@@ -112,6 +112,9 @@ config IOMMU_HELPER
 config KERNEL_MODE_NEON
 	def_bool y
 
+config FIX_EARLYCON_MEM
+	def_bool y
+
 source "init/Kconfig"
 
 source "kernel/Kconfig.freezer"
@@ -323,8 +326,6 @@ menu "CPU Power Management"
 
 source "drivers/cpuidle/Kconfig"
 
-source "kernel/power/Kconfig"
-
 source "drivers/cpufreq/Kconfig"
 
 endmenu
diff --git a/arch/arm64/Kconfig.debug b/arch/arm64/Kconfig.debug
index d10ec334c93..1c1b7562984 100644
--- a/arch/arm64/Kconfig.debug
+++ b/arch/arm64/Kconfig.debug
@@ -20,15 +20,6 @@ config STRICT_DEVMEM
 
 	  If in doubt, say Y.
 
-config EARLY_PRINTK
-	bool "Early printk support"
-	default y
-	help
-	  Say Y here if you want to have an early console using the
-	  earlyprintk=<name>[,<addr>][,<options>] kernel parameter. It
-	  is assumed that the early console device has been initialised
-	  by the boot loader prior to starting the Linux kernel.
-
 config PID_IN_CONTEXTIDR
 	bool "Write the current PID to the CONTEXTIDR register"
 	help
diff --git a/arch/arm64/boot/dts/apm-storm.dtsi b/arch/arm64/boot/dts/apm-storm.dtsi
index 93f4b2dd924..f8c40a66e65 100644
--- a/arch/arm64/boot/dts/apm-storm.dtsi
+++ b/arch/arm64/boot/dts/apm-storm.dtsi
@@ -307,6 +307,7 @@
 			      <0x0 0x1f21e000 0x0 0x1000>,
 			      <0x0 0x1f217000 0x0 0x1000>;
 			interrupts = <0x0 0x86 0x4>;
+			dma-coherent;
 			status = "disabled";
 			clocks = <&sata01clk 0>;
 			phys = <&phy1 0>;
@@ -321,6 +322,7 @@
 			      <0x0 0x1f22e000 0x0 0x1000>,
 			      <0x0 0x1f227000 0x0 0x1000>;
 			interrupts = <0x0 0x87 0x4>;
+			dma-coherent;
 			status = "ok";
 			clocks = <&sata23clk 0>;
 			phys = <&phy2 0>;
@@ -334,6 +336,7 @@
 			      <0x0 0x1f23d000 0x0 0x1000>,
 			      <0x0 0x1f23e000 0x0 0x1000>;
 			interrupts = <0x0 0x88 0x4>;
+			dma-coherent;
 			status = "ok";
 			clocks = <&sata45clk 0>;
 			phys = <&phy3 0>;
diff --git a/arch/arm64/boot/dts/rtsm_ve-motherboard.dtsi b/arch/arm64/boot/dts/rtsm_ve-motherboard.dtsi
index 2f2ecd21736..ac2cb241802 100644
--- a/arch/arm64/boot/dts/rtsm_ve-motherboard.dtsi
+++ b/arch/arm64/boot/dts/rtsm_ve-motherboard.dtsi
@@ -200,7 +200,7 @@
 		};
 
 		mcc {
-			compatible = "arm,vexpress,config-bus", "simple-bus";
+			compatible = "arm,vexpress,config-bus";
 			arm,vexpress,config-bridge = <&v2m_sysreg>;
 
 			v2m_oscclk1: osc@1 {
diff --git a/arch/arm64/include/asm/memory.h b/arch/arm64/include/asm/memory.h
index e94f9458aa6..993bce527b8 100644
--- a/arch/arm64/include/asm/memory.h
+++ b/arch/arm64/include/asm/memory.h
@@ -138,6 +138,7 @@ static inline void *phys_to_virt(phys_addr_t x)
 #define __pa(x)			__virt_to_phys((unsigned long)(x))
 #define __va(x)			((void *)__phys_to_virt((phys_addr_t)(x)))
 #define pfn_to_kaddr(pfn)	__va((pfn) << PAGE_SHIFT)
+#define virt_to_pfn(x)      __phys_to_pfn(__virt_to_phys(x))
 
 /*
  *  virt_to_page(k)	convert a _valid_ virtual address to struct page *
diff --git a/arch/arm64/include/asm/mmu.h b/arch/arm64/include/asm/mmu.h
index f600d400c07..aff0292c8f4 100644
--- a/arch/arm64/include/asm/mmu.h
+++ b/arch/arm64/include/asm/mmu.h
@@ -22,6 +22,9 @@ typedef struct {
 	void *vdso;
 } mm_context_t;
 
+#define INIT_MM_CONTEXT(name) \
+	.context.id_lock = __RAW_SPIN_LOCK_UNLOCKED(name.context.id_lock),
+
 #define ASID(mm)	((mm)->context.id & 0xffff)
 
 extern void paging_init(void);
diff --git a/arch/arm64/include/asm/pgtable.h b/arch/arm64/include/asm/pgtable.h
index 90c811f05a2..7b1c67a0b48 100644
--- a/arch/arm64/include/asm/pgtable.h
+++ b/arch/arm64/include/asm/pgtable.h
@@ -266,7 +266,7 @@ static inline pmd_t pte_pmd(pte_t pte)
 
 #define pmd_page(pmd)           pfn_to_page(__phys_to_pfn(pmd_val(pmd) & PHYS_MASK))
 
-#define set_pmd_at(mm, addr, pmdp, pmd)	set_pmd(pmdp, pmd)
+#define set_pmd_at(mm, addr, pmdp, pmd)	set_pte_at(mm, addr, (pte_t *)pmdp, pmd_pte(pmd))
 
 static inline int has_transparent_hugepage(void)
 {
diff --git a/arch/arm64/include/asm/tlb.h b/arch/arm64/include/asm/tlb.h
index 72cadf52ca8..80e2c08900d 100644
--- a/arch/arm64/include/asm/tlb.h
+++ b/arch/arm64/include/asm/tlb.h
@@ -19,6 +19,7 @@
 #ifndef __ASM_TLB_H
 #define __ASM_TLB_H
 
+#define  __tlb_remove_pmd_tlb_entry __tlb_remove_pmd_tlb_entry
 
 #include <asm-generic/tlb.h>
 
@@ -99,5 +100,10 @@ static inline void __pmd_free_tlb(struct mmu_gather *tlb, pmd_t *pmdp,
 }
 #endif
 
+static inline void __tlb_remove_pmd_tlb_entry(struct mmu_gather *tlb, pmd_t *pmdp,
+						unsigned long address)
+{
+	tlb_add_flush(tlb, address);
+}
 
 #endif
diff --git a/arch/arm64/include/asm/unistd32.h b/arch/arm64/include/asm/unistd32.h
index bb8eb8a78e6..c8d8fc17bd5 100644
--- a/arch/arm64/include/asm/unistd32.h
+++ b/arch/arm64/include/asm/unistd32.h
@@ -403,8 +403,9 @@ __SYSCALL(378, sys_kcmp)
 __SYSCALL(379, sys_finit_module)
 __SYSCALL(380, sys_sched_setattr)
 __SYSCALL(381, sys_sched_getattr)
+__SYSCALL(382, sys_renameat2)
 
-#define __NR_compat_syscalls		379
+#define __NR_compat_syscalls		383
 
 /*
  * Compat syscall numbers used by the AArch64 kernel.
diff --git a/arch/arm64/kernel/Makefile b/arch/arm64/kernel/Makefile
index 7d811d9522b..7a6fce5167e 100644
--- a/arch/arm64/kernel/Makefile
+++ b/arch/arm64/kernel/Makefile
@@ -18,7 +18,6 @@ arm64-obj-$(CONFIG_SMP)			+= smp.o smp_spin_table.o topology.o
 arm64-obj-$(CONFIG_PERF_EVENTS)		+= perf_regs.o
 arm64-obj-$(CONFIG_HW_PERF_EVENTS)	+= perf_event.o
 arm64-obj-$(CONFIG_HAVE_HW_BREAKPOINT)	+= hw_breakpoint.o
-arm64-obj-$(CONFIG_EARLY_PRINTK)	+= early_printk.o
 arm64-obj-$(CONFIG_ARM64_CPU_SUSPEND)	+= sleep.o suspend.o
 arm64-obj-$(CONFIG_JUMP_LABEL)		+= jump_label.o
 arm64-obj-$(CONFIG_KGDB)		+= kgdb.o
diff --git a/arch/arm64/kernel/debug-monitors.c b/arch/arm64/kernel/debug-monitors.c
index ed3955a9574..a7fb874b595 100644
--- a/arch/arm64/kernel/debug-monitors.c
+++ b/arch/arm64/kernel/debug-monitors.c
@@ -318,9 +318,6 @@ static int brk_handler(unsigned long addr, unsigned int esr,
 	if (call_break_hook(regs, esr) == DBG_HOOK_HANDLED)
 		return 0;
 
-	pr_warn("unexpected brk exception at %lx, esr=0x%x\n",
-			(long)instruction_pointer(regs), esr);
-
 	if (!user_mode(regs))
 		return -EFAULT;
 
diff --git a/arch/arm64/kernel/early_printk.c b/arch/arm64/kernel/early_printk.c
deleted file mode 100644
index ffbbdde7aba..00000000000
--- a/arch/arm64/kernel/early_printk.c
+++ /dev/null
@@ -1,158 +0,0 @@
-/*
- * Earlyprintk support.
- *
- * Copyright (C) 2012 ARM Ltd.
- * Author: Catalin Marinas <catalin.marinas@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/>.
- */
-#include <linux/kernel.h>
-#include <linux/console.h>
-#include <linux/init.h>
-#include <linux/string.h>
-#include <linux/mm.h>
-#include <linux/io.h>
-
-#include <linux/amba/serial.h>
-#include <linux/serial_reg.h>
-
-#include <asm/fixmap.h>
-
-static void __iomem *early_base;
-static void (*printch)(char ch);
-
-/*
- * PL011 single character TX.
- */
-static void pl011_printch(char ch)
-{
-	while (readl_relaxed(early_base + UART01x_FR) & UART01x_FR_TXFF)
-		;
-	writeb_relaxed(ch, early_base + UART01x_DR);
-	while (readl_relaxed(early_base + UART01x_FR) & UART01x_FR_BUSY)
-		;
-}
-
-/*
- * 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);
-};
-
-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, },
-	{}
-};
-
-static void early_write(struct console *con, const char *s, unsigned n)
-{
-	while (n-- > 0) {
-		if (*s == '\n')
-			printch('\r');
-		printch(*s);
-		s++;
-	}
-}
-
-static struct console early_console_dev = {
-	.name =		"earlycon",
-	.write =	early_write,
-	.flags =	CON_PRINTBUFFER | CON_BOOT,
-	.index =	-1,
-};
-
-/*
- * Parse earlyprintk=... parameter in the format:
- *
- *   <name>[,<addr>][,<options>]
- *
- * and register the early console. It is assumed that the UART has been
- * initialised by the bootloader already.
- */
-static int __init setup_early_printk(char *buf)
-{
-	const struct earlycon_match *match = earlycon_match;
-	phys_addr_t paddr = 0;
-
-	if (!buf) {
-		pr_warning("No earlyprintk arguments passed.\n");
-		return 0;
-	}
-
-	while (match->name) {
-		size_t len = strlen(match->name);
-		if (!strncmp(buf, match->name, len)) {
-			buf += len;
-			break;
-		}
-		match++;
-	}
-	if (!match->name) {
-		pr_warning("Unknown earlyprintk arguments: %s\n", buf);
-		return 0;
-	}
-
-	/* I/O address */
-	if (!strncmp(buf, ",0x", 3)) {
-		char *e;
-		paddr = simple_strtoul(buf + 1, &e, 16);
-		buf = e;
-	}
-	/* no options parsing yet */
-
-	if (paddr) {
-		set_fixmap_io(FIX_EARLYCON_MEM_BASE, paddr);
-		early_base = (void __iomem *)fix_to_virt(FIX_EARLYCON_MEM_BASE);
-	}
-
-	printch = match->printch;
-	early_console = &early_console_dev;
-	register_console(&early_console_dev);
-
-	return 0;
-}
-
-early_param("earlyprintk", setup_early_printk);
diff --git a/arch/arm64/kernel/irq.c b/arch/arm64/kernel/irq.c
index 473e5dbf8f3..0f08dfd69eb 100644
--- a/arch/arm64/kernel/irq.c
+++ b/arch/arm64/kernel/irq.c
@@ -97,11 +97,15 @@ static bool migrate_one_irq(struct irq_desc *desc)
 	if (irqd_is_per_cpu(d) || !cpumask_test_cpu(smp_processor_id(), affinity))
 		return false;
 
-	if (cpumask_any_and(affinity, cpu_online_mask) >= nr_cpu_ids) {
-		affinity = cpu_online_mask;
+	if (cpumask_any_and(affinity, cpu_online_mask) >= nr_cpu_ids)
 		ret = true;
-	}
 
+	/*
+	 * when using forced irq_set_affinity we must ensure that the cpu
+	 * being offlined is not present in the affinity mask, it may be
+	 * selected as the target CPU otherwise
+	 */
+	affinity = cpu_online_mask;
 	c = irq_data_get_irq_chip(d);
 	if (!c->irq_set_affinity)
 		pr_debug("IRQ%u: unable to set affinity\n", d->irq);
diff --git a/arch/arm64/kernel/setup.c b/arch/arm64/kernel/setup.c
index 720853f70b6..7ec784653b2 100644
--- a/arch/arm64/kernel/setup.c
+++ b/arch/arm64/kernel/setup.c
@@ -393,11 +393,10 @@ void __init setup_arch(char **cmdline_p)
 
 static int __init arm64_device_init(void)
 {
-	of_clk_init(NULL);
 	of_platform_populate(NULL, of_default_bus_match_table, NULL, NULL);
 	return 0;
 }
-arch_initcall(arm64_device_init);
+arch_initcall_sync(arm64_device_init);
 
 static DEFINE_PER_CPU(struct cpu, cpu_data);
 
diff --git a/arch/arm64/kernel/time.c b/arch/arm64/kernel/time.c
index 29c39d5d77e..6815987b50f 100644
--- a/arch/arm64/kernel/time.c
+++ b/arch/arm64/kernel/time.c
@@ -33,6 +33,7 @@
 #include <linux/irq.h>
 #include <linux/delay.h>
 #include <linux/clocksource.h>
+#include <linux/clk-provider.h>
 
 #include <clocksource/arm_arch_timer.h>
 
@@ -65,6 +66,7 @@ void __init time_init(void)
 {
 	u32 arch_timer_rate;
 
+	of_clk_init(NULL);
 	clocksource_of_init();
 
 	arch_timer_rate = arch_timer_get_rate();
diff --git a/arch/arm64/mm/dma-mapping.c b/arch/arm64/mm/dma-mapping.c
index 0ba347e59f0..c851eb44dc5 100644
--- a/arch/arm64/mm/dma-mapping.c
+++ b/arch/arm64/mm/dma-mapping.c
@@ -22,8 +22,11 @@
 #include <linux/slab.h>
 #include <linux/dma-mapping.h>
 #include <linux/dma-contiguous.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
 #include <linux/vmalloc.h>
 #include <linux/swiotlb.h>
+#include <linux/amba/bus.h>
 
 #include <asm/cacheflush.h>
 
@@ -305,17 +308,45 @@ struct dma_map_ops coherent_swiotlb_dma_ops = {
 };
 EXPORT_SYMBOL(coherent_swiotlb_dma_ops);
 
+static int dma_bus_notifier(struct notifier_block *nb,
+			    unsigned long event, void *_dev)
+{
+	struct device *dev = _dev;
+
+	if (event != BUS_NOTIFY_ADD_DEVICE)
+		return NOTIFY_DONE;
+
+	if (of_property_read_bool(dev->of_node, "dma-coherent"))
+		set_dma_ops(dev, &coherent_swiotlb_dma_ops);
+
+	return NOTIFY_OK;
+}
+
+static struct notifier_block platform_bus_nb = {
+	.notifier_call = dma_bus_notifier,
+};
+
+static struct notifier_block amba_bus_nb = {
+	.notifier_call = dma_bus_notifier,
+};
+
 extern int swiotlb_late_init_with_default_size(size_t default_size);
 
 static int __init swiotlb_late_init(void)
 {
 	size_t swiotlb_size = min(SZ_64M, MAX_ORDER_NR_PAGES << PAGE_SHIFT);
 
-	dma_ops = &coherent_swiotlb_dma_ops;
+	/*
+	 * These must be registered before of_platform_populate().
+	 */
+	bus_register_notifier(&platform_bus_type, &platform_bus_nb);
+	bus_register_notifier(&amba_bustype, &amba_bus_nb);
+
+	dma_ops = &noncoherent_swiotlb_dma_ops;
 
 	return swiotlb_late_init_with_default_size(swiotlb_size);
 }
-subsys_initcall(swiotlb_late_init);
+arch_initcall(swiotlb_late_init);
 
 #define PREALLOC_DMA_DEBUG_ENTRIES	4096
 
diff --git a/arch/arm64/mm/hugetlbpage.c b/arch/arm64/mm/hugetlbpage.c
index 5e9aec35830..31eb959e9aa 100644
--- a/arch/arm64/mm/hugetlbpage.c
+++ b/arch/arm64/mm/hugetlbpage.c
@@ -51,7 +51,11 @@ int pmd_huge(pmd_t pmd)
 
 int pud_huge(pud_t pud)
 {
+#ifndef __PAGETABLE_PMD_FOLDED
 	return !(pud_val(pud) & PUD_TABLE_BIT);
+#else
+	return 0;
+#endif
 }
 
 int pmd_huge_support(void)
diff --git a/arch/arm64/mm/mmu.c b/arch/arm64/mm/mmu.c
index 6b7e89569a3..0a472c41a67 100644
--- a/arch/arm64/mm/mmu.c
+++ b/arch/arm64/mm/mmu.c
@@ -374,6 +374,9 @@ int kern_addr_valid(unsigned long addr)
 	if (pmd_none(*pmd))
 		return 0;
 
+	if (pmd_sect(*pmd))
+		return pfn_valid(pmd_pfn(*pmd));
+
 	pte = pte_offset_kernel(pmd, addr);
 	if (pte_none(*pte))
 		return 0;
diff --git a/arch/arm64/xen/hypercall.S b/arch/arm64/xen/hypercall.S
index 531342ec4bc..8bbe9401f4f 100644
--- a/arch/arm64/xen/hypercall.S
+++ b/arch/arm64/xen/hypercall.S
@@ -80,6 +80,7 @@ HYPERCALL2(memory_op);
 HYPERCALL2(physdev_op);
 HYPERCALL3(vcpu_op);
 HYPERCALL1(tmem_op);
+HYPERCALL2(multicall);
 
 ENTRY(privcmd_call)
 	mov x16, x0
diff --git a/arch/avr32/configs/hammerhead_defconfig b/arch/avr32/configs/hammerhead_defconfig
index 18db853386c..4912f0aadaa 100644
--- a/arch/avr32/configs/hammerhead_defconfig
+++ b/arch/avr32/configs/hammerhead_defconfig
@@ -117,7 +117,6 @@ CONFIG_HID_SAMSUNG=m
 CONFIG_HID_SONY=m
 CONFIG_HID_SUNPLUS=m
 CONFIG_USB=m
-CONFIG_USB_DEVICEFS=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_MON=m
 CONFIG_USB_ISP116X_HCD=m
diff --git a/arch/blackfin/configs/BF526-EZBRD_defconfig b/arch/blackfin/configs/BF526-EZBRD_defconfig
index 2f2c6acf210..1759fad5401 100644
--- a/arch/blackfin/configs/BF526-EZBRD_defconfig
+++ b/arch/blackfin/configs/BF526-EZBRD_defconfig
@@ -123,7 +123,6 @@ CONFIG_HID_SAMSUNG=y
 CONFIG_HID_SONY=y
 CONFIG_HID_SUNPLUS=y
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_OTG_BLACKLIST_HUB=y
 CONFIG_USB_MON=y
diff --git a/arch/blackfin/configs/BF527-EZKIT-V2_defconfig b/arch/blackfin/configs/BF527-EZKIT-V2_defconfig
index 91535c38e7f..357729682c0 100644
--- a/arch/blackfin/configs/BF527-EZKIT-V2_defconfig
+++ b/arch/blackfin/configs/BF527-EZKIT-V2_defconfig
@@ -147,7 +147,6 @@ CONFIG_HID_SAMSUNG=y
 CONFIG_HID_SONY=y
 CONFIG_HID_SUNPLUS=y
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_OTG_BLACKLIST_HUB=y
 CONFIG_USB_MON=y
diff --git a/arch/blackfin/configs/BF527-EZKIT_defconfig b/arch/blackfin/configs/BF527-EZKIT_defconfig
index af2738c7441..2e73a5d33da 100644
--- a/arch/blackfin/configs/BF527-EZKIT_defconfig
+++ b/arch/blackfin/configs/BF527-EZKIT_defconfig
@@ -141,7 +141,6 @@ CONFIG_HID_SAMSUNG=y
 CONFIG_HID_SONY=y
 CONFIG_HID_SUNPLUS=y
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_OTG_BLACKLIST_HUB=y
 CONFIG_USB_MON=y
diff --git a/arch/blackfin/configs/BF548-EZKIT_defconfig b/arch/blackfin/configs/BF548-EZKIT_defconfig
index e716fdfd2cf..f0a2ddf5de4 100644
--- a/arch/blackfin/configs/BF548-EZKIT_defconfig
+++ b/arch/blackfin/configs/BF548-EZKIT_defconfig
@@ -159,7 +159,6 @@ CONFIG_HID_SAMSUNG=y
 CONFIG_HID_SONY=y
 CONFIG_HID_SUNPLUS=y
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_OTG_BLACKLIST_HUB=y
 CONFIG_USB_MON=y
diff --git a/arch/blackfin/configs/CM-BF527_defconfig b/arch/blackfin/configs/CM-BF527_defconfig
index f59c80ee78e..05108b85ab1 100644
--- a/arch/blackfin/configs/CM-BF527_defconfig
+++ b/arch/blackfin/configs/CM-BF527_defconfig
@@ -95,7 +95,6 @@ CONFIG_WATCHDOG=y
 CONFIG_BFIN_WDT=y
 CONFIG_USB=m
 CONFIG_USB_ANNOUNCE_NEW_DEVICES=y
-CONFIG_USB_DEVICEFS=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_OTG_BLACKLIST_HUB=y
 CONFIG_USB_MON=m
diff --git a/arch/blackfin/configs/CM-BF548_defconfig b/arch/blackfin/configs/CM-BF548_defconfig
index b9af4fa6998..9ff79df6825 100644
--- a/arch/blackfin/configs/CM-BF548_defconfig
+++ b/arch/blackfin/configs/CM-BF548_defconfig
@@ -94,7 +94,6 @@ CONFIG_WATCHDOG=y
 CONFIG_BFIN_WDT=y
 # CONFIG_HID_SUPPORT is not set
 CONFIG_USB=m
-CONFIG_USB_DEVICEFS=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_MON=m
 CONFIG_USB_MUSB_HDRC=m
diff --git a/arch/blackfin/configs/IP0X_defconfig b/arch/blackfin/configs/IP0X_defconfig
index 62951657876..5adf0da5849 100644
--- a/arch/blackfin/configs/IP0X_defconfig
+++ b/arch/blackfin/configs/IP0X_defconfig
@@ -73,7 +73,6 @@ CONFIG_SPI_BFIN5XX=y
 # CONFIG_HWMON is not set
 CONFIG_WATCHDOG=y
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_OTG_WHITELIST=y
 CONFIG_USB_MON=y
 CONFIG_USB_ISP1362_HCD=y
diff --git a/arch/blackfin/include/asm/pci.h b/arch/blackfin/include/asm/pci.h
index 74352c4597d..c737909fba4 100644
--- a/arch/blackfin/include/asm/pci.h
+++ b/arch/blackfin/include/asm/pci.h
@@ -10,9 +10,4 @@
 #define PCIBIOS_MIN_IO 0x00001000
 #define PCIBIOS_MIN_MEM 0x10000000
 
-static inline void pcibios_penalize_isa_irq(int irq)
-{
-	/* We don't do dynamic PCI IRQ allocation */
-}
-
 #endif				/* _ASM_BFIN_PCI_H */
diff --git a/arch/blackfin/mach-bf609/boards/ezkit.c b/arch/blackfin/mach-bf609/boards/ezkit.c
index 943f7e95ec1..1ba4600de69 100644
--- a/arch/blackfin/mach-bf609/boards/ezkit.c
+++ b/arch/blackfin/mach-bf609/boards/ezkit.c
@@ -20,7 +20,7 @@
 #include <linux/pinctrl/machine.h>
 #include <linux/pinctrl/pinconf-generic.h>
 #include <linux/platform_data/pinctrl-adi2.h>
-#include <asm/bfin_spi3.h>
+#include <linux/spi/adi_spi3.h>
 #include <asm/dma.h>
 #include <asm/gpio.h>
 #include <asm/nand.h>
@@ -767,13 +767,13 @@ static struct flash_platform_data bfin_spi_flash_data = {
 	.type = "w25q32",
 };
 
-static struct bfin_spi3_chip spi_flash_chip_info = {
+static struct adi_spi3_chip spi_flash_chip_info = {
 	.enable_dma = true,         /* use dma transfer with this chip*/
 };
 #endif
 
 #if IS_ENABLED(CONFIG_SPI_SPIDEV)
-static struct bfin_spi3_chip spidev_chip_info = {
+static struct adi_spi3_chip spidev_chip_info = {
 	.enable_dma = true,
 };
 #endif
@@ -1736,7 +1736,7 @@ static struct spi_board_info bfin_spi_board_info[] __initdata = {
 	},
 #endif
 };
-#if IS_ENABLED(CONFIG_SPI_BFIN_V3)
+#if IS_ENABLED(CONFIG_SPI_ADI_V3)
 /* SPI (0) */
 static struct resource bfin_spi0_resource[] = {
 	{
@@ -1777,13 +1777,13 @@ static struct resource bfin_spi1_resource[] = {
 };
 
 /* SPI controller data */
-static struct bfin_spi3_master bf60x_spi_master_info0 = {
+static struct adi_spi3_master bf60x_spi_master_info0 = {
 	.num_chipselect = MAX_CTRL_CS + MAX_BLACKFIN_GPIOS,
 	.pin_req = {P_SPI0_SCK, P_SPI0_MISO, P_SPI0_MOSI, 0},
 };
 
 static struct platform_device bf60x_spi_master0 = {
-	.name = "bfin-spi3",
+	.name = "adi-spi3",
 	.id = 0, /* Bus number */
 	.num_resources = ARRAY_SIZE(bfin_spi0_resource),
 	.resource = bfin_spi0_resource,
@@ -1792,13 +1792,13 @@ static struct platform_device bf60x_spi_master0 = {
 	},
 };
 
-static struct bfin_spi3_master bf60x_spi_master_info1 = {
+static struct adi_spi3_master bf60x_spi_master_info1 = {
 	.num_chipselect = MAX_CTRL_CS + MAX_BLACKFIN_GPIOS,
 	.pin_req = {P_SPI1_SCK, P_SPI1_MISO, P_SPI1_MOSI, 0},
 };
 
 static struct platform_device bf60x_spi_master1 = {
-	.name = "bfin-spi3",
+	.name = "adi-spi3",
 	.id = 1, /* Bus number */
 	.num_resources = ARRAY_SIZE(bfin_spi1_resource),
 	.resource = bfin_spi1_resource,
@@ -1990,7 +1990,7 @@ static struct platform_device *ezkit_devices[] __initdata = {
 	&bfin_sdh_device,
 #endif
 
-#if IS_ENABLED(CONFIG_SPI_BFIN_V3)
+#if IS_ENABLED(CONFIG_SPI_ADI_V3)
 	&bf60x_spi_master0,
 	&bf60x_spi_master1,
 #endif
@@ -2051,8 +2051,8 @@ static struct pinctrl_map __initdata bfin_pinmux_map[] = {
 	PIN_MAP_MUX_GROUP_DEFAULT("bfin_sir.1",  "pinctrl-adi2.0", NULL, "uart1"),
 	PIN_MAP_MUX_GROUP_DEFAULT("bfin-sdh.0",  "pinctrl-adi2.0", NULL, "rsi0"),
 	PIN_MAP_MUX_GROUP_DEFAULT("stmmaceth.0",  "pinctrl-adi2.0", NULL, "eth0"),
-	PIN_MAP_MUX_GROUP_DEFAULT("bfin-spi3.0",  "pinctrl-adi2.0", NULL, "spi0"),
-	PIN_MAP_MUX_GROUP_DEFAULT("bfin-spi3.1",  "pinctrl-adi2.0", NULL, "spi1"),
+	PIN_MAP_MUX_GROUP_DEFAULT("adi-spi3.0",  "pinctrl-adi2.0", NULL, "spi0"),
+	PIN_MAP_MUX_GROUP_DEFAULT("adi-spi3.1",  "pinctrl-adi2.0", NULL, "spi1"),
 	PIN_MAP_MUX_GROUP_DEFAULT("i2c-bfin-twi.0",  "pinctrl-adi2.0", NULL, "twi0"),
 	PIN_MAP_MUX_GROUP_DEFAULT("i2c-bfin-twi.1",  "pinctrl-adi2.0", NULL, "twi1"),
 	PIN_MAP_MUX_GROUP_DEFAULT("bfin-rotary",  "pinctrl-adi2.0", NULL, "rotary"),
diff --git a/arch/blackfin/mach-bf609/clock.c b/arch/blackfin/mach-bf609/clock.c
index 56200f37cfc..244fa4ab4c5 100644
--- a/arch/blackfin/mach-bf609/clock.c
+++ b/arch/blackfin/mach-bf609/clock.c
@@ -363,6 +363,12 @@ static struct clk ethclk = {
 	.ops	    = &dummy_clk_ops,
 };
 
+static struct clk spiclk = {
+	.name       = "spi",
+	.parent     = &sclk1,
+	.ops        = &dummy_clk_ops,
+};
+
 static struct clk_lookup bf609_clks[] = {
 	CLK(sys_clkin, NULL, "SYS_CLKIN"),
 	CLK(pll_clk, NULL, "PLLCLK"),
@@ -375,6 +381,7 @@ static struct clk_lookup bf609_clks[] = {
 	CLK(dclk, NULL, "DCLK"),
 	CLK(oclk, NULL, "OCLK"),
 	CLK(ethclk, NULL, "stmmaceth"),
+	CLK(spiclk, NULL, "spi"),
 };
 
 int __init clk_init(void)
diff --git a/arch/cris/include/asm/pci.h b/arch/cris/include/asm/pci.h
index f666734926d..cc2399c175e 100644
--- a/arch/cris/include/asm/pci.h
+++ b/arch/cris/include/asm/pci.h
@@ -20,7 +20,6 @@ void pcibios_config_init(void);
 struct pci_bus * pcibios_scan_root(int bus);
 
 void pcibios_set_master(struct pci_dev *dev);
-void pcibios_penalize_isa_irq(int irq);
 struct irq_routing_table *pcibios_get_irq_routing_table(void);
 int pcibios_set_irq_routing(struct pci_dev *dev, int pin, int irq);
 
diff --git a/arch/frv/include/asm/pci.h b/arch/frv/include/asm/pci.h
index ef03baf5d89..2035a4d3f9b 100644
--- a/arch/frv/include/asm/pci.h
+++ b/arch/frv/include/asm/pci.h
@@ -24,8 +24,6 @@ struct pci_dev;
 
 extern void pcibios_set_master(struct pci_dev *dev);
 
-extern void pcibios_penalize_isa_irq(int irq);
-
 #ifdef CONFIG_MMU
 extern void *consistent_alloc(gfp_t gfp, size_t size, dma_addr_t *dma_handle);
 extern void consistent_free(void *vaddr);
diff --git a/arch/frv/mb93090-mb00/pci-irq.c b/arch/frv/mb93090-mb00/pci-irq.c
index c677b9d81d3..1c35c93f942 100644
--- a/arch/frv/mb93090-mb00/pci-irq.c
+++ b/arch/frv/mb93090-mb00/pci-irq.c
@@ -55,10 +55,6 @@ void __init pcibios_fixup_irqs(void)
 	}
 }
 
-void __init pcibios_penalize_isa_irq(int irq)
-{
-}
-
 void pcibios_enable_irq(struct pci_dev *dev)
 {
 	pci_write_config_byte(dev, PCI_INTERRUPT_LINE, dev->irq);
diff --git a/arch/ia64/configs/bigsur_defconfig b/arch/ia64/configs/bigsur_defconfig
index cf5993f05d4..4c4ac163c60 100644
--- a/arch/ia64/configs/bigsur_defconfig
+++ b/arch/ia64/configs/bigsur_defconfig
@@ -75,7 +75,6 @@ CONFIG_SND_PCM_OSS=m
 CONFIG_SND_CS4281=m
 CONFIG_USB_HIDDEV=y
 CONFIG_USB=m
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=m
 CONFIG_USB_UHCI_HCD=m
 CONFIG_USB_ACM=m
diff --git a/arch/ia64/configs/generic_defconfig b/arch/ia64/configs/generic_defconfig
index b4efaf2bc13..e8ed3ae70aa 100644
--- a/arch/ia64/configs/generic_defconfig
+++ b/arch/ia64/configs/generic_defconfig
@@ -143,7 +143,6 @@ CONFIG_HID_SAMSUNG=m
 CONFIG_HID_SONY=m
 CONFIG_HID_SUNPLUS=m
 CONFIG_USB=m
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=m
 CONFIG_USB_EHCI_HCD=m
 CONFIG_USB_OHCI_HCD=m
diff --git a/arch/ia64/configs/gensparse_defconfig b/arch/ia64/configs/gensparse_defconfig
index f64980dd20c..d663efd1e4d 100644
--- a/arch/ia64/configs/gensparse_defconfig
+++ b/arch/ia64/configs/gensparse_defconfig
@@ -126,7 +126,6 @@ CONFIG_SND_CS46XX=m
 CONFIG_SND_EMU10K1=m
 CONFIG_SND_FM801=m
 CONFIG_USB=m
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=m
 CONFIG_USB_EHCI_HCD=m
 CONFIG_USB_OHCI_HCD=m
diff --git a/arch/ia64/configs/tiger_defconfig b/arch/ia64/configs/tiger_defconfig
index 0fed9ae5a42..c8a3f40e77f 100644
--- a/arch/ia64/configs/tiger_defconfig
+++ b/arch/ia64/configs/tiger_defconfig
@@ -102,7 +102,6 @@ CONFIG_DRM_RADEON=m
 CONFIG_DRM_MGA=m
 CONFIG_DRM_SIS=m
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_EHCI_HCD=m
 CONFIG_USB_OHCI_HCD=m
 CONFIG_USB_UHCI_HCD=y
diff --git a/arch/ia64/include/asm/pci.h b/arch/ia64/include/asm/pci.h
index 7d41cc08982..52af5ed9f60 100644
--- a/arch/ia64/include/asm/pci.h
+++ b/arch/ia64/include/asm/pci.h
@@ -50,12 +50,6 @@ struct pci_dev;
 extern unsigned long ia64_max_iommu_merge_mask;
 #define PCI_DMA_BUS_IS_PHYS	(ia64_max_iommu_merge_mask == ~0UL)
 
-static inline void
-pcibios_penalize_isa_irq (int irq, int active)
-{
-	/* We don't do dynamic PCI IRQ allocation */
-}
-
 #include <asm-generic/pci-dma-compat.h>
 
 #ifdef CONFIG_PCI
diff --git a/arch/ia64/include/asm/tlb.h b/arch/ia64/include/asm/tlb.h
index bc5efc7c3f3..39d64e0df1d 100644
--- a/arch/ia64/include/asm/tlb.h
+++ b/arch/ia64/include/asm/tlb.h
@@ -91,18 +91,9 @@ extern struct ia64_tr_entry *ia64_idtrs[NR_CPUS];
 #define RR_RID_MASK	0x00000000ffffff00L
 #define RR_TO_RID(val) 	((val >> 8) & 0xffffff)
 
-/*
- * Flush the TLB for address range START to END and, if not in fast mode, release the
- * freed pages that where gathered up to this point.
- */
 static inline void
-ia64_tlb_flush_mmu (struct mmu_gather *tlb, unsigned long start, unsigned long end)
+ia64_tlb_flush_mmu_tlbonly(struct mmu_gather *tlb, unsigned long start, unsigned long end)
 {
-	unsigned long i;
-	unsigned int nr;
-
-	if (!tlb->need_flush)
-		return;
 	tlb->need_flush = 0;
 
 	if (tlb->fullmm) {
@@ -135,6 +126,14 @@ ia64_tlb_flush_mmu (struct mmu_gather *tlb, unsigned long start, unsigned long e
 		flush_tlb_range(&vma, ia64_thash(start), ia64_thash(end));
 	}
 
+}
+
+static inline void
+ia64_tlb_flush_mmu_free(struct mmu_gather *tlb)
+{
+	unsigned long i;
+	unsigned int nr;
+
 	/* lastly, release the freed pages */
 	nr = tlb->nr;
 
@@ -144,6 +143,19 @@ ia64_tlb_flush_mmu (struct mmu_gather *tlb, unsigned long start, unsigned long e
 		free_page_and_swap_cache(tlb->pages[i]);
 }
 
+/*
+ * Flush the TLB for address range START to END and, if not in fast mode, release the
+ * freed pages that where gathered up to this point.
+ */
+static inline void
+ia64_tlb_flush_mmu (struct mmu_gather *tlb, unsigned long start, unsigned long end)
+{
+	if (!tlb->need_flush)
+		return;
+	ia64_tlb_flush_mmu_tlbonly(tlb, start, end);
+	ia64_tlb_flush_mmu_free(tlb);
+}
+
 static inline void __tlb_alloc_page(struct mmu_gather *tlb)
 {
 	unsigned long addr = __get_free_pages(GFP_NOWAIT | __GFP_NOWARN, 0);
@@ -206,6 +218,16 @@ static inline int __tlb_remove_page(struct mmu_gather *tlb, struct page *page)
 	return tlb->max - tlb->nr;
 }
 
+static inline void tlb_flush_mmu_tlbonly(struct mmu_gather *tlb)
+{
+	ia64_tlb_flush_mmu_tlbonly(tlb, tlb->start_addr, tlb->end_addr);
+}
+
+static inline void tlb_flush_mmu_free(struct mmu_gather *tlb)
+{
+	ia64_tlb_flush_mmu_free(tlb);
+}
+
 static inline void tlb_flush_mmu(struct mmu_gather *tlb)
 {
 	ia64_tlb_flush_mmu(tlb, tlb->start_addr, tlb->end_addr);
diff --git a/arch/ia64/include/asm/unistd.h b/arch/ia64/include/asm/unistd.h
index ae763d8bf55..fb13dc5e8f8 100644
--- a/arch/ia64/include/asm/unistd.h
+++ b/arch/ia64/include/asm/unistd.h
@@ -11,7 +11,7 @@
 
 
 
-#define NR_syscalls			314 /* length of syscall table */
+#define NR_syscalls			315 /* length of syscall table */
 
 /*
  * The following defines stop scripts/checksyscalls.sh from complaining about
diff --git a/arch/ia64/include/uapi/asm/unistd.h b/arch/ia64/include/uapi/asm/unistd.h
index 715e85f858d..7de0a2d65da 100644
--- a/arch/ia64/include/uapi/asm/unistd.h
+++ b/arch/ia64/include/uapi/asm/unistd.h
@@ -327,5 +327,6 @@
 #define __NR_finit_module		1335
 #define __NR_sched_setattr		1336
 #define __NR_sched_getattr		1337
+#define __NR_renameat2			1338
 
 #endif /* _UAPI_ASM_IA64_UNISTD_H */
diff --git a/arch/ia64/kernel/entry.S b/arch/ia64/kernel/entry.S
index fa8d61a312a..ba3d03503e8 100644
--- a/arch/ia64/kernel/entry.S
+++ b/arch/ia64/kernel/entry.S
@@ -1775,6 +1775,7 @@ sys_call_table:
 	data8 sys_finit_module			// 1335
 	data8 sys_sched_setattr
 	data8 sys_sched_getattr
+	data8 sys_renameat2
 
 	.org sys_call_table + 8*NR_syscalls	// guard against failures to increase NR_syscalls
 #endif /* __IA64_ASM_PARAVIRTUALIZED_NATIVE */
diff --git a/arch/ia64/pci/fixup.c b/arch/ia64/pci/fixup.c
index eee069a0b53..1fe9aa5068e 100644
--- a/arch/ia64/pci/fixup.c
+++ b/arch/ia64/pci/fixup.c
@@ -49,9 +49,7 @@ static void pci_fixup_video(struct pci_dev *pdev)
 		 * type BRIDGE, or CARDBUS. Host to PCI controllers use
 		 * PCI header type NORMAL.
 		 */
-		if (bridge
-		    &&((bridge->hdr_type == PCI_HEADER_TYPE_BRIDGE)
-		       ||(bridge->hdr_type == PCI_HEADER_TYPE_CARDBUS))) {
+		if (bridge && (pci_is_bridge(bridge))) {
 			pci_read_config_word(bridge, PCI_BRIDGE_CONTROL,
 						&config);
 			if (!(config & PCI_BRIDGE_CTL_VGA))
diff --git a/arch/m68k/Kconfig.debug b/arch/m68k/Kconfig.debug
index 22968272124..64776d7ac19 100644
--- a/arch/m68k/Kconfig.debug
+++ b/arch/m68k/Kconfig.debug
@@ -12,12 +12,17 @@ config BOOTPARAM_STRING
 
 config EARLY_PRINTK
 	bool "Early printk"
-	depends on MVME16x || MAC
+	depends on !(SUN3 || M68360 || M68000 || COLDFIRE)
 	help
           Write kernel log output directly to a serial port.
+          Where implemented, output goes to the framebuffer as well.
+          PROM console functionality on Sun 3x is not affected by this option.
+
+          Pass "earlyprintk" on the kernel command line to get a
+          boot console.
 
           This is useful for kernel debugging when your machine crashes very
-          early before the console code is initialized.
+          early, i.e. before the normal console driver is loaded.
           You should normally say N here, unless you want to debug such a crash.
 
 if !MMU
diff --git a/arch/m68k/amiga/amisound.c b/arch/m68k/amiga/amisound.c
index 2559eefc6af..90a60d758f8 100644
--- a/arch/m68k/amiga/amisound.c
+++ b/arch/m68k/amiga/amisound.c
@@ -51,7 +51,7 @@ void __init amiga_init_sound(void)
 
 	snd_data = amiga_chip_alloc_res(sizeof(sine_data), &beep_res);
 	if (!snd_data) {
-		printk (KERN_CRIT "amiga init_sound: failed to allocate chipmem\n");
+		pr_crit("amiga init_sound: failed to allocate chipmem\n");
 		return;
 	}
 	memcpy (snd_data, sine_data, sizeof(sine_data));
diff --git a/arch/m68k/amiga/config.c b/arch/m68k/amiga/config.c
index 9625b713222..01693df7f2f 100644
--- a/arch/m68k/amiga/config.c
+++ b/arch/m68k/amiga/config.c
@@ -183,7 +183,7 @@ int __init amiga_parse_bootinfo(const struct bi_record *record)
 			dev->boardaddr = be32_to_cpu(cd->cd_BoardAddr);
 			dev->boardsize = be32_to_cpu(cd->cd_BoardSize);
 		} else
-			printk("amiga_parse_bootinfo: too many AutoConfig devices\n");
+			pr_warn("amiga_parse_bootinfo: too many AutoConfig devices\n");
 #endif /* CONFIG_ZORRO */
 		break;
 
@@ -209,9 +209,9 @@ static void __init amiga_identify(void)
 
 	memset(&amiga_hw_present, 0, sizeof(amiga_hw_present));
 
-	printk("Amiga hardware found: ");
+	pr_info("Amiga hardware found: ");
 	if (amiga_model >= AMI_500 && amiga_model <= AMI_DRACO) {
-		printk("[%s] ", amiga_models[amiga_model-AMI_500]);
+		pr_cont("[%s] ", amiga_models[amiga_model-AMI_500]);
 		strcat(amiga_model_name, amiga_models[amiga_model-AMI_500]);
 	}
 
@@ -322,7 +322,7 @@ static void __init amiga_identify(void)
 
 #define AMIGAHW_ANNOUNCE(name, str)		\
 	if (AMIGAHW_PRESENT(name))		\
-		printk(str)
+		pr_cont(str)
 
 	AMIGAHW_ANNOUNCE(AMI_VIDEO, "VIDEO ");
 	AMIGAHW_ANNOUNCE(AMI_BLITTER, "BLITTER ");
@@ -354,8 +354,8 @@ static void __init amiga_identify(void)
 	AMIGAHW_ANNOUNCE(MAGIC_REKICK, "MAGIC_REKICK ");
 	AMIGAHW_ANNOUNCE(PCMCIA, "PCMCIA ");
 	if (AMIGAHW_PRESENT(ZORRO))
-		printk("ZORRO%s ", AMIGAHW_PRESENT(ZORRO3) ? "3" : "");
-	printk("\n");
+		pr_cont("ZORRO%s ", AMIGAHW_PRESENT(ZORRO3) ? "3" : "");
+	pr_cont("\n");
 
 #undef AMIGAHW_ANNOUNCE
 }
@@ -424,7 +424,7 @@ void __init config_amiga(void)
 			if (m68k_memory[i].addr < 16*1024*1024) {
 				if (i == 0) {
 					/* don't cut off the branch we're sitting on */
-					printk("Warning: kernel runs in Zorro II memory\n");
+					pr_warn("Warning: kernel runs in Zorro II memory\n");
 					continue;
 				}
 				disabled_z2mem += m68k_memory[i].size;
@@ -435,8 +435,8 @@ void __init config_amiga(void)
 			}
 		}
 		if (disabled_z2mem)
-		printk("%dK of Zorro II memory will not be used as system memory\n",
-		disabled_z2mem>>10);
+			pr_info("%dK of Zorro II memory will not be used as system memory\n",
+				disabled_z2mem>>10);
 	}
 
 	/* request all RAM */
@@ -475,7 +475,7 @@ static void __init amiga_sched_init(irq_handler_t timer_routine)
 	jiffy_ticks = DIV_ROUND_CLOSEST(amiga_eclock, HZ);
 
 	if (request_resource(&mb_resources._ciab, &sched_res))
-		printk("Cannot allocate ciab.ta{lo,hi}\n");
+		pr_warn("Cannot allocate ciab.ta{lo,hi}\n");
 	ciab.cra &= 0xC0;   /* turn off timer A, continuous mode, from Eclk */
 	ciab.talo = jiffy_ticks % 256;
 	ciab.tahi = jiffy_ticks / 256;
diff --git a/arch/m68k/apollo/config.c b/arch/m68k/apollo/config.c
index 9268c0f9637..6e62d66c396 100644
--- a/arch/m68k/apollo/config.c
+++ b/arch/m68k/apollo/config.c
@@ -65,8 +65,8 @@ int __init apollo_parse_bootinfo(const struct bi_record *record)
 
 static void __init dn_setup_model(void)
 {
-	printk("Apollo hardware found: ");
-	printk("[%s]\n", apollo_models[apollo_model - APOLLO_DN3000]);
+	pr_info("Apollo hardware found: [%s]\n",
+		apollo_models[apollo_model - APOLLO_DN3000]);
 
 	switch(apollo_model) {
 		case APOLLO_UNKNOWN:
@@ -197,8 +197,10 @@ void dn_sched_init(irq_handler_t timer_routine)
 	*(volatile unsigned char *)(pica+1)&=(~8);
 
 #if 0
-	printk("*(0x10803) %02x\n",*(volatile unsigned char *)(apollo_timer + 0x3));
-	printk("*(0x10803) %02x\n",*(volatile unsigned char *)(apollo_timer + 0x3));
+	pr_info("*(0x10803) %02x\n",
+		*(volatile unsigned char *)(apollo_timer + 0x3));
+	pr_info("*(0x10803) %02x\n",
+		*(volatile unsigned char *)(apollo_timer + 0x3));
 #endif
 
 	if (request_irq(IRQ_APOLLO, dn_timer_int, 0, "time", timer_routine))
@@ -236,12 +238,10 @@ int dn_dummy_hwclk(int op, struct rtc_time *t) {
 
 }
 
-int dn_dummy_set_clock_mmss(unsigned long nowtime) {
-
-  printk("set_clock_mmss\n");
-
-  return 0;
-
+int dn_dummy_set_clock_mmss(unsigned long nowtime)
+{
+	pr_info("set_clock_mmss\n");
+	return 0;
 }
 
 void dn_dummy_reset(void) {
diff --git a/arch/m68k/atari/stram.c b/arch/m68k/atari/stram.c
index 0810c8d56e5..5f8cb5a234d 100644
--- a/arch/m68k/atari/stram.c
+++ b/arch/m68k/atari/stram.c
@@ -47,6 +47,7 @@ static struct resource stram_pool = {
 
 static unsigned long pool_size = 1024*1024;
 
+static unsigned long stram_virt_offset;
 
 static int __init atari_stram_setup(char *arg)
 {
@@ -67,14 +68,12 @@ early_param("stram_pool", atari_stram_setup);
 void __init atari_stram_init(void)
 {
 	int i;
-	void *stram_start;
 
 	/*
 	 * determine whether kernel code resides in ST-RAM
 	 * (then ST-RAM is the first memory block at virtual 0x0)
 	 */
-	stram_start = phys_to_virt(0);
-	kernel_in_stram = (stram_start == 0);
+	kernel_in_stram = (m68k_memory[0].addr == 0);
 
 	for (i = 0; i < m68k_num_memory; ++i) {
 		if (m68k_memory[i].addr == 0) {
@@ -89,24 +88,62 @@ void __init atari_stram_init(void)
 
 /*
  * This function is called from setup_arch() to reserve the pages needed for
- * ST-RAM management.
+ * ST-RAM management, if the kernel resides in ST-RAM.
  */
 void __init atari_stram_reserve_pages(void *start_mem)
 {
-	/*
-	 * always reserve first page of ST-RAM, the first 2 KiB are
-	 * supervisor-only!
-	 */
-	if (!kernel_in_stram)
-		reserve_bootmem(0, PAGE_SIZE, BOOTMEM_DEFAULT);
+	if (kernel_in_stram) {
+		pr_debug("atari_stram pool: kernel in ST-RAM, using alloc_bootmem!\n");
+		stram_pool.start = (resource_size_t)alloc_bootmem_low_pages(pool_size);
+		stram_pool.end = stram_pool.start + pool_size - 1;
+		request_resource(&iomem_resource, &stram_pool);
+		stram_virt_offset = 0;
+		pr_debug("atari_stram pool: size = %lu bytes, resource = %pR\n",
+			pool_size, &stram_pool);
+		pr_debug("atari_stram pool: stram_virt_offset = %lx\n",
+			stram_virt_offset);
+	}
+}
 
-	stram_pool.start = (resource_size_t)alloc_bootmem_low_pages(pool_size);
-	stram_pool.end = stram_pool.start + pool_size - 1;
-	request_resource(&iomem_resource, &stram_pool);
 
-	pr_debug("atari_stram pool: size = %lu bytes, resource = %pR\n",
-		 pool_size, &stram_pool);
+/*
+ * This function is called as arch initcall to reserve the pages needed for
+ * ST-RAM management, if the kernel does not reside in ST-RAM.
+ */
+int __init atari_stram_map_pages(void)
+{
+	if (!kernel_in_stram) {
+		/*
+		 * Skip page 0, as the fhe first 2 KiB are supervisor-only!
+		 */
+		pr_debug("atari_stram pool: kernel not in ST-RAM, using ioremap!\n");
+		stram_pool.start = PAGE_SIZE;
+		stram_pool.end = stram_pool.start + pool_size - 1;
+		request_resource(&iomem_resource, &stram_pool);
+		stram_virt_offset = (unsigned long) ioremap(stram_pool.start,
+				resource_size(&stram_pool)) - stram_pool.start;
+		pr_debug("atari_stram pool: size = %lu bytes, resource = %pR\n",
+			pool_size, &stram_pool);
+		pr_debug("atari_stram pool: stram_virt_offset = %lx\n",
+			stram_virt_offset);
+	}
+	return 0;
+}
+arch_initcall(atari_stram_map_pages);
+
+
+void *atari_stram_to_virt(unsigned long phys)
+{
+	return (void *)(phys + stram_virt_offset);
+}
+EXPORT_SYMBOL(atari_stram_to_virt);
+
+
+unsigned long atari_stram_to_phys(void *virt)
+{
+	return (unsigned long)(virt - stram_virt_offset);
 }
+EXPORT_SYMBOL(atari_stram_to_phys);
 
 
 void *atari_stram_alloc(unsigned long size, const char *owner)
@@ -134,14 +171,14 @@ void *atari_stram_alloc(unsigned long size, const char *owner)
 	}
 
 	pr_debug("atari_stram_alloc: returning %pR\n", res);
-	return (void *)res->start;
+	return atari_stram_to_virt(res->start);
 }
 EXPORT_SYMBOL(atari_stram_alloc);
 
 
 void atari_stram_free(void *addr)
 {
-	unsigned long start = (unsigned long)addr;
+	unsigned long start = atari_stram_to_phys(addr);
 	struct resource *res;
 	unsigned long size;
 
diff --git a/arch/m68k/configs/amiga_defconfig b/arch/m68k/configs/amiga_defconfig
index 96da4963d14..d7eac833a94 100644
--- a/arch/m68k/configs/amiga_defconfig
+++ b/arch/m68k/configs/amiga_defconfig
@@ -159,6 +159,7 @@ CONFIG_IP_SET_BITMAP_IP=m
 CONFIG_IP_SET_BITMAP_IPMAC=m
 CONFIG_IP_SET_BITMAP_PORT=m
 CONFIG_IP_SET_HASH_IP=m
+CONFIG_IP_SET_HASH_IPMARK=m
 CONFIG_IP_SET_HASH_IPPORT=m
 CONFIG_IP_SET_HASH_IPPORTIP=m
 CONFIG_IP_SET_HASH_IPPORTNET=m
@@ -227,6 +228,7 @@ CONFIG_DNS_RESOLVER=y
 CONFIG_BATMAN_ADV=m
 CONFIG_BATMAN_ADV_DAT=y
 CONFIG_BATMAN_ADV_NC=y
+CONFIG_BATMAN_ADV_MCAST=y
 CONFIG_NETLINK_DIAG=m
 CONFIG_NET_MPLS_GSO=m
 # CONFIG_WIRELESS is not set
@@ -279,6 +281,7 @@ CONFIG_DM_CRYPT=m
 CONFIG_DM_SNAPSHOT=m
 CONFIG_DM_THIN_PROVISIONING=m
 CONFIG_DM_CACHE=m
+CONFIG_DM_ERA=m
 CONFIG_DM_MIRROR=m
 CONFIG_DM_RAID=m
 CONFIG_DM_ZERO=m
@@ -305,7 +308,6 @@ CONFIG_VETH=m
 CONFIG_A2065=y
 CONFIG_ARIADNE=y
 # CONFIG_NET_VENDOR_ARC is not set
-# CONFIG_NET_CADENCE is not set
 # CONFIG_NET_VENDOR_BROADCOM is not set
 # CONFIG_NET_VENDOR_CIRRUS is not set
 # CONFIG_NET_VENDOR_HP is not set
@@ -315,6 +317,7 @@ CONFIG_ARIADNE=y
 CONFIG_HYDRA=y
 CONFIG_APNE=y
 CONFIG_ZORRO8390=y
+# CONFIG_NET_VENDOR_SAMSUNG is not set
 # CONFIG_NET_VENDOR_SEEQ is not set
 # CONFIG_NET_VENDOR_SMSC is not set
 # CONFIG_NET_VENDOR_STMICRO is not set
diff --git a/arch/m68k/configs/apollo_defconfig b/arch/m68k/configs/apollo_defconfig
index 1b8739f50cb..650ee75de6c 100644
--- a/arch/m68k/configs/apollo_defconfig
+++ b/arch/m68k/configs/apollo_defconfig
@@ -157,6 +157,7 @@ CONFIG_IP_SET_BITMAP_IP=m
 CONFIG_IP_SET_BITMAP_IPMAC=m
 CONFIG_IP_SET_BITMAP_PORT=m
 CONFIG_IP_SET_HASH_IP=m
+CONFIG_IP_SET_HASH_IPMARK=m
 CONFIG_IP_SET_HASH_IPPORT=m
 CONFIG_IP_SET_HASH_IPPORTIP=m
 CONFIG_IP_SET_HASH_IPPORTNET=m
@@ -225,6 +226,7 @@ CONFIG_DNS_RESOLVER=y
 CONFIG_BATMAN_ADV=m
 CONFIG_BATMAN_ADV_DAT=y
 CONFIG_BATMAN_ADV_NC=y
+CONFIG_BATMAN_ADV_MCAST=y
 CONFIG_NETLINK_DIAG=m
 CONFIG_NET_MPLS_GSO=m
 # CONFIG_WIRELESS is not set
@@ -261,6 +263,7 @@ CONFIG_DM_CRYPT=m
 CONFIG_DM_SNAPSHOT=m
 CONFIG_DM_THIN_PROVISIONING=m
 CONFIG_DM_CACHE=m
+CONFIG_DM_ERA=m
 CONFIG_DM_MIRROR=m
 CONFIG_DM_RAID=m
 CONFIG_DM_ZERO=m
@@ -284,12 +287,12 @@ CONFIG_NETCONSOLE=m
 CONFIG_NETCONSOLE_DYNAMIC=y
 CONFIG_VETH=m
 # CONFIG_NET_VENDOR_ARC is not set
-# CONFIG_NET_CADENCE is not set
 # CONFIG_NET_VENDOR_BROADCOM is not set
 # CONFIG_NET_VENDOR_INTEL is not set
 # CONFIG_NET_VENDOR_MARVELL is not set
 # CONFIG_NET_VENDOR_MICREL is not set
 # CONFIG_NET_VENDOR_NATSEMI is not set
+# CONFIG_NET_VENDOR_SAMSUNG is not set
 # CONFIG_NET_VENDOR_SEEQ is not set
 # CONFIG_NET_VENDOR_STMICRO is not set
 # CONFIG_NET_VENDOR_VIA is not set
diff --git a/arch/m68k/configs/atari_defconfig b/arch/m68k/configs/atari_defconfig
index 6ea4e91f0ca..3142e69342f 100644
--- a/arch/m68k/configs/atari_defconfig
+++ b/arch/m68k/configs/atari_defconfig
@@ -156,6 +156,7 @@ CONFIG_IP_SET_BITMAP_IP=m
 CONFIG_IP_SET_BITMAP_IPMAC=m
 CONFIG_IP_SET_BITMAP_PORT=m
 CONFIG_IP_SET_HASH_IP=m
+CONFIG_IP_SET_HASH_IPMARK=m
 CONFIG_IP_SET_HASH_IPPORT=m
 CONFIG_IP_SET_HASH_IPPORTIP=m
 CONFIG_IP_SET_HASH_IPPORTNET=m
@@ -224,6 +225,7 @@ CONFIG_DNS_RESOLVER=y
 CONFIG_BATMAN_ADV=m
 CONFIG_BATMAN_ADV_DAT=y
 CONFIG_BATMAN_ADV_NC=y
+CONFIG_BATMAN_ADV_MCAST=y
 CONFIG_NETLINK_DIAG=m
 CONFIG_NET_MPLS_GSO=m
 # CONFIG_WIRELESS is not set
@@ -269,6 +271,7 @@ CONFIG_DM_CRYPT=m
 CONFIG_DM_SNAPSHOT=m
 CONFIG_DM_THIN_PROVISIONING=m
 CONFIG_DM_CACHE=m
+CONFIG_DM_ERA=m
 CONFIG_DM_MIRROR=m
 CONFIG_DM_RAID=m
 CONFIG_DM_ZERO=m
@@ -293,11 +296,11 @@ CONFIG_NETCONSOLE_DYNAMIC=y
 CONFIG_VETH=m
 CONFIG_ATARILANCE=y
 # CONFIG_NET_VENDOR_ARC is not set
-# CONFIG_NET_CADENCE is not set
 # CONFIG_NET_VENDOR_BROADCOM is not set
 # CONFIG_NET_VENDOR_INTEL is not set
 # CONFIG_NET_VENDOR_MARVELL is not set
 # CONFIG_NET_VENDOR_MICREL is not set
+# CONFIG_NET_VENDOR_SAMSUNG is not set
 # CONFIG_NET_VENDOR_SEEQ is not set
 # CONFIG_NET_VENDOR_STMICRO is not set
 # CONFIG_NET_VENDOR_VIA is not set
diff --git a/arch/m68k/configs/bvme6000_defconfig b/arch/m68k/configs/bvme6000_defconfig
index e5a12739ff2..0daa8a172f3 100644
--- a/arch/m68k/configs/bvme6000_defconfig
+++ b/arch/m68k/configs/bvme6000_defconfig
@@ -155,6 +155,7 @@ CONFIG_IP_SET_BITMAP_IP=m
 CONFIG_IP_SET_BITMAP_IPMAC=m
 CONFIG_IP_SET_BITMAP_PORT=m
 CONFIG_IP_SET_HASH_IP=m
+CONFIG_IP_SET_HASH_IPMARK=m
 CONFIG_IP_SET_HASH_IPPORT=m
 CONFIG_IP_SET_HASH_IPPORTIP=m
 CONFIG_IP_SET_HASH_IPPORTNET=m
@@ -223,6 +224,7 @@ CONFIG_DNS_RESOLVER=y
 CONFIG_BATMAN_ADV=m
 CONFIG_BATMAN_ADV_DAT=y
 CONFIG_BATMAN_ADV_NC=y
+CONFIG_BATMAN_ADV_MCAST=y
 CONFIG_NETLINK_DIAG=m
 CONFIG_NET_MPLS_GSO=m
 # CONFIG_WIRELESS is not set
@@ -260,6 +262,7 @@ CONFIG_DM_CRYPT=m
 CONFIG_DM_SNAPSHOT=m
 CONFIG_DM_THIN_PROVISIONING=m
 CONFIG_DM_CACHE=m
+CONFIG_DM_ERA=m
 CONFIG_DM_MIRROR=m
 CONFIG_DM_RAID=m
 CONFIG_DM_ZERO=m
@@ -283,12 +286,12 @@ CONFIG_NETCONSOLE=m
 CONFIG_NETCONSOLE_DYNAMIC=y
 CONFIG_VETH=m
 # CONFIG_NET_VENDOR_ARC is not set
-# CONFIG_NET_CADENCE is not set
 # CONFIG_NET_VENDOR_BROADCOM is not set
 CONFIG_BVME6000_NET=y
 # CONFIG_NET_VENDOR_MARVELL is not set
 # CONFIG_NET_VENDOR_MICREL is not set
 # CONFIG_NET_VENDOR_NATSEMI is not set
+# CONFIG_NET_VENDOR_SAMSUNG is not set
 # CONFIG_NET_VENDOR_SEEQ is not set
 # CONFIG_NET_VENDOR_STMICRO is not set
 # CONFIG_NET_VENDOR_VIA is not set
diff --git a/arch/m68k/configs/hp300_defconfig b/arch/m68k/configs/hp300_defconfig
index 8936d7fb0f0..88af78f7bad 100644
--- a/arch/m68k/configs/hp300_defconfig
+++ b/arch/m68k/configs/hp300_defconfig
@@ -157,6 +157,7 @@ CONFIG_IP_SET_BITMAP_IP=m
 CONFIG_IP_SET_BITMAP_IPMAC=m
 CONFIG_IP_SET_BITMAP_PORT=m
 CONFIG_IP_SET_HASH_IP=m
+CONFIG_IP_SET_HASH_IPMARK=m
 CONFIG_IP_SET_HASH_IPPORT=m
 CONFIG_IP_SET_HASH_IPPORTIP=m
 CONFIG_IP_SET_HASH_IPPORTNET=m
@@ -225,6 +226,7 @@ CONFIG_DNS_RESOLVER=y
 CONFIG_BATMAN_ADV=m
 CONFIG_BATMAN_ADV_DAT=y
 CONFIG_BATMAN_ADV_NC=y
+CONFIG_BATMAN_ADV_MCAST=y
 CONFIG_NETLINK_DIAG=m
 CONFIG_NET_MPLS_GSO=m
 # CONFIG_WIRELESS is not set
@@ -261,6 +263,7 @@ CONFIG_DM_CRYPT=m
 CONFIG_DM_SNAPSHOT=m
 CONFIG_DM_THIN_PROVISIONING=m
 CONFIG_DM_CACHE=m
+CONFIG_DM_ERA=m
 CONFIG_DM_MIRROR=m
 CONFIG_DM_RAID=m
 CONFIG_DM_ZERO=m
@@ -285,12 +288,12 @@ CONFIG_NETCONSOLE_DYNAMIC=y
 CONFIG_VETH=m
 CONFIG_HPLANCE=y
 # CONFIG_NET_VENDOR_ARC is not set
-# CONFIG_NET_CADENCE is not set
 # CONFIG_NET_VENDOR_BROADCOM is not set
 # CONFIG_NET_VENDOR_INTEL is not set
 # CONFIG_NET_VENDOR_MARVELL is not set
 # CONFIG_NET_VENDOR_MICREL is not set
 # CONFIG_NET_VENDOR_NATSEMI is not set
+# CONFIG_NET_VENDOR_SAMSUNG is not set
 # CONFIG_NET_VENDOR_SEEQ is not set
 # CONFIG_NET_VENDOR_STMICRO is not set
 # CONFIG_NET_VENDOR_VIA is not set
diff --git a/arch/m68k/configs/mac_defconfig b/arch/m68k/configs/mac_defconfig
index be5342cca25..66f915574a8 100644
--- a/arch/m68k/configs/mac_defconfig
+++ b/arch/m68k/configs/mac_defconfig
@@ -156,6 +156,7 @@ CONFIG_IP_SET_BITMAP_IP=m
 CONFIG_IP_SET_BITMAP_IPMAC=m
 CONFIG_IP_SET_BITMAP_PORT=m
 CONFIG_IP_SET_HASH_IP=m
+CONFIG_IP_SET_HASH_IPMARK=m
 CONFIG_IP_SET_HASH_IPPORT=m
 CONFIG_IP_SET_HASH_IPPORTIP=m
 CONFIG_IP_SET_HASH_IPPORTNET=m
@@ -227,6 +228,7 @@ CONFIG_DNS_RESOLVER=y
 CONFIG_BATMAN_ADV=m
 CONFIG_BATMAN_ADV_DAT=y
 CONFIG_BATMAN_ADV_NC=y
+CONFIG_BATMAN_ADV_MCAST=y
 CONFIG_NETLINK_DIAG=m
 CONFIG_NET_MPLS_GSO=m
 # CONFIG_WIRELESS is not set
@@ -270,6 +272,7 @@ CONFIG_DM_CRYPT=m
 CONFIG_DM_SNAPSHOT=m
 CONFIG_DM_THIN_PROVISIONING=m
 CONFIG_DM_CACHE=m
+CONFIG_DM_ERA=m
 CONFIG_DM_MIRROR=m
 CONFIG_DM_RAID=m
 CONFIG_DM_ZERO=m
@@ -301,7 +304,6 @@ CONFIG_NETCONSOLE_DYNAMIC=y
 CONFIG_VETH=m
 CONFIG_MACMACE=y
 # CONFIG_NET_VENDOR_ARC is not set
-# CONFIG_NET_CADENCE is not set
 # CONFIG_NET_VENDOR_BROADCOM is not set
 CONFIG_MAC89x0=y
 # CONFIG_NET_VENDOR_INTEL is not set
@@ -309,6 +311,7 @@ CONFIG_MAC89x0=y
 # CONFIG_NET_VENDOR_MICREL is not set
 CONFIG_MACSONIC=y
 CONFIG_MAC8390=y
+# CONFIG_NET_VENDOR_SAMSUNG is not set
 # CONFIG_NET_VENDOR_SEEQ is not set
 # CONFIG_NET_VENDOR_SMSC is not set
 # CONFIG_NET_VENDOR_STMICRO is not set
diff --git a/arch/m68k/configs/multi_defconfig b/arch/m68k/configs/multi_defconfig
index f27194ade16..5eaa49924fa 100644
--- a/arch/m68k/configs/multi_defconfig
+++ b/arch/m68k/configs/multi_defconfig
@@ -165,6 +165,7 @@ CONFIG_IP_SET_BITMAP_IP=m
 CONFIG_IP_SET_BITMAP_IPMAC=m
 CONFIG_IP_SET_BITMAP_PORT=m
 CONFIG_IP_SET_HASH_IP=m
+CONFIG_IP_SET_HASH_IPMARK=m
 CONFIG_IP_SET_HASH_IPPORT=m
 CONFIG_IP_SET_HASH_IPPORTIP=m
 CONFIG_IP_SET_HASH_IPPORTNET=m
@@ -236,6 +237,7 @@ CONFIG_DNS_RESOLVER=y
 CONFIG_BATMAN_ADV=m
 CONFIG_BATMAN_ADV_DAT=y
 CONFIG_BATMAN_ADV_NC=y
+CONFIG_BATMAN_ADV_MCAST=y
 CONFIG_NETLINK_DIAG=m
 CONFIG_NET_MPLS_GSO=m
 # CONFIG_WIRELESS is not set
@@ -302,6 +304,7 @@ CONFIG_DM_CRYPT=m
 CONFIG_DM_SNAPSHOT=m
 CONFIG_DM_THIN_PROVISIONING=m
 CONFIG_DM_CACHE=m
+CONFIG_DM_ERA=m
 CONFIG_DM_MIRROR=m
 CONFIG_DM_RAID=m
 CONFIG_DM_ZERO=m
@@ -340,7 +343,6 @@ CONFIG_MVME147_NET=y
 CONFIG_SUN3LANCE=y
 CONFIG_MACMACE=y
 # CONFIG_NET_VENDOR_ARC is not set
-# CONFIG_NET_CADENCE is not set
 # CONFIG_NET_VENDOR_BROADCOM is not set
 CONFIG_MAC89x0=y
 # CONFIG_NET_VENDOR_HP is not set
@@ -354,6 +356,7 @@ CONFIG_MAC8390=y
 CONFIG_NE2000=m
 CONFIG_APNE=y
 CONFIG_ZORRO8390=y
+# CONFIG_NET_VENDOR_SAMSUNG is not set
 # CONFIG_NET_VENDOR_SEEQ is not set
 # CONFIG_NET_VENDOR_STMICRO is not set
 # CONFIG_NET_VENDOR_VIA is not set
diff --git a/arch/m68k/configs/mvme147_defconfig b/arch/m68k/configs/mvme147_defconfig
index c3887603c1d..324d0b4d835 100644
--- a/arch/m68k/configs/mvme147_defconfig
+++ b/arch/m68k/configs/mvme147_defconfig
@@ -154,6 +154,7 @@ CONFIG_IP_SET_BITMAP_IP=m
 CONFIG_IP_SET_BITMAP_IPMAC=m
 CONFIG_IP_SET_BITMAP_PORT=m
 CONFIG_IP_SET_HASH_IP=m
+CONFIG_IP_SET_HASH_IPMARK=m
 CONFIG_IP_SET_HASH_IPPORT=m
 CONFIG_IP_SET_HASH_IPPORTIP=m
 CONFIG_IP_SET_HASH_IPPORTNET=m
@@ -222,6 +223,7 @@ CONFIG_DNS_RESOLVER=y
 CONFIG_BATMAN_ADV=m
 CONFIG_BATMAN_ADV_DAT=y
 CONFIG_BATMAN_ADV_NC=y
+CONFIG_BATMAN_ADV_MCAST=y
 CONFIG_NETLINK_DIAG=m
 CONFIG_NET_MPLS_GSO=m
 # CONFIG_WIRELESS is not set
@@ -259,6 +261,7 @@ CONFIG_DM_CRYPT=m
 CONFIG_DM_SNAPSHOT=m
 CONFIG_DM_THIN_PROVISIONING=m
 CONFIG_DM_CACHE=m
+CONFIG_DM_ERA=m
 CONFIG_DM_MIRROR=m
 CONFIG_DM_RAID=m
 CONFIG_DM_ZERO=m
@@ -283,12 +286,12 @@ CONFIG_NETCONSOLE_DYNAMIC=y
 CONFIG_VETH=m
 CONFIG_MVME147_NET=y
 # CONFIG_NET_VENDOR_ARC is not set
-# CONFIG_NET_CADENCE is not set
 # CONFIG_NET_VENDOR_BROADCOM is not set
 # CONFIG_NET_VENDOR_INTEL is not set
 # CONFIG_NET_VENDOR_MARVELL is not set
 # CONFIG_NET_VENDOR_MICREL is not set
 # CONFIG_NET_VENDOR_NATSEMI is not set
+# CONFIG_NET_VENDOR_SAMSUNG is not set
 # CONFIG_NET_VENDOR_SEEQ is not set
 # CONFIG_NET_VENDOR_STMICRO is not set
 # CONFIG_NET_VENDOR_VIA is not set
diff --git a/arch/m68k/configs/mvme16x_defconfig b/arch/m68k/configs/mvme16x_defconfig
index f7ff784d05a..f0cb4338952 100644
--- a/arch/m68k/configs/mvme16x_defconfig
+++ b/arch/m68k/configs/mvme16x_defconfig
@@ -155,6 +155,7 @@ CONFIG_IP_SET_BITMAP_IP=m
 CONFIG_IP_SET_BITMAP_IPMAC=m
 CONFIG_IP_SET_BITMAP_PORT=m
 CONFIG_IP_SET_HASH_IP=m
+CONFIG_IP_SET_HASH_IPMARK=m
 CONFIG_IP_SET_HASH_IPPORT=m
 CONFIG_IP_SET_HASH_IPPORTIP=m
 CONFIG_IP_SET_HASH_IPPORTNET=m
@@ -223,6 +224,7 @@ CONFIG_DNS_RESOLVER=y
 CONFIG_BATMAN_ADV=m
 CONFIG_BATMAN_ADV_DAT=y
 CONFIG_BATMAN_ADV_NC=y
+CONFIG_BATMAN_ADV_MCAST=y
 CONFIG_NETLINK_DIAG=m
 CONFIG_NET_MPLS_GSO=m
 # CONFIG_WIRELESS is not set
@@ -260,6 +262,7 @@ CONFIG_DM_CRYPT=m
 CONFIG_DM_SNAPSHOT=m
 CONFIG_DM_THIN_PROVISIONING=m
 CONFIG_DM_CACHE=m
+CONFIG_DM_ERA=m
 CONFIG_DM_MIRROR=m
 CONFIG_DM_RAID=m
 CONFIG_DM_ZERO=m
@@ -283,12 +286,12 @@ CONFIG_NETCONSOLE=m
 CONFIG_NETCONSOLE_DYNAMIC=y
 CONFIG_VETH=m
 # CONFIG_NET_VENDOR_ARC is not set
-# CONFIG_NET_CADENCE is not set
 # CONFIG_NET_VENDOR_BROADCOM is not set
 CONFIG_MVME16x_NET=y
 # CONFIG_NET_VENDOR_MARVELL is not set
 # CONFIG_NET_VENDOR_MICREL is not set
 # CONFIG_NET_VENDOR_NATSEMI is not set
+# CONFIG_NET_VENDOR_SAMSUNG is not set
 # CONFIG_NET_VENDOR_SEEQ is not set
 # CONFIG_NET_VENDOR_STMICRO is not set
 # CONFIG_NET_VENDOR_VIA is not set
diff --git a/arch/m68k/configs/q40_defconfig b/arch/m68k/configs/q40_defconfig
index f0c72ab037b..d6cf0880c46 100644
--- a/arch/m68k/configs/q40_defconfig
+++ b/arch/m68k/configs/q40_defconfig
@@ -155,6 +155,7 @@ CONFIG_IP_SET_BITMAP_IP=m
 CONFIG_IP_SET_BITMAP_IPMAC=m
 CONFIG_IP_SET_BITMAP_PORT=m
 CONFIG_IP_SET_HASH_IP=m
+CONFIG_IP_SET_HASH_IPMARK=m
 CONFIG_IP_SET_HASH_IPPORT=m
 CONFIG_IP_SET_HASH_IPPORTIP=m
 CONFIG_IP_SET_HASH_IPPORTNET=m
@@ -223,6 +224,7 @@ CONFIG_DNS_RESOLVER=y
 CONFIG_BATMAN_ADV=m
 CONFIG_BATMAN_ADV_DAT=y
 CONFIG_BATMAN_ADV_NC=y
+CONFIG_BATMAN_ADV_MCAST=y
 CONFIG_NETLINK_DIAG=m
 CONFIG_NET_MPLS_GSO=m
 # CONFIG_WIRELESS is not set
@@ -266,6 +268,7 @@ CONFIG_DM_CRYPT=m
 CONFIG_DM_SNAPSHOT=m
 CONFIG_DM_THIN_PROVISIONING=m
 CONFIG_DM_CACHE=m
+CONFIG_DM_ERA=m
 CONFIG_DM_MIRROR=m
 CONFIG_DM_RAID=m
 CONFIG_DM_ZERO=m
@@ -291,7 +294,6 @@ CONFIG_VETH=m
 # CONFIG_NET_VENDOR_3COM is not set
 # CONFIG_NET_VENDOR_AMD is not set
 # CONFIG_NET_VENDOR_ARC is not set
-# CONFIG_NET_CADENCE is not set
 # CONFIG_NET_VENDOR_BROADCOM is not set
 # CONFIG_NET_VENDOR_CIRRUS is not set
 # CONFIG_NET_VENDOR_HP is not set
@@ -299,6 +301,7 @@ CONFIG_VETH=m
 # CONFIG_NET_VENDOR_MARVELL is not set
 # CONFIG_NET_VENDOR_MICREL is not set
 CONFIG_NE2000=m
+# CONFIG_NET_VENDOR_SAMSUNG is not set
 # CONFIG_NET_VENDOR_SEEQ is not set
 # CONFIG_NET_VENDOR_SMSC is not set
 # CONFIG_NET_VENDOR_STMICRO is not set
diff --git a/arch/m68k/configs/sun3_defconfig b/arch/m68k/configs/sun3_defconfig
index 7bca0f46452..f4e88d1c747 100644
--- a/arch/m68k/configs/sun3_defconfig
+++ b/arch/m68k/configs/sun3_defconfig
@@ -152,6 +152,7 @@ CONFIG_IP_SET_BITMAP_IP=m
 CONFIG_IP_SET_BITMAP_IPMAC=m
 CONFIG_IP_SET_BITMAP_PORT=m
 CONFIG_IP_SET_HASH_IP=m
+CONFIG_IP_SET_HASH_IPMARK=m
 CONFIG_IP_SET_HASH_IPPORT=m
 CONFIG_IP_SET_HASH_IPPORTIP=m
 CONFIG_IP_SET_HASH_IPPORTNET=m
@@ -220,6 +221,7 @@ CONFIG_DNS_RESOLVER=y
 CONFIG_BATMAN_ADV=m
 CONFIG_BATMAN_ADV_DAT=y
 CONFIG_BATMAN_ADV_NC=y
+CONFIG_BATMAN_ADV_MCAST=y
 CONFIG_NETLINK_DIAG=m
 CONFIG_NET_MPLS_GSO=m
 # CONFIG_WIRELESS is not set
@@ -257,6 +259,7 @@ CONFIG_DM_CRYPT=m
 CONFIG_DM_SNAPSHOT=m
 CONFIG_DM_THIN_PROVISIONING=m
 CONFIG_DM_CACHE=m
+CONFIG_DM_ERA=m
 CONFIG_DM_MIRROR=m
 CONFIG_DM_RAID=m
 CONFIG_DM_ZERO=m
@@ -281,11 +284,11 @@ CONFIG_NETCONSOLE_DYNAMIC=y
 CONFIG_VETH=m
 CONFIG_SUN3LANCE=y
 # CONFIG_NET_VENDOR_ARC is not set
-# CONFIG_NET_CADENCE is not set
 CONFIG_SUN3_82586=y
 # CONFIG_NET_VENDOR_MARVELL is not set
 # CONFIG_NET_VENDOR_MICREL is not set
 # CONFIG_NET_VENDOR_NATSEMI is not set
+# CONFIG_NET_VENDOR_SAMSUNG is not set
 # CONFIG_NET_VENDOR_SEEQ is not set
 # CONFIG_NET_VENDOR_STMICRO is not set
 # CONFIG_NET_VENDOR_SUN is not set
diff --git a/arch/m68k/configs/sun3x_defconfig b/arch/m68k/configs/sun3x_defconfig
index 317f3e1fec9..49f4032c1ad 100644
--- a/arch/m68k/configs/sun3x_defconfig
+++ b/arch/m68k/configs/sun3x_defconfig
@@ -152,6 +152,7 @@ CONFIG_IP_SET_BITMAP_IP=m
 CONFIG_IP_SET_BITMAP_IPMAC=m
 CONFIG_IP_SET_BITMAP_PORT=m
 CONFIG_IP_SET_HASH_IP=m
+CONFIG_IP_SET_HASH_IPMARK=m
 CONFIG_IP_SET_HASH_IPPORT=m
 CONFIG_IP_SET_HASH_IPPORTIP=m
 CONFIG_IP_SET_HASH_IPPORTNET=m
@@ -220,6 +221,7 @@ CONFIG_DNS_RESOLVER=y
 CONFIG_BATMAN_ADV=m
 CONFIG_BATMAN_ADV_DAT=y
 CONFIG_BATMAN_ADV_NC=y
+CONFIG_BATMAN_ADV_MCAST=y
 CONFIG_NETLINK_DIAG=m
 CONFIG_NET_MPLS_GSO=m
 # CONFIG_WIRELESS is not set
@@ -257,6 +259,7 @@ CONFIG_DM_CRYPT=m
 CONFIG_DM_SNAPSHOT=m
 CONFIG_DM_THIN_PROVISIONING=m
 CONFIG_DM_CACHE=m
+CONFIG_DM_ERA=m
 CONFIG_DM_MIRROR=m
 CONFIG_DM_RAID=m
 CONFIG_DM_ZERO=m
@@ -281,12 +284,12 @@ CONFIG_NETCONSOLE_DYNAMIC=y
 CONFIG_VETH=m
 CONFIG_SUN3LANCE=y
 # CONFIG_NET_VENDOR_ARC is not set
-# CONFIG_NET_CADENCE is not set
 # CONFIG_NET_VENDOR_BROADCOM is not set
 # CONFIG_NET_VENDOR_INTEL is not set
 # CONFIG_NET_VENDOR_MARVELL is not set
 # CONFIG_NET_VENDOR_MICREL is not set
 # CONFIG_NET_VENDOR_NATSEMI is not set
+# CONFIG_NET_VENDOR_SAMSUNG is not set
 # CONFIG_NET_VENDOR_SEEQ is not set
 # CONFIG_NET_VENDOR_STMICRO is not set
 # CONFIG_NET_VENDOR_VIA is not set
diff --git a/arch/m68k/hp300/config.c b/arch/m68k/hp300/config.c
index 2e5a787ea11..a9befe65adc 100644
--- a/arch/m68k/hp300/config.c
+++ b/arch/m68k/hp300/config.c
@@ -87,7 +87,7 @@ int __init hp300_parse_bootinfo(const struct bi_record *record)
 		/* serial port address: ignored here */
 		break;
 
-        default:
+	default:
 		unknown = 1;
 	}
 
@@ -262,11 +262,12 @@ void __init config_hp300(void)
 #endif
 	mach_max_dma_address = 0xffffffff;
 
-	if (hp300_model >= HP_330 && hp300_model <= HP_433S && hp300_model != HP_350) {
-		printk(KERN_INFO "Detected HP9000 model %s\n", hp300_models[hp300_model-HP_320]);
+	if (hp300_model >= HP_330 && hp300_model <= HP_433S &&
+	    hp300_model != HP_350) {
+		pr_info("Detected HP9000 model %s\n",
+			hp300_models[hp300_model-HP_320]);
 		strcat(hp300_model_name, hp300_models[hp300_model-HP_320]);
-	}
-	else {
+	} else {
 		panic("Unknown HP9000 Model");
 	}
 #ifdef CONFIG_SERIAL_8250_CONSOLE
diff --git a/arch/m68k/include/asm/atari_stram.h b/arch/m68k/include/asm/atari_stram.h
index 62e27598af9..4e771c22d6a 100644
--- a/arch/m68k/include/asm/atari_stram.h
+++ b/arch/m68k/include/asm/atari_stram.h
@@ -8,6 +8,8 @@
 /* public interface */
 void *atari_stram_alloc(unsigned long size, const char *owner);
 void atari_stram_free(void *);
+void *atari_stram_to_virt(unsigned long phys);
+unsigned long atari_stram_to_phys(void *);
 
 /* functions called internally by other parts of the kernel */
 void atari_stram_init(void);
diff --git a/arch/m68k/include/asm/m525xsim.h b/arch/m68k/include/asm/m525xsim.h
index e33f5bb6aca..f186459072e 100644
--- a/arch/m68k/include/asm/m525xsim.h
+++ b/arch/m68k/include/asm/m525xsim.h
@@ -105,7 +105,7 @@
 /*
  *	QSPI module.
  */
-#define MCFQSPI_BASE		(MCF_MBAR + 0x300)	/* Base address QSPI */
+#define MCFQSPI_BASE		(MCF_MBAR + 0x400)	/* Base address QSPI */
 #define MCFQSPI_SIZE		0x40			/* Register set size */
 
 #ifdef CONFIG_M5249
diff --git a/arch/m68k/include/asm/m54xxsim.h b/arch/m68k/include/asm/m54xxsim.h
index d3bd8388742..a5fbd17ab0a 100644
--- a/arch/m68k/include/asm/m54xxsim.h
+++ b/arch/m68k/include/asm/m54xxsim.h
@@ -55,9 +55,15 @@
 /*
  *	Generic GPIO support
  */
-#define MCFGPIO_PIN_MAX		0	/* I am too lazy to count */
-#define MCFGPIO_IRQ_MAX		-1
-#define MCFGPIO_IRQ_VECBASE	-1
+#define MCFGPIO_PODR		(MCF_MBAR + 0xA00)
+#define MCFGPIO_PDDR		(MCF_MBAR + 0xA10)
+#define MCFGPIO_PPDR		(MCF_MBAR + 0xA20)
+#define MCFGPIO_SETR		(MCF_MBAR + 0xA20)
+#define MCFGPIO_CLRR		(MCF_MBAR + 0xA30)
+
+#define MCFGPIO_PIN_MAX		136	/* 128 gpio + 8 eport */
+#define MCFGPIO_IRQ_MAX		8
+#define MCFGPIO_IRQ_VECBASE	MCFINT_VECBASE
 
 /*
  *	EDGE Port support.
diff --git a/arch/m68k/include/asm/mcfgpio.h b/arch/m68k/include/asm/mcfgpio.h
index c41ebf45f1d..66203c334c6 100644
--- a/arch/m68k/include/asm/mcfgpio.h
+++ b/arch/m68k/include/asm/mcfgpio.h
@@ -139,7 +139,8 @@ static inline void gpio_free(unsigned gpio)
 
 #if defined(CONFIG_M520x) || defined(CONFIG_M523x) || \
     defined(CONFIG_M527x) || defined(CONFIG_M528x) || \
-    defined(CONFIG_M53xx) || defined(CONFIG_M5441x)
+    defined(CONFIG_M53xx) || defined(CONFIG_M54xx) || \
+    defined(CONFIG_M5441x)
 /*
  * These parts have an 'Edge' Port module (external interrupt/GPIO) which uses
  * read-modify-write to change an output and a GPIO module which has separate
@@ -195,7 +196,8 @@ static inline u32 __mcfgpio_ppdr(unsigned gpio)
 		return MCFSIM2_GPIO1READ;
 #elif defined(CONFIG_M520x) || defined(CONFIG_M523x) || \
       defined(CONFIG_M527x) || defined(CONFIG_M528x) || \
-      defined(CONFIG_M53xx) || defined(CONFIG_M5441x)
+      defined(CONFIG_M53xx) || defined(CONFIG_M54xx) || \
+      defined(CONFIG_M5441x)
 #if !defined(CONFIG_M5441x)
 	if (gpio < 8)
 		return MCFEPORT_EPPDR;
@@ -237,7 +239,8 @@ static inline u32 __mcfgpio_podr(unsigned gpio)
 		return MCFSIM2_GPIO1WRITE;
 #elif defined(CONFIG_M520x) || defined(CONFIG_M523x) || \
       defined(CONFIG_M527x) || defined(CONFIG_M528x) || \
-      defined(CONFIG_M53xx) || defined(CONFIG_M5441x)
+      defined(CONFIG_M53xx) || defined(CONFIG_M54xx) || \
+      defined(CONFIG_M5441x)
 #if !defined(CONFIG_M5441x)
 	if (gpio < 8)
 		return MCFEPORT_EPDR;
@@ -279,7 +282,8 @@ static inline u32 __mcfgpio_pddr(unsigned gpio)
 		return MCFSIM2_GPIO1ENABLE;
 #elif defined(CONFIG_M520x) || defined(CONFIG_M523x) || \
       defined(CONFIG_M527x) || defined(CONFIG_M528x) || \
-      defined(CONFIG_M53xx) || defined(CONFIG_M5441x)
+      defined(CONFIG_M53xx) || defined(CONFIG_M54xx) || \
+      defined(CONFIG_M5441x)
 #if !defined(CONFIG_M5441x)
 	if (gpio < 8)
 		return MCFEPORT_EPDDR;
diff --git a/arch/m68k/include/asm/unistd.h b/arch/m68k/include/asm/unistd.h
index 9d38b73989e..33afa56ad47 100644
--- a/arch/m68k/include/asm/unistd.h
+++ b/arch/m68k/include/asm/unistd.h
@@ -4,7 +4,7 @@
 #include <uapi/asm/unistd.h>
 
 
-#define NR_syscalls		351
+#define NR_syscalls		352
 
 #define __ARCH_WANT_OLD_READDIR
 #define __ARCH_WANT_OLD_STAT
diff --git a/arch/m68k/include/uapi/asm/unistd.h b/arch/m68k/include/uapi/asm/unistd.h
index b932dd47004..9cd82fbc781 100644
--- a/arch/m68k/include/uapi/asm/unistd.h
+++ b/arch/m68k/include/uapi/asm/unistd.h
@@ -356,5 +356,6 @@
 #define __NR_finit_module	348
 #define __NR_sched_setattr	349
 #define __NR_sched_getattr	350
+#define __NR_renameat2		351
 
 #endif /* _UAPI_ASM_M68K_UNISTD_H_ */
diff --git a/arch/m68k/kernel/Makefile b/arch/m68k/kernel/Makefile
index 2d5d9be1627..e47778f8588 100644
--- a/arch/m68k/kernel/Makefile
+++ b/arch/m68k/kernel/Makefile
@@ -25,3 +25,5 @@ obj-$(CONFIG_HAS_DMA)	+= dma.o
 obj-$(CONFIG_KEXEC)		+= machine_kexec.o relocate_kernel.o
 obj-$(CONFIG_BOOTINFO_PROC)	+= bootinfo_proc.o
 
+obj-$(CONFIG_EARLY_PRINTK)	+= early_printk.o
+
diff --git a/arch/m68k/kernel/early_printk.c b/arch/m68k/kernel/early_printk.c
new file mode 100644
index 00000000000..ff9708d7192
--- /dev/null
+++ b/arch/m68k/kernel/early_printk.c
@@ -0,0 +1,67 @@
+/*
+ * 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.
+ *
+ * Copyright (c) 2014 Finn Thain
+ */
+
+#include <linux/kernel.h>
+#include <linux/console.h>
+#include <linux/init.h>
+#include <linux/string.h>
+#include <asm/setup.h>
+
+extern void mvme16x_cons_write(struct console *co,
+			       const char *str, unsigned count);
+
+asmlinkage void __init debug_cons_nputs(const char *s, unsigned n);
+
+static void __ref debug_cons_write(struct console *c,
+				   const char *s, unsigned n)
+{
+#if !(defined(CONFIG_SUN3)   || defined(CONFIG_M68360) || \
+      defined(CONFIG_M68000) || defined(CONFIG_COLDFIRE))
+	if (MACH_IS_MVME16x)
+		mvme16x_cons_write(c, s, n);
+	else
+		debug_cons_nputs(s, n);
+#endif
+}
+
+static struct console early_console_instance = {
+	.name  = "debug",
+	.write = debug_cons_write,
+	.flags = CON_PRINTBUFFER | CON_BOOT,
+	.index = -1
+};
+
+static int __init setup_early_printk(char *buf)
+{
+	if (early_console || buf)
+		return 0;
+
+	early_console = &early_console_instance;
+	register_console(early_console);
+
+	return 0;
+}
+early_param("earlyprintk", setup_early_printk);
+
+/*
+ * debug_cons_nputs() defined in arch/m68k/kernel/head.S cannot be called
+ * after init sections are discarded (for platforms that use it).
+ */
+#if !(defined(CONFIG_SUN3)   || defined(CONFIG_M68360) || \
+      defined(CONFIG_M68000) || defined(CONFIG_COLDFIRE))
+
+static int __init unregister_early_console(void)
+{
+	if (!early_console || MACH_IS_MVME16x)
+		return 0;
+
+	return unregister_console(early_console);
+}
+late_initcall(unregister_early_console);
+
+#endif
diff --git a/arch/m68k/kernel/head.S b/arch/m68k/kernel/head.S
index 3ab329b8852..dbb118e1a4e 100644
--- a/arch/m68k/kernel/head.S
+++ b/arch/m68k/kernel/head.S
@@ -153,7 +153,7 @@
  * ------------
  *	The console is also able to be turned off.  The console in head.S
  * is specifically for debugging and can be very useful.  It is surrounded by
- * #ifdef CONSOLE/#endif clauses so it doesn't have to ship in known-good
+ * #ifdef / #endif clauses so it doesn't have to ship in known-good
  * kernels.  It's basic algorithm is to determine the size of the screen
  * (in height/width and bit depth) and then use that information for
  * displaying an 8x8 font or an 8x16 (widthxheight).  I prefer the 8x8 for
@@ -198,9 +198,8 @@
  * CONFIG_xxx:	These are the obvious machine configuration defines created
  * during configuration.  These are defined in autoconf.h.
  *
- * CONSOLE:	There is support for head.S console in this file.  This
- * console can talk to a Mac frame buffer, but could easily be extrapolated
- * to extend it to support other platforms.
+ * CONSOLE_DEBUG:  Only supports a Mac frame buffer but could easily be
+ * extended to support other platforms.
  *
  * TEST_MMU:	This is a test harness for running on any given machine but
  * getting an MMU dump for another class of machine.  The classes of machines
@@ -222,7 +221,7 @@
  * MMU_PRINT:	There is a routine built into head.S that can display the
  * MMU data structures.  It outputs its result through the serial_putc
  * interface.  So where ever that winds up driving data, that's where the
- * mmu struct will appear.  On the Macintosh that's typically the console.
+ * mmu struct will appear.
  *
  * SERIAL_DEBUG:	There are a series of putc() macro statements
  * scattered through out the code to give progress of status to the
@@ -250,8 +249,8 @@
  * USE_MFP:	Use the ST-MFP port (Modem1) for serial debug.
  *
  * Macintosh constants:
- * MAC_USE_SCC_A: Use SCC port A (modem) for serial debug and early console.
- * MAC_USE_SCC_B: Use SCC port B (printer) for serial debug and early console.
+ * MAC_USE_SCC_A: Use SCC port A (modem) for serial debug.
+ * MAC_USE_SCC_B: Use SCC port B (printer) for serial debug.
  */
 
 #include <linux/linkage.h>
@@ -268,27 +267,17 @@
 #include <asm/pgtable.h>
 #include <asm/page.h>
 #include <asm/asm-offsets.h>
-
 #ifdef CONFIG_MAC
-
-#include <asm/machw.h>
-
-#ifdef CONFIG_FRAMEBUFFER_CONSOLE
-#define CONSOLE
+#  include <asm/machw.h>
 #endif
 
 #ifdef CONFIG_EARLY_PRINTK
-#define SERIAL_DEBUG
-#else
-#undef SERIAL_DEBUG
+#  define SERIAL_DEBUG
+#  if defined(CONFIG_MAC) && defined(CONFIG_FONT_SUPPORT)
+#    define CONSOLE_DEBUG
+#  endif
 #endif
 
-#else /* !CONFIG_MAC */
-
-#define SERIAL_DEBUG
-
-#endif /* !CONFIG_MAC */
-
 #undef MMU_PRINT
 #undef MMU_NOCACHE_KERNEL
 #undef DEBUG
@@ -303,6 +292,7 @@
 
 .globl kernel_pg_dir
 .globl availmem
+.globl m68k_init_mapped_size
 .globl m68k_pgtable_cachemode
 .globl m68k_supervisor_cachemode
 #ifdef CONFIG_MVME16x
@@ -480,22 +470,21 @@ func_define	serial_putc,1
 func_define	console_putc,1
 
 func_define	console_init
-func_define	console_put_stats
 func_define	console_put_penguin
 func_define	console_plot_pixel,3
 func_define	console_scroll
 
 .macro	putc	ch
-#if defined(CONSOLE) || defined(SERIAL_DEBUG)
+#if defined(CONSOLE_DEBUG) || defined(SERIAL_DEBUG)
 	pea	\ch
 #endif
-#ifdef CONSOLE
+#ifdef CONSOLE_DEBUG
 	func_call	console_putc
 #endif
 #ifdef SERIAL_DEBUG
 	func_call	serial_putc
 #endif
-#if defined(CONSOLE) || defined(SERIAL_DEBUG)
+#if defined(CONSOLE_DEBUG) || defined(SERIAL_DEBUG)
 	addql	#4,%sp
 #endif
 .endm
@@ -515,7 +504,7 @@ func_define	putn,1
 .endm
 
 .macro	puts		string
-#if defined(CONSOLE) || defined(SERIAL_DEBUG)
+#if defined(CONSOLE_DEBUG) || defined(SERIAL_DEBUG)
 	__INITDATA
 .Lstr\@:
 	.string	"\string"
@@ -651,11 +640,9 @@ ENTRY(__start)
 	lea	%pc@(L(mac_rowbytes)),%a1
 	movel	%a0@,%a1@
 
-#ifdef SERIAL_DEBUG
 	get_bi_record	BI_MAC_SCCBASE
 	lea	%pc@(L(mac_sccbase)),%a1
 	movel	%a0@,%a1@
-#endif
 
 L(test_notmac):
 #endif /* CONFIG_MAC */
@@ -885,13 +872,12 @@ L(nothp):
  */
 #ifdef CONFIG_MAC
 	is_not_mac(L(nocon))
-#  ifdef CONSOLE
+#  ifdef CONSOLE_DEBUG
 	console_init
 #    ifdef CONFIG_LOGO
 	console_put_penguin
 #    endif /* CONFIG_LOGO */
-	console_put_stats
-#  endif /* CONSOLE */
+#  endif /* CONSOLE_DEBUG */
 L(nocon):
 #endif /* CONFIG_MAC */
 
@@ -922,10 +908,21 @@ L(nocon):
  *
  *	This block of code does what's necessary to map in the various kinds
  *	of machines for execution of Linux.
- *	First map the first 4 MB of kernel code & data
+ *	First map the first 4, 8, or 16 MB of kernel code & data
  */
 
-	mmu_map	#PAGE_OFFSET,%pc@(L(phys_kernel_start)),#4*1024*1024,\
+	get_bi_record BI_MEMCHUNK
+	movel	%a0@(4),%d0
+	movel	#16*1024*1024,%d1
+	cmpl	%d0,%d1
+	jls	1f
+	lsrl	#1,%d1
+	cmpl	%d0,%d1
+	jls	1f
+	lsrl	#1,%d1
+1:
+	movel	%d1,m68k_init_mapped_size
+	mmu_map	#PAGE_OFFSET,%pc@(L(phys_kernel_start)),%d1,\
 		%pc@(m68k_supervisor_cachemode)
 
 	putc	'C'
@@ -1396,15 +1393,13 @@ L(mmu_fixup_done):
 	andl	L(mac_videobase),%d0
 	addl	#VIDEOMEMBASE,%d0
 	movel	%d0,L(mac_videobase)
-#if defined(CONSOLE)
+#ifdef CONSOLE_DEBUG
 	movel	%pc@(L(phys_kernel_start)),%d0
 	subl	#PAGE_OFFSET,%d0
 	subl	%d0,L(console_font)
 	subl	%d0,L(console_font_data)
 #endif
-#ifdef SERIAL_DEBUG
 	orl	#0x50000000,L(mac_sccbase)
-#endif
 1:
 #endif
 
@@ -2734,7 +2729,12 @@ func_return	get_new_page
  */
 
 #ifdef CONFIG_MAC
+/* You may define either or both of these. */
+#define MAC_USE_SCC_A /* Modem port */
+#define MAC_USE_SCC_B /* Printer port */
 
+#if defined(MAC_USE_SCC_A) || defined(MAC_USE_SCC_B)
+/* Initialisation table for SCC with 3.6864 MHz PCLK */
 L(scc_initable_mac):
 	.byte	4,0x44		/* x16, 1 stopbit, no parity */
 	.byte	3,0xc0		/* receiver: 8 bpc */
@@ -2748,6 +2748,7 @@ L(scc_initable_mac):
 	.byte	-1
 	.even
 #endif
+#endif /* CONFIG_MAC */
 
 #ifdef CONFIG_ATARI
 /* #define USE_PRINTER */
@@ -2756,14 +2757,12 @@ L(scc_initable_mac):
 #define USE_MFP
 
 #if defined(USE_SCC_A) || defined(USE_SCC_B)
-#define USE_SCC
-/* Initialisation table for SCC */
-L(scc_initable):
-	.byte	9,12		/* Reset */
+/* Initialisation table for SCC with 7.9872 MHz PCLK */
+/* PCLK == 8.0539 gives baud == 9680.1 */
+L(scc_initable_atari):
 	.byte	4,0x44		/* x16, 1 stopbit, no parity */
 	.byte	3,0xc0		/* receiver: 8 bpc */
 	.byte	5,0xe2		/* transmitter: 8 bpc, assert dtr/rts */
-	.byte	9,0		/* no interrupts */
 	.byte	10,0		/* NRZ */
 	.byte	11,0x50		/* use baud rate generator */
 	.byte	12,24,13,0	/* 9600 baud */
@@ -2812,7 +2811,7 @@ LMFP_UDR     = 0xfffa2f
  */
 
 /*
- * Initialize serial port hardware for 9600/8/1
+ * Initialize serial port hardware
  */
 func_start	serial_init,%d0/%d1/%a0/%a1
 	/*
@@ -2822,7 +2821,7 @@ func_start	serial_init,%d0/%d1/%a0/%a1
 	 *		d0 = boot info offset
 	 *	CONFIG_ATARI
 	 *		a0 = address of SCC
-	 *		a1 = Liobase address/address of scc_initable
+	 *		a1 = Liobase address/address of scc_initable_atari
 	 *		d0 = init data for serial port
 	 *	CONFIG_MAC
 	 *		a0 = address of SCC
@@ -2843,6 +2842,7 @@ func_start	serial_init,%d0/%d1/%a0/%a1
 |	movew	#61,CUSTOMBASE+C_SERPER-ZTWOBASE
 1:
 #endif
+
 #ifdef CONFIG_ATARI
 	is_not_atari(4f)
 	movel	%pc@(L(iobase)),%a1
@@ -2857,9 +2857,21 @@ func_start	serial_init,%d0/%d1/%a0/%a1
 	moveb	%a1@(LPSG_READ),%d0
 	bset	#5,%d0
 	moveb	%d0,%a1@(LPSG_WRITE)
-#elif defined(USE_SCC)
+#elif defined(USE_SCC_A) || defined(USE_SCC_B)
 	lea	%a1@(LSCC_CTRL),%a0
-	lea	%pc@(L(scc_initable)),%a1
+	/* Reset SCC register pointer */
+	moveb	%a0@,%d0
+	/* Reset SCC device: write register pointer then register value */
+	moveb	#9,%a0@
+	moveb	#0xc0,%a0@
+	/* Wait for 5 PCLK cycles, which is about 63 CPU cycles */
+	/* 5 / 7.9872 MHz = approx. 0.63 us = 63 / 100 MHz */
+	movel	#32,%d0
+2:
+	subq	#1,%d0
+	jne	2b
+	/* Initialize channel */
+	lea	%pc@(L(scc_initable_atari)),%a1
 2:	moveb	%a1@+,%d0
 	jmi	3f
 	moveb	%d0,%a0@
@@ -2877,21 +2889,14 @@ func_start	serial_init,%d0/%d1/%a0/%a1
 	jra	L(serial_init_done)
 4:
 #endif
+
 #ifdef CONFIG_MAC
 	is_not_mac(L(serial_init_not_mac))
-
-#ifdef SERIAL_DEBUG
-
-/* You may define either or both of these. */
-#define MAC_USE_SCC_A /* Modem port */
-#define MAC_USE_SCC_B /* Printer port */
-
+#if defined(MAC_USE_SCC_A) || defined(MAC_USE_SCC_B)
 #define mac_scc_cha_b_ctrl_offset	0x0
 #define mac_scc_cha_a_ctrl_offset	0x2
 #define mac_scc_cha_b_data_offset	0x4
 #define mac_scc_cha_a_data_offset	0x6
-
-#if defined(MAC_USE_SCC_A) || defined(MAC_USE_SCC_B)
 	movel	%pc@(L(mac_sccbase)),%a0
 	/* Reset SCC register pointer */
 	moveb	%a0@(mac_scc_cha_a_ctrl_offset),%d0
@@ -2905,7 +2910,6 @@ func_start	serial_init,%d0/%d1/%a0/%a1
 	subq	#1,%d0
 	jne	5b
 #endif
-
 #ifdef MAC_USE_SCC_A
 	/* Initialize channel A */
 	lea	%pc@(L(scc_initable_mac)),%a1
@@ -2916,7 +2920,6 @@ func_start	serial_init,%d0/%d1/%a0/%a1
 	jra	5b
 6:
 #endif	/* MAC_USE_SCC_A */
-
 #ifdef MAC_USE_SCC_B
 	/* Initialize channel B */
 	lea	%pc@(L(scc_initable_mac)),%a1
@@ -2927,9 +2930,6 @@ func_start	serial_init,%d0/%d1/%a0/%a1
 	jra	7b
 8:
 #endif	/* MAC_USE_SCC_B */
-
-#endif	/* SERIAL_DEBUG */
-
 	jra	L(serial_init_done)
 L(serial_init_not_mac):
 #endif	/* CONFIG_MAC */
@@ -2959,6 +2959,15 @@ L(serial_init_not_mac):
 2:
 #endif
 
+#ifdef CONFIG_MVME16x
+	is_not_mvme16x(L(serial_init_not_mvme16x))
+	moveb	#0x10,M167_PCSCCMICR
+	moveb	#0x10,M167_PCSCCTICR
+	moveb	#0x10,M167_PCSCCRICR
+	jra	L(serial_init_done)
+L(serial_init_not_mvme16x):
+#endif
+
 #ifdef CONFIG_APOLLO
 /* We count on the PROM initializing SIO1 */
 #endif
@@ -2998,27 +3007,19 @@ func_start	serial_putc,%d0/%d1/%a0/%a1
 
 #ifdef CONFIG_MAC
 	is_not_mac(5f)
-
-#ifdef SERIAL_DEBUG
-
 #if defined(MAC_USE_SCC_A) || defined(MAC_USE_SCC_B)
 	movel	%pc@(L(mac_sccbase)),%a1
 #endif
-
 #ifdef MAC_USE_SCC_A
 3:	btst	#2,%a1@(mac_scc_cha_a_ctrl_offset)
 	jeq	3b
 	moveb	%d0,%a1@(mac_scc_cha_a_data_offset)
 #endif	/* MAC_USE_SCC_A */
-
 #ifdef MAC_USE_SCC_B
 4:	btst	#2,%a1@(mac_scc_cha_b_ctrl_offset)
 	jeq	4b
 	moveb	%d0,%a1@(mac_scc_cha_b_data_offset)
 #endif	/* MAC_USE_SCC_B */
-
-#endif	/* SERIAL_DEBUG */
-
 	jra	L(serial_putc_done)
 5:
 #endif	/* CONFIG_MAC */
@@ -3039,7 +3040,7 @@ func_start	serial_putc,%d0/%d1/%a0/%a1
 	nop
 	bset	#5,%d0
 	moveb	%d0,%a1@(LPSG_WRITE)
-#elif defined(USE_SCC)
+#elif defined(USE_SCC_A) || defined(USE_SCC_B)
 3:	btst	#2,%a1@(LSCC_CTRL)
 	jeq	3b
 	moveb	%d0,%a1@(LSCC_DATA)
@@ -3195,7 +3196,7 @@ func_start	puts,%d0/%a0
 	movel	ARG1,%a0
 	jra	2f
 1:
-#ifdef CONSOLE
+#ifdef CONSOLE_DEBUG
 	console_putc	%d0
 #endif
 #ifdef SERIAL_DEBUG
@@ -3224,7 +3225,7 @@ func_start	putn,%d0-%d2
 	jls	2f
 	addb	#'A'-('9'+1),%d2
 2:
-#ifdef CONSOLE
+#ifdef CONSOLE_DEBUG
 	console_putc	%d2
 #endif
 #ifdef SERIAL_DEBUG
@@ -3234,21 +3235,19 @@ func_start	putn,%d0-%d2
 
 func_return	putn
 
-#ifdef CONFIG_MAC
+#ifdef CONFIG_EARLY_PRINTK
 /*
- *	mac_early_print
- *
  *	This routine takes its parameters on the stack.  It then
  *	turns around and calls the internal routines.  This routine
  *	is used by the boot console.
  *
  *	The calling parameters are:
- *		void mac_early_print(const char *str, unsigned length);
+ *		void debug_cons_nputs(const char *str, unsigned length)
  *
  *	This routine does NOT understand variable arguments only
  *	simple strings!
  */
-ENTRY(mac_early_print)
+ENTRY(debug_cons_nputs)
 	moveml	%d0/%d1/%a0,%sp@-
 	movew	%sr,%sp@-
 	ori	#0x0700,%sr
@@ -3256,7 +3255,7 @@ ENTRY(mac_early_print)
 	movel	%sp@(22),%d1		/* fetch parameter */
 	jra	2f
 1:
-#ifdef CONSOLE
+#ifdef CONSOLE_DEBUG
 	console_putc	%d0
 #endif
 #ifdef SERIAL_DEBUG
@@ -3270,7 +3269,7 @@ ENTRY(mac_early_print)
 	movew	%sp@+,%sr
 	moveml	%sp@+,%d0/%d1/%a0
 	rts
-#endif /* CONFIG_MAC */
+#endif /* CONFIG_EARLY_PRINTK */
 
 #if defined(CONFIG_HP300) || defined(CONFIG_APOLLO)
 func_start	set_leds,%d0/%a0
@@ -3292,7 +3291,7 @@ func_start	set_leds,%d0/%a0
 func_return	set_leds
 #endif
 
-#ifdef CONSOLE
+#ifdef CONSOLE_DEBUG
 /*
  *	For continuity, see the data alignment
  *	to which this structure is tied.
@@ -3396,43 +3395,6 @@ L(console_clear_loop):
 1:
 func_return	console_init
 
-func_start	console_put_stats,%a0/%d7
-	/*
-	 *	Some of the register usage that follows
-	 *		a0 = pointer to boot_info
-	 *		d7 = value of boot_info fields
-	 */
-	puts	"\nMacLinux\n"
-
-#ifdef SERIAL_DEBUG
-	puts	"\n vidaddr:"
-	putn	%pc@(L(mac_videobase))		/* video addr. */
-
-	puts	"\n  _stext:"
-	lea	%pc@(_stext),%a0
-	putn	%a0
-
-	puts	"\nbootinfo:"
-	lea	%pc@(_end),%a0
-	putn	%a0
-
-	puts	"\n   cpuid:"
-	putn	%pc@(L(cputype))
-
-#  ifdef CONFIG_MAC
-	puts	"\n sccbase:"
-	putn	%pc@(L(mac_sccbase))
-#  endif
-#  ifdef MMU_PRINT
-	putc	'\n'
-	jbsr	mmu_print_machine_cpu_types
-#  endif
-#endif /* SERIAL_DEBUG */
-
-	putc	'\n'
-
-func_return	console_put_stats
-
 #ifdef CONFIG_LOGO
 func_start	console_put_penguin,%a0-%a1/%d0-%d7
 	/*
@@ -3774,12 +3736,15 @@ L(white_16):
 
 L(console_plot_pixel_exit):
 func_return	console_plot_pixel
-#endif /* CONSOLE */
+#endif /* CONSOLE_DEBUG */
 
 
 __INITDATA
 	.align	4
 
+m68k_init_mapped_size:
+	.long	0
+
 #if defined(CONFIG_ATARI) || defined(CONFIG_AMIGA) || \
     defined(CONFIG_HP300) || defined(CONFIG_APOLLO)
 L(custom):
@@ -3787,7 +3752,7 @@ L(iobase):
 	.long 0
 #endif
 
-#if defined(CONSOLE)
+#ifdef CONSOLE_DEBUG
 L(console_globals):
 	.long	0		/* cursor column */
 	.long	0		/* cursor row */
@@ -3798,7 +3763,7 @@ L(console_font):
 	.long	0		/* pointer to console font (struct font_desc) */
 L(console_font_data):
 	.long	0		/* pointer to console font data */
-#endif /* CONSOLE */
+#endif /* CONSOLE_DEBUG */
 
 #if defined(MMU_PRINT)
 L(mmu_print_data):
@@ -3838,7 +3803,9 @@ M167_CYIER = 0xfff45011
 M167_CYLICR = 0xfff45026
 M167_CYTEOIR = 0xfff45085
 M167_CYTDR = 0xfff450f8
+M167_PCSCCMICR = 0xfff4201d
 M167_PCSCCTICR = 0xfff4201e
+M167_PCSCCRICR = 0xfff4201f
 M167_PCTPIACKR = 0xfff42025
 #endif
 
@@ -3856,10 +3823,8 @@ L(mac_dimensions):
 	.long	0
 L(mac_rowbytes):
 	.long	0
-#ifdef SERIAL_DEBUG
 L(mac_sccbase):
 	.long	0
-#endif
 #endif /* CONFIG_MAC */
 
 #if defined (CONFIG_APOLLO)
diff --git a/arch/m68k/kernel/setup_no.c b/arch/m68k/kernel/setup_no.c
index 5b16f5d61b4..88c27d94a72 100644
--- a/arch/m68k/kernel/setup_no.c
+++ b/arch/m68k/kernel/setup_no.c
@@ -58,17 +58,16 @@ void (*mach_halt)(void);
 void (*mach_power_off)(void);
 
 #ifdef CONFIG_M68000
-#define CPU_NAME	"MC68000"
-#endif
-#ifdef CONFIG_M68328
+#if defined(CONFIG_M68328)
 #define CPU_NAME	"MC68328"
-#endif
-#ifdef CONFIG_M68EZ328
+#elif defined(CONFIG_M68EZ328)
 #define CPU_NAME	"MC68EZ328"
-#endif
-#ifdef CONFIG_M68VZ328
+#elif defined(CONFIG_M68VZ328)
 #define CPU_NAME	"MC68VZ328"
+#else
+#define CPU_NAME	"MC68000"
 #endif
+#endif /* CONFIG_M68000 */
 #ifdef CONFIG_M68360
 #define CPU_NAME	"MC68360"
 #endif
diff --git a/arch/m68k/kernel/syscalltable.S b/arch/m68k/kernel/syscalltable.S
index b6223dc41d8..501e1021278 100644
--- a/arch/m68k/kernel/syscalltable.S
+++ b/arch/m68k/kernel/syscalltable.S
@@ -371,4 +371,5 @@ ENTRY(sys_call_table)
 	.long sys_finit_module
 	.long sys_sched_setattr
 	.long sys_sched_getattr		/* 350 */
+	.long sys_renameat2
 
diff --git a/arch/m68k/mac/config.c b/arch/m68k/mac/config.c
index 982c3fe73c4..a471eab1a4d 100644
--- a/arch/m68k/mac/config.c
+++ b/arch/m68k/mac/config.c
@@ -71,31 +71,6 @@ static void mac_get_model(char *str);
 static void mac_identify(void);
 static void mac_report_hardware(void);
 
-#ifdef CONFIG_EARLY_PRINTK
-asmlinkage void __init mac_early_print(const char *s, unsigned n);
-
-static void __init mac_early_cons_write(struct console *con,
-                                 const char *s, unsigned n)
-{
-	mac_early_print(s, n);
-}
-
-static struct console __initdata mac_early_cons = {
-	.name  = "early",
-	.write = mac_early_cons_write,
-	.flags = CON_PRINTBUFFER | CON_BOOT,
-	.index = -1
-};
-
-int __init mac_unregister_early_cons(void)
-{
-	/* mac_early_print can't be used after init sections are discarded */
-	return unregister_console(&mac_early_cons);
-}
-
-late_initcall(mac_unregister_early_cons);
-#endif
-
 static void __init mac_sched_init(irq_handler_t vector)
 {
 	via_init_clock(vector);
@@ -190,10 +165,6 @@ void __init config_mac(void)
 	mach_beep = mac_mksound;
 #endif
 
-#ifdef CONFIG_EARLY_PRINTK
-	register_console(&mac_early_cons);
-#endif
-
 	/*
 	 * Determine hardware present
 	 */
diff --git a/arch/m68k/mm/motorola.c b/arch/m68k/mm/motorola.c
index 7d402443216..b958916e5ea 100644
--- a/arch/m68k/mm/motorola.c
+++ b/arch/m68k/mm/motorola.c
@@ -45,7 +45,7 @@ EXPORT_SYMBOL(mm_cachebits);
 #endif
 
 /* size of memory already mapped in head.S */
-#define INIT_MAPPED_SIZE	(4UL<<20)
+extern __initdata unsigned long m68k_init_mapped_size;
 
 extern unsigned long availmem;
 
@@ -271,10 +271,12 @@ void __init paging_init(void)
 	 */
 	addr = m68k_memory[0].addr;
 	size = m68k_memory[0].size;
-	free_bootmem_node(NODE_DATA(0), availmem, min(INIT_MAPPED_SIZE, size) - (availmem - addr));
+	free_bootmem_node(NODE_DATA(0), availmem,
+			  min(m68k_init_mapped_size, size) - (availmem - addr));
 	map_node(0);
-	if (size > INIT_MAPPED_SIZE)
-		free_bootmem_node(NODE_DATA(0), addr + INIT_MAPPED_SIZE, size - INIT_MAPPED_SIZE);
+	if (size > m68k_init_mapped_size)
+		free_bootmem_node(NODE_DATA(0), addr + m68k_init_mapped_size,
+				  size - m68k_init_mapped_size);
 
 	for (i = 1; i < m68k_num_memory; i++)
 		map_node(i);
diff --git a/arch/m68k/mvme16x/config.c b/arch/m68k/mvme16x/config.c
index eab7d342757..a53803cc66c 100644
--- a/arch/m68k/mvme16x/config.c
+++ b/arch/m68k/mvme16x/config.c
@@ -213,7 +213,7 @@ static void __init mvme16x_init_IRQ (void)
 #define CySCRH		(0x22)
 #define CyTFTC		(0x80)
 
-static void cons_write(struct console *co, const char *str, unsigned count)
+void mvme16x_cons_write(struct console *co, const char *str, unsigned count)
 {
 	volatile unsigned char *base_addr = (u_char *)CD2401_ADDR;
 	volatile u_char sink;
@@ -268,20 +268,6 @@ static void cons_write(struct console *co, const char *str, unsigned count)
 	base_addr[CyIER] = ier;
 }
 
-static struct console cons_info =
-{
-	.name	= "sercon",
-	.write	= cons_write,
-	.flags	= CON_PRINTBUFFER | CON_BOOT,
-	.index	= -1,
-};
-
-static void __init mvme16x_early_console(void)
-{
-	register_console(&cons_info);
-
-	printk(KERN_INFO "MVME16x: early console registered\n");
-}
 #endif
 
 void __init config_mvme16x(void)
@@ -336,16 +322,6 @@ void __init config_mvme16x(void)
     else
     {
 	mvme16x_config = MVME16x_CONFIG_GOT_LP | MVME16x_CONFIG_GOT_CD2401;
-
-	/* Dont allow any interrupts from the CD2401 until the interrupt */
-	/* handlers are installed					 */
-
-	pcc2chip[PccSCCMICR] = 0x10;
-	pcc2chip[PccSCCTICR] = 0x10;
-	pcc2chip[PccSCCRICR] = 0x10;
-#ifdef CONFIG_EARLY_PRINTK
-	mvme16x_early_console();
-#endif
     }
 }
 
diff --git a/arch/m68k/platform/68000/m68EZ328.c b/arch/m68k/platform/68000/m68EZ328.c
index 332b5e8605f..21952906e9e 100644
--- a/arch/m68k/platform/68000/m68EZ328.c
+++ b/arch/m68k/platform/68000/m68EZ328.c
@@ -69,7 +69,8 @@ void __init config_BSP(char *command, int len)
   if (p) strcpy(p,command);
   else command[0] = 0;
 #endif
- 
+
+  mach_sched_init = hw_timer_init;
   mach_hwclk = m68328_hwclk;
   mach_reset = m68ez328_reset;
 }
diff --git a/arch/m68k/platform/68000/m68VZ328.c b/arch/m68k/platform/68000/m68VZ328.c
index fd6658358af..0e5e5a10a02 100644
--- a/arch/m68k/platform/68000/m68VZ328.c
+++ b/arch/m68k/platform/68000/m68VZ328.c
@@ -182,6 +182,7 @@ void __init config_BSP(char *command, int size)
 
 	init_hardware(command, size);
 
+	mach_sched_init = hw_timer_init;
 	mach_hwclk = m68328_hwclk;
 	mach_reset = m68vz328_reset;
 }
diff --git a/arch/m68k/platform/coldfire/gpio.c b/arch/m68k/platform/coldfire/gpio.c
index 9cd2b5c7051..e7e428681ec 100644
--- a/arch/m68k/platform/coldfire/gpio.c
+++ b/arch/m68k/platform/coldfire/gpio.c
@@ -76,10 +76,7 @@ int __mcfgpio_direction_output(unsigned gpio, int value)
 
 	local_irq_save(flags);
 	data = mcfgpio_read(__mcfgpio_pddr(gpio));
-	if (value)
-		data |= mcfgpio_bit(gpio);
-	else
-		data &= mcfgpio_bit(gpio);
+	data |= mcfgpio_bit(gpio);
 	mcfgpio_write(data, __mcfgpio_pddr(gpio));
 
 	/* now set the data to output */
@@ -117,37 +114,51 @@ EXPORT_SYMBOL(__mcfgpio_free);
 
 #ifdef CONFIG_GPIOLIB
 
-int mcfgpio_direction_input(struct gpio_chip *chip, unsigned offset)
+static int mcfgpio_direction_input(struct gpio_chip *chip, unsigned offset)
 {
 	return __mcfgpio_direction_input(offset);
 }
 
-int mcfgpio_get_value(struct gpio_chip *chip, unsigned offset)
+static int mcfgpio_get_value(struct gpio_chip *chip, unsigned offset)
 {
 	return __mcfgpio_get_value(offset);
 }
 
-int mcfgpio_direction_output(struct gpio_chip *chip, unsigned offset, int value)
+static int mcfgpio_direction_output(struct gpio_chip *chip, unsigned offset,
+				    int value)
 {
 	return __mcfgpio_direction_output(offset, value);
 }
 
-void mcfgpio_set_value(struct gpio_chip *chip, unsigned offset, int value)
+static void mcfgpio_set_value(struct gpio_chip *chip, unsigned offset,
+			      int value)
 {
 	__mcfgpio_set_value(offset, value);
 }
 
-int mcfgpio_request(struct gpio_chip *chip, unsigned offset)
+static int mcfgpio_request(struct gpio_chip *chip, unsigned offset)
 {
 	return __mcfgpio_request(offset);
 }
 
-void mcfgpio_free(struct gpio_chip *chip, unsigned offset)
+static void mcfgpio_free(struct gpio_chip *chip, unsigned offset)
 {
 	__mcfgpio_free(offset);
 }
 
-struct bus_type mcfgpio_subsys = {
+static int mcfgpio_to_irq(struct gpio_chip *chip, unsigned offset)
+{
+#if defined(MCFGPIO_IRQ_MIN)
+	if ((offset >= MCFGPIO_IRQ_MIN) && (offset < MCFGPIO_IRQ_MAX))
+#else
+	if (offset < MCFGPIO_IRQ_MAX)
+#endif
+		return MCFGPIO_IRQ_VECBASE + offset;
+	else
+		return -EINVAL;
+}
+
+static struct bus_type mcfgpio_subsys = {
 	.name		= "gpio",
 	.dev_name	= "gpio",
 };
@@ -160,6 +171,7 @@ static struct gpio_chip mcfgpio_chip = {
 	.direction_output	= mcfgpio_direction_output,
 	.get			= mcfgpio_get_value,
 	.set			= mcfgpio_set_value,
+	.to_irq			= mcfgpio_to_irq,
 	.base			= 0,
 	.ngpio			= MCFGPIO_PIN_MAX,
 };
diff --git a/arch/m68k/platform/coldfire/m520x.c b/arch/m68k/platform/coldfire/m520x.c
index ea1be0e98ad..4040a3c9373 100644
--- a/arch/m68k/platform/coldfire/m520x.c
+++ b/arch/m68k/platform/coldfire/m520x.c
@@ -118,10 +118,9 @@ static void __init m520x_clk_init(void)
 
 /***************************************************************************/
 
-#if IS_ENABLED(CONFIG_SPI_COLDFIRE_QSPI)
-
 static void __init m520x_qspi_init(void)
 {
+#if IS_ENABLED(CONFIG_SPI_COLDFIRE_QSPI)
 	u16 par;
 	/* setup Port QS for QSPI with gpio CS control */
 	writeb(0x3f, MCF_GPIO_PAR_QSPI);
@@ -129,9 +128,8 @@ static void __init m520x_qspi_init(void)
 	par = readw(MCF_GPIO_PAR_UART);
 	par &= 0x00ff;
 	writew(par, MCF_GPIO_PAR_UART);
-}
-
 #endif /* IS_ENABLED(CONFIG_SPI_COLDFIRE_QSPI) */
+}
 
 /***************************************************************************/
 
@@ -176,9 +174,7 @@ void __init config_BSP(char *commandp, int size)
 	m520x_clk_init();
 	m520x_uarts_init();
 	m520x_fec_init();
-#if IS_ENABLED(CONFIG_SPI_COLDFIRE_QSPI)
 	m520x_qspi_init();
-#endif
 }
 
 /***************************************************************************/
diff --git a/arch/m68k/platform/coldfire/m523x.c b/arch/m68k/platform/coldfire/m523x.c
index 2b10e9f198c..6b7135e6d5b 100644
--- a/arch/m68k/platform/coldfire/m523x.c
+++ b/arch/m68k/platform/coldfire/m523x.c
@@ -32,6 +32,7 @@ DEFINE_CLK(mcfpit3, "mcfpit.3", MCF_CLK);
 DEFINE_CLK(mcfuart0, "mcfuart.0", MCF_BUSCLK);
 DEFINE_CLK(mcfuart1, "mcfuart.1", MCF_BUSCLK);
 DEFINE_CLK(mcfuart2, "mcfuart.2", MCF_BUSCLK);
+DEFINE_CLK(mcfqspi0, "mcfqspi.0", MCF_BUSCLK);
 DEFINE_CLK(fec0, "fec.0", MCF_BUSCLK);
 
 struct clk *mcf_clks[] = {
@@ -44,16 +45,16 @@ struct clk *mcf_clks[] = {
 	&clk_mcfuart0,
 	&clk_mcfuart1,
 	&clk_mcfuart2,
+	&clk_mcfqspi0,
 	&clk_fec0,
 	NULL
 };
 
 /***************************************************************************/
 
-#if IS_ENABLED(CONFIG_SPI_COLDFIRE_QSPI)
-
 static void __init m523x_qspi_init(void)
 {
+#if IS_ENABLED(CONFIG_SPI_COLDFIRE_QSPI)
 	u16 par;
 
 	/* setup QSPS pins for QSPI with gpio CS control */
@@ -62,9 +63,8 @@ static void __init m523x_qspi_init(void)
 	par = readw(MCFGPIO_PAR_TIMER);
 	par &= 0x3f3f;
 	writew(par, MCFGPIO_PAR_TIMER);
-}
-
 #endif /* IS_ENABLED(CONFIG_SPI_COLDFIRE_QSPI) */
+}
 
 /***************************************************************************/
 
@@ -80,9 +80,7 @@ void __init config_BSP(char *commandp, int size)
 {
 	mach_sched_init = hw_timer_init;
 	m523x_fec_init();
-#if IS_ENABLED(CONFIG_SPI_COLDFIRE_QSPI)
 	m523x_qspi_init();
-#endif
 }
 
 /***************************************************************************/
diff --git a/arch/m68k/platform/coldfire/m5249.c b/arch/m68k/platform/coldfire/m5249.c
index c80b5e51d29..f6253a3313b 100644
--- a/arch/m68k/platform/coldfire/m5249.c
+++ b/arch/m68k/platform/coldfire/m5249.c
@@ -26,6 +26,7 @@ DEFINE_CLK(mcftmr0, "mcftmr.0", MCF_BUSCLK);
 DEFINE_CLK(mcftmr1, "mcftmr.1", MCF_BUSCLK);
 DEFINE_CLK(mcfuart0, "mcfuart.0", MCF_BUSCLK);
 DEFINE_CLK(mcfuart1, "mcfuart.1", MCF_BUSCLK);
+DEFINE_CLK(mcfqspi0, "mcfqspi.0", MCF_BUSCLK);
 
 struct clk *mcf_clks[] = {
 	&clk_pll,
@@ -34,6 +35,7 @@ struct clk *mcf_clks[] = {
 	&clk_mcftmr1,
 	&clk_mcfuart0,
 	&clk_mcfuart1,
+	&clk_mcfqspi0,
 	NULL
 };
 
@@ -71,17 +73,15 @@ static struct platform_device *m5249_devices[] __initdata = {
 
 /***************************************************************************/
 
-#if IS_ENABLED(CONFIG_SPI_COLDFIRE_QSPI)
-
 static void __init m5249_qspi_init(void)
 {
+#if IS_ENABLED(CONFIG_SPI_COLDFIRE_QSPI)
 	/* QSPI irq setup */
 	writeb(MCFSIM_ICR_AUTOVEC | MCFSIM_ICR_LEVEL4 | MCFSIM_ICR_PRI0,
 	       MCFSIM_QSPIICR);
 	mcf_mapirq2imr(MCF_IRQ_QSPI, MCFINTC_QSPI);
-}
-
 #endif /* IS_ENABLED(CONFIG_SPI_COLDFIRE_QSPI) */
+}
 
 /***************************************************************************/
 
@@ -110,9 +110,7 @@ void __init config_BSP(char *commandp, int size)
 #ifdef CONFIG_M5249C3
 	m5249_smc91x_init();
 #endif
-#if IS_ENABLED(CONFIG_SPI_COLDFIRE_QSPI)
 	m5249_qspi_init();
-#endif
 }
 
 /***************************************************************************/
diff --git a/arch/m68k/platform/coldfire/m525x.c b/arch/m68k/platform/coldfire/m525x.c
index 5b9f657b2df..1adba390903 100644
--- a/arch/m68k/platform/coldfire/m525x.c
+++ b/arch/m68k/platform/coldfire/m525x.c
@@ -26,6 +26,7 @@ DEFINE_CLK(mcftmr0, "mcftmr.0", MCF_BUSCLK);
 DEFINE_CLK(mcftmr1, "mcftmr.1", MCF_BUSCLK);
 DEFINE_CLK(mcfuart0, "mcfuart.0", MCF_BUSCLK);
 DEFINE_CLK(mcfuart1, "mcfuart.1", MCF_BUSCLK);
+DEFINE_CLK(mcfqspi0, "mcfqspi.0", MCF_BUSCLK);
 
 struct clk *mcf_clks[] = {
 	&clk_pll,
@@ -34,6 +35,7 @@ struct clk *mcf_clks[] = {
 	&clk_mcftmr1,
 	&clk_mcfuart0,
 	&clk_mcfuart1,
+	&clk_mcfqspi0,
 	NULL
 };
 
diff --git a/arch/m68k/platform/coldfire/m5272.c b/arch/m68k/platform/coldfire/m5272.c
index a8c5856fe5e..8a4d3cc322c 100644
--- a/arch/m68k/platform/coldfire/m5272.c
+++ b/arch/m68k/platform/coldfire/m5272.c
@@ -39,6 +39,7 @@ DEFINE_CLK(mcftmr2, "mcftmr.2", MCF_BUSCLK);
 DEFINE_CLK(mcftmr3, "mcftmr.3", MCF_BUSCLK);
 DEFINE_CLK(mcfuart0, "mcfuart.0", MCF_BUSCLK);
 DEFINE_CLK(mcfuart1, "mcfuart.1", MCF_BUSCLK);
+DEFINE_CLK(mcfqspi0, "mcfqspi.0", MCF_BUSCLK);
 DEFINE_CLK(fec0, "fec.0", MCF_BUSCLK);
 
 struct clk *mcf_clks[] = {
@@ -50,6 +51,7 @@ struct clk *mcf_clks[] = {
 	&clk_mcftmr3,
 	&clk_mcfuart0,
 	&clk_mcfuart1,
+	&clk_mcfqspi0,
 	&clk_fec0,
 	NULL
 };
diff --git a/arch/m68k/platform/coldfire/m527x.c b/arch/m68k/platform/coldfire/m527x.c
index 6fbfe9096c3..62d81ef016f 100644
--- a/arch/m68k/platform/coldfire/m527x.c
+++ b/arch/m68k/platform/coldfire/m527x.c
@@ -33,6 +33,7 @@ DEFINE_CLK(mcfpit3, "mcfpit.3", MCF_CLK);
 DEFINE_CLK(mcfuart0, "mcfuart.0", MCF_BUSCLK);
 DEFINE_CLK(mcfuart1, "mcfuart.1", MCF_BUSCLK);
 DEFINE_CLK(mcfuart2, "mcfuart.2", MCF_BUSCLK);
+DEFINE_CLK(mcfqspi0, "mcfqspi.0", MCF_BUSCLK);
 DEFINE_CLK(fec0, "fec.0", MCF_BUSCLK);
 DEFINE_CLK(fec1, "fec.1", MCF_BUSCLK);
 
@@ -46,6 +47,7 @@ struct clk *mcf_clks[] = {
 	&clk_mcfuart0,
 	&clk_mcfuart1,
 	&clk_mcfuart2,
+	&clk_mcfqspi0,
 	&clk_fec0,
 	&clk_fec1,
 	NULL
@@ -53,10 +55,9 @@ struct clk *mcf_clks[] = {
 
 /***************************************************************************/
 
-#if IS_ENABLED(CONFIG_SPI_COLDFIRE_QSPI)
-
 static void __init m527x_qspi_init(void)
 {
+#if IS_ENABLED(CONFIG_SPI_COLDFIRE_QSPI)
 #if defined(CONFIG_M5271)
 	u16 par;
 
@@ -70,9 +71,8 @@ static void __init m527x_qspi_init(void)
 	/* setup QSPS pins for QSPI with gpio CS control */
 	writew(0x003e, MCFGPIO_PAR_QSPI);
 #endif
-}
-
 #endif /* IS_ENABLED(CONFIG_SPI_COLDFIRE_QSPI) */
+}
 
 /***************************************************************************/
 
@@ -120,9 +120,7 @@ void __init config_BSP(char *commandp, int size)
 	mach_sched_init = hw_timer_init;
 	m527x_uarts_init();
 	m527x_fec_init();
-#if IS_ENABLED(CONFIG_SPI_COLDFIRE_QSPI)
 	m527x_qspi_init();
-#endif
 }
 
 /***************************************************************************/
diff --git a/arch/m68k/platform/coldfire/m528x.c b/arch/m68k/platform/coldfire/m528x.c
index b03a9d27183..21cd161d36f 100644
--- a/arch/m68k/platform/coldfire/m528x.c
+++ b/arch/m68k/platform/coldfire/m528x.c
@@ -34,6 +34,7 @@ DEFINE_CLK(mcfpit3, "mcfpit.3", MCF_CLK);
 DEFINE_CLK(mcfuart0, "mcfuart.0", MCF_BUSCLK);
 DEFINE_CLK(mcfuart1, "mcfuart.1", MCF_BUSCLK);
 DEFINE_CLK(mcfuart2, "mcfuart.2", MCF_BUSCLK);
+DEFINE_CLK(mcfqspi0, "mcfqspi.0", MCF_BUSCLK);
 DEFINE_CLK(fec0, "fec.0", MCF_BUSCLK);
 
 struct clk *mcf_clks[] = {
@@ -46,21 +47,20 @@ struct clk *mcf_clks[] = {
 	&clk_mcfuart0,
 	&clk_mcfuart1,
 	&clk_mcfuart2,
+	&clk_mcfqspi0,
 	&clk_fec0,
 	NULL
 };
 
 /***************************************************************************/
 
-#if IS_ENABLED(CONFIG_SPI_COLDFIRE_QSPI)
-
 static void __init m528x_qspi_init(void)
 {
+#if IS_ENABLED(CONFIG_SPI_COLDFIRE_QSPI)
 	/* setup Port QS for QSPI with gpio CS control */
 	__raw_writeb(0x07, MCFGPIO_PQSPAR);
-}
-
 #endif /* IS_ENABLED(CONFIG_SPI_COLDFIRE_QSPI) */
+}
 
 /***************************************************************************/
 
@@ -126,9 +126,7 @@ void __init config_BSP(char *commandp, int size)
 	mach_sched_init = hw_timer_init;
 	m528x_uarts_init();
 	m528x_fec_init();
-#if IS_ENABLED(CONFIG_SPI_COLDFIRE_QSPI)
 	m528x_qspi_init();
-#endif
 }
 
 /***************************************************************************/
diff --git a/arch/m68k/platform/coldfire/m53xx.c b/arch/m68k/platform/coldfire/m53xx.c
index 5286f98fbed..80879a7fe3d 100644
--- a/arch/m68k/platform/coldfire/m53xx.c
+++ b/arch/m68k/platform/coldfire/m53xx.c
@@ -166,15 +166,13 @@ static void __init m53xx_clk_init(void)
 
 /***************************************************************************/
 
-#if IS_ENABLED(CONFIG_SPI_COLDFIRE_QSPI)
-
 static void __init m53xx_qspi_init(void)
 {
+#if IS_ENABLED(CONFIG_SPI_COLDFIRE_QSPI)
 	/* setup QSPS pins for QSPI with gpio CS control */
 	writew(0x01f0, MCFGPIO_PAR_QSPI);
-}
-
 #endif /* IS_ENABLED(CONFIG_SPI_COLDFIRE_QSPI) */
+}
 
 /***************************************************************************/
 
@@ -219,9 +217,7 @@ void __init config_BSP(char *commandp, int size)
 	m53xx_clk_init();
 	m53xx_uarts_init();
 	m53xx_fec_init();
-#if IS_ENABLED(CONFIG_SPI_COLDFIRE_QSPI)
 	m53xx_qspi_init();
-#endif
 
 #ifdef CONFIG_BDM_DISABLE
 	/*
diff --git a/arch/metag/include/asm/barrier.h b/arch/metag/include/asm/barrier.h
index d1768c6f5b1..c7591e80067 100644
--- a/arch/metag/include/asm/barrier.h
+++ b/arch/metag/include/asm/barrier.h
@@ -15,6 +15,7 @@ static inline void wr_fence(void)
 	volatile int *flushptr = (volatile int *) LINSYSEVENT_WR_FENCE;
 	barrier();
 	*flushptr = 0;
+	barrier();
 }
 
 #else /* CONFIG_METAG_META21 */
@@ -35,6 +36,7 @@ static inline void wr_fence(void)
 	*flushptr = 0;
 	*flushptr = 0;
 	*flushptr = 0;
+	barrier();
 }
 
 #endif /* !CONFIG_METAG_META21 */
@@ -68,6 +70,7 @@ static inline void fence(void)
 	volatile int *flushptr = (volatile int *) LINSYSEVENT_WR_ATOMIC_UNLOCK;
 	barrier();
 	*flushptr = 0;
+	barrier();
 }
 #define smp_mb()        fence()
 #define smp_rmb()       fence()
diff --git a/arch/metag/include/asm/processor.h b/arch/metag/include/asm/processor.h
index f16477d1f57..a8a37477c66 100644
--- a/arch/metag/include/asm/processor.h
+++ b/arch/metag/include/asm/processor.h
@@ -22,6 +22,8 @@
 /* Add an extra page of padding at the top of the stack for the guard page. */
 #define STACK_TOP	(TASK_SIZE - PAGE_SIZE)
 #define STACK_TOP_MAX	STACK_TOP
+/* Maximum virtual space for stack */
+#define STACK_SIZE_MAX	(CONFIG_MAX_STACK_SIZE_MB*1024*1024)
 
 /* This decides where the kernel will search for a free chunk of vm
  * space during mmap's.
diff --git a/arch/metag/include/uapi/asm/Kbuild b/arch/metag/include/uapi/asm/Kbuild
index 84e09feb4d5..ab78be2b6eb 100644
--- a/arch/metag/include/uapi/asm/Kbuild
+++ b/arch/metag/include/uapi/asm/Kbuild
@@ -4,11 +4,11 @@ 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
 header-y += siginfo.h
 header-y += swab.h
 header-y += unistd.h
 
 generic-y += mman.h
+generic-y += resource.h
 generic-y += setup.h
diff --git a/arch/metag/include/uapi/asm/resource.h b/arch/metag/include/uapi/asm/resource.h
deleted file mode 100644
index 526d23cc305..00000000000
--- a/arch/metag/include/uapi/asm/resource.h
+++ /dev/null
@@ -1,7 +0,0 @@
-#ifndef _UAPI_METAG_RESOURCE_H
-#define _UAPI_METAG_RESOURCE_H
-
-#define _STK_LIM_MAX    (1 << 28)
-#include <asm-generic/resource.h>
-
-#endif /* _UAPI_METAG_RESOURCE_H */
diff --git a/arch/microblaze/include/asm/pci.h b/arch/microblaze/include/asm/pci.h
index 935f9bec414..335524040ff 100644
--- a/arch/microblaze/include/asm/pci.h
+++ b/arch/microblaze/include/asm/pci.h
@@ -44,11 +44,6 @@ struct pci_dev;
  */
 #define pcibios_assign_all_busses()	0
 
-static inline void pcibios_penalize_isa_irq(int irq, int active)
-{
-	/* We don't do dynamic PCI IRQ allocation */
-}
-
 #ifdef CONFIG_PCI
 extern void set_pci_dma_ops(struct dma_map_ops *dma_ops);
 extern struct dma_map_ops *get_pci_dma_ops(void);
diff --git a/arch/microblaze/pci/pci-common.c b/arch/microblaze/pci/pci-common.c
index 70996cc66aa..a59de1bc1ce 100644
--- a/arch/microblaze/pci/pci-common.c
+++ b/arch/microblaze/pci/pci-common.c
@@ -168,26 +168,6 @@ struct pci_controller *pci_find_hose_for_OF_device(struct device_node *node)
 	return NULL;
 }
 
-static ssize_t pci_show_devspec(struct device *dev,
-		struct device_attribute *attr, char *buf)
-{
-	struct pci_dev *pdev;
-	struct device_node *np;
-
-	pdev = to_pci_dev(dev);
-	np = pci_device_to_OF_node(pdev);
-	if (np == NULL || np->full_name == NULL)
-		return 0;
-	return sprintf(buf, "%s", np->full_name);
-}
-static DEVICE_ATTR(devspec, S_IRUGO, pci_show_devspec, NULL);
-
-/* Add sysfs properties */
-int pcibios_add_platform_entries(struct pci_dev *pdev)
-{
-	return device_create_file(&pdev->dev, &dev_attr_devspec);
-}
-
 void pcibios_set_master(struct pci_dev *dev)
 {
 	/* No special bus mastering setup handling */
diff --git a/arch/mips/Makefile b/arch/mips/Makefile
index 1a5b4032cb6..60a359cfa32 100644
--- a/arch/mips/Makefile
+++ b/arch/mips/Makefile
@@ -151,7 +151,7 @@ cflags-$(CONFIG_CPU_NEVADA)	+= $(call cc-option,-march=rm5200,-march=r5000) \
 			-Wa,--trap
 cflags-$(CONFIG_CPU_RM7000)	+= $(call cc-option,-march=rm7000,-march=r5000) \
 			-Wa,--trap
-cflags-$(CONFIG_CPU_SB1)	+= $(call cc-option,-march=sb1,-march=r5000) \
+cflags-$(CONFIG_CPU_SB1)	+= $(call cc-option,-march=sb1 -mno-mdmx -mno-mips3d,-march=r5000) \
 			-Wa,--trap
 cflags-$(CONFIG_CPU_R8000)	+= -march=r8000 -Wa,--trap
 cflags-$(CONFIG_CPU_R10000)	+= $(call cc-option,-march=r10000,-march=r8000) \
diff --git a/arch/mips/cavium-octeon/octeon-irq.c b/arch/mips/cavium-octeon/octeon-irq.c
index c2bb4f896ce..3aa5b46b2d4 100644
--- a/arch/mips/cavium-octeon/octeon-irq.c
+++ b/arch/mips/cavium-octeon/octeon-irq.c
@@ -635,7 +635,7 @@ static void octeon_irq_cpu_offline_ciu(struct irq_data *data)
 		cpumask_clear(&new_affinity);
 		cpumask_set_cpu(cpumask_first(cpu_online_mask), &new_affinity);
 	}
-	__irq_set_affinity_locked(data, &new_affinity);
+	irq_set_affinity_locked(data, &new_affinity, false);
 }
 
 static int octeon_irq_ciu_set_affinity(struct irq_data *data,
diff --git a/arch/mips/configs/fuloong2e_defconfig b/arch/mips/configs/fuloong2e_defconfig
index e5b73de08fc..002680648dc 100644
--- a/arch/mips/configs/fuloong2e_defconfig
+++ b/arch/mips/configs/fuloong2e_defconfig
@@ -188,7 +188,6 @@ CONFIG_USB_KBD=y
 CONFIG_USB_MOUSE=y
 CONFIG_USB=y
 CONFIG_USB_ANNOUNCE_NEW_DEVICES=y
-CONFIG_USB_DEVICEFS=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_OTG_WHITELIST=y
 CONFIG_USB_WUSB_CBAF=m
diff --git a/arch/mips/configs/lemote2f_defconfig b/arch/mips/configs/lemote2f_defconfig
index 343bebc4b63..227a9de3224 100644
--- a/arch/mips/configs/lemote2f_defconfig
+++ b/arch/mips/configs/lemote2f_defconfig
@@ -297,7 +297,6 @@ CONFIG_HID_WACOM=m
 CONFIG_HID_ZEROPLUS=m
 CONFIG_ZEROPLUS_FF=y
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_DYNAMIC_MINORS=y
 CONFIG_USB_OTG_WHITELIST=y
diff --git a/arch/mips/configs/mpc30x_defconfig b/arch/mips/configs/mpc30x_defconfig
index c16de981292..7a346605c49 100644
--- a/arch/mips/configs/mpc30x_defconfig
+++ b/arch/mips/configs/mpc30x_defconfig
@@ -47,7 +47,6 @@ CONFIG_GPIO_VR41XX=y
 # CONFIG_VGA_CONSOLE is not set
 # CONFIG_HID_SUPPORT is not set
 CONFIG_USB=m
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_OHCI_HCD=m
 CONFIG_RTC_CLASS=y
 CONFIG_RTC_DRV_VR41XX=y
diff --git a/arch/mips/configs/msp71xx_defconfig b/arch/mips/configs/msp71xx_defconfig
index d1142e9cd9a..201edfb2637 100644
--- a/arch/mips/configs/msp71xx_defconfig
+++ b/arch/mips/configs/msp71xx_defconfig
@@ -67,7 +67,6 @@ CONFIG_I2C_CHARDEV=y
 CONFIG_I2C_PMCMSP=y
 # CONFIG_USB_HID is not set
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=y
 CONFIG_USB_EHCI_HCD=y
 CONFIG_USB_EHCI_ROOT_HUB_TT=y
diff --git a/arch/mips/configs/mtx1_defconfig b/arch/mips/configs/mtx1_defconfig
index 593946afc48..d269a5326a3 100644
--- a/arch/mips/configs/mtx1_defconfig
+++ b/arch/mips/configs/mtx1_defconfig
@@ -575,7 +575,6 @@ CONFIG_USB_HIDDEV=y
 CONFIG_USB_KBD=m
 CONFIG_USB_MOUSE=m
 CONFIG_USB=m
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=m
 CONFIG_USB_EHCI_HCD=m
 CONFIG_USB_EHCI_ROOT_HUB_TT=y
diff --git a/arch/mips/configs/rm200_defconfig b/arch/mips/configs/rm200_defconfig
index 59d9d2fdcd4..73e7bf49461 100644
--- a/arch/mips/configs/rm200_defconfig
+++ b/arch/mips/configs/rm200_defconfig
@@ -301,7 +301,6 @@ CONFIG_USB_HIDDEV=y
 CONFIG_USB_KBD=m
 CONFIG_USB_MOUSE=m
 CONFIG_USB=m
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=m
 CONFIG_USB_EHCI_HCD=m
 # CONFIG_USB_EHCI_TT_NEWSCHED is not set
diff --git a/arch/mips/configs/sb1250_swarm_defconfig b/arch/mips/configs/sb1250_swarm_defconfig
index 5b0463ef938..51bab13ef6f 100644
--- a/arch/mips/configs/sb1250_swarm_defconfig
+++ b/arch/mips/configs/sb1250_swarm_defconfig
@@ -72,7 +72,6 @@ CONFIG_SERIO_RAW=m
 # CONFIG_HW_RANDOM is not set
 # CONFIG_HWMON is not set
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=y
 CONFIG_USB_OHCI_HCD=y
 CONFIG_EXT2_FS=y
diff --git a/arch/mips/configs/tb0219_defconfig b/arch/mips/configs/tb0219_defconfig
index 30036b4cbeb..11f51505d56 100644
--- a/arch/mips/configs/tb0219_defconfig
+++ b/arch/mips/configs/tb0219_defconfig
@@ -72,7 +72,6 @@ CONFIG_GPIO_TB0219=y
 # CONFIG_VGA_CONSOLE is not set
 # CONFIG_HID_SUPPORT is not set
 CONFIG_USB=m
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=m
 CONFIG_USB_EHCI_HCD=m
 # CONFIG_USB_EHCI_TT_NEWSCHED is not set
diff --git a/arch/mips/configs/tb0226_defconfig b/arch/mips/configs/tb0226_defconfig
index 81bfa1d4d8e..d99b1905a1b 100644
--- a/arch/mips/configs/tb0226_defconfig
+++ b/arch/mips/configs/tb0226_defconfig
@@ -69,7 +69,6 @@ CONFIG_SERIAL_VR41XX_CONSOLE=y
 # CONFIG_VGA_CONSOLE is not set
 # CONFIG_HID_SUPPORT is not set
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_EHCI_HCD=y
 # CONFIG_USB_EHCI_TT_NEWSCHED is not set
 CONFIG_USB_OHCI_HCD=y
diff --git a/arch/mips/dec/ecc-berr.c b/arch/mips/dec/ecc-berr.c
index 5abf4e89421..2a66e908f6a 100644
--- a/arch/mips/dec/ecc-berr.c
+++ b/arch/mips/dec/ecc-berr.c
@@ -21,6 +21,7 @@
 #include <asm/addrspace.h>
 #include <asm/bootinfo.h>
 #include <asm/cpu.h>
+#include <asm/cpu-type.h>
 #include <asm/irq_regs.h>
 #include <asm/processor.h>
 #include <asm/ptrace.h>
diff --git a/arch/mips/dec/kn02xa-berr.c b/arch/mips/dec/kn02xa-berr.c
index f434b759e3b..ec606363b80 100644
--- a/arch/mips/dec/kn02xa-berr.c
+++ b/arch/mips/dec/kn02xa-berr.c
@@ -19,6 +19,7 @@
 #include <linux/types.h>
 
 #include <asm/addrspace.h>
+#include <asm/cpu-type.h>
 #include <asm/irq_regs.h>
 #include <asm/ptrace.h>
 #include <asm/traps.h>
diff --git a/arch/mips/dec/prom/Makefile b/arch/mips/dec/prom/Makefile
index 064ae7a76bd..ae73e42ac20 100644
--- a/arch/mips/dec/prom/Makefile
+++ b/arch/mips/dec/prom/Makefile
@@ -6,4 +6,3 @@
 lib-y			+= init.o memory.o cmdline.o identify.o console.o
 
 lib-$(CONFIG_32BIT)	+= locore.o
-lib-$(CONFIG_64BIT)	+= call_o32.o
diff --git a/arch/mips/dec/prom/call_o32.S b/arch/mips/dec/prom/call_o32.S
deleted file mode 100644
index 8c8498159e4..00000000000
--- a/arch/mips/dec/prom/call_o32.S
+++ /dev/null
@@ -1,89 +0,0 @@
-/*
- *	O32 interface for the 64 (or N32) ABI.
- *
- *	Copyright (C) 2002  Maciej W. Rozycki
- *
- *	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 <asm/asm.h>
-#include <asm/regdef.h>
-
-/* Maximum number of arguments supported.  Must be even!  */
-#define O32_ARGC	32
-/* Number of static registers we save.  */
-#define O32_STATC	11
-/* Frame size for both of the above.  */
-#define O32_FRAMESZ	(4 * O32_ARGC + SZREG * O32_STATC)
-
-		.text
-
-/*
- * O32 function call dispatcher, for interfacing 32-bit ROM routines.
- *
- * The standard 64 (N32) calling sequence is supported, with a0
- * holding a function pointer, a1-a7 -- its first seven arguments
- * and the stack -- remaining ones (up to O32_ARGC, including a1-a7).
- * Static registers, gp and fp are preserved, v0 holds a result.
- * This code relies on the called o32 function for sp and ra
- * restoration and thus both this dispatcher and the current stack
- * have to be placed in a KSEGx (or KUSEG) address space.  Any
- * pointers passed have to point to addresses within one of these
- * spaces as well.
- */
-NESTED(call_o32, O32_FRAMESZ, ra)
-		REG_SUBU	sp,O32_FRAMESZ
-
-		REG_S		ra,O32_FRAMESZ-1*SZREG(sp)
-		REG_S		fp,O32_FRAMESZ-2*SZREG(sp)
-		REG_S		gp,O32_FRAMESZ-3*SZREG(sp)
-		REG_S		s7,O32_FRAMESZ-4*SZREG(sp)
-		REG_S		s6,O32_FRAMESZ-5*SZREG(sp)
-		REG_S		s5,O32_FRAMESZ-6*SZREG(sp)
-		REG_S		s4,O32_FRAMESZ-7*SZREG(sp)
-		REG_S		s3,O32_FRAMESZ-8*SZREG(sp)
-		REG_S		s2,O32_FRAMESZ-9*SZREG(sp)
-		REG_S		s1,O32_FRAMESZ-10*SZREG(sp)
-		REG_S		s0,O32_FRAMESZ-11*SZREG(sp)
-
-		move		jp,a0
-
-		sll		a0,a1,zero
-		sll		a1,a2,zero
-		sll		a2,a3,zero
-		sll		a3,a4,zero
-		sw		a5,0x10(sp)
-		sw		a6,0x14(sp)
-		sw		a7,0x18(sp)
-
-		PTR_LA		t0,O32_FRAMESZ(sp)
-		PTR_LA		t1,0x1c(sp)
-		li		t2,O32_ARGC-7
-1:
-		lw		t3,(t0)
-		REG_ADDU	t0,SZREG
-		sw		t3,(t1)
-		REG_SUBU	t2,1
-		REG_ADDU	t1,4
-		bnez		t2,1b
-
-		jalr		jp
-
-		REG_L		s0,O32_FRAMESZ-11*SZREG(sp)
-		REG_L		s1,O32_FRAMESZ-10*SZREG(sp)
-		REG_L		s2,O32_FRAMESZ-9*SZREG(sp)
-		REG_L		s3,O32_FRAMESZ-8*SZREG(sp)
-		REG_L		s4,O32_FRAMESZ-7*SZREG(sp)
-		REG_L		s5,O32_FRAMESZ-6*SZREG(sp)
-		REG_L		s6,O32_FRAMESZ-5*SZREG(sp)
-		REG_L		s7,O32_FRAMESZ-4*SZREG(sp)
-		REG_L		gp,O32_FRAMESZ-3*SZREG(sp)
-		REG_L		fp,O32_FRAMESZ-2*SZREG(sp)
-		REG_L		ra,O32_FRAMESZ-1*SZREG(sp)
-
-		REG_ADDU	sp,O32_FRAMESZ
-		jr		ra
-END(call_o32)
diff --git a/arch/mips/fw/lib/call_o32.S b/arch/mips/fw/lib/call_o32.S
index b308b2a0613..4703fe4dbd9 100644
--- a/arch/mips/fw/lib/call_o32.S
+++ b/arch/mips/fw/lib/call_o32.S
@@ -1,7 +1,7 @@
 /*
  *	O32 interface for the 64 (or N32) ABI.
  *
- *	Copyright (C) 2002  Maciej W. Rozycki
+ *	Copyright (C) 2002, 2014  Maciej W. Rozycki
  *
  *	This program is free software; you can redistribute it and/or
  *	modify it under the terms of the GNU General Public License
@@ -12,28 +12,37 @@
 #include <asm/asm.h>
 #include <asm/regdef.h>
 
+/* O32 register size.  */
+#define O32_SZREG	4
 /* Maximum number of arguments supported.  Must be even!  */
 #define O32_ARGC	32
-/* Number of static registers we save.	*/
+/* Number of static registers we save.  */
 #define O32_STATC	11
-/* Frame size for static register  */
-#define O32_FRAMESZ	(SZREG * O32_STATC)
-/* Frame size on new stack */
-#define O32_FRAMESZ_NEW (SZREG + 4 * O32_ARGC)
+/* Argument area frame size.  */
+#define O32_ARGSZ	(O32_SZREG * O32_ARGC)
+/* Static register save area frame size.  */
+#define O32_STATSZ	(SZREG * O32_STATC)
+/* Stack pointer register save area frame size.  */
+#define O32_SPSZ	SZREG
+/* Combined area frame size.  */
+#define O32_FRAMESZ	(O32_ARGSZ + O32_SPSZ + O32_STATSZ)
+/* Switched stack frame size.  */
+#define O32_NFRAMESZ	(O32_ARGSZ + O32_SPSZ)
 
 		.text
 
 /*
  * O32 function call dispatcher, for interfacing 32-bit ROM routines.
  *
- * The standard 64 (N32) calling sequence is supported, with a0
- * holding a function pointer, a1 a new stack pointer, a2-a7 -- its
- * first six arguments and the stack -- remaining ones (up to O32_ARGC,
- * including a2-a7). Static registers, gp and fp are preserved, v0 holds
- * a result. This code relies on the called o32 function for sp and ra
- * restoration and this dispatcher has to be placed in a KSEGx (or KUSEG)
- * address space.  Any pointers passed have to point to addresses within
- * one of these spaces as well.
+ * The standard 64 (N32) calling sequence is supported, with a0 holding
+ * a function pointer, a1 a pointer to the new stack to call the
+ * function with or 0 if no stack switching is requested, a2-a7 -- the
+ * function call's first six arguments, and the stack -- the remaining
+ * arguments (up to O32_ARGC, including a2-a7).  Static registers, gp
+ * and fp are preserved, v0 holds the result.  This code relies on the
+ * called o32 function for sp and ra restoration and this dispatcher has
+ * to be placed in a KSEGx (or KUSEG) address space.  Any pointers
+ * passed have to point to addresses within one of these spaces as well.
  */
 NESTED(call_o32, O32_FRAMESZ, ra)
 		REG_SUBU	sp,O32_FRAMESZ
@@ -51,32 +60,36 @@ NESTED(call_o32, O32_FRAMESZ, ra)
 		REG_S		s0,O32_FRAMESZ-11*SZREG(sp)
 
 		move		jp,a0
-		REG_SUBU	s0,a1,O32_FRAMESZ_NEW
-		REG_S		sp,O32_FRAMESZ_NEW-1*SZREG(s0)
+
+		move		fp,sp
+		beqz		a1,0f
+		REG_SUBU	fp,a1,O32_NFRAMESZ
+0:
+		REG_S		sp,O32_NFRAMESZ-1*SZREG(fp)
 
 		sll		a0,a2,zero
 		sll		a1,a3,zero
 		sll		a2,a4,zero
 		sll		a3,a5,zero
-		sw		a6,0x10(s0)
-		sw		a7,0x14(s0)
+		sw		a6,4*O32_SZREG(fp)
+		sw		a7,5*O32_SZREG(fp)
 
 		PTR_LA		t0,O32_FRAMESZ(sp)
-		PTR_LA		t1,0x18(s0)
+		PTR_LA		t1,6*O32_SZREG(fp)
 		li		t2,O32_ARGC-6
 1:
 		lw		t3,(t0)
 		REG_ADDU	t0,SZREG
 		sw		t3,(t1)
 		REG_SUBU	t2,1
-		REG_ADDU	t1,4
+		REG_ADDU	t1,O32_SZREG
 		bnez		t2,1b
 
-		move		sp,s0
+		move		sp,fp
 
 		jalr		jp
 
-		REG_L		sp,O32_FRAMESZ_NEW-1*SZREG(sp)
+		REG_L		sp,O32_NFRAMESZ-1*SZREG(sp)
 
 		REG_L		s0,O32_FRAMESZ-11*SZREG(sp)
 		REG_L		s1,O32_FRAMESZ-10*SZREG(sp)
diff --git a/arch/mips/fw/sni/sniprom.c b/arch/mips/fw/sni/sniprom.c
index 2c2cb182af4..6aa264b9856 100644
--- a/arch/mips/fw/sni/sniprom.c
+++ b/arch/mips/fw/sni/sniprom.c
@@ -40,7 +40,8 @@
 
 #ifdef CONFIG_64BIT
 
-static u8 o32_stk[16384];
+/* O32 stack has to be 8-byte aligned. */
+static u64 o32_stk[4096];
 #define O32_STK	  &o32_stk[sizeof(o32_stk)]
 
 #define __PROM_O32(fun, arg) fun arg __asm__(#fun); \
diff --git a/arch/mips/include/asm/cpu-info.h b/arch/mips/include/asm/cpu-info.h
index dc2135be2a3..ff2707ab329 100644
--- a/arch/mips/include/asm/cpu-info.h
+++ b/arch/mips/include/asm/cpu-info.h
@@ -39,14 +39,14 @@ struct cache_desc {
 #define MIPS_CACHE_PINDEX	0x00000020	/* Physically indexed cache */
 
 struct cpuinfo_mips {
-	unsigned int		udelay_val;
-	unsigned int		asid_cache;
+	unsigned long		asid_cache;
 
 	/*
 	 * Capability and feature descriptor structure for MIPS CPU
 	 */
 	unsigned long		options;
 	unsigned long		ases;
+	unsigned int		udelay_val;
 	unsigned int		processor_id;
 	unsigned int		fpu_id;
 	unsigned int		msa_id;
diff --git a/arch/mips/include/asm/dec/prom.h b/arch/mips/include/asm/dec/prom.h
index c0ead631384..b59a2103b61 100644
--- a/arch/mips/include/asm/dec/prom.h
+++ b/arch/mips/include/asm/dec/prom.h
@@ -113,31 +113,31 @@ extern int (*__pmax_close)(int);
 #define __DEC_PROM_O32(fun, arg) fun arg __asm__(#fun); \
 				 __asm__(#fun " = call_o32")
 
-int __DEC_PROM_O32(_rex_bootinit, (int (*)(void)));
-int __DEC_PROM_O32(_rex_bootread, (int (*)(void)));
-int __DEC_PROM_O32(_rex_getbitmap, (int (*)(memmap *), memmap *));
+int __DEC_PROM_O32(_rex_bootinit, (int (*)(void), void *));
+int __DEC_PROM_O32(_rex_bootread, (int (*)(void), void *));
+int __DEC_PROM_O32(_rex_getbitmap, (int (*)(memmap *), void *, memmap *));
 unsigned long *__DEC_PROM_O32(_rex_slot_address,
-			     (unsigned long *(*)(int), int));
-void *__DEC_PROM_O32(_rex_gettcinfo, (void *(*)(void)));
-int __DEC_PROM_O32(_rex_getsysid, (int (*)(void)));
-void __DEC_PROM_O32(_rex_clear_cache, (void (*)(void)));
-
-int __DEC_PROM_O32(_prom_getchar, (int (*)(void)));
-char *__DEC_PROM_O32(_prom_getenv, (char *(*)(char *), char *));
-int __DEC_PROM_O32(_prom_printf, (int (*)(char *, ...), char *, ...));
-
-
-#define rex_bootinit()		_rex_bootinit(__rex_bootinit)
-#define rex_bootread()		_rex_bootread(__rex_bootread)
-#define rex_getbitmap(x)	_rex_getbitmap(__rex_getbitmap, x)
-#define rex_slot_address(x)	_rex_slot_address(__rex_slot_address, x)
-#define rex_gettcinfo()		_rex_gettcinfo(__rex_gettcinfo)
-#define rex_getsysid()		_rex_getsysid(__rex_getsysid)
-#define rex_clear_cache()	_rex_clear_cache(__rex_clear_cache)
-
-#define prom_getchar()		_prom_getchar(__prom_getchar)
-#define prom_getenv(x)		_prom_getenv(__prom_getenv, x)
-#define prom_printf(x...)	_prom_printf(__prom_printf, x)
+			     (unsigned long *(*)(int), void *, int));
+void *__DEC_PROM_O32(_rex_gettcinfo, (void *(*)(void), void *));
+int __DEC_PROM_O32(_rex_getsysid, (int (*)(void), void *));
+void __DEC_PROM_O32(_rex_clear_cache, (void (*)(void), void *));
+
+int __DEC_PROM_O32(_prom_getchar, (int (*)(void), void *));
+char *__DEC_PROM_O32(_prom_getenv, (char *(*)(char *), void *, char *));
+int __DEC_PROM_O32(_prom_printf, (int (*)(char *, ...), void *, char *, ...));
+
+
+#define rex_bootinit()		_rex_bootinit(__rex_bootinit, NULL)
+#define rex_bootread()		_rex_bootread(__rex_bootread, NULL)
+#define rex_getbitmap(x)	_rex_getbitmap(__rex_getbitmap, NULL, x)
+#define rex_slot_address(x)	_rex_slot_address(__rex_slot_address, NULL, x)
+#define rex_gettcinfo()		_rex_gettcinfo(__rex_gettcinfo, NULL)
+#define rex_getsysid()		_rex_getsysid(__rex_getsysid, NULL)
+#define rex_clear_cache()	_rex_clear_cache(__rex_clear_cache, NULL)
+
+#define prom_getchar()		_prom_getchar(__prom_getchar, NULL)
+#define prom_getenv(x)		_prom_getenv(__prom_getenv, NULL, x)
+#define prom_printf(x...)	_prom_printf(__prom_printf, NULL, x)
 
 #else /* !CONFIG_64BIT */
 
diff --git a/arch/mips/include/asm/pci.h b/arch/mips/include/asm/pci.h
index 12d6842962b..974b0e30896 100644
--- a/arch/mips/include/asm/pci.h
+++ b/arch/mips/include/asm/pci.h
@@ -73,11 +73,6 @@ extern unsigned long PCIBIOS_MIN_MEM;
 
 extern void pcibios_set_master(struct pci_dev *dev);
 
-static inline void pcibios_penalize_isa_irq(int irq, int active)
-{
-	/* We don't do dynamic PCI IRQ allocation */
-}
-
 #define HAVE_PCI_MMAP
 
 extern int pci_mmap_page_range(struct pci_dev *dev, struct vm_area_struct *vma,
diff --git a/arch/mips/include/asm/rm9k-ocd.h b/arch/mips/include/asm/rm9k-ocd.h
deleted file mode 100644
index b0b80d9ecf9..00000000000
--- a/arch/mips/include/asm/rm9k-ocd.h
+++ /dev/null
@@ -1,56 +0,0 @@
-/*
- *  Copyright (C) 2004 by Basler Vision Technologies AG
- *  Author: Thomas Koeller <thomas.koeller@baslerweb.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
- */
-
-#if !defined(_ASM_RM9K_OCD_H)
-#define _ASM_RM9K_OCD_H
-
-#include <linux/types.h>
-#include <linux/spinlock.h>
-#include <asm/io.h>
-
-extern volatile void __iomem * const ocd_base;
-extern volatile void __iomem * const titan_base;
-
-#define ocd_addr(__x__)		(ocd_base + (__x__))
-#define titan_addr(__x__)	(titan_base + (__x__))
-#define scram_addr(__x__)	(scram_base + (__x__))
-
-/* OCD register access */
-#define ocd_readl(__offs__) __raw_readl(ocd_addr(__offs__))
-#define ocd_readw(__offs__) __raw_readw(ocd_addr(__offs__))
-#define ocd_readb(__offs__) __raw_readb(ocd_addr(__offs__))
-#define ocd_writel(__val__, __offs__) \
-	__raw_writel((__val__), ocd_addr(__offs__))
-#define ocd_writew(__val__, __offs__) \
-	__raw_writew((__val__), ocd_addr(__offs__))
-#define ocd_writeb(__val__, __offs__) \
-	__raw_writeb((__val__), ocd_addr(__offs__))
-
-/* TITAN register access - 32 bit-wide only */
-#define titan_readl(__offs__) __raw_readl(titan_addr(__offs__))
-#define titan_writel(__val__, __offs__) \
-	__raw_writel((__val__), titan_addr(__offs__))
-
-/* Protect access to shared TITAN registers */
-extern spinlock_t titan_lock;
-extern int titan_irqflags;
-#define lock_titan_regs() spin_lock_irqsave(&titan_lock, titan_irqflags)
-#define unlock_titan_regs() spin_unlock_irqrestore(&titan_lock, titan_irqflags)
-
-#endif	/* !defined(_ASM_RM9K_OCD_H) */
diff --git a/arch/mips/include/asm/syscall.h b/arch/mips/include/asm/syscall.h
index c6e9cd2bca8..17960fe7a8c 100644
--- a/arch/mips/include/asm/syscall.h
+++ b/arch/mips/include/asm/syscall.h
@@ -133,6 +133,8 @@ static inline int syscall_get_arch(void)
 #ifdef CONFIG_64BIT
 	if (!test_thread_flag(TIF_32BIT_REGS))
 		arch |= __AUDIT_ARCH_64BIT;
+	if (test_thread_flag(TIF_32BIT_ADDR))
+		arch |= __AUDIT_ARCH_CONVENTION_MIPS64_N32;
 #endif
 #if defined(__LITTLE_ENDIAN)
 	arch |=  __AUDIT_ARCH_LE;
diff --git a/arch/mips/include/uapi/asm/inst.h b/arch/mips/include/uapi/asm/inst.h
index df6e775f3fe..3125797f2a8 100644
--- a/arch/mips/include/uapi/asm/inst.h
+++ b/arch/mips/include/uapi/asm/inst.h
@@ -484,13 +484,13 @@ enum MIPS6e_i8_func {
  * Damn ...  bitfields depend from byteorder :-(
  */
 #ifdef __MIPSEB__
-#define BITFIELD_FIELD(field, more)					\
+#define __BITFIELD_FIELD(field, more)					\
 	field;								\
 	more
 
 #elif defined(__MIPSEL__)
 
-#define BITFIELD_FIELD(field, more)					\
+#define __BITFIELD_FIELD(field, more)					\
 	more								\
 	field;
 
@@ -499,112 +499,112 @@ enum MIPS6e_i8_func {
 #endif
 
 struct j_format {
-	BITFIELD_FIELD(unsigned int opcode : 6, /* Jump format */
-	BITFIELD_FIELD(unsigned int target : 26,
+	__BITFIELD_FIELD(unsigned int opcode : 6, /* Jump format */
+	__BITFIELD_FIELD(unsigned int target : 26,
 	;))
 };
 
 struct i_format {			/* signed immediate format */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(unsigned int rs : 5,
-	BITFIELD_FIELD(unsigned int rt : 5,
-	BITFIELD_FIELD(signed int simmediate : 16,
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(unsigned int rs : 5,
+	__BITFIELD_FIELD(unsigned int rt : 5,
+	__BITFIELD_FIELD(signed int simmediate : 16,
 	;))))
 };
 
 struct u_format {			/* unsigned immediate format */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(unsigned int rs : 5,
-	BITFIELD_FIELD(unsigned int rt : 5,
-	BITFIELD_FIELD(unsigned int uimmediate : 16,
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(unsigned int rs : 5,
+	__BITFIELD_FIELD(unsigned int rt : 5,
+	__BITFIELD_FIELD(unsigned int uimmediate : 16,
 	;))))
 };
 
 struct c_format {			/* Cache (>= R6000) format */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(unsigned int rs : 5,
-	BITFIELD_FIELD(unsigned int c_op : 3,
-	BITFIELD_FIELD(unsigned int cache : 2,
-	BITFIELD_FIELD(unsigned int simmediate : 16,
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(unsigned int rs : 5,
+	__BITFIELD_FIELD(unsigned int c_op : 3,
+	__BITFIELD_FIELD(unsigned int cache : 2,
+	__BITFIELD_FIELD(unsigned int simmediate : 16,
 	;)))))
 };
 
 struct r_format {			/* Register format */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(unsigned int rs : 5,
-	BITFIELD_FIELD(unsigned int rt : 5,
-	BITFIELD_FIELD(unsigned int rd : 5,
-	BITFIELD_FIELD(unsigned int re : 5,
-	BITFIELD_FIELD(unsigned int func : 6,
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(unsigned int rs : 5,
+	__BITFIELD_FIELD(unsigned int rt : 5,
+	__BITFIELD_FIELD(unsigned int rd : 5,
+	__BITFIELD_FIELD(unsigned int re : 5,
+	__BITFIELD_FIELD(unsigned int func : 6,
 	;))))))
 };
 
 struct p_format {		/* Performance counter format (R10000) */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(unsigned int rs : 5,
-	BITFIELD_FIELD(unsigned int rt : 5,
-	BITFIELD_FIELD(unsigned int rd : 5,
-	BITFIELD_FIELD(unsigned int re : 5,
-	BITFIELD_FIELD(unsigned int func : 6,
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(unsigned int rs : 5,
+	__BITFIELD_FIELD(unsigned int rt : 5,
+	__BITFIELD_FIELD(unsigned int rd : 5,
+	__BITFIELD_FIELD(unsigned int re : 5,
+	__BITFIELD_FIELD(unsigned int func : 6,
 	;))))))
 };
 
 struct f_format {			/* FPU register format */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(unsigned int : 1,
-	BITFIELD_FIELD(unsigned int fmt : 4,
-	BITFIELD_FIELD(unsigned int rt : 5,
-	BITFIELD_FIELD(unsigned int rd : 5,
-	BITFIELD_FIELD(unsigned int re : 5,
-	BITFIELD_FIELD(unsigned int func : 6,
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(unsigned int : 1,
+	__BITFIELD_FIELD(unsigned int fmt : 4,
+	__BITFIELD_FIELD(unsigned int rt : 5,
+	__BITFIELD_FIELD(unsigned int rd : 5,
+	__BITFIELD_FIELD(unsigned int re : 5,
+	__BITFIELD_FIELD(unsigned int func : 6,
 	;)))))))
 };
 
 struct ma_format {		/* FPU multiply and add format (MIPS IV) */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(unsigned int fr : 5,
-	BITFIELD_FIELD(unsigned int ft : 5,
-	BITFIELD_FIELD(unsigned int fs : 5,
-	BITFIELD_FIELD(unsigned int fd : 5,
-	BITFIELD_FIELD(unsigned int func : 4,
-	BITFIELD_FIELD(unsigned int fmt : 2,
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(unsigned int fr : 5,
+	__BITFIELD_FIELD(unsigned int ft : 5,
+	__BITFIELD_FIELD(unsigned int fs : 5,
+	__BITFIELD_FIELD(unsigned int fd : 5,
+	__BITFIELD_FIELD(unsigned int func : 4,
+	__BITFIELD_FIELD(unsigned int fmt : 2,
 	;)))))))
 };
 
 struct b_format {			/* BREAK and SYSCALL */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(unsigned int code : 20,
-	BITFIELD_FIELD(unsigned int func : 6,
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(unsigned int code : 20,
+	__BITFIELD_FIELD(unsigned int func : 6,
 	;)))
 };
 
 struct ps_format {			/* MIPS-3D / paired single format */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(unsigned int rs : 5,
-	BITFIELD_FIELD(unsigned int ft : 5,
-	BITFIELD_FIELD(unsigned int fs : 5,
-	BITFIELD_FIELD(unsigned int fd : 5,
-	BITFIELD_FIELD(unsigned int func : 6,
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(unsigned int rs : 5,
+	__BITFIELD_FIELD(unsigned int ft : 5,
+	__BITFIELD_FIELD(unsigned int fs : 5,
+	__BITFIELD_FIELD(unsigned int fd : 5,
+	__BITFIELD_FIELD(unsigned int func : 6,
 	;))))))
 };
 
 struct v_format {				/* MDMX vector format */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(unsigned int sel : 4,
-	BITFIELD_FIELD(unsigned int fmt : 1,
-	BITFIELD_FIELD(unsigned int vt : 5,
-	BITFIELD_FIELD(unsigned int vs : 5,
-	BITFIELD_FIELD(unsigned int vd : 5,
-	BITFIELD_FIELD(unsigned int func : 6,
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(unsigned int sel : 4,
+	__BITFIELD_FIELD(unsigned int fmt : 1,
+	__BITFIELD_FIELD(unsigned int vt : 5,
+	__BITFIELD_FIELD(unsigned int vs : 5,
+	__BITFIELD_FIELD(unsigned int vd : 5,
+	__BITFIELD_FIELD(unsigned int func : 6,
 	;)))))))
 };
 
 struct spec3_format {   /* SPEC3 */
-	BITFIELD_FIELD(unsigned int opcode:6,
-	BITFIELD_FIELD(unsigned int rs:5,
-	BITFIELD_FIELD(unsigned int rt:5,
-	BITFIELD_FIELD(signed int simmediate:9,
-	BITFIELD_FIELD(unsigned int func:7,
+	__BITFIELD_FIELD(unsigned int opcode:6,
+	__BITFIELD_FIELD(unsigned int rs:5,
+	__BITFIELD_FIELD(unsigned int rt:5,
+	__BITFIELD_FIELD(signed int simmediate:9,
+	__BITFIELD_FIELD(unsigned int func:7,
 	;)))))
 };
 
@@ -616,141 +616,141 @@ struct spec3_format {   /* SPEC3 */
  *	if it is MIPS32 instruction re-encoded for use in the microMIPS ASE.
  */
 struct fb_format {		/* FPU branch format (MIPS32) */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(unsigned int bc : 5,
-	BITFIELD_FIELD(unsigned int cc : 3,
-	BITFIELD_FIELD(unsigned int flag : 2,
-	BITFIELD_FIELD(signed int simmediate : 16,
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(unsigned int bc : 5,
+	__BITFIELD_FIELD(unsigned int cc : 3,
+	__BITFIELD_FIELD(unsigned int flag : 2,
+	__BITFIELD_FIELD(signed int simmediate : 16,
 	;)))))
 };
 
 struct fp0_format {		/* FPU multiply and add format (MIPS32) */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(unsigned int fmt : 5,
-	BITFIELD_FIELD(unsigned int ft : 5,
-	BITFIELD_FIELD(unsigned int fs : 5,
-	BITFIELD_FIELD(unsigned int fd : 5,
-	BITFIELD_FIELD(unsigned int func : 6,
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(unsigned int fmt : 5,
+	__BITFIELD_FIELD(unsigned int ft : 5,
+	__BITFIELD_FIELD(unsigned int fs : 5,
+	__BITFIELD_FIELD(unsigned int fd : 5,
+	__BITFIELD_FIELD(unsigned int func : 6,
 	;))))))
 };
 
 struct mm_fp0_format {		/* FPU multipy and add format (microMIPS) */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(unsigned int ft : 5,
-	BITFIELD_FIELD(unsigned int fs : 5,
-	BITFIELD_FIELD(unsigned int fd : 5,
-	BITFIELD_FIELD(unsigned int fmt : 3,
-	BITFIELD_FIELD(unsigned int op : 2,
-	BITFIELD_FIELD(unsigned int func : 6,
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(unsigned int ft : 5,
+	__BITFIELD_FIELD(unsigned int fs : 5,
+	__BITFIELD_FIELD(unsigned int fd : 5,
+	__BITFIELD_FIELD(unsigned int fmt : 3,
+	__BITFIELD_FIELD(unsigned int op : 2,
+	__BITFIELD_FIELD(unsigned int func : 6,
 	;)))))))
 };
 
 struct fp1_format {		/* FPU mfc1 and cfc1 format (MIPS32) */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(unsigned int op : 5,
-	BITFIELD_FIELD(unsigned int rt : 5,
-	BITFIELD_FIELD(unsigned int fs : 5,
-	BITFIELD_FIELD(unsigned int fd : 5,
-	BITFIELD_FIELD(unsigned int func : 6,
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(unsigned int op : 5,
+	__BITFIELD_FIELD(unsigned int rt : 5,
+	__BITFIELD_FIELD(unsigned int fs : 5,
+	__BITFIELD_FIELD(unsigned int fd : 5,
+	__BITFIELD_FIELD(unsigned int func : 6,
 	;))))))
 };
 
 struct mm_fp1_format {		/* FPU mfc1 and cfc1 format (microMIPS) */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(unsigned int rt : 5,
-	BITFIELD_FIELD(unsigned int fs : 5,
-	BITFIELD_FIELD(unsigned int fmt : 2,
-	BITFIELD_FIELD(unsigned int op : 8,
-	BITFIELD_FIELD(unsigned int func : 6,
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(unsigned int rt : 5,
+	__BITFIELD_FIELD(unsigned int fs : 5,
+	__BITFIELD_FIELD(unsigned int fmt : 2,
+	__BITFIELD_FIELD(unsigned int op : 8,
+	__BITFIELD_FIELD(unsigned int func : 6,
 	;))))))
 };
 
 struct mm_fp2_format {		/* FPU movt and movf format (microMIPS) */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(unsigned int fd : 5,
-	BITFIELD_FIELD(unsigned int fs : 5,
-	BITFIELD_FIELD(unsigned int cc : 3,
-	BITFIELD_FIELD(unsigned int zero : 2,
-	BITFIELD_FIELD(unsigned int fmt : 2,
-	BITFIELD_FIELD(unsigned int op : 3,
-	BITFIELD_FIELD(unsigned int func : 6,
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(unsigned int fd : 5,
+	__BITFIELD_FIELD(unsigned int fs : 5,
+	__BITFIELD_FIELD(unsigned int cc : 3,
+	__BITFIELD_FIELD(unsigned int zero : 2,
+	__BITFIELD_FIELD(unsigned int fmt : 2,
+	__BITFIELD_FIELD(unsigned int op : 3,
+	__BITFIELD_FIELD(unsigned int func : 6,
 	;))))))))
 };
 
 struct mm_fp3_format {		/* FPU abs and neg format (microMIPS) */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(unsigned int rt : 5,
-	BITFIELD_FIELD(unsigned int fs : 5,
-	BITFIELD_FIELD(unsigned int fmt : 3,
-	BITFIELD_FIELD(unsigned int op : 7,
-	BITFIELD_FIELD(unsigned int func : 6,
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(unsigned int rt : 5,
+	__BITFIELD_FIELD(unsigned int fs : 5,
+	__BITFIELD_FIELD(unsigned int fmt : 3,
+	__BITFIELD_FIELD(unsigned int op : 7,
+	__BITFIELD_FIELD(unsigned int func : 6,
 	;))))))
 };
 
 struct mm_fp4_format {		/* FPU c.cond format (microMIPS) */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(unsigned int rt : 5,
-	BITFIELD_FIELD(unsigned int fs : 5,
-	BITFIELD_FIELD(unsigned int cc : 3,
-	BITFIELD_FIELD(unsigned int fmt : 3,
-	BITFIELD_FIELD(unsigned int cond : 4,
-	BITFIELD_FIELD(unsigned int func : 6,
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(unsigned int rt : 5,
+	__BITFIELD_FIELD(unsigned int fs : 5,
+	__BITFIELD_FIELD(unsigned int cc : 3,
+	__BITFIELD_FIELD(unsigned int fmt : 3,
+	__BITFIELD_FIELD(unsigned int cond : 4,
+	__BITFIELD_FIELD(unsigned int func : 6,
 	;)))))))
 };
 
 struct mm_fp5_format {		/* FPU lwxc1 and swxc1 format (microMIPS) */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(unsigned int index : 5,
-	BITFIELD_FIELD(unsigned int base : 5,
-	BITFIELD_FIELD(unsigned int fd : 5,
-	BITFIELD_FIELD(unsigned int op : 5,
-	BITFIELD_FIELD(unsigned int func : 6,
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(unsigned int index : 5,
+	__BITFIELD_FIELD(unsigned int base : 5,
+	__BITFIELD_FIELD(unsigned int fd : 5,
+	__BITFIELD_FIELD(unsigned int op : 5,
+	__BITFIELD_FIELD(unsigned int func : 6,
 	;))))))
 };
 
 struct fp6_format {		/* FPU madd and msub format (MIPS IV) */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(unsigned int fr : 5,
-	BITFIELD_FIELD(unsigned int ft : 5,
-	BITFIELD_FIELD(unsigned int fs : 5,
-	BITFIELD_FIELD(unsigned int fd : 5,
-	BITFIELD_FIELD(unsigned int func : 6,
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(unsigned int fr : 5,
+	__BITFIELD_FIELD(unsigned int ft : 5,
+	__BITFIELD_FIELD(unsigned int fs : 5,
+	__BITFIELD_FIELD(unsigned int fd : 5,
+	__BITFIELD_FIELD(unsigned int func : 6,
 	;))))))
 };
 
 struct mm_fp6_format {		/* FPU madd and msub format (microMIPS) */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(unsigned int ft : 5,
-	BITFIELD_FIELD(unsigned int fs : 5,
-	BITFIELD_FIELD(unsigned int fd : 5,
-	BITFIELD_FIELD(unsigned int fr : 5,
-	BITFIELD_FIELD(unsigned int func : 6,
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(unsigned int ft : 5,
+	__BITFIELD_FIELD(unsigned int fs : 5,
+	__BITFIELD_FIELD(unsigned int fd : 5,
+	__BITFIELD_FIELD(unsigned int fr : 5,
+	__BITFIELD_FIELD(unsigned int func : 6,
 	;))))))
 };
 
 struct mm_i_format {		/* Immediate format (microMIPS) */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(unsigned int rt : 5,
-	BITFIELD_FIELD(unsigned int rs : 5,
-	BITFIELD_FIELD(signed int simmediate : 16,
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(unsigned int rt : 5,
+	__BITFIELD_FIELD(unsigned int rs : 5,
+	__BITFIELD_FIELD(signed int simmediate : 16,
 	;))))
 };
 
 struct mm_m_format {		/* Multi-word load/store format (microMIPS) */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(unsigned int rd : 5,
-	BITFIELD_FIELD(unsigned int base : 5,
-	BITFIELD_FIELD(unsigned int func : 4,
-	BITFIELD_FIELD(signed int simmediate : 12,
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(unsigned int rd : 5,
+	__BITFIELD_FIELD(unsigned int base : 5,
+	__BITFIELD_FIELD(unsigned int func : 4,
+	__BITFIELD_FIELD(signed int simmediate : 12,
 	;)))))
 };
 
 struct mm_x_format {		/* Scaled indexed load format (microMIPS) */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(unsigned int index : 5,
-	BITFIELD_FIELD(unsigned int base : 5,
-	BITFIELD_FIELD(unsigned int rd : 5,
-	BITFIELD_FIELD(unsigned int func : 11,
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(unsigned int index : 5,
+	__BITFIELD_FIELD(unsigned int base : 5,
+	__BITFIELD_FIELD(unsigned int rd : 5,
+	__BITFIELD_FIELD(unsigned int func : 11,
 	;)))))
 };
 
@@ -758,51 +758,51 @@ struct mm_x_format {		/* Scaled indexed load format (microMIPS) */
  * microMIPS instruction formats (16-bit length)
  */
 struct mm_b0_format {		/* Unconditional branch format (microMIPS) */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(signed int simmediate : 10,
-	BITFIELD_FIELD(unsigned int : 16, /* Ignored */
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(signed int simmediate : 10,
+	__BITFIELD_FIELD(unsigned int : 16, /* Ignored */
 	;)))
 };
 
 struct mm_b1_format {		/* Conditional branch format (microMIPS) */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(unsigned int rs : 3,
-	BITFIELD_FIELD(signed int simmediate : 7,
-	BITFIELD_FIELD(unsigned int : 16, /* Ignored */
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(unsigned int rs : 3,
+	__BITFIELD_FIELD(signed int simmediate : 7,
+	__BITFIELD_FIELD(unsigned int : 16, /* Ignored */
 	;))))
 };
 
 struct mm16_m_format {		/* Multi-word load/store format */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(unsigned int func : 4,
-	BITFIELD_FIELD(unsigned int rlist : 2,
-	BITFIELD_FIELD(unsigned int imm : 4,
-	BITFIELD_FIELD(unsigned int : 16, /* Ignored */
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(unsigned int func : 4,
+	__BITFIELD_FIELD(unsigned int rlist : 2,
+	__BITFIELD_FIELD(unsigned int imm : 4,
+	__BITFIELD_FIELD(unsigned int : 16, /* Ignored */
 	;)))))
 };
 
 struct mm16_rb_format {		/* Signed immediate format */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(unsigned int rt : 3,
-	BITFIELD_FIELD(unsigned int base : 3,
-	BITFIELD_FIELD(signed int simmediate : 4,
-	BITFIELD_FIELD(unsigned int : 16, /* Ignored */
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(unsigned int rt : 3,
+	__BITFIELD_FIELD(unsigned int base : 3,
+	__BITFIELD_FIELD(signed int simmediate : 4,
+	__BITFIELD_FIELD(unsigned int : 16, /* Ignored */
 	;)))))
 };
 
 struct mm16_r3_format {		/* Load from global pointer format */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(unsigned int rt : 3,
-	BITFIELD_FIELD(signed int simmediate : 7,
-	BITFIELD_FIELD(unsigned int : 16, /* Ignored */
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(unsigned int rt : 3,
+	__BITFIELD_FIELD(signed int simmediate : 7,
+	__BITFIELD_FIELD(unsigned int : 16, /* Ignored */
 	;))))
 };
 
 struct mm16_r5_format {		/* Load/store from stack pointer format */
-	BITFIELD_FIELD(unsigned int opcode : 6,
-	BITFIELD_FIELD(unsigned int rt : 5,
-	BITFIELD_FIELD(signed int simmediate : 5,
-	BITFIELD_FIELD(unsigned int : 16, /* Ignored */
+	__BITFIELD_FIELD(unsigned int opcode : 6,
+	__BITFIELD_FIELD(unsigned int rt : 5,
+	__BITFIELD_FIELD(signed int simmediate : 5,
+	__BITFIELD_FIELD(unsigned int : 16, /* Ignored */
 	;))))
 };
 
@@ -810,57 +810,57 @@ struct mm16_r5_format {		/* Load/store from stack pointer format */
  * MIPS16e instruction formats (16-bit length)
  */
 struct m16e_rr {
-	BITFIELD_FIELD(unsigned int opcode : 5,
-	BITFIELD_FIELD(unsigned int rx : 3,
-	BITFIELD_FIELD(unsigned int nd : 1,
-	BITFIELD_FIELD(unsigned int l : 1,
-	BITFIELD_FIELD(unsigned int ra : 1,
-	BITFIELD_FIELD(unsigned int func : 5,
+	__BITFIELD_FIELD(unsigned int opcode : 5,
+	__BITFIELD_FIELD(unsigned int rx : 3,
+	__BITFIELD_FIELD(unsigned int nd : 1,
+	__BITFIELD_FIELD(unsigned int l : 1,
+	__BITFIELD_FIELD(unsigned int ra : 1,
+	__BITFIELD_FIELD(unsigned int func : 5,
 	;))))))
 };
 
 struct m16e_jal {
-	BITFIELD_FIELD(unsigned int opcode : 5,
-	BITFIELD_FIELD(unsigned int x : 1,
-	BITFIELD_FIELD(unsigned int imm20_16 : 5,
-	BITFIELD_FIELD(signed int imm25_21 : 5,
+	__BITFIELD_FIELD(unsigned int opcode : 5,
+	__BITFIELD_FIELD(unsigned int x : 1,
+	__BITFIELD_FIELD(unsigned int imm20_16 : 5,
+	__BITFIELD_FIELD(signed int imm25_21 : 5,
 	;))))
 };
 
 struct m16e_i64 {
-	BITFIELD_FIELD(unsigned int opcode : 5,
-	BITFIELD_FIELD(unsigned int func : 3,
-	BITFIELD_FIELD(unsigned int imm : 8,
+	__BITFIELD_FIELD(unsigned int opcode : 5,
+	__BITFIELD_FIELD(unsigned int func : 3,
+	__BITFIELD_FIELD(unsigned int imm : 8,
 	;)))
 };
 
 struct m16e_ri64 {
-	BITFIELD_FIELD(unsigned int opcode : 5,
-	BITFIELD_FIELD(unsigned int func : 3,
-	BITFIELD_FIELD(unsigned int ry : 3,
-	BITFIELD_FIELD(unsigned int imm : 5,
+	__BITFIELD_FIELD(unsigned int opcode : 5,
+	__BITFIELD_FIELD(unsigned int func : 3,
+	__BITFIELD_FIELD(unsigned int ry : 3,
+	__BITFIELD_FIELD(unsigned int imm : 5,
 	;))))
 };
 
 struct m16e_ri {
-	BITFIELD_FIELD(unsigned int opcode : 5,
-	BITFIELD_FIELD(unsigned int rx : 3,
-	BITFIELD_FIELD(unsigned int imm : 8,
+	__BITFIELD_FIELD(unsigned int opcode : 5,
+	__BITFIELD_FIELD(unsigned int rx : 3,
+	__BITFIELD_FIELD(unsigned int imm : 8,
 	;)))
 };
 
 struct m16e_rri {
-	BITFIELD_FIELD(unsigned int opcode : 5,
-	BITFIELD_FIELD(unsigned int rx : 3,
-	BITFIELD_FIELD(unsigned int ry : 3,
-	BITFIELD_FIELD(unsigned int imm : 5,
+	__BITFIELD_FIELD(unsigned int opcode : 5,
+	__BITFIELD_FIELD(unsigned int rx : 3,
+	__BITFIELD_FIELD(unsigned int ry : 3,
+	__BITFIELD_FIELD(unsigned int imm : 5,
 	;))))
 };
 
 struct m16e_i8 {
-	BITFIELD_FIELD(unsigned int opcode : 5,
-	BITFIELD_FIELD(unsigned int func : 3,
-	BITFIELD_FIELD(unsigned int imm : 8,
+	__BITFIELD_FIELD(unsigned int opcode : 5,
+	__BITFIELD_FIELD(unsigned int func : 3,
+	__BITFIELD_FIELD(unsigned int imm : 8,
 	;)))
 };
 
diff --git a/arch/mips/include/uapi/asm/unistd.h b/arch/mips/include/uapi/asm/unistd.h
index d6e154a9e6a..5805414777e 100644
--- a/arch/mips/include/uapi/asm/unistd.h
+++ b/arch/mips/include/uapi/asm/unistd.h
@@ -371,16 +371,17 @@
 #define __NR_finit_module		(__NR_Linux + 348)
 #define __NR_sched_setattr		(__NR_Linux + 349)
 #define __NR_sched_getattr		(__NR_Linux + 350)
+#define __NR_renameat2			(__NR_Linux + 351)
 
 /*
  * Offset of the last Linux o32 flavoured syscall
  */
-#define __NR_Linux_syscalls		350
+#define __NR_Linux_syscalls		351
 
 #endif /* _MIPS_SIM == _MIPS_SIM_ABI32 */
 
 #define __NR_O32_Linux			4000
-#define __NR_O32_Linux_syscalls		350
+#define __NR_O32_Linux_syscalls		351
 
 #if _MIPS_SIM == _MIPS_SIM_ABI64
 
@@ -699,16 +700,17 @@
 #define __NR_getdents64			(__NR_Linux + 308)
 #define __NR_sched_setattr		(__NR_Linux + 309)
 #define __NR_sched_getattr		(__NR_Linux + 310)
+#define __NR_renameat2			(__NR_Linux + 311)
 
 /*
  * Offset of the last Linux 64-bit flavoured syscall
  */
-#define __NR_Linux_syscalls		310
+#define __NR_Linux_syscalls		311
 
 #endif /* _MIPS_SIM == _MIPS_SIM_ABI64 */
 
 #define __NR_64_Linux			5000
-#define __NR_64_Linux_syscalls		310
+#define __NR_64_Linux_syscalls		311
 
 #if _MIPS_SIM == _MIPS_SIM_NABI32
 
@@ -1031,15 +1033,16 @@
 #define __NR_finit_module		(__NR_Linux + 312)
 #define __NR_sched_setattr		(__NR_Linux + 313)
 #define __NR_sched_getattr		(__NR_Linux + 314)
+#define __NR_renameat2			(__NR_Linux + 315)
 
 /*
  * Offset of the last N32 flavoured syscall
  */
-#define __NR_Linux_syscalls		314
+#define __NR_Linux_syscalls		315
 
 #endif /* _MIPS_SIM == _MIPS_SIM_NABI32 */
 
 #define __NR_N32_Linux			6000
-#define __NR_N32_Linux_syscalls		314
+#define __NR_N32_Linux_syscalls		315
 
 #endif /* _UAPI_ASM_UNISTD_H */
diff --git a/arch/mips/kernel/branch.c b/arch/mips/kernel/branch.c
index 4d78bf445a9..76122ff5cb5 100644
--- a/arch/mips/kernel/branch.c
+++ b/arch/mips/kernel/branch.c
@@ -317,7 +317,7 @@ int __compute_return_epc_for_insn(struct pt_regs *regs,
 		if (regs->regs[insn.i_format.rs] ==
 		    regs->regs[insn.i_format.rt]) {
 			epc = epc + 4 + (insn.i_format.simmediate << 2);
-			if (insn.i_format.rt == beql_op)
+			if (insn.i_format.opcode == beql_op)
 				ret = BRANCH_LIKELY_TAKEN;
 		} else
 			epc += 8;
@@ -329,7 +329,7 @@ int __compute_return_epc_for_insn(struct pt_regs *regs,
 		if (regs->regs[insn.i_format.rs] !=
 		    regs->regs[insn.i_format.rt]) {
 			epc = epc + 4 + (insn.i_format.simmediate << 2);
-			if (insn.i_format.rt == bnel_op)
+			if (insn.i_format.opcode == bnel_op)
 				ret = BRANCH_LIKELY_TAKEN;
 		} else
 			epc += 8;
@@ -341,7 +341,7 @@ int __compute_return_epc_for_insn(struct pt_regs *regs,
 		/* rt field assumed to be zero */
 		if ((long)regs->regs[insn.i_format.rs] <= 0) {
 			epc = epc + 4 + (insn.i_format.simmediate << 2);
-			if (insn.i_format.rt == bnel_op)
+			if (insn.i_format.opcode == blezl_op)
 				ret = BRANCH_LIKELY_TAKEN;
 		} else
 			epc += 8;
@@ -353,7 +353,7 @@ int __compute_return_epc_for_insn(struct pt_regs *regs,
 		/* rt field assumed to be zero */
 		if ((long)regs->regs[insn.i_format.rs] > 0) {
 			epc = epc + 4 + (insn.i_format.simmediate << 2);
-			if (insn.i_format.rt == bnel_op)
+			if (insn.i_format.opcode == bgtzl_op)
 				ret = BRANCH_LIKELY_TAKEN;
 		} else
 			epc += 8;
diff --git a/arch/mips/kernel/proc.c b/arch/mips/kernel/proc.c
index e40971b51d2..037a44d962f 100644
--- a/arch/mips/kernel/proc.c
+++ b/arch/mips/kernel/proc.c
@@ -124,14 +124,7 @@ static int show_cpuinfo(struct seq_file *m, void *v)
 	seq_printf(m, "kscratch registers\t: %d\n",
 		      hweight8(cpu_data[n].kscratch_mask));
 	seq_printf(m, "core\t\t\t: %d\n", cpu_data[n].core);
-#if defined(CONFIG_MIPS_MT_SMP) || defined(CONFIG_MIPS_MT_SMTC)
-	if (cpu_has_mipsmt) {
-		seq_printf(m, "VPE\t\t\t: %d\n", cpu_data[n].vpe_id);
-#if defined(CONFIG_MIPS_MT_SMTC)
-		seq_printf(m, "TC\t\t\t: %d\n", cpu_data[n].tc_id);
-#endif
-	}
-#endif
+
 	sprintf(fmt, "VCE%%c exceptions\t\t: %s\n",
 		      cpu_has_vce ? "%u" : "not available");
 	seq_printf(m, fmt, 'D', vced_count);
diff --git a/arch/mips/kernel/ptrace.c b/arch/mips/kernel/ptrace.c
index 71f85f42703..f639ccd5060 100644
--- a/arch/mips/kernel/ptrace.c
+++ b/arch/mips/kernel/ptrace.c
@@ -163,7 +163,7 @@ int ptrace_get_watch_regs(struct task_struct *child,
 	enum pt_watch_style style;
 	int i;
 
-	if (!cpu_has_watch || current_cpu_data.watch_reg_use_cnt == 0)
+	if (!cpu_has_watch || boot_cpu_data.watch_reg_use_cnt == 0)
 		return -EIO;
 	if (!access_ok(VERIFY_WRITE, addr, sizeof(struct pt_watch_regs)))
 		return -EIO;
@@ -177,14 +177,14 @@ int ptrace_get_watch_regs(struct task_struct *child,
 #endif
 
 	__put_user(style, &addr->style);
-	__put_user(current_cpu_data.watch_reg_use_cnt,
+	__put_user(boot_cpu_data.watch_reg_use_cnt,
 		   &addr->WATCH_STYLE.num_valid);
-	for (i = 0; i < current_cpu_data.watch_reg_use_cnt; i++) {
+	for (i = 0; i < boot_cpu_data.watch_reg_use_cnt; i++) {
 		__put_user(child->thread.watch.mips3264.watchlo[i],
 			   &addr->WATCH_STYLE.watchlo[i]);
 		__put_user(child->thread.watch.mips3264.watchhi[i] & 0xfff,
 			   &addr->WATCH_STYLE.watchhi[i]);
-		__put_user(current_cpu_data.watch_reg_masks[i],
+		__put_user(boot_cpu_data.watch_reg_masks[i],
 			   &addr->WATCH_STYLE.watch_masks[i]);
 	}
 	for (; i < 8; i++) {
@@ -204,12 +204,12 @@ int ptrace_set_watch_regs(struct task_struct *child,
 	unsigned long lt[NUM_WATCH_REGS];
 	u16 ht[NUM_WATCH_REGS];
 
-	if (!cpu_has_watch || current_cpu_data.watch_reg_use_cnt == 0)
+	if (!cpu_has_watch || boot_cpu_data.watch_reg_use_cnt == 0)
 		return -EIO;
 	if (!access_ok(VERIFY_READ, addr, sizeof(struct pt_watch_regs)))
 		return -EIO;
 	/* Check the values. */
-	for (i = 0; i < current_cpu_data.watch_reg_use_cnt; i++) {
+	for (i = 0; i < boot_cpu_data.watch_reg_use_cnt; i++) {
 		__get_user(lt[i], &addr->WATCH_STYLE.watchlo[i]);
 #ifdef CONFIG_32BIT
 		if (lt[i] & __UA_LIMIT)
@@ -228,7 +228,7 @@ int ptrace_set_watch_regs(struct task_struct *child,
 			return -EINVAL;
 	}
 	/* Install them. */
-	for (i = 0; i < current_cpu_data.watch_reg_use_cnt; i++) {
+	for (i = 0; i < boot_cpu_data.watch_reg_use_cnt; i++) {
 		if (lt[i] & 7)
 			watch_active = 1;
 		child->thread.watch.mips3264.watchlo[i] = lt[i];
diff --git a/arch/mips/kernel/scall32-o32.S b/arch/mips/kernel/scall32-o32.S
index fdc70b40044..3245474f19d 100644
--- a/arch/mips/kernel/scall32-o32.S
+++ b/arch/mips/kernel/scall32-o32.S
@@ -577,3 +577,4 @@ EXPORT(sys_call_table)
 	PTR	sys_finit_module
 	PTR	sys_sched_setattr
 	PTR	sys_sched_getattr		/* 4350 */
+	PTR	sys_renameat2
diff --git a/arch/mips/kernel/scall64-64.S b/arch/mips/kernel/scall64-64.S
index dd99c3285ae..be2fedd4ae3 100644
--- a/arch/mips/kernel/scall64-64.S
+++ b/arch/mips/kernel/scall64-64.S
@@ -430,4 +430,5 @@ EXPORT(sys_call_table)
 	PTR	sys_getdents64
 	PTR	sys_sched_setattr
 	PTR	sys_sched_getattr		/* 5310 */
+	PTR	sys_renameat2
 	.size	sys_call_table,.-sys_call_table
diff --git a/arch/mips/kernel/scall64-n32.S b/arch/mips/kernel/scall64-n32.S
index f68d2f4f009..c1dbcda4b81 100644
--- a/arch/mips/kernel/scall64-n32.S
+++ b/arch/mips/kernel/scall64-n32.S
@@ -423,4 +423,5 @@ EXPORT(sysn32_call_table)
 	PTR	sys_finit_module
 	PTR	sys_sched_setattr
 	PTR	sys_sched_getattr
+	PTR	sys_renameat2			/* 6315 */
 	.size	sysn32_call_table,.-sysn32_call_table
diff --git a/arch/mips/kernel/scall64-o32.S b/arch/mips/kernel/scall64-o32.S
index 70f6acecd92..f1343ccd7ed 100644
--- a/arch/mips/kernel/scall64-o32.S
+++ b/arch/mips/kernel/scall64-o32.S
@@ -556,4 +556,5 @@ EXPORT(sys32_call_table)
 	PTR	sys_finit_module
 	PTR	sys_sched_setattr
 	PTR	sys_sched_getattr		/* 4350 */
+	PTR	sys_renameat2
 	.size	sys32_call_table,.-sys32_call_table
diff --git a/arch/mips/kernel/traps.c b/arch/mips/kernel/traps.c
index 074e857ced2..8119ac2fdfc 100644
--- a/arch/mips/kernel/traps.c
+++ b/arch/mips/kernel/traps.c
@@ -1545,7 +1545,7 @@ asmlinkage void cache_parity_error(void)
 	       reg_val & (1<<30) ? "secondary" : "primary",
 	       reg_val & (1<<31) ? "data" : "insn");
 	if (cpu_has_mips_r2 &&
-	    ((current_cpu_data.processor_id && 0xff0000) == PRID_COMP_MIPS)) {
+	    ((current_cpu_data.processor_id & 0xff0000) == PRID_COMP_MIPS)) {
 		pr_err("Error bits: %s%s%s%s%s%s%s%s\n",
 			reg_val & (1<<29) ? "ED " : "",
 			reg_val & (1<<28) ? "ET " : "",
@@ -1585,7 +1585,7 @@ asmlinkage void do_ftlb(void)
 
 	/* For the moment, report the problem and hang. */
 	if (cpu_has_mips_r2 &&
-	    ((current_cpu_data.processor_id && 0xff0000) == PRID_COMP_MIPS)) {
+	    ((current_cpu_data.processor_id & 0xff0000) == PRID_COMP_MIPS)) {
 		pr_err("FTLB error exception, cp0_ecc=0x%08x:\n",
 		       read_c0_ecc());
 		pr_err("cp0_errorepc == %0*lx\n", field, read_c0_errorepc());
diff --git a/arch/mips/lantiq/dts/easy50712.dts b/arch/mips/lantiq/dts/easy50712.dts
index fac1f5b178e..143b8a37b5e 100644
--- a/arch/mips/lantiq/dts/easy50712.dts
+++ b/arch/mips/lantiq/dts/easy50712.dts
@@ -8,6 +8,7 @@
 	};
 
 	memory@0 {
+		device_type = "memory";
 		reg = <0x0 0x2000000>;
 	};
 
diff --git a/arch/mips/lib/csum_partial.S b/arch/mips/lib/csum_partial.S
index 2e4825e4838..9901237563c 100644
--- a/arch/mips/lib/csum_partial.S
+++ b/arch/mips/lib/csum_partial.S
@@ -56,14 +56,20 @@
 #define UNIT(unit)  ((unit)*NBYTES)
 
 #define ADDC(sum,reg)						\
+	.set	push;						\
+	.set	noat;						\
 	ADD	sum, reg;					\
 	sltu	v1, sum, reg;					\
 	ADD	sum, v1;					\
+	.set	pop
 
 #define ADDC32(sum,reg)						\
+	.set	push;						\
+	.set	noat;						\
 	addu	sum, reg;					\
 	sltu	v1, sum, reg;					\
 	addu	sum, v1;					\
+	.set	pop
 
 #define CSUM_BIGCHUNK1(src, offset, sum, _t0, _t1, _t2, _t3)	\
 	LOAD	_t0, (offset + UNIT(0))(src);			\
@@ -710,6 +716,8 @@ LEAF(csum_partial)
 	ADDC(sum, t2)
 .Ldone\@:
 	/* fold checksum */
+	.set	push
+	.set	noat
 #ifdef USE_DOUBLE
 	dsll32	v1, sum, 0
 	daddu	sum, v1
@@ -732,6 +740,7 @@ LEAF(csum_partial)
 	or	sum, sum, t0
 1:
 #endif
+	.set	pop
 	.set reorder
 	ADDC32(sum, psum)
 	jr	ra
diff --git a/arch/mips/lib/delay.c b/arch/mips/lib/delay.c
index 44713af15a6..705cfb7c1a7 100644
--- a/arch/mips/lib/delay.c
+++ b/arch/mips/lib/delay.c
@@ -6,7 +6,7 @@
  * Copyright (C) 1994 by Waldorf Electronics
  * Copyright (C) 1995 - 2000, 01, 03 by Ralf Baechle
  * Copyright (C) 1999, 2000 Silicon Graphics, Inc.
- * Copyright (C) 2007  Maciej W. Rozycki
+ * Copyright (C) 2007, 2014 Maciej W. Rozycki
  */
 #include <linux/module.h>
 #include <linux/param.h>
@@ -15,6 +15,12 @@
 #include <asm/compiler.h>
 #include <asm/war.h>
 
+#ifndef CONFIG_CPU_DADDI_WORKAROUNDS
+#define GCC_DADDI_IMM_ASM() "I"
+#else
+#define GCC_DADDI_IMM_ASM() "r"
+#endif
+
 void __delay(unsigned long loops)
 {
 	__asm__ __volatile__ (
@@ -22,13 +28,13 @@ void __delay(unsigned long loops)
 	"	.align	3					\n"
 	"1:	bnez	%0, 1b					\n"
 #if BITS_PER_LONG == 32
-	"	subu	%0, 1					\n"
+	"	subu	%0, %1					\n"
 #else
-	"	dsubu	%0, 1					\n"
+	"	dsubu	%0, %1					\n"
 #endif
 	"	.set	reorder					\n"
 	: "=r" (loops)
-	: "0" (loops));
+	: GCC_DADDI_IMM_ASM() (1), "0" (loops));
 }
 EXPORT_SYMBOL(__delay);
 
diff --git a/arch/mips/lib/strncpy_user.S b/arch/mips/lib/strncpy_user.S
index d3301cd1e9a..3c32baf8b49 100644
--- a/arch/mips/lib/strncpy_user.S
+++ b/arch/mips/lib/strncpy_user.S
@@ -35,7 +35,6 @@ LEAF(__strncpy_from_\func\()_asm)
 	bnez		v0, .Lfault\@
 
 FEXPORT(__strncpy_from_\func\()_nocheck_asm)
-	.set		noreorder
 	move		t0, zero
 	move		v1, a1
 .ifeqs "\func","kernel"
@@ -45,21 +44,21 @@ FEXPORT(__strncpy_from_\func\()_nocheck_asm)
 .endif
 	PTR_ADDIU	v1, 1
 	R10KCBARRIER(0(ra))
+	sb		v0, (a0)
 	beqz		v0, 2f
-	 sb		v0, (a0)
 	PTR_ADDIU	t0, 1
+	PTR_ADDIU	a0, 1
 	bne		t0, a2, 1b
-	 PTR_ADDIU	a0, 1
 2:	PTR_ADDU	v0, a1, t0
 	xor		v0, a1
 	bltz		v0, .Lfault\@
-	 nop
+	move		v0, t0
 	jr		ra			# return n
-	 move		v0, t0
 	END(__strncpy_from_\func\()_asm)
 
-.Lfault\@: jr		ra
-	  li		v0, -EFAULT
+.Lfault\@:
+	li		v0, -EFAULT
+	jr		ra
 
 	.section	__ex_table,"a"
 	PTR		1b, .Lfault\@
diff --git a/arch/mips/loongson/Kconfig b/arch/mips/loongson/Kconfig
index 7397be226a0..603d79a95f4 100644
--- a/arch/mips/loongson/Kconfig
+++ b/arch/mips/loongson/Kconfig
@@ -64,7 +64,6 @@ config LEMOTE_MACH3A
 	bool "Lemote Loongson 3A family machines"
 	select ARCH_SPARSEMEM_ENABLE
 	select GENERIC_ISA_DMA_SUPPORT_BROKEN
-	select GENERIC_HARDIRQS_NO__DO_IRQ
 	select BOOT_ELF32
 	select BOARD_SCACHE
 	select CSRC_R4K
diff --git a/arch/mips/loongson/common/cs5536/cs5536_mfgpt.c b/arch/mips/loongson/common/cs5536/cs5536_mfgpt.c
index c639b9db001..12c75db2342 100644
--- a/arch/mips/loongson/common/cs5536/cs5536_mfgpt.c
+++ b/arch/mips/loongson/common/cs5536/cs5536_mfgpt.c
@@ -27,8 +27,7 @@
 
 #include <cs5536/cs5536_mfgpt.h>
 
-DEFINE_SPINLOCK(mfgpt_lock);
-EXPORT_SYMBOL(mfgpt_lock);
+static DEFINE_RAW_SPINLOCK(mfgpt_lock);
 
 static u32 mfgpt_base;
 
@@ -55,7 +54,7 @@ EXPORT_SYMBOL(enable_mfgpt0_counter);
 static void init_mfgpt_timer(enum clock_event_mode mode,
 			     struct clock_event_device *evt)
 {
-	spin_lock(&mfgpt_lock);
+	raw_spin_lock(&mfgpt_lock);
 
 	switch (mode) {
 	case CLOCK_EVT_MODE_PERIODIC:
@@ -79,7 +78,7 @@ static void init_mfgpt_timer(enum clock_event_mode mode,
 		/* Nothing to do here */
 		break;
 	}
-	spin_unlock(&mfgpt_lock);
+	raw_spin_unlock(&mfgpt_lock);
 }
 
 static struct clock_event_device mfgpt_clockevent = {
@@ -157,7 +156,7 @@ static cycle_t mfgpt_read(struct clocksource *cs)
 	static int old_count;
 	static u32 old_jifs;
 
-	spin_lock_irqsave(&mfgpt_lock, flags);
+	raw_spin_lock_irqsave(&mfgpt_lock, flags);
 	/*
 	 * Although our caller may have the read side of xtime_lock,
 	 * this is now a seqlock, and we are cheating in this routine
@@ -191,7 +190,7 @@ static cycle_t mfgpt_read(struct clocksource *cs)
 	old_count = count;
 	old_jifs = jifs;
 
-	spin_unlock_irqrestore(&mfgpt_lock, flags);
+	raw_spin_unlock_irqrestore(&mfgpt_lock, flags);
 
 	return (cycle_t) (jifs * COMPARE) + count;
 }
diff --git a/arch/mips/loongson/lemote-2f/clock.c b/arch/mips/loongson/lemote-2f/clock.c
index e1f427f4f5f..67dd94ef28e 100644
--- a/arch/mips/loongson/lemote-2f/clock.c
+++ b/arch/mips/loongson/lemote-2f/clock.c
@@ -91,6 +91,7 @@ EXPORT_SYMBOL(clk_put);
 
 int clk_set_rate(struct clk *clk, unsigned long rate)
 {
+	unsigned int rate_khz = rate / 1000;
 	int ret = 0;
 	int regval;
 	int i;
@@ -111,10 +112,10 @@ int clk_set_rate(struct clk *clk, unsigned long rate)
 		if (loongson2_clockmod_table[i].frequency ==
 		    CPUFREQ_ENTRY_INVALID)
 			continue;
-		if (rate == loongson2_clockmod_table[i].frequency)
+		if (rate_khz == loongson2_clockmod_table[i].frequency)
 			break;
 	}
-	if (rate != loongson2_clockmod_table[i].frequency)
+	if (rate_khz != loongson2_clockmod_table[i].frequency)
 		return -ENOTSUPP;
 
 	clk->rate = rate;
diff --git a/arch/mips/mm/cache.c b/arch/mips/mm/cache.c
index e422b38d311..9e67cdea3c7 100644
--- a/arch/mips/mm/cache.c
+++ b/arch/mips/mm/cache.c
@@ -29,15 +29,15 @@ void (*flush_cache_range)(struct vm_area_struct *vma, unsigned long start,
 void (*flush_cache_page)(struct vm_area_struct *vma, unsigned long page,
 	unsigned long pfn);
 void (*flush_icache_range)(unsigned long start, unsigned long end);
+EXPORT_SYMBOL_GPL(flush_icache_range);
 void (*local_flush_icache_range)(unsigned long start, unsigned long end);
 
 void (*__flush_cache_vmap)(void);
 void (*__flush_cache_vunmap)(void);
 
 void (*__flush_kernel_vmap_range)(unsigned long vaddr, int size);
-void (*__invalidate_kernel_vmap_range)(unsigned long vaddr, int size);
-
 EXPORT_SYMBOL_GPL(__flush_kernel_vmap_range);
+void (*__invalidate_kernel_vmap_range)(unsigned long vaddr, int size);
 
 /* MIPS specific cache operations */
 void (*flush_cache_sigtramp)(unsigned long addr);
diff --git a/arch/mips/mm/page.c b/arch/mips/mm/page.c
index 58033c44690..b611102e23b 100644
--- a/arch/mips/mm/page.c
+++ b/arch/mips/mm/page.c
@@ -273,7 +273,7 @@ void build_clear_page(void)
 		uasm_i_ori(&buf, A2, A0, off);
 
 	if (R4600_V2_HIT_CACHEOP_WAR && cpu_is_r4600_v2_x())
-		uasm_i_lui(&buf, AT, 0xa000);
+		uasm_i_lui(&buf, AT, uasm_rel_hi(0xa0000000));
 
 	off = cache_line_size ? min(8, pref_bias_clear_store / cache_line_size)
 				* cache_line_size : 0;
@@ -424,7 +424,7 @@ void build_copy_page(void)
 		uasm_i_ori(&buf, A2, A0, off);
 
 	if (R4600_V2_HIT_CACHEOP_WAR && cpu_is_r4600_v2_x())
-		uasm_i_lui(&buf, AT, 0xa000);
+		uasm_i_lui(&buf, AT, uasm_rel_hi(0xa0000000));
 
 	off = cache_line_size ? min(8, pref_bias_copy_load / cache_line_size) *
 				cache_line_size : 0;
diff --git a/arch/mips/mm/tlb-funcs.S b/arch/mips/mm/tlb-funcs.S
index 30a494db99c..a5427c6e975 100644
--- a/arch/mips/mm/tlb-funcs.S
+++ b/arch/mips/mm/tlb-funcs.S
@@ -16,8 +16,10 @@
 
 #define FASTPATH_SIZE	128
 
+EXPORT(tlbmiss_handler_setup_pgd_start)
 LEAF(tlbmiss_handler_setup_pgd)
-	.space		16 * 4
+1:	j	1b		/* Dummy, will be replaced. */
+	.space	64
 END(tlbmiss_handler_setup_pgd)
 EXPORT(tlbmiss_handler_setup_pgd_end)
 
diff --git a/arch/mips/mm/tlbex.c b/arch/mips/mm/tlbex.c
index ee88367ab3a..f99ec587b15 100644
--- a/arch/mips/mm/tlbex.c
+++ b/arch/mips/mm/tlbex.c
@@ -1422,16 +1422,17 @@ static void build_r4000_tlb_refill_handler(void)
 extern u32 handle_tlbl[], handle_tlbl_end[];
 extern u32 handle_tlbs[], handle_tlbs_end[];
 extern u32 handle_tlbm[], handle_tlbm_end[];
-extern u32 tlbmiss_handler_setup_pgd[], tlbmiss_handler_setup_pgd_end[];
+extern u32 tlbmiss_handler_setup_pgd_start[], tlbmiss_handler_setup_pgd[];
+extern u32 tlbmiss_handler_setup_pgd_end[];
 
 static void build_setup_pgd(void)
 {
 	const int a0 = 4;
 	const int __maybe_unused a1 = 5;
 	const int __maybe_unused a2 = 6;
-	u32 *p = tlbmiss_handler_setup_pgd;
+	u32 *p = tlbmiss_handler_setup_pgd_start;
 	const int tlbmiss_handler_setup_pgd_size =
-		tlbmiss_handler_setup_pgd_end - tlbmiss_handler_setup_pgd;
+		tlbmiss_handler_setup_pgd_end - tlbmiss_handler_setup_pgd_start;
 #ifndef CONFIG_MIPS_PGD_C0_CONTEXT
 	long pgdc = (long)pgd_current;
 #endif
diff --git a/arch/mips/mti-malta/malta-memory.c b/arch/mips/mti-malta/malta-memory.c
index 6d0f4ab3632..f2364e41968 100644
--- a/arch/mips/mti-malta/malta-memory.c
+++ b/arch/mips/mti-malta/malta-memory.c
@@ -27,7 +27,7 @@ unsigned long physical_memsize = 0L;
 fw_memblock_t * __init fw_getmdesc(int eva)
 {
 	char *memsize_str, *ememsize_str __maybe_unused = NULL, *ptr;
-	unsigned long memsize, ememsize __maybe_unused = 0;
+	unsigned long memsize = 0, ememsize __maybe_unused = 0;
 	static char cmdline[COMMAND_LINE_SIZE] __initdata;
 	int tmp;
 
diff --git a/arch/mips/pci/pci-rc32434.c b/arch/mips/pci/pci-rc32434.c
index b128cb973eb..7f6ce6d734c 100644
--- a/arch/mips/pci/pci-rc32434.c
+++ b/arch/mips/pci/pci-rc32434.c
@@ -53,7 +53,6 @@ static struct resource rc32434_res_pci_mem1 = {
 	.start = 0x50000000,
 	.end = 0x5FFFFFFF,
 	.flags = IORESOURCE_MEM,
-	.parent = &rc32434_res_pci_mem1,
 	.sibling = NULL,
 	.child = &rc32434_res_pci_mem2
 };
diff --git a/arch/mips/ralink/dts/mt7620a_eval.dts b/arch/mips/ralink/dts/mt7620a_eval.dts
index 35eb874ab7f..709f58132f5 100644
--- a/arch/mips/ralink/dts/mt7620a_eval.dts
+++ b/arch/mips/ralink/dts/mt7620a_eval.dts
@@ -7,6 +7,7 @@
 	model = "Ralink MT7620A evaluation board";
 
 	memory@0 {
+		device_type = "memory";
 		reg = <0x0 0x2000000>;
 	};
 
diff --git a/arch/mips/ralink/dts/rt2880_eval.dts b/arch/mips/ralink/dts/rt2880_eval.dts
index 322d7002595..0a685db093d 100644
--- a/arch/mips/ralink/dts/rt2880_eval.dts
+++ b/arch/mips/ralink/dts/rt2880_eval.dts
@@ -7,6 +7,7 @@
 	model = "Ralink RT2880 evaluation board";
 
 	memory@0 {
+		device_type = "memory";
 		reg = <0x8000000 0x2000000>;
 	};
 
diff --git a/arch/mips/ralink/dts/rt3052_eval.dts b/arch/mips/ralink/dts/rt3052_eval.dts
index 0ac73ea2819..ec9e9a03554 100644
--- a/arch/mips/ralink/dts/rt3052_eval.dts
+++ b/arch/mips/ralink/dts/rt3052_eval.dts
@@ -7,6 +7,7 @@
 	model = "Ralink RT3052 evaluation board";
 
 	memory@0 {
+		device_type = "memory";
 		reg = <0x0 0x2000000>;
 	};
 
diff --git a/arch/mips/ralink/dts/rt3883_eval.dts b/arch/mips/ralink/dts/rt3883_eval.dts
index 2fa6b330bf4..e8df21a5d10 100644
--- a/arch/mips/ralink/dts/rt3883_eval.dts
+++ b/arch/mips/ralink/dts/rt3883_eval.dts
@@ -7,6 +7,7 @@
 	model = "Ralink RT3883 evaluation board";
 
 	memory@0 {
+		device_type = "memory";
 		reg = <0x0 0x2000000>;
 	};
 
diff --git a/arch/mn10300/include/asm/pci.h b/arch/mn10300/include/asm/pci.h
index 16632382468..5f70af25c7d 100644
--- a/arch/mn10300/include/asm/pci.h
+++ b/arch/mn10300/include/asm/pci.h
@@ -48,7 +48,6 @@ extern void unit_pci_init(void);
 #define PCIBIOS_MIN_MEM		0xB8000000
 
 void pcibios_set_master(struct pci_dev *dev);
-void pcibios_penalize_isa_irq(int irq);
 
 /* Dynamic DMA mapping stuff.
  * i386 has everything mapped statically.
diff --git a/arch/mn10300/unit-asb2305/pci-irq.c b/arch/mn10300/unit-asb2305/pci-irq.c
index 77439da0467..fcb28ceb824 100644
--- a/arch/mn10300/unit-asb2305/pci-irq.c
+++ b/arch/mn10300/unit-asb2305/pci-irq.c
@@ -40,10 +40,6 @@ void __init pcibios_fixup_irqs(void)
 	}
 }
 
-void __init pcibios_penalize_isa_irq(int irq)
-{
-}
-
 void pcibios_enable_irq(struct pci_dev *dev)
 {
 	pci_write_config_byte(dev, PCI_INTERRUPT_LINE, dev->irq);
diff --git a/arch/parisc/Kconfig b/arch/parisc/Kconfig
index 1faefed3274..108d48e652a 100644
--- a/arch/parisc/Kconfig
+++ b/arch/parisc/Kconfig
@@ -22,6 +22,7 @@ config PARISC
 	select GENERIC_SMP_IDLE_THREAD
 	select GENERIC_STRNCPY_FROM_USER
 	select SYSCTL_ARCH_UNALIGN_ALLOW
+	select SYSCTL_EXCEPTION_TRACE
 	select HAVE_MOD_ARCH_SPECIFIC
 	select VIRT_TO_BUS
 	select MODULES_USE_ELF_RELA
diff --git a/arch/parisc/configs/c3000_defconfig b/arch/parisc/configs/c3000_defconfig
index acacd348df8..fb92b892078 100644
--- a/arch/parisc/configs/c3000_defconfig
+++ b/arch/parisc/configs/c3000_defconfig
@@ -127,8 +127,6 @@ CONFIG_SND_SEQUENCER_OSS=y
 CONFIG_SND_AD1889=y
 CONFIG_USB_HIDDEV=y
 CONFIG_USB=y
-CONFIG_USB_DEBUG=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_OHCI_HCD=y
 CONFIG_USB_PRINTER=m
 CONFIG_USB_STORAGE=m
diff --git a/arch/parisc/configs/default_defconfig b/arch/parisc/configs/default_defconfig
index ba61495e1fa..4d8127e8428 100644
--- a/arch/parisc/configs/default_defconfig
+++ b/arch/parisc/configs/default_defconfig
@@ -145,7 +145,6 @@ CONFIG_HID_SONY=y
 CONFIG_HID_SUNPLUS=y
 CONFIG_HID_TOPSEED=y
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=y
 CONFIG_USB_OHCI_HCD=y
 CONFIG_USB_UHCI_HCD=y
diff --git a/arch/parisc/configs/generic-64bit_defconfig b/arch/parisc/configs/generic-64bit_defconfig
index 28c1b5de044..dc0d7ce71ea 100644
--- a/arch/parisc/configs/generic-64bit_defconfig
+++ b/arch/parisc/configs/generic-64bit_defconfig
@@ -219,7 +219,6 @@ CONFIG_HIDRAW=y
 CONFIG_HID_PID=y
 CONFIG_USB_HIDDEV=y
 CONFIG_USB=y
-CONFIG_USB_DEBUG=y
 CONFIG_USB_ANNOUNCE_NEW_DEVICES=y
 CONFIG_USB_DYNAMIC_MINORS=y
 CONFIG_USB_MON=m
diff --git a/arch/parisc/include/asm/pci.h b/arch/parisc/include/asm/pci.h
index 465154076d2..20df2b04fc0 100644
--- a/arch/parisc/include/asm/pci.h
+++ b/arch/parisc/include/asm/pci.h
@@ -215,11 +215,6 @@ static inline void pci_dma_burst_advice(struct pci_dev *pdev,
 }
 #endif
 
-static inline void pcibios_penalize_isa_irq(int irq, int active)
-{
-	/* We don't need to penalize isa irq's */
-}
-
 static inline int pci_get_legacy_ide_irq(struct pci_dev *dev, int channel)
 {
 	return channel ? 15 : 14;
diff --git a/arch/parisc/include/asm/processor.h b/arch/parisc/include/asm/processor.h
index 198a86feb57..d951c9681ab 100644
--- a/arch/parisc/include/asm/processor.h
+++ b/arch/parisc/include/asm/processor.h
@@ -55,6 +55,11 @@
 #define STACK_TOP	TASK_SIZE
 #define STACK_TOP_MAX	DEFAULT_TASK_SIZE
 
+/* Allow bigger stacks for 64-bit processes */
+#define STACK_SIZE_MAX	(USER_WIDE_MODE					\
+			 ? (1 << 30)	/* 1 GB */			\
+			 : (CONFIG_MAX_STACK_SIZE_MB*1024*1024))
+
 #endif
 
 #ifndef __ASSEMBLY__
diff --git a/arch/parisc/include/uapi/asm/Kbuild b/arch/parisc/include/uapi/asm/Kbuild
index a580642555b..348356c9951 100644
--- a/arch/parisc/include/uapi/asm/Kbuild
+++ b/arch/parisc/include/uapi/asm/Kbuild
@@ -1,6 +1,8 @@
 # UAPI Header export list
 include include/uapi/asm-generic/Kbuild.asm
 
+generic-y += resource.h
+
 header-y += bitsperlong.h
 header-y += byteorder.h
 header-y += errno.h
@@ -13,7 +15,6 @@ header-y += msgbuf.h
 header-y += pdc.h
 header-y += posix_types.h
 header-y += ptrace.h
-header-y += resource.h
 header-y += sembuf.h
 header-y += setup.h
 header-y += shmbuf.h
diff --git a/arch/parisc/include/uapi/asm/resource.h b/arch/parisc/include/uapi/asm/resource.h
deleted file mode 100644
index 8b06343b62e..00000000000
--- a/arch/parisc/include/uapi/asm/resource.h
+++ /dev/null
@@ -1,7 +0,0 @@
-#ifndef _ASM_PARISC_RESOURCE_H
-#define _ASM_PARISC_RESOURCE_H
-
-#define _STK_LIM_MAX	10 * _STK_LIM
-#include <asm-generic/resource.h>
-
-#endif
diff --git a/arch/parisc/include/uapi/asm/unistd.h b/arch/parisc/include/uapi/asm/unistd.h
index 265ae5190b0..47e0e21d227 100644
--- a/arch/parisc/include/uapi/asm/unistd.h
+++ b/arch/parisc/include/uapi/asm/unistd.h
@@ -829,8 +829,9 @@
 #define __NR_sched_setattr	(__NR_Linux + 334)
 #define __NR_sched_getattr	(__NR_Linux + 335)
 #define __NR_utimes		(__NR_Linux + 336)
+#define __NR_renameat2		(__NR_Linux + 337)
 
-#define __NR_Linux_syscalls	(__NR_utimes + 1)
+#define __NR_Linux_syscalls	(__NR_renameat2 + 1)
 
 
 #define __IGNORE_select		/* newselect */
diff --git a/arch/parisc/kernel/sys_parisc.c b/arch/parisc/kernel/sys_parisc.c
index 31ffa9b5532..e1ffea2f9a0 100644
--- a/arch/parisc/kernel/sys_parisc.c
+++ b/arch/parisc/kernel/sys_parisc.c
@@ -72,10 +72,10 @@ static unsigned long mmap_upper_limit(void)
 {
 	unsigned long stack_base;
 
-	/* Limit stack size to 1GB - see setup_arg_pages() in fs/exec.c */
+	/* Limit stack size - see setup_arg_pages() in fs/exec.c */
 	stack_base = rlimit_max(RLIMIT_STACK);
-	if (stack_base > (1 << 30))
-		stack_base = 1 << 30;
+	if (stack_base > STACK_SIZE_MAX)
+		stack_base = STACK_SIZE_MAX;
 
 	return PAGE_ALIGN(STACK_TOP - stack_base);
 }
diff --git a/arch/parisc/kernel/syscall.S b/arch/parisc/kernel/syscall.S
index a63bb179f79..83878601103 100644
--- a/arch/parisc/kernel/syscall.S
+++ b/arch/parisc/kernel/syscall.S
@@ -589,10 +589,13 @@ cas_nocontend:
 # endif
 /* ENABLE_LWS_DEBUG */
 
+	rsm	PSW_SM_I, %r0				/* Disable interrupts */
+	/* COW breaks can cause contention on UP systems */
 	LDCW	0(%sr2,%r20), %r28			/* Try to acquire the lock */
 	cmpb,<>,n	%r0, %r28, cas_action		/* Did we get it? */
 cas_wouldblock:
 	ldo	2(%r0), %r28				/* 2nd case */
+	ssm	PSW_SM_I, %r0
 	b	lws_exit				/* Contended... */
 	ldo	-EAGAIN(%r0), %r21			/* Spin in userspace */
 
@@ -619,15 +622,17 @@ cas_action:
 	stw	%r1, 4(%sr2,%r20)
 #endif
 	/* The load and store could fail */
-1:	ldw	0(%sr3,%r26), %r28
+1:	ldw,ma	0(%sr3,%r26), %r28
 	sub,<>	%r28, %r25, %r0
-2:	stw	%r24, 0(%sr3,%r26)
+2:	stw,ma	%r24, 0(%sr3,%r26)
 	/* Free lock */
-	stw	%r20, 0(%sr2,%r20)
+	stw,ma	%r20, 0(%sr2,%r20)
 #if ENABLE_LWS_DEBUG
 	/* Clear thread register indicator */
 	stw	%r0, 4(%sr2,%r20)
 #endif
+	/* Enable interrupts */
+	ssm	PSW_SM_I, %r0
 	/* Return to userspace, set no error */
 	b	lws_exit
 	copy	%r0, %r21
@@ -639,6 +644,7 @@ cas_action:
 #if ENABLE_LWS_DEBUG
 	stw	%r0, 4(%sr2,%r20)
 #endif
+	ssm	PSW_SM_I, %r0
 	b	lws_exit
 	ldo	-EFAULT(%r0),%r21	/* set errno */
 	nop
diff --git a/arch/parisc/kernel/syscall_table.S b/arch/parisc/kernel/syscall_table.S
index 83ead0ea127..c5fa7a697fb 100644
--- a/arch/parisc/kernel/syscall_table.S
+++ b/arch/parisc/kernel/syscall_table.S
@@ -432,6 +432,7 @@
 	ENTRY_SAME(sched_setattr)
 	ENTRY_SAME(sched_getattr)	/* 335 */
 	ENTRY_COMP(utimes)
+	ENTRY_SAME(renameat2)
 
 	/* Nothing yet */
 
diff --git a/arch/parisc/kernel/traps.c b/arch/parisc/kernel/traps.c
index 1cd1d0c83b6..47ee620d15d 100644
--- a/arch/parisc/kernel/traps.c
+++ b/arch/parisc/kernel/traps.c
@@ -25,6 +25,7 @@
 #include <linux/interrupt.h>
 #include <linux/console.h>
 #include <linux/bug.h>
+#include <linux/ratelimit.h>
 
 #include <asm/assembly.h>
 #include <asm/uaccess.h>
@@ -42,9 +43,6 @@
 
 #include "../math-emu/math-emu.h"	/* for handle_fpe() */
 
-#define PRINT_USER_FAULTS /* (turn this on if you want user faults to be */
-			  /*  dumped to the console via printk)          */
-
 #if defined(CONFIG_SMP) || defined(CONFIG_DEBUG_SPINLOCK)
 DEFINE_SPINLOCK(pa_dbit_lock);
 #endif
@@ -160,6 +158,17 @@ void show_regs(struct pt_regs *regs)
 	}
 }
 
+static DEFINE_RATELIMIT_STATE(_hppa_rs,
+	DEFAULT_RATELIMIT_INTERVAL, DEFAULT_RATELIMIT_BURST);
+
+#define parisc_printk_ratelimited(critical, regs, fmt, ...)	{	      \
+	if ((critical || show_unhandled_signals) && __ratelimit(&_hppa_rs)) { \
+		printk(fmt, ##__VA_ARGS__);				      \
+		show_regs(regs);					      \
+	}								      \
+}
+
+
 static void do_show_stack(struct unwind_frame_info *info)
 {
 	int i = 1;
@@ -229,12 +238,10 @@ void die_if_kernel(char *str, struct pt_regs *regs, long err)
 		if (err == 0)
 			return; /* STFU */
 
-		printk(KERN_CRIT "%s (pid %d): %s (code %ld) at " RFMT "\n",
+		parisc_printk_ratelimited(1, regs,
+			KERN_CRIT "%s (pid %d): %s (code %ld) at " RFMT "\n",
 			current->comm, task_pid_nr(current), str, err, regs->iaoq[0]);
-#ifdef PRINT_USER_FAULTS
-		/* XXX for debugging only */
-		show_regs(regs);
-#endif
+
 		return;
 	}
 
@@ -321,14 +328,11 @@ static void handle_break(struct pt_regs *regs)
 			(tt == BUG_TRAP_TYPE_NONE) ? 9 : 0);
 	}
 
-#ifdef PRINT_USER_FAULTS
-	if (unlikely(iir != GDB_BREAK_INSN)) {
-		printk(KERN_DEBUG "break %d,%d: pid=%d command='%s'\n",
+	if (unlikely(iir != GDB_BREAK_INSN))
+		parisc_printk_ratelimited(0, regs,
+			KERN_DEBUG "break %d,%d: pid=%d command='%s'\n",
 			iir & 31, (iir>>13) & ((1<<13)-1),
 			task_pid_nr(current), current->comm);
-		show_regs(regs);
-	}
-#endif
 
 	/* send standard GDB signal */
 	handle_gdb_break(regs, TRAP_BRKPT);
@@ -758,11 +762,9 @@ void notrace handle_interruption(int code, struct pt_regs *regs)
 
 	default:
 		if (user_mode(regs)) {
-#ifdef PRINT_USER_FAULTS
-			printk(KERN_DEBUG "\nhandle_interruption() pid=%d command='%s'\n",
-			    task_pid_nr(current), current->comm);
-			show_regs(regs);
-#endif
+			parisc_printk_ratelimited(0, regs, KERN_DEBUG
+				"handle_interruption() pid=%d command='%s'\n",
+				task_pid_nr(current), current->comm);
 			/* SIGBUS, for lack of a better one. */
 			si.si_signo = SIGBUS;
 			si.si_code = BUS_OBJERR;
@@ -779,16 +781,10 @@ void notrace handle_interruption(int code, struct pt_regs *regs)
 
 	if (user_mode(regs)) {
 	    if ((fault_space >> SPACEID_SHIFT) != (regs->sr[7] >> SPACEID_SHIFT)) {
-#ifdef PRINT_USER_FAULTS
-		if (fault_space == 0)
-			printk(KERN_DEBUG "User Fault on Kernel Space ");
-		else
-			printk(KERN_DEBUG "User Fault (long pointer) (fault %d) ",
-			       code);
-		printk(KERN_CONT "pid=%d command='%s'\n",
-		       task_pid_nr(current), current->comm);
-		show_regs(regs);
-#endif
+		parisc_printk_ratelimited(0, regs, KERN_DEBUG
+				"User fault %d on space 0x%08lx, pid=%d command='%s'\n",
+				code, fault_space,
+				task_pid_nr(current), current->comm);
 		si.si_signo = SIGSEGV;
 		si.si_errno = 0;
 		si.si_code = SEGV_MAPERR;
diff --git a/arch/parisc/mm/fault.c b/arch/parisc/mm/fault.c
index 747550762f3..3ca9c1131cf 100644
--- a/arch/parisc/mm/fault.c
+++ b/arch/parisc/mm/fault.c
@@ -19,10 +19,6 @@
 #include <asm/uaccess.h>
 #include <asm/traps.h>
 
-#define PRINT_USER_FAULTS /* (turn this on if you want user faults to be */
-			 /*  dumped to the console via printk)          */
-
-
 /* Various important other fields */
 #define bit22set(x)		(x & 0x00000200)
 #define bits23_25set(x)		(x & 0x000001c0)
@@ -34,6 +30,8 @@
 
 DEFINE_PER_CPU(struct exception_data, exception_data);
 
+int show_unhandled_signals = 1;
+
 /*
  * parisc_acctyp(unsigned int inst) --
  *    Given a PA-RISC memory access instruction, determine if the
@@ -173,6 +171,32 @@ int fixup_exception(struct pt_regs *regs)
 	return 0;
 }
 
+/*
+ * Print out info about fatal segfaults, if the show_unhandled_signals
+ * sysctl is set:
+ */
+static inline void
+show_signal_msg(struct pt_regs *regs, unsigned long code,
+		unsigned long address, struct task_struct *tsk,
+		struct vm_area_struct *vma)
+{
+	if (!unhandled_signal(tsk, SIGSEGV))
+		return;
+
+	if (!printk_ratelimit())
+		return;
+
+	pr_warn("\n");
+	pr_warn("do_page_fault() command='%s' type=%lu address=0x%08lx",
+	    tsk->comm, code, address);
+	print_vma_addr(KERN_CONT " in ", regs->iaoq[0]);
+	if (vma)
+		pr_warn(" vm_start = 0x%08lx, vm_end = 0x%08lx\n",
+				vma->vm_start, vma->vm_end);
+
+	show_regs(regs);
+}
+
 void do_page_fault(struct pt_regs *regs, unsigned long code,
 			      unsigned long address)
 {
@@ -270,16 +294,8 @@ bad_area:
 	if (user_mode(regs)) {
 		struct siginfo si;
 
-#ifdef PRINT_USER_FAULTS
-		printk(KERN_DEBUG "\n");
-		printk(KERN_DEBUG "do_page_fault() pid=%d command='%s' type=%lu address=0x%08lx\n",
-		    task_pid_nr(tsk), tsk->comm, code, address);
-		if (vma) {
-			printk(KERN_DEBUG "vm_start = 0x%08lx, vm_end = 0x%08lx\n",
-					vma->vm_start, vma->vm_end);
-		}
-		show_regs(regs);
-#endif
+		show_signal_msg(regs, code, address, tsk, vma);
+
 		switch (code) {
 		case 15:	/* Data TLB miss fault/Data page fault */
 			/* send SIGSEGV when outside of vma */
diff --git a/arch/powerpc/Makefile b/arch/powerpc/Makefile
index 4c0cedf4e2c..ce4c68a4a82 100644
--- a/arch/powerpc/Makefile
+++ b/arch/powerpc/Makefile
@@ -150,7 +150,9 @@ endif
 
 CFLAGS-$(CONFIG_TUNE_CELL) += $(call cc-option,-mtune=cell)
 
-KBUILD_CPPFLAGS	+= -Iarch/$(ARCH)
+asinstr := $(call as-instr,lis 9$(comma)foo@high,-DHAVE_AS_ATHIGH=1)
+
+KBUILD_CPPFLAGS	+= -Iarch/$(ARCH) $(asinstr)
 KBUILD_AFLAGS	+= -Iarch/$(ARCH)
 KBUILD_CFLAGS	+= -msoft-float -pipe -Iarch/$(ARCH) $(CFLAGS-y)
 CPP		= $(CC) -E $(KBUILD_CFLAGS)
diff --git a/arch/powerpc/boot/main.c b/arch/powerpc/boot/main.c
index a28f02165e9..d367a0aece2 100644
--- a/arch/powerpc/boot/main.c
+++ b/arch/powerpc/boot/main.c
@@ -139,18 +139,18 @@ static struct addr_range prep_initrd(struct addr_range vmlinux, void *chosen,
  * edit the command line passed to vmlinux (by setting /chosen/bootargs).
  * The buffer is put in it's own section so that tools may locate it easier.
  */
-static char cmdline[COMMAND_LINE_SIZE]
+static char cmdline[BOOT_COMMAND_LINE_SIZE]
 	__attribute__((__section__("__builtin_cmdline")));
 
 static void prep_cmdline(void *chosen)
 {
 	if (cmdline[0] == '\0')
-		getprop(chosen, "bootargs", cmdline, COMMAND_LINE_SIZE-1);
+		getprop(chosen, "bootargs", cmdline, BOOT_COMMAND_LINE_SIZE-1);
 
 	printf("\n\rLinux/PowerPC load: %s", cmdline);
 	/* If possible, edit the command line */
 	if (console_ops.edit_cmdline)
-		console_ops.edit_cmdline(cmdline, COMMAND_LINE_SIZE);
+		console_ops.edit_cmdline(cmdline, BOOT_COMMAND_LINE_SIZE);
 	printf("\n\r");
 
 	/* Put the command line back into the devtree for the kernel */
@@ -174,7 +174,7 @@ void start(void)
 	 * built-in command line wasn't set by an external tool */
 	if ((loader_info.cmdline_len > 0) && (cmdline[0] == '\0'))
 		memmove(cmdline, loader_info.cmdline,
-			min(loader_info.cmdline_len, COMMAND_LINE_SIZE-1));
+			min(loader_info.cmdline_len, BOOT_COMMAND_LINE_SIZE-1));
 
 	if (console_ops.open && (console_ops.open() < 0))
 		exit();
diff --git a/arch/powerpc/boot/ops.h b/arch/powerpc/boot/ops.h
index b3218ce451b..8aad3c55aed 100644
--- a/arch/powerpc/boot/ops.h
+++ b/arch/powerpc/boot/ops.h
@@ -15,7 +15,7 @@
 #include "types.h"
 #include "string.h"
 
-#define	COMMAND_LINE_SIZE	512
+#define	BOOT_COMMAND_LINE_SIZE	2048
 #define	MAX_PATH_LEN		256
 #define	MAX_PROP_LEN		256 /* What should this be? */
 
diff --git a/arch/powerpc/boot/ps3.c b/arch/powerpc/boot/ps3.c
index 9954d98871d..4ec2d86d3c5 100644
--- a/arch/powerpc/boot/ps3.c
+++ b/arch/powerpc/boot/ps3.c
@@ -47,13 +47,13 @@ BSS_STACK(4096);
  * The buffer is put in it's own section so that tools may locate it easier.
  */
 
-static char cmdline[COMMAND_LINE_SIZE]
+static char cmdline[BOOT_COMMAND_LINE_SIZE]
 	__attribute__((__section__("__builtin_cmdline")));
 
 static void prep_cmdline(void *chosen)
 {
 	if (cmdline[0] == '\0')
-		getprop(chosen, "bootargs", cmdline, COMMAND_LINE_SIZE-1);
+		getprop(chosen, "bootargs", cmdline, BOOT_COMMAND_LINE_SIZE-1);
 	else
 		setprop_str(chosen, "bootargs", cmdline);
 
diff --git a/arch/powerpc/configs/40x/ep405_defconfig b/arch/powerpc/configs/40x/ep405_defconfig
index cf06d42f2c0..e9d84b5d0ab 100644
--- a/arch/powerpc/configs/40x/ep405_defconfig
+++ b/arch/powerpc/configs/40x/ep405_defconfig
@@ -57,7 +57,6 @@ CONFIG_SERIAL_OF_PLATFORM=y
 CONFIG_THERMAL=y
 CONFIG_VIDEO_OUTPUT_CONTROL=m
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=y
 CONFIG_USB_OHCI_HCD=y
 CONFIG_USB_OHCI_HCD_PPC_OF_BE=y
diff --git a/arch/powerpc/configs/44x/canyonlands_defconfig b/arch/powerpc/configs/44x/canyonlands_defconfig
index 7b8abd1b88b..9919a91add1 100644
--- a/arch/powerpc/configs/44x/canyonlands_defconfig
+++ b/arch/powerpc/configs/44x/canyonlands_defconfig
@@ -71,7 +71,6 @@ CONFIG_I2C_IBM_IIC=y
 CONFIG_SENSORS_AD7414=y
 CONFIG_USB=y
 CONFIG_USB_ANNOUNCE_NEW_DEVICES=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=y
 CONFIG_USB_EHCI_HCD=m
 CONFIG_USB_OHCI_HCD=y
diff --git a/arch/powerpc/configs/44x/currituck_defconfig b/arch/powerpc/configs/44x/currituck_defconfig
index 4192322f8a7..47de6826144 100644
--- a/arch/powerpc/configs/44x/currituck_defconfig
+++ b/arch/powerpc/configs/44x/currituck_defconfig
@@ -71,7 +71,6 @@ CONFIG_I2C_IBM_IIC=y
 # CONFIG_HWMON is not set
 CONFIG_THERMAL=y
 CONFIG_USB=y
-CONFIG_USB_DEBUG=y
 CONFIG_USB_EHCI_HCD=y
 CONFIG_USB_OHCI_HCD=y
 CONFIG_RTC_CLASS=y
diff --git a/arch/powerpc/configs/44x/sam440ep_defconfig b/arch/powerpc/configs/44x/sam440ep_defconfig
index ca088cd581a..9622eb2a3e3 100644
--- a/arch/powerpc/configs/44x/sam440ep_defconfig
+++ b/arch/powerpc/configs/44x/sam440ep_defconfig
@@ -83,7 +83,6 @@ CONFIG_HID_SAMSUNG=y
 CONFIG_HID_SONY=y
 CONFIG_HID_SUNPLUS=y
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_EHCI_HCD=m
 CONFIG_USB_OHCI_HCD=y
diff --git a/arch/powerpc/configs/52xx/cm5200_defconfig b/arch/powerpc/configs/52xx/cm5200_defconfig
index 4f84a0b2fbf..0dc99e14103 100644
--- a/arch/powerpc/configs/52xx/cm5200_defconfig
+++ b/arch/powerpc/configs/52xx/cm5200_defconfig
@@ -64,7 +64,6 @@ CONFIG_I2C_MPC=y
 # CONFIG_HWMON is not set
 CONFIG_WATCHDOG=y
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_OHCI_HCD=y
 CONFIG_USB_OHCI_HCD_PPC_OF_BE=y
diff --git a/arch/powerpc/configs/52xx/pcm030_defconfig b/arch/powerpc/configs/52xx/pcm030_defconfig
index 2401e255432..1d03c35540c 100644
--- a/arch/powerpc/configs/52xx/pcm030_defconfig
+++ b/arch/powerpc/configs/52xx/pcm030_defconfig
@@ -76,7 +76,6 @@ CONFIG_I2C_CHARDEV=y
 CONFIG_I2C_MPC=y
 # CONFIG_HWMON is not set
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_OHCI_HCD=m
 # CONFIG_USB_OHCI_HCD_PPC_SOC is not set
diff --git a/arch/powerpc/configs/52xx/tqm5200_defconfig b/arch/powerpc/configs/52xx/tqm5200_defconfig
index 21c841e0f48..ca83ec88b11 100644
--- a/arch/powerpc/configs/52xx/tqm5200_defconfig
+++ b/arch/powerpc/configs/52xx/tqm5200_defconfig
@@ -75,7 +75,6 @@ CONFIG_FB_FOREIGN_ENDIAN=y
 CONFIG_FB_SM501=y
 CONFIG_FRAMEBUFFER_CONSOLE=y
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_MON=y
 CONFIG_USB_OHCI_HCD=y
diff --git a/arch/powerpc/configs/83xx/mpc8313_rdb_defconfig b/arch/powerpc/configs/83xx/mpc8313_rdb_defconfig
index 0b73b7f9d11..4b4a2a9133a 100644
--- a/arch/powerpc/configs/83xx/mpc8313_rdb_defconfig
+++ b/arch/powerpc/configs/83xx/mpc8313_rdb_defconfig
@@ -74,7 +74,6 @@ CONFIG_WATCHDOG=y
 CONFIG_VIDEO_OUTPUT_CONTROL=m
 # CONFIG_USB_HID is not set
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=y
 CONFIG_USB_EHCI_HCD=y
 CONFIG_USB_EHCI_FSL=y
diff --git a/arch/powerpc/configs/83xx/mpc8315_rdb_defconfig b/arch/powerpc/configs/83xx/mpc8315_rdb_defconfig
index 97ac3b993cb..5871395573c 100644
--- a/arch/powerpc/configs/83xx/mpc8315_rdb_defconfig
+++ b/arch/powerpc/configs/83xx/mpc8315_rdb_defconfig
@@ -71,7 +71,6 @@ CONFIG_WATCHDOG=y
 CONFIG_VIDEO_OUTPUT_CONTROL=m
 # CONFIG_USB_HID is not set
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=y
 CONFIG_USB_EHCI_HCD=y
 CONFIG_USB_EHCI_FSL=y
diff --git a/arch/powerpc/configs/83xx/mpc832x_rdb_defconfig b/arch/powerpc/configs/83xx/mpc832x_rdb_defconfig
index b4da1a7e644..5adc4cea42d 100644
--- a/arch/powerpc/configs/83xx/mpc832x_rdb_defconfig
+++ b/arch/powerpc/configs/83xx/mpc832x_rdb_defconfig
@@ -61,7 +61,6 @@ CONFIG_WATCHDOG=y
 CONFIG_VIDEO_OUTPUT_CONTROL=m
 # CONFIG_USB_HID is not set
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=y
 CONFIG_USB_EHCI_HCD=y
 CONFIG_USB_OHCI_HCD=y
diff --git a/arch/powerpc/configs/83xx/mpc834x_itx_defconfig b/arch/powerpc/configs/83xx/mpc834x_itx_defconfig
index 291f8221d5a..82b6b6c88d6 100644
--- a/arch/powerpc/configs/83xx/mpc834x_itx_defconfig
+++ b/arch/powerpc/configs/83xx/mpc834x_itx_defconfig
@@ -71,7 +71,6 @@ CONFIG_SPI_BITBANG=y
 CONFIG_WATCHDOG=y
 CONFIG_VIDEO_OUTPUT_CONTROL=m
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=y
 CONFIG_USB_EHCI_HCD=y
 CONFIG_USB_EHCI_FSL=y
diff --git a/arch/powerpc/configs/83xx/sbc834x_defconfig b/arch/powerpc/configs/83xx/sbc834x_defconfig
index a3bcda67d2d..4ae385894c6 100644
--- a/arch/powerpc/configs/83xx/sbc834x_defconfig
+++ b/arch/powerpc/configs/83xx/sbc834x_defconfig
@@ -70,7 +70,6 @@ CONFIG_I2C_MPC=y
 CONFIG_WATCHDOG=y
 # CONFIG_USB_HID is not set
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=y
 CONFIG_USB_EHCI_HCD=y
 CONFIG_USB_EHCI_FSL=y
diff --git a/arch/powerpc/configs/85xx/ge_imp3a_defconfig b/arch/powerpc/configs/85xx/ge_imp3a_defconfig
index c9765b54dd1..dc939de9b5b 100644
--- a/arch/powerpc/configs/85xx/ge_imp3a_defconfig
+++ b/arch/powerpc/configs/85xx/ge_imp3a_defconfig
@@ -158,7 +158,6 @@ CONFIG_HID_TOPSEED=y
 CONFIG_HID_THRUSTMASTER=y
 CONFIG_HID_ZEROPLUS=y
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_EHCI_HCD=y
 # CONFIG_USB_EHCI_TT_NEWSCHED is not set
 CONFIG_USB_EHCI_FSL=y
diff --git a/arch/powerpc/configs/85xx/socrates_defconfig b/arch/powerpc/configs/85xx/socrates_defconfig
index e5147488c00..435fd408eef 100644
--- a/arch/powerpc/configs/85xx/socrates_defconfig
+++ b/arch/powerpc/configs/85xx/socrates_defconfig
@@ -86,7 +86,6 @@ CONFIG_FONTS=y
 CONFIG_FONT_8x16=y
 CONFIG_USB=y
 CONFIG_USB_ANNOUNCE_NEW_DEVICES=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=y
 CONFIG_USB_EHCI_HCD=y
 CONFIG_USB_OHCI_HCD=y
diff --git a/arch/powerpc/configs/85xx/xes_mpc85xx_defconfig b/arch/powerpc/configs/85xx/xes_mpc85xx_defconfig
index 07bb81df27e..72df8ab8449 100644
--- a/arch/powerpc/configs/85xx/xes_mpc85xx_defconfig
+++ b/arch/powerpc/configs/85xx/xes_mpc85xx_defconfig
@@ -125,7 +125,6 @@ CONFIG_SENSORS_LM90=y
 CONFIG_WATCHDOG=y
 CONFIG_VIDEO_OUTPUT_CONTROL=y
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_MON=y
 CONFIG_USB_ISP1760_HCD=y
diff --git a/arch/powerpc/configs/86xx/mpc8641_hpcn_defconfig b/arch/powerpc/configs/86xx/mpc8641_hpcn_defconfig
index f51c7ebc181..76f43df3dec 100644
--- a/arch/powerpc/configs/86xx/mpc8641_hpcn_defconfig
+++ b/arch/powerpc/configs/86xx/mpc8641_hpcn_defconfig
@@ -123,7 +123,6 @@ CONFIG_HID_SAMSUNG=y
 CONFIG_HID_SONY=y
 CONFIG_HID_SUNPLUS=y
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=y
 CONFIG_USB_EHCI_HCD=y
 CONFIG_USB_OHCI_HCD=y
diff --git a/arch/powerpc/configs/amigaone_defconfig b/arch/powerpc/configs/amigaone_defconfig
index b6d49da9c82..8c66b13e59f 100644
--- a/arch/powerpc/configs/amigaone_defconfig
+++ b/arch/powerpc/configs/amigaone_defconfig
@@ -108,7 +108,6 @@ CONFIG_HID_SONY=y
 CONFIG_HID_SUNPLUS=y
 CONFIG_HID_TOPSEED=y
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=y
 CONFIG_USB_OHCI_HCD=y
 CONFIG_USB_UHCI_HCD=y
diff --git a/arch/powerpc/configs/c2k_defconfig b/arch/powerpc/configs/c2k_defconfig
index c69f6162090..5e2aa43562b 100644
--- a/arch/powerpc/configs/c2k_defconfig
+++ b/arch/powerpc/configs/c2k_defconfig
@@ -261,7 +261,6 @@ CONFIG_USBPCWATCHDOG=m
 # CONFIG_VGA_CONSOLE is not set
 # CONFIG_HID_SUPPORT is not set
 CONFIG_USB=m
-CONFIG_USB_DEVICEFS=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_MON=m
 CONFIG_USB_EHCI_HCD=m
diff --git a/arch/powerpc/configs/cell_defconfig b/arch/powerpc/configs/cell_defconfig
index 22a403d78d3..4bee1a6d41d 100644
--- a/arch/powerpc/configs/cell_defconfig
+++ b/arch/powerpc/configs/cell_defconfig
@@ -179,7 +179,6 @@ CONFIG_VIDEO_OUTPUT_CONTROL=m
 CONFIG_HID=m
 # CONFIG_USB_HID is not set
 CONFIG_USB=m
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=m
 CONFIG_USB_EHCI_HCD=m
 # CONFIG_USB_EHCI_HCD_PPC_OF is not set
diff --git a/arch/powerpc/configs/celleb_defconfig b/arch/powerpc/configs/celleb_defconfig
index 895449ed971..6d7b22f41b5 100644
--- a/arch/powerpc/configs/celleb_defconfig
+++ b/arch/powerpc/configs/celleb_defconfig
@@ -87,7 +87,6 @@ CONFIG_WATCHDOG=y
 # CONFIG_VGA_CONSOLE is not set
 CONFIG_USB_HIDDEV=y
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_MON=y
 CONFIG_USB_EHCI_HCD=m
diff --git a/arch/powerpc/configs/chrp32_defconfig b/arch/powerpc/configs/chrp32_defconfig
index b20554efddc..db5b30857e1 100644
--- a/arch/powerpc/configs/chrp32_defconfig
+++ b/arch/powerpc/configs/chrp32_defconfig
@@ -111,7 +111,6 @@ CONFIG_HID_SAMSUNG=y
 CONFIG_HID_SONY=y
 CONFIG_HID_SUNPLUS=y
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=y
 CONFIG_USB_EHCI_HCD=m
 # CONFIG_USB_EHCI_HCD_PPC_OF is not set
diff --git a/arch/powerpc/configs/g5_defconfig b/arch/powerpc/configs/g5_defconfig
index 1ea22fc24ea..3c72fa615bd 100644
--- a/arch/powerpc/configs/g5_defconfig
+++ b/arch/powerpc/configs/g5_defconfig
@@ -175,7 +175,6 @@ CONFIG_HID_SAMSUNG=y
 CONFIG_HID_SONY=y
 CONFIG_HID_SUNPLUS=y
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=y
 CONFIG_USB_EHCI_HCD=y
 # CONFIG_USB_EHCI_HCD_PPC_OF is not set
diff --git a/arch/powerpc/configs/linkstation_defconfig b/arch/powerpc/configs/linkstation_defconfig
index 353435256f4..b5e684640fd 100644
--- a/arch/powerpc/configs/linkstation_defconfig
+++ b/arch/powerpc/configs/linkstation_defconfig
@@ -111,7 +111,6 @@ CONFIG_VIDEO_OUTPUT_CONTROL=m
 CONFIG_HID=m
 # CONFIG_USB_HID is not set
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=y
 CONFIG_USB_EHCI_HCD=y
 CONFIG_USB_OHCI_HCD=y
diff --git a/arch/powerpc/configs/maple_defconfig b/arch/powerpc/configs/maple_defconfig
index 2a5afac2986..95e545d9f25 100644
--- a/arch/powerpc/configs/maple_defconfig
+++ b/arch/powerpc/configs/maple_defconfig
@@ -79,7 +79,6 @@ CONFIG_HID_SAMSUNG=y
 CONFIG_HID_SONY=y
 CONFIG_HID_SUNPLUS=y
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=y
 CONFIG_USB_EHCI_HCD=y
 CONFIG_USB_EHCI_ROOT_HUB_TT=y
diff --git a/arch/powerpc/configs/mpc5200_defconfig b/arch/powerpc/configs/mpc5200_defconfig
index 8b682d1cf4d..530601e8ccf 100644
--- a/arch/powerpc/configs/mpc5200_defconfig
+++ b/arch/powerpc/configs/mpc5200_defconfig
@@ -113,7 +113,6 @@ CONFIG_HID_TOPSEED=y
 CONFIG_HID_THRUSTMASTER=y
 CONFIG_HID_ZEROPLUS=y
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_MON=y
 CONFIG_USB_OHCI_HCD=y
diff --git a/arch/powerpc/configs/mpc86xx_defconfig b/arch/powerpc/configs/mpc86xx_defconfig
index a1cc8179e9f..35595ea74ff 100644
--- a/arch/powerpc/configs/mpc86xx_defconfig
+++ b/arch/powerpc/configs/mpc86xx_defconfig
@@ -126,7 +126,6 @@ CONFIG_HID_SAMSUNG=y
 CONFIG_HID_SONY=y
 CONFIG_HID_SUNPLUS=y
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=y
 CONFIG_USB_EHCI_HCD=y
 CONFIG_USB_OHCI_HCD=y
diff --git a/arch/powerpc/configs/pmac32_defconfig b/arch/powerpc/configs/pmac32_defconfig
index a73626b0905..553e6627801 100644
--- a/arch/powerpc/configs/pmac32_defconfig
+++ b/arch/powerpc/configs/pmac32_defconfig
@@ -279,7 +279,6 @@ CONFIG_HID_SAMSUNG=y
 CONFIG_HID_SONY=y
 CONFIG_HID_SUNPLUS=y
 CONFIG_HID_TOPSEED=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_DYNAMIC_MINORS=y
 CONFIG_USB_MON=y
 CONFIG_USB_EHCI_HCD=m
diff --git a/arch/powerpc/configs/ppc6xx_defconfig b/arch/powerpc/configs/ppc6xx_defconfig
index 175a8b99c19..c9106694484 100644
--- a/arch/powerpc/configs/ppc6xx_defconfig
+++ b/arch/powerpc/configs/ppc6xx_defconfig
@@ -964,9 +964,7 @@ CONFIG_HID_SAMSUNG=y
 CONFIG_HID_SONY=y
 CONFIG_HID_SUNPLUS=y
 CONFIG_USB=y
-CONFIG_USB_DEBUG=y
 CONFIG_USB_ANNOUNCE_NEW_DEVICES=y
-CONFIG_USB_DEVICEFS=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_MON=y
 CONFIG_USB_EHCI_HCD=m
diff --git a/arch/powerpc/configs/storcenter_defconfig b/arch/powerpc/configs/storcenter_defconfig
index ba39c785445..60ad2c08caa 100644
--- a/arch/powerpc/configs/storcenter_defconfig
+++ b/arch/powerpc/configs/storcenter_defconfig
@@ -72,7 +72,6 @@ CONFIG_I2C_CHARDEV=y
 CONFIG_I2C_MPC=y
 # CONFIG_HWMON is not set
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_EHCI_HCD=y
 CONFIG_USB_OHCI_HCD=y
 CONFIG_USB_STORAGE=y
diff --git a/arch/powerpc/include/asm/opal.h b/arch/powerpc/include/asm/opal.h
index a2efdaa020b..66ad7a74116 100644
--- a/arch/powerpc/include/asm/opal.h
+++ b/arch/powerpc/include/asm/opal.h
@@ -41,14 +41,14 @@ struct opal_takeover_args {
  * size except the last one in the list to be as well.
  */
 struct opal_sg_entry {
-	void    *data;
-	long    length;
+	__be64 data;
+	__be64 length;
 };
 
-/* sg list */
+/* SG list */
 struct opal_sg_list {
-	unsigned long num_entries;
-	struct opal_sg_list *next;
+	__be64 length;
+	__be64 next;
 	struct opal_sg_entry entry[];
 };
 
@@ -858,8 +858,8 @@ int64_t opal_lpc_write(uint32_t chip_id, enum OpalLPCAddressType addr_type,
 int64_t opal_lpc_read(uint32_t chip_id, enum OpalLPCAddressType addr_type,
 		      uint32_t addr, __be32 *data, uint32_t sz);
 
-int64_t opal_read_elog(uint64_t buffer, size_t size, uint64_t log_id);
-int64_t opal_get_elog_size(uint64_t *log_id, size_t *size, uint64_t *elog_type);
+int64_t opal_read_elog(uint64_t buffer, uint64_t size, uint64_t log_id);
+int64_t opal_get_elog_size(__be64 *log_id, __be64 *size, __be64 *elog_type);
 int64_t opal_write_elog(uint64_t buffer, uint64_t size, uint64_t offset);
 int64_t opal_send_ack_elog(uint64_t log_id);
 void opal_resend_pending_logs(void);
@@ -868,23 +868,24 @@ int64_t opal_validate_flash(uint64_t buffer, uint32_t *size, uint32_t *result);
 int64_t opal_manage_flash(uint8_t op);
 int64_t opal_update_flash(uint64_t blk_list);
 int64_t opal_dump_init(uint8_t dump_type);
-int64_t opal_dump_info(uint32_t *dump_id, uint32_t *dump_size);
-int64_t opal_dump_info2(uint32_t *dump_id, uint32_t *dump_size, uint32_t *dump_type);
+int64_t opal_dump_info(__be32 *dump_id, __be32 *dump_size);
+int64_t opal_dump_info2(__be32 *dump_id, __be32 *dump_size, __be32 *dump_type);
 int64_t opal_dump_read(uint32_t dump_id, uint64_t buffer);
 int64_t opal_dump_ack(uint32_t dump_id);
 int64_t opal_dump_resend_notification(void);
 
-int64_t opal_get_msg(uint64_t buffer, size_t size);
-int64_t opal_check_completion(uint64_t buffer, size_t size, uint64_t token);
+int64_t opal_get_msg(uint64_t buffer, uint64_t size);
+int64_t opal_check_completion(uint64_t buffer, uint64_t size, uint64_t token);
 int64_t opal_sync_host_reboot(void);
 int64_t opal_get_param(uint64_t token, uint32_t param_id, uint64_t buffer,
-		size_t length);
+		uint64_t length);
 int64_t opal_set_param(uint64_t token, uint32_t param_id, uint64_t buffer,
-		size_t length);
+		uint64_t length);
 int64_t opal_sensor_read(uint32_t sensor_hndl, int token, __be32 *sensor_data);
 
 /* Internal functions */
-extern int early_init_dt_scan_opal(unsigned long node, const char *uname, int depth, void *data);
+extern int early_init_dt_scan_opal(unsigned long node, const char *uname,
+				   int depth, void *data);
 extern int early_init_dt_scan_recoverable_ranges(unsigned long node,
 				 const char *uname, int depth, void *data);
 
@@ -893,10 +894,6 @@ extern int opal_put_chars(uint32_t vtermno, const char *buf, int total_len);
 
 extern void hvc_opal_init_early(void);
 
-/* Internal functions */
-extern int early_init_dt_scan_opal(unsigned long node, const char *uname,
-				   int depth, void *data);
-
 extern int opal_notifier_register(struct notifier_block *nb);
 extern int opal_notifier_unregister(struct notifier_block *nb);
 
@@ -906,9 +903,6 @@ extern void opal_notifier_enable(void);
 extern void opal_notifier_disable(void);
 extern void opal_notifier_update_evt(uint64_t evt_mask, uint64_t evt_val);
 
-extern int opal_get_chars(uint32_t vtermno, char *buf, int count);
-extern int opal_put_chars(uint32_t vtermno, const char *buf, int total_len);
-
 extern int __opal_async_get_token(void);
 extern int opal_async_get_token_interruptible(void);
 extern int __opal_async_release_token(int token);
@@ -916,8 +910,6 @@ extern int opal_async_release_token(int token);
 extern int opal_async_wait_response(uint64_t token, struct opal_msg *msg);
 extern int opal_get_sensor_data(u32 sensor_hndl, u32 *sensor_data);
 
-extern void hvc_opal_init_early(void);
-
 struct rtc_time;
 extern int opal_set_rtc_time(struct rtc_time *tm);
 extern void opal_get_rtc_time(struct rtc_time *tm);
@@ -937,6 +929,10 @@ extern int opal_resync_timebase(void);
 
 extern void opal_lpc_init(void);
 
+struct opal_sg_list *opal_vmalloc_to_sg_list(void *vmalloc_addr,
+					     unsigned long vmalloc_size);
+void opal_free_sg_list(struct opal_sg_list *sg);
+
 #endif /* __ASSEMBLY__ */
 
 #endif /* __OPAL_H */
diff --git a/arch/powerpc/include/asm/pci.h b/arch/powerpc/include/asm/pci.h
index 95145a15c70..1b0739bc14b 100644
--- a/arch/powerpc/include/asm/pci.h
+++ b/arch/powerpc/include/asm/pci.h
@@ -46,11 +46,6 @@ struct pci_dev;
 #define pcibios_assign_all_busses() \
 	(pci_has_flag(PCI_REASSIGN_ALL_BUS))
 
-static inline void pcibios_penalize_isa_irq(int irq, int active)
-{
-	/* We don't do dynamic PCI IRQ allocation */
-}
-
 #define HAVE_ARCH_PCI_GET_LEGACY_IDE_IRQ
 static inline int pci_get_legacy_ide_irq(struct pci_dev *dev, int channel)
 {
diff --git a/arch/powerpc/include/asm/ppc_asm.h b/arch/powerpc/include/asm/ppc_asm.h
index 6586a40a46c..cded7c1278e 100644
--- a/arch/powerpc/include/asm/ppc_asm.h
+++ b/arch/powerpc/include/asm/ppc_asm.h
@@ -318,11 +318,16 @@ n:
 	addi	reg,reg,(name - 0b)@l;
 
 #ifdef __powerpc64__
+#ifdef HAVE_AS_ATHIGH
+#define __AS_ATHIGH high
+#else
+#define __AS_ATHIGH h
+#endif
 #define LOAD_REG_IMMEDIATE(reg,expr)		\
 	lis     reg,(expr)@highest;		\
 	ori     reg,reg,(expr)@higher;	\
 	rldicr  reg,reg,32,31;		\
-	oris    reg,reg,(expr)@h;		\
+	oris    reg,reg,(expr)@__AS_ATHIGH;	\
 	ori     reg,reg,(expr)@l;
 
 #define LOAD_REG_ADDR(reg,name)			\
diff --git a/arch/powerpc/include/asm/sections.h b/arch/powerpc/include/asm/sections.h
index d0e784e0ff4..52179033067 100644
--- a/arch/powerpc/include/asm/sections.h
+++ b/arch/powerpc/include/asm/sections.h
@@ -39,6 +39,17 @@ static inline int overlaps_kernel_text(unsigned long start, unsigned long end)
 		(unsigned long)_stext < end;
 }
 
+static inline int overlaps_kvm_tmp(unsigned long start, unsigned long end)
+{
+#ifdef CONFIG_KVM_GUEST
+	extern char kvm_tmp[];
+	return start < (unsigned long)kvm_tmp &&
+		(unsigned long)&kvm_tmp[1024 * 1024] < end;
+#else
+	return 0;
+#endif
+}
+
 #undef dereference_function_descriptor
 static inline void *dereference_function_descriptor(void *ptr)
 {
diff --git a/arch/powerpc/include/asm/systbl.h b/arch/powerpc/include/asm/systbl.h
index 3ddf7027670..ea4dc3a89c1 100644
--- a/arch/powerpc/include/asm/systbl.h
+++ b/arch/powerpc/include/asm/systbl.h
@@ -361,3 +361,4 @@ SYSCALL(finit_module)
 SYSCALL(ni_syscall) /* sys_kcmp */
 SYSCALL_SPU(sched_setattr)
 SYSCALL_SPU(sched_getattr)
+SYSCALL_SPU(renameat2)
diff --git a/arch/powerpc/include/asm/unistd.h b/arch/powerpc/include/asm/unistd.h
index 4494f029b63..9b892bbd9d8 100644
--- a/arch/powerpc/include/asm/unistd.h
+++ b/arch/powerpc/include/asm/unistd.h
@@ -12,7 +12,7 @@
 #include <uapi/asm/unistd.h>
 
 
-#define __NR_syscalls		357
+#define __NR_syscalls		358
 
 #define __NR__exit __NR_exit
 #define NR_syscalls	__NR_syscalls
diff --git a/arch/powerpc/include/uapi/asm/setup.h b/arch/powerpc/include/uapi/asm/setup.h
index 552df83f1a4..ae3fb68cb28 100644
--- a/arch/powerpc/include/uapi/asm/setup.h
+++ b/arch/powerpc/include/uapi/asm/setup.h
@@ -1 +1,6 @@
-#include <asm-generic/setup.h>
+#ifndef _UAPI_ASM_POWERPC_SETUP_H
+#define _UAPI_ASM_POWERPC_SETUP_H
+
+#define COMMAND_LINE_SIZE	2048
+
+#endif /* _UAPI_ASM_POWERPC_SETUP_H */
diff --git a/arch/powerpc/include/uapi/asm/unistd.h b/arch/powerpc/include/uapi/asm/unistd.h
index 881bf2e2560..2d526f7b48d 100644
--- a/arch/powerpc/include/uapi/asm/unistd.h
+++ b/arch/powerpc/include/uapi/asm/unistd.h
@@ -379,5 +379,6 @@
 #define __NR_kcmp		354
 #define __NR_sched_setattr	355
 #define __NR_sched_getattr	356
+#define __NR_renameat2		357
 
 #endif /* _UAPI_ASM_POWERPC_UNISTD_H_ */
diff --git a/arch/powerpc/kernel/kvm.c b/arch/powerpc/kernel/kvm.c
index 6a0175297b0..dd8695f6cb6 100644
--- a/arch/powerpc/kernel/kvm.c
+++ b/arch/powerpc/kernel/kvm.c
@@ -74,7 +74,7 @@
 #define KVM_INST_MTSRIN		0x7c0001e4
 
 static bool kvm_patching_worked = true;
-static char kvm_tmp[1024 * 1024];
+char kvm_tmp[1024 * 1024];
 static int kvm_tmp_index;
 
 static inline void kvm_patch_ins(u32 *inst, u32 new_inst)
diff --git a/arch/powerpc/kernel/machine_kexec_64.c b/arch/powerpc/kernel/machine_kexec_64.c
index 59d229a2a3e..879b3aacac3 100644
--- a/arch/powerpc/kernel/machine_kexec_64.c
+++ b/arch/powerpc/kernel/machine_kexec_64.c
@@ -237,7 +237,7 @@ static void wake_offline_cpus(void)
 		if (!cpu_online(cpu)) {
 			printk(KERN_INFO "kexec: Waking offline cpu %d.\n",
 			       cpu);
-			cpu_up(cpu);
+			WARN_ON(cpu_up(cpu));
 		}
 	}
 }
diff --git a/arch/powerpc/kernel/pci-common.c b/arch/powerpc/kernel/pci-common.c
index d9476c1fc95..24d342e9179 100644
--- a/arch/powerpc/kernel/pci-common.c
+++ b/arch/powerpc/kernel/pci-common.c
@@ -201,26 +201,6 @@ struct pci_controller* pci_find_hose_for_OF_device(struct device_node* node)
 	return NULL;
 }
 
-static ssize_t pci_show_devspec(struct device *dev,
-		struct device_attribute *attr, char *buf)
-{
-	struct pci_dev *pdev;
-	struct device_node *np;
-
-	pdev = to_pci_dev (dev);
-	np = pci_device_to_OF_node(pdev);
-	if (np == NULL || np->full_name == NULL)
-		return 0;
-	return sprintf(buf, "%s", np->full_name);
-}
-static DEVICE_ATTR(devspec, S_IRUGO, pci_show_devspec, NULL);
-
-/* Add sysfs properties */
-int pcibios_add_platform_entries(struct pci_dev *pdev)
-{
-	return device_create_file(&pdev->dev, &dev_attr_devspec);
-}
-
 /*
  * Reads the interrupt pin to determine if interrupt is use by card.
  * If the interrupt is used, then gets the interrupt line from the
diff --git a/arch/powerpc/kernel/pci-hotplug.c b/arch/powerpc/kernel/pci-hotplug.c
index c1e17ae68a0..5b789177aa2 100644
--- a/arch/powerpc/kernel/pci-hotplug.c
+++ b/arch/powerpc/kernel/pci-hotplug.c
@@ -98,8 +98,7 @@ void pcibios_add_pci_devices(struct pci_bus * bus)
 		max = bus->busn_res.start;
 		for (pass = 0; pass < 2; pass++) {
 			list_for_each_entry(dev, &bus->devices, bus_list) {
-				if (dev->hdr_type == PCI_HEADER_TYPE_BRIDGE ||
-				    dev->hdr_type == PCI_HEADER_TYPE_CARDBUS)
+				if (pci_is_bridge(dev))
 					max = pci_scan_bridge(bus, dev,
 							      max, pass);
 			}
diff --git a/arch/powerpc/kernel/pci_64.c b/arch/powerpc/kernel/pci_64.c
index 2a4779091a5..155013da27e 100644
--- a/arch/powerpc/kernel/pci_64.c
+++ b/arch/powerpc/kernel/pci_64.c
@@ -208,7 +208,7 @@ long sys_pciconfig_iobase(long which, unsigned long in_bus,
 			  unsigned long in_devfn)
 {
 	struct pci_controller* hose;
-	struct pci_bus *bus = NULL;
+	struct pci_bus *tmp_bus, *bus = NULL;
 	struct device_node *hose_node;
 
 	/* Argh ! Please forgive me for that hack, but that's the
@@ -229,10 +229,12 @@ long sys_pciconfig_iobase(long which, unsigned long in_bus,
 	 * used on pre-domains setup. We return the first match
 	 */
 
-	list_for_each_entry(bus, &pci_root_buses, node) {
-		if (in_bus >= bus->number && in_bus <= bus->busn_res.end)
+	list_for_each_entry(tmp_bus, &pci_root_buses, node) {
+		if (in_bus >= tmp_bus->number &&
+		    in_bus <= tmp_bus->busn_res.end) {
+			bus = tmp_bus;
 			break;
-		bus = NULL;
+		}
 	}
 	if (bus == NULL || bus->dev.of_node == NULL)
 		return -ENODEV;
diff --git a/arch/powerpc/kernel/pci_of_scan.c b/arch/powerpc/kernel/pci_of_scan.c
index 83c26d82999..059e244484f 100644
--- a/arch/powerpc/kernel/pci_of_scan.c
+++ b/arch/powerpc/kernel/pci_of_scan.c
@@ -362,8 +362,7 @@ static void __of_scan_bus(struct device_node *node, struct pci_bus *bus,
 
 	/* Now scan child busses */
 	list_for_each_entry(dev, &bus->devices, bus_list) {
-		if (dev->hdr_type == PCI_HEADER_TYPE_BRIDGE ||
-		    dev->hdr_type == PCI_HEADER_TYPE_CARDBUS) {
+		if (pci_is_bridge(dev)) {
 			of_scan_pci_bridge(dev);
 		}
 	}
diff --git a/arch/powerpc/kernel/ppc_ksyms.c b/arch/powerpc/kernel/ppc_ksyms.c
index 3bd77edd761..450850a49dc 100644
--- a/arch/powerpc/kernel/ppc_ksyms.c
+++ b/arch/powerpc/kernel/ppc_ksyms.c
@@ -120,6 +120,7 @@ EXPORT_SYMBOL(giveup_spe);
 EXPORT_SYMBOL(flush_instruction_cache);
 #endif
 EXPORT_SYMBOL(flush_dcache_range);
+EXPORT_SYMBOL(flush_icache_range);
 
 #ifdef CONFIG_SMP
 #ifdef CONFIG_PPC32
diff --git a/arch/powerpc/kernel/rtas_flash.c b/arch/powerpc/kernel/rtas_flash.c
index 2f3cdb01506..658e89d2025 100644
--- a/arch/powerpc/kernel/rtas_flash.c
+++ b/arch/powerpc/kernel/rtas_flash.c
@@ -705,7 +705,7 @@ static int __init rtas_flash_init(void)
 	if (rtas_token("ibm,update-flash-64-and-reboot") ==
 		       RTAS_UNKNOWN_SERVICE) {
 		pr_info("rtas_flash: no firmware flash support\n");
-		return 1;
+		return -EINVAL;
 	}
 
 	rtas_validate_flash_data.buf = kzalloc(VALIDATE_BUF_SIZE, GFP_KERNEL);
diff --git a/arch/powerpc/kernel/time.c b/arch/powerpc/kernel/time.c
index 122a580f732..7e711bdcc6d 100644
--- a/arch/powerpc/kernel/time.c
+++ b/arch/powerpc/kernel/time.c
@@ -813,9 +813,6 @@ static void __init clocksource_init(void)
 static int decrementer_set_next_event(unsigned long evt,
 				      struct clock_event_device *dev)
 {
-	/* Don't adjust the decrementer if some irq work is pending */
-	if (test_irq_work_pending())
-		return 0;
 	__get_cpu_var(decrementers_next_tb) = get_tb_or_rtc() + evt;
 	set_dec(evt);
 
diff --git a/arch/powerpc/kvm/book3s.c b/arch/powerpc/kvm/book3s.c
index 94e597e6f15..7af190a266b 100644
--- a/arch/powerpc/kvm/book3s.c
+++ b/arch/powerpc/kvm/book3s.c
@@ -886,7 +886,7 @@ static int kvmppc_book3s_init(void)
 	r = kvm_init(NULL, sizeof(struct kvm_vcpu), 0, THIS_MODULE);
 	if (r)
 		return r;
-#ifdef CONFIG_KVM_BOOK3S_32
+#ifdef CONFIG_KVM_BOOK3S_32_HANDLER
 	r = kvmppc_book3s_init_pr();
 #endif
 	return r;
@@ -895,7 +895,7 @@ static int kvmppc_book3s_init(void)
 
 static void kvmppc_book3s_exit(void)
 {
-#ifdef CONFIG_KVM_BOOK3S_32
+#ifdef CONFIG_KVM_BOOK3S_32_HANDLER
 	kvmppc_book3s_exit_pr();
 #endif
 	kvm_exit();
@@ -905,7 +905,7 @@ module_init(kvmppc_book3s_init);
 module_exit(kvmppc_book3s_exit);
 
 /* On 32bit this is our one and only kernel module */
-#ifdef CONFIG_KVM_BOOK3S_32
+#ifdef CONFIG_KVM_BOOK3S_32_HANDLER
 MODULE_ALIAS_MISCDEV(KVM_MINOR);
 MODULE_ALIAS("devname:kvm");
 #endif
diff --git a/arch/powerpc/kvm/book3s_hv_rm_mmu.c b/arch/powerpc/kvm/book3s_hv_rm_mmu.c
index 1d6c56ad5b6..8fcc36306a0 100644
--- a/arch/powerpc/kvm/book3s_hv_rm_mmu.c
+++ b/arch/powerpc/kvm/book3s_hv_rm_mmu.c
@@ -234,7 +234,7 @@ long kvmppc_do_h_enter(struct kvm *kvm, unsigned long flags,
 		pte_size = psize;
 		pte = lookup_linux_pte_and_update(pgdir, hva, writing,
 						  &pte_size);
-		if (pte_present(pte)) {
+		if (pte_present(pte) && !pte_numa(pte)) {
 			if (writing && !pte_write(pte))
 				/* make the actual HPTE be read-only */
 				ptel = hpte_make_readonly(ptel);
diff --git a/arch/powerpc/kvm/book3s_hv_rmhandlers.S b/arch/powerpc/kvm/book3s_hv_rmhandlers.S
index ffbb871c2bd..07c8b5b0f9d 100644
--- a/arch/powerpc/kvm/book3s_hv_rmhandlers.S
+++ b/arch/powerpc/kvm/book3s_hv_rmhandlers.S
@@ -242,6 +242,12 @@ kvm_novcpu_exit:
  */
 	.globl	kvm_start_guest
 kvm_start_guest:
+
+	/* Set runlatch bit the minute you wake up from nap */
+	mfspr	r1, SPRN_CTRLF
+	ori 	r1, r1, 1
+	mtspr	SPRN_CTRLT, r1
+
 	ld	r2,PACATOC(r13)
 
 	li	r0,KVM_HWTHREAD_IN_KVM
@@ -309,6 +315,11 @@ kvm_no_guest:
 	li	r0, KVM_HWTHREAD_IN_NAP
 	stb	r0, HSTATE_HWTHREAD_STATE(r13)
 kvm_do_nap:
+	/* Clear the runlatch bit before napping */
+	mfspr	r2, SPRN_CTRLF
+	clrrdi	r2, r2, 1
+	mtspr	SPRN_CTRLT, r2
+
 	li	r3, LPCR_PECE0
 	mfspr	r4, SPRN_LPCR
 	rlwimi	r4, r3, 0, LPCR_PECE0 | LPCR_PECE1
@@ -1312,6 +1323,110 @@ END_FTR_SECTION_IFSET(CPU_FTR_ARCH_206)
 	mr	r3, r9
 	bl	kvmppc_save_fp
 
+#ifdef CONFIG_PPC_TRANSACTIONAL_MEM
+BEGIN_FTR_SECTION
+	b	2f
+END_FTR_SECTION_IFCLR(CPU_FTR_TM)
+	/* Turn on TM. */
+	mfmsr	r8
+	li	r0, 1
+	rldimi	r8, r0, MSR_TM_LG, 63-MSR_TM_LG
+	mtmsrd	r8
+
+	ld	r5, VCPU_MSR(r9)
+	rldicl. r5, r5, 64 - MSR_TS_S_LG, 62
+	beq	1f	/* TM not active in guest. */
+
+	li	r3, TM_CAUSE_KVM_RESCHED
+
+	/* Clear the MSR RI since r1, r13 are all going to be foobar. */
+	li	r5, 0
+	mtmsrd	r5, 1
+
+	/* All GPRs are volatile at this point. */
+	TRECLAIM(R3)
+
+	/* Temporarily store r13 and r9 so we have some regs to play with */
+	SET_SCRATCH0(r13)
+	GET_PACA(r13)
+	std	r9, PACATMSCRATCH(r13)
+	ld	r9, HSTATE_KVM_VCPU(r13)
+
+	/* Get a few more GPRs free. */
+	std	r29, VCPU_GPRS_TM(29)(r9)
+	std	r30, VCPU_GPRS_TM(30)(r9)
+	std	r31, VCPU_GPRS_TM(31)(r9)
+
+	/* Save away PPR and DSCR soon so don't run with user values. */
+	mfspr	r31, SPRN_PPR
+	HMT_MEDIUM
+	mfspr	r30, SPRN_DSCR
+	ld	r29, HSTATE_DSCR(r13)
+	mtspr	SPRN_DSCR, r29
+
+	/* Save all but r9, r13 & r29-r31 */
+	reg = 0
+	.rept	29
+	.if (reg != 9) && (reg != 13)
+	std	reg, VCPU_GPRS_TM(reg)(r9)
+	.endif
+	reg = reg + 1
+	.endr
+	/* ... now save r13 */
+	GET_SCRATCH0(r4)
+	std	r4, VCPU_GPRS_TM(13)(r9)
+	/* ... and save r9 */
+	ld	r4, PACATMSCRATCH(r13)
+	std	r4, VCPU_GPRS_TM(9)(r9)
+
+	/* Reload stack pointer and TOC. */
+	ld	r1, HSTATE_HOST_R1(r13)
+	ld	r2, PACATOC(r13)
+
+	/* Set MSR RI now we have r1 and r13 back. */
+	li	r5, MSR_RI
+	mtmsrd	r5, 1
+
+	/* Save away checkpinted SPRs. */
+	std	r31, VCPU_PPR_TM(r9)
+	std	r30, VCPU_DSCR_TM(r9)
+	mflr	r5
+	mfcr	r6
+	mfctr	r7
+	mfspr	r8, SPRN_AMR
+	mfspr	r10, SPRN_TAR
+	std	r5, VCPU_LR_TM(r9)
+	stw	r6, VCPU_CR_TM(r9)
+	std	r7, VCPU_CTR_TM(r9)
+	std	r8, VCPU_AMR_TM(r9)
+	std	r10, VCPU_TAR_TM(r9)
+
+	/* Restore r12 as trap number. */
+	lwz	r12, VCPU_TRAP(r9)
+
+	/* Save FP/VSX. */
+	addi	r3, r9, VCPU_FPRS_TM
+	bl	.store_fp_state
+	addi	r3, r9, VCPU_VRS_TM
+	bl	.store_vr_state
+	mfspr	r6, SPRN_VRSAVE
+	stw	r6, VCPU_VRSAVE_TM(r9)
+1:
+	/*
+	 * We need to save these SPRs after the treclaim so that the software
+	 * error code is recorded correctly in the TEXASR.  Also the user may
+	 * change these outside of a transaction, so they must always be
+	 * context switched.
+	 */
+	mfspr	r5, SPRN_TFHAR
+	mfspr	r6, SPRN_TFIAR
+	mfspr	r7, SPRN_TEXASR
+	std	r5, VCPU_TFHAR(r9)
+	std	r6, VCPU_TFIAR(r9)
+	std	r7, VCPU_TEXASR(r9)
+2:
+#endif
+
 	/* Increment yield count if they have a VPA */
 	ld	r8, VCPU_VPA(r9)	/* do they have a VPA? */
 	cmpdi	r8, 0
@@ -1999,8 +2114,13 @@ END_FTR_SECTION_IFCLR(CPU_FTR_ARCH_206)
 
 	/*
 	 * Take a nap until a decrementer or external or doobell interrupt
-	 * occurs, with PECE1, PECE0 and PECEDP set in LPCR
+	 * occurs, with PECE1, PECE0 and PECEDP set in LPCR. Also clear the
+	 * runlatch bit before napping.
 	 */
+	mfspr	r2, SPRN_CTRLF
+	clrrdi	r2, r2, 1
+	mtspr	SPRN_CTRLT, r2
+
 	li	r0,1
 	stb	r0,HSTATE_HWTHREAD_REQ(r13)
 	mfspr	r5,SPRN_LPCR
diff --git a/arch/powerpc/kvm/book3s_pr.c b/arch/powerpc/kvm/book3s_pr.c
index c5c052a9729..02f1defd8bb 100644
--- a/arch/powerpc/kvm/book3s_pr.c
+++ b/arch/powerpc/kvm/book3s_pr.c
@@ -1153,7 +1153,7 @@ static struct kvm_vcpu *kvmppc_core_vcpu_create_pr(struct kvm *kvm,
 		goto free_vcpu;
 	vcpu->arch.book3s = vcpu_book3s;
 
-#ifdef CONFIG_KVM_BOOK3S_32
+#ifdef CONFIG_KVM_BOOK3S_32_HANDLER
 	vcpu->arch.shadow_vcpu =
 		kzalloc(sizeof(*vcpu->arch.shadow_vcpu), GFP_KERNEL);
 	if (!vcpu->arch.shadow_vcpu)
@@ -1198,7 +1198,7 @@ static struct kvm_vcpu *kvmppc_core_vcpu_create_pr(struct kvm *kvm,
 uninit_vcpu:
 	kvm_vcpu_uninit(vcpu);
 free_shadow_vcpu:
-#ifdef CONFIG_KVM_BOOK3S_32
+#ifdef CONFIG_KVM_BOOK3S_32_HANDLER
 	kfree(vcpu->arch.shadow_vcpu);
 free_vcpu3s:
 #endif
@@ -1215,7 +1215,7 @@ static void kvmppc_core_vcpu_free_pr(struct kvm_vcpu *vcpu)
 
 	free_page((unsigned long)vcpu->arch.shared & PAGE_MASK);
 	kvm_vcpu_uninit(vcpu);
-#ifdef CONFIG_KVM_BOOK3S_32
+#ifdef CONFIG_KVM_BOOK3S_32_HANDLER
 	kfree(vcpu->arch.shadow_vcpu);
 #endif
 	vfree(vcpu_book3s);
diff --git a/arch/powerpc/mm/hash_native_64.c b/arch/powerpc/mm/hash_native_64.c
index 3ea26c25590..cf1d325eae8 100644
--- a/arch/powerpc/mm/hash_native_64.c
+++ b/arch/powerpc/mm/hash_native_64.c
@@ -82,17 +82,14 @@ static inline void __tlbie(unsigned long vpn, int psize, int apsize, int ssize)
 		va &= ~((1ul << mmu_psize_defs[apsize].shift) - 1);
 		va |= penc << 12;
 		va |= ssize << 8;
-		/* Add AVAL part */
-		if (psize != apsize) {
-			/*
-			 * MPSS, 64K base page size and 16MB parge page size
-			 * We don't need all the bits, but rest of the bits
-			 * must be ignored by the processor.
-			 * vpn cover upto 65 bits of va. (0...65) and we need
-			 * 58..64 bits of va.
-			 */
-			va |= (vpn & 0xfe);
-		}
+		/*
+		 * AVAL bits:
+		 * We don't need all the bits, but rest of the bits
+		 * must be ignored by the processor.
+		 * vpn cover upto 65 bits of va. (0...65) and we need
+		 * 58..64 bits of va.
+		 */
+		va |= (vpn & 0xfe); /* AVAL */
 		va |= 1; /* L */
 		asm volatile(ASM_FTR_IFCLR("tlbie %0,1", PPC_TLBIE(%1,%0), %2)
 			     : : "r" (va), "r"(0), "i" (CPU_FTR_ARCH_206)
@@ -133,17 +130,14 @@ static inline void __tlbiel(unsigned long vpn, int psize, int apsize, int ssize)
 		va &= ~((1ul << mmu_psize_defs[apsize].shift) - 1);
 		va |= penc << 12;
 		va |= ssize << 8;
-		/* Add AVAL part */
-		if (psize != apsize) {
-			/*
-			 * MPSS, 64K base page size and 16MB parge page size
-			 * We don't need all the bits, but rest of the bits
-			 * must be ignored by the processor.
-			 * vpn cover upto 65 bits of va. (0...65) and we need
-			 * 58..64 bits of va.
-			 */
-			va |= (vpn & 0xfe);
-		}
+		/*
+		 * AVAL bits:
+		 * We don't need all the bits, but rest of the bits
+		 * must be ignored by the processor.
+		 * vpn cover upto 65 bits of va. (0...65) and we need
+		 * 58..64 bits of va.
+		 */
+		va |= (vpn & 0xfe);
 		va |= 1; /* L */
 		asm volatile(".long 0x7c000224 | (%0 << 11) | (1 << 21)"
 			     : : "r"(va) : "memory");
diff --git a/arch/powerpc/mm/hash_utils_64.c b/arch/powerpc/mm/hash_utils_64.c
index d766d6ee33f..06ba83b036d 100644
--- a/arch/powerpc/mm/hash_utils_64.c
+++ b/arch/powerpc/mm/hash_utils_64.c
@@ -207,6 +207,10 @@ int htab_bolt_mapping(unsigned long vstart, unsigned long vend,
 		if (overlaps_kernel_text(vaddr, vaddr + step))
 			tprot &= ~HPTE_R_N;
 
+		/* Make kvm guest trampolines executable */
+		if (overlaps_kvm_tmp(vaddr, vaddr + step))
+			tprot &= ~HPTE_R_N;
+
 		/*
 		 * If relocatable, check if it overlaps interrupt vectors that
 		 * are copied down to real 0. For relocatable kernel
diff --git a/arch/powerpc/mm/numa.c b/arch/powerpc/mm/numa.c
index 4ebbb9e9928..3b181b22cd4 100644
--- a/arch/powerpc/mm/numa.c
+++ b/arch/powerpc/mm/numa.c
@@ -232,6 +232,7 @@ int __node_distance(int a, int b)
 
 	return distance;
 }
+EXPORT_SYMBOL(__node_distance);
 
 static void initialize_distance_lookup_table(int nid,
 		const __be32 *associativity)
diff --git a/arch/powerpc/perf/hv-24x7.c b/arch/powerpc/perf/hv-24x7.c
index 297c9105141..e0766b82e16 100644
--- a/arch/powerpc/perf/hv-24x7.c
+++ b/arch/powerpc/perf/hv-24x7.c
@@ -155,16 +155,28 @@ static ssize_t read_offset_data(void *dest, size_t dest_len,
 	return copy_len;
 }
 
-static unsigned long h_get_24x7_catalog_page(char page[static 4096],
-					     u32 version, u32 index)
+static unsigned long h_get_24x7_catalog_page_(unsigned long phys_4096,
+					      unsigned long version,
+					      unsigned long index)
 {
-	WARN_ON(!IS_ALIGNED((unsigned long)page, 4096));
+	pr_devel("h_get_24x7_catalog_page(0x%lx, %lu, %lu)",
+			phys_4096,
+			version,
+			index);
+	WARN_ON(!IS_ALIGNED(phys_4096, 4096));
 	return plpar_hcall_norets(H_GET_24X7_CATALOG_PAGE,
-			virt_to_phys(page),
+			phys_4096,
 			version,
 			index);
 }
 
+static unsigned long h_get_24x7_catalog_page(char page[],
+					     u64 version, u32 index)
+{
+	return h_get_24x7_catalog_page_(virt_to_phys(page),
+					version, index);
+}
+
 static ssize_t catalog_read(struct file *filp, struct kobject *kobj,
 			    struct bin_attribute *bin_attr, char *buf,
 			    loff_t offset, size_t count)
@@ -173,7 +185,7 @@ static ssize_t catalog_read(struct file *filp, struct kobject *kobj,
 	ssize_t ret = 0;
 	size_t catalog_len = 0, catalog_page_len = 0, page_count = 0;
 	loff_t page_offset = 0;
-	uint32_t catalog_version_num = 0;
+	uint64_t catalog_version_num = 0;
 	void *page = kmem_cache_alloc(hv_page_cache, GFP_USER);
 	struct hv_24x7_catalog_page_0 *page_0 = page;
 	if (!page)
@@ -185,7 +197,7 @@ static ssize_t catalog_read(struct file *filp, struct kobject *kobj,
 		goto e_free;
 	}
 
-	catalog_version_num = be32_to_cpu(page_0->version);
+	catalog_version_num = be64_to_cpu(page_0->version);
 	catalog_page_len = be32_to_cpu(page_0->length);
 	catalog_len = catalog_page_len * 4096;
 
@@ -208,8 +220,9 @@ static ssize_t catalog_read(struct file *filp, struct kobject *kobj,
 				page, 4096, page_offset * 4096);
 e_free:
 	if (hret)
-		pr_err("h_get_24x7_catalog_page(ver=%d, page=%lld) failed: rc=%ld\n",
-				catalog_version_num, page_offset, hret);
+		pr_err("h_get_24x7_catalog_page(ver=%lld, page=%lld) failed:"
+		       " rc=%ld\n",
+		       catalog_version_num, page_offset, hret);
 	kfree(page);
 
 	pr_devel("catalog_read: offset=%lld(%lld) count=%zu(%zu) catalog_len=%zu(%zu) => %zd\n",
@@ -243,7 +256,7 @@ e_free:								\
 static DEVICE_ATTR_RO(_name)
 
 PAGE_0_ATTR(catalog_version, "%lld\n",
-		(unsigned long long)be32_to_cpu(page_0->version));
+		(unsigned long long)be64_to_cpu(page_0->version));
 PAGE_0_ATTR(catalog_len, "%lld\n",
 		(unsigned long long)be32_to_cpu(page_0->length) * 4096);
 static BIN_ATTR_RO(catalog, 0/* real length varies */);
@@ -485,13 +498,13 @@ static int hv_24x7_init(void)
 	struct hv_perf_caps caps;
 
 	if (!firmware_has_feature(FW_FEATURE_LPAR)) {
-		pr_info("not a virtualized system, not enabling\n");
+		pr_debug("not a virtualized system, not enabling\n");
 		return -ENODEV;
 	}
 
 	hret = hv_perf_caps_get(&caps);
 	if (hret) {
-		pr_info("could not obtain capabilities, error 0x%80lx, not enabling\n",
+		pr_debug("could not obtain capabilities, not enabling, rc=%ld\n",
 				hret);
 		return -ENODEV;
 	}
diff --git a/arch/powerpc/perf/hv-gpci.c b/arch/powerpc/perf/hv-gpci.c
index 278ba7b9c2b..c9d399a2df8 100644
--- a/arch/powerpc/perf/hv-gpci.c
+++ b/arch/powerpc/perf/hv-gpci.c
@@ -78,7 +78,7 @@ static ssize_t kernel_version_show(struct device *dev,
 	return sprintf(page, "0x%x\n", COUNTER_INFO_VERSION_CURRENT);
 }
 
-DEVICE_ATTR_RO(kernel_version);
+static DEVICE_ATTR_RO(kernel_version);
 HV_CAPS_ATTR(version, "0x%x\n");
 HV_CAPS_ATTR(ga, "%d\n");
 HV_CAPS_ATTR(expanded, "%d\n");
@@ -273,13 +273,13 @@ static int hv_gpci_init(void)
 	struct hv_perf_caps caps;
 
 	if (!firmware_has_feature(FW_FEATURE_LPAR)) {
-		pr_info("not a virtualized system, not enabling\n");
+		pr_debug("not a virtualized system, not enabling\n");
 		return -ENODEV;
 	}
 
 	hret = hv_perf_caps_get(&caps);
 	if (hret) {
-		pr_info("could not obtain capabilities, error 0x%80lx, not enabling\n",
+		pr_debug("could not obtain capabilities, not enabling, rc=%ld\n",
 				hret);
 		return -ENODEV;
 	}
diff --git a/arch/powerpc/platforms/powernv/eeh-ioda.c b/arch/powerpc/platforms/powernv/eeh-ioda.c
index 253fefe3d1a..5b51079f3e3 100644
--- a/arch/powerpc/platforms/powernv/eeh-ioda.c
+++ b/arch/powerpc/platforms/powernv/eeh-ioda.c
@@ -549,7 +549,8 @@ static int ioda_eeh_reset(struct eeh_pe *pe, int option)
 		ret = ioda_eeh_phb_reset(hose, option);
 	} else {
 		bus = eeh_pe_bus_get(pe);
-		if (pci_is_root_bus(bus))
+		if (pci_is_root_bus(bus) ||
+		    pci_is_root_bus(bus->parent))
 			ret = ioda_eeh_root_reset(hose, option);
 		else
 			ret = ioda_eeh_bridge_reset(hose, bus->self, option);
diff --git a/arch/powerpc/platforms/powernv/opal-dump.c b/arch/powerpc/platforms/powernv/opal-dump.c
index b9827b0d87e..788a1977b9a 100644
--- a/arch/powerpc/platforms/powernv/opal-dump.c
+++ b/arch/powerpc/platforms/powernv/opal-dump.c
@@ -209,89 +209,20 @@ static struct kobj_type dump_ktype = {
 	.default_attrs = dump_default_attrs,
 };
 
-static void free_dump_sg_list(struct opal_sg_list *list)
-{
-	struct opal_sg_list *sg1;
-	while (list) {
-		sg1 = list->next;
-		kfree(list);
-		list = sg1;
-	}
-	list = NULL;
-}
-
-static struct opal_sg_list *dump_data_to_sglist(struct dump_obj *dump)
-{
-	struct opal_sg_list *sg1, *list = NULL;
-	void *addr;
-	int64_t size;
-
-	addr = dump->buffer;
-	size = dump->size;
-
-	sg1 = kzalloc(PAGE_SIZE, GFP_KERNEL);
-	if (!sg1)
-		goto nomem;
-
-	list = sg1;
-	sg1->num_entries = 0;
-	while (size > 0) {
-		/* Translate virtual address to physical address */
-		sg1->entry[sg1->num_entries].data =
-			(void *)(vmalloc_to_pfn(addr) << PAGE_SHIFT);
-
-		if (size > PAGE_SIZE)
-			sg1->entry[sg1->num_entries].length = PAGE_SIZE;
-		else
-			sg1->entry[sg1->num_entries].length = size;
-
-		sg1->num_entries++;
-		if (sg1->num_entries >= SG_ENTRIES_PER_NODE) {
-			sg1->next = kzalloc(PAGE_SIZE, GFP_KERNEL);
-			if (!sg1->next)
-				goto nomem;
-
-			sg1 = sg1->next;
-			sg1->num_entries = 0;
-		}
-		addr += PAGE_SIZE;
-		size -= PAGE_SIZE;
-	}
-	return list;
-
-nomem:
-	pr_err("%s : Failed to allocate memory\n", __func__);
-	free_dump_sg_list(list);
-	return NULL;
-}
-
-static void sglist_to_phy_addr(struct opal_sg_list *list)
-{
-	struct opal_sg_list *sg, *next;
-
-	for (sg = list; sg; sg = next) {
-		next = sg->next;
-		/* Don't translate NULL pointer for last entry */
-		if (sg->next)
-			sg->next = (struct opal_sg_list *)__pa(sg->next);
-		else
-			sg->next = NULL;
-
-		/* Convert num_entries to length */
-		sg->num_entries =
-			sg->num_entries * sizeof(struct opal_sg_entry) + 16;
-	}
-}
-
-static int64_t dump_read_info(uint32_t *id, uint32_t *size, uint32_t *type)
+static int64_t dump_read_info(uint32_t *dump_id, uint32_t *dump_size, uint32_t *dump_type)
 {
+	__be32 id, size, type;
 	int rc;
-	*type = 0xffffffff;
 
-	rc = opal_dump_info2(id, size, type);
+	type = cpu_to_be32(0xffffffff);
 
+	rc = opal_dump_info2(&id, &size, &type);
 	if (rc == OPAL_PARAMETER)
-		rc = opal_dump_info(id, size);
+		rc = opal_dump_info(&id, &size);
+
+	*dump_id = be32_to_cpu(id);
+	*dump_size = be32_to_cpu(size);
+	*dump_type = be32_to_cpu(type);
 
 	if (rc)
 		pr_warn("%s: Failed to get dump info (%d)\n",
@@ -314,15 +245,12 @@ static int64_t dump_read_data(struct dump_obj *dump)
 	}
 
 	/* Generate SG list */
-	list = dump_data_to_sglist(dump);
+	list = opal_vmalloc_to_sg_list(dump->buffer, dump->size);
 	if (!list) {
 		rc = -ENOMEM;
 		goto out;
 	}
 
-	/* Translate sg list addr to real address */
-	sglist_to_phy_addr(list);
-
 	/* First entry address */
 	addr = __pa(list);
 
@@ -341,7 +269,7 @@ static int64_t dump_read_data(struct dump_obj *dump)
 			__func__, dump->id);
 
 	/* Free SG list */
-	free_dump_sg_list(list);
+	opal_free_sg_list(list);
 
 out:
 	return rc;
diff --git a/arch/powerpc/platforms/powernv/opal-elog.c b/arch/powerpc/platforms/powernv/opal-elog.c
index ef7bc2a9786..10268c41d83 100644
--- a/arch/powerpc/platforms/powernv/opal-elog.c
+++ b/arch/powerpc/platforms/powernv/opal-elog.c
@@ -238,18 +238,25 @@ static struct elog_obj *create_elog_obj(uint64_t id, size_t size, uint64_t type)
 
 static void elog_work_fn(struct work_struct *work)
 {
-	size_t elog_size;
+	__be64 size;
+	__be64 id;
+	__be64 type;
+	uint64_t elog_size;
 	uint64_t log_id;
 	uint64_t elog_type;
 	int rc;
 	char name[2+16+1];
 
-	rc = opal_get_elog_size(&log_id, &elog_size, &elog_type);
+	rc = opal_get_elog_size(&id, &size, &type);
 	if (rc != OPAL_SUCCESS) {
 		pr_err("ELOG: Opal log read failed\n");
 		return;
 	}
 
+	elog_size = be64_to_cpu(size);
+	log_id = be64_to_cpu(id);
+	elog_type = be64_to_cpu(type);
+
 	BUG_ON(elog_size > OPAL_MAX_ERRLOG_SIZE);
 
 	if (elog_size >= OPAL_MAX_ERRLOG_SIZE)
diff --git a/arch/powerpc/platforms/powernv/opal-flash.c b/arch/powerpc/platforms/powernv/opal-flash.c
index 714ef972406..dc487ff0470 100644
--- a/arch/powerpc/platforms/powernv/opal-flash.c
+++ b/arch/powerpc/platforms/powernv/opal-flash.c
@@ -79,9 +79,6 @@
 /* XXX: Assume candidate image size is <= 1GB */
 #define MAX_IMAGE_SIZE	0x40000000
 
-/* Flash sg list version */
-#define SG_LIST_VERSION (1UL)
-
 /* Image status */
 enum {
 	IMAGE_INVALID,
@@ -131,11 +128,15 @@ static DEFINE_MUTEX(image_data_mutex);
  */
 static inline void opal_flash_validate(void)
 {
-	struct validate_flash_t *args_buf = &validate_flash_data;
+	long ret;
+	void *buf = validate_flash_data.buf;
+	__be32 size, result;
 
-	args_buf->status = opal_validate_flash(__pa(args_buf->buf),
-					       &(args_buf->buf_size),
-					       &(args_buf->result));
+	ret = opal_validate_flash(__pa(buf), &size, &result);
+
+	validate_flash_data.status = ret;
+	validate_flash_data.buf_size = be32_to_cpu(size);
+	validate_flash_data.result = be32_to_cpu(result);
 }
 
 /*
@@ -268,93 +269,11 @@ static ssize_t manage_store(struct kobject *kobj,
 }
 
 /*
- * Free sg list
- */
-static void free_sg_list(struct opal_sg_list *list)
-{
-	struct opal_sg_list *sg1;
-	while (list) {
-		sg1 = list->next;
-		kfree(list);
-		list = sg1;
-	}
-	list = NULL;
-}
-
-/*
- * Build candidate image scatter gather list
- *
- * list format:
- *   -----------------------------------
- *  |  VER (8) | Entry length in bytes  |
- *   -----------------------------------
- *  |  Pointer to next entry            |
- *   -----------------------------------
- *  |  Address of memory area 1         |
- *   -----------------------------------
- *  |  Length of memory area 1          |
- *   -----------------------------------
- *  |   .........                       |
- *   -----------------------------------
- *  |   .........                       |
- *   -----------------------------------
- *  |  Address of memory area N         |
- *   -----------------------------------
- *  |  Length of memory area N          |
- *   -----------------------------------
- */
-static struct opal_sg_list *image_data_to_sglist(void)
-{
-	struct opal_sg_list *sg1, *list = NULL;
-	void *addr;
-	int size;
-
-	addr = image_data.data;
-	size = image_data.size;
-
-	sg1 = kzalloc(PAGE_SIZE, GFP_KERNEL);
-	if (!sg1)
-		return NULL;
-
-	list = sg1;
-	sg1->num_entries = 0;
-	while (size > 0) {
-		/* Translate virtual address to physical address */
-		sg1->entry[sg1->num_entries].data =
-			(void *)(vmalloc_to_pfn(addr) << PAGE_SHIFT);
-
-		if (size > PAGE_SIZE)
-			sg1->entry[sg1->num_entries].length = PAGE_SIZE;
-		else
-			sg1->entry[sg1->num_entries].length = size;
-
-		sg1->num_entries++;
-		if (sg1->num_entries >= SG_ENTRIES_PER_NODE) {
-			sg1->next = kzalloc(PAGE_SIZE, GFP_KERNEL);
-			if (!sg1->next) {
-				pr_err("%s : Failed to allocate memory\n",
-				       __func__);
-				goto nomem;
-			}
-
-			sg1 = sg1->next;
-			sg1->num_entries = 0;
-		}
-		addr += PAGE_SIZE;
-		size -= PAGE_SIZE;
-	}
-	return list;
-nomem:
-	free_sg_list(list);
-	return NULL;
-}
-
-/*
  * OPAL update flash
  */
 static int opal_flash_update(int op)
 {
-	struct opal_sg_list *sg, *list, *next;
+	struct opal_sg_list *list;
 	unsigned long addr;
 	int64_t rc = OPAL_PARAMETER;
 
@@ -364,30 +283,13 @@ static int opal_flash_update(int op)
 		goto flash;
 	}
 
-	list = image_data_to_sglist();
+	list = opal_vmalloc_to_sg_list(image_data.data, image_data.size);
 	if (!list)
 		goto invalid_img;
 
 	/* First entry address */
 	addr = __pa(list);
 
-	/* Translate sg list address to absolute */
-	for (sg = list; sg; sg = next) {
-		next = sg->next;
-		/* Don't translate NULL pointer for last entry */
-		if (sg->next)
-			sg->next = (struct opal_sg_list *)__pa(sg->next);
-		else
-			sg->next = NULL;
-
-		/*
-		 * Convert num_entries to version/length format
-		 * to satisfy OPAL.
-		 */
-		sg->num_entries = (SG_LIST_VERSION << 56) |
-			(sg->num_entries * sizeof(struct opal_sg_entry) + 16);
-	}
-
 	pr_alert("FLASH: Image is %u bytes\n", image_data.size);
 	pr_alert("FLASH: Image update requested\n");
 	pr_alert("FLASH: Image will be updated during system reboot\n");
diff --git a/arch/powerpc/platforms/powernv/opal-sysparam.c b/arch/powerpc/platforms/powernv/opal-sysparam.c
index 6b614726baf..d202f9bc368 100644
--- a/arch/powerpc/platforms/powernv/opal-sysparam.c
+++ b/arch/powerpc/platforms/powernv/opal-sysparam.c
@@ -39,10 +39,11 @@ struct param_attr {
 	struct kobj_attribute kobj_attr;
 };
 
-static int opal_get_sys_param(u32 param_id, u32 length, void *buffer)
+static ssize_t opal_get_sys_param(u32 param_id, u32 length, void *buffer)
 {
 	struct opal_msg msg;
-	int ret, token;
+	ssize_t ret;
+	int token;
 
 	token = opal_async_get_token_interruptible();
 	if (token < 0) {
@@ -59,7 +60,7 @@ static int opal_get_sys_param(u32 param_id, u32 length, void *buffer)
 
 	ret = opal_async_wait_response(token, &msg);
 	if (ret) {
-		pr_err("%s: Failed to wait for the async response, %d\n",
+		pr_err("%s: Failed to wait for the async response, %zd\n",
 				__func__, ret);
 		goto out_token;
 	}
@@ -111,7 +112,7 @@ static ssize_t sys_param_show(struct kobject *kobj,
 {
 	struct param_attr *attr = container_of(kobj_attr, struct param_attr,
 			kobj_attr);
-	int ret;
+	ssize_t ret;
 
 	mutex_lock(&opal_sysparam_mutex);
 	ret = opal_get_sys_param(attr->param_id, attr->param_size,
@@ -121,9 +122,10 @@ static ssize_t sys_param_show(struct kobject *kobj,
 
 	memcpy(buf, param_data_buf, attr->param_size);
 
+	ret = attr->param_size;
 out:
 	mutex_unlock(&opal_sysparam_mutex);
-	return ret ? ret : attr->param_size;
+	return ret;
 }
 
 static ssize_t sys_param_store(struct kobject *kobj,
@@ -131,14 +133,20 @@ static ssize_t sys_param_store(struct kobject *kobj,
 {
 	struct param_attr *attr = container_of(kobj_attr, struct param_attr,
 			kobj_attr);
-	int ret;
+	ssize_t ret;
+
+        /* MAX_PARAM_DATA_LEN is sizeof(param_data_buf) */
+        if (count > MAX_PARAM_DATA_LEN)
+                count = MAX_PARAM_DATA_LEN;
 
 	mutex_lock(&opal_sysparam_mutex);
 	memcpy(param_data_buf, buf, count);
 	ret = opal_set_sys_param(attr->param_id, attr->param_size,
 			param_data_buf);
 	mutex_unlock(&opal_sysparam_mutex);
-	return ret ? ret : count;
+	if (!ret)
+		ret = count;
+	return ret;
 }
 
 void __init opal_sys_param_init(void)
@@ -214,13 +222,13 @@ void __init opal_sys_param_init(void)
 	}
 
 	if (of_property_read_u32_array(sysparam, "param-len", size, count)) {
-		pr_err("SYSPARAM: Missing propery param-len in the DT\n");
+		pr_err("SYSPARAM: Missing property param-len in the DT\n");
 		goto out_free_perm;
 	}
 
 
 	if (of_property_read_u8_array(sysparam, "param-perm", perm, count)) {
-		pr_err("SYSPARAM: Missing propery param-perm in the DT\n");
+		pr_err("SYSPARAM: Missing property param-perm in the DT\n");
 		goto out_free_perm;
 	}
 
@@ -233,6 +241,12 @@ void __init opal_sys_param_init(void)
 
 	/* For each of the parameters, populate the parameter attributes */
 	for (i = 0; i < count; i++) {
+		if (size[i] > MAX_PARAM_DATA_LEN) {
+			pr_warn("SYSPARAM: Not creating parameter %d as size "
+				"exceeds buffer length\n", i);
+			continue;
+		}
+
 		sysfs_attr_init(&attr[i].kobj_attr.attr);
 		attr[i].param_id = id[i];
 		attr[i].param_size = size[i];
diff --git a/arch/powerpc/platforms/powernv/opal.c b/arch/powerpc/platforms/powernv/opal.c
index 49d2f00019e..360ad80c754 100644
--- a/arch/powerpc/platforms/powernv/opal.c
+++ b/arch/powerpc/platforms/powernv/opal.c
@@ -242,14 +242,14 @@ void opal_notifier_update_evt(uint64_t evt_mask,
 void opal_notifier_enable(void)
 {
 	int64_t rc;
-	uint64_t evt = 0;
+	__be64 evt = 0;
 
 	atomic_set(&opal_notifier_hold, 0);
 
 	/* Process pending events */
 	rc = opal_poll_events(&evt);
 	if (rc == OPAL_SUCCESS && evt)
-		opal_do_notifier(evt);
+		opal_do_notifier(be64_to_cpu(evt));
 }
 
 void opal_notifier_disable(void)
@@ -529,7 +529,7 @@ static irqreturn_t opal_interrupt(int irq, void *data)
 
 	opal_handle_interrupt(virq_to_hw(irq), &events);
 
-	opal_do_notifier(events);
+	opal_do_notifier(be64_to_cpu(events));
 
 	return IRQ_HANDLED;
 }
@@ -638,3 +638,66 @@ void opal_shutdown(void)
 
 /* Export this so that test modules can use it */
 EXPORT_SYMBOL_GPL(opal_invalid_call);
+
+/* Convert a region of vmalloc memory to an opal sg list */
+struct opal_sg_list *opal_vmalloc_to_sg_list(void *vmalloc_addr,
+					     unsigned long vmalloc_size)
+{
+	struct opal_sg_list *sg, *first = NULL;
+	unsigned long i = 0;
+
+	sg = kzalloc(PAGE_SIZE, GFP_KERNEL);
+	if (!sg)
+		goto nomem;
+
+	first = sg;
+
+	while (vmalloc_size > 0) {
+		uint64_t data = vmalloc_to_pfn(vmalloc_addr) << PAGE_SHIFT;
+		uint64_t length = min(vmalloc_size, PAGE_SIZE);
+
+		sg->entry[i].data = cpu_to_be64(data);
+		sg->entry[i].length = cpu_to_be64(length);
+		i++;
+
+		if (i >= SG_ENTRIES_PER_NODE) {
+			struct opal_sg_list *next;
+
+			next = kzalloc(PAGE_SIZE, GFP_KERNEL);
+			if (!next)
+				goto nomem;
+
+			sg->length = cpu_to_be64(
+					i * sizeof(struct opal_sg_entry) + 16);
+			i = 0;
+			sg->next = cpu_to_be64(__pa(next));
+			sg = next;
+		}
+
+		vmalloc_addr += length;
+		vmalloc_size -= length;
+	}
+
+	sg->length = cpu_to_be64(i * sizeof(struct opal_sg_entry) + 16);
+
+	return first;
+
+nomem:
+	pr_err("%s : Failed to allocate memory\n", __func__);
+	opal_free_sg_list(first);
+	return NULL;
+}
+
+void opal_free_sg_list(struct opal_sg_list *sg)
+{
+	while (sg) {
+		uint64_t next = be64_to_cpu(sg->next);
+
+		kfree(sg);
+
+		if (next)
+			sg = __va(next);
+		else
+			sg = NULL;
+	}
+}
diff --git a/arch/powerpc/platforms/powernv/pci-ioda.c b/arch/powerpc/platforms/powernv/pci-ioda.c
index 3b2b4fb3585..98824aa9917 100644
--- a/arch/powerpc/platforms/powernv/pci-ioda.c
+++ b/arch/powerpc/platforms/powernv/pci-ioda.c
@@ -343,7 +343,6 @@ static void pnv_ioda_setup_same_PE(struct pci_bus *bus, struct pnv_ioda_pe *pe)
 				pci_name(dev));
 			continue;
 		}
-		pci_dev_get(dev);
 		pdn->pcidev = dev;
 		pdn->pe_number = pe->pe_number;
 		pe->dma_weight += pnv_ioda_dma_weight(dev);
@@ -462,7 +461,7 @@ static void pnv_pci_ioda_dma_dev_setup(struct pnv_phb *phb, struct pci_dev *pdev
 
 	pe = &phb->ioda.pe_array[pdn->pe_number];
 	WARN_ON(get_dma_ops(&pdev->dev) != &dma_iommu_ops);
-	set_iommu_table_base_and_group(&pdev->dev, &pe->tce32_table);
+	set_iommu_table_base(&pdev->dev, &pe->tce32_table);
 }
 
 static int pnv_pci_ioda_dma_set_mask(struct pnv_phb *phb,
diff --git a/arch/powerpc/platforms/powernv/setup.c b/arch/powerpc/platforms/powernv/setup.c
index 61cf8fa9c61..8723d32632f 100644
--- a/arch/powerpc/platforms/powernv/setup.c
+++ b/arch/powerpc/platforms/powernv/setup.c
@@ -162,18 +162,62 @@ static void pnv_shutdown(void)
 }
 
 #ifdef CONFIG_KEXEC
+static void pnv_kexec_wait_secondaries_down(void)
+{
+	int my_cpu, i, notified = -1;
+
+	my_cpu = get_cpu();
+
+	for_each_online_cpu(i) {
+		uint8_t status;
+		int64_t rc;
+
+		if (i == my_cpu)
+			continue;
+
+		for (;;) {
+			rc = opal_query_cpu_status(get_hard_smp_processor_id(i),
+						   &status);
+			if (rc != OPAL_SUCCESS || status != OPAL_THREAD_STARTED)
+				break;
+			barrier();
+			if (i != notified) {
+				printk(KERN_INFO "kexec: waiting for cpu %d "
+				       "(physical %d) to enter OPAL\n",
+				       i, paca[i].hw_cpu_id);
+				notified = i;
+			}
+		}
+	}
+}
+
 static void pnv_kexec_cpu_down(int crash_shutdown, int secondary)
 {
 	xics_kexec_teardown_cpu(secondary);
 
-	/* Return secondary CPUs to firmware on OPAL v3 */
-	if (firmware_has_feature(FW_FEATURE_OPALv3) && secondary) {
+	/* On OPAL v3, we return all CPUs to firmware */
+
+	if (!firmware_has_feature(FW_FEATURE_OPALv3))
+		return;
+
+	if (secondary) {
+		/* Return secondary CPUs to firmware on OPAL v3 */
 		mb();
 		get_paca()->kexec_state = KEXEC_STATE_REAL_MODE;
 		mb();
 
 		/* Return the CPU to OPAL */
 		opal_return_cpu();
+	} else if (crash_shutdown) {
+		/*
+		 * On crash, we don't wait for secondaries to go
+		 * down as they might be unreachable or hung, so
+		 * instead we just wait a bit and move on.
+		 */
+		mdelay(1);
+	} else {
+		/* Primary waits for the secondaries to have reached OPAL */
+		pnv_kexec_wait_secondaries_down();
 	}
 }
 #endif /* CONFIG_KEXEC */
diff --git a/arch/powerpc/platforms/powernv/smp.c b/arch/powerpc/platforms/powernv/smp.c
index 908672bdcea..bf5fcd45216 100644
--- a/arch/powerpc/platforms/powernv/smp.c
+++ b/arch/powerpc/platforms/powernv/smp.c
@@ -30,6 +30,7 @@
 #include <asm/cputhreads.h>
 #include <asm/xics.h>
 #include <asm/opal.h>
+#include <asm/runlatch.h>
 
 #include "powernv.h"
 
@@ -156,7 +157,9 @@ static void pnv_smp_cpu_kill_self(void)
 	 */
 	mtspr(SPRN_LPCR, mfspr(SPRN_LPCR) & ~(u64)LPCR_PECE1);
 	while (!generic_check_cpu_restart(cpu)) {
+		ppc64_runlatch_off();
 		power7_nap();
+		ppc64_runlatch_on();
 		if (!generic_check_cpu_restart(cpu)) {
 			DBG("CPU%d Unexpected exit while offline !\n", cpu);
 			/* We may be getting an IPI, so we re-enable
diff --git a/arch/powerpc/platforms/pseries/hotplug-cpu.c b/arch/powerpc/platforms/pseries/hotplug-cpu.c
index 9b8e05078a6..20d62975856 100644
--- a/arch/powerpc/platforms/pseries/hotplug-cpu.c
+++ b/arch/powerpc/platforms/pseries/hotplug-cpu.c
@@ -88,13 +88,14 @@ void set_default_offline_state(int cpu)
 
 static void rtas_stop_self(void)
 {
-	struct rtas_args args = {
-		.token = cpu_to_be32(rtas_stop_self_token),
+	static struct rtas_args args = {
 		.nargs = 0,
 		.nret = 1,
 		.rets = &args.args[0],
 	};
 
+	args.token = cpu_to_be32(rtas_stop_self_token);
+
 	local_irq_disable();
 
 	BUG_ON(rtas_stop_self_token == RTAS_UNKNOWN_SERVICE);
diff --git a/arch/powerpc/platforms/pseries/hotplug-memory.c b/arch/powerpc/platforms/pseries/hotplug-memory.c
index 573b488fc48..7f75c94af82 100644
--- a/arch/powerpc/platforms/pseries/hotplug-memory.c
+++ b/arch/powerpc/platforms/pseries/hotplug-memory.c
@@ -100,10 +100,10 @@ static int pseries_remove_memblock(unsigned long base, unsigned int memblock_siz
 
 	start_pfn = base >> PAGE_SHIFT;
 
-	if (!pfn_valid(start_pfn)) {
-		memblock_remove(base, memblock_size);
-		return 0;
-	}
+	lock_device_hotplug();
+
+	if (!pfn_valid(start_pfn))
+		goto out;
 
 	block_sz = memory_block_size_bytes();
 	sections_per_block = block_sz / MIN_MEMORY_BLOCK_SIZE;
@@ -114,8 +114,10 @@ static int pseries_remove_memblock(unsigned long base, unsigned int memblock_siz
 		base += MIN_MEMORY_BLOCK_SIZE;
 	}
 
+out:
 	/* Update memory regions for memory remove */
 	memblock_remove(base, memblock_size);
+	unlock_device_hotplug();
 	return 0;
 }
 
diff --git a/arch/powerpc/sysdev/ppc4xx_pci.c b/arch/powerpc/sysdev/ppc4xx_pci.c
index 64603a10b86..4914fd3f41e 100644
--- a/arch/powerpc/sysdev/ppc4xx_pci.c
+++ b/arch/powerpc/sysdev/ppc4xx_pci.c
@@ -1058,7 +1058,7 @@ static int __init apm821xx_pciex_core_init(struct device_node *np)
 	return 1;
 }
 
-static int apm821xx_pciex_init_port_hw(struct ppc4xx_pciex_port *port)
+static int __init apm821xx_pciex_init_port_hw(struct ppc4xx_pciex_port *port)
 {
 	u32 val;
 
diff --git a/arch/s390/Kconfig b/arch/s390/Kconfig
index d68fe34799b..bb63499fc5d 100644
--- a/arch/s390/Kconfig
+++ b/arch/s390/Kconfig
@@ -60,7 +60,6 @@ config PCI_QUIRKS
 
 config S390
 	def_bool y
-	select ARCH_DISCARD_MEMBLOCK
 	select ARCH_HAS_ATOMIC64_DEC_IF_POSITIVE
 	select ARCH_HAS_DEBUG_STRICT_USER_COPY_CHECKS
 	select ARCH_HAVE_NMI_SAFE_CMPXCHG
@@ -130,6 +129,7 @@ config S390
 	select HAVE_KVM if 64BIT
 	select HAVE_MEMBLOCK
 	select HAVE_MEMBLOCK_NODE_MAP
+	select HAVE_MEMBLOCK_PHYS_MAP
 	select HAVE_MOD_ARCH_SPECIFIC
 	select HAVE_OPROFILE
 	select HAVE_PERF_EVENTS
@@ -139,6 +139,7 @@ config S390
 	select HAVE_VIRT_CPU_ACCOUNTING
 	select KTIME_SCALAR if 32BIT
 	select MODULES_USE_ELF_RELA
+	select NO_BOOTMEM
 	select OLD_SIGACTION
 	select OLD_SIGSUSPEND3
 	select SYSCTL_EXCEPTION_TRACE
@@ -592,21 +593,14 @@ config CRASH_DUMP
 	bool "kernel crash dumps"
 	depends on 64BIT && SMP
 	select KEXEC
-	select ZFCPDUMP
 	help
 	  Generate crash dump after being started by kexec.
 	  Crash dump kernels are loaded in the main kernel with kexec-tools
 	  into a specially reserved region and then later executed after
 	  a crash by kdump/kexec.
-	  For more details see Documentation/kdump/kdump.txt
-
-config ZFCPDUMP
-	def_bool n
-	prompt "zfcpdump support"
-	depends on 64BIT && SMP
-	help
-	  Select this option if you want to build an zfcpdump enabled kernel.
 	  Refer to <file:Documentation/s390/zfcpdump.txt> for more details on this.
+	  This option also enables s390 zfcpdump.
+	  See also <file:Documentation/s390/zfcpdump.txt>
 
 endmenu
 
diff --git a/arch/s390/appldata/appldata_mem.c b/arch/s390/appldata/appldata_mem.c
index 42be5374313..edcf2a70694 100644
--- a/arch/s390/appldata/appldata_mem.c
+++ b/arch/s390/appldata/appldata_mem.c
@@ -13,6 +13,7 @@
 #include <linux/kernel_stat.h>
 #include <linux/pagemap.h>
 #include <linux/swap.h>
+#include <linux/slab.h>
 #include <asm/io.h>
 
 #include "appldata.h"
diff --git a/arch/s390/boot/compressed/Makefile b/arch/s390/boot/compressed/Makefile
index 866ecbe670e..f90d1fc6d60 100644
--- a/arch/s390/boot/compressed/Makefile
+++ b/arch/s390/boot/compressed/Makefile
@@ -12,7 +12,7 @@ targets += misc.o piggy.o sizes.h head$(BITS).o
 
 KBUILD_CFLAGS := -m$(BITS) -D__KERNEL__ $(LINUX_INCLUDE) -O2
 KBUILD_CFLAGS += -DDISABLE_BRANCH_PROFILING
-KBUILD_CFLAGS += $(cflags-y)
+KBUILD_CFLAGS += $(cflags-y) -fno-delete-null-pointer-checks
 KBUILD_CFLAGS += $(call cc-option,-mpacked-stack)
 KBUILD_CFLAGS += $(call cc-option,-ffreestanding)
 
diff --git a/arch/s390/crypto/aes_s390.c b/arch/s390/crypto/aes_s390.c
index cf3c0089bef..23223cd63e5 100644
--- a/arch/s390/crypto/aes_s390.c
+++ b/arch/s390/crypto/aes_s390.c
@@ -820,6 +820,9 @@ static int ctr_aes_crypt(struct blkcipher_desc *desc, long func,
 		else
 			memcpy(walk->iv, ctrptr, AES_BLOCK_SIZE);
 		spin_unlock(&ctrblk_lock);
+	} else {
+		if (!nbytes)
+			memcpy(walk->iv, ctrptr, AES_BLOCK_SIZE);
 	}
 	/*
 	 * final block may be < AES_BLOCK_SIZE, copy only nbytes
diff --git a/arch/s390/crypto/des_s390.c b/arch/s390/crypto/des_s390.c
index 0a5aac8a941..7acb77f7ef1 100644
--- a/arch/s390/crypto/des_s390.c
+++ b/arch/s390/crypto/des_s390.c
@@ -429,6 +429,9 @@ static int ctr_desall_crypt(struct blkcipher_desc *desc, long func,
 		else
 			memcpy(walk->iv, ctrptr, DES_BLOCK_SIZE);
 		spin_unlock(&ctrblk_lock);
+	} else {
+		if (!nbytes)
+			memcpy(walk->iv, ctrptr, DES_BLOCK_SIZE);
 	}
 	/* final block may be < DES_BLOCK_SIZE, copy only nbytes */
 	if (nbytes) {
diff --git a/arch/s390/include/asm/ccwdev.h b/arch/s390/include/asm/ccwdev.h
index a9c2c068617..b80e456d642 100644
--- a/arch/s390/include/asm/ccwdev.h
+++ b/arch/s390/include/asm/ccwdev.h
@@ -229,5 +229,5 @@ int ccw_device_siosl(struct ccw_device *);
 
 extern void ccw_device_get_schid(struct ccw_device *, struct subchannel_id *);
 
-extern void *ccw_device_get_chp_desc(struct ccw_device *, int);
+struct channel_path_desc *ccw_device_get_chp_desc(struct ccw_device *, int);
 #endif /* _S390_CCWDEV_H_ */
diff --git a/arch/s390/include/asm/ccwgroup.h b/arch/s390/include/asm/ccwgroup.h
index 6e670f88d12..057ce0ca637 100644
--- a/arch/s390/include/asm/ccwgroup.h
+++ b/arch/s390/include/asm/ccwgroup.h
@@ -10,6 +10,8 @@ struct ccw_driver;
  * @count: number of attached slave devices
  * @dev: embedded device structure
  * @cdev: variable number of slave devices, allocated as needed
+ * @ungroup_work: work to be done when a ccwgroup notifier has action
+ *	type %BUS_NOTIFY_UNBIND_DRIVER
  */
 struct ccwgroup_device {
 	enum {
@@ -22,8 +24,8 @@ struct ccwgroup_device {
 /* public: */
 	unsigned int count;
 	struct device	dev;
-	struct ccw_device *cdev[0];
 	struct work_struct ungroup_work;
+	struct ccw_device *cdev[0];
 };
 
 /**
diff --git a/arch/s390/include/asm/chpid.h b/arch/s390/include/asm/chpid.h
index 38c405ef89c..7298eec9854 100644
--- a/arch/s390/include/asm/chpid.h
+++ b/arch/s390/include/asm/chpid.h
@@ -8,6 +8,17 @@
 #include <uapi/asm/chpid.h>
 #include <asm/cio.h>
 
+struct channel_path_desc {
+	u8 flags;
+	u8 lsn;
+	u8 desc;
+	u8 chpid;
+	u8 swla;
+	u8 zeroes;
+	u8 chla;
+	u8 chpp;
+} __packed;
+
 static inline void chp_id_init(struct chp_id *chpid)
 {
 	memset(chpid, 0, sizeof(struct chp_id));
diff --git a/arch/s390/include/asm/futex.h b/arch/s390/include/asm/futex.h
index 69cf5b5eddc..a4811aa0304 100644
--- a/arch/s390/include/asm/futex.h
+++ b/arch/s390/include/asm/futex.h
@@ -29,7 +29,7 @@ static inline int futex_atomic_op_inuser(int encoded_op, u32 __user *uaddr)
 	int cmparg = (encoded_op << 20) >> 20;
 	int oldval = 0, newval, ret;
 
-	update_primary_asce(current);
+	load_kernel_asce();
 	if (encoded_op & (FUTEX_OP_OPARG_SHIFT << 28))
 		oparg = 1 << oparg;
 
@@ -79,7 +79,7 @@ static inline int futex_atomic_cmpxchg_inatomic(u32 *uval, u32 __user *uaddr,
 {
 	int ret;
 
-	update_primary_asce(current);
+	load_kernel_asce();
 	asm volatile(
 		"   sacf 256\n"
 		"0: cs   %1,%4,0(%5)\n"
diff --git a/arch/s390/include/asm/lowcore.h b/arch/s390/include/asm/lowcore.h
index bbf8141408c..2070cad80e9 100644
--- a/arch/s390/include/asm/lowcore.h
+++ b/arch/s390/include/asm/lowcore.h
@@ -93,7 +93,9 @@ struct _lowcore {
 	__u32	save_area_sync[8];		/* 0x0200 */
 	__u32	save_area_async[8];		/* 0x0220 */
 	__u32	save_area_restart[1];		/* 0x0240 */
-	__u8	pad_0x0244[0x0248-0x0244];	/* 0x0244 */
+
+	/* CPU flags. */
+	__u32	cpu_flags;			/* 0x0244 */
 
 	/* Return psws. */
 	psw_t	return_psw;			/* 0x0248 */
@@ -139,12 +141,9 @@ struct _lowcore {
 	__u32	percpu_offset;			/* 0x02f0 */
 	__u32	machine_flags;			/* 0x02f4 */
 	__u32	ftrace_func;			/* 0x02f8 */
-	__u8	pad_0x02fc[0x0300-0x02fc];	/* 0x02fc */
-
-	/* Interrupt response block */
-	__u8	irb[64];			/* 0x0300 */
+	__u32	spinlock_lockval;		/* 0x02fc */
 
-	__u8	pad_0x0340[0x0e00-0x0340];	/* 0x0340 */
+	__u8	pad_0x0300[0x0e00-0x0300];	/* 0x0300 */
 
 	/*
 	 * 0xe00 contains the address of the IPL Parameter Information
@@ -237,7 +236,9 @@ struct _lowcore {
 	__u64	save_area_sync[8];		/* 0x0200 */
 	__u64	save_area_async[8];		/* 0x0240 */
 	__u64	save_area_restart[1];		/* 0x0280 */
-	__u8	pad_0x0288[0x0290-0x0288];	/* 0x0288 */
+
+	/* CPU flags. */
+	__u64	cpu_flags;			/* 0x0288 */
 
 	/* Return psws. */
 	psw_t	return_psw;			/* 0x0290 */
@@ -285,15 +286,13 @@ struct _lowcore {
 	__u64	machine_flags;			/* 0x0388 */
 	__u64	ftrace_func;			/* 0x0390 */
 	__u64	gmap;				/* 0x0398 */
-	__u8	pad_0x03a0[0x0400-0x03a0];	/* 0x03a0 */
-
-	/* Interrupt response block. */
-	__u8	irb[64];			/* 0x0400 */
+	__u32	spinlock_lockval;		/* 0x03a0 */
+	__u8	pad_0x03a0[0x0400-0x03a4];	/* 0x03a4 */
 
 	/* Per cpu primary space access list */
-	__u32	paste[16];			/* 0x0440 */
+	__u32	paste[16];			/* 0x0400 */
 
-	__u8	pad_0x0480[0x0e00-0x0480];	/* 0x0480 */
+	__u8	pad_0x04c0[0x0e00-0x0440];	/* 0x0440 */
 
 	/*
 	 * 0xe00 contains the address of the IPL Parameter Information
diff --git a/arch/s390/include/asm/mmu_context.h b/arch/s390/include/asm/mmu_context.h
index 71be346d0e3..056d7eff2a1 100644
--- a/arch/s390/include/asm/mmu_context.h
+++ b/arch/s390/include/asm/mmu_context.h
@@ -30,33 +30,31 @@ static inline int init_new_context(struct task_struct *tsk,
 
 #define destroy_context(mm)             do { } while (0)
 
-static inline void update_user_asce(struct mm_struct *mm, int load_primary)
+static inline void set_user_asce(struct mm_struct *mm)
 {
 	pgd_t *pgd = mm->pgd;
 
 	S390_lowcore.user_asce = mm->context.asce_bits | __pa(pgd);
-	if (load_primary)
-		__ctl_load(S390_lowcore.user_asce, 1, 1);
 	set_fs(current->thread.mm_segment);
+	set_cpu_flag(CIF_ASCE);
 }
 
-static inline void clear_user_asce(struct mm_struct *mm, int load_primary)
+static inline void clear_user_asce(void)
 {
 	S390_lowcore.user_asce = S390_lowcore.kernel_asce;
 
-	if (load_primary)
-		__ctl_load(S390_lowcore.user_asce, 1, 1);
+	__ctl_load(S390_lowcore.user_asce, 1, 1);
 	__ctl_load(S390_lowcore.user_asce, 7, 7);
 }
 
-static inline void update_primary_asce(struct task_struct *tsk)
+static inline void load_kernel_asce(void)
 {
 	unsigned long asce;
 
 	__ctl_store(asce, 1, 1);
 	if (asce != S390_lowcore.kernel_asce)
 		__ctl_load(S390_lowcore.kernel_asce, 1, 1);
-	set_tsk_thread_flag(tsk, TIF_ASCE);
+	set_cpu_flag(CIF_ASCE);
 }
 
 static inline void switch_mm(struct mm_struct *prev, struct mm_struct *next,
@@ -64,25 +62,17 @@ static inline void switch_mm(struct mm_struct *prev, struct mm_struct *next,
 {
 	int cpu = smp_processor_id();
 
-	update_primary_asce(tsk);
 	if (prev == next)
 		return;
 	if (MACHINE_HAS_TLB_LC)
 		cpumask_set_cpu(cpu, &next->context.cpu_attach_mask);
-	if (atomic_inc_return(&next->context.attach_count) >> 16) {
-		/* Delay update_user_asce until all TLB flushes are done. */
-		set_tsk_thread_flag(tsk, TIF_TLB_WAIT);
-		/* Clear old ASCE by loading the kernel ASCE. */
-		clear_user_asce(next, 0);
-	} else {
-		cpumask_set_cpu(cpu, mm_cpumask(next));
-		update_user_asce(next, 0);
-		if (next->context.flush_mm)
-			/* Flush pending TLBs */
-			__tlb_flush_mm(next);
-	}
+	/* Clear old ASCE by loading the kernel ASCE. */
+	__ctl_load(S390_lowcore.kernel_asce, 1, 1);
+	__ctl_load(S390_lowcore.kernel_asce, 7, 7);
+	/* Delay loading of the new ASCE to control registers CR1 & CR7 */
+	set_cpu_flag(CIF_ASCE);
+	atomic_inc(&next->context.attach_count);
 	atomic_dec(&prev->context.attach_count);
-	WARN_ON(atomic_read(&prev->context.attach_count) < 0);
 	if (MACHINE_HAS_TLB_LC)
 		cpumask_clear_cpu(cpu, &prev->context.cpu_attach_mask);
 }
@@ -93,15 +83,14 @@ static inline void finish_arch_post_lock_switch(void)
 	struct task_struct *tsk = current;
 	struct mm_struct *mm = tsk->mm;
 
-	if (!test_tsk_thread_flag(tsk, TIF_TLB_WAIT))
+	if (!mm)
 		return;
 	preempt_disable();
-	clear_tsk_thread_flag(tsk, TIF_TLB_WAIT);
 	while (atomic_read(&mm->context.attach_count) >> 16)
 		cpu_relax();
 
 	cpumask_set_cpu(smp_processor_id(), mm_cpumask(mm));
-	update_user_asce(mm, 0);
+	set_user_asce(mm);
 	if (mm->context.flush_mm)
 		__tlb_flush_mm(mm);
 	preempt_enable();
@@ -113,7 +102,9 @@ static inline void finish_arch_post_lock_switch(void)
 static inline void activate_mm(struct mm_struct *prev,
                                struct mm_struct *next)
 {
-        switch_mm(prev, next, current);
+	switch_mm(prev, next, current);
+	cpumask_set_cpu(smp_processor_id(), mm_cpumask(next));
+	set_user_asce(next);
 }
 
 static inline void arch_dup_mmap(struct mm_struct *oldmm,
diff --git a/arch/s390/include/asm/pci.h b/arch/s390/include/asm/pci.h
index 2583466f576..c030900320e 100644
--- a/arch/s390/include/asm/pci.h
+++ b/arch/s390/include/asm/pci.h
@@ -78,10 +78,16 @@ struct zpci_dev {
 	enum zpci_state state;
 	u32		fid;		/* function ID, used by sclp */
 	u32		fh;		/* function handle, used by insn's */
+	u16		vfn;		/* virtual function number */
 	u16		pchid;		/* physical channel ID */
 	u8		pfgid;		/* function group ID */
+	u8		pft;		/* pci function type */
 	u16		domain;
 
+	u8 pfip[CLP_PFIP_NR_SEGMENTS];	/* pci function internal path */
+	u32 uid;			/* user defined id */
+	u8 util_str[CLP_UTIL_STR_LEN];	/* utility string */
+
 	/* IRQ stuff */
 	u64		msi_addr;	/* MSI address */
 	struct airq_iv *aibv;		/* adapter interrupt bit vector */
@@ -120,6 +126,8 @@ static inline bool zdev_enabled(struct zpci_dev *zdev)
 	return (zdev->fh & (1UL << 31)) ? true : false;
 }
 
+extern const struct attribute_group *zpci_attr_groups[];
+
 /* -----------------------------------------------------------------------------
   Prototypes
 ----------------------------------------------------------------------------- */
@@ -166,10 +174,6 @@ static inline void zpci_exit_slot(struct zpci_dev *zdev) {}
 struct zpci_dev *get_zdev(struct pci_dev *);
 struct zpci_dev *get_zdev_by_fid(u32);
 
-/* sysfs */
-int zpci_sysfs_add_device(struct device *);
-void zpci_sysfs_remove_device(struct device *);
-
 /* DMA */
 int zpci_dma_init(void);
 void zpci_dma_exit(void);
diff --git a/arch/s390/include/asm/pci_clp.h b/arch/s390/include/asm/pci_clp.h
index d31d739f868..dd78f92f1cc 100644
--- a/arch/s390/include/asm/pci_clp.h
+++ b/arch/s390/include/asm/pci_clp.h
@@ -44,6 +44,7 @@ struct clp_fh_list_entry {
 #define CLP_SET_DISABLE_PCI_FN	1	/* Yes, 1 disables it */
 
 #define CLP_UTIL_STR_LEN	64
+#define CLP_PFIP_NR_SEGMENTS	4
 
 /* List PCI functions request */
 struct clp_req_list_pci {
@@ -85,7 +86,7 @@ struct clp_rsp_query_pci {
 	struct clp_rsp_hdr hdr;
 	u32 fmt			:  4;	/* cmd request block format */
 	u32			: 28;
-	u64 reserved1;
+	u64			: 64;
 	u16 vfn;			/* virtual fn number */
 	u16			:  7;
 	u16 util_str_avail	:  1;	/* utility string available? */
@@ -94,10 +95,13 @@ struct clp_rsp_query_pci {
 	u8 bar_size[PCI_BAR_COUNT];
 	u16 pchid;
 	u32 bar[PCI_BAR_COUNT];
-	u64 reserved2;
+	u8 pfip[CLP_PFIP_NR_SEGMENTS];	/* pci function internal path */
+	u32			: 24;
+	u8 pft;				/* pci function type */
 	u64 sdma;			/* start dma as */
 	u64 edma;			/* end dma as */
-	u64 reserved3[6];
+	u32 reserved[11];
+	u32 uid;			/* user defined id */
 	u8 util_str[CLP_UTIL_STR_LEN];	/* utility string */
 } __packed;
 
diff --git a/arch/s390/include/asm/processor.h b/arch/s390/include/asm/processor.h
index dc5fc4f90e5..6f02d452bbe 100644
--- a/arch/s390/include/asm/processor.h
+++ b/arch/s390/include/asm/processor.h
@@ -11,6 +11,13 @@
 #ifndef __ASM_S390_PROCESSOR_H
 #define __ASM_S390_PROCESSOR_H
 
+#define CIF_MCCK_PENDING	0	/* machine check handling is pending */
+#define CIF_ASCE		1	/* user asce needs fixup / uaccess */
+
+#define _CIF_MCCK_PENDING	(1<<CIF_MCCK_PENDING)
+#define _CIF_ASCE		(1<<CIF_ASCE)
+
+
 #ifndef __ASSEMBLY__
 
 #include <linux/linkage.h>
@@ -21,6 +28,21 @@
 #include <asm/setup.h>
 #include <asm/runtime_instr.h>
 
+static inline void set_cpu_flag(int flag)
+{
+	S390_lowcore.cpu_flags |= (1U << flag);
+}
+
+static inline void clear_cpu_flag(int flag)
+{
+	S390_lowcore.cpu_flags &= ~(1U << flag);
+}
+
+static inline int test_cpu_flag(int flag)
+{
+	return !!(S390_lowcore.cpu_flags & (1U << flag));
+}
+
 /*
  * Default implementation of macro that returns current
  * instruction pointer ("program counter").
diff --git a/arch/s390/include/asm/ptrace.h b/arch/s390/include/asm/ptrace.h
index f4783c0b7b4..1b5300cd6d2 100644
--- a/arch/s390/include/asm/ptrace.h
+++ b/arch/s390/include/asm/ptrace.h
@@ -8,6 +8,12 @@
 
 #include <uapi/asm/ptrace.h>
 
+#define PIF_SYSCALL		0	/* inside a system call */
+#define PIF_PER_TRAP		1	/* deliver sigtrap on return to user */
+
+#define _PIF_SYSCALL		(1<<PIF_SYSCALL)
+#define _PIF_PER_TRAP		(1<<PIF_PER_TRAP)
+
 #ifndef __ASSEMBLY__
 
 #define PSW_KERNEL_BITS	(PSW_DEFAULT_KEY | PSW_MASK_BASE | PSW_ASC_HOME | \
@@ -29,6 +35,7 @@ struct pt_regs
 	unsigned int int_code;
 	unsigned int int_parm;
 	unsigned long int_parm_long;
+	unsigned long flags;
 };
 
 /*
@@ -79,6 +86,21 @@ struct per_struct_kernel {
 #define PER_CONTROL_SUSPENSION		0x00400000UL
 #define PER_CONTROL_ALTERATION		0x00200000UL
 
+static inline void set_pt_regs_flag(struct pt_regs *regs, int flag)
+{
+	regs->flags |= (1U << flag);
+}
+
+static inline void clear_pt_regs_flag(struct pt_regs *regs, int flag)
+{
+	regs->flags &= ~(1U << flag);
+}
+
+static inline int test_pt_regs_flag(struct pt_regs *regs, int flag)
+{
+	return !!(regs->flags & (1U << flag));
+}
+
 /*
  * These are defined as per linux/ptrace.h, which see.
  */
diff --git a/arch/s390/include/asm/setup.h b/arch/s390/include/asm/setup.h
index b31b22dba94..089a49814c5 100644
--- a/arch/s390/include/asm/setup.h
+++ b/arch/s390/include/asm/setup.h
@@ -9,7 +9,6 @@
 
 
 #define PARMAREA		0x10400
-#define MEMORY_CHUNKS		256
 
 #ifndef __ASSEMBLY__
 
@@ -31,22 +30,11 @@
 #endif /* CONFIG_64BIT */
 #define COMMAND_LINE      ((char *)            (0x10480))
 
-#define CHUNK_READ_WRITE 0
-#define CHUNK_READ_ONLY  1
-
-struct mem_chunk {
-	unsigned long addr;
-	unsigned long size;
-	int type;
-};
-
-extern struct mem_chunk memory_chunk[];
 extern int memory_end_set;
 extern unsigned long memory_end;
+extern unsigned long max_physmem_end;
 
-void detect_memory_layout(struct mem_chunk chunk[], unsigned long maxsize);
-void create_mem_hole(struct mem_chunk mem_chunk[], unsigned long addr,
-		     unsigned long size);
+extern void detect_memory_memblock(void);
 
 /*
  * Machine features detected in head.S
diff --git a/arch/s390/include/asm/smp.h b/arch/s390/include/asm/smp.h
index 21703f85b48..4f1307962a9 100644
--- a/arch/s390/include/asm/smp.h
+++ b/arch/s390/include/asm/smp.h
@@ -30,7 +30,6 @@ extern int smp_store_status(int cpu);
 extern int smp_vcpu_scheduled(int cpu);
 extern void smp_yield_cpu(int cpu);
 extern void smp_yield(void);
-extern void smp_stop_cpu(void);
 extern void smp_cpu_set_polarization(int cpu, int val);
 extern int smp_cpu_get_polarization(int cpu);
 extern void smp_fill_possible_mask(void);
@@ -54,6 +53,8 @@ static inline void smp_yield_cpu(int cpu) { }
 static inline void smp_yield(void) { }
 static inline void smp_fill_possible_mask(void) { }
 
+#endif /* CONFIG_SMP */
+
 static inline void smp_stop_cpu(void)
 {
 	u16 pcpu = stap();
@@ -64,8 +65,6 @@ static inline void smp_stop_cpu(void)
 	}
 }
 
-#endif /* CONFIG_SMP */
-
 #ifdef CONFIG_HOTPLUG_CPU
 extern int smp_rescan_cpus(void);
 extern void __noreturn cpu_die(void);
diff --git a/arch/s390/include/asm/spinlock.h b/arch/s390/include/asm/spinlock.h
index 83e5d216105..96879f7ad6d 100644
--- a/arch/s390/include/asm/spinlock.h
+++ b/arch/s390/include/asm/spinlock.h
@@ -11,18 +11,21 @@
 
 #include <linux/smp.h>
 
+#define SPINLOCK_LOCKVAL (S390_lowcore.spinlock_lockval)
+
 extern int spin_retry;
 
 static inline int
-_raw_compare_and_swap(volatile unsigned int *lock,
-		      unsigned int old, unsigned int new)
+_raw_compare_and_swap(unsigned int *lock, unsigned int old, unsigned int new)
 {
+	unsigned int old_expected = old;
+
 	asm volatile(
 		"	cs	%0,%3,%1"
 		: "=d" (old), "=Q" (*lock)
 		: "0" (old), "d" (new), "Q" (*lock)
 		: "cc", "memory" );
-	return old;
+	return old == old_expected;
 }
 
 /*
@@ -34,57 +37,69 @@ _raw_compare_and_swap(volatile unsigned int *lock,
  * (the type definitions are in asm/spinlock_types.h)
  */
 
-#define arch_spin_is_locked(x) ((x)->owner_cpu != 0)
-#define arch_spin_unlock_wait(lock) \
-	do { while (arch_spin_is_locked(lock)) \
-		 arch_spin_relax(lock); } while (0)
+void arch_spin_lock_wait(arch_spinlock_t *);
+int arch_spin_trylock_retry(arch_spinlock_t *);
+void arch_spin_relax(arch_spinlock_t *);
+void arch_spin_lock_wait_flags(arch_spinlock_t *, unsigned long flags);
 
-extern void arch_spin_lock_wait(arch_spinlock_t *);
-extern void arch_spin_lock_wait_flags(arch_spinlock_t *, unsigned long flags);
-extern int arch_spin_trylock_retry(arch_spinlock_t *);
-extern void arch_spin_relax(arch_spinlock_t *lock);
+static inline u32 arch_spin_lockval(int cpu)
+{
+	return ~cpu;
+}
 
 static inline int arch_spin_value_unlocked(arch_spinlock_t lock)
 {
-	return lock.owner_cpu == 0;
+	return lock.lock == 0;
 }
 
-static inline void arch_spin_lock(arch_spinlock_t *lp)
+static inline int arch_spin_is_locked(arch_spinlock_t *lp)
 {
-	int old;
+	return ACCESS_ONCE(lp->lock) != 0;
+}
 
-	old = _raw_compare_and_swap(&lp->owner_cpu, 0, ~smp_processor_id());
-	if (likely(old == 0))
-		return;
-	arch_spin_lock_wait(lp);
+static inline int arch_spin_trylock_once(arch_spinlock_t *lp)
+{
+	barrier();
+	return likely(arch_spin_value_unlocked(*lp) &&
+		      _raw_compare_and_swap(&lp->lock, 0, SPINLOCK_LOCKVAL));
 }
 
-static inline void arch_spin_lock_flags(arch_spinlock_t *lp,
-					 unsigned long flags)
+static inline int arch_spin_tryrelease_once(arch_spinlock_t *lp)
 {
-	int old;
+	return _raw_compare_and_swap(&lp->lock, SPINLOCK_LOCKVAL, 0);
+}
 
-	old = _raw_compare_and_swap(&lp->owner_cpu, 0, ~smp_processor_id());
-	if (likely(old == 0))
-		return;
-	arch_spin_lock_wait_flags(lp, flags);
+static inline void arch_spin_lock(arch_spinlock_t *lp)
+{
+	if (!arch_spin_trylock_once(lp))
+		arch_spin_lock_wait(lp);
 }
 
-static inline int arch_spin_trylock(arch_spinlock_t *lp)
+static inline void arch_spin_lock_flags(arch_spinlock_t *lp,
+					unsigned long flags)
 {
-	int old;
+	if (!arch_spin_trylock_once(lp))
+		arch_spin_lock_wait_flags(lp, flags);
+}
 
-	old = _raw_compare_and_swap(&lp->owner_cpu, 0, ~smp_processor_id());
-	if (likely(old == 0))
-		return 1;
-	return arch_spin_trylock_retry(lp);
+static inline int arch_spin_trylock(arch_spinlock_t *lp)
+{
+	if (!arch_spin_trylock_once(lp))
+		return arch_spin_trylock_retry(lp);
+	return 1;
 }
 
 static inline void arch_spin_unlock(arch_spinlock_t *lp)
 {
-	_raw_compare_and_swap(&lp->owner_cpu, lp->owner_cpu, 0);
+	arch_spin_tryrelease_once(lp);
+}
+
+static inline void arch_spin_unlock_wait(arch_spinlock_t *lock)
+{
+	while (arch_spin_is_locked(lock))
+		arch_spin_relax(lock);
 }
-		
+
 /*
  * Read-write spinlocks, allowing multiple readers
  * but only one writer.
@@ -115,42 +130,50 @@ extern void _raw_write_lock_wait(arch_rwlock_t *lp);
 extern void _raw_write_lock_wait_flags(arch_rwlock_t *lp, unsigned long flags);
 extern int _raw_write_trylock_retry(arch_rwlock_t *lp);
 
+static inline int arch_read_trylock_once(arch_rwlock_t *rw)
+{
+	unsigned int old = ACCESS_ONCE(rw->lock);
+	return likely((int) old >= 0 &&
+		      _raw_compare_and_swap(&rw->lock, old, old + 1));
+}
+
+static inline int arch_write_trylock_once(arch_rwlock_t *rw)
+{
+	unsigned int old = ACCESS_ONCE(rw->lock);
+	return likely(old == 0 &&
+		      _raw_compare_and_swap(&rw->lock, 0, 0x80000000));
+}
+
 static inline void arch_read_lock(arch_rwlock_t *rw)
 {
-	unsigned int old;
-	old = rw->lock & 0x7fffffffU;
-	if (_raw_compare_and_swap(&rw->lock, old, old + 1) != old)
+	if (!arch_read_trylock_once(rw))
 		_raw_read_lock_wait(rw);
 }
 
 static inline void arch_read_lock_flags(arch_rwlock_t *rw, unsigned long flags)
 {
-	unsigned int old;
-	old = rw->lock & 0x7fffffffU;
-	if (_raw_compare_and_swap(&rw->lock, old, old + 1) != old)
+	if (!arch_read_trylock_once(rw))
 		_raw_read_lock_wait_flags(rw, flags);
 }
 
 static inline void arch_read_unlock(arch_rwlock_t *rw)
 {
-	unsigned int old, cmp;
+	unsigned int old;
 
-	old = rw->lock;
 	do {
-		cmp = old;
-		old = _raw_compare_and_swap(&rw->lock, old, old - 1);
-	} while (cmp != old);
+		old = ACCESS_ONCE(rw->lock);
+	} while (!_raw_compare_and_swap(&rw->lock, old, old - 1));
 }
 
 static inline void arch_write_lock(arch_rwlock_t *rw)
 {
-	if (unlikely(_raw_compare_and_swap(&rw->lock, 0, 0x80000000) != 0))
+	if (!arch_write_trylock_once(rw))
 		_raw_write_lock_wait(rw);
 }
 
 static inline void arch_write_lock_flags(arch_rwlock_t *rw, unsigned long flags)
 {
-	if (unlikely(_raw_compare_and_swap(&rw->lock, 0, 0x80000000) != 0))
+	if (!arch_write_trylock_once(rw))
 		_raw_write_lock_wait_flags(rw, flags);
 }
 
@@ -161,18 +184,16 @@ static inline void arch_write_unlock(arch_rwlock_t *rw)
 
 static inline int arch_read_trylock(arch_rwlock_t *rw)
 {
-	unsigned int old;
-	old = rw->lock & 0x7fffffffU;
-	if (likely(_raw_compare_and_swap(&rw->lock, old, old + 1) == old))
-		return 1;
-	return _raw_read_trylock_retry(rw);
+	if (!arch_read_trylock_once(rw))
+		return _raw_read_trylock_retry(rw);
+	return 1;
 }
 
 static inline int arch_write_trylock(arch_rwlock_t *rw)
 {
-	if (likely(_raw_compare_and_swap(&rw->lock, 0, 0x80000000) == 0))
-		return 1;
-	return _raw_write_trylock_retry(rw);
+	if (!arch_write_trylock_once(rw))
+		return _raw_write_trylock_retry(rw);
+	return 1;
 }
 
 #define arch_read_relax(lock)	cpu_relax()
diff --git a/arch/s390/include/asm/spinlock_types.h b/arch/s390/include/asm/spinlock_types.h
index 9c76656a0af..b2cd6ff7c2c 100644
--- a/arch/s390/include/asm/spinlock_types.h
+++ b/arch/s390/include/asm/spinlock_types.h
@@ -6,13 +6,13 @@
 #endif
 
 typedef struct {
-	volatile unsigned int owner_cpu;
+	unsigned int lock;
 } __attribute__ ((aligned (4))) arch_spinlock_t;
 
-#define __ARCH_SPIN_LOCK_UNLOCKED	{ 0 }
+#define __ARCH_SPIN_LOCK_UNLOCKED { .lock = 0, }
 
 typedef struct {
-	volatile unsigned int lock;
+	unsigned int lock;
 } arch_rwlock_t;
 
 #define __ARCH_RW_LOCK_UNLOCKED		{ 0 }
diff --git a/arch/s390/include/asm/switch_to.h b/arch/s390/include/asm/switch_to.h
index e759181357f..29c81f82705 100644
--- a/arch/s390/include/asm/switch_to.h
+++ b/arch/s390/include/asm/switch_to.h
@@ -132,7 +132,6 @@ static inline void restore_access_regs(unsigned int *acrs)
 		update_cr_regs(next);					\
 	}								\
 	prev = __switch_to(prev,next);					\
-	update_primary_asce(current);					\
 } while (0)
 
 #define finish_arch_switch(prev) do {					     \
diff --git a/arch/s390/include/asm/syscall.h b/arch/s390/include/asm/syscall.h
index 777687055e7..abad78d5b10 100644
--- a/arch/s390/include/asm/syscall.h
+++ b/arch/s390/include/asm/syscall.h
@@ -28,7 +28,7 @@ extern const unsigned int sys_call_table_emu[];
 static inline long syscall_get_nr(struct task_struct *task,
 				  struct pt_regs *regs)
 {
-	return test_tsk_thread_flag(task, TIF_SYSCALL) ?
+	return test_pt_regs_flag(regs, PIF_SYSCALL) ?
 		(regs->int_code & 0xffff) : -1;
 }
 
diff --git a/arch/s390/include/asm/thread_info.h b/arch/s390/include/asm/thread_info.h
index 50630e6a35d..b833e9c0bfb 100644
--- a/arch/s390/include/asm/thread_info.h
+++ b/arch/s390/include/asm/thread_info.h
@@ -77,32 +77,22 @@ static inline struct thread_info *current_thread_info(void)
 /*
  * thread information flags bit numbers
  */
-#define TIF_SYSCALL		0	/* inside a system call */
-#define TIF_NOTIFY_RESUME	1	/* callback before returning to user */
-#define TIF_SIGPENDING		2	/* signal pending */
-#define TIF_NEED_RESCHED	3	/* rescheduling necessary */
-#define TIF_TLB_WAIT		4	/* wait for TLB flush completion */
-#define TIF_ASCE		5	/* primary asce needs fixup / uaccess */
-#define TIF_PER_TRAP		6	/* deliver sigtrap on return to user */
-#define TIF_MCCK_PENDING	7	/* machine check handling is pending */
-#define TIF_SYSCALL_TRACE	8	/* syscall trace active */
-#define TIF_SYSCALL_AUDIT	9	/* syscall auditing active */
-#define TIF_SECCOMP		10	/* secure computing */
-#define TIF_SYSCALL_TRACEPOINT	11	/* syscall tracepoint instrumentation */
-#define TIF_31BIT		17	/* 32bit process */
-#define TIF_MEMDIE		18	/* is terminating due to OOM killer */
-#define TIF_RESTORE_SIGMASK	19	/* restore signal mask in do_signal() */
-#define TIF_SINGLE_STEP		20	/* This task is single stepped */
-#define TIF_BLOCK_STEP		21	/* This task is block stepped */
+#define TIF_NOTIFY_RESUME	0	/* callback before returning to user */
+#define TIF_SIGPENDING		1	/* signal pending */
+#define TIF_NEED_RESCHED	2	/* rescheduling necessary */
+#define TIF_SYSCALL_TRACE	3	/* syscall trace active */
+#define TIF_SYSCALL_AUDIT	4	/* syscall auditing active */
+#define TIF_SECCOMP		5	/* secure computing */
+#define TIF_SYSCALL_TRACEPOINT	6	/* syscall tracepoint instrumentation */
+#define TIF_31BIT		16	/* 32bit process */
+#define TIF_MEMDIE		17	/* is terminating due to OOM killer */
+#define TIF_RESTORE_SIGMASK	18	/* restore signal mask in do_signal() */
+#define TIF_SINGLE_STEP		19	/* This task is single stepped */
+#define TIF_BLOCK_STEP		20	/* This task is block stepped */
 
-#define _TIF_SYSCALL		(1<<TIF_SYSCALL)
 #define _TIF_NOTIFY_RESUME	(1<<TIF_NOTIFY_RESUME)
 #define _TIF_SIGPENDING		(1<<TIF_SIGPENDING)
 #define _TIF_NEED_RESCHED	(1<<TIF_NEED_RESCHED)
-#define _TIF_TLB_WAIT		(1<<TIF_TLB_WAIT)
-#define _TIF_ASCE		(1<<TIF_ASCE)
-#define _TIF_PER_TRAP		(1<<TIF_PER_TRAP)
-#define _TIF_MCCK_PENDING	(1<<TIF_MCCK_PENDING)
 #define _TIF_SYSCALL_TRACE	(1<<TIF_SYSCALL_TRACE)
 #define _TIF_SYSCALL_AUDIT	(1<<TIF_SYSCALL_AUDIT)
 #define _TIF_SECCOMP		(1<<TIF_SECCOMP)
diff --git a/arch/s390/include/asm/tlb.h b/arch/s390/include/asm/tlb.h
index c544b6f05d9..a25f09fbaf3 100644
--- a/arch/s390/include/asm/tlb.h
+++ b/arch/s390/include/asm/tlb.h
@@ -59,12 +59,23 @@ static inline void tlb_gather_mmu(struct mmu_gather *tlb,
 	tlb->batch = NULL;
 }
 
-static inline void tlb_flush_mmu(struct mmu_gather *tlb)
+static inline void tlb_flush_mmu_tlbonly(struct mmu_gather *tlb)
 {
 	__tlb_flush_mm_lazy(tlb->mm);
+}
+
+static inline void tlb_flush_mmu_free(struct mmu_gather *tlb)
+{
 	tlb_table_flush(tlb);
 }
 
+
+static inline void tlb_flush_mmu(struct mmu_gather *tlb)
+{
+	tlb_flush_mmu_tlbonly(tlb);
+	tlb_flush_mmu_free(tlb);
+}
+
 static inline void tlb_finish_mmu(struct mmu_gather *tlb,
 				  unsigned long start, unsigned long end)
 {
diff --git a/arch/s390/include/asm/uaccess.h b/arch/s390/include/asm/uaccess.h
index 1be64a1506d..cd4c68e0398 100644
--- a/arch/s390/include/asm/uaccess.h
+++ b/arch/s390/include/asm/uaccess.h
@@ -132,6 +132,34 @@ unsigned long __must_check __copy_to_user(void __user *to, const void *from,
 #define __copy_to_user_inatomic __copy_to_user
 #define __copy_from_user_inatomic __copy_from_user
 
+#ifdef CONFIG_HAVE_MARCH_Z10_FEATURES
+
+#define __put_get_user_asm(to, from, size, spec)		\
+({								\
+	register unsigned long __reg0 asm("0") = spec;		\
+	int __rc;						\
+								\
+	asm volatile(						\
+		"0:	mvcos	%1,%3,%2\n"			\
+		"1:	xr	%0,%0\n"			\
+		"2:\n"						\
+		".pushsection .fixup, \"ax\"\n"			\
+		"3:	lhi	%0,%5\n"			\
+		"	jg	2b\n"				\
+		".popsection\n"					\
+		EX_TABLE(0b,3b) EX_TABLE(1b,3b)			\
+		: "=d" (__rc), "=Q" (*(to))			\
+		: "d" (size), "Q" (*(from)),			\
+		  "d" (__reg0), "K" (-EFAULT)			\
+		: "cc");					\
+	__rc;							\
+})
+
+#define __put_user_fn(x, ptr, size) __put_get_user_asm(ptr, x, size, 0x810000UL)
+#define __get_user_fn(x, ptr, size) __put_get_user_asm(x, ptr, size, 0x81UL)
+
+#else /* CONFIG_HAVE_MARCH_Z10_FEATURES */
+
 static inline int __put_user_fn(void *x, void __user *ptr, unsigned long size)
 {
 	size = __copy_to_user(ptr, x, size);
@@ -144,6 +172,8 @@ static inline int __get_user_fn(void *x, const void __user *ptr, unsigned long s
 	return size ? -EFAULT : 0;
 }
 
+#endif /* CONFIG_HAVE_MARCH_Z10_FEATURES */
+
 /*
  * These are the main single-value transfer routines.  They automatically
  * use the right size if we just have the right pointer type.
diff --git a/arch/s390/kernel/asm-offsets.c b/arch/s390/kernel/asm-offsets.c
index cc10cdd4d6a..0c070c44cde 100644
--- a/arch/s390/kernel/asm-offsets.c
+++ b/arch/s390/kernel/asm-offsets.c
@@ -50,6 +50,7 @@ int main(void)
 	DEFINE(__PT_INT_CODE, offsetof(struct pt_regs, int_code));
 	DEFINE(__PT_INT_PARM, offsetof(struct pt_regs, int_parm));
 	DEFINE(__PT_INT_PARM_LONG, offsetof(struct pt_regs, int_parm_long));
+	DEFINE(__PT_FLAGS, offsetof(struct pt_regs, flags));
 	DEFINE(__PT_SIZE, sizeof(struct pt_regs));
 	BLANK();
 	DEFINE(__SF_BACKCHAIN, offsetof(struct stack_frame, back_chain));
@@ -115,6 +116,7 @@ int main(void)
 	DEFINE(__LC_SAVE_AREA_SYNC, offsetof(struct _lowcore, save_area_sync));
 	DEFINE(__LC_SAVE_AREA_ASYNC, offsetof(struct _lowcore, save_area_async));
 	DEFINE(__LC_SAVE_AREA_RESTART, offsetof(struct _lowcore, save_area_restart));
+	DEFINE(__LC_CPU_FLAGS, offsetof(struct _lowcore, cpu_flags));
 	DEFINE(__LC_RETURN_PSW, offsetof(struct _lowcore, return_psw));
 	DEFINE(__LC_RETURN_MCCK_PSW, offsetof(struct _lowcore, return_mcck_psw));
 	DEFINE(__LC_SYNC_ENTER_TIMER, offsetof(struct _lowcore, sync_enter_timer));
@@ -142,7 +144,6 @@ int main(void)
 	DEFINE(__LC_MCCK_CLOCK, offsetof(struct _lowcore, mcck_clock));
 	DEFINE(__LC_MACHINE_FLAGS, offsetof(struct _lowcore, machine_flags));
 	DEFINE(__LC_FTRACE_FUNC, offsetof(struct _lowcore, ftrace_func));
-	DEFINE(__LC_IRB, offsetof(struct _lowcore, irb));
 	DEFINE(__LC_DUMP_REIPL, offsetof(struct _lowcore, ipib));
 	BLANK();
 	DEFINE(__LC_CPU_TIMER_SAVE_AREA, offsetof(struct _lowcore, cpu_timer_save_area));
diff --git a/arch/s390/kernel/compat_signal.c b/arch/s390/kernel/compat_signal.c
index 7df5ed9f44d..f204d692036 100644
--- a/arch/s390/kernel/compat_signal.c
+++ b/arch/s390/kernel/compat_signal.c
@@ -213,7 +213,7 @@ static int restore_sigregs32(struct pt_regs *regs,_sigregs32 __user *sregs)
 	       sizeof(current->thread.fp_regs));
 
 	restore_fp_regs(current->thread.fp_regs.fprs);
-	clear_thread_flag(TIF_SYSCALL);	/* No longer in a system call */
+	clear_pt_regs_flag(regs, PIF_SYSCALL); /* No longer in a system call */
 	return 0;
 }
 
diff --git a/arch/s390/kernel/crash_dump.c b/arch/s390/kernel/crash_dump.c
index d7658c4b2ed..a3b9150e680 100644
--- a/arch/s390/kernel/crash_dump.c
+++ b/arch/s390/kernel/crash_dump.c
@@ -13,6 +13,7 @@
 #include <linux/slab.h>
 #include <linux/bootmem.h>
 #include <linux/elf.h>
+#include <linux/memblock.h>
 #include <asm/os_info.h>
 #include <asm/elf.h>
 #include <asm/ipl.h>
@@ -22,6 +23,24 @@
 #define PTR_SUB(x, y) (((char *) (x)) - ((unsigned long) (y)))
 #define PTR_DIFF(x, y) ((unsigned long)(((char *) (x)) - ((unsigned long) (y))))
 
+static struct memblock_region oldmem_region;
+
+static struct memblock_type oldmem_type = {
+	.cnt = 1,
+	.max = 1,
+	.total_size = 0,
+	.regions = &oldmem_region,
+};
+
+#define for_each_dump_mem_range(i, nid, p_start, p_end, p_nid)		\
+	for (i = 0, __next_mem_range(&i, nid, &memblock.physmem,	\
+				     &oldmem_type, p_start,		\
+				     p_end, p_nid);			\
+	     i != (u64)ULLONG_MAX;					\
+	     __next_mem_range(&i, nid, &memblock.physmem,		\
+			      &oldmem_type,				\
+			      p_start, p_end, p_nid))
+
 struct dump_save_areas dump_save_areas;
 
 /*
@@ -264,19 +283,6 @@ static void *kzalloc_panic(int len)
 }
 
 /*
- * Get memory layout and create hole for oldmem
- */
-static struct mem_chunk *get_memory_layout(void)
-{
-	struct mem_chunk *chunk_array;
-
-	chunk_array = kzalloc_panic(MEMORY_CHUNKS * sizeof(struct mem_chunk));
-	detect_memory_layout(chunk_array, 0);
-	create_mem_hole(chunk_array, OLDMEM_BASE, OLDMEM_SIZE);
-	return chunk_array;
-}
-
-/*
  * Initialize ELF note
  */
 static void *nt_init(void *buf, Elf64_Word type, void *desc, int d_len,
@@ -490,52 +496,33 @@ static int get_cpu_cnt(void)
  */
 static int get_mem_chunk_cnt(void)
 {
-	struct mem_chunk *chunk_array, *mem_chunk;
-	int i, cnt = 0;
+	int cnt = 0;
+	u64 idx;
 
-	chunk_array = get_memory_layout();
-	for (i = 0; i < MEMORY_CHUNKS; i++) {
-		mem_chunk = &chunk_array[i];
-		if (chunk_array[i].type != CHUNK_READ_WRITE &&
-		    chunk_array[i].type != CHUNK_READ_ONLY)
-			continue;
-		if (mem_chunk->size == 0)
-			continue;
+	for_each_dump_mem_range(idx, NUMA_NO_NODE, NULL, NULL, NULL)
 		cnt++;
-	}
-	kfree(chunk_array);
 	return cnt;
 }
 
 /*
  * Initialize ELF loads (new kernel)
  */
-static int loads_init(Elf64_Phdr *phdr, u64 loads_offset)
+static void loads_init(Elf64_Phdr *phdr, u64 loads_offset)
 {
-	struct mem_chunk *chunk_array, *mem_chunk;
-	int i;
+	phys_addr_t start, end;
+	u64 idx;
 
-	chunk_array = get_memory_layout();
-	for (i = 0; i < MEMORY_CHUNKS; i++) {
-		mem_chunk = &chunk_array[i];
-		if (mem_chunk->size == 0)
-			continue;
-		if (chunk_array[i].type != CHUNK_READ_WRITE &&
-		    chunk_array[i].type != CHUNK_READ_ONLY)
-			continue;
-		else
-			phdr->p_filesz = mem_chunk->size;
+	for_each_dump_mem_range(idx, NUMA_NO_NODE, &start, &end, NULL) {
+		phdr->p_filesz = end - start;
 		phdr->p_type = PT_LOAD;
-		phdr->p_offset = mem_chunk->addr;
-		phdr->p_vaddr = mem_chunk->addr;
-		phdr->p_paddr = mem_chunk->addr;
-		phdr->p_memsz = mem_chunk->size;
+		phdr->p_offset = start;
+		phdr->p_vaddr = start;
+		phdr->p_paddr = start;
+		phdr->p_memsz = end - start;
 		phdr->p_flags = PF_R | PF_W | PF_X;
 		phdr->p_align = PAGE_SIZE;
 		phdr++;
 	}
-	kfree(chunk_array);
-	return i;
 }
 
 /*
@@ -584,6 +571,14 @@ int elfcorehdr_alloc(unsigned long long *addr, unsigned long long *size)
 	/* If we cannot get HSA size for zfcpdump return error */
 	if (ipl_info.type == IPL_TYPE_FCP_DUMP && !sclp_get_hsa_size())
 		return -ENODEV;
+
+	/* For kdump, exclude previous crashkernel memory */
+	if (OLDMEM_BASE) {
+		oldmem_region.base = OLDMEM_BASE;
+		oldmem_region.size = OLDMEM_SIZE;
+		oldmem_type.total_size = OLDMEM_SIZE;
+	}
+
 	mem_chunk_cnt = get_mem_chunk_cnt();
 
 	alloc_size = 0x1000 + get_cpu_cnt() * 0x300 +
diff --git a/arch/s390/kernel/early.c b/arch/s390/kernel/early.c
index a734f3585ce..0dff972a169 100644
--- a/arch/s390/kernel/early.c
+++ b/arch/s390/kernel/early.c
@@ -258,13 +258,19 @@ static __init void setup_topology(void)
 static void early_pgm_check_handler(void)
 {
 	const struct exception_table_entry *fixup;
+	unsigned long cr0, cr0_new;
 	unsigned long addr;
 
 	addr = S390_lowcore.program_old_psw.addr;
 	fixup = search_exception_tables(addr & PSW_ADDR_INSN);
 	if (!fixup)
 		disabled_wait(0);
+	/* Disable low address protection before storing into lowcore. */
+	__ctl_store(cr0, 0, 0);
+	cr0_new = cr0 & ~(1UL << 28);
+	__ctl_load(cr0_new, 0, 0);
 	S390_lowcore.program_old_psw.addr = extable_fixup(fixup)|PSW_ADDR_AMODE;
+	__ctl_load(cr0, 0, 0);
 }
 
 static noinline __init void setup_lowcore_early(void)
diff --git a/arch/s390/kernel/entry.S b/arch/s390/kernel/entry.S
index 1662038516c..18e5af848f9 100644
--- a/arch/s390/kernel/entry.S
+++ b/arch/s390/kernel/entry.S
@@ -10,6 +10,7 @@
 
 #include <linux/init.h>
 #include <linux/linkage.h>
+#include <asm/processor.h>
 #include <asm/cache.h>
 #include <asm/errno.h>
 #include <asm/ptrace.h>
@@ -37,18 +38,16 @@ __PT_R13     =	__PT_GPRS + 524
 __PT_R14     =	__PT_GPRS + 56
 __PT_R15     =	__PT_GPRS + 60
 
-_TIF_WORK_SVC = (_TIF_SIGPENDING | _TIF_NOTIFY_RESUME | _TIF_NEED_RESCHED | \
-		 _TIF_MCCK_PENDING | _TIF_PER_TRAP | _TIF_ASCE)
-_TIF_WORK_INT = (_TIF_SIGPENDING | _TIF_NOTIFY_RESUME | _TIF_NEED_RESCHED | \
-		 _TIF_MCCK_PENDING | _TIF_ASCE)
-_TIF_TRACE    = (_TIF_SYSCALL_TRACE | _TIF_SYSCALL_AUDIT | _TIF_SECCOMP | \
-		 _TIF_SYSCALL_TRACEPOINT)
-_TIF_TRANSFER = (_TIF_MCCK_PENDING | _TIF_TLB_WAIT)
-
 STACK_SHIFT = PAGE_SHIFT + THREAD_ORDER
 STACK_SIZE  = 1 << STACK_SHIFT
 STACK_INIT  = STACK_SIZE - STACK_FRAME_OVERHEAD - __PT_SIZE
 
+_TIF_WORK	= (_TIF_SIGPENDING | _TIF_NOTIFY_RESUME | _TIF_NEED_RESCHED)
+_TIF_TRACE	= (_TIF_SYSCALL_TRACE | _TIF_SYSCALL_AUDIT | _TIF_SECCOMP | \
+		   _TIF_SYSCALL_TRACEPOINT)
+_CIF_WORK	= (_CIF_MCCK_PENDING | _CIF_ASCE)
+_PIF_WORK	= (_PIF_PER_TRAP)
+
 #define BASED(name) name-system_call(%r13)
 
 	.macro	TRACE_IRQS_ON
@@ -160,13 +159,7 @@ ENTRY(__switch_to)
 	lctl	%c4,%c4,__TASK_pid(%r3)		# load pid to control reg. 4
 	mvc	__LC_CURRENT_PID(4,%r0),__TASK_pid(%r3)	# store pid of next
 	l	%r15,__THREAD_ksp(%r3)		# load kernel stack of next
-	lhi	%r6,_TIF_TRANSFER		# transfer TIF bits
-	n	%r6,__TI_flags(%r4)		# isolate TIF bits
-	jz	0f
-	o	%r6,__TI_flags(%r5)		# set TIF bits of next
-	st	%r6,__TI_flags(%r5)
-	ni	__TI_flags+3(%r4),255-_TIF_TRANSFER # clear TIF bits of prev
-0:	lm	%r6,%r15,__SF_GPRS(%r15)	# load gprs of next task
+	lm	%r6,%r15,__SF_GPRS(%r15)	# load gprs of next task
 	br	%r14
 
 __critical_start:
@@ -181,6 +174,7 @@ sysc_stm:
 	stm	%r8,%r15,__LC_SAVE_AREA_SYNC
 	l	%r12,__LC_THREAD_INFO
 	l	%r13,__LC_SVC_NEW_PSW+4
+	lhi	%r14,_PIF_SYSCALL
 sysc_per:
 	l	%r15,__LC_KERNEL_STACK
 	la	%r11,STACK_FRAME_OVERHEAD(%r15)	# pointer to pt_regs
@@ -190,8 +184,8 @@ sysc_vtime:
 	mvc	__PT_R8(32,%r11),__LC_SAVE_AREA_SYNC
 	mvc	__PT_PSW(8,%r11),__LC_SVC_OLD_PSW
 	mvc	__PT_INT_CODE(4,%r11),__LC_SVC_ILC
+	st	%r14,__PT_FLAGS(%r11)
 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
@@ -207,7 +201,7 @@ sysc_nr_ok:
 	st	%r2,__PT_ORIG_GPR2(%r11)
 	st	%r7,STACK_FRAME_OVERHEAD(%r15)
 	l	%r9,0(%r8,%r10)			# get system call addr.
-	tm	__TI_flags+2(%r12),_TIF_TRACE >> 8
+	tm	__TI_flags+3(%r12),_TIF_TRACE
 	jnz	sysc_tracesys
 	basr	%r14,%r9			# call sys_xxxx
 	st	%r2,__PT_R2(%r11)		# store return value
@@ -217,9 +211,12 @@ sysc_return:
 sysc_tif:
 	tm	__PT_PSW+1(%r11),0x01		# returning to user ?
 	jno	sysc_restore
-	tm	__TI_flags+3(%r12),_TIF_WORK_SVC
-	jnz	sysc_work			# check for work
-	ni	__TI_flags+3(%r12),255-_TIF_SYSCALL
+	tm	__PT_FLAGS+3(%r11),_PIF_WORK
+	jnz	sysc_work
+	tm	__TI_flags+3(%r12),_TIF_WORK
+	jnz	sysc_work			# check for thread work
+	tm	__LC_CPU_FLAGS+3,_CIF_WORK
+	jnz	sysc_work
 sysc_restore:
 	mvc	__LC_RETURN_PSW(8),__PT_PSW(%r11)
 	stpt	__LC_EXIT_TIMER
@@ -231,17 +228,17 @@ sysc_done:
 # One of the work bits is on. Find out which one.
 #
 sysc_work:
-	tm	__TI_flags+3(%r12),_TIF_MCCK_PENDING
+	tm	__LC_CPU_FLAGS+3,_CIF_MCCK_PENDING
 	jo	sysc_mcck_pending
 	tm	__TI_flags+3(%r12),_TIF_NEED_RESCHED
 	jo	sysc_reschedule
-	tm	__TI_flags+3(%r12),_TIF_PER_TRAP
+	tm	__PT_FLAGS+3(%r11),_PIF_PER_TRAP
 	jo	sysc_singlestep
 	tm	__TI_flags+3(%r12),_TIF_SIGPENDING
 	jo	sysc_sigpending
 	tm	__TI_flags+3(%r12),_TIF_NOTIFY_RESUME
 	jo	sysc_notify_resume
-	tm	__TI_flags+3(%r12),_TIF_ASCE
+	tm	__LC_CPU_FLAGS+3,_CIF_ASCE
 	jo	sysc_uaccess
 	j	sysc_return		# beware of critical section cleanup
 
@@ -254,7 +251,7 @@ sysc_reschedule:
 	br	%r1			# call schedule
 
 #
-# _TIF_MCCK_PENDING is set, call handler
+# _CIF_MCCK_PENDING is set, call handler
 #
 sysc_mcck_pending:
 	l	%r1,BASED(.Lhandle_mcck)
@@ -262,10 +259,10 @@ sysc_mcck_pending:
 	br	%r1			# TIF bit will be cleared by handler
 
 #
-# _TIF_ASCE is set, load user space asce
+# _CIF_ASCE is set, load user space asce
 #
 sysc_uaccess:
-	ni	__TI_flags+3(%r12),255-_TIF_ASCE
+	ni	__LC_CPU_FLAGS+3,255-_CIF_ASCE
 	lctl	%c1,%c1,__LC_USER_ASCE	# load primary asce
 	j	sysc_return
 
@@ -276,7 +273,7 @@ sysc_sigpending:
 	lr	%r2,%r11		# pass pointer to pt_regs
 	l	%r1,BASED(.Ldo_signal)
 	basr	%r14,%r1		# call do_signal
-	tm	__TI_flags+3(%r12),_TIF_SYSCALL
+	tm	__PT_FLAGS+3(%r11),_PIF_SYSCALL
 	jno	sysc_return
 	lm	%r2,%r7,__PT_R2(%r11)	# load svc arguments
 	l	%r10,__TI_sysc_table(%r12)	# 31 bit system call table
@@ -297,10 +294,10 @@ sysc_notify_resume:
 	br	%r1			# call do_notify_resume
 
 #
-# _TIF_PER_TRAP is set, call do_per_trap
+# _PIF_PER_TRAP is set, call do_per_trap
 #
 sysc_singlestep:
-	ni	__TI_flags+3(%r12),255-_TIF_PER_TRAP
+	ni	__PT_FLAGS+3(%r11),255-_PIF_PER_TRAP
 	lr	%r2,%r11		# pass pointer to pt_regs
 	l	%r1,BASED(.Ldo_per_trap)
 	la	%r14,BASED(sysc_return)
@@ -330,7 +327,7 @@ sysc_tracego:
 	basr	%r14,%r9		# call sys_xxx
 	st	%r2,__PT_R2(%r11)	# store return value
 sysc_tracenogo:
-	tm	__TI_flags+2(%r12),_TIF_TRACE >> 8
+	tm	__TI_flags+3(%r12),_TIF_TRACE
 	jz	sysc_return
 	l	%r1,BASED(.Ltrace_exit)
 	lr	%r2,%r11		# pass pointer to pt_regs
@@ -384,12 +381,13 @@ ENTRY(pgm_check_handler)
 	stm	%r8,%r9,__PT_PSW(%r11)
 	mvc	__PT_INT_CODE(4,%r11),__LC_PGM_ILC
 	mvc	__PT_INT_PARM_LONG(4,%r11),__LC_TRANS_EXC_CODE
+	xc	__PT_FLAGS(4,%r11),__PT_FLAGS(%r11)
 	tm	__LC_PGM_ILC+3,0x80	# check for per exception
 	jz	0f
 	l	%r1,__TI_task(%r12)
 	tmh	%r8,0x0001		# kernel per event ?
 	jz	pgm_kprobe
-	oi	__TI_flags+3(%r12),_TIF_PER_TRAP
+	oi	__PT_FLAGS+3(%r11),_PIF_PER_TRAP
 	mvc	__THREAD_per_address(4,%r1),__LC_PER_ADDRESS
 	mvc	__THREAD_per_cause(2,%r1),__LC_PER_CAUSE
 	mvc	__THREAD_per_paid(1,%r1),__LC_PER_PAID
@@ -420,9 +418,9 @@ pgm_kprobe:
 # single stepped system call
 #
 pgm_svcper:
-	oi	__TI_flags+3(%r12),_TIF_PER_TRAP
 	mvc	__LC_RETURN_PSW(4),__LC_SVC_NEW_PSW
 	mvc	__LC_RETURN_PSW+4(4),BASED(.Lsysc_per)
+	lhi	%r14,_PIF_SYSCALL | _PIF_PER_TRAP
 	lpsw	__LC_RETURN_PSW		# branch to sysc_per and enable irqs
 
 /*
@@ -445,6 +443,7 @@ io_skip:
 	mvc	__PT_R8(32,%r11),__LC_SAVE_AREA_ASYNC
 	stm	%r8,%r9,__PT_PSW(%r11)
 	mvc	__PT_INT_CODE(12,%r11),__LC_SUBCHANNEL_ID
+	xc	__PT_FLAGS(4,%r11),__PT_FLAGS(%r11)
 	TRACE_IRQS_OFF
 	xc	__SF_BACKCHAIN(4,%r15),__SF_BACKCHAIN(%r15)
 io_loop:
@@ -466,8 +465,10 @@ io_return:
 	LOCKDEP_SYS_EXIT
 	TRACE_IRQS_ON
 io_tif:
-	tm	__TI_flags+3(%r12),_TIF_WORK_INT
+	tm	__TI_flags+3(%r12),_TIF_WORK
 	jnz	io_work			# there is work to do (signals etc.)
+	tm	__LC_CPU_FLAGS+3,_CIF_WORK
+	jnz	io_work
 io_restore:
 	mvc	__LC_RETURN_PSW(8),__PT_PSW(%r11)
 	stpt	__LC_EXIT_TIMER
@@ -477,7 +478,7 @@ io_done:
 
 #
 # There is work todo, find out in which context we have been interrupted:
-# 1) if we return to user space we can do all _TIF_WORK_INT work
+# 1) if we return to user space we can do all _TIF_WORK work
 # 2) if we return to kernel code and preemptive scheduling is enabled check
 #    the preemption counter and if it is zero call preempt_schedule_irq
 # Before any work can be done, a switch to the kernel stack is required.
@@ -520,11 +521,9 @@ io_work_user:
 
 #
 # One of the work bits is on. Find out which one.
-# Checked are: _TIF_SIGPENDING, _TIF_NOTIFY_RESUME, _TIF_NEED_RESCHED
-#		and _TIF_MCCK_PENDING
 #
 io_work_tif:
-	tm	__TI_flags+3(%r12),_TIF_MCCK_PENDING
+	tm	__LC_CPU_FLAGS+3(%r12),_CIF_MCCK_PENDING
 	jo	io_mcck_pending
 	tm	__TI_flags+3(%r12),_TIF_NEED_RESCHED
 	jo	io_reschedule
@@ -532,12 +531,12 @@ io_work_tif:
 	jo	io_sigpending
 	tm	__TI_flags+3(%r12),_TIF_NOTIFY_RESUME
 	jo	io_notify_resume
-	tm	__TI_flags+3(%r12),_TIF_ASCE
+	tm	__LC_CPU_FLAGS+3,_CIF_ASCE
 	jo	io_uaccess
 	j	io_return		# beware of critical section cleanup
 
 #
-# _TIF_MCCK_PENDING is set, call handler
+# _CIF_MCCK_PENDING is set, call handler
 #
 io_mcck_pending:
 	# TRACE_IRQS_ON already done at io_return
@@ -547,10 +546,10 @@ io_mcck_pending:
 	j	io_return
 
 #
-# _TIF_ASCE is set, load user space asce
+# _CIF_ASCE is set, load user space asce
 #
 io_uaccess:
-	ni	__TI_flags+3(%r12),255-_TIF_ASCE
+	ni	__LC_CPU_FLAGS+3,255-_CIF_ASCE
 	lctl	%c1,%c1,__LC_USER_ASCE	# load primary asce
 	j	io_return
 
@@ -613,6 +612,7 @@ ext_skip:
 	stm	%r8,%r9,__PT_PSW(%r11)
 	mvc	__PT_INT_CODE(4,%r11),__LC_EXT_CPU_ADDR
 	mvc	__PT_INT_PARM(4,%r11),__LC_EXT_PARAMS
+	xc	__PT_FLAGS(4,%r11),__PT_FLAGS(%r11)
 	TRACE_IRQS_OFF
 	l	%r1,BASED(.Ldo_IRQ)
 	lr	%r2,%r11		# pass pointer to pt_regs
@@ -677,6 +677,7 @@ mcck_skip:
 	stm	%r0,%r7,__PT_R0(%r11)
 	mvc	__PT_R8(32,%r11),__LC_GPREGS_SAVE_AREA+32
 	stm	%r8,%r9,__PT_PSW(%r11)
+	xc	__PT_FLAGS(4,%r11),__PT_FLAGS(%r11)
 	xc	__SF_BACKCHAIN(4,%r15),__SF_BACKCHAIN(%r15)
 	l	%r1,BASED(.Ldo_machine_check)
 	lr	%r2,%r11		# pass pointer to pt_regs
@@ -689,7 +690,7 @@ mcck_skip:
 	la	%r11,STACK_FRAME_OVERHEAD(%r15)
 	lr	%r15,%r1
 	ssm	__LC_PGM_NEW_PSW	# turn dat on, keep irqs off
-	tm	__TI_flags+3(%r12),_TIF_MCCK_PENDING
+	tm	__LC_CPU_FLAGS+3,_CIF_MCCK_PENDING
 	jno	mcck_return
 	TRACE_IRQS_OFF
 	l	%r1,BASED(.Lhandle_mcck)
@@ -842,6 +843,8 @@ cleanup_system_call:
 	stm	%r0,%r7,__PT_R0(%r9)
 	mvc	__PT_PSW(8,%r9),__LC_SVC_OLD_PSW
 	mvc	__PT_INT_CODE(4,%r9),__LC_SVC_ILC
+	xc	__PT_FLAGS(4,%r9),__PT_FLAGS(%r9)
+	mvi	__PT_FLAGS+3(%r9),_PIF_SYSCALL
 	# setup saved register 15
 	st	%r15,28(%r11)		# r15 stack pointer
 	# set new psw address and exit
diff --git a/arch/s390/kernel/entry64.S b/arch/s390/kernel/entry64.S
index 5963e43618b..c41f3f90672 100644
--- a/arch/s390/kernel/entry64.S
+++ b/arch/s390/kernel/entry64.S
@@ -42,13 +42,11 @@ 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 | _TIF_ASCE)
-_TIF_WORK_INT = (_TIF_SIGPENDING | _TIF_NOTIFY_RESUME | _TIF_NEED_RESCHED | \
-		 _TIF_MCCK_PENDING | _TIF_ASCE)
-_TIF_TRACE    = (_TIF_SYSCALL_TRACE | _TIF_SYSCALL_AUDIT | _TIF_SECCOMP | \
-		 _TIF_SYSCALL_TRACEPOINT)
-_TIF_TRANSFER = (_TIF_MCCK_PENDING | _TIF_TLB_WAIT)
+_TIF_WORK	= (_TIF_SIGPENDING | _TIF_NOTIFY_RESUME | _TIF_NEED_RESCHED)
+_TIF_TRACE	= (_TIF_SYSCALL_TRACE | _TIF_SYSCALL_AUDIT | _TIF_SECCOMP | \
+		   _TIF_SYSCALL_TRACEPOINT)
+_CIF_WORK	= (_CIF_MCCK_PENDING | _CIF_ASCE)
+_PIF_WORK	= (_PIF_PER_TRAP)
 
 #define BASED(name) name-system_call(%r13)
 
@@ -190,13 +188,7 @@ ENTRY(__switch_to)
 	lctl	%c4,%c4,__TASK_pid(%r3)		# load pid to control reg. 4
 	mvc	__LC_CURRENT_PID+4(4,%r0),__TASK_pid(%r3) # store pid of next
 	lg	%r15,__THREAD_ksp(%r3)		# load kernel stack of next
-	llill	%r6,_TIF_TRANSFER		# transfer TIF bits
-	ng	%r6,__TI_flags(%r4)		# isolate TIF bits
-	jz	0f
-	og	%r6,__TI_flags(%r5)		# set TIF bits of next
-	stg	%r6,__TI_flags(%r5)
-	ni	__TI_flags+7(%r4),255-_TIF_TRANSFER # clear TIF bits of prev
-0:	lmg	%r6,%r15,__SF_GPRS(%r15)	# load gprs of next task
+	lmg	%r6,%r15,__SF_GPRS(%r15)	# load gprs of next task
 	br	%r14
 
 __critical_start:
@@ -211,6 +203,7 @@ sysc_stmg:
 	stmg	%r8,%r15,__LC_SAVE_AREA_SYNC
 	lg	%r10,__LC_LAST_BREAK
 	lg	%r12,__LC_THREAD_INFO
+	lghi	%r14,_PIF_SYSCALL
 sysc_per:
 	lg	%r15,__LC_KERNEL_STACK
 	la	%r11,STACK_FRAME_OVERHEAD(%r15)	# pointer to pt_regs
@@ -221,8 +214,8 @@ sysc_vtime:
 	mvc	__PT_R8(64,%r11),__LC_SAVE_AREA_SYNC
 	mvc	__PT_PSW(16,%r11),__LC_SVC_OLD_PSW
 	mvc	__PT_INT_CODE(4,%r11),__LC_SVC_ILC
+	stg	%r14,__PT_FLAGS(%r11)
 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
@@ -238,7 +231,7 @@ sysc_nr_ok:
 	stg	%r2,__PT_ORIG_GPR2(%r11)
 	stg	%r7,STACK_FRAME_OVERHEAD(%r15)
 	lgf	%r9,0(%r8,%r10)			# get system call add.
-	tm	__TI_flags+6(%r12),_TIF_TRACE >> 8
+	tm	__TI_flags+7(%r12),_TIF_TRACE
 	jnz	sysc_tracesys
 	basr	%r14,%r9			# call sys_xxxx
 	stg	%r2,__PT_R2(%r11)		# store return value
@@ -248,9 +241,12 @@ sysc_return:
 sysc_tif:
 	tm	__PT_PSW+1(%r11),0x01		# returning to user ?
 	jno	sysc_restore
-	tm	__TI_flags+7(%r12),_TIF_WORK_SVC
+	tm	__PT_FLAGS+7(%r11),_PIF_WORK
+	jnz	sysc_work
+	tm	__TI_flags+7(%r12),_TIF_WORK
 	jnz	sysc_work			# check for work
-	ni	__TI_flags+7(%r12),255-_TIF_SYSCALL
+	tm	__LC_CPU_FLAGS+7,_CIF_WORK
+	jnz	sysc_work
 sysc_restore:
 	lg	%r14,__LC_VDSO_PER_CPU
 	lmg	%r0,%r10,__PT_R0(%r11)
@@ -265,17 +261,17 @@ sysc_done:
 # One of the work bits is on. Find out which one.
 #
 sysc_work:
-	tm	__TI_flags+7(%r12),_TIF_MCCK_PENDING
+	tm	__LC_CPU_FLAGS+7,_CIF_MCCK_PENDING
 	jo	sysc_mcck_pending
 	tm	__TI_flags+7(%r12),_TIF_NEED_RESCHED
 	jo	sysc_reschedule
-	tm	__TI_flags+7(%r12),_TIF_PER_TRAP
+	tm	__PT_FLAGS+7(%r11),_PIF_PER_TRAP
 	jo	sysc_singlestep
 	tm	__TI_flags+7(%r12),_TIF_SIGPENDING
 	jo	sysc_sigpending
 	tm	__TI_flags+7(%r12),_TIF_NOTIFY_RESUME
 	jo	sysc_notify_resume
-	tm	__TI_flags+7(%r12),_TIF_ASCE
+	tm	__LC_CPU_FLAGS+7,_CIF_ASCE
 	jo	sysc_uaccess
 	j	sysc_return		# beware of critical section cleanup
 
@@ -287,17 +283,17 @@ sysc_reschedule:
 	jg	schedule
 
 #
-# _TIF_MCCK_PENDING is set, call handler
+# _CIF_MCCK_PENDING is set, call handler
 #
 sysc_mcck_pending:
 	larl	%r14,sysc_return
 	jg	s390_handle_mcck	# TIF bit will be cleared by handler
 
 #
-# _TIF_ASCE is set, load user space asce
+# _CIF_ASCE is set, load user space asce
 #
 sysc_uaccess:
-	ni	__TI_flags+7(%r12),255-_TIF_ASCE
+	ni	__LC_CPU_FLAGS+7,255-_CIF_ASCE
 	lctlg	%c1,%c1,__LC_USER_ASCE		# load primary asce
 	j	sysc_return
 
@@ -307,7 +303,7 @@ sysc_uaccess:
 sysc_sigpending:
 	lgr	%r2,%r11		# pass pointer to pt_regs
 	brasl	%r14,do_signal
-	tm	__TI_flags+7(%r12),_TIF_SYSCALL
+	tm	__PT_FLAGS+7(%r11),_PIF_SYSCALL
 	jno	sysc_return
 	lmg	%r2,%r7,__PT_R2(%r11)	# load svc arguments
 	lg	%r10,__TI_sysc_table(%r12)	# address of system call table
@@ -327,10 +323,10 @@ sysc_notify_resume:
 	jg	do_notify_resume
 
 #
-# _TIF_PER_TRAP is set, call do_per_trap
+# _PIF_PER_TRAP is set, call do_per_trap
 #
 sysc_singlestep:
-	ni	__TI_flags+7(%r12),255-_TIF_PER_TRAP
+	ni	__PT_FLAGS+7(%r11),255-_PIF_PER_TRAP
 	lgr	%r2,%r11		# pass pointer to pt_regs
 	larl	%r14,sysc_return
 	jg	do_per_trap
@@ -357,7 +353,7 @@ sysc_tracego:
 	basr	%r14,%r9		# call sys_xxx
 	stg	%r2,__PT_R2(%r11)	# store return value
 sysc_tracenogo:
-	tm	__TI_flags+6(%r12),_TIF_TRACE >> 8
+	tm	__TI_flags+7(%r12),_TIF_TRACE
 	jz	sysc_return
 	lgr	%r2,%r11		# pass pointer to pt_regs
 	larl	%r14,sysc_return
@@ -416,12 +412,13 @@ ENTRY(pgm_check_handler)
 	stmg	%r8,%r9,__PT_PSW(%r11)
 	mvc	__PT_INT_CODE(4,%r11),__LC_PGM_ILC
 	mvc	__PT_INT_PARM_LONG(8,%r11),__LC_TRANS_EXC_CODE
+	xc	__PT_FLAGS(8,%r11),__PT_FLAGS(%r11)
 	stg	%r10,__PT_ARGS(%r11)
 	tm	__LC_PGM_ILC+3,0x80	# check for per exception
 	jz	0f
 	tmhh	%r8,0x0001		# kernel per event ?
 	jz	pgm_kprobe
-	oi	__TI_flags+7(%r12),_TIF_PER_TRAP
+	oi	__PT_FLAGS+7(%r11),_PIF_PER_TRAP
 	mvc	__THREAD_per_address(8,%r14),__LC_PER_ADDRESS
 	mvc	__THREAD_per_cause(2,%r14),__LC_PER_CAUSE
 	mvc	__THREAD_per_paid(1,%r14),__LC_PER_PAID
@@ -451,10 +448,10 @@ pgm_kprobe:
 # single stepped system call
 #
 pgm_svcper:
-	oi	__TI_flags+7(%r12),_TIF_PER_TRAP
 	mvc	__LC_RETURN_PSW(8),__LC_SVC_NEW_PSW
 	larl	%r14,sysc_per
 	stg	%r14,__LC_RETURN_PSW+8
+	lghi	%r14,_PIF_SYSCALL | _PIF_PER_TRAP
 	lpswe	__LC_RETURN_PSW		# branch to sysc_per and enable irqs
 
 /*
@@ -479,6 +476,7 @@ io_skip:
 	mvc	__PT_R8(64,%r11),__LC_SAVE_AREA_ASYNC
 	stmg	%r8,%r9,__PT_PSW(%r11)
 	mvc	__PT_INT_CODE(12,%r11),__LC_SUBCHANNEL_ID
+	xc	__PT_FLAGS(8,%r11),__PT_FLAGS(%r11)
 	TRACE_IRQS_OFF
 	xc	__SF_BACKCHAIN(8,%r15),__SF_BACKCHAIN(%r15)
 io_loop:
@@ -499,8 +497,10 @@ io_return:
 	LOCKDEP_SYS_EXIT
 	TRACE_IRQS_ON
 io_tif:
-	tm	__TI_flags+7(%r12),_TIF_WORK_INT
+	tm	__TI_flags+7(%r12),_TIF_WORK
 	jnz	io_work 		# there is work to do (signals etc.)
+	tm	__LC_CPU_FLAGS+7,_CIF_WORK
+	jnz	io_work
 io_restore:
 	lg	%r14,__LC_VDSO_PER_CPU
 	lmg	%r0,%r10,__PT_R0(%r11)
@@ -513,7 +513,7 @@ io_done:
 
 #
 # There is work todo, find out in which context we have been interrupted:
-# 1) if we return to user space we can do all _TIF_WORK_INT work
+# 1) if we return to user space we can do all _TIF_WORK work
 # 2) if we return to kernel code and kvm is enabled check if we need to
 #    modify the psw to leave SIE
 # 3) if we return to kernel code and preemptive scheduling is enabled check
@@ -557,11 +557,9 @@ io_work_user:
 
 #
 # One of the work bits is on. Find out which one.
-# Checked are: _TIF_SIGPENDING, _TIF_NOTIFY_RESUME, _TIF_NEED_RESCHED
-#	       and _TIF_MCCK_PENDING
 #
 io_work_tif:
-	tm	__TI_flags+7(%r12),_TIF_MCCK_PENDING
+	tm	__LC_CPU_FLAGS+7,_CIF_MCCK_PENDING
 	jo	io_mcck_pending
 	tm	__TI_flags+7(%r12),_TIF_NEED_RESCHED
 	jo	io_reschedule
@@ -569,12 +567,12 @@ io_work_tif:
 	jo	io_sigpending
 	tm	__TI_flags+7(%r12),_TIF_NOTIFY_RESUME
 	jo	io_notify_resume
-	tm	__TI_flags+7(%r12),_TIF_ASCE
+	tm	__LC_CPU_FLAGS+7,_CIF_ASCE
 	jo	io_uaccess
 	j	io_return		# beware of critical section cleanup
 
 #
-# _TIF_MCCK_PENDING is set, call handler
+# _CIF_MCCK_PENDING is set, call handler
 #
 io_mcck_pending:
 	# TRACE_IRQS_ON already done at io_return
@@ -583,10 +581,10 @@ io_mcck_pending:
 	j	io_return
 
 #
-# _TIF_ASCE is set, load user space asce
+# _CIF_ASCE is set, load user space asce
 #
 io_uaccess:
-	ni	__TI_flags+7(%r12),255-_TIF_ASCE
+	ni	__LC_CPU_FLAGS+7,255-_CIF_ASCE
 	lctlg	%c1,%c1,__LC_USER_ASCE		# load primary asce
 	j	io_return
 
@@ -650,6 +648,7 @@ ext_skip:
 	mvc	__PT_INT_CODE(4,%r11),__LC_EXT_CPU_ADDR
 	mvc	__PT_INT_PARM(4,%r11),__LC_EXT_PARAMS
 	mvc	__PT_INT_PARM_LONG(8,%r11),0(%r1)
+	xc	__PT_FLAGS(8,%r11),__PT_FLAGS(%r11)
 	TRACE_IRQS_OFF
 	xc	__SF_BACKCHAIN(8,%r15),__SF_BACKCHAIN(%r15)
 	lgr	%r2,%r11		# pass pointer to pt_regs
@@ -716,6 +715,7 @@ mcck_skip:
 	stmg	%r0,%r7,__PT_R0(%r11)
 	mvc	__PT_R8(64,%r11),0(%r14)
 	stmg	%r8,%r9,__PT_PSW(%r11)
+	xc	__PT_FLAGS(8,%r11),__PT_FLAGS(%r11)
 	xc	__SF_BACKCHAIN(8,%r15),__SF_BACKCHAIN(%r15)
 	lgr	%r2,%r11		# pass pointer to pt_regs
 	brasl	%r14,s390_do_machine_check
@@ -727,7 +727,7 @@ mcck_skip:
 	la	%r11,STACK_FRAME_OVERHEAD(%r1)
 	lgr	%r15,%r1
 	ssm	__LC_PGM_NEW_PSW	# turn dat on, keep irqs off
-	tm	__TI_flags+7(%r12),_TIF_MCCK_PENDING
+	tm	__LC_CPU_FLAGS+7,_CIF_MCCK_PENDING
 	jno	mcck_return
 	TRACE_IRQS_OFF
 	brasl	%r14,s390_handle_mcck
@@ -884,6 +884,8 @@ cleanup_system_call:
 	stmg	%r0,%r7,__PT_R0(%r9)
 	mvc	__PT_PSW(16,%r9),__LC_SVC_OLD_PSW
 	mvc	__PT_INT_CODE(4,%r9),__LC_SVC_ILC
+	xc	__PT_FLAGS(8,%r9),__PT_FLAGS(%r9)
+	mvi	__PT_FLAGS+7(%r9),_PIF_SYSCALL
 	# setup saved register r15
 	stg	%r15,56(%r11)		# r15 stack pointer
 	# set new psw address and exit
diff --git a/arch/s390/kernel/head.S b/arch/s390/kernel/head.S
index 429afcc480c..7ba7d678451 100644
--- a/arch/s390/kernel/head.S
+++ b/arch/s390/kernel/head.S
@@ -437,13 +437,13 @@ ENTRY(startup_kdump)
 
 #if defined(CONFIG_64BIT)
 #if defined(CONFIG_MARCH_ZEC12)
-	.long 3, 0xc100efe3, 0xf46ce800, 0x00400000
+	.long 3, 0xc100efea, 0xf46ce800, 0x00400000
 #elif defined(CONFIG_MARCH_Z196)
-	.long 2, 0xc100efe3, 0xf46c0000
+	.long 2, 0xc100efea, 0xf46c0000
 #elif defined(CONFIG_MARCH_Z10)
-	.long 2, 0xc100efe3, 0xf0680000
+	.long 2, 0xc100efea, 0xf0680000
 #elif defined(CONFIG_MARCH_Z9_109)
-	.long 1, 0xc100efc3
+	.long 1, 0xc100efc2
 #elif defined(CONFIG_MARCH_Z990)
 	.long 1, 0xc0002000
 #elif defined(CONFIG_MARCH_Z900)
diff --git a/arch/s390/kernel/head31.S b/arch/s390/kernel/head31.S
index 9a99856df1c..6dbe80983a2 100644
--- a/arch/s390/kernel/head31.S
+++ b/arch/s390/kernel/head31.S
@@ -59,7 +59,6 @@ ENTRY(startup_continue)
 	.long	0			# cr13: home space segment table
 	.long	0xc0000000		# cr14: machine check handling off
 	.long	0			# cr15: linkage stack operations
-.Lmchunk:.long	memory_chunk
 .Lbss_bgn:  .long __bss_start
 .Lbss_end:  .long _end
 .Lparmaddr: .long PARMAREA
diff --git a/arch/s390/kernel/nmi.c b/arch/s390/kernel/nmi.c
index c4c03381987..210e1285f75 100644
--- a/arch/s390/kernel/nmi.c
+++ b/arch/s390/kernel/nmi.c
@@ -55,7 +55,7 @@ void s390_handle_mcck(void)
 	local_mcck_disable();
 	mcck = __get_cpu_var(cpu_mcck);
 	memset(&__get_cpu_var(cpu_mcck), 0, sizeof(struct mcck_struct));
-	clear_thread_flag(TIF_MCCK_PENDING);
+	clear_cpu_flag(CIF_MCCK_PENDING);
 	local_mcck_enable();
 	local_irq_restore(flags);
 
@@ -313,7 +313,7 @@ void notrace s390_do_machine_check(struct pt_regs *regs)
 			 */
 			mcck->kill_task = 1;
 			mcck->mcck_code = *(unsigned long long *) mci;
-			set_thread_flag(TIF_MCCK_PENDING);
+			set_cpu_flag(CIF_MCCK_PENDING);
 		} else {
 			/*
 			 * Couldn't restore all register contents while in
@@ -352,12 +352,12 @@ void notrace s390_do_machine_check(struct pt_regs *regs)
 	if (mci->cp) {
 		/* Channel report word pending */
 		mcck->channel_report = 1;
-		set_thread_flag(TIF_MCCK_PENDING);
+		set_cpu_flag(CIF_MCCK_PENDING);
 	}
 	if (mci->w) {
 		/* Warning pending */
 		mcck->warning = 1;
-		set_thread_flag(TIF_MCCK_PENDING);
+		set_cpu_flag(CIF_MCCK_PENDING);
 	}
 	nmi_exit();
 }
diff --git a/arch/s390/kernel/process.c b/arch/s390/kernel/process.c
index dd145321d21..93b9ca42e5c 100644
--- a/arch/s390/kernel/process.c
+++ b/arch/s390/kernel/process.c
@@ -64,7 +64,7 @@ unsigned long thread_saved_pc(struct task_struct *tsk)
 void arch_cpu_idle(void)
 {
 	local_mcck_disable();
-	if (test_thread_flag(TIF_MCCK_PENDING)) {
+	if (test_cpu_flag(CIF_MCCK_PENDING)) {
 		local_mcck_enable();
 		local_irq_enable();
 		return;
@@ -76,7 +76,7 @@ void arch_cpu_idle(void)
 
 void arch_cpu_idle_exit(void)
 {
-	if (test_thread_flag(TIF_MCCK_PENDING))
+	if (test_cpu_flag(CIF_MCCK_PENDING))
 		s390_handle_mcck();
 }
 
@@ -123,7 +123,6 @@ int copy_thread(unsigned long clone_flags, unsigned long new_stackp,
 	memset(&p->thread.per_user, 0, sizeof(p->thread.per_user));
 	memset(&p->thread.per_event, 0, sizeof(p->thread.per_event));
 	clear_tsk_thread_flag(p, TIF_SINGLE_STEP);
-	clear_tsk_thread_flag(p, TIF_PER_TRAP);
 	/* Initialize per thread user and system timer values */
 	ti = task_thread_info(p);
 	ti->user_timer = 0;
@@ -152,6 +151,7 @@ int copy_thread(unsigned long clone_flags, unsigned long new_stackp,
 	}
 	frame->childregs = *current_pt_regs();
 	frame->childregs.gprs[2] = 0;	/* child returns 0 on fork. */
+	frame->childregs.flags = 0;
 	if (new_stackp)
 		frame->childregs.gprs[15] = new_stackp;
 
diff --git a/arch/s390/kernel/ptrace.c b/arch/s390/kernel/ptrace.c
index 1c82619eb4f..2d716734b5b 100644
--- a/arch/s390/kernel/ptrace.c
+++ b/arch/s390/kernel/ptrace.c
@@ -136,7 +136,7 @@ void ptrace_disable(struct task_struct *task)
 	memset(&task->thread.per_user, 0, sizeof(task->thread.per_user));
 	memset(&task->thread.per_event, 0, sizeof(task->thread.per_event));
 	clear_tsk_thread_flag(task, TIF_SINGLE_STEP);
-	clear_tsk_thread_flag(task, TIF_PER_TRAP);
+	clear_pt_regs_flag(task_pt_regs(task), PIF_PER_TRAP);
 	task->thread.per_flags = 0;
 }
 
@@ -813,7 +813,7 @@ asmlinkage long do_syscall_trace_enter(struct pt_regs *regs)
 		 * debugger stored an invalid system call number. Skip
 		 * the system call and the system call restart handling.
 		 */
-		clear_thread_flag(TIF_SYSCALL);
+		clear_pt_regs_flag(regs, PIF_SYSCALL);
 		ret = -1;
 	}
 
diff --git a/arch/s390/kernel/setup.c b/arch/s390/kernel/setup.c
index 88d1ca81e2d..1e2264b46e4 100644
--- a/arch/s390/kernel/setup.c
+++ b/arch/s390/kernel/setup.c
@@ -78,10 +78,9 @@ EXPORT_SYMBOL(console_irq);
 unsigned long elf_hwcap = 0;
 char elf_platform[ELF_PLATFORM_SIZE];
 
-struct mem_chunk __initdata memory_chunk[MEMORY_CHUNKS];
-
 int __initdata memory_end_set;
 unsigned long __initdata memory_end;
+unsigned long __initdata max_physmem_end;
 
 unsigned long VMALLOC_START;
 EXPORT_SYMBOL(VMALLOC_START);
@@ -212,7 +211,7 @@ static void __init conmode_default(void)
 	}
 }
 
-#ifdef CONFIG_ZFCPDUMP
+#ifdef CONFIG_CRASH_DUMP
 static void __init setup_zfcpdump(void)
 {
 	if (ipl_info.type != IPL_TYPE_FCP_DUMP)
@@ -224,7 +223,7 @@ static void __init setup_zfcpdump(void)
 }
 #else
 static inline void setup_zfcpdump(void) {}
-#endif /* CONFIG_ZFCPDUMP */
+#endif /* CONFIG_CRASH_DUMP */
 
  /*
  * Reboot, halt and power_off stubs. They just call _machine_restart,
@@ -273,6 +272,7 @@ EXPORT_SYMBOL_GPL(pm_power_off);
 static int __init early_parse_mem(char *p)
 {
 	memory_end = memparse(p, &p);
+	memory_end &= PAGE_MASK;
 	memory_end_set = 1;
 	return 0;
 }
@@ -373,6 +373,10 @@ static void __init setup_lowcore(void)
 	mem_assign_absolute(S390_lowcore.restart_source, lc->restart_source);
 	mem_assign_absolute(S390_lowcore.restart_psw, lc->restart_psw);
 
+#ifdef CONFIG_SMP
+	lc->spinlock_lockval = arch_spin_lockval(0);
+#endif
+
 	set_prefix((u32)(unsigned long) lc);
 	lowcore_ptr[0] = lc;
 }
@@ -401,7 +405,8 @@ static struct resource __initdata *standard_resources[] = {
 static void __init setup_resources(void)
 {
 	struct resource *res, *std_res, *sub_res;
-	int i, j;
+	struct memblock_region *reg;
+	int j;
 
 	code_resource.start = (unsigned long) &_text;
 	code_resource.end = (unsigned long) &_etext - 1;
@@ -410,24 +415,13 @@ static void __init setup_resources(void)
 	bss_resource.start = (unsigned long) &__bss_start;
 	bss_resource.end = (unsigned long) &__bss_stop - 1;
 
-	for (i = 0; i < MEMORY_CHUNKS; i++) {
-		if (!memory_chunk[i].size)
-			continue;
+	for_each_memblock(memory, reg) {
 		res = alloc_bootmem_low(sizeof(*res));
 		res->flags = IORESOURCE_BUSY | IORESOURCE_MEM;
-		switch (memory_chunk[i].type) {
-		case CHUNK_READ_WRITE:
-			res->name = "System RAM";
-			break;
-		case CHUNK_READ_ONLY:
-			res->name = "System ROM";
-			res->flags |= IORESOURCE_READONLY;
-			break;
-		default:
-			res->name = "reserved";
-		}
-		res->start = memory_chunk[i].addr;
-		res->end = res->start + memory_chunk[i].size - 1;
+
+		res->name = "System RAM";
+		res->start = reg->base;
+		res->end = reg->base + reg->size - 1;
 		request_resource(&iomem_resource, res);
 
 		for (j = 0; j < ARRAY_SIZE(standard_resources); j++) {
@@ -451,48 +445,11 @@ static void __init setup_resources(void)
 static void __init setup_memory_end(void)
 {
 	unsigned long vmax, vmalloc_size, tmp;
-	unsigned long real_memory_size = 0;
-	int i;
-
-
-#ifdef CONFIG_ZFCPDUMP
-	if (ipl_info.type == IPL_TYPE_FCP_DUMP &&
-	    !OLDMEM_BASE && sclp_get_hsa_size()) {
-		memory_end = sclp_get_hsa_size();
-		memory_end_set = 1;
-	}
-#endif
-	memory_end &= PAGE_MASK;
-
-	/*
-	 * Make sure all chunks are MAX_ORDER aligned so we don't need the
-	 * extra checks that HOLES_IN_ZONE would require.
-	 */
-	for (i = 0; i < MEMORY_CHUNKS; i++) {
-		unsigned long start, end;
-		struct mem_chunk *chunk;
-		unsigned long align;
-
-		chunk = &memory_chunk[i];
-		if (!chunk->size)
-			continue;
-		align = 1UL << (MAX_ORDER + PAGE_SHIFT - 1);
-		start = (chunk->addr + align - 1) & ~(align - 1);
-		end = (chunk->addr + chunk->size) & ~(align - 1);
-		if (start >= end)
-			memset(chunk, 0, sizeof(*chunk));
-		else {
-			chunk->addr = start;
-			chunk->size = end - start;
-		}
-		real_memory_size = max(real_memory_size,
-				       chunk->addr + chunk->size);
-	}
 
 	/* Choose kernel address space layout: 2, 3, or 4 levels. */
 #ifdef CONFIG_64BIT
 	vmalloc_size = VMALLOC_END ?: (128UL << 30) - MODULES_LEN;
-	tmp = (memory_end ?: real_memory_size) / PAGE_SIZE;
+	tmp = (memory_end ?: max_physmem_end) / PAGE_SIZE;
 	tmp = tmp * (sizeof(struct page) + PAGE_SIZE) + vmalloc_size;
 	if (tmp <= (1UL << 42))
 		vmax = 1UL << 42;	/* 3-level kernel page table */
@@ -520,21 +477,11 @@ static void __init setup_memory_end(void)
 	vmemmap = (struct page *) tmp;
 
 	/* Take care that memory_end is set and <= vmemmap */
-	memory_end = min(memory_end ?: real_memory_size, tmp);
-
-	/* Fixup memory chunk array to fit into 0..memory_end */
-	for (i = 0; i < MEMORY_CHUNKS; i++) {
-		struct mem_chunk *chunk = &memory_chunk[i];
+	memory_end = min(memory_end ?: max_physmem_end, tmp);
+	max_pfn = max_low_pfn = PFN_DOWN(memory_end);
+	memblock_remove(memory_end, ULONG_MAX);
 
-		if (!chunk->size)
-			continue;
-		if (chunk->addr >= memory_end) {
-			memset(chunk, 0, sizeof(*chunk));
-			continue;
-		}
-		if (chunk->addr + chunk->size > memory_end)
-			chunk->size = memory_end - chunk->addr;
-	}
+	pr_notice("Max memory size: %luMB\n", memory_end >> 20);
 }
 
 static void __init setup_vmcoreinfo(void)
@@ -545,89 +492,6 @@ static void __init setup_vmcoreinfo(void)
 #ifdef CONFIG_CRASH_DUMP
 
 /*
- * Find suitable location for crashkernel memory
- */
-static unsigned long __init find_crash_base(unsigned long crash_size,
-					    char **msg)
-{
-	unsigned long crash_base;
-	struct mem_chunk *chunk;
-	int i;
-
-	if (memory_chunk[0].size < crash_size) {
-		*msg = "first memory chunk must be at least crashkernel size";
-		return 0;
-	}
-	if (OLDMEM_BASE && crash_size == OLDMEM_SIZE)
-		return OLDMEM_BASE;
-
-	for (i = MEMORY_CHUNKS - 1; i >= 0; i--) {
-		chunk = &memory_chunk[i];
-		if (chunk->size == 0)
-			continue;
-		if (chunk->type != CHUNK_READ_WRITE)
-			continue;
-		if (chunk->size < crash_size)
-			continue;
-		crash_base = (chunk->addr + chunk->size) - crash_size;
-		if (crash_base < crash_size)
-			continue;
-		if (crash_base < sclp_get_hsa_size())
-			continue;
-		if (crash_base < (unsigned long) INITRD_START + INITRD_SIZE)
-			continue;
-		return crash_base;
-	}
-	*msg = "no suitable area found";
-	return 0;
-}
-
-/*
- * Check if crash_base and crash_size is valid
- */
-static int __init verify_crash_base(unsigned long crash_base,
-				    unsigned long crash_size,
-				    char **msg)
-{
-	struct mem_chunk *chunk;
-	int i;
-
-	/*
-	 * Because we do the swap to zero, we must have at least 'crash_size'
-	 * bytes free space before crash_base
-	 */
-	if (crash_size > crash_base) {
-		*msg = "crashkernel offset must be greater than size";
-		return -EINVAL;
-	}
-
-	/* First memory chunk must be at least crash_size */
-	if (memory_chunk[0].size < crash_size) {
-		*msg = "first memory chunk must be at least crashkernel size";
-		return -EINVAL;
-	}
-	/* Check if we fit into the respective memory chunk */
-	for (i = 0; i < MEMORY_CHUNKS; i++) {
-		chunk = &memory_chunk[i];
-		if (chunk->size == 0)
-			continue;
-		if (crash_base < chunk->addr)
-			continue;
-		if (crash_base >= chunk->addr + chunk->size)
-			continue;
-		/* we have found the memory chunk */
-		if (crash_base + crash_size > chunk->addr + chunk->size) {
-			*msg = "selected memory chunk is too small for "
-				"crashkernel memory";
-			return -EINVAL;
-		}
-		return 0;
-	}
-	*msg = "invalid memory range specified";
-	return -EINVAL;
-}
-
-/*
  * When kdump is enabled, we have to ensure that no memory from
  * the area [0 - crashkernel memory size] and
  * [crashk_res.start - crashk_res.end] is set offline.
@@ -653,23 +517,44 @@ static struct notifier_block kdump_mem_nb = {
 #endif
 
 /*
+ * Make sure that the area behind memory_end is protected
+ */
+static void reserve_memory_end(void)
+{
+#ifdef CONFIG_CRASH_DUMP
+	if (ipl_info.type == IPL_TYPE_FCP_DUMP &&
+	    !OLDMEM_BASE && sclp_get_hsa_size()) {
+		memory_end = sclp_get_hsa_size();
+		memory_end &= PAGE_MASK;
+		memory_end_set = 1;
+	}
+#endif
+	if (!memory_end_set)
+		return;
+	memblock_reserve(memory_end, ULONG_MAX);
+}
+
+/*
  * Make sure that oldmem, where the dump is stored, is protected
  */
 static void reserve_oldmem(void)
 {
 #ifdef CONFIG_CRASH_DUMP
-	unsigned long real_size = 0;
-	int i;
-
-	if (!OLDMEM_BASE)
-		return;
-	for (i = 0; i < MEMORY_CHUNKS; i++) {
-		struct mem_chunk *chunk = &memory_chunk[i];
+	if (OLDMEM_BASE)
+		/* Forget all memory above the running kdump system */
+		memblock_reserve(OLDMEM_SIZE, (phys_addr_t)ULONG_MAX);
+#endif
+}
 
-		real_size = max(real_size, chunk->addr + chunk->size);
-	}
-	create_mem_hole(memory_chunk, OLDMEM_BASE, OLDMEM_SIZE);
-	create_mem_hole(memory_chunk, OLDMEM_SIZE, real_size - OLDMEM_SIZE);
+/*
+ * Make sure that oldmem, where the dump is stored, is protected
+ */
+static void remove_oldmem(void)
+{
+#ifdef CONFIG_CRASH_DUMP
+	if (OLDMEM_BASE)
+		/* Forget all memory above the running kdump system */
+		memblock_remove(OLDMEM_SIZE, (phys_addr_t)ULONG_MAX);
 #endif
 }
 
@@ -680,167 +565,132 @@ static void __init reserve_crashkernel(void)
 {
 #ifdef CONFIG_CRASH_DUMP
 	unsigned long long crash_base, crash_size;
-	char *msg = NULL;
+	phys_addr_t low, high;
 	int rc;
 
 	rc = parse_crashkernel(boot_command_line, memory_end, &crash_size,
 			       &crash_base);
-	if (rc || crash_size == 0)
-		return;
+
 	crash_base = ALIGN(crash_base, KEXEC_CRASH_MEM_ALIGN);
 	crash_size = ALIGN(crash_size, KEXEC_CRASH_MEM_ALIGN);
-	if (register_memory_notifier(&kdump_mem_nb))
+	if (rc || crash_size == 0)
 		return;
-	if (!crash_base)
-		crash_base = find_crash_base(crash_size, &msg);
-	if (!crash_base) {
-		pr_info("crashkernel reservation failed: %s\n", msg);
-		unregister_memory_notifier(&kdump_mem_nb);
+
+	if (memblock.memory.regions[0].size < crash_size) {
+		pr_info("crashkernel reservation failed: %s\n",
+			"first memory chunk must be at least crashkernel size");
 		return;
 	}
-	if (verify_crash_base(crash_base, crash_size, &msg)) {
-		pr_info("crashkernel reservation failed: %s\n", msg);
-		unregister_memory_notifier(&kdump_mem_nb);
+
+	low = crash_base ?: OLDMEM_BASE;
+	high = low + crash_size;
+	if (low >= OLDMEM_BASE && high <= OLDMEM_BASE + OLDMEM_SIZE) {
+		/* The crashkernel fits into OLDMEM, reuse OLDMEM */
+		crash_base = low;
+	} else {
+		/* Find suitable area in free memory */
+		low = max_t(unsigned long, crash_size, sclp_get_hsa_size());
+		high = crash_base ? crash_base + crash_size : ULONG_MAX;
+
+		if (crash_base && crash_base < low) {
+			pr_info("crashkernel reservation failed: %s\n",
+				"crash_base too low");
+			return;
+		}
+		low = crash_base ?: low;
+		crash_base = memblock_find_in_range(low, high, crash_size,
+						    KEXEC_CRASH_MEM_ALIGN);
+	}
+
+	if (!crash_base) {
+		pr_info("crashkernel reservation failed: %s\n",
+			"no suitable area found");
 		return;
 	}
+
+	if (register_memory_notifier(&kdump_mem_nb))
+		return;
+
 	if (!OLDMEM_BASE && MACHINE_IS_VM)
 		diag10_range(PFN_DOWN(crash_base), PFN_DOWN(crash_size));
 	crashk_res.start = crash_base;
 	crashk_res.end = crash_base + crash_size - 1;
 	insert_resource(&iomem_resource, &crashk_res);
-	create_mem_hole(memory_chunk, crash_base, crash_size);
+	memblock_remove(crash_base, crash_size);
 	pr_info("Reserving %lluMB of memory at %lluMB "
 		"for crashkernel (System RAM: %luMB)\n",
-		crash_size >> 20, crash_base >> 20, memory_end >> 20);
+		crash_size >> 20, crash_base >> 20,
+		(unsigned long)memblock.memory.total_size >> 20);
 	os_info_crashkernel_add(crash_base, crash_size);
 #endif
 }
 
-static void __init setup_memory(void)
+/*
+ * Reserve the initrd from being used by memblock
+ */
+static void __init reserve_initrd(void)
 {
-        unsigned long bootmap_size;
-	unsigned long start_pfn, end_pfn;
-	int i;
+#ifdef CONFIG_BLK_DEV_INITRD
+	initrd_start = INITRD_START;
+	initrd_end = initrd_start + INITRD_SIZE;
+	memblock_reserve(INITRD_START, INITRD_SIZE);
+#endif
+}
 
-	/*
-	 * partially used pages are not usable - thus
-	 * we are rounding upwards:
-	 */
+/*
+ * Check for initrd being in usable memory
+ */
+static void __init check_initrd(void)
+{
+#ifdef CONFIG_BLK_DEV_INITRD
+	if (INITRD_START && INITRD_SIZE &&
+	    !memblock_is_region_memory(INITRD_START, INITRD_SIZE)) {
+		pr_err("initrd does not fit memory.\n");
+		memblock_free(INITRD_START, INITRD_SIZE);
+		initrd_start = initrd_end = 0;
+	}
+#endif
+}
+
+/*
+ * Reserve all kernel text
+ */
+static void __init reserve_kernel(void)
+{
+	unsigned long start_pfn;
 	start_pfn = PFN_UP(__pa(&_end));
-	end_pfn = max_pfn = PFN_DOWN(memory_end);
 
-#ifdef CONFIG_BLK_DEV_INITRD
 	/*
-	 * Move the initrd in case the bitmap of the bootmem allocater
-	 * would overwrite it.
+	 * Reserve memory used for lowcore/command line/kernel image.
 	 */
+	memblock_reserve(0, (unsigned long)_ehead);
+	memblock_reserve((unsigned long)_stext, PFN_PHYS(start_pfn)
+			 - (unsigned long)_stext);
+}
 
-	if (INITRD_START && INITRD_SIZE) {
-		unsigned long bmap_size;
-		unsigned long start;
-
-		bmap_size = bootmem_bootmap_pages(end_pfn - start_pfn + 1);
-		bmap_size = PFN_PHYS(bmap_size);
-
-		if (PFN_PHYS(start_pfn) + bmap_size > INITRD_START) {
-			start = PFN_PHYS(start_pfn) + bmap_size + PAGE_SIZE;
-
+static void __init reserve_elfcorehdr(void)
+{
 #ifdef CONFIG_CRASH_DUMP
-			if (OLDMEM_BASE) {
-				/* Move initrd behind kdump oldmem */
-				if (start + INITRD_SIZE > OLDMEM_BASE &&
-				    start < OLDMEM_BASE + OLDMEM_SIZE)
-					start = OLDMEM_BASE + OLDMEM_SIZE;
-			}
-#endif
-			if (start + INITRD_SIZE > memory_end) {
-				pr_err("initrd extends beyond end of "
-				       "memory (0x%08lx > 0x%08lx) "
-				       "disabling initrd\n",
-				       start + INITRD_SIZE, memory_end);
-				INITRD_START = INITRD_SIZE = 0;
-			} else {
-				pr_info("Moving initrd (0x%08lx -> "
-					"0x%08lx, size: %ld)\n",
-					INITRD_START, start, INITRD_SIZE);
-				memmove((void *) start, (void *) INITRD_START,
-					INITRD_SIZE);
-				INITRD_START = start;
-			}
-		}
-	}
+	if (is_kdump_kernel())
+		memblock_reserve(elfcorehdr_addr - OLDMEM_BASE,
+				 PAGE_ALIGN(elfcorehdr_size));
 #endif
+}
 
-	/*
-	 * Initialize the boot-time allocator
-	 */
-	bootmap_size = init_bootmem(start_pfn, end_pfn);
+static void __init setup_memory(void)
+{
+	struct memblock_region *reg;
 
 	/*
-	 * Register RAM areas with the bootmem allocator.
+	 * Init storage key for present memory
 	 */
-
-	for (i = 0; i < MEMORY_CHUNKS; i++) {
-		unsigned long start_chunk, end_chunk, pfn;
-
-		if (!memory_chunk[i].size)
-			continue;
-		start_chunk = PFN_DOWN(memory_chunk[i].addr);
-		end_chunk = start_chunk + PFN_DOWN(memory_chunk[i].size);
-		end_chunk = min(end_chunk, end_pfn);
-		if (start_chunk >= end_chunk)
-			continue;
-		memblock_add_node(PFN_PHYS(start_chunk),
-				  PFN_PHYS(end_chunk - start_chunk), 0);
-		pfn = max(start_chunk, start_pfn);
-		storage_key_init_range(PFN_PHYS(pfn), PFN_PHYS(end_chunk));
+	for_each_memblock(memory, reg) {
+		storage_key_init_range(reg->base, reg->base + reg->size);
 	}
-
 	psw_set_key(PAGE_DEFAULT_KEY);
 
-	free_bootmem_with_active_regions(0, max_pfn);
-
-	/*
-	 * Reserve memory used for lowcore/command line/kernel image.
-	 */
-	reserve_bootmem(0, (unsigned long)_ehead, BOOTMEM_DEFAULT);
-	reserve_bootmem((unsigned long)_stext,
-			PFN_PHYS(start_pfn) - (unsigned long)_stext,
-			BOOTMEM_DEFAULT);
-	/*
-	 * Reserve the bootmem bitmap itself as well. We do this in two
-	 * steps (first step was init_bootmem()) because this catches
-	 * the (very unlikely) case of us accidentally initializing the
-	 * bootmem allocator with an invalid RAM area.
-	 */
-	reserve_bootmem(start_pfn << PAGE_SHIFT, bootmap_size,
-			BOOTMEM_DEFAULT);
-
-#ifdef CONFIG_CRASH_DUMP
-	if (crashk_res.start)
-		reserve_bootmem(crashk_res.start,
-				crashk_res.end - crashk_res.start + 1,
-				BOOTMEM_DEFAULT);
-	if (is_kdump_kernel())
-		reserve_bootmem(elfcorehdr_addr - OLDMEM_BASE,
-				PAGE_ALIGN(elfcorehdr_size), BOOTMEM_DEFAULT);
-#endif
-#ifdef CONFIG_BLK_DEV_INITRD
-	if (INITRD_START && INITRD_SIZE) {
-		if (INITRD_START + INITRD_SIZE <= memory_end) {
-			reserve_bootmem(INITRD_START, INITRD_SIZE,
-					BOOTMEM_DEFAULT);
-			initrd_start = INITRD_START;
-			initrd_end = initrd_start + INITRD_SIZE;
-		} else {
-			pr_err("initrd extends beyond end of "
-			       "memory (0x%08lx > 0x%08lx) "
-			       "disabling initrd\n",
-			       initrd_start + INITRD_SIZE, memory_end);
-			initrd_start = initrd_end = 0;
-		}
-	}
-#endif
+	/* Only cosmetics */
+	memblock_enforce_memory_limit(memblock_end_of_DRAM());
 }
 
 /*
@@ -989,23 +839,46 @@ void __init setup_arch(char **cmdline_p)
 
         ROOT_DEV = Root_RAM0;
 
+	/* Is init_mm really needed? */
 	init_mm.start_code = PAGE_OFFSET;
 	init_mm.end_code = (unsigned long) &_etext;
 	init_mm.end_data = (unsigned long) &_edata;
 	init_mm.brk = (unsigned long) &_end;
 
 	parse_early_param();
-	detect_memory_layout(memory_chunk, memory_end);
 	os_info_init();
 	setup_ipl();
+
+	/* Do some memory reservations *before* memory is added to memblock */
+	reserve_memory_end();
 	reserve_oldmem();
+	reserve_kernel();
+	reserve_initrd();
+	reserve_elfcorehdr();
+	memblock_allow_resize();
+
+	/* Get information about *all* installed memory */
+	detect_memory_memblock();
+
+	remove_oldmem();
+
+	/*
+	 * Make sure all chunks are MAX_ORDER aligned so we don't need the
+	 * extra checks that HOLES_IN_ZONE would require.
+	 *
+	 * Is this still required?
+	 */
+	memblock_trim_memory(1UL << (MAX_ORDER - 1 + PAGE_SHIFT));
+
 	setup_memory_end();
-	reserve_crashkernel();
 	setup_memory();
+
+	check_initrd();
+	reserve_crashkernel();
+
 	setup_resources();
 	setup_vmcoreinfo();
 	setup_lowcore();
-
 	smp_fill_possible_mask();
         cpu_init();
 	s390_init_cpu_topology();
diff --git a/arch/s390/kernel/signal.c b/arch/s390/kernel/signal.c
index d8fd508ccd1..42b49f9e19b 100644
--- a/arch/s390/kernel/signal.c
+++ b/arch/s390/kernel/signal.c
@@ -113,7 +113,7 @@ static int restore_sigregs(struct pt_regs *regs, _sigregs __user *sregs)
 	       sizeof(current->thread.fp_regs));
 
 	restore_fp_regs(current->thread.fp_regs.fprs);
-	clear_thread_flag(TIF_SYSCALL);	/* No longer in a system call */
+	clear_pt_regs_flag(regs, PIF_SYSCALL); /* No longer in a system call */
 	return 0;
 }
 
@@ -356,7 +356,7 @@ void do_signal(struct pt_regs *regs)
 	 * call information.
 	 */
 	current_thread_info()->system_call =
-		test_thread_flag(TIF_SYSCALL) ? regs->int_code : 0;
+		test_pt_regs_flag(regs, PIF_SYSCALL) ? regs->int_code : 0;
 	signr = get_signal_to_deliver(&info, &ka, regs, NULL);
 
 	if (signr > 0) {
@@ -384,7 +384,7 @@ void do_signal(struct pt_regs *regs)
 			}
 		}
 		/* No longer in a system call */
-		clear_thread_flag(TIF_SYSCALL);
+		clear_pt_regs_flag(regs, PIF_SYSCALL);
 
 		if (is_compat_task())
 			handle_signal32(signr, &ka, &info, oldset, regs);
@@ -394,7 +394,7 @@ void do_signal(struct pt_regs *regs)
 	}
 
 	/* No handlers present - check for system call restart */
-	clear_thread_flag(TIF_SYSCALL);
+	clear_pt_regs_flag(regs, PIF_SYSCALL);
 	if (current_thread_info()->system_call) {
 		regs->int_code = current_thread_info()->system_call;
 		switch (regs->gprs[2]) {
@@ -407,9 +407,9 @@ void do_signal(struct pt_regs *regs)
 		case -ERESTARTNOINTR:
 			/* Restart system call with magic TIF bit. */
 			regs->gprs[2] = regs->orig_gpr2;
-			set_thread_flag(TIF_SYSCALL);
+			set_pt_regs_flag(regs, PIF_SYSCALL);
 			if (test_thread_flag(TIF_SINGLE_STEP))
-				set_thread_flag(TIF_PER_TRAP);
+				clear_pt_regs_flag(regs, PIF_PER_TRAP);
 			break;
 		}
 	}
diff --git a/arch/s390/kernel/smp.c b/arch/s390/kernel/smp.c
index 86e65ec3422..243c7e51260 100644
--- a/arch/s390/kernel/smp.c
+++ b/arch/s390/kernel/smp.c
@@ -170,6 +170,7 @@ static int pcpu_alloc_lowcore(struct pcpu *pcpu, int cpu)
 	lc->panic_stack = pcpu->panic_stack + PAGE_SIZE
 		- STACK_FRAME_OVERHEAD - sizeof(struct pt_regs);
 	lc->cpu_nr = cpu;
+	lc->spinlock_lockval = arch_spin_lockval(cpu);
 #ifndef CONFIG_64BIT
 	if (MACHINE_HAS_IEEE) {
 		lc->extended_save_area_addr = get_zeroed_page(GFP_KERNEL);
@@ -226,6 +227,7 @@ static void pcpu_prepare_secondary(struct pcpu *pcpu, int cpu)
 	cpumask_set_cpu(cpu, mm_cpumask(&init_mm));
 	atomic_inc(&init_mm.context.attach_count);
 	lc->cpu_nr = cpu;
+	lc->spinlock_lockval = arch_spin_lockval(cpu);
 	lc->percpu_offset = __per_cpu_offset[cpu];
 	lc->kernel_asce = S390_lowcore.kernel_asce;
 	lc->machine_flags = S390_lowcore.machine_flags;
@@ -403,15 +405,6 @@ void smp_send_stop(void)
 }
 
 /*
- * Stop the current cpu.
- */
-void smp_stop_cpu(void)
-{
-	pcpu_sigp_retry(pcpu_devices + smp_processor_id(), SIGP_STOP, 0);
-	for (;;) ;
-}
-
-/*
  * This is the main routine where commands issued by other
  * cpus are handled.
  */
@@ -519,7 +512,7 @@ void smp_ctl_clear_bit(int cr, int bit)
 }
 EXPORT_SYMBOL(smp_ctl_clear_bit);
 
-#if defined(CONFIG_ZFCPDUMP) || defined(CONFIG_CRASH_DUMP)
+#ifdef CONFIG_CRASH_DUMP
 
 static void __init smp_get_save_area(int cpu, u16 address)
 {
@@ -534,14 +527,12 @@ static void __init smp_get_save_area(int cpu, u16 address)
 	save_area = dump_save_area_create(cpu);
 	if (!save_area)
 		panic("could not allocate memory for save area\n");
-#ifdef CONFIG_CRASH_DUMP
 	if (address == boot_cpu_address) {
 		/* Copy the registers of the boot cpu. */
 		copy_oldmem_page(1, (void *) save_area, sizeof(*save_area),
 				 SAVE_AREA_BASE - PAGE_SIZE, 0);
 		return;
 	}
-#endif
 	/* Get the registers of a non-boot cpu. */
 	__pcpu_sigp_relax(address, SIGP_STOP_AND_STORE_STATUS, 0, NULL);
 	memcpy_real(save_area, lc + SAVE_AREA_BASE, sizeof(*save_area));
@@ -558,11 +549,11 @@ int smp_store_status(int cpu)
 	return 0;
 }
 
-#else /* CONFIG_ZFCPDUMP || CONFIG_CRASH_DUMP */
+#else /* CONFIG_CRASH_DUMP */
 
 static inline void smp_get_save_area(int cpu, u16 address) { }
 
-#endif /* CONFIG_ZFCPDUMP || CONFIG_CRASH_DUMP */
+#endif /* CONFIG_CRASH_DUMP */
 
 void smp_cpu_set_polarization(int cpu, int val)
 {
@@ -809,6 +800,7 @@ void __init smp_cpus_done(unsigned int max_cpus)
 void __init smp_setup_processor_id(void)
 {
 	S390_lowcore.cpu_nr = 0;
+	S390_lowcore.spinlock_lockval = arch_spin_lockval(0);
 }
 
 /*
diff --git a/arch/s390/kernel/time.c b/arch/s390/kernel/time.c
index 386d37a228b..0931b110c82 100644
--- a/arch/s390/kernel/time.c
+++ b/arch/s390/kernel/time.c
@@ -226,7 +226,7 @@ void update_vsyscall(struct timekeeper *tk)
 	vdso_data->wtom_clock_sec =
 		tk->xtime_sec + tk->wall_to_monotonic.tv_sec;
 	vdso_data->wtom_clock_nsec = tk->xtime_nsec +
-		+ (tk->wall_to_monotonic.tv_nsec << tk->shift);
+		+ ((u64) tk->wall_to_monotonic.tv_nsec << tk->shift);
 	nsecps = (u64) NSEC_PER_SEC << tk->shift;
 	while (vdso_data->wtom_clock_nsec >= nsecps) {
 		vdso_data->wtom_clock_nsec -= nsecps;
diff --git a/arch/s390/kernel/topology.c b/arch/s390/kernel/topology.c
index 6298fed11ce..fa3b8cdaada 100644
--- a/arch/s390/kernel/topology.c
+++ b/arch/s390/kernel/topology.c
@@ -333,7 +333,9 @@ static void __init alloc_masks(struct sysinfo_15_1_x *info,
 		nr_masks *= info->mag[TOPOLOGY_NR_MAG - offset - 1 - i];
 	nr_masks = max(nr_masks, 1);
 	for (i = 0; i < nr_masks; i++) {
-		mask->next = alloc_bootmem(sizeof(struct mask_info));
+		mask->next = alloc_bootmem_align(
+			roundup_pow_of_two(sizeof(struct mask_info)),
+			roundup_pow_of_two(sizeof(struct mask_info)));
 		mask = mask->next;
 	}
 }
diff --git a/arch/s390/kvm/kvm-s390.c b/arch/s390/kvm/kvm-s390.c
index b3ecb8f5b6c..825fe7bf95a 100644
--- a/arch/s390/kvm/kvm-s390.c
+++ b/arch/s390/kvm/kvm-s390.c
@@ -158,6 +158,7 @@ int kvm_dev_ioctl_check_extension(long ext)
 	case KVM_CAP_ONE_REG:
 	case KVM_CAP_ENABLE_CAP:
 	case KVM_CAP_S390_CSS_SUPPORT:
+	case KVM_CAP_IRQFD:
 	case KVM_CAP_IOEVENTFD:
 	case KVM_CAP_DEVICE_CTRL:
 	case KVM_CAP_ENABLE_CAP_VM:
@@ -906,7 +907,7 @@ static int vcpu_pre_run(struct kvm_vcpu *vcpu)
 	if (need_resched())
 		schedule();
 
-	if (test_thread_flag(TIF_MCCK_PENDING))
+	if (test_cpu_flag(CIF_MCCK_PENDING))
 		s390_handle_mcck();
 
 	if (!kvm_is_ucontrol(vcpu->kvm))
diff --git a/arch/s390/lib/spinlock.c b/arch/s390/lib/spinlock.c
index f709983f41f..5b0e445bc3f 100644
--- a/arch/s390/lib/spinlock.c
+++ b/arch/s390/lib/spinlock.c
@@ -26,83 +26,81 @@ __setup("spin_retry=", spin_retry_setup);
 
 void arch_spin_lock_wait(arch_spinlock_t *lp)
 {
-	int count = spin_retry;
-	unsigned int cpu = ~smp_processor_id();
+	unsigned int cpu = SPINLOCK_LOCKVAL;
 	unsigned int owner;
+	int count;
 
 	while (1) {
-		owner = lp->owner_cpu;
-		if (!owner || smp_vcpu_scheduled(~owner)) {
-			for (count = spin_retry; count > 0; count--) {
-				if (arch_spin_is_locked(lp))
-					continue;
-				if (_raw_compare_and_swap(&lp->owner_cpu, 0,
-							  cpu) == 0)
-					return;
-			}
-			if (MACHINE_IS_LPAR)
-				continue;
+		owner = ACCESS_ONCE(lp->lock);
+		/* Try to get the lock if it is free. */
+		if (!owner) {
+			if (_raw_compare_and_swap(&lp->lock, 0, cpu))
+				return;
+			continue;
 		}
-		owner = lp->owner_cpu;
-		if (owner)
+		/* Check if the lock owner is running. */
+		if (!smp_vcpu_scheduled(~owner)) {
+			smp_yield_cpu(~owner);
+			continue;
+		}
+		/* Loop for a while on the lock value. */
+		count = spin_retry;
+		do {
+			owner = ACCESS_ONCE(lp->lock);
+		} while (owner && count-- > 0);
+		if (!owner)
+			continue;
+		/*
+		 * For multiple layers of hypervisors, e.g. z/VM + LPAR
+		 * yield the CPU if the lock is still unavailable.
+		 */
+		if (!MACHINE_IS_LPAR)
 			smp_yield_cpu(~owner);
-		if (_raw_compare_and_swap(&lp->owner_cpu, 0, cpu) == 0)
-			return;
 	}
 }
 EXPORT_SYMBOL(arch_spin_lock_wait);
 
 void arch_spin_lock_wait_flags(arch_spinlock_t *lp, unsigned long flags)
 {
-	int count = spin_retry;
-	unsigned int cpu = ~smp_processor_id();
+	unsigned int cpu = SPINLOCK_LOCKVAL;
 	unsigned int owner;
+	int count;
 
 	local_irq_restore(flags);
 	while (1) {
-		owner = lp->owner_cpu;
-		if (!owner || smp_vcpu_scheduled(~owner)) {
-			for (count = spin_retry; count > 0; count--) {
-				if (arch_spin_is_locked(lp))
-					continue;
-				local_irq_disable();
-				if (_raw_compare_and_swap(&lp->owner_cpu, 0,
-							  cpu) == 0)
-					return;
-				local_irq_restore(flags);
-			}
-			if (MACHINE_IS_LPAR)
-				continue;
+		owner = ACCESS_ONCE(lp->lock);
+		/* Try to get the lock if it is free. */
+		if (!owner) {
+			local_irq_disable();
+			if (_raw_compare_and_swap(&lp->lock, 0, cpu))
+				return;
+			local_irq_restore(flags);
 		}
-		owner = lp->owner_cpu;
-		if (owner)
+		/* Check if the lock owner is running. */
+		if (!smp_vcpu_scheduled(~owner)) {
 			smp_yield_cpu(~owner);
-		local_irq_disable();
-		if (_raw_compare_and_swap(&lp->owner_cpu, 0, cpu) == 0)
-			return;
-		local_irq_restore(flags);
-	}
-}
-EXPORT_SYMBOL(arch_spin_lock_wait_flags);
-
-int arch_spin_trylock_retry(arch_spinlock_t *lp)
-{
-	unsigned int cpu = ~smp_processor_id();
-	int count;
-
-	for (count = spin_retry; count > 0; count--) {
-		if (arch_spin_is_locked(lp))
 			continue;
-		if (_raw_compare_and_swap(&lp->owner_cpu, 0, cpu) == 0)
-			return 1;
+		}
+		/* Loop for a while on the lock value. */
+		count = spin_retry;
+		do {
+			owner = ACCESS_ONCE(lp->lock);
+		} while (owner && count-- > 0);
+		if (!owner)
+			continue;
+		/*
+		 * For multiple layers of hypervisors, e.g. z/VM + LPAR
+		 * yield the CPU if the lock is still unavailable.
+		 */
+		if (!MACHINE_IS_LPAR)
+			smp_yield_cpu(~owner);
 	}
-	return 0;
 }
-EXPORT_SYMBOL(arch_spin_trylock_retry);
+EXPORT_SYMBOL(arch_spin_lock_wait_flags);
 
-void arch_spin_relax(arch_spinlock_t *lock)
+void arch_spin_relax(arch_spinlock_t *lp)
 {
-	unsigned int cpu = lock->owner_cpu;
+	unsigned int cpu = lp->lock;
 	if (cpu != 0) {
 		if (MACHINE_IS_VM || MACHINE_IS_KVM ||
 		    !smp_vcpu_scheduled(~cpu))
@@ -111,6 +109,17 @@ void arch_spin_relax(arch_spinlock_t *lock)
 }
 EXPORT_SYMBOL(arch_spin_relax);
 
+int arch_spin_trylock_retry(arch_spinlock_t *lp)
+{
+	int count;
+
+	for (count = spin_retry; count > 0; count--)
+		if (arch_spin_trylock_once(lp))
+			return 1;
+	return 0;
+}
+EXPORT_SYMBOL(arch_spin_trylock_retry);
+
 void _raw_read_lock_wait(arch_rwlock_t *rw)
 {
 	unsigned int old;
@@ -121,10 +130,10 @@ void _raw_read_lock_wait(arch_rwlock_t *rw)
 			smp_yield();
 			count = spin_retry;
 		}
-		if (!arch_read_can_lock(rw))
+		old = ACCESS_ONCE(rw->lock);
+		if ((int) old < 0)
 			continue;
-		old = rw->lock & 0x7fffffffU;
-		if (_raw_compare_and_swap(&rw->lock, old, old + 1) == old)
+		if (_raw_compare_and_swap(&rw->lock, old, old + 1))
 			return;
 	}
 }
@@ -141,12 +150,13 @@ void _raw_read_lock_wait_flags(arch_rwlock_t *rw, unsigned long flags)
 			smp_yield();
 			count = spin_retry;
 		}
-		if (!arch_read_can_lock(rw))
+		old = ACCESS_ONCE(rw->lock);
+		if ((int) old < 0)
 			continue;
-		old = rw->lock & 0x7fffffffU;
 		local_irq_disable();
-		if (_raw_compare_and_swap(&rw->lock, old, old + 1) == old)
+		if (_raw_compare_and_swap(&rw->lock, old, old + 1))
 			return;
+		local_irq_restore(flags);
 	}
 }
 EXPORT_SYMBOL(_raw_read_lock_wait_flags);
@@ -157,10 +167,10 @@ int _raw_read_trylock_retry(arch_rwlock_t *rw)
 	int count = spin_retry;
 
 	while (count-- > 0) {
-		if (!arch_read_can_lock(rw))
+		old = ACCESS_ONCE(rw->lock);
+		if ((int) old < 0)
 			continue;
-		old = rw->lock & 0x7fffffffU;
-		if (_raw_compare_and_swap(&rw->lock, old, old + 1) == old)
+		if (_raw_compare_and_swap(&rw->lock, old, old + 1))
 			return 1;
 	}
 	return 0;
@@ -169,6 +179,7 @@ EXPORT_SYMBOL(_raw_read_trylock_retry);
 
 void _raw_write_lock_wait(arch_rwlock_t *rw)
 {
+	unsigned int old;
 	int count = spin_retry;
 
 	while (1) {
@@ -176,9 +187,10 @@ void _raw_write_lock_wait(arch_rwlock_t *rw)
 			smp_yield();
 			count = spin_retry;
 		}
-		if (!arch_write_can_lock(rw))
+		old = ACCESS_ONCE(rw->lock);
+		if (old)
 			continue;
-		if (_raw_compare_and_swap(&rw->lock, 0, 0x80000000) == 0)
+		if (_raw_compare_and_swap(&rw->lock, 0, 0x80000000))
 			return;
 	}
 }
@@ -186,6 +198,7 @@ EXPORT_SYMBOL(_raw_write_lock_wait);
 
 void _raw_write_lock_wait_flags(arch_rwlock_t *rw, unsigned long flags)
 {
+	unsigned int old;
 	int count = spin_retry;
 
 	local_irq_restore(flags);
@@ -194,23 +207,27 @@ void _raw_write_lock_wait_flags(arch_rwlock_t *rw, unsigned long flags)
 			smp_yield();
 			count = spin_retry;
 		}
-		if (!arch_write_can_lock(rw))
+		old = ACCESS_ONCE(rw->lock);
+		if (old)
 			continue;
 		local_irq_disable();
-		if (_raw_compare_and_swap(&rw->lock, 0, 0x80000000) == 0)
+		if (_raw_compare_and_swap(&rw->lock, 0, 0x80000000))
 			return;
+		local_irq_restore(flags);
 	}
 }
 EXPORT_SYMBOL(_raw_write_lock_wait_flags);
 
 int _raw_write_trylock_retry(arch_rwlock_t *rw)
 {
+	unsigned int old;
 	int count = spin_retry;
 
 	while (count-- > 0) {
-		if (!arch_write_can_lock(rw))
+		old = ACCESS_ONCE(rw->lock);
+		if (old)
 			continue;
-		if (_raw_compare_and_swap(&rw->lock, 0, 0x80000000) == 0)
+		if (_raw_compare_and_swap(&rw->lock, 0, 0x80000000))
 			return 1;
 	}
 	return 0;
diff --git a/arch/s390/lib/uaccess.c b/arch/s390/lib/uaccess.c
index 7416efe8eae..53dd5d7a0c9 100644
--- a/arch/s390/lib/uaccess.c
+++ b/arch/s390/lib/uaccess.c
@@ -76,7 +76,7 @@ static inline unsigned long copy_from_user_mvcp(void *x, const void __user *ptr,
 {
 	unsigned long tmp1, tmp2;
 
-	update_primary_asce(current);
+	load_kernel_asce();
 	tmp1 = -256UL;
 	asm volatile(
 		"   sacf  0\n"
@@ -159,7 +159,7 @@ static inline unsigned long copy_to_user_mvcs(void __user *ptr, const void *x,
 {
 	unsigned long tmp1, tmp2;
 
-	update_primary_asce(current);
+	load_kernel_asce();
 	tmp1 = -256UL;
 	asm volatile(
 		"   sacf  0\n"
@@ -225,7 +225,7 @@ static inline unsigned long copy_in_user_mvc(void __user *to, const void __user
 {
 	unsigned long tmp1;
 
-	update_primary_asce(current);
+	load_kernel_asce();
 	asm volatile(
 		"   sacf  256\n"
 		"  "AHI"  %0,-1\n"
@@ -292,7 +292,7 @@ static inline unsigned long clear_user_xc(void __user *to, unsigned long size)
 {
 	unsigned long tmp1, tmp2;
 
-	update_primary_asce(current);
+	load_kernel_asce();
 	asm volatile(
 		"   sacf  256\n"
 		"  "AHI"  %0,-1\n"
@@ -358,7 +358,7 @@ unsigned long __strnlen_user(const char __user *src, unsigned long size)
 {
 	if (unlikely(!size))
 		return 0;
-	update_primary_asce(current);
+	load_kernel_asce();
 	return strnlen_user_srst(src, size);
 }
 EXPORT_SYMBOL(__strnlen_user);
diff --git a/arch/s390/mm/fault.c b/arch/s390/mm/fault.c
index 2f51a998a67..3f3b35403d0 100644
--- a/arch/s390/mm/fault.c
+++ b/arch/s390/mm/fault.c
@@ -415,7 +415,7 @@ static inline int do_exception(struct pt_regs *regs, int access)
 	 * The instruction that caused the program check has
 	 * been nullified. Don't signal single step via SIGTRAP.
 	 */
-	clear_tsk_thread_flag(tsk, TIF_PER_TRAP);
+	clear_pt_regs_flag(regs, PIF_PER_TRAP);
 
 	if (notify_page_fault(regs))
 		return 0;
diff --git a/arch/s390/mm/mem_detect.c b/arch/s390/mm/mem_detect.c
index cca388253a3..5535cfe0ee1 100644
--- a/arch/s390/mm/mem_detect.c
+++ b/arch/s390/mm/mem_detect.c
@@ -6,130 +6,60 @@
 
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/memblock.h>
+#include <linux/init.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
 #include <asm/ipl.h>
 #include <asm/sclp.h>
 #include <asm/setup.h>
 
 #define ADDR2G (1ULL << 31)
 
-static void find_memory_chunks(struct mem_chunk chunk[], unsigned long maxsize)
+#define CHUNK_READ_WRITE 0
+#define CHUNK_READ_ONLY  1
+
+static inline void memblock_physmem_add(phys_addr_t start, phys_addr_t size)
+{
+	memblock_add_range(&memblock.memory, start, size, 0, 0);
+	memblock_add_range(&memblock.physmem, start, size, 0, 0);
+}
+
+void __init detect_memory_memblock(void)
 {
 	unsigned long long memsize, rnmax, rzm;
-	unsigned long addr = 0, size;
-	int i = 0, type;
+	unsigned long addr, size;
+	int type;
 
 	rzm = sclp_get_rzm();
 	rnmax = sclp_get_rnmax();
 	memsize = rzm * rnmax;
 	if (!rzm)
 		rzm = 1ULL << 17;
-	if (sizeof(long) == 4) {
+	if (IS_ENABLED(CONFIG_32BIT)) {
 		rzm = min(ADDR2G, rzm);
-		memsize = memsize ? min(ADDR2G, memsize) : ADDR2G;
+		memsize = min(ADDR2G, memsize);
 	}
-	if (maxsize)
-		memsize = memsize ? min((unsigned long)memsize, maxsize) : maxsize;
+	max_physmem_end = memsize;
+	addr = 0;
+	/* keep memblock lists close to the kernel */
+	memblock_set_bottom_up(true);
 	do {
 		size = 0;
 		type = tprot(addr);
 		do {
 			size += rzm;
-			if (memsize && addr + size >= memsize)
+			if (max_physmem_end && addr + size >= max_physmem_end)
 				break;
 		} while (type == tprot(addr + size));
 		if (type == CHUNK_READ_WRITE || type == CHUNK_READ_ONLY) {
-			if (memsize && (addr + size > memsize))
-				size = memsize - addr;
-			chunk[i].addr = addr;
-			chunk[i].size = size;
-			chunk[i].type = type;
-			i++;
+			if (max_physmem_end && (addr + size > max_physmem_end))
+				size = max_physmem_end - addr;
+			memblock_physmem_add(addr, size);
 		}
 		addr += size;
-	} while (addr < memsize && i < MEMORY_CHUNKS);
-}
-
-/**
- * detect_memory_layout - fill mem_chunk array with memory layout data
- * @chunk: mem_chunk array to be filled
- * @maxsize: maximum address where memory detection should stop
- *
- * Fills the passed in memory chunk array with the memory layout of the
- * machine. The array must have a size of at least MEMORY_CHUNKS and will
- * be fully initialized afterwards.
- * If the maxsize paramater has a value > 0 memory detection will stop at
- * that address. It is guaranteed that all chunks have an ending address
- * that is smaller than maxsize.
- * If maxsize is 0 all memory will be detected.
- */
-void detect_memory_layout(struct mem_chunk chunk[], unsigned long maxsize)
-{
-	unsigned long flags, flags_dat, cr0;
-
-	memset(chunk, 0, MEMORY_CHUNKS * sizeof(struct mem_chunk));
-	/*
-	 * Disable IRQs, DAT and low address protection so tprot does the
-	 * right thing and we don't get scheduled away with low address
-	 * protection disabled.
-	 */
-	local_irq_save(flags);
-	flags_dat = __arch_local_irq_stnsm(0xfb);
-	/*
-	 * In case DAT was enabled, make sure chunk doesn't reside in vmalloc
-	 * space. We have disabled DAT and any access to vmalloc area will
-	 * cause an exception.
-	 * If DAT was disabled we are called from early ipl code.
-	 */
-	if (test_bit(5, &flags_dat)) {
-		if (WARN_ON_ONCE(is_vmalloc_or_module_addr(chunk)))
-			goto out;
-	}
-	__ctl_store(cr0, 0, 0);
-	__ctl_clear_bit(0, 28);
-	find_memory_chunks(chunk, maxsize);
-	__ctl_load(cr0, 0, 0);
-out:
-	__arch_local_irq_ssm(flags_dat);
-	local_irq_restore(flags);
-}
-EXPORT_SYMBOL(detect_memory_layout);
-
-/*
- * Create memory hole with given address and size.
- */
-void create_mem_hole(struct mem_chunk mem_chunk[], unsigned long addr,
-		     unsigned long size)
-{
-	int i;
-
-	for (i = 0; i < MEMORY_CHUNKS; i++) {
-		struct mem_chunk *chunk = &mem_chunk[i];
-
-		if (chunk->size == 0)
-			continue;
-		if (addr > chunk->addr + chunk->size)
-			continue;
-		if (addr + size <= chunk->addr)
-			continue;
-		/* Split */
-		if ((addr > chunk->addr) &&
-		    (addr + size < chunk->addr + chunk->size)) {
-			struct mem_chunk *new = chunk + 1;
-
-			memmove(new, chunk, (MEMORY_CHUNKS-i-1) * sizeof(*new));
-			new->addr = addr + size;
-			new->size = chunk->addr + chunk->size - new->addr;
-			chunk->size = addr - chunk->addr;
-			continue;
-		} else if ((addr <= chunk->addr) &&
-			   (addr + size >= chunk->addr + chunk->size)) {
-			memmove(chunk, chunk + 1, (MEMORY_CHUNKS-i-1) * sizeof(*chunk));
-			memset(&mem_chunk[MEMORY_CHUNKS-1], 0, sizeof(*chunk));
-		} else if (addr + size < chunk->addr + chunk->size) {
-			chunk->size =  chunk->addr + chunk->size - addr - size;
-			chunk->addr = addr + size;
-		} else if (addr > chunk->addr) {
-			chunk->size = addr - chunk->addr;
-		}
-	}
+	} while (addr < max_physmem_end);
+	memblock_set_bottom_up(false);
+	if (!max_physmem_end)
+		max_physmem_end = memblock_end_of_DRAM();
 }
diff --git a/arch/s390/mm/page-states.c b/arch/s390/mm/page-states.c
index 27c50f4d90c..a90d45e9dfb 100644
--- a/arch/s390/mm/page-states.c
+++ b/arch/s390/mm/page-states.c
@@ -12,8 +12,6 @@
 #include <linux/mm.h>
 #include <linux/gfp.h>
 #include <linux/init.h>
-#include <asm/setup.h>
-#include <asm/ipl.h>
 
 #define ESSA_SET_STABLE		1
 #define ESSA_SET_UNUSED		2
@@ -43,14 +41,6 @@ void __init cmma_init(void)
 
 	if (!cmma_flag)
 		return;
-	/*
-	 * Disable CMM for dump, otherwise  the tprot based memory
-	 * detection can fail because of unstable pages.
-	 */
-	if (OLDMEM_BASE || ipl_info.type == IPL_TYPE_FCP_DUMP) {
-		cmma_flag = 0;
-		return;
-	}
 	asm volatile(
 		"       .insn rrf,0xb9ab0000,%1,%1,0,0\n"
 		"0:     la      %0,0\n"
diff --git a/arch/s390/mm/pgtable.c b/arch/s390/mm/pgtable.c
index d7cfd57815f..7881d4eb8b6 100644
--- a/arch/s390/mm/pgtable.c
+++ b/arch/s390/mm/pgtable.c
@@ -53,8 +53,10 @@ static void __crst_table_upgrade(void *arg)
 {
 	struct mm_struct *mm = arg;
 
-	if (current->active_mm == mm)
-		update_user_asce(mm, 1);
+	if (current->active_mm == mm) {
+		clear_user_asce();
+		set_user_asce(mm);
+	}
 	__tlb_flush_local();
 }
 
@@ -108,7 +110,7 @@ void crst_table_downgrade(struct mm_struct *mm, unsigned long limit)
 	pgd_t *pgd;
 
 	if (current->active_mm == mm) {
-		clear_user_asce(mm, 1);
+		clear_user_asce();
 		__tlb_flush_mm(mm);
 	}
 	while (mm->context.asce_limit > limit) {
@@ -134,7 +136,7 @@ void crst_table_downgrade(struct mm_struct *mm, unsigned long limit)
 		crst_table_free(mm, (unsigned long *) pgd);
 	}
 	if (current->active_mm == mm)
-		update_user_asce(mm, 1);
+		set_user_asce(mm);
 }
 #endif
 
diff --git a/arch/s390/mm/vmem.c b/arch/s390/mm/vmem.c
index 72b04de1828..fe9012a49aa 100644
--- a/arch/s390/mm/vmem.c
+++ b/arch/s390/mm/vmem.c
@@ -10,6 +10,7 @@
 #include <linux/list.h>
 #include <linux/hugetlb.h>
 #include <linux/slab.h>
+#include <linux/memblock.h>
 #include <asm/pgalloc.h>
 #include <asm/pgtable.h>
 #include <asm/setup.h>
@@ -66,7 +67,8 @@ static pte_t __ref *vmem_pte_alloc(unsigned long address)
 	if (slab_is_available())
 		pte = (pte_t *) page_table_alloc(&init_mm, address);
 	else
-		pte = alloc_bootmem(PTRS_PER_PTE * sizeof(pte_t));
+		pte = alloc_bootmem_align(PTRS_PER_PTE * sizeof(pte_t),
+					  PTRS_PER_PTE * sizeof(pte_t));
 	if (!pte)
 		return NULL;
 	clear_table((unsigned long *) pte, _PAGE_INVALID,
@@ -371,16 +373,14 @@ out:
 void __init vmem_map_init(void)
 {
 	unsigned long ro_start, ro_end;
-	unsigned long start, end;
-	int i;
+	struct memblock_region *reg;
+	phys_addr_t start, end;
 
 	ro_start = PFN_ALIGN((unsigned long)&_stext);
 	ro_end = (unsigned long)&_eshared & PAGE_MASK;
-	for (i = 0; i < MEMORY_CHUNKS; i++) {
-		if (!memory_chunk[i].size)
-			continue;
-		start = memory_chunk[i].addr;
-		end = memory_chunk[i].addr + memory_chunk[i].size;
+	for_each_memblock(memory, reg) {
+		start = reg->base;
+		end = reg->base + reg->size - 1;
 		if (start >= ro_end || end <= ro_start)
 			vmem_add_mem(start, end - start, 0);
 		else if (start >= ro_start && end <= ro_end)
@@ -400,23 +400,21 @@ void __init vmem_map_init(void)
 }
 
 /*
- * Convert memory chunk array to a memory segment list so there is a single
- * list that contains both r/w memory and shared memory segments.
+ * Convert memblock.memory  to a memory segment list so there is a single
+ * list that contains all memory segments.
  */
 static int __init vmem_convert_memory_chunk(void)
 {
+	struct memblock_region *reg;
 	struct memory_segment *seg;
-	int i;
 
 	mutex_lock(&vmem_mutex);
-	for (i = 0; i < MEMORY_CHUNKS; i++) {
-		if (!memory_chunk[i].size)
-			continue;
+	for_each_memblock(memory, reg) {
 		seg = kzalloc(sizeof(*seg), GFP_KERNEL);
 		if (!seg)
 			panic("Out of memory...\n");
-		seg->start = memory_chunk[i].addr;
-		seg->size = memory_chunk[i].size;
+		seg->start = reg->base;
+		seg->size = reg->size;
 		insert_memory_segment(seg);
 	}
 	mutex_unlock(&vmem_mutex);
diff --git a/arch/s390/net/bpf_jit_comp.c b/arch/s390/net/bpf_jit_comp.c
index 9c36dc398f9..e9f8fa9337f 100644
--- a/arch/s390/net/bpf_jit_comp.c
+++ b/arch/s390/net/bpf_jit_comp.c
@@ -276,7 +276,6 @@ static void bpf_jit_noleaks(struct bpf_jit *jit, struct sock_filter *filter)
 	case BPF_S_LD_W_IND:
 	case BPF_S_LD_H_IND:
 	case BPF_S_LD_B_IND:
-	case BPF_S_LDX_B_MSH:
 	case BPF_S_LD_IMM:
 	case BPF_S_LD_MEM:
 	case BPF_S_MISC_TXA:
@@ -812,7 +811,7 @@ static struct bpf_binary_header *bpf_alloc_binary(unsigned int bpfsize,
 		return NULL;
 	memset(header, 0, sz);
 	header->pages = sz / PAGE_SIZE;
-	hole = sz - (bpfsize + sizeof(*header));
+	hole = min(sz - (bpfsize + sizeof(*header)), PAGE_SIZE - sizeof(*header));
 	/* Insert random number of illegal instructions before BPF code
 	 * and make sure the first instruction starts at an even address.
 	 */
diff --git a/arch/s390/oprofile/hwsampler.c b/arch/s390/oprofile/hwsampler.c
index 276f2e26c76..e53c6f26880 100644
--- a/arch/s390/oprofile/hwsampler.c
+++ b/arch/s390/oprofile/hwsampler.c
@@ -209,13 +209,11 @@ static void init_all_cpu_buffers(void)
 	}
 }
 
-static int prepare_cpu_buffers(void)
+static void prepare_cpu_buffers(void)
 {
-	int cpu;
-	int rc;
 	struct hws_cpu_buffer *cb;
+	int cpu;
 
-	rc = 0;
 	for_each_online_cpu(cpu) {
 		cb = &per_cpu(sampler_cpu_buffer, cpu);
 		atomic_set(&cb->ext_params, 0);
@@ -230,8 +228,6 @@ static int prepare_cpu_buffers(void)
 		cb->oom = 0;
 		cb->stop_mode = 0;
 	}
-
-	return rc;
 }
 
 /*
@@ -1107,9 +1103,7 @@ int hwsampler_start_all(unsigned long rate)
 	if (rc)
 		goto start_all_exit;
 
-	rc = prepare_cpu_buffers();
-	if (rc)
-		goto start_all_exit;
+	prepare_cpu_buffers();
 
 	for_each_online_cpu(cpu) {
 		rc = start_sampling(cpu);
@@ -1156,7 +1150,7 @@ int hwsampler_stop_all(void)
 	rc = 0;
 	if (hws_state == HWS_INIT) {
 		mutex_unlock(&hws_sem);
-		return rc;
+		return 0;
 	}
 	hws_state = HWS_STOPPING;
 	mutex_unlock(&hws_sem);
diff --git a/arch/s390/pci/pci.c b/arch/s390/pci/pci.c
index 1df1d29ac81..bdf02570d1d 100644
--- a/arch/s390/pci/pci.c
+++ b/arch/s390/pci/pci.c
@@ -530,11 +530,6 @@ static void zpci_unmap_resources(struct zpci_dev *zdev)
 	}
 }
 
-int pcibios_add_platform_entries(struct pci_dev *pdev)
-{
-	return zpci_sysfs_add_device(&pdev->dev);
-}
-
 static int __init zpci_irq_init(void)
 {
 	int rc;
@@ -671,6 +666,7 @@ int pcibios_add_device(struct pci_dev *pdev)
 	int i;
 
 	zdev->pdev = pdev;
+	pdev->dev.groups = zpci_attr_groups;
 	zpci_map_resources(zdev);
 
 	for (i = 0; i < PCI_BAR_COUNT; i++) {
diff --git a/arch/s390/pci/pci_clp.c b/arch/s390/pci/pci_clp.c
index c747394029e..96545d7659f 100644
--- a/arch/s390/pci/pci_clp.c
+++ b/arch/s390/pci/pci_clp.c
@@ -114,6 +114,16 @@ static int clp_store_query_pci_fn(struct zpci_dev *zdev,
 	zdev->end_dma = response->edma;
 	zdev->pchid = response->pchid;
 	zdev->pfgid = response->pfgid;
+	zdev->pft = response->pft;
+	zdev->vfn = response->vfn;
+	zdev->uid = response->uid;
+
+	memcpy(zdev->pfip, response->pfip, sizeof(zdev->pfip));
+	if (response->util_str_avail) {
+		memcpy(zdev->util_str, response->util_str,
+		       sizeof(zdev->util_str));
+	}
+
 	return 0;
 }
 
diff --git a/arch/s390/pci/pci_event.c b/arch/s390/pci/pci_event.c
index 01e251b1da0..6d7f5a3016c 100644
--- a/arch/s390/pci/pci_event.c
+++ b/arch/s390/pci/pci_event.c
@@ -76,7 +76,7 @@ static void __zpci_event_availability(struct zpci_ccdf_avail *ccdf)
 
 	switch (ccdf->pec) {
 	case 0x0301: /* Standby -> Configured */
-		if (!zdev || zdev->state == ZPCI_FN_STATE_CONFIGURED)
+		if (!zdev || zdev->state != ZPCI_FN_STATE_STANDBY)
 			break;
 		zdev->state = ZPCI_FN_STATE_CONFIGURED;
 		zdev->fh = ccdf->fh;
@@ -86,7 +86,8 @@ static void __zpci_event_availability(struct zpci_ccdf_avail *ccdf)
 		pci_rescan_bus(zdev->bus);
 		break;
 	case 0x0302: /* Reserved -> Standby */
-		clp_add_pci_device(ccdf->fid, ccdf->fh, 0);
+		if (!zdev)
+			clp_add_pci_device(ccdf->fid, ccdf->fh, 0);
 		break;
 	case 0x0303: /* Deconfiguration requested */
 		if (pdev)
diff --git a/arch/s390/pci/pci_sysfs.c b/arch/s390/pci/pci_sysfs.c
index ab4a9139300..9190214b870 100644
--- a/arch/s390/pci/pci_sysfs.c
+++ b/arch/s390/pci/pci_sysfs.c
@@ -12,43 +12,29 @@
 #include <linux/stat.h>
 #include <linux/pci.h>
 
-static ssize_t show_fid(struct device *dev, struct device_attribute *attr,
-			char *buf)
-{
-	struct zpci_dev *zdev = get_zdev(to_pci_dev(dev));
-
-	return sprintf(buf, "0x%08x\n", zdev->fid);
-}
-static DEVICE_ATTR(function_id, S_IRUGO, show_fid, NULL);
-
-static ssize_t show_fh(struct device *dev, struct device_attribute *attr,
-		       char *buf)
-{
-	struct zpci_dev *zdev = get_zdev(to_pci_dev(dev));
-
-	return sprintf(buf, "0x%08x\n", zdev->fh);
-}
-static DEVICE_ATTR(function_handle, S_IRUGO, show_fh, NULL);
-
-static ssize_t show_pchid(struct device *dev, struct device_attribute *attr,
-			  char *buf)
-{
-	struct zpci_dev *zdev = get_zdev(to_pci_dev(dev));
-
-	return sprintf(buf, "0x%04x\n", zdev->pchid);
-}
-static DEVICE_ATTR(pchid, S_IRUGO, show_pchid, NULL);
-
-static ssize_t show_pfgid(struct device *dev, struct device_attribute *attr,
-			  char *buf)
-{
-	struct zpci_dev *zdev = get_zdev(to_pci_dev(dev));
-
-	return sprintf(buf, "0x%02x\n", zdev->pfgid);
-}
-static DEVICE_ATTR(pfgid, S_IRUGO, show_pfgid, NULL);
-
-static ssize_t store_recover(struct device *dev, struct device_attribute *attr,
+#define zpci_attr(name, fmt, member)					\
+static ssize_t name##_show(struct device *dev,				\
+			   struct device_attribute *attr, char *buf)	\
+{									\
+	struct zpci_dev *zdev = get_zdev(to_pci_dev(dev));		\
+									\
+	return sprintf(buf, fmt, zdev->member);				\
+}									\
+static DEVICE_ATTR_RO(name)
+
+zpci_attr(function_id, "0x%08x\n", fid);
+zpci_attr(function_handle, "0x%08x\n", fh);
+zpci_attr(pchid, "0x%04x\n", pchid);
+zpci_attr(pfgid, "0x%02x\n", pfgid);
+zpci_attr(vfn, "0x%04x\n", vfn);
+zpci_attr(pft, "0x%02x\n", pft);
+zpci_attr(uid, "0x%x\n", uid);
+zpci_attr(segment0, "0x%02x\n", pfip[0]);
+zpci_attr(segment1, "0x%02x\n", pfip[1]);
+zpci_attr(segment2, "0x%02x\n", pfip[2]);
+zpci_attr(segment3, "0x%02x\n", pfip[3]);
+
+static ssize_t recover_store(struct device *dev, struct device_attribute *attr,
 			     const char *buf, size_t count)
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
@@ -70,38 +56,55 @@ static ssize_t store_recover(struct device *dev, struct device_attribute *attr,
 	pci_rescan_bus(zdev->bus);
 	return count;
 }
-static DEVICE_ATTR(recover, S_IWUSR, NULL, store_recover);
+static DEVICE_ATTR_WO(recover);
 
-static struct device_attribute *zpci_dev_attrs[] = {
-	&dev_attr_function_id,
-	&dev_attr_function_handle,
-	&dev_attr_pchid,
-	&dev_attr_pfgid,
-	&dev_attr_recover,
-	NULL,
-};
-
-int zpci_sysfs_add_device(struct device *dev)
+static ssize_t util_string_read(struct file *filp, struct kobject *kobj,
+				struct bin_attribute *attr, char *buf,
+				loff_t off, size_t count)
 {
-	int i, rc = 0;
-
-	for (i = 0; zpci_dev_attrs[i]; i++) {
-		rc = device_create_file(dev, zpci_dev_attrs[i]);
-		if (rc)
-			goto error;
-	}
-	return 0;
+	struct device *dev = kobj_to_dev(kobj);
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct zpci_dev *zdev = get_zdev(pdev);
 
-error:
-	while (--i >= 0)
-		device_remove_file(dev, zpci_dev_attrs[i]);
-	return rc;
+	return memory_read_from_buffer(buf, count, &off, zdev->util_str,
+				       sizeof(zdev->util_str));
 }
+static BIN_ATTR_RO(util_string, CLP_UTIL_STR_LEN);
+static struct bin_attribute *zpci_bin_attrs[] = {
+	&bin_attr_util_string,
+	NULL,
+};
 
-void zpci_sysfs_remove_device(struct device *dev)
-{
-	int i;
+static struct attribute *zpci_dev_attrs[] = {
+	&dev_attr_function_id.attr,
+	&dev_attr_function_handle.attr,
+	&dev_attr_pchid.attr,
+	&dev_attr_pfgid.attr,
+	&dev_attr_pft.attr,
+	&dev_attr_vfn.attr,
+	&dev_attr_uid.attr,
+	&dev_attr_recover.attr,
+	NULL,
+};
+static struct attribute_group zpci_attr_group = {
+	.attrs = zpci_dev_attrs,
+	.bin_attrs = zpci_bin_attrs,
+};
 
-	for (i = 0; zpci_dev_attrs[i]; i++)
-		device_remove_file(dev, zpci_dev_attrs[i]);
-}
+static struct attribute *pfip_attrs[] = {
+	&dev_attr_segment0.attr,
+	&dev_attr_segment1.attr,
+	&dev_attr_segment2.attr,
+	&dev_attr_segment3.attr,
+	NULL,
+};
+static struct attribute_group pfip_attr_group = {
+	.name = "pfip",
+	.attrs = pfip_attrs,
+};
+
+const struct attribute_group *zpci_attr_groups[] = {
+	&zpci_attr_group,
+	&pfip_attr_group,
+	NULL,
+};
diff --git a/arch/sh/configs/apsh4ad0a_defconfig b/arch/sh/configs/apsh4ad0a_defconfig
index 95ae23fcfdd..ec70475da89 100644
--- a/arch/sh/configs/apsh4ad0a_defconfig
+++ b/arch/sh/configs/apsh4ad0a_defconfig
@@ -93,7 +93,6 @@ CONFIG_FONT_8x8=y
 CONFIG_FONT_8x16=y
 CONFIG_LOGO=y
 CONFIG_USB=y
-CONFIG_USB_DEBUG=y
 CONFIG_USB_MON=y
 CONFIG_USB_OHCI_HCD=y
 CONFIG_USB_STORAGE=y
diff --git a/arch/sh/configs/ecovec24_defconfig b/arch/sh/configs/ecovec24_defconfig
index c6c2becdc8a..0b364e3b0ff 100644
--- a/arch/sh/configs/ecovec24_defconfig
+++ b/arch/sh/configs/ecovec24_defconfig
@@ -107,7 +107,6 @@ CONFIG_SND_SOC=y
 CONFIG_SND_SOC_SH4_FSI=y
 CONFIG_SND_FSI_DA7210=y
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=y
 CONFIG_USB_R8A66597_HCD=y
 CONFIG_USB_STORAGE=y
diff --git a/arch/sh/configs/landisk_defconfig b/arch/sh/configs/landisk_defconfig
index 3670e937f2b..6783f31315c 100644
--- a/arch/sh/configs/landisk_defconfig
+++ b/arch/sh/configs/landisk_defconfig
@@ -80,7 +80,6 @@ CONFIG_HID_SAMSUNG=m
 CONFIG_HID_SONY=m
 CONFIG_HID_SUNPLUS=m
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=y
 CONFIG_USB_EHCI_HCD=y
 # CONFIG_USB_EHCI_TT_NEWSCHED is not set
diff --git a/arch/sh/configs/rsk7203_defconfig b/arch/sh/configs/rsk7203_defconfig
index 47236573db8..3c4f6f4d52b 100644
--- a/arch/sh/configs/rsk7203_defconfig
+++ b/arch/sh/configs/rsk7203_defconfig
@@ -100,7 +100,6 @@ CONFIG_HID_SONY=y
 CONFIG_HID_SUNPLUS=y
 CONFIG_USB=y
 CONFIG_USB_ANNOUNCE_NEW_DEVICES=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=y
 CONFIG_USB_R8A66597_HCD=y
 CONFIG_NEW_LEDS=y
diff --git a/arch/sh/configs/rsk7264_defconfig b/arch/sh/configs/rsk7264_defconfig
index 1600426224c..eecdf65bb78 100644
--- a/arch/sh/configs/rsk7264_defconfig
+++ b/arch/sh/configs/rsk7264_defconfig
@@ -60,7 +60,6 @@ CONFIG_SERIAL_SH_SCI_NR_UARTS=8
 CONFIG_SERIAL_SH_SCI_CONSOLE=y
 # CONFIG_HWMON is not set
 CONFIG_USB=y
-CONFIG_USB_DEBUG=y
 CONFIG_USB_ANNOUNCE_NEW_DEVICES=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_R8A66597_HCD=y
diff --git a/arch/sh/configs/rsk7269_defconfig b/arch/sh/configs/rsk7269_defconfig
index 9f062b5837d..8370b10df35 100644
--- a/arch/sh/configs/rsk7269_defconfig
+++ b/arch/sh/configs/rsk7269_defconfig
@@ -43,7 +43,6 @@ CONFIG_SERIAL_SH_SCI_NR_UARTS=8
 CONFIG_SERIAL_SH_SCI_CONSOLE=y
 # CONFIG_HWMON is not set
 CONFIG_USB=y
-CONFIG_USB_DEBUG=y
 CONFIG_USB_ANNOUNCE_NEW_DEVICES=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_R8A66597_HCD=y
diff --git a/arch/sh/configs/sdk7780_defconfig b/arch/sh/configs/sdk7780_defconfig
index ae1115849dd..6a96b9a2f7a 100644
--- a/arch/sh/configs/sdk7780_defconfig
+++ b/arch/sh/configs/sdk7780_defconfig
@@ -100,8 +100,6 @@ CONFIG_HID_SAMSUNG=y
 CONFIG_HID_SONY=y
 CONFIG_HID_SUNPLUS=y
 CONFIG_USB=y
-CONFIG_USB_DEBUG=y
-CONFIG_USB_DEVICEFS=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_MON=y
 CONFIG_USB_EHCI_HCD=y
diff --git a/arch/sh/configs/se7343_defconfig b/arch/sh/configs/se7343_defconfig
index be9c474197b..201acb4652f 100644
--- a/arch/sh/configs/se7343_defconfig
+++ b/arch/sh/configs/se7343_defconfig
@@ -92,9 +92,7 @@ CONFIG_HID_SAMSUNG=y
 CONFIG_HID_SONY=y
 CONFIG_HID_SUNPLUS=y
 CONFIG_USB=y
-CONFIG_USB_DEBUG=y
 CONFIG_USB_ANNOUNCE_NEW_DEVICES=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_ISP116X_HCD=y
 CONFIG_UIO=y
 CONFIG_EXT2_FS=y
diff --git a/arch/sh/configs/se7780_defconfig b/arch/sh/configs/se7780_defconfig
index c8c5e7f7a68..b0ef63ce525 100644
--- a/arch/sh/configs/se7780_defconfig
+++ b/arch/sh/configs/se7780_defconfig
@@ -94,7 +94,6 @@ CONFIG_HID_SAMSUNG=y
 CONFIG_HID_SONY=y
 CONFIG_HID_SUNPLUS=y
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_MON=y
 CONFIG_USB_EHCI_HCD=y
diff --git a/arch/sh/configs/sh2007_defconfig b/arch/sh/configs/sh2007_defconfig
index 0d2f41472a1..0c08d9244c9 100644
--- a/arch/sh/configs/sh2007_defconfig
+++ b/arch/sh/configs/sh2007_defconfig
@@ -97,7 +97,6 @@ CONFIG_FRAMEBUFFER_CONSOLE_DETECT_PRIMARY=y
 CONFIG_LOGO=y
 # CONFIG_HID_SUPPORT is not set
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 # CONFIG_USB_DEVICE_CLASS is not set
 CONFIG_USB_MON=y
 CONFIG_NEW_LEDS=y
diff --git a/arch/sh/configs/sh7785lcr_defconfig b/arch/sh/configs/sh7785lcr_defconfig
index 51561f5677d..d29da4a0f6c 100644
--- a/arch/sh/configs/sh7785lcr_defconfig
+++ b/arch/sh/configs/sh7785lcr_defconfig
@@ -88,7 +88,6 @@ CONFIG_HID_SAMSUNG=y
 CONFIG_HID_SONY=y
 CONFIG_HID_SUNPLUS=y
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=y
 CONFIG_USB_EHCI_HCD=m
 # CONFIG_USB_EHCI_TT_NEWSCHED is not set
diff --git a/arch/sh/configs/titan_defconfig b/arch/sh/configs/titan_defconfig
index e2cbd92d520..a77b778c745 100644
--- a/arch/sh/configs/titan_defconfig
+++ b/arch/sh/configs/titan_defconfig
@@ -215,7 +215,6 @@ CONFIG_WATCHDOG=y
 CONFIG_SH_WDT=m
 # CONFIG_USB_HID is not set
 CONFIG_USB=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=y
 CONFIG_USB_EHCI_HCD=y
 CONFIG_USB_EHCI_ROOT_HUB_TT=y
diff --git a/arch/sh/configs/urquell_defconfig b/arch/sh/configs/urquell_defconfig
index d7f89be9f47..1e843dbed5f 100644
--- a/arch/sh/configs/urquell_defconfig
+++ b/arch/sh/configs/urquell_defconfig
@@ -117,7 +117,6 @@ CONFIG_HID_SONY=y
 CONFIG_HID_SUNPLUS=y
 CONFIG_USB=y
 CONFIG_USB_ANNOUNCE_NEW_DEVICES=y
-CONFIG_USB_DEVICEFS=y
 CONFIG_USB_MON=y
 CONFIG_USB_OHCI_HCD=y
 CONFIG_USB_STORAGE=y
diff --git a/arch/sh/drivers/pci/fixups-dreamcast.c b/arch/sh/drivers/pci/fixups-dreamcast.c
index d6cde700e31..1d1c5a227e5 100644
--- a/arch/sh/drivers/pci/fixups-dreamcast.c
+++ b/arch/sh/drivers/pci/fixups-dreamcast.c
@@ -31,6 +31,8 @@
 static void gapspci_fixup_resources(struct pci_dev *dev)
 {
 	struct pci_channel *p = dev->sysdata;
+	struct resource res;
+	struct pci_bus_region region;
 
 	printk(KERN_NOTICE "PCI: Fixing up device %s\n", pci_name(dev));
 
@@ -50,11 +52,21 @@ static void gapspci_fixup_resources(struct pci_dev *dev)
 
 		/*
 		 * Redirect dma memory allocations to special memory window.
+		 *
+		 * If this GAPSPCI region were mapped by a BAR, the CPU
+		 * phys_addr_t would be pci_resource_start(), and the bus
+		 * address would be pci_bus_address(pci_resource_start()).
+		 * But apparently there's no BAR mapping it, so we just
+		 * "know" its CPU address is GAPSPCI_DMA_BASE.
 		 */
+		res.start = GAPSPCI_DMA_BASE;
+		res.end = GAPSPCI_DMA_BASE + GAPSPCI_DMA_SIZE - 1;
+		res.flags = IORESOURCE_MEM;
+		pcibios_resource_to_bus(dev->bus, &region, &res);
 		BUG_ON(!dma_declare_coherent_memory(&dev->dev,
-						GAPSPCI_DMA_BASE,
-						GAPSPCI_DMA_BASE,
-						GAPSPCI_DMA_SIZE,
+						res.start,
+						region.start,
+						resource_size(&res),
 						DMA_MEMORY_MAP |
 						DMA_MEMORY_EXCLUSIVE));
 		break;
diff --git a/arch/sh/include/asm/pci.h b/arch/sh/include/asm/pci.h
index bff96c2e7d2..5b451155299 100644
--- a/arch/sh/include/asm/pci.h
+++ b/arch/sh/include/asm/pci.h
@@ -70,11 +70,6 @@ extern int pci_mmap_page_range(struct pci_dev *dev, struct vm_area_struct *vma,
 	enum pci_mmap_state mmap_state, int write_combine);
 extern void pcibios_set_master(struct pci_dev *dev);
 
-static inline void pcibios_penalize_isa_irq(int irq, int active)
-{
-	/* We don't do dynamic PCI IRQ allocation */
-}
-
 /* Dynamic DMA mapping stuff.
  * SuperH has everything mapped statically like x86.
  */
diff --git a/arch/sh/include/asm/tlb.h b/arch/sh/include/asm/tlb.h
index 362192ed12f..62f80d2a9df 100644
--- a/arch/sh/include/asm/tlb.h
+++ b/arch/sh/include/asm/tlb.h
@@ -86,6 +86,14 @@ tlb_end_vma(struct mmu_gather *tlb, struct vm_area_struct *vma)
 	}
 }
 
+static inline void tlb_flush_mmu_tlbonly(struct mmu_gather *tlb)
+{
+}
+
+static inline void tlb_flush_mmu_free(struct mmu_gather *tlb)
+{
+}
+
 static inline void tlb_flush_mmu(struct mmu_gather *tlb)
 {
 }
diff --git a/arch/sh/kernel/cpu/clock-cpg.c b/arch/sh/kernel/cpu/clock-cpg.c
index f59b1f30d44..8525a671266 100644
--- a/arch/sh/kernel/cpu/clock-cpg.c
+++ b/arch/sh/kernel/cpu/clock-cpg.c
@@ -56,9 +56,13 @@ int __init __deprecated cpg_clk_init(void)
 
 	clkdev_add_table(lookups, ARRAY_SIZE(lookups));
 
-	clk_add_alias("tmu_fck", NULL, "peripheral_clk", NULL);
-	clk_add_alias("mtu2_fck", NULL, "peripheral_clk", NULL);
-	clk_add_alias("cmt_fck", NULL, "peripheral_clk", NULL);
+	clk_add_alias("fck", "sh-tmu-sh3.0", "peripheral_clk", NULL);
+	clk_add_alias("fck", "sh-tmu.0", "peripheral_clk", NULL);
+	clk_add_alias("fck", "sh-tmu.1", "peripheral_clk", NULL);
+	clk_add_alias("fck", "sh-tmu.2", "peripheral_clk", NULL);
+	clk_add_alias("fck", "sh-mtu2", "peripheral_clk", NULL);
+	clk_add_alias("fck", "sh-cmt-16.0", "peripheral_clk", NULL);
+	clk_add_alias("fck", "sh-cmt-32.0", "peripheral_clk", NULL);
 	clk_add_alias("sci_ick", NULL, "peripheral_clk", NULL);
 
 	return ret;
diff --git a/arch/sh/kernel/cpu/sh2/setup-sh7619.c b/arch/sh/kernel/cpu/sh2/setup-sh7619.c
index 3860b0be56c..58c19adae90 100644
--- a/arch/sh/kernel/cpu/sh2/setup-sh7619.c
+++ b/arch/sh/kernel/cpu/sh2/setup-sh7619.c
@@ -152,62 +152,24 @@ static struct platform_device eth_device = {
 	.resource = eth_resources,
 };
 
-static struct sh_timer_config cmt0_platform_data = {
-	.channel_offset = 0x02,
-	.timer_bit = 0,
-	.clockevent_rating = 125,
-	.clocksource_rating = 0, /* disabled due to code generation issues */
+static struct sh_timer_config cmt_platform_data = {
+	.channels_mask = 3,
 };
 
-static struct resource cmt0_resources[] = {
-	[0] = {
-		.start	= 0xf84a0072,
-		.end	= 0xf84a0077,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= 86,
-		.flags	= IORESOURCE_IRQ,
-	},
+static struct resource cmt_resources[] = {
+	DEFINE_RES_MEM(0xf84a0070, 0x10),
+	DEFINE_RES_IRQ(86),
+	DEFINE_RES_IRQ(87),
 };
 
-static struct platform_device cmt0_device = {
-	.name		= "sh_cmt",
+static struct platform_device cmt_device = {
+	.name		= "sh-cmt-16",
 	.id		= 0,
 	.dev = {
-		.platform_data	= &cmt0_platform_data,
-	},
-	.resource	= cmt0_resources,
-	.num_resources	= ARRAY_SIZE(cmt0_resources),
-};
-
-static struct sh_timer_config cmt1_platform_data = {
-	.channel_offset = 0x08,
-	.timer_bit = 1,
-	.clockevent_rating = 125,
-	.clocksource_rating = 0, /* disabled due to code generation issues */
-};
-
-static struct resource cmt1_resources[] = {
-	[0] = {
-		.start	= 0xf84a0078,
-		.end	= 0xf84a007d,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= 87,
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device cmt1_device = {
-	.name		= "sh_cmt",
-	.id		= 1,
-	.dev = {
-		.platform_data	= &cmt1_platform_data,
+		.platform_data	= &cmt_platform_data,
 	},
-	.resource	= cmt1_resources,
-	.num_resources	= ARRAY_SIZE(cmt1_resources),
+	.resource	= cmt_resources,
+	.num_resources	= ARRAY_SIZE(cmt_resources),
 };
 
 static struct platform_device *sh7619_devices[] __initdata = {
@@ -215,8 +177,7 @@ static struct platform_device *sh7619_devices[] __initdata = {
 	&scif1_device,
 	&scif2_device,
 	&eth_device,
-	&cmt0_device,
-	&cmt1_device,
+	&cmt_device,
 };
 
 static int __init sh7619_devices_setup(void)
@@ -235,8 +196,7 @@ static struct platform_device *sh7619_early_devices[] __initdata = {
 	&scif0_device,
 	&scif1_device,
 	&scif2_device,
-	&cmt0_device,
-	&cmt1_device,
+	&cmt_device,
 };
 
 #define STBCR3 0xf80a0000
diff --git a/arch/sh/kernel/cpu/sh2a/clock-sh7264.c b/arch/sh/kernel/cpu/sh2a/clock-sh7264.c
index fdf585c9528..8638fba6cd7 100644
--- a/arch/sh/kernel/cpu/sh2a/clock-sh7264.c
+++ b/arch/sh/kernel/cpu/sh2a/clock-sh7264.c
@@ -117,9 +117,9 @@ static struct clk_lookup lookups[] = {
 	/* MSTP clocks */
 	CLKDEV_CON_ID("sci_ick", &mstp_clks[MSTP77]),
 	CLKDEV_CON_ID("vdc3", &mstp_clks[MSTP74]),
-	CLKDEV_CON_ID("cmt_fck", &mstp_clks[MSTP72]),
+	CLKDEV_ICK_ID("fck", "sh-cmt-16.0", &mstp_clks[MSTP72]),
 	CLKDEV_CON_ID("usb0", &mstp_clks[MSTP60]),
-	CLKDEV_CON_ID("mtu2_fck", &mstp_clks[MSTP35]),
+	CLKDEV_ICK_ID("fck", "sh-mtu2", &mstp_clks[MSTP35]),
 	CLKDEV_CON_ID("sdhi0", &mstp_clks[MSTP34]),
 	CLKDEV_CON_ID("sdhi1", &mstp_clks[MSTP33]),
 	CLKDEV_CON_ID("adc0", &mstp_clks[MSTP32]),
diff --git a/arch/sh/kernel/cpu/sh2a/clock-sh7269.c b/arch/sh/kernel/cpu/sh2a/clock-sh7269.c
index 6b787620de9..f8a5c2abdfb 100644
--- a/arch/sh/kernel/cpu/sh2a/clock-sh7269.c
+++ b/arch/sh/kernel/cpu/sh2a/clock-sh7269.c
@@ -158,9 +158,9 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_ICK_ID("sci_fck", "sh-sci.5", &mstp_clks[MSTP42]),
 	CLKDEV_ICK_ID("sci_fck", "sh-sci.6", &mstp_clks[MSTP41]),
 	CLKDEV_ICK_ID("sci_fck", "sh-sci.7", &mstp_clks[MSTP40]),
-	CLKDEV_CON_ID("cmt_fck", &mstp_clks[MSTP72]),
+	CLKDEV_ICK_ID("fck", "sh-cmt-16.0", &mstp_clks[MSTP72]),
 	CLKDEV_CON_ID("usb0", &mstp_clks[MSTP60]),
-	CLKDEV_CON_ID("mtu2_fck", &mstp_clks[MSTP35]),
+	CLKDEV_ICK_ID("fck", "sh-mtu2", &mstp_clks[MSTP35]),
 	CLKDEV_CON_ID("adc0", &mstp_clks[MSTP32]),
 	CLKDEV_CON_ID("rtc0", &mstp_clks[MSTP30]),
 };
diff --git a/arch/sh/kernel/cpu/sh2a/setup-mxg.c b/arch/sh/kernel/cpu/sh2a/setup-mxg.c
index 63e996f9a7e..26fcdbd4127 100644
--- a/arch/sh/kernel/cpu/sh2a/setup-mxg.c
+++ b/arch/sh/kernel/cpu/sh2a/setup-mxg.c
@@ -114,88 +114,18 @@ static struct intc_mask_reg mask_registers[] __initdata = {
 static DECLARE_INTC_DESC(intc_desc, "mxg", vectors, groups,
 			 mask_registers, prio_registers, NULL);
 
-static struct sh_timer_config mtu2_0_platform_data = {
-	.channel_offset = -0x80,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
+static struct resource mtu2_resources[] = {
+	DEFINE_RES_MEM(0xff801000, 0x400),
+	DEFINE_RES_IRQ_NAMED(228, "tgi0a"),
+	DEFINE_RES_IRQ_NAMED(234, "tgi1a"),
+	DEFINE_RES_IRQ_NAMED(240, "tgi2a"),
 };
 
-static struct resource mtu2_0_resources[] = {
-	[0] = {
-		.start	= 0xff801300,
-		.end	= 0xff801326,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= 228,
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device mtu2_0_device = {
-	.name		= "sh_mtu2",
-	.id		= 0,
-	.dev = {
-		.platform_data	= &mtu2_0_platform_data,
-	},
-	.resource	= mtu2_0_resources,
-	.num_resources	= ARRAY_SIZE(mtu2_0_resources),
-};
-
-static struct sh_timer_config mtu2_1_platform_data = {
-	.channel_offset = -0x100,
-	.timer_bit = 1,
-	.clockevent_rating = 200,
-};
-
-static struct resource mtu2_1_resources[] = {
-	[0] = {
-		.start	= 0xff801380,
-		.end	= 0xff801390,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= 234,
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device mtu2_1_device = {
-	.name		= "sh_mtu2",
-	.id		= 1,
-	.dev = {
-		.platform_data	= &mtu2_1_platform_data,
-	},
-	.resource	= mtu2_1_resources,
-	.num_resources	= ARRAY_SIZE(mtu2_1_resources),
-};
-
-static struct sh_timer_config mtu2_2_platform_data = {
-	.channel_offset = 0x80,
-	.timer_bit = 2,
-	.clockevent_rating = 200,
-};
-
-static struct resource mtu2_2_resources[] = {
-	[0] = {
-		.start	= 0xff801000,
-		.end	= 0xff80100a,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= 240,
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device mtu2_2_device = {
-	.name		= "sh_mtu2",
-	.id		= 2,
-	.dev = {
-		.platform_data	= &mtu2_2_platform_data,
-	},
-	.resource	= mtu2_2_resources,
-	.num_resources	= ARRAY_SIZE(mtu2_2_resources),
+static struct platform_device mtu2_device = {
+	.name		= "sh-mtu2",
+	.id		= -1,
+	.resource	= mtu2_resources,
+	.num_resources	= ARRAY_SIZE(mtu2_resources),
 };
 
 static struct plat_sci_port scif0_platform_data = {
@@ -221,9 +151,7 @@ static struct platform_device scif0_device = {
 
 static struct platform_device *mxg_devices[] __initdata = {
 	&scif0_device,
-	&mtu2_0_device,
-	&mtu2_1_device,
-	&mtu2_2_device,
+	&mtu2_device,
 };
 
 static int __init mxg_devices_setup(void)
@@ -240,9 +168,7 @@ void __init plat_irq_setup(void)
 
 static struct platform_device *mxg_early_devices[] __initdata = {
 	&scif0_device,
-	&mtu2_0_device,
-	&mtu2_1_device,
-	&mtu2_2_device,
+	&mtu2_device,
 };
 
 void __init plat_early_device_setup(void)
diff --git a/arch/sh/kernel/cpu/sh2a/setup-sh7201.c b/arch/sh/kernel/cpu/sh2a/setup-sh7201.c
index 2c687446153..abc0ce9fb80 100644
--- a/arch/sh/kernel/cpu/sh2a/setup-sh7201.c
+++ b/arch/sh/kernel/cpu/sh2a/setup-sh7201.c
@@ -365,88 +365,18 @@ static struct platform_device rtc_device = {
 	.resource	= rtc_resources,
 };
 
-static struct sh_timer_config mtu2_0_platform_data = {
-	.channel_offset = -0x80,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
+static struct resource mtu2_resources[] = {
+	DEFINE_RES_MEM(0xfffe4000, 0x400),
+	DEFINE_RES_IRQ_NAMED(108, "tgi0a"),
+	DEFINE_RES_IRQ_NAMED(116, "tgi1a"),
+	DEFINE_RES_IRQ_NAMED(124, "tgi1b"),
 };
 
-static struct resource mtu2_0_resources[] = {
-	[0] = {
-		.start	= 0xfffe4300,
-		.end	= 0xfffe4326,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= 108,
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device mtu2_0_device = {
-	.name		= "sh_mtu2",
-	.id		= 0,
-	.dev = {
-		.platform_data	= &mtu2_0_platform_data,
-	},
-	.resource	= mtu2_0_resources,
-	.num_resources	= ARRAY_SIZE(mtu2_0_resources),
-};
-
-static struct sh_timer_config mtu2_1_platform_data = {
-	.channel_offset = -0x100,
-	.timer_bit = 1,
-	.clockevent_rating = 200,
-};
-
-static struct resource mtu2_1_resources[] = {
-	[0] = {
-		.start	= 0xfffe4380,
-		.end	= 0xfffe4390,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= 116,
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device mtu2_1_device = {
-	.name		= "sh_mtu2",
-	.id		= 1,
-	.dev = {
-		.platform_data	= &mtu2_1_platform_data,
-	},
-	.resource	= mtu2_1_resources,
-	.num_resources	= ARRAY_SIZE(mtu2_1_resources),
-};
-
-static struct sh_timer_config mtu2_2_platform_data = {
-	.channel_offset = 0x80,
-	.timer_bit = 2,
-	.clockevent_rating = 200,
-};
-
-static struct resource mtu2_2_resources[] = {
-	[0] = {
-		.start	= 0xfffe4000,
-		.end	= 0xfffe400a,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= 124,
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device mtu2_2_device = {
-	.name		= "sh_mtu2",
-	.id		= 2,
-	.dev = {
-		.platform_data	= &mtu2_2_platform_data,
-	},
-	.resource	= mtu2_2_resources,
-	.num_resources	= ARRAY_SIZE(mtu2_2_resources),
+static struct platform_device mtu2_device = {
+	.name		= "sh-mtu2",
+	.id		= -1,
+	.resource	= mtu2_resources,
+	.num_resources	= ARRAY_SIZE(mtu2_resources),
 };
 
 static struct platform_device *sh7201_devices[] __initdata = {
@@ -459,9 +389,7 @@ static struct platform_device *sh7201_devices[] __initdata = {
 	&scif6_device,
 	&scif7_device,
 	&rtc_device,
-	&mtu2_0_device,
-	&mtu2_1_device,
-	&mtu2_2_device,
+	&mtu2_device,
 };
 
 static int __init sh7201_devices_setup(void)
@@ -485,9 +413,7 @@ static struct platform_device *sh7201_early_devices[] __initdata = {
 	&scif5_device,
 	&scif6_device,
 	&scif7_device,
-	&mtu2_0_device,
-	&mtu2_1_device,
-	&mtu2_2_device,
+	&mtu2_device,
 };
 
 #define STBCR3 0xfffe0408
diff --git a/arch/sh/kernel/cpu/sh2a/setup-sh7203.c b/arch/sh/kernel/cpu/sh2a/setup-sh7203.c
index d55a0f30ada..3b4894cba92 100644
--- a/arch/sh/kernel/cpu/sh2a/setup-sh7203.c
+++ b/arch/sh/kernel/cpu/sh2a/setup-sh7203.c
@@ -265,118 +265,37 @@ static struct platform_device scif3_device = {
 	},
 };
 
-static struct sh_timer_config cmt0_platform_data = {
-	.channel_offset = 0x02,
-	.timer_bit = 0,
-	.clockevent_rating = 125,
-	.clocksource_rating = 0, /* disabled due to code generation issues */
+static struct sh_timer_config cmt_platform_data = {
+	.channels_mask = 3,
 };
 
-static struct resource cmt0_resources[] = {
-	[0] = {
-		.start	= 0xfffec002,
-		.end	= 0xfffec007,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= 142,
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device cmt0_device = {
-	.name		= "sh_cmt",
-	.id		= 0,
-	.dev = {
-		.platform_data	= &cmt0_platform_data,
-	},
-	.resource	= cmt0_resources,
-	.num_resources	= ARRAY_SIZE(cmt0_resources),
-};
-
-static struct sh_timer_config cmt1_platform_data = {
-	.channel_offset = 0x08,
-	.timer_bit = 1,
-	.clockevent_rating = 125,
-	.clocksource_rating = 0, /* disabled due to code generation issues */
-};
-
-static struct resource cmt1_resources[] = {
-	[0] = {
-		.start	= 0xfffec008,
-		.end	= 0xfffec00d,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= 143,
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device cmt1_device = {
-	.name		= "sh_cmt",
-	.id		= 1,
-	.dev = {
-		.platform_data	= &cmt1_platform_data,
-	},
-	.resource	= cmt1_resources,
-	.num_resources	= ARRAY_SIZE(cmt1_resources),
-};
-
-static struct sh_timer_config mtu2_0_platform_data = {
-	.channel_offset = -0x80,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
-};
-
-static struct resource mtu2_0_resources[] = {
-	[0] = {
-		.start	= 0xfffe4300,
-		.end	= 0xfffe4326,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= 146,
-		.flags	= IORESOURCE_IRQ,
-	},
+static struct resource cmt_resources[] = {
+	DEFINE_RES_MEM(0xfffec000, 0x10),
+	DEFINE_RES_IRQ(142),
+	DEFINE_RES_IRQ(143),
 };
 
-static struct platform_device mtu2_0_device = {
-	.name		= "sh_mtu2",
+static struct platform_device cmt_device = {
+	.name		= "sh-cmt-16",
 	.id		= 0,
 	.dev = {
-		.platform_data	= &mtu2_0_platform_data,
+		.platform_data	= &cmt_platform_data,
 	},
-	.resource	= mtu2_0_resources,
-	.num_resources	= ARRAY_SIZE(mtu2_0_resources),
+	.resource	= cmt_resources,
+	.num_resources	= ARRAY_SIZE(cmt_resources),
 };
 
-static struct sh_timer_config mtu2_1_platform_data = {
-	.channel_offset = -0x100,
-	.timer_bit = 1,
-	.clockevent_rating = 200,
+static struct resource mtu2_resources[] = {
+	DEFINE_RES_MEM(0xfffe4000, 0x400),
+	DEFINE_RES_IRQ_NAMED(146, "tgi0a"),
+	DEFINE_RES_IRQ_NAMED(153, "tgi1a"),
 };
 
-static struct resource mtu2_1_resources[] = {
-	[0] = {
-		.start	= 0xfffe4380,
-		.end	= 0xfffe4390,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= 153,
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device mtu2_1_device = {
-	.name		= "sh_mtu2",
-	.id		= 1,
-	.dev = {
-		.platform_data	= &mtu2_1_platform_data,
-	},
-	.resource	= mtu2_1_resources,
-	.num_resources	= ARRAY_SIZE(mtu2_1_resources),
+static struct platform_device mtu2_device = {
+	.name		= "sh-mtu2",
+	.id		= -1,
+	.resource	= mtu2_resources,
+	.num_resources	= ARRAY_SIZE(mtu2_resources),
 };
 
 static struct resource rtc_resources[] = {
@@ -404,10 +323,8 @@ static struct platform_device *sh7203_devices[] __initdata = {
 	&scif1_device,
 	&scif2_device,
 	&scif3_device,
-	&cmt0_device,
-	&cmt1_device,
-	&mtu2_0_device,
-	&mtu2_1_device,
+	&cmt_device,
+	&mtu2_device,
 	&rtc_device,
 };
 
@@ -428,10 +345,8 @@ static struct platform_device *sh7203_early_devices[] __initdata = {
 	&scif1_device,
 	&scif2_device,
 	&scif3_device,
-	&cmt0_device,
-	&cmt1_device,
-	&mtu2_0_device,
-	&mtu2_1_device,
+	&cmt_device,
+	&mtu2_device,
 };
 
 #define STBCR3 0xfffe0408
diff --git a/arch/sh/kernel/cpu/sh2a/setup-sh7206.c b/arch/sh/kernel/cpu/sh2a/setup-sh7206.c
index 241e745e3ce..49bc5a34bec 100644
--- a/arch/sh/kernel/cpu/sh2a/setup-sh7206.c
+++ b/arch/sh/kernel/cpu/sh2a/setup-sh7206.c
@@ -217,146 +217,38 @@ static struct platform_device scif3_device = {
 	},
 };
 
-static struct sh_timer_config cmt0_platform_data = {
-	.channel_offset = 0x02,
-	.timer_bit = 0,
-	.clockevent_rating = 125,
-	.clocksource_rating = 0, /* disabled due to code generation issues */
+static struct sh_timer_config cmt_platform_data = {
+	.channels_mask = 3,
 };
 
-static struct resource cmt0_resources[] = {
-	[0] = {
-		.start	= 0xfffec002,
-		.end	= 0xfffec007,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= 140,
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device cmt0_device = {
-	.name		= "sh_cmt",
-	.id		= 0,
-	.dev = {
-		.platform_data	= &cmt0_platform_data,
-	},
-	.resource	= cmt0_resources,
-	.num_resources	= ARRAY_SIZE(cmt0_resources),
-};
-
-static struct sh_timer_config cmt1_platform_data = {
-	.channel_offset = 0x08,
-	.timer_bit = 1,
-	.clockevent_rating = 125,
-	.clocksource_rating = 0, /* disabled due to code generation issues */
-};
-
-static struct resource cmt1_resources[] = {
-	[0] = {
-		.start	= 0xfffec008,
-		.end	= 0xfffec00d,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= 144,
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device cmt1_device = {
-	.name		= "sh_cmt",
-	.id		= 1,
-	.dev = {
-		.platform_data	= &cmt1_platform_data,
-	},
-	.resource	= cmt1_resources,
-	.num_resources	= ARRAY_SIZE(cmt1_resources),
-};
-
-static struct sh_timer_config mtu2_0_platform_data = {
-	.channel_offset = -0x80,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
-};
-
-static struct resource mtu2_0_resources[] = {
-	[0] = {
-		.start	= 0xfffe4300,
-		.end	= 0xfffe4326,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= 156,
-		.flags	= IORESOURCE_IRQ,
-	},
+static struct resource cmt_resources[] = {
+	DEFINE_RES_MEM(0xfffec000, 0x10),
+	DEFINE_RES_IRQ(140),
+	DEFINE_RES_IRQ(144),
 };
 
-static struct platform_device mtu2_0_device = {
-	.name		= "sh_mtu2",
+static struct platform_device cmt_device = {
+	.name		= "sh-cmt-16",
 	.id		= 0,
 	.dev = {
-		.platform_data	= &mtu2_0_platform_data,
+		.platform_data	= &cmt_platform_data,
 	},
-	.resource	= mtu2_0_resources,
-	.num_resources	= ARRAY_SIZE(mtu2_0_resources),
+	.resource	= cmt_resources,
+	.num_resources	= ARRAY_SIZE(cmt_resources),
 };
 
-static struct sh_timer_config mtu2_1_platform_data = {
-	.channel_offset = -0x100,
-	.timer_bit = 1,
-	.clockevent_rating = 200,
+static struct resource mtu2_resources[] = {
+	DEFINE_RES_MEM(0xfffe4000, 0x400),
+	DEFINE_RES_IRQ_NAMED(156, "tgi0a"),
+	DEFINE_RES_IRQ_NAMED(164, "tgi1a"),
+	DEFINE_RES_IRQ_NAMED(180, "tgi2a"),
 };
 
-static struct resource mtu2_1_resources[] = {
-	[0] = {
-		.start	= 0xfffe4380,
-		.end	= 0xfffe4390,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= 164,
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device mtu2_1_device = {
-	.name		= "sh_mtu2",
-	.id		= 1,
-	.dev = {
-		.platform_data	= &mtu2_1_platform_data,
-	},
-	.resource	= mtu2_1_resources,
-	.num_resources	= ARRAY_SIZE(mtu2_1_resources),
-};
-
-static struct sh_timer_config mtu2_2_platform_data = {
-	.channel_offset = 0x80,
-	.timer_bit = 2,
-	.clockevent_rating = 200,
-};
-
-static struct resource mtu2_2_resources[] = {
-	[0] = {
-		.start	= 0xfffe4000,
-		.end	= 0xfffe400a,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= 180,
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device mtu2_2_device = {
-	.name		= "sh_mtu2",
-	.id		= 2,
-	.dev = {
-		.platform_data	= &mtu2_2_platform_data,
-	},
-	.resource	= mtu2_2_resources,
-	.num_resources	= ARRAY_SIZE(mtu2_2_resources),
+static struct platform_device mtu2_device = {
+	.name		= "sh-mtu2s",
+	.id		= -1,
+	.resource	= mtu2_resources,
+	.num_resources	= ARRAY_SIZE(mtu2_resources),
 };
 
 static struct platform_device *sh7206_devices[] __initdata = {
@@ -364,11 +256,8 @@ static struct platform_device *sh7206_devices[] __initdata = {
 	&scif1_device,
 	&scif2_device,
 	&scif3_device,
-	&cmt0_device,
-	&cmt1_device,
-	&mtu2_0_device,
-	&mtu2_1_device,
-	&mtu2_2_device,
+	&cmt_device,
+	&mtu2_device,
 };
 
 static int __init sh7206_devices_setup(void)
@@ -388,11 +277,8 @@ static struct platform_device *sh7206_early_devices[] __initdata = {
 	&scif1_device,
 	&scif2_device,
 	&scif3_device,
-	&cmt0_device,
-	&cmt1_device,
-	&mtu2_0_device,
-	&mtu2_1_device,
-	&mtu2_2_device,
+	&cmt_device,
+	&mtu2_device,
 };
 
 #define STBCR3 0xfffe0408
diff --git a/arch/sh/kernel/cpu/sh2a/setup-sh7264.c b/arch/sh/kernel/cpu/sh2a/setup-sh7264.c
index ad5b0f42988..60814645556 100644
--- a/arch/sh/kernel/cpu/sh2a/setup-sh7264.c
+++ b/arch/sh/kernel/cpu/sh2a/setup-sh7264.c
@@ -433,125 +433,37 @@ static struct platform_device scif7_device = {
 	},
 };
 
-static struct sh_timer_config cmt0_platform_data = {
-	.channel_offset = 0x02,
-	.timer_bit = 0,
-	.clockevent_rating = 125,
-	.clocksource_rating = 0, /* disabled due to code generation issues */
+static struct sh_timer_config cmt_platform_data = {
+	.channels_mask = 3,
 };
 
-static struct resource cmt0_resources[] = {
-	[0] = {
-		.name	= "CMT0",
-		.start	= 0xfffec002,
-		.end	= 0xfffec007,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= 175,
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device cmt0_device = {
-	.name		= "sh_cmt",
-	.id		= 0,
-	.dev = {
-		.platform_data	= &cmt0_platform_data,
-	},
-	.resource	= cmt0_resources,
-	.num_resources	= ARRAY_SIZE(cmt0_resources),
-};
-
-static struct sh_timer_config cmt1_platform_data = {
-	.name = "CMT1",
-	.channel_offset = 0x08,
-	.timer_bit = 1,
-	.clockevent_rating = 125,
-	.clocksource_rating = 0, /* disabled due to code generation issues */
-};
-
-static struct resource cmt1_resources[] = {
-	[0] = {
-		.name	= "CMT1",
-		.start	= 0xfffec008,
-		.end	= 0xfffec00d,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= 176,
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device cmt1_device = {
-	.name		= "sh_cmt",
-	.id		= 1,
-	.dev = {
-		.platform_data	= &cmt1_platform_data,
-	},
-	.resource	= cmt1_resources,
-	.num_resources	= ARRAY_SIZE(cmt1_resources),
-};
-
-static struct sh_timer_config mtu2_0_platform_data = {
-	.name = "MTU2_0",
-	.channel_offset = -0x80,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
-};
-
-static struct resource mtu2_0_resources[] = {
-	[0] = {
-		.name	= "MTU2_0",
-		.start	= 0xfffe4300,
-		.end	= 0xfffe4326,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= 179,
-		.flags	= IORESOURCE_IRQ,
-	},
+static struct resource cmt_resources[] = {
+	DEFINE_RES_MEM(0xfffec000, 0x10),
+	DEFINE_RES_IRQ(175),
+	DEFINE_RES_IRQ(176),
 };
 
-static struct platform_device mtu2_0_device = {
-	.name		= "sh_mtu2",
+static struct platform_device cmt_device = {
+	.name		= "sh-cmt-16",
 	.id		= 0,
 	.dev = {
-		.platform_data	= &mtu2_0_platform_data,
+		.platform_data	= &cmt_platform_data,
 	},
-	.resource	= mtu2_0_resources,
-	.num_resources	= ARRAY_SIZE(mtu2_0_resources),
+	.resource	= cmt_resources,
+	.num_resources	= ARRAY_SIZE(cmt_resources),
 };
 
-static struct sh_timer_config mtu2_1_platform_data = {
-	.name = "MTU2_1",
-	.channel_offset = -0x100,
-	.timer_bit = 1,
-	.clockevent_rating = 200,
+static struct resource mtu2_resources[] = {
+	DEFINE_RES_MEM(0xfffe4000, 0x400),
+	DEFINE_RES_IRQ_NAMED(179, "tgi0a"),
+	DEFINE_RES_IRQ_NAMED(186, "tgi1a"),
 };
 
-static struct resource mtu2_1_resources[] = {
-	[0] = {
-		.name	= "MTU2_1",
-		.start	= 0xfffe4380,
-		.end	= 0xfffe4390,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= 186,
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device mtu2_1_device = {
-	.name		= "sh_mtu2",
-	.id		= 1,
-	.dev = {
-		.platform_data	= &mtu2_1_platform_data,
-	},
-	.resource	= mtu2_1_resources,
-	.num_resources	= ARRAY_SIZE(mtu2_1_resources),
+static struct platform_device mtu2_device = {
+	.name		= "sh-mtu2",
+	.id		= -1,
+	.resource	= mtu2_resources,
+	.num_resources	= ARRAY_SIZE(mtu2_resources),
 };
 
 static struct resource rtc_resources[] = {
@@ -620,10 +532,8 @@ static struct platform_device *sh7264_devices[] __initdata = {
 	&scif5_device,
 	&scif6_device,
 	&scif7_device,
-	&cmt0_device,
-	&cmt1_device,
-	&mtu2_0_device,
-	&mtu2_1_device,
+	&cmt_device,
+	&mtu2_device,
 	&rtc_device,
 	&r8a66597_usb_host_device,
 };
@@ -649,10 +559,8 @@ static struct platform_device *sh7264_early_devices[] __initdata = {
 	&scif5_device,
 	&scif6_device,
 	&scif7_device,
-	&cmt0_device,
-	&cmt1_device,
-	&mtu2_0_device,
-	&mtu2_1_device,
+	&cmt_device,
+	&mtu2_device,
 };
 
 void __init plat_early_device_setup(void)
diff --git a/arch/sh/kernel/cpu/sh2a/setup-sh7269.c b/arch/sh/kernel/cpu/sh2a/setup-sh7269.c
index 3995119f65d..16ce5aa77bd 100644
--- a/arch/sh/kernel/cpu/sh2a/setup-sh7269.c
+++ b/arch/sh/kernel/cpu/sh2a/setup-sh7269.c
@@ -455,118 +455,37 @@ static struct platform_device scif7_device = {
 	},
 };
 
-static struct sh_timer_config cmt0_platform_data = {
-	.channel_offset = 0x02,
-	.timer_bit = 0,
-	.clockevent_rating = 125,
-	.clocksource_rating = 0, /* disabled due to code generation issues */
+static struct sh_timer_config cmt_platform_data = {
+	.channels_mask = 3,
 };
 
-static struct resource cmt0_resources[] = {
-	[0] = {
-		.start	= 0xfffec002,
-		.end	= 0xfffec007,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= 188,
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device cmt0_device = {
-	.name		= "sh_cmt",
-	.id		= 0,
-	.dev = {
-		.platform_data	= &cmt0_platform_data,
-	},
-	.resource	= cmt0_resources,
-	.num_resources	= ARRAY_SIZE(cmt0_resources),
-};
-
-static struct sh_timer_config cmt1_platform_data = {
-	.channel_offset = 0x08,
-	.timer_bit = 1,
-	.clockevent_rating = 125,
-	.clocksource_rating = 0, /* disabled due to code generation issues */
-};
-
-static struct resource cmt1_resources[] = {
-	[0] = {
-		.start	= 0xfffec008,
-		.end	= 0xfffec00d,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= 189,
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device cmt1_device = {
-	.name		= "sh_cmt",
-	.id		= 1,
-	.dev = {
-		.platform_data	= &cmt1_platform_data,
-	},
-	.resource	= cmt1_resources,
-	.num_resources	= ARRAY_SIZE(cmt1_resources),
-};
-
-static struct sh_timer_config mtu2_0_platform_data = {
-	.channel_offset = -0x80,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
-};
-
-static struct resource mtu2_0_resources[] = {
-	[0] = {
-		.start	= 0xfffe4300,
-		.end	= 0xfffe4326,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= 192,
-		.flags	= IORESOURCE_IRQ,
-	},
+static struct resource cmt_resources[] = {
+	DEFINE_RES_MEM(0xfffec000, 0x10),
+	DEFINE_RES_IRQ(188),
+	DEFINE_RES_IRQ(189),
 };
 
-static struct platform_device mtu2_0_device = {
-	.name		= "sh_mtu2",
+static struct platform_device cmt_device = {
+	.name		= "sh-cmt-16",
 	.id		= 0,
 	.dev = {
-		.platform_data	= &mtu2_0_platform_data,
+		.platform_data	= &cmt_platform_data,
 	},
-	.resource	= mtu2_0_resources,
-	.num_resources	= ARRAY_SIZE(mtu2_0_resources),
+	.resource	= cmt_resources,
+	.num_resources	= ARRAY_SIZE(cmt_resources),
 };
 
-static struct sh_timer_config mtu2_1_platform_data = {
-	.channel_offset = -0x100,
-	.timer_bit = 1,
-	.clockevent_rating = 200,
+static struct resource mtu2_resources[] = {
+	DEFINE_RES_MEM(0xfffe4000, 0x400),
+	DEFINE_RES_IRQ_NAMED(192, "tgi0a"),
+	DEFINE_RES_IRQ_NAMED(203, "tgi1a"),
 };
 
-static struct resource mtu2_1_resources[] = {
-	[0] = {
-		.start	= 0xfffe4380,
-		.end	= 0xfffe4390,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= 203,
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device mtu2_1_device = {
-	.name		= "sh_mtu2",
-	.id		= 1,
-	.dev = {
-		.platform_data	= &mtu2_1_platform_data,
-	},
-	.resource	= mtu2_1_resources,
-	.num_resources	= ARRAY_SIZE(mtu2_1_resources),
+static struct platform_device mtu2_device = {
+	.name		= "sh-mtu2",
+	.id		= -1,
+	.resource	= mtu2_resources,
+	.num_resources	= ARRAY_SIZE(mtu2_resources),
 };
 
 static struct resource rtc_resources[] = {
@@ -629,10 +548,8 @@ static struct platform_device *sh7269_devices[] __initdata = {
 	&scif5_device,
 	&scif6_device,
 	&scif7_device,
-	&cmt0_device,
-	&cmt1_device,
-	&mtu2_0_device,
-	&mtu2_1_device,
+	&cmt_device,
+	&mtu2_device,
 	&rtc_device,
 	&r8a66597_usb_host_device,
 };
@@ -658,10 +575,8 @@ static struct platform_device *sh7269_early_devices[] __initdata = {
 	&scif5_device,
 	&scif6_device,
 	&scif7_device,
-	&cmt0_device,
-	&cmt1_device,
-	&mtu2_0_device,
-	&mtu2_1_device,
+	&cmt_device,
+	&mtu2_device,
 };
 
 void __init plat_early_device_setup(void)
diff --git a/arch/sh/kernel/cpu/sh3/setup-sh7705.c b/arch/sh/kernel/cpu/sh3/setup-sh7705.c
index c76b2543b85..6a72fd14de2 100644
--- a/arch/sh/kernel/cpu/sh3/setup-sh7705.c
+++ b/arch/sh/kernel/cpu/sh3/setup-sh7705.c
@@ -143,25 +143,18 @@ static struct platform_device rtc_device = {
 };
 
 static struct sh_timer_config tmu0_platform_data = {
-	.channel_offset = 0x02,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
+	.channels_mask = 7,
 };
 
 static struct resource tmu0_resources[] = {
-	[0] = {
-		.start	= 0xfffffe94,
-		.end	= 0xfffffe9f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x400),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xfffffe90, 0x2c),
+	DEFINE_RES_IRQ(evt2irq(0x400)),
+	DEFINE_RES_IRQ(evt2irq(0x420)),
+	DEFINE_RES_IRQ(evt2irq(0x440)),
 };
 
 static struct platform_device tmu0_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu-sh3",
 	.id		= 0,
 	.dev = {
 		.platform_data	= &tmu0_platform_data,
@@ -170,67 +163,10 @@ static struct platform_device tmu0_device = {
 	.num_resources	= ARRAY_SIZE(tmu0_resources),
 };
 
-static struct sh_timer_config tmu1_platform_data = {
-	.channel_offset = 0xe,
-	.timer_bit = 1,
-	.clocksource_rating = 200,
-};
-
-static struct resource tmu1_resources[] = {
-	[0] = {
-		.start	= 0xfffffea0,
-		.end	= 0xfffffeab,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x420),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu1_device = {
-	.name		= "sh_tmu",
-	.id		= 1,
-	.dev = {
-		.platform_data	= &tmu1_platform_data,
-	},
-	.resource	= tmu1_resources,
-	.num_resources	= ARRAY_SIZE(tmu1_resources),
-};
-
-static struct sh_timer_config tmu2_platform_data = {
-	.channel_offset = 0x1a,
-	.timer_bit = 2,
-};
-
-static struct resource tmu2_resources[] = {
-	[0] = {
-		.start	= 0xfffffeac,
-		.end	= 0xfffffebb,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x440),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu2_device = {
-	.name		= "sh_tmu",
-	.id		= 2,
-	.dev = {
-		.platform_data	= &tmu2_platform_data,
-	},
-	.resource	= tmu2_resources,
-	.num_resources	= ARRAY_SIZE(tmu2_resources),
-};
-
 static struct platform_device *sh7705_devices[] __initdata = {
 	&scif0_device,
 	&scif1_device,
 	&tmu0_device,
-	&tmu1_device,
-	&tmu2_device,
 	&rtc_device,
 };
 
@@ -245,8 +181,6 @@ static struct platform_device *sh7705_early_devices[] __initdata = {
 	&scif0_device,
 	&scif1_device,
 	&tmu0_device,
-	&tmu1_device,
-	&tmu2_device,
 };
 
 void __init plat_early_device_setup(void)
diff --git a/arch/sh/kernel/cpu/sh3/setup-sh770x.c b/arch/sh/kernel/cpu/sh3/setup-sh770x.c
index ff1465c0519..9139d14b9c5 100644
--- a/arch/sh/kernel/cpu/sh3/setup-sh770x.c
+++ b/arch/sh/kernel/cpu/sh3/setup-sh770x.c
@@ -185,25 +185,18 @@ static struct platform_device scif2_device = {
 #endif
 
 static struct sh_timer_config tmu0_platform_data = {
-	.channel_offset = 0x02,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
+	.channels_mask = 7,
 };
 
 static struct resource tmu0_resources[] = {
-	[0] = {
-		.start	= 0xfffffe94,
-		.end	= 0xfffffe9f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x400),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xfffffe90, 0x2c),
+	DEFINE_RES_IRQ(evt2irq(0x400)),
+	DEFINE_RES_IRQ(evt2irq(0x420)),
+	DEFINE_RES_IRQ(evt2irq(0x440)),
 };
 
 static struct platform_device tmu0_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu-sh3",
 	.id		= 0,
 	.dev = {
 		.platform_data	= &tmu0_platform_data,
@@ -212,61 +205,6 @@ static struct platform_device tmu0_device = {
 	.num_resources	= ARRAY_SIZE(tmu0_resources),
 };
 
-static struct sh_timer_config tmu1_platform_data = {
-	.channel_offset = 0xe,
-	.timer_bit = 1,
-	.clocksource_rating = 200,
-};
-
-static struct resource tmu1_resources[] = {
-	[0] = {
-		.start	= 0xfffffea0,
-		.end	= 0xfffffeab,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x420),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu1_device = {
-	.name		= "sh_tmu",
-	.id		= 1,
-	.dev = {
-		.platform_data	= &tmu1_platform_data,
-	},
-	.resource	= tmu1_resources,
-	.num_resources	= ARRAY_SIZE(tmu1_resources),
-};
-
-static struct sh_timer_config tmu2_platform_data = {
-	.channel_offset = 0x1a,
-	.timer_bit = 2,
-};
-
-static struct resource tmu2_resources[] = {
-	[0] = {
-		.start	= 0xfffffeac,
-		.end	= 0xfffffebb,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x440),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu2_device = {
-	.name		= "sh_tmu",
-	.id		= 2,
-	.dev = {
-		.platform_data	= &tmu2_platform_data,
-	},
-	.resource	= tmu2_resources,
-	.num_resources	= ARRAY_SIZE(tmu2_resources),
-};
-
 static struct platform_device *sh770x_devices[] __initdata = {
 	&scif0_device,
 #if defined(CONFIG_CPU_SUBTYPE_SH7706) || \
@@ -279,8 +217,6 @@ static struct platform_device *sh770x_devices[] __initdata = {
 	&scif2_device,
 #endif
 	&tmu0_device,
-	&tmu1_device,
-	&tmu2_device,
 	&rtc_device,
 };
 
@@ -303,8 +239,6 @@ static struct platform_device *sh770x_early_devices[] __initdata = {
 	&scif2_device,
 #endif
 	&tmu0_device,
-	&tmu1_device,
-	&tmu2_device,
 };
 
 void __init plat_early_device_setup(void)
diff --git a/arch/sh/kernel/cpu/sh3/setup-sh7710.c b/arch/sh/kernel/cpu/sh3/setup-sh7710.c
index e2ce9360ed5..e9ed300dba5 100644
--- a/arch/sh/kernel/cpu/sh3/setup-sh7710.c
+++ b/arch/sh/kernel/cpu/sh3/setup-sh7710.c
@@ -142,25 +142,18 @@ static struct platform_device scif1_device = {
 };
 
 static struct sh_timer_config tmu0_platform_data = {
-	.channel_offset = 0x02,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
+	.channels_mask = 7,
 };
 
 static struct resource tmu0_resources[] = {
-	[0] = {
-		.start	= 0xa412fe94,
-		.end	= 0xa412fe9f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x400),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xa412fe90, 0x28),
+	DEFINE_RES_IRQ(evt2irq(0x400)),
+	DEFINE_RES_IRQ(evt2irq(0x420)),
+	DEFINE_RES_IRQ(evt2irq(0x440)),
 };
 
 static struct platform_device tmu0_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu-sh3",
 	.id		= 0,
 	.dev = {
 		.platform_data	= &tmu0_platform_data,
@@ -169,67 +162,10 @@ static struct platform_device tmu0_device = {
 	.num_resources	= ARRAY_SIZE(tmu0_resources),
 };
 
-static struct sh_timer_config tmu1_platform_data = {
-	.channel_offset = 0xe,
-	.timer_bit = 1,
-	.clocksource_rating = 200,
-};
-
-static struct resource tmu1_resources[] = {
-	[0] = {
-		.start	= 0xa412fea0,
-		.end	= 0xa412feab,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x420),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu1_device = {
-	.name		= "sh_tmu",
-	.id		= 1,
-	.dev = {
-		.platform_data	= &tmu1_platform_data,
-	},
-	.resource	= tmu1_resources,
-	.num_resources	= ARRAY_SIZE(tmu1_resources),
-};
-
-static struct sh_timer_config tmu2_platform_data = {
-	.channel_offset = 0x1a,
-	.timer_bit = 2,
-};
-
-static struct resource tmu2_resources[] = {
-	[0] = {
-		.start	= 0xa412feac,
-		.end	= 0xa412feb5,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x440),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu2_device = {
-	.name		= "sh_tmu",
-	.id		= 2,
-	.dev = {
-		.platform_data	= &tmu2_platform_data,
-	},
-	.resource	= tmu2_resources,
-	.num_resources	= ARRAY_SIZE(tmu2_resources),
-};
-
 static struct platform_device *sh7710_devices[] __initdata = {
 	&scif0_device,
 	&scif1_device,
 	&tmu0_device,
-	&tmu1_device,
-	&tmu2_device,
 	&rtc_device,
 };
 
@@ -244,8 +180,6 @@ static struct platform_device *sh7710_early_devices[] __initdata = {
 	&scif0_device,
 	&scif1_device,
 	&tmu0_device,
-	&tmu1_device,
-	&tmu2_device,
 };
 
 void __init plat_early_device_setup(void)
diff --git a/arch/sh/kernel/cpu/sh3/setup-sh7720.c b/arch/sh/kernel/cpu/sh3/setup-sh7720.c
index 1d5729dc072..84df85a5b80 100644
--- a/arch/sh/kernel/cpu/sh3/setup-sh7720.c
+++ b/arch/sh/kernel/cpu/sh3/setup-sh7720.c
@@ -152,163 +152,38 @@ static struct platform_device usbf_device = {
 	.resource	= usbf_resources,
 };
 
-static struct sh_timer_config cmt0_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 0,
-	.clockevent_rating = 125,
-	.clocksource_rating = 125,
+static struct sh_timer_config cmt_platform_data = {
+	.channels_mask = 0x1f,
 };
 
-static struct resource cmt0_resources[] = {
-	[0] = {
-		.start	= 0x044a0010,
-		.end	= 0x044a001b,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0xf00),
-		.flags	= IORESOURCE_IRQ,
-	},
+static struct resource cmt_resources[] = {
+	DEFINE_RES_MEM(0x044a0000, 0x60),
+	DEFINE_RES_IRQ(evt2irq(0xf00)),
 };
 
-static struct platform_device cmt0_device = {
-	.name		= "sh_cmt",
+static struct platform_device cmt_device = {
+	.name		= "sh-cmt-32",
 	.id		= 0,
 	.dev = {
-		.platform_data	= &cmt0_platform_data,
-	},
-	.resource	= cmt0_resources,
-	.num_resources	= ARRAY_SIZE(cmt0_resources),
-};
-
-static struct sh_timer_config cmt1_platform_data = {
-	.channel_offset = 0x20,
-	.timer_bit = 1,
-};
-
-static struct resource cmt1_resources[] = {
-	[0] = {
-		.start	= 0x044a0020,
-		.end	= 0x044a002b,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0xf00),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device cmt1_device = {
-	.name		= "sh_cmt",
-	.id		= 1,
-	.dev = {
-		.platform_data	= &cmt1_platform_data,
-	},
-	.resource	= cmt1_resources,
-	.num_resources	= ARRAY_SIZE(cmt1_resources),
-};
-
-static struct sh_timer_config cmt2_platform_data = {
-	.channel_offset = 0x30,
-	.timer_bit = 2,
-};
-
-static struct resource cmt2_resources[] = {
-	[0] = {
-		.start	= 0x044a0030,
-		.end	= 0x044a003b,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0xf00),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device cmt2_device = {
-	.name		= "sh_cmt",
-	.id		= 2,
-	.dev = {
-		.platform_data	= &cmt2_platform_data,
-	},
-	.resource	= cmt2_resources,
-	.num_resources	= ARRAY_SIZE(cmt2_resources),
-};
-
-static struct sh_timer_config cmt3_platform_data = {
-	.channel_offset = 0x40,
-	.timer_bit = 3,
-};
-
-static struct resource cmt3_resources[] = {
-	[0] = {
-		.start	= 0x044a0040,
-		.end	= 0x044a004b,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0xf00),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device cmt3_device = {
-	.name		= "sh_cmt",
-	.id		= 3,
-	.dev = {
-		.platform_data	= &cmt3_platform_data,
+		.platform_data	= &cmt_platform_data,
 	},
-	.resource	= cmt3_resources,
-	.num_resources	= ARRAY_SIZE(cmt3_resources),
-};
-
-static struct sh_timer_config cmt4_platform_data = {
-	.channel_offset = 0x50,
-	.timer_bit = 4,
-};
-
-static struct resource cmt4_resources[] = {
-	[0] = {
-		.start	= 0x044a0050,
-		.end	= 0x044a005b,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0xf00),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device cmt4_device = {
-	.name		= "sh_cmt",
-	.id		= 4,
-	.dev = {
-		.platform_data	= &cmt4_platform_data,
-	},
-	.resource	= cmt4_resources,
-	.num_resources	= ARRAY_SIZE(cmt4_resources),
+	.resource	= cmt_resources,
+	.num_resources	= ARRAY_SIZE(cmt_resources),
 };
 
 static struct sh_timer_config tmu0_platform_data = {
-	.channel_offset = 0x02,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
+	.channels_mask = 7,
 };
 
 static struct resource tmu0_resources[] = {
-	[0] = {
-		.start	= 0xa412fe94,
-		.end	= 0xa412fe9f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x400),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xa412fe90, 0x28),
+	DEFINE_RES_IRQ(evt2irq(0x400)),
+	DEFINE_RES_IRQ(evt2irq(0x420)),
+	DEFINE_RES_IRQ(evt2irq(0x440)),
 };
 
 static struct platform_device tmu0_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu-sh3",
 	.id		= 0,
 	.dev = {
 		.platform_data	= &tmu0_platform_data,
@@ -317,72 +192,11 @@ static struct platform_device tmu0_device = {
 	.num_resources	= ARRAY_SIZE(tmu0_resources),
 };
 
-static struct sh_timer_config tmu1_platform_data = {
-	.channel_offset = 0xe,
-	.timer_bit = 1,
-	.clocksource_rating = 200,
-};
-
-static struct resource tmu1_resources[] = {
-	[0] = {
-		.start	= 0xa412fea0,
-		.end	= 0xa412feab,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x420),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu1_device = {
-	.name		= "sh_tmu",
-	.id		= 1,
-	.dev = {
-		.platform_data	= &tmu1_platform_data,
-	},
-	.resource	= tmu1_resources,
-	.num_resources	= ARRAY_SIZE(tmu1_resources),
-};
-
-static struct sh_timer_config tmu2_platform_data = {
-	.channel_offset = 0x1a,
-	.timer_bit = 2,
-};
-
-static struct resource tmu2_resources[] = {
-	[0] = {
-		.start	= 0xa412feac,
-		.end	= 0xa412feb5,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x440),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu2_device = {
-	.name		= "sh_tmu",
-	.id		= 2,
-	.dev = {
-		.platform_data	= &tmu2_platform_data,
-	},
-	.resource	= tmu2_resources,
-	.num_resources	= ARRAY_SIZE(tmu2_resources),
-};
-
 static struct platform_device *sh7720_devices[] __initdata = {
 	&scif0_device,
 	&scif1_device,
-	&cmt0_device,
-	&cmt1_device,
-	&cmt2_device,
-	&cmt3_device,
-	&cmt4_device,
+	&cmt_device,
 	&tmu0_device,
-	&tmu1_device,
-	&tmu2_device,
 	&rtc_device,
 	&usb_ohci_device,
 	&usbf_device,
@@ -398,14 +212,8 @@ arch_initcall(sh7720_devices_setup);
 static struct platform_device *sh7720_early_devices[] __initdata = {
 	&scif0_device,
 	&scif1_device,
-	&cmt0_device,
-	&cmt1_device,
-	&cmt2_device,
-	&cmt3_device,
-	&cmt4_device,
+	&cmt_device,
 	&tmu0_device,
-	&tmu1_device,
-	&tmu2_device,
 };
 
 void __init plat_early_device_setup(void)
diff --git a/arch/sh/kernel/cpu/sh4/setup-sh4-202.c b/arch/sh/kernel/cpu/sh4/setup-sh4-202.c
index a8bd778d5ac..e7a7b3cdf68 100644
--- a/arch/sh/kernel/cpu/sh4/setup-sh4-202.c
+++ b/arch/sh/kernel/cpu/sh4/setup-sh4-202.c
@@ -41,25 +41,18 @@ static struct platform_device scif0_device = {
 };
 
 static struct sh_timer_config tmu0_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
+	.channels_mask = 7,
 };
 
 static struct resource tmu0_resources[] = {
-	[0] = {
-		.start	= 0xffd80008,
-		.end	= 0xffd80013,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x400),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xffd80000, 0x30),
+	DEFINE_RES_IRQ(evt2irq(0x400)),
+	DEFINE_RES_IRQ(evt2irq(0x420)),
+	DEFINE_RES_IRQ(evt2irq(0x440)),
 };
 
 static struct platform_device tmu0_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu",
 	.id		= 0,
 	.dev = {
 		.platform_data	= &tmu0_platform_data,
@@ -68,66 +61,9 @@ static struct platform_device tmu0_device = {
 	.num_resources	= ARRAY_SIZE(tmu0_resources),
 };
 
-static struct sh_timer_config tmu1_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-	.clocksource_rating = 200,
-};
-
-static struct resource tmu1_resources[] = {
-	[0] = {
-		.start	= 0xffd80014,
-		.end	= 0xffd8001f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x420),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu1_device = {
-	.name		= "sh_tmu",
-	.id		= 1,
-	.dev = {
-		.platform_data	= &tmu1_platform_data,
-	},
-	.resource	= tmu1_resources,
-	.num_resources	= ARRAY_SIZE(tmu1_resources),
-};
-
-static struct sh_timer_config tmu2_platform_data = {
-	.channel_offset = 0x1c,
-	.timer_bit = 2,
-};
-
-static struct resource tmu2_resources[] = {
-	[0] = {
-		.start	= 0xffd80020,
-		.end	= 0xffd8002f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x440),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu2_device = {
-	.name		= "sh_tmu",
-	.id		= 2,
-	.dev = {
-		.platform_data	= &tmu2_platform_data,
-	},
-	.resource	= tmu2_resources,
-	.num_resources	= ARRAY_SIZE(tmu2_resources),
-};
-
 static struct platform_device *sh4202_devices[] __initdata = {
 	&scif0_device,
 	&tmu0_device,
-	&tmu1_device,
-	&tmu2_device,
 };
 
 static int __init sh4202_devices_setup(void)
@@ -140,8 +76,6 @@ arch_initcall(sh4202_devices_setup);
 static struct platform_device *sh4202_early_devices[] __initdata = {
 	&scif0_device,
 	&tmu0_device,
-	&tmu1_device,
-	&tmu2_device,
 };
 
 void __init plat_early_device_setup(void)
diff --git a/arch/sh/kernel/cpu/sh4/setup-sh7750.c b/arch/sh/kernel/cpu/sh4/setup-sh7750.c
index a447a248491..5f08c59b9f3 100644
--- a/arch/sh/kernel/cpu/sh4/setup-sh7750.c
+++ b/arch/sh/kernel/cpu/sh4/setup-sh7750.c
@@ -82,25 +82,18 @@ static struct platform_device scif_device = {
 };
 
 static struct sh_timer_config tmu0_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
+	.channels_mask = 7,
 };
 
 static struct resource tmu0_resources[] = {
-	[0] = {
-		.start	= 0xffd80008,
-		.end	= 0xffd80013,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x400),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xffd80000, 0x30),
+	DEFINE_RES_IRQ(evt2irq(0x400)),
+	DEFINE_RES_IRQ(evt2irq(0x420)),
+	DEFINE_RES_IRQ(evt2irq(0x440)),
 };
 
 static struct platform_device tmu0_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu",
 	.id		= 0,
 	.dev = {
 		.platform_data	= &tmu0_platform_data,
@@ -109,26 +102,23 @@ static struct platform_device tmu0_device = {
 	.num_resources	= ARRAY_SIZE(tmu0_resources),
 };
 
+/* SH7750R, SH7751 and SH7751R all have two extra timer channels */
+#if defined(CONFIG_CPU_SUBTYPE_SH7750R) || \
+	defined(CONFIG_CPU_SUBTYPE_SH7751) || \
+	defined(CONFIG_CPU_SUBTYPE_SH7751R)
+
 static struct sh_timer_config tmu1_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-	.clocksource_rating = 200,
+	.channels_mask = 3,
 };
 
 static struct resource tmu1_resources[] = {
-	[0] = {
-		.start	= 0xffd80014,
-		.end	= 0xffd8001f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x420),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xfe100000, 0x20),
+	DEFINE_RES_IRQ(evt2irq(0xb00)),
+	DEFINE_RES_IRQ(evt2irq(0xb80)),
 };
 
 static struct platform_device tmu1_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu",
 	.id		= 1,
 	.dev = {
 		.platform_data	= &tmu1_platform_data,
@@ -137,104 +127,15 @@ static struct platform_device tmu1_device = {
 	.num_resources	= ARRAY_SIZE(tmu1_resources),
 };
 
-static struct sh_timer_config tmu2_platform_data = {
-	.channel_offset = 0x1c,
-	.timer_bit = 2,
-};
-
-static struct resource tmu2_resources[] = {
-	[0] = {
-		.start	= 0xffd80020,
-		.end	= 0xffd8002f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x440),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu2_device = {
-	.name		= "sh_tmu",
-	.id		= 2,
-	.dev = {
-		.platform_data	= &tmu2_platform_data,
-	},
-	.resource	= tmu2_resources,
-	.num_resources	= ARRAY_SIZE(tmu2_resources),
-};
-
-/* SH7750R, SH7751 and SH7751R all have two extra timer channels */
-#if defined(CONFIG_CPU_SUBTYPE_SH7750R) || \
-	defined(CONFIG_CPU_SUBTYPE_SH7751) || \
-	defined(CONFIG_CPU_SUBTYPE_SH7751R)
-
-static struct sh_timer_config tmu3_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
-};
-
-static struct resource tmu3_resources[] = {
-	[0] = {
-		.start	= 0xfe100008,
-		.end	= 0xfe100013,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0xb00),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu3_device = {
-	.name		= "sh_tmu",
-	.id		= 3,
-	.dev = {
-		.platform_data	= &tmu3_platform_data,
-	},
-	.resource	= tmu3_resources,
-	.num_resources	= ARRAY_SIZE(tmu3_resources),
-};
-
-static struct sh_timer_config tmu4_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-};
-
-static struct resource tmu4_resources[] = {
-	[0] = {
-		.start	= 0xfe100014,
-		.end	= 0xfe10001f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0xb80),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu4_device = {
-	.name		= "sh_tmu",
-	.id		= 4,
-	.dev = {
-		.platform_data	= &tmu4_platform_data,
-	},
-	.resource	= tmu4_resources,
-	.num_resources	= ARRAY_SIZE(tmu4_resources),
-};
-
 #endif
 
 static struct platform_device *sh7750_devices[] __initdata = {
 	&rtc_device,
 	&tmu0_device,
-	&tmu1_device,
-	&tmu2_device,
 #if defined(CONFIG_CPU_SUBTYPE_SH7750R) || \
 	defined(CONFIG_CPU_SUBTYPE_SH7751) || \
 	defined(CONFIG_CPU_SUBTYPE_SH7751R)
-	&tmu3_device,
-	&tmu4_device,
+	&tmu1_device,
 #endif
 };
 
@@ -254,13 +155,10 @@ arch_initcall(sh7750_devices_setup);
 
 static struct platform_device *sh7750_early_devices[] __initdata = {
 	&tmu0_device,
-	&tmu1_device,
-	&tmu2_device,
 #if defined(CONFIG_CPU_SUBTYPE_SH7750R) || \
 	defined(CONFIG_CPU_SUBTYPE_SH7751) || \
 	defined(CONFIG_CPU_SUBTYPE_SH7751R)
-	&tmu3_device,
-	&tmu4_device,
+	&tmu1_device,
 #endif
 };
 
diff --git a/arch/sh/kernel/cpu/sh4/setup-sh7760.c b/arch/sh/kernel/cpu/sh4/setup-sh7760.c
index 1abd9fb4a38..973b736b3b9 100644
--- a/arch/sh/kernel/cpu/sh4/setup-sh7760.c
+++ b/arch/sh/kernel/cpu/sh4/setup-sh7760.c
@@ -227,25 +227,18 @@ static struct platform_device scif3_device = {
 };
 
 static struct sh_timer_config tmu0_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
+	.channels_mask = 7,
 };
 
 static struct resource tmu0_resources[] = {
-	[0] = {
-		.start	= 0xffd80008,
-		.end	= 0xffd80013,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x400),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xffd80000, 0x30),
+	DEFINE_RES_IRQ(evt2irq(0x400)),
+	DEFINE_RES_IRQ(evt2irq(0x420)),
+	DEFINE_RES_IRQ(evt2irq(0x440)),
 };
 
 static struct platform_device tmu0_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu",
 	.id		= 0,
 	.dev = {
 		.platform_data	= &tmu0_platform_data,
@@ -254,61 +247,6 @@ static struct platform_device tmu0_device = {
 	.num_resources	= ARRAY_SIZE(tmu0_resources),
 };
 
-static struct sh_timer_config tmu1_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-	.clocksource_rating = 200,
-};
-
-static struct resource tmu1_resources[] = {
-	[0] = {
-		.start	= 0xffd80014,
-		.end	= 0xffd8001f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x420),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu1_device = {
-	.name		= "sh_tmu",
-	.id		= 1,
-	.dev = {
-		.platform_data	= &tmu1_platform_data,
-	},
-	.resource	= tmu1_resources,
-	.num_resources	= ARRAY_SIZE(tmu1_resources),
-};
-
-static struct sh_timer_config tmu2_platform_data = {
-	.channel_offset = 0x1c,
-	.timer_bit = 2,
-};
-
-static struct resource tmu2_resources[] = {
-	[0] = {
-		.start	= 0xffd80020,
-		.end	= 0xffd8002f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x440),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu2_device = {
-	.name		= "sh_tmu",
-	.id		= 2,
-	.dev = {
-		.platform_data	= &tmu2_platform_data,
-	},
-	.resource	= tmu2_resources,
-	.num_resources	= ARRAY_SIZE(tmu2_resources),
-};
-
 
 static struct platform_device *sh7760_devices[] __initdata = {
 	&scif0_device,
@@ -316,8 +254,6 @@ static struct platform_device *sh7760_devices[] __initdata = {
 	&scif2_device,
 	&scif3_device,
 	&tmu0_device,
-	&tmu1_device,
-	&tmu2_device,
 };
 
 static int __init sh7760_devices_setup(void)
@@ -333,8 +269,6 @@ static struct platform_device *sh7760_early_devices[] __initdata = {
 	&scif2_device,
 	&scif3_device,
 	&tmu0_device,
-	&tmu1_device,
-	&tmu2_device,
 };
 
 void __init plat_early_device_setup(void)
diff --git a/arch/sh/kernel/cpu/sh4a/clock-sh7343.c b/arch/sh/kernel/cpu/sh4a/clock-sh7343.c
index 53638e231cd..9edc06c02dc 100644
--- a/arch/sh/kernel/cpu/sh4a/clock-sh7343.c
+++ b/arch/sh/kernel/cpu/sh4a/clock-sh7343.c
@@ -227,7 +227,7 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_CON_ID("hudi0", &mstp_clks[MSTP019]),
 	CLKDEV_CON_ID("ubc0", &mstp_clks[MSTP017]),
 	CLKDEV_CON_ID("tmu_fck", &mstp_clks[MSTP015]),
-	CLKDEV_CON_ID("cmt_fck", &mstp_clks[MSTP014]),
+	CLKDEV_ICK_ID("fck", "sh-cmt-32.0", &mstp_clks[MSTP014]),
 	CLKDEV_CON_ID("rwdt0", &mstp_clks[MSTP013]),
 	CLKDEV_CON_ID("mfi0", &mstp_clks[MSTP011]),
 	CLKDEV_CON_ID("flctl0", &mstp_clks[MSTP010]),
diff --git a/arch/sh/kernel/cpu/sh4a/clock-sh7366.c b/arch/sh/kernel/cpu/sh4a/clock-sh7366.c
index 22e485d1990..955b9add781 100644
--- a/arch/sh/kernel/cpu/sh4a/clock-sh7366.c
+++ b/arch/sh/kernel/cpu/sh4a/clock-sh7366.c
@@ -225,7 +225,7 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_CON_ID("hudi0", &mstp_clks[MSTP019]),
 	CLKDEV_CON_ID("ubc0", &mstp_clks[MSTP017]),
 	CLKDEV_CON_ID("tmu_fck", &mstp_clks[MSTP015]),
-	CLKDEV_CON_ID("cmt_fck", &mstp_clks[MSTP014]),
+	CLKDEV_ICK_ID("fck", "sh-cmt-32.0", &mstp_clks[MSTP014]),
 	CLKDEV_CON_ID("rwdt0", &mstp_clks[MSTP013]),
 	CLKDEV_CON_ID("mfi0", &mstp_clks[MSTP011]),
 	CLKDEV_CON_ID("flctl0", &mstp_clks[MSTP010]),
diff --git a/arch/sh/kernel/cpu/sh4a/clock-sh7722.c b/arch/sh/kernel/cpu/sh4a/clock-sh7722.c
index c4cb740e4d1..8f07a1a3869 100644
--- a/arch/sh/kernel/cpu/sh4a/clock-sh7722.c
+++ b/arch/sh/kernel/cpu/sh4a/clock-sh7722.c
@@ -203,11 +203,9 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_CON_ID("uram0", &mstp_clks[HWBLK_URAM]),
 	CLKDEV_CON_ID("xymem0", &mstp_clks[HWBLK_XYMEM]),
 
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.0", &mstp_clks[HWBLK_TMU]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.1", &mstp_clks[HWBLK_TMU]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.2", &mstp_clks[HWBLK_TMU]),
+	CLKDEV_ICK_ID("fck", "sh-tmu.0", &mstp_clks[HWBLK_TMU]),
 
-	CLKDEV_CON_ID("cmt_fck", &mstp_clks[HWBLK_CMT]),
+	CLKDEV_ICK_ID("fck", "sh-cmt-32.0", &mstp_clks[HWBLK_CMT]),
 	CLKDEV_DEV_ID("sh-wdt.0", &mstp_clks[HWBLK_RWDT]),
 	CLKDEV_CON_ID("flctl0", &mstp_clks[HWBLK_FLCTL]),
 
diff --git a/arch/sh/kernel/cpu/sh4a/clock-sh7723.c b/arch/sh/kernel/cpu/sh4a/clock-sh7723.c
index 37c41c7747a..ccbcab550df 100644
--- a/arch/sh/kernel/cpu/sh4a/clock-sh7723.c
+++ b/arch/sh/kernel/cpu/sh4a/clock-sh7723.c
@@ -236,7 +236,7 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_CON_ID("sh0", &mstp_clks[HWBLK_SHYWAY]),
 	CLKDEV_CON_ID("hudi0", &mstp_clks[HWBLK_HUDI]),
 	CLKDEV_CON_ID("ubc0", &mstp_clks[HWBLK_UBC]),
-	CLKDEV_CON_ID("cmt_fck", &mstp_clks[HWBLK_CMT]),
+	CLKDEV_ICK_ID("fck", "sh-cmt-32.0", &mstp_clks[HWBLK_CMT]),
 	CLKDEV_DEV_ID("sh-wdt.0", &mstp_clks[HWBLK_RWDT]),
 	CLKDEV_DEV_ID("sh-dma-engine.1", &mstp_clks[HWBLK_DMAC1]),
 	CLKDEV_CON_ID("flctl0", &mstp_clks[HWBLK_FLCTL]),
@@ -264,12 +264,8 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_CON_ID("veu0", &mstp_clks[HWBLK_VEU2H0]),
 	CLKDEV_CON_ID("vpu0", &mstp_clks[HWBLK_VPU]),
 
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.0", &mstp_clks[HWBLK_TMU0]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.1", &mstp_clks[HWBLK_TMU0]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.2", &mstp_clks[HWBLK_TMU0]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.3", &mstp_clks[HWBLK_TMU1]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.4", &mstp_clks[HWBLK_TMU1]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.5", &mstp_clks[HWBLK_TMU1]),
+	CLKDEV_ICK_ID("fck", "sh-tmu.0", &mstp_clks[HWBLK_TMU0]),
+	CLKDEV_ICK_ID("fck", "sh-tmu.1", &mstp_clks[HWBLK_TMU1]),
 
 	CLKDEV_ICK_ID("sci_fck", "sh-sci.0", &mstp_clks[HWBLK_SCIF0]),
 	CLKDEV_ICK_ID("sci_fck", "sh-sci.1", &mstp_clks[HWBLK_SCIF1]),
diff --git a/arch/sh/kernel/cpu/sh4a/clock-sh7724.c b/arch/sh/kernel/cpu/sh4a/clock-sh7724.c
index 0128af3399b..f579dd52819 100644
--- a/arch/sh/kernel/cpu/sh4a/clock-sh7724.c
+++ b/arch/sh/kernel/cpu/sh4a/clock-sh7724.c
@@ -304,17 +304,13 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_CON_ID("hudi0", &mstp_clks[HWBLK_HUDI]),
 	CLKDEV_CON_ID("ubc0", &mstp_clks[HWBLK_UBC]),
 
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.0", &mstp_clks[HWBLK_TMU0]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.1", &mstp_clks[HWBLK_TMU0]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.2", &mstp_clks[HWBLK_TMU0]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.3", &mstp_clks[HWBLK_TMU1]),
+	CLKDEV_ICK_ID("fck", "sh-tmu.0", &mstp_clks[HWBLK_TMU0]),
+	CLKDEV_ICK_ID("fck", "sh-tmu.1", &mstp_clks[HWBLK_TMU1]),
 
-	CLKDEV_CON_ID("cmt_fck", &mstp_clks[HWBLK_CMT]),
+	CLKDEV_ICK_ID("fck", "sh-cmt-16.0", &mstp_clks[HWBLK_CMT]),
 	CLKDEV_DEV_ID("sh-wdt.0", &mstp_clks[HWBLK_RWDT]),
 	CLKDEV_DEV_ID("sh-dma-engine.1", &mstp_clks[HWBLK_DMAC1]),
 
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.4", &mstp_clks[HWBLK_TMU1]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.5", &mstp_clks[HWBLK_TMU1]),
 	CLKDEV_DEV_ID("sh-sci.0", &mstp_clks[HWBLK_SCIF0]),
 	CLKDEV_DEV_ID("sh-sci.1", &mstp_clks[HWBLK_SCIF1]),
 	CLKDEV_DEV_ID("sh-sci.2", &mstp_clks[HWBLK_SCIF2]),
diff --git a/arch/sh/kernel/cpu/sh4a/clock-sh7734.c b/arch/sh/kernel/cpu/sh4a/clock-sh7734.c
index ed9501519ab..1fdf1ee672d 100644
--- a/arch/sh/kernel/cpu/sh4a/clock-sh7734.c
+++ b/arch/sh/kernel/cpu/sh4a/clock-sh7734.c
@@ -201,15 +201,9 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_ICK_ID("sci_fck", "sh-sci.4", &mstp_clks[MSTP022]),
 	CLKDEV_ICK_ID("sci_fck", "sh-sci.5", &mstp_clks[MSTP021]),
 	CLKDEV_CON_ID("hscif", &mstp_clks[MSTP019]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.0", &mstp_clks[MSTP016]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.1", &mstp_clks[MSTP016]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.2", &mstp_clks[MSTP016]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.3", &mstp_clks[MSTP015]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.4", &mstp_clks[MSTP015]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.5", &mstp_clks[MSTP015]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.6", &mstp_clks[MSTP014]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.7", &mstp_clks[MSTP014]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.8", &mstp_clks[MSTP014]),
+	CLKDEV_ICK_ID("fck", "sh-tmu.0", &mstp_clks[MSTP016]),
+	CLKDEV_ICK_ID("fck", "sh-tmu.1", &mstp_clks[MSTP015]),
+	CLKDEV_ICK_ID("fck", "sh-tmu.2", &mstp_clks[MSTP014]),
 	CLKDEV_CON_ID("ssi0", &mstp_clks[MSTP012]),
 	CLKDEV_CON_ID("ssi1", &mstp_clks[MSTP011]),
 	CLKDEV_CON_ID("ssi2", &mstp_clks[MSTP010]),
diff --git a/arch/sh/kernel/cpu/sh4a/clock-sh7757.c b/arch/sh/kernel/cpu/sh4a/clock-sh7757.c
index 5c0e3c33516..9a28fdb3638 100644
--- a/arch/sh/kernel/cpu/sh4a/clock-sh7757.c
+++ b/arch/sh/kernel/cpu/sh4a/clock-sh7757.c
@@ -123,8 +123,8 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_CON_ID("riic6", &mstp_clks[MSTP000]),
 	CLKDEV_CON_ID("riic7", &mstp_clks[MSTP000]),
 
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.0", &mstp_clks[MSTP113]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.1", &mstp_clks[MSTP114]),
+	CLKDEV_ICK_ID("fck", "sh-tmu.0", &mstp_clks[MSTP113]),
+	CLKDEV_ICK_ID("fck", "sh-tmu.1", &mstp_clks[MSTP114]),
 	CLKDEV_ICK_ID("sci_fck", "sh-sci.2", &mstp_clks[MSTP112]),
 	CLKDEV_ICK_ID("sci_fck", "sh-sci.1", &mstp_clks[MSTP111]),
 	CLKDEV_ICK_ID("sci_fck", "sh-sci.0", &mstp_clks[MSTP110]),
diff --git a/arch/sh/kernel/cpu/sh4a/clock-sh7785.c b/arch/sh/kernel/cpu/sh4a/clock-sh7785.c
index 1c83788db76..17d0ea55a5a 100644
--- a/arch/sh/kernel/cpu/sh4a/clock-sh7785.c
+++ b/arch/sh/kernel/cpu/sh4a/clock-sh7785.c
@@ -146,12 +146,8 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_CON_ID("mmcif_fck", &mstp_clks[MSTP013]),
 	CLKDEV_CON_ID("flctl_fck", &mstp_clks[MSTP012]),
 
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.0", &mstp_clks[MSTP008]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.1", &mstp_clks[MSTP008]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.2", &mstp_clks[MSTP008]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.3", &mstp_clks[MSTP009]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.4", &mstp_clks[MSTP009]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.5", &mstp_clks[MSTP009]),
+	CLKDEV_ICK_ID("fck", "sh-tmu.0", &mstp_clks[MSTP008]),
+	CLKDEV_ICK_ID("fck", "sh-tmu.1", &mstp_clks[MSTP009]),
 
 	CLKDEV_CON_ID("siof_fck", &mstp_clks[MSTP003]),
 	CLKDEV_CON_ID("hspi_fck", &mstp_clks[MSTP002]),
diff --git a/arch/sh/kernel/cpu/sh4a/clock-sh7786.c b/arch/sh/kernel/cpu/sh4a/clock-sh7786.c
index 8bba6f15902..bec2a83f1ba 100644
--- a/arch/sh/kernel/cpu/sh4a/clock-sh7786.c
+++ b/arch/sh/kernel/cpu/sh4a/clock-sh7786.c
@@ -155,18 +155,10 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_CON_ID("i2c1_fck", &mstp_clks[MSTP015]),
 	CLKDEV_CON_ID("i2c0_fck", &mstp_clks[MSTP014]),
 
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.0", &mstp_clks[MSTP008]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.1", &mstp_clks[MSTP008]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.2", &mstp_clks[MSTP008]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.3", &mstp_clks[MSTP009]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.4", &mstp_clks[MSTP009]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.5", &mstp_clks[MSTP009]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.6", &mstp_clks[MSTP010]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.7", &mstp_clks[MSTP010]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.8", &mstp_clks[MSTP010]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.9", &mstp_clks[MSTP011]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.10", &mstp_clks[MSTP011]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.11", &mstp_clks[MSTP011]),
+	CLKDEV_ICK_ID("fck", "sh-tmu.0", &mstp_clks[MSTP008]),
+	CLKDEV_ICK_ID("fck", "sh-tmu.1", &mstp_clks[MSTP009]),
+	CLKDEV_ICK_ID("fck", "sh-tmu.2", &mstp_clks[MSTP010]),
+	CLKDEV_ICK_ID("fck", "sh-tmu.3", &mstp_clks[MSTP011]),
 
 	CLKDEV_CON_ID("sdif1_fck", &mstp_clks[MSTP005]),
 	CLKDEV_CON_ID("sdif0_fck", &mstp_clks[MSTP004]),
diff --git a/arch/sh/kernel/cpu/sh4a/clock-shx3.c b/arch/sh/kernel/cpu/sh4a/clock-shx3.c
index a9422dab0ce..9a49a44f6f9 100644
--- a/arch/sh/kernel/cpu/sh4a/clock-shx3.c
+++ b/arch/sh/kernel/cpu/sh4a/clock-shx3.c
@@ -124,12 +124,8 @@ static struct clk_lookup lookups[] = {
 	CLKDEV_CON_ID("fe1_fck", &mstp_clks[MSTP001]),
 	CLKDEV_CON_ID("fe0_fck", &mstp_clks[MSTP000]),
 
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.0", &mstp_clks[MSTP008]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.1", &mstp_clks[MSTP008]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.2", &mstp_clks[MSTP008]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.3", &mstp_clks[MSTP009]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.4", &mstp_clks[MSTP009]),
-	CLKDEV_ICK_ID("tmu_fck", "sh_tmu.5", &mstp_clks[MSTP009]),
+	CLKDEV_ICK_ID("fck", "sh-tmu.0", &mstp_clks[MSTP008]),
+	CLKDEV_ICK_ID("fck", "sh-tmu.1", &mstp_clks[MSTP009]),
 
 	CLKDEV_CON_ID("hudi_fck", &mstp_clks[MSTP119]),
 	CLKDEV_CON_ID("dmac_11_6_fck", &mstp_clks[MSTP105]),
diff --git a/arch/sh/kernel/cpu/sh4a/setup-sh7343.c b/arch/sh/kernel/cpu/sh4a/setup-sh7343.c
index 245d1925448..ceb3dedad98 100644
--- a/arch/sh/kernel/cpu/sh4a/setup-sh7343.c
+++ b/arch/sh/kernel/cpu/sh4a/setup-sh7343.c
@@ -228,26 +228,16 @@ static struct platform_device jpu_device = {
 };
 
 static struct sh_timer_config cmt_platform_data = {
-	.channel_offset = 0x60,
-	.timer_bit = 5,
-	.clockevent_rating = 125,
-	.clocksource_rating = 200,
+	.channels_mask = 0x20,
 };
 
 static struct resource cmt_resources[] = {
-	[0] = {
-		.start	= 0x044a0060,
-		.end	= 0x044a006b,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0xf00),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0x044a0000, 0x70),
+	DEFINE_RES_IRQ(evt2irq(0xf00)),
 };
 
 static struct platform_device cmt_device = {
-	.name		= "sh_cmt",
+	.name		= "sh-cmt-32",
 	.id		= 0,
 	.dev = {
 		.platform_data	= &cmt_platform_data,
@@ -257,25 +247,18 @@ static struct platform_device cmt_device = {
 };
 
 static struct sh_timer_config tmu0_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
+	.channels_mask = 7,
 };
 
 static struct resource tmu0_resources[] = {
-	[0] = {
-		.start	= 0xffd80008,
-		.end	= 0xffd80013,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x400),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xffd80000, 0x2c),
+	DEFINE_RES_IRQ(evt2irq(0x400)),
+	DEFINE_RES_IRQ(evt2irq(0x420)),
+	DEFINE_RES_IRQ(evt2irq(0x440)),
 };
 
 static struct platform_device tmu0_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu",
 	.id		= 0,
 	.dev = {
 		.platform_data	= &tmu0_platform_data,
@@ -284,61 +267,6 @@ static struct platform_device tmu0_device = {
 	.num_resources	= ARRAY_SIZE(tmu0_resources),
 };
 
-static struct sh_timer_config tmu1_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-	.clocksource_rating = 200,
-};
-
-static struct resource tmu1_resources[] = {
-	[0] = {
-		.start	= 0xffd80014,
-		.end	= 0xffd8001f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x420),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu1_device = {
-	.name		= "sh_tmu",
-	.id		= 1,
-	.dev = {
-		.platform_data	= &tmu1_platform_data,
-	},
-	.resource	= tmu1_resources,
-	.num_resources	= ARRAY_SIZE(tmu1_resources),
-};
-
-static struct sh_timer_config tmu2_platform_data = {
-	.channel_offset = 0x1c,
-	.timer_bit = 2,
-};
-
-static struct resource tmu2_resources[] = {
-	[0] = {
-		.start	= 0xffd80020,
-		.end	= 0xffd8002b,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x440),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu2_device = {
-	.name		= "sh_tmu",
-	.id		= 2,
-	.dev = {
-		.platform_data	= &tmu2_platform_data,
-	},
-	.resource	= tmu2_resources,
-	.num_resources	= ARRAY_SIZE(tmu2_resources),
-};
-
 static struct platform_device *sh7343_devices[] __initdata = {
 	&scif0_device,
 	&scif1_device,
@@ -346,8 +274,6 @@ static struct platform_device *sh7343_devices[] __initdata = {
 	&scif3_device,
 	&cmt_device,
 	&tmu0_device,
-	&tmu1_device,
-	&tmu2_device,
 	&iic0_device,
 	&iic1_device,
 	&vpu_device,
@@ -373,8 +299,6 @@ static struct platform_device *sh7343_early_devices[] __initdata = {
 	&scif3_device,
 	&cmt_device,
 	&tmu0_device,
-	&tmu1_device,
-	&tmu2_device,
 };
 
 void __init plat_early_device_setup(void)
diff --git a/arch/sh/kernel/cpu/sh4a/setup-sh7366.c b/arch/sh/kernel/cpu/sh4a/setup-sh7366.c
index 6f56cbd76b2..f75f6734313 100644
--- a/arch/sh/kernel/cpu/sh4a/setup-sh7366.c
+++ b/arch/sh/kernel/cpu/sh4a/setup-sh7366.c
@@ -176,26 +176,16 @@ static struct platform_device veu1_device = {
 };
 
 static struct sh_timer_config cmt_platform_data = {
-	.channel_offset = 0x60,
-	.timer_bit = 5,
-	.clockevent_rating = 125,
-	.clocksource_rating = 200,
+	.channels_mask = 0x20,
 };
 
 static struct resource cmt_resources[] = {
-	[0] = {
-		.start	= 0x044a0060,
-		.end	= 0x044a006b,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0xf00),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0x044a0000, 0x70),
+	DEFINE_RES_IRQ(evt2irq(0xf00)),
 };
 
 static struct platform_device cmt_device = {
-	.name		= "sh_cmt",
+	.name		= "sh-cmt-32",
 	.id		= 0,
 	.dev = {
 		.platform_data	= &cmt_platform_data,
@@ -205,25 +195,18 @@ static struct platform_device cmt_device = {
 };
 
 static struct sh_timer_config tmu0_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
+	.channels_mask = 7,
 };
 
 static struct resource tmu0_resources[] = {
-	[0] = {
-		.start	= 0xffd80008,
-		.end	= 0xffd80013,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= 16,
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xffd80000, 0x2c),
+	DEFINE_RES_IRQ(evt2irq(0x400)),
+	DEFINE_RES_IRQ(evt2irq(0x420)),
+	DEFINE_RES_IRQ(evt2irq(0x440)),
 };
 
 static struct platform_device tmu0_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu",
 	.id		= 0,
 	.dev = {
 		.platform_data	= &tmu0_platform_data,
@@ -232,67 +215,10 @@ static struct platform_device tmu0_device = {
 	.num_resources	= ARRAY_SIZE(tmu0_resources),
 };
 
-static struct sh_timer_config tmu1_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-	.clocksource_rating = 200,
-};
-
-static struct resource tmu1_resources[] = {
-	[0] = {
-		.start	= 0xffd80014,
-		.end	= 0xffd8001f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x420),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu1_device = {
-	.name		= "sh_tmu",
-	.id		= 1,
-	.dev = {
-		.platform_data	= &tmu1_platform_data,
-	},
-	.resource	= tmu1_resources,
-	.num_resources	= ARRAY_SIZE(tmu1_resources),
-};
-
-static struct sh_timer_config tmu2_platform_data = {
-	.channel_offset = 0x1c,
-	.timer_bit = 2,
-};
-
-static struct resource tmu2_resources[] = {
-	[0] = {
-		.start	= 0xffd80020,
-		.end	= 0xffd8002b,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x440),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu2_device = {
-	.name		= "sh_tmu",
-	.id		= 2,
-	.dev = {
-		.platform_data	= &tmu2_platform_data,
-	},
-	.resource	= tmu2_resources,
-	.num_resources	= ARRAY_SIZE(tmu2_resources),
-};
-
 static struct platform_device *sh7366_devices[] __initdata = {
 	&scif0_device,
 	&cmt_device,
 	&tmu0_device,
-	&tmu1_device,
-	&tmu2_device,
 	&iic_device,
 	&usb_host_device,
 	&vpu_device,
@@ -315,8 +241,6 @@ static struct platform_device *sh7366_early_devices[] __initdata = {
 	&scif0_device,
 	&cmt_device,
 	&tmu0_device,
-	&tmu1_device,
-	&tmu2_device,
 };
 
 void __init plat_early_device_setup(void)
diff --git a/arch/sh/kernel/cpu/sh4a/setup-sh7722.c b/arch/sh/kernel/cpu/sh4a/setup-sh7722.c
index 5a94efc8d4c..57f83a92a50 100644
--- a/arch/sh/kernel/cpu/sh4a/setup-sh7722.c
+++ b/arch/sh/kernel/cpu/sh4a/setup-sh7722.c
@@ -413,26 +413,16 @@ static struct platform_device jpu_device = {
 };
 
 static struct sh_timer_config cmt_platform_data = {
-	.channel_offset = 0x60,
-	.timer_bit = 5,
-	.clockevent_rating = 125,
-	.clocksource_rating = 125,
+	.channels_mask = 0x20,
 };
 
 static struct resource cmt_resources[] = {
-	[0] = {
-		.start	= 0x044a0060,
-		.end	= 0x044a006b,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0xf00),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0x044a0000, 0x70),
+	DEFINE_RES_IRQ(evt2irq(0xf00)),
 };
 
 static struct platform_device cmt_device = {
-	.name		= "sh_cmt",
+	.name		= "sh-cmt-32",
 	.id		= 0,
 	.dev = {
 		.platform_data	= &cmt_platform_data,
@@ -442,25 +432,18 @@ static struct platform_device cmt_device = {
 };
 
 static struct sh_timer_config tmu0_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
+	.channels_mask = 7,
 };
 
 static struct resource tmu0_resources[] = {
-	[0] = {
-		.start	= 0xffd80008,
-		.end	= 0xffd80013,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x400),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xffd80000, 0x2c),
+	DEFINE_RES_IRQ(evt2irq(0x400)),
+	DEFINE_RES_IRQ(evt2irq(0x420)),
+	DEFINE_RES_IRQ(evt2irq(0x440)),
 };
 
 static struct platform_device tmu0_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu",
 	.id		= 0,
 	.dev = {
 		.platform_data	= &tmu0_platform_data,
@@ -469,61 +452,6 @@ static struct platform_device tmu0_device = {
 	.num_resources	= ARRAY_SIZE(tmu0_resources),
 };
 
-static struct sh_timer_config tmu1_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-	.clocksource_rating = 200,
-};
-
-static struct resource tmu1_resources[] = {
-	[0] = {
-		.start	= 0xffd80014,
-		.end	= 0xffd8001f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x420),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu1_device = {
-	.name		= "sh_tmu",
-	.id		= 1,
-	.dev = {
-		.platform_data	= &tmu1_platform_data,
-	},
-	.resource	= tmu1_resources,
-	.num_resources	= ARRAY_SIZE(tmu1_resources),
-};
-
-static struct sh_timer_config tmu2_platform_data = {
-	.channel_offset = 0x1c,
-	.timer_bit = 2,
-};
-
-static struct resource tmu2_resources[] = {
-	[0] = {
-		.start	= 0xffd80020,
-		.end	= 0xffd8002b,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= 18,
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu2_device = {
-	.name		= "sh_tmu",
-	.id		= 2,
-	.dev = {
-		.platform_data	= &tmu2_platform_data,
-	},
-	.resource	= tmu2_resources,
-	.num_resources	= ARRAY_SIZE(tmu2_resources),
-};
-
 static struct siu_platform siu_platform_data = {
 	.dma_slave_tx_a	= SHDMA_SLAVE_SIUA_TX,
 	.dma_slave_rx_a	= SHDMA_SLAVE_SIUA_RX,
@@ -559,8 +487,6 @@ static struct platform_device *sh7722_devices[] __initdata = {
 	&scif2_device,
 	&cmt_device,
 	&tmu0_device,
-	&tmu1_device,
-	&tmu2_device,
 	&rtc_device,
 	&usbf_device,
 	&iic_device,
@@ -588,8 +514,6 @@ static struct platform_device *sh7722_early_devices[] __initdata = {
 	&scif2_device,
 	&cmt_device,
 	&tmu0_device,
-	&tmu1_device,
-	&tmu2_device,
 };
 
 void __init plat_early_device_setup(void)
diff --git a/arch/sh/kernel/cpu/sh4a/setup-sh7723.c b/arch/sh/kernel/cpu/sh4a/setup-sh7723.c
index 3c5eb0993a7..3533b56dd46 100644
--- a/arch/sh/kernel/cpu/sh4a/setup-sh7723.c
+++ b/arch/sh/kernel/cpu/sh4a/setup-sh7723.c
@@ -245,26 +245,16 @@ static struct platform_device veu1_device = {
 };
 
 static struct sh_timer_config cmt_platform_data = {
-	.channel_offset = 0x60,
-	.timer_bit = 5,
-	.clockevent_rating = 125,
-	.clocksource_rating = 125,
+	.channels_mask = 0x20,
 };
 
 static struct resource cmt_resources[] = {
-	[0] = {
-		.start	= 0x044a0060,
-		.end	= 0x044a006b,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0xf00),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0x044a0000, 0x70),
+	DEFINE_RES_IRQ(evt2irq(0xf00)),
 };
 
 static struct platform_device cmt_device = {
-	.name		= "sh_cmt",
+	.name		= "sh-cmt-32",
 	.id		= 0,
 	.dev = {
 		.platform_data	= &cmt_platform_data,
@@ -274,25 +264,18 @@ static struct platform_device cmt_device = {
 };
 
 static struct sh_timer_config tmu0_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
+	.channels_mask = 7,
 };
 
 static struct resource tmu0_resources[] = {
-	[0] = {
-		.start	= 0xffd80008,
-		.end	= 0xffd80013,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x400),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xffd80000, 0x2c),
+	DEFINE_RES_IRQ(evt2irq(0x400)),
+	DEFINE_RES_IRQ(evt2irq(0x420)),
+	DEFINE_RES_IRQ(evt2irq(0x440)),
 };
 
 static struct platform_device tmu0_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu",
 	.id		= 0,
 	.dev = {
 		.platform_data	= &tmu0_platform_data,
@@ -302,25 +285,18 @@ static struct platform_device tmu0_device = {
 };
 
 static struct sh_timer_config tmu1_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-	.clocksource_rating = 200,
+	.channels_mask = 7,
 };
 
 static struct resource tmu1_resources[] = {
-	[0] = {
-		.start	= 0xffd80014,
-		.end	= 0xffd8001f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x420),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xffd90000, 0x2c),
+	DEFINE_RES_IRQ(evt2irq(0x920)),
+	DEFINE_RES_IRQ(evt2irq(0x940)),
+	DEFINE_RES_IRQ(evt2irq(0x960)),
 };
 
 static struct platform_device tmu1_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu",
 	.id		= 1,
 	.dev = {
 		.platform_data	= &tmu1_platform_data,
@@ -329,114 +305,6 @@ static struct platform_device tmu1_device = {
 	.num_resources	= ARRAY_SIZE(tmu1_resources),
 };
 
-static struct sh_timer_config tmu2_platform_data = {
-	.channel_offset = 0x1c,
-	.timer_bit = 2,
-};
-
-static struct resource tmu2_resources[] = {
-	[0] = {
-		.start	= 0xffd80020,
-		.end	= 0xffd8002b,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x440),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu2_device = {
-	.name		= "sh_tmu",
-	.id		= 2,
-	.dev = {
-		.platform_data	= &tmu2_platform_data,
-	},
-	.resource	= tmu2_resources,
-	.num_resources	= ARRAY_SIZE(tmu2_resources),
-};
-
-static struct sh_timer_config tmu3_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
-};
-
-static struct resource tmu3_resources[] = {
-	[0] = {
-		.start	= 0xffd90008,
-		.end	= 0xffd90013,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x920),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu3_device = {
-	.name		= "sh_tmu",
-	.id		= 3,
-	.dev = {
-		.platform_data	= &tmu3_platform_data,
-	},
-	.resource	= tmu3_resources,
-	.num_resources	= ARRAY_SIZE(tmu3_resources),
-};
-
-static struct sh_timer_config tmu4_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-};
-
-static struct resource tmu4_resources[] = {
-	[0] = {
-		.start	= 0xffd90014,
-		.end	= 0xffd9001f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x940),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu4_device = {
-	.name		= "sh_tmu",
-	.id		= 4,
-	.dev = {
-		.platform_data	= &tmu4_platform_data,
-	},
-	.resource	= tmu4_resources,
-	.num_resources	= ARRAY_SIZE(tmu4_resources),
-};
-
-static struct sh_timer_config tmu5_platform_data = {
-	.channel_offset = 0x1c,
-	.timer_bit = 2,
-};
-
-static struct resource tmu5_resources[] = {
-	[0] = {
-		.start	= 0xffd90020,
-		.end	= 0xffd9002b,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x920),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu5_device = {
-	.name		= "sh_tmu",
-	.id		= 5,
-	.dev = {
-		.platform_data	= &tmu5_platform_data,
-	},
-	.resource	= tmu5_resources,
-	.num_resources	= ARRAY_SIZE(tmu5_resources),
-};
-
 static struct resource rtc_resources[] = {
 	[0] = {
 		.start	= 0xa465fec0,
@@ -527,10 +395,6 @@ static struct platform_device *sh7723_devices[] __initdata = {
 	&cmt_device,
 	&tmu0_device,
 	&tmu1_device,
-	&tmu2_device,
-	&tmu3_device,
-	&tmu4_device,
-	&tmu5_device,
 	&rtc_device,
 	&iic_device,
 	&sh7723_usb_host_device,
@@ -560,10 +424,6 @@ static struct platform_device *sh7723_early_devices[] __initdata = {
 	&cmt_device,
 	&tmu0_device,
 	&tmu1_device,
-	&tmu2_device,
-	&tmu3_device,
-	&tmu4_device,
-	&tmu5_device,
 };
 
 void __init plat_early_device_setup(void)
diff --git a/arch/sh/kernel/cpu/sh4a/setup-sh7724.c b/arch/sh/kernel/cpu/sh4a/setup-sh7724.c
index 60ebbc6842f..b9e84b1d3aa 100644
--- a/arch/sh/kernel/cpu/sh4a/setup-sh7724.c
+++ b/arch/sh/kernel/cpu/sh4a/setup-sh7724.c
@@ -648,26 +648,16 @@ static struct platform_device beu1_device = {
 };
 
 static struct sh_timer_config cmt_platform_data = {
-	.channel_offset = 0x60,
-	.timer_bit = 5,
-	.clockevent_rating = 125,
-	.clocksource_rating = 200,
+	.channels_mask = 0x20,
 };
 
 static struct resource cmt_resources[] = {
-	[0] = {
-		.start	= 0x044a0060,
-		.end	= 0x044a006b,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0xf00),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0x044a0000, 0x70),
+	DEFINE_RES_IRQ(evt2irq(0xf00)),
 };
 
 static struct platform_device cmt_device = {
-	.name		= "sh_cmt",
+	.name		= "sh-cmt-32",
 	.id		= 0,
 	.dev = {
 		.platform_data	= &cmt_platform_data,
@@ -677,25 +667,18 @@ static struct platform_device cmt_device = {
 };
 
 static struct sh_timer_config tmu0_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
+	.channels_mask = 7,
 };
 
 static struct resource tmu0_resources[] = {
-	[0] = {
-		.start	= 0xffd80008,
-		.end	= 0xffd80013,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x400),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xffd80000, 0x2c),
+	DEFINE_RES_IRQ(evt2irq(0x400)),
+	DEFINE_RES_IRQ(evt2irq(0x420)),
+	DEFINE_RES_IRQ(evt2irq(0x440)),
 };
 
 static struct platform_device tmu0_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu",
 	.id		= 0,
 	.dev = {
 		.platform_data	= &tmu0_platform_data,
@@ -705,25 +688,18 @@ static struct platform_device tmu0_device = {
 };
 
 static struct sh_timer_config tmu1_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-	.clocksource_rating = 200,
+	.channels_mask = 7,
 };
 
 static struct resource tmu1_resources[] = {
-	[0] = {
-		.start	= 0xffd80014,
-		.end	= 0xffd8001f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x420),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xffd90000, 0x2c),
+	DEFINE_RES_IRQ(evt2irq(0x920)),
+	DEFINE_RES_IRQ(evt2irq(0x940)),
+	DEFINE_RES_IRQ(evt2irq(0x960)),
 };
 
 static struct platform_device tmu1_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu",
 	.id		= 1,
 	.dev = {
 		.platform_data	= &tmu1_platform_data,
@@ -732,115 +708,6 @@ static struct platform_device tmu1_device = {
 	.num_resources	= ARRAY_SIZE(tmu1_resources),
 };
 
-static struct sh_timer_config tmu2_platform_data = {
-	.channel_offset = 0x1c,
-	.timer_bit = 2,
-};
-
-static struct resource tmu2_resources[] = {
-	[0] = {
-		.start	= 0xffd80020,
-		.end	= 0xffd8002b,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x440),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu2_device = {
-	.name		= "sh_tmu",
-	.id		= 2,
-	.dev = {
-		.platform_data	= &tmu2_platform_data,
-	},
-	.resource	= tmu2_resources,
-	.num_resources	= ARRAY_SIZE(tmu2_resources),
-};
-
-
-static struct sh_timer_config tmu3_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
-};
-
-static struct resource tmu3_resources[] = {
-	[0] = {
-		.start	= 0xffd90008,
-		.end	= 0xffd90013,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x920),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu3_device = {
-	.name		= "sh_tmu",
-	.id		= 3,
-	.dev = {
-		.platform_data	= &tmu3_platform_data,
-	},
-	.resource	= tmu3_resources,
-	.num_resources	= ARRAY_SIZE(tmu3_resources),
-};
-
-static struct sh_timer_config tmu4_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-};
-
-static struct resource tmu4_resources[] = {
-	[0] = {
-		.start	= 0xffd90014,
-		.end	= 0xffd9001f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x940),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu4_device = {
-	.name		= "sh_tmu",
-	.id		= 4,
-	.dev = {
-		.platform_data	= &tmu4_platform_data,
-	},
-	.resource	= tmu4_resources,
-	.num_resources	= ARRAY_SIZE(tmu4_resources),
-};
-
-static struct sh_timer_config tmu5_platform_data = {
-	.channel_offset = 0x1c,
-	.timer_bit = 2,
-};
-
-static struct resource tmu5_resources[] = {
-	[0] = {
-		.start	= 0xffd90020,
-		.end	= 0xffd9002b,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x920),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu5_device = {
-	.name		= "sh_tmu",
-	.id		= 5,
-	.dev = {
-		.platform_data	= &tmu5_platform_data,
-	},
-	.resource	= tmu5_resources,
-	.num_resources	= ARRAY_SIZE(tmu5_resources),
-};
-
 /* JPU */
 static struct uio_info jpu_platform_data = {
 	.name = "JPU",
@@ -938,10 +805,6 @@ static struct platform_device *sh7724_devices[] __initdata = {
 	&cmt_device,
 	&tmu0_device,
 	&tmu1_device,
-	&tmu2_device,
-	&tmu3_device,
-	&tmu4_device,
-	&tmu5_device,
 	&dma0_device,
 	&dma1_device,
 	&rtc_device,
@@ -981,10 +844,6 @@ static struct platform_device *sh7724_early_devices[] __initdata = {
 	&cmt_device,
 	&tmu0_device,
 	&tmu1_device,
-	&tmu2_device,
-	&tmu3_device,
-	&tmu4_device,
-	&tmu5_device,
 };
 
 void __init plat_early_device_setup(void)
diff --git a/arch/sh/kernel/cpu/sh4a/setup-sh7734.c b/arch/sh/kernel/cpu/sh4a/setup-sh7734.c
index dad4ed1b2f9..f617bcb734d 100644
--- a/arch/sh/kernel/cpu/sh4a/setup-sh7734.c
+++ b/arch/sh/kernel/cpu/sh4a/setup-sh7734.c
@@ -200,25 +200,18 @@ static struct platform_device i2c0_device = {
 
 /* TMU */
 static struct sh_timer_config tmu0_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
+	.channels_mask = 7,
 };
 
 static struct resource tmu0_resources[] = {
-	[0] = {
-		.start	= 0xFFD80008,
-		.end	= 0xFFD80014 - 1,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x400),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xffd80000, 0x30),
+	DEFINE_RES_IRQ(evt2irq(0x400)),
+	DEFINE_RES_IRQ(evt2irq(0x420)),
+	DEFINE_RES_IRQ(evt2irq(0x440)),
 };
 
 static struct platform_device tmu0_device = {
-	.name	= "sh_tmu",
+	.name		= "sh-tmu",
 	.id		= 0,
 	.dev = {
 		.platform_data	= &tmu0_platform_data,
@@ -228,26 +221,19 @@ static struct platform_device tmu0_device = {
 };
 
 static struct sh_timer_config tmu1_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-	.clocksource_rating = 200,
+	.channels_mask = 7,
 };
 
 static struct resource tmu1_resources[] = {
-	[0] = {
-		.start	= 0xFFD80014,
-		.end	= 0xFFD80020 - 1,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x420),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xffd81000, 0x30),
+	DEFINE_RES_IRQ(evt2irq(0x480)),
+	DEFINE_RES_IRQ(evt2irq(0x4a0)),
+	DEFINE_RES_IRQ(evt2irq(0x4c0)),
 };
 
 static struct platform_device tmu1_device = {
-	.name		= "sh_tmu",
-	.id			= 1,
+	.name		= "sh-tmu",
+	.id		= 1,
 	.dev = {
 		.platform_data	= &tmu1_platform_data,
 	},
@@ -256,25 +242,19 @@ static struct platform_device tmu1_device = {
 };
 
 static struct sh_timer_config tmu2_platform_data = {
-	.channel_offset = 0x1c,
-	.timer_bit = 2,
+	.channels_mask = 7,
 };
 
 static struct resource tmu2_resources[] = {
-	[0] = {
-		.start	= 0xFFD80020,
-		.end	= 0xFFD80030 - 1,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x440),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xffd82000, 0x30),
+	DEFINE_RES_IRQ(evt2irq(0x500)),
+	DEFINE_RES_IRQ(evt2irq(0x520)),
+	DEFINE_RES_IRQ(evt2irq(0x540)),
 };
 
 static struct platform_device tmu2_device = {
-	.name		= "sh_tmu",
-	.id			= 2,
+	.name		= "sh-tmu",
+	.id		= 2,
 	.dev = {
 		.platform_data	= &tmu2_platform_data,
 	},
@@ -282,169 +262,6 @@ static struct platform_device tmu2_device = {
 	.num_resources	= ARRAY_SIZE(tmu2_resources),
 };
 
-
-static struct sh_timer_config tmu3_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
-};
-
-static struct resource tmu3_resources[] = {
-	[0] = {
-		.start	= 0xFFD81008,
-		.end	= 0xFFD81014 - 1,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x480),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu3_device = {
-	.name		= "sh_tmu",
-	.id			= 3,
-	.dev = {
-		.platform_data	= &tmu3_platform_data,
-	},
-	.resource	= tmu3_resources,
-	.num_resources	= ARRAY_SIZE(tmu3_resources),
-};
-
-static struct sh_timer_config tmu4_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-};
-
-static struct resource tmu4_resources[] = {
-	[0] = {
-		.start	= 0xFFD81014,
-		.end	= 0xFFD81020 - 1,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x4A0),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu4_device = {
-	.name		= "sh_tmu",
-	.id			= 4,
-	.dev = {
-		.platform_data	= &tmu4_platform_data,
-	},
-	.resource	= tmu4_resources,
-	.num_resources	= ARRAY_SIZE(tmu4_resources),
-};
-
-static struct sh_timer_config tmu5_platform_data = {
-	.channel_offset = 0x1c,
-	.timer_bit = 2,
-};
-
-static struct resource tmu5_resources[] = {
-	[0] = {
-		.start	= 0xFFD81020,
-		.end	= 0xFFD81030 - 1,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x4C0),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu5_device = {
-	.name		= "sh_tmu",
-	.id			= 5,
-	.dev = {
-		.platform_data	= &tmu5_platform_data,
-	},
-	.resource	= tmu5_resources,
-	.num_resources	= ARRAY_SIZE(tmu5_resources),
-};
-
-static struct sh_timer_config tmu6_platform_data = {
-	.channel_offset = 0x4,
-	.timer_bit = 0,
-};
-
-static struct resource tmu6_resources[] = {
-	[0] = {
-		.start	= 0xFFD82008,
-		.end	= 0xFFD82014 - 1,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x500),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu6_device = {
-	.name		= "sh_tmu",
-	.id			= 6,
-	.dev = {
-		.platform_data	= &tmu6_platform_data,
-	},
-	.resource	= tmu6_resources,
-	.num_resources	= ARRAY_SIZE(tmu6_resources),
-};
-
-static struct sh_timer_config tmu7_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-};
-
-static struct resource tmu7_resources[] = {
-	[0] = {
-		.start	= 0xFFD82014,
-		.end	= 0xFFD82020 - 1,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x520),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu7_device = {
-	.name		= "sh_tmu",
-	.id			= 7,
-	.dev = {
-		.platform_data	= &tmu7_platform_data,
-	},
-	.resource	= tmu7_resources,
-	.num_resources	= ARRAY_SIZE(tmu7_resources),
-};
-
-static struct sh_timer_config tmu8_platform_data = {
-	.channel_offset = 0x1c,
-	.timer_bit = 2,
-};
-
-static struct resource tmu8_resources[] = {
-	[0] = {
-		.start	= 0xFFD82020,
-		.end	= 0xFFD82030 - 1,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x540),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu8_device = {
-	.name		= "sh_tmu",
-	.id			= 8,
-	.dev = {
-		.platform_data	= &tmu8_platform_data,
-	},
-	.resource	= tmu8_resources,
-	.num_resources	= ARRAY_SIZE(tmu8_resources),
-};
-
 static struct platform_device *sh7734_devices[] __initdata = {
 	&scif0_device,
 	&scif1_device,
@@ -455,12 +272,6 @@ static struct platform_device *sh7734_devices[] __initdata = {
 	&tmu0_device,
 	&tmu1_device,
 	&tmu2_device,
-	&tmu3_device,
-	&tmu4_device,
-	&tmu5_device,
-	&tmu6_device,
-	&tmu7_device,
-	&tmu8_device,
 	&rtc_device,
 };
 
@@ -474,12 +285,6 @@ static struct platform_device *sh7734_early_devices[] __initdata = {
 	&tmu0_device,
 	&tmu1_device,
 	&tmu2_device,
-	&tmu3_device,
-	&tmu4_device,
-	&tmu5_device,
-	&tmu6_device,
-	&tmu7_device,
-	&tmu8_device,
 };
 
 void __init plat_early_device_setup(void)
diff --git a/arch/sh/kernel/cpu/sh4a/setup-sh7757.c b/arch/sh/kernel/cpu/sh4a/setup-sh7757.c
index e43e5db5391..7b24ec4b409 100644
--- a/arch/sh/kernel/cpu/sh4a/setup-sh7757.c
+++ b/arch/sh/kernel/cpu/sh4a/setup-sh7757.c
@@ -87,25 +87,17 @@ static struct platform_device scif4_device = {
 };
 
 static struct sh_timer_config tmu0_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
+	.channels_mask = 3,
 };
 
 static struct resource tmu0_resources[] = {
-	[0] = {
-		.start	= 0xfe430008,
-		.end	= 0xfe430013,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x580),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xfe430000, 0x20),
+	DEFINE_RES_IRQ(evt2irq(0x580)),
+	DEFINE_RES_IRQ(evt2irq(0x5a0)),
 };
 
 static struct platform_device tmu0_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu",
 	.id		= 0,
 	.dev = {
 		.platform_data	= &tmu0_platform_data,
@@ -114,34 +106,6 @@ static struct platform_device tmu0_device = {
 	.num_resources	= ARRAY_SIZE(tmu0_resources),
 };
 
-static struct sh_timer_config tmu1_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-	.clocksource_rating = 200,
-};
-
-static struct resource tmu1_resources[] = {
-	[0] = {
-		.start	= 0xfe430014,
-		.end	= 0xfe43001f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x5a0),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu1_device = {
-	.name		= "sh_tmu",
-	.id		= 1,
-	.dev = {
-		.platform_data	= &tmu1_platform_data,
-	},
-	.resource	= tmu1_resources,
-	.num_resources	= ARRAY_SIZE(tmu1_resources),
-};
-
 static struct resource spi0_resources[] = {
 	[0] = {
 		.start	= 0xfe002000,
@@ -782,7 +746,6 @@ static struct platform_device *sh7757_devices[] __initdata = {
 	&scif3_device,
 	&scif4_device,
 	&tmu0_device,
-	&tmu1_device,
 	&dma0_device,
 	&dma1_device,
 	&dma2_device,
@@ -806,7 +769,6 @@ static struct platform_device *sh7757_early_devices[] __initdata = {
 	&scif3_device,
 	&scif4_device,
 	&tmu0_device,
-	&tmu1_device,
 };
 
 void __init plat_early_device_setup(void)
diff --git a/arch/sh/kernel/cpu/sh4a/setup-sh7763.c b/arch/sh/kernel/cpu/sh4a/setup-sh7763.c
index 5eebbd7f4c2..5a47d670dde 100644
--- a/arch/sh/kernel/cpu/sh4a/setup-sh7763.c
+++ b/arch/sh/kernel/cpu/sh4a/setup-sh7763.c
@@ -158,25 +158,18 @@ static struct platform_device usbf_device = {
 };
 
 static struct sh_timer_config tmu0_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
+	.channels_mask = 7,
 };
 
 static struct resource tmu0_resources[] = {
-	[0] = {
-		.start	= 0xffd80008,
-		.end	= 0xffd80013,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x580),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xffd80000, 0x30),
+	DEFINE_RES_IRQ(evt2irq(0x580)),
+	DEFINE_RES_IRQ(evt2irq(0x5a0)),
+	DEFINE_RES_IRQ(evt2irq(0x5c0)),
 };
 
 static struct platform_device tmu0_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu",
 	.id		= 0,
 	.dev = {
 		.platform_data	= &tmu0_platform_data,
@@ -186,25 +179,18 @@ static struct platform_device tmu0_device = {
 };
 
 static struct sh_timer_config tmu1_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-	.clocksource_rating = 200,
+	.channels_mask = 7,
 };
 
 static struct resource tmu1_resources[] = {
-	[0] = {
-		.start	= 0xffd80014,
-		.end	= 0xffd8001f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x5a0),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xffd88000, 0x2c),
+	DEFINE_RES_IRQ(evt2irq(0xe00)),
+	DEFINE_RES_IRQ(evt2irq(0xe20)),
+	DEFINE_RES_IRQ(evt2irq(0xe40)),
 };
 
 static struct platform_device tmu1_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu",
 	.id		= 1,
 	.dev = {
 		.platform_data	= &tmu1_platform_data,
@@ -213,124 +199,12 @@ static struct platform_device tmu1_device = {
 	.num_resources	= ARRAY_SIZE(tmu1_resources),
 };
 
-static struct sh_timer_config tmu2_platform_data = {
-	.channel_offset = 0x1c,
-	.timer_bit = 2,
-};
-
-static struct resource tmu2_resources[] = {
-	[0] = {
-		.start	= 0xffd80020,
-		.end	= 0xffd8002f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x5c0),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu2_device = {
-	.name		= "sh_tmu",
-	.id		= 2,
-	.dev = {
-		.platform_data	= &tmu2_platform_data,
-	},
-	.resource	= tmu2_resources,
-	.num_resources	= ARRAY_SIZE(tmu2_resources),
-};
-
-static struct sh_timer_config tmu3_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
-};
-
-static struct resource tmu3_resources[] = {
-	[0] = {
-		.start	= 0xffd88008,
-		.end	= 0xffd88013,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0xe00),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu3_device = {
-	.name		= "sh_tmu",
-	.id		= 3,
-	.dev = {
-		.platform_data	= &tmu3_platform_data,
-	},
-	.resource	= tmu3_resources,
-	.num_resources	= ARRAY_SIZE(tmu3_resources),
-};
-
-static struct sh_timer_config tmu4_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-};
-
-static struct resource tmu4_resources[] = {
-	[0] = {
-		.start	= 0xffd88014,
-		.end	= 0xffd8801f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0xe20),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu4_device = {
-	.name		= "sh_tmu",
-	.id		= 4,
-	.dev = {
-		.platform_data	= &tmu4_platform_data,
-	},
-	.resource	= tmu4_resources,
-	.num_resources	= ARRAY_SIZE(tmu4_resources),
-};
-
-static struct sh_timer_config tmu5_platform_data = {
-	.channel_offset = 0x1c,
-	.timer_bit = 2,
-};
-
-static struct resource tmu5_resources[] = {
-	[0] = {
-		.start	= 0xffd88020,
-		.end	= 0xffd8802b,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0xe40),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu5_device = {
-	.name		= "sh_tmu",
-	.id		= 5,
-	.dev = {
-		.platform_data	= &tmu5_platform_data,
-	},
-	.resource	= tmu5_resources,
-	.num_resources	= ARRAY_SIZE(tmu5_resources),
-};
-
 static struct platform_device *sh7763_devices[] __initdata = {
 	&scif0_device,
 	&scif1_device,
 	&scif2_device,
 	&tmu0_device,
 	&tmu1_device,
-	&tmu2_device,
-	&tmu3_device,
-	&tmu4_device,
-	&tmu5_device,
 	&rtc_device,
 	&usb_ohci_device,
 	&usbf_device,
@@ -349,10 +223,6 @@ static struct platform_device *sh7763_early_devices[] __initdata = {
 	&scif2_device,
 	&tmu0_device,
 	&tmu1_device,
-	&tmu2_device,
-	&tmu3_device,
-	&tmu4_device,
-	&tmu5_device,
 };
 
 void __init plat_early_device_setup(void)
diff --git a/arch/sh/kernel/cpu/sh4a/setup-sh7770.c b/arch/sh/kernel/cpu/sh4a/setup-sh7770.c
index e1ba8cb74e5..e9b532a76c3 100644
--- a/arch/sh/kernel/cpu/sh4a/setup-sh7770.c
+++ b/arch/sh/kernel/cpu/sh4a/setup-sh7770.c
@@ -226,25 +226,18 @@ static struct platform_device scif9_device = {
 };
 
 static struct sh_timer_config tmu0_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
+	.channels_mask = 7,
 };
 
 static struct resource tmu0_resources[] = {
-	[0] = {
-		.start	= 0xffd80008,
-		.end	= 0xffd80013,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x400),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xffd80000, 0x30),
+	DEFINE_RES_IRQ(evt2irq(0x400)),
+	DEFINE_RES_IRQ(evt2irq(0x420)),
+	DEFINE_RES_IRQ(evt2irq(0x440)),
 };
 
 static struct platform_device tmu0_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu",
 	.id		= 0,
 	.dev = {
 		.platform_data	= &tmu0_platform_data,
@@ -254,25 +247,18 @@ static struct platform_device tmu0_device = {
 };
 
 static struct sh_timer_config tmu1_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-	.clocksource_rating = 200,
+	.channels_mask = 7,
 };
 
 static struct resource tmu1_resources[] = {
-	[0] = {
-		.start	= 0xffd80014,
-		.end	= 0xffd8001f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x420),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xffd81000, 0x30),
+	DEFINE_RES_IRQ(evt2irq(0x460)),
+	DEFINE_RES_IRQ(evt2irq(0x480)),
+	DEFINE_RES_IRQ(evt2irq(0x4a0)),
 };
 
 static struct platform_device tmu1_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu",
 	.id		= 1,
 	.dev = {
 		.platform_data	= &tmu1_platform_data,
@@ -282,24 +268,18 @@ static struct platform_device tmu1_device = {
 };
 
 static struct sh_timer_config tmu2_platform_data = {
-	.channel_offset = 0x1c,
-	.timer_bit = 2,
+	.channels_mask = 7,
 };
 
 static struct resource tmu2_resources[] = {
-	[0] = {
-		.start	= 0xffd80020,
-		.end	= 0xffd8002f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x440),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xffd82000, 0x2c),
+	DEFINE_RES_IRQ(evt2irq(0x4c0)),
+	DEFINE_RES_IRQ(evt2irq(0x4e0)),
+	DEFINE_RES_IRQ(evt2irq(0x500)),
 };
 
 static struct platform_device tmu2_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu",
 	.id		= 2,
 	.dev = {
 		.platform_data	= &tmu2_platform_data,
@@ -308,168 +288,6 @@ static struct platform_device tmu2_device = {
 	.num_resources	= ARRAY_SIZE(tmu2_resources),
 };
 
-static struct sh_timer_config tmu3_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
-};
-
-static struct resource tmu3_resources[] = {
-	[0] = {
-		.start	= 0xffd81008,
-		.end	= 0xffd81013,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x460),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu3_device = {
-	.name		= "sh_tmu",
-	.id		= 3,
-	.dev = {
-		.platform_data	= &tmu3_platform_data,
-	},
-	.resource	= tmu3_resources,
-	.num_resources	= ARRAY_SIZE(tmu3_resources),
-};
-
-static struct sh_timer_config tmu4_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-};
-
-static struct resource tmu4_resources[] = {
-	[0] = {
-		.start	= 0xffd81014,
-		.end	= 0xffd8101f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x480),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu4_device = {
-	.name		= "sh_tmu",
-	.id		= 4,
-	.dev = {
-		.platform_data	= &tmu4_platform_data,
-	},
-	.resource	= tmu4_resources,
-	.num_resources	= ARRAY_SIZE(tmu4_resources),
-};
-
-static struct sh_timer_config tmu5_platform_data = {
-	.channel_offset = 0x1c,
-	.timer_bit = 2,
-};
-
-static struct resource tmu5_resources[] = {
-	[0] = {
-		.start	= 0xffd81020,
-		.end	= 0xffd8102f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x4a0),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu5_device = {
-	.name		= "sh_tmu",
-	.id		= 5,
-	.dev = {
-		.platform_data	= &tmu5_platform_data,
-	},
-	.resource	= tmu5_resources,
-	.num_resources	= ARRAY_SIZE(tmu5_resources),
-};
-
-static struct sh_timer_config tmu6_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
-};
-
-static struct resource tmu6_resources[] = {
-	[0] = {
-		.start	= 0xffd82008,
-		.end	= 0xffd82013,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x4c0),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu6_device = {
-	.name		= "sh_tmu",
-	.id		= 6,
-	.dev = {
-		.platform_data	= &tmu6_platform_data,
-	},
-	.resource	= tmu6_resources,
-	.num_resources	= ARRAY_SIZE(tmu6_resources),
-};
-
-static struct sh_timer_config tmu7_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-};
-
-static struct resource tmu7_resources[] = {
-	[0] = {
-		.start	= 0xffd82014,
-		.end	= 0xffd8201f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x4e0),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu7_device = {
-	.name		= "sh_tmu",
-	.id		= 7,
-	.dev = {
-		.platform_data	= &tmu7_platform_data,
-	},
-	.resource	= tmu7_resources,
-	.num_resources	= ARRAY_SIZE(tmu7_resources),
-};
-
-static struct sh_timer_config tmu8_platform_data = {
-	.channel_offset = 0x1c,
-	.timer_bit = 2,
-};
-
-static struct resource tmu8_resources[] = {
-	[0] = {
-		.start	= 0xffd82020,
-		.end	= 0xffd8202b,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x500),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu8_device = {
-	.name		= "sh_tmu",
-	.id		= 8,
-	.dev = {
-		.platform_data	= &tmu8_platform_data,
-	},
-	.resource	= tmu8_resources,
-	.num_resources	= ARRAY_SIZE(tmu8_resources),
-};
-
 static struct platform_device *sh7770_devices[] __initdata = {
 	&scif0_device,
 	&scif1_device,
@@ -484,12 +302,6 @@ static struct platform_device *sh7770_devices[] __initdata = {
 	&tmu0_device,
 	&tmu1_device,
 	&tmu2_device,
-	&tmu3_device,
-	&tmu4_device,
-	&tmu5_device,
-	&tmu6_device,
-	&tmu7_device,
-	&tmu8_device,
 };
 
 static int __init sh7770_devices_setup(void)
@@ -513,12 +325,6 @@ static struct platform_device *sh7770_early_devices[] __initdata = {
 	&tmu0_device,
 	&tmu1_device,
 	&tmu2_device,
-	&tmu3_device,
-	&tmu4_device,
-	&tmu5_device,
-	&tmu6_device,
-	&tmu7_device,
-	&tmu8_device,
 };
 
 void __init plat_early_device_setup(void)
diff --git a/arch/sh/kernel/cpu/sh4a/setup-sh7780.c b/arch/sh/kernel/cpu/sh4a/setup-sh7780.c
index 668e54bafa8..3ee7dd9b3a6 100644
--- a/arch/sh/kernel/cpu/sh4a/setup-sh7780.c
+++ b/arch/sh/kernel/cpu/sh4a/setup-sh7780.c
@@ -62,25 +62,18 @@ static struct platform_device scif1_device = {
 };
 
 static struct sh_timer_config tmu0_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
+	.channels_mask = 7,
 };
 
 static struct resource tmu0_resources[] = {
-	[0] = {
-		.start	= 0xffd80008,
-		.end	= 0xffd80013,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x580),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xffd80000, 0x30),
+	DEFINE_RES_IRQ(evt2irq(0x580)),
+	DEFINE_RES_IRQ(evt2irq(0x5a0)),
+	DEFINE_RES_IRQ(evt2irq(0x5c0)),
 };
 
 static struct platform_device tmu0_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu",
 	.id		= 0,
 	.dev = {
 		.platform_data	= &tmu0_platform_data,
@@ -90,25 +83,18 @@ static struct platform_device tmu0_device = {
 };
 
 static struct sh_timer_config tmu1_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-	.clocksource_rating = 200,
+	.channels_mask = 7,
 };
 
 static struct resource tmu1_resources[] = {
-	[0] = {
-		.start	= 0xffd80014,
-		.end	= 0xffd8001f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x5a0),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xffdc0000, 0x2c),
+	DEFINE_RES_IRQ(evt2irq(0xe00)),
+	DEFINE_RES_IRQ(evt2irq(0xe20)),
+	DEFINE_RES_IRQ(evt2irq(0xe40)),
 };
 
 static struct platform_device tmu1_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu",
 	.id		= 1,
 	.dev = {
 		.platform_data	= &tmu1_platform_data,
@@ -117,114 +103,6 @@ static struct platform_device tmu1_device = {
 	.num_resources	= ARRAY_SIZE(tmu1_resources),
 };
 
-static struct sh_timer_config tmu2_platform_data = {
-	.channel_offset = 0x1c,
-	.timer_bit = 2,
-};
-
-static struct resource tmu2_resources[] = {
-	[0] = {
-		.start	= 0xffd80020,
-		.end	= 0xffd8002f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x5c0),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu2_device = {
-	.name		= "sh_tmu",
-	.id		= 2,
-	.dev = {
-		.platform_data	= &tmu2_platform_data,
-	},
-	.resource	= tmu2_resources,
-	.num_resources	= ARRAY_SIZE(tmu2_resources),
-};
-
-static struct sh_timer_config tmu3_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
-};
-
-static struct resource tmu3_resources[] = {
-	[0] = {
-		.start	= 0xffdc0008,
-		.end	= 0xffdc0013,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0xe00),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu3_device = {
-	.name		= "sh_tmu",
-	.id		= 3,
-	.dev = {
-		.platform_data	= &tmu3_platform_data,
-	},
-	.resource	= tmu3_resources,
-	.num_resources	= ARRAY_SIZE(tmu3_resources),
-};
-
-static struct sh_timer_config tmu4_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-};
-
-static struct resource tmu4_resources[] = {
-	[0] = {
-		.start	= 0xffdc0014,
-		.end	= 0xffdc001f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0xe20),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu4_device = {
-	.name		= "sh_tmu",
-	.id		= 4,
-	.dev = {
-		.platform_data	= &tmu4_platform_data,
-	},
-	.resource	= tmu4_resources,
-	.num_resources	= ARRAY_SIZE(tmu4_resources),
-};
-
-static struct sh_timer_config tmu5_platform_data = {
-	.channel_offset = 0x1c,
-	.timer_bit = 2,
-};
-
-static struct resource tmu5_resources[] = {
-	[0] = {
-		.start	= 0xffdc0020,
-		.end	= 0xffdc002b,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0xe40),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu5_device = {
-	.name		= "sh_tmu",
-	.id		= 5,
-	.dev = {
-		.platform_data	= &tmu5_platform_data,
-	},
-	.resource	= tmu5_resources,
-	.num_resources	= ARRAY_SIZE(tmu5_resources),
-};
-
 static struct resource rtc_resources[] = {
 	[0] = {
 		.start	= 0xffe80000,
@@ -386,10 +264,6 @@ static struct platform_device *sh7780_devices[] __initdata = {
 	&scif1_device,
 	&tmu0_device,
 	&tmu1_device,
-	&tmu2_device,
-	&tmu3_device,
-	&tmu4_device,
-	&tmu5_device,
 	&rtc_device,
 	&dma0_device,
 	&dma1_device,
@@ -407,10 +281,6 @@ static struct platform_device *sh7780_early_devices[] __initdata = {
 	&scif1_device,
 	&tmu0_device,
 	&tmu1_device,
-	&tmu2_device,
-	&tmu3_device,
-	&tmu4_device,
-	&tmu5_device,
 };
 
 void __init plat_early_device_setup(void)
diff --git a/arch/sh/kernel/cpu/sh4a/setup-sh7785.c b/arch/sh/kernel/cpu/sh4a/setup-sh7785.c
index 4aa67914020..c72d5a5d099 100644
--- a/arch/sh/kernel/cpu/sh4a/setup-sh7785.c
+++ b/arch/sh/kernel/cpu/sh4a/setup-sh7785.c
@@ -152,25 +152,18 @@ static struct platform_device scif5_device = {
 };
 
 static struct sh_timer_config tmu0_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
+	.channels_mask = 7,
 };
 
 static struct resource tmu0_resources[] = {
-	[0] = {
-		.start	= 0xffd80008,
-		.end	= 0xffd80013,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x580),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xffd80000, 0x30),
+	DEFINE_RES_IRQ(evt2irq(0x580)),
+	DEFINE_RES_IRQ(evt2irq(0x5a0)),
+	DEFINE_RES_IRQ(evt2irq(0x5c0)),
 };
 
 static struct platform_device tmu0_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu",
 	.id		= 0,
 	.dev = {
 		.platform_data	= &tmu0_platform_data,
@@ -180,25 +173,18 @@ static struct platform_device tmu0_device = {
 };
 
 static struct sh_timer_config tmu1_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-	.clocksource_rating = 200,
+	.channels_mask = 7,
 };
 
 static struct resource tmu1_resources[] = {
-	[0] = {
-		.start	= 0xffd80014,
-		.end	= 0xffd8001f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x5a0),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xffdc0000, 0x2c),
+	DEFINE_RES_IRQ(evt2irq(0xe00)),
+	DEFINE_RES_IRQ(evt2irq(0xe20)),
+	DEFINE_RES_IRQ(evt2irq(0xe40)),
 };
 
 static struct platform_device tmu1_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu",
 	.id		= 1,
 	.dev = {
 		.platform_data	= &tmu1_platform_data,
@@ -207,114 +193,6 @@ static struct platform_device tmu1_device = {
 	.num_resources	= ARRAY_SIZE(tmu1_resources),
 };
 
-static struct sh_timer_config tmu2_platform_data = {
-	.channel_offset = 0x1c,
-	.timer_bit = 2,
-};
-
-static struct resource tmu2_resources[] = {
-	[0] = {
-		.start	= 0xffd80020,
-		.end	= 0xffd8002f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x5c0),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu2_device = {
-	.name		= "sh_tmu",
-	.id		= 2,
-	.dev = {
-		.platform_data	= &tmu2_platform_data,
-	},
-	.resource	= tmu2_resources,
-	.num_resources	= ARRAY_SIZE(tmu2_resources),
-};
-
-static struct sh_timer_config tmu3_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
-};
-
-static struct resource tmu3_resources[] = {
-	[0] = {
-		.start	= 0xffdc0008,
-		.end	= 0xffdc0013,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0xe00),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu3_device = {
-	.name		= "sh_tmu",
-	.id		= 3,
-	.dev = {
-		.platform_data	= &tmu3_platform_data,
-	},
-	.resource	= tmu3_resources,
-	.num_resources	= ARRAY_SIZE(tmu3_resources),
-};
-
-static struct sh_timer_config tmu4_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-};
-
-static struct resource tmu4_resources[] = {
-	[0] = {
-		.start	= 0xffdc0014,
-		.end	= 0xffdc001f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0xe20),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu4_device = {
-	.name		= "sh_tmu",
-	.id		= 4,
-	.dev = {
-		.platform_data	= &tmu4_platform_data,
-	},
-	.resource	= tmu4_resources,
-	.num_resources	= ARRAY_SIZE(tmu4_resources),
-};
-
-static struct sh_timer_config tmu5_platform_data = {
-	.channel_offset = 0x1c,
-	.timer_bit = 2,
-};
-
-static struct resource tmu5_resources[] = {
-	[0] = {
-		.start	= 0xffdc0020,
-		.end	= 0xffdc002b,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0xe40),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu5_device = {
-	.name		= "sh_tmu",
-	.id		= 5,
-	.dev = {
-		.platform_data	= &tmu5_platform_data,
-	},
-	.resource	= tmu5_resources,
-	.num_resources	= ARRAY_SIZE(tmu5_resources),
-};
-
 /* DMA */
 static const struct sh_dmae_channel sh7785_dmae0_channels[] = {
 	{
@@ -460,10 +338,6 @@ static struct platform_device *sh7785_devices[] __initdata = {
 	&scif5_device,
 	&tmu0_device,
 	&tmu1_device,
-	&tmu2_device,
-	&tmu3_device,
-	&tmu4_device,
-	&tmu5_device,
 	&dma0_device,
 	&dma1_device,
 };
@@ -484,10 +358,6 @@ static struct platform_device *sh7785_early_devices[] __initdata = {
 	&scif5_device,
 	&tmu0_device,
 	&tmu1_device,
-	&tmu2_device,
-	&tmu3_device,
-	&tmu4_device,
-	&tmu5_device,
 };
 
 void __init plat_early_device_setup(void)
diff --git a/arch/sh/kernel/cpu/sh4a/setup-sh7786.c b/arch/sh/kernel/cpu/sh4a/setup-sh7786.c
index 5d619a551a3..479e79bdd3d 100644
--- a/arch/sh/kernel/cpu/sh4a/setup-sh7786.c
+++ b/arch/sh/kernel/cpu/sh4a/setup-sh7786.c
@@ -175,25 +175,18 @@ static struct platform_device scif5_device = {
 };
 
 static struct sh_timer_config tmu0_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
+	.channels_mask = 7,
 };
 
 static struct resource tmu0_resources[] = {
-	[0] = {
-		.start	= 0xffd80008,
-		.end	= 0xffd80013,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x400),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xffd80000, 0x30),
+	DEFINE_RES_IRQ(evt2irq(0x400)),
+	DEFINE_RES_IRQ(evt2irq(0x420)),
+	DEFINE_RES_IRQ(evt2irq(0x440)),
 };
 
 static struct platform_device tmu0_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu",
 	.id		= 0,
 	.dev = {
 		.platform_data	= &tmu0_platform_data,
@@ -203,25 +196,18 @@ static struct platform_device tmu0_device = {
 };
 
 static struct sh_timer_config tmu1_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-	.clocksource_rating = 200,
+	.channels_mask = 7,
 };
 
 static struct resource tmu1_resources[] = {
-	[0] = {
-		.start	= 0xffd80014,
-		.end	= 0xffd8001f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x420),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xffda0000, 0x2c),
+	DEFINE_RES_IRQ(evt2irq(0x480)),
+	DEFINE_RES_IRQ(evt2irq(0x4a0)),
+	DEFINE_RES_IRQ(evt2irq(0x4c0)),
 };
 
 static struct platform_device tmu1_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu",
 	.id		= 1,
 	.dev = {
 		.platform_data	= &tmu1_platform_data,
@@ -231,24 +217,18 @@ static struct platform_device tmu1_device = {
 };
 
 static struct sh_timer_config tmu2_platform_data = {
-	.channel_offset = 0x1c,
-	.timer_bit = 2,
+	.channels_mask = 7,
 };
 
 static struct resource tmu2_resources[] = {
-	[0] = {
-		.start	= 0xffd80020,
-		.end	= 0xffd8002f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x440),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xffdc0000, 0x2c),
+	DEFINE_RES_IRQ(evt2irq(0x7a0)),
+	DEFINE_RES_IRQ(evt2irq(0x7a0)),
+	DEFINE_RES_IRQ(evt2irq(0x7a0)),
 };
 
 static struct platform_device tmu2_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu",
 	.id		= 2,
 	.dev = {
 		.platform_data	= &tmu2_platform_data,
@@ -258,24 +238,18 @@ static struct platform_device tmu2_device = {
 };
 
 static struct sh_timer_config tmu3_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
+	.channels_mask = 7,
 };
 
 static struct resource tmu3_resources[] = {
-	[0] = {
-		.start	= 0xffda0008,
-		.end	= 0xffda0013,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x480),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xffde0000, 0x2c),
+	DEFINE_RES_IRQ(evt2irq(0x7c0)),
+	DEFINE_RES_IRQ(evt2irq(0x7c0)),
+	DEFINE_RES_IRQ(evt2irq(0x7c0)),
 };
 
 static struct platform_device tmu3_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu",
 	.id		= 3,
 	.dev = {
 		.platform_data	= &tmu3_platform_data,
@@ -284,222 +258,6 @@ static struct platform_device tmu3_device = {
 	.num_resources	= ARRAY_SIZE(tmu3_resources),
 };
 
-static struct sh_timer_config tmu4_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-};
-
-static struct resource tmu4_resources[] = {
-	[0] = {
-		.start	= 0xffda0014,
-		.end	= 0xffda001f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x4a0),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu4_device = {
-	.name		= "sh_tmu",
-	.id		= 4,
-	.dev = {
-		.platform_data	= &tmu4_platform_data,
-	},
-	.resource	= tmu4_resources,
-	.num_resources	= ARRAY_SIZE(tmu4_resources),
-};
-
-static struct sh_timer_config tmu5_platform_data = {
-	.channel_offset = 0x1c,
-	.timer_bit = 2,
-};
-
-static struct resource tmu5_resources[] = {
-	[0] = {
-		.start	= 0xffda0020,
-		.end	= 0xffda002b,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x4c0),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu5_device = {
-	.name		= "sh_tmu",
-	.id		= 5,
-	.dev = {
-		.platform_data	= &tmu5_platform_data,
-	},
-	.resource	= tmu5_resources,
-	.num_resources	= ARRAY_SIZE(tmu5_resources),
-};
-
-static struct sh_timer_config tmu6_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
-};
-
-static struct resource tmu6_resources[] = {
-	[0] = {
-		.start	= 0xffdc0008,
-		.end	= 0xffdc0013,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x7a0),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu6_device = {
-	.name		= "sh_tmu",
-	.id		= 6,
-	.dev = {
-		.platform_data	= &tmu6_platform_data,
-	},
-	.resource	= tmu6_resources,
-	.num_resources	= ARRAY_SIZE(tmu6_resources),
-};
-
-static struct sh_timer_config tmu7_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-};
-
-static struct resource tmu7_resources[] = {
-	[0] = {
-		.start	= 0xffdc0014,
-		.end	= 0xffdc001f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x7a0),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu7_device = {
-	.name		= "sh_tmu",
-	.id		= 7,
-	.dev = {
-		.platform_data	= &tmu7_platform_data,
-	},
-	.resource	= tmu7_resources,
-	.num_resources	= ARRAY_SIZE(tmu7_resources),
-};
-
-static struct sh_timer_config tmu8_platform_data = {
-	.channel_offset = 0x1c,
-	.timer_bit = 2,
-};
-
-static struct resource tmu8_resources[] = {
-	[0] = {
-		.start	= 0xffdc0020,
-		.end	= 0xffdc002b,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x7a0),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu8_device = {
-	.name		= "sh_tmu",
-	.id		= 8,
-	.dev = {
-		.platform_data	= &tmu8_platform_data,
-	},
-	.resource	= tmu8_resources,
-	.num_resources	= ARRAY_SIZE(tmu8_resources),
-};
-
-static struct sh_timer_config tmu9_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
-};
-
-static struct resource tmu9_resources[] = {
-	[0] = {
-		.start	= 0xffde0008,
-		.end	= 0xffde0013,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x7c0),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu9_device = {
-	.name		= "sh_tmu",
-	.id		= 9,
-	.dev = {
-		.platform_data	= &tmu9_platform_data,
-	},
-	.resource	= tmu9_resources,
-	.num_resources	= ARRAY_SIZE(tmu9_resources),
-};
-
-static struct sh_timer_config tmu10_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-};
-
-static struct resource tmu10_resources[] = {
-	[0] = {
-		.start	= 0xffde0014,
-		.end	= 0xffde001f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x7c0),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu10_device = {
-	.name		= "sh_tmu",
-	.id		= 10,
-	.dev = {
-		.platform_data	= &tmu10_platform_data,
-	},
-	.resource	= tmu10_resources,
-	.num_resources	= ARRAY_SIZE(tmu10_resources),
-};
-
-static struct sh_timer_config tmu11_platform_data = {
-	.channel_offset = 0x1c,
-	.timer_bit = 2,
-};
-
-static struct resource tmu11_resources[] = {
-	[0] = {
-		.start	= 0xffde0020,
-		.end	= 0xffde002b,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x7c0),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu11_device = {
-	.name		= "sh_tmu",
-	.id		= 11,
-	.dev = {
-		.platform_data	= &tmu11_platform_data,
-	},
-	.resource	= tmu11_resources,
-	.num_resources	= ARRAY_SIZE(tmu11_resources),
-};
-
 static const struct sh_dmae_channel dmac0_channels[] = {
 	{
 		.offset = 0,
@@ -641,15 +399,6 @@ static struct platform_device *sh7786_early_devices[] __initdata = {
 	&tmu0_device,
 	&tmu1_device,
 	&tmu2_device,
-	&tmu3_device,
-	&tmu4_device,
-	&tmu5_device,
-	&tmu6_device,
-	&tmu7_device,
-	&tmu8_device,
-	&tmu9_device,
-	&tmu10_device,
-	&tmu11_device,
 };
 
 static struct platform_device *sh7786_devices[] __initdata = {
diff --git a/arch/sh/kernel/cpu/sh4a/setup-shx3.c b/arch/sh/kernel/cpu/sh4a/setup-shx3.c
index 0856bcbb1da..a78c5feb4e3 100644
--- a/arch/sh/kernel/cpu/sh4a/setup-shx3.c
+++ b/arch/sh/kernel/cpu/sh4a/setup-shx3.c
@@ -100,25 +100,18 @@ static struct platform_device scif2_device = {
 };
 
 static struct sh_timer_config tmu0_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
+	.channels_mask = 7,
 };
 
 static struct resource tmu0_resources[] = {
-	[0] = {
-		.start	= 0xffc10008,
-		.end	= 0xffc10013,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x400),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xffc10000, 0x30),
+	DEFINE_RES_IRQ(evt2irq(0x400)),
+	DEFINE_RES_IRQ(evt2irq(0x420)),
+	DEFINE_RES_IRQ(evt2irq(0x440)),
 };
 
 static struct platform_device tmu0_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu",
 	.id		= 0,
 	.dev = {
 		.platform_data	= &tmu0_platform_data,
@@ -128,25 +121,18 @@ static struct platform_device tmu0_device = {
 };
 
 static struct sh_timer_config tmu1_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-	.clocksource_rating = 200,
+	.channels_mask = 7,
 };
 
 static struct resource tmu1_resources[] = {
-	[0] = {
-		.start	= 0xffc10014,
-		.end	= 0xffc1001f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x420),
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(0xffc20000, 0x2c),
+	DEFINE_RES_IRQ(evt2irq(0x460)),
+	DEFINE_RES_IRQ(evt2irq(0x480)),
+	DEFINE_RES_IRQ(evt2irq(0x4a0)),
 };
 
 static struct platform_device tmu1_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu",
 	.id		= 1,
 	.dev = {
 		.platform_data	= &tmu1_platform_data,
@@ -155,124 +141,12 @@ static struct platform_device tmu1_device = {
 	.num_resources	= ARRAY_SIZE(tmu1_resources),
 };
 
-static struct sh_timer_config tmu2_platform_data = {
-	.channel_offset = 0x1c,
-	.timer_bit = 2,
-};
-
-static struct resource tmu2_resources[] = {
-	[0] = {
-		.start	= 0xffc10020,
-		.end	= 0xffc1002f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x440),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu2_device = {
-	.name		= "sh_tmu",
-	.id		= 2,
-	.dev = {
-		.platform_data	= &tmu2_platform_data,
-	},
-	.resource	= tmu2_resources,
-	.num_resources	= ARRAY_SIZE(tmu2_resources),
-};
-
-static struct sh_timer_config tmu3_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
-};
-
-static struct resource tmu3_resources[] = {
-	[0] = {
-		.start	= 0xffc20008,
-		.end	= 0xffc20013,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x460),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu3_device = {
-	.name		= "sh_tmu",
-	.id		= 3,
-	.dev = {
-		.platform_data	= &tmu3_platform_data,
-	},
-	.resource	= tmu3_resources,
-	.num_resources	= ARRAY_SIZE(tmu3_resources),
-};
-
-static struct sh_timer_config tmu4_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-};
-
-static struct resource tmu4_resources[] = {
-	[0] = {
-		.start	= 0xffc20014,
-		.end	= 0xffc2001f,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x480),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu4_device = {
-	.name		= "sh_tmu",
-	.id		= 4,
-	.dev = {
-		.platform_data	= &tmu4_platform_data,
-	},
-	.resource	= tmu4_resources,
-	.num_resources	= ARRAY_SIZE(tmu4_resources),
-};
-
-static struct sh_timer_config tmu5_platform_data = {
-	.channel_offset = 0x1c,
-	.timer_bit = 2,
-};
-
-static struct resource tmu5_resources[] = {
-	[0] = {
-		.start	= 0xffc20020,
-		.end	= 0xffc2002b,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= evt2irq(0x4a0),
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu5_device = {
-	.name		= "sh_tmu",
-	.id		= 5,
-	.dev = {
-		.platform_data	= &tmu5_platform_data,
-	},
-	.resource	= tmu5_resources,
-	.num_resources	= ARRAY_SIZE(tmu5_resources),
-};
-
 static struct platform_device *shx3_early_devices[] __initdata = {
 	&scif0_device,
 	&scif1_device,
 	&scif2_device,
 	&tmu0_device,
 	&tmu1_device,
-	&tmu2_device,
-	&tmu3_device,
-	&tmu4_device,
-	&tmu5_device,
 };
 
 static int __init shx3_devices_setup(void)
diff --git a/arch/sh/kernel/cpu/sh5/setup-sh5.c b/arch/sh/kernel/cpu/sh5/setup-sh5.c
index 14d68213d16..1bf0b2cf665 100644
--- a/arch/sh/kernel/cpu/sh5/setup-sh5.c
+++ b/arch/sh/kernel/cpu/sh5/setup-sh5.c
@@ -71,30 +71,20 @@ static struct platform_device rtc_device = {
 
 #define	TMU_BLOCK_OFF	0x01020000
 #define TMU_BASE	PHYS_PERIPHERAL_BLOCK + TMU_BLOCK_OFF
-#define TMU0_BASE	(TMU_BASE + 0x8 + (0xc * 0x0))
-#define TMU1_BASE	(TMU_BASE + 0x8 + (0xc * 0x1))
-#define TMU2_BASE	(TMU_BASE + 0x8 + (0xc * 0x2))
 
 static struct sh_timer_config tmu0_platform_data = {
-	.channel_offset = 0x04,
-	.timer_bit = 0,
-	.clockevent_rating = 200,
+	.channels_mask = 7,
 };
 
 static struct resource tmu0_resources[] = {
-	[0] = {
-		.start	= TMU0_BASE,
-		.end	= TMU0_BASE + 0xc - 1,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= IRQ_TUNI0,
-		.flags	= IORESOURCE_IRQ,
-	},
+	DEFINE_RES_MEM(TMU_BASE, 0x30),
+	DEFINE_RES_IRQ(IRQ_TUNI0),
+	DEFINE_RES_IRQ(IRQ_TUNI1),
+	DEFINE_RES_IRQ(IRQ_TUNI2),
 };
 
 static struct platform_device tmu0_device = {
-	.name		= "sh_tmu",
+	.name		= "sh-tmu",
 	.id		= 0,
 	.dev = {
 		.platform_data	= &tmu0_platform_data,
@@ -103,66 +93,9 @@ static struct platform_device tmu0_device = {
 	.num_resources	= ARRAY_SIZE(tmu0_resources),
 };
 
-static struct sh_timer_config tmu1_platform_data = {
-	.channel_offset = 0x10,
-	.timer_bit = 1,
-	.clocksource_rating = 200,
-};
-
-static struct resource tmu1_resources[] = {
-	[0] = {
-		.start	= TMU1_BASE,
-		.end	= TMU1_BASE + 0xc - 1,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= IRQ_TUNI1,
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu1_device = {
-	.name		= "sh_tmu",
-	.id		= 1,
-	.dev = {
-		.platform_data	= &tmu1_platform_data,
-	},
-	.resource	= tmu1_resources,
-	.num_resources	= ARRAY_SIZE(tmu1_resources),
-};
-
-static struct sh_timer_config tmu2_platform_data = {
-	.channel_offset = 0x1c,
-	.timer_bit = 2,
-};
-
-static struct resource tmu2_resources[] = {
-	[0] = {
-		.start	= TMU2_BASE,
-		.end	= TMU2_BASE + 0xc - 1,
-		.flags	= IORESOURCE_MEM,
-	},
-	[1] = {
-		.start	= IRQ_TUNI2,
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct platform_device tmu2_device = {
-	.name		= "sh_tmu",
-	.id		= 2,
-	.dev = {
-		.platform_data	= &tmu2_platform_data,
-	},
-	.resource	= tmu2_resources,
-	.num_resources	= ARRAY_SIZE(tmu2_resources),
-};
-
 static struct platform_device *sh5_early_devices[] __initdata = {
 	&scif0_device,
 	&tmu0_device,
-	&tmu1_device,
-	&tmu2_device,
 };
 
 static struct platform_device *sh5_devices[] __initdata = {
diff --git a/arch/sparc/include/asm/pci_32.h b/arch/sparc/include/asm/pci_32.h
index dc503297481..53e9b4987db 100644
--- a/arch/sparc/include/asm/pci_32.h
+++ b/arch/sparc/include/asm/pci_32.h
@@ -16,11 +16,6 @@
 
 #define PCI_IRQ_NONE		0xffffffff
 
-static inline void pcibios_penalize_isa_irq(int irq, int active)
-{
-	/* We don't do dynamic PCI IRQ allocation */
-}
-
 /* Dynamic DMA mapping stuff.
  */
 #define PCI_DMA_BUS_IS_PHYS	(0)
diff --git a/arch/sparc/include/asm/pci_64.h b/arch/sparc/include/asm/pci_64.h
index 1633b718d3b..c6c7396e762 100644
--- a/arch/sparc/include/asm/pci_64.h
+++ b/arch/sparc/include/asm/pci_64.h
@@ -16,11 +16,6 @@
 
 #define PCI_IRQ_NONE		0xffffffff
 
-static inline void pcibios_penalize_isa_irq(int irq, int active)
-{
-	/* We don't do dynamic PCI IRQ allocation */
-}
-
 /* The PCI address space does not equal the physical memory
  * address space.  The networking and block device layers use
  * this boolean for bounce buffer decisions.
diff --git a/arch/sparc/include/asm/pgtable_64.h b/arch/sparc/include/asm/pgtable_64.h
index 0f9e94537ee..1a49ffdf9da 100644
--- a/arch/sparc/include/asm/pgtable_64.h
+++ b/arch/sparc/include/asm/pgtable_64.h
@@ -24,7 +24,8 @@
 
 /* The kernel image occupies 0x4000000 to 0x6000000 (4MB --> 96MB).
  * The page copy blockops can use 0x6000000 to 0x8000000.
- * The TSB is mapped in the 0x8000000 to 0xa000000 range.
+ * The 8K TSB is mapped in the 0x8000000 to 0x8400000 range.
+ * The 4M TSB is mapped in the 0x8400000 to 0x8800000 range.
  * The PROM resides in an area spanning 0xf0000000 to 0x100000000.
  * The vmalloc area spans 0x100000000 to 0x200000000.
  * Since modules need to be in the lowest 32-bits of the address space,
@@ -33,7 +34,8 @@
  * 0x400000000.
  */
 #define	TLBTEMP_BASE		_AC(0x0000000006000000,UL)
-#define	TSBMAP_BASE		_AC(0x0000000008000000,UL)
+#define	TSBMAP_8K_BASE		_AC(0x0000000008000000,UL)
+#define	TSBMAP_4M_BASE		_AC(0x0000000008400000,UL)
 #define MODULES_VADDR		_AC(0x0000000010000000,UL)
 #define MODULES_LEN		_AC(0x00000000e0000000,UL)
 #define MODULES_END		_AC(0x00000000f0000000,UL)
@@ -71,6 +73,23 @@
 
 #include <linux/sched.h>
 
+extern unsigned long sparc64_valid_addr_bitmap[];
+
+/* Needs to be defined here and not in linux/mm.h, as it is arch dependent */
+static inline bool __kern_addr_valid(unsigned long paddr)
+{
+	if ((paddr >> MAX_PHYS_ADDRESS_BITS) != 0UL)
+		return false;
+	return test_bit(paddr >> ILOG2_4MB, sparc64_valid_addr_bitmap);
+}
+
+static inline bool kern_addr_valid(unsigned long addr)
+{
+	unsigned long paddr = __pa(addr);
+
+	return __kern_addr_valid(paddr);
+}
+
 /* Entries per page directory level. */
 #define PTRS_PER_PTE	(1UL << (PAGE_SHIFT-3))
 #define PTRS_PER_PMD	(1UL << PMD_BITS)
@@ -79,9 +98,12 @@
 /* Kernel has a separate 44bit address space. */
 #define FIRST_USER_ADDRESS	0
 
-#define pte_ERROR(e)	__builtin_trap()
-#define pmd_ERROR(e)	__builtin_trap()
-#define pgd_ERROR(e)	__builtin_trap()
+#define pmd_ERROR(e)							\
+	pr_err("%s:%d: bad pmd %p(%016lx) seen at (%pS)\n",		\
+	       __FILE__, __LINE__, &(e), pmd_val(e), __builtin_return_address(0))
+#define pgd_ERROR(e)							\
+	pr_err("%s:%d: bad pgd %p(%016lx) seen at (%pS)\n",		\
+	       __FILE__, __LINE__, &(e), pgd_val(e), __builtin_return_address(0))
 
 #endif /* !(__ASSEMBLY__) */
 
@@ -258,8 +280,8 @@ static inline pte_t pte_modify(pte_t pte, pgprot_t prot)
 {
 	unsigned long mask, tmp;
 
-	/* SUN4U: 0x600307ffffffecb8 (negated == 0x9ffcf80000001347)
-	 * SUN4V: 0x30ffffffffffee17 (negated == 0xcf000000000011e8)
+	/* SUN4U: 0x630107ffffffec38 (negated == 0x9cfef800000013c7)
+	 * SUN4V: 0x33ffffffffffee07 (negated == 0xcc000000000011f8)
 	 *
 	 * Even if we use negation tricks the result is still a 6
 	 * instruction sequence, so don't try to play fancy and just
@@ -289,10 +311,10 @@ static inline pte_t pte_modify(pte_t pte, pgprot_t prot)
 	"	.previous\n"
 	: "=r" (mask), "=r" (tmp)
 	: "i" (_PAGE_PADDR_4U | _PAGE_MODIFIED_4U | _PAGE_ACCESSED_4U |
-	       _PAGE_CP_4U | _PAGE_CV_4U | _PAGE_E_4U | _PAGE_PRESENT_4U |
+	       _PAGE_CP_4U | _PAGE_CV_4U | _PAGE_E_4U |
 	       _PAGE_SPECIAL | _PAGE_PMD_HUGE | _PAGE_SZALL_4U),
 	  "i" (_PAGE_PADDR_4V | _PAGE_MODIFIED_4V | _PAGE_ACCESSED_4V |
-	       _PAGE_CP_4V | _PAGE_CV_4V | _PAGE_E_4V | _PAGE_PRESENT_4V |
+	       _PAGE_CP_4V | _PAGE_CV_4V | _PAGE_E_4V |
 	       _PAGE_SPECIAL | _PAGE_PMD_HUGE | _PAGE_SZALL_4V));
 
 	return __pte((pte_val(pte) & mask) | (pgprot_val(prot) & ~mask));
@@ -633,7 +655,7 @@ static inline unsigned long pmd_large(pmd_t pmd)
 {
 	pte_t pte = __pte(pmd_val(pmd));
 
-	return (pte_val(pte) & _PAGE_PMD_HUGE) && pte_present(pte);
+	return pte_val(pte) & _PAGE_PMD_HUGE;
 }
 
 #ifdef CONFIG_TRANSPARENT_HUGEPAGE
@@ -719,20 +741,6 @@ static inline pmd_t pmd_mkwrite(pmd_t pmd)
 	return __pmd(pte_val(pte));
 }
 
-static inline pmd_t pmd_mknotpresent(pmd_t pmd)
-{
-	unsigned long mask;
-
-	if (tlb_type == hypervisor)
-		mask = _PAGE_PRESENT_4V;
-	else
-		mask = _PAGE_PRESENT_4U;
-
-	pmd_val(pmd) &= ~mask;
-
-	return pmd;
-}
-
 static inline pmd_t pmd_mksplitting(pmd_t pmd)
 {
 	pte_t pte = __pte(pmd_val(pmd));
@@ -757,6 +765,20 @@ static inline int pmd_present(pmd_t pmd)
 
 #define pmd_none(pmd)			(!pmd_val(pmd))
 
+/* pmd_bad() is only called on non-trans-huge PMDs.  Our encoding is
+ * very simple, it's just the physical address.  PTE tables are of
+ * size PAGE_SIZE so make sure the sub-PAGE_SIZE bits are clear and
+ * the top bits outside of the range of any physical address size we
+ * support are clear as well.  We also validate the physical itself.
+ */
+#define pmd_bad(pmd)			((pmd_val(pmd) & ~PAGE_MASK) || \
+					 !__kern_addr_valid(pmd_val(pmd)))
+
+#define pud_none(pud)			(!pud_val(pud))
+
+#define pud_bad(pud)			((pud_val(pud) & ~PAGE_MASK) || \
+					 !__kern_addr_valid(pud_val(pud)))
+
 #ifdef CONFIG_TRANSPARENT_HUGEPAGE
 extern void set_pmd_at(struct mm_struct *mm, unsigned long addr,
 		       pmd_t *pmdp, pmd_t pmd);
@@ -790,10 +812,7 @@ static inline unsigned long __pmd_page(pmd_t pmd)
 #define pud_page_vaddr(pud)		\
 	((unsigned long) __va(pud_val(pud)))
 #define pud_page(pud) 			virt_to_page((void *)pud_page_vaddr(pud))
-#define pmd_bad(pmd)			(0)
 #define pmd_clear(pmdp)			(pmd_val(*(pmdp)) = 0UL)
-#define pud_none(pud)			(!pud_val(pud))
-#define pud_bad(pud)			(0)
 #define pud_present(pud)		(pud_val(pud) != 0U)
 #define pud_clear(pudp)			(pud_val(*(pudp)) = 0UL)
 
@@ -893,6 +912,10 @@ extern void update_mmu_cache(struct vm_area_struct *, unsigned long, pte_t *);
 extern void update_mmu_cache_pmd(struct vm_area_struct *vma, unsigned long addr,
 				 pmd_t *pmd);
 
+#define __HAVE_ARCH_PMDP_INVALIDATE
+extern void pmdp_invalidate(struct vm_area_struct *vma, unsigned long address,
+			    pmd_t *pmdp);
+
 #define __HAVE_ARCH_PGTABLE_DEPOSIT
 extern void pgtable_trans_huge_deposit(struct mm_struct *mm, pmd_t *pmdp,
 				       pgtable_t pgtable);
@@ -919,18 +942,6 @@ extern unsigned long pte_file(pte_t);
 extern pte_t pgoff_to_pte(unsigned long);
 #define PTE_FILE_MAX_BITS	(64UL - PAGE_SHIFT - 1UL)
 
-extern unsigned long sparc64_valid_addr_bitmap[];
-
-/* Needs to be defined here and not in linux/mm.h, as it is arch dependent */
-static inline bool kern_addr_valid(unsigned long addr)
-{
-	unsigned long paddr = __pa(addr);
-
-	if ((paddr >> 41UL) != 0UL)
-		return false;
-	return test_bit(paddr >> 22, sparc64_valid_addr_bitmap);
-}
-
 extern int page_in_phys_avail(unsigned long paddr);
 
 /*
diff --git a/arch/sparc/include/asm/tsb.h b/arch/sparc/include/asm/tsb.h
index 2230f80d9fe..90916f955ca 100644
--- a/arch/sparc/include/asm/tsb.h
+++ b/arch/sparc/include/asm/tsb.h
@@ -171,7 +171,8 @@ extern struct tsb_phys_patch_entry __tsb_phys_patch, __tsb_phys_patch_end;
 	andcc		REG1, REG2, %g0;		\
 	be,pt		%xcc, 700f;			\
 	 sethi		%hi(4 * 1024 * 1024), REG2;	\
-	andn		REG1, REG2, REG1;		\
+	brgez,pn	REG1, FAIL_LABEL;		\
+	 andn		REG1, REG2, REG1;		\
 	and		VADDR, REG2, REG2;		\
 	brlz,pt		REG1, PTE_LABEL;		\
 	 or		REG1, REG2, REG1;		\
diff --git a/arch/sparc/kernel/head_64.S b/arch/sparc/kernel/head_64.S
index 26b706a1867..452f04fe8da 100644
--- a/arch/sparc/kernel/head_64.S
+++ b/arch/sparc/kernel/head_64.S
@@ -282,8 +282,8 @@ sun4v_chip_type:
 	stx	%l2, [%l4 + 0x0]
 	ldx	[%sp + 2047 + 128 + 0x50], %l3	! physaddr low
 	/* 4MB align */
-	srlx	%l3, 22, %l3
-	sllx	%l3, 22, %l3
+	srlx	%l3, ILOG2_4MB, %l3
+	sllx	%l3, ILOG2_4MB, %l3
 	stx	%l3, [%l4 + 0x8]
 
 	/* Leave service as-is, "call-method" */
diff --git a/arch/sparc/kernel/ktlb.S b/arch/sparc/kernel/ktlb.S
index 542e96ac4d3..605d4920458 100644
--- a/arch/sparc/kernel/ktlb.S
+++ b/arch/sparc/kernel/ktlb.S
@@ -277,7 +277,7 @@ kvmap_dtlb_load:
 #ifdef CONFIG_SPARSEMEM_VMEMMAP
 kvmap_vmemmap:
 	sub		%g4, %g5, %g5
-	srlx		%g5, 22, %g5
+	srlx		%g5, ILOG2_4MB, %g5
 	sethi		%hi(vmemmap_table), %g1
 	sllx		%g5, 3, %g5
 	or		%g1, %lo(vmemmap_table), %g1
diff --git a/arch/sparc/kernel/nmi.c b/arch/sparc/kernel/nmi.c
index 6479256fd5a..33709455691 100644
--- a/arch/sparc/kernel/nmi.c
+++ b/arch/sparc/kernel/nmi.c
@@ -68,27 +68,16 @@ EXPORT_SYMBOL(touch_nmi_watchdog);
 
 static void die_nmi(const char *str, struct pt_regs *regs, int do_panic)
 {
+	int this_cpu = smp_processor_id();
+
 	if (notify_die(DIE_NMIWATCHDOG, str, regs, 0,
 		       pt_regs_trap_type(regs), SIGINT) == NOTIFY_STOP)
 		return;
 
-	console_verbose();
-	bust_spinlocks(1);
-
-	printk(KERN_EMERG "%s", str);
-	printk(" on CPU%d, ip %08lx, registers:\n",
-	       smp_processor_id(), regs->tpc);
-	show_regs(regs);
-	dump_stack();
-
-	bust_spinlocks(0);
-
 	if (do_panic || panic_on_oops)
-		panic("Non maskable interrupt");
-
-	nmi_exit();
-	local_irq_enable();
-	do_exit(SIGBUS);
+		panic("Watchdog detected hard LOCKUP on cpu %d", this_cpu);
+	else
+		WARN(1, "Watchdog detected hard LOCKUP on cpu %d", this_cpu);
 }
 
 notrace __kprobes void perfctr_irq(int irq, struct pt_regs *regs)
diff --git a/arch/sparc/kernel/pci.c b/arch/sparc/kernel/pci.c
index 1555bbcae1e..857ad77df9c 100644
--- a/arch/sparc/kernel/pci.c
+++ b/arch/sparc/kernel/pci.c
@@ -543,8 +543,7 @@ static void pci_of_scan_bus(struct pci_pbm_info *pbm,
 			printk("PCI: dev header type: %x\n",
 			       dev->hdr_type);
 
-		if (dev->hdr_type == PCI_HEADER_TYPE_BRIDGE ||
-		    dev->hdr_type == PCI_HEADER_TYPE_CARDBUS)
+		if (pci_is_bridge(dev))
 			of_scan_pci_bridge(pbm, child, dev);
 	}
 }
diff --git a/arch/sparc/kernel/smp_64.c b/arch/sparc/kernel/smp_64.c
index 9781048161a..745a3633ce1 100644
--- a/arch/sparc/kernel/smp_64.c
+++ b/arch/sparc/kernel/smp_64.c
@@ -149,7 +149,7 @@ void cpu_panic(void)
 #define NUM_ROUNDS	64	/* magic value */
 #define NUM_ITERS	5	/* likewise */
 
-static DEFINE_SPINLOCK(itc_sync_lock);
+static DEFINE_RAW_SPINLOCK(itc_sync_lock);
 static unsigned long go[SLAVE + 1];
 
 #define DEBUG_TICK_SYNC	0
@@ -257,7 +257,7 @@ static void smp_synchronize_one_tick(int cpu)
 	go[MASTER] = 0;
 	membar_safe("#StoreLoad");
 
-	spin_lock_irqsave(&itc_sync_lock, flags);
+	raw_spin_lock_irqsave(&itc_sync_lock, flags);
 	{
 		for (i = 0; i < NUM_ROUNDS*NUM_ITERS; i++) {
 			while (!go[MASTER])
@@ -268,7 +268,7 @@ static void smp_synchronize_one_tick(int cpu)
 			membar_safe("#StoreLoad");
 		}
 	}
-	spin_unlock_irqrestore(&itc_sync_lock, flags);
+	raw_spin_unlock_irqrestore(&itc_sync_lock, flags);
 }
 
 #if defined(CONFIG_SUN_LDOMS) && defined(CONFIG_HOTPLUG_CPU)
diff --git a/arch/sparc/kernel/sys32.S b/arch/sparc/kernel/sys32.S
index f7c72b6efc2..d066eb18650 100644
--- a/arch/sparc/kernel/sys32.S
+++ b/arch/sparc/kernel/sys32.S
@@ -44,7 +44,7 @@ SIGN1(sys32_timer_settime, compat_sys_timer_settime, %o1)
 SIGN1(sys32_io_submit, compat_sys_io_submit, %o1)
 SIGN1(sys32_mq_open, compat_sys_mq_open, %o1)
 SIGN1(sys32_select, compat_sys_select, %o0)
-SIGN3(sys32_futex, compat_sys_futex, %o1, %o2, %o5)
+SIGN1(sys32_futex, compat_sys_futex, %o1)
 SIGN1(sys32_recvfrom, compat_sys_recvfrom, %o0)
 SIGN1(sys32_recvmsg, compat_sys_recvmsg, %o0)
 SIGN1(sys32_sendmsg, compat_sys_sendmsg, %o0)
diff --git a/arch/sparc/kernel/sysfs.c b/arch/sparc/kernel/sysfs.c
index a364000ca1a..7f41d40b7e6 100644
--- a/arch/sparc/kernel/sysfs.c
+++ b/arch/sparc/kernel/sysfs.c
@@ -151,7 +151,7 @@ static ssize_t store_mmustat_enable(struct device *s,
 			size_t count)
 {
 	unsigned long val, err;
-	int ret = sscanf(buf, "%ld", &val);
+	int ret = sscanf(buf, "%lu", &val);
 
 	if (ret != 1)
 		return -EINVAL;
diff --git a/arch/sparc/kernel/unaligned_64.c b/arch/sparc/kernel/unaligned_64.c
index 3c1a7cb3157..35ab8b60d25 100644
--- a/arch/sparc/kernel/unaligned_64.c
+++ b/arch/sparc/kernel/unaligned_64.c
@@ -166,17 +166,23 @@ static unsigned long *fetch_reg_addr(unsigned int reg, struct pt_regs *regs)
 unsigned long compute_effective_address(struct pt_regs *regs,
 					unsigned int insn, unsigned int rd)
 {
+	int from_kernel = (regs->tstate & TSTATE_PRIV) != 0;
 	unsigned int rs1 = (insn >> 14) & 0x1f;
 	unsigned int rs2 = insn & 0x1f;
-	int from_kernel = (regs->tstate & TSTATE_PRIV) != 0;
+	unsigned long addr;
 
 	if (insn & 0x2000) {
 		maybe_flush_windows(rs1, 0, rd, from_kernel);
-		return (fetch_reg(rs1, regs) + sign_extend_imm13(insn));
+		addr = (fetch_reg(rs1, regs) + sign_extend_imm13(insn));
 	} else {
 		maybe_flush_windows(rs1, rs2, rd, from_kernel);
-		return (fetch_reg(rs1, regs) + fetch_reg(rs2, regs));
+		addr = (fetch_reg(rs1, regs) + fetch_reg(rs2, regs));
 	}
+
+	if (!from_kernel && test_thread_flag(TIF_32BIT))
+		addr &= 0xffffffff;
+
+	return addr;
 }
 
 /* This is just to make gcc think die_if_kernel does return... */
diff --git a/arch/sparc/lib/NG2memcpy.S b/arch/sparc/lib/NG2memcpy.S
index 2c20ad63ddb..30eee6e8a81 100644
--- a/arch/sparc/lib/NG2memcpy.S
+++ b/arch/sparc/lib/NG2memcpy.S
@@ -236,6 +236,7 @@ FUNC_NAME:	/* %o0=dst, %o1=src, %o2=len */
 	 */
 	VISEntryHalf
 
+	membar		#Sync
 	alignaddr	%o1, %g0, %g0
 
 	add		%o1, (64 - 1), %o4
diff --git a/arch/sparc/mm/fault_64.c b/arch/sparc/mm/fault_64.c
index 69bb818fdd7..4ced3fc6613 100644
--- a/arch/sparc/mm/fault_64.c
+++ b/arch/sparc/mm/fault_64.c
@@ -96,38 +96,51 @@ static unsigned int get_user_insn(unsigned long tpc)
 	pte_t *ptep, pte;
 	unsigned long pa;
 	u32 insn = 0;
-	unsigned long pstate;
 
-	if (pgd_none(*pgdp))
-		goto outret;
+	if (pgd_none(*pgdp) || unlikely(pgd_bad(*pgdp)))
+		goto out;
 	pudp = pud_offset(pgdp, tpc);
-	if (pud_none(*pudp))
-		goto outret;
-	pmdp = pmd_offset(pudp, tpc);
-	if (pmd_none(*pmdp))
-		goto outret;
+	if (pud_none(*pudp) || unlikely(pud_bad(*pudp)))
+		goto out;
 
 	/* This disables preemption for us as well. */
-	__asm__ __volatile__("rdpr %%pstate, %0" : "=r" (pstate));
-	__asm__ __volatile__("wrpr %0, %1, %%pstate"
-				: : "r" (pstate), "i" (PSTATE_IE));
-	ptep = pte_offset_map(pmdp, tpc);
-	pte = *ptep;
-	if (!pte_present(pte))
-		goto out;
+	local_irq_disable();
+
+	pmdp = pmd_offset(pudp, tpc);
+	if (pmd_none(*pmdp) || unlikely(pmd_bad(*pmdp)))
+		goto out_irq_enable;
 
-	pa  = (pte_pfn(pte) << PAGE_SHIFT);
-	pa += (tpc & ~PAGE_MASK);
+#ifdef CONFIG_TRANSPARENT_HUGEPAGE
+	if (pmd_trans_huge(*pmdp)) {
+		if (pmd_trans_splitting(*pmdp))
+			goto out_irq_enable;
 
-	/* Use phys bypass so we don't pollute dtlb/dcache. */
-	__asm__ __volatile__("lduwa [%1] %2, %0"
-			     : "=r" (insn)
-			     : "r" (pa), "i" (ASI_PHYS_USE_EC));
+		pa  = pmd_pfn(*pmdp) << PAGE_SHIFT;
+		pa += tpc & ~HPAGE_MASK;
 
+		/* Use phys bypass so we don't pollute dtlb/dcache. */
+		__asm__ __volatile__("lduwa [%1] %2, %0"
+				     : "=r" (insn)
+				     : "r" (pa), "i" (ASI_PHYS_USE_EC));
+	} else
+#endif
+	{
+		ptep = pte_offset_map(pmdp, tpc);
+		pte = *ptep;
+		if (pte_present(pte)) {
+			pa  = (pte_pfn(pte) << PAGE_SHIFT);
+			pa += (tpc & ~PAGE_MASK);
+
+			/* Use phys bypass so we don't pollute dtlb/dcache. */
+			__asm__ __volatile__("lduwa [%1] %2, %0"
+					     : "=r" (insn)
+					     : "r" (pa), "i" (ASI_PHYS_USE_EC));
+		}
+		pte_unmap(ptep);
+	}
+out_irq_enable:
+	local_irq_enable();
 out:
-	pte_unmap(ptep);
-	__asm__ __volatile__("wrpr %0, 0x0, %%pstate" : : "r" (pstate));
-outret:
 	return insn;
 }
 
@@ -153,7 +166,8 @@ show_signal_msg(struct pt_regs *regs, int sig, int code,
 }
 
 static void do_fault_siginfo(int code, int sig, struct pt_regs *regs,
-			     unsigned int insn, int fault_code)
+			     unsigned long fault_addr, unsigned int insn,
+			     int fault_code)
 {
 	unsigned long addr;
 	siginfo_t info;
@@ -161,10 +175,18 @@ static void do_fault_siginfo(int code, int sig, struct pt_regs *regs,
 	info.si_code = code;
 	info.si_signo = sig;
 	info.si_errno = 0;
-	if (fault_code & FAULT_CODE_ITLB)
+	if (fault_code & FAULT_CODE_ITLB) {
 		addr = regs->tpc;
-	else
-		addr = compute_effective_address(regs, insn, 0);
+	} else {
+		/* If we were able to probe the faulting instruction, use it
+		 * to compute a precise fault address.  Otherwise use the fault
+		 * time provided address which may only have page granularity.
+		 */
+		if (insn)
+			addr = compute_effective_address(regs, insn, 0);
+		else
+			addr = fault_addr;
+	}
 	info.si_addr = (void __user *) addr;
 	info.si_trapno = 0;
 
@@ -239,7 +261,7 @@ static void __kprobes do_kernel_fault(struct pt_regs *regs, int si_code,
 		/* The si_code was set to make clear whether
 		 * this was a SEGV_MAPERR or SEGV_ACCERR fault.
 		 */
-		do_fault_siginfo(si_code, SIGSEGV, regs, insn, fault_code);
+		do_fault_siginfo(si_code, SIGSEGV, regs, address, insn, fault_code);
 		return;
 	}
 
@@ -259,18 +281,6 @@ static void noinline __kprobes bogus_32bit_fault_tpc(struct pt_regs *regs)
 	show_regs(regs);
 }
 
-static void noinline __kprobes bogus_32bit_fault_address(struct pt_regs *regs,
-							 unsigned long addr)
-{
-	static int times;
-
-	if (times++ < 10)
-		printk(KERN_ERR "FAULT[%s:%d]: 32-bit process "
-		       "reports 64-bit fault address [%lx]\n",
-		       current->comm, current->pid, addr);
-	show_regs(regs);
-}
-
 asmlinkage void __kprobes do_sparc64_fault(struct pt_regs *regs)
 {
 	enum ctx_state prev_state = exception_enter();
@@ -300,10 +310,8 @@ asmlinkage void __kprobes do_sparc64_fault(struct pt_regs *regs)
 				goto intr_or_no_mm;
 			}
 		}
-		if (unlikely((address >> 32) != 0)) {
-			bogus_32bit_fault_address(regs, address);
+		if (unlikely((address >> 32) != 0))
 			goto intr_or_no_mm;
-		}
 	}
 
 	if (regs->tstate & TSTATE_PRIV) {
@@ -525,7 +533,7 @@ do_sigbus:
 	 * Send a sigbus, regardless of whether we were in kernel
 	 * or user mode.
 	 */
-	do_fault_siginfo(BUS_ADRERR, SIGBUS, regs, insn, fault_code);
+	do_fault_siginfo(BUS_ADRERR, SIGBUS, regs, address, insn, fault_code);
 
 	/* Kernel mode? Handle exceptions or die */
 	if (regs->tstate & TSTATE_PRIV)
diff --git a/arch/sparc/mm/gup.c b/arch/sparc/mm/gup.c
index c4d3da68b80..1aed0432c64 100644
--- a/arch/sparc/mm/gup.c
+++ b/arch/sparc/mm/gup.c
@@ -73,7 +73,7 @@ static int gup_huge_pmd(pmd_t *pmdp, pmd_t pmd, unsigned long addr,
 	struct page *head, *page, *tail;
 	int refs;
 
-	if (!pmd_large(pmd))
+	if (!(pmd_val(pmd) & _PAGE_VALID))
 		return 0;
 
 	if (write && !pmd_write(pmd))
diff --git a/arch/sparc/mm/init_64.c b/arch/sparc/mm/init_64.c
index eafbc65c9c4..ed3c969a5f4 100644
--- a/arch/sparc/mm/init_64.c
+++ b/arch/sparc/mm/init_64.c
@@ -588,7 +588,7 @@ static void __init remap_kernel(void)
 	int i, tlb_ent = sparc64_highest_locked_tlbent();
 
 	tte_vaddr = (unsigned long) KERNBASE;
-	phys_page = (prom_boot_mapping_phys_low >> 22UL) << 22UL;
+	phys_page = (prom_boot_mapping_phys_low >> ILOG2_4MB) << ILOG2_4MB;
 	tte_data = kern_large_tte(phys_page);
 
 	kern_locked_tte_data = tte_data;
@@ -1881,7 +1881,7 @@ void __init paging_init(void)
 
 	BUILD_BUG_ON(NR_CPUS > 4096);
 
-	kern_base = (prom_boot_mapping_phys_low >> 22UL) << 22UL;
+	kern_base = (prom_boot_mapping_phys_low >> ILOG2_4MB) << ILOG2_4MB;
 	kern_size = (unsigned long)&_end - (unsigned long)KERNBASE;
 
 	/* Invalidate both kernel TSBs.  */
@@ -1937,7 +1937,7 @@ void __init paging_init(void)
 	shift = kern_base + PAGE_OFFSET - ((unsigned long)KERNBASE);
 
 	real_end = (unsigned long)_end;
-	num_kernel_image_mappings = DIV_ROUND_UP(real_end - KERNBASE, 1 << 22);
+	num_kernel_image_mappings = DIV_ROUND_UP(real_end - KERNBASE, 1 << ILOG2_4MB);
 	printk("Kernel: Using %d locked TLB entries for main kernel image.\n",
 	       num_kernel_image_mappings);
 
@@ -2094,7 +2094,7 @@ static void __init setup_valid_addr_bitmap_from_pavail(unsigned long *bitmap)
 
 				if (new_start <= old_start &&
 				    new_end >= (old_start + PAGE_SIZE)) {
-					set_bit(old_start >> 22, bitmap);
+					set_bit(old_start >> ILOG2_4MB, bitmap);
 					goto do_next_page;
 				}
 			}
@@ -2143,7 +2143,7 @@ void __init mem_init(void)
 	addr = PAGE_OFFSET + kern_base;
 	last = PAGE_ALIGN(kern_size) + addr;
 	while (addr < last) {
-		set_bit(__pa(addr) >> 22, sparc64_valid_addr_bitmap);
+		set_bit(__pa(addr) >> ILOG2_4MB, sparc64_valid_addr_bitmap);
 		addr += PAGE_SIZE;
 	}
 
@@ -2267,7 +2267,7 @@ int __meminit vmemmap_populate(unsigned long vstart, unsigned long vend,
 		void *block;
 
 		if (!(*vmem_pp & _PAGE_VALID)) {
-			block = vmemmap_alloc_block(1UL << 22, node);
+			block = vmemmap_alloc_block(1UL << ILOG2_4MB, node);
 			if (!block)
 				return -ENOMEM;
 
diff --git a/arch/sparc/mm/tlb.c b/arch/sparc/mm/tlb.c
index b12cb5e7281..b89aba217e3 100644
--- a/arch/sparc/mm/tlb.c
+++ b/arch/sparc/mm/tlb.c
@@ -134,7 +134,7 @@ no_cache_flush:
 
 #ifdef CONFIG_TRANSPARENT_HUGEPAGE
 static void tlb_batch_pmd_scan(struct mm_struct *mm, unsigned long vaddr,
-			       pmd_t pmd, bool exec)
+			       pmd_t pmd)
 {
 	unsigned long end;
 	pte_t *pte;
@@ -142,8 +142,11 @@ static void tlb_batch_pmd_scan(struct mm_struct *mm, unsigned long vaddr,
 	pte = pte_offset_map(&pmd, vaddr);
 	end = vaddr + HPAGE_SIZE;
 	while (vaddr < end) {
-		if (pte_val(*pte) & _PAGE_VALID)
+		if (pte_val(*pte) & _PAGE_VALID) {
+			bool exec = pte_exec(*pte);
+
 			tlb_batch_add_one(mm, vaddr, exec);
+		}
 		pte++;
 		vaddr += PAGE_SIZE;
 	}
@@ -177,19 +180,30 @@ void set_pmd_at(struct mm_struct *mm, unsigned long addr,
 	}
 
 	if (!pmd_none(orig)) {
-		pte_t orig_pte = __pte(pmd_val(orig));
-		bool exec = pte_exec(orig_pte);
-
 		addr &= HPAGE_MASK;
 		if (pmd_trans_huge(orig)) {
+			pte_t orig_pte = __pte(pmd_val(orig));
+			bool exec = pte_exec(orig_pte);
+
 			tlb_batch_add_one(mm, addr, exec);
 			tlb_batch_add_one(mm, addr + REAL_HPAGE_SIZE, exec);
 		} else {
-			tlb_batch_pmd_scan(mm, addr, orig, exec);
+			tlb_batch_pmd_scan(mm, addr, orig);
 		}
 	}
 }
 
+void pmdp_invalidate(struct vm_area_struct *vma, unsigned long address,
+		     pmd_t *pmdp)
+{
+	pmd_t entry = *pmdp;
+
+	pmd_val(entry) &= ~_PAGE_VALID;
+
+	set_pmd_at(vma->vm_mm, address, pmdp, entry);
+	flush_tlb_range(vma, address, address + HPAGE_PMD_SIZE);
+}
+
 void pgtable_trans_huge_deposit(struct mm_struct *mm, pmd_t *pmdp,
 				pgtable_t pgtable)
 {
diff --git a/arch/sparc/mm/tsb.c b/arch/sparc/mm/tsb.c
index f5d506fddda..fe19b81acc0 100644
--- a/arch/sparc/mm/tsb.c
+++ b/arch/sparc/mm/tsb.c
@@ -133,7 +133,19 @@ static void setup_tsb_params(struct mm_struct *mm, unsigned long tsb_idx, unsign
 	mm->context.tsb_block[tsb_idx].tsb_nentries =
 		tsb_bytes / sizeof(struct tsb);
 
-	base = TSBMAP_BASE;
+	switch (tsb_idx) {
+	case MM_TSB_BASE:
+		base = TSBMAP_8K_BASE;
+		break;
+#if defined(CONFIG_HUGETLB_PAGE) || defined(CONFIG_TRANSPARENT_HUGEPAGE)
+	case MM_TSB_HUGE:
+		base = TSBMAP_4M_BASE;
+		break;
+#endif
+	default:
+		BUG();
+	}
+
 	tte = pgprot_val(PAGE_KERNEL_LOCKED);
 	tsb_paddr = __pa(mm->context.tsb_block[tsb_idx].tsb);
 	BUG_ON(tsb_paddr & (tsb_bytes - 1UL));
diff --git a/arch/um/include/asm/tlb.h b/arch/um/include/asm/tlb.h
index 29b0301c18a..16eb63fac57 100644
--- a/arch/um/include/asm/tlb.h
+++ b/arch/um/include/asm/tlb.h
@@ -59,13 +59,25 @@ extern void flush_tlb_mm_range(struct mm_struct *mm, unsigned long start,
 			       unsigned long end);
 
 static inline void
+tlb_flush_mmu_tlbonly(struct mmu_gather *tlb)
+{
+	flush_tlb_mm_range(tlb->mm, tlb->start, tlb->end);
+}
+
+static inline void
+tlb_flush_mmu_free(struct mmu_gather *tlb)
+{
+	init_tlb_gather(tlb);
+}
+
+static inline void
 tlb_flush_mmu(struct mmu_gather *tlb)
 {
 	if (!tlb->need_flush)
 		return;
 
-	flush_tlb_mm_range(tlb->mm, tlb->start, tlb->end);
-	init_tlb_gather(tlb);
+	tlb_flush_mmu_tlbonly(tlb);
+	tlb_flush_mmu_free(tlb);
 }
 
 /* tlb_finish_mmu
diff --git a/arch/um/include/shared/os.h b/arch/um/include/shared/os.h
index 75298d3358e..08eec0b691b 100644
--- a/arch/um/include/shared/os.h
+++ b/arch/um/include/shared/os.h
@@ -136,6 +136,7 @@ extern int os_ioctl_generic(int fd, unsigned int cmd, unsigned long arg);
 extern int os_get_ifname(int fd, char *namebuf);
 extern int os_set_slip(int fd);
 extern int os_mode_fd(int fd, int mode);
+extern int os_fsync_file(int fd);
 
 extern int os_seek_file(int fd, unsigned long long offset);
 extern int os_open_file(const char *file, struct openflags flags, int mode);
diff --git a/arch/um/kernel/physmem.c b/arch/um/kernel/physmem.c
index f116db15d40..30fdd5d0067 100644
--- a/arch/um/kernel/physmem.c
+++ b/arch/um/kernel/physmem.c
@@ -103,6 +103,7 @@ void __init setup_physmem(unsigned long start, unsigned long reserve_end,
 	 */
 	os_seek_file(physmem_fd, __pa(&__syscall_stub_start));
 	os_write_file(physmem_fd, &__syscall_stub_start, PAGE_SIZE);
+	os_fsync_file(physmem_fd);
 
 	bootmap_size = init_bootmem(pfn, pfn + delta);
 	free_bootmem(__pa(reserve_end) + bootmap_size,
diff --git a/arch/um/os-Linux/file.c b/arch/um/os-Linux/file.c
index 07a750197bb..08d90fba952 100644
--- a/arch/um/os-Linux/file.c
+++ b/arch/um/os-Linux/file.c
@@ -237,6 +237,12 @@ void os_close_file(int fd)
 {
 	close(fd);
 }
+int os_fsync_file(int fd)
+{
+	if (fsync(fd) < 0)
+	    return -errno;
+	return 0;
+}
 
 int os_seek_file(int fd, unsigned long long offset)
 {
diff --git a/arch/um/os-Linux/main.c b/arch/um/os-Linux/main.c
index e1704ff600f..df9191acd92 100644
--- a/arch/um/os-Linux/main.c
+++ b/arch/um/os-Linux/main.c
@@ -151,6 +151,7 @@ int __init main(int argc, char **argv, char **envp)
 #endif
 
 	do_uml_initcalls();
+	change_sig(SIGPIPE, 0);
 	ret = linux_main(argc, argv);
 
 	/*
diff --git a/arch/um/os-Linux/mem.c b/arch/um/os-Linux/mem.c
index 3c4af77e51a..897e9ad0c10 100644
--- a/arch/um/os-Linux/mem.c
+++ b/arch/um/os-Linux/mem.c
@@ -12,337 +12,117 @@
 #include <string.h>
 #include <sys/stat.h>
 #include <sys/mman.h>
-#include <sys/param.h>
+#include <sys/vfs.h>
+#include <linux/magic.h>
 #include <init.h>
 #include <os.h>
 
-/* Modified by which_tmpdir, which is called during early boot */
-static char *default_tmpdir = "/tmp";
-
-/*
- *  Modified when creating the physical memory file and when checking
- * the tmp filesystem for usability, both happening during early boot.
- */
+/* Set by make_tempfile() during early boot. */
 static char *tempdir = NULL;
 
-static void __init find_tempdir(void)
+/* Check if dir is on tmpfs. Return 0 if yes, -1 if no or error. */
+static int __init check_tmpfs(const char *dir)
 {
-	const char *dirs[] = { "TMP", "TEMP", "TMPDIR", NULL };
-	int i;
-	char *dir = NULL;
-
-	if (tempdir != NULL)
-		/* We've already been called */
-		return;
-	for (i = 0; dirs[i]; i++) {
-		dir = getenv(dirs[i]);
-		if ((dir != NULL) && (*dir != '\0'))
-			break;
-	}
-	if ((dir == NULL) || (*dir == '\0'))
-		dir = default_tmpdir;
+	struct statfs st;
 
-	tempdir = malloc(strlen(dir) + 2);
-	if (tempdir == NULL) {
-		fprintf(stderr, "Failed to malloc tempdir, "
-			"errno = %d\n", errno);
-		return;
-	}
-	strcpy(tempdir, dir);
-	strcat(tempdir, "/");
-}
-
-/*
- * Remove bytes from the front of the buffer and refill it so that if there's a
- * partial string that we care about, it will be completed, and we can recognize
- * it.
- */
-static int pop(int fd, char *buf, size_t size, size_t npop)
-{
-	ssize_t n;
-	size_t len = strlen(&buf[npop]);
-
-	memmove(buf, &buf[npop], len + 1);
-	n = read(fd, &buf[len], size - len - 1);
-	if (n < 0)
-		return -errno;
-
-	buf[len + n] = '\0';
-	return 1;
-}
-
-/*
- * This will return 1, with the first character in buf being the
- * character following the next instance of c in the file.  This will
- * read the file as needed.  If there's an error, -errno is returned;
- * if the end of the file is reached, 0 is returned.
- */
-static int next(int fd, char *buf, size_t size, char c)
-{
-	ssize_t n;
-	char *ptr;
-
-	while ((ptr = strchr(buf, c)) == NULL) {
-		n = read(fd, buf, size - 1);
-		if (n == 0)
-			return 0;
-		else if (n < 0)
-			return -errno;
-
-		buf[n] = '\0';
+	printf("Checking if %s is on tmpfs...", dir);
+	if (statfs(dir, &st) < 0) {
+		printf("%s\n", strerror(errno));
+	} else if (st.f_type != TMPFS_MAGIC) {
+		printf("no\n");
+	} else {
+		printf("OK\n");
+		return 0;
 	}
-
-	return pop(fd, buf, size, ptr - buf + 1);
+	return -1;
 }
 
 /*
- * Decode an octal-escaped and space-terminated path of the form used by
- * /proc/mounts. May be used to decode a path in-place. "out" must be at least
- * as large as the input. The output is always null-terminated. "len" gets the
- * length of the output, excluding the trailing null. Returns 0 if a full path
- * was successfully decoded, otherwise an error.
+ * Choose the tempdir to use. We want something on tmpfs so that our memory is
+ * not subject to the host's vm.dirty_ratio. If a tempdir is specified in the
+ * environment, we use that even if it's not on tmpfs, but we warn the user.
+ * Otherwise, we try common tmpfs locations, and if no tmpfs directory is found
+ * then we fall back to /tmp.
  */
-static int decode_path(const char *in, char *out, size_t *len)
+static char * __init choose_tempdir(void)
 {
-	char *first = out;
-	int c;
+	static const char * const vars[] = {
+		"TMPDIR",
+		"TMP",
+		"TEMP",
+		NULL
+	};
+	static const char fallback_dir[] = "/tmp";
+	static const char * const tmpfs_dirs[] = {
+		"/dev/shm",
+		fallback_dir,
+		NULL
+	};
 	int i;
-	int ret = -EINVAL;
-	while (1) {
-		switch (*in) {
-		case '\0':
-			goto out;
-
-		case ' ':
-			ret = 0;
-			goto out;
-
-		case '\\':
-			in++;
-			c = 0;
-			for (i = 0; i < 3; i++) {
-				if (*in < '0' || *in > '7')
-					goto out;
-				c = (c << 3) | (*in++ - '0');
-			}
-			*(unsigned char *)out++ = (unsigned char) c;
-			break;
-
-		default:
-			*out++ = *in++;
-			break;
+	const char *dir;
+
+	printf("Checking environment variables for a tempdir...");
+	for (i = 0; vars[i]; i++) {
+		dir = getenv(vars[i]);
+		if ((dir != NULL) && (*dir != '\0')) {
+			printf("%s\n", dir);
+			if (check_tmpfs(dir) >= 0)
+				goto done;
+			else
+				goto warn;
 		}
 	}
+	printf("none found\n");
 
-out:
-	*out = '\0';
-	*len = out - first;
-	return ret;
-}
-
-/*
- * Computes the length of s when encoded with three-digit octal escape sequences
- * for the characters in chars.
- */
-static size_t octal_encoded_length(const char *s, const char *chars)
-{
-	size_t len = strlen(s);
-	while ((s = strpbrk(s, chars)) != NULL) {
-		len += 3;
-		s++;
-	}
-
-	return len;
-}
-
-enum {
-	OUTCOME_NOTHING_MOUNTED,
-	OUTCOME_TMPFS_MOUNT,
-	OUTCOME_NON_TMPFS_MOUNT,
-};
-
-/* Read a line of /proc/mounts data looking for a tmpfs mount at "path". */
-static int read_mount(int fd, char *buf, size_t bufsize, const char *path,
-		      int *outcome)
-{
-	int found;
-	int match;
-	char *space;
-	size_t len;
-
-	enum {
-		MATCH_NONE,
-		MATCH_EXACT,
-		MATCH_PARENT,
-	};
-
-	found = next(fd, buf, bufsize, ' ');
-	if (found != 1)
-		return found;
-
-	/*
-	 * If there's no following space in the buffer, then this path is
-	 * truncated, so it can't be the one we're looking for.
-	 */
-	space = strchr(buf, ' ');
-	if (space) {
-		match = MATCH_NONE;
-		if (!decode_path(buf, buf, &len)) {
-			if (!strcmp(buf, path))
-				match = MATCH_EXACT;
-			else if (!strncmp(buf, path, len)
-				 && (path[len] == '/' || !strcmp(buf, "/")))
-				match = MATCH_PARENT;
-		}
-
-		found = pop(fd, buf, bufsize, space - buf + 1);
-		if (found != 1)
-			return found;
-
-		switch (match) {
-		case MATCH_EXACT:
-			if (!strncmp(buf, "tmpfs", strlen("tmpfs")))
-				*outcome = OUTCOME_TMPFS_MOUNT;
-			else
-				*outcome = OUTCOME_NON_TMPFS_MOUNT;
-			break;
-
-		case MATCH_PARENT:
-			/* This mount obscures any previous ones. */
-			*outcome = OUTCOME_NOTHING_MOUNTED;
-			break;
-		}
+	for (i = 0; tmpfs_dirs[i]; i++) {
+		dir = tmpfs_dirs[i];
+		if (check_tmpfs(dir) >= 0)
+			goto done;
 	}
 
-	return next(fd, buf, bufsize, '\n');
+	dir = fallback_dir;
+warn:
+	printf("Warning: tempdir %s is not on tmpfs\n", dir);
+done:
+	/* Make a copy since getenv results may not remain valid forever. */
+	return strdup(dir);
 }
 
-/* which_tmpdir is called only during early boot */
-static int checked_tmpdir = 0;
-
 /*
- * Look for a tmpfs mounted at /dev/shm.  I couldn't find a cleaner
- * way to do this than to parse /proc/mounts.  statfs will return the
- * same filesystem magic number and fs id for both /dev and /dev/shm
- * when they are both tmpfs, so you can't tell if they are different
- * filesystems.  Also, there seems to be no other way of finding the
- * mount point of a filesystem from within it.
- *
- * If a /dev/shm tmpfs entry is found, then we switch to using it.
- * Otherwise, we stay with the default /tmp.
+ * Create an unlinked tempfile in a suitable tempdir. template must be the
+ * basename part of the template with a leading '/'.
  */
-static void which_tmpdir(void)
+static int __init make_tempfile(const char *template)
 {
+	char *tempname;
 	int fd;
-	int found;
-	int outcome;
-	char *path;
-	char *buf;
-	size_t bufsize;
 
-	if (checked_tmpdir)
-		return;
-
-	checked_tmpdir = 1;
-
-	printf("Checking for tmpfs mount on /dev/shm...");
-
-	path = realpath("/dev/shm", NULL);
-	if (!path) {
-		printf("failed to check real path, errno = %d\n", errno);
-		return;
-	}
-	printf("%s...", path);
-
-	/*
-	 * The buffer needs to be able to fit the full octal-escaped path, a
-	 * space, and a trailing null in order to successfully decode it.
-	 */
-	bufsize = octal_encoded_length(path, " \t\n\\") + 2;
-
-	if (bufsize < 128)
-		bufsize = 128;
-
-	buf = malloc(bufsize);
-	if (!buf) {
-		printf("malloc failed, errno = %d\n", errno);
-		goto out;
-	}
-	buf[0] = '\0';
-
-	fd = open("/proc/mounts", O_RDONLY);
-	if (fd < 0) {
-		printf("failed to open /proc/mounts, errno = %d\n", errno);
-		goto out1;
-	}
-
-	outcome = OUTCOME_NOTHING_MOUNTED;
-	while (1) {
-		found = read_mount(fd, buf, bufsize, path, &outcome);
-		if (found != 1)
-			break;
-	}
-
-	if (found < 0) {
-		printf("read returned errno %d\n", -found);
-	} else {
-		switch (outcome) {
-		case OUTCOME_TMPFS_MOUNT:
-			printf("OK\n");
-			default_tmpdir = "/dev/shm";
-			break;
-
-		case OUTCOME_NON_TMPFS_MOUNT:
-			printf("not tmpfs\n");
-			break;
-
-		default:
-			printf("nothing mounted on /dev/shm\n");
-			break;
+	if (tempdir == NULL) {
+		tempdir = choose_tempdir();
+		if (tempdir == NULL) {
+			fprintf(stderr, "Failed to choose tempdir: %s\n",
+				strerror(errno));
+			return -1;
 		}
 	}
 
-	close(fd);
-out1:
-	free(buf);
-out:
-	free(path);
-}
-
-static int __init make_tempfile(const char *template, char **out_tempname,
-				int do_unlink)
-{
-	char *tempname;
-	int fd;
-
-	which_tmpdir();
-	tempname = malloc(MAXPATHLEN);
+	tempname = malloc(strlen(tempdir) + strlen(template) + 1);
 	if (tempname == NULL)
 		return -1;
 
-	find_tempdir();
-	if ((tempdir == NULL) || (strlen(tempdir) >= MAXPATHLEN))
-		goto out;
-
-	if (template[0] != '/')
-		strcpy(tempname, tempdir);
-	else
-		tempname[0] = '\0';
-	strncat(tempname, template, MAXPATHLEN-1-strlen(tempname));
+	strcpy(tempname, tempdir);
+	strcat(tempname, template);
 	fd = mkstemp(tempname);
 	if (fd < 0) {
 		fprintf(stderr, "open - cannot create %s: %s\n", tempname,
 			strerror(errno));
 		goto out;
 	}
-	if (do_unlink && (unlink(tempname) < 0)) {
+	if (unlink(tempname) < 0) {
 		perror("unlink");
 		goto close;
 	}
-	if (out_tempname) {
-		*out_tempname = tempname;
-	} else
-		free(tempname);
+	free(tempname);
 	return fd;
 close:
 	close(fd);
@@ -351,14 +131,14 @@ out:
 	return -1;
 }
 
-#define TEMPNAME_TEMPLATE "vm_file-XXXXXX"
+#define TEMPNAME_TEMPLATE "/vm_file-XXXXXX"
 
 static int __init create_tmp_file(unsigned long long len)
 {
 	int fd, err;
 	char zero;
 
-	fd = make_tempfile(TEMPNAME_TEMPLATE, NULL, 1);
+	fd = make_tempfile(TEMPNAME_TEMPLATE);
 	if (fd < 0)
 		exit(1);
 
@@ -402,7 +182,6 @@ int __init create_mem_file(unsigned long long len)
 	return fd;
 }
 
-
 void __init check_tmpexec(void)
 {
 	void *addr;
@@ -410,14 +189,13 @@ void __init check_tmpexec(void)
 
 	addr = mmap(NULL, UM_KERN_PAGE_SIZE,
 		    PROT_READ | PROT_WRITE | PROT_EXEC, MAP_PRIVATE, fd, 0);
-	printf("Checking PROT_EXEC mmap in %s...",tempdir);
-	fflush(stdout);
+	printf("Checking PROT_EXEC mmap in %s...", tempdir);
 	if (addr == MAP_FAILED) {
 		err = errno;
-		perror("failed");
+		printf("%s\n", strerror(err));
 		close(fd);
 		if (err == EPERM)
-			printf("%s must be not mounted noexec\n",tempdir);
+			printf("%s must be not mounted noexec\n", tempdir);
 		exit(1);
 	}
 	printf("OK\n");
diff --git a/arch/unicore32/configs/unicore32_defconfig b/arch/unicore32/configs/unicore32_defconfig
index c9dd3198b6f..45f47f88d86 100644
--- a/arch/unicore32/configs/unicore32_defconfig
+++ b/arch/unicore32/configs/unicore32_defconfig
@@ -149,7 +149,6 @@ CONFIG_SND_PCM_OSS=m
 #	USB support
 CONFIG_USB_ARCH_HAS_HCD=n
 CONFIG_USB=n
-CONFIG_USB_DEVICEFS=n
 CONFIG_USB_PRINTER=n
 CONFIG_USB_STORAGE=n
 #	Inventra Highspeed Dual Role Controller
diff --git a/arch/unicore32/include/asm/pci.h b/arch/unicore32/include/asm/pci.h
index f5e108f4a15..654407e9861 100644
--- a/arch/unicore32/include/asm/pci.h
+++ b/arch/unicore32/include/asm/pci.h
@@ -18,11 +18,6 @@
 #include <asm-generic/pci.h>
 #include <mach/hardware.h> /* for PCIBIOS_MIN_* */
 
-static inline void pcibios_penalize_isa_irq(int irq, int active)
-{
-	/* We don't do dynamic PCI IRQ allocation */
-}
-
 #ifdef CONFIG_PCI
 static inline void pci_dma_burst_advice(struct pci_dev *pdev,
 					enum pci_dma_burst_strategy *strat,
diff --git a/arch/x86/Kconfig b/arch/x86/Kconfig
index 25d2c6f7325..650433ab184 100644
--- a/arch/x86/Kconfig
+++ b/arch/x86/Kconfig
@@ -261,6 +261,9 @@ config ARCH_HWEIGHT_CFLAGS
 config ARCH_SUPPORTS_UPROBES
 	def_bool y
 
+config FIX_EARLYCON_MEM
+	def_bool y
+
 source "init/Kconfig"
 source "kernel/Kconfig.freezer"
 
@@ -415,7 +418,6 @@ config X86_UV
 
 config X86_GOLDFISH
        bool "Goldfish (Virtual Platform)"
-       depends on X86_32
        depends on X86_EXTENDED_PLATFORM
        ---help---
 	 Enable support for the Goldfish virtual platform used primarily
diff --git a/arch/x86/Makefile b/arch/x86/Makefile
index d1b7c377a23..33f71b01fd2 100644
--- a/arch/x86/Makefile
+++ b/arch/x86/Makefile
@@ -79,11 +79,14 @@ else
         UTS_MACHINE := x86_64
         CHECKFLAGS += -D__x86_64__ -m64
 
+        biarch := -m64
         KBUILD_AFLAGS += -m64
         KBUILD_CFLAGS += -m64
 
         # Don't autogenerate traditional x87, MMX or SSE instructions
-        KBUILD_CFLAGS += -mno-mmx -mno-sse -mno-80387 -mno-fp-ret-in-387
+        KBUILD_CFLAGS += -mno-mmx -mno-sse
+        KBUILD_CFLAGS += $(call cc-option,-mno-80387)
+        KBUILD_CFLAGS += $(call cc-option,-mno-fp-ret-in-387)
 
 	# Use -mpreferred-stack-boundary=3 if supported.
 	KBUILD_CFLAGS += $(call cc-option,-mpreferred-stack-boundary=3)
diff --git a/arch/x86/boot/Makefile b/arch/x86/boot/Makefile
index abb9eba61b5..dbe8dd2fe24 100644
--- a/arch/x86/boot/Makefile
+++ b/arch/x86/boot/Makefile
@@ -71,7 +71,7 @@ $(obj)/vmlinux.bin: $(obj)/compressed/vmlinux FORCE
 
 SETUP_OBJS = $(addprefix $(obj)/,$(setup-y))
 
-sed-voffset := -e 's/^\([0-9a-fA-F]*\) . \(_text\|_end\)$$/\#define VO_\2 0x\1/p'
+sed-voffset := -e 's/^\([0-9a-fA-F]*\) [ABCDGRSTVW] \(_text\|_end\)$$/\#define VO_\2 0x\1/p'
 
 quiet_cmd_voffset = VOFFSET $@
       cmd_voffset = $(NM) $< | sed -n $(sed-voffset) > $@
@@ -80,7 +80,7 @@ targets += voffset.h
 $(obj)/voffset.h: vmlinux FORCE
 	$(call if_changed,voffset)
 
-sed-zoffset := -e 's/^\([0-9a-fA-F]*\) . \(startup_32\|startup_64\|efi32_stub_entry\|efi64_stub_entry\|efi_pe_entry\|input_data\|_end\|z_.*\)$$/\#define ZO_\2 0x\1/p'
+sed-zoffset := -e 's/^\([0-9a-fA-F]*\) [ABCDGRSTVW] \(startup_32\|startup_64\|efi32_stub_entry\|efi64_stub_entry\|efi_pe_entry\|input_data\|_end\|z_.*\)$$/\#define ZO_\2 0x\1/p'
 
 quiet_cmd_zoffset = ZOFFSET $@
       cmd_zoffset = $(NM) $< | sed -n $(sed-zoffset) > $@
diff --git a/arch/x86/boot/compressed/misc.c b/arch/x86/boot/compressed/misc.c
index 17684615374..57ab74df7ee 100644
--- a/arch/x86/boot/compressed/misc.c
+++ b/arch/x86/boot/compressed/misc.c
@@ -354,7 +354,7 @@ static void parse_elf(void *output)
 	free(phdrs);
 }
 
-asmlinkage void *decompress_kernel(void *rmode, memptr heap,
+asmlinkage __visible void *decompress_kernel(void *rmode, memptr heap,
 				  unsigned char *input_data,
 				  unsigned long input_len,
 				  unsigned char *output,
diff --git a/arch/x86/configs/i386_defconfig b/arch/x86/configs/i386_defconfig
index 619e7f7426c..32d2e7056c8 100644
--- a/arch/x86/configs/i386_defconfig
+++ b/arch/x86/configs/i386_defconfig
@@ -244,7 +244,6 @@ CONFIG_HID_TOPSEED=y
 CONFIG_HID_PID=y
 CONFIG_USB_HIDDEV=y
 CONFIG_USB=y
-CONFIG_USB_DEBUG=y
 CONFIG_USB_ANNOUNCE_NEW_DEVICES=y
 CONFIG_USB_MON=y
 CONFIG_USB_EHCI_HCD=y
diff --git a/arch/x86/configs/x86_64_defconfig b/arch/x86/configs/x86_64_defconfig
index 6181c69b786..a481dd4755d 100644
--- a/arch/x86/configs/x86_64_defconfig
+++ b/arch/x86/configs/x86_64_defconfig
@@ -239,7 +239,6 @@ CONFIG_HID_TOPSEED=y
 CONFIG_HID_PID=y
 CONFIG_USB_HIDDEV=y
 CONFIG_USB=y
-CONFIG_USB_DEBUG=y
 CONFIG_USB_ANNOUNCE_NEW_DEVICES=y
 CONFIG_USB_MON=y
 CONFIG_USB_EHCI_HCD=y
diff --git a/arch/x86/include/asm/hpet.h b/arch/x86/include/asm/hpet.h
index b18df579c0e..36f7125945e 100644
--- a/arch/x86/include/asm/hpet.h
+++ b/arch/x86/include/asm/hpet.h
@@ -63,6 +63,7 @@
 /* hpet memory map physical address */
 extern unsigned long hpet_address;
 extern unsigned long force_hpet_address;
+extern int boot_hpet_disable;
 extern u8 hpet_blockid;
 extern int hpet_force_user;
 extern u8 hpet_msi_disable;
diff --git a/arch/x86/include/asm/hugetlb.h b/arch/x86/include/asm/hugetlb.h
index a8091216963..68c05398bba 100644
--- a/arch/x86/include/asm/hugetlb.h
+++ b/arch/x86/include/asm/hugetlb.h
@@ -52,6 +52,7 @@ static inline pte_t huge_ptep_get_and_clear(struct mm_struct *mm,
 static inline void huge_ptep_clear_flush(struct vm_area_struct *vma,
 					 unsigned long addr, pte_t *ptep)
 {
+	ptep_clear_flush(vma, addr, ptep);
 }
 
 static inline int huge_pte_none(pte_t pte)
diff --git a/arch/x86/include/asm/page_64_types.h b/arch/x86/include/asm/page_64_types.h
index 8de6d9cf3b9..678205195ae 100644
--- a/arch/x86/include/asm/page_64_types.h
+++ b/arch/x86/include/asm/page_64_types.h
@@ -1,7 +1,7 @@
 #ifndef _ASM_X86_PAGE_64_DEFS_H
 #define _ASM_X86_PAGE_64_DEFS_H
 
-#define THREAD_SIZE_ORDER	1
+#define THREAD_SIZE_ORDER	2
 #define THREAD_SIZE  (PAGE_SIZE << THREAD_SIZE_ORDER)
 #define CURRENT_MASK (~(THREAD_SIZE - 1))
 
diff --git a/arch/x86/include/asm/pci.h b/arch/x86/include/asm/pci.h
index 96ae4f4040b..0892ea0e683 100644
--- a/arch/x86/include/asm/pci.h
+++ b/arch/x86/include/asm/pci.h
@@ -68,7 +68,6 @@ void pcibios_config_init(void);
 void pcibios_scan_root(int bus);
 
 void pcibios_set_master(struct pci_dev *dev);
-void pcibios_penalize_isa_irq(int irq, int active);
 struct irq_routing_table *pcibios_get_irq_routing_table(void);
 int pcibios_set_irq_routing(struct pci_dev *dev, int pin, int irq);
 
diff --git a/arch/x86/include/asm/xen/hypercall.h b/arch/x86/include/asm/xen/hypercall.h
index e709884d0ef..ca08a27b90b 100644
--- a/arch/x86/include/asm/xen/hypercall.h
+++ b/arch/x86/include/asm/xen/hypercall.h
@@ -343,7 +343,7 @@ HYPERVISOR_memory_op(unsigned int cmd, void *arg)
 }
 
 static inline int
-HYPERVISOR_multicall(void *call_list, int nr_calls)
+HYPERVISOR_multicall(void *call_list, uint32_t nr_calls)
 {
 	return _hypercall2(int, multicall, call_list, nr_calls);
 }
diff --git a/arch/x86/include/asm/xen/interface.h b/arch/x86/include/asm/xen/interface.h
index fd9cb7695b5..3400dbaec3c 100644
--- a/arch/x86/include/asm/xen/interface.h
+++ b/arch/x86/include/asm/xen/interface.h
@@ -54,6 +54,9 @@ typedef unsigned long xen_pfn_t;
 #define PRI_xen_pfn "lx"
 typedef unsigned long xen_ulong_t;
 #define PRI_xen_ulong "lx"
+typedef long xen_long_t;
+#define PRI_xen_long "lx"
+
 /* Guest handles for primitive C types. */
 __DEFINE_GUEST_HANDLE(uchar, unsigned char);
 __DEFINE_GUEST_HANDLE(uint,  unsigned int);
diff --git a/arch/x86/include/uapi/asm/msr-index.h b/arch/x86/include/uapi/asm/msr-index.h
index c827ace3121..fcf2b3ae1bf 100644
--- a/arch/x86/include/uapi/asm/msr-index.h
+++ b/arch/x86/include/uapi/asm/msr-index.h
@@ -384,7 +384,7 @@
 #define MSR_IA32_MISC_ENABLE_MWAIT_BIT			18
 #define MSR_IA32_MISC_ENABLE_MWAIT			(1ULL << MSR_IA32_MISC_ENABLE_MWAIT_BIT)
 #define MSR_IA32_MISC_ENABLE_LIMIT_CPUID_BIT		22
-#define MSR_IA32_MISC_ENABLE_LIMIT_CPUID		(1ULL << MSR_IA32_MISC_ENABLE_LIMIT_CPUID_BIT);
+#define MSR_IA32_MISC_ENABLE_LIMIT_CPUID		(1ULL << MSR_IA32_MISC_ENABLE_LIMIT_CPUID_BIT)
 #define MSR_IA32_MISC_ENABLE_XTPR_DISABLE_BIT		23
 #define MSR_IA32_MISC_ENABLE_XTPR_DISABLE		(1ULL << MSR_IA32_MISC_ENABLE_XTPR_DISABLE_BIT)
 #define MSR_IA32_MISC_ENABLE_XD_DISABLE_BIT		34
diff --git a/arch/x86/kernel/acpi/sleep.c b/arch/x86/kernel/acpi/sleep.c
index 3a2ae4c8894..31368207837 100644
--- a/arch/x86/kernel/acpi/sleep.c
+++ b/arch/x86/kernel/acpi/sleep.c
@@ -31,7 +31,7 @@ static char temp_stack[4096];
  *
  * Wrapper around acpi_enter_sleep_state() to be called by assmebly.
  */
-acpi_status asmlinkage x86_acpi_enter_sleep_state(u8 state)
+acpi_status asmlinkage __visible x86_acpi_enter_sleep_state(u8 state)
 {
 	return acpi_enter_sleep_state(state);
 }
diff --git a/arch/x86/kernel/aperture_64.c b/arch/x86/kernel/aperture_64.c
index 9fa8aa051f5..76164e173a2 100644
--- a/arch/x86/kernel/aperture_64.c
+++ b/arch/x86/kernel/aperture_64.c
@@ -10,6 +10,8 @@
  *
  * Copyright 2002 Andi Kleen, SuSE Labs.
  */
+#define pr_fmt(fmt) "AGP: " fmt
+
 #include <linux/kernel.h>
 #include <linux/types.h>
 #include <linux/init.h>
@@ -75,14 +77,13 @@ static u32 __init allocate_aperture(void)
 	addr = memblock_find_in_range(GART_MIN_ADDR, GART_MAX_ADDR,
 				      aper_size, aper_size);
 	if (!addr) {
-		printk(KERN_ERR
-			"Cannot allocate aperture memory hole (%lx,%uK)\n",
-				addr, aper_size>>10);
+		pr_err("Cannot allocate aperture memory hole [mem %#010lx-%#010lx] (%uKB)\n",
+		       addr, addr + aper_size - 1, aper_size >> 10);
 		return 0;
 	}
 	memblock_reserve(addr, aper_size);
-	printk(KERN_INFO "Mapping aperture over %d KB of RAM @ %lx\n",
-			aper_size >> 10, addr);
+	pr_info("Mapping aperture over RAM [mem %#010lx-%#010lx] (%uKB)\n",
+		addr, addr + aper_size - 1, aper_size >> 10);
 	register_nosave_region(addr >> PAGE_SHIFT,
 			       (addr+aper_size) >> PAGE_SHIFT);
 
@@ -126,10 +127,11 @@ static u32 __init read_agp(int bus, int slot, int func, int cap, u32 *order)
 	u64 aper;
 	u32 old_order;
 
-	printk(KERN_INFO "AGP bridge at %02x:%02x:%02x\n", bus, slot, func);
+	pr_info("pci 0000:%02x:%02x:%02x: AGP bridge\n", bus, slot, func);
 	apsizereg = read_pci_config_16(bus, slot, func, cap + 0x14);
 	if (apsizereg == 0xffffffff) {
-		printk(KERN_ERR "APSIZE in AGP bridge unreadable\n");
+		pr_err("pci 0000:%02x:%02x.%d: APSIZE unreadable\n",
+		       bus, slot, func);
 		return 0;
 	}
 
@@ -153,16 +155,18 @@ static u32 __init read_agp(int bus, int slot, int func, int cap, u32 *order)
 	 * On some sick chips, APSIZE is 0. It means it wants 4G
 	 * so let double check that order, and lets trust AMD NB settings:
 	 */
-	printk(KERN_INFO "Aperture from AGP @ %Lx old size %u MB\n",
-			aper, 32 << old_order);
+	pr_info("pci 0000:%02x:%02x.%d: AGP aperture [bus addr %#010Lx-%#010Lx] (old size %uMB)\n",
+		bus, slot, func, aper, aper + (32ULL << (old_order + 20)) - 1,
+		32 << old_order);
 	if (aper + (32ULL<<(20 + *order)) > 0x100000000ULL) {
-		printk(KERN_INFO "Aperture size %u MB (APSIZE %x) is not right, using settings from NB\n",
-				32 << *order, apsizereg);
+		pr_info("pci 0000:%02x:%02x.%d: AGP aperture size %uMB (APSIZE %#x) is not right, using settings from NB\n",
+			bus, slot, func, 32 << *order, apsizereg);
 		*order = old_order;
 	}
 
-	printk(KERN_INFO "Aperture from AGP @ %Lx size %u MB (APSIZE %x)\n",
-			aper, 32 << *order, apsizereg);
+	pr_info("pci 0000:%02x:%02x.%d: AGP aperture [bus addr %#010Lx-%#010Lx] (%uMB, APSIZE %#x)\n",
+		bus, slot, func, aper, aper + (32ULL << (*order + 20)) - 1,
+		32 << *order, apsizereg);
 
 	if (!aperture_valid(aper, (32*1024*1024) << *order, 32<<20))
 		return 0;
@@ -218,7 +222,7 @@ static u32 __init search_agp_bridge(u32 *order, int *valid_agp)
 			}
 		}
 	}
-	printk(KERN_INFO "No AGP bridge found\n");
+	pr_info("No AGP bridge found\n");
 
 	return 0;
 }
@@ -310,7 +314,8 @@ void __init early_gart_iommu_check(void)
 		if (e820_any_mapped(aper_base, aper_base + aper_size,
 				    E820_RAM)) {
 			/* reserve it, so we can reuse it in second kernel */
-			printk(KERN_INFO "update e820 for GART\n");
+			pr_info("e820: reserve [mem %#010Lx-%#010Lx] for GART\n",
+				aper_base, aper_base + aper_size - 1);
 			e820_add_region(aper_base, aper_size, E820_RESERVED);
 			update_e820();
 		}
@@ -354,7 +359,7 @@ int __init gart_iommu_hole_init(void)
 	    !early_pci_allowed())
 		return -ENODEV;
 
-	printk(KERN_INFO  "Checking aperture...\n");
+	pr_info("Checking aperture...\n");
 
 	if (!fallback_aper_force)
 		agp_aper_base = search_agp_bridge(&agp_aper_order, &valid_agp);
@@ -395,8 +400,9 @@ int __init gart_iommu_hole_init(void)
 			aper_base = read_pci_config(bus, slot, 3, AMD64_GARTAPERTUREBASE) & 0x7fff;
 			aper_base <<= 25;
 
-			printk(KERN_INFO "Node %d: aperture @ %Lx size %u MB\n",
-					node, aper_base, aper_size >> 20);
+			pr_info("Node %d: aperture [bus addr %#010Lx-%#010Lx] (%uMB)\n",
+				node, aper_base, aper_base + aper_size - 1,
+				aper_size >> 20);
 			node++;
 
 			if (!aperture_valid(aper_base, aper_size, 64<<20)) {
@@ -407,9 +413,9 @@ int __init gart_iommu_hole_init(void)
 					if (!no_iommu &&
 					    max_pfn > MAX_DMA32_PFN &&
 					    !printed_gart_size_msg) {
-						printk(KERN_ERR "you are using iommu with agp, but GART size is less than 64M\n");
-						printk(KERN_ERR "please increase GART size in your BIOS setup\n");
-						printk(KERN_ERR "if BIOS doesn't have that option, contact your HW vendor!\n");
+						pr_err("you are using iommu with agp, but GART size is less than 64MB\n");
+						pr_err("please increase GART size in your BIOS setup\n");
+						pr_err("if BIOS doesn't have that option, contact your HW vendor!\n");
 						printed_gart_size_msg = 1;
 					}
 				} else {
@@ -446,13 +452,10 @@ out:
 		   force_iommu ||
 		   valid_agp ||
 		   fallback_aper_force) {
-		printk(KERN_INFO
-			"Your BIOS doesn't leave a aperture memory hole\n");
-		printk(KERN_INFO
-			"Please enable the IOMMU option in the BIOS setup\n");
-		printk(KERN_INFO
-			"This costs you %d MB of RAM\n",
-				32 << fallback_aper_order);
+		pr_info("Your BIOS doesn't leave a aperture memory hole\n");
+		pr_info("Please enable the IOMMU option in the BIOS setup\n");
+		pr_info("This costs you %dMB of RAM\n",
+			32 << fallback_aper_order);
 
 		aper_order = fallback_aper_order;
 		aper_alloc = allocate_aperture();
diff --git a/arch/x86/kernel/apic/io_apic.c b/arch/x86/kernel/apic/io_apic.c
index 6ad4658de70..992060e0989 100644
--- a/arch/x86/kernel/apic/io_apic.c
+++ b/arch/x86/kernel/apic/io_apic.c
@@ -2189,7 +2189,7 @@ void send_cleanup_vector(struct irq_cfg *cfg)
 	cfg->move_in_progress = 0;
 }
 
-asmlinkage void smp_irq_move_cleanup_interrupt(void)
+asmlinkage __visible void smp_irq_move_cleanup_interrupt(void)
 {
 	unsigned vector, me;
 
@@ -3425,6 +3425,11 @@ int get_nr_irqs_gsi(void)
 	return nr_irqs_gsi;
 }
 
+unsigned int arch_dynirq_lower_bound(unsigned int from)
+{
+	return from < nr_irqs_gsi ? nr_irqs_gsi : from;
+}
+
 int __init arch_probe_nr_irqs(void)
 {
 	int nr;
diff --git a/arch/x86/kernel/cpu/mcheck/mce.c b/arch/x86/kernel/cpu/mcheck/mce.c
index eeee23ff75e..68317c80de7 100644
--- a/arch/x86/kernel/cpu/mcheck/mce.c
+++ b/arch/x86/kernel/cpu/mcheck/mce.c
@@ -598,7 +598,6 @@ void machine_check_poll(enum mcp_flags flags, mce_banks_t *b)
 {
 	struct mce m;
 	int i;
-	unsigned long *v;
 
 	this_cpu_inc(mce_poll_count);
 
@@ -618,8 +617,7 @@ void machine_check_poll(enum mcp_flags flags, mce_banks_t *b)
 		if (!(m.status & MCI_STATUS_VAL))
 			continue;
 
-		v = &get_cpu_var(mce_polled_error);
-		set_bit(0, v);
+		this_cpu_write(mce_polled_error, 1);
 		/*
 		 * Uncorrected or signalled events are handled by the exception
 		 * handler when it is enabled, so don't process those here.
diff --git a/arch/x86/kernel/cpu/mcheck/mce_intel.c b/arch/x86/kernel/cpu/mcheck/mce_intel.c
index 3bdb95ae8c4..9a316b21df8 100644
--- a/arch/x86/kernel/cpu/mcheck/mce_intel.c
+++ b/arch/x86/kernel/cpu/mcheck/mce_intel.c
@@ -42,7 +42,7 @@ static DEFINE_PER_CPU(mce_banks_t, mce_banks_owned);
  * cmci_discover_lock protects against parallel discovery attempts
  * which could race against each other.
  */
-static DEFINE_RAW_SPINLOCK(cmci_discover_lock);
+static DEFINE_SPINLOCK(cmci_discover_lock);
 
 #define CMCI_THRESHOLD		1
 #define CMCI_POLL_INTERVAL	(30 * HZ)
@@ -144,14 +144,14 @@ static void cmci_storm_disable_banks(void)
 	int bank;
 	u64 val;
 
-	raw_spin_lock_irqsave(&cmci_discover_lock, flags);
+	spin_lock_irqsave(&cmci_discover_lock, flags);
 	owned = __get_cpu_var(mce_banks_owned);
 	for_each_set_bit(bank, owned, MAX_NR_BANKS) {
 		rdmsrl(MSR_IA32_MCx_CTL2(bank), val);
 		val &= ~MCI_CTL2_CMCI_EN;
 		wrmsrl(MSR_IA32_MCx_CTL2(bank), val);
 	}
-	raw_spin_unlock_irqrestore(&cmci_discover_lock, flags);
+	spin_unlock_irqrestore(&cmci_discover_lock, flags);
 }
 
 static bool cmci_storm_detect(void)
@@ -211,7 +211,7 @@ static void cmci_discover(int banks)
 	int i;
 	int bios_wrong_thresh = 0;
 
-	raw_spin_lock_irqsave(&cmci_discover_lock, flags);
+	spin_lock_irqsave(&cmci_discover_lock, flags);
 	for (i = 0; i < banks; i++) {
 		u64 val;
 		int bios_zero_thresh = 0;
@@ -266,7 +266,7 @@ static void cmci_discover(int banks)
 			WARN_ON(!test_bit(i, __get_cpu_var(mce_poll_banks)));
 		}
 	}
-	raw_spin_unlock_irqrestore(&cmci_discover_lock, flags);
+	spin_unlock_irqrestore(&cmci_discover_lock, flags);
 	if (mca_cfg.bios_cmci_threshold && bios_wrong_thresh) {
 		pr_info_once(
 			"bios_cmci_threshold: Some banks do not have valid thresholds set\n");
@@ -316,10 +316,10 @@ void cmci_clear(void)
 
 	if (!cmci_supported(&banks))
 		return;
-	raw_spin_lock_irqsave(&cmci_discover_lock, flags);
+	spin_lock_irqsave(&cmci_discover_lock, flags);
 	for (i = 0; i < banks; i++)
 		__cmci_disable_bank(i);
-	raw_spin_unlock_irqrestore(&cmci_discover_lock, flags);
+	spin_unlock_irqrestore(&cmci_discover_lock, flags);
 }
 
 static void cmci_rediscover_work_func(void *arg)
@@ -360,9 +360,9 @@ void cmci_disable_bank(int bank)
 	if (!cmci_supported(&banks))
 		return;
 
-	raw_spin_lock_irqsave(&cmci_discover_lock, flags);
+	spin_lock_irqsave(&cmci_discover_lock, flags);
 	__cmci_disable_bank(bank);
-	raw_spin_unlock_irqrestore(&cmci_discover_lock, flags);
+	spin_unlock_irqrestore(&cmci_discover_lock, flags);
 }
 
 static void intel_init_cmci(void)
diff --git a/arch/x86/kernel/cpu/mcheck/therm_throt.c b/arch/x86/kernel/cpu/mcheck/therm_throt.c
index d921b7ee659..36a1bb6d1ee 100644
--- a/arch/x86/kernel/cpu/mcheck/therm_throt.c
+++ b/arch/x86/kernel/cpu/mcheck/therm_throt.c
@@ -429,14 +429,14 @@ static inline void __smp_thermal_interrupt(void)
 	smp_thermal_vector();
 }
 
-asmlinkage void smp_thermal_interrupt(struct pt_regs *regs)
+asmlinkage __visible void smp_thermal_interrupt(struct pt_regs *regs)
 {
 	entering_irq();
 	__smp_thermal_interrupt();
 	exiting_ack_irq();
 }
 
-asmlinkage void smp_trace_thermal_interrupt(struct pt_regs *regs)
+asmlinkage __visible void smp_trace_thermal_interrupt(struct pt_regs *regs)
 {
 	entering_irq();
 	trace_thermal_apic_entry(THERMAL_APIC_VECTOR);
diff --git a/arch/x86/kernel/cpu/mcheck/threshold.c b/arch/x86/kernel/cpu/mcheck/threshold.c
index fe6b1c86645..7245980186e 100644
--- a/arch/x86/kernel/cpu/mcheck/threshold.c
+++ b/arch/x86/kernel/cpu/mcheck/threshold.c
@@ -24,14 +24,14 @@ static inline void __smp_threshold_interrupt(void)
 	mce_threshold_vector();
 }
 
-asmlinkage void smp_threshold_interrupt(void)
+asmlinkage __visible void smp_threshold_interrupt(void)
 {
 	entering_irq();
 	__smp_threshold_interrupt();
 	exiting_ack_irq();
 }
 
-asmlinkage void smp_trace_threshold_interrupt(void)
+asmlinkage __visible void smp_trace_threshold_interrupt(void)
 {
 	entering_irq();
 	trace_threshold_apic_entry(THRESHOLD_APIC_VECTOR);
diff --git a/arch/x86/kernel/cpu/perf_event_intel.c b/arch/x86/kernel/cpu/perf_event_intel.c
index aa333d96688..adb02aa62af 100644
--- a/arch/x86/kernel/cpu/perf_event_intel.c
+++ b/arch/x86/kernel/cpu/perf_event_intel.c
@@ -169,7 +169,6 @@ static struct event_constraint intel_slm_event_constraints[] __read_mostly =
 {
 	FIXED_EVENT_CONSTRAINT(0x00c0, 0), /* INST_RETIRED.ANY */
 	FIXED_EVENT_CONSTRAINT(0x003c, 1), /* CPU_CLK_UNHALTED.CORE */
-	FIXED_EVENT_CONSTRAINT(0x013c, 2), /* CPU_CLK_UNHALTED.REF */
 	FIXED_EVENT_CONSTRAINT(0x0300, 2), /* pseudo CPU_CLK_UNHALTED.REF */
 	EVENT_CONSTRAINT_END
 };
diff --git a/arch/x86/kernel/cpu/perf_event_intel_rapl.c b/arch/x86/kernel/cpu/perf_event_intel_rapl.c
index 4b9a9e9466b..619f7699487 100644
--- a/arch/x86/kernel/cpu/perf_event_intel_rapl.c
+++ b/arch/x86/kernel/cpu/perf_event_intel_rapl.c
@@ -535,6 +535,7 @@ static int rapl_cpu_prepare(int cpu)
 	struct rapl_pmu *pmu = per_cpu(rapl_pmu, cpu);
 	int phys_id = topology_physical_package_id(cpu);
 	u64 ms;
+	u64 msr_rapl_power_unit_bits;
 
 	if (pmu)
 		return 0;
@@ -542,6 +543,10 @@ static int rapl_cpu_prepare(int cpu)
 	if (phys_id < 0)
 		return -1;
 
+	/* protect rdmsrl() to handle virtualization */
+	if (rdmsrl_safe(MSR_RAPL_POWER_UNIT, &msr_rapl_power_unit_bits))
+		return -1;
+
 	pmu = kzalloc_node(sizeof(*pmu), GFP_KERNEL, cpu_to_node(cpu));
 	if (!pmu)
 		return -1;
@@ -555,8 +560,7 @@ static int rapl_cpu_prepare(int cpu)
 	 *
 	 * we cache in local PMU instance
 	 */
-	rdmsrl(MSR_RAPL_POWER_UNIT, pmu->hw_unit);
-	pmu->hw_unit = (pmu->hw_unit >> 8) & 0x1FULL;
+	pmu->hw_unit = (msr_rapl_power_unit_bits >> 8) & 0x1FULL;
 	pmu->pmu = &rapl_pmu_class;
 
 	/*
@@ -677,7 +681,9 @@ static int __init rapl_pmu_init(void)
 	cpu_notifier_register_begin();
 
 	for_each_online_cpu(cpu) {
-		rapl_cpu_prepare(cpu);
+		ret = rapl_cpu_prepare(cpu);
+		if (ret)
+			goto out;
 		rapl_cpu_init(cpu);
 	}
 
@@ -700,6 +706,7 @@ static int __init rapl_pmu_init(void)
 		hweight32(rapl_cntr_mask),
 		ktime_to_ms(pmu->timer_interval));
 
+out:
 	cpu_notifier_register_done();
 
 	return 0;
diff --git a/arch/x86/kernel/cpu/rdrand.c b/arch/x86/kernel/cpu/rdrand.c
index 384df5105fb..136ac74dee8 100644
--- a/arch/x86/kernel/cpu/rdrand.c
+++ b/arch/x86/kernel/cpu/rdrand.c
@@ -27,6 +27,7 @@
 static int __init x86_rdrand_setup(char *s)
 {
 	setup_clear_cpu_cap(X86_FEATURE_RDRAND);
+	setup_clear_cpu_cap(X86_FEATURE_RDSEED);
 	return 1;
 }
 __setup("nordrand", x86_rdrand_setup);
diff --git a/arch/x86/kernel/early-quirks.c b/arch/x86/kernel/early-quirks.c
index 6e2537c3219..6cda0baeac9 100644
--- a/arch/x86/kernel/early-quirks.c
+++ b/arch/x86/kernel/early-quirks.c
@@ -17,6 +17,7 @@
 #include <asm/dma.h>
 #include <asm/io_apic.h>
 #include <asm/apic.h>
+#include <asm/hpet.h>
 #include <asm/iommu.h>
 #include <asm/gart.h>
 #include <asm/irq_remapping.h>
@@ -530,6 +531,15 @@ static void __init intel_graphics_stolen(int num, int slot, int func)
 	}
 }
 
+static void __init force_disable_hpet(int num, int slot, int func)
+{
+#ifdef CONFIG_HPET_TIMER
+	boot_hpet_disable = 1;
+	pr_info("x86/hpet: Will disable the HPET for this platform because it's not reliable\n");
+#endif
+}
+
+
 #define QFLAG_APPLY_ONCE 	0x1
 #define QFLAG_APPLIED		0x2
 #define QFLAG_DONE		(QFLAG_APPLY_ONCE|QFLAG_APPLIED)
@@ -567,6 +577,12 @@ static struct chipset early_qrk[] __initdata = {
 	  PCI_BASE_CLASS_BRIDGE, 0, intel_remapping_check },
 	{ PCI_VENDOR_ID_INTEL, PCI_ANY_ID, PCI_CLASS_DISPLAY_VGA, PCI_ANY_ID,
 	  QFLAG_APPLY_ONCE, intel_graphics_stolen },
+	/*
+	 * HPET on current version of Baytrail platform has accuracy
+	 * problems, disable it for now:
+	 */
+	{ PCI_VENDOR_ID_INTEL, 0x0f00,
+		PCI_CLASS_BRIDGE_HOST, PCI_ANY_ID, 0, force_disable_hpet},
 	{}
 };
 
diff --git a/arch/x86/kernel/head32.c b/arch/x86/kernel/head32.c
index c61a14a4a31..d6c1b983699 100644
--- a/arch/x86/kernel/head32.c
+++ b/arch/x86/kernel/head32.c
@@ -29,7 +29,7 @@ static void __init i386_default_early_setup(void)
 	reserve_ebda_region();
 }
 
-asmlinkage void __init i386_start_kernel(void)
+asmlinkage __visible void __init i386_start_kernel(void)
 {
 	sanitize_boot_params(&boot_params);
 
diff --git a/arch/x86/kernel/head64.c b/arch/x86/kernel/head64.c
index 85126ccbdf6..068054f4bf2 100644
--- a/arch/x86/kernel/head64.c
+++ b/arch/x86/kernel/head64.c
@@ -137,7 +137,7 @@ static void __init copy_bootdata(char *real_mode_data)
 	}
 }
 
-asmlinkage void __init x86_64_start_kernel(char * real_mode_data)
+asmlinkage __visible void __init x86_64_start_kernel(char * real_mode_data)
 {
 	int i;
 
diff --git a/arch/x86/kernel/hpet.c b/arch/x86/kernel/hpet.c
index 8d80ae01160..4177bfbc80b 100644
--- a/arch/x86/kernel/hpet.c
+++ b/arch/x86/kernel/hpet.c
@@ -88,7 +88,7 @@ static inline void hpet_clear_mapping(void)
 /*
  * HPET command line enable / disable
  */
-static int boot_hpet_disable;
+int boot_hpet_disable;
 int hpet_force_user;
 static int hpet_verbose;
 
diff --git a/arch/x86/kernel/kprobes/core.c b/arch/x86/kernel/kprobes/core.c
index 79a3f968287..61b17dc2c27 100644
--- a/arch/x86/kernel/kprobes/core.c
+++ b/arch/x86/kernel/kprobes/core.c
@@ -897,9 +897,10 @@ int __kprobes kprobe_fault_handler(struct pt_regs *regs, int trapnr)
 	struct kprobe *cur = kprobe_running();
 	struct kprobe_ctlblk *kcb = get_kprobe_ctlblk();
 
-	switch (kcb->kprobe_status) {
-	case KPROBE_HIT_SS:
-	case KPROBE_REENTER:
+	if (unlikely(regs->ip == (unsigned long)cur->ainsn.insn)) {
+		/* This must happen on single-stepping */
+		WARN_ON(kcb->kprobe_status != KPROBE_HIT_SS &&
+			kcb->kprobe_status != KPROBE_REENTER);
 		/*
 		 * We are here because the instruction being single
 		 * stepped caused a page fault. We reset the current
@@ -914,9 +915,8 @@ int __kprobes kprobe_fault_handler(struct pt_regs *regs, int trapnr)
 		else
 			reset_current_kprobe();
 		preempt_enable_no_resched();
-		break;
-	case KPROBE_HIT_ACTIVE:
-	case KPROBE_HIT_SSDONE:
+	} else if (kcb->kprobe_status == KPROBE_HIT_ACTIVE ||
+		   kcb->kprobe_status == KPROBE_HIT_SSDONE) {
 		/*
 		 * We increment the nmissed count for accounting,
 		 * we can also use npre/npostfault count for accounting
@@ -945,10 +945,8 @@ int __kprobes kprobe_fault_handler(struct pt_regs *regs, int trapnr)
 		 * fixup routine could not handle it,
 		 * Let do_page_fault() fix it.
 		 */
-		break;
-	default:
-		break;
 	}
+
 	return 0;
 }
 
diff --git a/arch/x86/kernel/ldt.c b/arch/x86/kernel/ldt.c
index af1d14a9ebd..dcbbaa165bd 100644
--- a/arch/x86/kernel/ldt.c
+++ b/arch/x86/kernel/ldt.c
@@ -20,6 +20,8 @@
 #include <asm/mmu_context.h>
 #include <asm/syscalls.h>
 
+int sysctl_ldt16 = 0;
+
 #ifdef CONFIG_SMP
 static void flush_ldt(void *current_mm)
 {
@@ -234,7 +236,7 @@ static int write_ldt(void __user *ptr, unsigned long bytecount, int oldmode)
 	 * IRET leaking the high bits of the kernel stack address.
 	 */
 #ifdef CONFIG_X86_64
-	if (!ldt_info.seg_32bit) {
+	if (!ldt_info.seg_32bit && !sysctl_ldt16) {
 		error = -EINVAL;
 		goto out_unlock;
 	}
diff --git a/arch/x86/kernel/process_64.c b/arch/x86/kernel/process_64.c
index 9c0280f93d0..898d077617a 100644
--- a/arch/x86/kernel/process_64.c
+++ b/arch/x86/kernel/process_64.c
@@ -52,7 +52,7 @@
 
 asmlinkage extern void ret_from_fork(void);
 
-asmlinkage DEFINE_PER_CPU(unsigned long, old_rsp);
+__visible DEFINE_PER_CPU(unsigned long, old_rsp);
 
 /* Prints also some state that isn't saved in the pt_regs */
 void __show_regs(struct pt_regs *regs, int all)
diff --git a/arch/x86/kernel/reboot.c b/arch/x86/kernel/reboot.c
index 3399d3a9973..52b1157c53e 100644
--- a/arch/x86/kernel/reboot.c
+++ b/arch/x86/kernel/reboot.c
@@ -191,6 +191,16 @@ static struct dmi_system_id __initdata reboot_dmi_table[] = {
 		},
 	},
 
+	/* Certec */
+	{       /* Handle problems with rebooting on Certec BPC600 */
+		.callback = set_pci_reboot,
+		.ident = "Certec BPC600",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "Certec"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "BPC600"),
+		},
+	},
+
 	/* Dell */
 	{	/* Handle problems with rebooting on Dell DXP061 */
 		.callback = set_bios_reboot,
diff --git a/arch/x86/kernel/smp.c b/arch/x86/kernel/smp.c
index 7c3a5a61f2e..be8e1bde07a 100644
--- a/arch/x86/kernel/smp.c
+++ b/arch/x86/kernel/smp.c
@@ -168,7 +168,7 @@ static int smp_stop_nmi_callback(unsigned int val, struct pt_regs *regs)
  * this function calls the 'stop' function on all other CPUs in the system.
  */
 
-asmlinkage void smp_reboot_interrupt(void)
+asmlinkage __visible void smp_reboot_interrupt(void)
 {
 	ack_APIC_irq();
 	irq_enter();
diff --git a/arch/x86/kernel/traps.c b/arch/x86/kernel/traps.c
index 57409f6b8c6..f73b5d435bd 100644
--- a/arch/x86/kernel/traps.c
+++ b/arch/x86/kernel/traps.c
@@ -357,7 +357,7 @@ exit:
  * for scheduling or signal handling. The actual stack switch is done in
  * entry.S
  */
-asmlinkage __kprobes struct pt_regs *sync_regs(struct pt_regs *eregs)
+asmlinkage __visible __kprobes struct pt_regs *sync_regs(struct pt_regs *eregs)
 {
 	struct pt_regs *regs = eregs;
 	/* Did already sync */
@@ -601,11 +601,11 @@ do_spurious_interrupt_bug(struct pt_regs *regs, long error_code)
 #endif
 }
 
-asmlinkage void __attribute__((weak)) smp_thermal_interrupt(void)
+asmlinkage __visible void __attribute__((weak)) smp_thermal_interrupt(void)
 {
 }
 
-asmlinkage void __attribute__((weak)) smp_threshold_interrupt(void)
+asmlinkage __visible void __attribute__((weak)) smp_threshold_interrupt(void)
 {
 }
 
diff --git a/arch/x86/kernel/vsmp_64.c b/arch/x86/kernel/vsmp_64.c
index f6584a90aba..b99b9ad8540 100644
--- a/arch/x86/kernel/vsmp_64.c
+++ b/arch/x86/kernel/vsmp_64.c
@@ -26,6 +26,9 @@
 
 #define TOPOLOGY_REGISTER_OFFSET 0x10
 
+/* Flag below is initialized once during vSMP PCI initialization. */
+static int irq_routing_comply = 1;
+
 #if defined CONFIG_PCI && defined CONFIG_PARAVIRT
 /*
  * Interrupt control on vSMPowered systems:
@@ -33,7 +36,7 @@
  * and vice versa.
  */
 
-asmlinkage unsigned long vsmp_save_fl(void)
+asmlinkage __visible unsigned long vsmp_save_fl(void)
 {
 	unsigned long flags = native_save_fl();
 
@@ -53,7 +56,7 @@ __visible void vsmp_restore_fl(unsigned long flags)
 }
 PV_CALLEE_SAVE_REGS_THUNK(vsmp_restore_fl);
 
-asmlinkage void vsmp_irq_disable(void)
+asmlinkage __visible void vsmp_irq_disable(void)
 {
 	unsigned long flags = native_save_fl();
 
@@ -61,7 +64,7 @@ asmlinkage void vsmp_irq_disable(void)
 }
 PV_CALLEE_SAVE_REGS_THUNK(vsmp_irq_disable);
 
-asmlinkage void vsmp_irq_enable(void)
+asmlinkage __visible void vsmp_irq_enable(void)
 {
 	unsigned long flags = native_save_fl();
 
@@ -101,6 +104,10 @@ static void __init set_vsmp_pv_ops(void)
 #ifdef CONFIG_SMP
 	if (cap & ctl & BIT(8)) {
 		ctl &= ~BIT(8);
+
+		/* Interrupt routing set to ignore */
+		irq_routing_comply = 0;
+
 #ifdef CONFIG_PROC_FS
 		/* Don't let users change irq affinity via procfs */
 		no_irq_affinity = 1;
@@ -218,7 +225,9 @@ static void vsmp_apic_post_init(void)
 {
 	/* need to update phys_pkg_id */
 	apic->phys_pkg_id = apicid_phys_pkg_id;
-	apic->vector_allocation_domain = fill_vector_allocation_domain;
+
+	if (!irq_routing_comply)
+		apic->vector_allocation_domain = fill_vector_allocation_domain;
 }
 
 void __init vsmp_init(void)
diff --git a/arch/x86/kernel/vsyscall_gtod.c b/arch/x86/kernel/vsyscall_gtod.c
index f9c6e56e14b..9531fbb123b 100644
--- a/arch/x86/kernel/vsyscall_gtod.c
+++ b/arch/x86/kernel/vsyscall_gtod.c
@@ -43,7 +43,7 @@ void update_vsyscall(struct timekeeper *tk)
 	vdata->monotonic_time_sec	= tk->xtime_sec
 					+ tk->wall_to_monotonic.tv_sec;
 	vdata->monotonic_time_snsec	= tk->xtime_nsec
-					+ (tk->wall_to_monotonic.tv_nsec
+					+ ((u64)tk->wall_to_monotonic.tv_nsec
 						<< tk->shift);
 	while (vdata->monotonic_time_snsec >=
 					(((u64)NSEC_PER_SEC) << tk->shift)) {
diff --git a/arch/x86/kvm/vmx.c b/arch/x86/kvm/vmx.c
index 1f68c583192..138ceffc637 100644
--- a/arch/x86/kvm/vmx.c
+++ b/arch/x86/kvm/vmx.c
@@ -503,7 +503,7 @@ static inline struct vcpu_vmx *to_vmx(struct kvm_vcpu *vcpu)
 				[number##_HIGH] = VMCS12_OFFSET(name)+4
 
 
-static const unsigned long shadow_read_only_fields[] = {
+static unsigned long shadow_read_only_fields[] = {
 	/*
 	 * We do NOT shadow fields that are modified when L0
 	 * traps and emulates any vmx instruction (e.g. VMPTRLD,
@@ -526,10 +526,10 @@ static const unsigned long shadow_read_only_fields[] = {
 	GUEST_LINEAR_ADDRESS,
 	GUEST_PHYSICAL_ADDRESS
 };
-static const int max_shadow_read_only_fields =
+static int max_shadow_read_only_fields =
 	ARRAY_SIZE(shadow_read_only_fields);
 
-static const unsigned long shadow_read_write_fields[] = {
+static unsigned long shadow_read_write_fields[] = {
 	GUEST_RIP,
 	GUEST_RSP,
 	GUEST_CR0,
@@ -558,7 +558,7 @@ static const unsigned long shadow_read_write_fields[] = {
 	HOST_FS_SELECTOR,
 	HOST_GS_SELECTOR
 };
-static const int max_shadow_read_write_fields =
+static int max_shadow_read_write_fields =
 	ARRAY_SIZE(shadow_read_write_fields);
 
 static const unsigned short vmcs_field_to_offset_table[] = {
@@ -3009,6 +3009,41 @@ static void free_kvm_area(void)
 	}
 }
 
+static void init_vmcs_shadow_fields(void)
+{
+	int i, j;
+
+	/* No checks for read only fields yet */
+
+	for (i = j = 0; i < max_shadow_read_write_fields; i++) {
+		switch (shadow_read_write_fields[i]) {
+		case GUEST_BNDCFGS:
+			if (!vmx_mpx_supported())
+				continue;
+			break;
+		default:
+			break;
+		}
+
+		if (j < i)
+			shadow_read_write_fields[j] =
+				shadow_read_write_fields[i];
+		j++;
+	}
+	max_shadow_read_write_fields = j;
+
+	/* shadowed fields guest access without vmexit */
+	for (i = 0; i < max_shadow_read_write_fields; i++) {
+		clear_bit(shadow_read_write_fields[i],
+			  vmx_vmwrite_bitmap);
+		clear_bit(shadow_read_write_fields[i],
+			  vmx_vmread_bitmap);
+	}
+	for (i = 0; i < max_shadow_read_only_fields; i++)
+		clear_bit(shadow_read_only_fields[i],
+			  vmx_vmread_bitmap);
+}
+
 static __init int alloc_kvm_area(void)
 {
 	int cpu;
@@ -3039,6 +3074,8 @@ static __init int hardware_setup(void)
 		enable_vpid = 0;
 	if (!cpu_has_vmx_shadow_vmcs())
 		enable_shadow_vmcs = 0;
+	if (enable_shadow_vmcs)
+		init_vmcs_shadow_fields();
 
 	if (!cpu_has_vmx_ept() ||
 	    !cpu_has_vmx_ept_4levels()) {
@@ -7741,7 +7778,8 @@ static void prepare_vmcs02(struct kvm_vcpu *vcpu, struct vmcs12 *vmcs12)
 
 	exec_control = vmcs12->pin_based_vm_exec_control;
 	exec_control |= vmcs_config.pin_based_exec_ctrl;
-	exec_control &= ~PIN_BASED_VMX_PREEMPTION_TIMER;
+	exec_control &= ~(PIN_BASED_VMX_PREEMPTION_TIMER |
+                          PIN_BASED_POSTED_INTR);
 	vmcs_write32(PIN_BASED_VM_EXEC_CONTROL, exec_control);
 
 	vmx->nested.preemption_timer_expired = false;
@@ -7778,7 +7816,9 @@ static void prepare_vmcs02(struct kvm_vcpu *vcpu, struct vmcs12 *vmcs12)
 		if (!vmx->rdtscp_enabled)
 			exec_control &= ~SECONDARY_EXEC_RDTSCP;
 		/* Take the following fields only from vmcs12 */
-		exec_control &= ~SECONDARY_EXEC_VIRTUALIZE_APIC_ACCESSES;
+		exec_control &= ~(SECONDARY_EXEC_VIRTUALIZE_APIC_ACCESSES |
+				  SECONDARY_EXEC_VIRTUAL_INTR_DELIVERY |
+                                  SECONDARY_EXEC_APIC_REGISTER_VIRT);
 		if (nested_cpu_has(vmcs12,
 				CPU_BASED_ACTIVATE_SECONDARY_CONTROLS))
 			exec_control |= vmcs12->secondary_vm_exec_control;
@@ -8803,14 +8843,6 @@ static int __init vmx_init(void)
 
 	memset(vmx_vmread_bitmap, 0xff, PAGE_SIZE);
 	memset(vmx_vmwrite_bitmap, 0xff, PAGE_SIZE);
-	/* shadowed read/write fields */
-	for (i = 0; i < max_shadow_read_write_fields; i++) {
-		clear_bit(shadow_read_write_fields[i], vmx_vmwrite_bitmap);
-		clear_bit(shadow_read_write_fields[i], vmx_vmread_bitmap);
-	}
-	/* shadowed read only fields */
-	for (i = 0; i < max_shadow_read_only_fields; i++)
-		clear_bit(shadow_read_only_fields[i], vmx_vmread_bitmap);
 
 	/*
 	 * Allow direct access to the PC debug port (it is often used for I/O
diff --git a/arch/x86/kvm/x86.c b/arch/x86/kvm/x86.c
index 8b8fc0b792b..20316c67b82 100644
--- a/arch/x86/kvm/x86.c
+++ b/arch/x86/kvm/x86.c
@@ -106,6 +106,8 @@ EXPORT_SYMBOL_GPL(kvm_max_guest_tsc_khz);
 static u32 tsc_tolerance_ppm = 250;
 module_param(tsc_tolerance_ppm, uint, S_IRUGO | S_IWUSR);
 
+static bool backwards_tsc_observed = false;
+
 #define KVM_NR_SHARED_MSRS 16
 
 struct kvm_shared_msrs_global {
@@ -280,7 +282,7 @@ int kvm_set_apic_base(struct kvm_vcpu *vcpu, struct msr_data *msr_info)
 }
 EXPORT_SYMBOL_GPL(kvm_set_apic_base);
 
-asmlinkage void kvm_spurious_fault(void)
+asmlinkage __visible void kvm_spurious_fault(void)
 {
 	/* Fault while not rebooting.  We want the trace. */
 	BUG();
@@ -1486,7 +1488,8 @@ static void pvclock_update_vm_gtod_copy(struct kvm *kvm)
 					&ka->master_kernel_ns,
 					&ka->master_cycle_now);
 
-	ka->use_master_clock = host_tsc_clocksource & vcpus_matched;
+	ka->use_master_clock = host_tsc_clocksource && vcpus_matched
+				&& !backwards_tsc_observed;
 
 	if (ka->use_master_clock)
 		atomic_set(&kvm_guest_has_master_clock, 1);
@@ -6945,6 +6948,7 @@ int kvm_arch_hardware_enable(void *garbage)
 	 */
 	if (backwards_tsc) {
 		u64 delta_cyc = max_tsc - local_tsc;
+		backwards_tsc_observed = true;
 		list_for_each_entry(kvm, &vm_list, vm_list) {
 			kvm_for_each_vcpu(i, vcpu, kvm) {
 				vcpu->arch.tsc_offset_adjustment += delta_cyc;
diff --git a/arch/x86/lguest/boot.c b/arch/x86/lguest/boot.c
index ad1fb5f5392..aae94132bc2 100644
--- a/arch/x86/lguest/boot.c
+++ b/arch/x86/lguest/boot.c
@@ -233,13 +233,13 @@ static void lguest_end_context_switch(struct task_struct *next)
  * flags word contains all kind of stuff, but in practice Linux only cares
  * about the interrupt flag.  Our "save_flags()" just returns that.
  */
-asmlinkage unsigned long lguest_save_fl(void)
+asmlinkage __visible unsigned long lguest_save_fl(void)
 {
 	return lguest_data.irq_enabled;
 }
 
 /* Interrupts go off... */
-asmlinkage void lguest_irq_disable(void)
+asmlinkage __visible void lguest_irq_disable(void)
 {
 	lguest_data.irq_enabled = 0;
 }
diff --git a/arch/x86/lib/msr.c b/arch/x86/lib/msr.c
index db9db446b71..43623739c7c 100644
--- a/arch/x86/lib/msr.c
+++ b/arch/x86/lib/msr.c
@@ -76,7 +76,7 @@ static inline int __flip_bit(u32 msr, u8 bit, bool set)
 	if (m1.q == m.q)
 		return 0;
 
-	err = msr_write(msr, &m);
+	err = msr_write(msr, &m1);
 	if (err)
 		return err;
 
diff --git a/arch/x86/math-emu/errors.c b/arch/x86/math-emu/errors.c
index a5449089cd9..9e6545f269e 100644
--- a/arch/x86/math-emu/errors.c
+++ b/arch/x86/math-emu/errors.c
@@ -302,7 +302,7 @@ static struct {
 	      0x242  in div_Xsig.S
  */
 
-asmlinkage void FPU_exception(int n)
+asmlinkage __visible void FPU_exception(int n)
 {
 	int i, int_type;
 
@@ -492,7 +492,7 @@ int real_2op_NaN(FPU_REG const *b, u_char tagb,
 
 /* Invalid arith operation on Valid registers */
 /* Returns < 0 if the exception is unmasked */
-asmlinkage int arith_invalid(int deststnr)
+asmlinkage __visible int arith_invalid(int deststnr)
 {
 
 	EXCEPTION(EX_Invalid);
@@ -507,7 +507,7 @@ asmlinkage int arith_invalid(int deststnr)
 }
 
 /* Divide a finite number by zero */
-asmlinkage int FPU_divide_by_zero(int deststnr, u_char sign)
+asmlinkage __visible int FPU_divide_by_zero(int deststnr, u_char sign)
 {
 	FPU_REG *dest = &st(deststnr);
 	int tag = TAG_Valid;
@@ -539,7 +539,7 @@ int set_precision_flag(int flags)
 }
 
 /* This may be called often, so keep it lean */
-asmlinkage void set_precision_flag_up(void)
+asmlinkage __visible void set_precision_flag_up(void)
 {
 	if (control_word & CW_Precision)
 		partial_status |= (SW_Precision | SW_C1);	/* The masked response */
@@ -548,7 +548,7 @@ asmlinkage void set_precision_flag_up(void)
 }
 
 /* This may be called often, so keep it lean */
-asmlinkage void set_precision_flag_down(void)
+asmlinkage __visible void set_precision_flag_down(void)
 {
 	if (control_word & CW_Precision) {	/* The masked response */
 		partial_status &= ~SW_C1;
@@ -557,7 +557,7 @@ asmlinkage void set_precision_flag_down(void)
 		EXCEPTION(EX_Precision);
 }
 
-asmlinkage int denormal_operand(void)
+asmlinkage __visible int denormal_operand(void)
 {
 	if (control_word & CW_Denormal) {	/* The masked response */
 		partial_status |= SW_Denorm_Op;
@@ -568,7 +568,7 @@ asmlinkage int denormal_operand(void)
 	}
 }
 
-asmlinkage int arith_overflow(FPU_REG *dest)
+asmlinkage __visible int arith_overflow(FPU_REG *dest)
 {
 	int tag = TAG_Valid;
 
@@ -596,7 +596,7 @@ asmlinkage int arith_overflow(FPU_REG *dest)
 
 }
 
-asmlinkage int arith_underflow(FPU_REG *dest)
+asmlinkage __visible int arith_underflow(FPU_REG *dest)
 {
 	int tag = TAG_Valid;
 
diff --git a/arch/x86/net/bpf_jit_comp.c b/arch/x86/net/bpf_jit_comp.c
index dc017735bb9..6d5663a599a 100644
--- a/arch/x86/net/bpf_jit_comp.c
+++ b/arch/x86/net/bpf_jit_comp.c
@@ -171,7 +171,7 @@ static struct bpf_binary_header *bpf_alloc_binary(unsigned int proglen,
 	memset(header, 0xcc, sz); /* fill whole space with int3 instructions */
 
 	header->pages = sz / PAGE_SIZE;
-	hole = sz - (proglen + sizeof(*header));
+	hole = min(sz - (proglen + sizeof(*header)), PAGE_SIZE - sizeof(*header));
 
 	/* insert a random number of int3 instructions before BPF code */
 	*image_ptr = &header->image[prandom_u32() % hole];
diff --git a/arch/x86/pci/acpi.c b/arch/x86/pci/acpi.c
index 01edac6c5e1..5075371ab59 100644
--- a/arch/x86/pci/acpi.c
+++ b/arch/x86/pci/acpi.c
@@ -489,8 +489,12 @@ struct pci_bus *pci_acpi_scan_root(struct acpi_pci_root *root)
 	}
 
 	node = acpi_get_node(device->handle);
-	if (node == NUMA_NO_NODE)
+	if (node == NUMA_NO_NODE) {
 		node = x86_pci_root_bus_node(busnum);
+		if (node != 0 && node != NUMA_NO_NODE)
+			dev_info(&device->dev, FW_BUG "no _PXM; falling back to node %d from hardware (may be inconsistent with ACPI node numbers)\n",
+				node);
+	}
 
 	if (node != NUMA_NO_NODE && !node_online(node))
 		node = NUMA_NO_NODE;
diff --git a/arch/x86/pci/amd_bus.c b/arch/x86/pci/amd_bus.c
index e88f4c53d7f..c20d2cc7ef6 100644
--- a/arch/x86/pci/amd_bus.c
+++ b/arch/x86/pci/amd_bus.c
@@ -11,27 +11,33 @@
 
 #include "bus_numa.h"
 
-/*
- * This discovers the pcibus <-> node mapping on AMD K8.
- * also get peer root bus resource for io,mmio
- */
+#define AMD_NB_F0_NODE_ID			0x60
+#define AMD_NB_F0_UNIT_ID			0x64
+#define AMD_NB_F1_CONFIG_MAP_REG		0xe0
+
+#define RANGE_NUM				16
+#define AMD_NB_F1_CONFIG_MAP_RANGES		4
 
-struct pci_hostbridge_probe {
+struct amd_hostbridge {
 	u32 bus;
 	u32 slot;
-	u32 vendor;
 	u32 device;
 };
 
-static struct pci_hostbridge_probe pci_probes[] __initdata = {
-	{ 0, 0x18, PCI_VENDOR_ID_AMD, 0x1100 },
-	{ 0, 0x18, PCI_VENDOR_ID_AMD, 0x1200 },
-	{ 0xff, 0, PCI_VENDOR_ID_AMD, 0x1200 },
-	{ 0, 0x18, PCI_VENDOR_ID_AMD, 0x1300 },
+/*
+ * IMPORTANT NOTE:
+ * hb_probes[] and early_root_info_init() is in maintenance mode.
+ * It only supports K8, Fam10h, Fam11h, and Fam15h_00h-0fh .
+ * Future processor will rely on information in ACPI.
+ */
+static struct amd_hostbridge hb_probes[] __initdata = {
+	{ 0, 0x18, 0x1100 }, /* K8 */
+	{ 0, 0x18, 0x1200 }, /* Family10h */
+	{ 0xff, 0, 0x1200 }, /* Family10h */
+	{ 0, 0x18, 0x1300 }, /* Family11h */
+	{ 0, 0x18, 0x1600 }, /* Family15h */
 };
 
-#define RANGE_NUM 16
-
 static struct pci_root_info __init *find_pci_root_info(int node, int link)
 {
 	struct pci_root_info *info;
@@ -45,12 +51,12 @@ static struct pci_root_info __init *find_pci_root_info(int node, int link)
 }
 
 /**
- * early_fill_mp_bus_to_node()
+ * early_root_info_init()
  * called before pcibios_scan_root and pci_scan_bus
- * fills the mp_bus_to_cpumask array based according to the LDT Bus Number
- * Registers found in the K8 northbridge
+ * fills the mp_bus_to_cpumask array based according
+ * to the LDT Bus Number Registers found in the northbridge.
  */
-static int __init early_fill_mp_bus_info(void)
+static int __init early_root_info_init(void)
 {
 	int i;
 	unsigned bus;
@@ -75,19 +81,21 @@ static int __init early_fill_mp_bus_info(void)
 		return -1;
 
 	found = false;
-	for (i = 0; i < ARRAY_SIZE(pci_probes); i++) {
+	for (i = 0; i < ARRAY_SIZE(hb_probes); i++) {
 		u32 id;
 		u16 device;
 		u16 vendor;
 
-		bus = pci_probes[i].bus;
-		slot = pci_probes[i].slot;
+		bus = hb_probes[i].bus;
+		slot = hb_probes[i].slot;
 		id = read_pci_config(bus, slot, 0, PCI_VENDOR_ID);
-
 		vendor = id & 0xffff;
 		device = (id>>16) & 0xffff;
-		if (pci_probes[i].vendor == vendor &&
-		    pci_probes[i].device == device) {
+
+		if (vendor != PCI_VENDOR_ID_AMD)
+			continue;
+
+		if (hb_probes[i].device == device) {
 			found = true;
 			break;
 		}
@@ -96,10 +104,16 @@ static int __init early_fill_mp_bus_info(void)
 	if (!found)
 		return 0;
 
-	for (i = 0; i < 4; i++) {
+	/*
+	 * We should learn topology and routing information from _PXM and
+	 * _CRS methods in the ACPI namespace.  We extract node numbers
+	 * here to work around BIOSes that don't supply _PXM.
+	 */
+	for (i = 0; i < AMD_NB_F1_CONFIG_MAP_RANGES; i++) {
 		int min_bus;
 		int max_bus;
-		reg = read_pci_config(bus, slot, 1, 0xe0 + (i << 2));
+		reg = read_pci_config(bus, slot, 1,
+				AMD_NB_F1_CONFIG_MAP_REG + (i << 2));
 
 		/* Check if that register is enabled for bus range */
 		if ((reg & 7) != 3)
@@ -113,10 +127,21 @@ static int __init early_fill_mp_bus_info(void)
 		info = alloc_pci_root_info(min_bus, max_bus, node, link);
 	}
 
+	/*
+	 * The following code extracts routing information for use on old
+	 * systems where Linux doesn't automatically use host bridge _CRS
+	 * methods (or when the user specifies "pci=nocrs").
+	 *
+	 * We only do this through Fam11h, because _CRS should be enough on
+	 * newer systems.
+	 */
+	if (boot_cpu_data.x86 > 0x11)
+		return 0;
+
 	/* get the default node and link for left over res */
-	reg = read_pci_config(bus, slot, 0, 0x60);
+	reg = read_pci_config(bus, slot, 0, AMD_NB_F0_NODE_ID);
 	def_node = (reg >> 8) & 0x07;
-	reg = read_pci_config(bus, slot, 0, 0x64);
+	reg = read_pci_config(bus, slot, 0, AMD_NB_F0_UNIT_ID);
 	def_link = (reg >> 8) & 0x03;
 
 	memset(range, 0, sizeof(range));
@@ -363,7 +388,7 @@ static int __init pci_io_ecs_init(void)
 	int cpu;
 
 	/* assume all cpus from fam10h have IO ECS */
-        if (boot_cpu_data.x86 < 0x10)
+	if (boot_cpu_data.x86 < 0x10)
 		return 0;
 
 	/* Try the PCI method first. */
@@ -387,7 +412,7 @@ static int __init amd_postcore_init(void)
 	if (boot_cpu_data.x86_vendor != X86_VENDOR_AMD)
 		return 0;
 
-	early_fill_mp_bus_info();
+	early_root_info_init();
 	pci_io_ecs_init();
 
 	return 0;
diff --git a/arch/x86/pci/broadcom_bus.c b/arch/x86/pci/broadcom_bus.c
index 614392ced7d..bb461cfd01a 100644
--- a/arch/x86/pci/broadcom_bus.c
+++ b/arch/x86/pci/broadcom_bus.c
@@ -60,8 +60,8 @@ static void __init cnb20le_res(u8 bus, u8 slot, u8 func)
 	word1 = read_pci_config_16(bus, slot, func, 0xc4);
 	word2 = read_pci_config_16(bus, slot, func, 0xc6);
 	if (word1 != word2) {
-		res.start = (word1 << 16) | 0x0000;
-		res.end   = (word2 << 16) | 0xffff;
+		res.start = ((resource_size_t) word1 << 16) | 0x0000;
+		res.end   = ((resource_size_t) word2 << 16) | 0xffff;
 		res.flags = IORESOURCE_MEM | IORESOURCE_PREFETCH;
 		update_res(info, res.start, res.end, res.flags, 0);
 	}
diff --git a/arch/x86/pci/fixup.c b/arch/x86/pci/fixup.c
index 94ae9ae9574..b5e60268d93 100644
--- a/arch/x86/pci/fixup.c
+++ b/arch/x86/pci/fixup.c
@@ -6,6 +6,7 @@
 #include <linux/dmi.h>
 #include <linux/pci.h>
 #include <linux/vgaarb.h>
+#include <asm/hpet.h>
 #include <asm/pci_x86.h>
 
 static void pci_fixup_i450nx(struct pci_dev *d)
@@ -337,9 +338,7 @@ static void pci_fixup_video(struct pci_dev *pdev)
 		 * type BRIDGE, or CARDBUS. Host to PCI controllers use
 		 * PCI header type NORMAL.
 		 */
-		if (bridge
-		    && ((bridge->hdr_type == PCI_HEADER_TYPE_BRIDGE)
-		       || (bridge->hdr_type == PCI_HEADER_TYPE_CARDBUS))) {
+		if (bridge && (pci_is_bridge(bridge))) {
 			pci_read_config_word(bridge, PCI_BRIDGE_CONTROL,
 						&config);
 			if (!(config & PCI_BRIDGE_CTL_VGA))
@@ -526,6 +525,19 @@ static void sb600_disable_hpet_bar(struct pci_dev *dev)
 }
 DECLARE_PCI_FIXUP_EARLY(PCI_VENDOR_ID_ATI, 0x4385, sb600_disable_hpet_bar);
 
+#ifdef CONFIG_HPET_TIMER
+static void sb600_hpet_quirk(struct pci_dev *dev)
+{
+	struct resource *r = &dev->resource[1];
+
+	if (r->flags & IORESOURCE_MEM && r->start == hpet_address) {
+		r->flags |= IORESOURCE_PCI_FIXED;
+		dev_info(&dev->dev, "reg 0x14 contains HPET; making it immovable\n");
+	}
+}
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_ATI, 0x4385, sb600_hpet_quirk);
+#endif
+
 /*
  * Twinhead H12Y needs us to block out a region otherwise we map devices
  * there and any access kills the box.
diff --git a/arch/x86/pci/i386.c b/arch/x86/pci/i386.c
index db6b1ab4325..a19ed92e74e 100644
--- a/arch/x86/pci/i386.c
+++ b/arch/x86/pci/i386.c
@@ -271,11 +271,16 @@ static void pcibios_allocate_dev_resources(struct pci_dev *dev, int pass)
 					"BAR %d: reserving %pr (d=%d, p=%d)\n",
 					idx, r, disabled, pass);
 				if (pci_claim_resource(dev, idx) < 0) {
-					/* We'll assign a new address later */
-					pcibios_save_fw_addr(dev,
-							idx, r->start);
-					r->end -= r->start;
-					r->start = 0;
+					if (r->flags & IORESOURCE_PCI_FIXED) {
+						dev_info(&dev->dev, "BAR %d %pR is immovable\n",
+							 idx, r);
+					} else {
+						/* We'll assign a new address later */
+						pcibios_save_fw_addr(dev,
+								idx, r->start);
+						r->end -= r->start;
+						r->start = 0;
+					}
 				}
 			}
 		}
@@ -356,6 +361,12 @@ static int __init pcibios_assign_resources(void)
 	return 0;
 }
 
+/**
+ * called in fs_initcall (one below subsys_initcall),
+ * give a chance for motherboard reserve resources
+ */
+fs_initcall(pcibios_assign_resources);
+
 void pcibios_resource_survey_bus(struct pci_bus *bus)
 {
 	dev_printk(KERN_DEBUG, &bus->dev, "Allocating resources\n");
@@ -392,12 +403,6 @@ void __init pcibios_resource_survey(void)
 	ioapic_insert_resources();
 }
 
-/**
- * called in fs_initcall (one below subsys_initcall),
- * give a chance for motherboard reserve resources
- */
-fs_initcall(pcibios_assign_resources);
-
 static const struct vm_operations_struct pci_mmap_ops = {
 	.access = generic_access_phys,
 };
diff --git a/arch/x86/platform/efi/early_printk.c b/arch/x86/platform/efi/early_printk.c
index 81b506d5bef..52414211729 100644
--- a/arch/x86/platform/efi/early_printk.c
+++ b/arch/x86/platform/efi/early_printk.c
@@ -14,48 +14,92 @@
 
 static const struct font_desc *font;
 static u32 efi_x, efi_y;
+static void *efi_fb;
+static bool early_efi_keep;
 
-static __init void early_efi_clear_scanline(unsigned int y)
+/*
+ * efi earlyprintk need use early_ioremap to map the framebuffer.
+ * But early_ioremap is not usable for earlyprintk=efi,keep, ioremap should
+ * be used instead. ioremap will be available after paging_init() which is
+ * earlier than initcall callbacks. Thus adding this early initcall function
+ * early_efi_map_fb to map the whole efi framebuffer.
+ */
+static __init int early_efi_map_fb(void)
 {
-	unsigned long base, *dst;
-	u16 len;
+	unsigned long base, size;
+
+	if (!early_efi_keep)
+		return 0;
 
 	base = boot_params.screen_info.lfb_base;
-	len = boot_params.screen_info.lfb_linelength;
+	size = boot_params.screen_info.lfb_size;
+	efi_fb = ioremap(base, size);
+
+	return efi_fb ? 0 : -ENOMEM;
+}
+early_initcall(early_efi_map_fb);
+
+/*
+ * early_efi_map maps efi framebuffer region [start, start + len -1]
+ * In case earlyprintk=efi,keep we have the whole framebuffer mapped already
+ * so just return the offset efi_fb + start.
+ */
+static __init_refok void *early_efi_map(unsigned long start, unsigned long len)
+{
+	unsigned long base;
+
+	base = boot_params.screen_info.lfb_base;
+
+	if (efi_fb)
+		return (efi_fb + start);
+	else
+		return early_ioremap(base + start, len);
+}
 
-	dst = early_ioremap(base + y*len, len);
+static __init_refok void early_efi_unmap(void *addr, unsigned long len)
+{
+	if (!efi_fb)
+		early_iounmap(addr, len);
+}
+
+static void early_efi_clear_scanline(unsigned int y)
+{
+	unsigned long *dst;
+	u16 len;
+
+	len = boot_params.screen_info.lfb_linelength;
+	dst = early_efi_map(y*len, len);
 	if (!dst)
 		return;
 
 	memset(dst, 0, len);
-	early_iounmap(dst, len);
+	early_efi_unmap(dst, len);
 }
 
-static __init void early_efi_scroll_up(void)
+static void early_efi_scroll_up(void)
 {
-	unsigned long base, *dst, *src;
+	unsigned long *dst, *src;
 	u16 len;
 	u32 i, height;
 
-	base = boot_params.screen_info.lfb_base;
 	len = boot_params.screen_info.lfb_linelength;
 	height = boot_params.screen_info.lfb_height;
 
 	for (i = 0; i < height - font->height; i++) {
-		dst = early_ioremap(base + i*len, len);
+		dst = early_efi_map(i*len, len);
 		if (!dst)
 			return;
 
-		src = early_ioremap(base + (i + font->height) * len, len);
+		src = early_efi_map((i + font->height) * len, len);
 		if (!src) {
-			early_iounmap(dst, len);
+			early_efi_unmap(dst, len);
 			return;
 		}
 
 		memmove(dst, src, len);
 
-		early_iounmap(src, len);
-		early_iounmap(dst, len);
+		early_efi_unmap(src, len);
+		early_efi_unmap(dst, len);
 	}
 }
 
@@ -79,16 +123,14 @@ static void early_efi_write_char(u32 *dst, unsigned char c, unsigned int h)
 	}
 }
 
-static __init void
+static void
 early_efi_write(struct console *con, const char *str, unsigned int num)
 {
 	struct screen_info *si;
-	unsigned long base;
 	unsigned int len;
 	const char *s;
 	void *dst;
 
-	base = boot_params.screen_info.lfb_base;
 	si = &boot_params.screen_info;
 	len = si->lfb_linelength;
 
@@ -109,7 +151,7 @@ early_efi_write(struct console *con, const char *str, unsigned int num)
 		for (h = 0; h < font->height; h++) {
 			unsigned int n, x;
 
-			dst = early_ioremap(base + (efi_y + h) * len, len);
+			dst = early_efi_map((efi_y + h) * len, len);
 			if (!dst)
 				return;
 
@@ -123,7 +165,7 @@ early_efi_write(struct console *con, const char *str, unsigned int num)
 				s++;
 			}
 
-			early_iounmap(dst, len);
+			early_efi_unmap(dst, len);
 		}
 
 		num -= count;
@@ -179,6 +221,9 @@ static __init int early_efi_setup(struct console *con, char *options)
 	for (i = 0; i < (yres - efi_y) / font->height; i++)
 		early_efi_scroll_up();
 
+	/* early_console_register will unset CON_BOOT in case ,keep */
+	if (!(con->flags & CON_BOOT))
+		early_efi_keep = true;
 	return 0;
 }
 
diff --git a/arch/x86/platform/olpc/olpc-xo1-pm.c b/arch/x86/platform/olpc/olpc-xo1-pm.c
index ff0174dda81..a9acde72d4e 100644
--- a/arch/x86/platform/olpc/olpc-xo1-pm.c
+++ b/arch/x86/platform/olpc/olpc-xo1-pm.c
@@ -75,7 +75,7 @@ static int xo1_power_state_enter(suspend_state_t pm_state)
 	return 0;
 }
 
-asmlinkage int xo1_do_sleep(u8 sleep_state)
+asmlinkage __visible int xo1_do_sleep(u8 sleep_state)
 {
 	void *pgd_addr = __va(read_cr3());
 
diff --git a/arch/x86/power/hibernate_64.c b/arch/x86/power/hibernate_64.c
index 304fca20d96..35e2bb6c0f3 100644
--- a/arch/x86/power/hibernate_64.c
+++ b/arch/x86/power/hibernate_64.c
@@ -23,7 +23,7 @@
 extern __visible const void __nosave_begin, __nosave_end;
 
 /* Defined in hibernate_asm_64.S */
-extern asmlinkage int restore_image(void);
+extern asmlinkage __visible int restore_image(void);
 
 /*
  * Address to jump to in the last phase of restore in order to get to the image
diff --git a/arch/x86/vdso/vdso-layout.lds.S b/arch/x86/vdso/vdso-layout.lds.S
index 2e263f367b1..9df017ab228 100644
--- a/arch/x86/vdso/vdso-layout.lds.S
+++ b/arch/x86/vdso/vdso-layout.lds.S
@@ -9,12 +9,9 @@ SECTIONS
 #ifdef BUILD_VDSO32
 #include <asm/vdso32.h>
 
-	.hpet_sect : {
-		hpet_page = . - VDSO_OFFSET(VDSO_HPET_PAGE);
-	} :text :hpet_sect
+	hpet_page = . - VDSO_OFFSET(VDSO_HPET_PAGE);
 
-	.vvar_sect : {
-		vvar = . - VDSO_OFFSET(VDSO_VVAR_PAGE);
+	vvar = . - VDSO_OFFSET(VDSO_VVAR_PAGE);
 
 	/* Place all vvars at the offsets in asm/vvar.h. */
 #define EMIT_VVAR(name, offset) vvar_ ## name = vvar + offset;
@@ -22,7 +19,6 @@ SECTIONS
 #include <asm/vvar.h>
 #undef __VVAR_KERNEL_LDS
 #undef EMIT_VVAR
-	} :text :vvar_sect
 #endif
 	. = SIZEOF_HEADERS;
 
@@ -61,7 +57,12 @@ SECTIONS
 	 */
 	. = ALIGN(0x100);
 
-	.text		: { *(.text*) }			:text	=0x90909090
+	.text		: { *(.text*) }			:text	=0x90909090,
+
+	/*
+	 * The comma above works around a bug in gold:
+	 * https://sourceware.org/bugzilla/show_bug.cgi?id=16804
+	 */
 
 	/DISCARD/ : {
 		*(.discard)
@@ -84,8 +85,4 @@ PHDRS
 	dynamic		PT_DYNAMIC	FLAGS(4);		/* PF_R */
 	note		PT_NOTE		FLAGS(4);		/* PF_R */
 	eh_frame_hdr	PT_GNU_EH_FRAME;
-#ifdef BUILD_VDSO32
-	vvar_sect	PT_NULL		FLAGS(4);		/* PF_R */
-	hpet_sect	PT_NULL		FLAGS(4);		/* PF_R */
-#endif
 }
diff --git a/arch/x86/vdso/vdso32-setup.c b/arch/x86/vdso/vdso32-setup.c
index 00348980a3a..e1f220e3ca6 100644
--- a/arch/x86/vdso/vdso32-setup.c
+++ b/arch/x86/vdso/vdso32-setup.c
@@ -39,6 +39,7 @@
 #ifdef CONFIG_X86_64
 #define vdso_enabled			sysctl_vsyscall32
 #define arch_setup_additional_pages	syscall32_setup_pages
+extern int sysctl_ldt16;
 #endif
 
 /*
@@ -249,6 +250,13 @@ static struct ctl_table abi_table2[] = {
 		.mode		= 0644,
 		.proc_handler	= proc_dointvec
 	},
+	{
+		.procname	= "ldt16",
+		.data		= &sysctl_ldt16,
+		.maxlen		= sizeof(int),
+		.mode		= 0644,
+		.proc_handler	= proc_dointvec
+	},
 	{}
 };
 
diff --git a/arch/x86/xen/enlighten.c b/arch/x86/xen/enlighten.c
index 201d09a7c46..f17b29210ac 100644
--- a/arch/x86/xen/enlighten.c
+++ b/arch/x86/xen/enlighten.c
@@ -1339,6 +1339,7 @@ xen_panic_event(struct notifier_block *this, unsigned long event, void *ptr)
 
 static struct notifier_block xen_panic_block = {
 	.notifier_call= xen_panic_event,
+	.priority = INT_MIN
 };
 
 int xen_panic_handler_init(void)
@@ -1515,7 +1516,7 @@ static void __init xen_pvh_early_guest_init(void)
 }
 
 /* First C function to be called on Xen boot */
-asmlinkage void __init xen_start_kernel(void)
+asmlinkage __visible void __init xen_start_kernel(void)
 {
 	struct physdev_set_iopl set_iopl;
 	int rc;
diff --git a/arch/x86/xen/irq.c b/arch/x86/xen/irq.c
index 08f763de26f..a1207cb6472 100644
--- a/arch/x86/xen/irq.c
+++ b/arch/x86/xen/irq.c
@@ -23,7 +23,7 @@ void xen_force_evtchn_callback(void)
 	(void)HYPERVISOR_xen_version(0, NULL);
 }
 
-asmlinkage unsigned long xen_save_fl(void)
+asmlinkage __visible unsigned long xen_save_fl(void)
 {
 	struct vcpu_info *vcpu;
 	unsigned long flags;
@@ -63,7 +63,7 @@ __visible void xen_restore_fl(unsigned long flags)
 }
 PV_CALLEE_SAVE_REGS_THUNK(xen_restore_fl);
 
-asmlinkage void xen_irq_disable(void)
+asmlinkage __visible void xen_irq_disable(void)
 {
 	/* There's a one instruction preempt window here.  We need to
 	   make sure we're don't switch CPUs between getting the vcpu
@@ -74,7 +74,7 @@ asmlinkage void xen_irq_disable(void)
 }
 PV_CALLEE_SAVE_REGS_THUNK(xen_irq_disable);
 
-asmlinkage void xen_irq_enable(void)
+asmlinkage __visible void xen_irq_enable(void)
 {
 	struct vcpu_info *vcpu;
 
diff --git a/arch/x86/xen/mmu.c b/arch/x86/xen/mmu.c
index 86e02eabb64..6f6e15d2846 100644
--- a/arch/x86/xen/mmu.c
+++ b/arch/x86/xen/mmu.c
@@ -2510,6 +2510,95 @@ void __init xen_hvm_init_mmu_ops(void)
 }
 #endif
 
+#ifdef CONFIG_XEN_PVH
+/*
+ * Map foreign gfn (fgfn), to local pfn (lpfn). This for the user
+ * space creating new guest on pvh dom0 and needing to map domU pages.
+ */
+static int xlate_add_to_p2m(unsigned long lpfn, unsigned long fgfn,
+			    unsigned int domid)
+{
+	int rc, err = 0;
+	xen_pfn_t gpfn = lpfn;
+	xen_ulong_t idx = fgfn;
+
+	struct xen_add_to_physmap_range xatp = {
+		.domid = DOMID_SELF,
+		.foreign_domid = domid,
+		.size = 1,
+		.space = XENMAPSPACE_gmfn_foreign,
+	};
+	set_xen_guest_handle(xatp.idxs, &idx);
+	set_xen_guest_handle(xatp.gpfns, &gpfn);
+	set_xen_guest_handle(xatp.errs, &err);
+
+	rc = HYPERVISOR_memory_op(XENMEM_add_to_physmap_range, &xatp);
+	if (rc < 0)
+		return rc;
+	return err;
+}
+
+static int xlate_remove_from_p2m(unsigned long spfn, int count)
+{
+	struct xen_remove_from_physmap xrp;
+	int i, rc;
+
+	for (i = 0; i < count; i++) {
+		xrp.domid = DOMID_SELF;
+		xrp.gpfn = spfn+i;
+		rc = HYPERVISOR_memory_op(XENMEM_remove_from_physmap, &xrp);
+		if (rc)
+			break;
+	}
+	return rc;
+}
+
+struct xlate_remap_data {
+	unsigned long fgfn; /* foreign domain's gfn */
+	pgprot_t prot;
+	domid_t  domid;
+	int index;
+	struct page **pages;
+};
+
+static int xlate_map_pte_fn(pte_t *ptep, pgtable_t token, unsigned long addr,
+			    void *data)
+{
+	int rc;
+	struct xlate_remap_data *remap = data;
+	unsigned long pfn = page_to_pfn(remap->pages[remap->index++]);
+	pte_t pteval = pte_mkspecial(pfn_pte(pfn, remap->prot));
+
+	rc = xlate_add_to_p2m(pfn, remap->fgfn, remap->domid);
+	if (rc)
+		return rc;
+	native_set_pte(ptep, pteval);
+
+	return 0;
+}
+
+static int xlate_remap_gfn_range(struct vm_area_struct *vma,
+				 unsigned long addr, unsigned long mfn,
+				 int nr, pgprot_t prot, unsigned domid,
+				 struct page **pages)
+{
+	int err;
+	struct xlate_remap_data pvhdata;
+
+	BUG_ON(!pages);
+
+	pvhdata.fgfn = mfn;
+	pvhdata.prot = prot;
+	pvhdata.domid = domid;
+	pvhdata.index = 0;
+	pvhdata.pages = pages;
+	err = apply_to_page_range(vma->vm_mm, addr, nr << PAGE_SHIFT,
+				  xlate_map_pte_fn, &pvhdata);
+	flush_tlb_all();
+	return err;
+}
+#endif
+
 #define REMAP_BATCH_SIZE 16
 
 struct remap_data {
@@ -2522,7 +2611,7 @@ static int remap_area_mfn_pte_fn(pte_t *ptep, pgtable_t token,
 				 unsigned long addr, void *data)
 {
 	struct remap_data *rmd = data;
-	pte_t pte = pte_mkspecial(pfn_pte(rmd->mfn++, rmd->prot));
+	pte_t pte = pte_mkspecial(mfn_pte(rmd->mfn++, rmd->prot));
 
 	rmd->mmu_update->ptr = virt_to_machine(ptep).maddr;
 	rmd->mmu_update->val = pte_val_ma(pte);
@@ -2544,13 +2633,18 @@ int xen_remap_domain_mfn_range(struct vm_area_struct *vma,
 	unsigned long range;
 	int err = 0;
 
-	if (xen_feature(XENFEAT_auto_translated_physmap))
-		return -EINVAL;
-
-	prot = __pgprot(pgprot_val(prot) | _PAGE_IOMAP);
-
 	BUG_ON(!((vma->vm_flags & (VM_PFNMAP | VM_IO)) == (VM_PFNMAP | VM_IO)));
 
+	if (xen_feature(XENFEAT_auto_translated_physmap)) {
+#ifdef CONFIG_XEN_PVH
+		/* We need to update the local page tables and the xen HAP */
+		return xlate_remap_gfn_range(vma, addr, mfn, nr, prot,
+					     domid, pages);
+#else
+		return -EINVAL;
+#endif
+        }
+
 	rmd.mfn = mfn;
 	rmd.prot = prot;
 
@@ -2588,6 +2682,25 @@ int xen_unmap_domain_mfn_range(struct vm_area_struct *vma,
 	if (!pages || !xen_feature(XENFEAT_auto_translated_physmap))
 		return 0;
 
+#ifdef CONFIG_XEN_PVH
+	while (numpgs--) {
+		/*
+		 * The mmu has already cleaned up the process mmu
+		 * resources at this point (lookup_address will return
+		 * NULL).
+		 */
+		unsigned long pfn = page_to_pfn(pages[numpgs]);
+
+		xlate_remove_from_p2m(pfn, 1);
+	}
+	/*
+	 * We don't need to flush tlbs because as part of
+	 * xlate_remove_from_p2m, the hypervisor will do tlb flushes
+	 * after removing the p2m entries from the EPT/NPT
+	 */
+	return 0;
+#else
 	return -EINVAL;
+#endif
 }
 EXPORT_SYMBOL_GPL(xen_unmap_domain_mfn_range);
diff --git a/arch/x86/xen/p2m.c b/arch/x86/xen/p2m.c
index 85e5d78c987..9bb3d82ffec 100644
--- a/arch/x86/xen/p2m.c
+++ b/arch/x86/xen/p2m.c
@@ -36,7 +36,7 @@
  *  pfn_to_mfn(0xc0000)=0xc0000
  *
  * The benefit of this is, that we can assume for non-RAM regions (think
- * PCI BARs, or ACPI spaces), we can create mappings easily b/c we
+ * PCI BARs, or ACPI spaces), we can create mappings easily because we
  * get the PFN value to match the MFN.
  *
  * For this to work efficiently we have one new page p2m_identity and
@@ -60,7 +60,7 @@
  * There is also a digram of the P2M at the end that can help.
  * Imagine your E820 looking as so:
  *
- *                    1GB                                           2GB
+ *                    1GB                                           2GB    4GB
  * /-------------------+---------\/----\         /----------\    /---+-----\
  * | System RAM        | Sys RAM ||ACPI|         | reserved |    | Sys RAM |
  * \-------------------+---------/\----/         \----------/    \---+-----/
@@ -77,9 +77,8 @@
  * of the PFN and the end PFN (263424 and 512256 respectively). The first step
  * is to reserve_brk a top leaf page if the p2m[1] is missing. The top leaf page
  * covers 512^2 of page estate (1GB) and in case the start or end PFN is not
- * aligned on 512^2*PAGE_SIZE (1GB) we loop on aligned 1GB PFNs from start pfn
- * to end pfn.  We reserve_brk top leaf pages if they are missing (means they
- * point to p2m_mid_missing).
+ * aligned on 512^2*PAGE_SIZE (1GB) we reserve_brk new middle and leaf pages as
+ * required to split any existing p2m_mid_missing middle pages.
  *
  * With the E820 example above, 263424 is not 1GB aligned so we allocate a
  * reserve_brk page which will cover the PFNs estate from 0x40000 to 0x80000.
@@ -88,7 +87,7 @@
  * Next stage is to determine if we need to do a more granular boundary check
  * on the 4MB (or 2MB depending on architecture) off the start and end pfn's.
  * We check if the start pfn and end pfn violate that boundary check, and if
- * so reserve_brk a middle (p2m[x][y]) leaf page. This way we have a much finer
+ * so reserve_brk a (p2m[x][y]) leaf page. This way we have a much finer
  * granularity of setting which PFNs are missing and which ones are identity.
  * In our example 263424 and 512256 both fail the check so we reserve_brk two
  * pages. Populate them with INVALID_P2M_ENTRY (so they both have "missing"
@@ -102,9 +101,10 @@
  *
  * The next step is to walk from the start pfn to the end pfn setting
  * the IDENTITY_FRAME_BIT on each PFN. This is done in set_phys_range_identity.
- * If we find that the middle leaf is pointing to p2m_missing we can swap it
- * over to p2m_identity - this way covering 4MB (or 2MB) PFN space.  At this
- * point we do not need to worry about boundary aligment (so no need to
+ * If we find that the middle entry is pointing to p2m_missing we can swap it
+ * over to p2m_identity - this way covering 4MB (or 2MB) PFN space (and
+ * similarly swapping p2m_mid_missing for p2m_mid_identity for larger regions).
+ * At this point we do not need to worry about boundary aligment (so no need to
  * reserve_brk a middle page, figure out which PFNs are "missing" and which
  * ones are identity), as that has been done earlier.  If we find that the
  * middle leaf is not occupied by p2m_identity or p2m_missing, we dereference
@@ -118,6 +118,9 @@
  * considered missing). In our case, p2m[1][2][0->255] and p2m[1][488][257->511]
  * contain the INVALID_P2M_ENTRY value and are considered "missing."
  *
+ * Finally, the region beyond the end of of the E820 (4 GB in this example)
+ * is set to be identity (in case there are MMIO regions placed here).
+ *
  * This is what the p2m ends up looking (for the E820 above) with this
  * fabulous drawing:
  *
@@ -129,21 +132,27 @@
  *  |-----|    \                      | [p2m_identity]+\\    | ....            |
  *  |  2  |--\  \-------------------->|  ...          | \\   \----------------/
  *  |-----|   \                       \---------------/  \\
- *  |  3  |\   \                                          \\  p2m_identity
- *  |-----| \   \-------------------->/---------------\   /-----------------\
- *  | ..  +->+                        | [p2m_identity]+-->| ~0, ~0, ~0, ... |
- *  \-----/ /                         | [p2m_identity]+-->| ..., ~0         |
- *         / /---------------\        | ....          |   \-----------------/
- *        /  | IDENTITY[@0]  |      /-+-[x], ~0, ~0.. |
- *       /   | IDENTITY[@256]|<----/  \---------------/
- *      /    | ~0, ~0, ....  |
- *     |     \---------------/
- *     |
- *   p2m_mid_missing           p2m_missing
- * /-----------------\     /------------\
- * | [p2m_missing]   +---->| ~0, ~0, ~0 |
- * | [p2m_missing]   +---->| ..., ~0    |
- * \-----------------/     \------------/
+ *  |  3  |-\  \                                          \\  p2m_identity [1]
+ *  |-----|  \  \-------------------->/---------------\   /-----------------\
+ *  | ..  |\  |                       | [p2m_identity]+-->| ~0, ~0, ~0, ... |
+ *  \-----/ | |                       | [p2m_identity]+-->| ..., ~0         |
+ *          | |                       | ....          |   \-----------------/
+ *          | |                       +-[x], ~0, ~0.. +\
+ *          | |                       \---------------/ \
+ *          | |                                          \-> /---------------\
+ *          | V  p2m_mid_missing       p2m_missing           | IDENTITY[@0]  |
+ *          | /-----------------\     /------------\         | IDENTITY[@256]|
+ *          | | [p2m_missing]   +---->| ~0, ~0, ...|         | ~0, ~0, ....  |
+ *          | | [p2m_missing]   +---->| ..., ~0    |         \---------------/
+ *          | | ...             |     \------------/
+ *          | \-----------------/
+ *          |
+ *          |     p2m_mid_identity
+ *          |   /-----------------\
+ *          \-->| [p2m_identity]  +---->[1]
+ *              | [p2m_identity]  +---->[1]
+ *              | ...             |
+ *              \-----------------/
  *
  * where ~0 is INVALID_P2M_ENTRY. IDENTITY is (PFN | IDENTITY_BIT)
  */
@@ -187,13 +196,15 @@ static RESERVE_BRK_ARRAY(unsigned long, p2m_top_mfn, P2M_TOP_PER_PAGE);
 static RESERVE_BRK_ARRAY(unsigned long *, p2m_top_mfn_p, P2M_TOP_PER_PAGE);
 
 static RESERVE_BRK_ARRAY(unsigned long, p2m_identity, P2M_PER_PAGE);
+static RESERVE_BRK_ARRAY(unsigned long *, p2m_mid_identity, P2M_MID_PER_PAGE);
+static RESERVE_BRK_ARRAY(unsigned long, p2m_mid_identity_mfn, P2M_MID_PER_PAGE);
 
 RESERVE_BRK(p2m_mid, PAGE_SIZE * (MAX_DOMAIN_PAGES / (P2M_PER_PAGE * P2M_MID_PER_PAGE)));
 RESERVE_BRK(p2m_mid_mfn, PAGE_SIZE * (MAX_DOMAIN_PAGES / (P2M_PER_PAGE * P2M_MID_PER_PAGE)));
 
 /* We might hit two boundary violations at the start and end, at max each
  * boundary violation will require three middle nodes. */
-RESERVE_BRK(p2m_mid_identity, PAGE_SIZE * 2 * 3);
+RESERVE_BRK(p2m_mid_extra, PAGE_SIZE * 2 * 3);
 
 /* When we populate back during bootup, the amount of pages can vary. The
  * max we have is seen is 395979, but that does not mean it can't be more.
@@ -242,20 +253,20 @@ static void p2m_top_mfn_p_init(unsigned long **top)
 		top[i] = p2m_mid_missing_mfn;
 }
 
-static void p2m_mid_init(unsigned long **mid)
+static void p2m_mid_init(unsigned long **mid, unsigned long *leaf)
 {
 	unsigned i;
 
 	for (i = 0; i < P2M_MID_PER_PAGE; i++)
-		mid[i] = p2m_missing;
+		mid[i] = leaf;
 }
 
-static void p2m_mid_mfn_init(unsigned long *mid)
+static void p2m_mid_mfn_init(unsigned long *mid, unsigned long *leaf)
 {
 	unsigned i;
 
 	for (i = 0; i < P2M_MID_PER_PAGE; i++)
-		mid[i] = virt_to_mfn(p2m_missing);
+		mid[i] = virt_to_mfn(leaf);
 }
 
 static void p2m_init(unsigned long *p2m)
@@ -286,7 +297,9 @@ void __ref xen_build_mfn_list_list(void)
 	/* Pre-initialize p2m_top_mfn to be completely missing */
 	if (p2m_top_mfn == NULL) {
 		p2m_mid_missing_mfn = extend_brk(PAGE_SIZE, PAGE_SIZE);
-		p2m_mid_mfn_init(p2m_mid_missing_mfn);
+		p2m_mid_mfn_init(p2m_mid_missing_mfn, p2m_missing);
+		p2m_mid_identity_mfn = extend_brk(PAGE_SIZE, PAGE_SIZE);
+		p2m_mid_mfn_init(p2m_mid_identity_mfn, p2m_identity);
 
 		p2m_top_mfn_p = extend_brk(PAGE_SIZE, PAGE_SIZE);
 		p2m_top_mfn_p_init(p2m_top_mfn_p);
@@ -295,7 +308,8 @@ void __ref xen_build_mfn_list_list(void)
 		p2m_top_mfn_init(p2m_top_mfn);
 	} else {
 		/* Reinitialise, mfn's all change after migration */
-		p2m_mid_mfn_init(p2m_mid_missing_mfn);
+		p2m_mid_mfn_init(p2m_mid_missing_mfn, p2m_missing);
+		p2m_mid_mfn_init(p2m_mid_identity_mfn, p2m_identity);
 	}
 
 	for (pfn = 0; pfn < xen_max_p2m_pfn; pfn += P2M_PER_PAGE) {
@@ -327,7 +341,7 @@ void __ref xen_build_mfn_list_list(void)
 			 * it too late.
 			 */
 			mid_mfn_p = extend_brk(PAGE_SIZE, PAGE_SIZE);
-			p2m_mid_mfn_init(mid_mfn_p);
+			p2m_mid_mfn_init(mid_mfn_p, p2m_missing);
 
 			p2m_top_mfn_p[topidx] = mid_mfn_p;
 		}
@@ -365,16 +379,17 @@ void __init xen_build_dynamic_phys_to_machine(void)
 
 	p2m_missing = extend_brk(PAGE_SIZE, PAGE_SIZE);
 	p2m_init(p2m_missing);
+	p2m_identity = extend_brk(PAGE_SIZE, PAGE_SIZE);
+	p2m_init(p2m_identity);
 
 	p2m_mid_missing = extend_brk(PAGE_SIZE, PAGE_SIZE);
-	p2m_mid_init(p2m_mid_missing);
+	p2m_mid_init(p2m_mid_missing, p2m_missing);
+	p2m_mid_identity = extend_brk(PAGE_SIZE, PAGE_SIZE);
+	p2m_mid_init(p2m_mid_identity, p2m_identity);
 
 	p2m_top = extend_brk(PAGE_SIZE, PAGE_SIZE);
 	p2m_top_init(p2m_top);
 
-	p2m_identity = extend_brk(PAGE_SIZE, PAGE_SIZE);
-	p2m_init(p2m_identity);
-
 	/*
 	 * The domain builder gives us a pre-constructed p2m array in
 	 * mfn_list for all the pages initially given to us, so we just
@@ -386,7 +401,7 @@ void __init xen_build_dynamic_phys_to_machine(void)
 
 		if (p2m_top[topidx] == p2m_mid_missing) {
 			unsigned long **mid = extend_brk(PAGE_SIZE, PAGE_SIZE);
-			p2m_mid_init(mid);
+			p2m_mid_init(mid, p2m_missing);
 
 			p2m_top[topidx] = mid;
 		}
@@ -492,7 +507,7 @@ unsigned long get_phys_to_machine(unsigned long pfn)
 	unsigned topidx, mididx, idx;
 
 	if (unlikely(pfn >= MAX_P2M_PFN))
-		return INVALID_P2M_ENTRY;
+		return IDENTITY_FRAME(pfn);
 
 	topidx = p2m_top_index(pfn);
 	mididx = p2m_mid_index(pfn);
@@ -545,7 +560,7 @@ static bool alloc_p2m(unsigned long pfn)
 		if (!mid)
 			return false;
 
-		p2m_mid_init(mid);
+		p2m_mid_init(mid, p2m_missing);
 
 		if (cmpxchg(top_p, p2m_mid_missing, mid) != p2m_mid_missing)
 			free_p2m_page(mid);
@@ -565,7 +580,7 @@ static bool alloc_p2m(unsigned long pfn)
 		if (!mid_mfn)
 			return false;
 
-		p2m_mid_mfn_init(mid_mfn);
+		p2m_mid_mfn_init(mid_mfn, p2m_missing);
 
 		missing_mfn = virt_to_mfn(p2m_mid_missing_mfn);
 		mid_mfn_mfn = virt_to_mfn(mid_mfn);
@@ -596,7 +611,7 @@ static bool alloc_p2m(unsigned long pfn)
 	return true;
 }
 
-static bool __init early_alloc_p2m_middle(unsigned long pfn, bool check_boundary)
+static bool __init early_alloc_p2m(unsigned long pfn, bool check_boundary)
 {
 	unsigned topidx, mididx, idx;
 	unsigned long *p2m;
@@ -638,7 +653,7 @@ static bool __init early_alloc_p2m_middle(unsigned long pfn, bool check_boundary
 	return true;
 }
 
-static bool __init early_alloc_p2m(unsigned long pfn)
+static bool __init early_alloc_p2m_middle(unsigned long pfn)
 {
 	unsigned topidx = p2m_top_index(pfn);
 	unsigned long *mid_mfn_p;
@@ -649,7 +664,7 @@ static bool __init early_alloc_p2m(unsigned long pfn)
 	if (mid == p2m_mid_missing) {
 		mid = extend_brk(PAGE_SIZE, PAGE_SIZE);
 
-		p2m_mid_init(mid);
+		p2m_mid_init(mid, p2m_missing);
 
 		p2m_top[topidx] = mid;
 
@@ -658,12 +673,12 @@ static bool __init early_alloc_p2m(unsigned long pfn)
 	/* And the save/restore P2M tables.. */
 	if (mid_mfn_p == p2m_mid_missing_mfn) {
 		mid_mfn_p = extend_brk(PAGE_SIZE, PAGE_SIZE);
-		p2m_mid_mfn_init(mid_mfn_p);
+		p2m_mid_mfn_init(mid_mfn_p, p2m_missing);
 
 		p2m_top_mfn_p[topidx] = mid_mfn_p;
 		p2m_top_mfn[topidx] = virt_to_mfn(mid_mfn_p);
 		/* Note: we don't set mid_mfn_p[midix] here,
-		 * look in early_alloc_p2m_middle */
+		 * look in early_alloc_p2m() */
 	}
 	return true;
 }
@@ -739,7 +754,7 @@ found:
 
 	/* This shouldn't happen */
 	if (WARN_ON(p2m_top[topidx] == p2m_mid_missing))
-		early_alloc_p2m(set_pfn);
+		early_alloc_p2m_middle(set_pfn);
 
 	if (WARN_ON(p2m_top[topidx][mididx] != p2m_missing))
 		return false;
@@ -754,13 +769,13 @@ found:
 bool __init early_set_phys_to_machine(unsigned long pfn, unsigned long mfn)
 {
 	if (unlikely(!__set_phys_to_machine(pfn, mfn)))  {
-		if (!early_alloc_p2m(pfn))
+		if (!early_alloc_p2m_middle(pfn))
 			return false;
 
 		if (early_can_reuse_p2m_middle(pfn, mfn))
 			return __set_phys_to_machine(pfn, mfn);
 
-		if (!early_alloc_p2m_middle(pfn, false /* boundary crossover OK!*/))
+		if (!early_alloc_p2m(pfn, false /* boundary crossover OK!*/))
 			return false;
 
 		if (!__set_phys_to_machine(pfn, mfn))
@@ -769,12 +784,30 @@ bool __init early_set_phys_to_machine(unsigned long pfn, unsigned long mfn)
 
 	return true;
 }
+
+static void __init early_split_p2m(unsigned long pfn)
+{
+	unsigned long mididx, idx;
+
+	mididx = p2m_mid_index(pfn);
+	idx = p2m_index(pfn);
+
+	/*
+	 * Allocate new middle and leaf pages if this pfn lies in the
+	 * middle of one.
+	 */
+	if (mididx || idx)
+		early_alloc_p2m_middle(pfn);
+	if (idx)
+		early_alloc_p2m(pfn, false);
+}
+
 unsigned long __init set_phys_range_identity(unsigned long pfn_s,
 				      unsigned long pfn_e)
 {
 	unsigned long pfn;
 
-	if (unlikely(pfn_s >= MAX_P2M_PFN || pfn_e >= MAX_P2M_PFN))
+	if (unlikely(pfn_s >= MAX_P2M_PFN))
 		return 0;
 
 	if (unlikely(xen_feature(XENFEAT_auto_translated_physmap)))
@@ -783,19 +816,30 @@ unsigned long __init set_phys_range_identity(unsigned long pfn_s,
 	if (pfn_s > pfn_e)
 		return 0;
 
-	for (pfn = (pfn_s & ~(P2M_MID_PER_PAGE * P2M_PER_PAGE - 1));
-		pfn < ALIGN(pfn_e, (P2M_MID_PER_PAGE * P2M_PER_PAGE));
-		pfn += P2M_MID_PER_PAGE * P2M_PER_PAGE)
-	{
-		WARN_ON(!early_alloc_p2m(pfn));
-	}
+	if (pfn_e > MAX_P2M_PFN)
+		pfn_e = MAX_P2M_PFN;
 
-	early_alloc_p2m_middle(pfn_s, true);
-	early_alloc_p2m_middle(pfn_e, true);
+	early_split_p2m(pfn_s);
+	early_split_p2m(pfn_e);
+
+	for (pfn = pfn_s; pfn < pfn_e;) {
+		unsigned topidx = p2m_top_index(pfn);
+		unsigned mididx = p2m_mid_index(pfn);
 
-	for (pfn = pfn_s; pfn < pfn_e; pfn++)
 		if (!__set_phys_to_machine(pfn, IDENTITY_FRAME(pfn)))
 			break;
+		pfn++;
+
+		/*
+		 * If the PFN was set to a middle or leaf identity
+		 * page the remainder must also be identity, so skip
+		 * ahead to the next middle or leaf entry.
+		 */
+		if (p2m_top[topidx] == p2m_mid_identity)
+			pfn = ALIGN(pfn, P2M_MID_PER_PAGE * P2M_PER_PAGE);
+		else if (p2m_top[topidx][mididx] == p2m_identity)
+			pfn = ALIGN(pfn, P2M_PER_PAGE);
+	}
 
 	if (!WARN((pfn - pfn_s) != (pfn_e - pfn_s),
 		"Identity mapping failed. We are %ld short of 1-1 mappings!\n",
@@ -825,8 +869,22 @@ bool __set_phys_to_machine(unsigned long pfn, unsigned long mfn)
 
 	/* For sparse holes were the p2m leaf has real PFN along with
 	 * PCI holes, stick in the PFN as the MFN value.
+	 *
+	 * set_phys_range_identity() will have allocated new middle
+	 * and leaf pages as required so an existing p2m_mid_missing
+	 * or p2m_missing mean that whole range will be identity so
+	 * these can be switched to p2m_mid_identity or p2m_identity.
 	 */
 	if (mfn != INVALID_P2M_ENTRY && (mfn & IDENTITY_FRAME_BIT)) {
+		if (p2m_top[topidx] == p2m_mid_identity)
+			return true;
+
+		if (p2m_top[topidx] == p2m_mid_missing) {
+			WARN_ON(cmpxchg(&p2m_top[topidx], p2m_mid_missing,
+					p2m_mid_identity) != p2m_mid_missing);
+			return true;
+		}
+
 		if (p2m_top[topidx][mididx] == p2m_identity)
 			return true;
 
diff --git a/arch/x86/xen/setup.c b/arch/x86/xen/setup.c
index 0982233b9b8..210426a26cc 100644
--- a/arch/x86/xen/setup.c
+++ b/arch/x86/xen/setup.c
@@ -89,10 +89,10 @@ static void __init xen_add_extra_mem(u64 start, u64 size)
 	for (pfn = PFN_DOWN(start); pfn < xen_max_p2m_pfn; pfn++) {
 		unsigned long mfn = pfn_to_mfn(pfn);
 
-		if (WARN(mfn == pfn, "Trying to over-write 1-1 mapping (pfn: %lx)\n", pfn))
+		if (WARN_ONCE(mfn == pfn, "Trying to over-write 1-1 mapping (pfn: %lx)\n", pfn))
 			continue;
-		WARN(mfn != INVALID_P2M_ENTRY, "Trying to remove %lx which has %lx mfn!\n",
-			pfn, mfn);
+		WARN_ONCE(mfn != INVALID_P2M_ENTRY, "Trying to remove %lx which has %lx mfn!\n",
+			  pfn, mfn);
 
 		__set_phys_to_machine(pfn, INVALID_P2M_ENTRY);
 	}
@@ -469,6 +469,15 @@ char * __init xen_memory_setup(void)
 	}
 
 	/*
+	 * Set the rest as identity mapped, in case PCI BARs are
+	 * located here.
+	 *
+	 * PFNs above MAX_P2M_PFN are considered identity mapped as
+	 * well.
+	 */
+	set_phys_range_identity(map[i-1].addr / PAGE_SIZE, ~0ul);
+
+	/*
 	 * In domU, the ISA region is normal, usable memory, but we
 	 * reserve ISA memory anyway because too many things poke
 	 * about in there.
diff --git a/arch/x86/xen/suspend.c b/arch/x86/xen/suspend.c
index 45329c8c226..c4df9dbd63b 100644
--- a/arch/x86/xen/suspend.c
+++ b/arch/x86/xen/suspend.c
@@ -12,8 +12,10 @@
 #include "xen-ops.h"
 #include "mmu.h"
 
-void xen_arch_pre_suspend(void)
+static void xen_pv_pre_suspend(void)
 {
+	xen_mm_pin_all();
+
 	xen_start_info->store_mfn = mfn_to_pfn(xen_start_info->store_mfn);
 	xen_start_info->console.domU.mfn =
 		mfn_to_pfn(xen_start_info->console.domU.mfn);
@@ -26,7 +28,7 @@ void xen_arch_pre_suspend(void)
 		BUG();
 }
 
-void xen_arch_hvm_post_suspend(int suspend_cancelled)
+static void xen_hvm_post_suspend(int suspend_cancelled)
 {
 #ifdef CONFIG_XEN_PVHVM
 	int cpu;
@@ -41,7 +43,7 @@ void xen_arch_hvm_post_suspend(int suspend_cancelled)
 #endif
 }
 
-void xen_arch_post_suspend(int suspend_cancelled)
+static void xen_pv_post_suspend(int suspend_cancelled)
 {
 	xen_build_mfn_list_list();
 
@@ -60,6 +62,21 @@ void xen_arch_post_suspend(int suspend_cancelled)
 		xen_vcpu_restore();
 	}
 
+	xen_mm_unpin_all();
+}
+
+void xen_arch_pre_suspend(void)
+{
+    if (xen_pv_domain())
+        xen_pv_pre_suspend();
+}
+
+void xen_arch_post_suspend(int cancelled)
+{
+    if (xen_pv_domain())
+        xen_pv_post_suspend(cancelled);
+    else
+        xen_hvm_post_suspend(cancelled);
 }
 
 static void xen_vcpu_notify_restore(void *data)
diff --git a/arch/x86/xen/xen-ops.h b/arch/x86/xen/xen-ops.h
index 1cb6f4c3730..c834d4b231f 100644
--- a/arch/x86/xen/xen-ops.h
+++ b/arch/x86/xen/xen-ops.h
@@ -31,6 +31,8 @@ void xen_setup_kernel_pagetable(pgd_t *pgd, unsigned long max_pfn);
 void xen_reserve_top(void);
 extern unsigned long xen_max_p2m_pfn;
 
+void xen_mm_pin_all(void);
+void xen_mm_unpin_all(void);
 void xen_set_pat(u64);
 
 char * __init xen_memory_setup(void);
diff --git a/arch/xtensa/Kconfig b/arch/xtensa/Kconfig
index 02d6d29a63c..3a617af60d4 100644
--- a/arch/xtensa/Kconfig
+++ b/arch/xtensa/Kconfig
@@ -14,6 +14,7 @@ config XTENSA
 	select GENERIC_PCI_IOMAP
 	select ARCH_WANT_IPC_PARSE_VERSION
 	select ARCH_WANT_OPTIONAL_GPIOLIB
+	select BUILDTIME_EXTABLE_SORT
 	select CLONE_BACKWARDS
 	select IRQ_DOMAIN
 	select HAVE_OPROFILE
@@ -189,6 +190,24 @@ config INITIALIZE_XTENSA_MMU_INSIDE_VMLINUX
 
 	  If in doubt, say Y.
 
+config HIGHMEM
+	bool "High Memory Support"
+	help
+	  Linux can use the full amount of RAM in the system by
+	  default. However, the default MMUv2 setup only maps the
+	  lowermost 128 MB of memory linearly to the areas starting
+	  at 0xd0000000 (cached) and 0xd8000000 (uncached).
+	  When there are more than 128 MB memory in the system not
+	  all of it can be "permanently mapped" by the kernel.
+	  The physical memory that's not permanently mapped is called
+	  "high memory".
+
+	  If you are compiling a kernel which will never run on a
+	  machine with more than 128 MB total physical RAM, answer
+	  N here.
+
+	  If unsure, say Y.
+
 endmenu
 
 config XTENSA_CALIBRATE_CCOUNT
@@ -224,7 +243,6 @@ choice
 
 config XTENSA_PLATFORM_ISS
 	bool "ISS"
-	depends on TTY
 	select XTENSA_CALIBRATE_CCOUNT
 	select SERIAL_CONSOLE
 	help
diff --git a/arch/xtensa/boot/dts/kc705.dts b/arch/xtensa/boot/dts/kc705.dts
new file mode 100644
index 00000000000..742a347be67
--- /dev/null
+++ b/arch/xtensa/boot/dts/kc705.dts
@@ -0,0 +1,11 @@
+/dts-v1/;
+/include/ "xtfpga.dtsi"
+/include/ "xtfpga-flash-128m.dtsi"
+
+/ {
+	compatible = "cdns,xtensa-kc705";
+	memory@0 {
+		device_type = "memory";
+		reg = <0x00000000 0x08000000>;
+	};
+};
diff --git a/arch/xtensa/boot/dts/xtfpga-flash-128m.dtsi b/arch/xtensa/boot/dts/xtfpga-flash-128m.dtsi
new file mode 100644
index 00000000000..d3a88e02987
--- /dev/null
+++ b/arch/xtensa/boot/dts/xtfpga-flash-128m.dtsi
@@ -0,0 +1,28 @@
+/ {
+	soc {
+		flash: flash@00000000 {
+			#address-cells = <1>;
+			#size-cells = <1>;
+			compatible = "cfi-flash";
+			reg = <0x00000000 0x08000000>;
+			bank-width = <2>;
+			device-width = <2>;
+			partition@0x0 {
+				label = "data";
+				reg = <0x00000000 0x06000000>;
+			};
+			partition@0x6000000 {
+				label = "boot loader area";
+				reg = <0x06000000 0x00800000>;
+			};
+			partition@0x6800000 {
+				label = "kernel image";
+				reg = <0x06800000 0x017e0000>;
+			};
+			partition@0x7fe0000 {
+				label = "boot environment";
+				reg = <0x07fe0000 0x00020000>;
+			};
+		};
+        };
+};
diff --git a/arch/xtensa/boot/dts/xtfpga-flash-16m.dtsi b/arch/xtensa/boot/dts/xtfpga-flash-16m.dtsi
index e5703c7beeb..1d97203c18e 100644
--- a/arch/xtensa/boot/dts/xtfpga-flash-16m.dtsi
+++ b/arch/xtensa/boot/dts/xtfpga-flash-16m.dtsi
@@ -1,26 +1,28 @@
 / {
-	flash: flash@f8000000 {
-		#address-cells = <1>;
-		#size-cells = <1>;
-		compatible = "cfi-flash";
-		reg = <0xf8000000 0x01000000>;
-		bank-width = <2>;
-		device-width = <2>;
-		partition@0x0 {
-			label = "boot loader area";
-			reg = <0x00000000 0x00400000>;
+	soc {
+		flash: flash@08000000 {
+			#address-cells = <1>;
+			#size-cells = <1>;
+			compatible = "cfi-flash";
+			reg = <0x08000000 0x01000000>;
+			bank-width = <2>;
+			device-width = <2>;
+			partition@0x0 {
+				label = "boot loader area";
+				reg = <0x00000000 0x00400000>;
+			};
+			partition@0x400000 {
+				label = "kernel image";
+				reg = <0x00400000 0x00600000>;
+			};
+			partition@0xa00000 {
+				label = "data";
+				reg = <0x00a00000 0x005e0000>;
+			};
+			partition@0xfe0000 {
+				label = "boot environment";
+				reg = <0x00fe0000 0x00020000>;
+			};
 		};
-		partition@0x400000 {
-			label = "kernel image";
-			reg = <0x00400000 0x00600000>;
-		};
-		partition@0xa00000 {
-			label = "data";
-			reg = <0x00a00000 0x005e0000>;
-		};
-		partition@0xfe0000 {
-			label = "boot environment";
-			reg = <0x00fe0000 0x00020000>;
-		};
-        };
+	};
 };
diff --git a/arch/xtensa/boot/dts/xtfpga-flash-4m.dtsi b/arch/xtensa/boot/dts/xtfpga-flash-4m.dtsi
index 6f9c10d6b68..d1c621ca8be 100644
--- a/arch/xtensa/boot/dts/xtfpga-flash-4m.dtsi
+++ b/arch/xtensa/boot/dts/xtfpga-flash-4m.dtsi
@@ -1,18 +1,20 @@
 / {
-	flash: flash@f8000000 {
-		#address-cells = <1>;
-		#size-cells = <1>;
-		compatible = "cfi-flash";
-		reg = <0xf8000000 0x00400000>;
-		bank-width = <2>;
-		device-width = <2>;
-		partition@0x0 {
-			label = "boot loader area";
-			reg = <0x00000000 0x003f0000>;
+	soc {
+		flash: flash@08000000 {
+			#address-cells = <1>;
+			#size-cells = <1>;
+			compatible = "cfi-flash";
+			reg = <0x08000000 0x00400000>;
+			bank-width = <2>;
+			device-width = <2>;
+			partition@0x0 {
+				label = "boot loader area";
+				reg = <0x00000000 0x003f0000>;
+			};
+			partition@0x3f0000 {
+				label = "boot environment";
+				reg = <0x003f0000 0x00010000>;
+			};
 		};
-		partition@0x3f0000 {
-			label = "boot environment";
-			reg = <0x003f0000 0x00010000>;
-		};
-        };
+	};
 };
diff --git a/arch/xtensa/boot/dts/xtfpga.dtsi b/arch/xtensa/boot/dts/xtfpga.dtsi
index e7370b11348..dec9178840f 100644
--- a/arch/xtensa/boot/dts/xtfpga.dtsi
+++ b/arch/xtensa/boot/dts/xtfpga.dtsi
@@ -42,21 +42,28 @@
 		};
 	};
 
-	serial0: serial@fd050020 {
-		device_type = "serial";
-		compatible = "ns16550a";
-		no-loopback-test;
-		reg = <0xfd050020 0x20>;
-		reg-shift = <2>;
-		interrupts = <0 1>; /* external irq 0 */
-		clocks = <&osc>;
-	};
+	soc {
+		#address-cells = <1>;
+		#size-cells = <1>;
+		compatible = "simple-bus";
+		ranges = <0x00000000 0xf0000000 0x10000000>;
 
-	enet0: ethoc@fd030000 {
-		compatible = "opencores,ethoc";
-		reg = <0xfd030000 0x4000 0xfd800000 0x4000>;
-		interrupts = <1 1>; /* external irq 1 */
-		local-mac-address = [00 50 c2 13 6f 00];
-		clocks = <&osc>;
+		serial0: serial@0d050020 {
+			device_type = "serial";
+			compatible = "ns16550a";
+			no-loopback-test;
+			reg = <0x0d050020 0x20>;
+			reg-shift = <2>;
+			interrupts = <0 1>; /* external irq 0 */
+			clocks = <&osc>;
+		};
+
+		enet0: ethoc@0d030000 {
+			compatible = "opencores,ethoc";
+			reg = <0x0d030000 0x4000 0x0d800000 0x4000>;
+			interrupts = <1 1>; /* external irq 1 */
+			local-mac-address = [00 50 c2 13 6f 00];
+			clocks = <&osc>;
+		};
 	};
 };
diff --git a/arch/xtensa/include/asm/bootparam.h b/arch/xtensa/include/asm/bootparam.h
index 23392c5630c..892aab399ac 100644
--- a/arch/xtensa/include/asm/bootparam.h
+++ b/arch/xtensa/include/asm/bootparam.h
@@ -37,23 +37,14 @@ typedef struct bp_tag {
 	unsigned long data[0];	/* data */
 } bp_tag_t;
 
-typedef struct meminfo {
+struct bp_meminfo {
 	unsigned long type;
 	unsigned long start;
 	unsigned long end;
-} meminfo_t;
-
-#define SYSMEM_BANKS_MAX 5
+};
 
 #define MEMORY_TYPE_CONVENTIONAL	0x1000
 #define MEMORY_TYPE_NONE		0x2000
 
-typedef struct sysmem_info {
-	int nr_banks;
-	meminfo_t bank[SYSMEM_BANKS_MAX];
-} sysmem_info_t;
-
-extern sysmem_info_t sysmem;
-
 #endif
 #endif
diff --git a/arch/xtensa/include/asm/fixmap.h b/arch/xtensa/include/asm/fixmap.h
new file mode 100644
index 00000000000..9f6c33d0428
--- /dev/null
+++ b/arch/xtensa/include/asm/fixmap.h
@@ -0,0 +1,58 @@
+/*
+ * fixmap.h: compile-time virtual memory allocation
+ *
+ * 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.
+ *
+ * Copyright (C) 1998 Ingo Molnar
+ *
+ * Support of BIGMEM added by Gerhard Wichert, Siemens AG, July 1999
+ */
+
+#ifndef _ASM_FIXMAP_H
+#define _ASM_FIXMAP_H
+
+#include <asm/pgtable.h>
+#ifdef CONFIG_HIGHMEM
+#include <linux/threads.h>
+#include <asm/kmap_types.h>
+#endif
+
+/*
+ * Here we define all the compile-time 'special' virtual
+ * addresses. The point is to have a constant address at
+ * compile time, but to set the physical address only
+ * in the boot process. We allocate these special  addresses
+ * from the end of the consistent memory region backwards.
+ * Also this lets us do fail-safe vmalloc(), we
+ * can guarantee that these special addresses and
+ * vmalloc()-ed addresses never overlap.
+ *
+ * these 'compile-time allocated' memory buffers are
+ * fixed-size 4k pages. (or larger if used with an increment
+ * higher than 1) use fixmap_set(idx,phys) to associate
+ * physical memory with fixmap indices.
+ */
+enum fixed_addresses {
+#ifdef CONFIG_HIGHMEM
+	/* reserved pte's for temporary kernel mappings */
+	FIX_KMAP_BEGIN,
+	FIX_KMAP_END = FIX_KMAP_BEGIN + (KM_TYPE_NR * NR_CPUS) - 1,
+#endif
+	__end_of_fixed_addresses
+};
+
+#define FIXADDR_TOP     (VMALLOC_START - PAGE_SIZE)
+#define FIXADDR_SIZE	(__end_of_fixed_addresses << PAGE_SHIFT)
+#define FIXADDR_START	((FIXADDR_TOP - FIXADDR_SIZE) & PMD_MASK)
+
+#include <asm-generic/fixmap.h>
+
+#define kmap_get_fixmap_pte(vaddr) \
+	pte_offset_kernel( \
+		pmd_offset(pud_offset(pgd_offset_k(vaddr), (vaddr)), (vaddr)), \
+		(vaddr) \
+	)
+
+#endif
diff --git a/arch/xtensa/include/asm/highmem.h b/arch/xtensa/include/asm/highmem.h
index 80be1512469..2653ef5d55f 100644
--- a/arch/xtensa/include/asm/highmem.h
+++ b/arch/xtensa/include/asm/highmem.h
@@ -6,11 +6,54 @@
  * this archive for more details.
  *
  * Copyright (C) 2003 - 2005 Tensilica Inc.
+ * Copyright (C) 2014 Cadence Design Systems Inc.
  */
 
 #ifndef _XTENSA_HIGHMEM_H
 #define _XTENSA_HIGHMEM_H
 
-extern void flush_cache_kmaps(void);
+#include <asm/cacheflush.h>
+#include <asm/fixmap.h>
+#include <asm/kmap_types.h>
+#include <asm/pgtable.h>
+
+#define PKMAP_BASE		(FIXADDR_START - PMD_SIZE)
+#define LAST_PKMAP		PTRS_PER_PTE
+#define LAST_PKMAP_MASK		(LAST_PKMAP - 1)
+#define PKMAP_NR(virt)		(((virt) - PKMAP_BASE) >> PAGE_SHIFT)
+#define PKMAP_ADDR(nr)		(PKMAP_BASE + ((nr) << PAGE_SHIFT))
+
+#define kmap_prot		PAGE_KERNEL
+
+extern pte_t *pkmap_page_table;
+
+void *kmap_high(struct page *page);
+void kunmap_high(struct page *page);
+
+static inline void *kmap(struct page *page)
+{
+	BUG_ON(in_interrupt());
+	if (!PageHighMem(page))
+		return page_address(page);
+	return kmap_high(page);
+}
+
+static inline void kunmap(struct page *page)
+{
+	BUG_ON(in_interrupt());
+	if (!PageHighMem(page))
+		return;
+	kunmap_high(page);
+}
+
+static inline void flush_cache_kmaps(void)
+{
+	flush_cache_all();
+}
+
+void *kmap_atomic(struct page *page);
+void __kunmap_atomic(void *kvaddr);
+
+void kmap_init(void);
 
 #endif
diff --git a/arch/xtensa/include/asm/pci.h b/arch/xtensa/include/asm/pci.h
index 614be031a79..5d52dc43dfe 100644
--- a/arch/xtensa/include/asm/pci.h
+++ b/arch/xtensa/include/asm/pci.h
@@ -22,11 +22,6 @@
 
 extern struct pci_controller* pcibios_alloc_controller(void);
 
-static inline void pcibios_penalize_isa_irq(int irq)
-{
-	/* We don't do dynamic PCI IRQ allocation */
-}
-
 /* Assume some values. (We should revise them, if necessary) */
 
 #define PCIBIOS_MIN_IO		0x2000
diff --git a/arch/xtensa/include/asm/pgtable.h b/arch/xtensa/include/asm/pgtable.h
index 216446295ad..4b0ca35a93b 100644
--- a/arch/xtensa/include/asm/pgtable.h
+++ b/arch/xtensa/include/asm/pgtable.h
@@ -310,6 +310,10 @@ set_pte_at(struct mm_struct *mm, unsigned long addr, pte_t *ptep, pte_t pteval)
 	update_pte(ptep, pteval);
 }
 
+static inline void set_pte(pte_t *ptep, pte_t pteval)
+{
+	update_pte(ptep, pteval);
+}
 
 static inline void
 set_pmd(pmd_t *pmdp, pmd_t pmdval)
diff --git a/arch/xtensa/include/asm/sysmem.h b/arch/xtensa/include/asm/sysmem.h
new file mode 100644
index 00000000000..c015c5c8e3f
--- /dev/null
+++ b/arch/xtensa/include/asm/sysmem.h
@@ -0,0 +1,38 @@
+/*
+ * sysmem-related prototypes.
+ *
+ * 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.
+ *
+ * Copyright (C) 2014 Cadence Design Systems Inc.
+ */
+
+#ifndef _XTENSA_SYSMEM_H
+#define _XTENSA_SYSMEM_H
+
+#define SYSMEM_BANKS_MAX 31
+
+struct meminfo {
+	unsigned long start;
+	unsigned long end;
+};
+
+/*
+ * Bank array is sorted by .start.
+ * Banks don't overlap and there's at least one page gap
+ * between adjacent bank entries.
+ */
+struct sysmem_info {
+	int nr_banks;
+	struct meminfo bank[SYSMEM_BANKS_MAX];
+};
+
+extern struct sysmem_info sysmem;
+
+int add_sysmem_bank(unsigned long start, unsigned long end);
+int mem_reserve(unsigned long, unsigned long, int);
+void bootmem_init(void);
+void zones_init(void);
+
+#endif /* _XTENSA_SYSMEM_H */
diff --git a/arch/xtensa/include/asm/tlbflush.h b/arch/xtensa/include/asm/tlbflush.h
index fc34274ce41..06875feb27c 100644
--- a/arch/xtensa/include/asm/tlbflush.h
+++ b/arch/xtensa/include/asm/tlbflush.h
@@ -36,6 +36,7 @@ void local_flush_tlb_page(struct vm_area_struct *vma,
 		unsigned long page);
 void local_flush_tlb_range(struct vm_area_struct *vma,
 		unsigned long start, unsigned long end);
+void local_flush_tlb_kernel_range(unsigned long start, unsigned long end);
 
 #ifdef CONFIG_SMP
 
@@ -44,12 +45,7 @@ void flush_tlb_mm(struct mm_struct *);
 void flush_tlb_page(struct vm_area_struct *, unsigned long);
 void flush_tlb_range(struct vm_area_struct *, unsigned long,
 		unsigned long);
-
-static inline void flush_tlb_kernel_range(unsigned long start,
-		unsigned long end)
-{
-	flush_tlb_all();
-}
+void flush_tlb_kernel_range(unsigned long start, unsigned long end);
 
 #else /* !CONFIG_SMP */
 
@@ -58,7 +54,8 @@ static inline void flush_tlb_kernel_range(unsigned long start,
 #define flush_tlb_page(vma, page)	   local_flush_tlb_page(vma, page)
 #define flush_tlb_range(vma, vmaddr, end)  local_flush_tlb_range(vma, vmaddr, \
 								 end)
-#define flush_tlb_kernel_range(start, end) local_flush_tlb_all()
+#define flush_tlb_kernel_range(start, end) local_flush_tlb_kernel_range(start, \
+									end)
 
 #endif /* CONFIG_SMP */
 
diff --git a/arch/xtensa/kernel/setup.c b/arch/xtensa/kernel/setup.c
index 84fe931bb60..9757bb74e53 100644
--- a/arch/xtensa/kernel/setup.c
+++ b/arch/xtensa/kernel/setup.c
@@ -50,6 +50,7 @@
 #include <asm/param.h>
 #include <asm/traps.h>
 #include <asm/smp.h>
+#include <asm/sysmem.h>
 
 #include <platform/hardware.h>
 
@@ -88,12 +89,6 @@ static char __initdata command_line[COMMAND_LINE_SIZE];
 static char default_command_line[COMMAND_LINE_SIZE] __initdata = CONFIG_CMDLINE;
 #endif
 
-sysmem_info_t __initdata sysmem;
-
-extern int mem_reserve(unsigned long, unsigned long, int);
-extern void bootmem_init(void);
-extern void zones_init(void);
-
 /*
  * Boot parameter parsing.
  *
@@ -113,31 +108,14 @@ typedef struct tagtable {
 
 /* parse current tag */
 
-static int __init add_sysmem_bank(unsigned long type, unsigned long start,
-		unsigned long end)
-{
-	if (sysmem.nr_banks >= SYSMEM_BANKS_MAX) {
-		printk(KERN_WARNING
-				"Ignoring memory bank 0x%08lx size %ldKB\n",
-				start, end - start);
-		return -EINVAL;
-	}
-	sysmem.bank[sysmem.nr_banks].type  = type;
-	sysmem.bank[sysmem.nr_banks].start = PAGE_ALIGN(start);
-	sysmem.bank[sysmem.nr_banks].end   = end & PAGE_MASK;
-	sysmem.nr_banks++;
-
-	return 0;
-}
-
 static int __init parse_tag_mem(const bp_tag_t *tag)
 {
-	meminfo_t *mi = (meminfo_t *)(tag->data);
+	struct bp_meminfo *mi = (struct bp_meminfo *)(tag->data);
 
 	if (mi->type != MEMORY_TYPE_CONVENTIONAL)
 		return -1;
 
-	return add_sysmem_bank(mi->type, mi->start, mi->end);
+	return add_sysmem_bank(mi->start, mi->end);
 }
 
 __tagtable(BP_TAG_MEMORY, parse_tag_mem);
@@ -146,8 +124,8 @@ __tagtable(BP_TAG_MEMORY, parse_tag_mem);
 
 static int __init parse_tag_initrd(const bp_tag_t* tag)
 {
-	meminfo_t* mi;
-	mi = (meminfo_t*)(tag->data);
+	struct bp_meminfo *mi = (struct bp_meminfo *)(tag->data);
+
 	initrd_start = (unsigned long)__va(mi->start);
 	initrd_end = (unsigned long)__va(mi->end);
 
@@ -255,7 +233,7 @@ void __init early_init_dt_add_memory_arch(u64 base, u64 size)
 		return;
 
 	size &= PAGE_MASK;
-	add_sysmem_bank(MEMORY_TYPE_CONVENTIONAL, base, base + size);
+	add_sysmem_bank(base, base + size);
 }
 
 void * __init early_init_dt_alloc_memory_arch(u64 size, u64 align)
@@ -292,8 +270,6 @@ device_initcall(xtensa_device_probe);
 
 void __init init_arch(bp_tag_t *bp_start)
 {
-	sysmem.nr_banks = 0;
-
 	/* Parse boot parameters */
 
 	if (bp_start)
@@ -304,10 +280,9 @@ void __init init_arch(bp_tag_t *bp_start)
 #endif
 
 	if (sysmem.nr_banks == 0) {
-		sysmem.nr_banks = 1;
-		sysmem.bank[0].start = PLATFORM_DEFAULT_MEM_START;
-		sysmem.bank[0].end = PLATFORM_DEFAULT_MEM_START
-				     + PLATFORM_DEFAULT_MEM_SIZE;
+		add_sysmem_bank(PLATFORM_DEFAULT_MEM_START,
+				PLATFORM_DEFAULT_MEM_START +
+				PLATFORM_DEFAULT_MEM_SIZE);
 	}
 
 #ifdef CONFIG_CMDLINE_BOOL
@@ -487,7 +462,7 @@ void __init setup_arch(char **cmdline_p)
 #ifdef CONFIG_BLK_DEV_INITRD
 	if (initrd_start < initrd_end) {
 		initrd_is_mapped = mem_reserve(__pa(initrd_start),
-					       __pa(initrd_end), 0);
+					       __pa(initrd_end), 0) == 0;
 		initrd_below_start_ok = 1;
 	} else {
 		initrd_start = 0;
@@ -532,6 +507,7 @@ void __init setup_arch(char **cmdline_p)
 		    __pa(&_Level6InterruptVector_text_end), 0);
 #endif
 
+	parse_early_param();
 	bootmem_init();
 
 	unflatten_and_copy_device_tree();
diff --git a/arch/xtensa/kernel/smp.c b/arch/xtensa/kernel/smp.c
index aa8bd871792..40b5a3771fb 100644
--- a/arch/xtensa/kernel/smp.c
+++ b/arch/xtensa/kernel/smp.c
@@ -496,6 +496,21 @@ void flush_tlb_range(struct vm_area_struct *vma,
 	on_each_cpu(ipi_flush_tlb_range, &fd, 1);
 }
 
+static void ipi_flush_tlb_kernel_range(void *arg)
+{
+	struct flush_data *fd = arg;
+	local_flush_tlb_kernel_range(fd->addr1, fd->addr2);
+}
+
+void flush_tlb_kernel_range(unsigned long start, unsigned long end)
+{
+	struct flush_data fd = {
+		.addr1 = start,
+		.addr2 = end,
+	};
+	on_each_cpu(ipi_flush_tlb_kernel_range, &fd, 1);
+}
+
 /* Cache flush functions */
 
 static void ipi_flush_cache_all(void *arg)
diff --git a/arch/xtensa/kernel/xtensa_ksyms.c b/arch/xtensa/kernel/xtensa_ksyms.c
index 80b33ed51f3..4d2872fd9bb 100644
--- a/arch/xtensa/kernel/xtensa_ksyms.c
+++ b/arch/xtensa/kernel/xtensa_ksyms.c
@@ -20,6 +20,7 @@
 #include <linux/in6.h>
 
 #include <asm/uaccess.h>
+#include <asm/cacheflush.h>
 #include <asm/checksum.h>
 #include <asm/dma.h>
 #include <asm/io.h>
@@ -105,6 +106,7 @@ EXPORT_SYMBOL(csum_partial_copy_generic);
  * Architecture-specific symbols
  */
 EXPORT_SYMBOL(__xtensa_copy_user);
+EXPORT_SYMBOL(__invalidate_icache_range);
 
 /*
  * Kernel hacking ...
@@ -127,3 +129,8 @@ EXPORT_SYMBOL(common_exception_return);
 #ifdef CONFIG_FUNCTION_TRACER
 EXPORT_SYMBOL(_mcount);
 #endif
+
+EXPORT_SYMBOL(__invalidate_dcache_range);
+#if XCHAL_DCACHE_IS_WRITEBACK
+EXPORT_SYMBOL(__flush_dcache_range);
+#endif
diff --git a/arch/xtensa/mm/Makefile b/arch/xtensa/mm/Makefile
index f0b646d2f84..f54f78e24d7 100644
--- a/arch/xtensa/mm/Makefile
+++ b/arch/xtensa/mm/Makefile
@@ -4,3 +4,4 @@
 
 obj-y			:= init.o cache.o misc.o
 obj-$(CONFIG_MMU)	+= fault.o mmu.o tlb.o
+obj-$(CONFIG_HIGHMEM)	+= highmem.o
diff --git a/arch/xtensa/mm/cache.c b/arch/xtensa/mm/cache.c
index ba4c47f291b..63cbb867dad 100644
--- a/arch/xtensa/mm/cache.c
+++ b/arch/xtensa/mm/cache.c
@@ -59,6 +59,10 @@
  *
  */
 
+#if (DCACHE_WAY_SIZE > PAGE_SIZE) && defined(CONFIG_HIGHMEM)
+#error "HIGHMEM is not supported on cores with aliasing cache."
+#endif
+
 #if (DCACHE_WAY_SIZE > PAGE_SIZE) && XCHAL_DCACHE_IS_WRITEBACK
 
 /*
@@ -179,10 +183,11 @@ update_mmu_cache(struct vm_area_struct * vma, unsigned long addr, pte_t *ptep)
 #else
 	if (!PageReserved(page) && !test_bit(PG_arch_1, &page->flags)
 	    && (vma->vm_flags & VM_EXEC) != 0) {
-	    	unsigned long paddr = (unsigned long) page_address(page);
+		unsigned long paddr = (unsigned long)kmap_atomic(page);
 		__flush_dcache_page(paddr);
 		__invalidate_icache_page(paddr);
 		set_bit(PG_arch_1, &page->flags);
+		kunmap_atomic((void *)paddr);
 	}
 #endif
 }
diff --git a/arch/xtensa/mm/highmem.c b/arch/xtensa/mm/highmem.c
new file mode 100644
index 00000000000..17a8c0d6fd1
--- /dev/null
+++ b/arch/xtensa/mm/highmem.c
@@ -0,0 +1,72 @@
+/*
+ * High memory support for Xtensa architecture
+ *
+ * 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.
+ *
+ * Copyright (C) 2014 Cadence Design Systems Inc.
+ */
+
+#include <linux/export.h>
+#include <linux/highmem.h>
+#include <asm/tlbflush.h>
+
+static pte_t *kmap_pte;
+
+void *kmap_atomic(struct page *page)
+{
+	enum fixed_addresses idx;
+	unsigned long vaddr;
+	int type;
+
+	pagefault_disable();
+	if (!PageHighMem(page))
+		return page_address(page);
+
+	type = kmap_atomic_idx_push();
+	idx = type + KM_TYPE_NR * smp_processor_id();
+	vaddr = __fix_to_virt(FIX_KMAP_BEGIN + idx);
+#ifdef CONFIG_DEBUG_HIGHMEM
+	BUG_ON(!pte_none(*(kmap_pte - idx)));
+#endif
+	set_pte(kmap_pte - idx, mk_pte(page, PAGE_KERNEL_EXEC));
+
+	return (void *)vaddr;
+}
+EXPORT_SYMBOL(kmap_atomic);
+
+void __kunmap_atomic(void *kvaddr)
+{
+	int idx, type;
+
+	if (kvaddr >= (void *)FIXADDR_START &&
+	    kvaddr < (void *)FIXADDR_TOP) {
+		type = kmap_atomic_idx();
+		idx = type + KM_TYPE_NR * smp_processor_id();
+
+		/*
+		 * Force other mappings to Oops if they'll try to access this
+		 * pte without first remap it.  Keeping stale mappings around
+		 * is a bad idea also, in case the page changes cacheability
+		 * attributes or becomes a protected page in a hypervisor.
+		 */
+		pte_clear(&init_mm, kvaddr, kmap_pte - idx);
+		local_flush_tlb_kernel_range((unsigned long)kvaddr,
+					     (unsigned long)kvaddr + PAGE_SIZE);
+
+		kmap_atomic_idx_pop();
+	}
+
+	pagefault_enable();
+}
+EXPORT_SYMBOL(__kunmap_atomic);
+
+void __init kmap_init(void)
+{
+	unsigned long kmap_vstart;
+
+	/* cache the first kmap pte */
+	kmap_vstart = __fix_to_virt(FIX_KMAP_BEGIN);
+	kmap_pte = kmap_get_fixmap_pte(kmap_vstart);
+}
diff --git a/arch/xtensa/mm/init.c b/arch/xtensa/mm/init.c
index aff108df92d..4224256bb21 100644
--- a/arch/xtensa/mm/init.c
+++ b/arch/xtensa/mm/init.c
@@ -8,6 +8,7 @@
  * for more details.
  *
  * Copyright (C) 2001 - 2005 Tensilica Inc.
+ * Copyright (C) 2014 Cadence Design Systems Inc.
  *
  * Chris Zankel	<chris@zankel.net>
  * Joe Taylor	<joe@tensilica.com, joetylr@yahoo.com>
@@ -19,6 +20,7 @@
 #include <linux/errno.h>
 #include <linux/bootmem.h>
 #include <linux/gfp.h>
+#include <linux/highmem.h>
 #include <linux/swap.h>
 #include <linux/mman.h>
 #include <linux/nodemask.h>
@@ -27,11 +29,133 @@
 #include <asm/bootparam.h>
 #include <asm/page.h>
 #include <asm/sections.h>
+#include <asm/sysmem.h>
+
+struct sysmem_info sysmem __initdata;
+
+static void __init sysmem_dump(void)
+{
+	unsigned i;
+
+	pr_debug("Sysmem:\n");
+	for (i = 0; i < sysmem.nr_banks; ++i)
+		pr_debug("  0x%08lx - 0x%08lx (%ldK)\n",
+			 sysmem.bank[i].start, sysmem.bank[i].end,
+			 (sysmem.bank[i].end - sysmem.bank[i].start) >> 10);
+}
+
+/*
+ * Find bank with maximal .start such that bank.start <= start
+ */
+static inline struct meminfo * __init find_bank(unsigned long start)
+{
+	unsigned i;
+	struct meminfo *it = NULL;
+
+	for (i = 0; i < sysmem.nr_banks; ++i)
+		if (sysmem.bank[i].start <= start)
+			it = sysmem.bank + i;
+		else
+			break;
+	return it;
+}
+
+/*
+ * Move all memory banks starting at 'from' to a new place at 'to',
+ * adjust nr_banks accordingly.
+ * Both 'from' and 'to' must be inside the sysmem.bank.
+ *
+ * Returns: 0 (success), -ENOMEM (not enough space in the sysmem.bank).
+ */
+static int __init move_banks(struct meminfo *to, struct meminfo *from)
+{
+	unsigned n = sysmem.nr_banks - (from - sysmem.bank);
+
+	if (to > from && to - from + sysmem.nr_banks > SYSMEM_BANKS_MAX)
+		return -ENOMEM;
+	if (to != from)
+		memmove(to, from, n * sizeof(struct meminfo));
+	sysmem.nr_banks += to - from;
+	return 0;
+}
+
+/*
+ * Add new bank to sysmem. Resulting sysmem is the union of bytes of the
+ * original sysmem and the new bank.
+ *
+ * Returns: 0 (success), < 0 (error)
+ */
+int __init add_sysmem_bank(unsigned long start, unsigned long end)
+{
+	unsigned i;
+	struct meminfo *it = NULL;
+	unsigned long sz;
+	unsigned long bank_sz = 0;
+
+	if (start == end ||
+	    (start < end) != (PAGE_ALIGN(start) < (end & PAGE_MASK))) {
+		pr_warn("Ignoring small memory bank 0x%08lx size: %ld bytes\n",
+			start, end - start);
+		return -EINVAL;
+	}
+
+	start = PAGE_ALIGN(start);
+	end &= PAGE_MASK;
+	sz = end - start;
+
+	it = find_bank(start);
+
+	if (it)
+		bank_sz = it->end - it->start;
+
+	if (it && bank_sz >= start - it->start) {
+		if (end - it->start > bank_sz)
+			it->end = end;
+		else
+			return 0;
+	} else {
+		if (!it)
+			it = sysmem.bank;
+		else
+			++it;
+
+		if (it - sysmem.bank < sysmem.nr_banks &&
+		    it->start - start <= sz) {
+			it->start = start;
+			if (it->end - it->start < sz)
+				it->end = end;
+			else
+				return 0;
+		} else {
+			if (move_banks(it + 1, it) < 0) {
+				pr_warn("Ignoring memory bank 0x%08lx size %ld bytes\n",
+					start, end - start);
+				return -EINVAL;
+			}
+			it->start = start;
+			it->end = end;
+			return 0;
+		}
+	}
+	sz = it->end - it->start;
+	for (i = it + 1 - sysmem.bank; i < sysmem.nr_banks; ++i)
+		if (sysmem.bank[i].start - it->start <= sz) {
+			if (sz < sysmem.bank[i].end - it->start)
+				it->end = sysmem.bank[i].end;
+		} else {
+			break;
+		}
+
+	move_banks(it + 1, sysmem.bank + i);
+	return 0;
+}
 
 /*
  * mem_reserve(start, end, must_exist)
  *
  * Reserve some memory from the memory pool.
+ * If must_exist is set and a part of the region being reserved does not exist
+ * memory map is not altered.
  *
  * Parameters:
  *  start	Start of region,
@@ -39,53 +163,69 @@
  *  must_exist	Must exist in memory pool.
  *
  * Returns:
- *  0 (memory area couldn't be mapped)
- * -1 (success)
+ *  0 (success)
+ *  < 0 (error)
  */
 
 int __init mem_reserve(unsigned long start, unsigned long end, int must_exist)
 {
-	int i;
-
-	if (start == end)
-		return 0;
+	struct meminfo *it;
+	struct meminfo *rm = NULL;
+	unsigned long sz;
+	unsigned long bank_sz = 0;
 
 	start = start & PAGE_MASK;
 	end = PAGE_ALIGN(end);
+	sz = end - start;
+	if (!sz)
+		return -EINVAL;
 
-	for (i = 0; i < sysmem.nr_banks; i++)
-		if (start < sysmem.bank[i].end
-		    && end >= sysmem.bank[i].start)
-			break;
+	it = find_bank(start);
+
+	if (it)
+		bank_sz = it->end - it->start;
 
-	if (i == sysmem.nr_banks) {
-		if (must_exist)
-			printk (KERN_WARNING "mem_reserve: [0x%0lx, 0x%0lx) "
-				"not in any region!\n", start, end);
-		return 0;
+	if ((!it || end - it->start > bank_sz) && must_exist) {
+		pr_warn("mem_reserve: [0x%0lx, 0x%0lx) not in any region!\n",
+			start, end);
+		return -EINVAL;
 	}
 
-	if (start > sysmem.bank[i].start) {
-		if (end < sysmem.bank[i].end) {
-			/* split entry */
-			if (sysmem.nr_banks >= SYSMEM_BANKS_MAX)
-				panic("meminfo overflow\n");
-			sysmem.bank[sysmem.nr_banks].start = end;
-			sysmem.bank[sysmem.nr_banks].end = sysmem.bank[i].end;
-			sysmem.nr_banks++;
+	if (it && start - it->start < bank_sz) {
+		if (start == it->start) {
+			if (end - it->start < bank_sz) {
+				it->start = end;
+				return 0;
+			} else {
+				rm = it;
+			}
+		} else {
+			it->end = start;
+			if (end - it->start < bank_sz)
+				return add_sysmem_bank(end,
+						       it->start + bank_sz);
+			++it;
 		}
-		sysmem.bank[i].end = start;
+	}
 
-	} else if (end < sysmem.bank[i].end) {
-		sysmem.bank[i].start = end;
+	if (!it)
+		it = sysmem.bank;
 
-	} else {
-		/* remove entry */
-		sysmem.nr_banks--;
-		sysmem.bank[i].start = sysmem.bank[sysmem.nr_banks].start;
-		sysmem.bank[i].end   = sysmem.bank[sysmem.nr_banks].end;
+	for (; it < sysmem.bank + sysmem.nr_banks; ++it) {
+		if (it->end - start <= sz) {
+			if (!rm)
+				rm = it;
+		} else {
+			if (it->start - start < sz)
+				it->start = end;
+			break;
+		}
 	}
-	return -1;
+
+	if (rm)
+		move_banks(rm, it);
+
+	return 0;
 }
 
 
@@ -99,6 +239,7 @@ void __init bootmem_init(void)
 	unsigned long bootmap_start, bootmap_size;
 	int i;
 
+	sysmem_dump();
 	max_low_pfn = max_pfn = 0;
 	min_low_pfn = ~0;
 
@@ -156,19 +297,13 @@ void __init bootmem_init(void)
 
 void __init zones_init(void)
 {
-	unsigned long zones_size[MAX_NR_ZONES];
-	int i;
-
 	/* All pages are DMA-able, so we put them all in the DMA zone. */
-
-	zones_size[ZONE_DMA] = max_low_pfn - ARCH_PFN_OFFSET;
-	for (i = 1; i < MAX_NR_ZONES; i++)
-		zones_size[i] = 0;
-
+	unsigned long zones_size[MAX_NR_ZONES] = {
+		[ZONE_DMA] = max_low_pfn - ARCH_PFN_OFFSET,
 #ifdef CONFIG_HIGHMEM
-	zones_size[ZONE_HIGHMEM] = max_pfn - max_low_pfn;
+		[ZONE_HIGHMEM] = max_pfn - max_low_pfn,
 #endif
-
+	};
 	free_area_init_node(0, zones_size, ARCH_PFN_OFFSET, NULL);
 }
 
@@ -178,16 +313,38 @@ void __init zones_init(void)
 
 void __init mem_init(void)
 {
-	max_mapnr = max_low_pfn - ARCH_PFN_OFFSET;
-	high_memory = (void *) __va(max_low_pfn << PAGE_SHIFT);
-
 #ifdef CONFIG_HIGHMEM
-#error HIGHGMEM not implemented in init.c
+	unsigned long tmp;
+
+	reset_all_zones_managed_pages();
+	for (tmp = max_low_pfn; tmp < max_pfn; tmp++)
+		free_highmem_page(pfn_to_page(tmp));
 #endif
 
+	max_mapnr = max_pfn - ARCH_PFN_OFFSET;
+	high_memory = (void *)__va(max_low_pfn << PAGE_SHIFT);
+
 	free_all_bootmem();
 
 	mem_init_print_info(NULL);
+	pr_info("virtual kernel memory layout:\n"
+#ifdef CONFIG_HIGHMEM
+		"    pkmap   : 0x%08lx - 0x%08lx  (%5lu kB)\n"
+		"    fixmap  : 0x%08lx - 0x%08lx  (%5lu kB)\n"
+#endif
+		"    vmalloc : 0x%08x - 0x%08x  (%5u MB)\n"
+		"    lowmem  : 0x%08x - 0x%08lx  (%5lu MB)\n",
+#ifdef CONFIG_HIGHMEM
+		PKMAP_BASE, PKMAP_BASE + LAST_PKMAP * PAGE_SIZE,
+		(LAST_PKMAP*PAGE_SIZE) >> 10,
+		FIXADDR_START, FIXADDR_TOP,
+		(FIXADDR_TOP - FIXADDR_START) >> 10,
+#endif
+		VMALLOC_START, VMALLOC_END,
+		(VMALLOC_END - VMALLOC_START) >> 20,
+		PAGE_OFFSET, PAGE_OFFSET +
+		(max_low_pfn - min_low_pfn) * PAGE_SIZE,
+		((max_low_pfn - min_low_pfn) * PAGE_SIZE) >> 20);
 }
 
 #ifdef CONFIG_BLK_DEV_INITRD
@@ -204,3 +361,53 @@ void free_initmem(void)
 {
 	free_initmem_default(-1);
 }
+
+static void __init parse_memmap_one(char *p)
+{
+	char *oldp;
+	unsigned long start_at, mem_size;
+
+	if (!p)
+		return;
+
+	oldp = p;
+	mem_size = memparse(p, &p);
+	if (p == oldp)
+		return;
+
+	switch (*p) {
+	case '@':
+		start_at = memparse(p + 1, &p);
+		add_sysmem_bank(start_at, start_at + mem_size);
+		break;
+
+	case '$':
+		start_at = memparse(p + 1, &p);
+		mem_reserve(start_at, start_at + mem_size, 0);
+		break;
+
+	case 0:
+		mem_reserve(mem_size, 0, 0);
+		break;
+
+	default:
+		pr_warn("Unrecognized memmap syntax: %s\n", p);
+		break;
+	}
+}
+
+static int __init parse_memmap_opt(char *str)
+{
+	while (str) {
+		char *k = strchr(str, ',');
+
+		if (k)
+			*k++ = 0;
+
+		parse_memmap_one(str);
+		str = k;
+	}
+
+	return 0;
+}
+early_param("memmap", parse_memmap_opt);
diff --git a/arch/xtensa/mm/mmu.c b/arch/xtensa/mm/mmu.c
index 861203e958d..3429b483d9f 100644
--- a/arch/xtensa/mm/mmu.c
+++ b/arch/xtensa/mm/mmu.c
@@ -3,6 +3,7 @@
  *
  * Extracted from init.c
  */
+#include <linux/bootmem.h>
 #include <linux/percpu.h>
 #include <linux/init.h>
 #include <linux/string.h>
@@ -16,9 +17,44 @@
 #include <asm/initialize_mmu.h>
 #include <asm/io.h>
 
+#if defined(CONFIG_HIGHMEM)
+static void * __init init_pmd(unsigned long vaddr)
+{
+	pgd_t *pgd = pgd_offset_k(vaddr);
+	pmd_t *pmd = pmd_offset(pgd, vaddr);
+
+	if (pmd_none(*pmd)) {
+		unsigned i;
+		pte_t *pte = alloc_bootmem_low_pages(PAGE_SIZE);
+
+		for (i = 0; i < 1024; i++)
+			pte_clear(NULL, 0, pte + i);
+
+		set_pmd(pmd, __pmd(((unsigned long)pte) & PAGE_MASK));
+		BUG_ON(pte != pte_offset_kernel(pmd, 0));
+		pr_debug("%s: vaddr: 0x%08lx, pmd: 0x%p, pte: 0x%p\n",
+			 __func__, vaddr, pmd, pte);
+		return pte;
+	} else {
+		return pte_offset_kernel(pmd, 0);
+	}
+}
+
+static void __init fixedrange_init(void)
+{
+	BUILD_BUG_ON(FIXADDR_SIZE > PMD_SIZE);
+	init_pmd(__fix_to_virt(__end_of_fixed_addresses - 1) & PMD_MASK);
+}
+#endif
+
 void __init paging_init(void)
 {
 	memset(swapper_pg_dir, 0, PAGE_SIZE);
+#ifdef CONFIG_HIGHMEM
+	fixedrange_init();
+	pkmap_page_table = init_pmd(PKMAP_BASE);
+	kmap_init();
+#endif
 }
 
 /*
diff --git a/arch/xtensa/mm/tlb.c b/arch/xtensa/mm/tlb.c
index ade62382678..5ece856c572 100644
--- a/arch/xtensa/mm/tlb.c
+++ b/arch/xtensa/mm/tlb.c
@@ -149,6 +149,21 @@ void local_flush_tlb_page(struct vm_area_struct *vma, unsigned long page)
 	local_irq_restore(flags);
 }
 
+void local_flush_tlb_kernel_range(unsigned long start, unsigned long end)
+{
+	if (end > start && start >= TASK_SIZE && end <= PAGE_OFFSET &&
+	    end - start < _TLB_ENTRIES << PAGE_SHIFT) {
+		start &= PAGE_MASK;
+		while (start < end) {
+			invalidate_itlb_mapping(start);
+			invalidate_dtlb_mapping(start);
+			start += PAGE_SIZE;
+		}
+	} else {
+		local_flush_tlb_all();
+	}
+}
+
 #ifdef CONFIG_DEBUG_TLB_SANITY
 
 static unsigned get_pte_for_vaddr(unsigned vaddr)
diff --git a/arch/xtensa/platforms/iss/Makefile b/arch/xtensa/platforms/iss/Makefile
index d2369b799c5..b3e89291cfb 100644
--- a/arch/xtensa/platforms/iss/Makefile
+++ b/arch/xtensa/platforms/iss/Makefile
@@ -4,6 +4,7 @@
 # "prom monitor" library routines under Linux.
 #
 
-obj-y			= console.o setup.o
+obj-y			= setup.o
+obj-$(CONFIG_TTY)	+= console.o
 obj-$(CONFIG_NET)	+= network.o
 obj-$(CONFIG_BLK_DEV_SIMDISK) += simdisk.o
diff --git a/arch/xtensa/platforms/xt2000/setup.c b/arch/xtensa/platforms/xt2000/setup.c
index f9bc8796629..b90555cb808 100644
--- a/arch/xtensa/platforms/xt2000/setup.c
+++ b/arch/xtensa/platforms/xt2000/setup.c
@@ -92,18 +92,8 @@ void __init platform_setup(char** cmdline)
 
 /* early initialization */
 
-extern sysmem_info_t __initdata sysmem;
-
-void platform_init(bp_tag_t* first)
+void __init platform_init(bp_tag_t *first)
 {
-	/* Set default memory block if not provided by the bootloader. */
-
-	if (sysmem.nr_banks == 0) {
-		sysmem.nr_banks = 1;
-		sysmem.bank[0].start = PLATFORM_DEFAULT_MEM_START;
-		sysmem.bank[0].end = PLATFORM_DEFAULT_MEM_START
-				     + PLATFORM_DEFAULT_MEM_SIZE;
-	}
 }
 
 /* Heartbeat. Let the LED blink. */
diff --git a/block/Makefile b/block/Makefile
index 20645e88fb5..a2ce6ac935e 100644
--- a/block/Makefile
+++ b/block/Makefile
@@ -2,13 +2,15 @@
 # Makefile for the kernel block layer
 #
 
-obj-$(CONFIG_BLOCK) := elevator.o blk-core.o blk-tag.o blk-sysfs.o \
+obj-$(CONFIG_BLOCK) := bio.o elevator.o blk-core.o blk-tag.o blk-sysfs.o \
 			blk-flush.o blk-settings.o blk-ioc.o blk-map.o \
 			blk-exec.o blk-merge.o blk-softirq.o blk-timeout.o \
 			blk-iopoll.o blk-lib.o blk-mq.o blk-mq-tag.o \
 			blk-mq-sysfs.o blk-mq-cpu.o blk-mq-cpumap.o ioctl.o \
-			genhd.o scsi_ioctl.o partition-generic.o partitions/
+			genhd.o scsi_ioctl.o partition-generic.o ioprio.o \
+			partitions/
 
+obj-$(CONFIG_BOUNCE)	+= bounce.o
 obj-$(CONFIG_BLK_DEV_BSG)	+= bsg.o
 obj-$(CONFIG_BLK_DEV_BSGLIB)	+= bsg-lib.o
 obj-$(CONFIG_BLK_CGROUP)	+= blk-cgroup.o
@@ -20,3 +22,4 @@ obj-$(CONFIG_IOSCHED_CFQ)	+= cfq-iosched.o
 obj-$(CONFIG_BLOCK_COMPAT)	+= compat_ioctl.o
 obj-$(CONFIG_BLK_DEV_INTEGRITY)	+= blk-integrity.o
 obj-$(CONFIG_BLK_CMDLINE_PARSER)	+= cmdline-parser.o
+obj-$(CONFIG_BLK_DEV_INTEGRITY) += bio-integrity.o
diff --git a/fs/bio-integrity.c b/block/bio-integrity.c
index 1c2ce0c8771..9e241063a61 100644
--- a/fs/bio-integrity.c
+++ b/block/bio-integrity.c
@@ -617,7 +617,7 @@ int bioset_integrity_create(struct bio_set *bs, int pool_size)
 	if (!bs->bio_integrity_pool)
 		return -1;
 
-	bs->bvec_integrity_pool = biovec_create_pool(bs, pool_size);
+	bs->bvec_integrity_pool = biovec_create_pool(pool_size);
 	if (!bs->bvec_integrity_pool) {
 		mempool_destroy(bs->bio_integrity_pool);
 		return -1;
diff --git a/fs/bio.c b/block/bio.c
index 6f0362b7780..96d28eee8a1 100644
--- a/fs/bio.c
+++ b/block/bio.c
@@ -305,6 +305,8 @@ static void bio_chain_endio(struct bio *bio, int error)
 
 /**
  * bio_chain - chain bio completions
+ * @bio: the target bio
+ * @parent: the @bio's parent bio
  *
  * The caller won't have a bi_end_io called when @bio completes - instead,
  * @parent's bi_end_io won't be called until both @parent and @bio have
@@ -1011,8 +1013,7 @@ static void bio_set_map_data(struct bio_map_data *bmd, struct bio *bio,
 	bio->bi_private = bmd;
 }
 
-static struct bio_map_data *bio_alloc_map_data(int nr_segs,
-					       unsigned int iov_count,
+static struct bio_map_data *bio_alloc_map_data(unsigned int iov_count,
 					       gfp_t gfp_mask)
 {
 	if (iov_count > UIO_MAXIOV)
@@ -1154,7 +1155,7 @@ struct bio *bio_copy_user_iov(struct request_queue *q,
 	if (offset)
 		nr_pages++;
 
-	bmd = bio_alloc_map_data(nr_pages, iov_count, gfp_mask);
+	bmd = bio_alloc_map_data(iov_count, gfp_mask);
 	if (!bmd)
 		return ERR_PTR(-ENOMEM);
 
@@ -1859,7 +1860,7 @@ EXPORT_SYMBOL_GPL(bio_trim);
  * create memory pools for biovec's in a bio_set.
  * use the global biovec slabs created for general use.
  */
-mempool_t *biovec_create_pool(struct bio_set *bs, int pool_entries)
+mempool_t *biovec_create_pool(int pool_entries)
 {
 	struct biovec_slab *bp = bvec_slabs + BIOVEC_MAX_IDX;
 
@@ -1922,7 +1923,7 @@ struct bio_set *bioset_create(unsigned int pool_size, unsigned int front_pad)
 	if (!bs->bio_pool)
 		goto bad;
 
-	bs->bvec_pool = biovec_create_pool(bs, pool_size);
+	bs->bvec_pool = biovec_create_pool(pool_size);
 	if (!bs->bvec_pool)
 		goto bad;
 
diff --git a/block/blk-cgroup.c b/block/blk-cgroup.c
index e4a4145926f..1039fb9ff5f 100644
--- a/block/blk-cgroup.c
+++ b/block/blk-cgroup.c
@@ -451,7 +451,20 @@ static int blkcg_reset_stats(struct cgroup_subsys_state *css,
 	struct blkcg_gq *blkg;
 	int i;
 
-	mutex_lock(&blkcg_pol_mutex);
+	/*
+	 * XXX: We invoke cgroup_add/rm_cftypes() under blkcg_pol_mutex
+	 * which ends up putting cgroup's internal cgroup_tree_mutex under
+	 * it; however, cgroup_tree_mutex is nested above cgroup file
+	 * active protection and grabbing blkcg_pol_mutex from a cgroup
+	 * file operation creates a possible circular dependency.  cgroup
+	 * internal locking is planned to go through further simplification
+	 * and this issue should go away soon.  For now, let's trylock
+	 * blkcg_pol_mutex and restart the write on failure.
+	 *
+	 * http://lkml.kernel.org/g/5363C04B.4010400@oracle.com
+	 */
+	if (!mutex_trylock(&blkcg_pol_mutex))
+		return restart_syscall();
 	spin_lock_irq(&blkcg->lock);
 
 	/*
diff --git a/block/blk-core.c b/block/blk-core.c
index a0e3096c4bb..40d654861c3 100644
--- a/block/blk-core.c
+++ b/block/blk-core.c
@@ -146,8 +146,8 @@ void blk_dump_rq_flags(struct request *rq, char *msg)
 	printk(KERN_INFO "  sector %llu, nr/cnr %u/%u\n",
 	       (unsigned long long)blk_rq_pos(rq),
 	       blk_rq_sectors(rq), blk_rq_cur_sectors(rq));
-	printk(KERN_INFO "  bio %p, biotail %p, buffer %p, len %u\n",
-	       rq->bio, rq->biotail, rq->buffer, blk_rq_bytes(rq));
+	printk(KERN_INFO "  bio %p, biotail %p, len %u\n",
+	       rq->bio, rq->biotail, blk_rq_bytes(rq));
 
 	if (rq->cmd_type == REQ_TYPE_BLOCK_PC) {
 		printk(KERN_INFO "  cdb: ");
@@ -251,8 +251,10 @@ void blk_sync_queue(struct request_queue *q)
 		struct blk_mq_hw_ctx *hctx;
 		int i;
 
-		queue_for_each_hw_ctx(q, hctx, i)
-			cancel_delayed_work_sync(&hctx->delayed_work);
+		queue_for_each_hw_ctx(q, hctx, i) {
+			cancel_delayed_work_sync(&hctx->run_work);
+			cancel_delayed_work_sync(&hctx->delay_work);
+		}
 	} else {
 		cancel_delayed_work_sync(&q->delay_work);
 	}
@@ -574,12 +576,9 @@ struct request_queue *blk_alloc_queue_node(gfp_t gfp_mask, int node_id)
 	if (!q)
 		return NULL;
 
-	if (percpu_counter_init(&q->mq_usage_counter, 0))
-		goto fail_q;
-
 	q->id = ida_simple_get(&blk_queue_ida, 0, 0, gfp_mask);
 	if (q->id < 0)
-		goto fail_c;
+		goto fail_q;
 
 	q->backing_dev_info.ra_pages =
 			(VM_MAX_READAHEAD * 1024) / PAGE_CACHE_SIZE;
@@ -637,8 +636,6 @@ fail_bdi:
 	bdi_destroy(&q->backing_dev_info);
 fail_id:
 	ida_simple_remove(&blk_queue_ida, q->id);
-fail_c:
-	percpu_counter_destroy(&q->mq_usage_counter);
 fail_q:
 	kmem_cache_free(blk_requestq_cachep, q);
 	return NULL;
@@ -846,6 +843,47 @@ static void freed_request(struct request_list *rl, unsigned int flags)
 		__freed_request(rl, sync ^ 1);
 }
 
+int blk_update_nr_requests(struct request_queue *q, unsigned int nr)
+{
+	struct request_list *rl;
+
+	spin_lock_irq(q->queue_lock);
+	q->nr_requests = nr;
+	blk_queue_congestion_threshold(q);
+
+	/* congestion isn't cgroup aware and follows root blkcg for now */
+	rl = &q->root_rl;
+
+	if (rl->count[BLK_RW_SYNC] >= queue_congestion_on_threshold(q))
+		blk_set_queue_congested(q, BLK_RW_SYNC);
+	else if (rl->count[BLK_RW_SYNC] < queue_congestion_off_threshold(q))
+		blk_clear_queue_congested(q, BLK_RW_SYNC);
+
+	if (rl->count[BLK_RW_ASYNC] >= queue_congestion_on_threshold(q))
+		blk_set_queue_congested(q, BLK_RW_ASYNC);
+	else if (rl->count[BLK_RW_ASYNC] < queue_congestion_off_threshold(q))
+		blk_clear_queue_congested(q, BLK_RW_ASYNC);
+
+	blk_queue_for_each_rl(rl, q) {
+		if (rl->count[BLK_RW_SYNC] >= q->nr_requests) {
+			blk_set_rl_full(rl, BLK_RW_SYNC);
+		} else {
+			blk_clear_rl_full(rl, BLK_RW_SYNC);
+			wake_up(&rl->wait[BLK_RW_SYNC]);
+		}
+
+		if (rl->count[BLK_RW_ASYNC] >= q->nr_requests) {
+			blk_set_rl_full(rl, BLK_RW_ASYNC);
+		} else {
+			blk_clear_rl_full(rl, BLK_RW_ASYNC);
+			wake_up(&rl->wait[BLK_RW_ASYNC]);
+		}
+	}
+
+	spin_unlock_irq(q->queue_lock);
+	return 0;
+}
+
 /*
  * Determine if elevator data should be initialized when allocating the
  * request associated with @bio.
@@ -1135,7 +1173,7 @@ static struct request *blk_old_get_request(struct request_queue *q, int rw,
 struct request *blk_get_request(struct request_queue *q, int rw, gfp_t gfp_mask)
 {
 	if (q->mq_ops)
-		return blk_mq_alloc_request(q, rw, gfp_mask);
+		return blk_mq_alloc_request(q, rw, gfp_mask, false);
 	else
 		return blk_old_get_request(q, rw, gfp_mask);
 }
@@ -1231,12 +1269,15 @@ static void add_acct_request(struct request_queue *q, struct request *rq,
 static void part_round_stats_single(int cpu, struct hd_struct *part,
 				    unsigned long now)
 {
+	int inflight;
+
 	if (now == part->stamp)
 		return;
 
-	if (part_in_flight(part)) {
+	inflight = part_in_flight(part);
+	if (inflight) {
 		__part_stat_add(cpu, part, time_in_queue,
-				part_in_flight(part) * (now - part->stamp));
+				inflight * (now - part->stamp));
 		__part_stat_add(cpu, part, io_ticks, (now - part->stamp));
 	}
 	part->stamp = now;
@@ -1360,7 +1401,6 @@ void blk_add_request_payload(struct request *rq, struct page *page,
 
 	rq->__data_len = rq->resid_len = len;
 	rq->nr_phys_segments = 1;
-	rq->buffer = bio_data(bio);
 }
 EXPORT_SYMBOL_GPL(blk_add_request_payload);
 
@@ -1402,12 +1442,6 @@ bool bio_attempt_front_merge(struct request_queue *q, struct request *req,
 	bio->bi_next = req->bio;
 	req->bio = bio;
 
-	/*
-	 * may not be valid. if the low level driver said
-	 * it didn't need a bounce buffer then it better
-	 * not touch req->buffer either...
-	 */
-	req->buffer = bio_data(bio);
 	req->__sector = bio->bi_iter.bi_sector;
 	req->__data_len += bio->bi_iter.bi_size;
 	req->ioprio = ioprio_best(req->ioprio, bio_prio(bio));
@@ -1432,6 +1466,8 @@ bool bio_attempt_front_merge(struct request_queue *q, struct request *req,
  * added on the elevator at this point.  In addition, we don't have
  * reliable access to the elevator outside queue lock.  Only check basic
  * merging parameters without querying the elevator.
+ *
+ * Caller must ensure !blk_queue_nomerges(q) beforehand.
  */
 bool blk_attempt_plug_merge(struct request_queue *q, struct bio *bio,
 			    unsigned int *request_count)
@@ -1441,9 +1477,6 @@ bool blk_attempt_plug_merge(struct request_queue *q, struct bio *bio,
 	bool ret = false;
 	struct list_head *plug_list;
 
-	if (blk_queue_nomerges(q))
-		goto out;
-
 	plug = current->plug;
 	if (!plug)
 		goto out;
@@ -1522,7 +1555,8 @@ void blk_queue_bio(struct request_queue *q, struct bio *bio)
 	 * Check if we can merge with the plugged list before grabbing
 	 * any locks.
 	 */
-	if (blk_attempt_plug_merge(q, bio, &request_count))
+	if (!blk_queue_nomerges(q) &&
+	    blk_attempt_plug_merge(q, bio, &request_count))
 		return;
 
 	spin_lock_irq(q->queue_lock);
@@ -1654,7 +1688,7 @@ static int __init fail_make_request_debugfs(void)
 	struct dentry *dir = fault_create_debugfs_attr("fail_make_request",
 						NULL, &fail_make_request);
 
-	return IS_ERR(dir) ? PTR_ERR(dir) : 0;
+	return PTR_ERR_OR_ZERO(dir);
 }
 
 late_initcall(fail_make_request_debugfs);
@@ -2434,7 +2468,6 @@ bool blk_update_request(struct request *req, int error, unsigned int nr_bytes)
 	}
 
 	req->__data_len -= total_bytes;
-	req->buffer = bio_data(req->bio);
 
 	/* update sector only for requests with clear definition of sector */
 	if (req->cmd_type == REQ_TYPE_FS)
@@ -2503,7 +2536,7 @@ EXPORT_SYMBOL_GPL(blk_unprep_request);
 /*
  * queue lock must be held
  */
-static void blk_finish_request(struct request *req, int error)
+void blk_finish_request(struct request *req, int error)
 {
 	if (blk_rq_tagged(req))
 		blk_queue_end_tag(req->q, req);
@@ -2529,6 +2562,7 @@ static void blk_finish_request(struct request *req, int error)
 		__blk_put_request(req->q, req);
 	}
 }
+EXPORT_SYMBOL(blk_finish_request);
 
 /**
  * blk_end_bidi_request - Complete a bidi request
@@ -2752,10 +2786,9 @@ void blk_rq_bio_prep(struct request_queue *q, struct request *rq,
 	/* Bit 0 (R/W) is identical in rq->cmd_flags and bio->bi_rw */
 	rq->cmd_flags |= bio->bi_rw & REQ_WRITE;
 
-	if (bio_has_data(bio)) {
+	if (bio_has_data(bio))
 		rq->nr_phys_segments = bio_phys_segments(q, bio);
-		rq->buffer = bio_data(bio);
-	}
+
 	rq->__data_len = bio->bi_iter.bi_size;
 	rq->bio = rq->biotail = bio;
 
@@ -2831,7 +2864,7 @@ EXPORT_SYMBOL_GPL(blk_rq_unprep_clone);
 
 /*
  * Copy attributes of the original request to the clone request.
- * The actual data parts (e.g. ->cmd, ->buffer, ->sense) are not copied.
+ * The actual data parts (e.g. ->cmd, ->sense) are not copied.
  */
 static void __blk_rq_prep_clone(struct request *dst, struct request *src)
 {
@@ -2857,7 +2890,7 @@ static void __blk_rq_prep_clone(struct request *dst, struct request *src)
  *
  * Description:
  *     Clones bios in @rq_src to @rq, and copies attributes of @rq_src to @rq.
- *     The actual data parts of @rq_src (e.g. ->cmd, ->buffer, ->sense)
+ *     The actual data parts of @rq_src (e.g. ->cmd, ->sense)
  *     are not copied, and copying such parts is the caller's responsibility.
  *     Also, pages which the original bios are pointing to are not copied
  *     and the cloned bios just point same pages.
@@ -2904,20 +2937,25 @@ free_and_out:
 }
 EXPORT_SYMBOL_GPL(blk_rq_prep_clone);
 
-int kblockd_schedule_work(struct request_queue *q, struct work_struct *work)
+int kblockd_schedule_work(struct work_struct *work)
 {
 	return queue_work(kblockd_workqueue, work);
 }
 EXPORT_SYMBOL(kblockd_schedule_work);
 
-int kblockd_schedule_delayed_work(struct request_queue *q,
-			struct delayed_work *dwork, unsigned long delay)
+int kblockd_schedule_delayed_work(struct delayed_work *dwork,
+				  unsigned long delay)
 {
 	return queue_delayed_work(kblockd_workqueue, dwork, delay);
 }
 EXPORT_SYMBOL(kblockd_schedule_delayed_work);
 
-#define PLUG_MAGIC	0x91827364
+int kblockd_schedule_delayed_work_on(int cpu, struct delayed_work *dwork,
+				     unsigned long delay)
+{
+	return queue_delayed_work_on(cpu, kblockd_workqueue, dwork, delay);
+}
+EXPORT_SYMBOL(kblockd_schedule_delayed_work_on);
 
 /**
  * blk_start_plug - initialize blk_plug and track it inside the task_struct
@@ -2937,7 +2975,6 @@ void blk_start_plug(struct blk_plug *plug)
 {
 	struct task_struct *tsk = current;
 
-	plug->magic = PLUG_MAGIC;
 	INIT_LIST_HEAD(&plug->list);
 	INIT_LIST_HEAD(&plug->mq_list);
 	INIT_LIST_HEAD(&plug->cb_list);
@@ -3034,8 +3071,6 @@ void blk_flush_plug_list(struct blk_plug *plug, bool from_schedule)
 	LIST_HEAD(list);
 	unsigned int depth;
 
-	BUG_ON(plug->magic != PLUG_MAGIC);
-
 	flush_plug_callbacks(plug, from_schedule);
 
 	if (!list_empty(&plug->mq_list))
diff --git a/block/blk-flush.c b/block/blk-flush.c
index 43e6b4755e9..ff87c664b7d 100644
--- a/block/blk-flush.c
+++ b/block/blk-flush.c
@@ -130,21 +130,13 @@ static void blk_flush_restore_request(struct request *rq)
 	blk_clear_rq_complete(rq);
 }
 
-static void mq_flush_run(struct work_struct *work)
-{
-	struct request *rq;
-
-	rq = container_of(work, struct request, mq_flush_work);
-
-	memset(&rq->csd, 0, sizeof(rq->csd));
-	blk_mq_insert_request(rq, false, true, false);
-}
-
 static bool blk_flush_queue_rq(struct request *rq, bool add_front)
 {
 	if (rq->q->mq_ops) {
-		INIT_WORK(&rq->mq_flush_work, mq_flush_run);
-		kblockd_schedule_work(rq->q, &rq->mq_flush_work);
+		struct request_queue *q = rq->q;
+
+		blk_mq_add_to_requeue_list(rq, add_front);
+		blk_mq_kick_requeue_list(q);
 		return false;
 	} else {
 		if (add_front)
@@ -231,8 +223,10 @@ static void flush_end_io(struct request *flush_rq, int error)
 	struct request *rq, *n;
 	unsigned long flags = 0;
 
-	if (q->mq_ops)
+	if (q->mq_ops) {
 		spin_lock_irqsave(&q->mq_flush_lock, flags);
+		q->flush_rq->cmd_flags = 0;
+	}
 
 	running = &q->flush_queue[q->flush_running_idx];
 	BUG_ON(q->flush_pending_idx == q->flush_running_idx);
@@ -306,23 +300,9 @@ static bool blk_kick_flush(struct request_queue *q)
 	 */
 	q->flush_pending_idx ^= 1;
 
-	if (q->mq_ops) {
-		struct blk_mq_ctx *ctx = first_rq->mq_ctx;
-		struct blk_mq_hw_ctx *hctx = q->mq_ops->map_queue(q, ctx->cpu);
-
-		blk_mq_rq_init(hctx, q->flush_rq);
-		q->flush_rq->mq_ctx = ctx;
-
-		/*
-		 * Reuse the tag value from the fist waiting request,
-		 * with blk-mq the tag is generated during request
-		 * allocation and drivers can rely on it being inside
-		 * the range they asked for.
-		 */
-		q->flush_rq->tag = first_rq->tag;
-	} else {
-		blk_rq_init(q, q->flush_rq);
-	}
+	blk_rq_init(q, q->flush_rq);
+	if (q->mq_ops)
+		blk_mq_clone_flush_request(q->flush_rq, first_rq);
 
 	q->flush_rq->cmd_type = REQ_TYPE_FS;
 	q->flush_rq->cmd_flags = WRITE_FLUSH | REQ_FLUSH_SEQ;
diff --git a/block/blk-iopoll.c b/block/blk-iopoll.c
index f8c6a11b13f..0736729d649 100644
--- a/block/blk-iopoll.c
+++ b/block/blk-iopoll.c
@@ -64,12 +64,12 @@ EXPORT_SYMBOL(__blk_iopoll_complete);
  *     iopoll handler will not be invoked again before blk_iopoll_sched_prep()
  *     is called.
  **/
-void blk_iopoll_complete(struct blk_iopoll *iopoll)
+void blk_iopoll_complete(struct blk_iopoll *iop)
 {
 	unsigned long flags;
 
 	local_irq_save(flags);
-	__blk_iopoll_complete(iopoll);
+	__blk_iopoll_complete(iop);
 	local_irq_restore(flags);
 }
 EXPORT_SYMBOL(blk_iopoll_complete);
diff --git a/block/blk-lib.c b/block/blk-lib.c
index 97a733cf3d5..8411be3c19d 100644
--- a/block/blk-lib.c
+++ b/block/blk-lib.c
@@ -226,8 +226,8 @@ EXPORT_SYMBOL(blkdev_issue_write_same);
  *  Generate and issue number of bios with zerofiled pages.
  */
 
-int __blkdev_issue_zeroout(struct block_device *bdev, sector_t sector,
-			sector_t nr_sects, gfp_t gfp_mask)
+static int __blkdev_issue_zeroout(struct block_device *bdev, sector_t sector,
+				  sector_t nr_sects, gfp_t gfp_mask)
 {
 	int ret;
 	struct bio *bio;
diff --git a/block/blk-map.c b/block/blk-map.c
index f7b22bc2151..f890d4345b0 100644
--- a/block/blk-map.c
+++ b/block/blk-map.c
@@ -155,7 +155,6 @@ int blk_rq_map_user(struct request_queue *q, struct request *rq,
 	if (!bio_flagged(bio, BIO_USER_MAPPED))
 		rq->cmd_flags |= REQ_COPY_USER;
 
-	rq->buffer = NULL;
 	return 0;
 unmap_rq:
 	blk_rq_unmap_user(bio);
@@ -238,7 +237,6 @@ int blk_rq_map_user_iov(struct request_queue *q, struct request *rq,
 	blk_queue_bounce(q, &bio);
 	bio_get(bio);
 	blk_rq_bio_prep(q, rq, bio);
-	rq->buffer = NULL;
 	return 0;
 }
 EXPORT_SYMBOL(blk_rq_map_user_iov);
@@ -325,7 +323,6 @@ int blk_rq_map_kern(struct request_queue *q, struct request *rq, void *kbuf,
 	}
 
 	blk_queue_bounce(q, &rq->bio);
-	rq->buffer = NULL;
 	return 0;
 }
 EXPORT_SYMBOL(blk_rq_map_kern);
diff --git a/block/blk-merge.c b/block/blk-merge.c
index 6c583f9c5b6..b3bf0df0f4c 100644
--- a/block/blk-merge.c
+++ b/block/blk-merge.c
@@ -13,7 +13,7 @@ static unsigned int __blk_recalc_rq_segments(struct request_queue *q,
 					     struct bio *bio)
 {
 	struct bio_vec bv, bvprv = { NULL };
-	int cluster, high, highprv = 1;
+	int cluster, high, highprv = 1, no_sg_merge;
 	unsigned int seg_size, nr_phys_segs;
 	struct bio *fbio, *bbio;
 	struct bvec_iter iter;
@@ -35,12 +35,21 @@ static unsigned int __blk_recalc_rq_segments(struct request_queue *q,
 	cluster = blk_queue_cluster(q);
 	seg_size = 0;
 	nr_phys_segs = 0;
+	no_sg_merge = test_bit(QUEUE_FLAG_NO_SG_MERGE, &q->queue_flags);
+	high = 0;
 	for_each_bio(bio) {
 		bio_for_each_segment(bv, bio, iter) {
 			/*
+			 * If SG merging is disabled, each bio vector is
+			 * a segment
+			 */
+			if (no_sg_merge)
+				goto new_segment;
+
+			/*
 			 * the trick here is making sure that a high page is
-			 * never considered part of another segment, since that
-			 * might change with the bounce page.
+			 * never considered part of another segment, since
+			 * that might change with the bounce page.
 			 */
 			high = page_to_pfn(bv.bv_page) > queue_bounce_pfn(q);
 			if (!high && !highprv && cluster) {
@@ -84,11 +93,16 @@ void blk_recalc_rq_segments(struct request *rq)
 
 void blk_recount_segments(struct request_queue *q, struct bio *bio)
 {
-	struct bio *nxt = bio->bi_next;
+	if (test_bit(QUEUE_FLAG_NO_SG_MERGE, &q->queue_flags))
+		bio->bi_phys_segments = bio->bi_vcnt;
+	else {
+		struct bio *nxt = bio->bi_next;
+
+		bio->bi_next = NULL;
+		bio->bi_phys_segments = __blk_recalc_rq_segments(q, bio);
+		bio->bi_next = nxt;
+	}
 
-	bio->bi_next = NULL;
-	bio->bi_phys_segments = __blk_recalc_rq_segments(q, bio);
-	bio->bi_next = nxt;
 	bio->bi_flags |= (1 << BIO_SEG_VALID);
 }
 EXPORT_SYMBOL(blk_recount_segments);
diff --git a/block/blk-mq-cpu.c b/block/blk-mq-cpu.c
index 136ef8643bb..bb3ed488f7b 100644
--- a/block/blk-mq-cpu.c
+++ b/block/blk-mq-cpu.c
@@ -1,3 +1,8 @@
+/*
+ * CPU notifier helper code for blk-mq
+ *
+ * Copyright (C) 2013-2014 Jens Axboe
+ */
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/init.h>
@@ -18,14 +23,18 @@ static int blk_mq_main_cpu_notify(struct notifier_block *self,
 {
 	unsigned int cpu = (unsigned long) hcpu;
 	struct blk_mq_cpu_notifier *notify;
+	int ret = NOTIFY_OK;
 
 	raw_spin_lock(&blk_mq_cpu_notify_lock);
 
-	list_for_each_entry(notify, &blk_mq_cpu_notify_list, list)
-		notify->notify(notify->data, action, cpu);
+	list_for_each_entry(notify, &blk_mq_cpu_notify_list, list) {
+		ret = notify->notify(notify->data, action, cpu);
+		if (ret != NOTIFY_OK)
+			break;
+	}
 
 	raw_spin_unlock(&blk_mq_cpu_notify_lock);
-	return NOTIFY_OK;
+	return ret;
 }
 
 void blk_mq_register_cpu_notifier(struct blk_mq_cpu_notifier *notifier)
@@ -45,7 +54,7 @@ void blk_mq_unregister_cpu_notifier(struct blk_mq_cpu_notifier *notifier)
 }
 
 void blk_mq_init_cpu_notifier(struct blk_mq_cpu_notifier *notifier,
-			      void (*fn)(void *, unsigned long, unsigned int),
+			      int (*fn)(void *, unsigned long, unsigned int),
 			      void *data)
 {
 	notifier->notify = fn;
diff --git a/block/blk-mq-cpumap.c b/block/blk-mq-cpumap.c
index 09792132961..1065d7c65fa 100644
--- a/block/blk-mq-cpumap.c
+++ b/block/blk-mq-cpumap.c
@@ -1,3 +1,8 @@
+/*
+ * CPU <-> hardware queue mapping helpers
+ *
+ * Copyright (C) 2013-2014 Jens Axboe
+ */
 #include <linux/kernel.h>
 #include <linux/threads.h>
 #include <linux/module.h>
@@ -80,19 +85,35 @@ int blk_mq_update_queue_map(unsigned int *map, unsigned int nr_queues)
 	return 0;
 }
 
-unsigned int *blk_mq_make_queue_map(struct blk_mq_reg *reg)
+unsigned int *blk_mq_make_queue_map(struct blk_mq_tag_set *set)
 {
 	unsigned int *map;
 
 	/* If cpus are offline, map them to first hctx */
 	map = kzalloc_node(sizeof(*map) * num_possible_cpus(), GFP_KERNEL,
-				reg->numa_node);
+				set->numa_node);
 	if (!map)
 		return NULL;
 
-	if (!blk_mq_update_queue_map(map, reg->nr_hw_queues))
+	if (!blk_mq_update_queue_map(map, set->nr_hw_queues))
 		return map;
 
 	kfree(map);
 	return NULL;
 }
+
+/*
+ * We have no quick way of doing reverse lookups. This is only used at
+ * queue init time, so runtime isn't important.
+ */
+int blk_mq_hw_queue_to_node(unsigned int *mq_map, unsigned int index)
+{
+	int i;
+
+	for_each_possible_cpu(i) {
+		if (index == mq_map[i])
+			return cpu_to_node(i);
+	}
+
+	return NUMA_NO_NODE;
+}
diff --git a/block/blk-mq-sysfs.c b/block/blk-mq-sysfs.c
index b0ba264b052..ed521786755 100644
--- a/block/blk-mq-sysfs.c
+++ b/block/blk-mq-sysfs.c
@@ -203,59 +203,24 @@ static ssize_t blk_mq_hw_sysfs_rq_list_show(struct blk_mq_hw_ctx *hctx,
 	return ret;
 }
 
-static ssize_t blk_mq_hw_sysfs_ipi_show(struct blk_mq_hw_ctx *hctx, char *page)
-{
-	ssize_t ret;
-
-	spin_lock(&hctx->lock);
-	ret = sprintf(page, "%u\n", !!(hctx->flags & BLK_MQ_F_SHOULD_IPI));
-	spin_unlock(&hctx->lock);
-
-	return ret;
-}
-
-static ssize_t blk_mq_hw_sysfs_ipi_store(struct blk_mq_hw_ctx *hctx,
-					 const char *page, size_t len)
+static ssize_t blk_mq_hw_sysfs_tags_show(struct blk_mq_hw_ctx *hctx, char *page)
 {
-	struct blk_mq_ctx *ctx;
-	unsigned long ret;
-	unsigned int i;
-
-	if (kstrtoul(page, 10, &ret)) {
-		pr_err("blk-mq-sysfs: invalid input '%s'\n", page);
-		return -EINVAL;
-	}
-
-	spin_lock(&hctx->lock);
-	if (ret)
-		hctx->flags |= BLK_MQ_F_SHOULD_IPI;
-	else
-		hctx->flags &= ~BLK_MQ_F_SHOULD_IPI;
-	spin_unlock(&hctx->lock);
-
-	hctx_for_each_ctx(hctx, ctx, i)
-		ctx->ipi_redirect = !!ret;
-
-	return len;
+	return blk_mq_tag_sysfs_show(hctx->tags, page);
 }
 
-static ssize_t blk_mq_hw_sysfs_tags_show(struct blk_mq_hw_ctx *hctx, char *page)
+static ssize_t blk_mq_hw_sysfs_active_show(struct blk_mq_hw_ctx *hctx, char *page)
 {
-	return blk_mq_tag_sysfs_show(hctx->tags, page);
+	return sprintf(page, "%u\n", atomic_read(&hctx->nr_active));
 }
 
 static ssize_t blk_mq_hw_sysfs_cpus_show(struct blk_mq_hw_ctx *hctx, char *page)
 {
-	unsigned int i, queue_num, first = 1;
+	unsigned int i, first = 1;
 	ssize_t ret = 0;
 
 	blk_mq_disable_hotplug();
 
-	for_each_online_cpu(i) {
-		queue_num = hctx->queue->mq_map[i];
-		if (queue_num != hctx->queue_num)
-			continue;
-
+	for_each_cpu(i, hctx->cpumask) {
 		if (first)
 			ret += sprintf(ret + page, "%u", i);
 		else
@@ -307,15 +272,14 @@ static struct blk_mq_hw_ctx_sysfs_entry blk_mq_hw_sysfs_dispatched = {
 	.attr = {.name = "dispatched", .mode = S_IRUGO },
 	.show = blk_mq_hw_sysfs_dispatched_show,
 };
+static struct blk_mq_hw_ctx_sysfs_entry blk_mq_hw_sysfs_active = {
+	.attr = {.name = "active", .mode = S_IRUGO },
+	.show = blk_mq_hw_sysfs_active_show,
+};
 static struct blk_mq_hw_ctx_sysfs_entry blk_mq_hw_sysfs_pending = {
 	.attr = {.name = "pending", .mode = S_IRUGO },
 	.show = blk_mq_hw_sysfs_rq_list_show,
 };
-static struct blk_mq_hw_ctx_sysfs_entry blk_mq_hw_sysfs_ipi = {
-	.attr = {.name = "ipi_redirect", .mode = S_IRUGO | S_IWUSR},
-	.show = blk_mq_hw_sysfs_ipi_show,
-	.store = blk_mq_hw_sysfs_ipi_store,
-};
 static struct blk_mq_hw_ctx_sysfs_entry blk_mq_hw_sysfs_tags = {
 	.attr = {.name = "tags", .mode = S_IRUGO },
 	.show = blk_mq_hw_sysfs_tags_show,
@@ -330,9 +294,9 @@ static struct attribute *default_hw_ctx_attrs[] = {
 	&blk_mq_hw_sysfs_run.attr,
 	&blk_mq_hw_sysfs_dispatched.attr,
 	&blk_mq_hw_sysfs_pending.attr,
-	&blk_mq_hw_sysfs_ipi.attr,
 	&blk_mq_hw_sysfs_tags.attr,
 	&blk_mq_hw_sysfs_cpus.attr,
+	&blk_mq_hw_sysfs_active.attr,
 	NULL,
 };
 
@@ -363,6 +327,42 @@ static struct kobj_type blk_mq_hw_ktype = {
 	.release	= blk_mq_sysfs_release,
 };
 
+static void blk_mq_unregister_hctx(struct blk_mq_hw_ctx *hctx)
+{
+	struct blk_mq_ctx *ctx;
+	int i;
+
+	if (!hctx->nr_ctx || !(hctx->flags & BLK_MQ_F_SYSFS_UP))
+		return;
+
+	hctx_for_each_ctx(hctx, ctx, i)
+		kobject_del(&ctx->kobj);
+
+	kobject_del(&hctx->kobj);
+}
+
+static int blk_mq_register_hctx(struct blk_mq_hw_ctx *hctx)
+{
+	struct request_queue *q = hctx->queue;
+	struct blk_mq_ctx *ctx;
+	int i, ret;
+
+	if (!hctx->nr_ctx || !(hctx->flags & BLK_MQ_F_SYSFS_UP))
+		return 0;
+
+	ret = kobject_add(&hctx->kobj, &q->mq_kobj, "%u", hctx->queue_num);
+	if (ret)
+		return ret;
+
+	hctx_for_each_ctx(hctx, ctx, i) {
+		ret = kobject_add(&ctx->kobj, &hctx->kobj, "cpu%u", ctx->cpu);
+		if (ret)
+			break;
+	}
+
+	return ret;
+}
+
 void blk_mq_unregister_disk(struct gendisk *disk)
 {
 	struct request_queue *q = disk->queue;
@@ -371,11 +371,11 @@ void blk_mq_unregister_disk(struct gendisk *disk)
 	int i, j;
 
 	queue_for_each_hw_ctx(q, hctx, i) {
-		hctx_for_each_ctx(hctx, ctx, j) {
-			kobject_del(&ctx->kobj);
+		blk_mq_unregister_hctx(hctx);
+
+		hctx_for_each_ctx(hctx, ctx, j)
 			kobject_put(&ctx->kobj);
-		}
-		kobject_del(&hctx->kobj);
+
 		kobject_put(&hctx->kobj);
 	}
 
@@ -386,15 +386,30 @@ void blk_mq_unregister_disk(struct gendisk *disk)
 	kobject_put(&disk_to_dev(disk)->kobj);
 }
 
+static void blk_mq_sysfs_init(struct request_queue *q)
+{
+	struct blk_mq_hw_ctx *hctx;
+	struct blk_mq_ctx *ctx;
+	int i, j;
+
+	kobject_init(&q->mq_kobj, &blk_mq_ktype);
+
+	queue_for_each_hw_ctx(q, hctx, i) {
+		kobject_init(&hctx->kobj, &blk_mq_hw_ktype);
+
+		hctx_for_each_ctx(hctx, ctx, j)
+			kobject_init(&ctx->kobj, &blk_mq_ctx_ktype);
+	}
+}
+
 int blk_mq_register_disk(struct gendisk *disk)
 {
 	struct device *dev = disk_to_dev(disk);
 	struct request_queue *q = disk->queue;
 	struct blk_mq_hw_ctx *hctx;
-	struct blk_mq_ctx *ctx;
-	int ret, i, j;
+	int ret, i;
 
-	kobject_init(&q->mq_kobj, &blk_mq_ktype);
+	blk_mq_sysfs_init(q);
 
 	ret = kobject_add(&q->mq_kobj, kobject_get(&dev->kobj), "%s", "mq");
 	if (ret < 0)
@@ -403,20 +418,10 @@ int blk_mq_register_disk(struct gendisk *disk)
 	kobject_uevent(&q->mq_kobj, KOBJ_ADD);
 
 	queue_for_each_hw_ctx(q, hctx, i) {
-		kobject_init(&hctx->kobj, &blk_mq_hw_ktype);
-		ret = kobject_add(&hctx->kobj, &q->mq_kobj, "%u", i);
+		hctx->flags |= BLK_MQ_F_SYSFS_UP;
+		ret = blk_mq_register_hctx(hctx);
 		if (ret)
 			break;
-
-		if (!hctx->nr_ctx)
-			continue;
-
-		hctx_for_each_ctx(hctx, ctx, j) {
-			kobject_init(&ctx->kobj, &blk_mq_ctx_ktype);
-			ret = kobject_add(&ctx->kobj, &hctx->kobj, "cpu%u", ctx->cpu);
-			if (ret)
-				break;
-		}
 	}
 
 	if (ret) {
@@ -426,3 +431,26 @@ int blk_mq_register_disk(struct gendisk *disk)
 
 	return 0;
 }
+
+void blk_mq_sysfs_unregister(struct request_queue *q)
+{
+	struct blk_mq_hw_ctx *hctx;
+	int i;
+
+	queue_for_each_hw_ctx(q, hctx, i)
+		blk_mq_unregister_hctx(hctx);
+}
+
+int blk_mq_sysfs_register(struct request_queue *q)
+{
+	struct blk_mq_hw_ctx *hctx;
+	int i, ret = 0;
+
+	queue_for_each_hw_ctx(q, hctx, i) {
+		ret = blk_mq_register_hctx(hctx);
+		if (ret)
+			break;
+	}
+
+	return ret;
+}
diff --git a/block/blk-mq-tag.c b/block/blk-mq-tag.c
index 83ae96c51a2..d90c4aeb7dd 100644
--- a/block/blk-mq-tag.c
+++ b/block/blk-mq-tag.c
@@ -1,78 +1,345 @@
+/*
+ * Fast and scalable bitmap tagging variant. Uses sparser bitmaps spread
+ * over multiple cachelines to avoid ping-pong between multiple submitters
+ * or submitter and completer. Uses rolling wakeups to avoid falling of
+ * the scaling cliff when we run out of tags and have to start putting
+ * submitters to sleep.
+ *
+ * Uses active queue tracking to support fairer distribution of tags
+ * between multiple submitters when a shared tag map is used.
+ *
+ * Copyright (C) 2013-2014 Jens Axboe
+ */
 #include <linux/kernel.h>
 #include <linux/module.h>
-#include <linux/percpu_ida.h>
+#include <linux/random.h>
 
 #include <linux/blk-mq.h>
 #include "blk.h"
 #include "blk-mq.h"
 #include "blk-mq-tag.h"
 
+static bool bt_has_free_tags(struct blk_mq_bitmap_tags *bt)
+{
+	int i;
+
+	for (i = 0; i < bt->map_nr; i++) {
+		struct blk_align_bitmap *bm = &bt->map[i];
+		int ret;
+
+		ret = find_first_zero_bit(&bm->word, bm->depth);
+		if (ret < bm->depth)
+			return true;
+	}
+
+	return false;
+}
+
+bool blk_mq_has_free_tags(struct blk_mq_tags *tags)
+{
+	if (!tags)
+		return true;
+
+	return bt_has_free_tags(&tags->bitmap_tags);
+}
+
+static inline void bt_index_inc(unsigned int *index)
+{
+	*index = (*index + 1) & (BT_WAIT_QUEUES - 1);
+}
+
 /*
- * Per tagged queue (tag address space) map
+ * If a previously inactive queue goes active, bump the active user count.
  */
-struct blk_mq_tags {
-	unsigned int nr_tags;
-	unsigned int nr_reserved_tags;
-	unsigned int nr_batch_move;
-	unsigned int nr_max_cache;
+bool __blk_mq_tag_busy(struct blk_mq_hw_ctx *hctx)
+{
+	if (!test_bit(BLK_MQ_S_TAG_ACTIVE, &hctx->state) &&
+	    !test_and_set_bit(BLK_MQ_S_TAG_ACTIVE, &hctx->state))
+		atomic_inc(&hctx->tags->active_queues);
 
-	struct percpu_ida free_tags;
-	struct percpu_ida reserved_tags;
-};
+	return true;
+}
 
-void blk_mq_wait_for_tags(struct blk_mq_tags *tags)
+/*
+ * Wakeup all potentially sleeping on normal (non-reserved) tags
+ */
+static void blk_mq_tag_wakeup_all(struct blk_mq_tags *tags)
 {
-	int tag = blk_mq_get_tag(tags, __GFP_WAIT, false);
-	blk_mq_put_tag(tags, tag);
+	struct blk_mq_bitmap_tags *bt;
+	int i, wake_index;
+
+	bt = &tags->bitmap_tags;
+	wake_index = bt->wake_index;
+	for (i = 0; i < BT_WAIT_QUEUES; i++) {
+		struct bt_wait_state *bs = &bt->bs[wake_index];
+
+		if (waitqueue_active(&bs->wait))
+			wake_up(&bs->wait);
+
+		bt_index_inc(&wake_index);
+	}
 }
 
-bool blk_mq_has_free_tags(struct blk_mq_tags *tags)
+/*
+ * If a previously busy queue goes inactive, potential waiters could now
+ * be allowed to queue. Wake them up and check.
+ */
+void __blk_mq_tag_idle(struct blk_mq_hw_ctx *hctx)
+{
+	struct blk_mq_tags *tags = hctx->tags;
+
+	if (!test_and_clear_bit(BLK_MQ_S_TAG_ACTIVE, &hctx->state))
+		return;
+
+	atomic_dec(&tags->active_queues);
+
+	blk_mq_tag_wakeup_all(tags);
+}
+
+/*
+ * For shared tag users, we track the number of currently active users
+ * and attempt to provide a fair share of the tag depth for each of them.
+ */
+static inline bool hctx_may_queue(struct blk_mq_hw_ctx *hctx,
+				  struct blk_mq_bitmap_tags *bt)
+{
+	unsigned int depth, users;
+
+	if (!hctx || !(hctx->flags & BLK_MQ_F_TAG_SHARED))
+		return true;
+	if (!test_bit(BLK_MQ_S_TAG_ACTIVE, &hctx->state))
+		return true;
+
+	/*
+	 * Don't try dividing an ant
+	 */
+	if (bt->depth == 1)
+		return true;
+
+	users = atomic_read(&hctx->tags->active_queues);
+	if (!users)
+		return true;
+
+	/*
+	 * Allow at least some tags
+	 */
+	depth = max((bt->depth + users - 1) / users, 4U);
+	return atomic_read(&hctx->nr_active) < depth;
+}
+
+static int __bt_get_word(struct blk_align_bitmap *bm, unsigned int last_tag)
 {
-	return !tags ||
-		percpu_ida_free_tags(&tags->free_tags, nr_cpu_ids) != 0;
+	int tag, org_last_tag, end;
+
+	org_last_tag = last_tag;
+	end = bm->depth;
+	do {
+restart:
+		tag = find_next_zero_bit(&bm->word, end, last_tag);
+		if (unlikely(tag >= end)) {
+			/*
+			 * We started with an offset, start from 0 to
+			 * exhaust the map.
+			 */
+			if (org_last_tag && last_tag) {
+				end = last_tag;
+				last_tag = 0;
+				goto restart;
+			}
+			return -1;
+		}
+		last_tag = tag + 1;
+	} while (test_and_set_bit_lock(tag, &bm->word));
+
+	return tag;
 }
 
-static unsigned int __blk_mq_get_tag(struct blk_mq_tags *tags, gfp_t gfp)
+/*
+ * Straight forward bitmap tag implementation, where each bit is a tag
+ * (cleared == free, and set == busy). The small twist is using per-cpu
+ * last_tag caches, which blk-mq stores in the blk_mq_ctx software queue
+ * contexts. This enables us to drastically limit the space searched,
+ * without dirtying an extra shared cacheline like we would if we stored
+ * the cache value inside the shared blk_mq_bitmap_tags structure. On top
+ * of that, each word of tags is in a separate cacheline. This means that
+ * multiple users will tend to stick to different cachelines, at least
+ * until the map is exhausted.
+ */
+static int __bt_get(struct blk_mq_hw_ctx *hctx, struct blk_mq_bitmap_tags *bt,
+		    unsigned int *tag_cache)
 {
+	unsigned int last_tag, org_last_tag;
+	int index, i, tag;
+
+	if (!hctx_may_queue(hctx, bt))
+		return -1;
+
+	last_tag = org_last_tag = *tag_cache;
+	index = TAG_TO_INDEX(bt, last_tag);
+
+	for (i = 0; i < bt->map_nr; i++) {
+		tag = __bt_get_word(&bt->map[index], TAG_TO_BIT(bt, last_tag));
+		if (tag != -1) {
+			tag += (index << bt->bits_per_word);
+			goto done;
+		}
+
+		last_tag = 0;
+		if (++index >= bt->map_nr)
+			index = 0;
+	}
+
+	*tag_cache = 0;
+	return -1;
+
+	/*
+	 * Only update the cache from the allocation path, if we ended
+	 * up using the specific cached tag.
+	 */
+done:
+	if (tag == org_last_tag) {
+		last_tag = tag + 1;
+		if (last_tag >= bt->depth - 1)
+			last_tag = 0;
+
+		*tag_cache = last_tag;
+	}
+
+	return tag;
+}
+
+static struct bt_wait_state *bt_wait_ptr(struct blk_mq_bitmap_tags *bt,
+					 struct blk_mq_hw_ctx *hctx)
+{
+	struct bt_wait_state *bs;
+
+	if (!hctx)
+		return &bt->bs[0];
+
+	bs = &bt->bs[hctx->wait_index];
+	bt_index_inc(&hctx->wait_index);
+	return bs;
+}
+
+static int bt_get(struct blk_mq_bitmap_tags *bt, struct blk_mq_hw_ctx *hctx,
+		  unsigned int *last_tag, gfp_t gfp)
+{
+	struct bt_wait_state *bs;
+	DEFINE_WAIT(wait);
 	int tag;
 
-	tag = percpu_ida_alloc(&tags->free_tags, (gfp & __GFP_WAIT) ?
-			       TASK_UNINTERRUPTIBLE : TASK_RUNNING);
-	if (tag < 0)
-		return BLK_MQ_TAG_FAIL;
-	return tag + tags->nr_reserved_tags;
+	tag = __bt_get(hctx, bt, last_tag);
+	if (tag != -1)
+		return tag;
+
+	if (!(gfp & __GFP_WAIT))
+		return -1;
+
+	bs = bt_wait_ptr(bt, hctx);
+	do {
+		bool was_empty;
+
+		was_empty = list_empty(&wait.task_list);
+		prepare_to_wait(&bs->wait, &wait, TASK_UNINTERRUPTIBLE);
+
+		tag = __bt_get(hctx, bt, last_tag);
+		if (tag != -1)
+			break;
+
+		if (was_empty)
+			atomic_set(&bs->wait_cnt, bt->wake_cnt);
+
+		io_schedule();
+	} while (1);
+
+	finish_wait(&bs->wait, &wait);
+	return tag;
+}
+
+static unsigned int __blk_mq_get_tag(struct blk_mq_tags *tags,
+				     struct blk_mq_hw_ctx *hctx,
+				     unsigned int *last_tag, gfp_t gfp)
+{
+	int tag;
+
+	tag = bt_get(&tags->bitmap_tags, hctx, last_tag, gfp);
+	if (tag >= 0)
+		return tag + tags->nr_reserved_tags;
+
+	return BLK_MQ_TAG_FAIL;
 }
 
 static unsigned int __blk_mq_get_reserved_tag(struct blk_mq_tags *tags,
 					      gfp_t gfp)
 {
-	int tag;
+	int tag, zero = 0;
 
 	if (unlikely(!tags->nr_reserved_tags)) {
 		WARN_ON_ONCE(1);
 		return BLK_MQ_TAG_FAIL;
 	}
 
-	tag = percpu_ida_alloc(&tags->reserved_tags, (gfp & __GFP_WAIT) ?
-			       TASK_UNINTERRUPTIBLE : TASK_RUNNING);
+	tag = bt_get(&tags->breserved_tags, NULL, &zero, gfp);
 	if (tag < 0)
 		return BLK_MQ_TAG_FAIL;
+
 	return tag;
 }
 
-unsigned int blk_mq_get_tag(struct blk_mq_tags *tags, gfp_t gfp, bool reserved)
+unsigned int blk_mq_get_tag(struct blk_mq_hw_ctx *hctx, unsigned int *last_tag,
+			    gfp_t gfp, bool reserved)
 {
 	if (!reserved)
-		return __blk_mq_get_tag(tags, gfp);
+		return __blk_mq_get_tag(hctx->tags, hctx, last_tag, gfp);
 
-	return __blk_mq_get_reserved_tag(tags, gfp);
+	return __blk_mq_get_reserved_tag(hctx->tags, gfp);
+}
+
+static struct bt_wait_state *bt_wake_ptr(struct blk_mq_bitmap_tags *bt)
+{
+	int i, wake_index;
+
+	wake_index = bt->wake_index;
+	for (i = 0; i < BT_WAIT_QUEUES; i++) {
+		struct bt_wait_state *bs = &bt->bs[wake_index];
+
+		if (waitqueue_active(&bs->wait)) {
+			if (wake_index != bt->wake_index)
+				bt->wake_index = wake_index;
+
+			return bs;
+		}
+
+		bt_index_inc(&wake_index);
+	}
+
+	return NULL;
+}
+
+static void bt_clear_tag(struct blk_mq_bitmap_tags *bt, unsigned int tag)
+{
+	const int index = TAG_TO_INDEX(bt, tag);
+	struct bt_wait_state *bs;
+
+	/*
+	 * The unlock memory barrier need to order access to req in free
+	 * path and clearing tag bit
+	 */
+	clear_bit_unlock(TAG_TO_BIT(bt, tag), &bt->map[index].word);
+
+	bs = bt_wake_ptr(bt);
+	if (bs && atomic_dec_and_test(&bs->wait_cnt)) {
+		atomic_set(&bs->wait_cnt, bt->wake_cnt);
+		bt_index_inc(&bt->wake_index);
+		wake_up(&bs->wait);
+	}
 }
 
 static void __blk_mq_put_tag(struct blk_mq_tags *tags, unsigned int tag)
 {
 	BUG_ON(tag >= tags->nr_tags);
 
-	percpu_ida_free(&tags->free_tags, tag - tags->nr_reserved_tags);
+	bt_clear_tag(&tags->bitmap_tags, tag);
 }
 
 static void __blk_mq_put_reserved_tag(struct blk_mq_tags *tags,
@@ -80,22 +347,43 @@ static void __blk_mq_put_reserved_tag(struct blk_mq_tags *tags,
 {
 	BUG_ON(tag >= tags->nr_reserved_tags);
 
-	percpu_ida_free(&tags->reserved_tags, tag);
+	bt_clear_tag(&tags->breserved_tags, tag);
 }
 
-void blk_mq_put_tag(struct blk_mq_tags *tags, unsigned int tag)
+void blk_mq_put_tag(struct blk_mq_hw_ctx *hctx, unsigned int tag,
+		    unsigned int *last_tag)
 {
-	if (tag >= tags->nr_reserved_tags)
-		__blk_mq_put_tag(tags, tag);
-	else
+	struct blk_mq_tags *tags = hctx->tags;
+
+	if (tag >= tags->nr_reserved_tags) {
+		const int real_tag = tag - tags->nr_reserved_tags;
+
+		__blk_mq_put_tag(tags, real_tag);
+		*last_tag = real_tag;
+	} else
 		__blk_mq_put_reserved_tag(tags, tag);
 }
 
-static int __blk_mq_tag_iter(unsigned id, void *data)
+static void bt_for_each_free(struct blk_mq_bitmap_tags *bt,
+			     unsigned long *free_map, unsigned int off)
 {
-	unsigned long *tag_map = data;
-	__set_bit(id, tag_map);
-	return 0;
+	int i;
+
+	for (i = 0; i < bt->map_nr; i++) {
+		struct blk_align_bitmap *bm = &bt->map[i];
+		int bit = 0;
+
+		do {
+			bit = find_next_zero_bit(&bm->word, bm->depth, bit);
+			if (bit >= bm->depth)
+				break;
+
+			__set_bit(bit + off, free_map);
+			bit++;
+		} while (1);
+
+		off += (1 << bt->bits_per_word);
+	}
 }
 
 void blk_mq_tag_busy_iter(struct blk_mq_tags *tags,
@@ -109,21 +397,128 @@ void blk_mq_tag_busy_iter(struct blk_mq_tags *tags,
 	if (!tag_map)
 		return;
 
-	percpu_ida_for_each_free(&tags->free_tags, __blk_mq_tag_iter, tag_map);
+	bt_for_each_free(&tags->bitmap_tags, tag_map, tags->nr_reserved_tags);
 	if (tags->nr_reserved_tags)
-		percpu_ida_for_each_free(&tags->reserved_tags, __blk_mq_tag_iter,
-			tag_map);
+		bt_for_each_free(&tags->breserved_tags, tag_map, 0);
 
 	fn(data, tag_map);
 	kfree(tag_map);
 }
+EXPORT_SYMBOL(blk_mq_tag_busy_iter);
+
+static unsigned int bt_unused_tags(struct blk_mq_bitmap_tags *bt)
+{
+	unsigned int i, used;
+
+	for (i = 0, used = 0; i < bt->map_nr; i++) {
+		struct blk_align_bitmap *bm = &bt->map[i];
+
+		used += bitmap_weight(&bm->word, bm->depth);
+	}
+
+	return bt->depth - used;
+}
+
+static void bt_update_count(struct blk_mq_bitmap_tags *bt,
+			    unsigned int depth)
+{
+	unsigned int tags_per_word = 1U << bt->bits_per_word;
+	unsigned int map_depth = depth;
+
+	if (depth) {
+		int i;
+
+		for (i = 0; i < bt->map_nr; i++) {
+			bt->map[i].depth = min(map_depth, tags_per_word);
+			map_depth -= bt->map[i].depth;
+		}
+	}
+
+	bt->wake_cnt = BT_WAIT_BATCH;
+	if (bt->wake_cnt > depth / 4)
+		bt->wake_cnt = max(1U, depth / 4);
+
+	bt->depth = depth;
+}
+
+static int bt_alloc(struct blk_mq_bitmap_tags *bt, unsigned int depth,
+			int node, bool reserved)
+{
+	int i;
+
+	bt->bits_per_word = ilog2(BITS_PER_LONG);
+
+	/*
+	 * Depth can be zero for reserved tags, that's not a failure
+	 * condition.
+	 */
+	if (depth) {
+		unsigned int nr, tags_per_word;
+
+		tags_per_word = (1 << bt->bits_per_word);
+
+		/*
+		 * If the tag space is small, shrink the number of tags
+		 * per word so we spread over a few cachelines, at least.
+		 * If less than 4 tags, just forget about it, it's not
+		 * going to work optimally anyway.
+		 */
+		if (depth >= 4) {
+			while (tags_per_word * 4 > depth) {
+				bt->bits_per_word--;
+				tags_per_word = (1 << bt->bits_per_word);
+			}
+		}
+
+		nr = ALIGN(depth, tags_per_word) / tags_per_word;
+		bt->map = kzalloc_node(nr * sizeof(struct blk_align_bitmap),
+						GFP_KERNEL, node);
+		if (!bt->map)
+			return -ENOMEM;
+
+		bt->map_nr = nr;
+	}
+
+	bt->bs = kzalloc(BT_WAIT_QUEUES * sizeof(*bt->bs), GFP_KERNEL);
+	if (!bt->bs) {
+		kfree(bt->map);
+		return -ENOMEM;
+	}
+
+	for (i = 0; i < BT_WAIT_QUEUES; i++)
+		init_waitqueue_head(&bt->bs[i].wait);
+
+	bt_update_count(bt, depth);
+	return 0;
+}
+
+static void bt_free(struct blk_mq_bitmap_tags *bt)
+{
+	kfree(bt->map);
+	kfree(bt->bs);
+}
+
+static struct blk_mq_tags *blk_mq_init_bitmap_tags(struct blk_mq_tags *tags,
+						   int node)
+{
+	unsigned int depth = tags->nr_tags - tags->nr_reserved_tags;
+
+	if (bt_alloc(&tags->bitmap_tags, depth, node, false))
+		goto enomem;
+	if (bt_alloc(&tags->breserved_tags, tags->nr_reserved_tags, node, true))
+		goto enomem;
+
+	return tags;
+enomem:
+	bt_free(&tags->bitmap_tags);
+	kfree(tags);
+	return NULL;
+}
 
 struct blk_mq_tags *blk_mq_init_tags(unsigned int total_tags,
 				     unsigned int reserved_tags, int node)
 {
-	unsigned int nr_tags, nr_cache;
 	struct blk_mq_tags *tags;
-	int ret;
 
 	if (total_tags > BLK_MQ_TAG_MAX) {
 		pr_err("blk-mq: tag depth too large\n");
@@ -134,73 +529,59 @@ struct blk_mq_tags *blk_mq_init_tags(unsigned int total_tags,
 	if (!tags)
 		return NULL;
 
-	nr_tags = total_tags - reserved_tags;
-	nr_cache = nr_tags / num_possible_cpus();
-
-	if (nr_cache < BLK_MQ_TAG_CACHE_MIN)
-		nr_cache = BLK_MQ_TAG_CACHE_MIN;
-	else if (nr_cache > BLK_MQ_TAG_CACHE_MAX)
-		nr_cache = BLK_MQ_TAG_CACHE_MAX;
-
 	tags->nr_tags = total_tags;
 	tags->nr_reserved_tags = reserved_tags;
-	tags->nr_max_cache = nr_cache;
-	tags->nr_batch_move = max(1u, nr_cache / 2);
 
-	ret = __percpu_ida_init(&tags->free_tags, tags->nr_tags -
-				tags->nr_reserved_tags,
-				tags->nr_max_cache,
-				tags->nr_batch_move);
-	if (ret)
-		goto err_free_tags;
+	return blk_mq_init_bitmap_tags(tags, node);
+}
 
-	if (reserved_tags) {
-		/*
-		 * With max_cahe and batch set to 1, the allocator fallbacks to
-		 * no cached. It's fine reserved tags allocation is slow.
-		 */
-		ret = __percpu_ida_init(&tags->reserved_tags, reserved_tags,
-				1, 1);
-		if (ret)
-			goto err_reserved_tags;
-	}
+void blk_mq_free_tags(struct blk_mq_tags *tags)
+{
+	bt_free(&tags->bitmap_tags);
+	bt_free(&tags->breserved_tags);
+	kfree(tags);
+}
 
-	return tags;
+void blk_mq_tag_init_last_tag(struct blk_mq_tags *tags, unsigned int *tag)
+{
+	unsigned int depth = tags->nr_tags - tags->nr_reserved_tags;
 
-err_reserved_tags:
-	percpu_ida_destroy(&tags->free_tags);
-err_free_tags:
-	kfree(tags);
-	return NULL;
+	*tag = prandom_u32() % depth;
 }
 
-void blk_mq_free_tags(struct blk_mq_tags *tags)
+int blk_mq_tag_update_depth(struct blk_mq_tags *tags, unsigned int tdepth)
 {
-	percpu_ida_destroy(&tags->free_tags);
-	percpu_ida_destroy(&tags->reserved_tags);
-	kfree(tags);
+	tdepth -= tags->nr_reserved_tags;
+	if (tdepth > tags->nr_tags)
+		return -EINVAL;
+
+	/*
+	 * Don't need (or can't) update reserved tags here, they remain
+	 * static and should never need resizing.
+	 */
+	bt_update_count(&tags->bitmap_tags, tdepth);
+	blk_mq_tag_wakeup_all(tags);
+	return 0;
 }
 
 ssize_t blk_mq_tag_sysfs_show(struct blk_mq_tags *tags, char *page)
 {
 	char *orig_page = page;
-	unsigned int cpu;
+	unsigned int free, res;
 
 	if (!tags)
 		return 0;
 
-	page += sprintf(page, "nr_tags=%u, reserved_tags=%u, batch_move=%u,"
-			" max_cache=%u\n", tags->nr_tags, tags->nr_reserved_tags,
-			tags->nr_batch_move, tags->nr_max_cache);
+	page += sprintf(page, "nr_tags=%u, reserved_tags=%u, "
+			"bits_per_word=%u\n",
+			tags->nr_tags, tags->nr_reserved_tags,
+			tags->bitmap_tags.bits_per_word);
 
-	page += sprintf(page, "nr_free=%u, nr_reserved=%u\n",
-			percpu_ida_free_tags(&tags->free_tags, nr_cpu_ids),
-			percpu_ida_free_tags(&tags->reserved_tags, nr_cpu_ids));
+	free = bt_unused_tags(&tags->bitmap_tags);
+	res = bt_unused_tags(&tags->breserved_tags);
 
-	for_each_possible_cpu(cpu) {
-		page += sprintf(page, "  cpu%02u: nr_free=%u\n", cpu,
-				percpu_ida_free_tags(&tags->free_tags, cpu));
-	}
+	page += sprintf(page, "nr_free=%u, nr_reserved=%u\n", free, res);
+	page += sprintf(page, "active_queues=%u\n", atomic_read(&tags->active_queues));
 
 	return page - orig_page;
 }
diff --git a/block/blk-mq-tag.h b/block/blk-mq-tag.h
index 947ba2c6148..c959de58d2a 100644
--- a/block/blk-mq-tag.h
+++ b/block/blk-mq-tag.h
@@ -1,17 +1,59 @@
 #ifndef INT_BLK_MQ_TAG_H
 #define INT_BLK_MQ_TAG_H
 
-struct blk_mq_tags;
+#include "blk-mq.h"
+
+enum {
+	BT_WAIT_QUEUES	= 8,
+	BT_WAIT_BATCH	= 8,
+};
+
+struct bt_wait_state {
+	atomic_t wait_cnt;
+	wait_queue_head_t wait;
+} ____cacheline_aligned_in_smp;
+
+#define TAG_TO_INDEX(bt, tag)	((tag) >> (bt)->bits_per_word)
+#define TAG_TO_BIT(bt, tag)	((tag) & ((1 << (bt)->bits_per_word) - 1))
+
+struct blk_mq_bitmap_tags {
+	unsigned int depth;
+	unsigned int wake_cnt;
+	unsigned int bits_per_word;
+
+	unsigned int map_nr;
+	struct blk_align_bitmap *map;
+
+	unsigned int wake_index;
+	struct bt_wait_state *bs;
+};
+
+/*
+ * Tag address space map.
+ */
+struct blk_mq_tags {
+	unsigned int nr_tags;
+	unsigned int nr_reserved_tags;
+
+	atomic_t active_queues;
+
+	struct blk_mq_bitmap_tags bitmap_tags;
+	struct blk_mq_bitmap_tags breserved_tags;
+
+	struct request **rqs;
+	struct list_head page_list;
+};
+
 
 extern struct blk_mq_tags *blk_mq_init_tags(unsigned int nr_tags, unsigned int reserved_tags, int node);
 extern void blk_mq_free_tags(struct blk_mq_tags *tags);
 
-extern unsigned int blk_mq_get_tag(struct blk_mq_tags *tags, gfp_t gfp, bool reserved);
-extern void blk_mq_wait_for_tags(struct blk_mq_tags *tags);
-extern void blk_mq_put_tag(struct blk_mq_tags *tags, unsigned int tag);
-extern void blk_mq_tag_busy_iter(struct blk_mq_tags *tags, void (*fn)(void *data, unsigned long *), void *data);
+extern unsigned int blk_mq_get_tag(struct blk_mq_hw_ctx *hctx, unsigned int *last_tag, gfp_t gfp, bool reserved);
+extern void blk_mq_put_tag(struct blk_mq_hw_ctx *hctx, unsigned int tag, unsigned int *last_tag);
 extern bool blk_mq_has_free_tags(struct blk_mq_tags *tags);
 extern ssize_t blk_mq_tag_sysfs_show(struct blk_mq_tags *tags, char *page);
+extern void blk_mq_tag_init_last_tag(struct blk_mq_tags *tags, unsigned int *last_tag);
+extern int blk_mq_tag_update_depth(struct blk_mq_tags *tags, unsigned int depth);
 
 enum {
 	BLK_MQ_TAG_CACHE_MIN	= 1,
@@ -24,4 +66,23 @@ enum {
 	BLK_MQ_TAG_MAX		= BLK_MQ_TAG_FAIL - 1,
 };
 
+extern bool __blk_mq_tag_busy(struct blk_mq_hw_ctx *);
+extern void __blk_mq_tag_idle(struct blk_mq_hw_ctx *);
+
+static inline bool blk_mq_tag_busy(struct blk_mq_hw_ctx *hctx)
+{
+	if (!(hctx->flags & BLK_MQ_F_TAG_SHARED))
+		return false;
+
+	return __blk_mq_tag_busy(hctx);
+}
+
+static inline void blk_mq_tag_idle(struct blk_mq_hw_ctx *hctx)
+{
+	if (!(hctx->flags & BLK_MQ_F_TAG_SHARED))
+		return;
+
+	__blk_mq_tag_idle(hctx);
+}
+
 #endif
diff --git a/block/blk-mq.c b/block/blk-mq.c
index 1d2a9bdbee5..0f5879c42dc 100644
--- a/block/blk-mq.c
+++ b/block/blk-mq.c
@@ -1,3 +1,9 @@
+/*
+ * Block multiqueue core code
+ *
+ * Copyright (C) 2013-2014 Jens Axboe
+ * Copyright (C) 2013-2014 Christoph Hellwig
+ */
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/backing-dev.h>
@@ -56,38 +62,40 @@ static bool blk_mq_hctx_has_pending(struct blk_mq_hw_ctx *hctx)
 {
 	unsigned int i;
 
-	for (i = 0; i < hctx->nr_ctx_map; i++)
-		if (hctx->ctx_map[i])
+	for (i = 0; i < hctx->ctx_map.map_size; i++)
+		if (hctx->ctx_map.map[i].word)
 			return true;
 
 	return false;
 }
 
+static inline struct blk_align_bitmap *get_bm(struct blk_mq_hw_ctx *hctx,
+					      struct blk_mq_ctx *ctx)
+{
+	return &hctx->ctx_map.map[ctx->index_hw / hctx->ctx_map.bits_per_word];
+}
+
+#define CTX_TO_BIT(hctx, ctx)	\
+	((ctx)->index_hw & ((hctx)->ctx_map.bits_per_word - 1))
+
 /*
  * Mark this ctx as having pending work in this hardware queue
  */
 static void blk_mq_hctx_mark_pending(struct blk_mq_hw_ctx *hctx,
 				     struct blk_mq_ctx *ctx)
 {
-	if (!test_bit(ctx->index_hw, hctx->ctx_map))
-		set_bit(ctx->index_hw, hctx->ctx_map);
+	struct blk_align_bitmap *bm = get_bm(hctx, ctx);
+
+	if (!test_bit(CTX_TO_BIT(hctx, ctx), &bm->word))
+		set_bit(CTX_TO_BIT(hctx, ctx), &bm->word);
 }
 
-static struct request *__blk_mq_alloc_request(struct blk_mq_hw_ctx *hctx,
-					      gfp_t gfp, bool reserved)
+static void blk_mq_hctx_clear_pending(struct blk_mq_hw_ctx *hctx,
+				      struct blk_mq_ctx *ctx)
 {
-	struct request *rq;
-	unsigned int tag;
+	struct blk_align_bitmap *bm = get_bm(hctx, ctx);
 
-	tag = blk_mq_get_tag(hctx->tags, gfp, reserved);
-	if (tag != BLK_MQ_TAG_FAIL) {
-		rq = hctx->rqs[tag];
-		rq->tag = tag;
-
-		return rq;
-	}
-
-	return NULL;
+	clear_bit(CTX_TO_BIT(hctx, ctx), &bm->word);
 }
 
 static int blk_mq_queue_enter(struct request_queue *q)
@@ -186,78 +194,95 @@ static void blk_mq_rq_ctx_init(struct request_queue *q, struct blk_mq_ctx *ctx,
 	if (blk_queue_io_stat(q))
 		rw_flags |= REQ_IO_STAT;
 
+	INIT_LIST_HEAD(&rq->queuelist);
+	/* csd/requeue_work/fifo_time is initialized before use */
+	rq->q = q;
 	rq->mq_ctx = ctx;
-	rq->cmd_flags = rw_flags;
-	rq->start_time = jiffies;
+	rq->cmd_flags |= rw_flags;
+	/* do not touch atomic flags, it needs atomic ops against the timer */
+	rq->cpu = -1;
+	INIT_HLIST_NODE(&rq->hash);
+	RB_CLEAR_NODE(&rq->rb_node);
+	rq->rq_disk = NULL;
+	rq->part = NULL;
+#ifdef CONFIG_BLK_CGROUP
+	rq->rl = NULL;
 	set_start_time_ns(rq);
+	rq->io_start_time_ns = 0;
+#endif
+	rq->nr_phys_segments = 0;
+#if defined(CONFIG_BLK_DEV_INTEGRITY)
+	rq->nr_integrity_segments = 0;
+#endif
+	rq->special = NULL;
+	/* tag was already set */
+	rq->errors = 0;
+
+	rq->extra_len = 0;
+	rq->sense_len = 0;
+	rq->resid_len = 0;
+	rq->sense = NULL;
+
+	INIT_LIST_HEAD(&rq->timeout_list);
+	rq->end_io = NULL;
+	rq->end_io_data = NULL;
+	rq->next_rq = NULL;
+
 	ctx->rq_dispatched[rw_is_sync(rw_flags)]++;
 }
 
-static struct request *blk_mq_alloc_request_pinned(struct request_queue *q,
-						   int rw, gfp_t gfp,
-						   bool reserved)
+static struct request *
+__blk_mq_alloc_request(struct request_queue *q, struct blk_mq_hw_ctx *hctx,
+		struct blk_mq_ctx *ctx, int rw, gfp_t gfp, bool reserved)
 {
 	struct request *rq;
+	unsigned int tag;
 
-	do {
-		struct blk_mq_ctx *ctx = blk_mq_get_ctx(q);
-		struct blk_mq_hw_ctx *hctx = q->mq_ops->map_queue(q, ctx->cpu);
+	tag = blk_mq_get_tag(hctx, &ctx->last_tag, gfp, reserved);
+	if (tag != BLK_MQ_TAG_FAIL) {
+		rq = hctx->tags->rqs[tag];
 
-		rq = __blk_mq_alloc_request(hctx, gfp & ~__GFP_WAIT, reserved);
-		if (rq) {
-			blk_mq_rq_ctx_init(q, ctx, rq, rw);
-			break;
+		rq->cmd_flags = 0;
+		if (blk_mq_tag_busy(hctx)) {
+			rq->cmd_flags = REQ_MQ_INFLIGHT;
+			atomic_inc(&hctx->nr_active);
 		}
 
-		blk_mq_put_ctx(ctx);
-		if (!(gfp & __GFP_WAIT))
-			break;
-
-		__blk_mq_run_hw_queue(hctx);
-		blk_mq_wait_for_tags(hctx->tags);
-	} while (1);
+		rq->tag = tag;
+		blk_mq_rq_ctx_init(q, ctx, rq, rw);
+		return rq;
+	}
 
-	return rq;
+	return NULL;
 }
 
-struct request *blk_mq_alloc_request(struct request_queue *q, int rw, gfp_t gfp)
+struct request *blk_mq_alloc_request(struct request_queue *q, int rw, gfp_t gfp,
+		bool reserved)
 {
+	struct blk_mq_ctx *ctx;
+	struct blk_mq_hw_ctx *hctx;
 	struct request *rq;
 
 	if (blk_mq_queue_enter(q))
 		return NULL;
 
-	rq = blk_mq_alloc_request_pinned(q, rw, gfp, false);
-	if (rq)
-		blk_mq_put_ctx(rq->mq_ctx);
-	return rq;
-}
-
-struct request *blk_mq_alloc_reserved_request(struct request_queue *q, int rw,
-					      gfp_t gfp)
-{
-	struct request *rq;
+	ctx = blk_mq_get_ctx(q);
+	hctx = q->mq_ops->map_queue(q, ctx->cpu);
 
-	if (blk_mq_queue_enter(q))
-		return NULL;
+	rq = __blk_mq_alloc_request(q, hctx, ctx, rw, gfp & ~__GFP_WAIT,
+				    reserved);
+	if (!rq && (gfp & __GFP_WAIT)) {
+		__blk_mq_run_hw_queue(hctx);
+		blk_mq_put_ctx(ctx);
 
-	rq = blk_mq_alloc_request_pinned(q, rw, gfp, true);
-	if (rq)
-		blk_mq_put_ctx(rq->mq_ctx);
+		ctx = blk_mq_get_ctx(q);
+		hctx = q->mq_ops->map_queue(q, ctx->cpu);
+		rq =  __blk_mq_alloc_request(q, hctx, ctx, rw, gfp, reserved);
+	}
+	blk_mq_put_ctx(ctx);
 	return rq;
 }
-EXPORT_SYMBOL(blk_mq_alloc_reserved_request);
-
-/*
- * Re-init and set pdu, if we have it
- */
-void blk_mq_rq_init(struct blk_mq_hw_ctx *hctx, struct request *rq)
-{
-	blk_rq_init(hctx->queue, rq);
-
-	if (hctx->cmd_size)
-		rq->special = blk_mq_rq_to_pdu(rq);
-}
+EXPORT_SYMBOL(blk_mq_alloc_request);
 
 static void __blk_mq_free_request(struct blk_mq_hw_ctx *hctx,
 				  struct blk_mq_ctx *ctx, struct request *rq)
@@ -265,9 +290,11 @@ static void __blk_mq_free_request(struct blk_mq_hw_ctx *hctx,
 	const int tag = rq->tag;
 	struct request_queue *q = rq->q;
 
-	blk_mq_rq_init(hctx, rq);
-	blk_mq_put_tag(hctx->tags, tag);
+	if (rq->cmd_flags & REQ_MQ_INFLIGHT)
+		atomic_dec(&hctx->nr_active);
 
+	clear_bit(REQ_ATOM_STARTED, &rq->atomic_flags);
+	blk_mq_put_tag(hctx, tag, &ctx->last_tag);
 	blk_mq_queue_exit(q);
 }
 
@@ -283,20 +310,47 @@ void blk_mq_free_request(struct request *rq)
 	__blk_mq_free_request(hctx, ctx, rq);
 }
 
-bool blk_mq_end_io_partial(struct request *rq, int error, unsigned int nr_bytes)
+/*
+ * Clone all relevant state from a request that has been put on hold in
+ * the flush state machine into the preallocated flush request that hangs
+ * off the request queue.
+ *
+ * For a driver the flush request should be invisible, that's why we are
+ * impersonating the original request here.
+ */
+void blk_mq_clone_flush_request(struct request *flush_rq,
+		struct request *orig_rq)
 {
-	if (blk_update_request(rq, error, blk_rq_bytes(rq)))
-		return true;
+	struct blk_mq_hw_ctx *hctx =
+		orig_rq->q->mq_ops->map_queue(orig_rq->q, orig_rq->mq_ctx->cpu);
+
+	flush_rq->mq_ctx = orig_rq->mq_ctx;
+	flush_rq->tag = orig_rq->tag;
+	memcpy(blk_mq_rq_to_pdu(flush_rq), blk_mq_rq_to_pdu(orig_rq),
+		hctx->cmd_size);
+}
 
+inline void __blk_mq_end_io(struct request *rq, int error)
+{
 	blk_account_io_done(rq);
 
-	if (rq->end_io)
+	if (rq->end_io) {
 		rq->end_io(rq, error);
-	else
+	} else {
+		if (unlikely(blk_bidi_rq(rq)))
+			blk_mq_free_request(rq->next_rq);
 		blk_mq_free_request(rq);
-	return false;
+	}
+}
+EXPORT_SYMBOL(__blk_mq_end_io);
+
+void blk_mq_end_io(struct request *rq, int error)
+{
+	if (blk_update_request(rq, error, blk_rq_bytes(rq)))
+		BUG();
+	__blk_mq_end_io(rq, error);
 }
-EXPORT_SYMBOL(blk_mq_end_io_partial);
+EXPORT_SYMBOL(blk_mq_end_io);
 
 static void __blk_mq_complete_request_remote(void *data)
 {
@@ -305,18 +359,22 @@ static void __blk_mq_complete_request_remote(void *data)
 	rq->q->softirq_done_fn(rq);
 }
 
-void __blk_mq_complete_request(struct request *rq)
+static void blk_mq_ipi_complete_request(struct request *rq)
 {
 	struct blk_mq_ctx *ctx = rq->mq_ctx;
+	bool shared = false;
 	int cpu;
 
-	if (!ctx->ipi_redirect) {
+	if (!test_bit(QUEUE_FLAG_SAME_COMP, &rq->q->queue_flags)) {
 		rq->q->softirq_done_fn(rq);
 		return;
 	}
 
 	cpu = get_cpu();
-	if (cpu != ctx->cpu && cpu_online(ctx->cpu)) {
+	if (!test_bit(QUEUE_FLAG_SAME_FORCE, &rq->q->queue_flags))
+		shared = cpus_share_cache(cpu, ctx->cpu);
+
+	if (cpu != ctx->cpu && !shared && cpu_online(ctx->cpu)) {
 		rq->csd.func = __blk_mq_complete_request_remote;
 		rq->csd.info = rq;
 		rq->csd.flags = 0;
@@ -327,6 +385,16 @@ void __blk_mq_complete_request(struct request *rq)
 	put_cpu();
 }
 
+void __blk_mq_complete_request(struct request *rq)
+{
+	struct request_queue *q = rq->q;
+
+	if (!q->softirq_done_fn)
+		blk_mq_end_io(rq, rq->errors);
+	else
+		blk_mq_ipi_complete_request(rq);
+}
+
 /**
  * blk_mq_complete_request - end I/O on a request
  * @rq:		the request being processed
@@ -337,7 +405,9 @@ void __blk_mq_complete_request(struct request *rq)
  **/
 void blk_mq_complete_request(struct request *rq)
 {
-	if (unlikely(blk_should_fake_timeout(rq->q)))
+	struct request_queue *q = rq->q;
+
+	if (unlikely(blk_should_fake_timeout(q)))
 		return;
 	if (!blk_mark_rq_complete(rq))
 		__blk_mq_complete_request(rq);
@@ -350,13 +420,31 @@ static void blk_mq_start_request(struct request *rq, bool last)
 
 	trace_block_rq_issue(q, rq);
 
+	rq->resid_len = blk_rq_bytes(rq);
+	if (unlikely(blk_bidi_rq(rq)))
+		rq->next_rq->resid_len = blk_rq_bytes(rq->next_rq);
+
 	/*
 	 * Just mark start time and set the started bit. Due to memory
 	 * ordering, we know we'll see the correct deadline as long as
-	 * REQ_ATOMIC_STARTED is seen.
+	 * REQ_ATOMIC_STARTED is seen. Use the default queue timeout,
+	 * unless one has been set in the request.
+	 */
+	if (!rq->timeout)
+		rq->deadline = jiffies + q->rq_timeout;
+	else
+		rq->deadline = jiffies + rq->timeout;
+
+	/*
+	 * Mark us as started and clear complete. Complete might have been
+	 * set if requeue raced with timeout, which then marked it as
+	 * complete. So be sure to clear complete again when we start
+	 * the request, otherwise we'll ignore the completion event.
 	 */
-	rq->deadline = jiffies + q->rq_timeout;
-	set_bit(REQ_ATOM_STARTED, &rq->atomic_flags);
+	if (!test_bit(REQ_ATOM_STARTED, &rq->atomic_flags))
+		set_bit(REQ_ATOM_STARTED, &rq->atomic_flags);
+	if (test_bit(REQ_ATOM_COMPLETE, &rq->atomic_flags))
+		clear_bit(REQ_ATOM_COMPLETE, &rq->atomic_flags);
 
 	if (q->dma_drain_size && blk_rq_bytes(rq)) {
 		/*
@@ -378,7 +466,7 @@ static void blk_mq_start_request(struct request *rq, bool last)
 		rq->cmd_flags |= REQ_END;
 }
 
-static void blk_mq_requeue_request(struct request *rq)
+static void __blk_mq_requeue_request(struct request *rq)
 {
 	struct request_queue *q = rq->q;
 
@@ -391,6 +479,86 @@ static void blk_mq_requeue_request(struct request *rq)
 		rq->nr_phys_segments--;
 }
 
+void blk_mq_requeue_request(struct request *rq)
+{
+	__blk_mq_requeue_request(rq);
+	blk_clear_rq_complete(rq);
+
+	BUG_ON(blk_queued_rq(rq));
+	blk_mq_add_to_requeue_list(rq, true);
+}
+EXPORT_SYMBOL(blk_mq_requeue_request);
+
+static void blk_mq_requeue_work(struct work_struct *work)
+{
+	struct request_queue *q =
+		container_of(work, struct request_queue, requeue_work);
+	LIST_HEAD(rq_list);
+	struct request *rq, *next;
+	unsigned long flags;
+
+	spin_lock_irqsave(&q->requeue_lock, flags);
+	list_splice_init(&q->requeue_list, &rq_list);
+	spin_unlock_irqrestore(&q->requeue_lock, flags);
+
+	list_for_each_entry_safe(rq, next, &rq_list, queuelist) {
+		if (!(rq->cmd_flags & REQ_SOFTBARRIER))
+			continue;
+
+		rq->cmd_flags &= ~REQ_SOFTBARRIER;
+		list_del_init(&rq->queuelist);
+		blk_mq_insert_request(rq, true, false, false);
+	}
+
+	while (!list_empty(&rq_list)) {
+		rq = list_entry(rq_list.next, struct request, queuelist);
+		list_del_init(&rq->queuelist);
+		blk_mq_insert_request(rq, false, false, false);
+	}
+
+	blk_mq_run_queues(q, false);
+}
+
+void blk_mq_add_to_requeue_list(struct request *rq, bool at_head)
+{
+	struct request_queue *q = rq->q;
+	unsigned long flags;
+
+	/*
+	 * We abuse this flag that is otherwise used by the I/O scheduler to
+	 * request head insertation from the workqueue.
+	 */
+	BUG_ON(rq->cmd_flags & REQ_SOFTBARRIER);
+
+	spin_lock_irqsave(&q->requeue_lock, flags);
+	if (at_head) {
+		rq->cmd_flags |= REQ_SOFTBARRIER;
+		list_add(&rq->queuelist, &q->requeue_list);
+	} else {
+		list_add_tail(&rq->queuelist, &q->requeue_list);
+	}
+	spin_unlock_irqrestore(&q->requeue_lock, flags);
+}
+EXPORT_SYMBOL(blk_mq_add_to_requeue_list);
+
+void blk_mq_kick_requeue_list(struct request_queue *q)
+{
+	kblockd_schedule_work(&q->requeue_work);
+}
+EXPORT_SYMBOL(blk_mq_kick_requeue_list);
+
+struct request *blk_mq_tag_to_rq(struct blk_mq_hw_ctx *hctx, unsigned int tag)
+{
+	struct request_queue *q = hctx->queue;
+
+	if ((q->flush_rq->cmd_flags & REQ_FLUSH_SEQ) &&
+	    q->flush_rq->tag == tag)
+		return q->flush_rq;
+
+	return hctx->tags->rqs[tag];
+}
+EXPORT_SYMBOL(blk_mq_tag_to_rq);
+
 struct blk_mq_timeout_data {
 	struct blk_mq_hw_ctx *hctx;
 	unsigned long *next;
@@ -412,12 +580,13 @@ static void blk_mq_timeout_check(void *__data, unsigned long *free_tags)
 	do {
 		struct request *rq;
 
-		tag = find_next_zero_bit(free_tags, hctx->queue_depth, tag);
-		if (tag >= hctx->queue_depth)
+		tag = find_next_zero_bit(free_tags, hctx->tags->nr_tags, tag);
+		if (tag >= hctx->tags->nr_tags)
 			break;
 
-		rq = hctx->rqs[tag++];
-
+		rq = blk_mq_tag_to_rq(hctx, tag++);
+		if (rq->q != hctx->queue)
+			continue;
 		if (!test_bit(REQ_ATOM_STARTED, &rq->atomic_flags))
 			continue;
 
@@ -442,6 +611,28 @@ static void blk_mq_hw_ctx_check_timeout(struct blk_mq_hw_ctx *hctx,
 	blk_mq_tag_busy_iter(hctx->tags, blk_mq_timeout_check, &data);
 }
 
+static enum blk_eh_timer_return blk_mq_rq_timed_out(struct request *rq)
+{
+	struct request_queue *q = rq->q;
+
+	/*
+	 * We know that complete is set at this point. If STARTED isn't set
+	 * anymore, then the request isn't active and the "timeout" should
+	 * just be ignored. This can happen due to the bitflag ordering.
+	 * Timeout first checks if STARTED is set, and if it is, assumes
+	 * the request is active. But if we race with completion, then
+	 * we both flags will get cleared. So check here again, and ignore
+	 * a timeout event with a request that isn't active.
+	 */
+	if (!test_bit(REQ_ATOM_STARTED, &rq->atomic_flags))
+		return BLK_EH_NOT_HANDLED;
+
+	if (!q->mq_ops->timeout)
+		return BLK_EH_RESET_TIMER;
+
+	return q->mq_ops->timeout(rq);
+}
+
 static void blk_mq_rq_timer(unsigned long data)
 {
 	struct request_queue *q = (struct request_queue *) data;
@@ -449,11 +640,24 @@ static void blk_mq_rq_timer(unsigned long data)
 	unsigned long next = 0;
 	int i, next_set = 0;
 
-	queue_for_each_hw_ctx(q, hctx, i)
+	queue_for_each_hw_ctx(q, hctx, i) {
+		/*
+		 * If not software queues are currently mapped to this
+		 * hardware queue, there's nothing to check
+		 */
+		if (!hctx->nr_ctx || !hctx->tags)
+			continue;
+
 		blk_mq_hw_ctx_check_timeout(hctx, &next, &next_set);
+	}
 
-	if (next_set)
-		mod_timer(&q->timeout, round_jiffies_up(next));
+	if (next_set) {
+		next = blk_rq_timeout(round_jiffies_up(next));
+		mod_timer(&q->timeout, next);
+	} else {
+		queue_for_each_hw_ctx(q, hctx, i)
+			blk_mq_tag_idle(hctx);
+	}
 }
 
 /*
@@ -495,9 +699,38 @@ static bool blk_mq_attempt_merge(struct request_queue *q,
 	return false;
 }
 
-void blk_mq_add_timer(struct request *rq)
+/*
+ * Process software queues that have been marked busy, splicing them
+ * to the for-dispatch
+ */
+static void flush_busy_ctxs(struct blk_mq_hw_ctx *hctx, struct list_head *list)
 {
-	__blk_add_timer(rq, NULL);
+	struct blk_mq_ctx *ctx;
+	int i;
+
+	for (i = 0; i < hctx->ctx_map.map_size; i++) {
+		struct blk_align_bitmap *bm = &hctx->ctx_map.map[i];
+		unsigned int off, bit;
+
+		if (!bm->word)
+			continue;
+
+		bit = 0;
+		off = i * hctx->ctx_map.bits_per_word;
+		do {
+			bit = find_next_bit(&bm->word, bm->depth, bit);
+			if (bit >= bm->depth)
+				break;
+
+			ctx = hctx->ctxs[bit + off];
+			clear_bit(bit, &bm->word);
+			spin_lock(&ctx->lock);
+			list_splice_tail_init(&ctx->rq_list, list);
+			spin_unlock(&ctx->lock);
+
+			bit++;
+		} while (1);
+	}
 }
 
 /*
@@ -509,10 +742,11 @@ void blk_mq_add_timer(struct request *rq)
 static void __blk_mq_run_hw_queue(struct blk_mq_hw_ctx *hctx)
 {
 	struct request_queue *q = hctx->queue;
-	struct blk_mq_ctx *ctx;
 	struct request *rq;
 	LIST_HEAD(rq_list);
-	int bit, queued;
+	int queued;
+
+	WARN_ON(!cpumask_test_cpu(raw_smp_processor_id(), hctx->cpumask));
 
 	if (unlikely(test_bit(BLK_MQ_S_STOPPED, &hctx->state)))
 		return;
@@ -522,15 +756,7 @@ static void __blk_mq_run_hw_queue(struct blk_mq_hw_ctx *hctx)
 	/*
 	 * Touch any software queue that has pending entries.
 	 */
-	for_each_set_bit(bit, hctx->ctx_map, hctx->nr_ctx) {
-		clear_bit(bit, hctx->ctx_map);
-		ctx = hctx->ctxs[bit];
-		BUG_ON(bit != ctx->index_hw);
-
-		spin_lock(&ctx->lock);
-		list_splice_tail_init(&ctx->rq_list, &rq_list);
-		spin_unlock(&ctx->lock);
-	}
+	flush_busy_ctxs(hctx, &rq_list);
 
 	/*
 	 * If we have previous entries on our dispatch list, grab them
@@ -544,13 +770,9 @@ static void __blk_mq_run_hw_queue(struct blk_mq_hw_ctx *hctx)
 	}
 
 	/*
-	 * Delete and return all entries from our dispatch list
-	 */
-	queued = 0;
-
-	/*
 	 * Now process all the entries, sending them to the driver.
 	 */
+	queued = 0;
 	while (!list_empty(&rq_list)) {
 		int ret;
 
@@ -565,13 +787,8 @@ static void __blk_mq_run_hw_queue(struct blk_mq_hw_ctx *hctx)
 			queued++;
 			continue;
 		case BLK_MQ_RQ_QUEUE_BUSY:
-			/*
-			 * FIXME: we should have a mechanism to stop the queue
-			 * like blk_stop_queue, otherwise we will waste cpu
-			 * time
-			 */
 			list_add(&rq->queuelist, &rq_list);
-			blk_mq_requeue_request(rq);
+			__blk_mq_requeue_request(rq);
 			break;
 		default:
 			pr_err("blk-mq: bad return on queue: %d\n", ret);
@@ -601,17 +818,44 @@ static void __blk_mq_run_hw_queue(struct blk_mq_hw_ctx *hctx)
 	}
 }
 
+/*
+ * It'd be great if the workqueue API had a way to pass
+ * in a mask and had some smarts for more clever placement.
+ * For now we just round-robin here, switching for every
+ * BLK_MQ_CPU_WORK_BATCH queued items.
+ */
+static int blk_mq_hctx_next_cpu(struct blk_mq_hw_ctx *hctx)
+{
+	int cpu = hctx->next_cpu;
+
+	if (--hctx->next_cpu_batch <= 0) {
+		int next_cpu;
+
+		next_cpu = cpumask_next(hctx->next_cpu, hctx->cpumask);
+		if (next_cpu >= nr_cpu_ids)
+			next_cpu = cpumask_first(hctx->cpumask);
+
+		hctx->next_cpu = next_cpu;
+		hctx->next_cpu_batch = BLK_MQ_CPU_WORK_BATCH;
+	}
+
+	return cpu;
+}
+
 void blk_mq_run_hw_queue(struct blk_mq_hw_ctx *hctx, bool async)
 {
 	if (unlikely(test_bit(BLK_MQ_S_STOPPED, &hctx->state)))
 		return;
 
-	if (!async)
+	if (!async && cpumask_test_cpu(smp_processor_id(), hctx->cpumask))
 		__blk_mq_run_hw_queue(hctx);
+	else if (hctx->queue->nr_hw_queues == 1)
+		kblockd_schedule_delayed_work(&hctx->run_work, 0);
 	else {
-		struct request_queue *q = hctx->queue;
+		unsigned int cpu;
 
-		kblockd_schedule_delayed_work(q, &hctx->delayed_work, 0);
+		cpu = blk_mq_hctx_next_cpu(hctx);
+		kblockd_schedule_delayed_work_on(cpu, &hctx->run_work, 0);
 	}
 }
 
@@ -626,14 +870,17 @@ void blk_mq_run_queues(struct request_queue *q, bool async)
 		    test_bit(BLK_MQ_S_STOPPED, &hctx->state))
 			continue;
 
+		preempt_disable();
 		blk_mq_run_hw_queue(hctx, async);
+		preempt_enable();
 	}
 }
 EXPORT_SYMBOL(blk_mq_run_queues);
 
 void blk_mq_stop_hw_queue(struct blk_mq_hw_ctx *hctx)
 {
-	cancel_delayed_work(&hctx->delayed_work);
+	cancel_delayed_work(&hctx->run_work);
+	cancel_delayed_work(&hctx->delay_work);
 	set_bit(BLK_MQ_S_STOPPED, &hctx->state);
 }
 EXPORT_SYMBOL(blk_mq_stop_hw_queue);
@@ -651,11 +898,25 @@ EXPORT_SYMBOL(blk_mq_stop_hw_queues);
 void blk_mq_start_hw_queue(struct blk_mq_hw_ctx *hctx)
 {
 	clear_bit(BLK_MQ_S_STOPPED, &hctx->state);
+
+	preempt_disable();
 	__blk_mq_run_hw_queue(hctx);
+	preempt_enable();
 }
 EXPORT_SYMBOL(blk_mq_start_hw_queue);
 
-void blk_mq_start_stopped_hw_queues(struct request_queue *q)
+void blk_mq_start_hw_queues(struct request_queue *q)
+{
+	struct blk_mq_hw_ctx *hctx;
+	int i;
+
+	queue_for_each_hw_ctx(q, hctx, i)
+		blk_mq_start_hw_queue(hctx);
+}
+EXPORT_SYMBOL(blk_mq_start_hw_queues);
+
+
+void blk_mq_start_stopped_hw_queues(struct request_queue *q, bool async)
 {
 	struct blk_mq_hw_ctx *hctx;
 	int i;
@@ -665,19 +926,47 @@ void blk_mq_start_stopped_hw_queues(struct request_queue *q)
 			continue;
 
 		clear_bit(BLK_MQ_S_STOPPED, &hctx->state);
-		blk_mq_run_hw_queue(hctx, true);
+		preempt_disable();
+		blk_mq_run_hw_queue(hctx, async);
+		preempt_enable();
 	}
 }
 EXPORT_SYMBOL(blk_mq_start_stopped_hw_queues);
 
-static void blk_mq_work_fn(struct work_struct *work)
+static void blk_mq_run_work_fn(struct work_struct *work)
 {
 	struct blk_mq_hw_ctx *hctx;
 
-	hctx = container_of(work, struct blk_mq_hw_ctx, delayed_work.work);
+	hctx = container_of(work, struct blk_mq_hw_ctx, run_work.work);
+
 	__blk_mq_run_hw_queue(hctx);
 }
 
+static void blk_mq_delay_work_fn(struct work_struct *work)
+{
+	struct blk_mq_hw_ctx *hctx;
+
+	hctx = container_of(work, struct blk_mq_hw_ctx, delay_work.work);
+
+	if (test_and_clear_bit(BLK_MQ_S_STOPPED, &hctx->state))
+		__blk_mq_run_hw_queue(hctx);
+}
+
+void blk_mq_delay_queue(struct blk_mq_hw_ctx *hctx, unsigned long msecs)
+{
+	unsigned long tmo = msecs_to_jiffies(msecs);
+
+	if (hctx->queue->nr_hw_queues == 1)
+		kblockd_schedule_delayed_work(&hctx->delay_work, tmo);
+	else {
+		unsigned int cpu;
+
+		cpu = blk_mq_hctx_next_cpu(hctx);
+		kblockd_schedule_delayed_work_on(cpu, &hctx->delay_work, tmo);
+	}
+}
+EXPORT_SYMBOL(blk_mq_delay_queue);
+
 static void __blk_mq_insert_request(struct blk_mq_hw_ctx *hctx,
 				    struct request *rq, bool at_head)
 {
@@ -689,12 +978,13 @@ static void __blk_mq_insert_request(struct blk_mq_hw_ctx *hctx,
 		list_add(&rq->queuelist, &ctx->rq_list);
 	else
 		list_add_tail(&rq->queuelist, &ctx->rq_list);
+
 	blk_mq_hctx_mark_pending(hctx, ctx);
 
 	/*
 	 * We do this early, to ensure we are on the right CPU.
 	 */
-	blk_mq_add_timer(rq);
+	blk_add_timer(rq);
 }
 
 void blk_mq_insert_request(struct request *rq, bool at_head, bool run_queue,
@@ -719,10 +1009,10 @@ void blk_mq_insert_request(struct request *rq, bool at_head, bool run_queue,
 		spin_unlock(&ctx->lock);
 	}
 
-	blk_mq_put_ctx(current_ctx);
-
 	if (run_queue)
 		blk_mq_run_hw_queue(hctx, async);
+
+	blk_mq_put_ctx(current_ctx);
 }
 
 static void blk_mq_insert_requests(struct request_queue *q,
@@ -758,9 +1048,8 @@ static void blk_mq_insert_requests(struct request_queue *q,
 	}
 	spin_unlock(&ctx->lock);
 
-	blk_mq_put_ctx(current_ctx);
-
 	blk_mq_run_hw_queue(hctx, from_schedule);
+	blk_mq_put_ctx(current_ctx);
 }
 
 static int plug_ctx_cmp(void *priv, struct list_head *a, struct list_head *b)
@@ -823,24 +1112,169 @@ void blk_mq_flush_plug_list(struct blk_plug *plug, bool from_schedule)
 static void blk_mq_bio_to_request(struct request *rq, struct bio *bio)
 {
 	init_request_from_bio(rq, bio);
-	blk_account_io_start(rq, 1);
+
+	if (blk_do_io_stat(rq)) {
+		rq->start_time = jiffies;
+		blk_account_io_start(rq, 1);
+	}
 }
 
-static void blk_mq_make_request(struct request_queue *q, struct bio *bio)
+static inline bool blk_mq_merge_queue_io(struct blk_mq_hw_ctx *hctx,
+					 struct blk_mq_ctx *ctx,
+					 struct request *rq, struct bio *bio)
+{
+	struct request_queue *q = hctx->queue;
+
+	if (!(hctx->flags & BLK_MQ_F_SHOULD_MERGE)) {
+		blk_mq_bio_to_request(rq, bio);
+		spin_lock(&ctx->lock);
+insert_rq:
+		__blk_mq_insert_request(hctx, rq, false);
+		spin_unlock(&ctx->lock);
+		return false;
+	} else {
+		spin_lock(&ctx->lock);
+		if (!blk_mq_attempt_merge(q, ctx, bio)) {
+			blk_mq_bio_to_request(rq, bio);
+			goto insert_rq;
+		}
+
+		spin_unlock(&ctx->lock);
+		__blk_mq_free_request(hctx, ctx, rq);
+		return true;
+	}
+}
+
+struct blk_map_ctx {
+	struct blk_mq_hw_ctx *hctx;
+	struct blk_mq_ctx *ctx;
+};
+
+static struct request *blk_mq_map_request(struct request_queue *q,
+					  struct bio *bio,
+					  struct blk_map_ctx *data)
 {
 	struct blk_mq_hw_ctx *hctx;
 	struct blk_mq_ctx *ctx;
+	struct request *rq;
+	int rw = bio_data_dir(bio);
+
+	if (unlikely(blk_mq_queue_enter(q))) {
+		bio_endio(bio, -EIO);
+		return NULL;
+	}
+
+	ctx = blk_mq_get_ctx(q);
+	hctx = q->mq_ops->map_queue(q, ctx->cpu);
+
+	if (rw_is_sync(bio->bi_rw))
+		rw |= REQ_SYNC;
+
+	trace_block_getrq(q, bio, rw);
+	rq = __blk_mq_alloc_request(q, hctx, ctx, rw, GFP_ATOMIC, false);
+	if (unlikely(!rq)) {
+		__blk_mq_run_hw_queue(hctx);
+		blk_mq_put_ctx(ctx);
+		trace_block_sleeprq(q, bio, rw);
+
+		ctx = blk_mq_get_ctx(q);
+		hctx = q->mq_ops->map_queue(q, ctx->cpu);
+		rq = __blk_mq_alloc_request(q, hctx, ctx, rw,
+					    __GFP_WAIT|GFP_ATOMIC, false);
+	}
+
+	hctx->queued++;
+	data->hctx = hctx;
+	data->ctx = ctx;
+	return rq;
+}
+
+/*
+ * Multiple hardware queue variant. This will not use per-process plugs,
+ * but will attempt to bypass the hctx queueing if we can go straight to
+ * hardware for SYNC IO.
+ */
+static void blk_mq_make_request(struct request_queue *q, struct bio *bio)
+{
 	const int is_sync = rw_is_sync(bio->bi_rw);
 	const int is_flush_fua = bio->bi_rw & (REQ_FLUSH | REQ_FUA);
-	int rw = bio_data_dir(bio);
+	struct blk_map_ctx data;
 	struct request *rq;
+
+	blk_queue_bounce(q, &bio);
+
+	if (bio_integrity_enabled(bio) && bio_integrity_prep(bio)) {
+		bio_endio(bio, -EIO);
+		return;
+	}
+
+	rq = blk_mq_map_request(q, bio, &data);
+	if (unlikely(!rq))
+		return;
+
+	if (unlikely(is_flush_fua)) {
+		blk_mq_bio_to_request(rq, bio);
+		blk_insert_flush(rq);
+		goto run_queue;
+	}
+
+	if (is_sync) {
+		int ret;
+
+		blk_mq_bio_to_request(rq, bio);
+		blk_mq_start_request(rq, true);
+		blk_add_timer(rq);
+
+		/*
+		 * For OK queue, we are done. For error, kill it. Any other
+		 * error (busy), just add it to our list as we previously
+		 * would have done
+		 */
+		ret = q->mq_ops->queue_rq(data.hctx, rq);
+		if (ret == BLK_MQ_RQ_QUEUE_OK)
+			goto done;
+		else {
+			__blk_mq_requeue_request(rq);
+
+			if (ret == BLK_MQ_RQ_QUEUE_ERROR) {
+				rq->errors = -EIO;
+				blk_mq_end_io(rq, rq->errors);
+				goto done;
+			}
+		}
+	}
+
+	if (!blk_mq_merge_queue_io(data.hctx, data.ctx, rq, bio)) {
+		/*
+		 * For a SYNC request, send it to the hardware immediately. For
+		 * an ASYNC request, just ensure that we run it later on. The
+		 * latter allows for merging opportunities and more efficient
+		 * dispatching.
+		 */
+run_queue:
+		blk_mq_run_hw_queue(data.hctx, !is_sync || is_flush_fua);
+	}
+done:
+	blk_mq_put_ctx(data.ctx);
+}
+
+/*
+ * Single hardware queue variant. This will attempt to use any per-process
+ * plug for merging and IO deferral.
+ */
+static void blk_sq_make_request(struct request_queue *q, struct bio *bio)
+{
+	const int is_sync = rw_is_sync(bio->bi_rw);
+	const int is_flush_fua = bio->bi_rw & (REQ_FLUSH | REQ_FUA);
 	unsigned int use_plug, request_count = 0;
+	struct blk_map_ctx data;
+	struct request *rq;
 
 	/*
 	 * If we have multiple hardware queues, just go directly to
 	 * one of those for sync IO.
 	 */
-	use_plug = !is_flush_fua && ((q->nr_hw_queues == 1) || !is_sync);
+	use_plug = !is_flush_fua && !is_sync;
 
 	blk_queue_bounce(q, &bio);
 
@@ -849,37 +1283,14 @@ static void blk_mq_make_request(struct request_queue *q, struct bio *bio)
 		return;
 	}
 
-	if (use_plug && blk_attempt_plug_merge(q, bio, &request_count))
+	if (use_plug && !blk_queue_nomerges(q) &&
+	    blk_attempt_plug_merge(q, bio, &request_count))
 		return;
 
-	if (blk_mq_queue_enter(q)) {
-		bio_endio(bio, -EIO);
-		return;
-	}
-
-	ctx = blk_mq_get_ctx(q);
-	hctx = q->mq_ops->map_queue(q, ctx->cpu);
-
-	if (is_sync)
-		rw |= REQ_SYNC;
-	trace_block_getrq(q, bio, rw);
-	rq = __blk_mq_alloc_request(hctx, GFP_ATOMIC, false);
-	if (likely(rq))
-		blk_mq_rq_ctx_init(q, ctx, rq, rw);
-	else {
-		blk_mq_put_ctx(ctx);
-		trace_block_sleeprq(q, bio, rw);
-		rq = blk_mq_alloc_request_pinned(q, rw, __GFP_WAIT|GFP_ATOMIC,
-							false);
-		ctx = rq->mq_ctx;
-		hctx = q->mq_ops->map_queue(q, ctx->cpu);
-	}
-
-	hctx->queued++;
+	rq = blk_mq_map_request(q, bio, &data);
 
 	if (unlikely(is_flush_fua)) {
 		blk_mq_bio_to_request(rq, bio);
-		blk_mq_put_ctx(ctx);
 		blk_insert_flush(rq);
 		goto run_queue;
 	}
@@ -901,31 +1312,23 @@ static void blk_mq_make_request(struct request_queue *q, struct bio *bio)
 				trace_block_plug(q);
 			}
 			list_add_tail(&rq->queuelist, &plug->mq_list);
-			blk_mq_put_ctx(ctx);
+			blk_mq_put_ctx(data.ctx);
 			return;
 		}
 	}
 
-	spin_lock(&ctx->lock);
-
-	if ((hctx->flags & BLK_MQ_F_SHOULD_MERGE) &&
-	    blk_mq_attempt_merge(q, ctx, bio))
-		__blk_mq_free_request(hctx, ctx, rq);
-	else {
-		blk_mq_bio_to_request(rq, bio);
-		__blk_mq_insert_request(hctx, rq, false);
+	if (!blk_mq_merge_queue_io(data.hctx, data.ctx, rq, bio)) {
+		/*
+		 * For a SYNC request, send it to the hardware immediately. For
+		 * an ASYNC request, just ensure that we run it later on. The
+		 * latter allows for merging opportunities and more efficient
+		 * dispatching.
+		 */
+run_queue:
+		blk_mq_run_hw_queue(data.hctx, !is_sync || is_flush_fua);
 	}
 
-	spin_unlock(&ctx->lock);
-	blk_mq_put_ctx(ctx);
-
-	/*
-	 * For a SYNC request, send it to the hardware immediately. For an
-	 * ASYNC request, just ensure that we run it later on. The latter
-	 * allows for merging opportunities and more efficient dispatching.
-	 */
-run_queue:
-	blk_mq_run_hw_queue(hctx, !is_sync || is_flush_fua);
+	blk_mq_put_ctx(data.ctx);
 }
 
 /*
@@ -937,32 +1340,153 @@ struct blk_mq_hw_ctx *blk_mq_map_queue(struct request_queue *q, const int cpu)
 }
 EXPORT_SYMBOL(blk_mq_map_queue);
 
-struct blk_mq_hw_ctx *blk_mq_alloc_single_hw_queue(struct blk_mq_reg *reg,
-						   unsigned int hctx_index)
+static void blk_mq_free_rq_map(struct blk_mq_tag_set *set,
+		struct blk_mq_tags *tags, unsigned int hctx_idx)
 {
-	return kmalloc_node(sizeof(struct blk_mq_hw_ctx),
-				GFP_KERNEL | __GFP_ZERO, reg->numa_node);
+	struct page *page;
+
+	if (tags->rqs && set->ops->exit_request) {
+		int i;
+
+		for (i = 0; i < tags->nr_tags; i++) {
+			if (!tags->rqs[i])
+				continue;
+			set->ops->exit_request(set->driver_data, tags->rqs[i],
+						hctx_idx, i);
+		}
+	}
+
+	while (!list_empty(&tags->page_list)) {
+		page = list_first_entry(&tags->page_list, struct page, lru);
+		list_del_init(&page->lru);
+		__free_pages(page, page->private);
+	}
+
+	kfree(tags->rqs);
+
+	blk_mq_free_tags(tags);
 }
-EXPORT_SYMBOL(blk_mq_alloc_single_hw_queue);
 
-void blk_mq_free_single_hw_queue(struct blk_mq_hw_ctx *hctx,
-				 unsigned int hctx_index)
+static size_t order_to_size(unsigned int order)
 {
-	kfree(hctx);
+	return (size_t)PAGE_SIZE << order;
 }
-EXPORT_SYMBOL(blk_mq_free_single_hw_queue);
 
-static void blk_mq_hctx_notify(void *data, unsigned long action,
-			       unsigned int cpu)
+static struct blk_mq_tags *blk_mq_init_rq_map(struct blk_mq_tag_set *set,
+		unsigned int hctx_idx)
+{
+	struct blk_mq_tags *tags;
+	unsigned int i, j, entries_per_page, max_order = 4;
+	size_t rq_size, left;
+
+	tags = blk_mq_init_tags(set->queue_depth, set->reserved_tags,
+				set->numa_node);
+	if (!tags)
+		return NULL;
+
+	INIT_LIST_HEAD(&tags->page_list);
+
+	tags->rqs = kmalloc_node(set->queue_depth * sizeof(struct request *),
+					GFP_KERNEL, set->numa_node);
+	if (!tags->rqs) {
+		blk_mq_free_tags(tags);
+		return NULL;
+	}
+
+	/*
+	 * rq_size is the size of the request plus driver payload, rounded
+	 * to the cacheline size
+	 */
+	rq_size = round_up(sizeof(struct request) + set->cmd_size,
+				cache_line_size());
+	left = rq_size * set->queue_depth;
+
+	for (i = 0; i < set->queue_depth; ) {
+		int this_order = max_order;
+		struct page *page;
+		int to_do;
+		void *p;
+
+		while (left < order_to_size(this_order - 1) && this_order)
+			this_order--;
+
+		do {
+			page = alloc_pages_node(set->numa_node, GFP_KERNEL,
+						this_order);
+			if (page)
+				break;
+			if (!this_order--)
+				break;
+			if (order_to_size(this_order) < rq_size)
+				break;
+		} while (1);
+
+		if (!page)
+			goto fail;
+
+		page->private = this_order;
+		list_add_tail(&page->lru, &tags->page_list);
+
+		p = page_address(page);
+		entries_per_page = order_to_size(this_order) / rq_size;
+		to_do = min(entries_per_page, set->queue_depth - i);
+		left -= to_do * rq_size;
+		for (j = 0; j < to_do; j++) {
+			tags->rqs[i] = p;
+			if (set->ops->init_request) {
+				if (set->ops->init_request(set->driver_data,
+						tags->rqs[i], hctx_idx, i,
+						set->numa_node))
+					goto fail;
+			}
+
+			p += rq_size;
+			i++;
+		}
+	}
+
+	return tags;
+
+fail:
+	pr_warn("%s: failed to allocate requests\n", __func__);
+	blk_mq_free_rq_map(set, tags, hctx_idx);
+	return NULL;
+}
+
+static void blk_mq_free_bitmap(struct blk_mq_ctxmap *bitmap)
+{
+	kfree(bitmap->map);
+}
+
+static int blk_mq_alloc_bitmap(struct blk_mq_ctxmap *bitmap, int node)
+{
+	unsigned int bpw = 8, total, num_maps, i;
+
+	bitmap->bits_per_word = bpw;
+
+	num_maps = ALIGN(nr_cpu_ids, bpw) / bpw;
+	bitmap->map = kzalloc_node(num_maps * sizeof(struct blk_align_bitmap),
+					GFP_KERNEL, node);
+	if (!bitmap->map)
+		return -ENOMEM;
+
+	bitmap->map_size = num_maps;
+
+	total = nr_cpu_ids;
+	for (i = 0; i < num_maps; i++) {
+		bitmap->map[i].depth = min(total, bitmap->bits_per_word);
+		total -= bitmap->map[i].depth;
+	}
+
+	return 0;
+}
+
+static int blk_mq_hctx_cpu_offline(struct blk_mq_hw_ctx *hctx, int cpu)
 {
-	struct blk_mq_hw_ctx *hctx = data;
 	struct request_queue *q = hctx->queue;
 	struct blk_mq_ctx *ctx;
 	LIST_HEAD(tmp);
 
-	if (action != CPU_DEAD && action != CPU_DEAD_FROZEN)
-		return;
-
 	/*
 	 * Move ctx entries to new CPU, if this one is going away.
 	 */
@@ -971,12 +1495,12 @@ static void blk_mq_hctx_notify(void *data, unsigned long action,
 	spin_lock(&ctx->lock);
 	if (!list_empty(&ctx->rq_list)) {
 		list_splice_init(&ctx->rq_list, &tmp);
-		clear_bit(ctx->index_hw, hctx->ctx_map);
+		blk_mq_hctx_clear_pending(hctx, ctx);
 	}
 	spin_unlock(&ctx->lock);
 
 	if (list_empty(&tmp))
-		return;
+		return NOTIFY_OK;
 
 	ctx = blk_mq_get_ctx(q);
 	spin_lock(&ctx->lock);
@@ -993,210 +1517,103 @@ static void blk_mq_hctx_notify(void *data, unsigned long action,
 	blk_mq_hctx_mark_pending(hctx, ctx);
 
 	spin_unlock(&ctx->lock);
-	blk_mq_put_ctx(ctx);
 
 	blk_mq_run_hw_queue(hctx, true);
+	blk_mq_put_ctx(ctx);
+	return NOTIFY_OK;
 }
 
-static int blk_mq_init_hw_commands(struct blk_mq_hw_ctx *hctx,
-				   int (*init)(void *, struct blk_mq_hw_ctx *,
-					struct request *, unsigned int),
-				   void *data)
+static int blk_mq_hctx_cpu_online(struct blk_mq_hw_ctx *hctx, int cpu)
 {
-	unsigned int i;
-	int ret = 0;
-
-	for (i = 0; i < hctx->queue_depth; i++) {
-		struct request *rq = hctx->rqs[i];
-
-		ret = init(data, hctx, rq, i);
-		if (ret)
-			break;
-	}
-
-	return ret;
-}
+	struct request_queue *q = hctx->queue;
+	struct blk_mq_tag_set *set = q->tag_set;
 
-int blk_mq_init_commands(struct request_queue *q,
-			 int (*init)(void *, struct blk_mq_hw_ctx *,
-					struct request *, unsigned int),
-			 void *data)
-{
-	struct blk_mq_hw_ctx *hctx;
-	unsigned int i;
-	int ret = 0;
+	if (set->tags[hctx->queue_num])
+		return NOTIFY_OK;
 
-	queue_for_each_hw_ctx(q, hctx, i) {
-		ret = blk_mq_init_hw_commands(hctx, init, data);
-		if (ret)
-			break;
-	}
+	set->tags[hctx->queue_num] = blk_mq_init_rq_map(set, hctx->queue_num);
+	if (!set->tags[hctx->queue_num])
+		return NOTIFY_STOP;
 
-	return ret;
+	hctx->tags = set->tags[hctx->queue_num];
+	return NOTIFY_OK;
 }
-EXPORT_SYMBOL(blk_mq_init_commands);
 
-static void blk_mq_free_hw_commands(struct blk_mq_hw_ctx *hctx,
-				    void (*free)(void *, struct blk_mq_hw_ctx *,
-					struct request *, unsigned int),
-				    void *data)
+static int blk_mq_hctx_notify(void *data, unsigned long action,
+			      unsigned int cpu)
 {
-	unsigned int i;
+	struct blk_mq_hw_ctx *hctx = data;
 
-	for (i = 0; i < hctx->queue_depth; i++) {
-		struct request *rq = hctx->rqs[i];
+	if (action == CPU_DEAD || action == CPU_DEAD_FROZEN)
+		return blk_mq_hctx_cpu_offline(hctx, cpu);
+	else if (action == CPU_ONLINE || action == CPU_ONLINE_FROZEN)
+		return blk_mq_hctx_cpu_online(hctx, cpu);
 
-		free(data, hctx, rq, i);
-	}
+	return NOTIFY_OK;
 }
 
-void blk_mq_free_commands(struct request_queue *q,
-			  void (*free)(void *, struct blk_mq_hw_ctx *,
-					struct request *, unsigned int),
-			  void *data)
+static void blk_mq_exit_hw_queues(struct request_queue *q,
+		struct blk_mq_tag_set *set, int nr_queue)
 {
 	struct blk_mq_hw_ctx *hctx;
 	unsigned int i;
 
-	queue_for_each_hw_ctx(q, hctx, i)
-		blk_mq_free_hw_commands(hctx, free, data);
-}
-EXPORT_SYMBOL(blk_mq_free_commands);
+	queue_for_each_hw_ctx(q, hctx, i) {
+		if (i == nr_queue)
+			break;
 
-static void blk_mq_free_rq_map(struct blk_mq_hw_ctx *hctx)
-{
-	struct page *page;
+		if (set->ops->exit_hctx)
+			set->ops->exit_hctx(hctx, i);
 
-	while (!list_empty(&hctx->page_list)) {
-		page = list_first_entry(&hctx->page_list, struct page, lru);
-		list_del_init(&page->lru);
-		__free_pages(page, page->private);
+		blk_mq_unregister_cpu_notifier(&hctx->cpu_notifier);
+		kfree(hctx->ctxs);
+		blk_mq_free_bitmap(&hctx->ctx_map);
 	}
 
-	kfree(hctx->rqs);
-
-	if (hctx->tags)
-		blk_mq_free_tags(hctx->tags);
-}
-
-static size_t order_to_size(unsigned int order)
-{
-	size_t ret = PAGE_SIZE;
-
-	while (order--)
-		ret *= 2;
-
-	return ret;
 }
 
-static int blk_mq_init_rq_map(struct blk_mq_hw_ctx *hctx,
-			      unsigned int reserved_tags, int node)
+static void blk_mq_free_hw_queues(struct request_queue *q,
+		struct blk_mq_tag_set *set)
 {
-	unsigned int i, j, entries_per_page, max_order = 4;
-	size_t rq_size, left;
-
-	INIT_LIST_HEAD(&hctx->page_list);
-
-	hctx->rqs = kmalloc_node(hctx->queue_depth * sizeof(struct request *),
-					GFP_KERNEL, node);
-	if (!hctx->rqs)
-		return -ENOMEM;
-
-	/*
-	 * rq_size is the size of the request plus driver payload, rounded
-	 * to the cacheline size
-	 */
-	rq_size = round_up(sizeof(struct request) + hctx->cmd_size,
-				cache_line_size());
-	left = rq_size * hctx->queue_depth;
-
-	for (i = 0; i < hctx->queue_depth;) {
-		int this_order = max_order;
-		struct page *page;
-		int to_do;
-		void *p;
-
-		while (left < order_to_size(this_order - 1) && this_order)
-			this_order--;
-
-		do {
-			page = alloc_pages_node(node, GFP_KERNEL, this_order);
-			if (page)
-				break;
-			if (!this_order--)
-				break;
-			if (order_to_size(this_order) < rq_size)
-				break;
-		} while (1);
-
-		if (!page)
-			break;
-
-		page->private = this_order;
-		list_add_tail(&page->lru, &hctx->page_list);
-
-		p = page_address(page);
-		entries_per_page = order_to_size(this_order) / rq_size;
-		to_do = min(entries_per_page, hctx->queue_depth - i);
-		left -= to_do * rq_size;
-		for (j = 0; j < to_do; j++) {
-			hctx->rqs[i] = p;
-			blk_mq_rq_init(hctx, hctx->rqs[i]);
-			p += rq_size;
-			i++;
-		}
-	}
-
-	if (i < (reserved_tags + BLK_MQ_TAG_MIN))
-		goto err_rq_map;
-	else if (i != hctx->queue_depth) {
-		hctx->queue_depth = i;
-		pr_warn("%s: queue depth set to %u because of low memory\n",
-					__func__, i);
-	}
+	struct blk_mq_hw_ctx *hctx;
+	unsigned int i;
 
-	hctx->tags = blk_mq_init_tags(hctx->queue_depth, reserved_tags, node);
-	if (!hctx->tags) {
-err_rq_map:
-		blk_mq_free_rq_map(hctx);
-		return -ENOMEM;
+	queue_for_each_hw_ctx(q, hctx, i) {
+		free_cpumask_var(hctx->cpumask);
+		kfree(hctx);
 	}
-
-	return 0;
 }
 
 static int blk_mq_init_hw_queues(struct request_queue *q,
-				 struct blk_mq_reg *reg, void *driver_data)
+		struct blk_mq_tag_set *set)
 {
 	struct blk_mq_hw_ctx *hctx;
-	unsigned int i, j;
+	unsigned int i;
 
 	/*
 	 * Initialize hardware queues
 	 */
 	queue_for_each_hw_ctx(q, hctx, i) {
-		unsigned int num_maps;
 		int node;
 
 		node = hctx->numa_node;
 		if (node == NUMA_NO_NODE)
-			node = hctx->numa_node = reg->numa_node;
+			node = hctx->numa_node = set->numa_node;
 
-		INIT_DELAYED_WORK(&hctx->delayed_work, blk_mq_work_fn);
+		INIT_DELAYED_WORK(&hctx->run_work, blk_mq_run_work_fn);
+		INIT_DELAYED_WORK(&hctx->delay_work, blk_mq_delay_work_fn);
 		spin_lock_init(&hctx->lock);
 		INIT_LIST_HEAD(&hctx->dispatch);
 		hctx->queue = q;
 		hctx->queue_num = i;
-		hctx->flags = reg->flags;
-		hctx->queue_depth = reg->queue_depth;
-		hctx->cmd_size = reg->cmd_size;
+		hctx->flags = set->flags;
+		hctx->cmd_size = set->cmd_size;
 
 		blk_mq_init_cpu_notifier(&hctx->cpu_notifier,
 						blk_mq_hctx_notify, hctx);
 		blk_mq_register_cpu_notifier(&hctx->cpu_notifier);
 
-		if (blk_mq_init_rq_map(hctx, reg->reserved_tags, node))
-			break;
+		hctx->tags = set->tags[i];
 
 		/*
 		 * Allocate space for all possible cpus to avoid allocation in
@@ -1207,17 +1624,13 @@ static int blk_mq_init_hw_queues(struct request_queue *q,
 		if (!hctx->ctxs)
 			break;
 
-		num_maps = ALIGN(nr_cpu_ids, BITS_PER_LONG) / BITS_PER_LONG;
-		hctx->ctx_map = kzalloc_node(num_maps * sizeof(unsigned long),
-						GFP_KERNEL, node);
-		if (!hctx->ctx_map)
+		if (blk_mq_alloc_bitmap(&hctx->ctx_map, node))
 			break;
 
-		hctx->nr_ctx_map = num_maps;
 		hctx->nr_ctx = 0;
 
-		if (reg->ops->init_hctx &&
-		    reg->ops->init_hctx(hctx, driver_data, i))
+		if (set->ops->init_hctx &&
+		    set->ops->init_hctx(hctx, set->driver_data, i))
 			break;
 	}
 
@@ -1227,17 +1640,7 @@ static int blk_mq_init_hw_queues(struct request_queue *q,
 	/*
 	 * Init failed
 	 */
-	queue_for_each_hw_ctx(q, hctx, j) {
-		if (i == j)
-			break;
-
-		if (reg->ops->exit_hctx)
-			reg->ops->exit_hctx(hctx, j);
-
-		blk_mq_unregister_cpu_notifier(&hctx->cpu_notifier);
-		blk_mq_free_rq_map(hctx);
-		kfree(hctx->ctxs);
-	}
+	blk_mq_exit_hw_queues(q, set, i);
 
 	return 1;
 }
@@ -1258,12 +1661,13 @@ static void blk_mq_init_cpu_queues(struct request_queue *q,
 		__ctx->queue = q;
 
 		/* If the cpu isn't online, the cpu is mapped to first hctx */
-		hctx = q->mq_ops->map_queue(q, i);
-		hctx->nr_ctx++;
-
 		if (!cpu_online(i))
 			continue;
 
+		hctx = q->mq_ops->map_queue(q, i);
+		cpumask_set_cpu(i, hctx->cpumask);
+		hctx->nr_ctx++;
+
 		/*
 		 * Set local node, IFF we have more than one hw queue. If
 		 * not, we remain on the home node of the device
@@ -1280,6 +1684,7 @@ static void blk_mq_map_swqueue(struct request_queue *q)
 	struct blk_mq_ctx *ctx;
 
 	queue_for_each_hw_ctx(q, hctx, i) {
+		cpumask_clear(hctx->cpumask);
 		hctx->nr_ctx = 0;
 	}
 
@@ -1288,115 +1693,208 @@ static void blk_mq_map_swqueue(struct request_queue *q)
 	 */
 	queue_for_each_ctx(q, ctx, i) {
 		/* If the cpu isn't online, the cpu is mapped to first hctx */
+		if (!cpu_online(i))
+			continue;
+
 		hctx = q->mq_ops->map_queue(q, i);
+		cpumask_set_cpu(i, hctx->cpumask);
 		ctx->index_hw = hctx->nr_ctx;
 		hctx->ctxs[hctx->nr_ctx++] = ctx;
 	}
+
+	queue_for_each_hw_ctx(q, hctx, i) {
+		/*
+		 * If not software queues are mapped to this hardware queue,
+		 * disable it and free the request entries
+		 */
+		if (!hctx->nr_ctx) {
+			struct blk_mq_tag_set *set = q->tag_set;
+
+			if (set->tags[i]) {
+				blk_mq_free_rq_map(set, set->tags[i], i);
+				set->tags[i] = NULL;
+				hctx->tags = NULL;
+			}
+			continue;
+		}
+
+		/*
+		 * Initialize batch roundrobin counts
+		 */
+		hctx->next_cpu = cpumask_first(hctx->cpumask);
+		hctx->next_cpu_batch = BLK_MQ_CPU_WORK_BATCH;
+	}
 }
 
-struct request_queue *blk_mq_init_queue(struct blk_mq_reg *reg,
-					void *driver_data)
+static void blk_mq_update_tag_set_depth(struct blk_mq_tag_set *set)
 {
-	struct blk_mq_hw_ctx **hctxs;
-	struct blk_mq_ctx *ctx;
+	struct blk_mq_hw_ctx *hctx;
 	struct request_queue *q;
+	bool shared;
 	int i;
 
-	if (!reg->nr_hw_queues ||
-	    !reg->ops->queue_rq || !reg->ops->map_queue ||
-	    !reg->ops->alloc_hctx || !reg->ops->free_hctx)
-		return ERR_PTR(-EINVAL);
+	if (set->tag_list.next == set->tag_list.prev)
+		shared = false;
+	else
+		shared = true;
+
+	list_for_each_entry(q, &set->tag_list, tag_set_list) {
+		blk_mq_freeze_queue(q);
 
-	if (!reg->queue_depth)
-		reg->queue_depth = BLK_MQ_MAX_DEPTH;
-	else if (reg->queue_depth > BLK_MQ_MAX_DEPTH) {
-		pr_err("blk-mq: queuedepth too large (%u)\n", reg->queue_depth);
-		reg->queue_depth = BLK_MQ_MAX_DEPTH;
+		queue_for_each_hw_ctx(q, hctx, i) {
+			if (shared)
+				hctx->flags |= BLK_MQ_F_TAG_SHARED;
+			else
+				hctx->flags &= ~BLK_MQ_F_TAG_SHARED;
+		}
+		blk_mq_unfreeze_queue(q);
 	}
+}
 
-	if (reg->queue_depth < (reg->reserved_tags + BLK_MQ_TAG_MIN))
-		return ERR_PTR(-EINVAL);
+static void blk_mq_del_queue_tag_set(struct request_queue *q)
+{
+	struct blk_mq_tag_set *set = q->tag_set;
+
+	blk_mq_freeze_queue(q);
+
+	mutex_lock(&set->tag_list_lock);
+	list_del_init(&q->tag_set_list);
+	blk_mq_update_tag_set_depth(set);
+	mutex_unlock(&set->tag_list_lock);
+
+	blk_mq_unfreeze_queue(q);
+}
+
+static void blk_mq_add_queue_tag_set(struct blk_mq_tag_set *set,
+				     struct request_queue *q)
+{
+	q->tag_set = set;
+
+	mutex_lock(&set->tag_list_lock);
+	list_add_tail(&q->tag_set_list, &set->tag_list);
+	blk_mq_update_tag_set_depth(set);
+	mutex_unlock(&set->tag_list_lock);
+}
+
+struct request_queue *blk_mq_init_queue(struct blk_mq_tag_set *set)
+{
+	struct blk_mq_hw_ctx **hctxs;
+	struct blk_mq_ctx *ctx;
+	struct request_queue *q;
+	unsigned int *map;
+	int i;
 
 	ctx = alloc_percpu(struct blk_mq_ctx);
 	if (!ctx)
 		return ERR_PTR(-ENOMEM);
 
-	hctxs = kmalloc_node(reg->nr_hw_queues * sizeof(*hctxs), GFP_KERNEL,
-			reg->numa_node);
+	hctxs = kmalloc_node(set->nr_hw_queues * sizeof(*hctxs), GFP_KERNEL,
+			set->numa_node);
 
 	if (!hctxs)
 		goto err_percpu;
 
-	for (i = 0; i < reg->nr_hw_queues; i++) {
-		hctxs[i] = reg->ops->alloc_hctx(reg, i);
+	map = blk_mq_make_queue_map(set);
+	if (!map)
+		goto err_map;
+
+	for (i = 0; i < set->nr_hw_queues; i++) {
+		int node = blk_mq_hw_queue_to_node(map, i);
+
+		hctxs[i] = kzalloc_node(sizeof(struct blk_mq_hw_ctx),
+					GFP_KERNEL, node);
 		if (!hctxs[i])
 			goto err_hctxs;
 
-		hctxs[i]->numa_node = NUMA_NO_NODE;
+		if (!zalloc_cpumask_var(&hctxs[i]->cpumask, GFP_KERNEL))
+			goto err_hctxs;
+
+		atomic_set(&hctxs[i]->nr_active, 0);
+		hctxs[i]->numa_node = node;
 		hctxs[i]->queue_num = i;
 	}
 
-	q = blk_alloc_queue_node(GFP_KERNEL, reg->numa_node);
+	q = blk_alloc_queue_node(GFP_KERNEL, set->numa_node);
 	if (!q)
 		goto err_hctxs;
 
-	q->mq_map = blk_mq_make_queue_map(reg);
-	if (!q->mq_map)
+	if (percpu_counter_init(&q->mq_usage_counter, 0))
 		goto err_map;
 
 	setup_timer(&q->timeout, blk_mq_rq_timer, (unsigned long) q);
 	blk_queue_rq_timeout(q, 30000);
 
 	q->nr_queues = nr_cpu_ids;
-	q->nr_hw_queues = reg->nr_hw_queues;
+	q->nr_hw_queues = set->nr_hw_queues;
+	q->mq_map = map;
 
 	q->queue_ctx = ctx;
 	q->queue_hw_ctx = hctxs;
 
-	q->mq_ops = reg->ops;
+	q->mq_ops = set->ops;
 	q->queue_flags |= QUEUE_FLAG_MQ_DEFAULT;
 
+	if (!(set->flags & BLK_MQ_F_SG_MERGE))
+		q->queue_flags |= 1 << QUEUE_FLAG_NO_SG_MERGE;
+
 	q->sg_reserved_size = INT_MAX;
 
-	blk_queue_make_request(q, blk_mq_make_request);
-	blk_queue_rq_timed_out(q, reg->ops->timeout);
-	if (reg->timeout)
-		blk_queue_rq_timeout(q, reg->timeout);
+	INIT_WORK(&q->requeue_work, blk_mq_requeue_work);
+	INIT_LIST_HEAD(&q->requeue_list);
+	spin_lock_init(&q->requeue_lock);
+
+	if (q->nr_hw_queues > 1)
+		blk_queue_make_request(q, blk_mq_make_request);
+	else
+		blk_queue_make_request(q, blk_sq_make_request);
+
+	blk_queue_rq_timed_out(q, blk_mq_rq_timed_out);
+	if (set->timeout)
+		blk_queue_rq_timeout(q, set->timeout);
+
+	/*
+	 * Do this after blk_queue_make_request() overrides it...
+	 */
+	q->nr_requests = set->queue_depth;
 
-	if (reg->ops->complete)
-		blk_queue_softirq_done(q, reg->ops->complete);
+	if (set->ops->complete)
+		blk_queue_softirq_done(q, set->ops->complete);
 
 	blk_mq_init_flush(q);
-	blk_mq_init_cpu_queues(q, reg->nr_hw_queues);
+	blk_mq_init_cpu_queues(q, set->nr_hw_queues);
 
-	q->flush_rq = kzalloc(round_up(sizeof(struct request) + reg->cmd_size,
-				cache_line_size()), GFP_KERNEL);
+	q->flush_rq = kzalloc(round_up(sizeof(struct request) +
+				set->cmd_size, cache_line_size()),
+				GFP_KERNEL);
 	if (!q->flush_rq)
 		goto err_hw;
 
-	if (blk_mq_init_hw_queues(q, reg, driver_data))
+	if (blk_mq_init_hw_queues(q, set))
 		goto err_flush_rq;
 
-	blk_mq_map_swqueue(q);
-
 	mutex_lock(&all_q_mutex);
 	list_add_tail(&q->all_q_node, &all_q_list);
 	mutex_unlock(&all_q_mutex);
 
+	blk_mq_add_queue_tag_set(set, q);
+
+	blk_mq_map_swqueue(q);
+
 	return q;
 
 err_flush_rq:
 	kfree(q->flush_rq);
 err_hw:
-	kfree(q->mq_map);
-err_map:
 	blk_cleanup_queue(q);
 err_hctxs:
-	for (i = 0; i < reg->nr_hw_queues; i++) {
+	kfree(map);
+	for (i = 0; i < set->nr_hw_queues; i++) {
 		if (!hctxs[i])
 			break;
-		reg->ops->free_hctx(hctxs[i], i);
+		free_cpumask_var(hctxs[i]->cpumask);
+		kfree(hctxs[i]);
 	}
+err_map:
 	kfree(hctxs);
 err_percpu:
 	free_percpu(ctx);
@@ -1406,18 +1904,14 @@ EXPORT_SYMBOL(blk_mq_init_queue);
 
 void blk_mq_free_queue(struct request_queue *q)
 {
-	struct blk_mq_hw_ctx *hctx;
-	int i;
+	struct blk_mq_tag_set	*set = q->tag_set;
 
-	queue_for_each_hw_ctx(q, hctx, i) {
-		kfree(hctx->ctx_map);
-		kfree(hctx->ctxs);
-		blk_mq_free_rq_map(hctx);
-		blk_mq_unregister_cpu_notifier(&hctx->cpu_notifier);
-		if (q->mq_ops->exit_hctx)
-			q->mq_ops->exit_hctx(hctx, i);
-		q->mq_ops->free_hctx(hctx, i);
-	}
+	blk_mq_del_queue_tag_set(q);
+
+	blk_mq_exit_hw_queues(q, set, set->nr_hw_queues);
+	blk_mq_free_hw_queues(q, set);
+
+	percpu_counter_destroy(&q->mq_usage_counter);
 
 	free_percpu(q->queue_ctx);
 	kfree(q->queue_hw_ctx);
@@ -1437,6 +1931,8 @@ static void blk_mq_queue_reinit(struct request_queue *q)
 {
 	blk_mq_freeze_queue(q);
 
+	blk_mq_sysfs_unregister(q);
+
 	blk_mq_update_queue_map(q->mq_map, q->nr_hw_queues);
 
 	/*
@@ -1447,6 +1943,8 @@ static void blk_mq_queue_reinit(struct request_queue *q)
 
 	blk_mq_map_swqueue(q);
 
+	blk_mq_sysfs_register(q);
+
 	blk_mq_unfreeze_queue(q);
 }
 
@@ -1456,10 +1954,10 @@ static int blk_mq_queue_reinit_notify(struct notifier_block *nb,
 	struct request_queue *q;
 
 	/*
-	 * Before new mapping is established, hotadded cpu might already start
-	 * handling requests. This doesn't break anything as we map offline
-	 * CPUs to first hardware queue. We will re-init queue below to get
-	 * optimal settings.
+	 * Before new mappings are established, hotadded cpu might already
+	 * start handling requests. This doesn't break anything as we map
+	 * offline CPUs to first hardware queue. We will re-init the queue
+	 * below to get optimal settings.
 	 */
 	if (action != CPU_DEAD && action != CPU_DEAD_FROZEN &&
 	    action != CPU_ONLINE && action != CPU_ONLINE_FROZEN)
@@ -1472,6 +1970,81 @@ static int blk_mq_queue_reinit_notify(struct notifier_block *nb,
 	return NOTIFY_OK;
 }
 
+int blk_mq_alloc_tag_set(struct blk_mq_tag_set *set)
+{
+	int i;
+
+	if (!set->nr_hw_queues)
+		return -EINVAL;
+	if (!set->queue_depth || set->queue_depth > BLK_MQ_MAX_DEPTH)
+		return -EINVAL;
+	if (set->queue_depth < set->reserved_tags + BLK_MQ_TAG_MIN)
+		return -EINVAL;
+
+	if (!set->nr_hw_queues || !set->ops->queue_rq || !set->ops->map_queue)
+		return -EINVAL;
+
+
+	set->tags = kmalloc_node(set->nr_hw_queues *
+				 sizeof(struct blk_mq_tags *),
+				 GFP_KERNEL, set->numa_node);
+	if (!set->tags)
+		goto out;
+
+	for (i = 0; i < set->nr_hw_queues; i++) {
+		set->tags[i] = blk_mq_init_rq_map(set, i);
+		if (!set->tags[i])
+			goto out_unwind;
+	}
+
+	mutex_init(&set->tag_list_lock);
+	INIT_LIST_HEAD(&set->tag_list);
+
+	return 0;
+
+out_unwind:
+	while (--i >= 0)
+		blk_mq_free_rq_map(set, set->tags[i], i);
+out:
+	return -ENOMEM;
+}
+EXPORT_SYMBOL(blk_mq_alloc_tag_set);
+
+void blk_mq_free_tag_set(struct blk_mq_tag_set *set)
+{
+	int i;
+
+	for (i = 0; i < set->nr_hw_queues; i++) {
+		if (set->tags[i])
+			blk_mq_free_rq_map(set, set->tags[i], i);
+	}
+
+	kfree(set->tags);
+}
+EXPORT_SYMBOL(blk_mq_free_tag_set);
+
+int blk_mq_update_nr_requests(struct request_queue *q, unsigned int nr)
+{
+	struct blk_mq_tag_set *set = q->tag_set;
+	struct blk_mq_hw_ctx *hctx;
+	int i, ret;
+
+	if (!set || nr > set->queue_depth)
+		return -EINVAL;
+
+	ret = 0;
+	queue_for_each_hw_ctx(q, hctx, i) {
+		ret = blk_mq_tag_update_depth(hctx->tags, nr);
+		if (ret)
+			break;
+	}
+
+	if (!ret)
+		q->nr_requests = nr;
+
+	return ret;
+}
+
 void blk_mq_disable_hotplug(void)
 {
 	mutex_lock(&all_q_mutex);
diff --git a/block/blk-mq.h b/block/blk-mq.h
index ebbe6bac9d6..de7b3bbd5bd 100644
--- a/block/blk-mq.h
+++ b/block/blk-mq.h
@@ -1,6 +1,8 @@
 #ifndef INT_BLK_MQ_H
 #define INT_BLK_MQ_H
 
+struct blk_mq_tag_set;
+
 struct blk_mq_ctx {
 	struct {
 		spinlock_t		lock;
@@ -9,7 +11,8 @@ struct blk_mq_ctx {
 
 	unsigned int		cpu;
 	unsigned int		index_hw;
-	unsigned int		ipi_redirect;
+
+	unsigned int		last_tag ____cacheline_aligned_in_smp;
 
 	/* incremented at dispatch time */
 	unsigned long		rq_dispatched[2];
@@ -20,21 +23,23 @@ struct blk_mq_ctx {
 
 	struct request_queue	*queue;
 	struct kobject		kobj;
-};
+} ____cacheline_aligned_in_smp;
 
 void __blk_mq_complete_request(struct request *rq);
 void blk_mq_run_hw_queue(struct blk_mq_hw_ctx *hctx, bool async);
 void blk_mq_init_flush(struct request_queue *q);
 void blk_mq_drain_queue(struct request_queue *q);
 void blk_mq_free_queue(struct request_queue *q);
-void blk_mq_rq_init(struct blk_mq_hw_ctx *hctx, struct request *rq);
+void blk_mq_clone_flush_request(struct request *flush_rq,
+		struct request *orig_rq);
+int blk_mq_update_nr_requests(struct request_queue *q, unsigned int nr);
 
 /*
  * CPU hotplug helpers
  */
 struct blk_mq_cpu_notifier;
 void blk_mq_init_cpu_notifier(struct blk_mq_cpu_notifier *notifier,
-			      void (*fn)(void *, unsigned long, unsigned int),
+			      int (*fn)(void *, unsigned long, unsigned int),
 			      void *data);
 void blk_mq_register_cpu_notifier(struct blk_mq_cpu_notifier *notifier);
 void blk_mq_unregister_cpu_notifier(struct blk_mq_cpu_notifier *notifier);
@@ -45,10 +50,23 @@ void blk_mq_disable_hotplug(void);
 /*
  * CPU -> queue mappings
  */
-struct blk_mq_reg;
-extern unsigned int *blk_mq_make_queue_map(struct blk_mq_reg *reg);
+extern unsigned int *blk_mq_make_queue_map(struct blk_mq_tag_set *set);
 extern int blk_mq_update_queue_map(unsigned int *map, unsigned int nr_queues);
+extern int blk_mq_hw_queue_to_node(unsigned int *map, unsigned int);
 
-void blk_mq_add_timer(struct request *rq);
+/*
+ * sysfs helpers
+ */
+extern int blk_mq_sysfs_register(struct request_queue *q);
+extern void blk_mq_sysfs_unregister(struct request_queue *q);
+
+/*
+ * Basic implementation of sparser bitmap, allowing the user to spread
+ * the bits over more cachelines.
+ */
+struct blk_align_bitmap {
+	unsigned long word;
+	unsigned long depth;
+} ____cacheline_aligned_in_smp;
 
 #endif
diff --git a/block/blk-sysfs.c b/block/blk-sysfs.c
index 7500f876dae..23321fbab29 100644
--- a/block/blk-sysfs.c
+++ b/block/blk-sysfs.c
@@ -48,11 +48,10 @@ static ssize_t queue_requests_show(struct request_queue *q, char *page)
 static ssize_t
 queue_requests_store(struct request_queue *q, const char *page, size_t count)
 {
-	struct request_list *rl;
 	unsigned long nr;
-	int ret;
+	int ret, err;
 
-	if (!q->request_fn)
+	if (!q->request_fn && !q->mq_ops)
 		return -EINVAL;
 
 	ret = queue_var_store(&nr, page, count);
@@ -62,40 +61,14 @@ queue_requests_store(struct request_queue *q, const char *page, size_t count)
 	if (nr < BLKDEV_MIN_RQ)
 		nr = BLKDEV_MIN_RQ;
 
-	spin_lock_irq(q->queue_lock);
-	q->nr_requests = nr;
-	blk_queue_congestion_threshold(q);
-
-	/* congestion isn't cgroup aware and follows root blkcg for now */
-	rl = &q->root_rl;
-
-	if (rl->count[BLK_RW_SYNC] >= queue_congestion_on_threshold(q))
-		blk_set_queue_congested(q, BLK_RW_SYNC);
-	else if (rl->count[BLK_RW_SYNC] < queue_congestion_off_threshold(q))
-		blk_clear_queue_congested(q, BLK_RW_SYNC);
-
-	if (rl->count[BLK_RW_ASYNC] >= queue_congestion_on_threshold(q))
-		blk_set_queue_congested(q, BLK_RW_ASYNC);
-	else if (rl->count[BLK_RW_ASYNC] < queue_congestion_off_threshold(q))
-		blk_clear_queue_congested(q, BLK_RW_ASYNC);
-
-	blk_queue_for_each_rl(rl, q) {
-		if (rl->count[BLK_RW_SYNC] >= q->nr_requests) {
-			blk_set_rl_full(rl, BLK_RW_SYNC);
-		} else {
-			blk_clear_rl_full(rl, BLK_RW_SYNC);
-			wake_up(&rl->wait[BLK_RW_SYNC]);
-		}
-
-		if (rl->count[BLK_RW_ASYNC] >= q->nr_requests) {
-			blk_set_rl_full(rl, BLK_RW_ASYNC);
-		} else {
-			blk_clear_rl_full(rl, BLK_RW_ASYNC);
-			wake_up(&rl->wait[BLK_RW_ASYNC]);
-		}
-	}
+	if (q->request_fn)
+		err = blk_update_nr_requests(q, nr);
+	else
+		err = blk_mq_update_nr_requests(q, nr);
+
+	if (err)
+		return err;
 
-	spin_unlock_irq(q->queue_lock);
 	return ret;
 }
 
@@ -544,8 +517,6 @@ static void blk_release_queue(struct kobject *kobj)
 	if (q->queue_tags)
 		__blk_queue_free_tags(q);
 
-	percpu_counter_destroy(&q->mq_usage_counter);
-
 	if (q->mq_ops)
 		blk_mq_free_queue(q);
 
diff --git a/block/blk-throttle.c b/block/blk-throttle.c
index 033745cd7fb..9353b468335 100644
--- a/block/blk-throttle.c
+++ b/block/blk-throttle.c
@@ -744,7 +744,7 @@ static inline void throtl_extend_slice(struct throtl_grp *tg, bool rw,
 static bool throtl_slice_used(struct throtl_grp *tg, bool rw)
 {
 	if (time_in_range(jiffies, tg->slice_start[rw], tg->slice_end[rw]))
-		return 0;
+		return false;
 
 	return 1;
 }
@@ -842,7 +842,7 @@ static bool tg_with_in_iops_limit(struct throtl_grp *tg, struct bio *bio,
 	if (tg->io_disp[rw] + 1 <= io_allowed) {
 		if (wait)
 			*wait = 0;
-		return 1;
+		return true;
 	}
 
 	/* Calc approx time to dispatch */
@@ -880,7 +880,7 @@ static bool tg_with_in_bps_limit(struct throtl_grp *tg, struct bio *bio,
 	if (tg->bytes_disp[rw] + bio->bi_iter.bi_size <= bytes_allowed) {
 		if (wait)
 			*wait = 0;
-		return 1;
+		return true;
 	}
 
 	/* Calc approx time to dispatch */
@@ -923,7 +923,7 @@ static bool tg_may_dispatch(struct throtl_grp *tg, struct bio *bio,
 	if (tg->bps[rw] == -1 && tg->iops[rw] == -1) {
 		if (wait)
 			*wait = 0;
-		return 1;
+		return true;
 	}
 
 	/*
@@ -1258,7 +1258,7 @@ out_unlock:
  * of throtl_data->service_queue.  Those bio's are ready and issued by this
  * function.
  */
-void blk_throtl_dispatch_work_fn(struct work_struct *work)
+static void blk_throtl_dispatch_work_fn(struct work_struct *work)
 {
 	struct throtl_data *td = container_of(work, struct throtl_data,
 					      dispatch_work);
diff --git a/block/blk-timeout.c b/block/blk-timeout.c
index d96f7061c6f..95a09590ccf 100644
--- a/block/blk-timeout.c
+++ b/block/blk-timeout.c
@@ -96,11 +96,7 @@ static void blk_rq_timed_out(struct request *req)
 			__blk_complete_request(req);
 		break;
 	case BLK_EH_RESET_TIMER:
-		if (q->mq_ops)
-			blk_mq_add_timer(req);
-		else
-			blk_add_timer(req);
-
+		blk_add_timer(req);
 		blk_clear_rq_complete(req);
 		break;
 	case BLK_EH_NOT_HANDLED:
@@ -170,7 +166,26 @@ void blk_abort_request(struct request *req)
 }
 EXPORT_SYMBOL_GPL(blk_abort_request);
 
-void __blk_add_timer(struct request *req, struct list_head *timeout_list)
+unsigned long blk_rq_timeout(unsigned long timeout)
+{
+	unsigned long maxt;
+
+	maxt = round_jiffies_up(jiffies + BLK_MAX_TIMEOUT);
+	if (time_after(timeout, maxt))
+		timeout = maxt;
+
+	return timeout;
+}
+
+/**
+ * blk_add_timer - Start timeout timer for a single request
+ * @req:	request that is about to start running.
+ *
+ * Notes:
+ *    Each request has its own timer, and as it is added to the queue, we
+ *    set up the timer. When the request completes, we cancel the timer.
+ */
+void blk_add_timer(struct request *req)
 {
 	struct request_queue *q = req->q;
 	unsigned long expiry;
@@ -188,32 +203,29 @@ void __blk_add_timer(struct request *req, struct list_head *timeout_list)
 		req->timeout = q->rq_timeout;
 
 	req->deadline = jiffies + req->timeout;
-	if (timeout_list)
-		list_add_tail(&req->timeout_list, timeout_list);
+	if (!q->mq_ops)
+		list_add_tail(&req->timeout_list, &req->q->timeout_list);
 
 	/*
 	 * If the timer isn't already pending or this timeout is earlier
 	 * than an existing one, modify the timer. Round up to next nearest
 	 * second.
 	 */
-	expiry = round_jiffies_up(req->deadline);
+	expiry = blk_rq_timeout(round_jiffies_up(req->deadline));
 
 	if (!timer_pending(&q->timeout) ||
-	    time_before(expiry, q->timeout.expires))
-		mod_timer(&q->timeout, expiry);
+	    time_before(expiry, q->timeout.expires)) {
+		unsigned long diff = q->timeout.expires - expiry;
 
-}
+		/*
+		 * Due to added timer slack to group timers, the timer
+		 * will often be a little in front of what we asked for.
+		 * So apply some tolerance here too, otherwise we keep
+		 * modifying the timer because expires for value X
+		 * will be X + something.
+		 */
+		if (!timer_pending(&q->timeout) || (diff >= HZ / 2))
+			mod_timer(&q->timeout, expiry);
+	}
 
-/**
- * blk_add_timer - Start timeout timer for a single request
- * @req:	request that is about to start running.
- *
- * Notes:
- *    Each request has its own timer, and as it is added to the queue, we
- *    set up the timer. When the request completes, we cancel the timer.
- */
-void blk_add_timer(struct request *req)
-{
-	__blk_add_timer(req, &req->q->timeout_list);
 }
-
diff --git a/block/blk.h b/block/blk.h
index 1d880f1f957..45385e9abf6 100644
--- a/block/blk.h
+++ b/block/blk.h
@@ -9,6 +9,9 @@
 /* Number of requests a "batching" process may submit */
 #define BLK_BATCH_REQ	32
 
+/* Max future timer expiry for timeouts */
+#define BLK_MAX_TIMEOUT		(5 * HZ)
+
 extern struct kmem_cache *blk_requestq_cachep;
 extern struct kmem_cache *request_cachep;
 extern struct kobj_type blk_queue_ktype;
@@ -37,9 +40,9 @@ bool __blk_end_bidi_request(struct request *rq, int error,
 void blk_rq_timed_out_timer(unsigned long data);
 void blk_rq_check_expired(struct request *rq, unsigned long *next_timeout,
 			  unsigned int *next_set);
-void __blk_add_timer(struct request *req, struct list_head *timeout_list);
+unsigned long blk_rq_timeout(unsigned long timeout);
+void blk_add_timer(struct request *req);
 void blk_delete_timer(struct request *);
-void blk_add_timer(struct request *);
 
 
 bool bio_attempt_front_merge(struct request_queue *q, struct request *req,
@@ -185,6 +188,8 @@ static inline int queue_congestion_off_threshold(struct request_queue *q)
 	return q->nr_congestion_off;
 }
 
+extern int blk_update_nr_requests(struct request_queue *, unsigned int);
+
 /*
  * Contribute to IO statistics IFF:
  *
diff --git a/mm/bounce.c b/block/bounce.c
index 523918b8c6d..523918b8c6d 100644
--- a/mm/bounce.c
+++ b/block/bounce.c
diff --git a/block/bsg.c b/block/bsg.c
index 420a5a9f1b2..e5214c14809 100644
--- a/block/bsg.c
+++ b/block/bsg.c
@@ -1008,7 +1008,7 @@ int bsg_register_queue(struct request_queue *q, struct device *parent,
 	/*
 	 * we need a proper transport to send commands, not a stacked device
 	 */
-	if (!q->request_fn)
+	if (!queue_is_rq_based(q))
 		return 0;
 
 	bcd = &q->bsg_dev;
diff --git a/block/cfq-iosched.c b/block/cfq-iosched.c
index e0985f1955e..22dffebc7c7 100644
--- a/block/cfq-iosched.c
+++ b/block/cfq-iosched.c
@@ -908,7 +908,7 @@ static inline void cfq_schedule_dispatch(struct cfq_data *cfqd)
 {
 	if (cfqd->busy_queues) {
 		cfq_log(cfqd, "schedule dispatch");
-		kblockd_schedule_work(cfqd->queue, &cfqd->unplug_work);
+		kblockd_schedule_work(&cfqd->unplug_work);
 	}
 }
 
@@ -4460,7 +4460,7 @@ out_free:
 static ssize_t
 cfq_var_show(unsigned int var, char *page)
 {
-	return sprintf(page, "%d\n", var);
+	return sprintf(page, "%u\n", var);
 }
 
 static ssize_t
diff --git a/fs/ioprio.c b/block/ioprio.c
index e50170ca7c3..e50170ca7c3 100644
--- a/fs/ioprio.c
+++ b/block/ioprio.c
diff --git a/block/scsi_ioctl.c b/block/scsi_ioctl.c
index 26487972ac5..9c28a5b3804 100644
--- a/block/scsi_ioctl.c
+++ b/block/scsi_ioctl.c
@@ -205,10 +205,6 @@ int blk_verify_command(unsigned char *cmd, fmode_t has_write_perm)
 	if (capable(CAP_SYS_RAWIO))
 		return 0;
 
-	/* if there's no filter set, assume we're filtering everything out */
-	if (!filter)
-		return -EPERM;
-
 	/* Anybody who can open the device can do a read-safe command */
 	if (test_bit(cmd[0], filter->read_ok))
 		return 0;
diff --git a/crypto/crypto_user.c b/crypto/crypto_user.c
index 1512e41cd93..43665d0d090 100644
--- a/crypto/crypto_user.c
+++ b/crypto/crypto_user.c
@@ -466,7 +466,7 @@ static int crypto_user_rcv_msg(struct sk_buff *skb, struct nlmsghdr *nlh)
 	type -= CRYPTO_MSG_BASE;
 	link = &crypto_dispatch[type];
 
-	if (!capable(CAP_NET_ADMIN))
+	if (!netlink_capable(skb, CAP_NET_ADMIN))
 		return -EPERM;
 
 	if ((type == (CRYPTO_MSG_GETALG - CRYPTO_MSG_BASE) &&
diff --git a/drivers/Kconfig b/drivers/Kconfig
index 0a0a90f52d2..0e87a34b647 100644
--- a/drivers/Kconfig
+++ b/drivers/Kconfig
@@ -132,6 +132,8 @@ source "drivers/staging/Kconfig"
 
 source "drivers/platform/Kconfig"
 
+source "drivers/soc/Kconfig"
+
 source "drivers/clk/Kconfig"
 
 source "drivers/hwspinlock/Kconfig"
diff --git a/drivers/Makefile b/drivers/Makefile
index d05d81b19b5..f98b50d8251 100644
--- a/drivers/Makefile
+++ b/drivers/Makefile
@@ -33,6 +33,9 @@ obj-y				+= amba/
 # really early.
 obj-$(CONFIG_DMADEVICES)	+= dma/
 
+# SOC specific infrastructure drivers.
+obj-y				+= soc/
+
 obj-$(CONFIG_VIRTIO)		+= virtio/
 obj-$(CONFIG_XEN)		+= xen/
 
@@ -80,7 +83,6 @@ obj-$(CONFIG_PCCARD)		+= pcmcia/
 obj-$(CONFIG_DIO)		+= dio/
 obj-$(CONFIG_SBUS)		+= sbus/
 obj-$(CONFIG_ZORRO)		+= zorro/
-obj-$(CONFIG_MAC)		+= macintosh/
 obj-$(CONFIG_ATA_OVER_ETH)	+= block/aoe/
 obj-$(CONFIG_PARIDE) 		+= block/paride/
 obj-$(CONFIG_TC)		+= tc/
@@ -119,7 +121,7 @@ obj-$(CONFIG_SGI_SN)		+= sn/
 obj-y				+= firmware/
 obj-$(CONFIG_CRYPTO)		+= crypto/
 obj-$(CONFIG_SUPERH)		+= sh/
-obj-$(CONFIG_ARCH_SHMOBILE_LEGACY)	+= sh/
+obj-$(CONFIG_ARCH_SHMOBILE)	+= sh/
 ifndef CONFIG_ARCH_USES_GETTIMEOFFSET
 obj-y				+= clocksource/
 endif
@@ -138,7 +140,6 @@ obj-y				+= clk/
 
 obj-$(CONFIG_MAILBOX)		+= mailbox/
 obj-$(CONFIG_HWSPINLOCK)	+= hwspinlock/
-obj-$(CONFIG_NFC)		+= nfc/
 obj-$(CONFIG_IOMMU_SUPPORT)	+= iommu/
 obj-$(CONFIG_REMOTEPROC)	+= remoteproc/
 obj-$(CONFIG_RPMSG)		+= rpmsg/
diff --git a/drivers/acpi/Kconfig b/drivers/acpi/Kconfig
index ab686b31010..a34a2284100 100644
--- a/drivers/acpi/Kconfig
+++ b/drivers/acpi/Kconfig
@@ -47,6 +47,23 @@ config ACPI_SLEEP
 	depends on SUSPEND || HIBERNATION
 	default y
 
+config ACPI_PROCFS_POWER
+	bool "Deprecated power /proc/acpi directories"
+	depends on PROC_FS
+	help
+	  For backwards compatibility, this option allows
+          deprecated power /proc/acpi/ directories to exist, even when
+          they have been replaced by functions in /sys.
+          The deprecated directories (and their replacements) include:
+	  /proc/acpi/battery/* (/sys/class/power_supply/*)
+	  /proc/acpi/ac_adapter/* (sys/class/power_supply/*)
+	  This option has no effect on /proc/acpi/ directories
+	  and functions, which do not yet exist in /sys
+	  This option, together with the proc directories, will be
+	  deleted in the future.
+
+	  Say N to delete power /proc/acpi/ directories that have moved to /sys/
+
 config ACPI_EC_DEBUGFS
 	tristate "EC read/write access through /sys/kernel/debug/ec"
 	default n
diff --git a/drivers/acpi/Makefile b/drivers/acpi/Makefile
index 0331f91d56e..bce34afadcd 100644
--- a/drivers/acpi/Makefile
+++ b/drivers/acpi/Makefile
@@ -47,6 +47,7 @@ acpi-y				+= sysfs.o
 acpi-$(CONFIG_X86)		+= acpi_cmos_rtc.o
 acpi-$(CONFIG_DEBUG_FS)		+= debugfs.o
 acpi-$(CONFIG_ACPI_NUMA)	+= numa.o
+acpi-$(CONFIG_ACPI_PROCFS_POWER) += cm_sbs.o
 ifdef CONFIG_ACPI_VIDEO
 acpi-y				+= video_detect.o
 endif
diff --git a/drivers/acpi/ac.c b/drivers/acpi/ac.c
index 2c01c1da29c..c67f6f5ad61 100644
--- a/drivers/acpi/ac.c
+++ b/drivers/acpi/ac.c
@@ -52,11 +52,39 @@ MODULE_AUTHOR("Paul Diefenbaugh");
 MODULE_DESCRIPTION("ACPI AC Adapter Driver");
 MODULE_LICENSE("GPL");
 
+static int acpi_ac_add(struct acpi_device *device);
+static int acpi_ac_remove(struct acpi_device *device);
+static void acpi_ac_notify(struct acpi_device *device, u32 event);
+
+static const struct acpi_device_id ac_device_ids[] = {
+	{"ACPI0003", 0},
+	{"", 0},
+};
+MODULE_DEVICE_TABLE(acpi, ac_device_ids);
+
+#ifdef CONFIG_PM_SLEEP
+static int acpi_ac_resume(struct device *dev);
+#endif
+static SIMPLE_DEV_PM_OPS(acpi_ac_pm, NULL, acpi_ac_resume);
+
 static int ac_sleep_before_get_state_ms;
 
+static struct acpi_driver acpi_ac_driver = {
+	.name = "ac",
+	.class = ACPI_AC_CLASS,
+	.ids = ac_device_ids,
+	.flags = ACPI_DRIVER_ALL_NOTIFY_EVENTS,
+	.ops = {
+		.add = acpi_ac_add,
+		.remove = acpi_ac_remove,
+		.notify = acpi_ac_notify,
+		},
+	.drv.pm = &acpi_ac_pm,
+};
+
 struct acpi_ac {
 	struct power_supply charger;
-	struct platform_device *pdev;
+	struct acpi_device * device;
 	unsigned long long state;
 	struct notifier_block battery_nb;
 };
@@ -69,10 +97,12 @@ struct acpi_ac {
 
 static int acpi_ac_get_state(struct acpi_ac *ac)
 {
-	acpi_status status;
-	acpi_handle handle = ACPI_HANDLE(&ac->pdev->dev);
+	acpi_status status = AE_OK;
+
+	if (!ac)
+		return -EINVAL;
 
-	status = acpi_evaluate_integer(handle, "_PSR", NULL,
+	status = acpi_evaluate_integer(ac->device->handle, "_PSR", NULL,
 				       &ac->state);
 	if (ACPI_FAILURE(status)) {
 		ACPI_EXCEPTION((AE_INFO, status,
@@ -117,10 +147,9 @@ static enum power_supply_property ac_props[] = {
                                    Driver Model
    -------------------------------------------------------------------------- */
 
-static void acpi_ac_notify_handler(acpi_handle handle, u32 event, void *data)
+static void acpi_ac_notify(struct acpi_device *device, u32 event)
 {
-	struct acpi_ac *ac = data;
-	struct acpi_device *adev;
+	struct acpi_ac *ac = acpi_driver_data(device);
 
 	if (!ac)
 		return;
@@ -143,11 +172,10 @@ static void acpi_ac_notify_handler(acpi_handle handle, u32 event, void *data)
 			msleep(ac_sleep_before_get_state_ms);
 
 		acpi_ac_get_state(ac);
-		adev = ACPI_COMPANION(&ac->pdev->dev);
-		acpi_bus_generate_netlink_event(adev->pnp.device_class,
-						dev_name(&ac->pdev->dev),
-						event, (u32) ac->state);
-		acpi_notifier_call_chain(adev, event, (u32) ac->state);
+		acpi_bus_generate_netlink_event(device->pnp.device_class,
+						  dev_name(&device->dev), event,
+						  (u32) ac->state);
+		acpi_notifier_call_chain(device, event, (u32) ac->state);
 		kobject_uevent(&ac->charger.dev->kobj, KOBJ_CHANGE);
 	}
 
@@ -192,49 +220,39 @@ static struct dmi_system_id ac_dmi_table[] = {
 	{},
 };
 
-static int acpi_ac_probe(struct platform_device *pdev)
+static int acpi_ac_add(struct acpi_device *device)
 {
 	int result = 0;
 	struct acpi_ac *ac = NULL;
-	struct acpi_device *adev;
 
-	if (!pdev)
-		return -EINVAL;
 
-	adev = ACPI_COMPANION(&pdev->dev);
-	if (!adev)
-		return -ENODEV;
+	if (!device)
+		return -EINVAL;
 
 	ac = kzalloc(sizeof(struct acpi_ac), GFP_KERNEL);
 	if (!ac)
 		return -ENOMEM;
 
-	strcpy(acpi_device_name(adev), ACPI_AC_DEVICE_NAME);
-	strcpy(acpi_device_class(adev), ACPI_AC_CLASS);
-	ac->pdev = pdev;
-	platform_set_drvdata(pdev, ac);
+	ac->device = device;
+	strcpy(acpi_device_name(device), ACPI_AC_DEVICE_NAME);
+	strcpy(acpi_device_class(device), ACPI_AC_CLASS);
+	device->driver_data = ac;
 
 	result = acpi_ac_get_state(ac);
 	if (result)
 		goto end;
 
-	ac->charger.name = acpi_device_bid(adev);
+	ac->charger.name = acpi_device_bid(device);
 	ac->charger.type = POWER_SUPPLY_TYPE_MAINS;
 	ac->charger.properties = ac_props;
 	ac->charger.num_properties = ARRAY_SIZE(ac_props);
 	ac->charger.get_property = get_ac_property;
-	result = power_supply_register(&pdev->dev, &ac->charger);
+	result = power_supply_register(&ac->device->dev, &ac->charger);
 	if (result)
 		goto end;
 
-	result = acpi_install_notify_handler(ACPI_HANDLE(&pdev->dev),
-			ACPI_ALL_NOTIFY, acpi_ac_notify_handler, ac);
-	if (result) {
-		power_supply_unregister(&ac->charger);
-		goto end;
-	}
 	printk(KERN_INFO PREFIX "%s [%s] (%s)\n",
-	       acpi_device_name(adev), acpi_device_bid(adev),
+	       acpi_device_name(device), acpi_device_bid(device),
 	       ac->state ? "on-line" : "off-line");
 
 	ac->battery_nb.notifier_call = acpi_ac_battery_notify;
@@ -256,7 +274,7 @@ static int acpi_ac_resume(struct device *dev)
 	if (!dev)
 		return -EINVAL;
 
-	ac = platform_get_drvdata(to_platform_device(dev));
+	ac = acpi_driver_data(to_acpi_device(dev));
 	if (!ac)
 		return -EINVAL;
 
@@ -270,19 +288,17 @@ static int acpi_ac_resume(struct device *dev)
 #else
 #define acpi_ac_resume NULL
 #endif
-static SIMPLE_DEV_PM_OPS(acpi_ac_pm_ops, NULL, acpi_ac_resume);
 
-static int acpi_ac_remove(struct platform_device *pdev)
+static int acpi_ac_remove(struct acpi_device *device)
 {
-	struct acpi_ac *ac;
+	struct acpi_ac *ac = NULL;
+
 
-	if (!pdev)
+	if (!device || !acpi_driver_data(device))
 		return -EINVAL;
 
-	acpi_remove_notify_handler(ACPI_HANDLE(&pdev->dev),
-			ACPI_ALL_NOTIFY, acpi_ac_notify_handler);
+	ac = acpi_driver_data(device);
 
-	ac = platform_get_drvdata(pdev);
 	if (ac->charger.dev)
 		power_supply_unregister(&ac->charger);
 	unregister_acpi_notifier(&ac->battery_nb);
@@ -292,23 +308,6 @@ static int acpi_ac_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static const struct acpi_device_id acpi_ac_match[] = {
-	{ "ACPI0003", 0 },
-	{ }
-};
-MODULE_DEVICE_TABLE(acpi, acpi_ac_match);
-
-static struct platform_driver acpi_ac_driver = {
-	.probe          = acpi_ac_probe,
-	.remove         = acpi_ac_remove,
-	.driver         = {
-		.name   = "acpi-ac",
-		.owner  = THIS_MODULE,
-		.pm     = &acpi_ac_pm_ops,
-		.acpi_match_table = ACPI_PTR(acpi_ac_match),
-	},
-};
-
 static int __init acpi_ac_init(void)
 {
 	int result;
@@ -316,7 +315,7 @@ static int __init acpi_ac_init(void)
 	if (acpi_disabled)
 		return -ENODEV;
 
-	result = platform_driver_register(&acpi_ac_driver);
+	result = acpi_bus_register_driver(&acpi_ac_driver);
 	if (result < 0)
 		return -ENODEV;
 
@@ -325,7 +324,7 @@ static int __init acpi_ac_init(void)
 
 static void __exit acpi_ac_exit(void)
 {
-	platform_driver_unregister(&acpi_ac_driver);
+	acpi_bus_unregister_driver(&acpi_ac_driver);
 }
 module_init(acpi_ac_init);
 module_exit(acpi_ac_exit);
diff --git a/drivers/acpi/acpi_lpss.c b/drivers/acpi/acpi_lpss.c
index 69e29f409d4..d79c6d7f598 100644
--- a/drivers/acpi/acpi_lpss.c
+++ b/drivers/acpi/acpi_lpss.c
@@ -180,6 +180,7 @@ static const struct acpi_device_id acpi_lpss_device_ids[] = {
 	{ "80860F14", (unsigned long)&byt_sdio_dev_desc },
 	{ "80860F41", (unsigned long)&byt_i2c_dev_desc },
 	{ "INT33B2", },
+	{ "INT33FC", },
 
 	{ "INT3430", (unsigned long)&lpt_dev_desc },
 	{ "INT3431", (unsigned long)&lpt_dev_desc },
diff --git a/drivers/acpi/acpi_platform.c b/drivers/acpi/acpi_platform.c
index dbfe49e5fd6..1d4950388fa 100644
--- a/drivers/acpi/acpi_platform.c
+++ b/drivers/acpi/acpi_platform.c
@@ -29,7 +29,6 @@ ACPI_MODULE_NAME("platform");
 static const struct acpi_device_id acpi_platform_device_ids[] = {
 
 	{ "PNP0D40" },
-	{ "ACPI0003" },
 	{ "VPC2004" },
 	{ "BCM4752" },
 
diff --git a/drivers/acpi/acpi_processor.c b/drivers/acpi/acpi_processor.c
index c29c2c3ec0a..52c81c49cc7 100644
--- a/drivers/acpi/acpi_processor.c
+++ b/drivers/acpi/acpi_processor.c
@@ -170,6 +170,9 @@ static int acpi_processor_hotadd_init(struct acpi_processor *pr)
 	acpi_status status;
 	int ret;
 
+	if (pr->apic_id == -1)
+		return -ENODEV;
+
 	status = acpi_evaluate_integer(pr->handle, "_STA", NULL, &sta);
 	if (ACPI_FAILURE(status) || !(sta & ACPI_STA_DEVICE_PRESENT))
 		return -ENODEV;
@@ -260,10 +263,8 @@ static int acpi_processor_get_info(struct acpi_device *device)
 	}
 
 	apic_id = acpi_get_apicid(pr->handle, device_declaration, pr->acpi_id);
-	if (apic_id < 0) {
+	if (apic_id < 0)
 		acpi_handle_debug(pr->handle, "failed to get CPU APIC ID.\n");
-		return -ENODEV;
-	}
 	pr->apic_id = apic_id;
 
 	cpu_index = acpi_map_cpuid(pr->apic_id, pr->acpi_id);
@@ -404,7 +405,6 @@ static int acpi_processor_add(struct acpi_device *device,
 		goto err;
 
 	pr->dev = dev;
-	dev->offline = pr->flags.need_hotplug_init;
 
 	/* Trigger the processor driver's .probe() if present. */
 	if (device_attach(dev) >= 0)
diff --git a/drivers/acpi/acpica/acglobal.h b/drivers/acpi/acpica/acglobal.h
index 49bbc71fad5..a08a448068d 100644
--- a/drivers/acpi/acpica/acglobal.h
+++ b/drivers/acpi/acpica/acglobal.h
@@ -141,9 +141,9 @@ ACPI_INIT_GLOBAL(u8, acpi_gbl_do_not_use_xsdt, FALSE);
  * address. Although ACPICA adheres to the ACPI specification which
  * requires the use of the corresponding 64-bit address if it is non-zero,
  * some machines have been found to have a corrupted non-zero 64-bit
- * address. Default is FALSE, do not favor the 32-bit addresses.
+ * address. Default is TRUE, favor the 32-bit addresses.
  */
-ACPI_INIT_GLOBAL(u8, acpi_gbl_use32_bit_fadt_addresses, FALSE);
+ACPI_INIT_GLOBAL(u8, acpi_gbl_use32_bit_fadt_addresses, TRUE);
 
 /*
  * Optionally truncate I/O addresses to 16 bits. Provides compatibility
diff --git a/drivers/acpi/acpica/exfield.c b/drivers/acpi/acpica/exfield.c
index 68d97441432..12878e1982f 100644
--- a/drivers/acpi/acpica/exfield.c
+++ b/drivers/acpi/acpica/exfield.c
@@ -45,10 +45,71 @@
 #include "accommon.h"
 #include "acdispat.h"
 #include "acinterp.h"
+#include "amlcode.h"
 
 #define _COMPONENT          ACPI_EXECUTER
 ACPI_MODULE_NAME("exfield")
 
+/* Local prototypes */
+static u32
+acpi_ex_get_serial_access_length(u32 accessor_type, u32 access_length);
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_get_serial_access_bytes
+ *
+ * PARAMETERS:  accessor_type   - The type of the protocol indicated by region
+ *                                field access attributes
+ *              access_length   - The access length of the region field
+ *
+ * RETURN:      Decoded access length
+ *
+ * DESCRIPTION: This routine returns the length of the generic_serial_bus
+ *              protocol bytes
+ *
+ ******************************************************************************/
+
+static u32
+acpi_ex_get_serial_access_length(u32 accessor_type, u32 access_length)
+{
+	u32 length;
+
+	switch (accessor_type) {
+	case AML_FIELD_ATTRIB_QUICK:
+
+		length = 0;
+		break;
+
+	case AML_FIELD_ATTRIB_SEND_RCV:
+	case AML_FIELD_ATTRIB_BYTE:
+
+		length = 1;
+		break;
+
+	case AML_FIELD_ATTRIB_WORD:
+	case AML_FIELD_ATTRIB_WORD_CALL:
+
+		length = 2;
+		break;
+
+	case AML_FIELD_ATTRIB_MULTIBYTE:
+	case AML_FIELD_ATTRIB_RAW_BYTES:
+	case AML_FIELD_ATTRIB_RAW_PROCESS:
+
+		length = access_length;
+		break;
+
+	case AML_FIELD_ATTRIB_BLOCK:
+	case AML_FIELD_ATTRIB_BLOCK_CALL:
+	default:
+
+		length = ACPI_GSBUS_BUFFER_SIZE;
+		break;
+	}
+
+	return (length);
+}
+
 /*******************************************************************************
  *
  * FUNCTION:    acpi_ex_read_data_from_field
@@ -63,8 +124,9 @@ ACPI_MODULE_NAME("exfield")
  *              Buffer, depending on the size of the field.
  *
  ******************************************************************************/
+
 acpi_status
-acpi_ex_read_data_from_field(struct acpi_walk_state *walk_state,
+acpi_ex_read_data_from_field(struct acpi_walk_state * walk_state,
 			     union acpi_operand_object *obj_desc,
 			     union acpi_operand_object **ret_buffer_desc)
 {
@@ -73,6 +135,7 @@ acpi_ex_read_data_from_field(struct acpi_walk_state *walk_state,
 	acpi_size length;
 	void *buffer;
 	u32 function;
+	u16 accessor_type;
 
 	ACPI_FUNCTION_TRACE_PTR(ex_read_data_from_field, obj_desc);
 
@@ -116,9 +179,22 @@ acpi_ex_read_data_from_field(struct acpi_walk_state *walk_state,
 			    ACPI_READ | (obj_desc->field.attribute << 16);
 		} else if (obj_desc->field.region_obj->region.space_id ==
 			   ACPI_ADR_SPACE_GSBUS) {
-			length = ACPI_GSBUS_BUFFER_SIZE;
-			function =
-			    ACPI_READ | (obj_desc->field.attribute << 16);
+			accessor_type = obj_desc->field.attribute;
+			length = acpi_ex_get_serial_access_length(accessor_type,
+								  obj_desc->
+								  field.
+								  access_length);
+
+			/*
+			 * Add additional 2 bytes for modeled generic_serial_bus data buffer:
+			 * typedef struct {
+			 *     BYTEStatus; // Byte 0 of the data buffer
+			 *     BYTELength; // Byte 1 of the data buffer
+			 *     BYTE[x-1]Data; // Bytes 2-x of the arbitrary length data buffer,
+			 * }
+			 */
+			length += 2;
+			function = ACPI_READ | (accessor_type << 16);
 		} else {	/* IPMI */
 
 			length = ACPI_IPMI_BUFFER_SIZE;
@@ -231,6 +307,7 @@ acpi_ex_write_data_to_field(union acpi_operand_object *source_desc,
 	void *buffer;
 	union acpi_operand_object *buffer_desc;
 	u32 function;
+	u16 accessor_type;
 
 	ACPI_FUNCTION_TRACE_PTR(ex_write_data_to_field, obj_desc);
 
@@ -284,9 +361,22 @@ acpi_ex_write_data_to_field(union acpi_operand_object *source_desc,
 			    ACPI_WRITE | (obj_desc->field.attribute << 16);
 		} else if (obj_desc->field.region_obj->region.space_id ==
 			   ACPI_ADR_SPACE_GSBUS) {
-			length = ACPI_GSBUS_BUFFER_SIZE;
-			function =
-			    ACPI_WRITE | (obj_desc->field.attribute << 16);
+			accessor_type = obj_desc->field.attribute;
+			length = acpi_ex_get_serial_access_length(accessor_type,
+								  obj_desc->
+								  field.
+								  access_length);
+
+			/*
+			 * Add additional 2 bytes for modeled generic_serial_bus data buffer:
+			 * typedef struct {
+			 *     BYTEStatus; // Byte 0 of the data buffer
+			 *     BYTELength; // Byte 1 of the data buffer
+			 *     BYTE[x-1]Data; // Bytes 2-x of the arbitrary length data buffer,
+			 * }
+			 */
+			length += 2;
+			function = ACPI_WRITE | (accessor_type << 16);
 		} else {	/* IPMI */
 
 			length = ACPI_IPMI_BUFFER_SIZE;
diff --git a/drivers/acpi/acpica/tbutils.c b/drivers/acpi/acpica/tbutils.c
index a4702eee91a..9fb85f38de9 100644
--- a/drivers/acpi/acpica/tbutils.c
+++ b/drivers/acpi/acpica/tbutils.c
@@ -461,6 +461,7 @@ acpi_status __init acpi_tb_parse_root_table(acpi_physical_address rsdp_address)
 	u32 table_count;
 	struct acpi_table_header *table;
 	acpi_physical_address address;
+	acpi_physical_address rsdt_address;
 	u32 length;
 	u8 *table_entry;
 	acpi_status status;
@@ -488,11 +489,14 @@ acpi_status __init acpi_tb_parse_root_table(acpi_physical_address rsdp_address)
 		 * as per the ACPI specification.
 		 */
 		address = (acpi_physical_address) rsdp->xsdt_physical_address;
+		rsdt_address =
+		    (acpi_physical_address) rsdp->rsdt_physical_address;
 		table_entry_size = ACPI_XSDT_ENTRY_SIZE;
 	} else {
 		/* Root table is an RSDT (32-bit physical addresses) */
 
 		address = (acpi_physical_address) rsdp->rsdt_physical_address;
+		rsdt_address = address;
 		table_entry_size = ACPI_RSDT_ENTRY_SIZE;
 	}
 
@@ -515,8 +519,7 @@ acpi_status __init acpi_tb_parse_root_table(acpi_physical_address rsdp_address)
 
 			/* Fall back to the RSDT */
 
-			address =
-			    (acpi_physical_address) rsdp->rsdt_physical_address;
+			address = rsdt_address;
 			table_entry_size = ACPI_RSDT_ENTRY_SIZE;
 		}
 	}
diff --git a/drivers/acpi/battery.c b/drivers/acpi/battery.c
index 9a2c63b2005..6e7b2a12860 100644
--- a/drivers/acpi/battery.c
+++ b/drivers/acpi/battery.c
@@ -36,6 +36,12 @@
 #include <linux/suspend.h>
 #include <asm/unaligned.h>
 
+#ifdef CONFIG_ACPI_PROCFS_POWER
+#include <linux/proc_fs.h>
+#include <linux/seq_file.h>
+#include <asm/uaccess.h>
+#endif
+
 #include <linux/acpi.h>
 #include <linux/power_supply.h>
 
@@ -64,6 +70,19 @@ static unsigned int cache_time = 1000;
 module_param(cache_time, uint, 0644);
 MODULE_PARM_DESC(cache_time, "cache time in milliseconds");
 
+#ifdef CONFIG_ACPI_PROCFS_POWER
+extern struct proc_dir_entry *acpi_lock_battery_dir(void);
+extern void *acpi_unlock_battery_dir(struct proc_dir_entry *acpi_battery_dir);
+
+enum acpi_battery_files {
+	info_tag = 0,
+	state_tag,
+	alarm_tag,
+	ACPI_BATTERY_NUMFILES,
+};
+
+#endif
+
 static const struct acpi_device_id battery_device_ids[] = {
 	{"PNP0C0A", 0},
 	{"", 0},
@@ -299,6 +318,14 @@ static enum power_supply_property energy_battery_props[] = {
 	POWER_SUPPLY_PROP_SERIAL_NUMBER,
 };
 
+#ifdef CONFIG_ACPI_PROCFS_POWER
+inline char *acpi_battery_units(struct acpi_battery *battery)
+{
+	return (battery->power_unit == ACPI_BATTERY_POWER_UNIT_MA) ?
+		"mA" : "mW";
+}
+#endif
+
 /* --------------------------------------------------------------------------
                                Battery Management
    -------------------------------------------------------------------------- */
@@ -717,6 +744,279 @@ static void acpi_battery_refresh(struct acpi_battery *battery)
 }
 
 /* --------------------------------------------------------------------------
+                              FS Interface (/proc)
+   -------------------------------------------------------------------------- */
+
+#ifdef CONFIG_ACPI_PROCFS_POWER
+static struct proc_dir_entry *acpi_battery_dir;
+
+static int acpi_battery_print_info(struct seq_file *seq, int result)
+{
+	struct acpi_battery *battery = seq->private;
+
+	if (result)
+		goto end;
+
+	seq_printf(seq, "present:                 %s\n",
+		   acpi_battery_present(battery) ? "yes" : "no");
+	if (!acpi_battery_present(battery))
+		goto end;
+	if (battery->design_capacity == ACPI_BATTERY_VALUE_UNKNOWN)
+		seq_printf(seq, "design capacity:         unknown\n");
+	else
+		seq_printf(seq, "design capacity:         %d %sh\n",
+			   battery->design_capacity,
+			   acpi_battery_units(battery));
+
+	if (battery->full_charge_capacity == ACPI_BATTERY_VALUE_UNKNOWN)
+		seq_printf(seq, "last full capacity:      unknown\n");
+	else
+		seq_printf(seq, "last full capacity:      %d %sh\n",
+			   battery->full_charge_capacity,
+			   acpi_battery_units(battery));
+
+	seq_printf(seq, "battery technology:      %srechargeable\n",
+		   (!battery->technology)?"non-":"");
+
+	if (battery->design_voltage == ACPI_BATTERY_VALUE_UNKNOWN)
+		seq_printf(seq, "design voltage:          unknown\n");
+	else
+		seq_printf(seq, "design voltage:          %d mV\n",
+			   battery->design_voltage);
+	seq_printf(seq, "design capacity warning: %d %sh\n",
+		   battery->design_capacity_warning,
+		   acpi_battery_units(battery));
+	seq_printf(seq, "design capacity low:     %d %sh\n",
+		   battery->design_capacity_low,
+		   acpi_battery_units(battery));
+	seq_printf(seq, "cycle count:		  %i\n", battery->cycle_count);
+	seq_printf(seq, "capacity granularity 1:  %d %sh\n",
+		   battery->capacity_granularity_1,
+		   acpi_battery_units(battery));
+	seq_printf(seq, "capacity granularity 2:  %d %sh\n",
+		   battery->capacity_granularity_2,
+		   acpi_battery_units(battery));
+	seq_printf(seq, "model number:            %s\n", battery->model_number);
+	seq_printf(seq, "serial number:           %s\n", battery->serial_number);
+	seq_printf(seq, "battery type:            %s\n", battery->type);
+	seq_printf(seq, "OEM info:                %s\n", battery->oem_info);
+      end:
+	if (result)
+		seq_printf(seq, "ERROR: Unable to read battery info\n");
+	return result;
+}
+
+static int acpi_battery_print_state(struct seq_file *seq, int result)
+{
+	struct acpi_battery *battery = seq->private;
+
+	if (result)
+		goto end;
+
+	seq_printf(seq, "present:                 %s\n",
+		   acpi_battery_present(battery) ? "yes" : "no");
+	if (!acpi_battery_present(battery))
+		goto end;
+
+	seq_printf(seq, "capacity state:          %s\n",
+			(battery->state & 0x04) ? "critical" : "ok");
+	if ((battery->state & 0x01) && (battery->state & 0x02))
+		seq_printf(seq,
+			   "charging state:          charging/discharging\n");
+	else if (battery->state & 0x01)
+		seq_printf(seq, "charging state:          discharging\n");
+	else if (battery->state & 0x02)
+		seq_printf(seq, "charging state:          charging\n");
+	else
+		seq_printf(seq, "charging state:          charged\n");
+
+	if (battery->rate_now == ACPI_BATTERY_VALUE_UNKNOWN)
+		seq_printf(seq, "present rate:            unknown\n");
+	else
+		seq_printf(seq, "present rate:            %d %s\n",
+			   battery->rate_now, acpi_battery_units(battery));
+
+	if (battery->capacity_now == ACPI_BATTERY_VALUE_UNKNOWN)
+		seq_printf(seq, "remaining capacity:      unknown\n");
+	else
+		seq_printf(seq, "remaining capacity:      %d %sh\n",
+			   battery->capacity_now, acpi_battery_units(battery));
+	if (battery->voltage_now == ACPI_BATTERY_VALUE_UNKNOWN)
+		seq_printf(seq, "present voltage:         unknown\n");
+	else
+		seq_printf(seq, "present voltage:         %d mV\n",
+			   battery->voltage_now);
+      end:
+	if (result)
+		seq_printf(seq, "ERROR: Unable to read battery state\n");
+
+	return result;
+}
+
+static int acpi_battery_print_alarm(struct seq_file *seq, int result)
+{
+	struct acpi_battery *battery = seq->private;
+
+	if (result)
+		goto end;
+
+	if (!acpi_battery_present(battery)) {
+		seq_printf(seq, "present:                 no\n");
+		goto end;
+	}
+	seq_printf(seq, "alarm:                   ");
+	if (!battery->alarm)
+		seq_printf(seq, "unsupported\n");
+	else
+		seq_printf(seq, "%u %sh\n", battery->alarm,
+				acpi_battery_units(battery));
+      end:
+	if (result)
+		seq_printf(seq, "ERROR: Unable to read battery alarm\n");
+	return result;
+}
+
+static ssize_t acpi_battery_write_alarm(struct file *file,
+					const char __user * buffer,
+					size_t count, loff_t * ppos)
+{
+	int result = 0;
+	char alarm_string[12] = { '\0' };
+	struct seq_file *m = file->private_data;
+	struct acpi_battery *battery = m->private;
+
+	if (!battery || (count > sizeof(alarm_string) - 1))
+		return -EINVAL;
+	if (!acpi_battery_present(battery)) {
+		result = -ENODEV;
+		goto end;
+	}
+	if (copy_from_user(alarm_string, buffer, count)) {
+		result = -EFAULT;
+		goto end;
+	}
+	alarm_string[count] = '\0';
+	battery->alarm = simple_strtol(alarm_string, NULL, 0);
+	result = acpi_battery_set_alarm(battery);
+      end:
+	if (!result)
+		return count;
+	return result;
+}
+
+typedef int(*print_func)(struct seq_file *seq, int result);
+
+static print_func acpi_print_funcs[ACPI_BATTERY_NUMFILES] = {
+	acpi_battery_print_info,
+	acpi_battery_print_state,
+	acpi_battery_print_alarm,
+};
+
+static int acpi_battery_read(int fid, struct seq_file *seq)
+{
+	struct acpi_battery *battery = seq->private;
+	int result = acpi_battery_update(battery);
+	return acpi_print_funcs[fid](seq, result);
+}
+
+#define DECLARE_FILE_FUNCTIONS(_name) \
+static int acpi_battery_read_##_name(struct seq_file *seq, void *offset) \
+{ \
+	return acpi_battery_read(_name##_tag, seq); \
+} \
+static int acpi_battery_##_name##_open_fs(struct inode *inode, struct file *file) \
+{ \
+	return single_open(file, acpi_battery_read_##_name, PDE_DATA(inode)); \
+}
+
+DECLARE_FILE_FUNCTIONS(info);
+DECLARE_FILE_FUNCTIONS(state);
+DECLARE_FILE_FUNCTIONS(alarm);
+
+#undef DECLARE_FILE_FUNCTIONS
+
+#define FILE_DESCRIPTION_RO(_name) \
+	{ \
+	.name = __stringify(_name), \
+	.mode = S_IRUGO, \
+	.ops = { \
+		.open = acpi_battery_##_name##_open_fs, \
+		.read = seq_read, \
+		.llseek = seq_lseek, \
+		.release = single_release, \
+		.owner = THIS_MODULE, \
+		}, \
+	}
+
+#define FILE_DESCRIPTION_RW(_name) \
+	{ \
+	.name = __stringify(_name), \
+	.mode = S_IFREG | S_IRUGO | S_IWUSR, \
+	.ops = { \
+		.open = acpi_battery_##_name##_open_fs, \
+		.read = seq_read, \
+		.llseek = seq_lseek, \
+		.write = acpi_battery_write_##_name, \
+		.release = single_release, \
+		.owner = THIS_MODULE, \
+		}, \
+	}
+
+static const struct battery_file {
+	struct file_operations ops;
+	umode_t mode;
+	const char *name;
+} acpi_battery_file[] = {
+	FILE_DESCRIPTION_RO(info),
+	FILE_DESCRIPTION_RO(state),
+	FILE_DESCRIPTION_RW(alarm),
+};
+
+#undef FILE_DESCRIPTION_RO
+#undef FILE_DESCRIPTION_RW
+
+static int acpi_battery_add_fs(struct acpi_device *device)
+{
+	struct proc_dir_entry *entry = NULL;
+	int i;
+
+	printk(KERN_WARNING PREFIX "Deprecated procfs I/F for battery is loaded,"
+			" please retry with CONFIG_ACPI_PROCFS_POWER cleared\n");
+	if (!acpi_device_dir(device)) {
+		acpi_device_dir(device) = proc_mkdir(acpi_device_bid(device),
+						     acpi_battery_dir);
+		if (!acpi_device_dir(device))
+			return -ENODEV;
+	}
+
+	for (i = 0; i < ACPI_BATTERY_NUMFILES; ++i) {
+		entry = proc_create_data(acpi_battery_file[i].name,
+					 acpi_battery_file[i].mode,
+					 acpi_device_dir(device),
+					 &acpi_battery_file[i].ops,
+					 acpi_driver_data(device));
+		if (!entry)
+			return -ENODEV;
+	}
+	return 0;
+}
+
+static void acpi_battery_remove_fs(struct acpi_device *device)
+{
+	int i;
+	if (!acpi_device_dir(device))
+		return;
+	for (i = 0; i < ACPI_BATTERY_NUMFILES; ++i)
+		remove_proc_entry(acpi_battery_file[i].name,
+				  acpi_device_dir(device));
+
+	remove_proc_entry(acpi_device_bid(device), acpi_battery_dir);
+	acpi_device_dir(device) = NULL;
+}
+
+#endif
+
+/* --------------------------------------------------------------------------
                                  Driver Interface
    -------------------------------------------------------------------------- */
 
@@ -790,6 +1090,15 @@ static int acpi_battery_add(struct acpi_device *device)
 	result = acpi_battery_update(battery);
 	if (result)
 		goto fail;
+#ifdef CONFIG_ACPI_PROCFS_POWER
+	result = acpi_battery_add_fs(device);
+#endif
+	if (result) {
+#ifdef CONFIG_ACPI_PROCFS_POWER
+		acpi_battery_remove_fs(device);
+#endif
+		goto fail;
+	}
 
 	printk(KERN_INFO PREFIX "%s Slot [%s] (battery %s)\n",
 		ACPI_BATTERY_DEVICE_NAME, acpi_device_bid(device),
@@ -816,6 +1125,9 @@ static int acpi_battery_remove(struct acpi_device *device)
 		return -EINVAL;
 	battery = acpi_driver_data(device);
 	unregister_pm_notifier(&battery->pm_nb);
+#ifdef CONFIG_ACPI_PROCFS_POWER
+	acpi_battery_remove_fs(device);
+#endif
 	sysfs_remove_battery(battery);
 	mutex_destroy(&battery->lock);
 	mutex_destroy(&battery->sysfs_lock);
@@ -866,7 +1178,19 @@ static void __init acpi_battery_init_async(void *unused, async_cookie_t cookie)
 
 	if (dmi_check_system(bat_dmi_table))
 		battery_bix_broken_package = 1;
-	acpi_bus_register_driver(&acpi_battery_driver);
+	
+#ifdef CONFIG_ACPI_PROCFS_POWER
+	acpi_battery_dir = acpi_lock_battery_dir();
+	if (!acpi_battery_dir)
+		return;
+#endif
+	if (acpi_bus_register_driver(&acpi_battery_driver) < 0) {
+#ifdef CONFIG_ACPI_PROCFS_POWER
+		acpi_unlock_battery_dir(acpi_battery_dir);
+#endif
+		return;
+	}
+	return;
 }
 
 static int __init acpi_battery_init(void)
@@ -878,6 +1202,9 @@ static int __init acpi_battery_init(void)
 static void __exit acpi_battery_exit(void)
 {
 	acpi_bus_unregister_driver(&acpi_battery_driver);
+#ifdef CONFIG_ACPI_PROCFS_POWER
+	acpi_unlock_battery_dir(acpi_battery_dir);
+#endif
 }
 
 module_init(acpi_battery_init);
diff --git a/drivers/acpi/blacklist.c b/drivers/acpi/blacklist.c
index afec4526c48..3d8413d02a9 100644
--- a/drivers/acpi/blacklist.c
+++ b/drivers/acpi/blacklist.c
@@ -314,6 +314,14 @@ static struct dmi_system_id acpi_osi_dmi_table[] __initdata = {
 		     DMI_MATCH(DMI_PRODUCT_VERSION, "2349D15"),
 		},
 	},
+	{
+	.callback = dmi_disable_osi_win8,
+	.ident = "Dell Inspiron 7737",
+	.matches = {
+		    DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
+		    DMI_MATCH(DMI_PRODUCT_NAME, "Inspiron 7737"),
+		},
+	},
 
 	/*
 	 * BIOS invocation of _OSI(Linux) is almost always a BIOS bug.
@@ -374,6 +382,19 @@ static struct dmi_system_id acpi_osi_dmi_table[] __initdata = {
 		     DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad T500"),
 		},
 	},
+	/*
+	 * Without this this EEEpc exports a non working WMI interface, with
+	 * this it exports a working "good old" eeepc_laptop interface, fixing
+	 * both brightness control, and rfkill not working.
+	 */
+	{
+	.callback = dmi_enable_osi_linux,
+	.ident = "Asus EEE PC 1015PX",
+	.matches = {
+		     DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK Computer INC."),
+		     DMI_MATCH(DMI_PRODUCT_NAME, "1015PX"),
+		},
+	},
 	{}
 };
 
diff --git a/drivers/acpi/bus.c b/drivers/acpi/bus.c
index e7e5844c87d..cf925c4f36b 100644
--- a/drivers/acpi/bus.c
+++ b/drivers/acpi/bus.c
@@ -380,9 +380,8 @@ static void acpi_bus_notify(acpi_handle handle, u32 type, void *data)
 		break;
 
 	default:
-		acpi_handle_warn(handle, "Unsupported event type 0x%x\n", type);
-		ost_code = ACPI_OST_SC_UNRECOGNIZED_NOTIFY;
-		goto err;
+		acpi_handle_debug(handle, "Unknown event type 0x%x\n", type);
+		break;
 	}
 
 	adev = acpi_bus_get_acpi_device(handle);
diff --git a/drivers/acpi/cm_sbs.c b/drivers/acpi/cm_sbs.c
new file mode 100644
index 00000000000..6c9ee68e46f
--- /dev/null
+++ b/drivers/acpi/cm_sbs.c
@@ -0,0 +1,105 @@
+/*
+ * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+ *
+ *  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/init.h>
+#include <linux/acpi.h>
+#include <linux/types.h>
+#include <linux/proc_fs.h>
+#include <linux/seq_file.h>
+#include <acpi/acpi_bus.h>
+#include <acpi/acpi_drivers.h>
+
+#define PREFIX "ACPI: "
+
+ACPI_MODULE_NAME("cm_sbs");
+#define ACPI_AC_CLASS		"ac_adapter"
+#define ACPI_BATTERY_CLASS	"battery"
+#define _COMPONENT		ACPI_SBS_COMPONENT
+static struct proc_dir_entry *acpi_ac_dir;
+static struct proc_dir_entry *acpi_battery_dir;
+
+static DEFINE_MUTEX(cm_sbs_mutex);
+
+static int lock_ac_dir_cnt;
+static int lock_battery_dir_cnt;
+
+struct proc_dir_entry *acpi_lock_ac_dir(void)
+{
+	mutex_lock(&cm_sbs_mutex);
+	if (!acpi_ac_dir)
+		acpi_ac_dir = proc_mkdir(ACPI_AC_CLASS, acpi_root_dir);
+	if (acpi_ac_dir) {
+		lock_ac_dir_cnt++;
+	} else {
+		printk(KERN_ERR PREFIX
+				  "Cannot create %s\n", ACPI_AC_CLASS);
+	}
+	mutex_unlock(&cm_sbs_mutex);
+	return acpi_ac_dir;
+}
+EXPORT_SYMBOL(acpi_lock_ac_dir);
+
+void acpi_unlock_ac_dir(struct proc_dir_entry *acpi_ac_dir_param)
+{
+	mutex_lock(&cm_sbs_mutex);
+	if (acpi_ac_dir_param)
+		lock_ac_dir_cnt--;
+	if (lock_ac_dir_cnt == 0 && acpi_ac_dir_param && acpi_ac_dir) {
+		remove_proc_entry(ACPI_AC_CLASS, acpi_root_dir);
+		acpi_ac_dir = NULL;
+	}
+	mutex_unlock(&cm_sbs_mutex);
+}
+EXPORT_SYMBOL(acpi_unlock_ac_dir);
+
+struct proc_dir_entry *acpi_lock_battery_dir(void)
+{
+	mutex_lock(&cm_sbs_mutex);
+	if (!acpi_battery_dir) {
+		acpi_battery_dir =
+		    proc_mkdir(ACPI_BATTERY_CLASS, acpi_root_dir);
+	}
+	if (acpi_battery_dir) {
+		lock_battery_dir_cnt++;
+	} else {
+		printk(KERN_ERR PREFIX
+				  "Cannot create %s\n", ACPI_BATTERY_CLASS);
+	}
+	mutex_unlock(&cm_sbs_mutex);
+	return acpi_battery_dir;
+}
+EXPORT_SYMBOL(acpi_lock_battery_dir);
+
+void acpi_unlock_battery_dir(struct proc_dir_entry *acpi_battery_dir_param)
+{
+	mutex_lock(&cm_sbs_mutex);
+	if (acpi_battery_dir_param)
+		lock_battery_dir_cnt--;
+	if (lock_battery_dir_cnt == 0 && acpi_battery_dir_param
+	    && acpi_battery_dir) {
+		remove_proc_entry(ACPI_BATTERY_CLASS, acpi_root_dir);
+		acpi_battery_dir = NULL;
+	}
+	mutex_unlock(&cm_sbs_mutex);
+	return;
+}
+EXPORT_SYMBOL(acpi_unlock_battery_dir);
diff --git a/drivers/acpi/ec.c b/drivers/acpi/ec.c
index d7d32c28829..ad11ba4a412 100644
--- a/drivers/acpi/ec.c
+++ b/drivers/acpi/ec.c
@@ -206,13 +206,13 @@ unlock:
 	spin_unlock_irqrestore(&ec->lock, flags);
 }
 
-static int acpi_ec_sync_query(struct acpi_ec *ec);
+static int acpi_ec_sync_query(struct acpi_ec *ec, u8 *data);
 
 static int ec_check_sci_sync(struct acpi_ec *ec, u8 state)
 {
 	if (state & ACPI_EC_FLAG_SCI) {
 		if (!test_and_set_bit(EC_FLAGS_QUERY_PENDING, &ec->flags))
-			return acpi_ec_sync_query(ec);
+			return acpi_ec_sync_query(ec, NULL);
 	}
 	return 0;
 }
@@ -443,10 +443,8 @@ acpi_handle ec_get_handle(void)
 
 EXPORT_SYMBOL(ec_get_handle);
 
-static int acpi_ec_query_unlocked(struct acpi_ec *ec, u8 *data);
-
 /*
- * Clears stale _Q events that might have accumulated in the EC.
+ * Process _Q events that might have accumulated in the EC.
  * Run with locked ec mutex.
  */
 static void acpi_ec_clear(struct acpi_ec *ec)
@@ -455,7 +453,7 @@ static void acpi_ec_clear(struct acpi_ec *ec)
 	u8 value = 0;
 
 	for (i = 0; i < ACPI_EC_CLEAR_MAX; i++) {
-		status = acpi_ec_query_unlocked(ec, &value);
+		status = acpi_ec_sync_query(ec, &value);
 		if (status || !value)
 			break;
 	}
@@ -582,13 +580,18 @@ static void acpi_ec_run(void *cxt)
 	kfree(handler);
 }
 
-static int acpi_ec_sync_query(struct acpi_ec *ec)
+static int acpi_ec_sync_query(struct acpi_ec *ec, u8 *data)
 {
 	u8 value = 0;
 	int status;
 	struct acpi_ec_query_handler *handler, *copy;
-	if ((status = acpi_ec_query_unlocked(ec, &value)))
+
+	status = acpi_ec_query_unlocked(ec, &value);
+	if (data)
+		*data = value;
+	if (status)
 		return status;
+
 	list_for_each_entry(handler, &ec->list, node) {
 		if (value == handler->query_bit) {
 			/* have custom handler for this bit */
@@ -612,7 +615,7 @@ static void acpi_ec_gpe_query(void *ec_cxt)
 	if (!ec)
 		return;
 	mutex_lock(&ec->mutex);
-	acpi_ec_sync_query(ec);
+	acpi_ec_sync_query(ec, NULL);
 	mutex_unlock(&ec->mutex);
 }
 
diff --git a/drivers/acpi/thermal.c b/drivers/acpi/thermal.c
index c1e31a41f94..25bbc55dca8 100644
--- a/drivers/acpi/thermal.c
+++ b/drivers/acpi/thermal.c
@@ -1278,8 +1278,8 @@ static int __init acpi_thermal_init(void)
 
 static void __exit acpi_thermal_exit(void)
 {
-	destroy_workqueue(acpi_thermal_pm_queue);
 	acpi_bus_unregister_driver(&acpi_thermal_driver);
+	destroy_workqueue(acpi_thermal_pm_queue);
 
 	return;
 }
diff --git a/drivers/acpi/video.c b/drivers/acpi/video.c
index 8b6990e417e..f8bc5a755dd 100644
--- a/drivers/acpi/video.c
+++ b/drivers/acpi/video.c
@@ -457,10 +457,10 @@ static struct dmi_system_id video_dmi_table[] __initdata = {
 	},
 	{
 	 .callback = video_set_use_native_backlight,
-	 .ident = "ThinkPad T430s",
+	 .ident = "ThinkPad T430 and T430s",
 	 .matches = {
 		DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-		DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad T430s"),
+		DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad T430"),
 		},
 	},
 	{
@@ -472,7 +472,7 @@ static struct dmi_system_id video_dmi_table[] __initdata = {
 		},
 	},
 	{
-	.callback = video_set_use_native_backlight,
+	 .callback = video_set_use_native_backlight,
 	.ident = "ThinkPad X1 Carbon",
 	.matches = {
 		DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
@@ -500,7 +500,7 @@ static struct dmi_system_id video_dmi_table[] __initdata = {
 	 .ident = "Dell Inspiron 7520",
 	 .matches = {
 		DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
-		DMI_MATCH(DMI_PRODUCT_VERSION, "Inspiron 7520"),
+		DMI_MATCH(DMI_PRODUCT_NAME, "Inspiron 7520"),
 		},
 	},
 	{
@@ -513,6 +513,14 @@ static struct dmi_system_id video_dmi_table[] __initdata = {
 	},
 	{
 	 .callback = video_set_use_native_backlight,
+	 .ident = "Acer Aspire 5742G",
+	 .matches = {
+		DMI_MATCH(DMI_SYS_VENDOR, "Acer"),
+		DMI_MATCH(DMI_PRODUCT_NAME, "Aspire 5742G"),
+		},
+	},
+	{
+	 .callback = video_set_use_native_backlight,
 	 .ident = "Acer Aspire V5-431",
 	 .matches = {
 		DMI_MATCH(DMI_SYS_VENDOR, "Acer"),
diff --git a/drivers/ata/Kconfig b/drivers/ata/Kconfig
index 20e03a7eb8b..0033fafc470 100644
--- a/drivers/ata/Kconfig
+++ b/drivers/ata/Kconfig
@@ -116,7 +116,7 @@ config AHCI_ST
 
 config AHCI_IMX
 	tristate "Freescale i.MX AHCI SATA support"
-	depends on MFD_SYSCON
+	depends on MFD_SYSCON && (ARCH_MXC || COMPILE_TEST)
 	help
 	  This option enables support for the Freescale i.MX SoC's
 	  onboard AHCI SATA.
@@ -134,8 +134,7 @@ config AHCI_SUNXI
 
 config AHCI_XGENE
 	tristate "APM X-Gene 6.0Gbps AHCI SATA host controller support"
-	depends on ARM64 || COMPILE_TEST
-	select PHY_XGENE
+	depends on PHY_XGENE
 	help
 	 This option enables support for APM X-Gene SoC SATA host controller.
 
@@ -816,7 +815,7 @@ config PATA_AT32
 
 config PATA_AT91
 	tristate "PATA support for AT91SAM9260"
-	depends on ARM && ARCH_AT91
+	depends on ARM && SOC_AT91SAM9
 	help
 	  This option enables support for IDE devices on the Atmel AT91SAM9260 SoC.
 
diff --git a/drivers/ata/ahci.c b/drivers/ata/ahci.c
index 5a0bf8ed649..60707814a84 100644
--- a/drivers/ata/ahci.c
+++ b/drivers/ata/ahci.c
@@ -1115,6 +1115,17 @@ static bool ahci_broken_online(struct pci_dev *pdev)
 	return pdev->bus->number == (val >> 8) && pdev->devfn == (val & 0xff);
 }
 
+static bool ahci_broken_devslp(struct pci_dev *pdev)
+{
+	/* device with broken DEVSLP but still showing SDS capability */
+	static const struct pci_device_id ids[] = {
+		{ PCI_VDEVICE(INTEL, 0x0f23)}, /* Valleyview SoC */
+		{}
+	};
+
+	return pci_match_id(ids, pdev);
+}
+
 #ifdef CONFIG_ATA_ACPI
 static void ahci_gtf_filter_workaround(struct ata_host *host)
 {
@@ -1164,9 +1175,9 @@ static inline void ahci_gtf_filter_workaround(struct ata_host *host)
 #endif
 
 static int ahci_init_interrupts(struct pci_dev *pdev, unsigned int n_ports,
-			 struct ahci_host_priv *hpriv)
+				struct ahci_host_priv *hpriv)
 {
-	int nvec;
+	int rc, nvec;
 
 	if (hpriv->flags & AHCI_HFLAG_NO_MSI)
 		goto intx;
@@ -1183,12 +1194,19 @@ static int ahci_init_interrupts(struct pci_dev *pdev, unsigned int n_ports,
 	if (nvec < n_ports)
 		goto single_msi;
 
-	nvec = pci_enable_msi_range(pdev, nvec, nvec);
-	if (nvec == -ENOSPC)
+	rc = pci_enable_msi_exact(pdev, nvec);
+	if (rc == -ENOSPC)
 		goto single_msi;
-	else if (nvec < 0)
+	else if (rc < 0)
 		goto intx;
 
+	/* fallback to single MSI mode if the controller enforced MRSM mode */
+	if (readl(hpriv->mmio + HOST_CTL) & HOST_MRSM) {
+		pci_disable_msi(pdev);
+		printk(KERN_INFO "ahci: MRSM is on, fallback to single MSI\n");
+		goto single_msi;
+	}
+
 	return nvec;
 
 single_msi:
@@ -1232,18 +1250,18 @@ int ahci_host_activate(struct ata_host *host, int irq, unsigned int n_msis)
 		return rc;
 
 	for (i = 0; i < host->n_ports; i++) {
-		const char* desc;
 		struct ahci_port_priv *pp = host->ports[i]->private_data;
 
-		/* pp is NULL for dummy ports */
-		if (pp)
-			desc = pp->irq_desc;
-		else
-			desc = dev_driver_string(host->dev);
+		/* Do not receive interrupts sent by dummy ports */
+		if (!pp) {
+			disable_irq(irq + i);
+			continue;
+		}
 
-		rc = devm_request_threaded_irq(host->dev,
-			irq + i, ahci_hw_interrupt, ahci_thread_fn, IRQF_SHARED,
-			desc, host->ports[i]);
+		rc = devm_request_threaded_irq(host->dev, irq + i,
+					       ahci_hw_interrupt,
+					       ahci_thread_fn, IRQF_SHARED,
+					       pp->irq_desc, host->ports[i]);
 		if (rc)
 			goto out_free_irqs;
 	}
@@ -1357,6 +1375,10 @@ static int ahci_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	hpriv->mmio = pcim_iomap_table(pdev)[ahci_pci_bar];
 
+	/* must set flag prior to save config in order to take effect */
+	if (ahci_broken_devslp(pdev))
+		hpriv->flags |= AHCI_HFLAG_NO_DEVSLP;
+
 	/* save initial config */
 	ahci_pci_save_initial_config(pdev, hpriv);
 
diff --git a/drivers/ata/ahci.h b/drivers/ata/ahci.h
index 51af275b338..af63c75c200 100644
--- a/drivers/ata/ahci.h
+++ b/drivers/ata/ahci.h
@@ -94,6 +94,7 @@ enum {
 	/* HOST_CTL bits */
 	HOST_RESET		= (1 << 0),  /* reset controller; self-clear */
 	HOST_IRQ_EN		= (1 << 1),  /* global IRQ enable */
+	HOST_MRSM		= (1 << 2),  /* MSI Revert to Single Message */
 	HOST_AHCI_EN		= (1 << 31), /* AHCI enabled */
 
 	/* HOST_CAP bits */
@@ -235,6 +236,7 @@ enum {
 						        port start (wait until
 						        error-handling stage) */
 	AHCI_HFLAG_MULTI_MSI		= (1 << 16), /* multiple PCI MSIs */
+	AHCI_HFLAG_NO_DEVSLP		= (1 << 17), /* no device sleep */
 
 	/* ap->flags bits */
 
diff --git a/drivers/ata/ahci_imx.c b/drivers/ata/ahci_imx.c
index 497c7abe1c7..8befeb69eeb 100644
--- a/drivers/ata/ahci_imx.c
+++ b/drivers/ata/ahci_imx.c
@@ -29,9 +29,25 @@
 #include "ahci.h"
 
 enum {
-	PORT_PHY_CTL = 0x178,			/* Port0 PHY Control */
-	PORT_PHY_CTL_PDDQ_LOC = 0x100000,	/* PORT_PHY_CTL bits */
-	HOST_TIMER1MS = 0xe0,			/* Timer 1-ms */
+	/* Timer 1-ms Register */
+	IMX_TIMER1MS				= 0x00e0,
+	/* Port0 PHY Control Register */
+	IMX_P0PHYCR				= 0x0178,
+	IMX_P0PHYCR_TEST_PDDQ			= 1 << 20,
+	IMX_P0PHYCR_CR_READ			= 1 << 19,
+	IMX_P0PHYCR_CR_WRITE			= 1 << 18,
+	IMX_P0PHYCR_CR_CAP_DATA			= 1 << 17,
+	IMX_P0PHYCR_CR_CAP_ADDR			= 1 << 16,
+	/* Port0 PHY Status Register */
+	IMX_P0PHYSR				= 0x017c,
+	IMX_P0PHYSR_CR_ACK			= 1 << 18,
+	IMX_P0PHYSR_CR_DATA_OUT			= 0xffff << 0,
+	/* Lane0 Output Status Register */
+	IMX_LANE0_OUT_STAT			= 0x2003,
+	IMX_LANE0_OUT_STAT_RX_PLL_STATE		= 1 << 1,
+	/* Clock Reset Register */
+	IMX_CLOCK_RESET				= 0x7f3f,
+	IMX_CLOCK_RESET_RESET			= 1 << 0,
 };
 
 enum ahci_imx_type {
@@ -54,9 +70,149 @@ MODULE_PARM_DESC(hotplug, "AHCI IMX hot-plug support (0=Don't support, 1=support
 
 static void ahci_imx_host_stop(struct ata_host *host);
 
+static int imx_phy_crbit_assert(void __iomem *mmio, u32 bit, bool assert)
+{
+	int timeout = 10;
+	u32 crval;
+	u32 srval;
+
+	/* Assert or deassert the bit */
+	crval = readl(mmio + IMX_P0PHYCR);
+	if (assert)
+		crval |= bit;
+	else
+		crval &= ~bit;
+	writel(crval, mmio + IMX_P0PHYCR);
+
+	/* Wait for the cr_ack signal */
+	do {
+		srval = readl(mmio + IMX_P0PHYSR);
+		if ((assert ? srval : ~srval) & IMX_P0PHYSR_CR_ACK)
+			break;
+		usleep_range(100, 200);
+	} while (--timeout);
+
+	return timeout ? 0 : -ETIMEDOUT;
+}
+
+static int imx_phy_reg_addressing(u16 addr, void __iomem *mmio)
+{
+	u32 crval = addr;
+	int ret;
+
+	/* Supply the address on cr_data_in */
+	writel(crval, mmio + IMX_P0PHYCR);
+
+	/* Assert the cr_cap_addr signal */
+	ret = imx_phy_crbit_assert(mmio, IMX_P0PHYCR_CR_CAP_ADDR, true);
+	if (ret)
+		return ret;
+
+	/* Deassert cr_cap_addr */
+	ret = imx_phy_crbit_assert(mmio, IMX_P0PHYCR_CR_CAP_ADDR, false);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int imx_phy_reg_write(u16 val, void __iomem *mmio)
+{
+	u32 crval = val;
+	int ret;
+
+	/* Supply the data on cr_data_in */
+	writel(crval, mmio + IMX_P0PHYCR);
+
+	/* Assert the cr_cap_data signal */
+	ret = imx_phy_crbit_assert(mmio, IMX_P0PHYCR_CR_CAP_DATA, true);
+	if (ret)
+		return ret;
+
+	/* Deassert cr_cap_data */
+	ret = imx_phy_crbit_assert(mmio, IMX_P0PHYCR_CR_CAP_DATA, false);
+	if (ret)
+		return ret;
+
+	if (val & IMX_CLOCK_RESET_RESET) {
+		/*
+		 * In case we're resetting the phy, it's unable to acknowledge,
+		 * so we return immediately here.
+		 */
+		crval |= IMX_P0PHYCR_CR_WRITE;
+		writel(crval, mmio + IMX_P0PHYCR);
+		goto out;
+	}
+
+	/* Assert the cr_write signal */
+	ret = imx_phy_crbit_assert(mmio, IMX_P0PHYCR_CR_WRITE, true);
+	if (ret)
+		return ret;
+
+	/* Deassert cr_write */
+	ret = imx_phy_crbit_assert(mmio, IMX_P0PHYCR_CR_WRITE, false);
+	if (ret)
+		return ret;
+
+out:
+	return 0;
+}
+
+static int imx_phy_reg_read(u16 *val, void __iomem *mmio)
+{
+	int ret;
+
+	/* Assert the cr_read signal */
+	ret = imx_phy_crbit_assert(mmio, IMX_P0PHYCR_CR_READ, true);
+	if (ret)
+		return ret;
+
+	/* Capture the data from cr_data_out[] */
+	*val = readl(mmio + IMX_P0PHYSR) & IMX_P0PHYSR_CR_DATA_OUT;
+
+	/* Deassert cr_read */
+	ret = imx_phy_crbit_assert(mmio, IMX_P0PHYCR_CR_READ, false);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int imx_sata_phy_reset(struct ahci_host_priv *hpriv)
+{
+	void __iomem *mmio = hpriv->mmio;
+	int timeout = 10;
+	u16 val;
+	int ret;
+
+	/* Reset SATA PHY by setting RESET bit of PHY register CLOCK_RESET */
+	ret = imx_phy_reg_addressing(IMX_CLOCK_RESET, mmio);
+	if (ret)
+		return ret;
+	ret = imx_phy_reg_write(IMX_CLOCK_RESET_RESET, mmio);
+	if (ret)
+		return ret;
+
+	/* Wait for PHY RX_PLL to be stable */
+	do {
+		usleep_range(100, 200);
+		ret = imx_phy_reg_addressing(IMX_LANE0_OUT_STAT, mmio);
+		if (ret)
+			return ret;
+		ret = imx_phy_reg_read(&val, mmio);
+		if (ret)
+			return ret;
+		if (val & IMX_LANE0_OUT_STAT_RX_PLL_STATE)
+			break;
+	} while (--timeout);
+
+	return timeout ? 0 : -ETIMEDOUT;
+}
+
 static int imx_sata_enable(struct ahci_host_priv *hpriv)
 {
 	struct imx_ahci_priv *imxpriv = hpriv->plat_data;
+	struct device *dev = &imxpriv->ahci_pdev->dev;
 	int ret;
 
 	if (imxpriv->no_device)
@@ -101,6 +257,14 @@ static int imx_sata_enable(struct ahci_host_priv *hpriv)
 		regmap_update_bits(imxpriv->gpr, IOMUXC_GPR13,
 				   IMX6Q_GPR13_SATA_MPLL_CLK_EN,
 				   IMX6Q_GPR13_SATA_MPLL_CLK_EN);
+
+		usleep_range(100, 200);
+
+		ret = imx_sata_phy_reset(hpriv);
+		if (ret) {
+			dev_err(dev, "failed to reset phy: %d\n", ret);
+			goto disable_regulator;
+		}
 	}
 
 	usleep_range(1000, 2000);
@@ -156,8 +320,8 @@ static void ahci_imx_error_handler(struct ata_port *ap)
 	 * without full reset once the pddq mode is enabled making it
 	 * impossible to use as part of libata LPM.
 	 */
-	reg_val = readl(mmio + PORT_PHY_CTL);
-	writel(reg_val | PORT_PHY_CTL_PDDQ_LOC, mmio + PORT_PHY_CTL);
+	reg_val = readl(mmio + IMX_P0PHYCR);
+	writel(reg_val | IMX_P0PHYCR_TEST_PDDQ, mmio + IMX_P0PHYCR);
 	imx_sata_disable(hpriv);
 	imxpriv->no_device = true;
 }
@@ -217,6 +381,7 @@ static int imx_ahci_probe(struct platform_device *pdev)
 	if (!imxpriv)
 		return -ENOMEM;
 
+	imxpriv->ahci_pdev = pdev;
 	imxpriv->no_device = false;
 	imxpriv->first_time = true;
 	imxpriv->type = (enum ahci_imx_type)of_id->data;
@@ -248,7 +413,7 @@ static int imx_ahci_probe(struct platform_device *pdev)
 
 	/*
 	 * Configure the HWINIT bits of the HOST_CAP and HOST_PORTS_IMPL,
-	 * and IP vendor specific register HOST_TIMER1MS.
+	 * and IP vendor specific register IMX_TIMER1MS.
 	 * Configure CAP_SSS (support stagered spin up).
 	 * Implement the port0.
 	 * Get the ahb clock rate, and configure the TIMER1MS register.
@@ -265,7 +430,7 @@ static int imx_ahci_probe(struct platform_device *pdev)
 	}
 
 	reg_val = clk_get_rate(imxpriv->ahb_clk) / 1000;
-	writel(reg_val, hpriv->mmio + HOST_TIMER1MS);
+	writel(reg_val, hpriv->mmio + IMX_TIMER1MS);
 
 	ret = ahci_platform_init_host(pdev, hpriv, &ahci_imx_port_info, 0, 0);
 	if (ret)
diff --git a/drivers/ata/libahci.c b/drivers/ata/libahci.c
index 6bd4f660b4e..b9861453fc8 100644
--- a/drivers/ata/libahci.c
+++ b/drivers/ata/libahci.c
@@ -452,6 +452,13 @@ void ahci_save_initial_config(struct device *dev,
 		cap &= ~HOST_CAP_SNTF;
 	}
 
+	if ((cap2 & HOST_CAP2_SDS) && (hpriv->flags & AHCI_HFLAG_NO_DEVSLP)) {
+		dev_info(dev,
+			 "controller can't do DEVSLP, turning off\n");
+		cap2 &= ~HOST_CAP2_SDS;
+		cap2 &= ~HOST_CAP2_SADM;
+	}
+
 	if (!(cap & HOST_CAP_FBS) && (hpriv->flags & AHCI_HFLAG_YES_FBS)) {
 		dev_info(dev, "controller can do FBS, turning on CAP_FBS\n");
 		cap |= HOST_CAP_FBS;
diff --git a/drivers/ata/libata-core.c b/drivers/ata/libata-core.c
index c19734d96d7..ea83828bfea 100644
--- a/drivers/ata/libata-core.c
+++ b/drivers/ata/libata-core.c
@@ -4224,8 +4224,10 @@ static const struct ata_blacklist_entry ata_device_blacklist [] = {
 	{ "PIONEER DVD-RW  DVR-216D",	NULL,	ATA_HORKAGE_NOSETXFER },
 
 	/* devices that don't properly handle queued TRIM commands */
-	{ "Micron_M500*",		NULL,	ATA_HORKAGE_NO_NCQ_TRIM, },
-	{ "Crucial_CT???M500SSD*",	NULL,	ATA_HORKAGE_NO_NCQ_TRIM, },
+	{ "Micron_M500*",		"MU0[1-4]*",	ATA_HORKAGE_NO_NCQ_TRIM, },
+	{ "Crucial_CT???M500SSD*",	"MU0[1-4]*",	ATA_HORKAGE_NO_NCQ_TRIM, },
+	{ "Micron_M550*",		NULL,		ATA_HORKAGE_NO_NCQ_TRIM, },
+	{ "Crucial_CT???M550SSD*",	NULL,		ATA_HORKAGE_NO_NCQ_TRIM, },
 
 	/*
 	 * Some WD SATA-I drives spin up and down erratically when the link
@@ -4792,21 +4794,26 @@ void swap_buf_le16(u16 *buf, unsigned int buf_words)
 static struct ata_queued_cmd *ata_qc_new(struct ata_port *ap)
 {
 	struct ata_queued_cmd *qc = NULL;
-	unsigned int i;
+	unsigned int i, tag;
 
 	/* no command while frozen */
 	if (unlikely(ap->pflags & ATA_PFLAG_FROZEN))
 		return NULL;
 
-	/* the last tag is reserved for internal command. */
-	for (i = 0; i < ATA_MAX_QUEUE - 1; i++)
-		if (!test_and_set_bit(i, &ap->qc_allocated)) {
-			qc = __ata_qc_from_tag(ap, i);
+	for (i = 0; i < ATA_MAX_QUEUE; i++) {
+		tag = (i + ap->last_tag + 1) % ATA_MAX_QUEUE;
+
+		/* the last tag is reserved for internal command. */
+		if (tag == ATA_TAG_INTERNAL)
+			continue;
+
+		if (!test_and_set_bit(tag, &ap->qc_allocated)) {
+			qc = __ata_qc_from_tag(ap, tag);
+			qc->tag = tag;
+			ap->last_tag = tag;
 			break;
 		}
-
-	if (qc)
-		qc->tag = i;
+	}
 
 	return qc;
 }
@@ -6307,6 +6314,8 @@ int ata_host_activate(struct ata_host *host, int irq,
 static void ata_port_detach(struct ata_port *ap)
 {
 	unsigned long flags;
+	struct ata_link *link;
+	struct ata_device *dev;
 
 	if (!ap->ops->error_handler)
 		goto skip_eh;
@@ -6326,6 +6335,13 @@ static void ata_port_detach(struct ata_port *ap)
 	cancel_delayed_work_sync(&ap->hotplug_task);
 
  skip_eh:
+	/* clean up zpodd on port removal */
+	ata_for_each_link(link, ap, HOST_FIRST) {
+		ata_for_each_dev(dev, link, ALL) {
+			if (zpodd_dev_enabled(dev))
+				zpodd_exit(dev);
+		}
+	}
 	if (ap->pmp_link) {
 		int i;
 		for (i = 0; i < SATA_PMP_MAX_PORTS; i++)
diff --git a/drivers/ata/pata_arasan_cf.c b/drivers/ata/pata_arasan_cf.c
index 6fac524c2f5..4edb1a81f63 100644
--- a/drivers/ata/pata_arasan_cf.c
+++ b/drivers/ata/pata_arasan_cf.c
@@ -898,9 +898,12 @@ static int arasan_cf_probe(struct platform_device *pdev)
 
 	cf_card_detect(acdev, 0);
 
-	return ata_host_activate(host, acdev->irq, irq_handler, 0,
-			&arasan_cf_sht);
+	ret = ata_host_activate(host, acdev->irq, irq_handler, 0,
+				&arasan_cf_sht);
+	if (!ret)
+		return 0;
 
+	cf_exit(acdev);
 free_clk:
 	clk_put(acdev->clk);
 	return ret;
diff --git a/drivers/ata/pata_at91.c b/drivers/ata/pata_at91.c
index e9c87274a78..8a66f23af4c 100644
--- a/drivers/ata/pata_at91.c
+++ b/drivers/ata/pata_at91.c
@@ -407,12 +407,13 @@ static int pata_at91_probe(struct platform_device *pdev)
 
 	host->private_data = info;
 
-	return ata_host_activate(host, gpio_is_valid(irq) ? gpio_to_irq(irq) : 0,
-			gpio_is_valid(irq) ? ata_sff_interrupt : NULL,
-			irq_flags, &pata_at91_sht);
+	ret = ata_host_activate(host, gpio_is_valid(irq) ? gpio_to_irq(irq) : 0,
+				gpio_is_valid(irq) ? ata_sff_interrupt : NULL,
+				irq_flags, &pata_at91_sht);
+	if (ret)
+		goto err_put;
 
-	if (!ret)
-		return 0;
+	return 0;
 
 err_put:
 	clk_put(info->mck);
diff --git a/drivers/ata/pata_samsung_cf.c b/drivers/ata/pata_samsung_cf.c
index a79566d0566..0610e78c8a2 100644
--- a/drivers/ata/pata_samsung_cf.c
+++ b/drivers/ata/pata_samsung_cf.c
@@ -594,9 +594,13 @@ static int __init pata_s3c_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, host);
 
-	return ata_host_activate(host, info->irq,
-			info->irq ? pata_s3c_irq : NULL,
-			0, &pata_s3c_sht);
+	ret = ata_host_activate(host, info->irq,
+				info->irq ? pata_s3c_irq : NULL,
+				0, &pata_s3c_sht);
+	if (ret)
+		goto stop_clk;
+
+	return 0;
 
 stop_clk:
 	clk_disable(info->clk);
diff --git a/drivers/base/Kconfig b/drivers/base/Kconfig
index 8fa8deab644..4b7b4522b64 100644
--- a/drivers/base/Kconfig
+++ b/drivers/base/Kconfig
@@ -1,10 +1,10 @@
 menu "Generic Driver Options"
 
-config UEVENT_HELPER_PATH
-	string "path to uevent helper"
-	default ""
+config UEVENT_HELPER
+	bool "Support for uevent helper"
+	default y
 	help
-	  Path to uevent helper program forked by the kernel for
+	  The uevent helper program is forked by the kernel for
 	  every uevent.
 	  Before the switch to the netlink-based uevent source, this was
 	  used to hook hotplug scripts into kernel device events. It
@@ -15,8 +15,13 @@ config UEVENT_HELPER_PATH
 	  that it creates a high system load, or on smaller systems
 	  it is known to create out-of-memory situations during bootup.
 
-	  To disable user space helper program execution at early boot
-	  time specify an empty string here. This setting can be altered
+config UEVENT_HELPER_PATH
+	string "path to uevent helper"
+	depends on UEVENT_HELPER
+	default ""
+	help
+	  To disable user space helper program execution at by default
+	  specify an empty string here. This setting can still be altered
 	  via /proc/sys/kernel/hotplug or via /sys/kernel/uevent_helper
 	  later at runtime.
 
diff --git a/drivers/base/base.h b/drivers/base/base.h
index 24f424249d9..251c5d30f96 100644
--- a/drivers/base/base.h
+++ b/drivers/base/base.h
@@ -63,8 +63,6 @@ struct driver_private {
  *	binding of drivers which were unable to get all the resources needed by
  *	the device; typically because it depends on another driver getting
  *	probed first.
- * @driver_data - private pointer for driver specific info.  Will turn into a
- * list soon.
  * @device - pointer back to the struct class that this structure is
  * associated with.
  *
@@ -76,7 +74,6 @@ struct device_private {
 	struct klist_node knode_driver;
 	struct klist_node knode_bus;
 	struct list_head deferred_probe;
-	void *driver_data;
 	struct device *device;
 };
 #define to_device_private_parent(obj)	\
diff --git a/drivers/base/core.c b/drivers/base/core.c
index 0dd65281cc6..20da3ad1696 100644
--- a/drivers/base/core.c
+++ b/drivers/base/core.c
@@ -614,39 +614,6 @@ void device_remove_bin_file(struct device *dev,
 }
 EXPORT_SYMBOL_GPL(device_remove_bin_file);
 
-/**
- * device_schedule_callback_owner - helper to schedule a callback for a device
- * @dev: device.
- * @func: callback function to invoke later.
- * @owner: module owning the callback routine
- *
- * Attribute methods must not unregister themselves or their parent device
- * (which would amount to the same thing).  Attempts to do so will deadlock,
- * since unregistration is mutually exclusive with driver callbacks.
- *
- * Instead methods can call this routine, which will attempt to allocate
- * and schedule a workqueue request to call back @func with @dev as its
- * argument in the workqueue's process context.  @dev will be pinned until
- * @func returns.
- *
- * This routine is usually called via the inline device_schedule_callback(),
- * which automatically sets @owner to THIS_MODULE.
- *
- * Returns 0 if the request was submitted, -ENOMEM if storage could not
- * be allocated, -ENODEV if a reference to @owner isn't available.
- *
- * NOTE: This routine won't work if CONFIG_SYSFS isn't set!  It uses an
- * underlying sysfs routine (since it is intended for use by attribute
- * methods), and if sysfs isn't available you'll get nothing but -ENOSYS.
- */
-int device_schedule_callback_owner(struct device *dev,
-		void (*func)(struct device *), struct module *owner)
-{
-	return sysfs_schedule_callback(&dev->kobj,
-			(void (*)(void *)) func, dev, owner);
-}
-EXPORT_SYMBOL_GPL(device_schedule_callback_owner);
-
 static void klist_children_get(struct klist_node *n)
 {
 	struct device_private *p = to_device_private_parent(n);
diff --git a/drivers/base/dd.c b/drivers/base/dd.c
index 06051767393..e4ffbcf2f51 100644
--- a/drivers/base/dd.c
+++ b/drivers/base/dd.c
@@ -52,6 +52,7 @@ static DEFINE_MUTEX(deferred_probe_mutex);
 static LIST_HEAD(deferred_probe_pending_list);
 static LIST_HEAD(deferred_probe_active_list);
 static struct workqueue_struct *deferred_wq;
+static atomic_t deferred_trigger_count = ATOMIC_INIT(0);
 
 /**
  * deferred_probe_work_func() - Retry probing devices in the active list.
@@ -135,6 +136,17 @@ static bool driver_deferred_probe_enable = false;
  * This functions moves all devices from the pending list to the active
  * list and schedules the deferred probe workqueue to process them.  It
  * should be called anytime a driver is successfully bound to a device.
+ *
+ * Note, there is a race condition in multi-threaded probe. In the case where
+ * more than one device is probing at the same time, it is possible for one
+ * probe to complete successfully while another is about to defer. If the second
+ * depends on the first, then it will get put on the pending list after the
+ * trigger event has already occured and will be stuck there.
+ *
+ * The atomic 'deferred_trigger_count' is used to determine if a successful
+ * trigger has occurred in the midst of probing a driver. If the trigger count
+ * changes in the midst of a probe, then deferred processing should be triggered
+ * again.
  */
 static void driver_deferred_probe_trigger(void)
 {
@@ -147,6 +159,7 @@ static void driver_deferred_probe_trigger(void)
 	 * into the active list so they can be retried by the workqueue
 	 */
 	mutex_lock(&deferred_probe_mutex);
+	atomic_inc(&deferred_trigger_count);
 	list_splice_tail_init(&deferred_probe_pending_list,
 			      &deferred_probe_active_list);
 	mutex_unlock(&deferred_probe_mutex);
@@ -187,8 +200,8 @@ static void driver_bound(struct device *dev)
 		return;
 	}
 
-	pr_debug("driver: '%s': %s: bound to device '%s'\n", dev_name(dev),
-		 __func__, dev->driver->name);
+	pr_debug("driver: '%s': %s: bound to device '%s'\n", dev->driver->name,
+		 __func__, dev_name(dev));
 
 	klist_add_tail(&dev->p->knode_driver, &dev->driver->p->klist_devices);
 
@@ -265,6 +278,7 @@ static DECLARE_WAIT_QUEUE_HEAD(probe_waitqueue);
 static int really_probe(struct device *dev, struct device_driver *drv)
 {
 	int ret = 0;
+	int local_trigger_count = atomic_read(&deferred_trigger_count);
 
 	atomic_inc(&probe_count);
 	pr_debug("bus: '%s': %s: probing driver %s with device %s\n",
@@ -310,6 +324,9 @@ probe_failed:
 		/* Driver requested deferred probing */
 		dev_info(dev, "Driver %s requests probe deferral\n", drv->name);
 		driver_deferred_probe_add(dev);
+		/* Did a trigger occur while probing? Need to re-trigger if yes */
+		if (local_trigger_count != atomic_read(&deferred_trigger_count))
+			driver_deferred_probe_trigger();
 	} else if (ret != -ENODEV && ret != -ENXIO) {
 		/* driver matched but the probe failed */
 		printk(KERN_WARNING
@@ -570,29 +587,3 @@ void driver_detach(struct device_driver *drv)
 		put_device(dev);
 	}
 }
-
-/*
- * These exports can't be _GPL due to .h files using this within them, and it
- * might break something that was previously working...
- */
-void *dev_get_drvdata(const struct device *dev)
-{
-	if (dev && dev->p)
-		return dev->p->driver_data;
-	return NULL;
-}
-EXPORT_SYMBOL(dev_get_drvdata);
-
-int dev_set_drvdata(struct device *dev, void *data)
-{
-	int error;
-
-	if (!dev->p) {
-		error = device_private_init(dev);
-		if (error)
-			return error;
-	}
-	dev->p->driver_data = data;
-	return 0;
-}
-EXPORT_SYMBOL(dev_set_drvdata);
diff --git a/drivers/base/devres.c b/drivers/base/devres.c
index db4e264eecb..52302946770 100644
--- a/drivers/base/devres.c
+++ b/drivers/base/devres.c
@@ -831,3 +831,100 @@ void devm_kfree(struct device *dev, void *p)
 	WARN_ON(rc);
 }
 EXPORT_SYMBOL_GPL(devm_kfree);
+
+/**
+ * devm_kmemdup - Resource-managed kmemdup
+ * @dev: Device this memory belongs to
+ * @src: Memory region to duplicate
+ * @len: Memory region length
+ * @gfp: GFP mask to use
+ *
+ * Duplicate region of a memory using resource managed kmalloc
+ */
+void *devm_kmemdup(struct device *dev, const void *src, size_t len, gfp_t gfp)
+{
+	void *p;
+
+	p = devm_kmalloc(dev, len, gfp);
+	if (p)
+		memcpy(p, src, len);
+
+	return p;
+}
+EXPORT_SYMBOL_GPL(devm_kmemdup);
+
+struct pages_devres {
+	unsigned long addr;
+	unsigned int order;
+};
+
+static int devm_pages_match(struct device *dev, void *res, void *p)
+{
+	struct pages_devres *devres = res;
+	struct pages_devres *target = p;
+
+	return devres->addr == target->addr;
+}
+
+static void devm_pages_release(struct device *dev, void *res)
+{
+	struct pages_devres *devres = res;
+
+	free_pages(devres->addr, devres->order);
+}
+
+/**
+ * devm_get_free_pages - Resource-managed __get_free_pages
+ * @dev: Device to allocate memory for
+ * @gfp_mask: Allocation gfp flags
+ * @order: Allocation size is (1 << order) pages
+ *
+ * Managed get_free_pages.  Memory allocated with this function is
+ * automatically freed on driver detach.
+ *
+ * RETURNS:
+ * Address of allocated memory on success, 0 on failure.
+ */
+
+unsigned long devm_get_free_pages(struct device *dev,
+				  gfp_t gfp_mask, unsigned int order)
+{
+	struct pages_devres *devres;
+	unsigned long addr;
+
+	addr = __get_free_pages(gfp_mask, order);
+
+	if (unlikely(!addr))
+		return 0;
+
+	devres = devres_alloc(devm_pages_release,
+			      sizeof(struct pages_devres), GFP_KERNEL);
+	if (unlikely(!devres)) {
+		free_pages(addr, order);
+		return 0;
+	}
+
+	devres->addr = addr;
+	devres->order = order;
+
+	devres_add(dev, devres);
+	return addr;
+}
+EXPORT_SYMBOL_GPL(devm_get_free_pages);
+
+/**
+ * devm_free_pages - Resource-managed free_pages
+ * @dev: Device this memory belongs to
+ * @addr: Memory to free
+ *
+ * Free memory allocated with devm_get_free_pages(). Unlike free_pages,
+ * there is no need to supply the @order.
+ */
+void devm_free_pages(struct device *dev, unsigned long addr)
+{
+	struct pages_devres devres = { .addr = addr };
+
+	WARN_ON(devres_release(dev, devm_pages_release, devm_pages_match,
+			       &devres));
+}
+EXPORT_SYMBOL_GPL(devm_free_pages);
diff --git a/drivers/base/dma-coherent.c b/drivers/base/dma-coherent.c
index bc256b64102..7d6e84a5142 100644
--- a/drivers/base/dma-coherent.c
+++ b/drivers/base/dma-coherent.c
@@ -10,13 +10,13 @@
 struct dma_coherent_mem {
 	void		*virt_base;
 	dma_addr_t	device_base;
-	phys_addr_t	pfn_base;
+	unsigned long	pfn_base;
 	int		size;
 	int		flags;
 	unsigned long	*bitmap;
 };
 
-int dma_declare_coherent_memory(struct device *dev, dma_addr_t bus_addr,
+int dma_declare_coherent_memory(struct device *dev, phys_addr_t phys_addr,
 				dma_addr_t device_addr, size_t size, int flags)
 {
 	void __iomem *mem_base = NULL;
@@ -32,7 +32,7 @@ int dma_declare_coherent_memory(struct device *dev, dma_addr_t bus_addr,
 
 	/* FIXME: this routine just ignores DMA_MEMORY_INCLUDES_CHILDREN */
 
-	mem_base = ioremap(bus_addr, size);
+	mem_base = ioremap(phys_addr, size);
 	if (!mem_base)
 		goto out;
 
@@ -45,7 +45,7 @@ int dma_declare_coherent_memory(struct device *dev, dma_addr_t bus_addr,
 
 	dev->dma_mem->virt_base = mem_base;
 	dev->dma_mem->device_base = device_addr;
-	dev->dma_mem->pfn_base = PFN_DOWN(bus_addr);
+	dev->dma_mem->pfn_base = PFN_DOWN(phys_addr);
 	dev->dma_mem->size = pages;
 	dev->dma_mem->flags = flags;
 
@@ -208,7 +208,7 @@ int dma_mmap_from_coherent(struct device *dev, struct vm_area_struct *vma,
 
 		*ret = -ENXIO;
 		if (off < count && user_count <= count - off) {
-			unsigned pfn = mem->pfn_base + start + off;
+			unsigned long pfn = mem->pfn_base + start + off;
 			*ret = remap_pfn_range(vma, vma->vm_start, pfn,
 					       user_count << PAGE_SHIFT,
 					       vma->vm_page_prot);
diff --git a/drivers/base/dma-contiguous.c b/drivers/base/dma-contiguous.c
index 165c2c299e5..c34ec336424 100644
--- a/drivers/base/dma-contiguous.c
+++ b/drivers/base/dma-contiguous.c
@@ -37,6 +37,7 @@ struct cma {
 	unsigned long	base_pfn;
 	unsigned long	count;
 	unsigned long	*bitmap;
+	struct mutex	lock;
 };
 
 struct cma *dma_contiguous_default_area;
@@ -161,6 +162,7 @@ static int __init cma_activate_area(struct cma *cma)
 		init_cma_reserved_pageblock(pfn_to_page(base_pfn));
 	} while (--i);
 
+	mutex_init(&cma->lock);
 	return 0;
 }
 
@@ -261,6 +263,13 @@ err:
 	return ret;
 }
 
+static void clear_cma_bitmap(struct cma *cma, unsigned long pfn, int count)
+{
+	mutex_lock(&cma->lock);
+	bitmap_clear(cma->bitmap, pfn - cma->base_pfn, count);
+	mutex_unlock(&cma->lock);
+}
+
 /**
  * dma_alloc_from_contiguous() - allocate pages from contiguous area
  * @dev:   Pointer to device for which the allocation is performed.
@@ -269,7 +278,7 @@ err:
  *
  * This function allocates memory buffer for specified device. It uses
  * device specific contiguous memory area if available or the default
- * global one. Requires architecture specific get_dev_cma_area() helper
+ * global one. Requires architecture specific dev_get_cma_area() helper
  * function.
  */
 struct page *dma_alloc_from_contiguous(struct device *dev, int count,
@@ -294,30 +303,41 @@ struct page *dma_alloc_from_contiguous(struct device *dev, int count,
 
 	mask = (1 << align) - 1;
 
-	mutex_lock(&cma_mutex);
 
 	for (;;) {
+		mutex_lock(&cma->lock);
 		pageno = bitmap_find_next_zero_area(cma->bitmap, cma->count,
 						    start, count, mask);
-		if (pageno >= cma->count)
+		if (pageno >= cma->count) {
+			mutex_unlock(&cma->lock);
 			break;
+		}
+		bitmap_set(cma->bitmap, pageno, count);
+		/*
+		 * It's safe to drop the lock here. We've marked this region for
+		 * our exclusive use. If the migration fails we will take the
+		 * lock again and unmark it.
+		 */
+		mutex_unlock(&cma->lock);
 
 		pfn = cma->base_pfn + pageno;
+		mutex_lock(&cma_mutex);
 		ret = alloc_contig_range(pfn, pfn + count, MIGRATE_CMA);
+		mutex_unlock(&cma_mutex);
 		if (ret == 0) {
-			bitmap_set(cma->bitmap, pageno, count);
 			page = pfn_to_page(pfn);
 			break;
 		} else if (ret != -EBUSY) {
+			clear_cma_bitmap(cma, pfn, count);
 			break;
 		}
+		clear_cma_bitmap(cma, pfn, count);
 		pr_debug("%s(): memory range at %p is busy, retrying\n",
 			 __func__, pfn_to_page(pfn));
 		/* try again with a bit different memory target */
 		start = pageno + mask + 1;
 	}
 
-	mutex_unlock(&cma_mutex);
 	pr_debug("%s(): returned %p\n", __func__, page);
 	return page;
 }
@@ -350,10 +370,8 @@ bool dma_release_from_contiguous(struct device *dev, struct page *pages,
 
 	VM_BUG_ON(pfn + count > cma->base_pfn + cma->count);
 
-	mutex_lock(&cma_mutex);
-	bitmap_clear(cma->bitmap, pfn - cma->base_pfn, count);
 	free_contig_range(pfn, count);
-	mutex_unlock(&cma_mutex);
+	clear_cma_bitmap(cma, pfn, count);
 
 	return true;
 }
diff --git a/drivers/base/dma-mapping.c b/drivers/base/dma-mapping.c
index 0ce39a33b3c..6cd08e145bf 100644
--- a/drivers/base/dma-mapping.c
+++ b/drivers/base/dma-mapping.c
@@ -175,7 +175,7 @@ static void dmam_coherent_decl_release(struct device *dev, void *res)
 /**
  * dmam_declare_coherent_memory - Managed dma_declare_coherent_memory()
  * @dev: Device to declare coherent memory for
- * @bus_addr: Bus address of coherent memory to be declared
+ * @phys_addr: Physical address of coherent memory to be declared
  * @device_addr: Device address of coherent memory to be declared
  * @size: Size of coherent memory to be declared
  * @flags: Flags
@@ -185,7 +185,7 @@ static void dmam_coherent_decl_release(struct device *dev, void *res)
  * RETURNS:
  * 0 on success, -errno on failure.
  */
-int dmam_declare_coherent_memory(struct device *dev, dma_addr_t bus_addr,
+int dmam_declare_coherent_memory(struct device *dev, phys_addr_t phys_addr,
 				 dma_addr_t device_addr, size_t size, int flags)
 {
 	void *res;
@@ -195,7 +195,7 @@ int dmam_declare_coherent_memory(struct device *dev, dma_addr_t bus_addr,
 	if (!res)
 		return -ENOMEM;
 
-	rc = dma_declare_coherent_memory(dev, bus_addr, device_addr, size,
+	rc = dma_declare_coherent_memory(dev, phys_addr, device_addr, size,
 					 flags);
 	if (rc == 0)
 		devres_add(dev, res);
diff --git a/drivers/base/platform.c b/drivers/base/platform.c
index e714709704e..5b47210889e 100644
--- a/drivers/base/platform.c
+++ b/drivers/base/platform.c
@@ -13,6 +13,7 @@
 #include <linux/string.h>
 #include <linux/platform_device.h>
 #include <linux/of_device.h>
+#include <linux/of_irq.h>
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/dma-mapping.h>
@@ -87,7 +88,11 @@ int platform_get_irq(struct platform_device *dev, unsigned int num)
 		return -ENXIO;
 	return dev->archdata.irqs[num];
 #else
-	struct resource *r = platform_get_resource(dev, IORESOURCE_IRQ, num);
+	struct resource *r;
+	if (IS_ENABLED(CONFIG_OF_IRQ) && dev->dev.of_node)
+		return of_irq_get(dev->dev.of_node, num);
+
+	r = platform_get_resource(dev, IORESOURCE_IRQ, num);
 
 	return r ? r->start : -ENXIO;
 #endif
diff --git a/drivers/base/regmap/regcache-rbtree.c b/drivers/base/regmap/regcache-rbtree.c
index 930cad4e5df..6a7e4fa1285 100644
--- a/drivers/base/regmap/regcache-rbtree.c
+++ b/drivers/base/regmap/regcache-rbtree.c
@@ -23,16 +23,16 @@ static int regcache_rbtree_write(struct regmap *map, unsigned int reg,
 static int regcache_rbtree_exit(struct regmap *map);
 
 struct regcache_rbtree_node {
-	/* the actual rbtree node holding this block */
-	struct rb_node node;
-	/* base register handled by this block */
-	unsigned int base_reg;
 	/* block of adjacent registers */
 	void *block;
 	/* Which registers are present */
 	long *cache_present;
+	/* base register handled by this block */
+	unsigned int base_reg;
 	/* number of registers available in the block */
 	unsigned int blklen;
+	/* the actual rbtree node holding this block */
+	struct rb_node node;
 } __attribute__ ((packed));
 
 struct regcache_rbtree_ctx {
diff --git a/drivers/base/regmap/regmap-i2c.c b/drivers/base/regmap/regmap-i2c.c
index ebd18952976..ca193d1ef47 100644
--- a/drivers/base/regmap/regmap-i2c.c
+++ b/drivers/base/regmap/regmap-i2c.c
@@ -14,6 +14,79 @@
 #include <linux/i2c.h>
 #include <linux/module.h>
 
+
+static int regmap_smbus_byte_reg_read(void *context, unsigned int reg,
+				      unsigned int *val)
+{
+	struct device *dev = context;
+	struct i2c_client *i2c = to_i2c_client(dev);
+	int ret;
+
+	if (reg > 0xff)
+		return -EINVAL;
+
+	ret = i2c_smbus_read_byte_data(i2c, reg);
+	if (ret < 0)
+		return ret;
+
+	*val = ret;
+
+	return 0;
+}
+
+static int regmap_smbus_byte_reg_write(void *context, unsigned int reg,
+				       unsigned int val)
+{
+	struct device *dev = context;
+	struct i2c_client *i2c = to_i2c_client(dev);
+
+	if (val > 0xff || reg > 0xff)
+		return -EINVAL;
+
+	return i2c_smbus_write_byte_data(i2c, reg, val);
+}
+
+static struct regmap_bus regmap_smbus_byte = {
+	.reg_write = regmap_smbus_byte_reg_write,
+	.reg_read = regmap_smbus_byte_reg_read,
+};
+
+static int regmap_smbus_word_reg_read(void *context, unsigned int reg,
+				      unsigned int *val)
+{
+	struct device *dev = context;
+	struct i2c_client *i2c = to_i2c_client(dev);
+	int ret;
+
+	if (reg > 0xff)
+		return -EINVAL;
+
+	ret = i2c_smbus_read_word_data(i2c, reg);
+	if (ret < 0)
+		return ret;
+
+	*val = ret;
+
+	return 0;
+}
+
+static int regmap_smbus_word_reg_write(void *context, unsigned int reg,
+				       unsigned int val)
+{
+	struct device *dev = context;
+	struct i2c_client *i2c = to_i2c_client(dev);
+
+	if (val > 0xffff || reg > 0xff)
+		return -EINVAL;
+
+	return i2c_smbus_write_word_data(i2c, reg, val);
+}
+
+static struct regmap_bus regmap_smbus_word = {
+	.reg_write = regmap_smbus_word_reg_write,
+	.reg_read = regmap_smbus_word_reg_read,
+};
+
 static int regmap_i2c_write(void *context, const void *data, size_t count)
 {
 	struct device *dev = context;
@@ -97,6 +170,23 @@ static struct regmap_bus regmap_i2c = {
 	.read = regmap_i2c_read,
 };
 
+static const struct regmap_bus *regmap_get_i2c_bus(struct i2c_client *i2c,
+					const struct regmap_config *config)
+{
+	if (i2c_check_functionality(i2c->adapter, I2C_FUNC_I2C))
+		return &regmap_i2c;
+	else if (config->val_bits == 16 && config->reg_bits == 8 &&
+		 i2c_check_functionality(i2c->adapter,
+					 I2C_FUNC_SMBUS_WORD_DATA))
+		return &regmap_smbus_word;
+	else if (config->val_bits == 8 && config->reg_bits == 8 &&
+		 i2c_check_functionality(i2c->adapter,
+					 I2C_FUNC_SMBUS_BYTE_DATA))
+		return &regmap_smbus_byte;
+
+	return ERR_PTR(-ENOTSUPP);
+}
+
 /**
  * regmap_init_i2c(): Initialise register map
  *
@@ -109,7 +199,12 @@ static struct regmap_bus regmap_i2c = {
 struct regmap *regmap_init_i2c(struct i2c_client *i2c,
 			       const struct regmap_config *config)
 {
-	return regmap_init(&i2c->dev, &regmap_i2c, &i2c->dev, config);
+	const struct regmap_bus *bus = regmap_get_i2c_bus(i2c, config);
+
+	if (IS_ERR(bus))
+		return ERR_CAST(bus);
+
+	return regmap_init(&i2c->dev, bus, &i2c->dev, config);
 }
 EXPORT_SYMBOL_GPL(regmap_init_i2c);
 
@@ -126,7 +221,12 @@ EXPORT_SYMBOL_GPL(regmap_init_i2c);
 struct regmap *devm_regmap_init_i2c(struct i2c_client *i2c,
 				    const struct regmap_config *config)
 {
-	return devm_regmap_init(&i2c->dev, &regmap_i2c, &i2c->dev, config);
+	const struct regmap_bus *bus = regmap_get_i2c_bus(i2c, config);
+
+	if (IS_ERR(bus))
+		return ERR_CAST(bus);
+
+	return devm_regmap_init(&i2c->dev, bus, &i2c->dev, config);
 }
 EXPORT_SYMBOL_GPL(devm_regmap_init_i2c);
 
diff --git a/drivers/base/regmap/regmap-irq.c b/drivers/base/regmap/regmap-irq.c
index edf88f20cbc..6299a50a596 100644
--- a/drivers/base/regmap/regmap-irq.c
+++ b/drivers/base/regmap/regmap-irq.c
@@ -10,13 +10,13 @@
  * published by the Free Software Foundation.
  */
 
-#include <linux/export.h>
 #include <linux/device.h>
-#include <linux/regmap.h>
-#include <linux/irq.h>
+#include <linux/export.h>
 #include <linux/interrupt.h>
+#include <linux/irq.h>
 #include <linux/irqdomain.h>
 #include <linux/pm_runtime.h>
+#include <linux/regmap.h>
 #include <linux/slab.h>
 
 #include "internal.h"
@@ -347,6 +347,9 @@ int regmap_add_irq_chip(struct regmap *map, int irq, int irq_flags,
 	int ret = -ENOMEM;
 	u32 reg;
 
+	if (chip->num_regs <= 0)
+		return -EINVAL;
+
 	for (i = 0; i < chip->num_irqs; i++) {
 		if (chip->irqs[i].reg_offset % map->reg_stride)
 			return -EINVAL;
diff --git a/drivers/base/regmap/regmap-mmio.c b/drivers/base/regmap/regmap-mmio.c
index 1e03e7f8bac..04a329a377e 100644
--- a/drivers/base/regmap/regmap-mmio.c
+++ b/drivers/base/regmap/regmap-mmio.c
@@ -61,9 +61,28 @@ static int regmap_mmio_regbits_check(size_t reg_bits)
 	}
 }
 
-static inline void regmap_mmio_count_check(size_t count)
+static inline void regmap_mmio_count_check(size_t count, u32 offset)
 {
-	BUG_ON(count % 2 != 0);
+	BUG_ON(count <= offset);
+}
+
+static inline unsigned int
+regmap_mmio_get_offset(const void *reg, size_t reg_size)
+{
+	switch (reg_size) {
+	case 1:
+		return *(u8 *)reg;
+	case 2:
+		return *(u16 *)reg;
+	case 4:
+		return *(u32 *)reg;
+#ifdef CONFIG_64BIT
+	case 8:
+		return *(u64 *)reg;
+#endif
+	default:
+		BUG();
+	}
 }
 
 static int regmap_mmio_gather_write(void *context,
@@ -71,7 +90,7 @@ static int regmap_mmio_gather_write(void *context,
 				    const void *val, size_t val_size)
 {
 	struct regmap_mmio_context *ctx = context;
-	u32 offset;
+	unsigned int offset;
 	int ret;
 
 	regmap_mmio_regsize_check(reg_size);
@@ -82,7 +101,7 @@ static int regmap_mmio_gather_write(void *context,
 			return ret;
 	}
 
-	offset = *(u32 *)reg;
+	offset = regmap_mmio_get_offset(reg, reg_size);
 
 	while (val_size) {
 		switch (ctx->val_bytes) {
@@ -118,9 +137,9 @@ static int regmap_mmio_gather_write(void *context,
 static int regmap_mmio_write(void *context, const void *data, size_t count)
 {
 	struct regmap_mmio_context *ctx = context;
-	u32 offset = ctx->reg_bytes + ctx->pad_bytes;
+	unsigned int offset = ctx->reg_bytes + ctx->pad_bytes;
 
-	regmap_mmio_count_check(count);
+	regmap_mmio_count_check(count, offset);
 
 	return regmap_mmio_gather_write(context, data, ctx->reg_bytes,
 					data + offset, count - offset);
@@ -131,7 +150,7 @@ static int regmap_mmio_read(void *context,
 			    void *val, size_t val_size)
 {
 	struct regmap_mmio_context *ctx = context;
-	u32 offset;
+	unsigned int offset;
 	int ret;
 
 	regmap_mmio_regsize_check(reg_size);
@@ -142,7 +161,7 @@ static int regmap_mmio_read(void *context,
 			return ret;
 	}
 
-	offset = *(u32 *)reg;
+	offset = regmap_mmio_get_offset(reg, reg_size);
 
 	while (val_size) {
 		switch (ctx->val_bytes) {
diff --git a/drivers/base/regmap/regmap.c b/drivers/base/regmap/regmap.c
index 63e30ef096e..74d8c0672cf 100644
--- a/drivers/base/regmap/regmap.c
+++ b/drivers/base/regmap/regmap.c
@@ -35,10 +35,14 @@ static int _regmap_update_bits(struct regmap *map, unsigned int reg,
 			       unsigned int mask, unsigned int val,
 			       bool *change);
 
+static int _regmap_bus_reg_read(void *context, unsigned int reg,
+				unsigned int *val);
 static int _regmap_bus_read(void *context, unsigned int reg,
 			    unsigned int *val);
 static int _regmap_bus_formatted_write(void *context, unsigned int reg,
 				       unsigned int val);
+static int _regmap_bus_reg_write(void *context, unsigned int reg,
+				 unsigned int val);
 static int _regmap_bus_raw_write(void *context, unsigned int reg,
 				 unsigned int val);
 
@@ -192,6 +196,13 @@ static void regmap_format_16_be(void *buf, unsigned int val, unsigned int shift)
 	b[0] = cpu_to_be16(val << shift);
 }
 
+static void regmap_format_16_le(void *buf, unsigned int val, unsigned int shift)
+{
+	__le16 *b = buf;
+
+	b[0] = cpu_to_le16(val << shift);
+}
+
 static void regmap_format_16_native(void *buf, unsigned int val,
 				    unsigned int shift)
 {
@@ -216,6 +227,13 @@ static void regmap_format_32_be(void *buf, unsigned int val, unsigned int shift)
 	b[0] = cpu_to_be32(val << shift);
 }
 
+static void regmap_format_32_le(void *buf, unsigned int val, unsigned int shift)
+{
+	__le32 *b = buf;
+
+	b[0] = cpu_to_le32(val << shift);
+}
+
 static void regmap_format_32_native(void *buf, unsigned int val,
 				    unsigned int shift)
 {
@@ -240,6 +258,13 @@ static unsigned int regmap_parse_16_be(const void *buf)
 	return be16_to_cpu(b[0]);
 }
 
+static unsigned int regmap_parse_16_le(const void *buf)
+{
+	const __le16 *b = buf;
+
+	return le16_to_cpu(b[0]);
+}
+
 static void regmap_parse_16_be_inplace(void *buf)
 {
 	__be16 *b = buf;
@@ -247,6 +272,13 @@ static void regmap_parse_16_be_inplace(void *buf)
 	b[0] = be16_to_cpu(b[0]);
 }
 
+static void regmap_parse_16_le_inplace(void *buf)
+{
+	__le16 *b = buf;
+
+	b[0] = le16_to_cpu(b[0]);
+}
+
 static unsigned int regmap_parse_16_native(const void *buf)
 {
 	return *(u16 *)buf;
@@ -269,6 +301,13 @@ static unsigned int regmap_parse_32_be(const void *buf)
 	return be32_to_cpu(b[0]);
 }
 
+static unsigned int regmap_parse_32_le(const void *buf)
+{
+	const __le32 *b = buf;
+
+	return le32_to_cpu(b[0]);
+}
+
 static void regmap_parse_32_be_inplace(void *buf)
 {
 	__be32 *b = buf;
@@ -276,6 +315,13 @@ static void regmap_parse_32_be_inplace(void *buf)
 	b[0] = be32_to_cpu(b[0]);
 }
 
+static void regmap_parse_32_le_inplace(void *buf)
+{
+	__le32 *b = buf;
+
+	b[0] = le32_to_cpu(b[0]);
+}
+
 static unsigned int regmap_parse_32_native(const void *buf)
 {
 	return *(u32 *)buf;
@@ -495,6 +541,12 @@ struct regmap *regmap_init(struct device *dev,
 
 		map->defer_caching = false;
 		goto skip_format_initialization;
+	} else if (!bus->read || !bus->write) {
+		map->reg_read = _regmap_bus_reg_read;
+		map->reg_write = _regmap_bus_reg_write;
+
+		map->defer_caching = false;
+		goto skip_format_initialization;
 	} else {
 		map->reg_read  = _regmap_bus_read;
 	}
@@ -608,6 +660,11 @@ struct regmap *regmap_init(struct device *dev,
 			map->format.parse_val = regmap_parse_16_be;
 			map->format.parse_inplace = regmap_parse_16_be_inplace;
 			break;
+		case REGMAP_ENDIAN_LITTLE:
+			map->format.format_val = regmap_format_16_le;
+			map->format.parse_val = regmap_parse_16_le;
+			map->format.parse_inplace = regmap_parse_16_le_inplace;
+			break;
 		case REGMAP_ENDIAN_NATIVE:
 			map->format.format_val = regmap_format_16_native;
 			map->format.parse_val = regmap_parse_16_native;
@@ -629,6 +686,11 @@ struct regmap *regmap_init(struct device *dev,
 			map->format.parse_val = regmap_parse_32_be;
 			map->format.parse_inplace = regmap_parse_32_be_inplace;
 			break;
+		case REGMAP_ENDIAN_LITTLE:
+			map->format.format_val = regmap_format_32_le;
+			map->format.parse_val = regmap_parse_32_le;
+			map->format.parse_inplace = regmap_parse_32_le_inplace;
+			break;
 		case REGMAP_ENDIAN_NATIVE:
 			map->format.format_val = regmap_format_32_native;
 			map->format.parse_val = regmap_parse_32_native;
@@ -1284,6 +1346,14 @@ static int _regmap_bus_formatted_write(void *context, unsigned int reg,
 	return ret;
 }
 
+static int _regmap_bus_reg_write(void *context, unsigned int reg,
+				 unsigned int val)
+{
+	struct regmap *map = context;
+
+	return map->bus->reg_write(map->bus_context, reg, val);
+}
+
 static int _regmap_bus_raw_write(void *context, unsigned int reg,
 				 unsigned int val)
 {
@@ -1615,6 +1685,9 @@ static int _regmap_raw_multi_reg_write(struct regmap *map,
 	size_t pair_size = reg_bytes + pad_bytes + val_bytes;
 	size_t len = pair_size * num_regs;
 
+	if (!len)
+		return -EINVAL;
+
 	buf = kzalloc(len, GFP_KERNEL);
 	if (!buf)
 		return -ENOMEM;
@@ -1662,7 +1735,7 @@ static int _regmap_range_multi_paged_reg_write(struct regmap *map,
 	int ret;
 	int i, n;
 	struct reg_default *base;
-	unsigned int this_page;
+	unsigned int this_page = 0;
 	/*
 	 * the set of registers are not neccessarily in order, but
 	 * since the order of write must be preserved this algorithm
@@ -1925,6 +1998,14 @@ static int _regmap_raw_read(struct regmap *map, unsigned int reg, void *val,
 	return ret;
 }
 
+static int _regmap_bus_reg_read(void *context, unsigned int reg,
+				unsigned int *val)
+{
+	struct regmap *map = context;
+
+	return map->bus->reg_read(map->bus_context, reg, val);
+}
+
 static int _regmap_bus_read(void *context, unsigned int reg,
 			    unsigned int *val)
 {
diff --git a/drivers/base/topology.c b/drivers/base/topology.c
index bbcbd3c4392..be7c1fb7c0c 100644
--- a/drivers/base/topology.c
+++ b/drivers/base/topology.c
@@ -39,8 +39,7 @@
 static ssize_t show_##name(struct device *dev,			\
 		struct device_attribute *attr, char *buf)	\
 {								\
-	unsigned int cpu = dev->id;				\
-	return sprintf(buf, "%d\n", topology_##name(cpu));	\
+	return sprintf(buf, "%d\n", topology_##name(dev->id));	\
 }
 
 #if defined(topology_thread_cpumask) || defined(topology_core_cpumask) || \
diff --git a/drivers/block/amiflop.c b/drivers/block/amiflop.c
index 748dea4f34d..758da2287d9 100644
--- a/drivers/block/amiflop.c
+++ b/drivers/block/amiflop.c
@@ -1406,7 +1406,7 @@ next_segment:
 
 		track = block / (floppy->dtype->sects * floppy->type->sect_mult);
 		sector = block % (floppy->dtype->sects * floppy->type->sect_mult);
-		data = rq->buffer + 512 * cnt;
+		data = bio_data(rq->bio) + 512 * cnt;
 #ifdef DEBUG
 		printk("access to track %d, sector %d, with buffer at "
 		       "0x%08lx\n", track, sector, data);
diff --git a/drivers/block/ataflop.c b/drivers/block/ataflop.c
index 96b629e1f0c..2104b1b4ccd 100644
--- a/drivers/block/ataflop.c
+++ b/drivers/block/ataflop.c
@@ -1484,7 +1484,7 @@ repeat:
 	ReqCnt = 0;
 	ReqCmd = rq_data_dir(fd_request);
 	ReqBlock = blk_rq_pos(fd_request);
-	ReqBuffer = fd_request->buffer;
+	ReqBuffer = bio_data(fd_request->bio);
 	setup_req_params( drive );
 	do_fd_action( drive );
 
@@ -1952,7 +1952,7 @@ static int __init atari_floppy_init (void)
 		goto Enomem;
 	}
 	TrackBuffer = DMABuffer + 512;
-	PhysDMABuffer = virt_to_phys(DMABuffer);
+	PhysDMABuffer = atari_stram_to_phys(DMABuffer);
 	PhysTrackBuffer = virt_to_phys(TrackBuffer);
 	BufferDrive = BufferSide = BufferTrack = -1;
 
diff --git a/drivers/block/cciss.c b/drivers/block/cciss.c
index 73894ca3395..4595c22f33f 100644
--- a/drivers/block/cciss.c
+++ b/drivers/block/cciss.c
@@ -4080,7 +4080,7 @@ static void cciss_interrupt_mode(ctlr_info_t *h)
 		goto default_int_mode;
 
 	if (pci_find_capability(h->pdev, PCI_CAP_ID_MSIX)) {
-		err = pci_enable_msix(h->pdev, cciss_msix_entries, 4);
+		err = pci_enable_msix_exact(h->pdev, cciss_msix_entries, 4);
 		if (!err) {
 			h->intr[0] = cciss_msix_entries[0].vector;
 			h->intr[1] = cciss_msix_entries[1].vector;
@@ -4088,10 +4088,6 @@ static void cciss_interrupt_mode(ctlr_info_t *h)
 			h->intr[3] = cciss_msix_entries[3].vector;
 			h->msix_vector = 1;
 			return;
-		}
-		if (err > 0) {
-			dev_warn(&h->pdev->dev,
-				"only %d MSI-X vectors available\n", err);
 		} else {
 			dev_warn(&h->pdev->dev,
 				"MSI-X init failed %d\n", err);
diff --git a/drivers/block/drbd/drbd_actlog.c b/drivers/block/drbd/drbd_actlog.c
index 90ae4ba8f9e..05a1780ffa8 100644
--- a/drivers/block/drbd/drbd_actlog.c
+++ b/drivers/block/drbd/drbd_actlog.c
@@ -29,7 +29,6 @@
 #include <linux/drbd_limits.h>
 #include <linux/dynamic_debug.h>
 #include "drbd_int.h"
-#include "drbd_wrappers.h"
 
 
 enum al_transaction_types {
@@ -204,7 +203,7 @@ int drbd_md_sync_page_io(struct drbd_device *device, struct drbd_backing_dev *bd
 
 	BUG_ON(!bdev->md_bdev);
 
-	drbd_dbg(device, "meta_data io: %s [%d]:%s(,%llus,%s) %pS\n",
+	dynamic_drbd_dbg(device, "meta_data io: %s [%d]:%s(,%llus,%s) %pS\n",
 	     current->comm, current->pid, __func__,
 	     (unsigned long long)sector, (rw & WRITE) ? "WRITE" : "READ",
 	     (void*)_RET_IP_ );
@@ -276,7 +275,6 @@ bool drbd_al_begin_io_fastpath(struct drbd_device *device, struct drbd_interval
 	return _al_get(device, first, true);
 }
 
-static
 bool drbd_al_begin_io_prepare(struct drbd_device *device, struct drbd_interval *i)
 {
 	/* for bios crossing activity log extent boundaries,
@@ -846,7 +844,7 @@ void __drbd_set_in_sync(struct drbd_device *device, sector_t sector, int size,
 	int wake_up = 0;
 	unsigned long flags;
 
-	if (size <= 0 || !IS_ALIGNED(size, 512) || size > DRBD_MAX_BIO_SIZE) {
+	if (size <= 0 || !IS_ALIGNED(size, 512) || size > DRBD_MAX_DISCARD_SIZE) {
 		drbd_err(device, "drbd_set_in_sync: sector=%llus size=%d nonsense!\n",
 				(unsigned long long)sector, size);
 		return;
@@ -920,7 +918,7 @@ int __drbd_set_out_of_sync(struct drbd_device *device, sector_t sector, int size
 	if (size == 0)
 		return 0;
 
-	if (size < 0 || !IS_ALIGNED(size, 512) || size > DRBD_MAX_BIO_SIZE) {
+	if (size < 0 || !IS_ALIGNED(size, 512) || size > DRBD_MAX_DISCARD_SIZE) {
 		drbd_err(device, "sector: %llus, size: %d\n",
 			(unsigned long long)sector, size);
 		return 0;
@@ -1023,8 +1021,7 @@ int drbd_rs_begin_io(struct drbd_device *device, sector_t sector)
 	unsigned int enr = BM_SECT_TO_EXT(sector);
 	struct bm_extent *bm_ext;
 	int i, sig;
-	int sa = 200; /* Step aside 200 times, then grab the extent and let app-IO wait.
-			 200 times -> 20 seconds. */
+	bool sa;
 
 retry:
 	sig = wait_event_interruptible(device->al_wait,
@@ -1035,12 +1032,15 @@ retry:
 	if (test_bit(BME_LOCKED, &bm_ext->flags))
 		return 0;
 
+	/* step aside only while we are above c-min-rate; unless disabled. */
+	sa = drbd_rs_c_min_rate_throttle(device);
+
 	for (i = 0; i < AL_EXT_PER_BM_SECT; i++) {
 		sig = wait_event_interruptible(device->al_wait,
 					       !_is_in_al(device, enr * AL_EXT_PER_BM_SECT + i) ||
-					       test_bit(BME_PRIORITY, &bm_ext->flags));
+					       (sa && test_bit(BME_PRIORITY, &bm_ext->flags)));
 
-		if (sig || (test_bit(BME_PRIORITY, &bm_ext->flags) && sa)) {
+		if (sig || (sa && test_bit(BME_PRIORITY, &bm_ext->flags))) {
 			spin_lock_irq(&device->al_lock);
 			if (lc_put(device->resync, &bm_ext->lce) == 0) {
 				bm_ext->flags = 0; /* clears BME_NO_WRITES and eventually BME_PRIORITY */
@@ -1052,9 +1052,6 @@ retry:
 				return -EINTR;
 			if (schedule_timeout_interruptible(HZ/10))
 				return -EINTR;
-			if (sa && --sa == 0)
-				drbd_warn(device, "drbd_rs_begin_io() stepped aside for 20sec."
-					 "Resync stalled?\n");
 			goto retry;
 		}
 	}
@@ -1288,7 +1285,7 @@ void drbd_rs_failed_io(struct drbd_device *device, sector_t sector, int size)
 	sector_t esector, nr_sectors;
 	int wake_up = 0;
 
-	if (size <= 0 || !IS_ALIGNED(size, 512) || size > DRBD_MAX_BIO_SIZE) {
+	if (size <= 0 || !IS_ALIGNED(size, 512) || size > DRBD_MAX_DISCARD_SIZE) {
 		drbd_err(device, "drbd_rs_failed_io: sector=%llus size=%d nonsense!\n",
 				(unsigned long long)sector, size);
 		return;
diff --git a/drivers/block/drbd/drbd_int.h b/drivers/block/drbd/drbd_int.h
index e7093d4291f..a76ceb344d6 100644
--- a/drivers/block/drbd/drbd_int.h
+++ b/drivers/block/drbd/drbd_int.h
@@ -382,6 +382,12 @@ enum {
 	__EE_CALL_AL_COMPLETE_IO,
 	__EE_MAY_SET_IN_SYNC,
 
+	/* is this a TRIM aka REQ_DISCARD? */
+	__EE_IS_TRIM,
+	/* our lower level cannot handle trim,
+	 * and we want to fall back to zeroout instead */
+	__EE_IS_TRIM_USE_ZEROOUT,
+
 	/* In case a barrier failed,
 	 * we need to resubmit without the barrier flag. */
 	__EE_RESUBMITTED,
@@ -405,7 +411,9 @@ enum {
 };
 #define EE_CALL_AL_COMPLETE_IO (1<<__EE_CALL_AL_COMPLETE_IO)
 #define EE_MAY_SET_IN_SYNC     (1<<__EE_MAY_SET_IN_SYNC)
-#define	EE_RESUBMITTED         (1<<__EE_RESUBMITTED)
+#define EE_IS_TRIM             (1<<__EE_IS_TRIM)
+#define EE_IS_TRIM_USE_ZEROOUT (1<<__EE_IS_TRIM_USE_ZEROOUT)
+#define EE_RESUBMITTED         (1<<__EE_RESUBMITTED)
 #define EE_WAS_ERROR           (1<<__EE_WAS_ERROR)
 #define EE_HAS_DIGEST          (1<<__EE_HAS_DIGEST)
 #define EE_RESTART_REQUESTS	(1<<__EE_RESTART_REQUESTS)
@@ -579,6 +587,7 @@ struct drbd_resource {
 	struct list_head resources;
 	struct res_opts res_opts;
 	struct mutex conf_update;	/* mutex for ready-copy-update of net_conf and disk_conf */
+	struct mutex adm_mutex;		/* mutex to serialize administrative requests */
 	spinlock_t req_lock;
 
 	unsigned susp:1;		/* IO suspended by user */
@@ -609,6 +618,7 @@ struct drbd_connection {
 	struct drbd_socket data;	/* data/barrier/cstate/parameter packets */
 	struct drbd_socket meta;	/* ping/ack (metadata) packets */
 	int agreed_pro_version;		/* actually used protocol version */
+	u32 agreed_features;
 	unsigned long last_received;	/* in jiffies, either socket */
 	unsigned int ko_count;
 
@@ -814,6 +824,28 @@ struct drbd_device {
 	struct submit_worker submit;
 };
 
+struct drbd_config_context {
+	/* assigned from drbd_genlmsghdr */
+	unsigned int minor;
+	/* assigned from request attributes, if present */
+	unsigned int volume;
+#define VOLUME_UNSPECIFIED		(-1U)
+	/* pointer into the request skb,
+	 * limited lifetime! */
+	char *resource_name;
+	struct nlattr *my_addr;
+	struct nlattr *peer_addr;
+
+	/* reply buffer */
+	struct sk_buff *reply_skb;
+	/* pointer into reply buffer */
+	struct drbd_genlmsghdr *reply_dh;
+	/* resolved from attributes, if possible */
+	struct drbd_device *device;
+	struct drbd_resource *resource;
+	struct drbd_connection *connection;
+};
+
 static inline struct drbd_device *minor_to_device(unsigned int minor)
 {
 	return (struct drbd_device *)idr_find(&drbd_devices, minor);
@@ -821,7 +853,7 @@ static inline struct drbd_device *minor_to_device(unsigned int minor)
 
 static inline struct drbd_peer_device *first_peer_device(struct drbd_device *device)
 {
-	return list_first_entry(&device->peer_devices, struct drbd_peer_device, peer_devices);
+	return list_first_entry_or_null(&device->peer_devices, struct drbd_peer_device, peer_devices);
 }
 
 #define for_each_resource(resource, _resources) \
@@ -1139,6 +1171,12 @@ struct bm_extent {
 #define DRBD_MAX_SIZE_H80_PACKET (1U << 15) /* Header 80 only allows packets up to 32KiB data */
 #define DRBD_MAX_BIO_SIZE_P95    (1U << 17) /* Protocol 95 to 99 allows bios up to 128KiB */
 
+/* For now, don't allow more than one activity log extent worth of data
+ * to be discarded in one go. We may need to rework drbd_al_begin_io()
+ * to allow for even larger discard ranges */
+#define DRBD_MAX_DISCARD_SIZE	AL_EXTENT_SIZE
+#define DRBD_MAX_DISCARD_SECTORS (DRBD_MAX_DISCARD_SIZE >> 9)
+
 extern int  drbd_bm_init(struct drbd_device *device);
 extern int  drbd_bm_resize(struct drbd_device *device, sector_t sectors, int set_new_bits);
 extern void drbd_bm_cleanup(struct drbd_device *device);
@@ -1229,9 +1267,9 @@ extern struct bio *bio_alloc_drbd(gfp_t gfp_mask);
 extern rwlock_t global_state_lock;
 
 extern int conn_lowest_minor(struct drbd_connection *connection);
-enum drbd_ret_code drbd_create_device(struct drbd_resource *resource, unsigned int minor, int vnr);
+extern enum drbd_ret_code drbd_create_device(struct drbd_config_context *adm_ctx, unsigned int minor);
 extern void drbd_destroy_device(struct kref *kref);
-extern void drbd_delete_device(struct drbd_device *mdev);
+extern void drbd_delete_device(struct drbd_device *device);
 
 extern struct drbd_resource *drbd_create_resource(const char *name);
 extern void drbd_free_resource(struct drbd_resource *resource);
@@ -1257,7 +1295,7 @@ extern int is_valid_ar_handle(struct drbd_request *, sector_t);
 
 
 /* drbd_nl.c */
-extern int drbd_msg_put_info(const char *info);
+extern int drbd_msg_put_info(struct sk_buff *skb, const char *info);
 extern void drbd_suspend_io(struct drbd_device *device);
 extern void drbd_resume_io(struct drbd_device *device);
 extern char *ppsize(char *buf, unsigned long long size);
@@ -1283,6 +1321,10 @@ extern void conn_try_outdate_peer_async(struct drbd_connection *connection);
 extern int drbd_khelper(struct drbd_device *device, char *cmd);
 
 /* drbd_worker.c */
+/* bi_end_io handlers */
+extern void drbd_md_io_complete(struct bio *bio, int error);
+extern void drbd_peer_request_endio(struct bio *bio, int error);
+extern void drbd_request_endio(struct bio *bio, int error);
 extern int drbd_worker(struct drbd_thread *thi);
 enum drbd_ret_code drbd_resync_after_valid(struct drbd_device *device, int o_minor);
 void drbd_resync_after_changed(struct drbd_device *device);
@@ -1332,16 +1374,20 @@ extern int w_start_resync(struct drbd_work *, int);
 extern void resync_timer_fn(unsigned long data);
 extern void start_resync_timer_fn(unsigned long data);
 
+extern void drbd_endio_write_sec_final(struct drbd_peer_request *peer_req);
+
 /* drbd_receiver.c */
 extern int drbd_receiver(struct drbd_thread *thi);
 extern int drbd_asender(struct drbd_thread *thi);
-extern int drbd_rs_should_slow_down(struct drbd_device *device, sector_t sector);
+extern bool drbd_rs_c_min_rate_throttle(struct drbd_device *device);
+extern bool drbd_rs_should_slow_down(struct drbd_device *device, sector_t sector);
 extern int drbd_submit_peer_request(struct drbd_device *,
 				    struct drbd_peer_request *, const unsigned,
 				    const int);
 extern int drbd_free_peer_reqs(struct drbd_device *, struct list_head *);
 extern struct drbd_peer_request *drbd_alloc_peer_req(struct drbd_peer_device *, u64,
 						     sector_t, unsigned int,
+						     bool,
 						     gfp_t) __must_hold(local);
 extern void __drbd_free_peer_req(struct drbd_device *, struct drbd_peer_request *,
 				 int);
@@ -1401,6 +1447,37 @@ static inline void drbd_tcp_quickack(struct socket *sock)
 			(char*)&val, sizeof(val));
 }
 
+/* sets the number of 512 byte sectors of our virtual device */
+static inline void drbd_set_my_capacity(struct drbd_device *device,
+					sector_t size)
+{
+	/* set_capacity(device->this_bdev->bd_disk, size); */
+	set_capacity(device->vdisk, size);
+	device->this_bdev->bd_inode->i_size = (loff_t)size << 9;
+}
+
+/*
+ * used to submit our private bio
+ */
+static inline void drbd_generic_make_request(struct drbd_device *device,
+					     int fault_type, struct bio *bio)
+{
+	__release(local);
+	if (!bio->bi_bdev) {
+		printk(KERN_ERR "drbd%d: drbd_generic_make_request: "
+				"bio->bi_bdev == NULL\n",
+		       device_to_minor(device));
+		dump_stack();
+		bio_endio(bio, -ENODEV);
+		return;
+	}
+
+	if (drbd_insert_fault(device, fault_type))
+		bio_endio(bio, -EIO);
+	else
+		generic_make_request(bio);
+}
+
 void drbd_bump_write_ordering(struct drbd_connection *connection, enum write_ordering_e wo);
 
 /* drbd_proc.c */
@@ -1410,6 +1487,7 @@ extern const char *drbd_conn_str(enum drbd_conns s);
 extern const char *drbd_role_str(enum drbd_role s);
 
 /* drbd_actlog.c */
+extern bool drbd_al_begin_io_prepare(struct drbd_device *device, struct drbd_interval *i);
 extern int drbd_al_begin_io_nonblock(struct drbd_device *device, struct drbd_interval *i);
 extern void drbd_al_begin_io_commit(struct drbd_device *device, bool delegate);
 extern bool drbd_al_begin_io_fastpath(struct drbd_device *device, struct drbd_interval *i);
@@ -2144,7 +2222,7 @@ static inline void drbd_md_flush(struct drbd_device *device)
 
 static inline struct drbd_connection *first_connection(struct drbd_resource *resource)
 {
-	return list_first_entry(&resource->connections,
+	return list_first_entry_or_null(&resource->connections,
 				struct drbd_connection, connections);
 }
 
diff --git a/drivers/block/drbd/drbd_main.c b/drivers/block/drbd/drbd_main.c
index 331e5cc1227..960645c26e6 100644
--- a/drivers/block/drbd/drbd_main.c
+++ b/drivers/block/drbd/drbd_main.c
@@ -1607,8 +1607,8 @@ static u32 bio_flags_to_wire(struct drbd_connection *connection, unsigned long b
 		return bi_rw & REQ_SYNC ? DP_RW_SYNC : 0;
 }
 
-/* Used to send write requests
- * R_PRIMARY -> Peer	(P_DATA)
+/* Used to send write or TRIM aka REQ_DISCARD requests
+ * R_PRIMARY -> Peer	(P_DATA, P_TRIM)
  */
 int drbd_send_dblock(struct drbd_peer_device *peer_device, struct drbd_request *req)
 {
@@ -1640,6 +1640,16 @@ int drbd_send_dblock(struct drbd_peer_device *peer_device, struct drbd_request *
 			dp_flags |= DP_SEND_WRITE_ACK;
 	}
 	p->dp_flags = cpu_to_be32(dp_flags);
+
+	if (dp_flags & DP_DISCARD) {
+		struct p_trim *t = (struct p_trim*)p;
+		t->size = cpu_to_be32(req->i.size);
+		err = __send_command(peer_device->connection, device->vnr, sock, P_TRIM, sizeof(*t), NULL, 0);
+		goto out;
+	}
+
+	/* our digest is still only over the payload.
+	 * TRIM does not carry any payload. */
 	if (dgs)
 		drbd_csum_bio(peer_device->connection->integrity_tfm, req->master_bio, p + 1);
 	err = __send_command(peer_device->connection, device->vnr, sock, P_DATA, sizeof(*p) + dgs, NULL, req->i.size);
@@ -1675,6 +1685,7 @@ int drbd_send_dblock(struct drbd_peer_device *peer_device, struct drbd_request *
 		     ... Be noisy about digest too large ...
 		} */
 	}
+out:
 	mutex_unlock(&sock->mutex);  /* locked by drbd_prepare_command() */
 
 	return err;
@@ -2570,6 +2581,7 @@ struct drbd_resource *drbd_create_resource(const char *name)
 	INIT_LIST_HEAD(&resource->connections);
 	list_add_tail_rcu(&resource->resources, &drbd_resources);
 	mutex_init(&resource->conf_update);
+	mutex_init(&resource->adm_mutex);
 	spin_lock_init(&resource->req_lock);
 	return resource;
 
@@ -2687,14 +2699,16 @@ static int init_submitter(struct drbd_device *device)
 	return 0;
 }
 
-enum drbd_ret_code drbd_create_device(struct drbd_resource *resource, unsigned int minor, int vnr)
+enum drbd_ret_code drbd_create_device(struct drbd_config_context *adm_ctx, unsigned int minor)
 {
+	struct drbd_resource *resource = adm_ctx->resource;
 	struct drbd_connection *connection;
 	struct drbd_device *device;
 	struct drbd_peer_device *peer_device, *tmp_peer_device;
 	struct gendisk *disk;
 	struct request_queue *q;
 	int id;
+	int vnr = adm_ctx->volume;
 	enum drbd_ret_code err = ERR_NOMEM;
 
 	device = minor_to_device(minor);
@@ -2763,7 +2777,7 @@ enum drbd_ret_code drbd_create_device(struct drbd_resource *resource, unsigned i
 	if (id < 0) {
 		if (id == -ENOSPC) {
 			err = ERR_MINOR_EXISTS;
-			drbd_msg_put_info("requested minor exists already");
+			drbd_msg_put_info(adm_ctx->reply_skb, "requested minor exists already");
 		}
 		goto out_no_minor_idr;
 	}
@@ -2773,7 +2787,7 @@ enum drbd_ret_code drbd_create_device(struct drbd_resource *resource, unsigned i
 	if (id < 0) {
 		if (id == -ENOSPC) {
 			err = ERR_MINOR_EXISTS;
-			drbd_msg_put_info("requested minor exists already");
+			drbd_msg_put_info(adm_ctx->reply_skb, "requested minor exists already");
 		}
 		goto out_idr_remove_minor;
 	}
@@ -2794,7 +2808,7 @@ enum drbd_ret_code drbd_create_device(struct drbd_resource *resource, unsigned i
 		if (id < 0) {
 			if (id == -ENOSPC) {
 				err = ERR_INVALID_REQUEST;
-				drbd_msg_put_info("requested volume exists already");
+				drbd_msg_put_info(adm_ctx->reply_skb, "requested volume exists already");
 			}
 			goto out_idr_remove_from_resource;
 		}
@@ -2803,7 +2817,7 @@ enum drbd_ret_code drbd_create_device(struct drbd_resource *resource, unsigned i
 
 	if (init_submitter(device)) {
 		err = ERR_NOMEM;
-		drbd_msg_put_info("unable to create submit workqueue");
+		drbd_msg_put_info(adm_ctx->reply_skb, "unable to create submit workqueue");
 		goto out_idr_remove_vol;
 	}
 
diff --git a/drivers/block/drbd/drbd_nl.c b/drivers/block/drbd/drbd_nl.c
index 526414bc2ca..1b35c45c92b 100644
--- a/drivers/block/drbd/drbd_nl.c
+++ b/drivers/block/drbd/drbd_nl.c
@@ -34,7 +34,6 @@
 #include "drbd_int.h"
 #include "drbd_protocol.h"
 #include "drbd_req.h"
-#include "drbd_wrappers.h"
 #include <asm/unaligned.h>
 #include <linux/drbd_limits.h>
 #include <linux/kthread.h>
@@ -82,32 +81,6 @@ int drbd_adm_get_status_all(struct sk_buff *skb, struct netlink_callback *cb);
 /* used blkdev_get_by_path, to claim our meta data device(s) */
 static char *drbd_m_holder = "Hands off! this is DRBD's meta data device.";
 
-/* Configuration is strictly serialized, because generic netlink message
- * processing is strictly serialized by the genl_lock().
- * Which means we can use one static global drbd_config_context struct.
- */
-static struct drbd_config_context {
-	/* assigned from drbd_genlmsghdr */
-	unsigned int minor;
-	/* assigned from request attributes, if present */
-	unsigned int volume;
-#define VOLUME_UNSPECIFIED		(-1U)
-	/* pointer into the request skb,
-	 * limited lifetime! */
-	char *resource_name;
-	struct nlattr *my_addr;
-	struct nlattr *peer_addr;
-
-	/* reply buffer */
-	struct sk_buff *reply_skb;
-	/* pointer into reply buffer */
-	struct drbd_genlmsghdr *reply_dh;
-	/* resolved from attributes, if possible */
-	struct drbd_device *device;
-	struct drbd_resource *resource;
-	struct drbd_connection *connection;
-} adm_ctx;
-
 static void drbd_adm_send_reply(struct sk_buff *skb, struct genl_info *info)
 {
 	genlmsg_end(skb, genlmsg_data(nlmsg_data(nlmsg_hdr(skb))));
@@ -117,9 +90,8 @@ static void drbd_adm_send_reply(struct sk_buff *skb, struct genl_info *info)
 
 /* Used on a fresh "drbd_adm_prepare"d reply_skb, this cannot fail: The only
  * reason it could fail was no space in skb, and there are 4k available. */
-int drbd_msg_put_info(const char *info)
+int drbd_msg_put_info(struct sk_buff *skb, const char *info)
 {
-	struct sk_buff *skb = adm_ctx.reply_skb;
 	struct nlattr *nla;
 	int err = -EMSGSIZE;
 
@@ -143,42 +115,46 @@ int drbd_msg_put_info(const char *info)
  * and per-family private info->pointers.
  * But we need to stay compatible with older kernels.
  * If it returns successfully, adm_ctx members are valid.
+ *
+ * At this point, we still rely on the global genl_lock().
+ * If we want to avoid that, and allow "genl_family.parallel_ops", we may need
+ * to add additional synchronization against object destruction/modification.
  */
 #define DRBD_ADM_NEED_MINOR	1
 #define DRBD_ADM_NEED_RESOURCE	2
 #define DRBD_ADM_NEED_CONNECTION 4
-static int drbd_adm_prepare(struct sk_buff *skb, struct genl_info *info,
-		unsigned flags)
+static int drbd_adm_prepare(struct drbd_config_context *adm_ctx,
+	struct sk_buff *skb, struct genl_info *info, unsigned flags)
 {
 	struct drbd_genlmsghdr *d_in = info->userhdr;
 	const u8 cmd = info->genlhdr->cmd;
 	int err;
 
-	memset(&adm_ctx, 0, sizeof(adm_ctx));
+	memset(adm_ctx, 0, sizeof(*adm_ctx));
 
 	/* genl_rcv_msg only checks for CAP_NET_ADMIN on "GENL_ADMIN_PERM" :( */
 	if (cmd != DRBD_ADM_GET_STATUS && !capable(CAP_NET_ADMIN))
 	       return -EPERM;
 
-	adm_ctx.reply_skb = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
-	if (!adm_ctx.reply_skb) {
+	adm_ctx->reply_skb = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
+	if (!adm_ctx->reply_skb) {
 		err = -ENOMEM;
 		goto fail;
 	}
 
-	adm_ctx.reply_dh = genlmsg_put_reply(adm_ctx.reply_skb,
+	adm_ctx->reply_dh = genlmsg_put_reply(adm_ctx->reply_skb,
 					info, &drbd_genl_family, 0, cmd);
 	/* put of a few bytes into a fresh skb of >= 4k will always succeed.
 	 * but anyways */
-	if (!adm_ctx.reply_dh) {
+	if (!adm_ctx->reply_dh) {
 		err = -ENOMEM;
 		goto fail;
 	}
 
-	adm_ctx.reply_dh->minor = d_in->minor;
-	adm_ctx.reply_dh->ret_code = NO_ERROR;
+	adm_ctx->reply_dh->minor = d_in->minor;
+	adm_ctx->reply_dh->ret_code = NO_ERROR;
 
-	adm_ctx.volume = VOLUME_UNSPECIFIED;
+	adm_ctx->volume = VOLUME_UNSPECIFIED;
 	if (info->attrs[DRBD_NLA_CFG_CONTEXT]) {
 		struct nlattr *nla;
 		/* parse and validate only */
@@ -188,111 +164,131 @@ static int drbd_adm_prepare(struct sk_buff *skb, struct genl_info *info,
 
 		/* It was present, and valid,
 		 * copy it over to the reply skb. */
-		err = nla_put_nohdr(adm_ctx.reply_skb,
+		err = nla_put_nohdr(adm_ctx->reply_skb,
 				info->attrs[DRBD_NLA_CFG_CONTEXT]->nla_len,
 				info->attrs[DRBD_NLA_CFG_CONTEXT]);
 		if (err)
 			goto fail;
 
-		/* and assign stuff to the global adm_ctx */
+		/* and assign stuff to the adm_ctx */
 		nla = nested_attr_tb[__nla_type(T_ctx_volume)];
 		if (nla)
-			adm_ctx.volume = nla_get_u32(nla);
+			adm_ctx->volume = nla_get_u32(nla);
 		nla = nested_attr_tb[__nla_type(T_ctx_resource_name)];
 		if (nla)
-			adm_ctx.resource_name = nla_data(nla);
-		adm_ctx.my_addr = nested_attr_tb[__nla_type(T_ctx_my_addr)];
-		adm_ctx.peer_addr = nested_attr_tb[__nla_type(T_ctx_peer_addr)];
-		if ((adm_ctx.my_addr &&
-		     nla_len(adm_ctx.my_addr) > sizeof(adm_ctx.connection->my_addr)) ||
-		    (adm_ctx.peer_addr &&
-		     nla_len(adm_ctx.peer_addr) > sizeof(adm_ctx.connection->peer_addr))) {
+			adm_ctx->resource_name = nla_data(nla);
+		adm_ctx->my_addr = nested_attr_tb[__nla_type(T_ctx_my_addr)];
+		adm_ctx->peer_addr = nested_attr_tb[__nla_type(T_ctx_peer_addr)];
+		if ((adm_ctx->my_addr &&
+		     nla_len(adm_ctx->my_addr) > sizeof(adm_ctx->connection->my_addr)) ||
+		    (adm_ctx->peer_addr &&
+		     nla_len(adm_ctx->peer_addr) > sizeof(adm_ctx->connection->peer_addr))) {
 			err = -EINVAL;
 			goto fail;
 		}
 	}
 
-	adm_ctx.minor = d_in->minor;
-	adm_ctx.device = minor_to_device(d_in->minor);
-	if (adm_ctx.resource_name) {
-		adm_ctx.resource = drbd_find_resource(adm_ctx.resource_name);
+	adm_ctx->minor = d_in->minor;
+	adm_ctx->device = minor_to_device(d_in->minor);
+
+	/* We are protected by the global genl_lock().
+	 * But we may explicitly drop it/retake it in drbd_adm_set_role(),
+	 * so make sure this object stays around. */
+	if (adm_ctx->device)
+		kref_get(&adm_ctx->device->kref);
+
+	if (adm_ctx->resource_name) {
+		adm_ctx->resource = drbd_find_resource(adm_ctx->resource_name);
 	}
 
-	if (!adm_ctx.device && (flags & DRBD_ADM_NEED_MINOR)) {
-		drbd_msg_put_info("unknown minor");
+	if (!adm_ctx->device && (flags & DRBD_ADM_NEED_MINOR)) {
+		drbd_msg_put_info(adm_ctx->reply_skb, "unknown minor");
 		return ERR_MINOR_INVALID;
 	}
-	if (!adm_ctx.resource && (flags & DRBD_ADM_NEED_RESOURCE)) {
-		drbd_msg_put_info("unknown resource");
-		if (adm_ctx.resource_name)
+	if (!adm_ctx->resource && (flags & DRBD_ADM_NEED_RESOURCE)) {
+		drbd_msg_put_info(adm_ctx->reply_skb, "unknown resource");
+		if (adm_ctx->resource_name)
 			return ERR_RES_NOT_KNOWN;
 		return ERR_INVALID_REQUEST;
 	}
 
 	if (flags & DRBD_ADM_NEED_CONNECTION) {
-		if (adm_ctx.resource) {
-			drbd_msg_put_info("no resource name expected");
+		if (adm_ctx->resource) {
+			drbd_msg_put_info(adm_ctx->reply_skb, "no resource name expected");
 			return ERR_INVALID_REQUEST;
 		}
-		if (adm_ctx.device) {
-			drbd_msg_put_info("no minor number expected");
+		if (adm_ctx->device) {
+			drbd_msg_put_info(adm_ctx->reply_skb, "no minor number expected");
 			return ERR_INVALID_REQUEST;
 		}
-		if (adm_ctx.my_addr && adm_ctx.peer_addr)
-			adm_ctx.connection = conn_get_by_addrs(nla_data(adm_ctx.my_addr),
-							  nla_len(adm_ctx.my_addr),
-							  nla_data(adm_ctx.peer_addr),
-							  nla_len(adm_ctx.peer_addr));
-		if (!adm_ctx.connection) {
-			drbd_msg_put_info("unknown connection");
+		if (adm_ctx->my_addr && adm_ctx->peer_addr)
+			adm_ctx->connection = conn_get_by_addrs(nla_data(adm_ctx->my_addr),
+							  nla_len(adm_ctx->my_addr),
+							  nla_data(adm_ctx->peer_addr),
+							  nla_len(adm_ctx->peer_addr));
+		if (!adm_ctx->connection) {
+			drbd_msg_put_info(adm_ctx->reply_skb, "unknown connection");
 			return ERR_INVALID_REQUEST;
 		}
 	}
 
 	/* some more paranoia, if the request was over-determined */
-	if (adm_ctx.device && adm_ctx.resource &&
-	    adm_ctx.device->resource != adm_ctx.resource) {
+	if (adm_ctx->device && adm_ctx->resource &&
+	    adm_ctx->device->resource != adm_ctx->resource) {
 		pr_warning("request: minor=%u, resource=%s; but that minor belongs to resource %s\n",
-				adm_ctx.minor, adm_ctx.resource->name,
-				adm_ctx.device->resource->name);
-		drbd_msg_put_info("minor exists in different resource");
+				adm_ctx->minor, adm_ctx->resource->name,
+				adm_ctx->device->resource->name);
+		drbd_msg_put_info(adm_ctx->reply_skb, "minor exists in different resource");
 		return ERR_INVALID_REQUEST;
 	}
-	if (adm_ctx.device &&
-	    adm_ctx.volume != VOLUME_UNSPECIFIED &&
-	    adm_ctx.volume != adm_ctx.device->vnr) {
+	if (adm_ctx->device &&
+	    adm_ctx->volume != VOLUME_UNSPECIFIED &&
+	    adm_ctx->volume != adm_ctx->device->vnr) {
 		pr_warning("request: minor=%u, volume=%u; but that minor is volume %u in %s\n",
-				adm_ctx.minor, adm_ctx.volume,
-				adm_ctx.device->vnr,
-				adm_ctx.device->resource->name);
-		drbd_msg_put_info("minor exists as different volume");
+				adm_ctx->minor, adm_ctx->volume,
+				adm_ctx->device->vnr,
+				adm_ctx->device->resource->name);
+		drbd_msg_put_info(adm_ctx->reply_skb, "minor exists as different volume");
 		return ERR_INVALID_REQUEST;
 	}
 
+	/* still, provide adm_ctx->resource always, if possible. */
+	if (!adm_ctx->resource) {
+		adm_ctx->resource = adm_ctx->device ? adm_ctx->device->resource
+			: adm_ctx->connection ? adm_ctx->connection->resource : NULL;
+		if (adm_ctx->resource)
+			kref_get(&adm_ctx->resource->kref);
+	}
+
 	return NO_ERROR;
 
 fail:
-	nlmsg_free(adm_ctx.reply_skb);
-	adm_ctx.reply_skb = NULL;
+	nlmsg_free(adm_ctx->reply_skb);
+	adm_ctx->reply_skb = NULL;
 	return err;
 }
 
-static int drbd_adm_finish(struct genl_info *info, int retcode)
+static int drbd_adm_finish(struct drbd_config_context *adm_ctx,
+	struct genl_info *info, int retcode)
 {
-	if (adm_ctx.connection) {
-		kref_put(&adm_ctx.connection->kref, drbd_destroy_connection);
-		adm_ctx.connection = NULL;
+	if (adm_ctx->device) {
+		kref_put(&adm_ctx->device->kref, drbd_destroy_device);
+		adm_ctx->device = NULL;
 	}
-	if (adm_ctx.resource) {
-		kref_put(&adm_ctx.resource->kref, drbd_destroy_resource);
-		adm_ctx.resource = NULL;
+	if (adm_ctx->connection) {
+		kref_put(&adm_ctx->connection->kref, &drbd_destroy_connection);
+		adm_ctx->connection = NULL;
+	}
+	if (adm_ctx->resource) {
+		kref_put(&adm_ctx->resource->kref, drbd_destroy_resource);
+		adm_ctx->resource = NULL;
 	}
 
-	if (!adm_ctx.reply_skb)
+	if (!adm_ctx->reply_skb)
 		return -ENOMEM;
 
-	adm_ctx.reply_dh->ret_code = retcode;
-	drbd_adm_send_reply(adm_ctx.reply_skb, info);
+	adm_ctx->reply_dh->ret_code = retcode;
+	drbd_adm_send_reply(adm_ctx->reply_skb, info);
 	return 0;
 }
 
@@ -426,6 +422,14 @@ static enum drbd_fencing_p highest_fencing_policy(struct drbd_connection *connec
 	}
 	rcu_read_unlock();
 
+	if (fp == FP_NOT_AVAIL) {
+		/* IO Suspending works on the whole resource.
+		   Do it only for one device. */
+		vnr = 0;
+		peer_device = idr_get_next(&connection->peer_devices, &vnr);
+		drbd_change_state(peer_device->device, CS_VERBOSE | CS_HARD, NS(susp_fen, 0));
+	}
+
 	return fp;
 }
 
@@ -438,12 +442,13 @@ bool conn_try_outdate_peer(struct drbd_connection *connection)
 	char *ex_to_string;
 	int r;
 
+	spin_lock_irq(&connection->resource->req_lock);
 	if (connection->cstate >= C_WF_REPORT_PARAMS) {
 		drbd_err(connection, "Expected cstate < C_WF_REPORT_PARAMS\n");
+		spin_unlock_irq(&connection->resource->req_lock);
 		return false;
 	}
 
-	spin_lock_irq(&connection->resource->req_lock);
 	connect_cnt = connection->connect_cnt;
 	spin_unlock_irq(&connection->resource->req_lock);
 
@@ -654,11 +659,11 @@ drbd_set_role(struct drbd_device *device, enum drbd_role new_role, int force)
 			put_ldev(device);
 		}
 	} else {
-		mutex_lock(&device->resource->conf_update);
+		/* Called from drbd_adm_set_role only.
+		 * We are still holding the conf_update mutex. */
 		nc = first_peer_device(device)->connection->net_conf;
 		if (nc)
 			nc->discard_my_data = 0; /* without copy; single bit op is atomic */
-		mutex_unlock(&device->resource->conf_update);
 
 		set_disk_ro(device->vdisk, false);
 		if (get_ldev(device)) {
@@ -700,11 +705,12 @@ static const char *from_attrs_err_to_txt(int err)
 
 int drbd_adm_set_role(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	struct set_role_parms parms;
 	int err;
 	enum drbd_ret_code retcode;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
@@ -715,17 +721,22 @@ int drbd_adm_set_role(struct sk_buff *skb, struct genl_info *info)
 		err = set_role_parms_from_attrs(&parms, info);
 		if (err) {
 			retcode = ERR_MANDATORY_TAG;
-			drbd_msg_put_info(from_attrs_err_to_txt(err));
+			drbd_msg_put_info(adm_ctx.reply_skb, from_attrs_err_to_txt(err));
 			goto out;
 		}
 	}
+	genl_unlock();
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 
 	if (info->genlhdr->cmd == DRBD_ADM_PRIMARY)
 		retcode = drbd_set_role(adm_ctx.device, R_PRIMARY, parms.assume_uptodate);
 	else
 		retcode = drbd_set_role(adm_ctx.device, R_SECONDARY, 0);
+
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
+	genl_lock();
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
@@ -1104,15 +1115,18 @@ static void drbd_setup_queue_param(struct drbd_device *device, unsigned int max_
 	struct request_queue * const q = device->rq_queue;
 	unsigned int max_hw_sectors = max_bio_size >> 9;
 	unsigned int max_segments = 0;
+	struct request_queue *b = NULL;
 
 	if (get_ldev_if_state(device, D_ATTACHING)) {
-		struct request_queue * const b = device->ldev->backing_bdev->bd_disk->queue;
+		b = device->ldev->backing_bdev->bd_disk->queue;
 
 		max_hw_sectors = min(queue_max_hw_sectors(b), max_bio_size >> 9);
 		rcu_read_lock();
 		max_segments = rcu_dereference(device->ldev->disk_conf)->max_bio_bvecs;
 		rcu_read_unlock();
-		put_ldev(device);
+
+		blk_set_stacking_limits(&q->limits);
+		blk_queue_max_write_same_sectors(q, 0);
 	}
 
 	blk_queue_logical_block_size(q, 512);
@@ -1121,8 +1135,25 @@ static void drbd_setup_queue_param(struct drbd_device *device, unsigned int max_
 	blk_queue_max_segments(q, max_segments ? max_segments : BLK_MAX_SEGMENTS);
 	blk_queue_segment_boundary(q, PAGE_CACHE_SIZE-1);
 
-	if (get_ldev_if_state(device, D_ATTACHING)) {
-		struct request_queue * const b = device->ldev->backing_bdev->bd_disk->queue;
+	if (b) {
+		struct drbd_connection *connection = first_peer_device(device)->connection;
+
+		if (blk_queue_discard(b) &&
+		    (connection->cstate < C_CONNECTED || connection->agreed_features & FF_TRIM)) {
+			/* For now, don't allow more than one activity log extent worth of data
+			 * to be discarded in one go. We may need to rework drbd_al_begin_io()
+			 * to allow for even larger discard ranges */
+			q->limits.max_discard_sectors = DRBD_MAX_DISCARD_SECTORS;
+
+			queue_flag_set_unlocked(QUEUE_FLAG_DISCARD, q);
+			/* REALLY? Is stacking secdiscard "legal"? */
+			if (blk_queue_secdiscard(b))
+				queue_flag_set_unlocked(QUEUE_FLAG_SECDISCARD, q);
+		} else {
+			q->limits.max_discard_sectors = 0;
+			queue_flag_clear_unlocked(QUEUE_FLAG_DISCARD, q);
+			queue_flag_clear_unlocked(QUEUE_FLAG_SECDISCARD, q);
+		}
 
 		blk_queue_stack_limits(q, b);
 
@@ -1164,8 +1195,14 @@ void drbd_reconsider_max_bio_size(struct drbd_device *device)
 			peer = DRBD_MAX_BIO_SIZE_P95;  /* drbd 8.3.8 onwards, before 8.4.0 */
 		else
 			peer = DRBD_MAX_BIO_SIZE;
-	}
 
+		/* We may later detach and re-attach on a disconnected Primary.
+		 * Avoid this setting to jump back in that case.
+		 * We want to store what we know the peer DRBD can handle,
+		 * not what the peer IO backend can handle. */
+		if (peer > device->peer_max_bio_size)
+			device->peer_max_bio_size = peer;
+	}
 	new = min(local, peer);
 
 	if (device->state.role == R_PRIMARY && new < now)
@@ -1258,19 +1295,21 @@ static unsigned int drbd_al_extents_max(struct drbd_backing_dev *bdev)
 
 int drbd_adm_disk_opts(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	enum drbd_ret_code retcode;
 	struct drbd_device *device;
 	struct disk_conf *new_disk_conf, *old_disk_conf;
 	struct fifo_buffer *old_plan = NULL, *new_plan = NULL;
 	int err, fifo_size;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
-		goto out;
+		goto finish;
 
 	device = adm_ctx.device;
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 
 	/* we also need a disk
 	 * to change the options on */
@@ -1294,7 +1333,7 @@ int drbd_adm_disk_opts(struct sk_buff *skb, struct genl_info *info)
 	err = disk_conf_from_attrs_for_change(new_disk_conf, info);
 	if (err && err != -ENOMSG) {
 		retcode = ERR_MANDATORY_TAG;
-		drbd_msg_put_info(from_attrs_err_to_txt(err));
+		drbd_msg_put_info(adm_ctx.reply_skb, from_attrs_err_to_txt(err));
 		goto fail_unlock;
 	}
 
@@ -1385,12 +1424,15 @@ fail_unlock:
 success:
 	put_ldev(device);
  out:
-	drbd_adm_finish(info, retcode);
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
+ finish:
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
 int drbd_adm_attach(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	struct drbd_device *device;
 	int err;
 	enum drbd_ret_code retcode;
@@ -1406,13 +1448,14 @@ int drbd_adm_attach(struct sk_buff *skb, struct genl_info *info)
 	enum drbd_state_rv rv;
 	struct net_conf *nc;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
 		goto finish;
 
 	device = adm_ctx.device;
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 	conn_reconfig_start(first_peer_device(device)->connection);
 
 	/* if you want to reconfigure, please tear down first */
@@ -1455,7 +1498,7 @@ int drbd_adm_attach(struct sk_buff *skb, struct genl_info *info)
 	err = disk_conf_from_attrs(new_disk_conf, info);
 	if (err) {
 		retcode = ERR_MANDATORY_TAG;
-		drbd_msg_put_info(from_attrs_err_to_txt(err));
+		drbd_msg_put_info(adm_ctx.reply_skb, from_attrs_err_to_txt(err));
 		goto fail;
 	}
 
@@ -1619,7 +1662,7 @@ int drbd_adm_attach(struct sk_buff *skb, struct genl_info *info)
 	}
 
 	if (device->state.conn < C_CONNECTED &&
-	    device->state.role == R_PRIMARY &&
+	    device->state.role == R_PRIMARY && device->ed_uuid &&
 	    (device->ed_uuid & ~((u64)1)) != (nbc->md.uuid[UI_CURRENT] & ~((u64)1))) {
 		drbd_err(device, "Can only attach to data with current UUID=%016llX\n",
 		    (unsigned long long)device->ed_uuid);
@@ -1797,7 +1840,8 @@ int drbd_adm_attach(struct sk_buff *skb, struct genl_info *info)
 	kobject_uevent(&disk_to_dev(device->vdisk)->kobj, KOBJ_CHANGE);
 	put_ldev(device);
 	conn_reconfig_done(first_peer_device(device)->connection);
-	drbd_adm_finish(info, retcode);
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 
  force_diskless_dec:
@@ -1819,9 +1863,9 @@ int drbd_adm_attach(struct sk_buff *skb, struct genl_info *info)
 	kfree(new_disk_conf);
 	lc_destroy(resync_lru);
 	kfree(new_plan);
-
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
  finish:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
@@ -1860,11 +1904,12 @@ out:
  * Only then we have finally detached. */
 int drbd_adm_detach(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	enum drbd_ret_code retcode;
 	struct detach_parms parms = { };
 	int err;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
@@ -1874,14 +1919,16 @@ int drbd_adm_detach(struct sk_buff *skb, struct genl_info *info)
 		err = detach_parms_from_attrs(&parms, info);
 		if (err) {
 			retcode = ERR_MANDATORY_TAG;
-			drbd_msg_put_info(from_attrs_err_to_txt(err));
+			drbd_msg_put_info(adm_ctx.reply_skb, from_attrs_err_to_txt(err));
 			goto out;
 		}
 	}
 
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 	retcode = adm_detach(adm_ctx.device, parms.force_detach);
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
@@ -2055,6 +2102,7 @@ static void free_crypto(struct crypto *crypto)
 
 int drbd_adm_net_opts(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	enum drbd_ret_code retcode;
 	struct drbd_connection *connection;
 	struct net_conf *old_net_conf, *new_net_conf = NULL;
@@ -2063,13 +2111,14 @@ int drbd_adm_net_opts(struct sk_buff *skb, struct genl_info *info)
 	int rsr; /* re-sync running */
 	struct crypto crypto = { };
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_CONNECTION);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_CONNECTION);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
-		goto out;
+		goto finish;
 
 	connection = adm_ctx.connection;
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 
 	new_net_conf = kzalloc(sizeof(struct net_conf), GFP_KERNEL);
 	if (!new_net_conf) {
@@ -2084,7 +2133,7 @@ int drbd_adm_net_opts(struct sk_buff *skb, struct genl_info *info)
 	old_net_conf = connection->net_conf;
 
 	if (!old_net_conf) {
-		drbd_msg_put_info("net conf missing, try connect");
+		drbd_msg_put_info(adm_ctx.reply_skb, "net conf missing, try connect");
 		retcode = ERR_INVALID_REQUEST;
 		goto fail;
 	}
@@ -2096,7 +2145,7 @@ int drbd_adm_net_opts(struct sk_buff *skb, struct genl_info *info)
 	err = net_conf_from_attrs_for_change(new_net_conf, info);
 	if (err && err != -ENOMSG) {
 		retcode = ERR_MANDATORY_TAG;
-		drbd_msg_put_info(from_attrs_err_to_txt(err));
+		drbd_msg_put_info(adm_ctx.reply_skb, from_attrs_err_to_txt(err));
 		goto fail;
 	}
 
@@ -2167,12 +2216,15 @@ int drbd_adm_net_opts(struct sk_buff *skb, struct genl_info *info)
  done:
 	conn_reconfig_done(connection);
  out:
-	drbd_adm_finish(info, retcode);
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
+ finish:
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
 int drbd_adm_connect(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	struct drbd_peer_device *peer_device;
 	struct net_conf *old_net_conf, *new_net_conf = NULL;
 	struct crypto crypto = { };
@@ -2182,14 +2234,14 @@ int drbd_adm_connect(struct sk_buff *skb, struct genl_info *info)
 	int i;
 	int err;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_RESOURCE);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_RESOURCE);
 
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
 		goto out;
 	if (!(adm_ctx.my_addr && adm_ctx.peer_addr)) {
-		drbd_msg_put_info("connection endpoint(s) missing");
+		drbd_msg_put_info(adm_ctx.reply_skb, "connection endpoint(s) missing");
 		retcode = ERR_INVALID_REQUEST;
 		goto out;
 	}
@@ -2215,6 +2267,7 @@ int drbd_adm_connect(struct sk_buff *skb, struct genl_info *info)
 		}
 	}
 
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 	connection = first_connection(adm_ctx.resource);
 	conn_reconfig_start(connection);
 
@@ -2235,7 +2288,7 @@ int drbd_adm_connect(struct sk_buff *skb, struct genl_info *info)
 	err = net_conf_from_attrs(new_net_conf, info);
 	if (err && err != -ENOMSG) {
 		retcode = ERR_MANDATORY_TAG;
-		drbd_msg_put_info(from_attrs_err_to_txt(err));
+		drbd_msg_put_info(adm_ctx.reply_skb, from_attrs_err_to_txt(err));
 		goto fail;
 	}
 
@@ -2284,7 +2337,8 @@ int drbd_adm_connect(struct sk_buff *skb, struct genl_info *info)
 	retcode = conn_request_state(connection, NS(conn, C_UNCONNECTED), CS_VERBOSE);
 
 	conn_reconfig_done(connection);
-	drbd_adm_finish(info, retcode);
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 
 fail:
@@ -2292,8 +2346,9 @@ fail:
 	kfree(new_net_conf);
 
 	conn_reconfig_done(connection);
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
@@ -2356,13 +2411,14 @@ static enum drbd_state_rv conn_try_disconnect(struct drbd_connection *connection
 
 int drbd_adm_disconnect(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	struct disconnect_parms parms;
 	struct drbd_connection *connection;
 	enum drbd_state_rv rv;
 	enum drbd_ret_code retcode;
 	int err;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_CONNECTION);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_CONNECTION);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
@@ -2374,18 +2430,20 @@ int drbd_adm_disconnect(struct sk_buff *skb, struct genl_info *info)
 		err = disconnect_parms_from_attrs(&parms, info);
 		if (err) {
 			retcode = ERR_MANDATORY_TAG;
-			drbd_msg_put_info(from_attrs_err_to_txt(err));
+			drbd_msg_put_info(adm_ctx.reply_skb, from_attrs_err_to_txt(err));
 			goto fail;
 		}
 	}
 
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 	rv = conn_try_disconnect(connection, parms.force_disconnect);
 	if (rv < SS_SUCCESS)
 		retcode = rv;  /* FIXME: Type mismatch. */
 	else
 		retcode = NO_ERROR;
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
  fail:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
@@ -2407,6 +2465,7 @@ void resync_after_online_grow(struct drbd_device *device)
 
 int drbd_adm_resize(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	struct disk_conf *old_disk_conf, *new_disk_conf = NULL;
 	struct resize_parms rs;
 	struct drbd_device *device;
@@ -2417,12 +2476,13 @@ int drbd_adm_resize(struct sk_buff *skb, struct genl_info *info)
 	sector_t u_size;
 	int err;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
-		goto fail;
+		goto finish;
 
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 	device = adm_ctx.device;
 	if (!get_ldev(device)) {
 		retcode = ERR_NO_DISK;
@@ -2436,7 +2496,7 @@ int drbd_adm_resize(struct sk_buff *skb, struct genl_info *info)
 		err = resize_parms_from_attrs(&rs, info);
 		if (err) {
 			retcode = ERR_MANDATORY_TAG;
-			drbd_msg_put_info(from_attrs_err_to_txt(err));
+			drbd_msg_put_info(adm_ctx.reply_skb, from_attrs_err_to_txt(err));
 			goto fail_ldev;
 		}
 	}
@@ -2482,7 +2542,7 @@ int drbd_adm_resize(struct sk_buff *skb, struct genl_info *info)
 			goto fail_ldev;
 		}
 
-		if (device->state.conn != C_CONNECTED) {
+		if (device->state.conn != C_CONNECTED && !rs.resize_force) {
 			retcode = ERR_MD_LAYOUT_CONNECTED;
 			goto fail_ldev;
 		}
@@ -2528,7 +2588,9 @@ int drbd_adm_resize(struct sk_buff *skb, struct genl_info *info)
 	}
 
  fail:
-	drbd_adm_finish(info, retcode);
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
+ finish:
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 
  fail_ldev:
@@ -2538,11 +2600,12 @@ int drbd_adm_resize(struct sk_buff *skb, struct genl_info *info)
 
 int drbd_adm_resource_opts(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	enum drbd_ret_code retcode;
 	struct res_opts res_opts;
 	int err;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_RESOURCE);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_RESOURCE);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
@@ -2555,33 +2618,37 @@ int drbd_adm_resource_opts(struct sk_buff *skb, struct genl_info *info)
 	err = res_opts_from_attrs(&res_opts, info);
 	if (err && err != -ENOMSG) {
 		retcode = ERR_MANDATORY_TAG;
-		drbd_msg_put_info(from_attrs_err_to_txt(err));
+		drbd_msg_put_info(adm_ctx.reply_skb, from_attrs_err_to_txt(err));
 		goto fail;
 	}
 
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 	err = set_resource_options(adm_ctx.resource, &res_opts);
 	if (err) {
 		retcode = ERR_INVALID_REQUEST;
 		if (err == -ENOMEM)
 			retcode = ERR_NOMEM;
 	}
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
 
 fail:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
 int drbd_adm_invalidate(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	struct drbd_device *device;
 	int retcode; /* enum drbd_ret_code rsp. enum drbd_state_rv */
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
 		goto out;
 
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 	device = adm_ctx.device;
 
 	/* If there is still bitmap IO pending, probably because of a previous
@@ -2605,26 +2672,29 @@ int drbd_adm_invalidate(struct sk_buff *skb, struct genl_info *info)
 	} else
 		retcode = drbd_request_state(device, NS(conn, C_STARTING_SYNC_T));
 	drbd_resume_io(device);
-
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
 static int drbd_adm_simple_request_state(struct sk_buff *skb, struct genl_info *info,
 		union drbd_state mask, union drbd_state val)
 {
+	struct drbd_config_context adm_ctx;
 	enum drbd_ret_code retcode;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
 		goto out;
 
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 	retcode = drbd_request_state(adm_ctx.device, mask, val);
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
@@ -2639,15 +2709,17 @@ static int drbd_bmio_set_susp_al(struct drbd_device *device)
 
 int drbd_adm_invalidate_peer(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	int retcode; /* drbd_ret_code, drbd_state_rv */
 	struct drbd_device *device;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
 		goto out;
 
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 	device = adm_ctx.device;
 
 	/* If there is still bitmap IO pending, probably because of a previous
@@ -2674,40 +2746,45 @@ int drbd_adm_invalidate_peer(struct sk_buff *skb, struct genl_info *info)
 	} else
 		retcode = drbd_request_state(device, NS(conn, C_STARTING_SYNC_S));
 	drbd_resume_io(device);
-
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
 int drbd_adm_pause_sync(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	enum drbd_ret_code retcode;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
 		goto out;
 
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 	if (drbd_request_state(adm_ctx.device, NS(user_isp, 1)) == SS_NOTHING_TO_DO)
 		retcode = ERR_PAUSE_IS_SET;
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
 int drbd_adm_resume_sync(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	union drbd_dev_state s;
 	enum drbd_ret_code retcode;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
 		goto out;
 
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 	if (drbd_request_state(adm_ctx.device, NS(user_isp, 0)) == SS_NOTHING_TO_DO) {
 		s = adm_ctx.device->state;
 		if (s.conn == C_PAUSED_SYNC_S || s.conn == C_PAUSED_SYNC_T) {
@@ -2717,9 +2794,9 @@ int drbd_adm_resume_sync(struct sk_buff *skb, struct genl_info *info)
 			retcode = ERR_PAUSE_IS_CLEAR;
 		}
 	}
-
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
@@ -2730,15 +2807,17 @@ int drbd_adm_suspend_io(struct sk_buff *skb, struct genl_info *info)
 
 int drbd_adm_resume_io(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	struct drbd_device *device;
 	int retcode; /* enum drbd_ret_code rsp. enum drbd_state_rv */
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
 		goto out;
 
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 	device = adm_ctx.device;
 	if (test_bit(NEW_CUR_UUID, &device->flags)) {
 		drbd_uuid_new_current(device);
@@ -2753,9 +2832,9 @@ int drbd_adm_resume_io(struct sk_buff *skb, struct genl_info *info)
 			tl_restart(first_peer_device(device)->connection, FAIL_FROZEN_DISK_IO);
 	}
 	drbd_resume_io(device);
-
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
@@ -2931,10 +3010,11 @@ nla_put_failure:
 
 int drbd_adm_get_status(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	enum drbd_ret_code retcode;
 	int err;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
@@ -2946,7 +3026,7 @@ int drbd_adm_get_status(struct sk_buff *skb, struct genl_info *info)
 		return err;
 	}
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
@@ -3133,11 +3213,12 @@ dump:
 
 int drbd_adm_get_timeout_type(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	enum drbd_ret_code retcode;
 	struct timeout_parms tp;
 	int err;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
@@ -3154,17 +3235,18 @@ int drbd_adm_get_timeout_type(struct sk_buff *skb, struct genl_info *info)
 		return err;
 	}
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
 int drbd_adm_start_ov(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	struct drbd_device *device;
 	enum drbd_ret_code retcode;
 	struct start_ov_parms parms;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
@@ -3179,10 +3261,12 @@ int drbd_adm_start_ov(struct sk_buff *skb, struct genl_info *info)
 		int err = start_ov_parms_from_attrs(&parms, info);
 		if (err) {
 			retcode = ERR_MANDATORY_TAG;
-			drbd_msg_put_info(from_attrs_err_to_txt(err));
+			drbd_msg_put_info(adm_ctx.reply_skb, from_attrs_err_to_txt(err));
 			goto out;
 		}
 	}
+	mutex_lock(&adm_ctx.resource->adm_mutex);
+
 	/* w_make_ov_request expects position to be aligned */
 	device->ov_start_sector = parms.ov_start_sector & ~(BM_SECT_PER_BIT-1);
 	device->ov_stop_sector = parms.ov_stop_sector;
@@ -3193,21 +3277,24 @@ int drbd_adm_start_ov(struct sk_buff *skb, struct genl_info *info)
 	wait_event(device->misc_wait, !test_bit(BITMAP_IO, &device->flags));
 	retcode = drbd_request_state(device, NS(conn, C_VERIFY_S));
 	drbd_resume_io(device);
+
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
 
 int drbd_adm_new_c_uuid(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	struct drbd_device *device;
 	enum drbd_ret_code retcode;
 	int skip_initial_sync = 0;
 	int err;
 	struct new_c_uuid_parms args;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
@@ -3219,11 +3306,12 @@ int drbd_adm_new_c_uuid(struct sk_buff *skb, struct genl_info *info)
 		err = new_c_uuid_parms_from_attrs(&args, info);
 		if (err) {
 			retcode = ERR_MANDATORY_TAG;
-			drbd_msg_put_info(from_attrs_err_to_txt(err));
+			drbd_msg_put_info(adm_ctx.reply_skb, from_attrs_err_to_txt(err));
 			goto out_nolock;
 		}
 	}
 
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 	mutex_lock(device->state_mutex); /* Protects us against serialized state changes. */
 
 	if (!get_ldev(device)) {
@@ -3268,22 +3356,24 @@ out_dec:
 	put_ldev(device);
 out:
 	mutex_unlock(device->state_mutex);
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
 out_nolock:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
 static enum drbd_ret_code
-drbd_check_resource_name(const char *name)
+drbd_check_resource_name(struct drbd_config_context *adm_ctx)
 {
+	const char *name = adm_ctx->resource_name;
 	if (!name || !name[0]) {
-		drbd_msg_put_info("resource name missing");
+		drbd_msg_put_info(adm_ctx->reply_skb, "resource name missing");
 		return ERR_MANDATORY_TAG;
 	}
 	/* if we want to use these in sysfs/configfs/debugfs some day,
 	 * we must not allow slashes */
 	if (strchr(name, '/')) {
-		drbd_msg_put_info("invalid resource name");
+		drbd_msg_put_info(adm_ctx->reply_skb, "invalid resource name");
 		return ERR_INVALID_REQUEST;
 	}
 	return NO_ERROR;
@@ -3291,11 +3381,12 @@ drbd_check_resource_name(const char *name)
 
 int drbd_adm_new_resource(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	enum drbd_ret_code retcode;
 	struct res_opts res_opts;
 	int err;
 
-	retcode = drbd_adm_prepare(skb, info, 0);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, 0);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
@@ -3305,48 +3396,50 @@ int drbd_adm_new_resource(struct sk_buff *skb, struct genl_info *info)
 	err = res_opts_from_attrs(&res_opts, info);
 	if (err && err != -ENOMSG) {
 		retcode = ERR_MANDATORY_TAG;
-		drbd_msg_put_info(from_attrs_err_to_txt(err));
+		drbd_msg_put_info(adm_ctx.reply_skb, from_attrs_err_to_txt(err));
 		goto out;
 	}
 
-	retcode = drbd_check_resource_name(adm_ctx.resource_name);
+	retcode = drbd_check_resource_name(&adm_ctx);
 	if (retcode != NO_ERROR)
 		goto out;
 
 	if (adm_ctx.resource) {
 		if (info->nlhdr->nlmsg_flags & NLM_F_EXCL) {
 			retcode = ERR_INVALID_REQUEST;
-			drbd_msg_put_info("resource exists");
+			drbd_msg_put_info(adm_ctx.reply_skb, "resource exists");
 		}
 		/* else: still NO_ERROR */
 		goto out;
 	}
 
+	/* not yet safe for genl_family.parallel_ops */
 	if (!conn_create(adm_ctx.resource_name, &res_opts))
 		retcode = ERR_NOMEM;
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
 int drbd_adm_new_minor(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	struct drbd_genlmsghdr *dh = info->userhdr;
 	enum drbd_ret_code retcode;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_RESOURCE);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_RESOURCE);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
 		goto out;
 
 	if (dh->minor > MINORMASK) {
-		drbd_msg_put_info("requested minor out of range");
+		drbd_msg_put_info(adm_ctx.reply_skb, "requested minor out of range");
 		retcode = ERR_INVALID_REQUEST;
 		goto out;
 	}
 	if (adm_ctx.volume > DRBD_VOLUME_MAX) {
-		drbd_msg_put_info("requested volume id out of range");
+		drbd_msg_put_info(adm_ctx.reply_skb, "requested volume id out of range");
 		retcode = ERR_INVALID_REQUEST;
 		goto out;
 	}
@@ -3360,9 +3453,11 @@ int drbd_adm_new_minor(struct sk_buff *skb, struct genl_info *info)
 		goto out;
 	}
 
-	retcode = drbd_create_device(adm_ctx.resource, dh->minor, adm_ctx.volume);
+	mutex_lock(&adm_ctx.resource->adm_mutex);
+	retcode = drbd_create_device(&adm_ctx, dh->minor);
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
@@ -3383,35 +3478,40 @@ static enum drbd_ret_code adm_del_minor(struct drbd_device *device)
 
 int drbd_adm_del_minor(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	enum drbd_ret_code retcode;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_MINOR);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_MINOR);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
 		goto out;
 
+	mutex_lock(&adm_ctx.resource->adm_mutex);
 	retcode = adm_del_minor(adm_ctx.device);
+	mutex_unlock(&adm_ctx.resource->adm_mutex);
 out:
-	drbd_adm_finish(info, retcode);
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
 int drbd_adm_down(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	struct drbd_resource *resource;
 	struct drbd_connection *connection;
 	struct drbd_device *device;
 	int retcode; /* enum drbd_ret_code rsp. enum drbd_state_rv */
 	unsigned i;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_RESOURCE);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_RESOURCE);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
-		goto out;
+		goto finish;
 
 	resource = adm_ctx.resource;
+	mutex_lock(&resource->adm_mutex);
 	/* demote */
 	for_each_connection(connection, resource) {
 		struct drbd_peer_device *peer_device;
@@ -3419,14 +3519,14 @@ int drbd_adm_down(struct sk_buff *skb, struct genl_info *info)
 		idr_for_each_entry(&connection->peer_devices, peer_device, i) {
 			retcode = drbd_set_role(peer_device->device, R_SECONDARY, 0);
 			if (retcode < SS_SUCCESS) {
-				drbd_msg_put_info("failed to demote");
+				drbd_msg_put_info(adm_ctx.reply_skb, "failed to demote");
 				goto out;
 			}
 		}
 
 		retcode = conn_try_disconnect(connection, 0);
 		if (retcode < SS_SUCCESS) {
-			drbd_msg_put_info("failed to disconnect");
+			drbd_msg_put_info(adm_ctx.reply_skb, "failed to disconnect");
 			goto out;
 		}
 	}
@@ -3435,7 +3535,7 @@ int drbd_adm_down(struct sk_buff *skb, struct genl_info *info)
 	idr_for_each_entry(&resource->devices, device, i) {
 		retcode = adm_detach(device, 0);
 		if (retcode < SS_SUCCESS || retcode > NO_ERROR) {
-			drbd_msg_put_info("failed to detach");
+			drbd_msg_put_info(adm_ctx.reply_skb, "failed to detach");
 			goto out;
 		}
 	}
@@ -3453,7 +3553,7 @@ int drbd_adm_down(struct sk_buff *skb, struct genl_info *info)
 		retcode = adm_del_minor(device);
 		if (retcode != NO_ERROR) {
 			/* "can not happen" */
-			drbd_msg_put_info("failed to delete volume");
+			drbd_msg_put_info(adm_ctx.reply_skb, "failed to delete volume");
 			goto out;
 		}
 	}
@@ -3462,25 +3562,28 @@ int drbd_adm_down(struct sk_buff *skb, struct genl_info *info)
 	synchronize_rcu();
 	drbd_free_resource(resource);
 	retcode = NO_ERROR;
-
 out:
-	drbd_adm_finish(info, retcode);
+	mutex_unlock(&resource->adm_mutex);
+finish:
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
 int drbd_adm_del_resource(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_config_context adm_ctx;
 	struct drbd_resource *resource;
 	struct drbd_connection *connection;
 	enum drbd_ret_code retcode;
 
-	retcode = drbd_adm_prepare(skb, info, DRBD_ADM_NEED_RESOURCE);
+	retcode = drbd_adm_prepare(&adm_ctx, skb, info, DRBD_ADM_NEED_RESOURCE);
 	if (!adm_ctx.reply_skb)
 		return retcode;
 	if (retcode != NO_ERROR)
-		goto out;
+		goto finish;
 
 	resource = adm_ctx.resource;
+	mutex_lock(&resource->adm_mutex);
 	for_each_connection(connection, resource) {
 		if (connection->cstate > C_STANDALONE) {
 			retcode = ERR_NET_CONFIGURED;
@@ -3499,7 +3602,9 @@ int drbd_adm_del_resource(struct sk_buff *skb, struct genl_info *info)
 	drbd_free_resource(resource);
 	retcode = NO_ERROR;
 out:
-	drbd_adm_finish(info, retcode);
+	mutex_unlock(&resource->adm_mutex);
+finish:
+	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
diff --git a/drivers/block/drbd/drbd_nla.c b/drivers/block/drbd/drbd_nla.c
index fa672b6df8d..b2d4791498a 100644
--- a/drivers/block/drbd/drbd_nla.c
+++ b/drivers/block/drbd/drbd_nla.c
@@ -1,4 +1,3 @@
-#include "drbd_wrappers.h"
 #include <linux/kernel.h>
 #include <net/netlink.h>
 #include <linux/drbd_genl_api.h>
diff --git a/drivers/block/drbd/drbd_proc.c b/drivers/block/drbd/drbd_proc.c
index 2f26e8ffa45..89736bdbbc7 100644
--- a/drivers/block/drbd/drbd_proc.c
+++ b/drivers/block/drbd/drbd_proc.c
@@ -116,7 +116,7 @@ static void drbd_syncer_progress(struct drbd_device *device, struct seq_file *se
 	/* ------------------------ ~18s average ------------------------ */
 	i = (device->rs_last_mark + 2) % DRBD_SYNC_MARKS;
 	dt = (jiffies - device->rs_mark_time[i]) / HZ;
-	if (dt > (DRBD_SYNC_MARK_STEP * DRBD_SYNC_MARKS))
+	if (dt > 180)
 		stalled = 1;
 
 	if (!dt)
diff --git a/drivers/block/drbd/drbd_protocol.h b/drivers/block/drbd/drbd_protocol.h
index 3c04ec0ea33..2da9104a385 100644
--- a/drivers/block/drbd/drbd_protocol.h
+++ b/drivers/block/drbd/drbd_protocol.h
@@ -54,6 +54,11 @@ enum drbd_packet {
 	P_CONN_ST_CHG_REPLY   = 0x2b, /* meta sock: Connection side state req reply */
 	P_RETRY_WRITE	      = 0x2c, /* Protocol C: retry conflicting write request */
 	P_PROTOCOL_UPDATE     = 0x2d, /* data sock: is used in established connections */
+        /* 0x2e to 0x30 reserved, used in drbd 9 */
+
+	/* REQ_DISCARD. We used "discard" in different contexts before,
+	 * which is why I chose TRIM here, to disambiguate. */
+	P_TRIM                = 0x31,
 
 	P_MAY_IGNORE	      = 0x100, /* Flag to test if (cmd > P_MAY_IGNORE) ... */
 	P_MAX_OPT_CMD	      = 0x101,
@@ -119,6 +124,11 @@ struct p_data {
 	u32	    dp_flags;
 } __packed;
 
+struct p_trim {
+	struct p_data p_data;
+	u32	    size;	/* == bio->bi_size */
+} __packed;
+
 /*
  * commands which share a struct:
  *  p_block_ack:
@@ -150,6 +160,8 @@ struct p_block_req {
  *   ReportParams
  */
 
+#define FF_TRIM      1
+
 struct p_connection_features {
 	u32 protocol_min;
 	u32 feature_flags;
diff --git a/drivers/block/drbd/drbd_receiver.c b/drivers/block/drbd/drbd_receiver.c
index 68e3992e883..b6c8aaf4931 100644
--- a/drivers/block/drbd/drbd_receiver.c
+++ b/drivers/block/drbd/drbd_receiver.c
@@ -46,9 +46,10 @@
 #include "drbd_int.h"
 #include "drbd_protocol.h"
 #include "drbd_req.h"
-
 #include "drbd_vli.h"
 
+#define PRO_FEATURES (FF_TRIM)
+
 struct packet_info {
 	enum drbd_packet cmd;
 	unsigned int size;
@@ -65,7 +66,7 @@ enum finish_epoch {
 static int drbd_do_features(struct drbd_connection *connection);
 static int drbd_do_auth(struct drbd_connection *connection);
 static int drbd_disconnected(struct drbd_peer_device *);
-
+static void conn_wait_active_ee_empty(struct drbd_connection *connection);
 static enum finish_epoch drbd_may_finish_epoch(struct drbd_connection *, struct drbd_epoch *, enum epoch_event);
 static int e_end_block(struct drbd_work *, int);
 
@@ -234,9 +235,17 @@ static void drbd_kick_lo_and_reclaim_net(struct drbd_device *device)
  * @retry:	whether to retry, if not enough pages are available right now
  *
  * Tries to allocate number pages, first from our own page pool, then from
- * the kernel, unless this allocation would exceed the max_buffers setting.
+ * the kernel.
  * Possibly retry until DRBD frees sufficient pages somewhere else.
  *
+ * If this allocation would exceed the max_buffers setting, we throttle
+ * allocation (schedule_timeout) to give the system some room to breathe.
+ *
+ * We do not use max-buffers as hard limit, because it could lead to
+ * congestion and further to a distributed deadlock during online-verify or
+ * (checksum based) resync, if the max-buffers, socket buffer sizes and
+ * resync-rate settings are mis-configured.
+ *
  * Returns a page chain linked via page->private.
  */
 struct page *drbd_alloc_pages(struct drbd_peer_device *peer_device, unsigned int number,
@@ -246,10 +255,8 @@ struct page *drbd_alloc_pages(struct drbd_peer_device *peer_device, unsigned int
 	struct page *page = NULL;
 	struct net_conf *nc;
 	DEFINE_WAIT(wait);
-	int mxb;
+	unsigned int mxb;
 
-	/* Yes, we may run up to @number over max_buffers. If we
-	 * follow it strictly, the admin will get it wrong anyways. */
 	rcu_read_lock();
 	nc = rcu_dereference(peer_device->connection->net_conf);
 	mxb = nc ? nc->max_buffers : 1000000;
@@ -277,7 +284,8 @@ struct page *drbd_alloc_pages(struct drbd_peer_device *peer_device, unsigned int
 			break;
 		}
 
-		schedule();
+		if (schedule_timeout(HZ/10) == 0)
+			mxb = UINT_MAX;
 	}
 	finish_wait(&drbd_pp_wait, &wait);
 
@@ -331,7 +339,7 @@ You must not have the req_lock:
 
 struct drbd_peer_request *
 drbd_alloc_peer_req(struct drbd_peer_device *peer_device, u64 id, sector_t sector,
-		    unsigned int data_size, gfp_t gfp_mask) __must_hold(local)
+		    unsigned int data_size, bool has_payload, gfp_t gfp_mask) __must_hold(local)
 {
 	struct drbd_device *device = peer_device->device;
 	struct drbd_peer_request *peer_req;
@@ -348,7 +356,7 @@ drbd_alloc_peer_req(struct drbd_peer_device *peer_device, u64 id, sector_t secto
 		return NULL;
 	}
 
-	if (data_size) {
+	if (has_payload && data_size) {
 		page = drbd_alloc_pages(peer_device, nr_pages, (gfp_mask & __GFP_WAIT));
 		if (!page)
 			goto fail;
@@ -1026,24 +1034,27 @@ randomize:
 	if (drbd_send_protocol(connection) == -EOPNOTSUPP)
 		return -1;
 
+	/* Prevent a race between resync-handshake and
+	 * being promoted to Primary.
+	 *
+	 * Grab and release the state mutex, so we know that any current
+	 * drbd_set_role() is finished, and any incoming drbd_set_role
+	 * will see the STATE_SENT flag, and wait for it to be cleared.
+	 */
+	idr_for_each_entry(&connection->peer_devices, peer_device, vnr)
+		mutex_lock(peer_device->device->state_mutex);
+
 	set_bit(STATE_SENT, &connection->flags);
 
+	idr_for_each_entry(&connection->peer_devices, peer_device, vnr)
+		mutex_unlock(peer_device->device->state_mutex);
+
 	rcu_read_lock();
 	idr_for_each_entry(&connection->peer_devices, peer_device, vnr) {
 		struct drbd_device *device = peer_device->device;
 		kref_get(&device->kref);
 		rcu_read_unlock();
 
-		/* Prevent a race between resync-handshake and
-		 * being promoted to Primary.
-		 *
-		 * Grab and release the state mutex, so we know that any current
-		 * drbd_set_role() is finished, and any incoming drbd_set_role
-		 * will see the STATE_SENT flag, and wait for it to be cleared.
-		 */
-		mutex_lock(device->state_mutex);
-		mutex_unlock(device->state_mutex);
-
 		if (discard_my_data)
 			set_bit(DISCARD_MY_DATA, &device->flags);
 		else
@@ -1315,6 +1326,20 @@ int drbd_submit_peer_request(struct drbd_device *device,
 	unsigned nr_pages = (ds + PAGE_SIZE -1) >> PAGE_SHIFT;
 	int err = -ENOMEM;
 
+	if (peer_req->flags & EE_IS_TRIM_USE_ZEROOUT) {
+		/* wait for all pending IO completions, before we start
+		 * zeroing things out. */
+		conn_wait_active_ee_empty(first_peer_device(device)->connection);
+		if (blkdev_issue_zeroout(device->ldev->backing_bdev,
+			sector, ds >> 9, GFP_NOIO))
+			peer_req->flags |= EE_WAS_ERROR;
+		drbd_endio_write_sec_final(peer_req);
+		return 0;
+	}
+
+	if (peer_req->flags & EE_IS_TRIM)
+		nr_pages = 0; /* discards don't have any payload. */
+
 	/* In most cases, we will only need one bio.  But in case the lower
 	 * level restrictions happen to be different at this offset on this
 	 * side than those of the sending peer, we may need to submit the
@@ -1326,7 +1351,7 @@ int drbd_submit_peer_request(struct drbd_device *device,
 next_bio:
 	bio = bio_alloc(GFP_NOIO, nr_pages);
 	if (!bio) {
-		drbd_err(device, "submit_ee: Allocation of a bio failed\n");
+		drbd_err(device, "submit_ee: Allocation of a bio failed (nr_pages=%u)\n", nr_pages);
 		goto fail;
 	}
 	/* > peer_req->i.sector, unless this is the first bio */
@@ -1340,6 +1365,11 @@ next_bio:
 	bios = bio;
 	++n_bios;
 
+	if (rw & REQ_DISCARD) {
+		bio->bi_iter.bi_size = ds;
+		goto submit;
+	}
+
 	page_chain_for_each(page) {
 		unsigned len = min_t(unsigned, ds, PAGE_SIZE);
 		if (!bio_add_page(bio, page, len, 0)) {
@@ -1360,8 +1390,9 @@ next_bio:
 		sector += len >> 9;
 		--nr_pages;
 	}
-	D_ASSERT(device, page == NULL);
 	D_ASSERT(device, ds == 0);
+submit:
+	D_ASSERT(device, page == NULL);
 
 	atomic_set(&peer_req->pending_bios, n_bios);
 	do {
@@ -1490,19 +1521,21 @@ static int receive_Barrier(struct drbd_connection *connection, struct packet_inf
  * and from receive_Data */
 static struct drbd_peer_request *
 read_in_block(struct drbd_peer_device *peer_device, u64 id, sector_t sector,
-	      int data_size) __must_hold(local)
+	      struct packet_info *pi) __must_hold(local)
 {
 	struct drbd_device *device = peer_device->device;
 	const sector_t capacity = drbd_get_capacity(device->this_bdev);
 	struct drbd_peer_request *peer_req;
 	struct page *page;
 	int dgs, ds, err;
+	int data_size = pi->size;
 	void *dig_in = peer_device->connection->int_dig_in;
 	void *dig_vv = peer_device->connection->int_dig_vv;
 	unsigned long *data;
+	struct p_trim *trim = (pi->cmd == P_TRIM) ? pi->data : NULL;
 
 	dgs = 0;
-	if (peer_device->connection->peer_integrity_tfm) {
+	if (!trim && peer_device->connection->peer_integrity_tfm) {
 		dgs = crypto_hash_digestsize(peer_device->connection->peer_integrity_tfm);
 		/*
 		 * FIXME: Receive the incoming digest into the receive buffer
@@ -1514,9 +1547,15 @@ read_in_block(struct drbd_peer_device *peer_device, u64 id, sector_t sector,
 		data_size -= dgs;
 	}
 
+	if (trim) {
+		D_ASSERT(peer_device, data_size == 0);
+		data_size = be32_to_cpu(trim->size);
+	}
+
 	if (!expect(IS_ALIGNED(data_size, 512)))
 		return NULL;
-	if (!expect(data_size <= DRBD_MAX_BIO_SIZE))
+	/* prepare for larger trim requests. */
+	if (!trim && !expect(data_size <= DRBD_MAX_BIO_SIZE))
 		return NULL;
 
 	/* even though we trust out peer,
@@ -1532,11 +1571,11 @@ read_in_block(struct drbd_peer_device *peer_device, u64 id, sector_t sector,
 	/* GFP_NOIO, because we must not cause arbitrary write-out: in a DRBD
 	 * "criss-cross" setup, that might cause write-out on some other DRBD,
 	 * which in turn might block on the other node at this very place.  */
-	peer_req = drbd_alloc_peer_req(peer_device, id, sector, data_size, GFP_NOIO);
+	peer_req = drbd_alloc_peer_req(peer_device, id, sector, data_size, trim == NULL, GFP_NOIO);
 	if (!peer_req)
 		return NULL;
 
-	if (!data_size)
+	if (trim)
 		return peer_req;
 
 	ds = data_size;
@@ -1676,12 +1715,12 @@ static int e_end_resync_block(struct drbd_work *w, int unused)
 }
 
 static int recv_resync_read(struct drbd_peer_device *peer_device, sector_t sector,
-			    int data_size) __releases(local)
+			    struct packet_info *pi) __releases(local)
 {
 	struct drbd_device *device = peer_device->device;
 	struct drbd_peer_request *peer_req;
 
-	peer_req = read_in_block(peer_device, ID_SYNCER, sector, data_size);
+	peer_req = read_in_block(peer_device, ID_SYNCER, sector, pi);
 	if (!peer_req)
 		goto fail;
 
@@ -1697,7 +1736,7 @@ static int recv_resync_read(struct drbd_peer_device *peer_device, sector_t secto
 	list_add(&peer_req->w.list, &device->sync_ee);
 	spin_unlock_irq(&device->resource->req_lock);
 
-	atomic_add(data_size >> 9, &device->rs_sect_ev);
+	atomic_add(pi->size >> 9, &device->rs_sect_ev);
 	if (drbd_submit_peer_request(device, peer_req, WRITE, DRBD_FAULT_RS_WR) == 0)
 		return 0;
 
@@ -1785,7 +1824,7 @@ static int receive_RSDataReply(struct drbd_connection *connection, struct packet
 		/* data is submitted to disk within recv_resync_read.
 		 * corresponding put_ldev done below on error,
 		 * or in drbd_peer_request_endio. */
-		err = recv_resync_read(peer_device, sector, pi->size);
+		err = recv_resync_read(peer_device, sector, pi);
 	} else {
 		if (__ratelimit(&drbd_ratelimit_state))
 			drbd_err(device, "Can not write resync data to local disk.\n");
@@ -2196,7 +2235,7 @@ static int receive_Data(struct drbd_connection *connection, struct packet_info *
 	 */
 
 	sector = be64_to_cpu(p->sector);
-	peer_req = read_in_block(peer_device, p->block_id, sector, pi->size);
+	peer_req = read_in_block(peer_device, p->block_id, sector, pi);
 	if (!peer_req) {
 		put_ldev(device);
 		return -EIO;
@@ -2206,7 +2245,15 @@ static int receive_Data(struct drbd_connection *connection, struct packet_info *
 
 	dp_flags = be32_to_cpu(p->dp_flags);
 	rw |= wire_flags_to_bio(dp_flags);
-	if (peer_req->pages == NULL) {
+	if (pi->cmd == P_TRIM) {
+		struct request_queue *q = bdev_get_queue(device->ldev->backing_bdev);
+		peer_req->flags |= EE_IS_TRIM;
+		if (!blk_queue_discard(q))
+			peer_req->flags |= EE_IS_TRIM_USE_ZEROOUT;
+		D_ASSERT(peer_device, peer_req->i.size > 0);
+		D_ASSERT(peer_device, rw & REQ_DISCARD);
+		D_ASSERT(peer_device, peer_req->pages == NULL);
+	} else if (peer_req->pages == NULL) {
 		D_ASSERT(device, peer_req->i.size == 0);
 		D_ASSERT(device, dp_flags & DP_FLUSH);
 	}
@@ -2242,7 +2289,12 @@ static int receive_Data(struct drbd_connection *connection, struct packet_info *
 		update_peer_seq(peer_device, peer_seq);
 		spin_lock_irq(&device->resource->req_lock);
 	}
-	list_add(&peer_req->w.list, &device->active_ee);
+	/* if we use the zeroout fallback code, we process synchronously
+	 * and we wait for all pending requests, respectively wait for
+	 * active_ee to become empty in drbd_submit_peer_request();
+	 * better not add ourselves here. */
+	if ((peer_req->flags & EE_IS_TRIM_USE_ZEROOUT) == 0)
+		list_add(&peer_req->w.list, &device->active_ee);
 	spin_unlock_irq(&device->resource->req_lock);
 
 	if (device->state.conn == C_SYNC_TARGET)
@@ -2313,39 +2365,45 @@ out_interrupted:
  * The current sync rate used here uses only the most recent two step marks,
  * to have a short time average so we can react faster.
  */
-int drbd_rs_should_slow_down(struct drbd_device *device, sector_t sector)
+bool drbd_rs_should_slow_down(struct drbd_device *device, sector_t sector)
 {
-	struct gendisk *disk = device->ldev->backing_bdev->bd_contains->bd_disk;
-	unsigned long db, dt, dbdt;
 	struct lc_element *tmp;
-	int curr_events;
-	int throttle = 0;
-	unsigned int c_min_rate;
-
-	rcu_read_lock();
-	c_min_rate = rcu_dereference(device->ldev->disk_conf)->c_min_rate;
-	rcu_read_unlock();
+	bool throttle = true;
 
-	/* feature disabled? */
-	if (c_min_rate == 0)
-		return 0;
+	if (!drbd_rs_c_min_rate_throttle(device))
+		return false;
 
 	spin_lock_irq(&device->al_lock);
 	tmp = lc_find(device->resync, BM_SECT_TO_EXT(sector));
 	if (tmp) {
 		struct bm_extent *bm_ext = lc_entry(tmp, struct bm_extent, lce);
-		if (test_bit(BME_PRIORITY, &bm_ext->flags)) {
-			spin_unlock_irq(&device->al_lock);
-			return 0;
-		}
+		if (test_bit(BME_PRIORITY, &bm_ext->flags))
+			throttle = false;
 		/* Do not slow down if app IO is already waiting for this extent */
 	}
 	spin_unlock_irq(&device->al_lock);
 
+	return throttle;
+}
+
+bool drbd_rs_c_min_rate_throttle(struct drbd_device *device)
+{
+	struct gendisk *disk = device->ldev->backing_bdev->bd_contains->bd_disk;
+	unsigned long db, dt, dbdt;
+	unsigned int c_min_rate;
+	int curr_events;
+
+	rcu_read_lock();
+	c_min_rate = rcu_dereference(device->ldev->disk_conf)->c_min_rate;
+	rcu_read_unlock();
+
+	/* feature disabled? */
+	if (c_min_rate == 0)
+		return false;
+
 	curr_events = (int)part_stat_read(&disk->part0, sectors[0]) +
 		      (int)part_stat_read(&disk->part0, sectors[1]) -
 			atomic_read(&device->rs_sect_ev);
-
 	if (!device->rs_last_events || curr_events - device->rs_last_events > 64) {
 		unsigned long rs_left;
 		int i;
@@ -2368,12 +2426,11 @@ int drbd_rs_should_slow_down(struct drbd_device *device, sector_t sector)
 		dbdt = Bit2KB(db/dt);
 
 		if (dbdt > c_min_rate)
-			throttle = 1;
+			return true;
 	}
-	return throttle;
+	return false;
 }
 
-
 static int receive_DataRequest(struct drbd_connection *connection, struct packet_info *pi)
 {
 	struct drbd_peer_device *peer_device;
@@ -2436,7 +2493,8 @@ static int receive_DataRequest(struct drbd_connection *connection, struct packet
 	/* GFP_NOIO, because we must not cause arbitrary write-out: in a DRBD
 	 * "criss-cross" setup, that might cause write-out on some other DRBD,
 	 * which in turn might block on the other node at this very place.  */
-	peer_req = drbd_alloc_peer_req(peer_device, p->block_id, sector, size, GFP_NOIO);
+	peer_req = drbd_alloc_peer_req(peer_device, p->block_id, sector, size,
+			true /* has real payload */, GFP_NOIO);
 	if (!peer_req) {
 		put_ldev(device);
 		return -ENOMEM;
@@ -3648,6 +3706,13 @@ static int receive_sizes(struct drbd_connection *connection, struct packet_info
 		put_ldev(device);
 	}
 
+	device->peer_max_bio_size = be32_to_cpu(p->max_bio_size);
+	drbd_reconsider_max_bio_size(device);
+	/* Leave drbd_reconsider_max_bio_size() before drbd_determine_dev_size().
+	   In case we cleared the QUEUE_FLAG_DISCARD from our queue in
+	   drbd_reconsider_max_bio_size(), we can be sure that after
+	   drbd_determine_dev_size() no REQ_DISCARDs are in the queue. */
+
 	ddsf = be16_to_cpu(p->dds_flags);
 	if (get_ldev(device)) {
 		dd = drbd_determine_dev_size(device, ddsf, NULL);
@@ -3660,9 +3725,6 @@ static int receive_sizes(struct drbd_connection *connection, struct packet_info
 		drbd_set_my_capacity(device, p_size);
 	}
 
-	device->peer_max_bio_size = be32_to_cpu(p->max_bio_size);
-	drbd_reconsider_max_bio_size(device);
-
 	if (get_ldev(device)) {
 		if (device->ldev->known_size != drbd_get_capacity(device->ldev->backing_bdev)) {
 			device->ldev->known_size = drbd_get_capacity(device->ldev->backing_bdev);
@@ -4423,6 +4485,7 @@ static struct data_cmd drbd_cmd_handler[] = {
 	[P_OUT_OF_SYNC]     = { 0, sizeof(struct p_block_desc), receive_out_of_sync },
 	[P_CONN_ST_CHG_REQ] = { 0, sizeof(struct p_req_state), receive_req_conn_state },
 	[P_PROTOCOL_UPDATE] = { 1, sizeof(struct p_protocol), receive_protocol },
+	[P_TRIM]	    = { 0, sizeof(struct p_trim), receive_Data },
 };
 
 static void drbdd(struct drbd_connection *connection)
@@ -4630,6 +4693,7 @@ static int drbd_send_features(struct drbd_connection *connection)
 	memset(p, 0, sizeof(*p));
 	p->protocol_min = cpu_to_be32(PRO_VERSION_MIN);
 	p->protocol_max = cpu_to_be32(PRO_VERSION_MAX);
+	p->feature_flags = cpu_to_be32(PRO_FEATURES);
 	return conn_send_command(connection, sock, P_CONNECTION_FEATURES, sizeof(*p), NULL, 0);
 }
 
@@ -4683,10 +4747,14 @@ static int drbd_do_features(struct drbd_connection *connection)
 		goto incompat;
 
 	connection->agreed_pro_version = min_t(int, PRO_VERSION_MAX, p->protocol_max);
+	connection->agreed_features = PRO_FEATURES & be32_to_cpu(p->feature_flags);
 
 	drbd_info(connection, "Handshake successful: "
 	     "Agreed network protocol version %d\n", connection->agreed_pro_version);
 
+	drbd_info(connection, "Agreed to%ssupport TRIM on protocol level\n",
+		  connection->agreed_features & FF_TRIM ? " " : " not ");
+
 	return 1;
 
  incompat:
@@ -4778,6 +4846,12 @@ static int drbd_do_auth(struct drbd_connection *connection)
 		goto fail;
 	}
 
+	if (pi.size < CHALLENGE_LEN) {
+		drbd_err(connection, "AuthChallenge payload too small.\n");
+		rv = -1;
+		goto fail;
+	}
+
 	peers_ch = kmalloc(pi.size, GFP_NOIO);
 	if (peers_ch == NULL) {
 		drbd_err(connection, "kmalloc of peers_ch failed\n");
@@ -4791,6 +4865,12 @@ static int drbd_do_auth(struct drbd_connection *connection)
 		goto fail;
 	}
 
+	if (!memcmp(my_challenge, peers_ch, CHALLENGE_LEN)) {
+		drbd_err(connection, "Peer presented the same challenge!\n");
+		rv = -1;
+		goto fail;
+	}
+
 	resp_size = crypto_hash_digestsize(connection->cram_hmac_tfm);
 	response = kmalloc(resp_size, GFP_NOIO);
 	if (response == NULL) {
diff --git a/drivers/block/drbd/drbd_req.c b/drivers/block/drbd/drbd_req.c
index 3779c8d2875..09803d0d520 100644
--- a/drivers/block/drbd/drbd_req.c
+++ b/drivers/block/drbd/drbd_req.c
@@ -522,6 +522,13 @@ int __req_mod(struct drbd_request *req, enum drbd_req_event what,
 		mod_rq_state(req, m, RQ_LOCAL_PENDING, RQ_LOCAL_COMPLETED);
 		break;
 
+	case DISCARD_COMPLETED_NOTSUPP:
+	case DISCARD_COMPLETED_WITH_ERROR:
+		/* I'd rather not detach from local disk just because it
+		 * failed a REQ_DISCARD. */
+		mod_rq_state(req, m, RQ_LOCAL_PENDING, RQ_LOCAL_COMPLETED);
+		break;
+
 	case QUEUE_FOR_NET_READ:
 		/* READ or READA, and
 		 * no local disk,
@@ -1235,6 +1242,7 @@ void do_submit(struct work_struct *ws)
 		if (list_empty(&incoming))
 			break;
 
+skip_fast_path:
 		wait_event(device->al_wait, prepare_al_transaction_nonblock(device, &incoming, &pending));
 		/* Maybe more was queued, while we prepared the transaction?
 		 * Try to stuff them into this transaction as well.
@@ -1273,6 +1281,25 @@ void do_submit(struct work_struct *ws)
 			list_del_init(&req->tl_requests);
 			drbd_send_and_submit(device, req);
 		}
+
+		/* If all currently hot activity log extents are kept busy by
+		 * incoming requests, we still must not totally starve new
+		 * requests to cold extents. In that case, prepare one request
+		 * in blocking mode. */
+		list_for_each_entry_safe(req, tmp, &incoming, tl_requests) {
+			list_del_init(&req->tl_requests);
+			req->rq_state |= RQ_IN_ACT_LOG;
+			if (!drbd_al_begin_io_prepare(device, &req->i)) {
+				/* Corresponding extent was hot after all? */
+				drbd_send_and_submit(device, req);
+			} else {
+				/* Found a request to a cold extent.
+				 * Put on "pending" list,
+				 * and try to cumulate with more. */
+				list_add(&req->tl_requests, &pending);
+				goto skip_fast_path;
+			}
+		}
 	}
 }
 
@@ -1326,23 +1353,35 @@ int drbd_merge_bvec(struct request_queue *q, struct bvec_merge_data *bvm, struct
 	return limit;
 }
 
-static struct drbd_request *find_oldest_request(struct drbd_connection *connection)
+static void find_oldest_requests(
+		struct drbd_connection *connection,
+		struct drbd_device *device,
+		struct drbd_request **oldest_req_waiting_for_peer,
+		struct drbd_request **oldest_req_waiting_for_disk)
 {
-	/* Walk the transfer log,
-	 * and find the oldest not yet completed request */
 	struct drbd_request *r;
+	*oldest_req_waiting_for_peer = NULL;
+	*oldest_req_waiting_for_disk = NULL;
 	list_for_each_entry(r, &connection->transfer_log, tl_requests) {
-		if (atomic_read(&r->completion_ref))
-			return r;
+		const unsigned s = r->rq_state;
+		if (!*oldest_req_waiting_for_peer
+		&& ((s & RQ_NET_MASK) && !(s & RQ_NET_DONE)))
+			*oldest_req_waiting_for_peer = r;
+
+		if (!*oldest_req_waiting_for_disk
+		&& (s & RQ_LOCAL_PENDING) && r->device == device)
+			*oldest_req_waiting_for_disk = r;
+
+		if (*oldest_req_waiting_for_peer && *oldest_req_waiting_for_disk)
+			break;
 	}
-	return NULL;
 }
 
 void request_timer_fn(unsigned long data)
 {
 	struct drbd_device *device = (struct drbd_device *) data;
 	struct drbd_connection *connection = first_peer_device(device)->connection;
-	struct drbd_request *req; /* oldest request */
+	struct drbd_request *req_disk, *req_peer; /* oldest request */
 	struct net_conf *nc;
 	unsigned long ent = 0, dt = 0, et, nt; /* effective timeout = ko_count * timeout */
 	unsigned long now;
@@ -1366,8 +1405,8 @@ void request_timer_fn(unsigned long data)
 	now = jiffies;
 
 	spin_lock_irq(&device->resource->req_lock);
-	req = find_oldest_request(connection);
-	if (!req) {
+	find_oldest_requests(connection, device, &req_peer, &req_disk);
+	if (req_peer == NULL && req_disk == NULL) {
 		spin_unlock_irq(&device->resource->req_lock);
 		mod_timer(&device->request_timer, now + et);
 		return;
@@ -1389,19 +1428,26 @@ void request_timer_fn(unsigned long data)
 	 * ~198 days with 250 HZ, we have a window where the timeout would need
 	 * to expire twice (worst case) to become effective. Good enough.
 	 */
-	if (ent && req->rq_state & RQ_NET_PENDING &&
-		 time_after(now, req->start_time + ent) &&
+	if (ent && req_peer &&
+		 time_after(now, req_peer->start_time + ent) &&
 		!time_in_range(now, connection->last_reconnect_jif, connection->last_reconnect_jif + ent)) {
 		drbd_warn(device, "Remote failed to finish a request within ko-count * timeout\n");
 		_drbd_set_state(_NS(device, conn, C_TIMEOUT), CS_VERBOSE | CS_HARD, NULL);
 	}
-	if (dt && req->rq_state & RQ_LOCAL_PENDING && req->device == device &&
-		 time_after(now, req->start_time + dt) &&
+	if (dt && req_disk &&
+		 time_after(now, req_disk->start_time + dt) &&
 		!time_in_range(now, device->last_reattach_jif, device->last_reattach_jif + dt)) {
 		drbd_warn(device, "Local backing device failed to meet the disk-timeout\n");
 		__drbd_chk_io_error(device, DRBD_FORCE_DETACH);
 	}
-	nt = (time_after(now, req->start_time + et) ? now : req->start_time) + et;
+
+	/* Reschedule timer for the nearest not already expired timeout.
+	 * Fallback to now + min(effective network timeout, disk timeout). */
+	ent = (ent && req_peer && time_before(now, req_peer->start_time + ent))
+		? req_peer->start_time + ent : now + et;
+	dt = (dt && req_disk && time_before(now, req_disk->start_time + dt))
+		? req_disk->start_time + dt : now + et;
+	nt = time_before(ent, dt) ? ent : dt;
 	spin_unlock_irq(&connection->resource->req_lock);
 	mod_timer(&device->request_timer, nt);
 }
diff --git a/drivers/block/drbd/drbd_req.h b/drivers/block/drbd/drbd_req.h
index c684c963538..8566cd5866b 100644
--- a/drivers/block/drbd/drbd_req.h
+++ b/drivers/block/drbd/drbd_req.h
@@ -30,7 +30,6 @@
 #include <linux/slab.h>
 #include <linux/drbd.h>
 #include "drbd_int.h"
-#include "drbd_wrappers.h"
 
 /* The request callbacks will be called in irq context by the IDE drivers,
    and in Softirqs/Tasklets/BH context by the SCSI drivers,
@@ -111,11 +110,14 @@ enum drbd_req_event {
 	BARRIER_ACKED, /* in protocol A and B */
 	DATA_RECEIVED, /* (remote read) */
 
+	COMPLETED_OK,
 	READ_COMPLETED_WITH_ERROR,
 	READ_AHEAD_COMPLETED_WITH_ERROR,
 	WRITE_COMPLETED_WITH_ERROR,
+	DISCARD_COMPLETED_NOTSUPP,
+	DISCARD_COMPLETED_WITH_ERROR,
+
 	ABORT_DISK_IO,
-	COMPLETED_OK,
 	RESEND,
 	FAIL_FROZEN_DISK_IO,
 	RESTART_FROZEN_DISK_IO,
diff --git a/drivers/block/drbd/drbd_state.c b/drivers/block/drbd/drbd_state.c
index 1a84345a386..a5d8aae00e0 100644
--- a/drivers/block/drbd/drbd_state.c
+++ b/drivers/block/drbd/drbd_state.c
@@ -54,8 +54,8 @@ static void after_state_ch(struct drbd_device *device, union drbd_state os,
 static enum drbd_state_rv is_valid_state(struct drbd_device *, union drbd_state);
 static enum drbd_state_rv is_valid_soft_transition(union drbd_state, union drbd_state, struct drbd_connection *);
 static enum drbd_state_rv is_valid_transition(union drbd_state os, union drbd_state ns);
-static union drbd_state sanitize_state(struct drbd_device *device, union drbd_state ns,
-				       enum sanitize_state_warnings *warn);
+static union drbd_state sanitize_state(struct drbd_device *device, union drbd_state os,
+				       union drbd_state ns, enum sanitize_state_warnings *warn);
 
 static inline bool is_susp(union drbd_state s)
 {
@@ -287,7 +287,7 @@ _req_st_cond(struct drbd_device *device, union drbd_state mask,
 
 	spin_lock_irqsave(&device->resource->req_lock, flags);
 	os = drbd_read_state(device);
-	ns = sanitize_state(device, apply_mask_val(os, mask, val), NULL);
+	ns = sanitize_state(device, os, apply_mask_val(os, mask, val), NULL);
 	rv = is_valid_transition(os, ns);
 	if (rv >= SS_SUCCESS)
 		rv = SS_UNKNOWN_ERROR;  /* cont waiting, otherwise fail. */
@@ -333,7 +333,7 @@ drbd_req_state(struct drbd_device *device, union drbd_state mask,
 
 	spin_lock_irqsave(&device->resource->req_lock, flags);
 	os = drbd_read_state(device);
-	ns = sanitize_state(device, apply_mask_val(os, mask, val), NULL);
+	ns = sanitize_state(device, os, apply_mask_val(os, mask, val), NULL);
 	rv = is_valid_transition(os, ns);
 	if (rv < SS_SUCCESS) {
 		spin_unlock_irqrestore(&device->resource->req_lock, flags);
@@ -740,8 +740,8 @@ static void print_sanitize_warnings(struct drbd_device *device, enum sanitize_st
  * When we loose connection, we have to set the state of the peers disk (pdsk)
  * to D_UNKNOWN. This rule and many more along those lines are in this function.
  */
-static union drbd_state sanitize_state(struct drbd_device *device, union drbd_state ns,
-				       enum sanitize_state_warnings *warn)
+static union drbd_state sanitize_state(struct drbd_device *device, union drbd_state os,
+				       union drbd_state ns, enum sanitize_state_warnings *warn)
 {
 	enum drbd_fencing_p fp;
 	enum drbd_disk_state disk_min, disk_max, pdsk_min, pdsk_max;
@@ -882,11 +882,13 @@ static union drbd_state sanitize_state(struct drbd_device *device, union drbd_st
 	}
 
 	if (fp == FP_STONITH &&
-	    (ns.role == R_PRIMARY && ns.conn < C_CONNECTED && ns.pdsk > D_OUTDATED))
+	    (ns.role == R_PRIMARY && ns.conn < C_CONNECTED && ns.pdsk > D_OUTDATED) &&
+	    !(os.role == R_PRIMARY && os.conn < C_CONNECTED && os.pdsk > D_OUTDATED))
 		ns.susp_fen = 1; /* Suspend IO while fence-peer handler runs (peer lost) */
 
 	if (device->resource->res_opts.on_no_data == OND_SUSPEND_IO &&
-	    (ns.role == R_PRIMARY && ns.disk < D_UP_TO_DATE && ns.pdsk < D_UP_TO_DATE))
+	    (ns.role == R_PRIMARY && ns.disk < D_UP_TO_DATE && ns.pdsk < D_UP_TO_DATE) &&
+	    !(os.role == R_PRIMARY && os.disk < D_UP_TO_DATE && os.pdsk < D_UP_TO_DATE))
 		ns.susp_nod = 1; /* Suspend IO while no data available (no accessible data available) */
 
 	if (ns.aftr_isp || ns.peer_isp || ns.user_isp) {
@@ -958,7 +960,7 @@ __drbd_set_state(struct drbd_device *device, union drbd_state ns,
 
 	os = drbd_read_state(device);
 
-	ns = sanitize_state(device, ns, &ssw);
+	ns = sanitize_state(device, os, ns, &ssw);
 	if (ns.i == os.i)
 		return SS_NOTHING_TO_DO;
 
@@ -1656,7 +1658,7 @@ conn_is_valid_transition(struct drbd_connection *connection, union drbd_state ma
 	idr_for_each_entry(&connection->peer_devices, peer_device, vnr) {
 		struct drbd_device *device = peer_device->device;
 		os = drbd_read_state(device);
-		ns = sanitize_state(device, apply_mask_val(os, mask, val), NULL);
+		ns = sanitize_state(device, os, apply_mask_val(os, mask, val), NULL);
 
 		if (flags & CS_IGN_OUTD_FAIL && ns.disk == D_OUTDATED && os.disk < D_OUTDATED)
 			ns.disk = os.disk;
@@ -1718,7 +1720,7 @@ conn_set_state(struct drbd_connection *connection, union drbd_state mask, union
 		number_of_volumes++;
 		os = drbd_read_state(device);
 		ns = apply_mask_val(os, mask, val);
-		ns = sanitize_state(device, ns, NULL);
+		ns = sanitize_state(device, os, ns, NULL);
 
 		if (flags & CS_IGN_OUTD_FAIL && ns.disk == D_OUTDATED && os.disk < D_OUTDATED)
 			ns.disk = os.disk;
@@ -1763,19 +1765,19 @@ conn_set_state(struct drbd_connection *connection, union drbd_state mask, union
 static enum drbd_state_rv
 _conn_rq_cond(struct drbd_connection *connection, union drbd_state mask, union drbd_state val)
 {
-	enum drbd_state_rv rv;
+	enum drbd_state_rv err, rv = SS_UNKNOWN_ERROR; /* continue waiting */;
 
 	if (test_and_clear_bit(CONN_WD_ST_CHG_OKAY, &connection->flags))
-		return SS_CW_SUCCESS;
+		rv = SS_CW_SUCCESS;
 
 	if (test_and_clear_bit(CONN_WD_ST_CHG_FAIL, &connection->flags))
-		return SS_CW_FAILED_BY_PEER;
+		rv = SS_CW_FAILED_BY_PEER;
 
-	rv = conn_is_valid_transition(connection, mask, val, 0);
-	if (rv == SS_SUCCESS && connection->cstate == C_WF_REPORT_PARAMS)
-		rv = SS_UNKNOWN_ERROR; /* continue waiting */
+	err = conn_is_valid_transition(connection, mask, val, 0);
+	if (err == SS_SUCCESS && connection->cstate == C_WF_REPORT_PARAMS)
+		return rv;
 
-	return rv;
+	return err;
 }
 
 enum drbd_state_rv
diff --git a/drivers/block/drbd/drbd_worker.c b/drivers/block/drbd/drbd_worker.c
index 2c4ce42c365..d8f57b6305c 100644
--- a/drivers/block/drbd/drbd_worker.c
+++ b/drivers/block/drbd/drbd_worker.c
@@ -118,7 +118,7 @@ static void drbd_endio_read_sec_final(struct drbd_peer_request *peer_req) __rele
 
 /* writes on behalf of the partner, or resync writes,
  * "submitted" by the receiver, final stage.  */
-static void drbd_endio_write_sec_final(struct drbd_peer_request *peer_req) __releases(local)
+void drbd_endio_write_sec_final(struct drbd_peer_request *peer_req) __releases(local)
 {
 	unsigned long flags = 0;
 	struct drbd_peer_device *peer_device = peer_req->peer_device;
@@ -150,7 +150,9 @@ static void drbd_endio_write_sec_final(struct drbd_peer_request *peer_req) __rel
 
 	do_wake = list_empty(block_id == ID_SYNCER ? &device->sync_ee : &device->active_ee);
 
-	if (test_bit(__EE_WAS_ERROR, &peer_req->flags))
+	/* FIXME do we want to detach for failed REQ_DISCARD?
+	 * ((peer_req->flags & (EE_WAS_ERROR|EE_IS_TRIM)) == EE_WAS_ERROR) */
+	if (peer_req->flags & EE_WAS_ERROR)
 		__drbd_chk_io_error(device, DRBD_WRITE_ERROR);
 	spin_unlock_irqrestore(&device->resource->req_lock, flags);
 
@@ -176,10 +178,12 @@ void drbd_peer_request_endio(struct bio *bio, int error)
 	struct drbd_device *device = peer_req->peer_device->device;
 	int uptodate = bio_flagged(bio, BIO_UPTODATE);
 	int is_write = bio_data_dir(bio) == WRITE;
+	int is_discard = !!(bio->bi_rw & REQ_DISCARD);
 
 	if (error && __ratelimit(&drbd_ratelimit_state))
 		drbd_warn(device, "%s: error=%d s=%llus\n",
-				is_write ? "write" : "read", error,
+				is_write ? (is_discard ? "discard" : "write")
+					: "read", error,
 				(unsigned long long)peer_req->i.sector);
 	if (!error && !uptodate) {
 		if (__ratelimit(&drbd_ratelimit_state))
@@ -263,7 +267,12 @@ void drbd_request_endio(struct bio *bio, int error)
 
 	/* to avoid recursion in __req_mod */
 	if (unlikely(error)) {
-		what = (bio_data_dir(bio) == WRITE)
+		if (bio->bi_rw & REQ_DISCARD)
+			what = (error == -EOPNOTSUPP)
+				? DISCARD_COMPLETED_NOTSUPP
+				: DISCARD_COMPLETED_WITH_ERROR;
+		else
+			what = (bio_data_dir(bio) == WRITE)
 			? WRITE_COMPLETED_WITH_ERROR
 			: (bio_rw(bio) == READ)
 			  ? READ_COMPLETED_WITH_ERROR
@@ -395,7 +404,7 @@ static int read_for_csum(struct drbd_peer_device *peer_device, sector_t sector,
 	/* GFP_TRY, because if there is no memory available right now, this may
 	 * be rescheduled for later. It is "only" background resync, after all. */
 	peer_req = drbd_alloc_peer_req(peer_device, ID_SYNCER /* unused */, sector,
-				       size, GFP_TRY);
+				       size, true /* has real payload */, GFP_TRY);
 	if (!peer_req)
 		goto defer;
 
@@ -492,10 +501,9 @@ struct fifo_buffer *fifo_alloc(int fifo_size)
 	return fb;
 }
 
-static int drbd_rs_controller(struct drbd_device *device)
+static int drbd_rs_controller(struct drbd_device *device, unsigned int sect_in)
 {
 	struct disk_conf *dc;
-	unsigned int sect_in;  /* Number of sectors that came in since the last turn */
 	unsigned int want;     /* The number of sectors we want in the proxy */
 	int req_sect; /* Number of sectors to request in this turn */
 	int correction; /* Number of sectors more we need in the proxy*/
@@ -505,9 +513,6 @@ static int drbd_rs_controller(struct drbd_device *device)
 	int max_sect;
 	struct fifo_buffer *plan;
 
-	sect_in = atomic_xchg(&device->rs_sect_in, 0); /* Number of sectors that came in */
-	device->rs_in_flight -= sect_in;
-
 	dc = rcu_dereference(device->ldev->disk_conf);
 	plan = rcu_dereference(device->rs_plan_s);
 
@@ -550,11 +555,16 @@ static int drbd_rs_controller(struct drbd_device *device)
 
 static int drbd_rs_number_requests(struct drbd_device *device)
 {
-	int number;
+	unsigned int sect_in;  /* Number of sectors that came in since the last turn */
+	int number, mxb;
+
+	sect_in = atomic_xchg(&device->rs_sect_in, 0);
+	device->rs_in_flight -= sect_in;
 
 	rcu_read_lock();
+	mxb = drbd_get_max_buffers(device) / 2;
 	if (rcu_dereference(device->rs_plan_s)->size) {
-		number = drbd_rs_controller(device) >> (BM_BLOCK_SHIFT - 9);
+		number = drbd_rs_controller(device, sect_in) >> (BM_BLOCK_SHIFT - 9);
 		device->c_sync_rate = number * HZ * (BM_BLOCK_SIZE / 1024) / SLEEP_TIME;
 	} else {
 		device->c_sync_rate = rcu_dereference(device->ldev->disk_conf)->resync_rate;
@@ -562,8 +572,14 @@ static int drbd_rs_number_requests(struct drbd_device *device)
 	}
 	rcu_read_unlock();
 
-	/* ignore the amount of pending requests, the resync controller should
-	 * throttle down to incoming reply rate soon enough anyways. */
+	/* Don't have more than "max-buffers"/2 in-flight.
+	 * Otherwise we may cause the remote site to stall on drbd_alloc_pages(),
+	 * potentially causing a distributed deadlock on congestion during
+	 * online-verify or (checksum-based) resync, if max-buffers,
+	 * socket buffer sizes and resync rate settings are mis-configured. */
+	if (mxb - device->rs_in_flight < number)
+		number = mxb - device->rs_in_flight;
+
 	return number;
 }
 
@@ -597,7 +613,7 @@ static int make_resync_request(struct drbd_device *device, int cancel)
 
 	max_bio_size = queue_max_hw_sectors(device->rq_queue) << 9;
 	number = drbd_rs_number_requests(device);
-	if (number == 0)
+	if (number <= 0)
 		goto requeue;
 
 	for (i = 0; i < number; i++) {
@@ -647,7 +663,7 @@ next_sector:
 		 */
 		align = 1;
 		rollback_i = i;
-		for (;;) {
+		while (i < number) {
 			if (size + BM_BLOCK_SIZE > max_bio_size)
 				break;
 
@@ -1670,11 +1686,15 @@ void drbd_start_resync(struct drbd_device *device, enum drbd_conns side)
 	}
 	clear_bit(B_RS_H_DONE, &device->flags);
 
-	write_lock_irq(&global_state_lock);
+	/* req_lock: serialize with drbd_send_and_submit() and others
+	 * global_state_lock: for stable sync-after dependencies */
+	spin_lock_irq(&device->resource->req_lock);
+	write_lock(&global_state_lock);
 	/* Did some connection breakage or IO error race with us? */
 	if (device->state.conn < C_CONNECTED
 	|| !get_ldev_if_state(device, D_NEGOTIATING)) {
-		write_unlock_irq(&global_state_lock);
+		write_unlock(&global_state_lock);
+		spin_unlock_irq(&device->resource->req_lock);
 		mutex_unlock(device->state_mutex);
 		return;
 	}
@@ -1714,7 +1734,8 @@ void drbd_start_resync(struct drbd_device *device, enum drbd_conns side)
 		}
 		_drbd_pause_after(device);
 	}
-	write_unlock_irq(&global_state_lock);
+	write_unlock(&global_state_lock);
+	spin_unlock_irq(&device->resource->req_lock);
 
 	if (r == SS_SUCCESS) {
 		/* reset rs_last_bcast when a resync or verify is started,
@@ -1778,34 +1799,6 @@ void drbd_start_resync(struct drbd_device *device, enum drbd_conns side)
 	mutex_unlock(device->state_mutex);
 }
 
-/* If the resource already closed the current epoch, but we did not
- * (because we have not yet seen new requests), we should send the
- * corresponding barrier now.  Must be checked within the same spinlock
- * that is used to check for new requests. */
-static bool need_to_send_barrier(struct drbd_connection *connection)
-{
-	if (!connection->send.seen_any_write_yet)
-		return false;
-
-	/* Skip barriers that do not contain any writes.
-	 * This may happen during AHEAD mode. */
-	if (!connection->send.current_epoch_writes)
-		return false;
-
-	/* ->req_lock is held when requests are queued on
-	 * connection->sender_work, and put into ->transfer_log.
-	 * It is also held when ->current_tle_nr is increased.
-	 * So either there are already new requests queued,
-	 * and corresponding barriers will be send there.
-	 * Or nothing new is queued yet, so the difference will be 1.
-	 */
-	if (atomic_read(&connection->current_tle_nr) !=
-	    connection->send.current_epoch_nr + 1)
-		return false;
-
-	return true;
-}
-
 static bool dequeue_work_batch(struct drbd_work_queue *queue, struct list_head *work_list)
 {
 	spin_lock_irq(&queue->q_lock);
@@ -1864,12 +1857,22 @@ static void wait_for_work(struct drbd_connection *connection, struct list_head *
 			spin_unlock_irq(&connection->resource->req_lock);
 			break;
 		}
-		send_barrier = need_to_send_barrier(connection);
+
+		/* We found nothing new to do, no to-be-communicated request,
+		 * no other work item.  We may still need to close the last
+		 * epoch.  Next incoming request epoch will be connection ->
+		 * current transfer log epoch number.  If that is different
+		 * from the epoch of the last request we communicated, it is
+		 * safe to send the epoch separating barrier now.
+		 */
+		send_barrier =
+			atomic_read(&connection->current_tle_nr) !=
+			connection->send.current_epoch_nr;
 		spin_unlock_irq(&connection->resource->req_lock);
-		if (send_barrier) {
-			drbd_send_barrier(connection);
-			connection->send.current_epoch_nr++;
-		}
+
+		if (send_barrier)
+			maybe_send_barrier(connection,
+					connection->send.current_epoch_nr + 1);
 		schedule();
 		/* may be woken up for other things but new work, too,
 		 * e.g. if the current epoch got closed.
diff --git a/drivers/block/drbd/drbd_wrappers.h b/drivers/block/drbd/drbd_wrappers.h
deleted file mode 100644
index 3db9ebaf64f..00000000000
--- a/drivers/block/drbd/drbd_wrappers.h
+++ /dev/null
@@ -1,54 +0,0 @@
-#ifndef _DRBD_WRAPPERS_H
-#define _DRBD_WRAPPERS_H
-
-#include <linux/ctype.h>
-#include <linux/mm.h>
-#include "drbd_int.h"
-
-/* see get_sb_bdev and bd_claim */
-extern char *drbd_sec_holder;
-
-/* sets the number of 512 byte sectors of our virtual device */
-static inline void drbd_set_my_capacity(struct drbd_device *device,
-					sector_t size)
-{
-	/* set_capacity(device->this_bdev->bd_disk, size); */
-	set_capacity(device->vdisk, size);
-	device->this_bdev->bd_inode->i_size = (loff_t)size << 9;
-}
-
-#define drbd_bio_uptodate(bio) bio_flagged(bio, BIO_UPTODATE)
-
-/* bi_end_io handlers */
-extern void drbd_md_io_complete(struct bio *bio, int error);
-extern void drbd_peer_request_endio(struct bio *bio, int error);
-extern void drbd_request_endio(struct bio *bio, int error);
-
-/*
- * used to submit our private bio
- */
-static inline void drbd_generic_make_request(struct drbd_device *device,
-					     int fault_type, struct bio *bio)
-{
-	__release(local);
-	if (!bio->bi_bdev) {
-		printk(KERN_ERR "drbd%d: drbd_generic_make_request: "
-				"bio->bi_bdev == NULL\n",
-		       device_to_minor(device));
-		dump_stack();
-		bio_endio(bio, -ENODEV);
-		return;
-	}
-
-	if (drbd_insert_fault(device, fault_type))
-		bio_endio(bio, -EIO);
-	else
-		generic_make_request(bio);
-}
-
-#ifndef __CHECKER__
-# undef __cond_lock
-# define __cond_lock(x,c) (c)
-#endif
-
-#endif
diff --git a/drivers/block/floppy.c b/drivers/block/floppy.c
index 8f5565bf34c..677db049f55 100644
--- a/drivers/block/floppy.c
+++ b/drivers/block/floppy.c
@@ -2351,7 +2351,7 @@ static void rw_interrupt(void)
 	}
 
 	if (CT(COMMAND) != FD_READ ||
-	    raw_cmd->kernel_data == current_req->buffer) {
+	    raw_cmd->kernel_data == bio_data(current_req->bio)) {
 		/* transfer directly from buffer */
 		cont->done(1);
 	} else if (CT(COMMAND) == FD_READ) {
@@ -2640,7 +2640,7 @@ static int make_raw_rw_request(void)
 		raw_cmd->flags &= ~FD_RAW_WRITE;
 		raw_cmd->flags |= FD_RAW_READ;
 		COMMAND = FM_MODE(_floppy, FD_READ);
-	} else if ((unsigned long)current_req->buffer < MAX_DMA_ADDRESS) {
+	} else if ((unsigned long)bio_data(current_req->bio) < MAX_DMA_ADDRESS) {
 		unsigned long dma_limit;
 		int direct, indirect;
 
@@ -2654,13 +2654,13 @@ static int make_raw_rw_request(void)
 		 */
 		max_size = buffer_chain_size();
 		dma_limit = (MAX_DMA_ADDRESS -
-			     ((unsigned long)current_req->buffer)) >> 9;
+			     ((unsigned long)bio_data(current_req->bio))) >> 9;
 		if ((unsigned long)max_size > dma_limit)
 			max_size = dma_limit;
 		/* 64 kb boundaries */
-		if (CROSS_64KB(current_req->buffer, max_size << 9))
+		if (CROSS_64KB(bio_data(current_req->bio), max_size << 9))
 			max_size = (K_64 -
-				    ((unsigned long)current_req->buffer) %
+				    ((unsigned long)bio_data(current_req->bio)) %
 				    K_64) >> 9;
 		direct = transfer_size(ssize, max_sector, max_size) - fsector_t;
 		/*
@@ -2677,7 +2677,7 @@ static int make_raw_rw_request(void)
 		       (DP->read_track & (1 << DRS->probed_format)))))) {
 			max_size = blk_rq_sectors(current_req);
 		} else {
-			raw_cmd->kernel_data = current_req->buffer;
+			raw_cmd->kernel_data = bio_data(current_req->bio);
 			raw_cmd->length = current_count_sectors << 9;
 			if (raw_cmd->length == 0) {
 				DPRINT("%s: zero dma transfer attempted\n", __func__);
@@ -2731,7 +2731,7 @@ static int make_raw_rw_request(void)
 	raw_cmd->length = ((raw_cmd->length - 1) | (ssize - 1)) + 1;
 	raw_cmd->length <<= 9;
 	if ((raw_cmd->length < current_count_sectors << 9) ||
-	    (raw_cmd->kernel_data != current_req->buffer &&
+	    (raw_cmd->kernel_data != bio_data(current_req->bio) &&
 	     CT(COMMAND) == FD_WRITE &&
 	     (aligned_sector_t + (raw_cmd->length >> 9) > buffer_max ||
 	      aligned_sector_t < buffer_min)) ||
@@ -2739,7 +2739,7 @@ static int make_raw_rw_request(void)
 	    raw_cmd->length <= 0 || current_count_sectors <= 0) {
 		DPRINT("fractionary current count b=%lx s=%lx\n",
 		       raw_cmd->length, current_count_sectors);
-		if (raw_cmd->kernel_data != current_req->buffer)
+		if (raw_cmd->kernel_data != bio_data(current_req->bio))
 			pr_info("addr=%d, length=%ld\n",
 				(int)((raw_cmd->kernel_data -
 				       floppy_track_buffer) >> 9),
@@ -2756,7 +2756,7 @@ static int make_raw_rw_request(void)
 		return 0;
 	}
 
-	if (raw_cmd->kernel_data != current_req->buffer) {
+	if (raw_cmd->kernel_data != bio_data(current_req->bio)) {
 		if (raw_cmd->kernel_data < floppy_track_buffer ||
 		    current_count_sectors < 0 ||
 		    raw_cmd->length < 0 ||
@@ -3067,7 +3067,10 @@ static int raw_cmd_copyout(int cmd, void __user *param,
 	int ret;
 
 	while (ptr) {
-		ret = copy_to_user(param, ptr, sizeof(*ptr));
+		struct floppy_raw_cmd cmd = *ptr;
+		cmd.next = NULL;
+		cmd.kernel_data = NULL;
+		ret = copy_to_user(param, &cmd, sizeof(cmd));
 		if (ret)
 			return -EFAULT;
 		param += sizeof(struct floppy_raw_cmd);
@@ -3121,10 +3124,11 @@ loop:
 		return -ENOMEM;
 	*rcmd = ptr;
 	ret = copy_from_user(ptr, param, sizeof(*ptr));
-	if (ret)
-		return -EFAULT;
 	ptr->next = NULL;
 	ptr->buffer_length = 0;
+	ptr->kernel_data = NULL;
+	if (ret)
+		return -EFAULT;
 	param += sizeof(struct floppy_raw_cmd);
 	if (ptr->cmd_count > 33)
 			/* the command may now also take up the space
@@ -3140,7 +3144,6 @@ loop:
 	for (i = 0; i < 16; i++)
 		ptr->reply[i] = 0;
 	ptr->resultcode = 0;
-	ptr->kernel_data = NULL;
 
 	if (ptr->flags & (FD_RAW_READ | FD_RAW_WRITE)) {
 		if (ptr->length <= 0)
@@ -3809,7 +3812,7 @@ static int __floppy_read_block_0(struct block_device *bdev, int drive)
 	bio.bi_iter.bi_size = size;
 	bio.bi_bdev = bdev;
 	bio.bi_iter.bi_sector = 0;
-	bio.bi_flags = (1 << BIO_QUIET);
+	bio.bi_flags |= (1 << BIO_QUIET);
 	bio.bi_private = &cbdata;
 	bio.bi_end_io = floppy_rb0_cb;
 
diff --git a/drivers/block/hd.c b/drivers/block/hd.c
index bf397bf108b..8a290c08262 100644
--- a/drivers/block/hd.c
+++ b/drivers/block/hd.c
@@ -464,11 +464,11 @@ static void read_intr(void)
 
 ok_to_read:
 	req = hd_req;
-	insw(HD_DATA, req->buffer, 256);
+	insw(HD_DATA, bio_data(req->bio), 256);
 #ifdef DEBUG
 	printk("%s: read: sector %ld, remaining = %u, buffer=%p\n",
 	       req->rq_disk->disk_name, blk_rq_pos(req) + 1,
-	       blk_rq_sectors(req) - 1, req->buffer+512);
+	       blk_rq_sectors(req) - 1, bio_data(req->bio)+512);
 #endif
 	if (hd_end_request(0, 512)) {
 		SET_HANDLER(&read_intr);
@@ -505,7 +505,7 @@ static void write_intr(void)
 ok_to_write:
 	if (hd_end_request(0, 512)) {
 		SET_HANDLER(&write_intr);
-		outsw(HD_DATA, req->buffer, 256);
+		outsw(HD_DATA, bio_data(req->bio), 256);
 		return;
 	}
 
@@ -624,7 +624,7 @@ repeat:
 	printk("%s: %sing: CHS=%d/%d/%d, sectors=%d, buffer=%p\n",
 		req->rq_disk->disk_name,
 		req_data_dir(req) == READ ? "read" : "writ",
-		cyl, head, sec, nsect, req->buffer);
+		cyl, head, sec, nsect, bio_data(req->bio));
 #endif
 	if (req->cmd_type == REQ_TYPE_FS) {
 		switch (rq_data_dir(req)) {
@@ -643,7 +643,7 @@ repeat:
 				bad_rw_intr();
 				goto repeat;
 			}
-			outsw(HD_DATA, req->buffer, 256);
+			outsw(HD_DATA, bio_data(req->bio), 256);
 			break;
 		default:
 			printk("unknown hd-command\n");
diff --git a/drivers/block/mg_disk.c b/drivers/block/mg_disk.c
index eb59b124136..e352cac707e 100644
--- a/drivers/block/mg_disk.c
+++ b/drivers/block/mg_disk.c
@@ -479,7 +479,7 @@ static unsigned int mg_out(struct mg_host *host,
 
 static void mg_read_one(struct mg_host *host, struct request *req)
 {
-	u16 *buff = (u16 *)req->buffer;
+	u16 *buff = (u16 *)bio_data(req->bio);
 	u32 i;
 
 	for (i = 0; i < MG_SECTOR_SIZE >> 1; i++)
@@ -496,7 +496,7 @@ static void mg_read(struct request *req)
 		mg_bad_rw_intr(host);
 
 	MG_DBG("requested %d sects (from %ld), buffer=0x%p\n",
-	       blk_rq_sectors(req), blk_rq_pos(req), req->buffer);
+	       blk_rq_sectors(req), blk_rq_pos(req), bio_data(req->bio));
 
 	do {
 		if (mg_wait(host, ATA_DRQ,
@@ -514,7 +514,7 @@ static void mg_read(struct request *req)
 
 static void mg_write_one(struct mg_host *host, struct request *req)
 {
-	u16 *buff = (u16 *)req->buffer;
+	u16 *buff = (u16 *)bio_data(req->bio);
 	u32 i;
 
 	for (i = 0; i < MG_SECTOR_SIZE >> 1; i++)
@@ -534,7 +534,7 @@ static void mg_write(struct request *req)
 	}
 
 	MG_DBG("requested %d sects (from %ld), buffer=0x%p\n",
-	       rem, blk_rq_pos(req), req->buffer);
+	       rem, blk_rq_pos(req), bio_data(req->bio));
 
 	if (mg_wait(host, ATA_DRQ,
 		    MG_TMAX_WAIT_WR_DRQ) != MG_ERR_NONE) {
@@ -585,7 +585,7 @@ ok_to_read:
 	mg_read_one(host, req);
 
 	MG_DBG("sector %ld, remaining=%ld, buffer=0x%p\n",
-	       blk_rq_pos(req), blk_rq_sectors(req) - 1, req->buffer);
+	       blk_rq_pos(req), blk_rq_sectors(req) - 1, bio_data(req->bio));
 
 	/* send read confirm */
 	outb(MG_CMD_RD_CONF, (unsigned long)host->dev_base + MG_REG_COMMAND);
@@ -624,7 +624,7 @@ ok_to_write:
 		/* write 1 sector and set handler if remains */
 		mg_write_one(host, req);
 		MG_DBG("sector %ld, remaining=%ld, buffer=0x%p\n",
-		       blk_rq_pos(req), blk_rq_sectors(req), req->buffer);
+		       blk_rq_pos(req), blk_rq_sectors(req), bio_data(req->bio));
 		host->mg_do_intr = mg_write_intr;
 		mod_timer(&host->timer, jiffies + 3 * HZ);
 	}
diff --git a/drivers/block/mtip32xx/mtip32xx.c b/drivers/block/mtip32xx/mtip32xx.c
index 4fd8d6c1c3d..abc858b3528 100644
--- a/drivers/block/mtip32xx/mtip32xx.c
+++ b/drivers/block/mtip32xx/mtip32xx.c
@@ -31,6 +31,7 @@
 #include <linux/module.h>
 #include <linux/genhd.h>
 #include <linux/blkdev.h>
+#include <linux/blk-mq.h>
 #include <linux/bio.h>
 #include <linux/dma-mapping.h>
 #include <linux/idr.h>
@@ -173,60 +174,34 @@ static bool mtip_check_surprise_removal(struct pci_dev *pdev)
 	return false; /* device present */
 }
 
-/*
- * Obtain an empty command slot.
- *
- * This function needs to be reentrant since it could be called
- * at the same time on multiple CPUs. The allocation of the
- * command slot must be atomic.
- *
- * @port Pointer to the port data structure.
- *
- * return value
- *	>= 0	Index of command slot obtained.
- *	-1	No command slots available.
- */
-static int get_slot(struct mtip_port *port)
+static struct mtip_cmd *mtip_get_int_command(struct driver_data *dd)
 {
-	int slot, i;
-	unsigned int num_command_slots = port->dd->slot_groups * 32;
+	struct request *rq;
 
-	/*
-	 * Try 10 times, because there is a small race here.
-	 *  that's ok, because it's still cheaper than a lock.
-	 *
-	 * Race: Since this section is not protected by lock, same bit
-	 * could be chosen by different process contexts running in
-	 * different processor. So instead of costly lock, we are going
-	 * with loop.
-	 */
-	for (i = 0; i < 10; i++) {
-		slot = find_next_zero_bit(port->allocated,
-					 num_command_slots, 1);
-		if ((slot < num_command_slots) &&
-		    (!test_and_set_bit(slot, port->allocated)))
-			return slot;
-	}
-	dev_warn(&port->dd->pdev->dev, "Failed to get a tag.\n");
+	rq = blk_mq_alloc_request(dd->queue, 0, __GFP_WAIT, true);
+	return blk_mq_rq_to_pdu(rq);
+}
 
-	mtip_check_surprise_removal(port->dd->pdev);
-	return -1;
+static void mtip_put_int_command(struct driver_data *dd, struct mtip_cmd *cmd)
+{
+	blk_put_request(blk_mq_rq_from_pdu(cmd));
 }
 
 /*
- * Release a command slot.
- *
- * @port Pointer to the port data structure.
- * @tag  Tag of command to release
- *
- * return value
- *	None
+ * Once we add support for one hctx per mtip group, this will change a bit
  */
-static inline void release_slot(struct mtip_port *port, int tag)
+static struct request *mtip_rq_from_tag(struct driver_data *dd,
+					unsigned int tag)
+{
+	return blk_mq_tag_to_rq(dd->queue->queue_hw_ctx[0], tag);
+}
+
+static struct mtip_cmd *mtip_cmd_from_tag(struct driver_data *dd,
+					  unsigned int tag)
 {
-	smp_mb__before_atomic();
-	clear_bit(tag, port->allocated);
-	smp_mb__after_atomic();
+	struct request *rq = mtip_rq_from_tag(dd, tag);
+
+	return blk_mq_rq_to_pdu(rq);
 }
 
 /*
@@ -248,93 +223,28 @@ static inline void release_slot(struct mtip_port *port, int tag)
  *	None
  */
 static void mtip_async_complete(struct mtip_port *port,
-				int tag,
-				void *data,
-				int status)
+				int tag, struct mtip_cmd *cmd, int status)
 {
-	struct mtip_cmd *cmd;
-	struct driver_data *dd = data;
-	int unaligned, cb_status = status ? -EIO : 0;
-	void (*func)(void *, int);
+	struct driver_data *dd = port->dd;
+	struct request *rq;
 
 	if (unlikely(!dd) || unlikely(!port))
 		return;
 
-	cmd = &port->commands[tag];
-
 	if (unlikely(status == PORT_IRQ_TF_ERR)) {
 		dev_warn(&port->dd->pdev->dev,
 			"Command tag %d failed due to TFE\n", tag);
 	}
 
-	/* Clear the active flag */
-	atomic_set(&port->commands[tag].active, 0);
+	/* Unmap the DMA scatter list entries */
+	dma_unmap_sg(&dd->pdev->dev, cmd->sg, cmd->scatter_ents, cmd->direction);
 
-	/* Upper layer callback */
-	func = cmd->async_callback;
-	if (likely(func && cmpxchg(&cmd->async_callback, func, 0) == func)) {
+	rq = mtip_rq_from_tag(dd, tag);
 
-		/* Unmap the DMA scatter list entries */
-		dma_unmap_sg(&dd->pdev->dev,
-			cmd->sg,
-			cmd->scatter_ents,
-			cmd->direction);
+	if (unlikely(cmd->unaligned))
+		up(&port->cmd_slot_unal);
 
-		func(cmd->async_data, cb_status);
-		unaligned = cmd->unaligned;
-
-		/* Clear the allocated bit for the command */
-		release_slot(port, tag);
-
-		if (unlikely(unaligned))
-			up(&port->cmd_slot_unal);
-		else
-			up(&port->cmd_slot);
-	}
-}
-
-/*
- * This function is called for clean the pending command in the
- * command slot during the surprise removal of device and return
- * error to the upper layer.
- *
- * @dd Pointer to the DRIVER_DATA structure.
- *
- * return value
- *	None
- */
-static void mtip_command_cleanup(struct driver_data *dd)
-{
-	int tag = 0;
-	struct mtip_cmd *cmd;
-	struct mtip_port *port = dd->port;
-	unsigned int num_cmd_slots = dd->slot_groups * 32;
-
-	if (!test_bit(MTIP_DDF_INIT_DONE_BIT, &dd->dd_flag))
-		return;
-
-	if (!port)
-		return;
-
-	cmd = &port->commands[MTIP_TAG_INTERNAL];
-	if (atomic_read(&cmd->active))
-		if (readl(port->cmd_issue[MTIP_TAG_INTERNAL]) &
-					(1 << MTIP_TAG_INTERNAL))
-			if (cmd->comp_func)
-				cmd->comp_func(port, MTIP_TAG_INTERNAL,
-					 cmd->comp_data, -ENODEV);
-
-	while (1) {
-		tag = find_next_bit(port->allocated, num_cmd_slots, tag);
-		if (tag >= num_cmd_slots)
-			break;
-
-		cmd = &port->commands[tag];
-		if (atomic_read(&cmd->active))
-			mtip_async_complete(port, tag, dd, -ENODEV);
-	}
-
-	set_bit(MTIP_DDF_CLEANUP_BIT, &dd->dd_flag);
+	blk_mq_end_io(rq, status ? -EIO : 0);
 }
 
 /*
@@ -388,8 +298,6 @@ static inline void mtip_issue_ncq_command(struct mtip_port *port, int tag)
 {
 	int group = tag >> 5;
 
-	atomic_set(&port->commands[tag].active, 1);
-
 	/* guard SACT and CI registers */
 	spin_lock(&port->cmd_issue_lock[group]);
 	writel((1 << MTIP_TAG_BIT(tag)),
@@ -397,10 +305,6 @@ static inline void mtip_issue_ncq_command(struct mtip_port *port, int tag)
 	writel((1 << MTIP_TAG_BIT(tag)),
 			port->cmd_issue[MTIP_TAG_INDEX(tag)]);
 	spin_unlock(&port->cmd_issue_lock[group]);
-
-	/* Set the command's timeout value.*/
-	port->commands[tag].comp_time = jiffies + msecs_to_jiffies(
-					MTIP_NCQ_COMMAND_TIMEOUT_MS);
 }
 
 /*
@@ -648,132 +552,13 @@ static void print_tags(struct driver_data *dd,
 
 	memset(tagmap, 0, sizeof(tagmap));
 	for (group = SLOTBITS_IN_LONGS; group > 0; group--)
-		tagmap_len = sprintf(tagmap + tagmap_len, "%016lX ",
+		tagmap_len += sprintf(tagmap + tagmap_len, "%016lX ",
 						tagbits[group-1]);
 	dev_warn(&dd->pdev->dev,
 			"%d command(s) %s: tagmap [%s]", cnt, msg, tagmap);
 }
 
 /*
- * Called periodically to see if any read/write commands are
- * taking too long to complete.
- *
- * @data Pointer to the PORT data structure.
- *
- * return value
- *	None
- */
-static void mtip_timeout_function(unsigned long int data)
-{
-	struct mtip_port *port = (struct mtip_port *) data;
-	struct host_to_dev_fis *fis;
-	struct mtip_cmd *cmd;
-	int unaligned, tag, cmdto_cnt = 0;
-	unsigned int bit, group;
-	unsigned int num_command_slots;
-	unsigned long to, tagaccum[SLOTBITS_IN_LONGS];
-	void (*func)(void *, int);
-
-	if (unlikely(!port))
-		return;
-
-	if (unlikely(port->dd->sr))
-		return;
-
-	if (test_bit(MTIP_DDF_RESUME_BIT, &port->dd->dd_flag)) {
-		mod_timer(&port->cmd_timer,
-			jiffies + msecs_to_jiffies(30000));
-		return;
-	}
-	/* clear the tag accumulator */
-	memset(tagaccum, 0, SLOTBITS_IN_LONGS * sizeof(long));
-	num_command_slots = port->dd->slot_groups * 32;
-
-	for (tag = 0; tag < num_command_slots; tag++) {
-		/*
-		 * Skip internal command slot as it has
-		 * its own timeout mechanism
-		 */
-		if (tag == MTIP_TAG_INTERNAL)
-			continue;
-
-		if (atomic_read(&port->commands[tag].active) &&
-		   (time_after(jiffies, port->commands[tag].comp_time))) {
-			group = tag >> 5;
-			bit = tag & 0x1F;
-
-			cmd = &port->commands[tag];
-			fis = (struct host_to_dev_fis *) cmd->command;
-
-			set_bit(tag, tagaccum);
-			cmdto_cnt++;
-			if (cmdto_cnt == 1)
-				set_bit(MTIP_PF_EH_ACTIVE_BIT, &port->flags);
-
-			/*
-			 * Clear the completed bit. This should prevent
-			 *  any interrupt handlers from trying to retire
-			 *  the command.
-			 */
-			writel(1 << bit, port->completed[group]);
-
-			/* Clear the active flag for the command */
-			atomic_set(&port->commands[tag].active, 0);
-
-			func = cmd->async_callback;
-			if (func &&
-			    cmpxchg(&cmd->async_callback, func, 0) == func) {
-
-				/* Unmap the DMA scatter list entries */
-				dma_unmap_sg(&port->dd->pdev->dev,
-						cmd->sg,
-						cmd->scatter_ents,
-						cmd->direction);
-
-				func(cmd->async_data, -EIO);
-				unaligned = cmd->unaligned;
-
-				/* Clear the allocated bit for the command. */
-				release_slot(port, tag);
-
-				if (unaligned)
-					up(&port->cmd_slot_unal);
-				else
-					up(&port->cmd_slot);
-			}
-		}
-	}
-
-	if (cmdto_cnt) {
-		print_tags(port->dd, "timed out", tagaccum, cmdto_cnt);
-		if (!test_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags)) {
-			mtip_device_reset(port->dd);
-			wake_up_interruptible(&port->svc_wait);
-		}
-		clear_bit(MTIP_PF_EH_ACTIVE_BIT, &port->flags);
-	}
-
-	if (port->ic_pause_timer) {
-		to  = port->ic_pause_timer + msecs_to_jiffies(1000);
-		if (time_after(jiffies, to)) {
-			if (!test_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags)) {
-				port->ic_pause_timer = 0;
-				clear_bit(MTIP_PF_SE_ACTIVE_BIT, &port->flags);
-				clear_bit(MTIP_PF_DM_ACTIVE_BIT, &port->flags);
-				clear_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags);
-				wake_up_interruptible(&port->svc_wait);
-			}
-
-
-		}
-	}
-
-	/* Restart the timer */
-	mod_timer(&port->cmd_timer,
-		jiffies + msecs_to_jiffies(MTIP_TIMEOUT_CHECK_PERIOD));
-}
-
-/*
  * Internal command completion callback function.
  *
  * This function is normally called by the driver ISR when an internal
@@ -789,28 +574,19 @@ static void mtip_timeout_function(unsigned long int data)
  *	None
  */
 static void mtip_completion(struct mtip_port *port,
-			    int tag,
-			    void *data,
-			    int status)
+			    int tag, struct mtip_cmd *command, int status)
 {
-	struct mtip_cmd *command = &port->commands[tag];
-	struct completion *waiting = data;
+	struct completion *waiting = command->comp_data;
 	if (unlikely(status == PORT_IRQ_TF_ERR))
 		dev_warn(&port->dd->pdev->dev,
 			"Internal command %d completed with TFE\n", tag);
 
-	command->async_callback = NULL;
-	command->comp_func = NULL;
-
 	complete(waiting);
 }
 
 static void mtip_null_completion(struct mtip_port *port,
-			    int tag,
-			    void *data,
-			    int status)
+			    int tag, struct mtip_cmd *command, int status)
 {
-	return;
 }
 
 static int mtip_read_log_page(struct mtip_port *port, u8 page, u16 *buffer,
@@ -842,19 +618,16 @@ static void mtip_handle_tfe(struct driver_data *dd)
 
 	port = dd->port;
 
-	/* Stop the timer to prevent command timeouts. */
-	del_timer(&port->cmd_timer);
 	set_bit(MTIP_PF_EH_ACTIVE_BIT, &port->flags);
 
 	if (test_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags) &&
 			test_bit(MTIP_TAG_INTERNAL, port->allocated)) {
-		cmd = &port->commands[MTIP_TAG_INTERNAL];
+		cmd = mtip_cmd_from_tag(dd, MTIP_TAG_INTERNAL);
 		dbg_printk(MTIP_DRV_NAME " TFE for the internal command\n");
 
-		atomic_inc(&cmd->active); /* active > 1 indicates error */
 		if (cmd->comp_data && cmd->comp_func) {
 			cmd->comp_func(port, MTIP_TAG_INTERNAL,
-					cmd->comp_data, PORT_IRQ_TF_ERR);
+					cmd, PORT_IRQ_TF_ERR);
 		}
 		goto handle_tfe_exit;
 	}
@@ -866,6 +639,8 @@ static void mtip_handle_tfe(struct driver_data *dd)
 	for (group = 0; group < dd->slot_groups; group++) {
 		completed = readl(port->completed[group]);
 
+		dev_warn(&dd->pdev->dev, "g=%u, comp=%x\n", group, completed);
+
 		/* clear completed status register in the hardware.*/
 		writel(completed, port->completed[group]);
 
@@ -879,15 +654,11 @@ static void mtip_handle_tfe(struct driver_data *dd)
 			if (tag == MTIP_TAG_INTERNAL)
 				continue;
 
-			cmd = &port->commands[tag];
+			cmd = mtip_cmd_from_tag(dd, tag);
 			if (likely(cmd->comp_func)) {
 				set_bit(tag, tagaccum);
 				cmd_cnt++;
-				atomic_set(&cmd->active, 0);
-				cmd->comp_func(port,
-					 tag,
-					 cmd->comp_data,
-					 0);
+				cmd->comp_func(port, tag, cmd, 0);
 			} else {
 				dev_err(&port->dd->pdev->dev,
 					"Missing completion func for tag %d",
@@ -947,11 +718,7 @@ static void mtip_handle_tfe(struct driver_data *dd)
 		for (bit = 0; bit < 32; bit++) {
 			reissue = 1;
 			tag = (group << 5) + bit;
-			cmd = &port->commands[tag];
-
-			/* If the active bit is set re-issue the command */
-			if (atomic_read(&cmd->active) == 0)
-				continue;
+			cmd = mtip_cmd_from_tag(dd, tag);
 
 			fis = (struct host_to_dev_fis *)cmd->command;
 
@@ -970,11 +737,9 @@ static void mtip_handle_tfe(struct driver_data *dd)
 					tag,
 					fail_reason != NULL ?
 						fail_reason : "unknown");
-					atomic_set(&cmd->active, 0);
 					if (cmd->comp_func) {
 						cmd->comp_func(port, tag,
-							cmd->comp_data,
-							-ENODATA);
+							cmd, -ENODATA);
 					}
 					continue;
 				}
@@ -997,14 +762,9 @@ static void mtip_handle_tfe(struct driver_data *dd)
 			/* Retire a command that will not be reissued */
 			dev_warn(&port->dd->pdev->dev,
 				"retiring tag %d\n", tag);
-			atomic_set(&cmd->active, 0);
 
 			if (cmd->comp_func)
-				cmd->comp_func(
-					port,
-					tag,
-					cmd->comp_data,
-					PORT_IRQ_TF_ERR);
+				cmd->comp_func(port, tag, cmd, PORT_IRQ_TF_ERR);
 			else
 				dev_warn(&port->dd->pdev->dev,
 					"Bad completion for tag %d\n",
@@ -1017,9 +777,6 @@ handle_tfe_exit:
 	/* clear eh_active */
 	clear_bit(MTIP_PF_EH_ACTIVE_BIT, &port->flags);
 	wake_up_interruptible(&port->svc_wait);
-
-	mod_timer(&port->cmd_timer,
-		 jiffies + msecs_to_jiffies(MTIP_TIMEOUT_CHECK_PERIOD));
 }
 
 /*
@@ -1048,15 +805,10 @@ static inline void mtip_workq_sdbfx(struct mtip_port *port, int group,
 			if (unlikely(tag == MTIP_TAG_INTERNAL))
 				continue;
 
-			command = &port->commands[tag];
-			/* make internal callback */
-			if (likely(command->comp_func)) {
-				command->comp_func(
-					port,
-					tag,
-					command->comp_data,
-					0);
-			} else {
+			command = mtip_cmd_from_tag(dd, tag);
+			if (likely(command->comp_func))
+				command->comp_func(port, tag, command, 0);
+			else {
 				dev_dbg(&dd->pdev->dev,
 					"Null completion for tag %d",
 					tag);
@@ -1081,16 +833,13 @@ static inline void mtip_workq_sdbfx(struct mtip_port *port, int group,
 static inline void mtip_process_legacy(struct driver_data *dd, u32 port_stat)
 {
 	struct mtip_port *port = dd->port;
-	struct mtip_cmd *cmd = &port->commands[MTIP_TAG_INTERNAL];
+	struct mtip_cmd *cmd = mtip_cmd_from_tag(dd, MTIP_TAG_INTERNAL);
 
 	if (test_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags) &&
 	    (cmd != NULL) && !(readl(port->cmd_issue[MTIP_TAG_INTERNAL])
 		& (1 << MTIP_TAG_INTERNAL))) {
 		if (cmd->comp_func) {
-			cmd->comp_func(port,
-				MTIP_TAG_INTERNAL,
-				cmd->comp_data,
-				0);
+			cmd->comp_func(port, MTIP_TAG_INTERNAL, cmd, 0);
 			return;
 		}
 	}
@@ -1103,8 +852,6 @@ static inline void mtip_process_legacy(struct driver_data *dd, u32 port_stat)
  */
 static inline void mtip_process_errors(struct driver_data *dd, u32 port_stat)
 {
-	if (likely(port_stat & (PORT_IRQ_TF_ERR | PORT_IRQ_IF_ERR)))
-		mtip_handle_tfe(dd);
 
 	if (unlikely(port_stat & PORT_IRQ_CONNECT)) {
 		dev_warn(&dd->pdev->dev,
@@ -1122,6 +869,12 @@ static inline void mtip_process_errors(struct driver_data *dd, u32 port_stat)
 		dev_warn(&dd->pdev->dev,
 			"Port stat errors %x unhandled\n",
 			(port_stat & ~PORT_IRQ_HANDLED));
+		if (mtip_check_surprise_removal(dd->pdev))
+			return;
+	}
+	if (likely(port_stat & (PORT_IRQ_TF_ERR | PORT_IRQ_IF_ERR))) {
+		set_bit(MTIP_PF_EH_ACTIVE_BIT, &dd->port->flags);
+		wake_up_interruptible(&dd->port->svc_wait);
 	}
 }
 
@@ -1222,7 +975,6 @@ static irqreturn_t mtip_irq_handler(int irq, void *instance)
 
 static void mtip_issue_non_ncq_command(struct mtip_port *port, int tag)
 {
-	atomic_set(&port->commands[tag].active, 1);
 	writel(1 << MTIP_TAG_BIT(tag),
 		port->cmd_issue[MTIP_TAG_INDEX(tag)]);
 }
@@ -1280,6 +1032,8 @@ static int mtip_quiesce_io(struct mtip_port *port, unsigned long timeout)
 	unsigned int n;
 	unsigned int active = 1;
 
+	blk_mq_stop_hw_queues(port->dd->queue);
+
 	to = jiffies + msecs_to_jiffies(timeout);
 	do {
 		if (test_bit(MTIP_PF_SVC_THD_ACTIVE_BIT, &port->flags) &&
@@ -1287,8 +1041,13 @@ static int mtip_quiesce_io(struct mtip_port *port, unsigned long timeout)
 			msleep(20);
 			continue; /* svc thd is actively issuing commands */
 		}
+
+		msleep(100);
+		if (mtip_check_surprise_removal(port->dd->pdev))
+			goto err_fault;
 		if (test_bit(MTIP_DDF_REMOVE_PENDING_BIT, &port->dd->dd_flag))
-			return -EFAULT;
+			goto err_fault;
+
 		/*
 		 * Ignore s_active bit 0 of array element 0.
 		 * This bit will always be set
@@ -1299,11 +1058,13 @@ static int mtip_quiesce_io(struct mtip_port *port, unsigned long timeout)
 
 		if (!active)
 			break;
-
-		msleep(20);
 	} while (time_before(jiffies, to));
 
+	blk_mq_start_stopped_hw_queues(port->dd->queue, true);
 	return active ? -EBUSY : 0;
+err_fault:
+	blk_mq_start_stopped_hw_queues(port->dd->queue, true);
+	return -EFAULT;
 }
 
 /*
@@ -1335,10 +1096,9 @@ static int mtip_exec_internal_command(struct mtip_port *port,
 {
 	struct mtip_cmd_sg *command_sg;
 	DECLARE_COMPLETION_ONSTACK(wait);
-	int rv = 0, ready2go = 1;
-	struct mtip_cmd *int_cmd = &port->commands[MTIP_TAG_INTERNAL];
-	unsigned long to;
+	struct mtip_cmd *int_cmd;
 	struct driver_data *dd = port->dd;
+	int rv = 0;
 
 	/* Make sure the buffer is 8 byte aligned. This is asic specific. */
 	if (buffer & 0x00000007) {
@@ -1346,19 +1106,8 @@ static int mtip_exec_internal_command(struct mtip_port *port,
 		return -EFAULT;
 	}
 
-	to = jiffies + msecs_to_jiffies(timeout);
-	do {
-		ready2go = !test_and_set_bit(MTIP_TAG_INTERNAL,
-						port->allocated);
-		if (ready2go)
-			break;
-		mdelay(100);
-	} while (time_before(jiffies, to));
-	if (!ready2go) {
-		dev_warn(&dd->pdev->dev,
-			"Internal cmd active. new cmd [%02X]\n", fis->command);
-		return -EBUSY;
-	}
+	int_cmd = mtip_get_int_command(dd);
+
 	set_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags);
 	port->ic_pause_timer = 0;
 
@@ -1368,10 +1117,11 @@ static int mtip_exec_internal_command(struct mtip_port *port,
 	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) {
+			if (mtip_quiesce_io(port,
+					MTIP_QUIESCE_IO_TIMEOUT_MS) < 0) {
 				dev_warn(&dd->pdev->dev,
 					"Failed to quiesce IO\n");
-				release_slot(port, MTIP_TAG_INTERNAL);
+				mtip_put_int_command(dd, int_cmd);
 				clear_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags);
 				wake_up_interruptible(&port->svc_wait);
 				return -EBUSY;
@@ -1416,9 +1166,9 @@ static int mtip_exec_internal_command(struct mtip_port *port,
 
 	if (atomic == GFP_KERNEL) {
 		/* Wait for the command to complete or timeout. */
-		if (wait_for_completion_interruptible_timeout(
+		if ((rv = wait_for_completion_interruptible_timeout(
 				&wait,
-				msecs_to_jiffies(timeout)) <= 0) {
+				msecs_to_jiffies(timeout))) <= 0) {
 			if (rv == -ERESTARTSYS) { /* interrupted */
 				dev_err(&dd->pdev->dev,
 					"Internal command [%02X] was interrupted after %lu ms\n",
@@ -1497,8 +1247,7 @@ static int mtip_exec_internal_command(struct mtip_port *port,
 	}
 exec_ic_exit:
 	/* Clear the allocated and active bits for the internal command. */
-	atomic_set(&int_cmd->active, 0);
-	release_slot(port, MTIP_TAG_INTERNAL);
+	mtip_put_int_command(dd, int_cmd);
 	if (rv >= 0 && mtip_pause_ncq(port, fis)) {
 		/* NCQ paused */
 		return rv;
@@ -1529,6 +1278,37 @@ static inline void ata_swap_string(u16 *buf, unsigned int len)
 		be16_to_cpus(&buf[i]);
 }
 
+static void mtip_set_timeout(struct driver_data *dd,
+					struct host_to_dev_fis *fis,
+					unsigned int *timeout, u8 erasemode)
+{
+	switch (fis->command) {
+	case ATA_CMD_DOWNLOAD_MICRO:
+		*timeout = 120000; /* 2 minutes */
+		break;
+	case ATA_CMD_SEC_ERASE_UNIT:
+	case 0xFC:
+		if (erasemode)
+			*timeout = ((*(dd->port->identify + 90) * 2) * 60000);
+		else
+			*timeout = ((*(dd->port->identify + 89) * 2) * 60000);
+		break;
+	case ATA_CMD_STANDBYNOW1:
+		*timeout = 120000;  /* 2 minutes */
+		break;
+	case 0xF7:
+	case 0xFA:
+		*timeout = 60000;  /* 60 seconds */
+		break;
+	case ATA_CMD_SMART:
+		*timeout = 15000;  /* 15 seconds */
+		break;
+	default:
+		*timeout = MTIP_IOCTL_CMD_TIMEOUT_MS;
+		break;
+	}
+}
+
 /*
  * Request the device identity information.
  *
@@ -1576,7 +1356,7 @@ static int mtip_get_identify(struct mtip_port *port, void __user *user_buffer)
 				sizeof(u16) * ATA_ID_WORDS,
 				0,
 				GFP_KERNEL,
-				MTIP_INTERNAL_COMMAND_TIMEOUT_MS)
+				MTIP_INT_CMD_TIMEOUT_MS)
 				< 0) {
 		rv = -1;
 		goto out;
@@ -1644,6 +1424,7 @@ static int mtip_standby_immediate(struct mtip_port *port)
 	int rv;
 	struct host_to_dev_fis	fis;
 	unsigned long start;
+	unsigned int timeout;
 
 	/* Build the FIS. */
 	memset(&fis, 0, sizeof(struct host_to_dev_fis));
@@ -1651,6 +1432,8 @@ static int mtip_standby_immediate(struct mtip_port *port)
 	fis.opts	= 1 << 7;
 	fis.command	= ATA_CMD_STANDBYNOW1;
 
+	mtip_set_timeout(port->dd, &fis, &timeout, 0);
+
 	start = jiffies;
 	rv = mtip_exec_internal_command(port,
 					&fis,
@@ -1659,7 +1442,7 @@ static int mtip_standby_immediate(struct mtip_port *port)
 					0,
 					0,
 					GFP_ATOMIC,
-					15000);
+					timeout);
 	dbg_printk(MTIP_DRV_NAME "Time taken to complete standby cmd: %d ms\n",
 			jiffies_to_msecs(jiffies - start));
 	if (rv)
@@ -1705,7 +1488,7 @@ static int mtip_read_log_page(struct mtip_port *port, u8 page, u16 *buffer,
 					sectors * ATA_SECT_SIZE,
 					0,
 					GFP_ATOMIC,
-					MTIP_INTERNAL_COMMAND_TIMEOUT_MS);
+					MTIP_INT_CMD_TIMEOUT_MS);
 }
 
 /*
@@ -1998,6 +1781,7 @@ static int exec_drive_task(struct mtip_port *port, u8 *command)
 {
 	struct host_to_dev_fis	fis;
 	struct host_to_dev_fis *reply = (port->rxfis + RX_FIS_D2H_REG);
+	unsigned int to;
 
 	/* Build the FIS. */
 	memset(&fis, 0, sizeof(struct host_to_dev_fis));
@@ -2011,6 +1795,8 @@ static int exec_drive_task(struct mtip_port *port, u8 *command)
 	fis.cyl_hi	= command[5];
 	fis.device	= command[6] & ~0x10; /* Clear the dev bit*/
 
+	mtip_set_timeout(port->dd, &fis, &to, 0);
+
 	dbg_printk(MTIP_DRV_NAME " %s: User Command: cmd %x, feat %x, nsect %x, sect %x, lcyl %x, hcyl %x, sel %x\n",
 		__func__,
 		command[0],
@@ -2029,7 +1815,7 @@ static int exec_drive_task(struct mtip_port *port, u8 *command)
 				 0,
 				 0,
 				 GFP_KERNEL,
-				 MTIP_IOCTL_COMMAND_TIMEOUT_MS) < 0) {
+				 to) < 0) {
 		return -1;
 	}
 
@@ -2069,6 +1855,7 @@ static int exec_drive_command(struct mtip_port *port, u8 *command,
 	u8 *buf = NULL;
 	dma_addr_t dma_addr = 0;
 	int rv = 0, xfer_sz = command[3];
+	unsigned int to;
 
 	if (xfer_sz) {
 		if (!user_buffer)
@@ -2100,6 +1887,8 @@ static int exec_drive_command(struct mtip_port *port, u8 *command,
 		fis.cyl_hi	= 0xC2;
 	}
 
+	mtip_set_timeout(port->dd, &fis, &to, 0);
+
 	if (xfer_sz)
 		reply = (port->rxfis + RX_FIS_PIO_SETUP);
 	else
@@ -2122,7 +1911,7 @@ static int exec_drive_command(struct mtip_port *port, u8 *command,
 				 (xfer_sz ? ATA_SECT_SIZE * xfer_sz : 0),
 				 0,
 				 GFP_KERNEL,
-				 MTIP_IOCTL_COMMAND_TIMEOUT_MS)
+				 to)
 				 < 0) {
 		rv = -EFAULT;
 		goto exit_drive_command;
@@ -2202,36 +1991,6 @@ static unsigned int implicit_sector(unsigned char command,
 	}
 	return rv;
 }
-static void mtip_set_timeout(struct driver_data *dd,
-					struct host_to_dev_fis *fis,
-					unsigned int *timeout, u8 erasemode)
-{
-	switch (fis->command) {
-	case ATA_CMD_DOWNLOAD_MICRO:
-		*timeout = 120000; /* 2 minutes */
-		break;
-	case ATA_CMD_SEC_ERASE_UNIT:
-	case 0xFC:
-		if (erasemode)
-			*timeout = ((*(dd->port->identify + 90) * 2) * 60000);
-		else
-			*timeout = ((*(dd->port->identify + 89) * 2) * 60000);
-		break;
-	case ATA_CMD_STANDBYNOW1:
-		*timeout = 120000;  /* 2 minutes */
-		break;
-	case 0xF7:
-	case 0xFA:
-		*timeout = 60000;  /* 60 seconds */
-		break;
-	case ATA_CMD_SMART:
-		*timeout = 15000;  /* 15 seconds */
-		break;
-	default:
-		*timeout = MTIP_IOCTL_COMMAND_TIMEOUT_MS;
-		break;
-	}
-}
 
 /*
  * Executes a taskfile
@@ -2606,22 +2365,21 @@ static int mtip_hw_ioctl(struct driver_data *dd, unsigned int cmd,
  * return value
  *	None
  */
-static void mtip_hw_submit_io(struct driver_data *dd, sector_t sector,
-			      int nsect, int nents, int tag, void *callback,
-			      void *data, int dir, int unaligned)
+static void mtip_hw_submit_io(struct driver_data *dd, struct request *rq,
+			      struct mtip_cmd *command, int nents,
+			      struct blk_mq_hw_ctx *hctx)
 {
 	struct host_to_dev_fis	*fis;
 	struct mtip_port *port = dd->port;
-	struct mtip_cmd *command = &port->commands[tag];
-	int dma_dir = (dir == READ) ? DMA_FROM_DEVICE : DMA_TO_DEVICE;
-	u64 start = sector;
+	int dma_dir = rq_data_dir(rq) == READ ? DMA_FROM_DEVICE : DMA_TO_DEVICE;
+	u64 start = blk_rq_pos(rq);
+	unsigned int nsect = blk_rq_sectors(rq);
 
 	/* Map the scatter list for DMA access */
 	nents = dma_map_sg(&dd->pdev->dev, command->sg, nents, dma_dir);
 
 	command->scatter_ents = nents;
 
-	command->unaligned = unaligned;
 	/*
 	 * The number of retries for this command before it is
 	 * reported as a failure to the upper layers.
@@ -2632,8 +2390,10 @@ static void mtip_hw_submit_io(struct driver_data *dd, sector_t sector,
 	fis = command->command;
 	fis->type        = 0x27;
 	fis->opts        = 1 << 7;
-	fis->command     =
-		(dir == READ ? ATA_CMD_FPDMA_READ : ATA_CMD_FPDMA_WRITE);
+	if (rq_data_dir(rq) == READ)
+		fis->command = ATA_CMD_FPDMA_READ;
+	else
+		fis->command = ATA_CMD_FPDMA_WRITE;
 	fis->lba_low     = start & 0xFF;
 	fis->lba_mid     = (start >> 8) & 0xFF;
 	fis->lba_hi      = (start >> 16) & 0xFF;
@@ -2643,14 +2403,14 @@ static void mtip_hw_submit_io(struct driver_data *dd, sector_t sector,
 	fis->device	 = 1 << 6;
 	fis->features    = nsect & 0xFF;
 	fis->features_ex = (nsect >> 8) & 0xFF;
-	fis->sect_count  = ((tag << 3) | (tag >> 5));
+	fis->sect_count  = ((rq->tag << 3) | (rq->tag >> 5));
 	fis->sect_cnt_ex = 0;
 	fis->control     = 0;
 	fis->res2        = 0;
 	fis->res3        = 0;
 	fill_command_sg(dd, command, nents);
 
-	if (unaligned)
+	if (command->unaligned)
 		fis->device |= 1 << 7;
 
 	/* Populate the command header */
@@ -2668,81 +2428,17 @@ static void mtip_hw_submit_io(struct driver_data *dd, sector_t sector,
 	command->direction = dma_dir;
 
 	/*
-	 * Set the completion function and data for the command passed
-	 * from the upper layer.
-	 */
-	command->async_data = data;
-	command->async_callback = callback;
-
-	/*
 	 * To prevent this command from being issued
 	 * if an internal command is in progress or error handling is active.
 	 */
 	if (port->flags & MTIP_PF_PAUSE_IO) {
-		set_bit(tag, port->cmds_to_issue);
+		set_bit(rq->tag, port->cmds_to_issue);
 		set_bit(MTIP_PF_ISSUE_CMDS_BIT, &port->flags);
 		return;
 	}
 
 	/* Issue the command to the hardware */
-	mtip_issue_ncq_command(port, tag);
-
-	return;
-}
-
-/*
- * Release a command slot.
- *
- * @dd  Pointer to the driver data structure.
- * @tag Slot tag
- *
- * return value
- *      None
- */
-static void mtip_hw_release_scatterlist(struct driver_data *dd, int tag,
-								int unaligned)
-{
-	struct semaphore *sem = unaligned ? &dd->port->cmd_slot_unal :
-							&dd->port->cmd_slot;
-	release_slot(dd->port, tag);
-	up(sem);
-}
-
-/*
- * Obtain a command slot and return its associated scatter list.
- *
- * @dd  Pointer to the driver data structure.
- * @tag Pointer to an int that will receive the allocated command
- *            slot tag.
- *
- * return value
- *	Pointer to the scatter list for the allocated command slot
- *	or NULL if no command slots are available.
- */
-static struct scatterlist *mtip_hw_get_scatterlist(struct driver_data *dd,
-						   int *tag, int unaligned)
-{
-	struct semaphore *sem = unaligned ? &dd->port->cmd_slot_unal :
-							&dd->port->cmd_slot;
-
-	/*
-	 * It is possible that, even with this semaphore, a thread
-	 * may think that no command slots are available. Therefore, we
-	 * need to make an attempt to get_slot().
-	 */
-	down(sem);
-	*tag = get_slot(dd->port);
-
-	if (unlikely(test_bit(MTIP_DDF_REMOVE_PENDING_BIT, &dd->dd_flag))) {
-		up(sem);
-		return NULL;
-	}
-	if (unlikely(*tag < 0)) {
-		up(sem);
-		return NULL;
-	}
-
-	return dd->port->commands[*tag].sg;
+	mtip_issue_ncq_command(port, rq->tag);
 }
 
 /*
@@ -3113,6 +2809,7 @@ static int mtip_free_orphan(struct driver_data *dd)
 		if (dd->queue) {
 			dd->queue->queuedata = NULL;
 			blk_cleanup_queue(dd->queue);
+			blk_mq_free_tag_set(&dd->tags);
 			dd->queue = NULL;
 		}
 	}
@@ -3270,6 +2967,11 @@ static int mtip_service_thread(void *data)
 	int ret;
 
 	while (1) {
+		if (kthread_should_stop() ||
+			test_bit(MTIP_PF_SVC_THD_STOP_BIT, &port->flags))
+			goto st_out;
+		clear_bit(MTIP_PF_SVC_THD_ACTIVE_BIT, &port->flags);
+
 		/*
 		 * the condition is to check neither an internal command is
 		 * is in progress nor error handling is active
@@ -3277,11 +2979,12 @@ static int mtip_service_thread(void *data)
 		wait_event_interruptible(port->svc_wait, (port->flags) &&
 			!(port->flags & MTIP_PF_PAUSE_IO));
 
-		if (kthread_should_stop())
-			goto st_out;
-
 		set_bit(MTIP_PF_SVC_THD_ACTIVE_BIT, &port->flags);
 
+		if (kthread_should_stop() ||
+			test_bit(MTIP_PF_SVC_THD_STOP_BIT, &port->flags))
+			goto st_out;
+
 		/* If I am an orphan, start self cleanup */
 		if (test_bit(MTIP_PF_SR_CLEANUP_BIT, &port->flags))
 			break;
@@ -3290,6 +2993,16 @@ static int mtip_service_thread(void *data)
 				&dd->dd_flag)))
 			goto st_out;
 
+restart_eh:
+		/* Demux bits: start with error handling */
+		if (test_bit(MTIP_PF_EH_ACTIVE_BIT, &port->flags)) {
+			mtip_handle_tfe(dd);
+			clear_bit(MTIP_PF_EH_ACTIVE_BIT, &port->flags);
+		}
+
+		if (test_bit(MTIP_PF_EH_ACTIVE_BIT, &port->flags))
+			goto restart_eh;
+
 		if (test_bit(MTIP_PF_ISSUE_CMDS_BIT, &port->flags)) {
 			slot = 1;
 			/* used to restrict the loop to one iteration */
@@ -3319,16 +3032,14 @@ static int mtip_service_thread(void *data)
 			}
 
 			clear_bit(MTIP_PF_ISSUE_CMDS_BIT, &port->flags);
-		} else if (test_bit(MTIP_PF_REBUILD_BIT, &port->flags)) {
+		}
+
+		if (test_bit(MTIP_PF_REBUILD_BIT, &port->flags)) {
 			if (mtip_ftl_rebuild_poll(dd) < 0)
 				set_bit(MTIP_DDF_REBUILD_FAILED_BIT,
 							&dd->dd_flag);
 			clear_bit(MTIP_PF_REBUILD_BIT, &port->flags);
 		}
-		clear_bit(MTIP_PF_SVC_THD_ACTIVE_BIT, &port->flags);
-
-		if (test_bit(MTIP_PF_SVC_THD_STOP_BIT, &port->flags))
-			goto st_out;
 	}
 
 	/* wait for pci remove to exit */
@@ -3365,7 +3076,6 @@ st_out:
  */
 static void mtip_dma_free(struct driver_data *dd)
 {
-	int i;
 	struct mtip_port *port = dd->port;
 
 	if (port->block1)
@@ -3376,13 +3086,6 @@ static void mtip_dma_free(struct driver_data *dd)
 		dmam_free_coherent(&dd->pdev->dev, AHCI_CMD_TBL_SZ,
 				port->command_list, port->command_list_dma);
 	}
-
-	for (i = 0; i < MTIP_MAX_COMMAND_SLOTS; i++) {
-		if (port->commands[i].command)
-			dmam_free_coherent(&dd->pdev->dev, CMD_DMA_ALLOC_SZ,
-				port->commands[i].command,
-				port->commands[i].command_dma);
-	}
 }
 
 /*
@@ -3396,8 +3099,6 @@ static void mtip_dma_free(struct driver_data *dd)
 static int mtip_dma_alloc(struct driver_data *dd)
 {
 	struct mtip_port *port = dd->port;
-	int i, rv = 0;
-	u32 host_cap_64 = readl(dd->mmio + HOST_CAP) & HOST_CAP_64;
 
 	/* Allocate dma memory for RX Fis, Identify, and Sector Bufffer */
 	port->block1 =
@@ -3430,41 +3131,63 @@ static int mtip_dma_alloc(struct driver_data *dd)
 	port->smart_buf     = port->block1 + AHCI_SMARTBUF_OFFSET;
 	port->smart_buf_dma = port->block1_dma + AHCI_SMARTBUF_OFFSET;
 
-	/* Setup per command SGL DMA region */
-
-	/* Point the command headers at the command tables */
-	for (i = 0; i < MTIP_MAX_COMMAND_SLOTS; i++) {
-		port->commands[i].command =
-			dmam_alloc_coherent(&dd->pdev->dev, CMD_DMA_ALLOC_SZ,
-				&port->commands[i].command_dma, GFP_KERNEL);
-		if (!port->commands[i].command) {
-			rv = -ENOMEM;
-			mtip_dma_free(dd);
-			return rv;
-		}
-		memset(port->commands[i].command, 0, CMD_DMA_ALLOC_SZ);
-
-		port->commands[i].command_header = port->command_list +
-					(sizeof(struct mtip_cmd_hdr) * i);
-		port->commands[i].command_header_dma =
-					dd->port->command_list_dma +
-					(sizeof(struct mtip_cmd_hdr) * i);
+	return 0;
+}
 
-		if (host_cap_64)
-			port->commands[i].command_header->ctbau =
-				__force_bit2int cpu_to_le32(
-				(port->commands[i].command_dma >> 16) >> 16);
+static int mtip_hw_get_identify(struct driver_data *dd)
+{
+	struct smart_attr attr242;
+	unsigned char *buf;
+	int rv;
 
-		port->commands[i].command_header->ctba =
-				__force_bit2int cpu_to_le32(
-				port->commands[i].command_dma & 0xFFFFFFFF);
+	if (mtip_get_identify(dd->port, NULL) < 0)
+		return -EFAULT;
 
-		sg_init_table(port->commands[i].sg, MTIP_MAX_SG);
+	if (*(dd->port->identify + MTIP_FTL_REBUILD_OFFSET) ==
+		MTIP_FTL_REBUILD_MAGIC) {
+		set_bit(MTIP_PF_REBUILD_BIT, &dd->port->flags);
+		return MTIP_FTL_REBUILD_MAGIC;
+	}
+	mtip_dump_identify(dd->port);
 
-		/* Mark command as currently inactive */
-		atomic_set(&dd->port->commands[i].active, 0);
+	/* check write protect, over temp and rebuild statuses */
+	rv = mtip_read_log_page(dd->port, ATA_LOG_SATA_NCQ,
+				dd->port->log_buf,
+				dd->port->log_buf_dma, 1);
+	if (rv) {
+		dev_warn(&dd->pdev->dev,
+			"Error in READ LOG EXT (10h) command\n");
+		/* non-critical error, don't fail the load */
+	} else {
+		buf = (unsigned char *)dd->port->log_buf;
+		if (buf[259] & 0x1) {
+			dev_info(&dd->pdev->dev,
+				"Write protect bit is set.\n");
+			set_bit(MTIP_DDF_WRITE_PROTECT_BIT, &dd->dd_flag);
+		}
+		if (buf[288] == 0xF7) {
+			dev_info(&dd->pdev->dev,
+				"Exceeded Tmax, drive in thermal shutdown.\n");
+			set_bit(MTIP_DDF_OVER_TEMP_BIT, &dd->dd_flag);
+		}
+		if (buf[288] == 0xBF) {
+			dev_info(&dd->pdev->dev,
+				"Drive indicates rebuild has failed.\n");
+			/* TODO */
+		}
 	}
-	return 0;
+
+	/* get write protect progess */
+	memset(&attr242, 0, sizeof(struct smart_attr));
+	if (mtip_get_smart_attr(dd->port, 242, &attr242))
+		dev_warn(&dd->pdev->dev,
+				"Unable to check write protect progress\n");
+	else
+		dev_info(&dd->pdev->dev,
+				"Write protect progress: %u%% (%u blocks)\n",
+				attr242.cur, le32_to_cpu(attr242.data));
+
+	return rv;
 }
 
 /*
@@ -3481,8 +3204,6 @@ static int mtip_hw_init(struct driver_data *dd)
 	int rv;
 	unsigned int num_command_slots;
 	unsigned long timeout, timetaken;
-	unsigned char *buf;
-	struct smart_attr attr242;
 
 	dd->mmio = pcim_iomap_table(dd->pdev)[MTIP_ABAR];
 
@@ -3513,8 +3234,6 @@ static int mtip_hw_init(struct driver_data *dd)
 	else
 		dd->unal_qdepth = 0;
 
-	/* Counting semaphore to track command slot usage */
-	sema_init(&dd->port->cmd_slot, num_command_slots - 1 - dd->unal_qdepth);
 	sema_init(&dd->port->cmd_slot_unal, dd->unal_qdepth);
 
 	/* Spinlock to prevent concurrent issue */
@@ -3599,73 +3318,16 @@ static int mtip_hw_init(struct driver_data *dd)
 	writel(readl(dd->mmio + HOST_CTL) | HOST_IRQ_EN,
 					dd->mmio + HOST_CTL);
 
-	init_timer(&dd->port->cmd_timer);
 	init_waitqueue_head(&dd->port->svc_wait);
 
-	dd->port->cmd_timer.data = (unsigned long int) dd->port;
-	dd->port->cmd_timer.function = mtip_timeout_function;
-	mod_timer(&dd->port->cmd_timer,
-		jiffies + msecs_to_jiffies(MTIP_TIMEOUT_CHECK_PERIOD));
-
-
 	if (test_bit(MTIP_DDF_REMOVE_PENDING_BIT, &dd->dd_flag)) {
 		rv = -EFAULT;
 		goto out3;
 	}
 
-	if (mtip_get_identify(dd->port, NULL) < 0) {
-		rv = -EFAULT;
-		goto out3;
-	}
-	mtip_dump_identify(dd->port);
-
-	if (*(dd->port->identify + MTIP_FTL_REBUILD_OFFSET) ==
-		MTIP_FTL_REBUILD_MAGIC) {
-		set_bit(MTIP_PF_REBUILD_BIT, &dd->port->flags);
-		return MTIP_FTL_REBUILD_MAGIC;
-	}
-
-	/* check write protect, over temp and rebuild statuses */
-	rv = mtip_read_log_page(dd->port, ATA_LOG_SATA_NCQ,
-				dd->port->log_buf,
-				dd->port->log_buf_dma, 1);
-	if (rv) {
-		dev_warn(&dd->pdev->dev,
-			"Error in READ LOG EXT (10h) command\n");
-		/* non-critical error, don't fail the load */
-	} else {
-		buf = (unsigned char *)dd->port->log_buf;
-		if (buf[259] & 0x1) {
-			dev_info(&dd->pdev->dev,
-				"Write protect bit is set.\n");
-			set_bit(MTIP_DDF_WRITE_PROTECT_BIT, &dd->dd_flag);
-		}
-		if (buf[288] == 0xF7) {
-			dev_info(&dd->pdev->dev,
-				"Exceeded Tmax, drive in thermal shutdown.\n");
-			set_bit(MTIP_DDF_OVER_TEMP_BIT, &dd->dd_flag);
-		}
-		if (buf[288] == 0xBF) {
-			dev_info(&dd->pdev->dev,
-				"Drive is in security locked state.\n");
-			set_bit(MTIP_DDF_SEC_LOCK_BIT, &dd->dd_flag);
-		}
-	}
-
-	/* get write protect progess */
-	memset(&attr242, 0, sizeof(struct smart_attr));
-	if (mtip_get_smart_attr(dd->port, 242, &attr242))
-		dev_warn(&dd->pdev->dev,
-				"Unable to check write protect progress\n");
-	else
-		dev_info(&dd->pdev->dev,
-				"Write protect progress: %u%% (%u blocks)\n",
-				attr242.cur, le32_to_cpu(attr242.data));
 	return rv;
 
 out3:
-	del_timer_sync(&dd->port->cmd_timer);
-
 	/* Disable interrupts on the HBA. */
 	writel(readl(dd->mmio + HOST_CTL) & ~HOST_IRQ_EN,
 			dd->mmio + HOST_CTL);
@@ -3685,6 +3347,22 @@ out1:
 	return rv;
 }
 
+static void mtip_standby_drive(struct driver_data *dd)
+{
+	if (dd->sr)
+		return;
+
+	/*
+	 * Send standby immediate (E0h) to the drive so that it
+	 * saves its state.
+	 */
+	if (!test_bit(MTIP_PF_REBUILD_BIT, &dd->port->flags) &&
+	    !test_bit(MTIP_DDF_SEC_LOCK_BIT, &dd->dd_flag))
+		if (mtip_standby_immediate(dd->port))
+			dev_warn(&dd->pdev->dev,
+				"STANDBY IMMEDIATE failed\n");
+}
+
 /*
  * Called to deinitialize an interface.
  *
@@ -3700,12 +3378,6 @@ static int mtip_hw_exit(struct driver_data *dd)
 	 * saves its state.
 	 */
 	if (!dd->sr) {
-		if (!test_bit(MTIP_PF_REBUILD_BIT, &dd->port->flags) &&
-		    !test_bit(MTIP_DDF_SEC_LOCK_BIT, &dd->dd_flag))
-			if (mtip_standby_immediate(dd->port))
-				dev_warn(&dd->pdev->dev,
-					"STANDBY IMMEDIATE failed\n");
-
 		/* de-initialize the port. */
 		mtip_deinit_port(dd->port);
 
@@ -3714,8 +3386,6 @@ static int mtip_hw_exit(struct driver_data *dd)
 				dd->mmio + HOST_CTL);
 	}
 
-	del_timer_sync(&dd->port->cmd_timer);
-
 	/* Release the IRQ. */
 	irq_set_affinity_hint(dd->pdev->irq, NULL);
 	devm_free_irq(&dd->pdev->dev, dd->pdev->irq, dd);
@@ -4032,100 +3702,138 @@ static const struct block_device_operations mtip_block_ops = {
  *
  * @queue Pointer to the request queue. Unused other than to obtain
  *              the driver data structure.
- * @bio   Pointer to the BIO.
+ * @rq    Pointer to the request.
  *
  */
-static void mtip_make_request(struct request_queue *queue, struct bio *bio)
+static int mtip_submit_request(struct blk_mq_hw_ctx *hctx, struct request *rq)
 {
-	struct driver_data *dd = queue->queuedata;
-	struct scatterlist *sg;
-	struct bio_vec bvec;
-	struct bvec_iter iter;
-	int nents = 0;
-	int tag = 0, unaligned = 0;
+	struct driver_data *dd = hctx->queue->queuedata;
+	struct mtip_cmd *cmd = blk_mq_rq_to_pdu(rq);
+	unsigned int nents;
 
 	if (unlikely(dd->dd_flag & MTIP_DDF_STOP_IO)) {
 		if (unlikely(test_bit(MTIP_DDF_REMOVE_PENDING_BIT,
 							&dd->dd_flag))) {
-			bio_endio(bio, -ENXIO);
-			return;
+			return -ENXIO;
 		}
 		if (unlikely(test_bit(MTIP_DDF_OVER_TEMP_BIT, &dd->dd_flag))) {
-			bio_endio(bio, -ENODATA);
-			return;
+			return -ENODATA;
 		}
 		if (unlikely(test_bit(MTIP_DDF_WRITE_PROTECT_BIT,
 							&dd->dd_flag) &&
-				bio_data_dir(bio))) {
-			bio_endio(bio, -ENODATA);
-			return;
-		}
-		if (unlikely(test_bit(MTIP_DDF_SEC_LOCK_BIT, &dd->dd_flag))) {
-			bio_endio(bio, -ENODATA);
-			return;
-		}
-		if (test_bit(MTIP_DDF_REBUILD_FAILED_BIT, &dd->dd_flag)) {
-			bio_endio(bio, -ENXIO);
-			return;
+				rq_data_dir(rq))) {
+			return -ENODATA;
 		}
+		if (unlikely(test_bit(MTIP_DDF_SEC_LOCK_BIT, &dd->dd_flag)))
+			return -ENODATA;
+		if (test_bit(MTIP_DDF_REBUILD_FAILED_BIT, &dd->dd_flag))
+			return -ENXIO;
 	}
 
-	if (unlikely(bio->bi_rw & REQ_DISCARD)) {
-		bio_endio(bio, mtip_send_trim(dd, bio->bi_iter.bi_sector,
-						bio_sectors(bio)));
-		return;
-	}
+	if (rq->cmd_flags & REQ_DISCARD) {
+		int err;
 
-	if (unlikely(!bio_has_data(bio))) {
-		blk_queue_flush(queue, 0);
-		bio_endio(bio, 0);
-		return;
+		err = mtip_send_trim(dd, blk_rq_pos(rq), blk_rq_sectors(rq));
+		blk_mq_end_io(rq, err);
+		return 0;
 	}
 
-	if (bio_data_dir(bio) == WRITE && bio_sectors(bio) <= 64 &&
-							dd->unal_qdepth) {
-		if (bio->bi_iter.bi_sector % 8 != 0)
-			/* Unaligned on 4k boundaries */
-			unaligned = 1;
-		else if (bio_sectors(bio) % 8 != 0) /* Aligned but not 4k/8k */
-			unaligned = 1;
+	/* Create the scatter list for this request. */
+	nents = blk_rq_map_sg(hctx->queue, rq, cmd->sg);
+
+	/* Issue the read/write. */
+	mtip_hw_submit_io(dd, rq, cmd, nents, hctx);
+	return 0;
+}
+
+static bool mtip_check_unal_depth(struct blk_mq_hw_ctx *hctx,
+				  struct request *rq)
+{
+	struct driver_data *dd = hctx->queue->queuedata;
+	struct mtip_cmd *cmd = blk_mq_rq_to_pdu(rq);
+
+	if (!dd->unal_qdepth || rq_data_dir(rq) == READ)
+		return false;
+
+	/*
+	 * If unaligned depth must be limited on this controller, mark it
+	 * as unaligned if the IO isn't on a 4k boundary (start of length).
+	 */
+	if (blk_rq_sectors(rq) <= 64) {
+		if ((blk_rq_pos(rq) & 7) || (blk_rq_sectors(rq) & 7))
+			cmd->unaligned = 1;
 	}
 
-	sg = mtip_hw_get_scatterlist(dd, &tag, unaligned);
-	if (likely(sg != NULL)) {
-		blk_queue_bounce(queue, &bio);
+	if (cmd->unaligned && down_trylock(&dd->port->cmd_slot_unal))
+		return true;
 
-		if (unlikely((bio)->bi_vcnt > MTIP_MAX_SG)) {
-			dev_warn(&dd->pdev->dev,
-				"Maximum number of SGL entries exceeded\n");
-			bio_io_error(bio);
-			mtip_hw_release_scatterlist(dd, tag, unaligned);
-			return;
-		}
+	return false;
+}
 
-		/* Create the scatter list for this bio. */
-		bio_for_each_segment(bvec, bio, iter) {
-			sg_set_page(&sg[nents],
-					bvec.bv_page,
-					bvec.bv_len,
-					bvec.bv_offset);
-			nents++;
-		}
+static int mtip_queue_rq(struct blk_mq_hw_ctx *hctx, struct request *rq)
+{
+	int ret;
 
-		/* Issue the read/write. */
-		mtip_hw_submit_io(dd,
-				bio->bi_iter.bi_sector,
-				bio_sectors(bio),
-				nents,
-				tag,
-				bio_endio,
-				bio,
-				bio_data_dir(bio),
-				unaligned);
-	} else
-		bio_io_error(bio);
+	if (mtip_check_unal_depth(hctx, rq))
+		return BLK_MQ_RQ_QUEUE_BUSY;
+
+	ret = mtip_submit_request(hctx, rq);
+	if (!ret)
+		return BLK_MQ_RQ_QUEUE_OK;
+
+	rq->errors = ret;
+	return BLK_MQ_RQ_QUEUE_ERROR;
+}
+
+static void mtip_free_cmd(void *data, struct request *rq,
+			  unsigned int hctx_idx, unsigned int request_idx)
+{
+	struct driver_data *dd = data;
+	struct mtip_cmd *cmd = blk_mq_rq_to_pdu(rq);
+
+	if (!cmd->command)
+		return;
+
+	dmam_free_coherent(&dd->pdev->dev, CMD_DMA_ALLOC_SZ,
+				cmd->command, cmd->command_dma);
+}
+
+static int mtip_init_cmd(void *data, struct request *rq, unsigned int hctx_idx,
+			 unsigned int request_idx, unsigned int numa_node)
+{
+	struct driver_data *dd = data;
+	struct mtip_cmd *cmd = blk_mq_rq_to_pdu(rq);
+	u32 host_cap_64 = readl(dd->mmio + HOST_CAP) & HOST_CAP_64;
+
+	cmd->command = dmam_alloc_coherent(&dd->pdev->dev, CMD_DMA_ALLOC_SZ,
+			&cmd->command_dma, GFP_KERNEL);
+	if (!cmd->command)
+		return -ENOMEM;
+
+	memset(cmd->command, 0, CMD_DMA_ALLOC_SZ);
+
+	/* Point the command headers at the command tables. */
+	cmd->command_header = dd->port->command_list +
+				(sizeof(struct mtip_cmd_hdr) * request_idx);
+	cmd->command_header_dma = dd->port->command_list_dma +
+				(sizeof(struct mtip_cmd_hdr) * request_idx);
+
+	if (host_cap_64)
+		cmd->command_header->ctbau = __force_bit2int cpu_to_le32((cmd->command_dma >> 16) >> 16);
+
+	cmd->command_header->ctba = __force_bit2int cpu_to_le32(cmd->command_dma & 0xFFFFFFFF);
+
+	sg_init_table(cmd->sg, MTIP_MAX_SG);
+	return 0;
 }
 
+static struct blk_mq_ops mtip_mq_ops = {
+	.queue_rq	= mtip_queue_rq,
+	.map_queue	= blk_mq_map_queue,
+	.init_request	= mtip_init_cmd,
+	.exit_request	= mtip_free_cmd,
+};
+
 /*
  * Block layer initialization function.
  *
@@ -4148,11 +3856,7 @@ static int mtip_block_initialize(struct driver_data *dd)
 	if (dd->disk)
 		goto skip_create_disk; /* hw init done, before rebuild */
 
-	/* Initialize the protocol layer. */
-	wait_for_rebuild = mtip_hw_init(dd);
-	if (wait_for_rebuild < 0) {
-		dev_err(&dd->pdev->dev,
-			"Protocol layer initialization failed\n");
+	if (mtip_hw_init(dd)) {
 		rv = -EINVAL;
 		goto protocol_init_error;
 	}
@@ -4194,29 +3898,53 @@ static int mtip_block_initialize(struct driver_data *dd)
 
 	mtip_hw_debugfs_init(dd);
 
-	/*
-	 * if rebuild pending, start the service thread, and delay the block
-	 * queue creation and add_disk()
-	 */
-	if (wait_for_rebuild == MTIP_FTL_REBUILD_MAGIC)
-		goto start_service_thread;
-
 skip_create_disk:
-	/* Allocate the request queue. */
-	dd->queue = blk_alloc_queue_node(GFP_KERNEL, dd->numa_node);
-	if (dd->queue == NULL) {
+	memset(&dd->tags, 0, sizeof(dd->tags));
+	dd->tags.ops = &mtip_mq_ops;
+	dd->tags.nr_hw_queues = 1;
+	dd->tags.queue_depth = MTIP_MAX_COMMAND_SLOTS;
+	dd->tags.reserved_tags = 1;
+	dd->tags.cmd_size = sizeof(struct mtip_cmd);
+	dd->tags.numa_node = dd->numa_node;
+	dd->tags.flags = BLK_MQ_F_SHOULD_MERGE;
+	dd->tags.driver_data = dd;
+
+	rv = blk_mq_alloc_tag_set(&dd->tags);
+	if (rv) {
 		dev_err(&dd->pdev->dev,
 			"Unable to allocate request queue\n");
 		rv = -ENOMEM;
 		goto block_queue_alloc_init_error;
 	}
 
-	/* Attach our request function to the request queue. */
-	blk_queue_make_request(dd->queue, mtip_make_request);
+	/* Allocate the request queue. */
+	dd->queue = blk_mq_init_queue(&dd->tags);
+	if (IS_ERR(dd->queue)) {
+		dev_err(&dd->pdev->dev,
+			"Unable to allocate request queue\n");
+		rv = -ENOMEM;
+		goto block_queue_alloc_init_error;
+	}
 
 	dd->disk->queue		= dd->queue;
 	dd->queue->queuedata	= dd;
 
+	/* Initialize the protocol layer. */
+	wait_for_rebuild = mtip_hw_get_identify(dd);
+	if (wait_for_rebuild < 0) {
+		dev_err(&dd->pdev->dev,
+			"Protocol layer initialization failed\n");
+		rv = -EINVAL;
+		goto init_hw_cmds_error;
+	}
+
+	/*
+	 * if rebuild pending, start the service thread, and delay the block
+	 * queue creation and add_disk()
+	 */
+	if (wait_for_rebuild == MTIP_FTL_REBUILD_MAGIC)
+		goto start_service_thread;
+
 	/* Set device limits. */
 	set_bit(QUEUE_FLAG_NONROT, &dd->queue->queue_flags);
 	blk_queue_max_segments(dd->queue, MTIP_MAX_SG);
@@ -4295,8 +4023,9 @@ kthread_run_error:
 	del_gendisk(dd->disk);
 
 read_capacity_error:
+init_hw_cmds_error:
 	blk_cleanup_queue(dd->queue);
-
+	blk_mq_free_tag_set(&dd->tags);
 block_queue_alloc_init_error:
 	mtip_hw_debugfs_exit(dd);
 disk_index_error:
@@ -4345,6 +4074,9 @@ static int mtip_block_remove(struct driver_data *dd)
 				kobject_put(kobj);
 			}
 		}
+
+		mtip_standby_drive(dd);
+
 		/*
 		 * Delete our gendisk structure. This also removes the device
 		 * from /dev
@@ -4357,6 +4089,7 @@ static int mtip_block_remove(struct driver_data *dd)
 			if (dd->disk->queue) {
 				del_gendisk(dd->disk);
 				blk_cleanup_queue(dd->queue);
+				blk_mq_free_tag_set(&dd->tags);
 				dd->queue = NULL;
 			} else
 				put_disk(dd->disk);
@@ -4391,6 +4124,8 @@ static int mtip_block_remove(struct driver_data *dd)
  */
 static int mtip_block_shutdown(struct driver_data *dd)
 {
+	mtip_hw_shutdown(dd);
+
 	/* Delete our gendisk structure, and cleanup the blk queue. */
 	if (dd->disk) {
 		dev_info(&dd->pdev->dev,
@@ -4399,6 +4134,7 @@ static int mtip_block_shutdown(struct driver_data *dd)
 		if (dd->disk->queue) {
 			del_gendisk(dd->disk);
 			blk_cleanup_queue(dd->queue);
+			blk_mq_free_tag_set(&dd->tags);
 		} else
 			put_disk(dd->disk);
 		dd->disk  = NULL;
@@ -4408,8 +4144,6 @@ static int mtip_block_shutdown(struct driver_data *dd)
 	spin_lock(&rssd_index_lock);
 	ida_remove(&rssd_index_ida, dd->index);
 	spin_unlock(&rssd_index_lock);
-
-	mtip_hw_shutdown(dd);
 	return 0;
 }
 
@@ -4479,6 +4213,57 @@ static DEFINE_HANDLER(5);
 static DEFINE_HANDLER(6);
 static DEFINE_HANDLER(7);
 
+static void mtip_disable_link_opts(struct driver_data *dd, struct pci_dev *pdev)
+{
+	int pos;
+	unsigned short pcie_dev_ctrl;
+
+	pos = pci_find_capability(pdev, PCI_CAP_ID_EXP);
+	if (pos) {
+		pci_read_config_word(pdev,
+			pos + PCI_EXP_DEVCTL,
+			&pcie_dev_ctrl);
+		if (pcie_dev_ctrl & (1 << 11) ||
+		    pcie_dev_ctrl & (1 << 4)) {
+			dev_info(&dd->pdev->dev,
+				"Disabling ERO/No-Snoop on bridge device %04x:%04x\n",
+					pdev->vendor, pdev->device);
+			pcie_dev_ctrl &= ~(PCI_EXP_DEVCTL_NOSNOOP_EN |
+						PCI_EXP_DEVCTL_RELAX_EN);
+			pci_write_config_word(pdev,
+				pos + PCI_EXP_DEVCTL,
+				pcie_dev_ctrl);
+		}
+	}
+}
+
+static void mtip_fix_ero_nosnoop(struct driver_data *dd, struct pci_dev *pdev)
+{
+	/*
+	 * This workaround is specific to AMD/ATI chipset with a PCI upstream
+	 * device with device id 0x5aXX
+	 */
+	if (pdev->bus && pdev->bus->self) {
+		if (pdev->bus->self->vendor == PCI_VENDOR_ID_ATI &&
+		    ((pdev->bus->self->device & 0xff00) == 0x5a00)) {
+			mtip_disable_link_opts(dd, pdev->bus->self);
+		} else {
+			/* Check further up the topology */
+			struct pci_dev *parent_dev = pdev->bus->self;
+			if (parent_dev->bus &&
+				parent_dev->bus->parent &&
+				parent_dev->bus->parent->self &&
+				parent_dev->bus->parent->self->vendor ==
+					 PCI_VENDOR_ID_ATI &&
+				(parent_dev->bus->parent->self->device &
+					0xff00) == 0x5a00) {
+				mtip_disable_link_opts(dd,
+					parent_dev->bus->parent->self);
+			}
+		}
+	}
+}
+
 /*
  * Called for each supported PCI device detected.
  *
@@ -4630,6 +4415,8 @@ static int mtip_pci_probe(struct pci_dev *pdev,
 		goto msi_initialize_err;
 	}
 
+	mtip_fix_ero_nosnoop(dd, pdev);
+
 	/* Initialize the block layer. */
 	rv = mtip_block_initialize(dd);
 	if (rv < 0) {
@@ -4710,8 +4497,6 @@ static void mtip_pci_remove(struct pci_dev *pdev)
 		dev_warn(&dd->pdev->dev,
 			"Completion workers still active!\n");
 	}
-	/* Cleanup the outstanding commands */
-	mtip_command_cleanup(dd);
 
 	/* Clean up the block layer. */
 	mtip_block_remove(dd);
@@ -4737,8 +4522,6 @@ static void mtip_pci_remove(struct pci_dev *pdev)
 
 	pcim_iounmap_regions(pdev, 1 << MTIP_ABAR);
 	pci_set_drvdata(pdev, NULL);
-	pci_dev_put(pdev);
-
 }
 
 /*
@@ -4935,13 +4718,13 @@ static int __init mtip_init(void)
  */
 static void __exit mtip_exit(void)
 {
-	debugfs_remove_recursive(dfs_parent);
-
 	/* Release the allocated major block device number. */
 	unregister_blkdev(mtip_major, MTIP_DRV_NAME);
 
 	/* Unregister the PCI driver. */
 	pci_unregister_driver(&mtip_pci_driver);
+
+	debugfs_remove_recursive(dfs_parent);
 }
 
 MODULE_AUTHOR("Micron Technology, Inc");
diff --git a/drivers/block/mtip32xx/mtip32xx.h b/drivers/block/mtip32xx/mtip32xx.h
index ffb955e7ccb..4b9b554234b 100644
--- a/drivers/block/mtip32xx/mtip32xx.h
+++ b/drivers/block/mtip32xx/mtip32xx.h
@@ -40,9 +40,11 @@
 #define MTIP_MAX_RETRIES	2
 
 /* Various timeout values in ms */
-#define MTIP_NCQ_COMMAND_TIMEOUT_MS       5000
-#define MTIP_IOCTL_COMMAND_TIMEOUT_MS     5000
-#define MTIP_INTERNAL_COMMAND_TIMEOUT_MS  5000
+#define MTIP_NCQ_CMD_TIMEOUT_MS      15000
+#define MTIP_IOCTL_CMD_TIMEOUT_MS    5000
+#define MTIP_INT_CMD_TIMEOUT_MS      5000
+#define MTIP_QUIESCE_IO_TIMEOUT_MS   (MTIP_NCQ_CMD_TIMEOUT_MS * \
+				     (MTIP_MAX_RETRIES + 1))
 
 /* check for timeouts every 500ms */
 #define MTIP_TIMEOUT_CHECK_PERIOD	500
@@ -331,12 +333,8 @@ struct mtip_cmd {
 	 */
 	void (*comp_func)(struct mtip_port *port,
 				int tag,
-				void *data,
+				struct mtip_cmd *cmd,
 				int status);
-	/* Additional callback function that may be called by comp_func() */
-	void (*async_callback)(void *data, int status);
-
-	void *async_data; /* Addl. data passed to async_callback() */
 
 	int scatter_ents; /* Number of scatter list entries used */
 
@@ -347,10 +345,6 @@ struct mtip_cmd {
 	int retries; /* The number of retries left for this command. */
 
 	int direction; /* Data transfer direction */
-
-	unsigned long comp_time; /* command completion time, in jiffies */
-
-	atomic_t active; /* declares if this command sent to the drive. */
 };
 
 /* Structure used to describe a port. */
@@ -436,12 +430,6 @@ struct mtip_port {
 	 * or error handling is active
 	 */
 	unsigned long cmds_to_issue[SLOTBITS_IN_LONGS];
-	/*
-	 * Array of command slots. Structure includes pointers to the
-	 * command header and command table, and completion function and data
-	 * pointers.
-	 */
-	struct mtip_cmd commands[MTIP_MAX_COMMAND_SLOTS];
 	/* Used by mtip_service_thread to wait for an event */
 	wait_queue_head_t svc_wait;
 	/*
@@ -452,13 +440,7 @@ struct mtip_port {
 	/*
 	 * Timer used to complete commands that have been active for too long.
 	 */
-	struct timer_list cmd_timer;
 	unsigned long ic_pause_timer;
-	/*
-	 * Semaphore used to block threads if there are no
-	 * command slots available.
-	 */
-	struct semaphore cmd_slot;
 
 	/* Semaphore to control queue depth of unaligned IOs */
 	struct semaphore cmd_slot_unal;
@@ -485,6 +467,8 @@ struct driver_data {
 
 	struct request_queue *queue; /* Our request queue. */
 
+	struct blk_mq_tag_set tags; /* blk_mq tags */
+
 	struct mtip_port *port; /* Pointer to the port data structure. */
 
 	unsigned product_type; /* magic value declaring the product type */
diff --git a/drivers/block/null_blk.c b/drivers/block/null_blk.c
index 091b9ea14fe..77087a29b12 100644
--- a/drivers/block/null_blk.c
+++ b/drivers/block/null_blk.c
@@ -32,6 +32,7 @@ struct nullb {
 	unsigned int index;
 	struct request_queue *q;
 	struct gendisk *disk;
+	struct blk_mq_tag_set tag_set;
 	struct hrtimer timer;
 	unsigned int queue_depth;
 	spinlock_t lock;
@@ -202,8 +203,8 @@ static enum hrtimer_restart null_cmd_timer_expired(struct hrtimer *timer)
 		entry = llist_reverse_order(entry);
 		do {
 			cmd = container_of(entry, struct nullb_cmd, ll_list);
-			end_cmd(cmd);
 			entry = entry->next;
+			end_cmd(cmd);
 		} while (entry);
 	}
 
@@ -226,7 +227,7 @@ static void null_cmd_end_timer(struct nullb_cmd *cmd)
 
 static void null_softirq_done_fn(struct request *rq)
 {
-	end_cmd(rq->special);
+	end_cmd(blk_mq_rq_to_pdu(rq));
 }
 
 static inline void null_handle_cmd(struct nullb_cmd *cmd)
@@ -311,7 +312,7 @@ static void null_request_fn(struct request_queue *q)
 
 static int null_queue_rq(struct blk_mq_hw_ctx *hctx, struct request *rq)
 {
-	struct nullb_cmd *cmd = rq->special;
+	struct nullb_cmd *cmd = blk_mq_rq_to_pdu(rq);
 
 	cmd->rq = rq;
 	cmd->nq = hctx->driver_data;
@@ -320,46 +321,6 @@ static int null_queue_rq(struct blk_mq_hw_ctx *hctx, struct request *rq)
 	return BLK_MQ_RQ_QUEUE_OK;
 }
 
-static struct blk_mq_hw_ctx *null_alloc_hctx(struct blk_mq_reg *reg, unsigned int hctx_index)
-{
-	int b_size = DIV_ROUND_UP(reg->nr_hw_queues, nr_online_nodes);
-	int tip = (reg->nr_hw_queues % nr_online_nodes);
-	int node = 0, i, n;
-
-	/*
-	 * Split submit queues evenly wrt to the number of nodes. If uneven,
-	 * fill the first buckets with one extra, until the rest is filled with
-	 * no extra.
-	 */
-	for (i = 0, n = 1; i < hctx_index; i++, n++) {
-		if (n % b_size == 0) {
-			n = 0;
-			node++;
-
-			tip--;
-			if (!tip)
-				b_size = reg->nr_hw_queues / nr_online_nodes;
-		}
-	}
-
-	/*
-	 * A node might not be online, therefore map the relative node id to the
-	 * real node id.
-	 */
-	for_each_online_node(n) {
-		if (!node)
-			break;
-		node--;
-	}
-
-	return kzalloc_node(sizeof(struct blk_mq_hw_ctx), GFP_KERNEL, n);
-}
-
-static void null_free_hctx(struct blk_mq_hw_ctx *hctx, unsigned int hctx_index)
-{
-	kfree(hctx);
-}
-
 static void null_init_queue(struct nullb *nullb, struct nullb_queue *nq)
 {
 	BUG_ON(!nullb);
@@ -389,19 +350,14 @@ static struct blk_mq_ops null_mq_ops = {
 	.complete	= null_softirq_done_fn,
 };
 
-static struct blk_mq_reg null_mq_reg = {
-	.ops		= &null_mq_ops,
-	.queue_depth	= 64,
-	.cmd_size	= sizeof(struct nullb_cmd),
-	.flags		= BLK_MQ_F_SHOULD_MERGE,
-};
-
 static void null_del_dev(struct nullb *nullb)
 {
 	list_del_init(&nullb->list);
 
 	del_gendisk(nullb->disk);
 	blk_cleanup_queue(nullb->q);
+	if (queue_mode == NULL_Q_MQ)
+		blk_mq_free_tag_set(&nullb->tag_set);
 	put_disk(nullb->disk);
 	kfree(nullb);
 }
@@ -506,7 +462,7 @@ static int null_add_dev(void)
 
 	nullb = kzalloc_node(sizeof(*nullb), GFP_KERNEL, home_node);
 	if (!nullb)
-		return -ENOMEM;
+		goto out;
 
 	spin_lock_init(&nullb->lock);
 
@@ -514,49 +470,44 @@ static int null_add_dev(void)
 		submit_queues = nr_online_nodes;
 
 	if (setup_queues(nullb))
-		goto err;
+		goto out_free_nullb;
 
 	if (queue_mode == NULL_Q_MQ) {
-		null_mq_reg.numa_node = home_node;
-		null_mq_reg.queue_depth = hw_queue_depth;
-		null_mq_reg.nr_hw_queues = submit_queues;
-
-		if (use_per_node_hctx) {
-			null_mq_reg.ops->alloc_hctx = null_alloc_hctx;
-			null_mq_reg.ops->free_hctx = null_free_hctx;
-		} else {
-			null_mq_reg.ops->alloc_hctx = blk_mq_alloc_single_hw_queue;
-			null_mq_reg.ops->free_hctx = blk_mq_free_single_hw_queue;
-		}
-
-		nullb->q = blk_mq_init_queue(&null_mq_reg, nullb);
+		nullb->tag_set.ops = &null_mq_ops;
+		nullb->tag_set.nr_hw_queues = submit_queues;
+		nullb->tag_set.queue_depth = hw_queue_depth;
+		nullb->tag_set.numa_node = home_node;
+		nullb->tag_set.cmd_size	= sizeof(struct nullb_cmd);
+		nullb->tag_set.flags = BLK_MQ_F_SHOULD_MERGE;
+		nullb->tag_set.driver_data = nullb;
+
+		if (blk_mq_alloc_tag_set(&nullb->tag_set))
+			goto out_cleanup_queues;
+
+		nullb->q = blk_mq_init_queue(&nullb->tag_set);
+		if (!nullb->q)
+			goto out_cleanup_tags;
 	} else if (queue_mode == NULL_Q_BIO) {
 		nullb->q = blk_alloc_queue_node(GFP_KERNEL, home_node);
+		if (!nullb->q)
+			goto out_cleanup_queues;
 		blk_queue_make_request(nullb->q, null_queue_bio);
 		init_driver_queues(nullb);
 	} else {
 		nullb->q = blk_init_queue_node(null_request_fn, &nullb->lock, home_node);
+		if (!nullb->q)
+			goto out_cleanup_queues;
 		blk_queue_prep_rq(nullb->q, null_rq_prep_fn);
-		if (nullb->q)
-			blk_queue_softirq_done(nullb->q, null_softirq_done_fn);
+		blk_queue_softirq_done(nullb->q, null_softirq_done_fn);
 		init_driver_queues(nullb);
 	}
 
-	if (!nullb->q)
-		goto queue_fail;
-
 	nullb->q->queuedata = nullb;
 	queue_flag_set_unlocked(QUEUE_FLAG_NONROT, nullb->q);
 
 	disk = nullb->disk = alloc_disk_node(1, home_node);
-	if (!disk) {
-queue_fail:
-		blk_cleanup_queue(nullb->q);
-		cleanup_queues(nullb);
-err:
-		kfree(nullb);
-		return -ENOMEM;
-	}
+	if (!disk)
+		goto out_cleanup_blk_queue;
 
 	mutex_lock(&lock);
 	list_add_tail(&nullb->list, &nullb_list);
@@ -579,6 +530,18 @@ err:
 	sprintf(disk->disk_name, "nullb%d", nullb->index);
 	add_disk(disk);
 	return 0;
+
+out_cleanup_blk_queue:
+	blk_cleanup_queue(nullb->q);
+out_cleanup_tags:
+	if (queue_mode == NULL_Q_MQ)
+		blk_mq_free_tag_set(&nullb->tag_set);
+out_cleanup_queues:
+	cleanup_queues(nullb);
+out_free_nullb:
+	kfree(nullb);
+out:
+	return -ENOMEM;
 }
 
 static int __init null_init(void)
diff --git a/drivers/block/nvme-core.c b/drivers/block/nvme-core.c
index 7c64fa756cc..a842c71dcc2 100644
--- a/drivers/block/nvme-core.c
+++ b/drivers/block/nvme-core.c
@@ -2775,6 +2775,16 @@ static int nvme_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	return result;
 }
 
+static void nvme_reset_notify(struct pci_dev *pdev, bool prepare)
+{
+       struct nvme_dev *dev = pci_get_drvdata(pdev);
+
+       if (prepare)
+               nvme_dev_shutdown(dev);
+       else
+               nvme_dev_resume(dev);
+}
+
 static void nvme_shutdown(struct pci_dev *pdev)
 {
 	struct nvme_dev *dev = pci_get_drvdata(pdev);
@@ -2839,6 +2849,7 @@ static const struct pci_error_handlers nvme_err_handler = {
 	.link_reset	= nvme_link_reset,
 	.slot_reset	= nvme_slot_reset,
 	.resume		= nvme_error_resume,
+	.reset_notify	= nvme_reset_notify,
 };
 
 /* Move to pci_ids.h later */
diff --git a/drivers/block/paride/pcd.c b/drivers/block/paride/pcd.c
index e76bdc074db..719cb1bc164 100644
--- a/drivers/block/paride/pcd.c
+++ b/drivers/block/paride/pcd.c
@@ -747,7 +747,7 @@ static void do_pcd_request(struct request_queue * q)
 			pcd_current = cd;
 			pcd_sector = blk_rq_pos(pcd_req);
 			pcd_count = blk_rq_cur_sectors(pcd_req);
-			pcd_buf = pcd_req->buffer;
+			pcd_buf = bio_data(pcd_req->bio);
 			pcd_busy = 1;
 			ps_set_intr(do_pcd_read, NULL, 0, nice);
 			return;
diff --git a/drivers/block/paride/pd.c b/drivers/block/paride/pd.c
index 19ad8f0c83e..fea7e76a00d 100644
--- a/drivers/block/paride/pd.c
+++ b/drivers/block/paride/pd.c
@@ -454,7 +454,7 @@ static enum action do_pd_io_start(void)
 		if (pd_block + pd_count > get_capacity(pd_req->rq_disk))
 			return Fail;
 		pd_run = blk_rq_sectors(pd_req);
-		pd_buf = pd_req->buffer;
+		pd_buf = bio_data(pd_req->bio);
 		pd_retries = 0;
 		if (pd_cmd == READ)
 			return do_pd_read_start();
@@ -485,7 +485,7 @@ static int pd_next_buf(void)
 	spin_lock_irqsave(&pd_lock, saved_flags);
 	__blk_end_request_cur(pd_req, 0);
 	pd_count = blk_rq_cur_sectors(pd_req);
-	pd_buf = pd_req->buffer;
+	pd_buf = bio_data(pd_req->bio);
 	spin_unlock_irqrestore(&pd_lock, saved_flags);
 	return 0;
 }
diff --git a/drivers/block/paride/pf.c b/drivers/block/paride/pf.c
index f5c86d523ba..9a15fd3c934 100644
--- a/drivers/block/paride/pf.c
+++ b/drivers/block/paride/pf.c
@@ -795,7 +795,7 @@ repeat:
 	}
 
 	pf_cmd = rq_data_dir(pf_req);
-	pf_buf = pf_req->buffer;
+	pf_buf = bio_data(pf_req->bio);
 	pf_retries = 0;
 
 	pf_busy = 1;
@@ -827,7 +827,7 @@ static int pf_next_buf(void)
 		if (!pf_req)
 			return 1;
 		pf_count = blk_rq_cur_sectors(pf_req);
-		pf_buf = pf_req->buffer;
+		pf_buf = bio_data(pf_req->bio);
 	}
 	return 0;
 }
diff --git a/drivers/block/skd_main.c b/drivers/block/skd_main.c
index a69dd93d1bd..608532d3f8c 100644
--- a/drivers/block/skd_main.c
+++ b/drivers/block/skd_main.c
@@ -563,7 +563,6 @@ skd_prep_discard_cdb(struct skd_scsi_request *scsi_req,
 
 	req = skreq->req;
 	blk_add_request_payload(req, page, len);
-	req->buffer = buf;
 }
 
 static void skd_request_fn_not_online(struct request_queue *q);
@@ -744,6 +743,7 @@ static void skd_request_fn(struct request_queue *q)
 				break;
 			}
 			skreq->discard_page = 1;
+			req->completion_data = page;
 			skd_prep_discard_cdb(scsi_req, skreq, page, lba, count);
 
 		} else if (flush == SKD_FLUSH_ZERO_SIZE_FIRST) {
@@ -858,8 +858,7 @@ static void skd_end_request(struct skd_device *skdev,
 		(skreq->discard_page == 1)) {
 		pr_debug("%s:%s:%d, free the page!",
 			 skdev->name, __func__, __LINE__);
-		free_page((unsigned long)req->buffer);
-		req->buffer = NULL;
+		__free_page(req->completion_data);
 	}
 
 	if (unlikely(error)) {
@@ -3945,15 +3944,14 @@ static int skd_acquire_msix(struct skd_device *skdev)
 	for (i = 0; i < SKD_MAX_MSIX_COUNT; i++)
 		entries[i].entry = i;
 
-	rc = pci_enable_msix_range(pdev, entries,
-				   SKD_MIN_MSIX_COUNT, SKD_MAX_MSIX_COUNT);
-	if (rc < 0) {
+	rc = pci_enable_msix_exact(pdev, entries, SKD_MAX_MSIX_COUNT);
+	if (rc) {
 		pr_err("(%s): failed to enable MSI-X %d\n",
 		       skd_name(skdev), rc);
 		goto msix_out;
 	}
 
-	skdev->msix_count = rc;
+	skdev->msix_count = SKD_MAX_MSIX_COUNT;
 	skdev->msix_entries = kzalloc(sizeof(struct skd_msix_entry) *
 				      skdev->msix_count, GFP_KERNEL);
 	if (!skdev->msix_entries) {
diff --git a/drivers/block/swim.c b/drivers/block/swim.c
index b02d53a399f..6b44bbe528b 100644
--- a/drivers/block/swim.c
+++ b/drivers/block/swim.c
@@ -549,7 +549,7 @@ static void redo_fd_request(struct request_queue *q)
 		case READ:
 			err = floppy_read_sectors(fs, blk_rq_pos(req),
 						  blk_rq_cur_sectors(req),
-						  req->buffer);
+						  bio_data(req->bio));
 			break;
 		}
 	done:
diff --git a/drivers/block/swim3.c b/drivers/block/swim3.c
index c74f7b56e7c..523ee8fd4c1 100644
--- a/drivers/block/swim3.c
+++ b/drivers/block/swim3.c
@@ -342,7 +342,7 @@ static void start_request(struct floppy_state *fs)
 		swim3_dbg("do_fd_req: dev=%s cmd=%d sec=%ld nr_sec=%u buf=%p\n",
 			  req->rq_disk->disk_name, req->cmd,
 			  (long)blk_rq_pos(req), blk_rq_sectors(req),
-			  req->buffer);
+			  bio_data(req->bio));
 		swim3_dbg("           errors=%d current_nr_sectors=%u\n",
 			  req->errors, blk_rq_cur_sectors(req));
 #endif
@@ -479,11 +479,11 @@ static inline void setup_transfer(struct floppy_state *fs)
 		/* Set up 3 dma commands: write preamble, data, postamble */
 		init_dma(cp, OUTPUT_MORE, write_preamble, sizeof(write_preamble));
 		++cp;
-		init_dma(cp, OUTPUT_MORE, req->buffer, 512);
+		init_dma(cp, OUTPUT_MORE, bio_data(req->bio), 512);
 		++cp;
 		init_dma(cp, OUTPUT_LAST, write_postamble, sizeof(write_postamble));
 	} else {
-		init_dma(cp, INPUT_LAST, req->buffer, n * 512);
+		init_dma(cp, INPUT_LAST, bio_data(req->bio), n * 512);
 	}
 	++cp;
 	out_le16(&cp->command, DBDMA_STOP);
diff --git a/drivers/block/virtio_blk.c b/drivers/block/virtio_blk.c
index 6d8a87f252d..f63d358f3d9 100644
--- a/drivers/block/virtio_blk.c
+++ b/drivers/block/virtio_blk.c
@@ -30,6 +30,9 @@ struct virtio_blk
 	/* The disk structure for the kernel. */
 	struct gendisk *disk;
 
+	/* Block layer tags. */
+	struct blk_mq_tag_set tag_set;
+
 	/* Process context for config space updates */
 	struct work_struct config_work;
 
@@ -112,7 +115,7 @@ static int __virtblk_add_req(struct virtqueue *vq,
 
 static inline void virtblk_request_done(struct request *req)
 {
-	struct virtblk_req *vbr = req->special;
+	struct virtblk_req *vbr = blk_mq_rq_to_pdu(req);
 	int error = virtblk_result(vbr);
 
 	if (req->cmd_type == REQ_TYPE_BLOCK_PC) {
@@ -144,21 +147,22 @@ static void virtblk_done(struct virtqueue *vq)
 		if (unlikely(virtqueue_is_broken(vq)))
 			break;
 	} while (!virtqueue_enable_cb(vq));
-	spin_unlock_irqrestore(&vblk->vq_lock, flags);
 
 	/* In case queue is stopped waiting for more buffers. */
 	if (req_done)
-		blk_mq_start_stopped_hw_queues(vblk->disk->queue);
+		blk_mq_start_stopped_hw_queues(vblk->disk->queue, true);
+	spin_unlock_irqrestore(&vblk->vq_lock, flags);
 }
 
 static int virtio_queue_rq(struct blk_mq_hw_ctx *hctx, struct request *req)
 {
 	struct virtio_blk *vblk = hctx->queue->queuedata;
-	struct virtblk_req *vbr = req->special;
+	struct virtblk_req *vbr = blk_mq_rq_to_pdu(req);
 	unsigned long flags;
 	unsigned int num;
 	const bool last = (req->cmd_flags & REQ_END) != 0;
 	int err;
+	bool notify = false;
 
 	BUG_ON(req->nr_phys_segments + 2 > vblk->sg_elems);
 
@@ -202,8 +206,8 @@ static int virtio_queue_rq(struct blk_mq_hw_ctx *hctx, struct request *req)
 	err = __virtblk_add_req(vblk->vq, vbr, vbr->sg, num);
 	if (err) {
 		virtqueue_kick(vblk->vq);
-		spin_unlock_irqrestore(&vblk->vq_lock, flags);
 		blk_mq_stop_hw_queue(hctx);
+		spin_unlock_irqrestore(&vblk->vq_lock, flags);
 		/* Out of mem doesn't actually happen, since we fall back
 		 * to direct descriptors */
 		if (err == -ENOMEM || err == -ENOSPC)
@@ -211,10 +215,12 @@ static int virtio_queue_rq(struct blk_mq_hw_ctx *hctx, struct request *req)
 		return BLK_MQ_RQ_QUEUE_ERROR;
 	}
 
-	if (last)
-		virtqueue_kick(vblk->vq);
-
+	if (last && virtqueue_kick_prepare(vblk->vq))
+		notify = true;
 	spin_unlock_irqrestore(&vblk->vq_lock, flags);
+
+	if (notify)
+		virtqueue_notify(vblk->vq);
 	return BLK_MQ_RQ_QUEUE_OK;
 }
 
@@ -480,33 +486,27 @@ static const struct device_attribute dev_attr_cache_type_rw =
 	__ATTR(cache_type, S_IRUGO|S_IWUSR,
 	       virtblk_cache_type_show, virtblk_cache_type_store);
 
-static struct blk_mq_ops virtio_mq_ops = {
-	.queue_rq	= virtio_queue_rq,
-	.map_queue	= blk_mq_map_queue,
-	.alloc_hctx	= blk_mq_alloc_single_hw_queue,
-	.free_hctx	= blk_mq_free_single_hw_queue,
-	.complete	= virtblk_request_done,
-};
-
-static struct blk_mq_reg virtio_mq_reg = {
-	.ops		= &virtio_mq_ops,
-	.nr_hw_queues	= 1,
-	.queue_depth	= 0, /* Set in virtblk_probe */
-	.numa_node	= NUMA_NO_NODE,
-	.flags		= BLK_MQ_F_SHOULD_MERGE,
-};
-module_param_named(queue_depth, virtio_mq_reg.queue_depth, uint, 0444);
-
-static int virtblk_init_vbr(void *data, struct blk_mq_hw_ctx *hctx,
-			     struct request *rq, unsigned int nr)
+static int virtblk_init_request(void *data, struct request *rq,
+		unsigned int hctx_idx, unsigned int request_idx,
+		unsigned int numa_node)
 {
 	struct virtio_blk *vblk = data;
-	struct virtblk_req *vbr = rq->special;
+	struct virtblk_req *vbr = blk_mq_rq_to_pdu(rq);
 
 	sg_init_table(vbr->sg, vblk->sg_elems);
 	return 0;
 }
 
+static struct blk_mq_ops virtio_mq_ops = {
+	.queue_rq	= virtio_queue_rq,
+	.map_queue	= blk_mq_map_queue,
+	.complete	= virtblk_request_done,
+	.init_request	= virtblk_init_request,
+};
+
+static unsigned int virtblk_queue_depth;
+module_param_named(queue_depth, virtblk_queue_depth, uint, 0444);
+
 static int virtblk_probe(struct virtio_device *vdev)
 {
 	struct virtio_blk *vblk;
@@ -561,24 +561,34 @@ static int virtblk_probe(struct virtio_device *vdev)
 	}
 
 	/* Default queue sizing is to fill the ring. */
-	if (!virtio_mq_reg.queue_depth) {
-		virtio_mq_reg.queue_depth = vblk->vq->num_free;
+	if (!virtblk_queue_depth) {
+		virtblk_queue_depth = vblk->vq->num_free;
 		/* ... but without indirect descs, we use 2 descs per req */
 		if (!virtio_has_feature(vdev, VIRTIO_RING_F_INDIRECT_DESC))
-			virtio_mq_reg.queue_depth /= 2;
+			virtblk_queue_depth /= 2;
 	}
-	virtio_mq_reg.cmd_size =
+
+	memset(&vblk->tag_set, 0, sizeof(vblk->tag_set));
+	vblk->tag_set.ops = &virtio_mq_ops;
+	vblk->tag_set.nr_hw_queues = 1;
+	vblk->tag_set.queue_depth = virtblk_queue_depth;
+	vblk->tag_set.numa_node = NUMA_NO_NODE;
+	vblk->tag_set.flags = BLK_MQ_F_SHOULD_MERGE;
+	vblk->tag_set.cmd_size =
 		sizeof(struct virtblk_req) +
 		sizeof(struct scatterlist) * sg_elems;
+	vblk->tag_set.driver_data = vblk;
+
+	err = blk_mq_alloc_tag_set(&vblk->tag_set);
+	if (err)
+		goto out_put_disk;
 
-	q = vblk->disk->queue = blk_mq_init_queue(&virtio_mq_reg, vblk);
+	q = vblk->disk->queue = blk_mq_init_queue(&vblk->tag_set);
 	if (!q) {
 		err = -ENOMEM;
-		goto out_put_disk;
+		goto out_free_tags;
 	}
 
-	blk_mq_init_commands(q, virtblk_init_vbr, vblk);
-
 	q->queuedata = vblk;
 
 	virtblk_name_format("vd", index, vblk->disk->disk_name, DISK_NAME_LEN);
@@ -679,6 +689,8 @@ static int virtblk_probe(struct virtio_device *vdev)
 out_del_disk:
 	del_gendisk(vblk->disk);
 	blk_cleanup_queue(vblk->disk->queue);
+out_free_tags:
+	blk_mq_free_tag_set(&vblk->tag_set);
 out_put_disk:
 	put_disk(vblk->disk);
 out_free_vq:
@@ -705,6 +717,8 @@ static void virtblk_remove(struct virtio_device *vdev)
 	del_gendisk(vblk->disk);
 	blk_cleanup_queue(vblk->disk->queue);
 
+	blk_mq_free_tag_set(&vblk->tag_set);
+
 	/* Stop all the virtqueues. */
 	vdev->config->reset(vdev);
 
@@ -749,7 +763,7 @@ static int virtblk_restore(struct virtio_device *vdev)
 	vblk->config_enable = true;
 	ret = init_vq(vdev->priv);
 	if (!ret)
-		blk_mq_start_stopped_hw_queues(vblk->disk->queue);
+		blk_mq_start_stopped_hw_queues(vblk->disk->queue, true);
 
 	return ret;
 }
diff --git a/drivers/block/xen-blkback/common.h b/drivers/block/xen-blkback/common.h
index be052773ad0..f65b807e323 100644
--- a/drivers/block/xen-blkback/common.h
+++ b/drivers/block/xen-blkback/common.h
@@ -314,7 +314,7 @@ struct xen_blkif {
 	unsigned long long			st_rd_sect;
 	unsigned long long			st_wr_sect;
 
-	wait_queue_head_t	waiting_to_free;
+	struct work_struct	free_work;
 	/* Thread shutdown wait queue. */
 	wait_queue_head_t	shutdown_wq;
 };
@@ -361,7 +361,7 @@ struct pending_req {
 #define xen_blkif_put(_b)				\
 	do {						\
 		if (atomic_dec_and_test(&(_b)->refcnt))	\
-			wake_up(&(_b)->waiting_to_free);\
+			schedule_work(&(_b)->free_work);\
 	} while (0)
 
 struct phys_req {
diff --git a/drivers/block/xen-blkback/xenbus.c b/drivers/block/xen-blkback/xenbus.c
index 9a547e6b6eb..3a8b810b498 100644
--- a/drivers/block/xen-blkback/xenbus.c
+++ b/drivers/block/xen-blkback/xenbus.c
@@ -35,12 +35,26 @@ static void connect(struct backend_info *);
 static int connect_ring(struct backend_info *);
 static void backend_changed(struct xenbus_watch *, const char **,
 			    unsigned int);
+static void xen_blkif_free(struct xen_blkif *blkif);
+static void xen_vbd_free(struct xen_vbd *vbd);
 
 struct xenbus_device *xen_blkbk_xenbus(struct backend_info *be)
 {
 	return be->dev;
 }
 
+/*
+ * The last request could free the device from softirq context and
+ * xen_blkif_free() can sleep.
+ */
+static void xen_blkif_deferred_free(struct work_struct *work)
+{
+	struct xen_blkif *blkif;
+
+	blkif = container_of(work, struct xen_blkif, free_work);
+	xen_blkif_free(blkif);
+}
+
 static int blkback_name(struct xen_blkif *blkif, char *buf)
 {
 	char *devpath, *devname;
@@ -121,7 +135,6 @@ static struct xen_blkif *xen_blkif_alloc(domid_t domid)
 	init_completion(&blkif->drain_complete);
 	atomic_set(&blkif->drain, 0);
 	blkif->st_print = jiffies;
-	init_waitqueue_head(&blkif->waiting_to_free);
 	blkif->persistent_gnts.rb_node = NULL;
 	spin_lock_init(&blkif->free_pages_lock);
 	INIT_LIST_HEAD(&blkif->free_pages);
@@ -132,6 +145,7 @@ static struct xen_blkif *xen_blkif_alloc(domid_t domid)
 	INIT_WORK(&blkif->persistent_purge_work, xen_blkbk_unmap_purged_grants);
 
 	INIT_LIST_HEAD(&blkif->pending_free);
+	INIT_WORK(&blkif->free_work, xen_blkif_deferred_free);
 
 	for (i = 0; i < XEN_BLKIF_REQS; i++) {
 		req = kzalloc(sizeof(*req), GFP_KERNEL);
@@ -231,7 +245,7 @@ static int xen_blkif_map(struct xen_blkif *blkif, unsigned long shared_page,
 	return 0;
 }
 
-static void xen_blkif_disconnect(struct xen_blkif *blkif)
+static int xen_blkif_disconnect(struct xen_blkif *blkif)
 {
 	if (blkif->xenblkd) {
 		kthread_stop(blkif->xenblkd);
@@ -239,9 +253,12 @@ static void xen_blkif_disconnect(struct xen_blkif *blkif)
 		blkif->xenblkd = NULL;
 	}
 
-	atomic_dec(&blkif->refcnt);
-	wait_event(blkif->waiting_to_free, atomic_read(&blkif->refcnt) == 0);
-	atomic_inc(&blkif->refcnt);
+	/* The above kthread_stop() guarantees that at this point we
+	 * don't have any discard_io or other_io requests. So, checking
+	 * for inflight IO is enough.
+	 */
+	if (atomic_read(&blkif->inflight) > 0)
+		return -EBUSY;
 
 	if (blkif->irq) {
 		unbind_from_irqhandler(blkif->irq, blkif);
@@ -252,6 +269,8 @@ static void xen_blkif_disconnect(struct xen_blkif *blkif)
 		xenbus_unmap_ring_vfree(blkif->be->dev, blkif->blk_ring);
 		blkif->blk_rings.common.sring = NULL;
 	}
+
+	return 0;
 }
 
 static void xen_blkif_free(struct xen_blkif *blkif)
@@ -259,8 +278,8 @@ static void xen_blkif_free(struct xen_blkif *blkif)
 	struct pending_req *req, *n;
 	int i = 0, j;
 
-	if (!atomic_dec_and_test(&blkif->refcnt))
-		BUG();
+	xen_blkif_disconnect(blkif);
+	xen_vbd_free(&blkif->vbd);
 
 	/* Remove all persistent grants and the cache of ballooned pages. */
 	xen_blkbk_free_caches(blkif);
@@ -449,16 +468,15 @@ static int xen_blkbk_remove(struct xenbus_device *dev)
 		be->backend_watch.node = NULL;
 	}
 
+	dev_set_drvdata(&dev->dev, NULL);
+
 	if (be->blkif) {
 		xen_blkif_disconnect(be->blkif);
-		xen_vbd_free(&be->blkif->vbd);
-		xen_blkif_free(be->blkif);
-		be->blkif = NULL;
+		xen_blkif_put(be->blkif);
 	}
 
 	kfree(be->mode);
 	kfree(be);
-	dev_set_drvdata(&dev->dev, NULL);
 	return 0;
 }
 
@@ -481,10 +499,15 @@ static void xen_blkbk_discard(struct xenbus_transaction xbt, struct backend_info
 	struct xenbus_device *dev = be->dev;
 	struct xen_blkif *blkif = be->blkif;
 	int err;
-	int state = 0;
+	int state = 0, discard_enable;
 	struct block_device *bdev = be->blkif->vbd.bdev;
 	struct request_queue *q = bdev_get_queue(bdev);
 
+	err = xenbus_scanf(XBT_NIL, dev->nodename, "discard-enable", "%d",
+			   &discard_enable);
+	if (err == 1 && !discard_enable)
+		return;
+
 	if (blk_queue_discard(q)) {
 		err = xenbus_printf(xbt, dev->nodename,
 			"discard-granularity", "%u",
@@ -700,7 +723,11 @@ static void frontend_changed(struct xenbus_device *dev,
 		 * Enforce precondition before potential leak point.
 		 * xen_blkif_disconnect() is idempotent.
 		 */
-		xen_blkif_disconnect(be->blkif);
+		err = xen_blkif_disconnect(be->blkif);
+		if (err) {
+			xenbus_dev_fatal(dev, err, "pending I/O");
+			break;
+		}
 
 		err = connect_ring(be);
 		if (err)
diff --git a/drivers/block/xen-blkfront.c b/drivers/block/xen-blkfront.c
index efe1b476173..5deb235bd18 100644
--- a/drivers/block/xen-blkfront.c
+++ b/drivers/block/xen-blkfront.c
@@ -612,10 +612,10 @@ static void do_blkif_request(struct request_queue *rq)
 		}
 
 		pr_debug("do_blk_req %p: cmd %p, sec %lx, "
-			 "(%u/%u) buffer:%p [%s]\n",
+			 "(%u/%u) [%s]\n",
 			 req, req->cmd, (unsigned long)blk_rq_pos(req),
 			 blk_rq_cur_sectors(req), blk_rq_sectors(req),
-			 req->buffer, rq_data_dir(req) ? "write" : "read");
+			 rq_data_dir(req) ? "write" : "read");
 
 		if (blkif_queue_request(req)) {
 			blk_requeue_request(rq, req);
@@ -1635,36 +1635,24 @@ blkfront_closing(struct blkfront_info *info)
 static void blkfront_setup_discard(struct blkfront_info *info)
 {
 	int err;
-	char *type;
 	unsigned int discard_granularity;
 	unsigned int discard_alignment;
 	unsigned int discard_secure;
 
-	type = xenbus_read(XBT_NIL, info->xbdev->otherend, "type", NULL);
-	if (IS_ERR(type))
-		return;
-
-	info->feature_secdiscard = 0;
-	if (strncmp(type, "phy", 3) == 0) {
-		err = xenbus_gather(XBT_NIL, info->xbdev->otherend,
-			"discard-granularity", "%u", &discard_granularity,
-			"discard-alignment", "%u", &discard_alignment,
-			NULL);
-		if (!err) {
-			info->feature_discard = 1;
-			info->discard_granularity = discard_granularity;
-			info->discard_alignment = discard_alignment;
-		}
-		err = xenbus_gather(XBT_NIL, info->xbdev->otherend,
-			    "discard-secure", "%d", &discard_secure,
-			    NULL);
-		if (!err)
-			info->feature_secdiscard = discard_secure;
-
-	} else if (strncmp(type, "file", 4) == 0)
-		info->feature_discard = 1;
-
-	kfree(type);
+	info->feature_discard = 1;
+	err = xenbus_gather(XBT_NIL, info->xbdev->otherend,
+		"discard-granularity", "%u", &discard_granularity,
+		"discard-alignment", "%u", &discard_alignment,
+		NULL);
+	if (!err) {
+		info->discard_granularity = discard_granularity;
+		info->discard_alignment = discard_alignment;
+	}
+	err = xenbus_gather(XBT_NIL, info->xbdev->otherend,
+		    "discard-secure", "%d", &discard_secure,
+		    NULL);
+	if (!err)
+		info->feature_secdiscard = !!discard_secure;
 }
 
 static int blkfront_setup_indirect(struct blkfront_info *info)
diff --git a/drivers/block/xsysace.c b/drivers/block/xsysace.c
index 1393b8871a2..ab3ea62e5df 100644
--- a/drivers/block/xsysace.c
+++ b/drivers/block/xsysace.c
@@ -661,7 +661,7 @@ static void ace_fsm_dostate(struct ace_device *ace)
 			rq_data_dir(req));
 
 		ace->req = req;
-		ace->data_ptr = req->buffer;
+		ace->data_ptr = bio_data(req->bio);
 		ace->data_count = blk_rq_cur_sectors(req) * ACE_BUF_PER_SECTOR;
 		ace_out32(ace, ACE_MPULBA, blk_rq_pos(req) & 0x0FFFFFFF);
 
@@ -733,7 +733,7 @@ static void ace_fsm_dostate(struct ace_device *ace)
 			 *      blk_rq_sectors(ace->req),
 			 *      blk_rq_cur_sectors(ace->req));
 			 */
-			ace->data_ptr = ace->req->buffer;
+			ace->data_ptr = bio_data(ace->req->bio);
 			ace->data_count = blk_rq_cur_sectors(ace->req) * 16;
 			ace_fsm_yieldirq(ace);
 			break;
diff --git a/drivers/block/z2ram.c b/drivers/block/z2ram.c
index 27de5046708..968f9e52eff 100644
--- a/drivers/block/z2ram.c
+++ b/drivers/block/z2ram.c
@@ -87,13 +87,15 @@ static void do_z2_request(struct request_queue *q)
 		while (len) {
 			unsigned long addr = start & Z2RAM_CHUNKMASK;
 			unsigned long size = Z2RAM_CHUNKSIZE - addr;
+			void *buffer = bio_data(req->bio);
+
 			if (len < size)
 				size = len;
 			addr += z2ram_map[ start >> Z2RAM_CHUNKSHIFT ];
 			if (rq_data_dir(req) == READ)
-				memcpy(req->buffer, (char *)addr, size);
+				memcpy(buffer, (char *)addr, size);
 			else
-				memcpy((char *)addr, req->buffer, size);
+				memcpy((char *)addr, buffer, size);
 			start += size;
 			len -= size;
 		}
diff --git a/drivers/bluetooth/ath3k.c b/drivers/bluetooth/ath3k.c
index be571fef185..a83b57e57b6 100644
--- a/drivers/bluetooth/ath3k.c
+++ b/drivers/bluetooth/ath3k.c
@@ -82,6 +82,7 @@ static const struct usb_device_id ath3k_table[] = {
 	{ USB_DEVICE(0x04CA, 0x3004) },
 	{ USB_DEVICE(0x04CA, 0x3005) },
 	{ USB_DEVICE(0x04CA, 0x3006) },
+	{ USB_DEVICE(0x04CA, 0x3007) },
 	{ USB_DEVICE(0x04CA, 0x3008) },
 	{ USB_DEVICE(0x04CA, 0x300b) },
 	{ USB_DEVICE(0x0930, 0x0219) },
@@ -131,6 +132,7 @@ static const struct usb_device_id ath3k_blist_tbl[] = {
 	{ USB_DEVICE(0x04ca, 0x3004), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x3005), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x3006), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x04ca, 0x3007), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x3008), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x300b), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0930, 0x0219), .driver_info = BTUSB_ATH3012 },
diff --git a/drivers/bluetooth/btusb.c b/drivers/bluetooth/btusb.c
index f338b0c5a8d..a7dfbf9a3af 100644
--- a/drivers/bluetooth/btusb.c
+++ b/drivers/bluetooth/btusb.c
@@ -152,6 +152,7 @@ static const struct usb_device_id blacklist_table[] = {
 	{ USB_DEVICE(0x04ca, 0x3004), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x3005), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x3006), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x04ca, 0x3007), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x3008), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x300b), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0930, 0x0219), .driver_info = BTUSB_ATH3012 },
@@ -1485,10 +1486,8 @@ static int btusb_probe(struct usb_interface *intf,
 	if (id->driver_info & BTUSB_BCM92035)
 		hdev->setup = btusb_setup_bcm92035;
 
-	if (id->driver_info & BTUSB_INTEL) {
-		usb_enable_autosuspend(data->udev);
+	if (id->driver_info & BTUSB_INTEL)
 		hdev->setup = btusb_setup_intel;
-	}
 
 	/* Interface numbers are hardcoded in the specification */
 	data->isoc = usb_ifnum_to_if(data->udev, 1);
diff --git a/drivers/bluetooth/hci_ldisc.c b/drivers/bluetooth/hci_ldisc.c
index f1fbf4f1e5b..e00f8f5b5c8 100644
--- a/drivers/bluetooth/hci_ldisc.c
+++ b/drivers/bluetooth/hci_ldisc.c
@@ -118,10 +118,6 @@ static inline struct sk_buff *hci_uart_dequeue(struct hci_uart *hu)
 
 int hci_uart_tx_wakeup(struct hci_uart *hu)
 {
-	struct tty_struct *tty = hu->tty;
-	struct hci_dev *hdev = hu->hdev;
-	struct sk_buff *skb;
-
 	if (test_and_set_bit(HCI_UART_SENDING, &hu->tx_state)) {
 		set_bit(HCI_UART_TX_WAKEUP, &hu->tx_state);
 		return 0;
@@ -129,6 +125,22 @@ int hci_uart_tx_wakeup(struct hci_uart *hu)
 
 	BT_DBG("");
 
+	schedule_work(&hu->write_work);
+
+	return 0;
+}
+
+static void hci_uart_write_work(struct work_struct *work)
+{
+	struct hci_uart *hu = container_of(work, struct hci_uart, write_work);
+	struct tty_struct *tty = hu->tty;
+	struct hci_dev *hdev = hu->hdev;
+	struct sk_buff *skb;
+
+	/* REVISIT: should we cope with bad skbs or ->write() returning
+	 * and error value ?
+	 */
+
 restart:
 	clear_bit(HCI_UART_TX_WAKEUP, &hu->tx_state);
 
@@ -153,7 +165,6 @@ restart:
 		goto restart;
 
 	clear_bit(HCI_UART_SENDING, &hu->tx_state);
-	return 0;
 }
 
 static void hci_uart_init_work(struct work_struct *work)
@@ -282,6 +293,7 @@ static int hci_uart_tty_open(struct tty_struct *tty)
 	tty->receive_room = 65536;
 
 	INIT_WORK(&hu->init_ready, hci_uart_init_work);
+	INIT_WORK(&hu->write_work, hci_uart_write_work);
 
 	spin_lock_init(&hu->rx_lock);
 
@@ -319,6 +331,8 @@ static void hci_uart_tty_close(struct tty_struct *tty)
 	if (hdev)
 		hci_uart_close(hdev);
 
+	cancel_work_sync(&hu->write_work);
+
 	if (test_and_clear_bit(HCI_UART_PROTO_SET, &hu->flags)) {
 		if (hdev) {
 			if (test_bit(HCI_UART_REGISTERED, &hu->flags))
diff --git a/drivers/bluetooth/hci_uart.h b/drivers/bluetooth/hci_uart.h
index fffa61ff5cb..12df101ca94 100644
--- a/drivers/bluetooth/hci_uart.h
+++ b/drivers/bluetooth/hci_uart.h
@@ -68,6 +68,7 @@ struct hci_uart {
 	unsigned long		hdev_flags;
 
 	struct work_struct	init_ready;
+	struct work_struct	write_work;
 
 	struct hci_uart_proto	*proto;
 	void			*priv;
diff --git a/drivers/bus/Kconfig b/drivers/bus/Kconfig
index 552373c4e36..a118ec1650f 100644
--- a/drivers/bus/Kconfig
+++ b/drivers/bus/Kconfig
@@ -4,6 +4,14 @@
 
 menu "Bus devices"
 
+config BRCMSTB_GISB_ARB
+	bool "Broadcom STB GISB bus arbiter"
+	depends on ARM
+	help
+	  Driver for the Broadcom Set Top Box System-on-a-chip internal bus
+	  arbiter. This driver provides timeout and target abort error handling
+	  and internal bus master decoding.
+
 config IMX_WEIM
 	bool "Freescale EIM DRIVER"
 	depends on ARCH_MXC
@@ -41,4 +49,14 @@ config ARM_CCI
 	help
 	  Driver supporting the CCI cache coherent interconnect for ARM
 	  platforms.
+
+config VEXPRESS_CONFIG
+	bool "Versatile Express configuration bus"
+	default y if ARCH_VEXPRESS
+	depends on ARM || ARM64
+	depends on OF
+	select REGMAP
+	help
+	  Platform configuration infrastructure for the ARM Ltd.
+	  Versatile Express.
 endmenu
diff --git a/drivers/bus/Makefile b/drivers/bus/Makefile
index 8947bdd0de8..6a4ea7e4af1 100644
--- a/drivers/bus/Makefile
+++ b/drivers/bus/Makefile
@@ -2,6 +2,7 @@
 # Makefile for the bus drivers.
 #
 
+obj-$(CONFIG_BRCMSTB_GISB_ARB)	+= brcmstb_gisb.o
 obj-$(CONFIG_IMX_WEIM)	+= imx-weim.o
 obj-$(CONFIG_MVEBU_MBUS) += mvebu-mbus.o
 obj-$(CONFIG_OMAP_OCP2SCP)	+= omap-ocp2scp.o
@@ -10,3 +11,5 @@ obj-$(CONFIG_OMAP_OCP2SCP)	+= omap-ocp2scp.o
 obj-$(CONFIG_OMAP_INTERCONNECT)	+= omap_l3_smx.o omap_l3_noc.o
 # CCI cache coherent interconnect for ARM platforms
 obj-$(CONFIG_ARM_CCI)		+= arm-cci.o
+
+obj-$(CONFIG_VEXPRESS_CONFIG)	+= vexpress-config.o
diff --git a/drivers/bus/brcmstb_gisb.c b/drivers/bus/brcmstb_gisb.c
new file mode 100644
index 00000000000..6159b7752a6
--- /dev/null
+++ b/drivers/bus/brcmstb_gisb.c
@@ -0,0 +1,289 @@
+/*
+ * Copyright (C) 2014 Broadcom Corporation
+ *
+ * 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/init.h>
+#include <linux/types.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/sysfs.h>
+#include <linux/io.h>
+#include <linux/string.h>
+#include <linux/device.h>
+#include <linux/list.h>
+#include <linux/of.h>
+#include <linux/bitops.h>
+
+#include <asm/bug.h>
+#include <asm/signal.h>
+
+#define ARB_TIMER			0x008
+#define ARB_ERR_CAP_CLR			0x7e4
+#define  ARB_ERR_CAP_CLEAR		(1 << 0)
+#define ARB_ERR_CAP_HI_ADDR		0x7e8
+#define ARB_ERR_CAP_ADDR		0x7ec
+#define ARB_ERR_CAP_DATA		0x7f0
+#define ARB_ERR_CAP_STATUS		0x7f4
+#define  ARB_ERR_CAP_STATUS_TIMEOUT	(1 << 12)
+#define  ARB_ERR_CAP_STATUS_TEA		(1 << 11)
+#define  ARB_ERR_CAP_STATUS_BS_SHIFT	(1 << 2)
+#define  ARB_ERR_CAP_STATUS_BS_MASK	0x3c
+#define  ARB_ERR_CAP_STATUS_WRITE	(1 << 1)
+#define  ARB_ERR_CAP_STATUS_VALID	(1 << 0)
+#define ARB_ERR_CAP_MASTER		0x7f8
+
+struct brcmstb_gisb_arb_device {
+	void __iomem	*base;
+	struct mutex	lock;
+	struct list_head next;
+	u32 valid_mask;
+	const char *master_names[sizeof(u32) * BITS_PER_BYTE];
+};
+
+static LIST_HEAD(brcmstb_gisb_arb_device_list);
+
+static ssize_t gisb_arb_get_timeout(struct device *dev,
+				    struct device_attribute *attr,
+				    char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct brcmstb_gisb_arb_device *gdev = platform_get_drvdata(pdev);
+	u32 timeout;
+
+	mutex_lock(&gdev->lock);
+	timeout = ioread32(gdev->base + ARB_TIMER);
+	mutex_unlock(&gdev->lock);
+
+	return sprintf(buf, "%d", timeout);
+}
+
+static ssize_t gisb_arb_set_timeout(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct brcmstb_gisb_arb_device *gdev = platform_get_drvdata(pdev);
+	int val, ret;
+
+	ret = kstrtoint(buf, 10, &val);
+	if (ret < 0)
+		return ret;
+
+	if (val == 0 || val >= 0xffffffff)
+		return -EINVAL;
+
+	mutex_lock(&gdev->lock);
+	iowrite32(val, gdev->base + ARB_TIMER);
+	mutex_unlock(&gdev->lock);
+
+	return count;
+}
+
+static const char *
+brcmstb_gisb_master_to_str(struct brcmstb_gisb_arb_device *gdev,
+						u32 masters)
+{
+	u32 mask = gdev->valid_mask & masters;
+
+	if (hweight_long(mask) != 1)
+		return NULL;
+
+	return gdev->master_names[ffs(mask) - 1];
+}
+
+static int brcmstb_gisb_arb_decode_addr(struct brcmstb_gisb_arb_device *gdev,
+					const char *reason)
+{
+	u32 cap_status;
+	unsigned long arb_addr;
+	u32 master;
+	const char *m_name;
+	char m_fmt[11];
+
+	cap_status = ioread32(gdev->base + ARB_ERR_CAP_STATUS);
+
+	/* Invalid captured address, bail out */
+	if (!(cap_status & ARB_ERR_CAP_STATUS_VALID))
+		return 1;
+
+	/* Read the address and master */
+	arb_addr = ioread32(gdev->base + ARB_ERR_CAP_ADDR) & 0xffffffff;
+#if (IS_ENABLED(CONFIG_PHYS_ADDR_T_64BIT))
+	arb_addr |= (u64)ioread32(gdev->base + ARB_ERR_CAP_HI_ADDR) << 32;
+#endif
+	master = ioread32(gdev->base + ARB_ERR_CAP_MASTER);
+
+	m_name = brcmstb_gisb_master_to_str(gdev, master);
+	if (!m_name) {
+		snprintf(m_fmt, sizeof(m_fmt), "0x%08x", master);
+		m_name = m_fmt;
+	}
+
+	pr_crit("%s: %s at 0x%lx [%c %s], core: %s\n",
+		__func__, reason, arb_addr,
+		cap_status & ARB_ERR_CAP_STATUS_WRITE ? 'W' : 'R',
+		cap_status & ARB_ERR_CAP_STATUS_TIMEOUT ? "timeout" : "",
+		m_name);
+
+	/* clear the GISB error */
+	iowrite32(ARB_ERR_CAP_CLEAR, gdev->base + ARB_ERR_CAP_CLR);
+
+	return 0;
+}
+
+static int brcmstb_bus_error_handler(unsigned long addr, unsigned int fsr,
+				     struct pt_regs *regs)
+{
+	int ret = 0;
+	struct brcmstb_gisb_arb_device *gdev;
+
+	/* iterate over each GISB arb registered handlers */
+	list_for_each_entry(gdev, &brcmstb_gisb_arb_device_list, next)
+		ret |= brcmstb_gisb_arb_decode_addr(gdev, "bus error");
+	/*
+	 * If it was an imprecise abort, then we need to correct the
+	 * return address to be _after_ the instruction.
+	*/
+	if (fsr & (1 << 10))
+		regs->ARM_pc += 4;
+
+	return ret;
+}
+
+void __init brcmstb_hook_fault_code(void)
+{
+	hook_fault_code(22, brcmstb_bus_error_handler, SIGBUS, 0,
+			"imprecise external abort");
+}
+
+static irqreturn_t brcmstb_gisb_timeout_handler(int irq, void *dev_id)
+{
+	brcmstb_gisb_arb_decode_addr(dev_id, "timeout");
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t brcmstb_gisb_tea_handler(int irq, void *dev_id)
+{
+	brcmstb_gisb_arb_decode_addr(dev_id, "target abort");
+
+	return IRQ_HANDLED;
+}
+
+static DEVICE_ATTR(gisb_arb_timeout, S_IWUSR | S_IRUGO,
+		gisb_arb_get_timeout, gisb_arb_set_timeout);
+
+static struct attribute *gisb_arb_sysfs_attrs[] = {
+	&dev_attr_gisb_arb_timeout.attr,
+	NULL,
+};
+
+static struct attribute_group gisb_arb_sysfs_attr_group = {
+	.attrs = gisb_arb_sysfs_attrs,
+};
+
+static int brcmstb_gisb_arb_probe(struct platform_device *pdev)
+{
+	struct device_node *dn = pdev->dev.of_node;
+	struct brcmstb_gisb_arb_device *gdev;
+	struct resource *r;
+	int err, timeout_irq, tea_irq;
+	unsigned int num_masters, j = 0;
+	int i, first, last;
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	timeout_irq = platform_get_irq(pdev, 0);
+	tea_irq = platform_get_irq(pdev, 1);
+
+	gdev = devm_kzalloc(&pdev->dev, sizeof(*gdev), GFP_KERNEL);
+	if (!gdev)
+		return -ENOMEM;
+
+	mutex_init(&gdev->lock);
+	INIT_LIST_HEAD(&gdev->next);
+
+	gdev->base = devm_request_and_ioremap(&pdev->dev, r);
+	if (!gdev->base)
+		return -ENOMEM;
+
+	err = devm_request_irq(&pdev->dev, timeout_irq,
+				brcmstb_gisb_timeout_handler, 0, pdev->name,
+				gdev);
+	if (err < 0)
+		return err;
+
+	err = devm_request_irq(&pdev->dev, tea_irq,
+				brcmstb_gisb_tea_handler, 0, pdev->name,
+				gdev);
+	if (err < 0)
+		return err;
+
+	/* If we do not have a valid mask, assume all masters are enabled */
+	if (of_property_read_u32(dn, "brcm,gisb-arb-master-mask",
+				&gdev->valid_mask))
+		gdev->valid_mask = 0xffffffff;
+
+	/* Proceed with reading the litteral names if we agree on the
+	 * number of masters
+	 */
+	num_masters = of_property_count_strings(dn,
+			"brcm,gisb-arb-master-names");
+	if (hweight_long(gdev->valid_mask) == num_masters) {
+		first = ffs(gdev->valid_mask) - 1;
+		last = fls(gdev->valid_mask) - 1;
+
+		for (i = first; i < last; i++) {
+			if (!(gdev->valid_mask & BIT(i)))
+				continue;
+
+			of_property_read_string_index(dn,
+					"brcm,gisb-arb-master-names", j,
+					&gdev->master_names[i]);
+			j++;
+		}
+	}
+
+	err = sysfs_create_group(&pdev->dev.kobj, &gisb_arb_sysfs_attr_group);
+	if (err)
+		return err;
+
+	platform_set_drvdata(pdev, gdev);
+
+	list_add_tail(&gdev->next, &brcmstb_gisb_arb_device_list);
+
+	dev_info(&pdev->dev, "registered mem: %p, irqs: %d, %d\n",
+			gdev->base, timeout_irq, tea_irq);
+
+	return 0;
+}
+
+static const struct of_device_id brcmstb_gisb_arb_of_match[] = {
+	{ .compatible = "brcm,gisb-arb" },
+	{ },
+};
+
+static struct platform_driver brcmstb_gisb_arb_driver = {
+	.probe	= brcmstb_gisb_arb_probe,
+	.driver = {
+		.name	= "brcm-gisb-arb",
+		.owner	= THIS_MODULE,
+		.of_match_table = brcmstb_gisb_arb_of_match,
+	},
+};
+
+static int __init brcm_gisb_driver_init(void)
+{
+	return platform_driver_register(&brcmstb_gisb_arb_driver);
+}
+
+module_init(brcm_gisb_driver_init);
diff --git a/drivers/bus/mvebu-mbus.c b/drivers/bus/mvebu-mbus.c
index 293e2e0a0a8..26c3779d871 100644
--- a/drivers/bus/mvebu-mbus.c
+++ b/drivers/bus/mvebu-mbus.c
@@ -56,6 +56,7 @@
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/debugfs.h>
+#include <linux/log2.h>
 
 /*
  * DDR target is the same on all platforms.
@@ -222,12 +223,6 @@ static int mvebu_mbus_window_conflicts(struct mvebu_mbus_state *mbus,
 		 */
 		if ((u64)base < wend && end > wbase)
 			return 0;
-
-		/*
-		 * Check if target/attribute conflicts
-		 */
-		if (target == wtarget && attr == wattr)
-			return 0;
 	}
 
 	return 1;
@@ -266,6 +261,17 @@ static int mvebu_mbus_setup_window(struct mvebu_mbus_state *mbus,
 		mbus->soc->win_cfg_offset(win);
 	u32 ctrl, remap_addr;
 
+	if (!is_power_of_2(size)) {
+		WARN(true, "Invalid MBus window size: 0x%zx\n", size);
+		return -EINVAL;
+	}
+
+	if ((base & (phys_addr_t)(size - 1)) != 0) {
+		WARN(true, "Invalid MBus base/size: %pa len 0x%zx\n", &base,
+		     size);
+		return -EINVAL;
+	}
+
 	ctrl = ((size - 1) & WIN_CTRL_SIZE_MASK) |
 		(attr << WIN_CTRL_ATTR_SHIFT)    |
 		(target << WIN_CTRL_TGT_SHIFT)   |
@@ -413,6 +419,10 @@ static int mvebu_devs_debug_show(struct seq_file *seq, void *v)
 			   win, (unsigned long long)wbase,
 			   (unsigned long long)(wbase + wsize), wtarget, wattr);
 
+		if (!is_power_of_2(wsize) ||
+		    ((wbase & (u64)(wsize - 1)) != 0))
+			seq_puts(seq, " (Invalid base/size!!)");
+
 		if (win < mbus->soc->num_remappable_wins) {
 			seq_printf(seq, " (remap %016llx)\n",
 				   (unsigned long long)wremap);
@@ -694,7 +704,6 @@ static int __init mvebu_mbus_common_init(struct mvebu_mbus_state *mbus,
 					 phys_addr_t sdramwins_phys_base,
 					 size_t sdramwins_size)
 {
-	struct device_node *np;
 	int win;
 
 	mbus->mbuswins_base = ioremap(mbuswins_phys_base, mbuswins_size);
@@ -707,12 +716,6 @@ static int __init mvebu_mbus_common_init(struct mvebu_mbus_state *mbus,
 		return -ENOMEM;
 	}
 
-	np = of_find_compatible_node(NULL, NULL, "marvell,coherency-fabric");
-	if (np) {
-		mbus->hw_io_coherency = 1;
-		of_node_put(np);
-	}
-
 	for (win = 0; win < mbus->soc->num_wins; win++)
 		mvebu_mbus_disable_window(mbus, win);
 
@@ -882,7 +885,7 @@ static void __init mvebu_mbus_get_pcie_resources(struct device_node *np,
 	}
 }
 
-int __init mvebu_mbus_dt_init(void)
+int __init mvebu_mbus_dt_init(bool is_coherent)
 {
 	struct resource mbuswins_res, sdramwins_res;
 	struct device_node *np, *controller;
@@ -920,6 +923,8 @@ int __init mvebu_mbus_dt_init(void)
 		return -EINVAL;
 	}
 
+	mbus_state.hw_io_coherency = is_coherent;
+
 	/* Get optional pcie-{mem,io}-aperture properties */
 	mvebu_mbus_get_pcie_resources(np, &mbus_state.pcie_mem_aperture,
 					  &mbus_state.pcie_io_aperture);
diff --git a/drivers/bus/omap_l3_noc.c b/drivers/bus/omap_l3_noc.c
index feeecae623f..531ae591783 100644
--- a/drivers/bus/omap_l3_noc.c
+++ b/drivers/bus/omap_l3_noc.c
@@ -1,43 +1,45 @@
 /*
- * OMAP4XXX L3 Interconnect error handling driver
+ * OMAP L3 Interconnect error handling driver
  *
- * Copyright (C) 2011 Texas Corporation
+ * Copyright (C) 2011-2014 Texas Instruments Incorporated - http://www.ti.com/
  *	Santosh Shilimkar <santosh.shilimkar@ti.com>
  *	Sricharan <r.sricharan@ti.com>
  *
  * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
  *
- * 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
+ * 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.
- *
- * 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/io.h>
-#include <linux/platform_device.h>
 #include <linux/interrupt.h>
+#include <linux/io.h>
 #include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
 #include <linux/slab.h>
 
 #include "omap_l3_noc.h"
 
-/*
- * Interrupt Handler for L3 error detection.
- *	1) Identify the L3 clockdomain partition to which the error belongs to.
- *	2) Identify the slave where the error information is logged
- *	3) Print the logged information.
- *	4) Add dump stack to provide kernel trace.
+/**
+ * l3_handle_target() - Handle Target specific parse and reporting
+ * @l3:		pointer to l3 struct
+ * @base:	base address of clkdm
+ * @flag_mux:	flagmux corresponding to the event
+ * @err_src:	error source index of the slave (target)
  *
- * Two Types of errors :
+ * This does the second part of the error interrupt handling:
+ *	3) Parse in the slave information
+ *	4) Print the logged information.
+ *	5) Add dump stack to provide kernel trace.
+ *	6) Clear the source if known.
+ *
+ * This handles two types of errors:
  *	1) Custom errors in L3 :
  *		Target like DMM/FW/EMIF generates SRESP=ERR error
  *	2) Standard L3 error:
@@ -53,214 +55,264 @@
  *	can be trapped as well. But the trapping is implemented as part
  *	secure software and hence need not be implemented here.
  */
-static irqreturn_t l3_interrupt_handler(int irq, void *_l3)
+static int l3_handle_target(struct omap_l3 *l3, void __iomem *base,
+			    struct l3_flagmux_data *flag_mux, int err_src)
 {
+	int k;
+	u32 std_err_main, clear, masterid;
+	u8 op_code, m_req_info;
+	void __iomem *l3_targ_base;
+	void __iomem *l3_targ_stderr, *l3_targ_slvofslsb, *l3_targ_mstaddr;
+	void __iomem *l3_targ_hdr, *l3_targ_info;
+	struct l3_target_data *l3_targ_inst;
+	struct l3_masters_data *master;
+	char *target_name, *master_name = "UN IDENTIFIED";
+	char *err_description;
+	char err_string[30] = { 0 };
+	char info_string[60] = { 0 };
+
+	/* We DONOT expect err_src to go out of bounds */
+	BUG_ON(err_src > MAX_CLKDM_TARGETS);
+
+	if (err_src < flag_mux->num_targ_data) {
+		l3_targ_inst = &flag_mux->l3_targ[err_src];
+		target_name = l3_targ_inst->name;
+		l3_targ_base = base + l3_targ_inst->offset;
+	} else {
+		target_name = L3_TARGET_NOT_SUPPORTED;
+	}
 
-	struct omap4_l3 *l3 = _l3;
-	int inttype, i, k;
+	if (target_name == L3_TARGET_NOT_SUPPORTED)
+		return -ENODEV;
+
+	/* Read the stderrlog_main_source from clk domain */
+	l3_targ_stderr = l3_targ_base + L3_TARG_STDERRLOG_MAIN;
+	l3_targ_slvofslsb = l3_targ_base + L3_TARG_STDERRLOG_SLVOFSLSB;
+
+	std_err_main = readl_relaxed(l3_targ_stderr);
+
+	switch (std_err_main & CUSTOM_ERROR) {
+	case STANDARD_ERROR:
+		err_description = "Standard";
+		snprintf(err_string, sizeof(err_string),
+			 ": At Address: 0x%08X ",
+			 readl_relaxed(l3_targ_slvofslsb));
+
+		l3_targ_mstaddr = l3_targ_base + L3_TARG_STDERRLOG_MSTADDR;
+		l3_targ_hdr = l3_targ_base + L3_TARG_STDERRLOG_HDR;
+		l3_targ_info = l3_targ_base + L3_TARG_STDERRLOG_INFO;
+		break;
+
+	case CUSTOM_ERROR:
+		err_description = "Custom";
+
+		l3_targ_mstaddr = l3_targ_base +
+				  L3_TARG_STDERRLOG_CINFO_MSTADDR;
+		l3_targ_hdr = l3_targ_base + L3_TARG_STDERRLOG_CINFO_OPCODE;
+		l3_targ_info = l3_targ_base + L3_TARG_STDERRLOG_CINFO_INFO;
+		break;
+
+	default:
+		/* Nothing to be handled here as of now */
+		return 0;
+	}
+
+	/* STDERRLOG_MSTADDR Stores the NTTP master address. */
+	masterid = (readl_relaxed(l3_targ_mstaddr) &
+		    l3->mst_addr_mask) >> __ffs(l3->mst_addr_mask);
+
+	for (k = 0, master = l3->l3_masters; k < l3->num_masters;
+	     k++, master++) {
+		if (masterid == master->id) {
+			master_name = master->name;
+			break;
+		}
+	}
+
+	op_code = readl_relaxed(l3_targ_hdr) & 0x7;
+
+	m_req_info = readl_relaxed(l3_targ_info) & 0xF;
+	snprintf(info_string, sizeof(info_string),
+		 ": %s in %s mode during %s access",
+		 (m_req_info & BIT(0)) ? "Opcode Fetch" : "Data Access",
+		 (m_req_info & BIT(1)) ? "Supervisor" : "User",
+		 (m_req_info & BIT(3)) ? "Debug" : "Functional");
+
+	WARN(true,
+	     "%s:L3 %s Error: MASTER %s TARGET %s (%s)%s%s\n",
+	     dev_name(l3->dev),
+	     err_description,
+	     master_name, target_name,
+	     l3_transaction_type[op_code],
+	     err_string, info_string);
+
+	/* clear the std error log*/
+	clear = std_err_main | CLEAR_STDERR_LOG;
+	writel_relaxed(clear, l3_targ_stderr);
+
+	return 0;
+}
+
+/**
+ * l3_interrupt_handler() - interrupt handler for l3 events
+ * @irq:	irq number
+ * @_l3:	pointer to l3 structure
+ *
+ * Interrupt Handler for L3 error detection.
+ *	1) Identify the L3 clockdomain partition to which the error belongs to.
+ *	2) Identify the slave where the error information is logged
+ *	... handle the slave event..
+ *	7) if the slave is unknown, mask out the slave.
+ */
+static irqreturn_t l3_interrupt_handler(int irq, void *_l3)
+{
+	struct omap_l3 *l3 = _l3;
+	int inttype, i, ret;
 	int err_src = 0;
-	u32 std_err_main, err_reg, clear, masterid;
-	void __iomem *base, *l3_targ_base;
-	char *target_name, *master_name = "UN IDENTIFIED";
+	u32 err_reg, mask_val;
+	void __iomem *base, *mask_reg;
+	struct l3_flagmux_data *flag_mux;
 
 	/* Get the Type of interrupt */
 	inttype = irq == l3->app_irq ? L3_APPLICATION_ERROR : L3_DEBUG_ERROR;
 
-	for (i = 0; i < L3_MODULES; i++) {
+	for (i = 0; i < l3->num_modules; i++) {
 		/*
 		 * Read the regerr register of the clock domain
 		 * to determine the source
 		 */
 		base = l3->l3_base[i];
-		err_reg = __raw_readl(base + l3_flagmux[i] +
-					+ L3_FLAGMUX_REGERR0 + (inttype << 3));
+		flag_mux = l3->l3_flagmux[i];
+		err_reg = readl_relaxed(base + flag_mux->offset +
+					L3_FLAGMUX_REGERR0 + (inttype << 3));
+
+		err_reg &= ~(inttype ? flag_mux->mask_app_bits :
+				flag_mux->mask_dbg_bits);
 
 		/* Get the corresponding error and analyse */
 		if (err_reg) {
 			/* Identify the source from control status register */
 			err_src = __ffs(err_reg);
 
-			/* Read the stderrlog_main_source from clk domain */
-			l3_targ_base = base + *(l3_targ[i] + err_src);
-			std_err_main =  __raw_readl(l3_targ_base +
-					L3_TARG_STDERRLOG_MAIN);
-			masterid = __raw_readl(l3_targ_base +
-					L3_TARG_STDERRLOG_MSTADDR);
-
-			switch (std_err_main & CUSTOM_ERROR) {
-			case STANDARD_ERROR:
-				target_name =
-					l3_targ_inst_name[i][err_src];
-				WARN(true, "L3 standard error: TARGET:%s at address 0x%x\n",
-					target_name,
-					__raw_readl(l3_targ_base +
-						L3_TARG_STDERRLOG_SLVOFSLSB));
-				/* clear the std error log*/
-				clear = std_err_main | CLEAR_STDERR_LOG;
-				writel(clear, l3_targ_base +
-					L3_TARG_STDERRLOG_MAIN);
-				break;
-
-			case CUSTOM_ERROR:
-				target_name =
-					l3_targ_inst_name[i][err_src];
-				for (k = 0; k < NUM_OF_L3_MASTERS; k++) {
-					if (masterid == l3_masters[k].id)
-						master_name =
-							l3_masters[k].name;
-				}
-				WARN(true, "L3 custom error: MASTER:%s TARGET:%s\n",
-					master_name, target_name);
-				/* clear the std error log*/
-				clear = std_err_main | CLEAR_STDERR_LOG;
-				writel(clear, l3_targ_base +
-					L3_TARG_STDERRLOG_MAIN);
-				break;
-
-			default:
-				/* Nothing to be handled here as of now */
-				break;
+			ret = l3_handle_target(l3, base, flag_mux, err_src);
+
+			/*
+			 * Certain plaforms may have "undocumented" status
+			 * pending on boot. So dont generate a severe warning
+			 * here. Just mask it off to prevent the error from
+			 * reoccuring and locking up the system.
+			 */
+			if (ret) {
+				dev_err(l3->dev,
+					"L3 %s error: target %d mod:%d %s\n",
+					inttype ? "debug" : "application",
+					err_src, i, "(unclearable)");
+
+				mask_reg = base + flag_mux->offset +
+					   L3_FLAGMUX_MASK0 + (inttype << 3);
+				mask_val = readl_relaxed(mask_reg);
+				mask_val &= ~(1 << err_src);
+				writel_relaxed(mask_val, mask_reg);
+
+				/* Mark these bits as to be ignored */
+				if (inttype)
+					flag_mux->mask_app_bits |= 1 << err_src;
+				else
+					flag_mux->mask_dbg_bits |= 1 << err_src;
 			}
-		/* Error found so break the for loop */
-		break;
+
+			/* Error found so break the for loop */
+			break;
 		}
 	}
 	return IRQ_HANDLED;
 }
 
-static int omap4_l3_probe(struct platform_device *pdev)
+static const struct of_device_id l3_noc_match[] = {
+	{.compatible = "ti,omap4-l3-noc", .data = &omap_l3_data},
+	{.compatible = "ti,dra7-l3-noc", .data = &dra_l3_data},
+	{.compatible = "ti,am4372-l3-noc", .data = &am4372_l3_data},
+	{},
+};
+MODULE_DEVICE_TABLE(of, l3_noc_match);
+
+static int omap_l3_probe(struct platform_device *pdev)
 {
-	static struct omap4_l3 *l3;
-	struct resource	*res;
-	int ret;
+	const struct of_device_id *of_id;
+	static struct omap_l3 *l3;
+	int ret, i, res_idx;
+
+	of_id = of_match_device(l3_noc_match, &pdev->dev);
+	if (!of_id) {
+		dev_err(&pdev->dev, "OF data missing\n");
+		return -EINVAL;
+	}
 
-	l3 = kzalloc(sizeof(*l3), GFP_KERNEL);
+	l3 = devm_kzalloc(&pdev->dev, sizeof(*l3), GFP_KERNEL);
 	if (!l3)
 		return -ENOMEM;
 
+	memcpy(l3, of_id->data, sizeof(*l3));
+	l3->dev = &pdev->dev;
 	platform_set_drvdata(pdev, l3);
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&pdev->dev, "couldn't find resource 0\n");
-		ret = -ENODEV;
-		goto err0;
-	}
-
-	l3->l3_base[0] = ioremap(res->start, resource_size(res));
-	if (!l3->l3_base[0]) {
-		dev_err(&pdev->dev, "ioremap failed\n");
-		ret = -ENOMEM;
-		goto err0;
-	}
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-	if (!res) {
-		dev_err(&pdev->dev, "couldn't find resource 1\n");
-		ret = -ENODEV;
-		goto err1;
-	}
-
-	l3->l3_base[1] = ioremap(res->start, resource_size(res));
-	if (!l3->l3_base[1]) {
-		dev_err(&pdev->dev, "ioremap failed\n");
-		ret = -ENOMEM;
-		goto err1;
-	}
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 2);
-	if (!res) {
-		dev_err(&pdev->dev, "couldn't find resource 2\n");
-		ret = -ENODEV;
-		goto err2;
-	}
+	/* Get mem resources */
+	for (i = 0, res_idx = 0; i < l3->num_modules; i++) {
+		struct resource	*res;
 
-	l3->l3_base[2] = ioremap(res->start, resource_size(res));
-	if (!l3->l3_base[2]) {
-		dev_err(&pdev->dev, "ioremap failed\n");
-		ret = -ENOMEM;
-		goto err2;
+		if (l3->l3_base[i] == L3_BASE_IS_SUBMODULE) {
+			/* First entry cannot be submodule */
+			BUG_ON(i == 0);
+			l3->l3_base[i] = l3->l3_base[i - 1];
+			continue;
+		}
+		res = platform_get_resource(pdev, IORESOURCE_MEM, res_idx);
+		l3->l3_base[i] = devm_ioremap_resource(&pdev->dev, res);
+		if (IS_ERR(l3->l3_base[i])) {
+			dev_err(l3->dev, "ioremap %d failed\n", i);
+			return PTR_ERR(l3->l3_base[i]);
+		}
+		res_idx++;
 	}
 
 	/*
 	 * Setup interrupt Handlers
 	 */
 	l3->debug_irq = platform_get_irq(pdev, 0);
-	ret = request_irq(l3->debug_irq,
-			l3_interrupt_handler,
-			IRQF_DISABLED, "l3-dbg-irq", l3);
+	ret = devm_request_irq(l3->dev, l3->debug_irq, l3_interrupt_handler,
+			       IRQF_DISABLED, "l3-dbg-irq", l3);
 	if (ret) {
-		pr_crit("L3: request_irq failed to register for 0x%x\n",
-						l3->debug_irq);
-		goto err3;
+		dev_err(l3->dev, "request_irq failed for %d\n",
+			l3->debug_irq);
+		return ret;
 	}
 
 	l3->app_irq = platform_get_irq(pdev, 1);
-	ret = request_irq(l3->app_irq,
-			l3_interrupt_handler,
-			IRQF_DISABLED, "l3-app-irq", l3);
-	if (ret) {
-		pr_crit("L3: request_irq failed to register for 0x%x\n",
-						l3->app_irq);
-		goto err4;
-	}
+	ret = devm_request_irq(l3->dev, l3->app_irq, l3_interrupt_handler,
+			       IRQF_DISABLED, "l3-app-irq", l3);
+	if (ret)
+		dev_err(l3->dev, "request_irq failed for %d\n", l3->app_irq);
 
-	return 0;
-
-err4:
-	free_irq(l3->debug_irq, l3);
-err3:
-	iounmap(l3->l3_base[2]);
-err2:
-	iounmap(l3->l3_base[1]);
-err1:
-	iounmap(l3->l3_base[0]);
-err0:
-	kfree(l3);
 	return ret;
 }
 
-static int omap4_l3_remove(struct platform_device *pdev)
-{
-	struct omap4_l3 *l3 = platform_get_drvdata(pdev);
-
-	free_irq(l3->app_irq, l3);
-	free_irq(l3->debug_irq, l3);
-	iounmap(l3->l3_base[0]);
-	iounmap(l3->l3_base[1]);
-	iounmap(l3->l3_base[2]);
-	kfree(l3);
-
-	return 0;
-}
-
-#if defined(CONFIG_OF)
-static const struct of_device_id l3_noc_match[] = {
-	{.compatible = "ti,omap4-l3-noc", },
-	{},
-};
-MODULE_DEVICE_TABLE(of, l3_noc_match);
-#else
-#define l3_noc_match NULL
-#endif
-
-static struct platform_driver omap4_l3_driver = {
-	.probe		= omap4_l3_probe,
-	.remove		= omap4_l3_remove,
+static struct platform_driver omap_l3_driver = {
+	.probe		= omap_l3_probe,
 	.driver		= {
 		.name		= "omap_l3_noc",
 		.owner		= THIS_MODULE,
-		.of_match_table = l3_noc_match,
+		.of_match_table = of_match_ptr(l3_noc_match),
 	},
 };
 
-static int __init omap4_l3_init(void)
+static int __init omap_l3_init(void)
 {
-	return platform_driver_register(&omap4_l3_driver);
+	return platform_driver_register(&omap_l3_driver);
 }
-postcore_initcall_sync(omap4_l3_init);
+postcore_initcall_sync(omap_l3_init);
 
-static void __exit omap4_l3_exit(void)
+static void __exit omap_l3_exit(void)
 {
-	platform_driver_unregister(&omap4_l3_driver);
+	platform_driver_unregister(&omap_l3_driver);
 }
-module_exit(omap4_l3_exit);
+module_exit(omap_l3_exit);
diff --git a/drivers/bus/omap_l3_noc.h b/drivers/bus/omap_l3_noc.h
index a6ce34dc481..551e0106143 100644
--- a/drivers/bus/omap_l3_noc.h
+++ b/drivers/bus/omap_l3_noc.h
@@ -1,29 +1,25 @@
 /*
- * OMAP4XXX L3 Interconnect  error handling driver header
+ * OMAP L3 Interconnect  error handling driver header
  *
- * Copyright (C) 2011 Texas Corporation
+ * Copyright (C) 2011-2014 Texas Instruments Incorporated - http://www.ti.com/
  *	Santosh Shilimkar <santosh.shilimkar@ti.com>
  *	sricharan <r.sricharan@ti.com>
  *
  * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
  *
- * 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
+ * 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.
- *
- * 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 __ARCH_ARM_MACH_OMAP2_L3_INTERCONNECT_3XXX_H
-#define __ARCH_ARM_MACH_OMAP2_L3_INTERCONNECT_3XXX_H
+#ifndef __OMAP_L3_NOC_H
+#define __OMAP_L3_NOC_H
+
+#define MAX_L3_MODULES			3
+#define MAX_CLKDM_TARGETS		31
 
-#define L3_MODULES			3
 #define CLEAR_STDERR_LOG		(1 << 31)
 #define CUSTOM_ERROR			0x2
 #define STANDARD_ERROR			0x0
@@ -33,63 +29,165 @@
 
 /* L3 TARG register offsets */
 #define L3_TARG_STDERRLOG_MAIN		0x48
+#define L3_TARG_STDERRLOG_HDR		0x4c
+#define L3_TARG_STDERRLOG_MSTADDR	0x50
+#define L3_TARG_STDERRLOG_INFO		0x58
 #define L3_TARG_STDERRLOG_SLVOFSLSB	0x5c
-#define L3_TARG_STDERRLOG_MSTADDR	0x68
+#define L3_TARG_STDERRLOG_CINFO_INFO	0x64
+#define L3_TARG_STDERRLOG_CINFO_MSTADDR	0x68
+#define L3_TARG_STDERRLOG_CINFO_OPCODE	0x6c
 #define L3_FLAGMUX_REGERR0		0xc
+#define L3_FLAGMUX_MASK0		0x8
+
+#define L3_TARGET_NOT_SUPPORTED		NULL
+
+#define L3_BASE_IS_SUBMODULE		((void __iomem *)(1 << 0))
+
+static const char * const l3_transaction_type[] = {
+	/* 0 0 0 */ "Idle",
+	/* 0 0 1 */ "Write",
+	/* 0 1 0 */ "Read",
+	/* 0 1 1 */ "ReadEx",
+	/* 1 0 0 */ "Read Link",
+	/* 1 0 1 */ "Write Non-Posted",
+	/* 1 1 0 */ "Write Conditional",
+	/* 1 1 1 */ "Write Broadcast",
+};
 
-#define NUM_OF_L3_MASTERS	(sizeof(l3_masters)/sizeof(l3_masters[0]))
-
-static u32 l3_flagmux[L3_MODULES] = {
-	0x500,
-	0x1000,
-	0X0200
-};
-
-/* L3 Target standard Error register offsets */
-static u32 l3_targ_inst_clk1[] = {
-	0x100, /* DMM1 */
-	0x200, /* DMM2 */
-	0x300, /* ABE */
-	0x400, /* L4CFG */
-	0x600,  /* CLK2 PWR DISC */
-	0x0,	/* Host CLK1 */
-	0x900	/* L4 Wakeup */
-};
-
-static u32 l3_targ_inst_clk2[] = {
-	0x500, /* CORTEX M3 */
-	0x300, /* DSS */
-	0x100, /* GPMC */
-	0x400, /* ISS */
-	0x700, /* IVAHD */
-	0xD00, /* missing in TRM  corresponds to AES1*/
-	0x900, /* L4 PER0*/
-	0x200, /* OCMRAM */
-	0x100, /* missing in TRM corresponds to GPMC sERROR*/
-	0x600, /* SGX */
-	0x800, /* SL2 */
-	0x1600, /* C2C */
-	0x1100,	/* missing in TRM corresponds PWR DISC CLK1*/
-	0xF00, /* missing in TRM corrsponds to SHA1*/
-	0xE00, /* missing in TRM corresponds to AES2*/
-	0xC00, /* L4 PER3 */
-	0xA00, /* L4 PER1*/
-	0xB00, /* L4 PER2*/
-	0x0, /* HOST CLK2 */
-	0x1800, /* CAL */
-	0x1700 /* LLI */
-};
-
-static u32 l3_targ_inst_clk3[] = {
-	0x0100	/* EMUSS */,
-	0x0300, /* DEBUGSS_CT_TBR */
-	0x0 /* HOST CLK3 */
-};
-
-static struct l3_masters_data {
+/**
+ * struct l3_masters_data - L3 Master information
+ * @id:		ID of the L3 Master
+ * @name:	master name
+ */
+struct l3_masters_data {
 	u32 id;
-	char name[10];
-} l3_masters[] = {
+	char *name;
+};
+
+/**
+ * struct l3_target_data - L3 Target information
+ * @offset:	Offset from base for L3 Target
+ * @name:	Target name
+ *
+ * Target information is organized indexed by bit field definitions.
+ */
+struct l3_target_data {
+	u32 offset;
+	char *name;
+};
+
+/**
+ * struct l3_flagmux_data - Flag Mux information
+ * @offset:	offset from base for flagmux register
+ * @l3_targ:	array indexed by flagmux index (bit offset) pointing to the
+ *		target data. unsupported ones are marked with
+ *		L3_TARGET_NOT_SUPPORTED
+ * @num_targ_data: number of entries in target data
+ * @mask_app_bits: ignore these from raw application irq status
+ * @mask_dbg_bits: ignore these from raw debug irq status
+ */
+struct l3_flagmux_data {
+	u32 offset;
+	struct l3_target_data *l3_targ;
+	u8 num_targ_data;
+	u32 mask_app_bits;
+	u32 mask_dbg_bits;
+};
+
+
+/**
+ * struct omap_l3 - Description of data relevant for L3 bus.
+ * @dev:	device representing the bus (populated runtime)
+ * @l3_base:	base addresses of modules (populated runtime if 0)
+ *		if set to L3_BASE_IS_SUBMODULE, then uses previous
+ *		module index as the base address
+ * @l3_flag_mux: array containing flag mux data per module
+ *		 offset from corresponding module base indexed per
+ *		 module.
+ * @num_modules: number of clock domains / modules.
+ * @l3_masters:	array pointing to master data containing name and register
+ *		offset for the master.
+ * @num_master: number of masters
+ * @mst_addr_mask: Mask representing MSTADDR information of NTTP packet
+ * @debug_irq:	irq number of the debug interrupt (populated runtime)
+ * @app_irq:	irq number of the application interrupt (populated runtime)
+ */
+struct omap_l3 {
+	struct device *dev;
+
+	void __iomem *l3_base[MAX_L3_MODULES];
+	struct l3_flagmux_data **l3_flagmux;
+	int num_modules;
+
+	struct l3_masters_data *l3_masters;
+	int num_masters;
+	u32 mst_addr_mask;
+
+	int debug_irq;
+	int app_irq;
+};
+
+static struct l3_target_data omap_l3_target_data_clk1[] = {
+	{0x100,	"DMM1",},
+	{0x200,	"DMM2",},
+	{0x300,	"ABE",},
+	{0x400,	"L4CFG",},
+	{0x600,	"CLK2PWRDISC",},
+	{0x0,	"HOSTCLK1",},
+	{0x900,	"L4WAKEUP",},
+};
+
+static struct l3_flagmux_data omap_l3_flagmux_clk1 = {
+	.offset = 0x500,
+	.l3_targ = omap_l3_target_data_clk1,
+	.num_targ_data = ARRAY_SIZE(omap_l3_target_data_clk1),
+};
+
+
+static struct l3_target_data omap_l3_target_data_clk2[] = {
+	{0x500,	"CORTEXM3",},
+	{0x300,	"DSS",},
+	{0x100,	"GPMC",},
+	{0x400,	"ISS",},
+	{0x700,	"IVAHD",},
+	{0xD00,	"AES1",},
+	{0x900,	"L4PER0",},
+	{0x200,	"OCMRAM",},
+	{0x100,	"GPMCsERROR",},
+	{0x600,	"SGX",},
+	{0x800,	"SL2",},
+	{0x1600, "C2C",},
+	{0x1100, "PWRDISCCLK1",},
+	{0xF00,	"SHA1",},
+	{0xE00,	"AES2",},
+	{0xC00,	"L4PER3",},
+	{0xA00,	"L4PER1",},
+	{0xB00,	"L4PER2",},
+	{0x0,	"HOSTCLK2",},
+	{0x1800, "CAL",},
+	{0x1700, "LLI",},
+};
+
+static struct l3_flagmux_data omap_l3_flagmux_clk2 = {
+	.offset = 0x1000,
+	.l3_targ = omap_l3_target_data_clk2,
+	.num_targ_data = ARRAY_SIZE(omap_l3_target_data_clk2),
+};
+
+
+static struct l3_target_data omap_l3_target_data_clk3[] = {
+	{0x0100, "EMUSS",},
+	{0x0300, "DEBUG SOURCE",},
+	{0x0,	"HOST CLK3",},
+};
+
+static struct l3_flagmux_data omap_l3_flagmux_clk3 = {
+	.offset = 0x0200,
+	.l3_targ = omap_l3_target_data_clk3,
+	.num_targ_data = ARRAY_SIZE(omap_l3_target_data_clk3),
+};
+
+static struct l3_masters_data omap_l3_masters[] = {
 	{ 0x0 , "MPU"},
 	{ 0x10, "CS_ADP"},
 	{ 0x14, "xxx"},
@@ -117,60 +215,261 @@ static struct l3_masters_data {
 	{ 0xC8, "USBHOSTFS"}
 };
 
-static char *l3_targ_inst_name[L3_MODULES][21] = {
-	{
-		"DMM1",
-		"DMM2",
-		"ABE",
-		"L4CFG",
-		"CLK2 PWR DISC",
-		"HOST CLK1",
-		"L4 WAKEUP"
-	},
-	{
-		"CORTEX M3" ,
-		"DSS ",
-		"GPMC ",
-		"ISS ",
-		"IVAHD ",
-		"AES1",
-		"L4 PER0",
-		"OCMRAM ",
-		"GPMC sERROR",
-		"SGX ",
-		"SL2 ",
-		"C2C ",
-		"PWR DISC CLK1",
-		"SHA1",
-		"AES2",
-		"L4 PER3",
-		"L4 PER1",
-		"L4 PER2",
-		"HOST CLK2",
-		"CAL",
-		"LLI"
-	},
-	{
-		"EMUSS",
-		"DEBUG SOURCE",
-		"HOST CLK3"
-	},
-};
-
-static u32 *l3_targ[L3_MODULES] = {
-	l3_targ_inst_clk1,
-	l3_targ_inst_clk2,
-	l3_targ_inst_clk3,
-};
-
-struct omap4_l3 {
-	struct device *dev;
-	struct clk *ick;
+static struct l3_flagmux_data *omap_l3_flagmux[] = {
+	&omap_l3_flagmux_clk1,
+	&omap_l3_flagmux_clk2,
+	&omap_l3_flagmux_clk3,
+};
+
+static const struct omap_l3 omap_l3_data = {
+	.l3_flagmux = omap_l3_flagmux,
+	.num_modules = ARRAY_SIZE(omap_l3_flagmux),
+	.l3_masters = omap_l3_masters,
+	.num_masters = ARRAY_SIZE(omap_l3_masters),
+	/* The 6 MSBs of register field used to distinguish initiator */
+	.mst_addr_mask = 0xFC,
+};
 
-	/* memory base */
-	void __iomem *l3_base[L3_MODULES];
+/* DRA7 data */
+static struct l3_target_data dra_l3_target_data_clk1[] = {
+	{0x2a00, "AES1",},
+	{0x0200, "DMM_P1",},
+	{0x0600, "DSP2_SDMA",},
+	{0x0b00, "EVE2",},
+	{0x1300, "DMM_P2",},
+	{0x2c00, "AES2",},
+	{0x0300, "DSP1_SDMA",},
+	{0x0a00, "EVE1",},
+	{0x0c00, "EVE3",},
+	{0x0d00, "EVE4",},
+	{0x2900, "DSS",},
+	{0x0100, "GPMC",},
+	{0x3700, "PCIE1",},
+	{0x1600, "IVA_CONFIG",},
+	{0x1800, "IVA_SL2IF",},
+	{0x0500, "L4_CFG",},
+	{0x1d00, "L4_WKUP",},
+	{0x3800, "PCIE2",},
+	{0x3300, "SHA2_1",},
+	{0x1200, "GPU",},
+	{0x1000, "IPU1",},
+	{0x1100, "IPU2",},
+	{0x2000, "TPCC_EDMA",},
+	{0x2e00, "TPTC1_EDMA",},
+	{0x2b00, "TPTC2_EDMA",},
+	{0x0700, "VCP1",},
+	{0x2500, "L4_PER2_P3",},
+	{0x0e00, "L4_PER3_P3",},
+	{0x2200, "MMU1",},
+	{0x1400, "PRUSS1",},
+	{0x1500, "PRUSS2"},
+	{0x0800, "VCP1",},
+};
 
-	int debug_irq;
-	int app_irq;
+static struct l3_flagmux_data dra_l3_flagmux_clk1 = {
+	.offset = 0x803500,
+	.l3_targ = dra_l3_target_data_clk1,
+	.num_targ_data = ARRAY_SIZE(dra_l3_target_data_clk1),
+};
+
+static struct l3_target_data dra_l3_target_data_clk2[] = {
+	{0x0,	"HOST CLK1",},
+	{0x0,	"HOST CLK2",},
+	{0xdead, L3_TARGET_NOT_SUPPORTED,},
+	{0x3400, "SHA2_2",},
+	{0x0900, "BB2D",},
+	{0xdead, L3_TARGET_NOT_SUPPORTED,},
+	{0x2100, "L4_PER1_P3",},
+	{0x1c00, "L4_PER1_P1",},
+	{0x1f00, "L4_PER1_P2",},
+	{0x2300, "L4_PER2_P1",},
+	{0x2400, "L4_PER2_P2",},
+	{0x2600, "L4_PER3_P1",},
+	{0x2700, "L4_PER3_P2",},
+	{0x2f00, "MCASP1",},
+	{0x3000, "MCASP2",},
+	{0x3100, "MCASP3",},
+	{0x2800, "MMU2",},
+	{0x0f00, "OCMC_RAM1",},
+	{0x1700, "OCMC_RAM2",},
+	{0x1900, "OCMC_RAM3",},
+	{0x1e00, "OCMC_ROM",},
+	{0x3900, "QSPI",},
+};
+
+static struct l3_flagmux_data dra_l3_flagmux_clk2 = {
+	.offset = 0x803600,
+	.l3_targ = dra_l3_target_data_clk2,
+	.num_targ_data = ARRAY_SIZE(dra_l3_target_data_clk2),
+};
+
+static struct l3_target_data dra_l3_target_data_clk3[] = {
+	{0x0100, "L3_INSTR"},
+	{0x0300, "DEBUGSS_CT_TBR"},
+	{0x0,	 "HOST CLK3"},
+};
+
+static struct l3_flagmux_data dra_l3_flagmux_clk3 = {
+	.offset = 0x200,
+	.l3_targ = dra_l3_target_data_clk3,
+	.num_targ_data = ARRAY_SIZE(dra_l3_target_data_clk3),
+};
+
+static struct l3_masters_data dra_l3_masters[] = {
+	{ 0x0, "MPU" },
+	{ 0x4, "CS_DAP" },
+	{ 0x5, "IEEE1500_2_OCP" },
+	{ 0x8, "DSP1_MDMA" },
+	{ 0x9, "DSP1_CFG" },
+	{ 0xA, "DSP1_DMA" },
+	{ 0xB, "DSP2_MDMA" },
+	{ 0xC, "DSP2_CFG" },
+	{ 0xD, "DSP2_DMA" },
+	{ 0xE, "IVA" },
+	{ 0x10, "EVE1_P1" },
+	{ 0x11, "EVE2_P1" },
+	{ 0x12, "EVE3_P1" },
+	{ 0x13, "EVE4_P1" },
+	{ 0x14, "PRUSS1 PRU1" },
+	{ 0x15, "PRUSS1 PRU2" },
+	{ 0x16, "PRUSS2 PRU1" },
+	{ 0x17, "PRUSS2 PRU2" },
+	{ 0x18, "IPU1" },
+	{ 0x19, "IPU2" },
+	{ 0x1A, "SDMA" },
+	{ 0x1B, "CDMA" },
+	{ 0x1C, "TC1_EDMA" },
+	{ 0x1D, "TC2_EDMA" },
+	{ 0x20, "DSS" },
+	{ 0x21, "MMU1" },
+	{ 0x22, "PCIE1" },
+	{ 0x23, "MMU2" },
+	{ 0x24, "VIP1" },
+	{ 0x25, "VIP2" },
+	{ 0x26, "VIP3" },
+	{ 0x27, "VPE" },
+	{ 0x28, "GPU_P1" },
+	{ 0x29, "BB2D" },
+	{ 0x29, "GPU_P2" },
+	{ 0x2B, "GMAC_SW" },
+	{ 0x2C, "USB3" },
+	{ 0x2D, "USB2_SS" },
+	{ 0x2E, "USB2_ULPI_SS1" },
+	{ 0x2F, "USB2_ULPI_SS2" },
+	{ 0x30, "CSI2_1" },
+	{ 0x31, "CSI2_2" },
+	{ 0x33, "SATA" },
+	{ 0x34, "EVE1_P2" },
+	{ 0x35, "EVE2_P2" },
+	{ 0x36, "EVE3_P2" },
+	{ 0x37, "EVE4_P2" }
 };
-#endif
+
+static struct l3_flagmux_data *dra_l3_flagmux[] = {
+	&dra_l3_flagmux_clk1,
+	&dra_l3_flagmux_clk2,
+	&dra_l3_flagmux_clk3,
+};
+
+static const struct omap_l3 dra_l3_data = {
+	.l3_base = { [1] = L3_BASE_IS_SUBMODULE },
+	.l3_flagmux = dra_l3_flagmux,
+	.num_modules = ARRAY_SIZE(dra_l3_flagmux),
+	.l3_masters = dra_l3_masters,
+	.num_masters = ARRAY_SIZE(dra_l3_masters),
+	/* The 6 MSBs of register field used to distinguish initiator */
+	.mst_addr_mask = 0xFC,
+};
+
+/* AM4372 data */
+static struct l3_target_data am4372_l3_target_data_200f[] = {
+	{0xf00,  "EMIF",},
+	{0x1200, "DES",},
+	{0x400,  "OCMCRAM",},
+	{0x700,  "TPTC0",},
+	{0x800,  "TPTC1",},
+	{0x900,  "TPTC2"},
+	{0xb00,  "TPCC",},
+	{0xd00,  "DEBUGSS",},
+	{0xdead, L3_TARGET_NOT_SUPPORTED,},
+	{0x200,  "SHA",},
+	{0xc00,  "SGX530",},
+	{0x500,  "AES0",},
+	{0xa00,  "L4_FAST",},
+	{0x300,  "MPUSS_L2_RAM",},
+	{0x100,  "ICSS",},
+};
+
+static struct l3_flagmux_data am4372_l3_flagmux_200f = {
+	.offset = 0x1000,
+	.l3_targ = am4372_l3_target_data_200f,
+	.num_targ_data = ARRAY_SIZE(am4372_l3_target_data_200f),
+};
+
+static struct l3_target_data am4372_l3_target_data_100s[] = {
+	{0x100, "L4_PER_0",},
+	{0x200, "L4_PER_1",},
+	{0x300, "L4_PER_2",},
+	{0x400, "L4_PER_3",},
+	{0x800, "McASP0",},
+	{0x900, "McASP1",},
+	{0xC00, "MMCHS2",},
+	{0x700, "GPMC",},
+	{0xD00, "L4_FW",},
+	{0xdead, L3_TARGET_NOT_SUPPORTED,},
+	{0x500, "ADCTSC",},
+	{0xE00, "L4_WKUP",},
+	{0xA00, "MAG_CARD",},
+};
+
+static struct l3_flagmux_data am4372_l3_flagmux_100s = {
+	.offset = 0x600,
+	.l3_targ = am4372_l3_target_data_100s,
+	.num_targ_data = ARRAY_SIZE(am4372_l3_target_data_100s),
+};
+
+static struct l3_masters_data am4372_l3_masters[] = {
+	{ 0x0, "M1 (128-bit)"},
+	{ 0x1, "M2 (64-bit)"},
+	{ 0x4, "DAP"},
+	{ 0x5, "P1500"},
+	{ 0xC, "ICSS0"},
+	{ 0xD, "ICSS1"},
+	{ 0x14, "Wakeup Processor"},
+	{ 0x18, "TPTC0 Read"},
+	{ 0x19, "TPTC0 Write"},
+	{ 0x1A, "TPTC1 Read"},
+	{ 0x1B, "TPTC1 Write"},
+	{ 0x1C, "TPTC2 Read"},
+	{ 0x1D, "TPTC2 Write"},
+	{ 0x20, "SGX530"},
+	{ 0x21, "OCP WP Traffic Probe"},
+	{ 0x22, "OCP WP DMA Profiling"},
+	{ 0x23, "OCP WP Event Trace"},
+	{ 0x25, "DSS"},
+	{ 0x28, "Crypto DMA RD"},
+	{ 0x29, "Crypto DMA WR"},
+	{ 0x2C, "VPFE0"},
+	{ 0x2D, "VPFE1"},
+	{ 0x30, "GEMAC"},
+	{ 0x34, "USB0 RD"},
+	{ 0x35, "USB0 WR"},
+	{ 0x36, "USB1 RD"},
+	{ 0x37, "USB1 WR"},
+};
+
+static struct l3_flagmux_data *am4372_l3_flagmux[] = {
+	&am4372_l3_flagmux_200f,
+	&am4372_l3_flagmux_100s,
+};
+
+static const struct omap_l3 am4372_l3_data = {
+	.l3_flagmux = am4372_l3_flagmux,
+	.num_modules = ARRAY_SIZE(am4372_l3_flagmux),
+	.l3_masters = am4372_l3_masters,
+	.num_masters = ARRAY_SIZE(am4372_l3_masters),
+	/* All 6 bits of register field used to distinguish initiator */
+	.mst_addr_mask = 0x3F,
+};
+
+#endif	/* __OMAP_L3_NOC_H */
diff --git a/drivers/bus/vexpress-config.c b/drivers/bus/vexpress-config.c
new file mode 100644
index 00000000000..a64763b6b5f
--- /dev/null
+++ b/drivers/bus/vexpress-config.c
@@ -0,0 +1,202 @@
+/*
+ * 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) 2014 ARM Limited
+ */
+
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/vexpress.h>
+
+
+struct vexpress_config_bridge {
+	struct vexpress_config_bridge_ops *ops;
+	void *context;
+};
+
+
+static DEFINE_MUTEX(vexpress_config_mutex);
+static struct class *vexpress_config_class;
+static u32 vexpress_config_site_master = VEXPRESS_SITE_MASTER;
+
+
+void vexpress_config_set_master(u32 site)
+{
+	vexpress_config_site_master = site;
+}
+
+u32 vexpress_config_get_master(void)
+{
+	return vexpress_config_site_master;
+}
+
+void vexpress_config_lock(void *arg)
+{
+	mutex_lock(&vexpress_config_mutex);
+}
+
+void vexpress_config_unlock(void *arg)
+{
+	mutex_unlock(&vexpress_config_mutex);
+}
+
+
+static void vexpress_config_find_prop(struct device_node *node,
+		const char *name, u32 *val)
+{
+	/* Default value */
+	*val = 0;
+
+	of_node_get(node);
+	while (node) {
+		if (of_property_read_u32(node, name, val) == 0) {
+			of_node_put(node);
+			return;
+		}
+		node = of_get_next_parent(node);
+	}
+}
+
+int vexpress_config_get_topo(struct device_node *node, u32 *site,
+		u32 *position, u32 *dcc)
+{
+	vexpress_config_find_prop(node, "arm,vexpress,site", site);
+	if (*site == VEXPRESS_SITE_MASTER)
+		*site = vexpress_config_site_master;
+	if (WARN_ON(vexpress_config_site_master == VEXPRESS_SITE_MASTER))
+		return -EINVAL;
+	vexpress_config_find_prop(node, "arm,vexpress,position", position);
+	vexpress_config_find_prop(node, "arm,vexpress,dcc", dcc);
+
+	return 0;
+}
+
+
+static void vexpress_config_devres_release(struct device *dev, void *res)
+{
+	struct vexpress_config_bridge *bridge = dev_get_drvdata(dev->parent);
+	struct regmap *regmap = res;
+
+	bridge->ops->regmap_exit(regmap, bridge->context);
+}
+
+struct regmap *devm_regmap_init_vexpress_config(struct device *dev)
+{
+	struct vexpress_config_bridge *bridge;
+	struct regmap *regmap;
+	struct regmap **res;
+
+	if (WARN_ON(dev->parent->class != vexpress_config_class))
+		return ERR_PTR(-ENODEV);
+
+	bridge = dev_get_drvdata(dev->parent);
+	if (WARN_ON(!bridge))
+		return ERR_PTR(-EINVAL);
+
+	res = devres_alloc(vexpress_config_devres_release, sizeof(*res),
+			GFP_KERNEL);
+	if (!res)
+		return ERR_PTR(-ENOMEM);
+
+	regmap = bridge->ops->regmap_init(dev, bridge->context);
+	if (IS_ERR(regmap)) {
+		devres_free(res);
+		return regmap;
+	}
+
+	*res = regmap;
+	devres_add(dev, res);
+
+	return regmap;
+}
+EXPORT_SYMBOL_GPL(devm_regmap_init_vexpress_config);
+
+struct device *vexpress_config_bridge_register(struct device *parent,
+		struct vexpress_config_bridge_ops *ops, void *context)
+{
+	struct device *dev;
+	struct vexpress_config_bridge *bridge;
+
+	if (!vexpress_config_class) {
+		vexpress_config_class = class_create(THIS_MODULE,
+				"vexpress-config");
+		if (IS_ERR(vexpress_config_class))
+			return (void *)vexpress_config_class;
+	}
+
+	dev = device_create(vexpress_config_class, parent, 0,
+			NULL, "%s.bridge", dev_name(parent));
+
+	if (IS_ERR(dev))
+		return dev;
+
+	bridge = devm_kmalloc(dev, sizeof(*bridge), GFP_KERNEL);
+	if (!bridge) {
+		put_device(dev);
+		device_unregister(dev);
+		return ERR_PTR(-ENOMEM);
+	}
+	bridge->ops = ops;
+	bridge->context = context;
+
+	dev_set_drvdata(dev, bridge);
+
+	dev_dbg(parent, "Registered bridge '%s', parent node %p\n",
+			dev_name(dev), parent->of_node);
+
+	return dev;
+}
+
+
+static int vexpress_config_node_match(struct device *dev, const void *data)
+{
+	const struct device_node *node = data;
+
+	dev_dbg(dev, "Parent node %p, looking for %p\n",
+			dev->parent->of_node, node);
+
+	return dev->parent->of_node == node;
+}
+
+static int vexpress_config_populate(struct device_node *node)
+{
+	struct device_node *bridge;
+	struct device *parent;
+
+	bridge = of_parse_phandle(node, "arm,vexpress,config-bridge", 0);
+	if (!bridge)
+		return -EINVAL;
+
+	parent = class_find_device(vexpress_config_class, NULL, bridge,
+			vexpress_config_node_match);
+	if (WARN_ON(!parent))
+		return -ENODEV;
+
+	return of_platform_populate(node, NULL, NULL, parent);
+}
+
+static int __init vexpress_config_init(void)
+{
+	int err = 0;
+	struct device_node *node;
+
+	/* Need the config devices early, before the "normal" devices... */
+	for_each_compatible_node(node, NULL, "arm,vexpress,config-bus") {
+		err = vexpress_config_populate(node);
+		if (err)
+			break;
+	}
+
+	return err;
+}
+postcore_initcall(vexpress_config_init);
+
diff --git a/drivers/cdrom/cdrom.c b/drivers/cdrom/cdrom.c
index 8a3aff724d9..49ac5662585 100644
--- a/drivers/cdrom/cdrom.c
+++ b/drivers/cdrom/cdrom.c
@@ -312,36 +312,24 @@ static const char *mrw_format_status[] = {
 
 static const char *mrw_address_space[] = { "DMA", "GAA" };
 
-#if (ERRLOGMASK!=CD_NOTHING)
-#define cdinfo(type, fmt, args...)			\
+#if (ERRLOGMASK != CD_NOTHING)
+#define cd_dbg(type, fmt, ...)				\
 do {							\
 	if ((ERRLOGMASK & type) || debug == 1)		\
-		pr_info(fmt, ##args);			\
+		pr_debug(fmt, ##__VA_ARGS__);		\
 } while (0)
 #else
-#define cdinfo(type, fmt, args...)			\
+#define cd_dbg(type, fmt, ...)				\
 do {							\
 	if (0 && (ERRLOGMASK & type) || debug == 1)	\
-		pr_info(fmt, ##args);			\
+		pr_debug(fmt, ##__VA_ARGS__);		\
 } while (0)
 #endif
 
-/* These are used to simplify getting data in from and back to user land */
-#define IOCTL_IN(arg, type, in)					\
-	if (copy_from_user(&(in), (type __user *) (arg), sizeof (in)))	\
-		return -EFAULT;
-
-#define IOCTL_OUT(arg, type, out) \
-	if (copy_to_user((type __user *) (arg), &(out), sizeof (out)))	\
-		return -EFAULT;
-
 /* The (cdo->capability & ~cdi->mask & CDC_XXX) construct was used in
    a lot of places. This macro makes the code more clear. */
 #define CDROM_CAN(type) (cdi->ops->capability & ~cdi->mask & (type))
 
-/* used in the audio ioctls */
-#define CHECKAUDIO if ((ret=check_for_audio_disc(cdi, cdo))) return ret
-
 /*
  * Another popular OS uses 7 seconds as the hard timeout for default
  * commands, so it is a good choice for us as well.
@@ -349,21 +337,6 @@ do {							\
 #define CDROM_DEF_TIMEOUT	(7 * HZ)
 
 /* Not-exported routines. */
-static int open_for_data(struct cdrom_device_info * cdi);
-static int check_for_audio_disc(struct cdrom_device_info * cdi,
-			 struct cdrom_device_ops * cdo);
-static void sanitize_format(union cdrom_addr *addr, 
-		u_char * curr, u_char requested);
-static int mmc_ioctl(struct cdrom_device_info *cdi, unsigned int cmd,
-		     unsigned long arg);
-
-int cdrom_get_last_written(struct cdrom_device_info *, long *);
-static int cdrom_get_next_writable(struct cdrom_device_info *, long *);
-static void cdrom_count_tracks(struct cdrom_device_info *, tracktype*);
-
-static int cdrom_mrw_exit(struct cdrom_device_info *cdi);
-
-static int cdrom_get_disc_info(struct cdrom_device_info *cdi, disc_information *di);
 
 static void cdrom_sysctl_register(void);
 
@@ -382,113 +355,65 @@ static int cdrom_dummy_generic_packet(struct cdrom_device_info *cdi,
 	return -EIO;
 }
 
-/* This macro makes sure we don't have to check on cdrom_device_ops
- * existence in the run-time routines below. Change_capability is a
- * hack to have the capability flags defined const, while we can still
- * change it here without gcc complaining at every line.
- */
-#define ENSURE(call, bits) if (cdo->call == NULL) *change_capability &= ~(bits)
-
-int register_cdrom(struct cdrom_device_info *cdi)
-{
-	static char banner_printed;
-        struct cdrom_device_ops *cdo = cdi->ops;
-        int *change_capability = (int *)&cdo->capability; /* hack */
-
-	cdinfo(CD_OPEN, "entering register_cdrom\n"); 
-
-	if (cdo->open == NULL || cdo->release == NULL)
-		return -EINVAL;
-	if (!banner_printed) {
-		pr_info("Uniform CD-ROM driver " REVISION "\n");
-		banner_printed = 1;
-		cdrom_sysctl_register();
-	}
-
-	ENSURE(drive_status, CDC_DRIVE_STATUS );
-	if (cdo->check_events == NULL && cdo->media_changed == NULL)
-		*change_capability = ~(CDC_MEDIA_CHANGED | CDC_SELECT_DISC);
-	ENSURE(tray_move, CDC_CLOSE_TRAY | CDC_OPEN_TRAY);
-	ENSURE(lock_door, CDC_LOCK);
-	ENSURE(select_speed, CDC_SELECT_SPEED);
-	ENSURE(get_last_session, CDC_MULTI_SESSION);
-	ENSURE(get_mcn, CDC_MCN);
-	ENSURE(reset, CDC_RESET);
-	ENSURE(generic_packet, CDC_GENERIC_PACKET);
-	cdi->mc_flags = 0;
-	cdo->n_minors = 0;
-        cdi->options = CDO_USE_FFLAGS;
-	
-	if (autoclose==1 && CDROM_CAN(CDC_CLOSE_TRAY))
-		cdi->options |= (int) CDO_AUTO_CLOSE;
-	if (autoeject==1 && CDROM_CAN(CDC_OPEN_TRAY))
-		cdi->options |= (int) CDO_AUTO_EJECT;
-	if (lockdoor==1)
-		cdi->options |= (int) CDO_LOCK;
-	if (check_media_type==1)
-		cdi->options |= (int) CDO_CHECK_TYPE;
-
-	if (CDROM_CAN(CDC_MRW_W))
-		cdi->exit = cdrom_mrw_exit;
-
-	if (cdi->disk)
-		cdi->cdda_method = CDDA_BPC_FULL;
-	else
-		cdi->cdda_method = CDDA_OLD;
-
-	if (!cdo->generic_packet)
-		cdo->generic_packet = cdrom_dummy_generic_packet;
-
-	cdinfo(CD_REG_UNREG, "drive \"/dev/%s\" registered\n", cdi->name);
-	mutex_lock(&cdrom_mutex);
-	list_add(&cdi->list, &cdrom_list);
-	mutex_unlock(&cdrom_mutex);
-	return 0;
-}
-#undef ENSURE
-
-void unregister_cdrom(struct cdrom_device_info *cdi)
+static int cdrom_flush_cache(struct cdrom_device_info *cdi)
 {
-	cdinfo(CD_OPEN, "entering unregister_cdrom\n"); 
+	struct packet_command cgc;
 
-	mutex_lock(&cdrom_mutex);
-	list_del(&cdi->list);
-	mutex_unlock(&cdrom_mutex);
+	init_cdrom_command(&cgc, NULL, 0, CGC_DATA_NONE);
+	cgc.cmd[0] = GPCMD_FLUSH_CACHE;
 
-	if (cdi->exit)
-		cdi->exit(cdi);
+	cgc.timeout = 5 * 60 * HZ;
 
-	cdi->ops->n_minors--;
-	cdinfo(CD_REG_UNREG, "drive \"/dev/%s\" unregistered\n", cdi->name);
+	return cdi->ops->generic_packet(cdi, &cgc);
 }
 
-int cdrom_get_media_event(struct cdrom_device_info *cdi,
-			  struct media_event_desc *med)
+/* requires CD R/RW */
+static int cdrom_get_disc_info(struct cdrom_device_info *cdi,
+			       disc_information *di)
 {
+	struct cdrom_device_ops *cdo = cdi->ops;
 	struct packet_command cgc;
-	unsigned char buffer[8];
-	struct event_header *eh = (struct event_header *) buffer;
+	int ret, buflen;
 
-	init_cdrom_command(&cgc, buffer, sizeof(buffer), CGC_DATA_READ);
-	cgc.cmd[0] = GPCMD_GET_EVENT_STATUS_NOTIFICATION;
-	cgc.cmd[1] = 1;		/* IMMED */
-	cgc.cmd[4] = 1 << 4;	/* media event */
-	cgc.cmd[8] = sizeof(buffer);
+	/* set up command and get the disc info */
+	init_cdrom_command(&cgc, di, sizeof(*di), CGC_DATA_READ);
+	cgc.cmd[0] = GPCMD_READ_DISC_INFO;
+	cgc.cmd[8] = cgc.buflen = 2;
 	cgc.quiet = 1;
 
-	if (cdi->ops->generic_packet(cdi, &cgc))
-		return 1;
+	ret = cdo->generic_packet(cdi, &cgc);
+	if (ret)
+		return ret;
 
-	if (be16_to_cpu(eh->data_len) < sizeof(*med))
-		return 1;
+	/* not all drives have the same disc_info length, so requeue
+	 * packet with the length the drive tells us it can supply
+	 */
+	buflen = be16_to_cpu(di->disc_information_length) +
+		sizeof(di->disc_information_length);
 
-	if (eh->nea || eh->notification_class != 0x4)
-		return 1;
+	if (buflen > sizeof(disc_information))
+		buflen = sizeof(disc_information);
 
-	memcpy(med, &buffer[sizeof(*eh)], sizeof(*med));
-	return 0;
+	cgc.cmd[8] = cgc.buflen = buflen;
+	ret = cdo->generic_packet(cdi, &cgc);
+	if (ret)
+		return ret;
+
+	/* return actual fill size */
+	return buflen;
 }
 
+/* This macro makes sure we don't have to check on cdrom_device_ops
+ * existence in the run-time routines below. Change_capability is a
+ * hack to have the capability flags defined const, while we can still
+ * change it here without gcc complaining at every line.
+ */
+#define ENSURE(call, bits)			\
+do {						\
+	if (cdo->call == NULL)			\
+		*change_capability &= ~(bits);	\
+} while (0)
+
 /*
  * the first prototypes used 0x2c as the page code for the mrw mode page,
  * subsequently this was changed to 0x03. probe the one used by this drive
@@ -605,18 +530,6 @@ static int cdrom_mrw_bgformat_susp(struct cdrom_device_info *cdi, int immed)
 	return cdi->ops->generic_packet(cdi, &cgc);
 }
 
-static int cdrom_flush_cache(struct cdrom_device_info *cdi)
-{
-	struct packet_command cgc;
-
-	init_cdrom_command(&cgc, NULL, 0, CGC_DATA_NONE);
-	cgc.cmd[0] = GPCMD_FLUSH_CACHE;
-
-	cgc.timeout = 5 * 60 * HZ;
-
-	return cdi->ops->generic_packet(cdi, &cgc);
-}
-
 static int cdrom_mrw_exit(struct cdrom_device_info *cdi)
 {
 	disc_information di;
@@ -650,17 +563,19 @@ static int cdrom_mrw_set_lba_space(struct cdrom_device_info *cdi, int space)
 	cgc.buffer = buffer;
 	cgc.buflen = sizeof(buffer);
 
-	if ((ret = cdrom_mode_sense(cdi, &cgc, cdi->mrw_mode_page, 0)))
+	ret = cdrom_mode_sense(cdi, &cgc, cdi->mrw_mode_page, 0);
+	if (ret)
 		return ret;
 
-	mph = (struct mode_page_header *) buffer;
+	mph = (struct mode_page_header *)buffer;
 	offset = be16_to_cpu(mph->desc_length);
 	size = be16_to_cpu(mph->mode_data_length) + 2;
 
 	buffer[offset + 3] = space;
 	cgc.buflen = size;
 
-	if ((ret = cdrom_mode_select(cdi, &cgc)))
+	ret = cdrom_mode_select(cdi, &cgc);
+	if (ret)
 		return ret;
 
 	pr_info("%s: mrw address space %s selected\n",
@@ -668,6 +583,106 @@ static int cdrom_mrw_set_lba_space(struct cdrom_device_info *cdi, int space)
 	return 0;
 }
 
+int register_cdrom(struct cdrom_device_info *cdi)
+{
+	static char banner_printed;
+	struct cdrom_device_ops *cdo = cdi->ops;
+	int *change_capability = (int *)&cdo->capability; /* hack */
+
+	cd_dbg(CD_OPEN, "entering register_cdrom\n");
+
+	if (cdo->open == NULL || cdo->release == NULL)
+		return -EINVAL;
+	if (!banner_printed) {
+		pr_info("Uniform CD-ROM driver " REVISION "\n");
+		banner_printed = 1;
+		cdrom_sysctl_register();
+	}
+
+	ENSURE(drive_status, CDC_DRIVE_STATUS);
+	if (cdo->check_events == NULL && cdo->media_changed == NULL)
+		*change_capability = ~(CDC_MEDIA_CHANGED | CDC_SELECT_DISC);
+	ENSURE(tray_move, CDC_CLOSE_TRAY | CDC_OPEN_TRAY);
+	ENSURE(lock_door, CDC_LOCK);
+	ENSURE(select_speed, CDC_SELECT_SPEED);
+	ENSURE(get_last_session, CDC_MULTI_SESSION);
+	ENSURE(get_mcn, CDC_MCN);
+	ENSURE(reset, CDC_RESET);
+	ENSURE(generic_packet, CDC_GENERIC_PACKET);
+	cdi->mc_flags = 0;
+	cdo->n_minors = 0;
+	cdi->options = CDO_USE_FFLAGS;
+
+	if (autoclose == 1 && CDROM_CAN(CDC_CLOSE_TRAY))
+		cdi->options |= (int) CDO_AUTO_CLOSE;
+	if (autoeject == 1 && CDROM_CAN(CDC_OPEN_TRAY))
+		cdi->options |= (int) CDO_AUTO_EJECT;
+	if (lockdoor == 1)
+		cdi->options |= (int) CDO_LOCK;
+	if (check_media_type == 1)
+		cdi->options |= (int) CDO_CHECK_TYPE;
+
+	if (CDROM_CAN(CDC_MRW_W))
+		cdi->exit = cdrom_mrw_exit;
+
+	if (cdi->disk)
+		cdi->cdda_method = CDDA_BPC_FULL;
+	else
+		cdi->cdda_method = CDDA_OLD;
+
+	if (!cdo->generic_packet)
+		cdo->generic_packet = cdrom_dummy_generic_packet;
+
+	cd_dbg(CD_REG_UNREG, "drive \"/dev/%s\" registered\n", cdi->name);
+	mutex_lock(&cdrom_mutex);
+	list_add(&cdi->list, &cdrom_list);
+	mutex_unlock(&cdrom_mutex);
+	return 0;
+}
+#undef ENSURE
+
+void unregister_cdrom(struct cdrom_device_info *cdi)
+{
+	cd_dbg(CD_OPEN, "entering unregister_cdrom\n");
+
+	mutex_lock(&cdrom_mutex);
+	list_del(&cdi->list);
+	mutex_unlock(&cdrom_mutex);
+
+	if (cdi->exit)
+		cdi->exit(cdi);
+
+	cdi->ops->n_minors--;
+	cd_dbg(CD_REG_UNREG, "drive \"/dev/%s\" unregistered\n", cdi->name);
+}
+
+int cdrom_get_media_event(struct cdrom_device_info *cdi,
+			  struct media_event_desc *med)
+{
+	struct packet_command cgc;
+	unsigned char buffer[8];
+	struct event_header *eh = (struct event_header *)buffer;
+
+	init_cdrom_command(&cgc, buffer, sizeof(buffer), CGC_DATA_READ);
+	cgc.cmd[0] = GPCMD_GET_EVENT_STATUS_NOTIFICATION;
+	cgc.cmd[1] = 1;		/* IMMED */
+	cgc.cmd[4] = 1 << 4;	/* media event */
+	cgc.cmd[8] = sizeof(buffer);
+	cgc.quiet = 1;
+
+	if (cdi->ops->generic_packet(cdi, &cgc))
+		return 1;
+
+	if (be16_to_cpu(eh->data_len) < sizeof(*med))
+		return 1;
+
+	if (eh->nea || eh->notification_class != 0x4)
+		return 1;
+
+	memcpy(med, &buffer[sizeof(*eh)], sizeof(*med));
+	return 0;
+}
+
 static int cdrom_get_random_writable(struct cdrom_device_info *cdi,
 			      struct rwrt_feature_desc *rfd)
 {
@@ -839,7 +854,7 @@ static int cdrom_ram_open_write(struct cdrom_device_info *cdi)
 	else if (CDF_RWRT == be16_to_cpu(rfd.feature_code))
 		ret = !rfd.curr;
 
-	cdinfo(CD_OPEN, "can open for random write\n");
+	cd_dbg(CD_OPEN, "can open for random write\n");
 	return ret;
 }
 
@@ -928,12 +943,12 @@ static void cdrom_dvd_rw_close_write(struct cdrom_device_info *cdi)
 	struct packet_command cgc;
 
 	if (cdi->mmc3_profile != 0x1a) {
-		cdinfo(CD_CLOSE, "%s: No DVD+RW\n", cdi->name);
+		cd_dbg(CD_CLOSE, "%s: No DVD+RW\n", cdi->name);
 		return;
 	}
 
 	if (!cdi->media_written) {
-		cdinfo(CD_CLOSE, "%s: DVD+RW media clean\n", cdi->name);
+		cd_dbg(CD_CLOSE, "%s: DVD+RW media clean\n", cdi->name);
 		return;
 	}
 
@@ -969,82 +984,74 @@ static int cdrom_close_write(struct cdrom_device_info *cdi)
 #endif
 }
 
-/* We use the open-option O_NONBLOCK to indicate that the
- * purpose of opening is only for subsequent ioctl() calls; no device
- * integrity checks are performed.
- *
- * We hope that all cd-player programs will adopt this convention. It
- * is in their own interest: device control becomes a lot easier
- * this way.
- */
-int cdrom_open(struct cdrom_device_info *cdi, struct block_device *bdev, fmode_t mode)
+/* badly broken, I know. Is due for a fixup anytime. */
+static void cdrom_count_tracks(struct cdrom_device_info *cdi, tracktype *tracks)
 {
-	int ret;
-
-	cdinfo(CD_OPEN, "entering cdrom_open\n"); 
-
-	/* open is event synchronization point, check events first */
-	check_disk_change(bdev);
-
-	/* if this was a O_NONBLOCK open and we should honor the flags,
-	 * do a quick open without drive/disc integrity checks. */
-	cdi->use_count++;
-	if ((mode & FMODE_NDELAY) && (cdi->options & CDO_USE_FFLAGS)) {
-		ret = cdi->ops->open(cdi, 1);
-	} else {
-		ret = open_for_data(cdi);
-		if (ret)
-			goto err;
-		cdrom_mmc3_profile(cdi);
-		if (mode & FMODE_WRITE) {
-			ret = -EROFS;
-			if (cdrom_open_write(cdi))
-				goto err_release;
-			if (!CDROM_CAN(CDC_RAM))
-				goto err_release;
-			ret = 0;
-			cdi->media_written = 0;
-		}
+	struct cdrom_tochdr header;
+	struct cdrom_tocentry entry;
+	int ret, i;
+	tracks->data = 0;
+	tracks->audio = 0;
+	tracks->cdi = 0;
+	tracks->xa = 0;
+	tracks->error = 0;
+	cd_dbg(CD_COUNT_TRACKS, "entering cdrom_count_tracks\n");
+	/* Grab the TOC header so we can see how many tracks there are */
+	ret = cdi->ops->audio_ioctl(cdi, CDROMREADTOCHDR, &header);
+	if (ret) {
+		if (ret == -ENOMEDIUM)
+			tracks->error = CDS_NO_DISC;
+		else
+			tracks->error = CDS_NO_INFO;
+		return;
 	}
-
-	if (ret)
-		goto err;
-
-	cdinfo(CD_OPEN, "Use count for \"/dev/%s\" now %d\n",
-			cdi->name, cdi->use_count);
-	return 0;
-err_release:
-	if (CDROM_CAN(CDC_LOCK) && cdi->options & CDO_LOCK) {
-		cdi->ops->lock_door(cdi, 0);
-		cdinfo(CD_OPEN, "door unlocked.\n");
+	/* check what type of tracks are on this disc */
+	entry.cdte_format = CDROM_MSF;
+	for (i = header.cdth_trk0; i <= header.cdth_trk1; i++) {
+		entry.cdte_track = i;
+		if (cdi->ops->audio_ioctl(cdi, CDROMREADTOCENTRY, &entry)) {
+			tracks->error = CDS_NO_INFO;
+			return;
+		}
+		if (entry.cdte_ctrl & CDROM_DATA_TRACK) {
+			if (entry.cdte_format == 0x10)
+				tracks->cdi++;
+			else if (entry.cdte_format == 0x20)
+				tracks->xa++;
+			else
+				tracks->data++;
+		} else {
+			tracks->audio++;
+		}
+		cd_dbg(CD_COUNT_TRACKS, "track %d: format=%d, ctrl=%d\n",
+		       i, entry.cdte_format, entry.cdte_ctrl);
 	}
-	cdi->ops->release(cdi);
-err:
-	cdi->use_count--;
-	return ret;
+	cd_dbg(CD_COUNT_TRACKS, "disc has %d tracks: %d=audio %d=data %d=Cd-I %d=XA\n",
+	       header.cdth_trk1, tracks->audio, tracks->data,
+	       tracks->cdi, tracks->xa);
 }
 
 static
-int open_for_data(struct cdrom_device_info * cdi)
+int open_for_data(struct cdrom_device_info *cdi)
 {
 	int ret;
 	struct cdrom_device_ops *cdo = cdi->ops;
 	tracktype tracks;
-	cdinfo(CD_OPEN, "entering open_for_data\n");
+	cd_dbg(CD_OPEN, "entering open_for_data\n");
 	/* Check if the driver can report drive status.  If it can, we
 	   can do clever things.  If it can't, well, we at least tried! */
 	if (cdo->drive_status != NULL) {
 		ret = cdo->drive_status(cdi, CDSL_CURRENT);
-		cdinfo(CD_OPEN, "drive_status=%d\n", ret); 
+		cd_dbg(CD_OPEN, "drive_status=%d\n", ret);
 		if (ret == CDS_TRAY_OPEN) {
-			cdinfo(CD_OPEN, "the tray is open...\n"); 
+			cd_dbg(CD_OPEN, "the tray is open...\n");
 			/* can/may i close it? */
 			if (CDROM_CAN(CDC_CLOSE_TRAY) &&
 			    cdi->options & CDO_AUTO_CLOSE) {
-				cdinfo(CD_OPEN, "trying to close the tray.\n"); 
+				cd_dbg(CD_OPEN, "trying to close the tray\n");
 				ret=cdo->tray_move(cdi,0);
 				if (ret) {
-					cdinfo(CD_OPEN, "bummer. tried to close the tray but failed.\n"); 
+					cd_dbg(CD_OPEN, "bummer. tried to close the tray but failed.\n");
 					/* Ignore the error from the low
 					level driver.  We don't care why it
 					couldn't close the tray.  We only care 
@@ -1054,19 +1061,19 @@ int open_for_data(struct cdrom_device_info * cdi)
 					goto clean_up_and_return;
 				}
 			} else {
-				cdinfo(CD_OPEN, "bummer. this drive can't close the tray.\n"); 
+				cd_dbg(CD_OPEN, "bummer. this drive can't close the tray.\n");
 				ret=-ENOMEDIUM;
 				goto clean_up_and_return;
 			}
 			/* Ok, the door should be closed now.. Check again */
 			ret = cdo->drive_status(cdi, CDSL_CURRENT);
 			if ((ret == CDS_NO_DISC) || (ret==CDS_TRAY_OPEN)) {
-				cdinfo(CD_OPEN, "bummer. the tray is still not closed.\n"); 
-				cdinfo(CD_OPEN, "tray might not contain a medium.\n");
+				cd_dbg(CD_OPEN, "bummer. the tray is still not closed.\n");
+				cd_dbg(CD_OPEN, "tray might not contain a medium\n");
 				ret=-ENOMEDIUM;
 				goto clean_up_and_return;
 			}
-			cdinfo(CD_OPEN, "the tray is now closed.\n"); 
+			cd_dbg(CD_OPEN, "the tray is now closed\n");
 		}
 		/* the door should be closed now, check for the disc */
 		ret = cdo->drive_status(cdi, CDSL_CURRENT);
@@ -1077,7 +1084,7 @@ int open_for_data(struct cdrom_device_info * cdi)
 	}
 	cdrom_count_tracks(cdi, &tracks);
 	if (tracks.error == CDS_NO_DISC) {
-		cdinfo(CD_OPEN, "bummer. no disc.\n");
+		cd_dbg(CD_OPEN, "bummer. no disc.\n");
 		ret=-ENOMEDIUM;
 		goto clean_up_and_return;
 	}
@@ -1087,34 +1094,34 @@ int open_for_data(struct cdrom_device_info * cdi)
 		if (cdi->options & CDO_CHECK_TYPE) {
 		    /* give people a warning shot, now that CDO_CHECK_TYPE
 		       is the default case! */
-		    cdinfo(CD_OPEN, "bummer. wrong media type.\n"); 
-		    cdinfo(CD_WARNING, "pid %d must open device O_NONBLOCK!\n",
-					(unsigned int)task_pid_nr(current));
+		    cd_dbg(CD_OPEN, "bummer. wrong media type.\n");
+		    cd_dbg(CD_WARNING, "pid %d must open device O_NONBLOCK!\n",
+			   (unsigned int)task_pid_nr(current));
 		    ret=-EMEDIUMTYPE;
 		    goto clean_up_and_return;
 		}
 		else {
-		    cdinfo(CD_OPEN, "wrong media type, but CDO_CHECK_TYPE not set.\n");
+		    cd_dbg(CD_OPEN, "wrong media type, but CDO_CHECK_TYPE not set\n");
 		}
 	}
 
-	cdinfo(CD_OPEN, "all seems well, opening the device.\n"); 
+	cd_dbg(CD_OPEN, "all seems well, opening the devicen");
 
 	/* all seems well, we can open the device */
 	ret = cdo->open(cdi, 0); /* open for data */
-	cdinfo(CD_OPEN, "opening the device gave me %d.\n", ret); 
+	cd_dbg(CD_OPEN, "opening the device gave me %d\n", ret);
 	/* After all this careful checking, we shouldn't have problems
 	   opening the device, but we don't want the device locked if 
 	   this somehow fails... */
 	if (ret) {
-		cdinfo(CD_OPEN, "open device failed.\n"); 
+		cd_dbg(CD_OPEN, "open device failed\n");
 		goto clean_up_and_return;
 	}
 	if (CDROM_CAN(CDC_LOCK) && (cdi->options & CDO_LOCK)) {
 			cdo->lock_door(cdi, 1);
-			cdinfo(CD_OPEN, "door locked.\n");
+			cd_dbg(CD_OPEN, "door locked\n");
 	}
-	cdinfo(CD_OPEN, "device opened successfully.\n"); 
+	cd_dbg(CD_OPEN, "device opened successfully\n");
 	return ret;
 
 	/* Something failed.  Try to unlock the drive, because some drivers
@@ -1123,14 +1130,70 @@ int open_for_data(struct cdrom_device_info * cdi)
 	This ensures that the drive gets unlocked after a mount fails.  This 
 	is a goto to avoid bloating the driver with redundant code. */ 
 clean_up_and_return:
-	cdinfo(CD_OPEN, "open failed.\n"); 
+	cd_dbg(CD_OPEN, "open failed\n");
 	if (CDROM_CAN(CDC_LOCK) && cdi->options & CDO_LOCK) {
 			cdo->lock_door(cdi, 0);
-			cdinfo(CD_OPEN, "door unlocked.\n");
+			cd_dbg(CD_OPEN, "door unlocked\n");
 	}
 	return ret;
 }
 
+/* We use the open-option O_NONBLOCK to indicate that the
+ * purpose of opening is only for subsequent ioctl() calls; no device
+ * integrity checks are performed.
+ *
+ * We hope that all cd-player programs will adopt this convention. It
+ * is in their own interest: device control becomes a lot easier
+ * this way.
+ */
+int cdrom_open(struct cdrom_device_info *cdi, struct block_device *bdev,
+	       fmode_t mode)
+{
+	int ret;
+
+	cd_dbg(CD_OPEN, "entering cdrom_open\n");
+
+	/* open is event synchronization point, check events first */
+	check_disk_change(bdev);
+
+	/* if this was a O_NONBLOCK open and we should honor the flags,
+	 * do a quick open without drive/disc integrity checks. */
+	cdi->use_count++;
+	if ((mode & FMODE_NDELAY) && (cdi->options & CDO_USE_FFLAGS)) {
+		ret = cdi->ops->open(cdi, 1);
+	} else {
+		ret = open_for_data(cdi);
+		if (ret)
+			goto err;
+		cdrom_mmc3_profile(cdi);
+		if (mode & FMODE_WRITE) {
+			ret = -EROFS;
+			if (cdrom_open_write(cdi))
+				goto err_release;
+			if (!CDROM_CAN(CDC_RAM))
+				goto err_release;
+			ret = 0;
+			cdi->media_written = 0;
+		}
+	}
+
+	if (ret)
+		goto err;
+
+	cd_dbg(CD_OPEN, "Use count for \"/dev/%s\" now %d\n",
+	       cdi->name, cdi->use_count);
+	return 0;
+err_release:
+	if (CDROM_CAN(CDC_LOCK) && cdi->options & CDO_LOCK) {
+		cdi->ops->lock_door(cdi, 0);
+		cd_dbg(CD_OPEN, "door unlocked\n");
+	}
+	cdi->ops->release(cdi);
+err:
+	cdi->use_count--;
+	return ret;
+}
+
 /* This code is similar to that in open_for_data. The routine is called
    whenever an audio play operation is requested.
 */
@@ -1139,21 +1202,21 @@ static int check_for_audio_disc(struct cdrom_device_info * cdi,
 {
         int ret;
 	tracktype tracks;
-	cdinfo(CD_OPEN, "entering check_for_audio_disc\n");
+	cd_dbg(CD_OPEN, "entering check_for_audio_disc\n");
 	if (!(cdi->options & CDO_CHECK_TYPE))
 		return 0;
 	if (cdo->drive_status != NULL) {
 		ret = cdo->drive_status(cdi, CDSL_CURRENT);
-		cdinfo(CD_OPEN, "drive_status=%d\n", ret); 
+		cd_dbg(CD_OPEN, "drive_status=%d\n", ret);
 		if (ret == CDS_TRAY_OPEN) {
-			cdinfo(CD_OPEN, "the tray is open...\n"); 
+			cd_dbg(CD_OPEN, "the tray is open...\n");
 			/* can/may i close it? */
 			if (CDROM_CAN(CDC_CLOSE_TRAY) &&
 			    cdi->options & CDO_AUTO_CLOSE) {
-				cdinfo(CD_OPEN, "trying to close the tray.\n"); 
+				cd_dbg(CD_OPEN, "trying to close the tray\n");
 				ret=cdo->tray_move(cdi,0);
 				if (ret) {
-					cdinfo(CD_OPEN, "bummer. tried to close tray but failed.\n"); 
+					cd_dbg(CD_OPEN, "bummer. tried to close tray but failed.\n");
 					/* Ignore the error from the low
 					level driver.  We don't care why it
 					couldn't close the tray.  We only care 
@@ -1162,20 +1225,20 @@ static int check_for_audio_disc(struct cdrom_device_info * cdi,
 					return -ENOMEDIUM;
 				}
 			} else {
-				cdinfo(CD_OPEN, "bummer. this driver can't close the tray.\n"); 
+				cd_dbg(CD_OPEN, "bummer. this driver can't close the tray.\n");
 				return -ENOMEDIUM;
 			}
 			/* Ok, the door should be closed now.. Check again */
 			ret = cdo->drive_status(cdi, CDSL_CURRENT);
 			if ((ret == CDS_NO_DISC) || (ret==CDS_TRAY_OPEN)) {
-				cdinfo(CD_OPEN, "bummer. the tray is still not closed.\n"); 
+				cd_dbg(CD_OPEN, "bummer. the tray is still not closed.\n");
 				return -ENOMEDIUM;
 			}	
 			if (ret!=CDS_DISC_OK) {
-				cdinfo(CD_OPEN, "bummer. disc isn't ready.\n"); 
+				cd_dbg(CD_OPEN, "bummer. disc isn't ready.\n");
 				return -EIO;
 			}	
-			cdinfo(CD_OPEN, "the tray is now closed.\n"); 
+			cd_dbg(CD_OPEN, "the tray is now closed\n");
 		}	
 	}
 	cdrom_count_tracks(cdi, &tracks);
@@ -1193,17 +1256,18 @@ void cdrom_release(struct cdrom_device_info *cdi, fmode_t mode)
 	struct cdrom_device_ops *cdo = cdi->ops;
 	int opened_for_data;
 
-	cdinfo(CD_CLOSE, "entering cdrom_release\n");
+	cd_dbg(CD_CLOSE, "entering cdrom_release\n");
 
 	if (cdi->use_count > 0)
 		cdi->use_count--;
 
 	if (cdi->use_count == 0) {
-		cdinfo(CD_CLOSE, "Use count for \"/dev/%s\" now zero\n", cdi->name);
+		cd_dbg(CD_CLOSE, "Use count for \"/dev/%s\" now zero\n",
+		       cdi->name);
 		cdrom_dvd_rw_close_write(cdi);
 
 		if ((cdo->capability & CDC_LOCK) && !cdi->keeplocked) {
-			cdinfo(CD_CLOSE, "Unlocking door!\n");
+			cd_dbg(CD_CLOSE, "Unlocking door!\n");
 			cdo->lock_door(cdi, 0);
 		}
 	}
@@ -1262,7 +1326,7 @@ static int cdrom_slot_status(struct cdrom_device_info *cdi, int slot)
 	struct cdrom_changer_info *info;
 	int ret;
 
-	cdinfo(CD_CHANGER, "entering cdrom_slot_status()\n"); 
+	cd_dbg(CD_CHANGER, "entering cdrom_slot_status()\n");
 	if (cdi->sanyo_slot)
 		return CDS_NO_INFO;
 	
@@ -1292,7 +1356,7 @@ int cdrom_number_of_slots(struct cdrom_device_info *cdi)
 	int nslots = 1;
 	struct cdrom_changer_info *info;
 
-	cdinfo(CD_CHANGER, "entering cdrom_number_of_slots()\n"); 
+	cd_dbg(CD_CHANGER, "entering cdrom_number_of_slots()\n");
 	/* cdrom_read_mech_status requires a valid value for capacity: */
 	cdi->capacity = 0; 
 
@@ -1313,7 +1377,7 @@ static int cdrom_load_unload(struct cdrom_device_info *cdi, int slot)
 {
 	struct packet_command cgc;
 
-	cdinfo(CD_CHANGER, "entering cdrom_load_unload()\n"); 
+	cd_dbg(CD_CHANGER, "entering cdrom_load_unload()\n");
 	if (cdi->sanyo_slot && slot < 0)
 		return 0;
 
@@ -1342,7 +1406,7 @@ static int cdrom_select_disc(struct cdrom_device_info *cdi, int slot)
 	int curslot;
 	int ret;
 
-	cdinfo(CD_CHANGER, "entering cdrom_select_disc()\n"); 
+	cd_dbg(CD_CHANGER, "entering cdrom_select_disc()\n");
 	if (!CDROM_CAN(CDC_SELECT_DISC))
 		return -EDRIVE_CANT_DO_THIS;
 
@@ -1476,51 +1540,6 @@ int cdrom_media_changed(struct cdrom_device_info *cdi)
 	return media_changed(cdi, 0);
 }
 
-/* badly broken, I know. Is due for a fixup anytime. */
-static void cdrom_count_tracks(struct cdrom_device_info *cdi, tracktype* tracks)
-{
-	struct cdrom_tochdr header;
-	struct cdrom_tocentry entry;
-	int ret, i;
-	tracks->data=0;
-	tracks->audio=0;
-	tracks->cdi=0;
-	tracks->xa=0;
-	tracks->error=0;
-	cdinfo(CD_COUNT_TRACKS, "entering cdrom_count_tracks\n"); 
-	/* Grab the TOC header so we can see how many tracks there are */
-	if ((ret = cdi->ops->audio_ioctl(cdi, CDROMREADTOCHDR, &header))) {
-		if (ret == -ENOMEDIUM)
-			tracks->error = CDS_NO_DISC;
-		else
-			tracks->error = CDS_NO_INFO;
-		return;
-	}	
-	/* check what type of tracks are on this disc */
-	entry.cdte_format = CDROM_MSF;
-	for (i = header.cdth_trk0; i <= header.cdth_trk1; i++) {
-		entry.cdte_track  = i;
-		if (cdi->ops->audio_ioctl(cdi, CDROMREADTOCENTRY, &entry)) {
-			tracks->error=CDS_NO_INFO;
-			return;
-		}	
-		if (entry.cdte_ctrl & CDROM_DATA_TRACK) {
-		    if (entry.cdte_format == 0x10)
-			tracks->cdi++;
-		    else if (entry.cdte_format == 0x20) 
-			tracks->xa++;
-		    else
-			tracks->data++;
-		} else
-		    tracks->audio++;
-		cdinfo(CD_COUNT_TRACKS, "track %d: format=%d, ctrl=%d\n",
-		       i, entry.cdte_format, entry.cdte_ctrl);
-	}	
-	cdinfo(CD_COUNT_TRACKS, "disc has %d tracks: %d=audio %d=data %d=Cd-I %d=XA\n", 
-		header.cdth_trk1, tracks->audio, tracks->data, 
-		tracks->cdi, tracks->xa);
-}	
-
 /* Requests to the low-level drivers will /always/ be done in the
    following format convention:
 
@@ -1632,7 +1651,7 @@ static int dvd_do_auth(struct cdrom_device_info *cdi, dvd_authinfo *ai)
 	switch (ai->type) {
 	/* LU data send */
 	case DVD_LU_SEND_AGID:
-		cdinfo(CD_DVD, "entering DVD_LU_SEND_AGID\n"); 
+		cd_dbg(CD_DVD, "entering DVD_LU_SEND_AGID\n");
 		cgc.quiet = 1;
 		setup_report_key(&cgc, ai->lsa.agid, 0);
 
@@ -1644,7 +1663,7 @@ static int dvd_do_auth(struct cdrom_device_info *cdi, dvd_authinfo *ai)
 		break;
 
 	case DVD_LU_SEND_KEY1:
-		cdinfo(CD_DVD, "entering DVD_LU_SEND_KEY1\n"); 
+		cd_dbg(CD_DVD, "entering DVD_LU_SEND_KEY1\n");
 		setup_report_key(&cgc, ai->lsk.agid, 2);
 
 		if ((ret = cdo->generic_packet(cdi, &cgc)))
@@ -1655,7 +1674,7 @@ static int dvd_do_auth(struct cdrom_device_info *cdi, dvd_authinfo *ai)
 		break;
 
 	case DVD_LU_SEND_CHALLENGE:
-		cdinfo(CD_DVD, "entering DVD_LU_SEND_CHALLENGE\n"); 
+		cd_dbg(CD_DVD, "entering DVD_LU_SEND_CHALLENGE\n");
 		setup_report_key(&cgc, ai->lsc.agid, 1);
 
 		if ((ret = cdo->generic_packet(cdi, &cgc)))
@@ -1667,7 +1686,7 @@ static int dvd_do_auth(struct cdrom_device_info *cdi, dvd_authinfo *ai)
 
 	/* Post-auth key */
 	case DVD_LU_SEND_TITLE_KEY:
-		cdinfo(CD_DVD, "entering DVD_LU_SEND_TITLE_KEY\n"); 
+		cd_dbg(CD_DVD, "entering DVD_LU_SEND_TITLE_KEY\n");
 		cgc.quiet = 1;
 		setup_report_key(&cgc, ai->lstk.agid, 4);
 		cgc.cmd[5] = ai->lstk.lba;
@@ -1686,7 +1705,7 @@ static int dvd_do_auth(struct cdrom_device_info *cdi, dvd_authinfo *ai)
 		break;
 
 	case DVD_LU_SEND_ASF:
-		cdinfo(CD_DVD, "entering DVD_LU_SEND_ASF\n"); 
+		cd_dbg(CD_DVD, "entering DVD_LU_SEND_ASF\n");
 		setup_report_key(&cgc, ai->lsasf.agid, 5);
 		
 		if ((ret = cdo->generic_packet(cdi, &cgc)))
@@ -1697,7 +1716,7 @@ static int dvd_do_auth(struct cdrom_device_info *cdi, dvd_authinfo *ai)
 
 	/* LU data receive (LU changes state) */
 	case DVD_HOST_SEND_CHALLENGE:
-		cdinfo(CD_DVD, "entering DVD_HOST_SEND_CHALLENGE\n"); 
+		cd_dbg(CD_DVD, "entering DVD_HOST_SEND_CHALLENGE\n");
 		setup_send_key(&cgc, ai->hsc.agid, 1);
 		buf[1] = 0xe;
 		copy_chal(&buf[4], ai->hsc.chal);
@@ -1709,7 +1728,7 @@ static int dvd_do_auth(struct cdrom_device_info *cdi, dvd_authinfo *ai)
 		break;
 
 	case DVD_HOST_SEND_KEY2:
-		cdinfo(CD_DVD, "entering DVD_HOST_SEND_KEY2\n"); 
+		cd_dbg(CD_DVD, "entering DVD_HOST_SEND_KEY2\n");
 		setup_send_key(&cgc, ai->hsk.agid, 3);
 		buf[1] = 0xa;
 		copy_key(&buf[4], ai->hsk.key);
@@ -1724,7 +1743,7 @@ static int dvd_do_auth(struct cdrom_device_info *cdi, dvd_authinfo *ai)
 	/* Misc */
 	case DVD_INVALIDATE_AGID:
 		cgc.quiet = 1;
-		cdinfo(CD_DVD, "entering DVD_INVALIDATE_AGID\n"); 
+		cd_dbg(CD_DVD, "entering DVD_INVALIDATE_AGID\n");
 		setup_report_key(&cgc, ai->lsa.agid, 0x3f);
 		if ((ret = cdo->generic_packet(cdi, &cgc)))
 			return ret;
@@ -1732,7 +1751,7 @@ static int dvd_do_auth(struct cdrom_device_info *cdi, dvd_authinfo *ai)
 
 	/* Get region settings */
 	case DVD_LU_SEND_RPC_STATE:
-		cdinfo(CD_DVD, "entering DVD_LU_SEND_RPC_STATE\n");
+		cd_dbg(CD_DVD, "entering DVD_LU_SEND_RPC_STATE\n");
 		setup_report_key(&cgc, 0, 8);
 		memset(&rpc_state, 0, sizeof(rpc_state_t));
 		cgc.buffer = (char *) &rpc_state;
@@ -1749,7 +1768,7 @@ static int dvd_do_auth(struct cdrom_device_info *cdi, dvd_authinfo *ai)
 
 	/* Set region settings */
 	case DVD_HOST_SEND_RPC_STATE:
-		cdinfo(CD_DVD, "entering DVD_HOST_SEND_RPC_STATE\n");
+		cd_dbg(CD_DVD, "entering DVD_HOST_SEND_RPC_STATE\n");
 		setup_send_key(&cgc, 0, 6);
 		buf[1] = 6;
 		buf[4] = ai->hrpcs.pdrc;
@@ -1759,7 +1778,7 @@ static int dvd_do_auth(struct cdrom_device_info *cdi, dvd_authinfo *ai)
 		break;
 
 	default:
-		cdinfo(CD_WARNING, "Invalid DVD key ioctl (%d)\n", ai->type);
+		cd_dbg(CD_WARNING, "Invalid DVD key ioctl (%d)\n", ai->type);
 		return -ENOTTY;
 	}
 
@@ -1891,7 +1910,8 @@ static int dvd_read_bca(struct cdrom_device_info *cdi, dvd_struct *s,
 
 	s->bca.len = buf[0] << 8 | buf[1];
 	if (s->bca.len < 12 || s->bca.len > 188) {
-		cdinfo(CD_WARNING, "Received invalid BCA length (%d)\n", s->bca.len);
+		cd_dbg(CD_WARNING, "Received invalid BCA length (%d)\n",
+		       s->bca.len);
 		ret = -EIO;
 		goto out;
 	}
@@ -1927,14 +1947,13 @@ static int dvd_read_manufact(struct cdrom_device_info *cdi, dvd_struct *s,
 
 	s->manufact.len = buf[0] << 8 | buf[1];
 	if (s->manufact.len < 0) {
-		cdinfo(CD_WARNING, "Received invalid manufacture info length"
-				   " (%d)\n", s->manufact.len);
+		cd_dbg(CD_WARNING, "Received invalid manufacture info length (%d)\n",
+		       s->manufact.len);
 		ret = -EIO;
 	} else {
 		if (s->manufact.len > 2048) {
-			cdinfo(CD_WARNING, "Received invalid manufacture info "
-					"length (%d): truncating to 2048\n",
-					s->manufact.len);
+			cd_dbg(CD_WARNING, "Received invalid manufacture info length (%d): truncating to 2048\n",
+			       s->manufact.len);
 			s->manufact.len = 2048;
 		}
 		memcpy(s->manufact.value, &buf[4], s->manufact.len);
@@ -1965,8 +1984,8 @@ static int dvd_read_struct(struct cdrom_device_info *cdi, dvd_struct *s,
 		return dvd_read_manufact(cdi, s, cgc);
 		
 	default:
-		cdinfo(CD_WARNING, ": Invalid DVD structure read requested (%d)\n",
-					s->type);
+		cd_dbg(CD_WARNING, ": Invalid DVD structure read requested (%d)\n",
+		       s->type);
 		return -EINVAL;
 	}
 }
@@ -2255,7 +2274,7 @@ static int cdrom_ioctl_multisession(struct cdrom_device_info *cdi,
 	u8 requested_format;
 	int ret;
 
-	cdinfo(CD_DO_IOCTL, "entering CDROMMULTISESSION\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROMMULTISESSION\n");
 
 	if (!(cdi->ops->capability & CDC_MULTI_SESSION))
 		return -ENOSYS;
@@ -2277,13 +2296,13 @@ static int cdrom_ioctl_multisession(struct cdrom_device_info *cdi,
 	if (copy_to_user(argp, &ms_info, sizeof(ms_info)))
 		return -EFAULT;
 
-	cdinfo(CD_DO_IOCTL, "CDROMMULTISESSION successful\n");
+	cd_dbg(CD_DO_IOCTL, "CDROMMULTISESSION successful\n");
 	return 0;
 }
 
 static int cdrom_ioctl_eject(struct cdrom_device_info *cdi)
 {
-	cdinfo(CD_DO_IOCTL, "entering CDROMEJECT\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROMEJECT\n");
 
 	if (!CDROM_CAN(CDC_OPEN_TRAY))
 		return -ENOSYS;
@@ -2300,7 +2319,7 @@ static int cdrom_ioctl_eject(struct cdrom_device_info *cdi)
 
 static int cdrom_ioctl_closetray(struct cdrom_device_info *cdi)
 {
-	cdinfo(CD_DO_IOCTL, "entering CDROMCLOSETRAY\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROMCLOSETRAY\n");
 
 	if (!CDROM_CAN(CDC_CLOSE_TRAY))
 		return -ENOSYS;
@@ -2310,7 +2329,7 @@ static int cdrom_ioctl_closetray(struct cdrom_device_info *cdi)
 static int cdrom_ioctl_eject_sw(struct cdrom_device_info *cdi,
 		unsigned long arg)
 {
-	cdinfo(CD_DO_IOCTL, "entering CDROMEJECT_SW\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROMEJECT_SW\n");
 
 	if (!CDROM_CAN(CDC_OPEN_TRAY))
 		return -ENOSYS;
@@ -2329,7 +2348,7 @@ static int cdrom_ioctl_media_changed(struct cdrom_device_info *cdi,
 	struct cdrom_changer_info *info;
 	int ret;
 
-	cdinfo(CD_DO_IOCTL, "entering CDROM_MEDIA_CHANGED\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROM_MEDIA_CHANGED\n");
 
 	if (!CDROM_CAN(CDC_MEDIA_CHANGED))
 		return -ENOSYS;
@@ -2355,7 +2374,7 @@ static int cdrom_ioctl_media_changed(struct cdrom_device_info *cdi,
 static int cdrom_ioctl_set_options(struct cdrom_device_info *cdi,
 		unsigned long arg)
 {
-	cdinfo(CD_DO_IOCTL, "entering CDROM_SET_OPTIONS\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROM_SET_OPTIONS\n");
 
 	/*
 	 * Options need to be in sync with capability.
@@ -2383,7 +2402,7 @@ static int cdrom_ioctl_set_options(struct cdrom_device_info *cdi,
 static int cdrom_ioctl_clear_options(struct cdrom_device_info *cdi,
 		unsigned long arg)
 {
-	cdinfo(CD_DO_IOCTL, "entering CDROM_CLEAR_OPTIONS\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROM_CLEAR_OPTIONS\n");
 
 	cdi->options &= ~(int) arg;
 	return cdi->options;
@@ -2392,7 +2411,7 @@ static int cdrom_ioctl_clear_options(struct cdrom_device_info *cdi,
 static int cdrom_ioctl_select_speed(struct cdrom_device_info *cdi,
 		unsigned long arg)
 {
-	cdinfo(CD_DO_IOCTL, "entering CDROM_SELECT_SPEED\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROM_SELECT_SPEED\n");
 
 	if (!CDROM_CAN(CDC_SELECT_SPEED))
 		return -ENOSYS;
@@ -2402,7 +2421,7 @@ static int cdrom_ioctl_select_speed(struct cdrom_device_info *cdi,
 static int cdrom_ioctl_select_disc(struct cdrom_device_info *cdi,
 		unsigned long arg)
 {
-	cdinfo(CD_DO_IOCTL, "entering CDROM_SELECT_DISC\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROM_SELECT_DISC\n");
 
 	if (!CDROM_CAN(CDC_SELECT_DISC))
 		return -ENOSYS;
@@ -2420,14 +2439,14 @@ static int cdrom_ioctl_select_disc(struct cdrom_device_info *cdi,
 	if (cdi->ops->select_disc)
 		return cdi->ops->select_disc(cdi, arg);
 
-	cdinfo(CD_CHANGER, "Using generic cdrom_select_disc()\n");
+	cd_dbg(CD_CHANGER, "Using generic cdrom_select_disc()\n");
 	return cdrom_select_disc(cdi, arg);
 }
 
 static int cdrom_ioctl_reset(struct cdrom_device_info *cdi,
 		struct block_device *bdev)
 {
-	cdinfo(CD_DO_IOCTL, "entering CDROM_RESET\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROM_RESET\n");
 
 	if (!capable(CAP_SYS_ADMIN))
 		return -EACCES;
@@ -2440,7 +2459,7 @@ static int cdrom_ioctl_reset(struct cdrom_device_info *cdi,
 static int cdrom_ioctl_lock_door(struct cdrom_device_info *cdi,
 		unsigned long arg)
 {
-	cdinfo(CD_DO_IOCTL, "%socking door.\n", arg ? "L" : "Unl");
+	cd_dbg(CD_DO_IOCTL, "%socking door\n", arg ? "L" : "Unl");
 
 	if (!CDROM_CAN(CDC_LOCK))
 		return -EDRIVE_CANT_DO_THIS;
@@ -2459,7 +2478,7 @@ static int cdrom_ioctl_lock_door(struct cdrom_device_info *cdi,
 static int cdrom_ioctl_debug(struct cdrom_device_info *cdi,
 		unsigned long arg)
 {
-	cdinfo(CD_DO_IOCTL, "%sabling debug.\n", arg ? "En" : "Dis");
+	cd_dbg(CD_DO_IOCTL, "%sabling debug\n", arg ? "En" : "Dis");
 
 	if (!capable(CAP_SYS_ADMIN))
 		return -EACCES;
@@ -2469,7 +2488,7 @@ static int cdrom_ioctl_debug(struct cdrom_device_info *cdi,
 
 static int cdrom_ioctl_get_capability(struct cdrom_device_info *cdi)
 {
-	cdinfo(CD_DO_IOCTL, "entering CDROM_GET_CAPABILITY\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROM_GET_CAPABILITY\n");
 	return (cdi->ops->capability & ~cdi->mask);
 }
 
@@ -2485,7 +2504,7 @@ static int cdrom_ioctl_get_mcn(struct cdrom_device_info *cdi,
 	struct cdrom_mcn mcn;
 	int ret;
 
-	cdinfo(CD_DO_IOCTL, "entering CDROM_GET_MCN\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROM_GET_MCN\n");
 
 	if (!(cdi->ops->capability & CDC_MCN))
 		return -ENOSYS;
@@ -2495,14 +2514,14 @@ static int cdrom_ioctl_get_mcn(struct cdrom_device_info *cdi,
 
 	if (copy_to_user(argp, &mcn, sizeof(mcn)))
 		return -EFAULT;
-	cdinfo(CD_DO_IOCTL, "CDROM_GET_MCN successful\n");
+	cd_dbg(CD_DO_IOCTL, "CDROM_GET_MCN successful\n");
 	return 0;
 }
 
 static int cdrom_ioctl_drive_status(struct cdrom_device_info *cdi,
 		unsigned long arg)
 {
-	cdinfo(CD_DO_IOCTL, "entering CDROM_DRIVE_STATUS\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROM_DRIVE_STATUS\n");
 
 	if (!(cdi->ops->capability & CDC_DRIVE_STATUS))
 		return -ENOSYS;
@@ -2535,7 +2554,7 @@ static int cdrom_ioctl_disc_status(struct cdrom_device_info *cdi)
 {
 	tracktype tracks;
 
-	cdinfo(CD_DO_IOCTL, "entering CDROM_DISC_STATUS\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROM_DISC_STATUS\n");
 
 	cdrom_count_tracks(cdi, &tracks);
 	if (tracks.error)
@@ -2557,13 +2576,13 @@ static int cdrom_ioctl_disc_status(struct cdrom_device_info *cdi)
 		return CDS_DATA_1;
 	/* Policy mode off */
 
-	cdinfo(CD_WARNING,"This disc doesn't have any tracks I recognize!\n");
+	cd_dbg(CD_WARNING, "This disc doesn't have any tracks I recognize!\n");
 	return CDS_NO_INFO;
 }
 
 static int cdrom_ioctl_changer_nslots(struct cdrom_device_info *cdi)
 {
-	cdinfo(CD_DO_IOCTL, "entering CDROM_CHANGER_NSLOTS\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROM_CHANGER_NSLOTS\n");
 	return cdi->capacity;
 }
 
@@ -2574,7 +2593,7 @@ static int cdrom_ioctl_get_subchnl(struct cdrom_device_info *cdi,
 	u8 requested, back;
 	int ret;
 
-	/* cdinfo(CD_DO_IOCTL,"entering CDROMSUBCHNL\n");*/
+	/* cd_dbg(CD_DO_IOCTL,"entering CDROMSUBCHNL\n");*/
 
 	if (copy_from_user(&q, argp, sizeof(q)))
 		return -EFAULT;
@@ -2594,7 +2613,7 @@ static int cdrom_ioctl_get_subchnl(struct cdrom_device_info *cdi,
 
 	if (copy_to_user(argp, &q, sizeof(q)))
 		return -EFAULT;
-	/* cdinfo(CD_DO_IOCTL, "CDROMSUBCHNL successful\n"); */
+	/* cd_dbg(CD_DO_IOCTL, "CDROMSUBCHNL successful\n"); */
 	return 0;
 }
 
@@ -2604,7 +2623,7 @@ static int cdrom_ioctl_read_tochdr(struct cdrom_device_info *cdi,
 	struct cdrom_tochdr header;
 	int ret;
 
-	/* cdinfo(CD_DO_IOCTL, "entering CDROMREADTOCHDR\n"); */
+	/* cd_dbg(CD_DO_IOCTL, "entering CDROMREADTOCHDR\n"); */
 
 	if (copy_from_user(&header, argp, sizeof(header)))
 		return -EFAULT;
@@ -2615,7 +2634,7 @@ static int cdrom_ioctl_read_tochdr(struct cdrom_device_info *cdi,
 
 	if (copy_to_user(argp, &header, sizeof(header)))
 		return -EFAULT;
-	/* cdinfo(CD_DO_IOCTL, "CDROMREADTOCHDR successful\n"); */
+	/* cd_dbg(CD_DO_IOCTL, "CDROMREADTOCHDR successful\n"); */
 	return 0;
 }
 
@@ -2626,7 +2645,7 @@ static int cdrom_ioctl_read_tocentry(struct cdrom_device_info *cdi,
 	u8 requested_format;
 	int ret;
 
-	/* cdinfo(CD_DO_IOCTL, "entering CDROMREADTOCENTRY\n"); */
+	/* cd_dbg(CD_DO_IOCTL, "entering CDROMREADTOCENTRY\n"); */
 
 	if (copy_from_user(&entry, argp, sizeof(entry)))
 		return -EFAULT;
@@ -2643,7 +2662,7 @@ static int cdrom_ioctl_read_tocentry(struct cdrom_device_info *cdi,
 
 	if (copy_to_user(argp, &entry, sizeof(entry)))
 		return -EFAULT;
-	/* cdinfo(CD_DO_IOCTL, "CDROMREADTOCENTRY successful\n"); */
+	/* cd_dbg(CD_DO_IOCTL, "CDROMREADTOCENTRY successful\n"); */
 	return 0;
 }
 
@@ -2652,7 +2671,7 @@ static int cdrom_ioctl_play_msf(struct cdrom_device_info *cdi,
 {
 	struct cdrom_msf msf;
 
-	cdinfo(CD_DO_IOCTL, "entering CDROMPLAYMSF\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROMPLAYMSF\n");
 
 	if (!CDROM_CAN(CDC_PLAY_AUDIO))
 		return -ENOSYS;
@@ -2667,7 +2686,7 @@ static int cdrom_ioctl_play_trkind(struct cdrom_device_info *cdi,
 	struct cdrom_ti ti;
 	int ret;
 
-	cdinfo(CD_DO_IOCTL, "entering CDROMPLAYTRKIND\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROMPLAYTRKIND\n");
 
 	if (!CDROM_CAN(CDC_PLAY_AUDIO))
 		return -ENOSYS;
@@ -2684,7 +2703,7 @@ static int cdrom_ioctl_volctrl(struct cdrom_device_info *cdi,
 {
 	struct cdrom_volctrl volume;
 
-	cdinfo(CD_DO_IOCTL, "entering CDROMVOLCTRL\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROMVOLCTRL\n");
 
 	if (!CDROM_CAN(CDC_PLAY_AUDIO))
 		return -ENOSYS;
@@ -2699,7 +2718,7 @@ static int cdrom_ioctl_volread(struct cdrom_device_info *cdi,
 	struct cdrom_volctrl volume;
 	int ret;
 
-	cdinfo(CD_DO_IOCTL, "entering CDROMVOLREAD\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROMVOLREAD\n");
 
 	if (!CDROM_CAN(CDC_PLAY_AUDIO))
 		return -ENOSYS;
@@ -2718,7 +2737,7 @@ static int cdrom_ioctl_audioctl(struct cdrom_device_info *cdi,
 {
 	int ret;
 
-	cdinfo(CD_DO_IOCTL, "doing audio ioctl (start/stop/pause/resume)\n");
+	cd_dbg(CD_DO_IOCTL, "doing audio ioctl (start/stop/pause/resume)\n");
 
 	if (!CDROM_CAN(CDC_PLAY_AUDIO))
 		return -ENOSYS;
@@ -2729,103 +2748,6 @@ static int cdrom_ioctl_audioctl(struct cdrom_device_info *cdi,
 }
 
 /*
- * Just about every imaginable ioctl is supported in the Uniform layer
- * these days.
- * ATAPI / SCSI specific code now mainly resides in mmc_ioctl().
- */
-int cdrom_ioctl(struct cdrom_device_info *cdi, struct block_device *bdev,
-		fmode_t mode, unsigned int cmd, unsigned long arg)
-{
-	void __user *argp = (void __user *)arg;
-	int ret;
-
-	/*
-	 * Try the generic SCSI command ioctl's first.
-	 */
-	ret = scsi_cmd_blk_ioctl(bdev, mode, cmd, argp);
-	if (ret != -ENOTTY)
-		return ret;
-
-	switch (cmd) {
-	case CDROMMULTISESSION:
-		return cdrom_ioctl_multisession(cdi, argp);
-	case CDROMEJECT:
-		return cdrom_ioctl_eject(cdi);
-	case CDROMCLOSETRAY:
-		return cdrom_ioctl_closetray(cdi);
-	case CDROMEJECT_SW:
-		return cdrom_ioctl_eject_sw(cdi, arg);
-	case CDROM_MEDIA_CHANGED:
-		return cdrom_ioctl_media_changed(cdi, arg);
-	case CDROM_SET_OPTIONS:
-		return cdrom_ioctl_set_options(cdi, arg);
-	case CDROM_CLEAR_OPTIONS:
-		return cdrom_ioctl_clear_options(cdi, arg);
-	case CDROM_SELECT_SPEED:
-		return cdrom_ioctl_select_speed(cdi, arg);
-	case CDROM_SELECT_DISC:
-		return cdrom_ioctl_select_disc(cdi, arg);
-	case CDROMRESET:
-		return cdrom_ioctl_reset(cdi, bdev);
-	case CDROM_LOCKDOOR:
-		return cdrom_ioctl_lock_door(cdi, arg);
-	case CDROM_DEBUG:
-		return cdrom_ioctl_debug(cdi, arg);
-	case CDROM_GET_CAPABILITY:
-		return cdrom_ioctl_get_capability(cdi);
-	case CDROM_GET_MCN:
-		return cdrom_ioctl_get_mcn(cdi, argp);
-	case CDROM_DRIVE_STATUS:
-		return cdrom_ioctl_drive_status(cdi, arg);
-	case CDROM_DISC_STATUS:
-		return cdrom_ioctl_disc_status(cdi);
-	case CDROM_CHANGER_NSLOTS:
-		return cdrom_ioctl_changer_nslots(cdi);
-	}
-
-	/*
-	 * Use the ioctls that are implemented through the generic_packet()
-	 * interface. this may look at bit funny, but if -ENOTTY is
-	 * returned that particular ioctl is not implemented and we
-	 * let it go through the device specific ones.
-	 */
-	if (CDROM_CAN(CDC_GENERIC_PACKET)) {
-		ret = mmc_ioctl(cdi, cmd, arg);
-		if (ret != -ENOTTY)
-			return ret;
-	}
-
-	/*
-	 * Note: most of the cdinfo() calls are commented out here,
-	 * because they fill up the sys log when CD players poll
-	 * the drive.
-	 */
-	switch (cmd) {
-	case CDROMSUBCHNL:
-		return cdrom_ioctl_get_subchnl(cdi, argp);
-	case CDROMREADTOCHDR:
-		return cdrom_ioctl_read_tochdr(cdi, argp);
-	case CDROMREADTOCENTRY:
-		return cdrom_ioctl_read_tocentry(cdi, argp);
-	case CDROMPLAYMSF:
-		return cdrom_ioctl_play_msf(cdi, argp);
-	case CDROMPLAYTRKIND:
-		return cdrom_ioctl_play_trkind(cdi, argp);
-	case CDROMVOLCTRL:
-		return cdrom_ioctl_volctrl(cdi, argp);
-	case CDROMVOLREAD:
-		return cdrom_ioctl_volread(cdi, argp);
-	case CDROMSTART:
-	case CDROMSTOP:
-	case CDROMPAUSE:
-	case CDROMRESUME:
-		return cdrom_ioctl_audioctl(cdi, cmd);
-	}
-
-	return -ENOSYS;
-}
-
-/*
  * Required when we need to use READ_10 to issue other than 2048 block
  * reads
  */
@@ -2854,10 +2776,158 @@ static int cdrom_switch_blocksize(struct cdrom_device_info *cdi, int size)
 	return cdo->generic_packet(cdi, &cgc);
 }
 
+static int cdrom_get_track_info(struct cdrom_device_info *cdi,
+				__u16 track, __u8 type, track_information *ti)
+{
+	struct cdrom_device_ops *cdo = cdi->ops;
+	struct packet_command cgc;
+	int ret, buflen;
+
+	init_cdrom_command(&cgc, ti, 8, CGC_DATA_READ);
+	cgc.cmd[0] = GPCMD_READ_TRACK_RZONE_INFO;
+	cgc.cmd[1] = type & 3;
+	cgc.cmd[4] = (track & 0xff00) >> 8;
+	cgc.cmd[5] = track & 0xff;
+	cgc.cmd[8] = 8;
+	cgc.quiet = 1;
+
+	ret = cdo->generic_packet(cdi, &cgc);
+	if (ret)
+		return ret;
+
+	buflen = be16_to_cpu(ti->track_information_length) +
+		sizeof(ti->track_information_length);
+
+	if (buflen > sizeof(track_information))
+		buflen = sizeof(track_information);
+
+	cgc.cmd[8] = cgc.buflen = buflen;
+	ret = cdo->generic_packet(cdi, &cgc);
+	if (ret)
+		return ret;
+
+	/* return actual fill size */
+	return buflen;
+}
+
+/* return the last written block on the CD-R media. this is for the udf
+   file system. */
+int cdrom_get_last_written(struct cdrom_device_info *cdi, long *last_written)
+{
+	struct cdrom_tocentry toc;
+	disc_information di;
+	track_information ti;
+	__u32 last_track;
+	int ret = -1, ti_size;
+
+	if (!CDROM_CAN(CDC_GENERIC_PACKET))
+		goto use_toc;
+
+	ret = cdrom_get_disc_info(cdi, &di);
+	if (ret < (int)(offsetof(typeof(di), last_track_lsb)
+			+ sizeof(di.last_track_lsb)))
+		goto use_toc;
+
+	/* if unit didn't return msb, it's zeroed by cdrom_get_disc_info */
+	last_track = (di.last_track_msb << 8) | di.last_track_lsb;
+	ti_size = cdrom_get_track_info(cdi, last_track, 1, &ti);
+	if (ti_size < (int)offsetof(typeof(ti), track_start))
+		goto use_toc;
+
+	/* if this track is blank, try the previous. */
+	if (ti.blank) {
+		if (last_track == 1)
+			goto use_toc;
+		last_track--;
+		ti_size = cdrom_get_track_info(cdi, last_track, 1, &ti);
+	}
+
+	if (ti_size < (int)(offsetof(typeof(ti), track_size)
+				+ sizeof(ti.track_size)))
+		goto use_toc;
+
+	/* if last recorded field is valid, return it. */
+	if (ti.lra_v && ti_size >= (int)(offsetof(typeof(ti), last_rec_address)
+				+ sizeof(ti.last_rec_address))) {
+		*last_written = be32_to_cpu(ti.last_rec_address);
+	} else {
+		/* make it up instead */
+		*last_written = be32_to_cpu(ti.track_start) +
+				be32_to_cpu(ti.track_size);
+		if (ti.free_blocks)
+			*last_written -= (be32_to_cpu(ti.free_blocks) + 7);
+	}
+	return 0;
+
+	/* this is where we end up if the drive either can't do a
+	   GPCMD_READ_DISC_INFO or GPCMD_READ_TRACK_RZONE_INFO or if
+	   it doesn't give enough information or fails. then we return
+	   the toc contents. */
+use_toc:
+	toc.cdte_format = CDROM_MSF;
+	toc.cdte_track = CDROM_LEADOUT;
+	if ((ret = cdi->ops->audio_ioctl(cdi, CDROMREADTOCENTRY, &toc)))
+		return ret;
+	sanitize_format(&toc.cdte_addr, &toc.cdte_format, CDROM_LBA);
+	*last_written = toc.cdte_addr.lba;
+	return 0;
+}
+
+/* return the next writable block. also for udf file system. */
+static int cdrom_get_next_writable(struct cdrom_device_info *cdi,
+				   long *next_writable)
+{
+	disc_information di;
+	track_information ti;
+	__u16 last_track;
+	int ret, ti_size;
+
+	if (!CDROM_CAN(CDC_GENERIC_PACKET))
+		goto use_last_written;
+
+	ret = cdrom_get_disc_info(cdi, &di);
+	if (ret < 0 || ret < offsetof(typeof(di), last_track_lsb)
+				+ sizeof(di.last_track_lsb))
+		goto use_last_written;
+
+	/* if unit didn't return msb, it's zeroed by cdrom_get_disc_info */
+	last_track = (di.last_track_msb << 8) | di.last_track_lsb;
+	ti_size = cdrom_get_track_info(cdi, last_track, 1, &ti);
+	if (ti_size < 0 || ti_size < offsetof(typeof(ti), track_start))
+		goto use_last_written;
+
+	/* if this track is blank, try the previous. */
+	if (ti.blank) {
+		if (last_track == 1)
+			goto use_last_written;
+		last_track--;
+		ti_size = cdrom_get_track_info(cdi, last_track, 1, &ti);
+		if (ti_size < 0)
+			goto use_last_written;
+	}
+
+	/* if next recordable address field is valid, use it. */
+	if (ti.nwa_v && ti_size >= offsetof(typeof(ti), next_writable)
+				+ sizeof(ti.next_writable)) {
+		*next_writable = be32_to_cpu(ti.next_writable);
+		return 0;
+	}
+
+use_last_written:
+	ret = cdrom_get_last_written(cdi, next_writable);
+	if (ret) {
+		*next_writable = 0;
+		return ret;
+	} else {
+		*next_writable += 7;
+		return 0;
+	}
+}
+
 static noinline int mmc_ioctl_cdrom_read_data(struct cdrom_device_info *cdi,
-					void __user *arg,
-					struct packet_command *cgc,
-					int cmd)
+					      void __user *arg,
+					      struct packet_command *cgc,
+					      int cmd)
 {
 	struct request_sense sense;
 	struct cdrom_msf msf;
@@ -2876,7 +2946,8 @@ static noinline int mmc_ioctl_cdrom_read_data(struct cdrom_device_info *cdi,
 		blocksize = CD_FRAMESIZE_RAW0;
 		break;
 	}
-	IOCTL_IN(arg, struct cdrom_msf, msf);
+	if (copy_from_user(&msf, (struct cdrom_msf __user *)arg, sizeof(msf)))
+		return -EFAULT;
 	lba = msf_to_lba(msf.cdmsf_min0, msf.cdmsf_sec0, msf.cdmsf_frame0);
 	/* FIXME: we need upper bound checking, too!! */
 	if (lba < 0)
@@ -2891,8 +2962,8 @@ static noinline int mmc_ioctl_cdrom_read_data(struct cdrom_device_info *cdi,
 	cgc->data_direction = CGC_DATA_READ;
 	ret = cdrom_read_block(cdi, cgc, lba, 1, format, blocksize);
 	if (ret && sense.sense_key == 0x05 &&
-		   sense.asc == 0x20 &&
-		   sense.ascq == 0x00) {
+	    sense.asc == 0x20 &&
+	    sense.ascq == 0x00) {
 		/*
 		 * SCSI-II devices are not required to support
 		 * READ_CD, so let's try switching block size
@@ -2913,12 +2984,14 @@ out:
 }
 
 static noinline int mmc_ioctl_cdrom_read_audio(struct cdrom_device_info *cdi,
-					void __user *arg)
+					       void __user *arg)
 {
 	struct cdrom_read_audio ra;
 	int lba;
 
-	IOCTL_IN(arg, struct cdrom_read_audio, ra);
+	if (copy_from_user(&ra, (struct cdrom_read_audio __user *)arg,
+			   sizeof(ra)))
+		return -EFAULT;
 
 	if (ra.addr_format == CDROM_MSF)
 		lba = msf_to_lba(ra.addr.msf.minute,
@@ -2937,12 +3010,13 @@ static noinline int mmc_ioctl_cdrom_read_audio(struct cdrom_device_info *cdi,
 }
 
 static noinline int mmc_ioctl_cdrom_subchannel(struct cdrom_device_info *cdi,
-					void __user *arg)
+					       void __user *arg)
 {
 	int ret;
 	struct cdrom_subchnl q;
 	u_char requested, back;
-	IOCTL_IN(arg, struct cdrom_subchnl, q);
+	if (copy_from_user(&q, (struct cdrom_subchnl __user *)arg, sizeof(q)))
+		return -EFAULT;
 	requested = q.cdsc_format;
 	if (!((requested == CDROM_MSF) ||
 	      (requested == CDROM_LBA)))
@@ -2954,19 +3028,21 @@ static noinline int mmc_ioctl_cdrom_subchannel(struct cdrom_device_info *cdi,
 	back = q.cdsc_format; /* local copy */
 	sanitize_format(&q.cdsc_absaddr, &back, requested);
 	sanitize_format(&q.cdsc_reladdr, &q.cdsc_format, requested);
-	IOCTL_OUT(arg, struct cdrom_subchnl, q);
-	/* cdinfo(CD_DO_IOCTL, "CDROMSUBCHNL successful\n"); */
+	if (copy_to_user((struct cdrom_subchnl __user *)arg, &q, sizeof(q)))
+		return -EFAULT;
+	/* cd_dbg(CD_DO_IOCTL, "CDROMSUBCHNL successful\n"); */
 	return 0;
 }
 
 static noinline int mmc_ioctl_cdrom_play_msf(struct cdrom_device_info *cdi,
-					void __user *arg,
-					struct packet_command *cgc)
+					     void __user *arg,
+					     struct packet_command *cgc)
 {
 	struct cdrom_device_ops *cdo = cdi->ops;
 	struct cdrom_msf msf;
-	cdinfo(CD_DO_IOCTL, "entering CDROMPLAYMSF\n");
-	IOCTL_IN(arg, struct cdrom_msf, msf);
+	cd_dbg(CD_DO_IOCTL, "entering CDROMPLAYMSF\n");
+	if (copy_from_user(&msf, (struct cdrom_msf __user *)arg, sizeof(msf)))
+		return -EFAULT;
 	cgc->cmd[0] = GPCMD_PLAY_AUDIO_MSF;
 	cgc->cmd[3] = msf.cdmsf_min0;
 	cgc->cmd[4] = msf.cdmsf_sec0;
@@ -2979,13 +3055,14 @@ static noinline int mmc_ioctl_cdrom_play_msf(struct cdrom_device_info *cdi,
 }
 
 static noinline int mmc_ioctl_cdrom_play_blk(struct cdrom_device_info *cdi,
-					void __user *arg,
-					struct packet_command *cgc)
+					     void __user *arg,
+					     struct packet_command *cgc)
 {
 	struct cdrom_device_ops *cdo = cdi->ops;
 	struct cdrom_blk blk;
-	cdinfo(CD_DO_IOCTL, "entering CDROMPLAYBLK\n");
-	IOCTL_IN(arg, struct cdrom_blk, blk);
+	cd_dbg(CD_DO_IOCTL, "entering CDROMPLAYBLK\n");
+	if (copy_from_user(&blk, (struct cdrom_blk __user *)arg, sizeof(blk)))
+		return -EFAULT;
 	cgc->cmd[0] = GPCMD_PLAY_AUDIO_10;
 	cgc->cmd[2] = (blk.from >> 24) & 0xff;
 	cgc->cmd[3] = (blk.from >> 16) & 0xff;
@@ -2998,9 +3075,9 @@ static noinline int mmc_ioctl_cdrom_play_blk(struct cdrom_device_info *cdi,
 }
 
 static noinline int mmc_ioctl_cdrom_volume(struct cdrom_device_info *cdi,
-					void __user *arg,
-					struct packet_command *cgc,
-					unsigned int cmd)
+					   void __user *arg,
+					   struct packet_command *cgc,
+					   unsigned int cmd)
 {
 	struct cdrom_volctrl volctrl;
 	unsigned char buffer[32];
@@ -3008,9 +3085,11 @@ static noinline int mmc_ioctl_cdrom_volume(struct cdrom_device_info *cdi,
 	unsigned short offset;
 	int ret;
 
-	cdinfo(CD_DO_IOCTL, "entering CDROMVOLUME\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROMVOLUME\n");
 
-	IOCTL_IN(arg, struct cdrom_volctrl, volctrl);
+	if (copy_from_user(&volctrl, (struct cdrom_volctrl __user *)arg,
+			   sizeof(volctrl)))
+		return -EFAULT;
 
 	cgc->buffer = buffer;
 	cgc->buflen = 24;
@@ -3030,14 +3109,14 @@ static noinline int mmc_ioctl_cdrom_volume(struct cdrom_device_info *cdi,
 	if (offset + 16 > cgc->buflen) {
 		cgc->buflen = offset + 16;
 		ret = cdrom_mode_sense(cdi, cgc,
-					GPMODE_AUDIO_CTL_PAGE, 0);
+				       GPMODE_AUDIO_CTL_PAGE, 0);
 		if (ret)
 			return ret;
 	}
 
 	/* sanity check */
 	if ((buffer[offset] & 0x3f) != GPMODE_AUDIO_CTL_PAGE ||
-			buffer[offset + 1] < 14)
+	    buffer[offset + 1] < 14)
 		return -EINVAL;
 
 	/* now we have the current volume settings. if it was only
@@ -3047,7 +3126,9 @@ static noinline int mmc_ioctl_cdrom_volume(struct cdrom_device_info *cdi,
 		volctrl.channel1 = buffer[offset+11];
 		volctrl.channel2 = buffer[offset+13];
 		volctrl.channel3 = buffer[offset+15];
-		IOCTL_OUT(arg, struct cdrom_volctrl, volctrl);
+		if (copy_to_user((struct cdrom_volctrl __user *)arg, &volctrl,
+				 sizeof(volctrl)))
+			return -EFAULT;
 		return 0;
 	}
 		
@@ -3069,11 +3150,11 @@ static noinline int mmc_ioctl_cdrom_volume(struct cdrom_device_info *cdi,
 }
 
 static noinline int mmc_ioctl_cdrom_start_stop(struct cdrom_device_info *cdi,
-					struct packet_command *cgc,
-					int cmd)
+					       struct packet_command *cgc,
+					       int cmd)
 {
 	struct cdrom_device_ops *cdo = cdi->ops;
-	cdinfo(CD_DO_IOCTL, "entering CDROMSTART/CDROMSTOP\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROMSTART/CDROMSTOP\n");
 	cgc->cmd[0] = GPCMD_START_STOP_UNIT;
 	cgc->cmd[1] = 1;
 	cgc->cmd[4] = (cmd == CDROMSTART) ? 1 : 0;
@@ -3082,11 +3163,11 @@ static noinline int mmc_ioctl_cdrom_start_stop(struct cdrom_device_info *cdi,
 }
 
 static noinline int mmc_ioctl_cdrom_pause_resume(struct cdrom_device_info *cdi,
-					struct packet_command *cgc,
-					int cmd)
+						 struct packet_command *cgc,
+						 int cmd)
 {
 	struct cdrom_device_ops *cdo = cdi->ops;
-	cdinfo(CD_DO_IOCTL, "entering CDROMPAUSE/CDROMRESUME\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROMPAUSE/CDROMRESUME\n");
 	cgc->cmd[0] = GPCMD_PAUSE_RESUME;
 	cgc->cmd[8] = (cmd == CDROMRESUME) ? 1 : 0;
 	cgc->data_direction = CGC_DATA_NONE;
@@ -3094,8 +3175,8 @@ static noinline int mmc_ioctl_cdrom_pause_resume(struct cdrom_device_info *cdi,
 }
 
 static noinline int mmc_ioctl_dvd_read_struct(struct cdrom_device_info *cdi,
-						void __user *arg,
-						struct packet_command *cgc)
+					      void __user *arg,
+					      struct packet_command *cgc)
 {
 	int ret;
 	dvd_struct *s;
@@ -3108,7 +3189,7 @@ static noinline int mmc_ioctl_dvd_read_struct(struct cdrom_device_info *cdi,
 	if (!s)
 		return -ENOMEM;
 
-	cdinfo(CD_DO_IOCTL, "entering DVD_READ_STRUCT\n");
+	cd_dbg(CD_DO_IOCTL, "entering DVD_READ_STRUCT\n");
 	if (copy_from_user(s, arg, size)) {
 		kfree(s);
 		return -EFAULT;
@@ -3126,44 +3207,48 @@ out:
 }
 
 static noinline int mmc_ioctl_dvd_auth(struct cdrom_device_info *cdi,
-					void __user *arg)
+				       void __user *arg)
 {
 	int ret;
 	dvd_authinfo ai;
 	if (!CDROM_CAN(CDC_DVD))
 		return -ENOSYS;
-	cdinfo(CD_DO_IOCTL, "entering DVD_AUTH\n");
-	IOCTL_IN(arg, dvd_authinfo, ai);
+	cd_dbg(CD_DO_IOCTL, "entering DVD_AUTH\n");
+	if (copy_from_user(&ai, (dvd_authinfo __user *)arg, sizeof(ai)))
+		return -EFAULT;
 	ret = dvd_do_auth(cdi, &ai);
 	if (ret)
 		return ret;
-	IOCTL_OUT(arg, dvd_authinfo, ai);
+	if (copy_to_user((dvd_authinfo __user *)arg, &ai, sizeof(ai)))
+		return -EFAULT;
 	return 0;
 }
 
 static noinline int mmc_ioctl_cdrom_next_writable(struct cdrom_device_info *cdi,
-						void __user *arg)
+						  void __user *arg)
 {
 	int ret;
 	long next = 0;
-	cdinfo(CD_DO_IOCTL, "entering CDROM_NEXT_WRITABLE\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROM_NEXT_WRITABLE\n");
 	ret = cdrom_get_next_writable(cdi, &next);
 	if (ret)
 		return ret;
-	IOCTL_OUT(arg, long, next);
+	if (copy_to_user((long __user *)arg, &next, sizeof(next)))
+		return -EFAULT;
 	return 0;
 }
 
 static noinline int mmc_ioctl_cdrom_last_written(struct cdrom_device_info *cdi,
-						void __user *arg)
+						 void __user *arg)
 {
 	int ret;
 	long last = 0;
-	cdinfo(CD_DO_IOCTL, "entering CDROM_LAST_WRITTEN\n");
+	cd_dbg(CD_DO_IOCTL, "entering CDROM_LAST_WRITTEN\n");
 	ret = cdrom_get_last_written(cdi, &last);
 	if (ret)
 		return ret;
-	IOCTL_OUT(arg, long, last);
+	if (copy_to_user((long __user *)arg, &last, sizeof(last)))
+		return -EFAULT;
 	return 0;
 }
 
@@ -3212,181 +3297,101 @@ static int mmc_ioctl(struct cdrom_device_info *cdi, unsigned int cmd,
 	return -ENOTTY;
 }
 
-static int cdrom_get_track_info(struct cdrom_device_info *cdi, __u16 track, __u8 type,
-			 track_information *ti)
-{
-	struct cdrom_device_ops *cdo = cdi->ops;
-	struct packet_command cgc;
-	int ret, buflen;
-
-	init_cdrom_command(&cgc, ti, 8, CGC_DATA_READ);
-	cgc.cmd[0] = GPCMD_READ_TRACK_RZONE_INFO;
-	cgc.cmd[1] = type & 3;
-	cgc.cmd[4] = (track & 0xff00) >> 8;
-	cgc.cmd[5] = track & 0xff;
-	cgc.cmd[8] = 8;
-	cgc.quiet = 1;
-
-	if ((ret = cdo->generic_packet(cdi, &cgc)))
-		return ret;
-	
-	buflen = be16_to_cpu(ti->track_information_length) +
-		     sizeof(ti->track_information_length);
-
-	if (buflen > sizeof(track_information))
-		buflen = sizeof(track_information);
-
-	cgc.cmd[8] = cgc.buflen = buflen;
-	if ((ret = cdo->generic_packet(cdi, &cgc)))
-		return ret;
-
-	/* return actual fill size */
-	return buflen;
-}
-
-/* requires CD R/RW */
-static int cdrom_get_disc_info(struct cdrom_device_info *cdi, disc_information *di)
+/*
+ * Just about every imaginable ioctl is supported in the Uniform layer
+ * these days.
+ * ATAPI / SCSI specific code now mainly resides in mmc_ioctl().
+ */
+int cdrom_ioctl(struct cdrom_device_info *cdi, struct block_device *bdev,
+		fmode_t mode, unsigned int cmd, unsigned long arg)
 {
-	struct cdrom_device_ops *cdo = cdi->ops;
-	struct packet_command cgc;
-	int ret, buflen;
-
-	/* set up command and get the disc info */
-	init_cdrom_command(&cgc, di, sizeof(*di), CGC_DATA_READ);
-	cgc.cmd[0] = GPCMD_READ_DISC_INFO;
-	cgc.cmd[8] = cgc.buflen = 2;
-	cgc.quiet = 1;
-
-	if ((ret = cdo->generic_packet(cdi, &cgc)))
-		return ret;
+	void __user *argp = (void __user *)arg;
+	int ret;
 
-	/* not all drives have the same disc_info length, so requeue
-	 * packet with the length the drive tells us it can supply
+	/*
+	 * Try the generic SCSI command ioctl's first.
 	 */
-	buflen = be16_to_cpu(di->disc_information_length) +
-		     sizeof(di->disc_information_length);
-
-	if (buflen > sizeof(disc_information))
-		buflen = sizeof(disc_information);
-
-	cgc.cmd[8] = cgc.buflen = buflen;
-	if ((ret = cdo->generic_packet(cdi, &cgc)))
+	ret = scsi_cmd_blk_ioctl(bdev, mode, cmd, argp);
+	if (ret != -ENOTTY)
 		return ret;
 
-	/* return actual fill size */
-	return buflen;
-}
-
-/* return the last written block on the CD-R media. this is for the udf
-   file system. */
-int cdrom_get_last_written(struct cdrom_device_info *cdi, long *last_written)
-{
-	struct cdrom_tocentry toc;
-	disc_information di;
-	track_information ti;
-	__u32 last_track;
-	int ret = -1, ti_size;
-
-	if (!CDROM_CAN(CDC_GENERIC_PACKET))
-		goto use_toc;
-
-	ret = cdrom_get_disc_info(cdi, &di);
-	if (ret < (int)(offsetof(typeof(di), last_track_lsb)
-			+ sizeof(di.last_track_lsb)))
-		goto use_toc;
-
-	/* if unit didn't return msb, it's zeroed by cdrom_get_disc_info */
-	last_track = (di.last_track_msb << 8) | di.last_track_lsb;
-	ti_size = cdrom_get_track_info(cdi, last_track, 1, &ti);
-	if (ti_size < (int)offsetof(typeof(ti), track_start))
-		goto use_toc;
-
-	/* if this track is blank, try the previous. */
-	if (ti.blank) {
-		if (last_track==1)
-			goto use_toc;
-		last_track--;
-		ti_size = cdrom_get_track_info(cdi, last_track, 1, &ti);
-	}
-
-	if (ti_size < (int)(offsetof(typeof(ti), track_size)
-				+ sizeof(ti.track_size)))
-		goto use_toc;
-
-	/* if last recorded field is valid, return it. */
-	if (ti.lra_v && ti_size >= (int)(offsetof(typeof(ti), last_rec_address)
-				+ sizeof(ti.last_rec_address))) {
-		*last_written = be32_to_cpu(ti.last_rec_address);
-	} else {
-		/* make it up instead */
-		*last_written = be32_to_cpu(ti.track_start) +
-				be32_to_cpu(ti.track_size);
-		if (ti.free_blocks)
-			*last_written -= (be32_to_cpu(ti.free_blocks) + 7);
+	switch (cmd) {
+	case CDROMMULTISESSION:
+		return cdrom_ioctl_multisession(cdi, argp);
+	case CDROMEJECT:
+		return cdrom_ioctl_eject(cdi);
+	case CDROMCLOSETRAY:
+		return cdrom_ioctl_closetray(cdi);
+	case CDROMEJECT_SW:
+		return cdrom_ioctl_eject_sw(cdi, arg);
+	case CDROM_MEDIA_CHANGED:
+		return cdrom_ioctl_media_changed(cdi, arg);
+	case CDROM_SET_OPTIONS:
+		return cdrom_ioctl_set_options(cdi, arg);
+	case CDROM_CLEAR_OPTIONS:
+		return cdrom_ioctl_clear_options(cdi, arg);
+	case CDROM_SELECT_SPEED:
+		return cdrom_ioctl_select_speed(cdi, arg);
+	case CDROM_SELECT_DISC:
+		return cdrom_ioctl_select_disc(cdi, arg);
+	case CDROMRESET:
+		return cdrom_ioctl_reset(cdi, bdev);
+	case CDROM_LOCKDOOR:
+		return cdrom_ioctl_lock_door(cdi, arg);
+	case CDROM_DEBUG:
+		return cdrom_ioctl_debug(cdi, arg);
+	case CDROM_GET_CAPABILITY:
+		return cdrom_ioctl_get_capability(cdi);
+	case CDROM_GET_MCN:
+		return cdrom_ioctl_get_mcn(cdi, argp);
+	case CDROM_DRIVE_STATUS:
+		return cdrom_ioctl_drive_status(cdi, arg);
+	case CDROM_DISC_STATUS:
+		return cdrom_ioctl_disc_status(cdi);
+	case CDROM_CHANGER_NSLOTS:
+		return cdrom_ioctl_changer_nslots(cdi);
 	}
-	return 0;
 
-	/* this is where we end up if the drive either can't do a
-	   GPCMD_READ_DISC_INFO or GPCMD_READ_TRACK_RZONE_INFO or if
-	   it doesn't give enough information or fails. then we return
-	   the toc contents. */
-use_toc:
-	toc.cdte_format = CDROM_MSF;
-	toc.cdte_track = CDROM_LEADOUT;
-	if ((ret = cdi->ops->audio_ioctl(cdi, CDROMREADTOCENTRY, &toc)))
-		return ret;
-	sanitize_format(&toc.cdte_addr, &toc.cdte_format, CDROM_LBA);
-	*last_written = toc.cdte_addr.lba;
-	return 0;
-}
-
-/* return the next writable block. also for udf file system. */
-static int cdrom_get_next_writable(struct cdrom_device_info *cdi, long *next_writable)
-{
-	disc_information di;
-	track_information ti;
-	__u16 last_track;
-	int ret, ti_size;
-
-	if (!CDROM_CAN(CDC_GENERIC_PACKET))
-		goto use_last_written;
-
-	ret = cdrom_get_disc_info(cdi, &di);
-	if (ret < 0 || ret < offsetof(typeof(di), last_track_lsb)
-				+ sizeof(di.last_track_lsb))
-		goto use_last_written;
-
-	/* if unit didn't return msb, it's zeroed by cdrom_get_disc_info */
-	last_track = (di.last_track_msb << 8) | di.last_track_lsb;
-	ti_size = cdrom_get_track_info(cdi, last_track, 1, &ti);
-	if (ti_size < 0 || ti_size < offsetof(typeof(ti), track_start))
-		goto use_last_written;
-
-        /* if this track is blank, try the previous. */
-	if (ti.blank) {
-		if (last_track == 1)
-			goto use_last_written;
-		last_track--;
-		ti_size = cdrom_get_track_info(cdi, last_track, 1, &ti);
-		if (ti_size < 0)
-			goto use_last_written;
+	/*
+	 * Use the ioctls that are implemented through the generic_packet()
+	 * interface. this may look at bit funny, but if -ENOTTY is
+	 * returned that particular ioctl is not implemented and we
+	 * let it go through the device specific ones.
+	 */
+	if (CDROM_CAN(CDC_GENERIC_PACKET)) {
+		ret = mmc_ioctl(cdi, cmd, arg);
+		if (ret != -ENOTTY)
+			return ret;
 	}
 
-	/* if next recordable address field is valid, use it. */
-	if (ti.nwa_v && ti_size >= offsetof(typeof(ti), next_writable)
-				+ sizeof(ti.next_writable)) {
-		*next_writable = be32_to_cpu(ti.next_writable);
-		return 0;
+	/*
+	 * Note: most of the cd_dbg() calls are commented out here,
+	 * because they fill up the sys log when CD players poll
+	 * the drive.
+	 */
+	switch (cmd) {
+	case CDROMSUBCHNL:
+		return cdrom_ioctl_get_subchnl(cdi, argp);
+	case CDROMREADTOCHDR:
+		return cdrom_ioctl_read_tochdr(cdi, argp);
+	case CDROMREADTOCENTRY:
+		return cdrom_ioctl_read_tocentry(cdi, argp);
+	case CDROMPLAYMSF:
+		return cdrom_ioctl_play_msf(cdi, argp);
+	case CDROMPLAYTRKIND:
+		return cdrom_ioctl_play_trkind(cdi, argp);
+	case CDROMVOLCTRL:
+		return cdrom_ioctl_volctrl(cdi, argp);
+	case CDROMVOLREAD:
+		return cdrom_ioctl_volread(cdi, argp);
+	case CDROMSTART:
+	case CDROMSTOP:
+	case CDROMPAUSE:
+	case CDROMRESUME:
+		return cdrom_ioctl_audioctl(cdi, cmd);
 	}
 
-use_last_written:
-	if ((ret = cdrom_get_last_written(cdi, next_writable))) {
-		*next_writable = 0;
-		return ret;
-	} else {
-		*next_writable += 7;
-		return 0;
-	}
+	return -ENOSYS;
 }
 
 EXPORT_SYMBOL(cdrom_get_last_written);
diff --git a/drivers/cdrom/gdrom.c b/drivers/cdrom/gdrom.c
index 51e75ad9642..584bc312640 100644
--- a/drivers/cdrom/gdrom.c
+++ b/drivers/cdrom/gdrom.c
@@ -602,7 +602,7 @@ static void gdrom_readdisk_dma(struct work_struct *work)
 		spin_unlock(&gdrom_lock);
 		block = blk_rq_pos(req)/GD_TO_BLK + GD_SESSION_OFFSET;
 		block_cnt = blk_rq_sectors(req)/GD_TO_BLK;
-		__raw_writel(virt_to_phys(req->buffer), GDROM_DMA_STARTADDR_REG);
+		__raw_writel(virt_to_phys(bio_data(req->bio)), GDROM_DMA_STARTADDR_REG);
 		__raw_writel(block_cnt * GDROM_HARD_SECTOR, GDROM_DMA_LENGTH_REG);
 		__raw_writel(1, GDROM_DMA_DIRECTION_REG);
 		__raw_writel(1, GDROM_DMA_ENABLE_REG);
diff --git a/drivers/char/Kconfig b/drivers/char/Kconfig
index fbae63e3d30..6e9f74a5c09 100644
--- a/drivers/char/Kconfig
+++ b/drivers/char/Kconfig
@@ -40,7 +40,7 @@ config SGI_MBCS
 source "drivers/tty/serial/Kconfig"
 
 config TTY_PRINTK
-	bool "TTY driver to output user messages via printk"
+	tristate "TTY driver to output user messages via printk"
 	depends on EXPERT && TTY
 	default n
 	---help---
diff --git a/drivers/char/agp/frontend.c b/drivers/char/agp/frontend.c
index 8121b4c70ed..b29703324e9 100644
--- a/drivers/char/agp/frontend.c
+++ b/drivers/char/agp/frontend.c
@@ -730,6 +730,7 @@ static int agpioc_info_wrap(struct agp_file_private *priv, void __user *arg)
 
 	agp_copy_info(agp_bridge, &kerninfo);
 
+	memset(&userinfo, 0, sizeof(userinfo));
 	userinfo.version.major = kerninfo.version.major;
 	userinfo.version.minor = kerninfo.version.minor;
 	userinfo.bridge_id = kerninfo.device->vendor |
diff --git a/drivers/char/applicom.c b/drivers/char/applicom.c
index 974321a2508..14790304b84 100644
--- a/drivers/char/applicom.c
+++ b/drivers/char/applicom.c
@@ -345,7 +345,6 @@ out:
 			free_irq(apbs[i].irq, &dummy);
 		iounmap(apbs[i].RamIO);
 	}
-	pci_disable_device(dev);
 	return ret;
 }
 
diff --git a/drivers/char/pcmcia/Kconfig b/drivers/char/pcmcia/Kconfig
index b27f5342fe7..8d3dfb0c8a2 100644
--- a/drivers/char/pcmcia/Kconfig
+++ b/drivers/char/pcmcia/Kconfig
@@ -15,7 +15,7 @@ config SYNCLINK_CS
 
 	  This driver may be built as a module ( = code which can be
 	  inserted in and removed from the running kernel whenever you want).
-	  The module will be called synclinkmp.  If you want to do that, say M
+	  The module will be called synclink_cs.  If you want to do that, say M
 	  here.
 
 config CARDMAN_4000
diff --git a/drivers/char/random.c b/drivers/char/random.c
index 6b75713d953..06cea7ff3a7 100644
--- a/drivers/char/random.c
+++ b/drivers/char/random.c
@@ -902,6 +902,7 @@ void add_disk_randomness(struct gendisk *disk)
 	add_timer_randomness(disk->random, 0x100 + disk_devt(disk));
 	trace_add_disk_randomness(disk_devt(disk), ENTROPY_BITS(&input_pool));
 }
+EXPORT_SYMBOL_GPL(add_disk_randomness);
 #endif
 
 /*********************************************************************
@@ -995,8 +996,11 @@ retry:
 		ibytes = min_t(size_t, ibytes, have_bytes - reserved);
 	if (ibytes < min)
 		ibytes = 0;
-	entropy_count = max_t(int, 0,
-			      entropy_count - (ibytes << (ENTROPY_SHIFT + 3)));
+	if (have_bytes >= ibytes + reserved)
+		entropy_count -= ibytes << (ENTROPY_SHIFT + 3);
+	else
+		entropy_count = reserved << (ENTROPY_SHIFT + 3);
+
 	if (cmpxchg(&r->entropy_count, orig, entropy_count) != orig)
 		goto retry;
 
diff --git a/drivers/char/tpm/tpm_ppi.c b/drivers/char/tpm/tpm_ppi.c
index b3ea223585b..61dcc8011ec 100644
--- a/drivers/char/tpm/tpm_ppi.c
+++ b/drivers/char/tpm/tpm_ppi.c
@@ -328,13 +328,11 @@ int tpm_add_ppi(struct kobject *parent)
 	/* Cache TPM ACPI handle and version string */
 	acpi_walk_namespace(ACPI_TYPE_DEVICE, ACPI_ROOT_OBJECT, ACPI_UINT32_MAX,
 			    ppi_callback, NULL, NULL, &tpm_ppi_handle);
-	if (tpm_ppi_handle == NULL)
-		return -ENODEV;
-
-	return sysfs_create_group(parent, &ppi_attr_grp);
+	return tpm_ppi_handle ? sysfs_create_group(parent, &ppi_attr_grp) : 0;
 }
 
 void tpm_remove_ppi(struct kobject *parent)
 {
-	sysfs_remove_group(parent, &ppi_attr_grp);
+	if (tpm_ppi_handle)
+		sysfs_remove_group(parent, &ppi_attr_grp);
 }
diff --git a/drivers/char/ttyprintk.c b/drivers/char/ttyprintk.c
index daea84c4174..a15ce4ef39c 100644
--- a/drivers/char/ttyprintk.c
+++ b/drivers/char/ttyprintk.c
@@ -17,7 +17,7 @@
 #include <linux/device.h>
 #include <linux/serial.h>
 #include <linux/tty.h>
-#include <linux/export.h>
+#include <linux/module.h>
 
 struct ttyprintk_port {
 	struct tty_port port;
@@ -210,10 +210,19 @@ static int __init ttyprintk_init(void)
 	return 0;
 
 error:
-	tty_unregister_driver(ttyprintk_driver);
 	put_tty_driver(ttyprintk_driver);
 	tty_port_destroy(&tpk_port.port);
-	ttyprintk_driver = NULL;
 	return ret;
 }
+
+static void __exit ttyprintk_exit(void)
+{
+	tty_unregister_driver(ttyprintk_driver);
+	put_tty_driver(ttyprintk_driver);
+	tty_port_destroy(&tpk_port.port);
+}
+
 device_initcall(ttyprintk_init);
+module_exit(ttyprintk_exit);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/clk/Kconfig b/drivers/clk/Kconfig
index 6f56d3a4f01..3a2196481b1 100644
--- a/drivers/clk/Kconfig
+++ b/drivers/clk/Kconfig
@@ -30,14 +30,7 @@ config COMMON_CLK_WM831X
           Supports the clocking subsystem of the WM831x/2x series of
 	  PMICs from Wolfson Microlectronics.
 
-config COMMON_CLK_VERSATILE
-	bool "Clock driver for ARM Reference designs"
-	depends on ARCH_INTEGRATOR || ARCH_REALVIEW || ARCH_VEXPRESS || ARM64
-	---help---
-          Supports clocking on ARM Reference designs:
-	  - Integrator/AP and Integrator/CP
-	  - RealView PB1176, EB, PB11MP and PBX
-	  - Versatile Express
+source "drivers/clk/versatile/Kconfig"
 
 config COMMON_CLK_MAX77686
 	tristate "Clock driver for Maxim 77686 MFD"
@@ -115,3 +108,5 @@ endmenu
 
 source "drivers/clk/bcm/Kconfig"
 source "drivers/clk/mvebu/Kconfig"
+
+source "drivers/clk/samsung/Kconfig"
diff --git a/drivers/clk/Makefile b/drivers/clk/Makefile
index 5f8a28735c9..17d7f13d19a 100644
--- a/drivers/clk/Makefile
+++ b/drivers/clk/Makefile
@@ -41,7 +41,7 @@ obj-$(CONFIG_PLAT_ORION)		+= mvebu/
 obj-$(CONFIG_ARCH_MXS)			+= mxs/
 obj-$(CONFIG_COMMON_CLK_QCOM)		+= qcom/
 obj-$(CONFIG_ARCH_ROCKCHIP)		+= rockchip/
-obj-$(CONFIG_PLAT_SAMSUNG)		+= samsung/
+obj-$(CONFIG_COMMON_CLK_SAMSUNG)	+= samsung/
 obj-$(CONFIG_ARCH_SHMOBILE_MULTI)	+= shmobile/
 obj-$(CONFIG_ARCH_SIRF)			+= sirf/
 obj-$(CONFIG_ARCH_SOCFPGA)		+= socfpga/
diff --git a/drivers/clk/at91/Makefile b/drivers/clk/at91/Makefile
index 46c1d3d0d66..4998aee5926 100644
--- a/drivers/clk/at91/Makefile
+++ b/drivers/clk/at91/Makefile
@@ -2,8 +2,8 @@
 # Makefile for at91 specific clk
 #
 
-obj-y += pmc.o
-obj-y += clk-main.o clk-pll.o clk-plldiv.o clk-master.o
+obj-y += pmc.o sckc.o
+obj-y += clk-slow.o clk-main.o clk-pll.o clk-plldiv.o clk-master.o
 obj-y += clk-system.o clk-peripheral.o clk-programmable.o
 
 obj-$(CONFIG_HAVE_AT91_UTMI)		+= clk-utmi.o
diff --git a/drivers/clk/at91/clk-main.c b/drivers/clk/at91/clk-main.c
index 8e9e8cc0412..733306131b9 100644
--- a/drivers/clk/at91/clk-main.c
+++ b/drivers/clk/at91/clk-main.c
@@ -30,99 +30,546 @@
 #define MAINF_LOOP_MIN_WAIT	(USEC_PER_SEC / SLOW_CLOCK_FREQ)
 #define MAINF_LOOP_MAX_WAIT	MAINFRDY_TIMEOUT
 
-struct clk_main {
+#define MOR_KEY_MASK		(0xff << 16)
+
+struct clk_main_osc {
 	struct clk_hw hw;
 	struct at91_pmc *pmc;
-	unsigned long rate;
 	unsigned int irq;
 	wait_queue_head_t wait;
 };
 
-#define to_clk_main(hw) container_of(hw, struct clk_main, hw)
+#define to_clk_main_osc(hw) container_of(hw, struct clk_main_osc, hw)
+
+struct clk_main_rc_osc {
+	struct clk_hw hw;
+	struct at91_pmc *pmc;
+	unsigned int irq;
+	wait_queue_head_t wait;
+	unsigned long frequency;
+	unsigned long accuracy;
+};
+
+#define to_clk_main_rc_osc(hw) container_of(hw, struct clk_main_rc_osc, hw)
+
+struct clk_rm9200_main {
+	struct clk_hw hw;
+	struct at91_pmc *pmc;
+};
+
+#define to_clk_rm9200_main(hw) container_of(hw, struct clk_rm9200_main, hw)
 
-static irqreturn_t clk_main_irq_handler(int irq, void *dev_id)
+struct clk_sam9x5_main {
+	struct clk_hw hw;
+	struct at91_pmc *pmc;
+	unsigned int irq;
+	wait_queue_head_t wait;
+	u8 parent;
+};
+
+#define to_clk_sam9x5_main(hw) container_of(hw, struct clk_sam9x5_main, hw)
+
+static irqreturn_t clk_main_osc_irq_handler(int irq, void *dev_id)
 {
-	struct clk_main *clkmain = (struct clk_main *)dev_id;
+	struct clk_main_osc *osc = dev_id;
 
-	wake_up(&clkmain->wait);
-	disable_irq_nosync(clkmain->irq);
+	wake_up(&osc->wait);
+	disable_irq_nosync(osc->irq);
 
 	return IRQ_HANDLED;
 }
 
-static int clk_main_prepare(struct clk_hw *hw)
+static int clk_main_osc_prepare(struct clk_hw *hw)
 {
-	struct clk_main *clkmain = to_clk_main(hw);
-	struct at91_pmc *pmc = clkmain->pmc;
-	unsigned long halt_time, timeout;
+	struct clk_main_osc *osc = to_clk_main_osc(hw);
+	struct at91_pmc *pmc = osc->pmc;
 	u32 tmp;
 
+	tmp = pmc_read(pmc, AT91_CKGR_MOR) & ~MOR_KEY_MASK;
+	if (tmp & AT91_PMC_OSCBYPASS)
+		return 0;
+
+	if (!(tmp & AT91_PMC_MOSCEN)) {
+		tmp |= AT91_PMC_MOSCEN | AT91_PMC_KEY;
+		pmc_write(pmc, AT91_CKGR_MOR, tmp);
+	}
+
 	while (!(pmc_read(pmc, AT91_PMC_SR) & AT91_PMC_MOSCS)) {
-		enable_irq(clkmain->irq);
-		wait_event(clkmain->wait,
+		enable_irq(osc->irq);
+		wait_event(osc->wait,
 			   pmc_read(pmc, AT91_PMC_SR) & AT91_PMC_MOSCS);
 	}
 
-	if (clkmain->rate)
-		return 0;
+	return 0;
+}
+
+static void clk_main_osc_unprepare(struct clk_hw *hw)
+{
+	struct clk_main_osc *osc = to_clk_main_osc(hw);
+	struct at91_pmc *pmc = osc->pmc;
+	u32 tmp = pmc_read(pmc, AT91_CKGR_MOR);
+
+	if (tmp & AT91_PMC_OSCBYPASS)
+		return;
+
+	if (!(tmp & AT91_PMC_MOSCEN))
+		return;
+
+	tmp &= ~(AT91_PMC_KEY | AT91_PMC_MOSCEN);
+	pmc_write(pmc, AT91_CKGR_MOR, tmp | AT91_PMC_KEY);
+}
+
+static int clk_main_osc_is_prepared(struct clk_hw *hw)
+{
+	struct clk_main_osc *osc = to_clk_main_osc(hw);
+	struct at91_pmc *pmc = osc->pmc;
+	u32 tmp = pmc_read(pmc, AT91_CKGR_MOR);
+
+	if (tmp & AT91_PMC_OSCBYPASS)
+		return 1;
+
+	return !!((pmc_read(pmc, AT91_PMC_SR) & AT91_PMC_MOSCS) &&
+		  (pmc_read(pmc, AT91_CKGR_MOR) & AT91_PMC_MOSCEN));
+}
+
+static const struct clk_ops main_osc_ops = {
+	.prepare = clk_main_osc_prepare,
+	.unprepare = clk_main_osc_unprepare,
+	.is_prepared = clk_main_osc_is_prepared,
+};
+
+static struct clk * __init
+at91_clk_register_main_osc(struct at91_pmc *pmc,
+			   unsigned int irq,
+			   const char *name,
+			   const char *parent_name,
+			   bool bypass)
+{
+	int ret;
+	struct clk_main_osc *osc;
+	struct clk *clk = NULL;
+	struct clk_init_data init;
+
+	if (!pmc || !irq || !name || !parent_name)
+		return ERR_PTR(-EINVAL);
+
+	osc = kzalloc(sizeof(*osc), GFP_KERNEL);
+	if (!osc)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &main_osc_ops;
+	init.parent_names = &parent_name;
+	init.num_parents = 1;
+	init.flags = CLK_IGNORE_UNUSED;
+
+	osc->hw.init = &init;
+	osc->pmc = pmc;
+	osc->irq = irq;
+
+	init_waitqueue_head(&osc->wait);
+	irq_set_status_flags(osc->irq, IRQ_NOAUTOEN);
+	ret = request_irq(osc->irq, clk_main_osc_irq_handler,
+			  IRQF_TRIGGER_HIGH, name, osc);
+	if (ret)
+		return ERR_PTR(ret);
+
+	if (bypass)
+		pmc_write(pmc, AT91_CKGR_MOR,
+			  (pmc_read(pmc, AT91_CKGR_MOR) &
+			   ~(MOR_KEY_MASK | AT91_PMC_MOSCEN)) |
+			  AT91_PMC_OSCBYPASS | AT91_PMC_KEY);
+
+	clk = clk_register(NULL, &osc->hw);
+	if (IS_ERR(clk)) {
+		free_irq(irq, osc);
+		kfree(osc);
+	}
+
+	return clk;
+}
+
+void __init of_at91rm9200_clk_main_osc_setup(struct device_node *np,
+					     struct at91_pmc *pmc)
+{
+	struct clk *clk;
+	unsigned int irq;
+	const char *name = np->name;
+	const char *parent_name;
+	bool bypass;
+
+	of_property_read_string(np, "clock-output-names", &name);
+	bypass = of_property_read_bool(np, "atmel,osc-bypass");
+	parent_name = of_clk_get_parent_name(np, 0);
+
+	irq = irq_of_parse_and_map(np, 0);
+	if (!irq)
+		return;
+
+	clk = at91_clk_register_main_osc(pmc, irq, name, parent_name, bypass);
+	if (IS_ERR(clk))
+		return;
+
+	of_clk_add_provider(np, of_clk_src_simple_get, clk);
+}
+
+static irqreturn_t clk_main_rc_osc_irq_handler(int irq, void *dev_id)
+{
+	struct clk_main_rc_osc *osc = dev_id;
+
+	wake_up(&osc->wait);
+	disable_irq_nosync(osc->irq);
+
+	return IRQ_HANDLED;
+}
+
+static int clk_main_rc_osc_prepare(struct clk_hw *hw)
+{
+	struct clk_main_rc_osc *osc = to_clk_main_rc_osc(hw);
+	struct at91_pmc *pmc = osc->pmc;
+	u32 tmp;
+
+	tmp = pmc_read(pmc, AT91_CKGR_MOR) & ~MOR_KEY_MASK;
+
+	if (!(tmp & AT91_PMC_MOSCRCEN)) {
+		tmp |= AT91_PMC_MOSCRCEN | AT91_PMC_KEY;
+		pmc_write(pmc, AT91_CKGR_MOR, tmp);
+	}
+
+	while (!(pmc_read(pmc, AT91_PMC_SR) & AT91_PMC_MOSCRCS)) {
+		enable_irq(osc->irq);
+		wait_event(osc->wait,
+			   pmc_read(pmc, AT91_PMC_SR) & AT91_PMC_MOSCRCS);
+	}
+
+	return 0;
+}
+
+static void clk_main_rc_osc_unprepare(struct clk_hw *hw)
+{
+	struct clk_main_rc_osc *osc = to_clk_main_rc_osc(hw);
+	struct at91_pmc *pmc = osc->pmc;
+	u32 tmp = pmc_read(pmc, AT91_CKGR_MOR);
+
+	if (!(tmp & AT91_PMC_MOSCRCEN))
+		return;
+
+	tmp &= ~(MOR_KEY_MASK | AT91_PMC_MOSCRCEN);
+	pmc_write(pmc, AT91_CKGR_MOR, tmp | AT91_PMC_KEY);
+}
+
+static int clk_main_rc_osc_is_prepared(struct clk_hw *hw)
+{
+	struct clk_main_rc_osc *osc = to_clk_main_rc_osc(hw);
+	struct at91_pmc *pmc = osc->pmc;
+
+	return !!((pmc_read(pmc, AT91_PMC_SR) & AT91_PMC_MOSCRCS) &&
+		  (pmc_read(pmc, AT91_CKGR_MOR) & AT91_PMC_MOSCRCEN));
+}
+
+static unsigned long clk_main_rc_osc_recalc_rate(struct clk_hw *hw,
+						 unsigned long parent_rate)
+{
+	struct clk_main_rc_osc *osc = to_clk_main_rc_osc(hw);
+
+	return osc->frequency;
+}
+
+static unsigned long clk_main_rc_osc_recalc_accuracy(struct clk_hw *hw,
+						     unsigned long parent_acc)
+{
+	struct clk_main_rc_osc *osc = to_clk_main_rc_osc(hw);
+
+	return osc->accuracy;
+}
+
+static const struct clk_ops main_rc_osc_ops = {
+	.prepare = clk_main_rc_osc_prepare,
+	.unprepare = clk_main_rc_osc_unprepare,
+	.is_prepared = clk_main_rc_osc_is_prepared,
+	.recalc_rate = clk_main_rc_osc_recalc_rate,
+	.recalc_accuracy = clk_main_rc_osc_recalc_accuracy,
+};
+
+static struct clk * __init
+at91_clk_register_main_rc_osc(struct at91_pmc *pmc,
+			      unsigned int irq,
+			      const char *name,
+			      u32 frequency, u32 accuracy)
+{
+	int ret;
+	struct clk_main_rc_osc *osc;
+	struct clk *clk = NULL;
+	struct clk_init_data init;
+
+	if (!pmc || !irq || !name || !frequency)
+		return ERR_PTR(-EINVAL);
+
+	osc = kzalloc(sizeof(*osc), GFP_KERNEL);
+	if (!osc)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &main_rc_osc_ops;
+	init.parent_names = NULL;
+	init.num_parents = 0;
+	init.flags = CLK_IS_ROOT | CLK_IGNORE_UNUSED;
+
+	osc->hw.init = &init;
+	osc->pmc = pmc;
+	osc->irq = irq;
+	osc->frequency = frequency;
+	osc->accuracy = accuracy;
+
+	init_waitqueue_head(&osc->wait);
+	irq_set_status_flags(osc->irq, IRQ_NOAUTOEN);
+	ret = request_irq(osc->irq, clk_main_rc_osc_irq_handler,
+			  IRQF_TRIGGER_HIGH, name, osc);
+	if (ret)
+		return ERR_PTR(ret);
+
+	clk = clk_register(NULL, &osc->hw);
+	if (IS_ERR(clk)) {
+		free_irq(irq, osc);
+		kfree(osc);
+	}
+
+	return clk;
+}
+
+void __init of_at91sam9x5_clk_main_rc_osc_setup(struct device_node *np,
+						struct at91_pmc *pmc)
+{
+	struct clk *clk;
+	unsigned int irq;
+	u32 frequency = 0;
+	u32 accuracy = 0;
+	const char *name = np->name;
+
+	of_property_read_string(np, "clock-output-names", &name);
+	of_property_read_u32(np, "clock-frequency", &frequency);
+	of_property_read_u32(np, "clock-accuracy", &accuracy);
+
+	irq = irq_of_parse_and_map(np, 0);
+	if (!irq)
+		return;
+
+	clk = at91_clk_register_main_rc_osc(pmc, irq, name, frequency,
+					    accuracy);
+	if (IS_ERR(clk))
+		return;
+
+	of_clk_add_provider(np, of_clk_src_simple_get, clk);
+}
+
+
+static int clk_main_probe_frequency(struct at91_pmc *pmc)
+{
+	unsigned long prep_time, timeout;
+	u32 tmp;
 
 	timeout = jiffies + usecs_to_jiffies(MAINFRDY_TIMEOUT);
 	do {
-		halt_time = jiffies;
+		prep_time = jiffies;
 		tmp = pmc_read(pmc, AT91_CKGR_MCFR);
 		if (tmp & AT91_PMC_MAINRDY)
 			return 0;
 		usleep_range(MAINF_LOOP_MIN_WAIT, MAINF_LOOP_MAX_WAIT);
-	} while (time_before(halt_time, timeout));
+	} while (time_before(prep_time, timeout));
 
-	return 0;
+	return -ETIMEDOUT;
 }
 
-static int clk_main_is_prepared(struct clk_hw *hw)
+static unsigned long clk_main_recalc_rate(struct at91_pmc *pmc,
+					  unsigned long parent_rate)
 {
-	struct clk_main *clkmain = to_clk_main(hw);
+	u32 tmp;
+
+	if (parent_rate)
+		return parent_rate;
+
+	tmp = pmc_read(pmc, AT91_CKGR_MCFR);
+	if (!(tmp & AT91_PMC_MAINRDY))
+		return 0;
 
-	return !!(pmc_read(clkmain->pmc, AT91_PMC_SR) & AT91_PMC_MOSCS);
+	return ((tmp & AT91_PMC_MAINF) * SLOW_CLOCK_FREQ) / MAINF_DIV;
 }
 
-static unsigned long clk_main_recalc_rate(struct clk_hw *hw,
-					  unsigned long parent_rate)
+static int clk_rm9200_main_prepare(struct clk_hw *hw)
 {
-	u32 tmp;
-	struct clk_main *clkmain = to_clk_main(hw);
+	struct clk_rm9200_main *clkmain = to_clk_rm9200_main(hw);
+
+	return clk_main_probe_frequency(clkmain->pmc);
+}
+
+static int clk_rm9200_main_is_prepared(struct clk_hw *hw)
+{
+	struct clk_rm9200_main *clkmain = to_clk_rm9200_main(hw);
+
+	return !!(pmc_read(clkmain->pmc, AT91_CKGR_MCFR) & AT91_PMC_MAINRDY);
+}
+
+static unsigned long clk_rm9200_main_recalc_rate(struct clk_hw *hw,
+						 unsigned long parent_rate)
+{
+	struct clk_rm9200_main *clkmain = to_clk_rm9200_main(hw);
+
+	return clk_main_recalc_rate(clkmain->pmc, parent_rate);
+}
+
+static const struct clk_ops rm9200_main_ops = {
+	.prepare = clk_rm9200_main_prepare,
+	.is_prepared = clk_rm9200_main_is_prepared,
+	.recalc_rate = clk_rm9200_main_recalc_rate,
+};
+
+static struct clk * __init
+at91_clk_register_rm9200_main(struct at91_pmc *pmc,
+			      const char *name,
+			      const char *parent_name)
+{
+	struct clk_rm9200_main *clkmain;
+	struct clk *clk = NULL;
+	struct clk_init_data init;
+
+	if (!pmc || !name)
+		return ERR_PTR(-EINVAL);
+
+	if (!parent_name)
+		return ERR_PTR(-EINVAL);
+
+	clkmain = kzalloc(sizeof(*clkmain), GFP_KERNEL);
+	if (!clkmain)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &rm9200_main_ops;
+	init.parent_names = &parent_name;
+	init.num_parents = 1;
+	init.flags = 0;
+
+	clkmain->hw.init = &init;
+	clkmain->pmc = pmc;
+
+	clk = clk_register(NULL, &clkmain->hw);
+	if (IS_ERR(clk))
+		kfree(clkmain);
+
+	return clk;
+}
+
+void __init of_at91rm9200_clk_main_setup(struct device_node *np,
+					 struct at91_pmc *pmc)
+{
+	struct clk *clk;
+	const char *parent_name;
+	const char *name = np->name;
+
+	parent_name = of_clk_get_parent_name(np, 0);
+	of_property_read_string(np, "clock-output-names", &name);
+
+	clk = at91_clk_register_rm9200_main(pmc, name, parent_name);
+	if (IS_ERR(clk))
+		return;
+
+	of_clk_add_provider(np, of_clk_src_simple_get, clk);
+}
+
+static irqreturn_t clk_sam9x5_main_irq_handler(int irq, void *dev_id)
+{
+	struct clk_sam9x5_main *clkmain = dev_id;
+
+	wake_up(&clkmain->wait);
+	disable_irq_nosync(clkmain->irq);
+
+	return IRQ_HANDLED;
+}
+
+static int clk_sam9x5_main_prepare(struct clk_hw *hw)
+{
+	struct clk_sam9x5_main *clkmain = to_clk_sam9x5_main(hw);
 	struct at91_pmc *pmc = clkmain->pmc;
 
-	if (clkmain->rate)
-		return clkmain->rate;
+	while (!(pmc_read(pmc, AT91_PMC_SR) & AT91_PMC_MOSCSELS)) {
+		enable_irq(clkmain->irq);
+		wait_event(clkmain->wait,
+			   pmc_read(pmc, AT91_PMC_SR) & AT91_PMC_MOSCSELS);
+	}
+
+	return clk_main_probe_frequency(pmc);
+}
 
-	tmp = pmc_read(pmc, AT91_CKGR_MCFR) & AT91_PMC_MAINF;
-	clkmain->rate = (tmp * parent_rate) / MAINF_DIV;
+static int clk_sam9x5_main_is_prepared(struct clk_hw *hw)
+{
+	struct clk_sam9x5_main *clkmain = to_clk_sam9x5_main(hw);
 
-	return clkmain->rate;
+	return !!(pmc_read(clkmain->pmc, AT91_PMC_SR) & AT91_PMC_MOSCSELS);
 }
 
-static const struct clk_ops main_ops = {
-	.prepare = clk_main_prepare,
-	.is_prepared = clk_main_is_prepared,
-	.recalc_rate = clk_main_recalc_rate,
+static unsigned long clk_sam9x5_main_recalc_rate(struct clk_hw *hw,
+						 unsigned long parent_rate)
+{
+	struct clk_sam9x5_main *clkmain = to_clk_sam9x5_main(hw);
+
+	return clk_main_recalc_rate(clkmain->pmc, parent_rate);
+}
+
+static int clk_sam9x5_main_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct clk_sam9x5_main *clkmain = to_clk_sam9x5_main(hw);
+	struct at91_pmc *pmc = clkmain->pmc;
+	u32 tmp;
+
+	if (index > 1)
+		return -EINVAL;
+
+	tmp = pmc_read(pmc, AT91_CKGR_MOR) & ~MOR_KEY_MASK;
+
+	if (index && !(tmp & AT91_PMC_MOSCSEL))
+		pmc_write(pmc, AT91_CKGR_MOR, tmp | AT91_PMC_MOSCSEL);
+	else if (!index && (tmp & AT91_PMC_MOSCSEL))
+		pmc_write(pmc, AT91_CKGR_MOR, tmp & ~AT91_PMC_MOSCSEL);
+
+	while (!(pmc_read(pmc, AT91_PMC_SR) & AT91_PMC_MOSCSELS)) {
+		enable_irq(clkmain->irq);
+		wait_event(clkmain->wait,
+			   pmc_read(pmc, AT91_PMC_SR) & AT91_PMC_MOSCSELS);
+	}
+
+	return 0;
+}
+
+static u8 clk_sam9x5_main_get_parent(struct clk_hw *hw)
+{
+	struct clk_sam9x5_main *clkmain = to_clk_sam9x5_main(hw);
+
+	return !!(pmc_read(clkmain->pmc, AT91_CKGR_MOR) & AT91_PMC_MOSCEN);
+}
+
+static const struct clk_ops sam9x5_main_ops = {
+	.prepare = clk_sam9x5_main_prepare,
+	.is_prepared = clk_sam9x5_main_is_prepared,
+	.recalc_rate = clk_sam9x5_main_recalc_rate,
+	.set_parent = clk_sam9x5_main_set_parent,
+	.get_parent = clk_sam9x5_main_get_parent,
 };
 
 static struct clk * __init
-at91_clk_register_main(struct at91_pmc *pmc,
-		       unsigned int irq,
-		       const char *name,
-		       const char *parent_name,
-		       unsigned long rate)
+at91_clk_register_sam9x5_main(struct at91_pmc *pmc,
+			      unsigned int irq,
+			      const char *name,
+			      const char **parent_names,
+			      int num_parents)
 {
 	int ret;
-	struct clk_main *clkmain;
+	struct clk_sam9x5_main *clkmain;
 	struct clk *clk = NULL;
 	struct clk_init_data init;
 
 	if (!pmc || !irq || !name)
 		return ERR_PTR(-EINVAL);
 
-	if (!rate && !parent_name)
+	if (!parent_names || !num_parents)
 		return ERR_PTR(-EINVAL);
 
 	clkmain = kzalloc(sizeof(*clkmain), GFP_KERNEL);
@@ -130,19 +577,20 @@ at91_clk_register_main(struct at91_pmc *pmc,
 		return ERR_PTR(-ENOMEM);
 
 	init.name = name;
-	init.ops = &main_ops;
-	init.parent_names = parent_name ? &parent_name : NULL;
-	init.num_parents = parent_name ? 1 : 0;
-	init.flags = parent_name ? 0 : CLK_IS_ROOT;
+	init.ops = &sam9x5_main_ops;
+	init.parent_names = parent_names;
+	init.num_parents = num_parents;
+	init.flags = CLK_SET_PARENT_GATE;
 
 	clkmain->hw.init = &init;
-	clkmain->rate = rate;
 	clkmain->pmc = pmc;
 	clkmain->irq = irq;
+	clkmain->parent = !!(pmc_read(clkmain->pmc, AT91_CKGR_MOR) &
+			     AT91_PMC_MOSCEN);
 	init_waitqueue_head(&clkmain->wait);
 	irq_set_status_flags(clkmain->irq, IRQ_NOAUTOEN);
-	ret = request_irq(clkmain->irq, clk_main_irq_handler,
-			  IRQF_TRIGGER_HIGH, "clk-main", clkmain);
+	ret = request_irq(clkmain->irq, clk_sam9x5_main_irq_handler,
+			  IRQF_TRIGGER_HIGH, name, clkmain);
 	if (ret)
 		return ERR_PTR(ret);
 
@@ -155,33 +603,36 @@ at91_clk_register_main(struct at91_pmc *pmc,
 	return clk;
 }
 
-
-
-static void __init
-of_at91_clk_main_setup(struct device_node *np, struct at91_pmc *pmc)
+void __init of_at91sam9x5_clk_main_setup(struct device_node *np,
+					 struct at91_pmc *pmc)
 {
 	struct clk *clk;
+	const char *parent_names[2];
+	int num_parents;
 	unsigned int irq;
-	const char *parent_name;
 	const char *name = np->name;
-	u32 rate = 0;
+	int i;
+
+	num_parents = of_count_phandle_with_args(np, "clocks", "#clock-cells");
+	if (num_parents <= 0 || num_parents > 2)
+		return;
+
+	for (i = 0; i < num_parents; ++i) {
+		parent_names[i] = of_clk_get_parent_name(np, i);
+		if (!parent_names[i])
+			return;
+	}
 
-	parent_name = of_clk_get_parent_name(np, 0);
 	of_property_read_string(np, "clock-output-names", &name);
-	of_property_read_u32(np, "clock-frequency", &rate);
+
 	irq = irq_of_parse_and_map(np, 0);
 	if (!irq)
 		return;
 
-	clk = at91_clk_register_main(pmc, irq, name, parent_name, rate);
+	clk = at91_clk_register_sam9x5_main(pmc, irq, name, parent_names,
+					    num_parents);
 	if (IS_ERR(clk))
 		return;
 
 	of_clk_add_provider(np, of_clk_src_simple_get, clk);
 }
-
-void __init of_at91rm9200_clk_main_setup(struct device_node *np,
-					 struct at91_pmc *pmc)
-{
-	of_at91_clk_main_setup(np, pmc);
-}
diff --git a/drivers/clk/at91/clk-slow.c b/drivers/clk/at91/clk-slow.c
new file mode 100644
index 00000000000..0300c46ee24
--- /dev/null
+++ b/drivers/clk/at91/clk-slow.c
@@ -0,0 +1,467 @@
+/*
+ * drivers/clk/at91/clk-slow.c
+ *
+ *  Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.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/clk-provider.h>
+#include <linux/clkdev.h>
+#include <linux/clk/at91_pmc.h>
+#include <linux/delay.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+
+#include "pmc.h"
+#include "sckc.h"
+
+#define SLOW_CLOCK_FREQ		32768
+#define SLOWCK_SW_CYCLES	5
+#define SLOWCK_SW_TIME_USEC	((SLOWCK_SW_CYCLES * USEC_PER_SEC) / \
+				 SLOW_CLOCK_FREQ)
+
+#define	AT91_SCKC_CR			0x00
+#define		AT91_SCKC_RCEN		(1 << 0)
+#define		AT91_SCKC_OSC32EN	(1 << 1)
+#define		AT91_SCKC_OSC32BYP	(1 << 2)
+#define		AT91_SCKC_OSCSEL	(1 << 3)
+
+struct clk_slow_osc {
+	struct clk_hw hw;
+	void __iomem *sckcr;
+	unsigned long startup_usec;
+};
+
+#define to_clk_slow_osc(hw) container_of(hw, struct clk_slow_osc, hw)
+
+struct clk_slow_rc_osc {
+	struct clk_hw hw;
+	void __iomem *sckcr;
+	unsigned long frequency;
+	unsigned long accuracy;
+	unsigned long startup_usec;
+};
+
+#define to_clk_slow_rc_osc(hw) container_of(hw, struct clk_slow_rc_osc, hw)
+
+struct clk_sam9260_slow {
+	struct clk_hw hw;
+	struct at91_pmc *pmc;
+};
+
+#define to_clk_sam9260_slow(hw) container_of(hw, struct clk_sam9260_slow, hw)
+
+struct clk_sam9x5_slow {
+	struct clk_hw hw;
+	void __iomem *sckcr;
+	u8 parent;
+};
+
+#define to_clk_sam9x5_slow(hw) container_of(hw, struct clk_sam9x5_slow, hw)
+
+
+static int clk_slow_osc_prepare(struct clk_hw *hw)
+{
+	struct clk_slow_osc *osc = to_clk_slow_osc(hw);
+	void __iomem *sckcr = osc->sckcr;
+	u32 tmp = readl(sckcr);
+
+	if (tmp & AT91_SCKC_OSC32BYP)
+		return 0;
+
+	writel(tmp | AT91_SCKC_OSC32EN, sckcr);
+
+	usleep_range(osc->startup_usec, osc->startup_usec + 1);
+
+	return 0;
+}
+
+static void clk_slow_osc_unprepare(struct clk_hw *hw)
+{
+	struct clk_slow_osc *osc = to_clk_slow_osc(hw);
+	void __iomem *sckcr = osc->sckcr;
+	u32 tmp = readl(sckcr);
+
+	if (tmp & AT91_SCKC_OSC32BYP)
+		return;
+
+	writel(tmp & ~AT91_SCKC_OSC32EN, sckcr);
+}
+
+static int clk_slow_osc_is_prepared(struct clk_hw *hw)
+{
+	struct clk_slow_osc *osc = to_clk_slow_osc(hw);
+	void __iomem *sckcr = osc->sckcr;
+	u32 tmp = readl(sckcr);
+
+	if (tmp & AT91_SCKC_OSC32BYP)
+		return 1;
+
+	return !!(tmp & AT91_SCKC_OSC32EN);
+}
+
+static const struct clk_ops slow_osc_ops = {
+	.prepare = clk_slow_osc_prepare,
+	.unprepare = clk_slow_osc_unprepare,
+	.is_prepared = clk_slow_osc_is_prepared,
+};
+
+static struct clk * __init
+at91_clk_register_slow_osc(void __iomem *sckcr,
+			   const char *name,
+			   const char *parent_name,
+			   unsigned long startup,
+			   bool bypass)
+{
+	struct clk_slow_osc *osc;
+	struct clk *clk = NULL;
+	struct clk_init_data init;
+
+	if (!sckcr || !name || !parent_name)
+		return ERR_PTR(-EINVAL);
+
+	osc = kzalloc(sizeof(*osc), GFP_KERNEL);
+	if (!osc)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &slow_osc_ops;
+	init.parent_names = &parent_name;
+	init.num_parents = 1;
+	init.flags = CLK_IGNORE_UNUSED;
+
+	osc->hw.init = &init;
+	osc->sckcr = sckcr;
+	osc->startup_usec = startup;
+
+	if (bypass)
+		writel((readl(sckcr) & ~AT91_SCKC_OSC32EN) | AT91_SCKC_OSC32BYP,
+		       sckcr);
+
+	clk = clk_register(NULL, &osc->hw);
+	if (IS_ERR(clk))
+		kfree(osc);
+
+	return clk;
+}
+
+void __init of_at91sam9x5_clk_slow_osc_setup(struct device_node *np,
+					     void __iomem *sckcr)
+{
+	struct clk *clk;
+	const char *parent_name;
+	const char *name = np->name;
+	u32 startup;
+	bool bypass;
+
+	parent_name = of_clk_get_parent_name(np, 0);
+	of_property_read_string(np, "clock-output-names", &name);
+	of_property_read_u32(np, "atmel,startup-time-usec", &startup);
+	bypass = of_property_read_bool(np, "atmel,osc-bypass");
+
+	clk = at91_clk_register_slow_osc(sckcr, name, parent_name, startup,
+					 bypass);
+	if (IS_ERR(clk))
+		return;
+
+	of_clk_add_provider(np, of_clk_src_simple_get, clk);
+}
+
+static unsigned long clk_slow_rc_osc_recalc_rate(struct clk_hw *hw,
+						 unsigned long parent_rate)
+{
+	struct clk_slow_rc_osc *osc = to_clk_slow_rc_osc(hw);
+
+	return osc->frequency;
+}
+
+static unsigned long clk_slow_rc_osc_recalc_accuracy(struct clk_hw *hw,
+						     unsigned long parent_acc)
+{
+	struct clk_slow_rc_osc *osc = to_clk_slow_rc_osc(hw);
+
+	return osc->accuracy;
+}
+
+static int clk_slow_rc_osc_prepare(struct clk_hw *hw)
+{
+	struct clk_slow_rc_osc *osc = to_clk_slow_rc_osc(hw);
+	void __iomem *sckcr = osc->sckcr;
+
+	writel(readl(sckcr) | AT91_SCKC_RCEN, sckcr);
+
+	usleep_range(osc->startup_usec, osc->startup_usec + 1);
+
+	return 0;
+}
+
+static void clk_slow_rc_osc_unprepare(struct clk_hw *hw)
+{
+	struct clk_slow_rc_osc *osc = to_clk_slow_rc_osc(hw);
+	void __iomem *sckcr = osc->sckcr;
+
+	writel(readl(sckcr) & ~AT91_SCKC_RCEN, sckcr);
+}
+
+static int clk_slow_rc_osc_is_prepared(struct clk_hw *hw)
+{
+	struct clk_slow_rc_osc *osc = to_clk_slow_rc_osc(hw);
+
+	return !!(readl(osc->sckcr) & AT91_SCKC_RCEN);
+}
+
+static const struct clk_ops slow_rc_osc_ops = {
+	.prepare = clk_slow_rc_osc_prepare,
+	.unprepare = clk_slow_rc_osc_unprepare,
+	.is_prepared = clk_slow_rc_osc_is_prepared,
+	.recalc_rate = clk_slow_rc_osc_recalc_rate,
+	.recalc_accuracy = clk_slow_rc_osc_recalc_accuracy,
+};
+
+static struct clk * __init
+at91_clk_register_slow_rc_osc(void __iomem *sckcr,
+			      const char *name,
+			      unsigned long frequency,
+			      unsigned long accuracy,
+			      unsigned long startup)
+{
+	struct clk_slow_rc_osc *osc;
+	struct clk *clk = NULL;
+	struct clk_init_data init;
+
+	if (!sckcr || !name)
+		return ERR_PTR(-EINVAL);
+
+	osc = kzalloc(sizeof(*osc), GFP_KERNEL);
+	if (!osc)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &slow_rc_osc_ops;
+	init.parent_names = NULL;
+	init.num_parents = 0;
+	init.flags = CLK_IS_ROOT | CLK_IGNORE_UNUSED;
+
+	osc->hw.init = &init;
+	osc->sckcr = sckcr;
+	osc->frequency = frequency;
+	osc->accuracy = accuracy;
+	osc->startup_usec = startup;
+
+	clk = clk_register(NULL, &osc->hw);
+	if (IS_ERR(clk))
+		kfree(osc);
+
+	return clk;
+}
+
+void __init of_at91sam9x5_clk_slow_rc_osc_setup(struct device_node *np,
+						void __iomem *sckcr)
+{
+	struct clk *clk;
+	u32 frequency = 0;
+	u32 accuracy = 0;
+	u32 startup = 0;
+	const char *name = np->name;
+
+	of_property_read_string(np, "clock-output-names", &name);
+	of_property_read_u32(np, "clock-frequency", &frequency);
+	of_property_read_u32(np, "clock-accuracy", &accuracy);
+	of_property_read_u32(np, "atmel,startup-time-usec", &startup);
+
+	clk = at91_clk_register_slow_rc_osc(sckcr, name, frequency, accuracy,
+					    startup);
+	if (IS_ERR(clk))
+		return;
+
+	of_clk_add_provider(np, of_clk_src_simple_get, clk);
+}
+
+static int clk_sam9x5_slow_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct clk_sam9x5_slow *slowck = to_clk_sam9x5_slow(hw);
+	void __iomem *sckcr = slowck->sckcr;
+	u32 tmp;
+
+	if (index > 1)
+		return -EINVAL;
+
+	tmp = readl(sckcr);
+
+	if ((!index && !(tmp & AT91_SCKC_OSCSEL)) ||
+	    (index && (tmp & AT91_SCKC_OSCSEL)))
+		return 0;
+
+	if (index)
+		tmp |= AT91_SCKC_OSCSEL;
+	else
+		tmp &= ~AT91_SCKC_OSCSEL;
+
+	writel(tmp, sckcr);
+
+	usleep_range(SLOWCK_SW_TIME_USEC, SLOWCK_SW_TIME_USEC + 1);
+
+	return 0;
+}
+
+static u8 clk_sam9x5_slow_get_parent(struct clk_hw *hw)
+{
+	struct clk_sam9x5_slow *slowck = to_clk_sam9x5_slow(hw);
+
+	return !!(readl(slowck->sckcr) & AT91_SCKC_OSCSEL);
+}
+
+static const struct clk_ops sam9x5_slow_ops = {
+	.set_parent = clk_sam9x5_slow_set_parent,
+	.get_parent = clk_sam9x5_slow_get_parent,
+};
+
+static struct clk * __init
+at91_clk_register_sam9x5_slow(void __iomem *sckcr,
+			      const char *name,
+			      const char **parent_names,
+			      int num_parents)
+{
+	struct clk_sam9x5_slow *slowck;
+	struct clk *clk = NULL;
+	struct clk_init_data init;
+
+	if (!sckcr || !name || !parent_names || !num_parents)
+		return ERR_PTR(-EINVAL);
+
+	slowck = kzalloc(sizeof(*slowck), GFP_KERNEL);
+	if (!slowck)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &sam9x5_slow_ops;
+	init.parent_names = parent_names;
+	init.num_parents = num_parents;
+	init.flags = 0;
+
+	slowck->hw.init = &init;
+	slowck->sckcr = sckcr;
+	slowck->parent = !!(readl(sckcr) & AT91_SCKC_OSCSEL);
+
+	clk = clk_register(NULL, &slowck->hw);
+	if (IS_ERR(clk))
+		kfree(slowck);
+
+	return clk;
+}
+
+void __init of_at91sam9x5_clk_slow_setup(struct device_node *np,
+					 void __iomem *sckcr)
+{
+	struct clk *clk;
+	const char *parent_names[2];
+	int num_parents;
+	const char *name = np->name;
+	int i;
+
+	num_parents = of_count_phandle_with_args(np, "clocks", "#clock-cells");
+	if (num_parents <= 0 || num_parents > 2)
+		return;
+
+	for (i = 0; i < num_parents; ++i) {
+		parent_names[i] = of_clk_get_parent_name(np, i);
+		if (!parent_names[i])
+			return;
+	}
+
+	of_property_read_string(np, "clock-output-names", &name);
+
+	clk = at91_clk_register_sam9x5_slow(sckcr, name, parent_names,
+					    num_parents);
+	if (IS_ERR(clk))
+		return;
+
+	of_clk_add_provider(np, of_clk_src_simple_get, clk);
+}
+
+static u8 clk_sam9260_slow_get_parent(struct clk_hw *hw)
+{
+	struct clk_sam9260_slow *slowck = to_clk_sam9260_slow(hw);
+
+	return !!(pmc_read(slowck->pmc, AT91_PMC_SR) & AT91_PMC_OSCSEL);
+}
+
+static const struct clk_ops sam9260_slow_ops = {
+	.get_parent = clk_sam9260_slow_get_parent,
+};
+
+static struct clk * __init
+at91_clk_register_sam9260_slow(struct at91_pmc *pmc,
+			       const char *name,
+			       const char **parent_names,
+			       int num_parents)
+{
+	struct clk_sam9260_slow *slowck;
+	struct clk *clk = NULL;
+	struct clk_init_data init;
+
+	if (!pmc || !name)
+		return ERR_PTR(-EINVAL);
+
+	if (!parent_names || !num_parents)
+		return ERR_PTR(-EINVAL);
+
+	slowck = kzalloc(sizeof(*slowck), GFP_KERNEL);
+	if (!slowck)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &sam9260_slow_ops;
+	init.parent_names = parent_names;
+	init.num_parents = num_parents;
+	init.flags = 0;
+
+	slowck->hw.init = &init;
+	slowck->pmc = pmc;
+
+	clk = clk_register(NULL, &slowck->hw);
+	if (IS_ERR(clk))
+		kfree(slowck);
+
+	return clk;
+}
+
+void __init of_at91sam9260_clk_slow_setup(struct device_node *np,
+					  struct at91_pmc *pmc)
+{
+	struct clk *clk;
+	const char *parent_names[2];
+	int num_parents;
+	const char *name = np->name;
+	int i;
+
+	num_parents = of_count_phandle_with_args(np, "clocks", "#clock-cells");
+	if (num_parents <= 0 || num_parents > 1)
+		return;
+
+	for (i = 0; i < num_parents; ++i) {
+		parent_names[i] = of_clk_get_parent_name(np, i);
+		if (!parent_names[i])
+			return;
+	}
+
+	of_property_read_string(np, "clock-output-names", &name);
+
+	clk = at91_clk_register_sam9260_slow(pmc, name, parent_names,
+					     num_parents);
+	if (IS_ERR(clk))
+		return;
+
+	of_clk_add_provider(np, of_clk_src_simple_get, clk);
+}
diff --git a/drivers/clk/at91/pmc.c b/drivers/clk/at91/pmc.c
index 6a61477a57e..524196bb35a 100644
--- a/drivers/clk/at91/pmc.c
+++ b/drivers/clk/at91/pmc.c
@@ -229,11 +229,28 @@ out_free_pmc:
 }
 
 static const struct of_device_id pmc_clk_ids[] __initconst = {
+	/* Slow oscillator */
+	{
+		.compatible = "atmel,at91sam9260-clk-slow",
+		.data = of_at91sam9260_clk_slow_setup,
+	},
 	/* Main clock */
 	{
+		.compatible = "atmel,at91rm9200-clk-main-osc",
+		.data = of_at91rm9200_clk_main_osc_setup,
+	},
+	{
+		.compatible = "atmel,at91sam9x5-clk-main-rc-osc",
+		.data = of_at91sam9x5_clk_main_rc_osc_setup,
+	},
+	{
 		.compatible = "atmel,at91rm9200-clk-main",
 		.data = of_at91rm9200_clk_main_setup,
 	},
+	{
+		.compatible = "atmel,at91sam9x5-clk-main",
+		.data = of_at91sam9x5_clk_main_setup,
+	},
 	/* PLL clocks */
 	{
 		.compatible = "atmel,at91rm9200-clk-pll",
diff --git a/drivers/clk/at91/pmc.h b/drivers/clk/at91/pmc.h
index 441350983cc..6c762597611 100644
--- a/drivers/clk/at91/pmc.h
+++ b/drivers/clk/at91/pmc.h
@@ -58,8 +58,17 @@ static inline void pmc_write(struct at91_pmc *pmc, int offset, u32 value)
 int of_at91_get_clk_range(struct device_node *np, const char *propname,
 			  struct clk_range *range);
 
+extern void __init of_at91sam9260_clk_slow_setup(struct device_node *np,
+						 struct at91_pmc *pmc);
+
+extern void __init of_at91rm9200_clk_main_osc_setup(struct device_node *np,
+						    struct at91_pmc *pmc);
+extern void __init of_at91sam9x5_clk_main_rc_osc_setup(struct device_node *np,
+						       struct at91_pmc *pmc);
 extern void __init of_at91rm9200_clk_main_setup(struct device_node *np,
 						struct at91_pmc *pmc);
+extern void __init of_at91sam9x5_clk_main_setup(struct device_node *np,
+						struct at91_pmc *pmc);
 
 extern void __init of_at91rm9200_clk_pll_setup(struct device_node *np,
 					       struct at91_pmc *pmc);
diff --git a/drivers/clk/at91/sckc.c b/drivers/clk/at91/sckc.c
new file mode 100644
index 00000000000..1184d76a7ab
--- /dev/null
+++ b/drivers/clk/at91/sckc.c
@@ -0,0 +1,57 @@
+/*
+ * drivers/clk/at91/sckc.c
+ *
+ *  Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.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/clk-provider.h>
+#include <linux/clkdev.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/io.h>
+
+#include "sckc.h"
+
+static const struct of_device_id sckc_clk_ids[] __initconst = {
+	/* Slow clock */
+	{
+		.compatible = "atmel,at91sam9x5-clk-slow-osc",
+		.data = of_at91sam9x5_clk_slow_osc_setup,
+	},
+	{
+		.compatible = "atmel,at91sam9x5-clk-slow-rc-osc",
+		.data = of_at91sam9x5_clk_slow_rc_osc_setup,
+	},
+	{
+		.compatible = "atmel,at91sam9x5-clk-slow",
+		.data = of_at91sam9x5_clk_slow_setup,
+	},
+	{ /*sentinel*/ }
+};
+
+static void __init of_at91sam9x5_sckc_setup(struct device_node *np)
+{
+	struct device_node *childnp;
+	void (*clk_setup)(struct device_node *, void __iomem *);
+	const struct of_device_id *clk_id;
+	void __iomem *regbase = of_iomap(np, 0);
+
+	if (!regbase)
+		return;
+
+	for_each_child_of_node(np, childnp) {
+		clk_id = of_match_node(sckc_clk_ids, childnp);
+		if (!clk_id)
+			continue;
+		clk_setup = clk_id->data;
+		clk_setup(childnp, regbase);
+	}
+}
+CLK_OF_DECLARE(at91sam9x5_clk_sckc, "atmel,at91sam9x5-sckc",
+	       of_at91sam9x5_sckc_setup);
diff --git a/drivers/clk/at91/sckc.h b/drivers/clk/at91/sckc.h
new file mode 100644
index 00000000000..836fcf59820
--- /dev/null
+++ b/drivers/clk/at91/sckc.h
@@ -0,0 +1,22 @@
+/*
+ * drivers/clk/at91/sckc.h
+ *
+ *  Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.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 __AT91_SCKC_H_
+#define __AT91_SCKC_H_
+
+extern void __init of_at91sam9x5_clk_slow_osc_setup(struct device_node *np,
+						    void __iomem *sckcr);
+extern void __init of_at91sam9x5_clk_slow_rc_osc_setup(struct device_node *np,
+						       void __iomem *sckcr);
+extern void __init of_at91sam9x5_clk_slow_setup(struct device_node *np,
+						void __iomem *sckcr);
+
+#endif /* __AT91_SCKC_H_ */
diff --git a/drivers/clk/bcm/clk-kona-setup.c b/drivers/clk/bcm/clk-kona-setup.c
index c7607feb18d..54a06526f64 100644
--- a/drivers/clk/bcm/clk-kona-setup.c
+++ b/drivers/clk/bcm/clk-kona-setup.c
@@ -27,7 +27,7 @@ LIST_HEAD(ccu_list);	/* The list of set up CCUs */
 
 static bool clk_requires_trigger(struct kona_clk *bcm_clk)
 {
-	struct peri_clk_data *peri = bcm_clk->peri;
+	struct peri_clk_data *peri = bcm_clk->u.peri;
 	struct bcm_clk_sel *sel;
 	struct bcm_clk_div *div;
 
@@ -63,7 +63,7 @@ static bool peri_clk_data_offsets_valid(struct kona_clk *bcm_clk)
 	u32 limit;
 
 	BUG_ON(bcm_clk->type != bcm_clk_peri);
-	peri = bcm_clk->peri;
+	peri = bcm_clk->u.peri;
 	name = bcm_clk->name;
 	range = bcm_clk->ccu->range;
 
@@ -81,19 +81,19 @@ static bool peri_clk_data_offsets_valid(struct kona_clk *bcm_clk)
 
 	div = &peri->div;
 	if (divider_exists(div)) {
-		if (div->offset > limit) {
+		if (div->u.s.offset > limit) {
 			pr_err("%s: bad divider offset for %s (%u > %u)\n",
-				__func__, name, div->offset, limit);
+				__func__, name, div->u.s.offset, limit);
 			return false;
 		}
 	}
 
 	div = &peri->pre_div;
 	if (divider_exists(div)) {
-		if (div->offset > limit) {
+		if (div->u.s.offset > limit) {
 			pr_err("%s: bad pre-divider offset for %s "
 					"(%u > %u)\n",
-				__func__, name, div->offset, limit);
+				__func__, name, div->u.s.offset, limit);
 			return false;
 		}
 	}
@@ -249,21 +249,22 @@ static bool div_valid(struct bcm_clk_div *div, const char *field_name,
 {
 	if (divider_is_fixed(div)) {
 		/* Any fixed divider value but 0 is OK */
-		if (div->fixed == 0) {
+		if (div->u.fixed == 0) {
 			pr_err("%s: bad %s fixed value 0 for %s\n", __func__,
 				field_name, clock_name);
 			return false;
 		}
 		return true;
 	}
-	if (!bitfield_valid(div->shift, div->width, field_name, clock_name))
+	if (!bitfield_valid(div->u.s.shift, div->u.s.width,
+				field_name, clock_name))
 		return false;
 
 	if (divider_has_fraction(div))
-		if (div->frac_width > div->width) {
+		if (div->u.s.frac_width > div->u.s.width) {
 			pr_warn("%s: bad %s fraction width for %s (%u > %u)\n",
 				__func__, field_name, clock_name,
-				div->frac_width, div->width);
+				div->u.s.frac_width, div->u.s.width);
 			return false;
 		}
 
@@ -278,7 +279,7 @@ static bool div_valid(struct bcm_clk_div *div, const char *field_name,
  */
 static bool kona_dividers_valid(struct kona_clk *bcm_clk)
 {
-	struct peri_clk_data *peri = bcm_clk->peri;
+	struct peri_clk_data *peri = bcm_clk->u.peri;
 	struct bcm_clk_div *div;
 	struct bcm_clk_div *pre_div;
 	u32 limit;
@@ -295,7 +296,7 @@ static bool kona_dividers_valid(struct kona_clk *bcm_clk)
 
 	limit = BITS_PER_BYTE * sizeof(u32);
 
-	return div->frac_width + pre_div->frac_width <= limit;
+	return div->u.s.frac_width + pre_div->u.s.frac_width <= limit;
 }
 
 
@@ -328,7 +329,7 @@ peri_clk_data_valid(struct kona_clk *bcm_clk)
 	if (!peri_clk_data_offsets_valid(bcm_clk))
 		return false;
 
-	peri = bcm_clk->peri;
+	peri = bcm_clk->u.peri;
 	name = bcm_clk->name;
 	gate = &peri->gate;
 	if (gate_exists(gate) && !gate_valid(gate, "gate", name))
@@ -588,12 +589,12 @@ static void bcm_clk_teardown(struct kona_clk *bcm_clk)
 {
 	switch (bcm_clk->type) {
 	case bcm_clk_peri:
-		peri_clk_teardown(bcm_clk->data, &bcm_clk->init_data);
+		peri_clk_teardown(bcm_clk->u.data, &bcm_clk->init_data);
 		break;
 	default:
 		break;
 	}
-	bcm_clk->data = NULL;
+	bcm_clk->u.data = NULL;
 	bcm_clk->type = bcm_clk_none;
 }
 
@@ -644,7 +645,7 @@ struct clk *kona_clk_setup(struct ccu_data *ccu, const char *name,
 		break;
 	}
 	bcm_clk->type = type;
-	bcm_clk->data = data;
+	bcm_clk->u.data = data;
 
 	/* Make sure everything makes sense before we set it up */
 	if (!kona_clk_valid(bcm_clk)) {
diff --git a/drivers/clk/bcm/clk-kona.c b/drivers/clk/bcm/clk-kona.c
index e3d339e0830..db11a87449f 100644
--- a/drivers/clk/bcm/clk-kona.c
+++ b/drivers/clk/bcm/clk-kona.c
@@ -61,7 +61,7 @@ u64 do_div_round_closest(u64 dividend, unsigned long divisor)
 /* Convert a divider into the scaled divisor value it represents. */
 static inline u64 scaled_div_value(struct bcm_clk_div *div, u32 reg_div)
 {
-	return (u64)reg_div + ((u64)1 << div->frac_width);
+	return (u64)reg_div + ((u64)1 << div->u.s.frac_width);
 }
 
 /*
@@ -77,7 +77,7 @@ u64 scaled_div_build(struct bcm_clk_div *div, u32 div_value, u32 billionths)
 	BUG_ON(billionths >= BILLION);
 
 	combined = (u64)div_value * BILLION + billionths;
-	combined <<= div->frac_width;
+	combined <<= div->u.s.frac_width;
 
 	return do_div_round_closest(combined, BILLION);
 }
@@ -87,7 +87,7 @@ static inline u64
 scaled_div_min(struct bcm_clk_div *div)
 {
 	if (divider_is_fixed(div))
-		return (u64)div->fixed;
+		return (u64)div->u.fixed;
 
 	return scaled_div_value(div, 0);
 }
@@ -98,9 +98,9 @@ u64 scaled_div_max(struct bcm_clk_div *div)
 	u32 reg_div;
 
 	if (divider_is_fixed(div))
-		return (u64)div->fixed;
+		return (u64)div->u.fixed;
 
-	reg_div = ((u32)1 << div->width) - 1;
+	reg_div = ((u32)1 << div->u.s.width) - 1;
 
 	return scaled_div_value(div, reg_div);
 }
@@ -115,7 +115,7 @@ divider(struct bcm_clk_div *div, u64 scaled_div)
 	BUG_ON(scaled_div < scaled_div_min(div));
 	BUG_ON(scaled_div > scaled_div_max(div));
 
-	return (u32)(scaled_div - ((u64)1 << div->frac_width));
+	return (u32)(scaled_div - ((u64)1 << div->u.s.frac_width));
 }
 
 /* Return a rate scaled for use when dividing by a scaled divisor. */
@@ -125,7 +125,7 @@ scale_rate(struct bcm_clk_div *div, u32 rate)
 	if (divider_is_fixed(div))
 		return (u64)rate;
 
-	return (u64)rate << div->frac_width;
+	return (u64)rate << div->u.s.frac_width;
 }
 
 /* CCU access */
@@ -398,14 +398,14 @@ static u64 divider_read_scaled(struct ccu_data *ccu, struct bcm_clk_div *div)
 	u32 reg_div;
 
 	if (divider_is_fixed(div))
-		return (u64)div->fixed;
+		return (u64)div->u.fixed;
 
 	flags = ccu_lock(ccu);
-	reg_val = __ccu_read(ccu, div->offset);
+	reg_val = __ccu_read(ccu, div->u.s.offset);
 	ccu_unlock(ccu, flags);
 
 	/* Extract the full divider field from the register value */
-	reg_div = bitfield_extract(reg_val, div->shift, div->width);
+	reg_div = bitfield_extract(reg_val, div->u.s.shift, div->u.s.width);
 
 	/* Return the scaled divisor value it represents */
 	return scaled_div_value(div, reg_div);
@@ -433,16 +433,17 @@ static int __div_commit(struct ccu_data *ccu, struct bcm_clk_gate *gate,
 	 * state was defined in the device tree, we just find out
 	 * what its current value is rather than updating it.
 	 */
-	if (div->scaled_div == BAD_SCALED_DIV_VALUE) {
-		reg_val = __ccu_read(ccu, div->offset);
-		reg_div = bitfield_extract(reg_val, div->shift, div->width);
-		div->scaled_div = scaled_div_value(div, reg_div);
+	if (div->u.s.scaled_div == BAD_SCALED_DIV_VALUE) {
+		reg_val = __ccu_read(ccu, div->u.s.offset);
+		reg_div = bitfield_extract(reg_val, div->u.s.shift,
+						div->u.s.width);
+		div->u.s.scaled_div = scaled_div_value(div, reg_div);
 
 		return 0;
 	}
 
 	/* Convert the scaled divisor to the value we need to record */
-	reg_div = divider(div, div->scaled_div);
+	reg_div = divider(div, div->u.s.scaled_div);
 
 	/* Clock needs to be enabled before changing the rate */
 	enabled = __is_clk_gate_enabled(ccu, gate);
@@ -452,9 +453,10 @@ static int __div_commit(struct ccu_data *ccu, struct bcm_clk_gate *gate,
 	}
 
 	/* Replace the divider value and record the result */
-	reg_val = __ccu_read(ccu, div->offset);
-	reg_val = bitfield_replace(reg_val, div->shift, div->width, reg_div);
-	__ccu_write(ccu, div->offset, reg_val);
+	reg_val = __ccu_read(ccu, div->u.s.offset);
+	reg_val = bitfield_replace(reg_val, div->u.s.shift, div->u.s.width,
+					reg_div);
+	__ccu_write(ccu, div->u.s.offset, reg_val);
 
 	/* If the trigger fails we still want to disable the gate */
 	if (!__clk_trigger(ccu, trig))
@@ -490,11 +492,11 @@ static int divider_write(struct ccu_data *ccu, struct bcm_clk_gate *gate,
 
 	BUG_ON(divider_is_fixed(div));
 
-	previous = div->scaled_div;
+	previous = div->u.s.scaled_div;
 	if (previous == scaled_div)
 		return 0;	/* No change */
 
-	div->scaled_div = scaled_div;
+	div->u.s.scaled_div = scaled_div;
 
 	flags = ccu_lock(ccu);
 	__ccu_write_enable(ccu);
@@ -505,7 +507,7 @@ static int divider_write(struct ccu_data *ccu, struct bcm_clk_gate *gate,
 	ccu_unlock(ccu, flags);
 
 	if (ret)
-		div->scaled_div = previous;		/* Revert the change */
+		div->u.s.scaled_div = previous;		/* Revert the change */
 
 	return ret;
 
@@ -802,7 +804,7 @@ static int selector_write(struct ccu_data *ccu, struct bcm_clk_gate *gate,
 static int kona_peri_clk_enable(struct clk_hw *hw)
 {
 	struct kona_clk *bcm_clk = to_kona_clk(hw);
-	struct bcm_clk_gate *gate = &bcm_clk->peri->gate;
+	struct bcm_clk_gate *gate = &bcm_clk->u.peri->gate;
 
 	return clk_gate(bcm_clk->ccu, bcm_clk->name, gate, true);
 }
@@ -810,7 +812,7 @@ static int kona_peri_clk_enable(struct clk_hw *hw)
 static void kona_peri_clk_disable(struct clk_hw *hw)
 {
 	struct kona_clk *bcm_clk = to_kona_clk(hw);
-	struct bcm_clk_gate *gate = &bcm_clk->peri->gate;
+	struct bcm_clk_gate *gate = &bcm_clk->u.peri->gate;
 
 	(void)clk_gate(bcm_clk->ccu, bcm_clk->name, gate, false);
 }
@@ -818,7 +820,7 @@ static void kona_peri_clk_disable(struct clk_hw *hw)
 static int kona_peri_clk_is_enabled(struct clk_hw *hw)
 {
 	struct kona_clk *bcm_clk = to_kona_clk(hw);
-	struct bcm_clk_gate *gate = &bcm_clk->peri->gate;
+	struct bcm_clk_gate *gate = &bcm_clk->u.peri->gate;
 
 	return is_clk_gate_enabled(bcm_clk->ccu, gate) ? 1 : 0;
 }
@@ -827,7 +829,7 @@ static unsigned long kona_peri_clk_recalc_rate(struct clk_hw *hw,
 			unsigned long parent_rate)
 {
 	struct kona_clk *bcm_clk = to_kona_clk(hw);
-	struct peri_clk_data *data = bcm_clk->peri;
+	struct peri_clk_data *data = bcm_clk->u.peri;
 
 	return clk_recalc_rate(bcm_clk->ccu, &data->div, &data->pre_div,
 				parent_rate);
@@ -837,20 +839,20 @@ static long kona_peri_clk_round_rate(struct clk_hw *hw, unsigned long rate,
 			unsigned long *parent_rate)
 {
 	struct kona_clk *bcm_clk = to_kona_clk(hw);
-	struct bcm_clk_div *div = &bcm_clk->peri->div;
+	struct bcm_clk_div *div = &bcm_clk->u.peri->div;
 
 	if (!divider_exists(div))
 		return __clk_get_rate(hw->clk);
 
 	/* Quietly avoid a zero rate */
-	return round_rate(bcm_clk->ccu, div, &bcm_clk->peri->pre_div,
+	return round_rate(bcm_clk->ccu, div, &bcm_clk->u.peri->pre_div,
 				rate ? rate : 1, *parent_rate, NULL);
 }
 
 static int kona_peri_clk_set_parent(struct clk_hw *hw, u8 index)
 {
 	struct kona_clk *bcm_clk = to_kona_clk(hw);
-	struct peri_clk_data *data = bcm_clk->peri;
+	struct peri_clk_data *data = bcm_clk->u.peri;
 	struct bcm_clk_sel *sel = &data->sel;
 	struct bcm_clk_trig *trig;
 	int ret;
@@ -884,7 +886,7 @@ static int kona_peri_clk_set_parent(struct clk_hw *hw, u8 index)
 static u8 kona_peri_clk_get_parent(struct clk_hw *hw)
 {
 	struct kona_clk *bcm_clk = to_kona_clk(hw);
-	struct peri_clk_data *data = bcm_clk->peri;
+	struct peri_clk_data *data = bcm_clk->u.peri;
 	u8 index;
 
 	index = selector_read_index(bcm_clk->ccu, &data->sel);
@@ -897,7 +899,7 @@ static int kona_peri_clk_set_rate(struct clk_hw *hw, unsigned long rate,
 			unsigned long parent_rate)
 {
 	struct kona_clk *bcm_clk = to_kona_clk(hw);
-	struct peri_clk_data *data = bcm_clk->peri;
+	struct peri_clk_data *data = bcm_clk->u.peri;
 	struct bcm_clk_div *div = &data->div;
 	u64 scaled_div = 0;
 	int ret;
@@ -958,7 +960,7 @@ struct clk_ops kona_peri_clk_ops = {
 static bool __peri_clk_init(struct kona_clk *bcm_clk)
 {
 	struct ccu_data *ccu = bcm_clk->ccu;
-	struct peri_clk_data *peri = bcm_clk->peri;
+	struct peri_clk_data *peri = bcm_clk->u.peri;
 	const char *name = bcm_clk->name;
 	struct bcm_clk_trig *trig;
 
diff --git a/drivers/clk/bcm/clk-kona.h b/drivers/clk/bcm/clk-kona.h
index 5e139adc3dc..dee690951bb 100644
--- a/drivers/clk/bcm/clk-kona.h
+++ b/drivers/clk/bcm/clk-kona.h
@@ -57,7 +57,7 @@
 #define divider_exists(div)		FLAG_TEST(div, DIV, EXISTS)
 #define divider_is_fixed(div)		FLAG_TEST(div, DIV, FIXED)
 #define divider_has_fraction(div)	(!divider_is_fixed(div) && \
-						(div)->frac_width > 0)
+						(div)->u.s.frac_width > 0)
 
 #define selector_exists(sel)		((sel)->width != 0)
 #define trigger_exists(trig)		FLAG_TEST(trig, TRIG, EXISTS)
@@ -244,9 +244,9 @@ struct bcm_clk_div {
 			u32 frac_width;	/* field fraction width */
 
 			u64 scaled_div;	/* scaled divider value */
-		};
+		} s;
 		u32 fixed;	/* non-zero fixed divider value */
-	};
+	} u;
 	u32 flags;		/* BCM_CLK_DIV_FLAGS_* below */
 };
 
@@ -263,28 +263,28 @@ struct bcm_clk_div {
 /* A fixed (non-zero) divider */
 #define FIXED_DIVIDER(_value)						\
 	{								\
-		.fixed = (_value),					\
+		.u.fixed = (_value),					\
 		.flags = FLAG(DIV, EXISTS)|FLAG(DIV, FIXED),		\
 	}
 
 /* A divider with an integral divisor */
 #define DIVIDER(_offset, _shift, _width)				\
 	{								\
-		.offset = (_offset),					\
-		.shift = (_shift),					\
-		.width = (_width),					\
-		.scaled_div = BAD_SCALED_DIV_VALUE,			\
+		.u.s.offset = (_offset),				\
+		.u.s.shift = (_shift),					\
+		.u.s.width = (_width),					\
+		.u.s.scaled_div = BAD_SCALED_DIV_VALUE,			\
 		.flags = FLAG(DIV, EXISTS),				\
 	}
 
 /* A divider whose divisor has an integer and fractional part */
 #define FRAC_DIVIDER(_offset, _shift, _width, _frac_width)		\
 	{								\
-		.offset = (_offset),					\
-		.shift = (_shift),					\
-		.width = (_width),					\
-		.frac_width = (_frac_width),				\
-		.scaled_div = BAD_SCALED_DIV_VALUE,			\
+		.u.s.offset = (_offset),				\
+		.u.s.shift = (_shift),					\
+		.u.s.width = (_width),					\
+		.u.s.frac_width = (_frac_width),			\
+		.u.s.scaled_div = BAD_SCALED_DIV_VALUE,			\
 		.flags = FLAG(DIV, EXISTS),				\
 	}
 
@@ -380,7 +380,7 @@ struct kona_clk {
 	union {
 		void *data;
 		struct peri_clk_data *peri;
-	};
+	} u;
 };
 #define to_kona_clk(_hw) \
 	container_of(_hw, struct kona_clk, hw)
diff --git a/drivers/clk/clk-divider.c b/drivers/clk/clk-divider.c
index ec22112e569..3fbee454022 100644
--- a/drivers/clk/clk-divider.c
+++ b/drivers/clk/clk-divider.c
@@ -144,6 +144,37 @@ static bool _is_valid_div(struct clk_divider *divider, unsigned int div)
 	return true;
 }
 
+static int _round_up_table(const struct clk_div_table *table, int div)
+{
+	const struct clk_div_table *clkt;
+	int up = INT_MAX;
+
+	for (clkt = table; clkt->div; clkt++) {
+		if (clkt->div == div)
+			return clkt->div;
+		else if (clkt->div < div)
+			continue;
+
+		if ((clkt->div - div) < (up - div))
+			up = clkt->div;
+	}
+
+	return up;
+}
+
+static int _div_round_up(struct clk_divider *divider,
+		unsigned long parent_rate, unsigned long rate)
+{
+	int div = DIV_ROUND_UP(parent_rate, rate);
+
+	if (divider->flags & CLK_DIVIDER_POWER_OF_TWO)
+		div = __roundup_pow_of_two(div);
+	if (divider->table)
+		div = _round_up_table(divider->table, div);
+
+	return div;
+}
+
 static int clk_divider_bestdiv(struct clk_hw *hw, unsigned long rate,
 		unsigned long *best_parent_rate)
 {
@@ -159,7 +190,7 @@ static int clk_divider_bestdiv(struct clk_hw *hw, unsigned long rate,
 
 	if (!(__clk_get_flags(hw->clk) & CLK_SET_RATE_PARENT)) {
 		parent_rate = *best_parent_rate;
-		bestdiv = DIV_ROUND_UP(parent_rate, rate);
+		bestdiv = _div_round_up(divider, parent_rate, rate);
 		bestdiv = bestdiv == 0 ? 1 : bestdiv;
 		bestdiv = bestdiv > maxdiv ? maxdiv : bestdiv;
 		return bestdiv;
@@ -219,6 +250,10 @@ static int clk_divider_set_rate(struct clk_hw *hw, unsigned long rate,
 	u32 val;
 
 	div = DIV_ROUND_UP(parent_rate, rate);
+
+	if (!_is_valid_div(divider, div))
+		return -EINVAL;
+
 	value = _get_val(divider, div);
 
 	if (value > div_mask(divider))
diff --git a/drivers/clk/clk.c b/drivers/clk/clk.c
index dff0373f53c..7cf2c093cc5 100644
--- a/drivers/clk/clk.c
+++ b/drivers/clk/clk.c
@@ -1984,9 +1984,28 @@ struct clk *__clk_register(struct device *dev, struct clk_hw *hw)
 }
 EXPORT_SYMBOL_GPL(__clk_register);
 
-static int _clk_register(struct device *dev, struct clk_hw *hw, struct clk *clk)
+/**
+ * clk_register - allocate a new clock, register it and return an opaque cookie
+ * @dev: device that is registering this clock
+ * @hw: link to hardware-specific clock data
+ *
+ * clk_register is the primary interface for populating the clock tree with new
+ * clock nodes.  It returns a pointer to the newly allocated struct clk which
+ * cannot be dereferenced by driver code but may be used in conjuction with the
+ * rest of the clock API.  In the event of an error clk_register will return an
+ * error code; drivers must test for an error code after calling clk_register.
+ */
+struct clk *clk_register(struct device *dev, struct clk_hw *hw)
 {
 	int i, ret;
+	struct clk *clk;
+
+	clk = kzalloc(sizeof(*clk), GFP_KERNEL);
+	if (!clk) {
+		pr_err("%s: could not allocate clk\n", __func__);
+		ret = -ENOMEM;
+		goto fail_out;
+	}
 
 	clk->name = kstrdup(hw->init->name, GFP_KERNEL);
 	if (!clk->name) {
@@ -2026,7 +2045,7 @@ static int _clk_register(struct device *dev, struct clk_hw *hw, struct clk *clk)
 
 	ret = __clk_init(dev, clk);
 	if (!ret)
-		return 0;
+		return clk;
 
 fail_parent_names_copy:
 	while (--i >= 0)
@@ -2035,36 +2054,6 @@ fail_parent_names_copy:
 fail_parent_names:
 	kfree(clk->name);
 fail_name:
-	return ret;
-}
-
-/**
- * clk_register - allocate a new clock, register it and return an opaque cookie
- * @dev: device that is registering this clock
- * @hw: link to hardware-specific clock data
- *
- * clk_register is the primary interface for populating the clock tree with new
- * clock nodes.  It returns a pointer to the newly allocated struct clk which
- * cannot be dereferenced by driver code but may be used in conjuction with the
- * rest of the clock API.  In the event of an error clk_register will return an
- * error code; drivers must test for an error code after calling clk_register.
- */
-struct clk *clk_register(struct device *dev, struct clk_hw *hw)
-{
-	int ret;
-	struct clk *clk;
-
-	clk = kzalloc(sizeof(*clk), GFP_KERNEL);
-	if (!clk) {
-		pr_err("%s: could not allocate clk\n", __func__);
-		ret = -ENOMEM;
-		goto fail_out;
-	}
-
-	ret = _clk_register(dev, hw, clk);
-	if (!ret)
-		return clk;
-
 	kfree(clk);
 fail_out:
 	return ERR_PTR(ret);
@@ -2151,9 +2140,10 @@ void clk_unregister(struct clk *clk)
 
 	if (!hlist_empty(&clk->children)) {
 		struct clk *child;
+		struct hlist_node *t;
 
 		/* Reparent all children to the orphan list. */
-		hlist_for_each_entry(child, &clk->children, child_node)
+		hlist_for_each_entry_safe(child, t, &clk->children, child_node)
 			clk_set_parent(child, NULL);
 	}
 
@@ -2173,7 +2163,7 @@ EXPORT_SYMBOL_GPL(clk_unregister);
 
 static void devm_clk_release(struct device *dev, void *res)
 {
-	clk_unregister(res);
+	clk_unregister(*(struct clk **)res);
 }
 
 /**
@@ -2188,18 +2178,18 @@ static void devm_clk_release(struct device *dev, void *res)
 struct clk *devm_clk_register(struct device *dev, struct clk_hw *hw)
 {
 	struct clk *clk;
-	int ret;
+	struct clk **clkp;
 
-	clk = devres_alloc(devm_clk_release, sizeof(*clk), GFP_KERNEL);
-	if (!clk)
+	clkp = devres_alloc(devm_clk_release, sizeof(*clkp), GFP_KERNEL);
+	if (!clkp)
 		return ERR_PTR(-ENOMEM);
 
-	ret = _clk_register(dev, hw, clk);
-	if (!ret) {
-		devres_add(dev, clk);
+	clk = clk_register(dev, hw);
+	if (!IS_ERR(clk)) {
+		*clkp = clk;
+		devres_add(dev, clkp);
 	} else {
-		devres_free(clk);
-		clk = ERR_PTR(ret);
+		devres_free(clkp);
 	}
 
 	return clk;
diff --git a/drivers/clk/samsung/Kconfig b/drivers/clk/samsung/Kconfig
new file mode 100644
index 00000000000..84196ecdaa1
--- /dev/null
+++ b/drivers/clk/samsung/Kconfig
@@ -0,0 +1,26 @@
+config COMMON_CLK_SAMSUNG
+	bool
+	select COMMON_CLK
+
+config S3C2410_COMMON_CLK
+	bool
+	select COMMON_CLK_SAMSUNG
+	help
+	  Build the s3c2410 clock driver based on the common clock framework.
+
+config S3C2410_COMMON_DCLK
+	bool
+	select COMMON_CLK_SAMSUNG
+	select REGMAP_MMIO
+	help
+	  Temporary symbol to build the dclk driver based on the common clock
+	  framework.
+
+config S3C2412_COMMON_CLK
+	bool
+	select COMMON_CLK_SAMSUNG
+
+config S3C2443_COMMON_CLK
+	bool
+	select COMMON_CLK_SAMSUNG
+
diff --git a/drivers/clk/samsung/Makefile b/drivers/clk/samsung/Makefile
index 8eb4799237f..69e81773164 100644
--- a/drivers/clk/samsung/Makefile
+++ b/drivers/clk/samsung/Makefile
@@ -3,9 +3,16 @@
 #
 
 obj-$(CONFIG_COMMON_CLK)	+= clk.o clk-pll.o
+obj-$(CONFIG_SOC_EXYNOS3250)	+= clk-exynos3250.o
 obj-$(CONFIG_ARCH_EXYNOS4)	+= clk-exynos4.o
 obj-$(CONFIG_SOC_EXYNOS5250)	+= clk-exynos5250.o
+obj-$(CONFIG_SOC_EXYNOS5260)	+= clk-exynos5260.o
+obj-$(CONFIG_SOC_EXYNOS5410)	+= clk-exynos5410.o
 obj-$(CONFIG_SOC_EXYNOS5420)	+= clk-exynos5420.o
 obj-$(CONFIG_SOC_EXYNOS5440)	+= clk-exynos5440.o
 obj-$(CONFIG_ARCH_EXYNOS)	+= clk-exynos-audss.o
+obj-$(CONFIG_S3C2410_COMMON_CLK)+= clk-s3c2410.o
+obj-$(CONFIG_S3C2410_COMMON_DCLK)+= clk-s3c2410-dclk.o
+obj-$(CONFIG_S3C2412_COMMON_CLK)+= clk-s3c2412.o
+obj-$(CONFIG_S3C2443_COMMON_CLK)+= clk-s3c2443.o
 obj-$(CONFIG_ARCH_S3C64XX)	+= clk-s3c64xx.o
diff --git a/drivers/clk/samsung/clk-exynos3250.c b/drivers/clk/samsung/clk-exynos3250.c
new file mode 100644
index 00000000000..7a17bd40d1d
--- /dev/null
+++ b/drivers/clk/samsung/clk-exynos3250.c
@@ -0,0 +1,780 @@
+/*
+ * Copyright (c) 2014 Samsung Electronics Co., Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * Common Clock Framework support for Exynos3250 SoC.
+ */
+
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/platform_device.h>
+#include <linux/syscore_ops.h>
+
+#include <dt-bindings/clock/exynos3250.h>
+
+#include "clk.h"
+#include "clk-pll.h"
+
+#define SRC_LEFTBUS		0x4200
+#define DIV_LEFTBUS		0x4500
+#define GATE_IP_LEFTBUS		0x4800
+#define SRC_RIGHTBUS		0x8200
+#define DIV_RIGHTBUS		0x8500
+#define GATE_IP_RIGHTBUS	0x8800
+#define GATE_IP_PERIR		0x8960
+#define MPLL_LOCK		0xc010
+#define MPLL_CON0		0xc110
+#define VPLL_LOCK		0xc020
+#define VPLL_CON0		0xc120
+#define UPLL_LOCK		0xc030
+#define UPLL_CON0		0xc130
+#define SRC_TOP0		0xc210
+#define SRC_TOP1		0xc214
+#define SRC_CAM			0xc220
+#define SRC_MFC			0xc228
+#define SRC_G3D			0xc22c
+#define SRC_LCD			0xc234
+#define SRC_ISP			0xc238
+#define SRC_FSYS		0xc240
+#define SRC_PERIL0		0xc250
+#define SRC_PERIL1		0xc254
+#define SRC_MASK_TOP		0xc310
+#define SRC_MASK_CAM		0xc320
+#define SRC_MASK_LCD		0xc334
+#define SRC_MASK_ISP		0xc338
+#define SRC_MASK_FSYS		0xc340
+#define SRC_MASK_PERIL0		0xc350
+#define SRC_MASK_PERIL1		0xc354
+#define DIV_TOP			0xc510
+#define DIV_CAM			0xc520
+#define DIV_MFC			0xc528
+#define DIV_G3D			0xc52c
+#define DIV_LCD			0xc534
+#define DIV_ISP			0xc538
+#define DIV_FSYS0		0xc540
+#define DIV_FSYS1		0xc544
+#define DIV_FSYS2		0xc548
+#define DIV_PERIL0		0xc550
+#define DIV_PERIL1		0xc554
+#define DIV_PERIL3		0xc55c
+#define DIV_PERIL4		0xc560
+#define DIV_PERIL5		0xc564
+#define DIV_CAM1		0xc568
+#define CLKDIV2_RATIO		0xc580
+#define GATE_SCLK_CAM		0xc820
+#define GATE_SCLK_MFC		0xc828
+#define GATE_SCLK_G3D		0xc82c
+#define GATE_SCLK_LCD		0xc834
+#define GATE_SCLK_ISP_TOP	0xc838
+#define GATE_SCLK_FSYS		0xc840
+#define GATE_SCLK_PERIL		0xc850
+#define GATE_IP_CAM		0xc920
+#define GATE_IP_MFC		0xc928
+#define GATE_IP_G3D		0xc92c
+#define GATE_IP_LCD		0xc934
+#define GATE_IP_ISP		0xc938
+#define GATE_IP_FSYS		0xc940
+#define GATE_IP_PERIL		0xc950
+#define GATE_BLOCK		0xc970
+#define APLL_LOCK		0x14000
+#define APLL_CON0		0x14100
+#define SRC_CPU			0x14200
+#define DIV_CPU0		0x14500
+#define DIV_CPU1		0x14504
+
+/* list of PLLs to be registered */
+enum exynos3250_plls {
+	apll, mpll, vpll, upll,
+	nr_plls
+};
+
+static void __iomem *reg_base;
+
+/*
+ * Support for CMU save/restore across system suspends
+ */
+#ifdef CONFIG_PM_SLEEP
+static struct samsung_clk_reg_dump *exynos3250_clk_regs;
+
+static unsigned long exynos3250_cmu_clk_regs[] __initdata = {
+	SRC_LEFTBUS,
+	DIV_LEFTBUS,
+	GATE_IP_LEFTBUS,
+	SRC_RIGHTBUS,
+	DIV_RIGHTBUS,
+	GATE_IP_RIGHTBUS,
+	GATE_IP_PERIR,
+	MPLL_LOCK,
+	MPLL_CON0,
+	VPLL_LOCK,
+	VPLL_CON0,
+	UPLL_LOCK,
+	UPLL_CON0,
+	SRC_TOP0,
+	SRC_TOP1,
+	SRC_CAM,
+	SRC_MFC,
+	SRC_G3D,
+	SRC_LCD,
+	SRC_ISP,
+	SRC_FSYS,
+	SRC_PERIL0,
+	SRC_PERIL1,
+	SRC_MASK_TOP,
+	SRC_MASK_CAM,
+	SRC_MASK_LCD,
+	SRC_MASK_ISP,
+	SRC_MASK_FSYS,
+	SRC_MASK_PERIL0,
+	SRC_MASK_PERIL1,
+	DIV_TOP,
+	DIV_CAM,
+	DIV_MFC,
+	DIV_G3D,
+	DIV_LCD,
+	DIV_ISP,
+	DIV_FSYS0,
+	DIV_FSYS1,
+	DIV_FSYS2,
+	DIV_PERIL0,
+	DIV_PERIL1,
+	DIV_PERIL3,
+	DIV_PERIL4,
+	DIV_PERIL5,
+	DIV_CAM1,
+	CLKDIV2_RATIO,
+	GATE_SCLK_CAM,
+	GATE_SCLK_MFC,
+	GATE_SCLK_G3D,
+	GATE_SCLK_LCD,
+	GATE_SCLK_ISP_TOP,
+	GATE_SCLK_FSYS,
+	GATE_SCLK_PERIL,
+	GATE_IP_CAM,
+	GATE_IP_MFC,
+	GATE_IP_G3D,
+	GATE_IP_LCD,
+	GATE_IP_ISP,
+	GATE_IP_FSYS,
+	GATE_IP_PERIL,
+	GATE_BLOCK,
+	APLL_LOCK,
+	SRC_CPU,
+	DIV_CPU0,
+	DIV_CPU1,
+};
+
+static int exynos3250_clk_suspend(void)
+{
+	samsung_clk_save(reg_base, exynos3250_clk_regs,
+				ARRAY_SIZE(exynos3250_cmu_clk_regs));
+	return 0;
+}
+
+static void exynos3250_clk_resume(void)
+{
+	samsung_clk_restore(reg_base, exynos3250_clk_regs,
+				ARRAY_SIZE(exynos3250_cmu_clk_regs));
+}
+
+static struct syscore_ops exynos3250_clk_syscore_ops = {
+	.suspend = exynos3250_clk_suspend,
+	.resume = exynos3250_clk_resume,
+};
+
+static void exynos3250_clk_sleep_init(void)
+{
+	exynos3250_clk_regs =
+		samsung_clk_alloc_reg_dump(exynos3250_cmu_clk_regs,
+					   ARRAY_SIZE(exynos3250_cmu_clk_regs));
+	if (!exynos3250_clk_regs) {
+		pr_warn("%s: Failed to allocate sleep save data\n", __func__);
+		goto err;
+	}
+
+	register_syscore_ops(&exynos3250_clk_syscore_ops);
+	return;
+err:
+	kfree(exynos3250_clk_regs);
+}
+#else
+static inline void exynos3250_clk_sleep_init(void) { }
+#endif
+
+/* list of all parent clock list */
+PNAME(mout_vpllsrc_p)		= { "fin_pll", };
+
+PNAME(mout_apll_p)		= { "fin_pll", "fout_apll", };
+PNAME(mout_mpll_p)		= { "fin_pll", "fout_mpll", };
+PNAME(mout_vpll_p)		= { "fin_pll", "fout_vpll", };
+PNAME(mout_upll_p)		= { "fin_pll", "fout_upll", };
+
+PNAME(mout_mpll_user_p)		= { "fin_pll", "div_mpll_pre", };
+PNAME(mout_epll_user_p)		= { "fin_pll", "mout_epll", };
+PNAME(mout_core_p)		= { "mout_apll", "mout_mpll_user_c", };
+PNAME(mout_hpm_p)		= { "mout_apll", "mout_mpll_user_c", };
+
+PNAME(mout_ebi_p)		= { "div_aclk_200", "div_aclk_160", };
+PNAME(mout_ebi_1_p)		= { "mout_ebi", "mout_vpll", };
+
+PNAME(mout_gdl_p)		= { "mout_mpll_user_l", };
+PNAME(mout_gdr_p)		= { "mout_mpll_user_r", };
+
+PNAME(mout_aclk_400_mcuisp_sub_p)
+				= { "fin_pll", "div_aclk_400_mcuisp", };
+PNAME(mout_aclk_266_0_p)	= { "div_mpll_pre", "mout_vpll", };
+PNAME(mout_aclk_266_1_p)	= { "mout_epll_user", };
+PNAME(mout_aclk_266_p)		= { "mout_aclk_266_0", "mout_aclk_266_1", };
+PNAME(mout_aclk_266_sub_p)	= { "fin_pll", "div_aclk_266", };
+
+PNAME(group_div_mpll_pre_p)	= { "div_mpll_pre", };
+PNAME(group_epll_vpll_p)	= { "mout_epll_user", "mout_vpll" };
+PNAME(group_sclk_p)		= { "xxti", "xusbxti",
+				    "none", "none",
+				    "none", "none", "div_mpll_pre",
+				    "mout_epll_user", "mout_vpll", };
+PNAME(group_sclk_audio_p)	= { "audiocdclk", "none",
+				    "none", "none",
+				    "xxti", "xusbxti",
+				    "div_mpll_pre", "mout_epll_user",
+				    "mout_vpll", };
+PNAME(group_sclk_cam_blk_p)	= { "xxti", "xusbxti",
+				    "none", "none", "none",
+				    "none", "div_mpll_pre",
+				    "mout_epll_user", "mout_vpll",
+				    "div_cam_blk_320", };
+PNAME(group_sclk_fimd0_p)	= { "xxti", "xusbxti",
+				    "m_bitclkhsdiv4_2l", "none",
+				    "none", "none", "div_mpll_pre",
+				    "mout_epll_user", "mout_vpll",
+				    "none", "none", "none",
+				    "div_lcd_blk_145", };
+
+PNAME(mout_mfc_p)		= { "mout_mfc_0", "mout_mfc_1" };
+PNAME(mout_g3d_p)		= { "mout_g3d_0", "mout_g3d_1" };
+
+static struct samsung_fixed_factor_clock fixed_factor_clks[] __initdata = {
+	FFACTOR(0, "sclk_mpll_1600", "mout_mpll", 1, 1, 0),
+	FFACTOR(0, "sclk_mpll_mif", "mout_mpll", 1, 2, 0),
+	FFACTOR(0, "sclk_bpll", "fout_bpll", 1, 2, 0),
+	FFACTOR(0, "div_cam_blk_320", "sclk_mpll_1600", 1, 5, 0),
+	FFACTOR(0, "div_lcd_blk_145", "sclk_mpll_1600", 1, 11, 0),
+
+	/* HACK: fin_pll hardcoded to xusbxti until detection is implemented. */
+	FFACTOR(CLK_FIN_PLL, "fin_pll", "xusbxti", 1, 1, 0),
+};
+
+static struct samsung_mux_clock mux_clks[] __initdata = {
+	/*
+	 * NOTE: Following table is sorted by register address in ascending
+	 * order and then bitfield shift in descending order, as it is done
+	 * in the User's Manual. When adding new entries, please make sure
+	 * that the order is preserved, to avoid merge conflicts and make
+	 * further work with defined data easier.
+	 */
+
+	/* SRC_LEFTBUS */
+	MUX(CLK_MOUT_MPLL_USER_L, "mout_mpll_user_l", mout_mpll_user_p,
+	    SRC_LEFTBUS, 4, 1),
+	MUX(CLK_MOUT_GDL, "mout_gdl", mout_gdl_p, SRC_LEFTBUS, 0, 1),
+
+	/* SRC_RIGHTBUS */
+	MUX(CLK_MOUT_MPLL_USER_R, "mout_mpll_user_r", mout_mpll_user_p,
+	    SRC_RIGHTBUS, 4, 1),
+	MUX(CLK_MOUT_GDR, "mout_gdr", mout_gdr_p, SRC_RIGHTBUS, 0, 1),
+
+	/* SRC_TOP0 */
+	MUX(CLK_MOUT_EBI, "mout_ebi", mout_ebi_p, SRC_TOP0, 28, 1),
+	MUX(CLK_MOUT_ACLK_200, "mout_aclk_200", group_div_mpll_pre_p,SRC_TOP0, 24, 1),
+	MUX(CLK_MOUT_ACLK_160, "mout_aclk_160", group_div_mpll_pre_p, SRC_TOP0, 20, 1),
+	MUX(CLK_MOUT_ACLK_100, "mout_aclk_100", group_div_mpll_pre_p, SRC_TOP0, 16, 1),
+	MUX(CLK_MOUT_ACLK_266_1, "mout_aclk_266_1", mout_aclk_266_1_p, SRC_TOP0, 14, 1),
+	MUX(CLK_MOUT_ACLK_266_0, "mout_aclk_266_0", mout_aclk_266_0_p, SRC_TOP0, 13, 1),
+	MUX(CLK_MOUT_ACLK_266, "mout_aclk_266", mout_aclk_266_p, SRC_TOP0, 12, 1),
+	MUX(CLK_MOUT_VPLL, "mout_vpll", mout_vpll_p, SRC_TOP0, 8, 1),
+	MUX(CLK_MOUT_EPLL_USER, "mout_epll_user", mout_epll_user_p, SRC_TOP0, 4, 1),
+	MUX(CLK_MOUT_EBI_1, "mout_ebi_1", mout_ebi_1_p, SRC_TOP0, 0, 1),
+
+	/* SRC_TOP1 */
+	MUX(CLK_MOUT_UPLL, "mout_upll", mout_upll_p, SRC_TOP1, 28, 1),
+	MUX(CLK_MOUT_ACLK_400_MCUISP_SUB, "mout_aclk_400_mcuisp_sub", mout_aclk_400_mcuisp_sub_p,
+		SRC_TOP1, 24, 1),
+	MUX(CLK_MOUT_ACLK_266_SUB, "mout_aclk_266_sub", mout_aclk_266_sub_p, SRC_TOP1, 20, 1),
+	MUX(CLK_MOUT_MPLL, "mout_mpll", mout_mpll_p, SRC_TOP1, 12, 1),
+	MUX(CLK_MOUT_ACLK_400_MCUISP, "mout_aclk_400_mcuisp", group_div_mpll_pre_p, SRC_TOP1, 8, 1),
+	MUX(CLK_MOUT_VPLLSRC, "mout_vpllsrc", mout_vpllsrc_p, SRC_TOP1, 0, 1),
+
+	/* SRC_CAM */
+	MUX(CLK_MOUT_CAM1, "mout_cam1", group_sclk_p, SRC_CAM, 20, 4),
+	MUX(CLK_MOUT_CAM_BLK, "mout_cam_blk", group_sclk_cam_blk_p, SRC_CAM, 0, 4),
+
+	/* SRC_MFC */
+	MUX(CLK_MOUT_MFC, "mout_mfc", mout_mfc_p, SRC_MFC, 8, 1),
+	MUX(CLK_MOUT_MFC_1, "mout_mfc_1", group_epll_vpll_p, SRC_MFC, 4, 1),
+	MUX(CLK_MOUT_MFC_0, "mout_mfc_0", group_div_mpll_pre_p, SRC_MFC, 0, 1),
+
+	/* SRC_G3D */
+	MUX(CLK_MOUT_G3D, "mout_g3d", mout_g3d_p, SRC_G3D, 8, 1),
+	MUX(CLK_MOUT_G3D_1, "mout_g3d_1", group_epll_vpll_p, SRC_G3D, 4, 1),
+	MUX(CLK_MOUT_G3D_0, "mout_g3d_0", group_div_mpll_pre_p, SRC_G3D, 0, 1),
+
+	/* SRC_LCD */
+	MUX(CLK_MOUT_MIPI0, "mout_mipi0", group_sclk_p, SRC_LCD, 12, 4),
+	MUX(CLK_MOUT_FIMD0, "mout_fimd0", group_sclk_fimd0_p, SRC_LCD, 0, 4),
+
+	/* SRC_ISP */
+	MUX(CLK_MOUT_UART_ISP, "mout_uart_isp", group_sclk_p, SRC_ISP, 12, 4),
+	MUX(CLK_MOUT_SPI1_ISP, "mout_spi1_isp", group_sclk_p, SRC_ISP, 8, 4),
+	MUX(CLK_MOUT_SPI0_ISP, "mout_spi0_isp", group_sclk_p, SRC_ISP, 4, 4),
+
+	/* SRC_FSYS */
+	MUX(CLK_MOUT_TSADC, "mout_tsadc", group_sclk_p, SRC_FSYS, 28, 4),
+	MUX(CLK_MOUT_MMC1, "mout_mmc1", group_sclk_p, SRC_FSYS, 4, 3),
+	MUX(CLK_MOUT_MMC0, "mout_mmc0", group_sclk_p, SRC_FSYS, 0, 3),
+
+	/* SRC_PERIL0 */
+	MUX(CLK_MOUT_UART1, "mout_uart1", group_sclk_p, SRC_PERIL0, 4, 4),
+	MUX(CLK_MOUT_UART0, "mout_uart0", group_sclk_p, SRC_PERIL0, 0, 4),
+
+	/* SRC_PERIL1 */
+	MUX(CLK_MOUT_SPI1, "mout_spi1", group_sclk_p, SRC_PERIL1, 20, 4),
+	MUX(CLK_MOUT_SPI0, "mout_spi0", group_sclk_p, SRC_PERIL1, 16, 4),
+	MUX(CLK_MOUT_AUDIO, "mout_audio", group_sclk_audio_p, SRC_PERIL1, 4, 4),
+
+	/* SRC_CPU */
+	MUX(CLK_MOUT_MPLL_USER_C, "mout_mpll_user_c", mout_mpll_user_p,
+	    SRC_CPU, 24, 1),
+	MUX(CLK_MOUT_HPM, "mout_hpm", mout_hpm_p, SRC_CPU, 20, 1),
+	MUX(CLK_MOUT_CORE, "mout_core", mout_core_p, SRC_CPU, 16, 1),
+	MUX(CLK_MOUT_APLL, "mout_apll", mout_apll_p, SRC_CPU, 0, 1),
+};
+
+static struct samsung_div_clock div_clks[] __initdata = {
+	/*
+	 * NOTE: Following table is sorted by register address in ascending
+	 * order and then bitfield shift in descending order, as it is done
+	 * in the User's Manual. When adding new entries, please make sure
+	 * that the order is preserved, to avoid merge conflicts and make
+	 * further work with defined data easier.
+	 */
+
+	/* DIV_LEFTBUS */
+	DIV(CLK_DIV_GPL, "div_gpl", "div_gdl", DIV_LEFTBUS, 4, 3),
+	DIV(CLK_DIV_GDL, "div_gdl", "mout_gdl", DIV_LEFTBUS, 0, 4),
+
+	/* DIV_RIGHTBUS */
+	DIV(CLK_DIV_GPR, "div_gpr", "div_gdr", DIV_RIGHTBUS, 4, 3),
+	DIV(CLK_DIV_GDR, "div_gdr", "mout_gdr", DIV_RIGHTBUS, 0, 4),
+
+	/* DIV_TOP */
+	DIV(CLK_DIV_MPLL_PRE, "div_mpll_pre", "sclk_mpll_mif", DIV_TOP, 28, 2),
+	DIV(CLK_DIV_ACLK_400_MCUISP, "div_aclk_400_mcuisp",
+	    "mout_aclk_400_mcuisp", DIV_TOP, 24, 3),
+	DIV(CLK_DIV_EBI, "div_ebi", "mout_ebi_1", DIV_TOP, 16, 3),
+	DIV(CLK_DIV_ACLK_200, "div_aclk_200", "mout_aclk_200", DIV_TOP, 12, 3),
+	DIV(CLK_DIV_ACLK_160, "div_aclk_160", "mout_aclk_160", DIV_TOP, 8, 3),
+	DIV(CLK_DIV_ACLK_100, "div_aclk_100", "mout_aclk_100", DIV_TOP, 4, 4),
+	DIV(CLK_DIV_ACLK_266, "div_aclk_266", "mout_aclk_266", DIV_TOP, 0, 3),
+
+	/* DIV_CAM */
+	DIV(CLK_DIV_CAM1, "div_cam1", "mout_cam1", DIV_CAM, 20, 4),
+	DIV(CLK_DIV_CAM_BLK, "div_cam_blk", "mout_cam_blk", DIV_CAM, 0, 4),
+
+	/* DIV_MFC */
+	DIV(CLK_DIV_MFC, "div_mfc", "mout_mfc", DIV_MFC, 0, 4),
+
+	/* DIV_G3D */
+	DIV(CLK_DIV_G3D, "div_g3d", "mout_g3d", DIV_G3D, 0, 4),
+
+	/* DIV_LCD */
+	DIV_F(CLK_DIV_MIPI0_PRE, "div_mipi0_pre", "div_mipi0", DIV_LCD, 20, 4,
+		CLK_SET_RATE_PARENT, 0),
+	DIV(CLK_DIV_MIPI0, "div_mipi0", "mout_mipi0", DIV_LCD, 16, 4),
+	DIV(CLK_DIV_FIMD0, "div_fimd0", "mout_fimd0", DIV_LCD, 0, 4),
+
+	/* DIV_ISP */
+	DIV(CLK_DIV_UART_ISP, "div_uart_isp", "mout_uart_isp", DIV_ISP, 28, 4),
+	DIV_F(CLK_DIV_SPI1_ISP_PRE, "div_spi1_isp_pre", "div_spi1_isp",
+		DIV_ISP, 20, 8, CLK_SET_RATE_PARENT, 0),
+	DIV(CLK_DIV_SPI1_ISP, "div_spi1_isp", "mout_spi1_isp", DIV_ISP, 16, 4),
+	DIV_F(CLK_DIV_SPI0_ISP_PRE, "div_spi0_isp_pre", "div_spi0_isp",
+		DIV_ISP, 8, 8, CLK_SET_RATE_PARENT, 0),
+	DIV(CLK_DIV_SPI0_ISP, "div_spi0_isp", "mout_spi0_isp", DIV_ISP, 0, 4),
+
+	/* DIV_FSYS0 */
+	DIV_F(CLK_DIV_TSADC_PRE, "div_tsadc_pre", "div_tsadc", DIV_FSYS0, 8, 8,
+		CLK_SET_RATE_PARENT, 0),
+	DIV(CLK_DIV_TSADC, "div_tsadc", "mout_tsadc", DIV_FSYS0, 0, 4),
+
+	/* DIV_FSYS1 */
+	DIV_F(CLK_DIV_MMC1_PRE, "div_mmc1_pre", "div_mmc1", DIV_FSYS1, 24, 8,
+		CLK_SET_RATE_PARENT, 0),
+	DIV(CLK_DIV_MMC1, "div_mmc1", "mout_mmc1", DIV_FSYS1, 16, 4),
+	DIV_F(CLK_DIV_MMC0_PRE, "div_mmc0_pre", "div_mmc0", DIV_FSYS1, 8, 8,
+		CLK_SET_RATE_PARENT, 0),
+	DIV(CLK_DIV_MMC0, "div_mmc0", "mout_mmc0", DIV_FSYS1, 0, 4),
+
+	/* DIV_PERIL0 */
+	DIV(CLK_DIV_UART1, "div_uart1", "mout_uart1", DIV_PERIL0, 4, 4),
+	DIV(CLK_DIV_UART0, "div_uart0", "mout_uart0", DIV_PERIL0, 0, 4),
+
+	/* DIV_PERIL1 */
+	DIV_F(CLK_DIV_SPI1_PRE, "div_spi1_pre", "div_spi1", DIV_PERIL1, 24, 8,
+		CLK_SET_RATE_PARENT, 0),
+	DIV(CLK_DIV_SPI1, "div_spi1", "mout_spi1", DIV_PERIL1, 16, 4),
+	DIV_F(CLK_DIV_SPI0_PRE, "div_spi0_pre", "div_spi0", DIV_PERIL1, 8, 8,
+		CLK_SET_RATE_PARENT, 0),
+	DIV(CLK_DIV_SPI0, "div_spi0", "mout_spi0", DIV_PERIL1, 0, 4),
+
+	/* DIV_PERIL4 */
+	DIV(CLK_DIV_PCM, "div_pcm", "div_audio", DIV_PERIL4, 20, 8),
+	DIV(CLK_DIV_AUDIO, "div_audio", "mout_audio", DIV_PERIL4, 16, 4),
+
+	/* DIV_PERIL5 */
+	DIV(CLK_DIV_I2S, "div_i2s", "div_audio", DIV_PERIL5, 8, 6),
+
+	/* DIV_CPU0 */
+	DIV(CLK_DIV_CORE2, "div_core2", "div_core", DIV_CPU0, 28, 3),
+	DIV(CLK_DIV_APLL, "div_apll", "mout_apll", DIV_CPU0, 24, 3),
+	DIV(CLK_DIV_PCLK_DBG, "div_pclk_dbg", "div_core2", DIV_CPU0, 20, 3),
+	DIV(CLK_DIV_ATB, "div_atb", "div_core2", DIV_CPU0, 16, 3),
+	DIV(CLK_DIV_COREM, "div_corem", "div_core2", DIV_CPU0, 4, 3),
+	DIV(CLK_DIV_CORE, "div_core", "mout_core", DIV_CPU0, 0, 3),
+
+	/* DIV_CPU1 */
+	DIV(CLK_DIV_HPM, "div_hpm", "div_copy", DIV_CPU1, 4, 3),
+	DIV(CLK_DIV_COPY, "div_copy", "mout_hpm", DIV_CPU1, 0, 3),
+};
+
+static struct samsung_gate_clock gate_clks[] __initdata = {
+	/*
+	 * NOTE: Following table is sorted by register address in ascending
+	 * order and then bitfield shift in descending order, as it is done
+	 * in the User's Manual. When adding new entries, please make sure
+	 * that the order is preserved, to avoid merge conflicts and make
+	 * further work with defined data easier.
+	 */
+
+	/* GATE_IP_LEFTBUS */
+	GATE(CLK_ASYNC_G3D, "async_g3d", "div_aclk_100", GATE_IP_LEFTBUS, 6,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_ASYNC_MFCL, "async_mfcl", "div_aclk_100", GATE_IP_LEFTBUS, 4,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PPMULEFT, "ppmuleft", "div_aclk_100", GATE_IP_LEFTBUS, 1,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_GPIO_LEFT, "gpio_left", "div_aclk_100", GATE_IP_LEFTBUS, 0,
+		CLK_IGNORE_UNUSED, 0),
+
+	/* GATE_IP_RIGHTBUS */
+	GATE(CLK_ASYNC_ISPMX, "async_ispmx", "div_aclk_100",
+		GATE_IP_RIGHTBUS, 9, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_ASYNC_FSYSD, "async_fsysd", "div_aclk_100",
+		GATE_IP_RIGHTBUS, 5, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_ASYNC_LCD0X, "async_lcd0x", "div_aclk_100",
+		GATE_IP_RIGHTBUS, 3, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_ASYNC_CAMX, "async_camx", "div_aclk_100", GATE_IP_RIGHTBUS, 2,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PPMURIGHT, "ppmuright", "div_aclk_100", GATE_IP_RIGHTBUS, 1,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_GPIO_RIGHT, "gpio_right", "div_aclk_100", GATE_IP_RIGHTBUS, 0,
+		CLK_IGNORE_UNUSED, 0),
+
+	/* GATE_IP_PERIR */
+	GATE(CLK_MONOCNT, "monocnt", "div_aclk_100", GATE_IP_PERIR, 22,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_TZPC6, "tzpc6", "div_aclk_100", GATE_IP_PERIR, 21,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PROVISIONKEY1, "provisionkey1", "div_aclk_100",
+		GATE_IP_PERIR, 20, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PROVISIONKEY0, "provisionkey0", "div_aclk_100",
+		GATE_IP_PERIR, 19, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_CMU_ISPPART, "cmu_isppart", "div_aclk_100", GATE_IP_PERIR, 18,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_TMU_APBIF, "tmu_apbif", "div_aclk_100",
+		GATE_IP_PERIR, 17, 0, 0),
+	GATE(CLK_KEYIF, "keyif", "div_aclk_100", GATE_IP_PERIR, 16, 0, 0),
+	GATE(CLK_RTC, "rtc", "div_aclk_100", GATE_IP_PERIR, 15, 0, 0),
+	GATE(CLK_WDT, "wdt", "div_aclk_100", GATE_IP_PERIR, 14, 0, 0),
+	GATE(CLK_MCT, "mct", "div_aclk_100", GATE_IP_PERIR, 13, 0, 0),
+	GATE(CLK_SECKEY, "seckey", "div_aclk_100", GATE_IP_PERIR, 12,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_TZPC5, "tzpc5", "div_aclk_100", GATE_IP_PERIR, 10,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_TZPC4, "tzpc4", "div_aclk_100", GATE_IP_PERIR, 9,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_TZPC3, "tzpc3", "div_aclk_100", GATE_IP_PERIR, 8,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_TZPC2, "tzpc2", "div_aclk_100", GATE_IP_PERIR, 7,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_TZPC1, "tzpc1", "div_aclk_100", GATE_IP_PERIR, 6,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_TZPC0, "tzpc0", "div_aclk_100", GATE_IP_PERIR, 5,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_CMU_COREPART, "cmu_corepart", "div_aclk_100", GATE_IP_PERIR, 4,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_CMU_TOPPART, "cmu_toppart", "div_aclk_100", GATE_IP_PERIR, 3,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PMU_APBIF, "pmu_apbif", "div_aclk_100", GATE_IP_PERIR, 2,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_SYSREG, "sysreg", "div_aclk_100", GATE_IP_PERIR, 1,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_CHIP_ID, "chip_id", "div_aclk_100", GATE_IP_PERIR, 0,
+		CLK_IGNORE_UNUSED, 0),
+
+	/* GATE_SCLK_CAM */
+	GATE(CLK_SCLK_JPEG, "sclk_jpeg", "div_cam_blk",
+		GATE_SCLK_CAM, 8, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_M2MSCALER, "sclk_m2mscaler", "div_cam_blk",
+		GATE_SCLK_CAM, 2, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_GSCALER1, "sclk_gscaler1", "div_cam_blk",
+		GATE_SCLK_CAM, 1, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_GSCALER0, "sclk_gscaler0", "div_cam_blk",
+		GATE_SCLK_CAM, 0, CLK_SET_RATE_PARENT, 0),
+
+	/* GATE_SCLK_MFC */
+	GATE(CLK_SCLK_MFC, "sclk_mfc", "div_mfc",
+		GATE_SCLK_MFC, 0, CLK_SET_RATE_PARENT, 0),
+
+	/* GATE_SCLK_G3D */
+	GATE(CLK_SCLK_G3D, "sclk_g3d", "div_g3d",
+		GATE_SCLK_G3D, 0, CLK_SET_RATE_PARENT, 0),
+
+	/* GATE_SCLK_LCD */
+	GATE(CLK_SCLK_MIPIDPHY2L, "sclk_mipidphy2l", "div_mipi0",
+		GATE_SCLK_LCD, 4, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_MIPI0, "sclk_mipi0", "div_mipi0_pre",
+		GATE_SCLK_LCD, 3, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_FIMD0, "sclk_fimd0", "div_fimd0",
+		GATE_SCLK_LCD, 0, CLK_SET_RATE_PARENT, 0),
+
+	/* GATE_SCLK_ISP_TOP */
+	GATE(CLK_SCLK_CAM1, "sclk_cam1", "div_cam1",
+		GATE_SCLK_ISP_TOP, 4, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_UART_ISP, "sclk_uart_isp", "div_uart_isp",
+		GATE_SCLK_ISP_TOP, 3, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_SPI1_ISP, "sclk_spi1_isp", "div_spi1_isp",
+		GATE_SCLK_ISP_TOP, 2, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_SPI0_ISP, "sclk_spi0_isp", "div_spi0_isp",
+		GATE_SCLK_ISP_TOP, 1, CLK_SET_RATE_PARENT, 0),
+
+	/* GATE_SCLK_FSYS */
+	GATE(CLK_SCLK_UPLL, "sclk_upll", "mout_upll", GATE_SCLK_FSYS, 10, 0, 0),
+	GATE(CLK_SCLK_TSADC, "sclk_tsadc", "div_tsadc_pre",
+		GATE_SCLK_FSYS, 9, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_EBI, "sclk_ebi", "div_ebi",
+		GATE_SCLK_FSYS, 6, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_MMC1, "sclk_mmc1", "div_mmc1_pre",
+		GATE_SCLK_FSYS, 1, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_MMC0, "sclk_mmc0", "div_mmc0_pre",
+		GATE_SCLK_FSYS, 0, CLK_SET_RATE_PARENT, 0),
+
+	/* GATE_SCLK_PERIL */
+	GATE(CLK_SCLK_I2S, "sclk_i2s", "div_i2s",
+		GATE_SCLK_PERIL, 18, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_PCM, "sclk_pcm", "div_pcm",
+		GATE_SCLK_PERIL, 16, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_SPI1, "sclk_spi1", "div_spi1_pre",
+		GATE_SCLK_PERIL, 7, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_SPI0, "sclk_spi0", "div_spi0_pre",
+		GATE_SCLK_PERIL, 6, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_UART1, "sclk_uart1", "div_uart1",
+		GATE_SCLK_PERIL, 1, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_UART0, "sclk_uart0", "div_uart0",
+		GATE_SCLK_PERIL, 0, CLK_SET_RATE_PARENT, 0),
+
+	/* GATE_IP_CAM */
+	GATE(CLK_QEJPEG, "qejpeg", "div_cam_blk_320", GATE_IP_CAM, 19,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PIXELASYNCM1, "pixelasyncm1", "div_cam_blk_320",
+		GATE_IP_CAM, 18, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PIXELASYNCM0, "pixelasyncm0", "div_cam_blk_320",
+		GATE_IP_CAM, 17, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PPMUCAMIF, "ppmucamif", "div_cam_blk_320",
+		GATE_IP_CAM, 16, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_QEM2MSCALER, "qem2mscaler", "div_cam_blk_320",
+		GATE_IP_CAM, 14, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_QEGSCALER1, "qegscaler1", "div_cam_blk_320",
+		GATE_IP_CAM, 13, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_QEGSCALER0, "qegscaler0", "div_cam_blk_320",
+		GATE_IP_CAM, 12, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_SMMUJPEG, "smmujpeg", "div_cam_blk_320",
+		GATE_IP_CAM, 11, 0, 0),
+	GATE(CLK_SMMUM2M2SCALER, "smmum2m2scaler", "div_cam_blk_320",
+		GATE_IP_CAM, 9, 0, 0),
+	GATE(CLK_SMMUGSCALER1, "smmugscaler1", "div_cam_blk_320",
+		GATE_IP_CAM, 8, 0, 0),
+	GATE(CLK_SMMUGSCALER0, "smmugscaler0", "div_cam_blk_320",
+		GATE_IP_CAM, 7, 0, 0),
+	GATE(CLK_JPEG, "jpeg", "div_cam_blk_320", GATE_IP_CAM, 6, 0, 0),
+	GATE(CLK_M2MSCALER, "m2mscaler", "div_cam_blk_320",
+		GATE_IP_CAM, 2, 0, 0),
+	GATE(CLK_GSCALER1, "gscaler1", "div_cam_blk_320", GATE_IP_CAM, 1, 0, 0),
+	GATE(CLK_GSCALER0, "gscaler0", "div_cam_blk_320", GATE_IP_CAM, 0, 0, 0),
+
+	/* GATE_IP_MFC */
+	GATE(CLK_QEMFC, "qemfc", "div_aclk_200", GATE_IP_MFC, 5,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PPMUMFC_L, "ppmumfc_l", "div_aclk_200", GATE_IP_MFC, 3,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_SMMUMFC_L, "smmumfc_l", "div_aclk_200", GATE_IP_MFC, 1, 0, 0),
+	GATE(CLK_MFC, "mfc", "div_aclk_200", GATE_IP_MFC, 0, 0, 0),
+
+	/* GATE_IP_G3D */
+	GATE(CLK_SMMUG3D, "smmug3d", "div_aclk_200", GATE_IP_G3D, 3, 0, 0),
+	GATE(CLK_QEG3D, "qeg3d", "div_aclk_200", GATE_IP_G3D, 2,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PPMUG3D, "ppmug3d", "div_aclk_200", GATE_IP_G3D, 1,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_G3D, "g3d", "div_aclk_200", GATE_IP_G3D, 0, 0, 0),
+
+	/* GATE_IP_LCD */
+	GATE(CLK_QE_CH1_LCD, "qe_ch1_lcd", "div_aclk_160", GATE_IP_LCD, 7,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_QE_CH0_LCD, "qe_ch0_lcd", "div_aclk_160", GATE_IP_LCD, 6,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_PPMULCD0, "ppmulcd0", "div_aclk_160", GATE_IP_LCD, 5,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_SMMUFIMD0, "smmufimd0", "div_aclk_160", GATE_IP_LCD, 4, 0, 0),
+	GATE(CLK_DSIM0, "dsim0", "div_aclk_160", GATE_IP_LCD, 3, 0, 0),
+	GATE(CLK_SMIES, "smies", "div_aclk_160", GATE_IP_LCD, 2, 0, 0),
+	GATE(CLK_FIMD0, "fimd0", "div_aclk_160", GATE_IP_LCD, 0, 0, 0),
+
+	/* GATE_IP_ISP */
+	GATE(CLK_CAM1, "cam1", "mout_aclk_266_sub", GATE_IP_ISP, 5, 0, 0),
+	GATE(CLK_UART_ISP_TOP, "uart_isp_top", "mout_aclk_266_sub",
+		GATE_IP_ISP, 3, 0, 0),
+	GATE(CLK_SPI1_ISP_TOP, "spi1_isp_top", "mout_aclk_266_sub",
+		GATE_IP_ISP, 2, 0, 0),
+	GATE(CLK_SPI0_ISP_TOP, "spi0_isp_top", "mout_aclk_266_sub",
+		GATE_IP_ISP, 1, 0, 0),
+
+	/* GATE_IP_FSYS */
+	GATE(CLK_TSADC, "tsadc", "div_aclk_200", GATE_IP_FSYS, 20, 0, 0),
+	GATE(CLK_PPMUFILE, "ppmufile", "div_aclk_200", GATE_IP_FSYS, 17,
+		CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_USBOTG, "usbotg", "div_aclk_200", GATE_IP_FSYS, 13, 0, 0),
+	GATE(CLK_USBHOST, "usbhost", "div_aclk_200", GATE_IP_FSYS, 12, 0, 0),
+	GATE(CLK_SROMC, "sromc", "div_aclk_200", GATE_IP_FSYS, 11, 0, 0),
+	GATE(CLK_SDMMC1, "sdmmc1", "div_aclk_200", GATE_IP_FSYS, 6, 0, 0),
+	GATE(CLK_SDMMC0, "sdmmc0", "div_aclk_200", GATE_IP_FSYS, 5, 0, 0),
+	GATE(CLK_PDMA1, "pdma1", "div_aclk_200", GATE_IP_FSYS, 1, 0, 0),
+	GATE(CLK_PDMA0, "pdma0", "div_aclk_200", GATE_IP_FSYS, 0, 0, 0),
+
+	/* GATE_IP_PERIL */
+	GATE(CLK_PWM, "pwm", "div_aclk_100", GATE_IP_PERIL, 24, 0, 0),
+	GATE(CLK_PCM, "pcm", "div_aclk_100", GATE_IP_PERIL, 23, 0, 0),
+	GATE(CLK_I2S, "i2s", "div_aclk_100", GATE_IP_PERIL, 21, 0, 0),
+	GATE(CLK_SPI1, "spi1", "div_aclk_100", GATE_IP_PERIL, 17, 0, 0),
+	GATE(CLK_SPI0, "spi0", "div_aclk_100", GATE_IP_PERIL, 16, 0, 0),
+	GATE(CLK_I2C7, "i2c7", "div_aclk_100", GATE_IP_PERIL, 13, 0, 0),
+	GATE(CLK_I2C6, "i2c6", "div_aclk_100", GATE_IP_PERIL, 12, 0, 0),
+	GATE(CLK_I2C5, "i2c5", "div_aclk_100", GATE_IP_PERIL, 11, 0, 0),
+	GATE(CLK_I2C4, "i2c4", "div_aclk_100", GATE_IP_PERIL, 10, 0, 0),
+	GATE(CLK_I2C3, "i2c3", "div_aclk_100", GATE_IP_PERIL, 9, 0, 0),
+	GATE(CLK_I2C2, "i2c2", "div_aclk_100", GATE_IP_PERIL, 8, 0, 0),
+	GATE(CLK_I2C1, "i2c1", "div_aclk_100", GATE_IP_PERIL, 7, 0, 0),
+	GATE(CLK_I2C0, "i2c0", "div_aclk_100", GATE_IP_PERIL, 6, 0, 0),
+	GATE(CLK_UART1, "uart1", "div_aclk_100", GATE_IP_PERIL, 1, 0, 0),
+	GATE(CLK_UART0, "uart0", "div_aclk_100", GATE_IP_PERIL, 0, 0, 0),
+};
+
+/* APLL & MPLL & BPLL & UPLL */
+static struct samsung_pll_rate_table exynos3250_pll_rates[] = {
+	PLL_35XX_RATE(1200000000, 400, 4, 1),
+	PLL_35XX_RATE(1100000000, 275, 3, 1),
+	PLL_35XX_RATE(1066000000, 533, 6, 1),
+	PLL_35XX_RATE(1000000000, 250, 3, 1),
+	PLL_35XX_RATE( 960000000, 320, 4, 1),
+	PLL_35XX_RATE( 900000000, 300, 4, 1),
+	PLL_35XX_RATE( 850000000, 425, 6, 1),
+	PLL_35XX_RATE( 800000000, 200, 3, 1),
+	PLL_35XX_RATE( 700000000, 175, 3, 1),
+	PLL_35XX_RATE( 667000000, 667, 12, 1),
+	PLL_35XX_RATE( 600000000, 400, 4, 2),
+	PLL_35XX_RATE( 533000000, 533, 6, 2),
+	PLL_35XX_RATE( 520000000, 260, 3, 2),
+	PLL_35XX_RATE( 500000000, 250, 3, 2),
+	PLL_35XX_RATE( 400000000, 200, 3, 2),
+	PLL_35XX_RATE( 200000000, 200, 3, 3),
+	PLL_35XX_RATE( 100000000, 200, 3, 4),
+	{ /* sentinel */ }
+};
+
+/* VPLL */
+static struct samsung_pll_rate_table exynos3250_vpll_rates[] = {
+	PLL_36XX_RATE(600000000, 100, 2, 1,     0),
+	PLL_36XX_RATE(533000000, 266, 3, 2, 32768),
+	PLL_36XX_RATE(519230987, 173, 2, 2,  5046),
+	PLL_36XX_RATE(500000000, 250, 3, 2,     0),
+	PLL_36XX_RATE(445500000, 148, 2, 2, 32768),
+	PLL_36XX_RATE(445055007, 148, 2, 2, 23047),
+	PLL_36XX_RATE(400000000, 200, 3, 2,     0),
+	PLL_36XX_RATE(371250000, 123, 2, 2, 49152),
+	PLL_36XX_RATE(370878997, 185, 3, 2, 28803),
+	PLL_36XX_RATE(340000000, 170, 3, 2,     0),
+	PLL_36XX_RATE(335000015, 111, 2, 2, 43691),
+	PLL_36XX_RATE(333000000, 111, 2, 2,     0),
+	PLL_36XX_RATE(330000000, 110, 2, 2,     0),
+	PLL_36XX_RATE(320000015, 106, 2, 2, 43691),
+	PLL_36XX_RATE(300000000, 100, 2, 2,     0),
+	PLL_36XX_RATE(275000000, 275, 3, 3,     0),
+	PLL_36XX_RATE(222750000, 148, 2, 3, 32768),
+	PLL_36XX_RATE(222528007, 148, 2, 3, 23069),
+	PLL_36XX_RATE(160000000, 160, 3, 3,     0),
+	PLL_36XX_RATE(148500000,  99, 2, 3,     0),
+	PLL_36XX_RATE(148352005,  98, 2, 3, 59070),
+	PLL_36XX_RATE(108000000, 144, 2, 4,     0),
+	PLL_36XX_RATE( 74250000,  99, 2, 4,     0),
+	PLL_36XX_RATE( 74176002,  98, 3, 4, 59070),
+	PLL_36XX_RATE( 54054000, 216, 3, 5, 14156),
+	PLL_36XX_RATE( 54000000, 144, 2, 5,     0),
+	{ /* sentinel */ }
+};
+
+static struct samsung_pll_clock exynos3250_plls[nr_plls] __initdata = {
+	[apll] = PLL(pll_35xx, CLK_FOUT_APLL, "fout_apll", "fin_pll",
+			APLL_LOCK, APLL_CON0, NULL),
+	[mpll] = PLL(pll_35xx, CLK_FOUT_MPLL, "fout_mpll", "fin_pll",
+			MPLL_LOCK, MPLL_CON0, NULL),
+	[vpll] = PLL(pll_36xx, CLK_FOUT_VPLL, "fout_vpll", "fin_pll",
+			VPLL_LOCK, VPLL_CON0, NULL),
+	[upll] = PLL(pll_35xx, CLK_FOUT_UPLL, "fout_upll", "fin_pll",
+			UPLL_LOCK, UPLL_CON0, NULL),
+};
+
+static void __init exynos3250_cmu_init(struct device_node *np)
+{
+	struct samsung_clk_provider *ctx;
+
+	reg_base = of_iomap(np, 0);
+	if (!reg_base)
+		panic("%s: failed to map registers\n", __func__);
+
+	ctx = samsung_clk_init(np, reg_base, CLK_NR_CLKS);
+	if (!ctx)
+		panic("%s: unable to allocate context.\n", __func__);
+
+	samsung_clk_register_fixed_factor(ctx, fixed_factor_clks,
+					  ARRAY_SIZE(fixed_factor_clks));
+
+	exynos3250_plls[apll].rate_table = exynos3250_pll_rates;
+	exynos3250_plls[mpll].rate_table = exynos3250_pll_rates;
+	exynos3250_plls[vpll].rate_table = exynos3250_vpll_rates;
+	exynos3250_plls[upll].rate_table = exynos3250_pll_rates;
+
+	samsung_clk_register_pll(ctx, exynos3250_plls,
+					ARRAY_SIZE(exynos3250_plls), reg_base);
+
+	samsung_clk_register_mux(ctx, mux_clks, ARRAY_SIZE(mux_clks));
+	samsung_clk_register_div(ctx, div_clks, ARRAY_SIZE(div_clks));
+	samsung_clk_register_gate(ctx, gate_clks, ARRAY_SIZE(gate_clks));
+
+	exynos3250_clk_sleep_init();
+}
+CLK_OF_DECLARE(exynos3250_cmu, "samsung,exynos3250-cmu", exynos3250_cmu_init);
diff --git a/drivers/clk/samsung/clk-exynos4.c b/drivers/clk/samsung/clk-exynos4.c
index b4f96721017..c4df294bb7f 100644
--- a/drivers/clk/samsung/clk-exynos4.c
+++ b/drivers/clk/samsung/clk-exynos4.c
@@ -428,7 +428,7 @@ static struct samsung_fixed_rate_clock exynos4_fixed_rate_ext_clks[] __initdata
 /* fixed rate clocks generated inside the soc */
 static struct samsung_fixed_rate_clock exynos4_fixed_rate_clks[] __initdata = {
 	FRATE(0, "sclk_hdmi24m", NULL, CLK_IS_ROOT, 24000000),
-	FRATE(0, "sclk_hdmiphy", NULL, CLK_IS_ROOT, 27000000),
+	FRATE(CLK_SCLK_HDMIPHY, "sclk_hdmiphy", NULL, CLK_IS_ROOT, 27000000),
 	FRATE(0, "sclk_usbphy0", NULL, CLK_IS_ROOT, 48000000),
 };
 
@@ -903,7 +903,7 @@ static struct samsung_gate_clock exynos4x12_gate_clks[] __initdata = {
 	GATE(CLK_AUDSS, "audss", "sclk_epll", E4X12_GATE_IP_MAUDIO, 0, 0, 0),
 	GATE(CLK_MDNIE0, "mdnie0", "aclk160", GATE_IP_LCD0, 2, 0, 0),
 	GATE(CLK_ROTATOR, "rotator", "aclk200", E4X12_GATE_IP_IMAGE, 1, 0, 0),
-	GATE(CLK_MDMA2, "mdma2", "aclk200", E4X12_GATE_IP_IMAGE, 2, 0, 0),
+	GATE(CLK_MDMA, "mdma", "aclk200", E4X12_GATE_IP_IMAGE, 2, 0, 0),
 	GATE(CLK_SMMU_MDMA, "smmu_mdma", "aclk200", E4X12_GATE_IP_IMAGE, 5, 0,
 		0),
 	GATE(CLK_MIPI_HSI, "mipi_hsi", "aclk133", GATE_IP_FSYS, 10, 0, 0),
@@ -1043,7 +1043,7 @@ static unsigned long exynos4_get_xom(void)
 	return xom;
 }
 
-static void __init exynos4_clk_register_finpll(void)
+static void __init exynos4_clk_register_finpll(struct samsung_clk_provider *ctx)
 {
 	struct samsung_fixed_rate_clock fclk;
 	struct clk *clk;
@@ -1066,7 +1066,7 @@ static void __init exynos4_clk_register_finpll(void)
 	fclk.parent_name = NULL;
 	fclk.flags = CLK_IS_ROOT;
 	fclk.fixed_rate = finpll_f;
-	samsung_clk_register_fixed_rate(&fclk, 1);
+	samsung_clk_register_fixed_rate(ctx, &fclk, 1);
 
 }
 
@@ -1176,22 +1176,25 @@ static struct samsung_pll_clock exynos4x12_plls[nr_plls] __initdata = {
 static void __init exynos4_clk_init(struct device_node *np,
 				    enum exynos4_soc soc)
 {
+	struct samsung_clk_provider *ctx;
 	exynos4_soc = soc;
 
 	reg_base = of_iomap(np, 0);
 	if (!reg_base)
 		panic("%s: failed to map registers\n", __func__);
 
-	samsung_clk_init(np, reg_base, CLK_NR_CLKS);
+	ctx = samsung_clk_init(np, reg_base, CLK_NR_CLKS);
+	if (!ctx)
+		panic("%s: unable to allocate context.\n", __func__);
 
-	samsung_clk_of_register_fixed_ext(exynos4_fixed_rate_ext_clks,
+	samsung_clk_of_register_fixed_ext(ctx, exynos4_fixed_rate_ext_clks,
 			ARRAY_SIZE(exynos4_fixed_rate_ext_clks),
 			ext_clk_match);
 
-	exynos4_clk_register_finpll();
+	exynos4_clk_register_finpll(ctx);
 
 	if (exynos4_soc == EXYNOS4210) {
-		samsung_clk_register_mux(exynos4210_mux_early,
+		samsung_clk_register_mux(ctx, exynos4210_mux_early,
 					ARRAY_SIZE(exynos4210_mux_early));
 
 		if (_get_rate("fin_pll") == 24000000) {
@@ -1205,7 +1208,7 @@ static void __init exynos4_clk_init(struct device_node *np,
 			exynos4210_plls[vpll].rate_table =
 							exynos4210_vpll_rates;
 
-		samsung_clk_register_pll(exynos4210_plls,
+		samsung_clk_register_pll(ctx, exynos4210_plls,
 					ARRAY_SIZE(exynos4210_plls), reg_base);
 	} else {
 		if (_get_rate("fin_pll") == 24000000) {
@@ -1217,42 +1220,42 @@ static void __init exynos4_clk_init(struct device_node *np,
 							exynos4x12_vpll_rates;
 		}
 
-		samsung_clk_register_pll(exynos4x12_plls,
+		samsung_clk_register_pll(ctx, exynos4x12_plls,
 					ARRAY_SIZE(exynos4x12_plls), reg_base);
 	}
 
-	samsung_clk_register_fixed_rate(exynos4_fixed_rate_clks,
+	samsung_clk_register_fixed_rate(ctx, exynos4_fixed_rate_clks,
 			ARRAY_SIZE(exynos4_fixed_rate_clks));
-	samsung_clk_register_mux(exynos4_mux_clks,
+	samsung_clk_register_mux(ctx, exynos4_mux_clks,
 			ARRAY_SIZE(exynos4_mux_clks));
-	samsung_clk_register_div(exynos4_div_clks,
+	samsung_clk_register_div(ctx, exynos4_div_clks,
 			ARRAY_SIZE(exynos4_div_clks));
-	samsung_clk_register_gate(exynos4_gate_clks,
+	samsung_clk_register_gate(ctx, exynos4_gate_clks,
 			ARRAY_SIZE(exynos4_gate_clks));
 
 	if (exynos4_soc == EXYNOS4210) {
-		samsung_clk_register_fixed_rate(exynos4210_fixed_rate_clks,
+		samsung_clk_register_fixed_rate(ctx, exynos4210_fixed_rate_clks,
 			ARRAY_SIZE(exynos4210_fixed_rate_clks));
-		samsung_clk_register_mux(exynos4210_mux_clks,
+		samsung_clk_register_mux(ctx, exynos4210_mux_clks,
 			ARRAY_SIZE(exynos4210_mux_clks));
-		samsung_clk_register_div(exynos4210_div_clks,
+		samsung_clk_register_div(ctx, exynos4210_div_clks,
 			ARRAY_SIZE(exynos4210_div_clks));
-		samsung_clk_register_gate(exynos4210_gate_clks,
+		samsung_clk_register_gate(ctx, exynos4210_gate_clks,
 			ARRAY_SIZE(exynos4210_gate_clks));
-		samsung_clk_register_alias(exynos4210_aliases,
+		samsung_clk_register_alias(ctx, exynos4210_aliases,
 			ARRAY_SIZE(exynos4210_aliases));
 	} else {
-		samsung_clk_register_mux(exynos4x12_mux_clks,
+		samsung_clk_register_mux(ctx, exynos4x12_mux_clks,
 			ARRAY_SIZE(exynos4x12_mux_clks));
-		samsung_clk_register_div(exynos4x12_div_clks,
+		samsung_clk_register_div(ctx, exynos4x12_div_clks,
 			ARRAY_SIZE(exynos4x12_div_clks));
-		samsung_clk_register_gate(exynos4x12_gate_clks,
+		samsung_clk_register_gate(ctx, exynos4x12_gate_clks,
 			ARRAY_SIZE(exynos4x12_gate_clks));
-		samsung_clk_register_alias(exynos4x12_aliases,
+		samsung_clk_register_alias(ctx, exynos4x12_aliases,
 			ARRAY_SIZE(exynos4x12_aliases));
 	}
 
-	samsung_clk_register_alias(exynos4_aliases,
+	samsung_clk_register_alias(ctx, exynos4_aliases,
 			ARRAY_SIZE(exynos4_aliases));
 
 	exynos4_clk_sleep_init();
diff --git a/drivers/clk/samsung/clk-exynos5250.c b/drivers/clk/samsung/clk-exynos5250.c
index e7ee4420da8..1fad4c5e3f5 100644
--- a/drivers/clk/samsung/clk-exynos5250.c
+++ b/drivers/clk/samsung/clk-exynos5250.c
@@ -24,10 +24,14 @@
 #define APLL_CON0		0x100
 #define SRC_CPU			0x200
 #define DIV_CPU0		0x500
+#define PWR_CTRL1		0x1020
+#define PWR_CTRL2		0x1024
 #define MPLL_LOCK		0x4000
 #define MPLL_CON0		0x4100
 #define SRC_CORE1		0x4204
 #define GATE_IP_ACP		0x8800
+#define GATE_IP_ISP0		0xc800
+#define GATE_IP_ISP1		0xc804
 #define CPLL_LOCK		0x10020
 #define EPLL_LOCK		0x10030
 #define VPLL_LOCK		0x10040
@@ -37,6 +41,7 @@
 #define VPLL_CON0		0x10140
 #define GPLL_CON0		0x10150
 #define SRC_TOP0		0x10210
+#define SRC_TOP1		0x10214
 #define SRC_TOP2		0x10218
 #define SRC_TOP3		0x1021c
 #define SRC_GSCL		0x10220
@@ -71,6 +76,7 @@
 #define GATE_IP_GSCL		0x10920
 #define GATE_IP_DISP1		0x10928
 #define GATE_IP_MFC		0x1092c
+#define GATE_IP_G3D		0x10930
 #define GATE_IP_GEN		0x10934
 #define GATE_IP_FSYS		0x10944
 #define GATE_IP_PERIC		0x10950
@@ -80,6 +86,23 @@
 #define SRC_CDREX		0x20200
 #define PLL_DIV2_SEL		0x20a24
 
+/*Below definitions are used for PWR_CTRL settings*/
+#define PWR_CTRL1_CORE2_DOWN_RATIO		(7 << 28)
+#define PWR_CTRL1_CORE1_DOWN_RATIO		(7 << 16)
+#define PWR_CTRL1_DIV2_DOWN_EN			(1 << 9)
+#define PWR_CTRL1_DIV1_DOWN_EN			(1 << 8)
+#define PWR_CTRL1_USE_CORE1_WFE			(1 << 5)
+#define PWR_CTRL1_USE_CORE0_WFE			(1 << 4)
+#define PWR_CTRL1_USE_CORE1_WFI			(1 << 1)
+#define PWR_CTRL1_USE_CORE0_WFI			(1 << 0)
+
+#define PWR_CTRL2_DIV2_UP_EN			(1 << 25)
+#define PWR_CTRL2_DIV1_UP_EN			(1 << 24)
+#define PWR_CTRL2_DUR_STANDBY2_VAL		(1 << 16)
+#define PWR_CTRL2_DUR_STANDBY1_VAL		(1 << 8)
+#define PWR_CTRL2_CORE2_UP_RATIO		(1 << 4)
+#define PWR_CTRL2_CORE1_UP_RATIO		(1 << 0)
+
 /* list of PLLs to be registered */
 enum exynos5250_plls {
 	apll, mpll, cpll, epll, vpll, gpll, bpll,
@@ -98,8 +121,11 @@ static struct samsung_clk_reg_dump *exynos5250_save;
 static unsigned long exynos5250_clk_regs[] __initdata = {
 	SRC_CPU,
 	DIV_CPU0,
+	PWR_CTRL1,
+	PWR_CTRL2,
 	SRC_CORE1,
 	SRC_TOP0,
+	SRC_TOP1,
 	SRC_TOP2,
 	SRC_TOP3,
 	SRC_GSCL,
@@ -133,6 +159,7 @@ static unsigned long exynos5250_clk_regs[] __initdata = {
 	DIV_PERIC5,
 	GATE_IP_GSCL,
 	GATE_IP_MFC,
+	GATE_IP_G3D,
 	GATE_IP_GEN,
 	GATE_IP_FSYS,
 	GATE_IP_PERIC,
@@ -141,6 +168,8 @@ static unsigned long exynos5250_clk_regs[] __initdata = {
 	PLL_DIV2_SEL,
 	GATE_IP_DISP1,
 	GATE_IP_ACP,
+	GATE_IP_ISP0,
+	GATE_IP_ISP1,
 };
 
 static int exynos5250_clk_suspend(void)
@@ -189,13 +218,16 @@ PNAME(mout_vpllsrc_p)	= { "fin_pll", "sclk_hdmi27m" };
 PNAME(mout_vpll_p)	= { "mout_vpllsrc", "fout_vpll" };
 PNAME(mout_cpll_p)	= { "fin_pll", "fout_cpll" };
 PNAME(mout_epll_p)	= { "fin_pll", "fout_epll" };
+PNAME(mout_gpll_p)	= { "fin_pll", "fout_gpll" };
 PNAME(mout_mpll_user_p)	= { "fin_pll", "mout_mpll" };
 PNAME(mout_bpll_user_p)	= { "fin_pll", "mout_bpll" };
 PNAME(mout_aclk166_p)	= { "mout_cpll", "mout_mpll_user" };
 PNAME(mout_aclk200_p)	= { "mout_mpll_user", "mout_bpll_user" };
+PNAME(mout_aclk400_p)	= { "mout_aclk400_g3d_mid", "mout_gpll" };
 PNAME(mout_aclk200_sub_p) = { "fin_pll", "div_aclk200" };
 PNAME(mout_aclk266_sub_p) = { "fin_pll", "div_aclk266" };
 PNAME(mout_aclk333_sub_p) = { "fin_pll", "div_aclk333" };
+PNAME(mout_aclk400_isp_sub_p) = { "fin_pll", "div_aclk400_isp" };
 PNAME(mout_hdmi_p)	= { "div_hdmi_pixel", "sclk_hdmiphy" };
 PNAME(mout_usb3_p)	= { "mout_mpll_user", "mout_cpll" };
 PNAME(mout_group1_p)	= { "fin_pll", "fin_pll", "sclk_hdmi27m",
@@ -273,15 +305,23 @@ static struct samsung_mux_clock exynos5250_mux_clks[] __initdata = {
 	MUX(0, "mout_aclk166", mout_aclk166_p, SRC_TOP0, 8, 1),
 	MUX(0, "mout_aclk200", mout_aclk200_p, SRC_TOP0, 12, 1),
 	MUX(0, "mout_aclk333", mout_aclk166_p, SRC_TOP0, 16, 1),
+	MUX(0, "mout_aclk400_g3d_mid", mout_aclk200_p, SRC_TOP0, 20, 1),
+
+	MUX(0, "mout_aclk400_isp", mout_aclk200_p, SRC_TOP1, 24, 1),
+	MUX(0, "mout_aclk400_g3d", mout_aclk400_p, SRC_TOP1, 28, 1),
 
 	MUX(0, "mout_cpll", mout_cpll_p, SRC_TOP2, 8, 1),
 	MUX(0, "mout_epll", mout_epll_p, SRC_TOP2, 12, 1),
 	MUX(0, "mout_vpll", mout_vpll_p, SRC_TOP2, 16, 1),
 	MUX(0, "mout_mpll_user", mout_mpll_user_p, SRC_TOP2, 20, 1),
 	MUX(0, "mout_bpll_user", mout_bpll_user_p, SRC_TOP2, 24, 1),
+	MUX(CLK_MOUT_GPLL, "mout_gpll", mout_gpll_p, SRC_TOP2, 28, 1),
 
 	MUX(0, "mout_aclk200_disp1_sub", mout_aclk200_sub_p, SRC_TOP3, 4, 1),
 	MUX(0, "mout_aclk266_gscl_sub", mout_aclk266_sub_p, SRC_TOP3, 8, 1),
+	MUX(0, "mout_aclk_266_isp_sub", mout_aclk266_sub_p, SRC_TOP3, 16, 1),
+	MUX(0, "mout_aclk_400_isp_sub", mout_aclk400_isp_sub_p,
+			SRC_TOP3, 20, 1),
 	MUX(0, "mout_aclk333_sub", mout_aclk333_sub_p, SRC_TOP3, 24, 1),
 
 	MUX(0, "mout_cam_bayer", mout_group1_p, SRC_GSCL, 12, 4),
@@ -351,7 +391,10 @@ static struct samsung_div_clock exynos5250_div_clks[] __initdata = {
 	DIV(0, "div_aclk200", "mout_aclk200", DIV_TOP0, 12, 3),
 	DIV(0, "div_aclk266", "mout_mpll_user", DIV_TOP0, 16, 3),
 	DIV(0, "div_aclk333", "mout_aclk333", DIV_TOP0, 20, 3),
+	DIV(0, "div_aclk400_g3d", "mout_aclk400_g3d", DIV_TOP0,
+							24, 3),
 
+	DIV(0, "div_aclk400_isp", "mout_aclk400_isp", DIV_TOP1, 20, 3),
 	DIV(0, "div_aclk66_pre", "mout_mpll_user", DIV_TOP1, 24, 3),
 
 	DIV(0, "div_cam_bayer", "mout_cam_bayer", DIV_GSCL, 12, 4),
@@ -428,6 +471,7 @@ static struct samsung_gate_clock exynos5250_gate_clks[] __initdata = {
 	 * CMU_ACP
 	 */
 	GATE(CLK_MDMA0, "mdma0", "div_aclk266", GATE_IP_ACP, 1, 0, 0),
+	GATE(CLK_SSS, "sss", "div_aclk266", GATE_IP_ACP, 2, 0, 0),
 	GATE(CLK_G2D, "g2d", "div_aclk200", GATE_IP_ACP, 3, 0, 0),
 	GATE(CLK_SMMU_MDMA0, "smmu_mdma0", "div_aclk266", GATE_IP_ACP, 5, 0, 0),
 
@@ -533,7 +577,8 @@ static struct samsung_gate_clock exynos5250_gate_clks[] __initdata = {
 		0),
 	GATE(CLK_SMMU_MFCL, "smmu_mfcl", "mout_aclk333_sub", GATE_IP_MFC, 2, 0,
 		0),
-
+	GATE(CLK_G3D, "g3d", "div_aclk400_g3d", GATE_IP_G3D, 0,
+					CLK_SET_RATE_PARENT, 0),
 	GATE(CLK_ROTATOR, "rotator", "div_aclk266", GATE_IP_GEN, 1, 0, 0),
 	GATE(CLK_JPEG, "jpeg", "div_aclk166", GATE_IP_GEN, 2, 0, 0),
 	GATE(CLK_MDMA1, "mdma1", "div_aclk266", GATE_IP_GEN, 4, 0, 0),
@@ -615,6 +660,31 @@ static struct samsung_gate_clock exynos5250_gate_clks[] __initdata = {
 	GATE(CLK_WDT, "wdt", "div_aclk66", GATE_IP_PERIS, 19, 0, 0),
 	GATE(CLK_RTC, "rtc", "div_aclk66", GATE_IP_PERIS, 20, 0, 0),
 	GATE(CLK_TMU, "tmu", "div_aclk66", GATE_IP_PERIS, 21, 0, 0),
+	GATE(CLK_SMMU_TV, "smmu_tv", "mout_aclk200_disp1_sub",
+			GATE_IP_DISP1, 2, 0, 0),
+	GATE(CLK_SMMU_FIMD1, "smmu_fimd1", "mout_aclk200_disp1_sub",
+			GATE_IP_DISP1, 8, 0, 0),
+	GATE(CLK_SMMU_2D, "smmu_2d", "div_aclk200", GATE_IP_ACP, 7, 0, 0),
+	GATE(CLK_SMMU_FIMC_ISP, "smmu_fimc_isp", "mout_aclk_266_isp_sub",
+			GATE_IP_ISP0, 8, 0, 0),
+	GATE(CLK_SMMU_FIMC_DRC, "smmu_fimc_drc", "mout_aclk_266_isp_sub",
+			GATE_IP_ISP0, 9, 0, 0),
+	GATE(CLK_SMMU_FIMC_FD, "smmu_fimc_fd", "mout_aclk_266_isp_sub",
+			GATE_IP_ISP0, 10, 0, 0),
+	GATE(CLK_SMMU_FIMC_SCC, "smmu_fimc_scc", "mout_aclk_266_isp_sub",
+			GATE_IP_ISP0, 11, 0, 0),
+	GATE(CLK_SMMU_FIMC_SCP, "smmu_fimc_scp", "mout_aclk_266_isp_sub",
+			GATE_IP_ISP0, 12, 0, 0),
+	GATE(CLK_SMMU_FIMC_MCU, "smmu_fimc_mcu", "mout_aclk_400_isp_sub",
+			GATE_IP_ISP0, 13, 0, 0),
+	GATE(CLK_SMMU_FIMC_ODC, "smmu_fimc_odc", "mout_aclk_266_isp_sub",
+			GATE_IP_ISP1, 4, 0, 0),
+	GATE(CLK_SMMU_FIMC_DIS0, "smmu_fimc_dis0", "mout_aclk_266_isp_sub",
+			GATE_IP_ISP1, 5, 0, 0),
+	GATE(CLK_SMMU_FIMC_DIS1, "smmu_fimc_dis1", "mout_aclk_266_isp_sub",
+			GATE_IP_ISP1, 6, 0, 0),
+	GATE(CLK_SMMU_FIMC_3DNR, "smmu_fimc_3dnr", "mout_aclk_266_isp_sub",
+			GATE_IP_ISP1, 7, 0, 0),
 };
 
 static struct samsung_pll_rate_table vpll_24mhz_tbl[] __initdata = {
@@ -686,6 +756,9 @@ static struct of_device_id ext_clk_match[] __initdata = {
 /* register exynox5250 clocks */
 static void __init exynos5250_clk_init(struct device_node *np)
 {
+	struct samsung_clk_provider *ctx;
+	unsigned int tmp;
+
 	if (np) {
 		reg_base = of_iomap(np, 0);
 		if (!reg_base)
@@ -694,11 +767,13 @@ static void __init exynos5250_clk_init(struct device_node *np)
 		panic("%s: unable to determine soc\n", __func__);
 	}
 
-	samsung_clk_init(np, reg_base, CLK_NR_CLKS);
-	samsung_clk_of_register_fixed_ext(exynos5250_fixed_rate_ext_clks,
+	ctx = samsung_clk_init(np, reg_base, CLK_NR_CLKS);
+	if (!ctx)
+		panic("%s: unable to allocate context.\n", __func__);
+	samsung_clk_of_register_fixed_ext(ctx, exynos5250_fixed_rate_ext_clks,
 			ARRAY_SIZE(exynos5250_fixed_rate_ext_clks),
 			ext_clk_match);
-	samsung_clk_register_mux(exynos5250_pll_pmux_clks,
+	samsung_clk_register_mux(ctx, exynos5250_pll_pmux_clks,
 				ARRAY_SIZE(exynos5250_pll_pmux_clks));
 
 	if (_get_rate("fin_pll") == 24 * MHZ) {
@@ -709,19 +784,40 @@ static void __init exynos5250_clk_init(struct device_node *np)
 	if (_get_rate("mout_vpllsrc") == 24 * MHZ)
 		exynos5250_plls[vpll].rate_table =  vpll_24mhz_tbl;
 
-	samsung_clk_register_pll(exynos5250_plls, ARRAY_SIZE(exynos5250_plls),
-					reg_base);
-	samsung_clk_register_fixed_rate(exynos5250_fixed_rate_clks,
+	samsung_clk_register_pll(ctx, exynos5250_plls,
+			ARRAY_SIZE(exynos5250_plls),
+			reg_base);
+	samsung_clk_register_fixed_rate(ctx, exynos5250_fixed_rate_clks,
 			ARRAY_SIZE(exynos5250_fixed_rate_clks));
-	samsung_clk_register_fixed_factor(exynos5250_fixed_factor_clks,
+	samsung_clk_register_fixed_factor(ctx, exynos5250_fixed_factor_clks,
 			ARRAY_SIZE(exynos5250_fixed_factor_clks));
-	samsung_clk_register_mux(exynos5250_mux_clks,
+	samsung_clk_register_mux(ctx, exynos5250_mux_clks,
 			ARRAY_SIZE(exynos5250_mux_clks));
-	samsung_clk_register_div(exynos5250_div_clks,
+	samsung_clk_register_div(ctx, exynos5250_div_clks,
 			ARRAY_SIZE(exynos5250_div_clks));
-	samsung_clk_register_gate(exynos5250_gate_clks,
+	samsung_clk_register_gate(ctx, exynos5250_gate_clks,
 			ARRAY_SIZE(exynos5250_gate_clks));
 
+	/*
+	 * Enable arm clock down (in idle) and set arm divider
+	 * ratios in WFI/WFE state.
+	 */
+	tmp = (PWR_CTRL1_CORE2_DOWN_RATIO | PWR_CTRL1_CORE1_DOWN_RATIO |
+		PWR_CTRL1_DIV2_DOWN_EN | PWR_CTRL1_DIV1_DOWN_EN |
+		PWR_CTRL1_USE_CORE1_WFE | PWR_CTRL1_USE_CORE0_WFE |
+		PWR_CTRL1_USE_CORE1_WFI | PWR_CTRL1_USE_CORE0_WFI);
+	__raw_writel(tmp, reg_base + PWR_CTRL1);
+
+	/*
+	 * Enable arm clock up (on exiting idle). Set arm divider
+	 * ratios when not in idle along with the standby duration
+	 * ratios.
+	 */
+	tmp = (PWR_CTRL2_DIV2_UP_EN | PWR_CTRL2_DIV1_UP_EN |
+		PWR_CTRL2_DUR_STANDBY2_VAL | PWR_CTRL2_DUR_STANDBY1_VAL |
+		PWR_CTRL2_CORE2_UP_RATIO | PWR_CTRL2_CORE1_UP_RATIO);
+	__raw_writel(tmp, reg_base + PWR_CTRL2);
+
 	exynos5250_clk_sleep_init();
 
 	pr_info("Exynos5250: clock setup completed, armclk=%ld\n",
diff --git a/drivers/clk/samsung/clk-exynos5260.c b/drivers/clk/samsung/clk-exynos5260.c
new file mode 100644
index 00000000000..64596ba58df
--- /dev/null
+++ b/drivers/clk/samsung/clk-exynos5260.c
@@ -0,0 +1,1980 @@
+/*
+ * Copyright (c) 2014 Samsung Electronics Co., Ltd.
+ * Author: Rahul Sharma <rahul.sharma@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.
+ *
+ * Common Clock Framework support for Exynos5260 SoC.
+ */
+
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/syscore_ops.h>
+
+#include "clk-exynos5260.h"
+#include "clk.h"
+#include "clk-pll.h"
+
+#include <dt-bindings/clock/exynos5260-clk.h>
+
+static LIST_HEAD(clock_reg_cache_list);
+
+struct exynos5260_clock_reg_cache {
+	struct list_head node;
+	void __iomem *reg_base;
+	struct samsung_clk_reg_dump *rdump;
+	unsigned int rd_num;
+};
+
+struct exynos5260_cmu_info {
+	/* list of pll clocks and respective count */
+	struct samsung_pll_clock *pll_clks;
+	unsigned int nr_pll_clks;
+	/* list of mux clocks and respective count */
+	struct samsung_mux_clock *mux_clks;
+	unsigned int nr_mux_clks;
+	/* list of div clocks and respective count */
+	struct samsung_div_clock *div_clks;
+	unsigned int nr_div_clks;
+	/* list of gate clocks and respective count */
+	struct samsung_gate_clock *gate_clks;
+	unsigned int nr_gate_clks;
+	/* list of fixed clocks and respective count */
+	struct samsung_fixed_rate_clock *fixed_clks;
+	unsigned int nr_fixed_clks;
+	/* total number of clocks with IDs assigned*/
+	unsigned int nr_clk_ids;
+
+	/* list and number of clocks registers */
+	unsigned long *clk_regs;
+	unsigned int nr_clk_regs;
+};
+
+/*
+ * Applicable for all 2550 Type PLLS for Exynos5260, listed below
+ * DISP_PLL, EGL_PLL, KFC_PLL, MEM_PLL, BUS_PLL, MEDIA_PLL, G3D_PLL.
+ */
+static struct samsung_pll_rate_table pll2550_24mhz_tbl[] __initdata = {
+	PLL_35XX_RATE(1700000000, 425, 6, 0),
+	PLL_35XX_RATE(1600000000, 200, 3, 0),
+	PLL_35XX_RATE(1500000000, 250, 4, 0),
+	PLL_35XX_RATE(1400000000, 175, 3, 0),
+	PLL_35XX_RATE(1300000000, 325, 6, 0),
+	PLL_35XX_RATE(1200000000, 400, 4, 1),
+	PLL_35XX_RATE(1100000000, 275, 3, 1),
+	PLL_35XX_RATE(1000000000, 250, 3, 1),
+	PLL_35XX_RATE(933000000, 311, 4, 1),
+	PLL_35XX_RATE(900000000, 300, 4, 1),
+	PLL_35XX_RATE(800000000, 200, 3, 1),
+	PLL_35XX_RATE(733000000, 733, 12, 1),
+	PLL_35XX_RATE(700000000, 175, 3, 1),
+	PLL_35XX_RATE(667000000, 667, 12, 1),
+	PLL_35XX_RATE(633000000, 211, 4, 1),
+	PLL_35XX_RATE(620000000, 310, 3, 2),
+	PLL_35XX_RATE(600000000, 400, 4, 2),
+	PLL_35XX_RATE(543000000, 362, 4, 2),
+	PLL_35XX_RATE(533000000, 533, 6, 2),
+	PLL_35XX_RATE(500000000, 250, 3, 2),
+	PLL_35XX_RATE(450000000, 300, 4, 2),
+	PLL_35XX_RATE(400000000, 200, 3, 2),
+	PLL_35XX_RATE(350000000, 175, 3, 2),
+	PLL_35XX_RATE(300000000, 400, 4, 3),
+	PLL_35XX_RATE(266000000, 266, 3, 3),
+	PLL_35XX_RATE(200000000, 200, 3, 3),
+	PLL_35XX_RATE(160000000, 160, 3, 3),
+};
+
+/*
+ * Applicable for 2650 Type PLL for AUD_PLL.
+ */
+static struct samsung_pll_rate_table pll2650_24mhz_tbl[] __initdata = {
+	PLL_36XX_RATE(1600000000, 200, 3, 0, 0),
+	PLL_36XX_RATE(1200000000, 100, 2, 0, 0),
+	PLL_36XX_RATE(1000000000, 250, 3, 1, 0),
+	PLL_36XX_RATE(800000000, 200, 3, 1, 0),
+	PLL_36XX_RATE(600000000, 100, 2, 1, 0),
+	PLL_36XX_RATE(532000000, 266, 3, 2, 0),
+	PLL_36XX_RATE(480000000, 160, 2, 2, 0),
+	PLL_36XX_RATE(432000000, 144, 2, 2, 0),
+	PLL_36XX_RATE(400000000, 200, 3, 2, 0),
+	PLL_36XX_RATE(394073130, 459, 7, 2, 49282),
+	PLL_36XX_RATE(333000000, 111, 2, 2, 0),
+	PLL_36XX_RATE(300000000, 100, 2, 2, 0),
+	PLL_36XX_RATE(266000000, 266, 3, 3, 0),
+	PLL_36XX_RATE(200000000, 200, 3, 3, 0),
+	PLL_36XX_RATE(166000000, 166, 3, 3, 0),
+	PLL_36XX_RATE(133000000, 266, 3, 4, 0),
+	PLL_36XX_RATE(100000000, 200, 3, 4, 0),
+	PLL_36XX_RATE(66000000, 176, 2, 5, 0),
+};
+
+#ifdef CONFIG_PM_SLEEP
+
+static int exynos5260_clk_suspend(void)
+{
+	struct exynos5260_clock_reg_cache *cache;
+
+	list_for_each_entry(cache, &clock_reg_cache_list, node)
+		samsung_clk_save(cache->reg_base, cache->rdump,
+				cache->rd_num);
+
+	return 0;
+}
+
+static void exynos5260_clk_resume(void)
+{
+	struct exynos5260_clock_reg_cache *cache;
+
+	list_for_each_entry(cache, &clock_reg_cache_list, node)
+		samsung_clk_restore(cache->reg_base, cache->rdump,
+				cache->rd_num);
+}
+
+static struct syscore_ops exynos5260_clk_syscore_ops = {
+	.suspend = exynos5260_clk_suspend,
+	.resume = exynos5260_clk_resume,
+};
+
+static void exynos5260_clk_sleep_init(void __iomem *reg_base,
+			unsigned long *rdump,
+			unsigned long nr_rdump)
+{
+	struct exynos5260_clock_reg_cache *reg_cache;
+
+	reg_cache = kzalloc(sizeof(struct exynos5260_clock_reg_cache),
+			GFP_KERNEL);
+	if (!reg_cache)
+		panic("could not allocate register cache.\n");
+
+	reg_cache->rdump = samsung_clk_alloc_reg_dump(rdump, nr_rdump);
+
+	if (!reg_cache->rdump)
+		panic("could not allocate register dump storage.\n");
+
+	if (list_empty(&clock_reg_cache_list))
+		register_syscore_ops(&exynos5260_clk_syscore_ops);
+
+	reg_cache->rd_num = nr_rdump;
+	reg_cache->reg_base = reg_base;
+	list_add_tail(&reg_cache->node, &clock_reg_cache_list);
+}
+
+#else
+static void exynos5260_clk_sleep_init(void __iomem *reg_base,
+			unsigned long *rdump,
+			unsigned long nr_rdump){}
+#endif
+
+/*
+ * Common function which registers plls, muxes, dividers and gates
+ * for each CMU. It also add CMU register list to register cache.
+ */
+
+void __init exynos5260_cmu_register_one(struct device_node *np,
+			struct exynos5260_cmu_info *cmu)
+{
+	void __iomem *reg_base;
+	struct samsung_clk_provider *ctx;
+
+	reg_base = of_iomap(np, 0);
+	if (!reg_base)
+		panic("%s: failed to map registers\n", __func__);
+
+	ctx = samsung_clk_init(np, reg_base, cmu->nr_clk_ids);
+	if (!ctx)
+		panic("%s: unable to alllocate ctx\n", __func__);
+
+	if (cmu->pll_clks)
+		samsung_clk_register_pll(ctx, cmu->pll_clks, cmu->nr_pll_clks,
+			reg_base);
+	if (cmu->mux_clks)
+		samsung_clk_register_mux(ctx,  cmu->mux_clks,
+			cmu->nr_mux_clks);
+	if (cmu->div_clks)
+		samsung_clk_register_div(ctx, cmu->div_clks, cmu->nr_div_clks);
+	if (cmu->gate_clks)
+		samsung_clk_register_gate(ctx, cmu->gate_clks,
+			cmu->nr_gate_clks);
+	if (cmu->fixed_clks)
+		samsung_clk_register_fixed_rate(ctx, cmu->fixed_clks,
+			cmu->nr_fixed_clks);
+	if (cmu->clk_regs)
+		exynos5260_clk_sleep_init(reg_base, cmu->clk_regs,
+			cmu->nr_clk_regs);
+}
+
+
+/* CMU_AUD */
+
+static unsigned long aud_clk_regs[] __initdata = {
+	MUX_SEL_AUD,
+	DIV_AUD0,
+	DIV_AUD1,
+	EN_ACLK_AUD,
+	EN_PCLK_AUD,
+	EN_SCLK_AUD,
+	EN_IP_AUD,
+};
+
+PNAME(mout_aud_pll_user_p) = {"fin_pll", "fout_aud_pll"};
+PNAME(mout_sclk_aud_i2s_p) = {"mout_aud_pll_user", "ioclk_i2s_cdclk"};
+PNAME(mout_sclk_aud_pcm_p) = {"mout_aud_pll_user", "ioclk_pcm_extclk"};
+
+struct samsung_mux_clock aud_mux_clks[] __initdata = {
+	MUX(AUD_MOUT_AUD_PLL_USER, "mout_aud_pll_user", mout_aud_pll_user_p,
+			MUX_SEL_AUD, 0, 1),
+	MUX(AUD_MOUT_SCLK_AUD_I2S, "mout_sclk_aud_i2s", mout_sclk_aud_i2s_p,
+			MUX_SEL_AUD, 4, 1),
+	MUX(AUD_MOUT_SCLK_AUD_PCM, "mout_sclk_aud_pcm", mout_sclk_aud_pcm_p,
+			MUX_SEL_AUD, 8, 1),
+};
+
+struct samsung_div_clock aud_div_clks[] __initdata = {
+	DIV(AUD_DOUT_ACLK_AUD_131, "dout_aclk_aud_131", "mout_aud_pll_user",
+			DIV_AUD0, 0, 4),
+
+	DIV(AUD_DOUT_SCLK_AUD_I2S, "dout_sclk_aud_i2s", "mout_sclk_aud_i2s",
+			DIV_AUD1, 0, 4),
+	DIV(AUD_DOUT_SCLK_AUD_PCM, "dout_sclk_aud_pcm", "mout_sclk_aud_pcm",
+			DIV_AUD1, 4, 8),
+	DIV(AUD_DOUT_SCLK_AUD_UART, "dout_sclk_aud_uart", "mout_aud_pll_user",
+			DIV_AUD1, 12, 4),
+};
+
+struct samsung_gate_clock aud_gate_clks[] __initdata = {
+	GATE(AUD_SCLK_I2S, "sclk_aud_i2s", "dout_sclk_aud_i2s",
+			EN_SCLK_AUD, 0, CLK_SET_RATE_PARENT, 0),
+	GATE(AUD_SCLK_PCM, "sclk_aud_pcm", "dout_sclk_aud_pcm",
+			EN_SCLK_AUD, 1, CLK_SET_RATE_PARENT, 0),
+	GATE(AUD_SCLK_AUD_UART, "sclk_aud_uart", "dout_sclk_aud_uart",
+			EN_SCLK_AUD, 2, CLK_SET_RATE_PARENT, 0),
+
+	GATE(AUD_CLK_SRAMC, "clk_sramc", "dout_aclk_aud_131", EN_IP_AUD,
+			0, 0, 0),
+	GATE(AUD_CLK_DMAC, "clk_dmac", "dout_aclk_aud_131",
+			EN_IP_AUD, 1, 0, 0),
+	GATE(AUD_CLK_I2S, "clk_i2s", "dout_aclk_aud_131", EN_IP_AUD, 2, 0, 0),
+	GATE(AUD_CLK_PCM, "clk_pcm", "dout_aclk_aud_131", EN_IP_AUD, 3, 0, 0),
+	GATE(AUD_CLK_AUD_UART, "clk_aud_uart", "dout_aclk_aud_131",
+			EN_IP_AUD, 4, 0, 0),
+};
+
+static void __init exynos5260_clk_aud_init(struct device_node *np)
+{
+	struct exynos5260_cmu_info cmu = {0};
+
+	cmu.mux_clks = aud_mux_clks;
+	cmu.nr_mux_clks = ARRAY_SIZE(aud_mux_clks);
+	cmu.div_clks = aud_div_clks;
+	cmu.nr_div_clks = ARRAY_SIZE(aud_div_clks);
+	cmu.gate_clks = aud_gate_clks;
+	cmu.nr_gate_clks = ARRAY_SIZE(aud_gate_clks);
+	cmu.nr_clk_ids = AUD_NR_CLK;
+	cmu.clk_regs = aud_clk_regs;
+	cmu.nr_clk_regs = ARRAY_SIZE(aud_clk_regs);
+
+	exynos5260_cmu_register_one(np, &cmu);
+}
+
+CLK_OF_DECLARE(exynos5260_clk_aud, "samsung,exynos5260-clock-aud",
+		exynos5260_clk_aud_init);
+
+
+/* CMU_DISP */
+
+static unsigned long disp_clk_regs[] __initdata = {
+	MUX_SEL_DISP0,
+	MUX_SEL_DISP1,
+	MUX_SEL_DISP2,
+	MUX_SEL_DISP3,
+	MUX_SEL_DISP4,
+	DIV_DISP,
+	EN_ACLK_DISP,
+	EN_PCLK_DISP,
+	EN_SCLK_DISP0,
+	EN_SCLK_DISP1,
+	EN_IP_DISP,
+	EN_IP_DISP_BUS,
+};
+
+PNAME(mout_phyclk_dptx_phy_ch3_txd_clk_user_p) = {"fin_pll",
+			"phyclk_dptx_phy_ch3_txd_clk"};
+PNAME(mout_phyclk_dptx_phy_ch2_txd_clk_user_p) = {"fin_pll",
+			"phyclk_dptx_phy_ch2_txd_clk"};
+PNAME(mout_phyclk_dptx_phy_ch1_txd_clk_user_p) = {"fin_pll",
+			"phyclk_dptx_phy_ch1_txd_clk"};
+PNAME(mout_phyclk_dptx_phy_ch0_txd_clk_user_p) = {"fin_pll",
+			"phyclk_dptx_phy_ch0_txd_clk"};
+PNAME(mout_aclk_disp_222_user_p) = {"fin_pll", "dout_aclk_disp_222"};
+PNAME(mout_sclk_disp_pixel_user_p) = {"fin_pll", "dout_sclk_disp_pixel"};
+PNAME(mout_aclk_disp_333_user_p) = {"fin_pll", "dout_aclk_disp_333"};
+PNAME(mout_phyclk_hdmi_phy_tmds_clko_user_p) = {"fin_pll",
+			"phyclk_hdmi_phy_tmds_clko"};
+PNAME(mout_phyclk_hdmi_phy_ref_clko_user_p) = {"fin_pll",
+			"phyclk_hdmi_phy_ref_clko"};
+PNAME(mout_phyclk_hdmi_phy_pixel_clko_user_p) = {"fin_pll",
+			"phyclk_hdmi_phy_pixel_clko"};
+PNAME(mout_phyclk_hdmi_link_o_tmds_clkhi_user_p) = {"fin_pll",
+			"phyclk_hdmi_link_o_tmds_clkhi"};
+PNAME(mout_phyclk_mipi_dphy_4l_m_txbyte_clkhs_p) = {"fin_pll",
+			"phyclk_mipi_dphy_4l_m_txbyte_clkhs"};
+PNAME(mout_phyclk_dptx_phy_o_ref_clk_24m_user_p) = {"fin_pll",
+			"phyclk_dptx_phy_o_ref_clk_24m"};
+PNAME(mout_phyclk_dptx_phy_clk_div2_user_p) = {"fin_pll",
+			"phyclk_dptx_phy_clk_div2"};
+PNAME(mout_sclk_hdmi_pixel_p) = {"mout_sclk_disp_pixel_user",
+			"mout_aclk_disp_222_user"};
+PNAME(mout_phyclk_mipi_dphy_4lmrxclk_esc0_user_p) = {"fin_pll",
+			"phyclk_mipi_dphy_4l_m_rxclkesc0"};
+PNAME(mout_sclk_hdmi_spdif_p) = {"fin_pll", "ioclk_spdif_extclk",
+			"dout_aclk_peri_aud", "phyclk_hdmi_phy_ref_cko"};
+
+struct samsung_mux_clock disp_mux_clks[] __initdata = {
+	MUX(DISP_MOUT_ACLK_DISP_333_USER, "mout_aclk_disp_333_user",
+			mout_aclk_disp_333_user_p,
+			MUX_SEL_DISP0, 0, 1),
+	MUX(DISP_MOUT_SCLK_DISP_PIXEL_USER, "mout_sclk_disp_pixel_user",
+			mout_sclk_disp_pixel_user_p,
+			MUX_SEL_DISP0, 4, 1),
+	MUX(DISP_MOUT_ACLK_DISP_222_USER, "mout_aclk_disp_222_user",
+			mout_aclk_disp_222_user_p,
+			MUX_SEL_DISP0, 8, 1),
+	MUX(DISP_MOUT_PHYCLK_DPTX_PHY_CH0_TXD_CLK_USER,
+			"mout_phyclk_dptx_phy_ch0_txd_clk_user",
+			mout_phyclk_dptx_phy_ch0_txd_clk_user_p,
+			MUX_SEL_DISP0, 16, 1),
+	MUX(DISP_MOUT_PHYCLK_DPTX_PHY_CH1_TXD_CLK_USER,
+			"mout_phyclk_dptx_phy_ch1_txd_clk_user",
+			mout_phyclk_dptx_phy_ch1_txd_clk_user_p,
+			MUX_SEL_DISP0, 20, 1),
+	MUX(DISP_MOUT_PHYCLK_DPTX_PHY_CH2_TXD_CLK_USER,
+			"mout_phyclk_dptx_phy_ch2_txd_clk_user",
+			mout_phyclk_dptx_phy_ch2_txd_clk_user_p,
+			MUX_SEL_DISP0, 24, 1),
+	MUX(DISP_MOUT_PHYCLK_DPTX_PHY_CH3_TXD_CLK_USER,
+			"mout_phyclk_dptx_phy_ch3_txd_clk_user",
+			mout_phyclk_dptx_phy_ch3_txd_clk_user_p,
+			MUX_SEL_DISP0, 28, 1),
+
+	MUX(DISP_MOUT_PHYCLK_DPTX_PHY_CLK_DIV2_USER,
+			"mout_phyclk_dptx_phy_clk_div2_user",
+			mout_phyclk_dptx_phy_clk_div2_user_p,
+			MUX_SEL_DISP1, 0, 1),
+	MUX(DISP_MOUT_PHYCLK_DPTX_PHY_O_REF_CLK_24M_USER,
+			"mout_phyclk_dptx_phy_o_ref_clk_24m_user",
+			mout_phyclk_dptx_phy_o_ref_clk_24m_user_p,
+			MUX_SEL_DISP1, 4, 1),
+	MUX(DISP_MOUT_PHYCLK_MIPI_DPHY_4L_M_TXBYTE_CLKHS,
+			"mout_phyclk_mipi_dphy_4l_m_txbyte_clkhs",
+			mout_phyclk_mipi_dphy_4l_m_txbyte_clkhs_p,
+			MUX_SEL_DISP1, 8, 1),
+	MUX(DISP_MOUT_PHYCLK_HDMI_LINK_O_TMDS_CLKHI_USER,
+			"mout_phyclk_hdmi_link_o_tmds_clkhi_user",
+			mout_phyclk_hdmi_link_o_tmds_clkhi_user_p,
+			MUX_SEL_DISP1, 16, 1),
+	MUX(DISP_MOUT_HDMI_PHY_PIXEL,
+			"mout_phyclk_hdmi_phy_pixel_clko_user",
+			mout_phyclk_hdmi_phy_pixel_clko_user_p,
+			MUX_SEL_DISP1, 20, 1),
+	MUX(DISP_MOUT_PHYCLK_HDMI_PHY_REF_CLKO_USER,
+			"mout_phyclk_hdmi_phy_ref_clko_user",
+			mout_phyclk_hdmi_phy_ref_clko_user_p,
+			MUX_SEL_DISP1, 24, 1),
+	MUX(DISP_MOUT_PHYCLK_HDMI_PHY_TMDS_CLKO_USER,
+			"mout_phyclk_hdmi_phy_tmds_clko_user",
+			mout_phyclk_hdmi_phy_tmds_clko_user_p,
+			MUX_SEL_DISP1, 28, 1),
+
+	MUX(DISP_MOUT_PHYCLK_MIPI_DPHY_4LMRXCLK_ESC0_USER,
+			"mout_phyclk_mipi_dphy_4lmrxclk_esc0_user",
+			mout_phyclk_mipi_dphy_4lmrxclk_esc0_user_p,
+			MUX_SEL_DISP2, 0, 1),
+	MUX(DISP_MOUT_SCLK_HDMI_PIXEL, "mout_sclk_hdmi_pixel",
+			mout_sclk_hdmi_pixel_p,
+			MUX_SEL_DISP2, 4, 1),
+
+	MUX(DISP_MOUT_SCLK_HDMI_SPDIF, "mout_sclk_hdmi_spdif",
+			mout_sclk_hdmi_spdif_p,
+			MUX_SEL_DISP4, 4, 2),
+};
+
+struct samsung_div_clock disp_div_clks[] __initdata = {
+	DIV(DISP_DOUT_PCLK_DISP_111, "dout_pclk_disp_111",
+			"mout_aclk_disp_222_user",
+			DIV_DISP, 8, 4),
+	DIV(DISP_DOUT_SCLK_FIMD1_EXTCLKPLL, "dout_sclk_fimd1_extclkpll",
+			"mout_sclk_disp_pixel_user",
+			DIV_DISP, 12, 4),
+	DIV(DISP_DOUT_SCLK_HDMI_PHY_PIXEL_CLKI,
+			"dout_sclk_hdmi_phy_pixel_clki",
+			"mout_sclk_hdmi_pixel",
+			DIV_DISP, 16, 4),
+};
+
+struct samsung_gate_clock disp_gate_clks[] __initdata = {
+	GATE(DISP_MOUT_HDMI_PHY_PIXEL_USER, "sclk_hdmi_link_i_pixel",
+			"mout_phyclk_hdmi_phy_pixel_clko_user",
+			EN_SCLK_DISP0, 26, CLK_SET_RATE_PARENT, 0),
+	GATE(DISP_SCLK_PIXEL, "sclk_hdmi_phy_pixel_clki",
+			"dout_sclk_hdmi_phy_pixel_clki",
+			EN_SCLK_DISP0, 29, CLK_SET_RATE_PARENT, 0),
+
+	GATE(DISP_CLK_DP, "clk_dptx_link", "mout_aclk_disp_222_user",
+			EN_IP_DISP, 4, 0, 0),
+	GATE(DISP_CLK_DPPHY, "clk_dptx_phy", "mout_aclk_disp_222_user",
+			EN_IP_DISP, 5, 0, 0),
+	GATE(DISP_CLK_DSIM1, "clk_dsim1", "mout_aclk_disp_222_user",
+			EN_IP_DISP, 6, 0, 0),
+	GATE(DISP_CLK_FIMD1, "clk_fimd1", "mout_aclk_disp_222_user",
+			EN_IP_DISP, 7, 0, 0),
+	GATE(DISP_CLK_HDMI, "clk_hdmi", "mout_aclk_disp_222_user",
+			EN_IP_DISP, 8, 0, 0),
+	GATE(DISP_CLK_HDMIPHY, "clk_hdmiphy", "mout_aclk_disp_222_user",
+			EN_IP_DISP, 9, 0, 0),
+	GATE(DISP_CLK_MIPIPHY, "clk_mipi_dphy", "mout_aclk_disp_222_user",
+			EN_IP_DISP, 10, 0, 0),
+	GATE(DISP_CLK_MIXER, "clk_mixer", "mout_aclk_disp_222_user",
+			EN_IP_DISP, 11, 0, 0),
+	GATE(DISP_CLK_PIXEL_DISP, "clk_pixel_disp", "mout_aclk_disp_222_user",
+			EN_IP_DISP, 12, CLK_IGNORE_UNUSED, 0),
+	GATE(DISP_CLK_PIXEL_MIXER, "clk_pixel_mixer", "mout_aclk_disp_222_user",
+			EN_IP_DISP, 13, CLK_IGNORE_UNUSED, 0),
+	GATE(DISP_CLK_SMMU_FIMD1M0, "clk_smmu3_fimd1m0",
+			"mout_aclk_disp_222_user",
+			EN_IP_DISP, 22, 0, 0),
+	GATE(DISP_CLK_SMMU_FIMD1M1, "clk_smmu3_fimd1m1",
+			"mout_aclk_disp_222_user",
+			EN_IP_DISP, 23, 0, 0),
+	GATE(DISP_CLK_SMMU_TV, "clk_smmu3_tv", "mout_aclk_disp_222_user",
+			EN_IP_DISP, 25, 0, 0),
+};
+
+static void __init exynos5260_clk_disp_init(struct device_node *np)
+{
+	struct exynos5260_cmu_info cmu = {0};
+
+	cmu.mux_clks = disp_mux_clks;
+	cmu.nr_mux_clks = ARRAY_SIZE(disp_mux_clks);
+	cmu.div_clks = disp_div_clks;
+	cmu.nr_div_clks = ARRAY_SIZE(disp_div_clks);
+	cmu.gate_clks = disp_gate_clks;
+	cmu.nr_gate_clks = ARRAY_SIZE(disp_gate_clks);
+	cmu.nr_clk_ids = DISP_NR_CLK;
+	cmu.clk_regs = disp_clk_regs;
+	cmu.nr_clk_regs = ARRAY_SIZE(disp_clk_regs);
+
+	exynos5260_cmu_register_one(np, &cmu);
+}
+
+CLK_OF_DECLARE(exynos5260_clk_disp, "samsung,exynos5260-clock-disp",
+		exynos5260_clk_disp_init);
+
+
+/* CMU_EGL */
+
+static unsigned long egl_clk_regs[] __initdata = {
+	EGL_PLL_LOCK,
+	EGL_PLL_CON0,
+	EGL_PLL_CON1,
+	EGL_PLL_FREQ_DET,
+	MUX_SEL_EGL,
+	MUX_ENABLE_EGL,
+	DIV_EGL,
+	DIV_EGL_PLL_FDET,
+	EN_ACLK_EGL,
+	EN_PCLK_EGL,
+	EN_SCLK_EGL,
+};
+
+PNAME(mout_egl_b_p) = {"mout_egl_pll", "dout_bus_pll"};
+PNAME(mout_egl_pll_p) = {"fin_pll", "fout_egl_pll"};
+
+struct samsung_mux_clock egl_mux_clks[] __initdata = {
+	MUX(EGL_MOUT_EGL_PLL, "mout_egl_pll", mout_egl_pll_p,
+			MUX_SEL_EGL, 4, 1),
+	MUX(EGL_MOUT_EGL_B, "mout_egl_b", mout_egl_b_p, MUX_SEL_EGL, 16, 1),
+};
+
+struct samsung_div_clock egl_div_clks[] __initdata = {
+	DIV(EGL_DOUT_EGL1, "dout_egl1", "mout_egl_b", DIV_EGL, 0, 3),
+	DIV(EGL_DOUT_EGL2, "dout_egl2", "dout_egl1", DIV_EGL, 4, 3),
+	DIV(EGL_DOUT_ACLK_EGL, "dout_aclk_egl", "dout_egl2", DIV_EGL, 8, 3),
+	DIV(EGL_DOUT_PCLK_EGL, "dout_pclk_egl", "dout_egl_atclk",
+			DIV_EGL, 12, 3),
+	DIV(EGL_DOUT_EGL_ATCLK, "dout_egl_atclk", "dout_egl2", DIV_EGL, 16, 3),
+	DIV(EGL_DOUT_EGL_PCLK_DBG, "dout_egl_pclk_dbg", "dout_egl_atclk",
+			DIV_EGL, 20, 3),
+	DIV(EGL_DOUT_EGL_PLL, "dout_egl_pll", "mout_egl_b", DIV_EGL, 24, 3),
+};
+
+static struct samsung_pll_clock egl_pll_clks[] __initdata = {
+	PLL(pll_2550xx, EGL_FOUT_EGL_PLL, "fout_egl_pll", "fin_pll",
+		EGL_PLL_LOCK, EGL_PLL_CON0,
+		pll2550_24mhz_tbl),
+};
+
+static void __init exynos5260_clk_egl_init(struct device_node *np)
+{
+	struct exynos5260_cmu_info cmu = {0};
+
+	cmu.pll_clks = egl_pll_clks;
+	cmu.nr_pll_clks =  ARRAY_SIZE(egl_pll_clks);
+	cmu.mux_clks = egl_mux_clks;
+	cmu.nr_mux_clks = ARRAY_SIZE(egl_mux_clks);
+	cmu.div_clks = egl_div_clks;
+	cmu.nr_div_clks = ARRAY_SIZE(egl_div_clks);
+	cmu.nr_clk_ids = EGL_NR_CLK;
+	cmu.clk_regs = egl_clk_regs;
+	cmu.nr_clk_regs = ARRAY_SIZE(egl_clk_regs);
+
+	exynos5260_cmu_register_one(np, &cmu);
+}
+
+CLK_OF_DECLARE(exynos5260_clk_egl, "samsung,exynos5260-clock-egl",
+		exynos5260_clk_egl_init);
+
+
+/* CMU_FSYS */
+
+static unsigned long fsys_clk_regs[] __initdata = {
+	MUX_SEL_FSYS0,
+	MUX_SEL_FSYS1,
+	EN_ACLK_FSYS,
+	EN_ACLK_FSYS_SECURE_RTIC,
+	EN_ACLK_FSYS_SECURE_SMMU_RTIC,
+	EN_SCLK_FSYS,
+	EN_IP_FSYS,
+	EN_IP_FSYS_SECURE_RTIC,
+	EN_IP_FSYS_SECURE_SMMU_RTIC,
+};
+
+PNAME(mout_phyclk_usbhost20_phyclk_user_p) = {"fin_pll",
+			"phyclk_usbhost20_phy_phyclock"};
+PNAME(mout_phyclk_usbhost20_freeclk_user_p) = {"fin_pll",
+			"phyclk_usbhost20_phy_freeclk"};
+PNAME(mout_phyclk_usbhost20_clk48mohci_user_p) = {"fin_pll",
+			"phyclk_usbhost20_phy_clk48mohci"};
+PNAME(mout_phyclk_usbdrd30_pipe_pclk_user_p) = {"fin_pll",
+			"phyclk_usbdrd30_udrd30_pipe_pclk"};
+PNAME(mout_phyclk_usbdrd30_phyclock_user_p) = {"fin_pll",
+			"phyclk_usbdrd30_udrd30_phyclock"};
+
+struct samsung_mux_clock fsys_mux_clks[] __initdata = {
+	MUX(FSYS_MOUT_PHYCLK_USBDRD30_PHYCLOCK_USER,
+			"mout_phyclk_usbdrd30_phyclock_user",
+			mout_phyclk_usbdrd30_phyclock_user_p,
+			MUX_SEL_FSYS1, 0, 1),
+	MUX(FSYS_MOUT_PHYCLK_USBDRD30_PIPE_PCLK_USER,
+			"mout_phyclk_usbdrd30_pipe_pclk_user",
+			mout_phyclk_usbdrd30_pipe_pclk_user_p,
+			MUX_SEL_FSYS1, 4, 1),
+	MUX(FSYS_MOUT_PHYCLK_USBHOST20_CLK48MOHCI_USER,
+			"mout_phyclk_usbhost20_clk48mohci_user",
+			mout_phyclk_usbhost20_clk48mohci_user_p,
+			MUX_SEL_FSYS1, 8, 1),
+	MUX(FSYS_MOUT_PHYCLK_USBHOST20_FREECLK_USER,
+			"mout_phyclk_usbhost20_freeclk_user",
+			mout_phyclk_usbhost20_freeclk_user_p,
+			MUX_SEL_FSYS1, 12, 1),
+	MUX(FSYS_MOUT_PHYCLK_USBHOST20_PHYCLK_USER,
+			"mout_phyclk_usbhost20_phyclk_user",
+			mout_phyclk_usbhost20_phyclk_user_p,
+			MUX_SEL_FSYS1, 16, 1),
+};
+
+struct samsung_gate_clock fsys_gate_clks[] __initdata = {
+	GATE(FSYS_PHYCLK_USBHOST20, "phyclk_usbhost20_phyclock",
+			"mout_phyclk_usbdrd30_phyclock_user",
+			EN_SCLK_FSYS, 1, 0, 0),
+	GATE(FSYS_PHYCLK_USBDRD30, "phyclk_usbdrd30_udrd30_phyclock_g",
+			"mout_phyclk_usbdrd30_phyclock_user",
+			EN_SCLK_FSYS, 7, 0, 0),
+
+	GATE(FSYS_CLK_MMC0, "clk_mmc0", "dout_aclk_fsys_200",
+			EN_IP_FSYS, 6, 0, 0),
+	GATE(FSYS_CLK_MMC1, "clk_mmc1", "dout_aclk_fsys_200",
+			EN_IP_FSYS, 7, 0, 0),
+	GATE(FSYS_CLK_MMC2, "clk_mmc2", "dout_aclk_fsys_200",
+			EN_IP_FSYS, 8, 0, 0),
+	GATE(FSYS_CLK_PDMA, "clk_pdma", "dout_aclk_fsys_200",
+			EN_IP_FSYS, 9, 0, 0),
+	GATE(FSYS_CLK_SROMC, "clk_sromc", "dout_aclk_fsys_200",
+			EN_IP_FSYS, 13, 0, 0),
+	GATE(FSYS_CLK_USBDRD30, "clk_usbdrd30", "dout_aclk_fsys_200",
+			EN_IP_FSYS, 14, 0, 0),
+	GATE(FSYS_CLK_USBHOST20, "clk_usbhost20", "dout_aclk_fsys_200",
+			EN_IP_FSYS, 15, 0, 0),
+	GATE(FSYS_CLK_USBLINK, "clk_usblink", "dout_aclk_fsys_200",
+			EN_IP_FSYS, 18, 0, 0),
+	GATE(FSYS_CLK_TSI, "clk_tsi", "dout_aclk_fsys_200",
+			EN_IP_FSYS, 20, 0, 0),
+
+	GATE(FSYS_CLK_RTIC, "clk_rtic", "dout_aclk_fsys_200",
+			EN_IP_FSYS_SECURE_RTIC, 11, 0, 0),
+	GATE(FSYS_CLK_SMMU_RTIC, "clk_smmu_rtic", "dout_aclk_fsys_200",
+			EN_IP_FSYS_SECURE_SMMU_RTIC, 12, 0, 0),
+};
+
+static void __init exynos5260_clk_fsys_init(struct device_node *np)
+{
+	struct exynos5260_cmu_info cmu = {0};
+
+	cmu.mux_clks = fsys_mux_clks;
+	cmu.nr_mux_clks = ARRAY_SIZE(fsys_mux_clks);
+	cmu.gate_clks = fsys_gate_clks;
+	cmu.nr_gate_clks = ARRAY_SIZE(fsys_gate_clks);
+	cmu.nr_clk_ids = FSYS_NR_CLK;
+	cmu.clk_regs = fsys_clk_regs;
+	cmu.nr_clk_regs = ARRAY_SIZE(fsys_clk_regs);
+
+	exynos5260_cmu_register_one(np, &cmu);
+}
+
+CLK_OF_DECLARE(exynos5260_clk_fsys, "samsung,exynos5260-clock-fsys",
+		exynos5260_clk_fsys_init);
+
+
+/* CMU_G2D */
+
+static unsigned long g2d_clk_regs[] __initdata = {
+	MUX_SEL_G2D,
+	MUX_STAT_G2D,
+	DIV_G2D,
+	EN_ACLK_G2D,
+	EN_ACLK_G2D_SECURE_SSS,
+	EN_ACLK_G2D_SECURE_SLIM_SSS,
+	EN_ACLK_G2D_SECURE_SMMU_SLIM_SSS,
+	EN_ACLK_G2D_SECURE_SMMU_SSS,
+	EN_ACLK_G2D_SECURE_SMMU_MDMA,
+	EN_ACLK_G2D_SECURE_SMMU_G2D,
+	EN_PCLK_G2D,
+	EN_PCLK_G2D_SECURE_SMMU_SLIM_SSS,
+	EN_PCLK_G2D_SECURE_SMMU_SSS,
+	EN_PCLK_G2D_SECURE_SMMU_MDMA,
+	EN_PCLK_G2D_SECURE_SMMU_G2D,
+	EN_IP_G2D,
+	EN_IP_G2D_SECURE_SSS,
+	EN_IP_G2D_SECURE_SLIM_SSS,
+	EN_IP_G2D_SECURE_SMMU_SLIM_SSS,
+	EN_IP_G2D_SECURE_SMMU_SSS,
+	EN_IP_G2D_SECURE_SMMU_MDMA,
+	EN_IP_G2D_SECURE_SMMU_G2D,
+};
+
+PNAME(mout_aclk_g2d_333_user_p) = {"fin_pll", "dout_aclk_g2d_333"};
+
+struct samsung_mux_clock g2d_mux_clks[] __initdata = {
+	MUX(G2D_MOUT_ACLK_G2D_333_USER, "mout_aclk_g2d_333_user",
+			mout_aclk_g2d_333_user_p,
+			MUX_SEL_G2D, 0, 1),
+};
+
+struct samsung_div_clock g2d_div_clks[] __initdata = {
+	DIV(G2D_DOUT_PCLK_G2D_83, "dout_pclk_g2d_83", "mout_aclk_g2d_333_user",
+			DIV_G2D, 0, 3),
+};
+
+struct samsung_gate_clock g2d_gate_clks[] __initdata = {
+	GATE(G2D_CLK_G2D, "clk_g2d", "mout_aclk_g2d_333_user",
+			EN_IP_G2D, 4, 0, 0),
+	GATE(G2D_CLK_JPEG, "clk_jpeg", "mout_aclk_g2d_333_user",
+			EN_IP_G2D, 5, 0, 0),
+	GATE(G2D_CLK_MDMA, "clk_mdma", "mout_aclk_g2d_333_user",
+			EN_IP_G2D, 6, 0, 0),
+	GATE(G2D_CLK_SMMU3_JPEG, "clk_smmu3_jpeg", "mout_aclk_g2d_333_user",
+			EN_IP_G2D, 16, 0, 0),
+
+	GATE(G2D_CLK_SSS, "clk_sss", "mout_aclk_g2d_333_user",
+			EN_IP_G2D_SECURE_SSS, 17, 0, 0),
+
+	GATE(G2D_CLK_SLIM_SSS, "clk_slim_sss", "mout_aclk_g2d_333_user",
+			EN_IP_G2D_SECURE_SLIM_SSS, 11, 0, 0),
+
+	GATE(G2D_CLK_SMMU_SLIM_SSS, "clk_smmu_slim_sss",
+			"mout_aclk_g2d_333_user",
+			EN_IP_G2D_SECURE_SMMU_SLIM_SSS, 13, 0, 0),
+
+	GATE(G2D_CLK_SMMU_SSS, "clk_smmu_sss", "mout_aclk_g2d_333_user",
+			EN_IP_G2D_SECURE_SMMU_SSS, 14, 0, 0),
+
+	GATE(G2D_CLK_SMMU_MDMA, "clk_smmu_mdma", "mout_aclk_g2d_333_user",
+			EN_IP_G2D_SECURE_SMMU_MDMA, 12, 0, 0),
+
+	GATE(G2D_CLK_SMMU3_G2D, "clk_smmu3_g2d", "mout_aclk_g2d_333_user",
+			EN_IP_G2D_SECURE_SMMU_G2D, 15, 0, 0),
+};
+
+static void __init exynos5260_clk_g2d_init(struct device_node *np)
+{
+	struct exynos5260_cmu_info cmu = {0};
+
+	cmu.mux_clks = g2d_mux_clks;
+	cmu.nr_mux_clks = ARRAY_SIZE(g2d_mux_clks);
+	cmu.div_clks = g2d_div_clks;
+	cmu.nr_div_clks = ARRAY_SIZE(g2d_div_clks);
+	cmu.gate_clks = g2d_gate_clks;
+	cmu.nr_gate_clks = ARRAY_SIZE(g2d_gate_clks);
+	cmu.nr_clk_ids = G2D_NR_CLK;
+	cmu.clk_regs = g2d_clk_regs;
+	cmu.nr_clk_regs = ARRAY_SIZE(g2d_clk_regs);
+
+	exynos5260_cmu_register_one(np, &cmu);
+}
+
+CLK_OF_DECLARE(exynos5260_clk_g2d, "samsung,exynos5260-clock-g2d",
+		exynos5260_clk_g2d_init);
+
+
+/* CMU_G3D */
+
+static unsigned long g3d_clk_regs[] __initdata = {
+	G3D_PLL_LOCK,
+	G3D_PLL_CON0,
+	G3D_PLL_CON1,
+	G3D_PLL_FDET,
+	MUX_SEL_G3D,
+	DIV_G3D,
+	DIV_G3D_PLL_FDET,
+	EN_ACLK_G3D,
+	EN_PCLK_G3D,
+	EN_SCLK_G3D,
+	EN_IP_G3D,
+};
+
+PNAME(mout_g3d_pll_p) = {"fin_pll", "fout_g3d_pll"};
+
+struct samsung_mux_clock g3d_mux_clks[] __initdata = {
+	MUX(G3D_MOUT_G3D_PLL, "mout_g3d_pll", mout_g3d_pll_p,
+			MUX_SEL_G3D, 0, 1),
+};
+
+struct samsung_div_clock g3d_div_clks[] __initdata = {
+	DIV(G3D_DOUT_PCLK_G3D, "dout_pclk_g3d", "dout_aclk_g3d", DIV_G3D, 0, 3),
+	DIV(G3D_DOUT_ACLK_G3D, "dout_aclk_g3d", "mout_g3d_pll", DIV_G3D, 4, 3),
+};
+
+struct samsung_gate_clock g3d_gate_clks[] __initdata = {
+	GATE(G3D_CLK_G3D, "clk_g3d", "dout_aclk_g3d", EN_IP_G3D, 2, 0, 0),
+	GATE(G3D_CLK_G3D_HPM, "clk_g3d_hpm", "dout_aclk_g3d",
+			EN_IP_G3D, 3, 0, 0),
+};
+
+static struct samsung_pll_clock g3d_pll_clks[] __initdata = {
+	PLL(pll_2550, G3D_FOUT_G3D_PLL, "fout_g3d_pll", "fin_pll",
+		G3D_PLL_LOCK, G3D_PLL_CON0,
+		pll2550_24mhz_tbl),
+};
+
+static void __init exynos5260_clk_g3d_init(struct device_node *np)
+{
+	struct exynos5260_cmu_info cmu = {0};
+
+	cmu.pll_clks = g3d_pll_clks;
+	cmu.nr_pll_clks =  ARRAY_SIZE(g3d_pll_clks);
+	cmu.mux_clks = g3d_mux_clks;
+	cmu.nr_mux_clks = ARRAY_SIZE(g3d_mux_clks);
+	cmu.div_clks = g3d_div_clks;
+	cmu.nr_div_clks = ARRAY_SIZE(g3d_div_clks);
+	cmu.gate_clks = g3d_gate_clks;
+	cmu.nr_gate_clks = ARRAY_SIZE(g3d_gate_clks);
+	cmu.nr_clk_ids = G3D_NR_CLK;
+	cmu.clk_regs = g3d_clk_regs;
+	cmu.nr_clk_regs = ARRAY_SIZE(g3d_clk_regs);
+
+	exynos5260_cmu_register_one(np, &cmu);
+}
+
+CLK_OF_DECLARE(exynos5260_clk_g3d, "samsung,exynos5260-clock-g3d",
+		exynos5260_clk_g3d_init);
+
+
+/* CMU_GSCL */
+
+static unsigned long gscl_clk_regs[] __initdata = {
+	MUX_SEL_GSCL,
+	DIV_GSCL,
+	EN_ACLK_GSCL,
+	EN_ACLK_GSCL_FIMC,
+	EN_ACLK_GSCL_SECURE_SMMU_GSCL0,
+	EN_ACLK_GSCL_SECURE_SMMU_GSCL1,
+	EN_ACLK_GSCL_SECURE_SMMU_MSCL0,
+	EN_ACLK_GSCL_SECURE_SMMU_MSCL1,
+	EN_PCLK_GSCL,
+	EN_PCLK_GSCL_FIMC,
+	EN_PCLK_GSCL_SECURE_SMMU_GSCL0,
+	EN_PCLK_GSCL_SECURE_SMMU_GSCL1,
+	EN_PCLK_GSCL_SECURE_SMMU_MSCL0,
+	EN_PCLK_GSCL_SECURE_SMMU_MSCL1,
+	EN_SCLK_GSCL,
+	EN_SCLK_GSCL_FIMC,
+	EN_IP_GSCL,
+	EN_IP_GSCL_FIMC,
+	EN_IP_GSCL_SECURE_SMMU_GSCL0,
+	EN_IP_GSCL_SECURE_SMMU_GSCL1,
+	EN_IP_GSCL_SECURE_SMMU_MSCL0,
+	EN_IP_GSCL_SECURE_SMMU_MSCL1,
+};
+
+PNAME(mout_aclk_gscl_333_user_p) = {"fin_pll", "dout_aclk_gscl_333"};
+PNAME(mout_aclk_m2m_400_user_p) = {"fin_pll", "dout_aclk_gscl_400"};
+PNAME(mout_aclk_gscl_fimc_user_p) = {"fin_pll", "dout_aclk_gscl_400"};
+PNAME(mout_aclk_csis_p) = {"dout_aclk_csis_200", "mout_aclk_gscl_fimc_user"};
+
+struct samsung_mux_clock gscl_mux_clks[] __initdata = {
+	MUX(GSCL_MOUT_ACLK_GSCL_333_USER, "mout_aclk_gscl_333_user",
+			mout_aclk_gscl_333_user_p,
+			MUX_SEL_GSCL, 0, 1),
+	MUX(GSCL_MOUT_ACLK_M2M_400_USER, "mout_aclk_m2m_400_user",
+			mout_aclk_m2m_400_user_p,
+			MUX_SEL_GSCL, 4, 1),
+	MUX(GSCL_MOUT_ACLK_GSCL_FIMC_USER, "mout_aclk_gscl_fimc_user",
+			mout_aclk_gscl_fimc_user_p,
+			MUX_SEL_GSCL, 8, 1),
+	MUX(GSCL_MOUT_ACLK_CSIS, "mout_aclk_csis", mout_aclk_csis_p,
+			MUX_SEL_GSCL, 24, 1),
+};
+
+struct samsung_div_clock gscl_div_clks[] __initdata = {
+	DIV(GSCL_DOUT_PCLK_M2M_100, "dout_pclk_m2m_100",
+			"mout_aclk_m2m_400_user",
+			DIV_GSCL, 0, 3),
+	DIV(GSCL_DOUT_ACLK_CSIS_200, "dout_aclk_csis_200",
+			"mout_aclk_m2m_400_user",
+			DIV_GSCL, 4, 3),
+};
+
+struct samsung_gate_clock gscl_gate_clks[] __initdata = {
+	GATE(GSCL_SCLK_CSIS0_WRAP, "sclk_csis0_wrap", "dout_aclk_csis_200",
+			EN_SCLK_GSCL_FIMC, 0, CLK_SET_RATE_PARENT, 0),
+	GATE(GSCL_SCLK_CSIS1_WRAP, "sclk_csis1_wrap", "dout_aclk_csis_200",
+			EN_SCLK_GSCL_FIMC, 1, CLK_SET_RATE_PARENT, 0),
+
+	GATE(GSCL_CLK_GSCL0, "clk_gscl0", "mout_aclk_gscl_333_user",
+			EN_IP_GSCL, 2, 0, 0),
+	GATE(GSCL_CLK_GSCL1, "clk_gscl1", "mout_aclk_gscl_333_user",
+			EN_IP_GSCL, 3, 0, 0),
+	GATE(GSCL_CLK_MSCL0, "clk_mscl0", "mout_aclk_gscl_333_user",
+			EN_IP_GSCL, 4, 0, 0),
+	GATE(GSCL_CLK_MSCL1, "clk_mscl1", "mout_aclk_gscl_333_user",
+			EN_IP_GSCL, 5, 0, 0),
+	GATE(GSCL_CLK_PIXEL_GSCL0, "clk_pixel_gscl0",
+			"mout_aclk_gscl_333_user",
+			EN_IP_GSCL, 8, 0, 0),
+	GATE(GSCL_CLK_PIXEL_GSCL1, "clk_pixel_gscl1",
+			"mout_aclk_gscl_333_user",
+			EN_IP_GSCL, 9, 0, 0),
+
+	GATE(GSCL_CLK_SMMU3_LITE_A, "clk_smmu3_lite_a",
+			"mout_aclk_gscl_fimc_user",
+			EN_IP_GSCL_FIMC, 5, 0, 0),
+	GATE(GSCL_CLK_SMMU3_LITE_B, "clk_smmu3_lite_b",
+			"mout_aclk_gscl_fimc_user",
+			EN_IP_GSCL_FIMC, 6, 0, 0),
+	GATE(GSCL_CLK_SMMU3_LITE_D, "clk_smmu3_lite_d",
+			"mout_aclk_gscl_fimc_user",
+			EN_IP_GSCL_FIMC, 7, 0, 0),
+	GATE(GSCL_CLK_CSIS0, "clk_csis0", "mout_aclk_gscl_fimc_user",
+			EN_IP_GSCL_FIMC, 8, 0, 0),
+	GATE(GSCL_CLK_CSIS1, "clk_csis1", "mout_aclk_gscl_fimc_user",
+			EN_IP_GSCL_FIMC, 9, 0, 0),
+	GATE(GSCL_CLK_FIMC_LITE_A, "clk_fimc_lite_a",
+			"mout_aclk_gscl_fimc_user",
+			EN_IP_GSCL_FIMC, 10, 0, 0),
+	GATE(GSCL_CLK_FIMC_LITE_B, "clk_fimc_lite_b",
+			"mout_aclk_gscl_fimc_user",
+			EN_IP_GSCL_FIMC, 11, 0, 0),
+	GATE(GSCL_CLK_FIMC_LITE_D, "clk_fimc_lite_d",
+			"mout_aclk_gscl_fimc_user",
+			EN_IP_GSCL_FIMC, 12, 0, 0),
+
+	GATE(GSCL_CLK_SMMU3_GSCL0, "clk_smmu3_gscl0",
+			"mout_aclk_gscl_333_user",
+			EN_IP_GSCL_SECURE_SMMU_GSCL0, 17, 0, 0),
+	GATE(GSCL_CLK_SMMU3_GSCL1, "clk_smmu3_gscl1", "mout_aclk_gscl_333_user",
+			EN_IP_GSCL_SECURE_SMMU_GSCL1, 18, 0, 0),
+	GATE(GSCL_CLK_SMMU3_MSCL0, "clk_smmu3_mscl0",
+			"mout_aclk_m2m_400_user",
+			EN_IP_GSCL_SECURE_SMMU_MSCL0, 19, 0, 0),
+	GATE(GSCL_CLK_SMMU3_MSCL1, "clk_smmu3_mscl1",
+			"mout_aclk_m2m_400_user",
+			EN_IP_GSCL_SECURE_SMMU_MSCL1, 20, 0, 0),
+};
+
+static void __init exynos5260_clk_gscl_init(struct device_node *np)
+{
+	struct exynos5260_cmu_info cmu = {0};
+
+	cmu.mux_clks = gscl_mux_clks;
+	cmu.nr_mux_clks = ARRAY_SIZE(gscl_mux_clks);
+	cmu.div_clks = gscl_div_clks;
+	cmu.nr_div_clks = ARRAY_SIZE(gscl_div_clks);
+	cmu.gate_clks = gscl_gate_clks;
+	cmu.nr_gate_clks = ARRAY_SIZE(gscl_gate_clks);
+	cmu.nr_clk_ids = GSCL_NR_CLK;
+	cmu.clk_regs = gscl_clk_regs;
+	cmu.nr_clk_regs = ARRAY_SIZE(gscl_clk_regs);
+
+	exynos5260_cmu_register_one(np, &cmu);
+}
+
+CLK_OF_DECLARE(exynos5260_clk_gscl, "samsung,exynos5260-clock-gscl",
+		exynos5260_clk_gscl_init);
+
+
+/* CMU_ISP */
+
+static unsigned long isp_clk_regs[] __initdata = {
+	MUX_SEL_ISP0,
+	MUX_SEL_ISP1,
+	DIV_ISP,
+	EN_ACLK_ISP0,
+	EN_ACLK_ISP1,
+	EN_PCLK_ISP0,
+	EN_PCLK_ISP1,
+	EN_SCLK_ISP,
+	EN_IP_ISP0,
+	EN_IP_ISP1,
+};
+
+PNAME(mout_isp_400_user_p) = {"fin_pll", "dout_aclk_isp1_400"};
+PNAME(mout_isp_266_user_p)	 = {"fin_pll", "dout_aclk_isp1_266"};
+
+struct samsung_mux_clock isp_mux_clks[] __initdata = {
+	MUX(ISP_MOUT_ISP_266_USER, "mout_isp_266_user", mout_isp_266_user_p,
+			MUX_SEL_ISP0, 0, 1),
+	MUX(ISP_MOUT_ISP_400_USER, "mout_isp_400_user", mout_isp_400_user_p,
+			MUX_SEL_ISP0, 4, 1),
+};
+
+struct samsung_div_clock isp_div_clks[] __initdata = {
+	DIV(ISP_DOUT_PCLK_ISP_66, "dout_pclk_isp_66", "mout_kfc",
+			DIV_ISP, 0, 3),
+	DIV(ISP_DOUT_PCLK_ISP_133, "dout_pclk_isp_133", "mout_kfc",
+			DIV_ISP, 4, 4),
+	DIV(ISP_DOUT_CA5_ATCLKIN, "dout_ca5_atclkin", "mout_kfc",
+			DIV_ISP, 12, 3),
+	DIV(ISP_DOUT_CA5_PCLKDBG, "dout_ca5_pclkdbg", "mout_kfc",
+			DIV_ISP, 16, 4),
+	DIV(ISP_DOUT_SCLK_MPWM, "dout_sclk_mpwm", "mout_kfc", DIV_ISP, 20, 2),
+};
+
+struct samsung_gate_clock isp_gate_clks[] __initdata = {
+	GATE(ISP_CLK_GIC, "clk_isp_gic", "mout_aclk_isp1_266",
+			EN_IP_ISP0, 15, 0, 0),
+
+	GATE(ISP_CLK_CA5, "clk_isp_ca5", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 1, 0, 0),
+	GATE(ISP_CLK_FIMC_DRC, "clk_isp_fimc_drc", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 2, 0, 0),
+	GATE(ISP_CLK_FIMC_FD, "clk_isp_fimc_fd", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 3, 0, 0),
+	GATE(ISP_CLK_FIMC, "clk_isp_fimc", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 4, 0, 0),
+	GATE(ISP_CLK_FIMC_SCALERC, "clk_isp_fimc_scalerc",
+			"mout_aclk_isp1_266",
+			EN_IP_ISP1, 5, 0, 0),
+	GATE(ISP_CLK_FIMC_SCALERP, "clk_isp_fimc_scalerp",
+			"mout_aclk_isp1_266",
+			EN_IP_ISP1, 6, 0, 0),
+	GATE(ISP_CLK_I2C0, "clk_isp_i2c0", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 7, 0, 0),
+	GATE(ISP_CLK_I2C1, "clk_isp_i2c1", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 8, 0, 0),
+	GATE(ISP_CLK_MCUCTL, "clk_isp_mcuctl", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 9, 0, 0),
+	GATE(ISP_CLK_MPWM, "clk_isp_mpwm", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 10, 0, 0),
+	GATE(ISP_CLK_MTCADC, "clk_isp_mtcadc", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 11, 0, 0),
+	GATE(ISP_CLK_PWM, "clk_isp_pwm", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 14, 0, 0),
+	GATE(ISP_CLK_SMMU_DRC, "clk_smmu_drc", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 21, 0, 0),
+	GATE(ISP_CLK_SMMU_FD, "clk_smmu_fd", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 22, 0, 0),
+	GATE(ISP_CLK_SMMU_ISP, "clk_smmu_isp", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 23, 0, 0),
+	GATE(ISP_CLK_SMMU_ISPCX, "clk_smmu_ispcx", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 24, 0, 0),
+	GATE(ISP_CLK_SMMU_SCALERC, "clk_isp_smmu_scalerc",
+			"mout_aclk_isp1_266",
+			EN_IP_ISP1, 25, 0, 0),
+	GATE(ISP_CLK_SMMU_SCALERP, "clk_isp_smmu_scalerp",
+			"mout_aclk_isp1_266",
+			EN_IP_ISP1, 26, 0, 0),
+	GATE(ISP_CLK_SPI0, "clk_isp_spi0", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 27, 0, 0),
+	GATE(ISP_CLK_SPI1, "clk_isp_spi1", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 28, 0, 0),
+	GATE(ISP_CLK_WDT, "clk_isp_wdt", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 31, 0, 0),
+	GATE(ISP_CLK_UART, "clk_isp_uart", "mout_aclk_isp1_266",
+			EN_IP_ISP1, 30, 0, 0),
+
+	GATE(ISP_SCLK_UART_EXT, "sclk_isp_uart_ext", "fin_pll",
+			EN_SCLK_ISP, 7, CLK_SET_RATE_PARENT, 0),
+	GATE(ISP_SCLK_SPI1_EXT, "sclk_isp_spi1_ext", "fin_pll",
+			EN_SCLK_ISP, 8, CLK_SET_RATE_PARENT, 0),
+	GATE(ISP_SCLK_SPI0_EXT, "sclk_isp_spi0_ext", "fin_pll",
+			EN_SCLK_ISP, 9, CLK_SET_RATE_PARENT, 0),
+};
+
+static void __init exynos5260_clk_isp_init(struct device_node *np)
+{
+	struct exynos5260_cmu_info cmu = {0};
+
+	cmu.mux_clks = isp_mux_clks;
+	cmu.nr_mux_clks = ARRAY_SIZE(isp_mux_clks);
+	cmu.div_clks = isp_div_clks;
+	cmu.nr_div_clks = ARRAY_SIZE(isp_div_clks);
+	cmu.gate_clks = isp_gate_clks;
+	cmu.nr_gate_clks = ARRAY_SIZE(isp_gate_clks);
+	cmu.nr_clk_ids = ISP_NR_CLK;
+	cmu.clk_regs = isp_clk_regs;
+	cmu.nr_clk_regs = ARRAY_SIZE(isp_clk_regs);
+
+	exynos5260_cmu_register_one(np, &cmu);
+}
+
+CLK_OF_DECLARE(exynos5260_clk_isp, "samsung,exynos5260-clock-isp",
+		exynos5260_clk_isp_init);
+
+
+/* CMU_KFC */
+
+static unsigned long kfc_clk_regs[] __initdata = {
+	KFC_PLL_LOCK,
+	KFC_PLL_CON0,
+	KFC_PLL_CON1,
+	KFC_PLL_FDET,
+	MUX_SEL_KFC0,
+	MUX_SEL_KFC2,
+	DIV_KFC,
+	DIV_KFC_PLL_FDET,
+	EN_ACLK_KFC,
+	EN_PCLK_KFC,
+	EN_SCLK_KFC,
+	EN_IP_KFC,
+};
+
+PNAME(mout_kfc_pll_p) = {"fin_pll", "fout_kfc_pll"};
+PNAME(mout_kfc_p)	 = {"mout_kfc_pll", "dout_media_pll"};
+
+struct samsung_mux_clock kfc_mux_clks[] __initdata = {
+	MUX(KFC_MOUT_KFC_PLL, "mout_kfc_pll", mout_kfc_pll_p,
+			MUX_SEL_KFC0, 0, 1),
+	MUX(KFC_MOUT_KFC, "mout_kfc", mout_kfc_p, MUX_SEL_KFC2, 0, 1),
+};
+
+struct samsung_div_clock kfc_div_clks[] __initdata = {
+	DIV(KFC_DOUT_KFC1, "dout_kfc1", "mout_kfc", DIV_KFC, 0, 3),
+	DIV(KFC_DOUT_KFC2, "dout_kfc2", "dout_kfc1", DIV_KFC, 4, 3),
+	DIV(KFC_DOUT_KFC_ATCLK, "dout_kfc_atclk", "dout_kfc2", DIV_KFC, 8, 3),
+	DIV(KFC_DOUT_KFC_PCLK_DBG, "dout_kfc_pclk_dbg", "dout_kfc2",
+			DIV_KFC, 12, 3),
+	DIV(KFC_DOUT_ACLK_KFC, "dout_aclk_kfc", "dout_kfc2", DIV_KFC, 16, 3),
+	DIV(KFC_DOUT_PCLK_KFC, "dout_pclk_kfc", "dout_kfc2", DIV_KFC, 20, 3),
+	DIV(KFC_DOUT_KFC_PLL, "dout_kfc_pll", "mout_kfc", DIV_KFC, 24, 3),
+};
+
+static struct samsung_pll_clock kfc_pll_clks[] __initdata = {
+	PLL(pll_2550xx, KFC_FOUT_KFC_PLL, "fout_kfc_pll", "fin_pll",
+		KFC_PLL_LOCK, KFC_PLL_CON0,
+		pll2550_24mhz_tbl),
+};
+
+static void __init exynos5260_clk_kfc_init(struct device_node *np)
+{
+	struct exynos5260_cmu_info cmu = {0};
+
+	cmu.pll_clks = kfc_pll_clks;
+	cmu.nr_pll_clks =  ARRAY_SIZE(kfc_pll_clks);
+	cmu.mux_clks = kfc_mux_clks;
+	cmu.nr_mux_clks = ARRAY_SIZE(kfc_mux_clks);
+	cmu.div_clks = kfc_div_clks;
+	cmu.nr_div_clks = ARRAY_SIZE(kfc_div_clks);
+	cmu.nr_clk_ids = KFC_NR_CLK;
+	cmu.clk_regs = kfc_clk_regs;
+	cmu.nr_clk_regs = ARRAY_SIZE(kfc_clk_regs);
+
+	exynos5260_cmu_register_one(np, &cmu);
+}
+
+CLK_OF_DECLARE(exynos5260_clk_kfc, "samsung,exynos5260-clock-kfc",
+		exynos5260_clk_kfc_init);
+
+
+/* CMU_MFC */
+
+static unsigned long mfc_clk_regs[] __initdata = {
+	MUX_SEL_MFC,
+	DIV_MFC,
+	EN_ACLK_MFC,
+	EN_ACLK_SECURE_SMMU2_MFC,
+	EN_PCLK_MFC,
+	EN_PCLK_SECURE_SMMU2_MFC,
+	EN_IP_MFC,
+	EN_IP_MFC_SECURE_SMMU2_MFC,
+};
+
+PNAME(mout_aclk_mfc_333_user_p) = {"fin_pll", "dout_aclk_mfc_333"};
+
+struct samsung_mux_clock mfc_mux_clks[] __initdata = {
+	MUX(MFC_MOUT_ACLK_MFC_333_USER, "mout_aclk_mfc_333_user",
+			mout_aclk_mfc_333_user_p,
+			MUX_SEL_MFC, 0, 1),
+};
+
+struct samsung_div_clock mfc_div_clks[] __initdata = {
+	DIV(MFC_DOUT_PCLK_MFC_83, "dout_pclk_mfc_83", "mout_aclk_mfc_333_user",
+			DIV_MFC, 0, 3),
+};
+
+struct samsung_gate_clock mfc_gate_clks[] __initdata = {
+	GATE(MFC_CLK_MFC, "clk_mfc", "mout_aclk_mfc_333_user",
+			EN_IP_MFC, 1, 0, 0),
+	GATE(MFC_CLK_SMMU2_MFCM0, "clk_smmu2_mfcm0", "mout_aclk_mfc_333_user",
+			EN_IP_MFC_SECURE_SMMU2_MFC, 6, 0, 0),
+	GATE(MFC_CLK_SMMU2_MFCM1, "clk_smmu2_mfcm1", "mout_aclk_mfc_333_user",
+			EN_IP_MFC_SECURE_SMMU2_MFC, 7, 0, 0),
+};
+
+static void __init exynos5260_clk_mfc_init(struct device_node *np)
+{
+	struct exynos5260_cmu_info cmu = {0};
+
+	cmu.mux_clks = mfc_mux_clks;
+	cmu.nr_mux_clks = ARRAY_SIZE(mfc_mux_clks);
+	cmu.div_clks = mfc_div_clks;
+	cmu.nr_div_clks = ARRAY_SIZE(mfc_div_clks);
+	cmu.gate_clks = mfc_gate_clks;
+	cmu.nr_gate_clks = ARRAY_SIZE(mfc_gate_clks);
+	cmu.nr_clk_ids = MFC_NR_CLK;
+	cmu.clk_regs = mfc_clk_regs;
+	cmu.nr_clk_regs = ARRAY_SIZE(mfc_clk_regs);
+
+	exynos5260_cmu_register_one(np, &cmu);
+}
+
+CLK_OF_DECLARE(exynos5260_clk_mfc, "samsung,exynos5260-clock-mfc",
+		exynos5260_clk_mfc_init);
+
+
+/* CMU_MIF */
+
+static unsigned long mif_clk_regs[] __initdata = {
+	MEM_PLL_LOCK,
+	BUS_PLL_LOCK,
+	MEDIA_PLL_LOCK,
+	MEM_PLL_CON0,
+	MEM_PLL_CON1,
+	MEM_PLL_FDET,
+	BUS_PLL_CON0,
+	BUS_PLL_CON1,
+	BUS_PLL_FDET,
+	MEDIA_PLL_CON0,
+	MEDIA_PLL_CON1,
+	MEDIA_PLL_FDET,
+	MUX_SEL_MIF,
+	DIV_MIF,
+	DIV_MIF_PLL_FDET,
+	EN_ACLK_MIF,
+	EN_ACLK_MIF_SECURE_DREX1_TZ,
+	EN_ACLK_MIF_SECURE_DREX0_TZ,
+	EN_ACLK_MIF_SECURE_INTMEM,
+	EN_PCLK_MIF,
+	EN_PCLK_MIF_SECURE_MONOCNT,
+	EN_PCLK_MIF_SECURE_RTC_APBIF,
+	EN_PCLK_MIF_SECURE_DREX1_TZ,
+	EN_PCLK_MIF_SECURE_DREX0_TZ,
+	EN_SCLK_MIF,
+	EN_IP_MIF,
+	EN_IP_MIF_SECURE_MONOCNT,
+	EN_IP_MIF_SECURE_RTC_APBIF,
+	EN_IP_MIF_SECURE_DREX1_TZ,
+	EN_IP_MIF_SECURE_DREX0_TZ,
+	EN_IP_MIF_SECURE_INTEMEM,
+};
+
+PNAME(mout_mem_pll_p) = {"fin_pll", "fout_mem_pll"};
+PNAME(mout_bus_pll_p) = {"fin_pll", "fout_bus_pll"};
+PNAME(mout_media_pll_p) = {"fin_pll", "fout_media_pll"};
+PNAME(mout_mif_drex_p) = {"dout_mem_pll", "dout_bus_pll"};
+PNAME(mout_mif_drex2x_p) = {"dout_mem_pll", "dout_bus_pll"};
+PNAME(mout_clkm_phy_p) = {"mout_mif_drex", "dout_media_pll"};
+PNAME(mout_clk2x_phy_p) = {"mout_mif_drex2x", "dout_media_pll"};
+
+struct samsung_mux_clock mif_mux_clks[] __initdata = {
+	MUX(MIF_MOUT_MEM_PLL, "mout_mem_pll", mout_mem_pll_p,
+			MUX_SEL_MIF, 0, 1),
+	MUX(MIF_MOUT_BUS_PLL, "mout_bus_pll", mout_bus_pll_p,
+			MUX_SEL_MIF, 4, 1),
+	MUX(MIF_MOUT_MEDIA_PLL, "mout_media_pll", mout_media_pll_p,
+			MUX_SEL_MIF, 8, 1),
+	MUX(MIF_MOUT_MIF_DREX, "mout_mif_drex", mout_mif_drex_p,
+			MUX_SEL_MIF, 12, 1),
+	MUX(MIF_MOUT_CLKM_PHY, "mout_clkm_phy", mout_clkm_phy_p,
+			MUX_SEL_MIF, 16, 1),
+	MUX(MIF_MOUT_MIF_DREX2X, "mout_mif_drex2x", mout_mif_drex2x_p,
+			MUX_SEL_MIF, 20, 1),
+	MUX(MIF_MOUT_CLK2X_PHY, "mout_clk2x_phy", mout_clk2x_phy_p,
+			MUX_SEL_MIF, 24, 1),
+};
+
+struct samsung_div_clock mif_div_clks[] __initdata = {
+	DIV(MIF_DOUT_MEDIA_PLL, "dout_media_pll", "mout_media_pll",
+			DIV_MIF, 0, 3),
+	DIV(MIF_DOUT_MEM_PLL, "dout_mem_pll", "mout_mem_pll",
+			DIV_MIF, 4, 3),
+	DIV(MIF_DOUT_BUS_PLL, "dout_bus_pll", "mout_bus_pll",
+			DIV_MIF, 8, 3),
+	DIV(MIF_DOUT_CLKM_PHY, "dout_clkm_phy", "mout_clkm_phy",
+			DIV_MIF, 12, 3),
+	DIV(MIF_DOUT_CLK2X_PHY, "dout_clk2x_phy", "mout_clk2x_phy",
+			DIV_MIF, 16, 4),
+	DIV(MIF_DOUT_ACLK_MIF_466, "dout_aclk_mif_466", "dout_clk2x_phy",
+			DIV_MIF, 20, 3),
+	DIV(MIF_DOUT_ACLK_BUS_200, "dout_aclk_bus_200", "dout_bus_pll",
+			DIV_MIF, 24, 3),
+	DIV(MIF_DOUT_ACLK_BUS_100, "dout_aclk_bus_100", "dout_bus_pll",
+			DIV_MIF, 28, 4),
+};
+
+struct samsung_gate_clock mif_gate_clks[] __initdata = {
+	GATE(MIF_CLK_LPDDR3PHY_WRAP0, "clk_lpddr3phy_wrap0", "dout_clk2x_phy",
+			EN_IP_MIF, 12, CLK_IGNORE_UNUSED, 0),
+	GATE(MIF_CLK_LPDDR3PHY_WRAP1, "clk_lpddr3phy_wrap1", "dout_clk2x_phy",
+			EN_IP_MIF, 13, CLK_IGNORE_UNUSED, 0),
+
+	GATE(MIF_CLK_MONOCNT, "clk_monocnt", "dout_aclk_bus_100",
+			EN_IP_MIF_SECURE_MONOCNT, 22,
+			CLK_IGNORE_UNUSED, 0),
+
+	GATE(MIF_CLK_MIF_RTC, "clk_mif_rtc", "dout_aclk_bus_100",
+			EN_IP_MIF_SECURE_RTC_APBIF, 23,
+			CLK_IGNORE_UNUSED, 0),
+
+	GATE(MIF_CLK_DREX1, "clk_drex1", "dout_aclk_mif_466",
+			EN_IP_MIF_SECURE_DREX1_TZ, 9,
+			CLK_IGNORE_UNUSED, 0),
+
+	GATE(MIF_CLK_DREX0, "clk_drex0", "dout_aclk_mif_466",
+			EN_IP_MIF_SECURE_DREX0_TZ, 9,
+			CLK_IGNORE_UNUSED, 0),
+
+	GATE(MIF_CLK_INTMEM, "clk_intmem", "dout_aclk_bus_200",
+			EN_IP_MIF_SECURE_INTEMEM, 11,
+			CLK_IGNORE_UNUSED, 0),
+
+	GATE(MIF_SCLK_LPDDR3PHY_WRAP_U0, "sclk_lpddr3phy_wrap_u0",
+			"dout_clkm_phy", EN_SCLK_MIF, 0,
+			CLK_IGNORE_UNUSED | CLK_SET_RATE_PARENT, 0),
+	GATE(MIF_SCLK_LPDDR3PHY_WRAP_U1, "sclk_lpddr3phy_wrap_u1",
+			"dout_clkm_phy", EN_SCLK_MIF, 1,
+			CLK_IGNORE_UNUSED | CLK_SET_RATE_PARENT, 0),
+};
+
+static struct samsung_pll_clock mif_pll_clks[] __initdata = {
+	PLL(pll_2550xx, MIF_FOUT_MEM_PLL, "fout_mem_pll", "fin_pll",
+		MEM_PLL_LOCK, MEM_PLL_CON0,
+		pll2550_24mhz_tbl),
+	PLL(pll_2550xx, MIF_FOUT_BUS_PLL, "fout_bus_pll", "fin_pll",
+		BUS_PLL_LOCK, BUS_PLL_CON0,
+		pll2550_24mhz_tbl),
+	PLL(pll_2550xx, MIF_FOUT_MEDIA_PLL, "fout_media_pll", "fin_pll",
+		MEDIA_PLL_LOCK, MEDIA_PLL_CON0,
+		pll2550_24mhz_tbl),
+};
+
+static void __init exynos5260_clk_mif_init(struct device_node *np)
+{
+	struct exynos5260_cmu_info cmu = {0};
+
+	cmu.pll_clks = mif_pll_clks;
+	cmu.nr_pll_clks =  ARRAY_SIZE(mif_pll_clks);
+	cmu.mux_clks = mif_mux_clks;
+	cmu.nr_mux_clks = ARRAY_SIZE(mif_mux_clks);
+	cmu.div_clks = mif_div_clks;
+	cmu.nr_div_clks = ARRAY_SIZE(mif_div_clks);
+	cmu.gate_clks = mif_gate_clks;
+	cmu.nr_gate_clks = ARRAY_SIZE(mif_gate_clks);
+	cmu.nr_clk_ids = MIF_NR_CLK;
+	cmu.clk_regs = mif_clk_regs;
+	cmu.nr_clk_regs = ARRAY_SIZE(mif_clk_regs);
+
+	exynos5260_cmu_register_one(np, &cmu);
+}
+
+CLK_OF_DECLARE(exynos5260_clk_mif, "samsung,exynos5260-clock-mif",
+		exynos5260_clk_mif_init);
+
+
+/* CMU_PERI */
+
+static unsigned long peri_clk_regs[] __initdata = {
+	MUX_SEL_PERI,
+	MUX_SEL_PERI1,
+	DIV_PERI,
+	EN_PCLK_PERI0,
+	EN_PCLK_PERI1,
+	EN_PCLK_PERI2,
+	EN_PCLK_PERI3,
+	EN_PCLK_PERI_SECURE_CHIPID,
+	EN_PCLK_PERI_SECURE_PROVKEY0,
+	EN_PCLK_PERI_SECURE_PROVKEY1,
+	EN_PCLK_PERI_SECURE_SECKEY,
+	EN_PCLK_PERI_SECURE_ANTIRBKCNT,
+	EN_PCLK_PERI_SECURE_TOP_RTC,
+	EN_PCLK_PERI_SECURE_TZPC,
+	EN_SCLK_PERI,
+	EN_SCLK_PERI_SECURE_TOP_RTC,
+	EN_IP_PERI0,
+	EN_IP_PERI1,
+	EN_IP_PERI2,
+	EN_IP_PERI_SECURE_CHIPID,
+	EN_IP_PERI_SECURE_PROVKEY0,
+	EN_IP_PERI_SECURE_PROVKEY1,
+	EN_IP_PERI_SECURE_SECKEY,
+	EN_IP_PERI_SECURE_ANTIRBKCNT,
+	EN_IP_PERI_SECURE_TOP_RTC,
+	EN_IP_PERI_SECURE_TZPC,
+};
+
+PNAME(mout_sclk_pcm_p) = {"ioclk_pcm_extclk", "fin_pll", "dout_aclk_peri_aud",
+			"phyclk_hdmi_phy_ref_cko"};
+PNAME(mout_sclk_i2scod_p) = {"ioclk_i2s_cdclk", "fin_pll", "dout_aclk_peri_aud",
+			"phyclk_hdmi_phy_ref_cko"};
+PNAME(mout_sclk_spdif_p) = {"ioclk_spdif_extclk", "fin_pll",
+			"dout_aclk_peri_aud", "phyclk_hdmi_phy_ref_cko"};
+
+struct samsung_mux_clock peri_mux_clks[] __initdata = {
+	MUX(PERI_MOUT_SCLK_PCM, "mout_sclk_pcm", mout_sclk_pcm_p,
+			MUX_SEL_PERI1, 4, 2),
+	MUX(PERI_MOUT_SCLK_I2SCOD, "mout_sclk_i2scod", mout_sclk_i2scod_p,
+			MUX_SEL_PERI1, 12, 2),
+	MUX(PERI_MOUT_SCLK_SPDIF, "mout_sclk_spdif", mout_sclk_spdif_p,
+			MUX_SEL_PERI1, 20, 2),
+};
+
+struct samsung_div_clock peri_div_clks[] __initdata = {
+	DIV(PERI_DOUT_PCM, "dout_pcm", "mout_sclk_pcm", DIV_PERI, 0, 8),
+	DIV(PERI_DOUT_I2S, "dout_i2s", "mout_sclk_i2scod", DIV_PERI, 8, 6),
+};
+
+struct samsung_gate_clock peri_gate_clks[] __initdata = {
+	GATE(PERI_SCLK_PCM1, "sclk_pcm1", "dout_pcm", EN_SCLK_PERI, 0,
+			CLK_SET_RATE_PARENT, 0),
+	GATE(PERI_SCLK_I2S, "sclk_i2s", "dout_i2s", EN_SCLK_PERI, 1,
+			CLK_SET_RATE_PARENT, 0),
+	GATE(PERI_SCLK_SPDIF, "sclk_spdif", "dout_sclk_peri_spi0_b",
+			EN_SCLK_PERI, 2, CLK_SET_RATE_PARENT, 0),
+	GATE(PERI_SCLK_SPI0, "sclk_spi0", "dout_sclk_peri_spi0_b",
+			EN_SCLK_PERI, 7, CLK_SET_RATE_PARENT, 0),
+	GATE(PERI_SCLK_SPI1, "sclk_spi1", "dout_sclk_peri_spi1_b",
+			EN_SCLK_PERI, 8, CLK_SET_RATE_PARENT, 0),
+	GATE(PERI_SCLK_SPI2, "sclk_spi2", "dout_sclk_peri_spi2_b",
+			EN_SCLK_PERI, 9, CLK_SET_RATE_PARENT, 0),
+	GATE(PERI_SCLK_UART0, "sclk_uart0", "dout_sclk_peri_uart0",
+			EN_SCLK_PERI, 10, CLK_SET_RATE_PARENT, 0),
+	GATE(PERI_SCLK_UART1, "sclk_uart1", "dout_sclk_peri_uart1",
+			EN_SCLK_PERI, 11, CLK_SET_RATE_PARENT, 0),
+	GATE(PERI_SCLK_UART2, "sclk_uart2", "dout_sclk_peri_uart2",
+			EN_SCLK_PERI, 12, CLK_SET_RATE_PARENT, 0),
+
+	GATE(PERI_CLK_ABB, "clk_abb", "dout_aclk_peri_66",
+		EN_IP_PERI0, 1, 0, 0),
+	GATE(PERI_CLK_EFUSE_WRITER, "clk_efuse_writer", "dout_aclk_peri_66",
+		EN_IP_PERI0, 5, 0, 0),
+	GATE(PERI_CLK_HDMICEC, "clk_hdmicec", "dout_aclk_peri_66",
+		EN_IP_PERI0, 6, 0, 0),
+	GATE(PERI_CLK_I2C10, "clk_i2c10", "dout_aclk_peri_66",
+		EN_IP_PERI0, 7, 0, 0),
+	GATE(PERI_CLK_I2C11, "clk_i2c11", "dout_aclk_peri_66",
+		EN_IP_PERI0, 8, 0, 0),
+	GATE(PERI_CLK_I2C8, "clk_i2c8", "dout_aclk_peri_66",
+		EN_IP_PERI0, 9, 0, 0),
+	GATE(PERI_CLK_I2C9, "clk_i2c9", "dout_aclk_peri_66",
+		EN_IP_PERI0, 10, 0, 0),
+	GATE(PERI_CLK_I2C4, "clk_i2c4", "dout_aclk_peri_66",
+		EN_IP_PERI0, 11, 0, 0),
+	GATE(PERI_CLK_I2C5, "clk_i2c5", "dout_aclk_peri_66",
+		EN_IP_PERI0, 12, 0, 0),
+	GATE(PERI_CLK_I2C6, "clk_i2c6", "dout_aclk_peri_66",
+		EN_IP_PERI0, 13, 0, 0),
+	GATE(PERI_CLK_I2C7, "clk_i2c7", "dout_aclk_peri_66",
+		EN_IP_PERI0, 14, 0, 0),
+	GATE(PERI_CLK_I2CHDMI, "clk_i2chdmi", "dout_aclk_peri_66",
+		EN_IP_PERI0, 15, 0, 0),
+	GATE(PERI_CLK_I2S, "clk_peri_i2s", "dout_aclk_peri_66",
+		EN_IP_PERI0, 16, 0, 0),
+	GATE(PERI_CLK_MCT, "clk_mct", "dout_aclk_peri_66",
+		EN_IP_PERI0, 17, 0, 0),
+	GATE(PERI_CLK_PCM, "clk_peri_pcm", "dout_aclk_peri_66",
+		EN_IP_PERI0, 18, 0, 0),
+	GATE(PERI_CLK_HSIC0, "clk_hsic0", "dout_aclk_peri_66",
+		EN_IP_PERI0, 20, 0, 0),
+	GATE(PERI_CLK_HSIC1, "clk_hsic1", "dout_aclk_peri_66",
+		EN_IP_PERI0, 21, 0, 0),
+	GATE(PERI_CLK_HSIC2, "clk_hsic2", "dout_aclk_peri_66",
+		EN_IP_PERI0, 22, 0, 0),
+	GATE(PERI_CLK_HSIC3, "clk_hsic3", "dout_aclk_peri_66",
+		EN_IP_PERI0, 23, 0, 0),
+	GATE(PERI_CLK_WDT_EGL, "clk_wdt_egl", "dout_aclk_peri_66",
+		EN_IP_PERI0, 24, 0, 0),
+	GATE(PERI_CLK_WDT_KFC, "clk_wdt_kfc", "dout_aclk_peri_66",
+		EN_IP_PERI0, 25, 0, 0),
+
+	GATE(PERI_CLK_UART4, "clk_uart4", "dout_aclk_peri_66",
+		EN_IP_PERI2, 0, 0, 0),
+	GATE(PERI_CLK_PWM, "clk_pwm", "dout_aclk_peri_66",
+		EN_IP_PERI2, 3, 0, 0),
+	GATE(PERI_CLK_SPDIF, "clk_spdif", "dout_aclk_peri_66",
+		EN_IP_PERI2, 6, 0, 0),
+	GATE(PERI_CLK_SPI0, "clk_spi0", "dout_aclk_peri_66",
+		EN_IP_PERI2, 7, 0, 0),
+	GATE(PERI_CLK_SPI1, "clk_spi1", "dout_aclk_peri_66",
+		EN_IP_PERI2, 8, 0, 0),
+	GATE(PERI_CLK_SPI2, "clk_spi2", "dout_aclk_peri_66",
+		EN_IP_PERI2, 9, 0, 0),
+	GATE(PERI_CLK_TMU0, "clk_tmu0", "dout_aclk_peri_66",
+		EN_IP_PERI2, 10, 0, 0),
+	GATE(PERI_CLK_TMU1, "clk_tmu1", "dout_aclk_peri_66",
+		EN_IP_PERI2, 11, 0, 0),
+	GATE(PERI_CLK_TMU2, "clk_tmu2", "dout_aclk_peri_66",
+		EN_IP_PERI2, 12, 0, 0),
+	GATE(PERI_CLK_TMU3, "clk_tmu3", "dout_aclk_peri_66",
+		EN_IP_PERI2, 13, 0, 0),
+	GATE(PERI_CLK_TMU4, "clk_tmu4", "dout_aclk_peri_66",
+		EN_IP_PERI2, 14, 0, 0),
+	GATE(PERI_CLK_ADC, "clk_adc", "dout_aclk_peri_66",
+		EN_IP_PERI2, 18, 0, 0),
+	GATE(PERI_CLK_UART0, "clk_uart0", "dout_aclk_peri_66",
+		EN_IP_PERI2, 19, 0, 0),
+	GATE(PERI_CLK_UART1, "clk_uart1", "dout_aclk_peri_66",
+		EN_IP_PERI2, 20, 0, 0),
+	GATE(PERI_CLK_UART2, "clk_uart2", "dout_aclk_peri_66",
+		EN_IP_PERI2, 21, 0, 0),
+
+	GATE(PERI_CLK_CHIPID, "clk_chipid", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_CHIPID, 2, 0, 0),
+
+	GATE(PERI_CLK_PROVKEY0, "clk_provkey0", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_PROVKEY0, 1, 0, 0),
+
+	GATE(PERI_CLK_PROVKEY1, "clk_provkey1", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_PROVKEY1, 2, 0, 0),
+
+	GATE(PERI_CLK_SECKEY, "clk_seckey", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_SECKEY, 5, 0, 0),
+
+	GATE(PERI_CLK_TOP_RTC, "clk_top_rtc", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_TOP_RTC, 5, 0, 0),
+
+	GATE(PERI_CLK_TZPC0, "clk_tzpc0", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_TZPC, 10, 0, 0),
+	GATE(PERI_CLK_TZPC1, "clk_tzpc1", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_TZPC, 11, 0, 0),
+	GATE(PERI_CLK_TZPC2, "clk_tzpc2", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_TZPC, 12, 0, 0),
+	GATE(PERI_CLK_TZPC3, "clk_tzpc3", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_TZPC, 13, 0, 0),
+	GATE(PERI_CLK_TZPC4, "clk_tzpc4", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_TZPC, 14, 0, 0),
+	GATE(PERI_CLK_TZPC5, "clk_tzpc5", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_TZPC, 15, 0, 0),
+	GATE(PERI_CLK_TZPC6, "clk_tzpc6", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_TZPC, 16, 0, 0),
+	GATE(PERI_CLK_TZPC7, "clk_tzpc7", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_TZPC, 17, 0, 0),
+	GATE(PERI_CLK_TZPC8, "clk_tzpc8", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_TZPC, 18, 0, 0),
+	GATE(PERI_CLK_TZPC9, "clk_tzpc9", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_TZPC, 19, 0, 0),
+	GATE(PERI_CLK_TZPC10, "clk_tzpc10", "dout_aclk_peri_66",
+		EN_IP_PERI_SECURE_TZPC, 20, 0, 0),
+};
+
+static void __init exynos5260_clk_peri_init(struct device_node *np)
+{
+	struct exynos5260_cmu_info cmu = {0};
+
+	cmu.mux_clks = peri_mux_clks;
+	cmu.nr_mux_clks = ARRAY_SIZE(peri_mux_clks);
+	cmu.div_clks = peri_div_clks;
+	cmu.nr_div_clks = ARRAY_SIZE(peri_div_clks);
+	cmu.gate_clks = peri_gate_clks;
+	cmu.nr_gate_clks = ARRAY_SIZE(peri_gate_clks);
+	cmu.nr_clk_ids = PERI_NR_CLK;
+	cmu.clk_regs = peri_clk_regs;
+	cmu.nr_clk_regs = ARRAY_SIZE(peri_clk_regs);
+
+	exynos5260_cmu_register_one(np, &cmu);
+}
+
+CLK_OF_DECLARE(exynos5260_clk_peri, "samsung,exynos5260-clock-peri",
+		exynos5260_clk_peri_init);
+
+
+/* CMU_TOP */
+
+static unsigned long top_clk_regs[] __initdata = {
+	DISP_PLL_LOCK,
+	AUD_PLL_LOCK,
+	DISP_PLL_CON0,
+	DISP_PLL_CON1,
+	DISP_PLL_FDET,
+	AUD_PLL_CON0,
+	AUD_PLL_CON1,
+	AUD_PLL_CON2,
+	AUD_PLL_FDET,
+	MUX_SEL_TOP_PLL0,
+	MUX_SEL_TOP_MFC,
+	MUX_SEL_TOP_G2D,
+	MUX_SEL_TOP_GSCL,
+	MUX_SEL_TOP_ISP10,
+	MUX_SEL_TOP_ISP11,
+	MUX_SEL_TOP_DISP0,
+	MUX_SEL_TOP_DISP1,
+	MUX_SEL_TOP_BUS,
+	MUX_SEL_TOP_PERI0,
+	MUX_SEL_TOP_PERI1,
+	MUX_SEL_TOP_FSYS,
+	DIV_TOP_G2D_MFC,
+	DIV_TOP_GSCL_ISP0,
+	DIV_TOP_ISP10,
+	DIV_TOP_ISP11,
+	DIV_TOP_DISP,
+	DIV_TOP_BUS,
+	DIV_TOP_PERI0,
+	DIV_TOP_PERI1,
+	DIV_TOP_PERI2,
+	DIV_TOP_FSYS0,
+	DIV_TOP_FSYS1,
+	DIV_TOP_HPM,
+	DIV_TOP_PLL_FDET,
+	EN_ACLK_TOP,
+	EN_SCLK_TOP,
+	EN_IP_TOP,
+};
+
+/* fixed rate clocks generated inside the soc */
+struct samsung_fixed_rate_clock fixed_rate_clks[] __initdata = {
+	FRATE(PHYCLK_DPTX_PHY_CH3_TXD_CLK, "phyclk_dptx_phy_ch3_txd_clk", NULL,
+			CLK_IS_ROOT, 270000000),
+	FRATE(PHYCLK_DPTX_PHY_CH2_TXD_CLK, "phyclk_dptx_phy_ch2_txd_clk", NULL,
+			CLK_IS_ROOT, 270000000),
+	FRATE(PHYCLK_DPTX_PHY_CH1_TXD_CLK, "phyclk_dptx_phy_ch1_txd_clk", NULL,
+			CLK_IS_ROOT, 270000000),
+	FRATE(PHYCLK_DPTX_PHY_CH0_TXD_CLK, "phyclk_dptx_phy_ch0_txd_clk", NULL,
+			CLK_IS_ROOT, 270000000),
+	FRATE(phyclk_hdmi_phy_tmds_clko, "phyclk_hdmi_phy_tmds_clko", NULL,
+			CLK_IS_ROOT, 250000000),
+	FRATE(PHYCLK_HDMI_PHY_PIXEL_CLKO, "phyclk_hdmi_phy_pixel_clko", NULL,
+			CLK_IS_ROOT, 1660000000),
+	FRATE(PHYCLK_HDMI_LINK_O_TMDS_CLKHI, "phyclk_hdmi_link_o_tmds_clkhi",
+			NULL, CLK_IS_ROOT, 125000000),
+	FRATE(PHYCLK_MIPI_DPHY_4L_M_TXBYTECLKHS,
+			"phyclk_mipi_dphy_4l_m_txbyteclkhs" , NULL,
+			CLK_IS_ROOT, 187500000),
+	FRATE(PHYCLK_DPTX_PHY_O_REF_CLK_24M, "phyclk_dptx_phy_o_ref_clk_24m",
+			NULL, CLK_IS_ROOT, 24000000),
+	FRATE(PHYCLK_DPTX_PHY_CLK_DIV2, "phyclk_dptx_phy_clk_div2", NULL,
+			CLK_IS_ROOT, 135000000),
+	FRATE(PHYCLK_MIPI_DPHY_4L_M_RXCLKESC0,
+			"phyclk_mipi_dphy_4l_m_rxclkesc0", NULL,
+			CLK_IS_ROOT, 20000000),
+	FRATE(PHYCLK_USBHOST20_PHY_PHYCLOCK, "phyclk_usbhost20_phy_phyclock",
+			NULL, CLK_IS_ROOT, 60000000),
+	FRATE(PHYCLK_USBHOST20_PHY_FREECLK, "phyclk_usbhost20_phy_freeclk",
+			NULL, CLK_IS_ROOT, 60000000),
+	FRATE(PHYCLK_USBHOST20_PHY_CLK48MOHCI,
+			"phyclk_usbhost20_phy_clk48mohci",
+			NULL, CLK_IS_ROOT, 48000000),
+	FRATE(PHYCLK_USBDRD30_UDRD30_PIPE_PCLK,
+			"phyclk_usbdrd30_udrd30_pipe_pclk", NULL,
+			CLK_IS_ROOT, 125000000),
+	FRATE(PHYCLK_USBDRD30_UDRD30_PHYCLOCK,
+			"phyclk_usbdrd30_udrd30_phyclock", NULL,
+			CLK_IS_ROOT, 60000000),
+};
+
+PNAME(mout_memtop_pll_user_p) = {"fin_pll", "dout_mem_pll"};
+PNAME(mout_bustop_pll_user_p) = {"fin_pll", "dout_bus_pll"};
+PNAME(mout_mediatop_pll_user_p) = {"fin_pll", "dout_media_pll"};
+PNAME(mout_audtop_pll_user_p) = {"fin_pll", "mout_aud_pll"};
+PNAME(mout_aud_pll_p) = {"fin_pll", "fout_aud_pll"};
+PNAME(mout_disp_pll_p) = {"fin_pll", "fout_disp_pll"};
+PNAME(mout_mfc_bustop_333_p) = {"mout_bustop_pll_user", "mout_disp_pll"};
+PNAME(mout_aclk_mfc_333_p) = {"mout_mediatop_pll_user", "mout_mfc_bustop_333"};
+PNAME(mout_g2d_bustop_333_p) = {"mout_bustop_pll_user", "mout_disp_pll"};
+PNAME(mout_aclk_g2d_333_p) = {"mout_mediatop_pll_user", "mout_g2d_bustop_333"};
+PNAME(mout_gscl_bustop_333_p) = {"mout_bustop_pll_user", "mout_disp_pll"};
+PNAME(mout_aclk_gscl_333_p) = {"mout_mediatop_pll_user",
+			"mout_gscl_bustop_333"};
+PNAME(mout_m2m_mediatop_400_p) = {"mout_mediatop_pll_user", "mout_disp_pll"};
+PNAME(mout_aclk_gscl_400_p) = {"mout_bustop_pll_user",
+			"mout_m2m_mediatop_400"};
+PNAME(mout_gscl_bustop_fimc_p) = {"mout_bustop_pll_user", "mout_disp_pll"};
+PNAME(mout_aclk_gscl_fimc_p) = {"mout_mediatop_pll_user",
+			"mout_gscl_bustop_fimc"};
+PNAME(mout_isp1_media_266_p) = {"mout_mediatop_pll_user",
+			"mout_memtop_pll_user"};
+PNAME(mout_aclk_isp1_266_p) = {"mout_bustop_pll_user", "mout_isp1_media_266"};
+PNAME(mout_isp1_media_400_p) = {"mout_mediatop_pll_user", "mout_disp_pll"};
+PNAME(mout_aclk_isp1_400_p) = {"mout_bustop_pll_user", "mout_isp1_media_400"};
+PNAME(mout_sclk_isp_spi_p) = {"fin_pll", "mout_bustop_pll_user"};
+PNAME(mout_sclk_isp_uart_p) = {"fin_pll", "mout_bustop_pll_user"};
+PNAME(mout_sclk_isp_sensor_p) = {"fin_pll", "mout_bustop_pll_user"};
+PNAME(mout_disp_disp_333_p) = {"mout_disp_pll", "mout_bustop_pll_user"};
+PNAME(mout_aclk_disp_333_p) = {"mout_mediatop_pll_user", "mout_disp_disp_333"};
+PNAME(mout_disp_disp_222_p) = {"mout_disp_pll", "mout_bustop_pll_user"};
+PNAME(mout_aclk_disp_222_p) = {"mout_mediatop_pll_user", "mout_disp_disp_222"};
+PNAME(mout_disp_media_pixel_p) = {"mout_mediatop_pll_user",
+			"mout_bustop_pll_user"};
+PNAME(mout_sclk_disp_pixel_p) = {"mout_disp_pll", "mout_disp_media_pixel"};
+PNAME(mout_bus_bustop_400_p) = {"mout_bustop_pll_user", "mout_memtop_pll_user"};
+PNAME(mout_bus_bustop_100_p) = {"mout_bustop_pll_user", "mout_memtop_pll_user"};
+PNAME(mout_sclk_peri_spi_clk_p) = {"fin_pll", "mout_bustop_pll_user"};
+PNAME(mout_sclk_peri_uart_uclk_p) = {"fin_pll", "mout_bustop_pll_user"};
+PNAME(mout_sclk_fsys_usb_p) = {"fin_pll", "mout_bustop_pll_user"};
+PNAME(mout_sclk_fsys_mmc_sdclkin_a_p) = {"fin_pll", "mout_bustop_pll_user"};
+PNAME(mout_sclk_fsys_mmc0_sdclkin_b_p) = {"mout_sclk_fsys_mmc0_sdclkin_a",
+			"mout_mediatop_pll_user"};
+PNAME(mout_sclk_fsys_mmc1_sdclkin_b_p) = {"mout_sclk_fsys_mmc1_sdclkin_a",
+			"mout_mediatop_pll_user"};
+PNAME(mout_sclk_fsys_mmc2_sdclkin_b_p) = {"mout_sclk_fsys_mmc2_sdclkin_a",
+			"mout_mediatop_pll_user"};
+
+struct samsung_mux_clock top_mux_clks[] __initdata = {
+	MUX(TOP_MOUT_MEDIATOP_PLL_USER, "mout_mediatop_pll_user",
+			mout_mediatop_pll_user_p,
+			MUX_SEL_TOP_PLL0, 0, 1),
+	MUX(TOP_MOUT_MEMTOP_PLL_USER, "mout_memtop_pll_user",
+			mout_memtop_pll_user_p,
+			MUX_SEL_TOP_PLL0, 4, 1),
+	MUX(TOP_MOUT_BUSTOP_PLL_USER, "mout_bustop_pll_user",
+			mout_bustop_pll_user_p,
+			MUX_SEL_TOP_PLL0, 8, 1),
+	MUX(TOP_MOUT_DISP_PLL, "mout_disp_pll", mout_disp_pll_p,
+			MUX_SEL_TOP_PLL0, 12, 1),
+	MUX(TOP_MOUT_AUD_PLL, "mout_aud_pll", mout_aud_pll_p,
+			MUX_SEL_TOP_PLL0, 16, 1),
+	MUX(TOP_MOUT_AUDTOP_PLL_USER, "mout_audtop_pll_user",
+			mout_audtop_pll_user_p,
+			MUX_SEL_TOP_PLL0, 24, 1),
+
+	MUX(TOP_MOUT_DISP_DISP_333, "mout_disp_disp_333", mout_disp_disp_333_p,
+			MUX_SEL_TOP_DISP0, 0, 1),
+	MUX(TOP_MOUT_ACLK_DISP_333, "mout_aclk_disp_333", mout_aclk_disp_333_p,
+			MUX_SEL_TOP_DISP0, 8, 1),
+	MUX(TOP_MOUT_DISP_DISP_222, "mout_disp_disp_222", mout_disp_disp_222_p,
+			MUX_SEL_TOP_DISP0, 12, 1),
+	MUX(TOP_MOUT_ACLK_DISP_222, "mout_aclk_disp_222", mout_aclk_disp_222_p,
+			MUX_SEL_TOP_DISP0, 20, 1),
+
+	MUX(TOP_MOUT_FIMD1, "mout_sclk_disp_pixel", mout_sclk_disp_pixel_p,
+			MUX_SEL_TOP_DISP1, 0, 1),
+	MUX(TOP_MOUT_DISP_MEDIA_PIXEL, "mout_disp_media_pixel",
+			mout_disp_media_pixel_p,
+			MUX_SEL_TOP_DISP1, 8, 1),
+
+	MUX(TOP_MOUT_SCLK_PERI_SPI2_CLK, "mout_sclk_peri_spi2_clk",
+			mout_sclk_peri_spi_clk_p,
+			MUX_SEL_TOP_PERI1, 0, 1),
+	MUX(TOP_MOUT_SCLK_PERI_SPI1_CLK, "mout_sclk_peri_spi1_clk",
+			mout_sclk_peri_spi_clk_p,
+			MUX_SEL_TOP_PERI1, 4, 1),
+	MUX(TOP_MOUT_SCLK_PERI_SPI0_CLK, "mout_sclk_peri_spi0_clk",
+			mout_sclk_peri_spi_clk_p,
+			MUX_SEL_TOP_PERI1, 8, 1),
+	MUX(TOP_MOUT_SCLK_PERI_UART1_UCLK, "mout_sclk_peri_uart1_uclk",
+			mout_sclk_peri_uart_uclk_p,
+			MUX_SEL_TOP_PERI1, 12, 1),
+	MUX(TOP_MOUT_SCLK_PERI_UART2_UCLK, "mout_sclk_peri_uart2_uclk",
+			mout_sclk_peri_uart_uclk_p,
+			MUX_SEL_TOP_PERI1, 16, 1),
+	MUX(TOP_MOUT_SCLK_PERI_UART0_UCLK, "mout_sclk_peri_uart0_uclk",
+			mout_sclk_peri_uart_uclk_p,
+			MUX_SEL_TOP_PERI1, 20, 1),
+
+
+	MUX(TOP_MOUT_BUS1_BUSTOP_400, "mout_bus1_bustop_400",
+			mout_bus_bustop_400_p,
+			MUX_SEL_TOP_BUS, 0, 1),
+	MUX(TOP_MOUT_BUS1_BUSTOP_100, "mout_bus1_bustop_100",
+			mout_bus_bustop_100_p,
+			MUX_SEL_TOP_BUS, 4, 1),
+	MUX(TOP_MOUT_BUS2_BUSTOP_100, "mout_bus2_bustop_100",
+			mout_bus_bustop_100_p,
+			MUX_SEL_TOP_BUS, 8, 1),
+	MUX(TOP_MOUT_BUS2_BUSTOP_400, "mout_bus2_bustop_400",
+			mout_bus_bustop_400_p,
+			MUX_SEL_TOP_BUS, 12, 1),
+	MUX(TOP_MOUT_BUS3_BUSTOP_400, "mout_bus3_bustop_400",
+			mout_bus_bustop_400_p,
+			MUX_SEL_TOP_BUS, 16, 1),
+	MUX(TOP_MOUT_BUS3_BUSTOP_100, "mout_bus3_bustop_100",
+			mout_bus_bustop_100_p,
+			MUX_SEL_TOP_BUS, 20, 1),
+	MUX(TOP_MOUT_BUS4_BUSTOP_400, "mout_bus4_bustop_400",
+			mout_bus_bustop_400_p,
+			MUX_SEL_TOP_BUS, 24, 1),
+	MUX(TOP_MOUT_BUS4_BUSTOP_100, "mout_bus4_bustop_100",
+			mout_bus_bustop_100_p,
+			MUX_SEL_TOP_BUS, 28, 1),
+
+	MUX(TOP_MOUT_SCLK_FSYS_USB, "mout_sclk_fsys_usb",
+			mout_sclk_fsys_usb_p,
+			MUX_SEL_TOP_FSYS, 0, 1),
+	MUX(TOP_MOUT_SCLK_FSYS_MMC2_SDCLKIN_A, "mout_sclk_fsys_mmc2_sdclkin_a",
+			mout_sclk_fsys_mmc_sdclkin_a_p,
+			MUX_SEL_TOP_FSYS, 4, 1),
+	MUX(TOP_MOUT_SCLK_FSYS_MMC2_SDCLKIN_B, "mout_sclk_fsys_mmc2_sdclkin_b",
+			mout_sclk_fsys_mmc2_sdclkin_b_p,
+			MUX_SEL_TOP_FSYS, 8, 1),
+	MUX(TOP_MOUT_SCLK_FSYS_MMC1_SDCLKIN_A, "mout_sclk_fsys_mmc1_sdclkin_a",
+			mout_sclk_fsys_mmc_sdclkin_a_p,
+			MUX_SEL_TOP_FSYS, 12, 1),
+	MUX(TOP_MOUT_SCLK_FSYS_MMC1_SDCLKIN_B, "mout_sclk_fsys_mmc1_sdclkin_b",
+			mout_sclk_fsys_mmc1_sdclkin_b_p,
+			MUX_SEL_TOP_FSYS, 16, 1),
+	MUX(TOP_MOUT_SCLK_FSYS_MMC0_SDCLKIN_A, "mout_sclk_fsys_mmc0_sdclkin_a",
+			mout_sclk_fsys_mmc_sdclkin_a_p,
+			MUX_SEL_TOP_FSYS, 20, 1),
+	MUX(TOP_MOUT_SCLK_FSYS_MMC0_SDCLKIN_B, "mout_sclk_fsys_mmc0_sdclkin_b",
+			mout_sclk_fsys_mmc0_sdclkin_b_p,
+			MUX_SEL_TOP_FSYS, 24, 1),
+
+	MUX(TOP_MOUT_ISP1_MEDIA_400, "mout_isp1_media_400",
+			mout_isp1_media_400_p,
+			MUX_SEL_TOP_ISP10, 4, 1),
+	MUX(TOP_MOUT_ACLK_ISP1_400, "mout_aclk_isp1_400", mout_aclk_isp1_400_p,
+			MUX_SEL_TOP_ISP10, 8 , 1),
+	MUX(TOP_MOUT_ISP1_MEDIA_266, "mout_isp1_media_266",
+			mout_isp1_media_266_p,
+			MUX_SEL_TOP_ISP10, 16, 1),
+	MUX(TOP_MOUT_ACLK_ISP1_266, "mout_aclk_isp1_266", mout_aclk_isp1_266_p,
+			MUX_SEL_TOP_ISP10, 20, 1),
+
+	MUX(TOP_MOUT_SCLK_ISP1_SPI0, "mout_sclk_isp1_spi0", mout_sclk_isp_spi_p,
+			MUX_SEL_TOP_ISP11, 4, 1),
+	MUX(TOP_MOUT_SCLK_ISP1_SPI1, "mout_sclk_isp1_spi1", mout_sclk_isp_spi_p,
+			MUX_SEL_TOP_ISP11, 8, 1),
+	MUX(TOP_MOUT_SCLK_ISP1_UART, "mout_sclk_isp1_uart",
+			mout_sclk_isp_uart_p,
+			MUX_SEL_TOP_ISP11, 12, 1),
+	MUX(TOP_MOUT_SCLK_ISP1_SENSOR0, "mout_sclk_isp1_sensor0",
+			mout_sclk_isp_sensor_p,
+			MUX_SEL_TOP_ISP11, 16, 1),
+	MUX(TOP_MOUT_SCLK_ISP1_SENSOR1, "mout_sclk_isp1_sensor1",
+			mout_sclk_isp_sensor_p,
+			MUX_SEL_TOP_ISP11, 20, 1),
+	MUX(TOP_MOUT_SCLK_ISP1_SENSOR2, "mout_sclk_isp1_sensor2",
+			mout_sclk_isp_sensor_p,
+			MUX_SEL_TOP_ISP11, 24, 1),
+
+	MUX(TOP_MOUT_MFC_BUSTOP_333, "mout_mfc_bustop_333",
+			mout_mfc_bustop_333_p,
+			MUX_SEL_TOP_MFC, 4, 1),
+	MUX(TOP_MOUT_ACLK_MFC_333, "mout_aclk_mfc_333", mout_aclk_mfc_333_p,
+			MUX_SEL_TOP_MFC, 8, 1),
+
+	MUX(TOP_MOUT_G2D_BUSTOP_333, "mout_g2d_bustop_333",
+			mout_g2d_bustop_333_p,
+			MUX_SEL_TOP_G2D, 4, 1),
+	MUX(TOP_MOUT_ACLK_G2D_333, "mout_aclk_g2d_333", mout_aclk_g2d_333_p,
+			MUX_SEL_TOP_G2D, 8, 1),
+
+	MUX(TOP_MOUT_M2M_MEDIATOP_400, "mout_m2m_mediatop_400",
+			mout_m2m_mediatop_400_p,
+			MUX_SEL_TOP_GSCL, 0, 1),
+	MUX(TOP_MOUT_ACLK_GSCL_400, "mout_aclk_gscl_400",
+			mout_aclk_gscl_400_p,
+			MUX_SEL_TOP_GSCL, 4, 1),
+	MUX(TOP_MOUT_GSCL_BUSTOP_333, "mout_gscl_bustop_333",
+			mout_gscl_bustop_333_p,
+			MUX_SEL_TOP_GSCL, 8, 1),
+	MUX(TOP_MOUT_ACLK_GSCL_333, "mout_aclk_gscl_333",
+			mout_aclk_gscl_333_p,
+			MUX_SEL_TOP_GSCL, 12, 1),
+	MUX(TOP_MOUT_GSCL_BUSTOP_FIMC, "mout_gscl_bustop_fimc",
+			mout_gscl_bustop_fimc_p,
+			MUX_SEL_TOP_GSCL, 16, 1),
+	MUX(TOP_MOUT_ACLK_GSCL_FIMC, "mout_aclk_gscl_fimc",
+			mout_aclk_gscl_fimc_p,
+			MUX_SEL_TOP_GSCL, 20, 1),
+};
+
+struct samsung_div_clock top_div_clks[] __initdata = {
+	DIV(TOP_DOUT_ACLK_G2D_333, "dout_aclk_g2d_333", "mout_aclk_g2d_333",
+			DIV_TOP_G2D_MFC, 0, 3),
+	DIV(TOP_DOUT_ACLK_MFC_333, "dout_aclk_mfc_333", "mout_aclk_mfc_333",
+			DIV_TOP_G2D_MFC, 4, 3),
+
+	DIV(TOP_DOUT_ACLK_GSCL_333, "dout_aclk_gscl_333", "mout_aclk_gscl_333",
+			DIV_TOP_GSCL_ISP0, 0, 3),
+	DIV(TOP_DOUT_ACLK_GSCL_400, "dout_aclk_gscl_400", "mout_aclk_gscl_400",
+			DIV_TOP_GSCL_ISP0, 4, 3),
+	DIV(TOP_DOUT_ACLK_GSCL_FIMC, "dout_aclk_gscl_fimc",
+			"mout_aclk_gscl_fimc", DIV_TOP_GSCL_ISP0, 8, 3),
+	DIV(TOP_DOUT_SCLK_ISP1_SENSOR0_A, "dout_sclk_isp1_sensor0_a",
+			"mout_aclk_gscl_fimc", DIV_TOP_GSCL_ISP0, 16, 4),
+	DIV(TOP_DOUT_SCLK_ISP1_SENSOR1_A, "dout_sclk_isp1_sensor1_a",
+			"mout_aclk_gscl_400", DIV_TOP_GSCL_ISP0, 20, 4),
+	DIV(TOP_DOUT_SCLK_ISP1_SENSOR2_A, "dout_sclk_isp1_sensor2_a",
+			"mout_aclk_gscl_fimc", DIV_TOP_GSCL_ISP0, 24, 4),
+
+	DIV(TOP_DOUT_ACLK_ISP1_266, "dout_aclk_isp1_266", "mout_aclk_isp1_266",
+			DIV_TOP_ISP10, 0, 3),
+	DIV(TOP_DOUT_ACLK_ISP1_400, "dout_aclk_isp1_400", "mout_aclk_isp1_400",
+			DIV_TOP_ISP10, 4, 3),
+	DIV(TOP_DOUT_SCLK_ISP1_SPI0_A, "dout_sclk_isp1_spi0_a",
+			"mout_sclk_isp1_spi0", DIV_TOP_ISP10, 12, 4),
+	DIV(TOP_DOUT_SCLK_ISP1_SPI0_B, "dout_sclk_isp1_spi0_b",
+			"dout_sclk_isp1_spi0_a", DIV_TOP_ISP10, 16, 8),
+
+	DIV(TOP_DOUT_SCLK_ISP1_SPI1_A, "dout_sclk_isp1_spi1_a",
+			"mout_sclk_isp1_spi1", DIV_TOP_ISP11, 0, 4),
+	DIV(TOP_DOUT_SCLK_ISP1_SPI1_B, "dout_sclk_isp1_spi1_b",
+			"dout_sclk_isp1_spi1_a", DIV_TOP_ISP11, 4, 8),
+	DIV(TOP_DOUT_SCLK_ISP1_UART, "dout_sclk_isp1_uart",
+			"mout_sclk_isp1_uart", DIV_TOP_ISP11, 12, 4),
+	DIV(TOP_DOUT_SCLK_ISP1_SENSOR0_B, "dout_sclk_isp1_sensor0_b",
+			"dout_sclk_isp1_sensor0_a", DIV_TOP_ISP11, 16, 4),
+	DIV(TOP_DOUT_SCLK_ISP1_SENSOR1_B, "dout_sclk_isp1_sensor1_b",
+			"dout_sclk_isp1_sensor1_a", DIV_TOP_ISP11, 20, 4),
+	DIV(TOP_DOUT_SCLK_ISP1_SENSOR2_B, "dout_sclk_isp1_sensor2_b",
+			"dout_sclk_isp1_sensor2_a", DIV_TOP_ISP11, 24, 4),
+
+	DIV(TOP_DOUTTOP__SCLK_HPM_TARGETCLK, "dout_sclk_hpm_targetclk",
+			"mout_bustop_pll_user", DIV_TOP_HPM, 0, 3),
+
+	DIV(TOP_DOUT_ACLK_DISP_333, "dout_aclk_disp_333", "mout_aclk_disp_333",
+			DIV_TOP_DISP, 0, 3),
+	DIV(TOP_DOUT_ACLK_DISP_222, "dout_aclk_disp_222", "mout_aclk_disp_222",
+			DIV_TOP_DISP, 4, 3),
+	DIV(TOP_DOUT_SCLK_DISP_PIXEL, "dout_sclk_disp_pixel",
+			"mout_sclk_disp_pixel",	DIV_TOP_DISP, 8, 3),
+
+	DIV(TOP_DOUT_ACLK_BUS1_400, "dout_aclk_bus1_400",
+			"mout_bus1_bustop_400",	DIV_TOP_BUS, 0, 3),
+	DIV(TOP_DOUT_ACLK_BUS1_100, "dout_aclk_bus1_100",
+			"mout_bus1_bustop_100",	DIV_TOP_BUS, 4, 4),
+	DIV(TOP_DOUT_ACLK_BUS2_400, "dout_aclk_bus2_400",
+			"mout_bus2_bustop_400",	DIV_TOP_BUS, 8, 3),
+	DIV(TOP_DOUT_ACLK_BUS2_100, "dout_aclk_bus2_100",
+			"mout_bus2_bustop_100",	DIV_TOP_BUS, 12, 4),
+	DIV(TOP_DOUT_ACLK_BUS3_400, "dout_aclk_bus3_400",
+			"mout_bus3_bustop_400", DIV_TOP_BUS, 16, 3),
+	DIV(TOP_DOUT_ACLK_BUS3_100, "dout_aclk_bus3_100",
+			"mout_bus3_bustop_100",	DIV_TOP_BUS, 20, 4),
+	DIV(TOP_DOUT_ACLK_BUS4_400, "dout_aclk_bus4_400",
+			"mout_bus4_bustop_400",	DIV_TOP_BUS, 24, 3),
+	DIV(TOP_DOUT_ACLK_BUS4_100, "dout_aclk_bus4_100",
+			"mout_bus4_bustop_100",	DIV_TOP_BUS, 28, 4),
+
+	DIV(TOP_DOUT_SCLK_PERI_SPI0_A, "dout_sclk_peri_spi0_a",
+			"mout_sclk_peri_spi0_clk", DIV_TOP_PERI0, 4, 4),
+	DIV(TOP_DOUT_SCLK_PERI_SPI0_B, "dout_sclk_peri_spi0_b",
+			"dout_sclk_peri_spi0_a", DIV_TOP_PERI0, 8, 8),
+	DIV(TOP_DOUT_SCLK_PERI_SPI1_A, "dout_sclk_peri_spi1_a",
+			"mout_sclk_peri_spi1_clk", DIV_TOP_PERI0, 16, 4),
+	DIV(TOP_DOUT_SCLK_PERI_SPI1_B, "dout_sclk_peri_spi1_b",
+			"dout_sclk_peri_spi1_a", DIV_TOP_PERI0, 20, 8),
+
+	DIV(TOP_DOUT_SCLK_PERI_SPI2_A, "dout_sclk_peri_spi2_a",
+			"mout_sclk_peri_spi2_clk", DIV_TOP_PERI1, 0, 4),
+	DIV(TOP_DOUT_SCLK_PERI_SPI2_B, "dout_sclk_peri_spi2_b",
+			"dout_sclk_peri_spi2_a", DIV_TOP_PERI1, 4, 8),
+	DIV(TOP_DOUT_SCLK_PERI_UART1, "dout_sclk_peri_uart1",
+			"mout_sclk_peri_uart1_uclk", DIV_TOP_PERI1, 16, 4),
+	DIV(TOP_DOUT_SCLK_PERI_UART2, "dout_sclk_peri_uart2",
+			"mout_sclk_peri_uart2_uclk", DIV_TOP_PERI1, 20, 4),
+	DIV(TOP_DOUT_SCLK_PERI_UART0, "dout_sclk_peri_uart0",
+			"mout_sclk_peri_uart0_uclk", DIV_TOP_PERI1, 24, 4),
+
+	DIV(TOP_DOUT_ACLK_PERI_66, "dout_aclk_peri_66", "mout_bustop_pll_user",
+			DIV_TOP_PERI2, 20, 4),
+	DIV(TOP_DOUT_ACLK_PERI_AUD, "dout_aclk_peri_aud",
+			"mout_audtop_pll_user", DIV_TOP_PERI2, 24, 3),
+
+	DIV(TOP_DOUT_ACLK_FSYS_200, "dout_aclk_fsys_200",
+			"mout_bustop_pll_user", DIV_TOP_FSYS0, 0, 3),
+	DIV(TOP_DOUT_SCLK_FSYS_USBDRD30_SUSPEND_CLK,
+			"dout_sclk_fsys_usbdrd30_suspend_clk",
+			"mout_sclk_fsys_usb", DIV_TOP_FSYS0, 4, 4),
+	DIV(TOP_DOUT_SCLK_FSYS_MMC0_SDCLKIN_A, "dout_sclk_fsys_mmc0_sdclkin_a",
+			"mout_sclk_fsys_mmc0_sdclkin_b",
+			DIV_TOP_FSYS0, 12, 4),
+	DIV(TOP_DOUT_SCLK_FSYS_MMC0_SDCLKIN_B, "dout_sclk_fsys_mmc0_sdclkin_b",
+			"dout_sclk_fsys_mmc0_sdclkin_a",
+			DIV_TOP_FSYS0, 16, 8),
+
+
+	DIV(TOP_DOUT_SCLK_FSYS_MMC1_SDCLKIN_A, "dout_sclk_fsys_mmc1_sdclkin_a",
+			"mout_sclk_fsys_mmc1_sdclkin_b",
+			DIV_TOP_FSYS1, 0, 4),
+	DIV(TOP_DOUT_SCLK_FSYS_MMC1_SDCLKIN_B, "dout_sclk_fsys_mmc1_sdclkin_b",
+			"dout_sclk_fsys_mmc1_sdclkin_a",
+			DIV_TOP_FSYS1, 4, 8),
+	DIV(TOP_DOUT_SCLK_FSYS_MMC2_SDCLKIN_A, "dout_sclk_fsys_mmc2_sdclkin_a",
+			"mout_sclk_fsys_mmc2_sdclkin_b",
+			DIV_TOP_FSYS1, 12, 4),
+	DIV(TOP_DOUT_SCLK_FSYS_MMC2_SDCLKIN_B, "dout_sclk_fsys_mmc2_sdclkin_b",
+			"dout_sclk_fsys_mmc2_sdclkin_a",
+			DIV_TOP_FSYS1, 16, 8),
+
+};
+
+struct samsung_gate_clock top_gate_clks[] __initdata = {
+	GATE(TOP_SCLK_MMC0, "sclk_fsys_mmc0_sdclkin",
+			"dout_sclk_fsys_mmc0_sdclkin_b",
+			EN_SCLK_TOP, 7, CLK_SET_RATE_PARENT, 0),
+	GATE(TOP_SCLK_MMC1, "sclk_fsys_mmc1_sdclkin",
+			"dout_sclk_fsys_mmc1_sdclkin_b",
+			EN_SCLK_TOP, 8, CLK_SET_RATE_PARENT, 0),
+	GATE(TOP_SCLK_MMC2, "sclk_fsys_mmc2_sdclkin",
+			"dout_sclk_fsys_mmc2_sdclkin_b",
+			EN_SCLK_TOP, 9, CLK_SET_RATE_PARENT, 0),
+	GATE(TOP_SCLK_FIMD1, "sclk_disp_pixel", "dout_sclk_disp_pixel",
+			EN_ACLK_TOP, 10, CLK_IGNORE_UNUSED |
+			CLK_SET_RATE_PARENT, 0),
+};
+
+static struct samsung_pll_clock top_pll_clks[] __initdata = {
+	PLL(pll_2550xx, TOP_FOUT_DISP_PLL, "fout_disp_pll", "fin_pll",
+		DISP_PLL_LOCK, DISP_PLL_CON0,
+		pll2550_24mhz_tbl),
+	PLL(pll_2650xx, TOP_FOUT_AUD_PLL, "fout_aud_pll", "fin_pll",
+		AUD_PLL_LOCK, AUD_PLL_CON0,
+		pll2650_24mhz_tbl),
+};
+
+static void __init exynos5260_clk_top_init(struct device_node *np)
+{
+	struct exynos5260_cmu_info cmu = {0};
+
+	cmu.pll_clks = top_pll_clks;
+	cmu.nr_pll_clks =  ARRAY_SIZE(top_pll_clks);
+	cmu.mux_clks = top_mux_clks;
+	cmu.nr_mux_clks = ARRAY_SIZE(top_mux_clks);
+	cmu.div_clks = top_div_clks;
+	cmu.nr_div_clks = ARRAY_SIZE(top_div_clks);
+	cmu.gate_clks = top_gate_clks;
+	cmu.nr_gate_clks = ARRAY_SIZE(top_gate_clks);
+	cmu.fixed_clks = fixed_rate_clks;
+	cmu.nr_fixed_clks = ARRAY_SIZE(fixed_rate_clks);
+	cmu.nr_clk_ids = TOP_NR_CLK;
+	cmu.clk_regs = top_clk_regs;
+	cmu.nr_clk_regs = ARRAY_SIZE(top_clk_regs);
+
+	exynos5260_cmu_register_one(np, &cmu);
+}
+
+CLK_OF_DECLARE(exynos5260_clk_top, "samsung,exynos5260-clock-top",
+		exynos5260_clk_top_init);
diff --git a/drivers/clk/samsung/clk-exynos5260.h b/drivers/clk/samsung/clk-exynos5260.h
new file mode 100644
index 00000000000..d739716d6ea
--- /dev/null
+++ b/drivers/clk/samsung/clk-exynos5260.h
@@ -0,0 +1,459 @@
+/*
+ * Copyright (c) 2014 Samsung Electronics Co., Ltd.
+ * Author: Rahul Sharma <rahul.sharma@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.
+ *
+ * Common Clock Framework support for Exynos5260 SoC.
+ */
+
+#ifndef __CLK_EXYNOS5260_H
+#define __CLK_EXYNOS5260_H
+
+/*
+*Registers for CMU_AUD
+*/
+#define MUX_SEL_AUD				0x0200
+#define MUX_ENABLE_AUD				0x0300
+#define MUX_STAT_AUD				0x0400
+#define MUX_IGNORE_AUD				0x0500
+#define DIV_AUD0				0x0600
+#define DIV_AUD1				0x0604
+#define DIV_STAT_AUD0				0x0700
+#define DIV_STAT_AUD1				0x0704
+#define EN_ACLK_AUD				0x0800
+#define EN_PCLK_AUD				0x0900
+#define EN_SCLK_AUD				0x0a00
+#define EN_IP_AUD				0x0b00
+
+/*
+*Registers for CMU_DISP
+*/
+#define MUX_SEL_DISP0				0x0200
+#define MUX_SEL_DISP1				0x0204
+#define MUX_SEL_DISP2				0x0208
+#define MUX_SEL_DISP3				0x020C
+#define MUX_SEL_DISP4				0x0210
+#define MUX_ENABLE_DISP0			0x0300
+#define MUX_ENABLE_DISP1			0x0304
+#define MUX_ENABLE_DISP2			0x0308
+#define MUX_ENABLE_DISP3			0x030c
+#define MUX_ENABLE_DISP4			0x0310
+#define MUX_STAT_DISP0				0x0400
+#define MUX_STAT_DISP1				0x0404
+#define MUX_STAT_DISP2				0x0408
+#define MUX_STAT_DISP3				0x040c
+#define MUX_STAT_DISP4				0x0410
+#define MUX_IGNORE_DISP0			0x0500
+#define MUX_IGNORE_DISP1			0x0504
+#define MUX_IGNORE_DISP2			0x0508
+#define MUX_IGNORE_DISP3			0x050c
+#define MUX_IGNORE_DISP4			0x0510
+#define DIV_DISP				0x0600
+#define DIV_STAT_DISP				0x0700
+#define EN_ACLK_DISP				0x0800
+#define EN_PCLK_DISP				0x0900
+#define EN_SCLK_DISP0				0x0a00
+#define EN_SCLK_DISP1				0x0a04
+#define EN_IP_DISP				0x0b00
+#define EN_IP_DISP_BUS				0x0b04
+
+
+/*
+*Registers for CMU_EGL
+*/
+#define EGL_PLL_LOCK				0x0000
+#define EGL_DPLL_LOCK				0x0004
+#define EGL_PLL_CON0				0x0100
+#define EGL_PLL_CON1				0x0104
+#define EGL_PLL_FREQ_DET			0x010c
+#define EGL_DPLL_CON0				0x0110
+#define EGL_DPLL_CON1				0x0114
+#define EGL_DPLL_FREQ_DET			0x011c
+#define MUX_SEL_EGL				0x0200
+#define MUX_ENABLE_EGL				0x0300
+#define MUX_STAT_EGL				0x0400
+#define DIV_EGL					0x0600
+#define DIV_EGL_PLL_FDET			0x0604
+#define DIV_STAT_EGL				0x0700
+#define DIV_STAT_EGL_PLL_FDET			0x0704
+#define EN_ACLK_EGL				0x0800
+#define EN_PCLK_EGL				0x0900
+#define EN_SCLK_EGL				0x0a00
+#define EN_IP_EGL				0x0b00
+#define CLKOUT_CMU_EGL				0x0c00
+#define CLKOUT_CMU_EGL_DIV_STAT			0x0c04
+#define ARMCLK_STOPCTRL				0x1000
+#define EAGLE_EMA_CTRL				0x1008
+#define EAGLE_EMA_STATUS			0x100c
+#define PWR_CTRL				0x1020
+#define PWR_CTRL2				0x1024
+#define CLKSTOP_CTRL				0x1028
+#define INTR_SPREAD_EN				0x1080
+#define INTR_SPREAD_USE_STANDBYWFI		0x1084
+#define INTR_SPREAD_BLOCKING_DURATION		0x1088
+#define CMU_EGL_SPARE0				0x2000
+#define CMU_EGL_SPARE1				0x2004
+#define CMU_EGL_SPARE2				0x2008
+#define CMU_EGL_SPARE3				0x200c
+#define CMU_EGL_SPARE4				0x2010
+
+/*
+*Registers for CMU_FSYS
+*/
+
+#define MUX_SEL_FSYS0				0x0200
+#define MUX_SEL_FSYS1				0x0204
+#define MUX_ENABLE_FSYS0			0x0300
+#define MUX_ENABLE_FSYS1			0x0304
+#define MUX_STAT_FSYS0				0x0400
+#define MUX_STAT_FSYS1				0x0404
+#define MUX_IGNORE_FSYS0			0x0500
+#define MUX_IGNORE_FSYS1			0x0504
+#define EN_ACLK_FSYS				0x0800
+#define EN_ACLK_FSYS_SECURE_RTIC		0x0804
+#define EN_ACLK_FSYS_SECURE_SMMU_RTIC		0x0808
+#define EN_PCLK_FSYS				0x0900
+#define EN_SCLK_FSYS				0x0a00
+#define EN_IP_FSYS				0x0b00
+#define EN_IP_FSYS_SECURE_RTIC			0x0b04
+#define EN_IP_FSYS_SECURE_SMMU_RTIC		0x0b08
+
+/*
+*Registers for CMU_G2D
+*/
+
+#define MUX_SEL_G2D				0x0200
+#define MUX_ENABLE_G2D				0x0300
+#define MUX_STAT_G2D				0x0400
+#define DIV_G2D					0x0600
+#define DIV_STAT_G2D				0x0700
+#define EN_ACLK_G2D				0x0800
+#define EN_ACLK_G2D_SECURE_SSS			0x0804
+#define EN_ACLK_G2D_SECURE_SLIM_SSS		0x0808
+#define EN_ACLK_G2D_SECURE_SMMU_SLIM_SSS	0x080c
+#define EN_ACLK_G2D_SECURE_SMMU_SSS		0x0810
+#define EN_ACLK_G2D_SECURE_SMMU_MDMA		0x0814
+#define EN_ACLK_G2D_SECURE_SMMU_G2D		0x0818
+#define EN_PCLK_G2D				0x0900
+#define EN_PCLK_G2D_SECURE_SMMU_SLIM_SSS	0x0904
+#define EN_PCLK_G2D_SECURE_SMMU_SSS		0x0908
+#define EN_PCLK_G2D_SECURE_SMMU_MDMA		0x090c
+#define EN_PCLK_G2D_SECURE_SMMU_G2D		0x0910
+#define EN_IP_G2D				0x0b00
+#define EN_IP_G2D_SECURE_SSS			0x0b04
+#define EN_IP_G2D_SECURE_SLIM_SSS		0x0b08
+#define EN_IP_G2D_SECURE_SMMU_SLIM_SSS		0x0b0c
+#define EN_IP_G2D_SECURE_SMMU_SSS		0x0b10
+#define EN_IP_G2D_SECURE_SMMU_MDMA		0x0b14
+#define EN_IP_G2D_SECURE_SMMU_G2D		0x0b18
+
+/*
+*Registers for CMU_G3D
+*/
+
+#define G3D_PLL_LOCK				0x0000
+#define G3D_PLL_CON0				0x0100
+#define G3D_PLL_CON1				0x0104
+#define G3D_PLL_FDET				0x010c
+#define MUX_SEL_G3D				0x0200
+#define MUX_EN_G3D				0x0300
+#define MUX_STAT_G3D				0x0400
+#define MUX_IGNORE_G3D				0x0500
+#define DIV_G3D					0x0600
+#define DIV_G3D_PLL_FDET			0x0604
+#define DIV_STAT_G3D				0x0700
+#define DIV_STAT_G3D_PLL_FDET			0x0704
+#define EN_ACLK_G3D				0x0800
+#define EN_PCLK_G3D				0x0900
+#define EN_SCLK_G3D				0x0a00
+#define EN_IP_G3D				0x0b00
+#define CLKOUT_CMU_G3D				0x0c00
+#define CLKOUT_CMU_G3D_DIV_STAT			0x0c04
+#define G3DCLK_STOPCTRL				0x1000
+#define G3D_EMA_CTRL				0x1008
+#define G3D_EMA_STATUS				0x100c
+
+/*
+*Registers for CMU_GSCL
+*/
+
+#define MUX_SEL_GSCL				0x0200
+#define MUX_EN_GSCL				0x0300
+#define MUX_STAT_GSCL				0x0400
+#define MUX_IGNORE_GSCL				0x0500
+#define DIV_GSCL				0x0600
+#define DIV_STAT_GSCL				0x0700
+#define EN_ACLK_GSCL				0x0800
+#define EN_ACLK_GSCL_FIMC			0x0804
+#define EN_ACLK_GSCL_SECURE_SMMU_GSCL0		0x0808
+#define EN_ACLK_GSCL_SECURE_SMMU_GSCL1		0x080c
+#define EN_ACLK_GSCL_SECURE_SMMU_MSCL0		0x0810
+#define EN_ACLK_GSCL_SECURE_SMMU_MSCL1		0x0814
+#define EN_PCLK_GSCL				0x0900
+#define EN_PCLK_GSCL_FIMC			0x0904
+#define EN_PCLK_GSCL_SECURE_SMMU_GSCL0		0x0908
+#define EN_PCLK_GSCL_SECURE_SMMU_GSCL1		0x090c
+#define EN_PCLK_GSCL_SECURE_SMMU_MSCL0		0x0910
+#define EN_PCLK_GSCL_SECURE_SMMU_MSCL1		0x0914
+#define EN_SCLK_GSCL				0x0a00
+#define EN_SCLK_GSCL_FIMC			0x0a04
+#define EN_IP_GSCL				0x0b00
+#define EN_IP_GSCL_FIMC				0x0b04
+#define EN_IP_GSCL_SECURE_SMMU_GSCL0		0x0b08
+#define EN_IP_GSCL_SECURE_SMMU_GSCL1		0x0b0c
+#define EN_IP_GSCL_SECURE_SMMU_MSCL0		0x0b10
+#define EN_IP_GSCL_SECURE_SMMU_MSCL1		0x0b14
+
+/*
+*Registers for CMU_ISP
+*/
+#define MUX_SEL_ISP0				0x0200
+#define MUX_SEL_ISP1				0x0204
+#define MUX_ENABLE_ISP0				0x0300
+#define MUX_ENABLE_ISP1				0x0304
+#define MUX_STAT_ISP0				0x0400
+#define MUX_STAT_ISP1				0x0404
+#define MUX_IGNORE_ISP0				0x0500
+#define MUX_IGNORE_ISP1				0x0504
+#define DIV_ISP					0x0600
+#define DIV_STAT_ISP				0x0700
+#define EN_ACLK_ISP0				0x0800
+#define EN_ACLK_ISP1				0x0804
+#define EN_PCLK_ISP0				0x0900
+#define EN_PCLK_ISP1				0x0904
+#define EN_SCLK_ISP				0x0a00
+#define EN_IP_ISP0				0x0b00
+#define EN_IP_ISP1				0x0b04
+
+/*
+*Registers for CMU_KFC
+*/
+#define KFC_PLL_LOCK				0x0000
+#define KFC_PLL_CON0				0x0100
+#define KFC_PLL_CON1				0x0104
+#define KFC_PLL_FDET				0x010c
+#define MUX_SEL_KFC0				0x0200
+#define MUX_SEL_KFC2				0x0208
+#define MUX_ENABLE_KFC0				0x0300
+#define MUX_ENABLE_KFC2				0x0308
+#define MUX_STAT_KFC0				0x0400
+#define MUX_STAT_KFC2				0x0408
+#define DIV_KFC					0x0600
+#define DIV_KFC_PLL_FDET			0x0604
+#define DIV_STAT_KFC				0x0700
+#define DIV_STAT_KFC_PLL_FDET			0x0704
+#define EN_ACLK_KFC				0x0800
+#define EN_PCLK_KFC				0x0900
+#define EN_SCLK_KFC				0x0a00
+#define EN_IP_KFC				0x0b00
+#define CLKOUT_CMU_KFC				0x0c00
+#define CLKOUT_CMU_KFC_DIV_STAT			0x0c04
+#define ARMCLK_STOPCTRL_KFC			0x1000
+#define ARM_EMA_CTRL				0x1008
+#define ARM_EMA_STATUS				0x100c
+#define PWR_CTRL_KFC				0x1020
+#define PWR_CTRL2_KFC				0x1024
+#define CLKSTOP_CTRL_KFC			0x1028
+#define INTR_SPREAD_ENABLE_KFC			0x1080
+#define INTR_SPREAD_USE_STANDBYWFI_KFC		0x1084
+#define INTR_SPREAD_BLOCKING_DURATION_KFC	0x1088
+#define CMU_KFC_SPARE0				0x2000
+#define CMU_KFC_SPARE1				0x2004
+#define CMU_KFC_SPARE2				0x2008
+#define CMU_KFC_SPARE3				0x200c
+#define CMU_KFC_SPARE4				0x2010
+
+/*
+*Registers for CMU_MFC
+*/
+#define MUX_SEL_MFC				0x0200
+#define MUX_ENABLE_MFC				0x0300
+#define MUX_STAT_MFC				0x0400
+#define DIV_MFC					0x0600
+#define DIV_STAT_MFC				0x0700
+#define EN_ACLK_MFC				0x0800
+#define EN_ACLK_SECURE_SMMU2_MFC		0x0804
+#define EN_PCLK_MFC				0x0900
+#define EN_PCLK_SECURE_SMMU2_MFC		0x0904
+#define EN_IP_MFC				0x0b00
+#define EN_IP_MFC_SECURE_SMMU2_MFC		0x0b04
+
+/*
+*Registers for CMU_MIF
+*/
+#define MEM_PLL_LOCK				0x0000
+#define BUS_PLL_LOCK				0x0004
+#define MEDIA_PLL_LOCK				0x0008
+#define MEM_PLL_CON0				0x0100
+#define MEM_PLL_CON1				0x0104
+#define MEM_PLL_FDET				0x010c
+#define BUS_PLL_CON0				0x0110
+#define BUS_PLL_CON1				0x0114
+#define BUS_PLL_FDET				0x011c
+#define MEDIA_PLL_CON0				0x0120
+#define MEDIA_PLL_CON1				0x0124
+#define MEDIA_PLL_FDET				0x012c
+#define MUX_SEL_MIF				0x0200
+#define MUX_ENABLE_MIF				0x0300
+#define MUX_STAT_MIF				0x0400
+#define MUX_IGNORE_MIF				0x0500
+#define DIV_MIF					0x0600
+#define DIV_MIF_PLL_FDET			0x0604
+#define DIV_STAT_MIF				0x0700
+#define DIV_STAT_MIF_PLL_FDET			0x0704
+#define EN_ACLK_MIF				0x0800
+#define EN_ACLK_MIF_SECURE_DREX1_TZ		0x0804
+#define EN_ACLK_MIF_SECURE_DREX0_TZ		0x0808
+#define EN_ACLK_MIF_SECURE_INTMEM		0x080c
+#define EN_PCLK_MIF				0x0900
+#define EN_PCLK_MIF_SECURE_MONOCNT		0x0904
+#define EN_PCLK_MIF_SECURE_RTC_APBIF		0x0908
+#define EN_PCLK_MIF_SECURE_DREX1_TZ		0x090c
+#define EN_PCLK_MIF_SECURE_DREX0_TZ		0x0910
+#define EN_SCLK_MIF				0x0a00
+#define EN_IP_MIF				0x0b00
+#define EN_IP_MIF_SECURE_MONOCNT		0x0b04
+#define EN_IP_MIF_SECURE_RTC_APBIF		0x0b08
+#define EN_IP_MIF_SECURE_DREX1_TZ		0x0b0c
+#define EN_IP_MIF_SECURE_DREX0_TZ		0x0b10
+#define EN_IP_MIF_SECURE_INTEMEM		0x0b14
+#define CLKOUT_CMU_MIF_DIV_STAT			0x0c04
+#define DREX_FREQ_CTRL				0x1000
+#define PAUSE					0x1004
+#define DDRPHY_LOCK_CTRL			0x1008
+#define CLKOUT_CMU_MIF				0xcb00
+
+/*
+*Registers for CMU_PERI
+*/
+#define MUX_SEL_PERI				0x0200
+#define MUX_SEL_PERI1				0x0204
+#define MUX_ENABLE_PERI				0x0300
+#define MUX_ENABLE_PERI1			0x0304
+#define MUX_STAT_PERI				0x0400
+#define MUX_STAT_PERI1				0x0404
+#define MUX_IGNORE_PERI				0x0500
+#define MUX_IGNORE_PERI1			0x0504
+#define DIV_PERI				0x0600
+#define DIV_STAT_PERI				0x0700
+#define EN_PCLK_PERI0				0x0800
+#define EN_PCLK_PERI1				0x0804
+#define EN_PCLK_PERI2				0x0808
+#define EN_PCLK_PERI3				0x080c
+#define EN_PCLK_PERI_SECURE_CHIPID		0x0810
+#define EN_PCLK_PERI_SECURE_PROVKEY0		0x0814
+#define EN_PCLK_PERI_SECURE_PROVKEY1		0x0818
+#define EN_PCLK_PERI_SECURE_SECKEY		0x081c
+#define EN_PCLK_PERI_SECURE_ANTIRBKCNT		0x0820
+#define EN_PCLK_PERI_SECURE_TOP_RTC		0x0824
+#define EN_PCLK_PERI_SECURE_TZPC		0x0828
+#define EN_SCLK_PERI				0x0a00
+#define EN_SCLK_PERI_SECURE_TOP_RTC		0x0a04
+#define EN_IP_PERI0				0x0b00
+#define EN_IP_PERI1				0x0b04
+#define EN_IP_PERI2				0x0b08
+#define EN_IP_PERI_SECURE_CHIPID		0x0b0c
+#define EN_IP_PERI_SECURE_PROVKEY0		0x0b10
+#define EN_IP_PERI_SECURE_PROVKEY1		0x0b14
+#define EN_IP_PERI_SECURE_SECKEY		0x0b18
+#define EN_IP_PERI_SECURE_ANTIRBKCNT		0x0b1c
+#define EN_IP_PERI_SECURE_TOP_RTC		0x0b20
+#define EN_IP_PERI_SECURE_TZPC			0x0b24
+
+/*
+*Registers for CMU_TOP
+*/
+#define DISP_PLL_LOCK				0x0000
+#define AUD_PLL_LOCK				0x0004
+#define DISP_PLL_CON0				0x0100
+#define DISP_PLL_CON1				0x0104
+#define DISP_PLL_FDET				0x0108
+#define AUD_PLL_CON0				0x0110
+#define AUD_PLL_CON1				0x0114
+#define AUD_PLL_CON2				0x0118
+#define AUD_PLL_FDET				0x011c
+#define MUX_SEL_TOP_PLL0			0x0200
+#define MUX_SEL_TOP_MFC				0x0204
+#define MUX_SEL_TOP_G2D				0x0208
+#define MUX_SEL_TOP_GSCL			0x020c
+#define MUX_SEL_TOP_ISP10			0x0214
+#define MUX_SEL_TOP_ISP11			0x0218
+#define MUX_SEL_TOP_DISP0			0x021c
+#define MUX_SEL_TOP_DISP1			0x0220
+#define MUX_SEL_TOP_BUS				0x0224
+#define MUX_SEL_TOP_PERI0			0x0228
+#define MUX_SEL_TOP_PERI1			0x022c
+#define MUX_SEL_TOP_FSYS			0x0230
+#define MUX_ENABLE_TOP_PLL0			0x0300
+#define MUX_ENABLE_TOP_MFC			0x0304
+#define MUX_ENABLE_TOP_G2D			0x0308
+#define MUX_ENABLE_TOP_GSCL			0x030c
+#define MUX_ENABLE_TOP_ISP10			0x0314
+#define MUX_ENABLE_TOP_ISP11			0x0318
+#define MUX_ENABLE_TOP_DISP0			0x031c
+#define MUX_ENABLE_TOP_DISP1			0x0320
+#define MUX_ENABLE_TOP_BUS			0x0324
+#define MUX_ENABLE_TOP_PERI0			0x0328
+#define MUX_ENABLE_TOP_PERI1			0x032c
+#define MUX_ENABLE_TOP_FSYS			0x0330
+#define MUX_STAT_TOP_PLL0			0x0400
+#define MUX_STAT_TOP_MFC			0x0404
+#define MUX_STAT_TOP_G2D			0x0408
+#define MUX_STAT_TOP_GSCL			0x040c
+#define MUX_STAT_TOP_ISP10			0x0414
+#define MUX_STAT_TOP_ISP11			0x0418
+#define MUX_STAT_TOP_DISP0			0x041c
+#define MUX_STAT_TOP_DISP1			0x0420
+#define MUX_STAT_TOP_BUS			0x0424
+#define MUX_STAT_TOP_PERI0			0x0428
+#define MUX_STAT_TOP_PERI1			0x042c
+#define MUX_STAT_TOP_FSYS			0x0430
+#define MUX_IGNORE_TOP_PLL0			0x0500
+#define MUX_IGNORE_TOP_MFC			0x0504
+#define MUX_IGNORE_TOP_G2D			0x0508
+#define MUX_IGNORE_TOP_GSCL			0x050c
+#define MUX_IGNORE_TOP_ISP10			0x0514
+#define MUX_IGNORE_TOP_ISP11			0x0518
+#define MUX_IGNORE_TOP_DISP0			0x051c
+#define MUX_IGNORE_TOP_DISP1			0x0520
+#define MUX_IGNORE_TOP_BUS			0x0524
+#define MUX_IGNORE_TOP_PERI0			0x0528
+#define MUX_IGNORE_TOP_PERI1			0x052c
+#define MUX_IGNORE_TOP_FSYS			0x0530
+#define DIV_TOP_G2D_MFC				0x0600
+#define DIV_TOP_GSCL_ISP0			0x0604
+#define DIV_TOP_ISP10				0x0608
+#define DIV_TOP_ISP11				0x060c
+#define DIV_TOP_DISP				0x0610
+#define DIV_TOP_BUS				0x0614
+#define DIV_TOP_PERI0				0x0618
+#define DIV_TOP_PERI1				0x061c
+#define DIV_TOP_PERI2				0x0620
+#define DIV_TOP_FSYS0				0x0624
+#define DIV_TOP_FSYS1				0x0628
+#define DIV_TOP_HPM				0x062c
+#define DIV_TOP_PLL_FDET			0x0630
+#define DIV_STAT_TOP_G2D_MFC			0x0700
+#define DIV_STAT_TOP_GSCL_ISP0			0x0704
+#define DIV_STAT_TOP_ISP10			0x0708
+#define DIV_STAT_TOP_ISP11			0x070c
+#define DIV_STAT_TOP_DISP			0x0710
+#define DIV_STAT_TOP_BUS			0x0714
+#define DIV_STAT_TOP_PERI0			0x0718
+#define DIV_STAT_TOP_PERI1			0x071c
+#define DIV_STAT_TOP_PERI2			0x0720
+#define DIV_STAT_TOP_FSYS0			0x0724
+#define DIV_STAT_TOP_FSYS1			0x0728
+#define DIV_STAT_TOP_HPM			0x072c
+#define DIV_STAT_TOP_PLL_FDET			0x0730
+#define EN_ACLK_TOP				0x0800
+#define EN_SCLK_TOP				0x0a00
+#define EN_IP_TOP				0x0b00
+#define CLKOUT_CMU_TOP				0x0c00
+#define CLKOUT_CMU_TOP_DIV_STAT			0x0c04
+
+#endif /*__CLK_EXYNOS5260_H */
+
diff --git a/drivers/clk/samsung/clk-exynos5410.c b/drivers/clk/samsung/clk-exynos5410.c
new file mode 100644
index 00000000000..c9505ab9ee7
--- /dev/null
+++ b/drivers/clk/samsung/clk-exynos5410.c
@@ -0,0 +1,209 @@
+/*
+ * Copyright (c) 2013 Samsung Electronics Co., Ltd.
+ * Author: Tarek Dakhran <t.dakhran@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.
+ *
+ * Common Clock Framework support for Exynos5410 SoC.
+*/
+
+#include <dt-bindings/clock/exynos5410.h>
+
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+
+#include "clk.h"
+
+#define APLL_LOCK               0x0
+#define APLL_CON0               0x100
+#define CPLL_LOCK               0x10020
+#define CPLL_CON0               0x10120
+#define MPLL_LOCK               0x4000
+#define MPLL_CON0               0x4100
+#define BPLL_LOCK               0x20010
+#define BPLL_CON0               0x20110
+#define KPLL_LOCK               0x28000
+#define KPLL_CON0               0x28100
+
+#define SRC_CPU			0x200
+#define DIV_CPU0		0x500
+#define SRC_CPERI1		0x4204
+#define DIV_TOP0		0x10510
+#define DIV_TOP1		0x10514
+#define DIV_FSYS1		0x1054c
+#define DIV_FSYS2		0x10550
+#define DIV_PERIC0		0x10558
+#define SRC_TOP0		0x10210
+#define SRC_TOP1		0x10214
+#define SRC_TOP2		0x10218
+#define SRC_FSYS		0x10244
+#define SRC_PERIC0		0x10250
+#define SRC_MASK_FSYS		0x10340
+#define SRC_MASK_PERIC0		0x10350
+#define GATE_BUS_FSYS0		0x10740
+#define GATE_IP_FSYS		0x10944
+#define GATE_IP_PERIC		0x10950
+#define GATE_IP_PERIS		0x10960
+#define SRC_CDREX		0x20200
+#define SRC_KFC			0x28200
+#define DIV_KFC0		0x28500
+
+/* list of PLLs */
+enum exynos5410_plls {
+	apll, cpll, mpll,
+	bpll, kpll,
+	nr_plls                 /* number of PLLs */
+};
+
+/* list of all parent clocks */
+PNAME(apll_p)		= { "fin_pll", "fout_apll", };
+PNAME(bpll_p)		= { "fin_pll", "fout_bpll", };
+PNAME(cpll_p)		= { "fin_pll", "fout_cpll" };
+PNAME(mpll_p)		= { "fin_pll", "fout_mpll", };
+PNAME(kpll_p)		= { "fin_pll", "fout_kpll", };
+
+PNAME(mout_cpu_p)	= { "mout_apll", "sclk_mpll", };
+PNAME(mout_kfc_p)	= { "mout_kpll", "sclk_mpll", };
+
+PNAME(mpll_user_p)	= { "fin_pll", "sclk_mpll", };
+PNAME(bpll_user_p)	= { "fin_pll", "sclk_bpll", };
+PNAME(mpll_bpll_p)	= { "sclk_mpll_muxed", "sclk_bpll_muxed", };
+
+PNAME(group2_p)		= { "fin_pll", "fin_pll", "none", "none",
+			"none", "none", "sclk_mpll_bpll",
+			 "none", "none", "sclk_cpll" };
+
+static struct samsung_mux_clock exynos5410_mux_clks[] __initdata = {
+	MUX(0, "mout_apll", apll_p, SRC_CPU, 0, 1),
+	MUX(0, "mout_cpu", mout_cpu_p, SRC_CPU, 16, 1),
+
+	MUX(0, "mout_kpll", kpll_p, SRC_KFC, 0, 1),
+	MUX(0, "mout_kfc", mout_kfc_p, SRC_KFC, 16, 1),
+
+	MUX(0, "sclk_mpll", mpll_p, SRC_CPERI1, 8, 1),
+	MUX(0, "sclk_mpll_muxed", mpll_user_p, SRC_TOP2, 20, 1),
+
+	MUX(0, "sclk_bpll", bpll_p, SRC_CDREX, 0, 1),
+	MUX(0, "sclk_bpll_muxed", bpll_user_p, SRC_TOP2, 24, 1),
+
+	MUX(0, "sclk_cpll", cpll_p, SRC_TOP2, 8, 1),
+
+	MUX(0, "sclk_mpll_bpll", mpll_bpll_p, SRC_TOP1, 20, 1),
+
+	MUX(0, "mout_mmc0", group2_p, SRC_FSYS, 0, 4),
+	MUX(0, "mout_mmc1", group2_p, SRC_FSYS, 4, 4),
+	MUX(0, "mout_mmc2", group2_p, SRC_FSYS, 8, 4),
+
+	MUX(0, "mout_uart0", group2_p, SRC_PERIC0, 0, 4),
+	MUX(0, "mout_uart1", group2_p, SRC_PERIC0, 4, 4),
+	MUX(0, "mout_uart2", group2_p, SRC_PERIC0, 8, 4),
+
+	MUX(0, "mout_aclk200", mpll_bpll_p, SRC_TOP0, 12, 1),
+	MUX(0, "mout_aclk400", mpll_bpll_p, SRC_TOP0, 20, 1),
+};
+
+static struct samsung_div_clock exynos5410_div_clks[] __initdata = {
+	DIV(0, "div_arm", "mout_cpu", DIV_CPU0, 0, 3),
+	DIV(0, "div_arm2", "div_arm", DIV_CPU0, 28, 3),
+
+	DIV(0, "div_acp", "div_arm2", DIV_CPU0, 8, 3),
+	DIV(0, "div_cpud", "div_arm2", DIV_CPU0, 4, 3),
+	DIV(0, "div_atb", "div_arm2", DIV_CPU0, 16, 3),
+	DIV(0, "pclk_dbg", "div_arm2", DIV_CPU0, 20, 3),
+
+	DIV(0, "div_kfc", "mout_kfc", DIV_KFC0, 0, 3),
+	DIV(0, "div_aclk", "div_kfc", DIV_KFC0, 4, 3),
+	DIV(0, "div_pclk", "div_kfc", DIV_KFC0, 20, 3),
+
+	DIV(0, "aclk66_pre", "sclk_mpll_muxed", DIV_TOP1, 24, 3),
+	DIV(0, "aclk66", "aclk66_pre", DIV_TOP0, 0, 3),
+
+	DIV(0, "div_mmc0", "mout_mmc0", DIV_FSYS1, 0, 4),
+	DIV(0, "div_mmc1", "mout_mmc1", DIV_FSYS1, 16, 4),
+	DIV(0, "div_mmc2", "mout_mmc2", DIV_FSYS2, 0, 4),
+
+	DIV_F(0, "div_mmc_pre0", "div_mmc0",
+			DIV_FSYS1, 8, 8, CLK_SET_RATE_PARENT, 0),
+	DIV_F(0, "div_mmc_pre1", "div_mmc1",
+			DIV_FSYS1, 24, 8, CLK_SET_RATE_PARENT, 0),
+	DIV_F(0, "div_mmc_pre2", "div_mmc2",
+			DIV_FSYS2, 8, 8, CLK_SET_RATE_PARENT, 0),
+
+	DIV(0, "div_uart0", "mout_uart0", DIV_PERIC0, 0, 4),
+	DIV(0, "div_uart1", "mout_uart1", DIV_PERIC0, 4, 4),
+	DIV(0, "div_uart2", "mout_uart2", DIV_PERIC0, 8, 4),
+	DIV(0, "div_uart3", "mout_uart3", DIV_PERIC0, 12, 4),
+
+	DIV(0, "aclk200", "mout_aclk200", DIV_TOP0, 12, 3),
+	DIV(0, "aclk400", "mout_aclk400", DIV_TOP0, 24, 3),
+};
+
+static struct samsung_gate_clock exynos5410_gate_clks[] __initdata = {
+	GATE(CLK_MCT, "mct", "aclk66", GATE_IP_PERIS, 18, 0, 0),
+
+	GATE(CLK_SCLK_MMC0, "sclk_mmc0", "div_mmc_pre0",
+			SRC_MASK_FSYS, 0, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_MMC1, "sclk_mmc1", "div_mmc_pre1",
+			SRC_MASK_FSYS, 4, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_MMC2, "sclk_mmc2", "div_mmc_pre2",
+			SRC_MASK_FSYS, 8, CLK_SET_RATE_PARENT, 0),
+
+	GATE(CLK_MMC0, "sdmmc0", "aclk200", GATE_BUS_FSYS0, 12, 0, 0),
+	GATE(CLK_MMC1, "sdmmc1", "aclk200", GATE_BUS_FSYS0, 13, 0, 0),
+	GATE(CLK_MMC2, "sdmmc2", "aclk200", GATE_BUS_FSYS0, 14, 0, 0),
+
+	GATE(CLK_UART0, "uart0", "aclk66", GATE_IP_PERIC, 0, 0, 0),
+	GATE(CLK_UART1, "uart1", "aclk66", GATE_IP_PERIC, 1, 0, 0),
+	GATE(CLK_UART2, "uart2", "aclk66", GATE_IP_PERIC, 2, 0, 0),
+
+	GATE(CLK_SCLK_UART0, "sclk_uart0", "div_uart0",
+			SRC_MASK_PERIC0, 0, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_UART1, "sclk_uart1", "div_uart1",
+			SRC_MASK_PERIC0, 4, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_UART2, "sclk_uart2", "div_uart2",
+			SRC_MASK_PERIC0, 8, CLK_SET_RATE_PARENT, 0),
+};
+
+static struct samsung_pll_clock exynos5410_plls[nr_plls] __initdata = {
+	[apll] = PLL(pll_35xx, CLK_FOUT_APLL, "fout_apll", "fin_pll", APLL_LOCK,
+		APLL_CON0, NULL),
+	[cpll] = PLL(pll_35xx, CLK_FOUT_CPLL, "fout_cpll", "fin_pll", CPLL_LOCK,
+		CPLL_CON0, NULL),
+	[mpll] = PLL(pll_35xx, CLK_FOUT_MPLL, "fout_mpll", "fin_pll", MPLL_LOCK,
+		MPLL_CON0, NULL),
+	[bpll] = PLL(pll_35xx, CLK_FOUT_BPLL, "fout_bpll", "fin_pll", BPLL_LOCK,
+		BPLL_CON0, NULL),
+	[kpll] = PLL(pll_35xx, CLK_FOUT_KPLL, "fout_kpll", "fin_pll", KPLL_LOCK,
+		KPLL_CON0, NULL),
+};
+
+/* register exynos5410 clocks */
+static void __init exynos5410_clk_init(struct device_node *np)
+{
+	struct samsung_clk_provider *ctx;
+	void __iomem *reg_base;
+
+	reg_base = of_iomap(np, 0);
+	if (!reg_base)
+		panic("%s: failed to map registers\n", __func__);
+
+	ctx = samsung_clk_init(np, reg_base, CLK_NR_CLKS);
+
+	samsung_clk_register_pll(ctx, exynos5410_plls,
+			ARRAY_SIZE(exynos5410_plls), reg_base);
+
+	samsung_clk_register_mux(ctx, exynos5410_mux_clks,
+			ARRAY_SIZE(exynos5410_mux_clks));
+	samsung_clk_register_div(ctx, exynos5410_div_clks,
+			ARRAY_SIZE(exynos5410_div_clks));
+	samsung_clk_register_gate(ctx, exynos5410_gate_clks,
+			ARRAY_SIZE(exynos5410_gate_clks));
+
+	pr_debug("Exynos5410: clock setup completed.\n");
+}
+CLK_OF_DECLARE(exynos5410_clk, "samsung,exynos5410-clock", exynos5410_clk_init);
diff --git a/drivers/clk/samsung/clk-exynos5420.c b/drivers/clk/samsung/clk-exynos5420.c
index 60b26819bed..9d7d7eed03f 100644
--- a/drivers/clk/samsung/clk-exynos5420.c
+++ b/drivers/clk/samsung/clk-exynos5420.c
@@ -27,18 +27,24 @@
 #define DIV_CPU1		0x504
 #define GATE_BUS_CPU		0x700
 #define GATE_SCLK_CPU		0x800
+#define CLKOUT_CMU_CPU		0xa00
+#define GATE_IP_G2D		0x8800
 #define CPLL_LOCK		0x10020
 #define DPLL_LOCK		0x10030
 #define EPLL_LOCK		0x10040
 #define RPLL_LOCK		0x10050
 #define IPLL_LOCK		0x10060
 #define SPLL_LOCK		0x10070
-#define VPLL_LOCK		0x10070
+#define VPLL_LOCK		0x10080
 #define MPLL_LOCK		0x10090
 #define CPLL_CON0		0x10120
 #define DPLL_CON0		0x10128
 #define EPLL_CON0		0x10130
+#define EPLL_CON1		0x10134
+#define EPLL_CON2		0x10138
 #define RPLL_CON0		0x10140
+#define RPLL_CON1		0x10144
+#define RPLL_CON2		0x10148
 #define IPLL_CON0		0x10150
 #define SPLL_CON0		0x10160
 #define VPLL_CON0		0x10170
@@ -51,21 +57,31 @@
 #define SRC_TOP5		0x10214
 #define SRC_TOP6		0x10218
 #define SRC_TOP7		0x1021c
+#define SRC_TOP8		0x10220 /* 5800 specific */
+#define SRC_TOP9		0x10224 /* 5800 specific */
 #define SRC_DISP10		0x1022c
 #define SRC_MAU			0x10240
 #define SRC_FSYS		0x10244
 #define SRC_PERIC0		0x10250
 #define SRC_PERIC1		0x10254
+#define SRC_ISP			0x10270
+#define SRC_CAM			0x10274 /* 5800 specific */
 #define SRC_TOP10		0x10280
 #define SRC_TOP11		0x10284
 #define SRC_TOP12		0x10288
-#define	SRC_MASK_DISP10		0x1032c
+#define SRC_TOP13		0x1028c /* 5800 specific */
+#define SRC_MASK_TOP2		0x10308
+#define SRC_MASK_TOP7		0x1031c
+#define SRC_MASK_DISP10		0x1032c
+#define SRC_MASK_MAU		0x10334
 #define SRC_MASK_FSYS		0x10340
 #define SRC_MASK_PERIC0		0x10350
 #define SRC_MASK_PERIC1		0x10354
 #define DIV_TOP0		0x10500
 #define DIV_TOP1		0x10504
 #define DIV_TOP2		0x10508
+#define DIV_TOP8		0x10520 /* 5800 specific */
+#define DIV_TOP9		0x10524 /* 5800 specific */
 #define DIV_DISP10		0x1052c
 #define DIV_MAU			0x10544
 #define DIV_FSYS0		0x10548
@@ -76,54 +92,82 @@
 #define DIV_PERIC2		0x10560
 #define DIV_PERIC3		0x10564
 #define DIV_PERIC4		0x10568
+#define DIV_CAM			0x10574 /* 5800 specific */
+#define SCLK_DIV_ISP0		0x10580
+#define SCLK_DIV_ISP1		0x10584
+#define DIV2_RATIO0		0x10590
+#define DIV4_RATIO		0x105a0
 #define GATE_BUS_TOP		0x10700
+#define GATE_BUS_GEN		0x1073c
 #define GATE_BUS_FSYS0		0x10740
+#define GATE_BUS_FSYS2		0x10748
 #define GATE_BUS_PERIC		0x10750
 #define GATE_BUS_PERIC1		0x10754
 #define GATE_BUS_PERIS0		0x10760
 #define GATE_BUS_PERIS1		0x10764
+#define GATE_BUS_NOC		0x10770
+#define GATE_TOP_SCLK_ISP	0x10870
 #define GATE_IP_GSCL0		0x10910
 #define GATE_IP_GSCL1		0x10920
+#define GATE_IP_CAM		0x10924 /* 5800 specific */
 #define GATE_IP_MFC		0x1092c
 #define GATE_IP_DISP1		0x10928
 #define GATE_IP_G3D		0x10930
 #define GATE_IP_GEN		0x10934
+#define GATE_IP_FSYS		0x10944
+#define GATE_IP_PERIC		0x10950
+#define GATE_IP_PERIS		0x10960
 #define GATE_IP_MSCL		0x10970
 #define GATE_TOP_SCLK_GSCL	0x10820
 #define GATE_TOP_SCLK_DISP1	0x10828
 #define GATE_TOP_SCLK_MAU	0x1083c
 #define GATE_TOP_SCLK_FSYS	0x10840
 #define GATE_TOP_SCLK_PERIC	0x10850
+#define TOP_SPARE2		0x10b08
 #define BPLL_LOCK		0x20010
 #define BPLL_CON0		0x20110
-#define SRC_CDREX		0x20200
 #define KPLL_LOCK		0x28000
 #define KPLL_CON0		0x28100
 #define SRC_KFC			0x28200
 #define DIV_KFC0		0x28500
 
+/* Exynos5x SoC type */
+enum exynos5x_soc {
+	EXYNOS5420,
+	EXYNOS5800,
+};
+
 /* list of PLLs */
-enum exynos5420_plls {
+enum exynos5x_plls {
 	apll, cpll, dpll, epll, rpll, ipll, spll, vpll, mpll,
 	bpll, kpll,
 	nr_plls			/* number of PLLs */
 };
 
 static void __iomem *reg_base;
+static enum exynos5x_soc exynos5x_soc;
 
 #ifdef CONFIG_PM_SLEEP
-static struct samsung_clk_reg_dump *exynos5420_save;
+static struct samsung_clk_reg_dump *exynos5x_save;
+static struct samsung_clk_reg_dump *exynos5800_save;
 
 /*
  * list of controller registers to be saved and restored during a
  * suspend/resume cycle.
  */
-static unsigned long exynos5420_clk_regs[] __initdata = {
+static unsigned long exynos5x_clk_regs[] __initdata = {
 	SRC_CPU,
 	DIV_CPU0,
 	DIV_CPU1,
 	GATE_BUS_CPU,
 	GATE_SCLK_CPU,
+	CLKOUT_CMU_CPU,
+	EPLL_CON0,
+	EPLL_CON1,
+	EPLL_CON2,
+	RPLL_CON0,
+	RPLL_CON1,
+	RPLL_CON2,
 	SRC_TOP0,
 	SRC_TOP1,
 	SRC_TOP2,
@@ -140,10 +184,13 @@ static unsigned long exynos5420_clk_regs[] __initdata = {
 	SRC_TOP10,
 	SRC_TOP11,
 	SRC_TOP12,
+	SRC_MASK_TOP2,
+	SRC_MASK_TOP7,
 	SRC_MASK_DISP10,
 	SRC_MASK_FSYS,
 	SRC_MASK_PERIC0,
 	SRC_MASK_PERIC1,
+	SRC_ISP,
 	DIV_TOP0,
 	DIV_TOP1,
 	DIV_TOP2,
@@ -157,41 +204,71 @@ static unsigned long exynos5420_clk_regs[] __initdata = {
 	DIV_PERIC2,
 	DIV_PERIC3,
 	DIV_PERIC4,
+	SCLK_DIV_ISP0,
+	SCLK_DIV_ISP1,
+	DIV2_RATIO0,
+	DIV4_RATIO,
 	GATE_BUS_TOP,
+	GATE_BUS_GEN,
 	GATE_BUS_FSYS0,
+	GATE_BUS_FSYS2,
 	GATE_BUS_PERIC,
 	GATE_BUS_PERIC1,
 	GATE_BUS_PERIS0,
 	GATE_BUS_PERIS1,
+	GATE_BUS_NOC,
+	GATE_TOP_SCLK_ISP,
 	GATE_IP_GSCL0,
 	GATE_IP_GSCL1,
 	GATE_IP_MFC,
 	GATE_IP_DISP1,
 	GATE_IP_G3D,
 	GATE_IP_GEN,
+	GATE_IP_FSYS,
+	GATE_IP_PERIC,
+	GATE_IP_PERIS,
 	GATE_IP_MSCL,
 	GATE_TOP_SCLK_GSCL,
 	GATE_TOP_SCLK_DISP1,
 	GATE_TOP_SCLK_MAU,
 	GATE_TOP_SCLK_FSYS,
 	GATE_TOP_SCLK_PERIC,
-	SRC_CDREX,
+	TOP_SPARE2,
 	SRC_KFC,
 	DIV_KFC0,
 };
 
+static unsigned long exynos5800_clk_regs[] __initdata = {
+	SRC_TOP8,
+	SRC_TOP9,
+	SRC_CAM,
+	SRC_TOP1,
+	DIV_TOP8,
+	DIV_TOP9,
+	DIV_CAM,
+	GATE_IP_CAM,
+};
+
 static int exynos5420_clk_suspend(void)
 {
-	samsung_clk_save(reg_base, exynos5420_save,
-				ARRAY_SIZE(exynos5420_clk_regs));
+	samsung_clk_save(reg_base, exynos5x_save,
+				ARRAY_SIZE(exynos5x_clk_regs));
+
+	if (exynos5x_soc == EXYNOS5800)
+		samsung_clk_save(reg_base, exynos5800_save,
+				ARRAY_SIZE(exynos5800_clk_regs));
 
 	return 0;
 }
 
 static void exynos5420_clk_resume(void)
 {
-	samsung_clk_restore(reg_base, exynos5420_save,
-				ARRAY_SIZE(exynos5420_clk_regs));
+	samsung_clk_restore(reg_base, exynos5x_save,
+				ARRAY_SIZE(exynos5x_clk_regs));
+
+	if (exynos5x_soc == EXYNOS5800)
+		samsung_clk_restore(reg_base, exynos5800_save,
+				ARRAY_SIZE(exynos5800_clk_regs));
 }
 
 static struct syscore_ops exynos5420_clk_syscore_ops = {
@@ -201,108 +278,183 @@ static struct syscore_ops exynos5420_clk_syscore_ops = {
 
 static void exynos5420_clk_sleep_init(void)
 {
-	exynos5420_save = samsung_clk_alloc_reg_dump(exynos5420_clk_regs,
-					ARRAY_SIZE(exynos5420_clk_regs));
-	if (!exynos5420_save) {
+	exynos5x_save = samsung_clk_alloc_reg_dump(exynos5x_clk_regs,
+					ARRAY_SIZE(exynos5x_clk_regs));
+	if (!exynos5x_save) {
 		pr_warn("%s: failed to allocate sleep save data, no sleep support!\n",
 			__func__);
 		return;
 	}
 
+	if (exynos5x_soc == EXYNOS5800) {
+		exynos5800_save =
+			samsung_clk_alloc_reg_dump(exynos5800_clk_regs,
+					ARRAY_SIZE(exynos5800_clk_regs));
+		if (!exynos5800_save)
+			goto err_soc;
+	}
+
 	register_syscore_ops(&exynos5420_clk_syscore_ops);
+	return;
+err_soc:
+	kfree(exynos5x_save);
+	pr_warn("%s: failed to allocate sleep save data, no sleep support!\n",
+		__func__);
+	return;
 }
 #else
 static void exynos5420_clk_sleep_init(void) {}
 #endif
 
 /* list of all parent clocks */
-PNAME(mspll_cpu_p)	= { "sclk_cpll", "sclk_dpll",
-				"sclk_mpll", "sclk_spll" };
-PNAME(cpu_p)		= { "mout_apll" , "mout_mspll_cpu" };
-PNAME(kfc_p)		= { "mout_kpll" , "mout_mspll_kfc" };
-PNAME(apll_p)		= { "fin_pll", "fout_apll", };
-PNAME(bpll_p)		= { "fin_pll", "fout_bpll", };
-PNAME(cpll_p)		= { "fin_pll", "fout_cpll", };
-PNAME(dpll_p)		= { "fin_pll", "fout_dpll", };
-PNAME(epll_p)		= { "fin_pll", "fout_epll", };
-PNAME(ipll_p)		= { "fin_pll", "fout_ipll", };
-PNAME(kpll_p)		= { "fin_pll", "fout_kpll", };
-PNAME(mpll_p)		= { "fin_pll", "fout_mpll", };
-PNAME(rpll_p)		= { "fin_pll", "fout_rpll", };
-PNAME(spll_p)		= { "fin_pll", "fout_spll", };
-PNAME(vpll_p)		= { "fin_pll", "fout_vpll", };
-
-PNAME(group1_p)		= { "sclk_cpll", "sclk_dpll", "sclk_mpll" };
-PNAME(group2_p)		= { "fin_pll", "sclk_cpll", "sclk_dpll", "sclk_mpll",
-			  "sclk_spll", "sclk_ipll", "sclk_epll", "sclk_rpll" };
-PNAME(group3_p)		= { "sclk_rpll", "sclk_spll" };
-PNAME(group4_p)		= { "sclk_ipll", "sclk_dpll", "sclk_mpll" };
-PNAME(group5_p)		= { "sclk_vpll", "sclk_dpll" };
-
-PNAME(sw_aclk66_p)	= { "dout_aclk66", "sclk_spll" };
-PNAME(aclk66_peric_p)	= { "fin_pll", "mout_sw_aclk66" };
-
-PNAME(sw_aclk200_fsys_p) = { "dout_aclk200_fsys", "sclk_spll"};
-PNAME(user_aclk200_fsys_p)	= { "fin_pll", "mout_sw_aclk200_fsys" };
-
-PNAME(sw_aclk200_fsys2_p) = { "dout_aclk200_fsys2", "sclk_spll"};
-PNAME(user_aclk200_fsys2_p)	= { "fin_pll", "mout_sw_aclk200_fsys2" };
-
-PNAME(sw_aclk200_p) = { "dout_aclk200", "sclk_spll"};
-PNAME(aclk200_disp1_p)	= { "fin_pll", "mout_sw_aclk200" };
-
-PNAME(sw_aclk400_mscl_p) = { "dout_aclk400_mscl", "sclk_spll"};
-PNAME(user_aclk400_mscl_p)	= { "fin_pll", "mout_sw_aclk400_mscl" };
-
-PNAME(sw_aclk333_p) = { "dout_aclk333", "sclk_spll"};
-PNAME(user_aclk333_p)	= { "fin_pll", "mout_sw_aclk333" };
-
-PNAME(sw_aclk166_p) = { "dout_aclk166", "sclk_spll"};
-PNAME(user_aclk166_p)	= { "fin_pll", "mout_sw_aclk166" };
-
-PNAME(sw_aclk266_p) = { "dout_aclk266", "sclk_spll"};
-PNAME(user_aclk266_p)	= { "fin_pll", "mout_sw_aclk266" };
-
-PNAME(sw_aclk333_432_gscl_p) = { "dout_aclk333_432_gscl", "sclk_spll"};
-PNAME(user_aclk333_432_gscl_p)	= { "fin_pll", "mout_sw_aclk333_432_gscl" };
-
-PNAME(sw_aclk300_gscl_p) = { "dout_aclk300_gscl", "sclk_spll"};
-PNAME(user_aclk300_gscl_p)	= { "fin_pll", "mout_sw_aclk300_gscl" };
-
-PNAME(sw_aclk300_disp1_p) = { "dout_aclk300_disp1", "sclk_spll"};
-PNAME(user_aclk300_disp1_p)	= { "fin_pll", "mout_sw_aclk300_disp1" };
-
-PNAME(sw_aclk300_jpeg_p) = { "dout_aclk300_jpeg", "sclk_spll"};
-PNAME(user_aclk300_jpeg_p)	= { "fin_pll", "mout_sw_aclk300_jpeg" };
-
-PNAME(sw_aclk_g3d_p) = { "dout_aclk_g3d", "sclk_spll"};
-PNAME(user_aclk_g3d_p)	= { "fin_pll", "mout_sw_aclk_g3d" };
-
-PNAME(sw_aclk266_g2d_p) = { "dout_aclk266_g2d", "sclk_spll"};
-PNAME(user_aclk266_g2d_p)	= { "fin_pll", "mout_sw_aclk266_g2d" };
-
-PNAME(sw_aclk333_g2d_p) = { "dout_aclk333_g2d", "sclk_spll"};
-PNAME(user_aclk333_g2d_p)	= { "fin_pll", "mout_sw_aclk333_g2d" };
-
-PNAME(audio0_p)	= { "fin_pll", "cdclk0", "sclk_dpll", "sclk_mpll",
-		  "sclk_spll", "sclk_ipll", "sclk_epll", "sclk_rpll" };
-PNAME(audio1_p)	= { "fin_pll", "cdclk1", "sclk_dpll", "sclk_mpll",
-		  "sclk_spll", "sclk_ipll", "sclk_epll", "sclk_rpll" };
-PNAME(audio2_p)	= { "fin_pll", "cdclk2", "sclk_dpll", "sclk_mpll",
-		  "sclk_spll", "sclk_ipll", "sclk_epll", "sclk_rpll" };
-PNAME(spdif_p)	= { "fin_pll", "dout_audio0", "dout_audio1", "dout_audio2",
-		  "spdif_extclk", "sclk_ipll", "sclk_epll", "sclk_rpll" };
-PNAME(hdmi_p)	= { "dout_hdmi_pixel", "sclk_hdmiphy" };
-PNAME(maudio0_p)	= { "fin_pll", "maudio_clk", "sclk_dpll", "sclk_mpll",
-			  "sclk_spll", "sclk_ipll", "sclk_epll", "sclk_rpll" };
+PNAME(mout_mspll_cpu_p) = {"mout_sclk_cpll", "mout_sclk_dpll",
+				"mout_sclk_mpll", "mout_sclk_spll"};
+PNAME(mout_cpu_p) = {"mout_apll" , "mout_mspll_cpu"};
+PNAME(mout_kfc_p) = {"mout_kpll" , "mout_mspll_kfc"};
+PNAME(mout_apll_p) = {"fin_pll", "fout_apll"};
+PNAME(mout_bpll_p) = {"fin_pll", "fout_bpll"};
+PNAME(mout_cpll_p) = {"fin_pll", "fout_cpll"};
+PNAME(mout_dpll_p) = {"fin_pll", "fout_dpll"};
+PNAME(mout_epll_p) = {"fin_pll", "fout_epll"};
+PNAME(mout_ipll_p) = {"fin_pll", "fout_ipll"};
+PNAME(mout_kpll_p) = {"fin_pll", "fout_kpll"};
+PNAME(mout_mpll_p) = {"fin_pll", "fout_mpll"};
+PNAME(mout_rpll_p) = {"fin_pll", "fout_rpll"};
+PNAME(mout_spll_p) = {"fin_pll", "fout_spll"};
+PNAME(mout_vpll_p) = {"fin_pll", "fout_vpll"};
+
+PNAME(mout_group1_p) = {"mout_sclk_cpll", "mout_sclk_dpll",
+					"mout_sclk_mpll"};
+PNAME(mout_group2_p) = {"fin_pll", "mout_sclk_cpll",
+			"mout_sclk_dpll", "mout_sclk_mpll", "mout_sclk_spll",
+			"mout_sclk_ipll", "mout_sclk_epll", "mout_sclk_rpll"};
+PNAME(mout_group3_p) = {"mout_sclk_rpll", "mout_sclk_spll"};
+PNAME(mout_group4_p) = {"mout_sclk_ipll", "mout_sclk_dpll", "mout_sclk_mpll"};
+PNAME(mout_group5_p) = {"mout_sclk_vpll", "mout_sclk_dpll"};
+
+PNAME(mout_fimd1_final_p) = {"mout_fimd1", "mout_fimd1_opt"};
+PNAME(mout_sw_aclk66_p)	= {"dout_aclk66", "mout_sclk_spll"};
+PNAME(mout_user_aclk66_peric_p)	= { "fin_pll", "mout_sw_aclk66"};
+PNAME(mout_user_pclk66_gpio_p) = {"mout_sw_aclk66", "ff_sw_aclk66"};
+
+PNAME(mout_sw_aclk200_fsys_p) = {"dout_aclk200_fsys", "mout_sclk_spll"};
+PNAME(mout_sw_pclk200_fsys_p) = {"dout_pclk200_fsys", "mout_sclk_spll"};
+PNAME(mout_user_pclk200_fsys_p)	= {"fin_pll", "mout_sw_pclk200_fsys"};
+PNAME(mout_user_aclk200_fsys_p)	= {"fin_pll", "mout_sw_aclk200_fsys"};
+
+PNAME(mout_sw_aclk200_fsys2_p) = {"dout_aclk200_fsys2", "mout_sclk_spll"};
+PNAME(mout_user_aclk200_fsys2_p) = {"fin_pll", "mout_sw_aclk200_fsys2"};
+PNAME(mout_sw_aclk100_noc_p) = {"dout_aclk100_noc", "mout_sclk_spll"};
+PNAME(mout_user_aclk100_noc_p) = {"fin_pll", "mout_sw_aclk100_noc"};
+
+PNAME(mout_sw_aclk400_wcore_p) = {"dout_aclk400_wcore", "mout_sclk_spll"};
+PNAME(mout_aclk400_wcore_bpll_p) = {"mout_aclk400_wcore", "sclk_bpll"};
+PNAME(mout_user_aclk400_wcore_p) = {"fin_pll", "mout_sw_aclk400_wcore"};
+
+PNAME(mout_sw_aclk400_isp_p) = {"dout_aclk400_isp", "mout_sclk_spll"};
+PNAME(mout_user_aclk400_isp_p) = {"fin_pll", "mout_sw_aclk400_isp"};
+
+PNAME(mout_sw_aclk333_432_isp0_p) = {"dout_aclk333_432_isp0",
+					"mout_sclk_spll"};
+PNAME(mout_user_aclk333_432_isp0_p) = {"fin_pll", "mout_sw_aclk333_432_isp0"};
+
+PNAME(mout_sw_aclk333_432_isp_p) = {"dout_aclk333_432_isp", "mout_sclk_spll"};
+PNAME(mout_user_aclk333_432_isp_p) = {"fin_pll", "mout_sw_aclk333_432_isp"};
+
+PNAME(mout_sw_aclk200_p) = {"dout_aclk200", "mout_sclk_spll"};
+PNAME(mout_user_aclk200_disp1_p) = {"fin_pll", "mout_sw_aclk200"};
+
+PNAME(mout_sw_aclk400_mscl_p) = {"dout_aclk400_mscl", "mout_sclk_spll"};
+PNAME(mout_user_aclk400_mscl_p)	= {"fin_pll", "mout_sw_aclk400_mscl"};
+
+PNAME(mout_sw_aclk333_p) = {"dout_aclk333", "mout_sclk_spll"};
+PNAME(mout_user_aclk333_p) = {"fin_pll", "mout_sw_aclk333"};
+
+PNAME(mout_sw_aclk166_p) = {"dout_aclk166", "mout_sclk_spll"};
+PNAME(mout_user_aclk166_p) = {"fin_pll", "mout_sw_aclk166"};
+
+PNAME(mout_sw_aclk266_p) = {"dout_aclk266", "mout_sclk_spll"};
+PNAME(mout_user_aclk266_p) = {"fin_pll", "mout_sw_aclk266"};
+PNAME(mout_user_aclk266_isp_p) = {"fin_pll", "mout_sw_aclk266"};
+
+PNAME(mout_sw_aclk333_432_gscl_p) = {"dout_aclk333_432_gscl", "mout_sclk_spll"};
+PNAME(mout_user_aclk333_432_gscl_p) = {"fin_pll", "mout_sw_aclk333_432_gscl"};
+
+PNAME(mout_sw_aclk300_gscl_p) = {"dout_aclk300_gscl", "mout_sclk_spll"};
+PNAME(mout_user_aclk300_gscl_p)	= {"fin_pll", "mout_sw_aclk300_gscl"};
+
+PNAME(mout_sw_aclk300_disp1_p) = {"dout_aclk300_disp1", "mout_sclk_spll"};
+PNAME(mout_sw_aclk400_disp1_p) = {"dout_aclk400_disp1", "mout_sclk_spll"};
+PNAME(mout_user_aclk300_disp1_p) = {"fin_pll", "mout_sw_aclk300_disp1"};
+PNAME(mout_user_aclk400_disp1_p) = {"fin_pll", "mout_sw_aclk400_disp1"};
+
+PNAME(mout_sw_aclk300_jpeg_p) = {"dout_aclk300_jpeg", "mout_sclk_spll"};
+PNAME(mout_user_aclk300_jpeg_p) = {"fin_pll", "mout_sw_aclk300_jpeg"};
+
+PNAME(mout_sw_aclk_g3d_p) = {"dout_aclk_g3d", "mout_sclk_spll"};
+PNAME(mout_user_aclk_g3d_p) = {"fin_pll", "mout_sw_aclk_g3d"};
+
+PNAME(mout_sw_aclk266_g2d_p) = {"dout_aclk266_g2d", "mout_sclk_spll"};
+PNAME(mout_user_aclk266_g2d_p) = {"fin_pll", "mout_sw_aclk266_g2d"};
+
+PNAME(mout_sw_aclk333_g2d_p) = {"dout_aclk333_g2d", "mout_sclk_spll"};
+PNAME(mout_user_aclk333_g2d_p) = {"fin_pll", "mout_sw_aclk333_g2d"};
+
+PNAME(mout_audio0_p) = {"fin_pll", "cdclk0", "mout_sclk_dpll",
+			"mout_sclk_mpll", "mout_sclk_spll", "mout_sclk_ipll",
+			"mout_sclk_epll", "mout_sclk_rpll"};
+PNAME(mout_audio1_p) = {"fin_pll", "cdclk1", "mout_sclk_dpll",
+			"mout_sclk_mpll", "mout_sclk_spll", "mout_sclk_ipll",
+			"mout_sclk_epll", "mout_sclk_rpll"};
+PNAME(mout_audio2_p) = {"fin_pll", "cdclk2", "mout_sclk_dpll",
+			"mout_sclk_mpll", "mout_sclk_spll", "mout_sclk_ipll",
+			"mout_sclk_epll", "mout_sclk_rpll"};
+PNAME(mout_spdif_p) = {"fin_pll", "dout_audio0", "dout_audio1",
+			"dout_audio2", "spdif_extclk", "mout_sclk_ipll",
+			"mout_sclk_epll", "mout_sclk_rpll"};
+PNAME(mout_hdmi_p) = {"dout_hdmi_pixel", "sclk_hdmiphy"};
+PNAME(mout_maudio0_p) = {"fin_pll", "maudio_clk", "mout_sclk_dpll",
+			 "mout_sclk_mpll", "mout_sclk_spll", "mout_sclk_ipll",
+			 "mout_sclk_epll", "mout_sclk_rpll"};
+PNAME(mout_mau_epll_clk_p) = {"mout_sclk_epll", "mout_sclk_dpll",
+				"mout_sclk_mpll", "mout_sclk_spll"};
+/* List of parents specific to exynos5800 */
+PNAME(mout_epll2_5800_p)	= { "mout_sclk_epll", "ff_dout_epll2" };
+PNAME(mout_group1_5800_p)	= { "mout_sclk_cpll", "mout_sclk_dpll",
+				"mout_sclk_mpll", "ff_dout_spll2" };
+PNAME(mout_group2_5800_p)	= { "mout_sclk_cpll", "mout_sclk_dpll",
+					"mout_sclk_mpll", "ff_dout_spll2",
+					"mout_epll2", "mout_sclk_ipll" };
+PNAME(mout_group3_5800_p)	= { "mout_sclk_cpll", "mout_sclk_dpll",
+					"mout_sclk_mpll", "ff_dout_spll2",
+					"mout_epll2" };
+PNAME(mout_group5_5800_p)	= { "mout_sclk_cpll", "mout_sclk_dpll",
+					"mout_sclk_mpll", "mout_sclk_spll" };
+PNAME(mout_group6_5800_p)	= { "mout_sclk_ipll", "mout_sclk_dpll",
+				"mout_sclk_mpll", "ff_dout_spll2" };
+PNAME(mout_group7_5800_p)	= { "mout_sclk_cpll", "mout_sclk_dpll",
+					"mout_sclk_mpll", "mout_sclk_spll",
+					"mout_epll2", "mout_sclk_ipll" };
+PNAME(mout_mau_epll_clk_5800_p)	= { "mout_sclk_epll", "mout_sclk_dpll",
+					"mout_sclk_mpll",
+					"ff_dout_spll2" };
+PNAME(mout_group8_5800_p)	= { "dout_aclk432_scaler", "dout_sclk_sw" };
+PNAME(mout_group9_5800_p)	= { "dout_osc_div", "mout_sw_aclk432_scaler" };
+PNAME(mout_group10_5800_p)	= { "dout_aclk432_cam", "dout_sclk_sw" };
+PNAME(mout_group11_5800_p)	= { "dout_osc_div", "mout_sw_aclk432_cam" };
+PNAME(mout_group12_5800_p)	= { "dout_aclkfl1_550_cam", "dout_sclk_sw" };
+PNAME(mout_group13_5800_p)	= { "dout_osc_div", "mout_sw_aclkfl1_550_cam" };
+PNAME(mout_group14_5800_p)	= { "dout_aclk550_cam", "dout_sclk_sw" };
+PNAME(mout_group15_5800_p)	= { "dout_osc_div", "mout_sw_aclk550_cam" };
 
 /* fixed rate clocks generated outside the soc */
-static struct samsung_fixed_rate_clock exynos5420_fixed_rate_ext_clks[] __initdata = {
+static struct samsung_fixed_rate_clock
+		exynos5x_fixed_rate_ext_clks[] __initdata = {
 	FRATE(CLK_FIN_PLL, "fin_pll", NULL, CLK_IS_ROOT, 0),
 };
 
 /* fixed rate clocks generated inside the soc */
-static struct samsung_fixed_rate_clock exynos5420_fixed_rate_clks[] __initdata = {
+static struct samsung_fixed_rate_clock exynos5x_fixed_rate_clks[] __initdata = {
 	FRATE(CLK_SCLK_HDMIPHY, "sclk_hdmiphy", NULL, CLK_IS_ROOT, 24000000),
 	FRATE(0, "sclk_pwi", NULL, CLK_IS_ROOT, 24000000),
 	FRATE(0, "sclk_usbh20", NULL, CLK_IS_ROOT, 48000000),
@@ -310,146 +462,309 @@ static struct samsung_fixed_rate_clock exynos5420_fixed_rate_clks[] __initdata =
 	FRATE(0, "sclk_usbh20_scan_clk", NULL, CLK_IS_ROOT, 480000000),
 };
 
-static struct samsung_fixed_factor_clock exynos5420_fixed_factor_clks[] __initdata = {
-	FFACTOR(0, "sclk_hsic_12m", "fin_pll", 1, 2, 0),
+static struct samsung_fixed_factor_clock
+		exynos5x_fixed_factor_clks[] __initdata = {
+	FFACTOR(0, "ff_hsic_12m", "fin_pll", 1, 2, 0),
+	FFACTOR(0, "ff_sw_aclk66", "mout_sw_aclk66", 1, 2, 0),
 };
 
-static struct samsung_mux_clock exynos5420_mux_clks[] __initdata = {
-	MUX(0, "mout_mspll_kfc", mspll_cpu_p, SRC_TOP7, 8, 2),
-	MUX(0, "mout_mspll_cpu", mspll_cpu_p, SRC_TOP7, 12, 2),
-	MUX(0, "mout_apll", apll_p, SRC_CPU, 0, 1),
-	MUX(0, "mout_cpu", cpu_p, SRC_CPU, 16, 1),
-	MUX(0, "mout_kpll", kpll_p, SRC_KFC, 0, 1),
-	MUX(0, "mout_cpu_kfc", kfc_p, SRC_KFC, 16, 1),
-
-	MUX(0, "sclk_bpll", bpll_p, SRC_CDREX, 0, 1),
-
-	MUX_A(0, "mout_aclk400_mscl", group1_p,
-			SRC_TOP0, 4, 2, "aclk400_mscl"),
-	MUX(0, "mout_aclk200", group1_p, SRC_TOP0, 8, 2),
-	MUX(0, "mout_aclk200_fsys2", group1_p, SRC_TOP0, 12, 2),
-	MUX(0, "mout_aclk200_fsys", group1_p, SRC_TOP0, 28, 2),
-
-	MUX(0, "mout_aclk333_432_gscl", group4_p, SRC_TOP1, 0, 2),
-	MUX(0, "mout_aclk66", group1_p, SRC_TOP1, 8, 2),
-	MUX(0, "mout_aclk266", group1_p, SRC_TOP1, 20, 2),
-	MUX(0, "mout_aclk166", group1_p, SRC_TOP1, 24, 2),
-	MUX(0, "mout_aclk333", group1_p, SRC_TOP1, 28, 2),
-
-	MUX(0, "mout_aclk333_g2d", group1_p, SRC_TOP2, 8, 2),
-	MUX(0, "mout_aclk266_g2d", group1_p, SRC_TOP2, 12, 2),
-	MUX(0, "mout_aclk_g3d", group5_p, SRC_TOP2, 16, 1),
-	MUX(0, "mout_aclk300_jpeg", group1_p, SRC_TOP2, 20, 2),
-	MUX(0, "mout_aclk300_disp1", group1_p, SRC_TOP2, 24, 2),
-	MUX(0, "mout_aclk300_gscl", group1_p, SRC_TOP2, 28, 2),
-
-	MUX(0, "mout_user_aclk400_mscl", user_aclk400_mscl_p,
+static struct samsung_fixed_factor_clock
+		exynos5800_fixed_factor_clks[] __initdata = {
+	FFACTOR(0, "ff_dout_epll2", "mout_sclk_epll", 1, 2, 0),
+	FFACTOR(0, "ff_dout_spll2", "mout_sclk_spll", 1, 2, 0),
+};
+
+struct samsung_mux_clock exynos5800_mux_clks[] __initdata = {
+	MUX(0, "mout_aclk400_isp", mout_group3_5800_p, SRC_TOP0, 0, 3),
+	MUX(0, "mout_aclk400_mscl", mout_group3_5800_p, SRC_TOP0, 4, 3),
+	MUX(0, "mout_aclk400_wcore", mout_group2_5800_p, SRC_TOP0, 16, 3),
+	MUX(0, "mout_aclk100_noc", mout_group1_5800_p, SRC_TOP0, 20, 2),
+
+	MUX(0, "mout_aclk333_432_gscl", mout_group6_5800_p, SRC_TOP1, 0, 2),
+	MUX(0, "mout_aclk333_432_isp", mout_group6_5800_p, SRC_TOP1, 4, 2),
+	MUX(0, "mout_aclk333_432_isp0", mout_group6_5800_p, SRC_TOP1, 12, 2),
+	MUX(0, "mout_aclk266", mout_group5_5800_p, SRC_TOP1, 20, 2),
+	MUX(0, "mout_aclk333", mout_group1_5800_p, SRC_TOP1, 28, 2),
+
+	MUX(0, "mout_aclk400_disp1", mout_group7_5800_p, SRC_TOP2, 4, 3),
+	MUX(0, "mout_aclk333_g2d", mout_group5_5800_p, SRC_TOP2, 8, 2),
+	MUX(0, "mout_aclk266_g2d", mout_group5_5800_p, SRC_TOP2, 12, 2),
+	MUX(0, "mout_aclk300_jpeg", mout_group5_5800_p, SRC_TOP2, 20, 2),
+	MUX(0, "mout_aclk300_disp1", mout_group5_5800_p, SRC_TOP2, 24, 2),
+	MUX(0, "mout_aclk300_gscl", mout_group5_5800_p, SRC_TOP2, 28, 2),
+
+	MUX(0, "mout_mau_epll_clk", mout_mau_epll_clk_5800_p, SRC_TOP7,
+			20, 2),
+	MUX(0, "sclk_bpll", mout_bpll_p, SRC_TOP7, 24, 1),
+	MUX(0, "mout_epll2", mout_epll2_5800_p, SRC_TOP7, 28, 1),
+
+	MUX(0, "mout_aclk550_cam", mout_group3_5800_p, SRC_TOP8, 16, 3),
+	MUX(0, "mout_aclkfl1_550_cam", mout_group3_5800_p, SRC_TOP8, 20, 3),
+	MUX(0, "mout_aclk432_cam", mout_group6_5800_p, SRC_TOP8, 24, 2),
+	MUX(0, "mout_aclk432_scaler", mout_group6_5800_p, SRC_TOP8, 28, 2),
+
+	MUX(0, "mout_user_aclk550_cam", mout_group15_5800_p,
+							SRC_TOP9, 16, 1),
+	MUX(0, "mout_user_aclkfl1_550_cam", mout_group13_5800_p,
+							SRC_TOP9, 20, 1),
+	MUX(0, "mout_user_aclk432_cam", mout_group11_5800_p,
+							SRC_TOP9, 24, 1),
+	MUX(0, "mout_user_aclk432_scaler", mout_group9_5800_p,
+							SRC_TOP9, 28, 1),
+
+	MUX(0, "mout_sw_aclk550_cam", mout_group14_5800_p, SRC_TOP13, 16, 1),
+	MUX(0, "mout_sw_aclkfl1_550_cam", mout_group12_5800_p,
+							SRC_TOP13, 20, 1),
+	MUX(0, "mout_sw_aclk432_cam", mout_group10_5800_p,
+							SRC_TOP13, 24, 1),
+	MUX(0, "mout_sw_aclk432_scaler", mout_group8_5800_p,
+							SRC_TOP13, 28, 1),
+
+	MUX(0, "mout_fimd1", mout_group2_p, SRC_DISP10, 4, 3),
+};
+
+struct samsung_div_clock exynos5800_div_clks[] __initdata = {
+	DIV(0, "dout_aclk400_wcore", "mout_aclk400_wcore", DIV_TOP0, 16, 3),
+
+	DIV(0, "dout_aclk550_cam", "mout_aclk550_cam",
+				DIV_TOP8, 16, 3),
+	DIV(0, "dout_aclkfl1_550_cam", "mout_aclkfl1_550_cam",
+				DIV_TOP8, 20, 3),
+	DIV(0, "dout_aclk432_cam", "mout_aclk432_cam",
+				DIV_TOP8, 24, 3),
+	DIV(0, "dout_aclk432_scaler", "mout_aclk432_scaler",
+				DIV_TOP8, 28, 3),
+
+	DIV(0, "dout_osc_div", "fin_pll", DIV_TOP9, 20, 3),
+	DIV(0, "dout_sclk_sw", "sclk_spll", DIV_TOP9, 24, 6),
+};
+
+struct samsung_gate_clock exynos5800_gate_clks[] __initdata = {
+	GATE(CLK_ACLK550_CAM, "aclk550_cam", "mout_user_aclk550_cam",
+				GATE_BUS_TOP, 24, 0, 0),
+	GATE(CLK_ACLK432_SCALER, "aclk432_scaler", "mout_user_aclk432_scaler",
+				GATE_BUS_TOP, 27, 0, 0),
+};
+
+struct samsung_mux_clock exynos5420_mux_clks[] __initdata = {
+	MUX(0, "sclk_bpll", mout_bpll_p, TOP_SPARE2, 0, 1),
+	MUX(0, "mout_aclk400_wcore_bpll", mout_aclk400_wcore_bpll_p,
+				TOP_SPARE2, 4, 1),
+
+	MUX(0, "mout_aclk400_isp", mout_group1_p, SRC_TOP0, 0, 2),
+	MUX_A(0, "mout_aclk400_mscl", mout_group1_p,
+				SRC_TOP0, 4, 2, "aclk400_mscl"),
+	MUX(0, "mout_aclk400_wcore", mout_group1_p, SRC_TOP0, 16, 2),
+	MUX(0, "mout_aclk100_noc", mout_group1_p, SRC_TOP0, 20, 2),
+
+	MUX(0, "mout_aclk333_432_gscl", mout_group4_p, SRC_TOP1, 0, 2),
+	MUX(0, "mout_aclk333_432_isp", mout_group4_p,
+				SRC_TOP1, 4, 2),
+	MUX(0, "mout_aclk333_432_isp0", mout_group4_p, SRC_TOP1, 12, 2),
+	MUX(0, "mout_aclk266", mout_group1_p, SRC_TOP1, 20, 2),
+	MUX(0, "mout_aclk333", mout_group1_p, SRC_TOP1, 28, 2),
+
+	MUX(0, "mout_aclk400_disp1", mout_group1_p, SRC_TOP2, 4, 2),
+	MUX(0, "mout_aclk333_g2d", mout_group1_p, SRC_TOP2, 8, 2),
+	MUX(0, "mout_aclk266_g2d", mout_group1_p, SRC_TOP2, 12, 2),
+	MUX(0, "mout_aclk300_jpeg", mout_group1_p, SRC_TOP2, 20, 2),
+	MUX(0, "mout_aclk300_disp1", mout_group1_p, SRC_TOP2, 24, 2),
+	MUX(0, "mout_aclk300_gscl", mout_group1_p, SRC_TOP2, 28, 2),
+
+	MUX(0, "mout_mau_epll_clk", mout_mau_epll_clk_p, SRC_TOP7, 20, 2),
+
+	MUX(0, "mout_fimd1", mout_group3_p, SRC_DISP10, 4, 1),
+};
+
+struct samsung_div_clock exynos5420_div_clks[] __initdata = {
+	DIV(0, "dout_aclk400_wcore", "mout_aclk400_wcore_bpll",
+			DIV_TOP0, 16, 3),
+};
+
+static struct samsung_mux_clock exynos5x_mux_clks[] __initdata = {
+	MUX(0, "mout_user_pclk66_gpio", mout_user_pclk66_gpio_p,
+			SRC_TOP7, 4, 1),
+	MUX(0, "mout_mspll_kfc", mout_mspll_cpu_p, SRC_TOP7, 8, 2),
+	MUX(0, "mout_mspll_cpu", mout_mspll_cpu_p, SRC_TOP7, 12, 2),
+
+	MUX(0, "mout_apll", mout_apll_p, SRC_CPU, 0, 1),
+	MUX(0, "mout_cpu", mout_cpu_p, SRC_CPU, 16, 1),
+	MUX(0, "mout_kpll", mout_kpll_p, SRC_KFC, 0, 1),
+	MUX(0, "mout_kfc", mout_kfc_p, SRC_KFC, 16, 1),
+
+	MUX(0, "mout_aclk200", mout_group1_p, SRC_TOP0, 8, 2),
+	MUX(0, "mout_aclk200_fsys2", mout_group1_p, SRC_TOP0, 12, 2),
+	MUX(0, "mout_pclk200_fsys", mout_group1_p, SRC_TOP0, 24, 2),
+	MUX(0, "mout_aclk200_fsys", mout_group1_p, SRC_TOP0, 28, 2),
+
+	MUX(0, "mout_aclk66", mout_group1_p, SRC_TOP1, 8, 2),
+	MUX(0, "mout_aclk166", mout_group1_p, SRC_TOP1, 24, 2),
+
+	MUX(0, "mout_aclk_g3d", mout_group5_p, SRC_TOP2, 16, 1),
+
+	MUX(0, "mout_user_aclk400_isp", mout_user_aclk400_isp_p,
+			SRC_TOP3, 0, 1),
+	MUX(0, "mout_user_aclk400_mscl", mout_user_aclk400_mscl_p,
 			SRC_TOP3, 4, 1),
-	MUX_A(0, "mout_aclk200_disp1", aclk200_disp1_p,
-			SRC_TOP3, 8, 1, "aclk200_disp1"),
-	MUX(0, "mout_user_aclk200_fsys2", user_aclk200_fsys2_p,
+	MUX(0, "mout_user_aclk200_disp1", mout_user_aclk200_disp1_p,
+			SRC_TOP3, 8, 1),
+	MUX(0, "mout_user_aclk200_fsys2", mout_user_aclk200_fsys2_p,
 			SRC_TOP3, 12, 1),
-	MUX(0, "mout_user_aclk200_fsys", user_aclk200_fsys_p,
+	MUX(0, "mout_user_aclk400_wcore", mout_user_aclk400_wcore_p,
+			SRC_TOP3, 16, 1),
+	MUX(0, "mout_user_aclk100_noc", mout_user_aclk100_noc_p,
+			SRC_TOP3, 20, 1),
+	MUX(0, "mout_user_pclk200_fsys", mout_user_pclk200_fsys_p,
+			SRC_TOP3, 24, 1),
+	MUX(0, "mout_user_aclk200_fsys", mout_user_aclk200_fsys_p,
 			SRC_TOP3, 28, 1),
 
-	MUX(0, "mout_user_aclk333_432_gscl", user_aclk333_432_gscl_p,
+	MUX(0, "mout_user_aclk333_432_gscl", mout_user_aclk333_432_gscl_p,
 			SRC_TOP4, 0, 1),
-	MUX(0, "mout_aclk66_peric", aclk66_peric_p, SRC_TOP4, 8, 1),
-	MUX(0, "mout_user_aclk266", user_aclk266_p, SRC_TOP4, 20, 1),
-	MUX(0, "mout_user_aclk166", user_aclk166_p, SRC_TOP4, 24, 1),
-	MUX(0, "mout_user_aclk333", user_aclk333_p, SRC_TOP4, 28, 1),
-
-	MUX(0, "mout_aclk66_psgen", aclk66_peric_p, SRC_TOP5, 4, 1),
-	MUX(0, "mout_user_aclk333_g2d", user_aclk333_g2d_p, SRC_TOP5, 8, 1),
-	MUX(0, "mout_user_aclk266_g2d", user_aclk266_g2d_p, SRC_TOP5, 12, 1),
-	MUX_A(0, "mout_user_aclk_g3d", user_aclk_g3d_p,
-			SRC_TOP5, 16, 1, "aclkg3d"),
-	MUX(0, "mout_user_aclk300_jpeg", user_aclk300_jpeg_p,
+	MUX(0, "mout_user_aclk333_432_isp", mout_user_aclk333_432_isp_p,
+			SRC_TOP4, 4, 1),
+	MUX(0, "mout_user_aclk66_peric", mout_user_aclk66_peric_p,
+			SRC_TOP4, 8, 1),
+	MUX(0, "mout_user_aclk333_432_isp0", mout_user_aclk333_432_isp0_p,
+			SRC_TOP4, 12, 1),
+	MUX(0, "mout_user_aclk266_isp", mout_user_aclk266_isp_p,
+			SRC_TOP4, 16, 1),
+	MUX(0, "mout_user_aclk266", mout_user_aclk266_p, SRC_TOP4, 20, 1),
+	MUX(0, "mout_user_aclk166", mout_user_aclk166_p, SRC_TOP4, 24, 1),
+	MUX(0, "mout_user_aclk333", mout_user_aclk333_p, SRC_TOP4, 28, 1),
+
+	MUX(0, "mout_user_aclk400_disp1", mout_user_aclk400_disp1_p,
+			SRC_TOP5, 0, 1),
+	MUX(0, "mout_user_aclk66_psgen", mout_user_aclk66_peric_p,
+			SRC_TOP5, 4, 1),
+	MUX(0, "mout_user_aclk333_g2d", mout_user_aclk333_g2d_p,
+			SRC_TOP5, 8, 1),
+	MUX(0, "mout_user_aclk266_g2d", mout_user_aclk266_g2d_p,
+			SRC_TOP5, 12, 1),
+	MUX(CLK_MOUT_G3D, "mout_user_aclk_g3d", mout_user_aclk_g3d_p,
+			SRC_TOP5, 16, 1),
+	MUX(0, "mout_user_aclk300_jpeg", mout_user_aclk300_jpeg_p,
 			SRC_TOP5, 20, 1),
-	MUX(0, "mout_user_aclk300_disp1", user_aclk300_disp1_p,
+	MUX(0, "mout_user_aclk300_disp1", mout_user_aclk300_disp1_p,
 			SRC_TOP5, 24, 1),
-	MUX(0, "mout_user_aclk300_gscl", user_aclk300_gscl_p,
+	MUX(0, "mout_user_aclk300_gscl", mout_user_aclk300_gscl_p,
 			SRC_TOP5, 28, 1),
 
-	MUX(0, "sclk_mpll", mpll_p, SRC_TOP6, 0, 1),
-	MUX(0, "sclk_vpll", vpll_p, SRC_TOP6, 4, 1),
-	MUX(0, "sclk_spll", spll_p, SRC_TOP6, 8, 1),
-	MUX(0, "sclk_ipll", ipll_p, SRC_TOP6, 12, 1),
-	MUX(0, "sclk_rpll", rpll_p, SRC_TOP6, 16, 1),
-	MUX(0, "sclk_epll", epll_p, SRC_TOP6, 20, 1),
-	MUX(0, "sclk_dpll", dpll_p, SRC_TOP6, 24, 1),
-	MUX(0, "sclk_cpll", cpll_p, SRC_TOP6, 28, 1),
-
-	MUX(0, "mout_sw_aclk400_mscl", sw_aclk400_mscl_p, SRC_TOP10, 4, 1),
-	MUX(0, "mout_sw_aclk200", sw_aclk200_p, SRC_TOP10, 8, 1),
-	MUX(0, "mout_sw_aclk200_fsys2", sw_aclk200_fsys2_p,
+	MUX(0, "mout_sclk_mpll", mout_mpll_p, SRC_TOP6, 0, 1),
+	MUX(CLK_MOUT_VPLL, "mout_sclk_vpll", mout_vpll_p, SRC_TOP6, 4, 1),
+	MUX(0, "mout_sclk_spll", mout_spll_p, SRC_TOP6, 8, 1),
+	MUX(0, "mout_sclk_ipll", mout_ipll_p, SRC_TOP6, 12, 1),
+	MUX(0, "mout_sclk_rpll", mout_rpll_p, SRC_TOP6, 16, 1),
+	MUX(0, "mout_sclk_epll", mout_epll_p, SRC_TOP6, 20, 1),
+	MUX(0, "mout_sclk_dpll", mout_dpll_p, SRC_TOP6, 24, 1),
+	MUX(0, "mout_sclk_cpll", mout_cpll_p, SRC_TOP6, 28, 1),
+
+	MUX(0, "mout_sw_aclk400_isp", mout_sw_aclk400_isp_p,
+			SRC_TOP10, 0, 1),
+	MUX(0, "mout_sw_aclk400_mscl", mout_sw_aclk400_mscl_p,
+			SRC_TOP10, 4, 1),
+	MUX(0, "mout_sw_aclk200", mout_sw_aclk200_p, SRC_TOP10, 8, 1),
+	MUX(0, "mout_sw_aclk200_fsys2", mout_sw_aclk200_fsys2_p,
 			SRC_TOP10, 12, 1),
-	MUX(0, "mout_sw_aclk200_fsys", sw_aclk200_fsys_p, SRC_TOP10, 28, 1),
-
-	MUX(0, "mout_sw_aclk333_432_gscl", sw_aclk333_432_gscl_p,
+	MUX(0, "mout_sw_aclk400_wcore", mout_sw_aclk400_wcore_p,
+			SRC_TOP10, 16, 1),
+	MUX(0, "mout_sw_aclk100_noc", mout_sw_aclk100_noc_p,
+			SRC_TOP10, 20, 1),
+	MUX(0, "mout_sw_pclk200_fsys", mout_sw_pclk200_fsys_p,
+			SRC_TOP10, 24, 1),
+	MUX(0, "mout_sw_aclk200_fsys", mout_sw_aclk200_fsys_p,
+			SRC_TOP10, 28, 1),
+
+	MUX(0, "mout_sw_aclk333_432_gscl", mout_sw_aclk333_432_gscl_p,
 			SRC_TOP11, 0, 1),
-	MUX(0, "mout_sw_aclk66", sw_aclk66_p, SRC_TOP11, 8, 1),
-	MUX(0, "mout_sw_aclk266", sw_aclk266_p, SRC_TOP11, 20, 1),
-	MUX(0, "mout_sw_aclk166", sw_aclk166_p, SRC_TOP11, 24, 1),
-	MUX(0, "mout_sw_aclk333", sw_aclk333_p, SRC_TOP11, 28, 1),
-
-	MUX(0, "mout_sw_aclk333_g2d", sw_aclk333_g2d_p, SRC_TOP12, 8, 1),
-	MUX(0, "mout_sw_aclk266_g2d", sw_aclk266_g2d_p, SRC_TOP12, 12, 1),
-	MUX(0, "mout_sw_aclk_g3d", sw_aclk_g3d_p, SRC_TOP12, 16, 1),
-	MUX(0, "mout_sw_aclk300_jpeg", sw_aclk300_jpeg_p, SRC_TOP12, 20, 1),
-	MUX(0, "mout_sw_aclk300_disp1", sw_aclk300_disp1_p,
+	MUX(0, "mout_sw_aclk333_432_isp", mout_sw_aclk333_432_isp_p,
+			SRC_TOP11, 4, 1),
+	MUX(0, "mout_sw_aclk66", mout_sw_aclk66_p, SRC_TOP11, 8, 1),
+	MUX(0, "mout_sw_aclk333_432_isp0", mout_sw_aclk333_432_isp0_p,
+			SRC_TOP11, 12, 1),
+	MUX(0, "mout_sw_aclk266", mout_sw_aclk266_p, SRC_TOP11, 20, 1),
+	MUX(0, "mout_sw_aclk166", mout_sw_aclk166_p, SRC_TOP11, 24, 1),
+	MUX(0, "mout_sw_aclk333", mout_sw_aclk333_p, SRC_TOP11, 28, 1),
+
+	MUX(0, "mout_sw_aclk400_disp1", mout_sw_aclk400_disp1_p,
+			SRC_TOP12, 4, 1),
+	MUX(0, "mout_sw_aclk333_g2d", mout_sw_aclk333_g2d_p,
+			SRC_TOP12, 8, 1),
+	MUX(0, "mout_sw_aclk266_g2d", mout_sw_aclk266_g2d_p,
+			SRC_TOP12, 12, 1),
+	MUX(0, "mout_sw_aclk_g3d", mout_sw_aclk_g3d_p, SRC_TOP12, 16, 1),
+	MUX(0, "mout_sw_aclk300_jpeg", mout_sw_aclk300_jpeg_p,
+			SRC_TOP12, 20, 1),
+	MUX(0, "mout_sw_aclk300_disp1", mout_sw_aclk300_disp1_p,
 			SRC_TOP12, 24, 1),
-	MUX(0, "mout_sw_aclk300_gscl", sw_aclk300_gscl_p, SRC_TOP12, 28, 1),
+	MUX(0, "mout_sw_aclk300_gscl", mout_sw_aclk300_gscl_p,
+			SRC_TOP12, 28, 1),
 
 	/* DISP1 Block */
-	MUX(0, "mout_fimd1", group3_p, SRC_DISP10, 4, 1),
-	MUX(0, "mout_mipi1", group2_p, SRC_DISP10, 16, 3),
-	MUX(0, "mout_dp1", group2_p, SRC_DISP10, 20, 3),
-	MUX(0, "mout_pixel", group2_p, SRC_DISP10, 24, 3),
-	MUX(CLK_MOUT_HDMI, "mout_hdmi", hdmi_p, SRC_DISP10, 28, 1),
+	MUX(0, "mout_mipi1", mout_group2_p, SRC_DISP10, 16, 3),
+	MUX(0, "mout_dp1", mout_group2_p, SRC_DISP10, 20, 3),
+	MUX(0, "mout_pixel", mout_group2_p, SRC_DISP10, 24, 3),
+	MUX(CLK_MOUT_HDMI, "mout_hdmi", mout_hdmi_p, SRC_DISP10, 28, 1),
+	MUX(0, "mout_fimd1_opt", mout_group2_p, SRC_DISP10, 8, 3),
+
+	MUX(0, "mout_fimd1_final", mout_fimd1_final_p, TOP_SPARE2, 8, 1),
 
 	/* MAU Block */
-	MUX(0, "mout_maudio0", maudio0_p, SRC_MAU, 28, 3),
+	MUX(CLK_MOUT_MAUDIO0, "mout_maudio0", mout_maudio0_p, SRC_MAU, 28, 3),
 
 	/* FSYS Block */
-	MUX(0, "mout_usbd301", group2_p, SRC_FSYS, 4, 3),
-	MUX(0, "mout_mmc0", group2_p, SRC_FSYS, 8, 3),
-	MUX(0, "mout_mmc1", group2_p, SRC_FSYS, 12, 3),
-	MUX(0, "mout_mmc2", group2_p, SRC_FSYS, 16, 3),
-	MUX(0, "mout_usbd300", group2_p, SRC_FSYS, 20, 3),
-	MUX(0, "mout_unipro", group2_p, SRC_FSYS, 24, 3),
+	MUX(0, "mout_usbd301", mout_group2_p, SRC_FSYS, 4, 3),
+	MUX(0, "mout_mmc0", mout_group2_p, SRC_FSYS, 8, 3),
+	MUX(0, "mout_mmc1", mout_group2_p, SRC_FSYS, 12, 3),
+	MUX(0, "mout_mmc2", mout_group2_p, SRC_FSYS, 16, 3),
+	MUX(0, "mout_usbd300", mout_group2_p, SRC_FSYS, 20, 3),
+	MUX(0, "mout_unipro", mout_group2_p, SRC_FSYS, 24, 3),
+	MUX(0, "mout_mphy_refclk", mout_group2_p, SRC_FSYS, 28, 3),
 
 	/* PERIC Block */
-	MUX(0, "mout_uart0", group2_p, SRC_PERIC0, 4, 3),
-	MUX(0, "mout_uart1", group2_p, SRC_PERIC0, 8, 3),
-	MUX(0, "mout_uart2", group2_p, SRC_PERIC0, 12, 3),
-	MUX(0, "mout_uart3", group2_p, SRC_PERIC0, 16, 3),
-	MUX(0, "mout_pwm", group2_p, SRC_PERIC0, 24, 3),
-	MUX(0, "mout_spdif", spdif_p, SRC_PERIC0, 28, 3),
-	MUX(0, "mout_audio0", audio0_p, SRC_PERIC1, 8, 3),
-	MUX(0, "mout_audio1", audio1_p, SRC_PERIC1, 12, 3),
-	MUX(0, "mout_audio2", audio2_p, SRC_PERIC1, 16, 3),
-	MUX(0, "mout_spi0", group2_p, SRC_PERIC1, 20, 3),
-	MUX(0, "mout_spi1", group2_p, SRC_PERIC1, 24, 3),
-	MUX(0, "mout_spi2", group2_p, SRC_PERIC1, 28, 3),
+	MUX(0, "mout_uart0", mout_group2_p, SRC_PERIC0, 4, 3),
+	MUX(0, "mout_uart1", mout_group2_p, SRC_PERIC0, 8, 3),
+	MUX(0, "mout_uart2", mout_group2_p, SRC_PERIC0, 12, 3),
+	MUX(0, "mout_uart3", mout_group2_p, SRC_PERIC0, 16, 3),
+	MUX(0, "mout_pwm", mout_group2_p, SRC_PERIC0, 24, 3),
+	MUX(0, "mout_spdif", mout_spdif_p, SRC_PERIC0, 28, 3),
+	MUX(0, "mout_audio0", mout_audio0_p, SRC_PERIC1, 8, 3),
+	MUX(0, "mout_audio1", mout_audio1_p, SRC_PERIC1, 12, 3),
+	MUX(0, "mout_audio2", mout_audio2_p, SRC_PERIC1, 16, 3),
+	MUX(0, "mout_spi0", mout_group2_p, SRC_PERIC1, 20, 3),
+	MUX(0, "mout_spi1", mout_group2_p, SRC_PERIC1, 24, 3),
+	MUX(0, "mout_spi2", mout_group2_p, SRC_PERIC1, 28, 3),
+
+	/* ISP Block */
+	MUX(0, "mout_pwm_isp", mout_group2_p, SRC_ISP, 24, 3),
+	MUX(0, "mout_uart_isp", mout_group2_p, SRC_ISP, 20, 3),
+	MUX(0, "mout_spi0_isp", mout_group2_p, SRC_ISP, 12, 3),
+	MUX(0, "mout_spi1_isp", mout_group2_p, SRC_ISP, 16, 3),
+	MUX(0, "mout_isp_sensor", mout_group2_p, SRC_ISP, 28, 3),
 };
 
-static struct samsung_div_clock exynos5420_div_clks[] __initdata = {
+static struct samsung_div_clock exynos5x_div_clks[] __initdata = {
 	DIV(0, "div_arm", "mout_cpu", DIV_CPU0, 0, 3),
 	DIV(0, "sclk_apll", "mout_apll", DIV_CPU0, 24, 3),
 	DIV(0, "armclk2", "div_arm", DIV_CPU0, 28, 3),
-	DIV(0, "div_kfc", "mout_cpu_kfc", DIV_KFC0, 0, 3),
+	DIV(0, "div_kfc", "mout_kfc", DIV_KFC0, 0, 3),
 	DIV(0, "sclk_kpll", "mout_kpll", DIV_KFC0, 24, 3),
 
+	DIV(0, "dout_aclk400_isp", "mout_aclk400_isp", DIV_TOP0, 0, 3),
 	DIV(0, "dout_aclk400_mscl", "mout_aclk400_mscl", DIV_TOP0, 4, 3),
 	DIV(0, "dout_aclk200", "mout_aclk200", DIV_TOP0, 8, 3),
 	DIV(0, "dout_aclk200_fsys2", "mout_aclk200_fsys2", DIV_TOP0, 12, 3),
+	DIV(0, "dout_aclk100_noc", "mout_aclk100_noc", DIV_TOP0, 20, 3),
 	DIV(0, "dout_pclk200_fsys", "mout_pclk200_fsys", DIV_TOP0, 24, 3),
 	DIV(0, "dout_aclk200_fsys", "mout_aclk200_fsys", DIV_TOP0, 28, 3),
 
 	DIV(0, "dout_aclk333_432_gscl", "mout_aclk333_432_gscl",
 			DIV_TOP1, 0, 3),
+	DIV(0, "dout_aclk333_432_isp", "mout_aclk333_432_isp",
+			DIV_TOP1, 4, 3),
 	DIV(0, "dout_aclk66", "mout_aclk66", DIV_TOP1, 8, 6),
+	DIV(0, "dout_aclk333_432_isp0", "mout_aclk333_432_isp0",
+			DIV_TOP1, 16, 3),
 	DIV(0, "dout_aclk266", "mout_aclk266", DIV_TOP1, 20, 3),
 	DIV(0, "dout_aclk166", "mout_aclk166", DIV_TOP1, 24, 3),
 	DIV(0, "dout_aclk333", "mout_aclk333", DIV_TOP1, 28, 3),
@@ -458,15 +773,16 @@ static struct samsung_div_clock exynos5420_div_clks[] __initdata = {
 	DIV(0, "dout_aclk266_g2d", "mout_aclk266_g2d", DIV_TOP2, 12, 3),
 	DIV(0, "dout_aclk_g3d", "mout_aclk_g3d", DIV_TOP2, 16, 3),
 	DIV(0, "dout_aclk300_jpeg", "mout_aclk300_jpeg", DIV_TOP2, 20, 3),
-	DIV_A(0, "dout_aclk300_disp1", "mout_aclk300_disp1",
-			DIV_TOP2, 24, 3, "aclk300_disp1"),
+	DIV(0, "dout_aclk300_disp1", "mout_aclk300_disp1", DIV_TOP2, 24, 3),
 	DIV(0, "dout_aclk300_gscl", "mout_aclk300_gscl", DIV_TOP2, 28, 3),
 
 	/* DISP1 Block */
-	DIV(0, "dout_fimd1", "mout_fimd1", DIV_DISP10, 0, 4),
+	DIV(0, "dout_fimd1", "mout_fimd1_final", DIV_DISP10, 0, 4),
 	DIV(0, "dout_mipi1", "mout_mipi1", DIV_DISP10, 16, 8),
 	DIV(0, "dout_dp1", "mout_dp1", DIV_DISP10, 24, 4),
 	DIV(CLK_DOUT_PIXEL, "dout_hdmi_pixel", "mout_pixel", DIV_DISP10, 28, 4),
+	DIV(0, "dout_disp1_blk", "aclk200_disp1", DIV2_RATIO0, 16, 2),
+	DIV(0, "dout_aclk400_disp1", "mout_aclk400_disp1", DIV_TOP2, 4, 3),
 
 	/* Audio Block */
 	DIV(0, "dout_maudio0", "mout_maudio0", DIV_MAU, 20, 4),
@@ -484,6 +800,7 @@ static struct samsung_div_clock exynos5420_div_clks[] __initdata = {
 	DIV(0, "dout_mmc2", "mout_mmc2", DIV_FSYS1, 20, 10),
 
 	DIV(0, "dout_unipro", "mout_unipro", DIV_FSYS2, 24, 8),
+	DIV(0, "dout_mphy_refclk", "mout_mphy_refclk", DIV_FSYS2, 16, 8),
 
 	/* UART and PWM */
 	DIV(0, "dout_uart0", "mout_uart0", DIV_PERIC0, 8, 4),
@@ -497,6 +814,9 @@ static struct samsung_div_clock exynos5420_div_clks[] __initdata = {
 	DIV(0, "dout_spi1", "mout_spi1", DIV_PERIC1, 24, 4),
 	DIV(0, "dout_spi2", "mout_spi2", DIV_PERIC1, 28, 4),
 
+	/* Mfc Block */
+	DIV(0, "dout_mfc_blk", "mout_user_aclk333", DIV4_RATIO, 0, 2),
+
 	/* PCM */
 	DIV(0, "dout_pcm1", "dout_audio1", DIV_PERIC2, 16, 8),
 	DIV(0, "dout_pcm2", "dout_audio2", DIV_PERIC2, 24, 8),
@@ -509,15 +829,43 @@ static struct samsung_div_clock exynos5420_div_clks[] __initdata = {
 	DIV(0, "dout_audio2", "mout_audio2", DIV_PERIC3, 28, 4),
 
 	/* SPI Pre-Ratio */
-	DIV(0, "dout_pre_spi0", "dout_spi0", DIV_PERIC4, 8, 8),
-	DIV(0, "dout_pre_spi1", "dout_spi1", DIV_PERIC4, 16, 8),
-	DIV(0, "dout_pre_spi2", "dout_spi2", DIV_PERIC4, 24, 8),
+	DIV(0, "dout_spi0_pre", "dout_spi0", DIV_PERIC4, 8, 8),
+	DIV(0, "dout_spi1_pre", "dout_spi1", DIV_PERIC4, 16, 8),
+	DIV(0, "dout_spi2_pre", "dout_spi2", DIV_PERIC4, 24, 8),
+
+	/* GSCL Block */
+	DIV(0, "dout_gscl_blk_300", "mout_user_aclk300_gscl",
+			DIV2_RATIO0, 4, 2),
+	DIV(0, "dout_gscl_blk_333", "aclk333_432_gscl", DIV2_RATIO0, 6, 2),
+
+	/* MSCL Block */
+	DIV(0, "dout_mscl_blk", "aclk400_mscl", DIV2_RATIO0, 28, 2),
+
+	/* PSGEN */
+	DIV(0, "dout_gen_blk", "mout_user_aclk266", DIV2_RATIO0, 8, 1),
+	DIV(0, "dout_jpg_blk", "aclk166", DIV2_RATIO0, 20, 1),
+
+	/* ISP Block */
+	DIV(0, "dout_isp_sensor0", "mout_isp_sensor", SCLK_DIV_ISP0, 8, 8),
+	DIV(0, "dout_isp_sensor1", "mout_isp_sensor", SCLK_DIV_ISP0, 16, 8),
+	DIV(0, "dout_isp_sensor2", "mout_isp_sensor", SCLK_DIV_ISP0, 24, 8),
+	DIV(0, "dout_pwm_isp", "mout_pwm_isp", SCLK_DIV_ISP1, 28, 4),
+	DIV(0, "dout_uart_isp", "mout_uart_isp", SCLK_DIV_ISP1, 24, 4),
+	DIV(0, "dout_spi0_isp", "mout_spi0_isp", SCLK_DIV_ISP1, 16, 4),
+	DIV(0, "dout_spi1_isp", "mout_spi1_isp", SCLK_DIV_ISP1, 20, 4),
+	DIV_F(0, "dout_spi0_isp_pre", "dout_spi0_isp", SCLK_DIV_ISP1, 0, 8,
+			CLK_SET_RATE_PARENT, 0),
+	DIV_F(0, "dout_spi1_isp_pre", "dout_spi1_isp", SCLK_DIV_ISP1, 8, 8,
+			CLK_SET_RATE_PARENT, 0),
 };
 
-static struct samsung_gate_clock exynos5420_gate_clks[] __initdata = {
-	/* TODO: Re-verify the CG bits for all the gate clocks */
-	GATE_A(CLK_MCT, "pclk_st", "aclk66_psgen", GATE_BUS_PERIS1, 2, 0, 0,
-		"mct"),
+static struct samsung_gate_clock exynos5x_gate_clks[] __initdata = {
+	/* G2D */
+	GATE(CLK_MDMA0, "mdma0", "aclk266_g2d", GATE_IP_G2D, 1, 0, 0),
+	GATE(CLK_SSS, "sss", "aclk266_g2d", GATE_IP_G2D, 2, 0, 0),
+	GATE(CLK_G2D, "g2d", "aclk333_g2d", GATE_IP_G2D, 3, 0, 0),
+	GATE(CLK_SMMU_MDMA0, "smmu_mdma0", "aclk266_g2d", GATE_IP_G2D, 5, 0, 0),
+	GATE(CLK_SMMU_G2D, "smmu_g2d", "aclk333_g2d", GATE_IP_G2D, 7, 0, 0),
 
 	GATE(0, "aclk200_fsys", "mout_user_aclk200_fsys",
 			GATE_BUS_FSYS0, 9, CLK_IGNORE_UNUSED, 0),
@@ -530,20 +878,42 @@ static struct samsung_gate_clock exynos5420_gate_clks[] __initdata = {
 			GATE_BUS_TOP, 1, CLK_IGNORE_UNUSED, 0),
 	GATE(0, "aclk300_jpeg", "mout_user_aclk300_jpeg",
 			GATE_BUS_TOP, 4, CLK_IGNORE_UNUSED, 0),
+	GATE(0, "aclk333_432_isp0", "mout_user_aclk333_432_isp0",
+			GATE_BUS_TOP, 5, 0, 0),
 	GATE(0, "aclk300_gscl", "mout_user_aclk300_gscl",
 			GATE_BUS_TOP, 6, CLK_IGNORE_UNUSED, 0),
 	GATE(0, "aclk333_432_gscl", "mout_user_aclk333_432_gscl",
 			GATE_BUS_TOP, 7, CLK_IGNORE_UNUSED, 0),
-	GATE(0, "pclk66_gpio", "mout_sw_aclk66",
+	GATE(0, "aclk333_432_isp", "mout_user_aclk333_432_isp",
+			GATE_BUS_TOP, 8, 0, 0),
+	GATE(CLK_PCLK66_GPIO, "pclk66_gpio", "mout_user_pclk66_gpio",
 			GATE_BUS_TOP, 9, CLK_IGNORE_UNUSED, 0),
-	GATE(0, "aclk66_psgen", "mout_aclk66_psgen",
+	GATE(0, "aclk66_psgen", "mout_user_aclk66_psgen",
 			GATE_BUS_TOP, 10, CLK_IGNORE_UNUSED, 0),
-	GATE(0, "aclk66_peric", "mout_aclk66_peric",
-			GATE_BUS_TOP, 11, 0, 0),
+	GATE(CLK_ACLK66_PERIC, "aclk66_peric", "mout_user_aclk66_peric",
+			GATE_BUS_TOP, 11, CLK_IGNORE_UNUSED, 0),
+	GATE(0, "aclk266_isp", "mout_user_aclk266_isp",
+			GATE_BUS_TOP, 13, 0, 0),
 	GATE(0, "aclk166", "mout_user_aclk166",
 			GATE_BUS_TOP, 14, CLK_IGNORE_UNUSED, 0),
 	GATE(0, "aclk333", "mout_aclk333",
 			GATE_BUS_TOP, 15, CLK_IGNORE_UNUSED, 0),
+	GATE(0, "aclk400_isp", "mout_user_aclk400_isp",
+			GATE_BUS_TOP, 16, 0, 0),
+	GATE(0, "aclk400_mscl", "mout_user_aclk400_mscl",
+			GATE_BUS_TOP, 17, 0, 0),
+	GATE(0, "aclk200_disp1", "mout_user_aclk200_disp1",
+			GATE_BUS_TOP, 18, 0, 0),
+	GATE(CLK_SCLK_MPHY_IXTAL24, "sclk_mphy_ixtal24", "mphy_refclk_ixtal24",
+			GATE_BUS_TOP, 28, 0, 0),
+	GATE(CLK_SCLK_HSIC_12M, "sclk_hsic_12m", "ff_hsic_12m",
+			GATE_BUS_TOP, 29, 0, 0),
+
+	GATE(0, "aclk300_disp1", "mout_user_aclk300_disp1",
+			SRC_MASK_TOP2, 24, 0, 0),
+
+	GATE(CLK_MAU_EPLL, "mau_epll", "mout_mau_epll_clk",
+			SRC_MASK_TOP7, 20, 0, 0),
 
 	/* sclk */
 	GATE(CLK_SCLK_UART0, "sclk_uart0", "dout_uart0",
@@ -554,11 +924,11 @@ static struct samsung_gate_clock exynos5420_gate_clks[] __initdata = {
 		GATE_TOP_SCLK_PERIC, 2, CLK_SET_RATE_PARENT, 0),
 	GATE(CLK_SCLK_UART3, "sclk_uart3", "dout_uart3",
 		GATE_TOP_SCLK_PERIC, 3, CLK_SET_RATE_PARENT, 0),
-	GATE(CLK_SCLK_SPI0, "sclk_spi0", "dout_pre_spi0",
+	GATE(CLK_SCLK_SPI0, "sclk_spi0", "dout_spi0_pre",
 		GATE_TOP_SCLK_PERIC, 6, CLK_SET_RATE_PARENT, 0),
-	GATE(CLK_SCLK_SPI1, "sclk_spi1", "dout_pre_spi1",
+	GATE(CLK_SCLK_SPI1, "sclk_spi1", "dout_spi1_pre",
 		GATE_TOP_SCLK_PERIC, 7, CLK_SET_RATE_PARENT, 0),
-	GATE(CLK_SCLK_SPI2, "sclk_spi2", "dout_pre_spi2",
+	GATE(CLK_SCLK_SPI2, "sclk_spi2", "dout_spi2_pre",
 		GATE_TOP_SCLK_PERIC, 8, CLK_SET_RATE_PARENT, 0),
 	GATE(CLK_SCLK_SPDIF, "sclk_spdif", "mout_spdif",
 		GATE_TOP_SCLK_PERIC, 9, CLK_SET_RATE_PARENT, 0),
@@ -588,164 +958,191 @@ static struct samsung_gate_clock exynos5420_gate_clks[] __initdata = {
 	GATE(CLK_SCLK_USBD301, "sclk_usbd301", "dout_usbd301",
 		GATE_TOP_SCLK_FSYS, 10, CLK_SET_RATE_PARENT, 0),
 
-	GATE(CLK_SCLK_USBD301, "sclk_unipro", "dout_unipro",
-		SRC_MASK_FSYS, 24, CLK_SET_RATE_PARENT, 0),
-
-	GATE(CLK_SCLK_GSCL_WA, "sclk_gscl_wa", "aclK333_432_gscl",
-		GATE_TOP_SCLK_GSCL, 6, CLK_SET_RATE_PARENT, 0),
-	GATE(CLK_SCLK_GSCL_WB, "sclk_gscl_wb", "aclk333_432_gscl",
-		GATE_TOP_SCLK_GSCL, 7, CLK_SET_RATE_PARENT, 0),
-
 	/* Display */
 	GATE(CLK_SCLK_FIMD1, "sclk_fimd1", "dout_fimd1",
-		GATE_TOP_SCLK_DISP1, 0, CLK_SET_RATE_PARENT, 0),
+			GATE_TOP_SCLK_DISP1, 0, CLK_SET_RATE_PARENT, 0),
 	GATE(CLK_SCLK_MIPI1, "sclk_mipi1", "dout_mipi1",
-		GATE_TOP_SCLK_DISP1, 3, CLK_SET_RATE_PARENT, 0),
+			GATE_TOP_SCLK_DISP1, 3, CLK_SET_RATE_PARENT, 0),
 	GATE(CLK_SCLK_HDMI, "sclk_hdmi", "mout_hdmi",
-		GATE_TOP_SCLK_DISP1, 9, CLK_SET_RATE_PARENT, 0),
+			GATE_TOP_SCLK_DISP1, 9, 0, 0),
 	GATE(CLK_SCLK_PIXEL, "sclk_pixel", "dout_hdmi_pixel",
-		GATE_TOP_SCLK_DISP1, 10, CLK_SET_RATE_PARENT, 0),
+			GATE_TOP_SCLK_DISP1, 10, CLK_SET_RATE_PARENT, 0),
 	GATE(CLK_SCLK_DP1, "sclk_dp1", "dout_dp1",
-		GATE_TOP_SCLK_DISP1, 20, CLK_SET_RATE_PARENT, 0),
+			GATE_TOP_SCLK_DISP1, 20, CLK_SET_RATE_PARENT, 0),
 
 	/* Maudio Block */
 	GATE(CLK_SCLK_MAUDIO0, "sclk_maudio0", "dout_maudio0",
 		GATE_TOP_SCLK_MAU, 0, CLK_SET_RATE_PARENT, 0),
 	GATE(CLK_SCLK_MAUPCM0, "sclk_maupcm0", "dout_maupcm0",
 		GATE_TOP_SCLK_MAU, 1, CLK_SET_RATE_PARENT, 0),
-	/* FSYS */
+
+	/* FSYS Block */
 	GATE(CLK_TSI, "tsi", "aclk200_fsys", GATE_BUS_FSYS0, 0, 0, 0),
 	GATE(CLK_PDMA0, "pdma0", "aclk200_fsys", GATE_BUS_FSYS0, 1, 0, 0),
 	GATE(CLK_PDMA1, "pdma1", "aclk200_fsys", GATE_BUS_FSYS0, 2, 0, 0),
 	GATE(CLK_UFS, "ufs", "aclk200_fsys2", GATE_BUS_FSYS0, 3, 0, 0),
-	GATE(CLK_RTIC, "rtic", "aclk200_fsys", GATE_BUS_FSYS0, 5, 0, 0),
-	GATE(CLK_MMC0, "mmc0", "aclk200_fsys2", GATE_BUS_FSYS0, 12, 0, 0),
-	GATE(CLK_MMC1, "mmc1", "aclk200_fsys2", GATE_BUS_FSYS0, 13, 0, 0),
-	GATE(CLK_MMC2, "mmc2", "aclk200_fsys2", GATE_BUS_FSYS0, 14, 0, 0),
+	GATE(CLK_RTIC, "rtic", "aclk200_fsys", GATE_IP_FSYS, 9, 0, 0),
+	GATE(CLK_MMC0, "mmc0", "aclk200_fsys2", GATE_IP_FSYS, 12, 0, 0),
+	GATE(CLK_MMC1, "mmc1", "aclk200_fsys2", GATE_IP_FSYS, 13, 0, 0),
+	GATE(CLK_MMC2, "mmc2", "aclk200_fsys2", GATE_IP_FSYS, 14, 0, 0),
 	GATE(CLK_SROMC, "sromc", "aclk200_fsys2",
-			GATE_BUS_FSYS0, 19, CLK_IGNORE_UNUSED, 0),
-	GATE(CLK_USBH20, "usbh20", "aclk200_fsys", GATE_BUS_FSYS0, 20, 0, 0),
-	GATE(CLK_USBD300, "usbd300", "aclk200_fsys", GATE_BUS_FSYS0, 21, 0, 0),
-	GATE(CLK_USBD301, "usbd301", "aclk200_fsys", GATE_BUS_FSYS0, 28, 0, 0),
-
-	/* UART */
-	GATE(CLK_UART0, "uart0", "aclk66_peric", GATE_BUS_PERIC, 4, 0, 0),
-	GATE(CLK_UART1, "uart1", "aclk66_peric", GATE_BUS_PERIC, 5, 0, 0),
-	GATE_A(CLK_UART2, "uart2", "aclk66_peric",
-		GATE_BUS_PERIC, 6, CLK_IGNORE_UNUSED, 0, "uart2"),
-	GATE(CLK_UART3, "uart3", "aclk66_peric", GATE_BUS_PERIC, 7, 0, 0),
-	/* I2C */
-	GATE(CLK_I2C0, "i2c0", "aclk66_peric", GATE_BUS_PERIC, 9, 0, 0),
-	GATE(CLK_I2C1, "i2c1", "aclk66_peric", GATE_BUS_PERIC, 10, 0, 0),
-	GATE(CLK_I2C2, "i2c2", "aclk66_peric", GATE_BUS_PERIC, 11, 0, 0),
-	GATE(CLK_I2C3, "i2c3", "aclk66_peric", GATE_BUS_PERIC, 12, 0, 0),
-	GATE(CLK_I2C4, "i2c4", "aclk66_peric", GATE_BUS_PERIC, 13, 0, 0),
-	GATE(CLK_I2C5, "i2c5", "aclk66_peric", GATE_BUS_PERIC, 14, 0, 0),
-	GATE(CLK_I2C6, "i2c6", "aclk66_peric", GATE_BUS_PERIC, 15, 0, 0),
-	GATE(CLK_I2C7, "i2c7", "aclk66_peric", GATE_BUS_PERIC, 16, 0, 0),
-	GATE(CLK_I2C_HDMI, "i2c_hdmi", "aclk66_peric", GATE_BUS_PERIC, 17, 0,
-		0),
-	GATE(CLK_TSADC, "tsadc", "aclk66_peric", GATE_BUS_PERIC, 18, 0, 0),
-	/* SPI */
-	GATE(CLK_SPI0, "spi0", "aclk66_peric", GATE_BUS_PERIC, 19, 0, 0),
-	GATE(CLK_SPI1, "spi1", "aclk66_peric", GATE_BUS_PERIC, 20, 0, 0),
-	GATE(CLK_SPI2, "spi2", "aclk66_peric", GATE_BUS_PERIC, 21, 0, 0),
-	GATE(CLK_KEYIF, "keyif", "aclk66_peric", GATE_BUS_PERIC, 22, 0, 0),
-	/* I2S */
-	GATE(CLK_I2S1, "i2s1", "aclk66_peric", GATE_BUS_PERIC, 23, 0, 0),
-	GATE(CLK_I2S2, "i2s2", "aclk66_peric", GATE_BUS_PERIC, 24, 0, 0),
-	/* PCM */
-	GATE(CLK_PCM1, "pcm1", "aclk66_peric", GATE_BUS_PERIC, 25, 0, 0),
-	GATE(CLK_PCM2, "pcm2", "aclk66_peric", GATE_BUS_PERIC, 26, 0, 0),
-	/* PWM */
-	GATE(CLK_PWM, "pwm", "aclk66_peric", GATE_BUS_PERIC, 27, 0, 0),
-	/* SPDIF */
-	GATE(CLK_SPDIF, "spdif", "aclk66_peric", GATE_BUS_PERIC, 29, 0, 0),
+			GATE_IP_FSYS, 17, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_USBH20, "usbh20", "aclk200_fsys", GATE_IP_FSYS, 18, 0, 0),
+	GATE(CLK_USBD300, "usbd300", "aclk200_fsys", GATE_IP_FSYS, 19, 0, 0),
+	GATE(CLK_USBD301, "usbd301", "aclk200_fsys", GATE_IP_FSYS, 20, 0, 0),
+	GATE(CLK_SCLK_UNIPRO, "sclk_unipro", "dout_unipro",
+			SRC_MASK_FSYS, 24, CLK_SET_RATE_PARENT, 0),
 
-	GATE(CLK_I2C8, "i2c8", "aclk66_peric", GATE_BUS_PERIC1, 0, 0, 0),
-	GATE(CLK_I2C9, "i2c9", "aclk66_peric", GATE_BUS_PERIC1, 1, 0, 0),
-	GATE(CLK_I2C10, "i2c10", "aclk66_peric", GATE_BUS_PERIC1, 2, 0, 0),
+	/* PERIC Block */
+	GATE(CLK_UART0, "uart0", "aclk66_peric", GATE_IP_PERIC, 0, 0, 0),
+	GATE(CLK_UART1, "uart1", "aclk66_peric", GATE_IP_PERIC, 1, 0, 0),
+	GATE(CLK_UART2, "uart2", "aclk66_peric", GATE_IP_PERIC, 2, 0, 0),
+	GATE(CLK_UART3, "uart3", "aclk66_peric", GATE_IP_PERIC, 3, 0, 0),
+	GATE(CLK_I2C0, "i2c0", "aclk66_peric", GATE_IP_PERIC, 6, 0, 0),
+	GATE(CLK_I2C1, "i2c1", "aclk66_peric", GATE_IP_PERIC, 7, 0, 0),
+	GATE(CLK_I2C2, "i2c2", "aclk66_peric", GATE_IP_PERIC, 8, 0, 0),
+	GATE(CLK_I2C3, "i2c3", "aclk66_peric", GATE_IP_PERIC, 9, 0, 0),
+	GATE(CLK_USI0, "usi0", "aclk66_peric", GATE_IP_PERIC, 10, 0, 0),
+	GATE(CLK_USI1, "usi1", "aclk66_peric", GATE_IP_PERIC, 11, 0, 0),
+	GATE(CLK_USI2, "usi2", "aclk66_peric", GATE_IP_PERIC, 12, 0, 0),
+	GATE(CLK_USI3, "usi3", "aclk66_peric", GATE_IP_PERIC, 13, 0, 0),
+	GATE(CLK_I2C_HDMI, "i2c_hdmi", "aclk66_peric", GATE_IP_PERIC, 14, 0, 0),
+	GATE(CLK_TSADC, "tsadc", "aclk66_peric", GATE_IP_PERIC, 15, 0, 0),
+	GATE(CLK_SPI0, "spi0", "aclk66_peric", GATE_IP_PERIC, 16, 0, 0),
+	GATE(CLK_SPI1, "spi1", "aclk66_peric", GATE_IP_PERIC, 17, 0, 0),
+	GATE(CLK_SPI2, "spi2", "aclk66_peric", GATE_IP_PERIC, 18, 0, 0),
+	GATE(CLK_I2S1, "i2s1", "aclk66_peric", GATE_IP_PERIC, 20, 0, 0),
+	GATE(CLK_I2S2, "i2s2", "aclk66_peric", GATE_IP_PERIC, 21, 0, 0),
+	GATE(CLK_PCM1, "pcm1", "aclk66_peric", GATE_IP_PERIC, 22, 0, 0),
+	GATE(CLK_PCM2, "pcm2", "aclk66_peric", GATE_IP_PERIC, 23, 0, 0),
+	GATE(CLK_PWM, "pwm", "aclk66_peric", GATE_IP_PERIC, 24, 0, 0),
+	GATE(CLK_SPDIF, "spdif", "aclk66_peric", GATE_IP_PERIC, 26, 0, 0),
+	GATE(CLK_USI4, "usi4", "aclk66_peric", GATE_IP_PERIC, 28, 0, 0),
+	GATE(CLK_USI5, "usi5", "aclk66_peric", GATE_IP_PERIC, 30, 0, 0),
+	GATE(CLK_USI6, "usi6", "aclk66_peric", GATE_IP_PERIC, 31, 0, 0),
 
+	GATE(CLK_KEYIF, "keyif", "aclk66_peric", GATE_BUS_PERIC, 22, 0, 0),
+
+	/* PERIS Block */
 	GATE(CLK_CHIPID, "chipid", "aclk66_psgen",
-			GATE_BUS_PERIS0, 12, CLK_IGNORE_UNUSED, 0),
+			GATE_IP_PERIS, 0, CLK_IGNORE_UNUSED, 0),
 	GATE(CLK_SYSREG, "sysreg", "aclk66_psgen",
-			GATE_BUS_PERIS0, 13, CLK_IGNORE_UNUSED, 0),
-	GATE(CLK_TZPC0, "tzpc0", "aclk66_psgen", GATE_BUS_PERIS0, 18, 0, 0),
-	GATE(CLK_TZPC1, "tzpc1", "aclk66_psgen", GATE_BUS_PERIS0, 19, 0, 0),
-	GATE(CLK_TZPC2, "tzpc2", "aclk66_psgen", GATE_BUS_PERIS0, 20, 0, 0),
-	GATE(CLK_TZPC3, "tzpc3", "aclk66_psgen", GATE_BUS_PERIS0, 21, 0, 0),
-	GATE(CLK_TZPC4, "tzpc4", "aclk66_psgen", GATE_BUS_PERIS0, 22, 0, 0),
-	GATE(CLK_TZPC5, "tzpc5", "aclk66_psgen", GATE_BUS_PERIS0, 23, 0, 0),
-	GATE(CLK_TZPC6, "tzpc6", "aclk66_psgen", GATE_BUS_PERIS0, 24, 0, 0),
-	GATE(CLK_TZPC7, "tzpc7", "aclk66_psgen", GATE_BUS_PERIS0, 25, 0, 0),
-	GATE(CLK_TZPC8, "tzpc8", "aclk66_psgen", GATE_BUS_PERIS0, 26, 0, 0),
-	GATE(CLK_TZPC9, "tzpc9", "aclk66_psgen", GATE_BUS_PERIS0, 27, 0, 0),
-
-	GATE(CLK_HDMI_CEC, "hdmi_cec", "aclk66_psgen", GATE_BUS_PERIS1, 0, 0,
-		0),
+			GATE_IP_PERIS, 1, CLK_IGNORE_UNUSED, 0),
+	GATE(CLK_TZPC0, "tzpc0", "aclk66_psgen", GATE_IP_PERIS, 6, 0, 0),
+	GATE(CLK_TZPC1, "tzpc1", "aclk66_psgen", GATE_IP_PERIS, 7, 0, 0),
+	GATE(CLK_TZPC2, "tzpc2", "aclk66_psgen", GATE_IP_PERIS, 8, 0, 0),
+	GATE(CLK_TZPC3, "tzpc3", "aclk66_psgen", GATE_IP_PERIS, 9, 0, 0),
+	GATE(CLK_TZPC4, "tzpc4", "aclk66_psgen", GATE_IP_PERIS, 10, 0, 0),
+	GATE(CLK_TZPC5, "tzpc5", "aclk66_psgen", GATE_IP_PERIS, 11, 0, 0),
+	GATE(CLK_TZPC6, "tzpc6", "aclk66_psgen", GATE_IP_PERIS, 12, 0, 0),
+	GATE(CLK_TZPC7, "tzpc7", "aclk66_psgen", GATE_IP_PERIS, 13, 0, 0),
+	GATE(CLK_TZPC8, "tzpc8", "aclk66_psgen", GATE_IP_PERIS, 14, 0, 0),
+	GATE(CLK_TZPC9, "tzpc9", "aclk66_psgen", GATE_IP_PERIS, 15, 0, 0),
+	GATE(CLK_HDMI_CEC, "hdmi_cec", "aclk66_psgen", GATE_IP_PERIS, 16, 0, 0),
+	GATE(CLK_MCT, "mct", "aclk66_psgen", GATE_IP_PERIS, 18, 0, 0),
+	GATE(CLK_WDT, "wdt", "aclk66_psgen", GATE_IP_PERIS, 19, 0, 0),
+	GATE(CLK_RTC, "rtc", "aclk66_psgen", GATE_IP_PERIS, 20, 0, 0),
+	GATE(CLK_TMU, "tmu", "aclk66_psgen", GATE_IP_PERIS, 21, 0, 0),
+	GATE(CLK_TMU_GPU, "tmu_gpu", "aclk66_psgen", GATE_IP_PERIS, 22, 0, 0),
+
 	GATE(CLK_SECKEY, "seckey", "aclk66_psgen", GATE_BUS_PERIS1, 1, 0, 0),
-	GATE(CLK_WDT, "wdt", "aclk66_psgen", GATE_BUS_PERIS1, 3, 0, 0),
-	GATE(CLK_RTC, "rtc", "aclk66_psgen", GATE_BUS_PERIS1, 4, 0, 0),
-	GATE(CLK_TMU, "tmu", "aclk66_psgen", GATE_BUS_PERIS1, 5, 0, 0),
-	GATE(CLK_TMU_GPU, "tmu_gpu", "aclk66_psgen", GATE_BUS_PERIS1, 6, 0, 0),
+
+	/* GEN Block */
+	GATE(CLK_ROTATOR, "rotator", "mout_user_aclk266", GATE_IP_GEN, 1, 0, 0),
+	GATE(CLK_JPEG, "jpeg", "aclk300_jpeg", GATE_IP_GEN, 2, 0, 0),
+	GATE(CLK_JPEG2, "jpeg2", "aclk300_jpeg", GATE_IP_GEN, 3, 0, 0),
+	GATE(CLK_MDMA1, "mdma1", "mout_user_aclk266", GATE_IP_GEN, 4, 0, 0),
+	GATE(CLK_TOP_RTC, "top_rtc", "aclk66_psgen", GATE_IP_GEN, 5, 0, 0),
+	GATE(CLK_SMMU_ROTATOR, "smmu_rotator", "dout_gen_blk",
+			GATE_IP_GEN, 6, 0, 0),
+	GATE(CLK_SMMU_JPEG, "smmu_jpeg", "dout_jpg_blk", GATE_IP_GEN, 7, 0, 0),
+	GATE(CLK_SMMU_MDMA1, "smmu_mdma1", "dout_gen_blk",
+			GATE_IP_GEN, 9, 0, 0),
+
+	/* GATE_IP_GEN doesn't list gates for smmu_jpeg2 and mc */
+	GATE(CLK_SMMU_JPEG2, "smmu_jpeg2", "dout_jpg_blk",
+			GATE_BUS_GEN, 28, 0, 0),
+	GATE(CLK_MC, "mc", "aclk66_psgen", GATE_BUS_GEN, 12, 0, 0),
+
+	/* GSCL Block */
+	GATE(CLK_SCLK_GSCL_WA, "sclk_gscl_wa", "mout_user_aclk333_432_gscl",
+			GATE_TOP_SCLK_GSCL, 6, 0, 0),
+	GATE(CLK_SCLK_GSCL_WB, "sclk_gscl_wb", "mout_user_aclk333_432_gscl",
+			GATE_TOP_SCLK_GSCL, 7, 0, 0),
 
 	GATE(CLK_GSCL0, "gscl0", "aclk300_gscl", GATE_IP_GSCL0, 0, 0, 0),
 	GATE(CLK_GSCL1, "gscl1", "aclk300_gscl", GATE_IP_GSCL0, 1, 0, 0),
-	GATE(CLK_CLK_3AA, "clk_3aa", "aclk300_gscl", GATE_IP_GSCL0, 4, 0, 0),
-
-	GATE(CLK_SMMU_3AA, "smmu_3aa", "aclk333_432_gscl", GATE_IP_GSCL1, 2, 0,
-		0),
-	GATE(CLK_SMMU_FIMCL0, "smmu_fimcl0", "aclk333_432_gscl",
+	GATE(CLK_FIMC_3AA, "fimc_3aa", "aclk333_432_gscl",
+			GATE_IP_GSCL0, 4, 0, 0),
+	GATE(CLK_FIMC_LITE0, "fimc_lite0", "aclk333_432_gscl",
+			GATE_IP_GSCL0, 5, 0, 0),
+	GATE(CLK_FIMC_LITE1, "fimc_lite1", "aclk333_432_gscl",
+			GATE_IP_GSCL0, 6, 0, 0),
+
+	GATE(CLK_SMMU_3AA, "smmu_3aa", "dout_gscl_blk_333",
+			GATE_IP_GSCL1, 2, 0, 0),
+	GATE(CLK_SMMU_FIMCL0, "smmu_fimcl0", "dout_gscl_blk_333",
 			GATE_IP_GSCL1, 3, 0, 0),
-	GATE(CLK_SMMU_FIMCL1, "smmu_fimcl1", "aclk333_432_gscl",
+	GATE(CLK_SMMU_FIMCL1, "smmu_fimcl1", "dout_gscl_blk_333",
 			GATE_IP_GSCL1, 4, 0, 0),
-	GATE(CLK_SMMU_GSCL0, "smmu_gscl0", "aclk300_gscl", GATE_IP_GSCL1, 6, 0,
-		0),
-	GATE(CLK_SMMU_GSCL1, "smmu_gscl1", "aclk300_gscl", GATE_IP_GSCL1, 7, 0,
-		0),
-	GATE(CLK_GSCL_WA, "gscl_wa", "aclk300_gscl", GATE_IP_GSCL1, 12, 0, 0),
-	GATE(CLK_GSCL_WB, "gscl_wb", "aclk300_gscl", GATE_IP_GSCL1, 13, 0, 0),
-	GATE(CLK_SMMU_FIMCL3, "smmu_fimcl3,", "aclk333_432_gscl",
+	GATE(CLK_SMMU_GSCL0, "smmu_gscl0", "dout_gscl_blk_300",
+			GATE_IP_GSCL1, 6, 0, 0),
+	GATE(CLK_SMMU_GSCL1, "smmu_gscl1", "dout_gscl_blk_300",
+			GATE_IP_GSCL1, 7, 0, 0),
+	GATE(CLK_GSCL_WA, "gscl_wa", "sclk_gscl_wa", GATE_IP_GSCL1, 12, 0, 0),
+	GATE(CLK_GSCL_WB, "gscl_wb", "sclk_gscl_wb", GATE_IP_GSCL1, 13, 0, 0),
+	GATE(CLK_SMMU_FIMCL3, "smmu_fimcl3,", "dout_gscl_blk_333",
 			GATE_IP_GSCL1, 16, 0, 0),
 	GATE(CLK_FIMC_LITE3, "fimc_lite3", "aclk333_432_gscl",
 			GATE_IP_GSCL1, 17, 0, 0),
 
+	/* MSCL Block */
+	GATE(CLK_MSCL0, "mscl0", "aclk400_mscl", GATE_IP_MSCL, 0, 0, 0),
+	GATE(CLK_MSCL1, "mscl1", "aclk400_mscl", GATE_IP_MSCL, 1, 0, 0),
+	GATE(CLK_MSCL2, "mscl2", "aclk400_mscl", GATE_IP_MSCL, 2, 0, 0),
+	GATE(CLK_SMMU_MSCL0, "smmu_mscl0", "dout_mscl_blk",
+			GATE_IP_MSCL, 8, 0, 0),
+	GATE(CLK_SMMU_MSCL1, "smmu_mscl1", "dout_mscl_blk",
+			GATE_IP_MSCL, 9, 0, 0),
+	GATE(CLK_SMMU_MSCL2, "smmu_mscl2", "dout_mscl_blk",
+			GATE_IP_MSCL, 10, 0, 0),
+
 	GATE(CLK_FIMD1, "fimd1", "aclk300_disp1", GATE_IP_DISP1, 0, 0, 0),
 	GATE(CLK_DSIM1, "dsim1", "aclk200_disp1", GATE_IP_DISP1, 3, 0, 0),
 	GATE(CLK_DP1, "dp1", "aclk200_disp1", GATE_IP_DISP1, 4, 0, 0),
-	GATE(CLK_MIXER, "mixer", "aclk166", GATE_IP_DISP1, 5, 0, 0),
+	GATE(CLK_MIXER, "mixer", "aclk200_disp1", GATE_IP_DISP1, 5, 0, 0),
 	GATE(CLK_HDMI, "hdmi", "aclk200_disp1", GATE_IP_DISP1, 6, 0, 0),
-	GATE(CLK_SMMU_FIMD1, "smmu_fimd1", "aclk300_disp1", GATE_IP_DISP1, 8, 0,
-		0),
+	GATE(CLK_SMMU_FIMD1M0, "smmu_fimd1m0", "dout_disp1_blk",
+			GATE_IP_DISP1, 7, 0, 0),
+	GATE(CLK_SMMU_FIMD1M1, "smmu_fimd1m1", "dout_disp1_blk",
+			GATE_IP_DISP1, 8, 0, 0),
+	GATE(CLK_SMMU_MIXER, "smmu_mixer", "aclk200_disp1",
+			GATE_IP_DISP1, 9, 0, 0),
+
+	/* ISP */
+	GATE(CLK_SCLK_UART_ISP, "sclk_uart_isp", "dout_uart_isp",
+			GATE_TOP_SCLK_ISP, 0, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_SPI0_ISP, "sclk_spi0_isp", "dout_spi0_isp_pre",
+			GATE_TOP_SCLK_ISP, 1, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_SPI1_ISP, "sclk_spi1_isp", "dout_spi1_isp_pre",
+			GATE_TOP_SCLK_ISP, 2, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_PWM_ISP, "sclk_pwm_isp", "dout_pwm_isp",
+			GATE_TOP_SCLK_ISP, 3, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_ISP_SENSOR0, "sclk_isp_sensor0", "dout_isp_sensor0",
+			GATE_TOP_SCLK_ISP, 4, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_ISP_SENSOR1, "sclk_isp_sensor1", "dout_isp_sensor1",
+			GATE_TOP_SCLK_ISP, 8, CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_SCLK_ISP_SENSOR2, "sclk_isp_sensor2", "dout_isp_sensor2",
+			GATE_TOP_SCLK_ISP, 12, CLK_SET_RATE_PARENT, 0),
 
 	GATE(CLK_MFC, "mfc", "aclk333", GATE_IP_MFC, 0, 0, 0),
-	GATE(CLK_SMMU_MFCL, "smmu_mfcl", "aclk333", GATE_IP_MFC, 1, 0, 0),
-	GATE(CLK_SMMU_MFCR, "smmu_mfcr", "aclk333", GATE_IP_MFC, 2, 0, 0),
-
-	GATE(CLK_G3D, "g3d", "aclkg3d", GATE_IP_G3D, 9, 0, 0),
+	GATE(CLK_SMMU_MFCL, "smmu_mfcl", "dout_mfc_blk", GATE_IP_MFC, 1, 0, 0),
+	GATE(CLK_SMMU_MFCR, "smmu_mfcr", "dout_mfc_blk", GATE_IP_MFC, 2, 0, 0),
 
-	GATE(CLK_ROTATOR, "rotator", "aclk266", GATE_IP_GEN, 1, 0, 0),
-	GATE(CLK_JPEG, "jpeg", "aclk300_jpeg", GATE_IP_GEN, 2, 0, 0),
-	GATE(CLK_JPEG2, "jpeg2", "aclk300_jpeg", GATE_IP_GEN, 3, 0, 0),
-	GATE(CLK_MDMA1, "mdma1", "aclk266", GATE_IP_GEN, 4, 0, 0),
-	GATE(CLK_SMMU_ROTATOR, "smmu_rotator", "aclk266", GATE_IP_GEN, 6, 0, 0),
-	GATE(CLK_SMMU_JPEG, "smmu_jpeg", "aclk300_jpeg", GATE_IP_GEN, 7, 0, 0),
-	GATE(CLK_SMMU_MDMA1, "smmu_mdma1", "aclk266", GATE_IP_GEN, 9, 0, 0),
-
-	GATE(CLK_MSCL0, "mscl0", "aclk400_mscl", GATE_IP_MSCL, 0, 0, 0),
-	GATE(CLK_MSCL1, "mscl1", "aclk400_mscl", GATE_IP_MSCL, 1, 0, 0),
-	GATE(CLK_MSCL2, "mscl2", "aclk400_mscl", GATE_IP_MSCL, 2, 0, 0),
-	GATE(CLK_SMMU_MSCL0, "smmu_mscl0", "aclk400_mscl", GATE_IP_MSCL, 8, 0,
-		0),
-	GATE(CLK_SMMU_MSCL1, "smmu_mscl1", "aclk400_mscl", GATE_IP_MSCL, 9, 0,
-		0),
-	GATE(CLK_SMMU_MSCL2, "smmu_mscl2", "aclk400_mscl", GATE_IP_MSCL, 10, 0,
-		0),
-	GATE(CLK_SMMU_MIXER, "smmu_mixer", "aclk200_disp1", GATE_IP_DISP1, 9, 0,
-		0),
+	GATE(CLK_G3D, "g3d", "mout_user_aclk_g3d", GATE_IP_G3D, 9, 0, 0),
 };
 
-static struct samsung_pll_clock exynos5420_plls[nr_plls] __initdata = {
+static struct samsung_pll_clock exynos5x_plls[nr_plls] __initdata = {
 	[apll] = PLL(pll_2550, CLK_FOUT_APLL, "fout_apll", "fin_pll", APLL_LOCK,
 		APLL_CON0, NULL),
 	[cpll] = PLL(pll_2550, CLK_FOUT_CPLL, "fout_cpll", "fin_pll", CPLL_LOCK,
@@ -776,8 +1173,11 @@ static struct of_device_id ext_clk_match[] __initdata = {
 };
 
 /* register exynos5420 clocks */
-static void __init exynos5420_clk_init(struct device_node *np)
+static void __init exynos5x_clk_init(struct device_node *np,
+		enum exynos5x_soc soc)
 {
+	struct samsung_clk_provider *ctx;
+
 	if (np) {
 		reg_base = of_iomap(np, 0);
 		if (!reg_base)
@@ -786,23 +1186,56 @@ static void __init exynos5420_clk_init(struct device_node *np)
 		panic("%s: unable to determine soc\n", __func__);
 	}
 
-	samsung_clk_init(np, reg_base, CLK_NR_CLKS);
-	samsung_clk_of_register_fixed_ext(exynos5420_fixed_rate_ext_clks,
-			ARRAY_SIZE(exynos5420_fixed_rate_ext_clks),
+	exynos5x_soc = soc;
+
+	ctx = samsung_clk_init(np, reg_base, CLK_NR_CLKS);
+	if (!ctx)
+		panic("%s: unable to allocate context.\n", __func__);
+
+	samsung_clk_of_register_fixed_ext(ctx, exynos5x_fixed_rate_ext_clks,
+			ARRAY_SIZE(exynos5x_fixed_rate_ext_clks),
 			ext_clk_match);
-	samsung_clk_register_pll(exynos5420_plls, ARRAY_SIZE(exynos5420_plls),
+	samsung_clk_register_pll(ctx, exynos5x_plls, ARRAY_SIZE(exynos5x_plls),
 					reg_base);
-	samsung_clk_register_fixed_rate(exynos5420_fixed_rate_clks,
-			ARRAY_SIZE(exynos5420_fixed_rate_clks));
-	samsung_clk_register_fixed_factor(exynos5420_fixed_factor_clks,
-			ARRAY_SIZE(exynos5420_fixed_factor_clks));
-	samsung_clk_register_mux(exynos5420_mux_clks,
-			ARRAY_SIZE(exynos5420_mux_clks));
-	samsung_clk_register_div(exynos5420_div_clks,
-			ARRAY_SIZE(exynos5420_div_clks));
-	samsung_clk_register_gate(exynos5420_gate_clks,
-			ARRAY_SIZE(exynos5420_gate_clks));
+	samsung_clk_register_fixed_rate(ctx, exynos5x_fixed_rate_clks,
+			ARRAY_SIZE(exynos5x_fixed_rate_clks));
+	samsung_clk_register_fixed_factor(ctx, exynos5x_fixed_factor_clks,
+			ARRAY_SIZE(exynos5x_fixed_factor_clks));
+	samsung_clk_register_mux(ctx, exynos5x_mux_clks,
+			ARRAY_SIZE(exynos5x_mux_clks));
+	samsung_clk_register_div(ctx, exynos5x_div_clks,
+			ARRAY_SIZE(exynos5x_div_clks));
+	samsung_clk_register_gate(ctx, exynos5x_gate_clks,
+			ARRAY_SIZE(exynos5x_gate_clks));
+
+	if (soc == EXYNOS5420) {
+		samsung_clk_register_mux(ctx, exynos5420_mux_clks,
+				ARRAY_SIZE(exynos5420_mux_clks));
+		samsung_clk_register_div(ctx, exynos5420_div_clks,
+				ARRAY_SIZE(exynos5420_div_clks));
+	} else {
+		samsung_clk_register_fixed_factor(
+				ctx, exynos5800_fixed_factor_clks,
+				ARRAY_SIZE(exynos5800_fixed_factor_clks));
+		samsung_clk_register_mux(ctx, exynos5800_mux_clks,
+				ARRAY_SIZE(exynos5800_mux_clks));
+		samsung_clk_register_div(ctx, exynos5800_div_clks,
+				ARRAY_SIZE(exynos5800_div_clks));
+		samsung_clk_register_gate(ctx, exynos5800_gate_clks,
+				ARRAY_SIZE(exynos5800_gate_clks));
+	}
 
 	exynos5420_clk_sleep_init();
 }
+
+static void __init exynos5420_clk_init(struct device_node *np)
+{
+	exynos5x_clk_init(np, EXYNOS5420);
+}
 CLK_OF_DECLARE(exynos5420_clk, "samsung,exynos5420-clock", exynos5420_clk_init);
+
+static void __init exynos5800_clk_init(struct device_node *np)
+{
+	exynos5x_clk_init(np, EXYNOS5800);
+}
+CLK_OF_DECLARE(exynos5800_clk, "samsung,exynos5800-clock", exynos5800_clk_init);
diff --git a/drivers/clk/samsung/clk-exynos5440.c b/drivers/clk/samsung/clk-exynos5440.c
index 2bfad5a993d..647f1440aa6 100644
--- a/drivers/clk/samsung/clk-exynos5440.c
+++ b/drivers/clk/samsung/clk-exynos5440.c
@@ -93,6 +93,7 @@ static struct of_device_id ext_clk_match[] __initdata = {
 static void __init exynos5440_clk_init(struct device_node *np)
 {
 	void __iomem *reg_base;
+	struct samsung_clk_provider *ctx;
 
 	reg_base = of_iomap(np, 0);
 	if (!reg_base) {
@@ -101,22 +102,25 @@ static void __init exynos5440_clk_init(struct device_node *np)
 		return;
 	}
 
-	samsung_clk_init(np, reg_base, CLK_NR_CLKS);
-	samsung_clk_of_register_fixed_ext(exynos5440_fixed_rate_ext_clks,
+	ctx = samsung_clk_init(np, reg_base, CLK_NR_CLKS);
+	if (!ctx)
+		panic("%s: unable to allocate context.\n", __func__);
+
+	samsung_clk_of_register_fixed_ext(ctx, exynos5440_fixed_rate_ext_clks,
 		ARRAY_SIZE(exynos5440_fixed_rate_ext_clks), ext_clk_match);
 
 	samsung_clk_register_pll2550x("cplla", "xtal", reg_base + 0x1c, 0x10);
 	samsung_clk_register_pll2550x("cpllb", "xtal", reg_base + 0x20, 0x10);
 
-	samsung_clk_register_fixed_rate(exynos5440_fixed_rate_clks,
+	samsung_clk_register_fixed_rate(ctx, exynos5440_fixed_rate_clks,
 			ARRAY_SIZE(exynos5440_fixed_rate_clks));
-	samsung_clk_register_fixed_factor(exynos5440_fixed_factor_clks,
+	samsung_clk_register_fixed_factor(ctx, exynos5440_fixed_factor_clks,
 			ARRAY_SIZE(exynos5440_fixed_factor_clks));
-	samsung_clk_register_mux(exynos5440_mux_clks,
+	samsung_clk_register_mux(ctx, exynos5440_mux_clks,
 			ARRAY_SIZE(exynos5440_mux_clks));
-	samsung_clk_register_div(exynos5440_div_clks,
+	samsung_clk_register_div(ctx, exynos5440_div_clks,
 			ARRAY_SIZE(exynos5440_div_clks));
-	samsung_clk_register_gate(exynos5440_gate_clks,
+	samsung_clk_register_gate(ctx, exynos5440_gate_clks,
 			ARRAY_SIZE(exynos5440_gate_clks));
 
 	pr_info("Exynos5440: arm_clk = %ldHz\n", _get_rate("arm_clk"));
diff --git a/drivers/clk/samsung/clk-pll.c b/drivers/clk/samsung/clk-pll.c
index 81e6d2f49aa..b07fad2a916 100644
--- a/drivers/clk/samsung/clk-pll.c
+++ b/drivers/clk/samsung/clk-pll.c
@@ -11,6 +11,7 @@
 
 #include <linux/errno.h>
 #include <linux/hrtimer.h>
+#include <linux/delay.h>
 #include "clk.h"
 #include "clk-pll.h"
 
@@ -59,6 +60,72 @@ static long samsung_pll_round_rate(struct clk_hw *hw,
 }
 
 /*
+ * PLL2126 Clock Type
+ */
+
+#define PLL2126_MDIV_MASK	(0xff)
+#define PLL2126_PDIV_MASK	(0x3f)
+#define PLL2126_SDIV_MASK	(0x3)
+#define PLL2126_MDIV_SHIFT	(16)
+#define PLL2126_PDIV_SHIFT	(8)
+#define PLL2126_SDIV_SHIFT	(0)
+
+static unsigned long samsung_pll2126_recalc_rate(struct clk_hw *hw,
+				unsigned long parent_rate)
+{
+	struct samsung_clk_pll *pll = to_clk_pll(hw);
+	u32 pll_con, mdiv, pdiv, sdiv;
+	u64 fvco = parent_rate;
+
+	pll_con = __raw_readl(pll->con_reg);
+	mdiv = (pll_con >> PLL2126_MDIV_SHIFT) & PLL2126_MDIV_MASK;
+	pdiv = (pll_con >> PLL2126_PDIV_SHIFT) & PLL2126_PDIV_MASK;
+	sdiv = (pll_con >> PLL2126_SDIV_SHIFT) & PLL2126_SDIV_MASK;
+
+	fvco *= (mdiv + 8);
+	do_div(fvco, (pdiv + 2) << sdiv);
+
+	return (unsigned long)fvco;
+}
+
+static const struct clk_ops samsung_pll2126_clk_ops = {
+	.recalc_rate = samsung_pll2126_recalc_rate,
+};
+
+/*
+ * PLL3000 Clock Type
+ */
+
+#define PLL3000_MDIV_MASK	(0xff)
+#define PLL3000_PDIV_MASK	(0x3)
+#define PLL3000_SDIV_MASK	(0x3)
+#define PLL3000_MDIV_SHIFT	(16)
+#define PLL3000_PDIV_SHIFT	(8)
+#define PLL3000_SDIV_SHIFT	(0)
+
+static unsigned long samsung_pll3000_recalc_rate(struct clk_hw *hw,
+				unsigned long parent_rate)
+{
+	struct samsung_clk_pll *pll = to_clk_pll(hw);
+	u32 pll_con, mdiv, pdiv, sdiv;
+	u64 fvco = parent_rate;
+
+	pll_con = __raw_readl(pll->con_reg);
+	mdiv = (pll_con >> PLL3000_MDIV_SHIFT) & PLL3000_MDIV_MASK;
+	pdiv = (pll_con >> PLL3000_PDIV_SHIFT) & PLL3000_PDIV_MASK;
+	sdiv = (pll_con >> PLL3000_SDIV_SHIFT) & PLL3000_SDIV_MASK;
+
+	fvco *= (2 * (mdiv + 8));
+	do_div(fvco, pdiv << sdiv);
+
+	return (unsigned long)fvco;
+}
+
+static const struct clk_ops samsung_pll3000_clk_ops = {
+	.recalc_rate = samsung_pll3000_recalc_rate,
+};
+
+/*
  * PLL35xx Clock Type
  */
 /* Maximum lock time can be 270 * PDIV cycles */
@@ -564,7 +631,9 @@ static const struct clk_ops samsung_pll46xx_clk_min_ops = {
 #define PLL6552_PDIV_MASK	0x3f
 #define PLL6552_SDIV_MASK	0x7
 #define PLL6552_MDIV_SHIFT	16
+#define PLL6552_MDIV_SHIFT_2416	14
 #define PLL6552_PDIV_SHIFT	8
+#define PLL6552_PDIV_SHIFT_2416	5
 #define PLL6552_SDIV_SHIFT	0
 
 static unsigned long samsung_pll6552_recalc_rate(struct clk_hw *hw,
@@ -575,8 +644,13 @@ static unsigned long samsung_pll6552_recalc_rate(struct clk_hw *hw,
 	u64 fvco = parent_rate;
 
 	pll_con = __raw_readl(pll->con_reg);
-	mdiv = (pll_con >> PLL6552_MDIV_SHIFT) & PLL6552_MDIV_MASK;
-	pdiv = (pll_con >> PLL6552_PDIV_SHIFT) & PLL6552_PDIV_MASK;
+	if (pll->type == pll_6552_s3c2416) {
+		mdiv = (pll_con >> PLL6552_MDIV_SHIFT_2416) & PLL6552_MDIV_MASK;
+		pdiv = (pll_con >> PLL6552_PDIV_SHIFT_2416) & PLL6552_PDIV_MASK;
+	} else {
+		mdiv = (pll_con >> PLL6552_MDIV_SHIFT) & PLL6552_MDIV_MASK;
+		pdiv = (pll_con >> PLL6552_PDIV_SHIFT) & PLL6552_PDIV_MASK;
+	}
 	sdiv = (pll_con >> PLL6552_SDIV_SHIFT) & PLL6552_SDIV_MASK;
 
 	fvco *= mdiv;
@@ -628,6 +702,169 @@ static const struct clk_ops samsung_pll6553_clk_ops = {
 };
 
 /*
+ * PLL Clock Type of S3C24XX before S3C2443
+ */
+
+#define PLLS3C2410_MDIV_MASK		(0xff)
+#define PLLS3C2410_PDIV_MASK		(0x1f)
+#define PLLS3C2410_SDIV_MASK		(0x3)
+#define PLLS3C2410_MDIV_SHIFT		(12)
+#define PLLS3C2410_PDIV_SHIFT		(4)
+#define PLLS3C2410_SDIV_SHIFT		(0)
+
+#define PLLS3C2410_ENABLE_REG_OFFSET	0x10
+
+static unsigned long samsung_s3c2410_pll_recalc_rate(struct clk_hw *hw,
+					unsigned long parent_rate)
+{
+	struct samsung_clk_pll *pll = to_clk_pll(hw);
+	u32 pll_con, mdiv, pdiv, sdiv;
+	u64 fvco = parent_rate;
+
+	pll_con = __raw_readl(pll->con_reg);
+	mdiv = (pll_con >> PLLS3C2410_MDIV_SHIFT) & PLLS3C2410_MDIV_MASK;
+	pdiv = (pll_con >> PLLS3C2410_PDIV_SHIFT) & PLLS3C2410_PDIV_MASK;
+	sdiv = (pll_con >> PLLS3C2410_SDIV_SHIFT) & PLLS3C2410_SDIV_MASK;
+
+	fvco *= (mdiv + 8);
+	do_div(fvco, (pdiv + 2) << sdiv);
+
+	return (unsigned int)fvco;
+}
+
+static unsigned long samsung_s3c2440_mpll_recalc_rate(struct clk_hw *hw,
+					unsigned long parent_rate)
+{
+	struct samsung_clk_pll *pll = to_clk_pll(hw);
+	u32 pll_con, mdiv, pdiv, sdiv;
+	u64 fvco = parent_rate;
+
+	pll_con = __raw_readl(pll->con_reg);
+	mdiv = (pll_con >> PLLS3C2410_MDIV_SHIFT) & PLLS3C2410_MDIV_MASK;
+	pdiv = (pll_con >> PLLS3C2410_PDIV_SHIFT) & PLLS3C2410_PDIV_MASK;
+	sdiv = (pll_con >> PLLS3C2410_SDIV_SHIFT) & PLLS3C2410_SDIV_MASK;
+
+	fvco *= (2 * (mdiv + 8));
+	do_div(fvco, (pdiv + 2) << sdiv);
+
+	return (unsigned int)fvco;
+}
+
+static int samsung_s3c2410_pll_set_rate(struct clk_hw *hw, unsigned long drate,
+					unsigned long prate)
+{
+	struct samsung_clk_pll *pll = to_clk_pll(hw);
+	const struct samsung_pll_rate_table *rate;
+	u32 tmp;
+
+	/* Get required rate settings from table */
+	rate = samsung_get_pll_settings(pll, drate);
+	if (!rate) {
+		pr_err("%s: Invalid rate : %lu for pll clk %s\n", __func__,
+			drate, __clk_get_name(hw->clk));
+		return -EINVAL;
+	}
+
+	tmp = __raw_readl(pll->con_reg);
+
+	/* Change PLL PMS values */
+	tmp &= ~((PLLS3C2410_MDIV_MASK << PLLS3C2410_MDIV_SHIFT) |
+			(PLLS3C2410_PDIV_MASK << PLLS3C2410_PDIV_SHIFT) |
+			(PLLS3C2410_SDIV_MASK << PLLS3C2410_SDIV_SHIFT));
+	tmp |= (rate->mdiv << PLLS3C2410_MDIV_SHIFT) |
+			(rate->pdiv << PLLS3C2410_PDIV_SHIFT) |
+			(rate->sdiv << PLLS3C2410_SDIV_SHIFT);
+	__raw_writel(tmp, pll->con_reg);
+
+	/* Time to settle according to the manual */
+	udelay(300);
+
+	return 0;
+}
+
+static int samsung_s3c2410_pll_enable(struct clk_hw *hw, int bit, bool enable)
+{
+	struct samsung_clk_pll *pll = to_clk_pll(hw);
+	u32 pll_en = __raw_readl(pll->lock_reg + PLLS3C2410_ENABLE_REG_OFFSET);
+	u32 pll_en_orig = pll_en;
+
+	if (enable)
+		pll_en &= ~BIT(bit);
+	else
+		pll_en |= BIT(bit);
+
+	__raw_writel(pll_en, pll->lock_reg + PLLS3C2410_ENABLE_REG_OFFSET);
+
+	/* if we started the UPLL, then allow to settle */
+	if (enable && (pll_en_orig & BIT(bit)))
+		udelay(300);
+
+	return 0;
+}
+
+static int samsung_s3c2410_mpll_enable(struct clk_hw *hw)
+{
+	return samsung_s3c2410_pll_enable(hw, 5, true);
+}
+
+static void samsung_s3c2410_mpll_disable(struct clk_hw *hw)
+{
+	samsung_s3c2410_pll_enable(hw, 5, false);
+}
+
+static int samsung_s3c2410_upll_enable(struct clk_hw *hw)
+{
+	return samsung_s3c2410_pll_enable(hw, 7, true);
+}
+
+static void samsung_s3c2410_upll_disable(struct clk_hw *hw)
+{
+	samsung_s3c2410_pll_enable(hw, 7, false);
+}
+
+static const struct clk_ops samsung_s3c2410_mpll_clk_min_ops = {
+	.recalc_rate = samsung_s3c2410_pll_recalc_rate,
+	.enable = samsung_s3c2410_mpll_enable,
+	.disable = samsung_s3c2410_mpll_disable,
+};
+
+static const struct clk_ops samsung_s3c2410_upll_clk_min_ops = {
+	.recalc_rate = samsung_s3c2410_pll_recalc_rate,
+	.enable = samsung_s3c2410_upll_enable,
+	.disable = samsung_s3c2410_upll_disable,
+};
+
+static const struct clk_ops samsung_s3c2440_mpll_clk_min_ops = {
+	.recalc_rate = samsung_s3c2440_mpll_recalc_rate,
+	.enable = samsung_s3c2410_mpll_enable,
+	.disable = samsung_s3c2410_mpll_disable,
+};
+
+static const struct clk_ops samsung_s3c2410_mpll_clk_ops = {
+	.recalc_rate = samsung_s3c2410_pll_recalc_rate,
+	.enable = samsung_s3c2410_mpll_enable,
+	.disable = samsung_s3c2410_mpll_disable,
+	.round_rate = samsung_pll_round_rate,
+	.set_rate = samsung_s3c2410_pll_set_rate,
+};
+
+static const struct clk_ops samsung_s3c2410_upll_clk_ops = {
+	.recalc_rate = samsung_s3c2410_pll_recalc_rate,
+	.enable = samsung_s3c2410_upll_enable,
+	.disable = samsung_s3c2410_upll_disable,
+	.round_rate = samsung_pll_round_rate,
+	.set_rate = samsung_s3c2410_pll_set_rate,
+};
+
+static const struct clk_ops samsung_s3c2440_mpll_clk_ops = {
+	.recalc_rate = samsung_s3c2440_mpll_recalc_rate,
+	.enable = samsung_s3c2410_mpll_enable,
+	.disable = samsung_s3c2410_mpll_disable,
+	.round_rate = samsung_pll_round_rate,
+	.set_rate = samsung_s3c2410_pll_set_rate,
+};
+
+/*
  * PLL2550x Clock Type
  */
 
@@ -710,8 +947,206 @@ struct clk * __init samsung_clk_register_pll2550x(const char *name,
 	return clk;
 }
 
-static void __init _samsung_clk_register_pll(struct samsung_pll_clock *pll_clk,
-						void __iomem *base)
+/*
+ * PLL2550xx Clock Type
+ */
+
+/* Maximum lock time can be 270 * PDIV cycles */
+#define PLL2550XX_LOCK_FACTOR 270
+
+#define PLL2550XX_M_MASK		0x3FF
+#define PLL2550XX_P_MASK		0x3F
+#define PLL2550XX_S_MASK		0x7
+#define PLL2550XX_LOCK_STAT_MASK	0x1
+#define PLL2550XX_M_SHIFT		9
+#define PLL2550XX_P_SHIFT		3
+#define PLL2550XX_S_SHIFT		0
+#define PLL2550XX_LOCK_STAT_SHIFT	21
+
+static unsigned long samsung_pll2550xx_recalc_rate(struct clk_hw *hw,
+				unsigned long parent_rate)
+{
+	struct samsung_clk_pll *pll = to_clk_pll(hw);
+	u32 mdiv, pdiv, sdiv, pll_con;
+	u64 fvco = parent_rate;
+
+	pll_con = __raw_readl(pll->con_reg);
+	mdiv = (pll_con >> PLL2550XX_M_SHIFT) & PLL2550XX_M_MASK;
+	pdiv = (pll_con >> PLL2550XX_P_SHIFT) & PLL2550XX_P_MASK;
+	sdiv = (pll_con >> PLL2550XX_S_SHIFT) & PLL2550XX_S_MASK;
+
+	fvco *= mdiv;
+	do_div(fvco, (pdiv << sdiv));
+
+	return (unsigned long)fvco;
+}
+
+static inline bool samsung_pll2550xx_mp_change(u32 mdiv, u32 pdiv, u32 pll_con)
+{
+	u32 old_mdiv, old_pdiv;
+
+	old_mdiv = (pll_con >> PLL2550XX_M_SHIFT) & PLL2550XX_M_MASK;
+	old_pdiv = (pll_con >> PLL2550XX_P_SHIFT) & PLL2550XX_P_MASK;
+
+	return mdiv != old_mdiv || pdiv != old_pdiv;
+}
+
+static int samsung_pll2550xx_set_rate(struct clk_hw *hw, unsigned long drate,
+					unsigned long prate)
+{
+	struct samsung_clk_pll *pll = to_clk_pll(hw);
+	const struct samsung_pll_rate_table *rate;
+	u32 tmp;
+
+	/* Get required rate settings from table */
+	rate = samsung_get_pll_settings(pll, drate);
+	if (!rate) {
+		pr_err("%s: Invalid rate : %lu for pll clk %s\n", __func__,
+			drate, __clk_get_name(hw->clk));
+		return -EINVAL;
+	}
+
+	tmp = __raw_readl(pll->con_reg);
+
+	if (!(samsung_pll2550xx_mp_change(rate->mdiv, rate->pdiv, tmp))) {
+		/* If only s change, change just s value only*/
+		tmp &= ~(PLL2550XX_S_MASK << PLL2550XX_S_SHIFT);
+		tmp |= rate->sdiv << PLL2550XX_S_SHIFT;
+		__raw_writel(tmp, pll->con_reg);
+
+		return 0;
+	}
+
+	/* Set PLL lock time. */
+	__raw_writel(rate->pdiv * PLL2550XX_LOCK_FACTOR, pll->lock_reg);
+
+	/* Change PLL PMS values */
+	tmp &= ~((PLL2550XX_M_MASK << PLL2550XX_M_SHIFT) |
+			(PLL2550XX_P_MASK << PLL2550XX_P_SHIFT) |
+			(PLL2550XX_S_MASK << PLL2550XX_S_SHIFT));
+	tmp |= (rate->mdiv << PLL2550XX_M_SHIFT) |
+			(rate->pdiv << PLL2550XX_P_SHIFT) |
+			(rate->sdiv << PLL2550XX_S_SHIFT);
+	__raw_writel(tmp, pll->con_reg);
+
+	/* wait_lock_time */
+	do {
+		cpu_relax();
+		tmp = __raw_readl(pll->con_reg);
+	} while (!(tmp & (PLL2550XX_LOCK_STAT_MASK
+			<< PLL2550XX_LOCK_STAT_SHIFT)));
+
+	return 0;
+}
+
+static const struct clk_ops samsung_pll2550xx_clk_ops = {
+	.recalc_rate = samsung_pll2550xx_recalc_rate,
+	.round_rate = samsung_pll_round_rate,
+	.set_rate = samsung_pll2550xx_set_rate,
+};
+
+static const struct clk_ops samsung_pll2550xx_clk_min_ops = {
+	.recalc_rate = samsung_pll2550xx_recalc_rate,
+};
+
+/*
+ * PLL2650XX Clock Type
+ */
+
+/* Maximum lock time can be 3000 * PDIV cycles */
+#define PLL2650XX_LOCK_FACTOR 3000
+
+#define PLL2650XX_MDIV_SHIFT		9
+#define PLL2650XX_PDIV_SHIFT		3
+#define PLL2650XX_SDIV_SHIFT		0
+#define PLL2650XX_KDIV_SHIFT		0
+#define PLL2650XX_MDIV_MASK		0x1ff
+#define PLL2650XX_PDIV_MASK		0x3f
+#define PLL2650XX_SDIV_MASK		0x7
+#define PLL2650XX_KDIV_MASK		0xffff
+#define PLL2650XX_PLL_ENABLE_SHIFT	23
+#define PLL2650XX_PLL_LOCKTIME_SHIFT	21
+#define PLL2650XX_PLL_FOUTMASK_SHIFT	31
+
+static unsigned long samsung_pll2650xx_recalc_rate(struct clk_hw *hw,
+				unsigned long parent_rate)
+{
+	struct samsung_clk_pll *pll = to_clk_pll(hw);
+	u32 mdiv, pdiv, sdiv, pll_con0, pll_con2;
+	s16 kdiv;
+	u64 fvco = parent_rate;
+
+	pll_con0 = __raw_readl(pll->con_reg);
+	pll_con2 = __raw_readl(pll->con_reg + 8);
+	mdiv = (pll_con0 >> PLL2650XX_MDIV_SHIFT) & PLL2650XX_MDIV_MASK;
+	pdiv = (pll_con0 >> PLL2650XX_PDIV_SHIFT) & PLL2650XX_PDIV_MASK;
+	sdiv = (pll_con0 >> PLL2650XX_SDIV_SHIFT) & PLL2650XX_SDIV_MASK;
+	kdiv = (s16)(pll_con2 & PLL2650XX_KDIV_MASK);
+
+	fvco *= (mdiv << 16) + kdiv;
+	do_div(fvco, (pdiv << sdiv));
+	fvco >>= 16;
+
+	return (unsigned long)fvco;
+}
+
+static int samsung_pll2650xx_set_rate(struct clk_hw *hw, unsigned long drate,
+					unsigned long parent_rate)
+{
+	struct samsung_clk_pll *pll = to_clk_pll(hw);
+	u32 tmp, pll_con0, pll_con2;
+	const struct samsung_pll_rate_table *rate;
+
+	rate = samsung_get_pll_settings(pll, drate);
+	if (!rate) {
+		pr_err("%s: Invalid rate : %lu for pll clk %s\n", __func__,
+			drate, __clk_get_name(hw->clk));
+		return -EINVAL;
+	}
+
+	pll_con0 = __raw_readl(pll->con_reg);
+	pll_con2 = __raw_readl(pll->con_reg + 8);
+
+	 /* Change PLL PMS values */
+	pll_con0 &= ~(PLL2650XX_MDIV_MASK << PLL2650XX_MDIV_SHIFT |
+			PLL2650XX_PDIV_MASK << PLL2650XX_PDIV_SHIFT |
+			PLL2650XX_SDIV_MASK << PLL2650XX_SDIV_SHIFT);
+	pll_con0 |= rate->mdiv << PLL2650XX_MDIV_SHIFT;
+	pll_con0 |= rate->pdiv << PLL2650XX_PDIV_SHIFT;
+	pll_con0 |= rate->sdiv << PLL2650XX_SDIV_SHIFT;
+	pll_con0 |= 1 << PLL2650XX_PLL_ENABLE_SHIFT;
+	pll_con0 |= 1 << PLL2650XX_PLL_FOUTMASK_SHIFT;
+
+	pll_con2 &= ~(PLL2650XX_KDIV_MASK << PLL2650XX_KDIV_SHIFT);
+	pll_con2 |= ((~(rate->kdiv) + 1) & PLL2650XX_KDIV_MASK)
+			<< PLL2650XX_KDIV_SHIFT;
+
+	/* Set PLL lock time. */
+	__raw_writel(PLL2650XX_LOCK_FACTOR * rate->pdiv, pll->lock_reg);
+
+	__raw_writel(pll_con0, pll->con_reg);
+	__raw_writel(pll_con2, pll->con_reg + 8);
+
+	do {
+		tmp = __raw_readl(pll->con_reg);
+	} while (!(tmp & (0x1 << PLL2650XX_PLL_LOCKTIME_SHIFT)));
+
+	return 0;
+}
+
+static const struct clk_ops samsung_pll2650xx_clk_ops = {
+	.recalc_rate = samsung_pll2650xx_recalc_rate,
+	.set_rate = samsung_pll2650xx_set_rate,
+	.round_rate = samsung_pll_round_rate,
+};
+
+static const struct clk_ops samsung_pll2650xx_clk_min_ops = {
+	.recalc_rate = samsung_pll2650xx_recalc_rate,
+};
+
+static void __init _samsung_clk_register_pll(struct samsung_clk_provider *ctx,
+				struct samsung_pll_clock *pll_clk,
+				void __iomem *base)
 {
 	struct samsung_clk_pll *pll;
 	struct clk *clk;
@@ -746,6 +1181,12 @@ static void __init _samsung_clk_register_pll(struct samsung_pll_clock *pll_clk,
 	}
 
 	switch (pll_clk->type) {
+	case pll_2126:
+		init.ops = &samsung_pll2126_clk_ops;
+		break;
+	case pll_3000:
+		init.ops = &samsung_pll3000_clk_ops;
+		break;
 	/* clk_ops for 35xx and 2550 are similar */
 	case pll_35xx:
 	case pll_2550:
@@ -773,6 +1214,7 @@ static void __init _samsung_clk_register_pll(struct samsung_pll_clock *pll_clk,
 			init.ops = &samsung_pll36xx_clk_ops;
 		break;
 	case pll_6552:
+	case pll_6552_s3c2416:
 		init.ops = &samsung_pll6552_clk_ops;
 		break;
 	case pll_6553:
@@ -786,6 +1228,36 @@ static void __init _samsung_clk_register_pll(struct samsung_pll_clock *pll_clk,
 		else
 			init.ops = &samsung_pll46xx_clk_ops;
 		break;
+	case pll_s3c2410_mpll:
+		if (!pll->rate_table)
+			init.ops = &samsung_s3c2410_mpll_clk_min_ops;
+		else
+			init.ops = &samsung_s3c2410_mpll_clk_ops;
+		break;
+	case pll_s3c2410_upll:
+		if (!pll->rate_table)
+			init.ops = &samsung_s3c2410_upll_clk_min_ops;
+		else
+			init.ops = &samsung_s3c2410_upll_clk_ops;
+		break;
+	case pll_s3c2440_mpll:
+		if (!pll->rate_table)
+			init.ops = &samsung_s3c2440_mpll_clk_min_ops;
+		else
+			init.ops = &samsung_s3c2440_mpll_clk_ops;
+		break;
+	case pll_2550xx:
+		if (!pll->rate_table)
+			init.ops = &samsung_pll2550xx_clk_min_ops;
+		else
+			init.ops = &samsung_pll2550xx_clk_ops;
+		break;
+	case pll_2650xx:
+		if (!pll->rate_table)
+			init.ops = &samsung_pll2650xx_clk_min_ops;
+		else
+			init.ops = &samsung_pll2650xx_clk_ops;
+		break;
 	default:
 		pr_warn("%s: Unknown pll type for pll clk %s\n",
 			__func__, pll_clk->name);
@@ -804,7 +1276,7 @@ static void __init _samsung_clk_register_pll(struct samsung_pll_clock *pll_clk,
 		return;
 	}
 
-	samsung_clk_add_lookup(clk, pll_clk->id);
+	samsung_clk_add_lookup(ctx, clk, pll_clk->id);
 
 	if (!pll_clk->alias)
 		return;
@@ -815,11 +1287,12 @@ static void __init _samsung_clk_register_pll(struct samsung_pll_clock *pll_clk,
 			__func__, pll_clk->name, ret);
 }
 
-void __init samsung_clk_register_pll(struct samsung_pll_clock *pll_list,
-				unsigned int nr_pll, void __iomem *base)
+void __init samsung_clk_register_pll(struct samsung_clk_provider *ctx,
+			struct samsung_pll_clock *pll_list,
+			unsigned int nr_pll, void __iomem *base)
 {
 	int cnt;
 
 	for (cnt = 0; cnt < nr_pll; cnt++)
-		_samsung_clk_register_pll(&pll_list[cnt], base);
+		_samsung_clk_register_pll(ctx, &pll_list[cnt], base);
 }
diff --git a/drivers/clk/samsung/clk-pll.h b/drivers/clk/samsung/clk-pll.h
index 6c39030080f..c0ed4d41fd9 100644
--- a/drivers/clk/samsung/clk-pll.h
+++ b/drivers/clk/samsung/clk-pll.h
@@ -13,6 +13,8 @@
 #define __SAMSUNG_CLK_PLL_H
 
 enum samsung_pll_type {
+	pll_2126,
+	pll_3000,
 	pll_35xx,
 	pll_36xx,
 	pll_2550,
@@ -24,7 +26,13 @@ enum samsung_pll_type {
 	pll_4650,
 	pll_4650c,
 	pll_6552,
+	pll_6552_s3c2416,
 	pll_6553,
+	pll_s3c2410_mpll,
+	pll_s3c2410_upll,
+	pll_s3c2440_mpll,
+	pll_2550xx,
+	pll_2650xx,
 };
 
 #define PLL_35XX_RATE(_rate, _m, _p, _s)			\
diff --git a/drivers/clk/samsung/clk-s3c2410-dclk.c b/drivers/clk/samsung/clk-s3c2410-dclk.c
new file mode 100644
index 00000000000..0449cc0458e
--- /dev/null
+++ b/drivers/clk/samsung/clk-s3c2410-dclk.c
@@ -0,0 +1,440 @@
+/*
+ * Copyright (c) 2013 Heiko Stuebner <heiko@sntech.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.
+ *
+ * Common Clock Framework support for s3c24xx external clock output.
+ */
+
+#include <linux/platform_device.h>
+#include <linux/module.h>
+#include "clk.h"
+
+/* legacy access to misccr, until dt conversion is finished */
+#include <mach/hardware.h>
+#include <mach/regs-gpio.h>
+
+#define MUX_DCLK0	0
+#define MUX_DCLK1	1
+#define DIV_DCLK0	2
+#define DIV_DCLK1	3
+#define GATE_DCLK0	4
+#define GATE_DCLK1	5
+#define MUX_CLKOUT0	6
+#define MUX_CLKOUT1	7
+#define DCLK_MAX_CLKS	(MUX_CLKOUT1 + 1)
+
+enum supported_socs {
+	S3C2410,
+	S3C2412,
+	S3C2440,
+	S3C2443,
+};
+
+struct s3c24xx_dclk_drv_data {
+	const char **clkout0_parent_names;
+	int clkout0_num_parents;
+	const char **clkout1_parent_names;
+	int clkout1_num_parents;
+	const char **mux_parent_names;
+	int mux_num_parents;
+};
+
+/*
+ * Clock for output-parent selection in misccr
+ */
+
+struct s3c24xx_clkout {
+	struct clk_hw		hw;
+	u32			mask;
+	u8			shift;
+};
+
+#define to_s3c24xx_clkout(_hw) container_of(_hw, struct s3c24xx_clkout, hw)
+
+static u8 s3c24xx_clkout_get_parent(struct clk_hw *hw)
+{
+	struct s3c24xx_clkout *clkout = to_s3c24xx_clkout(hw);
+	int num_parents = __clk_get_num_parents(hw->clk);
+	u32 val;
+
+	val = readl_relaxed(S3C24XX_MISCCR) >> clkout->shift;
+	val >>= clkout->shift;
+	val &= clkout->mask;
+
+	if (val >= num_parents)
+		return -EINVAL;
+
+	return val;
+}
+
+static int s3c24xx_clkout_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct s3c24xx_clkout *clkout = to_s3c24xx_clkout(hw);
+	int ret = 0;
+
+	s3c2410_modify_misccr((clkout->mask << clkout->shift),
+			      (index << clkout->shift));
+
+	return ret;
+}
+
+const struct clk_ops s3c24xx_clkout_ops = {
+	.get_parent = s3c24xx_clkout_get_parent,
+	.set_parent = s3c24xx_clkout_set_parent,
+	.determine_rate = __clk_mux_determine_rate,
+};
+
+struct clk *s3c24xx_register_clkout(struct device *dev, const char *name,
+		const char **parent_names, u8 num_parents,
+		u8 shift, u32 mask)
+{
+	struct s3c24xx_clkout *clkout;
+	struct clk *clk;
+	struct clk_init_data init;
+
+	/* allocate the clkout */
+	clkout = kzalloc(sizeof(*clkout), GFP_KERNEL);
+	if (!clkout)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &s3c24xx_clkout_ops;
+	init.flags = CLK_IS_BASIC;
+	init.parent_names = parent_names;
+	init.num_parents = num_parents;
+
+	clkout->shift = shift;
+	clkout->mask = mask;
+	clkout->hw.init = &init;
+
+	clk = clk_register(dev, &clkout->hw);
+
+	return clk;
+}
+
+/*
+ * dclk and clkout init
+ */
+
+struct s3c24xx_dclk {
+	struct device *dev;
+	void __iomem *base;
+	struct clk_onecell_data clk_data;
+	struct notifier_block dclk0_div_change_nb;
+	struct notifier_block dclk1_div_change_nb;
+	spinlock_t dclk_lock;
+	unsigned long reg_save;
+};
+
+#define to_s3c24xx_dclk0(x) \
+		container_of(x, struct s3c24xx_dclk, dclk0_div_change_nb)
+
+#define to_s3c24xx_dclk1(x) \
+		container_of(x, struct s3c24xx_dclk, dclk1_div_change_nb)
+
+static const char *dclk_s3c2410_p[] = { "pclk", "uclk" };
+static const char *clkout0_s3c2410_p[] = { "mpll", "upll", "fclk", "hclk", "pclk",
+			     "gate_dclk0" };
+static const char *clkout1_s3c2410_p[] = { "mpll", "upll", "fclk", "hclk", "pclk",
+			     "gate_dclk1" };
+
+static const char *clkout0_s3c2412_p[] = { "mpll", "upll", "rtc_clkout",
+			     "hclk", "pclk", "gate_dclk0" };
+static const char *clkout1_s3c2412_p[] = { "xti", "upll", "fclk", "hclk", "pclk",
+			     "gate_dclk1" };
+
+static const char *clkout0_s3c2440_p[] = { "xti", "upll", "fclk", "hclk", "pclk",
+			     "gate_dclk0" };
+static const char *clkout1_s3c2440_p[] = { "mpll", "upll", "rtc_clkout",
+			     "hclk", "pclk", "gate_dclk1" };
+
+static const char *dclk_s3c2443_p[] = { "pclk", "epll" };
+static const char *clkout0_s3c2443_p[] = { "xti", "epll", "armclk", "hclk", "pclk",
+			     "gate_dclk0" };
+static const char *clkout1_s3c2443_p[] = { "dummy", "epll", "rtc_clkout",
+			     "hclk", "pclk", "gate_dclk1" };
+
+#define DCLKCON_DCLK_DIV_MASK		0xf
+#define DCLKCON_DCLK0_DIV_SHIFT		4
+#define DCLKCON_DCLK0_CMP_SHIFT		8
+#define DCLKCON_DCLK1_DIV_SHIFT		20
+#define DCLKCON_DCLK1_CMP_SHIFT		24
+
+static void s3c24xx_dclk_update_cmp(struct s3c24xx_dclk *s3c24xx_dclk,
+				    int div_shift, int cmp_shift)
+{
+	unsigned long flags = 0;
+	u32 dclk_con, div, cmp;
+
+	spin_lock_irqsave(&s3c24xx_dclk->dclk_lock, flags);
+
+	dclk_con = readl_relaxed(s3c24xx_dclk->base);
+
+	div = ((dclk_con >> div_shift) & DCLKCON_DCLK_DIV_MASK) + 1;
+	cmp = ((div + 1) / 2) - 1;
+
+	dclk_con &= ~(DCLKCON_DCLK_DIV_MASK << cmp_shift);
+	dclk_con |= (cmp << cmp_shift);
+
+	writel_relaxed(dclk_con, s3c24xx_dclk->base);
+
+	spin_unlock_irqrestore(&s3c24xx_dclk->dclk_lock, flags);
+}
+
+static int s3c24xx_dclk0_div_notify(struct notifier_block *nb,
+			       unsigned long event, void *data)
+{
+	struct s3c24xx_dclk *s3c24xx_dclk = to_s3c24xx_dclk0(nb);
+
+	if (event == POST_RATE_CHANGE) {
+		s3c24xx_dclk_update_cmp(s3c24xx_dclk,
+			DCLKCON_DCLK0_DIV_SHIFT, DCLKCON_DCLK0_CMP_SHIFT);
+	}
+
+	return NOTIFY_DONE;
+}
+
+static int s3c24xx_dclk1_div_notify(struct notifier_block *nb,
+			       unsigned long event, void *data)
+{
+	struct s3c24xx_dclk *s3c24xx_dclk = to_s3c24xx_dclk1(nb);
+
+	if (event == POST_RATE_CHANGE) {
+		s3c24xx_dclk_update_cmp(s3c24xx_dclk,
+			DCLKCON_DCLK1_DIV_SHIFT, DCLKCON_DCLK1_CMP_SHIFT);
+	}
+
+	return NOTIFY_DONE;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int s3c24xx_dclk_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct s3c24xx_dclk *s3c24xx_dclk = platform_get_drvdata(pdev);
+
+	s3c24xx_dclk->reg_save = readl_relaxed(s3c24xx_dclk->base);
+	return 0;
+}
+
+static int s3c24xx_dclk_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct s3c24xx_dclk *s3c24xx_dclk = platform_get_drvdata(pdev);
+
+	writel_relaxed(s3c24xx_dclk->reg_save, s3c24xx_dclk->base);
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(s3c24xx_dclk_pm_ops,
+			 s3c24xx_dclk_suspend, s3c24xx_dclk_resume);
+
+static int s3c24xx_dclk_probe(struct platform_device *pdev)
+{
+	struct s3c24xx_dclk *s3c24xx_dclk;
+	struct resource *mem;
+	struct clk **clk_table;
+	struct s3c24xx_dclk_drv_data *dclk_variant;
+	int ret, i;
+
+	s3c24xx_dclk = devm_kzalloc(&pdev->dev, sizeof(*s3c24xx_dclk),
+				    GFP_KERNEL);
+	if (!s3c24xx_dclk)
+		return -ENOMEM;
+
+	s3c24xx_dclk->dev = &pdev->dev;
+	platform_set_drvdata(pdev, s3c24xx_dclk);
+	spin_lock_init(&s3c24xx_dclk->dclk_lock);
+
+	clk_table = devm_kzalloc(&pdev->dev,
+				 sizeof(struct clk *) * DCLK_MAX_CLKS,
+				 GFP_KERNEL);
+	if (!clk_table)
+		return -ENOMEM;
+
+	s3c24xx_dclk->clk_data.clks = clk_table;
+	s3c24xx_dclk->clk_data.clk_num = DCLK_MAX_CLKS;
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	s3c24xx_dclk->base = devm_ioremap_resource(&pdev->dev, mem);
+	if (IS_ERR(s3c24xx_dclk->base))
+		return PTR_ERR(s3c24xx_dclk->base);
+
+	dclk_variant = (struct s3c24xx_dclk_drv_data *)
+				platform_get_device_id(pdev)->driver_data;
+
+
+	clk_table[MUX_DCLK0] = clk_register_mux(&pdev->dev, "mux_dclk0",
+				dclk_variant->mux_parent_names,
+				dclk_variant->mux_num_parents, 0,
+				s3c24xx_dclk->base, 1, 1, 0,
+				&s3c24xx_dclk->dclk_lock);
+	clk_table[MUX_DCLK1] = clk_register_mux(&pdev->dev, "mux_dclk1",
+				dclk_variant->mux_parent_names,
+				dclk_variant->mux_num_parents, 0,
+				s3c24xx_dclk->base, 17, 1, 0,
+				&s3c24xx_dclk->dclk_lock);
+
+	clk_table[DIV_DCLK0] = clk_register_divider(&pdev->dev, "div_dclk0",
+				"mux_dclk0", 0, s3c24xx_dclk->base,
+				4, 4, 0, &s3c24xx_dclk->dclk_lock);
+	clk_table[DIV_DCLK1] = clk_register_divider(&pdev->dev, "div_dclk1",
+				"mux_dclk1", 0, s3c24xx_dclk->base,
+				20, 4, 0, &s3c24xx_dclk->dclk_lock);
+
+	clk_table[GATE_DCLK0] = clk_register_gate(&pdev->dev, "gate_dclk0",
+				"div_dclk0", CLK_SET_RATE_PARENT,
+				s3c24xx_dclk->base, 0, 0,
+				&s3c24xx_dclk->dclk_lock);
+	clk_table[GATE_DCLK1] = clk_register_gate(&pdev->dev, "gate_dclk1",
+				"div_dclk1", CLK_SET_RATE_PARENT,
+				s3c24xx_dclk->base, 16, 0,
+				&s3c24xx_dclk->dclk_lock);
+
+	clk_table[MUX_CLKOUT0] = s3c24xx_register_clkout(&pdev->dev,
+				"clkout0", dclk_variant->clkout0_parent_names,
+				dclk_variant->clkout0_num_parents, 4, 7);
+	clk_table[MUX_CLKOUT1] = s3c24xx_register_clkout(&pdev->dev,
+				"clkout1", dclk_variant->clkout1_parent_names,
+				dclk_variant->clkout1_num_parents, 8, 7);
+
+	for (i = 0; i < DCLK_MAX_CLKS; i++)
+		if (IS_ERR(clk_table[i])) {
+			dev_err(&pdev->dev, "clock %d failed to register\n", i);
+			ret = PTR_ERR(clk_table[i]);
+			goto err_clk_register;
+		}
+
+	ret = clk_register_clkdev(clk_table[MUX_DCLK0], "dclk0", NULL);
+	if (!ret)
+		ret = clk_register_clkdev(clk_table[MUX_DCLK1], "dclk1", NULL);
+	if (!ret)
+		ret = clk_register_clkdev(clk_table[MUX_CLKOUT0],
+					  "clkout0", NULL);
+	if (!ret)
+		ret = clk_register_clkdev(clk_table[MUX_CLKOUT1],
+					  "clkout1", NULL);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to register aliases, %d\n", ret);
+		goto err_clk_register;
+	}
+
+	s3c24xx_dclk->dclk0_div_change_nb.notifier_call =
+						s3c24xx_dclk0_div_notify;
+
+	s3c24xx_dclk->dclk1_div_change_nb.notifier_call =
+						s3c24xx_dclk1_div_notify;
+
+	ret = clk_notifier_register(clk_table[DIV_DCLK0],
+				    &s3c24xx_dclk->dclk0_div_change_nb);
+	if (ret)
+		goto err_clk_register;
+
+	ret = clk_notifier_register(clk_table[DIV_DCLK1],
+				    &s3c24xx_dclk->dclk1_div_change_nb);
+	if (ret)
+		goto err_dclk_notify;
+
+	return 0;
+
+err_dclk_notify:
+	clk_notifier_unregister(clk_table[DIV_DCLK0],
+				&s3c24xx_dclk->dclk0_div_change_nb);
+err_clk_register:
+	for (i = 0; i < DCLK_MAX_CLKS; i++)
+		if (clk_table[i] && !IS_ERR(clk_table[i]))
+			clk_unregister(clk_table[i]);
+
+	return ret;
+}
+
+static int s3c24xx_dclk_remove(struct platform_device *pdev)
+{
+	struct s3c24xx_dclk *s3c24xx_dclk = platform_get_drvdata(pdev);
+	struct clk **clk_table = s3c24xx_dclk->clk_data.clks;
+	int i;
+
+	clk_notifier_unregister(clk_table[DIV_DCLK1],
+				&s3c24xx_dclk->dclk1_div_change_nb);
+	clk_notifier_unregister(clk_table[DIV_DCLK0],
+				&s3c24xx_dclk->dclk0_div_change_nb);
+
+	for (i = 0; i < DCLK_MAX_CLKS; i++)
+		clk_unregister(clk_table[i]);
+
+	return 0;
+}
+
+static struct s3c24xx_dclk_drv_data dclk_variants[] = {
+	[S3C2410] = {
+		.clkout0_parent_names = clkout0_s3c2410_p,
+		.clkout0_num_parents = ARRAY_SIZE(clkout0_s3c2410_p),
+		.clkout1_parent_names = clkout1_s3c2410_p,
+		.clkout1_num_parents = ARRAY_SIZE(clkout1_s3c2410_p),
+		.mux_parent_names = dclk_s3c2410_p,
+		.mux_num_parents = ARRAY_SIZE(dclk_s3c2410_p),
+	},
+	[S3C2412] = {
+		.clkout0_parent_names = clkout0_s3c2412_p,
+		.clkout0_num_parents = ARRAY_SIZE(clkout0_s3c2412_p),
+		.clkout1_parent_names = clkout1_s3c2412_p,
+		.clkout1_num_parents = ARRAY_SIZE(clkout1_s3c2412_p),
+		.mux_parent_names = dclk_s3c2410_p,
+		.mux_num_parents = ARRAY_SIZE(dclk_s3c2410_p),
+	},
+	[S3C2440] = {
+		.clkout0_parent_names = clkout0_s3c2440_p,
+		.clkout0_num_parents = ARRAY_SIZE(clkout0_s3c2440_p),
+		.clkout1_parent_names = clkout1_s3c2440_p,
+		.clkout1_num_parents = ARRAY_SIZE(clkout1_s3c2440_p),
+		.mux_parent_names = dclk_s3c2410_p,
+		.mux_num_parents = ARRAY_SIZE(dclk_s3c2410_p),
+	},
+	[S3C2443] = {
+		.clkout0_parent_names = clkout0_s3c2443_p,
+		.clkout0_num_parents = ARRAY_SIZE(clkout0_s3c2443_p),
+		.clkout1_parent_names = clkout1_s3c2443_p,
+		.clkout1_num_parents = ARRAY_SIZE(clkout1_s3c2443_p),
+		.mux_parent_names = dclk_s3c2443_p,
+		.mux_num_parents = ARRAY_SIZE(dclk_s3c2443_p),
+	},
+};
+
+static struct platform_device_id s3c24xx_dclk_driver_ids[] = {
+	{
+		.name		= "s3c2410-dclk",
+		.driver_data	= (kernel_ulong_t)&dclk_variants[S3C2410],
+	}, {
+		.name		= "s3c2412-dclk",
+		.driver_data	= (kernel_ulong_t)&dclk_variants[S3C2412],
+	}, {
+		.name		= "s3c2440-dclk",
+		.driver_data	= (kernel_ulong_t)&dclk_variants[S3C2440],
+	}, {
+		.name		= "s3c2443-dclk",
+		.driver_data	= (kernel_ulong_t)&dclk_variants[S3C2443],
+	},
+	{ }
+};
+
+MODULE_DEVICE_TABLE(platform, s3c24xx_dclk_driver_ids);
+
+static struct platform_driver s3c24xx_dclk_driver = {
+	.driver = {
+		.name		= "s3c24xx-dclk",
+		.owner		= THIS_MODULE,
+		.pm		= &s3c24xx_dclk_pm_ops,
+	},
+	.probe = s3c24xx_dclk_probe,
+	.remove = s3c24xx_dclk_remove,
+	.id_table = s3c24xx_dclk_driver_ids,
+};
+module_platform_driver(s3c24xx_dclk_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Heiko Stuebner <heiko@sntech.de>");
+MODULE_DESCRIPTION("Driver for the S3C24XX external clock outputs");
diff --git a/drivers/clk/samsung/clk-s3c2410.c b/drivers/clk/samsung/clk-s3c2410.c
new file mode 100644
index 00000000000..ba0716801db
--- /dev/null
+++ b/drivers/clk/samsung/clk-s3c2410.c
@@ -0,0 +1,482 @@
+/*
+ * Copyright (c) 2013 Heiko Stuebner <heiko@sntech.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.
+ *
+ * Common Clock Framework support for S3C2410 and following SoCs.
+ */
+
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/syscore_ops.h>
+
+#include <dt-bindings/clock/s3c2410.h>
+
+#include "clk.h"
+#include "clk-pll.h"
+
+#define LOCKTIME	0x00
+#define MPLLCON		0x04
+#define UPLLCON		0x08
+#define CLKCON		0x0c
+#define CLKSLOW		0x10
+#define CLKDIVN		0x14
+#define CAMDIVN		0x18
+
+/* the soc types */
+enum supported_socs {
+	S3C2410,
+	S3C2440,
+	S3C2442,
+};
+
+/* list of PLLs to be registered */
+enum s3c2410_plls {
+	mpll, upll,
+};
+
+static void __iomem *reg_base;
+
+#ifdef CONFIG_PM_SLEEP
+static struct samsung_clk_reg_dump *s3c2410_save;
+
+/*
+ * list of controller registers to be saved and restored during a
+ * suspend/resume cycle.
+ */
+static unsigned long s3c2410_clk_regs[] __initdata = {
+	LOCKTIME,
+	MPLLCON,
+	UPLLCON,
+	CLKCON,
+	CLKSLOW,
+	CLKDIVN,
+	CAMDIVN,
+};
+
+static int s3c2410_clk_suspend(void)
+{
+	samsung_clk_save(reg_base, s3c2410_save,
+				ARRAY_SIZE(s3c2410_clk_regs));
+
+	return 0;
+}
+
+static void s3c2410_clk_resume(void)
+{
+	samsung_clk_restore(reg_base, s3c2410_save,
+				ARRAY_SIZE(s3c2410_clk_regs));
+}
+
+static struct syscore_ops s3c2410_clk_syscore_ops = {
+	.suspend = s3c2410_clk_suspend,
+	.resume = s3c2410_clk_resume,
+};
+
+static void s3c2410_clk_sleep_init(void)
+{
+	s3c2410_save = samsung_clk_alloc_reg_dump(s3c2410_clk_regs,
+						ARRAY_SIZE(s3c2410_clk_regs));
+	if (!s3c2410_save) {
+		pr_warn("%s: failed to allocate sleep save data, no sleep support!\n",
+			__func__);
+		return;
+	}
+
+	register_syscore_ops(&s3c2410_clk_syscore_ops);
+	return;
+}
+#else
+static void s3c2410_clk_sleep_init(void) {}
+#endif
+
+PNAME(fclk_p) = { "mpll", "div_slow" };
+
+struct samsung_mux_clock s3c2410_common_muxes[] __initdata = {
+	MUX(FCLK, "fclk", fclk_p, CLKSLOW, 4, 1),
+};
+
+static struct clk_div_table divslow_d[] = {
+	{ .val = 0, .div = 1 },
+	{ .val = 1, .div = 2 },
+	{ .val = 2, .div = 4 },
+	{ .val = 3, .div = 6 },
+	{ .val = 4, .div = 8 },
+	{ .val = 5, .div = 10 },
+	{ .val = 6, .div = 12 },
+	{ .val = 7, .div = 14 },
+	{ /* sentinel */ },
+};
+
+struct samsung_div_clock s3c2410_common_dividers[] __initdata = {
+	DIV_T(0, "div_slow", "xti", CLKSLOW, 0, 3, divslow_d),
+	DIV(PCLK, "pclk", "hclk", CLKDIVN, 0, 1),
+};
+
+struct samsung_gate_clock s3c2410_common_gates[] __initdata = {
+	GATE(PCLK_SPI, "spi", "pclk", CLKCON, 18, 0, 0),
+	GATE(PCLK_I2S, "i2s", "pclk", CLKCON, 17, 0, 0),
+	GATE(PCLK_I2C, "i2c", "pclk", CLKCON, 16, 0, 0),
+	GATE(PCLK_ADC, "adc", "pclk", CLKCON, 15, 0, 0),
+	GATE(PCLK_RTC, "rtc", "pclk", CLKCON, 14, 0, 0),
+	GATE(PCLK_GPIO, "gpio", "pclk", CLKCON, 13, CLK_IGNORE_UNUSED, 0),
+	GATE(PCLK_UART2, "uart2", "pclk", CLKCON, 12, 0, 0),
+	GATE(PCLK_UART1, "uart1", "pclk", CLKCON, 11, 0, 0),
+	GATE(PCLK_UART0, "uart0", "pclk", CLKCON, 10, 0, 0),
+	GATE(PCLK_SDI, "sdi", "pclk", CLKCON, 9, 0, 0),
+	GATE(PCLK_PWM, "pwm", "pclk", CLKCON, 8, 0, 0),
+	GATE(HCLK_USBD, "usb-device", "hclk", CLKCON, 7, 0, 0),
+	GATE(HCLK_USBH, "usb-host", "hclk", CLKCON, 6, 0, 0),
+	GATE(HCLK_LCD, "lcd", "hclk", CLKCON, 5, 0, 0),
+	GATE(HCLK_NAND, "nand", "hclk", CLKCON, 4, 0, 0),
+};
+
+/* should be added _after_ the soc-specific clocks are created */
+struct samsung_clock_alias s3c2410_common_aliases[] __initdata = {
+	ALIAS(PCLK_I2C, "s3c2410-i2c.0", "i2c"),
+	ALIAS(PCLK_ADC, NULL, "adc"),
+	ALIAS(PCLK_RTC, NULL, "rtc"),
+	ALIAS(PCLK_PWM, NULL, "timers"),
+	ALIAS(HCLK_LCD, NULL, "lcd"),
+	ALIAS(HCLK_USBD, NULL, "usb-device"),
+	ALIAS(HCLK_USBH, NULL, "usb-host"),
+	ALIAS(UCLK, NULL, "usb-bus-host"),
+	ALIAS(UCLK, NULL, "usb-bus-gadget"),
+	ALIAS(ARMCLK, NULL, "armclk"),
+	ALIAS(UCLK, NULL, "uclk"),
+	ALIAS(HCLK, NULL, "hclk"),
+	ALIAS(MPLL, NULL, "mpll"),
+	ALIAS(FCLK, NULL, "fclk"),
+};
+
+/* S3C2410 specific clocks */
+
+static struct samsung_pll_rate_table pll_s3c2410_12mhz_tbl[] __initdata = {
+	/* sorted in descending order */
+	/* 2410A extras */
+	PLL_35XX_RATE(270000000, 127, 1, 1),
+	PLL_35XX_RATE(268000000, 126, 1, 1),
+	PLL_35XX_RATE(266000000, 125, 1, 1),
+	PLL_35XX_RATE(226000000, 105, 1, 1),
+	PLL_35XX_RATE(210000000, 132, 2, 1),
+	/* 2410 common */
+	PLL_35XX_RATE(203000000, 161, 3, 1),
+	PLL_35XX_RATE(192000000, 88, 1, 1),
+	PLL_35XX_RATE(186000000, 85, 1, 1),
+	PLL_35XX_RATE(180000000, 82, 1, 1),
+	PLL_35XX_RATE(170000000, 77, 1, 1),
+	PLL_35XX_RATE(158000000, 71, 1, 1),
+	PLL_35XX_RATE(152000000, 68, 1, 1),
+	PLL_35XX_RATE(147000000, 90, 2, 1),
+	PLL_35XX_RATE(135000000, 82, 2, 1),
+	PLL_35XX_RATE(124000000, 116, 1, 2),
+	PLL_35XX_RATE(118000000, 150, 2, 2),
+	PLL_35XX_RATE(113000000, 105, 1, 2),
+	PLL_35XX_RATE(101000000, 127, 2, 2),
+	PLL_35XX_RATE(90000000, 112, 2, 2),
+	PLL_35XX_RATE(85000000, 105, 2, 2),
+	PLL_35XX_RATE(79000000, 71, 1, 2),
+	PLL_35XX_RATE(68000000, 82, 2, 2),
+	PLL_35XX_RATE(56000000, 142, 2, 3),
+	PLL_35XX_RATE(48000000, 120, 2, 3),
+	PLL_35XX_RATE(51000000, 161, 3, 3),
+	PLL_35XX_RATE(45000000, 82, 1, 3),
+	PLL_35XX_RATE(34000000, 82, 2, 3),
+	{ /* sentinel */ },
+};
+
+static struct samsung_pll_clock s3c2410_plls[] __initdata = {
+	[mpll] = PLL(pll_s3c2410_mpll, MPLL, "mpll", "xti",
+						LOCKTIME, MPLLCON, NULL),
+	[upll] = PLL(pll_s3c2410_upll, UPLL, "upll", "xti",
+						LOCKTIME, UPLLCON, NULL),
+};
+
+struct samsung_div_clock s3c2410_dividers[] __initdata = {
+	DIV(HCLK, "hclk", "mpll", CLKDIVN, 1, 1),
+};
+
+struct samsung_fixed_factor_clock s3c2410_ffactor[] __initdata = {
+	/*
+	 * armclk is directly supplied by the fclk, without
+	 * switching possibility like on the s3c244x below.
+	 */
+	FFACTOR(ARMCLK, "armclk", "fclk", 1, 1, 0),
+
+	/* uclk is fed from the unmodified upll */
+	FFACTOR(UCLK, "uclk", "upll", 1, 1, 0),
+};
+
+struct samsung_clock_alias s3c2410_aliases[] __initdata = {
+	ALIAS(PCLK_UART0, "s3c2410-uart.0", "uart"),
+	ALIAS(PCLK_UART1, "s3c2410-uart.1", "uart"),
+	ALIAS(PCLK_UART2, "s3c2410-uart.2", "uart"),
+	ALIAS(PCLK_UART0, "s3c2410-uart.0", "clk_uart_baud0"),
+	ALIAS(PCLK_UART1, "s3c2410-uart.1", "clk_uart_baud0"),
+	ALIAS(PCLK_UART2, "s3c2410-uart.2", "clk_uart_baud0"),
+	ALIAS(UCLK, NULL, "clk_uart_baud1"),
+};
+
+/* S3C244x specific clocks */
+
+static struct samsung_pll_rate_table pll_s3c244x_12mhz_tbl[] __initdata = {
+	/* sorted in descending order */
+	PLL_35XX_RATE(400000000, 0x5c, 1, 1),
+	PLL_35XX_RATE(390000000, 0x7a, 2, 1),
+	PLL_35XX_RATE(380000000, 0x57, 1, 1),
+	PLL_35XX_RATE(370000000, 0xb1, 4, 1),
+	PLL_35XX_RATE(360000000, 0x70, 2, 1),
+	PLL_35XX_RATE(350000000, 0xa7, 4, 1),
+	PLL_35XX_RATE(340000000, 0x4d, 1, 1),
+	PLL_35XX_RATE(330000000, 0x66, 2, 1),
+	PLL_35XX_RATE(320000000, 0x98, 4, 1),
+	PLL_35XX_RATE(310000000, 0x93, 4, 1),
+	PLL_35XX_RATE(300000000, 0x75, 3, 1),
+	PLL_35XX_RATE(240000000, 0x70, 1, 2),
+	PLL_35XX_RATE(230000000, 0x6b, 1, 2),
+	PLL_35XX_RATE(220000000, 0x66, 1, 2),
+	PLL_35XX_RATE(210000000, 0x84, 2, 2),
+	PLL_35XX_RATE(200000000, 0x5c, 1, 2),
+	PLL_35XX_RATE(190000000, 0x57, 1, 2),
+	PLL_35XX_RATE(180000000, 0x70, 2, 2),
+	PLL_35XX_RATE(170000000, 0x4d, 1, 2),
+	PLL_35XX_RATE(160000000, 0x98, 4, 2),
+	PLL_35XX_RATE(150000000, 0x75, 3, 2),
+	PLL_35XX_RATE(120000000, 0x70, 1, 3),
+	PLL_35XX_RATE(110000000, 0x66, 1, 3),
+	PLL_35XX_RATE(100000000, 0x5c, 1, 3),
+	PLL_35XX_RATE(90000000, 0x70, 2, 3),
+	PLL_35XX_RATE(80000000, 0x98, 4, 3),
+	PLL_35XX_RATE(75000000, 0x75, 3, 3),
+	{ /* sentinel */ },
+};
+
+static struct samsung_pll_clock s3c244x_common_plls[] __initdata = {
+	[mpll] = PLL(pll_s3c2440_mpll, MPLL, "mpll", "xti",
+						LOCKTIME, MPLLCON, NULL),
+	[upll] = PLL(pll_s3c2410_upll, UPLL, "upll", "xti",
+						LOCKTIME, UPLLCON, NULL),
+};
+
+PNAME(hclk_p) = { "fclk", "div_hclk_2", "div_hclk_4", "div_hclk_3" };
+PNAME(armclk_p) = { "fclk", "hclk" };
+
+struct samsung_mux_clock s3c244x_common_muxes[] __initdata = {
+	MUX(HCLK, "hclk", hclk_p, CLKDIVN, 1, 2),
+	MUX(ARMCLK, "armclk", armclk_p, CAMDIVN, 12, 1),
+};
+
+struct samsung_fixed_factor_clock s3c244x_common_ffactor[] __initdata = {
+	FFACTOR(0, "div_hclk_2", "fclk", 1, 2, 0),
+	FFACTOR(0, "ff_cam", "div_cam", 2, 1, CLK_SET_RATE_PARENT),
+};
+
+static struct clk_div_table div_hclk_4_d[] = {
+	{ .val = 0, .div = 4 },
+	{ .val = 1, .div = 8 },
+	{ /* sentinel */ },
+};
+
+static struct clk_div_table div_hclk_3_d[] = {
+	{ .val = 0, .div = 3 },
+	{ .val = 1, .div = 6 },
+	{ /* sentinel */ },
+};
+
+struct samsung_div_clock s3c244x_common_dividers[] __initdata = {
+	DIV(UCLK, "uclk", "upll", CLKDIVN, 3, 1),
+	DIV(0, "div_hclk", "fclk", CLKDIVN, 1, 1),
+	DIV_T(0, "div_hclk_4", "fclk", CAMDIVN, 9, 1, div_hclk_4_d),
+	DIV_T(0, "div_hclk_3", "fclk", CAMDIVN, 8, 1, div_hclk_3_d),
+	DIV(0, "div_cam", "upll", CAMDIVN, 0, 3),
+};
+
+struct samsung_gate_clock s3c244x_common_gates[] __initdata = {
+	GATE(HCLK_CAM, "cam", "hclk", CLKCON, 19, 0, 0),
+};
+
+struct samsung_clock_alias s3c244x_common_aliases[] __initdata = {
+	ALIAS(PCLK_UART0, "s3c2440-uart.0", "uart"),
+	ALIAS(PCLK_UART1, "s3c2440-uart.1", "uart"),
+	ALIAS(PCLK_UART2, "s3c2440-uart.2", "uart"),
+	ALIAS(PCLK_UART0, "s3c2440-uart.0", "clk_uart_baud2"),
+	ALIAS(PCLK_UART1, "s3c2440-uart.1", "clk_uart_baud2"),
+	ALIAS(PCLK_UART2, "s3c2440-uart.2", "clk_uart_baud2"),
+	ALIAS(HCLK_CAM, NULL, "camif"),
+	ALIAS(CAMIF, NULL, "camif-upll"),
+};
+
+/* S3C2440 specific clocks */
+
+PNAME(s3c2440_camif_p) = { "upll", "ff_cam" };
+
+struct samsung_mux_clock s3c2440_muxes[] __initdata = {
+	MUX(CAMIF, "camif", s3c2440_camif_p, CAMDIVN, 4, 1),
+};
+
+struct samsung_gate_clock s3c2440_gates[] __initdata = {
+	GATE(PCLK_AC97, "ac97", "pclk", CLKCON, 20, 0, 0),
+};
+
+/* S3C2442 specific clocks */
+
+struct samsung_fixed_factor_clock s3c2442_ffactor[] __initdata = {
+	FFACTOR(0, "upll_3", "upll", 1, 3, 0),
+};
+
+PNAME(s3c2442_camif_p) = { "upll", "ff_cam", "upll", "upll_3" };
+
+struct samsung_mux_clock s3c2442_muxes[] __initdata = {
+	MUX(CAMIF, "camif", s3c2442_camif_p, CAMDIVN, 4, 2),
+};
+
+/*
+ * fixed rate clocks generated outside the soc
+ * Only necessary until the devicetree-move is complete
+ */
+#define XTI	1
+struct samsung_fixed_rate_clock s3c2410_common_frate_clks[] __initdata = {
+	FRATE(XTI, "xti", NULL, CLK_IS_ROOT, 0),
+};
+
+static void __init s3c2410_common_clk_register_fixed_ext(
+		struct samsung_clk_provider *ctx,
+		unsigned long xti_f)
+{
+	struct samsung_clock_alias xti_alias = ALIAS(XTI, NULL, "xtal");
+
+	s3c2410_common_frate_clks[0].fixed_rate = xti_f;
+	samsung_clk_register_fixed_rate(ctx, s3c2410_common_frate_clks,
+				ARRAY_SIZE(s3c2410_common_frate_clks));
+
+	samsung_clk_register_alias(ctx, &xti_alias, 1);
+}
+
+void __init s3c2410_common_clk_init(struct device_node *np, unsigned long xti_f,
+				    int current_soc,
+				    void __iomem *base)
+{
+	struct samsung_clk_provider *ctx;
+	reg_base = base;
+
+	if (np) {
+		reg_base = of_iomap(np, 0);
+		if (!reg_base)
+			panic("%s: failed to map registers\n", __func__);
+	}
+
+	ctx = samsung_clk_init(np, reg_base, NR_CLKS);
+	if (!ctx)
+		panic("%s: unable to allocate context.\n", __func__);
+
+	/* Register external clocks only in non-dt cases */
+	if (!np)
+		s3c2410_common_clk_register_fixed_ext(ctx, xti_f);
+
+	if (current_soc == 2410) {
+		if (_get_rate("xti") == 12 * MHZ) {
+			s3c2410_plls[mpll].rate_table = pll_s3c2410_12mhz_tbl;
+			s3c2410_plls[upll].rate_table = pll_s3c2410_12mhz_tbl;
+		}
+
+		/* Register PLLs. */
+		samsung_clk_register_pll(ctx, s3c2410_plls,
+				ARRAY_SIZE(s3c2410_plls), reg_base);
+
+	} else { /* S3C2440, S3C2442 */
+		if (_get_rate("xti") == 12 * MHZ) {
+			/*
+			 * plls follow different calculation schemes, with the
+			 * upll following the same scheme as the s3c2410 plls
+			 */
+			s3c244x_common_plls[mpll].rate_table =
+							pll_s3c244x_12mhz_tbl;
+			s3c244x_common_plls[upll].rate_table =
+							pll_s3c2410_12mhz_tbl;
+		}
+
+		/* Register PLLs. */
+		samsung_clk_register_pll(ctx, s3c244x_common_plls,
+				ARRAY_SIZE(s3c244x_common_plls), reg_base);
+	}
+
+	/* Register common internal clocks. */
+	samsung_clk_register_mux(ctx, s3c2410_common_muxes,
+			ARRAY_SIZE(s3c2410_common_muxes));
+	samsung_clk_register_div(ctx, s3c2410_common_dividers,
+			ARRAY_SIZE(s3c2410_common_dividers));
+	samsung_clk_register_gate(ctx, s3c2410_common_gates,
+		ARRAY_SIZE(s3c2410_common_gates));
+
+	if (current_soc == S3C2440 || current_soc == S3C2442) {
+		samsung_clk_register_div(ctx, s3c244x_common_dividers,
+				ARRAY_SIZE(s3c244x_common_dividers));
+		samsung_clk_register_gate(ctx, s3c244x_common_gates,
+				ARRAY_SIZE(s3c244x_common_gates));
+		samsung_clk_register_mux(ctx, s3c244x_common_muxes,
+				ARRAY_SIZE(s3c244x_common_muxes));
+		samsung_clk_register_fixed_factor(ctx, s3c244x_common_ffactor,
+				ARRAY_SIZE(s3c244x_common_ffactor));
+	}
+
+	/* Register SoC-specific clocks. */
+	switch (current_soc) {
+	case S3C2410:
+		samsung_clk_register_div(ctx, s3c2410_dividers,
+				ARRAY_SIZE(s3c2410_dividers));
+		samsung_clk_register_fixed_factor(ctx, s3c2410_ffactor,
+				ARRAY_SIZE(s3c2410_ffactor));
+		samsung_clk_register_alias(ctx, s3c2410_aliases,
+			ARRAY_SIZE(s3c2410_common_aliases));
+		break;
+	case S3C2440:
+		samsung_clk_register_mux(ctx, s3c2440_muxes,
+				ARRAY_SIZE(s3c2440_muxes));
+		samsung_clk_register_gate(ctx, s3c2440_gates,
+				ARRAY_SIZE(s3c2440_gates));
+		break;
+	case S3C2442:
+		samsung_clk_register_mux(ctx, s3c2442_muxes,
+				ARRAY_SIZE(s3c2442_muxes));
+		samsung_clk_register_fixed_factor(ctx, s3c2442_ffactor,
+				ARRAY_SIZE(s3c2442_ffactor));
+		break;
+	}
+
+	/*
+	 * Register common aliases at the end, as some of the aliased clocks
+	 * are SoC specific.
+	 */
+	samsung_clk_register_alias(ctx, s3c2410_common_aliases,
+		ARRAY_SIZE(s3c2410_common_aliases));
+
+	if (current_soc == S3C2440 || current_soc == S3C2442) {
+		samsung_clk_register_alias(ctx, s3c244x_common_aliases,
+			ARRAY_SIZE(s3c244x_common_aliases));
+	}
+
+	s3c2410_clk_sleep_init();
+}
+
+static void __init s3c2410_clk_init(struct device_node *np)
+{
+	s3c2410_common_clk_init(np, 0, S3C2410, 0);
+}
+CLK_OF_DECLARE(s3c2410_clk, "samsung,s3c2410-clock", s3c2410_clk_init);
+
+static void __init s3c2440_clk_init(struct device_node *np)
+{
+	s3c2410_common_clk_init(np, 0, S3C2440, 0);
+}
+CLK_OF_DECLARE(s3c2440_clk, "samsung,s3c2440-clock", s3c2440_clk_init);
+
+static void __init s3c2442_clk_init(struct device_node *np)
+{
+	s3c2410_common_clk_init(np, 0, S3C2442, 0);
+}
+CLK_OF_DECLARE(s3c2442_clk, "samsung,s3c2442-clock", s3c2442_clk_init);
diff --git a/drivers/clk/samsung/clk-s3c2412.c b/drivers/clk/samsung/clk-s3c2412.c
new file mode 100644
index 00000000000..23e4313f625
--- /dev/null
+++ b/drivers/clk/samsung/clk-s3c2412.c
@@ -0,0 +1,274 @@
+/*
+ * Copyright (c) 2013 Heiko Stuebner <heiko@sntech.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.
+ *
+ * Common Clock Framework support for S3C2412 and S3C2413.
+ */
+
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/syscore_ops.h>
+
+#include <dt-bindings/clock/s3c2412.h>
+
+#include "clk.h"
+#include "clk-pll.h"
+
+#define LOCKTIME	0x00
+#define MPLLCON		0x04
+#define UPLLCON		0x08
+#define CLKCON		0x0c
+#define CLKDIVN		0x14
+#define CLKSRC		0x1c
+
+/* list of PLLs to be registered */
+enum s3c2412_plls {
+	mpll, upll,
+};
+
+static void __iomem *reg_base;
+
+#ifdef CONFIG_PM_SLEEP
+static struct samsung_clk_reg_dump *s3c2412_save;
+
+/*
+ * list of controller registers to be saved and restored during a
+ * suspend/resume cycle.
+ */
+static unsigned long s3c2412_clk_regs[] __initdata = {
+	LOCKTIME,
+	MPLLCON,
+	UPLLCON,
+	CLKCON,
+	CLKDIVN,
+	CLKSRC,
+};
+
+static int s3c2412_clk_suspend(void)
+{
+	samsung_clk_save(reg_base, s3c2412_save,
+				ARRAY_SIZE(s3c2412_clk_regs));
+
+	return 0;
+}
+
+static void s3c2412_clk_resume(void)
+{
+	samsung_clk_restore(reg_base, s3c2412_save,
+				ARRAY_SIZE(s3c2412_clk_regs));
+}
+
+static struct syscore_ops s3c2412_clk_syscore_ops = {
+	.suspend = s3c2412_clk_suspend,
+	.resume = s3c2412_clk_resume,
+};
+
+static void s3c2412_clk_sleep_init(void)
+{
+	s3c2412_save = samsung_clk_alloc_reg_dump(s3c2412_clk_regs,
+						ARRAY_SIZE(s3c2412_clk_regs));
+	if (!s3c2412_save) {
+		pr_warn("%s: failed to allocate sleep save data, no sleep support!\n",
+			__func__);
+		return;
+	}
+
+	register_syscore_ops(&s3c2412_clk_syscore_ops);
+	return;
+}
+#else
+static void s3c2412_clk_sleep_init(void) {}
+#endif
+
+static struct clk_div_table divxti_d[] = {
+	{ .val = 0, .div = 1 },
+	{ .val = 1, .div = 2 },
+	{ .val = 2, .div = 4 },
+	{ .val = 3, .div = 6 },
+	{ .val = 4, .div = 8 },
+	{ .val = 5, .div = 10 },
+	{ .val = 6, .div = 12 },
+	{ .val = 7, .div = 14 },
+	{ /* sentinel */ },
+};
+
+struct samsung_div_clock s3c2412_dividers[] __initdata = {
+	DIV_T(0, "div_xti", "xti", CLKSRC, 0, 3, divxti_d),
+	DIV(0, "div_cam", "mux_cam", CLKDIVN, 16, 4),
+	DIV(0, "div_i2s", "mux_i2s", CLKDIVN, 12, 4),
+	DIV(0, "div_uart", "mux_uart", CLKDIVN, 8, 4),
+	DIV(0, "div_usb", "mux_usb", CLKDIVN, 6, 1),
+	DIV(0, "div_hclk_half", "hclk", CLKDIVN, 5, 1),
+	DIV(ARMDIV, "armdiv", "msysclk", CLKDIVN, 3, 1),
+	DIV(PCLK, "pclk", "hclk", CLKDIVN, 2, 1),
+	DIV(HCLK, "hclk", "armdiv", CLKDIVN, 0, 2),
+};
+
+struct samsung_fixed_factor_clock s3c2412_ffactor[] __initdata = {
+	FFACTOR(0, "ff_hclk", "hclk", 2, 1, CLK_SET_RATE_PARENT),
+};
+
+/*
+ * The first two use the OM[4] setting, which is not readable from
+ * software, so assume it is set to xti.
+ */
+PNAME(erefclk_p) = { "xti", "xti", "xti", "ext" };
+PNAME(urefclk_p) = { "xti", "xti", "xti", "ext" };
+
+PNAME(camclk_p) = { "usysclk", "hclk" };
+PNAME(usbclk_p) = { "usysclk", "hclk" };
+PNAME(i2sclk_p) = { "erefclk", "mpll" };
+PNAME(uartclk_p) = { "erefclk", "mpll" };
+PNAME(usysclk_p) = { "urefclk", "upll" };
+PNAME(msysclk_p) = { "mdivclk", "mpll" };
+PNAME(mdivclk_p) = { "xti", "div_xti" };
+PNAME(armclk_p) = { "armdiv", "hclk" };
+
+struct samsung_mux_clock s3c2412_muxes[] __initdata = {
+	MUX(0, "erefclk", erefclk_p, CLKSRC, 14, 2),
+	MUX(0, "urefclk", urefclk_p, CLKSRC, 12, 2),
+	MUX(0, "mux_cam", camclk_p, CLKSRC, 11, 1),
+	MUX(0, "mux_usb", usbclk_p, CLKSRC, 10, 1),
+	MUX(0, "mux_i2s", i2sclk_p, CLKSRC, 9, 1),
+	MUX(0, "mux_uart", uartclk_p, CLKSRC, 8, 1),
+	MUX(USYSCLK, "usysclk", usysclk_p, CLKSRC, 5, 1),
+	MUX(MSYSCLK, "msysclk", msysclk_p, CLKSRC, 4, 1),
+	MUX(MDIVCLK, "mdivclk", mdivclk_p, CLKSRC, 3, 1),
+	MUX(ARMCLK, "armclk", armclk_p, CLKDIVN, 4, 1),
+};
+
+static struct samsung_pll_clock s3c2412_plls[] __initdata = {
+	[mpll] = PLL(pll_s3c2440_mpll, MPLL, "mpll", "xti",
+						LOCKTIME, MPLLCON, NULL),
+	[upll] = PLL(pll_s3c2410_upll, UPLL, "upll", "urefclk",
+						LOCKTIME, UPLLCON, NULL),
+};
+
+struct samsung_gate_clock s3c2412_gates[] __initdata = {
+	GATE(PCLK_WDT, "wdt", "pclk", CLKCON, 28, 0, 0),
+	GATE(PCLK_SPI, "spi", "pclk", CLKCON, 27, 0, 0),
+	GATE(PCLK_I2S, "i2s", "pclk", CLKCON, 26, 0, 0),
+	GATE(PCLK_I2C, "i2c", "pclk", CLKCON, 25, 0, 0),
+	GATE(PCLK_ADC, "adc", "pclk", CLKCON, 24, 0, 0),
+	GATE(PCLK_RTC, "rtc", "pclk", CLKCON, 23, 0, 0),
+	GATE(PCLK_GPIO, "gpio", "pclk", CLKCON, 22, CLK_IGNORE_UNUSED, 0),
+	GATE(PCLK_UART2, "uart2", "pclk", CLKCON, 21, 0, 0),
+	GATE(PCLK_UART1, "uart1", "pclk", CLKCON, 20, 0, 0),
+	GATE(PCLK_UART0, "uart0", "pclk", CLKCON, 19, 0, 0),
+	GATE(PCLK_SDI, "sdi", "pclk", CLKCON, 18, 0, 0),
+	GATE(PCLK_PWM, "pwm", "pclk", CLKCON, 17, 0, 0),
+	GATE(PCLK_USBD, "usb-device", "pclk", CLKCON, 16, 0, 0),
+	GATE(SCLK_CAM, "sclk_cam", "div_cam", CLKCON, 15, 0, 0),
+	GATE(SCLK_UART, "sclk_uart", "div_uart", CLKCON, 14, 0, 0),
+	GATE(SCLK_I2S, "sclk_i2s", "div_i2s", CLKCON, 13, 0, 0),
+	GATE(SCLK_USBH, "sclk_usbh", "div_usb", CLKCON, 12, 0, 0),
+	GATE(SCLK_USBD, "sclk_usbd", "div_usb", CLKCON, 11, 0, 0),
+	GATE(HCLK_HALF, "hclk_half", "div_hclk_half", CLKCON, 10, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_X2, "hclkx2", "ff_hclk", CLKCON, 9, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_SDRAM, "sdram", "hclk", CLKCON, 8, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_USBH, "usb-host", "hclk", CLKCON, 6, 0, 0),
+	GATE(HCLK_LCD, "lcd", "hclk", CLKCON, 5, 0, 0),
+	GATE(HCLK_NAND, "nand", "hclk", CLKCON, 4, 0, 0),
+	GATE(HCLK_DMA3, "dma3", "hclk", CLKCON, 3, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_DMA2, "dma2", "hclk", CLKCON, 2, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_DMA1, "dma1", "hclk", CLKCON, 1, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_DMA0, "dma0", "hclk", CLKCON, 0, CLK_IGNORE_UNUSED, 0),
+};
+
+struct samsung_clock_alias s3c2412_aliases[] __initdata = {
+	ALIAS(PCLK_UART0, "s3c2412-uart.0", "uart"),
+	ALIAS(PCLK_UART1, "s3c2412-uart.1", "uart"),
+	ALIAS(PCLK_UART2, "s3c2412-uart.2", "uart"),
+	ALIAS(PCLK_UART0, "s3c2412-uart.0", "clk_uart_baud2"),
+	ALIAS(PCLK_UART1, "s3c2412-uart.1", "clk_uart_baud2"),
+	ALIAS(PCLK_UART2, "s3c2412-uart.2", "clk_uart_baud2"),
+	ALIAS(SCLK_UART, NULL, "clk_uart_baud3"),
+	ALIAS(PCLK_I2C, "s3c2410-i2c.0", "i2c"),
+	ALIAS(PCLK_ADC, NULL, "adc"),
+	ALIAS(PCLK_RTC, NULL, "rtc"),
+	ALIAS(PCLK_PWM, NULL, "timers"),
+	ALIAS(HCLK_LCD, NULL, "lcd"),
+	ALIAS(PCLK_USBD, NULL, "usb-device"),
+	ALIAS(SCLK_USBD, NULL, "usb-bus-gadget"),
+	ALIAS(HCLK_USBH, NULL, "usb-host"),
+	ALIAS(SCLK_USBH, NULL, "usb-bus-host"),
+	ALIAS(ARMCLK, NULL, "armclk"),
+	ALIAS(HCLK, NULL, "hclk"),
+	ALIAS(MPLL, NULL, "mpll"),
+	ALIAS(MSYSCLK, NULL, "fclk"),
+};
+
+/*
+ * fixed rate clocks generated outside the soc
+ * Only necessary until the devicetree-move is complete
+ */
+#define XTI	1
+struct samsung_fixed_rate_clock s3c2412_common_frate_clks[] __initdata = {
+	FRATE(XTI, "xti", NULL, CLK_IS_ROOT, 0),
+	FRATE(0, "ext", NULL, CLK_IS_ROOT, 0),
+};
+
+static void __init s3c2412_common_clk_register_fixed_ext(
+		struct samsung_clk_provider *ctx,
+		unsigned long xti_f, unsigned long ext_f)
+{
+	/* xtal alias is necessary for the current cpufreq driver */
+	struct samsung_clock_alias xti_alias = ALIAS(XTI, NULL, "xtal");
+
+	s3c2412_common_frate_clks[0].fixed_rate = xti_f;
+	s3c2412_common_frate_clks[1].fixed_rate = ext_f;
+	samsung_clk_register_fixed_rate(ctx, s3c2412_common_frate_clks,
+				ARRAY_SIZE(s3c2412_common_frate_clks));
+
+	samsung_clk_register_alias(ctx, &xti_alias, 1);
+}
+
+void __init s3c2412_common_clk_init(struct device_node *np, unsigned long xti_f,
+				    unsigned long ext_f, void __iomem *base)
+{
+	struct samsung_clk_provider *ctx;
+	reg_base = base;
+
+	if (np) {
+		reg_base = of_iomap(np, 0);
+		if (!reg_base)
+			panic("%s: failed to map registers\n", __func__);
+	}
+
+	ctx = samsung_clk_init(np, reg_base, NR_CLKS);
+	if (!ctx)
+		panic("%s: unable to allocate context.\n", __func__);
+
+	/* Register external clocks only in non-dt cases */
+	if (!np)
+		s3c2412_common_clk_register_fixed_ext(ctx, xti_f, ext_f);
+
+	/* Register PLLs. */
+	samsung_clk_register_pll(ctx, s3c2412_plls, ARRAY_SIZE(s3c2412_plls),
+				 reg_base);
+
+	/* Register common internal clocks. */
+	samsung_clk_register_mux(ctx, s3c2412_muxes, ARRAY_SIZE(s3c2412_muxes));
+	samsung_clk_register_div(ctx, s3c2412_dividers,
+					  ARRAY_SIZE(s3c2412_dividers));
+	samsung_clk_register_gate(ctx, s3c2412_gates,
+					ARRAY_SIZE(s3c2412_gates));
+	samsung_clk_register_fixed_factor(ctx, s3c2412_ffactor,
+					  ARRAY_SIZE(s3c2412_ffactor));
+	samsung_clk_register_alias(ctx, s3c2412_aliases,
+				   ARRAY_SIZE(s3c2412_aliases));
+
+	s3c2412_clk_sleep_init();
+}
+
+static void __init s3c2412_clk_init(struct device_node *np)
+{
+	s3c2412_common_clk_init(np, 0, 0, 0);
+}
+CLK_OF_DECLARE(s3c2412_clk, "samsung,s3c2412-clock", s3c2412_clk_init);
diff --git a/drivers/clk/samsung/clk-s3c2443.c b/drivers/clk/samsung/clk-s3c2443.c
new file mode 100644
index 00000000000..c4bbdabebaa
--- /dev/null
+++ b/drivers/clk/samsung/clk-s3c2443.c
@@ -0,0 +1,466 @@
+/*
+ * Copyright (c) 2013 Heiko Stuebner <heiko@sntech.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.
+ *
+ * Common Clock Framework support for S3C2443 and following SoCs.
+ */
+
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/syscore_ops.h>
+
+#include <dt-bindings/clock/s3c2443.h>
+
+#include "clk.h"
+#include "clk-pll.h"
+
+/* S3C2416 clock controller register offsets */
+#define LOCKCON0	0x00
+#define LOCKCON1	0x04
+#define MPLLCON		0x10
+#define EPLLCON		0x18
+#define EPLLCON_K	0x1C
+#define CLKSRC		0x20
+#define CLKDIV0		0x24
+#define CLKDIV1		0x28
+#define CLKDIV2		0x2C
+#define HCLKCON		0x30
+#define PCLKCON		0x34
+#define SCLKCON		0x38
+
+/* the soc types */
+enum supported_socs {
+	S3C2416,
+	S3C2443,
+	S3C2450,
+};
+
+/* list of PLLs to be registered */
+enum s3c2443_plls {
+	mpll, epll,
+};
+
+static void __iomem *reg_base;
+
+#ifdef CONFIG_PM_SLEEP
+static struct samsung_clk_reg_dump *s3c2443_save;
+
+/*
+ * list of controller registers to be saved and restored during a
+ * suspend/resume cycle.
+ */
+static unsigned long s3c2443_clk_regs[] __initdata = {
+	LOCKCON0,
+	LOCKCON1,
+	MPLLCON,
+	EPLLCON,
+	EPLLCON_K,
+	CLKSRC,
+	CLKDIV0,
+	CLKDIV1,
+	CLKDIV2,
+	PCLKCON,
+	HCLKCON,
+	SCLKCON,
+};
+
+static int s3c2443_clk_suspend(void)
+{
+	samsung_clk_save(reg_base, s3c2443_save,
+				ARRAY_SIZE(s3c2443_clk_regs));
+
+	return 0;
+}
+
+static void s3c2443_clk_resume(void)
+{
+	samsung_clk_restore(reg_base, s3c2443_save,
+				ARRAY_SIZE(s3c2443_clk_regs));
+}
+
+static struct syscore_ops s3c2443_clk_syscore_ops = {
+	.suspend = s3c2443_clk_suspend,
+	.resume = s3c2443_clk_resume,
+};
+
+static void s3c2443_clk_sleep_init(void)
+{
+	s3c2443_save = samsung_clk_alloc_reg_dump(s3c2443_clk_regs,
+						ARRAY_SIZE(s3c2443_clk_regs));
+	if (!s3c2443_save) {
+		pr_warn("%s: failed to allocate sleep save data, no sleep support!\n",
+			__func__);
+		return;
+	}
+
+	register_syscore_ops(&s3c2443_clk_syscore_ops);
+	return;
+}
+#else
+static void s3c2443_clk_sleep_init(void) {}
+#endif
+
+PNAME(epllref_p) = { "mpllref", "mpllref", "xti", "ext" };
+PNAME(esysclk_p) = { "epllref", "epll" };
+PNAME(mpllref_p) = { "xti", "mdivclk" };
+PNAME(msysclk_p) = { "mpllref", "mpll" };
+PNAME(armclk_p) = { "armdiv" , "hclk" };
+PNAME(i2s0_p) = { "div_i2s0", "ext_i2s", "epllref", "epllref" };
+
+struct samsung_mux_clock s3c2443_common_muxes[] __initdata = {
+	MUX(0, "epllref", epllref_p, CLKSRC, 7, 2),
+	MUX(ESYSCLK, "esysclk", esysclk_p, CLKSRC, 6, 1),
+	MUX(0, "mpllref", mpllref_p, CLKSRC, 3, 1),
+	MUX_A(MSYSCLK, "msysclk", msysclk_p, CLKSRC, 4, 1, "msysclk"),
+	MUX_A(ARMCLK, "armclk", armclk_p, CLKDIV0, 13, 1, "armclk"),
+	MUX(0, "mux_i2s0", i2s0_p, CLKSRC, 14, 2),
+};
+
+static struct clk_div_table hclk_d[] = {
+	{ .val = 0, .div = 1 },
+	{ .val = 1, .div = 2 },
+	{ .val = 3, .div = 4 },
+	{ /* sentinel */ },
+};
+
+static struct clk_div_table mdivclk_d[] = {
+	{ .val = 0, .div = 1 },
+	{ .val = 1, .div = 3 },
+	{ .val = 2, .div = 5 },
+	{ .val = 3, .div = 7 },
+	{ .val = 4, .div = 9 },
+	{ .val = 5, .div = 11 },
+	{ .val = 6, .div = 13 },
+	{ .val = 7, .div = 15 },
+	{ /* sentinel */ },
+};
+
+struct samsung_div_clock s3c2443_common_dividers[] __initdata = {
+	DIV_T(0, "mdivclk", "xti", CLKDIV0, 6, 3, mdivclk_d),
+	DIV(0, "prediv", "msysclk", CLKDIV0, 4, 2),
+	DIV_T(HCLK, "hclk", "prediv", CLKDIV0, 0, 2, hclk_d),
+	DIV(PCLK, "pclk", "hclk", CLKDIV0, 2, 1),
+	DIV(0, "div_hsspi0_epll", "esysclk", CLKDIV1, 24, 2),
+	DIV(0, "div_fimd", "esysclk", CLKDIV1, 16, 8),
+	DIV(0, "div_i2s0", "esysclk", CLKDIV1, 12, 4),
+	DIV(0, "div_uart", "esysclk", CLKDIV1, 8, 4),
+	DIV(0, "div_hsmmc1", "esysclk", CLKDIV1, 6, 2),
+	DIV(0, "div_usbhost", "esysclk", CLKDIV1, 4, 2),
+};
+
+struct samsung_gate_clock s3c2443_common_gates[] __initdata = {
+	GATE(SCLK_HSMMC_EXT, "sclk_hsmmcext", "ext", SCLKCON, 13, 0, 0),
+	GATE(SCLK_HSMMC1, "sclk_hsmmc1", "div_hsmmc1", SCLKCON, 12, 0, 0),
+	GATE(SCLK_FIMD, "sclk_fimd", "div_fimd", SCLKCON, 10, 0, 0),
+	GATE(SCLK_I2S0, "sclk_i2s0", "mux_i2s0", SCLKCON, 9, 0, 0),
+	GATE(SCLK_UART, "sclk_uart", "div_uart", SCLKCON, 8, 0, 0),
+	GATE(SCLK_USBH, "sclk_usbhost", "div_usbhost", SCLKCON, 1, 0, 0),
+	GATE(HCLK_DRAM, "dram", "hclk", HCLKCON, 19, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_SSMC, "ssmc", "hclk", HCLKCON, 18, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_HSMMC1, "hsmmc1", "hclk", HCLKCON, 16, 0, 0),
+	GATE(HCLK_USBD, "usb-device", "hclk", HCLKCON, 12, 0, 0),
+	GATE(HCLK_USBH, "usb-host", "hclk", HCLKCON, 11, 0, 0),
+	GATE(HCLK_LCD, "lcd", "hclk", HCLKCON, 9, 0, 0),
+	GATE(HCLK_DMA5, "dma5", "hclk", HCLKCON, 5, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_DMA4, "dma4", "hclk", HCLKCON, 4, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_DMA3, "dma3", "hclk", HCLKCON, 3, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_DMA2, "dma2", "hclk", HCLKCON, 2, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_DMA1, "dma1", "hclk", HCLKCON, 1, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_DMA0, "dma0", "hclk", HCLKCON, 0, CLK_IGNORE_UNUSED, 0),
+	GATE(PCLK_GPIO, "gpio", "pclk", PCLKCON, 13, CLK_IGNORE_UNUSED, 0),
+	GATE(PCLK_RTC, "rtc", "pclk", PCLKCON, 12, 0, 0),
+	GATE(PCLK_WDT, "wdt", "pclk", PCLKCON, 11, 0, 0),
+	GATE(PCLK_PWM, "pwm", "pclk", PCLKCON, 10, 0, 0),
+	GATE(PCLK_I2S0, "i2s0", "pclk", PCLKCON, 9, 0, 0),
+	GATE(PCLK_AC97, "ac97", "pclk", PCLKCON, 8, 0, 0),
+	GATE(PCLK_ADC, "adc", "pclk", PCLKCON, 7, 0, 0),
+	GATE(PCLK_SPI0, "spi0", "pclk", PCLKCON, 6, 0, 0),
+	GATE(PCLK_I2C0, "i2c0", "pclk", PCLKCON, 4, 0, 0),
+	GATE(PCLK_UART3, "uart3", "pclk", PCLKCON, 3, 0, 0),
+	GATE(PCLK_UART2, "uart2", "pclk", PCLKCON, 2, 0, 0),
+	GATE(PCLK_UART1, "uart1", "pclk", PCLKCON, 1, 0, 0),
+	GATE(PCLK_UART0, "uart0", "pclk", PCLKCON, 0, 0, 0),
+};
+
+struct samsung_clock_alias s3c2443_common_aliases[] __initdata = {
+	ALIAS(HCLK, NULL, "hclk"),
+	ALIAS(HCLK_SSMC, NULL, "nand"),
+	ALIAS(PCLK_UART0, "s3c2440-uart.0", "uart"),
+	ALIAS(PCLK_UART1, "s3c2440-uart.1", "uart"),
+	ALIAS(PCLK_UART2, "s3c2440-uart.2", "uart"),
+	ALIAS(PCLK_UART3, "s3c2440-uart.3", "uart"),
+	ALIAS(PCLK_UART0, "s3c2440-uart.0", "clk_uart_baud2"),
+	ALIAS(PCLK_UART1, "s3c2440-uart.1", "clk_uart_baud2"),
+	ALIAS(PCLK_UART2, "s3c2440-uart.2", "clk_uart_baud2"),
+	ALIAS(PCLK_UART3, "s3c2440-uart.3", "clk_uart_baud2"),
+	ALIAS(SCLK_UART, NULL, "clk_uart_baud3"),
+	ALIAS(PCLK_PWM, NULL, "timers"),
+	ALIAS(PCLK_RTC, NULL, "rtc"),
+	ALIAS(PCLK_WDT, NULL, "watchdog"),
+	ALIAS(PCLK_ADC, NULL, "adc"),
+	ALIAS(PCLK_I2C0, "s3c2410-i2c.0", "i2c"),
+	ALIAS(HCLK_USBD, NULL, "usb-device"),
+	ALIAS(HCLK_USBH, NULL, "usb-host"),
+	ALIAS(SCLK_USBH, NULL, "usb-bus-host"),
+	ALIAS(PCLK_SPI0, "s3c2443-spi.0", "spi"),
+	ALIAS(PCLK_SPI0, "s3c2443-spi.0", "spi_busclk0"),
+	ALIAS(HCLK_HSMMC1, "s3c-sdhci.1", "hsmmc"),
+	ALIAS(HCLK_HSMMC1, "s3c-sdhci.1", "mmc_busclk.0"),
+	ALIAS(PCLK_I2S0, "samsung-i2s.0", "iis"),
+	ALIAS(SCLK_I2S0, NULL, "i2s-if"),
+	ALIAS(HCLK_LCD, NULL, "lcd"),
+	ALIAS(SCLK_FIMD, NULL, "sclk_fimd"),
+};
+
+/* S3C2416 specific clocks */
+
+static struct samsung_pll_clock s3c2416_pll_clks[] __initdata = {
+	[mpll] = PLL(pll_6552_s3c2416, 0, "mpll", "mpllref",
+						LOCKCON0, MPLLCON, NULL),
+	[epll] = PLL(pll_6553, 0, "epll", "epllref",
+						LOCKCON1, EPLLCON, NULL),
+};
+
+PNAME(s3c2416_hsmmc0_p) = { "sclk_hsmmc0", "sclk_hsmmcext" };
+PNAME(s3c2416_hsmmc1_p) = { "sclk_hsmmc1", "sclk_hsmmcext" };
+PNAME(s3c2416_hsspi0_p) = { "hsspi0_epll", "hsspi0_mpll" };
+
+static struct clk_div_table armdiv_s3c2416_d[] = {
+	{ .val = 0, .div = 1 },
+	{ .val = 1, .div = 2 },
+	{ .val = 2, .div = 3 },
+	{ .val = 3, .div = 4 },
+	{ .val = 5, .div = 6 },
+	{ .val = 7, .div = 8 },
+	{ /* sentinel */ },
+};
+
+struct samsung_div_clock s3c2416_dividers[] __initdata = {
+	DIV_T(ARMDIV, "armdiv", "msysclk", CLKDIV0, 9, 3, armdiv_s3c2416_d),
+	DIV(0, "div_hsspi0_mpll", "msysclk", CLKDIV2, 0, 4),
+	DIV(0, "div_hsmmc0", "esysclk", CLKDIV2, 6, 2),
+};
+
+struct samsung_mux_clock s3c2416_muxes[] __initdata = {
+	MUX(MUX_HSMMC0, "mux_hsmmc0", s3c2416_hsmmc0_p, CLKSRC, 16, 1),
+	MUX(MUX_HSMMC1, "mux_hsmmc1", s3c2416_hsmmc1_p, CLKSRC, 17, 1),
+	MUX(MUX_HSSPI0, "mux_hsspi0", s3c2416_hsspi0_p, CLKSRC, 18, 1),
+};
+
+struct samsung_gate_clock s3c2416_gates[] __initdata = {
+	GATE(0, "hsspi0_mpll", "div_hsspi0_mpll", SCLKCON, 19, 0, 0),
+	GATE(0, "hsspi0_epll", "div_hsspi0_epll", SCLKCON, 14, 0, 0),
+	GATE(0, "sclk_hsmmc0", "div_hsmmc0", SCLKCON, 6, 0, 0),
+	GATE(HCLK_2D, "2d", "hclk", HCLKCON, 20, 0, 0),
+	GATE(HCLK_HSMMC0, "hsmmc0", "hclk", HCLKCON, 15, 0, 0),
+	GATE(HCLK_IROM, "irom", "hclk", HCLKCON, 13, CLK_IGNORE_UNUSED, 0),
+	GATE(PCLK_PCM, "pcm", "pclk", PCLKCON, 19, 0, 0),
+};
+
+struct samsung_clock_alias s3c2416_aliases[] __initdata = {
+	ALIAS(HCLK_HSMMC0, "s3c-sdhci.0", "hsmmc"),
+	ALIAS(HCLK_HSMMC0, "s3c-sdhci.0", "mmc_busclk.0"),
+	ALIAS(MUX_HSMMC0, "s3c-sdhci.0", "mmc_busclk.2"),
+	ALIAS(MUX_HSMMC1, "s3c-sdhci.1", "mmc_busclk.2"),
+	ALIAS(MUX_HSSPI0, "s3c2443-spi.0", "spi_busclk2"),
+	ALIAS(ARMDIV, NULL, "armdiv"),
+};
+
+/* S3C2443 specific clocks */
+
+static struct samsung_pll_clock s3c2443_pll_clks[] __initdata = {
+	[mpll] = PLL(pll_3000, 0, "mpll", "mpllref",
+						LOCKCON0, MPLLCON, NULL),
+	[epll] = PLL(pll_2126, 0, "epll", "epllref",
+						LOCKCON1, EPLLCON, NULL),
+};
+
+static struct clk_div_table armdiv_s3c2443_d[] = {
+	{ .val = 0, .div = 1 },
+	{ .val = 8, .div = 2 },
+	{ .val = 2, .div = 3 },
+	{ .val = 9, .div = 4 },
+	{ .val = 10, .div = 6 },
+	{ .val = 11, .div = 8 },
+	{ .val = 13, .div = 12 },
+	{ .val = 15, .div = 16 },
+	{ /* sentinel */ },
+};
+
+struct samsung_div_clock s3c2443_dividers[] __initdata = {
+	DIV_T(ARMDIV, "armdiv", "msysclk", CLKDIV0, 9, 4, armdiv_s3c2443_d),
+	DIV(0, "div_cam", "esysclk", CLKDIV1, 26, 4),
+};
+
+struct samsung_gate_clock s3c2443_gates[] __initdata = {
+	GATE(SCLK_HSSPI0, "sclk_hsspi0", "div_hsspi0_epll", SCLKCON, 14, 0, 0),
+	GATE(SCLK_CAM, "sclk_cam", "div_cam", SCLKCON, 11, 0, 0),
+	GATE(HCLK_CFC, "cfc", "hclk", HCLKCON, 17, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_CAM, "cam", "hclk", HCLKCON, 8, 0, 0),
+	GATE(PCLK_SPI1, "spi1", "pclk", PCLKCON, 15, 0, 0),
+	GATE(PCLK_SDI, "sdi", "pclk", PCLKCON, 5, 0, 0),
+};
+
+struct samsung_clock_alias s3c2443_aliases[] __initdata = {
+	ALIAS(SCLK_HSSPI0, "s3c2443-spi.0", "spi_busclk2"),
+	ALIAS(SCLK_HSMMC1, "s3c-sdhci.1", "mmc_busclk.2"),
+	ALIAS(SCLK_CAM, NULL, "camif-upll"),
+	ALIAS(PCLK_SPI1, "s3c2410-spi.0", "spi"),
+	ALIAS(PCLK_SDI, NULL, "sdi"),
+	ALIAS(HCLK_CFC, NULL, "cfc"),
+	ALIAS(ARMDIV, NULL, "armdiv"),
+};
+
+/* S3C2450 specific clocks */
+
+PNAME(s3c2450_cam_p) = { "div_cam", "hclk" };
+PNAME(s3c2450_hsspi1_p) = { "hsspi1_epll", "hsspi1_mpll" };
+PNAME(i2s1_p) = { "div_i2s1", "ext_i2s", "epllref", "epllref" };
+
+struct samsung_div_clock s3c2450_dividers[] __initdata = {
+	DIV(0, "div_cam", "esysclk", CLKDIV1, 26, 4),
+	DIV(0, "div_hsspi1_epll", "esysclk", CLKDIV2, 24, 2),
+	DIV(0, "div_hsspi1_mpll", "msysclk", CLKDIV2, 16, 4),
+	DIV(0, "div_i2s1", "esysclk", CLKDIV2, 12, 4),
+};
+
+struct samsung_mux_clock s3c2450_muxes[] __initdata = {
+	MUX(0, "mux_cam", s3c2450_cam_p, CLKSRC, 20, 1),
+	MUX(MUX_HSSPI1, "mux_hsspi1", s3c2450_hsspi1_p, CLKSRC, 19, 1),
+	MUX(0, "mux_i2s1", i2s1_p, CLKSRC, 12, 2),
+};
+
+struct samsung_gate_clock s3c2450_gates[] __initdata = {
+	GATE(SCLK_I2S1, "sclk_i2s1", "div_i2s1", SCLKCON, 5, 0, 0),
+	GATE(HCLK_CFC, "cfc", "hclk", HCLKCON, 17, 0, 0),
+	GATE(HCLK_CAM, "cam", "hclk", HCLKCON, 8, 0, 0),
+	GATE(HCLK_DMA7, "dma7", "hclk", HCLKCON, 7, CLK_IGNORE_UNUSED, 0),
+	GATE(HCLK_DMA6, "dma6", "hclk", HCLKCON, 6, CLK_IGNORE_UNUSED, 0),
+	GATE(PCLK_I2S1, "i2s1", "pclk", PCLKCON, 17, 0, 0),
+	GATE(PCLK_I2C1, "i2c1", "pclk", PCLKCON, 16, 0, 0),
+	GATE(PCLK_SPI1, "spi1", "pclk", PCLKCON, 14, 0, 0),
+};
+
+struct samsung_clock_alias s3c2450_aliases[] __initdata = {
+	ALIAS(PCLK_SPI1, "s3c2443-spi.1", "spi"),
+	ALIAS(PCLK_SPI1, "s3c2443-spi.1", "spi_busclk0"),
+	ALIAS(MUX_HSSPI1, "s3c2443-spi.1", "spi_busclk2"),
+	ALIAS(PCLK_I2C1, "s3c2410-i2c.1", "i2c"),
+};
+
+/*
+ * fixed rate clocks generated outside the soc
+ * Only necessary until the devicetree-move is complete
+ */
+struct samsung_fixed_rate_clock s3c2443_common_frate_clks[] __initdata = {
+	FRATE(0, "xti", NULL, CLK_IS_ROOT, 0),
+	FRATE(0, "ext", NULL, CLK_IS_ROOT, 0),
+	FRATE(0, "ext_i2s", NULL, CLK_IS_ROOT, 0),
+	FRATE(0, "ext_uart", NULL, CLK_IS_ROOT, 0),
+};
+
+static void __init s3c2443_common_clk_register_fixed_ext(
+		struct samsung_clk_provider *ctx, unsigned long xti_f)
+{
+	s3c2443_common_frate_clks[0].fixed_rate = xti_f;
+	samsung_clk_register_fixed_rate(ctx, s3c2443_common_frate_clks,
+				ARRAY_SIZE(s3c2443_common_frate_clks));
+}
+
+void __init s3c2443_common_clk_init(struct device_node *np, unsigned long xti_f,
+				    int current_soc,
+				    void __iomem *base)
+{
+	struct samsung_clk_provider *ctx;
+	reg_base = base;
+
+	if (np) {
+		reg_base = of_iomap(np, 0);
+		if (!reg_base)
+			panic("%s: failed to map registers\n", __func__);
+	}
+
+	ctx = samsung_clk_init(np, reg_base, NR_CLKS);
+	if (!ctx)
+		panic("%s: unable to allocate context.\n", __func__);
+
+	/* Register external clocks only in non-dt cases */
+	if (!np)
+		s3c2443_common_clk_register_fixed_ext(ctx, xti_f);
+
+	/* Register PLLs. */
+	if (current_soc == S3C2416 || current_soc == S3C2450)
+		samsung_clk_register_pll(ctx, s3c2416_pll_clks,
+				ARRAY_SIZE(s3c2416_pll_clks), reg_base);
+	else
+		samsung_clk_register_pll(ctx, s3c2443_pll_clks,
+				ARRAY_SIZE(s3c2443_pll_clks), reg_base);
+
+	/* Register common internal clocks. */
+	samsung_clk_register_mux(ctx, s3c2443_common_muxes,
+			ARRAY_SIZE(s3c2443_common_muxes));
+	samsung_clk_register_div(ctx, s3c2443_common_dividers,
+			ARRAY_SIZE(s3c2443_common_dividers));
+	samsung_clk_register_gate(ctx, s3c2443_common_gates,
+		ARRAY_SIZE(s3c2443_common_gates));
+	samsung_clk_register_alias(ctx, s3c2443_common_aliases,
+		ARRAY_SIZE(s3c2443_common_aliases));
+
+	/* Register SoC-specific clocks. */
+	switch (current_soc) {
+	case S3C2450:
+		samsung_clk_register_div(ctx, s3c2450_dividers,
+				ARRAY_SIZE(s3c2450_dividers));
+		samsung_clk_register_mux(ctx, s3c2450_muxes,
+				ARRAY_SIZE(s3c2450_muxes));
+		samsung_clk_register_gate(ctx, s3c2450_gates,
+				ARRAY_SIZE(s3c2450_gates));
+		samsung_clk_register_alias(ctx, s3c2450_aliases,
+				ARRAY_SIZE(s3c2450_aliases));
+		/* fall through, as s3c2450 extends the s3c2416 clocks */
+	case S3C2416:
+		samsung_clk_register_div(ctx, s3c2416_dividers,
+				ARRAY_SIZE(s3c2416_dividers));
+		samsung_clk_register_mux(ctx, s3c2416_muxes,
+				ARRAY_SIZE(s3c2416_muxes));
+		samsung_clk_register_gate(ctx, s3c2416_gates,
+				ARRAY_SIZE(s3c2416_gates));
+		samsung_clk_register_alias(ctx, s3c2416_aliases,
+				ARRAY_SIZE(s3c2416_aliases));
+		break;
+	case S3C2443:
+		samsung_clk_register_div(ctx, s3c2443_dividers,
+				ARRAY_SIZE(s3c2443_dividers));
+		samsung_clk_register_gate(ctx, s3c2443_gates,
+				ARRAY_SIZE(s3c2443_gates));
+		samsung_clk_register_alias(ctx, s3c2443_aliases,
+				ARRAY_SIZE(s3c2443_aliases));
+		break;
+	}
+
+	s3c2443_clk_sleep_init();
+}
+
+static void __init s3c2416_clk_init(struct device_node *np)
+{
+	s3c2443_common_clk_init(np, 0, S3C2416, 0);
+}
+CLK_OF_DECLARE(s3c2416_clk, "samsung,s3c2416-clock", s3c2416_clk_init);
+
+static void __init s3c2443_clk_init(struct device_node *np)
+{
+	s3c2443_common_clk_init(np, 0, S3C2443, 0);
+}
+CLK_OF_DECLARE(s3c2443_clk, "samsung,s3c2443-clock", s3c2443_clk_init);
+
+static void __init s3c2450_clk_init(struct device_node *np)
+{
+	s3c2443_common_clk_init(np, 0, S3C2450, 0);
+}
+CLK_OF_DECLARE(s3c2450_clk, "samsung,s3c2450-clock", s3c2450_clk_init);
diff --git a/drivers/clk/samsung/clk-s3c64xx.c b/drivers/clk/samsung/clk-s3c64xx.c
index 8bda658137a..efa16ee592c 100644
--- a/drivers/clk/samsung/clk-s3c64xx.c
+++ b/drivers/clk/samsung/clk-s3c64xx.c
@@ -442,12 +442,14 @@ static struct samsung_clock_alias s3c6410_clock_aliases[] = {
 	ALIAS(MEM0_SROM, NULL, "srom"),
 };
 
-static void __init s3c64xx_clk_register_fixed_ext(unsigned long fin_pll_f,
-							unsigned long xusbxti_f)
+static void __init s3c64xx_clk_register_fixed_ext(
+				struct samsung_clk_provider *ctx,
+				unsigned long fin_pll_f,
+				unsigned long xusbxti_f)
 {
 	s3c64xx_fixed_rate_ext_clks[0].fixed_rate = fin_pll_f;
 	s3c64xx_fixed_rate_ext_clks[1].fixed_rate = xusbxti_f;
-	samsung_clk_register_fixed_rate(s3c64xx_fixed_rate_ext_clks,
+	samsung_clk_register_fixed_rate(ctx, s3c64xx_fixed_rate_ext_clks,
 				ARRAY_SIZE(s3c64xx_fixed_rate_ext_clks));
 }
 
@@ -456,6 +458,8 @@ void __init s3c64xx_clk_init(struct device_node *np, unsigned long xtal_f,
 			     unsigned long xusbxti_f, bool s3c6400,
 			     void __iomem *base)
 {
+	struct samsung_clk_provider *ctx;
+
 	reg_base = base;
 	is_s3c6400 = s3c6400;
 
@@ -465,48 +469,50 @@ void __init s3c64xx_clk_init(struct device_node *np, unsigned long xtal_f,
 			panic("%s: failed to map registers\n", __func__);
 	}
 
-	samsung_clk_init(np, reg_base, NR_CLKS);
+	ctx = samsung_clk_init(np, reg_base, NR_CLKS);
+	if (!ctx)
+		panic("%s: unable to allocate context.\n", __func__);
 
 	/* Register external clocks. */
 	if (!np)
-		s3c64xx_clk_register_fixed_ext(xtal_f, xusbxti_f);
+		s3c64xx_clk_register_fixed_ext(ctx, xtal_f, xusbxti_f);
 
 	/* Register PLLs. */
-	samsung_clk_register_pll(s3c64xx_pll_clks,
+	samsung_clk_register_pll(ctx, s3c64xx_pll_clks,
 				ARRAY_SIZE(s3c64xx_pll_clks), reg_base);
 
 	/* Register common internal clocks. */
-	samsung_clk_register_fixed_rate(s3c64xx_fixed_rate_clks,
+	samsung_clk_register_fixed_rate(ctx, s3c64xx_fixed_rate_clks,
 					ARRAY_SIZE(s3c64xx_fixed_rate_clks));
-	samsung_clk_register_mux(s3c64xx_mux_clks,
+	samsung_clk_register_mux(ctx, s3c64xx_mux_clks,
 					ARRAY_SIZE(s3c64xx_mux_clks));
-	samsung_clk_register_div(s3c64xx_div_clks,
+	samsung_clk_register_div(ctx, s3c64xx_div_clks,
 					ARRAY_SIZE(s3c64xx_div_clks));
-	samsung_clk_register_gate(s3c64xx_gate_clks,
+	samsung_clk_register_gate(ctx, s3c64xx_gate_clks,
 					ARRAY_SIZE(s3c64xx_gate_clks));
 
 	/* Register SoC-specific clocks. */
 	if (is_s3c6400) {
-		samsung_clk_register_mux(s3c6400_mux_clks,
+		samsung_clk_register_mux(ctx, s3c6400_mux_clks,
 					ARRAY_SIZE(s3c6400_mux_clks));
-		samsung_clk_register_div(s3c6400_div_clks,
+		samsung_clk_register_div(ctx, s3c6400_div_clks,
 					ARRAY_SIZE(s3c6400_div_clks));
-		samsung_clk_register_gate(s3c6400_gate_clks,
+		samsung_clk_register_gate(ctx, s3c6400_gate_clks,
 					ARRAY_SIZE(s3c6400_gate_clks));
-		samsung_clk_register_alias(s3c6400_clock_aliases,
+		samsung_clk_register_alias(ctx, s3c6400_clock_aliases,
 					ARRAY_SIZE(s3c6400_clock_aliases));
 	} else {
-		samsung_clk_register_mux(s3c6410_mux_clks,
+		samsung_clk_register_mux(ctx, s3c6410_mux_clks,
 					ARRAY_SIZE(s3c6410_mux_clks));
-		samsung_clk_register_div(s3c6410_div_clks,
+		samsung_clk_register_div(ctx, s3c6410_div_clks,
 					ARRAY_SIZE(s3c6410_div_clks));
-		samsung_clk_register_gate(s3c6410_gate_clks,
+		samsung_clk_register_gate(ctx, s3c6410_gate_clks,
 					ARRAY_SIZE(s3c6410_gate_clks));
-		samsung_clk_register_alias(s3c6410_clock_aliases,
+		samsung_clk_register_alias(ctx, s3c6410_clock_aliases,
 					ARRAY_SIZE(s3c6410_clock_aliases));
 	}
 
-	samsung_clk_register_alias(s3c64xx_clock_aliases,
+	samsung_clk_register_alias(ctx, s3c64xx_clock_aliases,
 					ARRAY_SIZE(s3c64xx_clock_aliases));
 	s3c64xx_clk_sleep_init();
 
diff --git a/drivers/clk/samsung/clk.c b/drivers/clk/samsung/clk.c
index 91bec3ebdc8..49629c71c9e 100644
--- a/drivers/clk/samsung/clk.c
+++ b/drivers/clk/samsung/clk.c
@@ -14,13 +14,6 @@
 #include <linux/syscore_ops.h>
 #include "clk.h"
 
-static DEFINE_SPINLOCK(lock);
-static struct clk **clk_table;
-static void __iomem *reg_base;
-#ifdef CONFIG_OF
-static struct clk_onecell_data clk_data;
-#endif
-
 void samsung_clk_save(void __iomem *base,
 				    struct samsung_clk_reg_dump *rd,
 				    unsigned int num_regs)
@@ -55,40 +48,58 @@ struct samsung_clk_reg_dump *samsung_clk_alloc_reg_dump(
 }
 
 /* setup the essentials required to support clock lookup using ccf */
-void __init samsung_clk_init(struct device_node *np, void __iomem *base,
-			     unsigned long nr_clks)
+struct samsung_clk_provider *__init samsung_clk_init(struct device_node *np,
+			void __iomem *base, unsigned long nr_clks)
 {
-	reg_base = base;
+	struct samsung_clk_provider *ctx;
+	struct clk **clk_table;
+	int ret;
+	int i;
 
-	clk_table = kzalloc(sizeof(struct clk *) * nr_clks, GFP_KERNEL);
+	ctx = kzalloc(sizeof(struct samsung_clk_provider), GFP_KERNEL);
+	if (!ctx)
+		panic("could not allocate clock provider context.\n");
+
+	clk_table = kcalloc(nr_clks, sizeof(struct clk *), GFP_KERNEL);
 	if (!clk_table)
 		panic("could not allocate clock lookup table\n");
 
+	for (i = 0; i < nr_clks; ++i)
+		clk_table[i] = ERR_PTR(-ENOENT);
+
+	ctx->reg_base = base;
+	ctx->clk_data.clks = clk_table;
+	ctx->clk_data.clk_num = nr_clks;
+	spin_lock_init(&ctx->lock);
+
 	if (!np)
-		return;
+		return ctx;
 
-#ifdef CONFIG_OF
-	clk_data.clks = clk_table;
-	clk_data.clk_num = nr_clks;
-	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
-#endif
+	ret = of_clk_add_provider(np, of_clk_src_onecell_get,
+			&ctx->clk_data);
+	if (ret)
+		panic("could not register clock provide\n");
+
+	return ctx;
 }
 
 /* add a clock instance to the clock lookup table used for dt based lookup */
-void samsung_clk_add_lookup(struct clk *clk, unsigned int id)
+void samsung_clk_add_lookup(struct samsung_clk_provider *ctx, struct clk *clk,
+				unsigned int id)
 {
-	if (clk_table && id)
-		clk_table[id] = clk;
+	if (ctx->clk_data.clks && id)
+		ctx->clk_data.clks[id] = clk;
 }
 
 /* register a list of aliases */
-void __init samsung_clk_register_alias(struct samsung_clock_alias *list,
-					unsigned int nr_clk)
+void __init samsung_clk_register_alias(struct samsung_clk_provider *ctx,
+				struct samsung_clock_alias *list,
+				unsigned int nr_clk)
 {
 	struct clk *clk;
 	unsigned int idx, ret;
 
-	if (!clk_table) {
+	if (!ctx->clk_data.clks) {
 		pr_err("%s: clock table missing\n", __func__);
 		return;
 	}
@@ -100,7 +111,7 @@ void __init samsung_clk_register_alias(struct samsung_clock_alias *list,
 			continue;
 		}
 
-		clk = clk_table[list->id];
+		clk = ctx->clk_data.clks[list->id];
 		if (!clk) {
 			pr_err("%s: failed to find clock %d\n", __func__,
 				list->id);
@@ -115,7 +126,7 @@ void __init samsung_clk_register_alias(struct samsung_clock_alias *list,
 }
 
 /* register a list of fixed clocks */
-void __init samsung_clk_register_fixed_rate(
+void __init samsung_clk_register_fixed_rate(struct samsung_clk_provider *ctx,
 		struct samsung_fixed_rate_clock *list, unsigned int nr_clk)
 {
 	struct clk *clk;
@@ -130,7 +141,7 @@ void __init samsung_clk_register_fixed_rate(
 			continue;
 		}
 
-		samsung_clk_add_lookup(clk, list->id);
+		samsung_clk_add_lookup(ctx, clk, list->id);
 
 		/*
 		 * Unconditionally add a clock lookup for the fixed rate clocks.
@@ -144,7 +155,7 @@ void __init samsung_clk_register_fixed_rate(
 }
 
 /* register a list of fixed factor clocks */
-void __init samsung_clk_register_fixed_factor(
+void __init samsung_clk_register_fixed_factor(struct samsung_clk_provider *ctx,
 		struct samsung_fixed_factor_clock *list, unsigned int nr_clk)
 {
 	struct clk *clk;
@@ -159,28 +170,30 @@ void __init samsung_clk_register_fixed_factor(
 			continue;
 		}
 
-		samsung_clk_add_lookup(clk, list->id);
+		samsung_clk_add_lookup(ctx, clk, list->id);
 	}
 }
 
 /* register a list of mux clocks */
-void __init samsung_clk_register_mux(struct samsung_mux_clock *list,
-					unsigned int nr_clk)
+void __init samsung_clk_register_mux(struct samsung_clk_provider *ctx,
+				struct samsung_mux_clock *list,
+				unsigned int nr_clk)
 {
 	struct clk *clk;
 	unsigned int idx, ret;
 
 	for (idx = 0; idx < nr_clk; idx++, list++) {
 		clk = clk_register_mux(NULL, list->name, list->parent_names,
-			list->num_parents, list->flags, reg_base + list->offset,
-			list->shift, list->width, list->mux_flags, &lock);
+			list->num_parents, list->flags,
+			ctx->reg_base + list->offset,
+			list->shift, list->width, list->mux_flags, &ctx->lock);
 		if (IS_ERR(clk)) {
 			pr_err("%s: failed to register clock %s\n", __func__,
 				list->name);
 			continue;
 		}
 
-		samsung_clk_add_lookup(clk, list->id);
+		samsung_clk_add_lookup(ctx, clk, list->id);
 
 		/* register a clock lookup only if a clock alias is specified */
 		if (list->alias) {
@@ -194,8 +207,9 @@ void __init samsung_clk_register_mux(struct samsung_mux_clock *list,
 }
 
 /* register a list of div clocks */
-void __init samsung_clk_register_div(struct samsung_div_clock *list,
-					unsigned int nr_clk)
+void __init samsung_clk_register_div(struct samsung_clk_provider *ctx,
+				struct samsung_div_clock *list,
+				unsigned int nr_clk)
 {
 	struct clk *clk;
 	unsigned int idx, ret;
@@ -203,22 +217,22 @@ void __init samsung_clk_register_div(struct samsung_div_clock *list,
 	for (idx = 0; idx < nr_clk; idx++, list++) {
 		if (list->table)
 			clk = clk_register_divider_table(NULL, list->name,
-					list->parent_name, list->flags,
-					reg_base + list->offset, list->shift,
-					list->width, list->div_flags,
-					list->table, &lock);
+				list->parent_name, list->flags,
+				ctx->reg_base + list->offset,
+				list->shift, list->width, list->div_flags,
+				list->table, &ctx->lock);
 		else
 			clk = clk_register_divider(NULL, list->name,
-					list->parent_name, list->flags,
-					reg_base + list->offset, list->shift,
-					list->width, list->div_flags, &lock);
+				list->parent_name, list->flags,
+				ctx->reg_base + list->offset, list->shift,
+				list->width, list->div_flags, &ctx->lock);
 		if (IS_ERR(clk)) {
 			pr_err("%s: failed to register clock %s\n", __func__,
 				list->name);
 			continue;
 		}
 
-		samsung_clk_add_lookup(clk, list->id);
+		samsung_clk_add_lookup(ctx, clk, list->id);
 
 		/* register a clock lookup only if a clock alias is specified */
 		if (list->alias) {
@@ -232,16 +246,17 @@ void __init samsung_clk_register_div(struct samsung_div_clock *list,
 }
 
 /* register a list of gate clocks */
-void __init samsung_clk_register_gate(struct samsung_gate_clock *list,
-						unsigned int nr_clk)
+void __init samsung_clk_register_gate(struct samsung_clk_provider *ctx,
+				struct samsung_gate_clock *list,
+				unsigned int nr_clk)
 {
 	struct clk *clk;
 	unsigned int idx, ret;
 
 	for (idx = 0; idx < nr_clk; idx++, list++) {
 		clk = clk_register_gate(NULL, list->name, list->parent_name,
-				list->flags, reg_base + list->offset,
-				list->bit_idx, list->gate_flags, &lock);
+				list->flags, ctx->reg_base + list->offset,
+				list->bit_idx, list->gate_flags, &ctx->lock);
 		if (IS_ERR(clk)) {
 			pr_err("%s: failed to register clock %s\n", __func__,
 				list->name);
@@ -257,7 +272,7 @@ void __init samsung_clk_register_gate(struct samsung_gate_clock *list,
 					__func__, list->alias);
 		}
 
-		samsung_clk_add_lookup(clk, list->id);
+		samsung_clk_add_lookup(ctx, clk, list->id);
 	}
 }
 
@@ -266,21 +281,21 @@ void __init samsung_clk_register_gate(struct samsung_gate_clock *list,
  * tree and register it
  */
 #ifdef CONFIG_OF
-void __init samsung_clk_of_register_fixed_ext(
+void __init samsung_clk_of_register_fixed_ext(struct samsung_clk_provider *ctx,
 			struct samsung_fixed_rate_clock *fixed_rate_clk,
 			unsigned int nr_fixed_rate_clk,
 			struct of_device_id *clk_matches)
 {
 	const struct of_device_id *match;
-	struct device_node *np;
+	struct device_node *clk_np;
 	u32 freq;
 
-	for_each_matching_node_and_match(np, clk_matches, &match) {
-		if (of_property_read_u32(np, "clock-frequency", &freq))
+	for_each_matching_node_and_match(clk_np, clk_matches, &match) {
+		if (of_property_read_u32(clk_np, "clock-frequency", &freq))
 			continue;
-		fixed_rate_clk[(u32)match->data].fixed_rate = freq;
+		fixed_rate_clk[(unsigned long)match->data].fixed_rate = freq;
 	}
-	samsung_clk_register_fixed_rate(fixed_rate_clk, nr_fixed_rate_clk);
+	samsung_clk_register_fixed_rate(ctx, fixed_rate_clk, nr_fixed_rate_clk);
 }
 #endif
 
diff --git a/drivers/clk/samsung/clk.h b/drivers/clk/samsung/clk.h
index c7141ba826e..9693b80d924 100644
--- a/drivers/clk/samsung/clk.h
+++ b/drivers/clk/samsung/clk.h
@@ -22,6 +22,18 @@
 #include "clk-pll.h"
 
 /**
+ * struct samsung_clk_provider: information about clock provider
+ * @reg_base: virtual address for the register base.
+ * @clk_data: holds clock related data like clk* and number of clocks.
+ * @lock: maintains exclusion bwtween callbacks for a given clock-provider.
+ */
+struct samsung_clk_provider {
+	void __iomem *reg_base;
+	struct clk_onecell_data clk_data;
+	spinlock_t lock;
+};
+
+/**
  * struct samsung_clock_alias: information about mux clock
  * @id: platform specific id of the clock.
  * @dev_name: name of the device to which this clock belongs.
@@ -312,40 +324,52 @@ struct samsung_pll_clock {
 	__PLL(_typ, _id, NULL, _name, _pname, CLK_GET_RATE_NOCACHE,	\
 		_lock, _con, _rtable, _alias)
 
-extern void __init samsung_clk_init(struct device_node *np, void __iomem *base,
-				    unsigned long nr_clks);
+extern struct samsung_clk_provider *__init samsung_clk_init(
+			struct device_node *np, void __iomem *base,
+			unsigned long nr_clks);
 extern void __init samsung_clk_of_register_fixed_ext(
-		struct samsung_fixed_rate_clock *fixed_rate_clk,
-		unsigned int nr_fixed_rate_clk,
-		struct of_device_id *clk_matches);
+			struct samsung_clk_provider *ctx,
+			struct samsung_fixed_rate_clock *fixed_rate_clk,
+			unsigned int nr_fixed_rate_clk,
+			struct of_device_id *clk_matches);
 
-extern void samsung_clk_add_lookup(struct clk *clk, unsigned int id);
+extern void samsung_clk_add_lookup(struct samsung_clk_provider *ctx,
+			struct clk *clk, unsigned int id);
 
-extern void samsung_clk_register_alias(struct samsung_clock_alias *list,
-		unsigned int nr_clk);
+extern void samsung_clk_register_alias(struct samsung_clk_provider *ctx,
+			struct samsung_clock_alias *list,
+			unsigned int nr_clk);
 extern void __init samsung_clk_register_fixed_rate(
-		struct samsung_fixed_rate_clock *clk_list, unsigned int nr_clk);
+			struct samsung_clk_provider *ctx,
+			struct samsung_fixed_rate_clock *clk_list,
+			unsigned int nr_clk);
 extern void __init samsung_clk_register_fixed_factor(
-		struct samsung_fixed_factor_clock *list, unsigned int nr_clk);
-extern void __init samsung_clk_register_mux(struct samsung_mux_clock *clk_list,
-		unsigned int nr_clk);
-extern void __init samsung_clk_register_div(struct samsung_div_clock *clk_list,
-		unsigned int nr_clk);
-extern void __init samsung_clk_register_gate(
-		struct samsung_gate_clock *clk_list, unsigned int nr_clk);
-extern void __init samsung_clk_register_pll(struct samsung_pll_clock *pll_list,
-		unsigned int nr_clk, void __iomem *base);
+			struct samsung_clk_provider *ctx,
+			struct samsung_fixed_factor_clock *list,
+			unsigned int nr_clk);
+extern void __init samsung_clk_register_mux(struct samsung_clk_provider *ctx,
+			struct samsung_mux_clock *clk_list,
+			unsigned int nr_clk);
+extern void __init samsung_clk_register_div(struct samsung_clk_provider *ctx,
+			struct samsung_div_clock *clk_list,
+			unsigned int nr_clk);
+extern void __init samsung_clk_register_gate(struct samsung_clk_provider *ctx,
+			struct samsung_gate_clock *clk_list,
+			unsigned int nr_clk);
+extern void __init samsung_clk_register_pll(struct samsung_clk_provider *ctx,
+			struct samsung_pll_clock *pll_list,
+			unsigned int nr_clk, void __iomem *base);
 
 extern unsigned long _get_rate(const char *clk_name);
 
 extern void samsung_clk_save(void __iomem *base,
-			     struct samsung_clk_reg_dump *rd,
-			     unsigned int num_regs);
+			struct samsung_clk_reg_dump *rd,
+			unsigned int num_regs);
 extern void samsung_clk_restore(void __iomem *base,
-				const struct samsung_clk_reg_dump *rd,
-				unsigned int num_regs);
+			const struct samsung_clk_reg_dump *rd,
+			unsigned int num_regs);
 extern struct samsung_clk_reg_dump *samsung_clk_alloc_reg_dump(
-						const unsigned long *rdump,
-						unsigned long nr_rdump);
+			const unsigned long *rdump,
+			unsigned long nr_rdump);
 
 #endif /* __SAMSUNG_CLK_H */
diff --git a/drivers/clk/shmobile/clk-mstp.c b/drivers/clk/shmobile/clk-mstp.c
index 2e5810c88d1..1f6324e29a8 100644
--- a/drivers/clk/shmobile/clk-mstp.c
+++ b/drivers/clk/shmobile/clk-mstp.c
@@ -156,6 +156,7 @@ cpg_mstp_clock_register(const char *name, const char *parent_name,
 static void __init cpg_mstp_clocks_init(struct device_node *np)
 {
 	struct mstp_clock_group *group;
+	const char *idxname;
 	struct clk **clks;
 	unsigned int i;
 
@@ -184,6 +185,11 @@ static void __init cpg_mstp_clocks_init(struct device_node *np)
 	for (i = 0; i < MSTP_MAX_CLOCKS; ++i)
 		clks[i] = ERR_PTR(-ENOENT);
 
+	if (of_find_property(np, "clock-indices", &i))
+		idxname = "clock-indices";
+	else
+		idxname = "renesas,clock-indices";
+
 	for (i = 0; i < MSTP_MAX_CLOCKS; ++i) {
 		const char *parent_name;
 		const char *name;
@@ -197,8 +203,7 @@ static void __init cpg_mstp_clocks_init(struct device_node *np)
 			continue;
 
 		parent_name = of_clk_get_parent_name(np, i);
-		ret = of_property_read_u32_index(np, "renesas,clock-indices", i,
-						 &clkidx);
+		ret = of_property_read_u32_index(np, idxname, i, &clkidx);
 		if (parent_name == NULL || ret < 0)
 			break;
 
diff --git a/drivers/clk/socfpga/clk-pll.c b/drivers/clk/socfpga/clk-pll.c
index 88dafb5e962..de6da957a09 100644
--- a/drivers/clk/socfpga/clk-pll.c
+++ b/drivers/clk/socfpga/clk-pll.c
@@ -20,6 +20,7 @@
 #include <linux/clk-provider.h>
 #include <linux/io.h>
 #include <linux/of.h>
+#include <linux/of_address.h>
 
 #include "clk.h"
 
@@ -43,6 +44,8 @@
 
 #define to_socfpga_clk(p) container_of(p, struct socfpga_pll, hw.hw)
 
+void __iomem *clk_mgr_base_addr;
+
 static unsigned long clk_pll_recalc_rate(struct clk_hw *hwclk,
 					 unsigned long parent_rate)
 {
@@ -87,6 +90,7 @@ static __init struct clk *__socfpga_pll_init(struct device_node *node,
 	const char *clk_name = node->name;
 	const char *parent_name[SOCFPGA_MAX_PARENTS];
 	struct clk_init_data init;
+	struct device_node *clkmgr_np;
 	int rc;
 	int i = 0;
 
@@ -96,6 +100,9 @@ static __init struct clk *__socfpga_pll_init(struct device_node *node,
 	if (WARN_ON(!pll_clk))
 		return NULL;
 
+	clkmgr_np = of_find_compatible_node(NULL, NULL, "altr,clk-mgr");
+	clk_mgr_base_addr = of_iomap(clkmgr_np, 0);
+	BUG_ON(!clk_mgr_base_addr);
 	pll_clk->hw.reg = clk_mgr_base_addr + reg;
 
 	of_property_read_string(node, "clock-output-names", &clk_name);
diff --git a/drivers/clk/socfpga/clk.c b/drivers/clk/socfpga/clk.c
index 35a960a993f..43db947e5f0 100644
--- a/drivers/clk/socfpga/clk.c
+++ b/drivers/clk/socfpga/clk.c
@@ -17,28 +17,11 @@
  * 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/clkdev.h>
-#include <linux/clk-provider.h>
-#include <linux/io.h>
 #include <linux/of.h>
-#include <linux/of_address.h>
 
 #include "clk.h"
 
-void __iomem *clk_mgr_base_addr;
-
-static const struct of_device_id socfpga_child_clocks[] __initconst = {
-	{ .compatible = "altr,socfpga-pll-clock", socfpga_pll_init, },
-	{ .compatible = "altr,socfpga-perip-clk", socfpga_periph_init, },
-	{ .compatible = "altr,socfpga-gate-clk", socfpga_gate_init, },
-	{},
-};
-
-static void __init socfpga_clkmgr_init(struct device_node *node)
-{
-	clk_mgr_base_addr = of_iomap(node, 0);
-	of_clk_init(socfpga_child_clocks);
-}
-CLK_OF_DECLARE(socfpga_mgr, "altr,clk-mgr", socfpga_clkmgr_init);
+CLK_OF_DECLARE(socfpga_pll_clk, "altr,socfpga-pll-clock", socfpga_pll_init);
+CLK_OF_DECLARE(socfpga_perip_clk, "altr,socfpga-perip-clk", socfpga_periph_init);
+CLK_OF_DECLARE(socfpga_gate_clk, "altr,socfpga-gate-clk", socfpga_gate_init);
 
diff --git a/drivers/clk/st/clkgen-pll.c b/drivers/clk/st/clkgen-pll.c
index bca0a0badbf..a886702f7c8 100644
--- a/drivers/clk/st/clkgen-pll.c
+++ b/drivers/clk/st/clkgen-pll.c
@@ -521,8 +521,10 @@ static struct clk * __init clkgen_odf_register(const char *parent_name,
 	gate->lock = odf_lock;
 
 	div = kzalloc(sizeof(*div), GFP_KERNEL);
-	if (!div)
+	if (!div) {
+		kfree(gate);
 		return ERR_PTR(-ENOMEM);
+	}
 
 	div->flags = CLK_DIVIDER_ONE_BASED | CLK_DIVIDER_ALLOW_ZERO;
 	div->reg = reg + pll_data->odf[odf].offset;
diff --git a/drivers/clk/tegra/clk-pll.c b/drivers/clk/tegra/clk-pll.c
index 0d20241e077..6aad8abc69a 100644
--- a/drivers/clk/tegra/clk-pll.c
+++ b/drivers/clk/tegra/clk-pll.c
@@ -58,9 +58,9 @@
 #define PLLDU_LFCON_SET_DIVN 600
 
 #define PLLE_BASE_DIVCML_SHIFT 24
-#define PLLE_BASE_DIVCML_WIDTH 4
+#define PLLE_BASE_DIVCML_MASK 0xf
 #define PLLE_BASE_DIVP_SHIFT 16
-#define PLLE_BASE_DIVP_WIDTH 7
+#define PLLE_BASE_DIVP_WIDTH 6
 #define PLLE_BASE_DIVN_SHIFT 8
 #define PLLE_BASE_DIVN_WIDTH 8
 #define PLLE_BASE_DIVM_SHIFT 0
@@ -183,6 +183,14 @@
 #define divp_mask(p) (p->params->flags & TEGRA_PLLU ? PLLU_POST_DIVP_MASK :\
 		      mask(p->params->div_nmp->divp_width))
 
+#define divm_shift(p) (p)->params->div_nmp->divm_shift
+#define divn_shift(p) (p)->params->div_nmp->divn_shift
+#define divp_shift(p) (p)->params->div_nmp->divp_shift
+
+#define divm_mask_shifted(p) (divm_mask(p) << divm_shift(p))
+#define divn_mask_shifted(p) (divn_mask(p) << divn_shift(p))
+#define divp_mask_shifted(p) (divp_mask(p) << divp_shift(p))
+
 #define divm_max(p) (divm_mask(p))
 #define divn_max(p) (divn_mask(p))
 #define divp_max(p) (1 << (divp_mask(p)))
@@ -476,13 +484,12 @@ static void _update_pll_mnp(struct tegra_clk_pll *pll,
 	} else {
 		val = pll_readl_base(pll);
 
-		val &= ~((divm_mask(pll) << div_nmp->divm_shift) |
-		 (divn_mask(pll) << div_nmp->divn_shift) |
-		 (divp_mask(pll) << div_nmp->divp_shift));
+		val &= ~(divm_mask_shifted(pll) | divn_mask_shifted(pll) |
+			 divp_mask_shifted(pll));
 
-		val |= ((cfg->m << div_nmp->divm_shift) |
-			(cfg->n << div_nmp->divn_shift) |
-			(cfg->p << div_nmp->divp_shift));
+		val |= (cfg->m << divm_shift(pll)) |
+		       (cfg->n << divn_shift(pll)) |
+		       (cfg->p << divp_shift(pll));
 
 		pll_writel_base(val, pll);
 	}
@@ -730,11 +737,12 @@ static int clk_plle_enable(struct clk_hw *hw)
 	if (pll->params->flags & TEGRA_PLLE_CONFIGURE) {
 		/* configure dividers */
 		val = pll_readl_base(pll);
-		val &= ~(divm_mask(pll) | divn_mask(pll) | divp_mask(pll));
-		val &= ~(PLLE_BASE_DIVCML_WIDTH << PLLE_BASE_DIVCML_SHIFT);
-		val |= sel.m << pll->params->div_nmp->divm_shift;
-		val |= sel.n << pll->params->div_nmp->divn_shift;
-		val |= sel.p << pll->params->div_nmp->divp_shift;
+		val &= ~(divp_mask_shifted(pll) | divn_mask_shifted(pll) |
+			 divm_mask_shifted(pll));
+		val &= ~(PLLE_BASE_DIVCML_MASK << PLLE_BASE_DIVCML_SHIFT);
+		val |= sel.m << divm_shift(pll);
+		val |= sel.n << divn_shift(pll);
+		val |= sel.p << divp_shift(pll);
 		val |= sel.cpcon << PLLE_BASE_DIVCML_SHIFT;
 		pll_writel_base(val, pll);
 	}
@@ -745,10 +753,11 @@ static int clk_plle_enable(struct clk_hw *hw)
 	pll_writel_misc(val, pll);
 
 	val = readl(pll->clk_base + PLLE_SS_CTRL);
+	val &= ~PLLE_SS_COEFFICIENTS_MASK;
 	val |= PLLE_SS_DISABLE;
 	writel(val, pll->clk_base + PLLE_SS_CTRL);
 
-	val |= pll_readl_base(pll);
+	val = pll_readl_base(pll);
 	val |= (PLL_BASE_BYPASS | PLL_BASE_ENABLE);
 	pll_writel_base(val, pll);
 
@@ -1292,10 +1301,11 @@ static int clk_plle_tegra114_enable(struct clk_hw *hw)
 	pll_writel(val, PLLE_SS_CTRL, pll);
 
 	val = pll_readl_base(pll);
-	val &= ~(divm_mask(pll) | divn_mask(pll) | divp_mask(pll));
-	val &= ~(PLLE_BASE_DIVCML_WIDTH << PLLE_BASE_DIVCML_SHIFT);
-	val |= sel.m << pll->params->div_nmp->divm_shift;
-	val |= sel.n << pll->params->div_nmp->divn_shift;
+	val &= ~(divp_mask_shifted(pll) | divn_mask_shifted(pll) |
+		 divm_mask_shifted(pll));
+	val &= ~(PLLE_BASE_DIVCML_MASK << PLLE_BASE_DIVCML_SHIFT);
+	val |= sel.m << divm_shift(pll);
+	val |= sel.n << divn_shift(pll);
 	val |= sel.cpcon << PLLE_BASE_DIVCML_SHIFT;
 	pll_writel_base(val, pll);
 	udelay(1);
@@ -1410,6 +1420,15 @@ struct clk *tegra_clk_register_pll(const char *name, const char *parent_name,
 	return clk;
 }
 
+static struct div_nmp pll_e_nmp = {
+	.divn_shift = PLLE_BASE_DIVN_SHIFT,
+	.divn_width = PLLE_BASE_DIVN_WIDTH,
+	.divm_shift = PLLE_BASE_DIVM_SHIFT,
+	.divm_width = PLLE_BASE_DIVM_WIDTH,
+	.divp_shift = PLLE_BASE_DIVP_SHIFT,
+	.divp_width = PLLE_BASE_DIVP_WIDTH,
+};
+
 struct clk *tegra_clk_register_plle(const char *name, const char *parent_name,
 		void __iomem *clk_base, void __iomem *pmc,
 		unsigned long flags, struct tegra_clk_pll_params *pll_params,
@@ -1420,6 +1439,10 @@ struct clk *tegra_clk_register_plle(const char *name, const char *parent_name,
 
 	pll_params->flags |= TEGRA_PLL_LOCK_MISC | TEGRA_PLL_BYPASS;
 	pll_params->flags |= TEGRA_PLL_HAS_LOCK_ENABLE;
+
+	if (!pll_params->div_nmp)
+		pll_params->div_nmp = &pll_e_nmp;
+
 	pll = _tegra_init_pll(clk_base, pmc, pll_params, lock);
 	if (IS_ERR(pll))
 		return ERR_CAST(pll);
@@ -1557,9 +1580,8 @@ struct clk *tegra_clk_register_pllre(const char *name, const char *parent_name,
 		int m;
 
 		m = _pll_fixed_mdiv(pll_params, parent_rate);
-		val = m << PLL_BASE_DIVM_SHIFT;
-		val |= (pll_params->vco_min / parent_rate)
-				<< PLL_BASE_DIVN_SHIFT;
+		val = m << divm_shift(pll);
+		val |= (pll_params->vco_min / parent_rate) << divn_shift(pll);
 		pll_writel_base(val, pll);
 	}
 
@@ -1718,7 +1740,7 @@ struct clk *tegra_clk_register_plle_tegra114(const char *name,
 					"pll_re_vco");
 	} else {
 		val_aux &= ~(PLLE_AUX_PLLRE_SEL | PLLE_AUX_PLLP_SEL);
-		pll_writel(val, pll_params->aux_reg, pll);
+		pll_writel(val_aux, pll_params->aux_reg, pll);
 	}
 
 	clk = _tegra_clk_register_pll(pll, name, parent_name, flags,
diff --git a/drivers/clk/tegra/clk-tegra124.c b/drivers/clk/tegra/clk-tegra124.c
index 166e02f16c8..cc37c342c4c 100644
--- a/drivers/clk/tegra/clk-tegra124.c
+++ b/drivers/clk/tegra/clk-tegra124.c
@@ -764,7 +764,6 @@ static struct tegra_clk tegra124_clks[tegra_clk_max] __initdata = {
 	[tegra_clk_sdmmc2_8] = { .dt_id = TEGRA124_CLK_SDMMC2, .present = true },
 	[tegra_clk_i2s1] = { .dt_id = TEGRA124_CLK_I2S1, .present = true },
 	[tegra_clk_i2c1] = { .dt_id = TEGRA124_CLK_I2C1, .present = true },
-	[tegra_clk_ndflash] = { .dt_id = TEGRA124_CLK_NDFLASH, .present = true },
 	[tegra_clk_sdmmc1_8] = { .dt_id = TEGRA124_CLK_SDMMC1, .present = true },
 	[tegra_clk_sdmmc4_8] = { .dt_id = TEGRA124_CLK_SDMMC4, .present = true },
 	[tegra_clk_pwm] = { .dt_id = TEGRA124_CLK_PWM, .present = true },
@@ -809,7 +808,6 @@ static struct tegra_clk tegra124_clks[tegra_clk_max] __initdata = {
 	[tegra_clk_trace] = { .dt_id = TEGRA124_CLK_TRACE, .present = true },
 	[tegra_clk_soc_therm] = { .dt_id = TEGRA124_CLK_SOC_THERM, .present = true },
 	[tegra_clk_dtv] = { .dt_id = TEGRA124_CLK_DTV, .present = true },
-	[tegra_clk_ndspeed] = { .dt_id = TEGRA124_CLK_NDSPEED, .present = true },
 	[tegra_clk_i2cslow] = { .dt_id = TEGRA124_CLK_I2CSLOW, .present = true },
 	[tegra_clk_dsib] = { .dt_id = TEGRA124_CLK_DSIB, .present = true },
 	[tegra_clk_tsec] = { .dt_id = TEGRA124_CLK_TSEC, .present = true },
@@ -952,7 +950,6 @@ static struct tegra_clk tegra124_clks[tegra_clk_max] __initdata = {
 	[tegra_clk_clk_out_3_mux] = { .dt_id = TEGRA124_CLK_CLK_OUT_3_MUX, .present = true },
 	[tegra_clk_dsia_mux] = { .dt_id = TEGRA124_CLK_DSIA_MUX, .present = true },
 	[tegra_clk_dsib_mux] = { .dt_id = TEGRA124_CLK_DSIB_MUX, .present = true },
-	[tegra_clk_uarte] = { .dt_id = TEGRA124_CLK_UARTE, .present = true },
 };
 
 static struct tegra_devclk devclks[] __initdata = {
diff --git a/drivers/clk/ti/clk-43xx.c b/drivers/clk/ti/clk-43xx.c
index 67c8de572c5..527a43da3d3 100644
--- a/drivers/clk/ti/clk-43xx.c
+++ b/drivers/clk/ti/clk-43xx.c
@@ -105,6 +105,12 @@ static struct ti_dt_clk am43xx_clks[] = {
 	DT_CLK(NULL, "func_12m_clk", "func_12m_clk"),
 	DT_CLK(NULL, "vtp_clk_div", "vtp_clk_div"),
 	DT_CLK(NULL, "usbphy_32khz_clkmux", "usbphy_32khz_clkmux"),
+	DT_CLK("48300200.ehrpwm", "tbclk", "ehrpwm0_tbclk"),
+	DT_CLK("48302200.ehrpwm", "tbclk", "ehrpwm1_tbclk"),
+	DT_CLK("48304200.ehrpwm", "tbclk", "ehrpwm2_tbclk"),
+	DT_CLK("48306200.ehrpwm", "tbclk", "ehrpwm3_tbclk"),
+	DT_CLK("48308200.ehrpwm", "tbclk", "ehrpwm4_tbclk"),
+	DT_CLK("4830a200.ehrpwm", "tbclk", "ehrpwm5_tbclk"),
 	{ .node_name = NULL },
 };
 
diff --git a/drivers/clk/versatile/Kconfig b/drivers/clk/versatile/Kconfig
new file mode 100644
index 00000000000..1530c9352a7
--- /dev/null
+++ b/drivers/clk/versatile/Kconfig
@@ -0,0 +1,26 @@
+config COMMON_CLK_VERSATILE
+	bool "Clock driver for ARM Reference designs"
+	depends on ARCH_INTEGRATOR || ARCH_REALVIEW || ARCH_VEXPRESS || ARM64
+	---help---
+          Supports clocking on ARM Reference designs:
+	  - Integrator/AP and Integrator/CP
+	  - RealView PB1176, EB, PB11MP and PBX
+	  - Versatile Express
+
+config CLK_SP810
+	bool "Clock driver for ARM SP810 System Controller"
+	depends on COMMON_CLK_VERSATILE
+	default y if ARCH_VEXPRESS
+	---help---
+	  Supports clock muxing (REFCLK/TIMCLK to TIMERCLKEN0-3) capabilities
+	  of the ARM SP810 System Controller cell.
+
+config CLK_VEXPRESS_OSC
+	bool "Clock driver for Versatile Express OSC clock generators"
+	depends on COMMON_CLK_VERSATILE
+	depends on VEXPRESS_CONFIG
+	default y if ARCH_VEXPRESS
+	---help---
+	  Simple regmap-based driver driving clock generators on Versatile
+	  Express platforms hidden behind its configuration infrastructure,
+	  commonly known as OSCs.
diff --git a/drivers/clk/versatile/Makefile b/drivers/clk/versatile/Makefile
index c16ca787170..fd449f9b006 100644
--- a/drivers/clk/versatile/Makefile
+++ b/drivers/clk/versatile/Makefile
@@ -3,5 +3,6 @@ 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 clk-sp810.o
-obj-$(CONFIG_VEXPRESS_CONFIG)	+= clk-vexpress-osc.o
+obj-$(CONFIG_ARCH_VEXPRESS)	+= clk-vexpress.o
+obj-$(CONFIG_CLK_SP810)		+= clk-sp810.o
+obj-$(CONFIG_CLK_VEXPRESS_OSC)	+= clk-vexpress-osc.o
diff --git a/drivers/clk/versatile/clk-vexpress-osc.c b/drivers/clk/versatile/clk-vexpress-osc.c
index 2dc8b41a339..529a59c0fbf 100644
--- a/drivers/clk/versatile/clk-vexpress-osc.c
+++ b/drivers/clk/versatile/clk-vexpress-osc.c
@@ -11,8 +11,6 @@
  * Copyright (C) 2012 ARM Limited
  */
 
-#define pr_fmt(fmt) "vexpress-osc: " fmt
-
 #include <linux/clkdev.h>
 #include <linux/clk-provider.h>
 #include <linux/err.h>
@@ -22,7 +20,7 @@
 #include <linux/vexpress.h>
 
 struct vexpress_osc {
-	struct vexpress_config_func *func;
+	struct regmap *reg;
 	struct clk_hw hw;
 	unsigned long rate_min;
 	unsigned long rate_max;
@@ -36,7 +34,7 @@ static unsigned long vexpress_osc_recalc_rate(struct clk_hw *hw,
 	struct vexpress_osc *osc = to_vexpress_osc(hw);
 	u32 rate;
 
-	vexpress_config_read(osc->func, 0, &rate);
+	regmap_read(osc->reg, 0, &rate);
 
 	return rate;
 }
@@ -60,7 +58,7 @@ static int vexpress_osc_set_rate(struct clk_hw *hw, unsigned long rate,
 {
 	struct vexpress_osc *osc = to_vexpress_osc(hw);
 
-	return vexpress_config_write(osc->func, 0, rate);
+	return regmap_write(osc->reg, 0, rate);
 }
 
 static struct clk_ops vexpress_osc_ops = {
@@ -70,56 +68,31 @@ static struct clk_ops vexpress_osc_ops = {
 };
 
 
-struct clk * __init vexpress_osc_setup(struct device *dev)
-{
-	struct clk_init_data init;
-	struct vexpress_osc *osc = kzalloc(sizeof(*osc), GFP_KERNEL);
-
-	if (!osc)
-		return NULL;
-
-	osc->func = vexpress_config_func_get_by_dev(dev);
-	if (!osc->func) {
-		kfree(osc);
-		return NULL;
-	}
-
-	init.name = dev_name(dev);
-	init.ops = &vexpress_osc_ops;
-	init.flags = CLK_IS_ROOT;
-	init.num_parents = 0;
-	osc->hw.init = &init;
-
-	return clk_register(NULL, &osc->hw);
-}
-
-void __init vexpress_osc_of_setup(struct device_node *node)
+static int vexpress_osc_probe(struct platform_device *pdev)
 {
+	struct clk_lookup *cl = pdev->dev.platform_data; /* Non-DT lookup */
 	struct clk_init_data init;
 	struct vexpress_osc *osc;
 	struct clk *clk;
 	u32 range[2];
 
-	osc = kzalloc(sizeof(*osc), GFP_KERNEL);
+	osc = devm_kzalloc(&pdev->dev, sizeof(*osc), GFP_KERNEL);
 	if (!osc)
-		goto error;
+		return -ENOMEM;
 
-	osc->func = vexpress_config_func_get_by_node(node);
-	if (!osc->func) {
-		pr_err("Failed to obtain config func for node '%s'!\n",
-				node->full_name);
-		goto error;
-	}
+	osc->reg = devm_regmap_init_vexpress_config(&pdev->dev);
+	if (IS_ERR(osc->reg))
+		return PTR_ERR(osc->reg);
 
-	if (of_property_read_u32_array(node, "freq-range", range,
+	if (of_property_read_u32_array(pdev->dev.of_node, "freq-range", range,
 			ARRAY_SIZE(range)) == 0) {
 		osc->rate_min = range[0];
 		osc->rate_max = range[1];
 	}
 
-	of_property_read_string(node, "clock-output-names", &init.name);
-	if (!init.name)
-		init.name = node->full_name;
+	if (of_property_read_string(pdev->dev.of_node, "clock-output-names",
+			&init.name) != 0)
+		init.name = dev_name(&pdev->dev);
 
 	init.ops = &vexpress_osc_ops;
 	init.flags = CLK_IS_ROOT;
@@ -128,20 +101,37 @@ void __init vexpress_osc_of_setup(struct device_node *node)
 	osc->hw.init = &init;
 
 	clk = clk_register(NULL, &osc->hw);
-	if (IS_ERR(clk)) {
-		pr_err("Failed to register clock '%s'!\n", init.name);
-		goto error;
+	if (IS_ERR(clk))
+		return PTR_ERR(clk);
+
+	of_clk_add_provider(pdev->dev.of_node, of_clk_src_simple_get, clk);
+
+	/* Only happens for non-DT cases */
+	if (cl) {
+		cl->clk = clk;
+		clkdev_add(cl);
 	}
 
-	of_clk_add_provider(node, of_clk_src_simple_get, clk);
+	dev_dbg(&pdev->dev, "Registered clock '%s'\n", init.name);
 
-	pr_debug("Registered clock '%s'\n", init.name);
+	return 0;
+}
 
-	return;
+static struct of_device_id vexpress_osc_of_match[] = {
+	{ .compatible = "arm,vexpress-osc", },
+	{}
+};
 
-error:
-	if (osc->func)
-		vexpress_config_func_put(osc->func);
-	kfree(osc);
+static struct platform_driver vexpress_osc_driver = {
+	.driver	= {
+		.name = "vexpress-osc",
+		.of_match_table = vexpress_osc_of_match,
+	},
+	.probe = vexpress_osc_probe,
+};
+
+static int __init vexpress_osc_init(void)
+{
+	return platform_driver_register(&vexpress_osc_driver);
 }
-CLK_OF_DECLARE(vexpress_soc, "arm,vexpress-osc", vexpress_osc_of_setup);
+core_initcall(vexpress_osc_init);
diff --git a/drivers/clocksource/Kconfig b/drivers/clocksource/Kconfig
index 96918e1f26a..43f1acf0d1d 100644
--- a/drivers/clocksource/Kconfig
+++ b/drivers/clocksource/Kconfig
@@ -191,3 +191,14 @@ config EM_TIMER_STI
 
 config CLKSRC_QCOM
 	bool
+
+config CLKSRC_VERSATILE
+	bool "ARM Versatile (Express) reference platforms clock source"
+	depends on GENERIC_SCHED_CLOCK && !ARCH_USES_GETTIMEOFFSET
+	select CLKSRC_OF
+	default y if MFD_VEXPRESS_SYSREG
+	help
+	  This option enables clock source based on free running
+	  counter available in the "System Registers" block of
+	  ARM Versatile, RealView and Versatile Express reference
+	  platforms.
diff --git a/drivers/clocksource/Makefile b/drivers/clocksource/Makefile
index 98cb6c51aa8..6f25bdffc17 100644
--- a/drivers/clocksource/Makefile
+++ b/drivers/clocksource/Makefile
@@ -39,3 +39,4 @@ obj-$(CONFIG_ARM_GLOBAL_TIMER)		+= arm_global_timer.o
 obj-$(CONFIG_CLKSRC_METAG_GENERIC)	+= metag_generic.o
 obj-$(CONFIG_ARCH_HAS_TICK_BROADCAST)	+= dummy_timer.o
 obj-$(CONFIG_ARCH_KEYSTONE)		+= timer-keystone.o
+obj-$(CONFIG_CLKSRC_VERSATILE)		+= versatile.o
diff --git a/drivers/clocksource/arm_arch_timer.c b/drivers/clocksource/arm_arch_timer.c
index 57e823c44d2..5163ec13429 100644
--- a/drivers/clocksource/arm_arch_timer.c
+++ b/drivers/clocksource/arm_arch_timer.c
@@ -66,6 +66,7 @@ static int arch_timer_ppi[MAX_TIMER_PPI];
 static struct clock_event_device __percpu *arch_timer_evt;
 
 static bool arch_timer_use_virtual = true;
+static bool arch_timer_c3stop;
 static bool arch_timer_mem_use_virtual;
 
 /*
@@ -263,7 +264,8 @@ static void __arch_timer_setup(unsigned type,
 	clk->features = CLOCK_EVT_FEAT_ONESHOT;
 
 	if (type == ARCH_CP15_TIMER) {
-		clk->features |= CLOCK_EVT_FEAT_C3STOP;
+		if (arch_timer_c3stop)
+			clk->features |= CLOCK_EVT_FEAT_C3STOP;
 		clk->name = "arch_sys_timer";
 		clk->rating = 450;
 		clk->cpumask = cpumask_of(smp_processor_id());
@@ -665,6 +667,8 @@ static void __init arch_timer_init(struct device_node *np)
 		}
 	}
 
+	arch_timer_c3stop = !of_property_read_bool(np, "always-on");
+
 	arch_timer_register();
 	arch_timer_common_init();
 }
diff --git a/drivers/clocksource/cadence_ttc_timer.c b/drivers/clocksource/cadence_ttc_timer.c
index 49fbe2847c8..7a08811df9a 100644
--- a/drivers/clocksource/cadence_ttc_timer.c
+++ b/drivers/clocksource/cadence_ttc_timer.c
@@ -118,11 +118,11 @@ static void ttc_set_interval(struct ttc_timer *timer,
 	u32 ctrl_reg;
 
 	/* Disable the counter, set the counter value  and re-enable counter */
-	ctrl_reg = __raw_readl(timer->base_addr + TTC_CNT_CNTRL_OFFSET);
+	ctrl_reg = readl_relaxed(timer->base_addr + TTC_CNT_CNTRL_OFFSET);
 	ctrl_reg |= TTC_CNT_CNTRL_DISABLE_MASK;
-	__raw_writel(ctrl_reg, timer->base_addr + TTC_CNT_CNTRL_OFFSET);
+	writel_relaxed(ctrl_reg, timer->base_addr + TTC_CNT_CNTRL_OFFSET);
 
-	__raw_writel(cycles, timer->base_addr + TTC_INTR_VAL_OFFSET);
+	writel_relaxed(cycles, timer->base_addr + TTC_INTR_VAL_OFFSET);
 
 	/*
 	 * Reset the counter (0x10) so that it starts from 0, one-shot
@@ -130,7 +130,7 @@ static void ttc_set_interval(struct ttc_timer *timer,
 	 */
 	ctrl_reg |= CNT_CNTRL_RESET;
 	ctrl_reg &= ~TTC_CNT_CNTRL_DISABLE_MASK;
-	__raw_writel(ctrl_reg, timer->base_addr + TTC_CNT_CNTRL_OFFSET);
+	writel_relaxed(ctrl_reg, timer->base_addr + TTC_CNT_CNTRL_OFFSET);
 }
 
 /**
@@ -147,7 +147,7 @@ static irqreturn_t ttc_clock_event_interrupt(int irq, void *dev_id)
 	struct ttc_timer *timer = &ttce->ttc;
 
 	/* Acknowledge the interrupt and call event handler */
-	__raw_readl(timer->base_addr + TTC_ISR_OFFSET);
+	readl_relaxed(timer->base_addr + TTC_ISR_OFFSET);
 
 	ttce->ce.event_handler(&ttce->ce);
 
@@ -163,13 +163,13 @@ static cycle_t __ttc_clocksource_read(struct clocksource *cs)
 {
 	struct ttc_timer *timer = &to_ttc_timer_clksrc(cs)->ttc;
 
-	return (cycle_t)__raw_readl(timer->base_addr +
+	return (cycle_t)readl_relaxed(timer->base_addr +
 				TTC_COUNT_VAL_OFFSET);
 }
 
 static u64 notrace ttc_sched_clock_read(void)
 {
-	return __raw_readl(ttc_sched_clock_val_reg);
+	return readl_relaxed(ttc_sched_clock_val_reg);
 }
 
 /**
@@ -211,17 +211,17 @@ static void ttc_set_mode(enum clock_event_mode mode,
 	case CLOCK_EVT_MODE_ONESHOT:
 	case CLOCK_EVT_MODE_UNUSED:
 	case CLOCK_EVT_MODE_SHUTDOWN:
-		ctrl_reg = __raw_readl(timer->base_addr +
+		ctrl_reg = readl_relaxed(timer->base_addr +
 					TTC_CNT_CNTRL_OFFSET);
 		ctrl_reg |= TTC_CNT_CNTRL_DISABLE_MASK;
-		__raw_writel(ctrl_reg,
+		writel_relaxed(ctrl_reg,
 				timer->base_addr + TTC_CNT_CNTRL_OFFSET);
 		break;
 	case CLOCK_EVT_MODE_RESUME:
-		ctrl_reg = __raw_readl(timer->base_addr +
+		ctrl_reg = readl_relaxed(timer->base_addr +
 					TTC_CNT_CNTRL_OFFSET);
 		ctrl_reg &= ~TTC_CNT_CNTRL_DISABLE_MASK;
-		__raw_writel(ctrl_reg,
+		writel_relaxed(ctrl_reg,
 				timer->base_addr + TTC_CNT_CNTRL_OFFSET);
 		break;
 	}
@@ -266,8 +266,8 @@ static int ttc_rate_change_clocksource_cb(struct notifier_block *nb,
 		 * of an abort.
 		 */
 		ttccs->scale_clk_ctrl_reg_old =
-			__raw_readl(ttccs->ttc.base_addr +
-					TTC_CLK_CNTRL_OFFSET);
+			readl_relaxed(ttccs->ttc.base_addr +
+			TTC_CLK_CNTRL_OFFSET);
 
 		psv = (ttccs->scale_clk_ctrl_reg_old &
 				TTC_CLK_CNTRL_PSV_MASK) >>
@@ -291,8 +291,8 @@ static int ttc_rate_change_clocksource_cb(struct notifier_block *nb,
 			return NOTIFY_DONE;
 
 		/* scale up: adjust divider now - before frequency change */
-		__raw_writel(ttccs->scale_clk_ctrl_reg_new,
-				ttccs->ttc.base_addr + TTC_CLK_CNTRL_OFFSET);
+		writel_relaxed(ttccs->scale_clk_ctrl_reg_new,
+			       ttccs->ttc.base_addr + TTC_CLK_CNTRL_OFFSET);
 		break;
 	}
 	case POST_RATE_CHANGE:
@@ -301,8 +301,8 @@ static int ttc_rate_change_clocksource_cb(struct notifier_block *nb,
 			return NOTIFY_OK;
 
 		/* scale down: adjust divider now - after frequency change */
-		__raw_writel(ttccs->scale_clk_ctrl_reg_new,
-				ttccs->ttc.base_addr + TTC_CLK_CNTRL_OFFSET);
+		writel_relaxed(ttccs->scale_clk_ctrl_reg_new,
+			       ttccs->ttc.base_addr + TTC_CLK_CNTRL_OFFSET);
 		break;
 
 	case ABORT_RATE_CHANGE:
@@ -311,8 +311,8 @@ static int ttc_rate_change_clocksource_cb(struct notifier_block *nb,
 			return NOTIFY_OK;
 
 		/* restore original register value */
-		__raw_writel(ttccs->scale_clk_ctrl_reg_old,
-				ttccs->ttc.base_addr + TTC_CLK_CNTRL_OFFSET);
+		writel_relaxed(ttccs->scale_clk_ctrl_reg_old,
+			       ttccs->ttc.base_addr + TTC_CLK_CNTRL_OFFSET);
 		/* fall through */
 	default:
 		return NOTIFY_DONE;
@@ -359,10 +359,10 @@ static void __init ttc_setup_clocksource(struct clk *clk, void __iomem *base)
 	 * with no interrupt and it rolls over at 0xFFFF. Pre-scale
 	 * it by 32 also. Let it start running now.
 	 */
-	__raw_writel(0x0,  ttccs->ttc.base_addr + TTC_IER_OFFSET);
-	__raw_writel(CLK_CNTRL_PRESCALE | CLK_CNTRL_PRESCALE_EN,
+	writel_relaxed(0x0,  ttccs->ttc.base_addr + TTC_IER_OFFSET);
+	writel_relaxed(CLK_CNTRL_PRESCALE | CLK_CNTRL_PRESCALE_EN,
 		     ttccs->ttc.base_addr + TTC_CLK_CNTRL_OFFSET);
-	__raw_writel(CNT_CNTRL_RESET,
+	writel_relaxed(CNT_CNTRL_RESET,
 		     ttccs->ttc.base_addr + TTC_CNT_CNTRL_OFFSET);
 
 	err = clocksource_register_hz(&ttccs->cs, ttccs->ttc.freq / PRESCALE);
@@ -438,10 +438,10 @@ static void __init ttc_setup_clockevent(struct clk *clk,
 	 * is prescaled by 32 using the interval interrupt. Leave it
 	 * disabled for now.
 	 */
-	__raw_writel(0x23, ttcce->ttc.base_addr + TTC_CNT_CNTRL_OFFSET);
-	__raw_writel(CLK_CNTRL_PRESCALE | CLK_CNTRL_PRESCALE_EN,
+	writel_relaxed(0x23, ttcce->ttc.base_addr + TTC_CNT_CNTRL_OFFSET);
+	writel_relaxed(CLK_CNTRL_PRESCALE | CLK_CNTRL_PRESCALE_EN,
 		     ttcce->ttc.base_addr + TTC_CLK_CNTRL_OFFSET);
-	__raw_writel(0x1,  ttcce->ttc.base_addr + TTC_IER_OFFSET);
+	writel_relaxed(0x1,  ttcce->ttc.base_addr + TTC_IER_OFFSET);
 
 	err = request_irq(irq, ttc_clock_event_interrupt,
 			  IRQF_TIMER, ttcce->ce.name, ttcce);
@@ -490,7 +490,7 @@ static void __init ttc_timer_init(struct device_node *timer)
 		BUG();
 	}
 
-	clksel = __raw_readl(timer_baseaddr + TTC_CLK_CNTRL_OFFSET);
+	clksel = readl_relaxed(timer_baseaddr + TTC_CLK_CNTRL_OFFSET);
 	clksel = !!(clksel & TTC_CLK_CNTRL_CSRC_MASK);
 	clk_cs = of_clk_get(timer, clksel);
 	if (IS_ERR(clk_cs)) {
@@ -498,7 +498,7 @@ static void __init ttc_timer_init(struct device_node *timer)
 		BUG();
 	}
 
-	clksel = __raw_readl(timer_baseaddr + 4 + TTC_CLK_CNTRL_OFFSET);
+	clksel = readl_relaxed(timer_baseaddr + 4 + TTC_CLK_CNTRL_OFFSET);
 	clksel = !!(clksel & TTC_CLK_CNTRL_CSRC_MASK);
 	clk_ce = of_clk_get(timer, clksel);
 	if (IS_ERR(clk_ce)) {
diff --git a/drivers/clocksource/exynos_mct.c b/drivers/clocksource/exynos_mct.c
index a6ee6d7cd63..8d6420013a0 100644
--- a/drivers/clocksource/exynos_mct.c
+++ b/drivers/clocksource/exynos_mct.c
@@ -24,6 +24,7 @@
 #include <linux/of_irq.h>
 #include <linux/of_address.h>
 #include <linux/clocksource.h>
+#include <linux/sched_clock.h>
 
 #define EXYNOS4_MCTREG(x)		(x)
 #define EXYNOS4_MCT_G_CNT_L		EXYNOS4_MCTREG(0x100)
@@ -192,12 +193,19 @@ struct clocksource mct_frc = {
 	.resume		= exynos4_frc_resume,
 };
 
+static u64 notrace exynos4_read_sched_clock(void)
+{
+	return exynos4_frc_read(&mct_frc);
+}
+
 static void __init exynos4_clocksource_init(void)
 {
 	exynos4_mct_frc_start(0, 0);
 
 	if (clocksource_register_hz(&mct_frc, clk_rate))
 		panic("%s: can't register clocksource\n", mct_frc.name);
+
+	sched_clock_register(exynos4_read_sched_clock, 64, clk_rate);
 }
 
 static void exynos4_mct_comp0_stop(void)
@@ -416,8 +424,6 @@ static int exynos4_local_timer_setup(struct clock_event_device *evt)
 	evt->set_mode = exynos4_tick_set_mode;
 	evt->features = CLOCK_EVT_FEAT_PERIODIC | CLOCK_EVT_FEAT_ONESHOT;
 	evt->rating = 450;
-	clockevents_config_and_register(evt, clk_rate / (TICK_BASE_CNT + 1),
-					0xf, 0x7fffffff);
 
 	exynos4_mct_write(TICK_BASE_CNT, mevt->base + MCT_L_TCNTB_OFFSET);
 
@@ -430,9 +436,12 @@ static int exynos4_local_timer_setup(struct clock_event_device *evt)
 				evt->irq);
 			return -EIO;
 		}
+		irq_force_affinity(mct_irqs[MCT_L0_IRQ + cpu], cpumask_of(cpu));
 	} else {
 		enable_percpu_irq(mct_irqs[MCT_L0_IRQ], 0);
 	}
+	clockevents_config_and_register(evt, clk_rate / (TICK_BASE_CNT + 1),
+					0xf, 0x7fffffff);
 
 	return 0;
 }
@@ -450,7 +459,6 @@ static int exynos4_mct_cpu_notify(struct notifier_block *self,
 					   unsigned long action, void *hcpu)
 {
 	struct mct_clock_event_device *mevt;
-	unsigned int cpu;
 
 	/*
 	 * Grab cpu pointer in each case to avoid spurious
@@ -461,12 +469,6 @@ static int exynos4_mct_cpu_notify(struct notifier_block *self,
 		mevt = this_cpu_ptr(&percpu_mct_tick);
 		exynos4_local_timer_setup(&mevt->evt);
 		break;
-	case CPU_ONLINE:
-		cpu = (unsigned long)hcpu;
-		if (mct_int_type == MCT_INT_SPI)
-			irq_set_affinity(mct_irqs[MCT_L0_IRQ + cpu],
-						cpumask_of(cpu));
-		break;
 	case CPU_DYING:
 		mevt = this_cpu_ptr(&percpu_mct_tick);
 		exynos4_local_timer_stop(&mevt->evt);
diff --git a/drivers/clocksource/sh_cmt.c b/drivers/clocksource/sh_cmt.c
index 0b1836a6c53..bc8d025ce86 100644
--- a/drivers/clocksource/sh_cmt.c
+++ b/drivers/clocksource/sh_cmt.c
@@ -11,40 +11,93 @@
  * 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/clockchips.h>
+#include <linux/clocksource.h>
+#include <linux/delay.h>
+#include <linux/err.h>
 #include <linux/init.h>
-#include <linux/platform_device.h>
-#include <linux/spinlock.h>
 #include <linux/interrupt.h>
-#include <linux/ioport.h>
 #include <linux/io.h>
-#include <linux/clk.h>
+#include <linux/ioport.h>
 #include <linux/irq.h>
-#include <linux/err.h>
-#include <linux/delay.h>
-#include <linux/clocksource.h>
-#include <linux/clockchips.h>
-#include <linux/sh_timer.h>
-#include <linux/slab.h>
 #include <linux/module.h>
+#include <linux/platform_device.h>
 #include <linux/pm_domain.h>
 #include <linux/pm_runtime.h>
+#include <linux/sh_timer.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+
+struct sh_cmt_device;
+
+/*
+ * The CMT comes in 5 different identified flavours, depending not only on the
+ * SoC but also on the particular instance. The following table lists the main
+ * characteristics of those flavours.
+ *
+ *			16B	32B	32B-F	48B	48B-2
+ * -----------------------------------------------------------------------------
+ * Channels		2	1/4	1	6	2/8
+ * Control Width	16	16	16	16	32
+ * Counter Width	16	32	32	32/48	32/48
+ * Shared Start/Stop	Y	Y	Y	Y	N
+ *
+ * The 48-bit gen2 version has a per-channel start/stop register located in the
+ * channel registers block. All other versions have a shared start/stop register
+ * located in the global space.
+ *
+ * Channels are indexed from 0 to N-1 in the documentation. The channel index
+ * infers the start/stop bit position in the control register and the channel
+ * registers block address. Some CMT instances have a subset of channels
+ * available, in which case the index in the documentation doesn't match the
+ * "real" index as implemented in hardware. This is for instance the case with
+ * CMT0 on r8a7740, which is a 32-bit variant with a single channel numbered 0
+ * in the documentation but using start/stop bit 5 and having its registers
+ * block at 0x60.
+ *
+ * Similarly CMT0 on r8a73a4, r8a7790 and r8a7791, while implementing 32-bit
+ * channels only, is a 48-bit gen2 CMT with the 48-bit channels unavailable.
+ */
+
+enum sh_cmt_model {
+	SH_CMT_16BIT,
+	SH_CMT_32BIT,
+	SH_CMT_32BIT_FAST,
+	SH_CMT_48BIT,
+	SH_CMT_48BIT_GEN2,
+};
+
+struct sh_cmt_info {
+	enum sh_cmt_model model;
 
-struct sh_cmt_priv {
-	void __iomem *mapbase;
-	void __iomem *mapbase_str;
-	struct clk *clk;
 	unsigned long width; /* 16 or 32 bit version of hardware block */
 	unsigned long overflow_bit;
 	unsigned long clear_bits;
-	struct irqaction irqaction;
-	struct platform_device *pdev;
 
+	/* callbacks for CMSTR and CMCSR access */
+	unsigned long (*read_control)(void __iomem *base, unsigned long offs);
+	void (*write_control)(void __iomem *base, unsigned long offs,
+			      unsigned long value);
+
+	/* callbacks for CMCNT and CMCOR access */
+	unsigned long (*read_count)(void __iomem *base, unsigned long offs);
+	void (*write_count)(void __iomem *base, unsigned long offs,
+			    unsigned long value);
+};
+
+struct sh_cmt_channel {
+	struct sh_cmt_device *cmt;
+
+	unsigned int index;	/* Index in the documentation */
+	unsigned int hwidx;	/* Real hardware index */
+
+	void __iomem *iostart;
+	void __iomem *ioctrl;
+
+	unsigned int timer_bit;
 	unsigned long flags;
 	unsigned long match_value;
 	unsigned long next_match_value;
@@ -55,38 +108,52 @@ struct sh_cmt_priv {
 	struct clocksource cs;
 	unsigned long total_cycles;
 	bool cs_enabled;
+};
 
-	/* callbacks for CMSTR and CMCSR access */
-	unsigned long (*read_control)(void __iomem *base, unsigned long offs);
-	void (*write_control)(void __iomem *base, unsigned long offs,
-			      unsigned long value);
+struct sh_cmt_device {
+	struct platform_device *pdev;
 
-	/* callbacks for CMCNT and CMCOR access */
-	unsigned long (*read_count)(void __iomem *base, unsigned long offs);
-	void (*write_count)(void __iomem *base, unsigned long offs,
-			    unsigned long value);
+	const struct sh_cmt_info *info;
+	bool legacy;
+
+	void __iomem *mapbase_ch;
+	void __iomem *mapbase;
+	struct clk *clk;
+
+	struct sh_cmt_channel *channels;
+	unsigned int num_channels;
+
+	bool has_clockevent;
+	bool has_clocksource;
 };
 
-/* Examples of supported CMT timer register layouts and I/O access widths:
- *
- * "16-bit counter and 16-bit control" as found on sh7263:
- * CMSTR 0xfffec000 16-bit
- * CMCSR 0xfffec002 16-bit
- * CMCNT 0xfffec004 16-bit
- * CMCOR 0xfffec006 16-bit
- *
- * "32-bit counter and 16-bit control" as found on sh7372, sh73a0, r8a7740:
- * CMSTR 0xffca0000 16-bit
- * CMCSR 0xffca0060 16-bit
- * CMCNT 0xffca0064 32-bit
- * CMCOR 0xffca0068 32-bit
- *
- * "32-bit counter and 32-bit control" as found on r8a73a4 and r8a7790:
- * CMSTR 0xffca0500 32-bit
- * CMCSR 0xffca0510 32-bit
- * CMCNT 0xffca0514 32-bit
- * CMCOR 0xffca0518 32-bit
- */
+#define SH_CMT16_CMCSR_CMF		(1 << 7)
+#define SH_CMT16_CMCSR_CMIE		(1 << 6)
+#define SH_CMT16_CMCSR_CKS8		(0 << 0)
+#define SH_CMT16_CMCSR_CKS32		(1 << 0)
+#define SH_CMT16_CMCSR_CKS128		(2 << 0)
+#define SH_CMT16_CMCSR_CKS512		(3 << 0)
+#define SH_CMT16_CMCSR_CKS_MASK		(3 << 0)
+
+#define SH_CMT32_CMCSR_CMF		(1 << 15)
+#define SH_CMT32_CMCSR_OVF		(1 << 14)
+#define SH_CMT32_CMCSR_WRFLG		(1 << 13)
+#define SH_CMT32_CMCSR_STTF		(1 << 12)
+#define SH_CMT32_CMCSR_STPF		(1 << 11)
+#define SH_CMT32_CMCSR_SSIE		(1 << 10)
+#define SH_CMT32_CMCSR_CMS		(1 << 9)
+#define SH_CMT32_CMCSR_CMM		(1 << 8)
+#define SH_CMT32_CMCSR_CMTOUT_IE	(1 << 7)
+#define SH_CMT32_CMCSR_CMR_NONE		(0 << 4)
+#define SH_CMT32_CMCSR_CMR_DMA		(1 << 4)
+#define SH_CMT32_CMCSR_CMR_IRQ		(2 << 4)
+#define SH_CMT32_CMCSR_CMR_MASK		(3 << 4)
+#define SH_CMT32_CMCSR_DBGIVD		(1 << 3)
+#define SH_CMT32_CMCSR_CKS_RCLK8	(4 << 0)
+#define SH_CMT32_CMCSR_CKS_RCLK32	(5 << 0)
+#define SH_CMT32_CMCSR_CKS_RCLK128	(6 << 0)
+#define SH_CMT32_CMCSR_CKS_RCLK1	(7 << 0)
+#define SH_CMT32_CMCSR_CKS_MASK		(7 << 0)
 
 static unsigned long sh_cmt_read16(void __iomem *base, unsigned long offs)
 {
@@ -110,64 +177,123 @@ static void sh_cmt_write32(void __iomem *base, unsigned long offs,
 	iowrite32(value, base + (offs << 2));
 }
 
+static const struct sh_cmt_info sh_cmt_info[] = {
+	[SH_CMT_16BIT] = {
+		.model = SH_CMT_16BIT,
+		.width = 16,
+		.overflow_bit = SH_CMT16_CMCSR_CMF,
+		.clear_bits = ~SH_CMT16_CMCSR_CMF,
+		.read_control = sh_cmt_read16,
+		.write_control = sh_cmt_write16,
+		.read_count = sh_cmt_read16,
+		.write_count = sh_cmt_write16,
+	},
+	[SH_CMT_32BIT] = {
+		.model = SH_CMT_32BIT,
+		.width = 32,
+		.overflow_bit = SH_CMT32_CMCSR_CMF,
+		.clear_bits = ~(SH_CMT32_CMCSR_CMF | SH_CMT32_CMCSR_OVF),
+		.read_control = sh_cmt_read16,
+		.write_control = sh_cmt_write16,
+		.read_count = sh_cmt_read32,
+		.write_count = sh_cmt_write32,
+	},
+	[SH_CMT_32BIT_FAST] = {
+		.model = SH_CMT_32BIT_FAST,
+		.width = 32,
+		.overflow_bit = SH_CMT32_CMCSR_CMF,
+		.clear_bits = ~(SH_CMT32_CMCSR_CMF | SH_CMT32_CMCSR_OVF),
+		.read_control = sh_cmt_read16,
+		.write_control = sh_cmt_write16,
+		.read_count = sh_cmt_read32,
+		.write_count = sh_cmt_write32,
+	},
+	[SH_CMT_48BIT] = {
+		.model = SH_CMT_48BIT,
+		.width = 32,
+		.overflow_bit = SH_CMT32_CMCSR_CMF,
+		.clear_bits = ~(SH_CMT32_CMCSR_CMF | SH_CMT32_CMCSR_OVF),
+		.read_control = sh_cmt_read32,
+		.write_control = sh_cmt_write32,
+		.read_count = sh_cmt_read32,
+		.write_count = sh_cmt_write32,
+	},
+	[SH_CMT_48BIT_GEN2] = {
+		.model = SH_CMT_48BIT_GEN2,
+		.width = 32,
+		.overflow_bit = SH_CMT32_CMCSR_CMF,
+		.clear_bits = ~(SH_CMT32_CMCSR_CMF | SH_CMT32_CMCSR_OVF),
+		.read_control = sh_cmt_read32,
+		.write_control = sh_cmt_write32,
+		.read_count = sh_cmt_read32,
+		.write_count = sh_cmt_write32,
+	},
+};
+
 #define CMCSR 0 /* channel register */
 #define CMCNT 1 /* channel register */
 #define CMCOR 2 /* channel register */
 
-static inline unsigned long sh_cmt_read_cmstr(struct sh_cmt_priv *p)
+static inline unsigned long sh_cmt_read_cmstr(struct sh_cmt_channel *ch)
 {
-	return p->read_control(p->mapbase_str, 0);
+	if (ch->iostart)
+		return ch->cmt->info->read_control(ch->iostart, 0);
+	else
+		return ch->cmt->info->read_control(ch->cmt->mapbase, 0);
 }
 
-static inline unsigned long sh_cmt_read_cmcsr(struct sh_cmt_priv *p)
+static inline void sh_cmt_write_cmstr(struct sh_cmt_channel *ch,
+				      unsigned long value)
 {
-	return p->read_control(p->mapbase, CMCSR);
+	if (ch->iostart)
+		ch->cmt->info->write_control(ch->iostart, 0, value);
+	else
+		ch->cmt->info->write_control(ch->cmt->mapbase, 0, value);
 }
 
-static inline unsigned long sh_cmt_read_cmcnt(struct sh_cmt_priv *p)
+static inline unsigned long sh_cmt_read_cmcsr(struct sh_cmt_channel *ch)
 {
-	return p->read_count(p->mapbase, CMCNT);
+	return ch->cmt->info->read_control(ch->ioctrl, CMCSR);
 }
 
-static inline void sh_cmt_write_cmstr(struct sh_cmt_priv *p,
+static inline void sh_cmt_write_cmcsr(struct sh_cmt_channel *ch,
 				      unsigned long value)
 {
-	p->write_control(p->mapbase_str, 0, value);
+	ch->cmt->info->write_control(ch->ioctrl, CMCSR, value);
 }
 
-static inline void sh_cmt_write_cmcsr(struct sh_cmt_priv *p,
-				      unsigned long value)
+static inline unsigned long sh_cmt_read_cmcnt(struct sh_cmt_channel *ch)
 {
-	p->write_control(p->mapbase, CMCSR, value);
+	return ch->cmt->info->read_count(ch->ioctrl, CMCNT);
 }
 
-static inline void sh_cmt_write_cmcnt(struct sh_cmt_priv *p,
+static inline void sh_cmt_write_cmcnt(struct sh_cmt_channel *ch,
 				      unsigned long value)
 {
-	p->write_count(p->mapbase, CMCNT, value);
+	ch->cmt->info->write_count(ch->ioctrl, CMCNT, value);
 }
 
-static inline void sh_cmt_write_cmcor(struct sh_cmt_priv *p,
+static inline void sh_cmt_write_cmcor(struct sh_cmt_channel *ch,
 				      unsigned long value)
 {
-	p->write_count(p->mapbase, CMCOR, value);
+	ch->cmt->info->write_count(ch->ioctrl, CMCOR, value);
 }
 
-static unsigned long sh_cmt_get_counter(struct sh_cmt_priv *p,
+static unsigned long sh_cmt_get_counter(struct sh_cmt_channel *ch,
 					int *has_wrapped)
 {
 	unsigned long v1, v2, v3;
 	int o1, o2;
 
-	o1 = sh_cmt_read_cmcsr(p) & p->overflow_bit;
+	o1 = sh_cmt_read_cmcsr(ch) & ch->cmt->info->overflow_bit;
 
 	/* Make sure the timer value is stable. Stolen from acpi_pm.c */
 	do {
 		o2 = o1;
-		v1 = sh_cmt_read_cmcnt(p);
-		v2 = sh_cmt_read_cmcnt(p);
-		v3 = sh_cmt_read_cmcnt(p);
-		o1 = sh_cmt_read_cmcsr(p) & p->overflow_bit;
+		v1 = sh_cmt_read_cmcnt(ch);
+		v2 = sh_cmt_read_cmcnt(ch);
+		v3 = sh_cmt_read_cmcnt(ch);
+		o1 = sh_cmt_read_cmcsr(ch) & ch->cmt->info->overflow_bit;
 	} while (unlikely((o1 != o2) || (v1 > v2 && v1 < v3)
 			  || (v2 > v3 && v2 < v1) || (v3 > v1 && v3 < v2)));
 
@@ -177,52 +303,56 @@ static unsigned long sh_cmt_get_counter(struct sh_cmt_priv *p,
 
 static DEFINE_RAW_SPINLOCK(sh_cmt_lock);
 
-static void sh_cmt_start_stop_ch(struct sh_cmt_priv *p, int start)
+static void sh_cmt_start_stop_ch(struct sh_cmt_channel *ch, int start)
 {
-	struct sh_timer_config *cfg = p->pdev->dev.platform_data;
 	unsigned long flags, value;
 
 	/* start stop register shared by multiple timer channels */
 	raw_spin_lock_irqsave(&sh_cmt_lock, flags);
-	value = sh_cmt_read_cmstr(p);
+	value = sh_cmt_read_cmstr(ch);
 
 	if (start)
-		value |= 1 << cfg->timer_bit;
+		value |= 1 << ch->timer_bit;
 	else
-		value &= ~(1 << cfg->timer_bit);
+		value &= ~(1 << ch->timer_bit);
 
-	sh_cmt_write_cmstr(p, value);
+	sh_cmt_write_cmstr(ch, value);
 	raw_spin_unlock_irqrestore(&sh_cmt_lock, flags);
 }
 
-static int sh_cmt_enable(struct sh_cmt_priv *p, unsigned long *rate)
+static int sh_cmt_enable(struct sh_cmt_channel *ch, unsigned long *rate)
 {
 	int k, ret;
 
-	pm_runtime_get_sync(&p->pdev->dev);
-	dev_pm_syscore_device(&p->pdev->dev, true);
+	pm_runtime_get_sync(&ch->cmt->pdev->dev);
+	dev_pm_syscore_device(&ch->cmt->pdev->dev, true);
 
 	/* enable clock */
-	ret = clk_enable(p->clk);
+	ret = clk_enable(ch->cmt->clk);
 	if (ret) {
-		dev_err(&p->pdev->dev, "cannot enable clock\n");
+		dev_err(&ch->cmt->pdev->dev, "ch%u: cannot enable clock\n",
+			ch->index);
 		goto err0;
 	}
 
 	/* make sure channel is disabled */
-	sh_cmt_start_stop_ch(p, 0);
+	sh_cmt_start_stop_ch(ch, 0);
 
 	/* configure channel, periodic mode and maximum timeout */
-	if (p->width == 16) {
-		*rate = clk_get_rate(p->clk) / 512;
-		sh_cmt_write_cmcsr(p, 0x43);
+	if (ch->cmt->info->width == 16) {
+		*rate = clk_get_rate(ch->cmt->clk) / 512;
+		sh_cmt_write_cmcsr(ch, SH_CMT16_CMCSR_CMIE |
+				   SH_CMT16_CMCSR_CKS512);
 	} else {
-		*rate = clk_get_rate(p->clk) / 8;
-		sh_cmt_write_cmcsr(p, 0x01a4);
+		*rate = clk_get_rate(ch->cmt->clk) / 8;
+		sh_cmt_write_cmcsr(ch, SH_CMT32_CMCSR_CMM |
+				   SH_CMT32_CMCSR_CMTOUT_IE |
+				   SH_CMT32_CMCSR_CMR_IRQ |
+				   SH_CMT32_CMCSR_CKS_RCLK8);
 	}
 
-	sh_cmt_write_cmcor(p, 0xffffffff);
-	sh_cmt_write_cmcnt(p, 0);
+	sh_cmt_write_cmcor(ch, 0xffffffff);
+	sh_cmt_write_cmcnt(ch, 0);
 
 	/*
 	 * According to the sh73a0 user's manual, as CMCNT can be operated
@@ -236,41 +366,42 @@ static int sh_cmt_enable(struct sh_cmt_priv *p, unsigned long *rate)
 	 * take RCLKx2 at maximum.
 	 */
 	for (k = 0; k < 100; k++) {
-		if (!sh_cmt_read_cmcnt(p))
+		if (!sh_cmt_read_cmcnt(ch))
 			break;
 		udelay(1);
 	}
 
-	if (sh_cmt_read_cmcnt(p)) {
-		dev_err(&p->pdev->dev, "cannot clear CMCNT\n");
+	if (sh_cmt_read_cmcnt(ch)) {
+		dev_err(&ch->cmt->pdev->dev, "ch%u: cannot clear CMCNT\n",
+			ch->index);
 		ret = -ETIMEDOUT;
 		goto err1;
 	}
 
 	/* enable channel */
-	sh_cmt_start_stop_ch(p, 1);
+	sh_cmt_start_stop_ch(ch, 1);
 	return 0;
  err1:
 	/* stop clock */
-	clk_disable(p->clk);
+	clk_disable(ch->cmt->clk);
 
  err0:
 	return ret;
 }
 
-static void sh_cmt_disable(struct sh_cmt_priv *p)
+static void sh_cmt_disable(struct sh_cmt_channel *ch)
 {
 	/* disable channel */
-	sh_cmt_start_stop_ch(p, 0);
+	sh_cmt_start_stop_ch(ch, 0);
 
 	/* disable interrupts in CMT block */
-	sh_cmt_write_cmcsr(p, 0);
+	sh_cmt_write_cmcsr(ch, 0);
 
 	/* stop clock */
-	clk_disable(p->clk);
+	clk_disable(ch->cmt->clk);
 
-	dev_pm_syscore_device(&p->pdev->dev, false);
-	pm_runtime_put(&p->pdev->dev);
+	dev_pm_syscore_device(&ch->cmt->pdev->dev, false);
+	pm_runtime_put(&ch->cmt->pdev->dev);
 }
 
 /* private flags */
@@ -280,24 +411,24 @@ static void sh_cmt_disable(struct sh_cmt_priv *p)
 #define FLAG_SKIPEVENT (1 << 3)
 #define FLAG_IRQCONTEXT (1 << 4)
 
-static void sh_cmt_clock_event_program_verify(struct sh_cmt_priv *p,
+static void sh_cmt_clock_event_program_verify(struct sh_cmt_channel *ch,
 					      int absolute)
 {
 	unsigned long new_match;
-	unsigned long value = p->next_match_value;
+	unsigned long value = ch->next_match_value;
 	unsigned long delay = 0;
 	unsigned long now = 0;
 	int has_wrapped;
 
-	now = sh_cmt_get_counter(p, &has_wrapped);
-	p->flags |= FLAG_REPROGRAM; /* force reprogram */
+	now = sh_cmt_get_counter(ch, &has_wrapped);
+	ch->flags |= FLAG_REPROGRAM; /* force reprogram */
 
 	if (has_wrapped) {
 		/* we're competing with the interrupt handler.
 		 *  -> let the interrupt handler reprogram the timer.
 		 *  -> interrupt number two handles the event.
 		 */
-		p->flags |= FLAG_SKIPEVENT;
+		ch->flags |= FLAG_SKIPEVENT;
 		return;
 	}
 
@@ -309,20 +440,20 @@ static void sh_cmt_clock_event_program_verify(struct sh_cmt_priv *p,
 		 * but don't save the new match value yet.
 		 */
 		new_match = now + value + delay;
-		if (new_match > p->max_match_value)
-			new_match = p->max_match_value;
+		if (new_match > ch->max_match_value)
+			new_match = ch->max_match_value;
 
-		sh_cmt_write_cmcor(p, new_match);
+		sh_cmt_write_cmcor(ch, new_match);
 
-		now = sh_cmt_get_counter(p, &has_wrapped);
-		if (has_wrapped && (new_match > p->match_value)) {
+		now = sh_cmt_get_counter(ch, &has_wrapped);
+		if (has_wrapped && (new_match > ch->match_value)) {
 			/* we are changing to a greater match value,
 			 * so this wrap must be caused by the counter
 			 * matching the old value.
 			 * -> first interrupt reprograms the timer.
 			 * -> interrupt number two handles the event.
 			 */
-			p->flags |= FLAG_SKIPEVENT;
+			ch->flags |= FLAG_SKIPEVENT;
 			break;
 		}
 
@@ -333,7 +464,7 @@ static void sh_cmt_clock_event_program_verify(struct sh_cmt_priv *p,
 			 * -> save programmed match value.
 			 * -> let isr handle the event.
 			 */
-			p->match_value = new_match;
+			ch->match_value = new_match;
 			break;
 		}
 
@@ -344,7 +475,7 @@ static void sh_cmt_clock_event_program_verify(struct sh_cmt_priv *p,
 			 * -> save programmed match value.
 			 * -> let isr handle the event.
 			 */
-			p->match_value = new_match;
+			ch->match_value = new_match;
 			break;
 		}
 
@@ -360,138 +491,141 @@ static void sh_cmt_clock_event_program_verify(struct sh_cmt_priv *p,
 			delay = 1;
 
 		if (!delay)
-			dev_warn(&p->pdev->dev, "too long delay\n");
+			dev_warn(&ch->cmt->pdev->dev, "ch%u: too long delay\n",
+				 ch->index);
 
 	} while (delay);
 }
 
-static void __sh_cmt_set_next(struct sh_cmt_priv *p, unsigned long delta)
+static void __sh_cmt_set_next(struct sh_cmt_channel *ch, unsigned long delta)
 {
-	if (delta > p->max_match_value)
-		dev_warn(&p->pdev->dev, "delta out of range\n");
+	if (delta > ch->max_match_value)
+		dev_warn(&ch->cmt->pdev->dev, "ch%u: delta out of range\n",
+			 ch->index);
 
-	p->next_match_value = delta;
-	sh_cmt_clock_event_program_verify(p, 0);
+	ch->next_match_value = delta;
+	sh_cmt_clock_event_program_verify(ch, 0);
 }
 
-static void sh_cmt_set_next(struct sh_cmt_priv *p, unsigned long delta)
+static void sh_cmt_set_next(struct sh_cmt_channel *ch, unsigned long delta)
 {
 	unsigned long flags;
 
-	raw_spin_lock_irqsave(&p->lock, flags);
-	__sh_cmt_set_next(p, delta);
-	raw_spin_unlock_irqrestore(&p->lock, flags);
+	raw_spin_lock_irqsave(&ch->lock, flags);
+	__sh_cmt_set_next(ch, delta);
+	raw_spin_unlock_irqrestore(&ch->lock, flags);
 }
 
 static irqreturn_t sh_cmt_interrupt(int irq, void *dev_id)
 {
-	struct sh_cmt_priv *p = dev_id;
+	struct sh_cmt_channel *ch = dev_id;
 
 	/* clear flags */
-	sh_cmt_write_cmcsr(p, sh_cmt_read_cmcsr(p) & p->clear_bits);
+	sh_cmt_write_cmcsr(ch, sh_cmt_read_cmcsr(ch) &
+			   ch->cmt->info->clear_bits);
 
 	/* update clock source counter to begin with if enabled
 	 * the wrap flag should be cleared by the timer specific
 	 * isr before we end up here.
 	 */
-	if (p->flags & FLAG_CLOCKSOURCE)
-		p->total_cycles += p->match_value + 1;
+	if (ch->flags & FLAG_CLOCKSOURCE)
+		ch->total_cycles += ch->match_value + 1;
 
-	if (!(p->flags & FLAG_REPROGRAM))
-		p->next_match_value = p->max_match_value;
+	if (!(ch->flags & FLAG_REPROGRAM))
+		ch->next_match_value = ch->max_match_value;
 
-	p->flags |= FLAG_IRQCONTEXT;
+	ch->flags |= FLAG_IRQCONTEXT;
 
-	if (p->flags & FLAG_CLOCKEVENT) {
-		if (!(p->flags & FLAG_SKIPEVENT)) {
-			if (p->ced.mode == CLOCK_EVT_MODE_ONESHOT) {
-				p->next_match_value = p->max_match_value;
-				p->flags |= FLAG_REPROGRAM;
+	if (ch->flags & FLAG_CLOCKEVENT) {
+		if (!(ch->flags & FLAG_SKIPEVENT)) {
+			if (ch->ced.mode == CLOCK_EVT_MODE_ONESHOT) {
+				ch->next_match_value = ch->max_match_value;
+				ch->flags |= FLAG_REPROGRAM;
 			}
 
-			p->ced.event_handler(&p->ced);
+			ch->ced.event_handler(&ch->ced);
 		}
 	}
 
-	p->flags &= ~FLAG_SKIPEVENT;
+	ch->flags &= ~FLAG_SKIPEVENT;
 
-	if (p->flags & FLAG_REPROGRAM) {
-		p->flags &= ~FLAG_REPROGRAM;
-		sh_cmt_clock_event_program_verify(p, 1);
+	if (ch->flags & FLAG_REPROGRAM) {
+		ch->flags &= ~FLAG_REPROGRAM;
+		sh_cmt_clock_event_program_verify(ch, 1);
 
-		if (p->flags & FLAG_CLOCKEVENT)
-			if ((p->ced.mode == CLOCK_EVT_MODE_SHUTDOWN)
-			    || (p->match_value == p->next_match_value))
-				p->flags &= ~FLAG_REPROGRAM;
+		if (ch->flags & FLAG_CLOCKEVENT)
+			if ((ch->ced.mode == CLOCK_EVT_MODE_SHUTDOWN)
+			    || (ch->match_value == ch->next_match_value))
+				ch->flags &= ~FLAG_REPROGRAM;
 	}
 
-	p->flags &= ~FLAG_IRQCONTEXT;
+	ch->flags &= ~FLAG_IRQCONTEXT;
 
 	return IRQ_HANDLED;
 }
 
-static int sh_cmt_start(struct sh_cmt_priv *p, unsigned long flag)
+static int sh_cmt_start(struct sh_cmt_channel *ch, unsigned long flag)
 {
 	int ret = 0;
 	unsigned long flags;
 
-	raw_spin_lock_irqsave(&p->lock, flags);
+	raw_spin_lock_irqsave(&ch->lock, flags);
 
-	if (!(p->flags & (FLAG_CLOCKEVENT | FLAG_CLOCKSOURCE)))
-		ret = sh_cmt_enable(p, &p->rate);
+	if (!(ch->flags & (FLAG_CLOCKEVENT | FLAG_CLOCKSOURCE)))
+		ret = sh_cmt_enable(ch, &ch->rate);
 
 	if (ret)
 		goto out;
-	p->flags |= flag;
+	ch->flags |= flag;
 
 	/* setup timeout if no clockevent */
-	if ((flag == FLAG_CLOCKSOURCE) && (!(p->flags & FLAG_CLOCKEVENT)))
-		__sh_cmt_set_next(p, p->max_match_value);
+	if ((flag == FLAG_CLOCKSOURCE) && (!(ch->flags & FLAG_CLOCKEVENT)))
+		__sh_cmt_set_next(ch, ch->max_match_value);
  out:
-	raw_spin_unlock_irqrestore(&p->lock, flags);
+	raw_spin_unlock_irqrestore(&ch->lock, flags);
 
 	return ret;
 }
 
-static void sh_cmt_stop(struct sh_cmt_priv *p, unsigned long flag)
+static void sh_cmt_stop(struct sh_cmt_channel *ch, unsigned long flag)
 {
 	unsigned long flags;
 	unsigned long f;
 
-	raw_spin_lock_irqsave(&p->lock, flags);
+	raw_spin_lock_irqsave(&ch->lock, flags);
 
-	f = p->flags & (FLAG_CLOCKEVENT | FLAG_CLOCKSOURCE);
-	p->flags &= ~flag;
+	f = ch->flags & (FLAG_CLOCKEVENT | FLAG_CLOCKSOURCE);
+	ch->flags &= ~flag;
 
-	if (f && !(p->flags & (FLAG_CLOCKEVENT | FLAG_CLOCKSOURCE)))
-		sh_cmt_disable(p);
+	if (f && !(ch->flags & (FLAG_CLOCKEVENT | FLAG_CLOCKSOURCE)))
+		sh_cmt_disable(ch);
 
 	/* adjust the timeout to maximum if only clocksource left */
-	if ((flag == FLAG_CLOCKEVENT) && (p->flags & FLAG_CLOCKSOURCE))
-		__sh_cmt_set_next(p, p->max_match_value);
+	if ((flag == FLAG_CLOCKEVENT) && (ch->flags & FLAG_CLOCKSOURCE))
+		__sh_cmt_set_next(ch, ch->max_match_value);
 
-	raw_spin_unlock_irqrestore(&p->lock, flags);
+	raw_spin_unlock_irqrestore(&ch->lock, flags);
 }
 
-static struct sh_cmt_priv *cs_to_sh_cmt(struct clocksource *cs)
+static struct sh_cmt_channel *cs_to_sh_cmt(struct clocksource *cs)
 {
-	return container_of(cs, struct sh_cmt_priv, cs);
+	return container_of(cs, struct sh_cmt_channel, cs);
 }
 
 static cycle_t sh_cmt_clocksource_read(struct clocksource *cs)
 {
-	struct sh_cmt_priv *p = cs_to_sh_cmt(cs);
+	struct sh_cmt_channel *ch = cs_to_sh_cmt(cs);
 	unsigned long flags, raw;
 	unsigned long value;
 	int has_wrapped;
 
-	raw_spin_lock_irqsave(&p->lock, flags);
-	value = p->total_cycles;
-	raw = sh_cmt_get_counter(p, &has_wrapped);
+	raw_spin_lock_irqsave(&ch->lock, flags);
+	value = ch->total_cycles;
+	raw = sh_cmt_get_counter(ch, &has_wrapped);
 
 	if (unlikely(has_wrapped))
-		raw += p->match_value + 1;
-	raw_spin_unlock_irqrestore(&p->lock, flags);
+		raw += ch->match_value + 1;
+	raw_spin_unlock_irqrestore(&ch->lock, flags);
 
 	return value + raw;
 }
@@ -499,53 +633,53 @@ static cycle_t sh_cmt_clocksource_read(struct clocksource *cs)
 static int sh_cmt_clocksource_enable(struct clocksource *cs)
 {
 	int ret;
-	struct sh_cmt_priv *p = cs_to_sh_cmt(cs);
+	struct sh_cmt_channel *ch = cs_to_sh_cmt(cs);
 
-	WARN_ON(p->cs_enabled);
+	WARN_ON(ch->cs_enabled);
 
-	p->total_cycles = 0;
+	ch->total_cycles = 0;
 
-	ret = sh_cmt_start(p, FLAG_CLOCKSOURCE);
+	ret = sh_cmt_start(ch, FLAG_CLOCKSOURCE);
 	if (!ret) {
-		__clocksource_updatefreq_hz(cs, p->rate);
-		p->cs_enabled = true;
+		__clocksource_updatefreq_hz(cs, ch->rate);
+		ch->cs_enabled = true;
 	}
 	return ret;
 }
 
 static void sh_cmt_clocksource_disable(struct clocksource *cs)
 {
-	struct sh_cmt_priv *p = cs_to_sh_cmt(cs);
+	struct sh_cmt_channel *ch = cs_to_sh_cmt(cs);
 
-	WARN_ON(!p->cs_enabled);
+	WARN_ON(!ch->cs_enabled);
 
-	sh_cmt_stop(p, FLAG_CLOCKSOURCE);
-	p->cs_enabled = false;
+	sh_cmt_stop(ch, FLAG_CLOCKSOURCE);
+	ch->cs_enabled = false;
 }
 
 static void sh_cmt_clocksource_suspend(struct clocksource *cs)
 {
-	struct sh_cmt_priv *p = cs_to_sh_cmt(cs);
+	struct sh_cmt_channel *ch = cs_to_sh_cmt(cs);
 
-	sh_cmt_stop(p, FLAG_CLOCKSOURCE);
-	pm_genpd_syscore_poweroff(&p->pdev->dev);
+	sh_cmt_stop(ch, FLAG_CLOCKSOURCE);
+	pm_genpd_syscore_poweroff(&ch->cmt->pdev->dev);
 }
 
 static void sh_cmt_clocksource_resume(struct clocksource *cs)
 {
-	struct sh_cmt_priv *p = cs_to_sh_cmt(cs);
+	struct sh_cmt_channel *ch = cs_to_sh_cmt(cs);
 
-	pm_genpd_syscore_poweron(&p->pdev->dev);
-	sh_cmt_start(p, FLAG_CLOCKSOURCE);
+	pm_genpd_syscore_poweron(&ch->cmt->pdev->dev);
+	sh_cmt_start(ch, FLAG_CLOCKSOURCE);
 }
 
-static int sh_cmt_register_clocksource(struct sh_cmt_priv *p,
-				       char *name, unsigned long rating)
+static int sh_cmt_register_clocksource(struct sh_cmt_channel *ch,
+				       const char *name)
 {
-	struct clocksource *cs = &p->cs;
+	struct clocksource *cs = &ch->cs;
 
 	cs->name = name;
-	cs->rating = rating;
+	cs->rating = 125;
 	cs->read = sh_cmt_clocksource_read;
 	cs->enable = sh_cmt_clocksource_enable;
 	cs->disable = sh_cmt_clocksource_disable;
@@ -554,47 +688,48 @@ static int sh_cmt_register_clocksource(struct sh_cmt_priv *p,
 	cs->mask = CLOCKSOURCE_MASK(sizeof(unsigned long) * 8);
 	cs->flags = CLOCK_SOURCE_IS_CONTINUOUS;
 
-	dev_info(&p->pdev->dev, "used as clock source\n");
+	dev_info(&ch->cmt->pdev->dev, "ch%u: used as clock source\n",
+		 ch->index);
 
 	/* Register with dummy 1 Hz value, gets updated in ->enable() */
 	clocksource_register_hz(cs, 1);
 	return 0;
 }
 
-static struct sh_cmt_priv *ced_to_sh_cmt(struct clock_event_device *ced)
+static struct sh_cmt_channel *ced_to_sh_cmt(struct clock_event_device *ced)
 {
-	return container_of(ced, struct sh_cmt_priv, ced);
+	return container_of(ced, struct sh_cmt_channel, ced);
 }
 
-static void sh_cmt_clock_event_start(struct sh_cmt_priv *p, int periodic)
+static void sh_cmt_clock_event_start(struct sh_cmt_channel *ch, int periodic)
 {
-	struct clock_event_device *ced = &p->ced;
+	struct clock_event_device *ced = &ch->ced;
 
-	sh_cmt_start(p, FLAG_CLOCKEVENT);
+	sh_cmt_start(ch, FLAG_CLOCKEVENT);
 
 	/* TODO: calculate good shift from rate and counter bit width */
 
 	ced->shift = 32;
-	ced->mult = div_sc(p->rate, NSEC_PER_SEC, ced->shift);
-	ced->max_delta_ns = clockevent_delta2ns(p->max_match_value, ced);
+	ced->mult = div_sc(ch->rate, NSEC_PER_SEC, ced->shift);
+	ced->max_delta_ns = clockevent_delta2ns(ch->max_match_value, ced);
 	ced->min_delta_ns = clockevent_delta2ns(0x1f, ced);
 
 	if (periodic)
-		sh_cmt_set_next(p, ((p->rate + HZ/2) / HZ) - 1);
+		sh_cmt_set_next(ch, ((ch->rate + HZ/2) / HZ) - 1);
 	else
-		sh_cmt_set_next(p, p->max_match_value);
+		sh_cmt_set_next(ch, ch->max_match_value);
 }
 
 static void sh_cmt_clock_event_mode(enum clock_event_mode mode,
 				    struct clock_event_device *ced)
 {
-	struct sh_cmt_priv *p = ced_to_sh_cmt(ced);
+	struct sh_cmt_channel *ch = ced_to_sh_cmt(ced);
 
 	/* deal with old setting first */
 	switch (ced->mode) {
 	case CLOCK_EVT_MODE_PERIODIC:
 	case CLOCK_EVT_MODE_ONESHOT:
-		sh_cmt_stop(p, FLAG_CLOCKEVENT);
+		sh_cmt_stop(ch, FLAG_CLOCKEVENT);
 		break;
 	default:
 		break;
@@ -602,16 +737,18 @@ static void sh_cmt_clock_event_mode(enum clock_event_mode mode,
 
 	switch (mode) {
 	case CLOCK_EVT_MODE_PERIODIC:
-		dev_info(&p->pdev->dev, "used for periodic clock events\n");
-		sh_cmt_clock_event_start(p, 1);
+		dev_info(&ch->cmt->pdev->dev,
+			 "ch%u: used for periodic clock events\n", ch->index);
+		sh_cmt_clock_event_start(ch, 1);
 		break;
 	case CLOCK_EVT_MODE_ONESHOT:
-		dev_info(&p->pdev->dev, "used for oneshot clock events\n");
-		sh_cmt_clock_event_start(p, 0);
+		dev_info(&ch->cmt->pdev->dev,
+			 "ch%u: used for oneshot clock events\n", ch->index);
+		sh_cmt_clock_event_start(ch, 0);
 		break;
 	case CLOCK_EVT_MODE_SHUTDOWN:
 	case CLOCK_EVT_MODE_UNUSED:
-		sh_cmt_stop(p, FLAG_CLOCKEVENT);
+		sh_cmt_stop(ch, FLAG_CLOCKEVENT);
 		break;
 	default:
 		break;
@@ -621,196 +758,341 @@ static void sh_cmt_clock_event_mode(enum clock_event_mode mode,
 static int sh_cmt_clock_event_next(unsigned long delta,
 				   struct clock_event_device *ced)
 {
-	struct sh_cmt_priv *p = ced_to_sh_cmt(ced);
+	struct sh_cmt_channel *ch = ced_to_sh_cmt(ced);
 
 	BUG_ON(ced->mode != CLOCK_EVT_MODE_ONESHOT);
-	if (likely(p->flags & FLAG_IRQCONTEXT))
-		p->next_match_value = delta - 1;
+	if (likely(ch->flags & FLAG_IRQCONTEXT))
+		ch->next_match_value = delta - 1;
 	else
-		sh_cmt_set_next(p, delta - 1);
+		sh_cmt_set_next(ch, delta - 1);
 
 	return 0;
 }
 
 static void sh_cmt_clock_event_suspend(struct clock_event_device *ced)
 {
-	struct sh_cmt_priv *p = ced_to_sh_cmt(ced);
+	struct sh_cmt_channel *ch = ced_to_sh_cmt(ced);
 
-	pm_genpd_syscore_poweroff(&p->pdev->dev);
-	clk_unprepare(p->clk);
+	pm_genpd_syscore_poweroff(&ch->cmt->pdev->dev);
+	clk_unprepare(ch->cmt->clk);
 }
 
 static void sh_cmt_clock_event_resume(struct clock_event_device *ced)
 {
-	struct sh_cmt_priv *p = ced_to_sh_cmt(ced);
+	struct sh_cmt_channel *ch = ced_to_sh_cmt(ced);
 
-	clk_prepare(p->clk);
-	pm_genpd_syscore_poweron(&p->pdev->dev);
+	clk_prepare(ch->cmt->clk);
+	pm_genpd_syscore_poweron(&ch->cmt->pdev->dev);
 }
 
-static void sh_cmt_register_clockevent(struct sh_cmt_priv *p,
-				       char *name, unsigned long rating)
+static int sh_cmt_register_clockevent(struct sh_cmt_channel *ch,
+				      const char *name)
 {
-	struct clock_event_device *ced = &p->ced;
+	struct clock_event_device *ced = &ch->ced;
+	int irq;
+	int ret;
 
-	memset(ced, 0, sizeof(*ced));
+	irq = platform_get_irq(ch->cmt->pdev, ch->cmt->legacy ? 0 : ch->index);
+	if (irq < 0) {
+		dev_err(&ch->cmt->pdev->dev, "ch%u: failed to get irq\n",
+			ch->index);
+		return irq;
+	}
+
+	ret = request_irq(irq, sh_cmt_interrupt,
+			  IRQF_TIMER | IRQF_IRQPOLL | IRQF_NOBALANCING,
+			  dev_name(&ch->cmt->pdev->dev), ch);
+	if (ret) {
+		dev_err(&ch->cmt->pdev->dev, "ch%u: failed to request irq %d\n",
+			ch->index, irq);
+		return ret;
+	}
 
 	ced->name = name;
 	ced->features = CLOCK_EVT_FEAT_PERIODIC;
 	ced->features |= CLOCK_EVT_FEAT_ONESHOT;
-	ced->rating = rating;
-	ced->cpumask = cpumask_of(0);
+	ced->rating = 125;
+	ced->cpumask = cpu_possible_mask;
 	ced->set_next_event = sh_cmt_clock_event_next;
 	ced->set_mode = sh_cmt_clock_event_mode;
 	ced->suspend = sh_cmt_clock_event_suspend;
 	ced->resume = sh_cmt_clock_event_resume;
 
-	dev_info(&p->pdev->dev, "used for clock events\n");
+	dev_info(&ch->cmt->pdev->dev, "ch%u: used for clock events\n",
+		 ch->index);
 	clockevents_register_device(ced);
+
+	return 0;
 }
 
-static int sh_cmt_register(struct sh_cmt_priv *p, char *name,
-			   unsigned long clockevent_rating,
-			   unsigned long clocksource_rating)
+static int sh_cmt_register(struct sh_cmt_channel *ch, const char *name,
+			   bool clockevent, bool clocksource)
 {
-	if (clockevent_rating)
-		sh_cmt_register_clockevent(p, name, clockevent_rating);
+	int ret;
+
+	if (clockevent) {
+		ch->cmt->has_clockevent = true;
+		ret = sh_cmt_register_clockevent(ch, name);
+		if (ret < 0)
+			return ret;
+	}
 
-	if (clocksource_rating)
-		sh_cmt_register_clocksource(p, name, clocksource_rating);
+	if (clocksource) {
+		ch->cmt->has_clocksource = true;
+		sh_cmt_register_clocksource(ch, name);
+	}
 
 	return 0;
 }
 
-static int sh_cmt_setup(struct sh_cmt_priv *p, struct platform_device *pdev)
+static int sh_cmt_setup_channel(struct sh_cmt_channel *ch, unsigned int index,
+				unsigned int hwidx, bool clockevent,
+				bool clocksource, struct sh_cmt_device *cmt)
 {
-	struct sh_timer_config *cfg = pdev->dev.platform_data;
-	struct resource *res, *res2;
-	int irq, ret;
-	ret = -ENXIO;
+	int ret;
 
-	memset(p, 0, sizeof(*p));
-	p->pdev = pdev;
+	/* Skip unused channels. */
+	if (!clockevent && !clocksource)
+		return 0;
 
-	if (!cfg) {
-		dev_err(&p->pdev->dev, "missing platform data\n");
-		goto err0;
+	ch->cmt = cmt;
+	ch->index = index;
+	ch->hwidx = hwidx;
+
+	/*
+	 * Compute the address of the channel control register block. For the
+	 * timers with a per-channel start/stop register, compute its address
+	 * as well.
+	 *
+	 * For legacy configuration the address has been mapped explicitly.
+	 */
+	if (cmt->legacy) {
+		ch->ioctrl = cmt->mapbase_ch;
+	} else {
+		switch (cmt->info->model) {
+		case SH_CMT_16BIT:
+			ch->ioctrl = cmt->mapbase + 2 + ch->hwidx * 6;
+			break;
+		case SH_CMT_32BIT:
+		case SH_CMT_48BIT:
+			ch->ioctrl = cmt->mapbase + 0x10 + ch->hwidx * 0x10;
+			break;
+		case SH_CMT_32BIT_FAST:
+			/*
+			 * The 32-bit "fast" timer has a single channel at hwidx
+			 * 5 but is located at offset 0x40 instead of 0x60 for
+			 * some reason.
+			 */
+			ch->ioctrl = cmt->mapbase + 0x40;
+			break;
+		case SH_CMT_48BIT_GEN2:
+			ch->iostart = cmt->mapbase + ch->hwidx * 0x100;
+			ch->ioctrl = ch->iostart + 0x10;
+			break;
+		}
 	}
 
-	res = platform_get_resource(p->pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&p->pdev->dev, "failed to get I/O memory\n");
-		goto err0;
+	if (cmt->info->width == (sizeof(ch->max_match_value) * 8))
+		ch->max_match_value = ~0;
+	else
+		ch->max_match_value = (1 << cmt->info->width) - 1;
+
+	ch->match_value = ch->max_match_value;
+	raw_spin_lock_init(&ch->lock);
+
+	if (cmt->legacy) {
+		ch->timer_bit = ch->hwidx;
+	} else {
+		ch->timer_bit = cmt->info->model == SH_CMT_48BIT_GEN2
+			      ? 0 : ch->hwidx;
 	}
 
-	/* optional resource for the shared timer start/stop register */
-	res2 = platform_get_resource(p->pdev, IORESOURCE_MEM, 1);
+	ret = sh_cmt_register(ch, dev_name(&cmt->pdev->dev),
+			      clockevent, clocksource);
+	if (ret) {
+		dev_err(&cmt->pdev->dev, "ch%u: registration failed\n",
+			ch->index);
+		return ret;
+	}
+	ch->cs_enabled = false;
 
-	irq = platform_get_irq(p->pdev, 0);
-	if (irq < 0) {
-		dev_err(&p->pdev->dev, "failed to get irq\n");
-		goto err0;
+	return 0;
+}
+
+static int sh_cmt_map_memory(struct sh_cmt_device *cmt)
+{
+	struct resource *mem;
+
+	mem = platform_get_resource(cmt->pdev, IORESOURCE_MEM, 0);
+	if (!mem) {
+		dev_err(&cmt->pdev->dev, "failed to get I/O memory\n");
+		return -ENXIO;
 	}
 
-	/* map memory, let mapbase point to our channel */
-	p->mapbase = ioremap_nocache(res->start, resource_size(res));
-	if (p->mapbase == NULL) {
-		dev_err(&p->pdev->dev, "failed to remap I/O memory\n");
-		goto err0;
+	cmt->mapbase = ioremap_nocache(mem->start, resource_size(mem));
+	if (cmt->mapbase == NULL) {
+		dev_err(&cmt->pdev->dev, "failed to remap I/O memory\n");
+		return -ENXIO;
 	}
 
-	/* map second resource for CMSTR */
-	p->mapbase_str = ioremap_nocache(res2 ? res2->start :
-					 res->start - cfg->channel_offset,
-					 res2 ? resource_size(res2) : 2);
-	if (p->mapbase_str == NULL) {
-		dev_err(&p->pdev->dev, "failed to remap I/O second memory\n");
-		goto err1;
+	return 0;
+}
+
+static int sh_cmt_map_memory_legacy(struct sh_cmt_device *cmt)
+{
+	struct sh_timer_config *cfg = cmt->pdev->dev.platform_data;
+	struct resource *res, *res2;
+
+	/* map memory, let mapbase_ch point to our channel */
+	res = platform_get_resource(cmt->pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&cmt->pdev->dev, "failed to get I/O memory\n");
+		return -ENXIO;
 	}
 
-	/* request irq using setup_irq() (too early for request_irq()) */
-	p->irqaction.name = dev_name(&p->pdev->dev);
-	p->irqaction.handler = sh_cmt_interrupt;
-	p->irqaction.dev_id = p;
-	p->irqaction.flags = IRQF_TIMER | IRQF_IRQPOLL | IRQF_NOBALANCING;
-
-	/* get hold of clock */
-	p->clk = clk_get(&p->pdev->dev, "cmt_fck");
-	if (IS_ERR(p->clk)) {
-		dev_err(&p->pdev->dev, "cannot get clock\n");
-		ret = PTR_ERR(p->clk);
-		goto err2;
+	cmt->mapbase_ch = ioremap_nocache(res->start, resource_size(res));
+	if (cmt->mapbase_ch == NULL) {
+		dev_err(&cmt->pdev->dev, "failed to remap I/O memory\n");
+		return -ENXIO;
 	}
 
-	ret = clk_prepare(p->clk);
-	if (ret < 0)
-		goto err3;
+	/* optional resource for the shared timer start/stop register */
+	res2 = platform_get_resource(cmt->pdev, IORESOURCE_MEM, 1);
 
-	if (res2 && (resource_size(res2) == 4)) {
-		/* assume both CMSTR and CMCSR to be 32-bit */
-		p->read_control = sh_cmt_read32;
-		p->write_control = sh_cmt_write32;
-	} else {
-		p->read_control = sh_cmt_read16;
-		p->write_control = sh_cmt_write16;
+	/* map second resource for CMSTR */
+	cmt->mapbase = ioremap_nocache(res2 ? res2->start :
+				       res->start - cfg->channel_offset,
+				       res2 ? resource_size(res2) : 2);
+	if (cmt->mapbase == NULL) {
+		dev_err(&cmt->pdev->dev, "failed to remap I/O second memory\n");
+		iounmap(cmt->mapbase_ch);
+		return -ENXIO;
 	}
 
-	if (resource_size(res) == 6) {
-		p->width = 16;
-		p->read_count = sh_cmt_read16;
-		p->write_count = sh_cmt_write16;
-		p->overflow_bit = 0x80;
-		p->clear_bits = ~0x80;
-	} else {
-		p->width = 32;
-		p->read_count = sh_cmt_read32;
-		p->write_count = sh_cmt_write32;
-		p->overflow_bit = 0x8000;
-		p->clear_bits = ~0xc000;
+	/* identify the model based on the resources */
+	if (resource_size(res) == 6)
+		cmt->info = &sh_cmt_info[SH_CMT_16BIT];
+	else if (res2 && (resource_size(res2) == 4))
+		cmt->info = &sh_cmt_info[SH_CMT_48BIT_GEN2];
+	else
+		cmt->info = &sh_cmt_info[SH_CMT_32BIT];
+
+	return 0;
+}
+
+static void sh_cmt_unmap_memory(struct sh_cmt_device *cmt)
+{
+	iounmap(cmt->mapbase);
+	if (cmt->mapbase_ch)
+		iounmap(cmt->mapbase_ch);
+}
+
+static int sh_cmt_setup(struct sh_cmt_device *cmt, struct platform_device *pdev)
+{
+	struct sh_timer_config *cfg = pdev->dev.platform_data;
+	const struct platform_device_id *id = pdev->id_entry;
+	unsigned int hw_channels;
+	int ret;
+
+	memset(cmt, 0, sizeof(*cmt));
+	cmt->pdev = pdev;
+
+	if (!cfg) {
+		dev_err(&cmt->pdev->dev, "missing platform data\n");
+		return -ENXIO;
 	}
 
-	if (p->width == (sizeof(p->max_match_value) * 8))
-		p->max_match_value = ~0;
+	cmt->info = (const struct sh_cmt_info *)id->driver_data;
+	cmt->legacy = cmt->info ? false : true;
+
+	/* Get hold of clock. */
+	cmt->clk = clk_get(&cmt->pdev->dev, cmt->legacy ? "cmt_fck" : "fck");
+	if (IS_ERR(cmt->clk)) {
+		dev_err(&cmt->pdev->dev, "cannot get clock\n");
+		return PTR_ERR(cmt->clk);
+	}
+
+	ret = clk_prepare(cmt->clk);
+	if (ret < 0)
+		goto err_clk_put;
+
+	/*
+	 * Map the memory resource(s). We need to support both the legacy
+	 * platform device configuration (with one device per channel) and the
+	 * new version (with multiple channels per device).
+	 */
+	if (cmt->legacy)
+		ret = sh_cmt_map_memory_legacy(cmt);
 	else
-		p->max_match_value = (1 << p->width) - 1;
+		ret = sh_cmt_map_memory(cmt);
 
-	p->match_value = p->max_match_value;
-	raw_spin_lock_init(&p->lock);
+	if (ret < 0)
+		goto err_clk_unprepare;
 
-	ret = sh_cmt_register(p, (char *)dev_name(&p->pdev->dev),
-			      cfg->clockevent_rating,
-			      cfg->clocksource_rating);
-	if (ret) {
-		dev_err(&p->pdev->dev, "registration failed\n");
-		goto err4;
+	/* Allocate and setup the channels. */
+	if (cmt->legacy) {
+		cmt->num_channels = 1;
+		hw_channels = 0;
+	} else {
+		cmt->num_channels = hweight8(cfg->channels_mask);
+		hw_channels = cfg->channels_mask;
 	}
-	p->cs_enabled = false;
 
-	ret = setup_irq(irq, &p->irqaction);
-	if (ret) {
-		dev_err(&p->pdev->dev, "failed to request irq %d\n", irq);
-		goto err4;
+	cmt->channels = kzalloc(cmt->num_channels * sizeof(*cmt->channels),
+				GFP_KERNEL);
+	if (cmt->channels == NULL) {
+		ret = -ENOMEM;
+		goto err_unmap;
 	}
 
-	platform_set_drvdata(pdev, p);
+	if (cmt->legacy) {
+		ret = sh_cmt_setup_channel(&cmt->channels[0],
+					   cfg->timer_bit, cfg->timer_bit,
+					   cfg->clockevent_rating != 0,
+					   cfg->clocksource_rating != 0, cmt);
+		if (ret < 0)
+			goto err_unmap;
+	} else {
+		unsigned int mask = hw_channels;
+		unsigned int i;
+
+		/*
+		 * Use the first channel as a clock event device and the second
+		 * channel as a clock source. If only one channel is available
+		 * use it for both.
+		 */
+		for (i = 0; i < cmt->num_channels; ++i) {
+			unsigned int hwidx = ffs(mask) - 1;
+			bool clocksource = i == 1 || cmt->num_channels == 1;
+			bool clockevent = i == 0;
+
+			ret = sh_cmt_setup_channel(&cmt->channels[i], i, hwidx,
+						   clockevent, clocksource,
+						   cmt);
+			if (ret < 0)
+				goto err_unmap;
+
+			mask &= ~(1 << hwidx);
+		}
+	}
+
+	platform_set_drvdata(pdev, cmt);
 
 	return 0;
-err4:
-	clk_unprepare(p->clk);
-err3:
-	clk_put(p->clk);
-err2:
-	iounmap(p->mapbase_str);
-err1:
-	iounmap(p->mapbase);
-err0:
+
+err_unmap:
+	kfree(cmt->channels);
+	sh_cmt_unmap_memory(cmt);
+err_clk_unprepare:
+	clk_unprepare(cmt->clk);
+err_clk_put:
+	clk_put(cmt->clk);
 	return ret;
 }
 
 static int sh_cmt_probe(struct platform_device *pdev)
 {
-	struct sh_cmt_priv *p = platform_get_drvdata(pdev);
-	struct sh_timer_config *cfg = pdev->dev.platform_data;
+	struct sh_cmt_device *cmt = platform_get_drvdata(pdev);
 	int ret;
 
 	if (!is_early_platform_device(pdev)) {
@@ -818,20 +1100,20 @@ static int sh_cmt_probe(struct platform_device *pdev)
 		pm_runtime_enable(&pdev->dev);
 	}
 
-	if (p) {
+	if (cmt) {
 		dev_info(&pdev->dev, "kept as earlytimer\n");
 		goto out;
 	}
 
-	p = kmalloc(sizeof(*p), GFP_KERNEL);
-	if (p == NULL) {
+	cmt = kzalloc(sizeof(*cmt), GFP_KERNEL);
+	if (cmt == NULL) {
 		dev_err(&pdev->dev, "failed to allocate driver data\n");
 		return -ENOMEM;
 	}
 
-	ret = sh_cmt_setup(p, pdev);
+	ret = sh_cmt_setup(cmt, pdev);
 	if (ret) {
-		kfree(p);
+		kfree(cmt);
 		pm_runtime_idle(&pdev->dev);
 		return ret;
 	}
@@ -839,7 +1121,7 @@ static int sh_cmt_probe(struct platform_device *pdev)
 		return 0;
 
  out:
-	if (cfg->clockevent_rating || cfg->clocksource_rating)
+	if (cmt->has_clockevent || cmt->has_clocksource)
 		pm_runtime_irq_safe(&pdev->dev);
 	else
 		pm_runtime_idle(&pdev->dev);
@@ -852,12 +1134,24 @@ static int sh_cmt_remove(struct platform_device *pdev)
 	return -EBUSY; /* cannot unregister clockevent and clocksource */
 }
 
+static const struct platform_device_id sh_cmt_id_table[] = {
+	{ "sh_cmt", 0 },
+	{ "sh-cmt-16", (kernel_ulong_t)&sh_cmt_info[SH_CMT_16BIT] },
+	{ "sh-cmt-32", (kernel_ulong_t)&sh_cmt_info[SH_CMT_32BIT] },
+	{ "sh-cmt-32-fast", (kernel_ulong_t)&sh_cmt_info[SH_CMT_32BIT_FAST] },
+	{ "sh-cmt-48", (kernel_ulong_t)&sh_cmt_info[SH_CMT_48BIT] },
+	{ "sh-cmt-48-gen2", (kernel_ulong_t)&sh_cmt_info[SH_CMT_48BIT_GEN2] },
+	{ }
+};
+MODULE_DEVICE_TABLE(platform, sh_cmt_id_table);
+
 static struct platform_driver sh_cmt_device_driver = {
 	.probe		= sh_cmt_probe,
 	.remove		= sh_cmt_remove,
 	.driver		= {
 		.name	= "sh_cmt",
-	}
+	},
+	.id_table	= sh_cmt_id_table,
 };
 
 static int __init sh_cmt_init(void)
diff --git a/drivers/clocksource/sh_mtu2.c b/drivers/clocksource/sh_mtu2.c
index e30d76e0a6f..f2c1c36139e 100644
--- a/drivers/clocksource/sh_mtu2.c
+++ b/drivers/clocksource/sh_mtu2.c
@@ -11,37 +11,48 @@
  * 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/clockchips.h>
+#include <linux/delay.h>
+#include <linux/err.h>
 #include <linux/init.h>
-#include <linux/platform_device.h>
-#include <linux/spinlock.h>
 #include <linux/interrupt.h>
-#include <linux/ioport.h>
-#include <linux/delay.h>
 #include <linux/io.h>
-#include <linux/clk.h>
+#include <linux/ioport.h>
 #include <linux/irq.h>
-#include <linux/err.h>
-#include <linux/clockchips.h>
-#include <linux/sh_timer.h>
-#include <linux/slab.h>
 #include <linux/module.h>
+#include <linux/platform_device.h>
 #include <linux/pm_domain.h>
 #include <linux/pm_runtime.h>
+#include <linux/sh_timer.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+
+struct sh_mtu2_device;
+
+struct sh_mtu2_channel {
+	struct sh_mtu2_device *mtu;
+	unsigned int index;
+
+	void __iomem *base;
+	int irq;
+
+	struct clock_event_device ced;
+};
+
+struct sh_mtu2_device {
+	struct platform_device *pdev;
 
-struct sh_mtu2_priv {
 	void __iomem *mapbase;
 	struct clk *clk;
-	struct irqaction irqaction;
-	struct platform_device *pdev;
-	unsigned long rate;
-	unsigned long periodic;
-	struct clock_event_device ced;
+
+	struct sh_mtu2_channel *channels;
+	unsigned int num_channels;
+
+	bool legacy;
+	bool has_clockevent;
 };
 
 static DEFINE_RAW_SPINLOCK(sh_mtu2_lock);
@@ -55,6 +66,88 @@ static DEFINE_RAW_SPINLOCK(sh_mtu2_lock);
 #define TCNT 5 /* channel register */
 #define TGR  6 /* channel register */
 
+#define TCR_CCLR_NONE		(0 << 5)
+#define TCR_CCLR_TGRA		(1 << 5)
+#define TCR_CCLR_TGRB		(2 << 5)
+#define TCR_CCLR_SYNC		(3 << 5)
+#define TCR_CCLR_TGRC		(5 << 5)
+#define TCR_CCLR_TGRD		(6 << 5)
+#define TCR_CCLR_MASK		(7 << 5)
+#define TCR_CKEG_RISING		(0 << 3)
+#define TCR_CKEG_FALLING	(1 << 3)
+#define TCR_CKEG_BOTH		(2 << 3)
+#define TCR_CKEG_MASK		(3 << 3)
+/* Values 4 to 7 are channel-dependent */
+#define TCR_TPSC_P1		(0 << 0)
+#define TCR_TPSC_P4		(1 << 0)
+#define TCR_TPSC_P16		(2 << 0)
+#define TCR_TPSC_P64		(3 << 0)
+#define TCR_TPSC_CH0_TCLKA	(4 << 0)
+#define TCR_TPSC_CH0_TCLKB	(5 << 0)
+#define TCR_TPSC_CH0_TCLKC	(6 << 0)
+#define TCR_TPSC_CH0_TCLKD	(7 << 0)
+#define TCR_TPSC_CH1_TCLKA	(4 << 0)
+#define TCR_TPSC_CH1_TCLKB	(5 << 0)
+#define TCR_TPSC_CH1_P256	(6 << 0)
+#define TCR_TPSC_CH1_TCNT2	(7 << 0)
+#define TCR_TPSC_CH2_TCLKA	(4 << 0)
+#define TCR_TPSC_CH2_TCLKB	(5 << 0)
+#define TCR_TPSC_CH2_TCLKC	(6 << 0)
+#define TCR_TPSC_CH2_P1024	(7 << 0)
+#define TCR_TPSC_CH34_P256	(4 << 0)
+#define TCR_TPSC_CH34_P1024	(5 << 0)
+#define TCR_TPSC_CH34_TCLKA	(6 << 0)
+#define TCR_TPSC_CH34_TCLKB	(7 << 0)
+#define TCR_TPSC_MASK		(7 << 0)
+
+#define TMDR_BFE		(1 << 6)
+#define TMDR_BFB		(1 << 5)
+#define TMDR_BFA		(1 << 4)
+#define TMDR_MD_NORMAL		(0 << 0)
+#define TMDR_MD_PWM_1		(2 << 0)
+#define TMDR_MD_PWM_2		(3 << 0)
+#define TMDR_MD_PHASE_1		(4 << 0)
+#define TMDR_MD_PHASE_2		(5 << 0)
+#define TMDR_MD_PHASE_3		(6 << 0)
+#define TMDR_MD_PHASE_4		(7 << 0)
+#define TMDR_MD_PWM_SYNC	(8 << 0)
+#define TMDR_MD_PWM_COMP_CREST	(13 << 0)
+#define TMDR_MD_PWM_COMP_TROUGH	(14 << 0)
+#define TMDR_MD_PWM_COMP_BOTH	(15 << 0)
+#define TMDR_MD_MASK		(15 << 0)
+
+#define TIOC_IOCH(n)		((n) << 4)
+#define TIOC_IOCL(n)		((n) << 0)
+#define TIOR_OC_RETAIN		(0 << 0)
+#define TIOR_OC_0_CLEAR		(1 << 0)
+#define TIOR_OC_0_SET		(2 << 0)
+#define TIOR_OC_0_TOGGLE	(3 << 0)
+#define TIOR_OC_1_CLEAR		(5 << 0)
+#define TIOR_OC_1_SET		(6 << 0)
+#define TIOR_OC_1_TOGGLE	(7 << 0)
+#define TIOR_IC_RISING		(8 << 0)
+#define TIOR_IC_FALLING		(9 << 0)
+#define TIOR_IC_BOTH		(10 << 0)
+#define TIOR_IC_TCNT		(12 << 0)
+#define TIOR_MASK		(15 << 0)
+
+#define TIER_TTGE		(1 << 7)
+#define TIER_TTGE2		(1 << 6)
+#define TIER_TCIEU		(1 << 5)
+#define TIER_TCIEV		(1 << 4)
+#define TIER_TGIED		(1 << 3)
+#define TIER_TGIEC		(1 << 2)
+#define TIER_TGIEB		(1 << 1)
+#define TIER_TGIEA		(1 << 0)
+
+#define TSR_TCFD		(1 << 7)
+#define TSR_TCFU		(1 << 5)
+#define TSR_TCFV		(1 << 4)
+#define TSR_TGFD		(1 << 3)
+#define TSR_TGFC		(1 << 2)
+#define TSR_TGFB		(1 << 1)
+#define TSR_TGFA		(1 << 0)
+
 static unsigned long mtu2_reg_offs[] = {
 	[TCR] = 0,
 	[TMDR] = 1,
@@ -65,135 +158,143 @@ static unsigned long mtu2_reg_offs[] = {
 	[TGR] = 8,
 };
 
-static inline unsigned long sh_mtu2_read(struct sh_mtu2_priv *p, int reg_nr)
+static inline unsigned long sh_mtu2_read(struct sh_mtu2_channel *ch, int reg_nr)
 {
-	struct sh_timer_config *cfg = p->pdev->dev.platform_data;
-	void __iomem *base = p->mapbase;
 	unsigned long offs;
 
-	if (reg_nr == TSTR)
-		return ioread8(base + cfg->channel_offset);
+	if (reg_nr == TSTR) {
+		if (ch->mtu->legacy)
+			return ioread8(ch->mtu->mapbase);
+		else
+			return ioread8(ch->mtu->mapbase + 0x280);
+	}
 
 	offs = mtu2_reg_offs[reg_nr];
 
 	if ((reg_nr == TCNT) || (reg_nr == TGR))
-		return ioread16(base + offs);
+		return ioread16(ch->base + offs);
 	else
-		return ioread8(base + offs);
+		return ioread8(ch->base + offs);
 }
 
-static inline void sh_mtu2_write(struct sh_mtu2_priv *p, int reg_nr,
+static inline void sh_mtu2_write(struct sh_mtu2_channel *ch, int reg_nr,
 				unsigned long value)
 {
-	struct sh_timer_config *cfg = p->pdev->dev.platform_data;
-	void __iomem *base = p->mapbase;
 	unsigned long offs;
 
 	if (reg_nr == TSTR) {
-		iowrite8(value, base + cfg->channel_offset);
-		return;
+		if (ch->mtu->legacy)
+			return iowrite8(value, ch->mtu->mapbase);
+		else
+			return iowrite8(value, ch->mtu->mapbase + 0x280);
 	}
 
 	offs = mtu2_reg_offs[reg_nr];
 
 	if ((reg_nr == TCNT) || (reg_nr == TGR))
-		iowrite16(value, base + offs);
+		iowrite16(value, ch->base + offs);
 	else
-		iowrite8(value, base + offs);
+		iowrite8(value, ch->base + offs);
 }
 
-static void sh_mtu2_start_stop_ch(struct sh_mtu2_priv *p, int start)
+static void sh_mtu2_start_stop_ch(struct sh_mtu2_channel *ch, int start)
 {
-	struct sh_timer_config *cfg = p->pdev->dev.platform_data;
 	unsigned long flags, value;
 
 	/* start stop register shared by multiple timer channels */
 	raw_spin_lock_irqsave(&sh_mtu2_lock, flags);
-	value = sh_mtu2_read(p, TSTR);
+	value = sh_mtu2_read(ch, TSTR);
 
 	if (start)
-		value |= 1 << cfg->timer_bit;
+		value |= 1 << ch->index;
 	else
-		value &= ~(1 << cfg->timer_bit);
+		value &= ~(1 << ch->index);
 
-	sh_mtu2_write(p, TSTR, value);
+	sh_mtu2_write(ch, TSTR, value);
 	raw_spin_unlock_irqrestore(&sh_mtu2_lock, flags);
 }
 
-static int sh_mtu2_enable(struct sh_mtu2_priv *p)
+static int sh_mtu2_enable(struct sh_mtu2_channel *ch)
 {
+	unsigned long periodic;
+	unsigned long rate;
 	int ret;
 
-	pm_runtime_get_sync(&p->pdev->dev);
-	dev_pm_syscore_device(&p->pdev->dev, true);
+	pm_runtime_get_sync(&ch->mtu->pdev->dev);
+	dev_pm_syscore_device(&ch->mtu->pdev->dev, true);
 
 	/* enable clock */
-	ret = clk_enable(p->clk);
+	ret = clk_enable(ch->mtu->clk);
 	if (ret) {
-		dev_err(&p->pdev->dev, "cannot enable clock\n");
+		dev_err(&ch->mtu->pdev->dev, "ch%u: cannot enable clock\n",
+			ch->index);
 		return ret;
 	}
 
 	/* make sure channel is disabled */
-	sh_mtu2_start_stop_ch(p, 0);
-
-	p->rate = clk_get_rate(p->clk) / 64;
-	p->periodic = (p->rate + HZ/2) / HZ;
-
-	/* "Periodic Counter Operation" */
-	sh_mtu2_write(p, TCR, 0x23); /* TGRA clear, divide clock by 64 */
-	sh_mtu2_write(p, TIOR, 0);
-	sh_mtu2_write(p, TGR, p->periodic);
-	sh_mtu2_write(p, TCNT, 0);
-	sh_mtu2_write(p, TMDR, 0);
-	sh_mtu2_write(p, TIER, 0x01);
+	sh_mtu2_start_stop_ch(ch, 0);
+
+	rate = clk_get_rate(ch->mtu->clk) / 64;
+	periodic = (rate + HZ/2) / HZ;
+
+	/*
+	 * "Periodic Counter Operation"
+	 * Clear on TGRA compare match, divide clock by 64.
+	 */
+	sh_mtu2_write(ch, TCR, TCR_CCLR_TGRA | TCR_TPSC_P64);
+	sh_mtu2_write(ch, TIOR, TIOC_IOCH(TIOR_OC_0_CLEAR) |
+		      TIOC_IOCL(TIOR_OC_0_CLEAR));
+	sh_mtu2_write(ch, TGR, periodic);
+	sh_mtu2_write(ch, TCNT, 0);
+	sh_mtu2_write(ch, TMDR, TMDR_MD_NORMAL);
+	sh_mtu2_write(ch, TIER, TIER_TGIEA);
 
 	/* enable channel */
-	sh_mtu2_start_stop_ch(p, 1);
+	sh_mtu2_start_stop_ch(ch, 1);
 
 	return 0;
 }
 
-static void sh_mtu2_disable(struct sh_mtu2_priv *p)
+static void sh_mtu2_disable(struct sh_mtu2_channel *ch)
 {
 	/* disable channel */
-	sh_mtu2_start_stop_ch(p, 0);
+	sh_mtu2_start_stop_ch(ch, 0);
 
 	/* stop clock */
-	clk_disable(p->clk);
+	clk_disable(ch->mtu->clk);
 
-	dev_pm_syscore_device(&p->pdev->dev, false);
-	pm_runtime_put(&p->pdev->dev);
+	dev_pm_syscore_device(&ch->mtu->pdev->dev, false);
+	pm_runtime_put(&ch->mtu->pdev->dev);
 }
 
 static irqreturn_t sh_mtu2_interrupt(int irq, void *dev_id)
 {
-	struct sh_mtu2_priv *p = dev_id;
+	struct sh_mtu2_channel *ch = dev_id;
 
 	/* acknowledge interrupt */
-	sh_mtu2_read(p, TSR);
-	sh_mtu2_write(p, TSR, 0xfe);
+	sh_mtu2_read(ch, TSR);
+	sh_mtu2_write(ch, TSR, ~TSR_TGFA);
 
 	/* notify clockevent layer */
-	p->ced.event_handler(&p->ced);
+	ch->ced.event_handler(&ch->ced);
 	return IRQ_HANDLED;
 }
 
-static struct sh_mtu2_priv *ced_to_sh_mtu2(struct clock_event_device *ced)
+static struct sh_mtu2_channel *ced_to_sh_mtu2(struct clock_event_device *ced)
 {
-	return container_of(ced, struct sh_mtu2_priv, ced);
+	return container_of(ced, struct sh_mtu2_channel, ced);
 }
 
 static void sh_mtu2_clock_event_mode(enum clock_event_mode mode,
 				    struct clock_event_device *ced)
 {
-	struct sh_mtu2_priv *p = ced_to_sh_mtu2(ced);
+	struct sh_mtu2_channel *ch = ced_to_sh_mtu2(ced);
 	int disabled = 0;
 
 	/* deal with old setting first */
 	switch (ced->mode) {
 	case CLOCK_EVT_MODE_PERIODIC:
-		sh_mtu2_disable(p);
+		sh_mtu2_disable(ch);
 		disabled = 1;
 		break;
 	default:
@@ -202,12 +303,13 @@ static void sh_mtu2_clock_event_mode(enum clock_event_mode mode,
 
 	switch (mode) {
 	case CLOCK_EVT_MODE_PERIODIC:
-		dev_info(&p->pdev->dev, "used for periodic clock events\n");
-		sh_mtu2_enable(p);
+		dev_info(&ch->mtu->pdev->dev,
+			 "ch%u: used for periodic clock events\n", ch->index);
+		sh_mtu2_enable(ch);
 		break;
 	case CLOCK_EVT_MODE_UNUSED:
 		if (!disabled)
-			sh_mtu2_disable(p);
+			sh_mtu2_disable(ch);
 		break;
 	case CLOCK_EVT_MODE_SHUTDOWN:
 	default:
@@ -217,125 +319,207 @@ static void sh_mtu2_clock_event_mode(enum clock_event_mode mode,
 
 static void sh_mtu2_clock_event_suspend(struct clock_event_device *ced)
 {
-	pm_genpd_syscore_poweroff(&ced_to_sh_mtu2(ced)->pdev->dev);
+	pm_genpd_syscore_poweroff(&ced_to_sh_mtu2(ced)->mtu->pdev->dev);
 }
 
 static void sh_mtu2_clock_event_resume(struct clock_event_device *ced)
 {
-	pm_genpd_syscore_poweron(&ced_to_sh_mtu2(ced)->pdev->dev);
+	pm_genpd_syscore_poweron(&ced_to_sh_mtu2(ced)->mtu->pdev->dev);
 }
 
-static void sh_mtu2_register_clockevent(struct sh_mtu2_priv *p,
-				       char *name, unsigned long rating)
+static void sh_mtu2_register_clockevent(struct sh_mtu2_channel *ch,
+					const char *name)
 {
-	struct clock_event_device *ced = &p->ced;
+	struct clock_event_device *ced = &ch->ced;
 	int ret;
 
-	memset(ced, 0, sizeof(*ced));
-
 	ced->name = name;
 	ced->features = CLOCK_EVT_FEAT_PERIODIC;
-	ced->rating = rating;
-	ced->cpumask = cpumask_of(0);
+	ced->rating = 200;
+	ced->cpumask = cpu_possible_mask;
 	ced->set_mode = sh_mtu2_clock_event_mode;
 	ced->suspend = sh_mtu2_clock_event_suspend;
 	ced->resume = sh_mtu2_clock_event_resume;
 
-	dev_info(&p->pdev->dev, "used for clock events\n");
+	dev_info(&ch->mtu->pdev->dev, "ch%u: used for clock events\n",
+		 ch->index);
 	clockevents_register_device(ced);
 
-	ret = setup_irq(p->irqaction.irq, &p->irqaction);
+	ret = request_irq(ch->irq, sh_mtu2_interrupt,
+			  IRQF_TIMER | IRQF_IRQPOLL | IRQF_NOBALANCING,
+			  dev_name(&ch->mtu->pdev->dev), ch);
 	if (ret) {
-		dev_err(&p->pdev->dev, "failed to request irq %d\n",
-			p->irqaction.irq);
+		dev_err(&ch->mtu->pdev->dev, "ch%u: failed to request irq %d\n",
+			ch->index, ch->irq);
 		return;
 	}
 }
 
-static int sh_mtu2_register(struct sh_mtu2_priv *p, char *name,
-			    unsigned long clockevent_rating)
+static int sh_mtu2_register(struct sh_mtu2_channel *ch, const char *name,
+			    bool clockevent)
 {
-	if (clockevent_rating)
-		sh_mtu2_register_clockevent(p, name, clockevent_rating);
+	if (clockevent) {
+		ch->mtu->has_clockevent = true;
+		sh_mtu2_register_clockevent(ch, name);
+	}
 
 	return 0;
 }
 
-static int sh_mtu2_setup(struct sh_mtu2_priv *p, struct platform_device *pdev)
+static int sh_mtu2_setup_channel(struct sh_mtu2_channel *ch, unsigned int index,
+				 struct sh_mtu2_device *mtu)
 {
-	struct sh_timer_config *cfg = pdev->dev.platform_data;
-	struct resource *res;
-	int irq, ret;
-	ret = -ENXIO;
+	static const unsigned int channel_offsets[] = {
+		0x300, 0x380, 0x000,
+	};
+	bool clockevent;
+
+	ch->mtu = mtu;
+
+	if (mtu->legacy) {
+		struct sh_timer_config *cfg = mtu->pdev->dev.platform_data;
+
+		clockevent = cfg->clockevent_rating != 0;
 
-	memset(p, 0, sizeof(*p));
-	p->pdev = pdev;
+		ch->irq = platform_get_irq(mtu->pdev, 0);
+		ch->base = mtu->mapbase - cfg->channel_offset;
+		ch->index = cfg->timer_bit;
+	} else {
+		char name[6];
 
-	if (!cfg) {
-		dev_err(&p->pdev->dev, "missing platform data\n");
-		goto err0;
+		clockevent = true;
+
+		sprintf(name, "tgi%ua", index);
+		ch->irq = platform_get_irq_byname(mtu->pdev, name);
+		ch->base = mtu->mapbase + channel_offsets[index];
+		ch->index = index;
 	}
 
-	platform_set_drvdata(pdev, p);
+	if (ch->irq < 0) {
+		/* Skip channels with no declared interrupt. */
+		if (!mtu->legacy)
+			return 0;
+
+		dev_err(&mtu->pdev->dev, "ch%u: failed to get irq\n",
+			ch->index);
+		return ch->irq;
+	}
+
+	return sh_mtu2_register(ch, dev_name(&mtu->pdev->dev), clockevent);
+}
 
-	res = platform_get_resource(p->pdev, IORESOURCE_MEM, 0);
+static int sh_mtu2_map_memory(struct sh_mtu2_device *mtu)
+{
+	struct resource *res;
+
+	res = platform_get_resource(mtu->pdev, IORESOURCE_MEM, 0);
 	if (!res) {
-		dev_err(&p->pdev->dev, "failed to get I/O memory\n");
-		goto err0;
+		dev_err(&mtu->pdev->dev, "failed to get I/O memory\n");
+		return -ENXIO;
 	}
 
-	irq = platform_get_irq(p->pdev, 0);
-	if (irq < 0) {
-		dev_err(&p->pdev->dev, "failed to get irq\n");
-		goto err0;
+	mtu->mapbase = ioremap_nocache(res->start, resource_size(res));
+	if (mtu->mapbase == NULL)
+		return -ENXIO;
+
+	/*
+	 * In legacy platform device configuration (with one device per channel)
+	 * the resource points to the channel base address.
+	 */
+	if (mtu->legacy) {
+		struct sh_timer_config *cfg = mtu->pdev->dev.platform_data;
+		mtu->mapbase += cfg->channel_offset;
 	}
 
-	/* map memory, let mapbase point to our channel */
-	p->mapbase = ioremap_nocache(res->start, resource_size(res));
-	if (p->mapbase == NULL) {
-		dev_err(&p->pdev->dev, "failed to remap I/O memory\n");
-		goto err0;
+	return 0;
+}
+
+static void sh_mtu2_unmap_memory(struct sh_mtu2_device *mtu)
+{
+	if (mtu->legacy) {
+		struct sh_timer_config *cfg = mtu->pdev->dev.platform_data;
+		mtu->mapbase -= cfg->channel_offset;
 	}
 
-	/* setup data for setup_irq() (too early for request_irq()) */
-	p->irqaction.name = dev_name(&p->pdev->dev);
-	p->irqaction.handler = sh_mtu2_interrupt;
-	p->irqaction.dev_id = p;
-	p->irqaction.irq = irq;
-	p->irqaction.flags = IRQF_TIMER | IRQF_IRQPOLL | IRQF_NOBALANCING;
-
-	/* get hold of clock */
-	p->clk = clk_get(&p->pdev->dev, "mtu2_fck");
-	if (IS_ERR(p->clk)) {
-		dev_err(&p->pdev->dev, "cannot get clock\n");
-		ret = PTR_ERR(p->clk);
-		goto err1;
+	iounmap(mtu->mapbase);
+}
+
+static int sh_mtu2_setup(struct sh_mtu2_device *mtu,
+			 struct platform_device *pdev)
+{
+	struct sh_timer_config *cfg = pdev->dev.platform_data;
+	const struct platform_device_id *id = pdev->id_entry;
+	unsigned int i;
+	int ret;
+
+	mtu->pdev = pdev;
+	mtu->legacy = id->driver_data;
+
+	if (mtu->legacy && !cfg) {
+		dev_err(&mtu->pdev->dev, "missing platform data\n");
+		return -ENXIO;
 	}
 
-	ret = clk_prepare(p->clk);
-	if (ret < 0)
-		goto err2;
+	/* Get hold of clock. */
+	mtu->clk = clk_get(&mtu->pdev->dev, mtu->legacy ? "mtu2_fck" : "fck");
+	if (IS_ERR(mtu->clk)) {
+		dev_err(&mtu->pdev->dev, "cannot get clock\n");
+		return PTR_ERR(mtu->clk);
+	}
 
-	ret = sh_mtu2_register(p, (char *)dev_name(&p->pdev->dev),
-			       cfg->clockevent_rating);
+	ret = clk_prepare(mtu->clk);
 	if (ret < 0)
-		goto err3;
+		goto err_clk_put;
+
+	/* Map the memory resource. */
+	ret = sh_mtu2_map_memory(mtu);
+	if (ret < 0) {
+		dev_err(&mtu->pdev->dev, "failed to remap I/O memory\n");
+		goto err_clk_unprepare;
+	}
+
+	/* Allocate and setup the channels. */
+	if (mtu->legacy)
+		mtu->num_channels = 1;
+	else
+		mtu->num_channels = 3;
+
+	mtu->channels = kzalloc(sizeof(*mtu->channels) * mtu->num_channels,
+				GFP_KERNEL);
+	if (mtu->channels == NULL) {
+		ret = -ENOMEM;
+		goto err_unmap;
+	}
+
+	if (mtu->legacy) {
+		ret = sh_mtu2_setup_channel(&mtu->channels[0], 0, mtu);
+		if (ret < 0)
+			goto err_unmap;
+	} else {
+		for (i = 0; i < mtu->num_channels; ++i) {
+			ret = sh_mtu2_setup_channel(&mtu->channels[i], i, mtu);
+			if (ret < 0)
+				goto err_unmap;
+		}
+	}
+
+	platform_set_drvdata(pdev, mtu);
 
 	return 0;
- err3:
-	clk_unprepare(p->clk);
- err2:
-	clk_put(p->clk);
- err1:
-	iounmap(p->mapbase);
- err0:
+
+err_unmap:
+	kfree(mtu->channels);
+	sh_mtu2_unmap_memory(mtu);
+err_clk_unprepare:
+	clk_unprepare(mtu->clk);
+err_clk_put:
+	clk_put(mtu->clk);
 	return ret;
 }
 
 static int sh_mtu2_probe(struct platform_device *pdev)
 {
-	struct sh_mtu2_priv *p = platform_get_drvdata(pdev);
-	struct sh_timer_config *cfg = pdev->dev.platform_data;
+	struct sh_mtu2_device *mtu = platform_get_drvdata(pdev);
 	int ret;
 
 	if (!is_early_platform_device(pdev)) {
@@ -343,20 +527,20 @@ static int sh_mtu2_probe(struct platform_device *pdev)
 		pm_runtime_enable(&pdev->dev);
 	}
 
-	if (p) {
+	if (mtu) {
 		dev_info(&pdev->dev, "kept as earlytimer\n");
 		goto out;
 	}
 
-	p = kmalloc(sizeof(*p), GFP_KERNEL);
-	if (p == NULL) {
+	mtu = kzalloc(sizeof(*mtu), GFP_KERNEL);
+	if (mtu == NULL) {
 		dev_err(&pdev->dev, "failed to allocate driver data\n");
 		return -ENOMEM;
 	}
 
-	ret = sh_mtu2_setup(p, pdev);
+	ret = sh_mtu2_setup(mtu, pdev);
 	if (ret) {
-		kfree(p);
+		kfree(mtu);
 		pm_runtime_idle(&pdev->dev);
 		return ret;
 	}
@@ -364,7 +548,7 @@ static int sh_mtu2_probe(struct platform_device *pdev)
 		return 0;
 
  out:
-	if (cfg->clockevent_rating)
+	if (mtu->has_clockevent)
 		pm_runtime_irq_safe(&pdev->dev);
 	else
 		pm_runtime_idle(&pdev->dev);
@@ -377,12 +561,20 @@ static int sh_mtu2_remove(struct platform_device *pdev)
 	return -EBUSY; /* cannot unregister clockevent */
 }
 
+static const struct platform_device_id sh_mtu2_id_table[] = {
+	{ "sh_mtu2", 1 },
+	{ "sh-mtu2", 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(platform, sh_mtu2_id_table);
+
 static struct platform_driver sh_mtu2_device_driver = {
 	.probe		= sh_mtu2_probe,
 	.remove		= sh_mtu2_remove,
 	.driver		= {
 		.name	= "sh_mtu2",
-	}
+	},
+	.id_table	= sh_mtu2_id_table,
 };
 
 static int __init sh_mtu2_init(void)
diff --git a/drivers/clocksource/sh_tmu.c b/drivers/clocksource/sh_tmu.c
index ecd7b60bfdf..4ba2c0fea58 100644
--- a/drivers/clocksource/sh_tmu.c
+++ b/drivers/clocksource/sh_tmu.c
@@ -11,35 +11,41 @@
  * 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/clockchips.h>
+#include <linux/clocksource.h>
+#include <linux/delay.h>
+#include <linux/err.h>
 #include <linux/init.h>
-#include <linux/platform_device.h>
-#include <linux/spinlock.h>
 #include <linux/interrupt.h>
-#include <linux/ioport.h>
-#include <linux/delay.h>
 #include <linux/io.h>
-#include <linux/clk.h>
+#include <linux/ioport.h>
 #include <linux/irq.h>
-#include <linux/err.h>
-#include <linux/clocksource.h>
-#include <linux/clockchips.h>
-#include <linux/sh_timer.h>
-#include <linux/slab.h>
 #include <linux/module.h>
+#include <linux/platform_device.h>
 #include <linux/pm_domain.h>
 #include <linux/pm_runtime.h>
+#include <linux/sh_timer.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+
+enum sh_tmu_model {
+	SH_TMU_LEGACY,
+	SH_TMU,
+	SH_TMU_SH3,
+};
+
+struct sh_tmu_device;
+
+struct sh_tmu_channel {
+	struct sh_tmu_device *tmu;
+	unsigned int index;
+
+	void __iomem *base;
+	int irq;
 
-struct sh_tmu_priv {
-	void __iomem *mapbase;
-	struct clk *clk;
-	struct irqaction irqaction;
-	struct platform_device *pdev;
 	unsigned long rate;
 	unsigned long periodic;
 	struct clock_event_device ced;
@@ -48,6 +54,21 @@ struct sh_tmu_priv {
 	unsigned int enable_count;
 };
 
+struct sh_tmu_device {
+	struct platform_device *pdev;
+
+	void __iomem *mapbase;
+	struct clk *clk;
+
+	enum sh_tmu_model model;
+
+	struct sh_tmu_channel *channels;
+	unsigned int num_channels;
+
+	bool has_clockevent;
+	bool has_clocksource;
+};
+
 static DEFINE_RAW_SPINLOCK(sh_tmu_lock);
 
 #define TSTR -1 /* shared register */
@@ -55,189 +76,208 @@ static DEFINE_RAW_SPINLOCK(sh_tmu_lock);
 #define TCNT 1 /* channel register */
 #define TCR 2 /* channel register */
 
-static inline unsigned long sh_tmu_read(struct sh_tmu_priv *p, int reg_nr)
+#define TCR_UNF			(1 << 8)
+#define TCR_UNIE		(1 << 5)
+#define TCR_TPSC_CLK4		(0 << 0)
+#define TCR_TPSC_CLK16		(1 << 0)
+#define TCR_TPSC_CLK64		(2 << 0)
+#define TCR_TPSC_CLK256		(3 << 0)
+#define TCR_TPSC_CLK1024	(4 << 0)
+#define TCR_TPSC_MASK		(7 << 0)
+
+static inline unsigned long sh_tmu_read(struct sh_tmu_channel *ch, int reg_nr)
 {
-	struct sh_timer_config *cfg = p->pdev->dev.platform_data;
-	void __iomem *base = p->mapbase;
 	unsigned long offs;
 
-	if (reg_nr == TSTR)
-		return ioread8(base - cfg->channel_offset);
+	if (reg_nr == TSTR) {
+		switch (ch->tmu->model) {
+		case SH_TMU_LEGACY:
+			return ioread8(ch->tmu->mapbase);
+		case SH_TMU_SH3:
+			return ioread8(ch->tmu->mapbase + 2);
+		case SH_TMU:
+			return ioread8(ch->tmu->mapbase + 4);
+		}
+	}
 
 	offs = reg_nr << 2;
 
 	if (reg_nr == TCR)
-		return ioread16(base + offs);
+		return ioread16(ch->base + offs);
 	else
-		return ioread32(base + offs);
+		return ioread32(ch->base + offs);
 }
 
-static inline void sh_tmu_write(struct sh_tmu_priv *p, int reg_nr,
+static inline void sh_tmu_write(struct sh_tmu_channel *ch, int reg_nr,
 				unsigned long value)
 {
-	struct sh_timer_config *cfg = p->pdev->dev.platform_data;
-	void __iomem *base = p->mapbase;
 	unsigned long offs;
 
 	if (reg_nr == TSTR) {
-		iowrite8(value, base - cfg->channel_offset);
-		return;
+		switch (ch->tmu->model) {
+		case SH_TMU_LEGACY:
+			return iowrite8(value, ch->tmu->mapbase);
+		case SH_TMU_SH3:
+			return iowrite8(value, ch->tmu->mapbase + 2);
+		case SH_TMU:
+			return iowrite8(value, ch->tmu->mapbase + 4);
+		}
 	}
 
 	offs = reg_nr << 2;
 
 	if (reg_nr == TCR)
-		iowrite16(value, base + offs);
+		iowrite16(value, ch->base + offs);
 	else
-		iowrite32(value, base + offs);
+		iowrite32(value, ch->base + offs);
 }
 
-static void sh_tmu_start_stop_ch(struct sh_tmu_priv *p, int start)
+static void sh_tmu_start_stop_ch(struct sh_tmu_channel *ch, int start)
 {
-	struct sh_timer_config *cfg = p->pdev->dev.platform_data;
 	unsigned long flags, value;
 
 	/* start stop register shared by multiple timer channels */
 	raw_spin_lock_irqsave(&sh_tmu_lock, flags);
-	value = sh_tmu_read(p, TSTR);
+	value = sh_tmu_read(ch, TSTR);
 
 	if (start)
-		value |= 1 << cfg->timer_bit;
+		value |= 1 << ch->index;
 	else
-		value &= ~(1 << cfg->timer_bit);
+		value &= ~(1 << ch->index);
 
-	sh_tmu_write(p, TSTR, value);
+	sh_tmu_write(ch, TSTR, value);
 	raw_spin_unlock_irqrestore(&sh_tmu_lock, flags);
 }
 
-static int __sh_tmu_enable(struct sh_tmu_priv *p)
+static int __sh_tmu_enable(struct sh_tmu_channel *ch)
 {
 	int ret;
 
 	/* enable clock */
-	ret = clk_enable(p->clk);
+	ret = clk_enable(ch->tmu->clk);
 	if (ret) {
-		dev_err(&p->pdev->dev, "cannot enable clock\n");
+		dev_err(&ch->tmu->pdev->dev, "ch%u: cannot enable clock\n",
+			ch->index);
 		return ret;
 	}
 
 	/* make sure channel is disabled */
-	sh_tmu_start_stop_ch(p, 0);
+	sh_tmu_start_stop_ch(ch, 0);
 
 	/* maximum timeout */
-	sh_tmu_write(p, TCOR, 0xffffffff);
-	sh_tmu_write(p, TCNT, 0xffffffff);
+	sh_tmu_write(ch, TCOR, 0xffffffff);
+	sh_tmu_write(ch, TCNT, 0xffffffff);
 
 	/* configure channel to parent clock / 4, irq off */
-	p->rate = clk_get_rate(p->clk) / 4;
-	sh_tmu_write(p, TCR, 0x0000);
+	ch->rate = clk_get_rate(ch->tmu->clk) / 4;
+	sh_tmu_write(ch, TCR, TCR_TPSC_CLK4);
 
 	/* enable channel */
-	sh_tmu_start_stop_ch(p, 1);
+	sh_tmu_start_stop_ch(ch, 1);
 
 	return 0;
 }
 
-static int sh_tmu_enable(struct sh_tmu_priv *p)
+static int sh_tmu_enable(struct sh_tmu_channel *ch)
 {
-	if (p->enable_count++ > 0)
+	if (ch->enable_count++ > 0)
 		return 0;
 
-	pm_runtime_get_sync(&p->pdev->dev);
-	dev_pm_syscore_device(&p->pdev->dev, true);
+	pm_runtime_get_sync(&ch->tmu->pdev->dev);
+	dev_pm_syscore_device(&ch->tmu->pdev->dev, true);
 
-	return __sh_tmu_enable(p);
+	return __sh_tmu_enable(ch);
 }
 
-static void __sh_tmu_disable(struct sh_tmu_priv *p)
+static void __sh_tmu_disable(struct sh_tmu_channel *ch)
 {
 	/* disable channel */
-	sh_tmu_start_stop_ch(p, 0);
+	sh_tmu_start_stop_ch(ch, 0);
 
 	/* disable interrupts in TMU block */
-	sh_tmu_write(p, TCR, 0x0000);
+	sh_tmu_write(ch, TCR, TCR_TPSC_CLK4);
 
 	/* stop clock */
-	clk_disable(p->clk);
+	clk_disable(ch->tmu->clk);
 }
 
-static void sh_tmu_disable(struct sh_tmu_priv *p)
+static void sh_tmu_disable(struct sh_tmu_channel *ch)
 {
-	if (WARN_ON(p->enable_count == 0))
+	if (WARN_ON(ch->enable_count == 0))
 		return;
 
-	if (--p->enable_count > 0)
+	if (--ch->enable_count > 0)
 		return;
 
-	__sh_tmu_disable(p);
+	__sh_tmu_disable(ch);
 
-	dev_pm_syscore_device(&p->pdev->dev, false);
-	pm_runtime_put(&p->pdev->dev);
+	dev_pm_syscore_device(&ch->tmu->pdev->dev, false);
+	pm_runtime_put(&ch->tmu->pdev->dev);
 }
 
-static void sh_tmu_set_next(struct sh_tmu_priv *p, unsigned long delta,
+static void sh_tmu_set_next(struct sh_tmu_channel *ch, unsigned long delta,
 			    int periodic)
 {
 	/* stop timer */
-	sh_tmu_start_stop_ch(p, 0);
+	sh_tmu_start_stop_ch(ch, 0);
 
 	/* acknowledge interrupt */
-	sh_tmu_read(p, TCR);
+	sh_tmu_read(ch, TCR);
 
 	/* enable interrupt */
-	sh_tmu_write(p, TCR, 0x0020);
+	sh_tmu_write(ch, TCR, TCR_UNIE | TCR_TPSC_CLK4);
 
 	/* reload delta value in case of periodic timer */
 	if (periodic)
-		sh_tmu_write(p, TCOR, delta);
+		sh_tmu_write(ch, TCOR, delta);
 	else
-		sh_tmu_write(p, TCOR, 0xffffffff);
+		sh_tmu_write(ch, TCOR, 0xffffffff);
 
-	sh_tmu_write(p, TCNT, delta);
+	sh_tmu_write(ch, TCNT, delta);
 
 	/* start timer */
-	sh_tmu_start_stop_ch(p, 1);
+	sh_tmu_start_stop_ch(ch, 1);
 }
 
 static irqreturn_t sh_tmu_interrupt(int irq, void *dev_id)
 {
-	struct sh_tmu_priv *p = dev_id;
+	struct sh_tmu_channel *ch = dev_id;
 
 	/* disable or acknowledge interrupt */
-	if (p->ced.mode == CLOCK_EVT_MODE_ONESHOT)
-		sh_tmu_write(p, TCR, 0x0000);
+	if (ch->ced.mode == CLOCK_EVT_MODE_ONESHOT)
+		sh_tmu_write(ch, TCR, TCR_TPSC_CLK4);
 	else
-		sh_tmu_write(p, TCR, 0x0020);
+		sh_tmu_write(ch, TCR, TCR_UNIE | TCR_TPSC_CLK4);
 
 	/* notify clockevent layer */
-	p->ced.event_handler(&p->ced);
+	ch->ced.event_handler(&ch->ced);
 	return IRQ_HANDLED;
 }
 
-static struct sh_tmu_priv *cs_to_sh_tmu(struct clocksource *cs)
+static struct sh_tmu_channel *cs_to_sh_tmu(struct clocksource *cs)
 {
-	return container_of(cs, struct sh_tmu_priv, cs);
+	return container_of(cs, struct sh_tmu_channel, cs);
 }
 
 static cycle_t sh_tmu_clocksource_read(struct clocksource *cs)
 {
-	struct sh_tmu_priv *p = cs_to_sh_tmu(cs);
+	struct sh_tmu_channel *ch = cs_to_sh_tmu(cs);
 
-	return sh_tmu_read(p, TCNT) ^ 0xffffffff;
+	return sh_tmu_read(ch, TCNT) ^ 0xffffffff;
 }
 
 static int sh_tmu_clocksource_enable(struct clocksource *cs)
 {
-	struct sh_tmu_priv *p = cs_to_sh_tmu(cs);
+	struct sh_tmu_channel *ch = cs_to_sh_tmu(cs);
 	int ret;
 
-	if (WARN_ON(p->cs_enabled))
+	if (WARN_ON(ch->cs_enabled))
 		return 0;
 
-	ret = sh_tmu_enable(p);
+	ret = sh_tmu_enable(ch);
 	if (!ret) {
-		__clocksource_updatefreq_hz(cs, p->rate);
-		p->cs_enabled = true;
+		__clocksource_updatefreq_hz(cs, ch->rate);
+		ch->cs_enabled = true;
 	}
 
 	return ret;
@@ -245,48 +285,48 @@ static int sh_tmu_clocksource_enable(struct clocksource *cs)
 
 static void sh_tmu_clocksource_disable(struct clocksource *cs)
 {
-	struct sh_tmu_priv *p = cs_to_sh_tmu(cs);
+	struct sh_tmu_channel *ch = cs_to_sh_tmu(cs);
 
-	if (WARN_ON(!p->cs_enabled))
+	if (WARN_ON(!ch->cs_enabled))
 		return;
 
-	sh_tmu_disable(p);
-	p->cs_enabled = false;
+	sh_tmu_disable(ch);
+	ch->cs_enabled = false;
 }
 
 static void sh_tmu_clocksource_suspend(struct clocksource *cs)
 {
-	struct sh_tmu_priv *p = cs_to_sh_tmu(cs);
+	struct sh_tmu_channel *ch = cs_to_sh_tmu(cs);
 
-	if (!p->cs_enabled)
+	if (!ch->cs_enabled)
 		return;
 
-	if (--p->enable_count == 0) {
-		__sh_tmu_disable(p);
-		pm_genpd_syscore_poweroff(&p->pdev->dev);
+	if (--ch->enable_count == 0) {
+		__sh_tmu_disable(ch);
+		pm_genpd_syscore_poweroff(&ch->tmu->pdev->dev);
 	}
 }
 
 static void sh_tmu_clocksource_resume(struct clocksource *cs)
 {
-	struct sh_tmu_priv *p = cs_to_sh_tmu(cs);
+	struct sh_tmu_channel *ch = cs_to_sh_tmu(cs);
 
-	if (!p->cs_enabled)
+	if (!ch->cs_enabled)
 		return;
 
-	if (p->enable_count++ == 0) {
-		pm_genpd_syscore_poweron(&p->pdev->dev);
-		__sh_tmu_enable(p);
+	if (ch->enable_count++ == 0) {
+		pm_genpd_syscore_poweron(&ch->tmu->pdev->dev);
+		__sh_tmu_enable(ch);
 	}
 }
 
-static int sh_tmu_register_clocksource(struct sh_tmu_priv *p,
-				       char *name, unsigned long rating)
+static int sh_tmu_register_clocksource(struct sh_tmu_channel *ch,
+				       const char *name)
 {
-	struct clocksource *cs = &p->cs;
+	struct clocksource *cs = &ch->cs;
 
 	cs->name = name;
-	cs->rating = rating;
+	cs->rating = 200;
 	cs->read = sh_tmu_clocksource_read;
 	cs->enable = sh_tmu_clocksource_enable;
 	cs->disable = sh_tmu_clocksource_disable;
@@ -295,43 +335,44 @@ static int sh_tmu_register_clocksource(struct sh_tmu_priv *p,
 	cs->mask = CLOCKSOURCE_MASK(32);
 	cs->flags = CLOCK_SOURCE_IS_CONTINUOUS;
 
-	dev_info(&p->pdev->dev, "used as clock source\n");
+	dev_info(&ch->tmu->pdev->dev, "ch%u: used as clock source\n",
+		 ch->index);
 
 	/* Register with dummy 1 Hz value, gets updated in ->enable() */
 	clocksource_register_hz(cs, 1);
 	return 0;
 }
 
-static struct sh_tmu_priv *ced_to_sh_tmu(struct clock_event_device *ced)
+static struct sh_tmu_channel *ced_to_sh_tmu(struct clock_event_device *ced)
 {
-	return container_of(ced, struct sh_tmu_priv, ced);
+	return container_of(ced, struct sh_tmu_channel, ced);
 }
 
-static void sh_tmu_clock_event_start(struct sh_tmu_priv *p, int periodic)
+static void sh_tmu_clock_event_start(struct sh_tmu_channel *ch, int periodic)
 {
-	struct clock_event_device *ced = &p->ced;
+	struct clock_event_device *ced = &ch->ced;
 
-	sh_tmu_enable(p);
+	sh_tmu_enable(ch);
 
-	clockevents_config(ced, p->rate);
+	clockevents_config(ced, ch->rate);
 
 	if (periodic) {
-		p->periodic = (p->rate + HZ/2) / HZ;
-		sh_tmu_set_next(p, p->periodic, 1);
+		ch->periodic = (ch->rate + HZ/2) / HZ;
+		sh_tmu_set_next(ch, ch->periodic, 1);
 	}
 }
 
 static void sh_tmu_clock_event_mode(enum clock_event_mode mode,
 				    struct clock_event_device *ced)
 {
-	struct sh_tmu_priv *p = ced_to_sh_tmu(ced);
+	struct sh_tmu_channel *ch = ced_to_sh_tmu(ced);
 	int disabled = 0;
 
 	/* deal with old setting first */
 	switch (ced->mode) {
 	case CLOCK_EVT_MODE_PERIODIC:
 	case CLOCK_EVT_MODE_ONESHOT:
-		sh_tmu_disable(p);
+		sh_tmu_disable(ch);
 		disabled = 1;
 		break;
 	default:
@@ -340,16 +381,18 @@ static void sh_tmu_clock_event_mode(enum clock_event_mode mode,
 
 	switch (mode) {
 	case CLOCK_EVT_MODE_PERIODIC:
-		dev_info(&p->pdev->dev, "used for periodic clock events\n");
-		sh_tmu_clock_event_start(p, 1);
+		dev_info(&ch->tmu->pdev->dev,
+			 "ch%u: used for periodic clock events\n", ch->index);
+		sh_tmu_clock_event_start(ch, 1);
 		break;
 	case CLOCK_EVT_MODE_ONESHOT:
-		dev_info(&p->pdev->dev, "used for oneshot clock events\n");
-		sh_tmu_clock_event_start(p, 0);
+		dev_info(&ch->tmu->pdev->dev,
+			 "ch%u: used for oneshot clock events\n", ch->index);
+		sh_tmu_clock_event_start(ch, 0);
 		break;
 	case CLOCK_EVT_MODE_UNUSED:
 		if (!disabled)
-			sh_tmu_disable(p);
+			sh_tmu_disable(ch);
 		break;
 	case CLOCK_EVT_MODE_SHUTDOWN:
 	default:
@@ -360,147 +403,234 @@ static void sh_tmu_clock_event_mode(enum clock_event_mode mode,
 static int sh_tmu_clock_event_next(unsigned long delta,
 				   struct clock_event_device *ced)
 {
-	struct sh_tmu_priv *p = ced_to_sh_tmu(ced);
+	struct sh_tmu_channel *ch = ced_to_sh_tmu(ced);
 
 	BUG_ON(ced->mode != CLOCK_EVT_MODE_ONESHOT);
 
 	/* program new delta value */
-	sh_tmu_set_next(p, delta, 0);
+	sh_tmu_set_next(ch, delta, 0);
 	return 0;
 }
 
 static void sh_tmu_clock_event_suspend(struct clock_event_device *ced)
 {
-	pm_genpd_syscore_poweroff(&ced_to_sh_tmu(ced)->pdev->dev);
+	pm_genpd_syscore_poweroff(&ced_to_sh_tmu(ced)->tmu->pdev->dev);
 }
 
 static void sh_tmu_clock_event_resume(struct clock_event_device *ced)
 {
-	pm_genpd_syscore_poweron(&ced_to_sh_tmu(ced)->pdev->dev);
+	pm_genpd_syscore_poweron(&ced_to_sh_tmu(ced)->tmu->pdev->dev);
 }
 
-static void sh_tmu_register_clockevent(struct sh_tmu_priv *p,
-				       char *name, unsigned long rating)
+static void sh_tmu_register_clockevent(struct sh_tmu_channel *ch,
+				       const char *name)
 {
-	struct clock_event_device *ced = &p->ced;
+	struct clock_event_device *ced = &ch->ced;
 	int ret;
 
-	memset(ced, 0, sizeof(*ced));
-
 	ced->name = name;
 	ced->features = CLOCK_EVT_FEAT_PERIODIC;
 	ced->features |= CLOCK_EVT_FEAT_ONESHOT;
-	ced->rating = rating;
+	ced->rating = 200;
 	ced->cpumask = cpumask_of(0);
 	ced->set_next_event = sh_tmu_clock_event_next;
 	ced->set_mode = sh_tmu_clock_event_mode;
 	ced->suspend = sh_tmu_clock_event_suspend;
 	ced->resume = sh_tmu_clock_event_resume;
 
-	dev_info(&p->pdev->dev, "used for clock events\n");
+	dev_info(&ch->tmu->pdev->dev, "ch%u: used for clock events\n",
+		 ch->index);
 
 	clockevents_config_and_register(ced, 1, 0x300, 0xffffffff);
 
-	ret = setup_irq(p->irqaction.irq, &p->irqaction);
+	ret = request_irq(ch->irq, sh_tmu_interrupt,
+			  IRQF_TIMER | IRQF_IRQPOLL | IRQF_NOBALANCING,
+			  dev_name(&ch->tmu->pdev->dev), ch);
 	if (ret) {
-		dev_err(&p->pdev->dev, "failed to request irq %d\n",
-			p->irqaction.irq);
+		dev_err(&ch->tmu->pdev->dev, "ch%u: failed to request irq %d\n",
+			ch->index, ch->irq);
 		return;
 	}
 }
 
-static int sh_tmu_register(struct sh_tmu_priv *p, char *name,
-		    unsigned long clockevent_rating,
-		    unsigned long clocksource_rating)
+static int sh_tmu_register(struct sh_tmu_channel *ch, const char *name,
+			   bool clockevent, bool clocksource)
 {
-	if (clockevent_rating)
-		sh_tmu_register_clockevent(p, name, clockevent_rating);
-	else if (clocksource_rating)
-		sh_tmu_register_clocksource(p, name, clocksource_rating);
+	if (clockevent) {
+		ch->tmu->has_clockevent = true;
+		sh_tmu_register_clockevent(ch, name);
+	} else if (clocksource) {
+		ch->tmu->has_clocksource = true;
+		sh_tmu_register_clocksource(ch, name);
+	}
 
 	return 0;
 }
 
-static int sh_tmu_setup(struct sh_tmu_priv *p, struct platform_device *pdev)
+static int sh_tmu_channel_setup(struct sh_tmu_channel *ch, unsigned int index,
+				bool clockevent, bool clocksource,
+				struct sh_tmu_device *tmu)
 {
-	struct sh_timer_config *cfg = pdev->dev.platform_data;
-	struct resource *res;
-	int irq, ret;
-	ret = -ENXIO;
+	/* Skip unused channels. */
+	if (!clockevent && !clocksource)
+		return 0;
 
-	memset(p, 0, sizeof(*p));
-	p->pdev = pdev;
+	ch->tmu = tmu;
+
+	if (tmu->model == SH_TMU_LEGACY) {
+		struct sh_timer_config *cfg = tmu->pdev->dev.platform_data;
+
+		/*
+		 * The SH3 variant (SH770x, SH7705, SH7710 and SH7720) maps
+		 * channel registers blocks at base + 2 + 12 * index, while all
+		 * other variants map them at base + 4 + 12 * index. We can
+		 * compute the index by just dividing by 12, the 2 bytes or 4
+		 * bytes offset being hidden by the integer division.
+		 */
+		ch->index = cfg->channel_offset / 12;
+		ch->base = tmu->mapbase + cfg->channel_offset;
+	} else {
+		ch->index = index;
+
+		if (tmu->model == SH_TMU_SH3)
+			ch->base = tmu->mapbase + 4 + ch->index * 12;
+		else
+			ch->base = tmu->mapbase + 8 + ch->index * 12;
+	}
 
-	if (!cfg) {
-		dev_err(&p->pdev->dev, "missing platform data\n");
-		goto err0;
+	ch->irq = platform_get_irq(tmu->pdev, ch->index);
+	if (ch->irq < 0) {
+		dev_err(&tmu->pdev->dev, "ch%u: failed to get irq\n",
+			ch->index);
+		return ch->irq;
 	}
 
-	platform_set_drvdata(pdev, p);
+	ch->cs_enabled = false;
+	ch->enable_count = 0;
+
+	return sh_tmu_register(ch, dev_name(&tmu->pdev->dev),
+			       clockevent, clocksource);
+}
 
-	res = platform_get_resource(p->pdev, IORESOURCE_MEM, 0);
+static int sh_tmu_map_memory(struct sh_tmu_device *tmu)
+{
+	struct resource *res;
+
+	res = platform_get_resource(tmu->pdev, IORESOURCE_MEM, 0);
 	if (!res) {
-		dev_err(&p->pdev->dev, "failed to get I/O memory\n");
-		goto err0;
+		dev_err(&tmu->pdev->dev, "failed to get I/O memory\n");
+		return -ENXIO;
+	}
+
+	tmu->mapbase = ioremap_nocache(res->start, resource_size(res));
+	if (tmu->mapbase == NULL)
+		return -ENXIO;
+
+	/*
+	 * In legacy platform device configuration (with one device per channel)
+	 * the resource points to the channel base address.
+	 */
+	if (tmu->model == SH_TMU_LEGACY) {
+		struct sh_timer_config *cfg = tmu->pdev->dev.platform_data;
+		tmu->mapbase -= cfg->channel_offset;
 	}
 
-	irq = platform_get_irq(p->pdev, 0);
-	if (irq < 0) {
-		dev_err(&p->pdev->dev, "failed to get irq\n");
-		goto err0;
+	return 0;
+}
+
+static void sh_tmu_unmap_memory(struct sh_tmu_device *tmu)
+{
+	if (tmu->model == SH_TMU_LEGACY) {
+		struct sh_timer_config *cfg = tmu->pdev->dev.platform_data;
+		tmu->mapbase += cfg->channel_offset;
 	}
 
-	/* map memory, let mapbase point to our channel */
-	p->mapbase = ioremap_nocache(res->start, resource_size(res));
-	if (p->mapbase == NULL) {
-		dev_err(&p->pdev->dev, "failed to remap I/O memory\n");
-		goto err0;
+	iounmap(tmu->mapbase);
+}
+
+static int sh_tmu_setup(struct sh_tmu_device *tmu, struct platform_device *pdev)
+{
+	struct sh_timer_config *cfg = pdev->dev.platform_data;
+	const struct platform_device_id *id = pdev->id_entry;
+	unsigned int i;
+	int ret;
+
+	if (!cfg) {
+		dev_err(&tmu->pdev->dev, "missing platform data\n");
+		return -ENXIO;
 	}
 
-	/* setup data for setup_irq() (too early for request_irq()) */
-	p->irqaction.name = dev_name(&p->pdev->dev);
-	p->irqaction.handler = sh_tmu_interrupt;
-	p->irqaction.dev_id = p;
-	p->irqaction.irq = irq;
-	p->irqaction.flags = IRQF_TIMER | IRQF_IRQPOLL | IRQF_NOBALANCING;
-
-	/* get hold of clock */
-	p->clk = clk_get(&p->pdev->dev, "tmu_fck");
-	if (IS_ERR(p->clk)) {
-		dev_err(&p->pdev->dev, "cannot get clock\n");
-		ret = PTR_ERR(p->clk);
-		goto err1;
+	tmu->pdev = pdev;
+	tmu->model = id->driver_data;
+
+	/* Get hold of clock. */
+	tmu->clk = clk_get(&tmu->pdev->dev,
+			   tmu->model == SH_TMU_LEGACY ? "tmu_fck" : "fck");
+	if (IS_ERR(tmu->clk)) {
+		dev_err(&tmu->pdev->dev, "cannot get clock\n");
+		return PTR_ERR(tmu->clk);
 	}
 
-	ret = clk_prepare(p->clk);
+	ret = clk_prepare(tmu->clk);
 	if (ret < 0)
-		goto err2;
+		goto err_clk_put;
 
-	p->cs_enabled = false;
-	p->enable_count = 0;
+	/* Map the memory resource. */
+	ret = sh_tmu_map_memory(tmu);
+	if (ret < 0) {
+		dev_err(&tmu->pdev->dev, "failed to remap I/O memory\n");
+		goto err_clk_unprepare;
+	}
 
-	ret = sh_tmu_register(p, (char *)dev_name(&p->pdev->dev),
-			      cfg->clockevent_rating,
-			      cfg->clocksource_rating);
-	if (ret < 0)
-		goto err3;
+	/* Allocate and setup the channels. */
+	if (tmu->model == SH_TMU_LEGACY)
+		tmu->num_channels = 1;
+	else
+		tmu->num_channels = hweight8(cfg->channels_mask);
+
+	tmu->channels = kzalloc(sizeof(*tmu->channels) * tmu->num_channels,
+				GFP_KERNEL);
+	if (tmu->channels == NULL) {
+		ret = -ENOMEM;
+		goto err_unmap;
+	}
+
+	if (tmu->model == SH_TMU_LEGACY) {
+		ret = sh_tmu_channel_setup(&tmu->channels[0], 0,
+					   cfg->clockevent_rating != 0,
+					   cfg->clocksource_rating != 0, tmu);
+		if (ret < 0)
+			goto err_unmap;
+	} else {
+		/*
+		 * Use the first channel as a clock event device and the second
+		 * channel as a clock source.
+		 */
+		for (i = 0; i < tmu->num_channels; ++i) {
+			ret = sh_tmu_channel_setup(&tmu->channels[i], i,
+						   i == 0, i == 1, tmu);
+			if (ret < 0)
+				goto err_unmap;
+		}
+	}
+
+	platform_set_drvdata(pdev, tmu);
 
 	return 0;
 
- err3:
-	clk_unprepare(p->clk);
- err2:
-	clk_put(p->clk);
- err1:
-	iounmap(p->mapbase);
- err0:
+err_unmap:
+	kfree(tmu->channels);
+	sh_tmu_unmap_memory(tmu);
+err_clk_unprepare:
+	clk_unprepare(tmu->clk);
+err_clk_put:
+	clk_put(tmu->clk);
 	return ret;
 }
 
 static int sh_tmu_probe(struct platform_device *pdev)
 {
-	struct sh_tmu_priv *p = platform_get_drvdata(pdev);
-	struct sh_timer_config *cfg = pdev->dev.platform_data;
+	struct sh_tmu_device *tmu = platform_get_drvdata(pdev);
 	int ret;
 
 	if (!is_early_platform_device(pdev)) {
@@ -508,20 +638,20 @@ static int sh_tmu_probe(struct platform_device *pdev)
 		pm_runtime_enable(&pdev->dev);
 	}
 
-	if (p) {
+	if (tmu) {
 		dev_info(&pdev->dev, "kept as earlytimer\n");
 		goto out;
 	}
 
-	p = kmalloc(sizeof(*p), GFP_KERNEL);
-	if (p == NULL) {
+	tmu = kzalloc(sizeof(*tmu), GFP_KERNEL);
+	if (tmu == NULL) {
 		dev_err(&pdev->dev, "failed to allocate driver data\n");
 		return -ENOMEM;
 	}
 
-	ret = sh_tmu_setup(p, pdev);
+	ret = sh_tmu_setup(tmu, pdev);
 	if (ret) {
-		kfree(p);
+		kfree(tmu);
 		pm_runtime_idle(&pdev->dev);
 		return ret;
 	}
@@ -529,7 +659,7 @@ static int sh_tmu_probe(struct platform_device *pdev)
 		return 0;
 
  out:
-	if (cfg->clockevent_rating || cfg->clocksource_rating)
+	if (tmu->has_clockevent || tmu->has_clocksource)
 		pm_runtime_irq_safe(&pdev->dev);
 	else
 		pm_runtime_idle(&pdev->dev);
@@ -542,12 +672,21 @@ static int sh_tmu_remove(struct platform_device *pdev)
 	return -EBUSY; /* cannot unregister clockevent and clocksource */
 }
 
+static const struct platform_device_id sh_tmu_id_table[] = {
+	{ "sh_tmu", SH_TMU_LEGACY },
+	{ "sh-tmu", SH_TMU },
+	{ "sh-tmu-sh3", SH_TMU_SH3 },
+	{ }
+};
+MODULE_DEVICE_TABLE(platform, sh_tmu_id_table);
+
 static struct platform_driver sh_tmu_device_driver = {
 	.probe		= sh_tmu_probe,
 	.remove		= sh_tmu_remove,
 	.driver		= {
 		.name	= "sh_tmu",
-	}
+	},
+	.id_table	= sh_tmu_id_table,
 };
 
 static int __init sh_tmu_init(void)
diff --git a/drivers/clocksource/tcb_clksrc.c b/drivers/clocksource/tcb_clksrc.c
index 00fdd117028..a8d7ea14f18 100644
--- a/drivers/clocksource/tcb_clksrc.c
+++ b/drivers/clocksource/tcb_clksrc.c
@@ -100,7 +100,7 @@ static void tc_mode(enum clock_event_mode m, struct clock_event_device *d)
 			|| tcd->clkevt.mode == CLOCK_EVT_MODE_ONESHOT) {
 		__raw_writel(0xff, regs + ATMEL_TC_REG(2, IDR));
 		__raw_writel(ATMEL_TC_CLKDIS, regs + ATMEL_TC_REG(2, CCR));
-		clk_disable_unprepare(tcd->clk);
+		clk_disable(tcd->clk);
 	}
 
 	switch (m) {
@@ -109,7 +109,7 @@ static void tc_mode(enum clock_event_mode m, struct clock_event_device *d)
 	 * of oneshot, we get lower overhead and improved accuracy.
 	 */
 	case CLOCK_EVT_MODE_PERIODIC:
-		clk_prepare_enable(tcd->clk);
+		clk_enable(tcd->clk);
 
 		/* slow clock, count up to RC, then irq and restart */
 		__raw_writel(timer_clock
@@ -126,7 +126,7 @@ static void tc_mode(enum clock_event_mode m, struct clock_event_device *d)
 		break;
 
 	case CLOCK_EVT_MODE_ONESHOT:
-		clk_prepare_enable(tcd->clk);
+		clk_enable(tcd->clk);
 
 		/* slow clock, count up to RC, then irq and stop */
 		__raw_writel(timer_clock | ATMEL_TC_CPCSTOP
@@ -194,7 +194,7 @@ static int __init setup_clkevents(struct atmel_tc *tc, int clk32k_divisor_idx)
 	ret = clk_prepare_enable(t2_clk);
 	if (ret)
 		return ret;
-	clk_disable_unprepare(t2_clk);
+	clk_disable(t2_clk);
 
 	clkevt.regs = tc->regs;
 	clkevt.clk = t2_clk;
diff --git a/drivers/clocksource/timer-marco.c b/drivers/clocksource/timer-marco.c
index b52e1c078b9..dbd30398222 100644
--- a/drivers/clocksource/timer-marco.c
+++ b/drivers/clocksource/timer-marco.c
@@ -199,7 +199,7 @@ static int sirfsoc_local_timer_setup(struct clock_event_device *ce)
 
 	action->dev_id = ce;
 	BUG_ON(setup_irq(ce->irq, action));
-	irq_set_affinity(action->irq, cpumask_of(cpu));
+	irq_force_affinity(action->irq, cpumask_of(cpu));
 
 	clockevents_register_device(ce);
 	return 0;
@@ -252,15 +252,13 @@ static void __init sirfsoc_clockevent_init(void)
 }
 
 /* initialize the kernel jiffy timer source */
-static void __init sirfsoc_marco_timer_init(void)
+static void __init sirfsoc_marco_timer_init(struct device_node *np)
 {
 	unsigned long rate;
 	u32 timer_div;
 	struct clk *clk;
 
-	/* timer's input clock is io clock */
-	clk = clk_get_sys("io", NULL);
-
+	clk = of_clk_get(np, 0);
 	BUG_ON(IS_ERR(clk));
 	rate = clk_get_rate(clk);
 
@@ -303,6 +301,6 @@ static void __init sirfsoc_of_timer_init(struct device_node *np)
 	if (!sirfsoc_timer1_irq.irq)
 		panic("No irq passed for timer1 via DT\n");
 
-	sirfsoc_marco_timer_init();
+	sirfsoc_marco_timer_init(np);
 }
 CLOCKSOURCE_OF_DECLARE(sirfsoc_marco_timer, "sirf,marco-tick", sirfsoc_of_timer_init );
diff --git a/drivers/clocksource/timer-prima2.c b/drivers/clocksource/timer-prima2.c
index 1a6b2d6356d..a722aac7ac0 100644
--- a/drivers/clocksource/timer-prima2.c
+++ b/drivers/clocksource/timer-prima2.c
@@ -61,7 +61,8 @@ static irqreturn_t sirfsoc_timer_interrupt(int irq, void *dev_id)
 {
 	struct clock_event_device *ce = dev_id;
 
-	WARN_ON(!(readl_relaxed(sirfsoc_timer_base + SIRFSOC_TIMER_STATUS) & BIT(0)));
+	WARN_ON(!(readl_relaxed(sirfsoc_timer_base + SIRFSOC_TIMER_STATUS) &
+		BIT(0)));
 
 	/* clear timer0 interrupt */
 	writel_relaxed(BIT(0), sirfsoc_timer_base + SIRFSOC_TIMER_STATUS);
@@ -77,9 +78,11 @@ static cycle_t sirfsoc_timer_read(struct clocksource *cs)
 	u64 cycles;
 
 	/* latch the 64-bit timer counter */
-	writel_relaxed(SIRFSOC_TIMER_LATCH_BIT, sirfsoc_timer_base + SIRFSOC_TIMER_LATCH);
+	writel_relaxed(SIRFSOC_TIMER_LATCH_BIT,
+		sirfsoc_timer_base + SIRFSOC_TIMER_LATCH);
 	cycles = readl_relaxed(sirfsoc_timer_base + SIRFSOC_TIMER_LATCHED_HI);
-	cycles = (cycles << 32) | readl_relaxed(sirfsoc_timer_base + SIRFSOC_TIMER_LATCHED_LO);
+	cycles = (cycles << 32) |
+		readl_relaxed(sirfsoc_timer_base + SIRFSOC_TIMER_LATCHED_LO);
 
 	return cycles;
 }
@@ -89,11 +92,13 @@ static int sirfsoc_timer_set_next_event(unsigned long delta,
 {
 	unsigned long now, next;
 
-	writel_relaxed(SIRFSOC_TIMER_LATCH_BIT, sirfsoc_timer_base + SIRFSOC_TIMER_LATCH);
+	writel_relaxed(SIRFSOC_TIMER_LATCH_BIT,
+		sirfsoc_timer_base + SIRFSOC_TIMER_LATCH);
 	now = readl_relaxed(sirfsoc_timer_base + SIRFSOC_TIMER_LATCHED_LO);
 	next = now + delta;
 	writel_relaxed(next, sirfsoc_timer_base + SIRFSOC_TIMER_MATCH_0);
-	writel_relaxed(SIRFSOC_TIMER_LATCH_BIT, sirfsoc_timer_base + SIRFSOC_TIMER_LATCH);
+	writel_relaxed(SIRFSOC_TIMER_LATCH_BIT,
+		sirfsoc_timer_base + SIRFSOC_TIMER_LATCH);
 	now = readl_relaxed(sirfsoc_timer_base + SIRFSOC_TIMER_LATCHED_LO);
 
 	return next - now > delta ? -ETIME : 0;
@@ -108,10 +113,12 @@ static void sirfsoc_timer_set_mode(enum clock_event_mode mode,
 		WARN_ON(1);
 		break;
 	case CLOCK_EVT_MODE_ONESHOT:
-		writel_relaxed(val | BIT(0), sirfsoc_timer_base + SIRFSOC_TIMER_INT_EN);
+		writel_relaxed(val | BIT(0),
+			sirfsoc_timer_base + SIRFSOC_TIMER_INT_EN);
 		break;
 	case CLOCK_EVT_MODE_SHUTDOWN:
-		writel_relaxed(val & ~BIT(0), sirfsoc_timer_base + SIRFSOC_TIMER_INT_EN);
+		writel_relaxed(val & ~BIT(0),
+			sirfsoc_timer_base + SIRFSOC_TIMER_INT_EN);
 		break;
 	case CLOCK_EVT_MODE_UNUSED:
 	case CLOCK_EVT_MODE_RESUME:
@@ -123,10 +130,13 @@ static void sirfsoc_clocksource_suspend(struct clocksource *cs)
 {
 	int i;
 
-	writel_relaxed(SIRFSOC_TIMER_LATCH_BIT, sirfsoc_timer_base + SIRFSOC_TIMER_LATCH);
+	writel_relaxed(SIRFSOC_TIMER_LATCH_BIT,
+		sirfsoc_timer_base + SIRFSOC_TIMER_LATCH);
 
 	for (i = 0; i < SIRFSOC_TIMER_REG_CNT; i++)
-		sirfsoc_timer_reg_val[i] = readl_relaxed(sirfsoc_timer_base + sirfsoc_timer_reg_list[i]);
+		sirfsoc_timer_reg_val[i] =
+			readl_relaxed(sirfsoc_timer_base +
+				sirfsoc_timer_reg_list[i]);
 }
 
 static void sirfsoc_clocksource_resume(struct clocksource *cs)
@@ -134,10 +144,13 @@ static void sirfsoc_clocksource_resume(struct clocksource *cs)
 	int i;
 
 	for (i = 0; i < SIRFSOC_TIMER_REG_CNT - 2; i++)
-		writel_relaxed(sirfsoc_timer_reg_val[i], sirfsoc_timer_base + sirfsoc_timer_reg_list[i]);
+		writel_relaxed(sirfsoc_timer_reg_val[i],
+			sirfsoc_timer_base + sirfsoc_timer_reg_list[i]);
 
-	writel_relaxed(sirfsoc_timer_reg_val[SIRFSOC_TIMER_REG_CNT - 2], sirfsoc_timer_base + SIRFSOC_TIMER_COUNTER_LO);
-	writel_relaxed(sirfsoc_timer_reg_val[SIRFSOC_TIMER_REG_CNT - 1], sirfsoc_timer_base + SIRFSOC_TIMER_COUNTER_HI);
+	writel_relaxed(sirfsoc_timer_reg_val[SIRFSOC_TIMER_REG_CNT - 2],
+		sirfsoc_timer_base + SIRFSOC_TIMER_COUNTER_LO);
+	writel_relaxed(sirfsoc_timer_reg_val[SIRFSOC_TIMER_REG_CNT - 1],
+		sirfsoc_timer_base + SIRFSOC_TIMER_COUNTER_HI);
 }
 
 static struct clock_event_device sirfsoc_clockevent = {
@@ -185,11 +198,8 @@ static void __init sirfsoc_prima2_timer_init(struct device_node *np)
 	unsigned long rate;
 	struct clk *clk;
 
-	/* timer's input clock is io clock */
-	clk = clk_get_sys("io", NULL);
-
+	clk = of_clk_get(np, 0);
 	BUG_ON(IS_ERR(clk));
-
 	rate = clk_get_rate(clk);
 
 	BUG_ON(rate < PRIMA2_CLOCK_FREQ);
@@ -202,7 +212,7 @@ static void __init sirfsoc_prima2_timer_init(struct device_node *np)
 	sirfsoc_timer_irq.irq = irq_of_parse_and_map(np, 0);
 
 	writel_relaxed(rate / PRIMA2_CLOCK_FREQ / 2 - 1,
-		       sirfsoc_timer_base + SIRFSOC_TIMER_DIV);
+		sirfsoc_timer_base + SIRFSOC_TIMER_DIV);
 	writel_relaxed(0, sirfsoc_timer_base + SIRFSOC_TIMER_COUNTER_LO);
 	writel_relaxed(0, sirfsoc_timer_base + SIRFSOC_TIMER_COUNTER_HI);
 	writel_relaxed(BIT(0), sirfsoc_timer_base + SIRFSOC_TIMER_STATUS);
@@ -216,4 +226,5 @@ static void __init sirfsoc_prima2_timer_init(struct device_node *np)
 
 	sirfsoc_clockevent_init();
 }
-CLOCKSOURCE_OF_DECLARE(sirfsoc_prima2_timer, "sirf,prima2-tick", sirfsoc_prima2_timer_init);
+CLOCKSOURCE_OF_DECLARE(sirfsoc_prima2_timer,
+	"sirf,prima2-tick", sirfsoc_prima2_timer_init);
diff --git a/drivers/clocksource/versatile.c b/drivers/clocksource/versatile.c
new file mode 100644
index 00000000000..e4c50ad2f9d
--- /dev/null
+++ b/drivers/clocksource/versatile.c
@@ -0,0 +1,40 @@
+/*
+ * 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) 2014 ARM Limited
+ */
+
+#include <linux/clocksource.h>
+#include <linux/io.h>
+#include <linux/of_address.h>
+#include <linux/sched_clock.h>
+
+#define SYS_24MHZ 0x05c
+
+static void __iomem *versatile_sys_24mhz;
+
+static u32 notrace versatile_sys_24mhz_read(void)
+{
+	return readl(versatile_sys_24mhz);
+}
+
+static void __init versatile_sched_clock_init(struct device_node *node)
+{
+	void __iomem *base = of_iomap(node, 0);
+
+	if (!base)
+		return;
+
+	versatile_sys_24mhz = base + SYS_24MHZ;
+
+	setup_sched_clock(versatile_sys_24mhz_read, 32, 24000000);
+}
+CLOCKSOURCE_OF_DECLARE(versatile, "arm,vexpress-sysreg",
+		versatile_sched_clock_init);
diff --git a/drivers/clocksource/zevio-timer.c b/drivers/clocksource/zevio-timer.c
index ca81809d159..7ce442148c3 100644
--- a/drivers/clocksource/zevio-timer.c
+++ b/drivers/clocksource/zevio-timer.c
@@ -212,4 +212,9 @@ error_free:
 	return ret;
 }
 
-CLOCKSOURCE_OF_DECLARE(zevio_timer, "lsi,zevio-timer", zevio_timer_add);
+static void __init zevio_timer_init(struct device_node *node)
+{
+	BUG_ON(zevio_timer_add(node));
+}
+
+CLOCKSOURCE_OF_DECLARE(zevio_timer, "lsi,zevio-timer", zevio_timer_init);
diff --git a/drivers/connector/cn_proc.c b/drivers/connector/cn_proc.c
index 148d707a1d4..ccdd4c7e748 100644
--- a/drivers/connector/cn_proc.c
+++ b/drivers/connector/cn_proc.c
@@ -369,7 +369,7 @@ static void cn_proc_mcast_ctl(struct cn_msg *msg,
 		return;
 
 	/* Can only change if privileged. */
-	if (!capable(CAP_NET_ADMIN)) {
+	if (!__netlink_ns_capable(nsp, &init_user_ns, CAP_NET_ADMIN)) {
 		err = EPERM;
 		goto out;
 	}
diff --git a/drivers/connector/connector.c b/drivers/connector/connector.c
index b14f1d36f89..f612d68629d 100644
--- a/drivers/connector/connector.c
+++ b/drivers/connector/connector.c
@@ -43,6 +43,8 @@ static struct cn_dev cdev;
 static int cn_already_initialized;
 
 /*
+ * Sends mult (multiple) cn_msg at a time.
+ *
  * msg->seq and msg->ack are used to determine message genealogy.
  * When someone sends message it puts there locally unique sequence
  * and random acknowledge numbers.  Sequence number may be copied into
@@ -62,10 +64,13 @@ static int cn_already_initialized;
  * the acknowledgement number in the original message + 1, then it is
  * a new message.
  *
+ * If msg->len != len, then additional cn_msg messages are expected following
+ * the first msg.
+ *
  * The message is sent to, the portid if given, the group if given, both if
  * both, or if both are zero then the group is looked up and sent there.
  */
-int cn_netlink_send(struct cn_msg *msg, u32 portid, u32 __group,
+int cn_netlink_send_mult(struct cn_msg *msg, u16 len, u32 portid, u32 __group,
 	gfp_t gfp_mask)
 {
 	struct cn_callback_entry *__cbq;
@@ -98,7 +103,7 @@ int cn_netlink_send(struct cn_msg *msg, u32 portid, u32 __group,
 	if (!portid && !netlink_has_listeners(dev->nls, group))
 		return -ESRCH;
 
-	size = sizeof(*msg) + msg->len;
+	size = sizeof(*msg) + len;
 
 	skb = nlmsg_new(size, gfp_mask);
 	if (!skb)
@@ -121,6 +126,14 @@ int cn_netlink_send(struct cn_msg *msg, u32 portid, u32 __group,
 					 gfp_mask);
 	return netlink_unicast(dev->nls, skb, portid, !(gfp_mask&__GFP_WAIT));
 }
+EXPORT_SYMBOL_GPL(cn_netlink_send_mult);
+
+/* same as cn_netlink_send_mult except msg->len is used for len */
+int cn_netlink_send(struct cn_msg *msg, u32 portid, u32 __group,
+	gfp_t gfp_mask)
+{
+	return cn_netlink_send_mult(msg, msg->len, portid, __group, gfp_mask);
+}
 EXPORT_SYMBOL_GPL(cn_netlink_send);
 
 /*
diff --git a/drivers/cpufreq/Kconfig.arm b/drivers/cpufreq/Kconfig.arm
index 0e9cce82844..d2c7b4b8ffd 100644
--- a/drivers/cpufreq/Kconfig.arm
+++ b/drivers/cpufreq/Kconfig.arm
@@ -30,7 +30,7 @@ config ARM_EXYNOS_CPUFREQ
 
 config ARM_EXYNOS4210_CPUFREQ
 	bool "SAMSUNG EXYNOS4210"
-	depends on CPU_EXYNOS4210 && !ARCH_MULTIPLATFORM
+	depends on CPU_EXYNOS4210
 	default y
 	select ARM_EXYNOS_CPUFREQ
 	help
@@ -41,7 +41,7 @@ config ARM_EXYNOS4210_CPUFREQ
 
 config ARM_EXYNOS4X12_CPUFREQ
 	bool "SAMSUNG EXYNOS4x12"
-	depends on (SOC_EXYNOS4212 || SOC_EXYNOS4412) && !ARCH_MULTIPLATFORM
+	depends on SOC_EXYNOS4212 || SOC_EXYNOS4412
 	default y
 	select ARM_EXYNOS_CPUFREQ
 	help
@@ -52,7 +52,7 @@ config ARM_EXYNOS4X12_CPUFREQ
 
 config ARM_EXYNOS5250_CPUFREQ
 	bool "SAMSUNG EXYNOS5250"
-	depends on SOC_EXYNOS5250 && !ARCH_MULTIPLATFORM
+	depends on SOC_EXYNOS5250
 	default y
 	select ARM_EXYNOS_CPUFREQ
 	help
@@ -92,11 +92,7 @@ config ARM_EXYNOS_CPU_FREQ_BOOST_SW
 
 config ARM_HIGHBANK_CPUFREQ
 	tristate "Calxeda Highbank-based"
-	depends on ARCH_HIGHBANK
-	select GENERIC_CPUFREQ_CPU0
-	select PM_OPP
-	select REGULATOR
-
+	depends on ARCH_HIGHBANK && GENERIC_CPUFREQ_CPU0 && REGULATOR
 	default m
 	help
 	  This adds the CPUFreq driver for Calxeda Highbank SoC
diff --git a/drivers/cpufreq/cpufreq-cpu0.c b/drivers/cpufreq/cpufreq-cpu0.c
index 1bf6bbac3e0..09b9129c7bd 100644
--- a/drivers/cpufreq/cpufreq-cpu0.c
+++ b/drivers/cpufreq/cpufreq-cpu0.c
@@ -130,7 +130,7 @@ static int cpu0_cpufreq_probe(struct platform_device *pdev)
 		return -ENOENT;
 	}
 
-	cpu_reg = devm_regulator_get_optional(cpu_dev, "cpu0");
+	cpu_reg = regulator_get_optional(cpu_dev, "cpu0");
 	if (IS_ERR(cpu_reg)) {
 		/*
 		 * If cpu0 regulator supply node is present, but regulator is
@@ -145,23 +145,23 @@ static int cpu0_cpufreq_probe(struct platform_device *pdev)
 			PTR_ERR(cpu_reg));
 	}
 
-	cpu_clk = devm_clk_get(cpu_dev, NULL);
+	cpu_clk = clk_get(cpu_dev, NULL);
 	if (IS_ERR(cpu_clk)) {
 		ret = PTR_ERR(cpu_clk);
 		pr_err("failed to get cpu0 clock: %d\n", ret);
-		goto out_put_node;
+		goto out_put_reg;
 	}
 
 	ret = of_init_opp_table(cpu_dev);
 	if (ret) {
 		pr_err("failed to init OPP table: %d\n", ret);
-		goto out_put_node;
+		goto out_put_clk;
 	}
 
 	ret = dev_pm_opp_init_cpufreq_table(cpu_dev, &freq_table);
 	if (ret) {
 		pr_err("failed to init cpufreq table: %d\n", ret);
-		goto out_put_node;
+		goto out_put_clk;
 	}
 
 	of_property_read_u32(np, "voltage-tolerance", &voltage_tolerance);
@@ -216,6 +216,12 @@ static int cpu0_cpufreq_probe(struct platform_device *pdev)
 
 out_free_table:
 	dev_pm_opp_free_cpufreq_table(cpu_dev, &freq_table);
+out_put_clk:
+	if (!IS_ERR(cpu_clk))
+		clk_put(cpu_clk);
+out_put_reg:
+	if (!IS_ERR(cpu_reg))
+		regulator_put(cpu_reg);
 out_put_node:
 	of_node_put(np);
 	return ret;
diff --git a/drivers/cpufreq/cpufreq_governor.c b/drivers/cpufreq/cpufreq_governor.c
index ba43991ba98..e1c6433b16e 100644
--- a/drivers/cpufreq/cpufreq_governor.c
+++ b/drivers/cpufreq/cpufreq_governor.c
@@ -366,6 +366,11 @@ int cpufreq_governor_dbs(struct cpufreq_policy *policy,
 		break;
 
 	case CPUFREQ_GOV_LIMITS:
+		mutex_lock(&dbs_data->mutex);
+		if (!cpu_cdbs->cur_policy) {
+			mutex_unlock(&dbs_data->mutex);
+			break;
+		}
 		mutex_lock(&cpu_cdbs->timer_mutex);
 		if (policy->max < cpu_cdbs->cur_policy->cur)
 			__cpufreq_driver_target(cpu_cdbs->cur_policy,
@@ -375,6 +380,7 @@ int cpufreq_governor_dbs(struct cpufreq_policy *policy,
 					policy->min, CPUFREQ_RELATION_L);
 		dbs_check_cpu(dbs_data, cpu);
 		mutex_unlock(&cpu_cdbs->timer_mutex);
+		mutex_unlock(&dbs_data->mutex);
 		break;
 	}
 	return 0;
diff --git a/drivers/cpufreq/exynos-cpufreq.c b/drivers/cpufreq/exynos-cpufreq.c
index f99cfe24e7b..348c8bafe43 100644
--- a/drivers/cpufreq/exynos-cpufreq.c
+++ b/drivers/cpufreq/exynos-cpufreq.c
@@ -17,8 +17,7 @@
 #include <linux/regulator/consumer.h>
 #include <linux/cpufreq.h>
 #include <linux/platform_device.h>
-
-#include <plat/cpu.h>
+#include <linux/of.h>
 
 #include "exynos-cpufreq.h"
 
@@ -163,14 +162,22 @@ static int exynos_cpufreq_probe(struct platform_device *pdev)
 	if (!exynos_info)
 		return -ENOMEM;
 
-	if (soc_is_exynos4210())
+	if (of_machine_is_compatible("samsung,exynos4210")) {
+		exynos_info->type = EXYNOS_SOC_4210;
 		ret = exynos4210_cpufreq_init(exynos_info);
-	else if (soc_is_exynos4212() || soc_is_exynos4412())
+	} else if (of_machine_is_compatible("samsung,exynos4212")) {
+		exynos_info->type = EXYNOS_SOC_4212;
+		ret = exynos4x12_cpufreq_init(exynos_info);
+	} else if (of_machine_is_compatible("samsung,exynos4412")) {
+		exynos_info->type = EXYNOS_SOC_4412;
 		ret = exynos4x12_cpufreq_init(exynos_info);
-	else if (soc_is_exynos5250())
+	} else if (of_machine_is_compatible("samsung,exynos5250")) {
+		exynos_info->type = EXYNOS_SOC_5250;
 		ret = exynos5250_cpufreq_init(exynos_info);
-	else
-		return 0;
+	} else {
+		pr_err("%s: Unknown SoC type\n", __func__);
+		return -ENODEV;
+	}
 
 	if (ret)
 		goto err_vdd_arm;
diff --git a/drivers/cpufreq/exynos-cpufreq.h b/drivers/cpufreq/exynos-cpufreq.h
index 3ddade8a512..51af42e1b7f 100644
--- a/drivers/cpufreq/exynos-cpufreq.h
+++ b/drivers/cpufreq/exynos-cpufreq.h
@@ -17,6 +17,13 @@ enum cpufreq_level_index {
 	L20,
 };
 
+enum exynos_soc_type {
+	EXYNOS_SOC_4210,
+	EXYNOS_SOC_4212,
+	EXYNOS_SOC_4412,
+	EXYNOS_SOC_5250,
+};
+
 #define APLL_FREQ(f, a0, a1, a2, a3, a4, a5, a6, a7, b0, b1, b2, m, p, s) \
 	{ \
 		.freq = (f) * 1000, \
@@ -34,6 +41,7 @@ struct apll_freq {
 };
 
 struct exynos_dvfs_info {
+	enum exynos_soc_type type;
 	unsigned long	mpll_freq_khz;
 	unsigned int	pll_safe_idx;
 	struct clk	*cpu_clk;
@@ -41,6 +49,7 @@ struct exynos_dvfs_info {
 	struct cpufreq_frequency_table	*freq_table;
 	void (*set_freq)(unsigned int, unsigned int);
 	bool (*need_apll_change)(unsigned int, unsigned int);
+	void __iomem	*cmu_regs;
 };
 
 #ifdef CONFIG_ARM_EXYNOS4210_CPUFREQ
@@ -68,24 +77,21 @@ static inline int exynos5250_cpufreq_init(struct exynos_dvfs_info *info)
 }
 #endif
 
-#include <plat/cpu.h>
-#include <mach/map.h>
-
-#define EXYNOS4_CLKSRC_CPU			(S5P_VA_CMU + 0x14200)
-#define EXYNOS4_CLKMUX_STATCPU			(S5P_VA_CMU + 0x14400)
+#define EXYNOS4_CLKSRC_CPU			0x14200
+#define EXYNOS4_CLKMUX_STATCPU			0x14400
 
-#define EXYNOS4_CLKDIV_CPU			(S5P_VA_CMU + 0x14500)
-#define EXYNOS4_CLKDIV_CPU1			(S5P_VA_CMU + 0x14504)
-#define EXYNOS4_CLKDIV_STATCPU			(S5P_VA_CMU + 0x14600)
-#define EXYNOS4_CLKDIV_STATCPU1			(S5P_VA_CMU + 0x14604)
+#define EXYNOS4_CLKDIV_CPU			0x14500
+#define EXYNOS4_CLKDIV_CPU1			0x14504
+#define EXYNOS4_CLKDIV_STATCPU			0x14600
+#define EXYNOS4_CLKDIV_STATCPU1			0x14604
 
 #define EXYNOS4_CLKSRC_CPU_MUXCORE_SHIFT	(16)
 #define EXYNOS4_CLKMUX_STATCPU_MUXCORE_MASK	(0x7 << EXYNOS4_CLKSRC_CPU_MUXCORE_SHIFT)
 
-#define EXYNOS5_APLL_LOCK			(S5P_VA_CMU + 0x00000)
-#define EXYNOS5_APLL_CON0			(S5P_VA_CMU + 0x00100)
-#define EXYNOS5_CLKMUX_STATCPU			(S5P_VA_CMU + 0x00400)
-#define EXYNOS5_CLKDIV_CPU0			(S5P_VA_CMU + 0x00500)
-#define EXYNOS5_CLKDIV_CPU1			(S5P_VA_CMU + 0x00504)
-#define EXYNOS5_CLKDIV_STATCPU0			(S5P_VA_CMU + 0x00600)
-#define EXYNOS5_CLKDIV_STATCPU1			(S5P_VA_CMU + 0x00604)
+#define EXYNOS5_APLL_LOCK			0x00000
+#define EXYNOS5_APLL_CON0			0x00100
+#define EXYNOS5_CLKMUX_STATCPU			0x00400
+#define EXYNOS5_CLKDIV_CPU0			0x00500
+#define EXYNOS5_CLKDIV_CPU1			0x00504
+#define EXYNOS5_CLKDIV_STATCPU0			0x00600
+#define EXYNOS5_CLKDIV_STATCPU1			0x00604
diff --git a/drivers/cpufreq/exynos4210-cpufreq.c b/drivers/cpufreq/exynos4210-cpufreq.c
index 6384e5b9a34..61a54310a1b 100644
--- a/drivers/cpufreq/exynos4210-cpufreq.c
+++ b/drivers/cpufreq/exynos4210-cpufreq.c
@@ -16,6 +16,8 @@
 #include <linux/io.h>
 #include <linux/slab.h>
 #include <linux/cpufreq.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
 
 #include "exynos-cpufreq.h"
 
@@ -23,6 +25,7 @@ static struct clk *cpu_clk;
 static struct clk *moutcore;
 static struct clk *mout_mpll;
 static struct clk *mout_apll;
+static struct exynos_dvfs_info *cpufreq;
 
 static unsigned int exynos4210_volt_table[] = {
 	1250000, 1150000, 1050000, 975000, 950000,
@@ -60,20 +63,20 @@ static void exynos4210_set_clkdiv(unsigned int div_index)
 
 	tmp = apll_freq_4210[div_index].clk_div_cpu0;
 
-	__raw_writel(tmp, EXYNOS4_CLKDIV_CPU);
+	__raw_writel(tmp, cpufreq->cmu_regs + EXYNOS4_CLKDIV_CPU);
 
 	do {
-		tmp = __raw_readl(EXYNOS4_CLKDIV_STATCPU);
+		tmp = __raw_readl(cpufreq->cmu_regs + EXYNOS4_CLKDIV_STATCPU);
 	} while (tmp & 0x1111111);
 
 	/* Change Divider - CPU1 */
 
 	tmp = apll_freq_4210[div_index].clk_div_cpu1;
 
-	__raw_writel(tmp, EXYNOS4_CLKDIV_CPU1);
+	__raw_writel(tmp, cpufreq->cmu_regs + EXYNOS4_CLKDIV_CPU1);
 
 	do {
-		tmp = __raw_readl(EXYNOS4_CLKDIV_STATCPU1);
+		tmp = __raw_readl(cpufreq->cmu_regs + EXYNOS4_CLKDIV_STATCPU1);
 	} while (tmp & 0x11);
 }
 
@@ -85,7 +88,7 @@ static void exynos4210_set_apll(unsigned int index)
 	clk_set_parent(moutcore, mout_mpll);
 
 	do {
-		tmp = (__raw_readl(EXYNOS4_CLKMUX_STATCPU)
+		tmp = (__raw_readl(cpufreq->cmu_regs + EXYNOS4_CLKMUX_STATCPU)
 			>> EXYNOS4_CLKSRC_CPU_MUXCORE_SHIFT);
 		tmp &= 0x7;
 	} while (tmp != 0x2);
@@ -96,7 +99,7 @@ static void exynos4210_set_apll(unsigned int index)
 	clk_set_parent(moutcore, mout_apll);
 
 	do {
-		tmp = __raw_readl(EXYNOS4_CLKMUX_STATCPU);
+		tmp = __raw_readl(cpufreq->cmu_regs + EXYNOS4_CLKMUX_STATCPU);
 		tmp &= EXYNOS4_CLKMUX_STATCPU_MUXCORE_MASK;
 	} while (tmp != (0x1 << EXYNOS4_CLKSRC_CPU_MUXCORE_SHIFT));
 }
@@ -115,8 +118,30 @@ static void exynos4210_set_frequency(unsigned int old_index,
 
 int exynos4210_cpufreq_init(struct exynos_dvfs_info *info)
 {
+	struct device_node *np;
 	unsigned long rate;
 
+	/*
+	 * HACK: This is a temporary workaround to get access to clock
+	 * controller registers directly and remove static mappings and
+	 * dependencies on platform headers. It is necessary to enable
+	 * Exynos multi-platform support and will be removed together with
+	 * this whole driver as soon as Exynos gets migrated to use
+	 * cpufreq-cpu0 driver.
+	 */
+	np = of_find_compatible_node(NULL, NULL, "samsung,exynos4210-clock");
+	if (!np) {
+		pr_err("%s: failed to find clock controller DT node\n",
+			__func__);
+		return -ENODEV;
+	}
+
+	info->cmu_regs = of_iomap(np, 0);
+	if (!info->cmu_regs) {
+		pr_err("%s: failed to map CMU registers\n", __func__);
+		return -EFAULT;
+	}
+
 	cpu_clk = clk_get(NULL, "armclk");
 	if (IS_ERR(cpu_clk))
 		return PTR_ERR(cpu_clk);
@@ -143,6 +168,8 @@ int exynos4210_cpufreq_init(struct exynos_dvfs_info *info)
 	info->freq_table = exynos4210_freq_table;
 	info->set_freq = exynos4210_set_frequency;
 
+	cpufreq = info;
+
 	return 0;
 
 err_mout_apll:
diff --git a/drivers/cpufreq/exynos4x12-cpufreq.c b/drivers/cpufreq/exynos4x12-cpufreq.c
index 466c76ad335..351a2074cfe 100644
--- a/drivers/cpufreq/exynos4x12-cpufreq.c
+++ b/drivers/cpufreq/exynos4x12-cpufreq.c
@@ -16,6 +16,8 @@
 #include <linux/io.h>
 #include <linux/slab.h>
 #include <linux/cpufreq.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
 
 #include "exynos-cpufreq.h"
 
@@ -23,6 +25,7 @@ static struct clk *cpu_clk;
 static struct clk *moutcore;
 static struct clk *mout_mpll;
 static struct clk *mout_apll;
+static struct exynos_dvfs_info *cpufreq;
 
 static unsigned int exynos4x12_volt_table[] = {
 	1350000, 1287500, 1250000, 1187500, 1137500, 1087500, 1037500,
@@ -100,28 +103,26 @@ static struct apll_freq apll_freq_4412[] = {
 static void exynos4x12_set_clkdiv(unsigned int div_index)
 {
 	unsigned int tmp;
-	unsigned int stat_cpu1;
 
 	/* Change Divider - CPU0 */
 
 	tmp = apll_freq_4x12[div_index].clk_div_cpu0;
 
-	__raw_writel(tmp, EXYNOS4_CLKDIV_CPU);
+	__raw_writel(tmp, cpufreq->cmu_regs + EXYNOS4_CLKDIV_CPU);
 
-	while (__raw_readl(EXYNOS4_CLKDIV_STATCPU) & 0x11111111)
+	while (__raw_readl(cpufreq->cmu_regs + EXYNOS4_CLKDIV_STATCPU)
+	       & 0x11111111)
 		cpu_relax();
 
 	/* Change Divider - CPU1 */
 	tmp = apll_freq_4x12[div_index].clk_div_cpu1;
 
-	__raw_writel(tmp, EXYNOS4_CLKDIV_CPU1);
-	if (soc_is_exynos4212())
-		stat_cpu1 = 0x11;
-	else
-		stat_cpu1 = 0x111;
+	__raw_writel(tmp, cpufreq->cmu_regs + EXYNOS4_CLKDIV_CPU1);
 
-	while (__raw_readl(EXYNOS4_CLKDIV_STATCPU1) & stat_cpu1)
+	do {
 		cpu_relax();
+		tmp = __raw_readl(cpufreq->cmu_regs + EXYNOS4_CLKDIV_STATCPU1);
+	} while (tmp != 0x0);
 }
 
 static void exynos4x12_set_apll(unsigned int index)
@@ -133,7 +134,7 @@ static void exynos4x12_set_apll(unsigned int index)
 
 	do {
 		cpu_relax();
-		tmp = (__raw_readl(EXYNOS4_CLKMUX_STATCPU)
+		tmp = (__raw_readl(cpufreq->cmu_regs + EXYNOS4_CLKMUX_STATCPU)
 			>> EXYNOS4_CLKSRC_CPU_MUXCORE_SHIFT);
 		tmp &= 0x7;
 	} while (tmp != 0x2);
@@ -145,7 +146,7 @@ static void exynos4x12_set_apll(unsigned int index)
 
 	do {
 		cpu_relax();
-		tmp = __raw_readl(EXYNOS4_CLKMUX_STATCPU);
+		tmp = __raw_readl(cpufreq->cmu_regs + EXYNOS4_CLKMUX_STATCPU);
 		tmp &= EXYNOS4_CLKMUX_STATCPU_MUXCORE_MASK;
 	} while (tmp != (0x1 << EXYNOS4_CLKSRC_CPU_MUXCORE_SHIFT));
 }
@@ -164,8 +165,30 @@ static void exynos4x12_set_frequency(unsigned int old_index,
 
 int exynos4x12_cpufreq_init(struct exynos_dvfs_info *info)
 {
+	struct device_node *np;
 	unsigned long rate;
 
+	/*
+	 * HACK: This is a temporary workaround to get access to clock
+	 * controller registers directly and remove static mappings and
+	 * dependencies on platform headers. It is necessary to enable
+	 * Exynos multi-platform support and will be removed together with
+	 * this whole driver as soon as Exynos gets migrated to use
+	 * cpufreq-cpu0 driver.
+	 */
+	np = of_find_compatible_node(NULL, NULL, "samsung,exynos4412-clock");
+	if (!np) {
+		pr_err("%s: failed to find clock controller DT node\n",
+			__func__);
+		return -ENODEV;
+	}
+
+	info->cmu_regs = of_iomap(np, 0);
+	if (!info->cmu_regs) {
+		pr_err("%s: failed to map CMU registers\n", __func__);
+		return -EFAULT;
+	}
+
 	cpu_clk = clk_get(NULL, "armclk");
 	if (IS_ERR(cpu_clk))
 		return PTR_ERR(cpu_clk);
@@ -184,7 +207,7 @@ int exynos4x12_cpufreq_init(struct exynos_dvfs_info *info)
 	if (IS_ERR(mout_apll))
 		goto err_mout_apll;
 
-	if (soc_is_exynos4212())
+	if (info->type == EXYNOS_SOC_4212)
 		apll_freq_4x12 = apll_freq_4212;
 	else
 		apll_freq_4x12 = apll_freq_4412;
@@ -197,6 +220,8 @@ int exynos4x12_cpufreq_init(struct exynos_dvfs_info *info)
 	info->freq_table = exynos4x12_freq_table;
 	info->set_freq = exynos4x12_set_frequency;
 
+	cpufreq = info;
+
 	return 0;
 
 err_mout_apll:
diff --git a/drivers/cpufreq/exynos5250-cpufreq.c b/drivers/cpufreq/exynos5250-cpufreq.c
index 363a0b3fe1b..c91ce69dc63 100644
--- a/drivers/cpufreq/exynos5250-cpufreq.c
+++ b/drivers/cpufreq/exynos5250-cpufreq.c
@@ -16,8 +16,8 @@
 #include <linux/io.h>
 #include <linux/slab.h>
 #include <linux/cpufreq.h>
-
-#include <mach/map.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
 
 #include "exynos-cpufreq.h"
 
@@ -25,6 +25,7 @@ static struct clk *cpu_clk;
 static struct clk *moutcore;
 static struct clk *mout_mpll;
 static struct clk *mout_apll;
+static struct exynos_dvfs_info *cpufreq;
 
 static unsigned int exynos5250_volt_table[] = {
 	1300000, 1250000, 1225000, 1200000, 1150000,
@@ -87,17 +88,18 @@ static void set_clkdiv(unsigned int div_index)
 
 	tmp = apll_freq_5250[div_index].clk_div_cpu0;
 
-	__raw_writel(tmp, EXYNOS5_CLKDIV_CPU0);
+	__raw_writel(tmp, cpufreq->cmu_regs + EXYNOS5_CLKDIV_CPU0);
 
-	while (__raw_readl(EXYNOS5_CLKDIV_STATCPU0) & 0x11111111)
+	while (__raw_readl(cpufreq->cmu_regs + EXYNOS5_CLKDIV_STATCPU0)
+	       & 0x11111111)
 		cpu_relax();
 
 	/* Change Divider - CPU1 */
 	tmp = apll_freq_5250[div_index].clk_div_cpu1;
 
-	__raw_writel(tmp, EXYNOS5_CLKDIV_CPU1);
+	__raw_writel(tmp, cpufreq->cmu_regs + EXYNOS5_CLKDIV_CPU1);
 
-	while (__raw_readl(EXYNOS5_CLKDIV_STATCPU1) & 0x11)
+	while (__raw_readl(cpufreq->cmu_regs + EXYNOS5_CLKDIV_STATCPU1) & 0x11)
 		cpu_relax();
 }
 
@@ -111,7 +113,8 @@ static void set_apll(unsigned int index)
 
 	do {
 		cpu_relax();
-		tmp = (__raw_readl(EXYNOS5_CLKMUX_STATCPU) >> 16);
+		tmp = (__raw_readl(cpufreq->cmu_regs + EXYNOS5_CLKMUX_STATCPU)
+			>> 16);
 		tmp &= 0x7;
 	} while (tmp != 0x2);
 
@@ -122,7 +125,7 @@ static void set_apll(unsigned int index)
 
 	do {
 		cpu_relax();
-		tmp = __raw_readl(EXYNOS5_CLKMUX_STATCPU);
+		tmp = __raw_readl(cpufreq->cmu_regs + EXYNOS5_CLKMUX_STATCPU);
 		tmp &= (0x7 << 16);
 	} while (tmp != (0x1 << 16));
 }
@@ -141,8 +144,30 @@ static void exynos5250_set_frequency(unsigned int old_index,
 
 int exynos5250_cpufreq_init(struct exynos_dvfs_info *info)
 {
+	struct device_node *np;
 	unsigned long rate;
 
+	/*
+	 * HACK: This is a temporary workaround to get access to clock
+	 * controller registers directly and remove static mappings and
+	 * dependencies on platform headers. It is necessary to enable
+	 * Exynos multi-platform support and will be removed together with
+	 * this whole driver as soon as Exynos gets migrated to use
+	 * cpufreq-cpu0 driver.
+	 */
+	np = of_find_compatible_node(NULL, NULL, "samsung,exynos5250-clock");
+	if (!np) {
+		pr_err("%s: failed to find clock controller DT node\n",
+			__func__);
+		return -ENODEV;
+	}
+
+	info->cmu_regs = of_iomap(np, 0);
+	if (!info->cmu_regs) {
+		pr_err("%s: failed to map CMU registers\n", __func__);
+		return -EFAULT;
+	}
+
 	cpu_clk = clk_get(NULL, "armclk");
 	if (IS_ERR(cpu_clk))
 		return PTR_ERR(cpu_clk);
@@ -169,6 +194,8 @@ int exynos5250_cpufreq_init(struct exynos_dvfs_info *info)
 	info->freq_table = exynos5250_freq_table;
 	info->set_freq = exynos5250_set_frequency;
 
+	cpufreq = info;
+
 	return 0;
 
 err_mout_apll:
diff --git a/drivers/cpufreq/intel_pstate.c b/drivers/cpufreq/intel_pstate.c
index 099967302bf..eab8ccfe6be 100644
--- a/drivers/cpufreq/intel_pstate.c
+++ b/drivers/cpufreq/intel_pstate.c
@@ -37,6 +37,7 @@
 #define BYT_RATIOS		0x66a
 #define BYT_VIDS		0x66b
 #define BYT_TURBO_RATIOS	0x66c
+#define BYT_TURBO_VIDS		0x66d
 
 
 #define FRAC_BITS 6
@@ -70,8 +71,9 @@ struct pstate_data {
 };
 
 struct vid_data {
-	int32_t min;
-	int32_t max;
+	int min;
+	int max;
+	int turbo;
 	int32_t ratio;
 };
 
@@ -359,14 +361,14 @@ static int byt_get_min_pstate(void)
 {
 	u64 value;
 	rdmsrl(BYT_RATIOS, value);
-	return (value >> 8) & 0xFF;
+	return (value >> 8) & 0x3F;
 }
 
 static int byt_get_max_pstate(void)
 {
 	u64 value;
 	rdmsrl(BYT_RATIOS, value);
-	return (value >> 16) & 0xFF;
+	return (value >> 16) & 0x3F;
 }
 
 static int byt_get_turbo_pstate(void)
@@ -393,6 +395,9 @@ static void byt_set_pstate(struct cpudata *cpudata, int pstate)
 	vid_fp = clamp_t(int32_t, vid_fp, cpudata->vid.min, cpudata->vid.max);
 	vid = fp_toint(vid_fp);
 
+	if (pstate > cpudata->pstate.max_pstate)
+		vid = cpudata->vid.turbo;
+
 	val |= vid;
 
 	wrmsrl(MSR_IA32_PERF_CTL, val);
@@ -402,13 +407,17 @@ static void byt_get_vid(struct cpudata *cpudata)
 {
 	u64 value;
 
+
 	rdmsrl(BYT_VIDS, value);
-	cpudata->vid.min = int_tofp((value >> 8) & 0x7f);
-	cpudata->vid.max = int_tofp((value >> 16) & 0x7f);
+	cpudata->vid.min = int_tofp((value >> 8) & 0x3f);
+	cpudata->vid.max = int_tofp((value >> 16) & 0x3f);
 	cpudata->vid.ratio = div_fp(
 		cpudata->vid.max - cpudata->vid.min,
 		int_tofp(cpudata->pstate.max_pstate -
 			cpudata->pstate.min_pstate));
+
+	rdmsrl(BYT_TURBO_VIDS, value);
+	cpudata->vid.turbo = value & 0x7f;
 }
 
 
@@ -545,12 +554,7 @@ static void intel_pstate_get_cpu_pstates(struct cpudata *cpu)
 
 	if (pstate_funcs.get_vid)
 		pstate_funcs.get_vid(cpu);
-
-	/*
-	 * goto max pstate so we don't slow up boot if we are built-in if we are
-	 * a module we will take care of it during normal operation
-	 */
-	intel_pstate_set_pstate(cpu, cpu->pstate.max_pstate);
+	intel_pstate_set_pstate(cpu, cpu->pstate.min_pstate);
 }
 
 static inline void intel_pstate_calc_busy(struct cpudata *cpu,
@@ -695,11 +699,6 @@ static int intel_pstate_init_cpu(unsigned int cpunum)
 	cpu = all_cpu_data[cpunum];
 
 	intel_pstate_get_cpu_pstates(cpu);
-	if (!cpu->pstate.current_pstate) {
-		all_cpu_data[cpunum] = NULL;
-		kfree(cpu);
-		return -ENODATA;
-	}
 
 	cpu->cpu = cpunum;
 
@@ -710,7 +709,6 @@ static int intel_pstate_init_cpu(unsigned int cpunum)
 	cpu->timer.expires = jiffies + HZ/100;
 	intel_pstate_busy_pid_reset(cpu);
 	intel_pstate_sample(cpu);
-	intel_pstate_set_pstate(cpu, cpu->pstate.max_pstate);
 
 	add_timer_on(&cpu->timer, cpunum);
 
diff --git a/drivers/cpufreq/longhaul.c b/drivers/cpufreq/longhaul.c
index d00e5d1abd2..5c4369b5d83 100644
--- a/drivers/cpufreq/longhaul.c
+++ b/drivers/cpufreq/longhaul.c
@@ -242,7 +242,7 @@ static void do_powersaver(int cx_address, unsigned int mults_index,
  * Sets a new clock ratio.
  */
 
-static void longhaul_setstate(struct cpufreq_policy *policy,
+static int longhaul_setstate(struct cpufreq_policy *policy,
 		unsigned int table_index)
 {
 	unsigned int mults_index;
@@ -258,10 +258,12 @@ static void longhaul_setstate(struct cpufreq_policy *policy,
 	/* Safety precautions */
 	mult = mults[mults_index & 0x1f];
 	if (mult == -1)
-		return;
+		return -EINVAL;
+
 	speed = calc_speed(mult);
 	if ((speed > highest_speed) || (speed < lowest_speed))
-		return;
+		return -EINVAL;
+
 	/* Voltage transition before frequency transition? */
 	if (can_scale_voltage && longhaul_index < table_index)
 		dir = 1;
@@ -269,8 +271,6 @@ static void longhaul_setstate(struct cpufreq_policy *policy,
 	freqs.old = calc_speed(longhaul_get_cpu_mult());
 	freqs.new = speed;
 
-	cpufreq_freq_transition_begin(policy, &freqs);
-
 	pr_debug("Setting to FSB:%dMHz Mult:%d.%dx (%s)\n",
 			fsb, mult/10, mult%10, print_speed(speed/1000));
 retry_loop:
@@ -385,12 +385,14 @@ retry_loop:
 			goto retry_loop;
 		}
 	}
-	/* Report true CPU frequency */
-	cpufreq_freq_transition_end(policy, &freqs, 0);
 
-	if (!bm_timeout)
+	if (!bm_timeout) {
 		printk(KERN_INFO PFX "Warning: Timeout while waiting for "
 				"idle PCI bus.\n");
+		return -EBUSY;
+	}
+
+	return 0;
 }
 
 /*
@@ -631,9 +633,10 @@ static int longhaul_target(struct cpufreq_policy *policy,
 	unsigned int i;
 	unsigned int dir = 0;
 	u8 vid, current_vid;
+	int retval = 0;
 
 	if (!can_scale_voltage)
-		longhaul_setstate(policy, table_index);
+		retval = longhaul_setstate(policy, table_index);
 	else {
 		/* On test system voltage transitions exceeding single
 		 * step up or down were turning motherboard off. Both
@@ -648,7 +651,7 @@ static int longhaul_target(struct cpufreq_policy *policy,
 		while (i != table_index) {
 			vid = (longhaul_table[i].driver_data >> 8) & 0x1f;
 			if (vid != current_vid) {
-				longhaul_setstate(policy, i);
+				retval = longhaul_setstate(policy, i);
 				current_vid = vid;
 				msleep(200);
 			}
@@ -657,10 +660,11 @@ static int longhaul_target(struct cpufreq_policy *policy,
 			else
 				i--;
 		}
-		longhaul_setstate(policy, table_index);
+		retval = longhaul_setstate(policy, table_index);
 	}
+
 	longhaul_index = table_index;
-	return 0;
+	return retval;
 }
 
 
@@ -968,7 +972,15 @@ static void __exit longhaul_exit(void)
 
 	for (i = 0; i < numscales; i++) {
 		if (mults[i] == maxmult) {
+			struct cpufreq_freqs freqs;
+
+			freqs.old = policy->cur;
+			freqs.new = longhaul_table[i].frequency;
+			freqs.flags = 0;
+
+			cpufreq_freq_transition_begin(policy, &freqs);
 			longhaul_setstate(policy, i);
+			cpufreq_freq_transition_end(policy, &freqs, 0);
 			break;
 		}
 	}
diff --git a/drivers/cpufreq/loongson2_cpufreq.c b/drivers/cpufreq/loongson2_cpufreq.c
index f0bc31f5db2..d4add862194 100644
--- a/drivers/cpufreq/loongson2_cpufreq.c
+++ b/drivers/cpufreq/loongson2_cpufreq.c
@@ -62,7 +62,7 @@ static int loongson2_cpufreq_target(struct cpufreq_policy *policy,
 	set_cpus_allowed_ptr(current, &cpus_allowed);
 
 	/* setting the cpu frequency */
-	clk_set_rate(policy->clk, freq);
+	clk_set_rate(policy->clk, freq * 1000);
 
 	return 0;
 }
@@ -92,7 +92,7 @@ static int loongson2_cpufreq_cpu_init(struct cpufreq_policy *policy)
 	     i++)
 		loongson2_clockmod_table[i].frequency = (rate * i) / 8;
 
-	ret = clk_set_rate(cpuclk, rate);
+	ret = clk_set_rate(cpuclk, rate * 1000);
 	if (ret) {
 		clk_put(cpuclk);
 		return ret;
diff --git a/drivers/cpufreq/powernow-k6.c b/drivers/cpufreq/powernow-k6.c
index 49f120e1bc7..78904e6ca4a 100644
--- a/drivers/cpufreq/powernow-k6.c
+++ b/drivers/cpufreq/powernow-k6.c
@@ -138,22 +138,14 @@ static void powernow_k6_set_cpu_multiplier(unsigned int best_i)
 static int powernow_k6_target(struct cpufreq_policy *policy,
 		unsigned int best_i)
 {
-	struct cpufreq_freqs freqs;
 
 	if (clock_ratio[best_i].driver_data > max_multiplier) {
 		printk(KERN_ERR PFX "invalid target frequency\n");
 		return -EINVAL;
 	}
 
-	freqs.old = busfreq * powernow_k6_get_cpu_multiplier();
-	freqs.new = busfreq * clock_ratio[best_i].driver_data;
-
-	cpufreq_freq_transition_begin(policy, &freqs);
-
 	powernow_k6_set_cpu_multiplier(best_i);
 
-	cpufreq_freq_transition_end(policy, &freqs, 0);
-
 	return 0;
 }
 
@@ -227,9 +219,20 @@ have_busfreq:
 static int powernow_k6_cpu_exit(struct cpufreq_policy *policy)
 {
 	unsigned int i;
-	for (i = 0; i < 8; i++) {
-		if (i == max_multiplier)
+
+	for (i = 0; (clock_ratio[i].frequency != CPUFREQ_TABLE_END); i++) {
+		if (clock_ratio[i].driver_data == max_multiplier) {
+			struct cpufreq_freqs freqs;
+
+			freqs.old = policy->cur;
+			freqs.new = clock_ratio[i].frequency;
+			freqs.flags = 0;
+
+			cpufreq_freq_transition_begin(policy, &freqs);
 			powernow_k6_target(policy, i);
+			cpufreq_freq_transition_end(policy, &freqs, 0);
+			break;
+		}
 	}
 	return 0;
 }
diff --git a/drivers/cpufreq/powernow-k7.c b/drivers/cpufreq/powernow-k7.c
index f911645c3f6..e61e224475a 100644
--- a/drivers/cpufreq/powernow-k7.c
+++ b/drivers/cpufreq/powernow-k7.c
@@ -269,8 +269,6 @@ static int powernow_target(struct cpufreq_policy *policy, unsigned int index)
 
 	freqs.new = powernow_table[index].frequency;
 
-	cpufreq_freq_transition_begin(policy, &freqs);
-
 	/* Now do the magic poking into the MSRs.  */
 
 	if (have_a0 == 1)	/* A0 errata 5 */
@@ -290,8 +288,6 @@ static int powernow_target(struct cpufreq_policy *policy, unsigned int index)
 	if (have_a0 == 1)
 		local_irq_enable();
 
-	cpufreq_freq_transition_end(policy, &freqs, 0);
-
 	return 0;
 }
 
diff --git a/drivers/cpufreq/powernv-cpufreq.c b/drivers/cpufreq/powernv-cpufreq.c
index 9edccc63245..af4968813e7 100644
--- a/drivers/cpufreq/powernv-cpufreq.c
+++ b/drivers/cpufreq/powernv-cpufreq.c
@@ -29,6 +29,7 @@
 
 #include <asm/cputhreads.h>
 #include <asm/reg.h>
+#include <asm/smp.h> /* Required for cpu_sibling_mask() in UP configs */
 
 #define POWERNV_MAX_PSTATES	256
 
diff --git a/drivers/cpufreq/ppc-corenet-cpufreq.c b/drivers/cpufreq/ppc-corenet-cpufreq.c
index b7e677be1df..0af618abeba 100644
--- a/drivers/cpufreq/ppc-corenet-cpufreq.c
+++ b/drivers/cpufreq/ppc-corenet-cpufreq.c
@@ -138,6 +138,7 @@ static int corenet_cpufreq_cpu_init(struct cpufreq_policy *policy)
 	struct cpufreq_frequency_table *table;
 	struct cpu_data *data;
 	unsigned int cpu = policy->cpu;
+	u64 transition_latency_hz;
 
 	np = of_get_cpu_node(cpu, NULL);
 	if (!np)
@@ -205,8 +206,10 @@ static int corenet_cpufreq_cpu_init(struct cpufreq_policy *policy)
 	for_each_cpu(i, per_cpu(cpu_mask, cpu))
 		per_cpu(cpu_data, i) = data;
 
+	transition_latency_hz = 12ULL * NSEC_PER_SEC;
 	policy->cpuinfo.transition_latency =
-				(12 * NSEC_PER_SEC) / fsl_get_sys_freq();
+		do_div(transition_latency_hz, fsl_get_sys_freq());
+
 	of_node_put(np);
 
 	return 0;
diff --git a/drivers/cpufreq/s3c24xx-cpufreq.c b/drivers/cpufreq/s3c24xx-cpufreq.c
index be1b2b5c975..227ebf7c1ee 100644
--- a/drivers/cpufreq/s3c24xx-cpufreq.c
+++ b/drivers/cpufreq/s3c24xx-cpufreq.c
@@ -141,6 +141,7 @@ static int s3c_cpufreq_calcdivs(struct s3c_cpufreq_config *cfg)
 
 static void s3c_cpufreq_setfvco(struct s3c_cpufreq_config *cfg)
 {
+	cfg->mpll = _clk_mpll;
 	(cfg->info->set_fvco)(cfg);
 }
 
diff --git a/drivers/cpufreq/unicore2-cpufreq.c b/drivers/cpufreq/unicore2-cpufreq.c
index 8d045afa7fb..6f9dfa80563 100644
--- a/drivers/cpufreq/unicore2-cpufreq.c
+++ b/drivers/cpufreq/unicore2-cpufreq.c
@@ -60,9 +60,7 @@ static int __init ucv2_cpu_init(struct cpufreq_policy *policy)
 	policy->max = policy->cpuinfo.max_freq = 1000000;
 	policy->cpuinfo.transition_latency = CPUFREQ_ETERNAL;
 	policy->clk = clk_get(NULL, "MAIN_CLK");
-	if (IS_ERR(policy->clk))
-		return PTR_ERR(policy->clk);
-	return 0;
+	return PTR_ERR_OR_ZERO(policy->clk);
 }
 
 static struct cpufreq_driver ucv2_driver = {
diff --git a/drivers/cpuidle/Kconfig.arm b/drivers/cpuidle/Kconfig.arm
index 97ccc31dbdd..ae1d78ea7df 100644
--- a/drivers/cpuidle/Kconfig.arm
+++ b/drivers/cpuidle/Kconfig.arm
@@ -1,6 +1,11 @@
 #
 # ARM CPU Idle drivers
 #
+config ARM_ARMADA_370_XP_CPUIDLE
+	bool "CPU Idle Driver for Armada 370/XP family processors"
+	depends on ARCH_MVEBU
+	help
+	  Select this to enable cpuidle on Armada 370/XP processors.
 
 config ARM_BIG_LITTLE_CPUIDLE
 	bool "Support for ARM big.LITTLE processors"
@@ -44,3 +49,9 @@ config ARM_AT91_CPUIDLE
 	depends on ARCH_AT91
 	help
 	  Select this to enable cpuidle for AT91 processors
+
+config ARM_EXYNOS_CPUIDLE
+	bool "Cpu Idle Driver for the Exynos processors"
+	depends on ARCH_EXYNOS
+	help
+	  Select this to enable cpuidle for Exynos processors
diff --git a/drivers/cpuidle/Makefile b/drivers/cpuidle/Makefile
index f71ae1b373c..cd3ab59f846 100644
--- a/drivers/cpuidle/Makefile
+++ b/drivers/cpuidle/Makefile
@@ -7,12 +7,14 @@ obj-$(CONFIG_ARCH_NEEDS_CPU_IDLE_COUPLED) += coupled.o
 
 ##################################################################################
 # ARM SoC drivers
+obj-$(CONFIG_ARM_ARMADA_370_XP_CPUIDLE) += cpuidle-armada-370-xp.o
 obj-$(CONFIG_ARM_BIG_LITTLE_CPUIDLE)	+= cpuidle-big_little.o
 obj-$(CONFIG_ARM_HIGHBANK_CPUIDLE)	+= cpuidle-calxeda.o
 obj-$(CONFIG_ARM_KIRKWOOD_CPUIDLE)	+= cpuidle-kirkwood.o
 obj-$(CONFIG_ARM_ZYNQ_CPUIDLE)		+= cpuidle-zynq.o
 obj-$(CONFIG_ARM_U8500_CPUIDLE)         += cpuidle-ux500.o
 obj-$(CONFIG_ARM_AT91_CPUIDLE)          += cpuidle-at91.o
+obj-$(CONFIG_ARM_EXYNOS_CPUIDLE)        += cpuidle-exynos.o
 
 ###############################################################################
 # POWERPC drivers
diff --git a/drivers/cpuidle/cpuidle-armada-370-xp.c b/drivers/cpuidle/cpuidle-armada-370-xp.c
new file mode 100644
index 00000000000..28587d0f394
--- /dev/null
+++ b/drivers/cpuidle/cpuidle-armada-370-xp.c
@@ -0,0 +1,93 @@
+/*
+ * Marvell Armada 370 and Armada XP SoC cpuidle driver
+ *
+ * Copyright (C) 2014 Marvell
+ *
+ * Nadav Haklai <nadavh@marvell.com>
+ * 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.
+ *
+ * Maintainer: Gregory CLEMENT <gregory.clement@free-electrons.com>
+ */
+
+#include <linux/cpu_pm.h>
+#include <linux/cpuidle.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/suspend.h>
+#include <linux/platform_device.h>
+#include <asm/cpuidle.h>
+
+#define ARMADA_370_XP_MAX_STATES	3
+#define ARMADA_370_XP_FLAG_DEEP_IDLE	0x10000
+
+static int (*armada_370_xp_cpu_suspend)(int);
+
+static int armada_370_xp_enter_idle(struct cpuidle_device *dev,
+				struct cpuidle_driver *drv,
+				int index)
+{
+	int ret;
+	bool deepidle = false;
+	cpu_pm_enter();
+
+	if (drv->states[index].flags & ARMADA_370_XP_FLAG_DEEP_IDLE)
+		deepidle = true;
+
+	ret = armada_370_xp_cpu_suspend(deepidle);
+	if (ret)
+		return ret;
+
+	cpu_pm_exit();
+
+	return index;
+}
+
+static struct cpuidle_driver armada_370_xp_idle_driver = {
+	.name			= "armada_370_xp_idle",
+	.states[0]		= ARM_CPUIDLE_WFI_STATE,
+	.states[1]		= {
+		.enter			= armada_370_xp_enter_idle,
+		.exit_latency		= 10,
+		.power_usage		= 50,
+		.target_residency	= 100,
+		.flags			= CPUIDLE_FLAG_TIME_VALID,
+		.name			= "MV CPU IDLE",
+		.desc			= "CPU power down",
+	},
+	.states[2]		= {
+		.enter			= armada_370_xp_enter_idle,
+		.exit_latency		= 100,
+		.power_usage		= 5,
+		.target_residency	= 1000,
+		.flags			= CPUIDLE_FLAG_TIME_VALID |
+						ARMADA_370_XP_FLAG_DEEP_IDLE,
+		.name			= "MV CPU DEEP IDLE",
+		.desc			= "CPU and L2 Fabric power down",
+	},
+	.state_count = ARMADA_370_XP_MAX_STATES,
+};
+
+static int armada_370_xp_cpuidle_probe(struct platform_device *pdev)
+{
+
+	armada_370_xp_cpu_suspend = (void *)(pdev->dev.platform_data);
+	return cpuidle_register(&armada_370_xp_idle_driver, NULL);
+}
+
+static struct platform_driver armada_370_xp_cpuidle_plat_driver = {
+	.driver = {
+		.name = "cpuidle-armada-370-xp",
+		.owner = THIS_MODULE,
+	},
+	.probe = armada_370_xp_cpuidle_probe,
+};
+
+module_platform_driver(armada_370_xp_cpuidle_plat_driver);
+
+MODULE_AUTHOR("Gregory CLEMENT <gregory.clement@free-electrons.com>");
+MODULE_DESCRIPTION("Armada 370/XP cpu idle driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/cpuidle/cpuidle-exynos.c b/drivers/cpuidle/cpuidle-exynos.c
new file mode 100644
index 00000000000..7c015126382
--- /dev/null
+++ b/drivers/cpuidle/cpuidle-exynos.c
@@ -0,0 +1,99 @@
+/* linux/arch/arm/mach-exynos/cpuidle.c
+ *
+ * Copyright (c) 2011 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+*/
+
+#include <linux/cpuidle.h>
+#include <linux/cpu_pm.h>
+#include <linux/export.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+
+#include <asm/proc-fns.h>
+#include <asm/suspend.h>
+#include <asm/cpuidle.h>
+
+static void (*exynos_enter_aftr)(void);
+
+static int idle_finisher(unsigned long flags)
+{
+	exynos_enter_aftr();
+	cpu_do_idle();
+
+	return 1;
+}
+
+static int exynos_enter_core0_aftr(struct cpuidle_device *dev,
+				struct cpuidle_driver *drv,
+				int index)
+{
+	cpu_pm_enter();
+	cpu_suspend(0, idle_finisher);
+	cpu_pm_exit();
+
+	return index;
+}
+
+static int exynos_enter_lowpower(struct cpuidle_device *dev,
+				struct cpuidle_driver *drv,
+				int index)
+{
+	int new_index = index;
+
+	/* AFTR can only be entered when cores other than CPU0 are offline */
+	if (num_online_cpus() > 1 || dev->cpu != 0)
+		new_index = drv->safe_state_index;
+
+	if (new_index == 0)
+		return arm_cpuidle_simple_enter(dev, drv, new_index);
+	else
+		return exynos_enter_core0_aftr(dev, drv, new_index);
+}
+
+static struct cpuidle_driver exynos_idle_driver = {
+	.name			= "exynos_idle",
+	.owner			= THIS_MODULE,
+	.states = {
+		[0] = ARM_CPUIDLE_WFI_STATE,
+		[1] = {
+			.enter			= exynos_enter_lowpower,
+			.exit_latency		= 300,
+			.target_residency	= 100000,
+			.flags			= CPUIDLE_FLAG_TIME_VALID,
+			.name			= "C1",
+			.desc			= "ARM power down",
+		},
+	},
+	.state_count = 2,
+	.safe_state_index = 0,
+};
+
+static int exynos_cpuidle_probe(struct platform_device *pdev)
+{
+	int ret;
+
+	exynos_enter_aftr = (void *)(pdev->dev.platform_data);
+
+	ret = cpuidle_register(&exynos_idle_driver, NULL);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to register cpuidle driver\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static struct platform_driver exynos_cpuidle_driver = {
+	.probe	= exynos_cpuidle_probe,
+	.driver = {
+		.name = "exynos_cpuidle",
+		.owner = THIS_MODULE,
+	},
+};
+
+module_platform_driver(exynos_cpuidle_driver);
diff --git a/drivers/crypto/caam/error.c b/drivers/crypto/caam/error.c
index 9f25f529602..0eabd81e1a9 100644
--- a/drivers/crypto/caam/error.c
+++ b/drivers/crypto/caam/error.c
@@ -16,9 +16,13 @@
 	char *tmp;						\
 								\
 	tmp = kmalloc(sizeof(format) + max_alloc, GFP_ATOMIC);	\
-	sprintf(tmp, format, param);				\
-	strcat(str, tmp);					\
-	kfree(tmp);						\
+	if (likely(tmp)) {					\
+		sprintf(tmp, format, param);			\
+		strcat(str, tmp);				\
+		kfree(tmp);					\
+	} else {						\
+		strcat(str, "kmalloc failure in SPRINTFCAT");	\
+	}							\
 }
 
 static void report_jump_idx(u32 status, char *outstr)
diff --git a/drivers/crypto/nx/nx-842.c b/drivers/crypto/nx/nx-842.c
index 1e5481d88a2..5ce8b576512 100644
--- a/drivers/crypto/nx/nx-842.c
+++ b/drivers/crypto/nx/nx-842.c
@@ -1197,12 +1197,7 @@ static int __init nx842_probe(struct vio_dev *viodev,
 	}
 
 	rcu_read_lock();
-	if (dev_set_drvdata(&viodev->dev, rcu_dereference(devdata))) {
-		rcu_read_unlock();
-		dev_err(&viodev->dev, "failed to set driver data for device\n");
-		ret = -1;
-		goto error;
-	}
+	dev_set_drvdata(&viodev->dev, rcu_dereference(devdata));
 	rcu_read_unlock();
 
 	if (sysfs_create_group(&viodev->dev.kobj, &nx842_attribute_group)) {
diff --git a/drivers/dma/Kconfig b/drivers/dma/Kconfig
index ba06d1d2f99..5c5863842de 100644
--- a/drivers/dma/Kconfig
+++ b/drivers/dma/Kconfig
@@ -197,7 +197,7 @@ config AMCC_PPC440SPE_ADMA
 
 config TIMB_DMA
 	tristate "Timberdale FPGA DMA support"
-	depends on MFD_TIMBERDALE || HAS_IOMEM
+	depends on MFD_TIMBERDALE
 	select DMA_ENGINE
 	help
 	  Enable support for the Timberdale FPGA DMA engine.
diff --git a/drivers/dma/dmaengine.c b/drivers/dma/dmaengine.c
index a886713937f..d5d30ed863c 100644
--- a/drivers/dma/dmaengine.c
+++ b/drivers/dma/dmaengine.c
@@ -1009,6 +1009,7 @@ static void dmaengine_unmap(struct kref *kref)
 		dma_unmap_page(dev, unmap->addr[i], unmap->len,
 			       DMA_BIDIRECTIONAL);
 	}
+	cnt = unmap->map_cnt;
 	mempool_free(unmap, __get_unmap_pool(cnt)->pool);
 }
 
@@ -1074,6 +1075,7 @@ dmaengine_get_unmap_data(struct device *dev, int nr, gfp_t flags)
 	memset(unmap, 0, sizeof(*unmap));
 	kref_init(&unmap->kref);
 	unmap->dev = dev;
+	unmap->map_cnt = nr;
 
 	return unmap;
 }
diff --git a/drivers/dma/dw/core.c b/drivers/dma/dw/core.c
index cfdbb92aae1..7a740769c2f 100644
--- a/drivers/dma/dw/core.c
+++ b/drivers/dma/dw/core.c
@@ -1548,11 +1548,6 @@ int dw_dma_probe(struct dw_dma_chip *chip, struct dw_dma_platform_data *pdata)
 	/* Disable BLOCK interrupts as well */
 	channel_clear_bit(dw, MASK.BLOCK, dw->all_chan_mask);
 
-	err = devm_request_irq(chip->dev, chip->irq, dw_dma_interrupt,
-			       IRQF_SHARED, "dw_dmac", dw);
-	if (err)
-		return err;
-
 	/* Create a pool of consistent memory blocks for hardware descriptors */
 	dw->desc_pool = dmam_pool_create("dw_dmac_desc_pool", chip->dev,
 					 sizeof(struct dw_desc), 4, 0);
@@ -1563,6 +1558,11 @@ int dw_dma_probe(struct dw_dma_chip *chip, struct dw_dma_platform_data *pdata)
 
 	tasklet_init(&dw->tasklet, dw_dma_tasklet, (unsigned long)dw);
 
+	err = request_irq(chip->irq, dw_dma_interrupt, IRQF_SHARED,
+			  "dw_dmac", dw);
+	if (err)
+		return err;
+
 	INIT_LIST_HEAD(&dw->dma.channels);
 	for (i = 0; i < nr_channels; i++) {
 		struct dw_dma_chan	*dwc = &dw->chan[i];
@@ -1667,6 +1667,7 @@ int dw_dma_remove(struct dw_dma_chip *chip)
 	dw_dma_off(dw);
 	dma_async_device_unregister(&dw->dma);
 
+	free_irq(chip->irq, dw);
 	tasklet_kill(&dw->tasklet);
 
 	list_for_each_entry_safe(dwc, _dwc, &dw->dma.channels,
diff --git a/drivers/dma/edma.c b/drivers/dma/edma.c
index cd04eb7b182..d08c4dedef3 100644
--- a/drivers/dma/edma.c
+++ b/drivers/dma/edma.c
@@ -57,14 +57,48 @@
 #define EDMA_MAX_SLOTS		MAX_NR_SG
 #define EDMA_DESCRIPTORS	16
 
+struct edma_pset {
+	u32				len;
+	dma_addr_t			addr;
+	struct edmacc_param		param;
+};
+
 struct edma_desc {
 	struct virt_dma_desc		vdesc;
 	struct list_head		node;
+	enum dma_transfer_direction	direction;
 	int				cyclic;
 	int				absync;
 	int				pset_nr;
+	struct edma_chan		*echan;
 	int				processed;
-	struct edmacc_param		pset[0];
+
+	/*
+	 * The following 4 elements are used for residue accounting.
+	 *
+	 * - processed_stat: the number of SG elements we have traversed
+	 * so far to cover accounting. This is updated directly to processed
+	 * during edma_callback and is always <= processed, because processed
+	 * refers to the number of pending transfer (programmed to EDMA
+	 * controller), where as processed_stat tracks number of transfers
+	 * accounted for so far.
+	 *
+	 * - residue: The amount of bytes we have left to transfer for this desc
+	 *
+	 * - residue_stat: The residue in bytes of data we have covered
+	 * so far for accounting. This is updated directly to residue
+	 * during callbacks to keep it current.
+	 *
+	 * - sg_len: Tracks the length of the current intermediate transfer,
+	 * this is required to update the residue during intermediate transfer
+	 * completion callback.
+	 */
+	int				processed_stat;
+	u32				sg_len;
+	u32				residue;
+	u32				residue_stat;
+
+	struct edma_pset		pset[0];
 };
 
 struct edma_cc;
@@ -136,12 +170,14 @@ static void edma_execute(struct edma_chan *echan)
 	/* Find out how many left */
 	left = edesc->pset_nr - edesc->processed;
 	nslots = min(MAX_NR_SG, left);
+	edesc->sg_len = 0;
 
 	/* Write descriptor PaRAM set(s) */
 	for (i = 0; i < nslots; i++) {
 		j = i + edesc->processed;
-		edma_write_slot(echan->slot[i], &edesc->pset[j]);
-		dev_dbg(echan->vchan.chan.device->dev,
+		edma_write_slot(echan->slot[i], &edesc->pset[j].param);
+		edesc->sg_len += edesc->pset[j].len;
+		dev_vdbg(echan->vchan.chan.device->dev,
 			"\n pset[%d]:\n"
 			"  chnum\t%d\n"
 			"  slot\t%d\n"
@@ -154,14 +190,14 @@ static void edma_execute(struct edma_chan *echan)
 			"  cidx\t%08x\n"
 			"  lkrld\t%08x\n",
 			j, echan->ch_num, echan->slot[i],
-			edesc->pset[j].opt,
-			edesc->pset[j].src,
-			edesc->pset[j].dst,
-			edesc->pset[j].a_b_cnt,
-			edesc->pset[j].ccnt,
-			edesc->pset[j].src_dst_bidx,
-			edesc->pset[j].src_dst_cidx,
-			edesc->pset[j].link_bcntrld);
+			edesc->pset[j].param.opt,
+			edesc->pset[j].param.src,
+			edesc->pset[j].param.dst,
+			edesc->pset[j].param.a_b_cnt,
+			edesc->pset[j].param.ccnt,
+			edesc->pset[j].param.src_dst_bidx,
+			edesc->pset[j].param.src_dst_cidx,
+			edesc->pset[j].param.link_bcntrld);
 		/* Link to the previous slot if not the last set */
 		if (i != (nslots - 1))
 			edma_link(echan->slot[i], echan->slot[i+1]);
@@ -182,11 +218,14 @@ static void edma_execute(struct edma_chan *echan)
 				  echan->ecc->dummy_slot);
 	}
 
-	edma_resume(echan->ch_num);
-
 	if (edesc->processed <= MAX_NR_SG) {
-		dev_dbg(dev, "first transfer starting %d\n", echan->ch_num);
+		dev_dbg(dev, "first transfer starting on channel %d\n",
+			echan->ch_num);
 		edma_start(echan->ch_num);
+	} else {
+		dev_dbg(dev, "chan: %d: completed %d elements, resuming\n",
+			echan->ch_num, edesc->processed);
+		edma_resume(echan->ch_num);
 	}
 
 	/*
@@ -195,7 +234,7 @@ static void edma_execute(struct edma_chan *echan)
 	 * MAX_NR_SG
 	 */
 	if (echan->missed) {
-		dev_dbg(dev, "missed event in execute detected\n");
+		dev_dbg(dev, "missed event on channel %d\n", echan->ch_num);
 		edma_clean_channel(echan->ch_num);
 		edma_stop(echan->ch_num);
 		edma_start(echan->ch_num);
@@ -240,6 +279,26 @@ static int edma_slave_config(struct edma_chan *echan,
 	return 0;
 }
 
+static int edma_dma_pause(struct edma_chan *echan)
+{
+	/* Pause/Resume only allowed with cyclic mode */
+	if (!echan->edesc->cyclic)
+		return -EINVAL;
+
+	edma_pause(echan->ch_num);
+	return 0;
+}
+
+static int edma_dma_resume(struct edma_chan *echan)
+{
+	/* Pause/Resume only allowed with cyclic mode */
+	if (!echan->edesc->cyclic)
+		return -EINVAL;
+
+	edma_resume(echan->ch_num);
+	return 0;
+}
+
 static int edma_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
 			unsigned long arg)
 {
@@ -255,6 +314,14 @@ static int edma_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
 		config = (struct dma_slave_config *)arg;
 		ret = edma_slave_config(echan, config);
 		break;
+	case DMA_PAUSE:
+		ret = edma_dma_pause(echan);
+		break;
+
+	case DMA_RESUME:
+		ret = edma_dma_resume(echan);
+		break;
+
 	default:
 		ret = -ENOSYS;
 	}
@@ -273,18 +340,23 @@ static int edma_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
  * @dma_length: Total length of the DMA transfer
  * @direction: Direction of the transfer
  */
-static int edma_config_pset(struct dma_chan *chan, struct edmacc_param *pset,
+static int edma_config_pset(struct dma_chan *chan, struct edma_pset *epset,
 	dma_addr_t src_addr, dma_addr_t dst_addr, u32 burst,
 	enum dma_slave_buswidth dev_width, unsigned int dma_length,
 	enum dma_transfer_direction direction)
 {
 	struct edma_chan *echan = to_edma_chan(chan);
 	struct device *dev = chan->device->dev;
+	struct edmacc_param *param = &epset->param;
 	int acnt, bcnt, ccnt, cidx;
 	int src_bidx, dst_bidx, src_cidx, dst_cidx;
 	int absync;
 
 	acnt = dev_width;
+
+	/* src/dst_maxburst == 0 is the same case as src/dst_maxburst == 1 */
+	if (!burst)
+		burst = 1;
 	/*
 	 * If the maxburst is equal to the fifo width, use
 	 * A-synced transfers. This allows for large contiguous
@@ -335,41 +407,50 @@ static int edma_config_pset(struct dma_chan *chan, struct edmacc_param *pset,
 		cidx = acnt * bcnt;
 	}
 
+	epset->len = dma_length;
+
 	if (direction == DMA_MEM_TO_DEV) {
 		src_bidx = acnt;
 		src_cidx = cidx;
 		dst_bidx = 0;
 		dst_cidx = 0;
+		epset->addr = src_addr;
 	} else if (direction == DMA_DEV_TO_MEM)  {
 		src_bidx = 0;
 		src_cidx = 0;
 		dst_bidx = acnt;
 		dst_cidx = cidx;
+		epset->addr = dst_addr;
+	} else if (direction == DMA_MEM_TO_MEM)  {
+		src_bidx = acnt;
+		src_cidx = cidx;
+		dst_bidx = acnt;
+		dst_cidx = cidx;
 	} else {
 		dev_err(dev, "%s: direction not implemented yet\n", __func__);
 		return -EINVAL;
 	}
 
-	pset->opt = EDMA_TCC(EDMA_CHAN_SLOT(echan->ch_num));
+	param->opt = EDMA_TCC(EDMA_CHAN_SLOT(echan->ch_num));
 	/* Configure A or AB synchronized transfers */
 	if (absync)
-		pset->opt |= SYNCDIM;
+		param->opt |= SYNCDIM;
 
-	pset->src = src_addr;
-	pset->dst = dst_addr;
+	param->src = src_addr;
+	param->dst = dst_addr;
 
-	pset->src_dst_bidx = (dst_bidx << 16) | src_bidx;
-	pset->src_dst_cidx = (dst_cidx << 16) | src_cidx;
+	param->src_dst_bidx = (dst_bidx << 16) | src_bidx;
+	param->src_dst_cidx = (dst_cidx << 16) | src_cidx;
 
-	pset->a_b_cnt = bcnt << 16 | acnt;
-	pset->ccnt = ccnt;
+	param->a_b_cnt = bcnt << 16 | acnt;
+	param->ccnt = ccnt;
 	/*
 	 * Only time when (bcntrld) auto reload is required is for
 	 * A-sync case, and in this case, a requirement of reload value
 	 * of SZ_64K-1 only is assured. 'link' is initially set to NULL
 	 * and then later will be populated by edma_execute.
 	 */
-	pset->link_bcntrld = 0xffffffff;
+	param->link_bcntrld = 0xffffffff;
 	return absync;
 }
 
@@ -399,23 +480,26 @@ static struct dma_async_tx_descriptor *edma_prep_slave_sg(
 		dev_width = echan->cfg.dst_addr_width;
 		burst = echan->cfg.dst_maxburst;
 	} else {
-		dev_err(dev, "%s: bad direction?\n", __func__);
+		dev_err(dev, "%s: bad direction: %d\n", __func__, direction);
 		return NULL;
 	}
 
 	if (dev_width == DMA_SLAVE_BUSWIDTH_UNDEFINED) {
-		dev_err(dev, "Undefined slave buswidth\n");
+		dev_err(dev, "%s: Undefined slave buswidth\n", __func__);
 		return NULL;
 	}
 
 	edesc = kzalloc(sizeof(*edesc) + sg_len *
 		sizeof(edesc->pset[0]), GFP_ATOMIC);
 	if (!edesc) {
-		dev_dbg(dev, "Failed to allocate a descriptor\n");
+		dev_err(dev, "%s: Failed to allocate a descriptor\n", __func__);
 		return NULL;
 	}
 
 	edesc->pset_nr = sg_len;
+	edesc->residue = 0;
+	edesc->direction = direction;
+	edesc->echan = echan;
 
 	/* Allocate a PaRAM slot, if needed */
 	nslots = min_t(unsigned, MAX_NR_SG, sg_len);
@@ -427,7 +511,8 @@ static struct dma_async_tx_descriptor *edma_prep_slave_sg(
 						EDMA_SLOT_ANY);
 			if (echan->slot[i] < 0) {
 				kfree(edesc);
-				dev_err(dev, "Failed to allocate slot\n");
+				dev_err(dev, "%s: Failed to allocate slot\n",
+					__func__);
 				return NULL;
 			}
 		}
@@ -450,16 +535,56 @@ static struct dma_async_tx_descriptor *edma_prep_slave_sg(
 		}
 
 		edesc->absync = ret;
+		edesc->residue += sg_dma_len(sg);
 
 		/* If this is the last in a current SG set of transactions,
 		   enable interrupts so that next set is processed */
 		if (!((i+1) % MAX_NR_SG))
-			edesc->pset[i].opt |= TCINTEN;
+			edesc->pset[i].param.opt |= TCINTEN;
 
 		/* If this is the last set, enable completion interrupt flag */
 		if (i == sg_len - 1)
-			edesc->pset[i].opt |= TCINTEN;
+			edesc->pset[i].param.opt |= TCINTEN;
 	}
+	edesc->residue_stat = edesc->residue;
+
+	return vchan_tx_prep(&echan->vchan, &edesc->vdesc, tx_flags);
+}
+
+struct dma_async_tx_descriptor *edma_prep_dma_memcpy(
+	struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
+	size_t len, unsigned long tx_flags)
+{
+	int ret;
+	struct edma_desc *edesc;
+	struct device *dev = chan->device->dev;
+	struct edma_chan *echan = to_edma_chan(chan);
+
+	if (unlikely(!echan || !len))
+		return NULL;
+
+	edesc = kzalloc(sizeof(*edesc) + sizeof(edesc->pset[0]), GFP_ATOMIC);
+	if (!edesc) {
+		dev_dbg(dev, "Failed to allocate a descriptor\n");
+		return NULL;
+	}
+
+	edesc->pset_nr = 1;
+
+	ret = edma_config_pset(chan, &edesc->pset[0], src, dest, 1,
+			       DMA_SLAVE_BUSWIDTH_4_BYTES, len, DMA_MEM_TO_MEM);
+	if (ret < 0)
+		return NULL;
+
+	edesc->absync = ret;
+
+	/*
+	 * Enable intermediate transfer chaining to re-trigger channel
+	 * on completion of every TR, and enable transfer-completion
+	 * interrupt on completion of the whole transfer.
+	 */
+	edesc->pset[0].param.opt |= ITCCHEN;
+	edesc->pset[0].param.opt |= TCINTEN;
 
 	return vchan_tx_prep(&echan->vchan, &edesc->vdesc, tx_flags);
 }
@@ -491,12 +616,12 @@ static struct dma_async_tx_descriptor *edma_prep_dma_cyclic(
 		dev_width = echan->cfg.dst_addr_width;
 		burst = echan->cfg.dst_maxburst;
 	} else {
-		dev_err(dev, "%s: bad direction?\n", __func__);
+		dev_err(dev, "%s: bad direction: %d\n", __func__, direction);
 		return NULL;
 	}
 
 	if (dev_width == DMA_SLAVE_BUSWIDTH_UNDEFINED) {
-		dev_err(dev, "Undefined slave buswidth\n");
+		dev_err(dev, "%s: Undefined slave buswidth\n", __func__);
 		return NULL;
 	}
 
@@ -521,16 +646,18 @@ static struct dma_async_tx_descriptor *edma_prep_dma_cyclic(
 	edesc = kzalloc(sizeof(*edesc) + nslots *
 		sizeof(edesc->pset[0]), GFP_ATOMIC);
 	if (!edesc) {
-		dev_dbg(dev, "Failed to allocate a descriptor\n");
+		dev_err(dev, "%s: Failed to allocate a descriptor\n", __func__);
 		return NULL;
 	}
 
 	edesc->cyclic = 1;
 	edesc->pset_nr = nslots;
+	edesc->residue = edesc->residue_stat = buf_len;
+	edesc->direction = direction;
+	edesc->echan = echan;
 
-	dev_dbg(dev, "%s: nslots=%d\n", __func__, nslots);
-	dev_dbg(dev, "%s: period_len=%d\n", __func__, period_len);
-	dev_dbg(dev, "%s: buf_len=%d\n", __func__, buf_len);
+	dev_dbg(dev, "%s: channel=%d nslots=%d period_len=%zu buf_len=%zu\n",
+		__func__, echan->ch_num, nslots, period_len, buf_len);
 
 	for (i = 0; i < nslots; i++) {
 		/* Allocate a PaRAM slot, if needed */
@@ -540,7 +667,8 @@ static struct dma_async_tx_descriptor *edma_prep_dma_cyclic(
 						EDMA_SLOT_ANY);
 			if (echan->slot[i] < 0) {
 				kfree(edesc);
-				dev_err(dev, "Failed to allocate slot\n");
+				dev_err(dev, "%s: Failed to allocate slot\n",
+					__func__);
 				return NULL;
 			}
 		}
@@ -564,8 +692,8 @@ static struct dma_async_tx_descriptor *edma_prep_dma_cyclic(
 		else
 			src_addr += period_len;
 
-		dev_dbg(dev, "%s: Configure period %d of buf:\n", __func__, i);
-		dev_dbg(dev,
+		dev_vdbg(dev, "%s: Configure period %d of buf:\n", __func__, i);
+		dev_vdbg(dev,
 			"\n pset[%d]:\n"
 			"  chnum\t%d\n"
 			"  slot\t%d\n"
@@ -578,14 +706,14 @@ static struct dma_async_tx_descriptor *edma_prep_dma_cyclic(
 			"  cidx\t%08x\n"
 			"  lkrld\t%08x\n",
 			i, echan->ch_num, echan->slot[i],
-			edesc->pset[i].opt,
-			edesc->pset[i].src,
-			edesc->pset[i].dst,
-			edesc->pset[i].a_b_cnt,
-			edesc->pset[i].ccnt,
-			edesc->pset[i].src_dst_bidx,
-			edesc->pset[i].src_dst_cidx,
-			edesc->pset[i].link_bcntrld);
+			edesc->pset[i].param.opt,
+			edesc->pset[i].param.src,
+			edesc->pset[i].param.dst,
+			edesc->pset[i].param.a_b_cnt,
+			edesc->pset[i].param.ccnt,
+			edesc->pset[i].param.src_dst_bidx,
+			edesc->pset[i].param.src_dst_cidx,
+			edesc->pset[i].param.link_bcntrld);
 
 		edesc->absync = ret;
 
@@ -593,7 +721,7 @@ static struct dma_async_tx_descriptor *edma_prep_dma_cyclic(
 		 * Enable interrupts for every period because callback
 		 * has to be called for every period.
 		 */
-		edesc->pset[i].opt |= TCINTEN;
+		edesc->pset[i].param.opt |= TCINTEN;
 	}
 
 	return vchan_tx_prep(&echan->vchan, &edesc->vdesc, tx_flags);
@@ -604,7 +732,6 @@ static void edma_callback(unsigned ch_num, u16 ch_status, void *data)
 	struct edma_chan *echan = data;
 	struct device *dev = echan->vchan.chan.device->dev;
 	struct edma_desc *edesc;
-	unsigned long flags;
 	struct edmacc_param p;
 
 	edesc = echan->edesc;
@@ -615,27 +742,34 @@ static void edma_callback(unsigned ch_num, u16 ch_status, void *data)
 
 	switch (ch_status) {
 	case EDMA_DMA_COMPLETE:
-		spin_lock_irqsave(&echan->vchan.lock, flags);
+		spin_lock(&echan->vchan.lock);
 
 		if (edesc) {
 			if (edesc->cyclic) {
 				vchan_cyclic_callback(&edesc->vdesc);
 			} else if (edesc->processed == edesc->pset_nr) {
 				dev_dbg(dev, "Transfer complete, stopping channel %d\n", ch_num);
+				edesc->residue = 0;
 				edma_stop(echan->ch_num);
 				vchan_cookie_complete(&edesc->vdesc);
 				edma_execute(echan);
 			} else {
 				dev_dbg(dev, "Intermediate transfer complete on channel %d\n", ch_num);
+
+				/* Update statistics for tx_status */
+				edesc->residue -= edesc->sg_len;
+				edesc->residue_stat = edesc->residue;
+				edesc->processed_stat = edesc->processed;
+
 				edma_execute(echan);
 			}
 		}
 
-		spin_unlock_irqrestore(&echan->vchan.lock, flags);
+		spin_unlock(&echan->vchan.lock);
 
 		break;
 	case EDMA_DMA_CC_ERROR:
-		spin_lock_irqsave(&echan->vchan.lock, flags);
+		spin_lock(&echan->vchan.lock);
 
 		edma_read_slot(EDMA_CHAN_SLOT(echan->slot[0]), &p);
 
@@ -666,7 +800,7 @@ static void edma_callback(unsigned ch_num, u16 ch_status, void *data)
 			edma_trigger_channel(echan->ch_num);
 		}
 
-		spin_unlock_irqrestore(&echan->vchan.lock, flags);
+		spin_unlock(&echan->vchan.lock);
 
 		break;
 	default:
@@ -702,7 +836,7 @@ static int edma_alloc_chan_resources(struct dma_chan *chan)
 	echan->alloced = true;
 	echan->slot[0] = echan->ch_num;
 
-	dev_dbg(dev, "allocated channel for %u:%u\n",
+	dev_dbg(dev, "allocated channel %d for %u:%u\n", echan->ch_num,
 		EDMA_CTLR(echan->ch_num), EDMA_CHAN_SLOT(echan->ch_num));
 
 	return 0;
@@ -754,23 +888,52 @@ static void edma_issue_pending(struct dma_chan *chan)
 	spin_unlock_irqrestore(&echan->vchan.lock, flags);
 }
 
-static size_t edma_desc_size(struct edma_desc *edesc)
+static u32 edma_residue(struct edma_desc *edesc)
 {
+	bool dst = edesc->direction == DMA_DEV_TO_MEM;
+	struct edma_pset *pset = edesc->pset;
+	dma_addr_t done, pos;
 	int i;
-	size_t size;
-
-	if (edesc->absync)
-		for (size = i = 0; i < edesc->pset_nr; i++)
-			size += (edesc->pset[i].a_b_cnt & 0xffff) *
-				(edesc->pset[i].a_b_cnt >> 16) *
-				 edesc->pset[i].ccnt;
-	else
-		size = (edesc->pset[0].a_b_cnt & 0xffff) *
-			(edesc->pset[0].a_b_cnt >> 16) +
-			(edesc->pset[0].a_b_cnt & 0xffff) *
-			(SZ_64K - 1) * edesc->pset[0].ccnt;
-
-	return size;
+
+	/*
+	 * We always read the dst/src position from the first RamPar
+	 * pset. That's the one which is active now.
+	 */
+	pos = edma_get_position(edesc->echan->slot[0], dst);
+
+	/*
+	 * Cyclic is simple. Just subtract pset[0].addr from pos.
+	 *
+	 * We never update edesc->residue in the cyclic case, so we
+	 * can tell the remaining room to the end of the circular
+	 * buffer.
+	 */
+	if (edesc->cyclic) {
+		done = pos - pset->addr;
+		edesc->residue_stat = edesc->residue - done;
+		return edesc->residue_stat;
+	}
+
+	/*
+	 * For SG operation we catch up with the last processed
+	 * status.
+	 */
+	pset += edesc->processed_stat;
+
+	for (i = edesc->processed_stat; i < edesc->processed; i++, pset++) {
+		/*
+		 * If we are inside this pset address range, we know
+		 * this is the active one. Get the current delta and
+		 * stop walking the psets.
+		 */
+		if (pos >= pset->addr && pos < pset->addr + pset->len)
+			return edesc->residue_stat - (pos - pset->addr);
+
+		/* Otherwise mark it done and update residue_stat. */
+		edesc->processed_stat++;
+		edesc->residue_stat -= pset->len;
+	}
+	return edesc->residue_stat;
 }
 
 /* Check request completion status */
@@ -788,13 +951,10 @@ static enum dma_status edma_tx_status(struct dma_chan *chan,
 		return ret;
 
 	spin_lock_irqsave(&echan->vchan.lock, flags);
-	vdesc = vchan_find_desc(&echan->vchan, cookie);
-	if (vdesc) {
-		txstate->residue = edma_desc_size(to_edma_desc(&vdesc->tx));
-	} else if (echan->edesc && echan->edesc->vdesc.tx.cookie == cookie) {
-		struct edma_desc *edesc = echan->edesc;
-		txstate->residue = edma_desc_size(edesc);
-	}
+	if (echan->edesc && echan->edesc->vdesc.tx.cookie == cookie)
+		txstate->residue = edma_residue(echan->edesc);
+	else if ((vdesc = vchan_find_desc(&echan->vchan, cookie)))
+		txstate->residue = to_edma_desc(&vdesc->tx)->residue;
 	spin_unlock_irqrestore(&echan->vchan.lock, flags);
 
 	return ret;
@@ -820,18 +980,43 @@ static void __init edma_chan_init(struct edma_cc *ecc,
 	}
 }
 
+#define EDMA_DMA_BUSWIDTHS	(BIT(DMA_SLAVE_BUSWIDTH_1_BYTE) | \
+				 BIT(DMA_SLAVE_BUSWIDTH_2_BYTES) | \
+				 BIT(DMA_SLAVE_BUSWIDTH_4_BYTES))
+
+static int edma_dma_device_slave_caps(struct dma_chan *dchan,
+				      struct dma_slave_caps *caps)
+{
+	caps->src_addr_widths = EDMA_DMA_BUSWIDTHS;
+	caps->dstn_addr_widths = EDMA_DMA_BUSWIDTHS;
+	caps->directions = BIT(DMA_DEV_TO_MEM) | BIT(DMA_MEM_TO_DEV);
+	caps->cmd_pause = true;
+	caps->cmd_terminate = true;
+	caps->residue_granularity = DMA_RESIDUE_GRANULARITY_DESCRIPTOR;
+
+	return 0;
+}
+
 static void edma_dma_init(struct edma_cc *ecc, struct dma_device *dma,
 			  struct device *dev)
 {
 	dma->device_prep_slave_sg = edma_prep_slave_sg;
 	dma->device_prep_dma_cyclic = edma_prep_dma_cyclic;
+	dma->device_prep_dma_memcpy = edma_prep_dma_memcpy;
 	dma->device_alloc_chan_resources = edma_alloc_chan_resources;
 	dma->device_free_chan_resources = edma_free_chan_resources;
 	dma->device_issue_pending = edma_issue_pending;
 	dma->device_tx_status = edma_tx_status;
 	dma->device_control = edma_control;
+	dma->device_slave_caps = edma_dma_device_slave_caps;
 	dma->dev = dev;
 
+	/*
+	 * code using dma memcpy must make sure alignment of
+	 * length is at dma->copy_align boundary.
+	 */
+	dma->copy_align = DMA_SLAVE_BUSWIDTH_4_BYTES;
+
 	INIT_LIST_HEAD(&dma->channels);
 }
 
@@ -859,6 +1044,8 @@ static int edma_probe(struct platform_device *pdev)
 
 	dma_cap_zero(ecc->dma_slave.cap_mask);
 	dma_cap_set(DMA_SLAVE, ecc->dma_slave.cap_mask);
+	dma_cap_set(DMA_CYCLIC, ecc->dma_slave.cap_mask);
+	dma_cap_set(DMA_MEMCPY, ecc->dma_slave.cap_mask);
 
 	edma_dma_init(ecc, &ecc->dma_slave, &pdev->dev);
 
diff --git a/drivers/dma/fsl-edma.c b/drivers/dma/fsl-edma.c
index 381e793184b..b396a7fb53a 100644
--- a/drivers/dma/fsl-edma.c
+++ b/drivers/dma/fsl-edma.c
@@ -968,7 +968,17 @@ static struct platform_driver fsl_edma_driver = {
 	.remove		= fsl_edma_remove,
 };
 
-module_platform_driver(fsl_edma_driver);
+static int __init fsl_edma_init(void)
+{
+	return platform_driver_register(&fsl_edma_driver);
+}
+subsys_initcall(fsl_edma_init);
+
+static void __exit fsl_edma_exit(void)
+{
+	platform_driver_unregister(&fsl_edma_driver);
+}
+module_exit(fsl_edma_exit);
 
 MODULE_ALIAS("platform:fsl-edma");
 MODULE_DESCRIPTION("Freescale eDMA engine driver");
diff --git a/drivers/dma/mv_xor.c b/drivers/dma/mv_xor.c
index 766b68ed505..394cbc5c93e 100644
--- a/drivers/dma/mv_xor.c
+++ b/drivers/dma/mv_xor.c
@@ -191,12 +191,10 @@ static void mv_set_mode(struct mv_xor_chan *chan,
 
 static void mv_chan_activate(struct mv_xor_chan *chan)
 {
-	u32 activation;
-
 	dev_dbg(mv_chan_to_devp(chan), " activate chan.\n");
-	activation = readl_relaxed(XOR_ACTIVATION(chan));
-	activation |= 0x1;
-	writel_relaxed(activation, XOR_ACTIVATION(chan));
+
+	/* writel ensures all descriptors are flushed before activation */
+	writel(BIT(0), XOR_ACTIVATION(chan));
 }
 
 static char mv_chan_is_busy(struct mv_xor_chan *chan)
diff --git a/drivers/dma/sa11x0-dma.c b/drivers/dma/sa11x0-dma.c
index ab26d46bbe1..5ebdfbc1051 100644
--- a/drivers/dma/sa11x0-dma.c
+++ b/drivers/dma/sa11x0-dma.c
@@ -113,11 +113,9 @@ struct sa11x0_dma_phy {
 	struct sa11x0_dma_desc	*txd_load;
 	unsigned		sg_done;
 	struct sa11x0_dma_desc	*txd_done;
-#ifdef CONFIG_PM_SLEEP
 	u32			dbs[2];
 	u32			dbt[2];
 	u32			dcsr;
-#endif
 };
 
 struct sa11x0_dma_dev {
@@ -984,7 +982,6 @@ static int sa11x0_dma_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
 static int sa11x0_dma_suspend(struct device *dev)
 {
 	struct sa11x0_dma_dev *d = dev_get_drvdata(dev);
@@ -1054,7 +1051,6 @@ static int sa11x0_dma_resume(struct device *dev)
 
 	return 0;
 }
-#endif
 
 static const struct dev_pm_ops sa11x0_dma_pm_ops = {
 	.suspend_noirq = sa11x0_dma_suspend,
diff --git a/drivers/dma/sirf-dma.c b/drivers/dma/sirf-dma.c
index a1bd8298d55..03f7820fa33 100644
--- a/drivers/dma/sirf-dma.c
+++ b/drivers/dma/sirf-dma.c
@@ -666,7 +666,7 @@ static struct dma_chan *of_dma_sirfsoc_xlate(struct of_phandle_args *dma_spec,
 	struct sirfsoc_dma *sdma = ofdma->of_dma_data;
 	unsigned int request = dma_spec->args[0];
 
-	if (request > SIRFSOC_DMA_CHANNELS)
+	if (request >= SIRFSOC_DMA_CHANNELS)
 		return NULL;
 
 	return dma_get_slave_channel(&sdma->channels[request].chan);
diff --git a/drivers/edac/edac_mc.c b/drivers/edac/edac_mc.c
index 33edd676634..2c694b5297c 100644
--- a/drivers/edac/edac_mc.c
+++ b/drivers/edac/edac_mc.c
@@ -1018,7 +1018,7 @@ static void edac_ce_error(struct mem_ctl_info *mci,
 	}
 	edac_inc_ce_error(mci, enable_per_layer_report, pos, error_count);
 
-	if (mci->scrub_mode & SCRUB_SW_SRC) {
+	if (mci->scrub_mode == SCRUB_SW_SRC) {
 		/*
 			* Some memory controllers (called MCs below) can remap
 			* memory so that it is still available at a different
diff --git a/drivers/edac/i82875p_edac.c b/drivers/edac/i82875p_edac.c
index 8d0450b9b9a..64b68320249 100644
--- a/drivers/edac/i82875p_edac.c
+++ b/drivers/edac/i82875p_edac.c
@@ -275,7 +275,6 @@ static int i82875p_setup_overfl_dev(struct pci_dev *pdev,
 {
 	struct pci_dev *dev;
 	void __iomem *window;
-	int err;
 
 	*ovrfl_pdev = NULL;
 	*ovrfl_window = NULL;
@@ -293,13 +292,8 @@ static int i82875p_setup_overfl_dev(struct pci_dev *pdev,
 		if (dev == NULL)
 			return 1;
 
-		err = pci_bus_add_device(dev);
-		if (err) {
-			i82875p_printk(KERN_ERR,
-				"%s(): pci_bus_add_device() Failed\n",
-				__func__);
-		}
 		pci_bus_assign_resources(dev->bus);
+		pci_bus_add_device(dev);
 	}
 
 	*ovrfl_pdev = dev;
diff --git a/drivers/edac/mce_amd.c b/drivers/edac/mce_amd.c
index 51b9caa0b02..5f43620d580 100644
--- a/drivers/edac/mce_amd.c
+++ b/drivers/edac/mce_amd.c
@@ -6,7 +6,6 @@
 static struct amd_decoder_ops *fam_ops;
 
 static u8 xec_mask	 = 0xf;
-static u8 nb_err_cpumask = 0xf;
 
 static bool report_gart_errors;
 static void (*nb_bus_decoder)(int node_id, struct mce *m);
@@ -852,7 +851,6 @@ static int __init mce_amd_init(void)
 		break;
 
 	case 0x14:
-		nb_err_cpumask  = 0x3;
 		fam_ops->mc0_mce = cat_mc0_mce;
 		fam_ops->mc1_mce = cat_mc1_mce;
 		fam_ops->mc2_mce = k8_mc2_mce;
diff --git a/drivers/extcon/Kconfig b/drivers/extcon/Kconfig
index be56e8ac95e..aebde489c29 100644
--- a/drivers/extcon/Kconfig
+++ b/drivers/extcon/Kconfig
@@ -28,13 +28,13 @@ config EXTCON_ADC_JACK
 	  Say Y here to enable extcon device driver based on ADC values.
 
 config EXTCON_MAX14577
-	tristate "MAX14577 EXTCON Support"
+	tristate "MAX14577/77836 EXTCON Support"
 	depends on MFD_MAX14577
 	select IRQ_DOMAIN
 	select REGMAP_I2C
 	help
 	  If you say yes here you get support for the MUIC device of
-	  Maxim MAX14577 PMIC. The MAX14577 MUIC is a USB port accessory
+	  Maxim MAX14577/77836. The MAX14577/77836 MUIC is a USB port accessory
 	  detector and switch.
 
 config EXTCON_MAX77693
diff --git a/drivers/extcon/extcon-adc-jack.c b/drivers/extcon/extcon-adc-jack.c
index e23f1c2e505..e18f95be373 100644
--- a/drivers/extcon/extcon-adc-jack.c
+++ b/drivers/extcon/extcon-adc-jack.c
@@ -39,7 +39,7 @@
  * @chan:		iio channel being queried.
  */
 struct adc_jack_data {
-	struct extcon_dev edev;
+	struct extcon_dev *edev;
 
 	const char **cable_names;
 	int num_cables;
@@ -64,7 +64,7 @@ static void adc_jack_handler(struct work_struct *work)
 
 	ret = iio_read_channel_raw(data->chan, &adc_val);
 	if (ret < 0) {
-		dev_err(&data->edev.dev, "read channel() error: %d\n", ret);
+		dev_err(&data->edev->dev, "read channel() error: %d\n", ret);
 		return;
 	}
 
@@ -80,7 +80,7 @@ static void adc_jack_handler(struct work_struct *work)
 	}
 	/* if no def has met, it means state = 0 (no cables attached) */
 
-	extcon_set_state(&data->edev, state);
+	extcon_set_state(data->edev, state);
 }
 
 static irqreturn_t adc_jack_irq_thread(int irq, void *_data)
@@ -102,33 +102,33 @@ static int adc_jack_probe(struct platform_device *pdev)
 	if (!data)
 		return -ENOMEM;
 
-	data->edev.name = pdata->name;
-
 	if (!pdata->cable_names) {
-		err = -EINVAL;
 		dev_err(&pdev->dev, "error: cable_names not defined.\n");
-		goto out;
+		return -EINVAL;
 	}
 
-	data->edev.dev.parent = &pdev->dev;
-	data->edev.supported_cable = pdata->cable_names;
+	data->edev = devm_extcon_dev_allocate(&pdev->dev, pdata->cable_names);
+	if (IS_ERR(data->edev)) {
+		dev_err(&pdev->dev, "failed to allocate extcon device\n");
+		return -ENOMEM;
+	}
+	data->edev->dev.parent = &pdev->dev;
+	data->edev->name = pdata->name;
 
 	/* Check the length of array and set num_cables */
-	for (i = 0; data->edev.supported_cable[i]; i++)
+	for (i = 0; data->edev->supported_cable[i]; i++)
 		;
 	if (i == 0 || i > SUPPORTED_CABLE_MAX) {
-		err = -EINVAL;
 		dev_err(&pdev->dev, "error: pdata->cable_names size = %d\n",
 				i - 1);
-		goto out;
+		return -EINVAL;
 	}
 	data->num_cables = i;
 
 	if (!pdata->adc_conditions ||
 			!pdata->adc_conditions[0].state) {
-		err = -EINVAL;
 		dev_err(&pdev->dev, "error: adc_conditions not defined.\n");
-		goto out;
+		return -EINVAL;
 	}
 	data->adc_conditions = pdata->adc_conditions;
 
@@ -138,10 +138,8 @@ static int adc_jack_probe(struct platform_device *pdev)
 	data->num_conditions = i;
 
 	data->chan = iio_channel_get(&pdev->dev, pdata->consumer_channel);
-	if (IS_ERR(data->chan)) {
-		err = PTR_ERR(data->chan);
-		goto out;
-	}
+	if (IS_ERR(data->chan))
+		return PTR_ERR(data->chan);
 
 	data->handling_delay = msecs_to_jiffies(pdata->handling_delay_ms);
 
@@ -149,15 +147,14 @@ static int adc_jack_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, data);
 
-	err = extcon_dev_register(&data->edev);
+	err = devm_extcon_dev_register(&pdev->dev, data->edev);
 	if (err)
-		goto out;
+		return err;
 
 	data->irq = platform_get_irq(pdev, 0);
 	if (!data->irq) {
 		dev_err(&pdev->dev, "platform_get_irq failed\n");
-		err = -ENODEV;
-		goto err_irq;
+		return -ENODEV;
 	}
 
 	err = request_any_context_irq(data->irq, adc_jack_irq_thread,
@@ -165,15 +162,10 @@ static int adc_jack_probe(struct platform_device *pdev)
 
 	if (err < 0) {
 		dev_err(&pdev->dev, "error: irq %d\n", data->irq);
-		goto err_irq;
+		return err;
 	}
 
 	return 0;
-
-err_irq:
-	extcon_dev_unregister(&data->edev);
-out:
-	return err;
 }
 
 static int adc_jack_remove(struct platform_device *pdev)
@@ -182,7 +174,6 @@ static int adc_jack_remove(struct platform_device *pdev)
 
 	free_irq(data->irq, data);
 	cancel_work_sync(&data->handler.work);
-	extcon_dev_unregister(&data->edev);
 
 	return 0;
 }
diff --git a/drivers/extcon/extcon-arizona.c b/drivers/extcon/extcon-arizona.c
index 98a14f6143a..6c84e3d1204 100644
--- a/drivers/extcon/extcon-arizona.c
+++ b/drivers/extcon/extcon-arizona.c
@@ -91,7 +91,7 @@ struct arizona_extcon_info {
 
 	int hpdet_ip;
 
-	struct extcon_dev edev;
+	struct extcon_dev *edev;
 };
 
 static const struct arizona_micd_config micd_default_modes[] = {
@@ -546,7 +546,7 @@ static irqreturn_t arizona_hpdet_irq(int irq, void *data)
 	}
 
 	/* If the cable was removed while measuring ignore the result */
-	ret = extcon_get_cable_state_(&info->edev, ARIZONA_CABLE_MECHANICAL);
+	ret = extcon_get_cable_state_(info->edev, ARIZONA_CABLE_MECHANICAL);
 	if (ret < 0) {
 		dev_err(arizona->dev, "Failed to check cable state: %d\n",
 			ret);
@@ -581,7 +581,7 @@ static irqreturn_t arizona_hpdet_irq(int irq, void *data)
 	else
 		report = ARIZONA_CABLE_HEADPHONE;
 
-	ret = extcon_set_cable_state_(&info->edev, report, true);
+	ret = extcon_set_cable_state_(info->edev, report, true);
 	if (ret != 0)
 		dev_err(arizona->dev, "Failed to report HP/line: %d\n",
 			ret);
@@ -664,7 +664,7 @@ err:
 			   ARIZONA_ACCDET_MODE_MASK, ARIZONA_ACCDET_MODE_MIC);
 
 	/* Just report headphone */
-	ret = extcon_update_state(&info->edev,
+	ret = extcon_update_state(info->edev,
 				  1 << ARIZONA_CABLE_HEADPHONE,
 				  1 << ARIZONA_CABLE_HEADPHONE);
 	if (ret != 0)
@@ -723,7 +723,7 @@ err:
 			   ARIZONA_ACCDET_MODE_MASK, ARIZONA_ACCDET_MODE_MIC);
 
 	/* Just report headphone */
-	ret = extcon_update_state(&info->edev,
+	ret = extcon_update_state(info->edev,
 				  1 << ARIZONA_CABLE_HEADPHONE,
 				  1 << ARIZONA_CABLE_HEADPHONE);
 	if (ret != 0)
@@ -764,7 +764,7 @@ static void arizona_micd_detect(struct work_struct *work)
 	mutex_lock(&info->lock);
 
 	/* If the cable was removed while measuring ignore the result */
-	ret = extcon_get_cable_state_(&info->edev, ARIZONA_CABLE_MECHANICAL);
+	ret = extcon_get_cable_state_(info->edev, ARIZONA_CABLE_MECHANICAL);
 	if (ret < 0) {
 		dev_err(arizona->dev, "Failed to check cable state: %d\n",
 				ret);
@@ -812,7 +812,7 @@ static void arizona_micd_detect(struct work_struct *work)
 	if (info->detecting && (val & ARIZONA_MICD_LVL_8)) {
 		arizona_identify_headphone(info);
 
-		ret = extcon_update_state(&info->edev,
+		ret = extcon_update_state(info->edev,
 					  1 << ARIZONA_CABLE_MICROPHONE,
 					  1 << ARIZONA_CABLE_MICROPHONE);
 
@@ -999,7 +999,7 @@ static irqreturn_t arizona_jackdet(int irq, void *data)
 
 	if (info->last_jackdet == present) {
 		dev_dbg(arizona->dev, "Detected jack\n");
-		ret = extcon_set_cable_state_(&info->edev,
+		ret = extcon_set_cable_state_(info->edev,
 					      ARIZONA_CABLE_MECHANICAL, true);
 
 		if (ret != 0)
@@ -1038,7 +1038,7 @@ static irqreturn_t arizona_jackdet(int irq, void *data)
 					 info->micd_ranges[i].key, 0);
 		input_sync(info->input);
 
-		ret = extcon_update_state(&info->edev, 0xffffffff, 0);
+		ret = extcon_update_state(info->edev, 0xffffffff, 0);
 		if (ret != 0)
 			dev_err(arizona->dev, "Removal report failed: %d\n",
 				ret);
@@ -1105,15 +1105,14 @@ static int arizona_extcon_probe(struct platform_device *pdev)
 	info = devm_kzalloc(&pdev->dev, sizeof(*info), GFP_KERNEL);
 	if (!info) {
 		dev_err(&pdev->dev, "Failed to allocate memory\n");
-		ret = -ENOMEM;
-		goto err;
+		return -ENOMEM;
 	}
 
 	info->micvdd = devm_regulator_get(arizona->dev, "MICVDD");
 	if (IS_ERR(info->micvdd)) {
 		ret = PTR_ERR(info->micvdd);
 		dev_err(arizona->dev, "Failed to get MICVDD: %d\n", ret);
-		goto err;
+		return ret;
 	}
 
 	mutex_init(&info->lock);
@@ -1151,15 +1150,19 @@ static int arizona_extcon_probe(struct platform_device *pdev)
 		break;
 	}
 
-	info->edev.name = "Headset Jack";
-	info->edev.dev.parent = arizona->dev;
-	info->edev.supported_cable = arizona_cable;
+	info->edev = devm_extcon_dev_allocate(&pdev->dev, arizona_cable);
+	if (IS_ERR(info->edev)) {
+		dev_err(&pdev->dev, "failed to allocate extcon device\n");
+		return -ENOMEM;
+	}
+	info->edev->name = "Headset Jack";
+	info->edev->dev.parent = arizona->dev;
 
-	ret = extcon_dev_register(&info->edev);
+	ret = devm_extcon_dev_register(&pdev->dev, info->edev);
 	if (ret < 0) {
 		dev_err(arizona->dev, "extcon_dev_register() failed: %d\n",
 			ret);
-		goto err;
+		return ret;
 	}
 
 	info->input = devm_input_allocate_device(&pdev->dev);
@@ -1410,8 +1413,6 @@ err_rise:
 err_input:
 err_register:
 	pm_runtime_disable(&pdev->dev);
-	extcon_dev_unregister(&info->edev);
-err:
 	return ret;
 }
 
@@ -1445,7 +1446,6 @@ static int arizona_extcon_remove(struct platform_device *pdev)
 	regmap_update_bits(arizona->regmap, ARIZONA_JACK_DETECT_ANALOGUE,
 			   ARIZONA_JD1_ENA, 0);
 	arizona_clk32k_disable(arizona);
-	extcon_dev_unregister(&info->edev);
 
 	return 0;
 }
diff --git a/drivers/extcon/extcon-class.c b/drivers/extcon/extcon-class.c
index 7ab21aa6eaa..18d42c0e458 100644
--- a/drivers/extcon/extcon-class.c
+++ b/drivers/extcon/extcon-class.c
@@ -565,6 +565,100 @@ static void dummy_sysfs_dev_release(struct device *dev)
 {
 }
 
+/*
+ * extcon_dev_allocate() - Allocate the memory of extcon device.
+ * @supported_cable:	Array of supported cable names ending with NULL.
+ *			If supported_cable is NULL, cable name related APIs
+ *			are disabled.
+ *
+ * This function allocates the memory for extcon device without allocating
+ * memory in each extcon provider driver and initialize default setting for
+ * extcon device.
+ *
+ * Return the pointer of extcon device if success or ERR_PTR(err) if fail
+ */
+struct extcon_dev *extcon_dev_allocate(const char **supported_cable)
+{
+	struct extcon_dev *edev;
+
+	edev = kzalloc(sizeof(*edev), GFP_KERNEL);
+	if (!edev)
+		return ERR_PTR(-ENOMEM);
+
+	edev->max_supported = 0;
+	edev->supported_cable = supported_cable;
+
+	return edev;
+}
+
+/*
+ * extcon_dev_free() - Free the memory of extcon device.
+ * @edev:	the extcon device to free
+ */
+void extcon_dev_free(struct extcon_dev *edev)
+{
+	kfree(edev);
+}
+EXPORT_SYMBOL_GPL(extcon_dev_free);
+
+static int devm_extcon_dev_match(struct device *dev, void *res, void *data)
+{
+	struct extcon_dev **r = res;
+
+	if (WARN_ON(!r || !*r))
+		return 0;
+
+	return *r == data;
+}
+
+static void devm_extcon_dev_release(struct device *dev, void *res)
+{
+	extcon_dev_free(*(struct extcon_dev **)res);
+}
+
+/**
+ * devm_extcon_dev_allocate - Allocate managed extcon device
+ * @dev:		device owning the extcon device being created
+ * @supported_cable:	Array of supported cable names ending with NULL.
+ *			If supported_cable is NULL, cable name related APIs
+ *			are disabled.
+ *
+ * This function manages automatically the memory of extcon device using device
+ * resource management and simplify the control of freeing the memory of extcon
+ * device.
+ *
+ * Returns the pointer memory of allocated extcon_dev if success
+ * or ERR_PTR(err) if fail
+ */
+struct extcon_dev *devm_extcon_dev_allocate(struct device *dev,
+					    const char **supported_cable)
+{
+	struct extcon_dev **ptr, *edev;
+
+	ptr = devres_alloc(devm_extcon_dev_release, sizeof(*ptr), GFP_KERNEL);
+	if (!ptr)
+		return ERR_PTR(-ENOMEM);
+
+	edev = extcon_dev_allocate(supported_cable);
+	if (IS_ERR(edev)) {
+		devres_free(ptr);
+		return edev;
+	}
+
+	*ptr = edev;
+	devres_add(dev, ptr);
+
+	return edev;
+}
+EXPORT_SYMBOL_GPL(devm_extcon_dev_allocate);
+
+void devm_extcon_dev_free(struct device *dev, struct extcon_dev *edev)
+{
+	WARN_ON(devres_release(dev, devm_extcon_dev_release,
+			       devm_extcon_dev_match, edev));
+}
+EXPORT_SYMBOL_GPL(devm_extcon_dev_free);
+
 /**
  * extcon_dev_register() - Register a new extcon device
  * @edev	: the new extcon device (should be allocated before calling)
@@ -819,6 +913,63 @@ void extcon_dev_unregister(struct extcon_dev *edev)
 }
 EXPORT_SYMBOL_GPL(extcon_dev_unregister);
 
+static void devm_extcon_dev_unreg(struct device *dev, void *res)
+{
+	extcon_dev_unregister(*(struct extcon_dev **)res);
+}
+
+/**
+ * devm_extcon_dev_register() - Resource-managed extcon_dev_register()
+ * @dev:	device to allocate extcon device
+ * @edev:	the new extcon device to register
+ *
+ * Managed extcon_dev_register() function. If extcon device is attached with
+ * this function, that extcon device is automatically unregistered on driver
+ * detach. Internally this function calls extcon_dev_register() function.
+ * To get more information, refer that function.
+ *
+ * If extcon device is registered with this function and the device needs to be
+ * unregistered separately, devm_extcon_dev_unregister() should be used.
+ *
+ * Returns 0 if success or negaive error number if failure.
+ */
+int devm_extcon_dev_register(struct device *dev, struct extcon_dev *edev)
+{
+	struct extcon_dev **ptr;
+	int ret;
+
+	ptr = devres_alloc(devm_extcon_dev_unreg, sizeof(*ptr), GFP_KERNEL);
+	if (!ptr)
+		return -ENOMEM;
+
+	ret = extcon_dev_register(edev);
+	if (ret) {
+		devres_free(ptr);
+		return ret;
+	}
+
+	*ptr = edev;
+	devres_add(dev, ptr);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(devm_extcon_dev_register);
+
+/**
+ * devm_extcon_dev_unregister() - Resource-managed extcon_dev_unregister()
+ * @dev:	device the extcon belongs to
+ * @edev:	the extcon device to unregister
+ *
+ * Unregister extcon device that is registered with devm_extcon_dev_register()
+ * function.
+ */
+void devm_extcon_dev_unregister(struct device *dev, struct extcon_dev *edev)
+{
+	WARN_ON(devres_release(dev, devm_extcon_dev_unreg,
+			       devm_extcon_dev_match, edev));
+}
+EXPORT_SYMBOL_GPL(devm_extcon_dev_unregister);
+
 #ifdef CONFIG_OF
 /*
  * extcon_get_edev_by_phandle - Get the extcon device from devicetree
diff --git a/drivers/extcon/extcon-gpio.c b/drivers/extcon/extcon-gpio.c
index 13d522255d8..645b2835681 100644
--- a/drivers/extcon/extcon-gpio.c
+++ b/drivers/extcon/extcon-gpio.c
@@ -32,7 +32,7 @@
 #include <linux/extcon/extcon-gpio.h>
 
 struct gpio_extcon_data {
-	struct extcon_dev edev;
+	struct extcon_dev *edev;
 	unsigned gpio;
 	bool gpio_active_low;
 	const char *state_on;
@@ -53,7 +53,7 @@ static void gpio_extcon_work(struct work_struct *work)
 	state = gpio_get_value(data->gpio);
 	if (data->gpio_active_low)
 		state = !state;
-	extcon_set_state(&data->edev, state);
+	extcon_set_state(data->edev, state);
 }
 
 static irqreturn_t gpio_irq_handler(int irq, void *dev_id)
@@ -67,9 +67,10 @@ static irqreturn_t gpio_irq_handler(int irq, void *dev_id)
 
 static ssize_t extcon_gpio_print_state(struct extcon_dev *edev, char *buf)
 {
-	struct gpio_extcon_data	*extcon_data =
-		container_of(edev, struct gpio_extcon_data, edev);
+	struct device *dev = edev->dev.parent;
+	struct gpio_extcon_data *extcon_data = dev_get_drvdata(dev);
 	const char *state;
+
 	if (extcon_get_state(edev))
 		state = extcon_data->state_on;
 	else
@@ -98,15 +99,21 @@ static int gpio_extcon_probe(struct platform_device *pdev)
 	if (!extcon_data)
 		return -ENOMEM;
 
-	extcon_data->edev.name = pdata->name;
-	extcon_data->edev.dev.parent = &pdev->dev;
+	extcon_data->edev = devm_extcon_dev_allocate(&pdev->dev, NULL);
+	if (IS_ERR(extcon_data->edev)) {
+		dev_err(&pdev->dev, "failed to allocate extcon device\n");
+		return -ENOMEM;
+	}
+	extcon_data->edev->name = pdata->name;
+	extcon_data->edev->dev.parent = &pdev->dev;
+
 	extcon_data->gpio = pdata->gpio;
 	extcon_data->gpio_active_low = pdata->gpio_active_low;
 	extcon_data->state_on = pdata->state_on;
 	extcon_data->state_off = pdata->state_off;
 	extcon_data->check_on_resume = pdata->check_on_resume;
 	if (pdata->state_on && pdata->state_off)
-		extcon_data->edev.print_state = extcon_gpio_print_state;
+		extcon_data->edev->print_state = extcon_gpio_print_state;
 
 	ret = devm_gpio_request_one(&pdev->dev, extcon_data->gpio, GPIOF_DIR_IN,
 				    pdev->name);
@@ -121,34 +128,27 @@ static int gpio_extcon_probe(struct platform_device *pdev)
 				msecs_to_jiffies(pdata->debounce);
 	}
 
-	ret = extcon_dev_register(&extcon_data->edev);
+	ret = devm_extcon_dev_register(&pdev->dev, extcon_data->edev);
 	if (ret < 0)
 		return ret;
 
 	INIT_DELAYED_WORK(&extcon_data->work, gpio_extcon_work);
 
 	extcon_data->irq = gpio_to_irq(extcon_data->gpio);
-	if (extcon_data->irq < 0) {
-		ret = extcon_data->irq;
-		goto err;
-	}
+	if (extcon_data->irq < 0)
+		return extcon_data->irq;
 
 	ret = request_any_context_irq(extcon_data->irq, gpio_irq_handler,
 				      pdata->irq_flags, pdev->name,
 				      extcon_data);
 	if (ret < 0)
-		goto err;
+		return ret;
 
 	platform_set_drvdata(pdev, extcon_data);
 	/* Perform initial detection */
 	gpio_extcon_work(&extcon_data->work.work);
 
 	return 0;
-
-err:
-	extcon_dev_unregister(&extcon_data->edev);
-
-	return ret;
 }
 
 static int gpio_extcon_remove(struct platform_device *pdev)
@@ -157,7 +157,6 @@ static int gpio_extcon_remove(struct platform_device *pdev)
 
 	cancel_delayed_work_sync(&extcon_data->work);
 	free_irq(extcon_data->irq, extcon_data);
-	extcon_dev_unregister(&extcon_data->edev);
 
 	return 0;
 }
diff --git a/drivers/extcon/extcon-max14577.c b/drivers/extcon/extcon-max14577.c
index 3846941801b..d49e891b567 100644
--- a/drivers/extcon/extcon-max14577.c
+++ b/drivers/extcon/extcon-max14577.c
@@ -1,8 +1,9 @@
 /*
- * extcon-max14577.c - MAX14577 extcon driver to support MAX14577 MUIC
+ * extcon-max14577.c - MAX14577/77836 extcon driver to support MUIC
  *
- * Copyright (C) 2013 Samsung Electrnoics
+ * Copyright (C) 2013,2014 Samsung Electrnoics
  * Chanwoo Choi <cw00.choi@samsung.com>
+ * Krzysztof Kozlowski <k.kozlowski@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
@@ -24,7 +25,6 @@
 #include <linux/mfd/max14577-private.h>
 #include <linux/extcon.h>
 
-#define	DEV_NAME			"max14577-muic"
 #define	DELAY_MS_DEFAULT		17000		/* unit: millisecond */
 
 enum max14577_muic_adc_debounce_time {
@@ -40,6 +40,42 @@ enum max14577_muic_status {
 	MAX14577_MUIC_STATUS_END,
 };
 
+/**
+ * struct max14577_muic_irq
+ * @irq: the index of irq list of MUIC device.
+ * @name: the name of irq.
+ * @virq: the virtual irq to use irq domain
+ */
+struct max14577_muic_irq {
+	unsigned int irq;
+	const char *name;
+	unsigned int virq;
+};
+
+static struct max14577_muic_irq max14577_muic_irqs[] = {
+	{ MAX14577_IRQ_INT1_ADC,	"muic-ADC" },
+	{ MAX14577_IRQ_INT1_ADCLOW,	"muic-ADCLOW" },
+	{ MAX14577_IRQ_INT1_ADCERR,	"muic-ADCError" },
+	{ MAX14577_IRQ_INT2_CHGTYP,	"muic-CHGTYP" },
+	{ MAX14577_IRQ_INT2_CHGDETRUN,	"muic-CHGDETRUN" },
+	{ MAX14577_IRQ_INT2_DCDTMR,	"muic-DCDTMR" },
+	{ MAX14577_IRQ_INT2_DBCHG,	"muic-DBCHG" },
+	{ MAX14577_IRQ_INT2_VBVOLT,	"muic-VBVOLT" },
+};
+
+static struct max14577_muic_irq max77836_muic_irqs[] = {
+	{ MAX14577_IRQ_INT1_ADC,	"muic-ADC" },
+	{ MAX14577_IRQ_INT1_ADCLOW,	"muic-ADCLOW" },
+	{ MAX14577_IRQ_INT1_ADCERR,	"muic-ADCError" },
+	{ MAX77836_IRQ_INT1_ADC1K,	"muic-ADC1K" },
+	{ MAX14577_IRQ_INT2_CHGTYP,	"muic-CHGTYP" },
+	{ MAX14577_IRQ_INT2_CHGDETRUN,	"muic-CHGDETRUN" },
+	{ MAX14577_IRQ_INT2_DCDTMR,	"muic-DCDTMR" },
+	{ MAX14577_IRQ_INT2_DBCHG,	"muic-DBCHG" },
+	{ MAX14577_IRQ_INT2_VBVOLT,	"muic-VBVOLT" },
+	{ MAX77836_IRQ_INT2_VIDRM,	"muic-VIDRM" },
+};
+
 struct max14577_muic_info {
 	struct device *dev;
 	struct max14577 *max14577;
@@ -48,6 +84,8 @@ struct max14577_muic_info {
 	int prev_chg_type;
 	u8 status[MAX14577_MUIC_STATUS_END];
 
+	struct max14577_muic_irq *muic_irqs;
+	unsigned int muic_irqs_num;
 	bool irq_adc;
 	bool irq_chg;
 	struct work_struct irq_work;
@@ -74,29 +112,6 @@ enum max14577_muic_cable_group {
 	MAX14577_CABLE_GROUP_CHG,
 };
 
-/**
- * struct max14577_muic_irq
- * @irq: the index of irq list of MUIC device.
- * @name: the name of irq.
- * @virq: the virtual irq to use irq domain
- */
-struct max14577_muic_irq {
-	unsigned int irq;
-	const char *name;
-	unsigned int virq;
-};
-
-static struct max14577_muic_irq muic_irqs[] = {
-	{ MAX14577_IRQ_INT1_ADC,	"muic-ADC" },
-	{ MAX14577_IRQ_INT1_ADCLOW,	"muic-ADCLOW" },
-	{ MAX14577_IRQ_INT1_ADCERR,	"muic-ADCError" },
-	{ MAX14577_IRQ_INT2_CHGTYP,	"muic-CHGTYP" },
-	{ MAX14577_IRQ_INT2_CHGDETRUN,	"muic-CHGDETRUN" },
-	{ MAX14577_IRQ_INT2_DCDTMR,	"muic-DCDTMR" },
-	{ MAX14577_IRQ_INT2_DBCHG,	"muic-DBCHG" },
-	{ MAX14577_IRQ_INT2_VBVOLT,	"muic-VBVOLT" },
-};
-
 /* Define supported accessory type */
 enum max14577_muic_acc_type {
 	MAX14577_MUIC_ADC_GROUND = 0x0,
@@ -528,21 +543,12 @@ static void max14577_muic_irq_work(struct work_struct *work)
 	return;
 }
 
-static irqreturn_t max14577_muic_irq_handler(int irq, void *data)
+/*
+ * Sets irq_adc or irq_chg in max14577_muic_info and returns 1.
+ * Returns 0 if irq_type does not match registered IRQ for this device type.
+ */
+static int max14577_parse_irq(struct max14577_muic_info *info, int irq_type)
 {
-	struct max14577_muic_info *info = data;
-	int i, irq_type = -1;
-
-	/*
-	 * We may be called multiple times for different nested IRQ-s.
-	 * Including changes in INT1_ADC and INT2_CGHTYP at once.
-	 * However we only need to know whether it was ADC, charger
-	 * or both interrupts so decode IRQ and turn on proper flags.
-	 */
-	for (i = 0; i < ARRAY_SIZE(muic_irqs); i++)
-		if (irq == muic_irqs[i].virq)
-			irq_type = muic_irqs[i].irq;
-
 	switch (irq_type) {
 	case MAX14577_IRQ_INT1_ADC:
 	case MAX14577_IRQ_INT1_ADCLOW:
@@ -550,7 +556,7 @@ static irqreturn_t max14577_muic_irq_handler(int irq, void *data)
 		/* Handle all of accessory except for
 		   type of charger accessory */
 		info->irq_adc = true;
-		break;
+		return 1;
 	case MAX14577_IRQ_INT2_CHGTYP:
 	case MAX14577_IRQ_INT2_CHGDETRUN:
 	case MAX14577_IRQ_INT2_DCDTMR:
@@ -558,8 +564,62 @@ static irqreturn_t max14577_muic_irq_handler(int irq, void *data)
 	case MAX14577_IRQ_INT2_VBVOLT:
 		/* Handle charger accessory */
 		info->irq_chg = true;
+		return 1;
+	default:
+		return 0;
+	}
+}
+
+/*
+ * Sets irq_adc or irq_chg in max14577_muic_info and returns 1.
+ * Returns 0 if irq_type does not match registered IRQ for this device type.
+ */
+static int max77836_parse_irq(struct max14577_muic_info *info, int irq_type)
+{
+	/* First check common max14577 interrupts */
+	if (max14577_parse_irq(info, irq_type))
+		return 1;
+
+	switch (irq_type) {
+	case MAX77836_IRQ_INT1_ADC1K:
+		info->irq_adc = true;
+		return 1;
+	case MAX77836_IRQ_INT2_VIDRM:
+		/* Handle charger accessory */
+		info->irq_chg = true;
+		return 1;
+	default:
+		return 0;
+	}
+}
+
+static irqreturn_t max14577_muic_irq_handler(int irq, void *data)
+{
+	struct max14577_muic_info *info = data;
+	int i, irq_type = -1;
+	bool irq_parsed;
+
+	/*
+	 * We may be called multiple times for different nested IRQ-s.
+	 * Including changes in INT1_ADC and INT2_CGHTYP at once.
+	 * However we only need to know whether it was ADC, charger
+	 * or both interrupts so decode IRQ and turn on proper flags.
+	 */
+	for (i = 0; i < info->muic_irqs_num; i++)
+		if (irq == info->muic_irqs[i].virq)
+			irq_type = info->muic_irqs[i].irq;
+
+	switch (info->max14577->dev_type) {
+	case MAXIM_DEVICE_TYPE_MAX77836:
+		irq_parsed = max77836_parse_irq(info, irq_type);
 		break;
+	case MAXIM_DEVICE_TYPE_MAX14577:
 	default:
+		irq_parsed = max14577_parse_irq(info, irq_type);
+		break;
+	}
+
+	if (!irq_parsed) {
 		dev_err(info->dev, "muic interrupt: irq %d occurred, skipped\n",
 				irq_type);
 		return IRQ_HANDLED;
@@ -644,13 +704,24 @@ static int max14577_muic_probe(struct platform_device *pdev)
 
 	INIT_WORK(&info->irq_work, max14577_muic_irq_work);
 
+	switch (max14577->dev_type) {
+	case MAXIM_DEVICE_TYPE_MAX77836:
+		info->muic_irqs = max77836_muic_irqs;
+		info->muic_irqs_num = ARRAY_SIZE(max77836_muic_irqs);
+		break;
+	case MAXIM_DEVICE_TYPE_MAX14577:
+	default:
+		info->muic_irqs = max14577_muic_irqs;
+		info->muic_irqs_num = ARRAY_SIZE(max14577_muic_irqs);
+	}
+
 	/* Support irq domain for max14577 MUIC device */
-	for (i = 0; i < ARRAY_SIZE(muic_irqs); i++) {
-		struct max14577_muic_irq *muic_irq = &muic_irqs[i];
+	for (i = 0; i < info->muic_irqs_num; i++) {
+		struct max14577_muic_irq *muic_irq = &info->muic_irqs[i];
 		unsigned int virq = 0;
 
 		virq = regmap_irq_get_virq(max14577->irq_data, muic_irq->irq);
-		if (!virq)
+		if (virq <= 0)
 			return -EINVAL;
 		muic_irq->virq = virq;
 
@@ -668,14 +739,16 @@ static int max14577_muic_probe(struct platform_device *pdev)
 	}
 
 	/* Initialize extcon device */
-	info->edev = devm_kzalloc(&pdev->dev, sizeof(*info->edev), GFP_KERNEL);
-	if (!info->edev) {
+	info->edev = devm_extcon_dev_allocate(&pdev->dev,
+					      max14577_extcon_cable);
+	if (IS_ERR(info->edev)) {
 		dev_err(&pdev->dev, "failed to allocate memory for extcon\n");
 		return -ENOMEM;
 	}
-	info->edev->name = DEV_NAME;
-	info->edev->supported_cable = max14577_extcon_cable;
-	ret = extcon_dev_register(info->edev);
+
+	info->edev->name = dev_name(&pdev->dev);
+
+	ret = devm_extcon_dev_register(&pdev->dev, info->edev);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to register extcon device\n");
 		return ret;
@@ -694,7 +767,7 @@ static int max14577_muic_probe(struct platform_device *pdev)
 			MAX14577_REG_DEVICEID, &id);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "failed to read revision number\n");
-		goto err_extcon;
+		return ret;
 	}
 	dev_info(info->dev, "device ID : 0x%x\n", id);
 
@@ -710,19 +783,10 @@ static int max14577_muic_probe(struct platform_device *pdev)
 	 * driver should notify cable state to upper layer.
 	 */
 	INIT_DELAYED_WORK(&info->wq_detcable, max14577_muic_detect_cable_wq);
-	ret = queue_delayed_work(system_power_efficient_wq, &info->wq_detcable,
+	queue_delayed_work(system_power_efficient_wq, &info->wq_detcable,
 			delay_jiffies);
-	if (ret < 0) {
-		dev_err(&pdev->dev,
-			"failed to schedule delayed work for cable detect\n");
-		goto err_extcon;
-	}
 
 	return ret;
-
-err_extcon:
-	extcon_dev_unregister(info->edev);
-	return ret;
 }
 
 static int max14577_muic_remove(struct platform_device *pdev)
@@ -730,23 +794,30 @@ static int max14577_muic_remove(struct platform_device *pdev)
 	struct max14577_muic_info *info = platform_get_drvdata(pdev);
 
 	cancel_work_sync(&info->irq_work);
-	extcon_dev_unregister(info->edev);
 
 	return 0;
 }
 
+static const struct platform_device_id max14577_muic_id[] = {
+	{ "max14577-muic", MAXIM_DEVICE_TYPE_MAX14577, },
+	{ "max77836-muic", MAXIM_DEVICE_TYPE_MAX77836, },
+	{ }
+};
+MODULE_DEVICE_TABLE(platform, max14577_muic_id);
+
 static struct platform_driver max14577_muic_driver = {
 	.driver		= {
-		.name	= DEV_NAME,
+		.name	= "max14577-muic",
 		.owner	= THIS_MODULE,
 	},
 	.probe		= max14577_muic_probe,
 	.remove		= max14577_muic_remove,
+	.id_table	= max14577_muic_id,
 };
 
 module_platform_driver(max14577_muic_driver);
 
-MODULE_DESCRIPTION("MAXIM 14577 Extcon driver");
-MODULE_AUTHOR("Chanwoo Choi <cw00.choi@samsung.com>");
+MODULE_DESCRIPTION("Maxim 14577/77836 Extcon driver");
+MODULE_AUTHOR("Chanwoo Choi <cw00.choi@samsung.com>, Krzysztof Kozlowski <k.kozlowski@samsung.com>");
 MODULE_LICENSE("GPL");
 MODULE_ALIAS("platform:extcon-max14577");
diff --git a/drivers/extcon/extcon-max77693.c b/drivers/extcon/extcon-max77693.c
index da268fbc901..2c7c3e19159 100644
--- a/drivers/extcon/extcon-max77693.c
+++ b/drivers/extcon/extcon-max77693.c
@@ -1175,25 +1175,24 @@ static int max77693_muic_probe(struct platform_device *pdev)
 	}
 
 	/* Initialize extcon device */
-	info->edev = devm_kzalloc(&pdev->dev, sizeof(struct extcon_dev),
-				  GFP_KERNEL);
-	if (!info->edev) {
+	info->edev = devm_extcon_dev_allocate(&pdev->dev,
+					      max77693_extcon_cable);
+	if (IS_ERR(info->edev)) {
 		dev_err(&pdev->dev, "failed to allocate memory for extcon\n");
 		ret = -ENOMEM;
 		goto err_irq;
 	}
 	info->edev->name = DEV_NAME;
 	info->edev->dev.parent = &pdev->dev;
-	info->edev->supported_cable = max77693_extcon_cable;
-	ret = extcon_dev_register(info->edev);
+
+	ret = devm_extcon_dev_register(&pdev->dev, info->edev);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to register extcon device\n");
 		goto err_irq;
 	}
 
-
 	/* Initialize MUIC register by using platform data or default data */
-	if (pdata->muic_data) {
+	if (pdata && pdata->muic_data) {
 		init_data = pdata->muic_data->init_data;
 		num_init_data = pdata->muic_data->num_init_data;
 	} else {
@@ -1226,7 +1225,7 @@ static int max77693_muic_probe(struct platform_device *pdev)
 				= init_data[i].data;
 	}
 
-	if (pdata->muic_data) {
+	if (pdata && pdata->muic_data) {
 		struct max77693_muic_platform_data *muic_pdata
 						   = pdata->muic_data;
 
@@ -1267,7 +1266,7 @@ static int max77693_muic_probe(struct platform_device *pdev)
 			MAX77693_MUIC_REG_ID, &id);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "failed to read revision number\n");
-		goto err_extcon;
+		goto err_irq;
 	}
 	dev_info(info->dev, "device ID : 0x%x\n", id);
 
@@ -1283,12 +1282,11 @@ 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);
-	schedule_delayed_work(&info->wq_detcable, delay_jiffies);
+	queue_delayed_work(system_power_efficient_wq, &info->wq_detcable,
+			delay_jiffies);
 
 	return ret;
 
-err_extcon:
-	extcon_dev_unregister(info->edev);
 err_irq:
 	while (--i >= 0)
 		free_irq(muic_irqs[i].virq, info);
@@ -1304,7 +1302,6 @@ static int max77693_muic_remove(struct platform_device *pdev)
 		free_irq(muic_irqs[i].virq, info);
 	cancel_work_sync(&info->irq_work);
 	input_unregister_device(info->dock);
-	extcon_dev_unregister(info->edev);
 
 	return 0;
 }
diff --git a/drivers/extcon/extcon-max8997.c b/drivers/extcon/extcon-max8997.c
index 6a00464658c..d9f7f1baaa0 100644
--- a/drivers/extcon/extcon-max8997.c
+++ b/drivers/extcon/extcon-max8997.c
@@ -699,23 +699,22 @@ static int max8997_muic_probe(struct platform_device *pdev)
 	}
 
 	/* External connector */
-	info->edev = devm_kzalloc(&pdev->dev, sizeof(struct extcon_dev),
-				  GFP_KERNEL);
-	if (!info->edev) {
+	info->edev = devm_extcon_dev_allocate(&pdev->dev, max8997_extcon_cable);
+	if (IS_ERR(info->edev)) {
 		dev_err(&pdev->dev, "failed to allocate memory for extcon\n");
 		ret = -ENOMEM;
 		goto err_irq;
 	}
 	info->edev->name = DEV_NAME;
 	info->edev->dev.parent = &pdev->dev;
-	info->edev->supported_cable = max8997_extcon_cable;
-	ret = extcon_dev_register(info->edev);
+
+	ret = devm_extcon_dev_register(&pdev->dev, info->edev);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to register extcon device\n");
 		goto err_irq;
 	}
 
-	if (pdata->muic_pdata) {
+	if (pdata && pdata->muic_pdata) {
 		struct max8997_muic_platform_data *muic_pdata
 			= pdata->muic_pdata;
 
@@ -770,7 +769,8 @@ 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);
-	schedule_delayed_work(&info->wq_detcable, delay_jiffies);
+	queue_delayed_work(system_power_efficient_wq, &info->wq_detcable,
+			delay_jiffies);
 
 	return 0;
 
@@ -789,8 +789,6 @@ static int max8997_muic_remove(struct platform_device *pdev)
 		free_irq(muic_irqs[i].virq, info);
 	cancel_work_sync(&info->irq_work);
 
-	extcon_dev_unregister(info->edev);
-
 	return 0;
 }
 
diff --git a/drivers/extcon/extcon-palmas.c b/drivers/extcon/extcon-palmas.c
index ddff2b72f0a..7417ce84eb2 100644
--- a/drivers/extcon/extcon-palmas.c
+++ b/drivers/extcon/extcon-palmas.c
@@ -23,6 +23,7 @@
 #include <linux/module.h>
 #include <linux/interrupt.h>
 #include <linux/platform_device.h>
+#include <linux/slab.h>
 #include <linux/err.h>
 #include <linux/mfd/palmas.h>
 #include <linux/of.h>
@@ -56,7 +57,7 @@ static irqreturn_t palmas_vbus_irq_handler(int irq, void *_palmas_usb)
 	if (vbus_line_state & PALMAS_INT3_LINE_STATE_VBUS) {
 		if (palmas_usb->linkstat != PALMAS_USB_STATE_VBUS) {
 			palmas_usb->linkstat = PALMAS_USB_STATE_VBUS;
-			extcon_set_cable_state(&palmas_usb->edev, "USB", true);
+			extcon_set_cable_state(palmas_usb->edev, "USB", true);
 			dev_info(palmas_usb->dev, "USB cable is attached\n");
 		} else {
 			dev_dbg(palmas_usb->dev,
@@ -65,7 +66,7 @@ static irqreturn_t palmas_vbus_irq_handler(int irq, void *_palmas_usb)
 	} else if (!(vbus_line_state & PALMAS_INT3_LINE_STATE_VBUS)) {
 		if (palmas_usb->linkstat == PALMAS_USB_STATE_VBUS) {
 			palmas_usb->linkstat = PALMAS_USB_STATE_DISCONNECT;
-			extcon_set_cable_state(&palmas_usb->edev, "USB", false);
+			extcon_set_cable_state(palmas_usb->edev, "USB", false);
 			dev_info(palmas_usb->dev, "USB cable is detached\n");
 		} else {
 			dev_dbg(palmas_usb->dev,
@@ -92,7 +93,7 @@ static irqreturn_t palmas_id_irq_handler(int irq, void *_palmas_usb)
 			PALMAS_USB_ID_INT_LATCH_CLR,
 			PALMAS_USB_ID_INT_EN_HI_CLR_ID_GND);
 		palmas_usb->linkstat = PALMAS_USB_STATE_ID;
-		extcon_set_cable_state(&palmas_usb->edev, "USB-HOST", true);
+		extcon_set_cable_state(palmas_usb->edev, "USB-HOST", true);
 		dev_info(palmas_usb->dev, "USB-HOST cable is attached\n");
 	} else if ((set & PALMAS_USB_ID_INT_SRC_ID_FLOAT) &&
 				(id_src & PALMAS_USB_ID_INT_SRC_ID_FLOAT)) {
@@ -100,17 +101,17 @@ static irqreturn_t palmas_id_irq_handler(int irq, void *_palmas_usb)
 			PALMAS_USB_ID_INT_LATCH_CLR,
 			PALMAS_USB_ID_INT_EN_HI_CLR_ID_FLOAT);
 		palmas_usb->linkstat = PALMAS_USB_STATE_DISCONNECT;
-		extcon_set_cable_state(&palmas_usb->edev, "USB-HOST", false);
+		extcon_set_cable_state(palmas_usb->edev, "USB-HOST", false);
 		dev_info(palmas_usb->dev, "USB-HOST cable is detached\n");
 	} else if ((palmas_usb->linkstat == PALMAS_USB_STATE_ID) &&
 				(!(set & PALMAS_USB_ID_INT_SRC_ID_GND))) {
 		palmas_usb->linkstat = PALMAS_USB_STATE_DISCONNECT;
-		extcon_set_cable_state(&palmas_usb->edev, "USB-HOST", false);
+		extcon_set_cable_state(palmas_usb->edev, "USB-HOST", false);
 		dev_info(palmas_usb->dev, "USB-HOST cable is detached\n");
 	} else if ((palmas_usb->linkstat == PALMAS_USB_STATE_DISCONNECT) &&
 				(id_src & PALMAS_USB_ID_INT_SRC_ID_GND)) {
 		palmas_usb->linkstat = PALMAS_USB_STATE_ID;
-		extcon_set_cable_state(&palmas_usb->edev, "USB-HOST", true);
+		extcon_set_cable_state(palmas_usb->edev, "USB-HOST", true);
 		dev_info(palmas_usb->dev, " USB-HOST cable is attached\n");
 	}
 
@@ -186,13 +187,20 @@ static int palmas_usb_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, palmas_usb);
 
-	palmas_usb->edev.supported_cable = palmas_extcon_cable;
-	palmas_usb->edev.dev.parent = palmas_usb->dev;
-	palmas_usb->edev.mutually_exclusive = mutually_exclusive;
+	palmas_usb->edev = devm_extcon_dev_allocate(&pdev->dev,
+						    palmas_extcon_cable);
+	if (IS_ERR(palmas_usb->edev)) {
+		dev_err(&pdev->dev, "failed to allocate extcon device\n");
+		return -ENOMEM;
+	}
+	palmas_usb->edev->name = kstrdup(node->name, GFP_KERNEL);
+	palmas_usb->edev->dev.parent = palmas_usb->dev;
+	palmas_usb->edev->mutually_exclusive = mutually_exclusive;
 
-	status = extcon_dev_register(&palmas_usb->edev);
+	status = devm_extcon_dev_register(&pdev->dev, palmas_usb->edev);
 	if (status) {
 		dev_err(&pdev->dev, "failed to register extcon device\n");
+		kfree(palmas_usb->edev->name);
 		return status;
 	}
 
@@ -206,7 +214,8 @@ static int palmas_usb_probe(struct platform_device *pdev)
 		if (status < 0) {
 			dev_err(&pdev->dev, "can't get IRQ %d, err %d\n",
 					palmas_usb->id_irq, status);
-			goto fail_extcon;
+			kfree(palmas_usb->edev->name);
+			return status;
 		}
 	}
 
@@ -220,25 +229,21 @@ static int palmas_usb_probe(struct platform_device *pdev)
 		if (status < 0) {
 			dev_err(&pdev->dev, "can't get IRQ %d, err %d\n",
 					palmas_usb->vbus_irq, status);
-			goto fail_extcon;
+			kfree(palmas_usb->edev->name);
+			return status;
 		}
 	}
 
 	palmas_enable_irq(palmas_usb);
 	device_set_wakeup_capable(&pdev->dev, true);
 	return 0;
-
-fail_extcon:
-	extcon_dev_unregister(&palmas_usb->edev);
-
-	return status;
 }
 
 static int palmas_usb_remove(struct platform_device *pdev)
 {
 	struct palmas_usb *palmas_usb = platform_get_drvdata(pdev);
 
-	extcon_dev_unregister(&palmas_usb->edev);
+	kfree(palmas_usb->edev->name);
 
 	return 0;
 }
diff --git a/drivers/firewire/core.h b/drivers/firewire/core.h
index c98764aeeec..f477308b6e9 100644
--- a/drivers/firewire/core.h
+++ b/drivers/firewire/core.h
@@ -237,8 +237,8 @@ static inline bool is_next_generation(int new_generation, int old_generation)
 
 #define LOCAL_BUS 0xffc0
 
-/* arbitrarily chosen maximum range for physical DMA: 128 TB */
-#define FW_MAX_PHYSICAL_RANGE		(128ULL << 40)
+/* OHCI-1394's default upper bound for physical DMA: 4 GB */
+#define FW_MAX_PHYSICAL_RANGE		(1ULL << 32)
 
 void fw_core_handle_request(struct fw_card *card, struct fw_packet *request);
 void fw_core_handle_response(struct fw_card *card, struct fw_packet *packet);
diff --git a/drivers/firewire/ohci.c b/drivers/firewire/ohci.c
index 995dd42a262..ce7a5812ae9 100644
--- a/drivers/firewire/ohci.c
+++ b/drivers/firewire/ohci.c
@@ -3716,7 +3716,7 @@ static int pci_probe(struct pci_dev *dev,
 		    version >> 16, version & 0xff, ohci->card.index,
 		    ohci->n_ir, ohci->n_it, ohci->quirks,
 		    reg_read(ohci, OHCI1394_PhyUpperBound) ?
-			", >4 GB phys DMA" : "");
+			", physUB" : "");
 
 	return 0;
 
diff --git a/drivers/firmware/iscsi_ibft.c b/drivers/firmware/iscsi_ibft.c
index 3ee852c9925..071c2c969ee 100644
--- a/drivers/firmware/iscsi_ibft.c
+++ b/drivers/firmware/iscsi_ibft.c
@@ -756,6 +756,7 @@ static const struct {
 	 */
 	{ ACPI_SIG_IBFT },
 	{ "iBFT" },
+	{ "BIFT" },	/* Broadcom iSCSI Offload */
 };
 
 static void __init acpi_find_ibft_region(void)
diff --git a/drivers/gpio/Kconfig b/drivers/gpio/Kconfig
index a86c49a605c..4a1b5113e52 100644
--- a/drivers/gpio/Kconfig
+++ b/drivers/gpio/Kconfig
@@ -56,6 +56,7 @@ config GPIO_ACPI
 	depends on ACPI
 
 config GPIOLIB_IRQCHIP
+	select IRQ_DOMAIN
 	bool
 
 config DEBUG_GPIO
@@ -243,6 +244,15 @@ config GPIO_OCTEON
 	  Say yes here to support the on-chip GPIO lines on the OCTEON
 	  family of SOCs.
 
+config GPIO_OMAP
+	bool "TI OMAP GPIO support" if COMPILE_TEST && !ARCH_OMAP2PLUS
+	default y if ARCH_OMAP
+	depends on ARM
+	select GENERIC_IRQ_CHIP
+	select GPIOLIB_IRQCHIP
+	help
+	  Say yes here to enable GPIO support for TI OMAP SoCs.
+
 config GPIO_PL061
 	bool "PrimeCell PL061 GPIO support"
 	depends on ARM_AMBA
@@ -259,7 +269,7 @@ config GPIO_PXA
 
 config GPIO_RCAR
 	tristate "Renesas R-Car GPIO"
-	depends on ARM
+	depends on ARM && (ARCH_SHMOBILE || COMPILE_TEST)
 	help
 	  Say yes here to support GPIO on Renesas R-Car SoCs.
 
@@ -510,6 +520,7 @@ config GPIO_PCA953X
 config GPIO_PCA953X_IRQ
 	bool "Interrupt controller support for PCA953x"
 	depends on GPIO_PCA953X=y
+	select GPIOLIB_IRQCHIP
 	help
 	  Say yes here to enable the pca953x to be used as an interrupt
 	  controller. It requires the driver to be built in the kernel.
@@ -579,6 +590,7 @@ config GPIO_STP_XWAY
 config GPIO_TC3589X
 	bool "TC3589X GPIOs"
 	depends on MFD_TC3589X
+	select GPIOLIB_IRQCHIP
 	help
 	  This enables support for the GPIOs found on the TC3589X
 	  I/O Expander.
@@ -699,13 +711,13 @@ config GPIO_AMD8111
 config GPIO_INTEL_MID
 	bool "Intel Mid GPIO support"
 	depends on PCI && X86
-	select IRQ_DOMAIN
+	select GPIOLIB_IRQCHIP
 	help
 	  Say Y here to support Intel Mid GPIO.
 
 config GPIO_PCH
 	tristate "Intel EG20T PCH/LAPIS Semiconductor IOH(ML7223/ML7831) GPIO"
-	depends on PCI && X86
+	depends on PCI && (X86_32 || COMPILE_TEST)
 	select GENERIC_IRQ_CHIP
 	help
 	  This driver is for PCH(Platform controller Hub) GPIO of Intel Topcliff
@@ -739,7 +751,7 @@ config GPIO_SODAVILLE
 
 config GPIO_TIMBERDALE
 	bool "Support for timberdale GPIO IP"
-	depends on MFD_TIMBERDALE && HAS_IOMEM
+	depends on MFD_TIMBERDALE
 	---help---
 	Add support for the GPIO IP in the timberdale FPGA.
 
diff --git a/drivers/gpio/Makefile b/drivers/gpio/Makefile
index 6309aff1d80..d10f6a9d875 100644
--- a/drivers/gpio/Makefile
+++ b/drivers/gpio/Makefile
@@ -58,7 +58,7 @@ obj-$(CONFIG_GPIO_MVEBU)        += gpio-mvebu.o
 obj-$(CONFIG_GPIO_MXC)		+= gpio-mxc.o
 obj-$(CONFIG_GPIO_MXS)		+= gpio-mxs.o
 obj-$(CONFIG_GPIO_OCTEON)	+= gpio-octeon.o
-obj-$(CONFIG_ARCH_OMAP)		+= gpio-omap.o
+obj-$(CONFIG_GPIO_OMAP)		+= gpio-omap.o
 obj-$(CONFIG_GPIO_PCA953X)	+= gpio-pca953x.o
 obj-$(CONFIG_GPIO_PCF857X)	+= gpio-pcf857x.o
 obj-$(CONFIG_GPIO_PCH)		+= gpio-pch.o
diff --git a/drivers/gpio/devres.c b/drivers/gpio/devres.c
index 307464fd015..65978cf85f7 100644
--- a/drivers/gpio/devres.c
+++ b/drivers/gpio/devres.c
@@ -52,6 +52,22 @@ struct gpio_desc *__must_check devm_gpiod_get(struct device *dev,
 EXPORT_SYMBOL(devm_gpiod_get);
 
 /**
+ * devm_gpiod_get_optional - Resource-managed gpiod_get_optional()
+ * @dev: GPIO consumer
+ * @con_id: function within the GPIO consumer
+ *
+ * Managed gpiod_get_optional(). GPIO descriptors returned from this function
+ * are automatically disposed on driver detach. See gpiod_get_optional() for
+ * detailed information about behavior and return values.
+ */
+struct gpio_desc *__must_check devm_gpiod_get_optional(struct device *dev,
+						       const char *con_id)
+{
+	return devm_gpiod_get_index_optional(dev, con_id, 0);
+}
+EXPORT_SYMBOL(devm_gpiod_get_optional);
+
+/**
  * devm_gpiod_get_index - Resource-managed gpiod_get_index()
  * @dev:	GPIO consumer
  * @con_id:	function within the GPIO consumer
@@ -87,6 +103,33 @@ struct gpio_desc *__must_check devm_gpiod_get_index(struct device *dev,
 EXPORT_SYMBOL(devm_gpiod_get_index);
 
 /**
+ * devm_gpiod_get_index_optional - Resource-managed gpiod_get_index_optional()
+ * @dev: GPIO consumer
+ * @con_id: function within the GPIO consumer
+ * @index: index of the GPIO to obtain in the consumer
+ *
+ * Managed gpiod_get_index_optional(). GPIO descriptors returned from this
+ * function are automatically disposed on driver detach. See
+ * gpiod_get_index_optional() for detailed information about behavior and
+ * return values.
+ */
+struct gpio_desc *__must_check devm_gpiod_get_index_optional(struct device *dev,
+							     const char *con_id,
+							     unsigned int index)
+{
+	struct gpio_desc *desc;
+
+	desc = devm_gpiod_get_index(dev, con_id, index);
+	if (IS_ERR(desc)) {
+		if (PTR_ERR(desc) == -ENOENT)
+			return NULL;
+	}
+
+	return desc;
+}
+EXPORT_SYMBOL(devm_gpiod_get_index_optional);
+
+/**
  * devm_gpiod_put - Resource-managed gpiod_put()
  * @desc:	GPIO descriptor to dispose of
  *
diff --git a/drivers/gpio/gpio-adp5520.c b/drivers/gpio/gpio-adp5520.c
index 613265944e2..f1ade8fa321 100644
--- a/drivers/gpio/gpio-adp5520.c
+++ b/drivers/gpio/gpio-adp5520.c
@@ -106,10 +106,8 @@ static int adp5520_gpio_probe(struct platform_device *pdev)
 	}
 
 	dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
-	if (dev == NULL) {
-		dev_err(&pdev->dev, "failed to alloc memory\n");
+	if (dev == NULL)
 		return -ENOMEM;
-	}
 
 	dev->master = pdev->dev.parent;
 
diff --git a/drivers/gpio/gpio-adp5588.c b/drivers/gpio/gpio-adp5588.c
index d974020b78b..ef19bc33f2b 100644
--- a/drivers/gpio/gpio-adp5588.c
+++ b/drivers/gpio/gpio-adp5588.c
@@ -379,10 +379,8 @@ static int adp5588_gpio_probe(struct i2c_client *client,
 	}
 
 	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
-	if (dev == NULL) {
-		dev_err(&client->dev, "failed to alloc memory\n");
+	if (dev == NULL)
 		return -ENOMEM;
-	}
 
 	dev->client = client;
 
diff --git a/drivers/gpio/gpio-bt8xx.c b/drivers/gpio/gpio-bt8xx.c
index ecb3ca2d1d1..6557147d933 100644
--- a/drivers/gpio/gpio-bt8xx.c
+++ b/drivers/gpio/gpio-bt8xx.c
@@ -178,7 +178,7 @@ static int bt8xxgpio_probe(struct pci_dev *dev,
 	struct bt8xxgpio *bg;
 	int err;
 
-	bg = kzalloc(sizeof(*bg), GFP_KERNEL);
+	bg = devm_kzalloc(&dev->dev, sizeof(struct bt8xxgpio), GFP_KERNEL);
 	if (!bg)
 		return -ENOMEM;
 
@@ -188,9 +188,9 @@ static int bt8xxgpio_probe(struct pci_dev *dev,
 	err = pci_enable_device(dev);
 	if (err) {
 		printk(KERN_ERR "bt8xxgpio: Can't enable device.\n");
-		goto err_freebg;
+		return err;
 	}
-	if (!request_mem_region(pci_resource_start(dev, 0),
+	if (!devm_request_mem_region(&dev->dev, pci_resource_start(dev, 0),
 				pci_resource_len(dev, 0),
 				"bt8xxgpio")) {
 		printk(KERN_WARNING "bt8xxgpio: Can't request iomem (0x%llx).\n",
@@ -201,11 +201,11 @@ static int bt8xxgpio_probe(struct pci_dev *dev,
 	pci_set_master(dev);
 	pci_set_drvdata(dev, bg);
 
-	bg->mmio = ioremap(pci_resource_start(dev, 0), 0x1000);
+	bg->mmio = devm_ioremap(&dev->dev, pci_resource_start(dev, 0), 0x1000);
 	if (!bg->mmio) {
 		printk(KERN_ERR "bt8xxgpio: ioremap() failed\n");
 		err = -EIO;
-		goto err_release_mem;
+		goto err_disable;
 	}
 
 	/* Disable interrupts */
@@ -220,18 +220,13 @@ static int bt8xxgpio_probe(struct pci_dev *dev,
 	err = gpiochip_add(&bg->gpio);
 	if (err) {
 		printk(KERN_ERR "bt8xxgpio: Failed to register GPIOs\n");
-		goto err_release_mem;
+		goto err_disable;
 	}
 
 	return 0;
 
-err_release_mem:
-	release_mem_region(pci_resource_start(dev, 0),
-			   pci_resource_len(dev, 0));
 err_disable:
 	pci_disable_device(dev);
-err_freebg:
-	kfree(bg);
 
 	return err;
 }
@@ -250,8 +245,6 @@ static void bt8xxgpio_remove(struct pci_dev *pdev)
 	release_mem_region(pci_resource_start(pdev, 0),
 			   pci_resource_len(pdev, 0));
 	pci_disable_device(pdev);
-
-	kfree(bg);
 }
 
 #ifdef CONFIG_PM
diff --git a/drivers/gpio/gpio-davinci.c b/drivers/gpio/gpio-davinci.c
index 339f9dac591..9f0682534e2 100644
--- a/drivers/gpio/gpio-davinci.c
+++ b/drivers/gpio/gpio-davinci.c
@@ -230,10 +230,8 @@ static int davinci_gpio_probe(struct platform_device *pdev)
 	chips = devm_kzalloc(dev,
 			     ngpio * sizeof(struct davinci_gpio_controller),
 			     GFP_KERNEL);
-	if (!chips) {
-		dev_err(dev, "Memory allocation failed\n");
+	if (!chips)
 		return -ENOMEM;
-	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res) {
diff --git a/drivers/gpio/gpio-dwapb.c b/drivers/gpio/gpio-dwapb.c
index ed5711f77e2..cd3b8143527 100644
--- a/drivers/gpio/gpio-dwapb.c
+++ b/drivers/gpio/gpio-dwapb.c
@@ -198,6 +198,8 @@ static int dwapb_irq_set_type(struct irq_data *d, u32 type)
 		break;
 	}
 
+	irq_setup_alt_chip(d, type);
+
 	writel(level, gpio->regs + GPIO_INTTYPE_LEVEL);
 	writel(polarity, gpio->regs + GPIO_INT_POLARITY);
 	spin_unlock_irqrestore(&bgc->lock, flags);
@@ -213,7 +215,7 @@ static void dwapb_configure_irqs(struct dwapb_gpio *gpio,
 	struct irq_chip_generic	*irq_gc;
 	unsigned int hwirq, ngpio = gc->ngpio;
 	struct irq_chip_type *ct;
-	int err, irq;
+	int err, irq, i;
 
 	irq = irq_of_parse_and_map(node, 0);
 	if (!irq) {
@@ -227,7 +229,7 @@ static void dwapb_configure_irqs(struct dwapb_gpio *gpio,
 	if (!gpio->domain)
 		return;
 
-	err = irq_alloc_domain_generic_chips(gpio->domain, ngpio, 1,
+	err = irq_alloc_domain_generic_chips(gpio->domain, ngpio, 2,
 					     "gpio-dwapb", handle_level_irq,
 					     IRQ_NOREQUEST, 0,
 					     IRQ_GC_INIT_NESTED_LOCK);
@@ -248,20 +250,24 @@ static void dwapb_configure_irqs(struct dwapb_gpio *gpio,
 	irq_gc->reg_base = gpio->regs;
 	irq_gc->private = gpio;
 
-	ct = irq_gc->chip_types;
-	ct->chip.irq_ack = irq_gc_ack_set_bit;
-	ct->chip.irq_mask = irq_gc_mask_set_bit;
-	ct->chip.irq_unmask = irq_gc_mask_clr_bit;
-	ct->chip.irq_set_type = dwapb_irq_set_type;
-	ct->chip.irq_enable = dwapb_irq_enable;
-	ct->chip.irq_disable = dwapb_irq_disable;
-	ct->chip.irq_request_resources = dwapb_irq_reqres;
-	ct->chip.irq_release_resources = dwapb_irq_relres;
-	ct->regs.ack = GPIO_PORTA_EOI;
-	ct->regs.mask = GPIO_INTMASK;
-
-	irq_setup_generic_chip(irq_gc, IRQ_MSK(port->bgc.gc.ngpio),
-			IRQ_GC_INIT_NESTED_LOCK, IRQ_NOREQUEST, 0);
+	for (i = 0; i < 2; i++) {
+		ct = &irq_gc->chip_types[i];
+		ct->chip.irq_ack = irq_gc_ack_set_bit;
+		ct->chip.irq_mask = irq_gc_mask_set_bit;
+		ct->chip.irq_unmask = irq_gc_mask_clr_bit;
+		ct->chip.irq_set_type = dwapb_irq_set_type;
+		ct->chip.irq_enable = dwapb_irq_enable;
+		ct->chip.irq_disable = dwapb_irq_disable;
+		ct->chip.irq_request_resources = dwapb_irq_reqres;
+		ct->chip.irq_release_resources = dwapb_irq_relres;
+		ct->regs.ack = GPIO_PORTA_EOI;
+		ct->regs.mask = GPIO_INTMASK;
+		ct->type = IRQ_TYPE_LEVEL_MASK;
+	}
+
+	irq_gc->chip_types[0].type = IRQ_TYPE_LEVEL_MASK;
+	irq_gc->chip_types[1].type = IRQ_TYPE_EDGE_BOTH;
+	irq_gc->chip_types[1].handler = handle_edge_irq;
 
 	irq_set_chained_handler(irq, dwapb_irq_handler);
 	irq_set_handler_data(irq, gpio);
diff --git a/drivers/gpio/gpio-em.c b/drivers/gpio/gpio-em.c
index 8765bd6f48e..cde36054c38 100644
--- a/drivers/gpio/gpio-em.c
+++ b/drivers/gpio/gpio-em.c
@@ -212,7 +212,7 @@ static void __em_gio_set(struct gpio_chip *chip, unsigned int reg,
 {
 	/* upper 16 bits contains mask and lower 16 actual value */
 	em_gio_write(gpio_to_priv(chip), reg,
-		     (1 << (shift + 16)) | (value << shift));
+		     (BIT(shift + 16)) | (value << shift));
 }
 
 static void em_gio_set(struct gpio_chip *chip, unsigned offset, int value)
@@ -284,7 +284,6 @@ static int em_gio_probe(struct platform_device *pdev)
 
 	p = devm_kzalloc(&pdev->dev, sizeof(*p), GFP_KERNEL);
 	if (!p) {
-		dev_err(&pdev->dev, "failed to allocate driver data\n");
 		ret = -ENOMEM;
 		goto err0;
 	}
diff --git a/drivers/gpio/gpio-ep93xx.c b/drivers/gpio/gpio-ep93xx.c
index 80829f3c654..dcc2bb4074e 100644
--- a/drivers/gpio/gpio-ep93xx.c
+++ b/drivers/gpio/gpio-ep93xx.c
@@ -344,37 +344,24 @@ static int ep93xx_gpio_probe(struct platform_device *pdev)
 {
 	struct ep93xx_gpio *ep93xx_gpio;
 	struct resource *res;
-	void __iomem *mmio;
 	int i;
-	int ret;
+	struct device *dev = &pdev->dev;
 
-	ep93xx_gpio = kzalloc(sizeof(*ep93xx_gpio), GFP_KERNEL);
+	ep93xx_gpio = devm_kzalloc(dev, sizeof(struct ep93xx_gpio), GFP_KERNEL);
 	if (!ep93xx_gpio)
 		return -ENOMEM;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		ret = -ENXIO;
-		goto exit_free;
-	}
-
-	if (!request_mem_region(res->start, resource_size(res), pdev->name)) {
-		ret = -EBUSY;
-		goto exit_free;
-	}
-
-	mmio = ioremap(res->start, resource_size(res));
-	if (!mmio) {
-		ret = -ENXIO;
-		goto exit_release;
-	}
-	ep93xx_gpio->mmio_base = mmio;
+	ep93xx_gpio->mmio_base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(ep93xx_gpio->mmio_base))
+		return PTR_ERR(ep93xx_gpio->mmio_base);
 
 	for (i = 0; i < ARRAY_SIZE(ep93xx_gpio_banks); i++) {
 		struct bgpio_chip *bgc = &ep93xx_gpio->bgc[i];
 		struct ep93xx_gpio_bank *bank = &ep93xx_gpio_banks[i];
 
-		if (ep93xx_gpio_add_bank(bgc, &pdev->dev, mmio, bank))
+		if (ep93xx_gpio_add_bank(bgc, &pdev->dev,
+					 ep93xx_gpio->mmio_base, bank))
 			dev_warn(&pdev->dev, "Unable to add gpio bank %s\n",
 				bank->label);
 	}
@@ -382,13 +369,6 @@ static int ep93xx_gpio_probe(struct platform_device *pdev)
 	ep93xx_gpio_init_irq();
 
 	return 0;
-
-exit_release:
-	release_mem_region(res->start, resource_size(res));
-exit_free:
-	kfree(ep93xx_gpio);
-	dev_info(&pdev->dev, "%s failed with errno %d\n", __func__, ret);
-	return ret;
 }
 
 static struct platform_driver ep93xx_gpio_driver = {
diff --git a/drivers/gpio/gpio-ge.c b/drivers/gpio/gpio-ge.c
index 7b95a4a8318..1237a73c3c9 100644
--- a/drivers/gpio/gpio-ge.c
+++ b/drivers/gpio/gpio-ge.c
@@ -18,15 +18,9 @@
  */
 
 #include <linux/kernel.h>
-#include <linux/compiler.h>
-#include <linux/init.h>
 #include <linux/io.h>
-#include <linux/of.h>
 #include <linux/of_device.h>
-#include <linux/of_platform.h>
 #include <linux/of_gpio.h>
-#include <linux/gpio.h>
-#include <linux/slab.h>
 #include <linux/module.h>
 
 #define GEF_GPIO_DIRECT		0x00
@@ -39,28 +33,26 @@
 #define GEF_GPIO_OVERRUN	0x1C
 #define GEF_GPIO_MODE		0x20
 
-static void _gef_gpio_set(void __iomem *reg, unsigned int offset, int value)
+static void gef_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
+	struct of_mm_gpio_chip *mmchip = to_of_mm_gpio_chip(chip);
 	unsigned int data;
 
-	data = ioread32be(reg);
-	/* value: 0=low; 1=high */
-	if (value & 0x1)
-		data = data | (0x1 << offset);
+	data = ioread32be(mmchip->regs + GEF_GPIO_OUT);
+	if (value)
+		data = data | BIT(offset);
 	else
-		data = data & ~(0x1 << offset);
-
-	iowrite32be(data, reg);
+		data = data & ~BIT(offset);
+	iowrite32be(data, mmchip->regs + GEF_GPIO_OUT);
 }
 
-
 static int gef_gpio_dir_in(struct gpio_chip *chip, unsigned offset)
 {
 	unsigned int data;
 	struct of_mm_gpio_chip *mmchip = to_of_mm_gpio_chip(chip);
 
 	data = ioread32be(mmchip->regs + GEF_GPIO_DIRECT);
-	data = data | (0x1 << offset);
+	data = data | BIT(offset);
 	iowrite32be(data, mmchip->regs + GEF_GPIO_DIRECT);
 
 	return 0;
@@ -71,11 +63,11 @@ static int gef_gpio_dir_out(struct gpio_chip *chip, unsigned offset, int value)
 	unsigned int data;
 	struct of_mm_gpio_chip *mmchip = to_of_mm_gpio_chip(chip);
 
-	/* Set direction before switching to input */
-	_gef_gpio_set(mmchip->regs + GEF_GPIO_OUT, offset, value);
+	/* Set value before switching to output */
+	gef_gpio_set(mmchip->regs + GEF_GPIO_OUT, offset, value);
 
 	data = ioread32be(mmchip->regs + GEF_GPIO_DIRECT);
-	data = data & ~(0x1 << offset);
+	data = data & ~BIT(offset);
 	iowrite32be(data, mmchip->regs + GEF_GPIO_DIRECT);
 
 	return 0;
@@ -83,116 +75,56 @@ static int gef_gpio_dir_out(struct gpio_chip *chip, unsigned offset, int value)
 
 static int gef_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	unsigned int data;
-	int state = 0;
 	struct of_mm_gpio_chip *mmchip = to_of_mm_gpio_chip(chip);
 
-	data = ioread32be(mmchip->regs + GEF_GPIO_IN);
-	state = (int)((data >> offset) & 0x1);
-
-	return state;
+	return !!(ioread32be(mmchip->regs + GEF_GPIO_IN) & BIT(offset));
 }
 
-static void gef_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
-{
-	struct of_mm_gpio_chip *mmchip = to_of_mm_gpio_chip(chip);
-
-	_gef_gpio_set(mmchip->regs + GEF_GPIO_OUT, offset, value);
-}
+static const struct of_device_id gef_gpio_ids[] = {
+	{
+		.compatible	= "gef,sbc610-gpio",
+		.data		= (void *)19,
+	}, {
+		.compatible	= "gef,sbc310-gpio",
+		.data		= (void *)6,
+	}, {
+		.compatible	= "ge,imp3a-gpio",
+		.data		= (void *)16,
+	},
+	{ }
+};
+MODULE_DEVICE_TABLE(of, gef_gpio_ids);
 
-static int __init gef_gpio_init(void)
+static int __init gef_gpio_probe(struct platform_device *pdev)
 {
-	struct device_node *np;
-	int retval;
-	struct of_mm_gpio_chip *gef_gpio_chip;
-
-	for_each_compatible_node(np, NULL, "gef,sbc610-gpio") {
-
-		pr_debug("%s: Initialising GEF GPIO\n", np->full_name);
-
-		/* Allocate chip structure */
-		gef_gpio_chip = kzalloc(sizeof(*gef_gpio_chip), GFP_KERNEL);
-		if (!gef_gpio_chip) {
-			pr_err("%s: Unable to allocate structure\n",
-				np->full_name);
-			continue;
-		}
-
-		/* Setup pointers to chip functions */
-		gef_gpio_chip->gc.of_gpio_n_cells = 2;
-		gef_gpio_chip->gc.ngpio = 19;
-		gef_gpio_chip->gc.direction_input = gef_gpio_dir_in;
-		gef_gpio_chip->gc.direction_output = gef_gpio_dir_out;
-		gef_gpio_chip->gc.get = gef_gpio_get;
-		gef_gpio_chip->gc.set = gef_gpio_set;
-
-		/* This function adds a memory mapped GPIO chip */
-		retval = of_mm_gpiochip_add(np, gef_gpio_chip);
-		if (retval) {
-			kfree(gef_gpio_chip);
-			pr_err("%s: Unable to add GPIO\n", np->full_name);
-		}
-	}
-
-	for_each_compatible_node(np, NULL, "gef,sbc310-gpio") {
-
-		pr_debug("%s: Initialising GEF GPIO\n", np->full_name);
-
-		/* Allocate chip structure */
-		gef_gpio_chip = kzalloc(sizeof(*gef_gpio_chip), GFP_KERNEL);
-		if (!gef_gpio_chip) {
-			pr_err("%s: Unable to allocate structure\n",
-				np->full_name);
-			continue;
-		}
-
-		/* Setup pointers to chip functions */
-		gef_gpio_chip->gc.of_gpio_n_cells = 2;
-		gef_gpio_chip->gc.ngpio = 6;
-		gef_gpio_chip->gc.direction_input = gef_gpio_dir_in;
-		gef_gpio_chip->gc.direction_output = gef_gpio_dir_out;
-		gef_gpio_chip->gc.get = gef_gpio_get;
-		gef_gpio_chip->gc.set = gef_gpio_set;
-
-		/* This function adds a memory mapped GPIO chip */
-		retval = of_mm_gpiochip_add(np, gef_gpio_chip);
-		if (retval) {
-			kfree(gef_gpio_chip);
-			pr_err("%s: Unable to add GPIO\n", np->full_name);
-		}
-	}
-
-	for_each_compatible_node(np, NULL, "ge,imp3a-gpio") {
-
-		pr_debug("%s: Initialising GE GPIO\n", np->full_name);
-
-		/* Allocate chip structure */
-		gef_gpio_chip = kzalloc(sizeof(*gef_gpio_chip), GFP_KERNEL);
-		if (!gef_gpio_chip) {
-			pr_err("%s: Unable to allocate structure\n",
-				np->full_name);
-			continue;
-		}
-
-		/* Setup pointers to chip functions */
-		gef_gpio_chip->gc.of_gpio_n_cells = 2;
-		gef_gpio_chip->gc.ngpio = 16;
-		gef_gpio_chip->gc.direction_input = gef_gpio_dir_in;
-		gef_gpio_chip->gc.direction_output = gef_gpio_dir_out;
-		gef_gpio_chip->gc.get = gef_gpio_get;
-		gef_gpio_chip->gc.set = gef_gpio_set;
-
-		/* This function adds a memory mapped GPIO chip */
-		retval = of_mm_gpiochip_add(np, gef_gpio_chip);
-		if (retval) {
-			kfree(gef_gpio_chip);
-			pr_err("%s: Unable to add GPIO\n", np->full_name);
-		}
-	}
+	const struct of_device_id *of_id =
+		of_match_device(gef_gpio_ids, &pdev->dev);
+	struct of_mm_gpio_chip *mmchip;
+
+	mmchip = devm_kzalloc(&pdev->dev, sizeof(*mmchip), GFP_KERNEL);
+	if (!mmchip)
+		return -ENOMEM;
+
+	/* Setup pointers to chip functions */
+	mmchip->gc.ngpio = (u16)(uintptr_t)of_id->data;
+	mmchip->gc.of_gpio_n_cells = 2;
+	mmchip->gc.direction_input = gef_gpio_dir_in;
+	mmchip->gc.direction_output = gef_gpio_dir_out;
+	mmchip->gc.get = gef_gpio_get;
+	mmchip->gc.set = gef_gpio_set;
+
+	/* This function adds a memory mapped GPIO chip */
+	return of_mm_gpiochip_add(pdev->dev.of_node, mmchip);
+};
 
-	return 0;
+static struct platform_driver gef_gpio_driver = {
+	.driver = {
+		.name		= "gef-gpio",
+		.owner		= THIS_MODULE,
+		.of_match_table	= gef_gpio_ids,
+	},
 };
-arch_initcall(gef_gpio_init);
+module_platform_driver_probe(gef_gpio_driver, gef_gpio_probe);
 
 MODULE_DESCRIPTION("GE I/O FPGA GPIO driver");
 MODULE_AUTHOR("Martyn Welch <martyn.welch@ge.com");
diff --git a/drivers/gpio/gpio-generic.c b/drivers/gpio/gpio-generic.c
index b5dff9e742f..fea8c82bb8f 100644
--- a/drivers/gpio/gpio-generic.c
+++ b/drivers/gpio/gpio-generic.c
@@ -388,6 +388,14 @@ static int bgpio_setup_direction(struct bgpio_chip *bgc,
 	return 0;
 }
 
+static int bgpio_request(struct gpio_chip *chip, unsigned gpio_pin)
+{
+	if (gpio_pin < chip->ngpio)
+		return 0;
+
+	return -EINVAL;
+}
+
 int bgpio_remove(struct bgpio_chip *bgc)
 {
 	return gpiochip_remove(&bgc->gc);
@@ -413,6 +421,7 @@ int bgpio_init(struct bgpio_chip *bgc, struct device *dev,
 	bgc->gc.label = dev_name(dev);
 	bgc->gc.base = -1;
 	bgc->gc.ngpio = bgc->bits;
+	bgc->gc.request = bgpio_request;
 
 	ret = bgpio_setup_io(bgc, dat, set, clr);
 	if (ret)
diff --git a/drivers/gpio/gpio-grgpio.c b/drivers/gpio/gpio-grgpio.c
index 84d2478ec29..3c3f515b791 100644
--- a/drivers/gpio/gpio-grgpio.c
+++ b/drivers/gpio/gpio-grgpio.c
@@ -481,7 +481,7 @@ out:
 	return ret;
 }
 
-static struct of_device_id grgpio_match[] = {
+static const struct of_device_id grgpio_match[] = {
 	{.name = "GAISLER_GPIO"},
 	{.name = "01_01a"},
 	{},
diff --git a/drivers/gpio/gpio-ich.c b/drivers/gpio/gpio-ich.c
index e73c6755a5e..70304220a47 100644
--- a/drivers/gpio/gpio-ich.c
+++ b/drivers/gpio/gpio-ich.c
@@ -305,6 +305,8 @@ static struct ichx_desc ich6_desc = {
 
 	.ngpio = 50,
 	.have_blink = true,
+	.regs = ichx_regs,
+	.reglen = ichx_reglen,
 };
 
 /* Intel 3100 */
@@ -324,6 +326,8 @@ static struct ichx_desc i3100_desc = {
 	.uses_gpe0 = true,
 
 	.ngpio = 50,
+	.regs = ichx_regs,
+	.reglen = ichx_reglen,
 };
 
 /* ICH7 and ICH8-based */
diff --git a/drivers/gpio/gpio-janz-ttl.c b/drivers/gpio/gpio-janz-ttl.c
index 2ecd3a09c74..42852eaaf02 100644
--- a/drivers/gpio/gpio-janz-ttl.c
+++ b/drivers/gpio/gpio-janz-ttl.c
@@ -152,34 +152,21 @@ static int ttl_probe(struct platform_device *pdev)
 	pdata = dev_get_platdata(&pdev->dev);
 	if (!pdata) {
 		dev_err(dev, "no platform data\n");
-		ret = -ENXIO;
-		goto out_return;
+		return -ENXIO;
 	}
 
-	mod = kzalloc(sizeof(*mod), GFP_KERNEL);
-	if (!mod) {
-		dev_err(dev, "unable to allocate private data\n");
-		ret = -ENOMEM;
-		goto out_return;
-	}
+	mod = devm_kzalloc(dev, sizeof(*mod), GFP_KERNEL);
+	if (!mod)
+		return -ENOMEM;
 
 	platform_set_drvdata(pdev, mod);
 	spin_lock_init(&mod->lock);
 
 	/* get access to the MODULbus registers for this module */
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(dev, "MODULbus registers not found\n");
-		ret = -ENODEV;
-		goto out_free_mod;
-	}
-
-	mod->regs = ioremap(res->start, resource_size(res));
-	if (!mod->regs) {
-		dev_err(dev, "MODULbus registers not ioremap\n");
-		ret = -ENOMEM;
-		goto out_free_mod;
-	}
+	mod->regs = devm_ioremap_resource(dev, res);
+	if (IS_ERR(mod->regs))
+		return PTR_ERR(mod->regs);
 
 	ttl_setup_device(mod);
 
@@ -198,17 +185,10 @@ static int ttl_probe(struct platform_device *pdev)
 	ret = gpiochip_add(gpio);
 	if (ret) {
 		dev_err(dev, "unable to add GPIO chip\n");
-		goto out_iounmap_regs;
+		return ret;
 	}
 
 	return 0;
-
-out_iounmap_regs:
-	iounmap(mod->regs);
-out_free_mod:
-	kfree(mod);
-out_return:
-	return ret;
 }
 
 static int ttl_remove(struct platform_device *pdev)
@@ -223,8 +203,6 @@ static int ttl_remove(struct platform_device *pdev)
 		return ret;
 	}
 
-	iounmap(mod->regs);
-	kfree(mod);
 	return 0;
 }
 
diff --git a/drivers/gpio/gpio-kempld.c b/drivers/gpio/gpio-kempld.c
index c6d88173f5a..1e5e51987d3 100644
--- a/drivers/gpio/gpio-kempld.c
+++ b/drivers/gpio/gpio-kempld.c
@@ -24,7 +24,7 @@
 #include <linux/mfd/kempld.h>
 
 #define KEMPLD_GPIO_MAX_NUM		16
-#define KEMPLD_GPIO_MASK(x)		(1 << ((x) % 8))
+#define KEMPLD_GPIO_MASK(x)		(BIT((x) % 8))
 #define KEMPLD_GPIO_DIR_NUM(x)		(0x40 + (x) / 8)
 #define KEMPLD_GPIO_LVL_NUM(x)		(0x42 + (x) / 8)
 #define KEMPLD_GPIO_EVT_LVL_EDGE	0x46
@@ -216,4 +216,4 @@ module_platform_driver(kempld_gpio_driver);
 MODULE_DESCRIPTION("KEM PLD GPIO Driver");
 MODULE_AUTHOR("Michael Brunner <michael.brunner@kontron.com>");
 MODULE_LICENSE("GPL");
-MODULE_ALIAS("platform:gpio-kempld");
+MODULE_ALIAS("platform:kempld-gpio");
diff --git a/drivers/gpio/gpio-lynxpoint.c b/drivers/gpio/gpio-lynxpoint.c
index 9a82a9074a2..2bea89b7250 100644
--- a/drivers/gpio/gpio-lynxpoint.c
+++ b/drivers/gpio/gpio-lynxpoint.c
@@ -375,10 +375,8 @@ static int lp_gpio_probe(struct platform_device *pdev)
 	int ret = -ENODEV;
 
 	lg = devm_kzalloc(dev, sizeof(struct lp_gpio), GFP_KERNEL);
-	if (!lg) {
-		dev_err(dev, "can't allocate lp_gpio chip data\n");
+	if (!lg)
 		return -ENOMEM;
-	}
 
 	lg->pdev = pdev;
 	platform_set_drvdata(pdev, lg);
diff --git a/drivers/gpio/gpio-max730x.c b/drivers/gpio/gpio-max730x.c
index 8672755f95c..0814584fcdc 100644
--- a/drivers/gpio/gpio-max730x.c
+++ b/drivers/gpio/gpio-max730x.c
@@ -237,10 +237,9 @@ int __max730x_remove(struct device *dev)
 	ts->write(dev, 0x04, 0x00);
 
 	ret = gpiochip_remove(&ts->chip);
-	if (!ret) {
+	if (!ret)
 		mutex_destroy(&ts->lock);
-		kfree(ts);
-	} else
+	else
 		dev_err(dev, "Failed to remove GPIO controller: %d\n", ret);
 
 	return ret;
diff --git a/drivers/gpio/gpio-mcp23s08.c b/drivers/gpio/gpio-mcp23s08.c
index 99a68310e7c..fe7c0e211f9 100644
--- a/drivers/gpio/gpio-mcp23s08.c
+++ b/drivers/gpio/gpio-mcp23s08.c
@@ -714,7 +714,7 @@ fail:
 
 #ifdef CONFIG_OF
 #ifdef CONFIG_SPI_MASTER
-static struct of_device_id mcp23s08_spi_of_match[] = {
+static const struct of_device_id mcp23s08_spi_of_match[] = {
 	{
 		.compatible = "microchip,mcp23s08",
 		.data = (void *) MCP_TYPE_S08,
@@ -738,7 +738,7 @@ MODULE_DEVICE_TABLE(of, mcp23s08_spi_of_match);
 #endif
 
 #if IS_ENABLED(CONFIG_I2C)
-static struct of_device_id mcp23s08_i2c_of_match[] = {
+static const struct of_device_id mcp23s08_i2c_of_match[] = {
 	{
 		.compatible = "microchip,mcp23008",
 		.data = (void *) MCP_TYPE_008,
@@ -867,7 +867,7 @@ static int mcp23s08_probe(struct spi_device *spi)
 {
 	struct mcp23s08_platform_data	*pdata;
 	unsigned			addr;
-	unsigned			chips = 0;
+	int				chips = 0;
 	struct mcp23s08_driver_data	*data;
 	int				status, type;
 	unsigned			base = -1,
@@ -895,8 +895,13 @@ static int mcp23s08_probe(struct spi_device *spi)
 			return -ENODEV;
 		}
 
-		for (addr = 0; addr < ARRAY_SIZE(pdata->chip); addr++)
+		for (addr = 0; addr < ARRAY_SIZE(pdata->chip); addr++) {
 			pullups[addr] = 0;
+			if (spi_present_mask & (1 << addr))
+				chips++;
+		}
+		if (!chips)
+			return -ENODEV;
 	} else {
 		type = spi_get_device_id(spi)->driver_data;
 		pdata = dev_get_platdata(&spi->dev);
@@ -919,12 +924,12 @@ static int mcp23s08_probe(struct spi_device *spi)
 			pullups[addr] = pdata->chip[addr].pullups;
 		}
 
-		if (!chips)
-			return -ENODEV;
-
 		base = pdata->base;
 	}
 
+	if (!chips)
+		return -ENODEV;
+
 	data = kzalloc(sizeof(*data) + chips * sizeof(struct mcp23s08),
 			GFP_KERNEL);
 	if (!data)
@@ -935,6 +940,10 @@ static int mcp23s08_probe(struct spi_device *spi)
 		if (!(spi_present_mask & (1 << addr)))
 			continue;
 		chips--;
+		if (chips < 0) {
+			dev_err(&spi->dev, "FATAL: invalid negative chip id\n");
+			goto fail;
+		}
 		data->mcp[addr] = &data->chip[chips];
 		status = mcp23s08_probe_one(data->mcp[addr], &spi->dev, spi,
 					    0x40 | (addr << 1), type, base,
diff --git a/drivers/gpio/gpio-moxart.c b/drivers/gpio/gpio-moxart.c
index ccd45704e5f..4661e181be0 100644
--- a/drivers/gpio/gpio-moxart.c
+++ b/drivers/gpio/gpio-moxart.c
@@ -113,10 +113,8 @@ static int moxart_gpio_probe(struct platform_device *pdev)
 	int ret;
 
 	mgc = devm_kzalloc(dev, sizeof(*mgc), GFP_KERNEL);
-	if (!mgc) {
-		dev_err(dev, "can't allocate GPIO chip container\n");
+	if (!mgc)
 		return -ENOMEM;
-	}
 	mgc->gpio = moxart_template_chip;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
diff --git a/drivers/gpio/gpio-mvebu.c b/drivers/gpio/gpio-mvebu.c
index d4250942239..418e3865036 100644
--- a/drivers/gpio/gpio-mvebu.c
+++ b/drivers/gpio/gpio-mvebu.c
@@ -535,7 +535,7 @@ static void mvebu_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 #define mvebu_gpio_dbg_show NULL
 #endif
 
-static struct of_device_id mvebu_gpio_of_match[] = {
+static const struct of_device_id mvebu_gpio_of_match[] = {
 	{
 		.compatible = "marvell,orion-gpio",
 		.data       = (void *) MVEBU_GPIO_SOC_VARIANT_ORION,
@@ -574,10 +574,8 @@ static int mvebu_gpio_probe(struct platform_device *pdev)
 		soc_variant = MVEBU_GPIO_SOC_VARIANT_ORION;
 
 	mvchip = devm_kzalloc(&pdev->dev, sizeof(struct mvebu_gpio_chip), GFP_KERNEL);
-	if (!mvchip) {
-		dev_err(&pdev->dev, "Cannot allocate memory\n");
+	if (!mvchip)
 		return -ENOMEM;
-	}
 
 	if (of_property_read_u32(pdev->dev.of_node, "ngpios", &ngpios)) {
 		dev_err(&pdev->dev, "Missing ngpios OF property\n");
@@ -738,9 +736,4 @@ static struct platform_driver mvebu_gpio_driver = {
 	},
 	.probe		= mvebu_gpio_probe,
 };
-
-static int __init mvebu_gpio_init(void)
-{
-	return platform_driver_register(&mvebu_gpio_driver);
-}
-postcore_initcall(mvebu_gpio_init);
+module_platform_driver(mvebu_gpio_driver);
diff --git a/drivers/gpio/gpio-omap.c b/drivers/gpio/gpio-omap.c
index 19b886c21b1..00f29aa1fb9 100644
--- a/drivers/gpio/gpio-omap.c
+++ b/drivers/gpio/gpio-omap.c
@@ -24,9 +24,9 @@
 #include <linux/pm.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
-#include <linux/irqdomain.h>
 #include <linux/irqchip/chained_irq.h>
 #include <linux/gpio.h>
+#include <linux/bitops.h>
 #include <linux/platform_data/gpio-omap.h>
 
 #define OFF_MODE	1
@@ -52,7 +52,6 @@ struct gpio_bank {
 	struct list_head node;
 	void __iomem *base;
 	u16 irq;
-	struct irq_domain *domain;
 	u32 non_wakeup_gpios;
 	u32 enabled_non_wakeup_gpios;
 	struct gpio_regs context;
@@ -84,22 +83,21 @@ struct gpio_bank {
 };
 
 #define GPIO_INDEX(bank, gpio) (gpio % bank->width)
-#define GPIO_BIT(bank, gpio) (1 << GPIO_INDEX(bank, gpio))
+#define GPIO_BIT(bank, gpio) (BIT(GPIO_INDEX(bank, gpio)))
 #define GPIO_MOD_CTRL_BIT	BIT(0)
 
 #define BANK_USED(bank) (bank->mod_usage || bank->irq_usage)
-#define LINE_USED(line, offset) (line & (1 << offset))
+#define LINE_USED(line, offset) (line & (BIT(offset)))
 
 static int irq_to_gpio(struct gpio_bank *bank, unsigned int gpio_irq)
 {
 	return bank->chip.base + gpio_irq;
 }
 
-static int omap_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
+static inline struct gpio_bank *_irq_data_get_bank(struct irq_data *d)
 {
-	struct gpio_bank *bank = container_of(chip, struct gpio_bank, chip);
-
-	return irq_find_mapping(bank->domain, offset);
+	struct gpio_chip *chip = irq_data_get_irq_chip_data(d);
+	return container_of(chip, struct gpio_bank, chip);
 }
 
 static void _set_gpio_direction(struct gpio_bank *bank, int gpio, int is_input)
@@ -110,9 +108,9 @@ static void _set_gpio_direction(struct gpio_bank *bank, int gpio, int is_input)
 	reg += bank->regs->direction;
 	l = readl_relaxed(reg);
 	if (is_input)
-		l |= 1 << gpio;
+		l |= BIT(gpio);
 	else
-		l &= ~(1 << gpio);
+		l &= ~(BIT(gpio));
 	writel_relaxed(l, reg);
 	bank->context.oe = l;
 }
@@ -155,14 +153,14 @@ static int _get_gpio_datain(struct gpio_bank *bank, int offset)
 {
 	void __iomem *reg = bank->base + bank->regs->datain;
 
-	return (readl_relaxed(reg) & (1 << offset)) != 0;
+	return (readl_relaxed(reg) & (BIT(offset))) != 0;
 }
 
 static int _get_gpio_dataout(struct gpio_bank *bank, int offset)
 {
 	void __iomem *reg = bank->base + bank->regs->dataout;
 
-	return (readl_relaxed(reg) & (1 << offset)) != 0;
+	return (readl_relaxed(reg) & (BIT(offset))) != 0;
 }
 
 static inline void _gpio_rmw(void __iomem *base, u32 reg, u32 mask, bool set)
@@ -180,7 +178,7 @@ static inline void _gpio_rmw(void __iomem *base, u32 reg, u32 mask, bool set)
 static inline void _gpio_dbck_enable(struct gpio_bank *bank)
 {
 	if (bank->dbck_enable_mask && !bank->dbck_enabled) {
-		clk_enable(bank->dbck);
+		clk_prepare_enable(bank->dbck);
 		bank->dbck_enabled = true;
 
 		writel_relaxed(bank->dbck_enable_mask,
@@ -198,7 +196,7 @@ static inline void _gpio_dbck_disable(struct gpio_bank *bank)
 		 */
 		writel_relaxed(0, bank->base + bank->regs->debounce_en);
 
-		clk_disable(bank->dbck);
+		clk_disable_unprepare(bank->dbck);
 		bank->dbck_enabled = false;
 	}
 }
@@ -231,7 +229,7 @@ static void _set_gpio_debounce(struct gpio_bank *bank, unsigned gpio,
 
 	l = GPIO_BIT(bank, gpio);
 
-	clk_enable(bank->dbck);
+	clk_prepare_enable(bank->dbck);
 	reg = bank->base + bank->regs->debounce;
 	writel_relaxed(debounce, reg);
 
@@ -245,7 +243,7 @@ static void _set_gpio_debounce(struct gpio_bank *bank, unsigned gpio,
 	bank->dbck_enable_mask = val;
 
 	writel_relaxed(val, reg);
-	clk_disable(bank->dbck);
+	clk_disable_unprepare(bank->dbck);
 	/*
 	 * Enable debounce clock per module.
 	 * This call is mandatory because in omap_gpio_request() when
@@ -290,7 +288,7 @@ static void _clear_gpio_debounce(struct gpio_bank *bank, unsigned gpio)
 		bank->context.debounce = 0;
 		writel_relaxed(bank->context.debounce, bank->base +
 			     bank->regs->debounce);
-		clk_disable(bank->dbck);
+		clk_disable_unprepare(bank->dbck);
 		bank->dbck_enabled = false;
 	}
 }
@@ -299,7 +297,7 @@ static inline void set_gpio_trigger(struct gpio_bank *bank, int gpio,
 						unsigned trigger)
 {
 	void __iomem *base = bank->base;
-	u32 gpio_bit = 1 << gpio;
+	u32 gpio_bit = BIT(gpio);
 
 	_gpio_rmw(base, bank->regs->leveldetect0, gpio_bit,
 		  trigger & IRQ_TYPE_LEVEL_LOW);
@@ -368,9 +366,9 @@ static void _toggle_gpio_edge_triggering(struct gpio_bank *bank, int gpio)
 
 	l = readl_relaxed(reg);
 	if ((l >> gpio) & 1)
-		l &= ~(1 << gpio);
+		l &= ~(BIT(gpio));
 	else
-		l |= 1 << gpio;
+		l |= BIT(gpio);
 
 	writel_relaxed(l, reg);
 }
@@ -392,11 +390,11 @@ static int _set_gpio_triggering(struct gpio_bank *bank, int gpio,
 
 		l = readl_relaxed(reg);
 		if ((trigger & IRQ_TYPE_SENSE_MASK) == IRQ_TYPE_EDGE_BOTH)
-			bank->toggle_mask |= 1 << gpio;
+			bank->toggle_mask |= BIT(gpio);
 		if (trigger & IRQ_TYPE_EDGE_RISING)
-			l |= 1 << gpio;
+			l |= BIT(gpio);
 		else if (trigger & IRQ_TYPE_EDGE_FALLING)
-			l &= ~(1 << gpio);
+			l &= ~(BIT(gpio));
 		else
 			return -EINVAL;
 
@@ -413,10 +411,10 @@ static int _set_gpio_triggering(struct gpio_bank *bank, int gpio,
 		if (trigger & IRQ_TYPE_EDGE_RISING)
 			l |= 2 << (gpio << 1);
 		if (trigger & IRQ_TYPE_EDGE_FALLING)
-			l |= 1 << (gpio << 1);
+			l |= BIT(gpio << 1);
 
 		/* Enable wake-up during idle for dynamic tick */
-		_gpio_rmw(base, bank->regs->wkup_en, 1 << gpio, trigger);
+		_gpio_rmw(base, bank->regs->wkup_en, BIT(gpio), trigger);
 		bank->context.wake_en =
 			readl_relaxed(bank->base + bank->regs->wkup_en);
 		writel_relaxed(l, reg);
@@ -430,7 +428,7 @@ static void _enable_gpio_module(struct gpio_bank *bank, unsigned offset)
 		void __iomem *reg = bank->base + bank->regs->pinctrl;
 
 		/* Claim the pin for MPU */
-		writel_relaxed(readl_relaxed(reg) | (1 << offset), reg);
+		writel_relaxed(readl_relaxed(reg) | (BIT(offset)), reg);
 	}
 
 	if (bank->regs->ctrl && !BANK_USED(bank)) {
@@ -453,7 +451,7 @@ static void _disable_gpio_module(struct gpio_bank *bank, unsigned offset)
 	    !LINE_USED(bank->mod_usage, offset) &&
 	    !LINE_USED(bank->irq_usage, offset)) {
 		/* Disable wake-up during idle for dynamic tick */
-		_gpio_rmw(base, bank->regs->wkup_en, 1 << offset, 0);
+		_gpio_rmw(base, bank->regs->wkup_en, BIT(offset), 0);
 		bank->context.wake_en =
 			readl_relaxed(bank->base + bank->regs->wkup_en);
 	}
@@ -479,7 +477,7 @@ static int gpio_is_input(struct gpio_bank *bank, int mask)
 
 static int gpio_irq_type(struct irq_data *d, unsigned type)
 {
-	struct gpio_bank *bank = irq_data_get_irq_chip_data(d);
+	struct gpio_bank *bank = _irq_data_get_bank(d);
 	unsigned gpio = 0;
 	int retval;
 	unsigned long flags;
@@ -509,20 +507,12 @@ static int gpio_irq_type(struct irq_data *d, unsigned type)
 	if (!LINE_USED(bank->mod_usage, offset)) {
 		_enable_gpio_module(bank, offset);
 		_set_gpio_direction(bank, offset, 1);
-	} else if (!gpio_is_input(bank, 1 << offset)) {
+	} else if (!gpio_is_input(bank, BIT(offset))) {
 		spin_unlock_irqrestore(&bank->lock, flags);
 		return -EINVAL;
 	}
 
-	retval = gpio_lock_as_irq(&bank->chip, offset);
-	if (retval) {
-		dev_err(bank->dev, "unable to lock offset %d for IRQ\n",
-			offset);
-		spin_unlock_irqrestore(&bank->lock, flags);
-		return retval;
-	}
-
-	bank->irq_usage |= 1 << GPIO_INDEX(bank, gpio);
+	bank->irq_usage |= BIT(GPIO_INDEX(bank, gpio));
 	spin_unlock_irqrestore(&bank->lock, flags);
 
 	if (type & (IRQ_TYPE_LEVEL_LOW | IRQ_TYPE_LEVEL_HIGH))
@@ -559,7 +549,7 @@ static u32 _get_gpio_irqbank_mask(struct gpio_bank *bank)
 {
 	void __iomem *reg = bank->base;
 	u32 l;
-	u32 mask = (1 << bank->width) - 1;
+	u32 mask = (BIT(bank->width)) - 1;
 
 	reg += bank->regs->irqenable;
 	l = readl_relaxed(reg);
@@ -664,7 +654,7 @@ static void _reset_gpio(struct gpio_bank *bank, int gpio)
 /* Use disable_irq_wake() and enable_irq_wake() functions from drivers */
 static int gpio_wake_enable(struct irq_data *d, unsigned int enable)
 {
-	struct gpio_bank *bank = irq_data_get_irq_chip_data(d);
+	struct gpio_bank *bank = _irq_data_get_bank(d);
 	unsigned int gpio = irq_to_gpio(bank, d->hwirq);
 
 	return _set_gpio_wakeup(bank, gpio, enable);
@@ -691,7 +681,7 @@ static int omap_gpio_request(struct gpio_chip *chip, unsigned offset)
 		_set_gpio_triggering(bank, offset, IRQ_TYPE_NONE);
 		_enable_gpio_module(bank, offset);
 	}
-	bank->mod_usage |= 1 << offset;
+	bank->mod_usage |= BIT(offset);
 	spin_unlock_irqrestore(&bank->lock, flags);
 
 	return 0;
@@ -703,7 +693,7 @@ static void omap_gpio_free(struct gpio_chip *chip, unsigned offset)
 	unsigned long flags;
 
 	spin_lock_irqsave(&bank->lock, flags);
-	bank->mod_usage &= ~(1 << offset);
+	bank->mod_usage &= ~(BIT(offset));
 	_disable_gpio_module(bank, offset);
 	_reset_gpio(bank, bank->chip.base + offset);
 	spin_unlock_irqrestore(&bank->lock, flags);
@@ -732,11 +722,12 @@ static void gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
 	unsigned int bit;
 	struct gpio_bank *bank;
 	int unmasked = 0;
-	struct irq_chip *chip = irq_desc_get_chip(desc);
+	struct irq_chip *irqchip = irq_desc_get_chip(desc);
+	struct gpio_chip *chip = irq_get_handler_data(irq);
 
-	chained_irq_enter(chip, desc);
+	chained_irq_enter(irqchip, desc);
 
-	bank = irq_get_handler_data(irq);
+	bank = container_of(chip, struct gpio_bank, chip);
 	isr_reg = bank->base + bank->regs->irqstatus;
 	pm_runtime_get_sync(bank->dev);
 
@@ -764,7 +755,7 @@ static void gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
 		configured, we could unmask GPIO bank interrupt immediately */
 		if (!level_mask && !unmasked) {
 			unmasked = 1;
-			chained_irq_exit(chip, desc);
+			chained_irq_exit(irqchip, desc);
 		}
 
 		if (!isr)
@@ -772,7 +763,7 @@ static void gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
 
 		while (isr) {
 			bit = __ffs(isr);
-			isr &= ~(1 << bit);
+			isr &= ~(BIT(bit));
 
 			/*
 			 * Some chips can't respond to both rising and falling
@@ -781,10 +772,11 @@ static void gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
 			 * to respond to the IRQ for the opposite direction.
 			 * This will be indicated in the bank toggle_mask.
 			 */
-			if (bank->toggle_mask & (1 << bit))
+			if (bank->toggle_mask & (BIT(bit)))
 				_toggle_gpio_edge_triggering(bank, bit);
 
-			generic_handle_irq(irq_find_mapping(bank->domain, bit));
+			generic_handle_irq(irq_find_mapping(bank->chip.irqdomain,
+							    bit));
 		}
 	}
 	/* if bank has any level sensitive GPIO pin interrupt
@@ -793,20 +785,20 @@ static void gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
 	interrupt */
 exit:
 	if (!unmasked)
-		chained_irq_exit(chip, desc);
+		chained_irq_exit(irqchip, desc);
 	pm_runtime_put(bank->dev);
 }
 
 static void gpio_irq_shutdown(struct irq_data *d)
 {
-	struct gpio_bank *bank = irq_data_get_irq_chip_data(d);
+	struct gpio_bank *bank = _irq_data_get_bank(d);
 	unsigned int gpio = irq_to_gpio(bank, d->hwirq);
 	unsigned long flags;
 	unsigned offset = GPIO_INDEX(bank, gpio);
 
 	spin_lock_irqsave(&bank->lock, flags);
 	gpio_unlock_as_irq(&bank->chip, offset);
-	bank->irq_usage &= ~(1 << offset);
+	bank->irq_usage &= ~(BIT(offset));
 	_disable_gpio_module(bank, offset);
 	_reset_gpio(bank, gpio);
 	spin_unlock_irqrestore(&bank->lock, flags);
@@ -821,7 +813,7 @@ static void gpio_irq_shutdown(struct irq_data *d)
 
 static void gpio_ack_irq(struct irq_data *d)
 {
-	struct gpio_bank *bank = irq_data_get_irq_chip_data(d);
+	struct gpio_bank *bank = _irq_data_get_bank(d);
 	unsigned int gpio = irq_to_gpio(bank, d->hwirq);
 
 	_clear_gpio_irqstatus(bank, gpio);
@@ -829,7 +821,7 @@ static void gpio_ack_irq(struct irq_data *d)
 
 static void gpio_mask_irq(struct irq_data *d)
 {
-	struct gpio_bank *bank = irq_data_get_irq_chip_data(d);
+	struct gpio_bank *bank = _irq_data_get_bank(d);
 	unsigned int gpio = irq_to_gpio(bank, d->hwirq);
 	unsigned long flags;
 
@@ -841,7 +833,7 @@ static void gpio_mask_irq(struct irq_data *d)
 
 static void gpio_unmask_irq(struct irq_data *d)
 {
-	struct gpio_bank *bank = irq_data_get_irq_chip_data(d);
+	struct gpio_bank *bank = _irq_data_get_bank(d);
 	unsigned int gpio = irq_to_gpio(bank, d->hwirq);
 	unsigned int irq_mask = GPIO_BIT(bank, gpio);
 	u32 trigger = irqd_get_trigger_type(d);
@@ -936,6 +928,21 @@ static inline void mpuio_init(struct gpio_bank *bank)
 
 /*---------------------------------------------------------------------*/
 
+static int gpio_get_direction(struct gpio_chip *chip, unsigned offset)
+{
+	struct gpio_bank *bank;
+	unsigned long flags;
+	void __iomem *reg;
+	int dir;
+
+	bank = container_of(chip, struct gpio_bank, chip);
+	reg = bank->base + bank->regs->direction;
+	spin_lock_irqsave(&bank->lock, flags);
+	dir = !!(readl_relaxed(reg) & BIT(offset));
+	spin_unlock_irqrestore(&bank->lock, flags);
+	return dir;
+}
+
 static int gpio_input(struct gpio_chip *chip, unsigned offset)
 {
 	struct gpio_bank *bank;
@@ -954,7 +961,7 @@ static int gpio_get(struct gpio_chip *chip, unsigned offset)
 	u32 mask;
 
 	bank = container_of(chip, struct gpio_bank, chip);
-	mask = (1 << offset);
+	mask = (BIT(offset));
 
 	if (gpio_is_input(bank, mask))
 		return _get_gpio_datain(bank, offset);
@@ -1081,10 +1088,12 @@ omap_mpuio_alloc_gc(struct gpio_bank *bank, unsigned int irq_start,
 			       IRQ_NOREQUEST | IRQ_NOPROBE, 0);
 }
 
-static void omap_gpio_chip_init(struct gpio_bank *bank)
+static int omap_gpio_chip_init(struct gpio_bank *bank)
 {
 	int j;
 	static int gpio;
+	int irq_base = 0;
+	int ret;
 
 	/*
 	 * REVISIT eventually switch from OMAP-specific gpio structs
@@ -1092,12 +1101,12 @@ static void omap_gpio_chip_init(struct gpio_bank *bank)
 	 */
 	bank->chip.request = omap_gpio_request;
 	bank->chip.free = omap_gpio_free;
+	bank->chip.get_direction = gpio_get_direction;
 	bank->chip.direction_input = gpio_input;
 	bank->chip.get = gpio_get;
 	bank->chip.direction_output = gpio_output;
 	bank->chip.set_debounce = gpio_debounce;
 	bank->chip.set = gpio_set;
-	bank->chip.to_irq = omap_gpio_to_irq;
 	if (bank->is_mpuio) {
 		bank->chip.label = "mpuio";
 		if (bank->regs->wkup_en)
@@ -1110,22 +1119,48 @@ static void omap_gpio_chip_init(struct gpio_bank *bank)
 	}
 	bank->chip.ngpio = bank->width;
 
-	gpiochip_add(&bank->chip);
+	ret = gpiochip_add(&bank->chip);
+	if (ret) {
+		dev_err(bank->dev, "Could not register gpio chip %d\n", ret);
+		return ret;
+	}
+
+#ifdef CONFIG_ARCH_OMAP1
+	/*
+	 * REVISIT: Once we have OMAP1 supporting SPARSE_IRQ, we can drop
+	 * irq_alloc_descs() since a base IRQ offset will no longer be needed.
+	 */
+	irq_base = irq_alloc_descs(-1, 0, bank->width, 0);
+	if (irq_base < 0) {
+		dev_err(bank->dev, "Couldn't allocate IRQ numbers\n");
+		return -ENODEV;
+	}
+#endif
+
+	ret = gpiochip_irqchip_add(&bank->chip, &gpio_irq_chip,
+				   irq_base, gpio_irq_handler,
+				   IRQ_TYPE_NONE);
+
+	if (ret) {
+		dev_err(bank->dev, "Couldn't add irqchip to gpiochip %d\n", ret);
+		ret = gpiochip_remove(&bank->chip);
+		return -ENODEV;
+	}
+
+	gpiochip_set_chained_irqchip(&bank->chip, &gpio_irq_chip,
+				     bank->irq, gpio_irq_handler);
 
 	for (j = 0; j < bank->width; j++) {
-		int irq = irq_create_mapping(bank->domain, j);
+		int irq = irq_find_mapping(bank->chip.irqdomain, j);
 		irq_set_lockdep_class(irq, &gpio_lock_class);
-		irq_set_chip_data(irq, bank);
 		if (bank->is_mpuio) {
 			omap_mpuio_alloc_gc(bank, irq, bank->width);
-		} else {
-			irq_set_chip_and_handler(irq, &gpio_irq_chip,
-						 handle_simple_irq);
-			set_irq_flags(irq, IRQF_VALID);
+			irq_set_chip_and_handler(irq, NULL, NULL);
+			set_irq_flags(irq, 0);
 		}
 	}
-	irq_set_chained_handler(bank->irq, gpio_irq_handler);
-	irq_set_handler_data(bank->irq, bank);
+
+	return 0;
 }
 
 static const struct of_device_id omap_gpio_match[];
@@ -1138,9 +1173,7 @@ static int omap_gpio_probe(struct platform_device *pdev)
 	const struct omap_gpio_platform_data *pdata;
 	struct resource *res;
 	struct gpio_bank *bank;
-#ifdef CONFIG_ARCH_OMAP1
-	int irq_base;
-#endif
+	int ret;
 
 	match = of_match_device(of_match_ptr(omap_gpio_match), dev);
 
@@ -1162,6 +1195,7 @@ static int omap_gpio_probe(struct platform_device *pdev)
 
 	bank->irq = res->start;
 	bank->dev = dev;
+	bank->chip.dev = dev;
 	bank->dbck_flag = pdata->dbck_flag;
 	bank->stride = pdata->bank_stride;
 	bank->width = pdata->bank_width;
@@ -1182,29 +1216,6 @@ static int omap_gpio_probe(struct platform_device *pdev)
 				pdata->get_context_loss_count;
 	}
 
-#ifdef CONFIG_ARCH_OMAP1
-	/*
-	 * REVISIT: Once we have OMAP1 supporting SPARSE_IRQ, we can drop
-	 * irq_alloc_descs() and irq_domain_add_legacy() and just use a
-	 * linear IRQ domain mapping for all OMAP platforms.
-	 */
-	irq_base = irq_alloc_descs(-1, 0, bank->width, 0);
-	if (irq_base < 0) {
-		dev_err(dev, "Couldn't allocate IRQ numbers\n");
-		return -ENODEV;
-	}
-
-	bank->domain = irq_domain_add_legacy(node, bank->width, irq_base,
-					     0, &irq_domain_simple_ops, NULL);
-#else
-	bank->domain = irq_domain_add_linear(node, bank->width,
-					     &irq_domain_simple_ops, NULL);
-#endif
-	if (!bank->domain) {
-		dev_err(dev, "Couldn't register an IRQ domain\n");
-		return -ENODEV;
-	}
-
 	if (bank->regs->set_dataout && bank->regs->clr_dataout)
 		bank->set_dataout = _set_gpio_dataout_reg;
 	else
@@ -1216,7 +1227,7 @@ static int omap_gpio_probe(struct platform_device *pdev)
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	bank->base = devm_ioremap_resource(dev, res);
 	if (IS_ERR(bank->base)) {
-		irq_domain_remove(bank->domain);
+		irq_domain_remove(bank->chip.irqdomain);
 		return PTR_ERR(bank->base);
 	}
 
@@ -1230,7 +1241,11 @@ static int omap_gpio_probe(struct platform_device *pdev)
 		mpuio_init(bank);
 
 	omap_gpio_mod_init(bank);
-	omap_gpio_chip_init(bank);
+
+	ret = omap_gpio_chip_init(bank);
+	if (ret)
+		return ret;
+
 	omap_gpio_show_rev(bank);
 
 	pm_runtime_put(bank->dev);
diff --git a/drivers/gpio/gpio-palmas.c b/drivers/gpio/gpio-palmas.c
index da9d33252e5..86bdbe36206 100644
--- a/drivers/gpio/gpio-palmas.c
+++ b/drivers/gpio/gpio-palmas.c
@@ -148,7 +148,7 @@ static const struct palmas_device_data tps80036_dev_data = {
 	.ngpio = 16,
 };
 
-static struct of_device_id of_palmas_gpio_match[] = {
+static const struct of_device_id of_palmas_gpio_match[] = {
 	{ .compatible = "ti,palmas-gpio", .data = &palmas_dev_data,},
 	{ .compatible = "ti,tps65913-gpio", .data = &palmas_dev_data,},
 	{ .compatible = "ti,tps65914-gpio", .data = &palmas_dev_data,},
@@ -173,10 +173,8 @@ static int palmas_gpio_probe(struct platform_device *pdev)
 
 	palmas_gpio = devm_kzalloc(&pdev->dev,
 				sizeof(*palmas_gpio), GFP_KERNEL);
-	if (!palmas_gpio) {
-		dev_err(&pdev->dev, "Could not allocate palmas_gpio\n");
+	if (!palmas_gpio)
 		return -ENOMEM;
-	}
 
 	palmas_gpio->palmas = palmas;
 	palmas_gpio->gpio_chip.owner = THIS_MODULE;
diff --git a/drivers/gpio/gpio-pca953x.c b/drivers/gpio/gpio-pca953x.c
index d550d8e5870..e721a37c347 100644
--- a/drivers/gpio/gpio-pca953x.c
+++ b/drivers/gpio/gpio-pca953x.c
@@ -15,8 +15,6 @@
 #include <linux/init.h>
 #include <linux/gpio.h>
 #include <linux/interrupt.h>
-#include <linux/irq.h>
-#include <linux/irqdomain.h>
 #include <linux/i2c.h>
 #include <linux/platform_data/pca953x.h>
 #include <linux/slab.h>
@@ -91,7 +89,6 @@ struct pca953x_chip {
 	u8 irq_stat[MAX_BANK];
 	u8 irq_trig_raise[MAX_BANK];
 	u8 irq_trig_fall[MAX_BANK];
-	struct irq_domain *domain;
 #endif
 
 	struct i2c_client *client;
@@ -100,6 +97,11 @@ struct pca953x_chip {
 	int	chip_type;
 };
 
+static inline struct pca953x_chip *to_pca(struct gpio_chip *gc)
+{
+	return container_of(gc, struct pca953x_chip, gpio_chip);
+}
+
 static int pca953x_read_single(struct pca953x_chip *chip, int reg, u32 *val,
 				int off)
 {
@@ -202,12 +204,10 @@ static int pca953x_read_regs(struct pca953x_chip *chip, int reg, u8 *val)
 
 static int pca953x_gpio_direction_input(struct gpio_chip *gc, unsigned off)
 {
-	struct pca953x_chip *chip;
+	struct pca953x_chip *chip = to_pca(gc);
 	u8 reg_val;
 	int ret, offset = 0;
 
-	chip = container_of(gc, struct pca953x_chip, gpio_chip);
-
 	mutex_lock(&chip->i2c_lock);
 	reg_val = chip->reg_direction[off / BANK_SZ] | (1u << (off % BANK_SZ));
 
@@ -233,12 +233,10 @@ exit:
 static int pca953x_gpio_direction_output(struct gpio_chip *gc,
 		unsigned off, int val)
 {
-	struct pca953x_chip *chip;
+	struct pca953x_chip *chip = to_pca(gc);
 	u8 reg_val;
 	int ret, offset = 0;
 
-	chip = container_of(gc, struct pca953x_chip, gpio_chip);
-
 	mutex_lock(&chip->i2c_lock);
 	/* set output level */
 	if (val)
@@ -285,12 +283,10 @@ exit:
 
 static int pca953x_gpio_get_value(struct gpio_chip *gc, unsigned off)
 {
-	struct pca953x_chip *chip;
+	struct pca953x_chip *chip = to_pca(gc);
 	u32 reg_val;
 	int ret, offset = 0;
 
-	chip = container_of(gc, struct pca953x_chip, gpio_chip);
-
 	mutex_lock(&chip->i2c_lock);
 	switch (chip->chip_type) {
 	case PCA953X_TYPE:
@@ -315,12 +311,10 @@ static int pca953x_gpio_get_value(struct gpio_chip *gc, unsigned off)
 
 static void pca953x_gpio_set_value(struct gpio_chip *gc, unsigned off, int val)
 {
-	struct pca953x_chip *chip;
+	struct pca953x_chip *chip = to_pca(gc);
 	u8 reg_val;
 	int ret, offset = 0;
 
-	chip = container_of(gc, struct pca953x_chip, gpio_chip);
-
 	mutex_lock(&chip->i2c_lock);
 	if (val)
 		reg_val = chip->reg_output[off / BANK_SZ]
@@ -367,38 +361,34 @@ static void pca953x_setup_gpio(struct pca953x_chip *chip, int gpios)
 }
 
 #ifdef CONFIG_GPIO_PCA953X_IRQ
-static int pca953x_gpio_to_irq(struct gpio_chip *gc, unsigned off)
-{
-	struct pca953x_chip *chip;
-
-	chip = container_of(gc, struct pca953x_chip, gpio_chip);
-	return irq_create_mapping(chip->domain, off);
-}
-
 static void pca953x_irq_mask(struct irq_data *d)
 {
-	struct pca953x_chip *chip = irq_data_get_irq_chip_data(d);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct pca953x_chip *chip = to_pca(gc);
 
 	chip->irq_mask[d->hwirq / BANK_SZ] &= ~(1 << (d->hwirq % BANK_SZ));
 }
 
 static void pca953x_irq_unmask(struct irq_data *d)
 {
-	struct pca953x_chip *chip = irq_data_get_irq_chip_data(d);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct pca953x_chip *chip = to_pca(gc);
 
 	chip->irq_mask[d->hwirq / BANK_SZ] |= 1 << (d->hwirq % BANK_SZ);
 }
 
 static void pca953x_irq_bus_lock(struct irq_data *d)
 {
-	struct pca953x_chip *chip = irq_data_get_irq_chip_data(d);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct pca953x_chip *chip = to_pca(gc);
 
 	mutex_lock(&chip->irq_lock);
 }
 
 static void pca953x_irq_bus_sync_unlock(struct irq_data *d)
 {
-	struct pca953x_chip *chip = irq_data_get_irq_chip_data(d);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct pca953x_chip *chip = to_pca(gc);
 	u8 new_irqs;
 	int level, i;
 
@@ -420,7 +410,8 @@ static void pca953x_irq_bus_sync_unlock(struct irq_data *d)
 
 static int pca953x_irq_set_type(struct irq_data *d, unsigned int type)
 {
-	struct pca953x_chip *chip = irq_data_get_irq_chip_data(d);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct pca953x_chip *chip = to_pca(gc);
 	int bank_nb = d->hwirq / BANK_SZ;
 	u8 mask = 1 << (d->hwirq % BANK_SZ);
 
@@ -503,44 +494,25 @@ static irqreturn_t pca953x_irq_handler(int irq, void *devid)
 	struct pca953x_chip *chip = devid;
 	u8 pending[MAX_BANK];
 	u8 level;
+	unsigned nhandled = 0;
 	int i;
 
 	if (!pca953x_irq_pending(chip, pending))
-		return IRQ_HANDLED;
+		return IRQ_NONE;
 
 	for (i = 0; i < NBANK(chip); i++) {
 		while (pending[i]) {
 			level = __ffs(pending[i]);
-			handle_nested_irq(irq_find_mapping(chip->domain,
+			handle_nested_irq(irq_find_mapping(chip->gpio_chip.irqdomain,
 							level + (BANK_SZ * i)));
 			pending[i] &= ~(1 << level);
+			nhandled++;
 		}
 	}
 
-	return IRQ_HANDLED;
+	return (nhandled > 0) ? IRQ_HANDLED : IRQ_NONE;
 }
 
-static int pca953x_gpio_irq_map(struct irq_domain *d, unsigned int irq,
-		       irq_hw_number_t hwirq)
-{
-	irq_clear_status_flags(irq, IRQ_NOREQUEST);
-	irq_set_chip_data(irq, d->host_data);
-	irq_set_chip(irq, &pca953x_irq_chip);
-	irq_set_nested_thread(irq, true);
-#ifdef CONFIG_ARM
-	set_irq_flags(irq, IRQF_VALID);
-#else
-	irq_set_noprobe(irq);
-#endif
-
-	return 0;
-}
-
-static const struct irq_domain_ops pca953x_irq_simple_ops = {
-	.map = pca953x_gpio_irq_map,
-	.xlate = irq_domain_xlate_twocell,
-};
-
 static int pca953x_irq_setup(struct pca953x_chip *chip,
 			     const struct i2c_device_id *id,
 			     int irq_base)
@@ -572,19 +544,12 @@ static int pca953x_irq_setup(struct pca953x_chip *chip,
 			chip->irq_stat[i] &= chip->reg_direction[i];
 		mutex_init(&chip->irq_lock);
 
-		chip->domain = irq_domain_add_simple(client->dev.of_node,
-						chip->gpio_chip.ngpio,
-						irq_base,
-						&pca953x_irq_simple_ops,
-						chip);
-		if (!chip->domain)
-			return -ENODEV;
-
 		ret = devm_request_threaded_irq(&client->dev,
 					client->irq,
 					   NULL,
 					   pca953x_irq_handler,
-					   IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+					   IRQF_TRIGGER_LOW | IRQF_ONESHOT |
+						   IRQF_SHARED,
 					   dev_name(&client->dev), chip);
 		if (ret) {
 			dev_err(&client->dev, "failed to request irq %d\n",
@@ -592,7 +557,16 @@ static int pca953x_irq_setup(struct pca953x_chip *chip,
 			return ret;
 		}
 
-		chip->gpio_chip.to_irq = pca953x_gpio_to_irq;
+		ret =  gpiochip_irqchip_add(&chip->gpio_chip,
+					    &pca953x_irq_chip,
+					    irq_base,
+					    handle_simple_irq,
+					    IRQ_TYPE_NONE);
+		if (ret) {
+			dev_err(&client->dev,
+				"could not connect irqchip to gpiochip\n");
+			return ret;
+		}
 	}
 
 	return 0;
@@ -756,11 +730,11 @@ static int pca953x_probe(struct i2c_client *client,
 	if (ret)
 		return ret;
 
-	ret = pca953x_irq_setup(chip, id, irq_base);
+	ret = gpiochip_add(&chip->gpio_chip);
 	if (ret)
 		return ret;
 
-	ret = gpiochip_add(&chip->gpio_chip);
+	ret = pca953x_irq_setup(chip, id, irq_base);
 	if (ret)
 		return ret;
 
diff --git a/drivers/gpio/gpio-pcf857x.c b/drivers/gpio/gpio-pcf857x.c
index 82735822bc9..27b46751ea7 100644
--- a/drivers/gpio/gpio-pcf857x.c
+++ b/drivers/gpio/gpio-pcf857x.c
@@ -262,7 +262,7 @@ static int pcf857x_irq_domain_init(struct pcf857x *gpio,
 	/* enable real irq */
 	status = devm_request_threaded_irq(&client->dev, client->irq,
 				NULL, pcf857x_irq, IRQF_ONESHOT |
-				IRQF_TRIGGER_FALLING,
+				IRQF_TRIGGER_FALLING | IRQF_SHARED,
 				dev_name(&client->dev), gpio);
 
 	if (status)
@@ -319,7 +319,7 @@ static int pcf857x_probe(struct i2c_client *client,
 		status = pcf857x_irq_domain_init(gpio, client);
 		if (status < 0) {
 			dev_err(&client->dev, "irq_domain init failed\n");
-			goto fail;
+			goto fail_irq_domain;
 		}
 	}
 
@@ -414,12 +414,13 @@ static int pcf857x_probe(struct i2c_client *client,
 	return 0;
 
 fail:
-	dev_dbg(&client->dev, "probe error %d for '%s'\n",
-			status, client->name);
-
 	if (client->irq)
 		pcf857x_irq_domain_cleanup(gpio);
 
+fail_irq_domain:
+	dev_dbg(&client->dev, "probe error %d for '%s'\n",
+		status, client->name);
+
 	return status;
 }
 
diff --git a/drivers/gpio/gpio-pch.c b/drivers/gpio/gpio-pch.c
index 83a15639747..d6eac9b17db 100644
--- a/drivers/gpio/gpio-pch.c
+++ b/drivers/gpio/gpio-pch.c
@@ -20,6 +20,7 @@
 #include <linux/gpio.h>
 #include <linux/interrupt.h>
 #include <linux/irq.h>
+#include <linux/slab.h>
 
 #define PCH_EDGE_FALLING	0
 #define PCH_EDGE_RISING		BIT(0)
diff --git a/drivers/gpio/gpio-pl061.c b/drivers/gpio/gpio-pl061.c
index b0f475243ce..84b49cfb81a 100644
--- a/drivers/gpio/gpio-pl061.c
+++ b/drivers/gpio/gpio-pl061.c
@@ -17,7 +17,6 @@
 #include <linux/irq.h>
 #include <linux/irqchip/chained_irq.h>
 #include <linux/bitops.h>
-#include <linux/workqueue.h>
 #include <linux/gpio.h>
 #include <linux/device.h>
 #include <linux/amba/bus.h>
@@ -88,7 +87,7 @@ static int pl061_direction_input(struct gpio_chip *gc, unsigned offset)
 
 	spin_lock_irqsave(&chip->lock, flags);
 	gpiodir = readb(chip->base + GPIODIR);
-	gpiodir &= ~(1 << offset);
+	gpiodir &= ~(BIT(offset));
 	writeb(gpiodir, chip->base + GPIODIR);
 	spin_unlock_irqrestore(&chip->lock, flags);
 
@@ -106,16 +105,16 @@ static int pl061_direction_output(struct gpio_chip *gc, unsigned offset,
 		return -EINVAL;
 
 	spin_lock_irqsave(&chip->lock, flags);
-	writeb(!!value << offset, chip->base + (1 << (offset + 2)));
+	writeb(!!value << offset, chip->base + (BIT(offset + 2)));
 	gpiodir = readb(chip->base + GPIODIR);
-	gpiodir |= 1 << offset;
+	gpiodir |= BIT(offset);
 	writeb(gpiodir, chip->base + GPIODIR);
 
 	/*
 	 * gpio value is set again, because pl061 doesn't allow to set value of
 	 * a gpio pin before configuring it in OUT mode.
 	 */
-	writeb(!!value << offset, chip->base + (1 << (offset + 2)));
+	writeb(!!value << offset, chip->base + (BIT(offset + 2)));
 	spin_unlock_irqrestore(&chip->lock, flags);
 
 	return 0;
@@ -125,14 +124,14 @@ static int pl061_get_value(struct gpio_chip *gc, unsigned offset)
 {
 	struct pl061_gpio *chip = container_of(gc, struct pl061_gpio, gc);
 
-	return !!readb(chip->base + (1 << (offset + 2)));
+	return !!readb(chip->base + (BIT(offset + 2)));
 }
 
 static void pl061_set_value(struct gpio_chip *gc, unsigned offset, int value)
 {
 	struct pl061_gpio *chip = container_of(gc, struct pl061_gpio, gc);
 
-	writeb(!!value << offset, chip->base + (1 << (offset + 2)));
+	writeb(!!value << offset, chip->base + (BIT(offset + 2)));
 }
 
 static int pl061_irq_type(struct irq_data *d, unsigned trigger)
@@ -207,7 +206,7 @@ static void pl061_irq_mask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
 	struct pl061_gpio *chip = container_of(gc, struct pl061_gpio, gc);
-	u8 mask = 1 << (irqd_to_hwirq(d) % PL061_GPIO_NR);
+	u8 mask = BIT(irqd_to_hwirq(d) % PL061_GPIO_NR);
 	u8 gpioie;
 
 	spin_lock(&chip->lock);
@@ -220,7 +219,7 @@ static void pl061_irq_unmask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
 	struct pl061_gpio *chip = container_of(gc, struct pl061_gpio, gc);
-	u8 mask = 1 << (irqd_to_hwirq(d) % PL061_GPIO_NR);
+	u8 mask = BIT(irqd_to_hwirq(d) % PL061_GPIO_NR);
 	u8 gpioie;
 
 	spin_lock(&chip->lock);
@@ -302,9 +301,9 @@ static int pl061_probe(struct amba_device *adev, const struct amba_id *id)
 
 	for (i = 0; i < PL061_GPIO_NR; i++) {
 		if (pdata) {
-			if (pdata->directions & (1 << i))
+			if (pdata->directions & (BIT(i)))
 				pl061_direction_output(&chip->gc, i,
-						pdata->values & (1 << i));
+						pdata->values & (BIT(i)));
 			else
 				pl061_direction_input(&chip->gc, i);
 		}
@@ -331,7 +330,7 @@ static int pl061_suspend(struct device *dev)
 	chip->csave_regs.gpio_ie = readb(chip->base + GPIOIE);
 
 	for (offset = 0; offset < PL061_GPIO_NR; offset++) {
-		if (chip->csave_regs.gpio_dir & (1 << offset))
+		if (chip->csave_regs.gpio_dir & (BIT(offset)))
 			chip->csave_regs.gpio_data |=
 				pl061_get_value(&chip->gc, offset) << offset;
 	}
@@ -345,10 +344,10 @@ static int pl061_resume(struct device *dev)
 	int offset;
 
 	for (offset = 0; offset < PL061_GPIO_NR; offset++) {
-		if (chip->csave_regs.gpio_dir & (1 << offset))
+		if (chip->csave_regs.gpio_dir & (BIT(offset)))
 			pl061_direction_output(&chip->gc, offset,
 					chip->csave_regs.gpio_data &
-					(1 << offset));
+					(BIT(offset)));
 		else
 			pl061_direction_input(&chip->gc, offset);
 	}
diff --git a/drivers/gpio/gpio-rc5t583.c b/drivers/gpio/gpio-rc5t583.c
index 9b423173ab5..562b0c4d9cc 100644
--- a/drivers/gpio/gpio-rc5t583.c
+++ b/drivers/gpio/gpio-rc5t583.c
@@ -119,10 +119,8 @@ static int rc5t583_gpio_probe(struct platform_device *pdev)
 
 	rc5t583_gpio = devm_kzalloc(&pdev->dev, sizeof(*rc5t583_gpio),
 					GFP_KERNEL);
-	if (!rc5t583_gpio) {
-		dev_warn(&pdev->dev, "Mem allocation for rc5t583_gpio failed");
+	if (!rc5t583_gpio)
 		return -ENOMEM;
-	}
 
 	rc5t583_gpio->gpio_chip.label = "gpio-rc5t583",
 	rc5t583_gpio->gpio_chip.owner = THIS_MODULE,
diff --git a/drivers/gpio/gpio-rcar.c b/drivers/gpio/gpio-rcar.c
index 03c91482432..0c9f803fc1a 100644
--- a/drivers/gpio/gpio-rcar.c
+++ b/drivers/gpio/gpio-rcar.c
@@ -26,6 +26,7 @@
 #include <linux/pinctrl/consumer.h>
 #include <linux/platform_data/gpio-rcar.h>
 #include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
 #include <linux/spinlock.h>
 #include <linux/slab.h>
 
@@ -362,7 +363,6 @@ static int gpio_rcar_probe(struct platform_device *pdev)
 
 	p = devm_kzalloc(dev, sizeof(*p), GFP_KERNEL);
 	if (!p) {
-		dev_err(dev, "failed to allocate driver data\n");
 		ret = -ENOMEM;
 		goto err0;
 	}
@@ -377,6 +377,9 @@ static int gpio_rcar_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, p);
 
+	pm_runtime_enable(dev);
+	pm_runtime_get_sync(dev);
+
 	io = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 
@@ -460,6 +463,8 @@ static int gpio_rcar_probe(struct platform_device *pdev)
 err1:
 	irq_domain_remove(p->irq_domain);
 err0:
+	pm_runtime_put(dev);
+	pm_runtime_disable(dev);
 	return ret;
 }
 
@@ -473,6 +478,8 @@ static int gpio_rcar_remove(struct platform_device *pdev)
 		return ret;
 
 	irq_domain_remove(p->irq_domain);
+	pm_runtime_put(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
 	return 0;
 }
 
diff --git a/drivers/gpio/gpio-rdc321x.c b/drivers/gpio/gpio-rdc321x.c
index 88577c3272a..9fa7e53331c 100644
--- a/drivers/gpio/gpio-rdc321x.c
+++ b/drivers/gpio/gpio-rdc321x.c
@@ -141,17 +141,15 @@ static int rdc321x_gpio_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	rdc321x_gpio_dev = kzalloc(sizeof(struct rdc321x_gpio), GFP_KERNEL);
-	if (!rdc321x_gpio_dev) {
-		dev_err(&pdev->dev, "failed to allocate private data\n");
+	rdc321x_gpio_dev = devm_kzalloc(&pdev->dev, sizeof(struct rdc321x_gpio),
+					GFP_KERNEL);
+	if (!rdc321x_gpio_dev)
 		return -ENOMEM;
-	}
 
 	r = platform_get_resource_byname(pdev, IORESOURCE_IO, "gpio-reg1");
 	if (!r) {
 		dev_err(&pdev->dev, "failed to get gpio-reg1 resource\n");
-		err = -ENODEV;
-		goto out_free;
+		return -ENODEV;
 	}
 
 	spin_lock_init(&rdc321x_gpio_dev->lock);
@@ -162,8 +160,7 @@ static int rdc321x_gpio_probe(struct platform_device *pdev)
 	r = platform_get_resource_byname(pdev, IORESOURCE_IO, "gpio-reg2");
 	if (!r) {
 		dev_err(&pdev->dev, "failed to get gpio-reg2 resource\n");
-		err = -ENODEV;
-		goto out_free;
+		return -ENODEV;
 	}
 
 	rdc321x_gpio_dev->reg2_ctrl_base = r->start;
@@ -187,21 +184,17 @@ static int rdc321x_gpio_probe(struct platform_device *pdev)
 					rdc321x_gpio_dev->reg1_data_base,
 					&rdc321x_gpio_dev->data_reg[0]);
 	if (err)
-		goto out_free;
+		return err;
 
 	err = pci_read_config_dword(rdc321x_gpio_dev->sb_pdev,
 					rdc321x_gpio_dev->reg2_data_base,
 					&rdc321x_gpio_dev->data_reg[1]);
 	if (err)
-		goto out_free;
+		return err;
 
 	dev_info(&pdev->dev, "registering %d GPIOs\n",
 					rdc321x_gpio_dev->chip.ngpio);
 	return gpiochip_add(&rdc321x_gpio_dev->chip);
-
-out_free:
-	kfree(rdc321x_gpio_dev);
-	return err;
 }
 
 static int rdc321x_gpio_remove(struct platform_device *pdev)
@@ -213,8 +206,6 @@ static int rdc321x_gpio_remove(struct platform_device *pdev)
 	if (ret)
 		dev_err(&pdev->dev, "failed to unregister chip\n");
 
-	kfree(rdc321x_gpio_dev);
-
 	return ret;
 }
 
diff --git a/drivers/gpio/gpio-sch.c b/drivers/gpio/gpio-sch.c
index 5af65719b95..a9b1cd16c84 100644
--- a/drivers/gpio/gpio-sch.c
+++ b/drivers/gpio/gpio-sch.c
@@ -97,8 +97,6 @@ static int sch_gpio_core_direction_out(struct gpio_chip *gc,
 	u8 curr_dirs;
 	unsigned short offset, bit;
 
-	sch_gpio_core_set(gc, gpio_num, val);
-
 	spin_lock(&gpio_lock);
 
 	offset = CGIO + gpio_num / 8;
@@ -109,6 +107,17 @@ static int sch_gpio_core_direction_out(struct gpio_chip *gc,
 		outb(curr_dirs & ~(1 << bit), gpio_ba + offset);
 
 	spin_unlock(&gpio_lock);
+
+	/*
+	 * according to the datasheet, writing to the level register has no
+	 * effect when GPIO is programmed as input.
+	 * Actually the the level register is read-only when configured as input.
+	 * Thus presetting the output level before switching to output is _NOT_ possible.
+	 * Hence we set the level after configuring the GPIO as output.
+	 * But we cannot prevent a short low pulse if direction is set to high
+	 * and an external pull-up is connected.
+	 */
+	sch_gpio_core_set(gc, gpio_num, val);
 	return 0;
 }
 
@@ -178,8 +187,6 @@ static int sch_gpio_resume_direction_out(struct gpio_chip *gc,
 	u8 curr_dirs;
 	unsigned short offset, bit;
 
-	sch_gpio_resume_set(gc, gpio_num, val);
-
 	offset = RGIO + gpio_num / 8;
 	bit = gpio_num % 8;
 
@@ -190,6 +197,17 @@ static int sch_gpio_resume_direction_out(struct gpio_chip *gc,
 		outb(curr_dirs & ~(1 << bit), gpio_ba + offset);
 
 	spin_unlock(&gpio_lock);
+
+	/*
+	* according to the datasheet, writing to the level register has no
+	* effect when GPIO is programmed as input.
+	* Actually the the level register is read-only when configured as input.
+	* Thus presetting the output level before switching to output is _NOT_ possible.
+	* Hence we set the level after configuring the GPIO as output.
+	* But we cannot prevent a short low pulse if direction is set to high
+	* and an external pull-up is connected.
+	*/
+	sch_gpio_resume_set(gc, gpio_num, val);
 	return 0;
 }
 
diff --git a/drivers/gpio/gpio-sch311x.c b/drivers/gpio/gpio-sch311x.c
index 0357387b364..f942b80ee40 100644
--- a/drivers/gpio/gpio-sch311x.c
+++ b/drivers/gpio/gpio-sch311x.c
@@ -327,14 +327,22 @@ static int __init sch311x_detect(int sio_config_port, unsigned short *addr)
 	if (err)
 		return err;
 
-	/* Check device ID. We currently know about:
-	 * SCH3112 (0x7c), SCH3114 (0x7d), and SCH3116 (0x7f). */
+	/* Check device ID. */
 	reg = sch311x_sio_inb(sio_config_port, 0x20);
-	if (!(reg == 0x7c || reg == 0x7d || reg == 0x7f)) {
+	switch (reg) {
+	case 0x7c: /* SCH3112 */
+		dev_id = 2;
+		break;
+	case 0x7d: /* SCH3114 */
+		dev_id = 4;
+		break;
+	case 0x7f: /* SCH3116 */
+		dev_id = 6;
+		break;
+	default:
 		err = -ENODEV;
 		goto exit;
 	}
-	dev_id = reg == 0x7c ? 2 : reg == 0x7d ? 4 : 6;
 
 	/* Select logical device A (runtime registers) */
 	sch311x_sio_outb(sio_config_port, 0x07, 0x0a);
diff --git a/drivers/gpio/gpio-spear-spics.c b/drivers/gpio/gpio-spear-spics.c
index e9a0415834e..353263c85d2 100644
--- a/drivers/gpio/gpio-spear-spics.c
+++ b/drivers/gpio/gpio-spear-spics.c
@@ -2,7 +2,7 @@
  * SPEAr platform SPI chipselect abstraction over gpiolib
  *
  * Copyright (C) 2012 ST Microelectronics
- * Shiraz Hashim <shiraz.hashim@st.com>
+ * Shiraz Hashim <shiraz.linux.kernel@gmail.com>
  *
  * This file is licensed under the terms of the GNU General Public
  * License version 2. This program is licensed "as is" without any
@@ -129,10 +129,8 @@ static int spics_gpio_probe(struct platform_device *pdev)
 	int ret;
 
 	spics = devm_kzalloc(&pdev->dev, sizeof(*spics), GFP_KERNEL);
-	if (!spics) {
-		dev_err(&pdev->dev, "memory allocation fail\n");
+	if (!spics)
 		return -ENOMEM;
-	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	spics->base = devm_ioremap_resource(&pdev->dev, res);
@@ -205,6 +203,6 @@ static int __init spics_gpio_init(void)
 }
 subsys_initcall(spics_gpio_init);
 
-MODULE_AUTHOR("Shiraz Hashim <shiraz.hashim@st.com>");
+MODULE_AUTHOR("Shiraz Hashim <shiraz.linux.kernel@gmail.com>");
 MODULE_DESCRIPTION("ST Microlectronics SPEAr SPI Chip Select Abstraction");
 MODULE_LICENSE("GPL");
diff --git a/drivers/gpio/gpio-sx150x.c b/drivers/gpio/gpio-sx150x.c
index 13d73fb2b5e..b51ca9f5c14 100644
--- a/drivers/gpio/gpio-sx150x.c
+++ b/drivers/gpio/gpio-sx150x.c
@@ -22,7 +22,6 @@
 #include <linux/module.h>
 #include <linux/mutex.h>
 #include <linux/slab.h>
-#include <linux/workqueue.h>
 #include <linux/i2c/sx150x.h>
 
 #define NO_UPDATE_PENDING	-1
diff --git a/drivers/gpio/gpio-tc3589x.c b/drivers/gpio/gpio-tc3589x.c
index 1019320984d..51f7cbd9ff7 100644
--- a/drivers/gpio/gpio-tc3589x.c
+++ b/drivers/gpio/gpio-tc3589x.c
@@ -12,8 +12,6 @@
 #include <linux/slab.h>
 #include <linux/gpio.h>
 #include <linux/of.h>
-#include <linux/irq.h>
-#include <linux/irqdomain.h>
 #include <linux/interrupt.h>
 #include <linux/mfd/tc3589x.h>
 
@@ -31,10 +29,6 @@ struct tc3589x_gpio {
 	struct tc3589x *tc3589x;
 	struct device *dev;
 	struct mutex irq_lock;
-	struct irq_domain *domain;
-
-	int irq_base;
-
 	/* Caches of interrupt control registers for bus_lock */
 	u8 regs[CACHE_NR_REGS][CACHE_NR_BANKS];
 	u8 oldregs[CACHE_NR_REGS][CACHE_NR_BANKS];
@@ -95,30 +89,6 @@ static int tc3589x_gpio_direction_input(struct gpio_chip *chip,
 	return tc3589x_set_bits(tc3589x, reg, 1 << pos, 0);
 }
 
-/**
- * tc3589x_gpio_irq_get_irq(): Map a hardware IRQ on a chip to a Linux IRQ
- *
- * @tc3589x_gpio: tc3589x_gpio_irq controller to operate on.
- * @irq: index of the hardware interrupt requested in the chip IRQs
- *
- * Useful for drivers to request their own IRQs.
- */
-static int tc3589x_gpio_irq_get_irq(struct tc3589x_gpio *tc3589x_gpio,
-				     int hwirq)
-{
-	if (!tc3589x_gpio)
-		return -EINVAL;
-
-	return irq_create_mapping(tc3589x_gpio->domain, hwirq);
-}
-
-static int tc3589x_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
-{
-	struct tc3589x_gpio *tc3589x_gpio = to_tc3589x_gpio(chip);
-
-	return tc3589x_gpio_irq_get_irq(tc3589x_gpio, offset);
-}
-
 static struct gpio_chip template_chip = {
 	.label			= "tc3589x",
 	.owner			= THIS_MODULE,
@@ -126,13 +96,13 @@ static struct gpio_chip template_chip = {
 	.get			= tc3589x_gpio_get,
 	.direction_output	= tc3589x_gpio_direction_output,
 	.set			= tc3589x_gpio_set,
-	.to_irq			= tc3589x_gpio_to_irq,
 	.can_sleep		= true,
 };
 
 static int tc3589x_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 {
-	struct tc3589x_gpio *tc3589x_gpio = irq_data_get_irq_chip_data(d);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct tc3589x_gpio *tc3589x_gpio = container_of(gc, struct tc3589x_gpio, chip);
 	int offset = d->hwirq;
 	int regoffset = offset / 8;
 	int mask = 1 << (offset % 8);
@@ -159,14 +129,16 @@ static int tc3589x_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 
 static void tc3589x_gpio_irq_lock(struct irq_data *d)
 {
-	struct tc3589x_gpio *tc3589x_gpio = irq_data_get_irq_chip_data(d);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct tc3589x_gpio *tc3589x_gpio = container_of(gc, struct tc3589x_gpio, chip);
 
 	mutex_lock(&tc3589x_gpio->irq_lock);
 }
 
 static void tc3589x_gpio_irq_sync_unlock(struct irq_data *d)
 {
-	struct tc3589x_gpio *tc3589x_gpio = irq_data_get_irq_chip_data(d);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct tc3589x_gpio *tc3589x_gpio = container_of(gc, struct tc3589x_gpio, chip);
 	struct tc3589x *tc3589x = tc3589x_gpio->tc3589x;
 	static const u8 regmap[] = {
 		[REG_IBE]	= TC3589x_GPIOIBE0,
@@ -194,7 +166,8 @@ static void tc3589x_gpio_irq_sync_unlock(struct irq_data *d)
 
 static void tc3589x_gpio_irq_mask(struct irq_data *d)
 {
-	struct tc3589x_gpio *tc3589x_gpio = irq_data_get_irq_chip_data(d);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct tc3589x_gpio *tc3589x_gpio = container_of(gc, struct tc3589x_gpio, chip);
 	int offset = d->hwirq;
 	int regoffset = offset / 8;
 	int mask = 1 << (offset % 8);
@@ -204,7 +177,8 @@ static void tc3589x_gpio_irq_mask(struct irq_data *d)
 
 static void tc3589x_gpio_irq_unmask(struct irq_data *d)
 {
-	struct tc3589x_gpio *tc3589x_gpio = irq_data_get_irq_chip_data(d);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct tc3589x_gpio *tc3589x_gpio = container_of(gc, struct tc3589x_gpio, chip);
 	int offset = d->hwirq;
 	int regoffset = offset / 8;
 	int mask = 1 << (offset % 8);
@@ -242,7 +216,8 @@ static irqreturn_t tc3589x_gpio_irq(int irq, void *dev)
 		while (stat) {
 			int bit = __ffs(stat);
 			int line = i * 8 + bit;
-			int irq = tc3589x_gpio_irq_get_irq(tc3589x_gpio, line);
+			int irq = irq_find_mapping(tc3589x_gpio->chip.irqdomain,
+						   line);
 
 			handle_nested_irq(irq);
 			stat &= ~(1 << bit);
@@ -254,61 +229,6 @@ static irqreturn_t tc3589x_gpio_irq(int irq, void *dev)
 	return IRQ_HANDLED;
 }
 
-static int tc3589x_gpio_irq_map(struct irq_domain *d, unsigned int irq,
-				irq_hw_number_t hwirq)
-{
-	struct tc3589x *tc3589x_gpio = d->host_data;
-
-	irq_set_chip_data(irq, tc3589x_gpio);
-	irq_set_chip_and_handler(irq, &tc3589x_gpio_irq_chip,
-				handle_simple_irq);
-	irq_set_nested_thread(irq, 1);
-#ifdef CONFIG_ARM
-	set_irq_flags(irq, IRQF_VALID);
-#else
-	irq_set_noprobe(irq);
-#endif
-
-	return 0;
-}
-
-static void tc3589x_gpio_irq_unmap(struct irq_domain *d, unsigned int irq)
-{
-#ifdef CONFIG_ARM
-	set_irq_flags(irq, 0);
-#endif
-	irq_set_chip_and_handler(irq, NULL, NULL);
-	irq_set_chip_data(irq, NULL);
-}
-
-static struct irq_domain_ops tc3589x_irq_ops = {
-	.map    = tc3589x_gpio_irq_map,
-	.unmap  = tc3589x_gpio_irq_unmap,
-	.xlate  = irq_domain_xlate_twocell,
-};
-
-static int tc3589x_gpio_irq_init(struct tc3589x_gpio *tc3589x_gpio,
-				struct device_node *np)
-{
-	int base = tc3589x_gpio->irq_base;
-
-	/*
-	 * If this results in a linear domain, irq_create_mapping() will
-	 * take care of allocating IRQ descriptors at runtime. When a base
-	 * is provided, the IRQ descriptors will be allocated when the
-	 * domain is instantiated.
-	 */
-	tc3589x_gpio->domain = irq_domain_add_simple(np,
-			tc3589x_gpio->chip.ngpio, base, &tc3589x_irq_ops,
-			tc3589x_gpio);
-	if (!tc3589x_gpio->domain) {
-		dev_err(tc3589x_gpio->dev, "Failed to create irqdomain\n");
-		return -ENOSYS;
-	}
-
-	return 0;
-}
-
 static int tc3589x_gpio_probe(struct platform_device *pdev)
 {
 	struct tc3589x *tc3589x = dev_get_drvdata(pdev->dev.parent);
@@ -329,7 +249,8 @@ static int tc3589x_gpio_probe(struct platform_device *pdev)
 	if (irq < 0)
 		return irq;
 
-	tc3589x_gpio = kzalloc(sizeof(struct tc3589x_gpio), GFP_KERNEL);
+	tc3589x_gpio = devm_kzalloc(&pdev->dev, sizeof(struct tc3589x_gpio),
+				    GFP_KERNEL);
 	if (!tc3589x_gpio)
 		return -ENOMEM;
 
@@ -347,30 +268,36 @@ static int tc3589x_gpio_probe(struct platform_device *pdev)
 	tc3589x_gpio->chip.of_node = np;
 #endif
 
-	tc3589x_gpio->irq_base = tc3589x->irq_base ?
-		tc3589x->irq_base + TC3589x_INT_GPIO(0) : 0;
-
 	/* Bring the GPIO module out of reset */
 	ret = tc3589x_set_bits(tc3589x, TC3589x_RSTCTRL,
 			       TC3589x_RSTCTRL_GPIRST, 0);
 	if (ret < 0)
-		goto out_free;
-
-	ret = tc3589x_gpio_irq_init(tc3589x_gpio, np);
-	if (ret)
-		goto out_free;
+		return ret;
 
-	ret = request_threaded_irq(irq, NULL, tc3589x_gpio_irq, IRQF_ONESHOT,
-				   "tc3589x-gpio", tc3589x_gpio);
+	ret = devm_request_threaded_irq(&pdev->dev,
+					irq, NULL, tc3589x_gpio_irq,
+					IRQF_ONESHOT, "tc3589x-gpio",
+					tc3589x_gpio);
 	if (ret) {
 		dev_err(&pdev->dev, "unable to get irq: %d\n", ret);
-		goto out_free;
+		return ret;
 	}
 
 	ret = gpiochip_add(&tc3589x_gpio->chip);
 	if (ret) {
 		dev_err(&pdev->dev, "unable to add gpiochip: %d\n", ret);
-		goto out_freeirq;
+		return ret;
+	}
+
+	ret =  gpiochip_irqchip_add(&tc3589x_gpio->chip,
+				    &tc3589x_gpio_irq_chip,
+				    0,
+				    handle_simple_irq,
+				    IRQ_TYPE_NONE);
+	if (ret) {
+		dev_err(&pdev->dev,
+			"could not connect irqchip to gpiochip\n");
+		return ret;
 	}
 
 	if (pdata && pdata->setup)
@@ -379,12 +306,6 @@ static int tc3589x_gpio_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, tc3589x_gpio);
 
 	return 0;
-
-out_freeirq:
-	free_irq(irq, tc3589x_gpio);
-out_free:
-	kfree(tc3589x_gpio);
-	return ret;
 }
 
 static int tc3589x_gpio_remove(struct platform_device *pdev)
@@ -392,7 +313,6 @@ static int tc3589x_gpio_remove(struct platform_device *pdev)
 	struct tc3589x_gpio *tc3589x_gpio = platform_get_drvdata(pdev);
 	struct tc3589x *tc3589x = tc3589x_gpio->tc3589x;
 	struct tc3589x_gpio_platform_data *pdata = tc3589x->pdata->gpio;
-	int irq = platform_get_irq(pdev, 0);
 	int ret;
 
 	if (pdata && pdata->remove)
@@ -405,10 +325,6 @@ static int tc3589x_gpio_remove(struct platform_device *pdev)
 		return ret;
 	}
 
-	free_irq(irq, tc3589x_gpio);
-
-	kfree(tc3589x_gpio);
-
 	return 0;
 }
 
diff --git a/drivers/gpio/gpio-tegra.c b/drivers/gpio/gpio-tegra.c
index 2b49f878b56..4e8fb8261a8 100644
--- a/drivers/gpio/gpio-tegra.c
+++ b/drivers/gpio/gpio-tegra.c
@@ -408,7 +408,7 @@ static struct tegra_gpio_soc_config tegra30_gpio_config = {
 	.upper_offset = 0x80,
 };
 
-static struct of_device_id tegra_gpio_of_match[] = {
+static const struct of_device_id tegra_gpio_of_match[] = {
 	{ .compatible = "nvidia,tegra30-gpio", .data = &tegra30_gpio_config },
 	{ .compatible = "nvidia,tegra20-gpio", .data = &tegra20_gpio_config },
 	{ },
@@ -458,10 +458,8 @@ static int tegra_gpio_probe(struct platform_device *pdev)
 	tegra_gpio_banks = devm_kzalloc(&pdev->dev,
 			tegra_gpio_bank_count * sizeof(*tegra_gpio_banks),
 			GFP_KERNEL);
-	if (!tegra_gpio_banks) {
-		dev_err(&pdev->dev, "Couldn't allocate bank structure\n");
+	if (!tegra_gpio_banks)
 		return -ENODEV;
-	}
 
 	irq_domain = irq_domain_add_linear(pdev->dev.of_node,
 					   tegra_gpio_chip.ngpio,
diff --git a/drivers/gpio/gpio-timberdale.c b/drivers/gpio/gpio-timberdale.c
index f9a8fbde108..efc7c129016 100644
--- a/drivers/gpio/gpio-timberdale.c
+++ b/drivers/gpio/gpio-timberdale.c
@@ -224,6 +224,7 @@ static struct irq_chip timbgpio_irqchip = {
 static int timbgpio_probe(struct platform_device *pdev)
 {
 	int err, i;
+	struct device *dev = &pdev->dev;
 	struct gpio_chip *gc;
 	struct timbgpio *tgpio;
 	struct resource *iomem;
@@ -231,35 +232,35 @@ static int timbgpio_probe(struct platform_device *pdev)
 	int irq = platform_get_irq(pdev, 0);
 
 	if (!pdata || pdata->nr_pins > 32) {
-		err = -EINVAL;
-		goto err_mem;
+		dev_err(dev, "Invalid platform data\n");
+		return -EINVAL;
 	}
 
 	iomem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!iomem) {
-		err = -EINVAL;
-		goto err_mem;
+		dev_err(dev, "Unable to get resource\n");
+		return -EINVAL;
 	}
 
-	tgpio = kzalloc(sizeof(*tgpio), GFP_KERNEL);
+	tgpio = devm_kzalloc(dev, sizeof(struct timbgpio), GFP_KERNEL);
 	if (!tgpio) {
-		err = -EINVAL;
-		goto err_mem;
+		dev_err(dev, "Memory alloc failed\n");
+		return -EINVAL;
 	}
 	tgpio->irq_base = pdata->irq_base;
 
 	spin_lock_init(&tgpio->lock);
 
-	if (!request_mem_region(iomem->start, resource_size(iomem),
-		DRIVER_NAME)) {
-		err = -EBUSY;
-		goto err_request;
+	if (!devm_request_mem_region(dev, iomem->start, resource_size(iomem),
+				     DRIVER_NAME)) {
+		dev_err(dev, "Region already claimed\n");
+		return -EBUSY;
 	}
 
-	tgpio->membase = ioremap(iomem->start, resource_size(iomem));
+	tgpio->membase = devm_ioremap(dev, iomem->start, resource_size(iomem));
 	if (!tgpio->membase) {
-		err = -ENOMEM;
-		goto err_ioremap;
+		dev_err(dev, "Cannot ioremap\n");
+		return -ENOMEM;
 	}
 
 	gc = &tgpio->gpio;
@@ -279,7 +280,7 @@ static int timbgpio_probe(struct platform_device *pdev)
 
 	err = gpiochip_add(gc);
 	if (err)
-		goto err_chipadd;
+		return err;
 
 	platform_set_drvdata(pdev, tgpio);
 
@@ -302,17 +303,6 @@ static int timbgpio_probe(struct platform_device *pdev)
 	irq_set_chained_handler(irq, timbgpio_irq);
 
 	return 0;
-
-err_chipadd:
-	iounmap(tgpio->membase);
-err_ioremap:
-	release_mem_region(iomem->start, resource_size(iomem));
-err_request:
-	kfree(tgpio);
-err_mem:
-	printk(KERN_ERR DRIVER_NAME": Failed to register GPIOs: %d\n", err);
-
-	return err;
 }
 
 static int timbgpio_remove(struct platform_device *pdev)
@@ -320,7 +310,6 @@ static int timbgpio_remove(struct platform_device *pdev)
 	int err;
 	struct timbgpio_platform_data *pdata = dev_get_platdata(&pdev->dev);
 	struct timbgpio *tgpio = platform_get_drvdata(pdev);
-	struct resource *iomem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	int irq = platform_get_irq(pdev, 0);
 
 	if (irq >= 0 && tgpio->irq_base > 0) {
@@ -338,10 +327,6 @@ static int timbgpio_remove(struct platform_device *pdev)
 	if (err)
 		printk(KERN_ERR DRIVER_NAME": failed to remove gpio_chip\n");
 
-	iounmap(tgpio->membase);
-	release_mem_region(iomem->start, resource_size(iomem));
-	kfree(tgpio);
-
 	return 0;
 }
 
diff --git a/drivers/gpio/gpio-tps6586x.c b/drivers/gpio/gpio-tps6586x.c
index 8994dfa1349..a69fbea4125 100644
--- a/drivers/gpio/gpio-tps6586x.c
+++ b/drivers/gpio/gpio-tps6586x.c
@@ -97,10 +97,8 @@ static int tps6586x_gpio_probe(struct platform_device *pdev)
 	pdata = dev_get_platdata(pdev->dev.parent);
 	tps6586x_gpio = devm_kzalloc(&pdev->dev,
 				sizeof(*tps6586x_gpio), GFP_KERNEL);
-	if (!tps6586x_gpio) {
-		dev_err(&pdev->dev, "Could not allocate tps6586x_gpio\n");
+	if (!tps6586x_gpio)
 		return -ENOMEM;
-	}
 
 	tps6586x_gpio->parent = pdev->dev.parent;
 
diff --git a/drivers/gpio/gpio-tps65910.c b/drivers/gpio/gpio-tps65910.c
index b6e818e6800..e2f8cda235e 100644
--- a/drivers/gpio/gpio-tps65910.c
+++ b/drivers/gpio/gpio-tps65910.c
@@ -123,10 +123,8 @@ static int tps65910_gpio_probe(struct platform_device *pdev)
 
 	tps65910_gpio = devm_kzalloc(&pdev->dev,
 				sizeof(*tps65910_gpio), GFP_KERNEL);
-	if (!tps65910_gpio) {
-		dev_err(&pdev->dev, "Could not allocate tps65910_gpio\n");
+	if (!tps65910_gpio)
 		return -ENOMEM;
-	}
 
 	tps65910_gpio->tps65910 = tps65910;
 
diff --git a/drivers/gpio/gpio-xilinx.c b/drivers/gpio/gpio-xilinx.c
index 792a05ad464..12481867daf 100644
--- a/drivers/gpio/gpio-xilinx.c
+++ b/drivers/gpio/gpio-xilinx.c
@@ -289,7 +289,7 @@ static int xgpio_of_probe(struct device_node *np)
 	return 0;
 }
 
-static struct of_device_id xgpio_of_match[] = {
+static const struct of_device_id xgpio_of_match[] = {
 	{ .compatible = "xlnx,xps-gpio-1.00.a", },
 	{ /* end of list */ },
 };
diff --git a/drivers/gpio/gpio-zevio.c b/drivers/gpio/gpio-zevio.c
index 9bf5034b6cd..54e54e4cc6c 100644
--- a/drivers/gpio/gpio-zevio.c
+++ b/drivers/gpio/gpio-zevio.c
@@ -81,9 +81,15 @@ static inline void zevio_gpio_port_set(struct zevio_gpio *c, unsigned pin,
 static int zevio_gpio_get(struct gpio_chip *chip, unsigned pin)
 {
 	struct zevio_gpio *controller = to_zevio_gpio(chip);
+	u32 val, dir;
 
-	/* Only reading allowed, so no spinlock needed */
-	u32 val = zevio_gpio_port_get(controller, pin, ZEVIO_GPIO_INPUT);
+	spin_lock(&controller->lock);
+	dir = zevio_gpio_port_get(controller, pin, ZEVIO_GPIO_DIRECTION);
+	if (dir & BIT(ZEVIO_GPIO_BIT(pin)))
+		val = zevio_gpio_port_get(controller, pin, ZEVIO_GPIO_INPUT);
+	else
+		val = zevio_gpio_port_get(controller, pin, ZEVIO_GPIO_OUTPUT);
+	spin_unlock(&controller->lock);
 
 	return (val >> ZEVIO_GPIO_BIT(pin)) & 0x1;
 }
@@ -172,10 +178,8 @@ static int zevio_gpio_probe(struct platform_device *pdev)
 	int status, i;
 
 	controller = devm_kzalloc(&pdev->dev, sizeof(*controller), GFP_KERNEL);
-	if (!controller) {
-		dev_err(&pdev->dev, "not enough free memory\n");
+	if (!controller)
 		return -ENOMEM;
-	}
 
 	/* Copy our reference */
 	controller->chip.gc = zevio_gpio_chip;
@@ -198,7 +202,7 @@ static int zevio_gpio_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id zevio_gpio_of_match[] = {
+static const struct of_device_id zevio_gpio_of_match[] = {
 	{ .compatible = "lsi,zevio-gpio", },
 	{ },
 };
@@ -209,7 +213,7 @@ static struct platform_driver zevio_gpio_driver = {
 	.driver		= {
 		.name	= "gpio-zevio",
 		.owner	= THIS_MODULE,
-		.of_match_table = of_match_ptr(zevio_gpio_of_match),
+		.of_match_table = zevio_gpio_of_match,
 	},
 	.probe		= zevio_gpio_probe,
 };
diff --git a/drivers/gpio/gpiolib-acpi.c b/drivers/gpio/gpiolib-acpi.c
index bf0f8b47669..4a987917c18 100644
--- a/drivers/gpio/gpiolib-acpi.c
+++ b/drivers/gpio/gpiolib-acpi.c
@@ -233,7 +233,7 @@ static void acpi_gpiochip_request_interrupts(struct acpi_gpio_chip *acpi_gpio)
 {
 	struct gpio_chip *chip = acpi_gpio->chip;
 
-	if (!chip->dev || !chip->to_irq)
+	if (!chip->to_irq)
 		return;
 
 	INIT_LIST_HEAD(&acpi_gpio->events);
@@ -253,7 +253,7 @@ static void acpi_gpiochip_free_interrupts(struct acpi_gpio_chip *acpi_gpio)
 	struct acpi_gpio_event *event, *ep;
 	struct gpio_chip *chip = acpi_gpio->chip;
 
-	if (!chip->dev || !chip->to_irq)
+	if (!chip->to_irq)
 		return;
 
 	list_for_each_entry_safe_reverse(event, ep, &acpi_gpio->events, node) {
@@ -449,9 +449,10 @@ acpi_gpio_adr_space_handler(u32 function, acpi_physical_address address,
 		mutex_unlock(&achip->conn_lock);
 
 		if (function == ACPI_WRITE)
-			gpiod_set_raw_value(desc, !!((1 << i) & *value));
+			gpiod_set_raw_value_cansleep(desc,
+						     !!((1 << i) & *value));
 		else
-			*value |= gpiod_get_raw_value(desc) << i;
+			*value |= (u64)gpiod_get_raw_value_cansleep(desc) << i;
 	}
 
 out:
@@ -501,6 +502,9 @@ void acpi_gpiochip_add(struct gpio_chip *chip)
 	acpi_handle handle;
 	acpi_status status;
 
+	if (!chip || !chip->dev)
+		return;
+
 	handle = ACPI_HANDLE(chip->dev);
 	if (!handle)
 		return;
@@ -531,6 +535,9 @@ void acpi_gpiochip_remove(struct gpio_chip *chip)
 	acpi_handle handle;
 	acpi_status status;
 
+	if (!chip || !chip->dev)
+		return;
+
 	handle = ACPI_HANDLE(chip->dev);
 	if (!handle)
 		return;
diff --git a/drivers/gpio/gpiolib-of.c b/drivers/gpio/gpiolib-of.c
index 2024d45e550..af7e25c9a9a 100644
--- a/drivers/gpio/gpiolib-of.c
+++ b/drivers/gpio/gpiolib-of.c
@@ -48,7 +48,7 @@ static int of_gpiochip_find_and_xlate(struct gpio_chip *gc, void *data)
 	if (ret < 0)
 		return false;
 
-	gg_data->out_gpio = gpio_to_desc(ret + gc->base);
+	gg_data->out_gpio = gpiochip_get_desc(gc, ret);
 	return true;
 }
 
@@ -96,6 +96,20 @@ struct gpio_desc *of_get_named_gpiod_flags(struct device_node *np,
 }
 EXPORT_SYMBOL(of_get_named_gpiod_flags);
 
+int of_get_named_gpio_flags(struct device_node *np, const char *list_name,
+			    int index, enum of_gpio_flags *flags)
+{
+	struct gpio_desc *desc;
+
+	desc = of_get_named_gpiod_flags(np, list_name, index, flags);
+
+	if (IS_ERR(desc))
+		return PTR_ERR(desc);
+	else
+		return desc_to_gpio(desc);
+}
+EXPORT_SYMBOL(of_get_named_gpio_flags);
+
 /**
  * of_gpio_simple_xlate - translate gpio_spec to the GPIO number and flags
  * @gc:		pointer to the gpio_chip structure
diff --git a/drivers/gpio/gpiolib.c b/drivers/gpio/gpiolib.c
index 761013f8b82..d9c9cb4665d 100644
--- a/drivers/gpio/gpiolib.c
+++ b/drivers/gpio/gpiolib.c
@@ -1363,6 +1363,11 @@ void gpiochip_set_chained_irqchip(struct gpio_chip *gpiochip,
 				  int parent_irq,
 				  irq_flow_handler_t parent_handler)
 {
+	if (gpiochip->can_sleep) {
+		chip_err(gpiochip, "you cannot have chained interrupts on a chip that may sleep\n");
+		return;
+	}
+
 	irq_set_chained_handler(parent_irq, parent_handler);
 	/*
 	 * The parent irqchip is already using the chip_data for this
@@ -1372,6 +1377,12 @@ void gpiochip_set_chained_irqchip(struct gpio_chip *gpiochip,
 }
 EXPORT_SYMBOL_GPL(gpiochip_set_chained_irqchip);
 
+/*
+ * This lock class tells lockdep that GPIO irqs are in a different
+ * category than their parents, so it won't report false recursion.
+ */
+static struct lock_class_key gpiochip_irq_lock_class;
+
 /**
  * gpiochip_irq_map() - maps an IRQ into a GPIO irqchip
  * @d: the irqdomain used by this irqchip
@@ -1387,23 +1398,36 @@ static int gpiochip_irq_map(struct irq_domain *d, unsigned int irq,
 {
 	struct gpio_chip *chip = d->host_data;
 
-	irq_set_chip_and_handler(irq, chip->irqchip, chip->irq_handler);
 	irq_set_chip_data(irq, chip);
+	irq_set_lockdep_class(irq, &gpiochip_irq_lock_class);
+	irq_set_chip_and_handler(irq, chip->irqchip, chip->irq_handler);
+	/* Chips that can sleep need nested thread handlers */
+	if (chip->can_sleep)
+		irq_set_nested_thread(irq, 1);
 #ifdef CONFIG_ARM
 	set_irq_flags(irq, IRQF_VALID);
 #else
 	irq_set_noprobe(irq);
 #endif
-	irq_set_irq_type(irq, chip->irq_default_type);
+	/*
+	 * No set-up of the hardware will happen if IRQ_TYPE_NONE
+	 * is passed as default type.
+	 */
+	if (chip->irq_default_type != IRQ_TYPE_NONE)
+		irq_set_irq_type(irq, chip->irq_default_type);
 
 	return 0;
 }
 
 static void gpiochip_irq_unmap(struct irq_domain *d, unsigned int irq)
 {
+	struct gpio_chip *chip = d->host_data;
+
 #ifdef CONFIG_ARM
 	set_irq_flags(irq, 0);
 #endif
+	if (chip->can_sleep)
+		irq_set_nested_thread(irq, 0);
 	irq_set_chip_and_handler(irq, NULL, NULL);
 	irq_set_chip_data(irq, NULL);
 }
@@ -1471,7 +1495,8 @@ static void gpiochip_irqchip_remove(struct gpio_chip *gpiochip)
  * @first_irq: if not dynamically assigned, the base (first) IRQ to
  * allocate gpiochip irqs from
  * @handler: the irq handler to use (often a predefined irq core function)
- * @type: the default type for IRQs on this irqchip
+ * @type: the default type for IRQs on this irqchip, pass IRQ_TYPE_NONE
+ * to have the core avoid setting up any default type in the hardware.
  *
  * This function closely associates a certain irqchip with a certain
  * gpiochip, providing an irq domain to translate the local IRQs to
@@ -2571,22 +2596,27 @@ void gpiod_add_lookup_table(struct gpiod_lookup_table *table)
 	mutex_unlock(&gpio_lookup_lock);
 }
 
-#ifdef CONFIG_OF
 static struct gpio_desc *of_find_gpio(struct device *dev, const char *con_id,
 				      unsigned int idx,
 				      enum gpio_lookup_flags *flags)
 {
+	static const char *suffixes[] = { "gpios", "gpio" };
 	char prop_name[32]; /* 32 is max size of property name */
 	enum of_gpio_flags of_flags;
 	struct gpio_desc *desc;
+	unsigned int i;
 
-	if (con_id)
-		snprintf(prop_name, 32, "%s-gpios", con_id);
-	else
-		snprintf(prop_name, 32, "gpios");
+	for (i = 0; i < ARRAY_SIZE(suffixes); i++) {
+		if (con_id)
+			snprintf(prop_name, 32, "%s-%s", con_id, suffixes[i]);
+		else
+			snprintf(prop_name, 32, "%s", suffixes[i]);
 
-	desc = of_get_named_gpiod_flags(dev->of_node, prop_name, idx,
-					&of_flags);
+		desc = of_get_named_gpiod_flags(dev->of_node, prop_name, idx,
+						&of_flags);
+		if (!IS_ERR(desc))
+			break;
+	}
 
 	if (IS_ERR(desc))
 		return desc;
@@ -2596,14 +2626,6 @@ static struct gpio_desc *of_find_gpio(struct device *dev, const char *con_id,
 
 	return desc;
 }
-#else
-static struct gpio_desc *of_find_gpio(struct device *dev, const char *con_id,
-				      unsigned int idx,
-				      enum gpio_lookup_flags *flags)
-{
-	return ERR_PTR(-ENODEV);
-}
-#endif
 
 static struct gpio_desc *acpi_find_gpio(struct device *dev, const char *con_id,
 					unsigned int idx,
@@ -2701,7 +2723,7 @@ static struct gpio_desc *gpiod_find(struct device *dev, const char *con_id,
 }
 
 /**
- * gpio_get - obtain a GPIO for a given GPIO function
+ * gpiod_get - obtain a GPIO for a given GPIO function
  * @dev:	GPIO consumer, can be NULL for system-global GPIOs
  * @con_id:	function within the GPIO consumer
  *
@@ -2716,6 +2738,22 @@ struct gpio_desc *__must_check gpiod_get(struct device *dev, const char *con_id)
 EXPORT_SYMBOL_GPL(gpiod_get);
 
 /**
+ * gpiod_get_optional - obtain an optional GPIO for a given GPIO function
+ * @dev: GPIO consumer, can be NULL for system-global GPIOs
+ * @con_id: function within the GPIO consumer
+ *
+ * This is equivalent to gpiod_get(), except that when no GPIO was assigned to
+ * the requested function it will return NULL. This is convenient for drivers
+ * that need to handle optional GPIOs.
+ */
+struct gpio_desc *__must_check gpiod_get_optional(struct device *dev,
+						  const char *con_id)
+{
+	return gpiod_get_index_optional(dev, con_id, 0);
+}
+EXPORT_SYMBOL_GPL(gpiod_get_optional);
+
+/**
  * gpiod_get_index - obtain a GPIO from a multi-index GPIO function
  * @dev:	GPIO consumer, can be NULL for system-global GPIOs
  * @con_id:	function within the GPIO consumer
@@ -2778,6 +2816,33 @@ struct gpio_desc *__must_check gpiod_get_index(struct device *dev,
 EXPORT_SYMBOL_GPL(gpiod_get_index);
 
 /**
+ * gpiod_get_index_optional - obtain an optional GPIO from a multi-index GPIO
+ *                            function
+ * @dev: GPIO consumer, can be NULL for system-global GPIOs
+ * @con_id: function within the GPIO consumer
+ * @index: index of the GPIO to obtain in the consumer
+ *
+ * This is equivalent to gpiod_get_index(), except that when no GPIO with the
+ * specified index was assigned to the requested function it will return NULL.
+ * This is convenient for drivers that need to handle optional GPIOs.
+ */
+struct gpio_desc *__must_check gpiod_get_index_optional(struct device *dev,
+							const char *con_id,
+							unsigned int index)
+{
+	struct gpio_desc *desc;
+
+	desc = gpiod_get_index(dev, con_id, index);
+	if (IS_ERR(desc)) {
+		if (PTR_ERR(desc) == -ENOENT)
+			return NULL;
+	}
+
+	return desc;
+}
+EXPORT_SYMBOL_GPL(gpiod_get_index_optional);
+
+/**
  * gpiod_put - dispose of a GPIO descriptor
  * @desc:	GPIO descriptor to dispose of
  *
diff --git a/drivers/gpio/gpiolib.h b/drivers/gpio/gpiolib.h
index cf092941a9f..1a4103dd38d 100644
--- a/drivers/gpio/gpiolib.h
+++ b/drivers/gpio/gpiolib.h
@@ -15,6 +15,8 @@
 #include <linux/err.h>
 #include <linux/device.h>
 
+enum of_gpio_flags;
+
 /**
  * struct acpi_gpio_info - ACPI GPIO specific information
  * @gpioint: if %true this GPIO is of type GpioInt otherwise type is GpioIo
@@ -46,4 +48,7 @@ acpi_get_gpiod_by_index(struct device *dev, int index,
 int gpiochip_request_own_desc(struct gpio_desc *desc, const char *label);
 void gpiochip_free_own_desc(struct gpio_desc *desc);
 
+struct gpio_desc *of_get_named_gpiod_flags(struct device_node *np,
+		   const char *list_name, int index, enum of_gpio_flags *flags);
+
 #endif /* GPIOLIB_H */
diff --git a/drivers/gpu/drm/Makefile b/drivers/gpu/drm/Makefile
index 9d25dbbe677..48e38ba2278 100644
--- a/drivers/gpu/drm/Makefile
+++ b/drivers/gpu/drm/Makefile
@@ -23,7 +23,7 @@ drm-$(CONFIG_DRM_PANEL) += drm_panel.o
 
 drm-usb-y   := drm_usb.o
 
-drm_kms_helper-y := drm_crtc_helper.o drm_dp_helper.o
+drm_kms_helper-y := drm_crtc_helper.o drm_dp_helper.o drm_probe_helper.o
 drm_kms_helper-$(CONFIG_DRM_LOAD_EDID_FIRMWARE) += drm_edid_load.o
 drm_kms_helper-$(CONFIG_DRM_KMS_FB_HELPER) += drm_fb_helper.o
 drm_kms_helper-$(CONFIG_DRM_KMS_CMA_HELPER) += drm_fb_cma_helper.o
diff --git a/drivers/gpu/drm/ast/ast_post.c b/drivers/gpu/drm/ast/ast_post.c
index 977cfb35837..635f6ffc27c 100644
--- a/drivers/gpu/drm/ast/ast_post.c
+++ b/drivers/gpu/drm/ast/ast_post.c
@@ -572,7 +572,7 @@ static u32 cbr_scan2(struct ast_private *ast)
 		for (loop = 0; loop < CBR_PASSNUM2; loop++) {
 			if ((data = cbr_test2(ast)) != 0) {
 				data2 &= data;
-				if (!data)
+				if (!data2)
 					return 0;
 				break;
 			}
diff --git a/drivers/gpu/drm/bochs/bochs.h b/drivers/gpu/drm/bochs/bochs.h
index 741965c001a..7eb52dd44b0 100644
--- a/drivers/gpu/drm/bochs/bochs.h
+++ b/drivers/gpu/drm/bochs/bochs.h
@@ -1,5 +1,6 @@
 #include <linux/io.h>
 #include <linux/fb.h>
+#include <linux/console.h>
 
 #include <drm/drmP.h>
 #include <drm/drm_crtc.h>
@@ -87,8 +88,6 @@ struct bochs_device {
 		struct bochs_framebuffer gfb;
 		struct drm_fb_helper helper;
 		int size;
-		int x1, y1, x2, y2; /* dirty rect */
-		spinlock_t dirty_lock;
 		bool initialized;
 	} fb;
 };
diff --git a/drivers/gpu/drm/bochs/bochs_drv.c b/drivers/gpu/drm/bochs/bochs_drv.c
index 395bba261c9..9c13df29fd2 100644
--- a/drivers/gpu/drm/bochs/bochs_drv.c
+++ b/drivers/gpu/drm/bochs/bochs_drv.c
@@ -95,6 +95,49 @@ static struct drm_driver bochs_driver = {
 };
 
 /* ---------------------------------------------------------------------- */
+/* pm interface                                                           */
+
+static int bochs_pm_suspend(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct drm_device *drm_dev = pci_get_drvdata(pdev);
+	struct bochs_device *bochs = drm_dev->dev_private;
+
+	drm_kms_helper_poll_disable(drm_dev);
+
+	if (bochs->fb.initialized) {
+		console_lock();
+		fb_set_suspend(bochs->fb.helper.fbdev, 1);
+		console_unlock();
+	}
+
+	return 0;
+}
+
+static int bochs_pm_resume(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct drm_device *drm_dev = pci_get_drvdata(pdev);
+	struct bochs_device *bochs = drm_dev->dev_private;
+
+	drm_helper_resume_force_mode(drm_dev);
+
+	if (bochs->fb.initialized) {
+		console_lock();
+		fb_set_suspend(bochs->fb.helper.fbdev, 0);
+		console_unlock();
+	}
+
+	drm_kms_helper_poll_enable(drm_dev);
+	return 0;
+}
+
+static const struct dev_pm_ops bochs_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(bochs_pm_suspend,
+				bochs_pm_resume)
+};
+
+/* ---------------------------------------------------------------------- */
 /* pci interface                                                          */
 
 static int bochs_kick_out_firmware_fb(struct pci_dev *pdev)
@@ -155,6 +198,7 @@ static struct pci_driver bochs_pci_driver = {
 	.id_table =	bochs_pci_tbl,
 	.probe =	bochs_pci_probe,
 	.remove =	bochs_pci_remove,
+	.driver.pm =    &bochs_pm_ops,
 };
 
 /* ---------------------------------------------------------------------- */
diff --git a/drivers/gpu/drm/bochs/bochs_fbdev.c b/drivers/gpu/drm/bochs/bochs_fbdev.c
index 4da5206b7cc..561b8447412 100644
--- a/drivers/gpu/drm/bochs/bochs_fbdev.c
+++ b/drivers/gpu/drm/bochs/bochs_fbdev.c
@@ -190,7 +190,6 @@ int bochs_fbdev_init(struct bochs_device *bochs)
 	int ret;
 
 	bochs->fb.helper.funcs = &bochs_fb_helper_funcs;
-	spin_lock_init(&bochs->fb.dirty_lock);
 
 	ret = drm_fb_helper_init(bochs->dev, &bochs->fb.helper,
 				 1, 1);
diff --git a/drivers/gpu/drm/cirrus/cirrus_drv.c b/drivers/gpu/drm/cirrus/cirrus_drv.c
index 953fc8aea69..08ce520f61a 100644
--- a/drivers/gpu/drm/cirrus/cirrus_drv.c
+++ b/drivers/gpu/drm/cirrus/cirrus_drv.c
@@ -11,6 +11,7 @@
 #include <linux/module.h>
 #include <linux/console.h>
 #include <drm/drmP.h>
+#include <drm/drm_crtc_helper.h>
 
 #include "cirrus_drv.h"
 
@@ -75,6 +76,41 @@ static void cirrus_pci_remove(struct pci_dev *pdev)
 	drm_put_dev(dev);
 }
 
+static int cirrus_pm_suspend(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct drm_device *drm_dev = pci_get_drvdata(pdev);
+	struct cirrus_device *cdev = drm_dev->dev_private;
+
+	drm_kms_helper_poll_disable(drm_dev);
+
+	if (cdev->mode_info.gfbdev) {
+		console_lock();
+		fb_set_suspend(cdev->mode_info.gfbdev->helper.fbdev, 1);
+		console_unlock();
+	}
+
+	return 0;
+}
+
+static int cirrus_pm_resume(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct drm_device *drm_dev = pci_get_drvdata(pdev);
+	struct cirrus_device *cdev = drm_dev->dev_private;
+
+	drm_helper_resume_force_mode(drm_dev);
+
+	if (cdev->mode_info.gfbdev) {
+		console_lock();
+		fb_set_suspend(cdev->mode_info.gfbdev->helper.fbdev, 0);
+		console_unlock();
+	}
+
+	drm_kms_helper_poll_enable(drm_dev);
+	return 0;
+}
+
 static const struct file_operations cirrus_driver_fops = {
 	.owner = THIS_MODULE,
 	.open = drm_open,
@@ -103,11 +139,17 @@ static struct drm_driver driver = {
 	.dumb_destroy = drm_gem_dumb_destroy,
 };
 
+static const struct dev_pm_ops cirrus_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(cirrus_pm_suspend,
+				cirrus_pm_resume)
+};
+
 static struct pci_driver cirrus_pci_driver = {
 	.name = DRIVER_NAME,
 	.id_table = pciidlist,
 	.probe = cirrus_pci_probe,
 	.remove = cirrus_pci_remove,
+	.driver.pm = &cirrus_pm_ops,
 };
 
 static int __init cirrus_init(void)
diff --git a/drivers/gpu/drm/cirrus/cirrus_mode.c b/drivers/gpu/drm/cirrus/cirrus_mode.c
index 2d64aea83df..f59433b7610 100644
--- a/drivers/gpu/drm/cirrus/cirrus_mode.c
+++ b/drivers/gpu/drm/cirrus/cirrus_mode.c
@@ -308,6 +308,9 @@ static int cirrus_crtc_mode_set(struct drm_crtc *crtc,
 
 	WREG_HDR(hdr);
 	cirrus_crtc_do_set_base(crtc, old_fb, x, y, 0);
+
+	/* Unblank (needed on S3 resume, vgabios doesn't do it then) */
+	outb(0x20, 0x3c0);
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/drm_crtc_helper.c b/drivers/gpu/drm/drm_crtc_helper.c
index c43825e8f5c..df281b54db0 100644
--- a/drivers/gpu/drm/drm_crtc_helper.c
+++ b/drivers/gpu/drm/drm_crtc_helper.c
@@ -72,147 +72,6 @@ void drm_helper_move_panel_connectors_to_head(struct drm_device *dev)
 }
 EXPORT_SYMBOL(drm_helper_move_panel_connectors_to_head);
 
-static bool drm_kms_helper_poll = true;
-module_param_named(poll, drm_kms_helper_poll, bool, 0600);
-
-static void drm_mode_validate_flag(struct drm_connector *connector,
-				   int flags)
-{
-	struct drm_display_mode *mode;
-
-	if (flags == (DRM_MODE_FLAG_DBLSCAN | DRM_MODE_FLAG_INTERLACE |
-		      DRM_MODE_FLAG_3D_MASK))
-		return;
-
-	list_for_each_entry(mode, &connector->modes, head) {
-		if ((mode->flags & DRM_MODE_FLAG_INTERLACE) &&
-				!(flags & DRM_MODE_FLAG_INTERLACE))
-			mode->status = MODE_NO_INTERLACE;
-		if ((mode->flags & DRM_MODE_FLAG_DBLSCAN) &&
-				!(flags & DRM_MODE_FLAG_DBLSCAN))
-			mode->status = MODE_NO_DBLESCAN;
-		if ((mode->flags & DRM_MODE_FLAG_3D_MASK) &&
-				!(flags & DRM_MODE_FLAG_3D_MASK))
-			mode->status = MODE_NO_STEREO;
-	}
-
-	return;
-}
-
-/**
- * drm_helper_probe_single_connector_modes - get complete set of display modes
- * @connector: connector to probe
- * @maxX: max width for modes
- * @maxY: max height for modes
- *
- * Based on the helper callbacks implemented by @connector try to detect all
- * valid modes.  Modes will first be added to the connector's probed_modes list,
- * then culled (based on validity and the @maxX, @maxY parameters) and put into
- * the normal modes list.
- *
- * Intended to be use as a generic implementation of the ->fill_modes()
- * @connector vfunc for drivers that use the crtc helpers for output mode
- * filtering and detection.
- *
- * Returns:
- * The number of modes found on @connector.
- */
-int drm_helper_probe_single_connector_modes(struct drm_connector *connector,
-					    uint32_t maxX, uint32_t maxY)
-{
-	struct drm_device *dev = connector->dev;
-	struct drm_display_mode *mode;
-	struct drm_connector_helper_funcs *connector_funcs =
-		connector->helper_private;
-	int count = 0;
-	int mode_flags = 0;
-	bool verbose_prune = true;
-
-	WARN_ON(!mutex_is_locked(&dev->mode_config.mutex));
-
-	DRM_DEBUG_KMS("[CONNECTOR:%d:%s]\n", connector->base.id,
-			drm_get_connector_name(connector));
-	/* set all modes to the unverified state */
-	list_for_each_entry(mode, &connector->modes, head)
-		mode->status = MODE_UNVERIFIED;
-
-	if (connector->force) {
-		if (connector->force == DRM_FORCE_ON)
-			connector->status = connector_status_connected;
-		else
-			connector->status = connector_status_disconnected;
-		if (connector->funcs->force)
-			connector->funcs->force(connector);
-	} else {
-		connector->status = connector->funcs->detect(connector, true);
-	}
-
-	/* Re-enable polling in case the global poll config changed. */
-	if (drm_kms_helper_poll != dev->mode_config.poll_running)
-		drm_kms_helper_poll_enable(dev);
-
-	dev->mode_config.poll_running = drm_kms_helper_poll;
-
-	if (connector->status == connector_status_disconnected) {
-		DRM_DEBUG_KMS("[CONNECTOR:%d:%s] disconnected\n",
-			connector->base.id, drm_get_connector_name(connector));
-		drm_mode_connector_update_edid_property(connector, NULL);
-		verbose_prune = false;
-		goto prune;
-	}
-
-#ifdef CONFIG_DRM_LOAD_EDID_FIRMWARE
-	count = drm_load_edid_firmware(connector);
-	if (count == 0)
-#endif
-		count = (*connector_funcs->get_modes)(connector);
-
-	if (count == 0 && connector->status == connector_status_connected)
-		count = drm_add_modes_noedid(connector, 1024, 768);
-	if (count == 0)
-		goto prune;
-
-	drm_mode_connector_list_update(connector);
-
-	if (maxX && maxY)
-		drm_mode_validate_size(dev, &connector->modes, maxX, maxY);
-
-	if (connector->interlace_allowed)
-		mode_flags |= DRM_MODE_FLAG_INTERLACE;
-	if (connector->doublescan_allowed)
-		mode_flags |= DRM_MODE_FLAG_DBLSCAN;
-	if (connector->stereo_allowed)
-		mode_flags |= DRM_MODE_FLAG_3D_MASK;
-	drm_mode_validate_flag(connector, mode_flags);
-
-	list_for_each_entry(mode, &connector->modes, head) {
-		if (mode->status == MODE_OK)
-			mode->status = connector_funcs->mode_valid(connector,
-								   mode);
-	}
-
-prune:
-	drm_mode_prune_invalid(dev, &connector->modes, verbose_prune);
-
-	if (list_empty(&connector->modes))
-		return 0;
-
-	list_for_each_entry(mode, &connector->modes, head)
-		mode->vrefresh = drm_mode_vrefresh(mode);
-
-	drm_mode_sort(&connector->modes);
-
-	DRM_DEBUG_KMS("[CONNECTOR:%d:%s] probed modes :\n", connector->base.id,
-			drm_get_connector_name(connector));
-	list_for_each_entry(mode, &connector->modes, head) {
-		drm_mode_set_crtcinfo(mode, CRTC_INTERLACE_HALVE_V);
-		drm_mode_debug_printmodeline(mode);
-	}
-
-	return count;
-}
-EXPORT_SYMBOL(drm_helper_probe_single_connector_modes);
-
 /**
  * drm_helper_encoder_in_use - check if a given encoder is in use
  * @encoder: encoder to check
@@ -1020,232 +879,3 @@ void drm_helper_resume_force_mode(struct drm_device *dev)
 	drm_modeset_unlock_all(dev);
 }
 EXPORT_SYMBOL(drm_helper_resume_force_mode);
-
-/**
- * drm_kms_helper_hotplug_event - fire off KMS hotplug events
- * @dev: drm_device whose connector state changed
- *
- * This function fires off the uevent for userspace and also calls the
- * output_poll_changed function, which is most commonly used to inform the fbdev
- * emulation code and allow it to update the fbcon output configuration.
- *
- * Drivers should call this from their hotplug handling code when a change is
- * detected. Note that this function does not do any output detection of its
- * own, like drm_helper_hpd_irq_event() does - this is assumed to be done by the
- * driver already.
- *
- * This function must be called from process context with no mode
- * setting locks held.
- */
-void drm_kms_helper_hotplug_event(struct drm_device *dev)
-{
-	/* send a uevent + call fbdev */
-	drm_sysfs_hotplug_event(dev);
-	if (dev->mode_config.funcs->output_poll_changed)
-		dev->mode_config.funcs->output_poll_changed(dev);
-}
-EXPORT_SYMBOL(drm_kms_helper_hotplug_event);
-
-#define DRM_OUTPUT_POLL_PERIOD (10*HZ)
-static void output_poll_execute(struct work_struct *work)
-{
-	struct delayed_work *delayed_work = to_delayed_work(work);
-	struct drm_device *dev = container_of(delayed_work, struct drm_device, mode_config.output_poll_work);
-	struct drm_connector *connector;
-	enum drm_connector_status old_status;
-	bool repoll = false, changed = false;
-
-	if (!drm_kms_helper_poll)
-		return;
-
-	mutex_lock(&dev->mode_config.mutex);
-	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
-
-		/* Ignore forced connectors. */
-		if (connector->force)
-			continue;
-
-		/* Ignore HPD capable connectors and connectors where we don't
-		 * want any hotplug detection at all for polling. */
-		if (!connector->polled || connector->polled == DRM_CONNECTOR_POLL_HPD)
-			continue;
-
-		repoll = true;
-
-		old_status = connector->status;
-		/* if we are connected and don't want to poll for disconnect
-		   skip it */
-		if (old_status == connector_status_connected &&
-		    !(connector->polled & DRM_CONNECTOR_POLL_DISCONNECT))
-			continue;
-
-		connector->status = connector->funcs->detect(connector, false);
-		if (old_status != connector->status) {
-			const char *old, *new;
-
-			old = drm_get_connector_status_name(old_status);
-			new = drm_get_connector_status_name(connector->status);
-
-			DRM_DEBUG_KMS("[CONNECTOR:%d:%s] "
-				      "status updated from %s to %s\n",
-				      connector->base.id,
-				      drm_get_connector_name(connector),
-				      old, new);
-
-			changed = true;
-		}
-	}
-
-	mutex_unlock(&dev->mode_config.mutex);
-
-	if (changed)
-		drm_kms_helper_hotplug_event(dev);
-
-	if (repoll)
-		schedule_delayed_work(delayed_work, DRM_OUTPUT_POLL_PERIOD);
-}
-
-/**
- * drm_kms_helper_poll_disable - disable output polling
- * @dev: drm_device
- *
- * This function disables the output polling work.
- *
- * Drivers can call this helper from their device suspend implementation. It is
- * not an error to call this even when output polling isn't enabled or arlready
- * disabled.
- */
-void drm_kms_helper_poll_disable(struct drm_device *dev)
-{
-	if (!dev->mode_config.poll_enabled)
-		return;
-	cancel_delayed_work_sync(&dev->mode_config.output_poll_work);
-}
-EXPORT_SYMBOL(drm_kms_helper_poll_disable);
-
-/**
- * drm_kms_helper_poll_enable - re-enable output polling.
- * @dev: drm_device
- *
- * This function re-enables the output polling work.
- *
- * Drivers can call this helper from their device resume implementation. It is
- * an error to call this when the output polling support has not yet been set
- * up.
- */
-void drm_kms_helper_poll_enable(struct drm_device *dev)
-{
-	bool poll = false;
-	struct drm_connector *connector;
-
-	if (!dev->mode_config.poll_enabled || !drm_kms_helper_poll)
-		return;
-
-	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
-		if (connector->polled & (DRM_CONNECTOR_POLL_CONNECT |
-					 DRM_CONNECTOR_POLL_DISCONNECT))
-			poll = true;
-	}
-
-	if (poll)
-		schedule_delayed_work(&dev->mode_config.output_poll_work, DRM_OUTPUT_POLL_PERIOD);
-}
-EXPORT_SYMBOL(drm_kms_helper_poll_enable);
-
-/**
- * drm_kms_helper_poll_init - initialize and enable output polling
- * @dev: drm_device
- *
- * This function intializes and then also enables output polling support for
- * @dev. Drivers which do not have reliable hotplug support in hardware can use
- * this helper infrastructure to regularly poll such connectors for changes in
- * their connection state.
- *
- * Drivers can control which connectors are polled by setting the
- * DRM_CONNECTOR_POLL_CONNECT and DRM_CONNECTOR_POLL_DISCONNECT flags. On
- * connectors where probing live outputs can result in visual distortion drivers
- * should not set the DRM_CONNECTOR_POLL_DISCONNECT flag to avoid this.
- * Connectors which have no flag or only DRM_CONNECTOR_POLL_HPD set are
- * completely ignored by the polling logic.
- *
- * Note that a connector can be both polled and probed from the hotplug handler,
- * in case the hotplug interrupt is known to be unreliable.
- */
-void drm_kms_helper_poll_init(struct drm_device *dev)
-{
-	INIT_DELAYED_WORK(&dev->mode_config.output_poll_work, output_poll_execute);
-	dev->mode_config.poll_enabled = true;
-
-	drm_kms_helper_poll_enable(dev);
-}
-EXPORT_SYMBOL(drm_kms_helper_poll_init);
-
-/**
- * drm_kms_helper_poll_fini - disable output polling and clean it up
- * @dev: drm_device
- */
-void drm_kms_helper_poll_fini(struct drm_device *dev)
-{
-	drm_kms_helper_poll_disable(dev);
-}
-EXPORT_SYMBOL(drm_kms_helper_poll_fini);
-
-/**
- * drm_helper_hpd_irq_event - hotplug processing
- * @dev: drm_device
- *
- * Drivers can use this helper function to run a detect cycle on all connectors
- * which have the DRM_CONNECTOR_POLL_HPD flag set in their &polled member. All
- * other connectors are ignored, which is useful to avoid reprobing fixed
- * panels.
- *
- * This helper function is useful for drivers which can't or don't track hotplug
- * interrupts for each connector.
- *
- * Drivers which support hotplug interrupts for each connector individually and
- * which have a more fine-grained detect logic should bypass this code and
- * directly call drm_kms_helper_hotplug_event() in case the connector state
- * changed.
- *
- * This function must be called from process context with no mode
- * setting locks held.
- *
- * Note that a connector can be both polled and probed from the hotplug handler,
- * in case the hotplug interrupt is known to be unreliable.
- */
-bool drm_helper_hpd_irq_event(struct drm_device *dev)
-{
-	struct drm_connector *connector;
-	enum drm_connector_status old_status;
-	bool changed = false;
-
-	if (!dev->mode_config.poll_enabled)
-		return false;
-
-	mutex_lock(&dev->mode_config.mutex);
-	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
-
-		/* Only handle HPD capable connectors. */
-		if (!(connector->polled & DRM_CONNECTOR_POLL_HPD))
-			continue;
-
-		old_status = connector->status;
-
-		connector->status = connector->funcs->detect(connector, false);
-		DRM_DEBUG_KMS("[CONNECTOR:%d:%s] status updated from %s to %s\n",
-			      connector->base.id,
-			      drm_get_connector_name(connector),
-			      drm_get_connector_status_name(old_status),
-			      drm_get_connector_status_name(connector->status));
-		if (old_status != connector->status)
-			changed = true;
-	}
-
-	mutex_unlock(&dev->mode_config.mutex);
-
-	if (changed)
-		drm_kms_helper_hotplug_event(dev);
-
-	return changed;
-}
-EXPORT_SYMBOL(drm_helper_hpd_irq_event);
diff --git a/drivers/gpu/drm/drm_dp_helper.c b/drivers/gpu/drm/drm_dp_helper.c
index 27671489477..4b6e6f3ba0a 100644
--- a/drivers/gpu/drm/drm_dp_helper.c
+++ b/drivers/gpu/drm/drm_dp_helper.c
@@ -577,7 +577,9 @@ static u32 drm_dp_i2c_functionality(struct i2c_adapter *adapter)
 
 /*
  * Transfer a single I2C-over-AUX message and handle various error conditions,
- * retrying the transaction as appropriate.
+ * retrying the transaction as appropriate.  It is assumed that the
+ * aux->transfer function does not modify anything in the msg other than the
+ * reply field.
  */
 static int drm_dp_i2c_do_msg(struct drm_dp_aux *aux, struct drm_dp_aux_msg *msg)
 {
@@ -665,11 +667,26 @@ static int drm_dp_i2c_xfer(struct i2c_adapter *adapter, struct i2c_msg *msgs,
 {
 	struct drm_dp_aux *aux = adapter->algo_data;
 	unsigned int i, j;
+	struct drm_dp_aux_msg msg;
+	int err = 0;
 
-	for (i = 0; i < num; i++) {
-		struct drm_dp_aux_msg msg;
-		int err;
+	memset(&msg, 0, sizeof(msg));
 
+	for (i = 0; i < num; i++) {
+		msg.address = msgs[i].addr;
+		msg.request = (msgs[i].flags & I2C_M_RD) ?
+			DP_AUX_I2C_READ :
+			DP_AUX_I2C_WRITE;
+		msg.request |= DP_AUX_I2C_MOT;
+		/* Send a bare address packet to start the transaction.
+		 * Zero sized messages specify an address only (bare
+		 * address) transaction.
+		 */
+		msg.buffer = NULL;
+		msg.size = 0;
+		err = drm_dp_i2c_do_msg(aux, &msg);
+		if (err < 0)
+			break;
 		/*
 		 * Many hardware implementations support FIFOs larger than a
 		 * single byte, but it has been empirically determined that
@@ -678,30 +695,28 @@ static int drm_dp_i2c_xfer(struct i2c_adapter *adapter, struct i2c_msg *msgs,
 		 * transferred byte-by-byte.
 		 */
 		for (j = 0; j < msgs[i].len; j++) {
-			memset(&msg, 0, sizeof(msg));
-			msg.address = msgs[i].addr;
-
-			msg.request = (msgs[i].flags & I2C_M_RD) ?
-					DP_AUX_I2C_READ :
-					DP_AUX_I2C_WRITE;
-
-			/*
-			 * All messages except the last one are middle-of-
-			 * transfer messages.
-			 */
-			if ((i < num - 1) || (j < msgs[i].len - 1))
-				msg.request |= DP_AUX_I2C_MOT;
-
 			msg.buffer = msgs[i].buf + j;
 			msg.size = 1;
 
 			err = drm_dp_i2c_do_msg(aux, &msg);
 			if (err < 0)
-				return err;
+				break;
 		}
+		if (err < 0)
+			break;
 	}
+	if (err >= 0)
+		err = num;
+	/* Send a bare address packet to close out the transaction.
+	 * Zero sized messages specify an address only (bare
+	 * address) transaction.
+	 */
+	msg.request &= ~DP_AUX_I2C_MOT;
+	msg.buffer = NULL;
+	msg.size = 0;
+	(void)drm_dp_i2c_do_msg(aux, &msg);
 
-	return num;
+	return err;
 }
 
 static const struct i2c_algorithm drm_dp_i2c_algo = {
diff --git a/drivers/gpu/drm/drm_mm.c b/drivers/gpu/drm/drm_mm.c
index 71e2d3fcd6e..04a209e2b66 100644
--- a/drivers/gpu/drm/drm_mm.c
+++ b/drivers/gpu/drm/drm_mm.c
@@ -207,8 +207,6 @@ int drm_mm_reserve_node(struct drm_mm *mm, struct drm_mm_node *node)
 		return 0;
 	}
 
-	WARN(1, "no hole found for node 0x%lx + 0x%lx\n",
-	     node->start, node->size);
 	return -ENOSPC;
 }
 EXPORT_SYMBOL(drm_mm_reserve_node);
diff --git a/drivers/gpu/drm/drm_plane_helper.c b/drivers/gpu/drm/drm_plane_helper.c
index e768d35ff22..d2b1c03b3d7 100644
--- a/drivers/gpu/drm/drm_plane_helper.c
+++ b/drivers/gpu/drm/drm_plane_helper.c
@@ -203,9 +203,9 @@ EXPORT_SYMBOL(drm_primary_helper_update);
  *
  * Provides a default plane disable handler for primary planes.  This is handler
  * is called in response to a userspace SetPlane operation on the plane with a
- * NULL framebuffer parameter.  We call the driver's modeset handler with a NULL
- * framebuffer to disable the CRTC if no other planes are currently enabled.
- * If other planes are still enabled on the same CRTC, we return -EBUSY.
+ * NULL framebuffer parameter.  It unconditionally fails the disable call with
+ * -EINVAL the only way to disable the primary plane without driver support is
+ * to disable the entier CRTC. Which does not match the plane ->disable hook.
  *
  * Note that some hardware may be able to disable the primary plane without
  * disabling the whole CRTC.  Drivers for such hardware should provide their
@@ -214,34 +214,11 @@ EXPORT_SYMBOL(drm_primary_helper_update);
  * disabled primary plane).
  *
  * RETURNS:
- * Zero on success, error code on failure
+ * Unconditionally returns -EINVAL.
  */
 int drm_primary_helper_disable(struct drm_plane *plane)
 {
-	struct drm_plane *p;
-	struct drm_mode_set set = {
-		.crtc = plane->crtc,
-		.fb = NULL,
-	};
-
-	if (plane->crtc == NULL || plane->fb == NULL)
-		/* Already disabled */
-		return 0;
-
-	list_for_each_entry(p, &plane->dev->mode_config.plane_list, head)
-		if (p != plane && p->fb) {
-			DRM_DEBUG_KMS("Cannot disable primary plane while other planes are still active on CRTC.\n");
-			return -EBUSY;
-		}
-
-	/*
-	 * N.B.  We call set_config() directly here rather than
-	 * drm_mode_set_config_internal() since drm_mode_setplane() already
-	 * handles the basic refcounting and we don't need the special
-	 * cross-CRTC refcounting (no chance of stealing connectors from
-	 * other CRTC's with this update).
-	 */
-	return plane->crtc->funcs->set_config(&set);
+	return -EINVAL;
 }
 EXPORT_SYMBOL(drm_primary_helper_disable);
 
diff --git a/drivers/gpu/drm/drm_probe_helper.c b/drivers/gpu/drm/drm_probe_helper.c
new file mode 100644
index 00000000000..e70f54d4a58
--- /dev/null
+++ b/drivers/gpu/drm/drm_probe_helper.c
@@ -0,0 +1,426 @@
+/*
+ * Copyright (c) 2006-2008 Intel Corporation
+ * Copyright (c) 2007 Dave Airlie <airlied@linux.ie>
+ *
+ * DRM core CRTC related functions
+ *
+ * Permission to use, copy, modify, distribute, and sell this software and its
+ * documentation for any purpose is hereby granted without fee, provided that
+ * the above copyright notice appear in all copies and that both that copyright
+ * notice and this permission notice appear in supporting documentation, and
+ * that the name of the copyright holders not be used in advertising or
+ * publicity pertaining to distribution of the software without specific,
+ * written prior permission.  The copyright holders make no representations
+ * about the suitability of this software for any purpose.  It is provided "as
+ * is" without express or implied warranty.
+ *
+ * THE COPYRIGHT HOLDERS DISCLAIM ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
+ * INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO
+ * EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE FOR ANY SPECIAL, INDIRECT OR
+ * CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE,
+ * DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER
+ * TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
+ * OF THIS SOFTWARE.
+ *
+ * Authors:
+ *      Keith Packard
+ *	Eric Anholt <eric@anholt.net>
+ *      Dave Airlie <airlied@linux.ie>
+ *      Jesse Barnes <jesse.barnes@intel.com>
+ */
+
+#include <linux/export.h>
+#include <linux/moduleparam.h>
+
+#include <drm/drmP.h>
+#include <drm/drm_crtc.h>
+#include <drm/drm_fourcc.h>
+#include <drm/drm_crtc_helper.h>
+#include <drm/drm_fb_helper.h>
+#include <drm/drm_edid.h>
+
+/**
+ * DOC: output probing helper overview
+ *
+ * This library provides some helper code for output probing. It provides an
+ * implementation of the core connector->fill_modes interface with
+ * drm_helper_probe_single_connector_modes.
+ *
+ * It also provides support for polling connectors with a work item and for
+ * generic hotplug interrupt handling where the driver doesn't or cannot keep
+ * track of a per-connector hpd interrupt.
+ *
+ * This helper library can be used independently of the modeset helper library.
+ * Drivers can also overwrite different parts e.g. use their own hotplug
+ * handling code to avoid probing unrelated outputs.
+ */
+
+static bool drm_kms_helper_poll = true;
+module_param_named(poll, drm_kms_helper_poll, bool, 0600);
+
+static void drm_mode_validate_flag(struct drm_connector *connector,
+				   int flags)
+{
+	struct drm_display_mode *mode;
+
+	if (flags == (DRM_MODE_FLAG_DBLSCAN | DRM_MODE_FLAG_INTERLACE |
+		      DRM_MODE_FLAG_3D_MASK))
+		return;
+
+	list_for_each_entry(mode, &connector->modes, head) {
+		if ((mode->flags & DRM_MODE_FLAG_INTERLACE) &&
+				!(flags & DRM_MODE_FLAG_INTERLACE))
+			mode->status = MODE_NO_INTERLACE;
+		if ((mode->flags & DRM_MODE_FLAG_DBLSCAN) &&
+				!(flags & DRM_MODE_FLAG_DBLSCAN))
+			mode->status = MODE_NO_DBLESCAN;
+		if ((mode->flags & DRM_MODE_FLAG_3D_MASK) &&
+				!(flags & DRM_MODE_FLAG_3D_MASK))
+			mode->status = MODE_NO_STEREO;
+	}
+
+	return;
+}
+
+/**
+ * drm_helper_probe_single_connector_modes - get complete set of display modes
+ * @connector: connector to probe
+ * @maxX: max width for modes
+ * @maxY: max height for modes
+ *
+ * Based on the helper callbacks implemented by @connector try to detect all
+ * valid modes.  Modes will first be added to the connector's probed_modes list,
+ * then culled (based on validity and the @maxX, @maxY parameters) and put into
+ * the normal modes list.
+ *
+ * Intended to be use as a generic implementation of the ->fill_modes()
+ * @connector vfunc for drivers that use the crtc helpers for output mode
+ * filtering and detection.
+ *
+ * Returns:
+ * The number of modes found on @connector.
+ */
+int drm_helper_probe_single_connector_modes(struct drm_connector *connector,
+					    uint32_t maxX, uint32_t maxY)
+{
+	struct drm_device *dev = connector->dev;
+	struct drm_display_mode *mode;
+	struct drm_connector_helper_funcs *connector_funcs =
+		connector->helper_private;
+	int count = 0;
+	int mode_flags = 0;
+	bool verbose_prune = true;
+
+	WARN_ON(!mutex_is_locked(&dev->mode_config.mutex));
+
+	DRM_DEBUG_KMS("[CONNECTOR:%d:%s]\n", connector->base.id,
+			drm_get_connector_name(connector));
+	/* set all modes to the unverified state */
+	list_for_each_entry(mode, &connector->modes, head)
+		mode->status = MODE_UNVERIFIED;
+
+	if (connector->force) {
+		if (connector->force == DRM_FORCE_ON)
+			connector->status = connector_status_connected;
+		else
+			connector->status = connector_status_disconnected;
+		if (connector->funcs->force)
+			connector->funcs->force(connector);
+	} else {
+		connector->status = connector->funcs->detect(connector, true);
+	}
+
+	/* Re-enable polling in case the global poll config changed. */
+	if (drm_kms_helper_poll != dev->mode_config.poll_running)
+		drm_kms_helper_poll_enable(dev);
+
+	dev->mode_config.poll_running = drm_kms_helper_poll;
+
+	if (connector->status == connector_status_disconnected) {
+		DRM_DEBUG_KMS("[CONNECTOR:%d:%s] disconnected\n",
+			connector->base.id, drm_get_connector_name(connector));
+		drm_mode_connector_update_edid_property(connector, NULL);
+		verbose_prune = false;
+		goto prune;
+	}
+
+#ifdef CONFIG_DRM_LOAD_EDID_FIRMWARE
+	count = drm_load_edid_firmware(connector);
+	if (count == 0)
+#endif
+		count = (*connector_funcs->get_modes)(connector);
+
+	if (count == 0 && connector->status == connector_status_connected)
+		count = drm_add_modes_noedid(connector, 1024, 768);
+	if (count == 0)
+		goto prune;
+
+	drm_mode_connector_list_update(connector);
+
+	if (maxX && maxY)
+		drm_mode_validate_size(dev, &connector->modes, maxX, maxY);
+
+	if (connector->interlace_allowed)
+		mode_flags |= DRM_MODE_FLAG_INTERLACE;
+	if (connector->doublescan_allowed)
+		mode_flags |= DRM_MODE_FLAG_DBLSCAN;
+	if (connector->stereo_allowed)
+		mode_flags |= DRM_MODE_FLAG_3D_MASK;
+	drm_mode_validate_flag(connector, mode_flags);
+
+	list_for_each_entry(mode, &connector->modes, head) {
+		if (mode->status == MODE_OK)
+			mode->status = connector_funcs->mode_valid(connector,
+								   mode);
+	}
+
+prune:
+	drm_mode_prune_invalid(dev, &connector->modes, verbose_prune);
+
+	if (list_empty(&connector->modes))
+		return 0;
+
+	list_for_each_entry(mode, &connector->modes, head)
+		mode->vrefresh = drm_mode_vrefresh(mode);
+
+	drm_mode_sort(&connector->modes);
+
+	DRM_DEBUG_KMS("[CONNECTOR:%d:%s] probed modes :\n", connector->base.id,
+			drm_get_connector_name(connector));
+	list_for_each_entry(mode, &connector->modes, head) {
+		drm_mode_set_crtcinfo(mode, CRTC_INTERLACE_HALVE_V);
+		drm_mode_debug_printmodeline(mode);
+	}
+
+	return count;
+}
+EXPORT_SYMBOL(drm_helper_probe_single_connector_modes);
+
+/**
+ * drm_kms_helper_hotplug_event - fire off KMS hotplug events
+ * @dev: drm_device whose connector state changed
+ *
+ * This function fires off the uevent for userspace and also calls the
+ * output_poll_changed function, which is most commonly used to inform the fbdev
+ * emulation code and allow it to update the fbcon output configuration.
+ *
+ * Drivers should call this from their hotplug handling code when a change is
+ * detected. Note that this function does not do any output detection of its
+ * own, like drm_helper_hpd_irq_event() does - this is assumed to be done by the
+ * driver already.
+ *
+ * This function must be called from process context with no mode
+ * setting locks held.
+ */
+void drm_kms_helper_hotplug_event(struct drm_device *dev)
+{
+	/* send a uevent + call fbdev */
+	drm_sysfs_hotplug_event(dev);
+	if (dev->mode_config.funcs->output_poll_changed)
+		dev->mode_config.funcs->output_poll_changed(dev);
+}
+EXPORT_SYMBOL(drm_kms_helper_hotplug_event);
+
+#define DRM_OUTPUT_POLL_PERIOD (10*HZ)
+static void output_poll_execute(struct work_struct *work)
+{
+	struct delayed_work *delayed_work = to_delayed_work(work);
+	struct drm_device *dev = container_of(delayed_work, struct drm_device, mode_config.output_poll_work);
+	struct drm_connector *connector;
+	enum drm_connector_status old_status;
+	bool repoll = false, changed = false;
+
+	if (!drm_kms_helper_poll)
+		return;
+
+	mutex_lock(&dev->mode_config.mutex);
+	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
+
+		/* Ignore forced connectors. */
+		if (connector->force)
+			continue;
+
+		/* Ignore HPD capable connectors and connectors where we don't
+		 * want any hotplug detection at all for polling. */
+		if (!connector->polled || connector->polled == DRM_CONNECTOR_POLL_HPD)
+			continue;
+
+		repoll = true;
+
+		old_status = connector->status;
+		/* if we are connected and don't want to poll for disconnect
+		   skip it */
+		if (old_status == connector_status_connected &&
+		    !(connector->polled & DRM_CONNECTOR_POLL_DISCONNECT))
+			continue;
+
+		connector->status = connector->funcs->detect(connector, false);
+		if (old_status != connector->status) {
+			const char *old, *new;
+
+			old = drm_get_connector_status_name(old_status);
+			new = drm_get_connector_status_name(connector->status);
+
+			DRM_DEBUG_KMS("[CONNECTOR:%d:%s] "
+				      "status updated from %s to %s\n",
+				      connector->base.id,
+				      drm_get_connector_name(connector),
+				      old, new);
+
+			changed = true;
+		}
+	}
+
+	mutex_unlock(&dev->mode_config.mutex);
+
+	if (changed)
+		drm_kms_helper_hotplug_event(dev);
+
+	if (repoll)
+		schedule_delayed_work(delayed_work, DRM_OUTPUT_POLL_PERIOD);
+}
+
+/**
+ * drm_kms_helper_poll_disable - disable output polling
+ * @dev: drm_device
+ *
+ * This function disables the output polling work.
+ *
+ * Drivers can call this helper from their device suspend implementation. It is
+ * not an error to call this even when output polling isn't enabled or arlready
+ * disabled.
+ */
+void drm_kms_helper_poll_disable(struct drm_device *dev)
+{
+	if (!dev->mode_config.poll_enabled)
+		return;
+	cancel_delayed_work_sync(&dev->mode_config.output_poll_work);
+}
+EXPORT_SYMBOL(drm_kms_helper_poll_disable);
+
+/**
+ * drm_kms_helper_poll_enable - re-enable output polling.
+ * @dev: drm_device
+ *
+ * This function re-enables the output polling work.
+ *
+ * Drivers can call this helper from their device resume implementation. It is
+ * an error to call this when the output polling support has not yet been set
+ * up.
+ */
+void drm_kms_helper_poll_enable(struct drm_device *dev)
+{
+	bool poll = false;
+	struct drm_connector *connector;
+
+	if (!dev->mode_config.poll_enabled || !drm_kms_helper_poll)
+		return;
+
+	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
+		if (connector->polled & (DRM_CONNECTOR_POLL_CONNECT |
+					 DRM_CONNECTOR_POLL_DISCONNECT))
+			poll = true;
+	}
+
+	if (poll)
+		schedule_delayed_work(&dev->mode_config.output_poll_work, DRM_OUTPUT_POLL_PERIOD);
+}
+EXPORT_SYMBOL(drm_kms_helper_poll_enable);
+
+/**
+ * drm_kms_helper_poll_init - initialize and enable output polling
+ * @dev: drm_device
+ *
+ * This function intializes and then also enables output polling support for
+ * @dev. Drivers which do not have reliable hotplug support in hardware can use
+ * this helper infrastructure to regularly poll such connectors for changes in
+ * their connection state.
+ *
+ * Drivers can control which connectors are polled by setting the
+ * DRM_CONNECTOR_POLL_CONNECT and DRM_CONNECTOR_POLL_DISCONNECT flags. On
+ * connectors where probing live outputs can result in visual distortion drivers
+ * should not set the DRM_CONNECTOR_POLL_DISCONNECT flag to avoid this.
+ * Connectors which have no flag or only DRM_CONNECTOR_POLL_HPD set are
+ * completely ignored by the polling logic.
+ *
+ * Note that a connector can be both polled and probed from the hotplug handler,
+ * in case the hotplug interrupt is known to be unreliable.
+ */
+void drm_kms_helper_poll_init(struct drm_device *dev)
+{
+	INIT_DELAYED_WORK(&dev->mode_config.output_poll_work, output_poll_execute);
+	dev->mode_config.poll_enabled = true;
+
+	drm_kms_helper_poll_enable(dev);
+}
+EXPORT_SYMBOL(drm_kms_helper_poll_init);
+
+/**
+ * drm_kms_helper_poll_fini - disable output polling and clean it up
+ * @dev: drm_device
+ */
+void drm_kms_helper_poll_fini(struct drm_device *dev)
+{
+	drm_kms_helper_poll_disable(dev);
+}
+EXPORT_SYMBOL(drm_kms_helper_poll_fini);
+
+/**
+ * drm_helper_hpd_irq_event - hotplug processing
+ * @dev: drm_device
+ *
+ * Drivers can use this helper function to run a detect cycle on all connectors
+ * which have the DRM_CONNECTOR_POLL_HPD flag set in their &polled member. All
+ * other connectors are ignored, which is useful to avoid reprobing fixed
+ * panels.
+ *
+ * This helper function is useful for drivers which can't or don't track hotplug
+ * interrupts for each connector.
+ *
+ * Drivers which support hotplug interrupts for each connector individually and
+ * which have a more fine-grained detect logic should bypass this code and
+ * directly call drm_kms_helper_hotplug_event() in case the connector state
+ * changed.
+ *
+ * This function must be called from process context with no mode
+ * setting locks held.
+ *
+ * Note that a connector can be both polled and probed from the hotplug handler,
+ * in case the hotplug interrupt is known to be unreliable.
+ */
+bool drm_helper_hpd_irq_event(struct drm_device *dev)
+{
+	struct drm_connector *connector;
+	enum drm_connector_status old_status;
+	bool changed = false;
+
+	if (!dev->mode_config.poll_enabled)
+		return false;
+
+	mutex_lock(&dev->mode_config.mutex);
+	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
+
+		/* Only handle HPD capable connectors. */
+		if (!(connector->polled & DRM_CONNECTOR_POLL_HPD))
+			continue;
+
+		old_status = connector->status;
+
+		connector->status = connector->funcs->detect(connector, false);
+		DRM_DEBUG_KMS("[CONNECTOR:%d:%s] status updated from %s to %s\n",
+			      connector->base.id,
+			      drm_get_connector_name(connector),
+			      drm_get_connector_status_name(old_status),
+			      drm_get_connector_status_name(connector->status));
+		if (old_status != connector->status)
+			changed = true;
+	}
+
+	mutex_unlock(&dev->mode_config.mutex);
+
+	if (changed)
+		drm_kms_helper_hotplug_event(dev);
+
+	return changed;
+}
+EXPORT_SYMBOL(drm_helper_hpd_irq_event);
diff --git a/drivers/gpu/drm/exynos/exynos_drm_crtc.c b/drivers/gpu/drm/exynos/exynos_drm_crtc.c
index e930d4fe29c..1ef5ab9c9d5 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_crtc.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_crtc.c
@@ -145,6 +145,7 @@ exynos_drm_crtc_mode_set(struct drm_crtc *crtc, struct drm_display_mode *mode,
 
 	plane->crtc = crtc;
 	plane->fb = crtc->primary->fb;
+	drm_framebuffer_reference(plane->fb);
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/exynos/exynos_drm_dmabuf.c b/drivers/gpu/drm/exynos/exynos_drm_dmabuf.c
index c786cd4f457..2a3ad24276f 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_dmabuf.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_dmabuf.c
@@ -263,7 +263,7 @@ struct drm_gem_object *exynos_dmabuf_prime_import(struct drm_device *drm_dev,
 	buffer->sgt = sgt;
 	exynos_gem_obj->base.import_attach = attach;
 
-	DRM_DEBUG_PRIME("dma_addr = 0x%x, size = 0x%lx\n", buffer->dma_addr,
+	DRM_DEBUG_PRIME("dma_addr = %pad, size = 0x%lx\n", &buffer->dma_addr,
 								buffer->size);
 
 	return &exynos_gem_obj->base;
diff --git a/drivers/gpu/drm/exynos/exynos_drm_dsi.c b/drivers/gpu/drm/exynos/exynos_drm_dsi.c
index eb73e3bf2a0..4ac43818756 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_dsi.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_dsi.c
@@ -1426,9 +1426,9 @@ static int exynos_dsi_probe(struct platform_device *pdev)
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	dsi->reg_base = devm_ioremap_resource(&pdev->dev, res);
-	if (!dsi->reg_base) {
+	if (IS_ERR(dsi->reg_base)) {
 		dev_err(&pdev->dev, "failed to remap io region\n");
-		return -EADDRNOTAVAIL;
+		return PTR_ERR(dsi->reg_base);
 	}
 
 	dsi->phy = devm_phy_get(&pdev->dev, "dsim");
diff --git a/drivers/gpu/drm/exynos/exynos_drm_vidi.c b/drivers/gpu/drm/exynos/exynos_drm_vidi.c
index 7afead9c3f3..852f2dadaeb 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_vidi.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_vidi.c
@@ -220,7 +220,7 @@ static void vidi_win_commit(struct exynos_drm_manager *mgr, int zpos)
 
 	win_data->enabled = true;
 
-	DRM_DEBUG_KMS("dma_addr = 0x%x\n", win_data->dma_addr);
+	DRM_DEBUG_KMS("dma_addr = %pad\n", &win_data->dma_addr);
 
 	if (ctx->vblank_on)
 		schedule_work(&ctx->work);
diff --git a/drivers/gpu/drm/i915/i915_dma.c b/drivers/gpu/drm/i915/i915_dma.c
index 96177eec0a0..eedb023af27 100644
--- a/drivers/gpu/drm/i915/i915_dma.c
+++ b/drivers/gpu/drm/i915/i915_dma.c
@@ -1833,7 +1833,6 @@ int i915_driver_unload(struct drm_device *dev)
 		flush_workqueue(dev_priv->wq);
 
 		mutex_lock(&dev->struct_mutex);
-		i915_gem_free_all_phys_object(dev);
 		i915_gem_cleanup_ringbuffer(dev);
 		i915_gem_context_fini(dev);
 		WARN_ON(dev_priv->mm.aliasing_ppgtt);
diff --git a/drivers/gpu/drm/i915/i915_drv.h b/drivers/gpu/drm/i915/i915_drv.h
index 0905cd91558..388c028e223 100644
--- a/drivers/gpu/drm/i915/i915_drv.h
+++ b/drivers/gpu/drm/i915/i915_drv.h
@@ -242,18 +242,6 @@ struct intel_ddi_plls {
 #define WATCH_LISTS	0
 #define WATCH_GTT	0
 
-#define I915_GEM_PHYS_CURSOR_0 1
-#define I915_GEM_PHYS_CURSOR_1 2
-#define I915_GEM_PHYS_OVERLAY_REGS 3
-#define I915_MAX_PHYS_OBJECT (I915_GEM_PHYS_OVERLAY_REGS)
-
-struct drm_i915_gem_phys_object {
-	int id;
-	struct page **page_list;
-	drm_dma_handle_t *handle;
-	struct drm_i915_gem_object *cur_obj;
-};
-
 struct opregion_header;
 struct opregion_acpi;
 struct opregion_swsci;
@@ -1187,9 +1175,6 @@ struct i915_gem_mm {
 	/** Bit 6 swizzling required for Y tiling */
 	uint32_t bit_6_swizzle_y;
 
-	/* storage for physical objects */
-	struct drm_i915_gem_phys_object *phys_objs[I915_MAX_PHYS_OBJECT];
-
 	/* accounting, useful for userland debugging */
 	spinlock_t object_stat_lock;
 	size_t object_memory;
@@ -1308,6 +1293,7 @@ struct intel_vbt_data {
 
 	struct {
 		u16 pwm_freq_hz;
+		bool present;
 		bool active_low_pwm;
 	} backlight;
 
@@ -1768,7 +1754,7 @@ struct drm_i915_gem_object {
 	struct drm_file *pin_filp;
 
 	/** for phy allocated objects */
-	struct drm_i915_gem_phys_object *phys_obj;
+	drm_dma_handle_t *phys_handle;
 };
 
 #define to_intel_bo(x) container_of(x, struct drm_i915_gem_object, base)
@@ -1953,6 +1939,9 @@ struct drm_i915_cmd_table {
 #define IS_ULT(dev)		(IS_HSW_ULT(dev) || IS_BDW_ULT(dev))
 #define IS_HSW_GT3(dev)		(IS_HASWELL(dev) && \
 				 ((dev)->pdev->device & 0x00F0) == 0x0020)
+/* ULX machines are also considered ULT. */
+#define IS_HSW_ULX(dev)		((dev)->pdev->device == 0x0A0E || \
+				 (dev)->pdev->device == 0x0A1E)
 #define IS_PRELIMINARY_HW(intel_info) ((intel_info)->is_preliminary)
 
 /*
@@ -2200,10 +2189,12 @@ void i915_gem_vma_destroy(struct i915_vma *vma);
 #define PIN_MAPPABLE 0x1
 #define PIN_NONBLOCK 0x2
 #define PIN_GLOBAL 0x4
+#define PIN_OFFSET_BIAS 0x8
+#define PIN_OFFSET_MASK (~4095)
 int __must_check i915_gem_object_pin(struct drm_i915_gem_object *obj,
 				     struct i915_address_space *vm,
 				     uint32_t alignment,
-				     unsigned flags);
+				     uint64_t flags);
 int __must_check i915_vma_unbind(struct i915_vma *vma);
 int i915_gem_object_put_pages(struct drm_i915_gem_object *obj);
 void i915_gem_release_all_mmaps(struct drm_i915_private *dev_priv);
@@ -2330,13 +2321,8 @@ i915_gem_object_pin_to_display_plane(struct drm_i915_gem_object *obj,
 				     u32 alignment,
 				     struct intel_ring_buffer *pipelined);
 void i915_gem_object_unpin_from_display_plane(struct drm_i915_gem_object *obj);
-int i915_gem_attach_phys_object(struct drm_device *dev,
-				struct drm_i915_gem_object *obj,
-				int id,
+int i915_gem_object_attach_phys(struct drm_i915_gem_object *obj,
 				int align);
-void i915_gem_detach_phys_object(struct drm_device *dev,
-				 struct drm_i915_gem_object *obj);
-void i915_gem_free_all_phys_object(struct drm_device *dev);
 int i915_gem_open(struct drm_device *dev, struct drm_file *file);
 void i915_gem_release(struct drm_device *dev, struct drm_file *file);
 
@@ -2431,20 +2417,18 @@ int i915_gem_context_open(struct drm_device *dev, struct drm_file *file);
 int i915_gem_context_enable(struct drm_i915_private *dev_priv);
 void i915_gem_context_close(struct drm_device *dev, struct drm_file *file);
 int i915_switch_context(struct intel_ring_buffer *ring,
-			struct drm_file *file, struct i915_hw_context *to);
+			struct i915_hw_context *to);
 struct i915_hw_context *
 i915_gem_context_get(struct drm_i915_file_private *file_priv, u32 id);
 void i915_gem_context_free(struct kref *ctx_ref);
 static inline void i915_gem_context_reference(struct i915_hw_context *ctx)
 {
-	if (ctx->obj && HAS_HW_CONTEXTS(ctx->obj->base.dev))
-		kref_get(&ctx->ref);
+	kref_get(&ctx->ref);
 }
 
 static inline void i915_gem_context_unreference(struct i915_hw_context *ctx)
 {
-	if (ctx->obj && HAS_HW_CONTEXTS(ctx->obj->base.dev))
-		kref_put(&ctx->ref, i915_gem_context_free);
+	kref_put(&ctx->ref, i915_gem_context_free);
 }
 
 static inline bool i915_gem_context_is_default(const struct i915_hw_context *c)
@@ -2463,6 +2447,8 @@ int __must_check i915_gem_evict_something(struct drm_device *dev,
 					  int min_size,
 					  unsigned alignment,
 					  unsigned cache_level,
+					  unsigned long start,
+					  unsigned long end,
 					  unsigned flags);
 int i915_gem_evict_vm(struct i915_address_space *vm, bool do_idle);
 int i915_gem_evict_everything(struct drm_device *dev);
diff --git a/drivers/gpu/drm/i915/i915_gem.c b/drivers/gpu/drm/i915/i915_gem.c
index 6370a761d13..3326770c9ed 100644
--- a/drivers/gpu/drm/i915/i915_gem.c
+++ b/drivers/gpu/drm/i915/i915_gem.c
@@ -43,10 +43,6 @@ static void i915_gem_object_flush_cpu_write_domain(struct drm_i915_gem_object *o
 static __must_check int
 i915_gem_object_wait_rendering(struct drm_i915_gem_object *obj,
 			       bool readonly);
-static int i915_gem_phys_pwrite(struct drm_device *dev,
-				struct drm_i915_gem_object *obj,
-				struct drm_i915_gem_pwrite *args,
-				struct drm_file *file);
 
 static void i915_gem_write_fence(struct drm_device *dev, int reg,
 				 struct drm_i915_gem_object *obj);
@@ -209,6 +205,128 @@ i915_gem_get_aperture_ioctl(struct drm_device *dev, void *data,
 	return 0;
 }
 
+static void i915_gem_object_detach_phys(struct drm_i915_gem_object *obj)
+{
+	drm_dma_handle_t *phys = obj->phys_handle;
+
+	if (!phys)
+		return;
+
+	if (obj->madv == I915_MADV_WILLNEED) {
+		struct address_space *mapping = file_inode(obj->base.filp)->i_mapping;
+		char *vaddr = phys->vaddr;
+		int i;
+
+		for (i = 0; i < obj->base.size / PAGE_SIZE; i++) {
+			struct page *page = shmem_read_mapping_page(mapping, i);
+			if (!IS_ERR(page)) {
+				char *dst = kmap_atomic(page);
+				memcpy(dst, vaddr, PAGE_SIZE);
+				drm_clflush_virt_range(dst, PAGE_SIZE);
+				kunmap_atomic(dst);
+
+				set_page_dirty(page);
+				mark_page_accessed(page);
+				page_cache_release(page);
+			}
+			vaddr += PAGE_SIZE;
+		}
+		i915_gem_chipset_flush(obj->base.dev);
+	}
+
+#ifdef CONFIG_X86
+	set_memory_wb((unsigned long)phys->vaddr, phys->size / PAGE_SIZE);
+#endif
+	drm_pci_free(obj->base.dev, phys);
+	obj->phys_handle = NULL;
+}
+
+int
+i915_gem_object_attach_phys(struct drm_i915_gem_object *obj,
+			    int align)
+{
+	drm_dma_handle_t *phys;
+	struct address_space *mapping;
+	char *vaddr;
+	int i;
+
+	if (obj->phys_handle) {
+		if ((unsigned long)obj->phys_handle->vaddr & (align -1))
+			return -EBUSY;
+
+		return 0;
+	}
+
+	if (obj->madv != I915_MADV_WILLNEED)
+		return -EFAULT;
+
+	if (obj->base.filp == NULL)
+		return -EINVAL;
+
+	/* create a new object */
+	phys = drm_pci_alloc(obj->base.dev, obj->base.size, align);
+	if (!phys)
+		return -ENOMEM;
+
+	vaddr = phys->vaddr;
+#ifdef CONFIG_X86
+	set_memory_wc((unsigned long)vaddr, phys->size / PAGE_SIZE);
+#endif
+	mapping = file_inode(obj->base.filp)->i_mapping;
+	for (i = 0; i < obj->base.size / PAGE_SIZE; i++) {
+		struct page *page;
+		char *src;
+
+		page = shmem_read_mapping_page(mapping, i);
+		if (IS_ERR(page)) {
+#ifdef CONFIG_X86
+			set_memory_wb((unsigned long)phys->vaddr, phys->size / PAGE_SIZE);
+#endif
+			drm_pci_free(obj->base.dev, phys);
+			return PTR_ERR(page);
+		}
+
+		src = kmap_atomic(page);
+		memcpy(vaddr, src, PAGE_SIZE);
+		kunmap_atomic(src);
+
+		mark_page_accessed(page);
+		page_cache_release(page);
+
+		vaddr += PAGE_SIZE;
+	}
+
+	obj->phys_handle = phys;
+	return 0;
+}
+
+static int
+i915_gem_phys_pwrite(struct drm_i915_gem_object *obj,
+		     struct drm_i915_gem_pwrite *args,
+		     struct drm_file *file_priv)
+{
+	struct drm_device *dev = obj->base.dev;
+	void *vaddr = obj->phys_handle->vaddr + args->offset;
+	char __user *user_data = to_user_ptr(args->data_ptr);
+
+	if (__copy_from_user_inatomic_nocache(vaddr, user_data, args->size)) {
+		unsigned long unwritten;
+
+		/* The physical object once assigned is fixed for the lifetime
+		 * of the obj, so we can safely drop the lock and continue
+		 * to access vaddr.
+		 */
+		mutex_unlock(&dev->struct_mutex);
+		unwritten = copy_from_user(vaddr, user_data, args->size);
+		mutex_lock(&dev->struct_mutex);
+		if (unwritten)
+			return -EFAULT;
+	}
+
+	i915_gem_chipset_flush(dev);
+	return 0;
+}
+
 void *i915_gem_object_alloc(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -921,8 +1039,8 @@ i915_gem_pwrite_ioctl(struct drm_device *dev, void *data,
 	 * pread/pwrite currently are reading and writing from the CPU
 	 * perspective, requiring manual detiling by the client.
 	 */
-	if (obj->phys_obj) {
-		ret = i915_gem_phys_pwrite(dev, obj, args, file);
+	if (obj->phys_handle) {
+		ret = i915_gem_phys_pwrite(obj, args, file);
 		goto out;
 	}
 
@@ -2790,7 +2908,7 @@ int i915_gpu_idle(struct drm_device *dev)
 
 	/* Flush everything onto the inactive list. */
 	for_each_ring(ring, dev_priv, i) {
-		ret = i915_switch_context(ring, NULL, ring->default_context);
+		ret = i915_switch_context(ring, ring->default_context);
 		if (ret)
 			return ret;
 
@@ -3208,12 +3326,14 @@ static struct i915_vma *
 i915_gem_object_bind_to_vm(struct drm_i915_gem_object *obj,
 			   struct i915_address_space *vm,
 			   unsigned alignment,
-			   unsigned flags)
+			   uint64_t flags)
 {
 	struct drm_device *dev = obj->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 size, fence_size, fence_alignment, unfenced_alignment;
-	size_t gtt_max =
+	unsigned long start =
+		flags & PIN_OFFSET_BIAS ? flags & PIN_OFFSET_MASK : 0;
+	unsigned long end =
 		flags & PIN_MAPPABLE ? dev_priv->gtt.mappable_end : vm->total;
 	struct i915_vma *vma;
 	int ret;
@@ -3242,11 +3362,11 @@ i915_gem_object_bind_to_vm(struct drm_i915_gem_object *obj,
 	/* If the object is bigger than the entire aperture, reject it early
 	 * before evicting everything in a vain attempt to find space.
 	 */
-	if (obj->base.size > gtt_max) {
-		DRM_DEBUG("Attempting to bind an object larger than the aperture: object=%zd > %s aperture=%zu\n",
+	if (obj->base.size > end) {
+		DRM_DEBUG("Attempting to bind an object larger than the aperture: object=%zd > %s aperture=%lu\n",
 			  obj->base.size,
 			  flags & PIN_MAPPABLE ? "mappable" : "total",
-			  gtt_max);
+			  end);
 		return ERR_PTR(-E2BIG);
 	}
 
@@ -3263,12 +3383,15 @@ i915_gem_object_bind_to_vm(struct drm_i915_gem_object *obj,
 search_free:
 	ret = drm_mm_insert_node_in_range_generic(&vm->mm, &vma->node,
 						  size, alignment,
-						  obj->cache_level, 0, gtt_max,
+						  obj->cache_level,
+						  start, end,
 						  DRM_MM_SEARCH_DEFAULT,
 						  DRM_MM_CREATE_DEFAULT);
 	if (ret) {
 		ret = i915_gem_evict_something(dev, vm, size, alignment,
-					       obj->cache_level, flags);
+					       obj->cache_level,
+					       start, end,
+					       flags);
 		if (ret == 0)
 			goto search_free;
 
@@ -3828,11 +3951,30 @@ i915_gem_ring_throttle(struct drm_device *dev, struct drm_file *file)
 	return ret;
 }
 
+static bool
+i915_vma_misplaced(struct i915_vma *vma, uint32_t alignment, uint64_t flags)
+{
+	struct drm_i915_gem_object *obj = vma->obj;
+
+	if (alignment &&
+	    vma->node.start & (alignment - 1))
+		return true;
+
+	if (flags & PIN_MAPPABLE && !obj->map_and_fenceable)
+		return true;
+
+	if (flags & PIN_OFFSET_BIAS &&
+	    vma->node.start < (flags & PIN_OFFSET_MASK))
+		return true;
+
+	return false;
+}
+
 int
 i915_gem_object_pin(struct drm_i915_gem_object *obj,
 		    struct i915_address_space *vm,
 		    uint32_t alignment,
-		    unsigned flags)
+		    uint64_t flags)
 {
 	struct i915_vma *vma;
 	int ret;
@@ -3845,15 +3987,13 @@ i915_gem_object_pin(struct drm_i915_gem_object *obj,
 		if (WARN_ON(vma->pin_count == DRM_I915_GEM_OBJECT_MAX_PIN_COUNT))
 			return -EBUSY;
 
-		if ((alignment &&
-		     vma->node.start & (alignment - 1)) ||
-		    (flags & PIN_MAPPABLE && !obj->map_and_fenceable)) {
+		if (i915_vma_misplaced(vma, alignment, flags)) {
 			WARN(vma->pin_count,
 			     "bo is already pinned with incorrect alignment:"
 			     " offset=%lx, req.alignment=%x, req.map_and_fenceable=%d,"
 			     " obj->map_and_fenceable=%d\n",
 			     i915_gem_obj_offset(obj, vm), alignment,
-			     flags & PIN_MAPPABLE,
+			     !!(flags & PIN_MAPPABLE),
 			     obj->map_and_fenceable);
 			ret = i915_vma_unbind(vma);
 			if (ret)
@@ -4163,9 +4303,6 @@ void i915_gem_free_object(struct drm_gem_object *gem_obj)
 
 	trace_i915_gem_object_destroy(obj);
 
-	if (obj->phys_obj)
-		i915_gem_detach_phys_object(dev, obj);
-
 	list_for_each_entry_safe(vma, next, &obj->vma_list, vma_link) {
 		int ret;
 
@@ -4183,6 +4320,8 @@ void i915_gem_free_object(struct drm_gem_object *gem_obj)
 		}
 	}
 
+	i915_gem_object_detach_phys(obj);
+
 	/* Stolen objects don't hold a ref, but do hold pin count. Fix that up
 	 * before progressing. */
 	if (obj->stolen)
@@ -4646,190 +4785,6 @@ i915_gem_load(struct drm_device *dev)
 	register_shrinker(&dev_priv->mm.inactive_shrinker);
 }
 
-/*
- * Create a physically contiguous memory object for this object
- * e.g. for cursor + overlay regs
- */
-static int i915_gem_init_phys_object(struct drm_device *dev,
-				     int id, int size, int align)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_i915_gem_phys_object *phys_obj;
-	int ret;
-
-	if (dev_priv->mm.phys_objs[id - 1] || !size)
-		return 0;
-
-	phys_obj = kzalloc(sizeof(*phys_obj), GFP_KERNEL);
-	if (!phys_obj)
-		return -ENOMEM;
-
-	phys_obj->id = id;
-
-	phys_obj->handle = drm_pci_alloc(dev, size, align);
-	if (!phys_obj->handle) {
-		ret = -ENOMEM;
-		goto kfree_obj;
-	}
-#ifdef CONFIG_X86
-	set_memory_wc((unsigned long)phys_obj->handle->vaddr, phys_obj->handle->size / PAGE_SIZE);
-#endif
-
-	dev_priv->mm.phys_objs[id - 1] = phys_obj;
-
-	return 0;
-kfree_obj:
-	kfree(phys_obj);
-	return ret;
-}
-
-static void i915_gem_free_phys_object(struct drm_device *dev, int id)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_i915_gem_phys_object *phys_obj;
-
-	if (!dev_priv->mm.phys_objs[id - 1])
-		return;
-
-	phys_obj = dev_priv->mm.phys_objs[id - 1];
-	if (phys_obj->cur_obj) {
-		i915_gem_detach_phys_object(dev, phys_obj->cur_obj);
-	}
-
-#ifdef CONFIG_X86
-	set_memory_wb((unsigned long)phys_obj->handle->vaddr, phys_obj->handle->size / PAGE_SIZE);
-#endif
-	drm_pci_free(dev, phys_obj->handle);
-	kfree(phys_obj);
-	dev_priv->mm.phys_objs[id - 1] = NULL;
-}
-
-void i915_gem_free_all_phys_object(struct drm_device *dev)
-{
-	int i;
-
-	for (i = I915_GEM_PHYS_CURSOR_0; i <= I915_MAX_PHYS_OBJECT; i++)
-		i915_gem_free_phys_object(dev, i);
-}
-
-void i915_gem_detach_phys_object(struct drm_device *dev,
-				 struct drm_i915_gem_object *obj)
-{
-	struct address_space *mapping = file_inode(obj->base.filp)->i_mapping;
-	char *vaddr;
-	int i;
-	int page_count;
-
-	if (!obj->phys_obj)
-		return;
-	vaddr = obj->phys_obj->handle->vaddr;
-
-	page_count = obj->base.size / PAGE_SIZE;
-	for (i = 0; i < page_count; i++) {
-		struct page *page = shmem_read_mapping_page(mapping, i);
-		if (!IS_ERR(page)) {
-			char *dst = kmap_atomic(page);
-			memcpy(dst, vaddr + i*PAGE_SIZE, PAGE_SIZE);
-			kunmap_atomic(dst);
-
-			drm_clflush_pages(&page, 1);
-
-			set_page_dirty(page);
-			mark_page_accessed(page);
-			page_cache_release(page);
-		}
-	}
-	i915_gem_chipset_flush(dev);
-
-	obj->phys_obj->cur_obj = NULL;
-	obj->phys_obj = NULL;
-}
-
-int
-i915_gem_attach_phys_object(struct drm_device *dev,
-			    struct drm_i915_gem_object *obj,
-			    int id,
-			    int align)
-{
-	struct address_space *mapping = file_inode(obj->base.filp)->i_mapping;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	int ret = 0;
-	int page_count;
-	int i;
-
-	if (id > I915_MAX_PHYS_OBJECT)
-		return -EINVAL;
-
-	if (obj->phys_obj) {
-		if (obj->phys_obj->id == id)
-			return 0;
-		i915_gem_detach_phys_object(dev, obj);
-	}
-
-	/* create a new object */
-	if (!dev_priv->mm.phys_objs[id - 1]) {
-		ret = i915_gem_init_phys_object(dev, id,
-						obj->base.size, align);
-		if (ret) {
-			DRM_ERROR("failed to init phys object %d size: %zu\n",
-				  id, obj->base.size);
-			return ret;
-		}
-	}
-
-	/* bind to the object */
-	obj->phys_obj = dev_priv->mm.phys_objs[id - 1];
-	obj->phys_obj->cur_obj = obj;
-
-	page_count = obj->base.size / PAGE_SIZE;
-
-	for (i = 0; i < page_count; i++) {
-		struct page *page;
-		char *dst, *src;
-
-		page = shmem_read_mapping_page(mapping, i);
-		if (IS_ERR(page))
-			return PTR_ERR(page);
-
-		src = kmap_atomic(page);
-		dst = obj->phys_obj->handle->vaddr + (i * PAGE_SIZE);
-		memcpy(dst, src, PAGE_SIZE);
-		kunmap_atomic(src);
-
-		mark_page_accessed(page);
-		page_cache_release(page);
-	}
-
-	return 0;
-}
-
-static int
-i915_gem_phys_pwrite(struct drm_device *dev,
-		     struct drm_i915_gem_object *obj,
-		     struct drm_i915_gem_pwrite *args,
-		     struct drm_file *file_priv)
-{
-	void *vaddr = obj->phys_obj->handle->vaddr + args->offset;
-	char __user *user_data = to_user_ptr(args->data_ptr);
-
-	if (__copy_from_user_inatomic_nocache(vaddr, user_data, args->size)) {
-		unsigned long unwritten;
-
-		/* The physical object once assigned is fixed for the lifetime
-		 * of the obj, so we can safely drop the lock and continue
-		 * to access vaddr.
-		 */
-		mutex_unlock(&dev->struct_mutex);
-		unwritten = copy_from_user(vaddr, user_data, args->size);
-		mutex_lock(&dev->struct_mutex);
-		if (unwritten)
-			return -EFAULT;
-	}
-
-	i915_gem_chipset_flush(dev);
-	return 0;
-}
-
 void i915_gem_release(struct drm_device *dev, struct drm_file *file)
 {
 	struct drm_i915_file_private *file_priv = file->driver_priv;
diff --git a/drivers/gpu/drm/i915/i915_gem_context.c b/drivers/gpu/drm/i915/i915_gem_context.c
index 6043062ffce..d72db15afa0 100644
--- a/drivers/gpu/drm/i915/i915_gem_context.c
+++ b/drivers/gpu/drm/i915/i915_gem_context.c
@@ -96,9 +96,6 @@
 #define GEN6_CONTEXT_ALIGN (64<<10)
 #define GEN7_CONTEXT_ALIGN 4096
 
-static int do_switch(struct intel_ring_buffer *ring,
-		     struct i915_hw_context *to);
-
 static void do_ppgtt_cleanup(struct i915_hw_ppgtt *ppgtt)
 {
 	struct drm_device *dev = ppgtt->base.dev;
@@ -185,13 +182,15 @@ void i915_gem_context_free(struct kref *ctx_ref)
 						   typeof(*ctx), ref);
 	struct i915_hw_ppgtt *ppgtt = NULL;
 
-	/* We refcount even the aliasing PPGTT to keep the code symmetric */
-	if (USES_PPGTT(ctx->obj->base.dev))
-		ppgtt = ctx_to_ppgtt(ctx);
+	if (ctx->obj) {
+		/* We refcount even the aliasing PPGTT to keep the code symmetric */
+		if (USES_PPGTT(ctx->obj->base.dev))
+			ppgtt = ctx_to_ppgtt(ctx);
 
-	/* XXX: Free up the object before tearing down the address space, in
-	 * case we're bound in the PPGTT */
-	drm_gem_object_unreference(&ctx->obj->base);
+		/* XXX: Free up the object before tearing down the address space, in
+		 * case we're bound in the PPGTT */
+		drm_gem_object_unreference(&ctx->obj->base);
+	}
 
 	if (ppgtt)
 		kref_put(&ppgtt->ref, ppgtt_release);
@@ -232,32 +231,32 @@ __create_hw_context(struct drm_device *dev,
 		return ERR_PTR(-ENOMEM);
 
 	kref_init(&ctx->ref);
-	ctx->obj = i915_gem_alloc_object(dev, dev_priv->hw_context_size);
-	INIT_LIST_HEAD(&ctx->link);
-	if (ctx->obj == NULL) {
-		kfree(ctx);
-		DRM_DEBUG_DRIVER("Context object allocated failed\n");
-		return ERR_PTR(-ENOMEM);
-	}
+	list_add_tail(&ctx->link, &dev_priv->context_list);
 
-	if (INTEL_INFO(dev)->gen >= 7) {
-		ret = i915_gem_object_set_cache_level(ctx->obj,
-						      I915_CACHE_L3_LLC);
-		/* Failure shouldn't ever happen this early */
-		if (WARN_ON(ret))
+	if (dev_priv->hw_context_size) {
+		ctx->obj = i915_gem_alloc_object(dev, dev_priv->hw_context_size);
+		if (ctx->obj == NULL) {
+			ret = -ENOMEM;
 			goto err_out;
-	}
+		}
 
-	list_add_tail(&ctx->link, &dev_priv->context_list);
+		if (INTEL_INFO(dev)->gen >= 7) {
+			ret = i915_gem_object_set_cache_level(ctx->obj,
+							      I915_CACHE_L3_LLC);
+			/* Failure shouldn't ever happen this early */
+			if (WARN_ON(ret))
+				goto err_out;
+		}
+	}
 
 	/* Default context will never have a file_priv */
-	if (file_priv == NULL)
-		return ctx;
-
-	ret = idr_alloc(&file_priv->context_idr, ctx, DEFAULT_CONTEXT_ID, 0,
-			GFP_KERNEL);
-	if (ret < 0)
-		goto err_out;
+	if (file_priv != NULL) {
+		ret = idr_alloc(&file_priv->context_idr, ctx,
+				DEFAULT_CONTEXT_ID, 0, GFP_KERNEL);
+		if (ret < 0)
+			goto err_out;
+	} else
+		ret = DEFAULT_CONTEXT_ID;
 
 	ctx->file_priv = file_priv;
 	ctx->id = ret;
@@ -294,7 +293,7 @@ i915_gem_create_context(struct drm_device *dev,
 	if (IS_ERR(ctx))
 		return ctx;
 
-	if (is_global_default_ctx) {
+	if (is_global_default_ctx && ctx->obj) {
 		/* We may need to do things with the shrinker which
 		 * require us to immediately switch back to the default
 		 * context. This can cause a problem as pinning the
@@ -342,7 +341,7 @@ i915_gem_create_context(struct drm_device *dev,
 	return ctx;
 
 err_unpin:
-	if (is_global_default_ctx)
+	if (is_global_default_ctx && ctx->obj)
 		i915_gem_object_ggtt_unpin(ctx->obj);
 err_destroy:
 	i915_gem_context_unreference(ctx);
@@ -352,32 +351,22 @@ err_destroy:
 void i915_gem_context_reset(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_ring_buffer *ring;
 	int i;
 
-	if (!HAS_HW_CONTEXTS(dev))
-		return;
-
 	/* Prevent the hardware from restoring the last context (which hung) on
 	 * the next switch */
 	for (i = 0; i < I915_NUM_RINGS; i++) {
-		struct i915_hw_context *dctx;
-		if (!(INTEL_INFO(dev)->ring_mask & (1<<i)))
-			continue;
+		struct intel_ring_buffer *ring = &dev_priv->ring[i];
+		struct i915_hw_context *dctx = ring->default_context;
 
 		/* Do a fake switch to the default context */
-		ring = &dev_priv->ring[i];
-		dctx = ring->default_context;
-		if (WARN_ON(!dctx))
+		if (ring->last_context == dctx)
 			continue;
 
 		if (!ring->last_context)
 			continue;
 
-		if (ring->last_context == dctx)
-			continue;
-
-		if (i == RCS) {
+		if (dctx->obj && i == RCS) {
 			WARN_ON(i915_gem_obj_ggtt_pin(dctx->obj,
 						      get_context_alignment(dev), 0));
 			/* Fake a finish/inactive */
@@ -394,44 +383,35 @@ void i915_gem_context_reset(struct drm_device *dev)
 int i915_gem_context_init(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_ring_buffer *ring;
+	struct i915_hw_context *ctx;
 	int i;
 
-	if (!HAS_HW_CONTEXTS(dev))
-		return 0;
-
 	/* Init should only be called once per module load. Eventually the
 	 * restriction on the context_disabled check can be loosened. */
 	if (WARN_ON(dev_priv->ring[RCS].default_context))
 		return 0;
 
-	dev_priv->hw_context_size = round_up(get_context_size(dev), 4096);
-
-	if (dev_priv->hw_context_size > (1<<20)) {
-		DRM_DEBUG_DRIVER("Disabling HW Contexts; invalid size\n");
-		return -E2BIG;
+	if (HAS_HW_CONTEXTS(dev)) {
+		dev_priv->hw_context_size = round_up(get_context_size(dev), 4096);
+		if (dev_priv->hw_context_size > (1<<20)) {
+			DRM_DEBUG_DRIVER("Disabling HW Contexts; invalid size %d\n",
+					 dev_priv->hw_context_size);
+			dev_priv->hw_context_size = 0;
+		}
 	}
 
-	dev_priv->ring[RCS].default_context =
-		i915_gem_create_context(dev, NULL, USES_PPGTT(dev));
-
-	if (IS_ERR_OR_NULL(dev_priv->ring[RCS].default_context)) {
-		DRM_DEBUG_DRIVER("Disabling HW Contexts; create failed %ld\n",
-				 PTR_ERR(dev_priv->ring[RCS].default_context));
-		return PTR_ERR(dev_priv->ring[RCS].default_context);
+	ctx = i915_gem_create_context(dev, NULL, USES_PPGTT(dev));
+	if (IS_ERR(ctx)) {
+		DRM_ERROR("Failed to create default global context (error %ld)\n",
+			  PTR_ERR(ctx));
+		return PTR_ERR(ctx);
 	}
 
-	for (i = RCS + 1; i < I915_NUM_RINGS; i++) {
-		if (!(INTEL_INFO(dev)->ring_mask & (1<<i)))
-			continue;
-
-		ring = &dev_priv->ring[i];
+	/* NB: RCS will hold a ref for all rings */
+	for (i = 0; i < I915_NUM_RINGS; i++)
+		dev_priv->ring[i].default_context = ctx;
 
-		/* NB: RCS will hold a ref for all rings */
-		ring->default_context = dev_priv->ring[RCS].default_context;
-	}
-
-	DRM_DEBUG_DRIVER("HW context support initialized\n");
+	DRM_DEBUG_DRIVER("%s context support initialized\n", dev_priv->hw_context_size ? "HW" : "fake");
 	return 0;
 }
 
@@ -441,33 +421,30 @@ void i915_gem_context_fini(struct drm_device *dev)
 	struct i915_hw_context *dctx = dev_priv->ring[RCS].default_context;
 	int i;
 
-	if (!HAS_HW_CONTEXTS(dev))
-		return;
-
-	/* The only known way to stop the gpu from accessing the hw context is
-	 * to reset it. Do this as the very last operation to avoid confusing
-	 * other code, leading to spurious errors. */
-	intel_gpu_reset(dev);
-
-	/* When default context is created and switched to, base object refcount
-	 * will be 2 (+1 from object creation and +1 from do_switch()).
-	 * i915_gem_context_fini() will be called after gpu_idle() has switched
-	 * to default context. So we need to unreference the base object once
-	 * to offset the do_switch part, so that i915_gem_context_unreference()
-	 * can then free the base object correctly. */
-	WARN_ON(!dev_priv->ring[RCS].last_context);
-	if (dev_priv->ring[RCS].last_context == dctx) {
-		/* Fake switch to NULL context */
-		WARN_ON(dctx->obj->active);
-		i915_gem_object_ggtt_unpin(dctx->obj);
-		i915_gem_context_unreference(dctx);
-		dev_priv->ring[RCS].last_context = NULL;
+	if (dctx->obj) {
+		/* The only known way to stop the gpu from accessing the hw context is
+		 * to reset it. Do this as the very last operation to avoid confusing
+		 * other code, leading to spurious errors. */
+		intel_gpu_reset(dev);
+
+		/* When default context is created and switched to, base object refcount
+		 * will be 2 (+1 from object creation and +1 from do_switch()).
+		 * i915_gem_context_fini() will be called after gpu_idle() has switched
+		 * to default context. So we need to unreference the base object once
+		 * to offset the do_switch part, so that i915_gem_context_unreference()
+		 * can then free the base object correctly. */
+		WARN_ON(!dev_priv->ring[RCS].last_context);
+		if (dev_priv->ring[RCS].last_context == dctx) {
+			/* Fake switch to NULL context */
+			WARN_ON(dctx->obj->active);
+			i915_gem_object_ggtt_unpin(dctx->obj);
+			i915_gem_context_unreference(dctx);
+			dev_priv->ring[RCS].last_context = NULL;
+		}
 	}
 
 	for (i = 0; i < I915_NUM_RINGS; i++) {
 		struct intel_ring_buffer *ring = &dev_priv->ring[i];
-		if (!(INTEL_INFO(dev)->ring_mask & (1<<i)))
-			continue;
 
 		if (ring->last_context)
 			i915_gem_context_unreference(ring->last_context);
@@ -478,7 +455,6 @@ void i915_gem_context_fini(struct drm_device *dev)
 
 	i915_gem_object_ggtt_unpin(dctx->obj);
 	i915_gem_context_unreference(dctx);
-	dev_priv->mm.aliasing_ppgtt = NULL;
 }
 
 int i915_gem_context_enable(struct drm_i915_private *dev_priv)
@@ -486,9 +462,6 @@ int i915_gem_context_enable(struct drm_i915_private *dev_priv)
 	struct intel_ring_buffer *ring;
 	int ret, i;
 
-	if (!HAS_HW_CONTEXTS(dev_priv->dev))
-		return 0;
-
 	/* This is the only place the aliasing PPGTT gets enabled, which means
 	 * it has to happen before we bail on reset */
 	if (dev_priv->mm.aliasing_ppgtt) {
@@ -503,7 +476,7 @@ int i915_gem_context_enable(struct drm_i915_private *dev_priv)
 	BUG_ON(!dev_priv->ring[RCS].default_context);
 
 	for_each_ring(ring, dev_priv, i) {
-		ret = do_switch(ring, ring->default_context);
+		ret = i915_switch_context(ring, ring->default_context);
 		if (ret)
 			return ret;
 	}
@@ -526,19 +499,6 @@ static int context_idr_cleanup(int id, void *p, void *data)
 int i915_gem_context_open(struct drm_device *dev, struct drm_file *file)
 {
 	struct drm_i915_file_private *file_priv = file->driver_priv;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	if (!HAS_HW_CONTEXTS(dev)) {
-		/* Cheat for hang stats */
-		file_priv->private_default_ctx =
-			kzalloc(sizeof(struct i915_hw_context), GFP_KERNEL);
-
-		if (file_priv->private_default_ctx == NULL)
-			return -ENOMEM;
-
-		file_priv->private_default_ctx->vm = &dev_priv->gtt.base;
-		return 0;
-	}
 
 	idr_init(&file_priv->context_idr);
 
@@ -559,14 +519,10 @@ void i915_gem_context_close(struct drm_device *dev, struct drm_file *file)
 {
 	struct drm_i915_file_private *file_priv = file->driver_priv;
 
-	if (!HAS_HW_CONTEXTS(dev)) {
-		kfree(file_priv->private_default_ctx);
-		return;
-	}
-
 	idr_for_each(&file_priv->context_idr, context_idr_cleanup, NULL);
-	i915_gem_context_unreference(file_priv->private_default_ctx);
 	idr_destroy(&file_priv->context_idr);
+
+	i915_gem_context_unreference(file_priv->private_default_ctx);
 }
 
 struct i915_hw_context *
@@ -574,9 +530,6 @@ i915_gem_context_get(struct drm_i915_file_private *file_priv, u32 id)
 {
 	struct i915_hw_context *ctx;
 
-	if (!HAS_HW_CONTEXTS(file_priv->dev_priv->dev))
-		return file_priv->private_default_ctx;
-
 	ctx = (struct i915_hw_context *)idr_find(&file_priv->context_idr, id);
 	if (!ctx)
 		return ERR_PTR(-ENOENT);
@@ -758,7 +711,6 @@ unpin_out:
 /**
  * i915_switch_context() - perform a GPU context switch.
  * @ring: ring for which we'll execute the context switch
- * @file_priv: file_priv associated with the context, may be NULL
  * @to: the context to switch to
  *
  * The context life cycle is simple. The context refcount is incremented and
@@ -767,24 +719,30 @@ unpin_out:
  * object while letting the normal object tracking destroy the backing BO.
  */
 int i915_switch_context(struct intel_ring_buffer *ring,
-			struct drm_file *file,
 			struct i915_hw_context *to)
 {
 	struct drm_i915_private *dev_priv = ring->dev->dev_private;
 
 	WARN_ON(!mutex_is_locked(&dev_priv->dev->struct_mutex));
 
-	BUG_ON(file && to == NULL);
-
-	/* We have the fake context */
-	if (!HAS_HW_CONTEXTS(ring->dev)) {
-		ring->last_context = to;
+	if (to->obj == NULL) { /* We have the fake context */
+		if (to != ring->last_context) {
+			i915_gem_context_reference(to);
+			if (ring->last_context)
+				i915_gem_context_unreference(ring->last_context);
+			ring->last_context = to;
+		}
 		return 0;
 	}
 
 	return do_switch(ring, to);
 }
 
+static bool hw_context_enabled(struct drm_device *dev)
+{
+	return to_i915(dev)->hw_context_size;
+}
+
 int i915_gem_context_create_ioctl(struct drm_device *dev, void *data,
 				  struct drm_file *file)
 {
@@ -793,7 +751,7 @@ int i915_gem_context_create_ioctl(struct drm_device *dev, void *data,
 	struct i915_hw_context *ctx;
 	int ret;
 
-	if (!HAS_HW_CONTEXTS(dev))
+	if (!hw_context_enabled(dev))
 		return -ENODEV;
 
 	ret = i915_mutex_lock_interruptible(dev);
diff --git a/drivers/gpu/drm/i915/i915_gem_evict.c b/drivers/gpu/drm/i915/i915_gem_evict.c
index 75fca63dc8c..bbf4b12d842 100644
--- a/drivers/gpu/drm/i915/i915_gem_evict.c
+++ b/drivers/gpu/drm/i915/i915_gem_evict.c
@@ -68,9 +68,9 @@ mark_free(struct i915_vma *vma, struct list_head *unwind)
 int
 i915_gem_evict_something(struct drm_device *dev, struct i915_address_space *vm,
 			 int min_size, unsigned alignment, unsigned cache_level,
+			 unsigned long start, unsigned long end,
 			 unsigned flags)
 {
-	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct list_head eviction_list, unwind_list;
 	struct i915_vma *vma;
 	int ret = 0;
@@ -102,11 +102,10 @@ i915_gem_evict_something(struct drm_device *dev, struct i915_address_space *vm,
 	 */
 
 	INIT_LIST_HEAD(&unwind_list);
-	if (flags & PIN_MAPPABLE) {
-		BUG_ON(!i915_is_ggtt(vm));
+	if (start != 0 || end != vm->total) {
 		drm_mm_init_scan_with_range(&vm->mm, min_size,
-					    alignment, cache_level, 0,
-					    dev_priv->gtt.mappable_end);
+					    alignment, cache_level,
+					    start, end);
 	} else
 		drm_mm_init_scan(&vm->mm, min_size, alignment, cache_level);
 
diff --git a/drivers/gpu/drm/i915/i915_gem_execbuffer.c b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
index 7447160155a..20fef6c5026 100644
--- a/drivers/gpu/drm/i915/i915_gem_execbuffer.c
+++ b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
@@ -35,6 +35,9 @@
 
 #define  __EXEC_OBJECT_HAS_PIN (1<<31)
 #define  __EXEC_OBJECT_HAS_FENCE (1<<30)
+#define  __EXEC_OBJECT_NEEDS_BIAS (1<<28)
+
+#define BATCH_OFFSET_BIAS (256*1024)
 
 struct eb_vmas {
 	struct list_head vmas;
@@ -545,7 +548,7 @@ i915_gem_execbuffer_reserve_vma(struct i915_vma *vma,
 	struct drm_i915_gem_exec_object2 *entry = vma->exec_entry;
 	bool has_fenced_gpu_access = INTEL_INFO(ring->dev)->gen < 4;
 	bool need_fence;
-	unsigned flags;
+	uint64_t flags;
 	int ret;
 
 	flags = 0;
@@ -559,6 +562,8 @@ i915_gem_execbuffer_reserve_vma(struct i915_vma *vma,
 
 	if (entry->flags & EXEC_OBJECT_NEEDS_GTT)
 		flags |= PIN_GLOBAL;
+	if (entry->flags & __EXEC_OBJECT_NEEDS_BIAS)
+		flags |= BATCH_OFFSET_BIAS | PIN_OFFSET_BIAS;
 
 	ret = i915_gem_object_pin(obj, vma->vm, entry->alignment, flags);
 	if (ret)
@@ -592,6 +597,36 @@ i915_gem_execbuffer_reserve_vma(struct i915_vma *vma,
 	return 0;
 }
 
+static bool
+eb_vma_misplaced(struct i915_vma *vma, bool has_fenced_gpu_access)
+{
+	struct drm_i915_gem_exec_object2 *entry = vma->exec_entry;
+	struct drm_i915_gem_object *obj = vma->obj;
+	bool need_fence, need_mappable;
+
+	need_fence =
+		has_fenced_gpu_access &&
+		entry->flags & EXEC_OBJECT_NEEDS_FENCE &&
+		obj->tiling_mode != I915_TILING_NONE;
+	need_mappable = need_fence || need_reloc_mappable(vma);
+
+	WARN_ON((need_mappable || need_fence) &&
+	       !i915_is_ggtt(vma->vm));
+
+	if (entry->alignment &&
+	    vma->node.start & (entry->alignment - 1))
+		return true;
+
+	if (need_mappable && !obj->map_and_fenceable)
+		return true;
+
+	if (entry->flags & __EXEC_OBJECT_NEEDS_BIAS &&
+	    vma->node.start < BATCH_OFFSET_BIAS)
+		return true;
+
+	return false;
+}
+
 static int
 i915_gem_execbuffer_reserve(struct intel_ring_buffer *ring,
 			    struct list_head *vmas,
@@ -653,26 +688,10 @@ i915_gem_execbuffer_reserve(struct intel_ring_buffer *ring,
 
 		/* Unbind any ill-fitting objects or pin. */
 		list_for_each_entry(vma, vmas, exec_list) {
-			struct drm_i915_gem_exec_object2 *entry = vma->exec_entry;
-			bool need_fence, need_mappable;
-
-			obj = vma->obj;
-
 			if (!drm_mm_node_allocated(&vma->node))
 				continue;
 
-			need_fence =
-				has_fenced_gpu_access &&
-				entry->flags & EXEC_OBJECT_NEEDS_FENCE &&
-				obj->tiling_mode != I915_TILING_NONE;
-			need_mappable = need_fence || need_reloc_mappable(vma);
-
-			WARN_ON((need_mappable || need_fence) &&
-			       !i915_is_ggtt(vma->vm));
-
-			if ((entry->alignment &&
-			     vma->node.start & (entry->alignment - 1)) ||
-			    (need_mappable && !obj->map_and_fenceable))
+			if (eb_vma_misplaced(vma, has_fenced_gpu_access))
 				ret = i915_vma_unbind(vma);
 			else
 				ret = i915_gem_execbuffer_reserve_vma(vma, ring, need_relocs);
@@ -773,9 +792,9 @@ i915_gem_execbuffer_relocate_slow(struct drm_device *dev,
 		 * relocations were valid.
 		 */
 		for (j = 0; j < exec[i].relocation_count; j++) {
-			if (copy_to_user(&user_relocs[j].presumed_offset,
-					 &invalid_offset,
-					 sizeof(invalid_offset))) {
+			if (__copy_to_user(&user_relocs[j].presumed_offset,
+					   &invalid_offset,
+					   sizeof(invalid_offset))) {
 				ret = -EFAULT;
 				mutex_lock(&dev->struct_mutex);
 				goto err;
@@ -999,6 +1018,25 @@ i915_reset_gen7_sol_offsets(struct drm_device *dev,
 	return 0;
 }
 
+static struct drm_i915_gem_object *
+eb_get_batch(struct eb_vmas *eb)
+{
+	struct i915_vma *vma = list_entry(eb->vmas.prev, typeof(*vma), exec_list);
+
+	/*
+	 * SNA is doing fancy tricks with compressing batch buffers, which leads
+	 * to negative relocation deltas. Usually that works out ok since the
+	 * relocate address is still positive, except when the batch is placed
+	 * very low in the GTT. Ensure this doesn't happen.
+	 *
+	 * Note that actual hangs have only been observed on gen7, but for
+	 * paranoia do it everywhere.
+	 */
+	vma->exec_entry->flags |= __EXEC_OBJECT_NEEDS_BIAS;
+
+	return vma->obj;
+}
+
 static int
 i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 		       struct drm_file *file,
@@ -1153,7 +1191,7 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 		goto err;
 
 	/* take note of the batch buffer before we might reorder the lists */
-	batch_obj = list_entry(eb->vmas.prev, struct i915_vma, exec_list)->obj;
+	batch_obj = eb_get_batch(eb);
 
 	/* Move the objects en-masse into the GTT, evicting if necessary. */
 	need_relocs = (args->flags & I915_EXEC_NO_RELOC) == 0;
@@ -1221,7 +1259,7 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 	if (ret)
 		goto err;
 
-	ret = i915_switch_context(ring, file, ctx);
+	ret = i915_switch_context(ring, ctx);
 	if (ret)
 		goto err;
 
@@ -1355,18 +1393,21 @@ i915_gem_execbuffer(struct drm_device *dev, void *data,
 
 	ret = i915_gem_do_execbuffer(dev, data, file, &exec2, exec2_list);
 	if (!ret) {
+		struct drm_i915_gem_exec_object __user *user_exec_list =
+			to_user_ptr(args->buffers_ptr);
+
 		/* Copy the new buffer offsets back to the user's exec list. */
-		for (i = 0; i < args->buffer_count; i++)
-			exec_list[i].offset = exec2_list[i].offset;
-		/* ... and back out to userspace */
-		ret = copy_to_user(to_user_ptr(args->buffers_ptr),
-				   exec_list,
-				   sizeof(*exec_list) * args->buffer_count);
-		if (ret) {
-			ret = -EFAULT;
-			DRM_DEBUG("failed to copy %d exec entries "
-				  "back to user (%d)\n",
-				  args->buffer_count, ret);
+		for (i = 0; i < args->buffer_count; i++) {
+			ret = __copy_to_user(&user_exec_list[i].offset,
+					     &exec2_list[i].offset,
+					     sizeof(user_exec_list[i].offset));
+			if (ret) {
+				ret = -EFAULT;
+				DRM_DEBUG("failed to copy %d exec entries "
+					  "back to user (%d)\n",
+					  args->buffer_count, ret);
+				break;
+			}
 		}
 	}
 
@@ -1412,14 +1453,21 @@ i915_gem_execbuffer2(struct drm_device *dev, void *data,
 	ret = i915_gem_do_execbuffer(dev, data, file, args, exec2_list);
 	if (!ret) {
 		/* Copy the new buffer offsets back to the user's exec list. */
-		ret = copy_to_user(to_user_ptr(args->buffers_ptr),
-				   exec2_list,
-				   sizeof(*exec2_list) * args->buffer_count);
-		if (ret) {
-			ret = -EFAULT;
-			DRM_DEBUG("failed to copy %d exec entries "
-				  "back to user (%d)\n",
-				  args->buffer_count, ret);
+		struct drm_i915_gem_exec_object2 *user_exec_list =
+				   to_user_ptr(args->buffers_ptr);
+		int i;
+
+		for (i = 0; i < args->buffer_count; i++) {
+			ret = __copy_to_user(&user_exec_list[i].offset,
+					     &exec2_list[i].offset,
+					     sizeof(user_exec_list[i].offset));
+			if (ret) {
+				ret = -EFAULT;
+				DRM_DEBUG("failed to copy %d exec entries "
+					  "back to user\n",
+					  args->buffer_count);
+				break;
+			}
 		}
 	}
 
diff --git a/drivers/gpu/drm/i915/i915_gem_gtt.c b/drivers/gpu/drm/i915/i915_gem_gtt.c
index ab5e93c30aa..5deb22864c5 100644
--- a/drivers/gpu/drm/i915/i915_gem_gtt.c
+++ b/drivers/gpu/drm/i915/i915_gem_gtt.c
@@ -34,25 +34,35 @@ static void gen8_setup_private_ppat(struct drm_i915_private *dev_priv);
 
 bool intel_enable_ppgtt(struct drm_device *dev, bool full)
 {
-	if (i915.enable_ppgtt == 0 || !HAS_ALIASING_PPGTT(dev))
+	if (i915.enable_ppgtt == 0)
 		return false;
 
 	if (i915.enable_ppgtt == 1 && full)
 		return false;
 
+	return true;
+}
+
+static int sanitize_enable_ppgtt(struct drm_device *dev, int enable_ppgtt)
+{
+	if (enable_ppgtt == 0 || !HAS_ALIASING_PPGTT(dev))
+		return 0;
+
+	if (enable_ppgtt == 1)
+		return 1;
+
+	if (enable_ppgtt == 2 && HAS_PPGTT(dev))
+		return 2;
+
 #ifdef CONFIG_INTEL_IOMMU
 	/* Disable ppgtt on SNB if VT-d is on. */
 	if (INTEL_INFO(dev)->gen == 6 && intel_iommu_gfx_mapped) {
 		DRM_INFO("Disabling PPGTT because VT-d is on\n");
-		return false;
+		return 0;
 	}
 #endif
 
-	/* Full ppgtt disabled by default for now due to issues. */
-	if (full)
-		return false; /* HAS_PPGTT(dev) */
-	else
-		return HAS_ALIASING_PPGTT(dev);
+	return HAS_ALIASING_PPGTT(dev) ? 1 : 0;
 }
 
 #define GEN6_PPGTT_PD_ENTRIES 512
@@ -1079,7 +1089,9 @@ alloc:
 	if (ret == -ENOSPC && !retried) {
 		ret = i915_gem_evict_something(dev, &dev_priv->gtt.base,
 					       GEN6_PD_SIZE, GEN6_PD_ALIGN,
-					       I915_CACHE_NONE, 0);
+					       I915_CACHE_NONE,
+					       0, dev_priv->gtt.base.total,
+					       0);
 		if (ret)
 			return ret;
 
@@ -2031,6 +2043,14 @@ int i915_gem_gtt_init(struct drm_device *dev)
 		 gtt->base.total >> 20);
 	DRM_DEBUG_DRIVER("GMADR size = %ldM\n", gtt->mappable_end >> 20);
 	DRM_DEBUG_DRIVER("GTT stolen size = %zdM\n", gtt->stolen_size >> 20);
+	/*
+	 * i915.enable_ppgtt is read-only, so do an early pass to validate the
+	 * user's requested state against the hardware/driver capabilities.  We
+	 * do this now so that we can print out any log messages once rather
+	 * than every time we check intel_enable_ppgtt().
+	 */
+	i915.enable_ppgtt = sanitize_enable_ppgtt(dev, i915.enable_ppgtt);
+	DRM_DEBUG_DRIVER("ppgtt mode: %i\n", i915.enable_ppgtt);
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/i915/i915_irq.c b/drivers/gpu/drm/i915/i915_irq.c
index 5409bfafff6..0b99de95593 100644
--- a/drivers/gpu/drm/i915/i915_irq.c
+++ b/drivers/gpu/drm/i915/i915_irq.c
@@ -1362,10 +1362,20 @@ static inline void intel_hpd_irq_handler(struct drm_device *dev,
 	spin_lock(&dev_priv->irq_lock);
 	for (i = 1; i < HPD_NUM_PINS; i++) {
 
-		WARN_ONCE(hpd[i] & hotplug_trigger &&
-			  dev_priv->hpd_stats[i].hpd_mark == HPD_DISABLED,
-			  "Received HPD interrupt (0x%08x) on pin %d (0x%08x) although disabled\n",
-			  hotplug_trigger, i, hpd[i]);
+		if (hpd[i] & hotplug_trigger &&
+		    dev_priv->hpd_stats[i].hpd_mark == HPD_DISABLED) {
+			/*
+			 * On GMCH platforms the interrupt mask bits only
+			 * prevent irq generation, not the setting of the
+			 * hotplug bits itself. So only WARN about unexpected
+			 * interrupts on saner platforms.
+			 */
+			WARN_ONCE(INTEL_INFO(dev)->gen >= 5 && !IS_VALLEYVIEW(dev),
+				  "Received HPD interrupt (0x%08x) on pin %d (0x%08x) although disabled\n",
+				  hotplug_trigger, i, hpd[i]);
+
+			continue;
+		}
 
 		if (!(hpd[i] & hotplug_trigger) ||
 		    dev_priv->hpd_stats[i].hpd_mark != HPD_ENABLED)
diff --git a/drivers/gpu/drm/i915/i915_reg.h b/drivers/gpu/drm/i915/i915_reg.h
index 9f5b18d9d88..c77af69c2d8 100644
--- a/drivers/gpu/drm/i915/i915_reg.h
+++ b/drivers/gpu/drm/i915/i915_reg.h
@@ -827,6 +827,7 @@ enum punit_power_well {
 # define MI_FLUSH_ENABLE				(1 << 12)
 # define ASYNC_FLIP_PERF_DISABLE			(1 << 14)
 # define MODE_IDLE					(1 << 9)
+# define STOP_RING					(1 << 8)
 
 #define GEN6_GT_MODE	0x20d0
 #define GEN7_GT_MODE	0x7008
diff --git a/drivers/gpu/drm/i915/intel_bios.c b/drivers/gpu/drm/i915/intel_bios.c
index 4867f4cc093..aff4a113cda 100644
--- a/drivers/gpu/drm/i915/intel_bios.c
+++ b/drivers/gpu/drm/i915/intel_bios.c
@@ -287,6 +287,9 @@ parse_lfp_backlight(struct drm_i915_private *dev_priv, struct bdb_header *bdb)
 	const struct bdb_lfp_backlight_data *backlight_data;
 	const struct bdb_lfp_backlight_data_entry *entry;
 
+	/* Err to enabling backlight if no backlight block. */
+	dev_priv->vbt.backlight.present = true;
+
 	backlight_data = find_section(bdb, BDB_LVDS_BACKLIGHT);
 	if (!backlight_data)
 		return;
@@ -299,6 +302,13 @@ parse_lfp_backlight(struct drm_i915_private *dev_priv, struct bdb_header *bdb)
 
 	entry = &backlight_data->data[panel_type];
 
+	dev_priv->vbt.backlight.present = entry->type == BDB_BACKLIGHT_TYPE_PWM;
+	if (!dev_priv->vbt.backlight.present) {
+		DRM_DEBUG_KMS("PWM backlight not present in VBT (type %u)\n",
+			      entry->type);
+		return;
+	}
+
 	dev_priv->vbt.backlight.pwm_freq_hz = entry->pwm_freq_hz;
 	dev_priv->vbt.backlight.active_low_pwm = entry->active_low_pwm;
 	DRM_DEBUG_KMS("VBT backlight PWM modulation frequency %u Hz, "
@@ -550,47 +560,71 @@ parse_edp(struct drm_i915_private *dev_priv, struct bdb_header *bdb)
 
 	dev_priv->vbt.edp_pps = *edp_pps;
 
-	dev_priv->vbt.edp_rate = edp_link_params->rate ? DP_LINK_BW_2_7 :
-		DP_LINK_BW_1_62;
+	switch (edp_link_params->rate) {
+	case EDP_RATE_1_62:
+		dev_priv->vbt.edp_rate = DP_LINK_BW_1_62;
+		break;
+	case EDP_RATE_2_7:
+		dev_priv->vbt.edp_rate = DP_LINK_BW_2_7;
+		break;
+	default:
+		DRM_DEBUG_KMS("VBT has unknown eDP link rate value %u\n",
+			      edp_link_params->rate);
+		break;
+	}
+
 	switch (edp_link_params->lanes) {
-	case 0:
+	case EDP_LANE_1:
 		dev_priv->vbt.edp_lanes = 1;
 		break;
-	case 1:
+	case EDP_LANE_2:
 		dev_priv->vbt.edp_lanes = 2;
 		break;
-	case 3:
-	default:
+	case EDP_LANE_4:
 		dev_priv->vbt.edp_lanes = 4;
 		break;
+	default:
+		DRM_DEBUG_KMS("VBT has unknown eDP lane count value %u\n",
+			      edp_link_params->lanes);
+		break;
 	}
+
 	switch (edp_link_params->preemphasis) {
-	case 0:
+	case EDP_PREEMPHASIS_NONE:
 		dev_priv->vbt.edp_preemphasis = DP_TRAIN_PRE_EMPHASIS_0;
 		break;
-	case 1:
+	case EDP_PREEMPHASIS_3_5dB:
 		dev_priv->vbt.edp_preemphasis = DP_TRAIN_PRE_EMPHASIS_3_5;
 		break;
-	case 2:
+	case EDP_PREEMPHASIS_6dB:
 		dev_priv->vbt.edp_preemphasis = DP_TRAIN_PRE_EMPHASIS_6;
 		break;
-	case 3:
+	case EDP_PREEMPHASIS_9_5dB:
 		dev_priv->vbt.edp_preemphasis = DP_TRAIN_PRE_EMPHASIS_9_5;
 		break;
+	default:
+		DRM_DEBUG_KMS("VBT has unknown eDP pre-emphasis value %u\n",
+			      edp_link_params->preemphasis);
+		break;
 	}
+
 	switch (edp_link_params->vswing) {
-	case 0:
+	case EDP_VSWING_0_4V:
 		dev_priv->vbt.edp_vswing = DP_TRAIN_VOLTAGE_SWING_400;
 		break;
-	case 1:
+	case EDP_VSWING_0_6V:
 		dev_priv->vbt.edp_vswing = DP_TRAIN_VOLTAGE_SWING_600;
 		break;
-	case 2:
+	case EDP_VSWING_0_8V:
 		dev_priv->vbt.edp_vswing = DP_TRAIN_VOLTAGE_SWING_800;
 		break;
-	case 3:
+	case EDP_VSWING_1_2V:
 		dev_priv->vbt.edp_vswing = DP_TRAIN_VOLTAGE_SWING_1200;
 		break;
+	default:
+		DRM_DEBUG_KMS("VBT has unknown eDP voltage swing value %u\n",
+			      edp_link_params->vswing);
+		break;
 	}
 }
 
diff --git a/drivers/gpu/drm/i915/intel_bios.h b/drivers/gpu/drm/i915/intel_bios.h
index 83b7629e436..f27f7b28246 100644
--- a/drivers/gpu/drm/i915/intel_bios.h
+++ b/drivers/gpu/drm/i915/intel_bios.h
@@ -374,6 +374,9 @@ struct bdb_lvds_lfp_data {
 	struct bdb_lvds_lfp_data_entry data[16];
 } __packed;
 
+#define BDB_BACKLIGHT_TYPE_NONE	0
+#define BDB_BACKLIGHT_TYPE_PWM	2
+
 struct bdb_lfp_backlight_data_entry {
 	u8 type:2;
 	u8 active_low_pwm:1;
diff --git a/drivers/gpu/drm/i915/intel_display.c b/drivers/gpu/drm/i915/intel_display.c
index dae976f51d8..5b60e25baa3 100644
--- a/drivers/gpu/drm/i915/intel_display.c
+++ b/drivers/gpu/drm/i915/intel_display.c
@@ -7825,14 +7825,12 @@ static int intel_crtc_cursor_set(struct drm_crtc *crtc,
 		addr = i915_gem_obj_ggtt_offset(obj);
 	} else {
 		int align = IS_I830(dev) ? 16 * 1024 : 256;
-		ret = i915_gem_attach_phys_object(dev, obj,
-						  (intel_crtc->pipe == 0) ? I915_GEM_PHYS_CURSOR_0 : I915_GEM_PHYS_CURSOR_1,
-						  align);
+		ret = i915_gem_object_attach_phys(obj, align);
 		if (ret) {
 			DRM_DEBUG_KMS("failed to attach phys object\n");
 			goto fail_locked;
 		}
-		addr = obj->phys_obj->handle->busaddr;
+		addr = obj->phys_handle->busaddr;
 	}
 
 	if (IS_GEN2(dev))
@@ -7840,10 +7838,7 @@ static int intel_crtc_cursor_set(struct drm_crtc *crtc,
 
  finish:
 	if (intel_crtc->cursor_bo) {
-		if (INTEL_INFO(dev)->cursor_needs_physical) {
-			if (intel_crtc->cursor_bo != obj)
-				i915_gem_detach_phys_object(dev, intel_crtc->cursor_bo);
-		} else
+		if (!INTEL_INFO(dev)->cursor_needs_physical)
 			i915_gem_object_unpin_from_display_plane(intel_crtc->cursor_bo);
 		drm_gem_object_unreference(&intel_crtc->cursor_bo->base);
 	}
@@ -9654,11 +9649,22 @@ intel_pipe_config_compare(struct drm_device *dev,
 	PIPE_CONF_CHECK_I(pipe_src_w);
 	PIPE_CONF_CHECK_I(pipe_src_h);
 
-	PIPE_CONF_CHECK_I(gmch_pfit.control);
-	/* pfit ratios are autocomputed by the hw on gen4+ */
-	if (INTEL_INFO(dev)->gen < 4)
-		PIPE_CONF_CHECK_I(gmch_pfit.pgm_ratios);
-	PIPE_CONF_CHECK_I(gmch_pfit.lvds_border_bits);
+	/*
+	 * FIXME: BIOS likes to set up a cloned config with lvds+external
+	 * screen. Since we don't yet re-compute the pipe config when moving
+	 * just the lvds port away to another pipe the sw tracking won't match.
+	 *
+	 * Proper atomic modesets with recomputed global state will fix this.
+	 * Until then just don't check gmch state for inherited modes.
+	 */
+	if (!PIPE_CONF_QUIRK(PIPE_CONFIG_QUIRK_INHERITED_MODE)) {
+		PIPE_CONF_CHECK_I(gmch_pfit.control);
+		/* pfit ratios are autocomputed by the hw on gen4+ */
+		if (INTEL_INFO(dev)->gen < 4)
+			PIPE_CONF_CHECK_I(gmch_pfit.pgm_ratios);
+		PIPE_CONF_CHECK_I(gmch_pfit.lvds_border_bits);
+	}
+
 	PIPE_CONF_CHECK_I(pch_pfit.enabled);
 	if (current_config->pch_pfit.enabled) {
 		PIPE_CONF_CHECK_I(pch_pfit.pos);
@@ -11384,15 +11390,6 @@ void intel_modeset_init(struct drm_device *dev)
 	}
 }
 
-static void
-intel_connector_break_all_links(struct intel_connector *connector)
-{
-	connector->base.dpms = DRM_MODE_DPMS_OFF;
-	connector->base.encoder = NULL;
-	connector->encoder->connectors_active = false;
-	connector->encoder->base.crtc = NULL;
-}
-
 static void intel_enable_pipe_a(struct drm_device *dev)
 {
 	struct intel_connector *connector;
@@ -11474,8 +11471,17 @@ static void intel_sanitize_crtc(struct intel_crtc *crtc)
 			if (connector->encoder->base.crtc != &crtc->base)
 				continue;
 
-			intel_connector_break_all_links(connector);
+			connector->base.dpms = DRM_MODE_DPMS_OFF;
+			connector->base.encoder = NULL;
 		}
+		/* multiple connectors may have the same encoder:
+		 *  handle them and break crtc link separately */
+		list_for_each_entry(connector, &dev->mode_config.connector_list,
+				    base.head)
+			if (connector->encoder->base.crtc == &crtc->base) {
+				connector->encoder->base.crtc = NULL;
+				connector->encoder->connectors_active = false;
+			}
 
 		WARN_ON(crtc->active);
 		crtc->base.enabled = false;
@@ -11557,6 +11563,8 @@ static void intel_sanitize_encoder(struct intel_encoder *encoder)
 				      drm_get_encoder_name(&encoder->base));
 			encoder->disable(encoder);
 		}
+		encoder->base.crtc = NULL;
+		encoder->connectors_active = false;
 
 		/* Inconsistent output/port/pipe state happens presumably due to
 		 * a bug in one of the get_hw_state functions. Or someplace else
@@ -11567,8 +11575,8 @@ static void intel_sanitize_encoder(struct intel_encoder *encoder)
 				    base.head) {
 			if (connector->encoder != encoder)
 				continue;
-
-			intel_connector_break_all_links(connector);
+			connector->base.dpms = DRM_MODE_DPMS_OFF;
+			connector->base.encoder = NULL;
 		}
 	}
 	/* Enabled encoders without active connectors will be fixed in
@@ -11616,6 +11624,8 @@ static void intel_modeset_readout_hw_state(struct drm_device *dev)
 			    base.head) {
 		memset(&crtc->config, 0, sizeof(crtc->config));
 
+		crtc->config.quirks |= PIPE_CONFIG_QUIRK_INHERITED_MODE;
+
 		crtc->active = dev_priv->display.get_pipe_config(crtc,
 								 &crtc->config);
 
diff --git a/drivers/gpu/drm/i915/intel_dp.c b/drivers/gpu/drm/i915/intel_dp.c
index a0dad1a2f81..2a00cb828d2 100644
--- a/drivers/gpu/drm/i915/intel_dp.c
+++ b/drivers/gpu/drm/i915/intel_dp.c
@@ -105,7 +105,8 @@ intel_dp_max_link_bw(struct intel_dp *intel_dp)
 	case DP_LINK_BW_2_7:
 		break;
 	case DP_LINK_BW_5_4: /* 1.2 capable displays may advertise higher bw */
-		if ((IS_HASWELL(dev) || INTEL_INFO(dev)->gen >= 8) &&
+		if (((IS_HASWELL(dev) && !IS_HSW_ULX(dev)) ||
+		     INTEL_INFO(dev)->gen >= 8) &&
 		    intel_dp->dpcd[DP_DPCD_REV] >= 0x12)
 			max_link_bw = DP_LINK_BW_5_4;
 		else
@@ -120,6 +121,22 @@ intel_dp_max_link_bw(struct intel_dp *intel_dp)
 	return max_link_bw;
 }
 
+static u8 intel_dp_max_lane_count(struct intel_dp *intel_dp)
+{
+	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
+	struct drm_device *dev = intel_dig_port->base.base.dev;
+	u8 source_max, sink_max;
+
+	source_max = 4;
+	if (HAS_DDI(dev) && intel_dig_port->port == PORT_A &&
+	    (intel_dig_port->saved_port_bits & DDI_A_4_LANES) == 0)
+		source_max = 2;
+
+	sink_max = drm_dp_max_lane_count(intel_dp->dpcd);
+
+	return min(source_max, sink_max);
+}
+
 /*
  * The units on the numbers in the next two are... bizarre.  Examples will
  * make it clearer; this one parallels an example in the eDP spec.
@@ -170,7 +187,7 @@ intel_dp_mode_valid(struct drm_connector *connector,
 	}
 
 	max_link_clock = drm_dp_bw_code_to_link_rate(intel_dp_max_link_bw(intel_dp));
-	max_lanes = drm_dp_max_lane_count(intel_dp->dpcd);
+	max_lanes = intel_dp_max_lane_count(intel_dp);
 
 	max_rate = intel_dp_max_data_rate(max_link_clock, max_lanes);
 	mode_rate = intel_dp_link_required(target_clock, 18);
@@ -575,7 +592,8 @@ out:
 	return ret;
 }
 
-#define HEADER_SIZE	4
+#define BARE_ADDRESS_SIZE	3
+#define HEADER_SIZE		(BARE_ADDRESS_SIZE + 1)
 static ssize_t
 intel_dp_aux_transfer(struct drm_dp_aux *aux, struct drm_dp_aux_msg *msg)
 {
@@ -592,7 +610,7 @@ intel_dp_aux_transfer(struct drm_dp_aux *aux, struct drm_dp_aux_msg *msg)
 	switch (msg->request & ~DP_AUX_I2C_MOT) {
 	case DP_AUX_NATIVE_WRITE:
 	case DP_AUX_I2C_WRITE:
-		txsize = HEADER_SIZE + msg->size;
+		txsize = msg->size ? HEADER_SIZE + msg->size : BARE_ADDRESS_SIZE;
 		rxsize = 1;
 
 		if (WARN_ON(txsize > 20))
@@ -611,7 +629,7 @@ intel_dp_aux_transfer(struct drm_dp_aux *aux, struct drm_dp_aux_msg *msg)
 
 	case DP_AUX_NATIVE_READ:
 	case DP_AUX_I2C_READ:
-		txsize = HEADER_SIZE;
+		txsize = msg->size ? HEADER_SIZE : BARE_ADDRESS_SIZE;
 		rxsize = msg->size + 1;
 
 		if (WARN_ON(rxsize > 20))
@@ -749,8 +767,10 @@ intel_dp_compute_config(struct intel_encoder *encoder,
 	struct intel_crtc *intel_crtc = encoder->new_crtc;
 	struct intel_connector *intel_connector = intel_dp->attached_connector;
 	int lane_count, clock;
-	int max_lane_count = drm_dp_max_lane_count(intel_dp->dpcd);
+	int min_lane_count = 1;
+	int max_lane_count = intel_dp_max_lane_count(intel_dp);
 	/* Conveniently, the link BW constants become indices with a shift...*/
+	int min_clock = 0;
 	int max_clock = intel_dp_max_link_bw(intel_dp) >> 3;
 	int bpp, mode_rate;
 	static int bws[] = { DP_LINK_BW_1_62, DP_LINK_BW_2_7, DP_LINK_BW_5_4 };
@@ -783,19 +803,38 @@ intel_dp_compute_config(struct intel_encoder *encoder,
 	/* Walk through all bpp values. Luckily they're all nicely spaced with 2
 	 * bpc in between. */
 	bpp = pipe_config->pipe_bpp;
-	if (is_edp(intel_dp) && dev_priv->vbt.edp_bpp &&
-	    dev_priv->vbt.edp_bpp < bpp) {
-		DRM_DEBUG_KMS("clamping bpp for eDP panel to BIOS-provided %i\n",
-			      dev_priv->vbt.edp_bpp);
-		bpp = dev_priv->vbt.edp_bpp;
+	if (is_edp(intel_dp)) {
+		if (dev_priv->vbt.edp_bpp && dev_priv->vbt.edp_bpp < bpp) {
+			DRM_DEBUG_KMS("clamping bpp for eDP panel to BIOS-provided %i\n",
+				      dev_priv->vbt.edp_bpp);
+			bpp = dev_priv->vbt.edp_bpp;
+		}
+
+		if (IS_BROADWELL(dev)) {
+			/* Yes, it's an ugly hack. */
+			min_lane_count = max_lane_count;
+			DRM_DEBUG_KMS("forcing lane count to max (%u) on BDW\n",
+				      min_lane_count);
+		} else if (dev_priv->vbt.edp_lanes) {
+			min_lane_count = min(dev_priv->vbt.edp_lanes,
+					     max_lane_count);
+			DRM_DEBUG_KMS("using min %u lanes per VBT\n",
+				      min_lane_count);
+		}
+
+		if (dev_priv->vbt.edp_rate) {
+			min_clock = min(dev_priv->vbt.edp_rate >> 3, max_clock);
+			DRM_DEBUG_KMS("using min %02x link bw per VBT\n",
+				      bws[min_clock]);
+		}
 	}
 
 	for (; bpp >= 6*3; bpp -= 2*3) {
 		mode_rate = intel_dp_link_required(adjusted_mode->crtc_clock,
 						   bpp);
 
-		for (lane_count = 1; lane_count <= max_lane_count; lane_count <<= 1) {
-			for (clock = 0; clock <= max_clock; clock++) {
+		for (lane_count = min_lane_count; lane_count <= max_lane_count; lane_count <<= 1) {
+			for (clock = min_clock; clock <= max_clock; clock++) {
 				link_clock = drm_dp_bw_code_to_link_rate(bws[clock]);
 				link_avail = intel_dp_max_data_rate(link_clock,
 								    lane_count);
@@ -3618,7 +3657,8 @@ static bool intel_edp_init_connector(struct intel_dp *intel_dp,
 {
 	struct drm_connector *connector = &intel_connector->base;
 	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
-	struct drm_device *dev = intel_dig_port->base.base.dev;
+	struct intel_encoder *intel_encoder = &intel_dig_port->base;
+	struct drm_device *dev = intel_encoder->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct drm_display_mode *fixed_mode = NULL;
 	bool has_dpcd;
@@ -3628,6 +3668,14 @@ static bool intel_edp_init_connector(struct intel_dp *intel_dp,
 	if (!is_edp(intel_dp))
 		return true;
 
+	/* The VDD bit needs a power domain reference, so if the bit is already
+	 * enabled when we boot, grab this reference. */
+	if (edp_have_panel_vdd(intel_dp)) {
+		enum intel_display_power_domain power_domain;
+		power_domain = intel_display_port_power_domain(intel_encoder);
+		intel_display_power_get(dev_priv, power_domain);
+	}
+
 	/* Cache DPCD and EDID for edp. */
 	intel_edp_panel_vdd_on(intel_dp);
 	has_dpcd = intel_dp_get_dpcd(intel_dp);
diff --git a/drivers/gpu/drm/i915/intel_drv.h b/drivers/gpu/drm/i915/intel_drv.h
index 0542de98226..328b1a70264 100644
--- a/drivers/gpu/drm/i915/intel_drv.h
+++ b/drivers/gpu/drm/i915/intel_drv.h
@@ -236,7 +236,8 @@ struct intel_crtc_config {
 	 * tracked with quirk flags so that fastboot and state checker can act
 	 * accordingly.
 	 */
-#define PIPE_CONFIG_QUIRK_MODE_SYNC_FLAGS (1<<0) /* unreliable sync mode.flags */
+#define PIPE_CONFIG_QUIRK_MODE_SYNC_FLAGS	(1<<0) /* unreliable sync mode.flags */
+#define PIPE_CONFIG_QUIRK_INHERITED_MODE	(1<<1) /* mode inherited from firmware */
 	unsigned long quirks;
 
 	/* User requested mode, only valid as a starting point to
diff --git a/drivers/gpu/drm/i915/intel_fbdev.c b/drivers/gpu/drm/i915/intel_fbdev.c
index b4d44e62f0c..f73ba5e6b7a 100644
--- a/drivers/gpu/drm/i915/intel_fbdev.c
+++ b/drivers/gpu/drm/i915/intel_fbdev.c
@@ -132,6 +132,16 @@ static int intelfb_create(struct drm_fb_helper *helper,
 
 	mutex_lock(&dev->struct_mutex);
 
+	if (intel_fb &&
+	    (sizes->fb_width > intel_fb->base.width ||
+	     sizes->fb_height > intel_fb->base.height)) {
+		DRM_DEBUG_KMS("BIOS fb too small (%dx%d), we require (%dx%d),"
+			      " releasing it\n",
+			      intel_fb->base.width, intel_fb->base.height,
+			      sizes->fb_width, sizes->fb_height);
+		drm_framebuffer_unreference(&intel_fb->base);
+		intel_fb = ifbdev->fb = NULL;
+	}
 	if (!intel_fb || WARN_ON(!intel_fb->obj)) {
 		DRM_DEBUG_KMS("no BIOS fb, allocating a new one\n");
 		ret = intelfb_alloc(helper, sizes);
@@ -377,6 +387,15 @@ static bool intel_fb_initial_config(struct drm_fb_helper *fb_helper,
 							  height);
 		}
 
+		/* No preferred mode marked by the EDID? Are there any modes? */
+		if (!modes[i] && !list_empty(&connector->modes)) {
+			DRM_DEBUG_KMS("using first mode listed on connector %s\n",
+				      drm_get_connector_name(connector));
+			modes[i] = list_first_entry(&connector->modes,
+						    struct drm_display_mode,
+						    head);
+		}
+
 		/* last resort: use current mode */
 		if (!modes[i]) {
 			/*
diff --git a/drivers/gpu/drm/i915/intel_hdmi.c b/drivers/gpu/drm/i915/intel_hdmi.c
index b0413e19062..157267aa356 100644
--- a/drivers/gpu/drm/i915/intel_hdmi.c
+++ b/drivers/gpu/drm/i915/intel_hdmi.c
@@ -821,11 +821,11 @@ static void intel_disable_hdmi(struct intel_encoder *encoder)
 	}
 }
 
-static int hdmi_portclock_limit(struct intel_hdmi *hdmi)
+static int hdmi_portclock_limit(struct intel_hdmi *hdmi, bool respect_dvi_limit)
 {
 	struct drm_device *dev = intel_hdmi_to_dev(hdmi);
 
-	if (!hdmi->has_hdmi_sink || IS_G4X(dev))
+	if ((respect_dvi_limit && !hdmi->has_hdmi_sink) || IS_G4X(dev))
 		return 165000;
 	else if (IS_HASWELL(dev) || INTEL_INFO(dev)->gen >= 8)
 		return 300000;
@@ -837,7 +837,8 @@ static enum drm_mode_status
 intel_hdmi_mode_valid(struct drm_connector *connector,
 		      struct drm_display_mode *mode)
 {
-	if (mode->clock > hdmi_portclock_limit(intel_attached_hdmi(connector)))
+	if (mode->clock > hdmi_portclock_limit(intel_attached_hdmi(connector),
+					       true))
 		return MODE_CLOCK_HIGH;
 	if (mode->clock < 20000)
 		return MODE_CLOCK_LOW;
@@ -879,7 +880,7 @@ bool intel_hdmi_compute_config(struct intel_encoder *encoder,
 	struct drm_device *dev = encoder->base.dev;
 	struct drm_display_mode *adjusted_mode = &pipe_config->adjusted_mode;
 	int clock_12bpc = pipe_config->adjusted_mode.crtc_clock * 3 / 2;
-	int portclock_limit = hdmi_portclock_limit(intel_hdmi);
+	int portclock_limit = hdmi_portclock_limit(intel_hdmi, false);
 	int desired_bpp;
 
 	if (intel_hdmi->color_range_auto) {
diff --git a/drivers/gpu/drm/i915/intel_overlay.c b/drivers/gpu/drm/i915/intel_overlay.c
index d8adc9104dc..129db0c7d83 100644
--- a/drivers/gpu/drm/i915/intel_overlay.c
+++ b/drivers/gpu/drm/i915/intel_overlay.c
@@ -193,7 +193,7 @@ intel_overlay_map_regs(struct intel_overlay *overlay)
 	struct overlay_registers __iomem *regs;
 
 	if (OVERLAY_NEEDS_PHYSICAL(overlay->dev))
-		regs = (struct overlay_registers __iomem *)overlay->reg_bo->phys_obj->handle->vaddr;
+		regs = (struct overlay_registers __iomem *)overlay->reg_bo->phys_handle->vaddr;
 	else
 		regs = io_mapping_map_wc(dev_priv->gtt.mappable,
 					 i915_gem_obj_ggtt_offset(overlay->reg_bo));
@@ -1340,14 +1340,12 @@ void intel_setup_overlay(struct drm_device *dev)
 	overlay->reg_bo = reg_bo;
 
 	if (OVERLAY_NEEDS_PHYSICAL(dev)) {
-		ret = i915_gem_attach_phys_object(dev, reg_bo,
-						  I915_GEM_PHYS_OVERLAY_REGS,
-						  PAGE_SIZE);
+		ret = i915_gem_object_attach_phys(reg_bo, PAGE_SIZE);
 		if (ret) {
 			DRM_ERROR("failed to attach phys overlay regs\n");
 			goto out_free_bo;
 		}
-		overlay->flip_addr = reg_bo->phys_obj->handle->busaddr;
+		overlay->flip_addr = reg_bo->phys_handle->busaddr;
 	} else {
 		ret = i915_gem_obj_ggtt_pin(reg_bo, PAGE_SIZE, PIN_MAPPABLE);
 		if (ret) {
@@ -1428,7 +1426,7 @@ intel_overlay_map_regs_atomic(struct intel_overlay *overlay)
 		/* Cast to make sparse happy, but it's wc memory anyway, so
 		 * equivalent to the wc io mapping on X86. */
 		regs = (struct overlay_registers __iomem *)
-			overlay->reg_bo->phys_obj->handle->vaddr;
+			overlay->reg_bo->phys_handle->vaddr;
 	else
 		regs = io_mapping_map_atomic_wc(dev_priv->gtt.mappable,
 						i915_gem_obj_ggtt_offset(overlay->reg_bo));
@@ -1462,7 +1460,7 @@ intel_overlay_capture_error_state(struct drm_device *dev)
 	error->dovsta = I915_READ(DOVSTA);
 	error->isr = I915_READ(ISR);
 	if (OVERLAY_NEEDS_PHYSICAL(overlay->dev))
-		error->base = (__force long)overlay->reg_bo->phys_obj->handle->vaddr;
+		error->base = (__force long)overlay->reg_bo->phys_handle->vaddr;
 	else
 		error->base = i915_gem_obj_ggtt_offset(overlay->reg_bo);
 
diff --git a/drivers/gpu/drm/i915/intel_panel.c b/drivers/gpu/drm/i915/intel_panel.c
index cb058408c70..cb8cfb7e097 100644
--- a/drivers/gpu/drm/i915/intel_panel.c
+++ b/drivers/gpu/drm/i915/intel_panel.c
@@ -492,6 +492,7 @@ void intel_panel_set_backlight(struct intel_connector *connector, u32 level,
 	enum pipe pipe = intel_get_pipe_from_connector(connector);
 	u32 freq;
 	unsigned long flags;
+	u64 n;
 
 	if (!panel->backlight.present || pipe == INVALID_PIPE)
 		return;
@@ -502,10 +503,9 @@ void intel_panel_set_backlight(struct intel_connector *connector, u32 level,
 
 	/* scale to hardware max, but be careful to not overflow */
 	freq = panel->backlight.max;
-	if (freq < max)
-		level = level * freq / max;
-	else
-		level = freq / max * level;
+	n = (u64)level * freq;
+	do_div(n, max);
+	level = n;
 
 	panel->backlight.level = level;
 	if (panel->backlight.device)
@@ -1065,6 +1065,11 @@ int intel_panel_setup_backlight(struct drm_connector *connector)
 	unsigned long flags;
 	int ret;
 
+	if (!dev_priv->vbt.backlight.present) {
+		DRM_DEBUG_KMS("native backlight control not available per VBT\n");
+		return 0;
+	}
+
 	/* set level and max in panel struct */
 	spin_lock_irqsave(&dev_priv->backlight_lock, flags);
 	ret = dev_priv->display.setup_backlight(intel_connector);
diff --git a/drivers/gpu/drm/i915/intel_pm.c b/drivers/gpu/drm/i915/intel_pm.c
index 5874716774a..d93dcf683e8 100644
--- a/drivers/gpu/drm/i915/intel_pm.c
+++ b/drivers/gpu/drm/i915/intel_pm.c
@@ -1545,6 +1545,16 @@ static void i9xx_update_wm(struct drm_crtc *unused_crtc)
 
 	DRM_DEBUG_KMS("FIFO watermarks - A: %d, B: %d\n", planea_wm, planeb_wm);
 
+	if (IS_I915GM(dev) && enabled) {
+		struct intel_framebuffer *fb;
+
+		fb = to_intel_framebuffer(enabled->primary->fb);
+
+		/* self-refresh seems busted with untiled */
+		if (fb->obj->tiling_mode == I915_TILING_NONE)
+			enabled = NULL;
+	}
+
 	/*
 	 * Overlay gets an aggressive default since video jitter is bad.
 	 */
@@ -2085,6 +2095,43 @@ static void intel_print_wm_latency(struct drm_device *dev,
 	}
 }
 
+static bool ilk_increase_wm_latency(struct drm_i915_private *dev_priv,
+				    uint16_t wm[5], uint16_t min)
+{
+	int level, max_level = ilk_wm_max_level(dev_priv->dev);
+
+	if (wm[0] >= min)
+		return false;
+
+	wm[0] = max(wm[0], min);
+	for (level = 1; level <= max_level; level++)
+		wm[level] = max_t(uint16_t, wm[level], DIV_ROUND_UP(min, 5));
+
+	return true;
+}
+
+static void snb_wm_latency_quirk(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	bool changed;
+
+	/*
+	 * The BIOS provided WM memory latency values are often
+	 * inadequate for high resolution displays. Adjust them.
+	 */
+	changed = ilk_increase_wm_latency(dev_priv, dev_priv->wm.pri_latency, 12) |
+		ilk_increase_wm_latency(dev_priv, dev_priv->wm.spr_latency, 12) |
+		ilk_increase_wm_latency(dev_priv, dev_priv->wm.cur_latency, 12);
+
+	if (!changed)
+		return;
+
+	DRM_DEBUG_KMS("WM latency values increased to avoid potential underruns\n");
+	intel_print_wm_latency(dev, "Primary", dev_priv->wm.pri_latency);
+	intel_print_wm_latency(dev, "Sprite", dev_priv->wm.spr_latency);
+	intel_print_wm_latency(dev, "Cursor", dev_priv->wm.cur_latency);
+}
+
 static void ilk_setup_wm_latency(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -2102,6 +2149,9 @@ static void ilk_setup_wm_latency(struct drm_device *dev)
 	intel_print_wm_latency(dev, "Primary", dev_priv->wm.pri_latency);
 	intel_print_wm_latency(dev, "Sprite", dev_priv->wm.spr_latency);
 	intel_print_wm_latency(dev, "Cursor", dev_priv->wm.cur_latency);
+
+	if (IS_GEN6(dev))
+		snb_wm_latency_quirk(dev);
 }
 
 static void ilk_compute_wm_parameters(struct drm_crtc *crtc,
diff --git a/drivers/gpu/drm/i915/intel_ringbuffer.c b/drivers/gpu/drm/i915/intel_ringbuffer.c
index 6bc68bdcf43..79fb4cc2137 100644
--- a/drivers/gpu/drm/i915/intel_ringbuffer.c
+++ b/drivers/gpu/drm/i915/intel_ringbuffer.c
@@ -437,32 +437,41 @@ static void ring_setup_phys_status_page(struct intel_ring_buffer *ring)
 	I915_WRITE(HWS_PGA, addr);
 }
 
-static int init_ring_common(struct intel_ring_buffer *ring)
+static bool stop_ring(struct intel_ring_buffer *ring)
 {
-	struct drm_device *dev = ring->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_i915_gem_object *obj = ring->obj;
-	int ret = 0;
-	u32 head;
+	struct drm_i915_private *dev_priv = to_i915(ring->dev);
 
-	gen6_gt_force_wake_get(dev_priv, FORCEWAKE_ALL);
+	if (!IS_GEN2(ring->dev)) {
+		I915_WRITE_MODE(ring, _MASKED_BIT_ENABLE(STOP_RING));
+		if (wait_for_atomic((I915_READ_MODE(ring) & MODE_IDLE) != 0, 1000)) {
+			DRM_ERROR("%s :timed out trying to stop ring\n", ring->name);
+			return false;
+		}
+	}
 
-	/* Stop the ring if it's running. */
 	I915_WRITE_CTL(ring, 0);
 	I915_WRITE_HEAD(ring, 0);
 	ring->write_tail(ring, 0);
-	if (wait_for_atomic((I915_READ_MODE(ring) & MODE_IDLE) != 0, 1000))
-		DRM_ERROR("%s :timed out trying to stop ring\n", ring->name);
 
-	if (I915_NEED_GFX_HWS(dev))
-		intel_ring_setup_status_page(ring);
-	else
-		ring_setup_phys_status_page(ring);
+	if (!IS_GEN2(ring->dev)) {
+		(void)I915_READ_CTL(ring);
+		I915_WRITE_MODE(ring, _MASKED_BIT_DISABLE(STOP_RING));
+	}
 
-	head = I915_READ_HEAD(ring) & HEAD_ADDR;
+	return (I915_READ_HEAD(ring) & HEAD_ADDR) == 0;
+}
 
-	/* G45 ring initialization fails to reset head to zero */
-	if (head != 0) {
+static int init_ring_common(struct intel_ring_buffer *ring)
+{
+	struct drm_device *dev = ring->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_gem_object *obj = ring->obj;
+	int ret = 0;
+
+	gen6_gt_force_wake_get(dev_priv, FORCEWAKE_ALL);
+
+	if (!stop_ring(ring)) {
+		/* G45 ring initialization often fails to reset head to zero */
 		DRM_DEBUG_KMS("%s head not reset to zero "
 			      "ctl %08x head %08x tail %08x start %08x\n",
 			      ring->name,
@@ -471,9 +480,7 @@ static int init_ring_common(struct intel_ring_buffer *ring)
 			      I915_READ_TAIL(ring),
 			      I915_READ_START(ring));
 
-		I915_WRITE_HEAD(ring, 0);
-
-		if (I915_READ_HEAD(ring) & HEAD_ADDR) {
+		if (!stop_ring(ring)) {
 			DRM_ERROR("failed to set %s head to zero "
 				  "ctl %08x head %08x tail %08x start %08x\n",
 				  ring->name,
@@ -481,9 +488,16 @@ static int init_ring_common(struct intel_ring_buffer *ring)
 				  I915_READ_HEAD(ring),
 				  I915_READ_TAIL(ring),
 				  I915_READ_START(ring));
+			ret = -EIO;
+			goto out;
 		}
 	}
 
+	if (I915_NEED_GFX_HWS(dev))
+		intel_ring_setup_status_page(ring);
+	else
+		ring_setup_phys_status_page(ring);
+
 	/* Initialize the ring. This must happen _after_ we've cleared the ring
 	 * registers with the above sequence (the readback of the HEAD registers
 	 * also enforces ordering), otherwise the hw might lose the new ring
diff --git a/drivers/gpu/drm/i915/intel_ringbuffer.h b/drivers/gpu/drm/i915/intel_ringbuffer.h
index 270a6a97343..2b91c4b4d34 100644
--- a/drivers/gpu/drm/i915/intel_ringbuffer.h
+++ b/drivers/gpu/drm/i915/intel_ringbuffer.h
@@ -34,6 +34,7 @@ struct  intel_hw_status_page {
 #define I915_WRITE_IMR(ring, val) I915_WRITE(RING_IMR((ring)->mmio_base), val)
 
 #define I915_READ_MODE(ring) I915_READ(RING_MI_MODE((ring)->mmio_base))
+#define I915_WRITE_MODE(ring, val) I915_WRITE(RING_MI_MODE((ring)->mmio_base), val)
 
 enum intel_ring_hangcheck_action {
 	HANGCHECK_IDLE = 0,
diff --git a/drivers/gpu/drm/i915/intel_sdvo.c b/drivers/gpu/drm/i915/intel_sdvo.c
index d27155adf5d..46be00d66df 100644
--- a/drivers/gpu/drm/i915/intel_sdvo.c
+++ b/drivers/gpu/drm/i915/intel_sdvo.c
@@ -2424,8 +2424,8 @@ intel_sdvo_connector_init(struct intel_sdvo_connector *connector,
 	if (ret < 0)
 		goto err1;
 
-	ret = sysfs_create_link(&encoder->ddc.dev.kobj,
-				&drm_connector->kdev->kobj,
+	ret = sysfs_create_link(&drm_connector->kdev->kobj,
+				&encoder->ddc.dev.kobj,
 				encoder->ddc.dev.kobj.name);
 	if (ret < 0)
 		goto err2;
diff --git a/drivers/gpu/drm/i915/intel_uncore.c b/drivers/gpu/drm/i915/intel_uncore.c
index f729dc71d5b..d0c75779d3f 100644
--- a/drivers/gpu/drm/i915/intel_uncore.c
+++ b/drivers/gpu/drm/i915/intel_uncore.c
@@ -185,6 +185,8 @@ static void vlv_force_wake_reset(struct drm_i915_private *dev_priv)
 {
 	__raw_i915_write32(dev_priv, FORCEWAKE_VLV,
 			   _MASKED_BIT_DISABLE(0xffff));
+	__raw_i915_write32(dev_priv, FORCEWAKE_MEDIA_VLV,
+			   _MASKED_BIT_DISABLE(0xffff));
 	/* something from same cacheline, but !FORCEWAKE_VLV */
 	__raw_posting_read(dev_priv, FORCEWAKE_ACK_VLV);
 }
diff --git a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_crtc.c b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_crtc.c
index 3e6c0f3ed59..ef9957dbac9 100644
--- a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_crtc.c
+++ b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_crtc.c
@@ -510,9 +510,8 @@ static void update_cursor(struct drm_crtc *crtc)
 					MDP4_DMA_CURSOR_BLEND_CONFIG_CURSOR_EN);
 		} else {
 			/* disable cursor: */
-			mdp4_write(mdp4_kms, REG_MDP4_DMA_CURSOR_BASE(dma), 0);
-			mdp4_write(mdp4_kms, REG_MDP4_DMA_CURSOR_BLEND_CONFIG(dma),
-					MDP4_DMA_CURSOR_BLEND_CONFIG_FORMAT(CURSOR_ARGB));
+			mdp4_write(mdp4_kms, REG_MDP4_DMA_CURSOR_BASE(dma),
+					mdp4_kms->blank_cursor_iova);
 		}
 
 		/* and drop the iova ref + obj rev when done scanning out: */
@@ -574,11 +573,9 @@ static int mdp4_crtc_cursor_set(struct drm_crtc *crtc,
 
 	if (old_bo) {
 		/* drop our previous reference: */
-		msm_gem_put_iova(old_bo, mdp4_kms->id);
-		drm_gem_object_unreference_unlocked(old_bo);
+		drm_flip_work_queue(&mdp4_crtc->unref_cursor_work, old_bo);
 	}
 
-	crtc_flush(crtc);
 	request_pending(crtc, PENDING_CURSOR);
 
 	return 0;
diff --git a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_irq.c b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_irq.c
index c740ccd1cc6..8edd531cb62 100644
--- a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_irq.c
+++ b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_irq.c
@@ -70,12 +70,12 @@ irqreturn_t mdp4_irq(struct msm_kms *kms)
 
 	VERB("status=%08x", status);
 
+	mdp_dispatch_irqs(mdp_kms, status);
+
 	for (id = 0; id < priv->num_crtcs; id++)
 		if (status & mdp4_crtc_vblank(priv->crtcs[id]))
 			drm_handle_vblank(dev, id);
 
-	mdp_dispatch_irqs(mdp_kms, status);
-
 	return IRQ_HANDLED;
 }
 
diff --git a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.c b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.c
index 272e707c948..0bb4faa1752 100644
--- a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.c
+++ b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.c
@@ -144,6 +144,10 @@ static void mdp4_preclose(struct msm_kms *kms, struct drm_file *file)
 static void mdp4_destroy(struct msm_kms *kms)
 {
 	struct mdp4_kms *mdp4_kms = to_mdp4_kms(to_mdp_kms(kms));
+	if (mdp4_kms->blank_cursor_iova)
+		msm_gem_put_iova(mdp4_kms->blank_cursor_bo, mdp4_kms->id);
+	if (mdp4_kms->blank_cursor_bo)
+		drm_gem_object_unreference(mdp4_kms->blank_cursor_bo);
 	kfree(mdp4_kms);
 }
 
@@ -372,6 +376,23 @@ struct msm_kms *mdp4_kms_init(struct drm_device *dev)
 		goto fail;
 	}
 
+	mutex_lock(&dev->struct_mutex);
+	mdp4_kms->blank_cursor_bo = msm_gem_new(dev, SZ_16K, MSM_BO_WC);
+	mutex_unlock(&dev->struct_mutex);
+	if (IS_ERR(mdp4_kms->blank_cursor_bo)) {
+		ret = PTR_ERR(mdp4_kms->blank_cursor_bo);
+		dev_err(dev->dev, "could not allocate blank-cursor bo: %d\n", ret);
+		mdp4_kms->blank_cursor_bo = NULL;
+		goto fail;
+	}
+
+	ret = msm_gem_get_iova(mdp4_kms->blank_cursor_bo, mdp4_kms->id,
+			&mdp4_kms->blank_cursor_iova);
+	if (ret) {
+		dev_err(dev->dev, "could not pin blank-cursor bo: %d\n", ret);
+		goto fail;
+	}
+
 	return kms;
 
 fail:
diff --git a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.h b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.h
index 66a4d31aec8..715520c54cd 100644
--- a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.h
+++ b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.h
@@ -44,6 +44,10 @@ struct mdp4_kms {
 	struct clk *lut_clk;
 
 	struct mdp_irq error_handler;
+
+	/* empty/blank cursor bo to use when cursor is "disabled" */
+	struct drm_gem_object *blank_cursor_bo;
+	uint32_t blank_cursor_iova;
 };
 #define to_mdp4_kms(x) container_of(x, struct mdp4_kms, base)
 
diff --git a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_irq.c b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_irq.c
index 353d494a497..f2b985bc2ad 100644
--- a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_irq.c
+++ b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_irq.c
@@ -71,11 +71,11 @@ static void mdp5_irq_mdp(struct mdp_kms *mdp_kms)
 
 	VERB("status=%08x", status);
 
+	mdp_dispatch_irqs(mdp_kms, status);
+
 	for (id = 0; id < priv->num_crtcs; id++)
 		if (status & mdp5_crtc_vblank(priv->crtcs[id]))
 			drm_handle_vblank(dev, id);
-
-	mdp_dispatch_irqs(mdp_kms, status);
 }
 
 irqreturn_t mdp5_irq(struct msm_kms *kms)
diff --git a/drivers/gpu/drm/msm/msm_fbdev.c b/drivers/gpu/drm/msm/msm_fbdev.c
index 6c6d7d4c9b4..a752ab83b81 100644
--- a/drivers/gpu/drm/msm/msm_fbdev.c
+++ b/drivers/gpu/drm/msm/msm_fbdev.c
@@ -62,11 +62,8 @@ static int msm_fbdev_create(struct drm_fb_helper *helper,
 	dma_addr_t paddr;
 	int ret, size;
 
-	/* only doing ARGB32 since this is what is needed to alpha-blend
-	 * with video overlays:
-	 */
 	sizes->surface_bpp = 32;
-	sizes->surface_depth = 32;
+	sizes->surface_depth = 24;
 
 	DBG("create fbdev: %dx%d@%d (%dx%d)", sizes->surface_width,
 			sizes->surface_height, sizes->surface_bpp,
diff --git a/drivers/gpu/drm/msm/msm_gem.c b/drivers/gpu/drm/msm/msm_gem.c
index 3da8264d303..bb8026daebc 100644
--- a/drivers/gpu/drm/msm/msm_gem.c
+++ b/drivers/gpu/drm/msm/msm_gem.c
@@ -118,8 +118,10 @@ static void put_pages(struct drm_gem_object *obj)
 
 		if (iommu_present(&platform_bus_type))
 			drm_gem_put_pages(obj, msm_obj->pages, true, false);
-		else
+		else {
 			drm_mm_remove_node(msm_obj->vram_node);
+			drm_free_large(msm_obj->pages);
+		}
 
 		msm_obj->pages = NULL;
 	}
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/nvd0.c b/drivers/gpu/drm/nouveau/core/engine/disp/nvd0.c
index 7762665ad8f..876de9ac379 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/nvd0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/nvd0.c
@@ -1009,7 +1009,7 @@ exec_clkcmp(struct nv50_disp_priv *priv, int head, int id,
 	}
 
 	if (outp == 8)
-		return false;
+		return conf;
 
 	data = exec_lookup(priv, head, outp, ctrl, dcb, &ver, &hdr, &cnt, &len, &info1);
 	if (data == 0x0000)
diff --git a/drivers/gpu/drm/nouveau/core/engine/graph/ctxgm107.c b/drivers/gpu/drm/nouveau/core/engine/graph/ctxgm107.c
index 1dc37b1ddbf..b0d0fb2f4d0 100644
--- a/drivers/gpu/drm/nouveau/core/engine/graph/ctxgm107.c
+++ b/drivers/gpu/drm/nouveau/core/engine/graph/ctxgm107.c
@@ -863,7 +863,7 @@ gm107_grctx_generate_mods(struct nvc0_graph_priv *priv, struct nvc0_grctx *info)
 {
 	mmio_data(0x003000, 0x0100, NV_MEM_ACCESS_RW | NV_MEM_ACCESS_SYS);
 	mmio_data(0x008000, 0x0100, NV_MEM_ACCESS_RW | NV_MEM_ACCESS_SYS);
-	mmio_data(0x060000, 0x1000, NV_MEM_ACCESS_RW);
+	mmio_data(0x200000, 0x1000, NV_MEM_ACCESS_RW);
 
 	mmio_list(0x40800c, 0x00000000,  8, 1);
 	mmio_list(0x408010, 0x80000000,  0, 0);
@@ -877,6 +877,8 @@ gm107_grctx_generate_mods(struct nvc0_graph_priv *priv, struct nvc0_grctx *info)
 	mmio_list(0x418e24, 0x00000000,  8, 0);
 	mmio_list(0x418e28, 0x80000030,  0, 0);
 
+	mmio_list(0x4064c8, 0x018002c0,  0, 0);
+
 	mmio_list(0x418810, 0x80000000, 12, 2);
 	mmio_list(0x419848, 0x10000000, 12, 2);
 	mmio_list(0x419c2c, 0x10000000, 12, 2);
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/base.c b/drivers/gpu/drm/nouveau/core/subdev/bios/base.c
index e9df94f96d7..222e8ebb669 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/bios/base.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/base.c
@@ -109,7 +109,7 @@ nouveau_bios_shadow_pramin(struct nouveau_bios *bios)
 			return;
 		}
 
-		addr = (u64)(addr >> 8) << 8;
+		addr = (addr & 0xffffff00) << 8;
 		if (!addr) {
 			addr  = (u64)nv_rd32(bios, 0x001700) << 16;
 			addr += 0xf0000;
@@ -168,7 +168,8 @@ nouveau_bios_shadow_prom(struct nouveau_bios *bios)
 	 */
 	i = 16;
 	do {
-		if ((nv_rd32(bios, 0x300000) & 0xffff) == 0xaa55)
+		u32 data = le32_to_cpu(nv_rd32(bios, 0x300000)) & 0xffff;
+		if (data == 0xaa55)
 			break;
 	} while (i--);
 
@@ -176,14 +177,15 @@ nouveau_bios_shadow_prom(struct nouveau_bios *bios)
 		goto out;
 
 	/* read entire bios image to system memory */
-	bios->size = ((nv_rd32(bios, 0x300000) >> 16) & 0xff) * 512;
+	bios->size = (le32_to_cpu(nv_rd32(bios, 0x300000)) >> 16) & 0xff;
+	bios->size = bios->size * 512;
 	if (!bios->size)
 		goto out;
 
 	bios->data = kmalloc(bios->size, GFP_KERNEL);
 	if (bios->data) {
-		for (i = 0; i < bios->size; i+=4)
-			nv_wo32(bios, i, nv_rd32(bios, 0x300000 + i));
+		for (i = 0; i < bios->size; i += 4)
+			((u32 *)bios->data)[i/4] = nv_rd32(bios, 0x300000 + i);
 	}
 
 	/* check the PCI record header */
diff --git a/drivers/gpu/drm/nouveau/core/subdev/therm/nvd0.c b/drivers/gpu/drm/nouveau/core/subdev/therm/nvd0.c
index 43fec17ea54..bbf117be572 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/therm/nvd0.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/therm/nvd0.c
@@ -40,6 +40,7 @@ pwm_info(struct nouveau_therm *therm, int line)
 		case 0x00: return 2;
 		case 0x19: return 1;
 		case 0x1c: return 0;
+		case 0x1e: return 2;
 		default:
 			break;
 		}
diff --git a/drivers/gpu/drm/nouveau/nouveau_acpi.c b/drivers/gpu/drm/nouveau/nouveau_acpi.c
index 83face3f608..279206997e5 100644
--- a/drivers/gpu/drm/nouveau/nouveau_acpi.c
+++ b/drivers/gpu/drm/nouveau/nouveau_acpi.c
@@ -389,9 +389,6 @@ bool nouveau_acpi_rom_supported(struct pci_dev *pdev)
 	acpi_status status;
 	acpi_handle dhandle, rom_handle;
 
-	if (!nouveau_dsm_priv.dsm_detected && !nouveau_dsm_priv.optimus_detected)
-		return false;
-
 	dhandle = ACPI_HANDLE(&pdev->dev);
 	if (!dhandle)
 		return false;
diff --git a/drivers/gpu/drm/nouveau/nouveau_display.c b/drivers/gpu/drm/nouveau/nouveau_display.c
index 3ff030dc1ee..da764a4ed95 100644
--- a/drivers/gpu/drm/nouveau/nouveau_display.c
+++ b/drivers/gpu/drm/nouveau/nouveau_display.c
@@ -764,9 +764,9 @@ nouveau_crtc_page_flip(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 	}
 
 	ret = nouveau_page_flip_emit(chan, old_bo, new_bo, s, &fence);
-	mutex_unlock(&chan->cli->mutex);
 	if (ret)
 		goto fail_unreserve;
+	mutex_unlock(&chan->cli->mutex);
 
 	/* Update the crtc struct and cleanup */
 	crtc->primary->fb = fb;
diff --git a/drivers/gpu/drm/omapdrm/omap_crtc.c b/drivers/gpu/drm/omapdrm/omap_crtc.c
index 355157e4f78..e3c47a8005f 100644
--- a/drivers/gpu/drm/omapdrm/omap_crtc.c
+++ b/drivers/gpu/drm/omapdrm/omap_crtc.c
@@ -33,6 +33,7 @@ struct omap_crtc {
 	int pipe;
 	enum omap_channel channel;
 	struct omap_overlay_manager_info info;
+	struct drm_encoder *current_encoder;
 
 	/*
 	 * Temporary: eventually this will go away, but it is needed
@@ -120,13 +121,25 @@ static void omap_crtc_start_update(struct omap_overlay_manager *mgr)
 {
 }
 
+static void set_enabled(struct drm_crtc *crtc, bool enable);
+
 static int omap_crtc_enable(struct omap_overlay_manager *mgr)
 {
+	struct omap_crtc *omap_crtc = omap_crtcs[mgr->id];
+
+	dispc_mgr_setup(omap_crtc->channel, &omap_crtc->info);
+	dispc_mgr_set_timings(omap_crtc->channel,
+			&omap_crtc->timings);
+	set_enabled(&omap_crtc->base, true);
+
 	return 0;
 }
 
 static void omap_crtc_disable(struct omap_overlay_manager *mgr)
 {
+	struct omap_crtc *omap_crtc = omap_crtcs[mgr->id];
+
+	set_enabled(&omap_crtc->base, false);
 }
 
 static void omap_crtc_set_timings(struct omap_overlay_manager *mgr,
@@ -184,7 +197,6 @@ static void omap_crtc_destroy(struct drm_crtc *crtc)
 	WARN_ON(omap_crtc->apply_irq.registered);
 	omap_irq_unregister(crtc->dev, &omap_crtc->error_irq);
 
-	omap_crtc->plane->funcs->destroy(omap_crtc->plane);
 	drm_crtc_cleanup(crtc);
 
 	kfree(omap_crtc);
@@ -338,17 +350,23 @@ static int omap_crtc_page_flip_locked(struct drm_crtc *crtc,
 	struct omap_crtc *omap_crtc = to_omap_crtc(crtc);
 	struct drm_plane *primary = crtc->primary;
 	struct drm_gem_object *bo;
+	unsigned long flags;
 
 	DBG("%d -> %d (event=%p)", primary->fb ? primary->fb->base.id : -1,
 			fb->base.id, event);
 
+	spin_lock_irqsave(&dev->event_lock, flags);
+
 	if (omap_crtc->old_fb) {
+		spin_unlock_irqrestore(&dev->event_lock, flags);
 		dev_err(dev->dev, "already a pending flip\n");
 		return -EINVAL;
 	}
 
 	omap_crtc->event = event;
-	primary->fb = fb;
+	omap_crtc->old_fb = primary->fb = fb;
+
+	spin_unlock_irqrestore(&dev->event_lock, flags);
 
 	/*
 	 * Hold a reference temporarily until the crtc is updated
@@ -528,38 +546,46 @@ static void set_enabled(struct drm_crtc *crtc, bool enable)
 	struct drm_device *dev = crtc->dev;
 	struct omap_crtc *omap_crtc = to_omap_crtc(crtc);
 	enum omap_channel channel = omap_crtc->channel;
-	struct omap_irq_wait *wait = NULL;
+	struct omap_irq_wait *wait;
+	u32 framedone_irq, vsync_irq;
+	int ret;
 
 	if (dispc_mgr_is_enabled(channel) == enable)
 		return;
 
-	/* ignore sync-lost irqs during enable/disable */
+	/*
+	 * Digit output produces some sync lost interrupts during the first
+	 * frame when enabling, so we need to ignore those.
+	 */
 	omap_irq_unregister(crtc->dev, &omap_crtc->error_irq);
 
-	if (dispc_mgr_get_framedone_irq(channel)) {
-		if (!enable) {
-			wait = omap_irq_wait_init(dev,
-					dispc_mgr_get_framedone_irq(channel), 1);
-		}
+	framedone_irq = dispc_mgr_get_framedone_irq(channel);
+	vsync_irq = dispc_mgr_get_vsync_irq(channel);
+
+	if (enable) {
+		wait = omap_irq_wait_init(dev, vsync_irq, 1);
 	} else {
 		/*
-		 * When we disable digit output, we need to wait until fields
-		 * are done.  Otherwise the DSS is still working, and turning
-		 * off the clocks prevents DSS from going to OFF mode. And when
-		 * enabling, we need to wait for the extra sync losts
+		 * When we disable the digit output, we need to wait for
+		 * FRAMEDONE to know that DISPC has finished with the output.
+		 *
+		 * OMAP2/3 does not have FRAMEDONE irq for digit output, and in
+		 * that case we need to use vsync interrupt, and wait for both
+		 * even and odd frames.
 		 */
-		wait = omap_irq_wait_init(dev,
-				dispc_mgr_get_vsync_irq(channel), 2);
+
+		if (framedone_irq)
+			wait = omap_irq_wait_init(dev, framedone_irq, 1);
+		else
+			wait = omap_irq_wait_init(dev, vsync_irq, 2);
 	}
 
 	dispc_mgr_enable(channel, enable);
 
-	if (wait) {
-		int ret = omap_irq_wait(dev, wait, msecs_to_jiffies(100));
-		if (ret) {
-			dev_err(dev->dev, "%s: timeout waiting for %s\n",
-					omap_crtc->name, enable ? "enable" : "disable");
-		}
+	ret = omap_irq_wait(dev, wait, msecs_to_jiffies(100));
+	if (ret) {
+		dev_err(dev->dev, "%s: timeout waiting for %s\n",
+				omap_crtc->name, enable ? "enable" : "disable");
 	}
 
 	omap_irq_register(crtc->dev, &omap_crtc->error_irq);
@@ -586,8 +612,12 @@ static void omap_crtc_pre_apply(struct omap_drm_apply *apply)
 		}
 	}
 
+	if (omap_crtc->current_encoder && encoder != omap_crtc->current_encoder)
+		omap_encoder_set_enabled(omap_crtc->current_encoder, false);
+
+	omap_crtc->current_encoder = encoder;
+
 	if (!omap_crtc->enabled) {
-		set_enabled(&omap_crtc->base, false);
 		if (encoder)
 			omap_encoder_set_enabled(encoder, false);
 	} else {
@@ -596,13 +626,7 @@ static void omap_crtc_pre_apply(struct omap_drm_apply *apply)
 			omap_encoder_update(encoder, omap_crtc->mgr,
 					&omap_crtc->timings);
 			omap_encoder_set_enabled(encoder, true);
-			omap_crtc->full_update = false;
 		}
-
-		dispc_mgr_setup(omap_crtc->channel, &omap_crtc->info);
-		dispc_mgr_set_timings(omap_crtc->channel,
-				&omap_crtc->timings);
-		set_enabled(&omap_crtc->base, true);
 	}
 
 	omap_crtc->full_update = false;
@@ -613,10 +637,30 @@ static void omap_crtc_post_apply(struct omap_drm_apply *apply)
 	/* nothing needed for post-apply */
 }
 
+void omap_crtc_flush(struct drm_crtc *crtc)
+{
+	struct omap_crtc *omap_crtc = to_omap_crtc(crtc);
+	int loops = 0;
+
+	while (!list_empty(&omap_crtc->pending_applies) ||
+		!list_empty(&omap_crtc->queued_applies) ||
+		omap_crtc->event || omap_crtc->old_fb) {
+
+		if (++loops > 10) {
+			dev_err(crtc->dev->dev,
+				"omap_crtc_flush() timeout\n");
+			break;
+		}
+
+		schedule_timeout_uninterruptible(msecs_to_jiffies(20));
+	}
+}
+
 static const char *channel_names[] = {
 		[OMAP_DSS_CHANNEL_LCD] = "lcd",
 		[OMAP_DSS_CHANNEL_DIGIT] = "tv",
 		[OMAP_DSS_CHANNEL_LCD2] = "lcd2",
+		[OMAP_DSS_CHANNEL_LCD3] = "lcd3",
 };
 
 void omap_crtc_pre_init(void)
diff --git a/drivers/gpu/drm/omapdrm/omap_drv.c b/drivers/gpu/drm/omapdrm/omap_drv.c
index bf39fcc49e0..c8270e4b26f 100644
--- a/drivers/gpu/drm/omapdrm/omap_drv.c
+++ b/drivers/gpu/drm/omapdrm/omap_drv.c
@@ -513,12 +513,18 @@ static int dev_load(struct drm_device *dev, unsigned long flags)
 static int dev_unload(struct drm_device *dev)
 {
 	struct omap_drm_private *priv = dev->dev_private;
+	int i;
 
 	DBG("unload: dev=%p", dev);
 
 	drm_kms_helper_poll_fini(dev);
 
 	omap_fbdev_free(dev);
+
+	/* flush crtcs so the fbs get released */
+	for (i = 0; i < priv->num_crtcs; i++)
+		omap_crtc_flush(priv->crtcs[i]);
+
 	omap_modeset_free(dev);
 	omap_gem_deinit(dev);
 
@@ -696,10 +702,11 @@ static int pdev_remove(struct platform_device *device)
 {
 	DBG("");
 
+	drm_put_dev(platform_get_drvdata(device));
+
 	omap_disconnect_dssdevs();
 	omap_crtc_pre_uninit();
 
-	drm_put_dev(platform_get_drvdata(device));
 	return 0;
 }
 
@@ -726,18 +733,33 @@ static struct platform_driver pdev = {
 
 static int __init omap_drm_init(void)
 {
+	int r;
+
 	DBG("init");
-	if (platform_driver_register(&omap_dmm_driver)) {
-		/* we can continue on without DMM.. so not fatal */
-		dev_err(NULL, "DMM registration failed\n");
+
+	r = platform_driver_register(&omap_dmm_driver);
+	if (r) {
+		pr_err("DMM driver registration failed\n");
+		return r;
+	}
+
+	r = platform_driver_register(&pdev);
+	if (r) {
+		pr_err("omapdrm driver registration failed\n");
+		platform_driver_unregister(&omap_dmm_driver);
+		return r;
 	}
-	return platform_driver_register(&pdev);
+
+	return 0;
 }
 
 static void __exit omap_drm_fini(void)
 {
 	DBG("fini");
+
 	platform_driver_unregister(&pdev);
+
+	platform_driver_unregister(&omap_dmm_driver);
 }
 
 /* need late_initcall() so we load after dss_driver's are loaded */
diff --git a/drivers/gpu/drm/omapdrm/omap_drv.h b/drivers/gpu/drm/omapdrm/omap_drv.h
index 428b2981fd6..284b80fc3c5 100644
--- a/drivers/gpu/drm/omapdrm/omap_drv.h
+++ b/drivers/gpu/drm/omapdrm/omap_drv.h
@@ -163,6 +163,7 @@ void omap_crtc_pre_init(void);
 void omap_crtc_pre_uninit(void);
 struct drm_crtc *omap_crtc_init(struct drm_device *dev,
 		struct drm_plane *plane, enum omap_channel channel, int id);
+void omap_crtc_flush(struct drm_crtc *crtc);
 
 struct drm_plane *omap_plane_init(struct drm_device *dev,
 		int plane_id, bool private_plane);
diff --git a/drivers/gpu/drm/omapdrm/omap_fb.c b/drivers/gpu/drm/omapdrm/omap_fb.c
index d2b8c49bfb4..8b019602ffe 100644
--- a/drivers/gpu/drm/omapdrm/omap_fb.c
+++ b/drivers/gpu/drm/omapdrm/omap_fb.c
@@ -218,6 +218,20 @@ void omap_framebuffer_update_scanout(struct drm_framebuffer *fb,
 		info->rotation_type = OMAP_DSS_ROT_TILER;
 		info->screen_width  = omap_gem_tiled_stride(plane->bo, orient);
 	} else {
+		switch (win->rotation & 0xf) {
+		case 0:
+		case BIT(DRM_ROTATE_0):
+			/* OK */
+			break;
+
+		default:
+			dev_warn(fb->dev->dev,
+				"rotation '%d' ignored for non-tiled fb\n",
+				win->rotation);
+			win->rotation = 0;
+			break;
+		}
+
 		info->paddr         = get_linear_addr(plane, format, 0, x, y);
 		info->rotation_type = OMAP_DSS_ROT_DMA;
 		info->screen_width  = plane->pitch;
diff --git a/drivers/gpu/drm/omapdrm/omap_fbdev.c b/drivers/gpu/drm/omapdrm/omap_fbdev.c
index 002988d0902..1388ca7f87e 100644
--- a/drivers/gpu/drm/omapdrm/omap_fbdev.c
+++ b/drivers/gpu/drm/omapdrm/omap_fbdev.c
@@ -371,6 +371,9 @@ void omap_fbdev_free(struct drm_device *dev)
 
 	fbdev = to_omap_fbdev(priv->fbdev);
 
+	/* release the ref taken in omap_fbdev_create() */
+	omap_gem_put_paddr(fbdev->bo);
+
 	/* this will free the backing object */
 	if (fbdev->fb) {
 		drm_framebuffer_unregister_private(fbdev->fb);
diff --git a/drivers/gpu/drm/omapdrm/omap_gem.c b/drivers/gpu/drm/omapdrm/omap_gem.c
index c8d97276388..95dbce286a4 100644
--- a/drivers/gpu/drm/omapdrm/omap_gem.c
+++ b/drivers/gpu/drm/omapdrm/omap_gem.c
@@ -980,12 +980,9 @@ int omap_gem_resume(struct device *dev)
 #ifdef CONFIG_DEBUG_FS
 void omap_gem_describe(struct drm_gem_object *obj, struct seq_file *m)
 {
-	struct drm_device *dev = obj->dev;
 	struct omap_gem_object *omap_obj = to_omap_bo(obj);
 	uint64_t off;
 
-	WARN_ON(!mutex_is_locked(&dev->struct_mutex));
-
 	off = drm_vma_node_start(&obj->vma_node);
 
 	seq_printf(m, "%08x: %2d (%2d) %08llx %08Zx (%2d) %p %4d",
@@ -1050,10 +1047,10 @@ static inline bool is_waiting(struct omap_gem_sync_waiter *waiter)
 {
 	struct omap_gem_object *omap_obj = waiter->omap_obj;
 	if ((waiter->op & OMAP_GEM_READ) &&
-			(omap_obj->sync->read_complete < waiter->read_target))
+			(omap_obj->sync->write_complete < waiter->write_target))
 		return true;
 	if ((waiter->op & OMAP_GEM_WRITE) &&
-			(omap_obj->sync->write_complete < waiter->write_target))
+			(omap_obj->sync->read_complete < waiter->read_target))
 		return true;
 	return false;
 }
@@ -1229,6 +1226,8 @@ int omap_gem_op_async(struct drm_gem_object *obj, enum omap_gem_op op,
 		}
 
 		spin_unlock(&sync_lock);
+
+		kfree(waiter);
 	}
 
 	/* no waiting.. */
diff --git a/drivers/gpu/drm/omapdrm/omap_plane.c b/drivers/gpu/drm/omapdrm/omap_plane.c
index 046d5e660c0..3cf31ee59aa 100644
--- a/drivers/gpu/drm/omapdrm/omap_plane.c
+++ b/drivers/gpu/drm/omapdrm/omap_plane.c
@@ -225,6 +225,11 @@ int omap_plane_mode_set(struct drm_plane *plane,
 		omap_plane->apply_done_cb.arg = arg;
 	}
 
+	if (plane->fb)
+		drm_framebuffer_unreference(plane->fb);
+
+	drm_framebuffer_reference(fb);
+
 	plane->fb = fb;
 	plane->crtc = crtc;
 
@@ -241,10 +246,13 @@ static int omap_plane_update(struct drm_plane *plane,
 	struct omap_plane *omap_plane = to_omap_plane(plane);
 	omap_plane->enabled = true;
 
-	if (plane->fb)
-		drm_framebuffer_unreference(plane->fb);
-
-	drm_framebuffer_reference(fb);
+	/* omap_plane_mode_set() takes adjusted src */
+	switch (omap_plane->win.rotation & 0xf) {
+	case BIT(DRM_ROTATE_90):
+	case BIT(DRM_ROTATE_270):
+		swap(src_w, src_h);
+		break;
+	}
 
 	return omap_plane_mode_set(plane, crtc, fb,
 			crtc_x, crtc_y, crtc_w, crtc_h,
diff --git a/drivers/gpu/drm/radeon/atombios_crtc.c b/drivers/gpu/drm/radeon/atombios_crtc.c
index fb187c78978..c31c12b4e66 100644
--- a/drivers/gpu/drm/radeon/atombios_crtc.c
+++ b/drivers/gpu/drm/radeon/atombios_crtc.c
@@ -1177,27 +1177,43 @@ static int dce4_crtc_do_set_base(struct drm_crtc *crtc,
 
 		/* Set NUM_BANKS. */
 		if (rdev->family >= CHIP_TAHITI) {
-			unsigned tileb, index, num_banks, tile_split_bytes;
+			unsigned index, num_banks;
 
-			/* Calculate the macrotile mode index. */
-			tile_split_bytes = 64 << tile_split;
-			tileb = 8 * 8 * target_fb->bits_per_pixel / 8;
-			tileb = min(tile_split_bytes, tileb);
+			if (rdev->family >= CHIP_BONAIRE) {
+				unsigned tileb, tile_split_bytes;
 
-			for (index = 0; tileb > 64; index++) {
-				tileb >>= 1;
-			}
+				/* Calculate the macrotile mode index. */
+				tile_split_bytes = 64 << tile_split;
+				tileb = 8 * 8 * target_fb->bits_per_pixel / 8;
+				tileb = min(tile_split_bytes, tileb);
 
-			if (index >= 16) {
-				DRM_ERROR("Wrong screen bpp (%u) or tile split (%u)\n",
-					  target_fb->bits_per_pixel, tile_split);
-				return -EINVAL;
-			}
+				for (index = 0; tileb > 64; index++)
+					tileb >>= 1;
+
+				if (index >= 16) {
+					DRM_ERROR("Wrong screen bpp (%u) or tile split (%u)\n",
+						  target_fb->bits_per_pixel, tile_split);
+					return -EINVAL;
+				}
 
-			if (rdev->family >= CHIP_BONAIRE)
 				num_banks = (rdev->config.cik.macrotile_mode_array[index] >> 6) & 0x3;
-			else
+			} else {
+				switch (target_fb->bits_per_pixel) {
+				case 8:
+					index = 10;
+					break;
+				case 16:
+					index = SI_TILE_MODE_COLOR_2D_SCANOUT_16BPP;
+					break;
+				default:
+				case 32:
+					index = SI_TILE_MODE_COLOR_2D_SCANOUT_32BPP;
+					break;
+				}
+
 				num_banks = (rdev->config.si.tile_mode_array[index] >> 20) & 0x3;
+			}
+
 			fb_format |= EVERGREEN_GRPH_NUM_BANKS(num_banks);
 		} else {
 			/* NI and older. */
@@ -1720,8 +1736,9 @@ static int radeon_atom_pick_pll(struct drm_crtc *crtc)
 		}
 		/* otherwise, pick one of the plls */
 		if ((rdev->family == CHIP_KAVERI) ||
-		    (rdev->family == CHIP_KABINI)) {
-			/* KB/KV has PPLL1 and PPLL2 */
+		    (rdev->family == CHIP_KABINI) ||
+		    (rdev->family == CHIP_MULLINS)) {
+			/* KB/KV/ML has PPLL1 and PPLL2 */
 			pll_in_use = radeon_get_pll_use_mask(crtc);
 			if (!(pll_in_use & (1 << ATOM_PPLL2)))
 				return ATOM_PPLL2;
@@ -1885,6 +1902,9 @@ int atombios_crtc_mode_set(struct drm_crtc *crtc,
 	    (ATOM_DEVICE_TV_SUPPORT | ATOM_DEVICE_CV_SUPPORT))
 		is_tvcv = true;
 
+	if (!radeon_crtc->adjusted_clock)
+		return -EINVAL;
+
 	atombios_crtc_set_pll(crtc, adjusted_mode);
 
 	if (ASIC_IS_DCE4(rdev))
diff --git a/drivers/gpu/drm/radeon/atombios_dp.c b/drivers/gpu/drm/radeon/atombios_dp.c
index 8b0ab170cef..54e4f52549a 100644
--- a/drivers/gpu/drm/radeon/atombios_dp.c
+++ b/drivers/gpu/drm/radeon/atombios_dp.c
@@ -142,7 +142,8 @@ static int radeon_process_aux_ch(struct radeon_i2c_chan *chan,
 	return recv_bytes;
 }
 
-#define HEADER_SIZE 4
+#define BARE_ADDRESS_SIZE 3
+#define HEADER_SIZE (BARE_ADDRESS_SIZE + 1)
 
 static ssize_t
 radeon_dp_aux_transfer(struct drm_dp_aux *aux, struct drm_dp_aux_msg *msg)
@@ -160,13 +161,19 @@ radeon_dp_aux_transfer(struct drm_dp_aux *aux, struct drm_dp_aux_msg *msg)
 	tx_buf[0] = msg->address & 0xff;
 	tx_buf[1] = msg->address >> 8;
 	tx_buf[2] = msg->request << 4;
-	tx_buf[3] = msg->size - 1;
+	tx_buf[3] = msg->size ? (msg->size - 1) : 0;
 
 	switch (msg->request & ~DP_AUX_I2C_MOT) {
 	case DP_AUX_NATIVE_WRITE:
 	case DP_AUX_I2C_WRITE:
+		/* tx_size needs to be 4 even for bare address packets since the atom
+		 * table needs the info in tx_buf[3].
+		 */
 		tx_size = HEADER_SIZE + msg->size;
-		tx_buf[3] |= tx_size << 4;
+		if (msg->size == 0)
+			tx_buf[3] |= BARE_ADDRESS_SIZE << 4;
+		else
+			tx_buf[3] |= tx_size << 4;
 		memcpy(tx_buf + HEADER_SIZE, msg->buffer, msg->size);
 		ret = radeon_process_aux_ch(chan,
 					    tx_buf, tx_size, NULL, 0, delay, &ack);
@@ -176,8 +183,14 @@ radeon_dp_aux_transfer(struct drm_dp_aux *aux, struct drm_dp_aux_msg *msg)
 		break;
 	case DP_AUX_NATIVE_READ:
 	case DP_AUX_I2C_READ:
+		/* tx_size needs to be 4 even for bare address packets since the atom
+		 * table needs the info in tx_buf[3].
+		 */
 		tx_size = HEADER_SIZE;
-		tx_buf[3] |= tx_size << 4;
+		if (msg->size == 0)
+			tx_buf[3] |= BARE_ADDRESS_SIZE << 4;
+		else
+			tx_buf[3] |= tx_size << 4;
 		ret = radeon_process_aux_ch(chan,
 					    tx_buf, tx_size, msg->buffer, msg->size, delay, &ack);
 		break;
@@ -186,7 +199,7 @@ radeon_dp_aux_transfer(struct drm_dp_aux *aux, struct drm_dp_aux_msg *msg)
 		break;
 	}
 
-	if (ret > 0)
+	if (ret >= 0)
 		msg->reply = ack >> 4;
 
 	return ret;
@@ -194,98 +207,16 @@ radeon_dp_aux_transfer(struct drm_dp_aux *aux, struct drm_dp_aux_msg *msg)
 
 void radeon_dp_aux_init(struct radeon_connector *radeon_connector)
 {
-	struct radeon_connector_atom_dig *dig_connector = radeon_connector->con_priv;
-
-	dig_connector->dp_i2c_bus->aux.dev = radeon_connector->base.kdev;
-	dig_connector->dp_i2c_bus->aux.transfer = radeon_dp_aux_transfer;
-}
-
-int radeon_dp_i2c_aux_ch(struct i2c_adapter *adapter, int mode,
-			 u8 write_byte, u8 *read_byte)
-{
-	struct i2c_algo_dp_aux_data *algo_data = adapter->algo_data;
-	struct radeon_i2c_chan *auxch = i2c_get_adapdata(adapter);
-	u16 address = algo_data->address;
-	u8 msg[5];
-	u8 reply[2];
-	unsigned retry;
-	int msg_bytes;
-	int reply_bytes = 1;
 	int ret;
-	u8 ack;
 
-	/* Set up the address */
-	msg[0] = address;
-	msg[1] = address >> 8;
+	radeon_connector->ddc_bus->rec.hpd = radeon_connector->hpd.hpd;
+	radeon_connector->ddc_bus->aux.dev = radeon_connector->base.kdev;
+	radeon_connector->ddc_bus->aux.transfer = radeon_dp_aux_transfer;
+	ret = drm_dp_aux_register_i2c_bus(&radeon_connector->ddc_bus->aux);
+	if (!ret)
+		radeon_connector->ddc_bus->has_aux = true;
 
-	/* Set up the command byte */
-	if (mode & MODE_I2C_READ) {
-		msg[2] = DP_AUX_I2C_READ << 4;
-		msg_bytes = 4;
-		msg[3] = msg_bytes << 4;
-	} else {
-		msg[2] = DP_AUX_I2C_WRITE << 4;
-		msg_bytes = 5;
-		msg[3] = msg_bytes << 4;
-		msg[4] = write_byte;
-	}
-
-	/* special handling for start/stop */
-	if (mode & (MODE_I2C_START | MODE_I2C_STOP))
-		msg[3] = 3 << 4;
-
-	/* Set MOT bit for all but stop */
-	if ((mode & MODE_I2C_STOP) == 0)
-		msg[2] |= DP_AUX_I2C_MOT << 4;
-
-	for (retry = 0; retry < 7; retry++) {
-		ret = radeon_process_aux_ch(auxch,
-					    msg, msg_bytes, reply, reply_bytes, 0, &ack);
-		if (ret == -EBUSY)
-			continue;
-		else if (ret < 0) {
-			DRM_DEBUG_KMS("aux_ch failed %d\n", ret);
-			return ret;
-		}
-
-		switch ((ack >> 4) & DP_AUX_NATIVE_REPLY_MASK) {
-		case DP_AUX_NATIVE_REPLY_ACK:
-			/* I2C-over-AUX Reply field is only valid
-			 * when paired with AUX ACK.
-			 */
-			break;
-		case DP_AUX_NATIVE_REPLY_NACK:
-			DRM_DEBUG_KMS("aux_ch native nack\n");
-			return -EREMOTEIO;
-		case DP_AUX_NATIVE_REPLY_DEFER:
-			DRM_DEBUG_KMS("aux_ch native defer\n");
-			usleep_range(500, 600);
-			continue;
-		default:
-			DRM_ERROR("aux_ch invalid native reply 0x%02x\n", ack);
-			return -EREMOTEIO;
-		}
-
-		switch ((ack >> 4) & DP_AUX_I2C_REPLY_MASK) {
-		case DP_AUX_I2C_REPLY_ACK:
-			if (mode == MODE_I2C_READ)
-				*read_byte = reply[0];
-			return ret;
-		case DP_AUX_I2C_REPLY_NACK:
-			DRM_DEBUG_KMS("aux_i2c nack\n");
-			return -EREMOTEIO;
-		case DP_AUX_I2C_REPLY_DEFER:
-			DRM_DEBUG_KMS("aux_i2c defer\n");
-			usleep_range(400, 500);
-			break;
-		default:
-			DRM_ERROR("aux_i2c invalid reply 0x%02x\n", ack);
-			return -EREMOTEIO;
-		}
-	}
-
-	DRM_DEBUG_KMS("aux i2c too many retries, giving up\n");
-	return -EREMOTEIO;
+	WARN(ret, "drm_dp_aux_register_i2c_bus() failed with error %d\n", ret);
 }
 
 /***** general DP utility functions *****/
@@ -420,12 +351,11 @@ static u8 radeon_dp_encoder_service(struct radeon_device *rdev,
 
 u8 radeon_dp_getsinktype(struct radeon_connector *radeon_connector)
 {
-	struct radeon_connector_atom_dig *dig_connector = radeon_connector->con_priv;
 	struct drm_device *dev = radeon_connector->base.dev;
 	struct radeon_device *rdev = dev->dev_private;
 
 	return radeon_dp_encoder_service(rdev, ATOM_DP_ACTION_GET_SINK_TYPE, 0,
-					 dig_connector->dp_i2c_bus->rec.i2c_id, 0);
+					 radeon_connector->ddc_bus->rec.i2c_id, 0);
 }
 
 static void radeon_dp_probe_oui(struct radeon_connector *radeon_connector)
@@ -436,11 +366,11 @@ static void radeon_dp_probe_oui(struct radeon_connector *radeon_connector)
 	if (!(dig_connector->dpcd[DP_DOWN_STREAM_PORT_COUNT] & DP_OUI_SUPPORT))
 		return;
 
-	if (drm_dp_dpcd_read(&dig_connector->dp_i2c_bus->aux, DP_SINK_OUI, buf, 3))
+	if (drm_dp_dpcd_read(&radeon_connector->ddc_bus->aux, DP_SINK_OUI, buf, 3) == 3)
 		DRM_DEBUG_KMS("Sink OUI: %02hx%02hx%02hx\n",
 			      buf[0], buf[1], buf[2]);
 
-	if (drm_dp_dpcd_read(&dig_connector->dp_i2c_bus->aux, DP_BRANCH_OUI, buf, 3))
+	if (drm_dp_dpcd_read(&radeon_connector->ddc_bus->aux, DP_BRANCH_OUI, buf, 3) == 3)
 		DRM_DEBUG_KMS("Branch OUI: %02hx%02hx%02hx\n",
 			      buf[0], buf[1], buf[2]);
 }
@@ -451,7 +381,7 @@ bool radeon_dp_getdpcd(struct radeon_connector *radeon_connector)
 	u8 msg[DP_DPCD_SIZE];
 	int ret, i;
 
-	ret = drm_dp_dpcd_read(&dig_connector->dp_i2c_bus->aux, DP_DPCD_REV, msg,
+	ret = drm_dp_dpcd_read(&radeon_connector->ddc_bus->aux, DP_DPCD_REV, msg,
 			       DP_DPCD_SIZE);
 	if (ret > 0) {
 		memcpy(dig_connector->dpcd, msg, DP_DPCD_SIZE);
@@ -489,21 +419,23 @@ int radeon_dp_get_panel_mode(struct drm_encoder *encoder,
 
 	if (dp_bridge != ENCODER_OBJECT_ID_NONE) {
 		/* DP bridge chips */
-		drm_dp_dpcd_readb(&dig_connector->dp_i2c_bus->aux,
-				  DP_EDP_CONFIGURATION_CAP, &tmp);
-		if (tmp & 1)
-			panel_mode = DP_PANEL_MODE_INTERNAL_DP2_MODE;
-		else if ((dp_bridge == ENCODER_OBJECT_ID_NUTMEG) ||
-			 (dp_bridge == ENCODER_OBJECT_ID_TRAVIS))
-			panel_mode = DP_PANEL_MODE_INTERNAL_DP1_MODE;
-		else
-			panel_mode = DP_PANEL_MODE_EXTERNAL_DP_MODE;
+		if (drm_dp_dpcd_readb(&radeon_connector->ddc_bus->aux,
+				      DP_EDP_CONFIGURATION_CAP, &tmp) == 1) {
+			if (tmp & 1)
+				panel_mode = DP_PANEL_MODE_INTERNAL_DP2_MODE;
+			else if ((dp_bridge == ENCODER_OBJECT_ID_NUTMEG) ||
+				 (dp_bridge == ENCODER_OBJECT_ID_TRAVIS))
+				panel_mode = DP_PANEL_MODE_INTERNAL_DP1_MODE;
+			else
+				panel_mode = DP_PANEL_MODE_EXTERNAL_DP_MODE;
+		}
 	} else if (connector->connector_type == DRM_MODE_CONNECTOR_eDP) {
 		/* eDP */
-		drm_dp_dpcd_readb(&dig_connector->dp_i2c_bus->aux,
-				  DP_EDP_CONFIGURATION_CAP, &tmp);
-		if (tmp & 1)
-			panel_mode = DP_PANEL_MODE_INTERNAL_DP2_MODE;
+		if (drm_dp_dpcd_readb(&radeon_connector->ddc_bus->aux,
+				      DP_EDP_CONFIGURATION_CAP, &tmp) == 1) {
+			if (tmp & 1)
+				panel_mode = DP_PANEL_MODE_INTERNAL_DP2_MODE;
+		}
 	}
 
 	return panel_mode;
@@ -554,7 +486,8 @@ bool radeon_dp_needs_link_train(struct radeon_connector *radeon_connector)
 	u8 link_status[DP_LINK_STATUS_SIZE];
 	struct radeon_connector_atom_dig *dig = radeon_connector->con_priv;
 
-	if (drm_dp_dpcd_read_link_status(&dig->dp_i2c_bus->aux, link_status) <= 0)
+	if (drm_dp_dpcd_read_link_status(&radeon_connector->ddc_bus->aux, link_status)
+	    <= 0)
 		return false;
 	if (drm_dp_channel_eq_ok(link_status, dig->dp_lane_count))
 		return false;
@@ -574,7 +507,7 @@ void radeon_dp_set_rx_power_state(struct drm_connector *connector,
 
 	/* power up/down the sink */
 	if (dig_connector->dpcd[0] >= 0x11) {
-		drm_dp_dpcd_writeb(&dig_connector->dp_i2c_bus->aux,
+		drm_dp_dpcd_writeb(&radeon_connector->ddc_bus->aux,
 				   DP_SET_POWER, power_state);
 		usleep_range(1000, 2000);
 	}
@@ -878,11 +811,15 @@ void radeon_dp_link_train(struct drm_encoder *encoder,
 	else
 		dp_info.enc_id |= ATOM_DP_CONFIG_LINK_A;
 
-	drm_dp_dpcd_readb(&dig_connector->dp_i2c_bus->aux, DP_MAX_LANE_COUNT, &tmp);
-	if (ASIC_IS_DCE5(rdev) && (tmp & DP_TPS3_SUPPORTED))
-		dp_info.tp3_supported = true;
-	else
+	if (drm_dp_dpcd_readb(&radeon_connector->ddc_bus->aux, DP_MAX_LANE_COUNT, &tmp)
+	    == 1) {
+		if (ASIC_IS_DCE5(rdev) && (tmp & DP_TPS3_SUPPORTED))
+			dp_info.tp3_supported = true;
+		else
+			dp_info.tp3_supported = false;
+	} else {
 		dp_info.tp3_supported = false;
+	}
 
 	memcpy(dp_info.dpcd, dig_connector->dpcd, DP_RECEIVER_CAP_SIZE);
 	dp_info.rdev = rdev;
@@ -890,7 +827,7 @@ void radeon_dp_link_train(struct drm_encoder *encoder,
 	dp_info.connector = connector;
 	dp_info.dp_lane_count = dig_connector->dp_lane_count;
 	dp_info.dp_clock = dig_connector->dp_clock;
-	dp_info.aux = &dig_connector->dp_i2c_bus->aux;
+	dp_info.aux = &radeon_connector->ddc_bus->aux;
 
 	if (radeon_dp_link_train_init(&dp_info))
 		goto done;
diff --git a/drivers/gpu/drm/radeon/ci_dpm.c b/drivers/gpu/drm/radeon/ci_dpm.c
index cad89a97752..10dae4106c0 100644
--- a/drivers/gpu/drm/radeon/ci_dpm.c
+++ b/drivers/gpu/drm/radeon/ci_dpm.c
@@ -21,8 +21,10 @@
  *
  */
 
+#include <linux/firmware.h>
 #include "drmP.h"
 #include "radeon.h"
+#include "radeon_ucode.h"
 #include "cikd.h"
 #include "r600_dpm.h"
 #include "ci_dpm.h"
@@ -202,24 +204,29 @@ static void ci_initialize_powertune_defaults(struct radeon_device *rdev)
 	struct ci_power_info *pi = ci_get_pi(rdev);
 
 	switch (rdev->pdev->device) {
+	case 0x6649:
 	case 0x6650:
+	case 0x6651:
 	case 0x6658:
 	case 0x665C:
+	case 0x665D:
 	default:
 		pi->powertune_defaults = &defaults_bonaire_xt;
 		break;
-	case 0x6651:
-	case 0x665D:
-		pi->powertune_defaults = &defaults_bonaire_pro;
-		break;
 	case 0x6640:
-		pi->powertune_defaults = &defaults_saturn_xt;
-		break;
 	case 0x6641:
-		pi->powertune_defaults = &defaults_saturn_pro;
+	case 0x6646:
+	case 0x6647:
+		pi->powertune_defaults = &defaults_saturn_xt;
 		break;
 	case 0x67B8:
 	case 0x67B0:
+		pi->powertune_defaults = &defaults_hawaii_xt;
+		break;
+	case 0x67BA:
+	case 0x67B1:
+		pi->powertune_defaults = &defaults_hawaii_pro;
+		break;
 	case 0x67A0:
 	case 0x67A1:
 	case 0x67A2:
@@ -228,11 +235,7 @@ static void ci_initialize_powertune_defaults(struct radeon_device *rdev)
 	case 0x67AA:
 	case 0x67B9:
 	case 0x67BE:
-		pi->powertune_defaults = &defaults_hawaii_xt;
-		break;
-	case 0x67BA:
-	case 0x67B1:
-		pi->powertune_defaults = &defaults_hawaii_pro;
+		pi->powertune_defaults = &defaults_bonaire_xt;
 		break;
 	}
 
@@ -5146,6 +5149,12 @@ int ci_dpm_init(struct radeon_device *rdev)
 	pi->mclk_dpm_key_disabled = 0;
 	pi->pcie_dpm_key_disabled = 0;
 
+	/* mclk dpm is unstable on some R7 260X cards with the old mc ucode */
+	if ((rdev->pdev->device == 0x6658) &&
+	    (rdev->mc_fw->size == (BONAIRE_MC_UCODE_SIZE * 4))) {
+		pi->mclk_dpm_key_disabled = 1;
+	}
+
 	pi->caps_sclk_ds = true;
 
 	pi->mclk_strobe_mode_threshold = 40000;
diff --git a/drivers/gpu/drm/radeon/cik.c b/drivers/gpu/drm/radeon/cik.c
index 745143c2358..d2fd9896808 100644
--- a/drivers/gpu/drm/radeon/cik.c
+++ b/drivers/gpu/drm/radeon/cik.c
@@ -38,6 +38,7 @@ MODULE_FIRMWARE("radeon/BONAIRE_me.bin");
 MODULE_FIRMWARE("radeon/BONAIRE_ce.bin");
 MODULE_FIRMWARE("radeon/BONAIRE_mec.bin");
 MODULE_FIRMWARE("radeon/BONAIRE_mc.bin");
+MODULE_FIRMWARE("radeon/BONAIRE_mc2.bin");
 MODULE_FIRMWARE("radeon/BONAIRE_rlc.bin");
 MODULE_FIRMWARE("radeon/BONAIRE_sdma.bin");
 MODULE_FIRMWARE("radeon/BONAIRE_smc.bin");
@@ -46,6 +47,7 @@ MODULE_FIRMWARE("radeon/HAWAII_me.bin");
 MODULE_FIRMWARE("radeon/HAWAII_ce.bin");
 MODULE_FIRMWARE("radeon/HAWAII_mec.bin");
 MODULE_FIRMWARE("radeon/HAWAII_mc.bin");
+MODULE_FIRMWARE("radeon/HAWAII_mc2.bin");
 MODULE_FIRMWARE("radeon/HAWAII_rlc.bin");
 MODULE_FIRMWARE("radeon/HAWAII_sdma.bin");
 MODULE_FIRMWARE("radeon/HAWAII_smc.bin");
@@ -61,6 +63,12 @@ MODULE_FIRMWARE("radeon/KABINI_ce.bin");
 MODULE_FIRMWARE("radeon/KABINI_mec.bin");
 MODULE_FIRMWARE("radeon/KABINI_rlc.bin");
 MODULE_FIRMWARE("radeon/KABINI_sdma.bin");
+MODULE_FIRMWARE("radeon/MULLINS_pfp.bin");
+MODULE_FIRMWARE("radeon/MULLINS_me.bin");
+MODULE_FIRMWARE("radeon/MULLINS_ce.bin");
+MODULE_FIRMWARE("radeon/MULLINS_mec.bin");
+MODULE_FIRMWARE("radeon/MULLINS_rlc.bin");
+MODULE_FIRMWARE("radeon/MULLINS_sdma.bin");
 
 extern int r600_ih_ring_alloc(struct radeon_device *rdev);
 extern void r600_ih_ring_fini(struct radeon_device *rdev);
@@ -1471,6 +1479,43 @@ static const u32 hawaii_mgcg_cgcg_init[] =
 	0xd80c, 0xff000ff0, 0x00000100
 };
 
+static const u32 godavari_golden_registers[] =
+{
+	0x55e4, 0xff607fff, 0xfc000100,
+	0x6ed8, 0x00010101, 0x00010000,
+	0x9830, 0xffffffff, 0x00000000,
+	0x98302, 0xf00fffff, 0x00000400,
+	0x6130, 0xffffffff, 0x00010000,
+	0x5bb0, 0x000000f0, 0x00000070,
+	0x5bc0, 0xf0311fff, 0x80300000,
+	0x98f8, 0x73773777, 0x12010001,
+	0x98fc, 0xffffffff, 0x00000010,
+	0x8030, 0x00001f0f, 0x0000100a,
+	0x2f48, 0x73773777, 0x12010001,
+	0x2408, 0x000fffff, 0x000c007f,
+	0x8a14, 0xf000003f, 0x00000007,
+	0x8b24, 0xffffffff, 0x00ff0fff,
+	0x30a04, 0x0000ff0f, 0x00000000,
+	0x28a4c, 0x07ffffff, 0x06000000,
+	0x4d8, 0x00000fff, 0x00000100,
+	0xd014, 0x00010000, 0x00810001,
+	0xd814, 0x00010000, 0x00810001,
+	0x3e78, 0x00000001, 0x00000002,
+	0xc768, 0x00000008, 0x00000008,
+	0xc770, 0x00000f00, 0x00000800,
+	0xc774, 0x00000f00, 0x00000800,
+	0xc798, 0x00ffffff, 0x00ff7fbf,
+	0xc79c, 0x00ffffff, 0x00ff7faf,
+	0x8c00, 0x000000ff, 0x00000001,
+	0x214f8, 0x01ff01ff, 0x00000002,
+	0x21498, 0x007ff800, 0x00200000,
+	0x2015c, 0xffffffff, 0x00000f40,
+	0x88c4, 0x001f3ae3, 0x00000082,
+	0x88d4, 0x0000001f, 0x00000010,
+	0x30934, 0xffffffff, 0x00000000
+};
+
+
 static void cik_init_golden_registers(struct radeon_device *rdev)
 {
 	switch (rdev->family) {
@@ -1502,6 +1547,20 @@ static void cik_init_golden_registers(struct radeon_device *rdev)
 						 kalindi_golden_spm_registers,
 						 (const u32)ARRAY_SIZE(kalindi_golden_spm_registers));
 		break;
+	case CHIP_MULLINS:
+		radeon_program_register_sequence(rdev,
+						 kalindi_mgcg_cgcg_init,
+						 (const u32)ARRAY_SIZE(kalindi_mgcg_cgcg_init));
+		radeon_program_register_sequence(rdev,
+						 godavari_golden_registers,
+						 (const u32)ARRAY_SIZE(godavari_golden_registers));
+		radeon_program_register_sequence(rdev,
+						 kalindi_golden_common_registers,
+						 (const u32)ARRAY_SIZE(kalindi_golden_common_registers));
+		radeon_program_register_sequence(rdev,
+						 kalindi_golden_spm_registers,
+						 (const u32)ARRAY_SIZE(kalindi_golden_spm_registers));
+		break;
 	case CHIP_KAVERI:
 		radeon_program_register_sequence(rdev,
 						 spectre_mgcg_cgcg_init,
@@ -1703,20 +1762,20 @@ int ci_mc_load_microcode(struct radeon_device *rdev)
 	const __be32 *fw_data;
 	u32 running, blackout = 0;
 	u32 *io_mc_regs;
-	int i, ucode_size, regs_size;
+	int i, regs_size, ucode_size;
 
 	if (!rdev->mc_fw)
 		return -EINVAL;
 
+	ucode_size = rdev->mc_fw->size / 4;
+
 	switch (rdev->family) {
 	case CHIP_BONAIRE:
 		io_mc_regs = (u32 *)&bonaire_io_mc_regs;
-		ucode_size = CIK_MC_UCODE_SIZE;
 		regs_size = BONAIRE_IO_MC_REGS_SIZE;
 		break;
 	case CHIP_HAWAII:
 		io_mc_regs = (u32 *)&hawaii_io_mc_regs;
-		ucode_size = HAWAII_MC_UCODE_SIZE;
 		regs_size = HAWAII_IO_MC_REGS_SIZE;
 		break;
 	default:
@@ -1783,7 +1842,7 @@ static int cik_init_microcode(struct radeon_device *rdev)
 	const char *chip_name;
 	size_t pfp_req_size, me_req_size, ce_req_size,
 		mec_req_size, rlc_req_size, mc_req_size = 0,
-		sdma_req_size, smc_req_size = 0;
+		sdma_req_size, smc_req_size = 0, mc2_req_size = 0;
 	char fw_name[30];
 	int err;
 
@@ -1797,7 +1856,8 @@ static int cik_init_microcode(struct radeon_device *rdev)
 		ce_req_size = CIK_CE_UCODE_SIZE * 4;
 		mec_req_size = CIK_MEC_UCODE_SIZE * 4;
 		rlc_req_size = BONAIRE_RLC_UCODE_SIZE * 4;
-		mc_req_size = CIK_MC_UCODE_SIZE * 4;
+		mc_req_size = BONAIRE_MC_UCODE_SIZE * 4;
+		mc2_req_size = BONAIRE_MC2_UCODE_SIZE * 4;
 		sdma_req_size = CIK_SDMA_UCODE_SIZE * 4;
 		smc_req_size = ALIGN(BONAIRE_SMC_UCODE_SIZE, 4);
 		break;
@@ -1809,6 +1869,7 @@ static int cik_init_microcode(struct radeon_device *rdev)
 		mec_req_size = CIK_MEC_UCODE_SIZE * 4;
 		rlc_req_size = BONAIRE_RLC_UCODE_SIZE * 4;
 		mc_req_size = HAWAII_MC_UCODE_SIZE * 4;
+		mc2_req_size = HAWAII_MC2_UCODE_SIZE * 4;
 		sdma_req_size = CIK_SDMA_UCODE_SIZE * 4;
 		smc_req_size = ALIGN(HAWAII_SMC_UCODE_SIZE, 4);
 		break;
@@ -1830,6 +1891,15 @@ static int cik_init_microcode(struct radeon_device *rdev)
 		rlc_req_size = KB_RLC_UCODE_SIZE * 4;
 		sdma_req_size = CIK_SDMA_UCODE_SIZE * 4;
 		break;
+	case CHIP_MULLINS:
+		chip_name = "MULLINS";
+		pfp_req_size = CIK_PFP_UCODE_SIZE * 4;
+		me_req_size = CIK_ME_UCODE_SIZE * 4;
+		ce_req_size = CIK_CE_UCODE_SIZE * 4;
+		mec_req_size = CIK_MEC_UCODE_SIZE * 4;
+		rlc_req_size = ML_RLC_UCODE_SIZE * 4;
+		sdma_req_size = CIK_SDMA_UCODE_SIZE * 4;
+		break;
 	default: BUG();
 	}
 
@@ -1904,16 +1974,22 @@ static int cik_init_microcode(struct radeon_device *rdev)
 
 	/* No SMC, MC ucode on APUs */
 	if (!(rdev->flags & RADEON_IS_IGP)) {
-		snprintf(fw_name, sizeof(fw_name), "radeon/%s_mc.bin", chip_name);
+		snprintf(fw_name, sizeof(fw_name), "radeon/%s_mc2.bin", chip_name);
 		err = request_firmware(&rdev->mc_fw, fw_name, rdev->dev);
-		if (err)
-			goto out;
-		if (rdev->mc_fw->size != mc_req_size) {
+		if (err) {
+			snprintf(fw_name, sizeof(fw_name), "radeon/%s_mc.bin", chip_name);
+			err = request_firmware(&rdev->mc_fw, fw_name, rdev->dev);
+			if (err)
+				goto out;
+		}
+		if ((rdev->mc_fw->size != mc_req_size) &&
+		    (rdev->mc_fw->size != mc2_req_size)){
 			printk(KERN_ERR
 			       "cik_mc: Bogus length %zu in firmware \"%s\"\n",
 			       rdev->mc_fw->size, fw_name);
 			err = -EINVAL;
 		}
+		DRM_INFO("%s: %zu bytes\n", fw_name, rdev->mc_fw->size);
 
 		snprintf(fw_name, sizeof(fw_name), "radeon/%s_smc.bin", chip_name);
 		err = request_firmware(&rdev->smc_fw, fw_name, rdev->dev);
@@ -3262,6 +3338,7 @@ static void cik_gpu_init(struct radeon_device *rdev)
 		gb_addr_config = BONAIRE_GB_ADDR_CONFIG_GOLDEN;
 		break;
 	case CHIP_KABINI:
+	case CHIP_MULLINS:
 	default:
 		rdev->config.cik.max_shader_engines = 1;
 		rdev->config.cik.max_tile_pipes = 2;
@@ -3692,6 +3769,7 @@ int cik_copy_cpdma(struct radeon_device *rdev,
 	r = radeon_fence_emit(rdev, fence, ring->idx);
 	if (r) {
 		radeon_ring_unlock_undo(rdev, ring);
+		radeon_semaphore_free(rdev, &sem, NULL);
 		return r;
 	}
 
@@ -5790,6 +5868,9 @@ static int cik_rlc_resume(struct radeon_device *rdev)
 	case CHIP_KABINI:
 		size = KB_RLC_UCODE_SIZE;
 		break;
+	case CHIP_MULLINS:
+		size = ML_RLC_UCODE_SIZE;
+		break;
 	}
 
 	cik_rlc_stop(rdev);
@@ -6538,6 +6619,7 @@ void cik_get_csb_buffer(struct radeon_device *rdev, volatile u32 *buffer)
 		buffer[count++] = cpu_to_le32(0x00000000);
 		break;
 	case CHIP_KABINI:
+	case CHIP_MULLINS:
 		buffer[count++] = cpu_to_le32(0x00000000); /* XXX */
 		buffer[count++] = cpu_to_le32(0x00000000);
 		break;
@@ -6683,6 +6765,19 @@ static void cik_disable_interrupt_state(struct radeon_device *rdev)
 		WREG32(LB_INTERRUPT_MASK + EVERGREEN_CRTC4_REGISTER_OFFSET, 0);
 		WREG32(LB_INTERRUPT_MASK + EVERGREEN_CRTC5_REGISTER_OFFSET, 0);
 	}
+	/* pflip */
+	if (rdev->num_crtc >= 2) {
+		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC0_REGISTER_OFFSET, 0);
+		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC1_REGISTER_OFFSET, 0);
+	}
+	if (rdev->num_crtc >= 4) {
+		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC2_REGISTER_OFFSET, 0);
+		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC3_REGISTER_OFFSET, 0);
+	}
+	if (rdev->num_crtc >= 6) {
+		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC4_REGISTER_OFFSET, 0);
+		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC5_REGISTER_OFFSET, 0);
+	}
 
 	/* dac hotplug */
 	WREG32(DAC_AUTODETECT_INT_CONTROL, 0);
@@ -7039,6 +7134,25 @@ int cik_irq_set(struct radeon_device *rdev)
 		WREG32(LB_INTERRUPT_MASK + EVERGREEN_CRTC5_REGISTER_OFFSET, crtc6);
 	}
 
+	if (rdev->num_crtc >= 2) {
+		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC0_REGISTER_OFFSET,
+		       GRPH_PFLIP_INT_MASK);
+		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC1_REGISTER_OFFSET,
+		       GRPH_PFLIP_INT_MASK);
+	}
+	if (rdev->num_crtc >= 4) {
+		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC2_REGISTER_OFFSET,
+		       GRPH_PFLIP_INT_MASK);
+		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC3_REGISTER_OFFSET,
+		       GRPH_PFLIP_INT_MASK);
+	}
+	if (rdev->num_crtc >= 6) {
+		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC4_REGISTER_OFFSET,
+		       GRPH_PFLIP_INT_MASK);
+		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC5_REGISTER_OFFSET,
+		       GRPH_PFLIP_INT_MASK);
+	}
+
 	WREG32(DC_HPD1_INT_CONTROL, hpd1);
 	WREG32(DC_HPD2_INT_CONTROL, hpd2);
 	WREG32(DC_HPD3_INT_CONTROL, hpd3);
@@ -7075,6 +7189,29 @@ static inline void cik_irq_ack(struct radeon_device *rdev)
 	rdev->irq.stat_regs.cik.disp_int_cont5 = RREG32(DISP_INTERRUPT_STATUS_CONTINUE5);
 	rdev->irq.stat_regs.cik.disp_int_cont6 = RREG32(DISP_INTERRUPT_STATUS_CONTINUE6);
 
+	rdev->irq.stat_regs.cik.d1grph_int = RREG32(GRPH_INT_STATUS +
+		EVERGREEN_CRTC0_REGISTER_OFFSET);
+	rdev->irq.stat_regs.cik.d2grph_int = RREG32(GRPH_INT_STATUS +
+		EVERGREEN_CRTC1_REGISTER_OFFSET);
+	if (rdev->num_crtc >= 4) {
+		rdev->irq.stat_regs.cik.d3grph_int = RREG32(GRPH_INT_STATUS +
+			EVERGREEN_CRTC2_REGISTER_OFFSET);
+		rdev->irq.stat_regs.cik.d4grph_int = RREG32(GRPH_INT_STATUS +
+			EVERGREEN_CRTC3_REGISTER_OFFSET);
+	}
+	if (rdev->num_crtc >= 6) {
+		rdev->irq.stat_regs.cik.d5grph_int = RREG32(GRPH_INT_STATUS +
+			EVERGREEN_CRTC4_REGISTER_OFFSET);
+		rdev->irq.stat_regs.cik.d6grph_int = RREG32(GRPH_INT_STATUS +
+			EVERGREEN_CRTC5_REGISTER_OFFSET);
+	}
+
+	if (rdev->irq.stat_regs.cik.d1grph_int & GRPH_PFLIP_INT_OCCURRED)
+		WREG32(GRPH_INT_STATUS + EVERGREEN_CRTC0_REGISTER_OFFSET,
+		       GRPH_PFLIP_INT_CLEAR);
+	if (rdev->irq.stat_regs.cik.d2grph_int & GRPH_PFLIP_INT_OCCURRED)
+		WREG32(GRPH_INT_STATUS + EVERGREEN_CRTC1_REGISTER_OFFSET,
+		       GRPH_PFLIP_INT_CLEAR);
 	if (rdev->irq.stat_regs.cik.disp_int & LB_D1_VBLANK_INTERRUPT)
 		WREG32(LB_VBLANK_STATUS + EVERGREEN_CRTC0_REGISTER_OFFSET, VBLANK_ACK);
 	if (rdev->irq.stat_regs.cik.disp_int & LB_D1_VLINE_INTERRUPT)
@@ -7085,6 +7222,12 @@ static inline void cik_irq_ack(struct radeon_device *rdev)
 		WREG32(LB_VLINE_STATUS + EVERGREEN_CRTC1_REGISTER_OFFSET, VLINE_ACK);
 
 	if (rdev->num_crtc >= 4) {
+		if (rdev->irq.stat_regs.cik.d3grph_int & GRPH_PFLIP_INT_OCCURRED)
+			WREG32(GRPH_INT_STATUS + EVERGREEN_CRTC2_REGISTER_OFFSET,
+			       GRPH_PFLIP_INT_CLEAR);
+		if (rdev->irq.stat_regs.cik.d4grph_int & GRPH_PFLIP_INT_OCCURRED)
+			WREG32(GRPH_INT_STATUS + EVERGREEN_CRTC3_REGISTER_OFFSET,
+			       GRPH_PFLIP_INT_CLEAR);
 		if (rdev->irq.stat_regs.cik.disp_int_cont2 & LB_D3_VBLANK_INTERRUPT)
 			WREG32(LB_VBLANK_STATUS + EVERGREEN_CRTC2_REGISTER_OFFSET, VBLANK_ACK);
 		if (rdev->irq.stat_regs.cik.disp_int_cont2 & LB_D3_VLINE_INTERRUPT)
@@ -7096,6 +7239,12 @@ static inline void cik_irq_ack(struct radeon_device *rdev)
 	}
 
 	if (rdev->num_crtc >= 6) {
+		if (rdev->irq.stat_regs.cik.d5grph_int & GRPH_PFLIP_INT_OCCURRED)
+			WREG32(GRPH_INT_STATUS + EVERGREEN_CRTC4_REGISTER_OFFSET,
+			       GRPH_PFLIP_INT_CLEAR);
+		if (rdev->irq.stat_regs.cik.d6grph_int & GRPH_PFLIP_INT_OCCURRED)
+			WREG32(GRPH_INT_STATUS + EVERGREEN_CRTC5_REGISTER_OFFSET,
+			       GRPH_PFLIP_INT_CLEAR);
 		if (rdev->irq.stat_regs.cik.disp_int_cont4 & LB_D5_VBLANK_INTERRUPT)
 			WREG32(LB_VBLANK_STATUS + EVERGREEN_CRTC4_REGISTER_OFFSET, VBLANK_ACK);
 		if (rdev->irq.stat_regs.cik.disp_int_cont4 & LB_D5_VLINE_INTERRUPT)
@@ -7447,6 +7596,15 @@ restart_ih:
 				break;
 			}
 			break;
+		case 8: /* D1 page flip */
+		case 10: /* D2 page flip */
+		case 12: /* D3 page flip */
+		case 14: /* D4 page flip */
+		case 16: /* D5 page flip */
+		case 18: /* D6 page flip */
+			DRM_DEBUG("IH: D%d flip\n", ((src_id - 8) >> 1) + 1);
+			radeon_crtc_handle_flip(rdev, (src_id - 8) >> 1);
+			break;
 		case 42: /* HPD hotplug */
 			switch (src_data) {
 			case 0:
diff --git a/drivers/gpu/drm/radeon/cik_sdma.c b/drivers/gpu/drm/radeon/cik_sdma.c
index 89b4afa5041..72e464c79a8 100644
--- a/drivers/gpu/drm/radeon/cik_sdma.c
+++ b/drivers/gpu/drm/radeon/cik_sdma.c
@@ -562,6 +562,7 @@ int cik_copy_dma(struct radeon_device *rdev,
 	r = radeon_fence_emit(rdev, fence, ring->idx);
 	if (r) {
 		radeon_ring_unlock_undo(rdev, ring);
+		radeon_semaphore_free(rdev, &sem, NULL);
 		return r;
 	}
 
@@ -597,7 +598,7 @@ int cik_sdma_ring_test(struct radeon_device *rdev,
 	tmp = 0xCAFEDEAD;
 	writel(tmp, ptr);
 
-	r = radeon_ring_lock(rdev, ring, 4);
+	r = radeon_ring_lock(rdev, ring, 5);
 	if (r) {
 		DRM_ERROR("radeon: dma failed to lock ring %d (%d).\n", ring->idx, r);
 		return r;
diff --git a/drivers/gpu/drm/radeon/cikd.h b/drivers/gpu/drm/radeon/cikd.h
index 213873270d5..dd7926394a8 100644
--- a/drivers/gpu/drm/radeon/cikd.h
+++ b/drivers/gpu/drm/radeon/cikd.h
@@ -888,6 +888,15 @@
 #       define DC_HPD6_RX_INTERRUPT                     (1 << 18)
 #define DISP_INTERRUPT_STATUS_CONTINUE6                 0x6780
 
+/* 0x6858, 0x7458, 0x10058, 0x10c58, 0x11858, 0x12458 */
+#define GRPH_INT_STATUS                                 0x6858
+#       define GRPH_PFLIP_INT_OCCURRED                  (1 << 0)
+#       define GRPH_PFLIP_INT_CLEAR                     (1 << 8)
+/* 0x685c, 0x745c, 0x1005c, 0x10c5c, 0x1185c, 0x1245c */
+#define GRPH_INT_CONTROL                                0x685c
+#       define GRPH_PFLIP_INT_MASK                      (1 << 0)
+#       define GRPH_PFLIP_INT_TYPE                      (1 << 8)
+
 #define	DAC_AUTODETECT_INT_CONTROL			0x67c8
 
 #define DC_HPD1_INT_STATUS                              0x601c
diff --git a/drivers/gpu/drm/radeon/dce6_afmt.c b/drivers/gpu/drm/radeon/dce6_afmt.c
index 94e85875199..0a65dc7e93e 100644
--- a/drivers/gpu/drm/radeon/dce6_afmt.c
+++ b/drivers/gpu/drm/radeon/dce6_afmt.c
@@ -309,11 +309,17 @@ int dce6_audio_init(struct radeon_device *rdev)
 
 	rdev->audio.enabled = true;
 
-	if (ASIC_IS_DCE8(rdev))
+	if (ASIC_IS_DCE81(rdev)) /* KV: 4 streams, 7 endpoints */
+		rdev->audio.num_pins = 7;
+	else if (ASIC_IS_DCE83(rdev)) /* KB: 2 streams, 3 endpoints */
+		rdev->audio.num_pins = 3;
+	else if (ASIC_IS_DCE8(rdev)) /* BN/HW: 6 streams, 7 endpoints */
+		rdev->audio.num_pins = 7;
+	else if (ASIC_IS_DCE61(rdev)) /* TN: 4 streams, 6 endpoints */
 		rdev->audio.num_pins = 6;
-	else if (ASIC_IS_DCE61(rdev))
-		rdev->audio.num_pins = 4;
-	else
+	else if (ASIC_IS_DCE64(rdev)) /* OL: 2 streams, 2 endpoints */
+		rdev->audio.num_pins = 2;
+	else /* SI: 6 streams, 6 endpoints */
 		rdev->audio.num_pins = 6;
 
 	for (i = 0; i < rdev->audio.num_pins; i++) {
diff --git a/drivers/gpu/drm/radeon/evergreen.c b/drivers/gpu/drm/radeon/evergreen.c
index b406546440d..0f7a51a3694 100644
--- a/drivers/gpu/drm/radeon/evergreen.c
+++ b/drivers/gpu/drm/radeon/evergreen.c
@@ -4371,7 +4371,6 @@ int evergreen_irq_set(struct radeon_device *rdev)
 	u32 crtc1 = 0, crtc2 = 0, crtc3 = 0, crtc4 = 0, crtc5 = 0, crtc6 = 0;
 	u32 hpd1, hpd2, hpd3, hpd4, hpd5, hpd6;
 	u32 grbm_int_cntl = 0;
-	u32 grph1 = 0, grph2 = 0, grph3 = 0, grph4 = 0, grph5 = 0, grph6 = 0;
 	u32 afmt1 = 0, afmt2 = 0, afmt3 = 0, afmt4 = 0, afmt5 = 0, afmt6 = 0;
 	u32 dma_cntl, dma_cntl1 = 0;
 	u32 thermal_int = 0;
@@ -4554,15 +4553,21 @@ int evergreen_irq_set(struct radeon_device *rdev)
 		WREG32(INT_MASK + EVERGREEN_CRTC5_REGISTER_OFFSET, crtc6);
 	}
 
-	WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC0_REGISTER_OFFSET, grph1);
-	WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC1_REGISTER_OFFSET, grph2);
+	WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC0_REGISTER_OFFSET,
+	       GRPH_PFLIP_INT_MASK);
+	WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC1_REGISTER_OFFSET,
+	       GRPH_PFLIP_INT_MASK);
 	if (rdev->num_crtc >= 4) {
-		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC2_REGISTER_OFFSET, grph3);
-		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC3_REGISTER_OFFSET, grph4);
+		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC2_REGISTER_OFFSET,
+		       GRPH_PFLIP_INT_MASK);
+		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC3_REGISTER_OFFSET,
+		       GRPH_PFLIP_INT_MASK);
 	}
 	if (rdev->num_crtc >= 6) {
-		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC4_REGISTER_OFFSET, grph5);
-		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC5_REGISTER_OFFSET, grph6);
+		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC4_REGISTER_OFFSET,
+		       GRPH_PFLIP_INT_MASK);
+		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC5_REGISTER_OFFSET,
+		       GRPH_PFLIP_INT_MASK);
 	}
 
 	WREG32(DC_HPD1_INT_CONTROL, hpd1);
@@ -4951,6 +4956,15 @@ restart_ih:
 				break;
 			}
 			break;
+		case 8: /* D1 page flip */
+		case 10: /* D2 page flip */
+		case 12: /* D3 page flip */
+		case 14: /* D4 page flip */
+		case 16: /* D5 page flip */
+		case 18: /* D6 page flip */
+			DRM_DEBUG("IH: D%d flip\n", ((src_id - 8) >> 1) + 1);
+			radeon_crtc_handle_flip(rdev, (src_id - 8) >> 1);
+			break;
 		case 42: /* HPD hotplug */
 			switch (src_data) {
 			case 0:
diff --git a/drivers/gpu/drm/radeon/evergreen_dma.c b/drivers/gpu/drm/radeon/evergreen_dma.c
index 287fe966d7d..478caefe0fe 100644
--- a/drivers/gpu/drm/radeon/evergreen_dma.c
+++ b/drivers/gpu/drm/radeon/evergreen_dma.c
@@ -151,6 +151,7 @@ int evergreen_copy_dma(struct radeon_device *rdev,
 	r = radeon_fence_emit(rdev, fence, ring->idx);
 	if (r) {
 		radeon_ring_unlock_undo(rdev, ring);
+		radeon_semaphore_free(rdev, &sem, NULL);
 		return r;
 	}
 
diff --git a/drivers/gpu/drm/radeon/kv_dpm.c b/drivers/gpu/drm/radeon/kv_dpm.c
index 16ec9d56a23..3f6e817d97e 100644
--- a/drivers/gpu/drm/radeon/kv_dpm.c
+++ b/drivers/gpu/drm/radeon/kv_dpm.c
@@ -546,6 +546,52 @@ static int kv_set_divider_value(struct radeon_device *rdev,
 	return 0;
 }
 
+static u32 kv_convert_vid2_to_vid7(struct radeon_device *rdev,
+				   struct sumo_vid_mapping_table *vid_mapping_table,
+				   u32 vid_2bit)
+{
+	struct radeon_clock_voltage_dependency_table *vddc_sclk_table =
+		&rdev->pm.dpm.dyn_state.vddc_dependency_on_sclk;
+	u32 i;
+
+	if (vddc_sclk_table && vddc_sclk_table->count) {
+		if (vid_2bit < vddc_sclk_table->count)
+			return vddc_sclk_table->entries[vid_2bit].v;
+		else
+			return vddc_sclk_table->entries[vddc_sclk_table->count - 1].v;
+	} else {
+		for (i = 0; i < vid_mapping_table->num_entries; i++) {
+			if (vid_mapping_table->entries[i].vid_2bit == vid_2bit)
+				return vid_mapping_table->entries[i].vid_7bit;
+		}
+		return vid_mapping_table->entries[vid_mapping_table->num_entries - 1].vid_7bit;
+	}
+}
+
+static u32 kv_convert_vid7_to_vid2(struct radeon_device *rdev,
+				   struct sumo_vid_mapping_table *vid_mapping_table,
+				   u32 vid_7bit)
+{
+	struct radeon_clock_voltage_dependency_table *vddc_sclk_table =
+		&rdev->pm.dpm.dyn_state.vddc_dependency_on_sclk;
+	u32 i;
+
+	if (vddc_sclk_table && vddc_sclk_table->count) {
+		for (i = 0; i < vddc_sclk_table->count; i++) {
+			if (vddc_sclk_table->entries[i].v == vid_7bit)
+				return i;
+		}
+		return vddc_sclk_table->count - 1;
+	} else {
+		for (i = 0; i < vid_mapping_table->num_entries; i++) {
+			if (vid_mapping_table->entries[i].vid_7bit == vid_7bit)
+				return vid_mapping_table->entries[i].vid_2bit;
+		}
+
+		return vid_mapping_table->entries[vid_mapping_table->num_entries - 1].vid_2bit;
+	}
+}
+
 static u16 kv_convert_8bit_index_to_voltage(struct radeon_device *rdev,
 					    u16 voltage)
 {
@@ -556,9 +602,9 @@ static u16 kv_convert_2bit_index_to_voltage(struct radeon_device *rdev,
 					    u32 vid_2bit)
 {
 	struct kv_power_info *pi = kv_get_pi(rdev);
-	u32 vid_8bit = sumo_convert_vid2_to_vid7(rdev,
-						 &pi->sys_info.vid_mapping_table,
-						 vid_2bit);
+	u32 vid_8bit = kv_convert_vid2_to_vid7(rdev,
+					       &pi->sys_info.vid_mapping_table,
+					       vid_2bit);
 
 	return kv_convert_8bit_index_to_voltage(rdev, (u16)vid_8bit);
 }
@@ -639,7 +685,7 @@ static int kv_force_lowest_valid(struct radeon_device *rdev)
 
 static int kv_unforce_levels(struct radeon_device *rdev)
 {
-	if (rdev->family == CHIP_KABINI)
+	if (rdev->family == CHIP_KABINI || rdev->family == CHIP_MULLINS)
 		return kv_notify_message_to_smu(rdev, PPSMC_MSG_NoForcedLevel);
 	else
 		return kv_set_enabled_levels(rdev);
@@ -1362,13 +1408,20 @@ static int kv_update_uvd_dpm(struct radeon_device *rdev, bool gate)
 	struct radeon_uvd_clock_voltage_dependency_table *table =
 		&rdev->pm.dpm.dyn_state.uvd_clock_voltage_dependency_table;
 	int ret;
+	u32 mask;
 
 	if (!gate) {
-		if (!pi->caps_uvd_dpm || table->count || pi->caps_stable_p_state)
+		if (table->count)
 			pi->uvd_boot_level = table->count - 1;
 		else
 			pi->uvd_boot_level = 0;
 
+		if (!pi->caps_uvd_dpm || pi->caps_stable_p_state) {
+			mask = 1 << pi->uvd_boot_level;
+		} else {
+			mask = 0x1f;
+		}
+
 		ret = kv_copy_bytes_to_smc(rdev,
 					   pi->dpm_table_start +
 					   offsetof(SMU7_Fusion_DpmTable, UvdBootLevel),
@@ -1377,11 +1430,9 @@ static int kv_update_uvd_dpm(struct radeon_device *rdev, bool gate)
 		if (ret)
 			return ret;
 
-		if (!pi->caps_uvd_dpm ||
-		    pi->caps_stable_p_state)
-			kv_send_msg_to_smc_with_parameter(rdev,
-							  PPSMC_MSG_UVDDPM_SetEnabledMask,
-							  (1 << pi->uvd_boot_level));
+		kv_send_msg_to_smc_with_parameter(rdev,
+						  PPSMC_MSG_UVDDPM_SetEnabledMask,
+						  mask);
 	}
 
 	return kv_enable_uvd_dpm(rdev, !gate);
@@ -1617,7 +1668,7 @@ static void kv_dpm_powergate_acp(struct radeon_device *rdev, bool gate)
 	if (pi->acp_power_gated == gate)
 		return;
 
-	if (rdev->family == CHIP_KABINI)
+	if (rdev->family == CHIP_KABINI || rdev->family == CHIP_MULLINS)
 		return;
 
 	pi->acp_power_gated = gate;
@@ -1786,7 +1837,7 @@ int kv_dpm_set_power_state(struct radeon_device *rdev)
 		}
 	}
 
-	if (rdev->family == CHIP_KABINI) {
+	if (rdev->family == CHIP_KABINI || rdev->family == CHIP_MULLINS) {
 		if (pi->enable_dpm) {
 			kv_set_valid_clock_range(rdev, new_ps);
 			kv_update_dfs_bypass_settings(rdev, new_ps);
@@ -1812,6 +1863,8 @@ int kv_dpm_set_power_state(struct radeon_device *rdev)
 				return ret;
 			}
 			kv_update_sclk_t(rdev);
+			if (rdev->family == CHIP_MULLINS)
+				kv_enable_nb_dpm(rdev);
 		}
 	} else {
 		if (pi->enable_dpm) {
@@ -1862,7 +1915,7 @@ void kv_dpm_reset_asic(struct radeon_device *rdev)
 {
 	struct kv_power_info *pi = kv_get_pi(rdev);
 
-	if (rdev->family == CHIP_KABINI) {
+	if (rdev->family == CHIP_KABINI || rdev->family == CHIP_MULLINS) {
 		kv_force_lowest_valid(rdev);
 		kv_init_graphics_levels(rdev);
 		kv_program_bootup_state(rdev);
@@ -1901,14 +1954,41 @@ static void kv_construct_max_power_limits_table(struct radeon_device *rdev,
 static void kv_patch_voltage_values(struct radeon_device *rdev)
 {
 	int i;
-	struct radeon_uvd_clock_voltage_dependency_table *table =
+	struct radeon_uvd_clock_voltage_dependency_table *uvd_table =
 		&rdev->pm.dpm.dyn_state.uvd_clock_voltage_dependency_table;
+	struct radeon_vce_clock_voltage_dependency_table *vce_table =
+		&rdev->pm.dpm.dyn_state.vce_clock_voltage_dependency_table;
+	struct radeon_clock_voltage_dependency_table *samu_table =
+		&rdev->pm.dpm.dyn_state.samu_clock_voltage_dependency_table;
+	struct radeon_clock_voltage_dependency_table *acp_table =
+		&rdev->pm.dpm.dyn_state.acp_clock_voltage_dependency_table;
 
-	if (table->count) {
-		for (i = 0; i < table->count; i++)
-			table->entries[i].v =
+	if (uvd_table->count) {
+		for (i = 0; i < uvd_table->count; i++)
+			uvd_table->entries[i].v =
 				kv_convert_8bit_index_to_voltage(rdev,
-								 table->entries[i].v);
+								 uvd_table->entries[i].v);
+	}
+
+	if (vce_table->count) {
+		for (i = 0; i < vce_table->count; i++)
+			vce_table->entries[i].v =
+				kv_convert_8bit_index_to_voltage(rdev,
+								 vce_table->entries[i].v);
+	}
+
+	if (samu_table->count) {
+		for (i = 0; i < samu_table->count; i++)
+			samu_table->entries[i].v =
+				kv_convert_8bit_index_to_voltage(rdev,
+								 samu_table->entries[i].v);
+	}
+
+	if (acp_table->count) {
+		for (i = 0; i < acp_table->count; i++)
+			acp_table->entries[i].v =
+				kv_convert_8bit_index_to_voltage(rdev,
+								 acp_table->entries[i].v);
 	}
 
 }
@@ -1941,7 +2021,7 @@ static int kv_force_dpm_highest(struct radeon_device *rdev)
 			break;
 	}
 
-	if (rdev->family == CHIP_KABINI)
+	if (rdev->family == CHIP_KABINI || rdev->family == CHIP_MULLINS)
 		return kv_send_msg_to_smc_with_parameter(rdev, PPSMC_MSG_DPM_ForceState, i);
 	else
 		return kv_set_enabled_level(rdev, i);
@@ -1961,7 +2041,7 @@ static int kv_force_dpm_lowest(struct radeon_device *rdev)
 			break;
 	}
 
-	if (rdev->family == CHIP_KABINI)
+	if (rdev->family == CHIP_KABINI || rdev->family == CHIP_MULLINS)
 		return kv_send_msg_to_smc_with_parameter(rdev, PPSMC_MSG_DPM_ForceState, i);
 	else
 		return kv_set_enabled_level(rdev, i);
@@ -2118,7 +2198,7 @@ static void kv_apply_state_adjust_rules(struct radeon_device *rdev,
 	else
 		pi->battery_state = false;
 
-	if (rdev->family == CHIP_KABINI) {
+	if (rdev->family == CHIP_KABINI || rdev->family == CHIP_MULLINS) {
 		ps->dpm0_pg_nb_ps_lo = 0x1;
 		ps->dpm0_pg_nb_ps_hi = 0x0;
 		ps->dpmx_nb_ps_lo = 0x1;
@@ -2179,7 +2259,7 @@ static int kv_calculate_nbps_level_settings(struct radeon_device *rdev)
 	if (pi->lowest_valid > pi->highest_valid)
 		return -EINVAL;
 
-	if (rdev->family == CHIP_KABINI) {
+	if (rdev->family == CHIP_KABINI || rdev->family == CHIP_MULLINS) {
 		for (i = pi->lowest_valid; i <= pi->highest_valid; i++) {
 			pi->graphics_level[i].GnbSlow = 1;
 			pi->graphics_level[i].ForceNbPs1 = 0;
@@ -2253,9 +2333,9 @@ static void kv_init_graphics_levels(struct radeon_device *rdev)
 				break;
 
 			kv_set_divider_value(rdev, i, table->entries[i].clk);
-			vid_2bit = sumo_convert_vid7_to_vid2(rdev,
-							     &pi->sys_info.vid_mapping_table,
-							     table->entries[i].v);
+			vid_2bit = kv_convert_vid7_to_vid2(rdev,
+							   &pi->sys_info.vid_mapping_table,
+							   table->entries[i].v);
 			kv_set_vid(rdev, i, vid_2bit);
 			kv_set_at(rdev, i, pi->at[i]);
 			kv_dpm_power_level_enabled_for_throttle(rdev, i, true);
@@ -2324,7 +2404,7 @@ static void kv_program_nbps_index_settings(struct radeon_device *rdev,
 	struct kv_power_info *pi = kv_get_pi(rdev);
 	u32 nbdpmconfig1;
 
-	if (rdev->family == CHIP_KABINI)
+	if (rdev->family == CHIP_KABINI || rdev->family == CHIP_MULLINS)
 		return;
 
 	if (pi->sys_info.nb_dpm_enable) {
@@ -2631,9 +2711,6 @@ int kv_dpm_init(struct radeon_device *rdev)
 
         pi->sram_end = SMC_RAM_END;
 
-	if (rdev->family == CHIP_KABINI)
-		pi->high_voltage_t = 4001;
-
 	pi->enable_nb_dpm = true;
 
 	pi->caps_power_containment = true;
diff --git a/drivers/gpu/drm/radeon/r600.c b/drivers/gpu/drm/radeon/r600.c
index 6e887d004eb..bbc189fd3dd 100644
--- a/drivers/gpu/drm/radeon/r600.c
+++ b/drivers/gpu/drm/radeon/r600.c
@@ -2839,6 +2839,7 @@ int r600_copy_cpdma(struct radeon_device *rdev,
 	r = radeon_fence_emit(rdev, fence, ring->idx);
 	if (r) {
 		radeon_ring_unlock_undo(rdev, ring);
+		radeon_semaphore_free(rdev, &sem, NULL);
 		return r;
 	}
 
@@ -3505,7 +3506,6 @@ int r600_irq_set(struct radeon_device *rdev)
 	u32 hpd1, hpd2, hpd3, hpd4 = 0, hpd5 = 0, hpd6 = 0;
 	u32 grbm_int_cntl = 0;
 	u32 hdmi0, hdmi1;
-	u32 d1grph = 0, d2grph = 0;
 	u32 dma_cntl;
 	u32 thermal_int = 0;
 
@@ -3614,8 +3614,8 @@ int r600_irq_set(struct radeon_device *rdev)
 	WREG32(CP_INT_CNTL, cp_int_cntl);
 	WREG32(DMA_CNTL, dma_cntl);
 	WREG32(DxMODE_INT_MASK, mode_int);
-	WREG32(D1GRPH_INTERRUPT_CONTROL, d1grph);
-	WREG32(D2GRPH_INTERRUPT_CONTROL, d2grph);
+	WREG32(D1GRPH_INTERRUPT_CONTROL, DxGRPH_PFLIP_INT_MASK);
+	WREG32(D2GRPH_INTERRUPT_CONTROL, DxGRPH_PFLIP_INT_MASK);
 	WREG32(GRBM_INT_CNTL, grbm_int_cntl);
 	if (ASIC_IS_DCE3(rdev)) {
 		WREG32(DC_HPD1_INT_CONTROL, hpd1);
@@ -3918,6 +3918,14 @@ restart_ih:
 				break;
 			}
 			break;
+		case 9: /* D1 pflip */
+			DRM_DEBUG("IH: D1 flip\n");
+			radeon_crtc_handle_flip(rdev, 0);
+			break;
+		case 11: /* D2 pflip */
+			DRM_DEBUG("IH: D2 flip\n");
+			radeon_crtc_handle_flip(rdev, 1);
+			break;
 		case 19: /* HPD/DAC hotplug */
 			switch (src_data) {
 			case 0:
diff --git a/drivers/gpu/drm/radeon/r600_dma.c b/drivers/gpu/drm/radeon/r600_dma.c
index 53fcb28f557..4969cef44a1 100644
--- a/drivers/gpu/drm/radeon/r600_dma.c
+++ b/drivers/gpu/drm/radeon/r600_dma.c
@@ -489,6 +489,7 @@ int r600_copy_dma(struct radeon_device *rdev,
 	r = radeon_fence_emit(rdev, fence, ring->idx);
 	if (r) {
 		radeon_ring_unlock_undo(rdev, ring);
+		radeon_semaphore_free(rdev, &sem, NULL);
 		return r;
 	}
 
diff --git a/drivers/gpu/drm/radeon/r600_dpm.c b/drivers/gpu/drm/radeon/r600_dpm.c
index cbf7e3269f8..9c61b74ef44 100644
--- a/drivers/gpu/drm/radeon/r600_dpm.c
+++ b/drivers/gpu/drm/radeon/r600_dpm.c
@@ -158,16 +158,18 @@ u32 r600_dpm_get_vblank_time(struct radeon_device *rdev)
 	u32 line_time_us, vblank_lines;
 	u32 vblank_time_us = 0xffffffff; /* if the displays are off, vblank time is max */
 
-	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
-		radeon_crtc = to_radeon_crtc(crtc);
-		if (crtc->enabled && radeon_crtc->enabled && radeon_crtc->hw_mode.clock) {
-			line_time_us = (radeon_crtc->hw_mode.crtc_htotal * 1000) /
-				radeon_crtc->hw_mode.clock;
-			vblank_lines = radeon_crtc->hw_mode.crtc_vblank_end -
-				radeon_crtc->hw_mode.crtc_vdisplay +
-				(radeon_crtc->v_border * 2);
-			vblank_time_us = vblank_lines * line_time_us;
-			break;
+	if (rdev->num_crtc && rdev->mode_info.mode_config_initialized) {
+		list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
+			radeon_crtc = to_radeon_crtc(crtc);
+			if (crtc->enabled && radeon_crtc->enabled && radeon_crtc->hw_mode.clock) {
+				line_time_us = (radeon_crtc->hw_mode.crtc_htotal * 1000) /
+					radeon_crtc->hw_mode.clock;
+				vblank_lines = radeon_crtc->hw_mode.crtc_vblank_end -
+					radeon_crtc->hw_mode.crtc_vdisplay +
+					(radeon_crtc->v_border * 2);
+				vblank_time_us = vblank_lines * line_time_us;
+				break;
+			}
 		}
 	}
 
@@ -181,14 +183,15 @@ u32 r600_dpm_get_vrefresh(struct radeon_device *rdev)
 	struct radeon_crtc *radeon_crtc;
 	u32 vrefresh = 0;
 
-	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
-		radeon_crtc = to_radeon_crtc(crtc);
-		if (crtc->enabled && radeon_crtc->enabled && radeon_crtc->hw_mode.clock) {
-			vrefresh = radeon_crtc->hw_mode.vrefresh;
-			break;
+	if (rdev->num_crtc && rdev->mode_info.mode_config_initialized) {
+		list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
+			radeon_crtc = to_radeon_crtc(crtc);
+			if (crtc->enabled && radeon_crtc->enabled && radeon_crtc->hw_mode.clock) {
+				vrefresh = radeon_crtc->hw_mode.vrefresh;
+				break;
+			}
 		}
 	}
-
 	return vrefresh;
 }
 
diff --git a/drivers/gpu/drm/radeon/radeon.h b/drivers/gpu/drm/radeon/radeon.h
index f21db7a0b34..8149e7cf430 100644
--- a/drivers/gpu/drm/radeon/radeon.h
+++ b/drivers/gpu/drm/radeon/radeon.h
@@ -730,6 +730,12 @@ struct cik_irq_stat_regs {
 	u32 disp_int_cont4;
 	u32 disp_int_cont5;
 	u32 disp_int_cont6;
+	u32 d1grph_int;
+	u32 d2grph_int;
+	u32 d3grph_int;
+	u32 d4grph_int;
+	u32 d5grph_int;
+	u32 d6grph_int;
 };
 
 union radeon_irq_stat_regs {
@@ -739,7 +745,7 @@ union radeon_irq_stat_regs {
 	struct cik_irq_stat_regs cik;
 };
 
-#define RADEON_MAX_HPD_PINS 6
+#define RADEON_MAX_HPD_PINS 7
 #define RADEON_MAX_CRTCS 6
 #define RADEON_MAX_AFMT_BLOCKS 7
 
@@ -1636,6 +1642,7 @@ struct radeon_vce {
 	unsigned		fb_version;
 	atomic_t		handles[RADEON_MAX_VCE_HANDLES];
 	struct drm_file		*filp[RADEON_MAX_VCE_HANDLES];
+	unsigned		img_size[RADEON_MAX_VCE_HANDLES];
 	struct delayed_work	idle_work;
 };
 
@@ -1649,7 +1656,7 @@ int radeon_vce_get_destroy_msg(struct radeon_device *rdev, int ring,
 			       uint32_t handle, struct radeon_fence **fence);
 void radeon_vce_free_handles(struct radeon_device *rdev, struct drm_file *filp);
 void radeon_vce_note_usage(struct radeon_device *rdev);
-int radeon_vce_cs_reloc(struct radeon_cs_parser *p, int lo, int hi);
+int radeon_vce_cs_reloc(struct radeon_cs_parser *p, int lo, int hi, unsigned size);
 int radeon_vce_cs_parse(struct radeon_cs_parser *p);
 bool radeon_vce_semaphore_emit(struct radeon_device *rdev,
 			       struct radeon_ring *ring,
@@ -2321,6 +2328,7 @@ struct radeon_device {
 	bool have_disp_power_ref;
 };
 
+bool radeon_is_px(struct drm_device *dev);
 int radeon_device_init(struct radeon_device *rdev,
 		       struct drm_device *ddev,
 		       struct pci_dev *pdev,
@@ -2631,6 +2639,10 @@ void r100_pll_errata_after_index(struct radeon_device *rdev);
 #define ASIC_IS_DCE64(rdev) ((rdev->family == CHIP_OLAND))
 #define ASIC_IS_NODCE(rdev) ((rdev->family == CHIP_HAINAN))
 #define ASIC_IS_DCE8(rdev) ((rdev->family >= CHIP_BONAIRE))
+#define ASIC_IS_DCE81(rdev) ((rdev->family == CHIP_KAVERI))
+#define ASIC_IS_DCE82(rdev) ((rdev->family == CHIP_BONAIRE))
+#define ASIC_IS_DCE83(rdev) ((rdev->family == CHIP_KABINI) || \
+			     (rdev->family == CHIP_MULLINS))
 
 #define ASIC_IS_LOMBOK(rdev) ((rdev->ddev->pdev->device == 0x6849) || \
 			      (rdev->ddev->pdev->device == 0x6850) || \
diff --git a/drivers/gpu/drm/radeon/radeon_asic.c b/drivers/gpu/drm/radeon/radeon_asic.c
index b8a24a75d4f..be20e62dac8 100644
--- a/drivers/gpu/drm/radeon/radeon_asic.c
+++ b/drivers/gpu/drm/radeon/radeon_asic.c
@@ -2516,6 +2516,7 @@ int radeon_asic_init(struct radeon_device *rdev)
 		break;
 	case CHIP_KAVERI:
 	case CHIP_KABINI:
+	case CHIP_MULLINS:
 		rdev->asic = &kv_asic;
 		/* set num crtcs */
 		if (rdev->family == CHIP_KAVERI) {
diff --git a/drivers/gpu/drm/radeon/radeon_atpx_handler.c b/drivers/gpu/drm/radeon/radeon_atpx_handler.c
index fa9a9c02751..a9fb0d016d3 100644
--- a/drivers/gpu/drm/radeon/radeon_atpx_handler.c
+++ b/drivers/gpu/drm/radeon/radeon_atpx_handler.c
@@ -59,7 +59,7 @@ struct atpx_mux {
 	u16 mux;
 } __packed;
 
-bool radeon_is_px(void) {
+bool radeon_has_atpx(void) {
 	return radeon_atpx_priv.atpx_detected;
 }
 
@@ -528,6 +528,13 @@ static bool radeon_atpx_detect(void)
 		has_atpx |= (radeon_atpx_pci_probe_handle(pdev) == true);
 	}
 
+	/* some newer PX laptops mark the dGPU as a non-VGA display device */
+	while ((pdev = pci_get_class(PCI_CLASS_DISPLAY_OTHER << 8, pdev)) != NULL) {
+		vga_count++;
+
+		has_atpx |= (radeon_atpx_pci_probe_handle(pdev) == true);
+	}
+
 	if (has_atpx && vga_count == 2) {
 		acpi_get_name(radeon_atpx_priv.atpx.handle, ACPI_FULL_PATHNAME, &buffer);
 		printk(KERN_INFO "VGA switcheroo: detected switching method %s handle\n",
diff --git a/drivers/gpu/drm/radeon/radeon_bios.c b/drivers/gpu/drm/radeon/radeon_bios.c
index b3633d9a531..9ab30976287 100644
--- a/drivers/gpu/drm/radeon/radeon_bios.c
+++ b/drivers/gpu/drm/radeon/radeon_bios.c
@@ -196,6 +196,20 @@ static bool radeon_atrm_get_bios(struct radeon_device *rdev)
 		}
 	}
 
+	if (!found) {
+		while ((pdev = pci_get_class(PCI_CLASS_DISPLAY_OTHER << 8, pdev)) != NULL) {
+			dhandle = ACPI_HANDLE(&pdev->dev);
+			if (!dhandle)
+				continue;
+
+			status = acpi_get_handle(dhandle, "ATRM", &atrm_handle);
+			if (!ACPI_FAILURE(status)) {
+				found = true;
+				break;
+			}
+		}
+	}
+
 	if (!found)
 		return false;
 
diff --git a/drivers/gpu/drm/radeon/radeon_connectors.c b/drivers/gpu/drm/radeon/radeon_connectors.c
index c566b486ca0..ea50e0ae7bf 100644
--- a/drivers/gpu/drm/radeon/radeon_connectors.c
+++ b/drivers/gpu/drm/radeon/radeon_connectors.c
@@ -1261,21 +1261,6 @@ static const struct drm_connector_funcs radeon_dvi_connector_funcs = {
 	.force = radeon_dvi_force,
 };
 
-static void radeon_dp_connector_destroy(struct drm_connector *connector)
-{
-	struct radeon_connector *radeon_connector = to_radeon_connector(connector);
-	struct radeon_connector_atom_dig *radeon_dig_connector = radeon_connector->con_priv;
-
-	if (radeon_connector->edid)
-		kfree(radeon_connector->edid);
-	if (radeon_dig_connector->dp_i2c_bus)
-		radeon_i2c_destroy(radeon_dig_connector->dp_i2c_bus);
-	kfree(radeon_connector->con_priv);
-	drm_sysfs_connector_remove(connector);
-	drm_connector_cleanup(connector);
-	kfree(connector);
-}
-
 static int radeon_dp_get_modes(struct drm_connector *connector)
 {
 	struct radeon_connector *radeon_connector = to_radeon_connector(connector);
@@ -1553,7 +1538,7 @@ static const struct drm_connector_funcs radeon_dp_connector_funcs = {
 	.detect = radeon_dp_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.set_property = radeon_connector_set_property,
-	.destroy = radeon_dp_connector_destroy,
+	.destroy = radeon_connector_destroy,
 	.force = radeon_dvi_force,
 };
 
@@ -1562,7 +1547,7 @@ static const struct drm_connector_funcs radeon_edp_connector_funcs = {
 	.detect = radeon_dp_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.set_property = radeon_lvds_set_property,
-	.destroy = radeon_dp_connector_destroy,
+	.destroy = radeon_connector_destroy,
 	.force = radeon_dvi_force,
 };
 
@@ -1571,7 +1556,7 @@ static const struct drm_connector_funcs radeon_lvds_bridge_connector_funcs = {
 	.detect = radeon_dp_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.set_property = radeon_lvds_set_property,
-	.destroy = radeon_dp_connector_destroy,
+	.destroy = radeon_connector_destroy,
 	.force = radeon_dvi_force,
 };
 
@@ -1668,17 +1653,10 @@ radeon_add_atom_connector(struct drm_device *dev,
 		radeon_dig_connector->igp_lane_info = igp_lane_info;
 		radeon_connector->con_priv = radeon_dig_connector;
 		if (i2c_bus->valid) {
-			/* add DP i2c bus */
-			if (connector_type == DRM_MODE_CONNECTOR_eDP)
-				radeon_dig_connector->dp_i2c_bus = radeon_i2c_create_dp(dev, i2c_bus, "eDP-auxch");
-			else
-				radeon_dig_connector->dp_i2c_bus = radeon_i2c_create_dp(dev, i2c_bus, "DP-auxch");
-			if (radeon_dig_connector->dp_i2c_bus)
+			radeon_connector->ddc_bus = radeon_i2c_lookup(rdev, i2c_bus);
+			if (radeon_connector->ddc_bus)
 				has_aux = true;
 			else
-				DRM_ERROR("DP: Failed to assign dp ddc bus! Check dmesg for i2c errors.\n");
-			radeon_connector->ddc_bus = radeon_i2c_lookup(rdev, i2c_bus);
-			if (!radeon_connector->ddc_bus)
 				DRM_ERROR("DP: Failed to assign ddc bus! Check dmesg for i2c errors.\n");
 		}
 		switch (connector_type) {
@@ -1893,10 +1871,6 @@ radeon_add_atom_connector(struct drm_device *dev,
 			drm_connector_init(dev, &radeon_connector->base, &radeon_dp_connector_funcs, connector_type);
 			drm_connector_helper_add(&radeon_connector->base, &radeon_dp_connector_helper_funcs);
 			if (i2c_bus->valid) {
-				/* add DP i2c bus */
-				radeon_dig_connector->dp_i2c_bus = radeon_i2c_create_dp(dev, i2c_bus, "DP-auxch");
-				if (!radeon_dig_connector->dp_i2c_bus)
-					DRM_ERROR("DP: Failed to assign dp ddc bus! Check dmesg for i2c errors.\n");
 				radeon_connector->ddc_bus = radeon_i2c_lookup(rdev, i2c_bus);
 				if (radeon_connector->ddc_bus)
 					has_aux = true;
@@ -1942,14 +1916,10 @@ radeon_add_atom_connector(struct drm_device *dev,
 			drm_connector_init(dev, &radeon_connector->base, &radeon_edp_connector_funcs, connector_type);
 			drm_connector_helper_add(&radeon_connector->base, &radeon_dp_connector_helper_funcs);
 			if (i2c_bus->valid) {
-				/* add DP i2c bus */
-				radeon_dig_connector->dp_i2c_bus = radeon_i2c_create_dp(dev, i2c_bus, "eDP-auxch");
-				if (radeon_dig_connector->dp_i2c_bus)
+				radeon_connector->ddc_bus = radeon_i2c_lookup(rdev, i2c_bus);
+				if (radeon_connector->ddc_bus)
 					has_aux = true;
 				else
-					DRM_ERROR("DP: Failed to assign dp ddc bus! Check dmesg for i2c errors.\n");
-				radeon_connector->ddc_bus = radeon_i2c_lookup(rdev, i2c_bus);
-				if (!radeon_connector->ddc_bus)
 					DRM_ERROR("DP: Failed to assign ddc bus! Check dmesg for i2c errors.\n");
 			}
 			drm_object_attach_property(&radeon_connector->base.base,
diff --git a/drivers/gpu/drm/radeon/radeon_cs.c b/drivers/gpu/drm/radeon/radeon_cs.c
index 2b6e0ebcc13..41ecf8a6061 100644
--- a/drivers/gpu/drm/radeon/radeon_cs.c
+++ b/drivers/gpu/drm/radeon/radeon_cs.c
@@ -152,6 +152,12 @@ static int radeon_cs_parser_relocs(struct radeon_cs_parser *p)
 			uint32_t domain = r->write_domain ?
 				r->write_domain : r->read_domains;
 
+			if (domain & RADEON_GEM_DOMAIN_CPU) {
+				DRM_ERROR("RADEON_GEM_DOMAIN_CPU is not valid "
+					  "for command submission\n");
+				return -EINVAL;
+			}
+
 			p->relocs[i].domain = domain;
 			if (domain == RADEON_GEM_DOMAIN_VRAM)
 				domain |= RADEON_GEM_DOMAIN_GTT;
@@ -342,10 +348,17 @@ int radeon_cs_parser_init(struct radeon_cs_parser *p, void *data)
 			return -EINVAL;
 
 		/* we only support VM on some SI+ rings */
-		if ((p->rdev->asic->ring[p->ring]->cs_parse == NULL) &&
-		   ((p->cs_flags & RADEON_CS_USE_VM) == 0)) {
-			DRM_ERROR("Ring %d requires VM!\n", p->ring);
-			return -EINVAL;
+		if ((p->cs_flags & RADEON_CS_USE_VM) == 0) {
+			if (p->rdev->asic->ring[p->ring]->cs_parse == NULL) {
+				DRM_ERROR("Ring %d requires VM!\n", p->ring);
+				return -EINVAL;
+			}
+		} else {
+			if (p->rdev->asic->ring[p->ring]->ib_parse == NULL) {
+				DRM_ERROR("VM not supported on ring %d!\n",
+					  p->ring);
+				return -EINVAL;
+			}
 		}
 	}
 
diff --git a/drivers/gpu/drm/radeon/radeon_device.c b/drivers/gpu/drm/radeon/radeon_device.c
index 835516d2d25..14671406212 100644
--- a/drivers/gpu/drm/radeon/radeon_device.c
+++ b/drivers/gpu/drm/radeon/radeon_device.c
@@ -99,14 +99,18 @@ static const char radeon_family_name[][16] = {
 	"KAVERI",
 	"KABINI",
 	"HAWAII",
+	"MULLINS",
 	"LAST",
 };
 
-#if defined(CONFIG_VGA_SWITCHEROO)
-bool radeon_is_px(void);
-#else
-static inline bool radeon_is_px(void) { return false; }
-#endif
+bool radeon_is_px(struct drm_device *dev)
+{
+	struct radeon_device *rdev = dev->dev_private;
+
+	if (rdev->flags & RADEON_IS_PX)
+		return true;
+	return false;
+}
 
 /**
  * radeon_program_register_sequence - program an array of registers.
@@ -1082,7 +1086,7 @@ static void radeon_switcheroo_set_state(struct pci_dev *pdev, enum vga_switchero
 {
 	struct drm_device *dev = pci_get_drvdata(pdev);
 
-	if (radeon_is_px() && state == VGA_SWITCHEROO_OFF)
+	if (radeon_is_px(dev) && state == VGA_SWITCHEROO_OFF)
 		return;
 
 	if (state == VGA_SWITCHEROO_ON) {
@@ -1301,9 +1305,7 @@ int radeon_device_init(struct radeon_device *rdev,
 	 * ignore it */
 	vga_client_register(rdev->pdev, rdev, NULL, radeon_vga_set_decode);
 
-	if (radeon_runtime_pm == 1)
-		runtime = true;
-	if ((radeon_runtime_pm == -1) && radeon_is_px())
+	if (rdev->flags & RADEON_IS_PX)
 		runtime = true;
 	vga_switcheroo_register_client(rdev->pdev, &radeon_switcheroo_ops, runtime);
 	if (runtime)
@@ -1531,11 +1533,6 @@ int radeon_resume_kms(struct drm_device *dev, bool resume, bool fbcon)
 
 	radeon_restore_bios_scratch_regs(rdev);
 
-	if (fbcon) {
-		radeon_fbdev_set_suspend(rdev, 0);
-		console_unlock();
-	}
-
 	/* init dig PHYs, disp eng pll */
 	if (rdev->is_atom_bios) {
 		radeon_atom_encoder_init(rdev);
@@ -1560,6 +1557,12 @@ int radeon_resume_kms(struct drm_device *dev, bool resume, bool fbcon)
 	}
 
 	drm_kms_helper_poll_enable(dev);
+
+	if (fbcon) {
+		radeon_fbdev_set_suspend(rdev, 0);
+		console_unlock();
+	}
+
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/radeon/radeon_display.c b/drivers/gpu/drm/radeon/radeon_display.c
index 386cfa4c194..356b733caaf 100644
--- a/drivers/gpu/drm/radeon/radeon_display.c
+++ b/drivers/gpu/drm/radeon/radeon_display.c
@@ -284,6 +284,10 @@ void radeon_crtc_handle_flip(struct radeon_device *rdev, int crtc_id)
 	u32 update_pending;
 	int vpos, hpos;
 
+	/* can happen during initialization */
+	if (radeon_crtc == NULL)
+		return;
+
 	spin_lock_irqsave(&rdev->ddev->event_lock, flags);
 	work = radeon_crtc->unpin_work;
 	if (work == NULL ||
@@ -759,19 +763,18 @@ int radeon_ddc_get_modes(struct radeon_connector *radeon_connector)
 
 	if (radeon_connector_encoder_get_dp_bridge_encoder_id(&radeon_connector->base) !=
 	    ENCODER_OBJECT_ID_NONE) {
-		struct radeon_connector_atom_dig *dig = radeon_connector->con_priv;
-
-		if (dig->dp_i2c_bus)
+		if (radeon_connector->ddc_bus->has_aux)
 			radeon_connector->edid = drm_get_edid(&radeon_connector->base,
-							      &dig->dp_i2c_bus->adapter);
+							      &radeon_connector->ddc_bus->aux.ddc);
 	} else if ((radeon_connector->base.connector_type == DRM_MODE_CONNECTOR_DisplayPort) ||
 		   (radeon_connector->base.connector_type == DRM_MODE_CONNECTOR_eDP)) {
 		struct radeon_connector_atom_dig *dig = radeon_connector->con_priv;
 
 		if ((dig->dp_sink_type == CONNECTOR_OBJECT_ID_DISPLAYPORT ||
-		     dig->dp_sink_type == CONNECTOR_OBJECT_ID_eDP) && dig->dp_i2c_bus)
+		     dig->dp_sink_type == CONNECTOR_OBJECT_ID_eDP) &&
+		    radeon_connector->ddc_bus->has_aux)
 			radeon_connector->edid = drm_get_edid(&radeon_connector->base,
-							      &dig->dp_i2c_bus->adapter);
+							      &radeon_connector->ddc_bus->aux.ddc);
 		else if (radeon_connector->ddc_bus && !radeon_connector->edid)
 			radeon_connector->edid = drm_get_edid(&radeon_connector->base,
 							      &radeon_connector->ddc_bus->adapter);
@@ -827,20 +830,52 @@ static void avivo_reduce_ratio(unsigned *nom, unsigned *den,
 
 	/* make sure nominator is large enough */
         if (*nom < nom_min) {
-		tmp = (nom_min + *nom - 1) / *nom;
+		tmp = DIV_ROUND_UP(nom_min, *nom);
 		*nom *= tmp;
 		*den *= tmp;
 	}
 
 	/* make sure the denominator is large enough */
 	if (*den < den_min) {
-		tmp = (den_min + *den - 1) / *den;
+		tmp = DIV_ROUND_UP(den_min, *den);
 		*nom *= tmp;
 		*den *= tmp;
 	}
 }
 
 /**
+ * avivo_get_fb_ref_div - feedback and ref divider calculation
+ *
+ * @nom: nominator
+ * @den: denominator
+ * @post_div: post divider
+ * @fb_div_max: feedback divider maximum
+ * @ref_div_max: reference divider maximum
+ * @fb_div: resulting feedback divider
+ * @ref_div: resulting reference divider
+ *
+ * Calculate feedback and reference divider for a given post divider. Makes
+ * sure we stay within the limits.
+ */
+static void avivo_get_fb_ref_div(unsigned nom, unsigned den, unsigned post_div,
+				 unsigned fb_div_max, unsigned ref_div_max,
+				 unsigned *fb_div, unsigned *ref_div)
+{
+	/* limit reference * post divider to a maximum */
+	ref_div_max = max(min(100 / post_div, ref_div_max), 1u);
+
+	/* get matching reference and feedback divider */
+	*ref_div = min(max(DIV_ROUND_CLOSEST(den, post_div), 1u), ref_div_max);
+	*fb_div = DIV_ROUND_CLOSEST(nom * *ref_div * post_div, den);
+
+	/* limit fb divider to its maximum */
+        if (*fb_div > fb_div_max) {
+		*ref_div = DIV_ROUND_CLOSEST(*ref_div * fb_div_max, *fb_div);
+		*fb_div = fb_div_max;
+	}
+}
+
+/**
  * radeon_compute_pll_avivo - compute PLL paramaters
  *
  * @pll: information about the PLL
@@ -861,11 +896,14 @@ void radeon_compute_pll_avivo(struct radeon_pll *pll,
 			      u32 *ref_div_p,
 			      u32 *post_div_p)
 {
+	unsigned target_clock = pll->flags & RADEON_PLL_USE_FRAC_FB_DIV ?
+		freq : freq / 10;
+
 	unsigned fb_div_min, fb_div_max, fb_div;
 	unsigned post_div_min, post_div_max, post_div;
 	unsigned ref_div_min, ref_div_max, ref_div;
 	unsigned post_div_best, diff_best;
-	unsigned nom, den, tmp;
+	unsigned nom, den;
 
 	/* determine allowed feedback divider range */
 	fb_div_min = pll->min_feedback_div;
@@ -881,14 +919,18 @@ void radeon_compute_pll_avivo(struct radeon_pll *pll,
 		ref_div_min = pll->reference_div;
 	else
 		ref_div_min = pll->min_ref_div;
-	ref_div_max = pll->max_ref_div;
+
+	if (pll->flags & RADEON_PLL_USE_FRAC_FB_DIV &&
+	    pll->flags & RADEON_PLL_USE_REF_DIV)
+		ref_div_max = pll->reference_div;
+	else
+		ref_div_max = pll->max_ref_div;
 
 	/* determine allowed post divider range */
 	if (pll->flags & RADEON_PLL_USE_POST_DIV) {
 		post_div_min = pll->post_div;
 		post_div_max = pll->post_div;
 	} else {
-		unsigned target_clock = freq / 10;
 		unsigned vco_min, vco_max;
 
 		if (pll->flags & RADEON_PLL_IS_LCD) {
@@ -899,6 +941,11 @@ void radeon_compute_pll_avivo(struct radeon_pll *pll,
 			vco_max = pll->pll_out_max;
 		}
 
+		if (pll->flags & RADEON_PLL_USE_FRAC_FB_DIV) {
+			vco_min *= 10;
+			vco_max *= 10;
+		}
+
 		post_div_min = vco_min / target_clock;
 		if ((target_clock * post_div_min) < vco_min)
 			++post_div_min;
@@ -913,7 +960,7 @@ void radeon_compute_pll_avivo(struct radeon_pll *pll,
 	}
 
 	/* represent the searched ratio as fractional number */
-	nom = pll->flags & RADEON_PLL_USE_FRAC_FB_DIV ? freq : freq / 10;
+	nom = target_clock;
 	den = pll->reference_freq;
 
 	/* reduce the numbers to a simpler ratio */
@@ -927,7 +974,12 @@ void radeon_compute_pll_avivo(struct radeon_pll *pll,
 	diff_best = ~0;
 
 	for (post_div = post_div_min; post_div <= post_div_max; ++post_div) {
-		unsigned diff = abs(den - den / post_div * post_div);
+		unsigned diff;
+		avivo_get_fb_ref_div(nom, den, post_div, fb_div_max,
+				     ref_div_max, &fb_div, &ref_div);
+		diff = abs(target_clock - (pll->reference_freq * fb_div) /
+			(ref_div * post_div));
+
 		if (diff < diff_best || (diff == diff_best &&
 		    !(pll->flags & RADEON_PLL_PREFER_MINM_OVER_MAXP))) {
 
@@ -937,29 +989,24 @@ void radeon_compute_pll_avivo(struct radeon_pll *pll,
 	}
 	post_div = post_div_best;
 
-	/* get matching reference and feedback divider */
-	ref_div = max(den / post_div, 1u);
-	fb_div = nom;
-
-	/* we're almost done, but reference and feedback
-	   divider might be to large now */
-
-	tmp = ref_div;
-
-        if (fb_div > fb_div_max) {
-		ref_div = ref_div * fb_div_max / fb_div;
-		fb_div = fb_div_max;
-	}
-
-	if (ref_div > ref_div_max) {
-		ref_div = ref_div_max;
-		fb_div = nom * ref_div_max / tmp;
-	}
+	/* get the feedback and reference divider for the optimal value */
+	avivo_get_fb_ref_div(nom, den, post_div, fb_div_max, ref_div_max,
+			     &fb_div, &ref_div);
 
 	/* reduce the numbers to a simpler ratio once more */
 	/* this also makes sure that the reference divider is large enough */
 	avivo_reduce_ratio(&fb_div, &ref_div, fb_div_min, ref_div_min);
 
+	/* avoid high jitter with small fractional dividers */
+	if (pll->flags & RADEON_PLL_USE_FRAC_FB_DIV && (fb_div % 10)) {
+		fb_div_min = max(fb_div_min, (9 - (fb_div % 10)) * 20 + 50);
+		if (fb_div < fb_div_min) {
+			unsigned tmp = DIV_ROUND_UP(fb_div_min, fb_div);
+			fb_div *= tmp;
+			ref_div *= tmp;
+		}
+	}
+
 	/* and finally save the result */
 	if (pll->flags & RADEON_PLL_USE_FRAC_FB_DIV) {
 		*fb_div_p = fb_div / 10;
@@ -976,7 +1023,7 @@ void radeon_compute_pll_avivo(struct radeon_pll *pll,
 	*post_div_p = post_div;
 
 	DRM_DEBUG_KMS("%d - %d, pll dividers - fb: %d.%d ref: %d, post %d\n",
-		      freq, *dot_clock_p, *fb_div_p, *frac_fb_div_p,
+		      freq, *dot_clock_p * 10, *fb_div_p, *frac_fb_div_p,
 		      ref_div, post_div);
 }
 
diff --git a/drivers/gpu/drm/radeon/radeon_drv.c b/drivers/gpu/drm/radeon/radeon_drv.c
index d0eba48dd74..c00a2f58518 100644
--- a/drivers/gpu/drm/radeon/radeon_drv.c
+++ b/drivers/gpu/drm/radeon/radeon_drv.c
@@ -115,6 +115,7 @@ extern int radeon_get_crtc_scanoutpos(struct drm_device *dev, int crtc,
 				      unsigned int flags,
 				      int *vpos, int *hpos, ktime_t *stime,
 				      ktime_t *etime);
+extern bool radeon_is_px(struct drm_device *dev);
 extern const struct drm_ioctl_desc radeon_ioctls_kms[];
 extern int radeon_max_kms_ioctl;
 int radeon_mmap(struct file *filp, struct vm_area_struct *vma);
@@ -144,11 +145,9 @@ void radeon_debugfs_cleanup(struct drm_minor *minor);
 #if defined(CONFIG_VGA_SWITCHEROO)
 void radeon_register_atpx_handler(void);
 void radeon_unregister_atpx_handler(void);
-bool radeon_is_px(void);
 #else
 static inline void radeon_register_atpx_handler(void) {}
 static inline void radeon_unregister_atpx_handler(void) {}
-static inline bool radeon_is_px(void) { return false; }
 #endif
 
 int radeon_no_wb;
@@ -186,7 +185,7 @@ module_param_named(dynclks, radeon_dynclks, int, 0444);
 MODULE_PARM_DESC(r4xx_atom, "Enable ATOMBIOS modesetting for R4xx");
 module_param_named(r4xx_atom, radeon_r4xx_atom, int, 0444);
 
-MODULE_PARM_DESC(vramlimit, "Restrict VRAM for testing");
+MODULE_PARM_DESC(vramlimit, "Restrict VRAM for testing, in megabytes");
 module_param_named(vramlimit, radeon_vram_limit, int, 0600);
 
 MODULE_PARM_DESC(agpmode, "AGP Mode (-1 == PCI)");
@@ -405,12 +404,7 @@ static int radeon_pmops_runtime_suspend(struct device *dev)
 	struct drm_device *drm_dev = pci_get_drvdata(pdev);
 	int ret;
 
-	if (radeon_runtime_pm == 0) {
-		pm_runtime_forbid(dev);
-		return -EBUSY;
-	}
-
-	if (radeon_runtime_pm == -1 && !radeon_is_px()) {
+	if (!radeon_is_px(drm_dev)) {
 		pm_runtime_forbid(dev);
 		return -EBUSY;
 	}
@@ -434,10 +428,7 @@ static int radeon_pmops_runtime_resume(struct device *dev)
 	struct drm_device *drm_dev = pci_get_drvdata(pdev);
 	int ret;
 
-	if (radeon_runtime_pm == 0)
-		return -EINVAL;
-
-	if (radeon_runtime_pm == -1 && !radeon_is_px())
+	if (!radeon_is_px(drm_dev))
 		return -EINVAL;
 
 	drm_dev->switch_power_state = DRM_SWITCH_POWER_CHANGING;
@@ -462,14 +453,7 @@ static int radeon_pmops_runtime_idle(struct device *dev)
 	struct drm_device *drm_dev = pci_get_drvdata(pdev);
 	struct drm_crtc *crtc;
 
-	if (radeon_runtime_pm == 0) {
-		pm_runtime_forbid(dev);
-		return -EBUSY;
-	}
-
-	/* are we PX enabled? */
-	if (radeon_runtime_pm == -1 && !radeon_is_px()) {
-		DRM_DEBUG_DRIVER("failing to power off - not px\n");
+	if (!radeon_is_px(drm_dev)) {
 		pm_runtime_forbid(dev);
 		return -EBUSY;
 	}
diff --git a/drivers/gpu/drm/radeon/radeon_family.h b/drivers/gpu/drm/radeon/radeon_family.h
index 614ad549297..4b7b87f71a6 100644
--- a/drivers/gpu/drm/radeon/radeon_family.h
+++ b/drivers/gpu/drm/radeon/radeon_family.h
@@ -97,6 +97,7 @@ enum radeon_family {
 	CHIP_KAVERI,
 	CHIP_KABINI,
 	CHIP_HAWAII,
+	CHIP_MULLINS,
 	CHIP_LAST,
 };
 
@@ -115,6 +116,7 @@ enum radeon_chip_flags {
 	RADEON_NEW_MEMMAP = 0x00400000UL,
 	RADEON_IS_PCI = 0x00800000UL,
 	RADEON_IS_IGPGART = 0x01000000UL,
+	RADEON_IS_PX = 0x02000000UL,
 };
 
 #endif
diff --git a/drivers/gpu/drm/radeon/radeon_i2c.c b/drivers/gpu/drm/radeon/radeon_i2c.c
index e24ca6ab96d..7b944142a9f 100644
--- a/drivers/gpu/drm/radeon/radeon_i2c.c
+++ b/drivers/gpu/drm/radeon/radeon_i2c.c
@@ -64,8 +64,7 @@ bool radeon_ddc_probe(struct radeon_connector *radeon_connector, bool use_aux)
 		radeon_router_select_ddc_port(radeon_connector);
 
 	if (use_aux) {
-		struct radeon_connector_atom_dig *dig = radeon_connector->con_priv;
-		ret = i2c_transfer(&dig->dp_i2c_bus->adapter, msgs, 2);
+		ret = i2c_transfer(&radeon_connector->ddc_bus->aux.ddc, msgs, 2);
 	} else {
 		ret = i2c_transfer(&radeon_connector->ddc_bus->adapter, msgs, 2);
 	}
@@ -950,16 +949,16 @@ struct radeon_i2c_chan *radeon_i2c_create(struct drm_device *dev,
 		/* set the radeon bit adapter */
 		snprintf(i2c->adapter.name, sizeof(i2c->adapter.name),
 			 "Radeon i2c bit bus %s", name);
-		i2c->adapter.algo_data = &i2c->algo.bit;
-		i2c->algo.bit.pre_xfer = pre_xfer;
-		i2c->algo.bit.post_xfer = post_xfer;
-		i2c->algo.bit.setsda = set_data;
-		i2c->algo.bit.setscl = set_clock;
-		i2c->algo.bit.getsda = get_data;
-		i2c->algo.bit.getscl = get_clock;
-		i2c->algo.bit.udelay = 10;
-		i2c->algo.bit.timeout = usecs_to_jiffies(2200);	/* from VESA */
-		i2c->algo.bit.data = i2c;
+		i2c->adapter.algo_data = &i2c->bit;
+		i2c->bit.pre_xfer = pre_xfer;
+		i2c->bit.post_xfer = post_xfer;
+		i2c->bit.setsda = set_data;
+		i2c->bit.setscl = set_clock;
+		i2c->bit.getsda = get_data;
+		i2c->bit.getscl = get_clock;
+		i2c->bit.udelay = 10;
+		i2c->bit.timeout = usecs_to_jiffies(2200);	/* from VESA */
+		i2c->bit.data = i2c;
 		ret = i2c_bit_add_bus(&i2c->adapter);
 		if (ret) {
 			DRM_ERROR("Failed to register bit i2c %s\n", name);
@@ -974,46 +973,13 @@ out_free:
 
 }
 
-struct radeon_i2c_chan *radeon_i2c_create_dp(struct drm_device *dev,
-					     struct radeon_i2c_bus_rec *rec,
-					     const char *name)
-{
-	struct radeon_i2c_chan *i2c;
-	int ret;
-
-	i2c = kzalloc(sizeof(struct radeon_i2c_chan), GFP_KERNEL);
-	if (i2c == NULL)
-		return NULL;
-
-	i2c->rec = *rec;
-	i2c->adapter.owner = THIS_MODULE;
-	i2c->adapter.class = I2C_CLASS_DDC;
-	i2c->adapter.dev.parent = &dev->pdev->dev;
-	i2c->dev = dev;
-	snprintf(i2c->adapter.name, sizeof(i2c->adapter.name),
-		 "Radeon aux bus %s", name);
-	i2c_set_adapdata(&i2c->adapter, i2c);
-	i2c->adapter.algo_data = &i2c->algo.dp;
-	i2c->algo.dp.aux_ch = radeon_dp_i2c_aux_ch;
-	i2c->algo.dp.address = 0;
-	ret = i2c_dp_aux_add_bus(&i2c->adapter);
-	if (ret) {
-		DRM_INFO("Failed to register i2c %s\n", name);
-		goto out_free;
-	}
-
-	return i2c;
-out_free:
-	kfree(i2c);
-	return NULL;
-
-}
-
 void radeon_i2c_destroy(struct radeon_i2c_chan *i2c)
 {
 	if (!i2c)
 		return;
 	i2c_del_adapter(&i2c->adapter);
+	if (i2c->has_aux)
+		drm_dp_aux_unregister_i2c_bus(&i2c->aux);
 	kfree(i2c);
 }
 
diff --git a/drivers/gpu/drm/radeon/radeon_kms.c b/drivers/gpu/drm/radeon/radeon_kms.c
index 3e49342a20e..eaaedba0467 100644
--- a/drivers/gpu/drm/radeon/radeon_kms.c
+++ b/drivers/gpu/drm/radeon/radeon_kms.c
@@ -35,9 +35,9 @@
 #include <linux/pm_runtime.h>
 
 #if defined(CONFIG_VGA_SWITCHEROO)
-bool radeon_is_px(void);
+bool radeon_has_atpx(void);
 #else
-static inline bool radeon_is_px(void) { return false; }
+static inline bool radeon_has_atpx(void) { return false; }
 #endif
 
 /**
@@ -107,6 +107,11 @@ int radeon_driver_load_kms(struct drm_device *dev, unsigned long flags)
 		flags |= RADEON_IS_PCI;
 	}
 
+	if ((radeon_runtime_pm != 0) &&
+	    radeon_has_atpx() &&
+	    ((flags & RADEON_IS_IGP) == 0))
+		flags |= RADEON_IS_PX;
+
 	/* radeon_device_init should report only fatal error
 	 * like memory allocation failure or iomapping failure,
 	 * or memory manager initialization failure, it must
@@ -137,8 +142,7 @@ int radeon_driver_load_kms(struct drm_device *dev, unsigned long flags)
 				"Error during ACPI methods call\n");
 	}
 
-	if ((radeon_runtime_pm == 1) ||
-	    ((radeon_runtime_pm == -1) && radeon_is_px())) {
+	if (radeon_is_px(dev)) {
 		pm_runtime_use_autosuspend(dev->dev);
 		pm_runtime_set_autosuspend_delay(dev->dev, 5000);
 		pm_runtime_set_active(dev->dev);
@@ -568,28 +572,34 @@ int radeon_driver_open_kms(struct drm_device *dev, struct drm_file *file_priv)
 		}
 
 		r = radeon_vm_init(rdev, &fpriv->vm);
-		if (r)
-			return r;
-
-		r = radeon_bo_reserve(rdev->ring_tmp_bo.bo, false);
-		if (r)
-			return r;
-
-		/* map the ib pool buffer read only into
-		 * virtual address space */
-		bo_va = radeon_vm_bo_add(rdev, &fpriv->vm,
-					 rdev->ring_tmp_bo.bo);
-		r = radeon_vm_bo_set_addr(rdev, bo_va, RADEON_VA_IB_OFFSET,
-					  RADEON_VM_PAGE_READABLE |
-					  RADEON_VM_PAGE_SNOOPED);
-
-		radeon_bo_unreserve(rdev->ring_tmp_bo.bo);
 		if (r) {
-			radeon_vm_fini(rdev, &fpriv->vm);
 			kfree(fpriv);
 			return r;
 		}
 
+		if (rdev->accel_working) {
+			r = radeon_bo_reserve(rdev->ring_tmp_bo.bo, false);
+			if (r) {
+				radeon_vm_fini(rdev, &fpriv->vm);
+				kfree(fpriv);
+				return r;
+			}
+
+			/* map the ib pool buffer read only into
+			 * virtual address space */
+			bo_va = radeon_vm_bo_add(rdev, &fpriv->vm,
+						 rdev->ring_tmp_bo.bo);
+			r = radeon_vm_bo_set_addr(rdev, bo_va, RADEON_VA_IB_OFFSET,
+						  RADEON_VM_PAGE_READABLE |
+						  RADEON_VM_PAGE_SNOOPED);
+
+			radeon_bo_unreserve(rdev->ring_tmp_bo.bo);
+			if (r) {
+				radeon_vm_fini(rdev, &fpriv->vm);
+				kfree(fpriv);
+				return r;
+			}
+		}
 		file_priv->driver_priv = fpriv;
 	}
 
@@ -617,13 +627,15 @@ void radeon_driver_postclose_kms(struct drm_device *dev,
 		struct radeon_bo_va *bo_va;
 		int r;
 
-		r = radeon_bo_reserve(rdev->ring_tmp_bo.bo, false);
-		if (!r) {
-			bo_va = radeon_vm_bo_find(&fpriv->vm,
-						  rdev->ring_tmp_bo.bo);
-			if (bo_va)
-				radeon_vm_bo_rmv(rdev, bo_va);
-			radeon_bo_unreserve(rdev->ring_tmp_bo.bo);
+		if (rdev->accel_working) {
+			r = radeon_bo_reserve(rdev->ring_tmp_bo.bo, false);
+			if (!r) {
+				bo_va = radeon_vm_bo_find(&fpriv->vm,
+							  rdev->ring_tmp_bo.bo);
+				if (bo_va)
+					radeon_vm_bo_rmv(rdev, bo_va);
+				radeon_bo_unreserve(rdev->ring_tmp_bo.bo);
+			}
 		}
 
 		radeon_vm_fini(rdev, &fpriv->vm);
diff --git a/drivers/gpu/drm/radeon/radeon_mode.h b/drivers/gpu/drm/radeon/radeon_mode.h
index 832d9fa1a4c..6ddf31a2d34 100644
--- a/drivers/gpu/drm/radeon/radeon_mode.h
+++ b/drivers/gpu/drm/radeon/radeon_mode.h
@@ -187,12 +187,10 @@ struct radeon_pll {
 struct radeon_i2c_chan {
 	struct i2c_adapter adapter;
 	struct drm_device *dev;
-	union {
-		struct i2c_algo_bit_data bit;
-		struct i2c_algo_dp_aux_data dp;
-	} algo;
+	struct i2c_algo_bit_data bit;
 	struct radeon_i2c_bus_rec rec;
 	struct drm_dp_aux aux;
+	bool has_aux;
 };
 
 /* mostly for macs, but really any system without connector tables */
@@ -440,7 +438,6 @@ struct radeon_encoder {
 struct radeon_connector_atom_dig {
 	uint32_t igp_lane_info;
 	/* displayport */
-	struct radeon_i2c_chan *dp_i2c_bus;
 	u8 dpcd[DP_RECEIVER_CAP_SIZE];
 	u8 dp_sink_type;
 	int dp_clock;
@@ -702,8 +699,6 @@ extern void atombios_dig_transmitter_setup(struct drm_encoder *encoder,
 					   uint8_t lane_set);
 extern void radeon_atom_ext_encoder_setup_ddc(struct drm_encoder *encoder);
 extern struct drm_encoder *radeon_get_external_encoder(struct drm_encoder *encoder);
-extern int radeon_dp_i2c_aux_ch(struct i2c_adapter *adapter, int mode,
-				u8 write_byte, u8 *read_byte);
 void radeon_atom_copy_swap(u8 *dst, u8 *src, u8 num_bytes, bool to_le);
 
 extern void radeon_i2c_init(struct radeon_device *rdev);
@@ -715,9 +710,6 @@ extern void radeon_i2c_add(struct radeon_device *rdev,
 			   const char *name);
 extern struct radeon_i2c_chan *radeon_i2c_lookup(struct radeon_device *rdev,
 						 struct radeon_i2c_bus_rec *i2c_bus);
-extern struct radeon_i2c_chan *radeon_i2c_create_dp(struct drm_device *dev,
-						    struct radeon_i2c_bus_rec *rec,
-						    const char *name);
 extern struct radeon_i2c_chan *radeon_i2c_create(struct drm_device *dev,
 						 struct radeon_i2c_bus_rec *rec,
 						 const char *name);
diff --git a/drivers/gpu/drm/radeon/radeon_object.c b/drivers/gpu/drm/radeon/radeon_object.c
index 19bec0dbfa3..4faa4d6f9bb 100644
--- a/drivers/gpu/drm/radeon/radeon_object.c
+++ b/drivers/gpu/drm/radeon/radeon_object.c
@@ -458,7 +458,7 @@ int radeon_bo_list_validate(struct radeon_device *rdev,
 			 * into account. We don't want to disallow buffer moves
 			 * completely.
 			 */
-			if (current_domain != RADEON_GEM_DOMAIN_CPU &&
+			if ((lobj->alt_domain & current_domain) != 0 &&
 			    (domain & current_domain) == 0 && /* will be moved */
 			    bytes_moved > bytes_moved_threshold) {
 				/* don't move it */
@@ -699,22 +699,30 @@ int radeon_bo_fault_reserve_notify(struct ttm_buffer_object *bo)
 	rbo = container_of(bo, struct radeon_bo, tbo);
 	radeon_bo_check_tiling(rbo, 0, 0);
 	rdev = rbo->rdev;
-	if (bo->mem.mem_type == TTM_PL_VRAM) {
-		size = bo->mem.num_pages << PAGE_SHIFT;
-		offset = bo->mem.start << PAGE_SHIFT;
-		if ((offset + size) > rdev->mc.visible_vram_size) {
-			/* hurrah the memory is not visible ! */
-			radeon_ttm_placement_from_domain(rbo, RADEON_GEM_DOMAIN_VRAM);
-			rbo->placement.lpfn = rdev->mc.visible_vram_size >> PAGE_SHIFT;
-			r = ttm_bo_validate(bo, &rbo->placement, false, false);
-			if (unlikely(r != 0))
-				return r;
-			offset = bo->mem.start << PAGE_SHIFT;
-			/* this should not happen */
-			if ((offset + size) > rdev->mc.visible_vram_size)
-				return -EINVAL;
-		}
+	if (bo->mem.mem_type != TTM_PL_VRAM)
+		return 0;
+
+	size = bo->mem.num_pages << PAGE_SHIFT;
+	offset = bo->mem.start << PAGE_SHIFT;
+	if ((offset + size) <= rdev->mc.visible_vram_size)
+		return 0;
+
+	/* hurrah the memory is not visible ! */
+	radeon_ttm_placement_from_domain(rbo, RADEON_GEM_DOMAIN_VRAM);
+	rbo->placement.lpfn = rdev->mc.visible_vram_size >> PAGE_SHIFT;
+	r = ttm_bo_validate(bo, &rbo->placement, false, false);
+	if (unlikely(r == -ENOMEM)) {
+		radeon_ttm_placement_from_domain(rbo, RADEON_GEM_DOMAIN_GTT);
+		return ttm_bo_validate(bo, &rbo->placement, false, false);
+	} else if (unlikely(r != 0)) {
+		return r;
 	}
+
+	offset = bo->mem.start << PAGE_SHIFT;
+	/* this should never happen */
+	if ((offset + size) > rdev->mc.visible_vram_size)
+		return -EINVAL;
+
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/radeon/radeon_pm.c b/drivers/gpu/drm/radeon/radeon_pm.c
index ee738a52463..53d6e1bb48d 100644
--- a/drivers/gpu/drm/radeon/radeon_pm.c
+++ b/drivers/gpu/drm/radeon/radeon_pm.c
@@ -361,6 +361,11 @@ static ssize_t radeon_set_pm_profile(struct device *dev,
 	struct drm_device *ddev = dev_get_drvdata(dev);
 	struct radeon_device *rdev = ddev->dev_private;
 
+	/* Can't set profile when the card is off */
+	if  ((rdev->flags & RADEON_IS_PX) &&
+	     (ddev->switch_power_state != DRM_SWITCH_POWER_ON))
+		return -EINVAL;
+
 	mutex_lock(&rdev->pm.mutex);
 	if (rdev->pm.pm_method == PM_METHOD_PROFILE) {
 		if (strncmp("default", buf, strlen("default")) == 0)
@@ -409,6 +414,13 @@ static ssize_t radeon_set_pm_method(struct device *dev,
 	struct drm_device *ddev = dev_get_drvdata(dev);
 	struct radeon_device *rdev = ddev->dev_private;
 
+	/* Can't set method when the card is off */
+	if  ((rdev->flags & RADEON_IS_PX) &&
+	     (ddev->switch_power_state != DRM_SWITCH_POWER_ON)) {
+		count = -EINVAL;
+		goto fail;
+	}
+
 	/* we don't support the legacy modes with dpm */
 	if (rdev->pm.pm_method == PM_METHOD_DPM) {
 		count = -EINVAL;
@@ -446,6 +458,10 @@ static ssize_t radeon_get_dpm_state(struct device *dev,
 	struct radeon_device *rdev = ddev->dev_private;
 	enum radeon_pm_state_type pm = rdev->pm.dpm.user_state;
 
+	if  ((rdev->flags & RADEON_IS_PX) &&
+	     (ddev->switch_power_state != DRM_SWITCH_POWER_ON))
+		return snprintf(buf, PAGE_SIZE, "off\n");
+
 	return snprintf(buf, PAGE_SIZE, "%s\n",
 			(pm == POWER_STATE_TYPE_BATTERY) ? "battery" :
 			(pm == POWER_STATE_TYPE_BALANCED) ? "balanced" : "performance");
@@ -459,6 +475,11 @@ static ssize_t radeon_set_dpm_state(struct device *dev,
 	struct drm_device *ddev = dev_get_drvdata(dev);
 	struct radeon_device *rdev = ddev->dev_private;
 
+	/* Can't set dpm state when the card is off */
+	if  ((rdev->flags & RADEON_IS_PX) &&
+	     (ddev->switch_power_state != DRM_SWITCH_POWER_ON))
+		return -EINVAL;
+
 	mutex_lock(&rdev->pm.mutex);
 	if (strncmp("battery", buf, strlen("battery")) == 0)
 		rdev->pm.dpm.user_state = POWER_STATE_TYPE_BATTERY;
@@ -485,6 +506,10 @@ static ssize_t radeon_get_dpm_forced_performance_level(struct device *dev,
 	struct radeon_device *rdev = ddev->dev_private;
 	enum radeon_dpm_forced_level level = rdev->pm.dpm.forced_level;
 
+	if  ((rdev->flags & RADEON_IS_PX) &&
+	     (ddev->switch_power_state != DRM_SWITCH_POWER_ON))
+		return snprintf(buf, PAGE_SIZE, "off\n");
+
 	return snprintf(buf, PAGE_SIZE, "%s\n",
 			(level == RADEON_DPM_FORCED_LEVEL_AUTO) ? "auto" :
 			(level == RADEON_DPM_FORCED_LEVEL_LOW) ? "low" : "high");
@@ -500,6 +525,11 @@ static ssize_t radeon_set_dpm_forced_performance_level(struct device *dev,
 	enum radeon_dpm_forced_level level;
 	int ret = 0;
 
+	/* Can't force performance level when the card is off */
+	if  ((rdev->flags & RADEON_IS_PX) &&
+	     (ddev->switch_power_state != DRM_SWITCH_POWER_ON))
+		return -EINVAL;
+
 	mutex_lock(&rdev->pm.mutex);
 	if (strncmp("low", buf, strlen("low")) == 0) {
 		level = RADEON_DPM_FORCED_LEVEL_LOW;
@@ -538,8 +568,14 @@ static ssize_t radeon_hwmon_show_temp(struct device *dev,
 				      char *buf)
 {
 	struct radeon_device *rdev = dev_get_drvdata(dev);
+	struct drm_device *ddev = rdev->ddev;
 	int temp;
 
+	/* Can't get temperature when the card is off */
+	if  ((rdev->flags & RADEON_IS_PX) &&
+	     (ddev->switch_power_state != DRM_SWITCH_POWER_ON))
+		return -EINVAL;
+
 	if (rdev->asic->pm.get_temperature)
 		temp = radeon_get_temperature(rdev);
 	else
@@ -603,7 +639,6 @@ static const struct attribute_group *hwmon_groups[] = {
 static int radeon_hwmon_init(struct radeon_device *rdev)
 {
 	int err = 0;
-	struct device *hwmon_dev;
 
 	switch (rdev->pm.int_thermal_type) {
 	case THERMAL_TYPE_RV6XX:
@@ -616,11 +651,11 @@ static int radeon_hwmon_init(struct radeon_device *rdev)
 	case THERMAL_TYPE_KV:
 		if (rdev->asic->pm.get_temperature == NULL)
 			return err;
-		hwmon_dev = hwmon_device_register_with_groups(rdev->dev,
-							      "radeon", rdev,
-							      hwmon_groups);
-		if (IS_ERR(hwmon_dev)) {
-			err = PTR_ERR(hwmon_dev);
+		rdev->pm.int_hwmon_dev = hwmon_device_register_with_groups(rdev->dev,
+									   "radeon", rdev,
+									   hwmon_groups);
+		if (IS_ERR(rdev->pm.int_hwmon_dev)) {
+			err = PTR_ERR(rdev->pm.int_hwmon_dev);
 			dev_err(rdev->dev,
 				"Unable to register hwmon device: %d\n", err);
 		}
@@ -632,6 +667,12 @@ static int radeon_hwmon_init(struct radeon_device *rdev)
 	return err;
 }
 
+static void radeon_hwmon_fini(struct radeon_device *rdev)
+{
+	if (rdev->pm.int_hwmon_dev)
+		hwmon_device_unregister(rdev->pm.int_hwmon_dev);
+}
+
 static void radeon_dpm_thermal_work_handler(struct work_struct *work)
 {
 	struct radeon_device *rdev =
@@ -1257,6 +1298,7 @@ int radeon_pm_init(struct radeon_device *rdev)
 	case CHIP_RV670:
 	case CHIP_RS780:
 	case CHIP_RS880:
+	case CHIP_RV770:
 	case CHIP_BARTS:
 	case CHIP_TURKS:
 	case CHIP_CAICOS:
@@ -1273,7 +1315,6 @@ int radeon_pm_init(struct radeon_device *rdev)
 		else
 			rdev->pm.pm_method = PM_METHOD_PROFILE;
 		break;
-	case CHIP_RV770:
 	case CHIP_RV730:
 	case CHIP_RV710:
 	case CHIP_RV740:
@@ -1295,6 +1336,7 @@ int radeon_pm_init(struct radeon_device *rdev)
 	case CHIP_KABINI:
 	case CHIP_KAVERI:
 	case CHIP_HAWAII:
+	case CHIP_MULLINS:
 		/* DPM requires the RLC, RV770+ dGPU requires SMC */
 		if (!rdev->rlc_fw)
 			rdev->pm.pm_method = PM_METHOD_PROFILE;
@@ -1353,6 +1395,8 @@ static void radeon_pm_fini_old(struct radeon_device *rdev)
 		device_remove_file(rdev->dev, &dev_attr_power_method);
 	}
 
+	radeon_hwmon_fini(rdev);
+
 	if (rdev->pm.power_state)
 		kfree(rdev->pm.power_state);
 }
@@ -1372,6 +1416,8 @@ static void radeon_pm_fini_dpm(struct radeon_device *rdev)
 	}
 	radeon_dpm_fini(rdev);
 
+	radeon_hwmon_fini(rdev);
+
 	if (rdev->pm.power_state)
 		kfree(rdev->pm.power_state);
 }
@@ -1397,12 +1443,14 @@ static void radeon_pm_compute_clocks_old(struct radeon_device *rdev)
 
 	rdev->pm.active_crtcs = 0;
 	rdev->pm.active_crtc_count = 0;
-	list_for_each_entry(crtc,
-		&ddev->mode_config.crtc_list, head) {
-		radeon_crtc = to_radeon_crtc(crtc);
-		if (radeon_crtc->enabled) {
-			rdev->pm.active_crtcs |= (1 << radeon_crtc->crtc_id);
-			rdev->pm.active_crtc_count++;
+	if (rdev->num_crtc && rdev->mode_info.mode_config_initialized) {
+		list_for_each_entry(crtc,
+				    &ddev->mode_config.crtc_list, head) {
+			radeon_crtc = to_radeon_crtc(crtc);
+			if (radeon_crtc->enabled) {
+				rdev->pm.active_crtcs |= (1 << radeon_crtc->crtc_id);
+				rdev->pm.active_crtc_count++;
+			}
 		}
 	}
 
@@ -1469,12 +1517,14 @@ static void radeon_pm_compute_clocks_dpm(struct radeon_device *rdev)
 	/* update active crtc counts */
 	rdev->pm.dpm.new_active_crtcs = 0;
 	rdev->pm.dpm.new_active_crtc_count = 0;
-	list_for_each_entry(crtc,
-		&ddev->mode_config.crtc_list, head) {
-		radeon_crtc = to_radeon_crtc(crtc);
-		if (crtc->enabled) {
-			rdev->pm.dpm.new_active_crtcs |= (1 << radeon_crtc->crtc_id);
-			rdev->pm.dpm.new_active_crtc_count++;
+	if (rdev->num_crtc && rdev->mode_info.mode_config_initialized) {
+		list_for_each_entry(crtc,
+				    &ddev->mode_config.crtc_list, head) {
+			radeon_crtc = to_radeon_crtc(crtc);
+			if (crtc->enabled) {
+				rdev->pm.dpm.new_active_crtcs |= (1 << radeon_crtc->crtc_id);
+				rdev->pm.dpm.new_active_crtc_count++;
+			}
 		}
 	}
 
@@ -1600,8 +1650,12 @@ static int radeon_debugfs_pm_info(struct seq_file *m, void *data)
 	struct drm_info_node *node = (struct drm_info_node *) m->private;
 	struct drm_device *dev = node->minor->dev;
 	struct radeon_device *rdev = dev->dev_private;
+	struct drm_device *ddev = rdev->ddev;
 
-	if (rdev->pm.dpm_enabled) {
+	if  ((rdev->flags & RADEON_IS_PX) &&
+	     (ddev->switch_power_state != DRM_SWITCH_POWER_ON)) {
+		seq_printf(m, "PX asic powered off\n");
+	} else if (rdev->pm.dpm_enabled) {
 		mutex_lock(&rdev->pm.mutex);
 		if (rdev->asic->dpm.debugfs_print_current_performance_level)
 			radeon_dpm_debugfs_print_current_performance_level(rdev, m);
diff --git a/drivers/gpu/drm/radeon/radeon_ucode.h b/drivers/gpu/drm/radeon/radeon_ucode.h
index a77cd274dfc..4e7c3269b18 100644
--- a/drivers/gpu/drm/radeon/radeon_ucode.h
+++ b/drivers/gpu/drm/radeon/radeon_ucode.h
@@ -52,14 +52,20 @@
 #define BONAIRE_RLC_UCODE_SIZE       2048
 #define KB_RLC_UCODE_SIZE            2560
 #define KV_RLC_UCODE_SIZE            2560
+#define ML_RLC_UCODE_SIZE            2560
 
 /* MC */
 #define BTC_MC_UCODE_SIZE            6024
 #define CAYMAN_MC_UCODE_SIZE         6037
 #define SI_MC_UCODE_SIZE             7769
+#define TAHITI_MC_UCODE_SIZE         7808
+#define PITCAIRN_MC_UCODE_SIZE       7775
+#define VERDE_MC_UCODE_SIZE          7875
 #define OLAND_MC_UCODE_SIZE          7863
-#define CIK_MC_UCODE_SIZE            7866
+#define BONAIRE_MC_UCODE_SIZE        7866
+#define BONAIRE_MC2_UCODE_SIZE       7948
 #define HAWAII_MC_UCODE_SIZE         7933
+#define HAWAII_MC2_UCODE_SIZE        8091
 
 /* SDMA */
 #define CIK_SDMA_UCODE_SIZE          1050
diff --git a/drivers/gpu/drm/radeon/radeon_uvd.c b/drivers/gpu/drm/radeon/radeon_uvd.c
index 5748bdaeacc..1b65ae2433c 100644
--- a/drivers/gpu/drm/radeon/radeon_uvd.c
+++ b/drivers/gpu/drm/radeon/radeon_uvd.c
@@ -99,6 +99,7 @@ int radeon_uvd_init(struct radeon_device *rdev)
 	case CHIP_KABINI:
 	case CHIP_KAVERI:
 	case CHIP_HAWAII:
+	case CHIP_MULLINS:
 		fw_name = FIRMWARE_BONAIRE;
 		break;
 
@@ -465,6 +466,10 @@ static int radeon_uvd_cs_reloc(struct radeon_cs_parser *p,
 	cmd = radeon_get_ib_value(p, p->idx) >> 1;
 
 	if (cmd < 0x4) {
+		if (end <= start) {
+			DRM_ERROR("invalid reloc offset %X!\n", offset);
+			return -EINVAL;
+		}
 		if ((end - start) < buf_sizes[cmd]) {
 			DRM_ERROR("buffer (%d) to small (%d / %d)!\n", cmd,
 				  (unsigned)(end - start), buf_sizes[cmd]);
diff --git a/drivers/gpu/drm/radeon/radeon_vce.c b/drivers/gpu/drm/radeon/radeon_vce.c
index 76e9904bc53..3971d968af6 100644
--- a/drivers/gpu/drm/radeon/radeon_vce.c
+++ b/drivers/gpu/drm/radeon/radeon_vce.c
@@ -66,6 +66,7 @@ int radeon_vce_init(struct radeon_device *rdev)
 	case CHIP_BONAIRE:
 	case CHIP_KAVERI:
 	case CHIP_KABINI:
+	case CHIP_MULLINS:
 		fw_name = FIRMWARE_BONAIRE;
 		break;
 
@@ -442,13 +443,16 @@ int radeon_vce_get_destroy_msg(struct radeon_device *rdev, int ring,
  * @p: parser context
  * @lo: address of lower dword
  * @hi: address of higher dword
+ * @size: size of checker for relocation buffer
  *
  * Patch relocation inside command stream with real buffer address
  */
-int radeon_vce_cs_reloc(struct radeon_cs_parser *p, int lo, int hi)
+int radeon_vce_cs_reloc(struct radeon_cs_parser *p, int lo, int hi,
+			unsigned size)
 {
 	struct radeon_cs_chunk *relocs_chunk;
-	uint64_t offset;
+	struct radeon_cs_reloc *reloc;
+	uint64_t start, end, offset;
 	unsigned idx;
 
 	relocs_chunk = &p->chunks[p->chunk_relocs_idx];
@@ -461,15 +465,60 @@ int radeon_vce_cs_reloc(struct radeon_cs_parser *p, int lo, int hi)
 		return -EINVAL;
 	}
 
-	offset += p->relocs_ptr[(idx / 4)]->gpu_offset;
+	reloc = p->relocs_ptr[(idx / 4)];
+	start = reloc->gpu_offset;
+	end = start + radeon_bo_size(reloc->robj);
+	start += offset;
 
-        p->ib.ptr[lo] = offset & 0xFFFFFFFF;
-        p->ib.ptr[hi] = offset >> 32;
+	p->ib.ptr[lo] = start & 0xFFFFFFFF;
+	p->ib.ptr[hi] = start >> 32;
+
+	if (end <= start) {
+		DRM_ERROR("invalid reloc offset %llX!\n", offset);
+		return -EINVAL;
+	}
+	if ((end - start) < size) {
+		DRM_ERROR("buffer to small (%d / %d)!\n",
+			(unsigned)(end - start), size);
+		return -EINVAL;
+	}
 
 	return 0;
 }
 
 /**
+ * radeon_vce_validate_handle - validate stream handle
+ *
+ * @p: parser context
+ * @handle: handle to validate
+ *
+ * Validates the handle and return the found session index or -EINVAL
+ * we we don't have another free session index.
+ */
+int radeon_vce_validate_handle(struct radeon_cs_parser *p, uint32_t handle)
+{
+	unsigned i;
+
+	/* validate the handle */
+	for (i = 0; i < RADEON_MAX_VCE_HANDLES; ++i) {
+		if (atomic_read(&p->rdev->vce.handles[i]) == handle)
+			return i;
+	}
+
+	/* handle not found try to alloc a new one */
+	for (i = 0; i < RADEON_MAX_VCE_HANDLES; ++i) {
+		if (!atomic_cmpxchg(&p->rdev->vce.handles[i], 0, handle)) {
+			p->rdev->vce.filp[i] = p->filp;
+			p->rdev->vce.img_size[i] = 0;
+			return i;
+		}
+	}
+
+	DRM_ERROR("No more free VCE handles!\n");
+	return -EINVAL;
+}
+
+/**
  * radeon_vce_cs_parse - parse and validate the command stream
  *
  * @p: parser context
@@ -477,8 +526,10 @@ int radeon_vce_cs_reloc(struct radeon_cs_parser *p, int lo, int hi)
  */
 int radeon_vce_cs_parse(struct radeon_cs_parser *p)
 {
-	uint32_t handle = 0;
-	bool destroy = false;
+	int session_idx = -1;
+	bool destroyed = false;
+	uint32_t tmp, handle = 0;
+	uint32_t *size = &tmp;
 	int i, r;
 
 	while (p->idx < p->chunks[p->chunk_ib_idx].length_dw) {
@@ -490,13 +541,29 @@ int radeon_vce_cs_parse(struct radeon_cs_parser *p)
                 	return -EINVAL;
 		}
 
+		if (destroyed) {
+			DRM_ERROR("No other command allowed after destroy!\n");
+			return -EINVAL;
+		}
+
 		switch (cmd) {
 		case 0x00000001: // session
 			handle = radeon_get_ib_value(p, p->idx + 2);
+			session_idx = radeon_vce_validate_handle(p, handle);
+			if (session_idx < 0)
+				return session_idx;
+			size = &p->rdev->vce.img_size[session_idx];
 			break;
 
 		case 0x00000002: // task info
+			break;
+
 		case 0x01000001: // create
+			*size = radeon_get_ib_value(p, p->idx + 8) *
+				radeon_get_ib_value(p, p->idx + 10) *
+				8 * 3 / 2;
+			break;
+
 		case 0x04000001: // config extension
 		case 0x04000002: // pic control
 		case 0x04000005: // rate control
@@ -505,23 +572,39 @@ int radeon_vce_cs_parse(struct radeon_cs_parser *p)
 			break;
 
 		case 0x03000001: // encode
-			r = radeon_vce_cs_reloc(p, p->idx + 10, p->idx + 9);
+			r = radeon_vce_cs_reloc(p, p->idx + 10, p->idx + 9,
+						*size);
 			if (r)
 				return r;
 
-			r = radeon_vce_cs_reloc(p, p->idx + 12, p->idx + 11);
+			r = radeon_vce_cs_reloc(p, p->idx + 12, p->idx + 11,
+						*size / 3);
 			if (r)
 				return r;
 			break;
 
 		case 0x02000001: // destroy
-			destroy = true;
+			destroyed = true;
 			break;
 
 		case 0x05000001: // context buffer
+			r = radeon_vce_cs_reloc(p, p->idx + 3, p->idx + 2,
+						*size * 2);
+			if (r)
+				return r;
+			break;
+
 		case 0x05000004: // video bitstream buffer
+			tmp = radeon_get_ib_value(p, p->idx + 4);
+			r = radeon_vce_cs_reloc(p, p->idx + 3, p->idx + 2,
+						tmp);
+			if (r)
+				return r;
+			break;
+
 		case 0x05000005: // feedback buffer
-			r = radeon_vce_cs_reloc(p, p->idx + 3, p->idx + 2);
+			r = radeon_vce_cs_reloc(p, p->idx + 3, p->idx + 2,
+						4096);
 			if (r)
 				return r;
 			break;
@@ -531,33 +614,21 @@ int radeon_vce_cs_parse(struct radeon_cs_parser *p)
 			return -EINVAL;
 		}
 
+		if (session_idx == -1) {
+			DRM_ERROR("no session command at start of IB\n");
+			return -EINVAL;
+		}
+
 		p->idx += len / 4;
 	}
 
-	if (destroy) {
+	if (destroyed) {
 		/* IB contains a destroy msg, free the handle */
 		for (i = 0; i < RADEON_MAX_VCE_HANDLES; ++i)
 			atomic_cmpxchg(&p->rdev->vce.handles[i], handle, 0);
-
-		return 0;
-        }
-
-	/* create or encode, validate the handle */
-	for (i = 0; i < RADEON_MAX_VCE_HANDLES; ++i) {
-		if (atomic_read(&p->rdev->vce.handles[i]) == handle)
-			return 0;
 	}
 
-	/* handle not found try to alloc a new one */
-	for (i = 0; i < RADEON_MAX_VCE_HANDLES; ++i) {
-		if (!atomic_cmpxchg(&p->rdev->vce.handles[i], 0, handle)) {
-			p->rdev->vce.filp[i] = p->filp;
-			return 0;
-		}
-	}
-
-	DRM_ERROR("No more free VCE handles!\n");
-	return -EINVAL;
+	return 0;
 }
 
 /**
@@ -613,7 +684,7 @@ void radeon_vce_fence_emit(struct radeon_device *rdev,
 			   struct radeon_fence *fence)
 {
 	struct radeon_ring *ring = &rdev->ring[fence->ring];
-	uint32_t addr = rdev->fence_drv[fence->ring].gpu_addr;
+	uint64_t addr = rdev->fence_drv[fence->ring].gpu_addr;
 
 	radeon_ring_write(ring, VCE_CMD_FENCE);
 	radeon_ring_write(ring, addr);
diff --git a/drivers/gpu/drm/radeon/radeon_vm.c b/drivers/gpu/drm/radeon/radeon_vm.c
index 2aae6ce49d3..1f426696de3 100644
--- a/drivers/gpu/drm/radeon/radeon_vm.c
+++ b/drivers/gpu/drm/radeon/radeon_vm.c
@@ -130,10 +130,10 @@ struct radeon_cs_reloc *radeon_vm_get_bos(struct radeon_device *rdev,
 					  struct list_head *head)
 {
 	struct radeon_cs_reloc *list;
-	unsigned i, idx, size;
+	unsigned i, idx;
 
-	size = (radeon_vm_num_pdes(rdev) + 1) * sizeof(struct radeon_cs_reloc);
-	list = kmalloc(size, GFP_KERNEL);
+	list = kmalloc_array(vm->max_pde_used + 1,
+			     sizeof(struct radeon_cs_reloc), GFP_KERNEL);
 	if (!list)
 		return NULL;
 
@@ -595,7 +595,7 @@ int radeon_vm_update_page_directory(struct radeon_device *rdev,
 	ndw = 64;
 
 	/* assume the worst case */
-	ndw += vm->max_pde_used * 12;
+	ndw += vm->max_pde_used * 16;
 
 	/* update too big for an IB */
 	if (ndw > 0xfffff)
diff --git a/drivers/gpu/drm/radeon/rv770_dma.c b/drivers/gpu/drm/radeon/rv770_dma.c
index aca8cbe8a33..bbf2e076ee4 100644
--- a/drivers/gpu/drm/radeon/rv770_dma.c
+++ b/drivers/gpu/drm/radeon/rv770_dma.c
@@ -86,6 +86,7 @@ int rv770_copy_dma(struct radeon_device *rdev,
 	r = radeon_fence_emit(rdev, fence, ring->idx);
 	if (r) {
 		radeon_ring_unlock_undo(rdev, ring);
+		radeon_semaphore_free(rdev, &sem, NULL);
 		return r;
 	}
 
diff --git a/drivers/gpu/drm/radeon/si.c b/drivers/gpu/drm/radeon/si.c
index d589475fe9e..22a63c98ba1 100644
--- a/drivers/gpu/drm/radeon/si.c
+++ b/drivers/gpu/drm/radeon/si.c
@@ -39,30 +39,35 @@ MODULE_FIRMWARE("radeon/TAHITI_pfp.bin");
 MODULE_FIRMWARE("radeon/TAHITI_me.bin");
 MODULE_FIRMWARE("radeon/TAHITI_ce.bin");
 MODULE_FIRMWARE("radeon/TAHITI_mc.bin");
+MODULE_FIRMWARE("radeon/TAHITI_mc2.bin");
 MODULE_FIRMWARE("radeon/TAHITI_rlc.bin");
 MODULE_FIRMWARE("radeon/TAHITI_smc.bin");
 MODULE_FIRMWARE("radeon/PITCAIRN_pfp.bin");
 MODULE_FIRMWARE("radeon/PITCAIRN_me.bin");
 MODULE_FIRMWARE("radeon/PITCAIRN_ce.bin");
 MODULE_FIRMWARE("radeon/PITCAIRN_mc.bin");
+MODULE_FIRMWARE("radeon/PITCAIRN_mc2.bin");
 MODULE_FIRMWARE("radeon/PITCAIRN_rlc.bin");
 MODULE_FIRMWARE("radeon/PITCAIRN_smc.bin");
 MODULE_FIRMWARE("radeon/VERDE_pfp.bin");
 MODULE_FIRMWARE("radeon/VERDE_me.bin");
 MODULE_FIRMWARE("radeon/VERDE_ce.bin");
 MODULE_FIRMWARE("radeon/VERDE_mc.bin");
+MODULE_FIRMWARE("radeon/VERDE_mc2.bin");
 MODULE_FIRMWARE("radeon/VERDE_rlc.bin");
 MODULE_FIRMWARE("radeon/VERDE_smc.bin");
 MODULE_FIRMWARE("radeon/OLAND_pfp.bin");
 MODULE_FIRMWARE("radeon/OLAND_me.bin");
 MODULE_FIRMWARE("radeon/OLAND_ce.bin");
 MODULE_FIRMWARE("radeon/OLAND_mc.bin");
+MODULE_FIRMWARE("radeon/OLAND_mc2.bin");
 MODULE_FIRMWARE("radeon/OLAND_rlc.bin");
 MODULE_FIRMWARE("radeon/OLAND_smc.bin");
 MODULE_FIRMWARE("radeon/HAINAN_pfp.bin");
 MODULE_FIRMWARE("radeon/HAINAN_me.bin");
 MODULE_FIRMWARE("radeon/HAINAN_ce.bin");
 MODULE_FIRMWARE("radeon/HAINAN_mc.bin");
+MODULE_FIRMWARE("radeon/HAINAN_mc2.bin");
 MODULE_FIRMWARE("radeon/HAINAN_rlc.bin");
 MODULE_FIRMWARE("radeon/HAINAN_smc.bin");
 
@@ -1467,36 +1472,33 @@ int si_mc_load_microcode(struct radeon_device *rdev)
 	const __be32 *fw_data;
 	u32 running, blackout = 0;
 	u32 *io_mc_regs;
-	int i, ucode_size, regs_size;
+	int i, regs_size, ucode_size;
 
 	if (!rdev->mc_fw)
 		return -EINVAL;
 
+	ucode_size = rdev->mc_fw->size / 4;
+
 	switch (rdev->family) {
 	case CHIP_TAHITI:
 		io_mc_regs = (u32 *)&tahiti_io_mc_regs;
-		ucode_size = SI_MC_UCODE_SIZE;
 		regs_size = TAHITI_IO_MC_REGS_SIZE;
 		break;
 	case CHIP_PITCAIRN:
 		io_mc_regs = (u32 *)&pitcairn_io_mc_regs;
-		ucode_size = SI_MC_UCODE_SIZE;
 		regs_size = TAHITI_IO_MC_REGS_SIZE;
 		break;
 	case CHIP_VERDE:
 	default:
 		io_mc_regs = (u32 *)&verde_io_mc_regs;
-		ucode_size = SI_MC_UCODE_SIZE;
 		regs_size = TAHITI_IO_MC_REGS_SIZE;
 		break;
 	case CHIP_OLAND:
 		io_mc_regs = (u32 *)&oland_io_mc_regs;
-		ucode_size = OLAND_MC_UCODE_SIZE;
 		regs_size = TAHITI_IO_MC_REGS_SIZE;
 		break;
 	case CHIP_HAINAN:
 		io_mc_regs = (u32 *)&hainan_io_mc_regs;
-		ucode_size = OLAND_MC_UCODE_SIZE;
 		regs_size = TAHITI_IO_MC_REGS_SIZE;
 		break;
 	}
@@ -1552,7 +1554,7 @@ static int si_init_microcode(struct radeon_device *rdev)
 	const char *chip_name;
 	const char *rlc_chip_name;
 	size_t pfp_req_size, me_req_size, ce_req_size, rlc_req_size, mc_req_size;
-	size_t smc_req_size;
+	size_t smc_req_size, mc2_req_size;
 	char fw_name[30];
 	int err;
 
@@ -1567,6 +1569,7 @@ static int si_init_microcode(struct radeon_device *rdev)
 		ce_req_size = SI_CE_UCODE_SIZE * 4;
 		rlc_req_size = SI_RLC_UCODE_SIZE * 4;
 		mc_req_size = SI_MC_UCODE_SIZE * 4;
+		mc2_req_size = TAHITI_MC_UCODE_SIZE * 4;
 		smc_req_size = ALIGN(TAHITI_SMC_UCODE_SIZE, 4);
 		break;
 	case CHIP_PITCAIRN:
@@ -1577,6 +1580,7 @@ static int si_init_microcode(struct radeon_device *rdev)
 		ce_req_size = SI_CE_UCODE_SIZE * 4;
 		rlc_req_size = SI_RLC_UCODE_SIZE * 4;
 		mc_req_size = SI_MC_UCODE_SIZE * 4;
+		mc2_req_size = PITCAIRN_MC_UCODE_SIZE * 4;
 		smc_req_size = ALIGN(PITCAIRN_SMC_UCODE_SIZE, 4);
 		break;
 	case CHIP_VERDE:
@@ -1587,6 +1591,7 @@ static int si_init_microcode(struct radeon_device *rdev)
 		ce_req_size = SI_CE_UCODE_SIZE * 4;
 		rlc_req_size = SI_RLC_UCODE_SIZE * 4;
 		mc_req_size = SI_MC_UCODE_SIZE * 4;
+		mc2_req_size = VERDE_MC_UCODE_SIZE * 4;
 		smc_req_size = ALIGN(VERDE_SMC_UCODE_SIZE, 4);
 		break;
 	case CHIP_OLAND:
@@ -1596,7 +1601,7 @@ static int si_init_microcode(struct radeon_device *rdev)
 		me_req_size = SI_PM4_UCODE_SIZE * 4;
 		ce_req_size = SI_CE_UCODE_SIZE * 4;
 		rlc_req_size = SI_RLC_UCODE_SIZE * 4;
-		mc_req_size = OLAND_MC_UCODE_SIZE * 4;
+		mc_req_size = mc2_req_size = OLAND_MC_UCODE_SIZE * 4;
 		smc_req_size = ALIGN(OLAND_SMC_UCODE_SIZE, 4);
 		break;
 	case CHIP_HAINAN:
@@ -1606,7 +1611,7 @@ static int si_init_microcode(struct radeon_device *rdev)
 		me_req_size = SI_PM4_UCODE_SIZE * 4;
 		ce_req_size = SI_CE_UCODE_SIZE * 4;
 		rlc_req_size = SI_RLC_UCODE_SIZE * 4;
-		mc_req_size = OLAND_MC_UCODE_SIZE * 4;
+		mc_req_size = mc2_req_size = OLAND_MC_UCODE_SIZE * 4;
 		smc_req_size = ALIGN(HAINAN_SMC_UCODE_SIZE, 4);
 		break;
 	default: BUG();
@@ -1659,16 +1664,22 @@ static int si_init_microcode(struct radeon_device *rdev)
 		err = -EINVAL;
 	}
 
-	snprintf(fw_name, sizeof(fw_name), "radeon/%s_mc.bin", chip_name);
+	snprintf(fw_name, sizeof(fw_name), "radeon/%s_mc2.bin", chip_name);
 	err = request_firmware(&rdev->mc_fw, fw_name, rdev->dev);
-	if (err)
-		goto out;
-	if (rdev->mc_fw->size != mc_req_size) {
+	if (err) {
+		snprintf(fw_name, sizeof(fw_name), "radeon/%s_mc.bin", chip_name);
+		err = request_firmware(&rdev->mc_fw, fw_name, rdev->dev);
+		if (err)
+			goto out;
+	}
+	if ((rdev->mc_fw->size != mc_req_size) &&
+	    (rdev->mc_fw->size != mc2_req_size)) {
 		printk(KERN_ERR
 		       "si_mc: Bogus length %zu in firmware \"%s\"\n",
 		       rdev->mc_fw->size, fw_name);
 		err = -EINVAL;
 	}
+	DRM_INFO("%s: %zu bytes\n", fw_name, rdev->mc_fw->size);
 
 	snprintf(fw_name, sizeof(fw_name), "radeon/%s_smc.bin", chip_name);
 	err = request_firmware(&rdev->smc_fw, fw_name, rdev->dev);
@@ -5769,7 +5780,6 @@ int si_irq_set(struct radeon_device *rdev)
 	u32 crtc1 = 0, crtc2 = 0, crtc3 = 0, crtc4 = 0, crtc5 = 0, crtc6 = 0;
 	u32 hpd1 = 0, hpd2 = 0, hpd3 = 0, hpd4 = 0, hpd5 = 0, hpd6 = 0;
 	u32 grbm_int_cntl = 0;
-	u32 grph1 = 0, grph2 = 0, grph3 = 0, grph4 = 0, grph5 = 0, grph6 = 0;
 	u32 dma_cntl, dma_cntl1;
 	u32 thermal_int = 0;
 
@@ -5908,16 +5918,22 @@ int si_irq_set(struct radeon_device *rdev)
 	}
 
 	if (rdev->num_crtc >= 2) {
-		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC0_REGISTER_OFFSET, grph1);
-		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC1_REGISTER_OFFSET, grph2);
+		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC0_REGISTER_OFFSET,
+		       GRPH_PFLIP_INT_MASK);
+		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC1_REGISTER_OFFSET,
+		       GRPH_PFLIP_INT_MASK);
 	}
 	if (rdev->num_crtc >= 4) {
-		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC2_REGISTER_OFFSET, grph3);
-		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC3_REGISTER_OFFSET, grph4);
+		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC2_REGISTER_OFFSET,
+		       GRPH_PFLIP_INT_MASK);
+		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC3_REGISTER_OFFSET,
+		       GRPH_PFLIP_INT_MASK);
 	}
 	if (rdev->num_crtc >= 6) {
-		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC4_REGISTER_OFFSET, grph5);
-		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC5_REGISTER_OFFSET, grph6);
+		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC4_REGISTER_OFFSET,
+		       GRPH_PFLIP_INT_MASK);
+		WREG32(GRPH_INT_CONTROL + EVERGREEN_CRTC5_REGISTER_OFFSET,
+		       GRPH_PFLIP_INT_MASK);
 	}
 
 	if (!ASIC_IS_NODCE(rdev)) {
@@ -6281,6 +6297,15 @@ restart_ih:
 				break;
 			}
 			break;
+		case 8: /* D1 page flip */
+		case 10: /* D2 page flip */
+		case 12: /* D3 page flip */
+		case 14: /* D4 page flip */
+		case 16: /* D5 page flip */
+		case 18: /* D6 page flip */
+			DRM_DEBUG("IH: D%d flip\n", ((src_id - 8) >> 1) + 1);
+			radeon_crtc_handle_flip(rdev, (src_id - 8) >> 1);
+			break;
 		case 42: /* HPD hotplug */
 			switch (src_data) {
 			case 0:
diff --git a/drivers/gpu/drm/radeon/si_dma.c b/drivers/gpu/drm/radeon/si_dma.c
index cf0fdad8c27..de0ca070122 100644
--- a/drivers/gpu/drm/radeon/si_dma.c
+++ b/drivers/gpu/drm/radeon/si_dma.c
@@ -213,6 +213,7 @@ int si_copy_dma(struct radeon_device *rdev,
 	r = radeon_fence_emit(rdev, fence, ring->idx);
 	if (r) {
 		radeon_ring_unlock_undo(rdev, ring);
+		radeon_semaphore_free(rdev, &sem, NULL);
 		return r;
 	}
 
diff --git a/drivers/gpu/drm/radeon/sid.h b/drivers/gpu/drm/radeon/sid.h
index 683532f8493..7321283602c 100644
--- a/drivers/gpu/drm/radeon/sid.h
+++ b/drivers/gpu/drm/radeon/sid.h
@@ -107,8 +107,8 @@
 #define		SPLL_CHG_STATUS				(1 << 1)
 #define	SPLL_CNTL_MODE					0x618
 #define		SPLL_SW_DIR_CONTROL			(1 << 0)
-#	define SPLL_REFCLK_SEL(x)			((x) << 8)
-#	define SPLL_REFCLK_SEL_MASK			0xFF00
+#	define SPLL_REFCLK_SEL(x)			((x) << 26)
+#	define SPLL_REFCLK_SEL_MASK			(3 << 26)
 
 #define	CG_SPLL_SPREAD_SPECTRUM				0x620
 #define		SSEN					(1 << 0)
diff --git a/drivers/gpu/drm/radeon/uvd_v1_0.c b/drivers/gpu/drm/radeon/uvd_v1_0.c
index 0a243f0e5d6..be42c812520 100644
--- a/drivers/gpu/drm/radeon/uvd_v1_0.c
+++ b/drivers/gpu/drm/radeon/uvd_v1_0.c
@@ -83,7 +83,10 @@ int uvd_v1_0_init(struct radeon_device *rdev)
 	int r;
 
 	/* raise clocks while booting up the VCPU */
-	radeon_set_uvd_clocks(rdev, 53300, 40000);
+	if (rdev->family < CHIP_RV740)
+		radeon_set_uvd_clocks(rdev, 10000, 10000);
+	else
+		radeon_set_uvd_clocks(rdev, 53300, 40000);
 
 	r = uvd_v1_0_start(rdev);
 	if (r)
@@ -407,7 +410,10 @@ int uvd_v1_0_ib_test(struct radeon_device *rdev, struct radeon_ring *ring)
 	struct radeon_fence *fence = NULL;
 	int r;
 
-	r = radeon_set_uvd_clocks(rdev, 53300, 40000);
+	if (rdev->family < CHIP_RV740)
+		r = radeon_set_uvd_clocks(rdev, 10000, 10000);
+	else
+		r = radeon_set_uvd_clocks(rdev, 53300, 40000);
 	if (r) {
 		DRM_ERROR("radeon: failed to raise UVD clocks (%d).\n", r);
 		return r;
diff --git a/drivers/gpu/drm/tegra/dc.c b/drivers/gpu/drm/tegra/dc.c
index 36c717af6cf..edb871d7d39 100644
--- a/drivers/gpu/drm/tegra/dc.c
+++ b/drivers/gpu/drm/tegra/dc.c
@@ -312,7 +312,7 @@ static void tegra_crtc_disable(struct drm_crtc *crtc)
 	struct drm_device *drm = crtc->dev;
 	struct drm_plane *plane;
 
-	list_for_each_entry(plane, &drm->mode_config.plane_list, head) {
+	drm_for_each_legacy_plane(plane, &drm->mode_config.plane_list) {
 		if (plane->crtc == crtc) {
 			tegra_plane_disable(plane);
 			plane->crtc = NULL;
diff --git a/drivers/gpu/drm/tegra/dpaux.c b/drivers/gpu/drm/tegra/dpaux.c
index d536ed381fb..005c19bd92d 100644
--- a/drivers/gpu/drm/tegra/dpaux.c
+++ b/drivers/gpu/drm/tegra/dpaux.c
@@ -99,55 +99,73 @@ static void tegra_dpaux_read_fifo(struct tegra_dpaux *dpaux, u8 *buffer,
 static ssize_t tegra_dpaux_transfer(struct drm_dp_aux *aux,
 				    struct drm_dp_aux_msg *msg)
 {
-	unsigned long value = DPAUX_DP_AUXCTL_TRANSACTREQ;
 	unsigned long timeout = msecs_to_jiffies(250);
 	struct tegra_dpaux *dpaux = to_dpaux(aux);
 	unsigned long status;
 	ssize_t ret = 0;
+	u32 value;
 
-	if (msg->size < 1 || msg->size > 16)
+	/* Tegra has 4x4 byte DP AUX transmit and receive FIFOs. */
+	if (msg->size > 16)
 		return -EINVAL;
 
-	tegra_dpaux_writel(dpaux, msg->address, DPAUX_DP_AUXADDR);
+	/*
+	 * Allow zero-sized messages only for I2C, in which case they specify
+	 * address-only transactions.
+	 */
+	if (msg->size < 1) {
+		switch (msg->request & ~DP_AUX_I2C_MOT) {
+		case DP_AUX_I2C_WRITE:
+		case DP_AUX_I2C_READ:
+			value = DPAUX_DP_AUXCTL_CMD_ADDRESS_ONLY;
+			break;
+
+		default:
+			return -EINVAL;
+		}
+	} else {
+		/* For non-zero-sized messages, set the CMDLEN field. */
+		value = DPAUX_DP_AUXCTL_CMDLEN(msg->size - 1);
+	}
 
 	switch (msg->request & ~DP_AUX_I2C_MOT) {
 	case DP_AUX_I2C_WRITE:
 		if (msg->request & DP_AUX_I2C_MOT)
-			value = DPAUX_DP_AUXCTL_CMD_MOT_WR;
+			value |= DPAUX_DP_AUXCTL_CMD_MOT_WR;
 		else
-			value = DPAUX_DP_AUXCTL_CMD_I2C_WR;
+			value |= DPAUX_DP_AUXCTL_CMD_I2C_WR;
 
 		break;
 
 	case DP_AUX_I2C_READ:
 		if (msg->request & DP_AUX_I2C_MOT)
-			value = DPAUX_DP_AUXCTL_CMD_MOT_RD;
+			value |= DPAUX_DP_AUXCTL_CMD_MOT_RD;
 		else
-			value = DPAUX_DP_AUXCTL_CMD_I2C_RD;
+			value |= DPAUX_DP_AUXCTL_CMD_I2C_RD;
 
 		break;
 
 	case DP_AUX_I2C_STATUS:
 		if (msg->request & DP_AUX_I2C_MOT)
-			value = DPAUX_DP_AUXCTL_CMD_MOT_RQ;
+			value |= DPAUX_DP_AUXCTL_CMD_MOT_RQ;
 		else
-			value = DPAUX_DP_AUXCTL_CMD_I2C_RQ;
+			value |= DPAUX_DP_AUXCTL_CMD_I2C_RQ;
 
 		break;
 
 	case DP_AUX_NATIVE_WRITE:
-		value = DPAUX_DP_AUXCTL_CMD_AUX_WR;
+		value |= DPAUX_DP_AUXCTL_CMD_AUX_WR;
 		break;
 
 	case DP_AUX_NATIVE_READ:
-		value = DPAUX_DP_AUXCTL_CMD_AUX_RD;
+		value |= DPAUX_DP_AUXCTL_CMD_AUX_RD;
 		break;
 
 	default:
 		return -EINVAL;
 	}
 
-	value |= DPAUX_DP_AUXCTL_CMDLEN(msg->size - 1);
+	tegra_dpaux_writel(dpaux, msg->address, DPAUX_DP_AUXADDR);
 	tegra_dpaux_writel(dpaux, value, DPAUX_DP_AUXCTL);
 
 	if ((msg->request & DP_AUX_I2C_READ) == 0) {
@@ -198,7 +216,7 @@ static ssize_t tegra_dpaux_transfer(struct drm_dp_aux *aux,
 		break;
 	}
 
-	if (msg->reply == DP_AUX_NATIVE_REPLY_ACK) {
+	if ((msg->size > 0) && (msg->reply == DP_AUX_NATIVE_REPLY_ACK)) {
 		if (msg->request & DP_AUX_I2C_READ) {
 			size_t count = value & DPAUX_DP_AUXSTAT_REPLY_MASK;
 
diff --git a/drivers/gpu/drm/tegra/dpaux.h b/drivers/gpu/drm/tegra/dpaux.h
index 4f5bf10fdff..806e245ca78 100644
--- a/drivers/gpu/drm/tegra/dpaux.h
+++ b/drivers/gpu/drm/tegra/dpaux.h
@@ -32,6 +32,7 @@
 #define DPAUX_DP_AUXCTL_CMD_I2C_RQ (2 << 12)
 #define DPAUX_DP_AUXCTL_CMD_I2C_RD (1 << 12)
 #define DPAUX_DP_AUXCTL_CMD_I2C_WR (0 << 12)
+#define DPAUX_DP_AUXCTL_CMD_ADDRESS_ONLY (1 << 8)
 #define DPAUX_DP_AUXCTL_CMDLEN(x) ((x) & 0xff)
 
 #define DPAUX_DP_AUXSTAT 0x31
diff --git a/drivers/gpu/drm/udl/udl_main.c b/drivers/gpu/drm/udl/udl_main.c
index f5ae57406f3..afdf383f630 100644
--- a/drivers/gpu/drm/udl/udl_main.c
+++ b/drivers/gpu/drm/udl/udl_main.c
@@ -283,7 +283,7 @@ int udl_submit_urb(struct drm_device *dev, struct urb *urb, size_t len)
 int udl_driver_load(struct drm_device *dev, unsigned long flags)
 {
 	struct udl_device *udl;
-	int ret;
+	int ret = -ENOMEM;
 
 	DRM_DEBUG("\n");
 	udl = kzalloc(sizeof(struct udl_device), GFP_KERNEL);
@@ -299,7 +299,6 @@ int udl_driver_load(struct drm_device *dev, unsigned long flags)
 	}
 
 	if (!udl_alloc_urb_list(dev, WRITES_IN_FLIGHT, MAX_TRANSFER)) {
-		ret = -ENOMEM;
 		DRM_ERROR("udl_alloc_urb_list failed\n");
 		goto err;
 	}
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_execbuf.c b/drivers/gpu/drm/vmwgfx/vmwgfx_execbuf.c
index 931490b9cfe..87df0b3674f 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_execbuf.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_execbuf.c
@@ -1214,14 +1214,36 @@ static int vmw_cmd_dma(struct vmw_private *dev_priv,
 		SVGA3dCmdSurfaceDMA dma;
 	} *cmd;
 	int ret;
+	SVGA3dCmdSurfaceDMASuffix *suffix;
+	uint32_t bo_size;
 
 	cmd = container_of(header, struct vmw_dma_cmd, header);
+	suffix = (SVGA3dCmdSurfaceDMASuffix *)((unsigned long) &cmd->dma +
+					       header->size - sizeof(*suffix));
+
+	/* Make sure device and verifier stays in sync. */
+	if (unlikely(suffix->suffixSize != sizeof(*suffix))) {
+		DRM_ERROR("Invalid DMA suffix size.\n");
+		return -EINVAL;
+	}
+
 	ret = vmw_translate_guest_ptr(dev_priv, sw_context,
 				      &cmd->dma.guest.ptr,
 				      &vmw_bo);
 	if (unlikely(ret != 0))
 		return ret;
 
+	/* Make sure DMA doesn't cross BO boundaries. */
+	bo_size = vmw_bo->base.num_pages * PAGE_SIZE;
+	if (unlikely(cmd->dma.guest.ptr.offset > bo_size)) {
+		DRM_ERROR("Invalid DMA offset.\n");
+		return -EINVAL;
+	}
+
+	bo_size -= cmd->dma.guest.ptr.offset;
+	if (unlikely(suffix->maximumOffset > bo_size))
+		suffix->maximumOffset = bo_size;
+
 	ret = vmw_cmd_res_check(dev_priv, sw_context, vmw_res_surface,
 				user_surface_converter, &cmd->dma.host.sid,
 				NULL);
diff --git a/drivers/gpu/host1x/hw/intr_hw.c b/drivers/gpu/host1x/hw/intr_hw.c
index db9017adfe2..498b37e3905 100644
--- a/drivers/gpu/host1x/hw/intr_hw.c
+++ b/drivers/gpu/host1x/hw/intr_hw.c
@@ -47,7 +47,7 @@ static irqreturn_t syncpt_thresh_isr(int irq, void *dev_id)
 	unsigned long reg;
 	int i, id;
 
-	for (i = 0; i <= BIT_WORD(host->info->nb_pts); i++) {
+	for (i = 0; i < DIV_ROUND_UP(host->info->nb_pts, 32); i++) {
 		reg = host1x_sync_readl(host,
 			HOST1X_SYNC_SYNCPT_THRESH_CPU0_INT_STATUS(i));
 		for_each_set_bit(id, &reg, BITS_PER_LONG) {
@@ -64,7 +64,7 @@ static void _host1x_intr_disable_all_syncpt_intrs(struct host1x *host)
 {
 	u32 i;
 
-	for (i = 0; i <= BIT_WORD(host->info->nb_pts); ++i) {
+	for (i = 0; i < DIV_ROUND_UP(host->info->nb_pts, 32); ++i) {
 		host1x_sync_writel(host, 0xffffffffu,
 			HOST1X_SYNC_SYNCPT_THRESH_INT_DISABLE(i));
 		host1x_sync_writel(host, 0xffffffffu,
diff --git a/drivers/hid/hid-core.c b/drivers/hid/hid-core.c
index 9e8064205bc..da52279de93 100644
--- a/drivers/hid/hid-core.c
+++ b/drivers/hid/hid-core.c
@@ -718,6 +718,9 @@ static int hid_scan_main(struct hid_parser *parser, struct hid_item *item)
 	case HID_MAIN_ITEM_TAG_END_COLLECTION:
 		break;
 	case HID_MAIN_ITEM_TAG_INPUT:
+		/* ignore constant inputs, they will be ignored by hid-input */
+		if (data & HID_MAIN_ITEM_CONSTANT)
+			break;
 		for (i = 0; i < parser->local.usage_index; i++)
 			hid_scan_input_usage(parser, parser->local.usage[i]);
 		break;
@@ -1250,7 +1253,8 @@ EXPORT_SYMBOL_GPL(hid_output_report);
 
 static int hid_report_len(struct hid_report *report)
 {
-	return ((report->size - 1) >> 3) + 1 + (report->id > 0) + 7;
+	/* equivalent to DIV_ROUND_UP(report->size, 8) + !!(report->id > 0) */
+	return ((report->size - 1) >> 3) + 1 + (report->id > 0);
 }
 
 /*
@@ -1263,7 +1267,7 @@ u8 *hid_alloc_report_buf(struct hid_report *report, gfp_t flags)
 	 * of implement() working on 8 byte chunks
 	 */
 
-	int len = hid_report_len(report);
+	int len = hid_report_len(report) + 7;
 
 	return kmalloc(len, flags);
 }
@@ -1821,8 +1825,6 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ 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) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_WIRELESS_OPTICAL_DESKTOP_3_0) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_TYPE_COVER_2) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_TOUCH_COVER_2) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_OFFICE_KB) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MONTEREY, USB_DEVICE_ID_GENIUS_KB29E) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_NTRIG, USB_DEVICE_ID_NTRIG_TOUCH_SCREEN) },
diff --git a/drivers/hid/hid-ids.h b/drivers/hid/hid-ids.h
index bd221263c73..34bb2205d2e 100644
--- a/drivers/hid/hid-ids.h
+++ b/drivers/hid/hid-ids.h
@@ -301,6 +301,9 @@
 
 #define USB_VENDOR_ID_DREAM_CHEEKY	0x1d34
 
+#define USB_VENDOR_ID_ELITEGROUP	0x03fc
+#define USB_DEVICE_ID_ELITEGROUP_05D8	0x05d8
+
 #define USB_VENDOR_ID_ELO		0x04E7
 #define USB_DEVICE_ID_ELO_TS2515	0x0022
 #define USB_DEVICE_ID_ELO_TS2700	0x0020
@@ -455,7 +458,8 @@
 
 #define USB_VENDOR_ID_INTEL_0		0x8086
 #define USB_VENDOR_ID_INTEL_1		0x8087
-#define USB_DEVICE_ID_INTEL_HID_SENSOR	0x09fa
+#define USB_DEVICE_ID_INTEL_HID_SENSOR_0	0x09fa
+#define USB_DEVICE_ID_INTEL_HID_SENSOR_1	0x0a04
 
 #define USB_VENDOR_ID_STM_0             0x0483
 #define USB_DEVICE_ID_STM_HID_SENSOR    0x91d1
@@ -629,8 +633,6 @@
 #define USB_DEVICE_ID_MS_PRESENTER_8K_USB	0x0713
 #define USB_DEVICE_ID_MS_DIGITAL_MEDIA_3K	0x0730
 #define USB_DEVICE_ID_MS_COMFORT_MOUSE_4500	0x076c
-#define USB_DEVICE_ID_MS_TOUCH_COVER_2	0x07a7
-#define USB_DEVICE_ID_MS_TYPE_COVER_2	0x07a9
 
 #define USB_VENDOR_ID_MOJO		0x8282
 #define USB_DEVICE_ID_RETRO_ADAPTER	0x3201
@@ -835,6 +837,10 @@
 #define USB_DEVICE_ID_SYNAPTICS_LTS2	0x1d10
 #define USB_DEVICE_ID_SYNAPTICS_HD	0x0ac3
 #define USB_DEVICE_ID_SYNAPTICS_QUAD_HD	0x1ac3
+#define USB_DEVICE_ID_SYNAPTICS_TP_V103	0x5710
+
+#define USB_VENDOR_ID_TEXAS_INSTRUMENTS	0x2047
+#define USB_DEVICE_ID_TEXAS_INSTRUMENTS_LENOVO_YOGA	0x0855
 
 #define USB_VENDOR_ID_THINGM		0x27b8
 #define USB_DEVICE_ID_BLINK1		0x01ed
diff --git a/drivers/hid/hid-microsoft.c b/drivers/hid/hid-microsoft.c
index 6fd58175a29..8ba17a946f2 100644
--- a/drivers/hid/hid-microsoft.c
+++ b/drivers/hid/hid-microsoft.c
@@ -274,10 +274,6 @@ static const struct hid_device_id ms_devices[] = {
 		.driver_data = MS_NOGET },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_COMFORT_MOUSE_4500),
 		.driver_data = MS_DUPLICATE_USAGES },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_TYPE_COVER_2),
-		.driver_data = 0 },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_TOUCH_COVER_2),
-		.driver_data = 0 },
 
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_PRESENTER_8K_BT),
 		.driver_data = MS_PRESENTER },
diff --git a/drivers/hid/hid-multitouch.c b/drivers/hid/hid-multitouch.c
index 35278e43c7a..51e25b9407f 100644
--- a/drivers/hid/hid-multitouch.c
+++ b/drivers/hid/hid-multitouch.c
@@ -1155,6 +1155,11 @@ static const struct hid_device_id mt_devices[] = {
 		MT_USB_DEVICE(USB_VENDOR_ID_DWAV,
 			USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_A001) },
 
+	/* Elitegroup panel */
+	{ .driver_data = MT_CLS_SERIAL,
+		MT_USB_DEVICE(USB_VENDOR_ID_ELITEGROUP,
+			USB_DEVICE_ID_ELITEGROUP_05D8) },
+
 	/* Flatfrog Panels */
 	{ .driver_data = MT_CLS_FLATFROG,
 		MT_USB_DEVICE(USB_VENDOR_ID_FLATFROG,
diff --git a/drivers/hid/hid-sensor-hub.c b/drivers/hid/hid-sensor-hub.c
index 5182031f7b5..be14b5690e9 100644
--- a/drivers/hid/hid-sensor-hub.c
+++ b/drivers/hid/hid-sensor-hub.c
@@ -697,14 +697,20 @@ static void sensor_hub_remove(struct hid_device *hdev)
 
 static const struct hid_device_id sensor_hub_devices[] = {
 	{ HID_DEVICE(HID_BUS_ANY, HID_GROUP_SENSOR_HUB, USB_VENDOR_ID_INTEL_0,
-			USB_DEVICE_ID_INTEL_HID_SENSOR),
+			USB_DEVICE_ID_INTEL_HID_SENSOR_0),
 			.driver_data = HID_SENSOR_HUB_ENUM_QUIRK},
 	{ HID_DEVICE(HID_BUS_ANY, HID_GROUP_SENSOR_HUB, USB_VENDOR_ID_INTEL_1,
-			USB_DEVICE_ID_INTEL_HID_SENSOR),
+			USB_DEVICE_ID_INTEL_HID_SENSOR_0),
+			.driver_data = HID_SENSOR_HUB_ENUM_QUIRK},
+	{ HID_DEVICE(HID_BUS_ANY, HID_GROUP_SENSOR_HUB, USB_VENDOR_ID_INTEL_1,
+			USB_DEVICE_ID_INTEL_HID_SENSOR_1),
 			.driver_data = HID_SENSOR_HUB_ENUM_QUIRK},
 	{ HID_DEVICE(HID_BUS_ANY, HID_GROUP_SENSOR_HUB, USB_VENDOR_ID_STM_0,
 			USB_DEVICE_ID_STM_HID_SENSOR),
 			.driver_data = HID_SENSOR_HUB_ENUM_QUIRK},
+	{ HID_DEVICE(HID_BUS_ANY, HID_GROUP_SENSOR_HUB, USB_VENDOR_ID_TEXAS_INSTRUMENTS,
+			USB_DEVICE_ID_TEXAS_INSTRUMENTS_LENOVO_YOGA),
+			.driver_data = HID_SENSOR_HUB_ENUM_QUIRK},
 	{ HID_DEVICE(HID_BUS_ANY, HID_GROUP_SENSOR_HUB, HID_ANY_ID,
 		     HID_ANY_ID) },
 	{ }
diff --git a/drivers/hid/hid-sony.c b/drivers/hid/hid-sony.c
index 69204afea7a..908de278921 100644
--- a/drivers/hid/hid-sony.c
+++ b/drivers/hid/hid-sony.c
@@ -1721,8 +1721,6 @@ static void sony_remove(struct hid_device *hdev)
 	if (sc->quirks & SONY_LED_SUPPORT)
 		sony_leds_remove(hdev);
 
-	if (sc->worker_initialized)
-		cancel_work_sync(&sc->state_worker);
 	if (sc->quirks & SONY_BATTERY_SUPPORT) {
 		hid_hw_close(hdev);
 		sony_battery_remove(sc);
diff --git a/drivers/hid/usbhid/hid-quirks.c b/drivers/hid/usbhid/hid-quirks.c
index dbd83878ff9..8e4ddb36988 100644
--- a/drivers/hid/usbhid/hid-quirks.c
+++ b/drivers/hid/usbhid/hid-quirks.c
@@ -119,6 +119,7 @@ static const struct hid_blacklist {
 	{ USB_VENDOR_ID_SYNAPTICS, USB_DEVICE_ID_SYNAPTICS_LTS2, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_SYNAPTICS, USB_DEVICE_ID_SYNAPTICS_HD, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_SYNAPTICS, USB_DEVICE_ID_SYNAPTICS_QUAD_HD, HID_QUIRK_NO_INIT_REPORTS },
+	{ USB_VENDOR_ID_SYNAPTICS, USB_DEVICE_ID_SYNAPTICS_TP_V103, HID_QUIRK_NO_INIT_REPORTS },
 
 	{ 0, 0 }
 };
diff --git a/drivers/hsi/Kconfig b/drivers/hsi/Kconfig
index d94e38dd80c..2c76de438eb 100644
--- a/drivers/hsi/Kconfig
+++ b/drivers/hsi/Kconfig
@@ -14,6 +14,7 @@ config HSI_BOARDINFO
 	bool
 	default y
 
+source "drivers/hsi/controllers/Kconfig"
 source "drivers/hsi/clients/Kconfig"
 
 endif # HSI
diff --git a/drivers/hsi/Makefile b/drivers/hsi/Makefile
index 9d5d33f90de..360371e134f 100644
--- a/drivers/hsi/Makefile
+++ b/drivers/hsi/Makefile
@@ -3,4 +3,5 @@
 #
 obj-$(CONFIG_HSI_BOARDINFO)	+= hsi_boardinfo.o
 obj-$(CONFIG_HSI)		+= hsi.o
+obj-y				+= controllers/
 obj-y				+= clients/
diff --git a/drivers/hsi/clients/Kconfig b/drivers/hsi/clients/Kconfig
index 3bacd275f47..71b9f9ab86e 100644
--- a/drivers/hsi/clients/Kconfig
+++ b/drivers/hsi/clients/Kconfig
@@ -4,6 +4,23 @@
 
 comment "HSI clients"
 
+config NOKIA_MODEM
+	tristate "Nokia Modem"
+	depends on HSI && SSI_PROTOCOL
+	help
+	Say Y here if you want to add support for the modem on Nokia
+	N900 (Nokia RX-51) hardware.
+
+	If unsure, say N.
+
+config SSI_PROTOCOL
+	tristate "SSI protocol"
+	depends on HSI && PHONET && (OMAP_SSI=y || OMAP_SSI=m)
+	help
+	If you say Y here, you will enable the SSI protocol aka McSAAB.
+
+	If unsure, say N.
+
 config HSI_CHAR
 	tristate "HSI/SSI character driver"
 	depends on HSI
diff --git a/drivers/hsi/clients/Makefile b/drivers/hsi/clients/Makefile
index 327c0e27c8b..4d5bc0e0b27 100644
--- a/drivers/hsi/clients/Makefile
+++ b/drivers/hsi/clients/Makefile
@@ -2,4 +2,6 @@
 # Makefile for HSI clients
 #
 
-obj-$(CONFIG_HSI_CHAR)	+= hsi_char.o
+obj-$(CONFIG_NOKIA_MODEM)	+= nokia-modem.o
+obj-$(CONFIG_SSI_PROTOCOL)	+= ssi_protocol.o
+obj-$(CONFIG_HSI_CHAR)		+= hsi_char.o
diff --git a/drivers/hsi/clients/hsi_char.c b/drivers/hsi/clients/hsi_char.c
index e61e5f991aa..57f70c28fa3 100644
--- a/drivers/hsi/clients/hsi_char.c
+++ b/drivers/hsi/clients/hsi_char.c
@@ -367,7 +367,7 @@ static int hsc_rx_set(struct hsi_client *cl, struct hsc_rx_config *rxc)
 		return -EINVAL;
 	tmp = cl->rx_cfg;
 	cl->rx_cfg.mode = rxc->mode;
-	cl->rx_cfg.channels = rxc->channels;
+	cl->rx_cfg.num_hw_channels = rxc->channels;
 	cl->rx_cfg.flow = rxc->flow;
 	ret = hsi_setup(cl);
 	if (ret < 0) {
@@ -383,7 +383,7 @@ static int hsc_rx_set(struct hsi_client *cl, struct hsc_rx_config *rxc)
 static inline void hsc_rx_get(struct hsi_client *cl, struct hsc_rx_config *rxc)
 {
 	rxc->mode = cl->rx_cfg.mode;
-	rxc->channels = cl->rx_cfg.channels;
+	rxc->channels = cl->rx_cfg.num_hw_channels;
 	rxc->flow = cl->rx_cfg.flow;
 }
 
@@ -402,7 +402,7 @@ static int hsc_tx_set(struct hsi_client *cl, struct hsc_tx_config *txc)
 		return -EINVAL;
 	tmp = cl->tx_cfg;
 	cl->tx_cfg.mode = txc->mode;
-	cl->tx_cfg.channels = txc->channels;
+	cl->tx_cfg.num_hw_channels = txc->channels;
 	cl->tx_cfg.speed = txc->speed;
 	cl->tx_cfg.arb_mode = txc->arb_mode;
 	ret = hsi_setup(cl);
@@ -417,7 +417,7 @@ static int hsc_tx_set(struct hsi_client *cl, struct hsc_tx_config *txc)
 static inline void hsc_tx_get(struct hsi_client *cl, struct hsc_tx_config *txc)
 {
 	txc->mode = cl->tx_cfg.mode;
-	txc->channels = cl->tx_cfg.channels;
+	txc->channels = cl->tx_cfg.num_hw_channels;
 	txc->speed = cl->tx_cfg.speed;
 	txc->arb_mode = cl->tx_cfg.arb_mode;
 }
@@ -435,7 +435,7 @@ static ssize_t hsc_read(struct file *file, char __user *buf, size_t len,
 		return -EINVAL;
 	if (len > max_data_size)
 		len = max_data_size;
-	if (channel->ch >= channel->cl->rx_cfg.channels)
+	if (channel->ch >= channel->cl->rx_cfg.num_hw_channels)
 		return -ECHRNG;
 	if (test_and_set_bit(HSC_CH_READ, &channel->flags))
 		return -EBUSY;
@@ -492,7 +492,7 @@ static ssize_t hsc_write(struct file *file, const char __user *buf, size_t len,
 		return -EINVAL;
 	if (len > max_data_size)
 		len = max_data_size;
-	if (channel->ch >= channel->cl->tx_cfg.channels)
+	if (channel->ch >= channel->cl->tx_cfg.num_hw_channels)
 		return -ECHRNG;
 	if (test_and_set_bit(HSC_CH_WRITE, &channel->flags))
 		return -EBUSY;
@@ -705,7 +705,7 @@ static int hsc_probe(struct device *dev)
 	if (!hsc_major) {
 		ret = alloc_chrdev_region(&hsc_dev, hsc_baseminor,
 						HSC_DEVS, devname);
-		if (ret > 0)
+		if (ret == 0)
 			hsc_major = MAJOR(hsc_dev);
 	} else {
 		hsc_dev = MKDEV(hsc_major, hsc_baseminor);
diff --git a/drivers/hsi/clients/nokia-modem.c b/drivers/hsi/clients/nokia-modem.c
new file mode 100644
index 00000000000..363b780dace
--- /dev/null
+++ b/drivers/hsi/clients/nokia-modem.c
@@ -0,0 +1,285 @@
+/*
+ * nokia-modem.c
+ *
+ * HSI client driver for Nokia N900 modem.
+ *
+ * Copyright (C) 2014 Sebastian Reichel <sre@kernel.org>
+ *
+ * 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/gpio/consumer.h>
+#include <linux/hsi/hsi.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/of.h>
+#include <linux/of_irq.h>
+#include <linux/of_gpio.h>
+#include <linux/hsi/ssi_protocol.h>
+
+static unsigned int pm;
+module_param(pm, int, 0400);
+MODULE_PARM_DESC(pm,
+	"Enable power management (0=disabled, 1=userland based [default])");
+
+struct nokia_modem_gpio {
+	struct gpio_desc	*gpio;
+	const char		*name;
+};
+
+struct nokia_modem_device {
+	struct tasklet_struct	nokia_modem_rst_ind_tasklet;
+	int			nokia_modem_rst_ind_irq;
+	struct device		*device;
+	struct nokia_modem_gpio	*gpios;
+	int			gpio_amount;
+	struct hsi_client	*ssi_protocol;
+};
+
+static void do_nokia_modem_rst_ind_tasklet(unsigned long data)
+{
+	struct nokia_modem_device *modem = (struct nokia_modem_device *)data;
+
+	if (!modem)
+		return;
+
+	dev_info(modem->device, "CMT rst line change detected\n");
+
+	if (modem->ssi_protocol)
+		ssip_reset_event(modem->ssi_protocol);
+}
+
+static irqreturn_t nokia_modem_rst_ind_isr(int irq, void *data)
+{
+	struct nokia_modem_device *modem = (struct nokia_modem_device *)data;
+
+	tasklet_schedule(&modem->nokia_modem_rst_ind_tasklet);
+
+	return IRQ_HANDLED;
+}
+
+static void nokia_modem_gpio_unexport(struct device *dev)
+{
+	struct nokia_modem_device *modem = dev_get_drvdata(dev);
+	int i;
+
+	for (i = 0; i < modem->gpio_amount; i++) {
+		sysfs_remove_link(&dev->kobj, modem->gpios[i].name);
+		gpiod_unexport(modem->gpios[i].gpio);
+	}
+}
+
+static int nokia_modem_gpio_probe(struct device *dev)
+{
+	struct device_node *np = dev->of_node;
+	struct nokia_modem_device *modem = dev_get_drvdata(dev);
+	int gpio_count, gpio_name_count, i, err;
+
+	gpio_count = of_gpio_count(np);
+
+	if (gpio_count < 0) {
+		dev_err(dev, "missing gpios: %d\n", gpio_count);
+		return gpio_count;
+	}
+
+	gpio_name_count = of_property_count_strings(np, "gpio-names");
+
+	if (gpio_count != gpio_name_count) {
+		dev_err(dev, "number of gpios does not equal number of gpio names\n");
+		return -EINVAL;
+	}
+
+	modem->gpios = devm_kzalloc(dev, gpio_count *
+				sizeof(struct nokia_modem_gpio), GFP_KERNEL);
+	if (!modem->gpios) {
+		dev_err(dev, "Could not allocate memory for gpios\n");
+		return -ENOMEM;
+	}
+
+	modem->gpio_amount = gpio_count;
+
+	for (i = 0; i < gpio_count; i++) {
+		modem->gpios[i].gpio = devm_gpiod_get_index(dev, NULL, i);
+		if (IS_ERR(modem->gpios[i].gpio)) {
+			dev_err(dev, "Could not get gpio %d\n", i);
+			return PTR_ERR(modem->gpios[i].gpio);
+		}
+
+		err = of_property_read_string_index(np, "gpio-names", i,
+						&(modem->gpios[i].name));
+		if (err) {
+			dev_err(dev, "Could not get gpio name %d\n", i);
+			return err;
+		}
+
+		err = gpiod_direction_output(modem->gpios[i].gpio, 0);
+		if (err)
+			return err;
+
+		err = gpiod_export(modem->gpios[i].gpio, 0);
+		if (err)
+			return err;
+
+		err = gpiod_export_link(dev, modem->gpios[i].name,
+							modem->gpios[i].gpio);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
+static int nokia_modem_probe(struct device *dev)
+{
+	struct device_node *np;
+	struct nokia_modem_device *modem;
+	struct hsi_client *cl = to_hsi_client(dev);
+	struct hsi_port *port = hsi_get_port(cl);
+	int irq, pflags, err;
+	struct hsi_board_info ssip;
+
+	np = dev->of_node;
+	if (!np) {
+		dev_err(dev, "device tree node not found\n");
+		return -ENXIO;
+	}
+
+	modem = devm_kzalloc(dev, sizeof(*modem), GFP_KERNEL);
+	if (!modem) {
+		dev_err(dev, "Could not allocate memory for nokia_modem_device\n");
+		return -ENOMEM;
+	}
+	dev_set_drvdata(dev, modem);
+
+	irq = irq_of_parse_and_map(np, 0);
+	if (irq < 0) {
+		dev_err(dev, "Invalid rst_ind interrupt (%d)\n", irq);
+		return irq;
+	}
+	modem->nokia_modem_rst_ind_irq = irq;
+	pflags = irq_get_trigger_type(irq);
+
+	tasklet_init(&modem->nokia_modem_rst_ind_tasklet,
+			do_nokia_modem_rst_ind_tasklet, (unsigned long)modem);
+	err = devm_request_irq(dev, irq, nokia_modem_rst_ind_isr,
+				IRQF_DISABLED | pflags, "modem_rst_ind", modem);
+	if (err < 0) {
+		dev_err(dev, "Request rst_ind irq(%d) failed (flags %d)\n",
+								irq, pflags);
+		return err;
+	}
+	enable_irq_wake(irq);
+
+	if(pm) {
+		err = nokia_modem_gpio_probe(dev);
+		if (err < 0) {
+			dev_err(dev, "Could not probe GPIOs\n");
+			goto error1;
+		}
+	}
+
+	ssip.name = "ssi-protocol";
+	ssip.tx_cfg = cl->tx_cfg;
+	ssip.rx_cfg = cl->rx_cfg;
+	ssip.platform_data = NULL;
+	ssip.archdata = NULL;
+
+	modem->ssi_protocol = hsi_new_client(port, &ssip);
+	if (!modem->ssi_protocol) {
+		dev_err(dev, "Could not register ssi-protocol device\n");
+		goto error2;
+	}
+
+	err = device_attach(&modem->ssi_protocol->device);
+	if (err == 0) {
+		dev_err(dev, "Missing ssi-protocol driver\n");
+		err = -EPROBE_DEFER;
+		goto error3;
+	} else if (err < 0) {
+		dev_err(dev, "Could not load ssi-protocol driver (%d)\n", err);
+		goto error3;
+	}
+
+	/* TODO: register cmt-speech hsi client */
+
+	dev_info(dev, "Registered Nokia HSI modem\n");
+
+	return 0;
+
+error3:
+	hsi_remove_client(&modem->ssi_protocol->device, NULL);
+error2:
+	nokia_modem_gpio_unexport(dev);
+error1:
+	disable_irq_wake(modem->nokia_modem_rst_ind_irq);
+	tasklet_kill(&modem->nokia_modem_rst_ind_tasklet);
+
+	return err;
+}
+
+static int nokia_modem_remove(struct device *dev)
+{
+	struct nokia_modem_device *modem = dev_get_drvdata(dev);
+
+	if (!modem)
+		return 0;
+
+	if (modem->ssi_protocol) {
+		hsi_remove_client(&modem->ssi_protocol->device, NULL);
+		modem->ssi_protocol = NULL;
+	}
+
+	nokia_modem_gpio_unexport(dev);
+	dev_set_drvdata(dev, NULL);
+	disable_irq_wake(modem->nokia_modem_rst_ind_irq);
+	tasklet_kill(&modem->nokia_modem_rst_ind_tasklet);
+
+	return 0;
+}
+
+#ifdef CONFIG_OF
+static const struct of_device_id nokia_modem_of_match[] = {
+	{ .compatible = "nokia,n900-modem", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, nokia_modem_of_match);
+#endif
+
+static struct hsi_client_driver nokia_modem_driver = {
+	.driver = {
+		.name	= "nokia-modem",
+		.owner	= THIS_MODULE,
+		.probe	= nokia_modem_probe,
+		.remove	= nokia_modem_remove,
+		.of_match_table = of_match_ptr(nokia_modem_of_match),
+	},
+};
+
+static int __init nokia_modem_init(void)
+{
+	return hsi_register_client_driver(&nokia_modem_driver);
+}
+module_init(nokia_modem_init);
+
+static void __exit nokia_modem_exit(void)
+{
+	hsi_unregister_client_driver(&nokia_modem_driver);
+}
+module_exit(nokia_modem_exit);
+
+MODULE_ALIAS("hsi:nokia-modem");
+MODULE_AUTHOR("Sebastian Reichel <sre@kernel.org>");
+MODULE_DESCRIPTION("HSI driver module for Nokia N900 Modem");
+MODULE_LICENSE("GPL");
diff --git a/drivers/hsi/clients/ssi_protocol.c b/drivers/hsi/clients/ssi_protocol.c
new file mode 100644
index 00000000000..ce4be3738d4
--- /dev/null
+++ b/drivers/hsi/clients/ssi_protocol.c
@@ -0,0 +1,1191 @@
+/*
+ * ssi_protocol.c
+ *
+ * Implementation of the SSI McSAAB improved protocol.
+ *
+ * Copyright (C) 2010 Nokia Corporation. All rights reserved.
+ * Copyright (C) 2013 Sebastian Reichel <sre@kernel.org>
+ *
+ * Contact: Carlos Chinea <carlos.chinea@nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ */
+
+#include <linux/atomic.h>
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/if_ether.h>
+#include <linux/if_arp.h>
+#include <linux/if_phonet.h>
+#include <linux/init.h>
+#include <linux/irq.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <linux/notifier.h>
+#include <linux/scatterlist.h>
+#include <linux/skbuff.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/timer.h>
+#include <linux/hsi/hsi.h>
+#include <linux/hsi/ssi_protocol.h>
+
+void ssi_waketest(struct hsi_client *cl, unsigned int enable);
+
+#define SSIP_TXQUEUE_LEN	100
+#define SSIP_MAX_MTU		65535
+#define SSIP_DEFAULT_MTU	4000
+#define PN_MEDIA_SOS		21
+#define SSIP_MIN_PN_HDR		6	/* FIXME: Revisit */
+#define SSIP_WDTOUT		2000	/* FIXME: has to be 500 msecs */
+#define SSIP_KATOUT		15	/* 15 msecs */
+#define SSIP_MAX_CMDS		5 /* Number of pre-allocated commands buffers */
+#define SSIP_BYTES_TO_FRAMES(x) ((((x) - 1) >> 2) + 1)
+#define SSIP_CMT_LOADER_SYNC	0x11223344
+/*
+ * SSI protocol command definitions
+ */
+#define SSIP_COMMAND(data)	((data) >> 28)
+#define SSIP_PAYLOAD(data)	((data) & 0xfffffff)
+/* Commands */
+#define SSIP_SW_BREAK		0
+#define SSIP_BOOTINFO_REQ	1
+#define SSIP_BOOTINFO_RESP	2
+#define SSIP_WAKETEST_RESULT	3
+#define SSIP_START_TRANS	4
+#define SSIP_READY		5
+/* Payloads */
+#define SSIP_DATA_VERSION(data)	((data) & 0xff)
+#define SSIP_LOCAL_VERID	1
+#define SSIP_WAKETEST_OK	0
+#define SSIP_WAKETEST_FAILED	1
+#define SSIP_PDU_LENGTH(data)	(((data) >> 8) & 0xffff)
+#define SSIP_MSG_ID(data)	((data) & 0xff)
+/* Generic Command */
+#define SSIP_CMD(cmd, payload)	(((cmd) << 28) | ((payload) & 0xfffffff))
+/* Commands for the control channel */
+#define SSIP_BOOTINFO_REQ_CMD(ver) \
+		SSIP_CMD(SSIP_BOOTINFO_REQ, SSIP_DATA_VERSION(ver))
+#define SSIP_BOOTINFO_RESP_CMD(ver) \
+		SSIP_CMD(SSIP_BOOTINFO_RESP, SSIP_DATA_VERSION(ver))
+#define SSIP_START_TRANS_CMD(pdulen, id) \
+		SSIP_CMD(SSIP_START_TRANS, (((pdulen) << 8) | SSIP_MSG_ID(id)))
+#define SSIP_READY_CMD		SSIP_CMD(SSIP_READY, 0)
+#define SSIP_SWBREAK_CMD	SSIP_CMD(SSIP_SW_BREAK, 0)
+
+/* Main state machine states */
+enum {
+	INIT,
+	HANDSHAKE,
+	ACTIVE,
+};
+
+/* Send state machine states */
+enum {
+	SEND_IDLE,
+	WAIT4READY,
+	SEND_READY,
+	SENDING,
+	SENDING_SWBREAK,
+};
+
+/* Receive state machine states */
+enum {
+	RECV_IDLE,
+	RECV_READY,
+	RECEIVING,
+};
+
+/**
+ * struct ssi_protocol - SSI protocol (McSAAB) data
+ * @main_state: Main state machine
+ * @send_state: TX state machine
+ * @recv_state: RX state machine
+ * @waketest: Flag to follow wake line test
+ * @rxid: RX data id
+ * @txid: TX data id
+ * @txqueue_len: TX queue length
+ * @tx_wd: TX watchdog
+ * @rx_wd: RX watchdog
+ * @keep_alive: Workaround for SSI HW bug
+ * @lock: To serialize access to this struct
+ * @netdev: Phonet network device
+ * @txqueue: TX data queue
+ * @cmdqueue: Queue of free commands
+ * @cl: HSI client own reference
+ * @link: Link for ssip_list
+ * @tx_usecount: Refcount to keep track the slaves that use the wake line
+ * @channel_id_cmd: HSI channel id for command stream
+ * @channel_id_data: HSI channel id for data stream
+ */
+struct ssi_protocol {
+	unsigned int		main_state;
+	unsigned int		send_state;
+	unsigned int		recv_state;
+	unsigned int		waketest:1;
+	u8			rxid;
+	u8			txid;
+	unsigned int		txqueue_len;
+	struct timer_list	tx_wd;
+	struct timer_list	rx_wd;
+	struct timer_list	keep_alive; /* wake-up workaround */
+	spinlock_t		lock;
+	struct net_device	*netdev;
+	struct list_head	txqueue;
+	struct list_head	cmdqueue;
+	struct hsi_client	*cl;
+	struct list_head	link;
+	atomic_t		tx_usecnt;
+	int			channel_id_cmd;
+	int			channel_id_data;
+};
+
+/* List of ssi protocol instances */
+static LIST_HEAD(ssip_list);
+
+static void ssip_rxcmd_complete(struct hsi_msg *msg);
+
+static inline void ssip_set_cmd(struct hsi_msg *msg, u32 cmd)
+{
+	u32 *data;
+
+	data = sg_virt(msg->sgt.sgl);
+	*data = cmd;
+}
+
+static inline u32 ssip_get_cmd(struct hsi_msg *msg)
+{
+	u32 *data;
+
+	data = sg_virt(msg->sgt.sgl);
+
+	return *data;
+}
+
+static void ssip_skb_to_msg(struct sk_buff *skb, struct hsi_msg *msg)
+{
+	skb_frag_t *frag;
+	struct scatterlist *sg;
+	int i;
+
+	BUG_ON(msg->sgt.nents != (unsigned int)(skb_shinfo(skb)->nr_frags + 1));
+
+	sg = msg->sgt.sgl;
+	sg_set_buf(sg, skb->data, skb_headlen(skb));
+	for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
+		sg = sg_next(sg);
+		BUG_ON(!sg);
+		frag = &skb_shinfo(skb)->frags[i];
+		sg_set_page(sg, frag->page.p, frag->size, frag->page_offset);
+	}
+}
+
+static void ssip_free_data(struct hsi_msg *msg)
+{
+	struct sk_buff *skb;
+
+	skb = msg->context;
+	pr_debug("free data: msg %p context %p skb %p\n", msg, msg->context,
+								skb);
+	msg->destructor = NULL;
+	dev_kfree_skb(skb);
+	hsi_free_msg(msg);
+}
+
+static struct hsi_msg *ssip_alloc_data(struct ssi_protocol *ssi,
+					struct sk_buff *skb, gfp_t flags)
+{
+	struct hsi_msg *msg;
+
+	msg = hsi_alloc_msg(skb_shinfo(skb)->nr_frags + 1, flags);
+	if (!msg)
+		return NULL;
+	ssip_skb_to_msg(skb, msg);
+	msg->destructor = ssip_free_data;
+	msg->channel = ssi->channel_id_data;
+	msg->context = skb;
+
+	return msg;
+}
+
+static inline void ssip_release_cmd(struct hsi_msg *msg)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(msg->cl);
+
+	dev_dbg(&msg->cl->device, "Release cmd 0x%08x\n", ssip_get_cmd(msg));
+	spin_lock_bh(&ssi->lock);
+	list_add_tail(&msg->link, &ssi->cmdqueue);
+	spin_unlock_bh(&ssi->lock);
+}
+
+static struct hsi_msg *ssip_claim_cmd(struct ssi_protocol *ssi)
+{
+	struct hsi_msg *msg;
+
+	BUG_ON(list_empty(&ssi->cmdqueue));
+
+	spin_lock_bh(&ssi->lock);
+	msg = list_first_entry(&ssi->cmdqueue, struct hsi_msg, link);
+	list_del(&msg->link);
+	spin_unlock_bh(&ssi->lock);
+	msg->destructor = ssip_release_cmd;
+
+	return msg;
+}
+
+static void ssip_free_cmds(struct ssi_protocol *ssi)
+{
+	struct hsi_msg *msg, *tmp;
+
+	list_for_each_entry_safe(msg, tmp, &ssi->cmdqueue, link) {
+		list_del(&msg->link);
+		msg->destructor = NULL;
+		kfree(sg_virt(msg->sgt.sgl));
+		hsi_free_msg(msg);
+	}
+}
+
+static int ssip_alloc_cmds(struct ssi_protocol *ssi)
+{
+	struct hsi_msg *msg;
+	u32 *buf;
+	unsigned int i;
+
+	for (i = 0; i < SSIP_MAX_CMDS; i++) {
+		msg = hsi_alloc_msg(1, GFP_KERNEL);
+		if (!msg)
+			goto out;
+		buf = kmalloc(sizeof(*buf), GFP_KERNEL);
+		if (!buf) {
+			hsi_free_msg(msg);
+			goto out;
+		}
+		sg_init_one(msg->sgt.sgl, buf, sizeof(*buf));
+		msg->channel = ssi->channel_id_cmd;
+		list_add_tail(&msg->link, &ssi->cmdqueue);
+	}
+
+	return 0;
+out:
+	ssip_free_cmds(ssi);
+
+	return -ENOMEM;
+}
+
+static void ssip_set_rxstate(struct ssi_protocol *ssi, unsigned int state)
+{
+	ssi->recv_state = state;
+	switch (state) {
+	case RECV_IDLE:
+		del_timer(&ssi->rx_wd);
+		if (ssi->send_state == SEND_IDLE)
+			del_timer(&ssi->keep_alive);
+		break;
+	case RECV_READY:
+		/* CMT speech workaround */
+		if (atomic_read(&ssi->tx_usecnt))
+			break;
+		/* Otherwise fall through */
+	case RECEIVING:
+		mod_timer(&ssi->keep_alive, jiffies +
+						msecs_to_jiffies(SSIP_KATOUT));
+		mod_timer(&ssi->rx_wd, jiffies + msecs_to_jiffies(SSIP_WDTOUT));
+		break;
+	default:
+		break;
+	}
+}
+
+static void ssip_set_txstate(struct ssi_protocol *ssi, unsigned int state)
+{
+	ssi->send_state = state;
+	switch (state) {
+	case SEND_IDLE:
+	case SEND_READY:
+		del_timer(&ssi->tx_wd);
+		if (ssi->recv_state == RECV_IDLE)
+			del_timer(&ssi->keep_alive);
+		break;
+	case WAIT4READY:
+	case SENDING:
+	case SENDING_SWBREAK:
+		mod_timer(&ssi->keep_alive,
+				jiffies + msecs_to_jiffies(SSIP_KATOUT));
+		mod_timer(&ssi->tx_wd, jiffies + msecs_to_jiffies(SSIP_WDTOUT));
+		break;
+	default:
+		break;
+	}
+}
+
+struct hsi_client *ssip_slave_get_master(struct hsi_client *slave)
+{
+	struct hsi_client *master = ERR_PTR(-ENODEV);
+	struct ssi_protocol *ssi;
+
+	list_for_each_entry(ssi, &ssip_list, link)
+		if (slave->device.parent == ssi->cl->device.parent) {
+			master = ssi->cl;
+			break;
+		}
+
+	return master;
+}
+EXPORT_SYMBOL_GPL(ssip_slave_get_master);
+
+int ssip_slave_start_tx(struct hsi_client *master)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(master);
+
+	dev_dbg(&master->device, "start TX %d\n", atomic_read(&ssi->tx_usecnt));
+	spin_lock_bh(&ssi->lock);
+	if (ssi->send_state == SEND_IDLE) {
+		ssip_set_txstate(ssi, WAIT4READY);
+		hsi_start_tx(master);
+	}
+	spin_unlock_bh(&ssi->lock);
+	atomic_inc(&ssi->tx_usecnt);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ssip_slave_start_tx);
+
+int ssip_slave_stop_tx(struct hsi_client *master)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(master);
+
+	WARN_ON_ONCE(atomic_read(&ssi->tx_usecnt) == 0);
+
+	if (atomic_dec_and_test(&ssi->tx_usecnt)) {
+		spin_lock_bh(&ssi->lock);
+		if ((ssi->send_state == SEND_READY) ||
+			(ssi->send_state == WAIT4READY)) {
+			ssip_set_txstate(ssi, SEND_IDLE);
+			hsi_stop_tx(master);
+		}
+		spin_unlock_bh(&ssi->lock);
+	}
+	dev_dbg(&master->device, "stop TX %d\n", atomic_read(&ssi->tx_usecnt));
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ssip_slave_stop_tx);
+
+int ssip_slave_running(struct hsi_client *master)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(master);
+	return netif_running(ssi->netdev);
+}
+EXPORT_SYMBOL_GPL(ssip_slave_running);
+
+static void ssip_reset(struct hsi_client *cl)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+	struct list_head *head, *tmp;
+	struct hsi_msg *msg;
+
+	if (netif_running(ssi->netdev))
+		netif_carrier_off(ssi->netdev);
+	hsi_flush(cl);
+	spin_lock_bh(&ssi->lock);
+	if (ssi->send_state != SEND_IDLE)
+		hsi_stop_tx(cl);
+	if (ssi->waketest)
+		ssi_waketest(cl, 0);
+	del_timer(&ssi->rx_wd);
+	del_timer(&ssi->tx_wd);
+	del_timer(&ssi->keep_alive);
+	ssi->main_state = 0;
+	ssi->send_state = 0;
+	ssi->recv_state = 0;
+	ssi->waketest = 0;
+	ssi->rxid = 0;
+	ssi->txid = 0;
+	list_for_each_safe(head, tmp, &ssi->txqueue) {
+		msg = list_entry(head, struct hsi_msg, link);
+		dev_dbg(&cl->device, "Pending TX data\n");
+		list_del(head);
+		ssip_free_data(msg);
+	}
+	ssi->txqueue_len = 0;
+	spin_unlock_bh(&ssi->lock);
+}
+
+static void ssip_dump_state(struct hsi_client *cl)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+	struct hsi_msg *msg;
+
+	spin_lock_bh(&ssi->lock);
+	dev_err(&cl->device, "Main state: %d\n", ssi->main_state);
+	dev_err(&cl->device, "Recv state: %d\n", ssi->recv_state);
+	dev_err(&cl->device, "Send state: %d\n", ssi->send_state);
+	dev_err(&cl->device, "CMT %s\n", (ssi->main_state == ACTIVE) ?
+							"Online" : "Offline");
+	dev_err(&cl->device, "Wake test %d\n", ssi->waketest);
+	dev_err(&cl->device, "Data RX id: %d\n", ssi->rxid);
+	dev_err(&cl->device, "Data TX id: %d\n", ssi->txid);
+
+	list_for_each_entry(msg, &ssi->txqueue, link)
+		dev_err(&cl->device, "pending TX data (%p)\n", msg);
+	spin_unlock_bh(&ssi->lock);
+}
+
+static void ssip_error(struct hsi_client *cl)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+	struct hsi_msg *msg;
+
+	ssip_dump_state(cl);
+	ssip_reset(cl);
+	msg = ssip_claim_cmd(ssi);
+	msg->complete = ssip_rxcmd_complete;
+	hsi_async_read(cl, msg);
+}
+
+static void ssip_keep_alive(unsigned long data)
+{
+	struct hsi_client *cl = (struct hsi_client *)data;
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+
+	dev_dbg(&cl->device, "Keep alive kick in: m(%d) r(%d) s(%d)\n",
+		ssi->main_state, ssi->recv_state, ssi->send_state);
+
+	spin_lock(&ssi->lock);
+	if (ssi->recv_state == RECV_IDLE)
+		switch (ssi->send_state) {
+		case SEND_READY:
+			if (atomic_read(&ssi->tx_usecnt) == 0)
+				break;
+			/*
+			 * Fall through. Workaround for cmt-speech
+			 * in that case we relay on audio timers.
+			 */
+		case SEND_IDLE:
+			spin_unlock(&ssi->lock);
+			return;
+		}
+	mod_timer(&ssi->keep_alive, jiffies + msecs_to_jiffies(SSIP_KATOUT));
+	spin_unlock(&ssi->lock);
+}
+
+static void ssip_wd(unsigned long data)
+{
+	struct hsi_client *cl = (struct hsi_client *)data;
+
+	dev_err(&cl->device, "Watchdog trigerred\n");
+	ssip_error(cl);
+}
+
+static void ssip_send_bootinfo_req_cmd(struct hsi_client *cl)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+	struct hsi_msg *msg;
+
+	dev_dbg(&cl->device, "Issuing BOOT INFO REQ command\n");
+	msg = ssip_claim_cmd(ssi);
+	ssip_set_cmd(msg, SSIP_BOOTINFO_REQ_CMD(SSIP_LOCAL_VERID));
+	msg->complete = ssip_release_cmd;
+	hsi_async_write(cl, msg);
+	dev_dbg(&cl->device, "Issuing RX command\n");
+	msg = ssip_claim_cmd(ssi);
+	msg->complete = ssip_rxcmd_complete;
+	hsi_async_read(cl, msg);
+}
+
+static void ssip_start_rx(struct hsi_client *cl)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+	struct hsi_msg *msg;
+
+	dev_dbg(&cl->device, "RX start M(%d) R(%d)\n", ssi->main_state,
+						ssi->recv_state);
+	spin_lock(&ssi->lock);
+	/*
+	 * We can have two UP events in a row due to a short low
+	 * high transition. Therefore we need to ignore the sencond UP event.
+	 */
+	if ((ssi->main_state != ACTIVE) || (ssi->recv_state == RECV_READY)) {
+		if (ssi->main_state == INIT) {
+			ssi->main_state = HANDSHAKE;
+			spin_unlock(&ssi->lock);
+			ssip_send_bootinfo_req_cmd(cl);
+		} else {
+			spin_unlock(&ssi->lock);
+		}
+		return;
+	}
+	ssip_set_rxstate(ssi, RECV_READY);
+	spin_unlock(&ssi->lock);
+
+	msg = ssip_claim_cmd(ssi);
+	ssip_set_cmd(msg, SSIP_READY_CMD);
+	msg->complete = ssip_release_cmd;
+	dev_dbg(&cl->device, "Send READY\n");
+	hsi_async_write(cl, msg);
+}
+
+static void ssip_stop_rx(struct hsi_client *cl)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+
+	dev_dbg(&cl->device, "RX stop M(%d)\n", ssi->main_state);
+	spin_lock(&ssi->lock);
+	if (likely(ssi->main_state == ACTIVE))
+		ssip_set_rxstate(ssi, RECV_IDLE);
+	spin_unlock(&ssi->lock);
+}
+
+static void ssip_free_strans(struct hsi_msg *msg)
+{
+	ssip_free_data(msg->context);
+	ssip_release_cmd(msg);
+}
+
+static void ssip_strans_complete(struct hsi_msg *msg)
+{
+	struct hsi_client *cl = msg->cl;
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+	struct hsi_msg *data;
+
+	data = msg->context;
+	ssip_release_cmd(msg);
+	spin_lock(&ssi->lock);
+	ssip_set_txstate(ssi, SENDING);
+	spin_unlock(&ssi->lock);
+	hsi_async_write(cl, data);
+}
+
+static int ssip_xmit(struct hsi_client *cl)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+	struct hsi_msg *msg, *dmsg;
+	struct sk_buff *skb;
+
+	spin_lock_bh(&ssi->lock);
+	if (list_empty(&ssi->txqueue)) {
+		spin_unlock_bh(&ssi->lock);
+		return 0;
+	}
+	dmsg = list_first_entry(&ssi->txqueue, struct hsi_msg, link);
+	list_del(&dmsg->link);
+	ssi->txqueue_len--;
+	spin_unlock_bh(&ssi->lock);
+
+	msg = ssip_claim_cmd(ssi);
+	skb = dmsg->context;
+	msg->context = dmsg;
+	msg->complete = ssip_strans_complete;
+	msg->destructor = ssip_free_strans;
+
+	spin_lock_bh(&ssi->lock);
+	ssip_set_cmd(msg, SSIP_START_TRANS_CMD(SSIP_BYTES_TO_FRAMES(skb->len),
+								ssi->txid));
+	ssi->txid++;
+	ssip_set_txstate(ssi, SENDING);
+	spin_unlock_bh(&ssi->lock);
+
+	dev_dbg(&cl->device, "Send STRANS (%d frames)\n",
+						SSIP_BYTES_TO_FRAMES(skb->len));
+
+	return hsi_async_write(cl, msg);
+}
+
+/* In soft IRQ context */
+static void ssip_pn_rx(struct sk_buff *skb)
+{
+	struct net_device *dev = skb->dev;
+
+	if (unlikely(!netif_running(dev))) {
+		dev_dbg(&dev->dev, "Drop RX packet\n");
+		dev->stats.rx_dropped++;
+		dev_kfree_skb(skb);
+		return;
+	}
+	if (unlikely(!pskb_may_pull(skb, SSIP_MIN_PN_HDR))) {
+		dev_dbg(&dev->dev, "Error drop RX packet\n");
+		dev->stats.rx_errors++;
+		dev->stats.rx_length_errors++;
+		dev_kfree_skb(skb);
+		return;
+	}
+	dev->stats.rx_packets++;
+	dev->stats.rx_bytes += skb->len;
+
+	/* length field is exchanged in network byte order */
+	((u16 *)skb->data)[2] = ntohs(((u16 *)skb->data)[2]);
+	dev_dbg(&dev->dev, "RX length fixed (%04x -> %u)\n",
+			((u16 *)skb->data)[2], ntohs(((u16 *)skb->data)[2]));
+
+	skb->protocol = htons(ETH_P_PHONET);
+	skb_reset_mac_header(skb);
+	__skb_pull(skb, 1);
+	netif_rx(skb);
+}
+
+static void ssip_rx_data_complete(struct hsi_msg *msg)
+{
+	struct hsi_client *cl = msg->cl;
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+	struct sk_buff *skb;
+
+	if (msg->status == HSI_STATUS_ERROR) {
+		dev_err(&cl->device, "RX data error\n");
+		ssip_free_data(msg);
+		ssip_error(cl);
+		return;
+	}
+	del_timer(&ssi->rx_wd); /* FIXME: Revisit */
+	skb = msg->context;
+	ssip_pn_rx(skb);
+	hsi_free_msg(msg);
+}
+
+static void ssip_rx_bootinforeq(struct hsi_client *cl, u32 cmd)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+	struct hsi_msg *msg;
+
+	/* Workaroud: Ignore CMT Loader message leftover */
+	if (cmd == SSIP_CMT_LOADER_SYNC)
+		return;
+
+	switch (ssi->main_state) {
+	case ACTIVE:
+		dev_err(&cl->device, "Boot info req on active state\n");
+		ssip_error(cl);
+		/* Fall through */
+	case INIT:
+		spin_lock(&ssi->lock);
+		ssi->main_state = HANDSHAKE;
+		if (!ssi->waketest) {
+			ssi->waketest = 1;
+			ssi_waketest(cl, 1); /* FIXME: To be removed */
+		}
+		/* Start boot handshake watchdog */
+		mod_timer(&ssi->tx_wd, jiffies + msecs_to_jiffies(SSIP_WDTOUT));
+		spin_unlock(&ssi->lock);
+		dev_dbg(&cl->device, "Send BOOTINFO_RESP\n");
+		if (SSIP_DATA_VERSION(cmd) != SSIP_LOCAL_VERID)
+			dev_warn(&cl->device, "boot info req verid mismatch\n");
+		msg = ssip_claim_cmd(ssi);
+		ssip_set_cmd(msg, SSIP_BOOTINFO_RESP_CMD(SSIP_LOCAL_VERID));
+		msg->complete = ssip_release_cmd;
+		hsi_async_write(cl, msg);
+		break;
+	case HANDSHAKE:
+		/* Ignore */
+		break;
+	default:
+		dev_dbg(&cl->device, "Wrong state M(%d)\n", ssi->main_state);
+		break;
+	}
+}
+
+static void ssip_rx_bootinforesp(struct hsi_client *cl, u32 cmd)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+
+	if (SSIP_DATA_VERSION(cmd) != SSIP_LOCAL_VERID)
+		dev_warn(&cl->device, "boot info resp verid mismatch\n");
+
+	spin_lock(&ssi->lock);
+	if (ssi->main_state != ACTIVE)
+		/* Use tx_wd as a boot watchdog in non ACTIVE state */
+		mod_timer(&ssi->tx_wd, jiffies + msecs_to_jiffies(SSIP_WDTOUT));
+	else
+		dev_dbg(&cl->device, "boot info resp ignored M(%d)\n",
+							ssi->main_state);
+	spin_unlock(&ssi->lock);
+}
+
+static void ssip_rx_waketest(struct hsi_client *cl, u32 cmd)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+	unsigned int wkres = SSIP_PAYLOAD(cmd);
+
+	spin_lock(&ssi->lock);
+	if (ssi->main_state != HANDSHAKE) {
+		dev_dbg(&cl->device, "wake lines test ignored M(%d)\n",
+							ssi->main_state);
+		spin_unlock(&ssi->lock);
+		return;
+	}
+	if (ssi->waketest) {
+		ssi->waketest = 0;
+		ssi_waketest(cl, 0); /* FIXME: To be removed */
+	}
+	ssi->main_state = ACTIVE;
+	del_timer(&ssi->tx_wd); /* Stop boot handshake timer */
+	spin_unlock(&ssi->lock);
+
+	dev_notice(&cl->device, "WAKELINES TEST %s\n",
+				wkres & SSIP_WAKETEST_FAILED ? "FAILED" : "OK");
+	if (wkres & SSIP_WAKETEST_FAILED) {
+		ssip_error(cl);
+		return;
+	}
+	dev_dbg(&cl->device, "CMT is ONLINE\n");
+	netif_wake_queue(ssi->netdev);
+	netif_carrier_on(ssi->netdev);
+}
+
+static void ssip_rx_ready(struct hsi_client *cl)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+
+	spin_lock(&ssi->lock);
+	if (unlikely(ssi->main_state != ACTIVE)) {
+		dev_dbg(&cl->device, "READY on wrong state: S(%d) M(%d)\n",
+					ssi->send_state, ssi->main_state);
+		spin_unlock(&ssi->lock);
+		return;
+	}
+	if (ssi->send_state != WAIT4READY) {
+		dev_dbg(&cl->device, "Ignore spurious READY command\n");
+		spin_unlock(&ssi->lock);
+		return;
+	}
+	ssip_set_txstate(ssi, SEND_READY);
+	spin_unlock(&ssi->lock);
+	ssip_xmit(cl);
+}
+
+static void ssip_rx_strans(struct hsi_client *cl, u32 cmd)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+	struct sk_buff *skb;
+	struct hsi_msg *msg;
+	int len = SSIP_PDU_LENGTH(cmd);
+
+	dev_dbg(&cl->device, "RX strans: %d frames\n", len);
+	spin_lock(&ssi->lock);
+	if (unlikely(ssi->main_state != ACTIVE)) {
+		dev_err(&cl->device, "START TRANS wrong state: S(%d) M(%d)\n",
+					ssi->send_state, ssi->main_state);
+		spin_unlock(&ssi->lock);
+		return;
+	}
+	ssip_set_rxstate(ssi, RECEIVING);
+	if (unlikely(SSIP_MSG_ID(cmd) != ssi->rxid)) {
+		dev_err(&cl->device, "START TRANS id %d expeceted %d\n",
+					SSIP_MSG_ID(cmd), ssi->rxid);
+		spin_unlock(&ssi->lock);
+		goto out1;
+	}
+	ssi->rxid++;
+	spin_unlock(&ssi->lock);
+	skb = netdev_alloc_skb(ssi->netdev, len * 4);
+	if (unlikely(!skb)) {
+		dev_err(&cl->device, "No memory for rx skb\n");
+		goto out1;
+	}
+	skb->dev = ssi->netdev;
+	skb_put(skb, len * 4);
+	msg = ssip_alloc_data(ssi, skb, GFP_ATOMIC);
+	if (unlikely(!msg)) {
+		dev_err(&cl->device, "No memory for RX data msg\n");
+		goto out2;
+	}
+	msg->complete = ssip_rx_data_complete;
+	hsi_async_read(cl, msg);
+
+	return;
+out2:
+	dev_kfree_skb(skb);
+out1:
+	ssip_error(cl);
+}
+
+static void ssip_rxcmd_complete(struct hsi_msg *msg)
+{
+	struct hsi_client *cl = msg->cl;
+	u32 cmd = ssip_get_cmd(msg);
+	unsigned int cmdid = SSIP_COMMAND(cmd);
+
+	if (msg->status == HSI_STATUS_ERROR) {
+		dev_err(&cl->device, "RX error detected\n");
+		ssip_release_cmd(msg);
+		ssip_error(cl);
+		return;
+	}
+	hsi_async_read(cl, msg);
+	dev_dbg(&cl->device, "RX cmd: 0x%08x\n", cmd);
+	switch (cmdid) {
+	case SSIP_SW_BREAK:
+		/* Ignored */
+		break;
+	case SSIP_BOOTINFO_REQ:
+		ssip_rx_bootinforeq(cl, cmd);
+		break;
+	case SSIP_BOOTINFO_RESP:
+		ssip_rx_bootinforesp(cl, cmd);
+		break;
+	case SSIP_WAKETEST_RESULT:
+		ssip_rx_waketest(cl, cmd);
+		break;
+	case SSIP_START_TRANS:
+		ssip_rx_strans(cl, cmd);
+		break;
+	case SSIP_READY:
+		ssip_rx_ready(cl);
+		break;
+	default:
+		dev_warn(&cl->device, "command 0x%08x not supported\n", cmd);
+		break;
+	}
+}
+
+static void ssip_swbreak_complete(struct hsi_msg *msg)
+{
+	struct hsi_client *cl = msg->cl;
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+
+	ssip_release_cmd(msg);
+	spin_lock(&ssi->lock);
+	if (list_empty(&ssi->txqueue)) {
+		if (atomic_read(&ssi->tx_usecnt)) {
+			ssip_set_txstate(ssi, SEND_READY);
+		} else {
+			ssip_set_txstate(ssi, SEND_IDLE);
+			hsi_stop_tx(cl);
+		}
+		spin_unlock(&ssi->lock);
+	} else {
+		spin_unlock(&ssi->lock);
+		ssip_xmit(cl);
+	}
+	netif_wake_queue(ssi->netdev);
+}
+
+static void ssip_tx_data_complete(struct hsi_msg *msg)
+{
+	struct hsi_client *cl = msg->cl;
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+	struct hsi_msg *cmsg;
+
+	if (msg->status == HSI_STATUS_ERROR) {
+		dev_err(&cl->device, "TX data error\n");
+		ssip_error(cl);
+		goto out;
+	}
+	spin_lock(&ssi->lock);
+	if (list_empty(&ssi->txqueue)) {
+		ssip_set_txstate(ssi, SENDING_SWBREAK);
+		spin_unlock(&ssi->lock);
+		cmsg = ssip_claim_cmd(ssi);
+		ssip_set_cmd(cmsg, SSIP_SWBREAK_CMD);
+		cmsg->complete = ssip_swbreak_complete;
+		dev_dbg(&cl->device, "Send SWBREAK\n");
+		hsi_async_write(cl, cmsg);
+	} else {
+		spin_unlock(&ssi->lock);
+		ssip_xmit(cl);
+	}
+out:
+	ssip_free_data(msg);
+}
+
+void ssip_port_event(struct hsi_client *cl, unsigned long event)
+{
+	switch (event) {
+	case HSI_EVENT_START_RX:
+		ssip_start_rx(cl);
+		break;
+	case HSI_EVENT_STOP_RX:
+		ssip_stop_rx(cl);
+		break;
+	default:
+		return;
+	}
+}
+
+static int ssip_pn_open(struct net_device *dev)
+{
+	struct hsi_client *cl = to_hsi_client(dev->dev.parent);
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+	int err;
+
+	err = hsi_claim_port(cl, 1);
+	if (err < 0) {
+		dev_err(&cl->device, "SSI port already claimed\n");
+		return err;
+	}
+	err = hsi_register_port_event(cl, ssip_port_event);
+	if (err < 0) {
+		dev_err(&cl->device, "Register HSI port event failed (%d)\n",
+			err);
+		return err;
+	}
+	dev_dbg(&cl->device, "Configuring SSI port\n");
+	hsi_setup(cl);
+	spin_lock_bh(&ssi->lock);
+	if (!ssi->waketest) {
+		ssi->waketest = 1;
+		ssi_waketest(cl, 1); /* FIXME: To be removed */
+	}
+	ssi->main_state = INIT;
+	spin_unlock_bh(&ssi->lock);
+
+	return 0;
+}
+
+static int ssip_pn_stop(struct net_device *dev)
+{
+	struct hsi_client *cl = to_hsi_client(dev->dev.parent);
+
+	ssip_reset(cl);
+	hsi_unregister_port_event(cl);
+	hsi_release_port(cl);
+
+	return 0;
+}
+
+static int ssip_pn_set_mtu(struct net_device *dev, int new_mtu)
+{
+	if (new_mtu > SSIP_MAX_MTU || new_mtu < PHONET_MIN_MTU)
+		return -EINVAL;
+	dev->mtu = new_mtu;
+
+	return 0;
+}
+
+static int ssip_pn_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct hsi_client *cl = to_hsi_client(dev->dev.parent);
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+	struct hsi_msg *msg;
+
+	if ((skb->protocol != htons(ETH_P_PHONET)) ||
+					(skb->len < SSIP_MIN_PN_HDR))
+		goto drop;
+	/* Pad to 32-bits - FIXME: Revisit*/
+	if ((skb->len & 3) && skb_pad(skb, 4 - (skb->len & 3)))
+		goto drop;
+
+	/*
+	 * Modem sends Phonet messages over SSI with its own endianess...
+	 * Assume that modem has the same endianess as we do.
+	 */
+	if (skb_cow_head(skb, 0))
+		goto drop;
+
+	/* length field is exchanged in network byte order */
+	((u16 *)skb->data)[2] = htons(((u16 *)skb->data)[2]);
+
+	msg = ssip_alloc_data(ssi, skb, GFP_ATOMIC);
+	if (!msg) {
+		dev_dbg(&cl->device, "Dropping tx data: No memory\n");
+		goto drop;
+	}
+	msg->complete = ssip_tx_data_complete;
+
+	spin_lock_bh(&ssi->lock);
+	if (unlikely(ssi->main_state != ACTIVE)) {
+		spin_unlock_bh(&ssi->lock);
+		dev_dbg(&cl->device, "Dropping tx data: CMT is OFFLINE\n");
+		goto drop2;
+	}
+	list_add_tail(&msg->link, &ssi->txqueue);
+	ssi->txqueue_len++;
+	if (dev->tx_queue_len < ssi->txqueue_len) {
+		dev_info(&cl->device, "TX queue full %d\n", ssi->txqueue_len);
+		netif_stop_queue(dev);
+	}
+	if (ssi->send_state == SEND_IDLE) {
+		ssip_set_txstate(ssi, WAIT4READY);
+		spin_unlock_bh(&ssi->lock);
+		dev_dbg(&cl->device, "Start TX qlen %d\n", ssi->txqueue_len);
+		hsi_start_tx(cl);
+	} else if (ssi->send_state == SEND_READY) {
+		/* Needed for cmt-speech workaround */
+		dev_dbg(&cl->device, "Start TX on SEND READY qlen %d\n",
+							ssi->txqueue_len);
+		spin_unlock_bh(&ssi->lock);
+		ssip_xmit(cl);
+	} else {
+		spin_unlock_bh(&ssi->lock);
+	}
+	dev->stats.tx_packets++;
+	dev->stats.tx_bytes += skb->len;
+
+	return 0;
+drop2:
+	hsi_free_msg(msg);
+drop:
+	dev->stats.tx_dropped++;
+	dev_kfree_skb(skb);
+
+	return 0;
+}
+
+/* CMT reset event handler */
+void ssip_reset_event(struct hsi_client *master)
+{
+	struct ssi_protocol *ssi = hsi_client_drvdata(master);
+	dev_err(&ssi->cl->device, "CMT reset detected!\n");
+	ssip_error(ssi->cl);
+}
+EXPORT_SYMBOL_GPL(ssip_reset_event);
+
+static const struct net_device_ops ssip_pn_ops = {
+	.ndo_open	= ssip_pn_open,
+	.ndo_stop	= ssip_pn_stop,
+	.ndo_start_xmit	= ssip_pn_xmit,
+	.ndo_change_mtu	= ssip_pn_set_mtu,
+};
+
+static void ssip_pn_setup(struct net_device *dev)
+{
+	dev->features		= 0;
+	dev->netdev_ops		= &ssip_pn_ops;
+	dev->type		= ARPHRD_PHONET;
+	dev->flags		= IFF_POINTOPOINT | IFF_NOARP;
+	dev->mtu		= SSIP_DEFAULT_MTU;
+	dev->hard_header_len	= 1;
+	dev->dev_addr[0]	= PN_MEDIA_SOS;
+	dev->addr_len		= 1;
+	dev->tx_queue_len	= SSIP_TXQUEUE_LEN;
+
+	dev->destructor		= free_netdev;
+	dev->header_ops		= &phonet_header_ops;
+}
+
+static int ssi_protocol_probe(struct device *dev)
+{
+	static const char ifname[] = "phonet%d";
+	struct hsi_client *cl = to_hsi_client(dev);
+	struct ssi_protocol *ssi;
+	int err;
+
+	ssi = kzalloc(sizeof(*ssi), GFP_KERNEL);
+	if (!ssi) {
+		dev_err(dev, "No memory for ssi protocol\n");
+		return -ENOMEM;
+	}
+
+	spin_lock_init(&ssi->lock);
+	init_timer_deferrable(&ssi->rx_wd);
+	init_timer_deferrable(&ssi->tx_wd);
+	init_timer(&ssi->keep_alive);
+	ssi->rx_wd.data = (unsigned long)cl;
+	ssi->rx_wd.function = ssip_wd;
+	ssi->tx_wd.data = (unsigned long)cl;
+	ssi->tx_wd.function = ssip_wd;
+	ssi->keep_alive.data = (unsigned long)cl;
+	ssi->keep_alive.function = ssip_keep_alive;
+	INIT_LIST_HEAD(&ssi->txqueue);
+	INIT_LIST_HEAD(&ssi->cmdqueue);
+	atomic_set(&ssi->tx_usecnt, 0);
+	hsi_client_set_drvdata(cl, ssi);
+	ssi->cl = cl;
+
+	ssi->channel_id_cmd = hsi_get_channel_id_by_name(cl, "mcsaab-control");
+	if (ssi->channel_id_cmd < 0) {
+		err = ssi->channel_id_cmd;
+		dev_err(dev, "Could not get cmd channel (%d)\n", err);
+		goto out;
+	}
+
+	ssi->channel_id_data = hsi_get_channel_id_by_name(cl, "mcsaab-data");
+	if (ssi->channel_id_data < 0) {
+		err = ssi->channel_id_data;
+		dev_err(dev, "Could not get data channel (%d)\n", err);
+		goto out;
+	}
+
+	err = ssip_alloc_cmds(ssi);
+	if (err < 0) {
+		dev_err(dev, "No memory for commands\n");
+		goto out;
+	}
+
+	ssi->netdev = alloc_netdev(0, ifname, ssip_pn_setup);
+	if (!ssi->netdev) {
+		dev_err(dev, "No memory for netdev\n");
+		err = -ENOMEM;
+		goto out1;
+	}
+
+	SET_NETDEV_DEV(ssi->netdev, dev);
+	netif_carrier_off(ssi->netdev);
+	err = register_netdev(ssi->netdev);
+	if (err < 0) {
+		dev_err(dev, "Register netdev failed (%d)\n", err);
+		goto out2;
+	}
+
+	list_add(&ssi->link, &ssip_list);
+
+	dev_dbg(dev, "channel configuration: cmd=%d, data=%d\n",
+		ssi->channel_id_cmd, ssi->channel_id_data);
+
+	return 0;
+out2:
+	free_netdev(ssi->netdev);
+out1:
+	ssip_free_cmds(ssi);
+out:
+	kfree(ssi);
+
+	return err;
+}
+
+static int ssi_protocol_remove(struct device *dev)
+{
+	struct hsi_client *cl = to_hsi_client(dev);
+	struct ssi_protocol *ssi = hsi_client_drvdata(cl);
+
+	list_del(&ssi->link);
+	unregister_netdev(ssi->netdev);
+	ssip_free_cmds(ssi);
+	hsi_client_set_drvdata(cl, NULL);
+	kfree(ssi);
+
+	return 0;
+}
+
+static struct hsi_client_driver ssip_driver = {
+	.driver = {
+		.name	= "ssi-protocol",
+		.owner	= THIS_MODULE,
+		.probe	= ssi_protocol_probe,
+		.remove	= ssi_protocol_remove,
+	},
+};
+
+static int __init ssip_init(void)
+{
+	pr_info("SSI protocol aka McSAAB added\n");
+
+	return hsi_register_client_driver(&ssip_driver);
+}
+module_init(ssip_init);
+
+static void __exit ssip_exit(void)
+{
+	hsi_unregister_client_driver(&ssip_driver);
+	pr_info("SSI protocol driver removed\n");
+}
+module_exit(ssip_exit);
+
+MODULE_ALIAS("hsi:ssi-protocol");
+MODULE_AUTHOR("Carlos Chinea <carlos.chinea@nokia.com>");
+MODULE_AUTHOR("Remi Denis-Courmont <remi.denis-courmont@nokia.com>");
+MODULE_DESCRIPTION("SSI protocol improved aka McSAAB");
+MODULE_LICENSE("GPL");
diff --git a/drivers/hsi/controllers/Kconfig b/drivers/hsi/controllers/Kconfig
new file mode 100644
index 00000000000..6aba2780817
--- /dev/null
+++ b/drivers/hsi/controllers/Kconfig
@@ -0,0 +1,19 @@
+#
+# HSI controllers configuration
+#
+comment "HSI controllers"
+
+config OMAP_SSI
+	tristate "OMAP SSI hardware driver"
+	depends on HSI && OF && (ARCH_OMAP3 || (ARM && COMPILE_TEST))
+	---help---
+	  SSI is a legacy version of HSI. It is usually used to connect
+	  an application engine with a cellular modem.
+	  If you say Y here, you will enable the OMAP SSI hardware driver.
+
+	  If unsure, say N.
+
+config OMAP_SSI_PORT
+	tristate
+	default m if OMAP_SSI=m
+	default y if OMAP_SSI=y
diff --git a/drivers/hsi/controllers/Makefile b/drivers/hsi/controllers/Makefile
new file mode 100644
index 00000000000..d2665cf9c54
--- /dev/null
+++ b/drivers/hsi/controllers/Makefile
@@ -0,0 +1,6 @@
+#
+# Makefile for HSI controllers drivers
+#
+
+obj-$(CONFIG_OMAP_SSI)		+= omap_ssi.o
+obj-$(CONFIG_OMAP_SSI_PORT)	+= omap_ssi_port.o
diff --git a/drivers/hsi/controllers/omap_ssi.c b/drivers/hsi/controllers/omap_ssi.c
new file mode 100644
index 00000000000..0fc7a7fd014
--- /dev/null
+++ b/drivers/hsi/controllers/omap_ssi.c
@@ -0,0 +1,625 @@
+/* OMAP SSI driver.
+ *
+ * Copyright (C) 2010 Nokia Corporation. All rights reserved.
+ * Copyright (C) 2014 Sebastian Reichel <sre@kernel.org>
+ *
+ * Contact: Carlos Chinea <carlos.chinea@nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ */
+
+#include <linux/compiler.h>
+#include <linux/err.h>
+#include <linux/ioport.h>
+#include <linux/io.h>
+#include <linux/gpio.h>
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmaengine.h>
+#include <linux/delay.h>
+#include <linux/seq_file.h>
+#include <linux/scatterlist.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/debugfs.h>
+#include <linux/pm_runtime.h>
+#include <linux/of_platform.h>
+#include <linux/hsi/hsi.h>
+#include <linux/idr.h>
+
+#include "omap_ssi_regs.h"
+#include "omap_ssi.h"
+
+/* For automatically allocated device IDs */
+static DEFINE_IDA(platform_omap_ssi_ida);
+
+#ifdef CONFIG_DEBUG_FS
+static int ssi_debug_show(struct seq_file *m, void *p __maybe_unused)
+{
+	struct hsi_controller *ssi = m->private;
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem *sys = omap_ssi->sys;
+
+	pm_runtime_get_sync(ssi->device.parent);
+	seq_printf(m, "REVISION\t: 0x%08x\n",  readl(sys + SSI_REVISION_REG));
+	seq_printf(m, "SYSCONFIG\t: 0x%08x\n", readl(sys + SSI_SYSCONFIG_REG));
+	seq_printf(m, "SYSSTATUS\t: 0x%08x\n", readl(sys + SSI_SYSSTATUS_REG));
+	pm_runtime_put_sync(ssi->device.parent);
+
+	return 0;
+}
+
+static int ssi_debug_gdd_show(struct seq_file *m, void *p __maybe_unused)
+{
+	struct hsi_controller *ssi = m->private;
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem *gdd = omap_ssi->gdd;
+	void __iomem *sys = omap_ssi->sys;
+	int lch;
+
+	pm_runtime_get_sync(ssi->device.parent);
+
+	seq_printf(m, "GDD_MPU_STATUS\t: 0x%08x\n",
+		readl(sys + SSI_GDD_MPU_IRQ_STATUS_REG));
+	seq_printf(m, "GDD_MPU_ENABLE\t: 0x%08x\n\n",
+		readl(sys + SSI_GDD_MPU_IRQ_ENABLE_REG));
+	seq_printf(m, "HW_ID\t\t: 0x%08x\n",
+				readl(gdd + SSI_GDD_HW_ID_REG));
+	seq_printf(m, "PPORT_ID\t: 0x%08x\n",
+				readl(gdd + SSI_GDD_PPORT_ID_REG));
+	seq_printf(m, "MPORT_ID\t: 0x%08x\n",
+				readl(gdd + SSI_GDD_MPORT_ID_REG));
+	seq_printf(m, "TEST\t\t: 0x%08x\n",
+				readl(gdd + SSI_GDD_TEST_REG));
+	seq_printf(m, "GCR\t\t: 0x%08x\n",
+				readl(gdd + SSI_GDD_GCR_REG));
+
+	for (lch = 0; lch < SSI_MAX_GDD_LCH; lch++) {
+		seq_printf(m, "\nGDD LCH %d\n=========\n", lch);
+		seq_printf(m, "CSDP\t\t: 0x%04x\n",
+				readw(gdd + SSI_GDD_CSDP_REG(lch)));
+		seq_printf(m, "CCR\t\t: 0x%04x\n",
+				readw(gdd + SSI_GDD_CCR_REG(lch)));
+		seq_printf(m, "CICR\t\t: 0x%04x\n",
+				readw(gdd + SSI_GDD_CICR_REG(lch)));
+		seq_printf(m, "CSR\t\t: 0x%04x\n",
+				readw(gdd + SSI_GDD_CSR_REG(lch)));
+		seq_printf(m, "CSSA\t\t: 0x%08x\n",
+				readl(gdd + SSI_GDD_CSSA_REG(lch)));
+		seq_printf(m, "CDSA\t\t: 0x%08x\n",
+				readl(gdd + SSI_GDD_CDSA_REG(lch)));
+		seq_printf(m, "CEN\t\t: 0x%04x\n",
+				readw(gdd + SSI_GDD_CEN_REG(lch)));
+		seq_printf(m, "CSAC\t\t: 0x%04x\n",
+				readw(gdd + SSI_GDD_CSAC_REG(lch)));
+		seq_printf(m, "CDAC\t\t: 0x%04x\n",
+				readw(gdd + SSI_GDD_CDAC_REG(lch)));
+		seq_printf(m, "CLNK_CTRL\t: 0x%04x\n",
+				readw(gdd + SSI_GDD_CLNK_CTRL_REG(lch)));
+	}
+
+	pm_runtime_put_sync(ssi->device.parent);
+
+	return 0;
+}
+
+static int ssi_regs_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ssi_debug_show, inode->i_private);
+}
+
+static int ssi_gdd_regs_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ssi_debug_gdd_show, inode->i_private);
+}
+
+static const struct file_operations ssi_regs_fops = {
+	.open		= ssi_regs_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static const struct file_operations ssi_gdd_regs_fops = {
+	.open		= ssi_gdd_regs_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static int __init ssi_debug_add_ctrl(struct hsi_controller *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct dentry *dir;
+
+	/* SSI controller */
+	omap_ssi->dir = debugfs_create_dir(dev_name(&ssi->device), NULL);
+	if (IS_ERR(omap_ssi->dir))
+		return PTR_ERR(omap_ssi->dir);
+
+	debugfs_create_file("regs", S_IRUGO, omap_ssi->dir, ssi,
+								&ssi_regs_fops);
+	/* SSI GDD (DMA) */
+	dir = debugfs_create_dir("gdd", omap_ssi->dir);
+	if (IS_ERR(dir))
+		goto rback;
+	debugfs_create_file("regs", S_IRUGO, dir, ssi, &ssi_gdd_regs_fops);
+
+	return 0;
+rback:
+	debugfs_remove_recursive(omap_ssi->dir);
+
+	return PTR_ERR(dir);
+}
+
+static void ssi_debug_remove_ctrl(struct hsi_controller *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	debugfs_remove_recursive(omap_ssi->dir);
+}
+#endif /* CONFIG_DEBUG_FS */
+
+/*
+ * FIXME: Horrible HACK needed until we remove the useless wakeline test
+ * in the CMT. To be removed !!!!
+ */
+void ssi_waketest(struct hsi_client *cl, unsigned int enable)
+{
+	struct hsi_port *port = hsi_get_port(cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	omap_port->wktest = !!enable;
+	if (omap_port->wktest) {
+		pm_runtime_get_sync(ssi->device.parent);
+		writel_relaxed(SSI_WAKE(0),
+				omap_ssi->sys + SSI_SET_WAKE_REG(port->num));
+	} else {
+		writel_relaxed(SSI_WAKE(0),
+				omap_ssi->sys +	SSI_CLEAR_WAKE_REG(port->num));
+		pm_runtime_put_sync(ssi->device.parent);
+	}
+}
+EXPORT_SYMBOL_GPL(ssi_waketest);
+
+static void ssi_gdd_complete(struct hsi_controller *ssi, unsigned int lch)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct hsi_msg *msg = omap_ssi->gdd_trn[lch].msg;
+	struct hsi_port *port = to_hsi_port(msg->cl->device.parent);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	unsigned int dir;
+	u32 csr;
+	u32 val;
+
+	spin_lock(&omap_ssi->lock);
+
+	val = readl(omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	val &= ~SSI_GDD_LCH(lch);
+	writel_relaxed(val, omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+
+	if (msg->ttype == HSI_MSG_READ) {
+		dir = DMA_FROM_DEVICE;
+		val = SSI_DATAAVAILABLE(msg->channel);
+		pm_runtime_put_sync(ssi->device.parent);
+	} else {
+		dir = DMA_TO_DEVICE;
+		val = SSI_DATAACCEPT(msg->channel);
+		/* Keep clocks reference for write pio event */
+	}
+	dma_unmap_sg(&ssi->device, msg->sgt.sgl, msg->sgt.nents, dir);
+	csr = readw(omap_ssi->gdd + SSI_GDD_CSR_REG(lch));
+	omap_ssi->gdd_trn[lch].msg = NULL; /* release GDD lch */
+	dev_dbg(&port->device, "DMA completed ch %d ttype %d\n",
+				msg->channel, msg->ttype);
+	spin_unlock(&omap_ssi->lock);
+	if (csr & SSI_CSR_TOUR) { /* Timeout error */
+		msg->status = HSI_STATUS_ERROR;
+		msg->actual_len = 0;
+		spin_lock(&omap_port->lock);
+		list_del(&msg->link); /* Dequeue msg */
+		spin_unlock(&omap_port->lock);
+		msg->complete(msg);
+		return;
+	}
+	spin_lock(&omap_port->lock);
+	val |= readl(omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	writel_relaxed(val, omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	spin_unlock(&omap_port->lock);
+
+	msg->status = HSI_STATUS_COMPLETED;
+	msg->actual_len = sg_dma_len(msg->sgt.sgl);
+}
+
+static void ssi_gdd_tasklet(unsigned long dev)
+{
+	struct hsi_controller *ssi = (struct hsi_controller *)dev;
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem *sys = omap_ssi->sys;
+	unsigned int lch;
+	u32 status_reg;
+
+	pm_runtime_get_sync(ssi->device.parent);
+
+	status_reg = readl(sys + SSI_GDD_MPU_IRQ_STATUS_REG);
+	for (lch = 0; lch < SSI_MAX_GDD_LCH; lch++) {
+		if (status_reg & SSI_GDD_LCH(lch))
+			ssi_gdd_complete(ssi, lch);
+	}
+	writel_relaxed(status_reg, sys + SSI_GDD_MPU_IRQ_STATUS_REG);
+	status_reg = readl(sys + SSI_GDD_MPU_IRQ_STATUS_REG);
+
+	pm_runtime_put_sync(ssi->device.parent);
+
+	if (status_reg)
+		tasklet_hi_schedule(&omap_ssi->gdd_tasklet);
+	else
+		enable_irq(omap_ssi->gdd_irq);
+
+}
+
+static irqreturn_t ssi_gdd_isr(int irq, void *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	tasklet_hi_schedule(&omap_ssi->gdd_tasklet);
+	disable_irq_nosync(irq);
+
+	return IRQ_HANDLED;
+}
+
+static unsigned long ssi_get_clk_rate(struct hsi_controller *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	unsigned long rate = clk_get_rate(omap_ssi->fck);
+	return rate;
+}
+
+static int __init ssi_get_iomem(struct platform_device *pd,
+		const char *name, void __iomem **pbase, dma_addr_t *phy)
+{
+	struct resource *mem;
+	struct resource *ioarea;
+	void __iomem *base;
+	struct hsi_controller *ssi = platform_get_drvdata(pd);
+
+	mem = platform_get_resource_byname(pd, IORESOURCE_MEM, name);
+	if (!mem) {
+		dev_err(&pd->dev, "IO memory region missing (%s)\n", name);
+		return -ENXIO;
+	}
+	ioarea = devm_request_mem_region(&ssi->device, mem->start,
+					resource_size(mem), dev_name(&pd->dev));
+	if (!ioarea) {
+		dev_err(&pd->dev, "%s IO memory region request failed\n",
+								mem->name);
+		return -ENXIO;
+	}
+	base = devm_ioremap(&ssi->device, mem->start, resource_size(mem));
+	if (!base) {
+		dev_err(&pd->dev, "%s IO remap failed\n", mem->name);
+		return -ENXIO;
+	}
+	*pbase = base;
+
+	if (phy)
+		*phy = mem->start;
+
+	return 0;
+}
+
+static int __init ssi_add_controller(struct hsi_controller *ssi,
+						struct platform_device *pd)
+{
+	struct omap_ssi_controller *omap_ssi;
+	int err;
+
+	omap_ssi = devm_kzalloc(&ssi->device, sizeof(*omap_ssi), GFP_KERNEL);
+	if (!omap_ssi) {
+		dev_err(&pd->dev, "not enough memory for omap ssi\n");
+		return -ENOMEM;
+	}
+
+	ssi->id = ida_simple_get(&platform_omap_ssi_ida, 0, 0, GFP_KERNEL);
+	if (ssi->id < 0) {
+		err = ssi->id;
+		goto out_err;
+	}
+
+	ssi->owner = THIS_MODULE;
+	ssi->device.parent = &pd->dev;
+	dev_set_name(&ssi->device, "ssi%d", ssi->id);
+	hsi_controller_set_drvdata(ssi, omap_ssi);
+	omap_ssi->dev = &ssi->device;
+	err = ssi_get_iomem(pd, "sys", &omap_ssi->sys, NULL);
+	if (err < 0)
+		goto out_err;
+	err = ssi_get_iomem(pd, "gdd", &omap_ssi->gdd, NULL);
+	if (err < 0)
+		goto out_err;
+	omap_ssi->gdd_irq = platform_get_irq_byname(pd, "gdd_mpu");
+	if (omap_ssi->gdd_irq < 0) {
+		dev_err(&pd->dev, "GDD IRQ resource missing\n");
+		err = omap_ssi->gdd_irq;
+		goto out_err;
+	}
+	tasklet_init(&omap_ssi->gdd_tasklet, ssi_gdd_tasklet,
+							(unsigned long)ssi);
+	err = devm_request_irq(&ssi->device, omap_ssi->gdd_irq, ssi_gdd_isr,
+						0, "gdd_mpu", ssi);
+	if (err < 0) {
+		dev_err(&ssi->device, "Request GDD IRQ %d failed (%d)",
+							omap_ssi->gdd_irq, err);
+		goto out_err;
+	}
+
+	omap_ssi->port = devm_kzalloc(&ssi->device,
+		sizeof(struct omap_ssi_port *) * ssi->num_ports, GFP_KERNEL);
+	if (!omap_ssi->port) {
+		err = -ENOMEM;
+		goto out_err;
+	}
+
+	omap_ssi->fck = devm_clk_get(&ssi->device, "ssi_ssr_fck");
+	if (IS_ERR(omap_ssi->fck)) {
+		dev_err(&pd->dev, "Could not acquire clock \"ssi_ssr_fck\": %li\n",
+			PTR_ERR(omap_ssi->fck));
+		err = -ENODEV;
+		goto out_err;
+	}
+
+	/* TODO: find register, which can be used to detect context loss */
+	omap_ssi->get_loss = NULL;
+
+	omap_ssi->max_speed = UINT_MAX;
+	spin_lock_init(&omap_ssi->lock);
+	err = hsi_register_controller(ssi);
+
+	if (err < 0)
+		goto out_err;
+
+	return 0;
+
+out_err:
+	ida_simple_remove(&platform_omap_ssi_ida, ssi->id);
+	return err;
+}
+
+static int __init ssi_hw_init(struct hsi_controller *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	unsigned int i;
+	u32 val;
+	int err;
+
+	err = pm_runtime_get_sync(ssi->device.parent);
+	if (err < 0) {
+		dev_err(&ssi->device, "runtime PM failed %d\n", err);
+		return err;
+	}
+	/* Reseting SSI controller */
+	writel_relaxed(SSI_SOFTRESET, omap_ssi->sys + SSI_SYSCONFIG_REG);
+	val = readl(omap_ssi->sys + SSI_SYSSTATUS_REG);
+	for (i = 0; ((i < 20) && !(val & SSI_RESETDONE)); i++) {
+		msleep(20);
+		val = readl(omap_ssi->sys + SSI_SYSSTATUS_REG);
+	}
+	if (!(val & SSI_RESETDONE)) {
+		dev_err(&ssi->device, "SSI HW reset failed\n");
+		pm_runtime_put_sync(ssi->device.parent);
+		return -EIO;
+	}
+	/* Reseting GDD */
+	writel_relaxed(SSI_SWRESET, omap_ssi->gdd + SSI_GDD_GRST_REG);
+	/* Get FCK rate in KHz */
+	omap_ssi->fck_rate = DIV_ROUND_CLOSEST(ssi_get_clk_rate(ssi), 1000);
+	dev_dbg(&ssi->device, "SSI fck rate %lu KHz\n", omap_ssi->fck_rate);
+	/* Set default PM settings */
+	val = SSI_AUTOIDLE | SSI_SIDLEMODE_SMART | SSI_MIDLEMODE_SMART;
+	writel_relaxed(val, omap_ssi->sys + SSI_SYSCONFIG_REG);
+	omap_ssi->sysconfig = val;
+	writel_relaxed(SSI_CLK_AUTOGATING_ON, omap_ssi->sys + SSI_GDD_GCR_REG);
+	omap_ssi->gdd_gcr = SSI_CLK_AUTOGATING_ON;
+	pm_runtime_put_sync(ssi->device.parent);
+
+	return 0;
+}
+
+static void ssi_remove_controller(struct hsi_controller *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	int id = ssi->id;
+	tasklet_kill(&omap_ssi->gdd_tasklet);
+	hsi_unregister_controller(ssi);
+	ida_simple_remove(&platform_omap_ssi_ida, id);
+}
+
+static inline int ssi_of_get_available_ports_count(const struct device_node *np)
+{
+	struct device_node *child;
+	int num = 0;
+
+	for_each_available_child_of_node(np, child)
+		if (of_device_is_compatible(child, "ti,omap3-ssi-port"))
+			num++;
+
+	return num;
+}
+
+static int ssi_remove_ports(struct device *dev, void *c)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+
+	of_device_unregister(pdev);
+
+	return 0;
+}
+
+static int __init ssi_probe(struct platform_device *pd)
+{
+	struct platform_device *childpdev;
+	struct device_node *np = pd->dev.of_node;
+	struct device_node *child;
+	struct hsi_controller *ssi;
+	int err;
+	int num_ports;
+
+	if (!np) {
+		dev_err(&pd->dev, "missing device tree data\n");
+		return -EINVAL;
+	}
+
+	num_ports = ssi_of_get_available_ports_count(np);
+
+	ssi = hsi_alloc_controller(num_ports, GFP_KERNEL);
+	if (!ssi) {
+		dev_err(&pd->dev, "No memory for controller\n");
+		return -ENOMEM;
+	}
+
+	platform_set_drvdata(pd, ssi);
+
+	err = ssi_add_controller(ssi, pd);
+	if (err < 0)
+		goto out1;
+
+	pm_runtime_irq_safe(&pd->dev);
+	pm_runtime_enable(&pd->dev);
+
+	err = ssi_hw_init(ssi);
+	if (err < 0)
+		goto out2;
+#ifdef CONFIG_DEBUG_FS
+	err = ssi_debug_add_ctrl(ssi);
+	if (err < 0)
+		goto out2;
+#endif
+
+	for_each_available_child_of_node(np, child) {
+		if (!of_device_is_compatible(child, "ti,omap3-ssi-port"))
+			continue;
+
+		childpdev = of_platform_device_create(child, NULL, &pd->dev);
+		if (!childpdev) {
+			err = -ENODEV;
+			dev_err(&pd->dev, "failed to create ssi controller port\n");
+			goto out3;
+		}
+	}
+
+	dev_info(&pd->dev, "ssi controller %d initialized (%d ports)!\n",
+		ssi->id, num_ports);
+	return err;
+out3:
+	device_for_each_child(&pd->dev, NULL, ssi_remove_ports);
+out2:
+	ssi_remove_controller(ssi);
+out1:
+	platform_set_drvdata(pd, NULL);
+	pm_runtime_disable(&pd->dev);
+
+	return err;
+}
+
+static int __exit ssi_remove(struct platform_device *pd)
+{
+	struct hsi_controller *ssi = platform_get_drvdata(pd);
+
+#ifdef CONFIG_DEBUG_FS
+	ssi_debug_remove_ctrl(ssi);
+#endif
+	ssi_remove_controller(ssi);
+	platform_set_drvdata(pd, NULL);
+
+	pm_runtime_disable(&pd->dev);
+
+	/* cleanup of of_platform_populate() call */
+	device_for_each_child(&pd->dev, NULL, ssi_remove_ports);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_RUNTIME
+static int omap_ssi_runtime_suspend(struct device *dev)
+{
+	struct hsi_controller *ssi = dev_get_drvdata(dev);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	dev_dbg(dev, "runtime suspend!\n");
+
+	if (omap_ssi->get_loss)
+		omap_ssi->loss_count =
+				omap_ssi->get_loss(ssi->device.parent);
+
+	return 0;
+}
+
+static int omap_ssi_runtime_resume(struct device *dev)
+{
+	struct hsi_controller *ssi = dev_get_drvdata(dev);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	dev_dbg(dev, "runtime resume!\n");
+
+	if ((omap_ssi->get_loss) && (omap_ssi->loss_count ==
+				omap_ssi->get_loss(ssi->device.parent)))
+		return 0;
+
+	writel_relaxed(omap_ssi->gdd_gcr, omap_ssi->gdd + SSI_GDD_GCR_REG);
+
+	return 0;
+}
+
+static const struct dev_pm_ops omap_ssi_pm_ops = {
+	SET_RUNTIME_PM_OPS(omap_ssi_runtime_suspend, omap_ssi_runtime_resume,
+		NULL)
+};
+
+#define DEV_PM_OPS     (&omap_ssi_pm_ops)
+#else
+#define DEV_PM_OPS     NULL
+#endif
+
+#ifdef CONFIG_OF
+static const struct of_device_id omap_ssi_of_match[] = {
+	{ .compatible = "ti,omap3-ssi", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, omap_ssi_of_match);
+#else
+#define omap_ssi_of_match NULL
+#endif
+
+static struct platform_driver ssi_pdriver = {
+	.remove	= __exit_p(ssi_remove),
+	.driver	= {
+		.name	= "omap_ssi",
+		.owner	= THIS_MODULE,
+		.pm     = DEV_PM_OPS,
+		.of_match_table = omap_ssi_of_match,
+	},
+};
+
+module_platform_driver_probe(ssi_pdriver, ssi_probe);
+
+MODULE_ALIAS("platform:omap_ssi");
+MODULE_AUTHOR("Carlos Chinea <carlos.chinea@nokia.com>");
+MODULE_AUTHOR("Sebastian Reichel <sre@kernel.org>");
+MODULE_DESCRIPTION("Synchronous Serial Interface Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/hsi/controllers/omap_ssi.h b/drivers/hsi/controllers/omap_ssi.h
new file mode 100644
index 00000000000..9d056417d88
--- /dev/null
+++ b/drivers/hsi/controllers/omap_ssi.h
@@ -0,0 +1,166 @@
+/* OMAP SSI internal interface.
+ *
+ * Copyright (C) 2010 Nokia Corporation. All rights reserved.
+ * Copyright (C) 2013 Sebastian Reichel
+ *
+ * Contact: Carlos Chinea <carlos.chinea@nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ */
+
+#ifndef __LINUX_HSI_OMAP_SSI_H__
+#define __LINUX_HSI_OMAP_SSI_H__
+
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/hsi/hsi.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+
+#define SSI_MAX_CHANNELS	8
+#define SSI_MAX_GDD_LCH		8
+#define SSI_BYTES_TO_FRAMES(x) ((((x) - 1) >> 2) + 1)
+
+/**
+ * struct omap_ssm_ctx - OMAP synchronous serial module (TX/RX) context
+ * @mode: Bit transmission mode
+ * @channels: Number of channels
+ * @framesize: Frame size in bits
+ * @timeout: RX frame timeout
+ * @divisor: TX divider
+ * @arb_mode: Arbitration mode for TX frame (Round robin, priority)
+ */
+struct omap_ssm_ctx {
+	u32	mode;
+	u32	channels;
+	u32	frame_size;
+	union	{
+			u32	timeout; /* Rx Only */
+			struct	{
+					u32	arb_mode;
+					u32	divisor;
+			}; /* Tx only */
+	};
+};
+
+/**
+ * struct omap_ssi_port - OMAP SSI port data
+ * @dev: device associated to the port (HSI port)
+ * @pdev: platform device associated to the port
+ * @sst_dma: SSI transmitter physical base address
+ * @ssr_dma: SSI receiver physical base address
+ * @sst_base: SSI transmitter base address
+ * @ssr_base: SSI receiver base address
+ * @wk_lock: spin lock to serialize access to the wake lines
+ * @lock: Spin lock to serialize access to the SSI port
+ * @channels: Current number of channels configured (1,2,4 or 8)
+ * @txqueue: TX message queues
+ * @rxqueue: RX message queues
+ * @brkqueue: Queue of incoming HWBREAK requests (FRAME mode)
+ * @irq: IRQ number
+ * @wake_irq: IRQ number for incoming wake line (-1 if none)
+ * @wake_gpio: GPIO number for incoming wake line (-1 if none)
+ * @pio_tasklet: Bottom half for PIO transfers and events
+ * @wake_tasklet: Bottom half for incoming wake events
+ * @wkin_cken: Keep track of clock references due to the incoming wake line
+ * @wk_refcount: Reference count for output wake line
+ * @sys_mpu_enable: Context for the interrupt enable register for irq 0
+ * @sst: Context for the synchronous serial transmitter
+ * @ssr: Context for the synchronous serial receiver
+ */
+struct omap_ssi_port {
+	struct device		*dev;
+	struct device           *pdev;
+	dma_addr_t		sst_dma;
+	dma_addr_t		ssr_dma;
+	void __iomem		*sst_base;
+	void __iomem		*ssr_base;
+	spinlock_t		wk_lock;
+	spinlock_t		lock;
+	unsigned int		channels;
+	struct list_head	txqueue[SSI_MAX_CHANNELS];
+	struct list_head	rxqueue[SSI_MAX_CHANNELS];
+	struct list_head	brkqueue;
+	unsigned int		irq;
+	int			wake_irq;
+	int			wake_gpio;
+	struct tasklet_struct	pio_tasklet;
+	struct tasklet_struct	wake_tasklet;
+	bool			wktest:1; /* FIXME: HACK to be removed */
+	bool			wkin_cken:1; /* Workaround */
+	unsigned int		wk_refcount;
+	/* OMAP SSI port context */
+	u32			sys_mpu_enable; /* We use only one irq */
+	struct omap_ssm_ctx	sst;
+	struct omap_ssm_ctx	ssr;
+	u32			loss_count;
+	u32			port_id;
+#ifdef CONFIG_DEBUG_FS
+	struct dentry *dir;
+#endif
+};
+
+/**
+ * struct gdd_trn - GDD transaction data
+ * @msg: Pointer to the HSI message being served
+ * @sg: Pointer to the current sg entry being served
+ */
+struct gdd_trn {
+	struct hsi_msg		*msg;
+	struct scatterlist	*sg;
+};
+
+/**
+ * struct omap_ssi_controller - OMAP SSI controller data
+ * @dev: device associated to the controller (HSI controller)
+ * @sys: SSI I/O base address
+ * @gdd: GDD I/O base address
+ * @fck: SSI functional clock
+ * @gdd_irq: IRQ line for GDD
+ * @gdd_tasklet: bottom half for DMA transfers
+ * @gdd_trn: Array of GDD transaction data for ongoing GDD transfers
+ * @lock: lock to serialize access to GDD
+ * @loss_count: To follow if we need to restore context or not
+ * @max_speed: Maximum TX speed (Kb/s) set by the clients.
+ * @sysconfig: SSI controller saved context
+ * @gdd_gcr: SSI GDD saved context
+ * @get_loss: Pointer to omap_pm_get_dev_context_loss_count, if any
+ * @port: Array of pointers of the ports of the controller
+ * @dir: Debugfs SSI root directory
+ */
+struct omap_ssi_controller {
+	struct device		*dev;
+	void __iomem		*sys;
+	void __iomem		*gdd;
+	struct clk		*fck;
+	unsigned int		gdd_irq;
+	struct tasklet_struct	gdd_tasklet;
+	struct gdd_trn		gdd_trn[SSI_MAX_GDD_LCH];
+	spinlock_t		lock;
+	unsigned long		fck_rate;
+	u32			loss_count;
+	u32			max_speed;
+	/* OMAP SSI Controller context */
+	u32			sysconfig;
+	u32			gdd_gcr;
+	int			(*get_loss)(struct device *dev);
+	struct omap_ssi_port	**port;
+#ifdef CONFIG_DEBUG_FS
+	struct dentry *dir;
+#endif
+};
+
+#endif /* __LINUX_HSI_OMAP_SSI_H__ */
diff --git a/drivers/hsi/controllers/omap_ssi_port.c b/drivers/hsi/controllers/omap_ssi_port.c
new file mode 100644
index 00000000000..b8693f0b27f
--- /dev/null
+++ b/drivers/hsi/controllers/omap_ssi_port.c
@@ -0,0 +1,1399 @@
+/* OMAP SSI port driver.
+ *
+ * Copyright (C) 2010 Nokia Corporation. All rights reserved.
+ * Copyright (C) 2014 Sebastian Reichel <sre@kernel.org>
+ *
+ * Contact: Carlos Chinea <carlos.chinea@nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ */
+
+#include <linux/platform_device.h>
+#include <linux/dma-mapping.h>
+#include <linux/pm_runtime.h>
+
+#include <linux/of_gpio.h>
+#include <linux/debugfs.h>
+
+#include "omap_ssi_regs.h"
+#include "omap_ssi.h"
+
+static inline int hsi_dummy_msg(struct hsi_msg *msg __maybe_unused)
+{
+	return 0;
+}
+
+static inline int hsi_dummy_cl(struct hsi_client *cl __maybe_unused)
+{
+	return 0;
+}
+
+static inline unsigned int ssi_wakein(struct hsi_port *port)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	return gpio_get_value(omap_port->wake_gpio);
+}
+
+#ifdef CONFIG_DEBUG_FS
+static void ssi_debug_remove_port(struct hsi_port *port)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+
+	debugfs_remove_recursive(omap_port->dir);
+}
+
+static int ssi_debug_port_show(struct seq_file *m, void *p __maybe_unused)
+{
+	struct hsi_port *port = m->private;
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem	*base = omap_ssi->sys;
+	unsigned int ch;
+
+	pm_runtime_get_sync(omap_port->pdev);
+	if (omap_port->wake_irq > 0)
+		seq_printf(m, "CAWAKE\t\t: %d\n", ssi_wakein(port));
+	seq_printf(m, "WAKE\t\t: 0x%08x\n",
+				readl(base + SSI_WAKE_REG(port->num)));
+	seq_printf(m, "MPU_ENABLE_IRQ%d\t: 0x%08x\n", 0,
+			readl(base + SSI_MPU_ENABLE_REG(port->num, 0)));
+	seq_printf(m, "MPU_STATUS_IRQ%d\t: 0x%08x\n", 0,
+			readl(base + SSI_MPU_STATUS_REG(port->num, 0)));
+	/* SST */
+	base = omap_port->sst_base;
+	seq_puts(m, "\nSST\n===\n");
+	seq_printf(m, "ID SST\t\t: 0x%08x\n",
+				readl(base + SSI_SST_ID_REG));
+	seq_printf(m, "MODE\t\t: 0x%08x\n",
+				readl(base + SSI_SST_MODE_REG));
+	seq_printf(m, "FRAMESIZE\t: 0x%08x\n",
+				readl(base + SSI_SST_FRAMESIZE_REG));
+	seq_printf(m, "DIVISOR\t\t: 0x%08x\n",
+				readl(base + SSI_SST_DIVISOR_REG));
+	seq_printf(m, "CHANNELS\t: 0x%08x\n",
+				readl(base + SSI_SST_CHANNELS_REG));
+	seq_printf(m, "ARBMODE\t\t: 0x%08x\n",
+				readl(base + SSI_SST_ARBMODE_REG));
+	seq_printf(m, "TXSTATE\t\t: 0x%08x\n",
+				readl(base + SSI_SST_TXSTATE_REG));
+	seq_printf(m, "BUFSTATE\t: 0x%08x\n",
+				readl(base + SSI_SST_BUFSTATE_REG));
+	seq_printf(m, "BREAK\t\t: 0x%08x\n",
+				readl(base + SSI_SST_BREAK_REG));
+	for (ch = 0; ch < omap_port->channels; ch++) {
+		seq_printf(m, "BUFFER_CH%d\t: 0x%08x\n", ch,
+				readl(base + SSI_SST_BUFFER_CH_REG(ch)));
+	}
+	/* SSR */
+	base = omap_port->ssr_base;
+	seq_puts(m, "\nSSR\n===\n");
+	seq_printf(m, "ID SSR\t\t: 0x%08x\n",
+				readl(base + SSI_SSR_ID_REG));
+	seq_printf(m, "MODE\t\t: 0x%08x\n",
+				readl(base + SSI_SSR_MODE_REG));
+	seq_printf(m, "FRAMESIZE\t: 0x%08x\n",
+				readl(base + SSI_SSR_FRAMESIZE_REG));
+	seq_printf(m, "CHANNELS\t: 0x%08x\n",
+				readl(base + SSI_SSR_CHANNELS_REG));
+	seq_printf(m, "TIMEOUT\t\t: 0x%08x\n",
+				readl(base + SSI_SSR_TIMEOUT_REG));
+	seq_printf(m, "RXSTATE\t\t: 0x%08x\n",
+				readl(base + SSI_SSR_RXSTATE_REG));
+	seq_printf(m, "BUFSTATE\t: 0x%08x\n",
+				readl(base + SSI_SSR_BUFSTATE_REG));
+	seq_printf(m, "BREAK\t\t: 0x%08x\n",
+				readl(base + SSI_SSR_BREAK_REG));
+	seq_printf(m, "ERROR\t\t: 0x%08x\n",
+				readl(base + SSI_SSR_ERROR_REG));
+	seq_printf(m, "ERRORACK\t: 0x%08x\n",
+				readl(base + SSI_SSR_ERRORACK_REG));
+	for (ch = 0; ch < omap_port->channels; ch++) {
+		seq_printf(m, "BUFFER_CH%d\t: 0x%08x\n", ch,
+				readl(base + SSI_SSR_BUFFER_CH_REG(ch)));
+	}
+	pm_runtime_put_sync(omap_port->pdev);
+
+	return 0;
+}
+
+static int ssi_port_regs_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ssi_debug_port_show, inode->i_private);
+}
+
+static const struct file_operations ssi_port_regs_fops = {
+	.open		= ssi_port_regs_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static int ssi_div_get(void *data, u64 *val)
+{
+	struct hsi_port *port = data;
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+
+	pm_runtime_get_sync(omap_port->pdev);
+	*val = readl(omap_port->sst_base + SSI_SST_DIVISOR_REG);
+	pm_runtime_put_sync(omap_port->pdev);
+
+	return 0;
+}
+
+static int ssi_div_set(void *data, u64 val)
+{
+	struct hsi_port *port = data;
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+
+	if (val > 127)
+		return -EINVAL;
+
+	pm_runtime_get_sync(omap_port->pdev);
+	writel(val, omap_port->sst_base + SSI_SST_DIVISOR_REG);
+	omap_port->sst.divisor = val;
+	pm_runtime_put_sync(omap_port->pdev);
+
+	return 0;
+}
+
+DEFINE_SIMPLE_ATTRIBUTE(ssi_sst_div_fops, ssi_div_get, ssi_div_set, "%llu\n");
+
+static int __init ssi_debug_add_port(struct omap_ssi_port *omap_port,
+				     struct dentry *dir)
+{
+	struct hsi_port *port = to_hsi_port(omap_port->dev);
+
+	dir = debugfs_create_dir(dev_name(omap_port->dev), dir);
+	if (IS_ERR(dir))
+		return PTR_ERR(dir);
+	omap_port->dir = dir;
+	debugfs_create_file("regs", S_IRUGO, dir, port, &ssi_port_regs_fops);
+	dir = debugfs_create_dir("sst", dir);
+	if (IS_ERR(dir))
+		return PTR_ERR(dir);
+	debugfs_create_file("divisor", S_IRUGO | S_IWUSR, dir, port,
+			    &ssi_sst_div_fops);
+
+	return 0;
+}
+#endif
+
+static int ssi_claim_lch(struct hsi_msg *msg)
+{
+
+	struct hsi_port *port = hsi_get_port(msg->cl);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	int lch;
+
+	for (lch = 0; lch < SSI_MAX_GDD_LCH; lch++)
+		if (!omap_ssi->gdd_trn[lch].msg) {
+			omap_ssi->gdd_trn[lch].msg = msg;
+			omap_ssi->gdd_trn[lch].sg = msg->sgt.sgl;
+			return lch;
+		}
+
+	return -EBUSY;
+}
+
+static int ssi_start_dma(struct hsi_msg *msg, int lch)
+{
+	struct hsi_port *port = hsi_get_port(msg->cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem *gdd = omap_ssi->gdd;
+	int err;
+	u16 csdp;
+	u16 ccr;
+	u32 s_addr;
+	u32 d_addr;
+	u32 tmp;
+
+	if (msg->ttype == HSI_MSG_READ) {
+		err = dma_map_sg(&ssi->device, msg->sgt.sgl, msg->sgt.nents,
+							DMA_FROM_DEVICE);
+		if (err < 0) {
+			dev_dbg(&ssi->device, "DMA map SG failed !\n");
+			return err;
+		}
+		csdp = SSI_DST_BURST_4x32_BIT | SSI_DST_MEMORY_PORT |
+			SSI_SRC_SINGLE_ACCESS0 | SSI_SRC_PERIPHERAL_PORT |
+			SSI_DATA_TYPE_S32;
+		ccr = msg->channel + 0x10 + (port->num * 8); /* Sync */
+		ccr |= SSI_DST_AMODE_POSTINC | SSI_SRC_AMODE_CONST |
+			SSI_CCR_ENABLE;
+		s_addr = omap_port->ssr_dma +
+					SSI_SSR_BUFFER_CH_REG(msg->channel);
+		d_addr = sg_dma_address(msg->sgt.sgl);
+	} else {
+		err = dma_map_sg(&ssi->device, msg->sgt.sgl, msg->sgt.nents,
+							DMA_TO_DEVICE);
+		if (err < 0) {
+			dev_dbg(&ssi->device, "DMA map SG failed !\n");
+			return err;
+		}
+		csdp = SSI_SRC_BURST_4x32_BIT | SSI_SRC_MEMORY_PORT |
+			SSI_DST_SINGLE_ACCESS0 | SSI_DST_PERIPHERAL_PORT |
+			SSI_DATA_TYPE_S32;
+		ccr = (msg->channel + 1 + (port->num * 8)) & 0xf; /* Sync */
+		ccr |= SSI_SRC_AMODE_POSTINC | SSI_DST_AMODE_CONST |
+			SSI_CCR_ENABLE;
+		s_addr = sg_dma_address(msg->sgt.sgl);
+		d_addr = omap_port->sst_dma +
+					SSI_SST_BUFFER_CH_REG(msg->channel);
+	}
+	dev_dbg(&ssi->device, "lch %d cdsp %08x ccr %04x s_addr %08x d_addr %08x\n",
+		lch, csdp, ccr, s_addr, d_addr);
+
+	/* Hold clocks during the transfer */
+	pm_runtime_get_sync(omap_port->pdev);
+
+	writew_relaxed(csdp, gdd + SSI_GDD_CSDP_REG(lch));
+	writew_relaxed(SSI_BLOCK_IE | SSI_TOUT_IE, gdd + SSI_GDD_CICR_REG(lch));
+	writel_relaxed(d_addr, gdd + SSI_GDD_CDSA_REG(lch));
+	writel_relaxed(s_addr, gdd + SSI_GDD_CSSA_REG(lch));
+	writew_relaxed(SSI_BYTES_TO_FRAMES(msg->sgt.sgl->length),
+						gdd + SSI_GDD_CEN_REG(lch));
+
+	spin_lock_bh(&omap_ssi->lock);
+	tmp = readl(omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	tmp |= SSI_GDD_LCH(lch);
+	writel_relaxed(tmp, omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	spin_unlock_bh(&omap_ssi->lock);
+	writew(ccr, gdd + SSI_GDD_CCR_REG(lch));
+	msg->status = HSI_STATUS_PROCEEDING;
+
+	return 0;
+}
+
+static int ssi_start_pio(struct hsi_msg *msg)
+{
+	struct hsi_port *port = hsi_get_port(msg->cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	u32 val;
+
+	pm_runtime_get_sync(omap_port->pdev);
+	if (msg->ttype == HSI_MSG_WRITE) {
+		val = SSI_DATAACCEPT(msg->channel);
+		/* Hold clocks for pio writes */
+		pm_runtime_get_sync(omap_port->pdev);
+	} else {
+		val = SSI_DATAAVAILABLE(msg->channel) | SSI_ERROROCCURED;
+	}
+	dev_dbg(&port->device, "Single %s transfer\n",
+						msg->ttype ? "write" : "read");
+	val |= readl(omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	writel(val, omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	pm_runtime_put_sync(omap_port->pdev);
+	msg->actual_len = 0;
+	msg->status = HSI_STATUS_PROCEEDING;
+
+	return 0;
+}
+
+static int ssi_start_transfer(struct list_head *queue)
+{
+	struct hsi_msg *msg;
+	int lch = -1;
+
+	if (list_empty(queue))
+		return 0;
+	msg = list_first_entry(queue, struct hsi_msg, link);
+	if (msg->status != HSI_STATUS_QUEUED)
+		return 0;
+	if ((msg->sgt.nents) && (msg->sgt.sgl->length > sizeof(u32)))
+		lch = ssi_claim_lch(msg);
+	if (lch >= 0)
+		return ssi_start_dma(msg, lch);
+	else
+		return ssi_start_pio(msg);
+}
+
+static int ssi_async_break(struct hsi_msg *msg)
+{
+	struct hsi_port *port = hsi_get_port(msg->cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	int err = 0;
+	u32 tmp;
+
+	pm_runtime_get_sync(omap_port->pdev);
+	if (msg->ttype == HSI_MSG_WRITE) {
+		if (omap_port->sst.mode != SSI_MODE_FRAME) {
+			err = -EINVAL;
+			goto out;
+		}
+		writel(1, omap_port->sst_base + SSI_SST_BREAK_REG);
+		msg->status = HSI_STATUS_COMPLETED;
+		msg->complete(msg);
+	} else {
+		if (omap_port->ssr.mode != SSI_MODE_FRAME) {
+			err = -EINVAL;
+			goto out;
+		}
+		spin_lock_bh(&omap_port->lock);
+		tmp = readl(omap_ssi->sys +
+					SSI_MPU_ENABLE_REG(port->num, 0));
+		writel(tmp | SSI_BREAKDETECTED,
+			omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+		msg->status = HSI_STATUS_PROCEEDING;
+		list_add_tail(&msg->link, &omap_port->brkqueue);
+		spin_unlock_bh(&omap_port->lock);
+	}
+out:
+	pm_runtime_put_sync(omap_port->pdev);
+
+	return err;
+}
+
+static int ssi_async(struct hsi_msg *msg)
+{
+	struct hsi_port *port = hsi_get_port(msg->cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct list_head *queue;
+	int err = 0;
+
+	BUG_ON(!msg);
+
+	if (msg->sgt.nents > 1)
+		return -ENOSYS; /* TODO: Add sg support */
+
+	if (msg->break_frame)
+		return ssi_async_break(msg);
+
+	if (msg->ttype) {
+		BUG_ON(msg->channel >= omap_port->sst.channels);
+		queue = &omap_port->txqueue[msg->channel];
+	} else {
+		BUG_ON(msg->channel >= omap_port->ssr.channels);
+		queue = &omap_port->rxqueue[msg->channel];
+	}
+	msg->status = HSI_STATUS_QUEUED;
+	spin_lock_bh(&omap_port->lock);
+	list_add_tail(&msg->link, queue);
+	err = ssi_start_transfer(queue);
+	if (err < 0) {
+		list_del(&msg->link);
+		msg->status = HSI_STATUS_ERROR;
+	}
+	spin_unlock_bh(&omap_port->lock);
+	dev_dbg(&port->device, "msg status %d ttype %d ch %d\n",
+				msg->status, msg->ttype, msg->channel);
+
+	return err;
+}
+
+static u32 ssi_calculate_div(struct hsi_controller *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	u32 tx_fckrate = (u32) omap_ssi->fck_rate;
+
+	/* / 2 : SSI TX clock is always half of the SSI functional clock */
+	tx_fckrate >>= 1;
+	/* Round down when tx_fckrate % omap_ssi->max_speed == 0 */
+	tx_fckrate--;
+	dev_dbg(&ssi->device, "TX div %d for fck_rate %lu Khz speed %d Kb/s\n",
+		tx_fckrate / omap_ssi->max_speed, omap_ssi->fck_rate,
+		omap_ssi->max_speed);
+
+	return tx_fckrate / omap_ssi->max_speed;
+}
+
+static void ssi_flush_queue(struct list_head *queue, struct hsi_client *cl)
+{
+	struct list_head *node, *tmp;
+	struct hsi_msg *msg;
+
+	list_for_each_safe(node, tmp, queue) {
+		msg = list_entry(node, struct hsi_msg, link);
+		if ((cl) && (cl != msg->cl))
+			continue;
+		list_del(node);
+		pr_debug("flush queue: ch %d, msg %p len %d type %d ctxt %p\n",
+			msg->channel, msg, msg->sgt.sgl->length,
+					msg->ttype, msg->context);
+		if (msg->destructor)
+			msg->destructor(msg);
+		else
+			hsi_free_msg(msg);
+	}
+}
+
+static int ssi_setup(struct hsi_client *cl)
+{
+	struct hsi_port *port = to_hsi_port(cl->device.parent);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem *sst = omap_port->sst_base;
+	void __iomem *ssr = omap_port->ssr_base;
+	u32 div;
+	u32 val;
+	int err = 0;
+
+	pm_runtime_get_sync(omap_port->pdev);
+	spin_lock_bh(&omap_port->lock);
+	if (cl->tx_cfg.speed)
+		omap_ssi->max_speed = cl->tx_cfg.speed;
+	div = ssi_calculate_div(ssi);
+	if (div > SSI_MAX_DIVISOR) {
+		dev_err(&cl->device, "Invalid TX speed %d Mb/s (div %d)\n",
+						cl->tx_cfg.speed, div);
+		err = -EINVAL;
+		goto out;
+	}
+	/* Set TX/RX module to sleep to stop TX/RX during cfg update */
+	writel_relaxed(SSI_MODE_SLEEP, sst + SSI_SST_MODE_REG);
+	writel_relaxed(SSI_MODE_SLEEP, ssr + SSI_SSR_MODE_REG);
+	/* Flush posted write */
+	val = readl(ssr + SSI_SSR_MODE_REG);
+	/* TX */
+	writel_relaxed(31, sst + SSI_SST_FRAMESIZE_REG);
+	writel_relaxed(div, sst + SSI_SST_DIVISOR_REG);
+	writel_relaxed(cl->tx_cfg.num_hw_channels, sst + SSI_SST_CHANNELS_REG);
+	writel_relaxed(cl->tx_cfg.arb_mode, sst + SSI_SST_ARBMODE_REG);
+	writel_relaxed(cl->tx_cfg.mode, sst + SSI_SST_MODE_REG);
+	/* RX */
+	writel_relaxed(31, ssr + SSI_SSR_FRAMESIZE_REG);
+	writel_relaxed(cl->rx_cfg.num_hw_channels, ssr + SSI_SSR_CHANNELS_REG);
+	writel_relaxed(0, ssr + SSI_SSR_TIMEOUT_REG);
+	/* Cleanup the break queue if we leave FRAME mode */
+	if ((omap_port->ssr.mode == SSI_MODE_FRAME) &&
+		(cl->rx_cfg.mode != SSI_MODE_FRAME))
+		ssi_flush_queue(&omap_port->brkqueue, cl);
+	writel_relaxed(cl->rx_cfg.mode, ssr + SSI_SSR_MODE_REG);
+	omap_port->channels = max(cl->rx_cfg.num_hw_channels,
+				  cl->tx_cfg.num_hw_channels);
+	/* Shadow registering for OFF mode */
+	/* SST */
+	omap_port->sst.divisor = div;
+	omap_port->sst.frame_size = 31;
+	omap_port->sst.channels = cl->tx_cfg.num_hw_channels;
+	omap_port->sst.arb_mode = cl->tx_cfg.arb_mode;
+	omap_port->sst.mode = cl->tx_cfg.mode;
+	/* SSR */
+	omap_port->ssr.frame_size = 31;
+	omap_port->ssr.timeout = 0;
+	omap_port->ssr.channels = cl->rx_cfg.num_hw_channels;
+	omap_port->ssr.mode = cl->rx_cfg.mode;
+out:
+	spin_unlock_bh(&omap_port->lock);
+	pm_runtime_put_sync(omap_port->pdev);
+
+	return err;
+}
+
+static int ssi_flush(struct hsi_client *cl)
+{
+	struct hsi_port *port = hsi_get_port(cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct hsi_msg *msg;
+	void __iomem *sst = omap_port->sst_base;
+	void __iomem *ssr = omap_port->ssr_base;
+	unsigned int i;
+	u32 err;
+
+	pm_runtime_get_sync(omap_port->pdev);
+	spin_lock_bh(&omap_port->lock);
+	/* Stop all DMA transfers */
+	for (i = 0; i < SSI_MAX_GDD_LCH; i++) {
+		msg = omap_ssi->gdd_trn[i].msg;
+		if (!msg || (port != hsi_get_port(msg->cl)))
+			continue;
+		writew_relaxed(0, omap_ssi->gdd + SSI_GDD_CCR_REG(i));
+		if (msg->ttype == HSI_MSG_READ)
+			pm_runtime_put_sync(omap_port->pdev);
+		omap_ssi->gdd_trn[i].msg = NULL;
+	}
+	/* Flush all SST buffers */
+	writel_relaxed(0, sst + SSI_SST_BUFSTATE_REG);
+	writel_relaxed(0, sst + SSI_SST_TXSTATE_REG);
+	/* Flush all SSR buffers */
+	writel_relaxed(0, ssr + SSI_SSR_RXSTATE_REG);
+	writel_relaxed(0, ssr + SSI_SSR_BUFSTATE_REG);
+	/* Flush all errors */
+	err = readl(ssr + SSI_SSR_ERROR_REG);
+	writel_relaxed(err, ssr + SSI_SSR_ERRORACK_REG);
+	/* Flush break */
+	writel_relaxed(0, ssr + SSI_SSR_BREAK_REG);
+	/* Clear interrupts */
+	writel_relaxed(0, omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	writel_relaxed(0xffffff00,
+			omap_ssi->sys + SSI_MPU_STATUS_REG(port->num, 0));
+	writel_relaxed(0, omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	writel(0xff, omap_ssi->sys + SSI_GDD_MPU_IRQ_STATUS_REG);
+	/* Dequeue all pending requests */
+	for (i = 0; i < omap_port->channels; i++) {
+		/* Release write clocks */
+		if (!list_empty(&omap_port->txqueue[i]))
+			pm_runtime_put_sync(omap_port->pdev);
+		ssi_flush_queue(&omap_port->txqueue[i], NULL);
+		ssi_flush_queue(&omap_port->rxqueue[i], NULL);
+	}
+	ssi_flush_queue(&omap_port->brkqueue, NULL);
+	spin_unlock_bh(&omap_port->lock);
+	pm_runtime_put_sync(omap_port->pdev);
+
+	return 0;
+}
+
+static int ssi_start_tx(struct hsi_client *cl)
+{
+	struct hsi_port *port = hsi_get_port(cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	dev_dbg(&port->device, "Wake out high %d\n", omap_port->wk_refcount);
+
+	spin_lock_bh(&omap_port->wk_lock);
+	if (omap_port->wk_refcount++) {
+		spin_unlock_bh(&omap_port->wk_lock);
+		return 0;
+	}
+	pm_runtime_get_sync(omap_port->pdev); /* Grab clocks */
+	writel(SSI_WAKE(0), omap_ssi->sys + SSI_SET_WAKE_REG(port->num));
+	spin_unlock_bh(&omap_port->wk_lock);
+
+	return 0;
+}
+
+static int ssi_stop_tx(struct hsi_client *cl)
+{
+	struct hsi_port *port = hsi_get_port(cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	dev_dbg(&port->device, "Wake out low %d\n", omap_port->wk_refcount);
+
+	spin_lock_bh(&omap_port->wk_lock);
+	BUG_ON(!omap_port->wk_refcount);
+	if (--omap_port->wk_refcount) {
+		spin_unlock_bh(&omap_port->wk_lock);
+		return 0;
+	}
+	writel(SSI_WAKE(0), omap_ssi->sys + SSI_CLEAR_WAKE_REG(port->num));
+	pm_runtime_put_sync(omap_port->pdev); /* Release clocks */
+	spin_unlock_bh(&omap_port->wk_lock);
+
+	return 0;
+}
+
+static void ssi_transfer(struct omap_ssi_port *omap_port,
+							struct list_head *queue)
+{
+	struct hsi_msg *msg;
+	int err = -1;
+
+	spin_lock_bh(&omap_port->lock);
+	while (err < 0) {
+		err = ssi_start_transfer(queue);
+		if (err < 0) {
+			msg = list_first_entry(queue, struct hsi_msg, link);
+			msg->status = HSI_STATUS_ERROR;
+			msg->actual_len = 0;
+			list_del(&msg->link);
+			spin_unlock_bh(&omap_port->lock);
+			msg->complete(msg);
+			spin_lock_bh(&omap_port->lock);
+		}
+	}
+	spin_unlock_bh(&omap_port->lock);
+}
+
+static void ssi_cleanup_queues(struct hsi_client *cl)
+{
+	struct hsi_port *port = hsi_get_port(cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct hsi_msg *msg;
+	unsigned int i;
+	u32 rxbufstate = 0;
+	u32 txbufstate = 0;
+	u32 status = SSI_ERROROCCURED;
+	u32 tmp;
+
+	ssi_flush_queue(&omap_port->brkqueue, cl);
+	if (list_empty(&omap_port->brkqueue))
+		status |= SSI_BREAKDETECTED;
+
+	for (i = 0; i < omap_port->channels; i++) {
+		if (list_empty(&omap_port->txqueue[i]))
+			continue;
+		msg = list_first_entry(&omap_port->txqueue[i], struct hsi_msg,
+									link);
+		if ((msg->cl == cl) && (msg->status == HSI_STATUS_PROCEEDING)) {
+			txbufstate |= (1 << i);
+			status |= SSI_DATAACCEPT(i);
+			/* Release the clocks writes, also GDD ones */
+			pm_runtime_put_sync(omap_port->pdev);
+		}
+		ssi_flush_queue(&omap_port->txqueue[i], cl);
+	}
+	for (i = 0; i < omap_port->channels; i++) {
+		if (list_empty(&omap_port->rxqueue[i]))
+			continue;
+		msg = list_first_entry(&omap_port->rxqueue[i], struct hsi_msg,
+									link);
+		if ((msg->cl == cl) && (msg->status == HSI_STATUS_PROCEEDING)) {
+			rxbufstate |= (1 << i);
+			status |= SSI_DATAAVAILABLE(i);
+		}
+		ssi_flush_queue(&omap_port->rxqueue[i], cl);
+		/* Check if we keep the error detection interrupt armed */
+		if (!list_empty(&omap_port->rxqueue[i]))
+			status &= ~SSI_ERROROCCURED;
+	}
+	/* Cleanup write buffers */
+	tmp = readl(omap_port->sst_base + SSI_SST_BUFSTATE_REG);
+	tmp &= ~txbufstate;
+	writel_relaxed(tmp, omap_port->sst_base + SSI_SST_BUFSTATE_REG);
+	/* Cleanup read buffers */
+	tmp = readl(omap_port->ssr_base + SSI_SSR_BUFSTATE_REG);
+	tmp &= ~rxbufstate;
+	writel_relaxed(tmp, omap_port->ssr_base + SSI_SSR_BUFSTATE_REG);
+	/* Disarm and ack pending interrupts */
+	tmp = readl(omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	tmp &= ~status;
+	writel_relaxed(tmp, omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	writel_relaxed(status, omap_ssi->sys +
+		SSI_MPU_STATUS_REG(port->num, 0));
+}
+
+static void ssi_cleanup_gdd(struct hsi_controller *ssi, struct hsi_client *cl)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct hsi_port *port = hsi_get_port(cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_msg *msg;
+	unsigned int i;
+	u32 val = 0;
+	u32 tmp;
+
+	for (i = 0; i < SSI_MAX_GDD_LCH; i++) {
+		msg = omap_ssi->gdd_trn[i].msg;
+		if ((!msg) || (msg->cl != cl))
+			continue;
+		writew_relaxed(0, omap_ssi->gdd + SSI_GDD_CCR_REG(i));
+		val |= (1 << i);
+		/*
+		 * Clock references for write will be handled in
+		 * ssi_cleanup_queues
+		 */
+		if (msg->ttype == HSI_MSG_READ)
+			pm_runtime_put_sync(omap_port->pdev);
+		omap_ssi->gdd_trn[i].msg = NULL;
+	}
+	tmp = readl_relaxed(omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	tmp &= ~val;
+	writel_relaxed(tmp, omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	writel(val, omap_ssi->sys + SSI_GDD_MPU_IRQ_STATUS_REG);
+}
+
+static int ssi_set_port_mode(struct omap_ssi_port *omap_port, u32 mode)
+{
+	writel(mode, omap_port->sst_base + SSI_SST_MODE_REG);
+	writel(mode, omap_port->ssr_base + SSI_SSR_MODE_REG);
+	/* OCP barrier */
+	mode = readl(omap_port->ssr_base + SSI_SSR_MODE_REG);
+
+	return 0;
+}
+
+static int ssi_release(struct hsi_client *cl)
+{
+	struct hsi_port *port = hsi_get_port(cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+
+	spin_lock_bh(&omap_port->lock);
+	pm_runtime_get_sync(omap_port->pdev);
+	/* Stop all the pending DMA requests for that client */
+	ssi_cleanup_gdd(ssi, cl);
+	/* Now cleanup all the queues */
+	ssi_cleanup_queues(cl);
+	pm_runtime_put_sync(omap_port->pdev);
+	/* If it is the last client of the port, do extra checks and cleanup */
+	if (port->claimed <= 1) {
+		/*
+		 * Drop the clock reference for the incoming wake line
+		 * if it is still kept high by the other side.
+		 */
+		if (omap_port->wkin_cken) {
+			pm_runtime_put_sync(omap_port->pdev);
+			omap_port->wkin_cken = 0;
+		}
+		pm_runtime_get_sync(omap_port->pdev);
+		/* Stop any SSI TX/RX without a client */
+		ssi_set_port_mode(omap_port, SSI_MODE_SLEEP);
+		omap_port->sst.mode = SSI_MODE_SLEEP;
+		omap_port->ssr.mode = SSI_MODE_SLEEP;
+		pm_runtime_put_sync(omap_port->pdev);
+		WARN_ON(omap_port->wk_refcount != 0);
+	}
+	spin_unlock_bh(&omap_port->lock);
+
+	return 0;
+}
+
+
+
+static void ssi_error(struct hsi_port *port)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct hsi_msg *msg;
+	unsigned int i;
+	u32 err;
+	u32 val;
+	u32 tmp;
+
+	/* ACK error */
+	err = readl(omap_port->ssr_base + SSI_SSR_ERROR_REG);
+	dev_err(&port->device, "SSI error: 0x%02x\n", err);
+	if (!err) {
+		dev_dbg(&port->device, "spurious SSI error ignored!\n");
+		return;
+	}
+	spin_lock(&omap_ssi->lock);
+	/* Cancel all GDD read transfers */
+	for (i = 0, val = 0; i < SSI_MAX_GDD_LCH; i++) {
+		msg = omap_ssi->gdd_trn[i].msg;
+		if ((msg) && (msg->ttype == HSI_MSG_READ)) {
+			writew_relaxed(0, omap_ssi->gdd + SSI_GDD_CCR_REG(i));
+			val |= (1 << i);
+			omap_ssi->gdd_trn[i].msg = NULL;
+		}
+	}
+	tmp = readl(omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	tmp &= ~val;
+	writel_relaxed(tmp, omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	spin_unlock(&omap_ssi->lock);
+	/* Cancel all PIO read transfers */
+	spin_lock(&omap_port->lock);
+	tmp = readl(omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	tmp &= 0xfeff00ff; /* Disable error & all dataavailable interrupts */
+	writel_relaxed(tmp, omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	/* ACK error */
+	writel_relaxed(err, omap_port->ssr_base + SSI_SSR_ERRORACK_REG);
+	writel_relaxed(SSI_ERROROCCURED,
+			omap_ssi->sys + SSI_MPU_STATUS_REG(port->num, 0));
+	/* Signal the error all current pending read requests */
+	for (i = 0; i < omap_port->channels; i++) {
+		if (list_empty(&omap_port->rxqueue[i]))
+			continue;
+		msg = list_first_entry(&omap_port->rxqueue[i], struct hsi_msg,
+									link);
+		list_del(&msg->link);
+		msg->status = HSI_STATUS_ERROR;
+		spin_unlock(&omap_port->lock);
+		msg->complete(msg);
+		/* Now restart queued reads if any */
+		ssi_transfer(omap_port, &omap_port->rxqueue[i]);
+		spin_lock(&omap_port->lock);
+	}
+	spin_unlock(&omap_port->lock);
+}
+
+static void ssi_break_complete(struct hsi_port *port)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct hsi_msg *msg;
+	struct hsi_msg *tmp;
+	u32 val;
+
+	dev_dbg(&port->device, "HWBREAK received\n");
+
+	spin_lock(&omap_port->lock);
+	val = readl(omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	val &= ~SSI_BREAKDETECTED;
+	writel_relaxed(val, omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	writel_relaxed(0, omap_port->ssr_base + SSI_SSR_BREAK_REG);
+	writel(SSI_BREAKDETECTED,
+			omap_ssi->sys + SSI_MPU_STATUS_REG(port->num, 0));
+	spin_unlock(&omap_port->lock);
+
+	list_for_each_entry_safe(msg, tmp, &omap_port->brkqueue, link) {
+		msg->status = HSI_STATUS_COMPLETED;
+		spin_lock(&omap_port->lock);
+		list_del(&msg->link);
+		spin_unlock(&omap_port->lock);
+		msg->complete(msg);
+	}
+
+}
+
+static void ssi_pio_complete(struct hsi_port *port, struct list_head *queue)
+{
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_msg *msg;
+	u32 *buf;
+	u32 reg;
+	u32 val;
+
+	spin_lock(&omap_port->lock);
+	msg = list_first_entry(queue, struct hsi_msg, link);
+	if ((!msg->sgt.nents) || (!msg->sgt.sgl->length)) {
+		msg->actual_len = 0;
+		msg->status = HSI_STATUS_PENDING;
+	}
+	if (msg->ttype == HSI_MSG_WRITE)
+		val = SSI_DATAACCEPT(msg->channel);
+	else
+		val = SSI_DATAAVAILABLE(msg->channel);
+	if (msg->status == HSI_STATUS_PROCEEDING) {
+		buf = sg_virt(msg->sgt.sgl) + msg->actual_len;
+		if (msg->ttype == HSI_MSG_WRITE)
+			writel(*buf, omap_port->sst_base +
+					SSI_SST_BUFFER_CH_REG(msg->channel));
+		 else
+			*buf = readl(omap_port->ssr_base +
+					SSI_SSR_BUFFER_CH_REG(msg->channel));
+		dev_dbg(&port->device, "ch %d ttype %d 0x%08x\n", msg->channel,
+							msg->ttype, *buf);
+		msg->actual_len += sizeof(*buf);
+		if (msg->actual_len >= msg->sgt.sgl->length)
+			msg->status = HSI_STATUS_COMPLETED;
+		/*
+		 * Wait for the last written frame to be really sent before
+		 * we call the complete callback
+		 */
+		if ((msg->status == HSI_STATUS_PROCEEDING) ||
+				((msg->status == HSI_STATUS_COMPLETED) &&
+					(msg->ttype == HSI_MSG_WRITE))) {
+			writel(val, omap_ssi->sys +
+					SSI_MPU_STATUS_REG(port->num, 0));
+			spin_unlock(&omap_port->lock);
+
+			return;
+		}
+
+	}
+	/* Transfer completed at this point */
+	reg = readl(omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	if (msg->ttype == HSI_MSG_WRITE) {
+		/* Release clocks for write transfer */
+		pm_runtime_put_sync(omap_port->pdev);
+	}
+	reg &= ~val;
+	writel_relaxed(reg, omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	writel_relaxed(val, omap_ssi->sys + SSI_MPU_STATUS_REG(port->num, 0));
+	list_del(&msg->link);
+	spin_unlock(&omap_port->lock);
+	msg->complete(msg);
+	ssi_transfer(omap_port, queue);
+}
+
+static void ssi_pio_tasklet(unsigned long ssi_port)
+{
+	struct hsi_port *port = (struct hsi_port *)ssi_port;
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem *sys = omap_ssi->sys;
+	unsigned int ch;
+	u32 status_reg;
+
+	pm_runtime_get_sync(omap_port->pdev);
+	status_reg = readl(sys + SSI_MPU_STATUS_REG(port->num, 0));
+	status_reg &= readl(sys + SSI_MPU_ENABLE_REG(port->num, 0));
+
+	for (ch = 0; ch < omap_port->channels; ch++) {
+		if (status_reg & SSI_DATAACCEPT(ch))
+			ssi_pio_complete(port, &omap_port->txqueue[ch]);
+		if (status_reg & SSI_DATAAVAILABLE(ch))
+			ssi_pio_complete(port, &omap_port->rxqueue[ch]);
+	}
+	if (status_reg & SSI_BREAKDETECTED)
+		ssi_break_complete(port);
+	if (status_reg & SSI_ERROROCCURED)
+		ssi_error(port);
+
+	status_reg = readl(sys + SSI_MPU_STATUS_REG(port->num, 0));
+	status_reg &= readl(sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	pm_runtime_put_sync(omap_port->pdev);
+
+	if (status_reg)
+		tasklet_hi_schedule(&omap_port->pio_tasklet);
+	else
+		enable_irq(omap_port->irq);
+}
+
+static irqreturn_t ssi_pio_isr(int irq, void *port)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+
+	tasklet_hi_schedule(&omap_port->pio_tasklet);
+	disable_irq_nosync(irq);
+
+	return IRQ_HANDLED;
+}
+
+static void ssi_wake_tasklet(unsigned long ssi_port)
+{
+	struct hsi_port *port = (struct hsi_port *)ssi_port;
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	if (ssi_wakein(port)) {
+		/**
+		 * We can have a quick High-Low-High transition in the line.
+		 * In such a case if we have long interrupt latencies,
+		 * we can miss the low event or get twice a high event.
+		 * This workaround will avoid breaking the clock reference
+		 * count when such a situation ocurrs.
+		 */
+		spin_lock(&omap_port->lock);
+		if (!omap_port->wkin_cken) {
+			omap_port->wkin_cken = 1;
+			pm_runtime_get_sync(omap_port->pdev);
+		}
+		spin_unlock(&omap_port->lock);
+		dev_dbg(&ssi->device, "Wake in high\n");
+		if (omap_port->wktest) { /* FIXME: HACK ! To be removed */
+			writel(SSI_WAKE(0),
+				omap_ssi->sys + SSI_SET_WAKE_REG(port->num));
+		}
+		hsi_event(port, HSI_EVENT_START_RX);
+	} else {
+		dev_dbg(&ssi->device, "Wake in low\n");
+		if (omap_port->wktest) { /* FIXME: HACK ! To be removed */
+			writel(SSI_WAKE(0),
+				omap_ssi->sys + SSI_CLEAR_WAKE_REG(port->num));
+		}
+		hsi_event(port, HSI_EVENT_STOP_RX);
+		spin_lock(&omap_port->lock);
+		if (omap_port->wkin_cken) {
+			pm_runtime_put_sync(omap_port->pdev);
+			omap_port->wkin_cken = 0;
+		}
+		spin_unlock(&omap_port->lock);
+	}
+}
+
+static irqreturn_t ssi_wake_isr(int irq __maybe_unused, void *ssi_port)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(ssi_port);
+
+	tasklet_hi_schedule(&omap_port->wake_tasklet);
+
+	return IRQ_HANDLED;
+}
+
+static int __init ssi_port_irq(struct hsi_port *port,
+						struct platform_device *pd)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	int err;
+
+	omap_port->irq = platform_get_irq(pd, 0);
+	if (omap_port->irq < 0) {
+		dev_err(&port->device, "Port IRQ resource missing\n");
+		return omap_port->irq;
+	}
+	tasklet_init(&omap_port->pio_tasklet, ssi_pio_tasklet,
+							(unsigned long)port);
+	err = devm_request_irq(&port->device, omap_port->irq, ssi_pio_isr,
+						0, "mpu_irq0", port);
+	if (err < 0)
+		dev_err(&port->device, "Request IRQ %d failed (%d)\n",
+							omap_port->irq, err);
+	return err;
+}
+
+static int __init ssi_wake_irq(struct hsi_port *port,
+						struct platform_device *pd)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	int cawake_irq;
+	int err;
+
+	if (omap_port->wake_gpio == -1) {
+		omap_port->wake_irq = -1;
+		return 0;
+	}
+
+	cawake_irq = gpio_to_irq(omap_port->wake_gpio);
+
+	omap_port->wake_irq = cawake_irq;
+	tasklet_init(&omap_port->wake_tasklet, ssi_wake_tasklet,
+							(unsigned long)port);
+	err = devm_request_irq(&port->device, cawake_irq, ssi_wake_isr,
+		IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
+							"cawake", port);
+	if (err < 0)
+		dev_err(&port->device, "Request Wake in IRQ %d failed %d\n",
+						cawake_irq, err);
+	err = enable_irq_wake(cawake_irq);
+	if (err < 0)
+		dev_err(&port->device, "Enable wake on the wakeline in irq %d failed %d\n",
+			cawake_irq, err);
+
+	return err;
+}
+
+static void __init ssi_queues_init(struct omap_ssi_port *omap_port)
+{
+	unsigned int ch;
+
+	for (ch = 0; ch < SSI_MAX_CHANNELS; ch++) {
+		INIT_LIST_HEAD(&omap_port->txqueue[ch]);
+		INIT_LIST_HEAD(&omap_port->rxqueue[ch]);
+	}
+	INIT_LIST_HEAD(&omap_port->brkqueue);
+}
+
+static int __init ssi_port_get_iomem(struct platform_device *pd,
+		const char *name, void __iomem **pbase, dma_addr_t *phy)
+{
+	struct hsi_port *port = platform_get_drvdata(pd);
+	struct resource *mem;
+	struct resource *ioarea;
+	void __iomem *base;
+
+	mem = platform_get_resource_byname(pd, IORESOURCE_MEM, name);
+	if (!mem) {
+		dev_err(&pd->dev, "IO memory region missing (%s)\n", name);
+		return -ENXIO;
+	}
+	ioarea = devm_request_mem_region(&port->device, mem->start,
+					resource_size(mem), dev_name(&pd->dev));
+	if (!ioarea) {
+		dev_err(&pd->dev, "%s IO memory region request failed\n",
+								mem->name);
+		return -ENXIO;
+	}
+	base = devm_ioremap(&port->device, mem->start, resource_size(mem));
+	if (!base) {
+		dev_err(&pd->dev, "%s IO remap failed\n", mem->name);
+		return -ENXIO;
+	}
+	*pbase = base;
+
+	if (phy)
+		*phy = mem->start;
+
+	return 0;
+}
+
+static int __init ssi_port_probe(struct platform_device *pd)
+{
+	struct device_node *np = pd->dev.of_node;
+	struct hsi_port *port;
+	struct omap_ssi_port *omap_port;
+	struct hsi_controller *ssi = dev_get_drvdata(pd->dev.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	u32 cawake_gpio = 0;
+	u32 port_id;
+	int err;
+
+	dev_dbg(&pd->dev, "init ssi port...\n");
+
+	err = ref_module(THIS_MODULE, ssi->owner);
+	if (err) {
+		dev_err(&pd->dev, "could not increment parent module refcount (err=%d)\n",
+			err);
+		return -ENODEV;
+	}
+
+	if (!ssi->port || !omap_ssi->port) {
+		dev_err(&pd->dev, "ssi controller not initialized!\n");
+		err = -ENODEV;
+		goto error;
+	}
+
+	/* get id of first uninitialized port in controller */
+	for (port_id = 0; port_id < ssi->num_ports && omap_ssi->port[port_id];
+		port_id++)
+		;
+
+	if (port_id >= ssi->num_ports) {
+		dev_err(&pd->dev, "port id out of range!\n");
+		err = -ENODEV;
+		goto error;
+	}
+
+	port = ssi->port[port_id];
+
+	if (!np) {
+		dev_err(&pd->dev, "missing device tree data\n");
+		err = -EINVAL;
+		goto error;
+	}
+
+	cawake_gpio = of_get_named_gpio(np, "ti,ssi-cawake-gpio", 0);
+	if (cawake_gpio < 0) {
+		dev_err(&pd->dev, "DT data is missing cawake gpio (err=%d)\n",
+			cawake_gpio);
+		err = -ENODEV;
+		goto error;
+	}
+
+	err = devm_gpio_request_one(&port->device, cawake_gpio, GPIOF_DIR_IN,
+		"cawake");
+	if (err) {
+		dev_err(&pd->dev, "could not request cawake gpio (err=%d)!\n",
+			err);
+		err = -ENXIO;
+		goto error;
+	}
+
+	omap_port = devm_kzalloc(&port->device, sizeof(*omap_port), GFP_KERNEL);
+	if (!omap_port) {
+		err = -ENOMEM;
+		goto error;
+	}
+	omap_port->wake_gpio = cawake_gpio;
+	omap_port->pdev = &pd->dev;
+	omap_port->port_id = port_id;
+
+	/* initialize HSI port */
+	port->async	= ssi_async;
+	port->setup	= ssi_setup;
+	port->flush	= ssi_flush;
+	port->start_tx	= ssi_start_tx;
+	port->stop_tx	= ssi_stop_tx;
+	port->release	= ssi_release;
+	hsi_port_set_drvdata(port, omap_port);
+	omap_ssi->port[port_id] = omap_port;
+
+	platform_set_drvdata(pd, port);
+
+	err = ssi_port_get_iomem(pd, "tx", &omap_port->sst_base,
+		&omap_port->sst_dma);
+	if (err < 0)
+		goto error;
+	err = ssi_port_get_iomem(pd, "rx", &omap_port->ssr_base,
+		&omap_port->ssr_dma);
+	if (err < 0)
+		goto error;
+
+	err = ssi_port_irq(port, pd);
+	if (err < 0)
+		goto error;
+	err = ssi_wake_irq(port, pd);
+	if (err < 0)
+		goto error;
+
+	ssi_queues_init(omap_port);
+	spin_lock_init(&omap_port->lock);
+	spin_lock_init(&omap_port->wk_lock);
+	omap_port->dev = &port->device;
+
+	pm_runtime_irq_safe(omap_port->pdev);
+	pm_runtime_enable(omap_port->pdev);
+
+#ifdef CONFIG_DEBUG_FS
+	err = ssi_debug_add_port(omap_port, omap_ssi->dir);
+	if (err < 0) {
+		pm_runtime_disable(omap_port->pdev);
+		goto error;
+	}
+#endif
+
+	hsi_add_clients_from_dt(port, np);
+
+	dev_info(&pd->dev, "ssi port %u successfully initialized (cawake=%d)\n",
+		port_id, cawake_gpio);
+
+	return 0;
+
+error:
+	return err;
+}
+
+static int __exit ssi_port_remove(struct platform_device *pd)
+{
+	struct hsi_port *port = platform_get_drvdata(pd);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+#ifdef CONFIG_DEBUG_FS
+	ssi_debug_remove_port(port);
+#endif
+
+	hsi_port_unregister_clients(port);
+
+	tasklet_kill(&omap_port->wake_tasklet);
+	tasklet_kill(&omap_port->pio_tasklet);
+
+	port->async	= hsi_dummy_msg;
+	port->setup	= hsi_dummy_cl;
+	port->flush	= hsi_dummy_cl;
+	port->start_tx	= hsi_dummy_cl;
+	port->stop_tx	= hsi_dummy_cl;
+	port->release	= hsi_dummy_cl;
+
+	omap_ssi->port[omap_port->port_id] = NULL;
+	platform_set_drvdata(pd, NULL);
+	pm_runtime_disable(&pd->dev);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_RUNTIME
+static int ssi_save_port_ctx(struct omap_ssi_port *omap_port)
+{
+	struct hsi_port *port = to_hsi_port(omap_port->dev);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	omap_port->sys_mpu_enable = readl(omap_ssi->sys +
+					SSI_MPU_ENABLE_REG(port->num, 0));
+
+	return 0;
+}
+
+static int ssi_restore_port_ctx(struct omap_ssi_port *omap_port)
+{
+	struct hsi_port *port = to_hsi_port(omap_port->dev);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem	*base;
+
+	writel_relaxed(omap_port->sys_mpu_enable,
+			omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+
+	/* SST context */
+	base = omap_port->sst_base;
+	writel_relaxed(omap_port->sst.frame_size, base + SSI_SST_FRAMESIZE_REG);
+	writel_relaxed(omap_port->sst.channels, base + SSI_SST_CHANNELS_REG);
+	writel_relaxed(omap_port->sst.arb_mode, base + SSI_SST_ARBMODE_REG);
+
+	/* SSR context */
+	base = omap_port->ssr_base;
+	writel_relaxed(omap_port->ssr.frame_size, base + SSI_SSR_FRAMESIZE_REG);
+	writel_relaxed(omap_port->ssr.channels, base + SSI_SSR_CHANNELS_REG);
+	writel_relaxed(omap_port->ssr.timeout, base + SSI_SSR_TIMEOUT_REG);
+
+	return 0;
+}
+
+static int ssi_restore_port_mode(struct omap_ssi_port *omap_port)
+{
+	u32 mode;
+
+	writel_relaxed(omap_port->sst.mode,
+				omap_port->sst_base + SSI_SST_MODE_REG);
+	writel_relaxed(omap_port->ssr.mode,
+				omap_port->ssr_base + SSI_SSR_MODE_REG);
+	/* OCP barrier */
+	mode = readl(omap_port->ssr_base + SSI_SSR_MODE_REG);
+
+	return 0;
+}
+
+static int ssi_restore_divisor(struct omap_ssi_port *omap_port)
+{
+	writel_relaxed(omap_port->sst.divisor,
+				omap_port->sst_base + SSI_SST_DIVISOR_REG);
+
+	return 0;
+}
+
+static int omap_ssi_port_runtime_suspend(struct device *dev)
+{
+	struct hsi_port *port = dev_get_drvdata(dev);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	dev_dbg(dev, "port runtime suspend!\n");
+
+	ssi_set_port_mode(omap_port, SSI_MODE_SLEEP);
+	if (omap_ssi->get_loss)
+		omap_port->loss_count =
+				omap_ssi->get_loss(ssi->device.parent);
+	ssi_save_port_ctx(omap_port);
+
+	return 0;
+}
+
+static int omap_ssi_port_runtime_resume(struct device *dev)
+{
+	struct hsi_port *port = dev_get_drvdata(dev);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	dev_dbg(dev, "port runtime resume!\n");
+
+	if ((omap_ssi->get_loss) && (omap_port->loss_count ==
+				omap_ssi->get_loss(ssi->device.parent)))
+		goto mode; /* We always need to restore the mode & TX divisor */
+
+	ssi_restore_port_ctx(omap_port);
+
+mode:
+	ssi_restore_divisor(omap_port);
+	ssi_restore_port_mode(omap_port);
+
+	return 0;
+}
+
+static const struct dev_pm_ops omap_ssi_port_pm_ops = {
+	SET_RUNTIME_PM_OPS(omap_ssi_port_runtime_suspend,
+		omap_ssi_port_runtime_resume, NULL)
+};
+
+#define DEV_PM_OPS     (&omap_ssi_port_pm_ops)
+#else
+#define DEV_PM_OPS     NULL
+#endif
+
+
+#ifdef CONFIG_OF
+static const struct of_device_id omap_ssi_port_of_match[] = {
+	{ .compatible = "ti,omap3-ssi-port", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, omap_ssi_port_of_match);
+#else
+#define omap_ssi_port_of_match NULL
+#endif
+
+static struct platform_driver ssi_port_pdriver = {
+	.remove	= __exit_p(ssi_port_remove),
+	.driver	= {
+		.name	= "omap_ssi_port",
+		.owner	= THIS_MODULE,
+		.of_match_table = omap_ssi_port_of_match,
+		.pm	= DEV_PM_OPS,
+	},
+};
+
+module_platform_driver_probe(ssi_port_pdriver, ssi_port_probe);
+
+MODULE_ALIAS("platform:omap_ssi_port");
+MODULE_AUTHOR("Carlos Chinea <carlos.chinea@nokia.com>");
+MODULE_AUTHOR("Sebastian Reichel <sre@kernel.org>");
+MODULE_DESCRIPTION("Synchronous Serial Interface Port Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/hsi/controllers/omap_ssi_regs.h b/drivers/hsi/controllers/omap_ssi_regs.h
new file mode 100644
index 00000000000..08f98dd1d01
--- /dev/null
+++ b/drivers/hsi/controllers/omap_ssi_regs.h
@@ -0,0 +1,171 @@
+/* Hardware definitions for SSI.
+ *
+ * Copyright (C) 2010 Nokia Corporation. All rights reserved.
+ *
+ * Contact: Carlos Chinea <carlos.chinea@nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ */
+
+#ifndef __OMAP_SSI_REGS_H__
+#define __OMAP_SSI_REGS_H__
+
+/*
+ * SSI SYS registers
+ */
+#define SSI_REVISION_REG    0
+#  define SSI_REV_MAJOR    0xf0
+#  define SSI_REV_MINOR    0xf
+#define SSI_SYSCONFIG_REG    0x10
+#  define SSI_AUTOIDLE    (1 << 0)
+#  define SSI_SOFTRESET    (1 << 1)
+#  define SSI_SIDLEMODE_FORCE  0
+#  define SSI_SIDLEMODE_NO    (1 << 3)
+#  define SSI_SIDLEMODE_SMART  (1 << 4)
+#  define SSI_SIDLEMODE_MASK  0x18
+#  define SSI_MIDLEMODE_FORCE  0
+#  define SSI_MIDLEMODE_NO    (1 << 12)
+#  define SSI_MIDLEMODE_SMART  (1 << 13)
+#  define SSI_MIDLEMODE_MASK  0x3000
+#define SSI_SYSSTATUS_REG    0x14
+#  define SSI_RESETDONE    1
+#define SSI_MPU_STATUS_REG(port, irq)  (0x808 + ((port) * 0x10) + ((irq) * 2))
+#define SSI_MPU_ENABLE_REG(port, irq)  (0x80c + ((port) * 0x10) + ((irq) * 8))
+#  define SSI_DATAACCEPT(channel)    (1 << (channel))
+#  define SSI_DATAAVAILABLE(channel)  (1 << ((channel) + 8))
+#  define SSI_DATAOVERRUN(channel)    (1 << ((channel) + 16))
+#  define SSI_ERROROCCURED      (1 << 24)
+#  define SSI_BREAKDETECTED    (1 << 25)
+#define SSI_GDD_MPU_IRQ_STATUS_REG  0x0800
+#define SSI_GDD_MPU_IRQ_ENABLE_REG  0x0804
+#  define SSI_GDD_LCH(channel)  (1 << (channel))
+#define SSI_WAKE_REG(port)    (0xc00 + ((port) * 0x10))
+#define SSI_CLEAR_WAKE_REG(port)  (0xc04 + ((port) * 0x10))
+#define SSI_SET_WAKE_REG(port)    (0xc08 + ((port) * 0x10))
+#  define SSI_WAKE(channel)  (1 << (channel))
+#  define SSI_WAKE_MASK    0xff
+
+/*
+ * SSI SST registers
+ */
+#define SSI_SST_ID_REG      0
+#define SSI_SST_MODE_REG    4
+#  define SSI_MODE_VAL_MASK  3
+#  define SSI_MODE_SLEEP    0
+#  define SSI_MODE_STREAM    1
+#  define SSI_MODE_FRAME    2
+#  define SSI_MODE_MULTIPOINTS  3
+#define SSI_SST_FRAMESIZE_REG    8
+#  define SSI_FRAMESIZE_DEFAULT  31
+#define SSI_SST_TXSTATE_REG    0xc
+#  define  SSI_TXSTATE_IDLE  0
+#define SSI_SST_BUFSTATE_REG    0x10
+#  define  SSI_FULL(channel)  (1 << (channel))
+#define SSI_SST_DIVISOR_REG    0x18
+#  define SSI_MAX_DIVISOR    127
+#define SSI_SST_BREAK_REG    0x20
+#define SSI_SST_CHANNELS_REG    0x24
+#  define SSI_CHANNELS_DEFAULT  4
+#define SSI_SST_ARBMODE_REG    0x28
+#  define SSI_ARBMODE_ROUNDROBIN  0
+#  define SSI_ARBMODE_PRIORITY  1
+#define SSI_SST_BUFFER_CH_REG(channel)  (0x80 + ((channel) * 4))
+#define SSI_SST_SWAPBUF_CH_REG(channel)  (0xc0 + ((channel) * 4))
+
+/*
+ * SSI SSR registers
+ */
+#define SSI_SSR_ID_REG      0
+#define SSI_SSR_MODE_REG    4
+#define SSI_SSR_FRAMESIZE_REG    8
+#define SSI_SSR_RXSTATE_REG    0xc
+#define SSI_SSR_BUFSTATE_REG    0x10
+#  define SSI_NOTEMPTY(channel)  (1 << (channel))
+#define SSI_SSR_BREAK_REG    0x1c
+#define SSI_SSR_ERROR_REG    0x20
+#define SSI_SSR_ERRORACK_REG    0x24
+#define SSI_SSR_OVERRUN_REG    0x2c
+#define SSI_SSR_OVERRUNACK_REG    0x30
+#define SSI_SSR_TIMEOUT_REG    0x34
+#  define SSI_TIMEOUT_DEFAULT  0
+#define SSI_SSR_CHANNELS_REG    0x28
+#define SSI_SSR_BUFFER_CH_REG(channel)  (0x80 + ((channel) * 4))
+#define SSI_SSR_SWAPBUF_CH_REG(channel)  (0xc0 + ((channel) * 4))
+
+/*
+ * SSI GDD registers
+ */
+#define SSI_GDD_HW_ID_REG    0
+#define SSI_GDD_PPORT_ID_REG    0x10
+#define SSI_GDD_MPORT_ID_REG    0x14
+#define SSI_GDD_PPORT_SR_REG    0x20
+#define SSI_GDD_MPORT_SR_REG    0x24
+#  define SSI_ACTIVE_LCH_NUM_MASK  0xff
+#define SSI_GDD_TEST_REG    0x40
+#  define SSI_TEST      1
+#define SSI_GDD_GCR_REG      0x100
+#  define  SSI_CLK_AUTOGATING_ON  (1 << 3)
+#  define  SSI_FREE    (1 << 2)
+#  define  SSI_SWITCH_OFF    (1 << 0)
+#define SSI_GDD_GRST_REG    0x200
+#  define SSI_SWRESET    1
+#define SSI_GDD_CSDP_REG(channel)  (0x800 + ((channel) * 0x40))
+#  define SSI_DST_BURST_EN_MASK  0xc000
+#  define SSI_DST_SINGLE_ACCESS0  0
+#  define SSI_DST_SINGLE_ACCESS  (1 << 14)
+#  define SSI_DST_BURST_4x32_BIT  (2 << 14)
+#  define SSI_DST_BURST_8x32_BIT  (3 << 14)
+#  define SSI_DST_MASK    0x1e00
+#  define SSI_DST_MEMORY_PORT  (8 << 9)
+#  define SSI_DST_PERIPHERAL_PORT  (9 << 9)
+#  define SSI_SRC_BURST_EN_MASK  0x180
+#  define SSI_SRC_SINGLE_ACCESS0  0
+#  define SSI_SRC_SINGLE_ACCESS  (1 << 7)
+#  define SSI_SRC_BURST_4x32_BIT  (2 << 7)
+#  define SSI_SRC_BURST_8x32_BIT  (3 << 7)
+#  define SSI_SRC_MASK    0x3c
+#  define SSI_SRC_MEMORY_PORT  (8 << 2)
+#  define SSI_SRC_PERIPHERAL_PORT  (9 << 2)
+#  define SSI_DATA_TYPE_MASK  3
+#  define SSI_DATA_TYPE_S32  2
+#define SSI_GDD_CCR_REG(channel)  (0x802 + ((channel) * 0x40))
+#  define SSI_DST_AMODE_MASK  (3 << 14)
+#  define SSI_DST_AMODE_CONST  0
+#  define SSI_DST_AMODE_POSTINC  (1 << 12)
+#  define SSI_SRC_AMODE_MASK  (3 << 12)
+#  define SSI_SRC_AMODE_CONST  0
+#  define SSI_SRC_AMODE_POSTINC  (1 << 12)
+#  define SSI_CCR_ENABLE    (1 << 7)
+#  define SSI_CCR_SYNC_MASK  0x1f
+#define SSI_GDD_CICR_REG(channel)  (0x804 + ((channel) * 0x40))
+#  define SSI_BLOCK_IE    (1 << 5)
+#  define SSI_HALF_IE    (1 << 2)
+#  define SSI_TOUT_IE    (1 << 0)
+#define SSI_GDD_CSR_REG(channel)  (0x806 + ((channel) * 0x40))
+#  define SSI_CSR_SYNC    (1 << 6)
+#  define SSI_CSR_BLOCK    (1 << 5)
+#  define SSI_CSR_HALF    (1 << 2)
+#  define SSI_CSR_TOUR    (1 << 0)
+#define SSI_GDD_CSSA_REG(channel)  (0x808 + ((channel) * 0x40))
+#define SSI_GDD_CDSA_REG(channel)  (0x80c + ((channel) * 0x40))
+#define SSI_GDD_CEN_REG(channel)  (0x810 + ((channel) * 0x40))
+#define SSI_GDD_CSAC_REG(channel)  (0x818 + ((channel) * 0x40))
+#define SSI_GDD_CDAC_REG(channel)  (0x81a + ((channel) * 0x40))
+#define SSI_GDD_CLNK_CTRL_REG(channel)  (0x828 + ((channel) * 0x40))
+#  define SSI_ENABLE_LNK    (1 << 15)
+#  define SSI_STOP_LNK    (1 << 14)
+#  define SSI_NEXT_CH_ID_MASK  0xf
+
+#endif /* __OMAP_SSI_REGS_H__ */
diff --git a/drivers/hsi/hsi.c b/drivers/hsi/hsi.c
index 749f7b5c817..fe9371271ce 100644
--- a/drivers/hsi/hsi.c
+++ b/drivers/hsi/hsi.c
@@ -26,6 +26,8 @@
 #include <linux/slab.h>
 #include <linux/string.h>
 #include <linux/notifier.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
 #include "hsi_core.h"
 
 static ssize_t modalias_show(struct device *dev,
@@ -50,7 +52,13 @@ static int hsi_bus_uevent(struct device *dev, struct kobj_uevent_env *env)
 
 static int hsi_bus_match(struct device *dev, struct device_driver *driver)
 {
-	return strcmp(dev_name(dev), driver->name) == 0;
+	if (of_driver_match_device(dev, driver))
+		return true;
+
+	if (strcmp(dev_name(dev), driver->name) == 0)
+		return true;
+
+	return false;
 }
 
 static struct bus_type hsi_bus_type = {
@@ -62,18 +70,37 @@ static struct bus_type hsi_bus_type = {
 
 static void hsi_client_release(struct device *dev)
 {
-	kfree(to_hsi_client(dev));
+	struct hsi_client *cl = to_hsi_client(dev);
+
+	kfree(cl->tx_cfg.channels);
+	kfree(cl->rx_cfg.channels);
+	kfree(cl);
 }
 
-static void hsi_new_client(struct hsi_port *port, struct hsi_board_info *info)
+struct hsi_client *hsi_new_client(struct hsi_port *port,
+						struct hsi_board_info *info)
 {
 	struct hsi_client *cl;
+	size_t size;
 
 	cl = kzalloc(sizeof(*cl), GFP_KERNEL);
 	if (!cl)
-		return;
+		return NULL;
+
 	cl->tx_cfg = info->tx_cfg;
+	if (cl->tx_cfg.channels) {
+		size = cl->tx_cfg.num_channels * sizeof(*cl->tx_cfg.channels);
+		cl->tx_cfg.channels = kzalloc(size , GFP_KERNEL);
+		memcpy(cl->tx_cfg.channels, info->tx_cfg.channels, size);
+	}
+
 	cl->rx_cfg = info->rx_cfg;
+	if (cl->rx_cfg.channels) {
+		size = cl->rx_cfg.num_channels * sizeof(*cl->rx_cfg.channels);
+		cl->rx_cfg.channels = kzalloc(size , GFP_KERNEL);
+		memcpy(cl->rx_cfg.channels, info->rx_cfg.channels, size);
+	}
+
 	cl->device.bus = &hsi_bus_type;
 	cl->device.parent = &port->device;
 	cl->device.release = hsi_client_release;
@@ -85,7 +112,10 @@ static void hsi_new_client(struct hsi_port *port, struct hsi_board_info *info)
 		pr_err("hsi: failed to register client: %s\n", info->name);
 		put_device(&cl->device);
 	}
+
+	return cl;
 }
+EXPORT_SYMBOL_GPL(hsi_new_client);
 
 static void hsi_scan_board_info(struct hsi_controller *hsi)
 {
@@ -101,12 +131,209 @@ static void hsi_scan_board_info(struct hsi_controller *hsi)
 		}
 }
 
-static int hsi_remove_client(struct device *dev, void *data __maybe_unused)
+#ifdef CONFIG_OF
+static struct hsi_board_info hsi_char_dev_info = {
+	.name = "hsi_char",
+};
+
+static int hsi_of_property_parse_mode(struct device_node *client, char *name,
+				      unsigned int *result)
+{
+	const char *mode;
+	int err;
+
+	err = of_property_read_string(client, name, &mode);
+	if (err < 0)
+		return err;
+
+	if (strcmp(mode, "stream") == 0)
+		*result = HSI_MODE_STREAM;
+	else if (strcmp(mode, "frame") == 0)
+		*result = HSI_MODE_FRAME;
+	else
+		return -EINVAL;
+
+	return 0;
+}
+
+static int hsi_of_property_parse_flow(struct device_node *client, char *name,
+				      unsigned int *result)
+{
+	const char *flow;
+	int err;
+
+	err = of_property_read_string(client, name, &flow);
+	if (err < 0)
+		return err;
+
+	if (strcmp(flow, "synchronized") == 0)
+		*result = HSI_FLOW_SYNC;
+	else if (strcmp(flow, "pipeline") == 0)
+		*result = HSI_FLOW_PIPE;
+	else
+		return -EINVAL;
+
+	return 0;
+}
+
+static int hsi_of_property_parse_arb_mode(struct device_node *client,
+					  char *name, unsigned int *result)
+{
+	const char *arb_mode;
+	int err;
+
+	err = of_property_read_string(client, name, &arb_mode);
+	if (err < 0)
+		return err;
+
+	if (strcmp(arb_mode, "round-robin") == 0)
+		*result = HSI_ARB_RR;
+	else if (strcmp(arb_mode, "priority") == 0)
+		*result = HSI_ARB_PRIO;
+	else
+		return -EINVAL;
+
+	return 0;
+}
+
+static void hsi_add_client_from_dt(struct hsi_port *port,
+						struct device_node *client)
+{
+	struct hsi_client *cl;
+	struct hsi_channel channel;
+	struct property *prop;
+	char name[32];
+	int length, cells, err, i, max_chan, mode;
+
+	cl = kzalloc(sizeof(*cl), GFP_KERNEL);
+	if (!cl)
+		return;
+
+	err = of_modalias_node(client, name, sizeof(name));
+	if (err)
+		goto err;
+
+	dev_set_name(&cl->device, "%s", name);
+
+	err = hsi_of_property_parse_mode(client, "hsi-mode", &mode);
+	if (err) {
+		err = hsi_of_property_parse_mode(client, "hsi-rx-mode",
+						 &cl->rx_cfg.mode);
+		if (err)
+			goto err;
+
+		err = hsi_of_property_parse_mode(client, "hsi-tx-mode",
+						 &cl->tx_cfg.mode);
+		if (err)
+			goto err;
+	} else {
+		cl->rx_cfg.mode = mode;
+		cl->tx_cfg.mode = mode;
+	}
+
+	err = of_property_read_u32(client, "hsi-speed-kbps",
+				   &cl->tx_cfg.speed);
+	if (err)
+		goto err;
+	cl->rx_cfg.speed = cl->tx_cfg.speed;
+
+	err = hsi_of_property_parse_flow(client, "hsi-flow",
+					 &cl->rx_cfg.flow);
+	if (err)
+		goto err;
+
+	err = hsi_of_property_parse_arb_mode(client, "hsi-arb-mode",
+					     &cl->rx_cfg.arb_mode);
+	if (err)
+		goto err;
+
+	prop = of_find_property(client, "hsi-channel-ids", &length);
+	if (!prop) {
+		err = -EINVAL;
+		goto err;
+	}
+
+	cells = length / sizeof(u32);
+
+	cl->rx_cfg.num_channels = cells;
+	cl->tx_cfg.num_channels = cells;
+
+	cl->rx_cfg.channels = kzalloc(cells * sizeof(channel), GFP_KERNEL);
+	if (!cl->rx_cfg.channels) {
+		err = -ENOMEM;
+		goto err;
+	}
+
+	cl->tx_cfg.channels = kzalloc(cells * sizeof(channel), GFP_KERNEL);
+	if (!cl->tx_cfg.channels) {
+		err = -ENOMEM;
+		goto err2;
+	}
+
+	max_chan = 0;
+	for (i = 0; i < cells; i++) {
+		err = of_property_read_u32_index(client, "hsi-channel-ids", i,
+						 &channel.id);
+		if (err)
+			goto err3;
+
+		err = of_property_read_string_index(client, "hsi-channel-names",
+						    i, &channel.name);
+		if (err)
+			channel.name = NULL;
+
+		if (channel.id > max_chan)
+			max_chan = channel.id;
+
+		cl->rx_cfg.channels[i] = channel;
+		cl->tx_cfg.channels[i] = channel;
+	}
+
+	cl->rx_cfg.num_hw_channels = max_chan + 1;
+	cl->tx_cfg.num_hw_channels = max_chan + 1;
+
+	cl->device.bus = &hsi_bus_type;
+	cl->device.parent = &port->device;
+	cl->device.release = hsi_client_release;
+	cl->device.of_node = client;
+
+	if (device_register(&cl->device) < 0) {
+		pr_err("hsi: failed to register client: %s\n", name);
+		put_device(&cl->device);
+		goto err3;
+	}
+
+	return;
+
+err3:
+	kfree(cl->tx_cfg.channels);
+err2:
+	kfree(cl->rx_cfg.channels);
+err:
+	kfree(cl);
+	pr_err("hsi client: missing or incorrect of property: err=%d\n", err);
+}
+
+void hsi_add_clients_from_dt(struct hsi_port *port, struct device_node *clients)
+{
+	struct device_node *child;
+
+	/* register hsi-char device */
+	hsi_new_client(port, &hsi_char_dev_info);
+
+	for_each_available_child_of_node(clients, child)
+		hsi_add_client_from_dt(port, child);
+}
+EXPORT_SYMBOL_GPL(hsi_add_clients_from_dt);
+#endif
+
+int hsi_remove_client(struct device *dev, void *data __maybe_unused)
 {
 	device_unregister(dev);
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(hsi_remove_client);
 
 static int hsi_remove_port(struct device *dev, void *data __maybe_unused)
 {
@@ -130,6 +357,16 @@ static void hsi_port_release(struct device *dev)
 }
 
 /**
+ * hsi_unregister_port - Unregister an HSI port
+ * @port: The HSI port to unregister
+ */
+void hsi_port_unregister_clients(struct hsi_port *port)
+{
+	device_for_each_child(&port->device, NULL, hsi_remove_client);
+}
+EXPORT_SYMBOL_GPL(hsi_port_unregister_clients);
+
+/**
  * hsi_unregister_controller - Unregister an HSI controller
  * @hsi: The HSI controller to register
  */
@@ -472,7 +709,7 @@ int hsi_unregister_port_event(struct hsi_client *cl)
 EXPORT_SYMBOL_GPL(hsi_unregister_port_event);
 
 /**
- * hsi_event -Notifies clients about port events
+ * hsi_event - Notifies clients about port events
  * @port: Port where the event occurred
  * @event: The event type
  *
@@ -492,6 +729,32 @@ int hsi_event(struct hsi_port *port, unsigned long event)
 }
 EXPORT_SYMBOL_GPL(hsi_event);
 
+/**
+ * hsi_get_channel_id_by_name - acquire channel id by channel name
+ * @cl: HSI client, which uses the channel
+ * @name: name the channel is known under
+ *
+ * Clients can call this function to get the hsi channel ids similar to
+ * requesting IRQs or GPIOs by name. This function assumes the same
+ * channel configuration is used for RX and TX.
+ *
+ * Returns -errno on error or channel id on success.
+ */
+int hsi_get_channel_id_by_name(struct hsi_client *cl, char *name)
+{
+	int i;
+
+	if (!cl->rx_cfg.channels)
+		return -ENOENT;
+
+	for (i = 0; i < cl->rx_cfg.num_channels; i++)
+		if (!strcmp(cl->rx_cfg.channels[i].name, name))
+			return cl->rx_cfg.channels[i].id;
+
+	return -ENXIO;
+}
+EXPORT_SYMBOL_GPL(hsi_get_channel_id_by_name);
+
 static int __init hsi_init(void)
 {
 	return bus_register(&hsi_bus_type);
diff --git a/drivers/hv/channel.c b/drivers/hv/channel.c
index 602ca86a648..284cf66489f 100644
--- a/drivers/hv/channel.c
+++ b/drivers/hv/channel.c
@@ -471,18 +471,26 @@ int vmbus_teardown_gpadl(struct vmbus_channel *channel, u32 gpadl_handle)
 }
 EXPORT_SYMBOL_GPL(vmbus_teardown_gpadl);
 
+static void reset_channel_cb(void *arg)
+{
+	struct vmbus_channel *channel = arg;
+
+	channel->onchannel_callback = NULL;
+}
+
 static void vmbus_close_internal(struct vmbus_channel *channel)
 {
 	struct vmbus_channel_close_channel *msg;
 	int ret;
-	unsigned long flags;
 
 	channel->state = CHANNEL_OPEN_STATE;
 	channel->sc_creation_callback = NULL;
 	/* Stop callback and cancel the timer asap */
-	spin_lock_irqsave(&channel->inbound_lock, flags);
-	channel->onchannel_callback = NULL;
-	spin_unlock_irqrestore(&channel->inbound_lock, flags);
+	if (channel->target_cpu != smp_processor_id())
+		smp_call_function_single(channel->target_cpu, reset_channel_cb,
+					 channel, true);
+	else
+		reset_channel_cb(channel);
 
 	/* Send a closing message */
 
@@ -674,8 +682,7 @@ int vmbus_sendpacket_multipagebuffer(struct vmbus_channel *channel,
 	u32 pfncount = NUM_PAGES_SPANNED(multi_pagebuffer->offset,
 					 multi_pagebuffer->len);
 
-
-	if ((pfncount < 0) || (pfncount > MAX_MULTIPAGE_BUFFER_COUNT))
+	if (pfncount > MAX_MULTIPAGE_BUFFER_COUNT)
 		return -EINVAL;
 
 	/*
diff --git a/drivers/hv/channel_mgmt.c b/drivers/hv/channel_mgmt.c
index fa920469bf1..6c8b032cacb 100644
--- a/drivers/hv/channel_mgmt.c
+++ b/drivers/hv/channel_mgmt.c
@@ -149,6 +149,7 @@ static struct vmbus_channel *alloc_channel(void)
 	spin_lock_init(&channel->sc_lock);
 
 	INIT_LIST_HEAD(&channel->sc_list);
+	INIT_LIST_HEAD(&channel->percpu_list);
 
 	channel->controlwq = create_workqueue("hv_vmbus_ctl");
 	if (!channel->controlwq) {
@@ -188,7 +189,20 @@ static void free_channel(struct vmbus_channel *channel)
 	queue_work(vmbus_connection.work_queue, &channel->work);
 }
 
+static void percpu_channel_enq(void *arg)
+{
+	struct vmbus_channel *channel = arg;
+	int cpu = smp_processor_id();
+
+	list_add_tail(&channel->percpu_list, &hv_context.percpu_list[cpu]);
+}
 
+static void percpu_channel_deq(void *arg)
+{
+	struct vmbus_channel *channel = arg;
+
+	list_del(&channel->percpu_list);
+}
 
 /*
  * vmbus_process_rescind_offer -
@@ -210,6 +224,12 @@ static void vmbus_process_rescind_offer(struct work_struct *work)
 	msg.header.msgtype = CHANNELMSG_RELID_RELEASED;
 	vmbus_post_msg(&msg, sizeof(struct vmbus_channel_relid_released));
 
+	if (channel->target_cpu != smp_processor_id())
+		smp_call_function_single(channel->target_cpu,
+					 percpu_channel_deq, channel, true);
+	else
+		percpu_channel_deq(channel);
+
 	if (channel->primary_channel == NULL) {
 		spin_lock_irqsave(&vmbus_connection.channel_lock, flags);
 		list_del(&channel->listentry);
@@ -245,6 +265,7 @@ static void vmbus_process_offer(struct work_struct *work)
 							work);
 	struct vmbus_channel *channel;
 	bool fnew = true;
+	bool enq = false;
 	int ret;
 	unsigned long flags;
 
@@ -264,12 +285,22 @@ static void vmbus_process_offer(struct work_struct *work)
 		}
 	}
 
-	if (fnew)
+	if (fnew) {
 		list_add_tail(&newchannel->listentry,
 			      &vmbus_connection.chn_list);
+		enq = true;
+	}
 
 	spin_unlock_irqrestore(&vmbus_connection.channel_lock, flags);
 
+	if (enq) {
+		if (newchannel->target_cpu != smp_processor_id())
+			smp_call_function_single(newchannel->target_cpu,
+						 percpu_channel_enq,
+						 newchannel, true);
+		else
+			percpu_channel_enq(newchannel);
+	}
 	if (!fnew) {
 		/*
 		 * Check to see if this is a sub-channel.
@@ -282,6 +313,14 @@ static void vmbus_process_offer(struct work_struct *work)
 			spin_lock_irqsave(&channel->sc_lock, flags);
 			list_add_tail(&newchannel->sc_list, &channel->sc_list);
 			spin_unlock_irqrestore(&channel->sc_lock, flags);
+
+			if (newchannel->target_cpu != smp_processor_id())
+				smp_call_function_single(newchannel->target_cpu,
+							 percpu_channel_enq,
+							 newchannel, true);
+			else
+				percpu_channel_enq(newchannel);
+
 			newchannel->state = CHANNEL_OPEN_STATE;
 			if (channel->sc_creation_callback != NULL)
 				channel->sc_creation_callback(newchannel);
@@ -365,7 +404,7 @@ static u32  next_vp;
  * performance critical channels (IDE, SCSI and Network) will be uniformly
  * distributed across all available CPUs.
  */
-static u32 get_vp_index(uuid_le *type_guid)
+static void init_vp_index(struct vmbus_channel *channel, uuid_le *type_guid)
 {
 	u32 cur_cpu;
 	int i;
@@ -387,10 +426,13 @@ static u32 get_vp_index(uuid_le *type_guid)
 		 * Also if the channel is not a performance critical
 		 * channel, bind it to cpu 0.
 		 */
-		return 0;
+		channel->target_cpu = 0;
+		channel->target_vp = 0;
+		return;
 	}
 	cur_cpu = (++next_vp % max_cpus);
-	return hv_context.vp_index[cur_cpu];
+	channel->target_cpu = cur_cpu;
+	channel->target_vp = hv_context.vp_index[cur_cpu];
 }
 
 /*
@@ -438,7 +480,7 @@ static void vmbus_onoffer(struct vmbus_channel_message_header *hdr)
 				offer->connection_id;
 	}
 
-	newchannel->target_vp = get_vp_index(&offer->offer.if_type);
+	init_vp_index(newchannel, &offer->offer.if_type);
 
 	memcpy(&newchannel->offermsg, offer,
 	       sizeof(struct vmbus_channel_offer_channel));
diff --git a/drivers/hv/connection.c b/drivers/hv/connection.c
index f2d7bf90c9f..e84f4526eb3 100644
--- a/drivers/hv/connection.c
+++ b/drivers/hv/connection.c
@@ -55,6 +55,9 @@ static __u32 vmbus_get_next_version(__u32 current_version)
 	case (VERSION_WIN8):
 		return VERSION_WIN7;
 
+	case (VERSION_WIN8_1):
+		return VERSION_WIN8;
+
 	case (VERSION_WS2008):
 	default:
 		return VERSION_INVAL;
@@ -77,7 +80,7 @@ static int vmbus_negotiate_version(struct vmbus_channel_msginfo *msginfo,
 	msg->interrupt_page = virt_to_phys(vmbus_connection.int_page);
 	msg->monitor_page1 = virt_to_phys(vmbus_connection.monitor_pages[0]);
 	msg->monitor_page2 = virt_to_phys(vmbus_connection.monitor_pages[1]);
-	if (version == VERSION_WIN8)
+	if (version == VERSION_WIN8_1)
 		msg->target_vcpu = hv_context.vp_index[smp_processor_id()];
 
 	/*
@@ -221,8 +224,8 @@ cleanup:
 		vmbus_connection.int_page = NULL;
 	}
 
-	free_pages((unsigned long)vmbus_connection.monitor_pages[0], 1);
-	free_pages((unsigned long)vmbus_connection.monitor_pages[1], 1);
+	free_pages((unsigned long)vmbus_connection.monitor_pages[0], 0);
+	free_pages((unsigned long)vmbus_connection.monitor_pages[1], 0);
 	vmbus_connection.monitor_pages[0] = NULL;
 	vmbus_connection.monitor_pages[1] = NULL;
 
@@ -231,6 +234,28 @@ cleanup:
 	return ret;
 }
 
+/*
+ * Map the given relid to the corresponding channel based on the
+ * per-cpu list of channels that have been affinitized to this CPU.
+ * This will be used in the channel callback path as we can do this
+ * mapping in a lock-free fashion.
+ */
+static struct vmbus_channel *pcpu_relid2channel(u32 relid)
+{
+	struct vmbus_channel *channel;
+	struct vmbus_channel *found_channel  = NULL;
+	int cpu = smp_processor_id();
+	struct list_head *pcpu_head = &hv_context.percpu_list[cpu];
+
+	list_for_each_entry(channel, pcpu_head, percpu_list) {
+		if (channel->offermsg.child_relid == relid) {
+			found_channel = channel;
+			break;
+		}
+	}
+
+	return found_channel;
+}
 
 /*
  * relid2channel - Get the channel object given its
@@ -274,7 +299,6 @@ struct vmbus_channel *relid2channel(u32 relid)
 static void process_chn_event(u32 relid)
 {
 	struct vmbus_channel *channel;
-	unsigned long flags;
 	void *arg;
 	bool read_state;
 	u32 bytes_to_read;
@@ -283,7 +307,7 @@ static void process_chn_event(u32 relid)
 	 * Find the channel based on this relid and invokes the
 	 * channel callback to process the event
 	 */
-	channel = relid2channel(relid);
+	channel = pcpu_relid2channel(relid);
 
 	if (!channel) {
 		pr_err("channel not found for relid - %u\n", relid);
@@ -293,13 +317,12 @@ static void process_chn_event(u32 relid)
 	/*
 	 * A channel once created is persistent even when there
 	 * is no driver handling the device. An unloading driver
-	 * sets the onchannel_callback to NULL under the
-	 * protection of the channel inbound_lock. Thus, checking
-	 * and invoking the driver specific callback takes care of
-	 * orderly unloading of the driver.
+	 * sets the onchannel_callback to NULL on the same CPU
+	 * as where this interrupt is handled (in an interrupt context).
+	 * Thus, checking and invoking the driver specific callback takes
+	 * care of orderly unloading of the driver.
 	 */
 
-	spin_lock_irqsave(&channel->inbound_lock, flags);
 	if (channel->onchannel_callback != NULL) {
 		arg = channel->channel_callback_context;
 		read_state = channel->batched_reading;
@@ -324,7 +347,6 @@ static void process_chn_event(u32 relid)
 		pr_err("no channel callback for relid - %u\n", relid);
 	}
 
-	spin_unlock_irqrestore(&channel->inbound_lock, flags);
 }
 
 /*
diff --git a/drivers/hv/hv.c b/drivers/hv/hv.c
index bcb49502c3b..edfc8488cb0 100644
--- a/drivers/hv/hv.c
+++ b/drivers/hv/hv.c
@@ -383,6 +383,8 @@ void hv_synic_init(void *arg)
 	 */
 	rdmsrl(HV_X64_MSR_VP_INDEX, vp_index);
 	hv_context.vp_index[cpu] = (u32)vp_index;
+
+	INIT_LIST_HEAD(&hv_context.percpu_list[cpu]);
 	return;
 }
 
diff --git a/drivers/hv/hv_balloon.c b/drivers/hv/hv_balloon.c
index 7e6d78dc943..5e90c5d771a 100644
--- a/drivers/hv/hv_balloon.c
+++ b/drivers/hv/hv_balloon.c
@@ -19,6 +19,7 @@
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #include <linux/kernel.h>
+#include <linux/jiffies.h>
 #include <linux/mman.h>
 #include <linux/delay.h>
 #include <linux/init.h>
@@ -459,6 +460,11 @@ static bool do_hot_add;
  */
 static uint pressure_report_delay = 45;
 
+/*
+ * The last time we posted a pressure report to host.
+ */
+static unsigned long last_post_time;
+
 module_param(hot_add, bool, (S_IRUGO | S_IWUSR));
 MODULE_PARM_DESC(hot_add, "If set attempt memory hot_add");
 
@@ -542,6 +548,7 @@ struct hv_dynmem_device {
 
 static struct hv_dynmem_device dm_device;
 
+static void post_status(struct hv_dynmem_device *dm);
 #ifdef CONFIG_MEMORY_HOTPLUG
 
 static void hv_bring_pgs_online(unsigned long start_pfn, unsigned long size)
@@ -612,7 +619,7 @@ static void hv_mem_hot_add(unsigned long start, unsigned long size,
 		 * have not been "onlined" within the allowed time.
 		 */
 		wait_for_completion_timeout(&dm_device.ol_waitevent, 5*HZ);
-
+		post_status(&dm_device);
 	}
 
 	return;
@@ -951,11 +958,17 @@ static void post_status(struct hv_dynmem_device *dm)
 {
 	struct dm_status status;
 	struct sysinfo val;
+	unsigned long now = jiffies;
+	unsigned long last_post = last_post_time;
 
 	if (pressure_report_delay > 0) {
 		--pressure_report_delay;
 		return;
 	}
+
+	if (!time_after(now, (last_post_time + HZ)))
+		return;
+
 	si_meminfo(&val);
 	memset(&status, 0, sizeof(struct dm_status));
 	status.hdr.type = DM_STATUS_REPORT;
@@ -983,6 +996,14 @@ static void post_status(struct hv_dynmem_device *dm)
 	if (status.hdr.trans_id != atomic_read(&trans_id))
 		return;
 
+	/*
+	 * If the last post time that we sampled has changed,
+	 * we have raced, don't post the status.
+	 */
+	if (last_post != last_post_time)
+		return;
+
+	last_post_time = jiffies;
 	vmbus_sendpacket(dm->dev->channel, &status,
 				sizeof(struct dm_status),
 				(unsigned long)NULL,
@@ -1117,7 +1138,7 @@ static void balloon_up(struct work_struct *dummy)
 
 			if (ret == -EAGAIN)
 				msleep(20);
-
+			post_status(&dm_device);
 		} while (ret == -EAGAIN);
 
 		if (ret) {
@@ -1144,8 +1165,10 @@ static void balloon_down(struct hv_dynmem_device *dm,
 	struct dm_unballoon_response resp;
 	int i;
 
-	for (i = 0; i < range_count; i++)
+	for (i = 0; i < range_count; i++) {
 		free_balloon_pages(dm, &range_array[i]);
+		post_status(&dm_device);
+	}
 
 	if (req->more_pages == 1)
 		return;
diff --git a/drivers/hv/hyperv_vmbus.h b/drivers/hv/hyperv_vmbus.h
index 860134da803..18d1a8404cb 100644
--- a/drivers/hv/hyperv_vmbus.h
+++ b/drivers/hv/hyperv_vmbus.h
@@ -510,6 +510,11 @@ struct hv_context {
 	 * basis.
 	 */
 	struct tasklet_struct *event_dpc[NR_CPUS];
+	/*
+	 * To optimize the mapping of relid to channel, maintain
+	 * per-cpu list of the channels based on their CPU affinity.
+	 */
+	struct list_head percpu_list[NR_CPUS];
 };
 
 extern struct hv_context hv_context;
diff --git a/drivers/hwmon/Kconfig b/drivers/hwmon/Kconfig
index bc196f49ec5..00343166feb 100644
--- a/drivers/hwmon/Kconfig
+++ b/drivers/hwmon/Kconfig
@@ -1053,7 +1053,7 @@ config SENSORS_PC87427
 
 config SENSORS_NTC_THERMISTOR
 	tristate "NTC thermistor support"
-	depends on (!OF && !IIO) || (OF && IIO)
+	depends on !OF || IIO=n || IIO
 	help
 	  This driver supports NTC thermistors sensor reading and its
 	  interpretation. The driver can also monitor the temperature and
@@ -1065,6 +1065,16 @@ config SENSORS_NTC_THERMISTOR
 	  This driver can also be built as a module.  If so, the module
 	  will be called ntc-thermistor.
 
+config SENSORS_NCT6683
+	tristate "Nuvoton NCT6683D"
+	depends on !PPC
+	help
+	  If you say yes here you get support for the hardware monitoring
+	  functionality of the Nuvoton NCT6683D eSIO chip.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called nct6683.
+
 config SENSORS_NCT6775
 	tristate "Nuvoton NCT6775F and compatibles"
 	depends on !PPC
diff --git a/drivers/hwmon/Makefile b/drivers/hwmon/Makefile
index c48f9873ac7..11798ad7e80 100644
--- a/drivers/hwmon/Makefile
+++ b/drivers/hwmon/Makefile
@@ -114,6 +114,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_NCT6683)	+= nct6683.o
 obj-$(CONFIG_SENSORS_NCT6775)	+= nct6775.o
 obj-$(CONFIG_SENSORS_NTC_THERMISTOR)	+= ntc_thermistor.o
 obj-$(CONFIG_SENSORS_PC87360)	+= pc87360.o
diff --git a/drivers/hwmon/adm1029.c b/drivers/hwmon/adm1029.c
index d19c790e410..78339e880bd 100644
--- a/drivers/hwmon/adm1029.c
+++ b/drivers/hwmon/adm1029.c
@@ -1,7 +1,7 @@
 /*
  * adm1029.c - Part of lm_sensors, Linux kernel modules for hardware monitoring
  *
- * Copyright (C) 2006 Corentin LABBE <corentin.labbe@geomatys.fr>
+ * Copyright (C) 2006 Corentin LABBE <clabbe.montjoie@gmail.com>
  *
  * Based on LM83 Driver by Jean Delvare <jdelvare@suse.de>
  *
@@ -449,6 +449,6 @@ static struct adm1029_data *adm1029_update_device(struct device *dev)
 
 module_i2c_driver(adm1029_driver);
 
-MODULE_AUTHOR("Corentin LABBE <corentin.labbe@geomatys.fr>");
+MODULE_AUTHOR("Corentin LABBE <clabbe.montjoie@gmail.com>");
 MODULE_DESCRIPTION("adm1029 driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/hwmon/coretemp.c b/drivers/hwmon/coretemp.c
index 6d02e3b0637..d76f0b70c6e 100644
--- a/drivers/hwmon/coretemp.c
+++ b/drivers/hwmon/coretemp.c
@@ -365,12 +365,12 @@ static int get_tjmax(struct cpuinfo_x86 *c, u32 id, struct device *dev)
 		if (cpu_has_tjmax(c))
 			dev_warn(dev, "Unable to read TjMax from CPU %u\n", id);
 	} else {
-		val = (eax >> 16) & 0x7f;
+		val = (eax >> 16) & 0xff;
 		/*
 		 * If the TjMax is not plausible, an assumption
 		 * will be used
 		 */
-		if (val >= 85) {
+		if (val) {
 			dev_dbg(dev, "TjMax is %d degrees C\n", val);
 			return val * 1000;
 		}
diff --git a/drivers/hwmon/emc1403.c b/drivers/hwmon/emc1403.c
index 90ec1173b8a..a37b2204a41 100644
--- a/drivers/hwmon/emc1403.c
+++ b/drivers/hwmon/emc1403.c
@@ -18,9 +18,6 @@
  * with this program; if not, write to the Free Software Foundation, Inc.,
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- *
- * TODO
- *	-	cache alarm and critical limit registers
  */
 
 #include <linux/module.h>
@@ -32,22 +29,18 @@
 #include <linux/err.h>
 #include <linux/sysfs.h>
 #include <linux/mutex.h>
-#include <linux/jiffies.h>
+#include <linux/regmap.h>
 
 #define THERMAL_PID_REG		0xfd
 #define THERMAL_SMSC_ID_REG	0xfe
 #define THERMAL_REVISION_REG	0xff
 
+enum emc1403_chip { emc1402, emc1403, emc1404 };
+
 struct thermal_data {
-	struct i2c_client *client;
-	const struct attribute_group *groups[3];
+	struct regmap *regmap;
 	struct mutex mutex;
-	/*
-	 * Cache the hyst value so we don't keep re-reading it. In theory
-	 * we could cache it forever as nobody else should be writing it.
-	 */
-	u8 cached_hyst;
-	unsigned long hyst_valid;
+	const struct attribute_group *groups[4];
 };
 
 static ssize_t show_temp(struct device *dev,
@@ -55,12 +48,13 @@ static ssize_t show_temp(struct device *dev,
 {
 	struct sensor_device_attribute *sda = to_sensor_dev_attr(attr);
 	struct thermal_data *data = dev_get_drvdata(dev);
+	unsigned int val;
 	int retval;
 
-	retval = i2c_smbus_read_byte_data(data->client, sda->index);
+	retval = regmap_read(data->regmap, sda->index, &val);
 	if (retval < 0)
 		return retval;
-	return sprintf(buf, "%d000\n", retval);
+	return sprintf(buf, "%d000\n", val);
 }
 
 static ssize_t show_bit(struct device *dev,
@@ -68,12 +62,13 @@ static ssize_t show_bit(struct device *dev,
 {
 	struct sensor_device_attribute_2 *sda = to_sensor_dev_attr_2(attr);
 	struct thermal_data *data = dev_get_drvdata(dev);
+	unsigned int val;
 	int retval;
 
-	retval = i2c_smbus_read_byte_data(data->client, sda->nr);
+	retval = regmap_read(data->regmap, sda->nr, &val);
 	if (retval < 0)
 		return retval;
-	return sprintf(buf, "%d\n", !!(retval & sda->index));
+	return sprintf(buf, "%d\n", !!(val & sda->index));
 }
 
 static ssize_t store_temp(struct device *dev,
@@ -86,8 +81,8 @@ static ssize_t store_temp(struct device *dev,
 
 	if (kstrtoul(buf, 10, &val))
 		return -EINVAL;
-	retval = i2c_smbus_write_byte_data(data->client, sda->index,
-					DIV_ROUND_CLOSEST(val, 1000));
+	retval = regmap_write(data->regmap, sda->index,
+			      DIV_ROUND_CLOSEST(val, 1000));
 	if (retval < 0)
 		return retval;
 	return count;
@@ -98,51 +93,51 @@ static ssize_t store_bit(struct device *dev,
 {
 	struct sensor_device_attribute_2 *sda = to_sensor_dev_attr_2(attr);
 	struct thermal_data *data = dev_get_drvdata(dev);
-	struct i2c_client *client = data->client;
 	unsigned long val;
 	int retval;
 
 	if (kstrtoul(buf, 10, &val))
 		return -EINVAL;
 
-	mutex_lock(&data->mutex);
-	retval = i2c_smbus_read_byte_data(client, sda->nr);
+	retval = regmap_update_bits(data->regmap, sda->nr, sda->index,
+				    val ? sda->index : 0);
 	if (retval < 0)
-		goto fail;
-
-	retval &= ~sda->index;
-	if (val)
-		retval |= sda->index;
-
-	retval = i2c_smbus_write_byte_data(client, sda->index, retval);
-	if (retval == 0)
-		retval = count;
-fail:
-	mutex_unlock(&data->mutex);
-	return retval;
+		return retval;
+	return count;
 }
 
-static ssize_t show_hyst(struct device *dev,
-			struct device_attribute *attr, char *buf)
+static ssize_t show_hyst_common(struct device *dev,
+				struct device_attribute *attr, char *buf,
+				bool is_min)
 {
 	struct sensor_device_attribute *sda = to_sensor_dev_attr(attr);
 	struct thermal_data *data = dev_get_drvdata(dev);
-	struct i2c_client *client = data->client;
+	struct regmap *regmap = data->regmap;
+	unsigned int limit;
+	unsigned int hyst;
 	int retval;
-	int hyst;
 
-	retval = i2c_smbus_read_byte_data(client, sda->index);
+	retval = regmap_read(regmap, sda->index, &limit);
 	if (retval < 0)
 		return retval;
 
-	if (time_after(jiffies, data->hyst_valid)) {
-		hyst = i2c_smbus_read_byte_data(client, 0x21);
-		if (hyst < 0)
-			return retval;
-		data->cached_hyst = hyst;
-		data->hyst_valid = jiffies + HZ;
-	}
-	return sprintf(buf, "%d000\n", retval - data->cached_hyst);
+	retval = regmap_read(regmap, 0x21, &hyst);
+	if (retval < 0)
+		return retval;
+
+	return sprintf(buf, "%d000\n", is_min ? limit + hyst : limit - hyst);
+}
+
+static ssize_t show_hyst(struct device *dev,
+			 struct device_attribute *attr, char *buf)
+{
+	return show_hyst_common(dev, attr, buf, false);
+}
+
+static ssize_t show_min_hyst(struct device *dev,
+			     struct device_attribute *attr, char *buf)
+{
+	return show_hyst_common(dev, attr, buf, true);
 }
 
 static ssize_t store_hyst(struct device *dev,
@@ -150,7 +145,8 @@ static ssize_t store_hyst(struct device *dev,
 {
 	struct sensor_device_attribute *sda = to_sensor_dev_attr(attr);
 	struct thermal_data *data = dev_get_drvdata(dev);
-	struct i2c_client *client = data->client;
+	struct regmap *regmap = data->regmap;
+	unsigned int limit;
 	int retval;
 	int hyst;
 	unsigned long val;
@@ -159,23 +155,15 @@ static ssize_t store_hyst(struct device *dev,
 		return -EINVAL;
 
 	mutex_lock(&data->mutex);
-	retval = i2c_smbus_read_byte_data(client, sda->index);
+	retval = regmap_read(regmap, sda->index, &limit);
 	if (retval < 0)
 		goto fail;
 
-	hyst = val - retval * 1000;
-	hyst = DIV_ROUND_CLOSEST(hyst, 1000);
-	if (hyst < 0 || hyst > 255) {
-		retval = -ERANGE;
-		goto fail;
-	}
-
-	retval = i2c_smbus_write_byte_data(client, 0x21, hyst);
-	if (retval == 0) {
+	hyst = limit * 1000 - val;
+	hyst = clamp_val(DIV_ROUND_CLOSEST(hyst, 1000), 0, 255);
+	retval = regmap_write(regmap, 0x21, hyst);
+	if (retval == 0)
 		retval = count;
-		data->cached_hyst = hyst;
-		data->hyst_valid = jiffies + HZ;
-	}
 fail:
 	mutex_unlock(&data->mutex);
 	return retval;
@@ -198,6 +186,8 @@ static SENSOR_DEVICE_ATTR_2(temp1_max_alarm, S_IRUGO,
 	show_bit, NULL, 0x35, 0x01);
 static SENSOR_DEVICE_ATTR_2(temp1_crit_alarm, S_IRUGO,
 	show_bit, NULL, 0x37, 0x01);
+static SENSOR_DEVICE_ATTR(temp1_min_hyst, S_IRUGO, show_min_hyst, NULL, 0x06);
+static SENSOR_DEVICE_ATTR(temp1_max_hyst, S_IRUGO, show_hyst, NULL, 0x05);
 static SENSOR_DEVICE_ATTR(temp1_crit_hyst, S_IRUGO | S_IWUSR,
 	show_hyst, store_hyst, 0x20);
 
@@ -208,14 +198,16 @@ static SENSOR_DEVICE_ATTR(temp2_max, S_IRUGO | S_IWUSR,
 static SENSOR_DEVICE_ATTR(temp2_crit, S_IRUGO | S_IWUSR,
 	show_temp, store_temp, 0x19);
 static SENSOR_DEVICE_ATTR(temp2_input, S_IRUGO, show_temp, NULL, 0x01);
+static SENSOR_DEVICE_ATTR_2(temp2_fault, S_IRUGO, show_bit, NULL, 0x1b, 0x02);
 static SENSOR_DEVICE_ATTR_2(temp2_min_alarm, S_IRUGO,
 	show_bit, NULL, 0x36, 0x02);
 static SENSOR_DEVICE_ATTR_2(temp2_max_alarm, S_IRUGO,
 	show_bit, NULL, 0x35, 0x02);
 static SENSOR_DEVICE_ATTR_2(temp2_crit_alarm, S_IRUGO,
 	show_bit, NULL, 0x37, 0x02);
-static SENSOR_DEVICE_ATTR(temp2_crit_hyst, S_IRUGO | S_IWUSR,
-	show_hyst, store_hyst, 0x19);
+static SENSOR_DEVICE_ATTR(temp2_min_hyst, S_IRUGO, show_min_hyst, NULL, 0x08);
+static SENSOR_DEVICE_ATTR(temp2_max_hyst, S_IRUGO, show_hyst, NULL, 0x07);
+static SENSOR_DEVICE_ATTR(temp2_crit_hyst, S_IRUGO, show_hyst, NULL, 0x19);
 
 static SENSOR_DEVICE_ATTR(temp3_min, S_IRUGO | S_IWUSR,
 	show_temp, store_temp, 0x16);
@@ -224,14 +216,16 @@ static SENSOR_DEVICE_ATTR(temp3_max, S_IRUGO | S_IWUSR,
 static SENSOR_DEVICE_ATTR(temp3_crit, S_IRUGO | S_IWUSR,
 	show_temp, store_temp, 0x1A);
 static SENSOR_DEVICE_ATTR(temp3_input, S_IRUGO, show_temp, NULL, 0x23);
+static SENSOR_DEVICE_ATTR_2(temp3_fault, S_IRUGO, show_bit, NULL, 0x1b, 0x04);
 static SENSOR_DEVICE_ATTR_2(temp3_min_alarm, S_IRUGO,
 	show_bit, NULL, 0x36, 0x04);
 static SENSOR_DEVICE_ATTR_2(temp3_max_alarm, S_IRUGO,
 	show_bit, NULL, 0x35, 0x04);
 static SENSOR_DEVICE_ATTR_2(temp3_crit_alarm, S_IRUGO,
 	show_bit, NULL, 0x37, 0x04);
-static SENSOR_DEVICE_ATTR(temp3_crit_hyst, S_IRUGO | S_IWUSR,
-	show_hyst, store_hyst, 0x1A);
+static SENSOR_DEVICE_ATTR(temp3_min_hyst, S_IRUGO, show_min_hyst, NULL, 0x16);
+static SENSOR_DEVICE_ATTR(temp3_max_hyst, S_IRUGO, show_hyst, NULL, 0x15);
+static SENSOR_DEVICE_ATTR(temp3_crit_hyst, S_IRUGO, show_hyst, NULL, 0x1A);
 
 static SENSOR_DEVICE_ATTR(temp4_min, S_IRUGO | S_IWUSR,
 	show_temp, store_temp, 0x2D);
@@ -240,44 +234,66 @@ static SENSOR_DEVICE_ATTR(temp4_max, S_IRUGO | S_IWUSR,
 static SENSOR_DEVICE_ATTR(temp4_crit, S_IRUGO | S_IWUSR,
 	show_temp, store_temp, 0x30);
 static SENSOR_DEVICE_ATTR(temp4_input, S_IRUGO, show_temp, NULL, 0x2A);
+static SENSOR_DEVICE_ATTR_2(temp4_fault, S_IRUGO, show_bit, NULL, 0x1b, 0x08);
 static SENSOR_DEVICE_ATTR_2(temp4_min_alarm, S_IRUGO,
 	show_bit, NULL, 0x36, 0x08);
 static SENSOR_DEVICE_ATTR_2(temp4_max_alarm, S_IRUGO,
 	show_bit, NULL, 0x35, 0x08);
 static SENSOR_DEVICE_ATTR_2(temp4_crit_alarm, S_IRUGO,
 	show_bit, NULL, 0x37, 0x08);
-static SENSOR_DEVICE_ATTR(temp4_crit_hyst, S_IRUGO | S_IWUSR,
-	show_hyst, store_hyst, 0x30);
+static SENSOR_DEVICE_ATTR(temp4_min_hyst, S_IRUGO, show_min_hyst, NULL, 0x2D);
+static SENSOR_DEVICE_ATTR(temp4_max_hyst, S_IRUGO, show_hyst, NULL, 0x2C);
+static SENSOR_DEVICE_ATTR(temp4_crit_hyst, S_IRUGO, show_hyst, NULL, 0x30);
 
 static SENSOR_DEVICE_ATTR_2(power_state, S_IRUGO | S_IWUSR,
 	show_bit, store_bit, 0x03, 0x40);
 
-static struct attribute *emc1403_attrs[] = {
+static struct attribute *emc1402_attrs[] = {
 	&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_input.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,
+	&sensor_dev_attr_temp1_min_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp1_max_hyst.dev_attr.attr,
 	&sensor_dev_attr_temp1_crit_hyst.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_input.dev_attr.attr,
+	&sensor_dev_attr_temp2_min_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp2_max_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp2_crit_hyst.dev_attr.attr,
+
+	&sensor_dev_attr_power_state.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group emc1402_group = {
+		.attrs = emc1402_attrs,
+};
+
+static struct attribute *emc1403_attrs[] = {
+	&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,
+
+	&sensor_dev_attr_temp2_fault.dev_attr.attr,
 	&sensor_dev_attr_temp2_min_alarm.dev_attr.attr,
 	&sensor_dev_attr_temp2_max_alarm.dev_attr.attr,
 	&sensor_dev_attr_temp2_crit_alarm.dev_attr.attr,
-	&sensor_dev_attr_temp2_crit_hyst.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_input.dev_attr.attr,
+	&sensor_dev_attr_temp3_fault.dev_attr.attr,
 	&sensor_dev_attr_temp3_min_alarm.dev_attr.attr,
 	&sensor_dev_attr_temp3_max_alarm.dev_attr.attr,
 	&sensor_dev_attr_temp3_crit_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp3_min_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp3_max_hyst.dev_attr.attr,
 	&sensor_dev_attr_temp3_crit_hyst.dev_attr.attr,
-	&sensor_dev_attr_power_state.dev_attr.attr,
 	NULL
 };
 
@@ -290,9 +306,12 @@ static struct attribute *emc1404_attrs[] = {
 	&sensor_dev_attr_temp4_max.dev_attr.attr,
 	&sensor_dev_attr_temp4_crit.dev_attr.attr,
 	&sensor_dev_attr_temp4_input.dev_attr.attr,
+	&sensor_dev_attr_temp4_fault.dev_attr.attr,
 	&sensor_dev_attr_temp4_min_alarm.dev_attr.attr,
 	&sensor_dev_attr_temp4_max_alarm.dev_attr.attr,
 	&sensor_dev_attr_temp4_crit_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp4_min_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp4_max_hyst.dev_attr.attr,
 	&sensor_dev_attr_temp4_crit_hyst.dev_attr.attr,
 	NULL
 };
@@ -301,6 +320,39 @@ static const struct attribute_group emc1404_group = {
 	.attrs = emc1404_attrs,
 };
 
+/*
+ * EMC14x2 uses a different register and different bits to report alarm and
+ * fault status. For simplicity, provide a separate attribute group for this
+ * chip series.
+ * Since we can not re-use the same attribute names, create a separate attribute
+ * array.
+ */
+static struct sensor_device_attribute_2 emc1402_alarms[] = {
+	SENSOR_ATTR_2(temp1_min_alarm, S_IRUGO, show_bit, NULL, 0x02, 0x20),
+	SENSOR_ATTR_2(temp1_max_alarm, S_IRUGO, show_bit, NULL, 0x02, 0x40),
+	SENSOR_ATTR_2(temp1_crit_alarm, S_IRUGO, show_bit, NULL, 0x02, 0x01),
+
+	SENSOR_ATTR_2(temp2_fault, S_IRUGO, show_bit, NULL, 0x02, 0x04),
+	SENSOR_ATTR_2(temp2_min_alarm, S_IRUGO, show_bit, NULL, 0x02, 0x08),
+	SENSOR_ATTR_2(temp2_max_alarm, S_IRUGO, show_bit, NULL, 0x02, 0x10),
+	SENSOR_ATTR_2(temp2_crit_alarm, S_IRUGO, show_bit, NULL, 0x02, 0x02),
+};
+
+static struct attribute *emc1402_alarm_attrs[] = {
+	&emc1402_alarms[0].dev_attr.attr,
+	&emc1402_alarms[1].dev_attr.attr,
+	&emc1402_alarms[2].dev_attr.attr,
+	&emc1402_alarms[3].dev_attr.attr,
+	&emc1402_alarms[4].dev_attr.attr,
+	&emc1402_alarms[5].dev_attr.attr,
+	&emc1402_alarms[6].dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group emc1402_alarm_group = {
+	.attrs = emc1402_alarm_attrs,
+};
+
 static int emc1403_detect(struct i2c_client *client,
 			struct i2c_board_info *info)
 {
@@ -313,9 +365,15 @@ static int emc1403_detect(struct i2c_client *client,
 
 	id = i2c_smbus_read_byte_data(client, THERMAL_PID_REG);
 	switch (id) {
+	case 0x20:
+		strlcpy(info->type, "emc1402", I2C_NAME_SIZE);
+		break;
 	case 0x21:
 		strlcpy(info->type, "emc1403", I2C_NAME_SIZE);
 		break;
+	case 0x22:
+		strlcpy(info->type, "emc1422", I2C_NAME_SIZE);
+		break;
 	case 0x23:
 		strlcpy(info->type, "emc1423", I2C_NAME_SIZE);
 		break;
@@ -330,12 +388,41 @@ static int emc1403_detect(struct i2c_client *client,
 	}
 
 	id = i2c_smbus_read_byte_data(client, THERMAL_REVISION_REG);
-	if (id != 0x01)
+	if (id < 0x01 || id > 0x04)
 		return -ENODEV;
 
 	return 0;
 }
 
+static bool emc1403_regmap_is_volatile(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case 0x00:	/* internal diode high byte */
+	case 0x01:	/* external diode 1 high byte */
+	case 0x02:	/* status */
+	case 0x10:	/* external diode 1 low byte */
+	case 0x1b:	/* external diode fault */
+	case 0x23:	/* external diode 2 high byte */
+	case 0x24:	/* external diode 2 low byte */
+	case 0x29:	/* internal diode low byte */
+	case 0x2a:	/* externl diode 3 high byte */
+	case 0x2b:	/* external diode 3 low byte */
+	case 0x35:	/* high limit status */
+	case 0x36:	/* low limit status */
+	case 0x37:	/* therm limit status */
+		return true;
+	default:
+		return false;
+	}
+}
+
+static struct regmap_config emc1403_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.cache_type = REGCACHE_RBTREE,
+	.volatile_reg = emc1403_regmap_is_volatile,
+};
+
 static int emc1403_probe(struct i2c_client *client,
 			const struct i2c_device_id *id)
 {
@@ -347,17 +434,27 @@ static int emc1403_probe(struct i2c_client *client,
 	if (data == NULL)
 		return -ENOMEM;
 
-	data->client = client;
+	data->regmap = devm_regmap_init_i2c(client, &emc1403_regmap_config);
+	if (IS_ERR(data->regmap))
+		return PTR_ERR(data->regmap);
+
 	mutex_init(&data->mutex);
-	data->hyst_valid = jiffies - 1;		/* Expired */
 
-	data->groups[0] = &emc1403_group;
-	if (id->driver_data)
-		data->groups[1] = &emc1404_group;
+	switch (id->driver_data) {
+	case emc1404:
+		data->groups[2] = &emc1404_group;
+	case emc1403:
+		data->groups[1] = &emc1403_group;
+	case emc1402:
+		data->groups[0] = &emc1402_group;
+	}
+
+	if (id->driver_data == emc1402)
+		data->groups[1] = &emc1402_alarm_group;
 
-	hwmon_dev = hwmon_device_register_with_groups(&client->dev,
-						      client->name, data,
-						      data->groups);
+	hwmon_dev = devm_hwmon_device_register_with_groups(&client->dev,
+							   client->name, data,
+							   data->groups);
 	if (IS_ERR(hwmon_dev))
 		return PTR_ERR(hwmon_dev);
 
@@ -366,14 +463,20 @@ static int emc1403_probe(struct i2c_client *client,
 }
 
 static const unsigned short emc1403_address_list[] = {
-	0x18, 0x29, 0x4c, 0x4d, I2C_CLIENT_END
+	0x18, 0x1c, 0x29, 0x4c, 0x4d, 0x5c, I2C_CLIENT_END
 };
 
+/* Last digit of chip name indicates number of channels */
 static const struct i2c_device_id emc1403_idtable[] = {
-	{ "emc1403", 0 },
-	{ "emc1404", 1 },
-	{ "emc1423", 0 },
-	{ "emc1424", 1 },
+	{ "emc1402", emc1402 },
+	{ "emc1403", emc1403 },
+	{ "emc1404", emc1404 },
+	{ "emc1412", emc1402 },
+	{ "emc1413", emc1403 },
+	{ "emc1414", emc1404 },
+	{ "emc1422", emc1402 },
+	{ "emc1423", emc1403 },
+	{ "emc1424", emc1404 },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, emc1403_idtable);
diff --git a/drivers/hwmon/f71805f.c b/drivers/hwmon/f71805f.c
index 32f5132c465..9e57b77ecd3 100644
--- a/drivers/hwmon/f71805f.c
+++ b/drivers/hwmon/f71805f.c
@@ -1387,10 +1387,8 @@ static int f71805f_probe(struct platform_device *pdev)
 
 	data = devm_kzalloc(&pdev->dev, sizeof(struct f71805f_data),
 			    GFP_KERNEL);
-	if (!data) {
-		pr_err("Out of memory\n");
+	if (!data)
 		return -ENOMEM;
-	}
 
 	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
 	if (!devm_request_region(&pdev->dev, res->start + ADDR_REG_OFFSET, 2,
diff --git a/drivers/hwmon/g762.c b/drivers/hwmon/g762.c
index b4b8b5bef71..98a8618d8fb 100644
--- a/drivers/hwmon/g762.c
+++ b/drivers/hwmon/g762.c
@@ -586,7 +586,7 @@ static int do_set_fan_startv(struct device *dev, unsigned long val)
  */
 
 #ifdef CONFIG_OF
-static struct of_device_id g762_dt_match[] = {
+static const struct of_device_id g762_dt_match[] = {
 	{ .compatible = "gmt,g762" },
 	{ .compatible = "gmt,g763" },
 	{ },
diff --git a/drivers/hwmon/gpio-fan.c b/drivers/hwmon/gpio-fan.c
index 73181be5b30..ba35e4d530b 100644
--- a/drivers/hwmon/gpio-fan.c
+++ b/drivers/hwmon/gpio-fan.c
@@ -482,7 +482,7 @@ static int gpio_fan_get_of_pdata(struct device *dev,
 	return 0;
 }
 
-static struct of_device_id of_gpio_fan_match[] = {
+static const struct of_device_id of_gpio_fan_match[] = {
 	{ .compatible = "gpio-fan", },
 	{},
 };
diff --git a/drivers/hwmon/ibmpex.c b/drivers/hwmon/ibmpex.c
index 20ab0fb8539..030e7ff589b 100644
--- a/drivers/hwmon/ibmpex.c
+++ b/drivers/hwmon/ibmpex.c
@@ -463,10 +463,8 @@ static void ibmpex_register_bmc(int iface, struct device *dev)
 	int err;
 
 	data = kzalloc(sizeof(*data), GFP_KERNEL);
-	if (!data) {
-		dev_err(dev, "Insufficient memory for BMC interface.\n");
+	if (!data)
 		return;
-	}
 
 	data->address.addr_type = IPMI_SYSTEM_INTERFACE_ADDR_TYPE;
 	data->address.channel = IPMI_BMC_CHANNEL;
diff --git a/drivers/hwmon/iio_hwmon.c b/drivers/hwmon/iio_hwmon.c
index 9fbb1b1fdff..14c82daab01 100644
--- a/drivers/hwmon/iio_hwmon.c
+++ b/drivers/hwmon/iio_hwmon.c
@@ -163,7 +163,7 @@ static int iio_hwmon_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id iio_hwmon_of_match[] = {
+static const struct of_device_id iio_hwmon_of_match[] = {
 	{ .compatible = "iio-hwmon", },
 	{ }
 };
diff --git a/drivers/hwmon/jc42.c b/drivers/hwmon/jc42.c
index 6013611e4f2..388f8bcd898 100644
--- a/drivers/hwmon/jc42.c
+++ b/drivers/hwmon/jc42.c
@@ -65,6 +65,7 @@ static const unsigned short normal_i2c[] = {
 /* Manufacturer IDs */
 #define ADT_MANID		0x11d4  /* Analog Devices */
 #define ATMEL_MANID		0x001f  /* Atmel */
+#define ATMEL_MANID2		0x1114	/* Atmel */
 #define MAX_MANID		0x004d  /* Maxim */
 #define IDT_MANID		0x00b3  /* IDT */
 #define MCP_MANID		0x0054  /* Microchip */
@@ -82,6 +83,9 @@ static const unsigned short normal_i2c[] = {
 #define AT30TS00_DEVID		0x8201
 #define AT30TS00_DEVID_MASK	0xffff
 
+#define AT30TSE004_DEVID	0x2200
+#define AT30TSE004_DEVID_MASK	0xffff
+
 /* IDT */
 #define TS3000B3_DEVID		0x2903  /* Also matches TSE2002B3 */
 #define TS3000B3_DEVID_MASK	0xffff
@@ -130,6 +134,9 @@ static const unsigned short normal_i2c[] = {
 #define STTS2002_DEVID		0x0300
 #define STTS2002_DEVID_MASK	0xffff
 
+#define STTS2004_DEVID		0x2201
+#define STTS2004_DEVID_MASK	0xffff
+
 #define STTS3000_DEVID		0x0200
 #define STTS3000_DEVID_MASK	0xffff
 
@@ -144,6 +151,7 @@ struct jc42_chips {
 static struct jc42_chips jc42_chips[] = {
 	{ ADT_MANID, ADT7408_DEVID, ADT7408_DEVID_MASK },
 	{ ATMEL_MANID, AT30TS00_DEVID, AT30TS00_DEVID_MASK },
+	{ ATMEL_MANID2, AT30TSE004_DEVID, AT30TSE004_DEVID_MASK },
 	{ IDT_MANID, TS3000B3_DEVID, TS3000B3_DEVID_MASK },
 	{ IDT_MANID, TS3000GB2_DEVID, TS3000GB2_DEVID_MASK },
 	{ MAX_MANID, MAX6604_DEVID, MAX6604_DEVID_MASK },
@@ -158,9 +166,25 @@ static struct jc42_chips jc42_chips[] = {
 	{ STM_MANID, STTS424_DEVID, STTS424_DEVID_MASK },
 	{ STM_MANID, STTS424E_DEVID, STTS424E_DEVID_MASK },
 	{ STM_MANID, STTS2002_DEVID, STTS2002_DEVID_MASK },
+	{ STM_MANID, STTS2004_DEVID, STTS2004_DEVID_MASK },
 	{ STM_MANID, STTS3000_DEVID, STTS3000_DEVID_MASK },
 };
 
+enum temp_index {
+	t_input = 0,
+	t_crit,
+	t_min,
+	t_max,
+	t_num_temp
+};
+
+static const u8 temp_regs[t_num_temp] = {
+	[t_input] = JC42_REG_TEMP,
+	[t_crit] = JC42_REG_TEMP_CRITICAL,
+	[t_min] = JC42_REG_TEMP_LOWER,
+	[t_max] = JC42_REG_TEMP_UPPER,
+};
+
 /* Each client has this additional data */
 struct jc42_data {
 	struct i2c_client *client;
@@ -170,69 +194,7 @@ struct jc42_data {
 	unsigned long	last_updated;	/* In jiffies */
 	u16		orig_config;	/* original configuration */
 	u16		config;		/* current configuration */
-	u16		temp_input;	/* Temperatures */
-	u16		temp_crit;
-	u16		temp_min;
-	u16		temp_max;
-};
-
-static int jc42_probe(struct i2c_client *client,
-		      const struct i2c_device_id *id);
-static int jc42_detect(struct i2c_client *client, struct i2c_board_info *info);
-static int jc42_remove(struct i2c_client *client);
-
-static struct jc42_data *jc42_update_device(struct device *dev);
-
-static const struct i2c_device_id jc42_id[] = {
-	{ "jc42", 0 },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, jc42_id);
-
-#ifdef CONFIG_PM
-
-static int jc42_suspend(struct device *dev)
-{
-	struct jc42_data *data = dev_get_drvdata(dev);
-
-	data->config |= JC42_CFG_SHUTDOWN;
-	i2c_smbus_write_word_swapped(data->client, JC42_REG_CONFIG,
-				     data->config);
-	return 0;
-}
-
-static int jc42_resume(struct device *dev)
-{
-	struct jc42_data *data = dev_get_drvdata(dev);
-
-	data->config &= ~JC42_CFG_SHUTDOWN;
-	i2c_smbus_write_word_swapped(data->client, JC42_REG_CONFIG,
-				     data->config);
-	return 0;
-}
-
-static const struct dev_pm_ops jc42_dev_pm_ops = {
-	.suspend = jc42_suspend,
-	.resume = jc42_resume,
-};
-
-#define JC42_DEV_PM_OPS (&jc42_dev_pm_ops)
-#else
-#define JC42_DEV_PM_OPS NULL
-#endif /* CONFIG_PM */
-
-/* This is the driver that will be inserted */
-static struct i2c_driver jc42_driver = {
-	.class		= I2C_CLASS_SPD,
-	.driver = {
-		.name	= "jc42",
-		.pm = JC42_DEV_PM_OPS,
-	},
-	.probe		= jc42_probe,
-	.remove		= jc42_remove,
-	.id_table	= jc42_id,
-	.detect		= jc42_detect,
-	.address_list	= normal_i2c,
+	u16		temp[t_num_temp];/* Temperatures */
 };
 
 #define JC42_TEMP_MIN_EXTENDED	(-40000)
@@ -261,79 +223,81 @@ static int jc42_temp_from_reg(s16 reg)
 	return reg * 125 / 2;
 }
 
-/* sysfs stuff */
-
-/* read routines for temperature limits */
-#define show(value)	\
-static ssize_t show_##value(struct device *dev,				\
-			    struct device_attribute *attr,		\
-			    char *buf)					\
-{									\
-	struct jc42_data *data = jc42_update_device(dev);		\
-	if (IS_ERR(data))						\
-		return PTR_ERR(data);					\
-	return sprintf(buf, "%d\n", jc42_temp_from_reg(data->value));	\
+static struct jc42_data *jc42_update_device(struct device *dev)
+{
+	struct jc42_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
+	struct jc42_data *ret = data;
+	int i, val;
+
+	mutex_lock(&data->update_lock);
+
+	if (time_after(jiffies, data->last_updated + HZ) || !data->valid) {
+		for (i = 0; i < t_num_temp; i++) {
+			val = i2c_smbus_read_word_swapped(client, temp_regs[i]);
+			if (val < 0) {
+				ret = ERR_PTR(val);
+				goto abort;
+			}
+			data->temp[i] = val;
+		}
+		data->last_updated = jiffies;
+		data->valid = true;
+	}
+abort:
+	mutex_unlock(&data->update_lock);
+	return ret;
 }
 
-show(temp_input);
-show(temp_crit);
-show(temp_min);
-show(temp_max);
+/* sysfs functions */
 
-/* read routines for hysteresis values */
-static ssize_t show_temp_crit_hyst(struct device *dev,
-				   struct device_attribute *attr, char *buf)
+static ssize_t show_temp(struct device *dev, struct device_attribute *devattr,
+			 char *buf)
 {
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
 	struct jc42_data *data = jc42_update_device(dev);
-	int temp, hyst;
-
 	if (IS_ERR(data))
 		return PTR_ERR(data);
-
-	temp = jc42_temp_from_reg(data->temp_crit);
-	hyst = jc42_hysteresis[(data->config & JC42_CFG_HYST_MASK)
-			       >> JC42_CFG_HYST_SHIFT];
-	return sprintf(buf, "%d\n", temp - hyst);
+	return sprintf(buf, "%d\n",
+		       jc42_temp_from_reg(data->temp[attr->index]));
 }
 
-static ssize_t show_temp_max_hyst(struct device *dev,
-				  struct device_attribute *attr, char *buf)
+static ssize_t show_temp_hyst(struct device *dev,
+			      struct device_attribute *devattr, char *buf)
 {
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
 	struct jc42_data *data = jc42_update_device(dev);
 	int temp, hyst;
 
 	if (IS_ERR(data))
 		return PTR_ERR(data);
 
-	temp = jc42_temp_from_reg(data->temp_max);
+	temp = jc42_temp_from_reg(data->temp[attr->index]);
 	hyst = jc42_hysteresis[(data->config & JC42_CFG_HYST_MASK)
 			       >> JC42_CFG_HYST_SHIFT];
 	return sprintf(buf, "%d\n", temp - hyst);
 }
 
-/* write routines */
-#define set(value, reg)	\
-static ssize_t set_##value(struct device *dev,				\
-			   struct device_attribute *attr,		\
-			   const char *buf, size_t count)		\
-{									\
-	struct jc42_data *data = dev_get_drvdata(dev);			\
-	int err, ret = count;						\
-	long val;							\
-	if (kstrtol(buf, 10, &val) < 0)					\
-		return -EINVAL;						\
-	mutex_lock(&data->update_lock);					\
-	data->value = jc42_temp_to_reg(val, data->extended);		\
-	err = i2c_smbus_write_word_swapped(data->client, reg, data->value); \
-	if (err < 0)							\
-		ret = err;						\
-	mutex_unlock(&data->update_lock);				\
-	return ret;							\
-}
+static ssize_t set_temp(struct device *dev, struct device_attribute *devattr,
+			const char *buf, size_t count)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	struct jc42_data *data = dev_get_drvdata(dev);
+	int err, ret = count;
+	int nr = attr->index;
+	long val;
 
-set(temp_min, JC42_REG_TEMP_LOWER);
-set(temp_max, JC42_REG_TEMP_UPPER);
-set(temp_crit, JC42_REG_TEMP_CRITICAL);
+	if (kstrtol(buf, 10, &val) < 0)
+		return -EINVAL;
+	mutex_lock(&data->update_lock);
+	data->temp[nr] = jc42_temp_to_reg(val, data->extended);
+	err = i2c_smbus_write_word_swapped(data->client, temp_regs[nr],
+					   data->temp[nr]);
+	if (err < 0)
+		ret = err;
+	mutex_unlock(&data->update_lock);
+	return ret;
+}
 
 /*
  * JC42.4 compliant chips only support four hysteresis values.
@@ -352,7 +316,7 @@ static ssize_t set_temp_crit_hyst(struct device *dev,
 	if (kstrtoul(buf, 10, &val) < 0)
 		return -EINVAL;
 
-	diff = jc42_temp_from_reg(data->temp_crit) - val;
+	diff = jc42_temp_from_reg(data->temp[t_crit]) - val;
 	hyst = 0;
 	if (diff > 0) {
 		if (diff < 2250)
@@ -384,25 +348,20 @@ static ssize_t show_alarm(struct device *dev,
 	if (IS_ERR(data))
 		return PTR_ERR(data);
 
-	val = data->temp_input;
+	val = data->temp[t_input];
 	if (bit != JC42_ALARM_CRIT_BIT && (data->config & JC42_CFG_CRIT_ONLY))
 		val = 0;
 	return sprintf(buf, "%u\n", (val >> bit) & 1);
 }
 
-static DEVICE_ATTR(temp1_input, S_IRUGO,
-		   show_temp_input, NULL);
-static DEVICE_ATTR(temp1_crit, S_IRUGO,
-		   show_temp_crit, set_temp_crit);
-static DEVICE_ATTR(temp1_min, S_IRUGO,
-		   show_temp_min, set_temp_min);
-static DEVICE_ATTR(temp1_max, S_IRUGO,
-		   show_temp_max, set_temp_max);
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, show_temp, NULL, t_input);
+static SENSOR_DEVICE_ATTR(temp1_crit, S_IRUGO, show_temp, set_temp, t_crit);
+static SENSOR_DEVICE_ATTR(temp1_min, S_IRUGO, show_temp, set_temp, t_min);
+static SENSOR_DEVICE_ATTR(temp1_max, S_IRUGO, show_temp, set_temp, t_max);
 
-static DEVICE_ATTR(temp1_crit_hyst, S_IRUGO,
-		   show_temp_crit_hyst, set_temp_crit_hyst);
-static DEVICE_ATTR(temp1_max_hyst, S_IRUGO,
-		   show_temp_max_hyst, NULL);
+static SENSOR_DEVICE_ATTR(temp1_crit_hyst, S_IRUGO, show_temp_hyst,
+			  set_temp_crit_hyst, t_crit);
+static SENSOR_DEVICE_ATTR(temp1_max_hyst, S_IRUGO, show_temp_hyst, NULL, t_max);
 
 static SENSOR_DEVICE_ATTR(temp1_crit_alarm, S_IRUGO, show_alarm, NULL,
 			  JC42_ALARM_CRIT_BIT);
@@ -412,12 +371,12 @@ static SENSOR_DEVICE_ATTR(temp1_max_alarm, S_IRUGO, show_alarm, NULL,
 			  JC42_ALARM_MAX_BIT);
 
 static struct attribute *jc42_attributes[] = {
-	&dev_attr_temp1_input.attr,
-	&dev_attr_temp1_crit.attr,
-	&dev_attr_temp1_min.attr,
-	&dev_attr_temp1_max.attr,
-	&dev_attr_temp1_crit_hyst.attr,
-	&dev_attr_temp1_max_hyst.attr,
+	&sensor_dev_attr_temp1_input.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit.dev_attr.attr,
+	&sensor_dev_attr_temp1_min.dev_attr.attr,
+	&sensor_dev_attr_temp1_max.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp1_max_hyst.dev_attr.attr,
 	&sensor_dev_attr_temp1_crit_alarm.dev_attr.attr,
 	&sensor_dev_attr_temp1_min_alarm.dev_attr.attr,
 	&sensor_dev_attr_temp1_max_alarm.dev_attr.attr,
@@ -432,12 +391,12 @@ static umode_t jc42_attribute_mode(struct kobject *kobj,
 	unsigned int config = data->config;
 	bool readonly;
 
-	if (attr == &dev_attr_temp1_crit.attr)
+	if (attr == &sensor_dev_attr_temp1_crit.dev_attr.attr)
 		readonly = config & JC42_CFG_TCRIT_LOCK;
-	else if (attr == &dev_attr_temp1_min.attr ||
-		 attr == &dev_attr_temp1_max.attr)
+	else if (attr == &sensor_dev_attr_temp1_min.dev_attr.attr ||
+		 attr == &sensor_dev_attr_temp1_max.dev_attr.attr)
 		readonly = config & JC42_CFG_EVENT_LOCK;
-	else if (attr == &dev_attr_temp1_crit_hyst.attr)
+	else if (attr == &sensor_dev_attr_temp1_crit_hyst.dev_attr.attr)
 		readonly = config & (JC42_CFG_EVENT_LOCK | JC42_CFG_TCRIT_LOCK);
 	else
 		readonly = true;
@@ -537,52 +496,56 @@ static int jc42_remove(struct i2c_client *client)
 	return 0;
 }
 
-static struct jc42_data *jc42_update_device(struct device *dev)
+#ifdef CONFIG_PM
+
+static int jc42_suspend(struct device *dev)
 {
 	struct jc42_data *data = dev_get_drvdata(dev);
-	struct i2c_client *client = data->client;
-	struct jc42_data *ret = data;
-	int val;
 
-	mutex_lock(&data->update_lock);
+	data->config |= JC42_CFG_SHUTDOWN;
+	i2c_smbus_write_word_swapped(data->client, JC42_REG_CONFIG,
+				     data->config);
+	return 0;
+}
 
-	if (time_after(jiffies, data->last_updated + HZ) || !data->valid) {
-		val = i2c_smbus_read_word_swapped(client, JC42_REG_TEMP);
-		if (val < 0) {
-			ret = ERR_PTR(val);
-			goto abort;
-		}
-		data->temp_input = val;
+static int jc42_resume(struct device *dev)
+{
+	struct jc42_data *data = dev_get_drvdata(dev);
 
-		val = i2c_smbus_read_word_swapped(client,
-						  JC42_REG_TEMP_CRITICAL);
-		if (val < 0) {
-			ret = ERR_PTR(val);
-			goto abort;
-		}
-		data->temp_crit = val;
+	data->config &= ~JC42_CFG_SHUTDOWN;
+	i2c_smbus_write_word_swapped(data->client, JC42_REG_CONFIG,
+				     data->config);
+	return 0;
+}
 
-		val = i2c_smbus_read_word_swapped(client, JC42_REG_TEMP_LOWER);
-		if (val < 0) {
-			ret = ERR_PTR(val);
-			goto abort;
-		}
-		data->temp_min = val;
+static const struct dev_pm_ops jc42_dev_pm_ops = {
+	.suspend = jc42_suspend,
+	.resume = jc42_resume,
+};
 
-		val = i2c_smbus_read_word_swapped(client, JC42_REG_TEMP_UPPER);
-		if (val < 0) {
-			ret = ERR_PTR(val);
-			goto abort;
-		}
-		data->temp_max = val;
+#define JC42_DEV_PM_OPS (&jc42_dev_pm_ops)
+#else
+#define JC42_DEV_PM_OPS NULL
+#endif /* CONFIG_PM */
 
-		data->last_updated = jiffies;
-		data->valid = true;
-	}
-abort:
-	mutex_unlock(&data->update_lock);
-	return ret;
-}
+static const struct i2c_device_id jc42_id[] = {
+	{ "jc42", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, jc42_id);
+
+static struct i2c_driver jc42_driver = {
+	.class		= I2C_CLASS_SPD,
+	.driver = {
+		.name	= "jc42",
+		.pm = JC42_DEV_PM_OPS,
+	},
+	.probe		= jc42_probe,
+	.remove		= jc42_remove,
+	.id_table	= jc42_id,
+	.detect		= jc42_detect,
+	.address_list	= normal_i2c,
+};
 
 module_i2c_driver(jc42_driver);
 
diff --git a/drivers/hwmon/lm70.c b/drivers/hwmon/lm70.c
index 505a59e100b..97204dce162 100644
--- a/drivers/hwmon/lm70.c
+++ b/drivers/hwmon/lm70.c
@@ -47,7 +47,7 @@
 #define LM70_CHIP_LM74		3	/* NS LM74 */
 
 struct lm70 {
-	struct device *hwmon_dev;
+	struct spi_device *spi;
 	struct mutex lock;
 	unsigned int chip;
 };
@@ -56,11 +56,11 @@ struct lm70 {
 static ssize_t lm70_sense_temp(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
-	struct spi_device *spi = to_spi_device(dev);
+	struct lm70 *p_lm70 = dev_get_drvdata(dev);
+	struct spi_device *spi = p_lm70->spi;
 	int status, val = 0;
 	u8 rxbuf[2];
 	s16 raw = 0;
-	struct lm70 *p_lm70 = spi_get_drvdata(spi);
 
 	if (mutex_lock_interruptible(&p_lm70->lock))
 		return -ERESTARTSYS;
@@ -121,21 +121,20 @@ out:
 
 static DEVICE_ATTR(temp1_input, S_IRUGO, lm70_sense_temp, NULL);
 
-static ssize_t lm70_show_name(struct device *dev, struct device_attribute
-			      *devattr, char *buf)
-{
-	return sprintf(buf, "%s\n", to_spi_device(dev)->modalias);
-}
+static struct attribute *lm70_attrs[] = {
+	&dev_attr_temp1_input.attr,
+	NULL
+};
 
-static DEVICE_ATTR(name, S_IRUGO, lm70_show_name, NULL);
+ATTRIBUTE_GROUPS(lm70);
 
 /*----------------------------------------------------------------------*/
 
 static int lm70_probe(struct spi_device *spi)
 {
 	int chip = spi_get_device_id(spi)->driver_data;
+	struct device *hwmon_dev;
 	struct lm70 *p_lm70;
-	int status;
 
 	/* signaling is SPI_MODE_0 */
 	if (spi->mode & (SPI_CPOL | SPI_CPHA))
@@ -149,46 +148,14 @@ static int lm70_probe(struct spi_device *spi)
 
 	mutex_init(&p_lm70->lock);
 	p_lm70->chip = chip;
+	p_lm70->spi = spi;
 
-	spi_set_drvdata(spi, p_lm70);
-
-	status = device_create_file(&spi->dev, &dev_attr_temp1_input);
-	if (status)
-		goto out_dev_create_temp_file_failed;
-	status = device_create_file(&spi->dev, &dev_attr_name);
-	if (status)
-		goto out_dev_create_file_failed;
-
-	/* sysfs hook */
-	p_lm70->hwmon_dev = hwmon_device_register(&spi->dev);
-	if (IS_ERR(p_lm70->hwmon_dev)) {
-		dev_dbg(&spi->dev, "hwmon_device_register failed.\n");
-		status = PTR_ERR(p_lm70->hwmon_dev);
-		goto out_dev_reg_failed;
-	}
-
-	return 0;
-
-out_dev_reg_failed:
-	device_remove_file(&spi->dev, &dev_attr_name);
-out_dev_create_file_failed:
-	device_remove_file(&spi->dev, &dev_attr_temp1_input);
-out_dev_create_temp_file_failed:
-	return status;
+	hwmon_dev = devm_hwmon_device_register_with_groups(&spi->dev,
+							   spi->modalias,
+							   p_lm70, lm70_groups);
+	return PTR_ERR_OR_ZERO(hwmon_dev);
 }
 
-static int lm70_remove(struct spi_device *spi)
-{
-	struct lm70 *p_lm70 = spi_get_drvdata(spi);
-
-	hwmon_device_unregister(p_lm70->hwmon_dev);
-	device_remove_file(&spi->dev, &dev_attr_temp1_input);
-	device_remove_file(&spi->dev, &dev_attr_name);
-
-	return 0;
-}
-
-
 static const struct spi_device_id lm70_ids[] = {
 	{ "lm70",   LM70_CHIP_LM70 },
 	{ "tmp121", LM70_CHIP_TMP121 },
@@ -205,7 +172,6 @@ static struct spi_driver lm70_driver = {
 	},
 	.id_table = lm70_ids,
 	.probe	= lm70_probe,
-	.remove	= lm70_remove,
 };
 
 module_spi_driver(lm70_driver);
diff --git a/drivers/hwmon/lm75.c b/drivers/hwmon/lm75.c
index 84a55eacd90..479ffbeed3f 100644
--- a/drivers/hwmon/lm75.c
+++ b/drivers/hwmon/lm75.c
@@ -72,6 +72,7 @@ static const u8 LM75_REG_TEMP[3] = {
 
 /* Each client has this additional data */
 struct lm75_data {
+	struct i2c_client	*client;
 	struct device		*hwmon_dev;
 	struct thermal_zone_device	*tz;
 	struct mutex		update_lock;
@@ -130,8 +131,8 @@ static ssize_t 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 i2c_client *client = to_i2c_client(dev);
-	struct lm75_data *data = i2c_get_clientdata(client);
+	struct lm75_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
 	int nr = attr->index;
 	long temp;
 	int error;
@@ -165,17 +166,14 @@ static SENSOR_DEVICE_ATTR(temp1_max_hyst, S_IWUSR | S_IRUGO,
 			show_temp, set_temp, 2);
 static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, show_temp, NULL, 0);
 
-static struct attribute *lm75_attributes[] = {
+static struct attribute *lm75_attrs[] = {
 	&sensor_dev_attr_temp1_input.dev_attr.attr,
 	&sensor_dev_attr_temp1_max.dev_attr.attr,
 	&sensor_dev_attr_temp1_max_hyst.dev_attr.attr,
 
 	NULL
 };
-
-static const struct attribute_group lm75_group = {
-	.attrs = lm75_attributes,
-};
+ATTRIBUTE_GROUPS(lm75);
 
 /*-----------------------------------------------------------------------*/
 
@@ -184,6 +182,7 @@ static const struct attribute_group lm75_group = {
 static int
 lm75_probe(struct i2c_client *client, const struct i2c_device_id *id)
 {
+	struct device *dev = &client->dev;
 	struct lm75_data *data;
 	int status;
 	u8 set_mask, clr_mask;
@@ -194,10 +193,11 @@ lm75_probe(struct i2c_client *client, const struct i2c_device_id *id)
 			I2C_FUNC_SMBUS_BYTE_DATA | I2C_FUNC_SMBUS_WORD_DATA))
 		return -EIO;
 
-	data = devm_kzalloc(&client->dev, sizeof(struct lm75_data), GFP_KERNEL);
+	data = devm_kzalloc(dev, sizeof(struct lm75_data), GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
 
+	data->client = client;
 	i2c_set_clientdata(client, data);
 	mutex_init(&data->update_lock);
 
@@ -269,7 +269,7 @@ lm75_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	/* configure as specified */
 	status = lm75_read_value(client, LM75_REG_CONF);
 	if (status < 0) {
-		dev_dbg(&client->dev, "Can't read config? %d\n", status);
+		dev_dbg(dev, "Can't read config? %d\n", status);
 		return status;
 	}
 	data->orig_conf = status;
@@ -277,43 +277,32 @@ lm75_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	new |= set_mask;
 	if (status != new)
 		lm75_write_value(client, LM75_REG_CONF, new);
-	dev_dbg(&client->dev, "Config %02x\n", new);
+	dev_dbg(dev, "Config %02x\n", new);
 
-	/* Register sysfs hooks */
-	status = sysfs_create_group(&client->dev.kobj, &lm75_group);
-	if (status)
-		return status;
+	data->hwmon_dev = hwmon_device_register_with_groups(dev, client->name,
+							    data, lm75_groups);
+	if (IS_ERR(data->hwmon_dev))
+		return PTR_ERR(data->hwmon_dev);
 
-	data->hwmon_dev = hwmon_device_register(&client->dev);
-	if (IS_ERR(data->hwmon_dev)) {
-		status = PTR_ERR(data->hwmon_dev);
-		goto exit_remove;
-	}
-
-	data->tz = thermal_zone_of_sensor_register(&client->dev,
+	data->tz = thermal_zone_of_sensor_register(data->hwmon_dev,
 						   0,
-						   &client->dev,
+						   data->hwmon_dev,
 						   lm75_read_temp, NULL);
 	if (IS_ERR(data->tz))
 		data->tz = NULL;
 
-	dev_info(&client->dev, "%s: sensor '%s'\n",
+	dev_info(dev, "%s: sensor '%s'\n",
 		 dev_name(data->hwmon_dev), client->name);
 
 	return 0;
-
-exit_remove:
-	sysfs_remove_group(&client->dev.kobj, &lm75_group);
-	return status;
 }
 
 static int lm75_remove(struct i2c_client *client)
 {
 	struct lm75_data *data = i2c_get_clientdata(client);
 
-	thermal_zone_of_sensor_unregister(&client->dev, data->tz);
+	thermal_zone_of_sensor_unregister(data->hwmon_dev, data->tz);
 	hwmon_device_unregister(data->hwmon_dev);
-	sysfs_remove_group(&client->dev.kobj, &lm75_group);
 	lm75_write_value(client, LM75_REG_CONF, data->orig_conf);
 	return 0;
 }
@@ -507,8 +496,8 @@ static int lm75_write_value(struct i2c_client *client, u8 reg, u16 value)
 
 static struct lm75_data *lm75_update_device(struct device *dev)
 {
-	struct i2c_client *client = to_i2c_client(dev);
-	struct lm75_data *data = i2c_get_clientdata(client);
+	struct lm75_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
 	struct lm75_data *ret = data;
 
 	mutex_lock(&data->update_lock);
diff --git a/drivers/hwmon/lm77.c b/drivers/hwmon/lm77.c
index 502771c06fd..5ceb443b938 100644
--- a/drivers/hwmon/lm77.c
+++ b/drivers/hwmon/lm77.c
@@ -19,10 +19,6 @@
  * 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>
@@ -47,50 +43,33 @@ static const unsigned short normal_i2c[] = { 0x48, 0x49, 0x4a, 0x4b,
 #define LM77_REG_TEMP_MIN	0x04
 #define LM77_REG_TEMP_MAX	0x05
 
+enum temp_index {
+	t_input = 0,
+	t_crit,
+	t_min,
+	t_max,
+	t_hyst,
+	t_num_temp
+};
+
+static const u8 temp_regs[t_num_temp] = {
+	[t_input] = LM77_REG_TEMP,
+	[t_min] = LM77_REG_TEMP_MIN,
+	[t_max] = LM77_REG_TEMP_MAX,
+	[t_crit] = LM77_REG_TEMP_CRIT,
+	[t_hyst] = LM77_REG_TEMP_HYST,
+};
+
 /* Each client has this additional data */
 struct lm77_data {
-	struct device		*hwmon_dev;
+	struct i2c_client	*client;
 	struct mutex		update_lock;
 	char			valid;
 	unsigned long		last_updated;	/* In jiffies */
-	int			temp_input;	/* Temperatures */
-	int			temp_crit;
-	int			temp_min;
-	int			temp_max;
-	int			temp_hyst;
+	int			temp[t_num_temp]; /* index using temp_index */
 	u8			alarms;
 };
 
-static int lm77_probe(struct i2c_client *client,
-		      const struct i2c_device_id *id);
-static int lm77_detect(struct i2c_client *client, struct i2c_board_info *info);
-static void lm77_init_client(struct i2c_client *client);
-static int lm77_remove(struct i2c_client *client);
-static u16 lm77_read_value(struct i2c_client *client, u8 reg);
-static int lm77_write_value(struct i2c_client *client, u8 reg, u16 value);
-
-static struct lm77_data *lm77_update_device(struct device *dev);
-
-
-static const struct i2c_device_id lm77_id[] = {
-	{ "lm77", 0 },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, lm77_id);
-
-/* This is the driver that will be inserted */
-static struct i2c_driver lm77_driver = {
-	.class		= I2C_CLASS_HWMON,
-	.driver = {
-		.name	= "lm77",
-	},
-	.probe		= lm77_probe,
-	.remove		= lm77_remove,
-	.id_table	= lm77_id,
-	.detect		= lm77_detect,
-	.address_list	= normal_i2c,
-};
-
 /* straight from the datasheet */
 #define LM77_TEMP_MIN (-55000)
 #define LM77_TEMP_MAX 125000
@@ -110,97 +89,109 @@ static inline int LM77_TEMP_FROM_REG(s16 reg)
 	return (reg / 8) * 500;
 }
 
-/* sysfs stuff */
-
-/* read routines for temperature limits */
-#define show(value)	\
-static ssize_t show_##value(struct device *dev,			\
-			    struct device_attribute *attr,	\
-			    char *buf)				\
-{								\
-	struct lm77_data *data = lm77_update_device(dev);	\
-	return sprintf(buf, "%d\n", data->value);		\
+/*
+ * All registers are word-sized, except for the configuration register.
+ * The LM77 uses the high-byte first convention.
+ */
+static u16 lm77_read_value(struct i2c_client *client, u8 reg)
+{
+	if (reg == LM77_REG_CONF)
+		return i2c_smbus_read_byte_data(client, reg);
+	else
+		return i2c_smbus_read_word_swapped(client, reg);
 }
 
-show(temp_input);
-show(temp_crit);
-show(temp_min);
-show(temp_max);
+static int lm77_write_value(struct i2c_client *client, u8 reg, u16 value)
+{
+	if (reg == LM77_REG_CONF)
+		return i2c_smbus_write_byte_data(client, reg, value);
+	else
+		return i2c_smbus_write_word_swapped(client, reg, value);
+}
 
-/* read routines for hysteresis values */
-static ssize_t show_temp_crit_hyst(struct device *dev,
-				   struct device_attribute *attr, char *buf)
+static struct lm77_data *lm77_update_device(struct device *dev)
 {
-	struct lm77_data *data = lm77_update_device(dev);
-	return sprintf(buf, "%d\n", data->temp_crit - data->temp_hyst);
+	struct lm77_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
+	int i;
+
+	mutex_lock(&data->update_lock);
+
+	if (time_after(jiffies, data->last_updated + HZ + HZ / 2)
+	    || !data->valid) {
+		dev_dbg(&client->dev, "Starting lm77 update\n");
+		for (i = 0; i < t_num_temp; i++) {
+			data->temp[i] =
+			  LM77_TEMP_FROM_REG(lm77_read_value(client,
+							     temp_regs[i]));
+		}
+		data->alarms =
+			lm77_read_value(client, LM77_REG_TEMP) & 0x0007;
+		data->last_updated = jiffies;
+		data->valid = 1;
+	}
+
+	mutex_unlock(&data->update_lock);
+
+	return data;
 }
-static ssize_t show_temp_min_hyst(struct device *dev,
-				  struct device_attribute *attr, char *buf)
+
+/* sysfs stuff */
+
+static ssize_t show_temp(struct device *dev, struct device_attribute *devattr,
+			 char *buf)
 {
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
 	struct lm77_data *data = lm77_update_device(dev);
-	return sprintf(buf, "%d\n", data->temp_min + data->temp_hyst);
+
+	return sprintf(buf, "%d\n", data->temp[attr->index]);
 }
-static ssize_t show_temp_max_hyst(struct device *dev,
-				  struct device_attribute *attr, char *buf)
+
+static ssize_t show_temp_hyst(struct device *dev,
+			      struct device_attribute *devattr, char *buf)
 {
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
 	struct lm77_data *data = lm77_update_device(dev);
-	return sprintf(buf, "%d\n", data->temp_max - data->temp_hyst);
-}
+	int nr = attr->index;
+	int temp;
 
-/* write routines */
-#define set(value, reg)	\
-static ssize_t set_##value(struct device *dev, struct device_attribute *attr, \
-			   const char *buf, size_t count)		\
-{									\
-	struct i2c_client *client = to_i2c_client(dev);			\
-	struct lm77_data *data = i2c_get_clientdata(client);		\
-	long val;							\
-	int err = kstrtol(buf, 10, &val);				\
-	if (err)							\
-		return err;						\
-									\
-	mutex_lock(&data->update_lock);					\
-	data->value = val;						\
-	lm77_write_value(client, reg, LM77_TEMP_TO_REG(data->value));	\
-	mutex_unlock(&data->update_lock);				\
-	return count;							\
-}
+	temp = nr == t_min ? data->temp[nr] + data->temp[t_hyst] :
+			     data->temp[nr] - data->temp[t_hyst];
 
-set(temp_min, LM77_REG_TEMP_MIN);
-set(temp_max, LM77_REG_TEMP_MAX);
+	return sprintf(buf, "%d\n", temp);
+}
 
-/*
- * hysteresis is stored as a relative value on the chip, so it has to be
- * converted first
- */
-static ssize_t set_temp_crit_hyst(struct device *dev,
-				  struct device_attribute *attr,
-				  const char *buf, size_t count)
+static ssize_t set_temp(struct device *dev, struct device_attribute *devattr,
+			const char *buf, size_t count)
 {
-	struct i2c_client *client = to_i2c_client(dev);
-	struct lm77_data *data = i2c_get_clientdata(client);
-	unsigned long val;
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	struct lm77_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
+	int nr = attr->index;
+	long val;
 	int err;
 
-	err = kstrtoul(buf, 10, &val);
+	err = kstrtol(buf, 10, &val);
 	if (err)
 		return err;
 
 	mutex_lock(&data->update_lock);
-	data->temp_hyst = data->temp_crit - val;
-	lm77_write_value(client, LM77_REG_TEMP_HYST,
-			 LM77_TEMP_TO_REG(data->temp_hyst));
+	data->temp[nr] = val;
+	lm77_write_value(client, temp_regs[nr], LM77_TEMP_TO_REG(val));
 	mutex_unlock(&data->update_lock);
 	return count;
 }
 
-/* preserve hysteresis when setting T_crit */
-static ssize_t set_temp_crit(struct device *dev, struct device_attribute *attr,
+/*
+ * hysteresis is stored as a relative value on the chip, so it has to be
+ * converted first.
+ */
+static ssize_t set_temp_hyst(struct device *dev,
+			     struct device_attribute *devattr,
 			     const char *buf, size_t count)
 {
-	struct i2c_client *client = to_i2c_client(dev);
-	struct lm77_data *data = i2c_get_clientdata(client);
-	int oldcrithyst;
+	struct lm77_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
 	unsigned long val;
 	int err;
 
@@ -209,13 +200,9 @@ static ssize_t set_temp_crit(struct device *dev, struct device_attribute *attr,
 		return err;
 
 	mutex_lock(&data->update_lock);
-	oldcrithyst = data->temp_crit - data->temp_hyst;
-	data->temp_crit = val;
-	data->temp_hyst = data->temp_crit - oldcrithyst;
-	lm77_write_value(client, LM77_REG_TEMP_CRIT,
-			 LM77_TEMP_TO_REG(data->temp_crit));
+	data->temp[t_hyst] = data->temp[t_crit] - val;
 	lm77_write_value(client, LM77_REG_TEMP_HYST,
-			 LM77_TEMP_TO_REG(data->temp_hyst));
+			 LM77_TEMP_TO_REG(data->temp[t_hyst]));
 	mutex_unlock(&data->update_lock);
 	return count;
 }
@@ -228,43 +215,37 @@ static ssize_t show_alarm(struct device *dev, struct device_attribute *attr,
 	return sprintf(buf, "%u\n", (data->alarms >> bitnr) & 1);
 }
 
-static DEVICE_ATTR(temp1_input, S_IRUGO,
-		   show_temp_input, NULL);
-static DEVICE_ATTR(temp1_crit, S_IWUSR | S_IRUGO,
-		   show_temp_crit, set_temp_crit);
-static DEVICE_ATTR(temp1_min, S_IWUSR | S_IRUGO,
-		   show_temp_min, set_temp_min);
-static DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO,
-		   show_temp_max, set_temp_max);
-
-static DEVICE_ATTR(temp1_crit_hyst, S_IWUSR | S_IRUGO,
-		   show_temp_crit_hyst, set_temp_crit_hyst);
-static DEVICE_ATTR(temp1_min_hyst, S_IRUGO,
-		   show_temp_min_hyst, NULL);
-static DEVICE_ATTR(temp1_max_hyst, S_IRUGO,
-		   show_temp_max_hyst, NULL);
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, show_temp, NULL, t_input);
+static SENSOR_DEVICE_ATTR(temp1_crit, S_IWUSR | S_IRUGO, show_temp, set_temp,
+			  t_crit);
+static SENSOR_DEVICE_ATTR(temp1_min, S_IWUSR | S_IRUGO, show_temp, set_temp,
+			  t_min);
+static SENSOR_DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO, show_temp, set_temp,
+			  t_max);
+
+static SENSOR_DEVICE_ATTR(temp1_crit_hyst, S_IWUSR | S_IRUGO, show_temp_hyst,
+			  set_temp_hyst, t_crit);
+static SENSOR_DEVICE_ATTR(temp1_min_hyst, S_IRUGO, show_temp_hyst, NULL, t_min);
+static SENSOR_DEVICE_ATTR(temp1_max_hyst, S_IRUGO, show_temp_hyst, NULL, t_max);
 
 static SENSOR_DEVICE_ATTR(temp1_crit_alarm, S_IRUGO, show_alarm, NULL, 2);
 static SENSOR_DEVICE_ATTR(temp1_min_alarm, S_IRUGO, show_alarm, NULL, 0);
 static SENSOR_DEVICE_ATTR(temp1_max_alarm, S_IRUGO, show_alarm, NULL, 1);
 
-static struct attribute *lm77_attributes[] = {
-	&dev_attr_temp1_input.attr,
-	&dev_attr_temp1_crit.attr,
-	&dev_attr_temp1_min.attr,
-	&dev_attr_temp1_max.attr,
-	&dev_attr_temp1_crit_hyst.attr,
-	&dev_attr_temp1_min_hyst.attr,
-	&dev_attr_temp1_max_hyst.attr,
+static struct attribute *lm77_attrs[] = {
+	&sensor_dev_attr_temp1_input.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit.dev_attr.attr,
+	&sensor_dev_attr_temp1_min.dev_attr.attr,
+	&sensor_dev_attr_temp1_max.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp1_min_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp1_max_hyst.dev_attr.attr,
 	&sensor_dev_attr_temp1_crit_alarm.dev_attr.attr,
 	&sensor_dev_attr_temp1_min_alarm.dev_attr.attr,
 	&sensor_dev_attr_temp1_max_alarm.dev_attr.attr,
 	NULL
 };
-
-static const struct attribute_group lm77_group = {
-	.attrs = lm77_attributes,
-};
+ATTRIBUTE_GROUPS(lm77);
 
 /* Return 0 if detection is successful, -ENODEV otherwise */
 static int lm77_detect(struct i2c_client *client, struct i2c_board_info *info)
@@ -337,111 +318,52 @@ static int lm77_detect(struct i2c_client *client, struct i2c_board_info *info)
 	return 0;
 }
 
+static void lm77_init_client(struct i2c_client *client)
+{
+	/* Initialize the LM77 chip - turn off shutdown mode */
+	int conf = lm77_read_value(client, LM77_REG_CONF);
+	if (conf & 1)
+		lm77_write_value(client, LM77_REG_CONF, conf & 0xfe);
+}
+
 static int lm77_probe(struct i2c_client *client, const struct i2c_device_id *id)
 {
 	struct device *dev = &client->dev;
+	struct device *hwmon_dev;
 	struct lm77_data *data;
-	int err;
 
 	data = devm_kzalloc(dev, sizeof(struct lm77_data), GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
 
-	i2c_set_clientdata(client, data);
+	data->client = client;
 	mutex_init(&data->update_lock);
 
 	/* Initialize the LM77 chip */
 	lm77_init_client(client);
 
-	/* Register sysfs hooks */
-	err = sysfs_create_group(&dev->kobj, &lm77_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;
-	}
-
-	return 0;
-
-exit_remove:
-	sysfs_remove_group(&dev->kobj, &lm77_group);
-	return err;
+	hwmon_dev = devm_hwmon_device_register_with_groups(dev, client->name,
+							   data, lm77_groups);
+	return PTR_ERR_OR_ZERO(hwmon_dev);
 }
 
-static int lm77_remove(struct i2c_client *client)
-{
-	struct lm77_data *data = i2c_get_clientdata(client);
-	hwmon_device_unregister(data->hwmon_dev);
-	sysfs_remove_group(&client->dev.kobj, &lm77_group);
-	return 0;
-}
-
-/*
- * All registers are word-sized, except for the configuration register.
- * The LM77 uses the high-byte first convention.
- */
-static u16 lm77_read_value(struct i2c_client *client, u8 reg)
-{
-	if (reg == LM77_REG_CONF)
-		return i2c_smbus_read_byte_data(client, reg);
-	else
-		return i2c_smbus_read_word_swapped(client, reg);
-}
-
-static int lm77_write_value(struct i2c_client *client, u8 reg, u16 value)
-{
-	if (reg == LM77_REG_CONF)
-		return i2c_smbus_write_byte_data(client, reg, value);
-	else
-		return i2c_smbus_write_word_swapped(client, reg, value);
-}
-
-static void lm77_init_client(struct i2c_client *client)
-{
-	/* Initialize the LM77 chip - turn off shutdown mode */
-	int conf = lm77_read_value(client, LM77_REG_CONF);
-	if (conf & 1)
-		lm77_write_value(client, LM77_REG_CONF, conf & 0xfe);
-}
-
-static struct lm77_data *lm77_update_device(struct device *dev)
-{
-	struct i2c_client *client = to_i2c_client(dev);
-	struct lm77_data *data = i2c_get_clientdata(client);
-
-	mutex_lock(&data->update_lock);
-
-	if (time_after(jiffies, data->last_updated + HZ + HZ / 2)
-	    || !data->valid) {
-		dev_dbg(&client->dev, "Starting lm77 update\n");
-		data->temp_input =
-			LM77_TEMP_FROM_REG(lm77_read_value(client,
-							   LM77_REG_TEMP));
-		data->temp_hyst =
-			LM77_TEMP_FROM_REG(lm77_read_value(client,
-							   LM77_REG_TEMP_HYST));
-		data->temp_crit =
-			LM77_TEMP_FROM_REG(lm77_read_value(client,
-							   LM77_REG_TEMP_CRIT));
-		data->temp_min =
-			LM77_TEMP_FROM_REG(lm77_read_value(client,
-							   LM77_REG_TEMP_MIN));
-		data->temp_max =
-			LM77_TEMP_FROM_REG(lm77_read_value(client,
-							   LM77_REG_TEMP_MAX));
-		data->alarms =
-			lm77_read_value(client, LM77_REG_TEMP) & 0x0007;
-		data->last_updated = jiffies;
-		data->valid = 1;
-	}
-
-	mutex_unlock(&data->update_lock);
+static const struct i2c_device_id lm77_id[] = {
+	{ "lm77", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, lm77_id);
 
-	return data;
-}
+/* This is the driver that will be inserted */
+static struct i2c_driver lm77_driver = {
+	.class		= I2C_CLASS_HWMON,
+	.driver = {
+		.name	= "lm77",
+	},
+	.probe		= lm77_probe,
+	.id_table	= lm77_id,
+	.detect		= lm77_detect,
+	.address_list	= normal_i2c,
+};
 
 module_i2c_driver(lm77_driver);
 
diff --git a/drivers/hwmon/lm80.c b/drivers/hwmon/lm80.c
index bd0a1ebbf86..4bcd9b88294 100644
--- a/drivers/hwmon/lm80.c
+++ b/drivers/hwmon/lm80.c
@@ -86,26 +86,41 @@ static inline unsigned char FAN_TO_REG(unsigned rpm, unsigned div)
 #define FAN_FROM_REG(val, div)	((val) == 0 ? -1 : \
 				(val) == 255 ? 0 : 1350000/((div) * (val)))
 
-static inline long TEMP_FROM_REG(u16 temp)
-{
-	long res;
+#define TEMP_FROM_REG(reg)	((reg) * 125 / 32)
+#define TEMP_TO_REG(temp)	(DIV_ROUND_CLOSEST(clamp_val((temp), \
+					-128000, 127000), 1000) << 8)
 
-	temp >>= 4;
-	if (temp < 0x0800)
-		res = 625 * (long) temp;
-	else
-		res = ((long) temp - 0x01000) * 625;
+#define DIV_FROM_REG(val)		(1 << (val))
 
-	return res / 10;
-}
+enum temp_index {
+	t_input = 0,
+	t_hot_max,
+	t_hot_hyst,
+	t_os_max,
+	t_os_hyst,
+	t_num_temp
+};
 
-#define TEMP_LIMIT_FROM_REG(val)	(((val) > 0x80 ? \
-	(val) - 0x100 : (val)) * 1000)
+static const u8 temp_regs[t_num_temp] = {
+	[t_input] = LM80_REG_TEMP,
+	[t_hot_max] = LM80_REG_TEMP_HOT_MAX,
+	[t_hot_hyst] = LM80_REG_TEMP_HOT_HYST,
+	[t_os_max] = LM80_REG_TEMP_OS_MAX,
+	[t_os_hyst] = LM80_REG_TEMP_OS_HYST,
+};
 
-#define TEMP_LIMIT_TO_REG(val)		clamp_val((val) < 0 ? \
-	((val) - 500) / 1000 : ((val) + 500) / 1000, 0, 255)
+enum in_index {
+	i_input = 0,
+	i_max,
+	i_min,
+	i_num_in
+};
 
-#define DIV_FROM_REG(val)		(1 << (val))
+enum fan_index {
+	f_input,
+	f_min,
+	f_num_fan
+};
 
 /*
  * Client data (each client gets its own)
@@ -118,106 +133,187 @@ struct lm80_data {
 	char valid;		/* !=0 if following fields are valid */
 	unsigned long last_updated;	/* In jiffies */
 
-	u8 in[7];		/* Register value */
-	u8 in_max[7];		/* Register value */
-	u8 in_min[7];		/* Register value */
-	u8 fan[2];		/* Register value */
-	u8 fan_min[2];		/* Register value */
+	u8 in[i_num_in][7];	/* Register value, 1st index is enum in_index */
+	u8 fan[f_num_fan][2];	/* Register value, 1st index enum fan_index */
 	u8 fan_div[2];		/* Register encoding, shifted right */
-	u16 temp;		/* Register values, shifted right */
-	u8 temp_hot_max;	/* Register value */
-	u8 temp_hot_hyst;	/* Register value */
-	u8 temp_os_max;		/* Register value */
-	u8 temp_os_hyst;	/* Register value */
+	s16 temp[t_num_temp];	/* Register values, normalized to 16 bit */
 	u16 alarms;		/* Register encoding, combined */
 };
 
-/*
- * Functions declaration
- */
+static int lm80_read_value(struct i2c_client *client, u8 reg)
+{
+	return i2c_smbus_read_byte_data(client, reg);
+}
 
-static int lm80_probe(struct i2c_client *client,
-		      const struct i2c_device_id *id);
-static int lm80_detect(struct i2c_client *client, struct i2c_board_info *info);
-static void lm80_init_client(struct i2c_client *client);
-static struct lm80_data *lm80_update_device(struct device *dev);
-static int lm80_read_value(struct i2c_client *client, u8 reg);
-static int lm80_write_value(struct i2c_client *client, u8 reg, u8 value);
+static int lm80_write_value(struct i2c_client *client, u8 reg, u8 value)
+{
+	return i2c_smbus_write_byte_data(client, reg, value);
+}
 
-/*
- * Driver data (common to all clients)
- */
+/* Called when we have found a new LM80 and after read errors */
+static void lm80_init_client(struct i2c_client *client)
+{
+	/*
+	 * Reset all except Watchdog values and last conversion values
+	 * This sets fan-divs to 2, among others. This makes most other
+	 * initializations unnecessary
+	 */
+	lm80_write_value(client, LM80_REG_CONFIG, 0x80);
+	/* Set 11-bit temperature resolution */
+	lm80_write_value(client, LM80_REG_RES, 0x08);
 
-static const struct i2c_device_id lm80_id[] = {
-	{ "lm80", 0 },
-	{ "lm96080", 1 },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, lm80_id);
+	/* Start monitoring */
+	lm80_write_value(client, LM80_REG_CONFIG, 0x01);
+}
 
-static struct i2c_driver lm80_driver = {
-	.class		= I2C_CLASS_HWMON,
-	.driver = {
-		.name	= "lm80",
-	},
-	.probe		= lm80_probe,
-	.id_table	= lm80_id,
-	.detect		= lm80_detect,
-	.address_list	= normal_i2c,
-};
+static struct lm80_data *lm80_update_device(struct device *dev)
+{
+	struct lm80_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
+	int i;
+	int rv;
+	int prev_rv;
+	struct lm80_data *ret = data;
+
+	mutex_lock(&data->update_lock);
+
+	if (data->error)
+		lm80_init_client(client);
+
+	if (time_after(jiffies, data->last_updated + 2 * HZ) || !data->valid) {
+		dev_dbg(dev, "Starting lm80 update\n");
+		for (i = 0; i <= 6; i++) {
+			rv = lm80_read_value(client, LM80_REG_IN(i));
+			if (rv < 0)
+				goto abort;
+			data->in[i_input][i] = rv;
+
+			rv = lm80_read_value(client, LM80_REG_IN_MIN(i));
+			if (rv < 0)
+				goto abort;
+			data->in[i_min][i] = rv;
+
+			rv = lm80_read_value(client, LM80_REG_IN_MAX(i));
+			if (rv < 0)
+				goto abort;
+			data->in[i_max][i] = rv;
+		}
+
+		rv = lm80_read_value(client, LM80_REG_FAN1);
+		if (rv < 0)
+			goto abort;
+		data->fan[f_input][0] = rv;
+
+		rv = lm80_read_value(client, LM80_REG_FAN_MIN(1));
+		if (rv < 0)
+			goto abort;
+		data->fan[f_min][0] = rv;
+
+		rv = lm80_read_value(client, LM80_REG_FAN2);
+		if (rv < 0)
+			goto abort;
+		data->fan[f_input][1] = rv;
+
+		rv = lm80_read_value(client, LM80_REG_FAN_MIN(2));
+		if (rv < 0)
+			goto abort;
+		data->fan[f_min][1] = rv;
+
+		prev_rv = rv = lm80_read_value(client, LM80_REG_TEMP);
+		if (rv < 0)
+			goto abort;
+		rv = lm80_read_value(client, LM80_REG_RES);
+		if (rv < 0)
+			goto abort;
+		data->temp[t_input] = (prev_rv << 8) | (rv & 0xf0);
+
+		for (i = t_input + 1; i < t_num_temp; i++) {
+			rv = lm80_read_value(client, temp_regs[i]);
+			if (rv < 0)
+				goto abort;
+			data->temp[i] = rv << 8;
+		}
+
+		rv = lm80_read_value(client, LM80_REG_FANDIV);
+		if (rv < 0)
+			goto abort;
+		data->fan_div[0] = (rv >> 2) & 0x03;
+		data->fan_div[1] = (rv >> 4) & 0x03;
+
+		prev_rv = rv = lm80_read_value(client, LM80_REG_ALARM1);
+		if (rv < 0)
+			goto abort;
+		rv = lm80_read_value(client, LM80_REG_ALARM2);
+		if (rv < 0)
+			goto abort;
+		data->alarms = prev_rv + (rv << 8);
+
+		data->last_updated = jiffies;
+		data->valid = 1;
+		data->error = 0;
+	}
+	goto done;
+
+abort:
+	ret = ERR_PTR(rv);
+	data->valid = 0;
+	data->error = 1;
+
+done:
+	mutex_unlock(&data->update_lock);
+
+	return ret;
+}
 
 /*
  * Sysfs stuff
  */
 
-#define show_in(suffix, value) \
-static ssize_t show_in_##suffix(struct device *dev, \
-	struct device_attribute *attr, char *buf) \
-{ \
-	int nr = to_sensor_dev_attr(attr)->index; \
-	struct lm80_data *data = lm80_update_device(dev); \
-	if (IS_ERR(data)) \
-		return PTR_ERR(data); \
-	return sprintf(buf, "%d\n", IN_FROM_REG(data->value[nr])); \
+static ssize_t show_in(struct device *dev, struct device_attribute *attr,
+		       char *buf)
+{
+	struct lm80_data *data = lm80_update_device(dev);
+	int index = to_sensor_dev_attr_2(attr)->index;
+	int nr = to_sensor_dev_attr_2(attr)->nr;
+
+	if (IS_ERR(data))
+		return PTR_ERR(data);
+	return sprintf(buf, "%d\n", IN_FROM_REG(data->in[nr][index]));
 }
-show_in(min, in_min)
-show_in(max, in_max)
-show_in(input, in)
-
-#define set_in(suffix, value, reg) \
-static ssize_t set_in_##suffix(struct device *dev, \
-	struct device_attribute *attr, const char *buf, size_t count) \
-{ \
-	int nr = to_sensor_dev_attr(attr)->index; \
-	struct lm80_data *data = dev_get_drvdata(dev); \
-	struct i2c_client *client = data->client; \
-	long val; \
-	int err = kstrtol(buf, 10, &val); \
-	if (err < 0) \
-		return err; \
-\
-	mutex_lock(&data->update_lock);\
-	data->value[nr] = IN_TO_REG(val); \
-	lm80_write_value(client, reg(nr), data->value[nr]); \
-	mutex_unlock(&data->update_lock);\
-	return count; \
+
+static ssize_t set_in(struct device *dev, struct device_attribute *attr,
+		      const char *buf, size_t count)
+{
+	struct lm80_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
+	int index = to_sensor_dev_attr_2(attr)->index;
+	int nr = to_sensor_dev_attr_2(attr)->nr;
+	long val;
+	u8 reg;
+	int err = kstrtol(buf, 10, &val);
+	if (err < 0)
+		return err;
+
+	reg = nr == i_min ? LM80_REG_IN_MIN(index) : LM80_REG_IN_MAX(index);
+
+	mutex_lock(&data->update_lock);
+	data->in[nr][index] = IN_TO_REG(val);
+	lm80_write_value(client, reg, data->in[nr][index]);
+	mutex_unlock(&data->update_lock);
+	return count;
 }
-set_in(min, in_min, LM80_REG_IN_MIN)
-set_in(max, in_max, LM80_REG_IN_MAX)
-
-#define show_fan(suffix, value) \
-static ssize_t show_fan_##suffix(struct device *dev, \
-	struct device_attribute *attr, char *buf) \
-{ \
-	int nr = to_sensor_dev_attr(attr)->index; \
-	struct lm80_data *data = lm80_update_device(dev); \
-	if (IS_ERR(data)) \
-		return PTR_ERR(data); \
-	return sprintf(buf, "%d\n", FAN_FROM_REG(data->value[nr], \
-		       DIV_FROM_REG(data->fan_div[nr]))); \
+
+static ssize_t show_fan(struct device *dev, struct device_attribute *attr,
+			char *buf)
+{
+	int index = to_sensor_dev_attr_2(attr)->index;
+	int nr = to_sensor_dev_attr_2(attr)->nr;
+	struct lm80_data *data = lm80_update_device(dev);
+	if (IS_ERR(data))
+		return PTR_ERR(data);
+	return sprintf(buf, "%d\n", FAN_FROM_REG(data->fan[nr][index],
+		       DIV_FROM_REG(data->fan_div[index])));
 }
-show_fan(min, fan_min)
-show_fan(input, fan)
 
 static ssize_t show_fan_div(struct device *dev, struct device_attribute *attr,
 	char *buf)
@@ -232,7 +328,8 @@ static ssize_t show_fan_div(struct device *dev, struct device_attribute *attr,
 static ssize_t set_fan_min(struct device *dev, struct device_attribute *attr,
 	const char *buf, size_t count)
 {
-	int nr = to_sensor_dev_attr(attr)->index;
+	int index = to_sensor_dev_attr_2(attr)->index;
+	int nr = to_sensor_dev_attr_2(attr)->nr;
 	struct lm80_data *data = dev_get_drvdata(dev);
 	struct i2c_client *client = data->client;
 	unsigned long val;
@@ -241,8 +338,10 @@ static ssize_t set_fan_min(struct device *dev, struct device_attribute *attr,
 		return err;
 
 	mutex_lock(&data->update_lock);
-	data->fan_min[nr] = FAN_TO_REG(val, DIV_FROM_REG(data->fan_div[nr]));
-	lm80_write_value(client, LM80_REG_FAN_MIN(nr + 1), data->fan_min[nr]);
+	data->fan[nr][index] = FAN_TO_REG(val,
+					  DIV_FROM_REG(data->fan_div[index]));
+	lm80_write_value(client, LM80_REG_FAN_MIN(index + 1),
+			 data->fan[nr][index]);
 	mutex_unlock(&data->update_lock);
 	return count;
 }
@@ -267,7 +366,7 @@ static ssize_t set_fan_div(struct device *dev, struct device_attribute *attr,
 
 	/* Save fan_min */
 	mutex_lock(&data->update_lock);
-	min = FAN_FROM_REG(data->fan_min[nr],
+	min = FAN_FROM_REG(data->fan[f_min][nr],
 			   DIV_FROM_REG(data->fan_div[nr]));
 
 	switch (val) {
@@ -291,62 +390,47 @@ static ssize_t set_fan_div(struct device *dev, struct device_attribute *attr,
 		return -EINVAL;
 	}
 
-	reg = (lm80_read_value(client, LM80_REG_FANDIV) & ~(3 << (2 * (nr + 1))))
-	    | (data->fan_div[nr] << (2 * (nr + 1)));
+	reg = (lm80_read_value(client, LM80_REG_FANDIV) &
+	       ~(3 << (2 * (nr + 1)))) | (data->fan_div[nr] << (2 * (nr + 1)));
 	lm80_write_value(client, LM80_REG_FANDIV, reg);
 
 	/* Restore fan_min */
-	data->fan_min[nr] = FAN_TO_REG(min, DIV_FROM_REG(data->fan_div[nr]));
-	lm80_write_value(client, LM80_REG_FAN_MIN(nr + 1), data->fan_min[nr]);
+	data->fan[f_min][nr] = FAN_TO_REG(min, DIV_FROM_REG(data->fan_div[nr]));
+	lm80_write_value(client, LM80_REG_FAN_MIN(nr + 1),
+			 data->fan[f_min][nr]);
 	mutex_unlock(&data->update_lock);
 
 	return count;
 }
 
-static ssize_t show_temp_input1(struct device *dev,
-	struct device_attribute *attr, char *buf)
+static ssize_t show_temp(struct device *dev, struct device_attribute *devattr,
+			 char *buf)
 {
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
 	struct lm80_data *data = lm80_update_device(dev);
 	if (IS_ERR(data))
 		return PTR_ERR(data);
-	return sprintf(buf, "%ld\n", TEMP_FROM_REG(data->temp));
+	return sprintf(buf, "%d\n", TEMP_FROM_REG(data->temp[attr->index]));
 }
 
-#define show_temp(suffix, value) \
-static ssize_t show_temp_##suffix(struct device *dev, \
-	struct device_attribute *attr, char *buf) \
-{ \
-	struct lm80_data *data = lm80_update_device(dev); \
-	if (IS_ERR(data)) \
-		return PTR_ERR(data); \
-	return sprintf(buf, "%d\n", TEMP_LIMIT_FROM_REG(data->value)); \
-}
-show_temp(hot_max, temp_hot_max);
-show_temp(hot_hyst, temp_hot_hyst);
-show_temp(os_max, temp_os_max);
-show_temp(os_hyst, temp_os_hyst);
-
-#define set_temp(suffix, value, reg) \
-static ssize_t set_temp_##suffix(struct device *dev, \
-	struct device_attribute *attr, const char *buf, size_t count) \
-{ \
-	struct lm80_data *data = dev_get_drvdata(dev); \
-	struct i2c_client *client = data->client; \
-	long val; \
-	int err = kstrtol(buf, 10, &val); \
-	if (err < 0) \
-		return err; \
-\
-	mutex_lock(&data->update_lock); \
-	data->value = TEMP_LIMIT_TO_REG(val); \
-	lm80_write_value(client, reg, data->value); \
-	mutex_unlock(&data->update_lock); \
-	return count; \
+static ssize_t set_temp(struct device *dev, struct device_attribute *devattr,
+			const char *buf, size_t count)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	struct lm80_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
+	int nr = attr->index;
+	long val;
+	int err = kstrtol(buf, 10, &val);
+	if (err < 0)
+		return err;
+
+	mutex_lock(&data->update_lock);
+	data->temp[nr] = TEMP_TO_REG(val);
+	lm80_write_value(client, temp_regs[nr], data->temp[nr] >> 8);
+	mutex_unlock(&data->update_lock);
+	return count;
 }
-set_temp(hot_max, temp_hot_max, LM80_REG_TEMP_HOT_MAX);
-set_temp(hot_hyst, temp_hot_hyst, LM80_REG_TEMP_HOT_HYST);
-set_temp(os_max, temp_os_max, LM80_REG_TEMP_OS_MAX);
-set_temp(os_hyst, temp_os_hyst, LM80_REG_TEMP_OS_HYST);
 
 static ssize_t show_alarms(struct device *dev, struct device_attribute *attr,
 			   char *buf)
@@ -367,60 +451,60 @@ static ssize_t show_alarm(struct device *dev, struct device_attribute *attr,
 	return sprintf(buf, "%u\n", (data->alarms >> bitnr) & 1);
 }
 
-static SENSOR_DEVICE_ATTR(in0_min, S_IWUSR | S_IRUGO,
-		show_in_min, set_in_min, 0);
-static SENSOR_DEVICE_ATTR(in1_min, S_IWUSR | S_IRUGO,
-		show_in_min, set_in_min, 1);
-static SENSOR_DEVICE_ATTR(in2_min, S_IWUSR | S_IRUGO,
-		show_in_min, set_in_min, 2);
-static SENSOR_DEVICE_ATTR(in3_min, S_IWUSR | S_IRUGO,
-		show_in_min, set_in_min, 3);
-static SENSOR_DEVICE_ATTR(in4_min, S_IWUSR | S_IRUGO,
-		show_in_min, set_in_min, 4);
-static SENSOR_DEVICE_ATTR(in5_min, S_IWUSR | S_IRUGO,
-		show_in_min, set_in_min, 5);
-static SENSOR_DEVICE_ATTR(in6_min, S_IWUSR | S_IRUGO,
-		show_in_min, set_in_min, 6);
-static SENSOR_DEVICE_ATTR(in0_max, S_IWUSR | S_IRUGO,
-		show_in_max, set_in_max, 0);
-static SENSOR_DEVICE_ATTR(in1_max, S_IWUSR | S_IRUGO,
-		show_in_max, set_in_max, 1);
-static SENSOR_DEVICE_ATTR(in2_max, S_IWUSR | S_IRUGO,
-		show_in_max, set_in_max, 2);
-static SENSOR_DEVICE_ATTR(in3_max, S_IWUSR | S_IRUGO,
-		show_in_max, set_in_max, 3);
-static SENSOR_DEVICE_ATTR(in4_max, S_IWUSR | S_IRUGO,
-		show_in_max, set_in_max, 4);
-static SENSOR_DEVICE_ATTR(in5_max, S_IWUSR | S_IRUGO,
-		show_in_max, set_in_max, 5);
-static SENSOR_DEVICE_ATTR(in6_max, S_IWUSR | S_IRUGO,
-		show_in_max, set_in_max, 6);
-static SENSOR_DEVICE_ATTR(in0_input, S_IRUGO, show_in_input, NULL, 0);
-static SENSOR_DEVICE_ATTR(in1_input, S_IRUGO, show_in_input, NULL, 1);
-static SENSOR_DEVICE_ATTR(in2_input, S_IRUGO, show_in_input, NULL, 2);
-static SENSOR_DEVICE_ATTR(in3_input, S_IRUGO, show_in_input, NULL, 3);
-static SENSOR_DEVICE_ATTR(in4_input, S_IRUGO, show_in_input, NULL, 4);
-static SENSOR_DEVICE_ATTR(in5_input, S_IRUGO, show_in_input, NULL, 5);
-static SENSOR_DEVICE_ATTR(in6_input, S_IRUGO, show_in_input, NULL, 6);
-static SENSOR_DEVICE_ATTR(fan1_min, S_IWUSR | S_IRUGO,
-		show_fan_min, set_fan_min, 0);
-static SENSOR_DEVICE_ATTR(fan2_min, S_IWUSR | S_IRUGO,
-		show_fan_min, set_fan_min, 1);
-static SENSOR_DEVICE_ATTR(fan1_input, S_IRUGO, show_fan_input, NULL, 0);
-static SENSOR_DEVICE_ATTR(fan2_input, S_IRUGO, show_fan_input, NULL, 1);
+static SENSOR_DEVICE_ATTR_2(in0_min, S_IWUSR | S_IRUGO,
+		show_in, set_in, i_min, 0);
+static SENSOR_DEVICE_ATTR_2(in1_min, S_IWUSR | S_IRUGO,
+		show_in, set_in, i_min, 1);
+static SENSOR_DEVICE_ATTR_2(in2_min, S_IWUSR | S_IRUGO,
+		show_in, set_in, i_min, 2);
+static SENSOR_DEVICE_ATTR_2(in3_min, S_IWUSR | S_IRUGO,
+		show_in, set_in, i_min, 3);
+static SENSOR_DEVICE_ATTR_2(in4_min, S_IWUSR | S_IRUGO,
+		show_in, set_in, i_min, 4);
+static SENSOR_DEVICE_ATTR_2(in5_min, S_IWUSR | S_IRUGO,
+		show_in, set_in, i_min, 5);
+static SENSOR_DEVICE_ATTR_2(in6_min, S_IWUSR | S_IRUGO,
+		show_in, set_in, i_min, 6);
+static SENSOR_DEVICE_ATTR_2(in0_max, S_IWUSR | S_IRUGO,
+		show_in, set_in, i_max, 0);
+static SENSOR_DEVICE_ATTR_2(in1_max, S_IWUSR | S_IRUGO,
+		show_in, set_in, i_max, 1);
+static SENSOR_DEVICE_ATTR_2(in2_max, S_IWUSR | S_IRUGO,
+		show_in, set_in, i_max, 2);
+static SENSOR_DEVICE_ATTR_2(in3_max, S_IWUSR | S_IRUGO,
+		show_in, set_in, i_max, 3);
+static SENSOR_DEVICE_ATTR_2(in4_max, S_IWUSR | S_IRUGO,
+		show_in, set_in, i_max, 4);
+static SENSOR_DEVICE_ATTR_2(in5_max, S_IWUSR | S_IRUGO,
+		show_in, set_in, i_max, 5);
+static SENSOR_DEVICE_ATTR_2(in6_max, S_IWUSR | S_IRUGO,
+		show_in, set_in, i_max, 6);
+static SENSOR_DEVICE_ATTR_2(in0_input, S_IRUGO, show_in, NULL, i_input, 0);
+static SENSOR_DEVICE_ATTR_2(in1_input, S_IRUGO, show_in, NULL, i_input, 1);
+static SENSOR_DEVICE_ATTR_2(in2_input, S_IRUGO, show_in, NULL, i_input, 2);
+static SENSOR_DEVICE_ATTR_2(in3_input, S_IRUGO, show_in, NULL, i_input, 3);
+static SENSOR_DEVICE_ATTR_2(in4_input, S_IRUGO, show_in, NULL, i_input, 4);
+static SENSOR_DEVICE_ATTR_2(in5_input, S_IRUGO, show_in, NULL, i_input, 5);
+static SENSOR_DEVICE_ATTR_2(in6_input, S_IRUGO, show_in, NULL, i_input, 6);
+static SENSOR_DEVICE_ATTR_2(fan1_min, S_IWUSR | S_IRUGO,
+		show_fan, set_fan_min, f_min, 0);
+static SENSOR_DEVICE_ATTR_2(fan2_min, S_IWUSR | S_IRUGO,
+		show_fan, set_fan_min, f_min, 1);
+static SENSOR_DEVICE_ATTR_2(fan1_input, S_IRUGO, show_fan, NULL, f_input, 0);
+static SENSOR_DEVICE_ATTR_2(fan2_input, S_IRUGO, show_fan, NULL, f_input, 1);
 static SENSOR_DEVICE_ATTR(fan1_div, S_IWUSR | S_IRUGO,
 		show_fan_div, set_fan_div, 0);
 static SENSOR_DEVICE_ATTR(fan2_div, S_IWUSR | S_IRUGO,
 		show_fan_div, set_fan_div, 1);
-static DEVICE_ATTR(temp1_input, S_IRUGO, show_temp_input1, NULL);
-static DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO, show_temp_hot_max,
-	set_temp_hot_max);
-static DEVICE_ATTR(temp1_max_hyst, S_IWUSR | S_IRUGO, show_temp_hot_hyst,
-	set_temp_hot_hyst);
-static DEVICE_ATTR(temp1_crit, S_IWUSR | S_IRUGO, show_temp_os_max,
-	set_temp_os_max);
-static DEVICE_ATTR(temp1_crit_hyst, S_IWUSR | S_IRUGO, show_temp_os_hyst,
-	set_temp_os_hyst);
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, show_temp, NULL, t_input);
+static SENSOR_DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO, show_temp,
+		set_temp, t_hot_max);
+static SENSOR_DEVICE_ATTR(temp1_max_hyst, S_IWUSR | S_IRUGO, show_temp,
+		set_temp, t_hot_hyst);
+static SENSOR_DEVICE_ATTR(temp1_crit, S_IWUSR | S_IRUGO, show_temp,
+		set_temp, t_os_max);
+static SENSOR_DEVICE_ATTR(temp1_crit_hyst, S_IWUSR | S_IRUGO, show_temp,
+		set_temp, t_os_hyst);
 static DEVICE_ATTR(alarms, S_IRUGO, show_alarms, NULL);
 static SENSOR_DEVICE_ATTR(in0_alarm, S_IRUGO, show_alarm, NULL, 0);
 static SENSOR_DEVICE_ATTR(in1_alarm, S_IRUGO, show_alarm, NULL, 1);
@@ -466,11 +550,11 @@ static struct attribute *lm80_attrs[] = {
 	&sensor_dev_attr_fan2_input.dev_attr.attr,
 	&sensor_dev_attr_fan1_div.dev_attr.attr,
 	&sensor_dev_attr_fan2_div.dev_attr.attr,
-	&dev_attr_temp1_input.attr,
-	&dev_attr_temp1_max.attr,
-	&dev_attr_temp1_max_hyst.attr,
-	&dev_attr_temp1_crit.attr,
-	&dev_attr_temp1_crit_hyst.attr,
+	&sensor_dev_attr_temp1_input.dev_attr.attr,
+	&sensor_dev_attr_temp1_max.dev_attr.attr,
+	&sensor_dev_attr_temp1_max_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit_hyst.dev_attr.attr,
 	&dev_attr_alarms.attr,
 	&sensor_dev_attr_in0_alarm.dev_attr.attr,
 	&sensor_dev_attr_in1_alarm.dev_attr.attr,
@@ -551,8 +635,8 @@ static int lm80_probe(struct i2c_client *client,
 	lm80_init_client(client);
 
 	/* A few vars need to be filled upon startup */
-	data->fan_min[0] = lm80_read_value(client, LM80_REG_FAN_MIN(1));
-	data->fan_min[1] = lm80_read_value(client, LM80_REG_FAN_MIN(2));
+	data->fan[f_min][0] = lm80_read_value(client, LM80_REG_FAN_MIN(1));
+	data->fan[f_min][1] = lm80_read_value(client, LM80_REG_FAN_MIN(2));
 
 	hwmon_dev = devm_hwmon_device_register_with_groups(dev, client->name,
 							   data, lm80_groups);
@@ -560,143 +644,27 @@ static int lm80_probe(struct i2c_client *client,
 	return PTR_ERR_OR_ZERO(hwmon_dev);
 }
 
-static int lm80_read_value(struct i2c_client *client, u8 reg)
-{
-	return i2c_smbus_read_byte_data(client, reg);
-}
-
-static int lm80_write_value(struct i2c_client *client, u8 reg, u8 value)
-{
-	return i2c_smbus_write_byte_data(client, reg, value);
-}
-
-/* Called when we have found a new LM80. */
-static void lm80_init_client(struct i2c_client *client)
-{
-	/*
-	 * Reset all except Watchdog values and last conversion values
-	 * This sets fan-divs to 2, among others. This makes most other
-	 * initializations unnecessary
-	 */
-	lm80_write_value(client, LM80_REG_CONFIG, 0x80);
-	/* Set 11-bit temperature resolution */
-	lm80_write_value(client, LM80_REG_RES, 0x08);
-
-	/* Start monitoring */
-	lm80_write_value(client, LM80_REG_CONFIG, 0x01);
-}
-
-static struct lm80_data *lm80_update_device(struct device *dev)
-{
-	struct lm80_data *data = dev_get_drvdata(dev);
-	struct i2c_client *client = data->client;
-	int i;
-	int rv;
-	int prev_rv;
-	struct lm80_data *ret = data;
-
-	mutex_lock(&data->update_lock);
-
-	if (data->error)
-		lm80_init_client(client);
-
-	if (time_after(jiffies, data->last_updated + 2 * HZ) || !data->valid) {
-		dev_dbg(dev, "Starting lm80 update\n");
-		for (i = 0; i <= 6; i++) {
-			rv = lm80_read_value(client, LM80_REG_IN(i));
-			if (rv < 0)
-				goto abort;
-			data->in[i] = rv;
-
-			rv = lm80_read_value(client, LM80_REG_IN_MIN(i));
-			if (rv < 0)
-				goto abort;
-			data->in_min[i] = rv;
-
-			rv = lm80_read_value(client, LM80_REG_IN_MAX(i));
-			if (rv < 0)
-				goto abort;
-			data->in_max[i] = rv;
-		}
-
-		rv = lm80_read_value(client, LM80_REG_FAN1);
-		if (rv < 0)
-			goto abort;
-		data->fan[0] = rv;
-
-		rv = lm80_read_value(client, LM80_REG_FAN_MIN(1));
-		if (rv < 0)
-			goto abort;
-		data->fan_min[0] = rv;
-
-		rv = lm80_read_value(client, LM80_REG_FAN2);
-		if (rv < 0)
-			goto abort;
-		data->fan[1] = rv;
-
-		rv = lm80_read_value(client, LM80_REG_FAN_MIN(2));
-		if (rv < 0)
-			goto abort;
-		data->fan_min[1] = rv;
-
-		prev_rv = rv = lm80_read_value(client, LM80_REG_TEMP);
-		if (rv < 0)
-			goto abort;
-		rv = lm80_read_value(client, LM80_REG_RES);
-		if (rv < 0)
-			goto abort;
-		data->temp = (prev_rv << 8) | (rv & 0xf0);
-
-		rv = lm80_read_value(client, LM80_REG_TEMP_OS_MAX);
-		if (rv < 0)
-			goto abort;
-		data->temp_os_max = rv;
-
-		rv = lm80_read_value(client, LM80_REG_TEMP_OS_HYST);
-		if (rv < 0)
-			goto abort;
-		data->temp_os_hyst = rv;
-
-		rv = lm80_read_value(client, LM80_REG_TEMP_HOT_MAX);
-		if (rv < 0)
-			goto abort;
-		data->temp_hot_max = rv;
-
-		rv = lm80_read_value(client, LM80_REG_TEMP_HOT_HYST);
-		if (rv < 0)
-			goto abort;
-		data->temp_hot_hyst = rv;
-
-		rv = lm80_read_value(client, LM80_REG_FANDIV);
-		if (rv < 0)
-			goto abort;
-		data->fan_div[0] = (rv >> 2) & 0x03;
-		data->fan_div[1] = (rv >> 4) & 0x03;
-
-		prev_rv = rv = lm80_read_value(client, LM80_REG_ALARM1);
-		if (rv < 0)
-			goto abort;
-		rv = lm80_read_value(client, LM80_REG_ALARM2);
-		if (rv < 0)
-			goto abort;
-		data->alarms = prev_rv + (rv << 8);
-
-		data->last_updated = jiffies;
-		data->valid = 1;
-		data->error = 0;
-	}
-	goto done;
-
-abort:
-	ret = ERR_PTR(rv);
-	data->valid = 0;
-	data->error = 1;
+/*
+ * Driver data (common to all clients)
+ */
 
-done:
-	mutex_unlock(&data->update_lock);
+static const struct i2c_device_id lm80_id[] = {
+	{ "lm80", 0 },
+	{ "lm96080", 1 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, lm80_id);
 
-	return ret;
-}
+static struct i2c_driver lm80_driver = {
+	.class		= I2C_CLASS_HWMON,
+	.driver = {
+		.name	= "lm80",
+	},
+	.probe		= lm80_probe,
+	.id_table	= lm80_id,
+	.detect		= lm80_detect,
+	.address_list	= normal_i2c,
+};
 
 module_i2c_driver(lm80_driver);
 
diff --git a/drivers/hwmon/lm83.c b/drivers/hwmon/lm83.c
index be02155788c..9e4d0e1d3c4 100644
--- a/drivers/hwmon/lm83.c
+++ b/drivers/hwmon/lm83.c
@@ -25,10 +25,6 @@
  * 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>
@@ -111,45 +107,12 @@ static const u8 LM83_REG_W_HIGH[] = {
 };
 
 /*
- * Functions declaration
- */
-
-static int lm83_detect(struct i2c_client *new_client,
-		       struct i2c_board_info *info);
-static int lm83_probe(struct i2c_client *client,
-		      const struct i2c_device_id *id);
-static int lm83_remove(struct i2c_client *client);
-static struct lm83_data *lm83_update_device(struct device *dev);
-
-/*
- * Driver data (common to all clients)
- */
-
-static const struct i2c_device_id lm83_id[] = {
-	{ "lm83", lm83 },
-	{ "lm82", lm82 },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, lm83_id);
-
-static struct i2c_driver lm83_driver = {
-	.class		= I2C_CLASS_HWMON,
-	.driver = {
-		.name	= "lm83",
-	},
-	.probe		= lm83_probe,
-	.remove		= lm83_remove,
-	.id_table	= lm83_id,
-	.detect		= lm83_detect,
-	.address_list	= normal_i2c,
-};
-
-/*
  * Client data (each client gets its own)
  */
 
 struct lm83_data {
-	struct device *hwmon_dev;
+	struct i2c_client *client;
+	const struct attribute_group *groups[3];
 	struct mutex update_lock;
 	char valid; /* zero until following fields are valid */
 	unsigned long last_updated; /* in jiffies */
@@ -161,6 +124,36 @@ struct lm83_data {
 	u16 alarms; /* bitvector, combined */
 };
 
+static struct lm83_data *lm83_update_device(struct device *dev)
+{
+	struct lm83_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
+
+	mutex_lock(&data->update_lock);
+
+	if (time_after(jiffies, data->last_updated + HZ * 2) || !data->valid) {
+		int nr;
+
+		dev_dbg(&client->dev, "Updating lm83 data.\n");
+		for (nr = 0; nr < 9; nr++) {
+			data->temp[nr] =
+			    i2c_smbus_read_byte_data(client,
+			    LM83_REG_R_TEMP[nr]);
+		}
+		data->alarms =
+		    i2c_smbus_read_byte_data(client, LM83_REG_R_STATUS1)
+		    + (i2c_smbus_read_byte_data(client, LM83_REG_R_STATUS2)
+		    << 8);
+
+		data->last_updated = jiffies;
+		data->valid = 1;
+	}
+
+	mutex_unlock(&data->update_lock);
+
+	return data;
+}
+
 /*
  * Sysfs stuff
  */
@@ -177,8 +170,8 @@ static ssize_t set_temp(struct device *dev, struct device_attribute *devattr,
 			const char *buf, size_t count)
 {
 	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
-	struct i2c_client *client = to_i2c_client(dev);
-	struct lm83_data *data = i2c_get_clientdata(client);
+	struct lm83_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
 	long val;
 	int nr = attr->index;
 	int err;
@@ -340,15 +333,15 @@ static int lm83_detect(struct i2c_client *new_client,
 static int lm83_probe(struct i2c_client *new_client,
 		      const struct i2c_device_id *id)
 {
+	struct device *hwmon_dev;
 	struct lm83_data *data;
-	int err;
 
 	data = devm_kzalloc(&new_client->dev, sizeof(struct lm83_data),
 			    GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
 
-	i2c_set_clientdata(new_client, data);
+	data->client = new_client;
 	mutex_init(&data->update_lock);
 
 	/*
@@ -357,72 +350,37 @@ static int lm83_probe(struct i2c_client *new_client,
 	 * at the same register as the LM83 temp3 entry - so we
 	 * declare 1 and 3 common, and then 2 and 4 only for the LM83.
 	 */
-
-	err = sysfs_create_group(&new_client->dev.kobj, &lm83_group);
-	if (err)
-		return err;
-
-	if (id->driver_data == lm83) {
-		err = sysfs_create_group(&new_client->dev.kobj,
-					 &lm83_group_opt);
-		if (err)
-			goto exit_remove_files;
-	}
-
-	data->hwmon_dev = hwmon_device_register(&new_client->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(&new_client->dev.kobj, &lm83_group);
-	sysfs_remove_group(&new_client->dev.kobj, &lm83_group_opt);
-	return err;
-}
-
-static int lm83_remove(struct i2c_client *client)
-{
-	struct lm83_data *data = i2c_get_clientdata(client);
-
-	hwmon_device_unregister(data->hwmon_dev);
-	sysfs_remove_group(&client->dev.kobj, &lm83_group);
-	sysfs_remove_group(&client->dev.kobj, &lm83_group_opt);
-
-	return 0;
+	data->groups[0] = &lm83_group;
+	if (id->driver_data == lm83)
+		data->groups[1] = &lm83_group_opt;
+
+	hwmon_dev = devm_hwmon_device_register_with_groups(&new_client->dev,
+							   new_client->name,
+							   data, data->groups);
+	return PTR_ERR_OR_ZERO(hwmon_dev);
 }
 
-static struct lm83_data *lm83_update_device(struct device *dev)
-{
-	struct i2c_client *client = to_i2c_client(dev);
-	struct lm83_data *data = i2c_get_clientdata(client);
-
-	mutex_lock(&data->update_lock);
-
-	if (time_after(jiffies, data->last_updated + HZ * 2) || !data->valid) {
-		int nr;
-
-		dev_dbg(&client->dev, "Updating lm83 data.\n");
-		for (nr = 0; nr < 9; nr++) {
-			data->temp[nr] =
-			    i2c_smbus_read_byte_data(client,
-			    LM83_REG_R_TEMP[nr]);
-		}
-		data->alarms =
-		    i2c_smbus_read_byte_data(client, LM83_REG_R_STATUS1)
-		    + (i2c_smbus_read_byte_data(client, LM83_REG_R_STATUS2)
-		    << 8);
-
-		data->last_updated = jiffies;
-		data->valid = 1;
-	}
+/*
+ * Driver data (common to all clients)
+ */
 
-	mutex_unlock(&data->update_lock);
+static const struct i2c_device_id lm83_id[] = {
+	{ "lm83", lm83 },
+	{ "lm82", lm82 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, lm83_id);
 
-	return data;
-}
+static struct i2c_driver lm83_driver = {
+	.class		= I2C_CLASS_HWMON,
+	.driver = {
+		.name	= "lm83",
+	},
+	.probe		= lm83_probe,
+	.id_table	= lm83_id,
+	.detect		= lm83_detect,
+	.address_list	= normal_i2c,
+};
 
 module_i2c_driver(lm83_driver);
 
diff --git a/drivers/hwmon/lm92.c b/drivers/hwmon/lm92.c
index b9022db6511..d2060e245ff 100644
--- a/drivers/hwmon/lm92.c
+++ b/drivers/hwmon/lm92.c
@@ -34,10 +34,6 @@
  * 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>
@@ -93,46 +89,53 @@ static inline u8 ALARMS_FROM_REG(s16 reg)
 	return reg & 0x0007;
 }
 
-/* Driver data (common to all clients) */
-static struct i2c_driver lm92_driver;
+enum temp_index {
+	t_input,
+	t_crit,
+	t_min,
+	t_max,
+	t_hyst,
+	t_num_regs
+};
+
+static const u8 regs[t_num_regs] = {
+	[t_input] = LM92_REG_TEMP,
+	[t_crit] = LM92_REG_TEMP_CRIT,
+	[t_min] = LM92_REG_TEMP_LOW,
+	[t_max] = LM92_REG_TEMP_HIGH,
+	[t_hyst] = LM92_REG_TEMP_HYST,
+};
 
 /* Client data (each client gets its own) */
 struct lm92_data {
-	struct device *hwmon_dev;
+	struct i2c_client *client;
 	struct mutex update_lock;
 	char valid; /* zero until following fields are valid */
 	unsigned long last_updated; /* in jiffies */
 
 	/* registers values */
-	s16 temp1_input, temp1_crit, temp1_min, temp1_max, temp1_hyst;
+	s16 temp[t_num_regs];	/* index with enum temp_index */
 };
 
-
 /*
  * Sysfs attributes and callback functions
  */
 
 static struct lm92_data *lm92_update_device(struct device *dev)
 {
-	struct i2c_client *client = to_i2c_client(dev);
-	struct lm92_data *data = i2c_get_clientdata(client);
+	struct lm92_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
+	int i;
 
 	mutex_lock(&data->update_lock);
 
 	if (time_after(jiffies, data->last_updated + HZ)
 	 || !data->valid) {
 		dev_dbg(&client->dev, "Updating lm92 data\n");
-		data->temp1_input = i2c_smbus_read_word_swapped(client,
-				    LM92_REG_TEMP);
-		data->temp1_hyst = i2c_smbus_read_word_swapped(client,
-				    LM92_REG_TEMP_HYST);
-		data->temp1_crit = i2c_smbus_read_word_swapped(client,
-				    LM92_REG_TEMP_CRIT);
-		data->temp1_min = i2c_smbus_read_word_swapped(client,
-				    LM92_REG_TEMP_LOW);
-		data->temp1_max = i2c_smbus_read_word_swapped(client,
-				    LM92_REG_TEMP_HIGH);
-
+		for (i = 0; i < t_num_regs; i++) {
+			data->temp[i] =
+				i2c_smbus_read_word_swapped(client, regs[i]);
+		}
 		data->last_updated = jiffies;
 		data->valid = 1;
 	}
@@ -142,68 +145,60 @@ static struct lm92_data *lm92_update_device(struct device *dev)
 	return data;
 }
 
-#define show_temp(value) \
-static ssize_t show_##value(struct device *dev, struct device_attribute *attr, \
-			    char *buf) \
-{ \
-	struct lm92_data *data = lm92_update_device(dev); \
-	return sprintf(buf, "%d\n", TEMP_FROM_REG(data->value)); \
-}
-show_temp(temp1_input);
-show_temp(temp1_crit);
-show_temp(temp1_min);
-show_temp(temp1_max);
-
-#define set_temp(value, reg) \
-static ssize_t set_##value(struct device *dev, struct device_attribute *attr, \
-			   const char *buf, \
-	size_t count) \
-{ \
-	struct i2c_client *client = to_i2c_client(dev); \
-	struct lm92_data *data = i2c_get_clientdata(client); \
-	long val; \
-	int err = kstrtol(buf, 10, &val); \
-	if (err) \
-		return err; \
-\
-	mutex_lock(&data->update_lock); \
-	data->value = TEMP_TO_REG(val); \
-	i2c_smbus_write_word_swapped(client, reg, data->value); \
-	mutex_unlock(&data->update_lock); \
-	return count; \
+static ssize_t show_temp(struct device *dev, struct device_attribute *devattr,
+			 char *buf)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	struct lm92_data *data = lm92_update_device(dev);
+
+	return sprintf(buf, "%d\n", TEMP_FROM_REG(data->temp[attr->index]));
 }
-set_temp(temp1_crit, LM92_REG_TEMP_CRIT);
-set_temp(temp1_min, LM92_REG_TEMP_LOW);
-set_temp(temp1_max, LM92_REG_TEMP_HIGH);
 
-static ssize_t show_temp1_crit_hyst(struct device *dev,
-				    struct device_attribute *attr, char *buf)
+static ssize_t set_temp(struct device *dev, struct device_attribute *devattr,
+			   const char *buf, size_t count)
 {
-	struct lm92_data *data = lm92_update_device(dev);
-	return sprintf(buf, "%d\n", TEMP_FROM_REG(data->temp1_crit)
-		       - TEMP_FROM_REG(data->temp1_hyst));
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	struct lm92_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
+	int nr = attr->index;
+	long val;
+	int err;
+	
+	err = kstrtol(buf, 10, &val);
+	if (err)
+		return err;
+
+	mutex_lock(&data->update_lock);
+	data->temp[nr] = TEMP_TO_REG(val);
+	i2c_smbus_write_word_swapped(client, regs[nr], data->temp[nr]);
+	mutex_unlock(&data->update_lock);
+	return count;
 }
-static ssize_t show_temp1_max_hyst(struct device *dev,
-				   struct device_attribute *attr, char *buf)
+
+static ssize_t show_temp_hyst(struct device *dev,
+			      struct device_attribute *devattr, char *buf)
 {
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
 	struct lm92_data *data = lm92_update_device(dev);
-	return sprintf(buf, "%d\n", TEMP_FROM_REG(data->temp1_max)
-		       - TEMP_FROM_REG(data->temp1_hyst));
+	return sprintf(buf, "%d\n", TEMP_FROM_REG(data->temp[attr->index])
+		       - TEMP_FROM_REG(data->temp[t_hyst]));
 }
-static ssize_t show_temp1_min_hyst(struct device *dev,
-				   struct device_attribute *attr, char *buf)
+
+static ssize_t show_temp_min_hyst(struct device *dev,
+				  struct device_attribute *attr, char *buf)
 {
 	struct lm92_data *data = lm92_update_device(dev);
-	return sprintf(buf, "%d\n", TEMP_FROM_REG(data->temp1_min)
-		       + TEMP_FROM_REG(data->temp1_hyst));
+	return sprintf(buf, "%d\n", TEMP_FROM_REG(data->temp[t_min])
+		       + TEMP_FROM_REG(data->temp[t_hyst]));
 }
 
-static ssize_t set_temp1_crit_hyst(struct device *dev,
-				   struct device_attribute *attr,
-				   const char *buf, size_t count)
+static ssize_t set_temp_hyst(struct device *dev,
+			     struct device_attribute *devattr,
+			     const char *buf, size_t count)
 {
-	struct i2c_client *client = to_i2c_client(dev);
-	struct lm92_data *data = i2c_get_clientdata(client);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	struct lm92_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
 	long val;
 	int err;
 
@@ -212,9 +207,9 @@ static ssize_t set_temp1_crit_hyst(struct device *dev,
 		return err;
 
 	mutex_lock(&data->update_lock);
-	data->temp1_hyst = TEMP_FROM_REG(data->temp1_crit) - val;
+	data->temp[t_hyst] = TEMP_FROM_REG(data->temp[attr->index]) - val;
 	i2c_smbus_write_word_swapped(client, LM92_REG_TEMP_HYST,
-				     TEMP_TO_REG(data->temp1_hyst));
+				     TEMP_TO_REG(data->temp[t_hyst]));
 	mutex_unlock(&data->update_lock);
 	return count;
 }
@@ -223,7 +218,7 @@ static ssize_t show_alarms(struct device *dev, struct device_attribute *attr,
 			   char *buf)
 {
 	struct lm92_data *data = lm92_update_device(dev);
-	return sprintf(buf, "%d\n", ALARMS_FROM_REG(data->temp1_input));
+	return sprintf(buf, "%d\n", ALARMS_FROM_REG(data->temp[t_input]));
 }
 
 static ssize_t show_alarm(struct device *dev, struct device_attribute *attr,
@@ -231,26 +226,25 @@ static ssize_t show_alarm(struct device *dev, struct device_attribute *attr,
 {
 	int bitnr = to_sensor_dev_attr(attr)->index;
 	struct lm92_data *data = lm92_update_device(dev);
-	return sprintf(buf, "%d\n", (data->temp1_input >> bitnr) & 1);
+	return sprintf(buf, "%d\n", (data->temp[t_input] >> bitnr) & 1);
 }
 
-static DEVICE_ATTR(temp1_input, S_IRUGO, show_temp1_input, NULL);
-static DEVICE_ATTR(temp1_crit, S_IWUSR | S_IRUGO, show_temp1_crit,
-	set_temp1_crit);
-static DEVICE_ATTR(temp1_crit_hyst, S_IWUSR | S_IRUGO, show_temp1_crit_hyst,
-	set_temp1_crit_hyst);
-static DEVICE_ATTR(temp1_min, S_IWUSR | S_IRUGO, show_temp1_min,
-	set_temp1_min);
-static DEVICE_ATTR(temp1_min_hyst, S_IRUGO, show_temp1_min_hyst, NULL);
-static DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO, show_temp1_max,
-	set_temp1_max);
-static DEVICE_ATTR(temp1_max_hyst, S_IRUGO, show_temp1_max_hyst, NULL);
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, show_temp, NULL, t_input);
+static SENSOR_DEVICE_ATTR(temp1_crit, S_IWUSR | S_IRUGO, show_temp, set_temp,
+			  t_crit);
+static SENSOR_DEVICE_ATTR(temp1_crit_hyst, S_IWUSR | S_IRUGO, show_temp_hyst,
+			  set_temp_hyst, t_crit);
+static SENSOR_DEVICE_ATTR(temp1_min, S_IWUSR | S_IRUGO, show_temp, set_temp,
+			  t_min);
+static DEVICE_ATTR(temp1_min_hyst, S_IRUGO, show_temp_min_hyst, NULL);
+static SENSOR_DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO, show_temp, set_temp,
+			  t_max);
+static SENSOR_DEVICE_ATTR(temp1_max_hyst, S_IRUGO, show_temp_hyst, NULL, t_max);
 static DEVICE_ATTR(alarms, S_IRUGO, show_alarms, NULL);
 static SENSOR_DEVICE_ATTR(temp1_crit_alarm, S_IRUGO, show_alarm, NULL, 2);
 static SENSOR_DEVICE_ATTR(temp1_min_alarm, S_IRUGO, show_alarm, NULL, 0);
 static SENSOR_DEVICE_ATTR(temp1_max_alarm, S_IRUGO, show_alarm, NULL, 1);
 
-
 /*
  * Detection and registration
  */
@@ -322,24 +316,21 @@ static int max6635_check(struct i2c_client *client)
 	return 1;
 }
 
-static struct attribute *lm92_attributes[] = {
-	&dev_attr_temp1_input.attr,
-	&dev_attr_temp1_crit.attr,
-	&dev_attr_temp1_crit_hyst.attr,
-	&dev_attr_temp1_min.attr,
+static struct attribute *lm92_attrs[] = {
+	&sensor_dev_attr_temp1_input.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp1_min.dev_attr.attr,
 	&dev_attr_temp1_min_hyst.attr,
-	&dev_attr_temp1_max.attr,
-	&dev_attr_temp1_max_hyst.attr,
+	&sensor_dev_attr_temp1_max.dev_attr.attr,
+	&sensor_dev_attr_temp1_max_hyst.dev_attr.attr,
 	&dev_attr_alarms.attr,
 	&sensor_dev_attr_temp1_crit_alarm.dev_attr.attr,
 	&sensor_dev_attr_temp1_min_alarm.dev_attr.attr,
 	&sensor_dev_attr_temp1_max_alarm.dev_attr.attr,
 	NULL
 };
-
-static const struct attribute_group lm92_group = {
-	.attrs = lm92_attributes,
-};
+ATTRIBUTE_GROUPS(lm92);
 
 /* Return 0 if detection is successful, -ENODEV otherwise */
 static int lm92_detect(struct i2c_client *new_client,
@@ -371,46 +362,24 @@ static int lm92_detect(struct i2c_client *new_client,
 static int lm92_probe(struct i2c_client *new_client,
 		      const struct i2c_device_id *id)
 {
+	struct device *hwmon_dev;
 	struct lm92_data *data;
-	int err;
 
 	data = devm_kzalloc(&new_client->dev, sizeof(struct lm92_data),
 			    GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
 
-	i2c_set_clientdata(new_client, data);
+	data->client = new_client;
 	mutex_init(&data->update_lock);
 
 	/* Initialize the chipset */
 	lm92_init_client(new_client);
 
-	/* Register sysfs hooks */
-	err = sysfs_create_group(&new_client->dev.kobj, &lm92_group);
-	if (err)
-		return err;
-
-	data->hwmon_dev = hwmon_device_register(&new_client->dev);
-	if (IS_ERR(data->hwmon_dev)) {
-		err = PTR_ERR(data->hwmon_dev);
-		goto exit_remove;
-	}
-
-	return 0;
-
-exit_remove:
-	sysfs_remove_group(&new_client->dev.kobj, &lm92_group);
-	return err;
-}
-
-static int lm92_remove(struct i2c_client *client)
-{
-	struct lm92_data *data = i2c_get_clientdata(client);
-
-	hwmon_device_unregister(data->hwmon_dev);
-	sysfs_remove_group(&client->dev.kobj, &lm92_group);
-
-	return 0;
+	hwmon_dev = devm_hwmon_device_register_with_groups(&new_client->dev,
+							   new_client->name,
+							   data, lm92_groups);
+	return PTR_ERR_OR_ZERO(hwmon_dev);
 }
 
 
@@ -431,7 +400,6 @@ static struct i2c_driver lm92_driver = {
 		.name	= "lm92",
 	},
 	.probe		= lm92_probe,
-	.remove		= lm92_remove,
 	.id_table	= lm92_id,
 	.detect		= lm92_detect,
 	.address_list	= normal_i2c,
diff --git a/drivers/hwmon/lm93.c b/drivers/hwmon/lm93.c
index adf23165a6a..6c2df576f25 100644
--- a/drivers/hwmon/lm93.c
+++ b/drivers/hwmon/lm93.c
@@ -2747,10 +2747,8 @@ static int lm93_probe(struct i2c_client *client,
 	}
 
 	data = devm_kzalloc(&client->dev, sizeof(struct lm93_data), GFP_KERNEL);
-	if (!data) {
-		dev_dbg(&client->dev, "out of memory!\n");
+	if (!data)
 		return -ENOMEM;
-	}
 	i2c_set_clientdata(client, data);
 
 	/* housekeeping */
diff --git a/drivers/hwmon/ltc2945.c b/drivers/hwmon/ltc2945.c
index c104cc32989..3701b329b6a 100644
--- a/drivers/hwmon/ltc2945.c
+++ b/drivers/hwmon/ltc2945.c
@@ -314,8 +314,8 @@ static ssize_t ltc2945_reset_history(struct device *dev,
 		reg = LTC2945_MAX_ADIN_H;
 		break;
 	default:
-		BUG();
-		break;
+		WARN_ONCE(1, "Bad register: 0x%x\n", reg);
+		return -EINVAL;
 	}
 	/* Reset maximum */
 	ret = regmap_bulk_write(regmap, reg, buf_max, num_regs);
diff --git a/drivers/hwmon/max1111.c b/drivers/hwmon/max1111.c
index eda077de8a9..f67d71ee838 100644
--- a/drivers/hwmon/max1111.c
+++ b/drivers/hwmon/max1111.c
@@ -192,10 +192,8 @@ static int max1111_probe(struct spi_device *spi)
 		return err;
 
 	data = devm_kzalloc(&spi->dev, sizeof(struct max1111_data), GFP_KERNEL);
-	if (data == NULL) {
-		dev_err(&spi->dev, "failed to allocate memory\n");
+	if (data == NULL)
 		return -ENOMEM;
-	}
 
 	switch (chip) {
 	case max1110:
diff --git a/drivers/hwmon/max1619.c b/drivers/hwmon/max1619.c
index 4c23afe113e..eda9cf59968 100644
--- a/drivers/hwmon/max1619.c
+++ b/drivers/hwmon/max1619.c
@@ -19,13 +19,8 @@
  * 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>
@@ -76,38 +71,14 @@ static int temp_to_reg(int val)
 	return (val < 0 ? val+0x100*1000 : val) / 1000;
 }
 
-/*
- * Functions declaration
- */
-
-static int max1619_probe(struct i2c_client *client,
-			 const struct i2c_device_id *id);
-static int max1619_detect(struct i2c_client *client,
-			  struct i2c_board_info *info);
-static void max1619_init_client(struct i2c_client *client);
-static int max1619_remove(struct i2c_client *client);
-static struct max1619_data *max1619_update_device(struct device *dev);
-
-/*
- * Driver data (common to all clients)
- */
-
-static const struct i2c_device_id max1619_id[] = {
-	{ "max1619", 0 },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, max1619_id);
-
-static struct i2c_driver max1619_driver = {
-	.class		= I2C_CLASS_HWMON,
-	.driver = {
-		.name	= "max1619",
-	},
-	.probe		= max1619_probe,
-	.remove		= max1619_remove,
-	.id_table	= max1619_id,
-	.detect		= max1619_detect,
-	.address_list	= normal_i2c,
+enum temp_index {
+	t_input1 = 0,
+	t_input2,
+	t_low2,
+	t_high2,
+	t_crit2,
+	t_hyst2,
+	t_num_regs
 };
 
 /*
@@ -115,60 +86,92 @@ static struct i2c_driver max1619_driver = {
  */
 
 struct max1619_data {
-	struct device *hwmon_dev;
+	struct i2c_client *client;
 	struct mutex update_lock;
 	char valid; /* zero until following fields are valid */
 	unsigned long last_updated; /* in jiffies */
 
 	/* registers values */
-	u8 temp_input1; /* local */
-	u8 temp_input2, temp_low2, temp_high2; /* remote */
-	u8 temp_crit2;
-	u8 temp_hyst2;
+	u8 temp[t_num_regs];	/* index with enum temp_index */
 	u8 alarms;
 };
 
+static const u8 regs_read[t_num_regs] = {
+	[t_input1] = MAX1619_REG_R_LOCAL_TEMP,
+	[t_input2] = MAX1619_REG_R_REMOTE_TEMP,
+	[t_low2] = MAX1619_REG_R_REMOTE_LOW,
+	[t_high2] = MAX1619_REG_R_REMOTE_HIGH,
+	[t_crit2] = MAX1619_REG_R_REMOTE_CRIT,
+	[t_hyst2] = MAX1619_REG_R_TCRIT_HYST,
+};
+
+static const u8 regs_write[t_num_regs] = {
+	[t_low2] = MAX1619_REG_W_REMOTE_LOW,
+	[t_high2] = MAX1619_REG_W_REMOTE_HIGH,
+	[t_crit2] = MAX1619_REG_W_REMOTE_CRIT,
+	[t_hyst2] = MAX1619_REG_W_TCRIT_HYST,
+};
+
+static struct max1619_data *max1619_update_device(struct device *dev)
+{
+	struct max1619_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
+	int config, i;
+
+	mutex_lock(&data->update_lock);
+
+	if (time_after(jiffies, data->last_updated + HZ * 2) || !data->valid) {
+		dev_dbg(&client->dev, "Updating max1619 data.\n");
+		for (i = 0; i < t_num_regs; i++)
+			data->temp[i] = i2c_smbus_read_byte_data(client,
+					regs_read[i]);
+		data->alarms = i2c_smbus_read_byte_data(client,
+					MAX1619_REG_R_STATUS);
+		/* If OVERT polarity is low, reverse alarm bit */
+		config = i2c_smbus_read_byte_data(client, MAX1619_REG_R_CONFIG);
+		if (!(config & 0x20))
+			data->alarms ^= 0x02;
+
+		data->last_updated = jiffies;
+		data->valid = 1;
+	}
+
+	mutex_unlock(&data->update_lock);
+
+	return data;
+}
+
 /*
  * Sysfs stuff
  */
 
-#define show_temp(value) \
-static ssize_t show_##value(struct device *dev, struct device_attribute *attr, \
-			    char *buf) \
-{ \
-	struct max1619_data *data = max1619_update_device(dev); \
-	return sprintf(buf, "%d\n", temp_from_reg(data->value)); \
-}
-show_temp(temp_input1);
-show_temp(temp_input2);
-show_temp(temp_low2);
-show_temp(temp_high2);
-show_temp(temp_crit2);
-show_temp(temp_hyst2);
-
-#define set_temp2(value, reg) \
-static ssize_t set_##value(struct device *dev, struct device_attribute *attr, \
-			   const char *buf, \
-	size_t count) \
-{ \
-	struct i2c_client *client = to_i2c_client(dev); \
-	struct max1619_data *data = i2c_get_clientdata(client); \
-	long val; \
-	int err = kstrtol(buf, 10, &val); \
-	if (err) \
-		return err; \
-\
-	mutex_lock(&data->update_lock); \
-	data->value = temp_to_reg(val); \
-	i2c_smbus_write_byte_data(client, reg, data->value); \
-	mutex_unlock(&data->update_lock); \
-	return count; \
+static ssize_t show_temp(struct device *dev, struct device_attribute *devattr,
+			 char *buf)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	struct max1619_data *data = max1619_update_device(dev);
+
+	return sprintf(buf, "%d\n", temp_from_reg(data->temp[attr->index]));
 }
 
-set_temp2(temp_low2, MAX1619_REG_W_REMOTE_LOW);
-set_temp2(temp_high2, MAX1619_REG_W_REMOTE_HIGH);
-set_temp2(temp_crit2, MAX1619_REG_W_REMOTE_CRIT);
-set_temp2(temp_hyst2, MAX1619_REG_W_TCRIT_HYST);
+static ssize_t set_temp(struct device *dev, struct device_attribute *devattr,
+			   const char *buf, size_t count)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	struct max1619_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
+	long val;
+	int err = kstrtol(buf, 10, &val);
+	if (err)
+		return err;
+
+	mutex_lock(&data->update_lock);
+	data->temp[attr->index] = temp_to_reg(val);
+	i2c_smbus_write_byte_data(client, regs_write[attr->index],
+				  data->temp[attr->index]);
+	mutex_unlock(&data->update_lock);
+	return count;
+}
 
 static ssize_t show_alarms(struct device *dev, struct device_attribute *attr,
 			   char *buf)
@@ -185,29 +188,30 @@ static ssize_t show_alarm(struct device *dev, struct device_attribute *attr,
 	return sprintf(buf, "%d\n", (data->alarms >> bitnr) & 1);
 }
 
-static DEVICE_ATTR(temp1_input, S_IRUGO, show_temp_input1, NULL);
-static DEVICE_ATTR(temp2_input, S_IRUGO, show_temp_input2, NULL);
-static DEVICE_ATTR(temp2_min, S_IWUSR | S_IRUGO, show_temp_low2,
-	set_temp_low2);
-static DEVICE_ATTR(temp2_max, S_IWUSR | S_IRUGO, show_temp_high2,
-	set_temp_high2);
-static DEVICE_ATTR(temp2_crit, S_IWUSR | S_IRUGO, show_temp_crit2,
-	set_temp_crit2);
-static DEVICE_ATTR(temp2_crit_hyst, S_IWUSR | S_IRUGO, show_temp_hyst2,
-	set_temp_hyst2);
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, show_temp, NULL, t_input1);
+static SENSOR_DEVICE_ATTR(temp2_input, S_IRUGO, show_temp, NULL, t_input2);
+static SENSOR_DEVICE_ATTR(temp2_min, S_IWUSR | S_IRUGO, show_temp, set_temp,
+			  t_low2);
+static SENSOR_DEVICE_ATTR(temp2_max, S_IWUSR | S_IRUGO, show_temp, set_temp,
+			  t_high2);
+static SENSOR_DEVICE_ATTR(temp2_crit, S_IWUSR | S_IRUGO, show_temp, set_temp,
+			  t_crit2);
+static SENSOR_DEVICE_ATTR(temp2_crit_hyst, S_IWUSR | S_IRUGO, show_temp,
+			  set_temp, t_hyst2);
+
 static DEVICE_ATTR(alarms, S_IRUGO, show_alarms, NULL);
 static SENSOR_DEVICE_ATTR(temp2_crit_alarm, S_IRUGO, show_alarm, NULL, 1);
 static SENSOR_DEVICE_ATTR(temp2_fault, S_IRUGO, show_alarm, NULL, 2);
 static SENSOR_DEVICE_ATTR(temp2_min_alarm, S_IRUGO, show_alarm, NULL, 3);
 static SENSOR_DEVICE_ATTR(temp2_max_alarm, S_IRUGO, show_alarm, NULL, 4);
 
-static struct attribute *max1619_attributes[] = {
-	&dev_attr_temp1_input.attr,
-	&dev_attr_temp2_input.attr,
-	&dev_attr_temp2_min.attr,
-	&dev_attr_temp2_max.attr,
-	&dev_attr_temp2_crit.attr,
-	&dev_attr_temp2_crit_hyst.attr,
+static struct attribute *max1619_attrs[] = {
+	&sensor_dev_attr_temp1_input.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,
 
 	&dev_attr_alarms.attr,
 	&sensor_dev_attr_temp2_crit_alarm.dev_attr.attr,
@@ -216,14 +220,7 @@ static struct attribute *max1619_attributes[] = {
 	&sensor_dev_attr_temp2_max_alarm.dev_attr.attr,
 	NULL
 };
-
-static const struct attribute_group max1619_group = {
-	.attrs = max1619_attributes,
-};
-
-/*
- * Real code
- */
+ATTRIBUTE_GROUPS(max1619);
 
 /* Return 0 if detection is successful, -ENODEV otherwise */
 static int max1619_detect(struct i2c_client *client,
@@ -261,41 +258,6 @@ static int max1619_detect(struct i2c_client *client,
 	return 0;
 }
 
-static int max1619_probe(struct i2c_client *new_client,
-			 const struct i2c_device_id *id)
-{
-	struct max1619_data *data;
-	int err;
-
-	data = devm_kzalloc(&new_client->dev, sizeof(struct max1619_data),
-			    GFP_KERNEL);
-	if (!data)
-		return -ENOMEM;
-
-	i2c_set_clientdata(new_client, data);
-	mutex_init(&data->update_lock);
-
-	/* Initialize the MAX1619 chip */
-	max1619_init_client(new_client);
-
-	/* Register sysfs hooks */
-	err = sysfs_create_group(&new_client->dev.kobj, &max1619_group);
-	if (err)
-		return err;
-
-	data->hwmon_dev = hwmon_device_register(&new_client->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(&new_client->dev.kobj, &max1619_group);
-	return err;
-}
-
 static void max1619_init_client(struct i2c_client *client)
 {
 	u8 config;
@@ -311,48 +273,46 @@ static void max1619_init_client(struct i2c_client *client)
 					  config & 0xBF); /* run */
 }
 
-static int max1619_remove(struct i2c_client *client)
+static int max1619_probe(struct i2c_client *new_client,
+			 const struct i2c_device_id *id)
 {
-	struct max1619_data *data = i2c_get_clientdata(client);
-
-	hwmon_device_unregister(data->hwmon_dev);
-	sysfs_remove_group(&client->dev.kobj, &max1619_group);
-
-	return 0;
-}
+	struct max1619_data *data;
+	struct device *hwmon_dev;
 
-static struct max1619_data *max1619_update_device(struct device *dev)
-{
-	struct i2c_client *client = to_i2c_client(dev);
-	struct max1619_data *data = i2c_get_clientdata(client);
+	data = devm_kzalloc(&new_client->dev, sizeof(struct max1619_data),
+			    GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
-	mutex_lock(&data->update_lock);
+	data->client = new_client;
+	mutex_init(&data->update_lock);
 
-	if (time_after(jiffies, data->last_updated + HZ * 2) || !data->valid) {
-		dev_dbg(&client->dev, "Updating max1619 data.\n");
-		data->temp_input1 = i2c_smbus_read_byte_data(client,
-					MAX1619_REG_R_LOCAL_TEMP);
-		data->temp_input2 = i2c_smbus_read_byte_data(client,
-					MAX1619_REG_R_REMOTE_TEMP);
-		data->temp_high2 = i2c_smbus_read_byte_data(client,
-					MAX1619_REG_R_REMOTE_HIGH);
-		data->temp_low2 = i2c_smbus_read_byte_data(client,
-					MAX1619_REG_R_REMOTE_LOW);
-		data->temp_crit2 = i2c_smbus_read_byte_data(client,
-					MAX1619_REG_R_REMOTE_CRIT);
-		data->temp_hyst2 = i2c_smbus_read_byte_data(client,
-					MAX1619_REG_R_TCRIT_HYST);
-		data->alarms = i2c_smbus_read_byte_data(client,
-					MAX1619_REG_R_STATUS);
+	/* Initialize the MAX1619 chip */
+	max1619_init_client(new_client);
 
-		data->last_updated = jiffies;
-		data->valid = 1;
-	}
+	hwmon_dev = devm_hwmon_device_register_with_groups(&new_client->dev,
+							   new_client->name,
+							   data,
+							   max1619_groups);
+	return PTR_ERR_OR_ZERO(hwmon_dev);
+}
 
-	mutex_unlock(&data->update_lock);
+static const struct i2c_device_id max1619_id[] = {
+	{ "max1619", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, max1619_id);
 
-	return data;
-}
+static struct i2c_driver max1619_driver = {
+	.class		= I2C_CLASS_HWMON,
+	.driver = {
+		.name	= "max1619",
+	},
+	.probe		= max1619_probe,
+	.id_table	= max1619_id,
+	.detect		= max1619_detect,
+	.address_list	= normal_i2c,
+};
 
 module_i2c_driver(max1619_driver);
 
diff --git a/drivers/hwmon/max197.c b/drivers/hwmon/max197.c
index 96dccaf919d..82128ad79a9 100644
--- a/drivers/hwmon/max197.c
+++ b/drivers/hwmon/max197.c
@@ -275,10 +275,8 @@ static int max197_probe(struct platform_device *pdev)
 	}
 
 	data = devm_kzalloc(&pdev->dev, sizeof(struct max197_data), GFP_KERNEL);
-	if (!data) {
-		dev_err(&pdev->dev, "devm_kzalloc failed\n");
+	if (!data)
 		return -ENOMEM;
-	}
 
 	data->pdata = pdata;
 	mutex_init(&data->lock);
diff --git a/drivers/hwmon/nct6683.c b/drivers/hwmon/nct6683.c
new file mode 100644
index 00000000000..7710f4694ba
--- /dev/null
+++ b/drivers/hwmon/nct6683.c
@@ -0,0 +1,1457 @@
+/*
+ * nct6683 - Driver for the hardware monitoring functionality of
+ *	     Nuvoton NCT6683D eSIO
+ *
+ * Copyright (C) 2013  Guenter Roeck <linux@roeck-us.net>
+ *
+ * Derived from nct6775 driver
+ * Copyright (C) 2012, 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
+ * (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.
+ *
+ * Supports the following chips:
+ *
+ * Chip        #vin    #fan    #pwm    #temp  chip ID
+ * nct6683d     21(1)   16      8       32(1) 0xc730
+ *
+ * Notes:
+ *	(1) Total number of vin and temp inputs is 32.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/acpi.h>
+#include <linux/dmi.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/jiffies.h>
+#include <linux/hwmon.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+enum kinds { nct6683 };
+
+static bool force;
+module_param(force, bool, 0);
+MODULE_PARM_DESC(force, "Set to one to enable detection on non-Intel boards");
+
+static const char * const nct6683_device_names[] = {
+	"nct6683",
+};
+
+static const char * const nct6683_chip_names[] = {
+	"NCT6683D",
+};
+
+#define DRVNAME "nct6683"
+
+/*
+ * Super-I/O constants and functions
+ */
+
+#define NCT6683_LD_ACPI		0x0a
+#define NCT6683_LD_HWM		0x0b
+#define NCT6683_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_NCT6681_ID		0xb270	/* for later */
+#define SIO_NCT6683_ID		0xc730
+#define SIO_ID_MASK		0xFFF0
+
+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		4	/* Use EC port 1 */
+#define IOREGION_LENGTH		4
+
+#define EC_PAGE_REG		0
+#define EC_INDEX_REG		1
+#define EC_DATA_REG		2
+#define EC_EVENT_REG		3
+
+/* Common and NCT6683 specific data */
+
+#define NCT6683_NUM_REG_MON		32
+#define NCT6683_NUM_REG_FAN		16
+#define NCT6683_NUM_REG_PWM		8
+
+#define NCT6683_REG_MON(x)		(0x100 + (x) * 2)
+#define NCT6683_REG_FAN_RPM(x)		(0x140 + (x) * 2)
+#define NCT6683_REG_PWM(x)		(0x160 + (x))
+
+#define NCT6683_REG_MON_STS(x)		(0x174 + (x))
+#define NCT6683_REG_IDLE(x)		(0x178 + (x))
+
+#define NCT6683_REG_FAN_STS(x)		(0x17c + (x))
+#define NCT6683_REG_FAN_ERRSTS		0x17e
+#define NCT6683_REG_FAN_INITSTS		0x17f
+
+#define NCT6683_HWM_CFG			0x180
+
+#define NCT6683_REG_MON_CFG(x)		(0x1a0 + (x))
+#define NCT6683_REG_FANIN_CFG(x)	(0x1c0 + (x))
+#define NCT6683_REG_FANOUT_CFG(x)	(0x1d0 + (x))
+
+#define NCT6683_REG_INTEL_TEMP_MAX(x)	(0x901 + (x) * 16)
+#define NCT6683_REG_INTEL_TEMP_CRIT(x)	(0x90d + (x) * 16)
+
+#define NCT6683_REG_TEMP_HYST(x)	(0x330 + (x))		/* 8 bit */
+#define NCT6683_REG_TEMP_MAX(x)		(0x350 + (x))		/* 8 bit */
+#define NCT6683_REG_MON_HIGH(x)		(0x370 + (x) * 2)	/* 8 bit */
+#define NCT6683_REG_MON_LOW(x)		(0x371 + (x) * 2)	/* 8 bit */
+
+#define NCT6683_REG_FAN_MIN(x)		(0x3b8 + (x) * 2)	/* 16 bit */
+
+#define NCT6683_REG_CUSTOMER_ID		0x602
+#define NCT6683_CUSTOMER_ID_INTEL	0x805
+
+#define NCT6683_REG_BUILD_YEAR		0x604
+#define NCT6683_REG_BUILD_MONTH		0x605
+#define NCT6683_REG_BUILD_DAY		0x606
+#define NCT6683_REG_SERIAL		0x607
+#define NCT6683_REG_VERSION_HI		0x608
+#define NCT6683_REG_VERSION_LO		0x609
+
+#define NCT6683_REG_CR_CASEOPEN		0xe8
+#define NCT6683_CR_CASEOPEN_MASK	(1 << 7)
+
+#define NCT6683_REG_CR_BEEP		0xe0
+#define NCT6683_CR_BEEP_MASK		(1 << 6)
+
+static const char *const nct6683_mon_label[] = {
+	NULL,	/* disabled */
+	"Local",
+	"Diode 0 (curr)",
+	"Diode 1 (curr)",
+	"Diode 2 (curr)",
+	"Diode 0 (volt)",
+	"Diode 1 (volt)",
+	"Diode 2 (volt)",
+	"Thermistor 14",
+	"Thermistor 15",
+	"Thermistor 16",
+	"Thermistor 0",
+	"Thermistor 1",
+	"Thermistor 2",
+	"Thermistor 3",
+	"Thermistor 4",
+	"Thermistor 5",		/* 0x10 */
+	"Thermistor 6",
+	"Thermistor 7",
+	"Thermistor 8",
+	"Thermistor 9",
+	"Thermistor 10",
+	"Thermistor 11",
+	"Thermistor 12",
+	"Thermistor 13",
+	NULL, NULL, NULL, NULL, NULL, NULL, NULL,
+	"PECI 0.0",		/* 0x20 */
+	"PECI 1.0",
+	"PECI 2.0",
+	"PECI 3.0",
+	"PECI 0.1",
+	"PECI 1.1",
+	"PECI 2.1",
+	"PECI 3.1",
+	"PECI DIMM 0",
+	"PECI DIMM 1",
+	"PECI DIMM 2",
+	"PECI DIMM 3",
+	NULL, NULL, NULL, NULL,
+	"PCH CPU",		/* 0x30 */
+	"PCH CHIP",
+	"PCH CHIP CPU MAX",
+	"PCH MCH",
+	"PCH DIMM 0",
+	"PCH DIMM 1",
+	"PCH DIMM 2",
+	"PCH DIMM 3",
+	"SMBus 0",
+	"SMBus 1",
+	"SMBus 2",
+	"SMBus 3",
+	"SMBus 4",
+	"SMBus 5",
+	"DIMM 0",
+	"DIMM 1",
+	"DIMM 2",		/* 0x40 */
+	"DIMM 3",
+	"AMD TSI Addr 90h",
+	"AMD TSI Addr 92h",
+	"AMD TSI Addr 94h",
+	"AMD TSI Addr 96h",
+	"AMD TSI Addr 98h",
+	"AMD TSI Addr 9ah",
+	"AMD TSI Addr 9ch",
+	"AMD TSI Addr 9dh",
+	NULL, NULL, NULL, NULL, NULL, NULL,
+	"Virtual 0",		/* 0x50 */
+	"Virtual 1",
+	"Virtual 2",
+	"Virtual 3",
+	"Virtual 4",
+	"Virtual 5",
+	"Virtual 6",
+	"Virtual 7",
+	NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL,
+	"VCC",			/* 0x60 voltage sensors */
+	"VSB",
+	"AVSB",
+	"VTT",
+	"VBAT",
+	"VREF",
+	"VIN0",
+	"VIN1",
+	"VIN2",
+	"VIN3",
+	"VIN4",
+	"VIN5",
+	"VIN6",
+	"VIN7",
+	"VIN8",
+	"VIN9",
+	"VIN10",
+	"VIN11",
+	"VIN12",
+	"VIN13",
+	"VIN14",
+	"VIN15",
+	"VIN16",
+};
+
+#define NUM_MON_LABELS		ARRAY_SIZE(nct6683_mon_label)
+#define MON_VOLTAGE_START	0x60
+
+/* ------------------------------------------------------- */
+
+struct nct6683_data {
+	int addr;		/* IO base of EC space */
+	int sioreg;		/* SIO register */
+	enum kinds kind;
+	u16 customer_id;
+
+	struct device *hwmon_dev;
+	const struct attribute_group *groups[6];
+
+	int temp_num;			/* number of temperature attributes */
+	u8 temp_index[NCT6683_NUM_REG_MON];
+	u8 temp_src[NCT6683_NUM_REG_MON];
+
+	u8 in_num;			/* number of voltage attributes */
+	u8 in_index[NCT6683_NUM_REG_MON];
+	u8 in_src[NCT6683_NUM_REG_MON];
+
+	struct mutex update_lock;	/* used to protect sensor updates */
+	bool valid;			/* true if following fields are valid */
+	unsigned long last_updated;	/* In jiffies */
+
+	/* Voltage attribute values */
+	u8 in[3][NCT6683_NUM_REG_MON];	/* [0]=in, [1]=in_max, [2]=in_min */
+
+	/* Temperature attribute values */
+	s16 temp_in[NCT6683_NUM_REG_MON];
+	s8 temp[4][NCT6683_NUM_REG_MON];/* [0]=min, [1]=max, [2]=hyst,
+					 * [3]=crit
+					 */
+
+	/* Fan attribute values */
+	unsigned int rpm[NCT6683_NUM_REG_FAN];
+	u16 fan_min[NCT6683_NUM_REG_FAN];
+	u8 fanin_cfg[NCT6683_NUM_REG_FAN];
+	u8 fanout_cfg[NCT6683_NUM_REG_FAN];
+	u16 have_fan;			/* some fan inputs can be disabled */
+
+	u8 have_pwm;
+	u8 pwm[NCT6683_NUM_REG_PWM];
+
+#ifdef CONFIG_PM
+	/* Remember extra register values over suspend/resume */
+	u8 hwm_cfg;
+#endif
+};
+
+struct nct6683_sio_data {
+	int sioreg;
+	enum kinds kind;
+};
+
+struct sensor_device_template {
+	struct device_attribute dev_attr;
+	union {
+		struct {
+			u8 nr;
+			u8 index;
+		} s;
+		int index;
+	} u;
+	bool s2;	/* true if both index and nr are used */
+};
+
+struct sensor_device_attr_u {
+	union {
+		struct sensor_device_attribute a1;
+		struct sensor_device_attribute_2 a2;
+	} u;
+	char name[32];
+};
+
+#define __TEMPLATE_ATTR(_template, _mode, _show, _store) {	\
+	.attr = {.name = _template, .mode = _mode },		\
+	.show	= _show,					\
+	.store	= _store,					\
+}
+
+#define SENSOR_DEVICE_TEMPLATE(_template, _mode, _show, _store, _index)	\
+	{ .dev_attr = __TEMPLATE_ATTR(_template, _mode, _show, _store),	\
+	  .u.index = _index,						\
+	  .s2 = false }
+
+#define SENSOR_DEVICE_TEMPLATE_2(_template, _mode, _show, _store,	\
+				 _nr, _index)				\
+	{ .dev_attr = __TEMPLATE_ATTR(_template, _mode, _show, _store),	\
+	  .u.s.index = _index,						\
+	  .u.s.nr = _nr,						\
+	  .s2 = true }
+
+#define SENSOR_TEMPLATE(_name, _template, _mode, _show, _store, _index)	\
+static struct sensor_device_template sensor_dev_template_##_name	\
+	= SENSOR_DEVICE_TEMPLATE(_template, _mode, _show, _store,	\
+				 _index)
+
+#define SENSOR_TEMPLATE_2(_name, _template, _mode, _show, _store,	\
+			  _nr, _index)					\
+static struct sensor_device_template sensor_dev_template_##_name	\
+	= SENSOR_DEVICE_TEMPLATE_2(_template, _mode, _show, _store,	\
+				 _nr, _index)
+
+struct sensor_template_group {
+	struct sensor_device_template **templates;
+	umode_t (*is_visible)(struct kobject *, struct attribute *, int);
+	int base;
+};
+
+static struct attribute_group *
+nct6683_create_attr_group(struct device *dev, struct sensor_template_group *tg,
+			  int repeat)
+{
+	struct sensor_device_attribute_2 *a2;
+	struct sensor_device_attribute *a;
+	struct sensor_device_template **t;
+	struct sensor_device_attr_u *su;
+	struct attribute_group *group;
+	struct attribute **attrs;
+	int i, j, count;
+
+	if (repeat <= 0)
+		return ERR_PTR(-EINVAL);
+
+	t = tg->templates;
+	for (count = 0; *t; t++, count++)
+		;
+
+	if (count == 0)
+		return ERR_PTR(-EINVAL);
+
+	group = devm_kzalloc(dev, sizeof(*group), GFP_KERNEL);
+	if (group == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	attrs = devm_kzalloc(dev, sizeof(*attrs) * (repeat * count + 1),
+			     GFP_KERNEL);
+	if (attrs == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	su = devm_kzalloc(dev, sizeof(*su) * repeat * count,
+			  GFP_KERNEL);
+	if (su == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	group->attrs = attrs;
+	group->is_visible = tg->is_visible;
+
+	for (i = 0; i < repeat; i++) {
+		t = tg->templates;
+		for (j = 0; *t != NULL; j++) {
+			snprintf(su->name, sizeof(su->name),
+				 (*t)->dev_attr.attr.name, tg->base + i);
+			if ((*t)->s2) {
+				a2 = &su->u.a2;
+				a2->dev_attr.attr.name = su->name;
+				a2->nr = (*t)->u.s.nr + i;
+				a2->index = (*t)->u.s.index;
+				a2->dev_attr.attr.mode =
+				  (*t)->dev_attr.attr.mode;
+				a2->dev_attr.show = (*t)->dev_attr.show;
+				a2->dev_attr.store = (*t)->dev_attr.store;
+				*attrs = &a2->dev_attr.attr;
+			} else {
+				a = &su->u.a1;
+				a->dev_attr.attr.name = su->name;
+				a->index = (*t)->u.index + i;
+				a->dev_attr.attr.mode =
+				  (*t)->dev_attr.attr.mode;
+				a->dev_attr.show = (*t)->dev_attr.show;
+				a->dev_attr.store = (*t)->dev_attr.store;
+				*attrs = &a->dev_attr.attr;
+			}
+			attrs++;
+			su++;
+			t++;
+		}
+	}
+
+	return group;
+}
+
+/* LSB is 16 mV, except for the following sources, where it is 32 mV */
+#define MON_SRC_VCC	0x60
+#define MON_SRC_VSB	0x61
+#define MON_SRC_AVSB	0x62
+#define MON_SRC_VBAT	0x64
+
+static inline long in_from_reg(u16 reg, u8 src)
+{
+	int scale = 16;
+
+	if (src == MON_SRC_VCC || src == MON_SRC_VSB || src == MON_SRC_AVSB ||
+	    src == MON_SRC_VBAT)
+		scale <<= 1;
+	return reg * scale;
+}
+
+static inline u16 in_to_reg(u32 val, u8 src)
+{
+	int scale = 16;
+
+	if (src == MON_SRC_VCC || src == MON_SRC_VSB || src == MON_SRC_AVSB ||
+	    src == MON_SRC_VBAT)
+		scale <<= 1;
+
+	return clamp_val(DIV_ROUND_CLOSEST(val, scale), 0, 127);
+}
+
+static u16 nct6683_read(struct nct6683_data *data, u16 reg)
+{
+	int res;
+
+	outb_p(0xff, data->addr + EC_PAGE_REG);		/* unlock */
+	outb_p(reg >> 8, data->addr + EC_PAGE_REG);
+	outb_p(reg & 0xff, data->addr + EC_INDEX_REG);
+	res = inb_p(data->addr + EC_DATA_REG);
+	return res;
+}
+
+static u16 nct6683_read16(struct nct6683_data *data, u16 reg)
+{
+	return (nct6683_read(data, reg) << 8) | nct6683_read(data, reg + 1);
+}
+
+static void nct6683_write(struct nct6683_data *data, u16 reg, u16 value)
+{
+	outb_p(0xff, data->addr + EC_PAGE_REG);		/* unlock */
+	outb_p(reg >> 8, data->addr + EC_PAGE_REG);
+	outb_p(reg & 0xff, data->addr + EC_INDEX_REG);
+	outb_p(value & 0xff, data->addr + EC_DATA_REG);
+}
+
+static int get_in_reg(struct nct6683_data *data, int nr, int index)
+{
+	int ch = data->in_index[index];
+	int reg = -EINVAL;
+
+	switch (nr) {
+	case 0:
+		reg = NCT6683_REG_MON(ch);
+		break;
+	case 1:
+		if (data->customer_id != NCT6683_CUSTOMER_ID_INTEL)
+			reg = NCT6683_REG_MON_LOW(ch);
+		break;
+	case 2:
+		if (data->customer_id != NCT6683_CUSTOMER_ID_INTEL)
+			reg = NCT6683_REG_MON_HIGH(ch);
+		break;
+	default:
+		break;
+	}
+	return reg;
+}
+
+static int get_temp_reg(struct nct6683_data *data, int nr, int index)
+{
+	int ch = data->temp_index[index];
+	int reg = -EINVAL;
+
+	switch (data->customer_id) {
+	case NCT6683_CUSTOMER_ID_INTEL:
+		switch (nr) {
+		default:
+		case 1:	/* max */
+			reg = NCT6683_REG_INTEL_TEMP_MAX(ch);
+			break;
+		case 3:	/* crit */
+			reg = NCT6683_REG_INTEL_TEMP_CRIT(ch);
+			break;
+		}
+		break;
+	default:
+		switch (nr) {
+		default:
+		case 0:	/* min */
+			reg = NCT6683_REG_MON_LOW(ch);
+			break;
+		case 1:	/* max */
+			reg = NCT6683_REG_TEMP_MAX(ch);
+			break;
+		case 2:	/* hyst */
+			reg = NCT6683_REG_TEMP_HYST(ch);
+			break;
+		case 3:	/* crit */
+			reg = NCT6683_REG_MON_HIGH(ch);
+			break;
+		}
+		break;
+	}
+	return reg;
+}
+
+static void nct6683_update_pwm(struct device *dev)
+{
+	struct nct6683_data *data = dev_get_drvdata(dev);
+	int i;
+
+	for (i = 0; i < NCT6683_NUM_REG_PWM; i++) {
+		if (!(data->have_pwm & (1 << i)))
+			continue;
+		data->pwm[i] = nct6683_read(data, NCT6683_REG_PWM(i));
+	}
+}
+
+static struct nct6683_data *nct6683_update_device(struct device *dev)
+{
+	struct nct6683_data *data = dev_get_drvdata(dev);
+	int i, j;
+
+	mutex_lock(&data->update_lock);
+
+	if (time_after(jiffies, data->last_updated + HZ) || !data->valid) {
+		/* Measured voltages and limits */
+		for (i = 0; i < data->in_num; i++) {
+			for (j = 0; j < 3; j++) {
+				int reg = get_in_reg(data, j, i);
+
+				if (reg >= 0)
+					data->in[j][i] =
+						nct6683_read(data, reg);
+			}
+		}
+
+		/* Measured temperatures and limits */
+		for (i = 0; i < data->temp_num; i++) {
+			u8 ch = data->temp_index[i];
+
+			data->temp_in[i] = nct6683_read16(data,
+							  NCT6683_REG_MON(ch));
+			for (j = 0; j < 4; j++) {
+				int reg = get_temp_reg(data, j, i);
+
+				if (reg >= 0)
+					data->temp[j][i] =
+						nct6683_read(data, reg);
+			}
+		}
+
+		/* Measured fan speeds and limits */
+		for (i = 0; i < ARRAY_SIZE(data->rpm); i++) {
+			if (!(data->have_fan & (1 << i)))
+				continue;
+
+			data->rpm[i] = nct6683_read16(data,
+						NCT6683_REG_FAN_RPM(i));
+			data->fan_min[i] = nct6683_read16(data,
+						NCT6683_REG_FAN_MIN(i));
+		}
+
+		nct6683_update_pwm(dev);
+
+		data->last_updated = jiffies;
+		data->valid = true;
+	}
+
+	mutex_unlock(&data->update_lock);
+	return data;
+}
+
+/*
+ * Sysfs callback functions
+ */
+static ssize_t
+show_in_label(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	struct nct6683_data *data = nct6683_update_device(dev);
+	int nr = sattr->index;
+
+	return sprintf(buf, "%s\n", nct6683_mon_label[data->in_src[nr]]);
+}
+
+static ssize_t
+show_in_reg(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
+	struct nct6683_data *data = nct6683_update_device(dev);
+	int index = sattr->index;
+	int nr = sattr->nr;
+
+	return sprintf(buf, "%ld\n",
+		       in_from_reg(data->in[index][nr], data->in_index[index]));
+}
+
+static umode_t nct6683_in_is_visible(struct kobject *kobj,
+				     struct attribute *attr, int index)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct nct6683_data *data = dev_get_drvdata(dev);
+	int nr = index % 4;	/* attribute */
+
+	/*
+	 * Voltage limits exist for Intel boards,
+	 * but register location and encoding is unknown
+	 */
+	if ((nr == 2 || nr == 3) &&
+	    data->customer_id == NCT6683_CUSTOMER_ID_INTEL)
+		return 0;
+
+	return attr->mode;
+}
+
+SENSOR_TEMPLATE(in_label, "in%d_label", S_IRUGO, show_in_label, NULL, 0);
+SENSOR_TEMPLATE_2(in_input, "in%d_input", S_IRUGO, show_in_reg, NULL, 0, 0);
+SENSOR_TEMPLATE_2(in_min, "in%d_min", S_IRUGO, show_in_reg, NULL, 0, 1);
+SENSOR_TEMPLATE_2(in_max, "in%d_max", S_IRUGO, show_in_reg, NULL, 0, 2);
+
+static struct sensor_device_template *nct6683_attributes_in_template[] = {
+	&sensor_dev_template_in_label,
+	&sensor_dev_template_in_input,
+	&sensor_dev_template_in_min,
+	&sensor_dev_template_in_max,
+	NULL
+};
+
+static struct sensor_template_group nct6683_in_template_group = {
+	.templates = nct6683_attributes_in_template,
+	.is_visible = nct6683_in_is_visible,
+};
+
+static ssize_t
+show_fan(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	struct nct6683_data *data = nct6683_update_device(dev);
+
+	return sprintf(buf, "%d\n", data->rpm[sattr->index]);
+}
+
+static ssize_t
+show_fan_min(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6683_data *data = nct6683_update_device(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	int nr = sattr->index;
+
+	return sprintf(buf, "%d\n", data->fan_min[nr]);
+}
+
+static ssize_t
+show_fan_pulses(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	struct nct6683_data *data = nct6683_update_device(dev);
+
+	return sprintf(buf, "%d\n",
+		       ((data->fanin_cfg[sattr->index] >> 5) & 0x03) + 1);
+}
+
+static umode_t nct6683_fan_is_visible(struct kobject *kobj,
+				      struct attribute *attr, int index)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct nct6683_data *data = dev_get_drvdata(dev);
+	int fan = index / 3;	/* fan index */
+	int nr = index % 3;	/* attribute index */
+
+	if (!(data->have_fan & (1 << fan)))
+		return 0;
+
+	/*
+	 * Intel may have minimum fan speed limits,
+	 * but register location and encoding are unknown.
+	 */
+	if (nr == 2 && data->customer_id == NCT6683_CUSTOMER_ID_INTEL)
+		return 0;
+
+	return attr->mode;
+}
+
+SENSOR_TEMPLATE(fan_input, "fan%d_input", S_IRUGO, show_fan, NULL, 0);
+SENSOR_TEMPLATE(fan_pulses, "fan%d_pulses", S_IRUGO, show_fan_pulses, NULL, 0);
+SENSOR_TEMPLATE(fan_min, "fan%d_min", S_IRUGO, show_fan_min, NULL, 0);
+
+/*
+ * nct6683_fan_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.
+ */
+static struct sensor_device_template *nct6683_attributes_fan_template[] = {
+	&sensor_dev_template_fan_input,
+	&sensor_dev_template_fan_pulses,
+	&sensor_dev_template_fan_min,
+	NULL
+};
+
+static struct sensor_template_group nct6683_fan_template_group = {
+	.templates = nct6683_attributes_fan_template,
+	.is_visible = nct6683_fan_is_visible,
+	.base = 1,
+};
+
+static ssize_t
+show_temp_label(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	struct nct6683_data *data = nct6683_update_device(dev);
+	int nr = sattr->index;
+
+	return sprintf(buf, "%s\n", nct6683_mon_label[data->temp_src[nr]]);
+}
+
+static ssize_t
+show_temp8(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
+	struct nct6683_data *data = nct6683_update_device(dev);
+	int index = sattr->index;
+	int nr = sattr->nr;
+
+	return sprintf(buf, "%d\n", data->temp[index][nr] * 1000);
+}
+
+static ssize_t
+show_temp_hyst(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	struct nct6683_data *data = nct6683_update_device(dev);
+	int nr = sattr->index;
+	int temp = data->temp[1][nr] - data->temp[2][nr];
+
+	return sprintf(buf, "%d\n", temp * 1000);
+}
+
+static ssize_t
+show_temp16(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	struct nct6683_data *data = nct6683_update_device(dev);
+	int index = sattr->index;
+
+	return sprintf(buf, "%d\n", (data->temp_in[index] / 128) * 500);
+}
+
+/*
+ * Temperature sensor type is determined by temperature source
+ * and can not be modified.
+ * 0x02..0x07: Thermal diode
+ * 0x08..0x18: Thermistor
+ * 0x20..0x2b: Intel PECI
+ * 0x42..0x49: AMD TSI
+ * Others are unspecified (not visible)
+ */
+
+static int get_temp_type(u8 src)
+{
+	if (src >= 0x02 && src <= 0x07)
+		return 3;	/* thermal diode */
+	else if (src >= 0x08 && src <= 0x18)
+		return 4;	/* thermistor */
+	else if (src >= 0x20 && src <= 0x2b)
+		return 6;	/* PECI */
+	else if (src >= 0x42 && src <= 0x49)
+		return 5;
+
+	return 0;
+}
+
+static ssize_t
+show_temp_type(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6683_data *data = nct6683_update_device(dev);
+	struct sensor_device_attribute *sattr = to_sensor_dev_attr(attr);
+	int nr = sattr->index;
+	return sprintf(buf, "%d\n", get_temp_type(data->temp_src[nr]));
+}
+
+static umode_t nct6683_temp_is_visible(struct kobject *kobj,
+				       struct attribute *attr, int index)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct nct6683_data *data = dev_get_drvdata(dev);
+	int temp = index / 7;	/* temp index */
+	int nr = index % 7;	/* attribute index */
+
+	/*
+	 * Intel does not have low temperature limits or temperature hysteresis
+	 * registers, or at least register location and encoding is unknown.
+	 */
+	if ((nr == 2 || nr == 4) &&
+	    data->customer_id == NCT6683_CUSTOMER_ID_INTEL)
+		return 0;
+
+	if (nr == 6 && get_temp_type(data->temp_src[temp]) == 0)
+		return 0;				/* type */
+
+	return attr->mode;
+}
+
+SENSOR_TEMPLATE(temp_input, "temp%d_input", S_IRUGO, show_temp16, NULL, 0);
+SENSOR_TEMPLATE(temp_label, "temp%d_label", S_IRUGO, show_temp_label, NULL, 0);
+SENSOR_TEMPLATE_2(temp_min, "temp%d_min", S_IRUGO, show_temp8, NULL, 0, 0);
+SENSOR_TEMPLATE_2(temp_max, "temp%d_max", S_IRUGO, show_temp8, NULL, 0, 1);
+SENSOR_TEMPLATE(temp_max_hyst, "temp%d_max_hyst", S_IRUGO, show_temp_hyst, NULL,
+		0);
+SENSOR_TEMPLATE_2(temp_crit, "temp%d_crit", S_IRUGO, show_temp8, NULL, 0, 3);
+SENSOR_TEMPLATE(temp_type, "temp%d_type", S_IRUGO, show_temp_type, NULL, 0);
+
+/*
+ * nct6683_temp_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.
+ */
+static struct sensor_device_template *nct6683_attributes_temp_template[] = {
+	&sensor_dev_template_temp_input,
+	&sensor_dev_template_temp_label,
+	&sensor_dev_template_temp_min,		/* 2 */
+	&sensor_dev_template_temp_max,		/* 3 */
+	&sensor_dev_template_temp_max_hyst,	/* 4 */
+	&sensor_dev_template_temp_crit,		/* 5 */
+	&sensor_dev_template_temp_type,		/* 6 */
+	NULL
+};
+
+static struct sensor_template_group nct6683_temp_template_group = {
+	.templates = nct6683_attributes_temp_template,
+	.is_visible = nct6683_temp_is_visible,
+	.base = 1,
+};
+
+static ssize_t
+show_pwm(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6683_data *data = nct6683_update_device(dev);
+	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
+	int index = sattr->index;
+
+	return sprintf(buf, "%d\n", data->pwm[index]);
+}
+
+SENSOR_TEMPLATE(pwm, "pwm%d", S_IRUGO, show_pwm, NULL, 0);
+
+static umode_t nct6683_pwm_is_visible(struct kobject *kobj,
+				      struct attribute *attr, int index)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct nct6683_data *data = dev_get_drvdata(dev);
+	int pwm = index;	/* pwm index */
+
+	if (!(data->have_pwm & (1 << pwm)))
+		return 0;
+
+	return attr->mode;
+}
+
+static struct sensor_device_template *nct6683_attributes_pwm_template[] = {
+	&sensor_dev_template_pwm,
+	NULL
+};
+
+static struct sensor_template_group nct6683_pwm_template_group = {
+	.templates = nct6683_attributes_pwm_template,
+	.is_visible = nct6683_pwm_is_visible,
+	.base = 1,
+};
+
+static ssize_t
+show_global_beep(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6683_data *data = dev_get_drvdata(dev);
+	int ret;
+	u8 reg;
+
+	mutex_lock(&data->update_lock);
+
+	ret = superio_enter(data->sioreg);
+	if (ret)
+		goto error;
+	superio_select(data->sioreg, NCT6683_LD_HWM);
+	reg = superio_inb(data->sioreg, NCT6683_REG_CR_BEEP);
+	superio_exit(data->sioreg);
+
+	mutex_unlock(&data->update_lock);
+
+	return sprintf(buf, "%u\n", !!(reg & NCT6683_CR_BEEP_MASK));
+
+error:
+	mutex_unlock(&data->update_lock);
+	return ret;
+}
+
+static ssize_t
+store_global_beep(struct device *dev, struct device_attribute *attr,
+		  const char *buf, size_t count)
+{
+	struct nct6683_data *data = dev_get_drvdata(dev);
+	unsigned long val;
+	u8 reg;
+	int ret;
+
+	if (kstrtoul(buf, 10, &val) || (val != 0 && val != 1))
+		return -EINVAL;
+
+	mutex_lock(&data->update_lock);
+
+	ret = superio_enter(data->sioreg);
+	if (ret) {
+		count = ret;
+		goto error;
+	}
+
+	superio_select(data->sioreg, NCT6683_LD_HWM);
+	reg = superio_inb(data->sioreg, NCT6683_REG_CR_BEEP);
+	if (val)
+		reg |= NCT6683_CR_BEEP_MASK;
+	else
+		reg &= ~NCT6683_CR_BEEP_MASK;
+	superio_outb(data->sioreg, NCT6683_REG_CR_BEEP, reg);
+	superio_exit(data->sioreg);
+error:
+	mutex_unlock(&data->update_lock);
+	return count;
+}
+
+/* Case open detection */
+
+static ssize_t
+show_caseopen(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct nct6683_data *data = dev_get_drvdata(dev);
+	int ret;
+	u8 reg;
+
+	mutex_lock(&data->update_lock);
+
+	ret = superio_enter(data->sioreg);
+	if (ret)
+		goto error;
+	superio_select(data->sioreg, NCT6683_LD_ACPI);
+	reg = superio_inb(data->sioreg, NCT6683_REG_CR_CASEOPEN);
+	superio_exit(data->sioreg);
+
+	mutex_unlock(&data->update_lock);
+
+	return sprintf(buf, "%u\n", !(reg & NCT6683_CR_CASEOPEN_MASK));
+
+error:
+	mutex_unlock(&data->update_lock);
+	return ret;
+}
+
+static ssize_t
+clear_caseopen(struct device *dev, struct device_attribute *attr,
+	       const char *buf, size_t count)
+{
+	struct nct6683_data *data = dev_get_drvdata(dev);
+	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.
+	 * Caseopen is activ low, clear by writing 1 into the register.
+	 */
+
+	ret = superio_enter(data->sioreg);
+	if (ret) {
+		count = ret;
+		goto error;
+	}
+
+	superio_select(data->sioreg, NCT6683_LD_ACPI);
+	reg = superio_inb(data->sioreg, NCT6683_REG_CR_CASEOPEN);
+	reg |= NCT6683_CR_CASEOPEN_MASK;
+	superio_outb(data->sioreg, NCT6683_REG_CR_CASEOPEN, reg);
+	reg &= ~NCT6683_CR_CASEOPEN_MASK;
+	superio_outb(data->sioreg, NCT6683_REG_CR_CASEOPEN, reg);
+	superio_exit(data->sioreg);
+
+	data->valid = false;	/* Force cache refresh */
+error:
+	mutex_unlock(&data->update_lock);
+	return count;
+}
+
+static DEVICE_ATTR(intrusion0_alarm, S_IWUSR | S_IRUGO, show_caseopen,
+		   clear_caseopen);
+static DEVICE_ATTR(beep_enable, S_IWUSR | S_IRUGO, show_global_beep,
+		   store_global_beep);
+
+static struct attribute *nct6683_attributes_other[] = {
+	&dev_attr_intrusion0_alarm.attr,
+	&dev_attr_beep_enable.attr,
+	NULL
+};
+
+static const struct attribute_group nct6683_group_other = {
+	.attrs = nct6683_attributes_other,
+};
+
+/* Get the monitoring functions started */
+static inline void nct6683_init_device(struct nct6683_data *data)
+{
+	u8 tmp;
+
+	/* Start hardware monitoring if needed */
+	tmp = nct6683_read(data, NCT6683_HWM_CFG);
+	if (!(tmp & 0x80))
+		nct6683_write(data, NCT6683_HWM_CFG, tmp | 0x80);
+}
+
+/*
+ * There are a total of 24 fan inputs. Each can be configured as input
+ * or as output. A maximum of 16 inputs and 8 outputs is configurable.
+ */
+static void
+nct6683_setup_fans(struct nct6683_data *data)
+{
+	int i;
+	u8 reg;
+
+	for (i = 0; i < NCT6683_NUM_REG_FAN; i++) {
+		reg = nct6683_read(data, NCT6683_REG_FANIN_CFG(i));
+		if (reg & 0x80)
+			data->have_fan |= 1 << i;
+		data->fanin_cfg[i] = reg;
+	}
+	for (i = 0; i < NCT6683_NUM_REG_PWM; i++) {
+		reg = nct6683_read(data, NCT6683_REG_FANOUT_CFG(i));
+		if (reg & 0x80)
+			data->have_pwm |= 1 << i;
+		data->fanout_cfg[i] = reg;
+	}
+}
+
+/*
+ * Translation from monitoring register to temperature and voltage attributes
+ * ==========================================================================
+ *
+ * There are a total of 32 monitoring registers. Each can be assigned to either
+ * a temperature or voltage monitoring source.
+ * NCT6683_REG_MON_CFG(x) defines assignment for each monitoring source.
+ *
+ * Temperature and voltage attribute mapping is determined by walking through
+ * the NCT6683_REG_MON_CFG registers. If the assigned source is
+ * a temperature, temp_index[n] is set to the monitor register index, and
+ * temp_src[n] is set to the temperature source. If the assigned source is
+ * a voltage, the respective values are stored in in_index[] and in_src[],
+ * respectively.
+ */
+
+static void nct6683_setup_sensors(struct nct6683_data *data)
+{
+	u8 reg;
+	int i;
+
+	data->temp_num = 0;
+	data->in_num = 0;
+	for (i = 0; i < NCT6683_NUM_REG_MON; i++) {
+		reg = nct6683_read(data, NCT6683_REG_MON_CFG(i)) & 0x7f;
+		/* Ignore invalid assignments */
+		if (reg >= NUM_MON_LABELS)
+			continue;
+		/* Skip if disabled or reserved */
+		if (nct6683_mon_label[reg] == NULL)
+			continue;
+		if (reg < MON_VOLTAGE_START) {
+			data->temp_index[data->temp_num] = i;
+			data->temp_src[data->temp_num] = reg;
+			data->temp_num++;
+		} else {
+			data->in_index[data->in_num] = i;
+			data->in_src[data->in_num] = reg;
+			data->in_num++;
+		}
+	}
+}
+
+static int nct6683_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct nct6683_sio_data *sio_data = dev->platform_data;
+	struct attribute_group *group;
+	struct nct6683_data *data;
+	struct device *hwmon_dev;
+	struct resource *res;
+	int groups = 0;
+
+	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
+	if (!devm_request_region(dev, res->start, IOREGION_LENGTH, DRVNAME))
+		return -EBUSY;
+
+	data = devm_kzalloc(dev, sizeof(struct nct6683_data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	data->kind = sio_data->kind;
+	data->sioreg = sio_data->sioreg;
+	data->addr = res->start;
+	mutex_init(&data->update_lock);
+	platform_set_drvdata(pdev, data);
+
+	data->customer_id = nct6683_read16(data, NCT6683_REG_CUSTOMER_ID);
+
+	nct6683_init_device(data);
+	nct6683_setup_fans(data);
+	nct6683_setup_sensors(data);
+
+	/* Register sysfs hooks */
+
+	if (data->have_pwm) {
+		group = nct6683_create_attr_group(dev,
+						  &nct6683_pwm_template_group,
+						  fls(data->have_pwm));
+		if (IS_ERR(group))
+			return PTR_ERR(group);
+		data->groups[groups++] = group;
+	}
+
+	if (data->in_num) {
+		group = nct6683_create_attr_group(dev,
+						  &nct6683_in_template_group,
+						  data->in_num);
+		if (IS_ERR(group))
+			return PTR_ERR(group);
+		data->groups[groups++] = group;
+	}
+
+	if (data->have_fan) {
+		group = nct6683_create_attr_group(dev,
+						  &nct6683_fan_template_group,
+						  fls(data->have_fan));
+		if (IS_ERR(group))
+			return PTR_ERR(group);
+		data->groups[groups++] = group;
+	}
+
+	if (data->temp_num) {
+		group = nct6683_create_attr_group(dev,
+						  &nct6683_temp_template_group,
+						  data->temp_num);
+		if (IS_ERR(group))
+			return PTR_ERR(group);
+		data->groups[groups++] = group;
+	}
+	data->groups[groups++] = &nct6683_group_other;
+
+	dev_info(dev, "%s EC firmware version %d.%d build %02x/%02x/%02x\n",
+		 nct6683_chip_names[data->kind],
+		 nct6683_read(data, NCT6683_REG_VERSION_HI),
+		 nct6683_read(data, NCT6683_REG_VERSION_LO),
+		 nct6683_read(data, NCT6683_REG_BUILD_MONTH),
+		 nct6683_read(data, NCT6683_REG_BUILD_DAY),
+		 nct6683_read(data, NCT6683_REG_BUILD_YEAR));
+
+	hwmon_dev = devm_hwmon_device_register_with_groups(dev,
+			nct6683_device_names[data->kind], data, data->groups);
+	return PTR_ERR_OR_ZERO(hwmon_dev);
+}
+
+#ifdef CONFIG_PM
+static int nct6683_suspend(struct device *dev)
+{
+	struct nct6683_data *data = nct6683_update_device(dev);
+
+	mutex_lock(&data->update_lock);
+	data->hwm_cfg = nct6683_read(data, NCT6683_HWM_CFG);
+	mutex_unlock(&data->update_lock);
+
+	return 0;
+}
+
+static int nct6683_resume(struct device *dev)
+{
+	struct nct6683_data *data = dev_get_drvdata(dev);
+
+	mutex_lock(&data->update_lock);
+
+	nct6683_write(data, NCT6683_HWM_CFG, data->hwm_cfg);
+
+	/* Force re-reading all values */
+	data->valid = false;
+	mutex_unlock(&data->update_lock);
+
+	return 0;
+}
+
+static const struct dev_pm_ops nct6683_dev_pm_ops = {
+	.suspend = nct6683_suspend,
+	.resume = nct6683_resume,
+	.freeze = nct6683_suspend,
+	.restore = nct6683_resume,
+};
+
+#define NCT6683_DEV_PM_OPS	(&nct6683_dev_pm_ops)
+#else
+#define NCT6683_DEV_PM_OPS	NULL
+#endif /* CONFIG_PM */
+
+static struct platform_driver nct6683_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= DRVNAME,
+		.pm	= NCT6683_DEV_PM_OPS,
+	},
+	.probe		= nct6683_probe,
+};
+
+static int __init nct6683_find(int sioaddr, struct nct6683_sio_data *sio_data)
+{
+	const char *board_vendor;
+	int addr;
+	u16 val;
+	int err;
+
+	/*
+	 * Only run on Intel boards unless the 'force' module parameter is set
+	 */
+	if (!force) {
+		board_vendor = dmi_get_system_info(DMI_BOARD_VENDOR);
+		if (!board_vendor || strcmp(board_vendor, "Intel Corporation"))
+			return -ENODEV;
+	}
+
+	err = superio_enter(sioaddr);
+	if (err)
+		return err;
+
+	val = (superio_inb(sioaddr, SIO_REG_DEVID) << 8)
+	       | superio_inb(sioaddr, SIO_REG_DEVID + 1);
+
+	switch (val & SIO_ID_MASK) {
+	case SIO_NCT6683_ID:
+		sio_data->kind = nct6683;
+		break;
+	default:
+		if (val != 0xffff)
+			pr_debug("unsupported chip ID: 0x%04x\n", val);
+		goto fail;
+	}
+
+	/* We have a known chip, find the HWM I/O address */
+	superio_select(sioaddr, NCT6683_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("EC base I/O port unconfigured\n");
+		goto fail;
+	}
+
+	/* Activate logical device if needed */
+	val = superio_inb(sioaddr, SIO_REG_ENABLE);
+	if (!(val & 0x01)) {
+		pr_err("EC is disabled\n");
+		goto fail;
+	}
+
+	superio_exit(sioaddr);
+	pr_info("Found %s or compatible chip at %#x:%#x\n",
+		nct6683_chip_names[sio_data->kind], sioaddr, addr);
+	sio_data->sioreg = sioaddr;
+
+	return addr;
+
+fail:
+	superio_exit(sioaddr);
+	return -ENODEV;
+}
+
+/*
+ * 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 nct6683 driver. But since we use platform_device_alloc(), we
+ * must keep track of the device
+ */
+static struct platform_device *pdev[2];
+
+static int __init sensors_nct6683_init(void)
+{
+	struct nct6683_sio_data sio_data;
+	int sioaddr[2] = { 0x2e, 0x4e };
+	struct resource res;
+	bool found = false;
+	int address;
+	int i, err;
+
+	err = platform_driver_register(&nct6683_driver);
+	if (err)
+		return err;
+
+	/*
+	 * 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
+	 * nct6683 hardware monitor, and call probe()
+	 */
+	for (i = 0; i < ARRAY_SIZE(pdev); i++) {
+		address = nct6683_find(sioaddr[i], &sio_data);
+		if (address <= 0)
+			continue;
+
+		found = true;
+
+		pdev[i] = platform_device_alloc(DRVNAME, address);
+		if (!pdev[i]) {
+			err = -ENOMEM;
+			goto exit_device_unregister;
+		}
+
+		err = platform_device_add_data(pdev[i], &sio_data,
+					       sizeof(struct nct6683_sio_data));
+		if (err)
+			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) {
+			platform_device_put(pdev[i]);
+			pdev[i] = NULL;
+			continue;
+		}
+
+		err = platform_device_add_resources(pdev[i], &res, 1);
+		if (err)
+			goto exit_device_put;
+
+		/* platform_device_add calls probe() */
+		err = platform_device_add(pdev[i]);
+		if (err)
+			goto exit_device_put;
+	}
+	if (!found) {
+		err = -ENODEV;
+		goto exit_unregister;
+	}
+
+	return 0;
+
+exit_device_put:
+	platform_device_put(pdev[i]);
+exit_device_unregister:
+	while (--i >= 0) {
+		if (pdev[i])
+			platform_device_unregister(pdev[i]);
+	}
+exit_unregister:
+	platform_driver_unregister(&nct6683_driver);
+	return err;
+}
+
+static void __exit sensors_nct6683_exit(void)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(pdev); i++) {
+		if (pdev[i])
+			platform_device_unregister(pdev[i]);
+	}
+	platform_driver_unregister(&nct6683_driver);
+}
+
+MODULE_AUTHOR("Guenter Roeck <linux@roeck-us.net>");
+MODULE_DESCRIPTION("NCT6683D driver");
+MODULE_LICENSE("GPL");
+
+module_init(sensors_nct6683_init);
+module_exit(sensors_nct6683_exit);
diff --git a/drivers/hwmon/nct6775.c b/drivers/hwmon/nct6775.c
index 38d5a633405..59d9a3fc96b 100644
--- a/drivers/hwmon/nct6775.c
+++ b/drivers/hwmon/nct6775.c
@@ -4160,7 +4160,7 @@ static int __init sensors_nct6775_init(void)
 		pdev[i] = platform_device_alloc(DRVNAME, address);
 		if (!pdev[i]) {
 			err = -ENOMEM;
-			goto exit_device_put;
+			goto exit_device_unregister;
 		}
 
 		err = platform_device_add_data(pdev[i], &sio_data,
@@ -4198,9 +4198,11 @@ static int __init sensors_nct6775_init(void)
 	return 0;
 
 exit_device_put:
-	for (i = 0; i < ARRAY_SIZE(pdev); i++) {
+	platform_device_put(pdev[i]);
+exit_device_unregister:
+	while (--i >= 0) {
 		if (pdev[i])
-			platform_device_put(pdev[i]);
+			platform_device_unregister(pdev[i]);
 	}
 exit_unregister:
 	platform_driver_unregister(&nct6775_driver);
diff --git a/drivers/hwmon/ntc_thermistor.c b/drivers/hwmon/ntc_thermistor.c
index 8a17f01e867..e76feb86a1d 100644
--- a/drivers/hwmon/ntc_thermistor.c
+++ b/drivers/hwmon/ntc_thermistor.c
@@ -44,6 +44,7 @@ struct ntc_compensation {
 	unsigned int	ohm;
 };
 
+/* Order matters, ntc_match references the entries by index */
 static const struct platform_device_id ntc_thermistor_id[] = {
 	{ "ncp15wb473", TYPE_NCPXXWB473 },
 	{ "ncp18wb473", TYPE_NCPXXWB473 },
@@ -141,7 +142,7 @@ struct ntc_data {
 	char name[PLATFORM_NAME_SIZE];
 };
 
-#ifdef CONFIG_OF
+#if defined(CONFIG_OF) && IS_ENABLED(CONFIG_IIO)
 static int ntc_adc_iio_read(struct ntc_thermistor_platform_data *pdata)
 {
 	struct iio_channel *channel = pdata->chan;
@@ -163,15 +164,15 @@ static int ntc_adc_iio_read(struct ntc_thermistor_platform_data *pdata)
 
 static const struct of_device_id ntc_match[] = {
 	{ .compatible = "ntc,ncp15wb473",
-		.data = &ntc_thermistor_id[TYPE_NCPXXWB473] },
+		.data = &ntc_thermistor_id[0] },
 	{ .compatible = "ntc,ncp18wb473",
-		.data = &ntc_thermistor_id[TYPE_NCPXXWB473] },
+		.data = &ntc_thermistor_id[1] },
 	{ .compatible = "ntc,ncp21wb473",
-		.data = &ntc_thermistor_id[TYPE_NCPXXWB473] },
+		.data = &ntc_thermistor_id[2] },
 	{ .compatible = "ntc,ncp03wb473",
-		.data = &ntc_thermistor_id[TYPE_NCPXXWB473] },
+		.data = &ntc_thermistor_id[3] },
 	{ .compatible = "ntc,ncp15wl333",
-		.data = &ntc_thermistor_id[TYPE_NCPXXWL333] },
+		.data = &ntc_thermistor_id[4] },
 	{ },
 };
 MODULE_DEVICE_TABLE(of, ntc_match);
@@ -223,6 +224,8 @@ ntc_thermistor_parse_dt(struct platform_device *pdev)
 	return NULL;
 }
 
+#define ntc_match	NULL
+
 static void ntc_iio_channel_release(struct ntc_thermistor_platform_data *pdata)
 { }
 #endif
diff --git a/drivers/hwmon/pc87427.c b/drivers/hwmon/pc87427.c
index d847e0a084e..9e4684e747e 100644
--- a/drivers/hwmon/pc87427.c
+++ b/drivers/hwmon/pc87427.c
@@ -1081,10 +1081,8 @@ static int pc87427_probe(struct platform_device *pdev)
 
 	data = devm_kzalloc(&pdev->dev, sizeof(struct pc87427_data),
 			    GFP_KERNEL);
-	if (!data) {
-		pr_err("Out of memory\n");
+	if (!data)
 		return -ENOMEM;
-	}
 
 	data->address[0] = sio_data->address[0];
 	data->address[1] = sio_data->address[1];
diff --git a/drivers/hwmon/s3c-hwmon.c b/drivers/hwmon/s3c-hwmon.c
index 73bd64e8c30..0674c13bbd4 100644
--- a/drivers/hwmon/s3c-hwmon.c
+++ b/drivers/hwmon/s3c-hwmon.c
@@ -285,10 +285,8 @@ static int s3c_hwmon_probe(struct platform_device *dev)
 	}
 
 	hwmon = devm_kzalloc(&dev->dev, sizeof(struct s3c_hwmon), GFP_KERNEL);
-	if (hwmon == NULL) {
-		dev_err(&dev->dev, "no memory\n");
+	if (hwmon == NULL)
 		return -ENOMEM;
-	}
 
 	platform_set_drvdata(dev, hwmon);
 
diff --git a/drivers/hwmon/tmp102.c b/drivers/hwmon/tmp102.c
index 6748b4583e7..51719956cc0 100644
--- a/drivers/hwmon/tmp102.c
+++ b/drivers/hwmon/tmp102.c
@@ -51,6 +51,7 @@
 #define	TMP102_THIGH_REG		0x03
 
 struct tmp102 {
+	struct i2c_client *client;
 	struct device *hwmon_dev;
 	struct thermal_zone_device *tz;
 	struct mutex lock;
@@ -77,9 +78,10 @@ static const u8 tmp102_reg[] = {
 	TMP102_THIGH_REG,
 };
 
-static struct tmp102 *tmp102_update_device(struct i2c_client *client)
+static struct tmp102 *tmp102_update_device(struct device *dev)
 {
-	struct tmp102 *tmp102 = i2c_get_clientdata(client);
+	struct tmp102 *tmp102 = dev_get_drvdata(dev);
+	struct i2c_client *client = tmp102->client;
 
 	mutex_lock(&tmp102->lock);
 	if (time_after(jiffies, tmp102->last_update + HZ / 3)) {
@@ -98,7 +100,7 @@ static struct tmp102 *tmp102_update_device(struct i2c_client *client)
 
 static int tmp102_read_temp(void *dev, long *temp)
 {
-	struct tmp102 *tmp102 = tmp102_update_device(to_i2c_client(dev));
+	struct tmp102 *tmp102 = tmp102_update_device(dev);
 
 	*temp = tmp102->temp[0];
 
@@ -110,7 +112,7 @@ static ssize_t tmp102_show_temp(struct device *dev,
 				char *buf)
 {
 	struct sensor_device_attribute *sda = to_sensor_dev_attr(attr);
-	struct tmp102 *tmp102 = tmp102_update_device(to_i2c_client(dev));
+	struct tmp102 *tmp102 = tmp102_update_device(dev);
 
 	return sprintf(buf, "%d\n", tmp102->temp[sda->index]);
 }
@@ -120,8 +122,8 @@ static ssize_t tmp102_set_temp(struct device *dev,
 			       const char *buf, size_t count)
 {
 	struct sensor_device_attribute *sda = to_sensor_dev_attr(attr);
-	struct i2c_client *client = to_i2c_client(dev);
-	struct tmp102 *tmp102 = i2c_get_clientdata(client);
+	struct tmp102 *tmp102 = dev_get_drvdata(dev);
+	struct i2c_client *client = tmp102->client;
 	long val;
 	int status;
 
@@ -145,16 +147,13 @@ static SENSOR_DEVICE_ATTR(temp1_max_hyst, S_IWUSR | S_IRUGO, tmp102_show_temp,
 static SENSOR_DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO, tmp102_show_temp,
 			  tmp102_set_temp, 2);
 
-static struct attribute *tmp102_attributes[] = {
+static struct attribute *tmp102_attrs[] = {
 	&sensor_dev_attr_temp1_input.dev_attr.attr,
 	&sensor_dev_attr_temp1_max_hyst.dev_attr.attr,
 	&sensor_dev_attr_temp1_max.dev_attr.attr,
 	NULL
 };
-
-static const struct attribute_group tmp102_attr_group = {
-	.attrs = tmp102_attributes,
-};
+ATTRIBUTE_GROUPS(tmp102);
 
 #define TMP102_CONFIG  (TMP102_CONF_TM | TMP102_CONF_EM | TMP102_CONF_CR1)
 #define TMP102_CONFIG_RD_ONLY (TMP102_CONF_R0 | TMP102_CONF_R1 | TMP102_CONF_AL)
@@ -162,72 +161,68 @@ static const struct attribute_group tmp102_attr_group = {
 static int tmp102_probe(struct i2c_client *client,
 				  const struct i2c_device_id *id)
 {
+	struct device *dev = &client->dev;
+	struct device *hwmon_dev;
 	struct tmp102 *tmp102;
 	int status;
 
 	if (!i2c_check_functionality(client->adapter,
 				     I2C_FUNC_SMBUS_WORD_DATA)) {
-		dev_err(&client->dev,
+		dev_err(dev,
 			"adapter doesn't support SMBus word transactions\n");
 		return -ENODEV;
 	}
 
-	tmp102 = devm_kzalloc(&client->dev, sizeof(*tmp102), GFP_KERNEL);
+	tmp102 = devm_kzalloc(dev, sizeof(*tmp102), GFP_KERNEL);
 	if (!tmp102)
 		return -ENOMEM;
 
 	i2c_set_clientdata(client, tmp102);
+	tmp102->client = client;
 
 	status = i2c_smbus_read_word_swapped(client, TMP102_CONF_REG);
 	if (status < 0) {
-		dev_err(&client->dev, "error reading config register\n");
+		dev_err(dev, "error reading config register\n");
 		return status;
 	}
 	tmp102->config_orig = status;
 	status = i2c_smbus_write_word_swapped(client, TMP102_CONF_REG,
 					      TMP102_CONFIG);
 	if (status < 0) {
-		dev_err(&client->dev, "error writing config register\n");
+		dev_err(dev, "error writing config register\n");
 		goto fail_restore_config;
 	}
 	status = i2c_smbus_read_word_swapped(client, TMP102_CONF_REG);
 	if (status < 0) {
-		dev_err(&client->dev, "error reading config register\n");
+		dev_err(dev, "error reading config register\n");
 		goto fail_restore_config;
 	}
 	status &= ~TMP102_CONFIG_RD_ONLY;
 	if (status != TMP102_CONFIG) {
-		dev_err(&client->dev, "config settings did not stick\n");
+		dev_err(dev, "config settings did not stick\n");
 		status = -ENODEV;
 		goto fail_restore_config;
 	}
 	tmp102->last_update = jiffies - HZ;
 	mutex_init(&tmp102->lock);
 
-	status = sysfs_create_group(&client->dev.kobj, &tmp102_attr_group);
-	if (status) {
-		dev_dbg(&client->dev, "could not create sysfs files\n");
+	hwmon_dev = hwmon_device_register_with_groups(dev, client->name,
+						      tmp102, tmp102_groups);
+	if (IS_ERR(hwmon_dev)) {
+		dev_dbg(dev, "unable to register hwmon device\n");
+		status = PTR_ERR(hwmon_dev);
 		goto fail_restore_config;
 	}
-	tmp102->hwmon_dev = hwmon_device_register(&client->dev);
-	if (IS_ERR(tmp102->hwmon_dev)) {
-		dev_dbg(&client->dev, "unable to register hwmon device\n");
-		status = PTR_ERR(tmp102->hwmon_dev);
-		goto fail_remove_sysfs;
-	}
-
-	tmp102->tz = thermal_zone_of_sensor_register(&client->dev, 0,
-						     &client->dev,
+	tmp102->hwmon_dev = hwmon_dev;
+	tmp102->tz = thermal_zone_of_sensor_register(hwmon_dev, 0, hwmon_dev,
 						     tmp102_read_temp, NULL);
 	if (IS_ERR(tmp102->tz))
 		tmp102->tz = NULL;
 
-	dev_info(&client->dev, "initialized\n");
+	dev_info(dev, "initialized\n");
 
 	return 0;
 
-fail_remove_sysfs:
-	sysfs_remove_group(&client->dev.kobj, &tmp102_attr_group);
 fail_restore_config:
 	i2c_smbus_write_word_swapped(client, TMP102_CONF_REG,
 				     tmp102->config_orig);
@@ -238,9 +233,8 @@ static int tmp102_remove(struct i2c_client *client)
 {
 	struct tmp102 *tmp102 = i2c_get_clientdata(client);
 
-	thermal_zone_of_sensor_unregister(&client->dev, tmp102->tz);
+	thermal_zone_of_sensor_unregister(tmp102->hwmon_dev, tmp102->tz);
 	hwmon_device_unregister(tmp102->hwmon_dev);
-	sysfs_remove_group(&client->dev.kobj, &tmp102_attr_group);
 
 	/* Stop monitoring if device was stopped originally */
 	if (tmp102->config_orig & TMP102_CONF_SD) {
diff --git a/drivers/hwmon/tmp421.c b/drivers/hwmon/tmp421.c
index ae26b06fa81..7bab7a9bedc 100644
--- a/drivers/hwmon/tmp421.c
+++ b/drivers/hwmon/tmp421.c
@@ -69,7 +69,7 @@ static const struct i2c_device_id tmp421_id[] = {
 MODULE_DEVICE_TABLE(i2c, tmp421_id);
 
 struct tmp421_data {
-	struct device *hwmon_dev;
+	struct i2c_client *client;
 	struct mutex update_lock;
 	char valid;
 	unsigned long last_updated;
@@ -99,8 +99,8 @@ static int temp_from_u16(u16 reg)
 
 static struct tmp421_data *tmp421_update_device(struct device *dev)
 {
-	struct i2c_client *client = to_i2c_client(dev);
-	struct tmp421_data *data = i2c_get_clientdata(client);
+	struct tmp421_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
 	int i;
 
 	mutex_lock(&data->update_lock);
@@ -198,6 +198,11 @@ static const struct attribute_group tmp421_group = {
 	.is_visible = tmp421_is_visible,
 };
 
+static const struct attribute_group *tmp421_groups[] = {
+	&tmp421_group,
+	NULL
+};
+
 static int tmp421_init_client(struct i2c_client *client)
 {
 	int config, config_orig;
@@ -264,47 +269,26 @@ static int tmp421_detect(struct i2c_client *client,
 static int tmp421_probe(struct i2c_client *client,
 			const struct i2c_device_id *id)
 {
+	struct device *dev = &client->dev;
+	struct device *hwmon_dev;
 	struct tmp421_data *data;
 	int err;
 
-	data = devm_kzalloc(&client->dev, sizeof(struct tmp421_data),
-			    GFP_KERNEL);
+	data = devm_kzalloc(dev, sizeof(struct tmp421_data), GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
 
-	i2c_set_clientdata(client, data);
 	mutex_init(&data->update_lock);
 	data->channels = id->driver_data;
+	data->client = client;
 
 	err = tmp421_init_client(client);
 	if (err)
 		return err;
 
-	err = sysfs_create_group(&client->dev.kobj, &tmp421_group);
-	if (err)
-		return err;
-
-	data->hwmon_dev = hwmon_device_register(&client->dev);
-	if (IS_ERR(data->hwmon_dev)) {
-		err = PTR_ERR(data->hwmon_dev);
-		data->hwmon_dev = NULL;
-		goto exit_remove;
-	}
-	return 0;
-
-exit_remove:
-	sysfs_remove_group(&client->dev.kobj, &tmp421_group);
-	return err;
-}
-
-static int tmp421_remove(struct i2c_client *client)
-{
-	struct tmp421_data *data = i2c_get_clientdata(client);
-
-	hwmon_device_unregister(data->hwmon_dev);
-	sysfs_remove_group(&client->dev.kobj, &tmp421_group);
-
-	return 0;
+	hwmon_dev = devm_hwmon_device_register_with_groups(dev, client->name,
+							   data, tmp421_groups);
+	return PTR_ERR_OR_ZERO(hwmon_dev);
 }
 
 static struct i2c_driver tmp421_driver = {
@@ -313,7 +297,6 @@ static struct i2c_driver tmp421_driver = {
 		.name	= "tmp421",
 	},
 	.probe = tmp421_probe,
-	.remove = tmp421_remove,
 	.id_table = tmp421_id,
 	.detect = tmp421_detect,
 	.address_list = normal_i2c,
diff --git a/drivers/hwmon/ultra45_env.c b/drivers/hwmon/ultra45_env.c
index fb3e69341c1..7d465863606 100644
--- a/drivers/hwmon/ultra45_env.c
+++ b/drivers/hwmon/ultra45_env.c
@@ -252,7 +252,7 @@ static const struct attribute_group env_group = {
 
 static int env_probe(struct platform_device *op)
 {
-	struct env *p = kzalloc(sizeof(*p), GFP_KERNEL);
+	struct env *p = devm_kzalloc(&op->dev, sizeof(*p), GFP_KERNEL);
 	int err = -ENOMEM;
 
 	if (!p)
@@ -262,7 +262,7 @@ static int env_probe(struct platform_device *op)
 
 	p->regs = of_ioremap(&op->resource[0], 0, REG_SIZE, "pic16f747");
 	if (!p->regs)
-		goto out_free;
+		goto out;
 
 	err = sysfs_create_group(&op->dev.kobj, &env_group);
 	if (err)
@@ -286,8 +286,6 @@ out_sysfs_remove_group:
 out_iounmap:
 	of_iounmap(&op->resource[0], p->regs, REG_SIZE);
 
-out_free:
-	kfree(p);
 	goto out;
 }
 
@@ -299,7 +297,6 @@ static int env_remove(struct platform_device *op)
 		sysfs_remove_group(&op->dev.kobj, &env_group);
 		hwmon_device_unregister(p->hwmon_dev);
 		of_iounmap(&op->resource[0], p->regs, REG_SIZE);
-		kfree(p);
 	}
 
 	return 0;
diff --git a/drivers/hwmon/vexpress.c b/drivers/hwmon/vexpress.c
index d867e6bb2be..611f34c7333 100644
--- a/drivers/hwmon/vexpress.c
+++ b/drivers/hwmon/vexpress.c
@@ -26,16 +26,16 @@
 
 struct vexpress_hwmon_data {
 	struct device *hwmon_dev;
-	struct vexpress_config_func *func;
+	struct regmap *reg;
+	const char *name;
 };
 
 static ssize_t vexpress_hwmon_name_show(struct device *dev,
 		struct device_attribute *dev_attr, char *buffer)
 {
-	const char *compatible = of_get_property(dev->of_node, "compatible",
-			NULL);
+	struct vexpress_hwmon_data *data = dev_get_drvdata(dev);
 
-	return sprintf(buffer, "%s\n", compatible);
+	return sprintf(buffer, "%s\n", data->name);
 }
 
 static ssize_t vexpress_hwmon_label_show(struct device *dev,
@@ -43,9 +43,6 @@ static ssize_t vexpress_hwmon_label_show(struct device *dev,
 {
 	const char *label = of_get_property(dev->of_node, "label", NULL);
 
-	if (!label)
-		return -ENOENT;
-
 	return snprintf(buffer, PAGE_SIZE, "%s\n", label);
 }
 
@@ -56,7 +53,7 @@ static ssize_t vexpress_hwmon_u32_show(struct device *dev,
 	int err;
 	u32 value;
 
-	err = vexpress_config_read(data->func, 0, &value);
+	err = regmap_read(data->reg, 0, &value);
 	if (err)
 		return err;
 
@@ -71,11 +68,11 @@ static ssize_t vexpress_hwmon_u64_show(struct device *dev,
 	int err;
 	u32 value_hi, value_lo;
 
-	err = vexpress_config_read(data->func, 0, &value_lo);
+	err = regmap_read(data->reg, 0, &value_lo);
 	if (err)
 		return err;
 
-	err = vexpress_config_read(data->func, 1, &value_hi);
+	err = regmap_read(data->reg, 1, &value_hi);
 	if (err)
 		return err;
 
@@ -84,6 +81,20 @@ static ssize_t vexpress_hwmon_u64_show(struct device *dev,
 			to_sensor_dev_attr(dev_attr)->index));
 }
 
+static umode_t vexpress_hwmon_attr_is_visible(struct kobject *kobj,
+		struct attribute *attr, int index)
+{
+	struct device *dev = kobj_to_dev(kobj);
+	struct device_attribute *dev_attr = container_of(attr,
+				struct device_attribute, attr);
+
+	if (dev_attr->show == vexpress_hwmon_label_show &&
+			!of_get_property(dev->of_node, "label", NULL))
+		return 0;
+
+	return attr->mode;
+}
+
 static DEVICE_ATTR(name, S_IRUGO, vexpress_hwmon_name_show, NULL);
 
 #define VEXPRESS_HWMON_ATTRS(_name, _label_attr, _input_attr)	\
@@ -94,14 +105,27 @@ struct attribute *vexpress_hwmon_attrs_##_name[] = {		\
 	NULL							\
 }
 
+struct vexpress_hwmon_type {
+	const char *name;
+	const struct attribute_group **attr_groups;
+};
+
 #if !defined(CONFIG_REGULATOR_VEXPRESS)
 static DEVICE_ATTR(in1_label, S_IRUGO, vexpress_hwmon_label_show, NULL);
 static SENSOR_DEVICE_ATTR(in1_input, S_IRUGO, vexpress_hwmon_u32_show,
 		NULL, 1000);
 static VEXPRESS_HWMON_ATTRS(volt, in1_label, in1_input);
 static struct attribute_group vexpress_hwmon_group_volt = {
+	.is_visible = vexpress_hwmon_attr_is_visible,
 	.attrs = vexpress_hwmon_attrs_volt,
 };
+static struct vexpress_hwmon_type vexpress_hwmon_volt = {
+	.name = "vexpress_volt",
+	.attr_groups = (const struct attribute_group *[]) {
+		&vexpress_hwmon_group_volt,
+		NULL,
+	},
+};
 #endif
 
 static DEVICE_ATTR(curr1_label, S_IRUGO, vexpress_hwmon_label_show, NULL);
@@ -109,52 +133,84 @@ static SENSOR_DEVICE_ATTR(curr1_input, S_IRUGO, vexpress_hwmon_u32_show,
 		NULL, 1000);
 static VEXPRESS_HWMON_ATTRS(amp, curr1_label, curr1_input);
 static struct attribute_group vexpress_hwmon_group_amp = {
+	.is_visible = vexpress_hwmon_attr_is_visible,
 	.attrs = vexpress_hwmon_attrs_amp,
 };
+static struct vexpress_hwmon_type vexpress_hwmon_amp = {
+	.name = "vexpress_amp",
+	.attr_groups = (const struct attribute_group *[]) {
+		&vexpress_hwmon_group_amp,
+		NULL
+	},
+};
 
 static DEVICE_ATTR(temp1_label, S_IRUGO, vexpress_hwmon_label_show, NULL);
 static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, vexpress_hwmon_u32_show,
 		NULL, 1000);
 static VEXPRESS_HWMON_ATTRS(temp, temp1_label, temp1_input);
 static struct attribute_group vexpress_hwmon_group_temp = {
+	.is_visible = vexpress_hwmon_attr_is_visible,
 	.attrs = vexpress_hwmon_attrs_temp,
 };
+static struct vexpress_hwmon_type vexpress_hwmon_temp = {
+	.name = "vexpress_temp",
+	.attr_groups = (const struct attribute_group *[]) {
+		&vexpress_hwmon_group_temp,
+		NULL
+	},
+};
 
 static DEVICE_ATTR(power1_label, S_IRUGO, vexpress_hwmon_label_show, NULL);
 static SENSOR_DEVICE_ATTR(power1_input, S_IRUGO, vexpress_hwmon_u32_show,
 		NULL, 1);
 static VEXPRESS_HWMON_ATTRS(power, power1_label, power1_input);
 static struct attribute_group vexpress_hwmon_group_power = {
+	.is_visible = vexpress_hwmon_attr_is_visible,
 	.attrs = vexpress_hwmon_attrs_power,
 };
+static struct vexpress_hwmon_type vexpress_hwmon_power = {
+	.name = "vexpress_power",
+	.attr_groups = (const struct attribute_group *[]) {
+		&vexpress_hwmon_group_power,
+		NULL
+	},
+};
 
 static DEVICE_ATTR(energy1_label, S_IRUGO, vexpress_hwmon_label_show, NULL);
 static SENSOR_DEVICE_ATTR(energy1_input, S_IRUGO, vexpress_hwmon_u64_show,
 		NULL, 1);
 static VEXPRESS_HWMON_ATTRS(energy, energy1_label, energy1_input);
 static struct attribute_group vexpress_hwmon_group_energy = {
+	.is_visible = vexpress_hwmon_attr_is_visible,
 	.attrs = vexpress_hwmon_attrs_energy,
 };
+static struct vexpress_hwmon_type vexpress_hwmon_energy = {
+	.name = "vexpress_energy",
+	.attr_groups = (const struct attribute_group *[]) {
+		&vexpress_hwmon_group_energy,
+		NULL
+	},
+};
 
 static struct of_device_id vexpress_hwmon_of_match[] = {
 #if !defined(CONFIG_REGULATOR_VEXPRESS)
 	{
 		.compatible = "arm,vexpress-volt",
-		.data = &vexpress_hwmon_group_volt,
+		.data = &vexpress_hwmon_volt,
 	},
 #endif
 	{
 		.compatible = "arm,vexpress-amp",
-		.data = &vexpress_hwmon_group_amp,
+		.data = &vexpress_hwmon_amp,
 	}, {
 		.compatible = "arm,vexpress-temp",
-		.data = &vexpress_hwmon_group_temp,
+		.data = &vexpress_hwmon_temp,
 	}, {
 		.compatible = "arm,vexpress-power",
-		.data = &vexpress_hwmon_group_power,
+		.data = &vexpress_hwmon_power,
 	}, {
 		.compatible = "arm,vexpress-energy",
-		.data = &vexpress_hwmon_group_energy,
+		.data = &vexpress_hwmon_energy,
 	},
 	{}
 };
@@ -165,6 +221,7 @@ static int vexpress_hwmon_probe(struct platform_device *pdev)
 	int err;
 	const struct of_device_id *match;
 	struct vexpress_hwmon_data *data;
+	const struct vexpress_hwmon_type *type;
 
 	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
 	if (!data)
@@ -174,12 +231,14 @@ static int vexpress_hwmon_probe(struct platform_device *pdev)
 	match = of_match_device(vexpress_hwmon_of_match, &pdev->dev);
 	if (!match)
 		return -ENODEV;
+	type = match->data;
+	data->name = type->name;
 
-	data->func = vexpress_config_func_get_by_dev(&pdev->dev);
-	if (!data->func)
-		return -ENODEV;
+	data->reg = devm_regmap_init_vexpress_config(&pdev->dev);
+	if (IS_ERR(data->reg))
+		return PTR_ERR(data->reg);
 
-	err = sysfs_create_group(&pdev->dev.kobj, match->data);
+	err = sysfs_create_groups(&pdev->dev.kobj, type->attr_groups);
 	if (err)
 		goto error;
 
@@ -193,7 +252,6 @@ static int vexpress_hwmon_probe(struct platform_device *pdev)
 
 error:
 	sysfs_remove_group(&pdev->dev.kobj, match->data);
-	vexpress_config_func_put(data->func);
 	return err;
 }
 
@@ -207,8 +265,6 @@ static int vexpress_hwmon_remove(struct platform_device *pdev)
 	match = of_match_device(vexpress_hwmon_of_match, &pdev->dev);
 	sysfs_remove_group(&pdev->dev.kobj, match->data);
 
-	vexpress_config_func_put(data->func);
-
 	return 0;
 }
 
diff --git a/drivers/hwmon/vt1211.c b/drivers/hwmon/vt1211.c
index 6b2f1a42b3f..344b22ec255 100644
--- a/drivers/hwmon/vt1211.c
+++ b/drivers/hwmon/vt1211.c
@@ -1152,10 +1152,8 @@ static int vt1211_probe(struct platform_device *pdev)
 	int i, err;
 
 	data = devm_kzalloc(dev, sizeof(struct vt1211_data), GFP_KERNEL);
-	if (!data) {
-		dev_err(dev, "Out of memory\n");
+	if (!data)
 		return -ENOMEM;
-	}
 
 	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
 	if (!devm_request_region(dev, res->start, resource_size(res),
diff --git a/drivers/i2c/busses/i2c-designware-core.c b/drivers/i2c/busses/i2c-designware-core.c
index 22e92c3d3d0..3c20e4bd6dd 100644
--- a/drivers/i2c/busses/i2c-designware-core.c
+++ b/drivers/i2c/busses/i2c-designware-core.c
@@ -422,6 +422,9 @@ static void i2c_dw_xfer_init(struct dw_i2c_dev *dev)
 	 */
 	dw_writel(dev, msgs[dev->msg_write_idx].addr | ic_tar, DW_IC_TAR);
 
+	/* enforce disabled interrupts (due to HW issues) */
+	i2c_dw_disable_int(dev);
+
 	/* Enable the adapter */
 	__i2c_dw_enable(dev, true);
 
diff --git a/drivers/i2c/busses/i2c-nomadik.c b/drivers/i2c/busses/i2c-nomadik.c
index 28cbe1b2a2e..32c85e9ecda 100644
--- a/drivers/i2c/busses/i2c-nomadik.c
+++ b/drivers/i2c/busses/i2c-nomadik.c
@@ -999,7 +999,7 @@ static int nmk_i2c_probe(struct amba_device *adev, const struct amba_id *id)
 
 	dev->virtbase = devm_ioremap(&adev->dev, adev->res.start,
 				resource_size(&adev->res));
-	if (IS_ERR(dev->virtbase)) {
+	if (!dev->virtbase) {
 		ret = -ENOMEM;
 		goto err_no_mem;
 	}
diff --git a/drivers/i2c/busses/i2c-qup.c b/drivers/i2c/busses/i2c-qup.c
index 1b4cf14f110..2a5efb5b487 100644
--- a/drivers/i2c/busses/i2c-qup.c
+++ b/drivers/i2c/busses/i2c-qup.c
@@ -479,7 +479,7 @@ static int qup_i2c_xfer(struct i2c_adapter *adap,
 	int ret, idx;
 
 	ret = pm_runtime_get_sync(qup->dev);
-	if (ret)
+	if (ret < 0)
 		goto out;
 
 	writel(1, qup->base + QUP_SW_RESET);
diff --git a/drivers/i2c/busses/i2c-rcar.c b/drivers/i2c/busses/i2c-rcar.c
index d4fa8eba6e9..06d47aafbb7 100644
--- a/drivers/i2c/busses/i2c-rcar.c
+++ b/drivers/i2c/busses/i2c-rcar.c
@@ -561,6 +561,12 @@ static int rcar_i2c_master_xfer(struct i2c_adapter *adap,
 
 	ret = -EINVAL;
 	for (i = 0; i < num; i++) {
+		/* This HW can't send STOP after address phase */
+		if (msgs[i].len == 0) {
+			ret = -EOPNOTSUPP;
+			break;
+		}
+
 		/*-------------- spin lock -----------------*/
 		spin_lock_irqsave(&priv->lock, flags);
 
@@ -625,7 +631,8 @@ static int rcar_i2c_master_xfer(struct i2c_adapter *adap,
 
 static u32 rcar_i2c_func(struct i2c_adapter *adap)
 {
-	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
+	/* This HW can't do SMBUS_QUICK and NOSTART */
+	return I2C_FUNC_I2C | (I2C_FUNC_SMBUS_EMUL & ~I2C_FUNC_SMBUS_QUICK);
 }
 
 static const struct i2c_algorithm rcar_i2c_algo = {
diff --git a/drivers/i2c/busses/i2c-s3c2410.c b/drivers/i2c/busses/i2c-s3c2410.c
index ae4491062e4..bb3a9964f7e 100644
--- a/drivers/i2c/busses/i2c-s3c2410.c
+++ b/drivers/i2c/busses/i2c-s3c2410.c
@@ -1276,10 +1276,10 @@ static int s3c24xx_i2c_resume(struct device *dev)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct s3c24xx_i2c *i2c = platform_get_drvdata(pdev);
 
-	i2c->suspended = 0;
 	clk_prepare_enable(i2c->clk);
 	s3c24xx_i2c_init(i2c);
 	clk_disable_unprepare(i2c->clk);
+	i2c->suspended = 0;
 
 	return 0;
 }
diff --git a/drivers/ide/ide-disk.c b/drivers/ide/ide-disk.c
index 16f69be820c..ee880382e3b 100644
--- a/drivers/ide/ide-disk.c
+++ b/drivers/ide/ide-disk.c
@@ -188,10 +188,9 @@ static ide_startstop_t ide_do_rw_disk(ide_drive_t *drive, struct request *rq,
 
 	ledtrig_ide_activity();
 
-	pr_debug("%s: %sing: block=%llu, sectors=%u, buffer=0x%08lx\n",
+	pr_debug("%s: %sing: block=%llu, sectors=%u\n",
 		 drive->name, rq_data_dir(rq) == READ ? "read" : "writ",
-		 (unsigned long long)block, blk_rq_sectors(rq),
-		 (unsigned long)rq->buffer);
+		 (unsigned long long)block, blk_rq_sectors(rq));
 
 	if (hwif->rw_disk)
 		hwif->rw_disk(drive, rq);
diff --git a/drivers/idle/intel_idle.c b/drivers/idle/intel_idle.c
index a43220c2e3d..4d140bbbe10 100644
--- a/drivers/idle/intel_idle.c
+++ b/drivers/idle/intel_idle.c
@@ -750,9 +750,10 @@ void intel_idle_state_table_update(void)
 			if (package_num + 1 > num_sockets) {
 				num_sockets = package_num + 1;
 
-				if (num_sockets > 4)
+				if (num_sockets > 4) {
 					cpuidle_state_table = ivt_cstates_8s;
 					return;
+				}
 			}
 		}
 
diff --git a/drivers/iio/Kconfig b/drivers/iio/Kconfig
index 5dd0e120a50..345395e9dc6 100644
--- a/drivers/iio/Kconfig
+++ b/drivers/iio/Kconfig
@@ -4,6 +4,7 @@
 
 menuconfig IIO
 	tristate "Industrial I/O support"
+	select ANON_INODES
 	help
 	  The industrial I/O subsystem provides a unified framework for
 	  drivers for many different types of embedded sensors using a
@@ -74,6 +75,7 @@ if IIO_TRIGGER
    source "drivers/iio/trigger/Kconfig"
 endif #IIO_TRIGGER
 source "drivers/iio/pressure/Kconfig"
+source "drivers/iio/proximity/Kconfig"
 source "drivers/iio/temperature/Kconfig"
 
 endif # IIO
diff --git a/drivers/iio/Makefile b/drivers/iio/Makefile
index 887d39090d7..698afc2d17c 100644
--- a/drivers/iio/Makefile
+++ b/drivers/iio/Makefile
@@ -24,5 +24,6 @@ obj-y += light/
 obj-y += magnetometer/
 obj-y += orientation/
 obj-y += pressure/
+obj-y += proximity/
 obj-y += temperature/
 obj-y += trigger/
diff --git a/drivers/iio/accel/Kconfig b/drivers/iio/accel/Kconfig
index e23e5085065..1e120fa1e15 100644
--- a/drivers/iio/accel/Kconfig
+++ b/drivers/iio/accel/Kconfig
@@ -65,4 +65,16 @@ config KXSD9
 	  Say yes here to build support for the Kionix KXSD9 accelerometer.
 	  Currently this only supports the device via an SPI interface.
 
+config MMA8452
+	tristate "Freescale MMA8452Q Accelerometer Driver"
+	depends on I2C
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	help
+	  Say yes here to build support for the Freescale MMA8452Q 3-axis
+	  accelerometer.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called mma8452.
+
 endmenu
diff --git a/drivers/iio/accel/Makefile b/drivers/iio/accel/Makefile
index c48d15f2561..dc0e379c259 100644
--- a/drivers/iio/accel/Makefile
+++ b/drivers/iio/accel/Makefile
@@ -6,6 +6,7 @@
 obj-$(CONFIG_BMA180) += bma180.o
 obj-$(CONFIG_HID_SENSOR_ACCEL_3D) += hid-sensor-accel-3d.o
 obj-$(CONFIG_KXSD9)	+= kxsd9.o
+obj-$(CONFIG_MMA8452)	+= mma8452.o
 
 obj-$(CONFIG_IIO_ST_ACCEL_3AXIS) += st_accel.o
 st_accel-y := st_accel_core.o
diff --git a/drivers/iio/accel/hid-sensor-accel-3d.c b/drivers/iio/accel/hid-sensor-accel-3d.c
index 3dcdbad6545..69abf9163df 100644
--- a/drivers/iio/accel/hid-sensor-accel-3d.c
+++ b/drivers/iio/accel/hid-sensor-accel-3d.c
@@ -22,6 +22,7 @@
 #include <linux/interrupt.h>
 #include <linux/irq.h>
 #include <linux/slab.h>
+#include <linux/delay.h>
 #include <linux/hid-sensor-hub.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
@@ -42,6 +43,10 @@ struct accel_3d_state {
 	struct hid_sensor_common common_attributes;
 	struct hid_sensor_hub_attribute_info accel[ACCEL_3D_CHANNEL_MAX];
 	u32 accel_val[ACCEL_3D_CHANNEL_MAX];
+	int scale_pre_decml;
+	int scale_post_decml;
+	int scale_precision;
+	int value_offset;
 };
 
 static const u32 accel_3d_addresses[ACCEL_3D_CHANNEL_MAX] = {
@@ -56,6 +61,7 @@ static const struct iio_chan_spec accel_3d_channels[] = {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
 		BIT(IIO_CHAN_INFO_SCALE) |
 		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
@@ -65,6 +71,7 @@ static const struct iio_chan_spec accel_3d_channels[] = {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Y,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
 		BIT(IIO_CHAN_INFO_SCALE) |
 		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
@@ -74,6 +81,7 @@ static const struct iio_chan_spec accel_3d_channels[] = {
 		.type = IIO_ACCEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
 		BIT(IIO_CHAN_INFO_SCALE) |
 		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
@@ -104,31 +112,42 @@ static int accel_3d_read_raw(struct iio_dev *indio_dev,
 	u32 address;
 	int ret;
 	int ret_type;
+	s32 poll_value;
 
 	*val = 0;
 	*val2 = 0;
 	switch (mask) {
 	case 0:
+		poll_value = hid_sensor_read_poll_value(
+					&accel_state->common_attributes);
+		if (poll_value < 0)
+			return -EINVAL;
+
+		hid_sensor_power_state(&accel_state->common_attributes, true);
+		msleep_interruptible(poll_value * 2);
 		report_id = accel_state->accel[chan->scan_index].report_id;
 		address = accel_3d_addresses[chan->scan_index];
 		if (report_id >= 0)
 			*val = sensor_hub_input_attr_get_raw_value(
-				accel_state->common_attributes.hsdev,
-				HID_USAGE_SENSOR_ACCEL_3D, address,
-				report_id);
+					accel_state->common_attributes.hsdev,
+					HID_USAGE_SENSOR_ACCEL_3D, address,
+					report_id);
 		else {
 			*val = 0;
+			hid_sensor_power_state(&accel_state->common_attributes,
+						 false);
 			return -EINVAL;
 		}
+		hid_sensor_power_state(&accel_state->common_attributes, false);
 		ret_type = IIO_VAL_INT;
 		break;
 	case IIO_CHAN_INFO_SCALE:
-		*val = accel_state->accel[CHANNEL_SCAN_INDEX_X].units;
-		ret_type = IIO_VAL_INT;
+		*val = accel_state->scale_pre_decml;
+		*val2 = accel_state->scale_post_decml;
+		ret_type = accel_state->scale_precision;
 		break;
 	case IIO_CHAN_INFO_OFFSET:
-		*val = hid_sensor_convert_exponent(
-			accel_state->accel[CHANNEL_SCAN_INDEX_X].unit_expo);
+		*val = accel_state->value_offset;
 		ret_type = IIO_VAL_INT;
 		break;
 	case IIO_CHAN_INFO_SAMP_FREQ:
@@ -197,9 +216,8 @@ static int accel_3d_proc_event(struct hid_sensor_hub_device *hsdev,
 	struct iio_dev *indio_dev = platform_get_drvdata(priv);
 	struct accel_3d_state *accel_state = iio_priv(indio_dev);
 
-	dev_dbg(&indio_dev->dev, "accel_3d_proc_event [%d]\n",
-				accel_state->common_attributes.data_ready);
-	if (accel_state->common_attributes.data_ready)
+	dev_dbg(&indio_dev->dev, "accel_3d_proc_event\n");
+	if (atomic_read(&accel_state->common_attributes.data_ready))
 		hid_sensor_push_data(indio_dev,
 				accel_state->accel_val,
 				sizeof(accel_state->accel_val));
@@ -262,6 +280,11 @@ static int accel_3d_parse_report(struct platform_device *pdev,
 			st->accel[1].index, st->accel[1].report_id,
 			st->accel[2].index, st->accel[2].report_id);
 
+	st->scale_precision = hid_sensor_format_scale(
+				HID_USAGE_SENSOR_ACCEL_3D,
+				&st->accel[CHANNEL_SCAN_INDEX_X],
+				&st->scale_pre_decml, &st->scale_post_decml);
+
 	/* Set Sensitivity field ids, when there is no individual modifier */
 	if (st->common_attributes.sensitivity.index < 0) {
 		sensor_hub_input_get_attribute_info(hsdev,
@@ -333,7 +356,7 @@ static int hid_accel_3d_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
 		goto error_free_dev_mem;
 	}
-	accel_state->common_attributes.data_ready = false;
+	atomic_set(&accel_state->common_attributes.data_ready, 0);
 	ret = hid_sensor_setup_trigger(indio_dev, name,
 					&accel_state->common_attributes);
 	if (ret < 0) {
diff --git a/drivers/iio/accel/mma8452.c b/drivers/iio/accel/mma8452.c
new file mode 100644
index 00000000000..17aeea17056
--- /dev/null
+++ b/drivers/iio/accel/mma8452.c
@@ -0,0 +1,439 @@
+/*
+ * mma8452.c - Support for Freescale MMA8452Q 3-axis 12-bit accelerometer
+ *
+ * Copyright 2014 Peter Meerwald <pmeerw@pmeerw.net>
+ *
+ * This file is subject to the terms and conditions of version 2 of
+ * the GNU General Public License.  See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * 7-bit I2C slave address 0x1c/0x1d (pin selectable)
+ *
+ * TODO: interrupt, thresholding, orientation / freefall events, autosleep
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/delay.h>
+
+#define MMA8452_STATUS 0x00
+#define MMA8452_OUT_X 0x01 /* MSB first, 12-bit  */
+#define MMA8452_OUT_Y 0x03
+#define MMA8452_OUT_Z 0x05
+#define MMA8452_WHO_AM_I 0x0d
+#define MMA8452_DATA_CFG 0x0e
+#define MMA8452_OFF_X 0x2f
+#define MMA8452_OFF_Y 0x30
+#define MMA8452_OFF_Z 0x31
+#define MMA8452_CTRL_REG1 0x2a
+#define MMA8452_CTRL_REG2 0x2b
+
+#define MMA8452_STATUS_DRDY (BIT(2) | BIT(1) | BIT(0))
+
+#define MMA8452_CTRL_DR_MASK (BIT(5) | BIT(4) | BIT(3))
+#define MMA8452_CTRL_DR_SHIFT 3
+#define MMA8452_CTRL_DR_DEFAULT 0x4 /* 50 Hz sample frequency */
+#define MMA8452_CTRL_ACTIVE BIT(0)
+
+#define MMA8452_DATA_CFG_FS_MASK (BIT(1) | BIT(0))
+#define MMA8452_DATA_CFG_FS_2G 0
+#define MMA8452_DATA_CFG_FS_4G 1
+#define MMA8452_DATA_CFG_FS_8G 2
+
+#define MMA8452_DEVICE_ID 0x2a
+
+struct mma8452_data {
+	struct i2c_client *client;
+	struct mutex lock;
+	u8 ctrl_reg1;
+	u8 data_cfg;
+};
+
+static int mma8452_drdy(struct mma8452_data *data)
+{
+	int tries = 150;
+
+	while (tries-- > 0) {
+		int ret = i2c_smbus_read_byte_data(data->client,
+			MMA8452_STATUS);
+		if (ret < 0)
+			return ret;
+		if ((ret & MMA8452_STATUS_DRDY) == MMA8452_STATUS_DRDY)
+			return 0;
+		msleep(20);
+	}
+
+	dev_err(&data->client->dev, "data not ready\n");
+	return -EIO;
+}
+
+static int mma8452_read(struct mma8452_data *data, __be16 buf[3])
+{
+	int ret = mma8452_drdy(data);
+	if (ret < 0)
+		return ret;
+	return i2c_smbus_read_i2c_block_data(data->client,
+		MMA8452_OUT_X, 3 * sizeof(__be16), (u8 *) buf);
+}
+
+static ssize_t mma8452_show_int_plus_micros(char *buf,
+	const int (*vals)[2], int n)
+{
+	size_t len = 0;
+
+	while (n-- > 0)
+		len += scnprintf(buf + len, PAGE_SIZE - len,
+			"%d.%06d ", vals[n][0], vals[n][1]);
+
+	/* replace trailing space by newline */
+	buf[len - 1] = '\n';
+
+	return len;
+}
+
+static int mma8452_get_int_plus_micros_index(const int (*vals)[2], int n,
+					int val, int val2)
+{
+	while (n-- > 0)
+		if (val == vals[n][0] && val2 == vals[n][1])
+			return n;
+
+	return -EINVAL;
+}
+
+static const int mma8452_samp_freq[8][2] = {
+	{800, 0}, {400, 0}, {200, 0}, {100, 0}, {50, 0}, {12, 500000},
+	{6, 250000}, {1, 560000}
+};
+
+static const int mma8452_scales[3][2] = {
+	{0, 977}, {0, 1953}, {0, 3906}
+};
+
+static ssize_t mma8452_show_samp_freq_avail(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	return mma8452_show_int_plus_micros(buf, mma8452_samp_freq,
+		ARRAY_SIZE(mma8452_samp_freq));
+}
+
+static ssize_t mma8452_show_scale_avail(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	return mma8452_show_int_plus_micros(buf, mma8452_scales,
+		ARRAY_SIZE(mma8452_scales));
+}
+
+static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(mma8452_show_samp_freq_avail);
+static IIO_DEVICE_ATTR(in_accel_scale_available, S_IRUGO,
+	mma8452_show_scale_avail, NULL, 0);
+
+static int mma8452_get_samp_freq_index(struct mma8452_data *data,
+	int val, int val2)
+{
+	return mma8452_get_int_plus_micros_index(mma8452_samp_freq,
+		ARRAY_SIZE(mma8452_samp_freq), val, val2);
+}
+
+static int mma8452_get_scale_index(struct mma8452_data *data,
+	int val, int val2)
+{
+	return mma8452_get_int_plus_micros_index(mma8452_scales,
+		ARRAY_SIZE(mma8452_scales), val, val2);
+}
+
+static int mma8452_read_raw(struct iio_dev *indio_dev,
+			    struct iio_chan_spec const *chan,
+			    int *val, int *val2, long mask)
+{
+	struct mma8452_data *data = iio_priv(indio_dev);
+	__be16 buffer[3];
+	int i, ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		if (iio_buffer_enabled(indio_dev))
+			return -EBUSY;
+
+		mutex_lock(&data->lock);
+		ret = mma8452_read(data, buffer);
+		mutex_unlock(&data->lock);
+		if (ret < 0)
+			return ret;
+		*val = sign_extend32(
+			be16_to_cpu(buffer[chan->scan_index]) >> 4, 11);
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		i = data->data_cfg & MMA8452_DATA_CFG_FS_MASK;
+		*val = mma8452_scales[i][0];
+		*val2 = mma8452_scales[i][1];
+		return IIO_VAL_INT_PLUS_MICRO;
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		i = (data->ctrl_reg1 & MMA8452_CTRL_DR_MASK) >>
+			MMA8452_CTRL_DR_SHIFT;
+		*val = mma8452_samp_freq[i][0];
+		*val2 = mma8452_samp_freq[i][1];
+		return IIO_VAL_INT_PLUS_MICRO;
+	case IIO_CHAN_INFO_CALIBBIAS:
+		ret = i2c_smbus_read_byte_data(data->client, MMA8452_OFF_X +
+			chan->scan_index);
+		if (ret < 0)
+			return ret;
+		*val = sign_extend32(ret, 7);
+		return IIO_VAL_INT;
+	}
+	return -EINVAL;
+}
+
+static int mma8452_standby(struct mma8452_data *data)
+{
+	return i2c_smbus_write_byte_data(data->client, MMA8452_CTRL_REG1,
+		data->ctrl_reg1 & ~MMA8452_CTRL_ACTIVE);
+}
+
+static int mma8452_active(struct mma8452_data *data)
+{
+	return i2c_smbus_write_byte_data(data->client, MMA8452_CTRL_REG1,
+		data->ctrl_reg1);
+}
+
+static int mma8452_change_config(struct mma8452_data *data, u8 reg, u8 val)
+{
+	int ret;
+
+	mutex_lock(&data->lock);
+
+	/* config can only be changed when in standby */
+	ret = mma8452_standby(data);
+	if (ret < 0)
+		goto fail;
+
+	ret = i2c_smbus_write_byte_data(data->client, reg, val);
+	if (ret < 0)
+		goto fail;
+
+	ret = mma8452_active(data);
+	if (ret < 0)
+		goto fail;
+
+	ret = 0;
+fail:
+	mutex_unlock(&data->lock);
+	return ret;
+}
+
+static int mma8452_write_raw(struct iio_dev *indio_dev,
+			     struct iio_chan_spec const *chan,
+			     int val, int val2, long mask)
+{
+	struct mma8452_data *data = iio_priv(indio_dev);
+	int i;
+
+	if (iio_buffer_enabled(indio_dev))
+		return -EBUSY;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		i = mma8452_get_samp_freq_index(data, val, val2);
+		if (i < 0)
+			return -EINVAL;
+
+		data->ctrl_reg1 &= ~MMA8452_CTRL_DR_MASK;
+		data->ctrl_reg1 |= i << MMA8452_CTRL_DR_SHIFT;
+		return mma8452_change_config(data, MMA8452_CTRL_REG1,
+			data->ctrl_reg1);
+	case IIO_CHAN_INFO_SCALE:
+		i = mma8452_get_scale_index(data, val, val2);
+		if (i < 0)
+			return -EINVAL;
+		data->data_cfg &= ~MMA8452_DATA_CFG_FS_MASK;
+		data->data_cfg |= i;
+		return mma8452_change_config(data, MMA8452_DATA_CFG,
+			data->data_cfg);
+	case IIO_CHAN_INFO_CALIBBIAS:
+		if (val < -128 || val > 127)
+			return -EINVAL;
+		return mma8452_change_config(data, MMA8452_OFF_X +
+			chan->scan_index, val);
+	default:
+		return -EINVAL;
+	}
+}
+
+static irqreturn_t mma8452_trigger_handler(int irq, void *p)
+{
+	struct iio_poll_func *pf = p;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct mma8452_data *data = iio_priv(indio_dev);
+	u8 buffer[16]; /* 3 16-bit channels + padding + ts */
+	int ret;
+
+	ret = mma8452_read(data, (__be16 *) buffer);
+	if (ret < 0)
+		goto done;
+
+	iio_push_to_buffers_with_timestamp(indio_dev, buffer,
+		iio_get_time_ns());
+
+done:
+	iio_trigger_notify_done(indio_dev->trig);
+	return IRQ_HANDLED;
+}
+
+#define MMA8452_CHANNEL(axis, idx) { \
+	.type = IIO_ACCEL, \
+	.modified = 1, \
+	.channel2 = IIO_MOD_##axis, \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+		BIT(IIO_CHAN_INFO_CALIBBIAS), \
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SAMP_FREQ) | \
+		BIT(IIO_CHAN_INFO_SCALE), \
+	.scan_index = idx, \
+	.scan_type = { \
+		.sign = 's', \
+		.realbits = 12, \
+		.storagebits = 16, \
+		.shift = 4, \
+		.endianness = IIO_BE, \
+	}, \
+}
+
+static const struct iio_chan_spec mma8452_channels[] = {
+	MMA8452_CHANNEL(X, 0),
+	MMA8452_CHANNEL(Y, 1),
+	MMA8452_CHANNEL(Z, 2),
+	IIO_CHAN_SOFT_TIMESTAMP(3),
+};
+
+static struct attribute *mma8452_attributes[] = {
+	&iio_dev_attr_sampling_frequency_available.dev_attr.attr,
+	&iio_dev_attr_in_accel_scale_available.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group mma8452_group = {
+	.attrs = mma8452_attributes,
+};
+
+static const struct iio_info mma8452_info = {
+	.attrs = &mma8452_group,
+	.read_raw = &mma8452_read_raw,
+	.write_raw = &mma8452_write_raw,
+	.driver_module = THIS_MODULE,
+};
+
+static const unsigned long mma8452_scan_masks[] = {0x7, 0};
+
+static int mma8452_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct mma8452_data *data;
+	struct iio_dev *indio_dev;
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(client, MMA8452_WHO_AM_I);
+	if (ret < 0)
+		return ret;
+	if (ret != MMA8452_DEVICE_ID)
+		return -ENODEV;
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	data = iio_priv(indio_dev);
+	data->client = client;
+	mutex_init(&data->lock);
+
+	i2c_set_clientdata(client, indio_dev);
+	indio_dev->info = &mma8452_info;
+	indio_dev->name = id->name;
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = mma8452_channels;
+	indio_dev->num_channels = ARRAY_SIZE(mma8452_channels);
+	indio_dev->available_scan_masks = mma8452_scan_masks;
+
+	data->ctrl_reg1 = MMA8452_CTRL_ACTIVE |
+		(MMA8452_CTRL_DR_DEFAULT << MMA8452_CTRL_DR_SHIFT);
+	ret = i2c_smbus_write_byte_data(client, MMA8452_CTRL_REG1,
+		data->ctrl_reg1);
+	if (ret < 0)
+		return ret;
+
+	data->data_cfg = MMA8452_DATA_CFG_FS_2G;
+	ret = i2c_smbus_write_byte_data(client, MMA8452_DATA_CFG,
+		data->data_cfg);
+	if (ret < 0)
+		return ret;
+
+	ret = iio_triggered_buffer_setup(indio_dev, NULL,
+		mma8452_trigger_handler, NULL);
+	if (ret < 0)
+		return ret;
+
+	ret = iio_device_register(indio_dev);
+	if (ret < 0)
+		goto buffer_cleanup;
+	return 0;
+
+buffer_cleanup:
+	iio_triggered_buffer_cleanup(indio_dev);
+	return ret;
+}
+
+static int mma8452_remove(struct i2c_client *client)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+
+	iio_device_unregister(indio_dev);
+	iio_triggered_buffer_cleanup(indio_dev);
+	mma8452_standby(iio_priv(indio_dev));
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int mma8452_suspend(struct device *dev)
+{
+	return mma8452_standby(iio_priv(i2c_get_clientdata(
+		to_i2c_client(dev))));
+}
+
+static int mma8452_resume(struct device *dev)
+{
+	return mma8452_active(iio_priv(i2c_get_clientdata(
+		to_i2c_client(dev))));
+}
+
+static SIMPLE_DEV_PM_OPS(mma8452_pm_ops, mma8452_suspend, mma8452_resume);
+#define MMA8452_PM_OPS (&mma8452_pm_ops)
+#else
+#define MMA8452_PM_OPS NULL
+#endif
+
+static const struct i2c_device_id mma8452_id[] = {
+	{ "mma8452", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, mma8452_id);
+
+static struct i2c_driver mma8452_driver = {
+	.driver = {
+		.name	= "mma8452",
+		.pm	= MMA8452_PM_OPS,
+	},
+	.probe = mma8452_probe,
+	.remove = mma8452_remove,
+	.id_table = mma8452_id,
+};
+module_i2c_driver(mma8452_driver);
+
+MODULE_AUTHOR("Peter Meerwald <pmeerw@pmeerw.net>");
+MODULE_DESCRIPTION("Freescale MMA8452 accelerometer driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/iio/accel/st_accel_core.c b/drivers/iio/accel/st_accel_core.c
index 38caedc76b9..a2abf7c2ce3 100644
--- a/drivers/iio/accel/st_accel_core.c
+++ b/drivers/iio/accel/st_accel_core.c
@@ -459,6 +459,8 @@ int st_accel_common_probe(struct iio_dev *indio_dev,
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->info = &accel_info;
 
+	st_sensors_power_enable(indio_dev);
+
 	err = st_sensors_check_device_support(indio_dev,
 				ARRAY_SIZE(st_accel_sensors), st_accel_sensors);
 	if (err < 0)
@@ -496,6 +498,9 @@ int st_accel_common_probe(struct iio_dev *indio_dev,
 	if (err)
 		goto st_accel_device_register_error;
 
+	dev_info(&indio_dev->dev, "registered accelerometer %s\n",
+		 indio_dev->name);
+
 	return 0;
 
 st_accel_device_register_error:
@@ -512,6 +517,8 @@ void st_accel_common_remove(struct iio_dev *indio_dev)
 {
 	struct st_sensor_data *adata = iio_priv(indio_dev);
 
+	st_sensors_power_disable(indio_dev);
+
 	iio_device_unregister(indio_dev);
 	if (adata->get_irq_data_ready(indio_dev) > 0)
 		st_sensors_deallocate_trigger(indio_dev);
diff --git a/drivers/iio/adc/Kconfig b/drivers/iio/adc/Kconfig
index d86196cfe4b..a80d23628f1 100644
--- a/drivers/iio/adc/Kconfig
+++ b/drivers/iio/adc/Kconfig
@@ -96,9 +96,21 @@ config AD7923
 	  To compile this driver as a module, choose M here: the
 	  module will be called ad7923.
 
+config AD799X
+	tristate "Analog Devices AD799x ADC driver"
+	depends on I2C
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	help
+	  Say yes here to build support for Analog Devices:
+	  ad7991, ad7995, ad7999, ad7992, ad7993, ad7994, ad7997, ad7998
+	  i2c analog to digital converters (ADC). Provides direct access
+	  via sysfs.
+
 config AT91_ADC
 	tristate "Atmel AT91 ADC"
 	depends on ARCH_AT91
+	depends on INPUT
 	select IIO_BUFFER
 	select IIO_TRIGGERED_BUFFER
 	select SYSFS
@@ -106,15 +118,15 @@ config AT91_ADC
 	  Say yes here to build support for Atmel AT91 ADC.
 
 config EXYNOS_ADC
-	bool "Exynos ADC driver support"
-	depends on OF
+	tristate "Exynos ADC driver support"
+	depends on ARCH_EXYNOS || (OF && COMPILE_TEST)
 	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"
+	tristate "LP8788 ADC driver"
 	depends on MFD_LP8788
 	help
 	  Say yes here to build support for TI LP8788 ADC.
@@ -146,11 +158,12 @@ config MCP320X
 	  called mcp320x.
 
 config MCP3422
-	tristate "Microchip Technology MCP3422/3/4 driver"
+	tristate "Microchip Technology MCP3422/3/4/6/7/8 driver"
 	depends on I2C
 	help
-	  Say yes here to build support for Microchip Technology's MCP3422,
-	  MCP3423 or MCP3424 analog to digital converters.
+	  Say yes here to build support for Microchip Technology's
+	  MCP3422, MCP3423, MCP3424, MCP3426, MCP3427 or MCP3428
+	  analog to digital converters.
 
 	  This driver can also be built as a module. If so, the module will be
 	  called mcp3422.
diff --git a/drivers/iio/adc/Makefile b/drivers/iio/adc/Makefile
index ab346d88c68..9d60f2deaaa 100644
--- a/drivers/iio/adc/Makefile
+++ b/drivers/iio/adc/Makefile
@@ -11,6 +11,7 @@ obj-$(CONFIG_AD7476) += ad7476.o
 obj-$(CONFIG_AD7791) += ad7791.o
 obj-$(CONFIG_AD7793) += ad7793.o
 obj-$(CONFIG_AD7887) += ad7887.o
+obj-$(CONFIG_AD799X) += ad799x.o
 obj-$(CONFIG_AT91_ADC) += at91_adc.o
 obj-$(CONFIG_EXYNOS_ADC) += exynos_adc.o
 obj-$(CONFIG_LP8788_ADC) += lp8788_adc.o
diff --git a/drivers/staging/iio/adc/ad799x_core.c b/drivers/iio/adc/ad799x.c
index 979ec77d6c2..39b4cb48d73 100644
--- a/drivers/staging/iio/adc/ad799x_core.c
+++ b/drivers/iio/adc/ad799x.c
@@ -37,8 +37,144 @@
 #include <linux/iio/sysfs.h>
 #include <linux/iio/events.h>
 #include <linux/iio/buffer.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
 
-#include "ad799x.h"
+#define AD799X_CHANNEL_SHIFT			4
+#define AD799X_STORAGEBITS			16
+/*
+ * AD7991, AD7995 and AD7999 defines
+ */
+
+#define AD7991_REF_SEL				0x08
+#define AD7991_FLTR				0x04
+#define AD7991_BIT_TRIAL_DELAY			0x02
+#define AD7991_SAMPLE_DELAY			0x01
+
+/*
+ * AD7992, AD7993, AD7994, AD7997 and AD7998 defines
+ */
+
+#define AD7998_FLTR				0x08
+#define AD7998_ALERT_EN				0x04
+#define AD7998_BUSY_ALERT			0x02
+#define AD7998_BUSY_ALERT_POL			0x01
+
+#define AD7998_CONV_RES_REG			0x0
+#define AD7998_ALERT_STAT_REG			0x1
+#define AD7998_CONF_REG				0x2
+#define AD7998_CYCLE_TMR_REG			0x3
+
+#define AD7998_DATALOW_REG(x)			((x) * 3 + 0x4)
+#define AD7998_DATAHIGH_REG(x)			((x) * 3 + 0x5)
+#define AD7998_HYST_REG(x)			((x) * 3 + 0x6)
+
+#define AD7998_CYC_MASK				0x7
+#define AD7998_CYC_DIS				0x0
+#define AD7998_CYC_TCONF_32			0x1
+#define AD7998_CYC_TCONF_64			0x2
+#define AD7998_CYC_TCONF_128			0x3
+#define AD7998_CYC_TCONF_256			0x4
+#define AD7998_CYC_TCONF_512			0x5
+#define AD7998_CYC_TCONF_1024			0x6
+#define AD7998_CYC_TCONF_2048			0x7
+
+#define AD7998_ALERT_STAT_CLEAR			0xFF
+
+/*
+ * AD7997 and AD7997 defines
+ */
+
+#define AD7997_8_READ_SINGLE			0x80
+#define AD7997_8_READ_SEQUENCE			0x70
+/* TODO: move this into a common header */
+#define RES_MASK(bits)	((1 << (bits)) - 1)
+
+enum {
+	ad7991,
+	ad7995,
+	ad7999,
+	ad7992,
+	ad7993,
+	ad7994,
+	ad7997,
+	ad7998
+};
+
+/**
+ * struct ad799x_chip_info - chip specific information
+ * @channel:		channel specification
+ * @num_channels:	number of channels
+ * @monitor_mode:	whether the chip supports monitor interrupts
+ * @default_config:	device default configuration
+ * @event_attrs:	pointer to the monitor event attribute group
+ */
+struct ad799x_chip_info {
+	struct iio_chan_spec		channel[9];
+	int				num_channels;
+	u16				default_config;
+	const struct iio_info		*info;
+};
+
+struct ad799x_state {
+	struct i2c_client		*client;
+	const struct ad799x_chip_info	*chip_info;
+	struct regulator		*reg;
+	struct regulator		*vref;
+	unsigned			id;
+	u16				config;
+
+	u8				*rx_buf;
+	unsigned int			transfer_size;
+};
+
+/**
+ * ad799x_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 ad799x_trigger_handler(int irq, void *p)
+{
+	struct iio_poll_func *pf = p;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct ad799x_state *st = iio_priv(indio_dev);
+	int b_sent;
+	u8 cmd;
+
+	switch (st->id) {
+	case ad7991:
+	case ad7995:
+	case ad7999:
+		cmd = st->config |
+			(*indio_dev->active_scan_mask << AD799X_CHANNEL_SHIFT);
+		break;
+	case ad7992:
+	case ad7993:
+	case ad7994:
+		cmd = (*indio_dev->active_scan_mask << AD799X_CHANNEL_SHIFT) |
+			AD7998_CONV_RES_REG;
+		break;
+	case ad7997:
+	case ad7998:
+		cmd = AD7997_8_READ_SEQUENCE | AD7998_CONV_RES_REG;
+		break;
+	default:
+		cmd = 0;
+	}
+
+	b_sent = i2c_smbus_read_i2c_block_data(st->client,
+			cmd, st->transfer_size, st->rx_buf);
+	if (b_sent < 0)
+		goto out;
+
+	iio_push_to_buffers_with_timestamp(indio_dev, st->rx_buf,
+			iio_get_time_ns());
+out:
+	iio_trigger_notify_done(indio_dev->trig);
+
+	return IRQ_HANDLED;
+}
 
 /*
  * ad799x register access by I2C
@@ -578,9 +714,10 @@ static int ad799x_probe(struct i2c_client *client,
 	indio_dev->channels = st->chip_info->channel;
 	indio_dev->num_channels = st->chip_info->num_channels;
 
-	ret = ad799x_register_ring_funcs_and_init(indio_dev);
+	ret = iio_triggered_buffer_setup(indio_dev, NULL,
+		&ad799x_trigger_handler, NULL);
 	if (ret)
-		goto error_disable_reg;
+		goto error_disable_vref;
 
 	if (client->irq > 0) {
 		ret = devm_request_threaded_irq(&client->dev,
@@ -601,12 +738,11 @@ static int ad799x_probe(struct i2c_client *client,
 	return 0;
 
 error_cleanup_ring:
-	ad799x_ring_cleanup(indio_dev);
+	iio_triggered_buffer_cleanup(indio_dev);
+error_disable_vref:
+	regulator_disable(st->vref);
 error_disable_reg:
-	if (!IS_ERR(st->vref))
-		regulator_disable(st->vref);
-	if (!IS_ERR(st->reg))
-		regulator_disable(st->reg);
+	regulator_disable(st->reg);
 
 	return ret;
 }
@@ -618,11 +754,9 @@ static int ad799x_remove(struct i2c_client *client)
 
 	iio_device_unregister(indio_dev);
 
-	ad799x_ring_cleanup(indio_dev);
-	if (!IS_ERR(st->vref))
-		regulator_disable(st->vref);
-	if (!IS_ERR(st->reg))
-		regulator_disable(st->reg);
+	iio_triggered_buffer_cleanup(indio_dev);
+	regulator_disable(st->vref);
+	regulator_disable(st->reg);
 	kfree(st->rx_buf);
 
 	return 0;
diff --git a/drivers/iio/adc/at91_adc.c b/drivers/iio/adc/at91_adc.c
index 5b1aa027c03..3b5bacd4d8d 100644
--- a/drivers/iio/adc/at91_adc.c
+++ b/drivers/iio/adc/at91_adc.c
@@ -31,7 +31,108 @@
 #include <linux/iio/trigger_consumer.h>
 #include <linux/iio/triggered_buffer.h>
 
-#include <mach/at91_adc.h>
+/* Registers */
+#define AT91_ADC_CR		0x00		/* Control Register */
+#define		AT91_ADC_SWRST		(1 << 0)	/* Software Reset */
+#define		AT91_ADC_START		(1 << 1)	/* Start Conversion */
+
+#define AT91_ADC_MR		0x04		/* Mode Register */
+#define		AT91_ADC_TSAMOD		(3 << 0)	/* ADC mode */
+#define		AT91_ADC_TSAMOD_ADC_ONLY_MODE		(0 << 0)	/* ADC Mode */
+#define		AT91_ADC_TSAMOD_TS_ONLY_MODE		(1 << 0)	/* Touch Screen Only Mode */
+#define		AT91_ADC_TRGEN		(1 << 0)	/* Trigger Enable */
+#define		AT91_ADC_TRGSEL		(7 << 1)	/* Trigger Selection */
+#define			AT91_ADC_TRGSEL_TC0		(0 << 1)
+#define			AT91_ADC_TRGSEL_TC1		(1 << 1)
+#define			AT91_ADC_TRGSEL_TC2		(2 << 1)
+#define			AT91_ADC_TRGSEL_EXTERNAL	(6 << 1)
+#define		AT91_ADC_LOWRES		(1 << 4)	/* Low Resolution */
+#define		AT91_ADC_SLEEP		(1 << 5)	/* Sleep Mode */
+#define		AT91_ADC_PENDET		(1 << 6)	/* Pen contact detection enable */
+#define		AT91_ADC_PRESCAL_9260	(0x3f << 8)	/* Prescalar Rate Selection */
+#define		AT91_ADC_PRESCAL_9G45	(0xff << 8)
+#define			AT91_ADC_PRESCAL_(x)	((x) << 8)
+#define		AT91_ADC_STARTUP_9260	(0x1f << 16)	/* Startup Up Time */
+#define		AT91_ADC_STARTUP_9G45	(0x7f << 16)
+#define		AT91_ADC_STARTUP_9X5	(0xf << 16)
+#define			AT91_ADC_STARTUP_(x)	((x) << 16)
+#define		AT91_ADC_SHTIM		(0xf  << 24)	/* Sample & Hold Time */
+#define			AT91_ADC_SHTIM_(x)	((x) << 24)
+#define		AT91_ADC_PENDBC		(0x0f << 28)	/* Pen Debounce time */
+#define			AT91_ADC_PENDBC_(x)	((x) << 28)
+
+#define AT91_ADC_TSR		0x0C
+#define		AT91_ADC_TSR_SHTIM	(0xf  << 24)	/* Sample & Hold Time */
+#define			AT91_ADC_TSR_SHTIM_(x)	((x) << 24)
+
+#define AT91_ADC_CHER		0x10		/* Channel Enable Register */
+#define AT91_ADC_CHDR		0x14		/* Channel Disable Register */
+#define AT91_ADC_CHSR		0x18		/* Channel Status Register */
+#define		AT91_ADC_CH(n)		(1 << (n))	/* Channel Number */
+
+#define AT91_ADC_SR		0x1C		/* Status Register */
+#define		AT91_ADC_EOC(n)		(1 << (n))	/* End of Conversion on Channel N */
+#define		AT91_ADC_OVRE(n)	(1 << ((n) + 8))/* Overrun Error on Channel N */
+#define		AT91_ADC_DRDY		(1 << 16)	/* Data Ready */
+#define		AT91_ADC_GOVRE		(1 << 17)	/* General Overrun Error */
+#define		AT91_ADC_ENDRX		(1 << 18)	/* End of RX Buffer */
+#define		AT91_ADC_RXFUFF		(1 << 19)	/* RX Buffer Full */
+
+#define AT91_ADC_SR_9X5		0x30		/* Status Register for 9x5 */
+#define		AT91_ADC_SR_DRDY_9X5	(1 << 24)	/* Data Ready */
+
+#define AT91_ADC_LCDR		0x20		/* Last Converted Data Register */
+#define		AT91_ADC_LDATA		(0x3ff)
+
+#define AT91_ADC_IER		0x24		/* Interrupt Enable Register */
+#define AT91_ADC_IDR		0x28		/* Interrupt Disable Register */
+#define AT91_ADC_IMR		0x2C		/* Interrupt Mask Register */
+#define		AT91RL_ADC_IER_PEN	(1 << 20)
+#define		AT91RL_ADC_IER_NOPEN	(1 << 21)
+#define		AT91_ADC_IER_PEN	(1 << 29)
+#define		AT91_ADC_IER_NOPEN	(1 << 30)
+#define		AT91_ADC_IER_XRDY	(1 << 20)
+#define		AT91_ADC_IER_YRDY	(1 << 21)
+#define		AT91_ADC_IER_PRDY	(1 << 22)
+#define		AT91_ADC_ISR_PENS	(1 << 31)
+
+#define AT91_ADC_CHR(n)		(0x30 + ((n) * 4))	/* Channel Data Register N */
+#define		AT91_ADC_DATA		(0x3ff)
+
+#define AT91_ADC_CDR0_9X5	(0x50)			/* Channel Data Register 0 for 9X5 */
+
+#define AT91_ADC_ACR		0x94	/* Analog Control Register */
+#define		AT91_ADC_ACR_PENDETSENS	(0x3 << 0)	/* pull-up resistor */
+
+#define AT91_ADC_TSMR		0xB0
+#define		AT91_ADC_TSMR_TSMODE	(3 << 0)	/* Touch Screen Mode */
+#define			AT91_ADC_TSMR_TSMODE_NONE		(0 << 0)
+#define			AT91_ADC_TSMR_TSMODE_4WIRE_NO_PRESS	(1 << 0)
+#define			AT91_ADC_TSMR_TSMODE_4WIRE_PRESS	(2 << 0)
+#define			AT91_ADC_TSMR_TSMODE_5WIRE		(3 << 0)
+#define		AT91_ADC_TSMR_TSAV	(3 << 4)	/* Averages samples */
+#define			AT91_ADC_TSMR_TSAV_(x)		((x) << 4)
+#define		AT91_ADC_TSMR_SCTIM	(0x0f << 16)	/* Switch closure time */
+#define		AT91_ADC_TSMR_PENDBC	(0x0f << 28)	/* Pen Debounce time */
+#define			AT91_ADC_TSMR_PENDBC_(x)	((x) << 28)
+#define		AT91_ADC_TSMR_NOTSDMA	(1 << 22)	/* No Touchscreen DMA */
+#define		AT91_ADC_TSMR_PENDET_DIS	(0 << 24)	/* Pen contact detection disable */
+#define		AT91_ADC_TSMR_PENDET_ENA	(1 << 24)	/* Pen contact detection enable */
+
+#define AT91_ADC_TSXPOSR	0xB4
+#define AT91_ADC_TSYPOSR	0xB8
+#define AT91_ADC_TSPRESSR	0xBC
+
+#define AT91_ADC_TRGR_9260	AT91_ADC_MR
+#define AT91_ADC_TRGR_9G45	0x08
+#define AT91_ADC_TRGR_9X5	0xC0
+
+/* Trigger Register bit field */
+#define		AT91_ADC_TRGR_TRGPER	(0xffff << 16)
+#define			AT91_ADC_TRGR_TRGPER_(x)	((x) << 16)
+#define		AT91_ADC_TRGR_TRGMOD	(0x7 << 0)
+#define			AT91_ADC_TRGR_NONE		(0 << 0)
+#define			AT91_ADC_TRGR_MOD_PERIOD_TRIG	(5 << 0)
 
 #define AT91_ADC_CHAN(st, ch) \
 	(st->registers->channel_base + (ch * 4))
@@ -46,6 +147,29 @@
 #define TOUCH_SAMPLE_PERIOD_US		2000	/* 2ms */
 #define TOUCH_PEN_DETECT_DEBOUNCE_US	200
 
+#define MAX_RLPOS_BITS         10
+#define TOUCH_SAMPLE_PERIOD_US_RL      10000   /* 10ms, the SoC can't keep up with 2ms */
+#define TOUCH_SHTIM                    0xa
+
+/**
+ * struct at91_adc_reg_desc - Various informations relative to registers
+ * @channel_base:	Base offset for the channel data registers
+ * @drdy_mask:		Mask of the DRDY field in the relevant registers
+			(Interruptions registers mostly)
+ * @status_register:	Offset of the Interrupt Status Register
+ * @trigger_register:	Offset of the Trigger setup register
+ * @mr_prescal_mask:	Mask of the PRESCAL field in the adc MR register
+ * @mr_startup_mask:	Mask of the STARTUP field in the adc MR register
+ */
+struct at91_adc_reg_desc {
+	u8	channel_base;
+	u32	drdy_mask;
+	u8	status_register;
+	u8	trigger_register;
+	u32	mr_prescal_mask;
+	u32	mr_startup_mask;
+};
+
 struct at91_adc_caps {
 	bool	has_ts;		/* Support touch screen */
 	bool	has_tsmr;	/* only at91sam9x5, sama5d3 have TSMR reg */
@@ -64,12 +188,6 @@ struct at91_adc_caps {
 	struct at91_adc_reg_desc registers;
 };
 
-enum atmel_adc_ts_type {
-	ATMEL_ADC_TOUCHSCREEN_NONE = 0,
-	ATMEL_ADC_TOUCHSCREEN_4WIRE = 4,
-	ATMEL_ADC_TOUCHSCREEN_5WIRE = 5,
-};
-
 struct at91_adc_state {
 	struct clk		*adc_clk;
 	u16			*buffer;
@@ -114,6 +232,11 @@ struct at91_adc_state {
 
 	u16			ts_sample_period_val;
 	u32			ts_pressure_threshold;
+	u16			ts_pendbc;
+
+	bool			ts_bufferedmeasure;
+	u32			ts_prev_absx;
+	u32			ts_prev_absy;
 };
 
 static irqreturn_t at91_adc_trigger_handler(int irq, void *p)
@@ -220,7 +343,72 @@ static int at91_ts_sample(struct at91_adc_state *st)
 	return 0;
 }
 
-static irqreturn_t at91_adc_interrupt(int irq, void *private)
+static irqreturn_t at91_adc_rl_interrupt(int irq, void *private)
+{
+	struct iio_dev *idev = private;
+	struct at91_adc_state *st = iio_priv(idev);
+	u32 status = at91_adc_readl(st, st->registers->status_register);
+	unsigned int reg;
+
+	status &= at91_adc_readl(st, AT91_ADC_IMR);
+	if (status & st->registers->drdy_mask)
+		handle_adc_eoc_trigger(irq, idev);
+
+	if (status & AT91RL_ADC_IER_PEN) {
+		/* Disabling pen debounce is required to get a NOPEN irq */
+		reg = at91_adc_readl(st, AT91_ADC_MR);
+		reg &= ~AT91_ADC_PENDBC;
+		at91_adc_writel(st, AT91_ADC_MR, reg);
+
+		at91_adc_writel(st, AT91_ADC_IDR, AT91RL_ADC_IER_PEN);
+		at91_adc_writel(st, AT91_ADC_IER, AT91RL_ADC_IER_NOPEN
+				| AT91_ADC_EOC(3));
+		/* Set up period trigger for sampling */
+		at91_adc_writel(st, st->registers->trigger_register,
+			AT91_ADC_TRGR_MOD_PERIOD_TRIG |
+			AT91_ADC_TRGR_TRGPER_(st->ts_sample_period_val));
+	} else if (status & AT91RL_ADC_IER_NOPEN) {
+		reg = at91_adc_readl(st, AT91_ADC_MR);
+		reg |= AT91_ADC_PENDBC_(st->ts_pendbc) & AT91_ADC_PENDBC;
+		at91_adc_writel(st, AT91_ADC_MR, reg);
+		at91_adc_writel(st, st->registers->trigger_register,
+			AT91_ADC_TRGR_NONE);
+
+		at91_adc_writel(st, AT91_ADC_IDR, AT91RL_ADC_IER_NOPEN
+				| AT91_ADC_EOC(3));
+		at91_adc_writel(st, AT91_ADC_IER, AT91RL_ADC_IER_PEN);
+		st->ts_bufferedmeasure = false;
+		input_report_key(st->ts_input, BTN_TOUCH, 0);
+		input_sync(st->ts_input);
+	} else if (status & AT91_ADC_EOC(3)) {
+		/* Conversion finished */
+		if (st->ts_bufferedmeasure) {
+			/*
+			 * Last measurement is always discarded, since it can
+			 * be erroneous.
+			 * Always report previous measurement
+			 */
+			input_report_abs(st->ts_input, ABS_X, st->ts_prev_absx);
+			input_report_abs(st->ts_input, ABS_Y, st->ts_prev_absy);
+			input_report_key(st->ts_input, BTN_TOUCH, 1);
+			input_sync(st->ts_input);
+		} else
+			st->ts_bufferedmeasure = true;
+
+		/* Now make new measurement */
+		st->ts_prev_absx = at91_adc_readl(st, AT91_ADC_CHAN(st, 3))
+				   << MAX_RLPOS_BITS;
+		st->ts_prev_absx /= at91_adc_readl(st, AT91_ADC_CHAN(st, 2));
+
+		st->ts_prev_absy = at91_adc_readl(st, AT91_ADC_CHAN(st, 1))
+				   << MAX_RLPOS_BITS;
+		st->ts_prev_absy /= at91_adc_readl(st, AT91_ADC_CHAN(st, 0));
+	}
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t at91_adc_9x5_interrupt(int irq, void *private)
 {
 	struct iio_dev *idev = private;
 	struct at91_adc_state *st = iio_priv(idev);
@@ -653,6 +841,8 @@ static int at91_adc_probe_dt_ts(struct device_node *node,
 		return -EINVAL;
 	}
 
+	if (!st->caps->has_tsmr)
+		return 0;
 	prop = 0;
 	of_property_read_u32(node, "atmel,adc-ts-pressure-threshold", &prop);
 	st->ts_pressure_threshold = prop;
@@ -765,14 +955,18 @@ static int at91_adc_probe_pdata(struct at91_adc_state *st,
 	if (!pdata)
 		return -EINVAL;
 
+	st->caps = (struct at91_adc_caps *)
+			platform_get_device_id(pdev)->driver_data;
+
 	st->use_external = pdata->use_external_triggers;
 	st->vref_mv = pdata->vref;
 	st->channels_mask = pdata->channels_used;
-	st->num_channels = pdata->num_channels;
+	st->num_channels = st->caps->num_channels;
 	st->startup_time = pdata->startup_time;
 	st->trigger_number = pdata->trigger_number;
 	st->trigger_list = pdata->trigger_list;
-	st->registers = pdata->registers;
+	st->registers = &st->caps->registers;
+	st->touchscreen_type = pdata->touchscreen_type;
 
 	return 0;
 }
@@ -787,7 +981,10 @@ static int atmel_ts_open(struct input_dev *dev)
 {
 	struct at91_adc_state *st = input_get_drvdata(dev);
 
-	at91_adc_writel(st, AT91_ADC_IER, AT91_ADC_IER_PEN);
+	if (st->caps->has_tsmr)
+		at91_adc_writel(st, AT91_ADC_IER, AT91_ADC_IER_PEN);
+	else
+		at91_adc_writel(st, AT91_ADC_IER, AT91RL_ADC_IER_PEN);
 	return 0;
 }
 
@@ -795,45 +992,61 @@ static void atmel_ts_close(struct input_dev *dev)
 {
 	struct at91_adc_state *st = input_get_drvdata(dev);
 
-	at91_adc_writel(st, AT91_ADC_IDR, AT91_ADC_IER_PEN);
+	if (st->caps->has_tsmr)
+		at91_adc_writel(st, AT91_ADC_IDR, AT91_ADC_IER_PEN);
+	else
+		at91_adc_writel(st, AT91_ADC_IDR, AT91RL_ADC_IER_PEN);
 }
 
 static int at91_ts_hw_init(struct at91_adc_state *st, u32 adc_clk_khz)
 {
-	u32 reg = 0, pendbc;
+	u32 reg = 0;
 	int i = 0;
 
-	if (st->touchscreen_type == ATMEL_ADC_TOUCHSCREEN_4WIRE)
-		reg = AT91_ADC_TSMR_TSMODE_4WIRE_PRESS;
-	else
-		reg = AT91_ADC_TSMR_TSMODE_5WIRE;
-
 	/* a Pen Detect Debounce Time is necessary for the ADC Touch to avoid
 	 * pen detect noise.
 	 * The formula is : Pen Detect Debounce Time = (2 ^ pendbc) / ADCClock
 	 */
-	pendbc = round_up(TOUCH_PEN_DETECT_DEBOUNCE_US * adc_clk_khz / 1000, 1);
+	st->ts_pendbc = round_up(TOUCH_PEN_DETECT_DEBOUNCE_US * adc_clk_khz /
+				 1000, 1);
 
-	while (pendbc >> ++i)
+	while (st->ts_pendbc >> ++i)
 		;	/* Empty! Find the shift offset */
-	if (abs(pendbc - (1 << i)) < abs(pendbc - (1 << (i - 1))))
-		pendbc = i;
+	if (abs(st->ts_pendbc - (1 << i)) < abs(st->ts_pendbc - (1 << (i - 1))))
+		st->ts_pendbc = i;
 	else
-		pendbc = i - 1;
+		st->ts_pendbc = i - 1;
 
-	if (st->caps->has_tsmr) {
-		reg |= AT91_ADC_TSMR_TSAV_(st->caps->ts_filter_average)
-				& AT91_ADC_TSMR_TSAV;
-		reg |= AT91_ADC_TSMR_PENDBC_(pendbc) & AT91_ADC_TSMR_PENDBC;
-		reg |= AT91_ADC_TSMR_NOTSDMA;
-		reg |= AT91_ADC_TSMR_PENDET_ENA;
-		reg |= 0x03 << 8;	/* TSFREQ, need bigger than TSAV */
-
-		at91_adc_writel(st, AT91_ADC_TSMR, reg);
-	} else {
-		/* TODO: for 9g45 which has no TSMR */
+	if (!st->caps->has_tsmr) {
+		reg = at91_adc_readl(st, AT91_ADC_MR);
+		reg |= AT91_ADC_TSAMOD_TS_ONLY_MODE | AT91_ADC_PENDET;
+
+		reg |= AT91_ADC_PENDBC_(st->ts_pendbc) & AT91_ADC_PENDBC;
+		at91_adc_writel(st, AT91_ADC_MR, reg);
+
+		reg = AT91_ADC_TSR_SHTIM_(TOUCH_SHTIM) & AT91_ADC_TSR_SHTIM;
+		at91_adc_writel(st, AT91_ADC_TSR, reg);
+
+		st->ts_sample_period_val = round_up((TOUCH_SAMPLE_PERIOD_US_RL *
+						    adc_clk_khz / 1000) - 1, 1);
+
+		return 0;
 	}
 
+	if (st->touchscreen_type == ATMEL_ADC_TOUCHSCREEN_4WIRE)
+		reg = AT91_ADC_TSMR_TSMODE_4WIRE_PRESS;
+	else
+		reg = AT91_ADC_TSMR_TSMODE_5WIRE;
+
+	reg |= AT91_ADC_TSMR_TSAV_(st->caps->ts_filter_average)
+	       & AT91_ADC_TSMR_TSAV;
+	reg |= AT91_ADC_TSMR_PENDBC_(st->ts_pendbc) & AT91_ADC_TSMR_PENDBC;
+	reg |= AT91_ADC_TSMR_NOTSDMA;
+	reg |= AT91_ADC_TSMR_PENDET_ENA;
+	reg |= 0x03 << 8;	/* TSFREQ, needs to be bigger than TSAV */
+
+	at91_adc_writel(st, AT91_ADC_TSMR, reg);
+
 	/* Change adc internal resistor value for better pen detection,
 	 * default value is 100 kOhm.
 	 * 0 = 200 kOhm, 1 = 150 kOhm, 2 = 100 kOhm, 3 = 50 kOhm
@@ -842,7 +1055,7 @@ static int at91_ts_hw_init(struct at91_adc_state *st, u32 adc_clk_khz)
 	at91_adc_writel(st, AT91_ADC_ACR, st->caps->ts_pen_detect_sensitivity
 			& AT91_ADC_ACR_PENDETSENS);
 
-	/* Sample Peroid Time = (TRGPER + 1) / ADCClock */
+	/* Sample Period Time = (TRGPER + 1) / ADCClock */
 	st->ts_sample_period_val = round_up((TOUCH_SAMPLE_PERIOD_US *
 			adc_clk_khz / 1000) - 1, 1);
 
@@ -871,17 +1084,37 @@ static int at91_ts_register(struct at91_adc_state *st,
 	__set_bit(EV_ABS, input->evbit);
 	__set_bit(EV_KEY, input->evbit);
 	__set_bit(BTN_TOUCH, input->keybit);
-	input_set_abs_params(input, ABS_X, 0, (1 << MAX_POS_BITS) - 1, 0, 0);
-	input_set_abs_params(input, ABS_Y, 0, (1 << MAX_POS_BITS) - 1, 0, 0);
-	input_set_abs_params(input, ABS_PRESSURE, 0, 0xffffff, 0, 0);
+	if (st->caps->has_tsmr) {
+		input_set_abs_params(input, ABS_X, 0, (1 << MAX_POS_BITS) - 1,
+				     0, 0);
+		input_set_abs_params(input, ABS_Y, 0, (1 << MAX_POS_BITS) - 1,
+				     0, 0);
+		input_set_abs_params(input, ABS_PRESSURE, 0, 0xffffff, 0, 0);
+	} else {
+		if (st->touchscreen_type != ATMEL_ADC_TOUCHSCREEN_4WIRE) {
+			dev_err(&pdev->dev,
+				"This touchscreen controller only support 4 wires\n");
+			ret = -EINVAL;
+			goto err;
+		}
+
+		input_set_abs_params(input, ABS_X, 0, (1 << MAX_RLPOS_BITS) - 1,
+				     0, 0);
+		input_set_abs_params(input, ABS_Y, 0, (1 << MAX_RLPOS_BITS) - 1,
+				     0, 0);
+	}
 
 	st->ts_input = input;
 	input_set_drvdata(input, st);
 
 	ret = input_register_device(input);
 	if (ret)
-		input_free_device(st->ts_input);
+		goto err;
+
+	return ret;
 
+err:
+	input_free_device(st->ts_input);
 	return ret;
 }
 
@@ -940,11 +1173,13 @@ static int at91_adc_probe(struct platform_device *pdev)
 	 */
 	at91_adc_writel(st, AT91_ADC_CR, AT91_ADC_SWRST);
 	at91_adc_writel(st, AT91_ADC_IDR, 0xFFFFFFFF);
-	ret = request_irq(st->irq,
-			  at91_adc_interrupt,
-			  0,
-			  pdev->dev.driver->name,
-			  idev);
+
+	if (st->caps->has_tsmr)
+		ret = request_irq(st->irq, at91_adc_9x5_interrupt, 0,
+				  pdev->dev.driver->name, idev);
+	else
+		ret = request_irq(st->irq, at91_adc_rl_interrupt, 0,
+				  pdev->dev.driver->name, idev);
 	if (ret) {
 		dev_err(&pdev->dev, "Failed to allocate IRQ.\n");
 		return ret;
@@ -1004,8 +1239,11 @@ static int at91_adc_probe(struct platform_device *pdev)
 	 * 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_khz /
-			  1000) - 1, 1);
+	if (st->sample_hold_time > 0)
+		shtim = round_up((st->sample_hold_time * adc_clk_khz / 1000)
+				 - 1, 1);
+	else
+		shtim = 0;
 
 	reg = AT91_ADC_PRESCAL_(prsc) & st->registers->mr_prescal_mask;
 	reg |= AT91_ADC_STARTUP_(ticks) & st->registers->mr_startup_mask;
@@ -1045,12 +1283,6 @@ static int at91_adc_probe(struct platform_device *pdev)
 			goto error_disable_adc_clk;
 		}
 	} else {
-		if (!st->caps->has_tsmr) {
-			dev_err(&pdev->dev, "We don't support non-TSMR adc\n");
-			ret = -ENODEV;
-			goto error_disable_adc_clk;
-		}
-
 		ret = at91_ts_register(st, pdev);
 		if (ret)
 			goto error_disable_adc_clk;
@@ -1101,7 +1333,6 @@ static int at91_adc_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_OF
 static struct at91_adc_caps at91sam9260_caps = {
 	.calc_startup_ticks = calc_startup_ticks_9260,
 	.num_channels = 4,
@@ -1115,6 +1346,20 @@ static struct at91_adc_caps at91sam9260_caps = {
 	},
 };
 
+static struct at91_adc_caps at91sam9rl_caps = {
+	.has_ts = true,
+	.calc_startup_ticks = calc_startup_ticks_9260,	/* same as 9260 */
+	.num_channels = 6,
+	.registers = {
+		.channel_base = AT91_ADC_CHR(0),
+		.drdy_mask = AT91_ADC_DRDY,
+		.status_register = AT91_ADC_SR,
+		.trigger_register = AT91_ADC_TRGR_9G45,
+		.mr_prescal_mask = AT91_ADC_PRESCAL_9260,
+		.mr_startup_mask = AT91_ADC_STARTUP_9G45,
+	},
+};
+
 static struct at91_adc_caps at91sam9g45_caps = {
 	.has_ts = true,
 	.calc_startup_ticks = calc_startup_ticks_9260,	/* same as 9260 */
@@ -1149,16 +1394,36 @@ static struct at91_adc_caps at91sam9x5_caps = {
 
 static const struct of_device_id at91_adc_dt_ids[] = {
 	{ .compatible = "atmel,at91sam9260-adc", .data = &at91sam9260_caps },
+	{ .compatible = "atmel,at91sam9rl-adc", .data = &at91sam9rl_caps },
 	{ .compatible = "atmel,at91sam9g45-adc", .data = &at91sam9g45_caps },
 	{ .compatible = "atmel,at91sam9x5-adc", .data = &at91sam9x5_caps },
 	{},
 };
 MODULE_DEVICE_TABLE(of, at91_adc_dt_ids);
-#endif
+
+static const struct platform_device_id at91_adc_ids[] = {
+	{
+		.name = "at91sam9260-adc",
+		.driver_data = (unsigned long)&at91sam9260_caps,
+	}, {
+		.name = "at91sam9rl-adc",
+		.driver_data = (unsigned long)&at91sam9rl_caps,
+	}, {
+		.name = "at91sam9g45-adc",
+		.driver_data = (unsigned long)&at91sam9g45_caps,
+	}, {
+		.name = "at91sam9x5-adc",
+		.driver_data = (unsigned long)&at91sam9x5_caps,
+	}, {
+		/* terminator */
+	}
+};
+MODULE_DEVICE_TABLE(platform, at91_adc_ids);
 
 static struct platform_driver at91_adc_driver = {
 	.probe = at91_adc_probe,
 	.remove = at91_adc_remove,
+	.id_table = at91_adc_ids,
 	.driver = {
 		   .name = DRIVER_NAME,
 		   .of_match_table = of_match_ptr(at91_adc_dt_ids),
diff --git a/drivers/iio/adc/exynos_adc.c b/drivers/iio/adc/exynos_adc.c
index d25b262193a..010578f1d76 100644
--- a/drivers/iio/adc/exynos_adc.c
+++ b/drivers/iio/adc/exynos_adc.c
@@ -82,7 +82,7 @@ enum adc_version {
 #define ADC_CON_EN_START	(1u << 0)
 #define ADC_DATX_MASK		0xFFF
 
-#define EXYNOS_ADC_TIMEOUT	(msecs_to_jiffies(1000))
+#define EXYNOS_ADC_TIMEOUT	(msecs_to_jiffies(100))
 
 struct exynos_adc {
 	void __iomem		*regs;
@@ -112,6 +112,30 @@ static inline unsigned int exynos_adc_get_version(struct platform_device *pdev)
 	return (unsigned int)match->data;
 }
 
+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_read_raw(struct iio_dev *indio_dev,
 				struct iio_chan_spec const *chan,
 				int *val,
@@ -121,11 +145,13 @@ static int exynos_read_raw(struct iio_dev *indio_dev,
 	struct exynos_adc *info = iio_priv(indio_dev);
 	unsigned long timeout;
 	u32 con1, con2;
+	int ret;
 
 	if (mask != IIO_CHAN_INFO_RAW)
 		return -EINVAL;
 
 	mutex_lock(&indio_dev->mlock);
+	reinit_completion(&info->completion);
 
 	/* Select the channel to be used and Trigger conversion */
 	if (info->version == ADC_V2) {
@@ -145,16 +171,21 @@ static int exynos_read_raw(struct iio_dev *indio_dev,
 				ADC_V1_CON(info->regs));
 	}
 
-	timeout = wait_for_completion_interruptible_timeout
+	timeout = wait_for_completion_timeout
 			(&info->completion, EXYNOS_ADC_TIMEOUT);
-	*val = info->value;
+	if (timeout == 0) {
+		dev_warn(&indio_dev->dev, "Conversion timed out! Resetting\n");
+		exynos_adc_hw_init(info);
+		ret = -ETIMEDOUT;
+	} else {
+		*val = info->value;
+		*val2 = 0;
+		ret = IIO_VAL_INT;
+	}
 
 	mutex_unlock(&indio_dev->mlock);
 
-	if (timeout == 0)
-		return -ETIMEDOUT;
-
-	return IIO_VAL_INT;
+	return ret;
 }
 
 static irqreturn_t exynos_adc_isr(int irq, void *dev_id)
@@ -226,30 +257,6 @@ static int exynos_adc_remove_devices(struct device *dev, void *c)
 	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;
@@ -290,32 +297,30 @@ static int exynos_adc_probe(struct platform_device *pdev)
 
 	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);
-		return ret;
-	}
-
-	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;
+		return PTR_ERR(info->clk);
 	}
 
 	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;
+		return PTR_ERR(info->vdd);
 	}
 
+	ret = regulator_enable(info->vdd);
+	if (ret)
+		return ret;
+
+	ret = clk_prepare_enable(info->clk);
+	if (ret)
+		goto err_disable_reg;
+
+	writel(1, info->enable_reg);
+
 	info->version = exynos_adc_get_version(pdev);
 
 	platform_set_drvdata(pdev, indio_dev);
@@ -332,19 +337,21 @@ static int exynos_adc_probe(struct platform_device *pdev)
 	else
 		indio_dev->num_channels = MAX_ADC_V2_CHANNELS;
 
+	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_disable_clk;
+	}
+
 	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);
+	ret = of_platform_populate(np, exynos_adc_match, NULL, &indio_dev->dev);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "failed adding child nodes\n");
 		goto err_of_populate;
@@ -353,14 +360,16 @@ static int exynos_adc_probe(struct platform_device *pdev)
 	return 0;
 
 err_of_populate:
-	device_for_each_child(&pdev->dev, NULL,
+	device_for_each_child(&indio_dev->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_disable_clk:
+	writel(0, info->enable_reg);
+	clk_disable_unprepare(info->clk);
+err_disable_reg:
+	regulator_disable(info->vdd);
 	return ret;
 }
 
@@ -369,13 +378,13 @@ 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,
+	device_for_each_child(&indio_dev->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);
+	writel(0, info->enable_reg);
+	clk_disable_unprepare(info->clk);
+	regulator_disable(info->vdd);
 
 	return 0;
 }
@@ -397,8 +406,8 @@ static int exynos_adc_suspend(struct device *dev)
 		writel(con, ADC_V1_CON(info->regs));
 	}
 
-	clk_disable_unprepare(info->clk);
 	writel(0, info->enable_reg);
+	clk_disable_unprepare(info->clk);
 	regulator_disable(info->vdd);
 
 	return 0;
@@ -414,9 +423,11 @@ static int exynos_adc_resume(struct device *dev)
 	if (ret)
 		return ret;
 
-	writel(1, info->enable_reg);
-	clk_prepare_enable(info->clk);
+	ret = clk_prepare_enable(info->clk);
+	if (ret)
+		return ret;
 
+	writel(1, info->enable_reg);
 	exynos_adc_hw_init(info);
 
 	return 0;
diff --git a/drivers/iio/adc/max1363.c b/drivers/iio/adc/max1363.c
index 9cf3229a727..1b3b74be5c2 100644
--- a/drivers/iio/adc/max1363.c
+++ b/drivers/iio/adc/max1363.c
@@ -1252,8 +1252,8 @@ static const struct max1363_chip_info max1363_chip_info_tbl[] = {
 		.num_modes = ARRAY_SIZE(max1238_mode_list),
 		.default_mode = s0to11,
 		.info = &max1238_info,
-		.channels = max1238_channels,
-		.num_channels = ARRAY_SIZE(max1238_channels),
+		.channels = max1038_channels,
+		.num_channels = ARRAY_SIZE(max1038_channels),
 	},
 	[max11605] = {
 		.bits = 8,
@@ -1262,8 +1262,8 @@ static const struct max1363_chip_info max1363_chip_info_tbl[] = {
 		.num_modes = ARRAY_SIZE(max1238_mode_list),
 		.default_mode = s0to11,
 		.info = &max1238_info,
-		.channels = max1238_channels,
-		.num_channels = ARRAY_SIZE(max1238_channels),
+		.channels = max1038_channels,
+		.num_channels = ARRAY_SIZE(max1038_channels),
 	},
 	[max11606] = {
 		.bits = 10,
@@ -1312,8 +1312,8 @@ static const struct max1363_chip_info max1363_chip_info_tbl[] = {
 		.num_modes = ARRAY_SIZE(max1238_mode_list),
 		.default_mode = s0to11,
 		.info = &max1238_info,
-		.channels = max1238_channels,
-		.num_channels = ARRAY_SIZE(max1238_channels),
+		.channels = max1138_channels,
+		.num_channels = ARRAY_SIZE(max1138_channels),
 	},
 	[max11611] = {
 		.bits = 10,
@@ -1322,8 +1322,8 @@ static const struct max1363_chip_info max1363_chip_info_tbl[] = {
 		.num_modes = ARRAY_SIZE(max1238_mode_list),
 		.default_mode = s0to11,
 		.info = &max1238_info,
-		.channels = max1238_channels,
-		.num_channels = ARRAY_SIZE(max1238_channels),
+		.channels = max1138_channels,
+		.num_channels = ARRAY_SIZE(max1138_channels),
 	},
 	[max11612] = {
 		.bits = 12,
diff --git a/drivers/iio/adc/mcp3422.c b/drivers/iio/adc/mcp3422.c
index 47dcb34ff44..51672256072 100644
--- a/drivers/iio/adc/mcp3422.c
+++ b/drivers/iio/adc/mcp3422.c
@@ -1,10 +1,11 @@
 /*
- * mcp3422.c - driver for the Microchip mcp3422/3/4 chip family
+ * mcp3422.c - driver for the Microchip mcp3422/3/4/6/7/8 chip family
  *
  * Copyright (C) 2013, Angelo Compagnucci
  * Author: Angelo Compagnucci <angelo.compagnucci@gmail.com>
  *
  * Datasheet: http://ww1.microchip.com/downloads/en/devicedoc/22088b.pdf
+ *            http://ww1.microchip.com/downloads/en/DeviceDoc/22226a.pdf
  *
  * This driver exports the value of analog input voltage to sysfs, the
  * voltage unit is nV.
@@ -96,6 +97,7 @@ static const int mcp3422_sign_extend[4] = {
 /* Client data (each client gets its own) */
 struct mcp3422 {
 	struct i2c_client *i2c;
+	u8 id;
 	u8 config;
 	u8 pga[4];
 	struct mutex lock;
@@ -238,6 +240,8 @@ static int mcp3422_write_raw(struct iio_dev *iio,
 			temp = MCP3422_SRATE_15;
 			break;
 		case 3:
+			if (adc->id > 4)
+				return -EINVAL;
 			temp = MCP3422_SRATE_3;
 			break;
 		default:
@@ -271,6 +275,17 @@ static int mcp3422_write_raw_get_fmt(struct iio_dev *indio_dev,
 	}
 }
 
+static ssize_t mcp3422_show_samp_freqs(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct mcp3422 *adc = iio_priv(dev_to_iio_dev(dev));
+
+	if (adc->id > 4)
+		return sprintf(buf, "240 60 15\n");
+
+	return sprintf(buf, "240 60 15 3\n");
+}
+
 static ssize_t mcp3422_show_scales(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
@@ -284,12 +299,13 @@ static ssize_t mcp3422_show_scales(struct device *dev,
 		mcp3422_scales[sample_rate][3]);
 }
 
-static IIO_CONST_ATTR_SAMP_FREQ_AVAIL("240 60 15 3");
+static IIO_DEVICE_ATTR(sampling_frequency_available, S_IRUGO,
+		mcp3422_show_samp_freqs, NULL, 0);
 static IIO_DEVICE_ATTR(in_voltage_scale_available, S_IRUGO,
 		mcp3422_show_scales, NULL, 0);
 
 static struct attribute *mcp3422_attributes[] = {
-	&iio_const_attr_sampling_frequency_available.dev_attr.attr,
+	&iio_dev_attr_sampling_frequency_available.dev_attr.attr,
 	&iio_dev_attr_in_voltage_scale_available.dev_attr.attr,
 	NULL,
 };
@@ -335,6 +351,7 @@ static int mcp3422_probe(struct i2c_client *client,
 
 	adc = iio_priv(indio_dev);
 	adc->i2c = client;
+	adc->id = (u8)(id->driver_data);
 
 	mutex_init(&adc->lock);
 
@@ -343,13 +360,16 @@ static int mcp3422_probe(struct i2c_client *client,
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->info = &mcp3422_info;
 
-	switch ((unsigned int)(id->driver_data)) {
+	switch (adc->id) {
 	case 2:
 	case 3:
+	case 6:
+	case 7:
 		indio_dev->channels = mcp3422_channels;
 		indio_dev->num_channels = ARRAY_SIZE(mcp3422_channels);
 		break;
 	case 4:
+	case 8:
 		indio_dev->channels = mcp3424_channels;
 		indio_dev->num_channels = ARRAY_SIZE(mcp3424_channels);
 		break;
@@ -375,6 +395,9 @@ static const struct i2c_device_id mcp3422_id[] = {
 	{ "mcp3422", 2 },
 	{ "mcp3423", 3 },
 	{ "mcp3424", 4 },
+	{ "mcp3426", 6 },
+	{ "mcp3427", 7 },
+	{ "mcp3428", 8 },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, mcp3422_id);
@@ -399,5 +422,5 @@ static struct i2c_driver mcp3422_driver = {
 module_i2c_driver(mcp3422_driver);
 
 MODULE_AUTHOR("Angelo Compagnucci <angelo.compagnucci@gmail.com>");
-MODULE_DESCRIPTION("Microchip mcp3422/3/4 driver");
+MODULE_DESCRIPTION("Microchip mcp3422/3/4/6/7/8 driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/common/hid-sensors/hid-sensor-attributes.c b/drivers/iio/common/hid-sensors/hid-sensor-attributes.c
index 75b54730a96..403dd3d8986 100644
--- a/drivers/iio/common/hid-sensors/hid-sensor-attributes.c
+++ b/drivers/iio/common/hid-sensors/hid-sensor-attributes.c
@@ -26,6 +26,40 @@
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 
+struct {
+	u32 usage_id;
+	int unit; /* 0 for default others from HID sensor spec */
+	int scale_val0; /* scale, whole number */
+	int scale_val1; /* scale, fraction in micros */
+} static unit_conversion[] = {
+	{HID_USAGE_SENSOR_ACCEL_3D, 0, 9, 806650},
+	{HID_USAGE_SENSOR_ACCEL_3D,
+		HID_USAGE_SENSOR_UNITS_METERS_PER_SEC_SQRD, 1, 0},
+	{HID_USAGE_SENSOR_ACCEL_3D,
+		HID_USAGE_SENSOR_UNITS_G, 9, 806650},
+
+	{HID_USAGE_SENSOR_GYRO_3D, 0, 0, 17453},
+	{HID_USAGE_SENSOR_GYRO_3D,
+		HID_USAGE_SENSOR_UNITS_RADIANS_PER_SECOND, 1, 0},
+	{HID_USAGE_SENSOR_GYRO_3D,
+		HID_USAGE_SENSOR_UNITS_DEGREES_PER_SECOND, 0, 17453},
+
+	{HID_USAGE_SENSOR_COMPASS_3D, 0, 0, 1000},
+	{HID_USAGE_SENSOR_COMPASS_3D, HID_USAGE_SENSOR_UNITS_GAUSS, 1, 0},
+
+	{HID_USAGE_SENSOR_INCLINOMETER_3D, 0, 0, 17453},
+	{HID_USAGE_SENSOR_INCLINOMETER_3D,
+		HID_USAGE_SENSOR_UNITS_DEGREES, 0, 17453},
+	{HID_USAGE_SENSOR_INCLINOMETER_3D,
+		HID_USAGE_SENSOR_UNITS_RADIANS, 1, 0},
+
+	{HID_USAGE_SENSOR_ALS, 0, 1, 0},
+	{HID_USAGE_SENSOR_ALS, HID_USAGE_SENSOR_UNITS_LUX, 1, 0},
+
+	{HID_USAGE_SENSOR_PRESSURE, 0, 100000, 0},
+	{HID_USAGE_SENSOR_PRESSURE, HID_USAGE_SENSOR_UNITS_PASCAL, 1, 0},
+};
+
 static int pow_10(unsigned power)
 {
 	int i;
@@ -113,6 +147,26 @@ static u32 convert_to_vtf_format(int size, int exp, int val1, int val2)
 	return value;
 }
 
+s32 hid_sensor_read_poll_value(struct hid_sensor_common *st)
+{
+	s32 value = 0;
+	int ret;
+
+	ret = sensor_hub_get_feature(st->hsdev,
+		st->poll.report_id,
+		st->poll.index, &value);
+
+	if (ret < 0 || value < 0) {
+		return -EINVAL;
+	} else {
+		if (st->poll.units == HID_USAGE_SENSOR_UNITS_SECOND)
+			value = value * 1000;
+	}
+
+	return value;
+}
+EXPORT_SYMBOL(hid_sensor_read_poll_value);
+
 int hid_sensor_read_samp_freq_value(struct hid_sensor_common *st,
 				int *val1, int *val2)
 {
@@ -209,15 +263,108 @@ int hid_sensor_write_raw_hyst_value(struct hid_sensor_common *st,
 }
 EXPORT_SYMBOL(hid_sensor_write_raw_hyst_value);
 
-int hid_sensor_parse_common_attributes(struct hid_sensor_hub_device *hsdev,
+/*
+ * This fuction applies the unit exponent to the scale.
+ * For example:
+ * 9.806650 ->exp:2-> val0[980]val1[665000]
+ * 9.000806 ->exp:2-> val0[900]val1[80600]
+ * 0.174535 ->exp:2-> val0[17]val1[453500]
+ * 1.001745 ->exp:0-> val0[1]val1[1745]
+ * 1.001745 ->exp:2-> val0[100]val1[174500]
+ * 1.001745 ->exp:4-> val0[10017]val1[450000]
+ * 9.806650 ->exp:-2-> val0[0]val1[98066]
+ */
+static void adjust_exponent_micro(int *val0, int *val1, int scale0,
+				  int scale1, int exp)
+{
+	int i;
+	int x;
+	int res;
+	int rem;
+
+	if (exp > 0) {
+		*val0 = scale0 * pow_10(exp);
+		res = 0;
+		if (exp > 6) {
+			*val1 = 0;
+			return;
+		}
+		for (i = 0; i < exp; ++i) {
+			x = scale1 / pow_10(5 - i);
+			res += (pow_10(exp - 1 - i) * x);
+			scale1 = scale1 % pow_10(5 - i);
+		}
+		*val0 += res;
+			*val1 = scale1 * pow_10(exp);
+	} else if (exp < 0) {
+		exp = abs(exp);
+		if (exp > 6) {
+			*val0 = *val1 = 0;
+			return;
+		}
+		*val0 = scale0 / pow_10(exp);
+		rem = scale0 % pow_10(exp);
+		res = 0;
+		for (i = 0; i < (6 - exp); ++i) {
+			x = scale1 / pow_10(5 - i);
+			res += (pow_10(5 - exp - i) * x);
+			scale1 = scale1 % pow_10(5 - i);
+		}
+		*val1 = rem * pow_10(6 - exp) + res;
+	} else {
+		*val0 = scale0;
+		*val1 = scale1;
+	}
+}
+
+int hid_sensor_format_scale(u32 usage_id,
+			struct hid_sensor_hub_attribute_info *attr_info,
+			int *val0, int *val1)
+{
+	int i;
+	int exp;
+
+	*val0 = 1;
+	*val1 = 0;
+
+	for (i = 0; i < ARRAY_SIZE(unit_conversion); ++i) {
+		if (unit_conversion[i].usage_id == usage_id &&
+			unit_conversion[i].unit == attr_info->units) {
+			exp  = hid_sensor_convert_exponent(
+						attr_info->unit_expo);
+			adjust_exponent_micro(val0, val1,
+					unit_conversion[i].scale_val0,
+					unit_conversion[i].scale_val1, exp);
+			break;
+		}
+	}
+
+	return IIO_VAL_INT_PLUS_MICRO;
+}
+EXPORT_SYMBOL(hid_sensor_format_scale);
+
+int hid_sensor_get_reporting_interval(struct hid_sensor_hub_device *hsdev,
 					u32 usage_id,
 					struct hid_sensor_common *st)
 {
-
 	sensor_hub_input_get_attribute_info(hsdev,
 					HID_FEATURE_REPORT, usage_id,
 					HID_USAGE_SENSOR_PROP_REPORT_INTERVAL,
 					&st->poll);
+	/* Default unit of measure is milliseconds */
+	if (st->poll.units == 0)
+		st->poll.units = HID_USAGE_SENSOR_UNITS_MILLISECOND;
+	return 0;
+
+}
+
+int hid_sensor_parse_common_attributes(struct hid_sensor_hub_device *hsdev,
+					u32 usage_id,
+					struct hid_sensor_common *st)
+{
+
+
+	hid_sensor_get_reporting_interval(hsdev, usage_id, st);
 
 	sensor_hub_input_get_attribute_info(hsdev,
 					HID_FEATURE_REPORT, usage_id,
diff --git a/drivers/iio/common/hid-sensors/hid-sensor-trigger.c b/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
index dbefbdaf7cd..73282cee0c8 100644
--- a/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
+++ b/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
@@ -28,16 +28,17 @@
 #include <linux/iio/sysfs.h>
 #include "hid-sensor-trigger.h"
 
-static int hid_sensor_data_rdy_trigger_set_state(struct iio_trigger *trig,
-						bool state)
+int hid_sensor_power_state(struct hid_sensor_common *st, bool state)
 {
-	struct hid_sensor_common *st = iio_trigger_get_drvdata(trig);
 	int state_val;
 	int report_val;
 
 	if (state) {
 		if (sensor_hub_device_open(st->hsdev))
 			return -EIO;
+
+		atomic_inc(&st->data_ready);
+
 		state_val = hid_sensor_get_usage_index(st->hsdev,
 			st->power_state.report_id,
 			st->power_state.index,
@@ -47,6 +48,8 @@ static int hid_sensor_data_rdy_trigger_set_state(struct iio_trigger *trig,
 			st->report_state.index,
 			HID_USAGE_SENSOR_PROP_REPORTING_STATE_ALL_EVENTS_ENUM);
 	} else {
+		if (!atomic_dec_and_test(&st->data_ready))
+			return 0;
 		sensor_hub_device_close(st->hsdev);
 		state_val = hid_sensor_get_usage_index(st->hsdev,
 			st->power_state.report_id,
@@ -57,7 +60,6 @@ static int hid_sensor_data_rdy_trigger_set_state(struct iio_trigger *trig,
 			st->report_state.index,
 			HID_USAGE_SENSOR_PROP_REPORTING_STATE_NO_EVENTS_ENUM);
 	}
-	st->data_ready = state;
 
 	if (state_val >= 0) {
 		state_val += st->power_state.logical_minimum;
@@ -75,6 +77,13 @@ static int hid_sensor_data_rdy_trigger_set_state(struct iio_trigger *trig,
 
 	return 0;
 }
+EXPORT_SYMBOL(hid_sensor_power_state);
+
+static int hid_sensor_data_rdy_trigger_set_state(struct iio_trigger *trig,
+						bool state)
+{
+	return hid_sensor_power_state(iio_trigger_get_drvdata(trig), state);
+}
 
 void hid_sensor_remove_trigger(struct hid_sensor_common *attrb)
 {
diff --git a/drivers/iio/common/hid-sensors/hid-sensor-trigger.h b/drivers/iio/common/hid-sensors/hid-sensor-trigger.h
index ca02f7811aa..0f8e78c249d 100644
--- a/drivers/iio/common/hid-sensors/hid-sensor-trigger.h
+++ b/drivers/iio/common/hid-sensors/hid-sensor-trigger.h
@@ -22,5 +22,6 @@
 int hid_sensor_setup_trigger(struct iio_dev *indio_dev, const char *name,
 				struct hid_sensor_common *attrb);
 void hid_sensor_remove_trigger(struct hid_sensor_common *attrb);
+int hid_sensor_power_state(struct hid_sensor_common *st, bool state);
 
 #endif
diff --git a/drivers/iio/common/st_sensors/st_sensors_core.c b/drivers/iio/common/st_sensors/st_sensors_core.c
index 7ba1ef27021..e8b932fed70 100644
--- a/drivers/iio/common/st_sensors/st_sensors_core.c
+++ b/drivers/iio/common/st_sensors/st_sensors_core.c
@@ -13,6 +13,7 @@
 #include <linux/slab.h>
 #include <linux/delay.h>
 #include <linux/iio/iio.h>
+#include <linux/regulator/consumer.h>
 #include <asm/unaligned.h>
 
 #include <linux/iio/common/st_sensors.h>
@@ -198,6 +199,42 @@ int st_sensors_set_axis_enable(struct iio_dev *indio_dev, u8 axis_enable)
 }
 EXPORT_SYMBOL(st_sensors_set_axis_enable);
 
+void st_sensors_power_enable(struct iio_dev *indio_dev)
+{
+	struct st_sensor_data *pdata = iio_priv(indio_dev);
+	int err;
+
+	/* Regulators not mandatory, but if requested we should enable them. */
+	pdata->vdd = devm_regulator_get_optional(indio_dev->dev.parent, "vdd");
+	if (!IS_ERR(pdata->vdd)) {
+		err = regulator_enable(pdata->vdd);
+		if (err != 0)
+			dev_warn(&indio_dev->dev,
+				 "Failed to enable specified Vdd supply\n");
+	}
+
+	pdata->vdd_io = devm_regulator_get_optional(indio_dev->dev.parent, "vddio");
+	if (!IS_ERR(pdata->vdd_io)) {
+		err = regulator_enable(pdata->vdd_io);
+		if (err != 0)
+			dev_warn(&indio_dev->dev,
+				 "Failed to enable specified Vdd_IO supply\n");
+	}
+}
+EXPORT_SYMBOL(st_sensors_power_enable);
+
+void st_sensors_power_disable(struct iio_dev *indio_dev)
+{
+	struct st_sensor_data *pdata = iio_priv(indio_dev);
+
+	if (!IS_ERR(pdata->vdd))
+		regulator_disable(pdata->vdd);
+
+	if (!IS_ERR(pdata->vdd_io))
+		regulator_disable(pdata->vdd_io);
+}
+EXPORT_SYMBOL(st_sensors_power_disable);
+
 static int st_sensors_set_drdy_int_pin(struct iio_dev *indio_dev,
 				       struct st_sensors_platform_data *pdata)
 {
diff --git a/drivers/iio/gyro/hid-sensor-gyro-3d.c b/drivers/iio/gyro/hid-sensor-gyro-3d.c
index 59d6bc3e04d..40f4e4935d0 100644
--- a/drivers/iio/gyro/hid-sensor-gyro-3d.c
+++ b/drivers/iio/gyro/hid-sensor-gyro-3d.c
@@ -22,6 +22,7 @@
 #include <linux/interrupt.h>
 #include <linux/irq.h>
 #include <linux/slab.h>
+#include <linux/delay.h>
 #include <linux/hid-sensor-hub.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
@@ -42,6 +43,10 @@ struct gyro_3d_state {
 	struct hid_sensor_common common_attributes;
 	struct hid_sensor_hub_attribute_info gyro[GYRO_3D_CHANNEL_MAX];
 	u32 gyro_val[GYRO_3D_CHANNEL_MAX];
+	int scale_pre_decml;
+	int scale_post_decml;
+	int scale_precision;
+	int value_offset;
 };
 
 static const u32 gyro_3d_addresses[GYRO_3D_CHANNEL_MAX] = {
@@ -56,6 +61,7 @@ static const struct iio_chan_spec gyro_3d_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
 		BIT(IIO_CHAN_INFO_SCALE) |
 		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
@@ -65,6 +71,7 @@ static const struct iio_chan_spec gyro_3d_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Y,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
 		BIT(IIO_CHAN_INFO_SCALE) |
 		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
@@ -74,6 +81,7 @@ static const struct iio_chan_spec gyro_3d_channels[] = {
 		.type = IIO_ANGL_VEL,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
 		BIT(IIO_CHAN_INFO_SCALE) |
 		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
@@ -104,31 +112,42 @@ static int gyro_3d_read_raw(struct iio_dev *indio_dev,
 	u32 address;
 	int ret;
 	int ret_type;
+	s32 poll_value;
 
 	*val = 0;
 	*val2 = 0;
 	switch (mask) {
 	case 0:
+		poll_value = hid_sensor_read_poll_value(
+					&gyro_state->common_attributes);
+		if (poll_value < 0)
+			return -EINVAL;
+
+		hid_sensor_power_state(&gyro_state->common_attributes, true);
+		msleep_interruptible(poll_value * 2);
 		report_id = gyro_state->gyro[chan->scan_index].report_id;
 		address = gyro_3d_addresses[chan->scan_index];
 		if (report_id >= 0)
 			*val = sensor_hub_input_attr_get_raw_value(
-				gyro_state->common_attributes.hsdev,
-				HID_USAGE_SENSOR_GYRO_3D, address,
-				report_id);
+					gyro_state->common_attributes.hsdev,
+					HID_USAGE_SENSOR_GYRO_3D, address,
+					report_id);
 		else {
 			*val = 0;
+			hid_sensor_power_state(&gyro_state->common_attributes,
+						false);
 			return -EINVAL;
 		}
+		hid_sensor_power_state(&gyro_state->common_attributes, false);
 		ret_type = IIO_VAL_INT;
 		break;
 	case IIO_CHAN_INFO_SCALE:
-		*val = gyro_state->gyro[CHANNEL_SCAN_INDEX_X].units;
-		ret_type = IIO_VAL_INT;
+		*val = gyro_state->scale_pre_decml;
+		*val2 = gyro_state->scale_post_decml;
+		ret_type = gyro_state->scale_precision;
 		break;
 	case IIO_CHAN_INFO_OFFSET:
-		*val = hid_sensor_convert_exponent(
-			gyro_state->gyro[CHANNEL_SCAN_INDEX_X].unit_expo);
+		*val = gyro_state->value_offset;
 		ret_type = IIO_VAL_INT;
 		break;
 	case IIO_CHAN_INFO_SAMP_FREQ:
@@ -197,9 +216,8 @@ static int gyro_3d_proc_event(struct hid_sensor_hub_device *hsdev,
 	struct iio_dev *indio_dev = platform_get_drvdata(priv);
 	struct gyro_3d_state *gyro_state = iio_priv(indio_dev);
 
-	dev_dbg(&indio_dev->dev, "gyro_3d_proc_event [%d]\n",
-				gyro_state->common_attributes.data_ready);
-	if (gyro_state->common_attributes.data_ready)
+	dev_dbg(&indio_dev->dev, "gyro_3d_proc_event\n");
+	if (atomic_read(&gyro_state->common_attributes.data_ready))
 		hid_sensor_push_data(indio_dev,
 				gyro_state->gyro_val,
 				sizeof(gyro_state->gyro_val));
@@ -262,6 +280,11 @@ static int gyro_3d_parse_report(struct platform_device *pdev,
 			st->gyro[1].index, st->gyro[1].report_id,
 			st->gyro[2].index, st->gyro[2].report_id);
 
+	st->scale_precision = hid_sensor_format_scale(
+				HID_USAGE_SENSOR_GYRO_3D,
+				&st->gyro[CHANNEL_SCAN_INDEX_X],
+				&st->scale_pre_decml, &st->scale_post_decml);
+
 	/* Set Sensitivity field ids, when there is no individual modifier */
 	if (st->common_attributes.sensitivity.index < 0) {
 		sensor_hub_input_get_attribute_info(hsdev,
@@ -330,7 +353,7 @@ static int hid_gyro_3d_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
 		goto error_free_dev_mem;
 	}
-	gyro_state->common_attributes.data_ready = false;
+	atomic_set(&gyro_state->common_attributes.data_ready, 0);
 	ret = hid_sensor_setup_trigger(indio_dev, name,
 					&gyro_state->common_attributes);
 	if (ret < 0) {
diff --git a/drivers/iio/gyro/itg3200_core.c b/drivers/iio/gyro/itg3200_core.c
index 4d3f3b92b36..8295e318399 100644
--- a/drivers/iio/gyro/itg3200_core.c
+++ b/drivers/iio/gyro/itg3200_core.c
@@ -110,8 +110,6 @@ static int itg3200_read_raw(struct iio_dev *indio_dev,
 	default:
 		return -EINVAL;
 	}
-
-	return ret;
 }
 
 static ssize_t itg3200_read_frequency(struct device *dev,
diff --git a/drivers/iio/gyro/st_gyro_core.c b/drivers/iio/gyro/st_gyro_core.c
index a8e174a47bc..ed74a906998 100644
--- a/drivers/iio/gyro/st_gyro_core.c
+++ b/drivers/iio/gyro/st_gyro_core.c
@@ -311,6 +311,8 @@ int st_gyro_common_probe(struct iio_dev *indio_dev,
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->info = &gyro_info;
 
+	st_sensors_power_enable(indio_dev);
+
 	err = st_sensors_check_device_support(indio_dev,
 				ARRAY_SIZE(st_gyro_sensors), st_gyro_sensors);
 	if (err < 0)
@@ -344,6 +346,9 @@ int st_gyro_common_probe(struct iio_dev *indio_dev,
 	if (err)
 		goto st_gyro_device_register_error;
 
+	dev_info(&indio_dev->dev, "registered gyroscope %s\n",
+		 indio_dev->name);
+
 	return 0;
 
 st_gyro_device_register_error:
@@ -360,6 +365,8 @@ void st_gyro_common_remove(struct iio_dev *indio_dev)
 {
 	struct st_sensor_data *gdata = iio_priv(indio_dev);
 
+	st_sensors_power_disable(indio_dev);
+
 	iio_device_unregister(indio_dev);
 	if (gdata->get_irq_data_ready(indio_dev) > 0)
 		st_sensors_deallocate_trigger(indio_dev);
diff --git a/drivers/iio/iio_core.h b/drivers/iio/iio_core.h
index f6db6af36ba..5f0ea77fe71 100644
--- a/drivers/iio/iio_core.h
+++ b/drivers/iio/iio_core.h
@@ -35,7 +35,7 @@ int __iio_add_chan_devattr(const char *postfix,
 			   struct list_head *attr_list);
 void iio_free_chan_devattr_list(struct list_head *attr_list);
 
-ssize_t iio_format_value(char *buf, unsigned int type, int val, int val2);
+ssize_t iio_format_value(char *buf, unsigned int type, int size, int *vals);
 
 /* Event interface flags */
 #define IIO_BUSY_BIT_POS 1
diff --git a/drivers/iio/imu/inv_mpu6050/Kconfig b/drivers/iio/imu/inv_mpu6050/Kconfig
index 361b2328453..2d0608ba88d 100644
--- a/drivers/iio/imu/inv_mpu6050/Kconfig
+++ b/drivers/iio/imu/inv_mpu6050/Kconfig
@@ -9,6 +9,8 @@ config INV_MPU6050_IIO
 	select IIO_TRIGGERED_BUFFER
 	help
 	  This driver supports the Invensense MPU6050 devices.
+	  This driver can also support MPU6500 in MPU6050 compatibility mode
+	  and also in MPU6500 mode with some limitations.
 	  It is a gyroscope/accelerometer combo device.
 	  This driver can be built as a module. The module will be called
 	  inv-mpu6050.
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index cb9f96b446a..0c6517c94a9 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -660,6 +660,7 @@ static int inv_mpu_probe(struct i2c_client *client,
 {
 	struct inv_mpu6050_state *st;
 	struct iio_dev *indio_dev;
+	struct inv_mpu6050_platform_data *pdata;
 	int result;
 
 	if (!i2c_check_functionality(client->adapter,
@@ -672,8 +673,10 @@ static int inv_mpu_probe(struct i2c_client *client,
 
 	st = iio_priv(indio_dev);
 	st->client = client;
-	st->plat_data = *(struct inv_mpu6050_platform_data
-				*)dev_get_platdata(&client->dev);
+	pdata = (struct inv_mpu6050_platform_data
+			*)dev_get_platdata(&client->dev);
+	if (pdata)
+		st->plat_data = *pdata;
 	/* power is turned on inside check chip type*/
 	result = inv_check_and_setup_chip(st, id);
 	if (result)
@@ -764,6 +767,7 @@ static SIMPLE_DEV_PM_OPS(inv_mpu_pmops, inv_mpu_suspend, inv_mpu_resume);
  */
 static const struct i2c_device_id inv_mpu_id[] = {
 	{"mpu6050", INV_MPU6050},
+	{"mpu6500", INV_MPU6500},
 	{}
 };
 
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index 0ab382be1e6..e7799315d4d 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -59,6 +59,7 @@ struct inv_mpu6050_reg_map {
 /*device enum */
 enum inv_devices {
 	INV_MPU6050,
+	INV_MPU6500,
 	INV_NUM_PARTS
 };
 
diff --git a/drivers/iio/industrialio-buffer.c b/drivers/iio/industrialio-buffer.c
index e108f2a9d82..36b1ae92e23 100644
--- a/drivers/iio/industrialio-buffer.c
+++ b/drivers/iio/industrialio-buffer.c
@@ -150,7 +150,16 @@ static ssize_t iio_show_fixed_type(struct device *dev,
 		type = IIO_BE;
 #endif
 	}
-	return sprintf(buf, "%s:%c%d/%d>>%u\n",
+	if (this_attr->c->scan_type.repeat > 1)
+		return sprintf(buf, "%s:%c%d/%dX%d>>%u\n",
+		       iio_endian_prefix[type],
+		       this_attr->c->scan_type.sign,
+		       this_attr->c->scan_type.realbits,
+		       this_attr->c->scan_type.storagebits,
+		       this_attr->c->scan_type.repeat,
+		       this_attr->c->scan_type.shift);
+		else
+			return sprintf(buf, "%s:%c%d/%d>>%u\n",
 		       iio_endian_prefix[type],
 		       this_attr->c->scan_type.sign,
 		       this_attr->c->scan_type.realbits,
@@ -165,7 +174,8 @@ static ssize_t iio_scan_el_show(struct device *dev,
 	int ret;
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 
-	ret = test_bit(to_iio_dev_attr(attr)->address,
+	/* Ensure ret is 0 or 1. */
+	ret = !!test_bit(to_iio_dev_attr(attr)->address,
 		       indio_dev->buffer->scan_mask);
 
 	return sprintf(buf, "%d\n", ret);
@@ -474,14 +484,22 @@ static int iio_compute_scan_bytes(struct iio_dev *indio_dev,
 	for_each_set_bit(i, mask,
 			 indio_dev->masklength) {
 		ch = iio_find_channel_from_si(indio_dev, i);
-		length = ch->scan_type.storagebits / 8;
+		if (ch->scan_type.repeat > 1)
+			length = ch->scan_type.storagebits / 8 *
+				ch->scan_type.repeat;
+		else
+			length = ch->scan_type.storagebits / 8;
 		bytes = ALIGN(bytes, length);
 		bytes += length;
 	}
 	if (timestamp) {
 		ch = iio_find_channel_from_si(indio_dev,
 					      indio_dev->scan_index_timestamp);
-		length = ch->scan_type.storagebits / 8;
+		if (ch->scan_type.repeat > 1)
+			length = ch->scan_type.storagebits / 8 *
+				ch->scan_type.repeat;
+		else
+			length = ch->scan_type.storagebits / 8;
 		bytes = ALIGN(bytes, length);
 		bytes += length;
 	}
@@ -862,7 +880,8 @@ int iio_scan_mask_query(struct iio_dev *indio_dev,
 	if (!buffer->scan_mask)
 		return 0;
 
-	return test_bit(bit, buffer->scan_mask);
+	/* Ensure return value is 0 or 1. */
+	return !!test_bit(bit, buffer->scan_mask);
 };
 EXPORT_SYMBOL_GPL(iio_scan_mask_query);
 
@@ -957,7 +976,11 @@ static int iio_buffer_update_demux(struct iio_dev *indio_dev,
 					       indio_dev->masklength,
 					       in_ind + 1);
 			ch = iio_find_channel_from_si(indio_dev, in_ind);
-			length = ch->scan_type.storagebits/8;
+			if (ch->scan_type.repeat > 1)
+				length = ch->scan_type.storagebits / 8 *
+					ch->scan_type.repeat;
+			else
+				length = ch->scan_type.storagebits / 8;
 			/* Make sure we are aligned */
 			in_loc += length;
 			if (in_loc % length)
@@ -969,7 +992,11 @@ static int iio_buffer_update_demux(struct iio_dev *indio_dev,
 			goto error_clear_mux_table;
 		}
 		ch = iio_find_channel_from_si(indio_dev, in_ind);
-		length = ch->scan_type.storagebits/8;
+		if (ch->scan_type.repeat > 1)
+			length = ch->scan_type.storagebits / 8 *
+				ch->scan_type.repeat;
+		else
+			length = ch->scan_type.storagebits / 8;
 		if (out_loc % length)
 			out_loc += length - out_loc % length;
 		if (in_loc % length)
@@ -990,7 +1017,11 @@ static int iio_buffer_update_demux(struct iio_dev *indio_dev,
 		}
 		ch = iio_find_channel_from_si(indio_dev,
 			indio_dev->scan_index_timestamp);
-		length = ch->scan_type.storagebits/8;
+		if (ch->scan_type.repeat > 1)
+			length = ch->scan_type.storagebits / 8 *
+				ch->scan_type.repeat;
+		else
+			length = ch->scan_type.storagebits / 8;
 		if (out_loc % length)
 			out_loc += length - out_loc % length;
 		if (in_loc % length)
diff --git a/drivers/iio/industrialio-core.c b/drivers/iio/industrialio-core.c
index ede16aec20f..4b1f375c565 100644
--- a/drivers/iio/industrialio-core.c
+++ b/drivers/iio/industrialio-core.c
@@ -84,6 +84,9 @@ static const char * const iio_modifier_names[] = {
 	[IIO_MOD_LIGHT_RED] = "red",
 	[IIO_MOD_LIGHT_GREEN] = "green",
 	[IIO_MOD_LIGHT_BLUE] = "blue",
+	[IIO_MOD_QUATERNION] = "quaternion",
+	[IIO_MOD_TEMP_AMBIENT] = "ambient",
+	[IIO_MOD_TEMP_OBJECT] = "object",
 };
 
 /* relies on pairs of these shared then separate */
@@ -340,7 +343,7 @@ ssize_t iio_enum_read(struct iio_dev *indio_dev,
 	else if (i >= e->num_items)
 		return -EINVAL;
 
-	return sprintf(buf, "%s\n", e->items[i]);
+	return snprintf(buf, PAGE_SIZE, "%s\n", e->items[i]);
 }
 EXPORT_SYMBOL_GPL(iio_enum_read);
 
@@ -373,41 +376,53 @@ EXPORT_SYMBOL_GPL(iio_enum_write);
  * @buf: The buffer to which the formated value gets written
  * @type: One of the IIO_VAL_... constants. This decides how the val and val2
  *        parameters are formatted.
- * @val: First part of the value, exact meaning depends on the type parameter.
- * @val2: Second part of the value, exact meaning depends on the type parameter.
+ * @vals: pointer to the values, exact meaning depends on the type parameter.
  */
-ssize_t iio_format_value(char *buf, unsigned int type, int val, int val2)
+ssize_t iio_format_value(char *buf, unsigned int type, int size, int *vals)
 {
 	unsigned long long tmp;
 	bool scale_db = false;
 
 	switch (type) {
 	case IIO_VAL_INT:
-		return sprintf(buf, "%d\n", val);
+		return sprintf(buf, "%d\n", vals[0]);
 	case IIO_VAL_INT_PLUS_MICRO_DB:
 		scale_db = true;
 	case IIO_VAL_INT_PLUS_MICRO:
-		if (val2 < 0)
-			return sprintf(buf, "-%ld.%06u%s\n", abs(val), -val2,
+		if (vals[1] < 0)
+			return sprintf(buf, "-%ld.%06u%s\n", abs(vals[0]),
+					-vals[1],
 				scale_db ? " dB" : "");
 		else
-			return sprintf(buf, "%d.%06u%s\n", val, val2,
+			return sprintf(buf, "%d.%06u%s\n", vals[0], vals[1],
 				scale_db ? " dB" : "");
 	case IIO_VAL_INT_PLUS_NANO:
-		if (val2 < 0)
-			return sprintf(buf, "-%ld.%09u\n", abs(val), -val2);
+		if (vals[1] < 0)
+			return sprintf(buf, "-%ld.%09u\n", abs(vals[0]),
+					-vals[1]);
 		else
-			return sprintf(buf, "%d.%09u\n", val, val2);
+			return sprintf(buf, "%d.%09u\n", vals[0], vals[1]);
 	case IIO_VAL_FRACTIONAL:
-		tmp = div_s64((s64)val * 1000000000LL, val2);
-		val2 = do_div(tmp, 1000000000LL);
-		val = tmp;
-		return sprintf(buf, "%d.%09u\n", val, val2);
+		tmp = div_s64((s64)vals[0] * 1000000000LL, vals[1]);
+		vals[1] = do_div(tmp, 1000000000LL);
+		vals[0] = tmp;
+		return sprintf(buf, "%d.%09u\n", vals[0], vals[1]);
 	case IIO_VAL_FRACTIONAL_LOG2:
-		tmp = (s64)val * 1000000000LL >> val2;
-		val2 = do_div(tmp, 1000000000LL);
-		val = tmp;
-		return sprintf(buf, "%d.%09u\n", val, val2);
+		tmp = (s64)vals[0] * 1000000000LL >> vals[1];
+		vals[1] = do_div(tmp, 1000000000LL);
+		vals[0] = tmp;
+		return sprintf(buf, "%d.%09u\n", vals[0], vals[1]);
+	case IIO_VAL_INT_MULTIPLE:
+	{
+		int i;
+		int len = 0;
+
+		for (i = 0; i < size; ++i)
+			len += snprintf(&buf[len], PAGE_SIZE - len, "%d ",
+								vals[i]);
+		len += snprintf(&buf[len], PAGE_SIZE - len, "\n");
+		return len;
+	}
 	default:
 		return 0;
 	}
@@ -419,14 +434,23 @@ static ssize_t iio_read_channel_info(struct device *dev,
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
-	int val, val2;
-	int ret = indio_dev->info->read_raw(indio_dev, this_attr->c,
-					    &val, &val2, this_attr->address);
+	int vals[INDIO_MAX_RAW_ELEMENTS];
+	int ret;
+	int val_len = 2;
+
+	if (indio_dev->info->read_raw_multi)
+		ret = indio_dev->info->read_raw_multi(indio_dev, this_attr->c,
+							INDIO_MAX_RAW_ELEMENTS,
+							vals, &val_len,
+							this_attr->address);
+	else
+		ret = indio_dev->info->read_raw(indio_dev, this_attr->c,
+				    &vals[0], &vals[1], this_attr->address);
 
 	if (ret < 0)
 		return ret;
 
-	return iio_format_value(buf, ret, val, val2);
+	return iio_format_value(buf, ret, val_len, vals);
 }
 
 /**
@@ -716,6 +740,8 @@ static int iio_device_add_info_mask_type(struct iio_dev *indio_dev,
 	int i, ret, attrcount = 0;
 
 	for_each_set_bit(i, infomask, sizeof(infomask)*8) {
+		if (i >= ARRAY_SIZE(iio_chan_info_postfix))
+			return -EINVAL;
 		ret = __iio_add_chan_devattr(iio_chan_info_postfix[i],
 					     chan,
 					     &iio_read_channel_info,
@@ -820,7 +846,7 @@ static ssize_t iio_show_dev_name(struct device *dev,
 				 char *buf)
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
-	return sprintf(buf, "%s\n", indio_dev->name);
+	return snprintf(buf, PAGE_SIZE, "%s\n", indio_dev->name);
 }
 
 static DEVICE_ATTR(name, S_IRUGO, iio_show_dev_name, NULL);
diff --git a/drivers/iio/industrialio-event.c b/drivers/iio/industrialio-event.c
index ea6e06b9c7d..258a973a1fb 100644
--- a/drivers/iio/industrialio-event.c
+++ b/drivers/iio/industrialio-event.c
@@ -270,7 +270,7 @@ static ssize_t iio_ev_value_show(struct device *dev,
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
-	int val, val2;
+	int val, val2, val_arr[2];
 	int ret;
 
 	ret = indio_dev->info->read_event_value(indio_dev,
@@ -279,7 +279,9 @@ static ssize_t iio_ev_value_show(struct device *dev,
 		&val, &val2);
 	if (ret < 0)
 		return ret;
-	return iio_format_value(buf, ret, val, val2);
+	val_arr[0] = val;
+	val_arr[1] = val2;
+	return iio_format_value(buf, ret, 2, val_arr);
 }
 
 static ssize_t iio_ev_value_store(struct device *dev,
@@ -321,7 +323,9 @@ static int iio_device_add_event(struct iio_dev *indio_dev,
 	char *postfix;
 	int ret;
 
-	for_each_set_bit(i, mask, sizeof(*mask)) {
+	for_each_set_bit(i, mask, sizeof(*mask)*8) {
+		if (i >= ARRAY_SIZE(iio_ev_info_text))
+			return -EINVAL;
 		postfix = kasprintf(GFP_KERNEL, "%s_%s_%s",
 				iio_ev_type_text[type], iio_ev_dir_text[dir],
 				iio_ev_info_text[i]);
diff --git a/drivers/iio/inkern.c b/drivers/iio/inkern.c
index 0cf5f8e06cf..d833d55052e 100644
--- a/drivers/iio/inkern.c
+++ b/drivers/iio/inkern.c
@@ -417,12 +417,24 @@ static int iio_channel_read(struct iio_channel *chan, int *val, int *val2,
 	enum iio_chan_info_enum info)
 {
 	int unused;
+	int vals[INDIO_MAX_RAW_ELEMENTS];
+	int ret;
+	int val_len = 2;
 
 	if (val2 == NULL)
 		val2 = &unused;
 
-	return chan->indio_dev->info->read_raw(chan->indio_dev, chan->channel,
-						val, val2, info);
+	if (chan->indio_dev->info->read_raw_multi) {
+		ret = chan->indio_dev->info->read_raw_multi(chan->indio_dev,
+					chan->channel, INDIO_MAX_RAW_ELEMENTS,
+					vals, &val_len, info);
+		*val = vals[0];
+		*val2 = vals[1];
+	} else
+		ret = chan->indio_dev->info->read_raw(chan->indio_dev,
+					chan->channel, val, val2, info);
+
+	return ret;
 }
 
 int iio_read_channel_raw(struct iio_channel *chan, int *val)
@@ -443,6 +455,24 @@ err_unlock:
 }
 EXPORT_SYMBOL_GPL(iio_read_channel_raw);
 
+int iio_read_channel_average_raw(struct iio_channel *chan, int *val)
+{
+	int ret;
+
+	mutex_lock(&chan->indio_dev->info_exist_lock);
+	if (chan->indio_dev->info == NULL) {
+		ret = -ENODEV;
+		goto err_unlock;
+	}
+
+	ret = iio_channel_read(chan, val, NULL, IIO_CHAN_INFO_AVERAGE_RAW);
+err_unlock:
+	mutex_unlock(&chan->indio_dev->info_exist_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(iio_read_channel_average_raw);
+
 static int iio_convert_raw_to_processed_unlocked(struct iio_channel *chan,
 	int raw, int *processed, unsigned int scale)
 {
diff --git a/drivers/iio/light/cm32181.c b/drivers/iio/light/cm32181.c
index 47a6dbac2d0..d976e6ce60d 100644
--- a/drivers/iio/light/cm32181.c
+++ b/drivers/iio/light/cm32181.c
@@ -221,6 +221,7 @@ static int cm32181_read_raw(struct iio_dev *indio_dev,
 		*val = cm32181->calibscale;
 		return IIO_VAL_INT;
 	case IIO_CHAN_INFO_INT_TIME:
+		*val = 0;
 		ret = cm32181_read_als_it(cm32181, val2);
 		return ret;
 	}
diff --git a/drivers/iio/light/cm36651.c b/drivers/iio/light/cm36651.c
index a45e07492db..39fc67e8213 100644
--- a/drivers/iio/light/cm36651.c
+++ b/drivers/iio/light/cm36651.c
@@ -652,7 +652,19 @@ static int cm36651_probe(struct i2c_client *client,
 	cm36651->client = client;
 	cm36651->ps_client = i2c_new_dummy(client->adapter,
 						     CM36651_I2C_ADDR_PS);
+	if (!cm36651->ps_client) {
+		dev_err(&client->dev, "%s: new i2c device failed\n", __func__);
+		ret = -ENODEV;
+		goto error_disable_reg;
+	}
+
 	cm36651->ara_client = i2c_new_dummy(client->adapter, CM36651_ARA);
+	if (!cm36651->ara_client) {
+		dev_err(&client->dev, "%s: new i2c device failed\n", __func__);
+		ret = -ENODEV;
+		goto error_i2c_unregister_ps;
+	}
+
 	mutex_init(&cm36651->lock);
 	indio_dev->dev.parent = &client->dev;
 	indio_dev->channels = cm36651_channels;
@@ -664,7 +676,7 @@ static int cm36651_probe(struct i2c_client *client,
 	ret = cm36651_setup_reg(cm36651);
 	if (ret) {
 		dev_err(&client->dev, "%s: register setup failed\n", __func__);
-		goto error_disable_reg;
+		goto error_i2c_unregister_ara;
 	}
 
 	ret = request_threaded_irq(client->irq, NULL, cm36651_irq_handler,
@@ -672,7 +684,7 @@ static int cm36651_probe(struct i2c_client *client,
 							"cm36651", indio_dev);
 	if (ret) {
 		dev_err(&client->dev, "%s: request irq failed\n", __func__);
-		goto error_disable_reg;
+		goto error_i2c_unregister_ara;
 	}
 
 	ret = iio_device_register(indio_dev);
@@ -685,6 +697,10 @@ static int cm36651_probe(struct i2c_client *client,
 
 error_free_irq:
 	free_irq(client->irq, indio_dev);
+error_i2c_unregister_ara:
+	i2c_unregister_device(cm36651->ara_client);
+error_i2c_unregister_ps:
+	i2c_unregister_device(cm36651->ps_client);
 error_disable_reg:
 	regulator_disable(cm36651->vled_reg);
 	return ret;
@@ -698,6 +714,8 @@ static int cm36651_remove(struct i2c_client *client)
 	iio_device_unregister(indio_dev);
 	regulator_disable(cm36651->vled_reg);
 	free_irq(client->irq, indio_dev);
+	i2c_unregister_device(cm36651->ps_client);
+	i2c_unregister_device(cm36651->ara_client);
 
 	return 0;
 }
diff --git a/drivers/iio/light/gp2ap020a00f.c b/drivers/iio/light/gp2ap020a00f.c
index 5ea4a03c7e7..04bdb85d2d9 100644
--- a/drivers/iio/light/gp2ap020a00f.c
+++ b/drivers/iio/light/gp2ap020a00f.c
@@ -5,13 +5,13 @@
  * IIO features supported by the driver:
  *
  * Read-only raw channels:
- *   - illiminance_clear [lux]
- *   - illiminance_ir
+ *   - illuminance_clear [lux]
+ *   - illuminance_ir
  *   - proximity
  *
  * Triggered buffer:
- *   - illiminance_clear
- *   - illiminance_ir
+ *   - illuminance_clear
+ *   - illuminance_ir
  *   - proximity
  *
  * Events:
diff --git a/drivers/iio/light/hid-sensor-als.c b/drivers/iio/light/hid-sensor-als.c
index 621541fb10a..f34c94380b4 100644
--- a/drivers/iio/light/hid-sensor-als.c
+++ b/drivers/iio/light/hid-sensor-als.c
@@ -22,6 +22,7 @@
 #include <linux/interrupt.h>
 #include <linux/irq.h>
 #include <linux/slab.h>
+#include <linux/delay.h>
 #include <linux/hid-sensor-hub.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
@@ -37,6 +38,10 @@ struct als_state {
 	struct hid_sensor_common common_attributes;
 	struct hid_sensor_hub_attribute_info als_illum;
 	u32 illum;
+	int scale_pre_decml;
+	int scale_post_decml;
+	int scale_precision;
+	int value_offset;
 };
 
 /* Channel definitions */
@@ -45,6 +50,7 @@ static const struct iio_chan_spec als_channels[] = {
 		.type = IIO_INTENSITY,
 		.modified = 1,
 		.channel2 = IIO_MOD_LIGHT_BOTH,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
 		BIT(IIO_CHAN_INFO_SCALE) |
 		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
@@ -75,6 +81,7 @@ static int als_read_raw(struct iio_dev *indio_dev,
 	u32 address;
 	int ret;
 	int ret_type;
+	s32 poll_value;
 
 	*val = 0;
 	*val2 = 0;
@@ -90,24 +97,35 @@ static int als_read_raw(struct iio_dev *indio_dev,
 			report_id = -1;
 			break;
 		}
-		if (report_id >= 0)
+		if (report_id >= 0) {
+			poll_value = hid_sensor_read_poll_value(
+						&als_state->common_attributes);
+			if (poll_value < 0)
+				return -EINVAL;
+
+			hid_sensor_power_state(&als_state->common_attributes,
+						true);
+			msleep_interruptible(poll_value * 2);
+
 			*val = sensor_hub_input_attr_get_raw_value(
-				als_state->common_attributes.hsdev,
-				HID_USAGE_SENSOR_ALS, address,
-				report_id);
-		else {
+					als_state->common_attributes.hsdev,
+					HID_USAGE_SENSOR_ALS, address,
+					report_id);
+			hid_sensor_power_state(&als_state->common_attributes,
+						false);
+		} else {
 			*val = 0;
 			return -EINVAL;
 		}
 		ret_type = IIO_VAL_INT;
 		break;
 	case IIO_CHAN_INFO_SCALE:
-		*val = als_state->als_illum.units;
-		ret_type = IIO_VAL_INT;
+		*val = als_state->scale_pre_decml;
+		*val2 = als_state->scale_post_decml;
+		ret_type = als_state->scale_precision;
 		break;
 	case IIO_CHAN_INFO_OFFSET:
-		*val = hid_sensor_convert_exponent(
-				als_state->als_illum.unit_expo);
+		*val = als_state->value_offset;
 		ret_type = IIO_VAL_INT;
 		break;
 	case IIO_CHAN_INFO_SAMP_FREQ:
@@ -176,9 +194,8 @@ static int als_proc_event(struct hid_sensor_hub_device *hsdev,
 	struct iio_dev *indio_dev = platform_get_drvdata(priv);
 	struct als_state *als_state = iio_priv(indio_dev);
 
-	dev_dbg(&indio_dev->dev, "als_proc_event [%d]\n",
-				als_state->common_attributes.data_ready);
-	if (als_state->common_attributes.data_ready)
+	dev_dbg(&indio_dev->dev, "als_proc_event\n");
+	if (atomic_read(&als_state->common_attributes.data_ready))
 		hid_sensor_push_data(indio_dev,
 				&als_state->illum,
 				sizeof(als_state->illum));
@@ -229,6 +246,11 @@ static int als_parse_report(struct platform_device *pdev,
 	dev_dbg(&pdev->dev, "als %x:%x\n", st->als_illum.index,
 			st->als_illum.report_id);
 
+	st->scale_precision = hid_sensor_format_scale(
+				HID_USAGE_SENSOR_ALS,
+				&st->als_illum,
+				&st->scale_pre_decml, &st->scale_post_decml);
+
 	/* Set Sensitivity field ids, when there is no individual modifier */
 	if (st->common_attributes.sensitivity.index < 0) {
 		sensor_hub_input_get_attribute_info(hsdev,
@@ -296,7 +318,7 @@ static int hid_als_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
 		goto error_free_dev_mem;
 	}
-	als_state->common_attributes.data_ready = false;
+	atomic_set(&als_state->common_attributes.data_ready, 0);
 	ret = hid_sensor_setup_trigger(indio_dev, name,
 				&als_state->common_attributes);
 	if (ret < 0) {
diff --git a/drivers/iio/light/hid-sensor-prox.c b/drivers/iio/light/hid-sensor-prox.c
index 1894ab196f9..d203ef4d892 100644
--- a/drivers/iio/light/hid-sensor-prox.c
+++ b/drivers/iio/light/hid-sensor-prox.c
@@ -21,6 +21,7 @@
 #include <linux/interrupt.h>
 #include <linux/irq.h>
 #include <linux/slab.h>
+#include <linux/delay.h>
 #include <linux/hid-sensor-hub.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
@@ -75,6 +76,7 @@ static int prox_read_raw(struct iio_dev *indio_dev,
 	u32 address;
 	int ret;
 	int ret_type;
+	s32 poll_value;
 
 	*val = 0;
 	*val2 = 0;
@@ -90,12 +92,24 @@ static int prox_read_raw(struct iio_dev *indio_dev,
 			report_id = -1;
 			break;
 		}
-		if (report_id >= 0)
+		if (report_id >= 0) {
+			poll_value = hid_sensor_read_poll_value(
+					&prox_state->common_attributes);
+			if (poll_value < 0)
+				return -EINVAL;
+
+			hid_sensor_power_state(&prox_state->common_attributes,
+						true);
+
+			msleep_interruptible(poll_value * 2);
+
 			*val = sensor_hub_input_attr_get_raw_value(
 				prox_state->common_attributes.hsdev,
 				HID_USAGE_SENSOR_PROX, address,
 				report_id);
-		else {
+			hid_sensor_power_state(&prox_state->common_attributes,
+						false);
+		} else {
 			*val = 0;
 			return -EINVAL;
 		}
@@ -176,9 +190,8 @@ static int prox_proc_event(struct hid_sensor_hub_device *hsdev,
 	struct iio_dev *indio_dev = platform_get_drvdata(priv);
 	struct prox_state *prox_state = iio_priv(indio_dev);
 
-	dev_dbg(&indio_dev->dev, "prox_proc_event [%d]\n",
-				prox_state->common_attributes.data_ready);
-	if (prox_state->common_attributes.data_ready)
+	dev_dbg(&indio_dev->dev, "prox_proc_event\n");
+	if (atomic_read(&prox_state->common_attributes.data_ready))
 		hid_sensor_push_data(indio_dev,
 				&prox_state->human_presence,
 				sizeof(prox_state->human_presence));
@@ -297,7 +310,7 @@ static int hid_prox_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
 		goto error_free_dev_mem;
 	}
-	prox_state->common_attributes.data_ready = false;
+	atomic_set(&prox_state->common_attributes.data_ready, 0);
 	ret = hid_sensor_setup_trigger(indio_dev, name,
 				&prox_state->common_attributes);
 	if (ret) {
diff --git a/drivers/iio/magnetometer/Kconfig b/drivers/iio/magnetometer/Kconfig
index d86d226dcd6..05a364c543f 100644
--- a/drivers/iio/magnetometer/Kconfig
+++ b/drivers/iio/magnetometer/Kconfig
@@ -11,7 +11,8 @@ config AK8975
 	depends on GPIOLIB
 	help
 	  Say yes here to build support for Asahi Kasei AK8975 3-Axis
-	  Magnetometer.
+	  Magnetometer. This driver can also support AK8963, if i2c
+	  device name is identified as ak8963.
 
 	  To compile this driver as a module, choose M here: the module
 	  will be called ak8975.
diff --git a/drivers/iio/magnetometer/ak8975.c b/drivers/iio/magnetometer/ak8975.c
index 74866d1efd1..09ea5c481f4 100644
--- a/drivers/iio/magnetometer/ak8975.c
+++ b/drivers/iio/magnetometer/ak8975.c
@@ -31,6 +31,7 @@
 #include <linux/bitops.h>
 #include <linux/gpio.h>
 #include <linux/of_gpio.h>
+#include <linux/acpi.h>
 
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
@@ -85,7 +86,14 @@
 #define AK8975_MAX_CONVERSION_TIMEOUT	500
 #define AK8975_CONVERSION_DONE_POLL_TIME 10
 #define AK8975_DATA_READY_TIMEOUT	((100*HZ)/1000)
-#define RAW_TO_GAUSS(asa) ((((asa) + 128) * 3000) / 256)
+#define RAW_TO_GAUSS_8975(asa) ((((asa) + 128) * 3000) / 256)
+#define RAW_TO_GAUSS_8963(asa) ((((asa) + 128) * 6000) / 256)
+
+/* Compatible Asahi Kasei Compass parts */
+enum asahi_compass_chipset {
+	AK8975,
+	AK8963,
+};
 
 /*
  * Per-instance context data for the device.
@@ -101,6 +109,7 @@ struct ak8975_data {
 	int			eoc_irq;
 	wait_queue_head_t	data_ready_queue;
 	unsigned long		flags;
+	enum asahi_compass_chipset chipset;
 };
 
 static const int ak8975_index_to_reg[] = {
@@ -272,9 +281,21 @@ static int ak8975_setup(struct i2c_client *client)
  * Since ASA doesn't change, we cache the resultant scale factor into the
  * device context in ak8975_setup().
  */
-	data->raw_to_gauss[0] = RAW_TO_GAUSS(data->asa[0]);
-	data->raw_to_gauss[1] = RAW_TO_GAUSS(data->asa[1]);
-	data->raw_to_gauss[2] = RAW_TO_GAUSS(data->asa[2]);
+	if (data->chipset == AK8963) {
+		/*
+		 * H range is +-8190 and magnetometer range is +-4912.
+		 * So HuT using the above explanation for 8975,
+		 * 4912/8190 = ~ 6/10.
+		 * So the Hadj should use 6/10 instead of 3/10.
+		 */
+		data->raw_to_gauss[0] = RAW_TO_GAUSS_8963(data->asa[0]);
+		data->raw_to_gauss[1] = RAW_TO_GAUSS_8963(data->asa[1]);
+		data->raw_to_gauss[2] = RAW_TO_GAUSS_8963(data->asa[2]);
+	} else {
+		data->raw_to_gauss[0] = RAW_TO_GAUSS_8975(data->asa[0]);
+		data->raw_to_gauss[1] = RAW_TO_GAUSS_8975(data->asa[1]);
+		data->raw_to_gauss[2] = RAW_TO_GAUSS_8975(data->asa[2]);
+	}
 
 	return 0;
 }
@@ -455,6 +476,27 @@ static const struct iio_info ak8975_info = {
 	.driver_module = THIS_MODULE,
 };
 
+static const struct acpi_device_id ak_acpi_match[] = {
+	{"AK8975", AK8975},
+	{"AK8963", AK8963},
+	{"INVN6500", AK8963},
+	{ },
+};
+MODULE_DEVICE_TABLE(acpi, ak_acpi_match);
+
+static char *ak8975_match_acpi_device(struct device *dev,
+				enum asahi_compass_chipset *chipset)
+{
+	const struct acpi_device_id *id;
+
+	id = acpi_match_device(dev->driver->acpi_match_table, dev);
+	if (!id)
+		return NULL;
+	*chipset = (int)id->driver_data;
+
+	return (char *)dev_name(dev);
+}
+
 static int ak8975_probe(struct i2c_client *client,
 			const struct i2c_device_id *id)
 {
@@ -462,6 +504,7 @@ static int ak8975_probe(struct i2c_client *client,
 	struct iio_dev *indio_dev;
 	int eoc_gpio;
 	int err;
+	char *name = NULL;
 
 	/* Grab and set up the supplied GPIO. */
 	if (client->dev.platform_data)
@@ -499,6 +542,19 @@ static int ak8975_probe(struct i2c_client *client,
 	data->eoc_gpio = eoc_gpio;
 	data->eoc_irq = 0;
 
+	/* id will be NULL when enumerated via ACPI */
+	if (id) {
+		data->chipset =
+			(enum asahi_compass_chipset)(id->driver_data);
+		name = (char *) id->name;
+	} else if (ACPI_HANDLE(&client->dev))
+		name = ak8975_match_acpi_device(&client->dev, &data->chipset);
+	else {
+		err = -ENOSYS;
+		goto exit_free_iio;
+	}
+	dev_dbg(&client->dev, "Asahi compass chip %s\n", name);
+
 	/* Perform some basic start-of-day setup of the device. */
 	err = ak8975_setup(client);
 	if (err < 0) {
@@ -513,9 +569,8 @@ static int ak8975_probe(struct i2c_client *client,
 	indio_dev->channels = ak8975_channels;
 	indio_dev->num_channels = ARRAY_SIZE(ak8975_channels);
 	indio_dev->info = &ak8975_info;
-	indio_dev->name = id->name;
 	indio_dev->modes = INDIO_DIRECT_MODE;
-
+	indio_dev->name = name;
 	err = iio_device_register(indio_dev);
 	if (err < 0)
 		goto exit_free_iio;
@@ -552,7 +607,8 @@ static int ak8975_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id ak8975_id[] = {
-	{"ak8975", 0},
+	{"ak8975", AK8975},
+	{"ak8963", AK8963},
 	{}
 };
 
@@ -569,6 +625,7 @@ static struct i2c_driver ak8975_driver = {
 	.driver = {
 		.name	= "ak8975",
 		.of_match_table = ak8975_of_match,
+		.acpi_match_table = ACPI_PTR(ak_acpi_match),
 	},
 	.probe		= ak8975_probe,
 	.remove		= ak8975_remove,
diff --git a/drivers/iio/magnetometer/hid-sensor-magn-3d.c b/drivers/iio/magnetometer/hid-sensor-magn-3d.c
index 6d162b7e7af..41cf29e2a37 100644
--- a/drivers/iio/magnetometer/hid-sensor-magn-3d.c
+++ b/drivers/iio/magnetometer/hid-sensor-magn-3d.c
@@ -22,6 +22,7 @@
 #include <linux/interrupt.h>
 #include <linux/irq.h>
 #include <linux/slab.h>
+#include <linux/delay.h>
 #include <linux/hid-sensor-hub.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
@@ -42,6 +43,10 @@ struct magn_3d_state {
 	struct hid_sensor_common common_attributes;
 	struct hid_sensor_hub_attribute_info magn[MAGN_3D_CHANNEL_MAX];
 	u32 magn_val[MAGN_3D_CHANNEL_MAX];
+	int scale_pre_decml;
+	int scale_post_decml;
+	int scale_precision;
+	int value_offset;
 };
 
 static const u32 magn_3d_addresses[MAGN_3D_CHANNEL_MAX] = {
@@ -56,6 +61,7 @@ static const struct iio_chan_spec magn_3d_channels[] = {
 		.type = IIO_MAGN,
 		.modified = 1,
 		.channel2 = IIO_MOD_X,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
 		BIT(IIO_CHAN_INFO_SCALE) |
 		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
@@ -65,6 +71,7 @@ static const struct iio_chan_spec magn_3d_channels[] = {
 		.type = IIO_MAGN,
 		.modified = 1,
 		.channel2 = IIO_MOD_Y,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
 		BIT(IIO_CHAN_INFO_SCALE) |
 		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
@@ -74,6 +81,7 @@ static const struct iio_chan_spec magn_3d_channels[] = {
 		.type = IIO_MAGN,
 		.modified = 1,
 		.channel2 = IIO_MOD_Z,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
 		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
 		BIT(IIO_CHAN_INFO_SCALE) |
 		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
@@ -104,11 +112,20 @@ static int magn_3d_read_raw(struct iio_dev *indio_dev,
 	u32 address;
 	int ret;
 	int ret_type;
+	s32 poll_value;
 
 	*val = 0;
 	*val2 = 0;
 	switch (mask) {
 	case 0:
+		poll_value = hid_sensor_read_poll_value(
+					&magn_state->common_attributes);
+		if (poll_value < 0)
+				return -EINVAL;
+
+		hid_sensor_power_state(&magn_state->common_attributes, true);
+		msleep_interruptible(poll_value * 2);
+
 		report_id =
 			magn_state->magn[chan->scan_index].report_id;
 		address = magn_3d_addresses[chan->scan_index];
@@ -119,17 +136,20 @@ static int magn_3d_read_raw(struct iio_dev *indio_dev,
 				report_id);
 		else {
 			*val = 0;
+			hid_sensor_power_state(&magn_state->common_attributes,
+						false);
 			return -EINVAL;
 		}
+		hid_sensor_power_state(&magn_state->common_attributes, false);
 		ret_type = IIO_VAL_INT;
 		break;
 	case IIO_CHAN_INFO_SCALE:
-		*val = magn_state->magn[CHANNEL_SCAN_INDEX_X].units;
-		ret_type = IIO_VAL_INT;
+		*val = magn_state->scale_pre_decml;
+		*val2 = magn_state->scale_post_decml;
+		ret_type = magn_state->scale_precision;
 		break;
 	case IIO_CHAN_INFO_OFFSET:
-		*val = hid_sensor_convert_exponent(
-			magn_state->magn[CHANNEL_SCAN_INDEX_X].unit_expo);
+		*val = magn_state->value_offset;
 		ret_type = IIO_VAL_INT;
 		break;
 	case IIO_CHAN_INFO_SAMP_FREQ:
@@ -198,9 +218,8 @@ static int magn_3d_proc_event(struct hid_sensor_hub_device *hsdev,
 	struct iio_dev *indio_dev = platform_get_drvdata(priv);
 	struct magn_3d_state *magn_state = iio_priv(indio_dev);
 
-	dev_dbg(&indio_dev->dev, "magn_3d_proc_event [%d]\n",
-				magn_state->common_attributes.data_ready);
-	if (magn_state->common_attributes.data_ready)
+	dev_dbg(&indio_dev->dev, "magn_3d_proc_event\n");
+	if (atomic_read(&magn_state->common_attributes.data_ready))
 		hid_sensor_push_data(indio_dev,
 				magn_state->magn_val,
 				sizeof(magn_state->magn_val));
@@ -263,6 +282,11 @@ static int magn_3d_parse_report(struct platform_device *pdev,
 			st->magn[1].index, st->magn[1].report_id,
 			st->magn[2].index, st->magn[2].report_id);
 
+	st->scale_precision = hid_sensor_format_scale(
+				HID_USAGE_SENSOR_COMPASS_3D,
+				&st->magn[CHANNEL_SCAN_INDEX_X],
+				&st->scale_pre_decml, &st->scale_post_decml);
+
 	/* Set Sensitivity field ids, when there is no individual modifier */
 	if (st->common_attributes.sensitivity.index < 0) {
 		sensor_hub_input_get_attribute_info(hsdev,
@@ -334,7 +358,7 @@ static int hid_magn_3d_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
 		goto error_free_dev_mem;
 	}
-	magn_state->common_attributes.data_ready = false;
+	atomic_set(&magn_state->common_attributes.data_ready, 0);
 	ret = hid_sensor_setup_trigger(indio_dev, name,
 					&magn_state->common_attributes);
 	if (ret < 0) {
diff --git a/drivers/iio/magnetometer/mag3110.c b/drivers/iio/magnetometer/mag3110.c
index 8b77782474d..e3106b43ef4 100644
--- a/drivers/iio/magnetometer/mag3110.c
+++ b/drivers/iio/magnetometer/mag3110.c
@@ -199,6 +199,13 @@ static int mag3110_read_raw(struct iio_dev *indio_dev,
 		*val = mag3110_samp_freq[i][0];
 		*val2 = mag3110_samp_freq[i][1];
 		return IIO_VAL_INT_PLUS_MICRO;
+	case IIO_CHAN_INFO_CALIBBIAS:
+		ret = i2c_smbus_read_word_swapped(data->client,
+			MAG3110_OFF_X +	2 * chan->scan_index);
+		if (ret < 0)
+			return ret;
+		*val = sign_extend32(ret >> 1, 14);
+		return IIO_VAL_INT;
 	}
 	return -EINVAL;
 }
@@ -223,6 +230,11 @@ static int mag3110_write_raw(struct iio_dev *indio_dev,
 		data->ctrl_reg1 |= rate << MAG3110_CTRL_DR_SHIFT;
 		return i2c_smbus_write_byte_data(data->client,
 			MAG3110_CTRL_REG1, data->ctrl_reg1);
+	case IIO_CHAN_INFO_CALIBBIAS:
+		if (val < -10000 || val > 10000)
+			return -EINVAL;
+		return i2c_smbus_write_word_swapped(data->client,
+			MAG3110_OFF_X + 2 * chan->scan_index, val << 1);
 	default:
 		return -EINVAL;
 	}
@@ -260,7 +272,8 @@ done:
 	.type = IIO_MAGN, \
 	.modified = 1, \
 	.channel2 = IIO_MOD_##axis, \
-	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+		BIT(IIO_CHAN_INFO_CALIBBIAS), \
 	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SAMP_FREQ) | \
 		BIT(IIO_CHAN_INFO_SCALE), \
 	.scan_index = idx, \
@@ -338,14 +351,14 @@ static int mag3110_probe(struct i2c_client *client,
 	indio_dev->num_channels = ARRAY_SIZE(mag3110_channels);
 	indio_dev->available_scan_masks = mag3110_scan_masks;
 
-	data->ctrl_reg1 = MAG3110_CTRL_DR_DEFAULT;
+	data->ctrl_reg1 = MAG3110_CTRL_DR_DEFAULT << MAG3110_CTRL_DR_SHIFT;
 	ret = i2c_smbus_write_byte_data(client, MAG3110_CTRL_REG1,
 		data->ctrl_reg1);
 	if (ret < 0)
 		return ret;
 
 	ret = i2c_smbus_write_byte_data(client, MAG3110_CTRL_REG2,
-		MAG3110_CTRL_AUTO_MRST_EN | MAG3110_CTRL_RAW);
+		MAG3110_CTRL_AUTO_MRST_EN);
 	if (ret < 0)
 		return ret;
 
diff --git a/drivers/iio/magnetometer/st_magn_core.c b/drivers/iio/magnetometer/st_magn_core.c
index 52bbcfa1e07..240a21dd0c6 100644
--- a/drivers/iio/magnetometer/st_magn_core.c
+++ b/drivers/iio/magnetometer/st_magn_core.c
@@ -355,6 +355,8 @@ int st_magn_common_probe(struct iio_dev *indio_dev,
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->info = &magn_info;
 
+	st_sensors_power_enable(indio_dev);
+
 	err = st_sensors_check_device_support(indio_dev,
 				ARRAY_SIZE(st_magn_sensors), st_magn_sensors);
 	if (err < 0)
@@ -387,6 +389,9 @@ int st_magn_common_probe(struct iio_dev *indio_dev,
 	if (err)
 		goto st_magn_device_register_error;
 
+	dev_info(&indio_dev->dev, "registered magnetometer %s\n",
+		 indio_dev->name);
+
 	return 0;
 
 st_magn_device_register_error:
@@ -403,6 +408,8 @@ void st_magn_common_remove(struct iio_dev *indio_dev)
 {
 	struct st_sensor_data *mdata = iio_priv(indio_dev);
 
+	st_sensors_power_disable(indio_dev);
+
 	iio_device_unregister(indio_dev);
 	if (mdata->get_irq_data_ready(indio_dev) > 0)
 		st_sensors_deallocate_trigger(indio_dev);
diff --git a/drivers/iio/orientation/Kconfig b/drivers/iio/orientation/Kconfig
index 58c62c837e1..e3aa1e58d92 100644
--- a/drivers/iio/orientation/Kconfig
+++ b/drivers/iio/orientation/Kconfig
@@ -16,4 +16,16 @@ config HID_SENSOR_INCLINOMETER_3D
 	  Say yes here to build support for the HID SENSOR
 	  Inclinometer 3D.
 
+config HID_SENSOR_DEVICE_ROTATION
+	depends on HID_SENSOR_HUB
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	select HID_SENSOR_IIO_COMMON
+	select HID_SENSOR_IIO_TRIGGER
+	tristate "HID Device Rotation"
+	help
+	  Say yes here to build support for the HID SENSOR
+	  device rotation. The output of a device rotation sensor
+	  is presented using quaternion format.
+
 endmenu
diff --git a/drivers/iio/orientation/Makefile b/drivers/iio/orientation/Makefile
index 2c97572ee91..4734dabbde1 100644
--- a/drivers/iio/orientation/Makefile
+++ b/drivers/iio/orientation/Makefile
@@ -4,3 +4,4 @@
 
 # When adding new entries keep the list in alphabetical order
 obj-$(CONFIG_HID_SENSOR_INCLINOMETER_3D) += hid-sensor-incl-3d.o
+obj-$(CONFIG_HID_SENSOR_DEVICE_ROTATION) += hid-sensor-rotation.o
diff --git a/drivers/iio/orientation/hid-sensor-incl-3d.c b/drivers/iio/orientation/hid-sensor-incl-3d.c
index 070feab08fa..2478f6c2ef2 100644
--- a/drivers/iio/orientation/hid-sensor-incl-3d.c
+++ b/drivers/iio/orientation/hid-sensor-incl-3d.c
@@ -22,6 +22,7 @@
 #include <linux/interrupt.h>
 #include <linux/irq.h>
 #include <linux/slab.h>
+#include <linux/delay.h>
 #include <linux/hid-sensor-hub.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
@@ -42,6 +43,10 @@ struct incl_3d_state {
 	struct hid_sensor_common common_attributes;
 	struct hid_sensor_hub_attribute_info incl[INCLI_3D_CHANNEL_MAX];
 	u32 incl_val[INCLI_3D_CHANNEL_MAX];
+	int scale_pre_decml;
+	int scale_post_decml;
+	int scale_precision;
+	int value_offset;
 };
 
 static const u32 incl_3d_addresses[INCLI_3D_CHANNEL_MAX] = {
@@ -106,11 +111,20 @@ static int incl_3d_read_raw(struct iio_dev *indio_dev,
 	int report_id = -1;
 	u32 address;
 	int ret_type;
+	s32 poll_value;
 
 	*val = 0;
 	*val2 = 0;
 	switch (mask) {
 	case IIO_CHAN_INFO_RAW:
+		poll_value = hid_sensor_read_poll_value(
+					&incl_state->common_attributes);
+		if (poll_value < 0)
+			return -EINVAL;
+
+		hid_sensor_power_state(&incl_state->common_attributes, true);
+		msleep_interruptible(poll_value * 2);
+
 		report_id =
 			incl_state->incl[chan->scan_index].report_id;
 		address = incl_3d_addresses[chan->scan_index];
@@ -120,17 +134,20 @@ static int incl_3d_read_raw(struct iio_dev *indio_dev,
 				HID_USAGE_SENSOR_INCLINOMETER_3D, address,
 				report_id);
 		else {
+			hid_sensor_power_state(&incl_state->common_attributes,
+						false);
 			return -EINVAL;
 		}
+		hid_sensor_power_state(&incl_state->common_attributes, false);
 		ret_type = IIO_VAL_INT;
 		break;
 	case IIO_CHAN_INFO_SCALE:
-		*val = incl_state->incl[CHANNEL_SCAN_INDEX_X].units;
-		ret_type = IIO_VAL_INT;
+		*val = incl_state->scale_pre_decml;
+		*val2 = incl_state->scale_post_decml;
+		ret_type = incl_state->scale_precision;
 		break;
 	case IIO_CHAN_INFO_OFFSET:
-		*val = hid_sensor_convert_exponent(
-			incl_state->incl[CHANNEL_SCAN_INDEX_X].unit_expo);
+		*val = incl_state->value_offset;
 		ret_type = IIO_VAL_INT;
 		break;
 	case IIO_CHAN_INFO_SAMP_FREQ:
@@ -196,9 +213,8 @@ static int incl_3d_proc_event(struct hid_sensor_hub_device *hsdev,
 	struct iio_dev *indio_dev = platform_get_drvdata(priv);
 	struct incl_3d_state *incl_state = iio_priv(indio_dev);
 
-	dev_dbg(&indio_dev->dev, "incl_3d_proc_event [%d]\n",
-				incl_state->common_attributes.data_ready);
-	if (incl_state->common_attributes.data_ready)
+	dev_dbg(&indio_dev->dev, "incl_3d_proc_event\n");
+	if (atomic_read(&incl_state->common_attributes.data_ready))
 		hid_sensor_push_data(indio_dev,
 				(u8 *)incl_state->incl_val,
 				sizeof(incl_state->incl_val));
@@ -279,6 +295,11 @@ static int incl_3d_parse_report(struct platform_device *pdev,
 			st->incl[1].index, st->incl[1].report_id,
 			st->incl[2].index, st->incl[2].report_id);
 
+	st->scale_precision = hid_sensor_format_scale(
+				HID_USAGE_SENSOR_INCLINOMETER_3D,
+				&st->incl[CHANNEL_SCAN_INDEX_X],
+				&st->scale_pre_decml, &st->scale_post_decml);
+
 	/* Set Sensitivity field ids, when there is no individual modifier */
 	if (st->common_attributes.sensitivity.index < 0) {
 		sensor_hub_input_get_attribute_info(hsdev,
@@ -349,7 +370,7 @@ static int hid_incl_3d_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
 		goto error_free_dev_mem;
 	}
-	incl_state->common_attributes.data_ready = false;
+	atomic_set(&incl_state->common_attributes.data_ready, 0);
 	ret = hid_sensor_setup_trigger(indio_dev, name,
 					&incl_state->common_attributes);
 	if (ret) {
diff --git a/drivers/iio/orientation/hid-sensor-rotation.c b/drivers/iio/orientation/hid-sensor-rotation.c
new file mode 100644
index 00000000000..dccf848e8b0
--- /dev/null
+++ b/drivers/iio/orientation/hid-sensor-rotation.c
@@ -0,0 +1,346 @@
+/*
+ * HID Sensors Driver
+ * Copyright (c) 2014, 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/device.h>
+#include <linux/platform_device.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/slab.h>
+#include <linux/hid-sensor-hub.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>
+#include "../common/hid-sensors/hid-sensor-trigger.h"
+
+struct dev_rot_state {
+	struct hid_sensor_hub_callbacks callbacks;
+	struct hid_sensor_common common_attributes;
+	struct hid_sensor_hub_attribute_info quaternion;
+	u32 sampled_vals[4];
+};
+
+/* Channel definitions */
+static const struct iio_chan_spec dev_rot_channels[] = {
+	{
+		.type = IIO_ROT,
+		.modified = 1,
+		.channel2 = IIO_MOD_QUATERNION,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SAMP_FREQ) |
+					BIT(IIO_CHAN_INFO_HYSTERESIS)
+	}
+};
+
+/* Adjust channel real bits based on report descriptor */
+static void dev_rot_adjust_channel_bit_mask(struct iio_chan_spec *chan,
+						int size)
+{
+	chan->scan_type.sign = 's';
+	/* Real storage bits will change based on the report desc. */
+	chan->scan_type.realbits = size * 8;
+	/* Maximum size of a sample to capture is u32 */
+	chan->scan_type.storagebits = sizeof(u32) * 8;
+	chan->scan_type.repeat = 4;
+}
+
+/* Channel read_raw handler */
+static int dev_rot_read_raw(struct iio_dev *indio_dev,
+				struct iio_chan_spec const *chan,
+				int size, int *vals, int *val_len,
+				long mask)
+{
+	struct dev_rot_state *rot_state = iio_priv(indio_dev);
+	int ret_type;
+	int i;
+
+	vals[0] = 0;
+	vals[1] = 0;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		if (size >= 4) {
+			for (i = 0; i < 4; ++i)
+				vals[i] = rot_state->sampled_vals[i];
+			ret_type = IIO_VAL_INT_MULTIPLE;
+			*val_len =  4;
+		} else
+			ret_type = -EINVAL;
+		break;
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		ret_type = hid_sensor_read_samp_freq_value(
+			&rot_state->common_attributes, &vals[0], &vals[1]);
+		break;
+	case IIO_CHAN_INFO_HYSTERESIS:
+		ret_type = hid_sensor_read_raw_hyst_value(
+			&rot_state->common_attributes, &vals[0], &vals[1]);
+		break;
+	default:
+		ret_type = -EINVAL;
+		break;
+	}
+
+	return ret_type;
+}
+
+/* Channel write_raw handler */
+static int dev_rot_write_raw(struct iio_dev *indio_dev,
+			       struct iio_chan_spec const *chan,
+			       int val,
+			       int val2,
+			       long mask)
+{
+	struct dev_rot_state *rot_state = iio_priv(indio_dev);
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		ret = hid_sensor_write_samp_freq_value(
+				&rot_state->common_attributes, val, val2);
+		break;
+	case IIO_CHAN_INFO_HYSTERESIS:
+		ret = hid_sensor_write_raw_hyst_value(
+				&rot_state->common_attributes, val, val2);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static const struct iio_info dev_rot_info = {
+	.driver_module = THIS_MODULE,
+	.read_raw_multi = &dev_rot_read_raw,
+	.write_raw = &dev_rot_write_raw,
+};
+
+/* Function to push data to buffer */
+static void hid_sensor_push_data(struct iio_dev *indio_dev, u8 *data, int len)
+{
+	dev_dbg(&indio_dev->dev, "hid_sensor_push_data >>\n");
+	iio_push_to_buffers(indio_dev, (u8 *)data);
+	dev_dbg(&indio_dev->dev, "hid_sensor_push_data <<\n");
+
+}
+
+/* Callback handler to send event after all samples are received and captured */
+static int dev_rot_proc_event(struct hid_sensor_hub_device *hsdev,
+				unsigned usage_id,
+				void *priv)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(priv);
+	struct dev_rot_state *rot_state = iio_priv(indio_dev);
+
+	dev_dbg(&indio_dev->dev, "dev_rot_proc_event\n");
+	if (atomic_read(&rot_state->common_attributes.data_ready))
+		hid_sensor_push_data(indio_dev,
+				(u8 *)rot_state->sampled_vals,
+				sizeof(rot_state->sampled_vals));
+
+	return 0;
+}
+
+/* Capture samples in local storage */
+static int dev_rot_capture_sample(struct hid_sensor_hub_device *hsdev,
+				unsigned usage_id,
+				size_t raw_len, char *raw_data,
+				void *priv)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(priv);
+	struct dev_rot_state *rot_state = iio_priv(indio_dev);
+
+	if (usage_id == HID_USAGE_SENSOR_ORIENT_QUATERNION) {
+		memcpy(rot_state->sampled_vals, raw_data,
+					sizeof(rot_state->sampled_vals));
+		dev_dbg(&indio_dev->dev, "Recd Quat len:%zu::%zu\n", raw_len,
+					sizeof(rot_state->sampled_vals));
+	}
+
+	return 0;
+}
+
+/* Parse report which is specific to an usage id*/
+static int dev_rot_parse_report(struct platform_device *pdev,
+				struct hid_sensor_hub_device *hsdev,
+				struct iio_chan_spec *channels,
+				unsigned usage_id,
+				struct dev_rot_state *st)
+{
+	int ret;
+
+	ret = sensor_hub_input_get_attribute_info(hsdev,
+				HID_INPUT_REPORT,
+				usage_id,
+				HID_USAGE_SENSOR_ORIENT_QUATERNION,
+				&st->quaternion);
+	if (ret)
+		return ret;
+
+	dev_rot_adjust_channel_bit_mask(&channels[0],
+		st->quaternion.size / 4);
+
+	dev_dbg(&pdev->dev, "dev_rot %x:%x\n", st->quaternion.index,
+		st->quaternion.report_id);
+
+	dev_dbg(&pdev->dev, "dev_rot: attrib size %d\n",
+				st->quaternion.size);
+
+	/* Set Sensitivity field ids, when there is no individual modifier */
+	if (st->common_attributes.sensitivity.index < 0) {
+		sensor_hub_input_get_attribute_info(hsdev,
+			HID_FEATURE_REPORT, usage_id,
+			HID_USAGE_SENSOR_DATA_MOD_CHANGE_SENSITIVITY_ABS |
+			HID_USAGE_SENSOR_DATA_ORIENTATION,
+			&st->common_attributes.sensitivity);
+		dev_dbg(&pdev->dev, "Sensitivity index:report %d:%d\n",
+			st->common_attributes.sensitivity.index,
+			st->common_attributes.sensitivity.report_id);
+	}
+
+	return 0;
+}
+
+/* Function to initialize the processing for usage id */
+static int hid_dev_rot_probe(struct platform_device *pdev)
+{
+	int ret;
+	static char *name = "dev_rotation";
+	struct iio_dev *indio_dev;
+	struct dev_rot_state *rot_state;
+	struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data;
+	struct iio_chan_spec *channels;
+
+	indio_dev = devm_iio_device_alloc(&pdev->dev,
+					  sizeof(struct dev_rot_state));
+	if (indio_dev == NULL)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, indio_dev);
+
+	rot_state = iio_priv(indio_dev);
+	rot_state->common_attributes.hsdev = hsdev;
+	rot_state->common_attributes.pdev = pdev;
+
+	ret = hid_sensor_parse_common_attributes(hsdev,
+				HID_USAGE_SENSOR_DEVICE_ORIENTATION,
+				&rot_state->common_attributes);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to setup common attributes\n");
+		return ret;
+	}
+
+	channels = devm_kmemdup(&pdev->dev, dev_rot_channels,
+					sizeof(dev_rot_channels), GFP_KERNEL);
+	if (!channels) {
+		dev_err(&pdev->dev, "failed to duplicate channels\n");
+		return -ENOMEM;
+	}
+
+	ret = dev_rot_parse_report(pdev, hsdev, channels,
+			HID_USAGE_SENSOR_DEVICE_ORIENTATION, rot_state);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to setup attributes\n");
+		return ret;
+	}
+
+	indio_dev->channels = channels;
+	indio_dev->num_channels = ARRAY_SIZE(dev_rot_channels);
+	indio_dev->dev.parent = &pdev->dev;
+	indio_dev->info = &dev_rot_info;
+	indio_dev->name = name;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	ret = iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
+		NULL, NULL);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
+		return ret;
+	}
+	atomic_set(&rot_state->common_attributes.data_ready, 0);
+	ret = hid_sensor_setup_trigger(indio_dev, name,
+					&rot_state->common_attributes);
+	if (ret) {
+		dev_err(&pdev->dev, "trigger setup failed\n");
+		goto error_unreg_buffer_funcs;
+	}
+
+	ret = iio_device_register(indio_dev);
+	if (ret) {
+		dev_err(&pdev->dev, "device register failed\n");
+		goto error_remove_trigger;
+	}
+
+	rot_state->callbacks.send_event = dev_rot_proc_event;
+	rot_state->callbacks.capture_sample = dev_rot_capture_sample;
+	rot_state->callbacks.pdev = pdev;
+	ret = sensor_hub_register_callback(hsdev,
+					HID_USAGE_SENSOR_DEVICE_ORIENTATION,
+					&rot_state->callbacks);
+	if (ret) {
+		dev_err(&pdev->dev, "callback reg failed\n");
+		goto error_iio_unreg;
+	}
+
+	return 0;
+
+error_iio_unreg:
+	iio_device_unregister(indio_dev);
+error_remove_trigger:
+	hid_sensor_remove_trigger(&rot_state->common_attributes);
+error_unreg_buffer_funcs:
+	iio_triggered_buffer_cleanup(indio_dev);
+	return ret;
+}
+
+/* Function to deinitialize the processing for usage id */
+static int hid_dev_rot_remove(struct platform_device *pdev)
+{
+	struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data;
+	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
+	struct dev_rot_state *rot_state = iio_priv(indio_dev);
+
+	sensor_hub_remove_callback(hsdev, HID_USAGE_SENSOR_DEVICE_ORIENTATION);
+	iio_device_unregister(indio_dev);
+	hid_sensor_remove_trigger(&rot_state->common_attributes);
+	iio_triggered_buffer_cleanup(indio_dev);
+
+	return 0;
+}
+
+static struct platform_device_id hid_dev_rot_ids[] = {
+	{
+		/* Format: HID-SENSOR-usage_id_in_hex_lowercase */
+		.name = "HID-SENSOR-20008a",
+	},
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(platform, hid_dev_rot_ids);
+
+static struct platform_driver hid_dev_rot_platform_driver = {
+	.id_table = hid_dev_rot_ids,
+	.driver = {
+		.name	= KBUILD_MODNAME,
+		.owner	= THIS_MODULE,
+	},
+	.probe		= hid_dev_rot_probe,
+	.remove		= hid_dev_rot_remove,
+};
+module_platform_driver(hid_dev_rot_platform_driver);
+
+MODULE_DESCRIPTION("HID Sensor Device Rotation");
+MODULE_AUTHOR("Srinivas Pandruvada <srinivas.pandruvada@linux.intel.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/iio/pressure/Kconfig b/drivers/iio/pressure/Kconfig
index d88ff17fedb..ffac8ac1efc 100644
--- a/drivers/iio/pressure/Kconfig
+++ b/drivers/iio/pressure/Kconfig
@@ -19,6 +19,16 @@ config HID_SENSOR_PRESS
           To compile this driver as a module, choose M here: the module
           will be called hid-sensor-press.
 
+config MPL115
+	tristate "Freescale MPL115A2 pressure sensor driver"
+	depends on I2C
+	help
+	  Say yes here to build support for the Freescale MPL115A2
+	  pressure sensor connected via I2C.
+
+          To compile this driver as a module, choose M here: the module
+          will be called mpl115.
+
 config MPL3115
 	tristate "Freescale MPL3115A2 pressure sensor driver"
 	depends on I2C
diff --git a/drivers/iio/pressure/Makefile b/drivers/iio/pressure/Makefile
index 4a57bf65b04..c53d2500737 100644
--- a/drivers/iio/pressure/Makefile
+++ b/drivers/iio/pressure/Makefile
@@ -4,6 +4,7 @@
 
 # When adding new entries keep the list in alphabetical order
 obj-$(CONFIG_HID_SENSOR_PRESS)   += hid-sensor-press.o
+obj-$(CONFIG_MPL115) += mpl115.o
 obj-$(CONFIG_MPL3115) += mpl3115.o
 obj-$(CONFIG_IIO_ST_PRESS) += st_pressure.o
 st_pressure-y := st_pressure_core.o
diff --git a/drivers/iio/pressure/hid-sensor-press.c b/drivers/iio/pressure/hid-sensor-press.c
index e0e6409aa94..1cd190c7378 100644
--- a/drivers/iio/pressure/hid-sensor-press.c
+++ b/drivers/iio/pressure/hid-sensor-press.c
@@ -21,6 +21,7 @@
 #include <linux/interrupt.h>
 #include <linux/irq.h>
 #include <linux/slab.h>
+#include <linux/delay.h>
 #include <linux/hid-sensor-hub.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
@@ -36,6 +37,10 @@ struct press_state {
 	struct hid_sensor_common common_attributes;
 	struct hid_sensor_hub_attribute_info press_attr;
 	u32 press_data;
+	int scale_pre_decml;
+	int scale_post_decml;
+	int scale_precision;
+	int value_offset;
 };
 
 /* Channel definitions */
@@ -75,6 +80,7 @@ static int press_read_raw(struct iio_dev *indio_dev,
 	u32 address;
 	int ret;
 	int ret_type;
+	s32 poll_value;
 
 	*val = 0;
 	*val2 = 0;
@@ -90,24 +96,35 @@ static int press_read_raw(struct iio_dev *indio_dev,
 			report_id = -1;
 			break;
 		}
-		if (report_id >= 0)
+		if (report_id >= 0) {
+			poll_value = hid_sensor_read_poll_value(
+					&press_state->common_attributes);
+			if (poll_value < 0)
+				return -EINVAL;
+			hid_sensor_power_state(&press_state->common_attributes,
+						true);
+
+			msleep_interruptible(poll_value * 2);
+
 			*val = sensor_hub_input_attr_get_raw_value(
 				press_state->common_attributes.hsdev,
 				HID_USAGE_SENSOR_PRESSURE, address,
 				report_id);
-		else {
+			hid_sensor_power_state(&press_state->common_attributes,
+						false);
+		} else {
 			*val = 0;
 			return -EINVAL;
 		}
 		ret_type = IIO_VAL_INT;
 		break;
 	case IIO_CHAN_INFO_SCALE:
-		*val = press_state->press_attr.units;
-		ret_type = IIO_VAL_INT;
+		*val = press_state->scale_pre_decml;
+		*val2 = press_state->scale_post_decml;
+		ret_type = press_state->scale_precision;
 		break;
 	case IIO_CHAN_INFO_OFFSET:
-		*val = hid_sensor_convert_exponent(
-				press_state->press_attr.unit_expo);
+		*val = press_state->value_offset;
 		ret_type = IIO_VAL_INT;
 		break;
 	case IIO_CHAN_INFO_SAMP_FREQ:
@@ -176,9 +193,8 @@ static int press_proc_event(struct hid_sensor_hub_device *hsdev,
 	struct iio_dev *indio_dev = platform_get_drvdata(priv);
 	struct press_state *press_state = iio_priv(indio_dev);
 
-	dev_dbg(&indio_dev->dev, "press_proc_event [%d]\n",
-				press_state->common_attributes.data_ready);
-	if (press_state->common_attributes.data_ready)
+	dev_dbg(&indio_dev->dev, "press_proc_event\n");
+	if (atomic_read(&press_state->common_attributes.data_ready))
 		hid_sensor_push_data(indio_dev,
 				&press_state->press_data,
 				sizeof(press_state->press_data));
@@ -229,6 +245,11 @@ static int press_parse_report(struct platform_device *pdev,
 	dev_dbg(&pdev->dev, "press %x:%x\n", st->press_attr.index,
 			st->press_attr.report_id);
 
+	st->scale_precision = hid_sensor_format_scale(
+				HID_USAGE_SENSOR_PRESSURE,
+				&st->press_attr,
+				&st->scale_pre_decml, &st->scale_post_decml);
+
 	/* Set Sensitivity field ids, when there is no individual modifier */
 	if (st->common_attributes.sensitivity.index < 0) {
 		sensor_hub_input_get_attribute_info(hsdev,
@@ -298,7 +319,7 @@ static int hid_press_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
 		goto error_free_dev_mem;
 	}
-	press_state->common_attributes.data_ready = false;
+	atomic_set(&press_state->common_attributes.data_ready, 0);
 	ret = hid_sensor_setup_trigger(indio_dev, name,
 				&press_state->common_attributes);
 	if (ret) {
diff --git a/drivers/iio/pressure/mpl115.c b/drivers/iio/pressure/mpl115.c
new file mode 100644
index 00000000000..f5ecd6e19f5
--- /dev/null
+++ b/drivers/iio/pressure/mpl115.c
@@ -0,0 +1,211 @@
+/*
+ * mpl115.c - Support for Freescale MPL115A2 pressure/temperature sensor
+ *
+ * Copyright (c) 2014 Peter Meerwald <pmeerw@pmeerw.net>
+ *
+ * This file is subject to the terms and conditions of version 2 of
+ * the GNU General Public License.  See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * (7-bit I2C slave address 0x60)
+ *
+ * TODO: shutdown pin
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/iio/iio.h>
+#include <linux/delay.h>
+
+#define MPL115_PADC 0x00 /* pressure ADC output value, MSB first, 10 bit */
+#define MPL115_TADC 0x02 /* temperature ADC output value, MSB first, 10 bit */
+#define MPL115_A0 0x04 /* 12 bit integer, 3 bit fraction */
+#define MPL115_B1 0x06 /* 2 bit integer, 13 bit fraction */
+#define MPL115_B2 0x08 /* 1 bit integer, 14 bit fraction */
+#define MPL115_C12 0x0a /* 0 bit integer, 13 bit fraction */
+#define MPL115_CONVERT 0x12 /* convert temperature and pressure */
+
+struct mpl115_data {
+	struct i2c_client *client;
+	struct mutex lock;
+	s16 a0;
+	s16 b1, b2;
+	s16 c12;
+};
+
+static int mpl115_request(struct mpl115_data *data)
+{
+	int ret = i2c_smbus_write_byte_data(data->client, MPL115_CONVERT, 0);
+	if (ret < 0)
+		return ret;
+
+	usleep_range(3000, 4000);
+
+	return 0;
+}
+
+static int mpl115_comp_pressure(struct mpl115_data *data, int *val, int *val2)
+{
+	int ret;
+	u16 padc, tadc;
+	int a1, y1, pcomp;
+	unsigned kpa;
+
+	mutex_lock(&data->lock);
+	ret = mpl115_request(data);
+	if (ret < 0)
+		goto done;
+
+	ret = i2c_smbus_read_word_swapped(data->client, MPL115_PADC);
+	if (ret < 0)
+		goto done;
+	padc = ret >> 6;
+
+	ret = i2c_smbus_read_word_swapped(data->client, MPL115_TADC);
+	if (ret < 0)
+		goto done;
+	tadc = ret >> 6;
+
+	/* see Freescale AN3785 */
+	a1 = data->b1 + ((data->c12 * tadc) >> 11);
+	y1 = (data->a0 << 10) + a1 * padc;
+
+	/* compensated pressure with 4 fractional bits */
+	pcomp = (y1 + ((data->b2 * (int) tadc) >> 1)) >> 9;
+
+	kpa = pcomp * (115 - 50) / 1023 + (50 << 4);
+	*val = kpa >> 4;
+	*val2 = (kpa & 15) * (1000000 >> 4);
+done:
+	mutex_unlock(&data->lock);
+	return ret;
+}
+
+static int mpl115_read_temp(struct mpl115_data *data)
+{
+	int ret;
+
+	mutex_lock(&data->lock);
+	ret = mpl115_request(data);
+	if (ret < 0)
+		goto done;
+	ret = i2c_smbus_read_word_swapped(data->client, MPL115_TADC);
+done:
+	mutex_unlock(&data->lock);
+	return ret;
+}
+
+static int mpl115_read_raw(struct iio_dev *indio_dev,
+			    struct iio_chan_spec const *chan,
+			    int *val, int *val2, long mask)
+{
+	struct mpl115_data *data = iio_priv(indio_dev);
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_PROCESSED:
+		ret = mpl115_comp_pressure(data, val, val2);
+		if (ret < 0)
+			return ret;
+		return IIO_VAL_INT_PLUS_MICRO;
+	case IIO_CHAN_INFO_RAW:
+		/* temperature -5.35 C / LSB, 472 LSB is 25 C */
+		ret = mpl115_read_temp(data);
+		if (ret < 0)
+			return ret;
+		*val = ret >> 6;
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_OFFSET:
+		*val = 605;
+		*val2 = 750000;
+		return IIO_VAL_INT_PLUS_MICRO;
+	case IIO_CHAN_INFO_SCALE:
+		*val = -186;
+		*val2 = 915888;
+		return IIO_VAL_INT_PLUS_MICRO;
+	}
+	return -EINVAL;
+}
+
+static const struct iio_chan_spec mpl115_channels[] = {
+	{
+		.type = IIO_PRESSURE,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED),
+	},
+	{
+		.type = IIO_TEMP,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+			BIT(IIO_CHAN_INFO_OFFSET) | BIT(IIO_CHAN_INFO_SCALE),
+	},
+};
+
+static const struct iio_info mpl115_info = {
+	.read_raw = &mpl115_read_raw,
+	.driver_module = THIS_MODULE,
+};
+
+static int mpl115_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct mpl115_data *data;
+	struct iio_dev *indio_dev;
+	int ret;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_WORD_DATA))
+		return -ENODEV;
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	data = iio_priv(indio_dev);
+	data->client = client;
+	mutex_init(&data->lock);
+
+	i2c_set_clientdata(client, indio_dev);
+	indio_dev->info = &mpl115_info;
+	indio_dev->name = id->name;
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = mpl115_channels;
+	indio_dev->num_channels = ARRAY_SIZE(mpl115_channels);
+
+	ret = i2c_smbus_read_word_swapped(data->client, MPL115_A0);
+	if (ret < 0)
+		return ret;
+	data->a0 = ret;
+	ret = i2c_smbus_read_word_swapped(data->client, MPL115_B1);
+	if (ret < 0)
+		return ret;
+	data->b1 = ret;
+	ret = i2c_smbus_read_word_swapped(data->client, MPL115_B2);
+	if (ret < 0)
+		return ret;
+	data->b2 = ret;
+	ret = i2c_smbus_read_word_swapped(data->client, MPL115_C12);
+	if (ret < 0)
+		return ret;
+	data->c12 = ret;
+
+	return devm_iio_device_register(&client->dev, indio_dev);
+}
+
+static const struct i2c_device_id mpl115_id[] = {
+	{ "mpl115", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, mpl115_id);
+
+static struct i2c_driver mpl115_driver = {
+	.driver = {
+		.name	= "mpl115",
+	},
+	.probe = mpl115_probe,
+	.id_table = mpl115_id,
+};
+module_i2c_driver(mpl115_driver);
+
+MODULE_AUTHOR("Peter Meerwald <pmeerw@pmeerw.net>");
+MODULE_DESCRIPTION("Freescale MPL115 pressure/temperature driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/iio/pressure/st_pressure_core.c b/drivers/iio/pressure/st_pressure_core.c
index 7418768ed49..cd7e01f3a93 100644
--- a/drivers/iio/pressure/st_pressure_core.c
+++ b/drivers/iio/pressure/st_pressure_core.c
@@ -23,7 +23,6 @@
 #include <linux/iio/sysfs.h>
 #include <linux/iio/trigger.h>
 #include <linux/iio/buffer.h>
-#include <linux/regulator/consumer.h>
 #include <asm/unaligned.h>
 
 #include <linux/iio/common/st_sensors.h>
@@ -387,40 +386,6 @@ static const struct iio_trigger_ops st_press_trigger_ops = {
 #define ST_PRESS_TRIGGER_OPS NULL
 #endif
 
-static void st_press_power_enable(struct iio_dev *indio_dev)
-{
-	struct st_sensor_data *pdata = iio_priv(indio_dev);
-	int err;
-
-	/* Regulators not mandatory, but if requested we should enable them. */
-	pdata->vdd = devm_regulator_get_optional(&indio_dev->dev, "vdd");
-	if (!IS_ERR(pdata->vdd)) {
-		err = regulator_enable(pdata->vdd);
-		if (err != 0)
-			dev_warn(&indio_dev->dev,
-				 "Failed to enable specified Vdd supply\n");
-	}
-
-	pdata->vdd_io = devm_regulator_get_optional(&indio_dev->dev, "vddio");
-	if (!IS_ERR(pdata->vdd_io)) {
-		err = regulator_enable(pdata->vdd_io);
-		if (err != 0)
-			dev_warn(&indio_dev->dev,
-				 "Failed to enable specified Vdd_IO supply\n");
-	}
-}
-
-static void st_press_power_disable(struct iio_dev *indio_dev)
-{
-	struct st_sensor_data *pdata = iio_priv(indio_dev);
-
-	if (!IS_ERR(pdata->vdd))
-		regulator_disable(pdata->vdd);
-
-	if (!IS_ERR(pdata->vdd_io))
-		regulator_disable(pdata->vdd_io);
-}
-
 int st_press_common_probe(struct iio_dev *indio_dev,
 				struct st_sensors_platform_data *plat_data)
 {
@@ -431,7 +396,7 @@ int st_press_common_probe(struct iio_dev *indio_dev,
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->info = &press_info;
 
-	st_press_power_enable(indio_dev);
+	st_sensors_power_enable(indio_dev);
 
 	err = st_sensors_check_device_support(indio_dev,
 					      ARRAY_SIZE(st_press_sensors),
@@ -474,6 +439,9 @@ int st_press_common_probe(struct iio_dev *indio_dev,
 	if (err)
 		goto st_press_device_register_error;
 
+	dev_info(&indio_dev->dev, "registered pressure sensor %s\n",
+		 indio_dev->name);
+
 	return err;
 
 st_press_device_register_error:
@@ -490,7 +458,7 @@ void st_press_common_remove(struct iio_dev *indio_dev)
 {
 	struct st_sensor_data *pdata = iio_priv(indio_dev);
 
-	st_press_power_disable(indio_dev);
+	st_sensors_power_disable(indio_dev);
 
 	iio_device_unregister(indio_dev);
 	if (pdata->get_irq_data_ready(indio_dev) > 0)
diff --git a/drivers/iio/proximity/Kconfig b/drivers/iio/proximity/Kconfig
new file mode 100644
index 00000000000..0c8cdf58f6a
--- /dev/null
+++ b/drivers/iio/proximity/Kconfig
@@ -0,0 +1,19 @@
+#
+# Proximity sensors
+#
+
+menu "Lightning sensors"
+
+config AS3935
+	tristate "AS3935 Franklin lightning sensor"
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	depends on SPI
+	help
+	  Say Y here to build SPI interface support for the Austrian
+	  Microsystems AS3935 lightning detection sensor.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called as3935
+
+endmenu
diff --git a/drivers/iio/proximity/Makefile b/drivers/iio/proximity/Makefile
new file mode 100644
index 00000000000..743adee1c8b
--- /dev/null
+++ b/drivers/iio/proximity/Makefile
@@ -0,0 +1,6 @@
+#
+# Makefile for IIO proximity sensors
+#
+
+# When adding new entries keep the list in alphabetical order
+obj-$(CONFIG_AS3935)		+= as3935.o
diff --git a/drivers/iio/proximity/as3935.c b/drivers/iio/proximity/as3935.c
new file mode 100644
index 00000000000..bf677bfe8eb
--- /dev/null
+++ b/drivers/iio/proximity/as3935.c
@@ -0,0 +1,456 @@
+/*
+ * as3935.c - Support for AS3935 Franklin lightning sensor
+ *
+ * Copyright (C) 2014 Matt Ranostay <mranostay@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/interrupt.h>
+#include <linux/delay.h>
+#include <linux/workqueue.h>
+#include <linux/mutex.h>
+#include <linux/err.h>
+#include <linux/irq.h>
+#include <linux/gpio.h>
+#include <linux/spi/spi.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/of_gpio.h>
+
+
+#define AS3935_AFE_GAIN		0x00
+#define AS3935_AFE_MASK		0x3F
+#define AS3935_AFE_GAIN_MAX	0x1F
+#define AS3935_AFE_PWR_BIT	BIT(0)
+
+#define AS3935_INT		0x03
+#define AS3935_INT_MASK		0x07
+#define AS3935_EVENT_INT	BIT(3)
+#define AS3935_NOISE_INT	BIT(1)
+
+#define AS3935_DATA		0x07
+#define AS3935_DATA_MASK	0x3F
+
+#define AS3935_TUNE_CAP		0x08
+#define AS3935_CALIBRATE	0x3D
+
+#define AS3935_WRITE_DATA	BIT(15)
+#define AS3935_READ_DATA	BIT(14)
+#define AS3935_ADDRESS(x)	((x) << 8)
+
+#define MAX_PF_CAP		120
+#define TUNE_CAP_DIV		8
+
+struct as3935_state {
+	struct spi_device *spi;
+	struct iio_trigger *trig;
+	struct mutex lock;
+	struct delayed_work work;
+
+	u32 tune_cap;
+	u8 buf[2] ____cacheline_aligned;
+};
+
+static const struct iio_chan_spec as3935_channels[] = {
+	{
+		.type           = IIO_PROXIMITY,
+		.info_mask_separate =
+			BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_PROCESSED),
+		.scan_index     = 0,
+		.scan_type = {
+			.sign           = 'u',
+			.realbits       = 6,
+			.storagebits    = 8,
+		},
+	},
+	IIO_CHAN_SOFT_TIMESTAMP(1),
+};
+
+static int as3935_read(struct as3935_state *st, unsigned int reg, int *val)
+{
+	u8 cmd;
+	int ret;
+
+	cmd = (AS3935_READ_DATA | AS3935_ADDRESS(reg)) >> 8;
+	ret = spi_w8r8(st->spi, cmd);
+	if (ret < 0)
+		return ret;
+	*val = ret;
+
+	return 0;
+};
+
+static int as3935_write(struct as3935_state *st,
+				unsigned int reg,
+				unsigned int val)
+{
+	u8 *buf = st->buf;
+
+	buf[0] = (AS3935_WRITE_DATA | AS3935_ADDRESS(reg)) >> 8;
+	buf[1] = val;
+
+	return spi_write(st->spi, buf, 2);
+};
+
+static ssize_t as3935_sensor_sensitivity_show(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct as3935_state *st = iio_priv(dev_to_iio_dev(dev));
+	int val, ret;
+
+	ret = as3935_read(st, AS3935_AFE_GAIN, &val);
+	if (ret)
+		return ret;
+	val = (val & AS3935_AFE_MASK) >> 1;
+
+	return sprintf(buf, "%d\n", val);
+};
+
+static ssize_t as3935_sensor_sensitivity_store(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t len)
+{
+	struct as3935_state *st = iio_priv(dev_to_iio_dev(dev));
+	unsigned long val;
+	int ret;
+
+	ret = kstrtoul((const char *) buf, 10, &val);
+	if (ret)
+		return -EINVAL;
+
+	if (val > AS3935_AFE_GAIN_MAX)
+		return -EINVAL;
+
+	as3935_write(st, AS3935_AFE_GAIN, val << 1);
+
+	return len;
+};
+
+static IIO_DEVICE_ATTR(sensor_sensitivity, S_IRUGO | S_IWUSR,
+	as3935_sensor_sensitivity_show, as3935_sensor_sensitivity_store, 0);
+
+
+static struct attribute *as3935_attributes[] = {
+	&iio_dev_attr_sensor_sensitivity.dev_attr.attr,
+	NULL,
+};
+
+static struct attribute_group as3935_attribute_group = {
+	.attrs = as3935_attributes,
+};
+
+static int as3935_read_raw(struct iio_dev *indio_dev,
+			   struct iio_chan_spec const *chan,
+			   int *val,
+			   int *val2,
+			   long m)
+{
+	struct as3935_state *st = iio_priv(indio_dev);
+	int ret;
+
+
+	switch (m) {
+	case IIO_CHAN_INFO_PROCESSED:
+	case IIO_CHAN_INFO_RAW:
+		*val2 = 0;
+		ret = as3935_read(st, AS3935_DATA, val);
+		if (ret)
+			return ret;
+
+		if (m == IIO_CHAN_INFO_RAW)
+			return IIO_VAL_INT;
+
+		/* storm out of range */
+		if (*val == AS3935_DATA_MASK)
+			return -EINVAL;
+		*val *= 1000;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return IIO_VAL_INT;
+}
+
+static const struct iio_info as3935_info = {
+	.driver_module = THIS_MODULE,
+	.attrs = &as3935_attribute_group,
+	.read_raw = &as3935_read_raw,
+};
+
+static irqreturn_t as3935_trigger_handler(int irq, void *private)
+{
+	struct iio_poll_func *pf = private;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct as3935_state *st = iio_priv(indio_dev);
+	int val, ret;
+
+	ret = as3935_read(st, AS3935_DATA, &val);
+	if (ret)
+		goto err_read;
+	val &= AS3935_DATA_MASK;
+	val *= 1000;
+
+	iio_push_to_buffers_with_timestamp(indio_dev, &val, pf->timestamp);
+err_read:
+	iio_trigger_notify_done(indio_dev->trig);
+
+	return IRQ_HANDLED;
+};
+
+static const struct iio_trigger_ops iio_interrupt_trigger_ops = {
+	.owner = THIS_MODULE,
+};
+
+static void as3935_event_work(struct work_struct *work)
+{
+	struct as3935_state *st;
+	int val;
+
+	st = container_of(work, struct as3935_state, work.work);
+
+	as3935_read(st, AS3935_INT, &val);
+	val &= AS3935_INT_MASK;
+
+	switch (val) {
+	case AS3935_EVENT_INT:
+		iio_trigger_poll(st->trig, iio_get_time_ns());
+		break;
+	case AS3935_NOISE_INT:
+		dev_warn(&st->spi->dev, "noise level is too high");
+		break;
+	}
+};
+
+static irqreturn_t as3935_interrupt_handler(int irq, void *private)
+{
+	struct iio_dev *indio_dev = private;
+	struct as3935_state *st = iio_priv(indio_dev);
+
+	/*
+	 * Delay work for >2 milliseconds after an interrupt to allow
+	 * estimated distance to recalculated.
+	 */
+
+	schedule_delayed_work(&st->work, msecs_to_jiffies(3));
+
+	return IRQ_HANDLED;
+}
+
+static void calibrate_as3935(struct as3935_state *st)
+{
+	mutex_lock(&st->lock);
+
+	/* mask disturber interrupt bit */
+	as3935_write(st, AS3935_INT, BIT(5));
+
+	as3935_write(st, AS3935_CALIBRATE, 0x96);
+	as3935_write(st, AS3935_TUNE_CAP,
+		BIT(5) | (st->tune_cap / TUNE_CAP_DIV));
+
+	mdelay(2);
+	as3935_write(st, AS3935_TUNE_CAP, (st->tune_cap / TUNE_CAP_DIV));
+
+	mutex_unlock(&st->lock);
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int as3935_suspend(struct spi_device *spi, pm_message_t msg)
+{
+	struct iio_dev *indio_dev = spi_get_drvdata(spi);
+	struct as3935_state *st = iio_priv(indio_dev);
+	int val, ret;
+
+	mutex_lock(&st->lock);
+	ret = as3935_read(st, AS3935_AFE_GAIN, &val);
+	if (ret)
+		goto err_suspend;
+	val |= AS3935_AFE_PWR_BIT;
+
+	ret = as3935_write(st, AS3935_AFE_GAIN, val);
+
+err_suspend:
+	mutex_unlock(&st->lock);
+
+	return ret;
+}
+
+static int as3935_resume(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev = spi_get_drvdata(spi);
+	struct as3935_state *st = iio_priv(indio_dev);
+	int val, ret;
+
+	mutex_lock(&st->lock);
+	ret = as3935_read(st, AS3935_AFE_GAIN, &val);
+	if (ret)
+		goto err_resume;
+	val &= ~AS3935_AFE_PWR_BIT;
+	ret = as3935_write(st, AS3935_AFE_GAIN, val);
+
+err_resume:
+	mutex_unlock(&st->lock);
+
+	return ret;
+}
+#else
+#define as3935_suspend	NULL
+#define as3935_resume	NULL
+#endif
+
+static int as3935_probe(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev;
+	struct iio_trigger *trig;
+	struct as3935_state *st;
+	struct device_node *np = spi->dev.of_node;
+	int ret;
+
+	/* Be sure lightning event interrupt is specified */
+	if (!spi->irq) {
+		dev_err(&spi->dev, "unable to get event interrupt\n");
+		return -EINVAL;
+	}
+
+	indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(st));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	st = iio_priv(indio_dev);
+	st->spi = spi;
+	st->tune_cap = 0;
+
+	spi_set_drvdata(spi, indio_dev);
+	mutex_init(&st->lock);
+	INIT_DELAYED_WORK(&st->work, as3935_event_work);
+
+	ret = of_property_read_u32(np,
+			"ams,tuning-capacitor-pf", &st->tune_cap);
+	if (ret) {
+		st->tune_cap = 0;
+		dev_warn(&spi->dev,
+			"no tuning-capacitor-pf set, defaulting to %d",
+			st->tune_cap);
+	}
+
+	if (st->tune_cap > MAX_PF_CAP) {
+		dev_err(&spi->dev,
+			"wrong tuning-capacitor-pf setting of %d\n",
+			st->tune_cap);
+		return -EINVAL;
+	}
+
+	indio_dev->dev.parent = &spi->dev;
+	indio_dev->name = spi_get_device_id(spi)->name;
+	indio_dev->channels = as3935_channels;
+	indio_dev->num_channels = ARRAY_SIZE(as3935_channels);
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->info = &as3935_info;
+
+	trig = devm_iio_trigger_alloc(&spi->dev, "%s-dev%d",
+				      indio_dev->name, indio_dev->id);
+
+	if (!trig)
+		return -ENOMEM;
+
+	st->trig = trig;
+	trig->dev.parent = indio_dev->dev.parent;
+	iio_trigger_set_drvdata(trig, indio_dev);
+	trig->ops = &iio_interrupt_trigger_ops;
+
+	ret = iio_trigger_register(trig);
+	if (ret) {
+		dev_err(&spi->dev, "failed to register trigger\n");
+		return ret;
+	}
+
+	ret = iio_triggered_buffer_setup(indio_dev, NULL,
+		&as3935_trigger_handler, NULL);
+
+	if (ret) {
+		dev_err(&spi->dev, "cannot setup iio trigger\n");
+		goto unregister_trigger;
+	}
+
+	calibrate_as3935(st);
+
+	ret = devm_request_irq(&spi->dev, spi->irq,
+				&as3935_interrupt_handler,
+				IRQF_TRIGGER_RISING,
+				dev_name(&spi->dev),
+				indio_dev);
+
+	if (ret) {
+		dev_err(&spi->dev, "unable to request irq\n");
+		goto unregister_buffer;
+	}
+
+	ret = iio_device_register(indio_dev);
+	if (ret < 0) {
+		dev_err(&spi->dev, "unable to register device\n");
+		goto unregister_buffer;
+	}
+	return 0;
+
+unregister_buffer:
+	iio_triggered_buffer_cleanup(indio_dev);
+
+unregister_trigger:
+	iio_trigger_unregister(st->trig);
+
+	return ret;
+};
+
+static int as3935_remove(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev = spi_get_drvdata(spi);
+	struct as3935_state *st = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+	iio_triggered_buffer_cleanup(indio_dev);
+	iio_trigger_unregister(st->trig);
+
+	return 0;
+};
+
+static const struct spi_device_id as3935_id[] = {
+	{"as3935", 0},
+	{},
+};
+MODULE_DEVICE_TABLE(spi, as3935_id);
+
+static struct spi_driver as3935_driver = {
+	.driver = {
+		.name	= "as3935",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= as3935_probe,
+	.remove		= as3935_remove,
+	.id_table	= as3935_id,
+	.suspend	= as3935_suspend,
+	.resume		= as3935_resume,
+};
+module_spi_driver(as3935_driver);
+
+MODULE_AUTHOR("Matt Ranostay <mranostay@gmail.com>");
+MODULE_DESCRIPTION("AS3935 lightning sensor");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("spi:as3935");
diff --git a/drivers/iio/temperature/Kconfig b/drivers/iio/temperature/Kconfig
index 372f8fb3085..21feaa4661b 100644
--- a/drivers/iio/temperature/Kconfig
+++ b/drivers/iio/temperature/Kconfig
@@ -3,6 +3,16 @@
 #
 menu "Temperature sensors"
 
+config MLX90614
+	tristate "MLX90614 contact-less infrared sensor"
+	depends on I2C
+	help
+	  If you say yes here you get support for the Melexis
+	  MLX90614 contact-less infrared sensor connected with I2C.
+
+	  This driver can also be built as a module. If so, the module will
+	  be called mlx90614.
+
 config TMP006
 	tristate "TMP006 infrared thermopile sensor"
 	depends on I2C
diff --git a/drivers/iio/temperature/Makefile b/drivers/iio/temperature/Makefile
index 24d7b602db3..40710a81158 100644
--- a/drivers/iio/temperature/Makefile
+++ b/drivers/iio/temperature/Makefile
@@ -2,4 +2,5 @@
 # Makefile for industrial I/O temperature drivers
 #
 
+obj-$(CONFIG_MLX90614) += mlx90614.o
 obj-$(CONFIG_TMP006) += tmp006.o
diff --git a/drivers/iio/temperature/mlx90614.c b/drivers/iio/temperature/mlx90614.c
new file mode 100644
index 00000000000..c8b6ac8b2d6
--- /dev/null
+++ b/drivers/iio/temperature/mlx90614.c
@@ -0,0 +1,150 @@
+/*
+ * mlx90614.c - Support for Melexis MLX90614 contactless IR temperature sensor
+ *
+ * Copyright (c) 2014 Peter Meerwald <pmeerw@pmeerw.net>
+ *
+ * This file is subject to the terms and conditions of version 2 of
+ * the GNU General Public License.  See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * Driver for the Melexis MLX90614 I2C 16-bit IR thermopile sensor
+ *
+ * (7-bit I2C slave address 0x5a, 100KHz bus speed only!)
+ *
+ * TODO: sleep mode, configuration EEPROM
+ */
+
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+
+#include <linux/iio/iio.h>
+
+#define MLX90614_OP_RAM 0x00
+
+/* RAM offsets with 16-bit data, MSB first */
+#define MLX90614_TA 0x06 /* ambient temperature */
+#define MLX90614_TOBJ1 0x07 /* object temperature */
+
+struct mlx90614_data {
+	struct i2c_client *client;
+};
+
+static int mlx90614_read_raw(struct iio_dev *indio_dev,
+			    struct iio_chan_spec const *channel, int *val,
+			    int *val2, long mask)
+{
+	struct mlx90614_data *data = iio_priv(indio_dev);
+	s32 ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW: /* 0.02K / LSB */
+		switch (channel->channel2) {
+		case IIO_MOD_TEMP_AMBIENT:
+			ret = i2c_smbus_read_word_data(data->client,
+			    MLX90614_OP_RAM | MLX90614_TA);
+			if (ret < 0)
+				return ret;
+			break;
+		case IIO_MOD_TEMP_OBJECT:
+			ret = i2c_smbus_read_word_data(data->client,
+			    MLX90614_OP_RAM | MLX90614_TOBJ1);
+			if (ret < 0)
+				return ret;
+			break;
+		default:
+			return -EINVAL;
+		}
+		*val = ret;
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_OFFSET:
+		*val = 13657;
+		*val2 = 500000;
+		return IIO_VAL_INT_PLUS_MICRO;
+	case IIO_CHAN_INFO_SCALE:
+		*val = 20;
+		return IIO_VAL_INT;
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct iio_chan_spec mlx90614_channels[] = {
+	{
+		.type = IIO_TEMP,
+		.modified = 1,
+		.channel2 = IIO_MOD_TEMP_AMBIENT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
+		    BIT(IIO_CHAN_INFO_SCALE),
+	},
+	{
+		.type = IIO_TEMP,
+		.modified = 1,
+		.channel2 = IIO_MOD_TEMP_OBJECT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
+		    BIT(IIO_CHAN_INFO_SCALE),
+	},
+};
+
+static const struct iio_info mlx90614_info = {
+	.read_raw = mlx90614_read_raw,
+	.driver_module = THIS_MODULE,
+};
+
+static int mlx90614_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct iio_dev *indio_dev;
+	struct mlx90614_data *data;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_WORD_DATA))
+		return -ENODEV;
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	data = iio_priv(indio_dev);
+	i2c_set_clientdata(client, indio_dev);
+	data->client = client;
+
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->name = id->name;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->info = &mlx90614_info;
+
+	indio_dev->channels = mlx90614_channels;
+	indio_dev->num_channels = ARRAY_SIZE(mlx90614_channels);
+
+	return iio_device_register(indio_dev);
+}
+
+static int mlx90614_remove(struct i2c_client *client)
+{
+	iio_device_unregister(i2c_get_clientdata(client));
+
+	return 0;
+}
+
+static const struct i2c_device_id mlx90614_id[] = {
+	{ "mlx90614", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, mlx90614_id);
+
+static struct i2c_driver mlx90614_driver = {
+	.driver = {
+		.name	= "mlx90614",
+		.owner	= THIS_MODULE,
+	},
+	.probe = mlx90614_probe,
+	.remove = mlx90614_remove,
+	.id_table = mlx90614_id,
+};
+module_i2c_driver(mlx90614_driver);
+
+MODULE_AUTHOR("Peter Meerwald <pmeerw@pmeerw.net>");
+MODULE_DESCRIPTION("Melexis MLX90614 contactless IR temperature sensor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/infiniband/hw/cxgb4/Kconfig b/drivers/infiniband/hw/cxgb4/Kconfig
index d4e8983fba5..23f38cf2c5c 100644
--- a/drivers/infiniband/hw/cxgb4/Kconfig
+++ b/drivers/infiniband/hw/cxgb4/Kconfig
@@ -1,10 +1,10 @@
 config INFINIBAND_CXGB4
-	tristate "Chelsio T4 RDMA Driver"
+	tristate "Chelsio T4/T5 RDMA Driver"
 	depends on CHELSIO_T4 && INET && (IPV6 || IPV6=n)
 	select GENERIC_ALLOCATOR
 	---help---
-	  This is an iWARP/RDMA driver for the Chelsio T4 1GbE and
-	  10GbE adapters.
+	  This is an iWARP/RDMA driver for the Chelsio T4 and T5
+	  1GbE, 10GbE adapters and T5 40GbE adapter.
 
 	  For general information about Chelsio and our products, visit
 	  our website at <http://www.chelsio.com>.
diff --git a/drivers/infiniband/hw/cxgb4/cm.c b/drivers/infiniband/hw/cxgb4/cm.c
index 02436d5d0da..1f863a96a48 100644
--- a/drivers/infiniband/hw/cxgb4/cm.c
+++ b/drivers/infiniband/hw/cxgb4/cm.c
@@ -173,12 +173,15 @@ static void start_ep_timer(struct c4iw_ep *ep)
 	add_timer(&ep->timer);
 }
 
-static void stop_ep_timer(struct c4iw_ep *ep)
+static int stop_ep_timer(struct c4iw_ep *ep)
 {
 	PDBG("%s ep %p stopping\n", __func__, ep);
 	del_timer_sync(&ep->timer);
-	if (!test_and_set_bit(TIMEOUT, &ep->com.flags))
+	if (!test_and_set_bit(TIMEOUT, &ep->com.flags)) {
 		c4iw_put_ep(&ep->com);
+		return 0;
+	}
+	return 1;
 }
 
 static int c4iw_l2t_send(struct c4iw_rdev *rdev, struct sk_buff *skb,
@@ -584,6 +587,10 @@ static int send_connect(struct c4iw_ep *ep)
 		opt2 |= SACK_EN(1);
 	if (wscale && enable_tcp_window_scaling)
 		opt2 |= WND_SCALE_EN(1);
+	if (is_t5(ep->com.dev->rdev.lldi.adapter_type)) {
+		opt2 |= T5_OPT_2_VALID;
+		opt2 |= V_CONG_CNTRL(CONG_ALG_TAHOE);
+	}
 	t4_set_arp_err_handler(skb, NULL, act_open_req_arp_failure);
 
 	if (is_t4(ep->com.dev->rdev.lldi.adapter_type)) {
@@ -993,7 +1000,7 @@ static void close_complete_upcall(struct c4iw_ep *ep, int status)
 static int abort_connection(struct c4iw_ep *ep, struct sk_buff *skb, gfp_t gfp)
 {
 	PDBG("%s ep %p tid %u\n", __func__, ep, ep->hwtid);
-	state_set(&ep->com, ABORTING);
+	__state_set(&ep->com, ABORTING);
 	set_bit(ABORT_CONN, &ep->com.history);
 	return send_abort(ep, skb, gfp);
 }
@@ -1151,7 +1158,7 @@ static int update_rx_credits(struct c4iw_ep *ep, u32 credits)
 	return credits;
 }
 
-static void process_mpa_reply(struct c4iw_ep *ep, struct sk_buff *skb)
+static int process_mpa_reply(struct c4iw_ep *ep, struct sk_buff *skb)
 {
 	struct mpa_message *mpa;
 	struct mpa_v2_conn_params *mpa_v2_params;
@@ -1161,17 +1168,17 @@ static void process_mpa_reply(struct c4iw_ep *ep, struct sk_buff *skb)
 	struct c4iw_qp_attributes attrs;
 	enum c4iw_qp_attr_mask mask;
 	int err;
+	int disconnect = 0;
 
 	PDBG("%s ep %p tid %u\n", __func__, ep, ep->hwtid);
 
 	/*
-	 * Stop mpa timer.  If it expired, then the state has
-	 * changed and we bail since ep_timeout already aborted
-	 * the connection.
+	 * Stop mpa timer.  If it expired, then
+	 * we ignore the MPA reply.  process_timeout()
+	 * will abort the connection.
 	 */
-	stop_ep_timer(ep);
-	if (ep->com.state != MPA_REQ_SENT)
-		return;
+	if (stop_ep_timer(ep))
+		return 0;
 
 	/*
 	 * If we get more than the supported amount of private data
@@ -1193,7 +1200,7 @@ static void process_mpa_reply(struct c4iw_ep *ep, struct sk_buff *skb)
 	 * if we don't even have the mpa message, then bail.
 	 */
 	if (ep->mpa_pkt_len < sizeof(*mpa))
-		return;
+		return 0;
 	mpa = (struct mpa_message *) ep->mpa_pkt;
 
 	/* Validate MPA header. */
@@ -1233,7 +1240,7 @@ static void process_mpa_reply(struct c4iw_ep *ep, struct sk_buff *skb)
 	 * We'll continue process when more data arrives.
 	 */
 	if (ep->mpa_pkt_len < (sizeof(*mpa) + plen))
-		return;
+		return 0;
 
 	if (mpa->flags & MPA_REJECT) {
 		err = -ECONNREFUSED;
@@ -1335,9 +1342,11 @@ static void process_mpa_reply(struct c4iw_ep *ep, struct sk_buff *skb)
 		attrs.layer_etype = LAYER_MPA | DDP_LLP;
 		attrs.ecode = MPA_NOMATCH_RTR;
 		attrs.next_state = C4IW_QP_STATE_TERMINATE;
+		attrs.send_term = 1;
 		err = c4iw_modify_qp(ep->com.qp->rhp, ep->com.qp,
-				C4IW_QP_ATTR_NEXT_STATE, &attrs, 0);
+				C4IW_QP_ATTR_NEXT_STATE, &attrs, 1);
 		err = -ENOMEM;
+		disconnect = 1;
 		goto out;
 	}
 
@@ -1353,9 +1362,11 @@ static void process_mpa_reply(struct c4iw_ep *ep, struct sk_buff *skb)
 		attrs.layer_etype = LAYER_MPA | DDP_LLP;
 		attrs.ecode = MPA_INSUFF_IRD;
 		attrs.next_state = C4IW_QP_STATE_TERMINATE;
+		attrs.send_term = 1;
 		err = c4iw_modify_qp(ep->com.qp->rhp, ep->com.qp,
-				C4IW_QP_ATTR_NEXT_STATE, &attrs, 0);
+				C4IW_QP_ATTR_NEXT_STATE, &attrs, 1);
 		err = -ENOMEM;
+		disconnect = 1;
 		goto out;
 	}
 	goto out;
@@ -1364,7 +1375,7 @@ err:
 	send_abort(ep, skb, GFP_KERNEL);
 out:
 	connect_reply_upcall(ep, err);
-	return;
+	return disconnect;
 }
 
 static void process_mpa_request(struct c4iw_ep *ep, struct sk_buff *skb)
@@ -1375,15 +1386,12 @@ static void process_mpa_request(struct c4iw_ep *ep, struct sk_buff *skb)
 
 	PDBG("%s ep %p tid %u\n", __func__, ep, ep->hwtid);
 
-	if (ep->com.state != MPA_REQ_WAIT)
-		return;
-
 	/*
 	 * If we get more than the supported amount of private data
 	 * then we must fail this connection.
 	 */
 	if (ep->mpa_pkt_len + skb->len > sizeof(ep->mpa_pkt)) {
-		stop_ep_timer(ep);
+		(void)stop_ep_timer(ep);
 		abort_connection(ep, skb, GFP_KERNEL);
 		return;
 	}
@@ -1413,13 +1421,13 @@ static void process_mpa_request(struct c4iw_ep *ep, struct sk_buff *skb)
 	if (mpa->revision > mpa_rev) {
 		printk(KERN_ERR MOD "%s MPA version mismatch. Local = %d,"
 		       " Received = %d\n", __func__, mpa_rev, mpa->revision);
-		stop_ep_timer(ep);
+		(void)stop_ep_timer(ep);
 		abort_connection(ep, skb, GFP_KERNEL);
 		return;
 	}
 
 	if (memcmp(mpa->key, MPA_KEY_REQ, sizeof(mpa->key))) {
-		stop_ep_timer(ep);
+		(void)stop_ep_timer(ep);
 		abort_connection(ep, skb, GFP_KERNEL);
 		return;
 	}
@@ -1430,7 +1438,7 @@ static void process_mpa_request(struct c4iw_ep *ep, struct sk_buff *skb)
 	 * Fail if there's too much private data.
 	 */
 	if (plen > MPA_MAX_PRIVATE_DATA) {
-		stop_ep_timer(ep);
+		(void)stop_ep_timer(ep);
 		abort_connection(ep, skb, GFP_KERNEL);
 		return;
 	}
@@ -1439,7 +1447,7 @@ static void process_mpa_request(struct c4iw_ep *ep, struct sk_buff *skb)
 	 * If plen does not account for pkt size
 	 */
 	if (ep->mpa_pkt_len > (sizeof(*mpa) + plen)) {
-		stop_ep_timer(ep);
+		(void)stop_ep_timer(ep);
 		abort_connection(ep, skb, GFP_KERNEL);
 		return;
 	}
@@ -1496,18 +1504,24 @@ static void process_mpa_request(struct c4iw_ep *ep, struct sk_buff *skb)
 	     ep->mpa_attr.xmit_marker_enabled, ep->mpa_attr.version,
 	     ep->mpa_attr.p2p_type);
 
-	__state_set(&ep->com, MPA_REQ_RCVD);
-	stop_ep_timer(ep);
-
-	/* drive upcall */
-	mutex_lock(&ep->parent_ep->com.mutex);
-	if (ep->parent_ep->com.state != DEAD) {
-		if (connect_request_upcall(ep))
+	/*
+	 * If the endpoint timer already expired, then we ignore
+	 * the start request.  process_timeout() will abort
+	 * the connection.
+	 */
+	if (!stop_ep_timer(ep)) {
+		__state_set(&ep->com, MPA_REQ_RCVD);
+
+		/* drive upcall */
+		mutex_lock(&ep->parent_ep->com.mutex);
+		if (ep->parent_ep->com.state != DEAD) {
+			if (connect_request_upcall(ep))
+				abort_connection(ep, skb, GFP_KERNEL);
+		} else {
 			abort_connection(ep, skb, GFP_KERNEL);
-	} else {
-		abort_connection(ep, skb, GFP_KERNEL);
+		}
+		mutex_unlock(&ep->parent_ep->com.mutex);
 	}
-	mutex_unlock(&ep->parent_ep->com.mutex);
 	return;
 }
 
@@ -1519,6 +1533,7 @@ static int rx_data(struct c4iw_dev *dev, struct sk_buff *skb)
 	unsigned int tid = GET_TID(hdr);
 	struct tid_info *t = dev->rdev.lldi.tids;
 	__u8 status = hdr->status;
+	int disconnect = 0;
 
 	ep = lookup_tid(t, tid);
 	if (!ep)
@@ -1534,7 +1549,7 @@ static int rx_data(struct c4iw_dev *dev, struct sk_buff *skb)
 	switch (ep->com.state) {
 	case MPA_REQ_SENT:
 		ep->rcv_seq += dlen;
-		process_mpa_reply(ep, skb);
+		disconnect = process_mpa_reply(ep, skb);
 		break;
 	case MPA_REQ_WAIT:
 		ep->rcv_seq += dlen;
@@ -1550,13 +1565,16 @@ static int rx_data(struct c4iw_dev *dev, struct sk_buff *skb)
 			       ep->com.state, ep->hwtid, status);
 		attrs.next_state = C4IW_QP_STATE_TERMINATE;
 		c4iw_modify_qp(ep->com.qp->rhp, ep->com.qp,
-			       C4IW_QP_ATTR_NEXT_STATE, &attrs, 0);
+			       C4IW_QP_ATTR_NEXT_STATE, &attrs, 1);
+		disconnect = 1;
 		break;
 	}
 	default:
 		break;
 	}
 	mutex_unlock(&ep->com.mutex);
+	if (disconnect)
+		c4iw_ep_disconnect(ep, 0, GFP_KERNEL);
 	return 0;
 }
 
@@ -2004,6 +2022,10 @@ static void accept_cr(struct c4iw_ep *ep, struct sk_buff *skb,
 		if (tcph->ece && tcph->cwr)
 			opt2 |= CCTRL_ECN(1);
 	}
+	if (is_t5(ep->com.dev->rdev.lldi.adapter_type)) {
+		opt2 |= T5_OPT_2_VALID;
+		opt2 |= V_CONG_CNTRL(CONG_ALG_TAHOE);
+	}
 
 	rpl = cplhdr(skb);
 	INIT_TP_WR(rpl, ep->hwtid);
@@ -2265,7 +2287,7 @@ static int peer_close(struct c4iw_dev *dev, struct sk_buff *skb)
 		disconnect = 0;
 		break;
 	case MORIBUND:
-		stop_ep_timer(ep);
+		(void)stop_ep_timer(ep);
 		if (ep->com.cm_id && ep->com.qp) {
 			attrs.next_state = C4IW_QP_STATE_IDLE;
 			c4iw_modify_qp(ep->com.qp->rhp, ep->com.qp,
@@ -2325,10 +2347,10 @@ static int peer_abort(struct c4iw_dev *dev, struct sk_buff *skb)
 	case CONNECTING:
 		break;
 	case MPA_REQ_WAIT:
-		stop_ep_timer(ep);
+		(void)stop_ep_timer(ep);
 		break;
 	case MPA_REQ_SENT:
-		stop_ep_timer(ep);
+		(void)stop_ep_timer(ep);
 		if (mpa_rev == 1 || (mpa_rev == 2 && ep->tried_with_mpa_v1))
 			connect_reply_upcall(ep, -ECONNRESET);
 		else {
@@ -2433,7 +2455,7 @@ static int close_con_rpl(struct c4iw_dev *dev, struct sk_buff *skb)
 		__state_set(&ep->com, MORIBUND);
 		break;
 	case MORIBUND:
-		stop_ep_timer(ep);
+		(void)stop_ep_timer(ep);
 		if ((ep->com.cm_id) && (ep->com.qp)) {
 			attrs.next_state = C4IW_QP_STATE_IDLE;
 			c4iw_modify_qp(ep->com.qp->rhp,
@@ -3028,7 +3050,7 @@ int c4iw_ep_disconnect(struct c4iw_ep *ep, int abrupt, gfp_t gfp)
 		if (!test_and_set_bit(CLOSE_SENT, &ep->com.flags)) {
 			close = 1;
 			if (abrupt) {
-				stop_ep_timer(ep);
+				(void)stop_ep_timer(ep);
 				ep->com.state = ABORTING;
 			} else
 				ep->com.state = MORIBUND;
@@ -3462,14 +3484,24 @@ static void process_timeout(struct c4iw_ep *ep)
 		__state_set(&ep->com, ABORTING);
 		close_complete_upcall(ep, -ETIMEDOUT);
 		break;
+	case ABORTING:
+	case DEAD:
+
+		/*
+		 * These states are expected if the ep timed out at the same
+		 * time as another thread was calling stop_ep_timer().
+		 * So we silently do nothing for these states.
+		 */
+		abort = 0;
+		break;
 	default:
 		WARN(1, "%s unexpected state ep %p tid %u state %u\n",
 			__func__, ep, ep->hwtid, ep->com.state);
 		abort = 0;
 	}
-	mutex_unlock(&ep->com.mutex);
 	if (abort)
 		abort_connection(ep, NULL, GFP_KERNEL);
+	mutex_unlock(&ep->com.mutex);
 	c4iw_put_ep(&ep->com);
 }
 
@@ -3483,6 +3515,8 @@ static void process_timedout_eps(void)
 
 		tmp = timeout_list.next;
 		list_del(tmp);
+		tmp->next = NULL;
+		tmp->prev = NULL;
 		spin_unlock_irq(&timeout_lock);
 		ep = list_entry(tmp, struct c4iw_ep, entry);
 		process_timeout(ep);
@@ -3499,6 +3533,7 @@ static void process_work(struct work_struct *work)
 	unsigned int opcode;
 	int ret;
 
+	process_timedout_eps();
 	while ((skb = skb_dequeue(&rxq))) {
 		rpl = cplhdr(skb);
 		dev = *((struct c4iw_dev **) (skb->cb + sizeof(void *)));
@@ -3508,8 +3543,8 @@ static void process_work(struct work_struct *work)
 		ret = work_handlers[opcode](dev, skb);
 		if (!ret)
 			kfree_skb(skb);
+		process_timedout_eps();
 	}
-	process_timedout_eps();
 }
 
 static DECLARE_WORK(skb_work, process_work);
@@ -3521,8 +3556,13 @@ static void ep_timeout(unsigned long arg)
 
 	spin_lock(&timeout_lock);
 	if (!test_and_set_bit(TIMEOUT, &ep->com.flags)) {
-		list_add_tail(&ep->entry, &timeout_list);
-		kickit = 1;
+		/*
+		 * Only insert if it is not already on the list.
+		 */
+		if (!ep->entry.next) {
+			list_add_tail(&ep->entry, &timeout_list);
+			kickit = 1;
+		}
 	}
 	spin_unlock(&timeout_lock);
 	if (kickit)
diff --git a/drivers/infiniband/hw/cxgb4/cq.c b/drivers/infiniband/hw/cxgb4/cq.c
index ce468e54242..cfaa56ada18 100644
--- a/drivers/infiniband/hw/cxgb4/cq.c
+++ b/drivers/infiniband/hw/cxgb4/cq.c
@@ -235,27 +235,21 @@ int c4iw_flush_sq(struct c4iw_qp *qhp)
 	struct t4_cq *cq = &chp->cq;
 	int idx;
 	struct t4_swsqe *swsqe;
-	int error = (qhp->attr.state != C4IW_QP_STATE_CLOSING &&
-			qhp->attr.state != C4IW_QP_STATE_IDLE);
 
 	if (wq->sq.flush_cidx == -1)
 		wq->sq.flush_cidx = wq->sq.cidx;
 	idx = wq->sq.flush_cidx;
 	BUG_ON(idx >= wq->sq.size);
 	while (idx != wq->sq.pidx) {
-		if (error) {
-			swsqe = &wq->sq.sw_sq[idx];
-			BUG_ON(swsqe->flushed);
-			swsqe->flushed = 1;
-			insert_sq_cqe(wq, cq, swsqe);
-			if (wq->sq.oldest_read == swsqe) {
-				BUG_ON(swsqe->opcode != FW_RI_READ_REQ);
-				advance_oldest_read(wq);
-			}
-			flushed++;
-		} else {
-			t4_sq_consume(wq);
+		swsqe = &wq->sq.sw_sq[idx];
+		BUG_ON(swsqe->flushed);
+		swsqe->flushed = 1;
+		insert_sq_cqe(wq, cq, swsqe);
+		if (wq->sq.oldest_read == swsqe) {
+			BUG_ON(swsqe->opcode != FW_RI_READ_REQ);
+			advance_oldest_read(wq);
 		}
+		flushed++;
 		if (++idx == wq->sq.size)
 			idx = 0;
 	}
@@ -678,7 +672,7 @@ skip_cqe:
 static int c4iw_poll_cq_one(struct c4iw_cq *chp, struct ib_wc *wc)
 {
 	struct c4iw_qp *qhp = NULL;
-	struct t4_cqe cqe = {0, 0}, *rd_cqe;
+	struct t4_cqe uninitialized_var(cqe), *rd_cqe;
 	struct t4_wq *wq;
 	u32 credit = 0;
 	u8 cqe_flushed;
diff --git a/drivers/infiniband/hw/cxgb4/device.c b/drivers/infiniband/hw/cxgb4/device.c
index 9489a388376..f4fa50a609e 100644
--- a/drivers/infiniband/hw/cxgb4/device.c
+++ b/drivers/infiniband/hw/cxgb4/device.c
@@ -682,7 +682,10 @@ static void c4iw_dealloc(struct uld_ctx *ctx)
 	idr_destroy(&ctx->dev->hwtid_idr);
 	idr_destroy(&ctx->dev->stid_idr);
 	idr_destroy(&ctx->dev->atid_idr);
-	iounmap(ctx->dev->rdev.oc_mw_kva);
+	if (ctx->dev->rdev.bar2_kva)
+		iounmap(ctx->dev->rdev.bar2_kva);
+	if (ctx->dev->rdev.oc_mw_kva)
+		iounmap(ctx->dev->rdev.oc_mw_kva);
 	ib_dealloc_device(&ctx->dev->ibdev);
 	ctx->dev = NULL;
 }
@@ -722,11 +725,31 @@ static struct c4iw_dev *c4iw_alloc(const struct cxgb4_lld_info *infop)
 	}
 	devp->rdev.lldi = *infop;
 
-	devp->rdev.oc_mw_pa = pci_resource_start(devp->rdev.lldi.pdev, 2) +
-		(pci_resource_len(devp->rdev.lldi.pdev, 2) -
-		 roundup_pow_of_two(devp->rdev.lldi.vr->ocq.size));
-	devp->rdev.oc_mw_kva = ioremap_wc(devp->rdev.oc_mw_pa,
-					       devp->rdev.lldi.vr->ocq.size);
+	/*
+	 * For T5 devices, we map all of BAR2 with WC.
+	 * For T4 devices with onchip qp mem, we map only that part
+	 * of BAR2 with WC.
+	 */
+	devp->rdev.bar2_pa = pci_resource_start(devp->rdev.lldi.pdev, 2);
+	if (is_t5(devp->rdev.lldi.adapter_type)) {
+		devp->rdev.bar2_kva = ioremap_wc(devp->rdev.bar2_pa,
+			pci_resource_len(devp->rdev.lldi.pdev, 2));
+		if (!devp->rdev.bar2_kva) {
+			pr_err(MOD "Unable to ioremap BAR2\n");
+			return ERR_PTR(-EINVAL);
+		}
+	} else if (ocqp_supported(infop)) {
+		devp->rdev.oc_mw_pa =
+			pci_resource_start(devp->rdev.lldi.pdev, 2) +
+			pci_resource_len(devp->rdev.lldi.pdev, 2) -
+			roundup_pow_of_two(devp->rdev.lldi.vr->ocq.size);
+		devp->rdev.oc_mw_kva = ioremap_wc(devp->rdev.oc_mw_pa,
+			devp->rdev.lldi.vr->ocq.size);
+		if (!devp->rdev.oc_mw_kva) {
+			pr_err(MOD "Unable to ioremap onchip mem\n");
+			return ERR_PTR(-EINVAL);
+		}
+	}
 
 	PDBG(KERN_INFO MOD "ocq memory: "
 	       "hw_start 0x%x size %u mw_pa 0x%lx mw_kva %p\n",
@@ -1003,9 +1026,11 @@ static int enable_qp_db(int id, void *p, void *data)
 static void resume_rc_qp(struct c4iw_qp *qp)
 {
 	spin_lock(&qp->lock);
-	t4_ring_sq_db(&qp->wq, qp->wq.sq.wq_pidx_inc);
+	t4_ring_sq_db(&qp->wq, qp->wq.sq.wq_pidx_inc,
+		      is_t5(qp->rhp->rdev.lldi.adapter_type), NULL);
 	qp->wq.sq.wq_pidx_inc = 0;
-	t4_ring_rq_db(&qp->wq, qp->wq.rq.wq_pidx_inc);
+	t4_ring_rq_db(&qp->wq, qp->wq.rq.wq_pidx_inc,
+		      is_t5(qp->rhp->rdev.lldi.adapter_type), NULL);
 	qp->wq.rq.wq_pidx_inc = 0;
 	spin_unlock(&qp->lock);
 }
diff --git a/drivers/infiniband/hw/cxgb4/iw_cxgb4.h b/drivers/infiniband/hw/cxgb4/iw_cxgb4.h
index e872203c542..7474b490760 100644
--- a/drivers/infiniband/hw/cxgb4/iw_cxgb4.h
+++ b/drivers/infiniband/hw/cxgb4/iw_cxgb4.h
@@ -149,6 +149,8 @@ struct c4iw_rdev {
 	struct gen_pool *ocqp_pool;
 	u32 flags;
 	struct cxgb4_lld_info lldi;
+	unsigned long bar2_pa;
+	void __iomem *bar2_kva;
 	unsigned long oc_mw_pa;
 	void __iomem *oc_mw_kva;
 	struct c4iw_stats stats;
@@ -433,6 +435,7 @@ struct c4iw_qp_attributes {
 	u8 ecode;
 	u16 sq_db_inc;
 	u16 rq_db_inc;
+	u8 send_term;
 };
 
 struct c4iw_qp {
diff --git a/drivers/infiniband/hw/cxgb4/mem.c b/drivers/infiniband/hw/cxgb4/mem.c
index f9ca072a99e..ec7a2988a70 100644
--- a/drivers/infiniband/hw/cxgb4/mem.c
+++ b/drivers/infiniband/hw/cxgb4/mem.c
@@ -259,8 +259,12 @@ static int write_tpt_entry(struct c4iw_rdev *rdev, u32 reset_tpt_entry,
 
 	if ((!reset_tpt_entry) && (*stag == T4_STAG_UNSET)) {
 		stag_idx = c4iw_get_resource(&rdev->resource.tpt_table);
-		if (!stag_idx)
+		if (!stag_idx) {
+			mutex_lock(&rdev->stats.lock);
+			rdev->stats.stag.fail++;
+			mutex_unlock(&rdev->stats.lock);
 			return -ENOMEM;
+		}
 		mutex_lock(&rdev->stats.lock);
 		rdev->stats.stag.cur += 32;
 		if (rdev->stats.stag.cur > rdev->stats.stag.max)
diff --git a/drivers/infiniband/hw/cxgb4/provider.c b/drivers/infiniband/hw/cxgb4/provider.c
index 79429256023..a94a3e12c34 100644
--- a/drivers/infiniband/hw/cxgb4/provider.c
+++ b/drivers/infiniband/hw/cxgb4/provider.c
@@ -328,7 +328,7 @@ static int c4iw_query_device(struct ib_device *ibdev,
 	props->max_mr = c4iw_num_stags(&dev->rdev);
 	props->max_pd = T4_MAX_NUM_PD;
 	props->local_ca_ack_delay = 0;
-	props->max_fast_reg_page_list_len = T4_MAX_FR_DEPTH;
+	props->max_fast_reg_page_list_len = t4_max_fr_depth(use_dsgl);
 
 	return 0;
 }
diff --git a/drivers/infiniband/hw/cxgb4/qp.c b/drivers/infiniband/hw/cxgb4/qp.c
index cb76eb5eee1..086f62f5dc9 100644
--- a/drivers/infiniband/hw/cxgb4/qp.c
+++ b/drivers/infiniband/hw/cxgb4/qp.c
@@ -212,13 +212,23 @@ static int create_qp(struct c4iw_rdev *rdev, struct t4_wq *wq,
 
 	wq->db = rdev->lldi.db_reg;
 	wq->gts = rdev->lldi.gts_reg;
-	if (user) {
-		wq->sq.udb = (u64)pci_resource_start(rdev->lldi.pdev, 2) +
-					(wq->sq.qid << rdev->qpshift);
-		wq->sq.udb &= PAGE_MASK;
-		wq->rq.udb = (u64)pci_resource_start(rdev->lldi.pdev, 2) +
-					(wq->rq.qid << rdev->qpshift);
-		wq->rq.udb &= PAGE_MASK;
+	if (user || is_t5(rdev->lldi.adapter_type)) {
+		u32 off;
+
+		off = (wq->sq.qid << rdev->qpshift) & PAGE_MASK;
+		if (user) {
+			wq->sq.udb = (u64 __iomem *)(rdev->bar2_pa + off);
+		} else {
+			off += 128 * (wq->sq.qid & rdev->qpmask) + 8;
+			wq->sq.udb = (u64 __iomem *)(rdev->bar2_kva + off);
+		}
+		off = (wq->rq.qid << rdev->qpshift) & PAGE_MASK;
+		if (user) {
+			wq->rq.udb = (u64 __iomem *)(rdev->bar2_pa + off);
+		} else {
+			off += 128 * (wq->rq.qid & rdev->qpmask) + 8;
+			wq->rq.udb = (u64 __iomem *)(rdev->bar2_kva + off);
+		}
 	}
 	wq->rdev = rdev;
 	wq->rq.msn = 1;
@@ -299,9 +309,10 @@ static int create_qp(struct c4iw_rdev *rdev, struct t4_wq *wq,
 	if (ret)
 		goto free_dma;
 
-	PDBG("%s sqid 0x%x rqid 0x%x kdb 0x%p squdb 0x%llx rqudb 0x%llx\n",
+	PDBG("%s sqid 0x%x rqid 0x%x kdb 0x%p squdb 0x%lx rqudb 0x%lx\n",
 	     __func__, wq->sq.qid, wq->rq.qid, wq->db,
-	     (unsigned long long)wq->sq.udb, (unsigned long long)wq->rq.udb);
+	     (__force unsigned long) wq->sq.udb,
+	     (__force unsigned long) wq->rq.udb);
 
 	return 0;
 free_dma:
@@ -425,6 +436,8 @@ static int build_rdma_send(struct t4_sq *sq, union t4_wr *wqe,
 	default:
 		return -EINVAL;
 	}
+	wqe->send.r3 = 0;
+	wqe->send.r4 = 0;
 
 	plen = 0;
 	if (wr->num_sge) {
@@ -555,7 +568,8 @@ static int build_fastreg(struct t4_sq *sq, union t4_wr *wqe,
 	int pbllen = roundup(wr->wr.fast_reg.page_list_len * sizeof(u64), 32);
 	int rem;
 
-	if (wr->wr.fast_reg.page_list_len > T4_MAX_FR_DEPTH)
+	if (wr->wr.fast_reg.page_list_len >
+	    t4_max_fr_depth(use_dsgl))
 		return -EINVAL;
 
 	wqe->fr.qpbinde_to_dcacpu = 0;
@@ -650,9 +664,10 @@ static int ring_kernel_sq_db(struct c4iw_qp *qhp, u16 inc)
 
 	spin_lock_irqsave(&qhp->rhp->lock, flags);
 	spin_lock(&qhp->lock);
-	if (qhp->rhp->db_state == NORMAL) {
-		t4_ring_sq_db(&qhp->wq, inc);
-	} else {
+	if (qhp->rhp->db_state == NORMAL)
+		t4_ring_sq_db(&qhp->wq, inc,
+			      is_t5(qhp->rhp->rdev.lldi.adapter_type), NULL);
+	else {
 		add_to_fc_list(&qhp->rhp->db_fc_list, &qhp->db_fc_entry);
 		qhp->wq.sq.wq_pidx_inc += inc;
 	}
@@ -667,9 +682,10 @@ static int ring_kernel_rq_db(struct c4iw_qp *qhp, u16 inc)
 
 	spin_lock_irqsave(&qhp->rhp->lock, flags);
 	spin_lock(&qhp->lock);
-	if (qhp->rhp->db_state == NORMAL) {
-		t4_ring_rq_db(&qhp->wq, inc);
-	} else {
+	if (qhp->rhp->db_state == NORMAL)
+		t4_ring_rq_db(&qhp->wq, inc,
+			      is_t5(qhp->rhp->rdev.lldi.adapter_type), NULL);
+	else {
 		add_to_fc_list(&qhp->rhp->db_fc_list, &qhp->db_fc_entry);
 		qhp->wq.rq.wq_pidx_inc += inc;
 	}
@@ -686,7 +702,7 @@ int c4iw_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
 	enum fw_wr_opcodes fw_opcode = 0;
 	enum fw_ri_wr_flags fw_flags;
 	struct c4iw_qp *qhp;
-	union t4_wr *wqe;
+	union t4_wr *wqe = NULL;
 	u32 num_wrs;
 	struct t4_swsqe *swsqe;
 	unsigned long flag;
@@ -792,7 +808,8 @@ int c4iw_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
 		idx += DIV_ROUND_UP(len16*16, T4_EQ_ENTRY_SIZE);
 	}
 	if (!qhp->rhp->rdev.status_page->db_off) {
-		t4_ring_sq_db(&qhp->wq, idx);
+		t4_ring_sq_db(&qhp->wq, idx,
+			      is_t5(qhp->rhp->rdev.lldi.adapter_type), wqe);
 		spin_unlock_irqrestore(&qhp->lock, flag);
 	} else {
 		spin_unlock_irqrestore(&qhp->lock, flag);
@@ -806,7 +823,7 @@ int c4iw_post_receive(struct ib_qp *ibqp, struct ib_recv_wr *wr,
 {
 	int err = 0;
 	struct c4iw_qp *qhp;
-	union t4_recv_wr *wqe;
+	union t4_recv_wr *wqe = NULL;
 	u32 num_wrs;
 	u8 len16 = 0;
 	unsigned long flag;
@@ -858,7 +875,8 @@ int c4iw_post_receive(struct ib_qp *ibqp, struct ib_recv_wr *wr,
 		num_wrs--;
 	}
 	if (!qhp->rhp->rdev.status_page->db_off) {
-		t4_ring_rq_db(&qhp->wq, idx);
+		t4_ring_rq_db(&qhp->wq, idx,
+			      is_t5(qhp->rhp->rdev.lldi.adapter_type), wqe);
 		spin_unlock_irqrestore(&qhp->lock, flag);
 	} else {
 		spin_unlock_irqrestore(&qhp->lock, flag);
@@ -1352,6 +1370,7 @@ int c4iw_modify_qp(struct c4iw_dev *rhp, struct c4iw_qp *qhp,
 		switch (attrs->next_state) {
 		case C4IW_QP_STATE_CLOSING:
 			BUG_ON(atomic_read(&qhp->ep->com.kref.refcount) < 2);
+			t4_set_wq_in_error(&qhp->wq);
 			set_state(qhp, C4IW_QP_STATE_CLOSING);
 			ep = qhp->ep;
 			if (!internal) {
@@ -1359,30 +1378,30 @@ int c4iw_modify_qp(struct c4iw_dev *rhp, struct c4iw_qp *qhp,
 				disconnect = 1;
 				c4iw_get_ep(&qhp->ep->com);
 			}
-			t4_set_wq_in_error(&qhp->wq);
 			ret = rdma_fini(rhp, qhp, ep);
 			if (ret)
 				goto err;
 			break;
 		case C4IW_QP_STATE_TERMINATE:
+			t4_set_wq_in_error(&qhp->wq);
 			set_state(qhp, C4IW_QP_STATE_TERMINATE);
 			qhp->attr.layer_etype = attrs->layer_etype;
 			qhp->attr.ecode = attrs->ecode;
-			t4_set_wq_in_error(&qhp->wq);
 			ep = qhp->ep;
-			disconnect = 1;
-			if (!internal)
+			if (!internal) {
+				c4iw_get_ep(&qhp->ep->com);
 				terminate = 1;
-			else {
+				disconnect = 1;
+			} else {
+				terminate = qhp->attr.send_term;
 				ret = rdma_fini(rhp, qhp, ep);
 				if (ret)
 					goto err;
 			}
-			c4iw_get_ep(&qhp->ep->com);
 			break;
 		case C4IW_QP_STATE_ERROR:
-			set_state(qhp, C4IW_QP_STATE_ERROR);
 			t4_set_wq_in_error(&qhp->wq);
+			set_state(qhp, C4IW_QP_STATE_ERROR);
 			if (!internal) {
 				abort = 1;
 				disconnect = 1;
@@ -1677,11 +1696,11 @@ struct ib_qp *c4iw_create_qp(struct ib_pd *pd, struct ib_qp_init_attr *attrs,
 		mm2->len = PAGE_ALIGN(qhp->wq.rq.memsize);
 		insert_mmap(ucontext, mm2);
 		mm3->key = uresp.sq_db_gts_key;
-		mm3->addr = qhp->wq.sq.udb;
+		mm3->addr = (__force unsigned long) qhp->wq.sq.udb;
 		mm3->len = PAGE_SIZE;
 		insert_mmap(ucontext, mm3);
 		mm4->key = uresp.rq_db_gts_key;
-		mm4->addr = qhp->wq.rq.udb;
+		mm4->addr = (__force unsigned long) qhp->wq.rq.udb;
 		mm4->len = PAGE_SIZE;
 		insert_mmap(ucontext, mm4);
 		if (mm5) {
@@ -1758,11 +1777,15 @@ int c4iw_ib_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 	/*
 	 * Use SQ_PSN and RQ_PSN to pass in IDX_INC values for
 	 * ringing the queue db when we're in DB_FULL mode.
+	 * Only allow this on T4 devices.
 	 */
 	attrs.sq_db_inc = attr->sq_psn;
 	attrs.rq_db_inc = attr->rq_psn;
 	mask |= (attr_mask & IB_QP_SQ_PSN) ? C4IW_QP_ATTR_SQ_DB : 0;
 	mask |= (attr_mask & IB_QP_RQ_PSN) ? C4IW_QP_ATTR_RQ_DB : 0;
+	if (is_t5(to_c4iw_qp(ibqp)->rhp->rdev.lldi.adapter_type) &&
+	    (mask & (C4IW_QP_ATTR_SQ_DB|C4IW_QP_ATTR_RQ_DB)))
+		return -EINVAL;
 
 	return c4iw_modify_qp(rhp, qhp, mask, &attrs, 0);
 }
diff --git a/drivers/infiniband/hw/cxgb4/resource.c b/drivers/infiniband/hw/cxgb4/resource.c
index cdef4d7fb6d..67df71a7012 100644
--- a/drivers/infiniband/hw/cxgb4/resource.c
+++ b/drivers/infiniband/hw/cxgb4/resource.c
@@ -179,8 +179,12 @@ u32 c4iw_get_qpid(struct c4iw_rdev *rdev, struct c4iw_dev_ucontext *uctx)
 		kfree(entry);
 	} else {
 		qid = c4iw_get_resource(&rdev->resource.qid_table);
-		if (!qid)
+		if (!qid) {
+			mutex_lock(&rdev->stats.lock);
+			rdev->stats.qid.fail++;
+			mutex_unlock(&rdev->stats.lock);
 			goto out;
+		}
 		mutex_lock(&rdev->stats.lock);
 		rdev->stats.qid.cur += rdev->qpmask + 1;
 		mutex_unlock(&rdev->stats.lock);
@@ -322,8 +326,8 @@ u32 c4iw_rqtpool_alloc(struct c4iw_rdev *rdev, int size)
 	unsigned long addr = gen_pool_alloc(rdev->rqt_pool, size << 6);
 	PDBG("%s addr 0x%x size %d\n", __func__, (u32)addr, size << 6);
 	if (!addr)
-		printk_ratelimited(KERN_WARNING MOD "%s: Out of RQT memory\n",
-		       pci_name(rdev->lldi.pdev));
+		pr_warn_ratelimited(MOD "%s: Out of RQT memory\n",
+				    pci_name(rdev->lldi.pdev));
 	mutex_lock(&rdev->stats.lock);
 	if (addr) {
 		rdev->stats.rqt.cur += roundup(size << 6, 1 << MIN_RQT_SHIFT);
diff --git a/drivers/infiniband/hw/cxgb4/t4.h b/drivers/infiniband/hw/cxgb4/t4.h
index eeca8b1e637..2178f319841 100644
--- a/drivers/infiniband/hw/cxgb4/t4.h
+++ b/drivers/infiniband/hw/cxgb4/t4.h
@@ -84,7 +84,14 @@ struct t4_status_page {
 			sizeof(struct fw_ri_isgl)) / sizeof(struct fw_ri_sge))
 #define T4_MAX_FR_IMMD ((T4_SQ_NUM_BYTES - sizeof(struct fw_ri_fr_nsmr_wr) - \
 			sizeof(struct fw_ri_immd)) & ~31UL)
-#define T4_MAX_FR_DEPTH (1024 / sizeof(u64))
+#define T4_MAX_FR_IMMD_DEPTH (T4_MAX_FR_IMMD / sizeof(u64))
+#define T4_MAX_FR_DSGL 1024
+#define T4_MAX_FR_DSGL_DEPTH (T4_MAX_FR_DSGL / sizeof(u64))
+
+static inline int t4_max_fr_depth(int use_dsgl)
+{
+	return use_dsgl ? T4_MAX_FR_DSGL_DEPTH : T4_MAX_FR_IMMD_DEPTH;
+}
 
 #define T4_RQ_NUM_SLOTS 2
 #define T4_RQ_NUM_BYTES (T4_EQ_ENTRY_SIZE * T4_RQ_NUM_SLOTS)
@@ -292,7 +299,7 @@ struct t4_sq {
 	unsigned long phys_addr;
 	struct t4_swsqe *sw_sq;
 	struct t4_swsqe *oldest_read;
-	u64 udb;
+	u64 __iomem *udb;
 	size_t memsize;
 	u32 qid;
 	u16 in_use;
@@ -314,7 +321,7 @@ struct t4_rq {
 	dma_addr_t dma_addr;
 	DEFINE_DMA_UNMAP_ADDR(mapping);
 	struct t4_swrqe *sw_rq;
-	u64 udb;
+	u64 __iomem *udb;
 	size_t memsize;
 	u32 qid;
 	u32 msn;
@@ -435,15 +442,67 @@ static inline u16 t4_sq_wq_size(struct t4_wq *wq)
 		return wq->sq.size * T4_SQ_NUM_SLOTS;
 }
 
-static inline void t4_ring_sq_db(struct t4_wq *wq, u16 inc)
+/* This function copies 64 byte coalesced work request to memory
+ * mapped BAR2 space. For coalesced WRs, the SGE fetches data
+ * from the FIFO instead of from Host.
+ */
+static inline void pio_copy(u64 __iomem *dst, u64 *src)
+{
+	int count = 8;
+
+	while (count) {
+		writeq(*src, dst);
+		src++;
+		dst++;
+		count--;
+	}
+}
+
+static inline void t4_ring_sq_db(struct t4_wq *wq, u16 inc, u8 t5,
+				 union t4_wr *wqe)
 {
+
+	/* Flush host queue memory writes. */
 	wmb();
+	if (t5) {
+		if (inc == 1 && wqe) {
+			PDBG("%s: WC wq->sq.pidx = %d\n",
+			     __func__, wq->sq.pidx);
+			pio_copy(wq->sq.udb + 7, (void *)wqe);
+		} else {
+			PDBG("%s: DB wq->sq.pidx = %d\n",
+			     __func__, wq->sq.pidx);
+			writel(PIDX_T5(inc), wq->sq.udb);
+		}
+
+		/* Flush user doorbell area writes. */
+		wmb();
+		return;
+	}
 	writel(QID(wq->sq.qid) | PIDX(inc), wq->db);
 }
 
-static inline void t4_ring_rq_db(struct t4_wq *wq, u16 inc)
+static inline void t4_ring_rq_db(struct t4_wq *wq, u16 inc, u8 t5,
+				 union t4_recv_wr *wqe)
 {
+
+	/* Flush host queue memory writes. */
 	wmb();
+	if (t5) {
+		if (inc == 1 && wqe) {
+			PDBG("%s: WC wq->rq.pidx = %d\n",
+			     __func__, wq->rq.pidx);
+			pio_copy(wq->rq.udb + 7, (void *)wqe);
+		} else {
+			PDBG("%s: DB wq->rq.pidx = %d\n",
+			     __func__, wq->rq.pidx);
+			writel(PIDX_T5(inc), wq->rq.udb);
+		}
+
+		/* Flush user doorbell area writes. */
+		wmb();
+		return;
+	}
 	writel(QID(wq->rq.qid) | PIDX(inc), wq->db);
 }
 
@@ -568,6 +627,9 @@ static inline int t4_next_hw_cqe(struct t4_cq *cq, struct t4_cqe **cqe)
 		printk(KERN_ERR MOD "cq overflow cqid %u\n", cq->cqid);
 		BUG_ON(1);
 	} else if (t4_valid_cqe(cq, &cq->queue[cq->cidx])) {
+
+		/* Ensure CQE is flushed to memory */
+		rmb();
 		*cqe = &cq->queue[cq->cidx];
 		ret = 0;
 	} else
diff --git a/drivers/infiniband/hw/cxgb4/t4fw_ri_api.h b/drivers/infiniband/hw/cxgb4/t4fw_ri_api.h
index dc193c29267..6121ca08fe5 100644
--- a/drivers/infiniband/hw/cxgb4/t4fw_ri_api.h
+++ b/drivers/infiniband/hw/cxgb4/t4fw_ri_api.h
@@ -836,4 +836,18 @@ struct ulptx_idata {
 #define V_RX_DACK_CHANGE(x) ((x) << S_RX_DACK_CHANGE)
 #define F_RX_DACK_CHANGE    V_RX_DACK_CHANGE(1U)
 
+enum {                     /* TCP congestion control algorithms */
+	CONG_ALG_RENO,
+	CONG_ALG_TAHOE,
+	CONG_ALG_NEWRENO,
+	CONG_ALG_HIGHSPEED
+};
+
+#define S_CONG_CNTRL    14
+#define M_CONG_CNTRL    0x3
+#define V_CONG_CNTRL(x) ((x) << S_CONG_CNTRL)
+#define G_CONG_CNTRL(x) (((x) >> S_CONG_CNTRL) & M_CONG_CNTRL)
+
+#define T5_OPT_2_VALID       (1 << 31)
+
 #endif /* _T4FW_RI_API_H_ */
diff --git a/drivers/infiniband/hw/mlx4/main.c b/drivers/infiniband/hw/mlx4/main.c
index 1b6dbe156a3..199c7896f08 100644
--- a/drivers/infiniband/hw/mlx4/main.c
+++ b/drivers/infiniband/hw/mlx4/main.c
@@ -48,6 +48,7 @@
 
 #include <linux/mlx4/driver.h>
 #include <linux/mlx4/cmd.h>
+#include <linux/mlx4/qp.h>
 
 #include "mlx4_ib.h"
 #include "user.h"
@@ -1614,6 +1615,53 @@ static int mlx4_ib_inet6_event(struct notifier_block *this, unsigned long event,
 }
 #endif
 
+#define MLX4_IB_INVALID_MAC	((u64)-1)
+static void mlx4_ib_update_qps(struct mlx4_ib_dev *ibdev,
+			       struct net_device *dev,
+			       int port)
+{
+	u64 new_smac = 0;
+	u64 release_mac = MLX4_IB_INVALID_MAC;
+	struct mlx4_ib_qp *qp;
+
+	read_lock(&dev_base_lock);
+	new_smac = mlx4_mac_to_u64(dev->dev_addr);
+	read_unlock(&dev_base_lock);
+
+	mutex_lock(&ibdev->qp1_proxy_lock[port - 1]);
+	qp = ibdev->qp1_proxy[port - 1];
+	if (qp) {
+		int new_smac_index;
+		u64 old_smac = qp->pri.smac;
+		struct mlx4_update_qp_params update_params;
+
+		if (new_smac == old_smac)
+			goto unlock;
+
+		new_smac_index = mlx4_register_mac(ibdev->dev, port, new_smac);
+
+		if (new_smac_index < 0)
+			goto unlock;
+
+		update_params.smac_index = new_smac_index;
+		if (mlx4_update_qp(ibdev->dev, &qp->mqp, MLX4_UPDATE_QP_SMAC,
+				   &update_params)) {
+			release_mac = new_smac;
+			goto unlock;
+		}
+
+		qp->pri.smac = new_smac;
+		qp->pri.smac_index = new_smac_index;
+
+		release_mac = old_smac;
+	}
+
+unlock:
+	mutex_unlock(&ibdev->qp1_proxy_lock[port - 1]);
+	if (release_mac != MLX4_IB_INVALID_MAC)
+		mlx4_unregister_mac(ibdev->dev, port, release_mac);
+}
+
 static void mlx4_ib_get_dev_addr(struct net_device *dev,
 				 struct mlx4_ib_dev *ibdev, u8 port)
 {
@@ -1689,9 +1737,13 @@ static int mlx4_ib_init_gid_table(struct mlx4_ib_dev *ibdev)
 	return 0;
 }
 
-static void mlx4_ib_scan_netdevs(struct mlx4_ib_dev *ibdev)
+static void mlx4_ib_scan_netdevs(struct mlx4_ib_dev *ibdev,
+				 struct net_device *dev,
+				 unsigned long event)
+
 {
 	struct mlx4_ib_iboe *iboe;
+	int update_qps_port = -1;
 	int port;
 
 	iboe = &ibdev->iboe;
@@ -1719,6 +1771,11 @@ static void mlx4_ib_scan_netdevs(struct mlx4_ib_dev *ibdev)
 		}
 		curr_master = iboe->masters[port - 1];
 
+		if (dev == iboe->netdevs[port - 1] &&
+		    (event == NETDEV_CHANGEADDR || event == NETDEV_REGISTER ||
+		     event == NETDEV_UP || event == NETDEV_CHANGE))
+			update_qps_port = port;
+
 		if (curr_netdev) {
 			port_state = (netif_running(curr_netdev) && netif_carrier_ok(curr_netdev)) ?
 						IB_PORT_ACTIVE : IB_PORT_DOWN;
@@ -1752,6 +1809,9 @@ static void mlx4_ib_scan_netdevs(struct mlx4_ib_dev *ibdev)
 	}
 
 	spin_unlock(&iboe->lock);
+
+	if (update_qps_port > 0)
+		mlx4_ib_update_qps(ibdev, dev, update_qps_port);
 }
 
 static int mlx4_ib_netdev_event(struct notifier_block *this,
@@ -1764,7 +1824,7 @@ static int mlx4_ib_netdev_event(struct notifier_block *this,
 		return NOTIFY_DONE;
 
 	ibdev = container_of(this, struct mlx4_ib_dev, iboe.nb);
-	mlx4_ib_scan_netdevs(ibdev);
+	mlx4_ib_scan_netdevs(ibdev, dev, event);
 
 	return NOTIFY_DONE;
 }
@@ -2043,6 +2103,7 @@ static void *mlx4_ib_add(struct mlx4_dev *dev)
 		goto err_map;
 
 	for (i = 0; i < ibdev->num_ports; ++i) {
+		mutex_init(&ibdev->qp1_proxy_lock[i]);
 		if (mlx4_ib_port_link_layer(&ibdev->ib_dev, i + 1) ==
 						IB_LINK_LAYER_ETHERNET) {
 			err = mlx4_counter_alloc(ibdev->dev, &ibdev->counters[i]);
@@ -2126,7 +2187,7 @@ static void *mlx4_ib_add(struct mlx4_dev *dev)
 		for (i = 1 ; i <= ibdev->num_ports ; ++i)
 			reset_gid_table(ibdev, i);
 		rtnl_lock();
-		mlx4_ib_scan_netdevs(ibdev);
+		mlx4_ib_scan_netdevs(ibdev, NULL, 0);
 		rtnl_unlock();
 		mlx4_ib_init_gid_table(ibdev);
 	}
diff --git a/drivers/infiniband/hw/mlx4/mlx4_ib.h b/drivers/infiniband/hw/mlx4/mlx4_ib.h
index f589522fddf..66b0b7dbd9f 100644
--- a/drivers/infiniband/hw/mlx4/mlx4_ib.h
+++ b/drivers/infiniband/hw/mlx4/mlx4_ib.h
@@ -522,6 +522,9 @@ struct mlx4_ib_dev {
 	int steer_qpn_count;
 	int steer_qpn_base;
 	int steering_support;
+	struct mlx4_ib_qp      *qp1_proxy[MLX4_MAX_PORTS];
+	/* lock when destroying qp1_proxy and getting netdev events */
+	struct mutex		qp1_proxy_lock[MLX4_MAX_PORTS];
 };
 
 struct ib_event_work {
diff --git a/drivers/infiniband/hw/mlx4/qp.c b/drivers/infiniband/hw/mlx4/qp.c
index 41308af4163..dc57482ae7a 100644
--- a/drivers/infiniband/hw/mlx4/qp.c
+++ b/drivers/infiniband/hw/mlx4/qp.c
@@ -1132,6 +1132,12 @@ int mlx4_ib_destroy_qp(struct ib_qp *qp)
 	if (is_qp0(dev, mqp))
 		mlx4_CLOSE_PORT(dev->dev, mqp->port);
 
+	if (dev->qp1_proxy[mqp->port - 1] == mqp) {
+		mutex_lock(&dev->qp1_proxy_lock[mqp->port - 1]);
+		dev->qp1_proxy[mqp->port - 1] = NULL;
+		mutex_unlock(&dev->qp1_proxy_lock[mqp->port - 1]);
+	}
+
 	pd = get_pd(mqp);
 	destroy_qp_common(dev, mqp, !!pd->ibpd.uobject);
 
@@ -1646,6 +1652,8 @@ static int __mlx4_ib_modify_qp(struct ib_qp *ibqp,
 				err = handle_eth_ud_smac_index(dev, qp, (u8 *)attr->smac, context);
 				if (err)
 					return -EINVAL;
+				if (qp->mlx4_ib_qp_type == MLX4_IB_QPT_PROXY_GSI)
+					dev->qp1_proxy[qp->port - 1] = qp;
 			}
 		}
 	}
diff --git a/drivers/infiniband/hw/mlx5/main.c b/drivers/infiniband/hw/mlx5/main.c
index fa6dc870ada..364d4b6937f 100644
--- a/drivers/infiniband/hw/mlx5/main.c
+++ b/drivers/infiniband/hw/mlx5/main.c
@@ -282,6 +282,8 @@ static int mlx5_ib_query_device(struct ib_device *ibdev,
 		props->sig_guard_cap = IB_GUARD_T10DIF_CRC |
 				       IB_GUARD_T10DIF_CSUM;
 	}
+	if (flags & MLX5_DEV_CAP_FLAG_BLOCK_MCAST)
+		props->device_cap_flags |= IB_DEVICE_BLOCK_MULTICAST_LOOPBACK;
 
 	props->vendor_id	   = be32_to_cpup((__be32 *)(out_mad->data + 36)) &
 		0xffffff;
diff --git a/drivers/infiniband/hw/mlx5/qp.c b/drivers/infiniband/hw/mlx5/qp.c
index ae788d27b93..dc930ed21ec 100644
--- a/drivers/infiniband/hw/mlx5/qp.c
+++ b/drivers/infiniband/hw/mlx5/qp.c
@@ -807,6 +807,15 @@ static int create_qp_common(struct mlx5_ib_dev *dev, struct ib_pd *pd,
 	spin_lock_init(&qp->sq.lock);
 	spin_lock_init(&qp->rq.lock);
 
+	if (init_attr->create_flags & IB_QP_CREATE_BLOCK_MULTICAST_LOOPBACK) {
+		if (!(dev->mdev.caps.flags & MLX5_DEV_CAP_FLAG_BLOCK_MCAST)) {
+			mlx5_ib_dbg(dev, "block multicast loopback isn't supported\n");
+			return -EINVAL;
+		} else {
+			qp->flags |= MLX5_IB_QP_BLOCK_MULTICAST_LOOPBACK;
+		}
+	}
+
 	if (init_attr->sq_sig_type == IB_SIGNAL_ALL_WR)
 		qp->sq_signal_bits = MLX5_WQE_CTRL_CQ_UPDATE;
 
@@ -878,6 +887,9 @@ static int create_qp_common(struct mlx5_ib_dev *dev, struct ib_pd *pd,
 	if (qp->wq_sig)
 		in->ctx.flags_pd |= cpu_to_be32(MLX5_QP_ENABLE_SIG);
 
+	if (qp->flags & MLX5_IB_QP_BLOCK_MULTICAST_LOOPBACK)
+		in->ctx.flags_pd |= cpu_to_be32(MLX5_QP_BLOCK_MCAST);
+
 	if (qp->scat_cqe && is_connected(init_attr->qp_type)) {
 		int rcqe_sz;
 		int scqe_sz;
diff --git a/drivers/infiniband/hw/mthca/mthca_main.c b/drivers/infiniband/hw/mthca/mthca_main.c
index 87897b95666..ded76c101dd 100644
--- a/drivers/infiniband/hw/mthca/mthca_main.c
+++ b/drivers/infiniband/hw/mthca/mthca_main.c
@@ -858,13 +858,9 @@ static int mthca_enable_msi_x(struct mthca_dev *mdev)
 	entries[1].entry = 1;
 	entries[2].entry = 2;
 
-	err = pci_enable_msix(mdev->pdev, entries, ARRAY_SIZE(entries));
-	if (err) {
-		if (err > 0)
-			mthca_info(mdev, "Only %d MSI-X vectors available, "
-				   "not using MSI-X\n", err);
+	err = pci_enable_msix_exact(mdev->pdev, entries, ARRAY_SIZE(entries));
+	if (err)
 		return err;
-	}
 
 	mdev->eq_table.eq[MTHCA_EQ_COMP ].msi_x_vector = entries[0].vector;
 	mdev->eq_table.eq[MTHCA_EQ_ASYNC].msi_x_vector = entries[1].vector;
diff --git a/drivers/infiniband/hw/qib/qib_pcie.c b/drivers/infiniband/hw/qib/qib_pcie.c
index c8d9c4ab142..61a0046efb7 100644
--- a/drivers/infiniband/hw/qib/qib_pcie.c
+++ b/drivers/infiniband/hw/qib/qib_pcie.c
@@ -197,46 +197,47 @@ static void qib_msix_setup(struct qib_devdata *dd, int pos, u32 *msixcnt,
 			   struct qib_msix_entry *qib_msix_entry)
 {
 	int ret;
-	u32 tabsize = 0;
-	u16 msix_flags;
+	int nvec = *msixcnt;
 	struct msix_entry *msix_entry;
 	int i;
 
+	ret = pci_msix_vec_count(dd->pcidev);
+	if (ret < 0)
+		goto do_intx;
+
+	nvec = min(nvec, ret);
+
 	/* We can't pass qib_msix_entry array to qib_msix_setup
 	 * so use a dummy msix_entry array and copy the allocated
 	 * irq back to the qib_msix_entry array. */
-	msix_entry = kmalloc(*msixcnt * sizeof(*msix_entry), GFP_KERNEL);
-	if (!msix_entry) {
-		ret = -ENOMEM;
+	msix_entry = kmalloc(nvec * sizeof(*msix_entry), GFP_KERNEL);
+	if (!msix_entry)
 		goto do_intx;
-	}
-	for (i = 0; i < *msixcnt; i++)
+
+	for (i = 0; i < nvec; i++)
 		msix_entry[i] = qib_msix_entry[i].msix;
 
-	pci_read_config_word(dd->pcidev, pos + PCI_MSIX_FLAGS, &msix_flags);
-	tabsize = 1 + (msix_flags & PCI_MSIX_FLAGS_QSIZE);
-	if (tabsize > *msixcnt)
-		tabsize = *msixcnt;
-	ret = pci_enable_msix(dd->pcidev, msix_entry, tabsize);
-	if (ret > 0) {
-		tabsize = ret;
-		ret = pci_enable_msix(dd->pcidev, msix_entry, tabsize);
-	}
-do_intx:
-	if (ret) {
-		qib_dev_err(dd,
-			"pci_enable_msix %d vectors failed: %d, falling back to INTx\n",
-			tabsize, ret);
-		tabsize = 0;
-	}
-	for (i = 0; i < tabsize; i++)
+	ret = pci_enable_msix_range(dd->pcidev, msix_entry, 1, nvec);
+	if (ret < 0)
+		goto free_msix_entry;
+	else
+		nvec = ret;
+
+	for (i = 0; i < nvec; i++)
 		qib_msix_entry[i].msix = msix_entry[i];
+
 	kfree(msix_entry);
-	*msixcnt = tabsize;
+	*msixcnt = nvec;
+	return;
 
-	if (ret)
-		qib_enable_intx(dd->pcidev);
+free_msix_entry:
+	kfree(msix_entry);
 
+do_intx:
+	qib_dev_err(dd, "pci_enable_msix_range %d vectors failed: %d, "
+			"falling back to INTx\n", nvec, ret);
+	*msixcnt = 0;
+	qib_enable_intx(dd->pcidev);
 }
 
 /**
diff --git a/drivers/infiniband/ulp/isert/ib_isert.c b/drivers/infiniband/ulp/isert/ib_isert.c
index c98fdb18593..a1710465faa 100644
--- a/drivers/infiniband/ulp/isert/ib_isert.c
+++ b/drivers/infiniband/ulp/isert/ib_isert.c
@@ -28,6 +28,7 @@
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
 #include <target/iscsi/iscsi_transport.h>
+#include <linux/semaphore.h>
 
 #include "isert_proto.h"
 #include "ib_isert.h"
@@ -561,7 +562,15 @@ isert_connect_request(struct rdma_cm_id *cma_id, struct rdma_cm_event *event)
 	struct isert_device *device;
 	struct ib_device *ib_dev = cma_id->device;
 	int ret = 0;
-	u8 pi_support = np->tpg_np->tpg->tpg_attrib.t10_pi;
+	u8 pi_support;
+
+	spin_lock_bh(&np->np_thread_lock);
+	if (!np->enabled) {
+		spin_unlock_bh(&np->np_thread_lock);
+		pr_debug("iscsi_np is not enabled, reject connect request\n");
+		return rdma_reject(cma_id, NULL, 0);
+	}
+	spin_unlock_bh(&np->np_thread_lock);
 
 	pr_debug("Entering isert_connect_request cma_id: %p, context: %p\n",
 		 cma_id, cma_id->context);
@@ -652,6 +661,7 @@ isert_connect_request(struct rdma_cm_id *cma_id, struct rdma_cm_event *event)
 		goto out_mr;
 	}
 
+	pi_support = np->tpg_np->tpg->tpg_attrib.t10_pi;
 	if (pi_support && !device->pi_capable) {
 		pr_err("Protection information requested but not supported\n");
 		ret = -EINVAL;
@@ -663,11 +673,11 @@ isert_connect_request(struct rdma_cm_id *cma_id, struct rdma_cm_event *event)
 		goto out_conn_dev;
 
 	mutex_lock(&isert_np->np_accept_mutex);
-	list_add_tail(&isert_np->np_accept_list, &isert_conn->conn_accept_node);
+	list_add_tail(&isert_conn->conn_accept_node, &isert_np->np_accept_list);
 	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);
+	pr_debug("isert_connect_request() up np_sem np: %p\n", np);
+	up(&isert_np->np_sem);
 	return 0;
 
 out_conn_dev:
@@ -2999,7 +3009,7 @@ isert_setup_np(struct iscsi_np *np,
 		pr_err("Unable to allocate struct isert_np\n");
 		return -ENOMEM;
 	}
-	init_waitqueue_head(&isert_np->np_accept_wq);
+	sema_init(&isert_np->np_sem, 0);
 	mutex_init(&isert_np->np_accept_mutex);
 	INIT_LIST_HEAD(&isert_np->np_accept_list);
 	init_completion(&isert_np->np_login_comp);
@@ -3048,18 +3058,6 @@ out:
 }
 
 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;
@@ -3151,16 +3149,14 @@ isert_accept_np(struct iscsi_np *np, struct iscsi_conn *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);
+	ret = down_interruptible(&isert_np->np_sem);
 	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");
+		pr_debug("ISCSI_NP_THREAD_RESET for isert_accept_np\n");
 		return -ENODEV;
 	}
 	spin_unlock_bh(&np->np_thread_lock);
diff --git a/drivers/infiniband/ulp/isert/ib_isert.h b/drivers/infiniband/ulp/isert/ib_isert.h
index 4c072ae34c0..da6612e6800 100644
--- a/drivers/infiniband/ulp/isert/ib_isert.h
+++ b/drivers/infiniband/ulp/isert/ib_isert.h
@@ -182,7 +182,7 @@ struct isert_device {
 };
 
 struct isert_np {
-	wait_queue_head_t	np_accept_wq;
+	struct semaphore	np_sem;
 	struct rdma_cm_id	*np_cm_id;
 	struct mutex		np_accept_mutex;
 	struct list_head	np_accept_list;
diff --git a/drivers/input/keyboard/Kconfig b/drivers/input/keyboard/Kconfig
index 76842d7dc2e..ffc7ad3a2c8 100644
--- a/drivers/input/keyboard/Kconfig
+++ b/drivers/input/keyboard/Kconfig
@@ -71,7 +71,7 @@ config KEYBOARD_ATKBD
 	default y
 	select SERIO
 	select SERIO_LIBPS2
-	select SERIO_I8042 if X86
+	select SERIO_I8042 if ARCH_MIGHT_HAVE_PC_SERIO
 	select SERIO_GSCPS2 if GSC
 	help
 	  Say Y here if you want to use a standard AT or PS/2 keyboard. Usually
diff --git a/drivers/input/keyboard/atkbd.c b/drivers/input/keyboard/atkbd.c
index 2626773ff29..2dd1d0dd4f7 100644
--- a/drivers/input/keyboard/atkbd.c
+++ b/drivers/input/keyboard/atkbd.c
@@ -243,6 +243,12 @@ static void (*atkbd_platform_fixup)(struct atkbd *, const void *data);
 static void *atkbd_platform_fixup_data;
 static unsigned int (*atkbd_platform_scancode_fixup)(struct atkbd *, unsigned int);
 
+/*
+ * Certain keyboards to not like ATKBD_CMD_RESET_DIS and stop responding
+ * to many commands until full reset (ATKBD_CMD_RESET_BAT) is performed.
+ */
+static bool atkbd_skip_deactivate;
+
 static ssize_t atkbd_attr_show_helper(struct device *dev, char *buf,
 				ssize_t (*handler)(struct atkbd *, char *));
 static ssize_t atkbd_attr_set_helper(struct device *dev, const char *buf, size_t count,
@@ -768,7 +774,8 @@ static int atkbd_probe(struct atkbd *atkbd)
  * Make sure nothing is coming from the keyboard and disturbs our
  * internal state.
  */
-	atkbd_deactivate(atkbd);
+	if (!atkbd_skip_deactivate)
+		atkbd_deactivate(atkbd);
 
 	return 0;
 }
@@ -1638,6 +1645,12 @@ static int __init atkbd_setup_scancode_fixup(const struct dmi_system_id *id)
 	return 1;
 }
 
+static int __init atkbd_deactivate_fixup(const struct dmi_system_id *id)
+{
+	atkbd_skip_deactivate = true;
+	return 1;
+}
+
 static const struct dmi_system_id atkbd_dmi_quirk_table[] __initconst = {
 	{
 		.matches = {
@@ -1775,6 +1788,20 @@ static const struct dmi_system_id atkbd_dmi_quirk_table[] __initconst = {
 		.callback = atkbd_setup_scancode_fixup,
 		.driver_data = atkbd_oqo_01plus_scancode_fixup,
 	},
+	{
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "LG Electronics"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "LW25-B7HV"),
+		},
+		.callback = atkbd_deactivate_fixup,
+	},
+	{
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "LG Electronics"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "P1-J273B"),
+		},
+		.callback = atkbd_deactivate_fixup,
+	},
 	{ }
 };
 
diff --git a/drivers/input/keyboard/pxa27x_keypad.c b/drivers/input/keyboard/pxa27x_keypad.c
index d8241ba0afa..a15063bea70 100644
--- a/drivers/input/keyboard/pxa27x_keypad.c
+++ b/drivers/input/keyboard/pxa27x_keypad.c
@@ -111,6 +111,8 @@ struct pxa27x_keypad {
 	unsigned short keycodes[MAX_KEYPAD_KEYS];
 	int rotary_rel_code[2];
 
+	unsigned int row_shift;
+
 	/* state row bits of each column scan */
 	uint32_t matrix_key_state[MAX_MATRIX_KEY_COLS];
 	uint32_t direct_key_state;
@@ -467,7 +469,8 @@ scan:
 			if ((bits_changed & (1 << row)) == 0)
 				continue;
 
-			code = MATRIX_SCAN_CODE(row, col, MATRIX_ROW_SHIFT);
+			code = MATRIX_SCAN_CODE(row, col, keypad->row_shift);
+
 			input_event(input_dev, EV_MSC, MSC_SCAN, code);
 			input_report_key(input_dev, keypad->keycodes[code],
 					 new_state[col] & (1 << row));
@@ -802,6 +805,8 @@ static int pxa27x_keypad_probe(struct platform_device *pdev)
 		goto failed_put_clk;
 	}
 
+	keypad->row_shift = get_count_order(pdata->matrix_key_cols);
+
 	if ((pdata->enable_rotary0 && keypad->rotary_rel_code[0] != -1) ||
 	    (pdata->enable_rotary1 && keypad->rotary_rel_code[1] != -1)) {
 		input_dev->evbit[0] |= BIT_MASK(EV_REL);
diff --git a/drivers/input/keyboard/tca8418_keypad.c b/drivers/input/keyboard/tca8418_keypad.c
index 55c15304ddb..4e491c1762c 100644
--- a/drivers/input/keyboard/tca8418_keypad.c
+++ b/drivers/input/keyboard/tca8418_keypad.c
@@ -392,6 +392,13 @@ static const struct of_device_id tca8418_dt_ids[] = {
 	{ }
 };
 MODULE_DEVICE_TABLE(of, tca8418_dt_ids);
+
+/*
+ * The device tree based i2c loader looks for
+ * "i2c:" + second_component_of(property("compatible"))
+ * and therefore we need an alias to be found.
+ */
+MODULE_ALIAS("i2c:tca8418");
 #endif
 
 static struct i2c_driver tca8418_keypad_driver = {
diff --git a/drivers/input/misc/bma150.c b/drivers/input/misc/bma150.c
index 52d3a9b28f0..b36831c828d 100644
--- a/drivers/input/misc/bma150.c
+++ b/drivers/input/misc/bma150.c
@@ -70,6 +70,7 @@
 #define BMA150_CFG_5_REG	0x11
 
 #define BMA150_CHIP_ID		2
+#define BMA180_CHIP_ID		3
 #define BMA150_CHIP_ID_REG	BMA150_DATA_0_REG
 
 #define BMA150_ACC_X_LSB_REG	BMA150_DATA_2_REG
@@ -539,7 +540,7 @@ static int bma150_probe(struct i2c_client *client,
 	}
 
 	chip_id = i2c_smbus_read_byte_data(client, BMA150_CHIP_ID_REG);
-	if (chip_id != BMA150_CHIP_ID) {
+	if (chip_id != BMA150_CHIP_ID && chip_id != BMA180_CHIP_ID) {
 		dev_err(&client->dev, "BMA150 chip id error: %d\n", chip_id);
 		return -EINVAL;
 	}
@@ -643,6 +644,7 @@ static UNIVERSAL_DEV_PM_OPS(bma150_pm, bma150_suspend, bma150_resume, NULL);
 
 static const struct i2c_device_id bma150_id[] = {
 	{ "bma150", 0 },
+	{ "bma180", 0 },
 	{ "smb380", 0 },
 	{ "bma023", 0 },
 	{ }
diff --git a/drivers/input/misc/da9055_onkey.c b/drivers/input/misc/da9055_onkey.c
index 4b11ede3495..4765799fef7 100644
--- a/drivers/input/misc/da9055_onkey.c
+++ b/drivers/input/misc/da9055_onkey.c
@@ -109,7 +109,6 @@ static int da9055_onkey_probe(struct platform_device *pdev)
 
 	INIT_DELAYED_WORK(&onkey->work, da9055_onkey_work);
 
-	irq = regmap_irq_get_virq(da9055->irq_data, irq);
 	err = request_threaded_irq(irq, NULL, da9055_onkey_irq,
 				   IRQF_TRIGGER_HIGH | IRQF_ONESHOT,
 				   "ONKEY", onkey);
diff --git a/drivers/input/misc/soc_button_array.c b/drivers/input/misc/soc_button_array.c
index 08ead2aaede..20c80f543d5 100644
--- a/drivers/input/misc/soc_button_array.c
+++ b/drivers/input/misc/soc_button_array.c
@@ -169,6 +169,7 @@ static int soc_button_pnp_probe(struct pnp_dev *pdev,
 				soc_button_remove(pdev);
 				return error;
 			}
+			continue;
 		}
 
 		priv->children[i] = pd;
diff --git a/drivers/input/mouse/Kconfig b/drivers/input/mouse/Kconfig
index effa9c5f2c5..6b8441f7bc3 100644
--- a/drivers/input/mouse/Kconfig
+++ b/drivers/input/mouse/Kconfig
@@ -17,7 +17,7 @@ config MOUSE_PS2
 	default y
 	select SERIO
 	select SERIO_LIBPS2
-	select SERIO_I8042 if X86
+	select SERIO_I8042 if ARCH_MIGHT_HAVE_PC_SERIO
 	select SERIO_GSCPS2 if GSC
 	help
 	  Say Y here if you have a PS/2 mouse connected to your system. This
diff --git a/drivers/input/mouse/elantech.c b/drivers/input/mouse/elantech.c
index ef1cf52f8bb..b96e978a37b 100644
--- a/drivers/input/mouse/elantech.c
+++ b/drivers/input/mouse/elantech.c
@@ -11,6 +11,7 @@
  */
 
 #include <linux/delay.h>
+#include <linux/dmi.h>
 #include <linux/slab.h>
 #include <linux/module.h>
 #include <linux/input.h>
@@ -831,7 +832,11 @@ static int elantech_set_absolute_mode(struct psmouse *psmouse)
 		break;
 
 	case 3:
-		etd->reg_10 = 0x0b;
+		if (etd->set_hw_resolution)
+			etd->reg_10 = 0x0b;
+		else
+			etd->reg_10 = 0x03;
+
 		if (elantech_write_reg(psmouse, 0x10, etd->reg_10))
 			rc = -1;
 
@@ -1331,6 +1336,22 @@ static int elantech_reconnect(struct psmouse *psmouse)
 }
 
 /*
+ * Some hw_version 3 models go into error state when we try to set bit 3 of r10
+ */
+static const struct dmi_system_id no_hw_res_dmi_table[] = {
+#if defined(CONFIG_DMI) && defined(CONFIG_X86)
+	{
+		/* Gigabyte U2442 */
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "GIGABYTE"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "U2442"),
+		},
+	},
+#endif
+	{ }
+};
+
+/*
  * determine hardware version and set some properties according to it.
  */
 static int elantech_set_properties(struct elantech_data *etd)
@@ -1353,6 +1374,7 @@ static int elantech_set_properties(struct elantech_data *etd)
 		case 6:
 		case 7:
 		case 8:
+		case 9:
 			etd->hw_version = 4;
 			break;
 		default:
@@ -1389,6 +1411,9 @@ static int elantech_set_properties(struct elantech_data *etd)
 	 */
 	etd->crc_enabled = ((etd->fw_version & 0x4000) == 0x4000);
 
+	/* Enable real hardware resolution on hw_version 3 ? */
+	etd->set_hw_resolution = !dmi_check_system(no_hw_res_dmi_table);
+
 	return 0;
 }
 
diff --git a/drivers/input/mouse/elantech.h b/drivers/input/mouse/elantech.h
index 036a04abaef..9e0e2a1f340 100644
--- a/drivers/input/mouse/elantech.h
+++ b/drivers/input/mouse/elantech.h
@@ -130,6 +130,7 @@ struct elantech_data {
 	bool jumpy_cursor;
 	bool reports_pressure;
 	bool crc_enabled;
+	bool set_hw_resolution;
 	unsigned char hw_version;
 	unsigned int fw_version;
 	unsigned int single_finger_reports;
diff --git a/drivers/input/mouse/synaptics.c b/drivers/input/mouse/synaptics.c
index d8d49d10f9b..c5ec703c727 100644
--- a/drivers/input/mouse/synaptics.c
+++ b/drivers/input/mouse/synaptics.c
@@ -117,6 +117,81 @@ void synaptics_reset(struct psmouse *psmouse)
 }
 
 #ifdef CONFIG_MOUSE_PS2_SYNAPTICS
+struct min_max_quirk {
+	const char * const *pnp_ids;
+	int x_min, x_max, y_min, y_max;
+};
+
+static const struct min_max_quirk min_max_pnpid_table[] = {
+	{
+		(const char * const []){"LEN0033", NULL},
+		1024, 5052, 2258, 4832
+	},
+	{
+		(const char * const []){"LEN0035", "LEN0042", NULL},
+		1232, 5710, 1156, 4696
+	},
+	{
+		(const char * const []){"LEN0034", "LEN0036", "LEN2004", NULL},
+		1024, 5112, 2024, 4832
+	},
+	{
+		(const char * const []){"LEN2001", NULL},
+		1024, 5022, 2508, 4832
+	},
+	{ }
+};
+
+/* This list has been kindly provided by Synaptics. */
+static const char * const topbuttonpad_pnp_ids[] = {
+	"LEN0017",
+	"LEN0018",
+	"LEN0019",
+	"LEN0023",
+	"LEN002A",
+	"LEN002B",
+	"LEN002C",
+	"LEN002D",
+	"LEN002E",
+	"LEN0033", /* Helix */
+	"LEN0034", /* T431s, L440, L540, T540, W540, X1 Carbon 2nd */
+	"LEN0035", /* X240 */
+	"LEN0036", /* T440 */
+	"LEN0037",
+	"LEN0038",
+	"LEN0041",
+	"LEN0042", /* Yoga */
+	"LEN0045",
+	"LEN0046",
+	"LEN0047",
+	"LEN0048",
+	"LEN0049",
+	"LEN2000",
+	"LEN2001", /* Edge E431 */
+	"LEN2002",
+	"LEN2003",
+	"LEN2004", /* L440 */
+	"LEN2005",
+	"LEN2006",
+	"LEN2007",
+	"LEN2008",
+	"LEN2009",
+	"LEN200A",
+	"LEN200B",
+	NULL
+};
+
+static bool matches_pnp_id(struct psmouse *psmouse, const char * const ids[])
+{
+	int i;
+
+	if (!strncmp(psmouse->ps2dev.serio->firmware_id, "PNP:", 4))
+		for (i = 0; ids[i]; i++)
+			if (strstr(psmouse->ps2dev.serio->firmware_id, ids[i]))
+				return true;
+
+	return false;
+}
 
 /*****************************************************************************
  *	Synaptics communications functions
@@ -266,20 +341,20 @@ static int synaptics_identify(struct psmouse *psmouse)
  * Resolution is left zero if touchpad does not support the query
  */
 
-static const int *quirk_min_max;
-
 static int synaptics_resolution(struct psmouse *psmouse)
 {
 	struct synaptics_data *priv = psmouse->private;
 	unsigned char resp[3];
+	int i;
 
-	if (quirk_min_max) {
-		priv->x_min = quirk_min_max[0];
-		priv->x_max = quirk_min_max[1];
-		priv->y_min = quirk_min_max[2];
-		priv->y_max = quirk_min_max[3];
-		return 0;
-	}
+	for (i = 0; min_max_pnpid_table[i].pnp_ids; i++)
+		if (matches_pnp_id(psmouse, min_max_pnpid_table[i].pnp_ids)) {
+			priv->x_min = min_max_pnpid_table[i].x_min;
+			priv->x_max = min_max_pnpid_table[i].x_max;
+			priv->y_min = min_max_pnpid_table[i].y_min;
+			priv->y_max = min_max_pnpid_table[i].y_max;
+			return 0;
+		}
 
 	if (SYN_ID_MAJOR(priv->identity) < 4)
 		return 0;
@@ -1255,8 +1330,10 @@ static void set_abs_position_params(struct input_dev *dev,
 	input_abs_set_res(dev, y_code, priv->y_res);
 }
 
-static void set_input_params(struct input_dev *dev, struct synaptics_data *priv)
+static void set_input_params(struct psmouse *psmouse,
+			     struct synaptics_data *priv)
 {
+	struct input_dev *dev = psmouse->dev;
 	int i;
 
 	/* Things that apply to both modes */
@@ -1325,6 +1402,8 @@ static void set_input_params(struct input_dev *dev, struct synaptics_data *priv)
 
 	if (SYN_CAP_CLICKPAD(priv->ext_cap_0c)) {
 		__set_bit(INPUT_PROP_BUTTONPAD, dev->propbit);
+		if (matches_pnp_id(psmouse, topbuttonpad_pnp_ids))
+			__set_bit(INPUT_PROP_TOPBUTTONPAD, dev->propbit);
 		/* Clickpads report only left button */
 		__clear_bit(BTN_RIGHT, dev->keybit);
 		__clear_bit(BTN_MIDDLE, dev->keybit);
@@ -1496,54 +1575,10 @@ static const struct dmi_system_id olpc_dmi_table[] __initconst = {
 	{ }
 };
 
-static const struct dmi_system_id min_max_dmi_table[] __initconst = {
-#if defined(CONFIG_DMI)
-	{
-		/* Lenovo ThinkPad Helix */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-			DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad Helix"),
-		},
-		.driver_data = (int []){1024, 5052, 2258, 4832},
-	},
-	{
-		/* Lenovo ThinkPad X240 */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-			DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad X240"),
-		},
-		.driver_data = (int []){1232, 5710, 1156, 4696},
-	},
-	{
-		/* Lenovo ThinkPad T440s */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-			DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad T440"),
-		},
-		.driver_data = (int []){1024, 5112, 2024, 4832},
-	},
-	{
-		/* Lenovo ThinkPad T540p */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-			DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad T540"),
-		},
-		.driver_data = (int []){1024, 5056, 2058, 4832},
-	},
-#endif
-	{ }
-};
-
 void __init synaptics_module_init(void)
 {
-	const struct dmi_system_id *min_max_dmi;
-
 	impaired_toshiba_kbc = dmi_check_system(toshiba_dmi_table);
 	broken_olpc_ec = dmi_check_system(olpc_dmi_table);
-
-	min_max_dmi = dmi_first_match(min_max_dmi_table);
-	if (min_max_dmi)
-		quirk_min_max = min_max_dmi->driver_data;
 }
 
 static int __synaptics_init(struct psmouse *psmouse, bool absolute_mode)
@@ -1593,7 +1628,7 @@ static int __synaptics_init(struct psmouse *psmouse, bool absolute_mode)
 		     priv->capabilities, priv->ext_cap, priv->ext_cap_0c,
 		     priv->board_id, priv->firmware_id);
 
-	set_input_params(psmouse->dev, priv);
+	set_input_params(psmouse, priv);
 
 	/*
 	 * Encode touchpad model so that it can be used to set
diff --git a/drivers/input/serio/ambakmi.c b/drivers/input/serio/ambakmi.c
index 762b08432de..8b748d99b93 100644
--- a/drivers/input/serio/ambakmi.c
+++ b/drivers/input/serio/ambakmi.c
@@ -79,7 +79,8 @@ static int amba_kmi_open(struct serio *io)
 	writeb(divisor, KMICLKDIV);
 	writeb(KMICR_EN, KMICR);
 
-	ret = request_irq(kmi->irq, amba_kmi_int, 0, "kmi-pl050", kmi);
+	ret = request_irq(kmi->irq, amba_kmi_int, IRQF_SHARED, "kmi-pl050",
+			  kmi);
 	if (ret) {
 		printk(KERN_ERR "kmi: failed to claim IRQ%d\n", kmi->irq);
 		writeb(0, KMICR);
diff --git a/drivers/input/serio/i8042-x86ia64io.h b/drivers/input/serio/i8042-x86ia64io.h
index 0ec9abbe31f..381b20d4c56 100644
--- a/drivers/input/serio/i8042-x86ia64io.h
+++ b/drivers/input/serio/i8042-x86ia64io.h
@@ -702,6 +702,17 @@ static int i8042_pnp_aux_irq;
 static char i8042_pnp_kbd_name[32];
 static char i8042_pnp_aux_name[32];
 
+static void i8042_pnp_id_to_string(struct pnp_id *id, char *dst, int dst_size)
+{
+	strlcpy(dst, "PNP:", dst_size);
+
+	while (id) {
+		strlcat(dst, " ", dst_size);
+		strlcat(dst, id->id, dst_size);
+		id = id->next;
+	}
+}
+
 static int i8042_pnp_kbd_probe(struct pnp_dev *dev, const struct pnp_device_id *did)
 {
 	if (pnp_port_valid(dev, 0) && pnp_port_len(dev, 0) == 1)
@@ -718,6 +729,8 @@ static int i8042_pnp_kbd_probe(struct pnp_dev *dev, const struct pnp_device_id *
 		strlcat(i8042_pnp_kbd_name, ":", sizeof(i8042_pnp_kbd_name));
 		strlcat(i8042_pnp_kbd_name, pnp_dev_name(dev), sizeof(i8042_pnp_kbd_name));
 	}
+	i8042_pnp_id_to_string(dev->id, i8042_kbd_firmware_id,
+			       sizeof(i8042_kbd_firmware_id));
 
 	/* Keyboard ports are always supposed to be wakeup-enabled */
 	device_set_wakeup_enable(&dev->dev, true);
@@ -742,6 +755,8 @@ static int i8042_pnp_aux_probe(struct pnp_dev *dev, const struct pnp_device_id *
 		strlcat(i8042_pnp_aux_name, ":", sizeof(i8042_pnp_aux_name));
 		strlcat(i8042_pnp_aux_name, pnp_dev_name(dev), sizeof(i8042_pnp_aux_name));
 	}
+	i8042_pnp_id_to_string(dev->id, i8042_aux_firmware_id,
+			       sizeof(i8042_aux_firmware_id));
 
 	i8042_pnp_aux_devices++;
 	return 0;
diff --git a/drivers/input/serio/i8042.c b/drivers/input/serio/i8042.c
index 020053fa5aa..3807c3e971c 100644
--- a/drivers/input/serio/i8042.c
+++ b/drivers/input/serio/i8042.c
@@ -87,6 +87,8 @@ MODULE_PARM_DESC(debug, "Turn i8042 debugging mode on and off");
 #endif
 
 static bool i8042_bypass_aux_irq_test;
+static char i8042_kbd_firmware_id[128];
+static char i8042_aux_firmware_id[128];
 
 #include "i8042.h"
 
@@ -1218,6 +1220,8 @@ static int __init i8042_create_kbd_port(void)
 	serio->dev.parent	= &i8042_platform_device->dev;
 	strlcpy(serio->name, "i8042 KBD port", sizeof(serio->name));
 	strlcpy(serio->phys, I8042_KBD_PHYS_DESC, sizeof(serio->phys));
+	strlcpy(serio->firmware_id, i8042_kbd_firmware_id,
+		sizeof(serio->firmware_id));
 
 	port->serio = serio;
 	port->irq = I8042_KBD_IRQ;
@@ -1244,6 +1248,8 @@ static int __init i8042_create_aux_port(int idx)
 	if (idx < 0) {
 		strlcpy(serio->name, "i8042 AUX port", sizeof(serio->name));
 		strlcpy(serio->phys, I8042_AUX_PHYS_DESC, sizeof(serio->phys));
+		strlcpy(serio->firmware_id, i8042_aux_firmware_id,
+			sizeof(serio->firmware_id));
 		serio->close = i8042_port_close;
 	} else {
 		snprintf(serio->name, sizeof(serio->name), "i8042 AUX%d port", idx);
diff --git a/drivers/input/serio/serio.c b/drivers/input/serio/serio.c
index 8f4c4ab04bc..b29134de983 100644
--- a/drivers/input/serio/serio.c
+++ b/drivers/input/serio/serio.c
@@ -451,6 +451,13 @@ static ssize_t serio_set_bind_mode(struct device *dev, struct device_attribute *
 	return retval;
 }
 
+static ssize_t firmware_id_show(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct serio *serio = to_serio_port(dev);
+
+	return sprintf(buf, "%s\n", serio->firmware_id);
+}
+
 static DEVICE_ATTR_RO(type);
 static DEVICE_ATTR_RO(proto);
 static DEVICE_ATTR_RO(id);
@@ -473,12 +480,14 @@ static DEVICE_ATTR_RO(modalias);
 static DEVICE_ATTR_WO(drvctl);
 static DEVICE_ATTR(description, S_IRUGO, serio_show_description, NULL);
 static DEVICE_ATTR(bind_mode, S_IWUSR | S_IRUGO, serio_show_bind_mode, serio_set_bind_mode);
+static DEVICE_ATTR_RO(firmware_id);
 
 static struct attribute *serio_device_attrs[] = {
 	&dev_attr_modalias.attr,
 	&dev_attr_description.attr,
 	&dev_attr_drvctl.attr,
 	&dev_attr_bind_mode.attr,
+	&dev_attr_firmware_id.attr,
 	NULL
 };
 
@@ -921,9 +930,14 @@ static int serio_uevent(struct device *dev, struct kobj_uevent_env *env)
 	SERIO_ADD_UEVENT_VAR("SERIO_PROTO=%02x", serio->id.proto);
 	SERIO_ADD_UEVENT_VAR("SERIO_ID=%02x", serio->id.id);
 	SERIO_ADD_UEVENT_VAR("SERIO_EXTRA=%02x", serio->id.extra);
+
 	SERIO_ADD_UEVENT_VAR("MODALIAS=serio:ty%02Xpr%02Xid%02Xex%02X",
 				serio->id.type, serio->id.proto, serio->id.id, serio->id.extra);
 
+	if (serio->firmware_id[0])
+		SERIO_ADD_UEVENT_VAR("SERIO_FIRMWARE_ID=%s",
+				     serio->firmware_id);
+
 	return 0;
 }
 #undef SERIO_ADD_UEVENT_VAR
diff --git a/drivers/input/tablet/wacom_sys.c b/drivers/input/tablet/wacom_sys.c
index b16ebef5b91..611fc3905d0 100644
--- a/drivers/input/tablet/wacom_sys.c
+++ b/drivers/input/tablet/wacom_sys.c
@@ -22,23 +22,18 @@
 #define HID_USAGE_PAGE_DIGITIZER	0x0d
 #define HID_USAGE_PAGE_DESKTOP		0x01
 #define HID_USAGE			0x09
-#define HID_USAGE_X			0x30
-#define HID_USAGE_Y			0x31
-#define HID_USAGE_X_TILT		0x3d
-#define HID_USAGE_Y_TILT		0x3e
-#define HID_USAGE_FINGER		0x22
-#define HID_USAGE_STYLUS		0x20
-#define HID_USAGE_CONTACTMAX		0x55
+#define HID_USAGE_X			((HID_USAGE_PAGE_DESKTOP << 16) | 0x30)
+#define HID_USAGE_Y			((HID_USAGE_PAGE_DESKTOP << 16) | 0x31)
+#define HID_USAGE_PRESSURE		((HID_USAGE_PAGE_DIGITIZER << 16) | 0x30)
+#define HID_USAGE_X_TILT		((HID_USAGE_PAGE_DIGITIZER << 16) | 0x3d)
+#define HID_USAGE_Y_TILT		((HID_USAGE_PAGE_DIGITIZER << 16) | 0x3e)
+#define HID_USAGE_FINGER		((HID_USAGE_PAGE_DIGITIZER << 16) | 0x22)
+#define HID_USAGE_STYLUS		((HID_USAGE_PAGE_DIGITIZER << 16) | 0x20)
+#define HID_USAGE_CONTACTMAX		((HID_USAGE_PAGE_DIGITIZER << 16) | 0x55)
 #define HID_COLLECTION			0xa1
 #define HID_COLLECTION_LOGICAL		0x02
 #define HID_COLLECTION_END		0xc0
 
-enum {
-	WCM_UNDEFINED = 0,
-	WCM_DESKTOP,
-	WCM_DIGITIZER,
-};
-
 struct hid_descriptor {
 	struct usb_descriptor_header header;
 	__le16   bcdHID;
@@ -305,7 +300,7 @@ static int wacom_parse_hid(struct usb_interface *intf,
 	char limit = 0;
 	/* result has to be defined as int for some devices */
 	int result = 0, touch_max = 0;
-	int i = 0, usage = WCM_UNDEFINED, finger = 0, pen = 0;
+	int i = 0, page = 0, finger = 0, pen = 0;
 	unsigned char *report;
 
 	report = kzalloc(hid_desc->wDescriptorLength, GFP_KERNEL);
@@ -332,134 +327,121 @@ static int wacom_parse_hid(struct usb_interface *intf,
 
 		switch (report[i]) {
 		case HID_USAGE_PAGE:
-			switch (report[i + 1]) {
-			case HID_USAGE_PAGE_DIGITIZER:
-				usage = WCM_DIGITIZER;
-				i++;
-				break;
-
-			case HID_USAGE_PAGE_DESKTOP:
-				usage = WCM_DESKTOP;
-				i++;
-				break;
-			}
+			page = report[i + 1];
+			i++;
 			break;
 
 		case HID_USAGE:
-			switch (report[i + 1]) {
+			switch (page << 16 | report[i + 1]) {
 			case HID_USAGE_X:
-				if (usage == WCM_DESKTOP) {
-					if (finger) {
-						features->device_type = BTN_TOOL_FINGER;
-						/* touch device at least supports one touch point */
-						touch_max = 1;
-						switch (features->type) {
-						case TABLETPC2FG:
-							features->pktlen = WACOM_PKGLEN_TPC2FG;
-							break;
-
-						case MTSCREEN:
-						case WACOM_24HDT:
-							features->pktlen = WACOM_PKGLEN_MTOUCH;
-							break;
-
-						case MTTPC:
-							features->pktlen = WACOM_PKGLEN_MTTPC;
-							break;
-
-						case BAMBOO_PT:
-							features->pktlen = WACOM_PKGLEN_BBTOUCH;
-							break;
-
-						default:
-							features->pktlen = WACOM_PKGLEN_GRAPHIRE;
-							break;
-						}
-
-						switch (features->type) {
-						case BAMBOO_PT:
-							features->x_phy =
-								get_unaligned_le16(&report[i + 5]);
-							features->x_max =
-								get_unaligned_le16(&report[i + 8]);
-							i += 15;
-							break;
-
-						case WACOM_24HDT:
-							features->x_max =
-								get_unaligned_le16(&report[i + 3]);
-							features->x_phy =
-								get_unaligned_le16(&report[i + 8]);
-							features->unit = report[i - 1];
-							features->unitExpo = report[i - 3];
-							i += 12;
-							break;
-
-						default:
-							features->x_max =
-								get_unaligned_le16(&report[i + 3]);
-							features->x_phy =
-								get_unaligned_le16(&report[i + 6]);
-							features->unit = report[i + 9];
-							features->unitExpo = report[i + 11];
-							i += 12;
-							break;
-						}
-					} else if (pen) {
-						/* penabled only accepts exact bytes of data */
-						if (features->type >= TABLETPC)
-							features->pktlen = WACOM_PKGLEN_GRAPHIRE;
-						features->device_type = BTN_TOOL_PEN;
+				if (finger) {
+					features->device_type = BTN_TOOL_FINGER;
+					/* touch device at least supports one touch point */
+					touch_max = 1;
+					switch (features->type) {
+					case TABLETPC2FG:
+						features->pktlen = WACOM_PKGLEN_TPC2FG;
+						break;
+
+					case MTSCREEN:
+					case WACOM_24HDT:
+						features->pktlen = WACOM_PKGLEN_MTOUCH;
+						break;
+
+					case MTTPC:
+						features->pktlen = WACOM_PKGLEN_MTTPC;
+						break;
+
+					case BAMBOO_PT:
+						features->pktlen = WACOM_PKGLEN_BBTOUCH;
+						break;
+
+					default:
+						features->pktlen = WACOM_PKGLEN_GRAPHIRE;
+						break;
+					}
+
+					switch (features->type) {
+					case BAMBOO_PT:
+						features->x_phy =
+							get_unaligned_le16(&report[i + 5]);
+						features->x_max =
+							get_unaligned_le16(&report[i + 8]);
+						i += 15;
+						break;
+
+					case WACOM_24HDT:
 						features->x_max =
 							get_unaligned_le16(&report[i + 3]);
-						i += 4;
+						features->x_phy =
+							get_unaligned_le16(&report[i + 8]);
+						features->unit = report[i - 1];
+						features->unitExpo = report[i - 3];
+						i += 12;
+						break;
+
+					default:
+						features->x_max =
+							get_unaligned_le16(&report[i + 3]);
+						features->x_phy =
+							get_unaligned_le16(&report[i + 6]);
+						features->unit = report[i + 9];
+						features->unitExpo = report[i + 11];
+						i += 12;
+						break;
 					}
+				} else if (pen) {
+					/* penabled only accepts exact bytes of data */
+					if (features->type >= TABLETPC)
+						features->pktlen = WACOM_PKGLEN_GRAPHIRE;
+					features->device_type = BTN_TOOL_PEN;
+					features->x_max =
+						get_unaligned_le16(&report[i + 3]);
+					i += 4;
 				}
 				break;
 
 			case HID_USAGE_Y:
-				if (usage == WCM_DESKTOP) {
-					if (finger) {
-						switch (features->type) {
-						case TABLETPC2FG:
-						case MTSCREEN:
-						case MTTPC:
-							features->y_max =
-								get_unaligned_le16(&report[i + 3]);
-							features->y_phy =
-								get_unaligned_le16(&report[i + 6]);
-							i += 7;
-							break;
-
-						case WACOM_24HDT:
-							features->y_max =
-								get_unaligned_le16(&report[i + 3]);
-							features->y_phy =
-								get_unaligned_le16(&report[i - 2]);
-							i += 7;
-							break;
-
-						case BAMBOO_PT:
-							features->y_phy =
-								get_unaligned_le16(&report[i + 3]);
-							features->y_max =
-								get_unaligned_le16(&report[i + 6]);
-							i += 12;
-							break;
-
-						default:
-							features->y_max =
-								features->x_max;
-							features->y_phy =
-								get_unaligned_le16(&report[i + 3]);
-							i += 4;
-							break;
-						}
-					} else if (pen) {
+				if (finger) {
+					switch (features->type) {
+					case TABLETPC2FG:
+					case MTSCREEN:
+					case MTTPC:
+						features->y_max =
+							get_unaligned_le16(&report[i + 3]);
+						features->y_phy =
+							get_unaligned_le16(&report[i + 6]);
+						i += 7;
+						break;
+
+					case WACOM_24HDT:
+						features->y_max =
+							get_unaligned_le16(&report[i + 3]);
+						features->y_phy =
+							get_unaligned_le16(&report[i - 2]);
+						i += 7;
+						break;
+
+					case BAMBOO_PT:
+						features->y_phy =
+							get_unaligned_le16(&report[i + 3]);
+						features->y_max =
+							get_unaligned_le16(&report[i + 6]);
+						i += 12;
+						break;
+
+					default:
 						features->y_max =
+							features->x_max;
+						features->y_phy =
 							get_unaligned_le16(&report[i + 3]);
 						i += 4;
+						break;
 					}
+				} else if (pen) {
+					features->y_max =
+						get_unaligned_le16(&report[i + 3]);
+					i += 4;
 				}
 				break;
 
@@ -484,12 +466,20 @@ static int wacom_parse_hid(struct usb_interface *intf,
 					wacom_retrieve_report_data(intf, features);
 				i++;
 				break;
+
+			case HID_USAGE_PRESSURE:
+				if (pen) {
+					features->pressure_max =
+						get_unaligned_le16(&report[i + 3]);
+					i += 4;
+				}
+				break;
 			}
 			break;
 
 		case HID_COLLECTION_END:
 			/* reset UsagePage and Finger */
-			finger = usage = 0;
+			finger = page = 0;
 			break;
 
 		case HID_COLLECTION:
diff --git a/drivers/input/tablet/wacom_wac.c b/drivers/input/tablet/wacom_wac.c
index 05f371df6c4..4822c57a375 100644
--- a/drivers/input/tablet/wacom_wac.c
+++ b/drivers/input/tablet/wacom_wac.c
@@ -178,10 +178,9 @@ static int wacom_ptu_irq(struct wacom_wac *wacom)
 
 static int wacom_dtu_irq(struct wacom_wac *wacom)
 {
-	struct wacom_features *features = &wacom->features;
-	char *data = wacom->data;
+	unsigned char *data = wacom->data;
 	struct input_dev *input = wacom->input;
-	int prox = data[1] & 0x20, pressure;
+	int prox = data[1] & 0x20;
 
 	dev_dbg(input->dev.parent,
 		"%s: received report #%d", __func__, data[0]);
@@ -198,10 +197,7 @@ static int wacom_dtu_irq(struct wacom_wac *wacom)
 	input_report_key(input, BTN_STYLUS2, data[1] & 0x10);
 	input_report_abs(input, ABS_X, le16_to_cpup((__le16 *)&data[2]));
 	input_report_abs(input, ABS_Y, le16_to_cpup((__le16 *)&data[4]));
-	pressure = ((data[7] & 0x01) << 8) | data[6];
-	if (pressure < 0)
-		pressure = features->pressure_max + pressure + 1;
-	input_report_abs(input, ABS_PRESSURE, pressure);
+	input_report_abs(input, ABS_PRESSURE, ((data[7] & 0x01) << 8) | data[6]);
 	input_report_key(input, BTN_TOUCH, data[1] & 0x05);
 	if (!prox) /* out-prox */
 		wacom->id[0] = 0;
@@ -906,7 +902,7 @@ static int int_dist(int x1, int y1, int x2, int y2)
 static int wacom_24hdt_irq(struct wacom_wac *wacom)
 {
 	struct input_dev *input = wacom->input;
-	char *data = wacom->data;
+	unsigned char *data = wacom->data;
 	int i;
 	int current_num_contacts = data[61];
 	int contacts_to_send = 0;
@@ -959,7 +955,7 @@ static int wacom_24hdt_irq(struct wacom_wac *wacom)
 static int wacom_mt_touch(struct wacom_wac *wacom)
 {
 	struct input_dev *input = wacom->input;
-	char *data = wacom->data;
+	unsigned char *data = wacom->data;
 	int i;
 	int current_num_contacts = data[2];
 	int contacts_to_send = 0;
@@ -1038,7 +1034,7 @@ static int wacom_tpc_mt_touch(struct wacom_wac *wacom)
 
 static int wacom_tpc_single_touch(struct wacom_wac *wacom, size_t len)
 {
-	char *data = wacom->data;
+	unsigned char *data = wacom->data;
 	struct input_dev *input = wacom->input;
 	bool prox;
 	int x = 0, y = 0;
@@ -1074,10 +1070,8 @@ static int wacom_tpc_single_touch(struct wacom_wac *wacom, size_t len)
 
 static int wacom_tpc_pen(struct wacom_wac *wacom)
 {
-	struct wacom_features *features = &wacom->features;
-	char *data = wacom->data;
+	unsigned char *data = wacom->data;
 	struct input_dev *input = wacom->input;
-	int pressure;
 	bool prox = data[1] & 0x20;
 
 	if (!wacom->shared->stylus_in_proximity) /* first in prox */
@@ -1093,10 +1087,7 @@ static int wacom_tpc_pen(struct wacom_wac *wacom)
 		input_report_key(input, BTN_STYLUS2, data[1] & 0x10);
 		input_report_abs(input, ABS_X, le16_to_cpup((__le16 *)&data[2]));
 		input_report_abs(input, ABS_Y, le16_to_cpup((__le16 *)&data[4]));
-		pressure = ((data[7] & 0x01) << 8) | data[6];
-		if (pressure < 0)
-			pressure = features->pressure_max + pressure + 1;
-		input_report_abs(input, ABS_PRESSURE, pressure);
+		input_report_abs(input, ABS_PRESSURE, ((data[7] & 0x03) << 8) | data[6]);
 		input_report_key(input, BTN_TOUCH, data[1] & 0x05);
 		input_report_key(input, wacom->tool[0], prox);
 		return 1;
@@ -1107,7 +1098,7 @@ static int wacom_tpc_pen(struct wacom_wac *wacom)
 
 static int wacom_tpc_irq(struct wacom_wac *wacom, size_t len)
 {
-	char *data = wacom->data;
+	unsigned char *data = wacom->data;
 
 	dev_dbg(wacom->input->dev.parent,
 		"%s: received report #%d\n", __func__, data[0]);
@@ -1838,7 +1829,7 @@ int wacom_setup_input_capabilities(struct input_dev *input_dev,
 	case DTU:
 		if (features->type == DTUS) {
 			input_set_capability(input_dev, EV_MSC, MSC_SERIAL);
-			for (i = 0; i < 3; i++)
+			for (i = 0; i < 4; i++)
 				__set_bit(BTN_0 + i, input_dev->keybit);
 		}
 		__set_bit(BTN_TOOL_PEN, input_dev->keybit);
diff --git a/drivers/input/touchscreen/Kconfig b/drivers/input/touchscreen/Kconfig
index 68edc9db2c6..d4e5ab57909 100644
--- a/drivers/input/touchscreen/Kconfig
+++ b/drivers/input/touchscreen/Kconfig
@@ -550,18 +550,6 @@ config TOUCHSCREEN_TI_AM335X_TSC
 	  To compile this driver as a module, choose M here: the
 	  module will be called ti_am335x_tsc.
 
-config TOUCHSCREEN_ATMEL_TSADCC
-	tristate "Atmel Touchscreen Interface"
-	depends on ARCH_AT91
-	help
-	  Say Y here if you have a 4-wire touchscreen connected to the
-          ADC Controller on your Atmel SoC.
-
-	  If unsure, say N.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called atmel_tsadcc.
-
 config TOUCHSCREEN_UCB1400
 	tristate "Philips UCB1400 touchscreen"
 	depends on AC97_BUS
@@ -640,7 +628,7 @@ config TOUCHSCREEN_WM9713
 
 config TOUCHSCREEN_WM97XX_ATMEL
 	tristate "WM97xx Atmel accelerated touch"
-	depends on TOUCHSCREEN_WM97XX && (AVR32 || ARCH_AT91)
+	depends on TOUCHSCREEN_WM97XX && AVR32
 	help
 	  Say Y here for support for streaming mode with WM97xx touchscreens
 	  on Atmel AT91 or AVR32 systems with an AC97C module.
diff --git a/drivers/input/touchscreen/Makefile b/drivers/input/touchscreen/Makefile
index 4bc954b7c7c..03f12a1f221 100644
--- a/drivers/input/touchscreen/Makefile
+++ b/drivers/input/touchscreen/Makefile
@@ -13,7 +13,6 @@ obj-$(CONFIG_TOUCHSCREEN_AD7879_I2C)	+= ad7879-i2c.o
 obj-$(CONFIG_TOUCHSCREEN_AD7879_SPI)	+= ad7879-spi.o
 obj-$(CONFIG_TOUCHSCREEN_ADS7846)	+= ads7846.o
 obj-$(CONFIG_TOUCHSCREEN_ATMEL_MXT)	+= atmel_mxt_ts.o
-obj-$(CONFIG_TOUCHSCREEN_ATMEL_TSADCC)	+= atmel_tsadcc.o
 obj-$(CONFIG_TOUCHSCREEN_AUO_PIXCIR)	+= auo-pixcir-ts.o
 obj-$(CONFIG_TOUCHSCREEN_BU21013)	+= bu21013_ts.o
 obj-$(CONFIG_TOUCHSCREEN_CY8CTMG110)	+= cy8ctmg110_ts.o
diff --git a/drivers/input/touchscreen/ads7846.c b/drivers/input/touchscreen/ads7846.c
index 45a06e495ed..7f8aa981500 100644
--- a/drivers/input/touchscreen/ads7846.c
+++ b/drivers/input/touchscreen/ads7846.c
@@ -425,7 +425,7 @@ static int ads7845_read12_ser(struct device *dev, unsigned command)
 name ## _show(struct device *dev, struct device_attribute *attr, char *buf) \
 { \
 	struct ads7846 *ts = dev_get_drvdata(dev); \
-	ssize_t v = ads7846_read12_ser(dev, \
+	ssize_t v = ads7846_read12_ser(&ts->spi->dev, \
 			READ_12BIT_SER(var)); \
 	if (v < 0) \
 		return v; \
diff --git a/drivers/input/touchscreen/atmel_tsadcc.c b/drivers/input/touchscreen/atmel_tsadcc.c
deleted file mode 100644
index a7c9d6967d1..00000000000
--- a/drivers/input/touchscreen/atmel_tsadcc.c
+++ /dev/null
@@ -1,358 +0,0 @@
-/*
- *  Atmel Touch Screen Driver
- *
- *  Copyright (c) 2008 ATMEL
- *  Copyright (c) 2008 Dan Liang
- *  Copyright (c) 2008 TimeSys Corporation
- *  Copyright (c) 2008 Justin Waters
- *
- *  Based on touchscreen code from Atmel Corporation.
- *
- *  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/err.h>
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/input.h>
-#include <linux/slab.h>
-#include <linux/interrupt.h>
-#include <linux/clk.h>
-#include <linux/platform_device.h>
-#include <linux/io.h>
-#include <linux/platform_data/atmel.h>
-#include <mach/cpu.h>
-
-/* Register definitions based on AT91SAM9RL64 preliminary draft datasheet */
-
-#define ATMEL_TSADCC_CR		0x00	/* Control register */
-#define   ATMEL_TSADCC_SWRST	(1 << 0)	/* Software Reset*/
-#define	  ATMEL_TSADCC_START	(1 << 1)	/* Start conversion */
-
-#define ATMEL_TSADCC_MR		0x04	/* Mode register */
-#define	  ATMEL_TSADCC_TSAMOD	(3    <<  0)	/* ADC mode */
-#define	    ATMEL_TSADCC_TSAMOD_ADC_ONLY_MODE	(0x0)	/* ADC Mode */
-#define	    ATMEL_TSADCC_TSAMOD_TS_ONLY_MODE	(0x1)	/* Touch Screen Only Mode */
-#define	  ATMEL_TSADCC_LOWRES	(1    <<  4)	/* Resolution selection */
-#define	  ATMEL_TSADCC_SLEEP	(1    <<  5)	/* Sleep mode */
-#define	  ATMEL_TSADCC_PENDET	(1    <<  6)	/* Pen Detect selection */
-#define	  ATMEL_TSADCC_PRES	(1    <<  7)	/* Pressure Measurement Selection */
-#define	  ATMEL_TSADCC_PRESCAL	(0x3f <<  8)	/* Prescalar Rate Selection */
-#define	  ATMEL_TSADCC_EPRESCAL	(0xff <<  8)	/* Prescalar Rate Selection (Extended) */
-#define	  ATMEL_TSADCC_STARTUP	(0x7f << 16)	/* Start Up time */
-#define	  ATMEL_TSADCC_SHTIM	(0xf  << 24)	/* Sample & Hold time */
-#define	  ATMEL_TSADCC_PENDBC	(0xf  << 28)	/* Pen Detect debouncing time */
-
-#define ATMEL_TSADCC_TRGR	0x08	/* Trigger register */
-#define	  ATMEL_TSADCC_TRGMOD	(7      <<  0)	/* Trigger mode */
-#define	    ATMEL_TSADCC_TRGMOD_NONE		(0 << 0)
-#define     ATMEL_TSADCC_TRGMOD_EXT_RISING	(1 << 0)
-#define     ATMEL_TSADCC_TRGMOD_EXT_FALLING	(2 << 0)
-#define     ATMEL_TSADCC_TRGMOD_EXT_ANY		(3 << 0)
-#define     ATMEL_TSADCC_TRGMOD_PENDET		(4 << 0)
-#define     ATMEL_TSADCC_TRGMOD_PERIOD		(5 << 0)
-#define     ATMEL_TSADCC_TRGMOD_CONTINUOUS	(6 << 0)
-#define   ATMEL_TSADCC_TRGPER	(0xffff << 16)	/* Trigger period */
-
-#define ATMEL_TSADCC_TSR	0x0C	/* Touch Screen register */
-#define	  ATMEL_TSADCC_TSFREQ	(0xf <<  0)	/* TS Frequency in Interleaved mode */
-#define	  ATMEL_TSADCC_TSSHTIM	(0xf << 24)	/* Sample & Hold time */
-
-#define ATMEL_TSADCC_CHER	0x10	/* Channel Enable register */
-#define ATMEL_TSADCC_CHDR	0x14	/* Channel Disable register */
-#define ATMEL_TSADCC_CHSR	0x18	/* Channel Status register */
-#define	  ATMEL_TSADCC_CH(n)	(1 << (n))	/* Channel number */
-
-#define ATMEL_TSADCC_SR		0x1C	/* Status register */
-#define	  ATMEL_TSADCC_EOC(n)	(1 << ((n)+0))	/* End of conversion for channel N */
-#define	  ATMEL_TSADCC_OVRE(n)	(1 << ((n)+8))	/* Overrun error for channel N */
-#define	  ATMEL_TSADCC_DRDY	(1 << 16)	/* Data Ready */
-#define	  ATMEL_TSADCC_GOVRE	(1 << 17)	/* General Overrun Error */
-#define	  ATMEL_TSADCC_ENDRX	(1 << 18)	/* End of RX Buffer */
-#define	  ATMEL_TSADCC_RXBUFF	(1 << 19)	/* TX Buffer full */
-#define	  ATMEL_TSADCC_PENCNT	(1 << 20)	/* Pen contact */
-#define	  ATMEL_TSADCC_NOCNT	(1 << 21)	/* No contact */
-
-#define ATMEL_TSADCC_LCDR	0x20	/* Last Converted Data register */
-#define	  ATMEL_TSADCC_DATA	(0x3ff << 0)	/* Channel data */
-
-#define ATMEL_TSADCC_IER	0x24	/* Interrupt Enable register */
-#define ATMEL_TSADCC_IDR	0x28	/* Interrupt Disable register */
-#define ATMEL_TSADCC_IMR	0x2C	/* Interrupt Mask register */
-#define ATMEL_TSADCC_CDR0	0x30	/* Channel Data 0 */
-#define ATMEL_TSADCC_CDR1	0x34	/* Channel Data 1 */
-#define ATMEL_TSADCC_CDR2	0x38	/* Channel Data 2 */
-#define ATMEL_TSADCC_CDR3	0x3C	/* Channel Data 3 */
-#define ATMEL_TSADCC_CDR4	0x40	/* Channel Data 4 */
-#define ATMEL_TSADCC_CDR5	0x44	/* Channel Data 5 */
-
-#define ATMEL_TSADCC_XPOS	0x50
-#define ATMEL_TSADCC_Z1DAT	0x54
-#define ATMEL_TSADCC_Z2DAT	0x58
-
-#define PRESCALER_VAL(x)	((x) >> 8)
-
-#define ADC_DEFAULT_CLOCK	100000
-
-struct atmel_tsadcc {
-	struct input_dev	*input;
-	char			phys[32];
-	struct clk		*clk;
-	int			irq;
-	unsigned int		prev_absx;
-	unsigned int		prev_absy;
-	unsigned char		bufferedmeasure;
-};
-
-static void __iomem		*tsc_base;
-
-#define atmel_tsadcc_read(reg)		__raw_readl(tsc_base + (reg))
-#define atmel_tsadcc_write(reg, val)	__raw_writel((val), tsc_base + (reg))
-
-static irqreturn_t atmel_tsadcc_interrupt(int irq, void *dev)
-{
-	struct atmel_tsadcc	*ts_dev = (struct atmel_tsadcc *)dev;
-	struct input_dev	*input_dev = ts_dev->input;
-
-	unsigned int status;
-	unsigned int reg;
-
-	status = atmel_tsadcc_read(ATMEL_TSADCC_SR);
-	status &= atmel_tsadcc_read(ATMEL_TSADCC_IMR);
-
-	if (status & ATMEL_TSADCC_NOCNT) {
-		/* Contact lost */
-		reg = atmel_tsadcc_read(ATMEL_TSADCC_MR) | ATMEL_TSADCC_PENDBC;
-
-		atmel_tsadcc_write(ATMEL_TSADCC_MR, reg);
-		atmel_tsadcc_write(ATMEL_TSADCC_TRGR, ATMEL_TSADCC_TRGMOD_NONE);
-		atmel_tsadcc_write(ATMEL_TSADCC_IDR,
-				   ATMEL_TSADCC_EOC(3) | ATMEL_TSADCC_NOCNT);
-		atmel_tsadcc_write(ATMEL_TSADCC_IER, ATMEL_TSADCC_PENCNT);
-
-		input_report_key(input_dev, BTN_TOUCH, 0);
-		ts_dev->bufferedmeasure = 0;
-		input_sync(input_dev);
-
-	} else if (status & ATMEL_TSADCC_PENCNT) {
-		/* Pen detected */
-		reg = atmel_tsadcc_read(ATMEL_TSADCC_MR);
-		reg &= ~ATMEL_TSADCC_PENDBC;
-
-		atmel_tsadcc_write(ATMEL_TSADCC_IDR, ATMEL_TSADCC_PENCNT);
-		atmel_tsadcc_write(ATMEL_TSADCC_MR, reg);
-		atmel_tsadcc_write(ATMEL_TSADCC_IER,
-				   ATMEL_TSADCC_EOC(3) | ATMEL_TSADCC_NOCNT);
-		atmel_tsadcc_write(ATMEL_TSADCC_TRGR,
-				   ATMEL_TSADCC_TRGMOD_PERIOD | (0x0FFF << 16));
-
-	} else if (status & ATMEL_TSADCC_EOC(3)) {
-		/* Conversion finished */
-
-		if (ts_dev->bufferedmeasure) {
-			/* Last measurement is always discarded, since it can
-			 * be erroneous.
-			 * Always report previous measurement */
-			input_report_abs(input_dev, ABS_X, ts_dev->prev_absx);
-			input_report_abs(input_dev, ABS_Y, ts_dev->prev_absy);
-			input_report_key(input_dev, BTN_TOUCH, 1);
-			input_sync(input_dev);
-		} else
-			ts_dev->bufferedmeasure = 1;
-
-		/* Now make new measurement */
-		ts_dev->prev_absx = atmel_tsadcc_read(ATMEL_TSADCC_CDR3) << 10;
-		ts_dev->prev_absx /= atmel_tsadcc_read(ATMEL_TSADCC_CDR2);
-
-		ts_dev->prev_absy = atmel_tsadcc_read(ATMEL_TSADCC_CDR1) << 10;
-		ts_dev->prev_absy /= atmel_tsadcc_read(ATMEL_TSADCC_CDR0);
-	}
-
-	return IRQ_HANDLED;
-}
-
-/*
- * The functions for inserting/removing us as a module.
- */
-
-static int atmel_tsadcc_probe(struct platform_device *pdev)
-{
-	struct atmel_tsadcc	*ts_dev;
-	struct input_dev	*input_dev;
-	struct resource		*res;
-	struct at91_tsadcc_data *pdata = dev_get_platdata(&pdev->dev);
-	int		err;
-	unsigned int	prsc;
-	unsigned int	reg;
-
-	if (!pdata)
-		return -EINVAL;
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&pdev->dev, "no mmio resource defined.\n");
-		return -ENXIO;
-	}
-
-	/* Allocate memory for device */
-	ts_dev = kzalloc(sizeof(struct atmel_tsadcc), GFP_KERNEL);
-	if (!ts_dev) {
-		dev_err(&pdev->dev, "failed to allocate memory.\n");
-		return -ENOMEM;
-	}
-	platform_set_drvdata(pdev, ts_dev);
-
-	input_dev = input_allocate_device();
-	if (!input_dev) {
-		dev_err(&pdev->dev, "failed to allocate input device.\n");
-		err = -EBUSY;
-		goto err_free_mem;
-	}
-
-	ts_dev->irq = platform_get_irq(pdev, 0);
-	if (ts_dev->irq < 0) {
-		dev_err(&pdev->dev, "no irq ID is designated.\n");
-		err = -ENODEV;
-		goto err_free_dev;
-	}
-
-	if (!request_mem_region(res->start, resource_size(res),
-				"atmel tsadcc regs")) {
-		dev_err(&pdev->dev, "resources is unavailable.\n");
-		err = -EBUSY;
-		goto err_free_dev;
-	}
-
-	tsc_base = ioremap(res->start, resource_size(res));
-	if (!tsc_base) {
-		dev_err(&pdev->dev, "failed to map registers.\n");
-		err = -ENOMEM;
-		goto err_release_mem;
-	}
-
-	err = request_irq(ts_dev->irq, atmel_tsadcc_interrupt, 0,
-			pdev->dev.driver->name, ts_dev);
-	if (err) {
-		dev_err(&pdev->dev, "failed to allocate irq.\n");
-		goto err_unmap_regs;
-	}
-
-	ts_dev->clk = clk_get(&pdev->dev, "tsc_clk");
-	if (IS_ERR(ts_dev->clk)) {
-		dev_err(&pdev->dev, "failed to get ts_clk\n");
-		err = PTR_ERR(ts_dev->clk);
-		goto err_free_irq;
-	}
-
-	ts_dev->input = input_dev;
-	ts_dev->bufferedmeasure = 0;
-
-	snprintf(ts_dev->phys, sizeof(ts_dev->phys),
-		 "%s/input0", dev_name(&pdev->dev));
-
-	input_dev->name = "atmel touch screen controller";
-	input_dev->phys = ts_dev->phys;
-	input_dev->dev.parent = &pdev->dev;
-
-	__set_bit(EV_ABS, input_dev->evbit);
-	input_set_abs_params(input_dev, ABS_X, 0, 0x3FF, 0, 0);
-	input_set_abs_params(input_dev, ABS_Y, 0, 0x3FF, 0, 0);
-
-	input_set_capability(input_dev, EV_KEY, BTN_TOUCH);
-
-	/* clk_enable() always returns 0, no need to check it */
-	clk_enable(ts_dev->clk);
-
-	prsc = clk_get_rate(ts_dev->clk);
-	dev_info(&pdev->dev, "Master clock is set at: %d Hz\n", prsc);
-
-	if (!pdata->adc_clock)
-		pdata->adc_clock = ADC_DEFAULT_CLOCK;
-
-	prsc = (prsc / (2 * pdata->adc_clock)) - 1;
-
-	/* saturate if this value is too high */
-	if (cpu_is_at91sam9rl()) {
-		if (prsc > PRESCALER_VAL(ATMEL_TSADCC_PRESCAL))
-			prsc = PRESCALER_VAL(ATMEL_TSADCC_PRESCAL);
-	} else {
-		if (prsc > PRESCALER_VAL(ATMEL_TSADCC_EPRESCAL))
-			prsc = PRESCALER_VAL(ATMEL_TSADCC_EPRESCAL);
-	}
-
-	dev_info(&pdev->dev, "Prescaler is set at: %d\n", prsc);
-
-	reg = ATMEL_TSADCC_TSAMOD_TS_ONLY_MODE		|
-		((0x00 << 5) & ATMEL_TSADCC_SLEEP)	|	/* Normal Mode */
-		((0x01 << 6) & ATMEL_TSADCC_PENDET)	|	/* Enable Pen Detect */
-		(prsc << 8)				|
-		((0x26 << 16) & ATMEL_TSADCC_STARTUP)	|
-		((pdata->pendet_debounce << 28) & ATMEL_TSADCC_PENDBC);
-
-	atmel_tsadcc_write(ATMEL_TSADCC_CR, ATMEL_TSADCC_SWRST);
-	atmel_tsadcc_write(ATMEL_TSADCC_MR, reg);
-	atmel_tsadcc_write(ATMEL_TSADCC_TRGR, ATMEL_TSADCC_TRGMOD_NONE);
-	atmel_tsadcc_write(ATMEL_TSADCC_TSR,
-		(pdata->ts_sample_hold_time << 24) & ATMEL_TSADCC_TSSHTIM);
-
-	atmel_tsadcc_read(ATMEL_TSADCC_SR);
-	atmel_tsadcc_write(ATMEL_TSADCC_IER, ATMEL_TSADCC_PENCNT);
-
-	/* All went ok, so register to the input system */
-	err = input_register_device(input_dev);
-	if (err)
-		goto err_fail;
-
-	return 0;
-
-err_fail:
-	clk_disable(ts_dev->clk);
-	clk_put(ts_dev->clk);
-err_free_irq:
-	free_irq(ts_dev->irq, ts_dev);
-err_unmap_regs:
-	iounmap(tsc_base);
-err_release_mem:
-	release_mem_region(res->start, resource_size(res));
-err_free_dev:
-	input_free_device(input_dev);
-err_free_mem:
-	kfree(ts_dev);
-	return err;
-}
-
-static int atmel_tsadcc_remove(struct platform_device *pdev)
-{
-	struct atmel_tsadcc *ts_dev = platform_get_drvdata(pdev);
-	struct resource *res;
-
-	free_irq(ts_dev->irq, ts_dev);
-
-	input_unregister_device(ts_dev->input);
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	iounmap(tsc_base);
-	release_mem_region(res->start, resource_size(res));
-
-	clk_disable(ts_dev->clk);
-	clk_put(ts_dev->clk);
-
-	kfree(ts_dev);
-
-	return 0;
-}
-
-static struct platform_driver atmel_tsadcc_driver = {
-	.probe		= atmel_tsadcc_probe,
-	.remove		= atmel_tsadcc_remove,
-	.driver		= {
-		.name	= "atmel_tsadcc",
-	},
-};
-module_platform_driver(atmel_tsadcc_driver);
-
-MODULE_LICENSE("GPL");
-MODULE_DESCRIPTION("Atmel TouchScreen Driver");
-MODULE_AUTHOR("Dan Liang <dan.liang@atmel.com>");
-
diff --git a/drivers/iommu/amd_iommu.c b/drivers/iommu/amd_iommu.c
index c949520bd19..57068e8035b 100644
--- a/drivers/iommu/amd_iommu.c
+++ b/drivers/iommu/amd_iommu.c
@@ -3999,7 +3999,7 @@ static struct irq_remap_table *get_irq_table(u16 devid, bool ioapic)
 	iommu_flush_dte(iommu, devid);
 	if (devid != alias) {
 		irq_lookup_table[alias] = table;
-		set_dte_irq_entry(devid, table);
+		set_dte_irq_entry(alias, table);
 		iommu_flush_dte(iommu, alias);
 	}
 
diff --git a/drivers/iommu/amd_iommu_init.c b/drivers/iommu/amd_iommu_init.c
index b76c58dbe30..0e08545d729 100644
--- a/drivers/iommu/amd_iommu_init.c
+++ b/drivers/iommu/amd_iommu_init.c
@@ -788,7 +788,7 @@ static void __init set_device_exclusion_range(u16 devid, struct ivmd_header *m)
 		 * per device. But we can enable the exclusion range per
 		 * device. This is done here
 		 */
-		set_dev_entry_bit(m->devid, DEV_ENTRY_EX);
+		set_dev_entry_bit(devid, DEV_ENTRY_EX);
 		iommu->exclusion_start = m->range_start;
 		iommu->exclusion_length = m->range_length;
 	}
diff --git a/drivers/iommu/amd_iommu_v2.c b/drivers/iommu/amd_iommu_v2.c
index 5208828792e..203b2e6a91c 100644
--- a/drivers/iommu/amd_iommu_v2.c
+++ b/drivers/iommu/amd_iommu_v2.c
@@ -504,8 +504,10 @@ static void do_fault(struct work_struct *work)
 
 	write = !!(fault->flags & PPR_FAULT_WRITE);
 
+	down_read(&fault->state->mm->mmap_sem);
 	npages = get_user_pages(fault->state->task, fault->state->mm,
 				fault->address, 1, write, 0, &page, NULL);
+	up_read(&fault->state->mm->mmap_sem);
 
 	if (npages == 1) {
 		put_page(page);
diff --git a/drivers/iommu/arm-smmu.c b/drivers/iommu/arm-smmu.c
index 8b89e33a89f..647c3c7fd74 100644
--- a/drivers/iommu/arm-smmu.c
+++ b/drivers/iommu/arm-smmu.c
@@ -1381,7 +1381,7 @@ static int arm_smmu_alloc_init_pmd(struct arm_smmu_device *smmu, pud_t *pud,
 
 	do {
 		next = pmd_addr_end(addr, end);
-		ret = arm_smmu_alloc_init_pte(smmu, pmd, addr, end, pfn,
+		ret = arm_smmu_alloc_init_pte(smmu, pmd, addr, next, pfn,
 					      prot, stage);
 		phys += next - addr;
 	} while (pmd++, addr = next, addr < end);
@@ -1499,7 +1499,7 @@ static size_t arm_smmu_unmap(struct iommu_domain *domain, unsigned long iova,
 
 	ret = arm_smmu_handle_mapping(smmu_domain, iova, 0, size, 0);
 	arm_smmu_tlb_inv_context(&smmu_domain->root_cfg);
-	return ret ? ret : size;
+	return ret ? 0 : size;
 }
 
 static phys_addr_t arm_smmu_iova_to_phys(struct iommu_domain *domain,
diff --git a/drivers/iommu/dmar.c b/drivers/iommu/dmar.c
index f445c10df8d..39f8b717fe8 100644
--- a/drivers/iommu/dmar.c
+++ b/drivers/iommu/dmar.c
@@ -152,7 +152,8 @@ dmar_alloc_pci_notify_info(struct pci_dev *dev, unsigned long event)
 	info->seg = pci_domain_nr(dev->bus);
 	info->level = level;
 	if (event == BUS_NOTIFY_ADD_DEVICE) {
-		for (tmp = dev, level--; tmp; tmp = tmp->bus->self) {
+		for (tmp = dev; tmp; tmp = tmp->bus->self) {
+			level--;
 			info->path[level].device = PCI_SLOT(tmp->devfn);
 			info->path[level].function = PCI_FUNC(tmp->devfn);
 			if (pci_is_root_bus(tmp->bus))
diff --git a/drivers/iommu/exynos-iommu.c b/drivers/iommu/exynos-iommu.c
index 074018979cd..685f9263cfe 100644
--- a/drivers/iommu/exynos-iommu.c
+++ b/drivers/iommu/exynos-iommu.c
@@ -542,12 +542,7 @@ static int exynos_sysmmu_probe(struct platform_device *pdev)
 		goto err_alloc;
 	}
 
-	ret = dev_set_drvdata(dev, data);
-	if (ret) {
-		dev_dbg(dev, "Unabled to initialize driver data\n");
-		goto err_init;
-	}
-
+	dev_set_drvdata(dev, data);
 	data->nsfrs = pdev->num_resources / 2;
 	data->sfrbases = kmalloc(sizeof(*data->sfrbases) * data->nsfrs,
 								GFP_KERNEL);
@@ -1011,13 +1006,13 @@ static phys_addr_t exynos_iommu_iova_to_phys(struct iommu_domain *domain,
 }
 
 static struct iommu_ops exynos_iommu_ops = {
-	.domain_init = &exynos_iommu_domain_init,
-	.domain_destroy = &exynos_iommu_domain_destroy,
-	.attach_dev = &exynos_iommu_attach_device,
-	.detach_dev = &exynos_iommu_detach_device,
-	.map = &exynos_iommu_map,
-	.unmap = &exynos_iommu_unmap,
-	.iova_to_phys = &exynos_iommu_iova_to_phys,
+	.domain_init = exynos_iommu_domain_init,
+	.domain_destroy = exynos_iommu_domain_destroy,
+	.attach_dev = exynos_iommu_attach_device,
+	.detach_dev = exynos_iommu_detach_device,
+	.map = exynos_iommu_map,
+	.unmap = exynos_iommu_unmap,
+	.iova_to_phys = exynos_iommu_iova_to_phys,
 	.pgsize_bitmap = SECT_SIZE | LPAGE_SIZE | SPAGE_SIZE,
 };
 
diff --git a/drivers/iommu/intel-iommu.c b/drivers/iommu/intel-iommu.c
index 69fa7da5e48..f256ffc02e2 100644
--- a/drivers/iommu/intel-iommu.c
+++ b/drivers/iommu/intel-iommu.c
@@ -1009,11 +1009,13 @@ static struct page *dma_pte_list_pagetables(struct dmar_domain *domain,
 	if (level == 1)
 		return freelist;
 
-	for (pte = page_address(pg); !first_pte_in_page(pte); pte++) {
+	pte = page_address(pg);
+	do {
 		if (dma_pte_present(pte) && !dma_pte_superpage(pte))
 			freelist = dma_pte_list_pagetables(domain, level - 1,
 							   pte, freelist);
-	}
+		pte++;
+	} while (!first_pte_in_page(pte));
 
 	return freelist;
 }
@@ -2235,7 +2237,9 @@ static struct dmar_domain *get_domain_for_dev(struct device *dev, int gaw)
 				bridge_devfn = dev_tmp->devfn;
 			}
 			spin_lock_irqsave(&device_domain_lock, flags);
-			info = dmar_search_domain_by_dev_info(segment, bus, devfn);
+			info = dmar_search_domain_by_dev_info(segment,
+							      bridge_bus,
+							      bridge_devfn);
 			if (info) {
 				iommu = info->iommu;
 				domain = info->domain;
diff --git a/drivers/irqchip/irq-armada-370-xp.c b/drivers/irqchip/irq-armada-370-xp.c
index 41be897df8d..c887e6eebc4 100644
--- a/drivers/irqchip/irq-armada-370-xp.c
+++ b/drivers/irqchip/irq-armada-370-xp.c
@@ -19,6 +19,7 @@
 #include <linux/irq.h>
 #include <linux/interrupt.h>
 #include <linux/irqchip/chained_irq.h>
+#include <linux/cpu.h>
 #include <linux/io.h>
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
@@ -41,6 +42,7 @@
 #define ARMADA_370_XP_INT_SET_ENABLE_OFFS	(0x30)
 #define ARMADA_370_XP_INT_CLEAR_ENABLE_OFFS	(0x34)
 #define ARMADA_370_XP_INT_SOURCE_CTL(irq)	(0x100 + irq*4)
+#define ARMADA_370_XP_INT_SOURCE_CPU_MASK	0xF
 
 #define ARMADA_370_XP_CPU_INTACK_OFFS		(0x44)
 #define ARMADA_375_PPI_CAUSE			(0x10)
@@ -132,8 +134,7 @@ static int armada_370_xp_setup_msi_irq(struct msi_chip *chip,
 				       struct msi_desc *desc)
 {
 	struct msi_msg msg;
-	irq_hw_number_t hwirq;
-	int virq;
+	int virq, hwirq;
 
 	hwirq = armada_370_xp_alloc_msi();
 	if (hwirq < 0)
@@ -159,8 +160,19 @@ static void armada_370_xp_teardown_msi_irq(struct msi_chip *chip,
 					   unsigned int irq)
 {
 	struct irq_data *d = irq_get_irq_data(irq);
+	unsigned long hwirq = d->hwirq;
+
 	irq_dispose_mapping(irq);
-	armada_370_xp_free_msi(d->hwirq);
+	armada_370_xp_free_msi(hwirq);
+}
+
+static int armada_370_xp_check_msi_device(struct msi_chip *chip, struct pci_dev *dev,
+					  int nvec, int type)
+{
+	/* We support MSI, but not MSI-X */
+	if (type == PCI_CAP_ID_MSI)
+		return 0;
+	return -EINVAL;
 }
 
 static struct irq_chip armada_370_xp_msi_irq_chip = {
@@ -201,6 +213,7 @@ static int armada_370_xp_msi_init(struct device_node *node,
 
 	msi_chip->setup_irq = armada_370_xp_setup_msi_irq;
 	msi_chip->teardown_irq = armada_370_xp_teardown_msi_irq;
+	msi_chip->check_device = armada_370_xp_check_msi_device;
 	msi_chip->of_node = node;
 
 	armada_370_xp_msi_domain =
@@ -244,35 +257,18 @@ static DEFINE_RAW_SPINLOCK(irq_controller_lock);
 static int armada_xp_set_affinity(struct irq_data *d,
 				  const struct cpumask *mask_val, bool force)
 {
-	unsigned long reg;
-	unsigned long new_mask = 0;
-	unsigned long online_mask = 0;
-	unsigned long count = 0;
 	irq_hw_number_t hwirq = irqd_to_hwirq(d);
+	unsigned long reg, mask;
 	int cpu;
 
-	for_each_cpu(cpu, mask_val) {
-		new_mask |= 1 << cpu_logical_map(cpu);
-		count++;
-	}
-
-	/*
-	 * Forbid mutlicore interrupt affinity
-	 * This is required since the MPIC HW doesn't limit
-	 * several CPUs from acknowledging the same interrupt.
-	 */
-	if (count > 1)
-		return -EINVAL;
-
-	for_each_cpu(cpu, cpu_online_mask)
-		online_mask |= 1 << cpu_logical_map(cpu);
+	/* Select a single core from the affinity mask which is online */
+	cpu = cpumask_any_and(mask_val, cpu_online_mask);
+	mask = 1UL << cpu_logical_map(cpu);
 
 	raw_spin_lock(&irq_controller_lock);
-
 	reg = readl(main_int_base + ARMADA_370_XP_INT_SOURCE_CTL(hwirq));
-	reg = (reg & (~online_mask)) | new_mask;
+	reg = (reg & (~ARMADA_370_XP_INT_SOURCE_CPU_MASK)) | mask;
 	writel(reg, main_int_base + ARMADA_370_XP_INT_SOURCE_CTL(hwirq));
-
 	raw_spin_unlock(&irq_controller_lock);
 
 	return 0;
@@ -315,7 +311,8 @@ static int armada_370_xp_mpic_irq_map(struct irq_domain *h,
 }
 
 #ifdef CONFIG_SMP
-void armada_mpic_send_doorbell(const struct cpumask *mask, unsigned int irq)
+static void armada_mpic_send_doorbell(const struct cpumask *mask,
+				      unsigned int irq)
 {
 	int cpu;
 	unsigned long map = 0;
@@ -335,7 +332,7 @@ void armada_mpic_send_doorbell(const struct cpumask *mask, unsigned int irq)
 		ARMADA_370_XP_SW_TRIG_INT_OFFS);
 }
 
-void armada_xp_mpic_smp_cpu_init(void)
+static void armada_xp_mpic_smp_cpu_init(void)
 {
 	/* Clear pending IPIs */
 	writel(0, per_cpu_int_base + ARMADA_370_XP_IN_DRBEL_CAUSE_OFFS);
@@ -347,6 +344,20 @@ void armada_xp_mpic_smp_cpu_init(void)
 	/* Unmask IPI interrupt */
 	writel(0, per_cpu_int_base + ARMADA_370_XP_INT_CLEAR_MASK_OFFS);
 }
+
+static int armada_xp_mpic_secondary_init(struct notifier_block *nfb,
+					 unsigned long action, void *hcpu)
+{
+	if (action == CPU_STARTING || action == CPU_STARTING_FROZEN)
+		armada_xp_mpic_smp_cpu_init();
+	return NOTIFY_OK;
+}
+
+static struct notifier_block armada_370_xp_mpic_cpu_notifier = {
+	.notifier_call = armada_xp_mpic_secondary_init,
+	.priority = 100,
+};
+
 #endif /* CONFIG_SMP */
 
 static struct irq_domain_ops armada_370_xp_mpic_irq_ops = {
@@ -494,15 +505,6 @@ static int __init armada_370_xp_mpic_of_init(struct device_node *node,
 
 #ifdef CONFIG_SMP
 	armada_xp_mpic_smp_cpu_init();
-
-	/*
-	 * Set the default affinity from all CPUs to the boot cpu.
-	 * This is required since the MPIC doesn't limit several CPUs
-	 * from acknowledging the same interrupt.
-	 */
-	cpumask_clear(irq_default_affinity);
-	cpumask_set_cpu(smp_processor_id(), irq_default_affinity);
-
 #endif
 
 	armada_370_xp_msi_init(node, main_int_res.start);
@@ -511,6 +513,10 @@ static int __init armada_370_xp_mpic_of_init(struct device_node *node,
 	if (parent_irq <= 0) {
 		irq_set_default_host(armada_370_xp_mpic_domain);
 		set_handle_irq(armada_370_xp_handle_irq);
+#ifdef CONFIG_SMP
+		set_smp_cross_call(armada_mpic_send_doorbell);
+		register_cpu_notifier(&armada_370_xp_mpic_cpu_notifier);
+#endif
 	} else {
 		irq_set_chained_handler(parent_irq,
 					armada_370_xp_mpic_handle_cascade_irq);
diff --git a/drivers/irqchip/irq-crossbar.c b/drivers/irqchip/irq-crossbar.c
index fc817d28d1f..3d15d16a708 100644
--- a/drivers/irqchip/irq-crossbar.c
+++ b/drivers/irqchip/irq-crossbar.c
@@ -107,7 +107,7 @@ static int __init crossbar_of_init(struct device_node *node)
 	int i, size, max, reserved = 0, entry;
 	const __be32 *irqsr;
 
-	cb = kzalloc(sizeof(struct cb_device *), GFP_KERNEL);
+	cb = kzalloc(sizeof(*cb), GFP_KERNEL);
 
 	if (!cb)
 		return -ENOMEM;
diff --git a/drivers/irqchip/irq-gic.c b/drivers/irqchip/irq-gic.c
index 4300b6606f5..57d165e026f 100644
--- a/drivers/irqchip/irq-gic.c
+++ b/drivers/irqchip/irq-gic.c
@@ -246,10 +246,14 @@ static int gic_set_affinity(struct irq_data *d, const struct cpumask *mask_val,
 			    bool force)
 {
 	void __iomem *reg = gic_dist_base(d) + GIC_DIST_TARGET + (gic_irq(d) & ~3);
-	unsigned int shift = (gic_irq(d) % 4) * 8;
-	unsigned int cpu = cpumask_any_and(mask_val, cpu_online_mask);
+	unsigned int cpu, shift = (gic_irq(d) % 4) * 8;
 	u32 val, mask, bit;
 
+	if (!force)
+		cpu = cpumask_any_and(mask_val, cpu_online_mask);
+	else
+		cpu = cpumask_first(mask_val);
+
 	if (cpu >= NR_GIC_CPU_IF || cpu >= nr_cpu_ids)
 		return -EINVAL;
 
diff --git a/drivers/irqchip/irq-orion.c b/drivers/irqchip/irq-orion.c
index e25f246cd2f..34d18b48bb7 100644
--- a/drivers/irqchip/irq-orion.c
+++ b/drivers/irqchip/irq-orion.c
@@ -42,7 +42,7 @@ __exception_irq_entry orion_handle_irq(struct pt_regs *regs)
 		u32 stat = readl_relaxed(gc->reg_base + ORION_IRQ_CAUSE) &
 			gc->mask_cache;
 		while (stat) {
-			u32 hwirq = ffs(stat) - 1;
+			u32 hwirq = __fls(stat);
 			u32 irq = irq_find_mapping(orion_irq_domain,
 						   gc->irq_base + hwirq);
 			handle_IRQ(irq, regs);
@@ -117,7 +117,7 @@ static void orion_bridge_irq_handler(unsigned int irq, struct irq_desc *desc)
 		   gc->mask_cache;
 
 	while (stat) {
-		u32 hwirq = ffs(stat) - 1;
+		u32 hwirq = __fls(stat);
 
 		generic_handle_irq(irq_find_mapping(d, gc->irq_base + hwirq));
 		stat &= ~(1 << hwirq);
diff --git a/drivers/irqchip/irq-sirfsoc.c b/drivers/irqchip/irq-sirfsoc.c
index 581eefe331a..5e54f6d71e7 100644
--- a/drivers/irqchip/irq-sirfsoc.c
+++ b/drivers/irqchip/irq-sirfsoc.c
@@ -58,7 +58,8 @@ static void __exception_irq_entry sirfsoc_handle_irq(struct pt_regs *regs)
 	handle_IRQ(irqnr, regs);
 }
 
-static int __init sirfsoc_irq_init(struct device_node *np, struct device_node *parent)
+static int __init sirfsoc_irq_init(struct device_node *np,
+	struct device_node *parent)
 {
 	void __iomem *base = of_iomap(np, 0);
 	if (!base)
diff --git a/drivers/irqchip/spear-shirq.c b/drivers/irqchip/spear-shirq.c
index 8527743b5ce..3fdda3a4026 100644
--- a/drivers/irqchip/spear-shirq.c
+++ b/drivers/irqchip/spear-shirq.c
@@ -5,7 +5,7 @@
  * Viresh Kumar <viresh.linux@gmail.com>
  *
  * Copyright (C) 2012 ST Microelectronics
- * Shiraz Hashim <shiraz.hashim@st.com>
+ * Shiraz Hashim <shiraz.linux.kernel@gmail.com>
  *
  * This file is licensed under the terms of the GNU General Public
  * License version 2. This program is licensed "as is" without any
diff --git a/drivers/isdn/hisax/icc.c b/drivers/isdn/hisax/icc.c
index 51dae916723..96d1df05044 100644
--- a/drivers/isdn/hisax/icc.c
+++ b/drivers/isdn/hisax/icc.c
@@ -425,7 +425,7 @@ afterXPR:
 				if (cs->debug & L1_DEB_MONITOR)
 					debugl1(cs, "ICC %02x -> MOX1", cs->dc.icc.mon_tx[cs->dc.icc.mon_txp - 1]);
 			}
-		AfterMOX1:
+		AfterMOX1: ;
 #endif
 		}
 	}
diff --git a/drivers/isdn/icn/icn.c b/drivers/isdn/icn/icn.c
index 53d487f0c79..6a7447c304a 100644
--- a/drivers/isdn/icn/icn.c
+++ b/drivers/isdn/icn/icn.c
@@ -1155,7 +1155,7 @@ icn_command(isdn_ctrl *c, icn_card *card)
 	ulong a;
 	ulong flags;
 	int i;
-	char cbuf[60];
+	char cbuf[80];
 	isdn_ctrl cmd;
 	icn_cdef cdef;
 	char __user *arg;
@@ -1309,7 +1309,6 @@ icn_command(isdn_ctrl *c, icn_card *card)
 			break;
 		if ((c->arg & 255) < ICN_BCH) {
 			char *p;
-			char dial[50];
 			char dcode[4];
 
 			a = c->arg;
@@ -1321,10 +1320,10 @@ icn_command(isdn_ctrl *c, icn_card *card)
 			} else
 				/* Normal Dial */
 				strcpy(dcode, "CAL");
-			strcpy(dial, p);
-			sprintf(cbuf, "%02d;D%s_R%s,%02d,%02d,%s\n", (int) (a + 1),
-				dcode, dial, c->parm.setup.si1,
-				c->parm.setup.si2, c->parm.setup.eazmsn);
+			snprintf(cbuf, sizeof(cbuf),
+				 "%02d;D%s_R%s,%02d,%02d,%s\n", (int) (a + 1),
+				 dcode, p, c->parm.setup.si1,
+				 c->parm.setup.si2, c->parm.setup.eazmsn);
 			i = icn_writecmd(cbuf, strlen(cbuf), 0, card);
 		}
 		break;
diff --git a/drivers/leds/Kconfig b/drivers/leds/Kconfig
index 6de9dfbf61c..39e717797cc 100644
--- a/drivers/leds/Kconfig
+++ b/drivers/leds/Kconfig
@@ -487,6 +487,14 @@ config LEDS_BLINKM
 	  This option enables support for the BlinkM RGB LED connected
 	  through I2C. Say Y to enable support for the BlinkM LED.
 
+config LEDS_VERSATILE
+	tristate "LED support for the ARM Versatile and RealView"
+	depends on ARCH_REALVIEW || ARCH_VERSATILE
+	depends on LEDS_CLASS
+	help
+	  This option enabled support for the LEDs on the ARM Versatile
+	  and RealView boards. Say Y to enabled these.
+
 comment "LED Triggers"
 source "drivers/leds/trigger/Kconfig"
 
diff --git a/drivers/leds/Makefile b/drivers/leds/Makefile
index 3cd76dbd9be..8b4c956e11b 100644
--- a/drivers/leds/Makefile
+++ b/drivers/leds/Makefile
@@ -54,6 +54,7 @@ obj-$(CONFIG_LEDS_ASIC3)		+= leds-asic3.o
 obj-$(CONFIG_LEDS_MAX8997)		+= leds-max8997.o
 obj-$(CONFIG_LEDS_LM355x)		+= leds-lm355x.o
 obj-$(CONFIG_LEDS_BLINKM)		+= leds-blinkm.o
+obj-$(CONFIG_LEDS_VERSATILE)		+= leds-versatile.o
 
 # LED SPI Drivers
 obj-$(CONFIG_LEDS_DAC124S085)		+= leds-dac124s085.o
diff --git a/arch/arm/plat-versatile/leds.c b/drivers/leds/leds-versatile.c
index d2490d00b46..80553022d66 100644
--- a/arch/arm/plat-versatile/leds.c
+++ b/drivers/leds/leds-versatile.c
@@ -7,22 +7,14 @@
  */
 #include <linux/kernel.h>
 #include <linux/init.h>
+#include <linux/module.h>
 #include <linux/io.h>
 #include <linux/slab.h>
 #include <linux/leds.h>
-
-#include <mach/hardware.h>
-#include <mach/platform.h>
-
-#ifdef VERSATILE_SYS_BASE
-#define LEDREG	(__io_address(VERSATILE_SYS_BASE) + VERSATILE_SYS_LED_OFFSET)
-#endif
-
-#ifdef REALVIEW_SYS_BASE
-#define LEDREG	(__io_address(REALVIEW_SYS_BASE) + REALVIEW_SYS_LED_OFFSET)
-#endif
+#include <linux/platform_device.h>
 
 struct versatile_led {
+	void __iomem		*base;
 	struct led_classdev	cdev;
 	u8			mask;
 };
@@ -50,30 +42,37 @@ static void versatile_led_set(struct led_classdev *cdev,
 {
 	struct versatile_led *led = container_of(cdev,
 						 struct versatile_led, cdev);
-	u32 reg = readl(LEDREG);
+	u32 reg = readl(led->base);
 
 	if (b != LED_OFF)
 		reg |= led->mask;
 	else
 		reg &= ~led->mask;
-	writel(reg, LEDREG);
+	writel(reg, led->base);
 }
 
 static enum led_brightness versatile_led_get(struct led_classdev *cdev)
 {
 	struct versatile_led *led = container_of(cdev,
 						 struct versatile_led, cdev);
-	u32 reg = readl(LEDREG);
+	u32 reg = readl(led->base);
 
 	return (reg & led->mask) ? LED_FULL : LED_OFF;
 }
 
-static int __init versatile_leds_init(void)
+static int versatile_leds_probe(struct platform_device *dev)
 {
 	int i;
+	struct resource *res;
+	void __iomem *base;
 
-	/* All ON */
-	writel(0xff, LEDREG);
+	res = platform_get_resource(dev, IORESOURCE_MEM, 0);
+	base = devm_ioremap_resource(&dev->dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	/* All off */
+	writel(0, base);
 	for (i = 0; i < ARRAY_SIZE(versatile_leds); i++) {
 		struct versatile_led *led;
 
@@ -81,6 +80,7 @@ static int __init versatile_leds_init(void)
 		if (!led)
 			break;
 
+		led->base = base;
 		led->cdev.name = versatile_leds[i].name;
 		led->cdev.brightness_set = versatile_led_set;
 		led->cdev.brightness_get = versatile_led_get;
@@ -96,8 +96,15 @@ static int __init versatile_leds_init(void)
 	return 0;
 }
 
-/*
- * Since we may have triggers on any subsystem, defer registration
- * until after subsystem_init.
- */
-fs_initcall(versatile_leds_init);
+static struct platform_driver versatile_leds_driver = {
+	.driver = {
+		.name   = "versatile-leds",
+	},
+	.probe = versatile_leds_probe,
+};
+
+module_platform_driver(versatile_leds_driver);
+
+MODULE_AUTHOR("Linus Walleij <linus.walleij@linaro.org>");
+MODULE_DESCRIPTION("ARM Versatile LED driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mcb/mcb-core.c b/drivers/mcb/mcb-core.c
index bbe12932d40..9018ab83517 100644
--- a/drivers/mcb/mcb-core.c
+++ b/drivers/mcb/mcb-core.c
@@ -183,14 +183,14 @@ EXPORT_SYMBOL_GPL(mcb_device_register);
  *
  * Allocate a new @mcb_bus.
  */
-struct mcb_bus *mcb_alloc_bus(void)
+struct mcb_bus *mcb_alloc_bus(struct device *carrier)
 {
 	struct mcb_bus *bus;
 	int bus_nr;
 
 	bus = kzalloc(sizeof(struct mcb_bus), GFP_KERNEL);
 	if (!bus)
-		return NULL;
+		return ERR_PTR(-ENOMEM);
 
 	bus_nr = ida_simple_get(&mcb_ida, 0, 0, GFP_KERNEL);
 	if (bus_nr < 0) {
@@ -200,7 +200,7 @@ struct mcb_bus *mcb_alloc_bus(void)
 
 	INIT_LIST_HEAD(&bus->children);
 	bus->bus_nr = bus_nr;
-
+	bus->carrier = carrier;
 	return bus;
 }
 EXPORT_SYMBOL_GPL(mcb_alloc_bus);
@@ -378,6 +378,13 @@ void mcb_release_mem(struct resource *mem)
 }
 EXPORT_SYMBOL_GPL(mcb_release_mem);
 
+static int __mcb_get_irq(struct mcb_device *dev)
+{
+	struct resource *irq = &dev->irq;
+
+	return irq->start;
+}
+
 /**
  * mcb_get_irq() - Get device's IRQ number
  * @dev: The @mcb_device the IRQ is for
@@ -386,9 +393,12 @@ EXPORT_SYMBOL_GPL(mcb_release_mem);
  */
 int mcb_get_irq(struct mcb_device *dev)
 {
-	struct resource *irq = &dev->irq;
+	struct mcb_bus *bus = dev->bus;
 
-	return irq->start;
+	if (bus->get_irq)
+		return bus->get_irq(dev);
+
+	return __mcb_get_irq(dev);
 }
 EXPORT_SYMBOL_GPL(mcb_get_irq);
 
diff --git a/drivers/mcb/mcb-parse.c b/drivers/mcb/mcb-parse.c
index d1278b5f302..00492695526 100644
--- a/drivers/mcb/mcb-parse.c
+++ b/drivers/mcb/mcb-parse.c
@@ -141,6 +141,7 @@ int chameleon_parse_cells(struct mcb_bus *bus, phys_addr_t mapbase,
 		default:
 			pr_err("Invalid chameleon descriptor type 0x%x\n",
 				dtype);
+			kfree(header);
 			return -EINVAL;
 		}
 		num_cells++;
diff --git a/drivers/mcb/mcb-pci.c b/drivers/mcb/mcb-pci.c
index 99c742cbfb5..b5918196564 100644
--- a/drivers/mcb/mcb-pci.c
+++ b/drivers/mcb/mcb-pci.c
@@ -20,6 +20,15 @@ struct priv {
 	void __iomem *base;
 };
 
+static int mcb_pci_get_irq(struct mcb_device *mdev)
+{
+	struct mcb_bus *mbus = mdev->bus;
+	struct device *dev = mbus->carrier;
+	struct pci_dev *pdev = to_pci_dev(dev);
+
+	return pdev->irq;
+}
+
 static int mcb_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 {
 	struct priv *priv;
@@ -67,7 +76,13 @@ static int mcb_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 
 	pci_set_drvdata(pdev, priv);
 
-	priv->bus = mcb_alloc_bus();
+	priv->bus = mcb_alloc_bus(&pdev->dev);
+	if (IS_ERR(priv->bus)) {
+		ret = PTR_ERR(priv->bus);
+		goto err_drvdata;
+	}
+
+	priv->bus->get_irq = mcb_pci_get_irq;
 
 	ret = chameleon_parse_cells(priv->bus, mapbase, priv->base);
 	if (ret < 0)
diff --git a/drivers/md/dm-cache-target.c b/drivers/md/dm-cache-target.c
index 1bf4a71919e..5f054c44b48 100644
--- a/drivers/md/dm-cache-target.c
+++ b/drivers/md/dm-cache-target.c
@@ -2178,6 +2178,8 @@ static int cache_create(struct cache_args *ca, struct cache **result)
 	ti->num_discard_bios = 1;
 	ti->discards_supported = true;
 	ti->discard_zeroes_data_unsupported = true;
+	/* Discard bios must be split on a block boundary */
+	ti->split_discard_bios = true;
 
 	cache->features = ca->features;
 	ti->per_bio_data_size = get_per_bio_data_size(cache);
@@ -2488,6 +2490,7 @@ static int cache_map(struct dm_target *ti, struct bio *bio)
 
 		} else {
 			inc_hit_counter(cache, bio);
+			pb->all_io_entry = dm_deferred_entry_inc(cache->all_io_ds);
 
 			if (bio_data_dir(bio) == WRITE && writethrough_mode(&cache->features) &&
 			    !is_dirty(cache, lookup_result.cblock))
diff --git a/drivers/md/dm-crypt.c b/drivers/md/dm-crypt.c
index 784695d22fd..53b213226c0 100644
--- a/drivers/md/dm-crypt.c
+++ b/drivers/md/dm-crypt.c
@@ -19,7 +19,6 @@
 #include <linux/crypto.h>
 #include <linux/workqueue.h>
 #include <linux/backing-dev.h>
-#include <linux/percpu.h>
 #include <linux/atomic.h>
 #include <linux/scatterlist.h>
 #include <asm/page.h>
@@ -43,6 +42,7 @@ struct convert_context {
 	struct bvec_iter iter_out;
 	sector_t cc_sector;
 	atomic_t cc_pending;
+	struct ablkcipher_request *req;
 };
 
 /*
@@ -111,15 +111,7 @@ struct iv_tcw_private {
 enum flags { DM_CRYPT_SUSPENDED, DM_CRYPT_KEY_VALID };
 
 /*
- * Duplicated per-CPU state for cipher.
- */
-struct crypt_cpu {
-	struct ablkcipher_request *req;
-};
-
-/*
- * The fields in here must be read only after initialization,
- * changing state should be in crypt_cpu.
+ * The fields in here must be read only after initialization.
  */
 struct crypt_config {
 	struct dm_dev *dev;
@@ -150,12 +142,6 @@ struct crypt_config {
 	sector_t iv_offset;
 	unsigned int iv_size;
 
-	/*
-	 * Duplicated per cpu state. Access through
-	 * per_cpu_ptr() only.
-	 */
-	struct crypt_cpu __percpu *cpu;
-
 	/* ESSIV: struct crypto_cipher *essiv_tfm */
 	void *iv_private;
 	struct crypto_ablkcipher **tfms;
@@ -192,11 +178,6 @@ static void clone_init(struct dm_crypt_io *, struct bio *);
 static void kcryptd_queue_crypt(struct dm_crypt_io *io);
 static u8 *iv_of_dmreq(struct crypt_config *cc, struct dm_crypt_request *dmreq);
 
-static struct crypt_cpu *this_crypt_config(struct crypt_config *cc)
-{
-	return this_cpu_ptr(cc->cpu);
-}
-
 /*
  * Use this to access cipher attributes that are the same for each CPU.
  */
@@ -903,16 +884,15 @@ static void kcryptd_async_done(struct crypto_async_request *async_req,
 static void crypt_alloc_req(struct crypt_config *cc,
 			    struct convert_context *ctx)
 {
-	struct crypt_cpu *this_cc = this_crypt_config(cc);
 	unsigned key_index = ctx->cc_sector & (cc->tfms_count - 1);
 
-	if (!this_cc->req)
-		this_cc->req = mempool_alloc(cc->req_pool, GFP_NOIO);
+	if (!ctx->req)
+		ctx->req = mempool_alloc(cc->req_pool, GFP_NOIO);
 
-	ablkcipher_request_set_tfm(this_cc->req, cc->tfms[key_index]);
-	ablkcipher_request_set_callback(this_cc->req,
+	ablkcipher_request_set_tfm(ctx->req, cc->tfms[key_index]);
+	ablkcipher_request_set_callback(ctx->req,
 	    CRYPTO_TFM_REQ_MAY_BACKLOG | CRYPTO_TFM_REQ_MAY_SLEEP,
-	    kcryptd_async_done, dmreq_of_req(cc, this_cc->req));
+	    kcryptd_async_done, dmreq_of_req(cc, ctx->req));
 }
 
 /*
@@ -921,7 +901,6 @@ static void crypt_alloc_req(struct crypt_config *cc,
 static int crypt_convert(struct crypt_config *cc,
 			 struct convert_context *ctx)
 {
-	struct crypt_cpu *this_cc = this_crypt_config(cc);
 	int r;
 
 	atomic_set(&ctx->cc_pending, 1);
@@ -932,7 +911,7 @@ static int crypt_convert(struct crypt_config *cc,
 
 		atomic_inc(&ctx->cc_pending);
 
-		r = crypt_convert_block(cc, ctx, this_cc->req);
+		r = crypt_convert_block(cc, ctx, ctx->req);
 
 		switch (r) {
 		/* async */
@@ -941,7 +920,7 @@ static int crypt_convert(struct crypt_config *cc,
 			reinit_completion(&ctx->restart);
 			/* fall through*/
 		case -EINPROGRESS:
-			this_cc->req = NULL;
+			ctx->req = NULL;
 			ctx->cc_sector++;
 			continue;
 
@@ -1040,6 +1019,7 @@ static struct dm_crypt_io *crypt_io_alloc(struct crypt_config *cc,
 	io->sector = sector;
 	io->error = 0;
 	io->base_io = NULL;
+	io->ctx.req = NULL;
 	atomic_set(&io->io_pending, 0);
 
 	return io;
@@ -1065,6 +1045,8 @@ static void crypt_dec_pending(struct dm_crypt_io *io)
 	if (!atomic_dec_and_test(&io->io_pending))
 		return;
 
+	if (io->ctx.req)
+		mempool_free(io->ctx.req, cc->req_pool);
 	mempool_free(io, cc->io_pool);
 
 	if (likely(!base_io))
@@ -1492,8 +1474,6 @@ static int crypt_wipe_key(struct crypt_config *cc)
 static void crypt_dtr(struct dm_target *ti)
 {
 	struct crypt_config *cc = ti->private;
-	struct crypt_cpu *cpu_cc;
-	int cpu;
 
 	ti->private = NULL;
 
@@ -1505,13 +1485,6 @@ static void crypt_dtr(struct dm_target *ti)
 	if (cc->crypt_queue)
 		destroy_workqueue(cc->crypt_queue);
 
-	if (cc->cpu)
-		for_each_possible_cpu(cpu) {
-			cpu_cc = per_cpu_ptr(cc->cpu, cpu);
-			if (cpu_cc->req)
-				mempool_free(cpu_cc->req, cc->req_pool);
-		}
-
 	crypt_free_tfms(cc);
 
 	if (cc->bs)
@@ -1530,9 +1503,6 @@ static void crypt_dtr(struct dm_target *ti)
 	if (cc->dev)
 		dm_put_device(ti, cc->dev);
 
-	if (cc->cpu)
-		free_percpu(cc->cpu);
-
 	kzfree(cc->cipher);
 	kzfree(cc->cipher_string);
 
@@ -1588,13 +1558,6 @@ static int crypt_ctr_cipher(struct dm_target *ti,
 	if (tmp)
 		DMWARN("Ignoring unexpected additional cipher options");
 
-	cc->cpu = __alloc_percpu(sizeof(*(cc->cpu)),
-				 __alignof__(struct crypt_cpu));
-	if (!cc->cpu) {
-		ti->error = "Cannot allocate per cpu state";
-		goto bad_mem;
-	}
-
 	/*
 	 * For compatibility with the original dm-crypt mapping format, if
 	 * only the cipher name is supplied, use cbc-plain.
diff --git a/drivers/md/dm-mpath.c b/drivers/md/dm-mpath.c
index aa009e86587..ebfa411d1a7 100644
--- a/drivers/md/dm-mpath.c
+++ b/drivers/md/dm-mpath.c
@@ -445,11 +445,11 @@ static int queue_if_no_path(struct multipath *m, unsigned queue_if_no_path,
 	else
 		m->saved_queue_if_no_path = queue_if_no_path;
 	m->queue_if_no_path = queue_if_no_path;
-	if (!m->queue_if_no_path)
-		dm_table_run_md_queue_async(m->ti->table);
-
 	spin_unlock_irqrestore(&m->lock, flags);
 
+	if (!queue_if_no_path)
+		dm_table_run_md_queue_async(m->ti->table);
+
 	return 0;
 }
 
@@ -954,7 +954,7 @@ out:
  */
 static int reinstate_path(struct pgpath *pgpath)
 {
-	int r = 0;
+	int r = 0, run_queue = 0;
 	unsigned long flags;
 	struct multipath *m = pgpath->pg->m;
 
@@ -978,7 +978,7 @@ static int reinstate_path(struct pgpath *pgpath)
 
 	if (!m->nr_valid_paths++) {
 		m->current_pgpath = NULL;
-		dm_table_run_md_queue_async(m->ti->table);
+		run_queue = 1;
 	} else if (m->hw_handler_name && (m->current_pg == pgpath->pg)) {
 		if (queue_work(kmpath_handlerd, &pgpath->activate_path.work))
 			m->pg_init_in_progress++;
@@ -991,6 +991,8 @@ static int reinstate_path(struct pgpath *pgpath)
 
 out:
 	spin_unlock_irqrestore(&m->lock, flags);
+	if (run_queue)
+		dm_table_run_md_queue_async(m->ti->table);
 
 	return r;
 }
diff --git a/drivers/md/dm-thin.c b/drivers/md/dm-thin.c
index 53728be84de..242ac2ea5f2 100644
--- a/drivers/md/dm-thin.c
+++ b/drivers/md/dm-thin.c
@@ -27,6 +27,9 @@
 #define MAPPING_POOL_SIZE 1024
 #define PRISON_CELLS 1024
 #define COMMIT_PERIOD HZ
+#define NO_SPACE_TIMEOUT_SECS 60
+
+static unsigned no_space_timeout_secs = NO_SPACE_TIMEOUT_SECS;
 
 DECLARE_DM_KCOPYD_THROTTLE_WITH_MODULE_PARM(snapshot_copy_throttle,
 		"A percentage of time allocated for copy on write");
@@ -175,6 +178,7 @@ struct pool {
 	struct workqueue_struct *wq;
 	struct work_struct worker;
 	struct delayed_work waker;
+	struct delayed_work no_space_timeout;
 
 	unsigned long last_commit_jiffies;
 	unsigned ref_count;
@@ -232,6 +236,13 @@ struct thin_c {
 	struct bio_list deferred_bio_list;
 	struct bio_list retry_on_resume_list;
 	struct rb_root sort_bio_list; /* sorted list of deferred bios */
+
+	/*
+	 * Ensures the thin is not destroyed until the worker has finished
+	 * iterating the active_thins list.
+	 */
+	atomic_t refcount;
+	struct completion can_destroy;
 };
 
 /*----------------------------------------------------------------*/
@@ -928,7 +939,7 @@ static int commit(struct pool *pool)
 {
 	int r;
 
-	if (get_pool_mode(pool) != PM_WRITE)
+	if (get_pool_mode(pool) >= PM_READ_ONLY)
 		return -EINVAL;
 
 	r = dm_pool_commit_metadata(pool->pmd);
@@ -1486,6 +1497,45 @@ static void process_thin_deferred_bios(struct thin_c *tc)
 	blk_finish_plug(&plug);
 }
 
+static void thin_get(struct thin_c *tc);
+static void thin_put(struct thin_c *tc);
+
+/*
+ * We can't hold rcu_read_lock() around code that can block.  So we
+ * find a thin with the rcu lock held; bump a refcount; then drop
+ * the lock.
+ */
+static struct thin_c *get_first_thin(struct pool *pool)
+{
+	struct thin_c *tc = NULL;
+
+	rcu_read_lock();
+	if (!list_empty(&pool->active_thins)) {
+		tc = list_entry_rcu(pool->active_thins.next, struct thin_c, list);
+		thin_get(tc);
+	}
+	rcu_read_unlock();
+
+	return tc;
+}
+
+static struct thin_c *get_next_thin(struct pool *pool, struct thin_c *tc)
+{
+	struct thin_c *old_tc = tc;
+
+	rcu_read_lock();
+	list_for_each_entry_continue_rcu(tc, &pool->active_thins, list) {
+		thin_get(tc);
+		thin_put(old_tc);
+		rcu_read_unlock();
+		return tc;
+	}
+	thin_put(old_tc);
+	rcu_read_unlock();
+
+	return NULL;
+}
+
 static void process_deferred_bios(struct pool *pool)
 {
 	unsigned long flags;
@@ -1493,10 +1543,11 @@ static void process_deferred_bios(struct pool *pool)
 	struct bio_list bios;
 	struct thin_c *tc;
 
-	rcu_read_lock();
-	list_for_each_entry_rcu(tc, &pool->active_thins, list)
+	tc = get_first_thin(pool);
+	while (tc) {
 		process_thin_deferred_bios(tc);
-	rcu_read_unlock();
+		tc = get_next_thin(pool, tc);
+	}
 
 	/*
 	 * If there are any deferred flush bios, we must commit
@@ -1543,6 +1594,20 @@ static void do_waker(struct work_struct *ws)
 	queue_delayed_work(pool->wq, &pool->waker, COMMIT_PERIOD);
 }
 
+/*
+ * We're holding onto IO to allow userland time to react.  After the
+ * timeout either the pool will have been resized (and thus back in
+ * PM_WRITE mode), or we degrade to PM_READ_ONLY and start erroring IO.
+ */
+static void do_no_space_timeout(struct work_struct *ws)
+{
+	struct pool *pool = container_of(to_delayed_work(ws), struct pool,
+					 no_space_timeout);
+
+	if (get_pool_mode(pool) == PM_OUT_OF_DATA_SPACE && !pool->pf.error_if_no_space)
+		set_pool_mode(pool, PM_READ_ONLY);
+}
+
 /*----------------------------------------------------------------*/
 
 struct noflush_work {
@@ -1578,7 +1643,7 @@ static void noflush_work(struct thin_c *tc, void (*fn)(struct work_struct *))
 {
 	struct noflush_work w;
 
-	INIT_WORK(&w.worker, fn);
+	INIT_WORK_ONSTACK(&w.worker, fn);
 	w.tc = tc;
 	atomic_set(&w.complete, 0);
 	init_waitqueue_head(&w.wait);
@@ -1607,6 +1672,7 @@ static void set_pool_mode(struct pool *pool, enum pool_mode new_mode)
 	struct pool_c *pt = pool->ti->private;
 	bool needs_check = dm_pool_metadata_needs_check(pool->pmd);
 	enum pool_mode old_mode = get_pool_mode(pool);
+	unsigned long no_space_timeout = ACCESS_ONCE(no_space_timeout_secs) * HZ;
 
 	/*
 	 * Never allow the pool to transition to PM_WRITE mode if user
@@ -1668,6 +1734,9 @@ static void set_pool_mode(struct pool *pool, enum pool_mode new_mode)
 		pool->process_discard = process_discard;
 		pool->process_prepared_mapping = process_prepared_mapping;
 		pool->process_prepared_discard = process_prepared_discard_passdown;
+
+		if (!pool->pf.error_if_no_space && no_space_timeout)
+			queue_delayed_work(pool->wq, &pool->no_space_timeout, no_space_timeout);
 		break;
 
 	case PM_WRITE:
@@ -2053,6 +2122,7 @@ static struct pool *pool_create(struct mapped_device *pool_md,
 
 	INIT_WORK(&pool->worker, do_worker);
 	INIT_DELAYED_WORK(&pool->waker, do_waker);
+	INIT_DELAYED_WORK(&pool->no_space_timeout, do_no_space_timeout);
 	spin_lock_init(&pool->lock);
 	bio_list_init(&pool->deferred_flush_bios);
 	INIT_LIST_HEAD(&pool->prepared_mappings);
@@ -2615,6 +2685,7 @@ static void pool_postsuspend(struct dm_target *ti)
 	struct pool *pool = pt->pool;
 
 	cancel_delayed_work(&pool->waker);
+	cancel_delayed_work(&pool->no_space_timeout);
 	flush_workqueue(pool->wq);
 	(void) commit(pool);
 }
@@ -3061,11 +3132,25 @@ static struct target_type pool_target = {
 /*----------------------------------------------------------------
  * Thin target methods
  *--------------------------------------------------------------*/
+static void thin_get(struct thin_c *tc)
+{
+	atomic_inc(&tc->refcount);
+}
+
+static void thin_put(struct thin_c *tc)
+{
+	if (atomic_dec_and_test(&tc->refcount))
+		complete(&tc->can_destroy);
+}
+
 static void thin_dtr(struct dm_target *ti)
 {
 	struct thin_c *tc = ti->private;
 	unsigned long flags;
 
+	thin_put(tc);
+	wait_for_completion(&tc->can_destroy);
+
 	spin_lock_irqsave(&tc->pool->lock, flags);
 	list_del_rcu(&tc->list);
 	spin_unlock_irqrestore(&tc->pool->lock, flags);
@@ -3101,6 +3186,7 @@ static int thin_ctr(struct dm_target *ti, unsigned argc, char **argv)
 	struct thin_c *tc;
 	struct dm_dev *pool_dev, *origin_dev;
 	struct mapped_device *pool_md;
+	unsigned long flags;
 
 	mutex_lock(&dm_thin_pool_table.mutex);
 
@@ -3191,9 +3277,12 @@ static int thin_ctr(struct dm_target *ti, unsigned argc, char **argv)
 
 	mutex_unlock(&dm_thin_pool_table.mutex);
 
-	spin_lock(&tc->pool->lock);
+	atomic_set(&tc->refcount, 1);
+	init_completion(&tc->can_destroy);
+
+	spin_lock_irqsave(&tc->pool->lock, flags);
 	list_add_tail_rcu(&tc->list, &tc->pool->active_thins);
-	spin_unlock(&tc->pool->lock);
+	spin_unlock_irqrestore(&tc->pool->lock, flags);
 	/*
 	 * This synchronize_rcu() call is needed here otherwise we risk a
 	 * wake_worker() call finding no bios to process (because the newly
@@ -3422,6 +3511,9 @@ static void dm_thin_exit(void)
 module_init(dm_thin_init);
 module_exit(dm_thin_exit);
 
+module_param_named(no_space_timeout, no_space_timeout_secs, uint, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(no_space_timeout, "Out of data space queue IO timeout in seconds");
+
 MODULE_DESCRIPTION(DM_NAME " thin provisioning target");
 MODULE_AUTHOR("Joe Thornber <dm-devel@redhat.com>");
 MODULE_LICENSE("GPL");
diff --git a/drivers/md/dm-verity.c b/drivers/md/dm-verity.c
index 796007a5e0e..7a7bab8947a 100644
--- a/drivers/md/dm-verity.c
+++ b/drivers/md/dm-verity.c
@@ -330,15 +330,17 @@ test_block_hash:
 				return r;
 			}
 		}
-
 		todo = 1 << v->data_dev_block_bits;
-		while (io->iter.bi_size) {
+		do {
 			u8 *page;
+			unsigned len;
 			struct bio_vec bv = bio_iter_iovec(bio, io->iter);
 
 			page = kmap_atomic(bv.bv_page);
-			r = crypto_shash_update(desc, page + bv.bv_offset,
-						bv.bv_len);
+			len = bv.bv_len;
+			if (likely(len >= todo))
+				len = todo;
+			r = crypto_shash_update(desc, page + bv.bv_offset, len);
 			kunmap_atomic(page);
 
 			if (r < 0) {
@@ -346,8 +348,9 @@ test_block_hash:
 				return r;
 			}
 
-			bio_advance_iter(bio, &io->iter, bv.bv_len);
-		}
+			bio_advance_iter(bio, &io->iter, len);
+			todo -= len;
+		} while (todo);
 
 		if (!v->version) {
 			r = crypto_shash_update(desc, v->salt, v->salt_size);
diff --git a/drivers/md/dm.c b/drivers/md/dm.c
index 2db768e4553..aa9e093343d 100644
--- a/drivers/md/dm.c
+++ b/drivers/md/dm.c
@@ -1544,7 +1544,6 @@ static int setup_clone(struct request *clone, struct request *rq,
 	clone->cmd = rq->cmd;
 	clone->cmd_len = rq->cmd_len;
 	clone->sense = rq->sense;
-	clone->buffer = rq->buffer;
 	clone->end_io = end_clone_request;
 	clone->end_io_data = tio;
 
diff --git a/drivers/md/md.c b/drivers/md/md.c
index 8fda38d23e3..237b7e0ddc7 100644
--- a/drivers/md/md.c
+++ b/drivers/md/md.c
@@ -8516,7 +8516,8 @@ static int md_notify_reboot(struct notifier_block *this,
 		if (mddev_trylock(mddev)) {
 			if (mddev->pers)
 				__md_stop_writes(mddev);
-			mddev->safemode = 2;
+			if (mddev->persistent)
+				mddev->safemode = 2;
 			mddev_unlock(mddev);
 		}
 		need_delay = 1;
diff --git a/drivers/md/raid10.c b/drivers/md/raid10.c
index 33fc408e5ea..cb882aae9e2 100644
--- a/drivers/md/raid10.c
+++ b/drivers/md/raid10.c
@@ -1172,6 +1172,13 @@ static void __make_request(struct mddev *mddev, struct bio *bio)
 	int max_sectors;
 	int sectors;
 
+	/*
+	 * Register the new request and wait if the reconstruction
+	 * thread has put up a bar for new requests.
+	 * Continue immediately if no resync is active currently.
+	 */
+	wait_barrier(conf);
+
 	sectors = bio_sectors(bio);
 	while (test_bit(MD_RECOVERY_RESHAPE, &mddev->recovery) &&
 	    bio->bi_iter.bi_sector < conf->reshape_progress &&
@@ -1552,12 +1559,6 @@ static void make_request(struct mddev *mddev, struct bio *bio)
 
 	md_write_start(mddev, bio);
 
-	/*
-	 * Register the new request and wait if the reconstruction
-	 * thread has put up a bar for new requests.
-	 * Continue immediately if no resync is active currently.
-	 */
-	wait_barrier(conf);
 
 	do {
 
diff --git a/drivers/media/i2c/ov7670.c b/drivers/media/i2c/ov7670.c
index e8a1ce20403..cdd7c1b7259 100644
--- a/drivers/media/i2c/ov7670.c
+++ b/drivers/media/i2c/ov7670.c
@@ -1109,7 +1109,7 @@ static int ov7670_enum_framesizes(struct v4l2_subdev *sd,
 	 * windows that fall outside that.
 	 */
 	for (i = 0; i < n_win_sizes; i++) {
-		struct ov7670_win_size *win = &info->devtype->win_sizes[index];
+		struct ov7670_win_size *win = &info->devtype->win_sizes[i];
 		if (info->min_width && win->width < info->min_width)
 			continue;
 		if (info->min_height && win->height < info->min_height)
diff --git a/drivers/media/i2c/s5c73m3/s5c73m3-core.c b/drivers/media/i2c/s5c73m3/s5c73m3-core.c
index a4459301b5f..ee0f57e01b5 100644
--- a/drivers/media/i2c/s5c73m3/s5c73m3-core.c
+++ b/drivers/media/i2c/s5c73m3/s5c73m3-core.c
@@ -1616,7 +1616,7 @@ static int s5c73m3_get_platform_data(struct s5c73m3 *state)
 	if (ret < 0)
 		return -EINVAL;
 
-	node_ep = v4l2_of_get_next_endpoint(node, NULL);
+	node_ep = of_graph_get_next_endpoint(node, NULL);
 	if (!node_ep) {
 		dev_warn(dev, "no endpoint defined for node: %s\n",
 						node->full_name);
diff --git a/drivers/media/media-device.c b/drivers/media/media-device.c
index d5a7a135f75..703560fa5e7 100644
--- a/drivers/media/media-device.c
+++ b/drivers/media/media-device.c
@@ -93,6 +93,7 @@ static long media_device_enum_entities(struct media_device *mdev,
 	struct media_entity *ent;
 	struct media_entity_desc u_ent;
 
+	memset(&u_ent, 0, sizeof(u_ent));
 	if (copy_from_user(&u_ent.id, &uent->id, sizeof(u_ent.id)))
 		return -EFAULT;
 
diff --git a/drivers/media/platform/Kconfig b/drivers/media/platform/Kconfig
index c137abfa0c5..20f1655e6d7 100644
--- a/drivers/media/platform/Kconfig
+++ b/drivers/media/platform/Kconfig
@@ -56,7 +56,7 @@ config VIDEO_VIU
 
 config VIDEO_TIMBERDALE
 	tristate "Support for timberdale Video In/LogiWIN"
-	depends on VIDEO_V4L2 && I2C && DMADEVICES
+	depends on MFD_TIMBERDALE && VIDEO_V4L2 && I2C && DMADEVICES
 	select DMA_ENGINE
 	select TIMB_DMA
 	select VIDEO_ADV7180
diff --git a/drivers/media/platform/davinci/vpbe_display.c b/drivers/media/platform/davinci/vpbe_display.c
index b4f12d00be0..65670825296 100644
--- a/drivers/media/platform/davinci/vpbe_display.c
+++ b/drivers/media/platform/davinci/vpbe_display.c
@@ -372,18 +372,32 @@ static int vpbe_stop_streaming(struct vb2_queue *vq)
 {
 	struct vpbe_fh *fh = vb2_get_drv_priv(vq);
 	struct vpbe_layer *layer = fh->layer;
+	struct vpbe_display *disp = fh->disp_dev;
+	unsigned long flags;
 
 	if (!vb2_is_streaming(vq))
 		return 0;
 
 	/* release all active buffers */
+	spin_lock_irqsave(&disp->dma_queue_lock, flags);
+	if (layer->cur_frm == layer->next_frm) {
+		vb2_buffer_done(&layer->cur_frm->vb, VB2_BUF_STATE_ERROR);
+	} else {
+		if (layer->cur_frm != NULL)
+			vb2_buffer_done(&layer->cur_frm->vb,
+					VB2_BUF_STATE_ERROR);
+		if (layer->next_frm != NULL)
+			vb2_buffer_done(&layer->next_frm->vb,
+					VB2_BUF_STATE_ERROR);
+	}
+
 	while (!list_empty(&layer->dma_queue)) {
 		layer->next_frm = list_entry(layer->dma_queue.next,
 						struct vpbe_disp_buffer, list);
 		list_del(&layer->next_frm->list);
 		vb2_buffer_done(&layer->next_frm->vb, VB2_BUF_STATE_ERROR);
 	}
-
+	spin_unlock_irqrestore(&disp->dma_queue_lock, flags);
 	return 0;
 }
 
diff --git a/drivers/media/platform/davinci/vpfe_capture.c b/drivers/media/platform/davinci/vpfe_capture.c
index d762246eabf..0379cb9f9a9 100644
--- a/drivers/media/platform/davinci/vpfe_capture.c
+++ b/drivers/media/platform/davinci/vpfe_capture.c
@@ -734,6 +734,8 @@ static int vpfe_release(struct file *file)
 		}
 		vpfe_dev->io_usrs = 0;
 		vpfe_dev->numbuffers = config_params.numbuffers;
+		videobuf_stop(&vpfe_dev->buffer_queue);
+		videobuf_mmap_free(&vpfe_dev->buffer_queue);
 	}
 
 	/* Decrement device usrs counter */
diff --git a/drivers/media/platform/davinci/vpif_capture.c b/drivers/media/platform/davinci/vpif_capture.c
index 756da78bac2..8dea0b84a3a 100644
--- a/drivers/media/platform/davinci/vpif_capture.c
+++ b/drivers/media/platform/davinci/vpif_capture.c
@@ -358,8 +358,31 @@ static int vpif_stop_streaming(struct vb2_queue *vq)
 
 	common = &ch->common[VPIF_VIDEO_INDEX];
 
+	/* Disable channel as per its device type and channel id */
+	if (VPIF_CHANNEL0_VIDEO == ch->channel_id) {
+		enable_channel0(0);
+		channel0_intr_enable(0);
+	}
+	if ((VPIF_CHANNEL1_VIDEO == ch->channel_id) ||
+		(2 == common->started)) {
+		enable_channel1(0);
+		channel1_intr_enable(0);
+	}
+	common->started = 0;
+
 	/* release all active buffers */
 	spin_lock_irqsave(&common->irqlock, flags);
+	if (common->cur_frm == common->next_frm) {
+		vb2_buffer_done(&common->cur_frm->vb, VB2_BUF_STATE_ERROR);
+	} else {
+		if (common->cur_frm != NULL)
+			vb2_buffer_done(&common->cur_frm->vb,
+					VB2_BUF_STATE_ERROR);
+		if (common->next_frm != NULL)
+			vb2_buffer_done(&common->next_frm->vb,
+					VB2_BUF_STATE_ERROR);
+	}
+
 	while (!list_empty(&common->dma_queue)) {
 		common->next_frm = list_entry(common->dma_queue.next,
 						struct vpif_cap_buffer, list);
@@ -933,17 +956,6 @@ static int vpif_release(struct file *filep)
 	if (fh->io_allowed[VPIF_VIDEO_INDEX]) {
 		/* Reset io_usrs member of channel object */
 		common->io_usrs = 0;
-		/* Disable channel as per its device type and channel id */
-		if (VPIF_CHANNEL0_VIDEO == ch->channel_id) {
-			enable_channel0(0);
-			channel0_intr_enable(0);
-		}
-		if ((VPIF_CHANNEL1_VIDEO == ch->channel_id) ||
-		    (2 == common->started)) {
-			enable_channel1(0);
-			channel1_intr_enable(0);
-		}
-		common->started = 0;
 		/* Free buffers allocated */
 		vb2_queue_release(&common->buffer_queue);
 		vb2_dma_contig_cleanup_ctx(common->alloc_ctx);
diff --git a/drivers/media/platform/davinci/vpif_display.c b/drivers/media/platform/davinci/vpif_display.c
index 0ac841e35aa..aed41edd050 100644
--- a/drivers/media/platform/davinci/vpif_display.c
+++ b/drivers/media/platform/davinci/vpif_display.c
@@ -320,8 +320,31 @@ static int vpif_stop_streaming(struct vb2_queue *vq)
 
 	common = &ch->common[VPIF_VIDEO_INDEX];
 
+	/* Disable channel */
+	if (VPIF_CHANNEL2_VIDEO == ch->channel_id) {
+		enable_channel2(0);
+		channel2_intr_enable(0);
+	}
+	if ((VPIF_CHANNEL3_VIDEO == ch->channel_id) ||
+		(2 == common->started)) {
+		enable_channel3(0);
+		channel3_intr_enable(0);
+	}
+	common->started = 0;
+
 	/* release all active buffers */
 	spin_lock_irqsave(&common->irqlock, flags);
+	if (common->cur_frm == common->next_frm) {
+		vb2_buffer_done(&common->cur_frm->vb, VB2_BUF_STATE_ERROR);
+	} else {
+		if (common->cur_frm != NULL)
+			vb2_buffer_done(&common->cur_frm->vb,
+					VB2_BUF_STATE_ERROR);
+		if (common->next_frm != NULL)
+			vb2_buffer_done(&common->next_frm->vb,
+					VB2_BUF_STATE_ERROR);
+	}
+
 	while (!list_empty(&common->dma_queue)) {
 		common->next_frm = list_entry(common->dma_queue.next,
 						struct vpif_disp_buffer, list);
@@ -773,18 +796,6 @@ static int vpif_release(struct file *filep)
 	if (fh->io_allowed[VPIF_VIDEO_INDEX]) {
 		/* Reset io_usrs member of channel object */
 		common->io_usrs = 0;
-		/* Disable channel */
-		if (VPIF_CHANNEL2_VIDEO == ch->channel_id) {
-			enable_channel2(0);
-			channel2_intr_enable(0);
-		}
-		if ((VPIF_CHANNEL3_VIDEO == ch->channel_id) ||
-		    (2 == common->started)) {
-			enable_channel3(0);
-			channel3_intr_enable(0);
-		}
-		common->started = 0;
-
 		/* Free buffers allocated */
 		vb2_queue_release(&common->buffer_queue);
 		vb2_dma_contig_cleanup_ctx(common->alloc_ctx);
diff --git a/drivers/media/platform/exynos4-is/fimc-core.c b/drivers/media/platform/exynos4-is/fimc-core.c
index da2fc86cc52..25dbf5b05a9 100644
--- a/drivers/media/platform/exynos4-is/fimc-core.c
+++ b/drivers/media/platform/exynos4-is/fimc-core.c
@@ -122,7 +122,7 @@ static struct fimc_fmt fimc_formats[] = {
 	}, {
 		.name		= "YUV 4:2:2 planar, Y/Cb/Cr",
 		.fourcc		= V4L2_PIX_FMT_YUV422P,
-		.depth		= { 12 },
+		.depth		= { 16 },
 		.color		= FIMC_FMT_YCBYCR422,
 		.memplanes	= 1,
 		.colplanes	= 3,
diff --git a/drivers/media/rc/redrat3.c b/drivers/media/rc/redrat3.c
index 47cd373e229..79abbc8d960 100644
--- a/drivers/media/rc/redrat3.c
+++ b/drivers/media/rc/redrat3.c
@@ -60,28 +60,6 @@
 #define DRIVER_DESC "RedRat3 USB IR Transceiver Driver"
 #define DRIVER_NAME "redrat3"
 
-/* module parameters */
-#ifdef CONFIG_USB_DEBUG
-static int debug = 1;
-#else
-static int debug;
-#endif
-
-#define RR3_DEBUG_STANDARD		0x1
-#define RR3_DEBUG_FUNCTION_TRACE	0x2
-
-#define rr3_dbg(dev, fmt, ...)					\
-	do {							\
-		if (debug & RR3_DEBUG_STANDARD)			\
-			dev_info(dev, fmt, ## __VA_ARGS__);	\
-	} while (0)
-
-#define rr3_ftr(dev, fmt, ...)					\
-	do {							\
-		if (debug & RR3_DEBUG_FUNCTION_TRACE)		\
-			dev_info(dev, fmt, ## __VA_ARGS__);	\
-	} while (0)
-
 /* bulk data transfer types */
 #define RR3_ERROR		0x01
 #define RR3_MOD_SIGNAL_IN	0x20
@@ -237,13 +215,11 @@ static void redrat3_issue_async(struct redrat3_dev *rr3)
 {
 	int res;
 
-	rr3_ftr(rr3->dev, "Entering %s\n", __func__);
-
 	res = usb_submit_urb(rr3->read_urb, GFP_ATOMIC);
 	if (res)
-		rr3_dbg(rr3->dev, "%s: receive request FAILED! "
-			"(res %d, len %d)\n", __func__, res,
-			rr3->read_urb->transfer_buffer_length);
+		dev_dbg(rr3->dev,
+			"%s: receive request FAILED! (res %d, len %d)\n",
+			__func__, res, rr3->read_urb->transfer_buffer_length);
 }
 
 static void redrat3_dump_fw_error(struct redrat3_dev *rr3, int code)
@@ -359,7 +335,7 @@ static void redrat3_rx_timeout(unsigned long data)
 {
 	struct redrat3_dev *rr3 = (struct redrat3_dev *)data;
 
-	rr3_dbg(rr3->dev, "calling ir_raw_event_reset\n");
+	dev_dbg(rr3->dev, "calling ir_raw_event_reset\n");
 	ir_raw_event_reset(rr3->rc);
 }
 
@@ -377,8 +353,6 @@ static void redrat3_process_ir_data(struct redrat3_dev *rr3)
 		return;
 	}
 
-	rr3_ftr(rr3->dev, "Entered %s\n", __func__);
-
 	dev = rr3->dev;
 
 	/* Make sure we reset the IR kfifo after a bit of inactivity */
@@ -386,7 +360,7 @@ static void redrat3_process_ir_data(struct redrat3_dev *rr3)
 	mod_timer(&rr3->rx_timeout, jiffies + delay);
 
 	mod_freq = redrat3_val_to_mod_freq(&rr3->irdata);
-	rr3_dbg(dev, "Got mod_freq of %u\n", mod_freq);
+	dev_dbg(dev, "Got mod_freq of %u\n", mod_freq);
 
 	/* process each rr3 encoded byte into an int */
 	sig_size = be16_to_cpu(rr3->irdata.sig_size);
@@ -408,7 +382,7 @@ static void redrat3_process_ir_data(struct redrat3_dev *rr3)
 		/* cap the value to IR_MAX_DURATION */
 		rawir.duration &= IR_MAX_DURATION;
 
-		rr3_dbg(dev, "storing %s with duration %d (i: %d)\n",
+		dev_dbg(dev, "storing %s with duration %d (i: %d)\n",
 			rawir.pulse ? "pulse" : "space", rawir.duration, i);
 		ir_raw_event_store_with_filter(rr3->rc, &rawir);
 	}
@@ -421,12 +395,12 @@ static void redrat3_process_ir_data(struct redrat3_dev *rr3)
 			rawir.duration = US_TO_NS(2800);
 		else
 			rawir.duration = trailer;
-		rr3_dbg(dev, "storing trailing space with duration %d\n",
+		dev_dbg(dev, "storing trailing space with duration %d\n",
 			rawir.duration);
 		ir_raw_event_store_with_filter(rr3->rc, &rawir);
 	}
 
-	rr3_dbg(dev, "calling ir_raw_event_handle\n");
+	dev_dbg(dev, "calling ir_raw_event_handle\n");
 	ir_raw_event_handle(rr3->rc);
 }
 
@@ -464,8 +438,6 @@ static int redrat3_enable_detector(struct redrat3_dev *rr3)
 	struct device *dev = rr3->dev;
 	u8 ret;
 
-	rr3_ftr(dev, "Entering %s\n", __func__);
-
 	ret = redrat3_send_cmd(RR3_RC_DET_ENABLE, rr3);
 	if (ret != 0)
 		dev_dbg(dev, "%s: unexpected ret of %d\n",
@@ -486,7 +458,6 @@ static int redrat3_enable_detector(struct redrat3_dev *rr3)
 static inline void redrat3_delete(struct redrat3_dev *rr3,
 				  struct usb_device *udev)
 {
-	rr3_ftr(rr3->dev, "%s cleaning up\n", __func__);
 	usb_kill_urb(rr3->read_urb);
 	usb_kill_urb(rr3->flash_urb);
 	usb_free_urb(rr3->read_urb);
@@ -519,7 +490,7 @@ static u32 redrat3_get_timeout(struct redrat3_dev *rr3)
 	else {
 		timeout = redrat3_len_to_us(be32_to_cpup(tmp));
 
-		rr3_dbg(rr3->dev, "Got timeout of %d ms\n", timeout / 1000);
+		dev_dbg(rr3->dev, "Got timeout of %d ms\n", timeout / 1000);
 	}
 
 	kfree(tmp);
@@ -535,8 +506,6 @@ static void redrat3_reset(struct redrat3_dev *rr3)
 	u8 *val;
 	int len = sizeof(u8);
 
-	rr3_ftr(dev, "Entering %s\n", __func__);
-
 	rxpipe = usb_rcvctrlpipe(udev, 0);
 	txpipe = usb_sndctrlpipe(udev, 0);
 
@@ -550,19 +519,19 @@ static void redrat3_reset(struct redrat3_dev *rr3)
 	rc = usb_control_msg(udev, rxpipe, RR3_RESET,
 			     USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
 			     RR3_CPUCS_REG_ADDR, 0, val, len, HZ * 25);
-	rr3_dbg(dev, "reset returned 0x%02x\n", rc);
+	dev_dbg(dev, "reset returned 0x%02x\n", rc);
 
 	*val = 5;
 	rc = usb_control_msg(udev, txpipe, RR3_SET_IR_PARAM,
 			     USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_OUT,
 			     RR3_IR_IO_LENGTH_FUZZ, 0, val, len, HZ * 25);
-	rr3_dbg(dev, "set ir parm len fuzz %d rc 0x%02x\n", *val, rc);
+	dev_dbg(dev, "set ir parm len fuzz %d rc 0x%02x\n", *val, rc);
 
 	*val = RR3_DRIVER_MAXLENS;
 	rc = usb_control_msg(udev, txpipe, RR3_SET_IR_PARAM,
 			     USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_OUT,
 			     RR3_IR_IO_MAX_LENGTHS, 0, val, len, HZ * 25);
-	rr3_dbg(dev, "set ir parm max lens %d rc 0x%02x\n", *val, rc);
+	dev_dbg(dev, "set ir parm max lens %d rc 0x%02x\n", *val, rc);
 
 	kfree(val);
 }
@@ -572,8 +541,6 @@ static void redrat3_get_firmware_rev(struct redrat3_dev *rr3)
 	int rc = 0;
 	char *buffer;
 
-	rr3_ftr(rr3->dev, "Entering %s\n", __func__);
-
 	buffer = kzalloc(sizeof(char) * (RR3_FW_VERSION_LEN + 1), GFP_KERNEL);
 	if (!buffer) {
 		dev_err(rr3->dev, "Memory allocation failure\n");
@@ -591,7 +558,6 @@ static void redrat3_get_firmware_rev(struct redrat3_dev *rr3)
 		dev_err(rr3->dev, "Problem fetching firmware ID\n");
 
 	kfree(buffer);
-	rr3_ftr(rr3->dev, "Exiting %s\n", __func__);
 }
 
 static void redrat3_read_packet_start(struct redrat3_dev *rr3, unsigned len)
@@ -599,8 +565,6 @@ static void redrat3_read_packet_start(struct redrat3_dev *rr3, unsigned len)
 	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 */
 	pktlen = be16_to_cpu(header->length);
 	pkttype = be16_to_cpu(header->transfer_type);
@@ -622,12 +586,12 @@ static void redrat3_read_packet_start(struct redrat3_dev *rr3, unsigned len)
 	case RR3_MOD_SIGNAL_IN:
 		memcpy(&rr3->irdata, rr3->bulk_in_buf, len);
 		rr3->bytes_read = len;
-		rr3_dbg(rr3->dev, "bytes_read %d, pktlen %d\n",
+		dev_dbg(rr3->dev, "bytes_read %d, pktlen %d\n",
 			rr3->bytes_read, pktlen);
 		break;
 
 	default:
-		rr3_dbg(rr3->dev, "ignoring packet with type 0x%02x, len of %d, 0x%02x\n",
+		dev_dbg(rr3->dev, "ignoring packet with type 0x%02x, len of %d, 0x%02x\n",
 						pkttype, len, pktlen);
 		break;
 	}
@@ -637,8 +601,6 @@ static void redrat3_read_packet_continue(struct redrat3_dev *rr3, unsigned len)
 {
 	void *irdata = &rr3->irdata;
 
-	rr3_ftr(rr3->dev, "Entering %s\n", __func__);
-
 	if (len + rr3->bytes_read > sizeof(rr3->irdata)) {
 		dev_warn(rr3->dev, "too much data for packet\n");
 		rr3->bytes_read = 0;
@@ -648,7 +610,7 @@ static void redrat3_read_packet_continue(struct redrat3_dev *rr3, unsigned len)
 	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,
+	dev_dbg(rr3->dev, "bytes_read %d, pktlen %d\n", rr3->bytes_read,
 				 be16_to_cpu(rr3->irdata.header.length));
 }
 
@@ -659,8 +621,6 @@ static int redrat3_get_ir_data(struct redrat3_dev *rr3, unsigned len)
 	unsigned pkttype;
 	int ret = 0;
 
-	rr3_ftr(dev, "Entering %s\n", __func__);
-
 	if (rr3->bytes_read == 0 && len >= sizeof(struct redrat3_header)) {
 		redrat3_read_packet_start(rr3, len);
 	} else if (rr3->bytes_read != 0) {
@@ -681,7 +641,7 @@ static int redrat3_get_ir_data(struct redrat3_dev *rr3, unsigned len)
 	if (pkttype == RR3_MOD_SIGNAL_IN)
 		redrat3_process_ir_data(rr3);
 	else
-		rr3_dbg(dev, "discarding non-signal data packet (type 0x%02x)\n",
+		dev_dbg(dev, "discarding non-signal data packet (type 0x%02x)\n",
 								pkttype);
 
 out:
@@ -705,8 +665,6 @@ static void redrat3_handle_async(struct urb *urb)
 		return;
 	}
 
-	rr3_ftr(rr3->dev, "Entering %s\n", __func__);
-
 	switch (urb->status) {
 	case 0:
 		ret = redrat3_get_ir_data(rr3, urb->actual_length);
@@ -743,7 +701,7 @@ static int redrat3_set_tx_carrier(struct rc_dev *rcdev, u32 carrier)
 	struct redrat3_dev *rr3 = rcdev->priv;
 	struct device *dev = rr3->dev;
 
-	rr3_dbg(dev, "Setting modulation frequency to %u", carrier);
+	dev_dbg(dev, "Setting modulation frequency to %u", carrier);
 	if (carrier == 0)
 		return -EINVAL;
 
@@ -764,8 +722,6 @@ static int redrat3_transmit_ir(struct rc_dev *rcdev, unsigned *txbuf,
 	u8 curlencheck = 0;
 	unsigned i, sendbuf_len;
 
-	rr3_ftr(dev, "Entering %s\n", __func__);
-
 	if (rr3->transmitting) {
 		dev_warn(dev, "%s: transmitter already in use\n", __func__);
 		return -EAGAIN;
@@ -801,7 +757,7 @@ static int redrat3_transmit_ir(struct rc_dev *rcdev, unsigned *txbuf,
 				break;
 		}
 		if (lencheck == curlencheck) {
-			rr3_dbg(dev, "txbuf[%d]=%u, pos %d, enc %u\n",
+			dev_dbg(dev, "txbuf[%d]=%u, pos %d, enc %u\n",
 				i, txbuf[i], curlencheck, cur_sample_len);
 			if (curlencheck < RR3_DRIVER_MAXLENS) {
 				/* now convert the value to a proper
@@ -835,7 +791,7 @@ static int redrat3_transmit_ir(struct rc_dev *rcdev, unsigned *txbuf,
 	pipe = usb_sndbulkpipe(rr3->udev, rr3->ep_out->bEndpointAddress);
 	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, ret);
+	dev_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);
@@ -957,8 +913,6 @@ static int redrat3_dev_probe(struct usb_interface *intf,
 	int pipe, i;
 	int retval = -ENOMEM;
 
-	rr3_ftr(dev, "%s called\n", __func__);
-
 	uhi = intf->cur_altsetting;
 
 	/* find our bulk-in and bulk-out endpoints */
@@ -971,7 +925,7 @@ static int redrat3_dev_probe(struct usb_interface *intf,
 		    ((addr & USB_ENDPOINT_DIR_MASK) == USB_DIR_IN) &&
 		    ((attrs & USB_ENDPOINT_XFERTYPE_MASK) ==
 		     USB_ENDPOINT_XFER_BULK)) {
-			rr3_dbg(dev, "found bulk-in endpoint at 0x%02x\n",
+			dev_dbg(dev, "found bulk-in endpoint at 0x%02x\n",
 				ep->bEndpointAddress);
 			/* data comes in on 0x82, 0x81 is for other data... */
 			if (ep->bEndpointAddress == RR3_BULK_IN_EP_ADDR)
@@ -982,7 +936,7 @@ static int redrat3_dev_probe(struct usb_interface *intf,
 		    ((addr & USB_ENDPOINT_DIR_MASK) == USB_DIR_OUT) &&
 		    ((attrs & USB_ENDPOINT_XFERTYPE_MASK) ==
 		     USB_ENDPOINT_XFER_BULK)) {
-			rr3_dbg(dev, "found bulk-out endpoint at 0x%02x\n",
+			dev_dbg(dev, "found bulk-out endpoint at 0x%02x\n",
 				ep->bEndpointAddress);
 			ep_out = ep;
 		}
@@ -1074,7 +1028,6 @@ static int redrat3_dev_probe(struct usb_interface *intf,
 	/* we can register the device now, as it is ready */
 	usb_set_intfdata(intf, rr3);
 
-	rr3_ftr(dev, "Exiting %s\n", __func__);
 	return 0;
 
 led_free_error:
@@ -1093,8 +1046,6 @@ static void redrat3_dev_disconnect(struct usb_interface *intf)
 	struct usb_device *udev = interface_to_usbdev(intf);
 	struct redrat3_dev *rr3 = usb_get_intfdata(intf);
 
-	rr3_ftr(&intf->dev, "Entering %s\n", __func__);
-
 	if (!rr3)
 		return;
 
@@ -1103,14 +1054,12 @@ static void redrat3_dev_disconnect(struct usb_interface *intf)
 	led_classdev_unregister(&rr3->led);
 	del_timer_sync(&rr3->rx_timeout);
 	redrat3_delete(rr3, udev);
-
-	rr3_ftr(&intf->dev, "RedRat3 IR Transceiver now disconnected\n");
 }
 
 static int redrat3_dev_suspend(struct usb_interface *intf, pm_message_t message)
 {
 	struct redrat3_dev *rr3 = usb_get_intfdata(intf);
-	rr3_ftr(rr3->dev, "suspend\n");
+
 	led_classdev_suspend(&rr3->led);
 	usb_kill_urb(rr3->read_urb);
 	usb_kill_urb(rr3->flash_urb);
@@ -1120,7 +1069,7 @@ static int redrat3_dev_suspend(struct usb_interface *intf, pm_message_t message)
 static int redrat3_dev_resume(struct usb_interface *intf)
 {
 	struct redrat3_dev *rr3 = usb_get_intfdata(intf);
-	rr3_ftr(rr3->dev, "resume\n");
+
 	if (usb_submit_urb(rr3->read_urb, GFP_ATOMIC))
 		return -EIO;
 	led_classdev_resume(&rr3->led);
@@ -1144,8 +1093,3 @@ MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_AUTHOR(DRIVER_AUTHOR2);
 MODULE_LICENSE("GPL");
 MODULE_DEVICE_TABLE(usb, redrat3_dev_table);
-
-module_param(debug, int, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(debug, "Enable module debug spew. 0 = no debugging (default) "
-		 "0x1 = standard debug messages, 0x2 = function tracing debug. "
-		 "Flag bits are addative (i.e., 0x3 for both debug types).");
diff --git a/drivers/media/rc/streamzap.c b/drivers/media/rc/streamzap.c
index f4e0bc3d382..bd5e4ff9e0b 100644
--- a/drivers/media/rc/streamzap.c
+++ b/drivers/media/rc/streamzap.c
@@ -42,12 +42,6 @@
 #define DRIVER_NAME	"streamzap"
 #define DRIVER_DESC	"Streamzap Remote Control driver"
 
-#ifdef CONFIG_USB_DEBUG
-static bool debug = 1;
-#else
-static bool debug;
-#endif
-
 #define USB_STREAMZAP_VENDOR_ID		0x0e9c
 #define USB_STREAMZAP_PRODUCT_ID	0x0000
 
@@ -528,6 +522,3 @@ module_usb_driver(streamzap_driver);
 MODULE_AUTHOR("Jarod Wilson <jarod@wilsonet.com>");
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_LICENSE("GPL");
-
-module_param(debug, bool, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(debug, "Enable debugging messages");
diff --git a/drivers/media/tuners/fc2580.c b/drivers/media/tuners/fc2580.c
index 3aecaf46509..f0c9c42867d 100644
--- a/drivers/media/tuners/fc2580.c
+++ b/drivers/media/tuners/fc2580.c
@@ -195,7 +195,7 @@ static int fc2580_set_params(struct dvb_frontend *fe)
 
 	f_ref = 2UL * priv->cfg->clock / r_val;
 	n_val = div_u64_rem(f_vco, f_ref, &k_val);
-	k_val_reg = 1UL * k_val * (1 << 20) / f_ref;
+	k_val_reg = div_u64(1ULL * k_val * (1 << 20), f_ref);
 
 	ret = fc2580_wr_reg(priv, 0x18, r18_val | ((k_val_reg >> 16) & 0xff));
 	if (ret < 0)
@@ -348,8 +348,8 @@ static int fc2580_set_params(struct dvb_frontend *fe)
 	if (ret < 0)
 		goto err;
 
-	ret = fc2580_wr_reg(priv, 0x37, 1UL * priv->cfg->clock * \
-			fc2580_if_filter_lut[i].mul / 1000000000);
+	ret = fc2580_wr_reg(priv, 0x37, div_u64(1ULL * priv->cfg->clock *
+			fc2580_if_filter_lut[i].mul, 1000000000));
 	if (ret < 0)
 		goto err;
 
diff --git a/drivers/media/tuners/fc2580_priv.h b/drivers/media/tuners/fc2580_priv.h
index be38a9e637e..646c9945213 100644
--- a/drivers/media/tuners/fc2580_priv.h
+++ b/drivers/media/tuners/fc2580_priv.h
@@ -22,6 +22,7 @@
 #define FC2580_PRIV_H
 
 #include "fc2580.h"
+#include <linux/math64.h>
 
 struct fc2580_reg_val {
 	u8 reg;
diff --git a/drivers/media/usb/dvb-usb-v2/Makefile b/drivers/media/usb/dvb-usb-v2/Makefile
index 7407b8338cc..bc38f03394c 100644
--- a/drivers/media/usb/dvb-usb-v2/Makefile
+++ b/drivers/media/usb/dvb-usb-v2/Makefile
@@ -41,4 +41,3 @@ 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
-ccflags-y += -I$(srctree)/drivers/staging/media/rtl2832u_sdr
diff --git a/drivers/media/usb/dvb-usb-v2/rtl28xxu.c b/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
index 61d196e8b3a..dcbd392e6ef 100644
--- a/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
+++ b/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
@@ -24,7 +24,6 @@
 
 #include "rtl2830.h"
 #include "rtl2832.h"
-#include "rtl2832_sdr.h"
 
 #include "qt1010.h"
 #include "mt2060.h"
@@ -36,6 +35,45 @@
 #include "tua9001.h"
 #include "r820t.h"
 
+/*
+ * RTL2832_SDR module is in staging. That logic is added in order to avoid any
+ * hard dependency to drivers/staging/ directory as we want compile mainline
+ * driver even whole staging directory is missing.
+ */
+#include <media/v4l2-subdev.h>
+
+#if IS_ENABLED(CONFIG_DVB_RTL2832_SDR)
+struct dvb_frontend *rtl2832_sdr_attach(struct dvb_frontend *fe,
+	struct i2c_adapter *i2c, const struct rtl2832_config *cfg,
+	struct v4l2_subdev *sd);
+#else
+static inline struct dvb_frontend *rtl2832_sdr_attach(struct dvb_frontend *fe,
+	struct i2c_adapter *i2c, const struct rtl2832_config *cfg,
+	struct v4l2_subdev *sd)
+{
+	return NULL;
+}
+#endif
+
+#ifdef CONFIG_MEDIA_ATTACH
+#define dvb_attach_sdr(FUNCTION, ARGS...) ({ \
+	void *__r = NULL; \
+	typeof(&FUNCTION) __a = symbol_request(FUNCTION); \
+	if (__a) { \
+		__r = (void *) __a(ARGS); \
+		if (__r == NULL) \
+			symbol_put(FUNCTION); \
+	} \
+	__r; \
+})
+
+#else
+#define dvb_attach_sdr(FUNCTION, ARGS...) ({ \
+	FUNCTION(ARGS); \
+})
+
+#endif
+
 static int rtl28xxu_disable_rc;
 module_param_named(disable_rc, rtl28xxu_disable_rc, int, 0644);
 MODULE_PARM_DESC(disable_rc, "disable RTL2832U remote controller");
@@ -908,7 +946,7 @@ static int rtl2832u_tuner_attach(struct dvb_usb_adapter *adap)
 				adap->fe[0]->ops.tuner_ops.get_rf_strength;
 
 		/* attach SDR */
-		dvb_attach(rtl2832_sdr_attach, adap->fe[0], &d->i2c_adap,
+		dvb_attach_sdr(rtl2832_sdr_attach, adap->fe[0], &d->i2c_adap,
 				&rtl28xxu_rtl2832_fc0012_config, NULL);
 		break;
 	case TUNER_RTL2832_FC0013:
@@ -920,7 +958,7 @@ static int rtl2832u_tuner_attach(struct dvb_usb_adapter *adap)
 				adap->fe[0]->ops.tuner_ops.get_rf_strength;
 
 		/* attach SDR */
-		dvb_attach(rtl2832_sdr_attach, adap->fe[0], &d->i2c_adap,
+		dvb_attach_sdr(rtl2832_sdr_attach, adap->fe[0], &d->i2c_adap,
 				&rtl28xxu_rtl2832_fc0013_config, NULL);
 		break;
 	case TUNER_RTL2832_E4000: {
@@ -951,7 +989,7 @@ static int rtl2832u_tuner_attach(struct dvb_usb_adapter *adap)
 			i2c_set_adapdata(i2c_adap_internal, d);
 
 			/* attach SDR */
-			dvb_attach(rtl2832_sdr_attach, adap->fe[0],
+			dvb_attach_sdr(rtl2832_sdr_attach, adap->fe[0],
 					i2c_adap_internal,
 					&rtl28xxu_rtl2832_e4000_config, sd);
 		}
@@ -982,7 +1020,7 @@ static int rtl2832u_tuner_attach(struct dvb_usb_adapter *adap)
 				adap->fe[0]->ops.tuner_ops.get_rf_strength;
 
 		/* attach SDR */
-		dvb_attach(rtl2832_sdr_attach, adap->fe[0], &d->i2c_adap,
+		dvb_attach_sdr(rtl2832_sdr_attach, adap->fe[0], &d->i2c_adap,
 				&rtl28xxu_rtl2832_r820t_config, NULL);
 		break;
 	case TUNER_RTL2832_R828D:
diff --git a/drivers/media/usb/gspca/sonixb.c b/drivers/media/usb/gspca/sonixb.c
index 7277dbd2afc..ecbcb39feb7 100644
--- a/drivers/media/usb/gspca/sonixb.c
+++ b/drivers/media/usb/gspca/sonixb.c
@@ -1430,10 +1430,8 @@ static const struct usb_device_id device_table[] = {
 	{USB_DEVICE(0x0c45, 0x600d), SB(PAS106, 101)},
 	{USB_DEVICE(0x0c45, 0x6011), SB(OV6650, 101)},
 	{USB_DEVICE(0x0c45, 0x6019), SB(OV7630, 101)},
-#if !IS_ENABLED(CONFIG_USB_SN9C102)
 	{USB_DEVICE(0x0c45, 0x6024), SB(TAS5130CXX, 102)},
 	{USB_DEVICE(0x0c45, 0x6025), SB(TAS5130CXX, 102)},
-#endif
 	{USB_DEVICE(0x0c45, 0x6027), SB(OV7630, 101)}, /* Genius Eye 310 */
 	{USB_DEVICE(0x0c45, 0x6028), SB(PAS202, 102)},
 	{USB_DEVICE(0x0c45, 0x6029), SB(PAS106, 102)},
diff --git a/drivers/media/v4l2-core/v4l2-compat-ioctl32.c b/drivers/media/v4l2-core/v4l2-compat-ioctl32.c
index 04b2daf567b..7e2411c3641 100644
--- a/drivers/media/v4l2-core/v4l2-compat-ioctl32.c
+++ b/drivers/media/v4l2-core/v4l2-compat-ioctl32.c
@@ -178,6 +178,9 @@ struct v4l2_create_buffers32 {
 
 static int __get_v4l2_format32(struct v4l2_format *kp, struct v4l2_format32 __user *up)
 {
+	if (get_user(kp->type, &up->type))
+		return -EFAULT;
+
 	switch (kp->type) {
 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
@@ -204,17 +207,16 @@ static int __get_v4l2_format32(struct v4l2_format *kp, struct v4l2_format32 __us
 
 static int get_v4l2_format32(struct v4l2_format *kp, struct v4l2_format32 __user *up)
 {
-	if (!access_ok(VERIFY_READ, up, sizeof(struct v4l2_format32)) ||
-			get_user(kp->type, &up->type))
-			return -EFAULT;
+	if (!access_ok(VERIFY_READ, up, sizeof(struct v4l2_format32)))
+		return -EFAULT;
 	return __get_v4l2_format32(kp, up);
 }
 
 static int get_v4l2_create32(struct v4l2_create_buffers *kp, struct v4l2_create_buffers32 __user *up)
 {
 	if (!access_ok(VERIFY_READ, up, sizeof(struct v4l2_create_buffers32)) ||
-	    copy_from_user(kp, up, offsetof(struct v4l2_create_buffers32, format.fmt)))
-			return -EFAULT;
+	    copy_from_user(kp, up, offsetof(struct v4l2_create_buffers32, format)))
+		return -EFAULT;
 	return __get_v4l2_format32(&kp->format, &up->format);
 }
 
diff --git a/drivers/memory/mvebu-devbus.c b/drivers/memory/mvebu-devbus.c
index 110c0362705..ff7138fd66d 100644
--- a/drivers/memory/mvebu-devbus.c
+++ b/drivers/memory/mvebu-devbus.c
@@ -2,7 +2,7 @@
  * Marvell EBU SoC Device Bus Controller
  * (memory controller for NOR/NAND/SRAM/FPGA devices)
  *
- * Copyright (C) 2013 Marvell
+ * Copyright (C) 2013-2014 Marvell
  *
  * 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
@@ -30,19 +30,47 @@
 #include <linux/platform_device.h>
 
 /* Register definitions */
-#define DEV_WIDTH_BIT		30
-#define BADR_SKEW_BIT		28
-#define RD_HOLD_BIT		23
-#define ACC_NEXT_BIT		17
-#define RD_SETUP_BIT		12
-#define ACC_FIRST_BIT		6
-
-#define SYNC_ENABLE_BIT		24
-#define WR_HIGH_BIT		16
-#define WR_LOW_BIT		8
-
-#define READ_PARAM_OFFSET	0x0
-#define WRITE_PARAM_OFFSET	0x4
+#define ARMADA_DEV_WIDTH_SHIFT		30
+#define ARMADA_BADR_SKEW_SHIFT		28
+#define ARMADA_RD_HOLD_SHIFT		23
+#define ARMADA_ACC_NEXT_SHIFT		17
+#define ARMADA_RD_SETUP_SHIFT		12
+#define ARMADA_ACC_FIRST_SHIFT		6
+
+#define ARMADA_SYNC_ENABLE_SHIFT	24
+#define ARMADA_WR_HIGH_SHIFT		16
+#define ARMADA_WR_LOW_SHIFT		8
+
+#define ARMADA_READ_PARAM_OFFSET	0x0
+#define ARMADA_WRITE_PARAM_OFFSET	0x4
+
+#define ORION_RESERVED			(0x2 << 30)
+#define ORION_BADR_SKEW_SHIFT		28
+#define ORION_WR_HIGH_EXT_BIT		BIT(27)
+#define ORION_WR_HIGH_EXT_MASK		0x8
+#define ORION_WR_LOW_EXT_BIT		BIT(26)
+#define ORION_WR_LOW_EXT_MASK		0x8
+#define ORION_ALE_WR_EXT_BIT		BIT(25)
+#define ORION_ALE_WR_EXT_MASK		0x8
+#define ORION_ACC_NEXT_EXT_BIT		BIT(24)
+#define ORION_ACC_NEXT_EXT_MASK		0x10
+#define ORION_ACC_FIRST_EXT_BIT		BIT(23)
+#define ORION_ACC_FIRST_EXT_MASK	0x10
+#define ORION_TURN_OFF_EXT_BIT		BIT(22)
+#define ORION_TURN_OFF_EXT_MASK		0x8
+#define ORION_DEV_WIDTH_SHIFT		20
+#define ORION_WR_HIGH_SHIFT		17
+#define ORION_WR_HIGH_MASK		0x7
+#define ORION_WR_LOW_SHIFT		14
+#define ORION_WR_LOW_MASK		0x7
+#define ORION_ALE_WR_SHIFT		11
+#define ORION_ALE_WR_MASK		0x7
+#define ORION_ACC_NEXT_SHIFT		7
+#define ORION_ACC_NEXT_MASK		0xF
+#define ORION_ACC_FIRST_SHIFT		3
+#define ORION_ACC_FIRST_MASK		0xF
+#define ORION_TURN_OFF_SHIFT		0
+#define ORION_TURN_OFF_MASK		0x7
 
 struct devbus_read_params {
 	u32 bus_width;
@@ -89,117 +117,167 @@ static int get_timing_param_ps(struct devbus *devbus,
 	return 0;
 }
 
-static int devbus_set_timing_params(struct devbus *devbus,
-				    struct device_node *node)
+static int devbus_get_timing_params(struct devbus *devbus,
+				    struct device_node *node,
+				    struct devbus_read_params *r,
+				    struct devbus_write_params *w)
 {
-	struct devbus_read_params r;
-	struct devbus_write_params w;
-	u32 value;
 	int err;
 
-	dev_dbg(devbus->dev, "Setting timing parameter, tick is %lu ps\n",
-		devbus->tick_ps);
-
-	/* Get read timings */
-	err = of_property_read_u32(node, "devbus,bus-width", &r.bus_width);
+	err = of_property_read_u32(node, "devbus,bus-width", &r->bus_width);
 	if (err < 0) {
 		dev_err(devbus->dev,
 			"%s has no 'devbus,bus-width' property\n",
 			node->full_name);
 		return err;
 	}
-	/* Convert bit width to byte width */
-	r.bus_width /= 8;
+
+	/*
+	 * The bus width is encoded into the register as 0 for 8 bits,
+	 * and 1 for 16 bits, so we do the necessary conversion here.
+	 */
+	if (r->bus_width == 8)
+		r->bus_width = 0;
+	else if (r->bus_width == 16)
+		r->bus_width = 1;
+	else {
+		dev_err(devbus->dev, "invalid bus width %d\n", r->bus_width);
+		return -EINVAL;
+	}
 
 	err = get_timing_param_ps(devbus, node, "devbus,badr-skew-ps",
-				 &r.badr_skew);
+				 &r->badr_skew);
 	if (err < 0)
 		return err;
 
 	err = get_timing_param_ps(devbus, node, "devbus,turn-off-ps",
-				 &r.turn_off);
+				 &r->turn_off);
 	if (err < 0)
 		return err;
 
 	err = get_timing_param_ps(devbus, node, "devbus,acc-first-ps",
-				 &r.acc_first);
+				 &r->acc_first);
 	if (err < 0)
 		return err;
 
 	err = get_timing_param_ps(devbus, node, "devbus,acc-next-ps",
-				 &r.acc_next);
+				 &r->acc_next);
 	if (err < 0)
 		return err;
 
-	err = get_timing_param_ps(devbus, node, "devbus,rd-setup-ps",
-				 &r.rd_setup);
-	if (err < 0)
-		return err;
-
-	err = get_timing_param_ps(devbus, node, "devbus,rd-hold-ps",
-				 &r.rd_hold);
-	if (err < 0)
-		return err;
-
-	/* Get write timings */
-	err = of_property_read_u32(node, "devbus,sync-enable",
-				  &w.sync_enable);
-	if (err < 0) {
-		dev_err(devbus->dev,
-			"%s has no 'devbus,sync-enable' property\n",
-			node->full_name);
-		return err;
+	if (of_device_is_compatible(devbus->dev->of_node, "marvell,mvebu-devbus")) {
+		err = get_timing_param_ps(devbus, node, "devbus,rd-setup-ps",
+					  &r->rd_setup);
+		if (err < 0)
+			return err;
+
+		err = get_timing_param_ps(devbus, node, "devbus,rd-hold-ps",
+					  &r->rd_hold);
+		if (err < 0)
+			return err;
+
+		err = of_property_read_u32(node, "devbus,sync-enable",
+					   &w->sync_enable);
+		if (err < 0) {
+			dev_err(devbus->dev,
+				"%s has no 'devbus,sync-enable' property\n",
+				node->full_name);
+			return err;
+		}
 	}
 
 	err = get_timing_param_ps(devbus, node, "devbus,ale-wr-ps",
-				 &w.ale_wr);
+				 &w->ale_wr);
 	if (err < 0)
 		return err;
 
 	err = get_timing_param_ps(devbus, node, "devbus,wr-low-ps",
-				 &w.wr_low);
+				 &w->wr_low);
 	if (err < 0)
 		return err;
 
 	err = get_timing_param_ps(devbus, node, "devbus,wr-high-ps",
-				 &w.wr_high);
+				 &w->wr_high);
 	if (err < 0)
 		return err;
 
+	return 0;
+}
+
+static void devbus_orion_set_timing_params(struct devbus *devbus,
+					  struct device_node *node,
+					  struct devbus_read_params *r,
+					  struct devbus_write_params *w)
+{
+	u32 value;
+
+	/*
+	 * The hardware designers found it would be a good idea to
+	 * split most of the values in the register into two fields:
+	 * one containing all the low-order bits, and another one
+	 * containing just the high-order bit. For all of those
+	 * fields, we have to split the value into these two parts.
+	 */
+	value =	(r->turn_off   & ORION_TURN_OFF_MASK)  << ORION_TURN_OFF_SHIFT  |
+		(r->acc_first  & ORION_ACC_FIRST_MASK) << ORION_ACC_FIRST_SHIFT |
+		(r->acc_next   & ORION_ACC_NEXT_MASK)  << ORION_ACC_NEXT_SHIFT  |
+		(w->ale_wr     & ORION_ALE_WR_MASK)    << ORION_ALE_WR_SHIFT    |
+		(w->wr_low     & ORION_WR_LOW_MASK)    << ORION_WR_LOW_SHIFT    |
+		(w->wr_high    & ORION_WR_HIGH_MASK)   << ORION_WR_HIGH_SHIFT   |
+		r->bus_width                           << ORION_DEV_WIDTH_SHIFT |
+		((r->turn_off  & ORION_TURN_OFF_EXT_MASK)  ? ORION_TURN_OFF_EXT_BIT  : 0) |
+		((r->acc_first & ORION_ACC_FIRST_EXT_MASK) ? ORION_ACC_FIRST_EXT_BIT : 0) |
+		((r->acc_next  & ORION_ACC_NEXT_EXT_MASK)  ? ORION_ACC_NEXT_EXT_BIT  : 0) |
+		((w->ale_wr    & ORION_ALE_WR_EXT_MASK)    ? ORION_ALE_WR_EXT_BIT    : 0) |
+		((w->wr_low    & ORION_WR_LOW_EXT_MASK)    ? ORION_WR_LOW_EXT_BIT    : 0) |
+		((w->wr_high   & ORION_WR_HIGH_EXT_MASK)   ? ORION_WR_HIGH_EXT_BIT   : 0) |
+		(r->badr_skew << ORION_BADR_SKEW_SHIFT) |
+		ORION_RESERVED;
+
+	writel(value, devbus->base);
+}
+
+static void devbus_armada_set_timing_params(struct devbus *devbus,
+					   struct device_node *node,
+					   struct devbus_read_params *r,
+					   struct devbus_write_params *w)
+{
+	u32 value;
+
 	/* Set read timings */
-	value = r.bus_width << DEV_WIDTH_BIT |
-		r.badr_skew << BADR_SKEW_BIT |
-		r.rd_hold   << RD_HOLD_BIT   |
-		r.acc_next  << ACC_NEXT_BIT  |
-		r.rd_setup  << RD_SETUP_BIT  |
-		r.acc_first << ACC_FIRST_BIT |
-		r.turn_off;
+	value = r->bus_width << ARMADA_DEV_WIDTH_SHIFT |
+		r->badr_skew << ARMADA_BADR_SKEW_SHIFT |
+		r->rd_hold   << ARMADA_RD_HOLD_SHIFT   |
+		r->acc_next  << ARMADA_ACC_NEXT_SHIFT  |
+		r->rd_setup  << ARMADA_RD_SETUP_SHIFT  |
+		r->acc_first << ARMADA_ACC_FIRST_SHIFT |
+		r->turn_off;
 
 	dev_dbg(devbus->dev, "read parameters register 0x%p = 0x%x\n",
-		devbus->base + READ_PARAM_OFFSET,
+		devbus->base + ARMADA_READ_PARAM_OFFSET,
 		value);
 
-	writel(value, devbus->base + READ_PARAM_OFFSET);
+	writel(value, devbus->base + ARMADA_READ_PARAM_OFFSET);
 
 	/* Set write timings */
-	value = w.sync_enable  << SYNC_ENABLE_BIT |
-		w.wr_low       << WR_LOW_BIT      |
-		w.wr_high      << WR_HIGH_BIT     |
-		w.ale_wr;
+	value = w->sync_enable  << ARMADA_SYNC_ENABLE_SHIFT |
+		w->wr_low       << ARMADA_WR_LOW_SHIFT      |
+		w->wr_high      << ARMADA_WR_HIGH_SHIFT     |
+		w->ale_wr;
 
 	dev_dbg(devbus->dev, "write parameters register: 0x%p = 0x%x\n",
-		devbus->base + WRITE_PARAM_OFFSET,
+		devbus->base + ARMADA_WRITE_PARAM_OFFSET,
 		value);
 
-	writel(value, devbus->base + WRITE_PARAM_OFFSET);
-
-	return 0;
+	writel(value, devbus->base + ARMADA_WRITE_PARAM_OFFSET);
 }
 
 static int mvebu_devbus_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
 	struct device_node *node = pdev->dev.of_node;
+	struct devbus_read_params r;
+	struct devbus_write_params w;
 	struct devbus *devbus;
 	struct resource *res;
 	struct clk *clk;
@@ -229,10 +307,21 @@ static int mvebu_devbus_probe(struct platform_device *pdev)
 	rate = clk_get_rate(clk) / 1000;
 	devbus->tick_ps = 1000000000 / rate;
 
-	/* Read the device tree node and set the new timing parameters */
-	err = devbus_set_timing_params(devbus, node);
-	if (err < 0)
-		return err;
+	dev_dbg(devbus->dev, "Setting timing parameter, tick is %lu ps\n",
+		devbus->tick_ps);
+
+	if (!of_property_read_bool(node, "devbus,keep-config")) {
+		/* Read the Device Tree node */
+		err = devbus_get_timing_params(devbus, node, &r, &w);
+		if (err < 0)
+			return err;
+
+		/* Set the new timing parameters */
+		if (of_device_is_compatible(node, "marvell,orion-devbus"))
+			devbus_orion_set_timing_params(devbus, node, &r, &w);
+		else
+			devbus_armada_set_timing_params(devbus, node, &r, &w);
+	}
 
 	/*
 	 * We need to create a child device explicitly from here to
@@ -248,6 +337,7 @@ static int mvebu_devbus_probe(struct platform_device *pdev)
 
 static const struct of_device_id mvebu_devbus_of_match[] = {
 	{ .compatible = "marvell,mvebu-devbus" },
+	{ .compatible = "marvell,orion-devbus" },
 	{},
 };
 MODULE_DEVICE_TABLE(of, mvebu_devbus_of_match);
diff --git a/drivers/mfd/Kconfig b/drivers/mfd/Kconfig
index 33834120d05..6deb8a11c12 100644
--- a/drivers/mfd/Kconfig
+++ b/drivers/mfd/Kconfig
@@ -331,15 +331,15 @@ config MFD_88PM860X
 	  battery-charger under the corresponding menus.
 
 config MFD_MAX14577
-	bool "Maxim Semiconductor MAX14577 MUIC + Charger Support"
+	bool "Maxim Semiconductor MAX14577/77836 MUIC + Charger Support"
 	depends on I2C=y
 	select MFD_CORE
 	select REGMAP_I2C
 	select REGMAP_IRQ
 	select IRQ_DOMAIN
 	help
-	  Say yes here to add support for Maxim Semiconductor MAX14577.
-	  This is a Micro-USB IC with Charger controls on chip.
+	  Say yes here to add support for Maxim Semiconductor MAX14577 and
+	  MAX77836 Micro-USB ICs with battery charger.
 	  This driver provides common support for accessing the device;
 	  additional drivers must be enabled in order to use the functionality
 	  of the device.
@@ -1227,12 +1227,17 @@ config MCP_UCB1200_TS
 
 endmenu
 
-config VEXPRESS_CONFIG
-	bool "ARM Versatile Express platform infrastructure"
-	depends on ARM || ARM64
+config MFD_VEXPRESS_SYSREG
+	bool "Versatile Express System Registers"
+	depends on VEXPRESS_CONFIG && GPIOLIB
+	default y
+	select CLKSRC_MMIO
+	select GPIO_GENERIC_PLATFORM
+	select MFD_CORE
+	select MFD_SYSCON
 	help
-	  Platform configuration infrastructure for the ARM Ltd.
-	  Versatile Express.
+	  System Registers are the platform configuration block
+	  on the ARM Ltd. Versatile Express board.
 
 endmenu
 endif
diff --git a/drivers/mfd/Makefile b/drivers/mfd/Makefile
index 2851275e265..cec3487b539 100644
--- a/drivers/mfd/Makefile
+++ b/drivers/mfd/Makefile
@@ -161,7 +161,7 @@ obj-$(CONFIG_MFD_RC5T583)	+= rc5t583.o rc5t583-irq.o
 obj-$(CONFIG_MFD_SEC_CORE)	+= sec-core.o sec-irq.o
 obj-$(CONFIG_MFD_SYSCON)	+= syscon.o
 obj-$(CONFIG_MFD_LM3533)	+= lm3533-core.o lm3533-ctrlbank.o
-obj-$(CONFIG_VEXPRESS_CONFIG)	+= vexpress-config.o vexpress-sysreg.o
+obj-$(CONFIG_MFD_VEXPRESS_SYSREG)	+= vexpress-sysreg.o
 obj-$(CONFIG_MFD_RETU)		+= retu-mfd.o
 obj-$(CONFIG_MFD_AS3711)	+= as3711.o
 obj-$(CONFIG_MFD_AS3722)	+= as3722.o
diff --git a/drivers/mfd/arizona-core.c b/drivers/mfd/arizona-core.c
index 1c3ae57082e..07e6e27be23 100644
--- a/drivers/mfd/arizona-core.c
+++ b/drivers/mfd/arizona-core.c
@@ -508,19 +508,31 @@ int arizona_of_get_type(struct device *dev)
 }
 EXPORT_SYMBOL_GPL(arizona_of_get_type);
 
+int arizona_of_get_named_gpio(struct arizona *arizona, const char *prop,
+			      bool mandatory)
+{
+	int gpio;
+
+	gpio = of_get_named_gpio(arizona->dev->of_node, prop, 0);
+	if (gpio < 0) {
+		if (mandatory)
+			dev_err(arizona->dev,
+				"Mandatory DT gpio %s missing/malformed: %d\n",
+				prop, gpio);
+
+		gpio = 0;
+	}
+
+	return gpio;
+}
+EXPORT_SYMBOL_GPL(arizona_of_get_named_gpio);
+
 static int arizona_of_get_core_pdata(struct arizona *arizona)
 {
+	struct arizona_pdata *pdata = &arizona->pdata;
 	int ret, i;
 
-	arizona->pdata.reset = of_get_named_gpio(arizona->dev->of_node,
-						 "wlf,reset", 0);
-	if (arizona->pdata.reset < 0)
-		arizona->pdata.reset = 0;
-
-	arizona->pdata.ldoena = of_get_named_gpio(arizona->dev->of_node,
-						  "wlf,ldoena", 0);
-	if (arizona->pdata.ldoena < 0)
-		arizona->pdata.ldoena = 0;
+	pdata->reset = arizona_of_get_named_gpio(arizona, "wlf,reset", true);
 
 	ret = of_property_read_u32_array(arizona->dev->of_node,
 					 "wlf,gpio-defaults",
@@ -652,6 +664,9 @@ int arizona_dev_init(struct arizona *arizona)
 		return -EINVAL;
 	}
 
+	/* Mark DCVDD as external, LDO1 driver will clear if internal */
+	arizona->external_dcvdd = true;
+
 	ret = mfd_add_devices(arizona->dev, -1, early_devs,
 			      ARRAY_SIZE(early_devs), NULL, 0, NULL);
 	if (ret != 0) {
@@ -851,14 +866,6 @@ int arizona_dev_init(struct arizona *arizona)
 			     arizona->pdata.gpio_defaults[i]);
 	}
 
-	/*
-	 * LDO1 can only be used to supply DCVDD so if it has no
-	 * consumers then DCVDD is supplied externally.
-	 */
-	if (arizona->pdata.ldo1 &&
-	    arizona->pdata.ldo1->num_consumer_supplies == 0)
-		arizona->external_dcvdd = true;
-
 	pm_runtime_set_autosuspend_delay(arizona->dev, 100);
 	pm_runtime_use_autosuspend(arizona->dev);
 	pm_runtime_enable(arizona->dev);
diff --git a/drivers/mfd/bcm590xx.c b/drivers/mfd/bcm590xx.c
index e9a33c79431..43cba1a1973 100644
--- a/drivers/mfd/bcm590xx.c
+++ b/drivers/mfd/bcm590xx.c
@@ -28,39 +28,71 @@ static const struct mfd_cell bcm590xx_devs[] = {
 	},
 };
 
-static const struct regmap_config bcm590xx_regmap_config = {
+static const struct regmap_config bcm590xx_regmap_config_pri = {
 	.reg_bits	= 8,
 	.val_bits	= 8,
-	.max_register	= BCM590XX_MAX_REGISTER,
+	.max_register	= BCM590XX_MAX_REGISTER_PRI,
 	.cache_type	= REGCACHE_RBTREE,
 };
 
-static int bcm590xx_i2c_probe(struct i2c_client *i2c,
+static const struct regmap_config bcm590xx_regmap_config_sec = {
+	.reg_bits	= 8,
+	.val_bits	= 8,
+	.max_register	= BCM590XX_MAX_REGISTER_SEC,
+	.cache_type	= REGCACHE_RBTREE,
+};
+
+static int bcm590xx_i2c_probe(struct i2c_client *i2c_pri,
 			      const struct i2c_device_id *id)
 {
 	struct bcm590xx *bcm590xx;
 	int ret;
 
-	bcm590xx = devm_kzalloc(&i2c->dev, sizeof(*bcm590xx), GFP_KERNEL);
+	bcm590xx = devm_kzalloc(&i2c_pri->dev, sizeof(*bcm590xx), GFP_KERNEL);
 	if (!bcm590xx)
 		return -ENOMEM;
 
-	i2c_set_clientdata(i2c, bcm590xx);
-	bcm590xx->dev = &i2c->dev;
-	bcm590xx->i2c_client = i2c;
+	i2c_set_clientdata(i2c_pri, bcm590xx);
+	bcm590xx->dev = &i2c_pri->dev;
+	bcm590xx->i2c_pri = i2c_pri;
 
-	bcm590xx->regmap = devm_regmap_init_i2c(i2c, &bcm590xx_regmap_config);
-	if (IS_ERR(bcm590xx->regmap)) {
-		ret = PTR_ERR(bcm590xx->regmap);
-		dev_err(&i2c->dev, "regmap initialization failed: %d\n", ret);
+	bcm590xx->regmap_pri = devm_regmap_init_i2c(i2c_pri,
+						 &bcm590xx_regmap_config_pri);
+	if (IS_ERR(bcm590xx->regmap_pri)) {
+		ret = PTR_ERR(bcm590xx->regmap_pri);
+		dev_err(&i2c_pri->dev, "primary regmap init failed: %d\n", ret);
 		return ret;
 	}
 
-	ret = mfd_add_devices(&i2c->dev, -1, bcm590xx_devs,
+	/* Secondary I2C slave address is the base address with A(2) asserted */
+	bcm590xx->i2c_sec = i2c_new_dummy(i2c_pri->adapter,
+					  i2c_pri->addr | BIT(2));
+	if (IS_ERR_OR_NULL(bcm590xx->i2c_sec)) {
+		dev_err(&i2c_pri->dev, "failed to add secondary I2C device\n");
+		return -ENODEV;
+	}
+	i2c_set_clientdata(bcm590xx->i2c_sec, bcm590xx);
+
+	bcm590xx->regmap_sec = devm_regmap_init_i2c(bcm590xx->i2c_sec,
+						&bcm590xx_regmap_config_sec);
+	if (IS_ERR(bcm590xx->regmap_sec)) {
+		ret = PTR_ERR(bcm590xx->regmap_sec);
+		dev_err(&bcm590xx->i2c_sec->dev,
+			"secondary regmap init failed: %d\n", ret);
+		goto err;
+	}
+
+	ret = mfd_add_devices(&i2c_pri->dev, -1, bcm590xx_devs,
 			      ARRAY_SIZE(bcm590xx_devs), NULL, 0, NULL);
-	if (ret < 0)
-		dev_err(&i2c->dev, "failed to add sub-devices: %d\n", ret);
+	if (ret < 0) {
+		dev_err(&i2c_pri->dev, "failed to add sub-devices: %d\n", ret);
+		goto err;
+	}
+
+	return 0;
 
+err:
+	i2c_unregister_device(bcm590xx->i2c_sec);
 	return ret;
 }
 
diff --git a/drivers/mfd/max14577.c b/drivers/mfd/max14577.c
index 5f13cefe8de..484d372a489 100644
--- a/drivers/mfd/max14577.c
+++ b/drivers/mfd/max14577.c
@@ -1,7 +1,7 @@
 /*
- * max14577.c - mfd core driver for the Maxim 14577
+ * max14577.c - mfd core driver for the Maxim 14577/77836
  *
- * Copyright (C) 2013 Samsung Electrnoics
+ * Copyright (C) 2014 Samsung Electrnoics
  * Chanwoo Choi <cw00.choi@samsung.com>
  * Krzysztof Kozlowski <k.kozlowski@samsung.com>
  *
@@ -21,6 +21,7 @@
 #include <linux/err.h>
 #include <linux/module.h>
 #include <linux/interrupt.h>
+#include <linux/of_device.h>
 #include <linux/mfd/core.h>
 #include <linux/mfd/max14577.h>
 #include <linux/mfd/max14577-private.h>
@@ -37,7 +38,38 @@ static struct mfd_cell max14577_devs[] = {
 	{ .name = "max14577-charger", },
 };
 
-static bool max14577_volatile_reg(struct device *dev, unsigned int reg)
+static struct mfd_cell max77836_devs[] = {
+	{
+		.name = "max77836-muic",
+		.of_compatible = "maxim,max77836-muic",
+	},
+	{
+		.name = "max77836-regulator",
+		.of_compatible = "maxim,max77836-regulator",
+	},
+	{
+		.name = "max77836-charger",
+		.of_compatible = "maxim,max77836-charger",
+	},
+	{
+		.name = "max77836-battery",
+		.of_compatible = "maxim,max77836-battery",
+	},
+};
+
+static struct of_device_id max14577_dt_match[] = {
+	{
+		.compatible = "maxim,max14577",
+		.data = (void *)MAXIM_DEVICE_TYPE_MAX14577,
+	},
+	{
+		.compatible = "maxim,max77836",
+		.data = (void *)MAXIM_DEVICE_TYPE_MAX77836,
+	},
+	{},
+};
+
+static bool max14577_muic_volatile_reg(struct device *dev, unsigned int reg)
 {
 	switch (reg) {
 	case MAX14577_REG_INT1 ... MAX14577_REG_STATUS3:
@@ -48,49 +80,221 @@ static bool max14577_volatile_reg(struct device *dev, unsigned int reg)
 	return false;
 }
 
-static const struct regmap_config max14577_regmap_config = {
+static bool max77836_muic_volatile_reg(struct device *dev, unsigned int reg)
+{
+	/* Any max14577 volatile registers are also max77836 volatile. */
+	if (max14577_muic_volatile_reg(dev, reg))
+		return true;
+
+	switch (reg) {
+	case MAX77836_FG_REG_VCELL_MSB ... MAX77836_FG_REG_SOC_LSB:
+	case MAX77836_FG_REG_CRATE_MSB ... MAX77836_FG_REG_CRATE_LSB:
+	case MAX77836_FG_REG_STATUS_H ... MAX77836_FG_REG_STATUS_L:
+	case MAX77836_PMIC_REG_INTSRC:
+	case MAX77836_PMIC_REG_TOPSYS_INT:
+	case MAX77836_PMIC_REG_TOPSYS_STAT:
+		return true;
+	default:
+		break;
+	}
+	return false;
+}
+
+static const struct regmap_config max14577_muic_regmap_config = {
 	.reg_bits	= 8,
 	.val_bits	= 8,
-	.volatile_reg	= max14577_volatile_reg,
+	.volatile_reg	= max14577_muic_volatile_reg,
 	.max_register	= MAX14577_REG_END,
 };
 
+static const struct regmap_config max77836_pmic_regmap_config = {
+	.reg_bits	= 8,
+	.val_bits	= 8,
+	.volatile_reg	= max77836_muic_volatile_reg,
+	.max_register	= MAX77836_PMIC_REG_END,
+};
+
 static const struct regmap_irq max14577_irqs[] = {
 	/* INT1 interrupts */
-	{ .reg_offset = 0, .mask = INT1_ADC_MASK, },
-	{ .reg_offset = 0, .mask = INT1_ADCLOW_MASK, },
-	{ .reg_offset = 0, .mask = INT1_ADCERR_MASK, },
+	{ .reg_offset = 0, .mask = MAX14577_INT1_ADC_MASK, },
+	{ .reg_offset = 0, .mask = MAX14577_INT1_ADCLOW_MASK, },
+	{ .reg_offset = 0, .mask = MAX14577_INT1_ADCERR_MASK, },
 	/* INT2 interrupts */
-	{ .reg_offset = 1, .mask = INT2_CHGTYP_MASK, },
-	{ .reg_offset = 1, .mask = INT2_CHGDETRUN_MASK, },
-	{ .reg_offset = 1, .mask = INT2_DCDTMR_MASK, },
-	{ .reg_offset = 1, .mask = INT2_DBCHG_MASK, },
-	{ .reg_offset = 1, .mask = INT2_VBVOLT_MASK, },
+	{ .reg_offset = 1, .mask = MAX14577_INT2_CHGTYP_MASK, },
+	{ .reg_offset = 1, .mask = MAX14577_INT2_CHGDETRUN_MASK, },
+	{ .reg_offset = 1, .mask = MAX14577_INT2_DCDTMR_MASK, },
+	{ .reg_offset = 1, .mask = MAX14577_INT2_DBCHG_MASK, },
+	{ .reg_offset = 1, .mask = MAX14577_INT2_VBVOLT_MASK, },
 	/* INT3 interrupts */
-	{ .reg_offset = 2, .mask = INT3_EOC_MASK, },
-	{ .reg_offset = 2, .mask = INT3_CGMBC_MASK, },
-	{ .reg_offset = 2, .mask = INT3_OVP_MASK, },
-	{ .reg_offset = 2, .mask = INT3_MBCCHGERR_MASK, },
+	{ .reg_offset = 2, .mask = MAX14577_INT3_EOC_MASK, },
+	{ .reg_offset = 2, .mask = MAX14577_INT3_CGMBC_MASK, },
+	{ .reg_offset = 2, .mask = MAX14577_INT3_OVP_MASK, },
+	{ .reg_offset = 2, .mask = MAX14577_INT3_MBCCHGERR_MASK, },
 };
 
 static const struct regmap_irq_chip max14577_irq_chip = {
 	.name			= "max14577",
 	.status_base		= MAX14577_REG_INT1,
 	.mask_base		= MAX14577_REG_INTMASK1,
-	.mask_invert		= 1,
+	.mask_invert		= true,
 	.num_regs		= 3,
 	.irqs			= max14577_irqs,
 	.num_irqs		= ARRAY_SIZE(max14577_irqs),
 };
 
+static const struct regmap_irq max77836_muic_irqs[] = {
+	/* INT1 interrupts */
+	{ .reg_offset = 0, .mask = MAX14577_INT1_ADC_MASK, },
+	{ .reg_offset = 0, .mask = MAX14577_INT1_ADCLOW_MASK, },
+	{ .reg_offset = 0, .mask = MAX14577_INT1_ADCERR_MASK, },
+	{ .reg_offset = 0, .mask = MAX77836_INT1_ADC1K_MASK, },
+	/* INT2 interrupts */
+	{ .reg_offset = 1, .mask = MAX14577_INT2_CHGTYP_MASK, },
+	{ .reg_offset = 1, .mask = MAX14577_INT2_CHGDETRUN_MASK, },
+	{ .reg_offset = 1, .mask = MAX14577_INT2_DCDTMR_MASK, },
+	{ .reg_offset = 1, .mask = MAX14577_INT2_DBCHG_MASK, },
+	{ .reg_offset = 1, .mask = MAX14577_INT2_VBVOLT_MASK, },
+	{ .reg_offset = 1, .mask = MAX77836_INT2_VIDRM_MASK, },
+	/* INT3 interrupts */
+	{ .reg_offset = 2, .mask = MAX14577_INT3_EOC_MASK, },
+	{ .reg_offset = 2, .mask = MAX14577_INT3_CGMBC_MASK, },
+	{ .reg_offset = 2, .mask = MAX14577_INT3_OVP_MASK, },
+	{ .reg_offset = 2, .mask = MAX14577_INT3_MBCCHGERR_MASK, },
+};
+
+static const struct regmap_irq_chip max77836_muic_irq_chip = {
+	.name			= "max77836-muic",
+	.status_base		= MAX14577_REG_INT1,
+	.mask_base		= MAX14577_REG_INTMASK1,
+	.mask_invert		= true,
+	.num_regs		= 3,
+	.irqs			= max77836_muic_irqs,
+	.num_irqs		= ARRAY_SIZE(max77836_muic_irqs),
+};
+
+static const struct regmap_irq max77836_pmic_irqs[] = {
+	{ .reg_offset = 0, .mask = MAX77836_TOPSYS_INT_T120C_MASK, },
+	{ .reg_offset = 0, .mask = MAX77836_TOPSYS_INT_T140C_MASK, },
+};
+
+static const struct regmap_irq_chip max77836_pmic_irq_chip = {
+	.name			= "max77836-pmic",
+	.status_base		= MAX77836_PMIC_REG_TOPSYS_INT,
+	.mask_base		= MAX77836_PMIC_REG_TOPSYS_INT_MASK,
+	.mask_invert		= false,
+	.num_regs		= 1,
+	.irqs			= max77836_pmic_irqs,
+	.num_irqs		= ARRAY_SIZE(max77836_pmic_irqs),
+};
+
+static void max14577_print_dev_type(struct max14577 *max14577)
+{
+	u8 reg_data, vendor_id, device_id;
+	int ret;
+
+	ret = max14577_read_reg(max14577->regmap, MAX14577_REG_DEVICEID,
+			&reg_data);
+	if (ret) {
+		dev_err(max14577->dev,
+			"Failed to read DEVICEID register: %d\n", ret);
+		return;
+	}
+
+	vendor_id = ((reg_data & DEVID_VENDORID_MASK) >>
+				DEVID_VENDORID_SHIFT);
+	device_id = ((reg_data & DEVID_DEVICEID_MASK) >>
+				DEVID_DEVICEID_SHIFT);
+
+	dev_info(max14577->dev, "Device type: %u (ID: 0x%x, vendor: 0x%x)\n",
+			max14577->dev_type, device_id, vendor_id);
+}
+
+/*
+ * Max77836 specific initialization code for driver probe.
+ * Adds new I2C dummy device, regmap and regmap IRQ chip.
+ * Unmasks Interrupt Source register.
+ *
+ * On success returns 0.
+ * On failure returns errno and reverts any changes done so far (e.g. remove
+ * I2C dummy device), except masking the INT SRC register.
+ */
+static int max77836_init(struct max14577 *max14577)
+{
+	int ret;
+	u8 intsrc_mask;
+
+	max14577->i2c_pmic = i2c_new_dummy(max14577->i2c->adapter,
+			I2C_ADDR_PMIC);
+	if (!max14577->i2c_pmic) {
+		dev_err(max14577->dev, "Failed to register PMIC I2C device\n");
+		return -ENODEV;
+	}
+	i2c_set_clientdata(max14577->i2c_pmic, max14577);
+
+	max14577->regmap_pmic = devm_regmap_init_i2c(max14577->i2c_pmic,
+			&max77836_pmic_regmap_config);
+	if (IS_ERR(max14577->regmap_pmic)) {
+		ret = PTR_ERR(max14577->regmap_pmic);
+		dev_err(max14577->dev, "Failed to allocate PMIC register map: %d\n",
+				ret);
+		goto err;
+	}
+
+	/* Un-mask MAX77836 Interrupt Source register */
+	ret = max14577_read_reg(max14577->regmap_pmic,
+			MAX77836_PMIC_REG_INTSRC_MASK, &intsrc_mask);
+	if (ret < 0) {
+		dev_err(max14577->dev, "Failed to read PMIC register\n");
+		goto err;
+	}
+
+	intsrc_mask &= ~(MAX77836_INTSRC_MASK_TOP_INT_MASK);
+	intsrc_mask &= ~(MAX77836_INTSRC_MASK_MUIC_CHG_INT_MASK);
+	ret = max14577_write_reg(max14577->regmap_pmic,
+			MAX77836_PMIC_REG_INTSRC_MASK, intsrc_mask);
+	if (ret < 0) {
+		dev_err(max14577->dev, "Failed to write PMIC register\n");
+		goto err;
+	}
+
+	ret = regmap_add_irq_chip(max14577->regmap_pmic, max14577->irq,
+			IRQF_TRIGGER_FALLING | IRQF_ONESHOT | IRQF_SHARED,
+			0, &max77836_pmic_irq_chip,
+			&max14577->irq_data_pmic);
+	if (ret != 0) {
+		dev_err(max14577->dev, "Failed to request PMIC IRQ %d: %d\n",
+				max14577->irq, ret);
+		goto err;
+	}
+
+	return 0;
+
+err:
+	i2c_unregister_device(max14577->i2c_pmic);
+
+	return ret;
+}
+
+/*
+ * Max77836 specific de-initialization code for driver remove.
+ */
+static void max77836_remove(struct max14577 *max14577)
+{
+	regmap_del_irq_chip(max14577->irq, max14577->irq_data_pmic);
+	i2c_unregister_device(max14577->i2c_pmic);
+}
+
 static int max14577_i2c_probe(struct i2c_client *i2c,
 			      const struct i2c_device_id *id)
 {
 	struct max14577 *max14577;
 	struct max14577_platform_data *pdata = dev_get_platdata(&i2c->dev);
 	struct device_node *np = i2c->dev.of_node;
-	u8 reg_data;
 	int ret = 0;
+	const struct regmap_irq_chip *irq_chip;
+	struct mfd_cell *mfd_devs;
+	unsigned int mfd_devs_size;
+	int irq_flags;
 
 	if (np) {
 		pdata = devm_kzalloc(&i2c->dev, sizeof(*pdata), GFP_KERNEL);
@@ -113,7 +317,8 @@ static int max14577_i2c_probe(struct i2c_client *i2c,
 	max14577->i2c = i2c;
 	max14577->irq = i2c->irq;
 
-	max14577->regmap = devm_regmap_init_i2c(i2c, &max14577_regmap_config);
+	max14577->regmap = devm_regmap_init_i2c(i2c,
+			&max14577_muic_regmap_config);
 	if (IS_ERR(max14577->regmap)) {
 		ret = PTR_ERR(max14577->regmap);
 		dev_err(max14577->dev, "Failed to allocate register map: %d\n",
@@ -121,23 +326,36 @@ static int max14577_i2c_probe(struct i2c_client *i2c,
 		return ret;
 	}
 
-	ret = max14577_read_reg(max14577->regmap, MAX14577_REG_DEVICEID,
-			&reg_data);
-	if (ret) {
-		dev_err(max14577->dev, "Device not found on this channel: %d\n",
-				ret);
-		return ret;
+	if (np) {
+		const struct of_device_id *of_id;
+
+		of_id = of_match_device(max14577_dt_match, &i2c->dev);
+		if (of_id)
+			max14577->dev_type = (unsigned int)of_id->data;
+	} else {
+		max14577->dev_type = id->driver_data;
+	}
+
+	max14577_print_dev_type(max14577);
+
+	switch (max14577->dev_type) {
+	case MAXIM_DEVICE_TYPE_MAX77836:
+		irq_chip = &max77836_muic_irq_chip;
+		mfd_devs = max77836_devs;
+		mfd_devs_size = ARRAY_SIZE(max77836_devs);
+		irq_flags = IRQF_TRIGGER_FALLING | IRQF_ONESHOT | IRQF_SHARED;
+		break;
+	case MAXIM_DEVICE_TYPE_MAX14577:
+	default:
+		irq_chip = &max14577_irq_chip;
+		mfd_devs = max14577_devs;
+		mfd_devs_size = ARRAY_SIZE(max14577_devs);
+		irq_flags = IRQF_TRIGGER_FALLING | IRQF_ONESHOT;
+		break;
 	}
-	max14577->vendor_id = ((reg_data & DEVID_VENDORID_MASK) >>
-				DEVID_VENDORID_SHIFT);
-	max14577->device_id = ((reg_data & DEVID_DEVICEID_MASK) >>
-				DEVID_DEVICEID_SHIFT);
-	dev_info(max14577->dev, "Device ID: 0x%x, vendor: 0x%x\n",
-			max14577->device_id, max14577->vendor_id);
 
 	ret = regmap_add_irq_chip(max14577->regmap, max14577->irq,
-				  IRQF_TRIGGER_FALLING | IRQF_ONESHOT, 0,
-				  &max14577_irq_chip,
+				  irq_flags, 0, irq_chip,
 				  &max14577->irq_data);
 	if (ret != 0) {
 		dev_err(&i2c->dev, "Failed to request IRQ %d: %d\n",
@@ -145,8 +363,15 @@ static int max14577_i2c_probe(struct i2c_client *i2c,
 		return ret;
 	}
 
-	ret = mfd_add_devices(max14577->dev, -1, max14577_devs,
-			ARRAY_SIZE(max14577_devs), NULL, 0,
+	/* Max77836 specific initialization code (additional regmap) */
+	if (max14577->dev_type == MAXIM_DEVICE_TYPE_MAX77836) {
+		ret = max77836_init(max14577);
+		if (ret < 0)
+			goto err_max77836;
+	}
+
+	ret = mfd_add_devices(max14577->dev, -1, mfd_devs,
+			mfd_devs_size, NULL, 0,
 			regmap_irq_get_domain(max14577->irq_data));
 	if (ret < 0)
 		goto err_mfd;
@@ -156,6 +381,9 @@ static int max14577_i2c_probe(struct i2c_client *i2c,
 	return 0;
 
 err_mfd:
+	if (max14577->dev_type == MAXIM_DEVICE_TYPE_MAX77836)
+		max77836_remove(max14577);
+err_max77836:
 	regmap_del_irq_chip(max14577->irq, max14577->irq_data);
 
 	return ret;
@@ -167,12 +395,15 @@ static int max14577_i2c_remove(struct i2c_client *i2c)
 
 	mfd_remove_devices(max14577->dev);
 	regmap_del_irq_chip(max14577->irq, max14577->irq_data);
+	if (max14577->dev_type == MAXIM_DEVICE_TYPE_MAX77836)
+		max77836_remove(max14577);
 
 	return 0;
 }
 
 static const struct i2c_device_id max14577_i2c_id[] = {
-	{ "max14577", 0 },
+	{ "max14577", MAXIM_DEVICE_TYPE_MAX14577, },
+	{ "max77836", MAXIM_DEVICE_TYPE_MAX77836, },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, max14577_i2c_id);
@@ -215,11 +446,6 @@ static int max14577_resume(struct device *dev)
 }
 #endif /* CONFIG_PM_SLEEP */
 
-static struct of_device_id max14577_dt_match[] = {
-	{ .compatible = "maxim,max14577", },
-	{},
-};
-
 static SIMPLE_DEV_PM_OPS(max14577_pm, max14577_suspend, max14577_resume);
 
 static struct i2c_driver max14577_i2c_driver = {
@@ -236,6 +462,9 @@ static struct i2c_driver max14577_i2c_driver = {
 
 static int __init max14577_i2c_init(void)
 {
+	BUILD_BUG_ON(ARRAY_SIZE(max14577_i2c_id) != MAXIM_DEVICE_TYPE_NUM);
+	BUILD_BUG_ON(ARRAY_SIZE(max14577_dt_match) != MAXIM_DEVICE_TYPE_NUM);
+
 	return i2c_add_driver(&max14577_i2c_driver);
 }
 subsys_initcall(max14577_i2c_init);
@@ -247,5 +476,5 @@ static void __exit max14577_i2c_exit(void)
 module_exit(max14577_i2c_exit);
 
 MODULE_AUTHOR("Chanwoo Choi <cw00.choi@samsung.com>, Krzysztof Kozlowski <k.kozlowski@samsung.com>");
-MODULE_DESCRIPTION("MAXIM 14577 multi-function core driver");
+MODULE_DESCRIPTION("Maxim 14577/77836 multi-function core driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/rtsx_pcr.c b/drivers/mfd/rtsx_pcr.c
index c9de3d598ea..1d15735f9ef 100644
--- a/drivers/mfd/rtsx_pcr.c
+++ b/drivers/mfd/rtsx_pcr.c
@@ -338,28 +338,58 @@ int rtsx_pci_transfer_data(struct rtsx_pcr *pcr, struct scatterlist *sglist,
 		int num_sg, bool read, int timeout)
 {
 	struct completion trans_done;
-	int err = 0, count;
+	u8 dir;
+	int err = 0, i, count;
 	long timeleft;
 	unsigned long flags;
+	struct scatterlist *sg;
+	enum dma_data_direction dma_dir;
+	u32 val;
+	dma_addr_t addr;
+	unsigned int len;
+
+	dev_dbg(&(pcr->pci->dev), "--> %s: num_sg = %d\n", __func__, num_sg);
+
+	/* don't transfer data during abort processing */
+	if (pcr->remove_pci)
+		return -EINVAL;
+
+	if ((sglist == NULL) || (num_sg <= 0))
+		return -EINVAL;
 
-	count = rtsx_pci_dma_map_sg(pcr, sglist, num_sg, read);
+	if (read) {
+		dir = DEVICE_TO_HOST;
+		dma_dir = DMA_FROM_DEVICE;
+	} else {
+		dir = HOST_TO_DEVICE;
+		dma_dir = DMA_TO_DEVICE;
+	}
+
+	count = dma_map_sg(&(pcr->pci->dev), sglist, num_sg, dma_dir);
 	if (count < 1) {
 		dev_err(&(pcr->pci->dev), "scatterlist map failed\n");
 		return -EINVAL;
 	}
 	dev_dbg(&(pcr->pci->dev), "DMA mapping count: %d\n", count);
 
+	val = ((u32)(dir & 0x01) << 29) | TRIG_DMA | ADMA_MODE;
+	pcr->sgi = 0;
+	for_each_sg(sglist, sg, count, i) {
+		addr = sg_dma_address(sg);
+		len = sg_dma_len(sg);
+		rtsx_pci_add_sg_tbl(pcr, addr, len, i == count - 1);
+	}
 
 	spin_lock_irqsave(&pcr->lock, flags);
 
 	pcr->done = &trans_done;
 	pcr->trans_result = TRANS_NOT_READY;
 	init_completion(&trans_done);
+	rtsx_pci_writel(pcr, RTSX_HDBAR, pcr->host_sg_tbl_addr);
+	rtsx_pci_writel(pcr, RTSX_HDBCTLR, val);
 
 	spin_unlock_irqrestore(&pcr->lock, flags);
 
-	rtsx_pci_dma_transfer(pcr, sglist, count, read);
-
 	timeleft = wait_for_completion_interruptible_timeout(
 			&trans_done, msecs_to_jiffies(timeout));
 	if (timeleft <= 0) {
@@ -383,7 +413,7 @@ out:
 	pcr->done = NULL;
 	spin_unlock_irqrestore(&pcr->lock, flags);
 
-	rtsx_pci_dma_unmap_sg(pcr, sglist, num_sg, read);
+	dma_unmap_sg(&(pcr->pci->dev), sglist, num_sg, dma_dir);
 
 	if ((err < 0) && (err != -ENODEV))
 		rtsx_pci_stop_cmd(pcr);
@@ -395,73 +425,6 @@ out:
 }
 EXPORT_SYMBOL_GPL(rtsx_pci_transfer_data);
 
-int rtsx_pci_dma_map_sg(struct rtsx_pcr *pcr, struct scatterlist *sglist,
-		int num_sg, bool read)
-{
-	enum dma_data_direction dir = read ? DMA_FROM_DEVICE : DMA_TO_DEVICE;
-
-	if (pcr->remove_pci)
-		return -EINVAL;
-
-	if ((sglist == NULL) || num_sg < 1)
-		return -EINVAL;
-
-	return dma_map_sg(&(pcr->pci->dev), sglist, num_sg, dir);
-}
-EXPORT_SYMBOL_GPL(rtsx_pci_dma_map_sg);
-
-int rtsx_pci_dma_unmap_sg(struct rtsx_pcr *pcr, struct scatterlist *sglist,
-		int num_sg, bool read)
-{
-	enum dma_data_direction dir = read ? DMA_FROM_DEVICE : DMA_TO_DEVICE;
-
-	if (pcr->remove_pci)
-		return -EINVAL;
-
-	if (sglist == NULL || num_sg < 1)
-		return -EINVAL;
-
-	dma_unmap_sg(&(pcr->pci->dev), sglist, num_sg, dir);
-	return num_sg;
-}
-EXPORT_SYMBOL_GPL(rtsx_pci_dma_unmap_sg);
-
-int rtsx_pci_dma_transfer(struct rtsx_pcr *pcr, struct scatterlist *sglist,
-		int sg_count, bool read)
-{
-	struct scatterlist *sg;
-	dma_addr_t addr;
-	unsigned int len;
-	int i;
-	u32 val;
-	u8 dir = read ? DEVICE_TO_HOST : HOST_TO_DEVICE;
-	unsigned long flags;
-
-	if (pcr->remove_pci)
-		return -EINVAL;
-
-	if ((sglist == NULL) || (sg_count < 1))
-		return -EINVAL;
-
-	val = ((u32)(dir & 0x01) << 29) | TRIG_DMA | ADMA_MODE;
-	pcr->sgi = 0;
-	for_each_sg(sglist, sg, sg_count, i) {
-		addr = sg_dma_address(sg);
-		len = sg_dma_len(sg);
-		rtsx_pci_add_sg_tbl(pcr, addr, len, i == sg_count - 1);
-	}
-
-	spin_lock_irqsave(&pcr->lock, flags);
-
-	rtsx_pci_writel(pcr, RTSX_HDBAR, pcr->host_sg_tbl_addr);
-	rtsx_pci_writel(pcr, RTSX_HDBCTLR, val);
-
-	spin_unlock_irqrestore(&pcr->lock, flags);
-
-	return 0;
-}
-EXPORT_SYMBOL_GPL(rtsx_pci_dma_transfer);
-
 int rtsx_pci_read_ppbuf(struct rtsx_pcr *pcr, u8 *buf, int buf_len)
 {
 	int err;
@@ -873,8 +836,6 @@ static irqreturn_t rtsx_pci_isr(int irq, void *dev_id)
 	int_reg = rtsx_pci_readl(pcr, RTSX_BIPR);
 	/* Clear interrupt flag */
 	rtsx_pci_writel(pcr, RTSX_BIPR, int_reg);
-	dev_dbg(&pcr->pci->dev, "=========== BIPR 0x%8x ==========\n", int_reg);
-
 	if ((int_reg & pcr->bier) == 0) {
 		spin_unlock(&pcr->lock);
 		return IRQ_NONE;
@@ -905,28 +866,17 @@ static irqreturn_t rtsx_pci_isr(int irq, void *dev_id)
 	}
 
 	if (int_reg & (NEED_COMPLETE_INT | DELINK_INT)) {
-		if (int_reg & (TRANS_FAIL_INT | DELINK_INT))
+		if (int_reg & (TRANS_FAIL_INT | DELINK_INT)) {
 			pcr->trans_result = TRANS_RESULT_FAIL;
-		else if (int_reg & TRANS_OK_INT)
+			if (pcr->done)
+				complete(pcr->done);
+		} else if (int_reg & TRANS_OK_INT) {
 			pcr->trans_result = TRANS_RESULT_OK;
-
-		if (pcr->done)
-			complete(pcr->done);
-
-		if (int_reg & SD_EXIST) {
-			struct rtsx_slot *slot = &pcr->slots[RTSX_SD_CARD];
-			if (slot && slot->done_transfer)
-				slot->done_transfer(slot->p_dev);
-		}
-
-		if (int_reg & MS_EXIST) {
-			struct rtsx_slot *slot = &pcr->slots[RTSX_SD_CARD];
-			if (slot && slot->done_transfer)
-				slot->done_transfer(slot->p_dev);
+			if (pcr->done)
+				complete(pcr->done);
 		}
 	}
 
-
 	if (pcr->card_inserted || pcr->card_removed)
 		schedule_delayed_work(&pcr->carddet_work,
 				msecs_to_jiffies(200));
diff --git a/drivers/mfd/syscon.c b/drivers/mfd/syscon.c
index dbea55de439..e2a04bb8bc1 100644
--- a/drivers/mfd/syscon.c
+++ b/drivers/mfd/syscon.c
@@ -18,6 +18,7 @@
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/of_platform.h>
+#include <linux/platform_data/syscon.h>
 #include <linux/platform_device.h>
 #include <linux/regmap.h>
 #include <linux/mfd/syscon.h>
@@ -119,6 +120,7 @@ static struct regmap_config syscon_regmap_config = {
 static int syscon_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
+	struct syscon_platform_data *pdata = dev_get_platdata(dev);
 	struct syscon *syscon;
 	struct resource *res;
 	void __iomem *base;
@@ -136,6 +138,8 @@ static int syscon_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	syscon_regmap_config.max_register = res->end - res->start - 3;
+	if (pdata)
+		syscon_regmap_config.name = pdata->label;
 	syscon->regmap = devm_regmap_init_mmio(dev, base,
 					&syscon_regmap_config);
 	if (IS_ERR(syscon->regmap)) {
diff --git a/drivers/mfd/tps65090.c b/drivers/mfd/tps65090.c
index ba1a25d758c..1c3e6e2efe4 100644
--- a/drivers/mfd/tps65090.c
+++ b/drivers/mfd/tps65090.c
@@ -32,14 +32,6 @@
 #define NUM_INT_REG 2
 #define TOTAL_NUM_REG 0x18
 
-/* interrupt status registers */
-#define TPS65090_INT_STS	0x0
-#define TPS65090_INT_STS2	0x1
-
-/* interrupt mask registers */
-#define TPS65090_INT_MSK	0x2
-#define TPS65090_INT_MSK2	0x3
-
 #define TPS65090_INT1_MASK_VAC_STATUS_CHANGE		1
 #define TPS65090_INT1_MASK_VSYS_STATUS_CHANGE		2
 #define TPS65090_INT1_MASK_BAT_STATUS_CHANGE		3
@@ -64,11 +56,16 @@ static struct resource charger_resources[] = {
 	}
 };
 
-static const struct mfd_cell tps65090s[] = {
-	{
+enum tps65090_cells {
+	PMIC = 0,
+	CHARGER = 1,
+};
+
+static struct mfd_cell tps65090s[] = {
+	[PMIC] = {
 		.name = "tps65090-pmic",
 	},
-	{
+	[CHARGER] = {
 		.name = "tps65090-charger",
 		.num_resources = ARRAY_SIZE(charger_resources),
 		.resources = &charger_resources[0],
@@ -139,17 +136,26 @@ static struct regmap_irq_chip tps65090_irq_chip = {
 	.irqs = tps65090_irqs,
 	.num_irqs = ARRAY_SIZE(tps65090_irqs),
 	.num_regs = NUM_INT_REG,
-	.status_base = TPS65090_INT_STS,
-	.mask_base = TPS65090_INT_MSK,
+	.status_base = TPS65090_REG_INTR_STS,
+	.mask_base = TPS65090_REG_INTR_MASK,
 	.mask_invert = true,
 };
 
 static bool is_volatile_reg(struct device *dev, unsigned int reg)
 {
-	if ((reg == TPS65090_INT_STS) || (reg == TPS65090_INT_STS2))
-		return true;
-	else
+	/* Nearly all registers have status bits mixed in, except a few */
+	switch (reg) {
+	case TPS65090_REG_INTR_MASK:
+	case TPS65090_REG_INTR_MASK2:
+	case TPS65090_REG_CG_CTRL0:
+	case TPS65090_REG_CG_CTRL1:
+	case TPS65090_REG_CG_CTRL2:
+	case TPS65090_REG_CG_CTRL3:
+	case TPS65090_REG_CG_CTRL4:
+	case TPS65090_REG_CG_CTRL5:
 		return false;
+	}
+	return true;
 }
 
 static const struct regmap_config tps65090_regmap_config = {
@@ -211,6 +217,9 @@ static int tps65090_i2c_probe(struct i2c_client *client,
 					"IRQ init failed with err: %d\n", ret);
 			return ret;
 		}
+	} else {
+		/* Don't tell children they have an IRQ that'll never fire */
+		tps65090s[CHARGER].num_resources = 0;
 	}
 
 	ret = mfd_add_devices(tps65090->dev, -1, tps65090s,
diff --git a/drivers/mfd/tps6586x.c b/drivers/mfd/tps6586x.c
index bbd54414a75..835e5549ecd 100644
--- a/drivers/mfd/tps6586x.c
+++ b/drivers/mfd/tps6586x.c
@@ -495,6 +495,10 @@ static void tps6586x_print_version(struct i2c_client *client, int version)
 	case TPS658623:
 		name = "TPS658623";
 		break;
+	case TPS658640:
+	case TPS658640v2:
+		name = "TPS658640";
+		break;
 	case TPS658643:
 		name = "TPS658643";
 		break;
diff --git a/drivers/mfd/twl-core.c b/drivers/mfd/twl-core.c
index e87140bef66..db11b4f4061 100644
--- a/drivers/mfd/twl-core.c
+++ b/drivers/mfd/twl-core.c
@@ -98,7 +98,11 @@
 #define TWL4030_BASEADD_BACKUP		0x0014
 #define TWL4030_BASEADD_INT		0x002E
 #define TWL4030_BASEADD_PM_MASTER	0x0036
+
 #define TWL4030_BASEADD_PM_RECEIVER	0x005B
+#define TWL4030_DCDC_GLOBAL_CFG		0x06
+#define SMARTREFLEX_ENABLE		BIT(3)
+
 #define TWL4030_BASEADD_RTC		0x001C
 #define TWL4030_BASEADD_SECURED_REG	0x0000
 
@@ -1204,6 +1208,11 @@ twl_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	 * Disable TWL4030/TWL5030 I2C Pull-up on I2C1 and I2C4(SR) interface.
 	 * Program I2C_SCL_CTRL_PU(bit 0)=0, I2C_SDA_CTRL_PU (bit 2)=0,
 	 * SR_I2C_SCL_CTRL_PU(bit 4)=0 and SR_I2C_SDA_CTRL_PU(bit 6)=0.
+	 *
+	 * Also, always enable SmartReflex bit as that's needed for omaps to
+	 * to do anything over I2C4 for voltage scaling even if SmartReflex
+	 * is disabled. Without the SmartReflex bit omap sys_clkreq idle
+	 * signal will never trigger for retention idle.
 	 */
 	if (twl_class_is_4030()) {
 		u8 temp;
@@ -1212,6 +1221,12 @@ twl_probe(struct i2c_client *client, const struct i2c_device_id *id)
 		temp &= ~(SR_I2C_SDA_CTRL_PU | SR_I2C_SCL_CTRL_PU | \
 			I2C_SDA_CTRL_PU | I2C_SCL_CTRL_PU);
 		twl_i2c_write_u8(TWL4030_MODULE_INTBR, temp, REG_GPPUPDCTR1);
+
+		twl_i2c_read_u8(TWL_MODULE_PM_RECEIVER, &temp,
+				TWL4030_DCDC_GLOBAL_CFG);
+		temp |= SMARTREFLEX_ENABLE;
+		twl_i2c_write_u8(TWL_MODULE_PM_RECEIVER, temp,
+				 TWL4030_DCDC_GLOBAL_CFG);
 	}
 
 	if (node) {
diff --git a/drivers/mfd/vexpress-config.c b/drivers/mfd/vexpress-config.c
deleted file mode 100644
index d0db89d13e0..00000000000
--- a/drivers/mfd/vexpress-config.c
+++ /dev/null
@@ -1,287 +0,0 @@
-/*
- * 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) 2012 ARM Limited
- */
-
-#define pr_fmt(fmt) "vexpress-config: " fmt
-
-#include <linux/bitops.h>
-#include <linux/completion.h>
-#include <linux/export.h>
-#include <linux/list.h>
-#include <linux/of.h>
-#include <linux/of_device.h>
-#include <linux/slab.h>
-#include <linux/string.h>
-#include <linux/vexpress.h>
-
-
-#define VEXPRESS_CONFIG_MAX_BRIDGES 2
-
-static struct vexpress_config_bridge {
-	struct device_node *node;
-	struct vexpress_config_bridge_info *info;
-	struct list_head transactions;
-	spinlock_t transactions_lock;
-} vexpress_config_bridges[VEXPRESS_CONFIG_MAX_BRIDGES];
-
-static DECLARE_BITMAP(vexpress_config_bridges_map,
-		ARRAY_SIZE(vexpress_config_bridges));
-static DEFINE_MUTEX(vexpress_config_bridges_mutex);
-
-struct vexpress_config_bridge *vexpress_config_bridge_register(
-		struct device_node *node,
-		struct vexpress_config_bridge_info *info)
-{
-	struct vexpress_config_bridge *bridge;
-	int i;
-
-	pr_debug("Registering bridge '%s'\n", info->name);
-
-	mutex_lock(&vexpress_config_bridges_mutex);
-	i = find_first_zero_bit(vexpress_config_bridges_map,
-			ARRAY_SIZE(vexpress_config_bridges));
-	if (i >= ARRAY_SIZE(vexpress_config_bridges)) {
-		pr_err("Can't register more bridges!\n");
-		mutex_unlock(&vexpress_config_bridges_mutex);
-		return NULL;
-	}
-	__set_bit(i, vexpress_config_bridges_map);
-	bridge = &vexpress_config_bridges[i];
-
-	bridge->node = node;
-	bridge->info = info;
-	INIT_LIST_HEAD(&bridge->transactions);
-	spin_lock_init(&bridge->transactions_lock);
-
-	mutex_unlock(&vexpress_config_bridges_mutex);
-
-	return bridge;
-}
-EXPORT_SYMBOL(vexpress_config_bridge_register);
-
-void vexpress_config_bridge_unregister(struct vexpress_config_bridge *bridge)
-{
-	struct vexpress_config_bridge __bridge = *bridge;
-	int i;
-
-	mutex_lock(&vexpress_config_bridges_mutex);
-	for (i = 0; i < ARRAY_SIZE(vexpress_config_bridges); i++)
-		if (&vexpress_config_bridges[i] == bridge)
-			__clear_bit(i, vexpress_config_bridges_map);
-	mutex_unlock(&vexpress_config_bridges_mutex);
-
-	WARN_ON(!list_empty(&__bridge.transactions));
-	while (!list_empty(&__bridge.transactions))
-		cpu_relax();
-}
-EXPORT_SYMBOL(vexpress_config_bridge_unregister);
-
-
-struct vexpress_config_func {
-	struct vexpress_config_bridge *bridge;
-	void *func;
-};
-
-struct vexpress_config_func *__vexpress_config_func_get(struct device *dev,
-		struct device_node *node)
-{
-	struct device_node *bridge_node;
-	struct vexpress_config_func *func;
-	int i;
-
-	if (WARN_ON(dev && node && dev->of_node != node))
-		return NULL;
-	if (dev && !node)
-		node = dev->of_node;
-
-	func = kzalloc(sizeof(*func), GFP_KERNEL);
-	if (!func)
-		return NULL;
-
-	bridge_node = of_node_get(node);
-	while (bridge_node) {
-		const __be32 *prop = of_get_property(bridge_node,
-				"arm,vexpress,config-bridge", NULL);
-
-		if (prop) {
-			bridge_node = of_find_node_by_phandle(
-					be32_to_cpup(prop));
-			break;
-		}
-
-		bridge_node = of_get_next_parent(bridge_node);
-	}
-
-	mutex_lock(&vexpress_config_bridges_mutex);
-	for (i = 0; i < ARRAY_SIZE(vexpress_config_bridges); i++) {
-		struct vexpress_config_bridge *bridge =
-				&vexpress_config_bridges[i];
-
-		if (test_bit(i, vexpress_config_bridges_map) &&
-				bridge->node == bridge_node) {
-			func->bridge = bridge;
-			func->func = bridge->info->func_get(dev, node);
-			break;
-		}
-	}
-	mutex_unlock(&vexpress_config_bridges_mutex);
-
-	if (!func->func) {
-		of_node_put(node);
-		kfree(func);
-		return NULL;
-	}
-
-	return func;
-}
-EXPORT_SYMBOL(__vexpress_config_func_get);
-
-void vexpress_config_func_put(struct vexpress_config_func *func)
-{
-	func->bridge->info->func_put(func->func);
-	of_node_put(func->bridge->node);
-	kfree(func);
-}
-EXPORT_SYMBOL(vexpress_config_func_put);
-
-struct vexpress_config_trans {
-	struct vexpress_config_func *func;
-	int offset;
-	bool write;
-	u32 *data;
-	int status;
-	struct completion completion;
-	struct list_head list;
-};
-
-static void vexpress_config_dump_trans(const char *what,
-		struct vexpress_config_trans *trans)
-{
-	pr_debug("%s %s trans %p func 0x%p offset %d data 0x%x status %d\n",
-			what, trans->write ? "write" : "read", trans,
-			trans->func->func, trans->offset,
-			trans->data ? *trans->data : 0, trans->status);
-}
-
-static int vexpress_config_schedule(struct vexpress_config_trans *trans)
-{
-	int status;
-	struct vexpress_config_bridge *bridge = trans->func->bridge;
-	unsigned long flags;
-
-	init_completion(&trans->completion);
-	trans->status = -EFAULT;
-
-	spin_lock_irqsave(&bridge->transactions_lock, flags);
-
-	if (list_empty(&bridge->transactions)) {
-		vexpress_config_dump_trans("Executing", trans);
-		status = bridge->info->func_exec(trans->func->func,
-				trans->offset, trans->write, trans->data);
-	} else {
-		vexpress_config_dump_trans("Queuing", trans);
-		status = VEXPRESS_CONFIG_STATUS_WAIT;
-	}
-
-	switch (status) {
-	case VEXPRESS_CONFIG_STATUS_DONE:
-		vexpress_config_dump_trans("Finished", trans);
-		trans->status = status;
-		break;
-	case VEXPRESS_CONFIG_STATUS_WAIT:
-		list_add_tail(&trans->list, &bridge->transactions);
-		break;
-	}
-
-	spin_unlock_irqrestore(&bridge->transactions_lock, flags);
-
-	return status;
-}
-
-void vexpress_config_complete(struct vexpress_config_bridge *bridge,
-		int status)
-{
-	struct vexpress_config_trans *trans;
-	unsigned long flags;
-	const char *message = "Completed";
-
-	spin_lock_irqsave(&bridge->transactions_lock, flags);
-
-	trans = list_first_entry(&bridge->transactions,
-			struct vexpress_config_trans, list);
-	trans->status = status;
-
-	do {
-		vexpress_config_dump_trans(message, trans);
-		list_del(&trans->list);
-		complete(&trans->completion);
-
-		if (list_empty(&bridge->transactions))
-			break;
-
-		trans = list_first_entry(&bridge->transactions,
-				struct vexpress_config_trans, list);
-		vexpress_config_dump_trans("Executing pending", trans);
-		trans->status = bridge->info->func_exec(trans->func->func,
-				trans->offset, trans->write, trans->data);
-		message = "Finished pending";
-	} while (trans->status == VEXPRESS_CONFIG_STATUS_DONE);
-
-	spin_unlock_irqrestore(&bridge->transactions_lock, flags);
-}
-EXPORT_SYMBOL(vexpress_config_complete);
-
-int vexpress_config_wait(struct vexpress_config_trans *trans)
-{
-	wait_for_completion(&trans->completion);
-
-	return trans->status;
-}
-EXPORT_SYMBOL(vexpress_config_wait);
-
-int vexpress_config_read(struct vexpress_config_func *func, int offset,
-		u32 *data)
-{
-	struct vexpress_config_trans trans = {
-		.func = func,
-		.offset = offset,
-		.write = false,
-		.data = data,
-		.status = 0,
-	};
-	int status = vexpress_config_schedule(&trans);
-
-	if (status == VEXPRESS_CONFIG_STATUS_WAIT)
-		status = vexpress_config_wait(&trans);
-
-	return status;
-}
-EXPORT_SYMBOL(vexpress_config_read);
-
-int vexpress_config_write(struct vexpress_config_func *func, int offset,
-		u32 data)
-{
-	struct vexpress_config_trans trans = {
-		.func = func,
-		.offset = offset,
-		.write = true,
-		.data = &data,
-		.status = 0,
-	};
-	int status = vexpress_config_schedule(&trans);
-
-	if (status == VEXPRESS_CONFIG_STATUS_WAIT)
-		status = vexpress_config_wait(&trans);
-
-	return status;
-}
-EXPORT_SYMBOL(vexpress_config_write);
diff --git a/drivers/mfd/vexpress-sysreg.c b/drivers/mfd/vexpress-sysreg.c
index 35281e804e7..9e21e4fc959 100644
--- a/drivers/mfd/vexpress-sysreg.c
+++ b/drivers/mfd/vexpress-sysreg.c
@@ -11,23 +11,22 @@
  * Copyright (C) 2012 ARM Limited
  */
 
+#include <linux/basic_mmio_gpio.h>
 #include <linux/err.h>
-#include <linux/gpio.h>
 #include <linux/io.h>
-#include <linux/leds.h>
+#include <linux/mfd/core.h>
 #include <linux/of_address.h>
+#include <linux/of_platform.h>
+#include <linux/platform_data/syscon.h>
 #include <linux/platform_device.h>
-#include <linux/regulator/driver.h>
 #include <linux/slab.h>
 #include <linux/stat.h>
-#include <linux/timer.h>
 #include <linux/vexpress.h>
 
 #define SYS_ID			0x000
 #define SYS_SW			0x004
 #define SYS_LED			0x008
 #define SYS_100HZ		0x024
-#define SYS_FLAGS		0x030
 #define SYS_FLAGSSET		0x030
 #define SYS_FLAGSCLR		0x034
 #define SYS_NVFLAGS		0x038
@@ -46,465 +45,209 @@
 #define SYS_CFGSTAT		0x0a8
 
 #define SYS_HBI_MASK		0xfff
-#define SYS_ID_HBI_SHIFT	16
 #define SYS_PROCIDx_HBI_SHIFT	0
 
-#define SYS_LED_LED(n)		(1 << (n))
-
 #define SYS_MCI_CARDIN		(1 << 0)
 #define SYS_MCI_WPROT		(1 << 1)
 
-#define SYS_FLASH_WPn		(1 << 0)
-
 #define SYS_MISC_MASTERSITE	(1 << 14)
 
-#define SYS_CFGCTRL_START	(1 << 31)
-#define SYS_CFGCTRL_WRITE	(1 << 30)
-#define SYS_CFGCTRL_DCC(n)	(((n) & 0xf) << 26)
-#define SYS_CFGCTRL_FUNC(n)	(((n) & 0x3f) << 20)
-#define SYS_CFGCTRL_SITE(n)	(((n) & 0x3) << 16)
-#define SYS_CFGCTRL_POSITION(n)	(((n) & 0xf) << 12)
-#define SYS_CFGCTRL_DEVICE(n)	(((n) & 0xfff) << 0)
-
-#define SYS_CFGSTAT_ERR		(1 << 1)
-#define SYS_CFGSTAT_COMPLETE	(1 << 0)
-
-
-static void __iomem *vexpress_sysreg_base;
-static struct device *vexpress_sysreg_dev;
-static int vexpress_master_site;
-
 
-void vexpress_flags_set(u32 data)
-{
-	writel(~0, vexpress_sysreg_base + SYS_FLAGSCLR);
-	writel(data, vexpress_sysreg_base + SYS_FLAGSSET);
-}
+static void __iomem *__vexpress_sysreg_base;
 
-u32 vexpress_get_procid(int site)
+static void __iomem *vexpress_sysreg_base(void)
 {
-	if (site == VEXPRESS_SITE_MASTER)
-		site = vexpress_master_site;
-
-	return readl(vexpress_sysreg_base + (site == VEXPRESS_SITE_DB1 ?
-			SYS_PROCID0 : SYS_PROCID1));
-}
+	if (!__vexpress_sysreg_base) {
+		struct device_node *node = of_find_compatible_node(NULL, NULL,
+				"arm,vexpress-sysreg");
 
-u32 vexpress_get_hbi(int site)
-{
-	u32 id;
-
-	switch (site) {
-	case VEXPRESS_SITE_MB:
-		id = readl(vexpress_sysreg_base + SYS_ID);
-		return (id >> SYS_ID_HBI_SHIFT) & SYS_HBI_MASK;
-	case VEXPRESS_SITE_MASTER:
-	case VEXPRESS_SITE_DB1:
-	case VEXPRESS_SITE_DB2:
-		id = vexpress_get_procid(site);
-		return (id >> SYS_PROCIDx_HBI_SHIFT) & SYS_HBI_MASK;
+		__vexpress_sysreg_base = of_iomap(node, 0);
 	}
 
-	return ~0;
-}
+	WARN_ON(!__vexpress_sysreg_base);
 
-void __iomem *vexpress_get_24mhz_clock_base(void)
-{
-	return vexpress_sysreg_base + SYS_24MHZ;
+	return __vexpress_sysreg_base;
 }
 
 
-static void vexpress_sysreg_find_prop(struct device_node *node,
-		const char *name, u32 *val)
+static int vexpress_sysreg_get_master(void)
 {
-	of_node_get(node);
-	while (node) {
-		if (of_property_read_u32(node, name, val) == 0) {
-			of_node_put(node);
-			return;
-		}
-		node = of_get_next_parent(node);
-	}
-}
-
-unsigned __vexpress_get_site(struct device *dev, struct device_node *node)
-{
-	u32 site = 0;
-
-	WARN_ON(dev && node && dev->of_node != node);
-	if (dev && !node)
-		node = dev->of_node;
-
-	if (node) {
-		vexpress_sysreg_find_prop(node, "arm,vexpress,site", &site);
-	} else if (dev && dev->bus == &platform_bus_type) {
-		struct platform_device *pdev = to_platform_device(dev);
-
-		if (pdev->num_resources == 1 &&
-				pdev->resource[0].flags == IORESOURCE_BUS)
-			site = pdev->resource[0].start;
-	} else if (dev && strncmp(dev_name(dev), "ct:", 3) == 0) {
-		site = VEXPRESS_SITE_MASTER;
-	}
+	if (readl(vexpress_sysreg_base() + SYS_MISC) & SYS_MISC_MASTERSITE)
+		return VEXPRESS_SITE_DB2;
 
-	if (site == VEXPRESS_SITE_MASTER)
-		site = vexpress_master_site;
-
-	return site;
+	return VEXPRESS_SITE_DB1;
 }
 
-
-struct vexpress_sysreg_config_func {
-	u32 template;
-	u32 device;
-};
-
-static struct vexpress_config_bridge *vexpress_sysreg_config_bridge;
-static struct timer_list vexpress_sysreg_config_timer;
-static u32 *vexpress_sysreg_config_data;
-static int vexpress_sysreg_config_tries;
-
-static void *vexpress_sysreg_config_func_get(struct device *dev,
-		struct device_node *node)
+void vexpress_flags_set(u32 data)
 {
-	struct vexpress_sysreg_config_func *config_func;
-	u32 site = 0;
-	u32 position = 0;
-	u32 dcc = 0;
-	u32 func_device[2];
-	int err = -EFAULT;
-
-	if (node) {
-		of_node_get(node);
-		vexpress_sysreg_find_prop(node, "arm,vexpress,site", &site);
-		vexpress_sysreg_find_prop(node, "arm,vexpress,position",
-				&position);
-		vexpress_sysreg_find_prop(node, "arm,vexpress,dcc", &dcc);
-		err = of_property_read_u32_array(node,
-				"arm,vexpress-sysreg,func", func_device,
-				ARRAY_SIZE(func_device));
-		of_node_put(node);
-	} else if (dev && dev->bus == &platform_bus_type) {
-		struct platform_device *pdev = to_platform_device(dev);
-
-		if (pdev->num_resources == 1 &&
-				pdev->resource[0].flags == IORESOURCE_BUS) {
-			site = pdev->resource[0].start;
-			func_device[0] = pdev->resource[0].end;
-			func_device[1] = pdev->id;
-			err = 0;
-		}
-	}
-	if (err)
-		return NULL;
-
-	config_func = kzalloc(sizeof(*config_func), GFP_KERNEL);
-	if (!config_func)
-		return NULL;
-
-	config_func->template = SYS_CFGCTRL_DCC(dcc);
-	config_func->template |= SYS_CFGCTRL_FUNC(func_device[0]);
-	config_func->template |= SYS_CFGCTRL_SITE(site == VEXPRESS_SITE_MASTER ?
-			vexpress_master_site : site);
-	config_func->template |= SYS_CFGCTRL_POSITION(position);
-	config_func->device |= func_device[1];
-
-	dev_dbg(vexpress_sysreg_dev, "func 0x%p = 0x%x, %d\n", config_func,
-			config_func->template, config_func->device);
-
-	return config_func;
+	writel(~0, vexpress_sysreg_base() + SYS_FLAGSCLR);
+	writel(data, vexpress_sysreg_base() + SYS_FLAGSSET);
 }
 
-static void vexpress_sysreg_config_func_put(void *func)
+unsigned int vexpress_get_mci_cardin(struct device *dev)
 {
-	kfree(func);
+	return readl(vexpress_sysreg_base() + SYS_MCI) & SYS_MCI_CARDIN;
 }
 
-static int vexpress_sysreg_config_func_exec(void *func, int offset,
-		bool write, u32 *data)
+u32 vexpress_get_procid(int site)
 {
-	int status;
-	struct vexpress_sysreg_config_func *config_func = func;
-	u32 command;
-
-	if (WARN_ON(!vexpress_sysreg_base))
-		return -ENOENT;
-
-	command = readl(vexpress_sysreg_base + SYS_CFGCTRL);
-	if (WARN_ON(command & SYS_CFGCTRL_START))
-		return -EBUSY;
-
-	command = SYS_CFGCTRL_START;
-	command |= write ? SYS_CFGCTRL_WRITE : 0;
-	command |= config_func->template;
-	command |= SYS_CFGCTRL_DEVICE(config_func->device + offset);
-
-	/* Use a canary for reads */
-	if (!write)
-		*data = 0xdeadbeef;
-
-	dev_dbg(vexpress_sysreg_dev, "command %x, data %x\n",
-			command, *data);
-	writel(*data, vexpress_sysreg_base + SYS_CFGDATA);
-	writel(0, vexpress_sysreg_base + SYS_CFGSTAT);
-	writel(command, vexpress_sysreg_base + SYS_CFGCTRL);
-	mb();
-
-	if (vexpress_sysreg_dev) {
-		/* Schedule completion check */
-		if (!write)
-			vexpress_sysreg_config_data = data;
-		vexpress_sysreg_config_tries = 100;
-		mod_timer(&vexpress_sysreg_config_timer,
-				jiffies + usecs_to_jiffies(100));
-		status = VEXPRESS_CONFIG_STATUS_WAIT;
-	} else {
-		/* Early execution, no timer available, have to spin */
-		u32 cfgstat;
-
-		do {
-			cpu_relax();
-			cfgstat = readl(vexpress_sysreg_base + SYS_CFGSTAT);
-		} while (!cfgstat);
-
-		if (!write && (cfgstat & SYS_CFGSTAT_COMPLETE))
-			*data = readl(vexpress_sysreg_base + SYS_CFGDATA);
-		status = VEXPRESS_CONFIG_STATUS_DONE;
-
-		if (cfgstat & SYS_CFGSTAT_ERR)
-			status = -EINVAL;
-	}
+	if (site == VEXPRESS_SITE_MASTER)
+		site = vexpress_sysreg_get_master();
 
-	return status;
+	return readl(vexpress_sysreg_base() + (site == VEXPRESS_SITE_DB1 ?
+			SYS_PROCID0 : SYS_PROCID1));
 }
 
-struct vexpress_config_bridge_info vexpress_sysreg_config_bridge_info = {
-	.name = "vexpress-sysreg",
-	.func_get = vexpress_sysreg_config_func_get,
-	.func_put = vexpress_sysreg_config_func_put,
-	.func_exec = vexpress_sysreg_config_func_exec,
-};
-
-static void vexpress_sysreg_config_complete(unsigned long data)
+void __iomem *vexpress_get_24mhz_clock_base(void)
 {
-	int status = VEXPRESS_CONFIG_STATUS_DONE;
-	u32 cfgstat = readl(vexpress_sysreg_base + SYS_CFGSTAT);
-
-	if (cfgstat & SYS_CFGSTAT_ERR)
-		status = -EINVAL;
-	if (!vexpress_sysreg_config_tries--)
-		status = -ETIMEDOUT;
-
-	if (status < 0) {
-		dev_err(vexpress_sysreg_dev, "error %d\n", status);
-	} else if (!(cfgstat & SYS_CFGSTAT_COMPLETE)) {
-		mod_timer(&vexpress_sysreg_config_timer,
-				jiffies + usecs_to_jiffies(50));
-		return;
-	}
-
-	if (vexpress_sysreg_config_data) {
-		*vexpress_sysreg_config_data = readl(vexpress_sysreg_base +
-				SYS_CFGDATA);
-		dev_dbg(vexpress_sysreg_dev, "read data %x\n",
-				*vexpress_sysreg_config_data);
-		vexpress_sysreg_config_data = NULL;
-	}
-
-	vexpress_config_complete(vexpress_sysreg_config_bridge, status);
+	return vexpress_sysreg_base() + SYS_24MHZ;
 }
 
 
-void vexpress_sysreg_setup(struct device_node *node)
-{
-	if (WARN_ON(!vexpress_sysreg_base))
-		return;
-
-	if (readl(vexpress_sysreg_base + SYS_MISC) & SYS_MISC_MASTERSITE)
-		vexpress_master_site = VEXPRESS_SITE_DB2;
-	else
-		vexpress_master_site = VEXPRESS_SITE_DB1;
-
-	vexpress_sysreg_config_bridge = vexpress_config_bridge_register(
-			node, &vexpress_sysreg_config_bridge_info);
-	WARN_ON(!vexpress_sysreg_config_bridge);
-}
-
 void __init vexpress_sysreg_early_init(void __iomem *base)
 {
-	vexpress_sysreg_base = base;
-	vexpress_sysreg_setup(NULL);
-}
-
-void __init vexpress_sysreg_of_early_init(void)
-{
-	struct device_node *node;
-
-	if (vexpress_sysreg_base)
-		return;
+	__vexpress_sysreg_base = base;
 
-	node = of_find_compatible_node(NULL, NULL, "arm,vexpress-sysreg");
-	if (node) {
-		vexpress_sysreg_base = of_iomap(node, 0);
-		vexpress_sysreg_setup(node);
-	}
+	vexpress_config_set_master(vexpress_sysreg_get_master());
 }
 
 
-#ifdef CONFIG_GPIOLIB
-
-#define VEXPRESS_SYSREG_GPIO(_name, _reg, _value) \
-	[VEXPRESS_GPIO_##_name] = { \
-		.reg = _reg, \
-		.value = _reg##_##_value, \
-	}
+/* The sysreg block is just a random collection of various functions... */
 
-static struct vexpress_sysreg_gpio {
-	unsigned long reg;
-	u32 value;
-} vexpress_sysreg_gpios[] = {
-	VEXPRESS_SYSREG_GPIO(MMC_CARDIN,	SYS_MCI,	CARDIN),
-	VEXPRESS_SYSREG_GPIO(MMC_WPROT,		SYS_MCI,	WPROT),
-	VEXPRESS_SYSREG_GPIO(FLASH_WPn,		SYS_FLASH,	WPn),
-	VEXPRESS_SYSREG_GPIO(LED0,		SYS_LED,	LED(0)),
-	VEXPRESS_SYSREG_GPIO(LED1,		SYS_LED,	LED(1)),
-	VEXPRESS_SYSREG_GPIO(LED2,		SYS_LED,	LED(2)),
-	VEXPRESS_SYSREG_GPIO(LED3,		SYS_LED,	LED(3)),
-	VEXPRESS_SYSREG_GPIO(LED4,		SYS_LED,	LED(4)),
-	VEXPRESS_SYSREG_GPIO(LED5,		SYS_LED,	LED(5)),
-	VEXPRESS_SYSREG_GPIO(LED6,		SYS_LED,	LED(6)),
-	VEXPRESS_SYSREG_GPIO(LED7,		SYS_LED,	LED(7)),
+static struct syscon_platform_data vexpress_sysreg_sys_id_pdata = {
+	.label = "sys_id",
 };
 
-static int vexpress_sysreg_gpio_direction_input(struct gpio_chip *chip,
-				       unsigned offset)
-{
-	return 0;
-}
-
-static int vexpress_sysreg_gpio_get(struct gpio_chip *chip,
-				       unsigned offset)
-{
-	struct vexpress_sysreg_gpio *gpio = &vexpress_sysreg_gpios[offset];
-	u32 reg_value = readl(vexpress_sysreg_base + gpio->reg);
-
-	return !!(reg_value & gpio->value);
-}
-
-static void vexpress_sysreg_gpio_set(struct gpio_chip *chip,
-				       unsigned offset, int value)
-{
-	struct vexpress_sysreg_gpio *gpio = &vexpress_sysreg_gpios[offset];
-	u32 reg_value = readl(vexpress_sysreg_base + gpio->reg);
-
-	if (value)
-		reg_value |= gpio->value;
-	else
-		reg_value &= ~gpio->value;
-
-	writel(reg_value, vexpress_sysreg_base + gpio->reg);
-}
-
-static int vexpress_sysreg_gpio_direction_output(struct gpio_chip *chip,
-						unsigned offset, int value)
-{
-	vexpress_sysreg_gpio_set(chip, offset, value);
-
-	return 0;
-}
-
-static struct gpio_chip vexpress_sysreg_gpio_chip = {
-	.label = "vexpress-sysreg",
-	.direction_input = vexpress_sysreg_gpio_direction_input,
-	.direction_output = vexpress_sysreg_gpio_direction_output,
-	.get = vexpress_sysreg_gpio_get,
-	.set = vexpress_sysreg_gpio_set,
-	.ngpio = ARRAY_SIZE(vexpress_sysreg_gpios),
-	.base = 0,
+static struct bgpio_pdata vexpress_sysreg_sys_led_pdata = {
+	.label = "sys_led",
+	.base = -1,
+	.ngpio = 8,
 };
 
-
-#define VEXPRESS_SYSREG_GREEN_LED(_name, _default_trigger, _gpio) \
-	{ \
-		.name = "v2m:green:"_name, \
-		.default_trigger = _default_trigger, \
-		.gpio = VEXPRESS_GPIO_##_gpio, \
-	}
-
-struct gpio_led vexpress_sysreg_leds[] = {
-	VEXPRESS_SYSREG_GREEN_LED("user1",	"heartbeat",	LED0),
-	VEXPRESS_SYSREG_GREEN_LED("user2",	"mmc0",		LED1),
-	VEXPRESS_SYSREG_GREEN_LED("user3",	"cpu0",		LED2),
-	VEXPRESS_SYSREG_GREEN_LED("user4",	"cpu1",		LED3),
-	VEXPRESS_SYSREG_GREEN_LED("user5",	"cpu2",		LED4),
-	VEXPRESS_SYSREG_GREEN_LED("user6",	"cpu3",		LED5),
-	VEXPRESS_SYSREG_GREEN_LED("user7",	"cpu4",		LED6),
-	VEXPRESS_SYSREG_GREEN_LED("user8",	"cpu5",		LED7),
+static struct bgpio_pdata vexpress_sysreg_sys_mci_pdata = {
+	.label = "sys_mci",
+	.base = -1,
+	.ngpio = 2,
 };
 
-struct gpio_led_platform_data vexpress_sysreg_leds_pdata = {
-	.num_leds = ARRAY_SIZE(vexpress_sysreg_leds),
-	.leds = vexpress_sysreg_leds,
+static struct bgpio_pdata vexpress_sysreg_sys_flash_pdata = {
+	.label = "sys_flash",
+	.base = -1,
+	.ngpio = 1,
 };
 
-#endif
-
+static struct syscon_platform_data vexpress_sysreg_sys_misc_pdata = {
+	.label = "sys_misc",
+};
 
-static ssize_t vexpress_sysreg_sys_id_show(struct device *dev,
-		struct device_attribute *attr, char *buf)
-{
-	return sprintf(buf, "0x%08x\n", readl(vexpress_sysreg_base + SYS_ID));
-}
+static struct syscon_platform_data vexpress_sysreg_sys_procid_pdata = {
+	.label = "sys_procid",
+};
 
-DEVICE_ATTR(sys_id, S_IRUGO, vexpress_sysreg_sys_id_show, NULL);
+static struct mfd_cell vexpress_sysreg_cells[] = {
+	{
+		.name = "syscon",
+		.num_resources = 1,
+		.resources = (struct resource []) {
+			DEFINE_RES_MEM(SYS_ID, 0x4),
+		},
+		.platform_data = &vexpress_sysreg_sys_id_pdata,
+		.pdata_size = sizeof(vexpress_sysreg_sys_id_pdata),
+	}, {
+		.name = "basic-mmio-gpio",
+		.of_compatible = "arm,vexpress-sysreg,sys_led",
+		.num_resources = 1,
+		.resources = (struct resource []) {
+			DEFINE_RES_MEM_NAMED(SYS_LED, 0x4, "dat"),
+		},
+		.platform_data = &vexpress_sysreg_sys_led_pdata,
+		.pdata_size = sizeof(vexpress_sysreg_sys_led_pdata),
+	}, {
+		.name = "basic-mmio-gpio",
+		.of_compatible = "arm,vexpress-sysreg,sys_mci",
+		.num_resources = 1,
+		.resources = (struct resource []) {
+			DEFINE_RES_MEM_NAMED(SYS_MCI, 0x4, "dat"),
+		},
+		.platform_data = &vexpress_sysreg_sys_mci_pdata,
+		.pdata_size = sizeof(vexpress_sysreg_sys_mci_pdata),
+	}, {
+		.name = "basic-mmio-gpio",
+		.of_compatible = "arm,vexpress-sysreg,sys_flash",
+		.num_resources = 1,
+		.resources = (struct resource []) {
+			DEFINE_RES_MEM_NAMED(SYS_FLASH, 0x4, "dat"),
+		},
+		.platform_data = &vexpress_sysreg_sys_flash_pdata,
+		.pdata_size = sizeof(vexpress_sysreg_sys_flash_pdata),
+	}, {
+		.name = "syscon",
+		.num_resources = 1,
+		.resources = (struct resource []) {
+			DEFINE_RES_MEM(SYS_MISC, 0x4),
+		},
+		.platform_data = &vexpress_sysreg_sys_misc_pdata,
+		.pdata_size = sizeof(vexpress_sysreg_sys_misc_pdata),
+	}, {
+		.name = "syscon",
+		.num_resources = 1,
+		.resources = (struct resource []) {
+			DEFINE_RES_MEM(SYS_PROCID0, 0x8),
+		},
+		.platform_data = &vexpress_sysreg_sys_procid_pdata,
+		.pdata_size = sizeof(vexpress_sysreg_sys_procid_pdata),
+	}, {
+		.name = "vexpress-syscfg",
+		.num_resources = 1,
+		.resources = (struct resource []) {
+			DEFINE_RES_MEM(SYS_CFGDATA, 0xc),
+		},
+	}
+};
 
 static int vexpress_sysreg_probe(struct platform_device *pdev)
 {
-	int err;
-	struct resource *res = platform_get_resource(pdev,
-			IORESOURCE_MEM, 0);
-
-	if (!devm_request_mem_region(&pdev->dev, res->start,
-			resource_size(res), pdev->name)) {
-		dev_err(&pdev->dev, "Failed to request memory region!\n");
-		return -EBUSY;
-	}
+	struct resource *mem;
+	void __iomem *base;
+	struct bgpio_chip *mmc_gpio_chip;
+	u32 dt_hbi;
 
-	if (!vexpress_sysreg_base) {
-		vexpress_sysreg_base = devm_ioremap(&pdev->dev, res->start,
-				resource_size(res));
-		vexpress_sysreg_setup(pdev->dev.of_node);
-	}
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!mem)
+		return -EINVAL;
 
-	if (!vexpress_sysreg_base) {
-		dev_err(&pdev->dev, "Failed to obtain base address!\n");
-		return -EFAULT;
-	}
+	base = devm_ioremap(&pdev->dev, mem->start, resource_size(mem));
+	if (!base)
+		return -ENOMEM;
 
-	setup_timer(&vexpress_sysreg_config_timer,
-			vexpress_sysreg_config_complete, 0);
+	vexpress_config_set_master(vexpress_sysreg_get_master());
 
-	vexpress_sysreg_dev = &pdev->dev;
+	/* Confirm board type against DT property, if available */
+	if (of_property_read_u32(of_allnodes, "arm,hbi", &dt_hbi) == 0) {
+		u32 id = vexpress_get_procid(VEXPRESS_SITE_MASTER);
+		u32 hbi = (id >> SYS_PROCIDx_HBI_SHIFT) & SYS_HBI_MASK;
 
-#ifdef CONFIG_GPIOLIB
-	vexpress_sysreg_gpio_chip.dev = &pdev->dev;
-	err = gpiochip_add(&vexpress_sysreg_gpio_chip);
-	if (err) {
-		vexpress_config_bridge_unregister(
-				vexpress_sysreg_config_bridge);
-		dev_err(&pdev->dev, "Failed to register GPIO chip! (%d)\n",
-				err);
-		return err;
+		if (WARN_ON(dt_hbi != hbi))
+			dev_warn(&pdev->dev, "DT HBI (%x) is not matching hardware (%x)!\n",
+					dt_hbi, hbi);
 	}
 
-	platform_device_register_data(vexpress_sysreg_dev, "leds-gpio",
-			PLATFORM_DEVID_AUTO, &vexpress_sysreg_leds_pdata,
-			sizeof(vexpress_sysreg_leds_pdata));
-#endif
-
-	device_create_file(vexpress_sysreg_dev, &dev_attr_sys_id);
-
-	return 0;
+	/*
+	 * Duplicated SYS_MCI pseudo-GPIO controller for compatibility with
+	 * older trees using sysreg node for MMC control lines.
+	 */
+	mmc_gpio_chip = devm_kzalloc(&pdev->dev, sizeof(*mmc_gpio_chip),
+			GFP_KERNEL);
+	if (!mmc_gpio_chip)
+		return -ENOMEM;
+	bgpio_init(mmc_gpio_chip, &pdev->dev, 0x4, base + SYS_MCI,
+			NULL, NULL, NULL, NULL, 0);
+	mmc_gpio_chip->gc.ngpio = 2;
+	gpiochip_add(&mmc_gpio_chip->gc);
+
+	return mfd_add_devices(&pdev->dev, PLATFORM_DEVID_AUTO,
+			vexpress_sysreg_cells,
+			ARRAY_SIZE(vexpress_sysreg_cells), mem, 0, NULL);
 }
 
 static const struct of_device_id vexpress_sysreg_match[] = {
@@ -522,7 +265,12 @@ static struct platform_driver vexpress_sysreg_driver = {
 
 static int __init vexpress_sysreg_init(void)
 {
-	vexpress_sysreg_of_early_init();
+	struct device_node *node;
+
+	/* Need the sysreg early, before any other device... */
+	for_each_matching_node(node, vexpress_sysreg_match)
+		of_platform_device_create(node, NULL, NULL);
+
 	return platform_driver_register(&vexpress_sysreg_driver);
 }
 core_initcall(vexpress_sysreg_init);
diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig
index 1cb74085e41..a43d0c46727 100644
--- a/drivers/misc/Kconfig
+++ b/drivers/misc/Kconfig
@@ -54,6 +54,7 @@ config AD525X_DPOT_SPI
 config ATMEL_PWM
 	tristate "Atmel AT32/AT91 PWM support"
 	depends on HAVE_CLK
+	depends on AVR32 || AT91SAM9263 || AT91SAM9RL || AT91SAM9G45
 	help
 	  This option enables device driver support for the PWM channels
 	  on certain Atmel processors.  Pulse Width Modulation is used for
@@ -200,7 +201,7 @@ config ICS932S401
 
 config ATMEL_SSC
 	tristate "Device driver for Atmel SSC peripheral"
-	depends on HAS_IOMEM
+	depends on HAS_IOMEM && (AVR32 || ARCH_AT91 || COMPILE_TEST)
 	---help---
 	  This option enables device driver support for Atmel Synchronized
 	  Serial Communication peripheral (SSC).
@@ -300,8 +301,8 @@ config SGI_GRU_DEBUG
 	depends on SGI_GRU
 	default n
 	---help---
-	This option enables addition debugging code for the SGI GRU driver. If
-	you are unsure, say N.
+	This option enables additional debugging code for the SGI GRU driver.
+	If you are unsure, say N.
 
 config APDS9802ALS
 	tristate "Medfield Avago APDS9802 ALS Sensor module"
@@ -468,7 +469,7 @@ config BMP085_SPI
 config PCH_PHUB
 	tristate "Intel EG20T PCH/LAPIS Semicon IOH(ML7213/ML7223/ML7831) PHUB"
 	select GENERIC_NET_UTILS
-	depends on PCI
+	depends on PCI && (X86_32 || COMPILE_TEST)
 	help
 	  This driver is for PCH(Platform controller Hub) PHUB(Packet Hub) of
 	  Intel Topcliff which is an IOH(Input/Output Hub) for x86 embedded
@@ -515,6 +516,15 @@ config SRAM
 	  the genalloc API. It is supposed to be used for small on-chip SRAM
 	  areas found on many SoCs.
 
+config VEXPRESS_SYSCFG
+	bool "Versatile Express System Configuration driver"
+	depends on VEXPRESS_CONFIG
+	default y
+	help
+	  ARM Ltd. Versatile Express uses specialised platform configuration
+	  bus. System Configuration interface is one of the possible means
+	  of generating transactions on this bus.
+
 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 7eb4b69580c..d59ce1261b3 100644
--- a/drivers/misc/Makefile
+++ b/drivers/misc/Makefile
@@ -55,3 +55,4 @@ obj-$(CONFIG_SRAM)		+= sram.o
 obj-y				+= mic/
 obj-$(CONFIG_GENWQE)		+= genwqe/
 obj-$(CONFIG_ECHO)		+= echo/
+obj-$(CONFIG_VEXPRESS_SYSCFG)	+= vexpress-syscfg.o
diff --git a/drivers/misc/arm-charlcd.c b/drivers/misc/arm-charlcd.c
index b7ebf8021d9..c72e96b523e 100644
--- a/drivers/misc/arm-charlcd.c
+++ b/drivers/misc/arm-charlcd.c
@@ -11,6 +11,7 @@
 #include <linux/module.h>
 #include <linux/interrupt.h>
 #include <linux/platform_device.h>
+#include <linux/of.h>
 #include <linux/completion.h>
 #include <linux/delay.h>
 #include <linux/io.h>
@@ -366,11 +367,17 @@ static const struct dev_pm_ops charlcd_pm_ops = {
 	.resume = charlcd_resume,
 };
 
+static const struct of_device_id charlcd_match[] = {
+	{ .compatible = "arm,versatile-lcd", },
+	{}
+};
+
 static struct platform_driver charlcd_driver = {
 	.driver = {
 		.name = DRIVERNAME,
 		.owner = THIS_MODULE,
 		.pm = &charlcd_pm_ops,
+		.of_match_table = of_match_ptr(charlcd_match),
 	},
 	.remove = __exit_p(charlcd_remove),
 };
diff --git a/drivers/misc/ds1682.c b/drivers/misc/ds1682.c
index 6a672f9ef52..b909fb30232 100644
--- a/drivers/misc/ds1682.c
+++ b/drivers/misc/ds1682.c
@@ -85,7 +85,6 @@ static ssize_t ds1682_store(struct device *dev, struct device_attribute *attr,
 {
 	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
 	struct i2c_client *client = to_i2c_client(dev);
-	char *endp;
 	u64 val;
 	__le32 val_le;
 	int rc;
@@ -93,8 +92,8 @@ static ssize_t ds1682_store(struct device *dev, struct device_attribute *attr,
 	dev_dbg(dev, "ds1682_store() called on %s\n", attr->attr.name);
 
 	/* Decode input */
-	val = simple_strtoull(buf, &endp, 0);
-	if (buf == endp) {
+	rc = kstrtoull(buf, 0, &val);
+	if (rc < 0) {
 		dev_dbg(dev, "input string not a number\n");
 		return -EINVAL;
 	}
diff --git a/drivers/misc/genwqe/card_base.h b/drivers/misc/genwqe/card_base.h
index 5e4dbd21f89..0e608a28860 100644
--- a/drivers/misc/genwqe/card_base.h
+++ b/drivers/misc/genwqe/card_base.h
@@ -337,6 +337,44 @@ enum genwqe_requ_state {
 };
 
 /**
+ * struct genwqe_sgl - Scatter gather list describing user-space memory
+ * @sgl:            scatter gather list needs to be 128 byte aligned
+ * @sgl_dma_addr:   dma address of sgl
+ * @sgl_size:       size of area used for sgl
+ * @user_addr:      user-space address of memory area
+ * @user_size:      size of user-space memory area
+ * @page:           buffer for partial pages if needed
+ * @page_dma_addr:  dma address partial pages
+ */
+struct genwqe_sgl {
+	dma_addr_t sgl_dma_addr;
+	struct sg_entry *sgl;
+	size_t sgl_size;	/* size of sgl */
+
+	void __user *user_addr; /* user-space base-address */
+	size_t user_size;       /* size of memory area */
+
+	unsigned long nr_pages;
+	unsigned long fpage_offs;
+	size_t fpage_size;
+	size_t lpage_size;
+
+	void *fpage;
+	dma_addr_t fpage_dma_addr;
+
+	void *lpage;
+	dma_addr_t lpage_dma_addr;
+};
+
+int genwqe_alloc_sync_sgl(struct genwqe_dev *cd, struct genwqe_sgl *sgl,
+			  void __user *user_addr, size_t user_size);
+
+int genwqe_setup_sgl(struct genwqe_dev *cd, struct genwqe_sgl *sgl,
+		     dma_addr_t *dma_list);
+
+int genwqe_free_sync_sgl(struct genwqe_dev *cd, struct genwqe_sgl *sgl);
+
+/**
  * struct ddcb_requ - Kernel internal representation of the DDCB request
  * @cmd:          User space representation of the DDCB execution request
  */
@@ -347,9 +385,7 @@ struct ddcb_requ {
 	struct ddcb_queue *queue;	  /* associated queue */
 
 	struct dma_mapping  dma_mappings[DDCB_FIXUPS];
-	struct sg_entry     *sgl[DDCB_FIXUPS];
-	dma_addr_t	    sgl_dma_addr[DDCB_FIXUPS];
-	size_t		    sgl_size[DDCB_FIXUPS];
+	struct genwqe_sgl sgls[DDCB_FIXUPS];
 
 	/* kernel/user shared content */
 	struct genwqe_ddcb_cmd cmd;	/* ddcb_no for this request */
@@ -453,22 +489,6 @@ int  genwqe_user_vmap(struct genwqe_dev *cd, struct dma_mapping *m,
 int  genwqe_user_vunmap(struct genwqe_dev *cd, struct dma_mapping *m,
 			struct ddcb_requ *req);
 
-struct sg_entry *genwqe_alloc_sgl(struct genwqe_dev *cd, int num_pages,
-				 dma_addr_t *dma_addr, size_t *sgl_size);
-
-void genwqe_free_sgl(struct genwqe_dev *cd, struct sg_entry *sg_list,
-		    dma_addr_t dma_addr, size_t size);
-
-int genwqe_setup_sgl(struct genwqe_dev *cd,
-		    unsigned long offs,
-		    unsigned long size,
-		    struct sg_entry *sgl, /* genwqe sgl */
-		    dma_addr_t dma_addr, size_t sgl_size,
-		    dma_addr_t *dma_list, int page_offs, int num_pages);
-
-int genwqe_check_sgl(struct genwqe_dev *cd, struct sg_entry *sg_list,
-		     int size);
-
 static inline bool dma_mapping_used(struct dma_mapping *m)
 {
 	if (!m)
diff --git a/drivers/misc/genwqe/card_ddcb.c b/drivers/misc/genwqe/card_ddcb.c
index 6f1acc0ccf8..c8046db2d5a 100644
--- a/drivers/misc/genwqe/card_ddcb.c
+++ b/drivers/misc/genwqe/card_ddcb.c
@@ -305,6 +305,8 @@ static int enqueue_ddcb(struct genwqe_dev *cd, struct ddcb_queue *queue,
 			break;
 
 		new = (old | DDCB_NEXT_BE32);
+
+		wmb();
 		icrc_hsi_shi = cmpxchg(&prev_ddcb->icrc_hsi_shi_32, old, new);
 
 		if (icrc_hsi_shi == old)
@@ -314,6 +316,8 @@ static int enqueue_ddcb(struct genwqe_dev *cd, struct ddcb_queue *queue,
 	/* Queue must be re-started by updating QUEUE_OFFSET */
 	ddcb_mark_tapped(pddcb);
 	num = (u64)ddcb_no << 8;
+
+	wmb();
 	__genwqe_writeq(cd, queue->IO_QUEUE_OFFSET, num); /* start queue */
 
 	return RET_DDCB_TAPPED;
@@ -1306,7 +1310,7 @@ static int queue_wake_up_all(struct genwqe_dev *cd)
  */
 int genwqe_finish_queue(struct genwqe_dev *cd)
 {
-	int i, rc, in_flight;
+	int i, rc = 0, in_flight;
 	int waitmax = genwqe_ddcb_software_timeout;
 	struct pci_dev *pci_dev = cd->pci_dev;
 	struct ddcb_queue *queue = &cd->queue;
diff --git a/drivers/misc/genwqe/card_debugfs.c b/drivers/misc/genwqe/card_debugfs.c
index 50d2096ea1c..0a33ade6410 100644
--- a/drivers/misc/genwqe/card_debugfs.c
+++ b/drivers/misc/genwqe/card_debugfs.c
@@ -348,7 +348,7 @@ int genwqe_init_debugfs(struct genwqe_dev *cd)
 	char name[64];
 	unsigned int i;
 
-	sprintf(card_name, "%s%u_card", GENWQE_DEVNAME, cd->card_idx);
+	sprintf(card_name, "%s%d_card", GENWQE_DEVNAME, cd->card_idx);
 
 	root = debugfs_create_dir(card_name, cd->debugfs_genwqe);
 	if (!root) {
@@ -454,7 +454,7 @@ int genwqe_init_debugfs(struct genwqe_dev *cd)
 	}
 
 	for (i = 0; i <  GENWQE_MAX_VFS; i++) {
-		sprintf(name, "vf%d_jobtimeout_msec", i);
+		sprintf(name, "vf%u_jobtimeout_msec", i);
 
 		file = debugfs_create_u32(name, 0666, root,
 					  &cd->vf_jobtimeout_msec[i]);
diff --git a/drivers/misc/genwqe/card_dev.c b/drivers/misc/genwqe/card_dev.c
index 2c2c9cc7523..1d2f163a190 100644
--- a/drivers/misc/genwqe/card_dev.c
+++ b/drivers/misc/genwqe/card_dev.c
@@ -531,7 +531,9 @@ static int do_flash_update(struct genwqe_file *cfile,
 	case '1':
 		cmdopts = 0x1C;
 		break;		/* download/erase_first/part_1 */
-	case 'v':		/* cmdopts = 0x0c (VPD) */
+	case 'v':
+		cmdopts = 0x0C;
+		break;		/* download/erase_first/vpd */
 	default:
 		return -EINVAL;
 	}
@@ -665,6 +667,8 @@ static int do_flash_read(struct genwqe_file *cfile,
 		cmdopts = 0x1A;
 		break;		/* upload/part_1 */
 	case 'v':
+		cmdopts = 0x0A;
+		break;		/* upload/vpd */
 	default:
 		return -EINVAL;
 	}
@@ -836,15 +840,8 @@ static int ddcb_cmd_cleanup(struct genwqe_file *cfile, struct ddcb_requ *req)
 			__genwqe_del_mapping(cfile, dma_map);
 			genwqe_user_vunmap(cd, dma_map, req);
 		}
-		if (req->sgl[i] != NULL) {
-			genwqe_free_sgl(cd, req->sgl[i],
-				       req->sgl_dma_addr[i],
-				       req->sgl_size[i]);
-			req->sgl[i] = NULL;
-			req->sgl_dma_addr[i] = 0x0;
-			req->sgl_size[i] = 0;
-		}
-
+		if (req->sgls[i].sgl != NULL)
+			genwqe_free_sync_sgl(cd, &req->sgls[i]);
 	}
 	return 0;
 }
@@ -913,7 +910,7 @@ static int ddcb_cmd_fixups(struct genwqe_file *cfile, struct ddcb_requ *req)
 
 		case ATS_TYPE_SGL_RDWR:
 		case ATS_TYPE_SGL_RD: {
-			int page_offs, nr_pages, offs;
+			int page_offs;
 
 			u_addr = be64_to_cpu(*((__be64 *)
 					       &cmd->asiv[asiv_offs]));
@@ -951,27 +948,18 @@ static int ddcb_cmd_fixups(struct genwqe_file *cfile, struct ddcb_requ *req)
 				page_offs = 0;
 			}
 
-			offs = offset_in_page(u_addr);
-			nr_pages = DIV_ROUND_UP(offs + u_size, PAGE_SIZE);
-
 			/* create genwqe style scatter gather list */
-			req->sgl[i] = genwqe_alloc_sgl(cd, m->nr_pages,
-						      &req->sgl_dma_addr[i],
-						      &req->sgl_size[i]);
-			if (req->sgl[i] == NULL) {
-				rc = -ENOMEM;
+			rc = genwqe_alloc_sync_sgl(cd, &req->sgls[i],
+						   (void __user *)u_addr,
+						   u_size);
+			if (rc != 0)
 				goto err_out;
-			}
-			genwqe_setup_sgl(cd, offs, u_size,
-					req->sgl[i],
-					req->sgl_dma_addr[i],
-					req->sgl_size[i],
-					m->dma_list,
-					page_offs,
-					nr_pages);
+
+			genwqe_setup_sgl(cd, &req->sgls[i],
+					 &m->dma_list[page_offs]);
 
 			*((__be64 *)&cmd->asiv[asiv_offs]) =
-				cpu_to_be64(req->sgl_dma_addr[i]);
+				cpu_to_be64(req->sgls[i].sgl_dma_addr);
 
 			break;
 		}
diff --git a/drivers/misc/genwqe/card_utils.c b/drivers/misc/genwqe/card_utils.c
index 6b1a6ef9f1a..62cc6bb3f62 100644
--- a/drivers/misc/genwqe/card_utils.c
+++ b/drivers/misc/genwqe/card_utils.c
@@ -275,67 +275,107 @@ static int genwqe_sgl_size(int num_pages)
 	return roundup(len, PAGE_SIZE);
 }
 
-struct sg_entry *genwqe_alloc_sgl(struct genwqe_dev *cd, int num_pages,
-				  dma_addr_t *dma_addr, size_t *sgl_size)
+/**
+ * genwqe_alloc_sync_sgl() - Allocate memory for sgl and overlapping pages
+ *
+ * Allocates memory for sgl and overlapping pages. Pages which might
+ * overlap other user-space memory blocks are being cached for DMAs,
+ * such that we do not run into syncronization issues. Data is copied
+ * from user-space into the cached pages.
+ */
+int genwqe_alloc_sync_sgl(struct genwqe_dev *cd, struct genwqe_sgl *sgl,
+			  void __user *user_addr, size_t user_size)
 {
+	int rc;
 	struct pci_dev *pci_dev = cd->pci_dev;
-	struct sg_entry *sgl;
 
-	*sgl_size = genwqe_sgl_size(num_pages);
-	if (get_order(*sgl_size) > MAX_ORDER) {
+	sgl->fpage_offs = offset_in_page((unsigned long)user_addr);
+	sgl->fpage_size = min_t(size_t, PAGE_SIZE-sgl->fpage_offs, user_size);
+	sgl->nr_pages = DIV_ROUND_UP(sgl->fpage_offs + user_size, PAGE_SIZE);
+	sgl->lpage_size = (user_size - sgl->fpage_size) % PAGE_SIZE;
+
+	dev_dbg(&pci_dev->dev, "[%s] uaddr=%p usize=%8ld nr_pages=%ld "
+		"fpage_offs=%lx fpage_size=%ld lpage_size=%ld\n",
+		__func__, user_addr, user_size, sgl->nr_pages,
+		sgl->fpage_offs, sgl->fpage_size, sgl->lpage_size);
+
+	sgl->user_addr = user_addr;
+	sgl->user_size = user_size;
+	sgl->sgl_size = genwqe_sgl_size(sgl->nr_pages);
+
+	if (get_order(sgl->sgl_size) > MAX_ORDER) {
 		dev_err(&pci_dev->dev,
 			"[%s] err: too much memory requested!\n", __func__);
-		return NULL;
+		return -ENOMEM;
 	}
 
-	sgl = __genwqe_alloc_consistent(cd, *sgl_size, dma_addr);
-	if (sgl == NULL) {
+	sgl->sgl = __genwqe_alloc_consistent(cd, sgl->sgl_size,
+					     &sgl->sgl_dma_addr);
+	if (sgl->sgl == NULL) {
 		dev_err(&pci_dev->dev,
 			"[%s] err: no memory available!\n", __func__);
-		return NULL;
+		return -ENOMEM;
 	}
 
-	return sgl;
+	/* Only use buffering on incomplete pages */
+	if ((sgl->fpage_size != 0) && (sgl->fpage_size != PAGE_SIZE)) {
+		sgl->fpage = __genwqe_alloc_consistent(cd, PAGE_SIZE,
+						       &sgl->fpage_dma_addr);
+		if (sgl->fpage == NULL)
+			goto err_out;
+
+		/* Sync with user memory */
+		if (copy_from_user(sgl->fpage + sgl->fpage_offs,
+				   user_addr, sgl->fpage_size)) {
+			rc = -EFAULT;
+			goto err_out;
+		}
+	}
+	if (sgl->lpage_size != 0) {
+		sgl->lpage = __genwqe_alloc_consistent(cd, PAGE_SIZE,
+						       &sgl->lpage_dma_addr);
+		if (sgl->lpage == NULL)
+			goto err_out1;
+
+		/* Sync with user memory */
+		if (copy_from_user(sgl->lpage, user_addr + user_size -
+				   sgl->lpage_size, sgl->lpage_size)) {
+			rc = -EFAULT;
+			goto err_out1;
+		}
+	}
+	return 0;
+
+ err_out1:
+	__genwqe_free_consistent(cd, PAGE_SIZE, sgl->fpage,
+				 sgl->fpage_dma_addr);
+ err_out:
+	__genwqe_free_consistent(cd, sgl->sgl_size, sgl->sgl,
+				 sgl->sgl_dma_addr);
+	return -ENOMEM;
 }
 
-int genwqe_setup_sgl(struct genwqe_dev *cd,
-		     unsigned long offs,
-		     unsigned long size,
-		     struct sg_entry *sgl,
-		     dma_addr_t dma_addr, size_t sgl_size,
-		     dma_addr_t *dma_list, int page_offs, int num_pages)
+int genwqe_setup_sgl(struct genwqe_dev *cd, struct genwqe_sgl *sgl,
+		     dma_addr_t *dma_list)
 {
 	int i = 0, j = 0, p;
 	unsigned long dma_offs, map_offs;
-	struct pci_dev *pci_dev = cd->pci_dev;
 	dma_addr_t prev_daddr = 0;
 	struct sg_entry *s, *last_s = NULL;
-
-	/* sanity checks */
-	if (offs > PAGE_SIZE) {
-		dev_err(&pci_dev->dev,
-			"[%s] too large start offs %08lx\n", __func__, offs);
-		return -EFAULT;
-	}
-	if (sgl_size < genwqe_sgl_size(num_pages)) {
-		dev_err(&pci_dev->dev,
-			"[%s] sgl_size too small %08lx for %d pages\n",
-			__func__, sgl_size, num_pages);
-		return -EFAULT;
-	}
+	size_t size = sgl->user_size;
 
 	dma_offs = 128;		/* next block if needed/dma_offset */
-	map_offs = offs;	/* offset in first page */
+	map_offs = sgl->fpage_offs; /* offset in first page */
 
-	s = &sgl[0];		/* first set of 8 entries */
+	s = &sgl->sgl[0];	/* first set of 8 entries */
 	p = 0;			/* page */
-	while (p < num_pages) {
+	while (p < sgl->nr_pages) {
 		dma_addr_t daddr;
 		unsigned int size_to_map;
 
 		/* always write the chaining entry, cleanup is done later */
 		j = 0;
-		s[j].target_addr = cpu_to_be64(dma_addr + dma_offs);
+		s[j].target_addr = cpu_to_be64(sgl->sgl_dma_addr + dma_offs);
 		s[j].len	 = cpu_to_be32(128);
 		s[j].flags	 = cpu_to_be32(SG_CHAINED);
 		j++;
@@ -343,7 +383,17 @@ int genwqe_setup_sgl(struct genwqe_dev *cd,
 		while (j < 8) {
 			/* DMA mapping for requested page, offs, size */
 			size_to_map = min(size, PAGE_SIZE - map_offs);
-			daddr = dma_list[page_offs + p] + map_offs;
+
+			if ((p == 0) && (sgl->fpage != NULL)) {
+				daddr = sgl->fpage_dma_addr + map_offs;
+
+			} else if ((p == sgl->nr_pages - 1) &&
+				   (sgl->lpage != NULL)) {
+				daddr = sgl->lpage_dma_addr;
+			} else {
+				daddr = dma_list[p] + map_offs;
+			}
+
 			size -= size_to_map;
 			map_offs = 0;
 
@@ -358,7 +408,7 @@ int genwqe_setup_sgl(struct genwqe_dev *cd,
 							  size_to_map);
 
 				p++; /* process next page */
-				if (p == num_pages)
+				if (p == sgl->nr_pages)
 					goto fixup;  /* nothing to do */
 
 				prev_daddr = daddr + size_to_map;
@@ -374,7 +424,7 @@ int genwqe_setup_sgl(struct genwqe_dev *cd,
 			j++;
 
 			p++;	/* process next page */
-			if (p == num_pages)
+			if (p == sgl->nr_pages)
 				goto fixup;  /* nothing to do */
 		}
 		dma_offs += 128;
@@ -395,10 +445,50 @@ int genwqe_setup_sgl(struct genwqe_dev *cd,
 	return 0;
 }
 
-void genwqe_free_sgl(struct genwqe_dev *cd, struct sg_entry *sg_list,
-		    dma_addr_t dma_addr, size_t size)
+/**
+ * genwqe_free_sync_sgl() - Free memory for sgl and overlapping pages
+ *
+ * After the DMA transfer has been completed we free the memory for
+ * the sgl and the cached pages. Data is being transfered from cached
+ * pages into user-space buffers.
+ */
+int genwqe_free_sync_sgl(struct genwqe_dev *cd, struct genwqe_sgl *sgl)
 {
-	__genwqe_free_consistent(cd, size, sg_list, dma_addr);
+	int rc = 0;
+	struct pci_dev *pci_dev = cd->pci_dev;
+
+	if (sgl->fpage) {
+		if (copy_to_user(sgl->user_addr, sgl->fpage + sgl->fpage_offs,
+				 sgl->fpage_size)) {
+			dev_err(&pci_dev->dev, "[%s] err: copying fpage!\n",
+				__func__);
+			rc = -EFAULT;
+		}
+		__genwqe_free_consistent(cd, PAGE_SIZE, sgl->fpage,
+					 sgl->fpage_dma_addr);
+		sgl->fpage = NULL;
+		sgl->fpage_dma_addr = 0;
+	}
+	if (sgl->lpage) {
+		if (copy_to_user(sgl->user_addr + sgl->user_size -
+				 sgl->lpage_size, sgl->lpage,
+				 sgl->lpage_size)) {
+			dev_err(&pci_dev->dev, "[%s] err: copying lpage!\n",
+				__func__);
+			rc = -EFAULT;
+		}
+		__genwqe_free_consistent(cd, PAGE_SIZE, sgl->lpage,
+					 sgl->lpage_dma_addr);
+		sgl->lpage = NULL;
+		sgl->lpage_dma_addr = 0;
+	}
+	__genwqe_free_consistent(cd, sgl->sgl_size, sgl->sgl,
+				 sgl->sgl_dma_addr);
+
+	sgl->sgl = NULL;
+	sgl->sgl_dma_addr = 0x0;
+	sgl->sgl_size = 0;
+	return rc;
 }
 
 /**
@@ -628,7 +718,7 @@ int genwqe_set_interrupt_capability(struct genwqe_dev *cd, int count)
 	int rc;
 	struct pci_dev *pci_dev = cd->pci_dev;
 
-	rc = pci_enable_msi_block(pci_dev, count);
+	rc = pci_enable_msi_exact(pci_dev, count);
 	if (rc == 0)
 		cd->flags |= GENWQE_FLAG_MSI_ENABLED;
 	return rc;
diff --git a/drivers/misc/genwqe/genwqe_driver.h b/drivers/misc/genwqe/genwqe_driver.h
index 46e916b36c7..cd5263163a6 100644
--- a/drivers/misc/genwqe/genwqe_driver.h
+++ b/drivers/misc/genwqe/genwqe_driver.h
@@ -36,7 +36,7 @@
 #include <asm/byteorder.h>
 #include <linux/genwqe/genwqe_card.h>
 
-#define DRV_VERS_STRING		"2.0.0"
+#define DRV_VERS_STRING		"2.0.15"
 
 /*
  * Static minor number assignement, until we decide/implement
diff --git a/drivers/misc/mei/amthif.c b/drivers/misc/mei/amthif.c
index b8deb345548..0d6234db00f 100644
--- a/drivers/misc/mei/amthif.c
+++ b/drivers/misc/mei/amthif.c
@@ -111,8 +111,6 @@ int mei_amthif_host_init(struct mei_device *dev)
 		return ret;
 	}
 
-	cl->state = MEI_FILE_CONNECTING;
-
 	ret = mei_cl_connect(cl, NULL);
 
 	dev->iamthif_state = MEI_IAMTHIF_IDLE;
diff --git a/drivers/misc/mei/bus.c b/drivers/misc/mei/bus.c
index ddc5ac92a20..0e993ef28b9 100644
--- a/drivers/misc/mei/bus.c
+++ b/drivers/misc/mei/bus.c
@@ -247,7 +247,7 @@ static int ___mei_cl_send(struct mei_cl *cl, u8 *buf, size_t length,
 		return id;
 
 	if (length > dev->me_clients[id].props.max_msg_length)
-		return -EINVAL;
+		return -EFBIG;
 
 	cb = mei_io_cb_init(cl, NULL);
 	if (!cb)
@@ -427,8 +427,6 @@ int mei_cl_enable_device(struct mei_cl_device *device)
 
 	mutex_lock(&dev->device_lock);
 
-	cl->state = MEI_FILE_CONNECTING;
-
 	err = mei_cl_connect(cl, NULL);
 	if (err < 0) {
 		mutex_unlock(&dev->device_lock);
diff --git a/drivers/misc/mei/client.c b/drivers/misc/mei/client.c
index 8c078b808cd..59d20c599b1 100644
--- a/drivers/misc/mei/client.c
+++ b/drivers/misc/mei/client.c
@@ -18,6 +18,7 @@
 #include <linux/sched.h>
 #include <linux/wait.h>
 #include <linux/delay.h>
+#include <linux/pm_runtime.h>
 
 #include <linux/mei.h>
 
@@ -415,6 +416,10 @@ void mei_host_client_init(struct work_struct *work)
 	dev->reset_count = 0;
 
 	mutex_unlock(&dev->device_lock);
+
+	pm_runtime_mark_last_busy(&dev->pdev->dev);
+	dev_dbg(&dev->pdev->dev, "rpm: autosuspend\n");
+	pm_runtime_autosuspend(&dev->pdev->dev);
 }
 
 /**
@@ -425,6 +430,12 @@ void mei_host_client_init(struct work_struct *work)
  */
 bool mei_hbuf_acquire(struct mei_device *dev)
 {
+	if (mei_pg_state(dev) == MEI_PG_ON ||
+	    dev->pg_event == MEI_PG_EVENT_WAIT) {
+		dev_dbg(&dev->pdev->dev, "device is in pg\n");
+		return false;
+	}
+
 	if (!dev->hbuf_is_ready) {
 		dev_dbg(&dev->pdev->dev, "hbuf is not ready\n");
 		return false;
@@ -460,9 +471,18 @@ int mei_cl_disconnect(struct mei_cl *cl)
 	if (cl->state != MEI_FILE_DISCONNECTING)
 		return 0;
 
+	rets = pm_runtime_get(&dev->pdev->dev);
+	if (rets < 0 && rets != -EINPROGRESS) {
+		pm_runtime_put_noidle(&dev->pdev->dev);
+		cl_err(dev, cl, "rpm: get failed %d\n", rets);
+		return rets;
+	}
+
 	cb = mei_io_cb_init(cl, NULL);
-	if (!cb)
-		return -ENOMEM;
+	if (!cb) {
+		rets = -ENOMEM;
+		goto free;
+	}
 
 	cb->fop_type = MEI_FOP_CLOSE;
 	if (mei_hbuf_acquire(dev)) {
@@ -494,8 +514,7 @@ int mei_cl_disconnect(struct mei_cl *cl)
 			cl_err(dev, cl, "wrong status client disconnect.\n");
 
 		if (err)
-			cl_dbg(dev, cl, "wait failed disconnect err=%08x\n",
-					err);
+			cl_dbg(dev, cl, "wait failed disconnect err=%d\n", err);
 
 		cl_err(dev, cl, "failed to disconnect from FW client.\n");
 	}
@@ -503,6 +522,10 @@ int mei_cl_disconnect(struct mei_cl *cl)
 	mei_io_list_flush(&dev->ctrl_rd_list, cl);
 	mei_io_list_flush(&dev->ctrl_wr_list, cl);
 free:
+	cl_dbg(dev, cl, "rpm: autosuspend\n");
+	pm_runtime_mark_last_busy(&dev->pdev->dev);
+	pm_runtime_put_autosuspend(&dev->pdev->dev);
+
 	mei_io_cb_free(cb);
 	return rets;
 }
@@ -557,6 +580,13 @@ int mei_cl_connect(struct mei_cl *cl, struct file *file)
 
 	dev = cl->dev;
 
+	rets = pm_runtime_get(&dev->pdev->dev);
+	if (rets < 0 && rets != -EINPROGRESS) {
+		pm_runtime_put_noidle(&dev->pdev->dev);
+		cl_err(dev, cl, "rpm: get failed %d\n", rets);
+		return rets;
+	}
+
 	cb = mei_io_cb_init(cl, file);
 	if (!cb) {
 		rets = -ENOMEM;
@@ -567,6 +597,7 @@ int mei_cl_connect(struct mei_cl *cl, struct file *file)
 
 	/* run hbuf acquire last so we don't have to undo */
 	if (!mei_cl_is_other_connecting(cl) && mei_hbuf_acquire(dev)) {
+		cl->state = MEI_FILE_CONNECTING;
 		if (mei_hbm_cl_connect_req(dev, cl)) {
 			rets = -ENODEV;
 			goto out;
@@ -596,6 +627,10 @@ int mei_cl_connect(struct mei_cl *cl, struct file *file)
 	rets = cl->status;
 
 out:
+	cl_dbg(dev, cl, "rpm: autosuspend\n");
+	pm_runtime_mark_last_busy(&dev->pdev->dev);
+	pm_runtime_put_autosuspend(&dev->pdev->dev);
+
 	mei_io_cb_free(cb);
 	return rets;
 }
@@ -713,23 +748,31 @@ int mei_cl_read_start(struct mei_cl *cl, size_t length)
 		return  -ENOTTY;
 	}
 
+	rets = pm_runtime_get(&dev->pdev->dev);
+	if (rets < 0 && rets != -EINPROGRESS) {
+		pm_runtime_put_noidle(&dev->pdev->dev);
+		cl_err(dev, cl, "rpm: get failed %d\n", rets);
+		return rets;
+	}
+
 	cb = mei_io_cb_init(cl, NULL);
-	if (!cb)
-		return -ENOMEM;
+	if (!cb) {
+		rets = -ENOMEM;
+		goto out;
+	}
 
 	/* 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;
+		goto out;
 
 	cb->fop_type = MEI_FOP_READ;
 	if (mei_hbuf_acquire(dev)) {
-		if (mei_hbm_cl_flow_control_req(dev, cl)) {
-			cl_err(dev, cl, "flow control send failed\n");
-			rets = -ENODEV;
-			goto err;
-		}
+		rets = mei_hbm_cl_flow_control_req(dev, cl);
+		if (rets < 0)
+			goto out;
+
 		list_add_tail(&cb->list, &dev->read_list.list);
 	} else {
 		list_add_tail(&cb->list, &dev->ctrl_wr_list.list);
@@ -737,9 +780,14 @@ int mei_cl_read_start(struct mei_cl *cl, size_t length)
 
 	cl->read_cb = cb;
 
-	return rets;
-err:
-	mei_io_cb_free(cb);
+out:
+	cl_dbg(dev, cl, "rpm: autosuspend\n");
+	pm_runtime_mark_last_busy(&dev->pdev->dev);
+	pm_runtime_put_autosuspend(&dev->pdev->dev);
+
+	if (rets)
+		mei_io_cb_free(cb);
+
 	return rets;
 }
 
@@ -776,7 +824,7 @@ int mei_cl_irq_write(struct mei_cl *cl, struct mei_cl_cb *cb,
 		return rets;
 
 	if (rets == 0) {
-		cl_dbg(dev, cl,	"No flow control credentials: not sending.\n");
+		cl_dbg(dev, cl, "No flow control credentials: not sending.\n");
 		return 0;
 	}
 
@@ -856,6 +904,12 @@ int mei_cl_write(struct mei_cl *cl, struct mei_cl_cb *cb, bool blocking)
 
 	cl_dbg(dev, cl, "mei_cl_write %d\n", buf->size);
 
+	rets = pm_runtime_get(&dev->pdev->dev);
+	if (rets < 0 && rets != -EINPROGRESS) {
+		pm_runtime_put_noidle(&dev->pdev->dev);
+		cl_err(dev, cl, "rpm: get failed %d\n", rets);
+		return rets;
+	}
 
 	cb->fop_type = MEI_FOP_WRITE;
 	cb->buf_idx = 0;
@@ -926,6 +980,10 @@ out:
 
 	rets = buf->size;
 err:
+	cl_dbg(dev, cl, "rpm: autosuspend\n");
+	pm_runtime_mark_last_busy(&dev->pdev->dev);
+	pm_runtime_put_autosuspend(&dev->pdev->dev);
+
 	return rets;
 }
 
diff --git a/drivers/misc/mei/hbm.c b/drivers/misc/mei/hbm.c
index 4960288e543..804106209d7 100644
--- a/drivers/misc/mei/hbm.c
+++ b/drivers/misc/mei/hbm.c
@@ -14,10 +14,12 @@
  *
  */
 
+#include <linux/export.h>
 #include <linux/pci.h>
 #include <linux/sched.h>
 #include <linux/wait.h>
 #include <linux/mei.h>
+#include <linux/pm_runtime.h>
 
 #include "mei_dev.h"
 #include "hbm.h"
@@ -58,6 +60,34 @@ static int mei_cl_conn_status_to_errno(enum mei_cl_connect_status status)
 }
 
 /**
+ * mei_hbm_idle - set hbm to idle state
+ *
+ * @dev: the device structure
+ */
+void mei_hbm_idle(struct mei_device *dev)
+{
+	dev->init_clients_timer = 0;
+	dev->hbm_state = MEI_HBM_IDLE;
+}
+
+/**
+ * mei_hbm_reset - reset hbm counters and book keeping data structurs
+ *
+ * @dev: the device structure
+ */
+void mei_hbm_reset(struct mei_device *dev)
+{
+	dev->me_clients_num = 0;
+	dev->me_client_presentation_num = 0;
+	dev->me_client_index = 0;
+
+	kfree(dev->me_clients);
+	dev->me_clients = NULL;
+
+	mei_hbm_idle(dev);
+}
+
+/**
  * mei_hbm_me_cl_allocate - allocates storage for me clients
  *
  * @dev: the device structure
@@ -69,9 +99,7 @@ static int mei_hbm_me_cl_allocate(struct mei_device *dev)
 	struct mei_me_client *clients;
 	int b;
 
-	dev->me_clients_num = 0;
-	dev->me_client_presentation_num = 0;
-	dev->me_client_index = 0;
+	mei_hbm_reset(dev);
 
 	/* count how many ME clients we have */
 	for_each_set_bit(b, dev->me_clients_map, MEI_CLIENTS_MAX)
@@ -80,9 +108,6 @@ static int mei_hbm_me_cl_allocate(struct mei_device *dev)
 	if (dev->me_clients_num == 0)
 		return 0;
 
-	kfree(dev->me_clients);
-	dev->me_clients = NULL;
-
 	dev_dbg(&dev->pdev->dev, "memory allocation for ME clients size=%ld.\n",
 		dev->me_clients_num * sizeof(struct mei_me_client));
 	/* allocate storage for ME clients representation */
@@ -133,17 +158,6 @@ bool mei_hbm_cl_addr_equal(struct mei_cl *cl, void *buf)
 }
 
 
-/**
- * mei_hbm_idle - set hbm to idle state
- *
- * @dev: the device structure
- */
-void mei_hbm_idle(struct mei_device *dev)
-{
-	dev->init_clients_timer = 0;
-	dev->hbm_state = MEI_HBM_IDLE;
-}
-
 int mei_hbm_start_wait(struct mei_device *dev)
 {
 	int ret;
@@ -289,6 +303,34 @@ static int mei_hbm_prop_req(struct mei_device *dev)
 	return 0;
 }
 
+/*
+ * mei_hbm_pg - sends pg command
+ *
+ * @dev: the device structure
+ * @pg_cmd: the pg command code
+ *
+ * This function returns -EIO on write failure
+ */
+int mei_hbm_pg(struct mei_device *dev, u8 pg_cmd)
+{
+	struct mei_msg_hdr *mei_hdr = &dev->wr_msg.hdr;
+	struct hbm_power_gate *req;
+	const size_t len = sizeof(struct hbm_power_gate);
+	int ret;
+
+	mei_hbm_hdr(mei_hdr, len);
+
+	req = (struct hbm_power_gate *)dev->wr_msg.data;
+	memset(req, 0, len);
+	req->hbm_cmd = pg_cmd;
+
+	ret = mei_write_message(dev, mei_hdr, dev->wr_msg.data);
+	if (ret)
+		dev_err(&dev->pdev->dev, "power gate command write failed.\n");
+	return ret;
+}
+EXPORT_SYMBOL_GPL(mei_hbm_pg);
+
 /**
  * mei_hbm_stop_req - send stop request message
  *
@@ -701,6 +743,27 @@ int mei_hbm_dispatch(struct mei_device *dev, struct mei_msg_hdr *hdr)
 		mei_hbm_cl_flow_control_res(dev, flow_control);
 		break;
 
+	case MEI_PG_ISOLATION_ENTRY_RES_CMD:
+		dev_dbg(&dev->pdev->dev, "power gate isolation entry response received\n");
+		dev->pg_event = MEI_PG_EVENT_RECEIVED;
+		if (waitqueue_active(&dev->wait_pg))
+			wake_up(&dev->wait_pg);
+		break;
+
+	case MEI_PG_ISOLATION_EXIT_REQ_CMD:
+		dev_dbg(&dev->pdev->dev, "power gate isolation exit request received\n");
+		dev->pg_event = MEI_PG_EVENT_RECEIVED;
+		if (waitqueue_active(&dev->wait_pg))
+			wake_up(&dev->wait_pg);
+		else
+			/*
+			* If the driver is not waiting on this then
+			* this is HW initiated exit from PG.
+			* Start runtime pm resume sequence to exit from PG.
+			*/
+			pm_request_resume(&dev->pdev->dev);
+		break;
+
 	case HOST_CLIENT_PROPERTIES_RES_CMD:
 		dev_dbg(&dev->pdev->dev, "hbm: properties response: message received.\n");
 
diff --git a/drivers/misc/mei/hbm.h b/drivers/misc/mei/hbm.h
index 20e8782711c..683eb2835ce 100644
--- a/drivers/misc/mei/hbm.h
+++ b/drivers/misc/mei/hbm.h
@@ -50,6 +50,7 @@ static inline void mei_hbm_hdr(struct mei_msg_hdr *hdr, size_t length)
 }
 
 void mei_hbm_idle(struct mei_device *dev);
+void mei_hbm_reset(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);
@@ -57,6 +58,7 @@ int mei_hbm_cl_disconnect_req(struct mei_device *dev, struct mei_cl *cl);
 int mei_hbm_cl_disconnect_rsp(struct mei_device *dev, struct mei_cl *cl);
 int mei_hbm_cl_connect_req(struct mei_device *dev, struct mei_cl *cl);
 bool mei_hbm_version_is_supported(struct mei_device *dev);
+int mei_hbm_pg(struct mei_device *dev, u8 pg_cmd);
 
 #endif /* _MEI_HBM_H_ */
 
diff --git a/drivers/misc/mei/hw-me-regs.h b/drivers/misc/mei/hw-me-regs.h
index 66f411a6e8e..a7856c0ac57 100644
--- a/drivers/misc/mei/hw-me-regs.h
+++ b/drivers/misc/mei/hw-me-regs.h
@@ -115,6 +115,11 @@
 #define MEI_DEV_ID_LPT_HR     0x8CBA  /* Lynx Point H Refresh */
 
 #define MEI_DEV_ID_WPT_LP     0x9CBA  /* Wildcat Point LP */
+
+/* Host Firmware Status Registers in PCI Config Space */
+#define PCI_CFG_HFS_1         0x40
+#define PCI_CFG_HFS_2         0x48
+
 /*
  * MEI HW Section
  */
@@ -128,6 +133,8 @@
 #define ME_CB_RW   8
 /* ME_CSR_HA - ME Control Status Host Access register (read only) */
 #define ME_CSR_HA  0xC
+/* H_HGC_CSR - PGI register */
+#define H_HPG_CSR  0x10
 
 
 /* register bits of H_CSR (Host Control Status register) */
@@ -157,6 +164,8 @@ access to ME_CBD */
 #define ME_CBWP_HRA       0x00FF0000
 /* ME CB Read Pointer HRA - host read only access to ME_CBRP */
 #define ME_CBRP_HRA       0x0000FF00
+/* ME Power Gate Isolation Capability HRA  - host ready only access */
+#define ME_PGIC_HRA       0x00000040
 /* ME Reset HRA - host read only access to ME_RST */
 #define ME_RST_HRA        0x00000010
 /* ME Ready HRA - host read only access to ME_RDY */
@@ -168,4 +177,9 @@ access to ME_CBD */
 /* ME Interrupt Enable HRA - host read only access to ME_IE */
 #define ME_IE_HRA         0x00000001
 
+
+/* register bits - H_HPG_CSR */
+#define H_HPG_CSR_PGIHEXR       0x00000001
+#define H_HPG_CSR_PGI           0x00000002
+
 #endif /* _MEI_HW_MEI_REGS_H_ */
diff --git a/drivers/misc/mei/hw-me.c b/drivers/misc/mei/hw-me.c
index 8dbdaaef1af..6a2d272cea4 100644
--- a/drivers/misc/mei/hw-me.c
+++ b/drivers/misc/mei/hw-me.c
@@ -109,10 +109,27 @@ static inline void mei_hcsr_set(struct mei_me_hw *hw, u32 hcsr)
  */
 static void mei_me_hw_config(struct mei_device *dev)
 {
+	struct mei_me_hw *hw = to_me_hw(dev);
 	u32 hcsr = mei_hcsr_read(to_me_hw(dev));
 	/* Doesn't change in runtime */
 	dev->hbuf_depth = (hcsr & H_CBD) >> 24;
+
+	hw->pg_state = MEI_PG_OFF;
+}
+
+/**
+ * mei_me_pg_state  - translate internal pg state
+ *   to the mei power gating state
+ *
+ * @hw -  me hardware
+ * returns: MEI_PG_OFF if aliveness is on and MEI_PG_ON otherwise
+ */
+static inline enum mei_pg_state mei_me_pg_state(struct mei_device *dev)
+{
+	struct mei_me_hw *hw = to_me_hw(dev);
+	return hw->pg_state;
 }
+
 /**
  * mei_clear_interrupts - clear and stop interrupts
  *
@@ -164,6 +181,9 @@ static void mei_me_hw_reset_release(struct mei_device *dev)
 	hcsr |= H_IG;
 	hcsr &= ~H_RST;
 	mei_hcsr_set(hw, hcsr);
+
+	/* complete this write before we set host ready on another CPU */
+	mmiowb();
 }
 /**
  * mei_me_hw_reset - resets fw via mei csr register.
@@ -183,8 +203,21 @@ static int mei_me_hw_reset(struct mei_device *dev, bool intr_enable)
 	else
 		hcsr &= ~H_IE;
 
+	dev->recvd_hw_ready = false;
 	mei_me_reg_write(hw, H_CSR, hcsr);
 
+	/*
+	 * Host reads the H_CSR once to ensure that the
+	 * posted write to H_CSR completes.
+	 */
+	hcsr = mei_hcsr_read(hw);
+
+	if ((hcsr & H_RST) == 0)
+		dev_warn(&dev->pdev->dev, "H_RST is not set = 0x%08X", hcsr);
+
+	if ((hcsr & H_RDY) == H_RDY)
+		dev_warn(&dev->pdev->dev, "H_RDY is not cleared 0x%08X", hcsr);
+
 	if (intr_enable == false)
 		mei_me_hw_reset_release(dev);
 
@@ -201,6 +234,7 @@ static int mei_me_hw_reset(struct mei_device *dev, bool intr_enable)
 static void mei_me_host_set_ready(struct mei_device *dev)
 {
 	struct mei_me_hw *hw = to_me_hw(dev);
+	hw->host_hw_state = mei_hcsr_read(hw);
 	hw->host_hw_state |= H_IE | H_IG | H_RDY;
 	mei_hcsr_set(hw, hw->host_hw_state);
 }
@@ -233,10 +267,7 @@ static bool mei_me_hw_is_ready(struct mei_device *dev)
 static int mei_me_hw_ready_wait(struct mei_device *dev)
 {
 	int err;
-	if (mei_me_hw_is_ready(dev))
-		return 0;
 
-	dev->recvd_hw_ready = false;
 	mutex_unlock(&dev->device_lock);
 	err = wait_event_interruptible_timeout(dev->wait_hw_ready,
 			dev->recvd_hw_ready,
@@ -431,6 +462,144 @@ static int mei_me_read_slots(struct mei_device *dev, unsigned char *buffer,
 }
 
 /**
+ * mei_me_pg_enter - write pg enter register to mei device.
+ *
+ * @dev: the device structure
+ */
+static void mei_me_pg_enter(struct mei_device *dev)
+{
+	struct mei_me_hw *hw = to_me_hw(dev);
+	u32 reg = mei_me_reg_read(hw, H_HPG_CSR);
+	reg |= H_HPG_CSR_PGI;
+	mei_me_reg_write(hw, H_HPG_CSR, reg);
+}
+
+/**
+ * mei_me_pg_enter - write pg enter register to mei device.
+ *
+ * @dev: the device structure
+ */
+static void mei_me_pg_exit(struct mei_device *dev)
+{
+	struct mei_me_hw *hw = to_me_hw(dev);
+	u32 reg = mei_me_reg_read(hw, H_HPG_CSR);
+
+	WARN(!(reg & H_HPG_CSR_PGI), "PGI is not set\n");
+
+	reg |= H_HPG_CSR_PGIHEXR;
+	mei_me_reg_write(hw, H_HPG_CSR, reg);
+}
+
+/**
+ * mei_me_pg_set_sync - perform pg entry procedure
+ *
+ * @dev: the device structure
+ *
+ * returns 0 on success an error code otherwise
+ */
+int mei_me_pg_set_sync(struct mei_device *dev)
+{
+	struct mei_me_hw *hw = to_me_hw(dev);
+	unsigned long timeout = mei_secs_to_jiffies(MEI_PGI_TIMEOUT);
+	int ret;
+
+	dev->pg_event = MEI_PG_EVENT_WAIT;
+
+	ret = mei_hbm_pg(dev, MEI_PG_ISOLATION_ENTRY_REQ_CMD);
+	if (ret)
+		return ret;
+
+	mutex_unlock(&dev->device_lock);
+	wait_event_timeout(dev->wait_pg,
+		dev->pg_event == MEI_PG_EVENT_RECEIVED, timeout);
+	mutex_lock(&dev->device_lock);
+
+	if (dev->pg_event == MEI_PG_EVENT_RECEIVED) {
+		mei_me_pg_enter(dev);
+		ret = 0;
+	} else {
+		ret = -ETIME;
+	}
+
+	dev->pg_event = MEI_PG_EVENT_IDLE;
+	hw->pg_state = MEI_PG_ON;
+
+	return ret;
+}
+
+/**
+ * mei_me_pg_unset_sync - perform pg exit procedure
+ *
+ * @dev: the device structure
+ *
+ * returns 0 on success an error code otherwise
+ */
+int mei_me_pg_unset_sync(struct mei_device *dev)
+{
+	struct mei_me_hw *hw = to_me_hw(dev);
+	unsigned long timeout = mei_secs_to_jiffies(MEI_PGI_TIMEOUT);
+	int ret;
+
+	if (dev->pg_event == MEI_PG_EVENT_RECEIVED)
+		goto reply;
+
+	dev->pg_event = MEI_PG_EVENT_WAIT;
+
+	mei_me_pg_exit(dev);
+
+	mutex_unlock(&dev->device_lock);
+	wait_event_timeout(dev->wait_pg,
+		dev->pg_event == MEI_PG_EVENT_RECEIVED, timeout);
+	mutex_lock(&dev->device_lock);
+
+reply:
+	if (dev->pg_event == MEI_PG_EVENT_RECEIVED)
+		ret = mei_hbm_pg(dev, MEI_PG_ISOLATION_EXIT_RES_CMD);
+	else
+		ret = -ETIME;
+
+	dev->pg_event = MEI_PG_EVENT_IDLE;
+	hw->pg_state = MEI_PG_OFF;
+
+	return ret;
+}
+
+/**
+ * mei_me_pg_is_enabled - detect if PG is supported by HW
+ *
+ * @dev: the device structure
+ *
+ * returns: true is pg supported, false otherwise
+ */
+static bool mei_me_pg_is_enabled(struct mei_device *dev)
+{
+	struct mei_me_hw *hw = to_me_hw(dev);
+	u32 reg = mei_me_reg_read(hw, ME_CSR_HA);
+
+	if ((reg & ME_PGIC_HRA) == 0)
+		goto notsupported;
+
+	if (dev->version.major_version < HBM_MAJOR_VERSION_PGI)
+		goto notsupported;
+
+	if (dev->version.major_version == HBM_MAJOR_VERSION_PGI &&
+	    dev->version.minor_version < HBM_MINOR_VERSION_PGI)
+		goto notsupported;
+
+	return true;
+
+notsupported:
+	dev_dbg(&dev->pdev->dev, "pg: not supported: HGP = %d hbm version %d.%d ?= %d.%d\n",
+		!!(reg & ME_PGIC_HRA),
+		dev->version.major_version,
+		dev->version.minor_version,
+		HBM_MAJOR_VERSION_PGI,
+		HBM_MINOR_VERSION_PGI);
+
+	return false;
+}
+
+/**
  * mei_me_irq_quick_handler - The ISR of the MEI device
  *
  * @irq: The irq number
@@ -491,14 +660,13 @@ irqreturn_t mei_me_irq_thread_handler(int irq, void *dev_id)
 	/*  check if we need to start the dev */
 	if (!mei_host_is_ready(dev)) {
 		if (mei_hw_is_ready(dev)) {
+			mei_me_hw_reset_release(dev);
 			dev_dbg(&dev->pdev->dev, "we need to start the dev.\n");
 
 			dev->recvd_hw_ready = true;
 			wake_up_interruptible(&dev->wait_hw_ready);
 		} else {
-
-			dev_dbg(&dev->pdev->dev, "Reset Completed.\n");
-			mei_me_hw_reset_release(dev);
+			dev_dbg(&dev->pdev->dev, "Spurious Interrupt\n");
 		}
 		goto end;
 	}
@@ -524,9 +692,15 @@ irqreturn_t mei_me_irq_thread_handler(int irq, void *dev_id)
 
 	dev->hbuf_is_ready = mei_hbuf_is_ready(dev);
 
-	rets = mei_irq_write_handler(dev, &complete_list);
-
-	dev->hbuf_is_ready = mei_hbuf_is_ready(dev);
+	/*
+	 * During PG handshake only allowed write is the replay to the
+	 * PG exit message, so block calling write function
+	 * if the pg state is not idle
+	 */
+	if (dev->pg_event == MEI_PG_EVENT_IDLE) {
+		rets = mei_irq_write_handler(dev, &complete_list);
+		dev->hbuf_is_ready = mei_hbuf_is_ready(dev);
+	}
 
 	mei_irq_compl_handler(dev, &complete_list);
 
@@ -535,8 +709,65 @@ end:
 	mutex_unlock(&dev->device_lock);
 	return IRQ_HANDLED;
 }
+
+/**
+ * mei_me_fw_status - retrieve fw status from the pci config space
+ *
+ * @dev: the device structure
+ * @fw_status: fw status registers storage
+ *
+ * returns 0 on success an error code otherwise
+ */
+static int mei_me_fw_status(struct mei_device *dev,
+			    struct mei_fw_status *fw_status)
+{
+	const u32 pci_cfg_reg[] = {PCI_CFG_HFS_1, PCI_CFG_HFS_2};
+	int i;
+
+	if (!fw_status)
+		return -EINVAL;
+
+	switch (dev->pdev->device) {
+	case MEI_DEV_ID_IBXPK_1:
+	case MEI_DEV_ID_IBXPK_2:
+	case MEI_DEV_ID_CPT_1:
+	case MEI_DEV_ID_PBG_1:
+	case MEI_DEV_ID_PPT_1:
+	case MEI_DEV_ID_PPT_2:
+	case MEI_DEV_ID_PPT_3:
+	case MEI_DEV_ID_LPT_H:
+	case MEI_DEV_ID_LPT_W:
+	case MEI_DEV_ID_LPT_LP:
+	case MEI_DEV_ID_LPT_HR:
+	case MEI_DEV_ID_WPT_LP:
+		fw_status->count = 2;
+		break;
+	case MEI_DEV_ID_ICH10_1:
+	case MEI_DEV_ID_ICH10_2:
+	case MEI_DEV_ID_ICH10_3:
+	case MEI_DEV_ID_ICH10_4:
+		fw_status->count = 1;
+		break;
+	default:
+		fw_status->count = 0;
+		break;
+	}
+
+	for (i = 0; i < fw_status->count && i < MEI_FW_STATUS_MAX; i++) {
+		int ret;
+		ret = pci_read_config_dword(dev->pdev,
+				pci_cfg_reg[i], &fw_status->status[i]);
+		if (ret)
+			return ret;
+	}
+	return 0;
+}
+
 static const struct mei_hw_ops mei_me_hw_ops = {
 
+	.pg_state  = mei_me_pg_state,
+
+	.fw_status = mei_me_fw_status,
 	.host_is_ready = mei_me_host_is_ready,
 
 	.hw_is_ready = mei_me_hw_is_ready,
@@ -544,6 +775,8 @@ static const struct mei_hw_ops mei_me_hw_ops = {
 	.hw_config = mei_me_hw_config,
 	.hw_start = mei_me_hw_start,
 
+	.pg_is_enabled = mei_me_pg_is_enabled,
+
 	.intr_clear = mei_me_intr_clear,
 	.intr_enable = mei_me_intr_enable,
 	.intr_disable = mei_me_intr_disable,
@@ -559,14 +792,81 @@ static const struct mei_hw_ops mei_me_hw_ops = {
 	.read = mei_me_read_slots
 };
 
+static bool mei_me_fw_type_nm(struct pci_dev *pdev)
+{
+	u32 reg;
+	pci_read_config_dword(pdev, PCI_CFG_HFS_2, &reg);
+	/* make sure that bit 9 (NM) is up and bit 10 (DM) is down */
+	return (reg & 0x600) == 0x200;
+}
+
+#define MEI_CFG_FW_NM                           \
+	.quirk_probe = mei_me_fw_type_nm
+
+static bool mei_me_fw_type_sps(struct pci_dev *pdev)
+{
+	u32 reg;
+	/* Read ME FW Status check for SPS Firmware */
+	pci_read_config_dword(pdev, PCI_CFG_HFS_1, &reg);
+	/* if bits [19:16] = 15, running SPS Firmware */
+	return (reg & 0xf0000) == 0xf0000;
+}
+
+#define MEI_CFG_FW_SPS                           \
+	.quirk_probe = mei_me_fw_type_sps
+
+
+#define MEI_CFG_LEGACY_HFS                      \
+	.fw_status.count = 0
+
+#define MEI_CFG_ICH_HFS                        \
+	.fw_status.count = 1,                   \
+	.fw_status.status[0] = PCI_CFG_HFS_1
+
+#define MEI_CFG_PCH_HFS                         \
+	.fw_status.count = 2,                   \
+	.fw_status.status[0] = PCI_CFG_HFS_1,   \
+	.fw_status.status[1] = PCI_CFG_HFS_2
+
+
+/* ICH Legacy devices */
+const struct mei_cfg mei_me_legacy_cfg = {
+	MEI_CFG_LEGACY_HFS,
+};
+
+/* ICH devices */
+const struct mei_cfg mei_me_ich_cfg = {
+	MEI_CFG_ICH_HFS,
+};
+
+/* PCH devices */
+const struct mei_cfg mei_me_pch_cfg = {
+	MEI_CFG_PCH_HFS,
+};
+
+
+/* PCH Cougar Point and Patsburg with quirk for Node Manager exclusion */
+const struct mei_cfg mei_me_pch_cpt_pbg_cfg = {
+	MEI_CFG_PCH_HFS,
+	MEI_CFG_FW_NM,
+};
+
+/* PCH Lynx Point with quirk for SPS Firmware exclusion */
+const struct mei_cfg mei_me_lpt_cfg = {
+	MEI_CFG_PCH_HFS,
+	MEI_CFG_FW_SPS,
+};
+
 /**
  * mei_me_dev_init - allocates and initializes the mei device structure
  *
  * @pdev: The pci device structure
+ * @cfg: per device generation config
  *
  * returns The mei_device_device pointer on success, NULL on failure.
  */
-struct mei_device *mei_me_dev_init(struct pci_dev *pdev)
+struct mei_device *mei_me_dev_init(struct pci_dev *pdev,
+				   const struct mei_cfg *cfg)
 {
 	struct mei_device *dev;
 
@@ -575,7 +875,7 @@ struct mei_device *mei_me_dev_init(struct pci_dev *pdev)
 	if (!dev)
 		return NULL;
 
-	mei_device_init(dev);
+	mei_device_init(dev, cfg);
 
 	dev->ops = &mei_me_hw_ops;
 
diff --git a/drivers/misc/mei/hw-me.h b/drivers/misc/mei/hw-me.h
index 893d5119fa9..12b0f4bbe1f 100644
--- a/drivers/misc/mei/hw-me.h
+++ b/drivers/misc/mei/hw-me.h
@@ -24,6 +24,8 @@
 #include "mei_dev.h"
 #include "client.h"
 
+#define MEI_ME_RPM_TIMEOUT    500 /* ms */
+
 struct mei_me_hw {
 	void __iomem *mem_addr;
 	/*
@@ -31,11 +33,22 @@ struct mei_me_hw {
 	 */
 	u32 host_hw_state;
 	u32 me_hw_state;
+	enum mei_pg_state pg_state;
 };
 
 #define to_me_hw(dev) (struct mei_me_hw *)((dev)->hw)
 
-struct mei_device *mei_me_dev_init(struct pci_dev *pdev);
+extern const struct mei_cfg mei_me_legacy_cfg;
+extern const struct mei_cfg mei_me_ich_cfg;
+extern const struct mei_cfg mei_me_pch_cfg;
+extern const struct mei_cfg mei_me_pch_cpt_pbg_cfg;
+extern const struct mei_cfg mei_me_lpt_cfg;
+
+struct mei_device *mei_me_dev_init(struct pci_dev *pdev,
+				   const struct mei_cfg *cfg);
+
+int mei_me_pg_set_sync(struct mei_device *dev);
+int mei_me_pg_unset_sync(struct mei_device *dev);
 
 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/hw-txe-regs.h b/drivers/misc/mei/hw-txe-regs.h
index 7283c24c1af..f19229c4e65 100644
--- a/drivers/misc/mei/hw-txe-regs.h
+++ b/drivers/misc/mei/hw-txe-regs.h
@@ -89,7 +89,7 @@ enum {
 #  define PCI_CFG_TXE_FW_STS0_ERR_CODE_MSK  0x0000F000
 #  define PCI_CFG_TXE_FW_STS0_OP_MODE_MSK   0x000F0000
 #  define PCI_CFG_TXE_FW_STS0_RST_CNT_MSK   0x00F00000
-
+#define PCI_CFG_TXE_FW_STS1   0x48
 
 #define IPC_BASE_ADDR	0x80400 /* SeC IPC Base Address */
 
diff --git a/drivers/misc/mei/hw-txe.c b/drivers/misc/mei/hw-txe.c
index f60182a52f9..93273783dec 100644
--- a/drivers/misc/mei/hw-txe.c
+++ b/drivers/misc/mei/hw-txe.c
@@ -158,7 +158,7 @@ static bool mei_txe_aliveness_set(struct mei_device *dev, u32 req)
 	dev_dbg(&dev->pdev->dev, "Aliveness current=%d request=%d\n",
 				hw->aliveness, req);
 	if (do_req) {
-		hw->recvd_aliveness = false;
+		dev->pg_event = MEI_PG_EVENT_WAIT;
 		mei_txe_br_reg_write(hw, SICR_HOST_ALIVENESS_REQ_REG, req);
 	}
 	return do_req;
@@ -213,6 +213,7 @@ static int mei_txe_aliveness_poll(struct mei_device *dev, u32 expected)
 	do {
 		hw->aliveness = mei_txe_aliveness_get(dev);
 		if (hw->aliveness == expected) {
+			dev->pg_event = MEI_PG_EVENT_IDLE;
 			dev_dbg(&dev->pdev->dev,
 				"aliveness settled after %d msecs\n", t);
 			return t;
@@ -223,6 +224,7 @@ static int mei_txe_aliveness_poll(struct mei_device *dev, u32 expected)
 		t += MSEC_PER_SEC / 5;
 	} while (t < SEC_ALIVENESS_WAIT_TIMEOUT);
 
+	dev->pg_event = MEI_PG_EVENT_IDLE;
 	dev_err(&dev->pdev->dev, "aliveness timed out\n");
 	return -ETIME;
 }
@@ -249,19 +251,22 @@ static int mei_txe_aliveness_wait(struct mei_device *dev, u32 expected)
 		return 0;
 
 	mutex_unlock(&dev->device_lock);
-	err = wait_event_timeout(hw->wait_aliveness,
-			hw->recvd_aliveness, timeout);
+	err = wait_event_timeout(hw->wait_aliveness_resp,
+			dev->pg_event == MEI_PG_EVENT_RECEIVED, timeout);
 	mutex_lock(&dev->device_lock);
 
 	hw->aliveness = mei_txe_aliveness_get(dev);
 	ret = hw->aliveness == expected ? 0 : -ETIME;
 
 	if (ret)
-		dev_err(&dev->pdev->dev, "aliveness timed out");
+		dev_warn(&dev->pdev->dev, "aliveness timed out = %ld aliveness = %d event = %d\n",
+			err, hw->aliveness, dev->pg_event);
 	else
-		dev_dbg(&dev->pdev->dev, "aliveness settled after %d msecs\n",
-				jiffies_to_msecs(timeout - err));
-	hw->recvd_aliveness = false;
+		dev_dbg(&dev->pdev->dev, "aliveness settled after = %d msec aliveness = %d event = %d\n",
+			jiffies_to_msecs(timeout - err),
+			hw->aliveness, dev->pg_event);
+
+	dev->pg_event = MEI_PG_EVENT_IDLE;
 	return ret;
 }
 
@@ -280,6 +285,32 @@ int mei_txe_aliveness_set_sync(struct mei_device *dev, u32 req)
 }
 
 /**
+ * mei_txe_pg_is_enabled - detect if PG is supported by HW
+ *
+ * @dev: the device structure
+ *
+ * returns: true is pg supported, false otherwise
+ */
+static bool mei_txe_pg_is_enabled(struct mei_device *dev)
+{
+	return true;
+}
+
+/**
+ * mei_txe_pg_state  - translate aliveness register value
+ *   to the mei power gating state
+ *
+ * @dev: the device structure
+ *
+ * returns: MEI_PG_OFF if aliveness is on and MEI_PG_ON otherwise
+ */
+static inline enum mei_pg_state mei_txe_pg_state(struct mei_device *dev)
+{
+	struct mei_txe_hw *hw = to_txe_hw(dev);
+	return hw->aliveness ? MEI_PG_OFF : MEI_PG_ON;
+}
+
+/**
  * mei_txe_input_ready_interrupt_enable - sets the Input Ready Interrupt
  *
  * @dev: the device structure
@@ -589,7 +620,10 @@ static int mei_txe_write(struct mei_device *dev,
 	mei_txe_input_ready_interrupt_enable(dev);
 
 	if (!mei_txe_is_input_ready(dev)) {
-		dev_err(&dev->pdev->dev, "Input is not ready");
+		struct mei_fw_status fw_status;
+		mei_fw_status(dev, &fw_status);
+		dev_err(&dev->pdev->dev, "Input is not ready " FW_STS_FMT "\n",
+			FW_STS_PRM(fw_status));
 		return -EAGAIN;
 	}
 
@@ -960,9 +994,9 @@ irqreturn_t mei_txe_irq_thread_handler(int irq, void *dev_id)
 		/* Clear the interrupt cause */
 		dev_dbg(&dev->pdev->dev,
 			"Aliveness Interrupt: Status: %d\n", hw->aliveness);
-		hw->recvd_aliveness = true;
-		if (waitqueue_active(&hw->wait_aliveness))
-			wake_up(&hw->wait_aliveness);
+		dev->pg_event = MEI_PG_EVENT_RECEIVED;
+		if (waitqueue_active(&hw->wait_aliveness_resp))
+			wake_up(&hw->wait_aliveness_resp);
 	}
 
 
@@ -1008,15 +1042,51 @@ end:
 	return IRQ_HANDLED;
 }
 
+
+/**
+ * mei_txe_fw_status - retrieve fw status from the pci config space
+ *
+ * @dev: the device structure
+ * @fw_status: fw status registers storage
+ *
+ * returns: 0 on success an error code otherwise
+ */
+static int mei_txe_fw_status(struct mei_device *dev,
+			     struct mei_fw_status *fw_status)
+{
+	const u32 pci_cfg_reg[] = {PCI_CFG_TXE_FW_STS0, PCI_CFG_TXE_FW_STS1};
+	int i;
+
+	if (!fw_status)
+		return -EINVAL;
+
+	fw_status->count = 2;
+
+	for (i = 0; i < fw_status->count && i < MEI_FW_STATUS_MAX; i++) {
+		int ret;
+		ret = pci_read_config_dword(dev->pdev,
+				pci_cfg_reg[i], &fw_status->status[i]);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
 static const struct mei_hw_ops mei_txe_hw_ops = {
 
+	.fw_status = mei_txe_fw_status,
 	.host_is_ready = mei_txe_host_is_ready,
 
+	.pg_state = mei_txe_pg_state,
+
 	.hw_is_ready = mei_txe_hw_is_ready,
 	.hw_reset = mei_txe_hw_reset,
 	.hw_config = mei_txe_hw_config,
 	.hw_start = mei_txe_hw_start,
 
+	.pg_is_enabled = mei_txe_pg_is_enabled,
+
 	.intr_clear = mei_txe_intr_clear,
 	.intr_enable = mei_txe_intr_enable,
 	.intr_disable = mei_txe_intr_disable,
@@ -1034,14 +1104,27 @@ static const struct mei_hw_ops mei_txe_hw_ops = {
 
 };
 
+#define MEI_CFG_TXE_FW_STS                            \
+	.fw_status.count = 2,                         \
+	.fw_status.status[0] = PCI_CFG_TXE_FW_STS0,   \
+	.fw_status.status[1] = PCI_CFG_TXE_FW_STS1
+
+const struct mei_cfg mei_txe_cfg = {
+	MEI_CFG_TXE_FW_STS,
+};
+
+
 /**
  * mei_txe_dev_init - allocates and initializes txe hardware specific structure
  *
  * @pdev - pci device
+ * @cfg - per device generation config
+ *
  * returns struct mei_device * on success or NULL;
  *
  */
-struct mei_device *mei_txe_dev_init(struct pci_dev *pdev)
+struct mei_device *mei_txe_dev_init(struct pci_dev *pdev,
+				    const struct mei_cfg *cfg)
 {
 	struct mei_device *dev;
 	struct mei_txe_hw *hw;
@@ -1051,11 +1134,11 @@ struct mei_device *mei_txe_dev_init(struct pci_dev *pdev)
 	if (!dev)
 		return NULL;
 
-	mei_device_init(dev);
+	mei_device_init(dev, cfg);
 
 	hw = to_txe_hw(dev);
 
-	init_waitqueue_head(&hw->wait_aliveness);
+	init_waitqueue_head(&hw->wait_aliveness_resp);
 
 	dev->ops = &mei_txe_hw_ops;
 
diff --git a/drivers/misc/mei/hw-txe.h b/drivers/misc/mei/hw-txe.h
index 0812d98633a..e244af79167 100644
--- a/drivers/misc/mei/hw-txe.h
+++ b/drivers/misc/mei/hw-txe.h
@@ -22,6 +22,8 @@
 #include "hw.h"
 #include "hw-txe-regs.h"
 
+#define MEI_TXI_RPM_TIMEOUT    500 /* ms */
+
 /* Flatten Hierarchy interrupt cause */
 #define TXE_INTR_READINESS_BIT  0 /* HISR_INT_0_STS */
 #define TXE_INTR_READINESS      HISR_INT_0_STS
@@ -35,12 +37,11 @@
 /**
  * struct mei_txe_hw - txe hardware specifics
  *
- * @mem_addr:        SeC and BRIDGE bars
- * @aliveness:       aliveness (power gating) state of the hardware
- * @readiness:       readiness state of the hardware
- * @wait_aliveness:  aliveness wait queue
- * @recvd_aliveness: aliveness interrupt was recived
- * @intr_cause:      translated interrupt cause
+ * @mem_addr:            SeC and BRIDGE bars
+ * @aliveness:           aliveness (power gating) state of the hardware
+ * @readiness:           readiness state of the hardware
+ * @wait_aliveness_resp: aliveness wait queue
+ * @intr_cause:          translated interrupt cause
  */
 struct mei_txe_hw {
 	void __iomem *mem_addr[NUM_OF_MEM_BARS];
@@ -48,8 +49,7 @@ struct mei_txe_hw {
 	u32 readiness;
 	u32 slots;
 
-	wait_queue_head_t wait_aliveness;
-	bool recvd_aliveness;
+	wait_queue_head_t wait_aliveness_resp;
 
 	unsigned long intr_cause;
 };
@@ -61,7 +61,10 @@ static inline struct mei_device *hw_txe_to_mei(struct mei_txe_hw *hw)
 	return container_of((void *)hw, struct mei_device, hw);
 }
 
-struct mei_device *mei_txe_dev_init(struct pci_dev *pdev);
+extern const struct mei_cfg mei_txe_cfg;
+
+struct mei_device *mei_txe_dev_init(struct pci_dev *pdev,
+	const struct mei_cfg *cfg);
 
 irqreturn_t mei_txe_irq_quick_handler(int irq, void *dev_id);
 irqreturn_t mei_txe_irq_thread_handler(int irq, void *dev_id);
diff --git a/drivers/misc/mei/hw.h b/drivers/misc/mei/hw.h
index 6b476ab49b2..dd448e58cc8 100644
--- a/drivers/misc/mei/hw.h
+++ b/drivers/misc/mei/hw.h
@@ -31,14 +31,21 @@
 #define MEI_IAMTHIF_STALL_TIMER    12  /* HPS */
 #define MEI_IAMTHIF_READ_TIMER     10  /* HPS */
 
+#define MEI_PGI_TIMEOUT            1  /* PG Isolation time response 1 sec */
 #define MEI_HBM_TIMEOUT            1   /* 1 second */
 
 /*
  * MEI Version
  */
-#define HBM_MINOR_VERSION                   0
+#define HBM_MINOR_VERSION                   1
 #define HBM_MAJOR_VERSION                   1
 
+/*
+ * MEI version with PGI support
+ */
+#define HBM_MINOR_VERSION_PGI               1
+#define HBM_MAJOR_VERSION_PGI               1
+
 /* Host bus message command opcode */
 #define MEI_HBM_CMD_OP_MSK                  0x7f
 /* Host bus message command RESPONSE */
@@ -69,6 +76,11 @@
 
 #define MEI_FLOW_CONTROL_CMD                0x08
 
+#define MEI_PG_ISOLATION_ENTRY_REQ_CMD      0x0a
+#define MEI_PG_ISOLATION_ENTRY_RES_CMD      0x8a
+#define MEI_PG_ISOLATION_EXIT_REQ_CMD       0x0b
+#define MEI_PG_ISOLATION_EXIT_RES_CMD       0x8b
+
 /*
  * MEI Stop Reason
  * used by hbm_host_stop_request.reason
@@ -208,6 +220,17 @@ struct hbm_props_response {
 } __packed;
 
 /**
+ * struct hbm_power_gate - power gate request/response
+ *
+ * @hbm_cmd - bus message command header
+ * @reserved[3]
+ */
+struct hbm_power_gate {
+	u8 hbm_cmd;
+	u8 reserved[3];
+} __packed;
+
+/**
  * struct hbm_client_connect_request - connect/disconnect request
  *
  * @hbm_cmd - bus message command header
diff --git a/drivers/misc/mei/init.c b/drivers/misc/mei/init.c
index 4460975c0ee..00692922248 100644
--- a/drivers/misc/mei/init.c
+++ b/drivers/misc/mei/init.c
@@ -74,9 +74,13 @@ int mei_reset(struct mei_device *dev)
 	if (state != MEI_DEV_INITIALIZING &&
 	    state != MEI_DEV_DISABLED &&
 	    state != MEI_DEV_POWER_DOWN &&
-	    state != MEI_DEV_POWER_UP)
-		dev_warn(&dev->pdev->dev, "unexpected reset: dev_state = %s\n",
-			 mei_dev_state_str(state));
+	    state != MEI_DEV_POWER_UP) {
+		struct mei_fw_status fw_status;
+		mei_fw_status(dev, &fw_status);
+		dev_warn(&dev->pdev->dev,
+			"unexpected reset: dev_state = %s " FW_STS_FMT "\n",
+			mei_dev_state_str(state), FW_STS_PRM(fw_status));
+	}
 
 	/* we're already in reset, cancel the init timer
 	 * if the reset was called due the hbm protocol error
@@ -118,8 +122,8 @@ int mei_reset(struct mei_device *dev)
 		mei_amthif_reset_params(dev);
 	}
 
+	mei_hbm_reset(dev);
 
-	dev->me_clients_num = 0;
 	dev->rd_msg_hdr = 0;
 	dev->wd_pending = false;
 
@@ -303,15 +307,58 @@ void mei_stop(struct mei_device *dev)
 }
 EXPORT_SYMBOL_GPL(mei_stop);
 
+/**
+ * mei_write_is_idle - check if the write queues are idle
+ *
+ * @dev: the device structure
+ *
+ * returns true of there is no pending write
+ */
+bool mei_write_is_idle(struct mei_device *dev)
+{
+	bool idle = (dev->dev_state == MEI_DEV_ENABLED &&
+		list_empty(&dev->ctrl_wr_list.list) &&
+		list_empty(&dev->write_list.list));
 
+	dev_dbg(&dev->pdev->dev, "write pg: is idle[%d] state=%s ctrl=%d write=%d\n",
+		idle,
+		mei_dev_state_str(dev->dev_state),
+		list_empty(&dev->ctrl_wr_list.list),
+		list_empty(&dev->write_list.list));
 
-void mei_device_init(struct mei_device *dev)
+	return idle;
+}
+EXPORT_SYMBOL_GPL(mei_write_is_idle);
+
+int mei_fw_status(struct mei_device *dev, struct mei_fw_status *fw_status)
+{
+	int i;
+	const struct mei_fw_status *fw_src = &dev->cfg->fw_status;
+
+	if (!fw_status)
+		return -EINVAL;
+
+	fw_status->count = fw_src->count;
+	for (i = 0; i < fw_src->count && i < MEI_FW_STATUS_MAX; i++) {
+		int ret;
+		ret = pci_read_config_dword(dev->pdev,
+			fw_src->status[i], &fw_status->status[i]);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mei_fw_status);
+
+void mei_device_init(struct mei_device *dev, const struct mei_cfg *cfg)
 {
 	/* 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_pg);
 	init_waitqueue_head(&dev->wait_recvd_msg);
 	init_waitqueue_head(&dev->wait_stop_wd);
 	dev->dev_state = MEI_DEV_INITIALIZING;
@@ -340,6 +387,9 @@ void mei_device_init(struct mei_device *dev)
 	 * 0: Reserved for MEI Bus Message communications
 	 */
 	bitmap_set(dev->host_clients_map, 0, 1);
+
+	dev->pg_event = MEI_PG_EVENT_IDLE;
+	dev->cfg      = cfg;
 }
 EXPORT_SYMBOL_GPL(mei_device_init);
 
diff --git a/drivers/misc/mei/interrupt.c b/drivers/misc/mei/interrupt.c
index 29b5af8efb7..4e3cba6da3f 100644
--- a/drivers/misc/mei/interrupt.c
+++ b/drivers/misc/mei/interrupt.c
@@ -455,8 +455,7 @@ int mei_irq_write_handler(struct mei_device *dev, struct mei_cl_cb *cmpl_list)
 
 		cl->status = 0;
 		list_del(&cb->list);
-		if (MEI_WRITING == cl->writing_state &&
-		    cb->fop_type == MEI_FOP_WRITE &&
+		if (cb->fop_type == MEI_FOP_WRITE &&
 		    cl != &dev->iamthif_cl) {
 			cl_dbg(dev, cl, "MEI WRITE COMPLETE\n");
 			cl->writing_state = MEI_WRITE_COMPLETE;
diff --git a/drivers/misc/mei/main.c b/drivers/misc/mei/main.c
index b35594dbf52..66f0a1a0645 100644
--- a/drivers/misc/mei/main.c
+++ b/drivers/misc/mei/main.c
@@ -467,7 +467,6 @@ static int mei_ioctl_connect_client(struct file *file,
 	}
 
 	cl->me_client_id = dev->me_clients[i].client_id;
-	cl->state = MEI_FILE_CONNECTING;
 
 	dev_dbg(&dev->pdev->dev, "Connect to FW Client ID = %d\n",
 			cl->me_client_id);
@@ -644,8 +643,7 @@ static unsigned int mei_poll(struct file *file, poll_table *wait)
 		goto out;
 	}
 
-	if (MEI_WRITE_COMPLETE == cl->writing_state)
-		mask |= (POLLIN | POLLRDNORM);
+	mask |= (POLLIN | POLLRDNORM);
 
 out:
 	mutex_unlock(&dev->device_lock);
diff --git a/drivers/misc/mei/mei_dev.h b/drivers/misc/mei/mei_dev.h
index 94a516716d2..5c7e990e2f2 100644
--- a/drivers/misc/mei/mei_dev.h
+++ b/drivers/misc/mei/mei_dev.h
@@ -153,6 +153,20 @@ struct mei_msg_data {
 	unsigned char *data;
 };
 
+/* Maximum number of processed FW status registers */
+#define MEI_FW_STATUS_MAX 2
+
+/*
+ * struct mei_fw_status - storage of FW status data
+ *
+ * @count - number of actually available elements in array
+ * @status - FW status registers
+ */
+struct mei_fw_status {
+	int count;
+	u32 status[MEI_FW_STATUS_MAX];
+};
+
 /**
  * struct mei_me_client - representation of me (fw) client
  *
@@ -213,6 +227,7 @@ struct mei_cl {
 
 /** struct mei_hw_ops
  *
+ * @fw_status        - read FW status from PCI config space
  * @host_is_ready    - query for host readiness
 
  * @hw_is_ready      - query if hw is ready
@@ -220,6 +235,9 @@ struct mei_cl {
  * @hw_start         - start hw after reset
  * @hw_config        - configure hw
 
+ * @pg_state         - power gating state of the device
+ * @pg_is_enabled    - is power gating enabled
+
  * @intr_clear       - clear pending interrupts
  * @intr_enable      - enable interrupts
  * @intr_disable     - disable interrupts
@@ -237,6 +255,8 @@ struct mei_cl {
  */
 struct mei_hw_ops {
 
+	int (*fw_status)(struct mei_device *dev,
+		struct mei_fw_status *fw_status);
 	bool (*host_is_ready)(struct mei_device *dev);
 
 	bool (*hw_is_ready)(struct mei_device *dev);
@@ -244,6 +264,9 @@ struct mei_hw_ops {
 	int (*hw_start)(struct mei_device *dev);
 	void (*hw_config)(struct mei_device *dev);
 
+	enum mei_pg_state (*pg_state)(struct mei_device *dev);
+	bool (*pg_is_enabled)(struct mei_device *dev);
+
 	void (*intr_clear)(struct mei_device *dev);
 	void (*intr_enable)(struct mei_device *dev);
 	void (*intr_disable)(struct mei_device *dev);
@@ -331,16 +354,61 @@ struct mei_cl_device {
 	void *priv_data;
 };
 
+
+ /**
+ * enum mei_pg_event - power gating transition events
+ *
+ * @MEI_PG_EVENT_IDLE: the driver is not in power gating transition
+ * @MEI_PG_EVENT_WAIT: the driver is waiting for a pg event to complete
+ * @MEI_PG_EVENT_RECEIVED: the driver received pg event
+ */
+enum mei_pg_event {
+	MEI_PG_EVENT_IDLE,
+	MEI_PG_EVENT_WAIT,
+	MEI_PG_EVENT_RECEIVED,
+};
+
+/**
+ * enum mei_pg_state - device internal power gating state
+ *
+ * @MEI_PG_OFF: device is not power gated - it is active
+ * @MEI_PG_ON:  device is power gated - it is in lower power state
+ */
+enum mei_pg_state {
+	MEI_PG_OFF = 0,
+	MEI_PG_ON =  1,
+};
+
+/*
+ * mei_cfg
+ *
+ * @fw_status - FW status
+ * @quirk_probe - device exclusion quirk
+ */
+struct mei_cfg {
+	const struct mei_fw_status fw_status;
+	bool (*quirk_probe)(struct pci_dev *pdev);
+};
+
+
+#define MEI_PCI_DEVICE(dev, cfg) \
+	.vendor = PCI_VENDOR_ID_INTEL, .device = (dev), \
+	.subvendor = PCI_ANY_ID, .subdevice = PCI_ANY_ID, \
+	.driver_data = (kernel_ulong_t)&(cfg)
+
+
 /**
  * struct mei_device -  MEI private device struct
 
  * @reset_count - limits the number of consecutive resets
  * @hbm_state - state of host bus message protocol
+ * @pg_event - power gating event
  * @mem_addr - mem mapped base register address
 
  * @hbuf_depth - depth of hardware host/write buffer is slots
  * @hbuf_is_ready - query if the host host/write buffer is ready
  * @wr_msg - the buffer for hbm control messages
+ * @cfg - per device generation config and ops
  */
 struct mei_device {
 	struct pci_dev *pdev;	/* pointer to pci device struct */
@@ -371,6 +439,7 @@ struct mei_device {
 	 * waiting queue for receive message from FW
 	 */
 	wait_queue_head_t wait_hw_ready;
+	wait_queue_head_t wait_pg;
 	wait_queue_head_t wait_recvd_msg;
 	wait_queue_head_t wait_stop_wd;
 
@@ -382,6 +451,14 @@ struct mei_device {
 	enum mei_hbm_state hbm_state;
 	u16 init_clients_timer;
 
+	/*
+	 * Power Gating support
+	 */
+	enum mei_pg_event pg_event;
+#ifdef CONFIG_PM_RUNTIME
+	struct dev_pm_domain pg_domain;
+#endif /* CONFIG_PM_RUNTIME */
+
 	unsigned char rd_msg_buf[MEI_RD_MSG_BUF_SIZE];	/* control messages */
 	u32 rd_msg_hdr;
 
@@ -442,6 +519,7 @@ struct mei_device {
 
 
 	const struct mei_hw_ops *ops;
+	const struct mei_cfg *cfg;
 	char hw[0] __aligned(sizeof(void *));
 };
 
@@ -474,7 +552,7 @@ static inline u32 mei_slots2data(int slots)
 /*
  * mei init function prototypes
  */
-void mei_device_init(struct mei_device *dev);
+void mei_device_init(struct mei_device *dev, const struct mei_cfg *cfg);
 int mei_reset(struct mei_device *dev);
 int mei_start(struct mei_device *dev);
 int mei_restart(struct mei_device *dev);
@@ -553,10 +631,22 @@ void mei_watchdog_unregister(struct mei_device *dev);
  * Register Access Function
  */
 
+
 static inline void mei_hw_config(struct mei_device *dev)
 {
 	dev->ops->hw_config(dev);
 }
+
+static inline enum mei_pg_state mei_pg_state(struct mei_device *dev)
+{
+	return dev->ops->pg_state(dev);
+}
+
+static inline bool mei_pg_is_enabled(struct mei_device *dev)
+{
+	return dev->ops->pg_is_enabled(dev);
+}
+
 static inline int mei_hw_reset(struct mei_device *dev, bool enable)
 {
 	return dev->ops->hw_reset(dev, enable);
@@ -629,8 +719,17 @@ static inline int mei_count_full_read_slots(struct mei_device *dev)
 	return dev->ops->rdbuf_full_slots(dev);
 }
 
+int mei_fw_status(struct mei_device *dev, struct mei_fw_status *fw_status);
+
+#define FW_STS_FMT "%08X %08X"
+#define FW_STS_PRM(fw_status) \
+	(fw_status).count > 0 ? (fw_status).status[0] : 0xDEADBEEF, \
+	(fw_status).count > 1 ? (fw_status).status[1] : 0xDEADBEEF
+
 bool mei_hbuf_acquire(struct mei_device *dev);
 
+bool mei_write_is_idle(struct mei_device *dev);
+
 #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);
diff --git a/drivers/misc/mei/pci-me.c b/drivers/misc/mei/pci-me.c
index 1c8fd3a3e13..1b46c64a649 100644
--- a/drivers/misc/mei/pci-me.c
+++ b/drivers/misc/mei/pci-me.c
@@ -33,6 +33,8 @@
 #include <linux/interrupt.h>
 #include <linux/miscdevice.h>
 
+#include <linux/pm_runtime.h>
+
 #include <linux/mei.h>
 
 #include "mei_dev.h"
@@ -42,42 +44,44 @@
 
 /* mei_pci_tbl - PCI Device ID Table */
 static const struct pci_device_id 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)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_82G965)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_82GM965)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_82GME965)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH9_82Q35)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH9_82G33)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH9_82Q33)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH9_82X38)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH9_3200)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH9_6)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH9_7)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH9_8)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH9_9)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH9_10)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH9M_1)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH9M_2)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH9M_3)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH9M_4)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH10_1)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH10_2)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH10_3)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_ICH10_4)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_IBXPK_1)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_IBXPK_2)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_CPT_1)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_PBG_1)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_PPT_1)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_PPT_2)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_PPT_3)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_LPT_H)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_LPT_W)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_LPT_LP)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_LPT_HR)},
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, MEI_DEV_ID_WPT_LP)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_82946GZ, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_82G35, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_82Q965, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_82G965, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_82GM965, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_82GME965, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH9_82Q35, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH9_82G33, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH9_82Q33, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH9_82X38, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH9_3200, mei_me_legacy_cfg)},
+
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH9_6, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH9_7, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH9_8, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH9_9, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH9_10, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH9M_1, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH9M_2, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH9M_3, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH9M_4, mei_me_legacy_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH10_1, mei_me_ich_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH10_2, mei_me_ich_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH10_3, mei_me_ich_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_ICH10_4, mei_me_ich_cfg)},
+
+	{MEI_PCI_DEVICE(MEI_DEV_ID_IBXPK_1, mei_me_pch_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_IBXPK_2, mei_me_pch_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_CPT_1, mei_me_pch_cpt_pbg_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_PBG_1, mei_me_pch_cpt_pbg_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_PPT_1, mei_me_pch_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_PPT_2, mei_me_pch_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_PPT_3, mei_me_pch_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_LPT_H, mei_me_lpt_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_LPT_W, mei_me_lpt_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_LPT_LP, mei_me_pch_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_LPT_HR, mei_me_lpt_cfg)},
+	{MEI_PCI_DEVICE(MEI_DEV_ID_WPT_LP, mei_me_pch_cfg)},
 
 	/* required last entry */
 	{0, }
@@ -85,28 +89,33 @@ static const struct pci_device_id mei_me_pci_tbl[] = {
 
 MODULE_DEVICE_TABLE(pci, mei_me_pci_tbl);
 
+#ifdef CONFIG_PM_RUNTIME
+static inline void mei_me_set_pm_domain(struct mei_device *dev);
+static inline void mei_me_unset_pm_domain(struct mei_device *dev);
+#else
+static inline void mei_me_set_pm_domain(struct mei_device *dev) {}
+static inline void mei_me_unset_pm_domain(struct mei_device *dev) {}
+#endif /* CONFIG_PM_RUNTIME */
+
 /**
  * mei_quirk_probe - probe for devices that doesn't valid ME interface
  *
  * @pdev: PCI device structure
- * @ent: entry into pci_device_table
+ * @cfg: per generation config
  *
  * returns true if ME Interface is valid, false otherwise
  */
 static bool mei_me_quirk_probe(struct pci_dev *pdev,
-				const struct pci_device_id *ent)
+				const struct mei_cfg *cfg)
 {
-	u32 reg;
-	if (ent->device == MEI_DEV_ID_PBG_1) {
-		pci_read_config_dword(pdev, 0x48, &reg);
-		/* make sure that bit 9 is up and bit 10 is down */
-		if ((reg & 0x600) == 0x200) {
-			dev_info(&pdev->dev, "Device doesn't have valid ME Interface\n");
-			return false;
-		}
+	if (cfg->quirk_probe && cfg->quirk_probe(pdev)) {
+		dev_info(&pdev->dev, "Device doesn't have valid ME Interface\n");
+		return false;
 	}
+
 	return true;
 }
+
 /**
  * mei_probe - Device Initialization Routine
  *
@@ -117,15 +126,14 @@ static bool mei_me_quirk_probe(struct pci_dev *pdev,
  */
 static int mei_me_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
+	const struct mei_cfg *cfg = (struct mei_cfg *)(ent->driver_data);
 	struct mei_device *dev;
 	struct mei_me_hw *hw;
 	int err;
 
 
-	if (!mei_me_quirk_probe(pdev, ent)) {
-		err = -ENODEV;
-		goto end;
-	}
+	if (!mei_me_quirk_probe(pdev, cfg))
+		return -ENODEV;
 
 	/* enable pci dev */
 	err = pci_enable_device(pdev);
@@ -157,7 +165,7 @@ static int mei_me_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 
 	/* allocates and initializes the mei dev structure */
-	dev = mei_me_dev_init(pdev);
+	dev = mei_me_dev_init(pdev, cfg);
 	if (!dev) {
 		err = -ENOMEM;
 		goto release_regions;
@@ -196,6 +204,9 @@ static int mei_me_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		goto release_irq;
 	}
 
+	pm_runtime_set_autosuspend_delay(&pdev->dev, MEI_ME_RPM_TIMEOUT);
+	pm_runtime_use_autosuspend(&pdev->dev);
+
 	err = mei_register(dev);
 	if (err)
 		goto release_irq;
@@ -204,6 +215,17 @@ static int mei_me_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	schedule_delayed_work(&dev->timer_work, HZ);
 
+	/*
+	* For not wake-able HW runtime pm framework
+	* can't be used on pci device level.
+	* Use domain runtime pm callbacks instead.
+	*/
+	if (!pci_dev_run_wake(pdev))
+		mei_me_set_pm_domain(dev);
+
+	if (mei_pg_is_enabled(dev))
+		pm_runtime_put_noidle(&pdev->dev);
+
 	dev_dbg(&pdev->dev, "initialization successful.\n");
 
 	return 0;
@@ -243,12 +265,18 @@ static void mei_me_remove(struct pci_dev *pdev)
 	if (!dev)
 		return;
 
+	if (mei_pg_is_enabled(dev))
+		pm_runtime_get_noresume(&pdev->dev);
+
 	hw = to_me_hw(dev);
 
 
 	dev_dbg(&pdev->dev, "stop\n");
 	mei_stop(dev);
 
+	if (!pci_dev_run_wake(pdev))
+		mei_me_unset_pm_domain(dev);
+
 	/* disable interrupts */
 	mei_disable_interrupts(dev);
 
@@ -327,12 +355,120 @@ static int mei_me_pci_resume(struct device *device)
 
 	return 0;
 }
+#endif /* CONFIG_PM_SLEEP */
+
+#ifdef CONFIG_PM_RUNTIME
+static int mei_me_pm_runtime_idle(struct device *device)
+{
+	struct pci_dev *pdev = to_pci_dev(device);
+	struct mei_device *dev;
+
+	dev_dbg(&pdev->dev, "rpm: me: runtime_idle\n");
+
+	dev = pci_get_drvdata(pdev);
+	if (!dev)
+		return -ENODEV;
+	if (mei_write_is_idle(dev))
+		pm_schedule_suspend(device, MEI_ME_RPM_TIMEOUT * 2);
+
+	return -EBUSY;
+}
+
+static int mei_me_pm_runtime_suspend(struct device *device)
+{
+	struct pci_dev *pdev = to_pci_dev(device);
+	struct mei_device *dev;
+	int ret;
+
+	dev_dbg(&pdev->dev, "rpm: me: runtime suspend\n");
+
+	dev = pci_get_drvdata(pdev);
+	if (!dev)
+		return -ENODEV;
+
+	mutex_lock(&dev->device_lock);
+
+	if (mei_write_is_idle(dev))
+		ret = mei_me_pg_set_sync(dev);
+	else
+		ret = -EAGAIN;
+
+	mutex_unlock(&dev->device_lock);
+
+	dev_dbg(&pdev->dev, "rpm: me: runtime suspend ret=%d\n", ret);
+
+	return ret;
+}
+
+static int mei_me_pm_runtime_resume(struct device *device)
+{
+	struct pci_dev *pdev = to_pci_dev(device);
+	struct mei_device *dev;
+	int ret;
+
+	dev_dbg(&pdev->dev, "rpm: me: runtime resume\n");
+
+	dev = pci_get_drvdata(pdev);
+	if (!dev)
+		return -ENODEV;
+
+	mutex_lock(&dev->device_lock);
+
+	ret = mei_me_pg_unset_sync(dev);
+
+	mutex_unlock(&dev->device_lock);
+
+	dev_dbg(&pdev->dev, "rpm: me: runtime resume ret = %d\n", ret);
+
+	return ret;
+}
+
+/**
+ * mei_me_set_pm_domain - fill and set pm domian stucture for device
+ *
+ * @dev: mei_device
+ */
+static inline void mei_me_set_pm_domain(struct mei_device *dev)
+{
+	struct pci_dev *pdev  = dev->pdev;
+
+	if (pdev->dev.bus && pdev->dev.bus->pm) {
+		dev->pg_domain.ops = *pdev->dev.bus->pm;
+
+		dev->pg_domain.ops.runtime_suspend = mei_me_pm_runtime_suspend;
+		dev->pg_domain.ops.runtime_resume = mei_me_pm_runtime_resume;
+		dev->pg_domain.ops.runtime_idle = mei_me_pm_runtime_idle;
+
+		pdev->dev.pm_domain = &dev->pg_domain;
+	}
+}
+
+/**
+ * mei_me_unset_pm_domain - clean pm domian stucture for device
+ *
+ * @dev: mei_device
+ */
+static inline void mei_me_unset_pm_domain(struct mei_device *dev)
+{
+	/* stop using pm callbacks if any */
+	dev->pdev->dev.pm_domain = NULL;
+}
+#endif /* CONFIG_PM_RUNTIME */
+
+#ifdef CONFIG_PM
+static const struct dev_pm_ops mei_me_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(mei_me_pci_suspend,
+				mei_me_pci_resume)
+	SET_RUNTIME_PM_OPS(
+		mei_me_pm_runtime_suspend,
+		mei_me_pm_runtime_resume,
+		mei_me_pm_runtime_idle)
+};
 
-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_ME_PM_OPS	NULL
-#endif /* CONFIG_PM_SLEEP */
+#endif /* CONFIG_PM */
 /*
  *  PCI driver structure
  */
diff --git a/drivers/misc/mei/pci-txe.c b/drivers/misc/mei/pci-txe.c
index ad3adb009a1..2343c6236df 100644
--- a/drivers/misc/mei/pci-txe.c
+++ b/drivers/misc/mei/pci-txe.c
@@ -27,6 +27,7 @@
 #include <linux/jiffies.h>
 #include <linux/interrupt.h>
 #include <linux/workqueue.h>
+#include <linux/pm_runtime.h>
 
 #include <linux/mei.h>
 
@@ -35,11 +36,18 @@
 #include "hw-txe.h"
 
 static const struct pci_device_id mei_txe_pci_tbl[] = {
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x0F18)}, /* Baytrail */
+	{MEI_PCI_DEVICE(0x0F18, mei_txe_cfg)}, /* Baytrail */
 	{0, }
 };
 MODULE_DEVICE_TABLE(pci, mei_txe_pci_tbl);
 
+#ifdef CONFIG_PM_RUNTIME
+static inline void mei_txe_set_pm_domain(struct mei_device *dev);
+static inline void mei_txe_unset_pm_domain(struct mei_device *dev);
+#else
+static inline void mei_txe_set_pm_domain(struct mei_device *dev) {}
+static inline void mei_txe_unset_pm_domain(struct mei_device *dev) {}
+#endif /* CONFIG_PM_RUNTIME */
 
 static void mei_txe_pci_iounmap(struct pci_dev *pdev, struct mei_txe_hw *hw)
 {
@@ -61,6 +69,7 @@ static void mei_txe_pci_iounmap(struct pci_dev *pdev, struct mei_txe_hw *hw)
  */
 static int mei_txe_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
+	const struct mei_cfg *cfg = (struct mei_cfg *)(ent->driver_data);
 	struct mei_device *dev;
 	struct mei_txe_hw *hw;
 	int err;
@@ -91,7 +100,7 @@ static int mei_txe_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	}
 
 	/* allocates and initializes the mei dev structure */
-	dev = mei_txe_dev_init(pdev);
+	dev = mei_txe_dev_init(pdev, cfg);
 	if (!dev) {
 		err = -ENOMEM;
 		goto release_regions;
@@ -137,12 +146,25 @@ static int mei_txe_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		goto release_irq;
 	}
 
+	pm_runtime_set_autosuspend_delay(&pdev->dev, MEI_TXI_RPM_TIMEOUT);
+	pm_runtime_use_autosuspend(&pdev->dev);
+
 	err = mei_register(dev);
 	if (err)
 		goto release_irq;
 
 	pci_set_drvdata(pdev, dev);
 
+	/*
+	* For not wake-able HW runtime pm framework
+	* can't be used on pci device level.
+	* Use domain runtime pm callbacks instead.
+	*/
+	if (!pci_dev_run_wake(pdev))
+		mei_txe_set_pm_domain(dev);
+
+	pm_runtime_put_noidle(&pdev->dev);
+
 	return 0;
 
 release_irq:
@@ -187,10 +209,15 @@ static void mei_txe_remove(struct pci_dev *pdev)
 		return;
 	}
 
+	pm_runtime_get_noresume(&pdev->dev);
+
 	hw = to_txe_hw(dev);
 
 	mei_stop(dev);
 
+	if (!pci_dev_run_wake(pdev))
+		mei_txe_unset_pm_domain(dev);
+
 	/* disable interrupts */
 	mei_disable_interrupts(dev);
 	free_irq(pdev->irq, dev);
@@ -265,15 +292,131 @@ static int mei_txe_pci_resume(struct device *device)
 
 	return err;
 }
+#endif /* CONFIG_PM_SLEEP */
+
+#ifdef CONFIG_PM_RUNTIME
+static int mei_txe_pm_runtime_idle(struct device *device)
+{
+	struct pci_dev *pdev = to_pci_dev(device);
+	struct mei_device *dev;
+
+	dev_dbg(&pdev->dev, "rpm: txe: runtime_idle\n");
+
+	dev = pci_get_drvdata(pdev);
+	if (!dev)
+		return -ENODEV;
+	if (mei_write_is_idle(dev))
+		pm_schedule_suspend(device, MEI_TXI_RPM_TIMEOUT * 2);
+
+	return -EBUSY;
+}
+static int mei_txe_pm_runtime_suspend(struct device *device)
+{
+	struct pci_dev *pdev = to_pci_dev(device);
+	struct mei_device *dev;
+	int ret;
+
+	dev_dbg(&pdev->dev, "rpm: txe: runtime suspend\n");
+
+	dev = pci_get_drvdata(pdev);
+	if (!dev)
+		return -ENODEV;
+
+	mutex_lock(&dev->device_lock);
+
+	if (mei_write_is_idle(dev))
+		ret = mei_txe_aliveness_set_sync(dev, 0);
+	else
+		ret = -EAGAIN;
+
+	/*
+	 * If everything is okay we're about to enter PCI low
+	 * power state (D3) therefor we need to disable the
+	 * interrupts towards host.
+	 * However if device is not wakeable we do not enter
+	 * D-low state and we need to keep the interrupt kicking
+	 */
+	 if (!ret && pci_dev_run_wake(pdev))
+		mei_disable_interrupts(dev);
+
+	dev_dbg(&pdev->dev, "rpm: txe: runtime suspend ret=%d\n", ret);
+
+	mutex_unlock(&dev->device_lock);
+	return ret;
+}
+
+static int mei_txe_pm_runtime_resume(struct device *device)
+{
+	struct pci_dev *pdev = to_pci_dev(device);
+	struct mei_device *dev;
+	int ret;
+
+	dev_dbg(&pdev->dev, "rpm: txe: runtime resume\n");
+
+	dev = pci_get_drvdata(pdev);
+	if (!dev)
+		return -ENODEV;
+
+	mutex_lock(&dev->device_lock);
+
+	mei_enable_interrupts(dev);
+
+	ret = mei_txe_aliveness_set_sync(dev, 1);
+
+	mutex_unlock(&dev->device_lock);
+
+	dev_dbg(&pdev->dev, "rpm: txe: runtime resume ret = %d\n", ret);
+
+	return ret;
+}
+
+/**
+ * mei_txe_set_pm_domain - fill and set pm domian stucture for device
+ *
+ * @dev: mei_device
+ */
+static inline void mei_txe_set_pm_domain(struct mei_device *dev)
+{
+	struct pci_dev *pdev  = dev->pdev;
+
+	if (pdev->dev.bus && pdev->dev.bus->pm) {
+		dev->pg_domain.ops = *pdev->dev.bus->pm;
+
+		dev->pg_domain.ops.runtime_suspend = mei_txe_pm_runtime_suspend;
+		dev->pg_domain.ops.runtime_resume = mei_txe_pm_runtime_resume;
+		dev->pg_domain.ops.runtime_idle = mei_txe_pm_runtime_idle;
+
+		pdev->dev.pm_domain = &dev->pg_domain;
+	}
+}
 
-static SIMPLE_DEV_PM_OPS(mei_txe_pm_ops,
-			 mei_txe_pci_suspend,
-			 mei_txe_pci_resume);
+/**
+ * mei_txe_unset_pm_domain - clean pm domian stucture for device
+ *
+ * @dev: mei_device
+ */
+static inline void mei_txe_unset_pm_domain(struct mei_device *dev)
+{
+	/* stop using pm callbacks if any */
+	dev->pdev->dev.pm_domain = NULL;
+}
+#endif /* CONFIG_PM_RUNTIME */
+
+#ifdef CONFIG_PM
+static const struct dev_pm_ops mei_txe_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(mei_txe_pci_suspend,
+				mei_txe_pci_resume)
+	SET_RUNTIME_PM_OPS(
+		mei_txe_pm_runtime_suspend,
+		mei_txe_pm_runtime_resume,
+		mei_txe_pm_runtime_idle)
+};
 
 #define MEI_TXE_PM_OPS	(&mei_txe_pm_ops)
 #else
 #define MEI_TXE_PM_OPS	NULL
-#endif /* CONFIG_PM_SLEEP */
+#endif /* CONFIG_PM */
+
 /*
  *  PCI driver structure
  */
diff --git a/drivers/misc/mei/wd.c b/drivers/misc/mei/wd.c
index ebf1cbc198f..a84a664dfcc 100644
--- a/drivers/misc/mei/wd.c
+++ b/drivers/misc/mei/wd.c
@@ -84,8 +84,6 @@ int mei_wd_host_init(struct mei_device *dev)
 		return ret;
 	}
 
-	cl->state = MEI_FILE_CONNECTING;
-
 	ret = mei_cl_connect(cl, NULL);
 
 	if (ret) {
diff --git a/drivers/misc/vexpress-syscfg.c b/drivers/misc/vexpress-syscfg.c
new file mode 100644
index 00000000000..73068e50e56
--- /dev/null
+++ b/drivers/misc/vexpress-syscfg.c
@@ -0,0 +1,324 @@
+/*
+ * 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) 2014 ARM Limited
+ */
+
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/syscore_ops.h>
+#include <linux/vexpress.h>
+
+
+#define SYS_CFGDATA		0x0
+
+#define SYS_CFGCTRL		0x4
+#define SYS_CFGCTRL_START	(1 << 31)
+#define SYS_CFGCTRL_WRITE	(1 << 30)
+#define SYS_CFGCTRL_DCC(n)	(((n) & 0xf) << 26)
+#define SYS_CFGCTRL_FUNC(n)	(((n) & 0x3f) << 20)
+#define SYS_CFGCTRL_SITE(n)	(((n) & 0x3) << 16)
+#define SYS_CFGCTRL_POSITION(n)	(((n) & 0xf) << 12)
+#define SYS_CFGCTRL_DEVICE(n)	(((n) & 0xfff) << 0)
+
+#define SYS_CFGSTAT		0x8
+#define SYS_CFGSTAT_ERR		(1 << 1)
+#define SYS_CFGSTAT_COMPLETE	(1 << 0)
+
+
+struct vexpress_syscfg {
+	struct device *dev;
+	void __iomem *base;
+	struct list_head funcs;
+};
+
+struct vexpress_syscfg_func {
+	struct list_head list;
+	struct vexpress_syscfg *syscfg;
+	struct regmap *regmap;
+	int num_templates;
+	u32 template[0]; /* Keep it last! */
+};
+
+
+static int vexpress_syscfg_exec(struct vexpress_syscfg_func *func,
+		int index, bool write, u32 *data)
+{
+	struct vexpress_syscfg *syscfg = func->syscfg;
+	u32 command, status;
+	int tries;
+	long timeout;
+
+	if (WARN_ON(index > func->num_templates))
+		return -EINVAL;
+
+	command = readl(syscfg->base + SYS_CFGCTRL);
+	if (WARN_ON(command & SYS_CFGCTRL_START))
+		return -EBUSY;
+
+	command = func->template[index];
+	command |= SYS_CFGCTRL_START;
+	command |= write ? SYS_CFGCTRL_WRITE : 0;
+
+	/* Use a canary for reads */
+	if (!write)
+		*data = 0xdeadbeef;
+
+	dev_dbg(syscfg->dev, "func %p, command %x, data %x\n",
+			func, command, *data);
+	writel(*data, syscfg->base + SYS_CFGDATA);
+	writel(0, syscfg->base + SYS_CFGSTAT);
+	writel(command, syscfg->base + SYS_CFGCTRL);
+	mb();
+
+	/* The operation can take ages... Go to sleep, 100us initially */
+	tries = 100;
+	timeout = 100;
+	do {
+		if (!irqs_disabled()) {
+			set_current_state(TASK_INTERRUPTIBLE);
+			schedule_timeout(usecs_to_jiffies(timeout));
+			if (signal_pending(current))
+				return -EINTR;
+		} else {
+			udelay(timeout);
+		}
+
+		status = readl(syscfg->base + SYS_CFGSTAT);
+		if (status & SYS_CFGSTAT_ERR)
+			return -EFAULT;
+
+		if (timeout > 20)
+			timeout -= 20;
+	} while (--tries && !(status & SYS_CFGSTAT_COMPLETE));
+	if (WARN_ON_ONCE(!tries))
+		return -ETIMEDOUT;
+
+	if (!write) {
+		*data = readl(syscfg->base + SYS_CFGDATA);
+		dev_dbg(syscfg->dev, "func %p, read data %x\n", func, *data);
+	}
+
+	return 0;
+}
+
+static int vexpress_syscfg_read(void *context, unsigned int index,
+		unsigned int *val)
+{
+	struct vexpress_syscfg_func *func = context;
+
+	return vexpress_syscfg_exec(func, index, false, val);
+}
+
+static int vexpress_syscfg_write(void *context, unsigned int index,
+		unsigned int val)
+{
+	struct vexpress_syscfg_func *func = context;
+
+	return vexpress_syscfg_exec(func, index, true, &val);
+}
+
+struct regmap_config vexpress_syscfg_regmap_config = {
+	.lock = vexpress_config_lock,
+	.unlock = vexpress_config_unlock,
+	.reg_bits = 32,
+	.val_bits = 32,
+	.reg_read = vexpress_syscfg_read,
+	.reg_write = vexpress_syscfg_write,
+	.reg_format_endian = REGMAP_ENDIAN_LITTLE,
+	.val_format_endian = REGMAP_ENDIAN_LITTLE,
+};
+
+
+static struct regmap *vexpress_syscfg_regmap_init(struct device *dev,
+		void *context)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct vexpress_syscfg *syscfg = context;
+	struct vexpress_syscfg_func *func;
+	struct property *prop;
+	const __be32 *val = NULL;
+	__be32 energy_quirk[4];
+	int num;
+	u32 site, position, dcc;
+	int i;
+
+	if (dev->of_node) {
+		int err = vexpress_config_get_topo(dev->of_node, &site,
+				&position, &dcc);
+
+		if (err)
+			return ERR_PTR(err);
+
+		prop = of_find_property(dev->of_node,
+				"arm,vexpress-sysreg,func", NULL);
+		if (!prop)
+			return ERR_PTR(-EINVAL);
+
+		num = prop->length / sizeof(u32) / 2;
+		val = prop->value;
+	} else {
+		if (pdev->num_resources != 1 ||
+				pdev->resource[0].flags != IORESOURCE_BUS)
+			return ERR_PTR(-EFAULT);
+
+		site = pdev->resource[0].start;
+		if (site == VEXPRESS_SITE_MASTER)
+			site = vexpress_config_get_master();
+		position = 0;
+		dcc = 0;
+		num = 1;
+	}
+
+	/*
+	 * "arm,vexpress-energy" function used to be described
+	 * by its first device only, now it requires both
+	 */
+	if (num == 1 && of_device_is_compatible(dev->of_node,
+			"arm,vexpress-energy")) {
+		num = 2;
+		energy_quirk[0] = *val;
+		energy_quirk[2] = *val++;
+		energy_quirk[1] = *val;
+		energy_quirk[3] = cpu_to_be32(be32_to_cpup(val) + 1);
+		val = energy_quirk;
+	}
+
+	func = kzalloc(sizeof(*func) + sizeof(*func->template) * num,
+			GFP_KERNEL);
+	if (!func)
+		return NULL;
+
+	func->syscfg = syscfg;
+	func->num_templates = num;
+
+	for (i = 0; i < num; i++) {
+		u32 function, device;
+
+		if (dev->of_node) {
+			function = be32_to_cpup(val++);
+			device = be32_to_cpup(val++);
+		} else {
+			function = pdev->resource[0].end;
+			device = pdev->id;
+		}
+
+		dev_dbg(dev, "func %p: %u/%u/%u/%u/%u\n",
+				func, site, position, dcc,
+				function, device);
+
+		func->template[i] = SYS_CFGCTRL_DCC(dcc);
+		func->template[i] |= SYS_CFGCTRL_SITE(site);
+		func->template[i] |= SYS_CFGCTRL_POSITION(position);
+		func->template[i] |= SYS_CFGCTRL_FUNC(function);
+		func->template[i] |= SYS_CFGCTRL_DEVICE(device);
+	}
+
+	vexpress_syscfg_regmap_config.max_register = num - 1;
+
+	func->regmap = regmap_init(dev, NULL, func,
+			&vexpress_syscfg_regmap_config);
+
+	if (IS_ERR(func->regmap))
+		kfree(func);
+	else
+		list_add(&func->list, &syscfg->funcs);
+
+	return func->regmap;
+}
+
+static void vexpress_syscfg_regmap_exit(struct regmap *regmap, void *context)
+{
+	struct vexpress_syscfg *syscfg = context;
+	struct vexpress_syscfg_func *func, *tmp;
+
+	regmap_exit(regmap);
+
+	list_for_each_entry_safe(func, tmp, &syscfg->funcs, list) {
+		if (func->regmap == regmap) {
+			list_del(&syscfg->funcs);
+			kfree(func);
+			break;
+		}
+	}
+}
+
+static struct vexpress_config_bridge_ops vexpress_syscfg_bridge_ops = {
+	.regmap_init = vexpress_syscfg_regmap_init,
+	.regmap_exit = vexpress_syscfg_regmap_exit,
+};
+
+
+/* Non-DT hack, to be gone... */
+static struct device *vexpress_syscfg_bridge;
+
+int vexpress_syscfg_device_register(struct platform_device *pdev)
+{
+	pdev->dev.parent = vexpress_syscfg_bridge;
+
+	return platform_device_register(pdev);
+}
+
+
+int vexpress_syscfg_probe(struct platform_device *pdev)
+{
+	struct vexpress_syscfg *syscfg;
+	struct resource *res;
+	struct device *bridge;
+
+	syscfg = devm_kzalloc(&pdev->dev, sizeof(*syscfg), GFP_KERNEL);
+	if (!syscfg)
+		return -ENOMEM;
+	syscfg->dev = &pdev->dev;
+	INIT_LIST_HEAD(&syscfg->funcs);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!devm_request_mem_region(&pdev->dev, res->start,
+			resource_size(res), pdev->name))
+		return -EBUSY;
+
+	syscfg->base = devm_ioremap(&pdev->dev, res->start, resource_size(res));
+	if (!syscfg->base)
+		return -EFAULT;
+
+	/* Must use dev.parent (MFD), as that's where DT phandle points at... */
+	bridge = vexpress_config_bridge_register(pdev->dev.parent,
+			&vexpress_syscfg_bridge_ops, syscfg);
+	if (IS_ERR(bridge))
+		return PTR_ERR(bridge);
+
+	/* Non-DT case */
+	if (!pdev->dev.of_node)
+		vexpress_syscfg_bridge = bridge;
+
+	return 0;
+}
+
+static const struct platform_device_id vexpress_syscfg_id_table[] = {
+	{ "vexpress-syscfg", },
+	{},
+};
+
+static struct platform_driver vexpress_syscfg_driver = {
+	.driver.name = "vexpress-syscfg",
+	.id_table = vexpress_syscfg_id_table,
+	.probe = vexpress_syscfg_probe,
+};
+
+static int __init vexpress_syscfg_init(void)
+{
+	return platform_driver_register(&vexpress_syscfg_driver);
+}
+core_initcall(vexpress_syscfg_init);
diff --git a/drivers/mmc/host/mmci.c b/drivers/mmc/host/mmci.c
index 771c60ab4a3..a084edd37af 100644
--- a/drivers/mmc/host/mmci.c
+++ b/drivers/mmc/host/mmci.c
@@ -13,6 +13,7 @@
 #include <linux/init.h>
 #include <linux/ioport.h>
 #include <linux/device.h>
+#include <linux/io.h>
 #include <linux/interrupt.h>
 #include <linux/kernel.h>
 #include <linux/slab.h>
@@ -23,6 +24,7 @@
 #include <linux/mmc/pm.h>
 #include <linux/mmc/host.h>
 #include <linux/mmc/card.h>
+#include <linux/mmc/slot-gpio.h>
 #include <linux/amba/bus.h>
 #include <linux/clk.h>
 #include <linux/scatterlist.h>
@@ -364,7 +366,6 @@ static void mmci_init_sg(struct mmci_host *host, struct mmc_data *data)
 #ifdef CONFIG_DMA_ENGINE
 static void mmci_dma_setup(struct mmci_host *host)
 {
-	struct mmci_platform_data *plat = host->plat;
 	const char *rxname, *txname;
 	dma_cap_mask_t mask;
 
@@ -378,25 +379,6 @@ static void mmci_dma_setup(struct mmci_host *host)
 	dma_cap_zero(mask);
 	dma_cap_set(DMA_SLAVE, mask);
 
-	if (plat && plat->dma_filter) {
-		if (!host->dma_rx_channel && plat->dma_rx_param) {
-			host->dma_rx_channel = dma_request_channel(mask,
-							   plat->dma_filter,
-							   plat->dma_rx_param);
-			/* E.g if no DMA hardware is present */
-			if (!host->dma_rx_channel)
-				dev_err(mmc_dev(host->mmc), "no RX DMA channel\n");
-		}
-
-		if (!host->dma_tx_channel && plat->dma_tx_param) {
-			host->dma_tx_channel = dma_request_channel(mask,
-							   plat->dma_filter,
-							   plat->dma_tx_param);
-			if (!host->dma_tx_channel)
-				dev_warn(mmc_dev(host->mmc), "no TX DMA channel\n");
-		}
-	}
-
 	/*
 	 * If only an RX channel is specified, the driver will
 	 * attempt to use it bidirectionally, however if it is
@@ -444,11 +426,9 @@ static void mmci_dma_setup(struct mmci_host *host)
  */
 static inline void mmci_dma_release(struct mmci_host *host)
 {
-	struct mmci_platform_data *plat = host->plat;
-
 	if (host->dma_rx_channel)
 		dma_release_channel(host->dma_rx_channel);
-	if (host->dma_tx_channel && plat->dma_tx_param)
+	if (host->dma_tx_channel)
 		dma_release_channel(host->dma_tx_channel);
 	host->dma_rx_channel = host->dma_tx_channel = NULL;
 }
@@ -1285,7 +1265,7 @@ static void mmci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 		 * indicating signal direction for the signals in
 		 * the SD/MMC bus and feedback-clock usage.
 		 */
-		pwr |= host->plat->sigdir;
+		pwr |= host->pwr_reg_add;
 
 		if (ios->bus_width == MMC_BUS_WIDTH_4)
 			pwr &= ~MCI_ST_DATA74DIREN;
@@ -1326,35 +1306,18 @@ static void mmci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 	pm_runtime_put_autosuspend(mmc_dev(mmc));
 }
 
-static int mmci_get_ro(struct mmc_host *mmc)
-{
-	struct mmci_host *host = mmc_priv(mmc);
-
-	if (host->gpio_wp == -ENOSYS)
-		return -ENOSYS;
-
-	return gpio_get_value_cansleep(host->gpio_wp);
-}
-
 static int mmci_get_cd(struct mmc_host *mmc)
 {
 	struct mmci_host *host = mmc_priv(mmc);
 	struct mmci_platform_data *plat = host->plat;
-	unsigned int status;
+	unsigned int status = mmc_gpio_get_cd(mmc);
 
-	if (host->gpio_cd == -ENOSYS) {
+	if (status == -ENOSYS) {
 		if (!plat->status)
 			return 1; /* Assume always present */
 
 		status = plat->status(mmc_dev(host->mmc));
-	} else
-		status = !!gpio_get_value_cansleep(host->gpio_cd)
-			^ plat->cd_invert;
-
-	/*
-	 * Use positive logic throughout - status is zero for no card,
-	 * non-zero for card inserted.
-	 */
+	}
 	return status;
 }
 
@@ -1391,70 +1354,44 @@ static int mmci_sig_volt_switch(struct mmc_host *mmc, struct mmc_ios *ios)
 	return ret;
 }
 
-static irqreturn_t mmci_cd_irq(int irq, void *dev_id)
-{
-	struct mmci_host *host = dev_id;
-
-	mmc_detect_change(host->mmc, msecs_to_jiffies(500));
-
-	return IRQ_HANDLED;
-}
-
 static struct mmc_host_ops mmci_ops = {
 	.request	= mmci_request,
 	.pre_req	= mmci_pre_request,
 	.post_req	= mmci_post_request,
 	.set_ios	= mmci_set_ios,
-	.get_ro		= mmci_get_ro,
+	.get_ro		= mmc_gpio_get_ro,
 	.get_cd		= mmci_get_cd,
 	.start_signal_voltage_switch = mmci_sig_volt_switch,
 };
 
-#ifdef CONFIG_OF
-static void mmci_dt_populate_generic_pdata(struct device_node *np,
-					struct mmci_platform_data *pdata)
+static int mmci_of_parse(struct device_node *np, struct mmc_host *mmc)
 {
-	int bus_width = 0;
-
-	pdata->gpio_wp = of_get_named_gpio(np, "wp-gpios", 0);
-	pdata->gpio_cd = of_get_named_gpio(np, "cd-gpios", 0);
+	struct mmci_host *host = mmc_priv(mmc);
+	int ret = mmc_of_parse(mmc);
 
-	if (of_get_property(np, "cd-inverted", NULL))
-		pdata->cd_invert = true;
-	else
-		pdata->cd_invert = false;
+	if (ret)
+		return ret;
 
-	of_property_read_u32(np, "max-frequency", &pdata->f_max);
-	if (!pdata->f_max)
-		pr_warn("%s has no 'max-frequency' property\n", np->full_name);
+	if (of_get_property(np, "st,sig-dir-dat0", NULL))
+		host->pwr_reg_add |= MCI_ST_DATA0DIREN;
+	if (of_get_property(np, "st,sig-dir-dat2", NULL))
+		host->pwr_reg_add |= MCI_ST_DATA2DIREN;
+	if (of_get_property(np, "st,sig-dir-dat31", NULL))
+		host->pwr_reg_add |= MCI_ST_DATA31DIREN;
+	if (of_get_property(np, "st,sig-dir-dat74", NULL))
+		host->pwr_reg_add |= MCI_ST_DATA74DIREN;
+	if (of_get_property(np, "st,sig-dir-cmd", NULL))
+		host->pwr_reg_add |= MCI_ST_CMDDIREN;
+	if (of_get_property(np, "st,sig-pin-fbclk", NULL))
+		host->pwr_reg_add |= MCI_ST_FBCLKEN;
 
 	if (of_get_property(np, "mmc-cap-mmc-highspeed", NULL))
-		pdata->capabilities |= MMC_CAP_MMC_HIGHSPEED;
+		mmc->caps |= MMC_CAP_MMC_HIGHSPEED;
 	if (of_get_property(np, "mmc-cap-sd-highspeed", NULL))
-		pdata->capabilities |= MMC_CAP_SD_HIGHSPEED;
+		mmc->caps |= MMC_CAP_SD_HIGHSPEED;
 
-	of_property_read_u32(np, "bus-width", &bus_width);
-	switch (bus_width) {
-	case 0 :
-		/* No bus-width supplied. */
-		break;
-	case 4 :
-		pdata->capabilities |= MMC_CAP_4_BIT_DATA;
-		break;
-	case 8 :
-		pdata->capabilities |= MMC_CAP_8_BIT_DATA;
-		break;
-	default :
-		pr_warn("%s: Unsupported bus width\n", np->full_name);
-	}
-}
-#else
-static void mmci_dt_populate_generic_pdata(struct device_node *np,
-					struct mmci_platform_data *pdata)
-{
-	return;
+	return 0;
 }
-#endif
 
 static int mmci_probe(struct amba_device *dev,
 	const struct amba_id *id)
@@ -1478,26 +1415,17 @@ static int mmci_probe(struct amba_device *dev,
 			return -ENOMEM;
 	}
 
-	if (np)
-		mmci_dt_populate_generic_pdata(np, plat);
+	mmc = mmc_alloc_host(sizeof(struct mmci_host), &dev->dev);
+	if (!mmc)
+		return -ENOMEM;
 
-	ret = amba_request_regions(dev, DRIVER_NAME);
+	ret = mmci_of_parse(np, mmc);
 	if (ret)
-		goto out;
-
-	mmc = mmc_alloc_host(sizeof(struct mmci_host), &dev->dev);
-	if (!mmc) {
-		ret = -ENOMEM;
-		goto rel_regions;
-	}
+		goto host_free;
 
 	host = mmc_priv(mmc);
 	host->mmc = mmc;
 
-	host->gpio_wp = -ENOSYS;
-	host->gpio_cd = -ENOSYS;
-	host->gpio_cd_irq = -1;
-
 	host->hw_designer = amba_manf(dev);
 	host->hw_revision = amba_rev(dev);
 	dev_dbg(mmc_dev(mmc), "designer ID = 0x%02x\n", host->hw_designer);
@@ -1529,10 +1457,11 @@ static int mmci_probe(struct amba_device *dev,
 		dev_dbg(mmc_dev(mmc), "eventual mclk rate: %u Hz\n",
 			host->mclk);
 	}
+
 	host->phybase = dev->res.start;
-	host->base = ioremap(dev->res.start, resource_size(&dev->res));
-	if (!host->base) {
-		ret = -ENOMEM;
+	host->base = devm_ioremap_resource(&dev->dev, &dev->res);
+	if (IS_ERR(host->base)) {
+		ret = PTR_ERR(host->base);
 		goto clk_disable;
 	}
 
@@ -1546,15 +1475,13 @@ static int mmci_probe(struct amba_device *dev,
 	else
 		mmc->f_min = DIV_ROUND_UP(host->mclk, 512);
 	/*
-	 * If the platform data supplies a maximum operating
-	 * frequency, this takes precedence. Else, we fall back
-	 * to using the module parameter, which has a (low)
-	 * default value in case it is not specified. Either
-	 * value must not exceed the clock rate into the block,
-	 * of course.
+	 * If no maximum operating frequency is supplied, fall back to use
+	 * the module parameter, which has a (low) default value in case it
+	 * is not specified. Either value must not exceed the clock rate into
+	 * the block, of course.
 	 */
-	if (plat->f_max)
-		mmc->f_max = min(host->mclk, plat->f_max);
+	if (mmc->f_max)
+		mmc->f_max = min(host->mclk, mmc->f_max);
 	else
 		mmc->f_max = min(host->mclk, fmax);
 	dev_dbg(mmc_dev(mmc), "clocking block at %u Hz\n", mmc->f_max);
@@ -1566,8 +1493,15 @@ static int mmci_probe(struct amba_device *dev,
 	else if (plat->ocr_mask)
 		dev_warn(mmc_dev(mmc), "Platform OCR mask is ignored\n");
 
-	mmc->caps = plat->capabilities;
-	mmc->caps2 = plat->capabilities2;
+	/* DT takes precedence over platform data. */
+	if (!np) {
+		if (!plat->cd_invert)
+			mmc->caps2 |= MMC_CAP2_CD_ACTIVE_HIGH;
+		mmc->caps2 |= MMC_CAP2_RO_ACTIVE_HIGH;
+	}
+
+	/* We support these capabilities. */
+	mmc->caps |= MMC_CAP_CMD23;
 
 	if (variant->busy_detect) {
 		mmci_ops.card_busy = mmci_card_busy;
@@ -1579,7 +1513,7 @@ static int mmci_probe(struct amba_device *dev,
 	mmc->ops = &mmci_ops;
 
 	/* We support these PM capabilities. */
-	mmc->pm_caps = MMC_PM_KEEP_POWER;
+	mmc->pm_caps |= MMC_PM_KEEP_POWER;
 
 	/*
 	 * We can do SGIO
@@ -1616,62 +1550,30 @@ static int mmci_probe(struct amba_device *dev,
 	writel(0, host->base + MMCIMASK1);
 	writel(0xfff, host->base + MMCICLEAR);
 
-	if (plat->gpio_cd == -EPROBE_DEFER) {
-		ret = -EPROBE_DEFER;
-		goto err_gpio_cd;
-	}
-	if (gpio_is_valid(plat->gpio_cd)) {
-		ret = gpio_request(plat->gpio_cd, DRIVER_NAME " (cd)");
-		if (ret == 0)
-			ret = gpio_direction_input(plat->gpio_cd);
-		if (ret == 0)
-			host->gpio_cd = plat->gpio_cd;
-		else if (ret != -ENOSYS)
-			goto err_gpio_cd;
-
-		/*
-		 * A gpio pin that will detect cards when inserted and removed
-		 * will most likely want to trigger on the edges if it is
-		 * 0 when ejected and 1 when inserted (or mutatis mutandis
-		 * for the inverted case) so we request triggers on both
-		 * edges.
-		 */
-		ret = request_any_context_irq(gpio_to_irq(plat->gpio_cd),
-				mmci_cd_irq,
-				IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
-				DRIVER_NAME " (cd)", host);
-		if (ret >= 0)
-			host->gpio_cd_irq = gpio_to_irq(plat->gpio_cd);
-	}
-	if (plat->gpio_wp == -EPROBE_DEFER) {
-		ret = -EPROBE_DEFER;
-		goto err_gpio_wp;
+	/* If DT, cd/wp gpios must be supplied through it. */
+	if (!np && gpio_is_valid(plat->gpio_cd)) {
+		ret = mmc_gpio_request_cd(mmc, plat->gpio_cd, 0);
+		if (ret)
+			goto clk_disable;
 	}
-	if (gpio_is_valid(plat->gpio_wp)) {
-		ret = gpio_request(plat->gpio_wp, DRIVER_NAME " (wp)");
-		if (ret == 0)
-			ret = gpio_direction_input(plat->gpio_wp);
-		if (ret == 0)
-			host->gpio_wp = plat->gpio_wp;
-		else if (ret != -ENOSYS)
-			goto err_gpio_wp;
+	if (!np && gpio_is_valid(plat->gpio_wp)) {
+		ret = mmc_gpio_request_ro(mmc, plat->gpio_wp);
+		if (ret)
+			goto clk_disable;
 	}
 
-	if ((host->plat->status || host->gpio_cd != -ENOSYS)
-	    && host->gpio_cd_irq < 0)
-		mmc->caps |= MMC_CAP_NEEDS_POLL;
-
-	ret = request_irq(dev->irq[0], mmci_irq, IRQF_SHARED, DRIVER_NAME " (cmd)", host);
+	ret = devm_request_irq(&dev->dev, dev->irq[0], mmci_irq, IRQF_SHARED,
+			DRIVER_NAME " (cmd)", host);
 	if (ret)
-		goto unmap;
+		goto clk_disable;
 
 	if (!dev->irq[1])
 		host->singleirq = true;
 	else {
-		ret = request_irq(dev->irq[1], mmci_pio_irq, IRQF_SHARED,
-				  DRIVER_NAME " (pio)", host);
+		ret = devm_request_irq(&dev->dev, dev->irq[1], mmci_pio_irq,
+				IRQF_SHARED, DRIVER_NAME " (pio)", host);
 		if (ret)
-			goto irq0_free;
+			goto clk_disable;
 	}
 
 	writel(MCI_IRQENABLE, host->base + MMCIMASK0);
@@ -1693,25 +1595,10 @@ static int mmci_probe(struct amba_device *dev,
 
 	return 0;
 
- irq0_free:
-	free_irq(dev->irq[0], host);
- unmap:
-	if (host->gpio_wp != -ENOSYS)
-		gpio_free(host->gpio_wp);
- err_gpio_wp:
-	if (host->gpio_cd_irq >= 0)
-		free_irq(host->gpio_cd_irq, host);
-	if (host->gpio_cd != -ENOSYS)
-		gpio_free(host->gpio_cd);
- err_gpio_cd:
-	iounmap(host->base);
  clk_disable:
 	clk_disable_unprepare(host->clk);
  host_free:
 	mmc_free_host(mmc);
- rel_regions:
-	amba_release_regions(dev);
- out:
 	return ret;
 }
 
@@ -1737,92 +1624,46 @@ static int mmci_remove(struct amba_device *dev)
 		writel(0, host->base + MMCIDATACTRL);
 
 		mmci_dma_release(host);
-		free_irq(dev->irq[0], host);
-		if (!host->singleirq)
-			free_irq(dev->irq[1], host);
-
-		if (host->gpio_wp != -ENOSYS)
-			gpio_free(host->gpio_wp);
-		if (host->gpio_cd_irq >= 0)
-			free_irq(host->gpio_cd_irq, host);
-		if (host->gpio_cd != -ENOSYS)
-			gpio_free(host->gpio_cd);
-
-		iounmap(host->base);
 		clk_disable_unprepare(host->clk);
-
 		mmc_free_host(mmc);
-
-		amba_release_regions(dev);
-	}
-
-	return 0;
-}
-
-#ifdef CONFIG_SUSPEND
-static int mmci_suspend(struct device *dev)
-{
-	struct amba_device *adev = to_amba_device(dev);
-	struct mmc_host *mmc = amba_get_drvdata(adev);
-
-	if (mmc) {
-		struct mmci_host *host = mmc_priv(mmc);
-		pm_runtime_get_sync(dev);
-		writel(0, host->base + MMCIMASK0);
 	}
 
 	return 0;
 }
 
-static int mmci_resume(struct device *dev)
-{
-	struct amba_device *adev = to_amba_device(dev);
-	struct mmc_host *mmc = amba_get_drvdata(adev);
-
-	if (mmc) {
-		struct mmci_host *host = mmc_priv(mmc);
-		writel(MCI_IRQENABLE, host->base + MMCIMASK0);
-		pm_runtime_put(dev);
-	}
-
-	return 0;
-}
-#endif
-
-#ifdef CONFIG_PM_RUNTIME
+#ifdef CONFIG_PM
 static void mmci_save(struct mmci_host *host)
 {
 	unsigned long flags;
 
-	if (host->variant->pwrreg_nopower) {
-		spin_lock_irqsave(&host->lock, flags);
+	spin_lock_irqsave(&host->lock, flags);
 
-		writel(0, host->base + MMCIMASK0);
+	writel(0, host->base + MMCIMASK0);
+	if (host->variant->pwrreg_nopower) {
 		writel(0, host->base + MMCIDATACTRL);
 		writel(0, host->base + MMCIPOWER);
 		writel(0, host->base + MMCICLOCK);
-		mmci_reg_delay(host);
-
-		spin_unlock_irqrestore(&host->lock, flags);
 	}
+	mmci_reg_delay(host);
 
+	spin_unlock_irqrestore(&host->lock, flags);
 }
 
 static void mmci_restore(struct mmci_host *host)
 {
 	unsigned long flags;
 
-	if (host->variant->pwrreg_nopower) {
-		spin_lock_irqsave(&host->lock, flags);
+	spin_lock_irqsave(&host->lock, flags);
 
+	if (host->variant->pwrreg_nopower) {
 		writel(host->clk_reg, host->base + MMCICLOCK);
 		writel(host->datactrl_reg, host->base + MMCIDATACTRL);
 		writel(host->pwr_reg, host->base + MMCIPOWER);
-		writel(MCI_IRQENABLE, host->base + MMCIMASK0);
-		mmci_reg_delay(host);
-
-		spin_unlock_irqrestore(&host->lock, flags);
 	}
+	writel(MCI_IRQENABLE, host->base + MMCIMASK0);
+	mmci_reg_delay(host);
+
+	spin_unlock_irqrestore(&host->lock, flags);
 }
 
 static int mmci_runtime_suspend(struct device *dev)
@@ -1857,8 +1698,9 @@ static int mmci_runtime_resume(struct device *dev)
 #endif
 
 static const struct dev_pm_ops mmci_dev_pm_ops = {
-	SET_SYSTEM_SLEEP_PM_OPS(mmci_suspend, mmci_resume)
-	SET_RUNTIME_PM_OPS(mmci_runtime_suspend, mmci_runtime_resume, NULL)
+	SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
+				pm_runtime_force_resume)
+	SET_PM_RUNTIME_PM_OPS(mmci_runtime_suspend, mmci_runtime_resume, NULL)
 };
 
 static struct amba_id mmci_ids[] = {
diff --git a/drivers/mmc/host/mmci.h b/drivers/mmc/host/mmci.h
index 58b1b8896bf..347d942d740 100644
--- a/drivers/mmc/host/mmci.h
+++ b/drivers/mmc/host/mmci.h
@@ -13,6 +13,16 @@
 #define MCI_PWR_ON		0x03
 #define MCI_OD			(1 << 6)
 #define MCI_ROD			(1 << 7)
+/*
+ * The ST Micro version does not have ROD and reuse the voltage registers for
+ * direction settings.
+ */
+#define MCI_ST_DATA2DIREN	(1 << 2)
+#define MCI_ST_CMDDIREN		(1 << 3)
+#define MCI_ST_DATA0DIREN	(1 << 4)
+#define MCI_ST_DATA31DIREN	(1 << 5)
+#define MCI_ST_FBCLKEN		(1 << 7)
+#define MCI_ST_DATA74DIREN	(1 << 8)
 
 #define MMCICLOCK		0x004
 #define MCI_CLK_ENABLE		(1 << 8)
@@ -176,9 +186,6 @@ struct mmci_host {
 	struct mmc_data		*data;
 	struct mmc_host		*mmc;
 	struct clk		*clk;
-	int			gpio_cd;
-	int			gpio_wp;
-	int			gpio_cd_irq;
 	bool			singleirq;
 
 	spinlock_t		lock;
@@ -186,6 +193,7 @@ struct mmci_host {
 	unsigned int		mclk;
 	unsigned int		cclk;
 	u32			pwr_reg;
+	u32			pwr_reg_add;
 	u32			clk_reg;
 	u32			datactrl_reg;
 	u32			busy_status;
diff --git a/drivers/mmc/host/rtsx_pci_sdmmc.c b/drivers/mmc/host/rtsx_pci_sdmmc.c
index 5fb994f9a65..0b9ded13a3a 100644
--- a/drivers/mmc/host/rtsx_pci_sdmmc.c
+++ b/drivers/mmc/host/rtsx_pci_sdmmc.c
@@ -31,28 +31,14 @@
 #include <linux/mfd/rtsx_pci.h>
 #include <asm/unaligned.h>
 
-struct realtek_next {
-	unsigned int	sg_count;
-	s32		cookie;
-};
-
 struct realtek_pci_sdmmc {
 	struct platform_device	*pdev;
 	struct rtsx_pcr		*pcr;
 	struct mmc_host		*mmc;
 	struct mmc_request	*mrq;
-	struct mmc_command	*cmd;
-	struct mmc_data		*data;
-
-	spinlock_t		lock;
-	struct timer_list	timer;
-	struct tasklet_struct	cmd_tasklet;
-	struct tasklet_struct	data_tasklet;
-	struct tasklet_struct	finish_tasklet;
-
-	u8			rsp_type;
-	u8			rsp_len;
-	int			sg_count;
+
+	struct mutex		host_mutex;
+
 	u8			ssc_depth;
 	unsigned int		clock;
 	bool			vpclk;
@@ -62,13 +48,8 @@ struct realtek_pci_sdmmc {
 	int			power_state;
 #define SDMMC_POWER_ON		1
 #define SDMMC_POWER_OFF		0
-
-	struct realtek_next	next_data;
 };
 
-static int sd_start_multi_rw(struct realtek_pci_sdmmc *host,
-		struct mmc_request *mrq);
-
 static inline struct device *sdmmc_dev(struct realtek_pci_sdmmc *host)
 {
 	return &(host->pdev->dev);
@@ -105,95 +86,6 @@ static void sd_print_debug_regs(struct realtek_pci_sdmmc *host)
 #define sd_print_debug_regs(host)
 #endif /* DEBUG */
 
-static void sd_isr_done_transfer(struct platform_device *pdev)
-{
-	struct realtek_pci_sdmmc *host = platform_get_drvdata(pdev);
-
-	spin_lock(&host->lock);
-	if (host->cmd)
-		tasklet_schedule(&host->cmd_tasklet);
-	if (host->data)
-		tasklet_schedule(&host->data_tasklet);
-	spin_unlock(&host->lock);
-}
-
-static void sd_request_timeout(unsigned long host_addr)
-{
-	struct realtek_pci_sdmmc *host = (struct realtek_pci_sdmmc *)host_addr;
-	unsigned long flags;
-
-	spin_lock_irqsave(&host->lock, flags);
-
-	if (!host->mrq) {
-		dev_err(sdmmc_dev(host), "error: no request exist\n");
-		goto out;
-	}
-
-	if (host->cmd)
-		host->cmd->error = -ETIMEDOUT;
-	if (host->data)
-		host->data->error = -ETIMEDOUT;
-
-	dev_dbg(sdmmc_dev(host), "timeout for request\n");
-
-out:
-	tasklet_schedule(&host->finish_tasklet);
-	spin_unlock_irqrestore(&host->lock, flags);
-}
-
-static void sd_finish_request(unsigned long host_addr)
-{
-	struct realtek_pci_sdmmc *host = (struct realtek_pci_sdmmc *)host_addr;
-	struct rtsx_pcr *pcr = host->pcr;
-	struct mmc_request *mrq;
-	struct mmc_command *cmd;
-	struct mmc_data *data;
-	unsigned long flags;
-	bool any_error;
-
-	spin_lock_irqsave(&host->lock, flags);
-
-	del_timer(&host->timer);
-	mrq = host->mrq;
-	if (!mrq) {
-		dev_err(sdmmc_dev(host), "error: no request need finish\n");
-		goto out;
-	}
-
-	cmd = mrq->cmd;
-	data = mrq->data;
-
-	any_error = (mrq->sbc && mrq->sbc->error) ||
-		(mrq->stop && mrq->stop->error) ||
-		(cmd && cmd->error) || (data && data->error);
-
-	if (any_error) {
-		rtsx_pci_stop_cmd(pcr);
-		sd_clear_error(host);
-	}
-
-	if (data) {
-		if (any_error)
-			data->bytes_xfered = 0;
-		else
-			data->bytes_xfered = data->blocks * data->blksz;
-
-		if (!data->host_cookie)
-			rtsx_pci_dma_unmap_sg(pcr, data->sg, data->sg_len,
-					data->flags & MMC_DATA_READ);
-
-	}
-
-	host->mrq = NULL;
-	host->cmd = NULL;
-	host->data = NULL;
-
-out:
-	spin_unlock_irqrestore(&host->lock, flags);
-	mutex_unlock(&pcr->pcr_mutex);
-	mmc_request_done(host->mmc, mrq);
-}
-
 static int sd_read_data(struct realtek_pci_sdmmc *host, u8 *cmd, u16 byte_cnt,
 		u8 *buf, int buf_len, int timeout)
 {
@@ -311,7 +203,8 @@ static int sd_write_data(struct realtek_pci_sdmmc *host, u8 *cmd, u16 byte_cnt,
 	return 0;
 }
 
-static void sd_send_cmd(struct realtek_pci_sdmmc *host, struct mmc_command *cmd)
+static void sd_send_cmd_get_rsp(struct realtek_pci_sdmmc *host,
+		struct mmc_command *cmd)
 {
 	struct rtsx_pcr *pcr = host->pcr;
 	u8 cmd_idx = (u8)cmd->opcode;
@@ -319,14 +212,11 @@ static void sd_send_cmd(struct realtek_pci_sdmmc *host, struct mmc_command *cmd)
 	int err = 0;
 	int timeout = 100;
 	int i;
+	u8 *ptr;
+	int stat_idx = 0;
 	u8 rsp_type;
 	int rsp_len = 5;
-	unsigned long flags;
-
-	if (host->cmd)
-		dev_err(sdmmc_dev(host), "error: cmd already exist\n");
-
-	host->cmd = cmd;
+	bool clock_toggled = false;
 
 	dev_dbg(sdmmc_dev(host), "%s: SD/MMC CMD %d, arg = 0x%08x\n",
 			__func__, cmd_idx, arg);
@@ -361,8 +251,6 @@ static void sd_send_cmd(struct realtek_pci_sdmmc *host, struct mmc_command *cmd)
 		err = -EINVAL;
 		goto out;
 	}
-	host->rsp_type = rsp_type;
-	host->rsp_len = rsp_len;
 
 	if (rsp_type == SD_RSP_TYPE_R1b)
 		timeout = 3000;
@@ -372,6 +260,8 @@ static void sd_send_cmd(struct realtek_pci_sdmmc *host, struct mmc_command *cmd)
 				0xFF, SD_CLK_TOGGLE_EN);
 		if (err < 0)
 			goto out;
+
+		clock_toggled = true;
 	}
 
 	rtsx_pci_init_cmd(pcr);
@@ -395,60 +285,25 @@ static void sd_send_cmd(struct realtek_pci_sdmmc *host, struct mmc_command *cmd)
 		/* Read data from ping-pong buffer */
 		for (i = PPBUF_BASE2; i < PPBUF_BASE2 + 16; i++)
 			rtsx_pci_add_cmd(pcr, READ_REG_CMD, (u16)i, 0, 0);
+		stat_idx = 16;
 	} else if (rsp_type != SD_RSP_TYPE_R0) {
 		/* Read data from SD_CMDx registers */
 		for (i = SD_CMD0; i <= SD_CMD4; i++)
 			rtsx_pci_add_cmd(pcr, READ_REG_CMD, (u16)i, 0, 0);
+		stat_idx = 5;
 	}
 
 	rtsx_pci_add_cmd(pcr, READ_REG_CMD, SD_STAT1, 0, 0);
 
-	mod_timer(&host->timer, jiffies + msecs_to_jiffies(timeout));
-
-	spin_lock_irqsave(&pcr->lock, flags);
-	pcr->trans_result = TRANS_NOT_READY;
-	rtsx_pci_send_cmd_no_wait(pcr);
-	spin_unlock_irqrestore(&pcr->lock, flags);
-
-	return;
-
-out:
-	cmd->error = err;
-	tasklet_schedule(&host->finish_tasklet);
-}
-
-static void sd_get_rsp(unsigned long host_addr)
-{
-	struct realtek_pci_sdmmc *host = (struct realtek_pci_sdmmc *)host_addr;
-	struct rtsx_pcr *pcr = host->pcr;
-	struct mmc_command *cmd;
-	int i, err = 0, stat_idx;
-	u8 *ptr, rsp_type;
-	unsigned long flags;
-
-	spin_lock_irqsave(&host->lock, flags);
-
-	cmd = host->cmd;
-	host->cmd = NULL;
-
-	if (!cmd) {
-		dev_err(sdmmc_dev(host), "error: cmd not exist\n");
+	err = rtsx_pci_send_cmd(pcr, timeout);
+	if (err < 0) {
+		sd_print_debug_regs(host);
+		sd_clear_error(host);
+		dev_dbg(sdmmc_dev(host),
+			"rtsx_pci_send_cmd error (err = %d)\n", err);
 		goto out;
 	}
 
-	spin_lock(&pcr->lock);
-	if (pcr->trans_result == TRANS_NO_DEVICE)
-		err = -ENODEV;
-	else if (pcr->trans_result != TRANS_RESULT_OK)
-		err = -EINVAL;
-	spin_unlock(&pcr->lock);
-
-	if (err < 0)
-		goto out;
-
-	rsp_type = host->rsp_type;
-	stat_idx = host->rsp_len;
-
 	if (rsp_type == SD_RSP_TYPE_R0) {
 		err = 0;
 		goto out;
@@ -485,106 +340,26 @@ static void sd_get_rsp(unsigned long host_addr)
 				cmd->resp[0]);
 	}
 
-	if (cmd == host->mrq->sbc) {
-		sd_send_cmd(host, host->mrq->cmd);
-		spin_unlock_irqrestore(&host->lock, flags);
-		return;
-	}
-
-	if (cmd == host->mrq->stop)
-		goto out;
-
-	if (cmd->data) {
-		sd_start_multi_rw(host, host->mrq);
-		spin_unlock_irqrestore(&host->lock, flags);
-		return;
-	}
-
 out:
 	cmd->error = err;
 
-	tasklet_schedule(&host->finish_tasklet);
-	spin_unlock_irqrestore(&host->lock, flags);
-}
-
-static int sd_pre_dma_transfer(struct realtek_pci_sdmmc *host,
-			struct mmc_data *data, struct realtek_next *next)
-{
-	struct rtsx_pcr *pcr = host->pcr;
-	int read = data->flags & MMC_DATA_READ;
-	int sg_count = 0;
-
-	if (!next && data->host_cookie &&
-		data->host_cookie != host->next_data.cookie) {
-		dev_err(sdmmc_dev(host),
-			"error: invalid cookie data[%d] host[%d]\n",
-			data->host_cookie, host->next_data.cookie);
-		data->host_cookie = 0;
-	}
-
-	if (next || (!next && data->host_cookie != host->next_data.cookie))
-		sg_count = rtsx_pci_dma_map_sg(pcr,
-				data->sg, data->sg_len, read);
-	else
-		sg_count = host->next_data.sg_count;
-
-	if (next) {
-		next->sg_count = sg_count;
-		if (++next->cookie < 0)
-			next->cookie = 1;
-		data->host_cookie = next->cookie;
-	}
-
-	return sg_count;
-}
-
-static void sdmmc_pre_req(struct mmc_host *mmc, struct mmc_request *mrq,
-		bool is_first_req)
-{
-	struct realtek_pci_sdmmc *host = mmc_priv(mmc);
-	struct mmc_data *data = mrq->data;
-
-	if (data->host_cookie) {
-		dev_err(sdmmc_dev(host),
-			"error: descard already cookie data[%d]\n",
-			data->host_cookie);
-		data->host_cookie = 0;
-	}
-
-	dev_dbg(sdmmc_dev(host), "dma sg prepared: %d\n",
-		sd_pre_dma_transfer(host, data, &host->next_data));
-}
-
-static void sdmmc_post_req(struct mmc_host *mmc, struct mmc_request *mrq,
-		int err)
-{
-	struct realtek_pci_sdmmc *host = mmc_priv(mmc);
-	struct rtsx_pcr *pcr = host->pcr;
-	struct mmc_data *data = mrq->data;
-	int read = data->flags & MMC_DATA_READ;
-
-	rtsx_pci_dma_unmap_sg(pcr, data->sg, data->sg_len, read);
-	data->host_cookie = 0;
+	if (err && clock_toggled)
+		rtsx_pci_write_register(pcr, SD_BUS_STAT,
+				SD_CLK_TOGGLE_EN | SD_CLK_FORCE_STOP, 0);
 }
 
-static int sd_start_multi_rw(struct realtek_pci_sdmmc *host,
-		struct mmc_request *mrq)
+static int sd_rw_multi(struct realtek_pci_sdmmc *host, struct mmc_request *mrq)
 {
 	struct rtsx_pcr *pcr = host->pcr;
 	struct mmc_host *mmc = host->mmc;
 	struct mmc_card *card = mmc->card;
 	struct mmc_data *data = mrq->data;
 	int uhs = mmc_card_uhs(card);
-	int read = data->flags & MMC_DATA_READ;
+	int read = (data->flags & MMC_DATA_READ) ? 1 : 0;
 	u8 cfg2, trans_mode;
 	int err;
 	size_t data_len = data->blksz * data->blocks;
 
-	if (host->data)
-		dev_err(sdmmc_dev(host), "error: data already exist\n");
-
-	host->data = data;
-
 	if (read) {
 		cfg2 = SD_CALCULATE_CRC7 | SD_CHECK_CRC16 |
 			SD_NO_WAIT_BUSY_END | SD_CHECK_CRC7 | SD_RSP_LEN_0;
@@ -635,54 +410,15 @@ static int sd_start_multi_rw(struct realtek_pci_sdmmc *host,
 	rtsx_pci_add_cmd(pcr, CHECK_REG_CMD, SD_TRANSFER,
 			SD_TRANSFER_END, SD_TRANSFER_END);
 
-	mod_timer(&host->timer, jiffies + 10 * HZ);
 	rtsx_pci_send_cmd_no_wait(pcr);
 
-	err = rtsx_pci_dma_transfer(pcr, data->sg, host->sg_count, read);
-	if (err < 0) {
-		data->error = err;
-		tasklet_schedule(&host->finish_tasklet);
-	}
-	return 0;
-}
-
-static void sd_finish_multi_rw(unsigned long host_addr)
-{
-	struct realtek_pci_sdmmc *host = (struct realtek_pci_sdmmc *)host_addr;
-	struct rtsx_pcr *pcr = host->pcr;
-	struct mmc_data *data;
-	int err = 0;
-	unsigned long flags;
-
-	spin_lock_irqsave(&host->lock, flags);
-
-	if (!host->data) {
-		dev_err(sdmmc_dev(host), "error: no data exist\n");
-		goto out;
-	}
-
-	data = host->data;
-	host->data = NULL;
-
-	if (pcr->trans_result == TRANS_NO_DEVICE)
-		err = -ENODEV;
-	else if (pcr->trans_result != TRANS_RESULT_OK)
-		err = -EINVAL;
-
+	err = rtsx_pci_transfer_data(pcr, data->sg, data->sg_len, read, 10000);
 	if (err < 0) {
-		data->error = err;
-		goto out;
-	}
-
-	if (!host->mrq->sbc && data->stop) {
-		sd_send_cmd(host, data->stop);
-		spin_unlock_irqrestore(&host->lock, flags);
-		return;
+		sd_clear_error(host);
+		return err;
 	}
 
-out:
-	tasklet_schedule(&host->finish_tasklet);
-	spin_unlock_irqrestore(&host->lock, flags);
+	return 0;
 }
 
 static inline void sd_enable_initial_mode(struct realtek_pci_sdmmc *host)
@@ -901,13 +637,6 @@ static int sd_tuning_rx(struct realtek_pci_sdmmc *host, u8 opcode)
 	return 0;
 }
 
-static inline bool sd_use_muti_rw(struct mmc_command *cmd)
-{
-	return mmc_op_multi(cmd->opcode) ||
-		(cmd->opcode == MMC_READ_SINGLE_BLOCK) ||
-		(cmd->opcode == MMC_WRITE_BLOCK);
-}
-
 static void sdmmc_request(struct mmc_host *mmc, struct mmc_request *mrq)
 {
 	struct realtek_pci_sdmmc *host = mmc_priv(mmc);
@@ -916,14 +645,6 @@ static void sdmmc_request(struct mmc_host *mmc, struct mmc_request *mrq)
 	struct mmc_data *data = mrq->data;
 	unsigned int data_size = 0;
 	int err;
-	unsigned long flags;
-
-	mutex_lock(&pcr->pcr_mutex);
-	spin_lock_irqsave(&host->lock, flags);
-
-	if (host->mrq)
-		dev_err(sdmmc_dev(host), "error: request already exist\n");
-	host->mrq = mrq;
 
 	if (host->eject) {
 		cmd->error = -ENOMEDIUM;
@@ -936,6 +657,8 @@ static void sdmmc_request(struct mmc_host *mmc, struct mmc_request *mrq)
 		goto finish;
 	}
 
+	mutex_lock(&pcr->pcr_mutex);
+
 	rtsx_pci_start_run(pcr);
 
 	rtsx_pci_switch_clock(pcr, host->clock, host->ssc_depth,
@@ -944,28 +667,46 @@ static void sdmmc_request(struct mmc_host *mmc, struct mmc_request *mrq)
 	rtsx_pci_write_register(pcr, CARD_SHARE_MODE,
 			CARD_SHARE_MASK, CARD_SHARE_48_SD);
 
+	mutex_lock(&host->host_mutex);
+	host->mrq = mrq;
+	mutex_unlock(&host->host_mutex);
+
 	if (mrq->data)
 		data_size = data->blocks * data->blksz;
 
-	if (sd_use_muti_rw(cmd))
-		host->sg_count = sd_pre_dma_transfer(host, data, NULL);
+	if (!data_size || mmc_op_multi(cmd->opcode) ||
+			(cmd->opcode == MMC_READ_SINGLE_BLOCK) ||
+			(cmd->opcode == MMC_WRITE_BLOCK)) {
+		sd_send_cmd_get_rsp(host, cmd);
 
-	if (!data_size || sd_use_muti_rw(cmd)) {
-		if (mrq->sbc)
-			sd_send_cmd(host, mrq->sbc);
-		else
-			sd_send_cmd(host, cmd);
-		spin_unlock_irqrestore(&host->lock, flags);
+		if (!cmd->error && data_size) {
+			sd_rw_multi(host, mrq);
+
+			if (mmc_op_multi(cmd->opcode) && mrq->stop)
+				sd_send_cmd_get_rsp(host, mrq->stop);
+		}
 	} else {
-		spin_unlock_irqrestore(&host->lock, flags);
 		sd_normal_rw(host, mrq);
-		tasklet_schedule(&host->finish_tasklet);
 	}
-	return;
+
+	if (mrq->data) {
+		if (cmd->error || data->error)
+			data->bytes_xfered = 0;
+		else
+			data->bytes_xfered = data->blocks * data->blksz;
+	}
+
+	mutex_unlock(&pcr->pcr_mutex);
 
 finish:
-	tasklet_schedule(&host->finish_tasklet);
-	spin_unlock_irqrestore(&host->lock, flags);
+	if (cmd->error)
+		dev_dbg(sdmmc_dev(host), "cmd->error = %d\n", cmd->error);
+
+	mutex_lock(&host->host_mutex);
+	host->mrq = NULL;
+	mutex_unlock(&host->host_mutex);
+
+	mmc_request_done(mmc, mrq);
 }
 
 static int sd_set_bus_width(struct realtek_pci_sdmmc *host,
@@ -1400,8 +1141,6 @@ out:
 }
 
 static const struct mmc_host_ops realtek_pci_sdmmc_ops = {
-	.pre_req = sdmmc_pre_req,
-	.post_req = sdmmc_post_req,
 	.request = sdmmc_request,
 	.set_ios = sdmmc_set_ios,
 	.get_ro = sdmmc_get_ro,
@@ -1465,7 +1204,6 @@ static int rtsx_pci_sdmmc_drv_probe(struct platform_device *pdev)
 	struct realtek_pci_sdmmc *host;
 	struct rtsx_pcr *pcr;
 	struct pcr_handle *handle = pdev->dev.platform_data;
-	unsigned long host_addr;
 
 	if (!handle)
 		return -ENXIO;
@@ -1489,15 +1227,8 @@ static int rtsx_pci_sdmmc_drv_probe(struct platform_device *pdev)
 	pcr->slots[RTSX_SD_CARD].p_dev = pdev;
 	pcr->slots[RTSX_SD_CARD].card_event = rtsx_pci_sdmmc_card_event;
 
-	host_addr = (unsigned long)host;
-	host->next_data.cookie = 1;
-	setup_timer(&host->timer, sd_request_timeout, host_addr);
-	tasklet_init(&host->cmd_tasklet, sd_get_rsp, host_addr);
-	tasklet_init(&host->data_tasklet, sd_finish_multi_rw, host_addr);
-	tasklet_init(&host->finish_tasklet, sd_finish_request, host_addr);
-	spin_lock_init(&host->lock);
+	mutex_init(&host->host_mutex);
 
-	pcr->slots[RTSX_SD_CARD].done_transfer = sd_isr_done_transfer;
 	realtek_init_host(host);
 
 	mmc_add_host(mmc);
@@ -1510,8 +1241,6 @@ static int rtsx_pci_sdmmc_drv_remove(struct platform_device *pdev)
 	struct realtek_pci_sdmmc *host = platform_get_drvdata(pdev);
 	struct rtsx_pcr *pcr;
 	struct mmc_host *mmc;
-	struct mmc_request *mrq;
-	unsigned long flags;
 
 	if (!host)
 		return 0;
@@ -1519,33 +1248,22 @@ static int rtsx_pci_sdmmc_drv_remove(struct platform_device *pdev)
 	pcr = host->pcr;
 	pcr->slots[RTSX_SD_CARD].p_dev = NULL;
 	pcr->slots[RTSX_SD_CARD].card_event = NULL;
-	pcr->slots[RTSX_SD_CARD].done_transfer = NULL;
 	mmc = host->mmc;
-	mrq = host->mrq;
 
-	spin_lock_irqsave(&host->lock, flags);
+	mutex_lock(&host->host_mutex);
 	if (host->mrq) {
 		dev_dbg(&(pdev->dev),
 			"%s: Controller removed during transfer\n",
 			mmc_hostname(mmc));
 
-		if (mrq->sbc)
-			mrq->sbc->error = -ENOMEDIUM;
-		if (mrq->cmd)
-			mrq->cmd->error = -ENOMEDIUM;
-		if (mrq->stop)
-			mrq->stop->error = -ENOMEDIUM;
-		if (mrq->data)
-			mrq->data->error = -ENOMEDIUM;
+		rtsx_pci_complete_unfinished_transfer(pcr);
 
-		tasklet_schedule(&host->finish_tasklet);
+		host->mrq->cmd->error = -ENOMEDIUM;
+		if (host->mrq->stop)
+			host->mrq->stop->error = -ENOMEDIUM;
+		mmc_request_done(mmc, host->mrq);
 	}
-	spin_unlock_irqrestore(&host->lock, flags);
-
-	del_timer_sync(&host->timer);
-	tasklet_kill(&host->cmd_tasklet);
-	tasklet_kill(&host->data_tasklet);
-	tasklet_kill(&host->finish_tasklet);
+	mutex_unlock(&host->host_mutex);
 
 	mmc_remove_host(mmc);
 	host->eject = true;
diff --git a/drivers/mtd/devices/spear_smi.c b/drivers/mtd/devices/spear_smi.c
index 363da96e689..c4176b0f382 100644
--- a/drivers/mtd/devices/spear_smi.c
+++ b/drivers/mtd/devices/spear_smi.c
@@ -6,7 +6,7 @@
  *
  * Copyright © 2010 STMicroelectronics.
  * Ashish Priyadarshi
- * Shiraz Hashim <shiraz.hashim@st.com>
+ * Shiraz Hashim <shiraz.linux.kernel@gmail.com>
  *
  * This file is licensed under the terms of the GNU General Public
  * License version 2. This program is licensed "as is" without any
@@ -1089,5 +1089,5 @@ static struct platform_driver spear_smi_driver = {
 module_platform_driver(spear_smi_driver);
 
 MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Ashish Priyadarshi, Shiraz Hashim <shiraz.hashim@st.com>");
+MODULE_AUTHOR("Ashish Priyadarshi, Shiraz Hashim <shiraz.linux.kernel@gmail.com>");
 MODULE_DESCRIPTION("MTD SMI driver for serial nor flash chips");
diff --git a/drivers/mtd/mtd_blkdevs.c b/drivers/mtd/mtd_blkdevs.c
index 0b2ccb68c0d..4dbfaee9aa9 100644
--- a/drivers/mtd/mtd_blkdevs.c
+++ b/drivers/mtd/mtd_blkdevs.c
@@ -82,8 +82,7 @@ static int do_blktrans_request(struct mtd_blktrans_ops *tr,
 
 	block = blk_rq_pos(req) << 9 >> tr->blkshift;
 	nsect = blk_rq_cur_bytes(req) >> tr->blkshift;
-
-	buf = req->buffer;
+	buf = bio_data(req->bio);
 
 	if (req->cmd_type != REQ_TYPE_FS)
 		return -EIO;
diff --git a/drivers/mtd/nand/davinci_nand.c b/drivers/mtd/nand/davinci_nand.c
index 4615d79fc93..b922c8efcf4 100644
--- a/drivers/mtd/nand/davinci_nand.c
+++ b/drivers/mtd/nand/davinci_nand.c
@@ -523,6 +523,7 @@ static struct nand_ecclayout hwecc4_2048 = {
 #if defined(CONFIG_OF)
 static const struct of_device_id davinci_nand_of_match[] = {
 	{.compatible = "ti,davinci-nand", },
+	{.compatible = "ti,keystone-nand", },
 	{},
 };
 MODULE_DEVICE_TABLE(of, davinci_nand_of_match);
@@ -581,6 +582,11 @@ static struct davinci_nand_pdata
 		    of_property_read_bool(pdev->dev.of_node,
 			"ti,davinci-nand-use-bbt"))
 			pdata->bbt_options = NAND_BBT_USE_FLASH;
+
+		if (of_device_is_compatible(pdev->dev.of_node,
+					    "ti,keystone-nand")) {
+			pdata->options |= NAND_NO_SUBPAGE_WRITE;
+		}
 	}
 
 	return dev_get_platdata(&pdev->dev);
diff --git a/drivers/mtd/ubi/block.c b/drivers/mtd/ubi/block.c
index 7ff473c871a..20a667c95da 100644
--- a/drivers/mtd/ubi/block.c
+++ b/drivers/mtd/ubi/block.c
@@ -253,7 +253,7 @@ static int do_ubiblock_request(struct ubiblock *dev, struct request *req)
 	 * flash access anyway.
 	 */
 	mutex_lock(&dev->dev_mutex);
-	ret = ubiblock_read(dev, req->buffer, sec, len);
+	ret = ubiblock_read(dev, bio_data(req->bio), sec, len);
 	mutex_unlock(&dev->dev_mutex);
 
 	return ret;
@@ -431,7 +431,7 @@ int ubiblock_create(struct ubi_volume_info *vi)
 	 * Create one workqueue per volume (per registered block device).
 	 * Rembember workqueues are cheap, they're not threads.
 	 */
-	dev->wq = alloc_workqueue(gd->disk_name, 0, 0);
+	dev->wq = alloc_workqueue("%s", 0, 0, gd->disk_name);
 	if (!dev->wq)
 		goto out_free_queue;
 	INIT_WORK(&dev->work, ubiblock_do_work);
diff --git a/drivers/mtd/ubi/wl.c b/drivers/mtd/ubi/wl.c
index 02317c1c023..0f3425dac91 100644
--- a/drivers/mtd/ubi/wl.c
+++ b/drivers/mtd/ubi/wl.c
@@ -671,6 +671,8 @@ static struct ubi_wl_entry *get_peb_for_wl(struct ubi_device *ubi)
 
 	e = find_wl_entry(ubi, &ubi->free, WL_FREE_MAX_DIFF);
 	self_check_in_wl_tree(ubi, e, &ubi->free);
+	ubi->free_count--;
+	ubi_assert(ubi->free_count >= 0);
 	rb_erase(&e->u.rb, &ubi->free);
 
 	return e;
@@ -684,6 +686,9 @@ int ubi_wl_get_peb(struct ubi_device *ubi)
 	peb = __wl_get_peb(ubi);
 	spin_unlock(&ubi->wl_lock);
 
+	if (peb < 0)
+		return peb;
+
 	err = ubi_self_check_all_ff(ubi, peb, ubi->vid_hdr_aloffset,
 				    ubi->peb_size - ubi->vid_hdr_aloffset);
 	if (err) {
@@ -1068,6 +1073,7 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 
 			/* Give the unused PEB back */
 			wl_tree_add(e2, &ubi->free);
+			ubi->free_count++;
 			goto out_cancel;
 		}
 		self_check_in_wl_tree(ubi, e1, &ubi->used);
diff --git a/drivers/net/bonding/bond_alb.c b/drivers/net/bonding/bond_alb.c
index 9f69e818b00..93580a47cc5 100644
--- a/drivers/net/bonding/bond_alb.c
+++ b/drivers/net/bonding/bond_alb.c
@@ -82,7 +82,8 @@ static inline struct arp_pkt *arp_pkt(const struct sk_buff *skb)
 }
 
 /* Forward declaration */
-static void alb_send_learning_packets(struct slave *slave, u8 mac_addr[]);
+static void alb_send_learning_packets(struct slave *slave, u8 mac_addr[],
+				      bool strict_match);
 static void rlb_purge_src_ip(struct bonding *bond, struct arp_pkt *arp);
 static void rlb_src_unlink(struct bonding *bond, u32 index);
 static void rlb_src_link(struct bonding *bond, u32 ip_src_hash,
@@ -459,7 +460,7 @@ static void rlb_teach_disabled_mac_on_primary(struct bonding *bond, u8 addr[])
 
 	bond->alb_info.rlb_promisc_timeout_counter = 0;
 
-	alb_send_learning_packets(bond->curr_active_slave, addr);
+	alb_send_learning_packets(bond->curr_active_slave, addr, true);
 }
 
 /* slave being removed should not be active at this point
@@ -995,7 +996,7 @@ static void rlb_clear_vlan(struct bonding *bond, unsigned short vlan_id)
 /*********************** tlb/rlb shared functions *********************/
 
 static void alb_send_lp_vid(struct slave *slave, u8 mac_addr[],
-			    u16 vid)
+			    __be16 vlan_proto, u16 vid)
 {
 	struct learning_pkt pkt;
 	struct sk_buff *skb;
@@ -1021,7 +1022,7 @@ static void alb_send_lp_vid(struct slave *slave, u8 mac_addr[],
 	skb->dev = slave->dev;
 
 	if (vid) {
-		skb = vlan_put_tag(skb, htons(ETH_P_8021Q), vid);
+		skb = vlan_put_tag(skb, vlan_proto, vid);
 		if (!skb) {
 			pr_err("%s: Error: failed to insert VLAN tag\n",
 			       slave->bond->dev->name);
@@ -1032,22 +1033,32 @@ static void alb_send_lp_vid(struct slave *slave, u8 mac_addr[],
 	dev_queue_xmit(skb);
 }
 
-
-static void alb_send_learning_packets(struct slave *slave, u8 mac_addr[])
+static void alb_send_learning_packets(struct slave *slave, u8 mac_addr[],
+				      bool strict_match)
 {
 	struct bonding *bond = bond_get_bond_by_slave(slave);
 	struct net_device *upper;
 	struct list_head *iter;
 
 	/* send untagged */
-	alb_send_lp_vid(slave, mac_addr, 0);
+	alb_send_lp_vid(slave, mac_addr, 0, 0);
 
 	/* loop through vlans and send one packet for each */
 	rcu_read_lock();
 	netdev_for_each_all_upper_dev_rcu(bond->dev, upper, iter) {
-		if (upper->priv_flags & IFF_802_1Q_VLAN)
-			alb_send_lp_vid(slave, mac_addr,
-					vlan_dev_vlan_id(upper));
+		if (is_vlan_dev(upper) && vlan_get_encap_level(upper) == 0) {
+			if (strict_match &&
+			    ether_addr_equal_64bits(mac_addr,
+						    upper->dev_addr)) {
+				alb_send_lp_vid(slave, mac_addr,
+						vlan_dev_vlan_proto(upper),
+						vlan_dev_vlan_id(upper));
+			} else if (!strict_match) {
+				alb_send_lp_vid(slave, upper->dev_addr,
+						vlan_dev_vlan_proto(upper),
+						vlan_dev_vlan_id(upper));
+			}
+		}
 	}
 	rcu_read_unlock();
 }
@@ -1107,7 +1118,7 @@ static void alb_fasten_mac_swap(struct bonding *bond, struct slave *slave1,
 
 	/* fasten the change in the switch */
 	if (SLAVE_IS_OK(slave1)) {
-		alb_send_learning_packets(slave1, slave1->dev->dev_addr);
+		alb_send_learning_packets(slave1, slave1->dev->dev_addr, false);
 		if (bond->alb_info.rlb_enabled) {
 			/* inform the clients that the mac address
 			 * has changed
@@ -1119,7 +1130,7 @@ static void alb_fasten_mac_swap(struct bonding *bond, struct slave *slave1,
 	}
 
 	if (SLAVE_IS_OK(slave2)) {
-		alb_send_learning_packets(slave2, slave2->dev->dev_addr);
+		alb_send_learning_packets(slave2, slave2->dev->dev_addr, false);
 		if (bond->alb_info.rlb_enabled) {
 			/* inform the clients that the mac address
 			 * has changed
@@ -1490,6 +1501,8 @@ void bond_alb_monitor(struct work_struct *work)
 
 	/* send learning packets */
 	if (bond_info->lp_counter >= BOND_ALB_LP_TICKS(bond)) {
+		bool strict_match;
+
 		/* change of curr_active_slave involves swapping of mac addresses.
 		 * in order to avoid this swapping from happening while
 		 * sending the learning packets, the curr_slave_lock must be held for
@@ -1497,8 +1510,15 @@ void bond_alb_monitor(struct work_struct *work)
 		 */
 		read_lock(&bond->curr_slave_lock);
 
-		bond_for_each_slave_rcu(bond, slave, iter)
-			alb_send_learning_packets(slave, slave->dev->dev_addr);
+		bond_for_each_slave_rcu(bond, slave, iter) {
+			/* If updating current_active, use all currently
+			 * user mac addreses (!strict_match).  Otherwise, only
+			 * use mac of the slave device.
+			 */
+			strict_match = (slave != bond->curr_active_slave);
+			alb_send_learning_packets(slave, slave->dev->dev_addr,
+						  strict_match);
+		}
 
 		read_unlock(&bond->curr_slave_lock);
 
@@ -1721,7 +1741,8 @@ void bond_alb_handle_active_change(struct bonding *bond, struct slave *new_slave
 	} else {
 		/* set the new_slave to the bond mac address */
 		alb_set_slave_mac_addr(new_slave, bond->dev->dev_addr);
-		alb_send_learning_packets(new_slave, bond->dev->dev_addr);
+		alb_send_learning_packets(new_slave, bond->dev->dev_addr,
+					  false);
 	}
 
 	write_lock_bh(&bond->curr_slave_lock);
@@ -1764,7 +1785,8 @@ int bond_alb_set_mac_address(struct net_device *bond_dev, void *addr)
 		alb_set_slave_mac_addr(bond->curr_active_slave, bond_dev->dev_addr);
 
 		read_lock(&bond->lock);
-		alb_send_learning_packets(bond->curr_active_slave, bond_dev->dev_addr);
+		alb_send_learning_packets(bond->curr_active_slave,
+					  bond_dev->dev_addr, false);
 		if (bond->alb_info.rlb_enabled) {
 			/* inform clients mac address has changed */
 			rlb_req_update_slave_clients(bond, bond->curr_active_slave);
diff --git a/drivers/net/bonding/bond_main.c b/drivers/net/bonding/bond_main.c
index 69aff72c895..d3a67896d43 100644
--- a/drivers/net/bonding/bond_main.c
+++ b/drivers/net/bonding/bond_main.c
@@ -2126,10 +2126,10 @@ static bool bond_has_this_ip(struct bonding *bond, __be32 ip)
  */
 static void bond_arp_send(struct net_device *slave_dev, int arp_op,
 			  __be32 dest_ip, __be32 src_ip,
-			  struct bond_vlan_tag *inner,
-			  struct bond_vlan_tag *outer)
+			  struct bond_vlan_tag *tags)
 {
 	struct sk_buff *skb;
+	int i;
 
 	pr_debug("arp %d on slave %s: dst %pI4 src %pI4\n",
 		 arp_op, slave_dev->name, &dest_ip, &src_ip);
@@ -2141,21 +2141,26 @@ static void bond_arp_send(struct net_device *slave_dev, int arp_op,
 		net_err_ratelimited("ARP packet allocation failed\n");
 		return;
 	}
-	if (outer->vlan_id) {
-		if (inner->vlan_id) {
-			pr_debug("inner tag: proto %X vid %X\n",
-				 ntohs(inner->vlan_proto), inner->vlan_id);
-			skb = __vlan_put_tag(skb, inner->vlan_proto,
-					     inner->vlan_id);
-			if (!skb) {
-				net_err_ratelimited("failed to insert inner VLAN tag\n");
-				return;
-			}
-		}
 
-		pr_debug("outer reg: proto %X vid %X\n",
-			 ntohs(outer->vlan_proto), outer->vlan_id);
-		skb = vlan_put_tag(skb, outer->vlan_proto, outer->vlan_id);
+	/* Go through all the tags backwards and add them to the packet */
+	for (i = BOND_MAX_VLAN_ENCAP - 1; i > 0; i--) {
+		if (!tags[i].vlan_id)
+			continue;
+
+		pr_debug("inner tag: proto %X vid %X\n",
+			 ntohs(tags[i].vlan_proto), tags[i].vlan_id);
+		skb = __vlan_put_tag(skb, tags[i].vlan_proto,
+				     tags[i].vlan_id);
+		if (!skb) {
+			net_err_ratelimited("failed to insert inner VLAN tag\n");
+			return;
+		}
+	}
+	/* Set the outer tag */
+	if (tags[0].vlan_id) {
+		pr_debug("outer tag: proto %X vid %X\n",
+			 ntohs(tags[0].vlan_proto), tags[0].vlan_id);
+		skb = vlan_put_tag(skb, tags[0].vlan_proto, tags[0].vlan_id);
 		if (!skb) {
 			net_err_ratelimited("failed to insert outer VLAN tag\n");
 			return;
@@ -2164,22 +2169,52 @@ static void bond_arp_send(struct net_device *slave_dev, int arp_op,
 	arp_xmit(skb);
 }
 
+/* Validate the device path between the @start_dev and the @end_dev.
+ * The path is valid if the @end_dev is reachable through device
+ * stacking.
+ * When the path is validated, collect any vlan information in the
+ * path.
+ */
+static bool bond_verify_device_path(struct net_device *start_dev,
+				    struct net_device *end_dev,
+				    struct bond_vlan_tag *tags)
+{
+	struct net_device *upper;
+	struct list_head  *iter;
+	int  idx;
+
+	if (start_dev == end_dev)
+		return true;
+
+	netdev_for_each_upper_dev_rcu(start_dev, upper, iter) {
+		if (bond_verify_device_path(upper, end_dev, tags)) {
+			if (is_vlan_dev(upper)) {
+				idx = vlan_get_encap_level(upper);
+				if (idx >= BOND_MAX_VLAN_ENCAP)
+					return false;
+
+				tags[idx].vlan_proto =
+						    vlan_dev_vlan_proto(upper);
+				tags[idx].vlan_id = vlan_dev_vlan_id(upper);
+			}
+			return true;
+		}
+	}
+
+	return false;
+}
 
 static void bond_arp_send_all(struct bonding *bond, struct slave *slave)
 {
-	struct net_device *upper, *vlan_upper;
-	struct list_head *iter, *vlan_iter;
 	struct rtable *rt;
-	struct bond_vlan_tag inner, outer;
+	struct bond_vlan_tag tags[BOND_MAX_VLAN_ENCAP];
 	__be32 *targets = bond->params.arp_targets, addr;
 	int i;
+	bool ret;
 
 	for (i = 0; i < BOND_MAX_ARP_TARGETS && targets[i]; i++) {
 		pr_debug("basa: target %pI4\n", &targets[i]);
-		inner.vlan_proto = 0;
-		inner.vlan_id = 0;
-		outer.vlan_proto = 0;
-		outer.vlan_id = 0;
+		memset(tags, 0, sizeof(tags));
 
 		/* Find out through which dev should the packet go */
 		rt = ip_route_output(dev_net(bond->dev), targets[i], 0,
@@ -2192,7 +2227,8 @@ static void bond_arp_send_all(struct bonding *bond, struct slave *slave)
 				net_warn_ratelimited("%s: no route to arp_ip_target %pI4 and arp_validate is set\n",
 						     bond->dev->name,
 						     &targets[i]);
-			bond_arp_send(slave->dev, ARPOP_REQUEST, targets[i], 0, &inner, &outer);
+			bond_arp_send(slave->dev, ARPOP_REQUEST, targets[i],
+				      0, tags);
 			continue;
 		}
 
@@ -2201,52 +2237,12 @@ static void bond_arp_send_all(struct bonding *bond, struct slave *slave)
 			goto found;
 
 		rcu_read_lock();
-		/* first we search only for vlan devices. for every vlan
-		 * found we verify its upper dev list, searching for the
-		 * rt->dst.dev. If found we save the tag of the vlan and
-		 * proceed to send the packet.
-		 */
-		netdev_for_each_all_upper_dev_rcu(bond->dev, vlan_upper,
-						  vlan_iter) {
-			if (!is_vlan_dev(vlan_upper))
-				continue;
-
-			if (vlan_upper == rt->dst.dev) {
-				outer.vlan_proto = vlan_dev_vlan_proto(vlan_upper);
-				outer.vlan_id = vlan_dev_vlan_id(vlan_upper);
-				rcu_read_unlock();
-				goto found;
-			}
-			netdev_for_each_all_upper_dev_rcu(vlan_upper, upper,
-							  iter) {
-				if (upper == rt->dst.dev) {
-					/* If the upper dev is a vlan dev too,
-					 *  set the vlan tag to inner tag.
-					 */
-					if (is_vlan_dev(upper)) {
-						inner.vlan_proto = vlan_dev_vlan_proto(upper);
-						inner.vlan_id = vlan_dev_vlan_id(upper);
-					}
-					outer.vlan_proto = vlan_dev_vlan_proto(vlan_upper);
-					outer.vlan_id = vlan_dev_vlan_id(vlan_upper);
-					rcu_read_unlock();
-					goto found;
-				}
-			}
-		}
-
-		/* if the device we're looking for is not on top of any of
-		 * our upper vlans, then just search for any dev that
-		 * matches, and in case it's a vlan - save the id
-		 */
-		netdev_for_each_all_upper_dev_rcu(bond->dev, upper, iter) {
-			if (upper == rt->dst.dev) {
-				rcu_read_unlock();
-				goto found;
-			}
-		}
+		ret = bond_verify_device_path(bond->dev, rt->dst.dev, tags);
 		rcu_read_unlock();
 
+		if (ret)
+			goto found;
+
 		/* Not our device - skip */
 		pr_debug("%s: no path to arp_ip_target %pI4 via rt.dev %s\n",
 			 bond->dev->name, &targets[i],
@@ -2259,7 +2255,7 @@ found:
 		addr = bond_confirm_addr(rt->dst.dev, targets[i], 0);
 		ip_rt_put(rt);
 		bond_arp_send(slave->dev, ARPOP_REQUEST, targets[i],
-			      addr, &inner, &outer);
+			      addr, tags);
 	}
 }
 
diff --git a/drivers/net/bonding/bond_options.c b/drivers/net/bonding/bond_options.c
index 724e30fa20b..83207029844 100644
--- a/drivers/net/bonding/bond_options.c
+++ b/drivers/net/bonding/bond_options.c
@@ -125,6 +125,7 @@ static const struct bond_opt_value bond_fail_over_mac_tbl[] = {
 static const struct bond_opt_value bond_intmax_tbl[] = {
 	{ "off",     0,       BOND_VALFLAG_DEFAULT},
 	{ "maxval",  INT_MAX, BOND_VALFLAG_MAX},
+	{ NULL,      -1,      0}
 };
 
 static const struct bond_opt_value bond_lacp_rate_tbl[] = {
diff --git a/drivers/net/bonding/bond_sysfs.c b/drivers/net/bonding/bond_sysfs.c
index 0e8b268da0a..5f6babcfc26 100644
--- a/drivers/net/bonding/bond_sysfs.c
+++ b/drivers/net/bonding/bond_sysfs.c
@@ -534,7 +534,7 @@ static ssize_t bonding_show_min_links(struct device *d,
 {
 	struct bonding *bond = to_bond(d);
 
-	return sprintf(buf, "%d\n", bond->params.min_links);
+	return sprintf(buf, "%u\n", bond->params.min_links);
 }
 
 static ssize_t bonding_store_min_links(struct device *d,
diff --git a/drivers/net/bonding/bonding.h b/drivers/net/bonding/bonding.h
index b8bdd0acc8f..00bea320e3b 100644
--- a/drivers/net/bonding/bonding.h
+++ b/drivers/net/bonding/bonding.h
@@ -36,6 +36,7 @@
 
 #define bond_version DRV_DESCRIPTION ": v" DRV_VERSION " (" DRV_RELDATE ")\n"
 
+#define BOND_MAX_VLAN_ENCAP	2
 #define BOND_MAX_ARP_TARGETS	16
 
 #define BOND_DEFAULT_MIIMON	100
diff --git a/drivers/net/can/c_can/c_can.c b/drivers/net/can/c_can/c_can.c
index a5c8dcfa835..95e04e2002d 100644
--- a/drivers/net/can/c_can/c_can.c
+++ b/drivers/net/can/c_can/c_can.c
@@ -60,6 +60,8 @@
 #define CONTROL_IE		BIT(1)
 #define CONTROL_INIT		BIT(0)
 
+#define CONTROL_IRQMSK		(CONTROL_EIE | CONTROL_IE | CONTROL_SIE)
+
 /* test register */
 #define TEST_RX			BIT(7)
 #define TEST_TX1		BIT(6)
@@ -108,11 +110,14 @@
 #define IF_COMM_CONTROL		BIT(4)
 #define IF_COMM_CLR_INT_PND	BIT(3)
 #define IF_COMM_TXRQST		BIT(2)
+#define IF_COMM_CLR_NEWDAT	IF_COMM_TXRQST
 #define IF_COMM_DATAA		BIT(1)
 #define IF_COMM_DATAB		BIT(0)
-#define IF_COMM_ALL		(IF_COMM_MASK | IF_COMM_ARB | \
-				IF_COMM_CONTROL | IF_COMM_TXRQST | \
-				IF_COMM_DATAA | IF_COMM_DATAB)
+
+/* TX buffer setup */
+#define IF_COMM_TX		(IF_COMM_ARB | IF_COMM_CONTROL | \
+				 IF_COMM_TXRQST |		 \
+				 IF_COMM_DATAA | IF_COMM_DATAB)
 
 /* For the low buffers we clear the interrupt bit, but keep newdat */
 #define IF_COMM_RCV_LOW		(IF_COMM_MASK | IF_COMM_ARB | \
@@ -120,12 +125,19 @@
 				 IF_COMM_DATAA | IF_COMM_DATAB)
 
 /* For the high buffers we clear the interrupt bit and newdat */
-#define IF_COMM_RCV_HIGH	(IF_COMM_RCV_LOW | IF_COMM_TXRQST)
+#define IF_COMM_RCV_HIGH	(IF_COMM_RCV_LOW | IF_COMM_CLR_NEWDAT)
+
+
+/* Receive setup of message objects */
+#define IF_COMM_RCV_SETUP	(IF_COMM_MASK | IF_COMM_ARB | IF_COMM_CONTROL)
+
+/* Invalidation of message objects */
+#define IF_COMM_INVAL		(IF_COMM_ARB | IF_COMM_CONTROL)
 
 /* IFx arbitration */
-#define IF_ARB_MSGVAL		BIT(15)
-#define IF_ARB_MSGXTD		BIT(14)
-#define IF_ARB_TRANSMIT		BIT(13)
+#define IF_ARB_MSGVAL		BIT(31)
+#define IF_ARB_MSGXTD		BIT(30)
+#define IF_ARB_TRANSMIT		BIT(29)
 
 /* IFx message control */
 #define IF_MCONT_NEWDAT		BIT(15)
@@ -139,19 +151,17 @@
 #define IF_MCONT_EOB		BIT(7)
 #define IF_MCONT_DLC_MASK	0xf
 
+#define IF_MCONT_RCV		(IF_MCONT_RXIE | IF_MCONT_UMASK)
+#define IF_MCONT_RCV_EOB	(IF_MCONT_RCV | IF_MCONT_EOB)
+
+#define IF_MCONT_TX		(IF_MCONT_TXIE | IF_MCONT_EOB)
+
 /*
  * Use IF1 for RX and IF2 for TX
  */
 #define IF_RX			0
 #define IF_TX			1
 
-/* status interrupt */
-#define STATUS_INTERRUPT	0x8000
-
-/* global interrupt masks */
-#define ENABLE_ALL_INTERRUPTS	1
-#define DISABLE_ALL_INTERRUPTS	0
-
 /* minimum timeout for checking BUSY status */
 #define MIN_TIMEOUT_VALUE	6
 
@@ -171,6 +181,7 @@ enum c_can_lec_type {
 	LEC_BIT0_ERROR,
 	LEC_CRC_ERROR,
 	LEC_UNUSED,
+	LEC_MASK = LEC_UNUSED,
 };
 
 /*
@@ -226,143 +237,115 @@ static inline void c_can_reset_ram(const struct c_can_priv *priv, bool enable)
 		priv->raminit(priv, enable);
 }
 
-static inline int get_tx_next_msg_obj(const struct c_can_priv *priv)
-{
-	return (priv->tx_next & C_CAN_NEXT_MSG_OBJ_MASK) +
-			C_CAN_MSG_OBJ_TX_FIRST;
-}
-
-static inline int get_tx_echo_msg_obj(int txecho)
-{
-	return (txecho & C_CAN_NEXT_MSG_OBJ_MASK) + C_CAN_MSG_OBJ_TX_FIRST;
-}
-
-static u32 c_can_read_reg32(struct c_can_priv *priv, enum reg index)
-{
-	u32 val = priv->read_reg(priv, index);
-	val |= ((u32) priv->read_reg(priv, index + 1)) << 16;
-	return val;
-}
-
-static void c_can_enable_all_interrupts(struct c_can_priv *priv,
-						int enable)
+static void c_can_irq_control(struct c_can_priv *priv, bool enable)
 {
-	unsigned int cntrl_save = priv->read_reg(priv,
-						C_CAN_CTRL_REG);
+	u32 ctrl = priv->read_reg(priv,	C_CAN_CTRL_REG) & ~CONTROL_IRQMSK;
 
 	if (enable)
-		cntrl_save |= (CONTROL_SIE | CONTROL_EIE | CONTROL_IE);
-	else
-		cntrl_save &= ~(CONTROL_EIE | CONTROL_IE | CONTROL_SIE);
+		ctrl |= CONTROL_IRQMSK;
 
-	priv->write_reg(priv, C_CAN_CTRL_REG, cntrl_save);
+	priv->write_reg(priv, C_CAN_CTRL_REG, ctrl);
 }
 
-static inline int c_can_msg_obj_is_busy(struct c_can_priv *priv, int iface)
+static void c_can_obj_update(struct net_device *dev, int iface, u32 cmd, u32 obj)
 {
-	int count = MIN_TIMEOUT_VALUE;
+	struct c_can_priv *priv = netdev_priv(dev);
+	int cnt, reg = C_CAN_IFACE(COMREQ_REG, iface);
 
-	while (count && priv->read_reg(priv,
-				C_CAN_IFACE(COMREQ_REG, iface)) &
-				IF_COMR_BUSY) {
-		count--;
+	priv->write_reg(priv, reg + 1, cmd);
+	priv->write_reg(priv, reg, obj);
+
+	for (cnt = MIN_TIMEOUT_VALUE; cnt; cnt--) {
+		if (!(priv->read_reg(priv, reg) & IF_COMR_BUSY))
+			return;
 		udelay(1);
 	}
+	netdev_err(dev, "Updating object timed out\n");
 
-	if (!count)
-		return 1;
+}
 
-	return 0;
+static inline void c_can_object_get(struct net_device *dev, int iface,
+				    u32 obj, u32 cmd)
+{
+	c_can_obj_update(dev, iface, cmd, obj);
 }
 
-static inline void c_can_object_get(struct net_device *dev,
-					int iface, int objno, int mask)
+static inline void c_can_object_put(struct net_device *dev, int iface,
+				    u32 obj, u32 cmd)
 {
-	struct c_can_priv *priv = netdev_priv(dev);
+	c_can_obj_update(dev, iface, cmd | IF_COMM_WR, obj);
+}
 
-	/*
-	 * As per specs, after writting the message object number in the
-	 * IF command request register the transfer b/w interface
-	 * register and message RAM must be complete in 6 CAN-CLK
-	 * period.
-	 */
-	priv->write_reg(priv, C_CAN_IFACE(COMMSK_REG, iface),
-			IFX_WRITE_LOW_16BIT(mask));
-	priv->write_reg(priv, C_CAN_IFACE(COMREQ_REG, iface),
-			IFX_WRITE_LOW_16BIT(objno));
+/*
+ * Note: According to documentation clearing TXIE while MSGVAL is set
+ * is not allowed, but works nicely on C/DCAN. And that lowers the I/O
+ * load significantly.
+ */
+static void c_can_inval_tx_object(struct net_device *dev, int iface, int obj)
+{
+	struct c_can_priv *priv = netdev_priv(dev);
 
-	if (c_can_msg_obj_is_busy(priv, iface))
-		netdev_err(dev, "timed out in object get\n");
+	priv->write_reg(priv, C_CAN_IFACE(MSGCTRL_REG, iface), 0);
+	c_can_object_put(dev, iface, obj, IF_COMM_INVAL);
 }
 
-static inline void c_can_object_put(struct net_device *dev,
-					int iface, int objno, int mask)
+static void c_can_inval_msg_object(struct net_device *dev, int iface, int obj)
 {
 	struct c_can_priv *priv = netdev_priv(dev);
 
-	/*
-	 * As per specs, after writting the message object number in the
-	 * IF command request register the transfer b/w interface
-	 * register and message RAM must be complete in 6 CAN-CLK
-	 * period.
-	 */
-	priv->write_reg(priv, C_CAN_IFACE(COMMSK_REG, iface),
-			(IF_COMM_WR | IFX_WRITE_LOW_16BIT(mask)));
-	priv->write_reg(priv, C_CAN_IFACE(COMREQ_REG, iface),
-			IFX_WRITE_LOW_16BIT(objno));
-
-	if (c_can_msg_obj_is_busy(priv, iface))
-		netdev_err(dev, "timed out in object put\n");
+	priv->write_reg(priv, C_CAN_IFACE(ARB1_REG, iface), 0);
+	priv->write_reg(priv, C_CAN_IFACE(ARB2_REG, iface), 0);
+	c_can_inval_tx_object(dev, iface, obj);
 }
 
-static void c_can_write_msg_object(struct net_device *dev,
-			int iface, struct can_frame *frame, int objno)
+static void c_can_setup_tx_object(struct net_device *dev, int iface,
+				  struct can_frame *frame, int idx)
 {
-	int i;
-	u16 flags = 0;
-	unsigned int id;
 	struct c_can_priv *priv = netdev_priv(dev);
-
-	if (!(frame->can_id & CAN_RTR_FLAG))
-		flags |= IF_ARB_TRANSMIT;
+	u16 ctrl = IF_MCONT_TX | frame->can_dlc;
+	bool rtr = frame->can_id & CAN_RTR_FLAG;
+	u32 arb = IF_ARB_MSGVAL;
+	int i;
 
 	if (frame->can_id & CAN_EFF_FLAG) {
-		id = frame->can_id & CAN_EFF_MASK;
-		flags |= IF_ARB_MSGXTD;
-	} else
-		id = ((frame->can_id & CAN_SFF_MASK) << 18);
+		arb |= frame->can_id & CAN_EFF_MASK;
+		arb |= IF_ARB_MSGXTD;
+	} else {
+		arb |= (frame->can_id & CAN_SFF_MASK) << 18;
+	}
+
+	if (!rtr)
+		arb |= IF_ARB_TRANSMIT;
+
+	/*
+	 * If we change the DIR bit, we need to invalidate the buffer
+	 * first, i.e. clear the MSGVAL flag in the arbiter.
+	 */
+	if (rtr != (bool)test_bit(idx, &priv->tx_dir)) {
+		u32 obj = idx + C_CAN_MSG_OBJ_TX_FIRST;
+
+		c_can_inval_msg_object(dev, iface, obj);
+		change_bit(idx, &priv->tx_dir);
+	}
 
-	flags |= IF_ARB_MSGVAL;
+	priv->write_reg(priv, C_CAN_IFACE(ARB1_REG, iface), arb);
+	priv->write_reg(priv, C_CAN_IFACE(ARB2_REG, iface), arb >> 16);
 
-	priv->write_reg(priv, C_CAN_IFACE(ARB1_REG, iface),
-				IFX_WRITE_LOW_16BIT(id));
-	priv->write_reg(priv, C_CAN_IFACE(ARB2_REG, iface), flags |
-				IFX_WRITE_HIGH_16BIT(id));
+	priv->write_reg(priv, C_CAN_IFACE(MSGCTRL_REG, iface), ctrl);
 
 	for (i = 0; i < frame->can_dlc; i += 2) {
 		priv->write_reg(priv, C_CAN_IFACE(DATA1_REG, iface) + i / 2,
 				frame->data[i] | (frame->data[i + 1] << 8));
 	}
-
-	/* enable interrupt for this message object */
-	priv->write_reg(priv, C_CAN_IFACE(MSGCTRL_REG, iface),
-			IF_MCONT_TXIE | IF_MCONT_TXRQST | IF_MCONT_EOB |
-			frame->can_dlc);
-	c_can_object_put(dev, iface, objno, IF_COMM_ALL);
 }
 
 static inline void c_can_activate_all_lower_rx_msg_obj(struct net_device *dev,
-						int iface,
-						int ctrl_mask)
+						       int iface)
 {
 	int i;
-	struct c_can_priv *priv = netdev_priv(dev);
 
-	for (i = C_CAN_MSG_OBJ_RX_FIRST; i <= C_CAN_MSG_RX_LOW_LAST; i++) {
-		priv->write_reg(priv, C_CAN_IFACE(MSGCTRL_REG, iface),
-				ctrl_mask & ~IF_MCONT_NEWDAT);
-		c_can_object_put(dev, iface, i, IF_COMM_CONTROL);
-	}
+	for (i = C_CAN_MSG_OBJ_RX_FIRST; i <= C_CAN_MSG_RX_LOW_LAST; i++)
+		c_can_object_get(dev, iface, i, IF_COMM_CLR_NEWDAT);
 }
 
 static int c_can_handle_lost_msg_obj(struct net_device *dev,
@@ -377,6 +360,9 @@ static int c_can_handle_lost_msg_obj(struct net_device *dev,
 	priv->write_reg(priv, C_CAN_IFACE(MSGCTRL_REG, iface), ctrl);
 	c_can_object_put(dev, iface, objno, IF_COMM_CONTROL);
 
+	stats->rx_errors++;
+	stats->rx_over_errors++;
+
 	/* create an error msg */
 	skb = alloc_can_err_skb(dev, &frame);
 	if (unlikely(!skb))
@@ -384,22 +370,18 @@ static int c_can_handle_lost_msg_obj(struct net_device *dev,
 
 	frame->can_id |= CAN_ERR_CRTL;
 	frame->data[1] = CAN_ERR_CRTL_RX_OVERFLOW;
-	stats->rx_errors++;
-	stats->rx_over_errors++;
 
 	netif_receive_skb(skb);
 	return 1;
 }
 
-static int c_can_read_msg_object(struct net_device *dev, int iface, int ctrl)
+static int c_can_read_msg_object(struct net_device *dev, int iface, u32 ctrl)
 {
-	u16 flags, data;
-	int i;
-	unsigned int val;
-	struct c_can_priv *priv = netdev_priv(dev);
 	struct net_device_stats *stats = &dev->stats;
-	struct sk_buff *skb;
+	struct c_can_priv *priv = netdev_priv(dev);
 	struct can_frame *frame;
+	struct sk_buff *skb;
+	u32 arb, data;
 
 	skb = alloc_can_skb(dev, &frame);
 	if (!skb) {
@@ -409,115 +391,82 @@ static int c_can_read_msg_object(struct net_device *dev, int iface, int ctrl)
 
 	frame->can_dlc = get_can_dlc(ctrl & 0x0F);
 
-	flags =	priv->read_reg(priv, C_CAN_IFACE(ARB2_REG, iface));
-	val = priv->read_reg(priv, C_CAN_IFACE(ARB1_REG, iface)) |
-		(flags << 16);
+	arb = priv->read_reg(priv, C_CAN_IFACE(ARB1_REG, iface));
+	arb |= priv->read_reg(priv, C_CAN_IFACE(ARB2_REG, iface)) << 16;
 
-	if (flags & IF_ARB_MSGXTD)
-		frame->can_id = (val & CAN_EFF_MASK) | CAN_EFF_FLAG;
+	if (arb & IF_ARB_MSGXTD)
+		frame->can_id = (arb & CAN_EFF_MASK) | CAN_EFF_FLAG;
 	else
-		frame->can_id = (val >> 18) & CAN_SFF_MASK;
+		frame->can_id = (arb >> 18) & CAN_SFF_MASK;
 
-	if (flags & IF_ARB_TRANSMIT)
+	if (arb & IF_ARB_TRANSMIT) {
 		frame->can_id |= CAN_RTR_FLAG;
-	else {
-		for (i = 0; i < frame->can_dlc; i += 2) {
-			data = priv->read_reg(priv,
-				C_CAN_IFACE(DATA1_REG, iface) + i / 2);
+	} else {
+		int i, dreg = C_CAN_IFACE(DATA1_REG, iface);
+
+		for (i = 0; i < frame->can_dlc; i += 2, dreg ++) {
+			data = priv->read_reg(priv, dreg);
 			frame->data[i] = data;
 			frame->data[i + 1] = data >> 8;
 		}
 	}
 
-	netif_receive_skb(skb);
-
 	stats->rx_packets++;
 	stats->rx_bytes += frame->can_dlc;
+
+	netif_receive_skb(skb);
 	return 0;
 }
 
 static void c_can_setup_receive_object(struct net_device *dev, int iface,
-					int objno, unsigned int mask,
-					unsigned int id, unsigned int mcont)
+				       u32 obj, u32 mask, u32 id, u32 mcont)
 {
 	struct c_can_priv *priv = netdev_priv(dev);
 
-	priv->write_reg(priv, C_CAN_IFACE(MASK1_REG, iface),
-			IFX_WRITE_LOW_16BIT(mask));
-
-	/* According to C_CAN documentation, the reserved bit
-	 * in IFx_MASK2 register is fixed 1
-	 */
-	priv->write_reg(priv, C_CAN_IFACE(MASK2_REG, iface),
-			IFX_WRITE_HIGH_16BIT(mask) | BIT(13));
+	mask |= BIT(29);
+	priv->write_reg(priv, C_CAN_IFACE(MASK1_REG, iface), mask);
+	priv->write_reg(priv, C_CAN_IFACE(MASK2_REG, iface), mask >> 16);
 
-	priv->write_reg(priv, C_CAN_IFACE(ARB1_REG, iface),
-			IFX_WRITE_LOW_16BIT(id));
-	priv->write_reg(priv, C_CAN_IFACE(ARB2_REG, iface),
-			(IF_ARB_MSGVAL | IFX_WRITE_HIGH_16BIT(id)));
+	id |= IF_ARB_MSGVAL;
+	priv->write_reg(priv, C_CAN_IFACE(ARB1_REG, iface), id);
+	priv->write_reg(priv, C_CAN_IFACE(ARB2_REG, iface), id >> 16);
 
 	priv->write_reg(priv, C_CAN_IFACE(MSGCTRL_REG, iface), mcont);
-	c_can_object_put(dev, iface, objno, IF_COMM_ALL & ~IF_COMM_TXRQST);
-
-	netdev_dbg(dev, "obj no:%d, msgval:0x%08x\n", objno,
-			c_can_read_reg32(priv, C_CAN_MSGVAL1_REG));
-}
-
-static void c_can_inval_msg_object(struct net_device *dev, int iface, int objno)
-{
-	struct c_can_priv *priv = netdev_priv(dev);
-
-	priv->write_reg(priv, C_CAN_IFACE(ARB1_REG, iface), 0);
-	priv->write_reg(priv, C_CAN_IFACE(ARB2_REG, iface), 0);
-	priv->write_reg(priv, C_CAN_IFACE(MSGCTRL_REG, iface), 0);
-
-	c_can_object_put(dev, iface, objno, IF_COMM_ARB | IF_COMM_CONTROL);
-
-	netdev_dbg(dev, "obj no:%d, msgval:0x%08x\n", objno,
-			c_can_read_reg32(priv, C_CAN_MSGVAL1_REG));
-}
-
-static inline int c_can_is_next_tx_obj_busy(struct c_can_priv *priv, int objno)
-{
-	int val = c_can_read_reg32(priv, C_CAN_TXRQST1_REG);
-
-	/*
-	 * as transmission request register's bit n-1 corresponds to
-	 * message object n, we need to handle the same properly.
-	 */
-	if (val & (1 << (objno - 1)))
-		return 1;
-
-	return 0;
+	c_can_object_put(dev, iface, obj, IF_COMM_RCV_SETUP);
 }
 
 static netdev_tx_t c_can_start_xmit(struct sk_buff *skb,
-					struct net_device *dev)
+				    struct net_device *dev)
 {
-	u32 msg_obj_no;
-	struct c_can_priv *priv = netdev_priv(dev);
 	struct can_frame *frame = (struct can_frame *)skb->data;
+	struct c_can_priv *priv = netdev_priv(dev);
+	u32 idx, obj;
 
 	if (can_dropped_invalid_skb(dev, skb))
 		return NETDEV_TX_OK;
-
-	spin_lock_bh(&priv->xmit_lock);
-	msg_obj_no = get_tx_next_msg_obj(priv);
-
-	/* prepare message object for transmission */
-	c_can_write_msg_object(dev, IF_TX, frame, msg_obj_no);
-	priv->dlc[msg_obj_no - C_CAN_MSG_OBJ_TX_FIRST] = frame->can_dlc;
-	can_put_echo_skb(skb, dev, msg_obj_no - C_CAN_MSG_OBJ_TX_FIRST);
-
 	/*
-	 * we have to stop the queue in case of a wrap around or
-	 * if the next TX message object is still in use
+	 * This is not a FIFO. C/D_CAN sends out the buffers
+	 * prioritized. The lowest buffer number wins.
 	 */
-	priv->tx_next++;
-	if (c_can_is_next_tx_obj_busy(priv, get_tx_next_msg_obj(priv)) ||
-			(priv->tx_next & C_CAN_NEXT_MSG_OBJ_MASK) == 0)
+	idx = fls(atomic_read(&priv->tx_active));
+	obj = idx + C_CAN_MSG_OBJ_TX_FIRST;
+
+	/* If this is the last buffer, stop the xmit queue */
+	if (idx == C_CAN_MSG_OBJ_TX_NUM - 1)
 		netif_stop_queue(dev);
-	spin_unlock_bh(&priv->xmit_lock);
+	/*
+	 * Store the message in the interface so we can call
+	 * can_put_echo_skb(). We must do this before we enable
+	 * transmit as we might race against do_tx().
+	 */
+	c_can_setup_tx_object(dev, IF_TX, frame, idx);
+	priv->dlc[idx] = frame->can_dlc;
+	can_put_echo_skb(skb, dev, idx);
+
+	/* Update the active bits */
+	atomic_add((1 << idx), &priv->tx_active);
+	/* Start transmission */
+	c_can_object_put(dev, IF_TX, obj, IF_COMM_TX);
 
 	return NETDEV_TX_OK;
 }
@@ -594,11 +543,10 @@ static void c_can_configure_msg_objects(struct net_device *dev)
 
 	/* setup receive message objects */
 	for (i = C_CAN_MSG_OBJ_RX_FIRST; i < C_CAN_MSG_OBJ_RX_LAST; i++)
-		c_can_setup_receive_object(dev, IF_RX, i, 0, 0,
-			(IF_MCONT_RXIE | IF_MCONT_UMASK) & ~IF_MCONT_EOB);
+		c_can_setup_receive_object(dev, IF_RX, i, 0, 0, IF_MCONT_RCV);
 
 	c_can_setup_receive_object(dev, IF_RX, C_CAN_MSG_OBJ_RX_LAST, 0, 0,
-			IF_MCONT_EOB | IF_MCONT_RXIE | IF_MCONT_UMASK);
+				   IF_MCONT_RCV_EOB);
 }
 
 /*
@@ -612,30 +560,22 @@ static int c_can_chip_config(struct net_device *dev)
 	struct c_can_priv *priv = netdev_priv(dev);
 
 	/* enable automatic retransmission */
-	priv->write_reg(priv, C_CAN_CTRL_REG,
-			CONTROL_ENABLE_AR);
+	priv->write_reg(priv, C_CAN_CTRL_REG, CONTROL_ENABLE_AR);
 
 	if ((priv->can.ctrlmode & CAN_CTRLMODE_LISTENONLY) &&
 	    (priv->can.ctrlmode & CAN_CTRLMODE_LOOPBACK)) {
 		/* loopback + silent mode : useful for hot self-test */
-		priv->write_reg(priv, C_CAN_CTRL_REG, CONTROL_EIE |
-				CONTROL_SIE | CONTROL_IE | CONTROL_TEST);
-		priv->write_reg(priv, C_CAN_TEST_REG,
-				TEST_LBACK | TEST_SILENT);
+		priv->write_reg(priv, C_CAN_CTRL_REG, CONTROL_TEST);
+		priv->write_reg(priv, C_CAN_TEST_REG, TEST_LBACK | TEST_SILENT);
 	} else if (priv->can.ctrlmode & CAN_CTRLMODE_LOOPBACK) {
 		/* loopback mode : useful for self-test function */
-		priv->write_reg(priv, C_CAN_CTRL_REG, CONTROL_EIE |
-				CONTROL_SIE | CONTROL_IE | CONTROL_TEST);
+		priv->write_reg(priv, C_CAN_CTRL_REG, CONTROL_TEST);
 		priv->write_reg(priv, C_CAN_TEST_REG, TEST_LBACK);
 	} else if (priv->can.ctrlmode & CAN_CTRLMODE_LISTENONLY) {
 		/* silent mode : bus-monitoring mode */
-		priv->write_reg(priv, C_CAN_CTRL_REG, CONTROL_EIE |
-				CONTROL_SIE | CONTROL_IE | CONTROL_TEST);
+		priv->write_reg(priv, C_CAN_CTRL_REG, CONTROL_TEST);
 		priv->write_reg(priv, C_CAN_TEST_REG, TEST_SILENT);
-	} else
-		/* normal mode*/
-		priv->write_reg(priv, C_CAN_CTRL_REG,
-				CONTROL_EIE | CONTROL_SIE | CONTROL_IE);
+	}
 
 	/* configure message objects */
 	c_can_configure_msg_objects(dev);
@@ -643,6 +583,11 @@ static int c_can_chip_config(struct net_device *dev)
 	/* set a `lec` value so that we can check for updates later */
 	priv->write_reg(priv, C_CAN_STS_REG, LEC_UNUSED);
 
+	/* Clear all internal status */
+	atomic_set(&priv->tx_active, 0);
+	priv->rxmasked = 0;
+	priv->tx_dir = 0;
+
 	/* set bittiming params */
 	return c_can_set_bittiming(dev);
 }
@@ -657,13 +602,11 @@ static int c_can_start(struct net_device *dev)
 	if (err)
 		return err;
 
-	priv->can.state = CAN_STATE_ERROR_ACTIVE;
-
-	/* reset tx helper pointers */
-	priv->tx_next = priv->tx_echo = 0;
+	/* Setup the command for new messages */
+	priv->comm_rcv_high = priv->type != BOSCH_D_CAN ?
+		IF_COMM_RCV_LOW : IF_COMM_RCV_HIGH;
 
-	/* enable status change, error and module interrupts */
-	c_can_enable_all_interrupts(priv, ENABLE_ALL_INTERRUPTS);
+	priv->can.state = CAN_STATE_ERROR_ACTIVE;
 
 	return 0;
 }
@@ -672,15 +615,13 @@ static void c_can_stop(struct net_device *dev)
 {
 	struct c_can_priv *priv = netdev_priv(dev);
 
-	/* disable all interrupts */
-	c_can_enable_all_interrupts(priv, DISABLE_ALL_INTERRUPTS);
-
-	/* set the state as STOPPED */
+	c_can_irq_control(priv, false);
 	priv->can.state = CAN_STATE_STOPPED;
 }
 
 static int c_can_set_mode(struct net_device *dev, enum can_mode mode)
 {
+	struct c_can_priv *priv = netdev_priv(dev);
 	int err;
 
 	switch (mode) {
@@ -689,6 +630,7 @@ static int c_can_set_mode(struct net_device *dev, enum can_mode mode)
 		if (err)
 			return err;
 		netif_wake_queue(dev);
+		c_can_irq_control(priv, true);
 		break;
 	default:
 		return -EOPNOTSUPP;
@@ -724,42 +666,29 @@ static int c_can_get_berr_counter(const struct net_device *dev,
 	return err;
 }
 
-/*
- * priv->tx_echo holds the number of the oldest can_frame put for
- * transmission into the hardware, but not yet ACKed by the CAN tx
- * complete IRQ.
- *
- * We iterate from priv->tx_echo to priv->tx_next and check if the
- * packet has been transmitted, echo it back to the CAN framework.
- * If we discover a not yet transmitted packet, stop looking for more.
- */
 static void c_can_do_tx(struct net_device *dev)
 {
 	struct c_can_priv *priv = netdev_priv(dev);
 	struct net_device_stats *stats = &dev->stats;
-	u32 val, obj, pkts = 0, bytes = 0;
-
-	spin_lock_bh(&priv->xmit_lock);
-
-	for (; (priv->tx_next - priv->tx_echo) > 0; priv->tx_echo++) {
-		obj = get_tx_echo_msg_obj(priv->tx_echo);
-		val = c_can_read_reg32(priv, C_CAN_TXRQST1_REG);
+	u32 idx, obj, pkts = 0, bytes = 0, pend, clr;
 
-		if (val & (1 << (obj - 1)))
-			break;
+	clr = pend = priv->read_reg(priv, C_CAN_INTPND2_REG);
 
-		can_get_echo_skb(dev, obj - C_CAN_MSG_OBJ_TX_FIRST);
-		bytes += priv->dlc[obj - C_CAN_MSG_OBJ_TX_FIRST];
+	while ((idx = ffs(pend))) {
+		idx--;
+		pend &= ~(1 << idx);
+		obj = idx + C_CAN_MSG_OBJ_TX_FIRST;
+		c_can_inval_tx_object(dev, IF_RX, obj);
+		can_get_echo_skb(dev, idx);
+		bytes += priv->dlc[idx];
 		pkts++;
-		c_can_inval_msg_object(dev, IF_TX, obj);
 	}
 
-	/* restart queue if wrap-up or if queue stalled on last pkt */
-	if (((priv->tx_next & C_CAN_NEXT_MSG_OBJ_MASK) != 0) ||
-			((priv->tx_echo & C_CAN_NEXT_MSG_OBJ_MASK) == 0))
-		netif_wake_queue(dev);
+	/* Clear the bits in the tx_active mask */
+	atomic_sub(clr, &priv->tx_active);
 
-	spin_unlock_bh(&priv->xmit_lock);
+	if (clr & (1 << (C_CAN_MSG_OBJ_TX_NUM - 1)))
+		netif_wake_queue(dev);
 
 	if (pkts) {
 		stats->tx_bytes += bytes;
@@ -800,18 +729,28 @@ static u32 c_can_adjust_pending(u32 pend)
 	return pend & ~((1 << lasts) - 1);
 }
 
+static inline void c_can_rx_object_get(struct net_device *dev,
+				       struct c_can_priv *priv, u32 obj)
+{
+		c_can_object_get(dev, IF_RX, obj, priv->comm_rcv_high);
+}
+
+static inline void c_can_rx_finalize(struct net_device *dev,
+				     struct c_can_priv *priv, u32 obj)
+{
+	if (priv->type != BOSCH_D_CAN)
+		c_can_object_get(dev, IF_RX, obj, IF_COMM_CLR_NEWDAT);
+}
+
 static int c_can_read_objects(struct net_device *dev, struct c_can_priv *priv,
 			      u32 pend, int quota)
 {
-	u32 pkts = 0, ctrl, obj, mcmd;
+	u32 pkts = 0, ctrl, obj;
 
 	while ((obj = ffs(pend)) && quota > 0) {
 		pend &= ~BIT(obj - 1);
 
-		mcmd = obj < C_CAN_MSG_RX_LOW_LAST ?
-			IF_COMM_RCV_LOW : IF_COMM_RCV_HIGH;
-
-		c_can_object_get(dev, IF_RX, obj, mcmd);
+		c_can_rx_object_get(dev, priv, obj);
 		ctrl = priv->read_reg(priv, C_CAN_IFACE(MSGCTRL_REG, IF_RX));
 
 		if (ctrl & IF_MCONT_MSGLST) {
@@ -833,9 +772,7 @@ static int c_can_read_objects(struct net_device *dev, struct c_can_priv *priv,
 		/* read the data from the message object */
 		c_can_read_msg_object(dev, IF_RX, ctrl);
 
-		if (obj == C_CAN_MSG_RX_LOW_LAST)
-			/* activate all lower message objects */
-			c_can_activate_all_lower_rx_msg_obj(dev, IF_RX, ctrl);
+		c_can_rx_finalize(dev, priv, obj);
 
 		pkts++;
 		quota--;
@@ -844,6 +781,13 @@ static int c_can_read_objects(struct net_device *dev, struct c_can_priv *priv,
 	return pkts;
 }
 
+static inline u32 c_can_get_pending(struct c_can_priv *priv)
+{
+	u32 pend = priv->read_reg(priv, C_CAN_NEWDAT1_REG);
+
+	return pend;
+}
+
 /*
  * theory of operation:
  *
@@ -853,18 +797,9 @@ static int c_can_read_objects(struct net_device *dev, struct c_can_priv *priv,
  * has arrived. To work-around this issue, we keep two groups of message
  * objects whose partitioning is defined by C_CAN_MSG_OBJ_RX_SPLIT.
  *
- * To ensure in-order frame reception we use the following
- * approach while re-activating a message object to receive further
- * frames:
- * - if the current message object number is lower than
- *   C_CAN_MSG_RX_LOW_LAST, do not clear the NEWDAT bit while clearing
- *   the INTPND bit.
- * - if the current message object number is equal to
- *   C_CAN_MSG_RX_LOW_LAST then clear the NEWDAT bit of all lower
- *   receive message objects.
- * - if the current message object number is greater than
- *   C_CAN_MSG_RX_LOW_LAST then clear the NEWDAT bit of
- *   only this message object.
+ * We clear the newdat bit right away.
+ *
+ * This can result in packet reordering when the readout is slow.
  */
 static int c_can_do_rx_poll(struct net_device *dev, int quota)
 {
@@ -880,7 +815,7 @@ static int c_can_do_rx_poll(struct net_device *dev, int quota)
 
 	while (quota > 0) {
 		if (!pend) {
-			pend = priv->read_reg(priv, C_CAN_INTPND1_REG);
+			pend = c_can_get_pending(priv);
 			if (!pend)
 				break;
 			/*
@@ -905,12 +840,6 @@ static int c_can_do_rx_poll(struct net_device *dev, int quota)
 	return pkts;
 }
 
-static inline int c_can_has_and_handle_berr(struct c_can_priv *priv)
-{
-	return (priv->can.ctrlmode & CAN_CTRLMODE_BERR_REPORTING) &&
-		(priv->current_status & LEC_UNUSED);
-}
-
 static int c_can_handle_state_change(struct net_device *dev,
 				enum c_can_bus_error_types error_type)
 {
@@ -922,6 +851,26 @@ static int c_can_handle_state_change(struct net_device *dev,
 	struct sk_buff *skb;
 	struct can_berr_counter bec;
 
+	switch (error_type) {
+	case C_CAN_ERROR_WARNING:
+		/* error warning state */
+		priv->can.can_stats.error_warning++;
+		priv->can.state = CAN_STATE_ERROR_WARNING;
+		break;
+	case C_CAN_ERROR_PASSIVE:
+		/* error passive state */
+		priv->can.can_stats.error_passive++;
+		priv->can.state = CAN_STATE_ERROR_PASSIVE;
+		break;
+	case C_CAN_BUS_OFF:
+		/* bus-off state */
+		priv->can.state = CAN_STATE_BUS_OFF;
+		can_bus_off(dev);
+		break;
+	default:
+		break;
+	}
+
 	/* propagate the error condition to the CAN stack */
 	skb = alloc_can_err_skb(dev, &cf);
 	if (unlikely(!skb))
@@ -935,8 +884,6 @@ static int c_can_handle_state_change(struct net_device *dev,
 	switch (error_type) {
 	case C_CAN_ERROR_WARNING:
 		/* error warning state */
-		priv->can.can_stats.error_warning++;
-		priv->can.state = CAN_STATE_ERROR_WARNING;
 		cf->can_id |= CAN_ERR_CRTL;
 		cf->data[1] = (bec.txerr > bec.rxerr) ?
 			CAN_ERR_CRTL_TX_WARNING :
@@ -947,8 +894,6 @@ static int c_can_handle_state_change(struct net_device *dev,
 		break;
 	case C_CAN_ERROR_PASSIVE:
 		/* error passive state */
-		priv->can.can_stats.error_passive++;
-		priv->can.state = CAN_STATE_ERROR_PASSIVE;
 		cf->can_id |= CAN_ERR_CRTL;
 		if (rx_err_passive)
 			cf->data[1] |= CAN_ERR_CRTL_RX_PASSIVE;
@@ -960,22 +905,16 @@ static int c_can_handle_state_change(struct net_device *dev,
 		break;
 	case C_CAN_BUS_OFF:
 		/* bus-off state */
-		priv->can.state = CAN_STATE_BUS_OFF;
 		cf->can_id |= CAN_ERR_BUSOFF;
-		/*
-		 * disable all interrupts in bus-off mode to ensure that
-		 * the CPU is not hogged down
-		 */
-		c_can_enable_all_interrupts(priv, DISABLE_ALL_INTERRUPTS);
 		can_bus_off(dev);
 		break;
 	default:
 		break;
 	}
 
-	netif_receive_skb(skb);
 	stats->rx_packets++;
 	stats->rx_bytes += cf->can_dlc;
+	netif_receive_skb(skb);
 
 	return 1;
 }
@@ -996,6 +935,13 @@ static int c_can_handle_bus_err(struct net_device *dev,
 	if (lec_type == LEC_UNUSED || lec_type == LEC_NO_ERROR)
 		return 0;
 
+	if (!(priv->can.ctrlmode & CAN_CTRLMODE_BERR_REPORTING))
+		return 0;
+
+	/* common for all type of bus errors */
+	priv->can.can_stats.bus_error++;
+	stats->rx_errors++;
+
 	/* propagate the error condition to the CAN stack */
 	skb = alloc_can_err_skb(dev, &cf);
 	if (unlikely(!skb))
@@ -1005,10 +951,6 @@ static int c_can_handle_bus_err(struct net_device *dev,
 	 * check for 'last error code' which tells us the
 	 * type of the last error to occur on the CAN bus
 	 */
-
-	/* common for all type of bus errors */
-	priv->can.can_stats.bus_error++;
-	stats->rx_errors++;
 	cf->can_id |= CAN_ERR_PROT | CAN_ERR_BUSERROR;
 	cf->data[2] |= CAN_ERR_PROT_UNSPEC;
 
@@ -1043,95 +985,64 @@ static int c_can_handle_bus_err(struct net_device *dev,
 		break;
 	}
 
-	/* set a `lec` value so that we can check for updates later */
-	priv->write_reg(priv, C_CAN_STS_REG, LEC_UNUSED);
-
-	netif_receive_skb(skb);
 	stats->rx_packets++;
 	stats->rx_bytes += cf->can_dlc;
-
+	netif_receive_skb(skb);
 	return 1;
 }
 
 static int c_can_poll(struct napi_struct *napi, int quota)
 {
-	u16 irqstatus;
-	int lec_type = 0;
-	int work_done = 0;
 	struct net_device *dev = napi->dev;
 	struct c_can_priv *priv = netdev_priv(dev);
+	u16 curr, last = priv->last_status;
+	int work_done = 0;
 
-	irqstatus = priv->irqstatus;
-	if (!irqstatus)
-		goto end;
+	priv->last_status = curr = priv->read_reg(priv, C_CAN_STS_REG);
+	/* Ack status on C_CAN. D_CAN is self clearing */
+	if (priv->type != BOSCH_D_CAN)
+		priv->write_reg(priv, C_CAN_STS_REG, LEC_UNUSED);
 
-	/* status events have the highest priority */
-	if (irqstatus == STATUS_INTERRUPT) {
-		priv->current_status = priv->read_reg(priv,
-					C_CAN_STS_REG);
-
-		/* handle Tx/Rx events */
-		if (priv->current_status & STATUS_TXOK)
-			priv->write_reg(priv, C_CAN_STS_REG,
-					priv->current_status & ~STATUS_TXOK);
-
-		if (priv->current_status & STATUS_RXOK)
-			priv->write_reg(priv, C_CAN_STS_REG,
-					priv->current_status & ~STATUS_RXOK);
-
-		/* handle state changes */
-		if ((priv->current_status & STATUS_EWARN) &&
-				(!(priv->last_status & STATUS_EWARN))) {
-			netdev_dbg(dev, "entered error warning state\n");
-			work_done += c_can_handle_state_change(dev,
-						C_CAN_ERROR_WARNING);
-		}
-		if ((priv->current_status & STATUS_EPASS) &&
-				(!(priv->last_status & STATUS_EPASS))) {
-			netdev_dbg(dev, "entered error passive state\n");
-			work_done += c_can_handle_state_change(dev,
-						C_CAN_ERROR_PASSIVE);
-		}
-		if ((priv->current_status & STATUS_BOFF) &&
-				(!(priv->last_status & STATUS_BOFF))) {
-			netdev_dbg(dev, "entered bus off state\n");
-			work_done += c_can_handle_state_change(dev,
-						C_CAN_BUS_OFF);
-		}
+	/* handle state changes */
+	if ((curr & STATUS_EWARN) && (!(last & STATUS_EWARN))) {
+		netdev_dbg(dev, "entered error warning state\n");
+		work_done += c_can_handle_state_change(dev, C_CAN_ERROR_WARNING);
+	}
 
-		/* handle bus recovery events */
-		if ((!(priv->current_status & STATUS_BOFF)) &&
-				(priv->last_status & STATUS_BOFF)) {
-			netdev_dbg(dev, "left bus off state\n");
-			priv->can.state = CAN_STATE_ERROR_ACTIVE;
-		}
-		if ((!(priv->current_status & STATUS_EPASS)) &&
-				(priv->last_status & STATUS_EPASS)) {
-			netdev_dbg(dev, "left error passive state\n");
-			priv->can.state = CAN_STATE_ERROR_ACTIVE;
-		}
+	if ((curr & STATUS_EPASS) && (!(last & STATUS_EPASS))) {
+		netdev_dbg(dev, "entered error passive state\n");
+		work_done += c_can_handle_state_change(dev, C_CAN_ERROR_PASSIVE);
+	}
 
-		priv->last_status = priv->current_status;
-
-		/* handle lec errors on the bus */
-		lec_type = c_can_has_and_handle_berr(priv);
-		if (lec_type)
-			work_done += c_can_handle_bus_err(dev, lec_type);
-	} else if ((irqstatus >= C_CAN_MSG_OBJ_RX_FIRST) &&
-			(irqstatus <= C_CAN_MSG_OBJ_RX_LAST)) {
-		/* handle events corresponding to receive message objects */
-		work_done += c_can_do_rx_poll(dev, (quota - work_done));
-	} else if ((irqstatus >= C_CAN_MSG_OBJ_TX_FIRST) &&
-			(irqstatus <= C_CAN_MSG_OBJ_TX_LAST)) {
-		/* handle events corresponding to transmit message objects */
-		c_can_do_tx(dev);
+	if ((curr & STATUS_BOFF) && (!(last & STATUS_BOFF))) {
+		netdev_dbg(dev, "entered bus off state\n");
+		work_done += c_can_handle_state_change(dev, C_CAN_BUS_OFF);
+		goto end;
 	}
 
+	/* handle bus recovery events */
+	if ((!(curr & STATUS_BOFF)) && (last & STATUS_BOFF)) {
+		netdev_dbg(dev, "left bus off state\n");
+		priv->can.state = CAN_STATE_ERROR_ACTIVE;
+	}
+	if ((!(curr & STATUS_EPASS)) && (last & STATUS_EPASS)) {
+		netdev_dbg(dev, "left error passive state\n");
+		priv->can.state = CAN_STATE_ERROR_ACTIVE;
+	}
+
+	/* handle lec errors on the bus */
+	work_done += c_can_handle_bus_err(dev, curr & LEC_MASK);
+
+	/* Handle Tx/Rx events. We do this unconditionally */
+	work_done += c_can_do_rx_poll(dev, (quota - work_done));
+	c_can_do_tx(dev);
+
 end:
 	if (work_done < quota) {
 		napi_complete(napi);
-		/* enable all IRQs */
-		c_can_enable_all_interrupts(priv, ENABLE_ALL_INTERRUPTS);
+		/* enable all IRQs if we are not in bus off state */
+		if (priv->can.state != CAN_STATE_BUS_OFF)
+			c_can_irq_control(priv, true);
 	}
 
 	return work_done;
@@ -1142,12 +1053,11 @@ static irqreturn_t c_can_isr(int irq, void *dev_id)
 	struct net_device *dev = (struct net_device *)dev_id;
 	struct c_can_priv *priv = netdev_priv(dev);
 
-	priv->irqstatus = priv->read_reg(priv, C_CAN_INT_REG);
-	if (!priv->irqstatus)
+	if (!priv->read_reg(priv, C_CAN_INT_REG))
 		return IRQ_NONE;
 
 	/* disable all interrupts and schedule the NAPI */
-	c_can_enable_all_interrupts(priv, DISABLE_ALL_INTERRUPTS);
+	c_can_irq_control(priv, false);
 	napi_schedule(&priv->napi);
 
 	return IRQ_HANDLED;
@@ -1184,6 +1094,8 @@ static int c_can_open(struct net_device *dev)
 	can_led_event(dev, CAN_LED_EVENT_OPEN);
 
 	napi_enable(&priv->napi);
+	/* enable status change, error and module interrupts */
+	c_can_irq_control(priv, true);
 	netif_start_queue(dev);
 
 	return 0;
@@ -1226,7 +1138,6 @@ struct net_device *alloc_c_can_dev(void)
 		return NULL;
 
 	priv = netdev_priv(dev);
-	spin_lock_init(&priv->xmit_lock);
 	netif_napi_add(dev, &priv->napi, c_can_poll, C_CAN_NAPI_WEIGHT);
 
 	priv->dev = dev;
@@ -1281,6 +1192,7 @@ int c_can_power_up(struct net_device *dev)
 	u32 val;
 	unsigned long time_out;
 	struct c_can_priv *priv = netdev_priv(dev);
+	int ret;
 
 	if (!(dev->flags & IFF_UP))
 		return 0;
@@ -1307,7 +1219,11 @@ int c_can_power_up(struct net_device *dev)
 	if (time_after(jiffies, time_out))
 		return -ETIMEDOUT;
 
-	return c_can_start(dev);
+	ret = c_can_start(dev);
+	if (!ret)
+		c_can_irq_control(priv, true);
+
+	return ret;
 }
 EXPORT_SYMBOL_GPL(c_can_power_up);
 #endif
diff --git a/drivers/net/can/c_can/c_can.h b/drivers/net/can/c_can/c_can.h
index faa8404162b..c56f1b1c11c 100644
--- a/drivers/net/can/c_can/c_can.h
+++ b/drivers/net/can/c_can/c_can.h
@@ -22,14 +22,6 @@
 #ifndef C_CAN_H
 #define C_CAN_H
 
-/*
- * IFx register masks:
- * allow easy operation on 16-bit registers when the
- * argument is 32-bit instead
- */
-#define IFX_WRITE_LOW_16BIT(x)	((x) & 0xFFFF)
-#define IFX_WRITE_HIGH_16BIT(x)	(((x) & 0xFFFF0000) >> 16)
-
 /* message object split */
 #define C_CAN_NO_OF_OBJECTS	32
 #define C_CAN_MSG_OBJ_RX_NUM	16
@@ -45,8 +37,6 @@
 
 #define C_CAN_MSG_OBJ_RX_SPLIT	9
 #define C_CAN_MSG_RX_LOW_LAST	(C_CAN_MSG_OBJ_RX_SPLIT - 1)
-
-#define C_CAN_NEXT_MSG_OBJ_MASK	(C_CAN_MSG_OBJ_TX_NUM - 1)
 #define RECEIVE_OBJECT_BITS	0x0000ffff
 
 enum reg {
@@ -183,23 +173,20 @@ struct c_can_priv {
 	struct napi_struct napi;
 	struct net_device *dev;
 	struct device *device;
-	spinlock_t xmit_lock;
-	int tx_object;
-	int current_status;
+	atomic_t tx_active;
+	unsigned long tx_dir;
 	int last_status;
 	u16 (*read_reg) (struct c_can_priv *priv, enum reg index);
 	void (*write_reg) (struct c_can_priv *priv, enum reg index, u16 val);
 	void __iomem *base;
 	const u16 *regs;
-	unsigned long irq_flags; /* for request_irq() */
-	unsigned int tx_next;
-	unsigned int tx_echo;
 	void *priv;		/* for board-specific data */
-	u16 irqstatus;
 	enum c_can_dev_id type;
 	u32 __iomem *raminit_ctrlreg;
-	unsigned int instance;
+	int instance;
 	void (*raminit) (const struct c_can_priv *priv, bool enable);
+	u32 comm_rcv_high;
+	u32 rxmasked;
 	u32 dlc[C_CAN_MSG_OBJ_TX_NUM];
 };
 
diff --git a/drivers/net/can/c_can/c_can_pci.c b/drivers/net/can/c_can/c_can_pci.c
index bce0be54c2f..fe5f6303b58 100644
--- a/drivers/net/can/c_can/c_can_pci.c
+++ b/drivers/net/can/c_can/c_can_pci.c
@@ -84,8 +84,11 @@ static int c_can_pci_probe(struct pci_dev *pdev,
 		goto out_disable_device;
 	}
 
-	pci_set_master(pdev);
-	pci_enable_msi(pdev);
+	ret = pci_enable_msi(pdev);
+	if (!ret) {
+		dev_info(&pdev->dev, "MSI enabled\n");
+		pci_set_master(pdev);
+	}
 
 	addr = pci_iomap(pdev, 0, pci_resource_len(pdev, 0));
 	if (!addr) {
@@ -132,6 +135,8 @@ static int c_can_pci_probe(struct pci_dev *pdev,
 		goto out_free_c_can;
 	}
 
+	priv->type = c_can_pci_data->type;
+
 	/* Configure access to registers */
 	switch (c_can_pci_data->reg_align) {
 	case C_CAN_REG_ALIGN_32:
diff --git a/drivers/net/can/c_can/c_can_platform.c b/drivers/net/can/c_can/c_can_platform.c
index 806d9275342..1df0b322d1e 100644
--- a/drivers/net/can/c_can/c_can_platform.c
+++ b/drivers/net/can/c_can/c_can_platform.c
@@ -222,7 +222,7 @@ static int c_can_plat_probe(struct platform_device *pdev)
 
 		res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
 		priv->raminit_ctrlreg = devm_ioremap_resource(&pdev->dev, res);
-		if (IS_ERR(priv->raminit_ctrlreg) || (int)priv->instance < 0)
+		if (IS_ERR(priv->raminit_ctrlreg) || priv->instance < 0)
 			dev_info(&pdev->dev, "control memory is not used for raminit\n");
 		else
 			priv->raminit = c_can_hw_raminit;
diff --git a/drivers/net/can/dev.c b/drivers/net/can/dev.c
index c7a26047874..e318e87e2bf 100644
--- a/drivers/net/can/dev.c
+++ b/drivers/net/can/dev.c
@@ -256,7 +256,7 @@ static int can_get_bittiming(struct net_device *dev, struct can_bittiming *bt,
 
 	/* Check if the CAN device has bit-timing parameters */
 	if (!btc)
-		return -ENOTSUPP;
+		return -EOPNOTSUPP;
 
 	/*
 	 * Depending on the given can_bittiming parameter structure the CAN
diff --git a/drivers/net/can/sja1000/peak_pci.c b/drivers/net/can/sja1000/peak_pci.c
index c540e3d12e3..564933ae218 100644
--- a/drivers/net/can/sja1000/peak_pci.c
+++ b/drivers/net/can/sja1000/peak_pci.c
@@ -551,7 +551,7 @@ static int peak_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
 	struct sja1000_priv *priv;
 	struct peak_pci_chan *chan;
-	struct net_device *dev;
+	struct net_device *dev, *prev_dev;
 	void __iomem *cfg_base, *reg_base;
 	u16 sub_sys_id, icr;
 	int i, err, channels;
@@ -688,11 +688,13 @@ failure_remove_channels:
 	writew(0x0, cfg_base + PITA_ICR + 2);
 
 	chan = NULL;
-	for (dev = pci_get_drvdata(pdev); dev; dev = chan->prev_dev) {
-		unregister_sja1000dev(dev);
-		free_sja1000dev(dev);
+	for (dev = pci_get_drvdata(pdev); dev; dev = prev_dev) {
 		priv = netdev_priv(dev);
 		chan = priv->priv;
+		prev_dev = chan->prev_dev;
+
+		unregister_sja1000dev(dev);
+		free_sja1000dev(dev);
 	}
 
 	/* free any PCIeC resources too */
@@ -726,10 +728,12 @@ static void peak_pci_remove(struct pci_dev *pdev)
 
 	/* Loop over all registered devices */
 	while (1) {
+		struct net_device *prev_dev = chan->prev_dev;
+
 		dev_info(&pdev->dev, "removing device %s\n", dev->name);
 		unregister_sja1000dev(dev);
 		free_sja1000dev(dev);
-		dev = chan->prev_dev;
+		dev = prev_dev;
 
 		if (!dev) {
 			/* do that only for first channel */
diff --git a/drivers/net/can/sja1000/sja1000_isa.c b/drivers/net/can/sja1000/sja1000_isa.c
index df136a2516c..014695d7e6a 100644
--- a/drivers/net/can/sja1000/sja1000_isa.c
+++ b/drivers/net/can/sja1000/sja1000_isa.c
@@ -46,6 +46,7 @@ static int clk[MAXDEV];
 static unsigned char cdr[MAXDEV] = {[0 ... (MAXDEV - 1)] = 0xff};
 static unsigned char ocr[MAXDEV] = {[0 ... (MAXDEV - 1)] = 0xff};
 static int indirect[MAXDEV] = {[0 ... (MAXDEV - 1)] = -1};
+static spinlock_t indirect_lock[MAXDEV];  /* lock for indirect access mode */
 
 module_param_array(port, ulong, NULL, S_IRUGO);
 MODULE_PARM_DESC(port, "I/O port number");
@@ -101,19 +102,26 @@ static void sja1000_isa_port_write_reg(const struct sja1000_priv *priv,
 static u8 sja1000_isa_port_read_reg_indirect(const struct sja1000_priv *priv,
 					     int reg)
 {
-	unsigned long base = (unsigned long)priv->reg_base;
+	unsigned long flags, base = (unsigned long)priv->reg_base;
+	u8 readval;
 
+	spin_lock_irqsave(&indirect_lock[priv->dev->dev_id], flags);
 	outb(reg, base);
-	return inb(base + 1);
+	readval = inb(base + 1);
+	spin_unlock_irqrestore(&indirect_lock[priv->dev->dev_id], flags);
+
+	return readval;
 }
 
 static void sja1000_isa_port_write_reg_indirect(const struct sja1000_priv *priv,
 						int reg, u8 val)
 {
-	unsigned long base = (unsigned long)priv->reg_base;
+	unsigned long flags, base = (unsigned long)priv->reg_base;
 
+	spin_lock_irqsave(&indirect_lock[priv->dev->dev_id], flags);
 	outb(reg, base);
 	outb(val, base + 1);
+	spin_unlock_irqrestore(&indirect_lock[priv->dev->dev_id], flags);
 }
 
 static int sja1000_isa_probe(struct platform_device *pdev)
@@ -169,6 +177,7 @@ static int sja1000_isa_probe(struct platform_device *pdev)
 		if (iosize == SJA1000_IOSIZE_INDIRECT) {
 			priv->read_reg = sja1000_isa_port_read_reg_indirect;
 			priv->write_reg = sja1000_isa_port_write_reg_indirect;
+			spin_lock_init(&indirect_lock[idx]);
 		} else {
 			priv->read_reg = sja1000_isa_port_read_reg;
 			priv->write_reg = sja1000_isa_port_write_reg;
@@ -198,6 +207,7 @@ static int sja1000_isa_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, dev);
 	SET_NETDEV_DEV(dev, &pdev->dev);
+	dev->dev_id = idx;
 
 	err = register_sja1000dev(dev);
 	if (err) {
diff --git a/drivers/net/can/slcan.c b/drivers/net/can/slcan.c
index f5b16e0e3a1..dcf9196f631 100644
--- a/drivers/net/can/slcan.c
+++ b/drivers/net/can/slcan.c
@@ -322,13 +322,13 @@ static void slcan_write_wakeup(struct tty_struct *tty)
 	if (!sl || sl->magic != SLCAN_MAGIC || !netif_running(sl->dev))
 		return;
 
-	spin_lock(&sl->lock);
+	spin_lock_bh(&sl->lock);
 	if (sl->xleft <= 0)  {
 		/* Now serial buffer is almost free & we can start
 		 * transmission of another packet */
 		sl->dev->stats.tx_packets++;
 		clear_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
-		spin_unlock(&sl->lock);
+		spin_unlock_bh(&sl->lock);
 		netif_wake_queue(sl->dev);
 		return;
 	}
@@ -336,7 +336,7 @@ static void slcan_write_wakeup(struct tty_struct *tty)
 	actual = tty->ops->write(tty, sl->xhead, sl->xleft);
 	sl->xleft -= actual;
 	sl->xhead += actual;
-	spin_unlock(&sl->lock);
+	spin_unlock_bh(&sl->lock);
 }
 
 /* Send a can_frame to a TTY queue. */
diff --git a/drivers/net/ethernet/Kconfig b/drivers/net/ethernet/Kconfig
index 39b26fe28d1..d7401017a3f 100644
--- a/drivers/net/ethernet/Kconfig
+++ b/drivers/net/ethernet/Kconfig
@@ -35,6 +35,18 @@ source "drivers/net/ethernet/calxeda/Kconfig"
 source "drivers/net/ethernet/chelsio/Kconfig"
 source "drivers/net/ethernet/cirrus/Kconfig"
 source "drivers/net/ethernet/cisco/Kconfig"
+
+config CX_ECAT
+	tristate "Beckhoff CX5020 EtherCAT master support"
+	depends on PCI
+	---help---
+	  Driver for EtherCAT master module located on CCAT FPGA
+	  that can be found on Beckhoff CX5020, and possibly other of CX
+	  Beckhoff CX series industrial PCs.
+
+	  To compile this driver as a module, choose M here. The module
+	  will be called ec_bhf.
+
 source "drivers/net/ethernet/davicom/Kconfig"
 
 config DNET
diff --git a/drivers/net/ethernet/Makefile b/drivers/net/ethernet/Makefile
index 545d0b3b9cb..35190e36c45 100644
--- a/drivers/net/ethernet/Makefile
+++ b/drivers/net/ethernet/Makefile
@@ -21,6 +21,7 @@ obj-$(CONFIG_NET_CALXEDA_XGMAC) += calxeda/
 obj-$(CONFIG_NET_VENDOR_CHELSIO) += chelsio/
 obj-$(CONFIG_NET_VENDOR_CIRRUS) += cirrus/
 obj-$(CONFIG_NET_VENDOR_CISCO) += cisco/
+obj-$(CONFIG_CX_ECAT) += ec_bhf.o
 obj-$(CONFIG_DM9000) += davicom/
 obj-$(CONFIG_DNET) += dnet.o
 obj-$(CONFIG_NET_VENDOR_DEC) += dec/
diff --git a/drivers/net/ethernet/altera/Kconfig b/drivers/net/ethernet/altera/Kconfig
index 80c1ab74a4b..fdddba51473 100644
--- a/drivers/net/ethernet/altera/Kconfig
+++ b/drivers/net/ethernet/altera/Kconfig
@@ -1,5 +1,6 @@
 config ALTERA_TSE
 	tristate "Altera Triple-Speed Ethernet MAC support"
+	depends on HAS_DMA
 	select PHYLIB
 	---help---
 	  This driver supports the Altera Triple-Speed (TSE) Ethernet MAC.
diff --git a/drivers/net/ethernet/altera/Makefile b/drivers/net/ethernet/altera/Makefile
index d4a187e4536..3eff2fd3997 100644
--- a/drivers/net/ethernet/altera/Makefile
+++ b/drivers/net/ethernet/altera/Makefile
@@ -5,3 +5,4 @@
 obj-$(CONFIG_ALTERA_TSE) += altera_tse.o
 altera_tse-objs := altera_tse_main.o altera_tse_ethtool.o \
 altera_msgdma.o altera_sgdma.o altera_utils.o
+ccflags-y += -D__CHECK_ENDIAN__
diff --git a/drivers/net/ethernet/altera/altera_msgdma.c b/drivers/net/ethernet/altera/altera_msgdma.c
index 3df18669ea3..0fb986ba329 100644
--- a/drivers/net/ethernet/altera/altera_msgdma.c
+++ b/drivers/net/ethernet/altera/altera_msgdma.c
@@ -18,6 +18,7 @@
 #include "altera_utils.h"
 #include "altera_tse.h"
 #include "altera_msgdmahw.h"
+#include "altera_msgdma.h"
 
 /* No initialization work to do for MSGDMA */
 int msgdma_initialize(struct altera_tse_private *priv)
@@ -29,21 +30,23 @@ void msgdma_uninitialize(struct altera_tse_private *priv)
 {
 }
 
+void msgdma_start_rxdma(struct altera_tse_private *priv)
+{
+}
+
 void msgdma_reset(struct altera_tse_private *priv)
 {
 	int counter;
-	struct msgdma_csr *txcsr =
-		(struct msgdma_csr *)priv->tx_dma_csr;
-	struct msgdma_csr *rxcsr =
-		(struct msgdma_csr *)priv->rx_dma_csr;
 
 	/* Reset Rx mSGDMA */
-	iowrite32(MSGDMA_CSR_STAT_MASK, &rxcsr->status);
-	iowrite32(MSGDMA_CSR_CTL_RESET, &rxcsr->control);
+	csrwr32(MSGDMA_CSR_STAT_MASK, priv->rx_dma_csr,
+		msgdma_csroffs(status));
+	csrwr32(MSGDMA_CSR_CTL_RESET, priv->rx_dma_csr,
+		msgdma_csroffs(control));
 
 	counter = 0;
 	while (counter++ < ALTERA_TSE_SW_RESET_WATCHDOG_CNTR) {
-		if (tse_bit_is_clear(&rxcsr->status,
+		if (tse_bit_is_clear(priv->rx_dma_csr, msgdma_csroffs(status),
 				     MSGDMA_CSR_STAT_RESETTING))
 			break;
 		udelay(1);
@@ -54,15 +57,18 @@ void msgdma_reset(struct altera_tse_private *priv)
 			   "TSE Rx mSGDMA resetting bit never cleared!\n");
 
 	/* clear all status bits */
-	iowrite32(MSGDMA_CSR_STAT_MASK, &rxcsr->status);
+	csrwr32(MSGDMA_CSR_STAT_MASK, priv->rx_dma_csr, msgdma_csroffs(status));
 
 	/* Reset Tx mSGDMA */
-	iowrite32(MSGDMA_CSR_STAT_MASK, &txcsr->status);
-	iowrite32(MSGDMA_CSR_CTL_RESET, &txcsr->control);
+	csrwr32(MSGDMA_CSR_STAT_MASK, priv->tx_dma_csr,
+		msgdma_csroffs(status));
+
+	csrwr32(MSGDMA_CSR_CTL_RESET, priv->tx_dma_csr,
+		msgdma_csroffs(control));
 
 	counter = 0;
 	while (counter++ < ALTERA_TSE_SW_RESET_WATCHDOG_CNTR) {
-		if (tse_bit_is_clear(&txcsr->status,
+		if (tse_bit_is_clear(priv->tx_dma_csr, msgdma_csroffs(status),
 				     MSGDMA_CSR_STAT_RESETTING))
 			break;
 		udelay(1);
@@ -73,58 +79,58 @@ void msgdma_reset(struct altera_tse_private *priv)
 			   "TSE Tx mSGDMA resetting bit never cleared!\n");
 
 	/* clear all status bits */
-	iowrite32(MSGDMA_CSR_STAT_MASK, &txcsr->status);
+	csrwr32(MSGDMA_CSR_STAT_MASK, priv->tx_dma_csr, msgdma_csroffs(status));
 }
 
 void msgdma_disable_rxirq(struct altera_tse_private *priv)
 {
-	struct msgdma_csr *csr = priv->rx_dma_csr;
-	tse_clear_bit(&csr->control, MSGDMA_CSR_CTL_GLOBAL_INTR);
+	tse_clear_bit(priv->rx_dma_csr, msgdma_csroffs(control),
+		      MSGDMA_CSR_CTL_GLOBAL_INTR);
 }
 
 void msgdma_enable_rxirq(struct altera_tse_private *priv)
 {
-	struct msgdma_csr *csr = priv->rx_dma_csr;
-	tse_set_bit(&csr->control, MSGDMA_CSR_CTL_GLOBAL_INTR);
+	tse_set_bit(priv->rx_dma_csr, msgdma_csroffs(control),
+		    MSGDMA_CSR_CTL_GLOBAL_INTR);
 }
 
 void msgdma_disable_txirq(struct altera_tse_private *priv)
 {
-	struct msgdma_csr *csr = priv->tx_dma_csr;
-	tse_clear_bit(&csr->control, MSGDMA_CSR_CTL_GLOBAL_INTR);
+	tse_clear_bit(priv->tx_dma_csr, msgdma_csroffs(control),
+		      MSGDMA_CSR_CTL_GLOBAL_INTR);
 }
 
 void msgdma_enable_txirq(struct altera_tse_private *priv)
 {
-	struct msgdma_csr *csr = priv->tx_dma_csr;
-	tse_set_bit(&csr->control, MSGDMA_CSR_CTL_GLOBAL_INTR);
+	tse_set_bit(priv->tx_dma_csr, msgdma_csroffs(control),
+		    MSGDMA_CSR_CTL_GLOBAL_INTR);
 }
 
 void msgdma_clear_rxirq(struct altera_tse_private *priv)
 {
-	struct msgdma_csr *csr = priv->rx_dma_csr;
-	iowrite32(MSGDMA_CSR_STAT_IRQ, &csr->status);
+	csrwr32(MSGDMA_CSR_STAT_IRQ, priv->rx_dma_csr, msgdma_csroffs(status));
 }
 
 void msgdma_clear_txirq(struct altera_tse_private *priv)
 {
-	struct msgdma_csr *csr = priv->tx_dma_csr;
-	iowrite32(MSGDMA_CSR_STAT_IRQ, &csr->status);
+	csrwr32(MSGDMA_CSR_STAT_IRQ, priv->tx_dma_csr, msgdma_csroffs(status));
 }
 
 /* return 0 to indicate transmit is pending */
 int msgdma_tx_buffer(struct altera_tse_private *priv, struct tse_buffer *buffer)
 {
-	struct msgdma_extended_desc *desc = priv->tx_dma_desc;
-
-	iowrite32(lower_32_bits(buffer->dma_addr), &desc->read_addr_lo);
-	iowrite32(upper_32_bits(buffer->dma_addr), &desc->read_addr_hi);
-	iowrite32(0, &desc->write_addr_lo);
-	iowrite32(0, &desc->write_addr_hi);
-	iowrite32(buffer->len, &desc->len);
-	iowrite32(0, &desc->burst_seq_num);
-	iowrite32(MSGDMA_DESC_TX_STRIDE, &desc->stride);
-	iowrite32(MSGDMA_DESC_CTL_TX_SINGLE, &desc->control);
+	csrwr32(lower_32_bits(buffer->dma_addr), priv->tx_dma_desc,
+		msgdma_descroffs(read_addr_lo));
+	csrwr32(upper_32_bits(buffer->dma_addr), priv->tx_dma_desc,
+		msgdma_descroffs(read_addr_hi));
+	csrwr32(0, priv->tx_dma_desc, msgdma_descroffs(write_addr_lo));
+	csrwr32(0, priv->tx_dma_desc, msgdma_descroffs(write_addr_hi));
+	csrwr32(buffer->len, priv->tx_dma_desc, msgdma_descroffs(len));
+	csrwr32(0, priv->tx_dma_desc, msgdma_descroffs(burst_seq_num));
+	csrwr32(MSGDMA_DESC_TX_STRIDE, priv->tx_dma_desc,
+		msgdma_descroffs(stride));
+	csrwr32(MSGDMA_DESC_CTL_TX_SINGLE, priv->tx_dma_desc,
+		msgdma_descroffs(control));
 	return 0;
 }
 
@@ -133,17 +139,16 @@ u32 msgdma_tx_completions(struct altera_tse_private *priv)
 	u32 ready = 0;
 	u32 inuse;
 	u32 status;
-	struct msgdma_csr *txcsr =
-		(struct msgdma_csr *)priv->tx_dma_csr;
 
 	/* Get number of sent descriptors */
-	inuse = ioread32(&txcsr->rw_fill_level) & 0xffff;
+	inuse = csrrd32(priv->tx_dma_csr, msgdma_csroffs(rw_fill_level))
+			& 0xffff;
 
 	if (inuse) { /* Tx FIFO is not empty */
 		ready = priv->tx_prod - priv->tx_cons - inuse - 1;
 	} else {
 		/* Check for buffered last packet */
-		status = ioread32(&txcsr->status);
+		status = csrrd32(priv->tx_dma_csr, msgdma_csroffs(status));
 		if (status & MSGDMA_CSR_STAT_BUSY)
 			ready = priv->tx_prod - priv->tx_cons - 1;
 		else
@@ -154,10 +159,9 @@ u32 msgdma_tx_completions(struct altera_tse_private *priv)
 
 /* Put buffer to the mSGDMA RX FIFO
  */
-int msgdma_add_rx_desc(struct altera_tse_private *priv,
+void msgdma_add_rx_desc(struct altera_tse_private *priv,
 			struct tse_buffer *rxbuffer)
 {
-	struct msgdma_extended_desc *desc = priv->rx_dma_desc;
 	u32 len = priv->rx_dma_buf_sz;
 	dma_addr_t dma_addr = rxbuffer->dma_addr;
 	u32 control = (MSGDMA_DESC_CTL_END_ON_EOP
@@ -167,15 +171,16 @@ int msgdma_add_rx_desc(struct altera_tse_private *priv,
 			| MSGDMA_DESC_CTL_TR_ERR_IRQ
 			| MSGDMA_DESC_CTL_GO);
 
-	iowrite32(0, &desc->read_addr_lo);
-	iowrite32(0, &desc->read_addr_hi);
-	iowrite32(lower_32_bits(dma_addr), &desc->write_addr_lo);
-	iowrite32(upper_32_bits(dma_addr), &desc->write_addr_hi);
-	iowrite32(len, &desc->len);
-	iowrite32(0, &desc->burst_seq_num);
-	iowrite32(0x00010001, &desc->stride);
-	iowrite32(control, &desc->control);
-	return 1;
+	csrwr32(0, priv->rx_dma_desc, msgdma_descroffs(read_addr_lo));
+	csrwr32(0, priv->rx_dma_desc, msgdma_descroffs(read_addr_hi));
+	csrwr32(lower_32_bits(dma_addr), priv->rx_dma_desc,
+		msgdma_descroffs(write_addr_lo));
+	csrwr32(upper_32_bits(dma_addr), priv->rx_dma_desc,
+		msgdma_descroffs(write_addr_hi));
+	csrwr32(len, priv->rx_dma_desc, msgdma_descroffs(len));
+	csrwr32(0, priv->rx_dma_desc, msgdma_descroffs(burst_seq_num));
+	csrwr32(0x00010001, priv->rx_dma_desc, msgdma_descroffs(stride));
+	csrwr32(control, priv->rx_dma_desc, msgdma_descroffs(control));
 }
 
 /* status is returned on upper 16 bits,
@@ -186,14 +191,13 @@ u32 msgdma_rx_status(struct altera_tse_private *priv)
 	u32 rxstatus = 0;
 	u32 pktlength;
 	u32 pktstatus;
-	struct msgdma_csr *rxcsr =
-		(struct msgdma_csr *)priv->rx_dma_csr;
-	struct msgdma_response *rxresp =
-		(struct msgdma_response *)priv->rx_dma_resp;
-
-	if (ioread32(&rxcsr->resp_fill_level) & 0xffff) {
-		pktlength = ioread32(&rxresp->bytes_transferred);
-		pktstatus = ioread32(&rxresp->status);
+
+	if (csrrd32(priv->rx_dma_csr, msgdma_csroffs(resp_fill_level))
+	    & 0xffff) {
+		pktlength = csrrd32(priv->rx_dma_resp,
+				    msgdma_respoffs(bytes_transferred));
+		pktstatus = csrrd32(priv->rx_dma_resp,
+				    msgdma_respoffs(status));
 		rxstatus = pktstatus;
 		rxstatus = rxstatus << 16;
 		rxstatus |= (pktlength & 0xffff);
diff --git a/drivers/net/ethernet/altera/altera_msgdma.h b/drivers/net/ethernet/altera/altera_msgdma.h
index 7f0f5bf2bba..42cf61c8105 100644
--- a/drivers/net/ethernet/altera/altera_msgdma.h
+++ b/drivers/net/ethernet/altera/altera_msgdma.h
@@ -25,10 +25,11 @@ void msgdma_disable_txirq(struct altera_tse_private *);
 void msgdma_clear_rxirq(struct altera_tse_private *);
 void msgdma_clear_txirq(struct altera_tse_private *);
 u32 msgdma_tx_completions(struct altera_tse_private *);
-int msgdma_add_rx_desc(struct altera_tse_private *, struct tse_buffer *);
+void msgdma_add_rx_desc(struct altera_tse_private *, struct tse_buffer *);
 int msgdma_tx_buffer(struct altera_tse_private *, struct tse_buffer *);
 u32 msgdma_rx_status(struct altera_tse_private *);
 int msgdma_initialize(struct altera_tse_private *);
 void msgdma_uninitialize(struct altera_tse_private *);
+void msgdma_start_rxdma(struct altera_tse_private *);
 
 #endif /*  __ALTERA_MSGDMA_H__ */
diff --git a/drivers/net/ethernet/altera/altera_msgdmahw.h b/drivers/net/ethernet/altera/altera_msgdmahw.h
index d7b59ba4019..e335626e1b6 100644
--- a/drivers/net/ethernet/altera/altera_msgdmahw.h
+++ b/drivers/net/ethernet/altera/altera_msgdmahw.h
@@ -17,15 +17,6 @@
 #ifndef __ALTERA_MSGDMAHW_H__
 #define __ALTERA_MSGDMAHW_H__
 
-/* mSGDMA standard descriptor format
- */
-struct msgdma_desc {
-	u32 read_addr;	/* data buffer source address */
-	u32 write_addr;	/* data buffer destination address */
-	u32 len;	/* the number of bytes to transfer per descriptor */
-	u32 control;	/* characteristics of the transfer */
-};
-
 /* mSGDMA extended descriptor format
  */
 struct msgdma_extended_desc {
@@ -159,6 +150,10 @@ struct msgdma_response {
 	u32 status;
 };
 
+#define msgdma_respoffs(a) (offsetof(struct msgdma_response, a))
+#define msgdma_csroffs(a) (offsetof(struct msgdma_csr, a))
+#define msgdma_descroffs(a) (offsetof(struct msgdma_extended_desc, a))
+
 /* mSGDMA response register bit definitions
  */
 #define MSGDMA_RESP_EARLY_TERM	BIT(8)
diff --git a/drivers/net/ethernet/altera/altera_sgdma.c b/drivers/net/ethernet/altera/altera_sgdma.c
index 0ee96639ae4..99cc56f451c 100644
--- a/drivers/net/ethernet/altera/altera_sgdma.c
+++ b/drivers/net/ethernet/altera/altera_sgdma.c
@@ -20,28 +20,28 @@
 #include "altera_sgdmahw.h"
 #include "altera_sgdma.h"
 
-static void sgdma_descrip(struct sgdma_descrip *desc,
-			  struct sgdma_descrip *ndesc,
-			  dma_addr_t ndesc_phys,
-			  dma_addr_t raddr,
-			  dma_addr_t waddr,
-			  u16 length,
-			  int generate_eop,
-			  int rfixed,
-			  int wfixed);
+static void sgdma_setup_descrip(struct sgdma_descrip __iomem *desc,
+				struct sgdma_descrip __iomem *ndesc,
+				dma_addr_t ndesc_phys,
+				dma_addr_t raddr,
+				dma_addr_t waddr,
+				u16 length,
+				int generate_eop,
+				int rfixed,
+				int wfixed);
 
 static int sgdma_async_write(struct altera_tse_private *priv,
-			      struct sgdma_descrip *desc);
+			      struct sgdma_descrip __iomem *desc);
 
 static int sgdma_async_read(struct altera_tse_private *priv);
 
 static dma_addr_t
 sgdma_txphysaddr(struct altera_tse_private *priv,
-		 struct sgdma_descrip *desc);
+		 struct sgdma_descrip __iomem *desc);
 
 static dma_addr_t
 sgdma_rxphysaddr(struct altera_tse_private *priv,
-		 struct sgdma_descrip *desc);
+		 struct sgdma_descrip __iomem *desc);
 
 static int sgdma_txbusy(struct altera_tse_private *priv);
 
@@ -64,18 +64,23 @@ queue_rx_peekhead(struct altera_tse_private *priv);
 
 int sgdma_initialize(struct altera_tse_private *priv)
 {
-	priv->txctrlreg = SGDMA_CTRLREG_ILASTD;
+	priv->txctrlreg = SGDMA_CTRLREG_ILASTD |
+		      SGDMA_CTRLREG_INTEN;
 
 	priv->rxctrlreg = SGDMA_CTRLREG_IDESCRIP |
+		      SGDMA_CTRLREG_INTEN |
 		      SGDMA_CTRLREG_ILASTD;
 
+	priv->sgdmadesclen = sizeof(struct sgdma_descrip);
+
 	INIT_LIST_HEAD(&priv->txlisthd);
 	INIT_LIST_HEAD(&priv->rxlisthd);
 
 	priv->rxdescphys = (dma_addr_t) 0;
 	priv->txdescphys = (dma_addr_t) 0;
 
-	priv->rxdescphys = dma_map_single(priv->device, priv->rx_dma_desc,
+	priv->rxdescphys = dma_map_single(priv->device,
+					  (void __force *)priv->rx_dma_desc,
 					  priv->rxdescmem, DMA_BIDIRECTIONAL);
 
 	if (dma_mapping_error(priv->device, priv->rxdescphys)) {
@@ -84,7 +89,8 @@ int sgdma_initialize(struct altera_tse_private *priv)
 		return -EINVAL;
 	}
 
-	priv->txdescphys = dma_map_single(priv->device, priv->tx_dma_desc,
+	priv->txdescphys = dma_map_single(priv->device,
+					  (void __force *)priv->tx_dma_desc,
 					  priv->txdescmem, DMA_TO_DEVICE);
 
 	if (dma_mapping_error(priv->device, priv->txdescphys)) {
@@ -93,6 +99,16 @@ int sgdma_initialize(struct altera_tse_private *priv)
 		return -EINVAL;
 	}
 
+	/* Initialize descriptor memory to all 0's, sync memory to cache */
+	memset_io(priv->tx_dma_desc, 0, priv->txdescmem);
+	memset_io(priv->rx_dma_desc, 0, priv->rxdescmem);
+
+	dma_sync_single_for_device(priv->device, priv->txdescphys,
+				   priv->txdescmem, DMA_TO_DEVICE);
+
+	dma_sync_single_for_device(priv->device, priv->rxdescphys,
+				   priv->rxdescmem, DMA_TO_DEVICE);
+
 	return 0;
 }
 
@@ -112,58 +128,48 @@ void sgdma_uninitialize(struct altera_tse_private *priv)
  */
 void sgdma_reset(struct altera_tse_private *priv)
 {
-	u32 *ptxdescripmem = (u32 *)priv->tx_dma_desc;
-	u32 txdescriplen   = priv->txdescmem;
-	u32 *prxdescripmem = (u32 *)priv->rx_dma_desc;
-	u32 rxdescriplen   = priv->rxdescmem;
-	struct sgdma_csr *ptxsgdma = (struct sgdma_csr *)priv->tx_dma_csr;
-	struct sgdma_csr *prxsgdma = (struct sgdma_csr *)priv->rx_dma_csr;
-
 	/* Initialize descriptor memory to 0 */
-	memset(ptxdescripmem, 0, txdescriplen);
-	memset(prxdescripmem, 0, rxdescriplen);
+	memset_io(priv->tx_dma_desc, 0, priv->txdescmem);
+	memset_io(priv->rx_dma_desc, 0, priv->rxdescmem);
 
-	iowrite32(SGDMA_CTRLREG_RESET, &ptxsgdma->control);
-	iowrite32(0, &ptxsgdma->control);
+	csrwr32(SGDMA_CTRLREG_RESET, priv->tx_dma_csr, sgdma_csroffs(control));
+	csrwr32(0, priv->tx_dma_csr, sgdma_csroffs(control));
 
-	iowrite32(SGDMA_CTRLREG_RESET, &prxsgdma->control);
-	iowrite32(0, &prxsgdma->control);
+	csrwr32(SGDMA_CTRLREG_RESET, priv->rx_dma_csr, sgdma_csroffs(control));
+	csrwr32(0, priv->rx_dma_csr, sgdma_csroffs(control));
 }
 
+/* For SGDMA, interrupts remain enabled after initially enabling,
+ * so no need to provide implementations for abstract enable
+ * and disable
+ */
+
 void sgdma_enable_rxirq(struct altera_tse_private *priv)
 {
-	struct sgdma_csr *csr = (struct sgdma_csr *)priv->rx_dma_csr;
-	priv->rxctrlreg |= SGDMA_CTRLREG_INTEN;
-	tse_set_bit(&csr->control, SGDMA_CTRLREG_INTEN);
 }
 
 void sgdma_enable_txirq(struct altera_tse_private *priv)
 {
-	struct sgdma_csr *csr = (struct sgdma_csr *)priv->tx_dma_csr;
-	priv->txctrlreg |= SGDMA_CTRLREG_INTEN;
-	tse_set_bit(&csr->control, SGDMA_CTRLREG_INTEN);
 }
 
-/* for SGDMA, RX interrupts remain enabled after enabling */
 void sgdma_disable_rxirq(struct altera_tse_private *priv)
 {
 }
 
-/* for SGDMA, TX interrupts remain enabled after enabling */
 void sgdma_disable_txirq(struct altera_tse_private *priv)
 {
 }
 
 void sgdma_clear_rxirq(struct altera_tse_private *priv)
 {
-	struct sgdma_csr *csr = (struct sgdma_csr *)priv->rx_dma_csr;
-	tse_set_bit(&csr->control, SGDMA_CTRLREG_CLRINT);
+	tse_set_bit(priv->rx_dma_csr, sgdma_csroffs(control),
+		    SGDMA_CTRLREG_CLRINT);
 }
 
 void sgdma_clear_txirq(struct altera_tse_private *priv)
 {
-	struct sgdma_csr *csr = (struct sgdma_csr *)priv->tx_dma_csr;
-	tse_set_bit(&csr->control, SGDMA_CTRLREG_CLRINT);
+	tse_set_bit(priv->tx_dma_csr, sgdma_csroffs(control),
+		    SGDMA_CTRLREG_CLRINT);
 }
 
 /* transmits buffer through SGDMA. Returns number of buffers
@@ -173,28 +179,27 @@ void sgdma_clear_txirq(struct altera_tse_private *priv)
  */
 int sgdma_tx_buffer(struct altera_tse_private *priv, struct tse_buffer *buffer)
 {
-	int pktstx = 0;
-	struct sgdma_descrip *descbase =
-		(struct sgdma_descrip *)priv->tx_dma_desc;
+	struct sgdma_descrip __iomem *descbase =
+		(struct sgdma_descrip __iomem *)priv->tx_dma_desc;
 
-	struct sgdma_descrip *cdesc = &descbase[0];
-	struct sgdma_descrip *ndesc = &descbase[1];
+	struct sgdma_descrip __iomem *cdesc = &descbase[0];
+	struct sgdma_descrip __iomem *ndesc = &descbase[1];
 
 	/* wait 'til the tx sgdma is ready for the next transmit request */
 	if (sgdma_txbusy(priv))
 		return 0;
 
-	sgdma_descrip(cdesc,			/* current descriptor */
-		      ndesc,			/* next descriptor */
-		      sgdma_txphysaddr(priv, ndesc),
-		      buffer->dma_addr,		/* address of packet to xmit */
-		      0,			/* write addr 0 for tx dma */
-		      buffer->len,		/* length of packet */
-		      SGDMA_CONTROL_EOP,	/* Generate EOP */
-		      0,			/* read fixed */
-		      SGDMA_CONTROL_WR_FIXED);	/* Generate SOP */
+	sgdma_setup_descrip(cdesc,			/* current descriptor */
+			    ndesc,			/* next descriptor */
+			    sgdma_txphysaddr(priv, ndesc),
+			    buffer->dma_addr,		/* address of packet to xmit */
+			    0,				/* write addr 0 for tx dma */
+			    buffer->len,		/* length of packet */
+			    SGDMA_CONTROL_EOP,		/* Generate EOP */
+			    0,				/* read fixed */
+			    SGDMA_CONTROL_WR_FIXED);	/* Generate SOP */
 
-	pktstx = sgdma_async_write(priv, cdesc);
+	sgdma_async_write(priv, cdesc);
 
 	/* enqueue the request to the pending transmit queue */
 	queue_tx(priv, buffer);
@@ -208,10 +213,10 @@ int sgdma_tx_buffer(struct altera_tse_private *priv, struct tse_buffer *buffer)
 u32 sgdma_tx_completions(struct altera_tse_private *priv)
 {
 	u32 ready = 0;
-	struct sgdma_descrip *desc = (struct sgdma_descrip *)priv->tx_dma_desc;
 
 	if (!sgdma_txbusy(priv) &&
-	    ((desc->control & SGDMA_CONTROL_HW_OWNED) == 0) &&
+	    ((csrrd8(priv->tx_dma_desc, sgdma_descroffs(control))
+	     & SGDMA_CONTROL_HW_OWNED) == 0) &&
 	    (dequeue_tx(priv))) {
 		ready = 1;
 	}
@@ -219,11 +224,15 @@ u32 sgdma_tx_completions(struct altera_tse_private *priv)
 	return ready;
 }
 
-int sgdma_add_rx_desc(struct altera_tse_private *priv,
-		      struct tse_buffer *rxbuffer)
+void sgdma_start_rxdma(struct altera_tse_private *priv)
+{
+	sgdma_async_read(priv);
+}
+
+void sgdma_add_rx_desc(struct altera_tse_private *priv,
+		       struct tse_buffer *rxbuffer)
 {
 	queue_rx(priv, rxbuffer);
-	return sgdma_async_read(priv);
 }
 
 /* status is returned on upper 16 bits,
@@ -231,38 +240,62 @@ int sgdma_add_rx_desc(struct altera_tse_private *priv,
  */
 u32 sgdma_rx_status(struct altera_tse_private *priv)
 {
-	struct sgdma_csr *csr = (struct sgdma_csr *)priv->rx_dma_csr;
-	struct sgdma_descrip *base = (struct sgdma_descrip *)priv->rx_dma_desc;
-	struct sgdma_descrip *desc = NULL;
-	int pktsrx;
-	unsigned int rxstatus = 0;
-	unsigned int pktlength = 0;
-	unsigned int pktstatus = 0;
+	struct sgdma_descrip __iomem *base =
+		(struct sgdma_descrip __iomem *)priv->rx_dma_desc;
+	struct sgdma_descrip __iomem *desc = NULL;
 	struct tse_buffer *rxbuffer = NULL;
+	unsigned int rxstatus = 0;
 
-	dma_sync_single_for_cpu(priv->device,
-				priv->rxdescphys,
-				priv->rxdescmem,
-				DMA_BIDIRECTIONAL);
+	u32 sts = csrrd32(priv->rx_dma_csr, sgdma_csroffs(status));
 
 	desc = &base[0];
-	if ((ioread32(&csr->status) & SGDMA_STSREG_EOP) ||
-	    (desc->status & SGDMA_STATUS_EOP)) {
-		pktlength = desc->bytes_xferred;
-		pktstatus = desc->status & 0x3f;
-		rxstatus = pktstatus;
+	if (sts & SGDMA_STSREG_EOP) {
+		unsigned int pktlength = 0;
+		unsigned int pktstatus = 0;
+		dma_sync_single_for_cpu(priv->device,
+					priv->rxdescphys,
+					priv->sgdmadesclen,
+					DMA_FROM_DEVICE);
+
+		pktlength = csrrd16(desc, sgdma_descroffs(bytes_xferred));
+		pktstatus = csrrd8(desc, sgdma_descroffs(status));
+		rxstatus = pktstatus & ~SGDMA_STATUS_EOP;
 		rxstatus = rxstatus << 16;
 		rxstatus |= (pktlength & 0xffff);
 
-		desc->status = 0;
-
-		rxbuffer = dequeue_rx(priv);
-		if (rxbuffer == NULL)
+		if (rxstatus) {
+			csrwr8(0, desc, sgdma_descroffs(status));
+
+			rxbuffer = dequeue_rx(priv);
+			if (rxbuffer == NULL)
+				netdev_info(priv->dev,
+					    "sgdma rx and rx queue empty!\n");
+
+			/* Clear control */
+			csrwr32(0, priv->rx_dma_csr, sgdma_csroffs(control));
+			/* clear status */
+			csrwr32(0xf, priv->rx_dma_csr, sgdma_csroffs(status));
+
+			/* kick the rx sgdma after reaping this descriptor */
+			sgdma_async_read(priv);
+
+		} else {
+			/* If the SGDMA indicated an end of packet on recv,
+			 * then it's expected that the rxstatus from the
+			 * descriptor is non-zero - meaning a valid packet
+			 * with a nonzero length, or an error has been
+			 * indicated. if not, then all we can do is signal
+			 * an error and return no packet received. Most likely
+			 * there is a system design error, or an error in the
+			 * underlying kernel (cache or cache management problem)
+			 */
 			netdev_err(priv->dev,
-				   "sgdma rx and rx queue empty!\n");
-
-		/* kick the rx sgdma after reaping this descriptor */
-		pktsrx = sgdma_async_read(priv);
+				   "SGDMA RX Error Info: %x, %x, %x\n",
+				   sts, csrrd8(desc, sgdma_descroffs(status)),
+				   rxstatus);
+		}
+	} else if (sts == 0) {
+		sgdma_async_read(priv);
 	}
 
 	return rxstatus;
@@ -270,38 +303,41 @@ u32 sgdma_rx_status(struct altera_tse_private *priv)
 
 
 /* Private functions */
-static void sgdma_descrip(struct sgdma_descrip *desc,
-			  struct sgdma_descrip *ndesc,
-			  dma_addr_t ndesc_phys,
-			  dma_addr_t raddr,
-			  dma_addr_t waddr,
-			  u16 length,
-			  int generate_eop,
-			  int rfixed,
-			  int wfixed)
+static void sgdma_setup_descrip(struct sgdma_descrip __iomem *desc,
+				struct sgdma_descrip __iomem *ndesc,
+				dma_addr_t ndesc_phys,
+				dma_addr_t raddr,
+				dma_addr_t waddr,
+				u16 length,
+				int generate_eop,
+				int rfixed,
+				int wfixed)
 {
 	/* Clear the next descriptor as not owned by hardware */
-	u32 ctrl = ndesc->control;
+
+	u32 ctrl = csrrd8(ndesc, sgdma_descroffs(control));
 	ctrl &= ~SGDMA_CONTROL_HW_OWNED;
-	ndesc->control = ctrl;
+	csrwr8(ctrl, ndesc, sgdma_descroffs(control));
 
-	ctrl = 0;
 	ctrl = SGDMA_CONTROL_HW_OWNED;
 	ctrl |= generate_eop;
 	ctrl |= rfixed;
 	ctrl |= wfixed;
 
 	/* Channel is implicitly zero, initialized to 0 by default */
-
-	desc->raddr = raddr;
-	desc->waddr = waddr;
-	desc->next = lower_32_bits(ndesc_phys);
-	desc->control = ctrl;
-	desc->status = 0;
-	desc->rburst = 0;
-	desc->wburst = 0;
-	desc->bytes = length;
-	desc->bytes_xferred = 0;
+	csrwr32(lower_32_bits(raddr), desc, sgdma_descroffs(raddr));
+	csrwr32(lower_32_bits(waddr), desc, sgdma_descroffs(waddr));
+
+	csrwr32(0, desc, sgdma_descroffs(pad1));
+	csrwr32(0, desc, sgdma_descroffs(pad2));
+	csrwr32(lower_32_bits(ndesc_phys), desc, sgdma_descroffs(next));
+
+	csrwr8(ctrl, desc, sgdma_descroffs(control));
+	csrwr8(0, desc, sgdma_descroffs(status));
+	csrwr8(0, desc, sgdma_descroffs(wburst));
+	csrwr8(0, desc, sgdma_descroffs(rburst));
+	csrwr16(length, desc, sgdma_descroffs(bytes));
+	csrwr16(0, desc, sgdma_descroffs(bytes_xferred));
 }
 
 /* If hardware is busy, don't restart async read.
@@ -312,48 +348,43 @@ static void sgdma_descrip(struct sgdma_descrip *desc,
  */
 static int sgdma_async_read(struct altera_tse_private *priv)
 {
-	struct sgdma_csr *csr = (struct sgdma_csr *)priv->rx_dma_csr;
-	struct sgdma_descrip *descbase =
-		(struct sgdma_descrip *)priv->rx_dma_desc;
+	struct sgdma_descrip __iomem *descbase =
+		(struct sgdma_descrip __iomem *)priv->rx_dma_desc;
 
-	struct sgdma_descrip *cdesc = &descbase[0];
-	struct sgdma_descrip *ndesc = &descbase[1];
+	struct sgdma_descrip __iomem *cdesc = &descbase[0];
+	struct sgdma_descrip __iomem *ndesc = &descbase[1];
 
-	unsigned int sts = ioread32(&csr->status);
 	struct tse_buffer *rxbuffer = NULL;
 
 	if (!sgdma_rxbusy(priv)) {
 		rxbuffer = queue_rx_peekhead(priv);
-		if (rxbuffer == NULL)
+		if (rxbuffer == NULL) {
+			netdev_err(priv->dev, "no rx buffers available\n");
 			return 0;
-
-		sgdma_descrip(cdesc,		/* current descriptor */
-			      ndesc,		/* next descriptor */
-			      sgdma_rxphysaddr(priv, ndesc),
-			      0,		/* read addr 0 for rx dma */
-			      rxbuffer->dma_addr, /* write addr for rx dma */
-			      0,		/* read 'til EOP */
-			      0,		/* EOP: NA for rx dma */
-			      0,		/* read fixed: NA for rx dma */
-			      0);		/* SOP: NA for rx DMA */
-
-		/* clear control and status */
-		iowrite32(0, &csr->control);
-
-		/* If status available, clear those bits */
-		if (sts & 0xf)
-			iowrite32(0xf, &csr->status);
+		}
+
+		sgdma_setup_descrip(cdesc,		/* current descriptor */
+				    ndesc,		/* next descriptor */
+				    sgdma_rxphysaddr(priv, ndesc),
+				    0,			/* read addr 0 for rx dma */
+				    rxbuffer->dma_addr, /* write addr for rx dma */
+				    0,			/* read 'til EOP */
+				    0,			/* EOP: NA for rx dma */
+				    0,			/* read fixed: NA for rx dma */
+				    0);			/* SOP: NA for rx DMA */
 
 		dma_sync_single_for_device(priv->device,
 					   priv->rxdescphys,
-					   priv->rxdescmem,
-					   DMA_BIDIRECTIONAL);
+					   priv->sgdmadesclen,
+					   DMA_TO_DEVICE);
 
-		iowrite32(lower_32_bits(sgdma_rxphysaddr(priv, cdesc)),
-			  &csr->next_descrip);
+		csrwr32(lower_32_bits(sgdma_rxphysaddr(priv, cdesc)),
+			priv->rx_dma_csr,
+			sgdma_csroffs(next_descrip));
 
-		iowrite32((priv->rxctrlreg | SGDMA_CTRLREG_START),
-			  &csr->control);
+		csrwr32((priv->rxctrlreg | SGDMA_CTRLREG_START),
+			priv->rx_dma_csr,
+			sgdma_csroffs(control));
 
 		return 1;
 	}
@@ -362,32 +393,32 @@ static int sgdma_async_read(struct altera_tse_private *priv)
 }
 
 static int sgdma_async_write(struct altera_tse_private *priv,
-			     struct sgdma_descrip *desc)
+			     struct sgdma_descrip __iomem *desc)
 {
-	struct sgdma_csr *csr = (struct sgdma_csr *)priv->tx_dma_csr;
-
 	if (sgdma_txbusy(priv))
 		return 0;
 
 	/* clear control and status */
-	iowrite32(0, &csr->control);
-	iowrite32(0x1f, &csr->status);
+	csrwr32(0, priv->tx_dma_csr, sgdma_csroffs(control));
+	csrwr32(0x1f, priv->tx_dma_csr, sgdma_csroffs(status));
 
 	dma_sync_single_for_device(priv->device, priv->txdescphys,
-				   priv->txdescmem, DMA_TO_DEVICE);
+				   priv->sgdmadesclen, DMA_TO_DEVICE);
 
-	iowrite32(lower_32_bits(sgdma_txphysaddr(priv, desc)),
-		  &csr->next_descrip);
+	csrwr32(lower_32_bits(sgdma_txphysaddr(priv, desc)),
+		priv->tx_dma_csr,
+		sgdma_csroffs(next_descrip));
 
-	iowrite32((priv->txctrlreg | SGDMA_CTRLREG_START),
-		  &csr->control);
+	csrwr32((priv->txctrlreg | SGDMA_CTRLREG_START),
+		priv->tx_dma_csr,
+		sgdma_csroffs(control));
 
 	return 1;
 }
 
 static dma_addr_t
 sgdma_txphysaddr(struct altera_tse_private *priv,
-		 struct sgdma_descrip *desc)
+		 struct sgdma_descrip __iomem *desc)
 {
 	dma_addr_t paddr = priv->txdescmem_busaddr;
 	uintptr_t offs = (uintptr_t)desc - (uintptr_t)priv->tx_dma_desc;
@@ -396,7 +427,7 @@ sgdma_txphysaddr(struct altera_tse_private *priv,
 
 static dma_addr_t
 sgdma_rxphysaddr(struct altera_tse_private *priv,
-		 struct sgdma_descrip *desc)
+		 struct sgdma_descrip __iomem *desc)
 {
 	dma_addr_t paddr = priv->rxdescmem_busaddr;
 	uintptr_t offs = (uintptr_t)desc - (uintptr_t)priv->rx_dma_desc;
@@ -485,8 +516,8 @@ queue_rx_peekhead(struct altera_tse_private *priv)
  */
 static int sgdma_rxbusy(struct altera_tse_private *priv)
 {
-	struct sgdma_csr *csr = (struct sgdma_csr *)priv->rx_dma_csr;
-	return ioread32(&csr->status) & SGDMA_STSREG_BUSY;
+	return csrrd32(priv->rx_dma_csr, sgdma_csroffs(status))
+		       & SGDMA_STSREG_BUSY;
 }
 
 /* waits for the tx sgdma to finish it's current operation, returns 0
@@ -495,13 +526,14 @@ static int sgdma_rxbusy(struct altera_tse_private *priv)
 static int sgdma_txbusy(struct altera_tse_private *priv)
 {
 	int delay = 0;
-	struct sgdma_csr *csr = (struct sgdma_csr *)priv->tx_dma_csr;
 
 	/* if DMA is busy, wait for current transactino to finish */
-	while ((ioread32(&csr->status) & SGDMA_STSREG_BUSY) && (delay++ < 100))
+	while ((csrrd32(priv->tx_dma_csr, sgdma_csroffs(status))
+		& SGDMA_STSREG_BUSY) && (delay++ < 100))
 		udelay(1);
 
-	if (ioread32(&csr->status) & SGDMA_STSREG_BUSY) {
+	if (csrrd32(priv->tx_dma_csr, sgdma_csroffs(status))
+	    & SGDMA_STSREG_BUSY) {
 		netdev_err(priv->dev, "timeout waiting for tx dma\n");
 		return 1;
 	}
diff --git a/drivers/net/ethernet/altera/altera_sgdma.h b/drivers/net/ethernet/altera/altera_sgdma.h
index 07d471729dc..584977e29ef 100644
--- a/drivers/net/ethernet/altera/altera_sgdma.h
+++ b/drivers/net/ethernet/altera/altera_sgdma.h
@@ -26,10 +26,11 @@ void sgdma_clear_rxirq(struct altera_tse_private *);
 void sgdma_clear_txirq(struct altera_tse_private *);
 int sgdma_tx_buffer(struct altera_tse_private *priv, struct tse_buffer *);
 u32 sgdma_tx_completions(struct altera_tse_private *);
-int sgdma_add_rx_desc(struct altera_tse_private *priv, struct tse_buffer *);
+void sgdma_add_rx_desc(struct altera_tse_private *priv, struct tse_buffer *);
 void sgdma_status(struct altera_tse_private *);
 u32 sgdma_rx_status(struct altera_tse_private *);
 int sgdma_initialize(struct altera_tse_private *);
 void sgdma_uninitialize(struct altera_tse_private *);
+void sgdma_start_rxdma(struct altera_tse_private *);
 
 #endif /*  __ALTERA_SGDMA_H__ */
diff --git a/drivers/net/ethernet/altera/altera_sgdmahw.h b/drivers/net/ethernet/altera/altera_sgdmahw.h
index ba3334f3538..85bc33b218d 100644
--- a/drivers/net/ethernet/altera/altera_sgdmahw.h
+++ b/drivers/net/ethernet/altera/altera_sgdmahw.h
@@ -19,16 +19,16 @@
 
 /* SGDMA descriptor structure */
 struct sgdma_descrip {
-	unsigned int	raddr; /* address of data to be read */
-	unsigned int	pad1;
-	unsigned int	waddr;
-	unsigned int    pad2;
-	unsigned int	next;
-	unsigned int	pad3;
-	unsigned short  bytes;
-	unsigned char   rburst;
-	unsigned char	wburst;
-	unsigned short	bytes_xferred;	/* 16 bits, bytes xferred */
+	u32	raddr; /* address of data to be read */
+	u32	pad1;
+	u32	waddr;
+	u32	pad2;
+	u32	next;
+	u32	pad3;
+	u16	bytes;
+	u8	rburst;
+	u8	wburst;
+	u16	bytes_xferred;	/* 16 bits, bytes xferred */
 
 	/* bit 0: error
 	 * bit 1: length error
@@ -39,7 +39,7 @@ struct sgdma_descrip {
 	 * bit 6: reserved
 	 * bit 7: status eop for recv case
 	 */
-	unsigned char	status;
+	u8	status;
 
 	/* bit 0: eop
 	 * bit 1: read_fixed
@@ -47,7 +47,7 @@ struct sgdma_descrip {
 	 * bits 3,4,5,6: Channel (always 0)
 	 * bit 7: hardware owned
 	 */
-	unsigned char	control;
+	u8	control;
 } __packed;
 
 
@@ -101,6 +101,8 @@ struct sgdma_csr {
 	u32	pad3[3];
 };
 
+#define sgdma_csroffs(a) (offsetof(struct sgdma_csr, a))
+#define sgdma_descroffs(a) (offsetof(struct sgdma_descrip, a))
 
 #define SGDMA_STSREG_ERR	BIT(0) /* Error */
 #define SGDMA_STSREG_EOP	BIT(1) /* EOP */
diff --git a/drivers/net/ethernet/altera/altera_tse.h b/drivers/net/ethernet/altera/altera_tse.h
index 8feeed05de0..2adb24d4523 100644
--- a/drivers/net/ethernet/altera/altera_tse.h
+++ b/drivers/net/ethernet/altera/altera_tse.h
@@ -58,6 +58,8 @@
 /* MAC function configuration default settings */
 #define ALTERA_TSE_TX_IPG_LENGTH	12
 
+#define ALTERA_TSE_PAUSE_QUANTA		0xffff
+
 #define GET_BIT_VALUE(v, bit)		(((v) >> (bit)) & 0x1)
 
 /* MAC Command_Config Register Bit Definitions
@@ -355,6 +357,8 @@ struct altera_tse_mac {
 	u32 reserved5[42];
 };
 
+#define tse_csroffs(a) (offsetof(struct altera_tse_mac, a))
+
 /* Transmit and Receive Command Registers Bit Definitions
  */
 #define ALTERA_TSE_TX_CMD_STAT_OMIT_CRC		BIT(17)
@@ -390,10 +394,11 @@ struct altera_dmaops {
 	void (*clear_rxirq)(struct altera_tse_private *);
 	int (*tx_buffer)(struct altera_tse_private *, struct tse_buffer *);
 	u32 (*tx_completions)(struct altera_tse_private *);
-	int (*add_rx_desc)(struct altera_tse_private *, struct tse_buffer *);
+	void (*add_rx_desc)(struct altera_tse_private *, struct tse_buffer *);
 	u32 (*get_rx_status)(struct altera_tse_private *);
 	int (*init_dma)(struct altera_tse_private *);
 	void (*uninit_dma)(struct altera_tse_private *);
+	void (*start_rxdma)(struct altera_tse_private *);
 };
 
 /* This structure is private to each device.
@@ -453,6 +458,7 @@ struct altera_tse_private {
 	u32 rxctrlreg;
 	dma_addr_t rxdescphys;
 	dma_addr_t txdescphys;
+	size_t sgdmadesclen;
 
 	struct list_head txlisthd;
 	struct list_head rxlisthd;
@@ -483,4 +489,49 @@ struct altera_tse_private {
  */
 void altera_tse_set_ethtool_ops(struct net_device *);
 
+static inline
+u32 csrrd32(void __iomem *mac, size_t offs)
+{
+	void __iomem *paddr = (void __iomem *)((uintptr_t)mac + offs);
+	return readl(paddr);
+}
+
+static inline
+u16 csrrd16(void __iomem *mac, size_t offs)
+{
+	void __iomem *paddr = (void __iomem *)((uintptr_t)mac + offs);
+	return readw(paddr);
+}
+
+static inline
+u8 csrrd8(void __iomem *mac, size_t offs)
+{
+	void __iomem *paddr = (void __iomem *)((uintptr_t)mac + offs);
+	return readb(paddr);
+}
+
+static inline
+void csrwr32(u32 val, void __iomem *mac, size_t offs)
+{
+	void __iomem *paddr = (void __iomem *)((uintptr_t)mac + offs);
+
+	writel(val, paddr);
+}
+
+static inline
+void csrwr16(u16 val, void __iomem *mac, size_t offs)
+{
+	void __iomem *paddr = (void __iomem *)((uintptr_t)mac + offs);
+
+	writew(val, paddr);
+}
+
+static inline
+void csrwr8(u8 val, void __iomem *mac, size_t offs)
+{
+	void __iomem *paddr = (void __iomem *)((uintptr_t)mac + offs);
+
+	writeb(val, paddr);
+}
+
 #endif /* __ALTERA_TSE_H__ */
diff --git a/drivers/net/ethernet/altera/altera_tse_ethtool.c b/drivers/net/ethernet/altera/altera_tse_ethtool.c
index 319ca74f5e7..54c25eff795 100644
--- a/drivers/net/ethernet/altera/altera_tse_ethtool.c
+++ b/drivers/net/ethernet/altera/altera_tse_ethtool.c
@@ -77,7 +77,7 @@ static void tse_get_drvinfo(struct net_device *dev,
 	struct altera_tse_private *priv = netdev_priv(dev);
 	u32 rev = ioread32(&priv->mac_dev->megacore_revision);
 
-	strcpy(info->driver, "Altera TSE MAC IP Driver");
+	strcpy(info->driver, "altera_tse");
 	strcpy(info->version, "v8.0");
 	snprintf(info->fw_version, ETHTOOL_FWVERS_LEN, "v%d.%d",
 		 rev & 0xFFFF, (rev & 0xFFFF0000) >> 16);
@@ -96,54 +96,89 @@ static void tse_fill_stats(struct net_device *dev, struct ethtool_stats *dummy,
 			   u64 *buf)
 {
 	struct altera_tse_private *priv = netdev_priv(dev);
-	struct altera_tse_mac *mac = priv->mac_dev;
 	u64 ext;
 
-	buf[0] = ioread32(&mac->frames_transmitted_ok);
-	buf[1] = ioread32(&mac->frames_received_ok);
-	buf[2] = ioread32(&mac->frames_check_sequence_errors);
-	buf[3] = ioread32(&mac->alignment_errors);
+	buf[0] = csrrd32(priv->mac_dev,
+			 tse_csroffs(frames_transmitted_ok));
+	buf[1] = csrrd32(priv->mac_dev,
+			 tse_csroffs(frames_received_ok));
+	buf[2] = csrrd32(priv->mac_dev,
+			 tse_csroffs(frames_check_sequence_errors));
+	buf[3] = csrrd32(priv->mac_dev,
+			 tse_csroffs(alignment_errors));
 
 	/* Extended aOctetsTransmittedOK counter */
-	ext = (u64) ioread32(&mac->msb_octets_transmitted_ok) << 32;
-	ext |= ioread32(&mac->octets_transmitted_ok);
+	ext = (u64) csrrd32(priv->mac_dev,
+			    tse_csroffs(msb_octets_transmitted_ok)) << 32;
+
+	ext |= csrrd32(priv->mac_dev,
+		       tse_csroffs(octets_transmitted_ok));
 	buf[4] = ext;
 
 	/* Extended aOctetsReceivedOK counter */
-	ext = (u64) ioread32(&mac->msb_octets_received_ok) << 32;
-	ext |= ioread32(&mac->octets_received_ok);
+	ext = (u64) csrrd32(priv->mac_dev,
+			    tse_csroffs(msb_octets_received_ok)) << 32;
+
+	ext |= csrrd32(priv->mac_dev,
+		       tse_csroffs(octets_received_ok));
 	buf[5] = ext;
 
-	buf[6] = ioread32(&mac->tx_pause_mac_ctrl_frames);
-	buf[7] = ioread32(&mac->rx_pause_mac_ctrl_frames);
-	buf[8] = ioread32(&mac->if_in_errors);
-	buf[9] = ioread32(&mac->if_out_errors);
-	buf[10] = ioread32(&mac->if_in_ucast_pkts);
-	buf[11] = ioread32(&mac->if_in_multicast_pkts);
-	buf[12] = ioread32(&mac->if_in_broadcast_pkts);
-	buf[13] = ioread32(&mac->if_out_discards);
-	buf[14] = ioread32(&mac->if_out_ucast_pkts);
-	buf[15] = ioread32(&mac->if_out_multicast_pkts);
-	buf[16] = ioread32(&mac->if_out_broadcast_pkts);
-	buf[17] = ioread32(&mac->ether_stats_drop_events);
+	buf[6] = csrrd32(priv->mac_dev,
+			 tse_csroffs(tx_pause_mac_ctrl_frames));
+	buf[7] = csrrd32(priv->mac_dev,
+			 tse_csroffs(rx_pause_mac_ctrl_frames));
+	buf[8] = csrrd32(priv->mac_dev,
+			 tse_csroffs(if_in_errors));
+	buf[9] = csrrd32(priv->mac_dev,
+			 tse_csroffs(if_out_errors));
+	buf[10] = csrrd32(priv->mac_dev,
+			  tse_csroffs(if_in_ucast_pkts));
+	buf[11] = csrrd32(priv->mac_dev,
+			  tse_csroffs(if_in_multicast_pkts));
+	buf[12] = csrrd32(priv->mac_dev,
+			  tse_csroffs(if_in_broadcast_pkts));
+	buf[13] = csrrd32(priv->mac_dev,
+			  tse_csroffs(if_out_discards));
+	buf[14] = csrrd32(priv->mac_dev,
+			  tse_csroffs(if_out_ucast_pkts));
+	buf[15] = csrrd32(priv->mac_dev,
+			  tse_csroffs(if_out_multicast_pkts));
+	buf[16] = csrrd32(priv->mac_dev,
+			  tse_csroffs(if_out_broadcast_pkts));
+	buf[17] = csrrd32(priv->mac_dev,
+			  tse_csroffs(ether_stats_drop_events));
 
 	/* Extended etherStatsOctets counter */
-	ext = (u64) ioread32(&mac->msb_ether_stats_octets) << 32;
-	ext |= ioread32(&mac->ether_stats_octets);
+	ext = (u64) csrrd32(priv->mac_dev,
+			    tse_csroffs(msb_ether_stats_octets)) << 32;
+	ext |= csrrd32(priv->mac_dev,
+		       tse_csroffs(ether_stats_octets));
 	buf[18] = ext;
 
-	buf[19] = ioread32(&mac->ether_stats_pkts);
-	buf[20] = ioread32(&mac->ether_stats_undersize_pkts);
-	buf[21] = ioread32(&mac->ether_stats_oversize_pkts);
-	buf[22] = ioread32(&mac->ether_stats_pkts_64_octets);
-	buf[23] = ioread32(&mac->ether_stats_pkts_65to127_octets);
-	buf[24] = ioread32(&mac->ether_stats_pkts_128to255_octets);
-	buf[25] = ioread32(&mac->ether_stats_pkts_256to511_octets);
-	buf[26] = ioread32(&mac->ether_stats_pkts_512to1023_octets);
-	buf[27] = ioread32(&mac->ether_stats_pkts_1024to1518_octets);
-	buf[28] = ioread32(&mac->ether_stats_pkts_1519tox_octets);
-	buf[29] = ioread32(&mac->ether_stats_jabbers);
-	buf[30] = ioread32(&mac->ether_stats_fragments);
+	buf[19] = csrrd32(priv->mac_dev,
+			  tse_csroffs(ether_stats_pkts));
+	buf[20] = csrrd32(priv->mac_dev,
+			  tse_csroffs(ether_stats_undersize_pkts));
+	buf[21] = csrrd32(priv->mac_dev,
+			  tse_csroffs(ether_stats_oversize_pkts));
+	buf[22] = csrrd32(priv->mac_dev,
+			  tse_csroffs(ether_stats_pkts_64_octets));
+	buf[23] = csrrd32(priv->mac_dev,
+			  tse_csroffs(ether_stats_pkts_65to127_octets));
+	buf[24] = csrrd32(priv->mac_dev,
+			  tse_csroffs(ether_stats_pkts_128to255_octets));
+	buf[25] = csrrd32(priv->mac_dev,
+			  tse_csroffs(ether_stats_pkts_256to511_octets));
+	buf[26] = csrrd32(priv->mac_dev,
+			  tse_csroffs(ether_stats_pkts_512to1023_octets));
+	buf[27] = csrrd32(priv->mac_dev,
+			  tse_csroffs(ether_stats_pkts_1024to1518_octets));
+	buf[28] = csrrd32(priv->mac_dev,
+			  tse_csroffs(ether_stats_pkts_1519tox_octets));
+	buf[29] = csrrd32(priv->mac_dev,
+			  tse_csroffs(ether_stats_jabbers));
+	buf[30] = csrrd32(priv->mac_dev,
+			  tse_csroffs(ether_stats_fragments));
 }
 
 static int tse_sset_count(struct net_device *dev, int sset)
@@ -178,19 +213,24 @@ static void tse_get_regs(struct net_device *dev, struct ethtool_regs *regs,
 {
 	int i;
 	struct altera_tse_private *priv = netdev_priv(dev);
-	u32 *tse_mac_regs = (u32 *)priv->mac_dev;
 	u32 *buf = regbuf;
 
 	/* Set version to a known value, so ethtool knows
 	 * how to do any special formatting of this data.
 	 * This version number will need to change if and
 	 * when this register table is changed.
+	 *
+	 * version[31:0] = 1: Dump the first 128 TSE Registers
+	 *      Upper bits are all 0 by default
+	 *
+	 * Upper 16-bits will indicate feature presence for
+	 * Ethtool register decoding in future version.
 	 */
 
 	regs->version = 1;
 
 	for (i = 0; i < TSE_NUM_REGS; i++)
-		buf[i] = ioread32(&tse_mac_regs[i]);
+		buf[i] = csrrd32(priv->mac_dev, i * 4);
 }
 
 static int tse_get_settings(struct net_device *dev, struct ethtool_cmd *cmd)
diff --git a/drivers/net/ethernet/altera/altera_tse_main.c b/drivers/net/ethernet/altera/altera_tse_main.c
index c70a29e0b9f..7330681574d 100644
--- a/drivers/net/ethernet/altera/altera_tse_main.c
+++ b/drivers/net/ethernet/altera/altera_tse_main.c
@@ -100,29 +100,30 @@ static inline u32 tse_tx_avail(struct altera_tse_private *priv)
  */
 static int altera_tse_mdio_read(struct mii_bus *bus, int mii_id, int regnum)
 {
-	struct altera_tse_mac *mac = (struct altera_tse_mac *)bus->priv;
-	unsigned int *mdio_regs = (unsigned int *)&mac->mdio_phy0;
-	u32 data;
+	struct net_device *ndev = bus->priv;
+	struct altera_tse_private *priv = netdev_priv(ndev);
 
 	/* set MDIO address */
-	iowrite32((mii_id & 0x1f), &mac->mdio_phy0_addr);
+	csrwr32((mii_id & 0x1f), priv->mac_dev,
+		tse_csroffs(mdio_phy0_addr));
 
 	/* get the data */
-	data = ioread32(&mdio_regs[regnum]) & 0xffff;
-	return data;
+	return csrrd32(priv->mac_dev,
+		       tse_csroffs(mdio_phy0) + regnum * 4) & 0xffff;
 }
 
 static int altera_tse_mdio_write(struct mii_bus *bus, int mii_id, int regnum,
 				 u16 value)
 {
-	struct altera_tse_mac *mac = (struct altera_tse_mac *)bus->priv;
-	unsigned int *mdio_regs = (unsigned int *)&mac->mdio_phy0;
+	struct net_device *ndev = bus->priv;
+	struct altera_tse_private *priv = netdev_priv(ndev);
 
 	/* set MDIO address */
-	iowrite32((mii_id & 0x1f), &mac->mdio_phy0_addr);
+	csrwr32((mii_id & 0x1f), priv->mac_dev,
+		tse_csroffs(mdio_phy0_addr));
 
 	/* write the data */
-	iowrite32((u32) value, &mdio_regs[regnum]);
+	csrwr32(value, priv->mac_dev, tse_csroffs(mdio_phy0) + regnum * 4);
 	return 0;
 }
 
@@ -168,7 +169,7 @@ static int altera_tse_mdio_create(struct net_device *dev, unsigned int id)
 	for (i = 0; i < PHY_MAX_ADDR; i++)
 		mdio->irq[i] = PHY_POLL;
 
-	mdio->priv = priv->mac_dev;
+	mdio->priv = dev;
 	mdio->parent = priv->device;
 
 	ret = of_mdiobus_register(mdio, mdio_node);
@@ -224,6 +225,7 @@ static int tse_init_rx_buffer(struct altera_tse_private *priv,
 		dev_kfree_skb_any(rxbuffer->skb);
 		return -EINVAL;
 	}
+	rxbuffer->dma_addr &= (dma_addr_t)~3;
 	rxbuffer->len = len;
 	return 0;
 }
@@ -425,9 +427,10 @@ static int tse_rx(struct altera_tse_private *priv, int limit)
 		priv->dev->stats.rx_bytes += pktlength;
 
 		entry = next_entry;
+
+		tse_rx_refill(priv);
 	}
 
-	tse_rx_refill(priv);
 	return count;
 }
 
@@ -520,7 +523,6 @@ static irqreturn_t altera_isr(int irq, void *dev_id)
 	struct altera_tse_private *priv;
 	unsigned long int flags;
 
-
 	if (unlikely(!dev)) {
 		pr_err("%s: invalid dev pointer\n", __func__);
 		return IRQ_NONE;
@@ -562,7 +564,6 @@ static int tse_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	unsigned int nopaged_len = skb_headlen(skb);
 	enum netdev_tx ret = NETDEV_TX_OK;
 	dma_addr_t dma_addr;
-	int txcomplete = 0;
 
 	spin_lock_bh(&priv->tx_lock);
 
@@ -598,7 +599,7 @@ static int tse_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	dma_sync_single_for_device(priv->device, buffer->dma_addr,
 				   buffer->len, DMA_TO_DEVICE);
 
-	txcomplete = priv->dmaops->tx_buffer(priv, buffer);
+	priv->dmaops->tx_buffer(priv, buffer);
 
 	skb_tx_timestamp(skb);
 
@@ -697,7 +698,6 @@ static struct phy_device *connect_local_phy(struct net_device *dev)
 	struct altera_tse_private *priv = netdev_priv(dev);
 	struct phy_device *phydev = NULL;
 	char phy_id_fmt[MII_BUS_ID_SIZE + 3];
-	int ret;
 
 	if (priv->phy_addr != POLL_PHY) {
 		snprintf(phy_id_fmt, MII_BUS_ID_SIZE + 3, PHY_ID_FMT,
@@ -711,6 +711,7 @@ static struct phy_device *connect_local_phy(struct net_device *dev)
 			netdev_err(dev, "Could not attach to PHY\n");
 
 	} else {
+		int ret;
 		phydev = phy_find_first(priv->mdio);
 		if (phydev == NULL) {
 			netdev_err(dev, "No PHY found\n");
@@ -790,7 +791,6 @@ static int init_phy(struct net_device *dev)
 
 static void tse_update_mac_addr(struct altera_tse_private *priv, u8 *addr)
 {
-	struct altera_tse_mac *mac = priv->mac_dev;
 	u32 msb;
 	u32 lsb;
 
@@ -798,8 +798,8 @@ static void tse_update_mac_addr(struct altera_tse_private *priv, u8 *addr)
 	lsb = ((addr[5] << 8) | addr[4]) & 0xffff;
 
 	/* Set primary MAC address */
-	iowrite32(msb, &mac->mac_addr_0);
-	iowrite32(lsb, &mac->mac_addr_1);
+	csrwr32(msb, priv->mac_dev, tse_csroffs(mac_addr_0));
+	csrwr32(lsb, priv->mac_dev, tse_csroffs(mac_addr_1));
 }
 
 /* MAC software reset.
@@ -810,26 +810,26 @@ static void tse_update_mac_addr(struct altera_tse_private *priv, u8 *addr)
  */
 static int reset_mac(struct altera_tse_private *priv)
 {
-	void __iomem *cmd_cfg_reg = &priv->mac_dev->command_config;
 	int counter;
 	u32 dat;
 
-	dat = ioread32(cmd_cfg_reg);
+	dat = csrrd32(priv->mac_dev, tse_csroffs(command_config));
 	dat &= ~(MAC_CMDCFG_TX_ENA | MAC_CMDCFG_RX_ENA);
 	dat |= MAC_CMDCFG_SW_RESET | MAC_CMDCFG_CNT_RESET;
-	iowrite32(dat, cmd_cfg_reg);
+	csrwr32(dat, priv->mac_dev, tse_csroffs(command_config));
 
 	counter = 0;
 	while (counter++ < ALTERA_TSE_SW_RESET_WATCHDOG_CNTR) {
-		if (tse_bit_is_clear(cmd_cfg_reg, MAC_CMDCFG_SW_RESET))
+		if (tse_bit_is_clear(priv->mac_dev, tse_csroffs(command_config),
+				     MAC_CMDCFG_SW_RESET))
 			break;
 		udelay(1);
 	}
 
 	if (counter >= ALTERA_TSE_SW_RESET_WATCHDOG_CNTR) {
-		dat = ioread32(cmd_cfg_reg);
+		dat = csrrd32(priv->mac_dev, tse_csroffs(command_config));
 		dat &= ~MAC_CMDCFG_SW_RESET;
-		iowrite32(dat, cmd_cfg_reg);
+		csrwr32(dat, priv->mac_dev, tse_csroffs(command_config));
 		return -1;
 	}
 	return 0;
@@ -839,42 +839,58 @@ static int reset_mac(struct altera_tse_private *priv)
 */
 static int init_mac(struct altera_tse_private *priv)
 {
-	struct altera_tse_mac *mac = priv->mac_dev;
 	unsigned int cmd = 0;
 	u32 frm_length;
 
 	/* Setup Rx FIFO */
-	iowrite32(priv->rx_fifo_depth - ALTERA_TSE_RX_SECTION_EMPTY,
-		  &mac->rx_section_empty);
-	iowrite32(ALTERA_TSE_RX_SECTION_FULL, &mac->rx_section_full);
-	iowrite32(ALTERA_TSE_RX_ALMOST_EMPTY, &mac->rx_almost_empty);
-	iowrite32(ALTERA_TSE_RX_ALMOST_FULL, &mac->rx_almost_full);
+	csrwr32(priv->rx_fifo_depth - ALTERA_TSE_RX_SECTION_EMPTY,
+		priv->mac_dev, tse_csroffs(rx_section_empty));
+
+	csrwr32(ALTERA_TSE_RX_SECTION_FULL, priv->mac_dev,
+		tse_csroffs(rx_section_full));
+
+	csrwr32(ALTERA_TSE_RX_ALMOST_EMPTY, priv->mac_dev,
+		tse_csroffs(rx_almost_empty));
+
+	csrwr32(ALTERA_TSE_RX_ALMOST_FULL, priv->mac_dev,
+		tse_csroffs(rx_almost_full));
 
 	/* Setup Tx FIFO */
-	iowrite32(priv->tx_fifo_depth - ALTERA_TSE_TX_SECTION_EMPTY,
-		  &mac->tx_section_empty);
-	iowrite32(ALTERA_TSE_TX_SECTION_FULL, &mac->tx_section_full);
-	iowrite32(ALTERA_TSE_TX_ALMOST_EMPTY, &mac->tx_almost_empty);
-	iowrite32(ALTERA_TSE_TX_ALMOST_FULL, &mac->tx_almost_full);
+	csrwr32(priv->tx_fifo_depth - ALTERA_TSE_TX_SECTION_EMPTY,
+		priv->mac_dev, tse_csroffs(tx_section_empty));
+
+	csrwr32(ALTERA_TSE_TX_SECTION_FULL, priv->mac_dev,
+		tse_csroffs(tx_section_full));
+
+	csrwr32(ALTERA_TSE_TX_ALMOST_EMPTY, priv->mac_dev,
+		tse_csroffs(tx_almost_empty));
+
+	csrwr32(ALTERA_TSE_TX_ALMOST_FULL, priv->mac_dev,
+		tse_csroffs(tx_almost_full));
 
 	/* MAC Address Configuration */
 	tse_update_mac_addr(priv, priv->dev->dev_addr);
 
 	/* MAC Function Configuration */
 	frm_length = ETH_HLEN + priv->dev->mtu + ETH_FCS_LEN;
-	iowrite32(frm_length, &mac->frm_length);
-	iowrite32(ALTERA_TSE_TX_IPG_LENGTH, &mac->tx_ipg_length);
+	csrwr32(frm_length, priv->mac_dev, tse_csroffs(frm_length));
+
+	csrwr32(ALTERA_TSE_TX_IPG_LENGTH, priv->mac_dev,
+		tse_csroffs(tx_ipg_length));
 
 	/* Disable RX/TX shift 16 for alignment of all received frames on 16-bit
 	 * start address
 	 */
-	tse_clear_bit(&mac->rx_cmd_stat, ALTERA_TSE_RX_CMD_STAT_RX_SHIFT16);
-	tse_clear_bit(&mac->tx_cmd_stat, ALTERA_TSE_TX_CMD_STAT_TX_SHIFT16 |
-					 ALTERA_TSE_TX_CMD_STAT_OMIT_CRC);
+	tse_set_bit(priv->mac_dev, tse_csroffs(rx_cmd_stat),
+		    ALTERA_TSE_RX_CMD_STAT_RX_SHIFT16);
+
+	tse_clear_bit(priv->mac_dev, tse_csroffs(tx_cmd_stat),
+		      ALTERA_TSE_TX_CMD_STAT_TX_SHIFT16 |
+		      ALTERA_TSE_TX_CMD_STAT_OMIT_CRC);
 
 	/* Set the MAC options */
-	cmd = ioread32(&mac->command_config);
-	cmd |= MAC_CMDCFG_PAD_EN;	/* Padding Removal on Receive */
+	cmd = csrrd32(priv->mac_dev, tse_csroffs(command_config));
+	cmd &= ~MAC_CMDCFG_PAD_EN;	/* No padding Removal on Receive */
 	cmd &= ~MAC_CMDCFG_CRC_FWD;	/* CRC Removal */
 	cmd |= MAC_CMDCFG_RX_ERR_DISC;	/* Automatically discard frames
 					 * with CRC errors
@@ -882,7 +898,16 @@ static int init_mac(struct altera_tse_private *priv)
 	cmd |= MAC_CMDCFG_CNTL_FRM_ENA;
 	cmd &= ~MAC_CMDCFG_TX_ENA;
 	cmd &= ~MAC_CMDCFG_RX_ENA;
-	iowrite32(cmd, &mac->command_config);
+
+	/* Default speed and duplex setting, full/100 */
+	cmd &= ~MAC_CMDCFG_HD_ENA;
+	cmd &= ~MAC_CMDCFG_ETH_SPEED;
+	cmd &= ~MAC_CMDCFG_ENA_10;
+
+	csrwr32(cmd, priv->mac_dev, tse_csroffs(command_config));
+
+	csrwr32(ALTERA_TSE_PAUSE_QUANTA, priv->mac_dev,
+		tse_csroffs(pause_quanta));
 
 	if (netif_msg_hw(priv))
 		dev_dbg(priv->device,
@@ -895,15 +920,14 @@ static int init_mac(struct altera_tse_private *priv)
  */
 static void tse_set_mac(struct altera_tse_private *priv, bool enable)
 {
-	struct altera_tse_mac *mac = priv->mac_dev;
-	u32 value = ioread32(&mac->command_config);
+	u32 value = csrrd32(priv->mac_dev, tse_csroffs(command_config));
 
 	if (enable)
 		value |= MAC_CMDCFG_TX_ENA | MAC_CMDCFG_RX_ENA;
 	else
 		value &= ~(MAC_CMDCFG_TX_ENA | MAC_CMDCFG_RX_ENA);
 
-	iowrite32(value, &mac->command_config);
+	csrwr32(value, priv->mac_dev, tse_csroffs(command_config));
 }
 
 /* Change the MTU
@@ -933,13 +957,12 @@ static int tse_change_mtu(struct net_device *dev, int new_mtu)
 static void altera_tse_set_mcfilter(struct net_device *dev)
 {
 	struct altera_tse_private *priv = netdev_priv(dev);
-	struct altera_tse_mac *mac = priv->mac_dev;
 	int i;
 	struct netdev_hw_addr *ha;
 
 	/* clear the hash filter */
 	for (i = 0; i < 64; i++)
-		iowrite32(0, &(mac->hash_table[i]));
+		csrwr32(0, priv->mac_dev, tse_csroffs(hash_table) + i * 4);
 
 	netdev_for_each_mc_addr(ha, dev) {
 		unsigned int hash = 0;
@@ -955,7 +978,7 @@ static void altera_tse_set_mcfilter(struct net_device *dev)
 
 			hash = (hash << 1) | xor_bit;
 		}
-		iowrite32(1, &(mac->hash_table[hash]));
+		csrwr32(1, priv->mac_dev, tse_csroffs(hash_table) + hash * 4);
 	}
 }
 
@@ -963,12 +986,11 @@ static void altera_tse_set_mcfilter(struct net_device *dev)
 static void altera_tse_set_mcfilterall(struct net_device *dev)
 {
 	struct altera_tse_private *priv = netdev_priv(dev);
-	struct altera_tse_mac *mac = priv->mac_dev;
 	int i;
 
 	/* set the hash filter */
 	for (i = 0; i < 64; i++)
-		iowrite32(1, &(mac->hash_table[i]));
+		csrwr32(1, priv->mac_dev, tse_csroffs(hash_table) + i * 4);
 }
 
 /* Set or clear the multicast filter for this adaptor
@@ -976,12 +998,12 @@ static void altera_tse_set_mcfilterall(struct net_device *dev)
 static void tse_set_rx_mode_hashfilter(struct net_device *dev)
 {
 	struct altera_tse_private *priv = netdev_priv(dev);
-	struct altera_tse_mac *mac = priv->mac_dev;
 
 	spin_lock(&priv->mac_cfg_lock);
 
 	if (dev->flags & IFF_PROMISC)
-		tse_set_bit(&mac->command_config, MAC_CMDCFG_PROMIS_EN);
+		tse_set_bit(priv->mac_dev, tse_csroffs(command_config),
+			    MAC_CMDCFG_PROMIS_EN);
 
 	if (dev->flags & IFF_ALLMULTI)
 		altera_tse_set_mcfilterall(dev);
@@ -996,15 +1018,16 @@ static void tse_set_rx_mode_hashfilter(struct net_device *dev)
 static void tse_set_rx_mode(struct net_device *dev)
 {
 	struct altera_tse_private *priv = netdev_priv(dev);
-	struct altera_tse_mac *mac = priv->mac_dev;
 
 	spin_lock(&priv->mac_cfg_lock);
 
 	if ((dev->flags & IFF_PROMISC) || (dev->flags & IFF_ALLMULTI) ||
 	    !netdev_mc_empty(dev) || !netdev_uc_empty(dev))
-		tse_set_bit(&mac->command_config, MAC_CMDCFG_PROMIS_EN);
+		tse_set_bit(priv->mac_dev, tse_csroffs(command_config),
+			    MAC_CMDCFG_PROMIS_EN);
 	else
-		tse_clear_bit(&mac->command_config, MAC_CMDCFG_PROMIS_EN);
+		tse_clear_bit(priv->mac_dev, tse_csroffs(command_config),
+			      MAC_CMDCFG_PROMIS_EN);
 
 	spin_unlock(&priv->mac_cfg_lock);
 }
@@ -1085,17 +1108,19 @@ static int tse_open(struct net_device *dev)
 
 	spin_unlock_irqrestore(&priv->rxdma_irq_lock, flags);
 
-	/* Start MAC Rx/Tx */
-	spin_lock(&priv->mac_cfg_lock);
-	tse_set_mac(priv, true);
-	spin_unlock(&priv->mac_cfg_lock);
-
 	if (priv->phydev)
 		phy_start(priv->phydev);
 
 	napi_enable(&priv->napi);
 	netif_start_queue(dev);
 
+	priv->dmaops->start_rxdma(priv);
+
+	/* Start MAC Rx/Tx */
+	spin_lock(&priv->mac_cfg_lock);
+	tse_set_mac(priv, true);
+	spin_unlock(&priv->mac_cfg_lock);
+
 	return 0;
 
 tx_request_irq_error:
@@ -1167,7 +1192,6 @@ static struct net_device_ops altera_tse_netdev_ops = {
 	.ndo_validate_addr	= eth_validate_addr,
 };
 
-
 static int request_and_map(struct platform_device *pdev, const char *name,
 			   struct resource **res, void __iomem **ptr)
 {
@@ -1235,7 +1259,7 @@ static int altera_tse_probe(struct platform_device *pdev)
 		/* Get the mapped address to the SGDMA descriptor memory */
 		ret = request_and_map(pdev, "s1", &dma_res, &descmap);
 		if (ret)
-			goto out_free;
+			goto err_free_netdev;
 
 		/* Start of that memory is for transmit descriptors */
 		priv->tx_dma_desc = descmap;
@@ -1254,24 +1278,24 @@ static int altera_tse_probe(struct platform_device *pdev)
 		if (upper_32_bits(priv->rxdescmem_busaddr)) {
 			dev_dbg(priv->device,
 				"SGDMA bus addresses greater than 32-bits\n");
-			goto out_free;
+			goto err_free_netdev;
 		}
 		if (upper_32_bits(priv->txdescmem_busaddr)) {
 			dev_dbg(priv->device,
 				"SGDMA bus addresses greater than 32-bits\n");
-			goto out_free;
+			goto err_free_netdev;
 		}
 	} else if (priv->dmaops &&
 		   priv->dmaops->altera_dtype == ALTERA_DTYPE_MSGDMA) {
 		ret = request_and_map(pdev, "rx_resp", &dma_res,
 				      &priv->rx_dma_resp);
 		if (ret)
-			goto out_free;
+			goto err_free_netdev;
 
 		ret = request_and_map(pdev, "tx_desc", &dma_res,
 				      &priv->tx_dma_desc);
 		if (ret)
-			goto out_free;
+			goto err_free_netdev;
 
 		priv->txdescmem = resource_size(dma_res);
 		priv->txdescmem_busaddr = dma_res->start;
@@ -1279,13 +1303,13 @@ static int altera_tse_probe(struct platform_device *pdev)
 		ret = request_and_map(pdev, "rx_desc", &dma_res,
 				      &priv->rx_dma_desc);
 		if (ret)
-			goto out_free;
+			goto err_free_netdev;
 
 		priv->rxdescmem = resource_size(dma_res);
 		priv->rxdescmem_busaddr = dma_res->start;
 
 	} else {
-		goto out_free;
+		goto err_free_netdev;
 	}
 
 	if (!dma_set_mask(priv->device, DMA_BIT_MASK(priv->dmaops->dmamask)))
@@ -1294,26 +1318,26 @@ static int altera_tse_probe(struct platform_device *pdev)
 	else if (!dma_set_mask(priv->device, DMA_BIT_MASK(32)))
 		dma_set_coherent_mask(priv->device, DMA_BIT_MASK(32));
 	else
-		goto out_free;
+		goto err_free_netdev;
 
 	/* MAC address space */
 	ret = request_and_map(pdev, "control_port", &control_port,
 			      (void __iomem **)&priv->mac_dev);
 	if (ret)
-		goto out_free;
+		goto err_free_netdev;
 
 	/* xSGDMA Rx Dispatcher address space */
 	ret = request_and_map(pdev, "rx_csr", &dma_res,
 			      &priv->rx_dma_csr);
 	if (ret)
-		goto out_free;
+		goto err_free_netdev;
 
 
 	/* xSGDMA Tx Dispatcher address space */
 	ret = request_and_map(pdev, "tx_csr", &dma_res,
 			      &priv->tx_dma_csr);
 	if (ret)
-		goto out_free;
+		goto err_free_netdev;
 
 
 	/* Rx IRQ */
@@ -1321,7 +1345,7 @@ static int altera_tse_probe(struct platform_device *pdev)
 	if (priv->rx_irq == -ENXIO) {
 		dev_err(&pdev->dev, "cannot obtain Rx IRQ\n");
 		ret = -ENXIO;
-		goto out_free;
+		goto err_free_netdev;
 	}
 
 	/* Tx IRQ */
@@ -1329,7 +1353,7 @@ static int altera_tse_probe(struct platform_device *pdev)
 	if (priv->tx_irq == -ENXIO) {
 		dev_err(&pdev->dev, "cannot obtain Tx IRQ\n");
 		ret = -ENXIO;
-		goto out_free;
+		goto err_free_netdev;
 	}
 
 	/* get FIFO depths from device tree */
@@ -1337,14 +1361,14 @@ static int altera_tse_probe(struct platform_device *pdev)
 				 &priv->rx_fifo_depth)) {
 		dev_err(&pdev->dev, "cannot obtain rx-fifo-depth\n");
 		ret = -ENXIO;
-		goto out_free;
+		goto err_free_netdev;
 	}
 
 	if (of_property_read_u32(pdev->dev.of_node, "tx-fifo-depth",
 				 &priv->rx_fifo_depth)) {
 		dev_err(&pdev->dev, "cannot obtain tx-fifo-depth\n");
 		ret = -ENXIO;
-		goto out_free;
+		goto err_free_netdev;
 	}
 
 	/* get hash filter settings for this instance */
@@ -1352,6 +1376,11 @@ static int altera_tse_probe(struct platform_device *pdev)
 		of_property_read_bool(pdev->dev.of_node,
 				      "altr,has-hash-multicast-filter");
 
+	/* Set hash filter to not set for now until the
+	 * multicast filter receive issue is debugged
+	 */
+	priv->hash_filter = 0;
+
 	/* get supplemental address settings for this instance */
 	priv->added_unicast =
 		of_property_read_bool(pdev->dev.of_node,
@@ -1393,7 +1422,7 @@ static int altera_tse_probe(struct platform_device *pdev)
 	      ((priv->phy_addr >= 0) && (priv->phy_addr < PHY_MAX_ADDR)))) {
 		dev_err(&pdev->dev, "invalid phy-addr specified %d\n",
 			priv->phy_addr);
-		goto out_free;
+		goto err_free_netdev;
 	}
 
 	/* Create/attach to MDIO bus */
@@ -1401,7 +1430,7 @@ static int altera_tse_probe(struct platform_device *pdev)
 				     atomic_add_return(1, &instance_count));
 
 	if (ret)
-		goto out_free;
+		goto err_free_netdev;
 
 	/* initialize netdev */
 	ether_setup(ndev);
@@ -1438,7 +1467,7 @@ static int altera_tse_probe(struct platform_device *pdev)
 	ret = register_netdev(ndev);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to register TSE net device\n");
-		goto out_free_mdio;
+		goto err_register_netdev;
 	}
 
 	platform_set_drvdata(pdev, ndev);
@@ -1455,13 +1484,16 @@ static int altera_tse_probe(struct platform_device *pdev)
 	ret = init_phy(ndev);
 	if (ret != 0) {
 		netdev_err(ndev, "Cannot attach to PHY (error: %d)\n", ret);
-		goto out_free_mdio;
+		goto err_init_phy;
 	}
 	return 0;
 
-out_free_mdio:
+err_init_phy:
+	unregister_netdev(ndev);
+err_register_netdev:
+	netif_napi_del(&priv->napi);
 	altera_tse_mdio_destroy(ndev);
-out_free:
+err_free_netdev:
 	free_netdev(ndev);
 	return ret;
 }
@@ -1480,7 +1512,7 @@ static int altera_tse_remove(struct platform_device *pdev)
 	return 0;
 }
 
-struct altera_dmaops altera_dtype_sgdma = {
+static const struct altera_dmaops altera_dtype_sgdma = {
 	.altera_dtype = ALTERA_DTYPE_SGDMA,
 	.dmamask = 32,
 	.reset_dma = sgdma_reset,
@@ -1496,9 +1528,10 @@ struct altera_dmaops altera_dtype_sgdma = {
 	.get_rx_status = sgdma_rx_status,
 	.init_dma = sgdma_initialize,
 	.uninit_dma = sgdma_uninitialize,
+	.start_rxdma = sgdma_start_rxdma,
 };
 
-struct altera_dmaops altera_dtype_msgdma = {
+static const struct altera_dmaops altera_dtype_msgdma = {
 	.altera_dtype = ALTERA_DTYPE_MSGDMA,
 	.dmamask = 64,
 	.reset_dma = msgdma_reset,
@@ -1514,6 +1547,7 @@ struct altera_dmaops altera_dtype_msgdma = {
 	.get_rx_status = msgdma_rx_status,
 	.init_dma = msgdma_initialize,
 	.uninit_dma = msgdma_uninitialize,
+	.start_rxdma = msgdma_start_rxdma,
 };
 
 static struct of_device_id altera_tse_ids[] = {
diff --git a/drivers/net/ethernet/altera/altera_utils.c b/drivers/net/ethernet/altera/altera_utils.c
index 70fa13f486b..d7eeb1713ad 100644
--- a/drivers/net/ethernet/altera/altera_utils.c
+++ b/drivers/net/ethernet/altera/altera_utils.c
@@ -17,28 +17,28 @@
 #include "altera_tse.h"
 #include "altera_utils.h"
 
-void tse_set_bit(void __iomem *ioaddr, u32 bit_mask)
+void tse_set_bit(void __iomem *ioaddr, size_t offs, u32 bit_mask)
 {
-	u32 value = ioread32(ioaddr);
+	u32 value = csrrd32(ioaddr, offs);
 	value |= bit_mask;
-	iowrite32(value, ioaddr);
+	csrwr32(value, ioaddr, offs);
 }
 
-void tse_clear_bit(void __iomem *ioaddr, u32 bit_mask)
+void tse_clear_bit(void __iomem *ioaddr, size_t offs, u32 bit_mask)
 {
-	u32 value = ioread32(ioaddr);
+	u32 value = csrrd32(ioaddr, offs);
 	value &= ~bit_mask;
-	iowrite32(value, ioaddr);
+	csrwr32(value, ioaddr, offs);
 }
 
-int tse_bit_is_set(void __iomem *ioaddr, u32 bit_mask)
+int tse_bit_is_set(void __iomem *ioaddr, size_t offs, u32 bit_mask)
 {
-	u32 value = ioread32(ioaddr);
+	u32 value = csrrd32(ioaddr, offs);
 	return (value & bit_mask) ? 1 : 0;
 }
 
-int tse_bit_is_clear(void __iomem *ioaddr, u32 bit_mask)
+int tse_bit_is_clear(void __iomem *ioaddr, size_t offs, u32 bit_mask)
 {
-	u32 value = ioread32(ioaddr);
+	u32 value = csrrd32(ioaddr, offs);
 	return (value & bit_mask) ? 0 : 1;
 }
diff --git a/drivers/net/ethernet/altera/altera_utils.h b/drivers/net/ethernet/altera/altera_utils.h
index ce1db36d358..baf100ccf58 100644
--- a/drivers/net/ethernet/altera/altera_utils.h
+++ b/drivers/net/ethernet/altera/altera_utils.h
@@ -19,9 +19,9 @@
 #ifndef __ALTERA_UTILS_H__
 #define __ALTERA_UTILS_H__
 
-void tse_set_bit(void __iomem *ioaddr, u32 bit_mask);
-void tse_clear_bit(void __iomem *ioaddr, u32 bit_mask);
-int tse_bit_is_set(void __iomem *ioaddr, u32 bit_mask);
-int tse_bit_is_clear(void __iomem *ioaddr, u32 bit_mask);
+void tse_set_bit(void __iomem *ioaddr, size_t offs, u32 bit_mask);
+void tse_clear_bit(void __iomem *ioaddr, size_t offs, u32 bit_mask);
+int tse_bit_is_set(void __iomem *ioaddr, size_t offs, u32 bit_mask);
+int tse_bit_is_clear(void __iomem *ioaddr, size_t offs, u32 bit_mask);
 
 #endif /* __ALTERA_UTILS_H__*/
diff --git a/drivers/net/ethernet/arc/emac.h b/drivers/net/ethernet/arc/emac.h
index 928fac6dd10..53f85bf7152 100644
--- a/drivers/net/ethernet/arc/emac.h
+++ b/drivers/net/ethernet/arc/emac.h
@@ -11,6 +11,7 @@
 #include <linux/dma-mapping.h>
 #include <linux/netdevice.h>
 #include <linux/phy.h>
+#include <linux/clk.h>
 
 /* STATUS and ENABLE Register bit masks */
 #define TXINT_MASK	(1<<0)	/* Transmit interrupt */
@@ -131,6 +132,7 @@ struct arc_emac_priv {
 	struct mii_bus *bus;
 
 	void __iomem *regs;
+	struct clk *clk;
 
 	struct napi_struct napi;
 	struct net_device_stats stats;
diff --git a/drivers/net/ethernet/arc/emac_main.c b/drivers/net/ethernet/arc/emac_main.c
index eeecc29cf5b..d647a7d115a 100644
--- a/drivers/net/ethernet/arc/emac_main.c
+++ b/drivers/net/ethernet/arc/emac_main.c
@@ -574,6 +574,18 @@ static int arc_emac_tx(struct sk_buff *skb, struct net_device *ndev)
 	return NETDEV_TX_OK;
 }
 
+static void arc_emac_set_address_internal(struct net_device *ndev)
+{
+	struct arc_emac_priv *priv = netdev_priv(ndev);
+	unsigned int addr_low, addr_hi;
+
+	addr_low = le32_to_cpu(*(__le32 *) &ndev->dev_addr[0]);
+	addr_hi = le16_to_cpu(*(__le16 *) &ndev->dev_addr[4]);
+
+	arc_reg_set(priv, R_ADDRL, addr_low);
+	arc_reg_set(priv, R_ADDRH, addr_hi);
+}
+
 /**
  * arc_emac_set_address - Set the MAC address for this device.
  * @ndev:	Pointer to net_device structure.
@@ -587,9 +599,7 @@ static int arc_emac_tx(struct sk_buff *skb, struct net_device *ndev)
  */
 static int arc_emac_set_address(struct net_device *ndev, void *p)
 {
-	struct arc_emac_priv *priv = netdev_priv(ndev);
 	struct sockaddr *addr = p;
-	unsigned int addr_low, addr_hi;
 
 	if (netif_running(ndev))
 		return -EBUSY;
@@ -599,11 +609,7 @@ static int arc_emac_set_address(struct net_device *ndev, void *p)
 
 	memcpy(ndev->dev_addr, addr->sa_data, ndev->addr_len);
 
-	addr_low = le32_to_cpu(*(__le32 *) &ndev->dev_addr[0]);
-	addr_hi = le16_to_cpu(*(__le16 *) &ndev->dev_addr[4]);
-
-	arc_reg_set(priv, R_ADDRL, addr_low);
-	arc_reg_set(priv, R_ADDRH, addr_hi);
+	arc_emac_set_address_internal(ndev);
 
 	return 0;
 }
@@ -643,13 +649,6 @@ static int arc_emac_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	/* Get CPU clock frequency from device tree */
-	if (of_property_read_u32(pdev->dev.of_node, "clock-frequency",
-				 &clock_frequency)) {
-		dev_err(&pdev->dev, "failed to retrieve <clock-frequency> from device tree\n");
-		return -EINVAL;
-	}
-
 	/* Get IRQ from device tree */
 	irq = irq_of_parse_and_map(pdev->dev.of_node, 0);
 	if (!irq) {
@@ -677,17 +676,36 @@ static int arc_emac_probe(struct platform_device *pdev)
 	priv->regs = devm_ioremap_resource(&pdev->dev, &res_regs);
 	if (IS_ERR(priv->regs)) {
 		err = PTR_ERR(priv->regs);
-		goto out;
+		goto out_netdev;
 	}
 	dev_dbg(&pdev->dev, "Registers base address is 0x%p\n", priv->regs);
 
+	priv->clk = of_clk_get(pdev->dev.of_node, 0);
+	if (IS_ERR(priv->clk)) {
+		/* Get CPU clock frequency from device tree */
+		if (of_property_read_u32(pdev->dev.of_node, "clock-frequency",
+					&clock_frequency)) {
+			dev_err(&pdev->dev, "failed to retrieve <clock-frequency> from device tree\n");
+			err = -EINVAL;
+			goto out_netdev;
+		}
+	} else {
+		err = clk_prepare_enable(priv->clk);
+		if (err) {
+			dev_err(&pdev->dev, "failed to enable clock\n");
+			goto out_clkget;
+		}
+
+		clock_frequency = clk_get_rate(priv->clk);
+	}
+
 	id = arc_reg_get(priv, R_ID);
 
 	/* Check for EMAC revision 5 or 7, magic number */
 	if (!(id == 0x0005fd02 || id == 0x0007fd02)) {
 		dev_err(&pdev->dev, "ARC EMAC not detected, id=0x%x\n", id);
 		err = -ENODEV;
-		goto out;
+		goto out_clken;
 	}
 	dev_info(&pdev->dev, "ARC EMAC detected with id: 0x%x\n", id);
 
@@ -702,7 +720,7 @@ static int arc_emac_probe(struct platform_device *pdev)
 			       ndev->name, ndev);
 	if (err) {
 		dev_err(&pdev->dev, "could not allocate IRQ\n");
-		goto out;
+		goto out_clken;
 	}
 
 	/* Get MAC address from device tree */
@@ -713,6 +731,7 @@ static int arc_emac_probe(struct platform_device *pdev)
 	else
 		eth_hw_addr_random(ndev);
 
+	arc_emac_set_address_internal(ndev);
 	dev_info(&pdev->dev, "MAC address is now %pM\n", ndev->dev_addr);
 
 	/* Do 1 allocation instead of 2 separate ones for Rx and Tx BD rings */
@@ -722,7 +741,7 @@ static int arc_emac_probe(struct platform_device *pdev)
 	if (!priv->rxbd) {
 		dev_err(&pdev->dev, "failed to allocate data buffers\n");
 		err = -ENOMEM;
-		goto out;
+		goto out_clken;
 	}
 
 	priv->txbd = priv->rxbd + RX_BD_NUM;
@@ -734,7 +753,7 @@ static int arc_emac_probe(struct platform_device *pdev)
 	err = arc_mdio_probe(pdev, priv);
 	if (err) {
 		dev_err(&pdev->dev, "failed to probe MII bus\n");
-		goto out;
+		goto out_clken;
 	}
 
 	priv->phy_dev = of_phy_connect(ndev, phy_node, arc_emac_adjust_link, 0,
@@ -742,7 +761,7 @@ static int arc_emac_probe(struct platform_device *pdev)
 	if (!priv->phy_dev) {
 		dev_err(&pdev->dev, "of_phy_connect() failed\n");
 		err = -ENODEV;
-		goto out;
+		goto out_mdio;
 	}
 
 	dev_info(&pdev->dev, "connected to %s phy with id 0x%x\n",
@@ -752,14 +771,25 @@ static int arc_emac_probe(struct platform_device *pdev)
 
 	err = register_netdev(ndev);
 	if (err) {
-		netif_napi_del(&priv->napi);
 		dev_err(&pdev->dev, "failed to register network device\n");
-		goto out;
+		goto out_netif_api;
 	}
 
 	return 0;
 
-out:
+out_netif_api:
+	netif_napi_del(&priv->napi);
+	phy_disconnect(priv->phy_dev);
+	priv->phy_dev = NULL;
+out_mdio:
+	arc_mdio_remove(priv);
+out_clken:
+	if (!IS_ERR(priv->clk))
+		clk_disable_unprepare(priv->clk);
+out_clkget:
+	if (!IS_ERR(priv->clk))
+		clk_put(priv->clk);
+out_netdev:
 	free_netdev(ndev);
 	return err;
 }
@@ -774,6 +804,12 @@ static int arc_emac_remove(struct platform_device *pdev)
 	arc_mdio_remove(priv);
 	unregister_netdev(ndev);
 	netif_napi_del(&priv->napi);
+
+	if (!IS_ERR(priv->clk)) {
+		clk_disable_unprepare(priv->clk);
+		clk_put(priv->clk);
+	}
+
 	free_netdev(ndev);
 
 	return 0;
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
index 16391db2e8c..3a8e51ed5be 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
@@ -10051,8 +10051,8 @@ static void bnx2x_prev_unload_close_mac(struct bnx2x *bp,
 #define BCM_5710_UNDI_FW_MF_MAJOR	(0x07)
 #define BCM_5710_UNDI_FW_MF_MINOR	(0x08)
 #define BCM_5710_UNDI_FW_MF_VERS	(0x05)
-#define BNX2X_PREV_UNDI_MF_PORT(p)	(0x1a150c + ((p) << 4))
-#define BNX2X_PREV_UNDI_MF_FUNC(f)	(0x1a184c + ((f) << 4))
+#define BNX2X_PREV_UNDI_MF_PORT(p) (BAR_TSTRORM_INTMEM + 0x150c + ((p) << 4))
+#define BNX2X_PREV_UNDI_MF_FUNC(f) (BAR_TSTRORM_INTMEM + 0x184c + ((f) << 4))
 static bool bnx2x_prev_unload_undi_fw_supports_mf(struct bnx2x *bp)
 {
 	u8 major, minor, version;
@@ -10352,6 +10352,7 @@ static int bnx2x_prev_unload_common(struct bnx2x *bp)
 	/* Reset should be performed after BRB is emptied */
 	if (reset_reg & MISC_REGISTERS_RESET_REG_1_RST_BRB1) {
 		u32 timer_count = 1000;
+		bool need_write = true;
 
 		/* Close the MAC Rx to prevent BRB from filling up */
 		bnx2x_prev_unload_close_mac(bp, &mac_vals);
@@ -10398,7 +10399,10 @@ static int bnx2x_prev_unload_common(struct bnx2x *bp)
 			 * cleaning methods - might be redundant but harmless.
 			 */
 			if (bnx2x_prev_unload_undi_fw_supports_mf(bp)) {
-				bnx2x_prev_unload_undi_mf(bp);
+				if (need_write) {
+					bnx2x_prev_unload_undi_mf(bp);
+					need_write = false;
+				}
 			} else if (prev_undi) {
 				/* If UNDI resides in memory,
 				 * manually increment it
@@ -13233,6 +13237,8 @@ static void __bnx2x_remove(struct pci_dev *pdev,
 				iounmap(bp->doorbells);
 
 			bnx2x_release_firmware(bp);
+		} else {
+			bnx2x_vf_pci_dealloc(bp);
 		}
 		bnx2x_free_mem_bp(bp);
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c
index f82ac5ac233..faf01488d26 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c
@@ -427,7 +427,9 @@ static int bnx2x_vf_mac_vlan_config(struct bnx2x *bp,
 	if (filter->add && filter->type == BNX2X_VF_FILTER_VLAN &&
 	    (atomic_read(&bnx2x_vfq(vf, qid, vlan_count)) >=
 	     vf_vlan_rules_cnt(vf))) {
-		BNX2X_ERR("No credits for vlan\n");
+		BNX2X_ERR("No credits for vlan [%d >= %d]\n",
+			  atomic_read(&bnx2x_vfq(vf, qid, vlan_count)),
+			  vf_vlan_rules_cnt(vf));
 		return -ENOMEM;
 	}
 
@@ -610,6 +612,7 @@ int bnx2x_vf_mcast(struct bnx2x *bp, struct bnx2x_virtf *vf,
 		}
 
 		/* add new mcasts */
+		mcast.mcast_list_len = mc_num;
 		rc = bnx2x_config_mcast(bp, &mcast, BNX2X_MCAST_CMD_ADD);
 		if (rc)
 			BNX2X_ERR("Faled to add multicasts\n");
@@ -837,6 +840,29 @@ int bnx2x_vf_flr_clnup_epilog(struct bnx2x *bp, u8 abs_vfid)
 	return 0;
 }
 
+static void bnx2x_iov_re_set_vlan_filters(struct bnx2x *bp,
+					  struct bnx2x_virtf *vf,
+					  int new)
+{
+	int num = vf_vlan_rules_cnt(vf);
+	int diff = new - num;
+	bool rc = true;
+
+	DP(BNX2X_MSG_IOV, "vf[%d] - %d vlan filter credits [previously %d]\n",
+	   vf->abs_vfid, new, num);
+
+	if (diff > 0)
+		rc = bp->vlans_pool.get(&bp->vlans_pool, diff);
+	else if (diff < 0)
+		rc = bp->vlans_pool.put(&bp->vlans_pool, -diff);
+
+	if (rc)
+		vf_vlan_rules_cnt(vf) = new;
+	else
+		DP(BNX2X_MSG_IOV, "vf[%d] - Failed to configure vlan filter credits change\n",
+		   vf->abs_vfid);
+}
+
 /* must be called after the number of PF queues and the number of VFs are
  * both known
  */
@@ -854,9 +880,11 @@ bnx2x_iov_static_resc(struct bnx2x *bp, struct bnx2x_virtf *vf)
 	resc->num_mac_filters = 1;
 
 	/* divvy up vlan rules */
+	bnx2x_iov_re_set_vlan_filters(bp, vf, 0);
 	vlan_count = bp->vlans_pool.check(&bp->vlans_pool);
 	vlan_count = 1 << ilog2(vlan_count);
-	resc->num_vlan_filters = vlan_count / BNX2X_NR_VIRTFN(bp);
+	bnx2x_iov_re_set_vlan_filters(bp, vf,
+				      vlan_count / BNX2X_NR_VIRTFN(bp));
 
 	/* no real limitation */
 	resc->num_mc_filters = 0;
@@ -1478,10 +1506,6 @@ int bnx2x_iov_nic_init(struct bnx2x *bp)
 		bnx2x_iov_static_resc(bp, vf);
 
 		/* queues are initialized during VF-ACQUIRE */
-
-		/* reserve the vf vlan credit */
-		bp->vlans_pool.get(&bp->vlans_pool, vf_vlan_rules_cnt(vf));
-
 		vf->filter_state = 0;
 		vf->sp_cl_id = bnx2x_fp(bp, 0, cl_id);
 
@@ -1912,11 +1936,12 @@ int bnx2x_vf_chk_avail_resc(struct bnx2x *bp, struct bnx2x_virtf *vf,
 	u8 rxq_cnt = vf_rxq_count(vf) ? : bnx2x_vf_max_queue_cnt(bp, vf);
 	u8 txq_cnt = vf_txq_count(vf) ? : bnx2x_vf_max_queue_cnt(bp, vf);
 
+	/* Save a vlan filter for the Hypervisor */
 	return ((req_resc->num_rxqs <= rxq_cnt) &&
 		(req_resc->num_txqs <= txq_cnt) &&
 		(req_resc->num_sbs <= vf_sb_count(vf))   &&
 		(req_resc->num_mac_filters <= vf_mac_rules_cnt(vf)) &&
-		(req_resc->num_vlan_filters <= vf_vlan_rules_cnt(vf)));
+		(req_resc->num_vlan_filters <= vf_vlan_rules_visible_cnt(vf)));
 }
 
 /* CORE VF API */
@@ -1972,14 +1997,14 @@ int bnx2x_vf_acquire(struct bnx2x *bp, struct bnx2x_virtf *vf,
 	vf_txq_count(vf) = resc->num_txqs ? : bnx2x_vf_max_queue_cnt(bp, vf);
 	if (resc->num_mac_filters)
 		vf_mac_rules_cnt(vf) = resc->num_mac_filters;
-	if (resc->num_vlan_filters)
-		vf_vlan_rules_cnt(vf) = resc->num_vlan_filters;
+	/* Add an additional vlan filter credit for the hypervisor */
+	bnx2x_iov_re_set_vlan_filters(bp, vf, resc->num_vlan_filters + 1);
 
 	DP(BNX2X_MSG_IOV,
 	   "Fulfilling vf request: sb count %d, tx_count %d, rx_count %d, mac_rules_count %d, vlan_rules_count %d\n",
 	   vf_sb_count(vf), vf_rxq_count(vf),
 	   vf_txq_count(vf), vf_mac_rules_cnt(vf),
-	   vf_vlan_rules_cnt(vf));
+	   vf_vlan_rules_visible_cnt(vf));
 
 	/* Initialize the queues */
 	if (!vf->vfqs) {
@@ -2670,7 +2695,7 @@ out:
 		bnx2x_unlock_vf_pf_channel(bp, vf, CHANNEL_TLV_PF_SET_MAC);
 	}
 
-	return 0;
+	return rc;
 }
 
 int bnx2x_set_vf_vlan(struct net_device *dev, int vfidx, u16 vlan, u8 qos)
@@ -2896,6 +2921,14 @@ void __iomem *bnx2x_vf_doorbells(struct bnx2x *bp)
 	return bp->regview + PXP_VF_ADDR_DB_START;
 }
 
+void bnx2x_vf_pci_dealloc(struct bnx2x *bp)
+{
+	BNX2X_PCI_FREE(bp->vf2pf_mbox, bp->vf2pf_mbox_mapping,
+		       sizeof(struct bnx2x_vf_mbx_msg));
+	BNX2X_PCI_FREE(bp->vf2pf_mbox, bp->pf2vf_bulletin_mapping,
+		       sizeof(union pf_vf_bulletin));
+}
+
 int bnx2x_vf_pci_alloc(struct bnx2x *bp)
 {
 	mutex_init(&bp->vf2pf_mutex);
@@ -2915,10 +2948,7 @@ int bnx2x_vf_pci_alloc(struct bnx2x *bp)
 	return 0;
 
 alloc_mem_err:
-	BNX2X_PCI_FREE(bp->vf2pf_mbox, bp->vf2pf_mbox_mapping,
-		       sizeof(struct bnx2x_vf_mbx_msg));
-	BNX2X_PCI_FREE(bp->vf2pf_mbox, bp->pf2vf_bulletin_mapping,
-		       sizeof(union pf_vf_bulletin));
+	bnx2x_vf_pci_dealloc(bp);
 	return -ENOMEM;
 }
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.h
index 8bf764570ee..6929adba52f 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.h
@@ -159,6 +159,8 @@ struct bnx2x_virtf {
 #define vf_mac_rules_cnt(vf)		((vf)->alloc_resc.num_mac_filters)
 #define vf_vlan_rules_cnt(vf)		((vf)->alloc_resc.num_vlan_filters)
 #define vf_mc_rules_cnt(vf)		((vf)->alloc_resc.num_mc_filters)
+	/* Hide a single vlan filter credit for the hypervisor */
+#define vf_vlan_rules_visible_cnt(vf)	(vf_vlan_rules_cnt(vf) - 1)
 
 	u8 sb_count;	/* actual number of SBs */
 	u8 igu_base_id;	/* base igu status block id */
@@ -502,6 +504,7 @@ static inline int bnx2x_vf_ustorm_prods_offset(struct bnx2x *bp,
 enum sample_bulletin_result bnx2x_sample_bulletin(struct bnx2x *bp);
 void bnx2x_timer_sriov(struct bnx2x *bp);
 void __iomem *bnx2x_vf_doorbells(struct bnx2x *bp);
+void bnx2x_vf_pci_dealloc(struct bnx2x *bp);
 int bnx2x_vf_pci_alloc(struct bnx2x *bp);
 int bnx2x_enable_sriov(struct bnx2x *bp);
 void bnx2x_disable_sriov(struct bnx2x *bp);
@@ -568,6 +571,7 @@ static inline void __iomem *bnx2x_vf_doorbells(struct bnx2x *bp)
 	return NULL;
 }
 
+static inline void bnx2x_vf_pci_dealloc(struct bnx2 *bp) {return 0; }
 static inline int bnx2x_vf_pci_alloc(struct bnx2x *bp) {return 0; }
 static inline void bnx2x_pf_set_vfs_vlan(struct bnx2x *bp) {}
 static inline int bnx2x_sriov_configure(struct pci_dev *dev, int num_vfs) {return 0; }
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_vfpf.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_vfpf.c
index 0622884596b..784c7155b98 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_vfpf.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_vfpf.c
@@ -747,7 +747,7 @@ int bnx2x_vfpf_config_mac(struct bnx2x *bp, u8 *addr, u8 vf_qid, bool set)
 out:
 	bnx2x_vfpf_finalize(bp, &req->first_tlv);
 
-	return 0;
+	return rc;
 }
 
 /* request pf to config rss table for vf queues*/
@@ -1163,7 +1163,7 @@ static void bnx2x_vf_mbx_acquire_resp(struct bnx2x *bp, struct bnx2x_virtf *vf,
 			bnx2x_vf_max_queue_cnt(bp, vf);
 		resc->num_sbs = vf_sb_count(vf);
 		resc->num_mac_filters = vf_mac_rules_cnt(vf);
-		resc->num_vlan_filters = vf_vlan_rules_cnt(vf);
+		resc->num_vlan_filters = vf_vlan_rules_visible_cnt(vf);
 		resc->num_mc_filters = 0;
 
 		if (status == PFVF_STATUS_SUCCESS) {
diff --git a/drivers/net/ethernet/broadcom/tg3.c b/drivers/net/ethernet/broadcom/tg3.c
index b9f7022f4e8..e5d95c5ce1a 100644
--- a/drivers/net/ethernet/broadcom/tg3.c
+++ b/drivers/net/ethernet/broadcom/tg3.c
@@ -12286,7 +12286,9 @@ static int tg3_set_ringparam(struct net_device *dev, struct ethtool_ringparam *e
 	if (tg3_flag(tp, MAX_RXPEND_64) &&
 	    tp->rx_pending > 63)
 		tp->rx_pending = 63;
-	tp->rx_jumbo_pending = ering->rx_jumbo_pending;
+
+	if (tg3_flag(tp, JUMBO_RING_ENABLE))
+		tp->rx_jumbo_pending = ering->rx_jumbo_pending;
 
 	for (i = 0; i < tp->irq_max; i++)
 		tp->napi[i].tx_pending = ering->tx_pending;
diff --git a/drivers/net/ethernet/cadence/Kconfig b/drivers/net/ethernet/cadence/Kconfig
index 7e49c43b7af..9e089d24466 100644
--- a/drivers/net/ethernet/cadence/Kconfig
+++ b/drivers/net/ethernet/cadence/Kconfig
@@ -4,7 +4,7 @@
 
 config NET_CADENCE
 	bool "Cadence devices"
-	depends on HAS_IOMEM && (ARM || AVR32 || COMPILE_TEST)
+	depends on HAS_IOMEM && (ARM || AVR32 || MICROBLAZE || COMPILE_TEST)
 	default y
 	---help---
 	  If you have a network (Ethernet) card belonging to this class, say Y.
@@ -30,7 +30,7 @@ config ARM_AT91_ETHER
 
 config MACB
 	tristate "Cadence MACB/GEM support"
-	depends on HAS_DMA && (PLATFORM_AT32AP || ARCH_AT91 || ARCH_PICOXCELL || ARCH_ZYNQ || COMPILE_TEST)
+	depends on HAS_DMA && (PLATFORM_AT32AP || ARCH_AT91 || ARCH_PICOXCELL || ARCH_ZYNQ || MICROBLAZE || COMPILE_TEST)
 	select PHYLIB
 	---help---
 	  The Cadence MACB ethernet interface is found on many Atmel AT32 and
diff --git a/drivers/net/ethernet/cadence/macb.c b/drivers/net/ethernet/cadence/macb.c
index ca97005e24b..e9daa072ebb 100644
--- a/drivers/net/ethernet/cadence/macb.c
+++ b/drivers/net/ethernet/cadence/macb.c
@@ -599,25 +599,16 @@ static void gem_rx_refill(struct macb *bp)
 {
 	unsigned int		entry;
 	struct sk_buff		*skb;
-	struct macb_dma_desc	*desc;
 	dma_addr_t		paddr;
 
 	while (CIRC_SPACE(bp->rx_prepared_head, bp->rx_tail, RX_RING_SIZE) > 0) {
-		u32 addr, ctrl;
-
 		entry = macb_rx_ring_wrap(bp->rx_prepared_head);
-		desc = &bp->rx_ring[entry];
 
 		/* Make hw descriptor updates visible to CPU */
 		rmb();
 
-		addr = desc->addr;
-		ctrl = desc->ctrl;
 		bp->rx_prepared_head++;
 
-		if ((addr & MACB_BIT(RX_USED)))
-			continue;
-
 		if (bp->rx_skbuff[entry] == NULL) {
 			/* allocate sk_buff for this free entry in ring */
 			skb = netdev_alloc_skb(bp->dev, bp->rx_buffer_size);
@@ -698,7 +689,6 @@ static int gem_rx(struct macb *bp, int budget)
 		if (!(addr & MACB_BIT(RX_USED)))
 			break;
 
-		desc->addr &= ~MACB_BIT(RX_USED);
 		bp->rx_tail++;
 		count++;
 
@@ -891,16 +881,15 @@ static int macb_poll(struct napi_struct *napi, int budget)
 	if (work_done < budget) {
 		napi_complete(napi);
 
-		/*
-		 * We've done what we can to clean the buffers. Make sure we
-		 * get notified when new packets arrive.
-		 */
-		macb_writel(bp, IER, MACB_RX_INT_FLAGS);
-
 		/* Packets received while interrupts were disabled */
 		status = macb_readl(bp, RSR);
-		if (unlikely(status))
+		if (status) {
+			if (bp->caps & MACB_CAPS_ISR_CLEAR_ON_WRITE)
+				macb_writel(bp, ISR, MACB_BIT(RCOMP));
 			napi_reschedule(napi);
+		} else {
+			macb_writel(bp, IER, MACB_RX_INT_FLAGS);
+		}
 	}
 
 	/* TODO: Handle errors */
@@ -951,6 +940,10 @@ static irqreturn_t macb_interrupt(int irq, void *dev_id)
 		if (unlikely(status & (MACB_TX_ERR_FLAGS))) {
 			macb_writel(bp, IDR, MACB_TX_INT_FLAGS);
 			schedule_work(&bp->tx_error_task);
+
+			if (bp->caps & MACB_CAPS_ISR_CLEAR_ON_WRITE)
+				macb_writel(bp, ISR, MACB_TX_ERR_FLAGS);
+
 			break;
 		}
 
@@ -968,6 +961,9 @@ static irqreturn_t macb_interrupt(int irq, void *dev_id)
 				bp->hw_stats.gem.rx_overruns++;
 			else
 				bp->hw_stats.macb.rx_overruns++;
+
+			if (bp->caps & MACB_CAPS_ISR_CLEAR_ON_WRITE)
+				macb_writel(bp, ISR, MACB_BIT(ISR_ROVR));
 		}
 
 		if (status & MACB_BIT(HRESP)) {
@@ -977,6 +973,9 @@ static irqreturn_t macb_interrupt(int irq, void *dev_id)
 			 * (work queue?)
 			 */
 			netdev_err(dev, "DMA bus error: HRESP not OK\n");
+
+			if (bp->caps & MACB_CAPS_ISR_CLEAR_ON_WRITE)
+				macb_writel(bp, ISR, MACB_BIT(HRESP));
 		}
 
 		status = macb_readl(bp, ISR);
@@ -1113,7 +1112,7 @@ static void gem_free_rx_buffers(struct macb *bp)
 
 		desc = &bp->rx_ring[i];
 		addr = MACB_BF(RX_WADDR, MACB_BFEXT(RX_WADDR, desc->addr));
-		dma_unmap_single(&bp->pdev->dev, addr, skb->len,
+		dma_unmap_single(&bp->pdev->dev, addr, bp->rx_buffer_size,
 				 DMA_FROM_DEVICE);
 		dev_kfree_skb_any(skb);
 		skb = NULL;
diff --git a/drivers/net/ethernet/chelsio/Kconfig b/drivers/net/ethernet/chelsio/Kconfig
index d40c994a4f6..570222c3341 100644
--- a/drivers/net/ethernet/chelsio/Kconfig
+++ b/drivers/net/ethernet/chelsio/Kconfig
@@ -67,13 +67,13 @@ config CHELSIO_T3
 	  will be called cxgb3.
 
 config CHELSIO_T4
-	tristate "Chelsio Communications T4 Ethernet support"
+	tristate "Chelsio Communications T4/T5 Ethernet support"
 	depends on PCI
 	select FW_LOADER
 	select MDIO
 	---help---
-	  This driver supports Chelsio T4-based gigabit and 10Gb Ethernet
-	  adapters.
+	  This driver supports Chelsio T4 and T5 based gigabit, 10Gb Ethernet
+	  adapter and T5 based 40Gb Ethernet adapter.
 
 	  For general information about Chelsio and our products, visit
 	  our website at <http://www.chelsio.com>.
@@ -87,11 +87,12 @@ config CHELSIO_T4
 	  will be called cxgb4.
 
 config CHELSIO_T4VF
-	tristate "Chelsio Communications T4 Virtual Function Ethernet support"
+	tristate "Chelsio Communications T4/T5 Virtual Function Ethernet support"
 	depends on PCI
 	---help---
-	  This driver supports Chelsio T4-based gigabit and 10Gb Ethernet
-	  adapters with PCI-E SR-IOV Virtual Functions.
+	  This driver supports Chelsio T4 and T5 based gigabit, 10Gb Ethernet
+	  adapters and T5 based 40Gb Ethernet adapters with PCI-E SR-IOV Virtual
+	  Functions.
 
 	  For general information about Chelsio and our products, visit
 	  our website at <http://www.chelsio.com>.
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
index 6fe58913403..24e16e3301e 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
@@ -5870,6 +5870,8 @@ static void print_port_info(const struct net_device *dev)
 		spd = " 2.5 GT/s";
 	else if (adap->params.pci.speed == PCI_EXP_LNKSTA_CLS_5_0GB)
 		spd = " 5 GT/s";
+	else if (adap->params.pci.speed == PCI_EXP_LNKSTA_CLS_8_0GB)
+		spd = " 8 GT/s";
 
 	if (pi->link_cfg.supported & FW_PORT_CAP_SPEED_100M)
 		bufp += sprintf(bufp, "100/");
diff --git a/drivers/net/ethernet/ec_bhf.c b/drivers/net/ethernet/ec_bhf.c
new file mode 100644
index 00000000000..4884205e56e
--- /dev/null
+++ b/drivers/net/ethernet/ec_bhf.c
@@ -0,0 +1,706 @@
+ /*
+ * drivers/net/ethernet/beckhoff/ec_bhf.c
+ *
+ * Copyright (C) 2014 Darek Marcinkiewicz <reksio@newterm.pl>
+ *
+ * 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.
+ *
+ */
+
+/* This is a driver for EtherCAT master module present on CCAT FPGA.
+ * Those can be found on Bechhoff CX50xx industrial PCs.
+ */
+
+#if 0
+#define DEBUG
+#endif
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/pci.h>
+#include <linux/init.h>
+
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/ip.h>
+#include <linux/skbuff.h>
+#include <linux/hrtimer.h>
+#include <linux/interrupt.h>
+#include <linux/stat.h>
+
+#define TIMER_INTERVAL_NSEC	20000
+
+#define INFO_BLOCK_SIZE		0x10
+#define INFO_BLOCK_TYPE		0x0
+#define INFO_BLOCK_REV		0x2
+#define INFO_BLOCK_BLK_CNT	0x4
+#define INFO_BLOCK_TX_CHAN	0x4
+#define INFO_BLOCK_RX_CHAN	0x5
+#define INFO_BLOCK_OFFSET	0x8
+
+#define EC_MII_OFFSET		0x4
+#define EC_FIFO_OFFSET		0x8
+#define EC_MAC_OFFSET		0xc
+
+#define MAC_FRAME_ERR_CNT	0x0
+#define MAC_RX_ERR_CNT		0x1
+#define MAC_CRC_ERR_CNT		0x2
+#define MAC_LNK_LST_ERR_CNT	0x3
+#define MAC_TX_FRAME_CNT	0x10
+#define MAC_RX_FRAME_CNT	0x14
+#define MAC_TX_FIFO_LVL		0x20
+#define MAC_DROPPED_FRMS	0x28
+#define MAC_CONNECTED_CCAT_FLAG	0x78
+
+#define MII_MAC_ADDR		0x8
+#define MII_MAC_FILT_FLAG	0xe
+#define MII_LINK_STATUS		0xf
+
+#define FIFO_TX_REG		0x0
+#define FIFO_TX_RESET		0x8
+#define FIFO_RX_REG		0x10
+#define FIFO_RX_ADDR_VALID	(1u << 31)
+#define FIFO_RX_RESET		0x18
+
+#define DMA_CHAN_OFFSET		0x1000
+#define DMA_CHAN_SIZE		0x8
+
+#define DMA_WINDOW_SIZE_MASK	0xfffffffc
+
+static struct pci_device_id ids[] = {
+	{ PCI_DEVICE(0x15ec, 0x5000), },
+	{ 0, }
+};
+MODULE_DEVICE_TABLE(pci, ids);
+
+struct rx_header {
+#define RXHDR_NEXT_ADDR_MASK	0xffffffu
+#define RXHDR_NEXT_VALID	(1u << 31)
+	__le32 next;
+#define RXHDR_NEXT_RECV_FLAG	0x1
+	__le32 recv;
+#define RXHDR_LEN_MASK		0xfffu
+	__le16 len;
+	__le16 port;
+	__le32 reserved;
+	u8 timestamp[8];
+} __packed;
+
+#define PKT_PAYLOAD_SIZE	0x7e8
+struct rx_desc {
+	struct rx_header header;
+	u8 data[PKT_PAYLOAD_SIZE];
+} __packed;
+
+struct tx_header {
+	__le16 len;
+#define TX_HDR_PORT_0		0x1
+#define TX_HDR_PORT_1		0x2
+	u8 port;
+	u8 ts_enable;
+#define TX_HDR_SENT		0x1
+	__le32 sent;
+	u8 timestamp[8];
+} __packed;
+
+struct tx_desc {
+	struct tx_header header;
+	u8 data[PKT_PAYLOAD_SIZE];
+} __packed;
+
+#define FIFO_SIZE		64
+
+static long polling_frequency = TIMER_INTERVAL_NSEC;
+
+struct bhf_dma {
+	u8 *buf;
+	size_t len;
+	dma_addr_t buf_phys;
+
+	u8 *alloc;
+	size_t alloc_len;
+	dma_addr_t alloc_phys;
+};
+
+struct ec_bhf_priv {
+	struct net_device *net_dev;
+
+	struct pci_dev *dev;
+
+	void * __iomem io;
+	void * __iomem dma_io;
+
+	struct hrtimer hrtimer;
+
+	int tx_dma_chan;
+	int rx_dma_chan;
+	void * __iomem ec_io;
+	void * __iomem fifo_io;
+	void * __iomem mii_io;
+	void * __iomem mac_io;
+
+	struct bhf_dma rx_buf;
+	struct rx_desc *rx_descs;
+	int rx_dnext;
+	int rx_dcount;
+
+	struct bhf_dma tx_buf;
+	struct tx_desc *tx_descs;
+	int tx_dcount;
+	int tx_dnext;
+
+	u64 stat_rx_bytes;
+	u64 stat_tx_bytes;
+};
+
+#define PRIV_TO_DEV(priv) (&(priv)->dev->dev)
+
+#define ETHERCAT_MASTER_ID	0x14
+
+static void ec_bhf_print_status(struct ec_bhf_priv *priv)
+{
+	struct device *dev = PRIV_TO_DEV(priv);
+
+	dev_dbg(dev, "Frame error counter: %d\n",
+		ioread8(priv->mac_io + MAC_FRAME_ERR_CNT));
+	dev_dbg(dev, "RX error counter: %d\n",
+		ioread8(priv->mac_io + MAC_RX_ERR_CNT));
+	dev_dbg(dev, "CRC error counter: %d\n",
+		ioread8(priv->mac_io + MAC_CRC_ERR_CNT));
+	dev_dbg(dev, "TX frame counter: %d\n",
+		ioread32(priv->mac_io + MAC_TX_FRAME_CNT));
+	dev_dbg(dev, "RX frame counter: %d\n",
+		ioread32(priv->mac_io + MAC_RX_FRAME_CNT));
+	dev_dbg(dev, "TX fifo level: %d\n",
+		ioread8(priv->mac_io + MAC_TX_FIFO_LVL));
+	dev_dbg(dev, "Dropped frames: %d\n",
+		ioread8(priv->mac_io + MAC_DROPPED_FRMS));
+	dev_dbg(dev, "Connected with CCAT slot: %d\n",
+		ioread8(priv->mac_io + MAC_CONNECTED_CCAT_FLAG));
+	dev_dbg(dev, "Link status: %d\n",
+		ioread8(priv->mii_io + MII_LINK_STATUS));
+}
+
+static void ec_bhf_reset(struct ec_bhf_priv *priv)
+{
+	iowrite8(0, priv->mac_io + MAC_FRAME_ERR_CNT);
+	iowrite8(0, priv->mac_io + MAC_RX_ERR_CNT);
+	iowrite8(0, priv->mac_io + MAC_CRC_ERR_CNT);
+	iowrite8(0, priv->mac_io + MAC_LNK_LST_ERR_CNT);
+	iowrite32(0, priv->mac_io + MAC_TX_FRAME_CNT);
+	iowrite32(0, priv->mac_io + MAC_RX_FRAME_CNT);
+	iowrite8(0, priv->mac_io + MAC_DROPPED_FRMS);
+
+	iowrite8(0, priv->fifo_io + FIFO_TX_RESET);
+	iowrite8(0, priv->fifo_io + FIFO_RX_RESET);
+
+	iowrite8(0, priv->mac_io + MAC_TX_FIFO_LVL);
+}
+
+static void ec_bhf_send_packet(struct ec_bhf_priv *priv, struct tx_desc *desc)
+{
+	u32 len = le16_to_cpu(desc->header.len) + sizeof(desc->header);
+	u32 addr = (u8 *)desc - priv->tx_buf.buf;
+
+	iowrite32((ALIGN(len, 8) << 24) | addr, priv->fifo_io + FIFO_TX_REG);
+
+	dev_dbg(PRIV_TO_DEV(priv), "Done sending packet\n");
+}
+
+static int ec_bhf_desc_sent(struct tx_desc *desc)
+{
+	return le32_to_cpu(desc->header.sent) & TX_HDR_SENT;
+}
+
+static void ec_bhf_process_tx(struct ec_bhf_priv *priv)
+{
+	if (unlikely(netif_queue_stopped(priv->net_dev))) {
+		/* Make sure that we perceive changes to tx_dnext. */
+		smp_rmb();
+
+		if (ec_bhf_desc_sent(&priv->tx_descs[priv->tx_dnext]))
+			netif_wake_queue(priv->net_dev);
+	}
+}
+
+static int ec_bhf_pkt_received(struct rx_desc *desc)
+{
+	return le32_to_cpu(desc->header.recv) & RXHDR_NEXT_RECV_FLAG;
+}
+
+static void ec_bhf_add_rx_desc(struct ec_bhf_priv *priv, struct rx_desc *desc)
+{
+	iowrite32(FIFO_RX_ADDR_VALID | ((u8 *)(desc) - priv->rx_buf.buf),
+		  priv->fifo_io + FIFO_RX_REG);
+}
+
+static void ec_bhf_process_rx(struct ec_bhf_priv *priv)
+{
+	struct rx_desc *desc = &priv->rx_descs[priv->rx_dnext];
+	struct device *dev = PRIV_TO_DEV(priv);
+
+	while (ec_bhf_pkt_received(desc)) {
+		int pkt_size = (le16_to_cpu(desc->header.len) &
+			       RXHDR_LEN_MASK) - sizeof(struct rx_header) - 4;
+		u8 *data = desc->data;
+		struct sk_buff *skb;
+
+		skb = netdev_alloc_skb_ip_align(priv->net_dev, pkt_size);
+		dev_dbg(dev, "Received packet, size: %d\n", pkt_size);
+
+		if (skb) {
+			memcpy(skb_put(skb, pkt_size), data, pkt_size);
+			skb->protocol = eth_type_trans(skb, priv->net_dev);
+			dev_dbg(dev, "Protocol type: %x\n", skb->protocol);
+
+			priv->stat_rx_bytes += pkt_size;
+
+			netif_rx(skb);
+		} else {
+			dev_err_ratelimited(dev,
+				"Couldn't allocate a skb_buff for a packet of size %u\n",
+				pkt_size);
+		}
+
+		desc->header.recv = 0;
+
+		ec_bhf_add_rx_desc(priv, desc);
+
+		priv->rx_dnext = (priv->rx_dnext + 1) % priv->rx_dcount;
+		desc = &priv->rx_descs[priv->rx_dnext];
+	}
+
+}
+
+static enum hrtimer_restart ec_bhf_timer_fun(struct hrtimer *timer)
+{
+	struct ec_bhf_priv *priv = container_of(timer, struct ec_bhf_priv,
+						hrtimer);
+	ec_bhf_process_rx(priv);
+	ec_bhf_process_tx(priv);
+
+	if (!netif_running(priv->net_dev))
+		return HRTIMER_NORESTART;
+
+	hrtimer_forward_now(timer, ktime_set(0, polling_frequency));
+	return HRTIMER_RESTART;
+}
+
+static int ec_bhf_setup_offsets(struct ec_bhf_priv *priv)
+{
+	struct device *dev = PRIV_TO_DEV(priv);
+	unsigned block_count, i;
+	void * __iomem ec_info;
+
+	dev_dbg(dev, "Info block:\n");
+	dev_dbg(dev, "Type of function: %x\n", (unsigned)ioread16(priv->io));
+	dev_dbg(dev, "Revision of function: %x\n",
+		(unsigned)ioread16(priv->io + INFO_BLOCK_REV));
+
+	block_count = ioread8(priv->io + INFO_BLOCK_BLK_CNT);
+	dev_dbg(dev, "Number of function blocks: %x\n", block_count);
+
+	for (i = 0; i < block_count; i++) {
+		u16 type = ioread16(priv->io + i * INFO_BLOCK_SIZE +
+				    INFO_BLOCK_TYPE);
+		if (type == ETHERCAT_MASTER_ID)
+			break;
+	}
+	if (i == block_count) {
+		dev_err(dev, "EtherCAT master with DMA block not found\n");
+		return -ENODEV;
+	}
+	dev_dbg(dev, "EtherCAT master with DMA block found at pos: %d\n", i);
+
+	ec_info = priv->io + i * INFO_BLOCK_SIZE;
+	dev_dbg(dev, "EtherCAT master revision: %d\n",
+		ioread16(ec_info + INFO_BLOCK_REV));
+
+	priv->tx_dma_chan = ioread8(ec_info + INFO_BLOCK_TX_CHAN);
+	dev_dbg(dev, "EtherCAT master tx dma channel: %d\n",
+		priv->tx_dma_chan);
+
+	priv->rx_dma_chan = ioread8(ec_info + INFO_BLOCK_RX_CHAN);
+	dev_dbg(dev, "EtherCAT master rx dma channel: %d\n",
+		 priv->rx_dma_chan);
+
+	priv->ec_io = priv->io + ioread32(ec_info + INFO_BLOCK_OFFSET);
+	priv->mii_io = priv->ec_io + ioread32(priv->ec_io + EC_MII_OFFSET);
+	priv->fifo_io = priv->ec_io + ioread32(priv->ec_io + EC_FIFO_OFFSET);
+	priv->mac_io = priv->ec_io + ioread32(priv->ec_io + EC_MAC_OFFSET);
+
+	dev_dbg(dev,
+		"EtherCAT block addres: %p, fifo address: %p, mii address: %p, mac address: %p\n",
+		priv->ec_io, priv->fifo_io, priv->mii_io, priv->mac_io);
+
+	return 0;
+}
+
+static netdev_tx_t ec_bhf_start_xmit(struct sk_buff *skb,
+				     struct net_device *net_dev)
+{
+	struct ec_bhf_priv *priv = netdev_priv(net_dev);
+	struct tx_desc *desc;
+	unsigned len;
+
+	dev_dbg(PRIV_TO_DEV(priv), "Starting xmit\n");
+
+	desc = &priv->tx_descs[priv->tx_dnext];
+
+	skb_copy_and_csum_dev(skb, desc->data);
+	len = skb->len;
+
+	memset(&desc->header, 0, sizeof(desc->header));
+	desc->header.len = cpu_to_le16(len);
+	desc->header.port = TX_HDR_PORT_0;
+
+	ec_bhf_send_packet(priv, desc);
+
+	priv->tx_dnext = (priv->tx_dnext + 1) % priv->tx_dcount;
+
+	if (!ec_bhf_desc_sent(&priv->tx_descs[priv->tx_dnext])) {
+		/* Make sure that update updates to tx_dnext are perceived
+		 * by timer routine.
+		 */
+		smp_wmb();
+
+		netif_stop_queue(net_dev);
+
+		dev_dbg(PRIV_TO_DEV(priv), "Stopping netif queue\n");
+		ec_bhf_print_status(priv);
+	}
+
+	priv->stat_tx_bytes += len;
+
+	dev_kfree_skb(skb);
+
+	return NETDEV_TX_OK;
+}
+
+static int ec_bhf_alloc_dma_mem(struct ec_bhf_priv *priv,
+				struct bhf_dma *buf,
+				int channel,
+				int size)
+{
+	int offset = channel * DMA_CHAN_SIZE + DMA_CHAN_OFFSET;
+	struct device *dev = PRIV_TO_DEV(priv);
+	u32 mask;
+
+	iowrite32(0xffffffff, priv->dma_io + offset);
+
+	mask = ioread32(priv->dma_io + offset);
+	mask &= DMA_WINDOW_SIZE_MASK;
+	dev_dbg(dev, "Read mask %x for channel %d\n", mask, channel);
+
+	/* We want to allocate a chunk of memory that is:
+	 * - aligned to the mask we just read
+	 * - is of size 2^mask bytes (at most)
+	 * In order to ensure that we will allocate buffer of
+	 * 2 * 2^mask bytes.
+	 */
+	buf->len = min_t(int, ~mask + 1, size);
+	buf->alloc_len = 2 * buf->len;
+
+	dev_dbg(dev, "Allocating %d bytes for channel %d",
+		(int)buf->alloc_len, channel);
+	buf->alloc = dma_alloc_coherent(dev, buf->alloc_len, &buf->alloc_phys,
+					GFP_KERNEL);
+	if (buf->alloc == NULL) {
+		dev_info(dev, "Failed to allocate buffer\n");
+		return -ENOMEM;
+	}
+
+	buf->buf_phys = (buf->alloc_phys + buf->len) & mask;
+	buf->buf = buf->alloc + (buf->buf_phys - buf->alloc_phys);
+
+	iowrite32(0, priv->dma_io + offset + 4);
+	iowrite32(buf->buf_phys, priv->dma_io + offset);
+	dev_dbg(dev, "Buffer: %x and read from dev: %x",
+		(unsigned)buf->buf_phys, ioread32(priv->dma_io + offset));
+
+	return 0;
+}
+
+static void ec_bhf_setup_tx_descs(struct ec_bhf_priv *priv)
+{
+	int i = 0;
+
+	priv->tx_dcount = priv->tx_buf.len / sizeof(struct tx_desc);
+	priv->tx_descs = (struct tx_desc *) priv->tx_buf.buf;
+	priv->tx_dnext = 0;
+
+	for (i = 0; i < priv->tx_dcount; i++)
+		priv->tx_descs[i].header.sent = cpu_to_le32(TX_HDR_SENT);
+}
+
+static void ec_bhf_setup_rx_descs(struct ec_bhf_priv *priv)
+{
+	int i;
+
+	priv->rx_dcount = priv->rx_buf.len / sizeof(struct rx_desc);
+	priv->rx_descs = (struct rx_desc *) priv->rx_buf.buf;
+	priv->rx_dnext = 0;
+
+	for (i = 0; i < priv->rx_dcount; i++) {
+		struct rx_desc *desc = &priv->rx_descs[i];
+		u32 next;
+
+		if (i != priv->rx_dcount - 1)
+			next = (u8 *)(desc + 1) - priv->rx_buf.buf;
+		else
+			next = 0;
+		next |= RXHDR_NEXT_VALID;
+		desc->header.next = cpu_to_le32(next);
+		desc->header.recv = 0;
+		ec_bhf_add_rx_desc(priv, desc);
+	}
+}
+
+static int ec_bhf_open(struct net_device *net_dev)
+{
+	struct ec_bhf_priv *priv = netdev_priv(net_dev);
+	struct device *dev = PRIV_TO_DEV(priv);
+	int err = 0;
+
+	dev_info(dev, "Opening device\n");
+
+	ec_bhf_reset(priv);
+
+	err = ec_bhf_alloc_dma_mem(priv, &priv->rx_buf, priv->rx_dma_chan,
+				   FIFO_SIZE * sizeof(struct rx_desc));
+	if (err) {
+		dev_err(dev, "Failed to allocate rx buffer\n");
+		goto out;
+	}
+	ec_bhf_setup_rx_descs(priv);
+
+	dev_info(dev, "RX buffer allocated, address: %x\n",
+		 (unsigned)priv->rx_buf.buf_phys);
+
+	err = ec_bhf_alloc_dma_mem(priv, &priv->tx_buf, priv->tx_dma_chan,
+				   FIFO_SIZE * sizeof(struct tx_desc));
+	if (err) {
+		dev_err(dev, "Failed to allocate tx buffer\n");
+		goto error_rx_free;
+	}
+	dev_dbg(dev, "TX buffer allocated, addres: %x\n",
+		(unsigned)priv->tx_buf.buf_phys);
+
+	iowrite8(0, priv->mii_io + MII_MAC_FILT_FLAG);
+
+	ec_bhf_setup_tx_descs(priv);
+
+	netif_start_queue(net_dev);
+
+	hrtimer_init(&priv->hrtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	priv->hrtimer.function = ec_bhf_timer_fun;
+	hrtimer_start(&priv->hrtimer, ktime_set(0, polling_frequency),
+		      HRTIMER_MODE_REL);
+
+	dev_info(PRIV_TO_DEV(priv), "Device open\n");
+
+	ec_bhf_print_status(priv);
+
+	return 0;
+
+error_rx_free:
+	dma_free_coherent(dev, priv->rx_buf.alloc_len, priv->rx_buf.alloc,
+			  priv->rx_buf.alloc_len);
+out:
+	return err;
+}
+
+static int ec_bhf_stop(struct net_device *net_dev)
+{
+	struct ec_bhf_priv *priv = netdev_priv(net_dev);
+	struct device *dev = PRIV_TO_DEV(priv);
+
+	hrtimer_cancel(&priv->hrtimer);
+
+	ec_bhf_reset(priv);
+
+	netif_tx_disable(net_dev);
+
+	dma_free_coherent(dev, priv->tx_buf.alloc_len,
+			  priv->tx_buf.alloc, priv->tx_buf.alloc_phys);
+	dma_free_coherent(dev, priv->rx_buf.alloc_len,
+			  priv->rx_buf.alloc, priv->rx_buf.alloc_phys);
+
+	return 0;
+}
+
+static struct rtnl_link_stats64 *
+ec_bhf_get_stats(struct net_device *net_dev,
+		 struct rtnl_link_stats64 *stats)
+{
+	struct ec_bhf_priv *priv = netdev_priv(net_dev);
+
+	stats->rx_errors = ioread8(priv->mac_io + MAC_RX_ERR_CNT) +
+				ioread8(priv->mac_io + MAC_CRC_ERR_CNT) +
+				ioread8(priv->mac_io + MAC_FRAME_ERR_CNT);
+	stats->rx_packets = ioread32(priv->mac_io + MAC_RX_FRAME_CNT);
+	stats->tx_packets = ioread32(priv->mac_io + MAC_TX_FRAME_CNT);
+	stats->rx_dropped = ioread8(priv->mac_io + MAC_DROPPED_FRMS);
+
+	stats->tx_bytes = priv->stat_tx_bytes;
+	stats->rx_bytes = priv->stat_rx_bytes;
+
+	return stats;
+}
+
+static const struct net_device_ops ec_bhf_netdev_ops = {
+	.ndo_start_xmit		= ec_bhf_start_xmit,
+	.ndo_open		= ec_bhf_open,
+	.ndo_stop		= ec_bhf_stop,
+	.ndo_get_stats64	= ec_bhf_get_stats,
+	.ndo_change_mtu		= eth_change_mtu,
+	.ndo_validate_addr	= eth_validate_addr,
+	.ndo_set_mac_address	= eth_mac_addr
+};
+
+static int ec_bhf_probe(struct pci_dev *dev, const struct pci_device_id *id)
+{
+	struct net_device *net_dev;
+	struct ec_bhf_priv *priv;
+	void * __iomem dma_io;
+	void * __iomem io;
+	int err = 0;
+
+	err = pci_enable_device(dev);
+	if (err)
+		return err;
+
+	pci_set_master(dev);
+
+	err = pci_set_dma_mask(dev, DMA_BIT_MASK(32));
+	if (err) {
+		dev_err(&dev->dev,
+			"Required dma mask not supported, failed to initialize device\n");
+		err = -EIO;
+		goto err_disable_dev;
+	}
+
+	err = pci_set_consistent_dma_mask(dev, DMA_BIT_MASK(32));
+	if (err) {
+		dev_err(&dev->dev,
+			"Required dma mask not supported, failed to initialize device\n");
+		goto err_disable_dev;
+	}
+
+	err = pci_request_regions(dev, "ec_bhf");
+	if (err) {
+		dev_err(&dev->dev, "Failed to request pci memory regions\n");
+		goto err_disable_dev;
+	}
+
+	io = pci_iomap(dev, 0, 0);
+	if (!io) {
+		dev_err(&dev->dev, "Failed to map pci card memory bar 0");
+		err = -EIO;
+		goto err_release_regions;
+	}
+
+	dma_io = pci_iomap(dev, 2, 0);
+	if (!dma_io) {
+		dev_err(&dev->dev, "Failed to map pci card memory bar 2");
+		err = -EIO;
+		goto err_unmap;
+	}
+
+	net_dev = alloc_etherdev(sizeof(struct ec_bhf_priv));
+	if (net_dev == 0) {
+		err = -ENOMEM;
+		goto err_unmap_dma_io;
+	}
+
+	pci_set_drvdata(dev, net_dev);
+	SET_NETDEV_DEV(net_dev, &dev->dev);
+
+	net_dev->features = 0;
+	net_dev->flags |= IFF_NOARP;
+
+	net_dev->netdev_ops = &ec_bhf_netdev_ops;
+
+	priv = netdev_priv(net_dev);
+	priv->net_dev = net_dev;
+	priv->io = io;
+	priv->dma_io = dma_io;
+	priv->dev = dev;
+
+	err = ec_bhf_setup_offsets(priv);
+	if (err < 0)
+		goto err_free_net_dev;
+
+	memcpy_fromio(net_dev->dev_addr, priv->mii_io + MII_MAC_ADDR, 6);
+
+	dev_dbg(&dev->dev, "CX5020 Ethercat master address: %pM\n",
+		net_dev->dev_addr);
+
+	err = register_netdev(net_dev);
+	if (err < 0)
+		goto err_free_net_dev;
+
+	return 0;
+
+err_free_net_dev:
+	free_netdev(net_dev);
+err_unmap_dma_io:
+	pci_iounmap(dev, dma_io);
+err_unmap:
+	pci_iounmap(dev, io);
+err_release_regions:
+	pci_release_regions(dev);
+err_disable_dev:
+	pci_clear_master(dev);
+	pci_disable_device(dev);
+
+	return err;
+}
+
+static void ec_bhf_remove(struct pci_dev *dev)
+{
+	struct net_device *net_dev = pci_get_drvdata(dev);
+	struct ec_bhf_priv *priv = netdev_priv(net_dev);
+
+	unregister_netdev(net_dev);
+	free_netdev(net_dev);
+
+	pci_iounmap(dev, priv->dma_io);
+	pci_iounmap(dev, priv->io);
+	pci_release_regions(dev);
+	pci_clear_master(dev);
+	pci_disable_device(dev);
+}
+
+static struct pci_driver pci_driver = {
+	.name		= "ec_bhf",
+	.id_table	= ids,
+	.probe		= ec_bhf_probe,
+	.remove		= ec_bhf_remove,
+};
+
+static int __init ec_bhf_init(void)
+{
+	return pci_register_driver(&pci_driver);
+}
+
+static void __exit ec_bhf_exit(void)
+{
+	pci_unregister_driver(&pci_driver);
+}
+
+module_init(ec_bhf_init);
+module_exit(ec_bhf_exit);
+
+module_param(polling_frequency, long, S_IRUGO);
+MODULE_PARM_DESC(polling_frequency, "Polling timer frequency in ns");
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Dariusz Marcinkiewicz <reksio@newterm.pl>");
diff --git a/drivers/net/ethernet/emulex/benet/be_main.c b/drivers/net/ethernet/emulex/benet/be_main.c
index a18645407d2..dc19bc5dec7 100644
--- a/drivers/net/ethernet/emulex/benet/be_main.c
+++ b/drivers/net/ethernet/emulex/benet/be_main.c
@@ -4949,6 +4949,12 @@ static void be_eeh_resume(struct pci_dev *pdev)
 	if (status)
 		goto err;
 
+	/* On some BE3 FW versions, after a HW reset,
+	 * interrupts will remain disabled for each function.
+	 * So, explicitly enable interrupts
+	 */
+	be_intr_set(adapter, true);
+
 	/* tell fw we're ready to fire cmds */
 	status = be_cmd_fw_init(adapter);
 	if (status)
diff --git a/drivers/net/ethernet/freescale/gianfar.c b/drivers/net/ethernet/freescale/gianfar.c
index d82f092cae9..ee6ddbd4f25 100644
--- a/drivers/net/ethernet/freescale/gianfar.c
+++ b/drivers/net/ethernet/freescale/gianfar.c
@@ -121,6 +121,7 @@ static irqreturn_t gfar_error(int irq, void *dev_id);
 static irqreturn_t gfar_transmit(int irq, void *dev_id);
 static irqreturn_t gfar_interrupt(int irq, void *dev_id);
 static void adjust_link(struct net_device *dev);
+static noinline void gfar_update_link_state(struct gfar_private *priv);
 static int init_phy(struct net_device *dev);
 static int gfar_probe(struct platform_device *ofdev);
 static int gfar_remove(struct platform_device *ofdev);
@@ -3076,41 +3077,6 @@ static irqreturn_t gfar_interrupt(int irq, void *grp_id)
 	return IRQ_HANDLED;
 }
 
-static u32 gfar_get_flowctrl_cfg(struct gfar_private *priv)
-{
-	struct phy_device *phydev = priv->phydev;
-	u32 val = 0;
-
-	if (!phydev->duplex)
-		return val;
-
-	if (!priv->pause_aneg_en) {
-		if (priv->tx_pause_en)
-			val |= MACCFG1_TX_FLOW;
-		if (priv->rx_pause_en)
-			val |= MACCFG1_RX_FLOW;
-	} else {
-		u16 lcl_adv, rmt_adv;
-		u8 flowctrl;
-		/* get link partner capabilities */
-		rmt_adv = 0;
-		if (phydev->pause)
-			rmt_adv = LPA_PAUSE_CAP;
-		if (phydev->asym_pause)
-			rmt_adv |= LPA_PAUSE_ASYM;
-
-		lcl_adv = mii_advertise_flowctrl(phydev->advertising);
-
-		flowctrl = mii_resolve_flowctrl_fdx(lcl_adv, rmt_adv);
-		if (flowctrl & FLOW_CTRL_TX)
-			val |= MACCFG1_TX_FLOW;
-		if (flowctrl & FLOW_CTRL_RX)
-			val |= MACCFG1_RX_FLOW;
-	}
-
-	return val;
-}
-
 /* Called every time the controller might need to be made
  * aware of new link state.  The PHY code conveys this
  * information through variables in the phydev structure, and this
@@ -3120,83 +3086,12 @@ static u32 gfar_get_flowctrl_cfg(struct gfar_private *priv)
 static void adjust_link(struct net_device *dev)
 {
 	struct gfar_private *priv = netdev_priv(dev);
-	struct gfar __iomem *regs = priv->gfargrp[0].regs;
 	struct phy_device *phydev = priv->phydev;
-	int new_state = 0;
 
-	if (test_bit(GFAR_RESETTING, &priv->state))
-		return;
-
-	if (phydev->link) {
-		u32 tempval1 = gfar_read(&regs->maccfg1);
-		u32 tempval = gfar_read(&regs->maccfg2);
-		u32 ecntrl = gfar_read(&regs->ecntrl);
-
-		/* Now we make sure that we can be in full duplex mode.
-		 * If not, we operate in half-duplex mode.
-		 */
-		if (phydev->duplex != priv->oldduplex) {
-			new_state = 1;
-			if (!(phydev->duplex))
-				tempval &= ~(MACCFG2_FULL_DUPLEX);
-			else
-				tempval |= MACCFG2_FULL_DUPLEX;
-
-			priv->oldduplex = phydev->duplex;
-		}
-
-		if (phydev->speed != priv->oldspeed) {
-			new_state = 1;
-			switch (phydev->speed) {
-			case 1000:
-				tempval =
-				    ((tempval & ~(MACCFG2_IF)) | MACCFG2_GMII);
-
-				ecntrl &= ~(ECNTRL_R100);
-				break;
-			case 100:
-			case 10:
-				tempval =
-				    ((tempval & ~(MACCFG2_IF)) | MACCFG2_MII);
-
-				/* Reduced mode distinguishes
-				 * between 10 and 100
-				 */
-				if (phydev->speed == SPEED_100)
-					ecntrl |= ECNTRL_R100;
-				else
-					ecntrl &= ~(ECNTRL_R100);
-				break;
-			default:
-				netif_warn(priv, link, dev,
-					   "Ack!  Speed (%d) is not 10/100/1000!\n",
-					   phydev->speed);
-				break;
-			}
-
-			priv->oldspeed = phydev->speed;
-		}
-
-		tempval1 &= ~(MACCFG1_TX_FLOW | MACCFG1_RX_FLOW);
-		tempval1 |= gfar_get_flowctrl_cfg(priv);
-
-		gfar_write(&regs->maccfg1, tempval1);
-		gfar_write(&regs->maccfg2, tempval);
-		gfar_write(&regs->ecntrl, ecntrl);
-
-		if (!priv->oldlink) {
-			new_state = 1;
-			priv->oldlink = 1;
-		}
-	} else if (priv->oldlink) {
-		new_state = 1;
-		priv->oldlink = 0;
-		priv->oldspeed = 0;
-		priv->oldduplex = -1;
-	}
-
-	if (new_state && netif_msg_link(priv))
-		phy_print_status(phydev);
+	if (unlikely(phydev->link != priv->oldlink ||
+		     phydev->duplex != priv->oldduplex ||
+		     phydev->speed != priv->oldspeed))
+		gfar_update_link_state(priv);
 }
 
 /* Update the hash table based on the current list of multicast
@@ -3442,6 +3337,114 @@ static irqreturn_t gfar_error(int irq, void *grp_id)
 	return IRQ_HANDLED;
 }
 
+static u32 gfar_get_flowctrl_cfg(struct gfar_private *priv)
+{
+	struct phy_device *phydev = priv->phydev;
+	u32 val = 0;
+
+	if (!phydev->duplex)
+		return val;
+
+	if (!priv->pause_aneg_en) {
+		if (priv->tx_pause_en)
+			val |= MACCFG1_TX_FLOW;
+		if (priv->rx_pause_en)
+			val |= MACCFG1_RX_FLOW;
+	} else {
+		u16 lcl_adv, rmt_adv;
+		u8 flowctrl;
+		/* get link partner capabilities */
+		rmt_adv = 0;
+		if (phydev->pause)
+			rmt_adv = LPA_PAUSE_CAP;
+		if (phydev->asym_pause)
+			rmt_adv |= LPA_PAUSE_ASYM;
+
+		lcl_adv = mii_advertise_flowctrl(phydev->advertising);
+
+		flowctrl = mii_resolve_flowctrl_fdx(lcl_adv, rmt_adv);
+		if (flowctrl & FLOW_CTRL_TX)
+			val |= MACCFG1_TX_FLOW;
+		if (flowctrl & FLOW_CTRL_RX)
+			val |= MACCFG1_RX_FLOW;
+	}
+
+	return val;
+}
+
+static noinline void gfar_update_link_state(struct gfar_private *priv)
+{
+	struct gfar __iomem *regs = priv->gfargrp[0].regs;
+	struct phy_device *phydev = priv->phydev;
+
+	if (unlikely(test_bit(GFAR_RESETTING, &priv->state)))
+		return;
+
+	if (phydev->link) {
+		u32 tempval1 = gfar_read(&regs->maccfg1);
+		u32 tempval = gfar_read(&regs->maccfg2);
+		u32 ecntrl = gfar_read(&regs->ecntrl);
+
+		if (phydev->duplex != priv->oldduplex) {
+			if (!(phydev->duplex))
+				tempval &= ~(MACCFG2_FULL_DUPLEX);
+			else
+				tempval |= MACCFG2_FULL_DUPLEX;
+
+			priv->oldduplex = phydev->duplex;
+		}
+
+		if (phydev->speed != priv->oldspeed) {
+			switch (phydev->speed) {
+			case 1000:
+				tempval =
+				    ((tempval & ~(MACCFG2_IF)) | MACCFG2_GMII);
+
+				ecntrl &= ~(ECNTRL_R100);
+				break;
+			case 100:
+			case 10:
+				tempval =
+				    ((tempval & ~(MACCFG2_IF)) | MACCFG2_MII);
+
+				/* Reduced mode distinguishes
+				 * between 10 and 100
+				 */
+				if (phydev->speed == SPEED_100)
+					ecntrl |= ECNTRL_R100;
+				else
+					ecntrl &= ~(ECNTRL_R100);
+				break;
+			default:
+				netif_warn(priv, link, priv->ndev,
+					   "Ack!  Speed (%d) is not 10/100/1000!\n",
+					   phydev->speed);
+				break;
+			}
+
+			priv->oldspeed = phydev->speed;
+		}
+
+		tempval1 &= ~(MACCFG1_TX_FLOW | MACCFG1_RX_FLOW);
+		tempval1 |= gfar_get_flowctrl_cfg(priv);
+
+		gfar_write(&regs->maccfg1, tempval1);
+		gfar_write(&regs->maccfg2, tempval);
+		gfar_write(&regs->ecntrl, ecntrl);
+
+		if (!priv->oldlink)
+			priv->oldlink = 1;
+
+	} else if (priv->oldlink) {
+		priv->oldlink = 0;
+		priv->oldspeed = 0;
+		priv->oldduplex = -1;
+	}
+
+	if (netif_msg_link(priv))
+		phy_print_status(phydev);
+}
+
 static struct of_device_id gfar_match[] =
 {
 	{
diff --git a/drivers/net/ethernet/freescale/gianfar_ethtool.c b/drivers/net/ethernet/freescale/gianfar_ethtool.c
index 891dbee6e6c..76d70708f86 100644
--- a/drivers/net/ethernet/freescale/gianfar_ethtool.c
+++ b/drivers/net/ethernet/freescale/gianfar_ethtool.c
@@ -533,6 +533,9 @@ static int gfar_spauseparam(struct net_device *dev,
 	struct gfar __iomem *regs = priv->gfargrp[0].regs;
 	u32 oldadv, newadv;
 
+	if (!phydev)
+		return -ENODEV;
+
 	if (!(phydev->supported & SUPPORTED_Pause) ||
 	    (!(phydev->supported & SUPPORTED_Asym_Pause) &&
 	     (epause->rx_pause != epause->tx_pause)))
diff --git a/drivers/net/ethernet/intel/e1000e/ich8lan.c b/drivers/net/ethernet/intel/e1000e/ich8lan.c
index 9866f264f55..f0bbd4246d7 100644
--- a/drivers/net/ethernet/intel/e1000e/ich8lan.c
+++ b/drivers/net/ethernet/intel/e1000e/ich8lan.c
@@ -186,7 +186,7 @@ static bool e1000_phy_is_accessible_pchlan(struct e1000_hw *hw)
 {
 	u16 phy_reg = 0;
 	u32 phy_id = 0;
-	s32 ret_val;
+	s32 ret_val = 0;
 	u16 retry_count;
 	u32 mac_reg = 0;
 
@@ -217,11 +217,13 @@ static bool e1000_phy_is_accessible_pchlan(struct e1000_hw *hw)
 	/* In case the PHY needs to be in mdio slow mode,
 	 * set slow mode and try to get the PHY id again.
 	 */
-	hw->phy.ops.release(hw);
-	ret_val = e1000_set_mdio_slow_mode_hv(hw);
-	if (!ret_val)
-		ret_val = e1000e_get_phy_id(hw);
-	hw->phy.ops.acquire(hw);
+	if (hw->mac.type < e1000_pch_lpt) {
+		hw->phy.ops.release(hw);
+		ret_val = e1000_set_mdio_slow_mode_hv(hw);
+		if (!ret_val)
+			ret_val = e1000e_get_phy_id(hw);
+		hw->phy.ops.acquire(hw);
+	}
 
 	if (ret_val)
 		return false;
@@ -842,6 +844,17 @@ s32 e1000_set_eee_pchlan(struct e1000_hw *hw)
 		}
 	}
 
+	if (hw->phy.type == e1000_phy_82579) {
+		ret_val = e1000_read_emi_reg_locked(hw, I82579_LPI_PLL_SHUT,
+						    &data);
+		if (ret_val)
+			goto release;
+
+		data &= ~I82579_LPI_100_PLL_SHUT;
+		ret_val = e1000_write_emi_reg_locked(hw, I82579_LPI_PLL_SHUT,
+						     data);
+	}
+
 	/* R/Clr IEEE MMD 3.1 bits 11:10 - Tx/Rx LPI Received */
 	ret_val = e1000_read_emi_reg_locked(hw, pcs_status, &data);
 	if (ret_val)
@@ -1314,14 +1327,17 @@ static s32 e1000_check_for_copper_link_ich8lan(struct e1000_hw *hw)
 			return ret_val;
 	}
 
-	/* When connected at 10Mbps half-duplex, 82579 parts are excessively
+	/* When connected at 10Mbps half-duplex, some parts are excessively
 	 * aggressive resulting in many collisions. To avoid this, increase
 	 * the IPG and reduce Rx latency in the PHY.
 	 */
-	if ((hw->mac.type == e1000_pch2lan) && link) {
+	if (((hw->mac.type == e1000_pch2lan) ||
+	     (hw->mac.type == e1000_pch_lpt)) && link) {
 		u32 reg;
 		reg = er32(STATUS);
 		if (!(reg & (E1000_STATUS_FD | E1000_STATUS_SPEED_MASK))) {
+			u16 emi_addr;
+
 			reg = er32(TIPG);
 			reg &= ~E1000_TIPG_IPGT_MASK;
 			reg |= 0xFF;
@@ -1332,8 +1348,12 @@ static s32 e1000_check_for_copper_link_ich8lan(struct e1000_hw *hw)
 			if (ret_val)
 				return ret_val;
 
-			ret_val =
-			    e1000_write_emi_reg_locked(hw, I82579_RX_CONFIG, 0);
+			if (hw->mac.type == e1000_pch2lan)
+				emi_addr = I82579_RX_CONFIG;
+			else
+				emi_addr = I217_RX_CONFIG;
+
+			ret_val = e1000_write_emi_reg_locked(hw, emi_addr, 0);
 
 			hw->phy.ops.release(hw);
 
@@ -2493,51 +2513,44 @@ release:
  *  e1000_k1_gig_workaround_lv - K1 Si workaround
  *  @hw:   pointer to the HW structure
  *
- *  Workaround to set the K1 beacon duration for 82579 parts
+ *  Workaround to set the K1 beacon duration for 82579 parts in 10Mbps
+ *  Disable K1 in 1000Mbps and 100Mbps
  **/
 static s32 e1000_k1_workaround_lv(struct e1000_hw *hw)
 {
 	s32 ret_val = 0;
 	u16 status_reg = 0;
-	u32 mac_reg;
-	u16 phy_reg;
 
 	if (hw->mac.type != e1000_pch2lan)
 		return 0;
 
-	/* Set K1 beacon duration based on 1Gbps speed or otherwise */
+	/* Set K1 beacon duration based on 10Mbs speed */
 	ret_val = e1e_rphy(hw, HV_M_STATUS, &status_reg);
 	if (ret_val)
 		return ret_val;
 
 	if ((status_reg & (HV_M_STATUS_LINK_UP | HV_M_STATUS_AUTONEG_COMPLETE))
 	    == (HV_M_STATUS_LINK_UP | HV_M_STATUS_AUTONEG_COMPLETE)) {
-		mac_reg = er32(FEXTNVM4);
-		mac_reg &= ~E1000_FEXTNVM4_BEACON_DURATION_MASK;
-
-		ret_val = e1e_rphy(hw, I82579_LPI_CTRL, &phy_reg);
-		if (ret_val)
-			return ret_val;
-
-		if (status_reg & HV_M_STATUS_SPEED_1000) {
+		if (status_reg &
+		    (HV_M_STATUS_SPEED_1000 | HV_M_STATUS_SPEED_100)) {
 			u16 pm_phy_reg;
 
-			mac_reg |= E1000_FEXTNVM4_BEACON_DURATION_8USEC;
-			phy_reg &= ~I82579_LPI_CTRL_FORCE_PLL_LOCK_COUNT;
-			/* LV 1G Packet drop issue wa  */
+			/* LV 1G/100 Packet drop issue wa  */
 			ret_val = e1e_rphy(hw, HV_PM_CTRL, &pm_phy_reg);
 			if (ret_val)
 				return ret_val;
-			pm_phy_reg &= ~HV_PM_CTRL_PLL_STOP_IN_K1_GIGA;
+			pm_phy_reg &= ~HV_PM_CTRL_K1_ENABLE;
 			ret_val = e1e_wphy(hw, HV_PM_CTRL, pm_phy_reg);
 			if (ret_val)
 				return ret_val;
 		} else {
+			u32 mac_reg;
+
+			mac_reg = er32(FEXTNVM4);
+			mac_reg &= ~E1000_FEXTNVM4_BEACON_DURATION_MASK;
 			mac_reg |= E1000_FEXTNVM4_BEACON_DURATION_16USEC;
-			phy_reg |= I82579_LPI_CTRL_FORCE_PLL_LOCK_COUNT;
+			ew32(FEXTNVM4, mac_reg);
 		}
-		ew32(FEXTNVM4, mac_reg);
-		ret_val = e1e_wphy(hw, I82579_LPI_CTRL, phy_reg);
 	}
 
 	return ret_val;
diff --git a/drivers/net/ethernet/intel/e1000e/ich8lan.h b/drivers/net/ethernet/intel/e1000e/ich8lan.h
index bead50f9187..5515126c81c 100644
--- a/drivers/net/ethernet/intel/e1000e/ich8lan.h
+++ b/drivers/net/ethernet/intel/e1000e/ich8lan.h
@@ -232,16 +232,19 @@
 #define I82577_MSE_THRESHOLD	0x0887	/* 82577 Mean Square Error Threshold */
 #define I82579_MSE_LINK_DOWN	0x2411	/* MSE count before dropping link */
 #define I82579_RX_CONFIG		0x3412	/* Receive configuration */
+#define I82579_LPI_PLL_SHUT		0x4412	/* LPI PLL Shut Enable */
 #define I82579_EEE_PCS_STATUS		0x182E	/* IEEE MMD Register 3.1 >> 8 */
 #define I82579_EEE_CAPABILITY		0x0410	/* IEEE MMD Register 3.20 */
 #define I82579_EEE_ADVERTISEMENT	0x040E	/* IEEE MMD Register 7.60 */
 #define I82579_EEE_LP_ABILITY		0x040F	/* IEEE MMD Register 7.61 */
 #define I82579_EEE_100_SUPPORTED	(1 << 1)	/* 100BaseTx EEE */
 #define I82579_EEE_1000_SUPPORTED	(1 << 2)	/* 1000BaseTx EEE */
+#define I82579_LPI_100_PLL_SHUT	(1 << 2)	/* 100M LPI PLL Shut Enabled */
 #define I217_EEE_PCS_STATUS	0x9401	/* IEEE MMD Register 3.1 */
 #define I217_EEE_CAPABILITY	0x8000	/* IEEE MMD Register 3.20 */
 #define I217_EEE_ADVERTISEMENT	0x8001	/* IEEE MMD Register 7.60 */
 #define I217_EEE_LP_ABILITY	0x8002	/* IEEE MMD Register 7.61 */
+#define I217_RX_CONFIG		0xB20C	/* Receive configuration */
 
 #define E1000_EEE_RX_LPI_RCVD	0x0400	/* Tx LP idle received */
 #define E1000_EEE_TX_LPI_RCVD	0x0800	/* Rx LP idle received */
diff --git a/drivers/net/ethernet/intel/e1000e/netdev.c b/drivers/net/ethernet/intel/e1000e/netdev.c
index d50c91e5052..3e69386add0 100644
--- a/drivers/net/ethernet/intel/e1000e/netdev.c
+++ b/drivers/net/ethernet/intel/e1000e/netdev.c
@@ -1165,7 +1165,7 @@ static void e1000e_tx_hwtstamp_work(struct work_struct *work)
 		dev_kfree_skb_any(adapter->tx_hwtstamp_skb);
 		adapter->tx_hwtstamp_skb = NULL;
 		adapter->tx_hwtstamp_timeouts++;
-		e_warn("clearing Tx timestamp hang");
+		e_warn("clearing Tx timestamp hang\n");
 	} else {
 		/* reschedule to check later */
 		schedule_work(&adapter->tx_hwtstamp_work);
@@ -5687,7 +5687,7 @@ struct rtnl_link_stats64 *e1000e_get_stats64(struct net_device *netdev,
 static int e1000_change_mtu(struct net_device *netdev, int new_mtu)
 {
 	struct e1000_adapter *adapter = netdev_priv(netdev);
-	int max_frame = new_mtu + ETH_HLEN + ETH_FCS_LEN;
+	int max_frame = new_mtu + VLAN_HLEN + ETH_HLEN + ETH_FCS_LEN;
 
 	/* Jumbo frame support */
 	if ((max_frame > ETH_FRAME_LEN + ETH_FCS_LEN) &&
@@ -6235,6 +6235,7 @@ static int __e1000_resume(struct pci_dev *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_PM_SLEEP
 static int e1000e_pm_thaw(struct device *dev)
 {
 	struct net_device *netdev = pci_get_drvdata(to_pci_dev(dev));
@@ -6255,7 +6256,6 @@ static int e1000e_pm_thaw(struct device *dev)
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
 static int e1000e_pm_suspend(struct device *dev)
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
diff --git a/drivers/net/ethernet/intel/e1000e/phy.h b/drivers/net/ethernet/intel/e1000e/phy.h
index 3841bccf058..537d2780b40 100644
--- a/drivers/net/ethernet/intel/e1000e/phy.h
+++ b/drivers/net/ethernet/intel/e1000e/phy.h
@@ -164,6 +164,7 @@ s32 e1000_get_cable_length_82577(struct e1000_hw *hw);
 #define HV_M_STATUS_AUTONEG_COMPLETE	0x1000
 #define HV_M_STATUS_SPEED_MASK		0x0300
 #define HV_M_STATUS_SPEED_1000		0x0200
+#define HV_M_STATUS_SPEED_100		0x0100
 #define HV_M_STATUS_LINK_UP		0x0040
 
 #define IGP01E1000_PHY_PCS_INIT_REG	0x00B4
diff --git a/drivers/net/ethernet/intel/i40e/i40e_main.c b/drivers/net/ethernet/intel/i40e/i40e_main.c
index 1e526c072a4..2e72449f126 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_main.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_main.c
@@ -2897,12 +2897,9 @@ static irqreturn_t i40e_intr(int irq, void *data)
 		u32 prttsyn_stat = rd32(hw, I40E_PRTTSYN_STAT_0);
 
 		if (prttsyn_stat & I40E_PRTTSYN_STAT_0_TXTIME_MASK) {
-			ena_mask &= ~I40E_PFINT_ICR0_ENA_TIMESYNC_MASK;
+			icr0 &= ~I40E_PFINT_ICR0_ENA_TIMESYNC_MASK;
 			i40e_ptp_tx_hwtstamp(pf);
-			prttsyn_stat &= ~I40E_PRTTSYN_STAT_0_TXTIME_MASK;
 		}
-
-		wr32(hw, I40E_PRTTSYN_STAT_0, prttsyn_stat);
 	}
 
 	/* If a critical error is pending we have no choice but to reset the
@@ -4271,6 +4268,14 @@ static int i40e_open(struct net_device *netdev)
 	if (err)
 		return err;
 
+	/* configure global TSO hardware offload settings */
+	wr32(&pf->hw, I40E_GLLAN_TSOMSK_F, be32_to_cpu(TCP_FLAG_PSH |
+						       TCP_FLAG_FIN) >> 16);
+	wr32(&pf->hw, I40E_GLLAN_TSOMSK_M, be32_to_cpu(TCP_FLAG_PSH |
+						       TCP_FLAG_FIN |
+						       TCP_FLAG_CWR) >> 16);
+	wr32(&pf->hw, I40E_GLLAN_TSOMSK_L, be32_to_cpu(TCP_FLAG_CWR) >> 16);
+
 #ifdef CONFIG_I40E_VXLAN
 	vxlan_get_rx_port(netdev);
 #endif
@@ -6712,6 +6717,7 @@ static int i40e_config_netdev(struct i40e_vsi *vsi)
 			   NETIF_F_HW_VLAN_CTAG_FILTER |
 			   NETIF_F_IPV6_CSUM	       |
 			   NETIF_F_TSO		       |
+			   NETIF_F_TSO_ECN	       |
 			   NETIF_F_TSO6		       |
 			   NETIF_F_RXCSUM	       |
 			   NETIF_F_NTUPLE	       |
diff --git a/drivers/net/ethernet/intel/i40e/i40e_nvm.c b/drivers/net/ethernet/intel/i40e/i40e_nvm.c
index 262bdf11d22..81299189a47 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_nvm.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_nvm.c
@@ -160,7 +160,7 @@ static i40e_status i40e_poll_sr_srctl_done_bit(struct i40e_hw *hw)
 		udelay(5);
 	}
 	if (ret_code == I40E_ERR_TIMEOUT)
-		hw_dbg(hw, "Done bit in GLNVM_SRCTL not set");
+		hw_dbg(hw, "Done bit in GLNVM_SRCTL not set\n");
 	return ret_code;
 }
 
diff --git a/drivers/net/ethernet/intel/i40e/i40e_ptp.c b/drivers/net/ethernet/intel/i40e/i40e_ptp.c
index e33ec6c842b..e61e6372080 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_ptp.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_ptp.c
@@ -239,7 +239,7 @@ static void i40e_ptp_tx_work(struct work_struct *work)
 		dev_kfree_skb_any(pf->ptp_tx_skb);
 		pf->ptp_tx_skb = NULL;
 		pf->tx_hwtstamp_timeouts++;
-		dev_warn(&pf->pdev->dev, "clearing Tx timestamp hang");
+		dev_warn(&pf->pdev->dev, "clearing Tx timestamp hang\n");
 		return;
 	}
 
@@ -321,7 +321,7 @@ void i40e_ptp_rx_hang(struct i40e_vsi *vsi)
 		pf->last_rx_ptp_check = jiffies;
 		pf->rx_hwtstamp_cleared++;
 		dev_warn(&vsi->back->pdev->dev,
-			 "%s: clearing Rx timestamp hang",
+			 "%s: clearing Rx timestamp hang\n",
 			 __func__);
 	}
 }
diff --git a/drivers/net/ethernet/intel/i40e/i40e_txrx.c b/drivers/net/ethernet/intel/i40e/i40e_txrx.c
index 0f5d96ad281..9478ddc66ca 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_txrx.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_txrx.c
@@ -418,7 +418,7 @@ int i40e_add_del_fdir(struct i40e_vsi *vsi,
 		}
 		break;
 	default:
-		dev_info(&pf->pdev->dev, "Could not specify spec type %d",
+		dev_info(&pf->pdev->dev, "Could not specify spec type %d\n",
 			 input->flow_type);
 		ret = -EINVAL;
 	}
@@ -478,7 +478,7 @@ static void i40e_fd_handle_status(struct i40e_ring *rx_ring,
 				pf->flags |= I40E_FLAG_FDIR_REQUIRES_REINIT;
 			}
 		} else {
-			dev_info(&pdev->dev, "FD filter programming error");
+			dev_info(&pdev->dev, "FD filter programming error\n");
 		}
 	} else if (error ==
 			  (0x1 << I40E_RX_PROG_STATUS_DESC_NO_FD_ENTRY_SHIFT)) {
@@ -1713,9 +1713,11 @@ static int i40e_tx_prepare_vlan_flags(struct sk_buff *skb,
 				I40E_TX_FLAGS_VLAN_PRIO_SHIFT;
 		if (tx_flags & I40E_TX_FLAGS_SW_VLAN) {
 			struct vlan_ethhdr *vhdr;
-			if (skb_header_cloned(skb) &&
-			    pskb_expand_head(skb, 0, 0, GFP_ATOMIC))
-				return -ENOMEM;
+			int rc;
+
+			rc = skb_cow_head(skb, 0);
+			if (rc < 0)
+				return rc;
 			vhdr = (struct vlan_ethhdr *)skb->data;
 			vhdr->h_vlan_TCI = htons(tx_flags >>
 						 I40E_TX_FLAGS_VLAN_SHIFT);
@@ -1743,20 +1745,18 @@ static int i40e_tso(struct i40e_ring *tx_ring, struct sk_buff *skb,
 		    u64 *cd_type_cmd_tso_mss, u32 *cd_tunneling)
 {
 	u32 cd_cmd, cd_tso_len, cd_mss;
+	struct ipv6hdr *ipv6h;
 	struct tcphdr *tcph;
 	struct iphdr *iph;
 	u32 l4len;
 	int err;
-	struct ipv6hdr *ipv6h;
 
 	if (!skb_is_gso(skb))
 		return 0;
 
-	if (skb_header_cloned(skb)) {
-		err = pskb_expand_head(skb, 0, 0, GFP_ATOMIC);
-		if (err)
-			return err;
-	}
+	err = skb_cow_head(skb, 0);
+	if (err < 0)
+		return err;
 
 	if (protocol == htons(ETH_P_IP)) {
 		iph = skb->encapsulation ? inner_ip_hdr(skb) : ip_hdr(skb);
diff --git a/drivers/net/ethernet/intel/igb/e1000_i210.c b/drivers/net/ethernet/intel/igb/e1000_i210.c
index db963397cc2..f67f8a170b9 100644
--- a/drivers/net/ethernet/intel/igb/e1000_i210.c
+++ b/drivers/net/ethernet/intel/igb/e1000_i210.c
@@ -365,7 +365,7 @@ static s32 igb_read_invm_word_i210(struct e1000_hw *hw, u8 address, u16 *data)
 			word_address = INVM_DWORD_TO_WORD_ADDRESS(invm_dword);
 			if (word_address == address) {
 				*data = INVM_DWORD_TO_WORD_DATA(invm_dword);
-				hw_dbg("Read INVM Word 0x%02x = %x",
+				hw_dbg("Read INVM Word 0x%02x = %x\n",
 					  address, *data);
 				status = E1000_SUCCESS;
 				break;
diff --git a/drivers/net/ethernet/intel/igb/e1000_mac.c b/drivers/net/ethernet/intel/igb/e1000_mac.c
index 5910a932ea7..1e0c404db81 100644
--- a/drivers/net/ethernet/intel/igb/e1000_mac.c
+++ b/drivers/net/ethernet/intel/igb/e1000_mac.c
@@ -929,11 +929,10 @@ s32 igb_config_fc_after_link_up(struct e1000_hw *hw)
 			 */
 			if (hw->fc.requested_mode == e1000_fc_full) {
 				hw->fc.current_mode = e1000_fc_full;
-				hw_dbg("Flow Control = FULL.\r\n");
+				hw_dbg("Flow Control = FULL.\n");
 			} else {
 				hw->fc.current_mode = e1000_fc_rx_pause;
-				hw_dbg("Flow Control = "
-				       "RX PAUSE frames only.\r\n");
+				hw_dbg("Flow Control = RX PAUSE frames only.\n");
 			}
 		}
 		/* For receiving PAUSE frames ONLY.
@@ -948,7 +947,7 @@ s32 igb_config_fc_after_link_up(struct e1000_hw *hw)
 			  (mii_nway_lp_ability_reg & NWAY_LPAR_PAUSE) &&
 			  (mii_nway_lp_ability_reg & NWAY_LPAR_ASM_DIR)) {
 			hw->fc.current_mode = e1000_fc_tx_pause;
-			hw_dbg("Flow Control = TX PAUSE frames only.\r\n");
+			hw_dbg("Flow Control = TX PAUSE frames only.\n");
 		}
 		/* For transmitting PAUSE frames ONLY.
 		 *
@@ -962,7 +961,7 @@ s32 igb_config_fc_after_link_up(struct e1000_hw *hw)
 			 !(mii_nway_lp_ability_reg & NWAY_LPAR_PAUSE) &&
 			 (mii_nway_lp_ability_reg & NWAY_LPAR_ASM_DIR)) {
 			hw->fc.current_mode = e1000_fc_rx_pause;
-			hw_dbg("Flow Control = RX PAUSE frames only.\r\n");
+			hw_dbg("Flow Control = RX PAUSE frames only.\n");
 		}
 		/* Per the IEEE spec, at this point flow control should be
 		 * disabled.  However, we want to consider that we could
@@ -988,10 +987,10 @@ s32 igb_config_fc_after_link_up(struct e1000_hw *hw)
 			 (hw->fc.requested_mode == e1000_fc_tx_pause) ||
 			 (hw->fc.strict_ieee)) {
 			hw->fc.current_mode = e1000_fc_none;
-			hw_dbg("Flow Control = NONE.\r\n");
+			hw_dbg("Flow Control = NONE.\n");
 		} else {
 			hw->fc.current_mode = e1000_fc_rx_pause;
-			hw_dbg("Flow Control = RX PAUSE frames only.\r\n");
+			hw_dbg("Flow Control = RX PAUSE frames only.\n");
 		}
 
 		/* Now we need to do one last check...  If we auto-
diff --git a/drivers/net/ethernet/intel/igb/igb_main.c b/drivers/net/ethernet/intel/igb/igb_main.c
index fb98d4602f9..16430a8440f 100644
--- a/drivers/net/ethernet/intel/igb/igb_main.c
+++ b/drivers/net/ethernet/intel/igb/igb_main.c
@@ -5193,8 +5193,10 @@ void igb_update_stats(struct igb_adapter *adapter,
 
 	rcu_read_lock();
 	for (i = 0; i < adapter->num_rx_queues; i++) {
-		u32 rqdpc = rd32(E1000_RQDPC(i));
 		struct igb_ring *ring = adapter->rx_ring[i];
+		u32 rqdpc = rd32(E1000_RQDPC(i));
+		if (hw->mac.type >= e1000_i210)
+			wr32(E1000_RQDPC(i), 0);
 
 		if (rqdpc) {
 			ring->rx_stats.drops += rqdpc;
diff --git a/drivers/net/ethernet/intel/igb/igb_ptp.c b/drivers/net/ethernet/intel/igb/igb_ptp.c
index 9209d652e1c..ab25e49365f 100644
--- a/drivers/net/ethernet/intel/igb/igb_ptp.c
+++ b/drivers/net/ethernet/intel/igb/igb_ptp.c
@@ -389,7 +389,7 @@ static void igb_ptp_tx_work(struct work_struct *work)
 		adapter->ptp_tx_skb = NULL;
 		clear_bit_unlock(__IGB_PTP_TX_IN_PROGRESS, &adapter->state);
 		adapter->tx_hwtstamp_timeouts++;
-		dev_warn(&adapter->pdev->dev, "clearing Tx timestamp hang");
+		dev_warn(&adapter->pdev->dev, "clearing Tx timestamp hang\n");
 		return;
 	}
 
@@ -451,7 +451,7 @@ void igb_ptp_rx_hang(struct igb_adapter *adapter)
 		rd32(E1000_RXSTMPH);
 		adapter->last_rx_ptp_check = jiffies;
 		adapter->rx_hwtstamp_cleared++;
-		dev_warn(&adapter->pdev->dev, "clearing Rx timestamp hang");
+		dev_warn(&adapter->pdev->dev, "clearing Rx timestamp hang\n");
 	}
 }
 
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe.h b/drivers/net/ethernet/intel/ixgbe/ixgbe.h
index 1a12c1dd7a2..c6c4ca7d68e 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe.h
@@ -256,7 +256,6 @@ struct ixgbe_ring {
 		struct ixgbe_tx_buffer *tx_buffer_info;
 		struct ixgbe_rx_buffer *rx_buffer_info;
 	};
-	unsigned long last_rx_timestamp;
 	unsigned long state;
 	u8 __iomem *tail;
 	dma_addr_t dma;			/* phys. address of descriptor ring */
@@ -770,6 +769,7 @@ struct ixgbe_adapter {
 	unsigned long ptp_tx_start;
 	unsigned long last_overflow_check;
 	unsigned long last_rx_ptp_check;
+	unsigned long last_rx_timestamp;
 	spinlock_t tmreg_lock;
 	struct cyclecounter cc;
 	struct timecounter tc;
@@ -944,24 +944,7 @@ void ixgbe_ptp_init(struct ixgbe_adapter *adapter);
 void ixgbe_ptp_stop(struct ixgbe_adapter *adapter);
 void ixgbe_ptp_overflow_check(struct ixgbe_adapter *adapter);
 void ixgbe_ptp_rx_hang(struct ixgbe_adapter *adapter);
-void __ixgbe_ptp_rx_hwtstamp(struct ixgbe_q_vector *q_vector,
-			     struct sk_buff *skb);
-static inline void ixgbe_ptp_rx_hwtstamp(struct ixgbe_ring *rx_ring,
-					 union ixgbe_adv_rx_desc *rx_desc,
-					 struct sk_buff *skb)
-{
-	if (unlikely(!ixgbe_test_staterr(rx_desc, IXGBE_RXDADV_STAT_TS)))
-		return;
-
-	__ixgbe_ptp_rx_hwtstamp(rx_ring->q_vector, skb);
-
-	/*
-	 * Update the last_rx_timestamp timer in order to enable watchdog check
-	 * for error case of latched timestamp on a dropped packet.
-	 */
-	rx_ring->last_rx_timestamp = jiffies;
-}
-
+void ixgbe_ptp_rx_hwtstamp(struct ixgbe_adapter *adapter, struct sk_buff *skb);
 int ixgbe_ptp_set_ts_config(struct ixgbe_adapter *adapter, struct ifreq *ifr);
 int ixgbe_ptp_get_ts_config(struct ixgbe_adapter *adapter, struct ifreq *ifr);
 void ixgbe_ptp_start_cyclecounter(struct ixgbe_adapter *adapter);
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c
index 24fba39e194..981b8a7b100 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c
@@ -1195,7 +1195,7 @@ static s32 ixgbe_detect_eeprom_page_size_generic(struct ixgbe_hw *hw,
 	 */
 	hw->eeprom.word_page_size = IXGBE_EEPROM_PAGE_SIZE_MAX - data[0];
 
-	hw_dbg(hw, "Detected EEPROM page size = %d words.",
+	hw_dbg(hw, "Detected EEPROM page size = %d words.\n",
 	       hw->eeprom.word_page_size);
 out:
 	return status;
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
index 2fecc2626de..c047c3ef8d7 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
@@ -1664,7 +1664,8 @@ static void ixgbe_process_skb_fields(struct ixgbe_ring *rx_ring,
 
 	ixgbe_rx_checksum(rx_ring, rx_desc, skb);
 
-	ixgbe_ptp_rx_hwtstamp(rx_ring, rx_desc, skb);
+	if (unlikely(ixgbe_test_staterr(rx_desc, IXGBE_RXDADV_STAT_TS)))
+		ixgbe_ptp_rx_hwtstamp(rx_ring->q_vector->adapter, skb);
 
 	if ((dev->features & NETIF_F_HW_VLAN_CTAG_RX) &&
 	    ixgbe_test_staterr(rx_desc, IXGBE_RXD_STAT_VP)) {
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c
index 23f765263f1..a76af8e28a0 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c
@@ -536,7 +536,7 @@ s32 ixgbe_setup_phy_link_generic(struct ixgbe_hw *hw)
 
 	if (time_out == max_time_out) {
 		status = IXGBE_ERR_LINK_SETUP;
-		hw_dbg(hw, "ixgbe_setup_phy_link_generic: time out");
+		hw_dbg(hw, "ixgbe_setup_phy_link_generic: time out\n");
 	}
 
 	return status;
@@ -745,7 +745,7 @@ s32 ixgbe_setup_phy_link_tnx(struct ixgbe_hw *hw)
 
 	if (time_out == max_time_out) {
 		status = IXGBE_ERR_LINK_SETUP;
-		hw_dbg(hw, "ixgbe_setup_phy_link_tnx: time out");
+		hw_dbg(hw, "ixgbe_setup_phy_link_tnx: time out\n");
 	}
 
 	return status;
@@ -1175,7 +1175,7 @@ s32 ixgbe_identify_sfp_module_generic(struct ixgbe_hw *hw)
 				status = 0;
 			} else {
 				if (hw->allow_unsupported_sfp) {
-					e_warn(drv, "WARNING: Intel (R) Network Connections are quality tested using Intel (R) Ethernet Optics.  Using untested modules is not supported and may cause unstable operation or damage to the module or the adapter.  Intel Corporation is not responsible for any harm caused by using untested modules.");
+					e_warn(drv, "WARNING: Intel (R) Network Connections are quality tested using Intel (R) Ethernet Optics.  Using untested modules is not supported and may cause unstable operation or damage to the module or the adapter.  Intel Corporation is not responsible for any harm caused by using untested modules.\n");
 					status = 0;
 				} else {
 					hw_dbg(hw,
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c
index 63515a6f67f..8902ae68345 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c
@@ -435,10 +435,8 @@ void ixgbe_ptp_overflow_check(struct ixgbe_adapter *adapter)
 void ixgbe_ptp_rx_hang(struct ixgbe_adapter *adapter)
 {
 	struct ixgbe_hw *hw = &adapter->hw;
-	struct ixgbe_ring *rx_ring;
 	u32 tsyncrxctl = IXGBE_READ_REG(hw, IXGBE_TSYNCRXCTL);
 	unsigned long rx_event;
-	int n;
 
 	/* if we don't have a valid timestamp in the registers, just update the
 	 * timeout counter and exit
@@ -450,18 +448,15 @@ void ixgbe_ptp_rx_hang(struct ixgbe_adapter *adapter)
 
 	/* determine the most recent watchdog or rx_timestamp event */
 	rx_event = adapter->last_rx_ptp_check;
-	for (n = 0; n < adapter->num_rx_queues; n++) {
-		rx_ring = adapter->rx_ring[n];
-		if (time_after(rx_ring->last_rx_timestamp, rx_event))
-			rx_event = rx_ring->last_rx_timestamp;
-	}
+	if (time_after(adapter->last_rx_timestamp, rx_event))
+		rx_event = adapter->last_rx_timestamp;
 
 	/* only need to read the high RXSTMP register to clear the lock */
 	if (time_is_before_jiffies(rx_event + 5*HZ)) {
 		IXGBE_READ_REG(hw, IXGBE_RXSTMPH);
 		adapter->last_rx_ptp_check = jiffies;
 
-		e_warn(drv, "clearing RX Timestamp hang");
+		e_warn(drv, "clearing RX Timestamp hang\n");
 	}
 }
 
@@ -517,7 +512,7 @@ static void ixgbe_ptp_tx_hwtstamp_work(struct work_struct *work)
 		dev_kfree_skb_any(adapter->ptp_tx_skb);
 		adapter->ptp_tx_skb = NULL;
 		clear_bit_unlock(__IXGBE_PTP_TX_IN_PROGRESS, &adapter->state);
-		e_warn(drv, "clearing Tx Timestamp hang");
+		e_warn(drv, "clearing Tx Timestamp hang\n");
 		return;
 	}
 
@@ -530,35 +525,22 @@ static void ixgbe_ptp_tx_hwtstamp_work(struct work_struct *work)
 }
 
 /**
- * __ixgbe_ptp_rx_hwtstamp - utility function which checks for RX time stamp
- * @q_vector: structure containing interrupt and ring information
+ * ixgbe_ptp_rx_hwtstamp - utility function which checks for RX time stamp
+ * @adapter: pointer to adapter struct
  * @skb: particular skb to send timestamp with
  *
  * if the timestamp is valid, we convert it into the timecounter ns
  * value, then store that result into the shhwtstamps structure which
  * is passed up the network stack
  */
-void __ixgbe_ptp_rx_hwtstamp(struct ixgbe_q_vector *q_vector,
-			     struct sk_buff *skb)
+void ixgbe_ptp_rx_hwtstamp(struct ixgbe_adapter *adapter, struct sk_buff *skb)
 {
-	struct ixgbe_adapter *adapter;
-	struct ixgbe_hw *hw;
+	struct ixgbe_hw *hw = &adapter->hw;
 	struct skb_shared_hwtstamps *shhwtstamps;
 	u64 regval = 0, ns;
 	u32 tsyncrxctl;
 	unsigned long flags;
 
-	/* we cannot process timestamps on a ring without a q_vector */
-	if (!q_vector || !q_vector->adapter)
-		return;
-
-	adapter = q_vector->adapter;
-	hw = &adapter->hw;
-
-	/*
-	 * Read the tsyncrxctl register afterwards in order to prevent taking an
-	 * I/O hit on every packet.
-	 */
 	tsyncrxctl = IXGBE_READ_REG(hw, IXGBE_TSYNCRXCTL);
 	if (!(tsyncrxctl & IXGBE_TSYNCRXCTL_VALID))
 		return;
@@ -566,13 +548,17 @@ void __ixgbe_ptp_rx_hwtstamp(struct ixgbe_q_vector *q_vector,
 	regval |= (u64)IXGBE_READ_REG(hw, IXGBE_RXSTMPL);
 	regval |= (u64)IXGBE_READ_REG(hw, IXGBE_RXSTMPH) << 32;
 
-
 	spin_lock_irqsave(&adapter->tmreg_lock, flags);
 	ns = timecounter_cyc2time(&adapter->tc, regval);
 	spin_unlock_irqrestore(&adapter->tmreg_lock, flags);
 
 	shhwtstamps = skb_hwtstamps(skb);
 	shhwtstamps->hwtstamp = ns_to_ktime(ns);
+
+	/* Update the last_rx_timestamp timer in order to enable watchdog check
+	 * for error case of latched timestamp on a dropped packet.
+	 */
+	adapter->last_rx_timestamp = jiffies;
 }
 
 int ixgbe_ptp_get_ts_config(struct ixgbe_adapter *adapter, struct ifreq *ifr)
diff --git a/drivers/net/ethernet/jme.c b/drivers/net/ethernet/jme.c
index b0c6050479e..b78378cea5e 100644
--- a/drivers/net/ethernet/jme.c
+++ b/drivers/net/ethernet/jme.c
@@ -1988,7 +1988,7 @@ jme_alloc_txdesc(struct jme_adapter *jme,
 	return idx;
 }
 
-static void
+static int
 jme_fill_tx_map(struct pci_dev *pdev,
 		struct txdesc *txdesc,
 		struct jme_buffer_info *txbi,
@@ -2005,6 +2005,9 @@ jme_fill_tx_map(struct pci_dev *pdev,
 				len,
 				PCI_DMA_TODEVICE);
 
+	if (unlikely(pci_dma_mapping_error(pdev, dmaaddr)))
+		return -EINVAL;
+
 	pci_dma_sync_single_for_device(pdev,
 				       dmaaddr,
 				       len,
@@ -2021,9 +2024,30 @@ jme_fill_tx_map(struct pci_dev *pdev,
 
 	txbi->mapping = dmaaddr;
 	txbi->len = len;
+	return 0;
 }
 
-static void
+static void jme_drop_tx_map(struct jme_adapter *jme, int startidx, int count)
+{
+	struct jme_ring *txring = &(jme->txring[0]);
+	struct jme_buffer_info *txbi = txring->bufinf, *ctxbi;
+	int mask = jme->tx_ring_mask;
+	int j;
+
+	for (j = 0 ; j < count ; j++) {
+		ctxbi = txbi + ((startidx + j + 2) & (mask));
+		pci_unmap_page(jme->pdev,
+				ctxbi->mapping,
+				ctxbi->len,
+				PCI_DMA_TODEVICE);
+
+				ctxbi->mapping = 0;
+				ctxbi->len = 0;
+	}
+
+}
+
+static int
 jme_map_tx_skb(struct jme_adapter *jme, struct sk_buff *skb, int idx)
 {
 	struct jme_ring *txring = &(jme->txring[0]);
@@ -2034,25 +2058,37 @@ jme_map_tx_skb(struct jme_adapter *jme, struct sk_buff *skb, int idx)
 	int mask = jme->tx_ring_mask;
 	const struct skb_frag_struct *frag;
 	u32 len;
+	int ret = 0;
 
 	for (i = 0 ; i < nr_frags ; ++i) {
 		frag = &skb_shinfo(skb)->frags[i];
 		ctxdesc = txdesc + ((idx + i + 2) & (mask));
 		ctxbi = txbi + ((idx + i + 2) & (mask));
 
-		jme_fill_tx_map(jme->pdev, ctxdesc, ctxbi,
+		ret = jme_fill_tx_map(jme->pdev, ctxdesc, ctxbi,
 				skb_frag_page(frag),
 				frag->page_offset, skb_frag_size(frag), hidma);
+		if (ret) {
+			jme_drop_tx_map(jme, idx, i);
+			goto out;
+		}
+
 	}
 
 	len = skb_is_nonlinear(skb) ? skb_headlen(skb) : skb->len;
 	ctxdesc = txdesc + ((idx + 1) & (mask));
 	ctxbi = txbi + ((idx + 1) & (mask));
-	jme_fill_tx_map(jme->pdev, ctxdesc, ctxbi, virt_to_page(skb->data),
+	ret = jme_fill_tx_map(jme->pdev, ctxdesc, ctxbi, virt_to_page(skb->data),
 			offset_in_page(skb->data), len, hidma);
+	if (ret)
+		jme_drop_tx_map(jme, idx, i);
+
+out:
+	return ret;
 
 }
 
+
 static int
 jme_tx_tso(struct sk_buff *skb, __le16 *mss, u8 *flags)
 {
@@ -2131,6 +2167,7 @@ jme_fill_tx_desc(struct jme_adapter *jme, struct sk_buff *skb, int idx)
 	struct txdesc *txdesc;
 	struct jme_buffer_info *txbi;
 	u8 flags;
+	int ret = 0;
 
 	txdesc = (struct txdesc *)txring->desc + idx;
 	txbi = txring->bufinf + idx;
@@ -2155,7 +2192,10 @@ jme_fill_tx_desc(struct jme_adapter *jme, struct sk_buff *skb, int idx)
 	if (jme_tx_tso(skb, &txdesc->desc1.mss, &flags))
 		jme_tx_csum(jme, skb, &flags);
 	jme_tx_vlan(skb, &txdesc->desc1.vlan, &flags);
-	jme_map_tx_skb(jme, skb, idx);
+	ret = jme_map_tx_skb(jme, skb, idx);
+	if (ret)
+		return ret;
+
 	txdesc->desc1.flags = flags;
 	/*
 	 * Set tx buffer info after telling NIC to send
@@ -2228,7 +2268,8 @@ jme_start_xmit(struct sk_buff *skb, struct net_device *netdev)
 		return NETDEV_TX_BUSY;
 	}
 
-	jme_fill_tx_desc(jme, skb, idx);
+	if (jme_fill_tx_desc(jme, skb, idx))
+		return NETDEV_TX_OK;
 
 	jwrite32(jme, JME_TXCS, jme->reg_txcs |
 				TXCS_SELECT_QUEUE0 |
diff --git a/drivers/net/ethernet/marvell/mvmdio.c b/drivers/net/ethernet/marvell/mvmdio.c
index b161a525fc5..9d5ced263a5 100644
--- a/drivers/net/ethernet/marvell/mvmdio.c
+++ b/drivers/net/ethernet/marvell/mvmdio.c
@@ -232,7 +232,7 @@ static int orion_mdio_probe(struct platform_device *pdev)
 		clk_prepare_enable(dev->clk);
 
 	dev->err_interrupt = platform_get_irq(pdev, 0);
-	if (dev->err_interrupt != -ENXIO) {
+	if (dev->err_interrupt > 0) {
 		ret = devm_request_irq(&pdev->dev, dev->err_interrupt,
 					orion_mdio_err_irq,
 					IRQF_SHARED, pdev->name, dev);
@@ -241,6 +241,9 @@ static int orion_mdio_probe(struct platform_device *pdev)
 
 		writel(MVMDIO_ERR_INT_SMI_DONE,
 			dev->regs + MVMDIO_ERR_INT_MASK);
+
+	} else if (dev->err_interrupt == -EPROBE_DEFER) {
+		return -EPROBE_DEFER;
 	}
 
 	mutex_init(&dev->lock);
diff --git a/drivers/net/ethernet/marvell/mvneta.c b/drivers/net/ethernet/marvell/mvneta.c
index b248bcbdae6..14786c8bf99 100644
--- a/drivers/net/ethernet/marvell/mvneta.c
+++ b/drivers/net/ethernet/marvell/mvneta.c
@@ -89,8 +89,9 @@
 #define      MVNETA_TX_IN_PRGRS                  BIT(1)
 #define      MVNETA_TX_FIFO_EMPTY                BIT(8)
 #define MVNETA_RX_MIN_FRAME_SIZE                 0x247c
-#define MVNETA_SGMII_SERDES_CFG			 0x24A0
+#define MVNETA_SERDES_CFG			 0x24A0
 #define      MVNETA_SGMII_SERDES_PROTO		 0x0cc7
+#define      MVNETA_QSGMII_SERDES_PROTO		 0x0667
 #define MVNETA_TYPE_PRIO                         0x24bc
 #define      MVNETA_FORCE_UNI                    BIT(21)
 #define MVNETA_TXQ_CMD_1                         0x24e4
@@ -711,35 +712,6 @@ static void mvneta_rxq_bm_disable(struct mvneta_port *pp,
 	mvreg_write(pp, MVNETA_RXQ_CONFIG_REG(rxq->id), val);
 }
 
-
-
-/* Sets the RGMII Enable bit (RGMIIEn) in port MAC control register */
-static void mvneta_gmac_rgmii_set(struct mvneta_port *pp, int enable)
-{
-	u32  val;
-
-	val = mvreg_read(pp, MVNETA_GMAC_CTRL_2);
-
-	if (enable)
-		val |= MVNETA_GMAC2_PORT_RGMII;
-	else
-		val &= ~MVNETA_GMAC2_PORT_RGMII;
-
-	mvreg_write(pp, MVNETA_GMAC_CTRL_2, val);
-}
-
-/* Config SGMII port */
-static void mvneta_port_sgmii_config(struct mvneta_port *pp)
-{
-	u32 val;
-
-	val = mvreg_read(pp, MVNETA_GMAC_CTRL_2);
-	val |= MVNETA_GMAC2_PCS_ENABLE;
-	mvreg_write(pp, MVNETA_GMAC_CTRL_2, val);
-
-	mvreg_write(pp, MVNETA_SGMII_SERDES_CFG, MVNETA_SGMII_SERDES_PROTO);
-}
-
 /* Start the Ethernet port RX and TX activity */
 static void mvneta_port_up(struct mvneta_port *pp)
 {
@@ -2749,26 +2721,44 @@ static void mvneta_conf_mbus_windows(struct mvneta_port *pp,
 }
 
 /* Power up the port */
-static void mvneta_port_power_up(struct mvneta_port *pp, int phy_mode)
+static int mvneta_port_power_up(struct mvneta_port *pp, int phy_mode)
 {
-	u32 val;
+	u32 ctrl;
 
 	/* MAC Cause register should be cleared */
 	mvreg_write(pp, MVNETA_UNIT_INTR_CAUSE, 0);
 
-	if (phy_mode == PHY_INTERFACE_MODE_SGMII)
-		mvneta_port_sgmii_config(pp);
+	ctrl = mvreg_read(pp, MVNETA_GMAC_CTRL_2);
 
-	mvneta_gmac_rgmii_set(pp, 1);
+	/* Even though it might look weird, when we're configured in
+	 * SGMII or QSGMII mode, the RGMII bit needs to be set.
+	 */
+	switch(phy_mode) {
+	case PHY_INTERFACE_MODE_QSGMII:
+		mvreg_write(pp, MVNETA_SERDES_CFG, MVNETA_QSGMII_SERDES_PROTO);
+		ctrl |= MVNETA_GMAC2_PCS_ENABLE | MVNETA_GMAC2_PORT_RGMII;
+		break;
+	case PHY_INTERFACE_MODE_SGMII:
+		mvreg_write(pp, MVNETA_SERDES_CFG, MVNETA_SGMII_SERDES_PROTO);
+		ctrl |= MVNETA_GMAC2_PCS_ENABLE | MVNETA_GMAC2_PORT_RGMII;
+		break;
+	case PHY_INTERFACE_MODE_RGMII:
+	case PHY_INTERFACE_MODE_RGMII_ID:
+		ctrl |= MVNETA_GMAC2_PORT_RGMII;
+		break;
+	default:
+		return -EINVAL;
+	}
 
 	/* Cancel Port Reset */
-	val = mvreg_read(pp, MVNETA_GMAC_CTRL_2);
-	val &= ~MVNETA_GMAC2_PORT_RESET;
-	mvreg_write(pp, MVNETA_GMAC_CTRL_2, val);
+	ctrl &= ~MVNETA_GMAC2_PORT_RESET;
+	mvreg_write(pp, MVNETA_GMAC_CTRL_2, ctrl);
 
 	while ((mvreg_read(pp, MVNETA_GMAC_CTRL_2) &
 		MVNETA_GMAC2_PORT_RESET) != 0)
 		continue;
+
+	return 0;
 }
 
 /* Device initialization routine */
@@ -2879,7 +2869,12 @@ static int mvneta_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "can't init eth hal\n");
 		goto err_free_stats;
 	}
-	mvneta_port_power_up(pp, phy_mode);
+
+	err = mvneta_port_power_up(pp, phy_mode);
+	if (err < 0) {
+		dev_err(&pdev->dev, "can't power up port\n");
+		goto err_deinit;
+	}
 
 	dram_target_info = mv_mbus_dram_info();
 	if (dram_target_info)
diff --git a/drivers/net/ethernet/mellanox/mlx4/cmd.c b/drivers/net/ethernet/mellanox/mlx4/cmd.c
index 78099eab767..92d3249f63f 100644
--- a/drivers/net/ethernet/mellanox/mlx4/cmd.c
+++ b/drivers/net/ethernet/mellanox/mlx4/cmd.c
@@ -1253,12 +1253,12 @@ static struct mlx4_cmd_info cmd_info[] = {
 	},
 	{
 		.opcode = MLX4_CMD_UPDATE_QP,
-		.has_inbox = false,
+		.has_inbox = true,
 		.has_outbox = false,
 		.out_is_imm = false,
 		.encode_slave_id = false,
 		.verify = NULL,
-		.wrapper = mlx4_CMD_EPERM_wrapper
+		.wrapper = mlx4_UPDATE_QP_wrapper
 	},
 	{
 		.opcode = MLX4_CMD_GET_OP_REQ,
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_cq.c b/drivers/net/ethernet/mellanox/mlx4/en_cq.c
index 70e95324a97..c2cd8d31bca 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_cq.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_cq.c
@@ -66,7 +66,6 @@ int mlx4_en_create_cq(struct mlx4_en_priv *priv,
 
 	cq->ring = ring;
 	cq->is_tx = mode;
-	spin_lock_init(&cq->lock);
 
 	/* Allocate HW buffers on provided NUMA node.
 	 * dev->numa_node is used in mtt range allocation flow.
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_netdev.c b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
index f085c2df5e6..7e4b1720c3d 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
@@ -1304,15 +1304,11 @@ static void mlx4_en_netpoll(struct net_device *dev)
 {
 	struct mlx4_en_priv *priv = netdev_priv(dev);
 	struct mlx4_en_cq *cq;
-	unsigned long flags;
 	int i;
 
 	for (i = 0; i < priv->rx_ring_num; i++) {
 		cq = priv->rx_cq[i];
-		spin_lock_irqsave(&cq->lock, flags);
-		napi_synchronize(&cq->napi);
-		mlx4_en_process_rx_cq(dev, cq, 0);
-		spin_unlock_irqrestore(&cq->lock, flags);
+		napi_schedule(&cq->napi);
 	}
 }
 #endif
diff --git a/drivers/net/ethernet/mellanox/mlx4/main.c b/drivers/net/ethernet/mellanox/mlx4/main.c
index cef267e24f9..7cf9dadcb47 100644
--- a/drivers/net/ethernet/mellanox/mlx4/main.c
+++ b/drivers/net/ethernet/mellanox/mlx4/main.c
@@ -754,10 +754,10 @@ static void mlx4_request_modules(struct mlx4_dev *dev)
 			has_eth_port = true;
 	}
 
-	if (has_ib_port || (dev->caps.flags & MLX4_DEV_CAP_FLAG_IBOE))
-		request_module_nowait(IB_DRV_NAME);
 	if (has_eth_port)
 		request_module_nowait(EN_DRV_NAME);
+	if (has_ib_port || (dev->caps.flags & MLX4_DEV_CAP_FLAG_IBOE))
+		request_module_nowait(IB_DRV_NAME);
 }
 
 /*
@@ -2440,7 +2440,8 @@ slave_start:
 	 * No return code for this call, just warn the user in case of PCI
 	 * express device capabilities are under-satisfied by the bus.
 	 */
-	mlx4_check_pcie_caps(dev);
+	if (!mlx4_is_slave(dev))
+		mlx4_check_pcie_caps(dev);
 
 	/* In master functions, the communication channel must be initialized
 	 * after obtaining its address from fw */
diff --git a/drivers/net/ethernet/mellanox/mlx4/mlx4.h b/drivers/net/ethernet/mellanox/mlx4/mlx4.h
index f9c46510196..212cea440f9 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mlx4.h
+++ b/drivers/net/ethernet/mellanox/mlx4/mlx4.h
@@ -1195,6 +1195,12 @@ int mlx4_QP_ATTACH_wrapper(struct mlx4_dev *dev, int slave,
 			   struct mlx4_cmd_mailbox *outbox,
 			   struct mlx4_cmd_info *cmd);
 
+int mlx4_UPDATE_QP_wrapper(struct mlx4_dev *dev, int slave,
+			   struct mlx4_vhcr *vhcr,
+			   struct mlx4_cmd_mailbox *inbox,
+			   struct mlx4_cmd_mailbox *outbox,
+			   struct mlx4_cmd_info *cmd);
+
 int mlx4_PROMISC_wrapper(struct mlx4_dev *dev, int slave,
 			 struct mlx4_vhcr *vhcr,
 			 struct mlx4_cmd_mailbox *inbox,
diff --git a/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h b/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
index 7a733c28774..04d9b6fe3e8 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
+++ b/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
@@ -319,7 +319,6 @@ struct mlx4_en_cq {
 	struct mlx4_cq          mcq;
 	struct mlx4_hwq_resources wqres;
 	int                     ring;
-	spinlock_t              lock;
 	struct net_device      *dev;
 	struct napi_struct	napi;
 	int size;
diff --git a/drivers/net/ethernet/mellanox/mlx4/port.c b/drivers/net/ethernet/mellanox/mlx4/port.c
index cfcad26ed40..b5b3549b0c8 100644
--- a/drivers/net/ethernet/mellanox/mlx4/port.c
+++ b/drivers/net/ethernet/mellanox/mlx4/port.c
@@ -1106,6 +1106,9 @@ int mlx4_get_slave_from_roce_gid(struct mlx4_dev *dev, int port, u8 *gid,
 	}
 
 	if (found_ix >= 0) {
+		/* Calculate a slave_gid which is the slave number in the gid
+		 * table and not a globally unique slave number.
+		 */
 		if (found_ix < MLX4_ROCE_PF_GIDS)
 			slave_gid = 0;
 		else if (found_ix < MLX4_ROCE_PF_GIDS + (vf_gids % num_vfs) *
@@ -1118,41 +1121,43 @@ int mlx4_get_slave_from_roce_gid(struct mlx4_dev *dev, int port, u8 *gid,
 			  ((vf_gids % num_vfs) * ((vf_gids / num_vfs + 1)))) /
 			 (vf_gids / num_vfs)) + vf_gids % num_vfs + 1;
 
+		/* Calculate the globally unique slave id */
 		if (slave_gid) {
 			struct mlx4_active_ports exclusive_ports;
 			struct mlx4_active_ports actv_ports;
 			struct mlx4_slaves_pport slaves_pport_actv;
 			unsigned max_port_p_one;
-			int num_slaves_before = 1;
+			int num_vfs_before = 0;
+			int candidate_slave_gid;
 
+			/* Calculate how many VFs are on the previous port, if exists */
 			for (i = 1; i < port; i++) {
 				bitmap_zero(exclusive_ports.ports, dev->caps.num_ports);
-				set_bit(i, exclusive_ports.ports);
+				set_bit(i - 1, exclusive_ports.ports);
 				slaves_pport_actv =
 					mlx4_phys_to_slaves_pport_actv(
 							dev, &exclusive_ports);
-				num_slaves_before += bitmap_weight(
+				num_vfs_before += bitmap_weight(
 						slaves_pport_actv.slaves,
 						dev->num_vfs + 1);
 			}
 
-			if (slave_gid < num_slaves_before) {
-				bitmap_zero(exclusive_ports.ports, dev->caps.num_ports);
-				set_bit(port - 1, exclusive_ports.ports);
-				slaves_pport_actv =
-					mlx4_phys_to_slaves_pport_actv(
-							dev, &exclusive_ports);
-				slave_gid += bitmap_weight(
-						slaves_pport_actv.slaves,
-						dev->num_vfs + 1) -
-						num_slaves_before;
-			}
-			actv_ports = mlx4_get_active_ports(dev, slave_gid);
+			/* candidate_slave_gid isn't necessarily the correct slave, but
+			 * it has the same number of ports and is assigned to the same
+			 * ports as the real slave we're looking for. On dual port VF,
+			 * slave_gid = [single port VFs on port <port>] +
+			 * [offset of the current slave from the first dual port VF] +
+			 * 1 (for the PF).
+			 */
+			candidate_slave_gid = slave_gid + num_vfs_before;
+
+			actv_ports = mlx4_get_active_ports(dev, candidate_slave_gid);
 			max_port_p_one = find_first_bit(
 				actv_ports.ports, dev->caps.num_ports) +
 				bitmap_weight(actv_ports.ports,
 					      dev->caps.num_ports) + 1;
 
+			/* Calculate the real slave number */
 			for (i = 1; i < max_port_p_one; i++) {
 				if (i == port)
 					continue;
diff --git a/drivers/net/ethernet/mellanox/mlx4/qp.c b/drivers/net/ethernet/mellanox/mlx4/qp.c
index 61d64ebffd5..fbd32af89c7 100644
--- a/drivers/net/ethernet/mellanox/mlx4/qp.c
+++ b/drivers/net/ethernet/mellanox/mlx4/qp.c
@@ -389,6 +389,41 @@ err_icm:
 
 EXPORT_SYMBOL_GPL(mlx4_qp_alloc);
 
+#define MLX4_UPDATE_QP_SUPPORTED_ATTRS MLX4_UPDATE_QP_SMAC
+int mlx4_update_qp(struct mlx4_dev *dev, struct mlx4_qp *qp,
+		   enum mlx4_update_qp_attr attr,
+		   struct mlx4_update_qp_params *params)
+{
+	struct mlx4_cmd_mailbox *mailbox;
+	struct mlx4_update_qp_context *cmd;
+	u64 pri_addr_path_mask = 0;
+	int err = 0;
+
+	mailbox = mlx4_alloc_cmd_mailbox(dev);
+	if (IS_ERR(mailbox))
+		return PTR_ERR(mailbox);
+
+	cmd = (struct mlx4_update_qp_context *)mailbox->buf;
+
+	if (!attr || (attr & ~MLX4_UPDATE_QP_SUPPORTED_ATTRS))
+		return -EINVAL;
+
+	if (attr & MLX4_UPDATE_QP_SMAC) {
+		pri_addr_path_mask |= 1ULL << MLX4_UPD_QP_PATH_MASK_MAC_INDEX;
+		cmd->qp_context.pri_path.grh_mylmc = params->smac_index;
+	}
+
+	cmd->primary_addr_path_mask = cpu_to_be64(pri_addr_path_mask);
+
+	err = mlx4_cmd(dev, mailbox->dma, qp->qpn & 0xffffff, 0,
+		       MLX4_CMD_UPDATE_QP, MLX4_CMD_TIME_CLASS_A,
+		       MLX4_CMD_NATIVE);
+
+	mlx4_free_cmd_mailbox(dev, mailbox);
+	return err;
+}
+EXPORT_SYMBOL_GPL(mlx4_update_qp);
+
 void mlx4_qp_remove(struct mlx4_dev *dev, struct mlx4_qp *qp)
 {
 	struct mlx4_qp_table *qp_table = &mlx4_priv(dev)->qp_table;
diff --git a/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c b/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
index 3b5f53ef29b..8f1254a7983 100644
--- a/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
+++ b/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
@@ -3733,6 +3733,25 @@ static int qp_detach(struct mlx4_dev *dev, struct mlx4_qp *qp,
 	}
 }
 
+static int mlx4_adjust_port(struct mlx4_dev *dev, int slave,
+			    u8 *gid, enum mlx4_protocol prot)
+{
+	int real_port;
+
+	if (prot != MLX4_PROT_ETH)
+		return 0;
+
+	if (dev->caps.steering_mode == MLX4_STEERING_MODE_B0 ||
+	    dev->caps.steering_mode == MLX4_STEERING_MODE_DEVICE_MANAGED) {
+		real_port = mlx4_slave_convert_port(dev, slave, gid[5]);
+		if (real_port < 0)
+			return -EINVAL;
+		gid[5] = real_port;
+	}
+
+	return 0;
+}
+
 int mlx4_QP_ATTACH_wrapper(struct mlx4_dev *dev, int slave,
 			       struct mlx4_vhcr *vhcr,
 			       struct mlx4_cmd_mailbox *inbox,
@@ -3768,6 +3787,10 @@ int mlx4_QP_ATTACH_wrapper(struct mlx4_dev *dev, int slave,
 		if (err)
 			goto ex_detach;
 	} else {
+		err = mlx4_adjust_port(dev, slave, gid, prot);
+		if (err)
+			goto ex_put;
+
 		err = rem_mcg_res(dev, slave, rqp, gid, prot, type, &reg_id);
 		if (err)
 			goto ex_put;
@@ -3872,6 +3895,60 @@ static int add_eth_header(struct mlx4_dev *dev, int slave,
 
 }
 
+#define MLX4_UPD_QP_PATH_MASK_SUPPORTED (1ULL << MLX4_UPD_QP_PATH_MASK_MAC_INDEX)
+int mlx4_UPDATE_QP_wrapper(struct mlx4_dev *dev, int slave,
+			   struct mlx4_vhcr *vhcr,
+			   struct mlx4_cmd_mailbox *inbox,
+			   struct mlx4_cmd_mailbox *outbox,
+			   struct mlx4_cmd_info *cmd_info)
+{
+	int err;
+	u32 qpn = vhcr->in_modifier & 0xffffff;
+	struct res_qp *rqp;
+	u64 mac;
+	unsigned port;
+	u64 pri_addr_path_mask;
+	struct mlx4_update_qp_context *cmd;
+	int smac_index;
+
+	cmd = (struct mlx4_update_qp_context *)inbox->buf;
+
+	pri_addr_path_mask = be64_to_cpu(cmd->primary_addr_path_mask);
+	if (cmd->qp_mask || cmd->secondary_addr_path_mask ||
+	    (pri_addr_path_mask & ~MLX4_UPD_QP_PATH_MASK_SUPPORTED))
+		return -EPERM;
+
+	/* Just change the smac for the QP */
+	err = get_res(dev, slave, qpn, RES_QP, &rqp);
+	if (err) {
+		mlx4_err(dev, "Updating qpn 0x%x for slave %d rejected\n", qpn, slave);
+		return err;
+	}
+
+	port = (rqp->sched_queue >> 6 & 1) + 1;
+	smac_index = cmd->qp_context.pri_path.grh_mylmc;
+	err = mac_find_smac_ix_in_slave(dev, slave, port,
+					smac_index, &mac);
+	if (err) {
+		mlx4_err(dev, "Failed to update qpn 0x%x, MAC is invalid. smac_ix: %d\n",
+			 qpn, smac_index);
+		goto err_mac;
+	}
+
+	err = mlx4_cmd(dev, inbox->dma,
+		       vhcr->in_modifier, 0,
+		       MLX4_CMD_UPDATE_QP, MLX4_CMD_TIME_CLASS_A,
+		       MLX4_CMD_NATIVE);
+	if (err) {
+		mlx4_err(dev, "Failed to update qpn on qpn 0x%x, command failed\n", qpn);
+		goto err_mac;
+	}
+
+err_mac:
+	put_res(dev, slave, qpn, RES_QP);
+	return err;
+}
+
 int mlx4_QP_FLOW_STEERING_ATTACH_wrapper(struct mlx4_dev *dev, int slave,
 					 struct mlx4_vhcr *vhcr,
 					 struct mlx4_cmd_mailbox *inbox,
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h b/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h
index 7b52a88923e..f785d01c7d1 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h
@@ -1719,22 +1719,6 @@ static inline u32 qlcnic_tx_avail(struct qlcnic_host_tx_ring *tx_ring)
 				tx_ring->producer;
 }
 
-static inline int qlcnic_set_real_num_queues(struct qlcnic_adapter *adapter,
-					     struct net_device *netdev)
-{
-	int err;
-
-	netdev->num_tx_queues = adapter->drv_tx_rings;
-	netdev->real_num_tx_queues = adapter->drv_tx_rings;
-
-	err = netif_set_real_num_tx_queues(netdev, adapter->drv_tx_rings);
-	if (err)
-		netdev_err(netdev, "failed to set %d Tx queues\n",
-			   adapter->drv_tx_rings);
-
-	return err;
-}
-
 struct qlcnic_nic_template {
 	int (*config_bridged_mode) (struct qlcnic_adapter *, u32);
 	int (*config_led) (struct qlcnic_adapter *, u32, u32);
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
index dbf75393f75..7e55e88a81b 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
@@ -2206,6 +2206,31 @@ static void qlcnic_82xx_set_mac_filter_count(struct qlcnic_adapter *adapter)
 	ahw->max_uc_count = count;
 }
 
+static int qlcnic_set_real_num_queues(struct qlcnic_adapter *adapter,
+				      u8 tx_queues, u8 rx_queues)
+{
+	struct net_device *netdev = adapter->netdev;
+	int err = 0;
+
+	if (tx_queues) {
+		err = netif_set_real_num_tx_queues(netdev, tx_queues);
+		if (err) {
+			netdev_err(netdev, "failed to set %d Tx queues\n",
+				   tx_queues);
+			return err;
+		}
+	}
+
+	if (rx_queues) {
+		err = netif_set_real_num_rx_queues(netdev, rx_queues);
+		if (err)
+			netdev_err(netdev, "failed to set %d Rx queues\n",
+				   rx_queues);
+	}
+
+	return err;
+}
+
 int
 qlcnic_setup_netdev(struct qlcnic_adapter *adapter, struct net_device *netdev,
 		    int pci_using_dac)
@@ -2269,7 +2294,8 @@ qlcnic_setup_netdev(struct qlcnic_adapter *adapter, struct net_device *netdev,
 	netdev->priv_flags |= IFF_UNICAST_FLT;
 	netdev->irq = adapter->msix_entries[0].vector;
 
-	err = qlcnic_set_real_num_queues(adapter, netdev);
+	err = qlcnic_set_real_num_queues(adapter, adapter->drv_tx_rings,
+					 adapter->drv_sds_rings);
 	if (err)
 		return err;
 
@@ -2374,6 +2400,14 @@ void qlcnic_set_drv_version(struct qlcnic_adapter *adapter)
 		qlcnic_fw_cmd_set_drv_version(adapter, fw_cmd);
 }
 
+/* Reset firmware API lock */
+static void qlcnic_reset_api_lock(struct qlcnic_adapter *adapter)
+{
+	qlcnic_api_lock(adapter);
+	qlcnic_api_unlock(adapter);
+}
+
+
 static int
 qlcnic_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
@@ -2476,6 +2510,7 @@ qlcnic_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	if (qlcnic_82xx_check(adapter)) {
 		qlcnic_check_vf(adapter, ent);
 		adapter->portnum = adapter->ahw->pci_func;
+		qlcnic_reset_api_lock(adapter);
 		err = qlcnic_start_firmware(adapter);
 		if (err) {
 			dev_err(&pdev->dev, "Loading fw failed.Please Reboot\n"
@@ -2934,9 +2969,13 @@ static void qlcnic_dump_tx_rings(struct qlcnic_adapter *adapter)
 			    tx_ring->tx_stats.xmit_called,
 			    tx_ring->tx_stats.xmit_on,
 			    tx_ring->tx_stats.xmit_off);
+
+		if (tx_ring->crb_intr_mask)
+			netdev_info(netdev, "crb_intr_mask=%d\n",
+				    readl(tx_ring->crb_intr_mask));
+
 		netdev_info(netdev,
-			    "crb_intr_mask=%d, hw_producer=%d, sw_producer=%d sw_consumer=%d, hw_consumer=%d\n",
-			    readl(tx_ring->crb_intr_mask),
+			    "hw_producer=%d, sw_producer=%d sw_consumer=%d, hw_consumer=%d\n",
 			    readl(tx_ring->crb_cmd_producer),
 			    tx_ring->producer, tx_ring->sw_consumer,
 			    le32_to_cpu(*(tx_ring->hw_consumer)));
@@ -3969,12 +4008,21 @@ int qlcnic_validate_rings(struct qlcnic_adapter *adapter, __u32 ring_cnt,
 int qlcnic_setup_rings(struct qlcnic_adapter *adapter)
 {
 	struct net_device *netdev = adapter->netdev;
+	u8 tx_rings, rx_rings;
 	int err;
 
 	if (test_bit(__QLCNIC_RESETTING, &adapter->state))
 		return -EBUSY;
 
+	tx_rings = adapter->drv_tss_rings;
+	rx_rings = adapter->drv_rss_rings;
+
 	netif_device_detach(netdev);
+
+	err = qlcnic_set_real_num_queues(adapter, tx_rings, rx_rings);
+	if (err)
+		goto done;
+
 	if (netif_running(netdev))
 		__qlcnic_down(adapter, netdev);
 
@@ -3994,7 +4042,17 @@ int qlcnic_setup_rings(struct qlcnic_adapter *adapter)
 		return err;
 	}
 
-	netif_set_real_num_tx_queues(netdev, adapter->drv_tx_rings);
+	/* Check if we need to update real_num_{tx|rx}_queues because
+	 * qlcnic_setup_intr() may change Tx/Rx rings size
+	 */
+	if ((tx_rings != adapter->drv_tx_rings) ||
+	    (rx_rings != adapter->drv_sds_rings)) {
+		err = qlcnic_set_real_num_queues(adapter,
+						 adapter->drv_tx_rings,
+						 adapter->drv_sds_rings);
+		if (err)
+			goto done;
+	}
 
 	if (qlcnic_83xx_check(adapter)) {
 		qlcnic_83xx_initialize_nic(adapter, 1);
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sriov_common.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sriov_common.c
index 0638c1810d5..6afe9c1f5ab 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sriov_common.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sriov_common.c
@@ -1370,7 +1370,7 @@ static int qlcnic_sriov_issue_cmd(struct qlcnic_adapter *adapter,
 
 	rsp = qlcnic_sriov_alloc_bc_trans(&trans);
 	if (rsp)
-		return rsp;
+		goto free_cmd;
 
 	rsp = qlcnic_sriov_prepare_bc_hdr(trans, cmd, seq, QLC_BC_COMMAND);
 	if (rsp)
@@ -1425,6 +1425,13 @@ err_out:
 
 cleanup_transaction:
 	qlcnic_sriov_cleanup_transaction(trans);
+
+free_cmd:
+	if (cmd->type == QLC_83XX_MBX_CMD_NO_WAIT) {
+		qlcnic_free_mbx_args(cmd);
+		kfree(cmd);
+	}
+
 	return rsp;
 }
 
diff --git a/drivers/net/ethernet/samsung/sxgbe/sxgbe_common.h b/drivers/net/ethernet/samsung/sxgbe/sxgbe_common.h
index 6203c7d8550..45019649bbb 100644
--- a/drivers/net/ethernet/samsung/sxgbe/sxgbe_common.h
+++ b/drivers/net/ethernet/samsung/sxgbe/sxgbe_common.h
@@ -358,6 +358,8 @@ struct sxgbe_core_ops {
 	/* Enable disable checksum offload operations */
 	void (*enable_rx_csum)(void __iomem *ioaddr);
 	void (*disable_rx_csum)(void __iomem *ioaddr);
+	void (*enable_rxqueue)(void __iomem *ioaddr, int queue_num);
+	void (*disable_rxqueue)(void __iomem *ioaddr, int queue_num);
 };
 
 const struct sxgbe_core_ops *sxgbe_get_core_ops(void);
diff --git a/drivers/net/ethernet/samsung/sxgbe/sxgbe_core.c b/drivers/net/ethernet/samsung/sxgbe/sxgbe_core.c
index c4da7a2b002..58c35692560 100644
--- a/drivers/net/ethernet/samsung/sxgbe/sxgbe_core.c
+++ b/drivers/net/ethernet/samsung/sxgbe/sxgbe_core.c
@@ -165,6 +165,26 @@ static void sxgbe_core_set_speed(void __iomem *ioaddr, unsigned char speed)
 	writel(tx_cfg, ioaddr + SXGBE_CORE_TX_CONFIG_REG);
 }
 
+static void sxgbe_core_enable_rxqueue(void __iomem *ioaddr, int queue_num)
+{
+	u32 reg_val;
+
+	reg_val = readl(ioaddr + SXGBE_CORE_RX_CTL0_REG);
+	reg_val &= ~(SXGBE_CORE_RXQ_ENABLE_MASK << queue_num);
+	reg_val |= SXGBE_CORE_RXQ_ENABLE;
+	writel(reg_val, ioaddr + SXGBE_CORE_RX_CTL0_REG);
+}
+
+static void sxgbe_core_disable_rxqueue(void __iomem *ioaddr, int queue_num)
+{
+	u32 reg_val;
+
+	reg_val = readl(ioaddr + SXGBE_CORE_RX_CTL0_REG);
+	reg_val &= ~(SXGBE_CORE_RXQ_ENABLE_MASK << queue_num);
+	reg_val |= SXGBE_CORE_RXQ_DISABLE;
+	writel(reg_val, ioaddr + SXGBE_CORE_RX_CTL0_REG);
+}
+
 static void  sxgbe_set_eee_mode(void __iomem *ioaddr)
 {
 	u32 ctrl;
@@ -254,6 +274,8 @@ static const struct sxgbe_core_ops core_ops = {
 	.set_eee_pls		= sxgbe_set_eee_pls,
 	.enable_rx_csum		= sxgbe_enable_rx_csum,
 	.disable_rx_csum	= sxgbe_disable_rx_csum,
+	.enable_rxqueue		= sxgbe_core_enable_rxqueue,
+	.disable_rxqueue	= sxgbe_core_disable_rxqueue,
 };
 
 const struct sxgbe_core_ops *sxgbe_get_core_ops(void)
diff --git a/drivers/net/ethernet/samsung/sxgbe/sxgbe_desc.c b/drivers/net/ethernet/samsung/sxgbe/sxgbe_desc.c
index e896dbbd2e1..2686bb5b676 100644
--- a/drivers/net/ethernet/samsung/sxgbe/sxgbe_desc.c
+++ b/drivers/net/ethernet/samsung/sxgbe/sxgbe_desc.c
@@ -45,10 +45,10 @@ static void sxgbe_prepare_tx_desc(struct sxgbe_tx_norm_desc *p, u8 is_fd,
 	p->tdes23.tx_rd_des23.first_desc = is_fd;
 	p->tdes23.tx_rd_des23.buf1_size = buf1_len;
 
-	p->tdes23.tx_rd_des23.tx_pkt_len.cksum_pktlen.total_pkt_len = pkt_len;
+	p->tdes23.tx_rd_des23.tx_pkt_len.pkt_len.total_pkt_len = pkt_len;
 
 	if (cksum)
-		p->tdes23.tx_rd_des23.tx_pkt_len.cksum_pktlen.cksum_ctl = cic_full;
+		p->tdes23.tx_rd_des23.cksum_ctl = cic_full;
 }
 
 /* Set VLAN control information */
@@ -233,6 +233,12 @@ static void sxgbe_set_rx_owner(struct sxgbe_rx_norm_desc *p)
 	p->rdes23.rx_rd_des23.own_bit = 1;
 }
 
+/* Set Interrupt on completion bit */
+static void sxgbe_set_rx_int_on_com(struct sxgbe_rx_norm_desc *p)
+{
+	p->rdes23.rx_rd_des23.int_on_com = 1;
+}
+
 /* Get the receive frame size */
 static int sxgbe_get_rx_frame_len(struct sxgbe_rx_norm_desc *p)
 {
@@ -498,6 +504,7 @@ static const struct sxgbe_desc_ops desc_ops = {
 	.init_rx_desc			= sxgbe_init_rx_desc,
 	.get_rx_owner			= sxgbe_get_rx_owner,
 	.set_rx_owner			= sxgbe_set_rx_owner,
+	.set_rx_int_on_com		= sxgbe_set_rx_int_on_com,
 	.get_rx_frame_len		= sxgbe_get_rx_frame_len,
 	.get_rx_fd_status		= sxgbe_get_rx_fd_status,
 	.get_rx_ld_status		= sxgbe_get_rx_ld_status,
diff --git a/drivers/net/ethernet/samsung/sxgbe/sxgbe_desc.h b/drivers/net/ethernet/samsung/sxgbe/sxgbe_desc.h
index 838cb9fb0ea..18609324db7 100644
--- a/drivers/net/ethernet/samsung/sxgbe/sxgbe_desc.h
+++ b/drivers/net/ethernet/samsung/sxgbe/sxgbe_desc.h
@@ -39,22 +39,22 @@ struct sxgbe_tx_norm_desc {
 			u32 int_on_com:1;
 			/* TDES3 */
 			union {
-				u32 tcp_payload_len:18;
+				u16 tcp_payload_len;
 				struct {
 					u32 total_pkt_len:15;
 					u32 reserved1:1;
-					u32 cksum_ctl:2;
-				} cksum_pktlen;
+				} pkt_len;
 			} tx_pkt_len;
 
-			u32 tse_bit:1;
-			u32 tcp_hdr_len:4;
-			u32 sa_insert_ctl:3;
-			u32 crc_pad_ctl:2;
-			u32 last_desc:1;
-			u32 first_desc:1;
-			u32 ctxt_bit:1;
-			u32 own_bit:1;
+			u16 cksum_ctl:2;
+			u16 tse_bit:1;
+			u16 tcp_hdr_len:4;
+			u16 sa_insert_ctl:3;
+			u16 crc_pad_ctl:2;
+			u16 last_desc:1;
+			u16 first_desc:1;
+			u16 ctxt_bit:1;
+			u16 own_bit:1;
 		} tx_rd_des23;
 
 		/* tx write back Desc 2,3 */
@@ -70,25 +70,20 @@ struct sxgbe_tx_norm_desc {
 
 struct sxgbe_rx_norm_desc {
 	union {
-		u32 rdes0; /* buf1 address */
-		struct {
+		u64 rdes01; /* buf1 address */
+		union {
 			u32 out_vlan_tag:16;
 			u32 in_vlan_tag:16;
-		} wb_rx_des0;
-	} rd_wb_des0;
-
-	union {
-		u32 rdes1;	/* buf2 address or buf1[63:32] */
-		u32 rss_hash;	/* Write-back RX */
-	} rd_wb_des1;
+			u32 rss_hash;
+		} rx_wb_des01;
+	} rdes01;
 
 	union {
 		/* RX Read format Desc 2,3 */
 		struct{
 			/* RDES2 */
-			u32 buf2_addr;
+			u64 buf2_addr:62;
 			/* RDES3 */
-			u32 buf2_hi_addr:30;
 			u32 int_on_com:1;
 			u32 own_bit:1;
 		} rx_rd_des23;
@@ -263,6 +258,9 @@ struct sxgbe_desc_ops {
 	/* Set own bit */
 	void (*set_rx_owner)(struct sxgbe_rx_norm_desc *p);
 
+	/* Set Interrupt on completion bit */
+	void (*set_rx_int_on_com)(struct sxgbe_rx_norm_desc *p);
+
 	/* Get the receive frame size */
 	int (*get_rx_frame_len)(struct sxgbe_rx_norm_desc *p);
 
diff --git a/drivers/net/ethernet/samsung/sxgbe/sxgbe_dma.c b/drivers/net/ethernet/samsung/sxgbe/sxgbe_dma.c
index 4d989ff6c97..bb9b5b8afc5 100644
--- a/drivers/net/ethernet/samsung/sxgbe/sxgbe_dma.c
+++ b/drivers/net/ethernet/samsung/sxgbe/sxgbe_dma.c
@@ -23,21 +23,8 @@
 /* DMA core initialization */
 static int sxgbe_dma_init(void __iomem *ioaddr, int fix_burst, int burst_map)
 {
-	int retry_count = 10;
 	u32 reg_val;
 
-	/* reset the DMA */
-	writel(SXGBE_DMA_SOFT_RESET, ioaddr + SXGBE_DMA_MODE_REG);
-	while (retry_count--) {
-		if (!(readl(ioaddr + SXGBE_DMA_MODE_REG) &
-		      SXGBE_DMA_SOFT_RESET))
-			break;
-		mdelay(10);
-	}
-
-	if (retry_count < 0)
-		return -EBUSY;
-
 	reg_val = readl(ioaddr + SXGBE_DMA_SYSBUS_MODE_REG);
 
 	/* if fix_burst = 0, Set UNDEF = 1 of DMA_Sys_Mode Register.
diff --git a/drivers/net/ethernet/samsung/sxgbe/sxgbe_main.c b/drivers/net/ethernet/samsung/sxgbe/sxgbe_main.c
index 27e8c824b20..82a9a983869 100644
--- a/drivers/net/ethernet/samsung/sxgbe/sxgbe_main.c
+++ b/drivers/net/ethernet/samsung/sxgbe/sxgbe_main.c
@@ -1076,6 +1076,9 @@ static int sxgbe_open(struct net_device *dev)
 
 	/* Initialize the MAC Core */
 	priv->hw->mac->core_init(priv->ioaddr);
+	SXGBE_FOR_EACH_QUEUE(SXGBE_RX_QUEUES, queue_num) {
+		priv->hw->mac->enable_rxqueue(priv->ioaddr, queue_num);
+	}
 
 	/* Request the IRQ lines */
 	ret = devm_request_irq(priv->device, priv->irq, sxgbe_common_interrupt,
@@ -1453,6 +1456,7 @@ static void sxgbe_rx_refill(struct sxgbe_priv_data *priv)
 		/* Added memory barrier for RX descriptor modification */
 		wmb();
 		priv->hw->desc->set_rx_owner(p);
+		priv->hw->desc->set_rx_int_on_com(p);
 		/* Added memory barrier for RX descriptor modification */
 		wmb();
 	}
@@ -2070,6 +2074,24 @@ static int sxgbe_hw_init(struct sxgbe_priv_data * const priv)
 	return 0;
 }
 
+static int sxgbe_sw_reset(void __iomem *addr)
+{
+	int retry_count = 10;
+
+	writel(SXGBE_DMA_SOFT_RESET, addr + SXGBE_DMA_MODE_REG);
+	while (retry_count--) {
+		if (!(readl(addr + SXGBE_DMA_MODE_REG) &
+		      SXGBE_DMA_SOFT_RESET))
+			break;
+		mdelay(10);
+	}
+
+	if (retry_count < 0)
+		return -EBUSY;
+
+	return 0;
+}
+
 /**
  * sxgbe_drv_probe
  * @device: device pointer
@@ -2102,6 +2124,10 @@ struct sxgbe_priv_data *sxgbe_drv_probe(struct device *device,
 	priv->plat = plat_dat;
 	priv->ioaddr = addr;
 
+	ret = sxgbe_sw_reset(priv->ioaddr);
+	if (ret)
+		goto error_free_netdev;
+
 	/* Verify driver arguments */
 	sxgbe_verify_args();
 
@@ -2218,9 +2244,14 @@ error_free_netdev:
 int sxgbe_drv_remove(struct net_device *ndev)
 {
 	struct sxgbe_priv_data *priv = netdev_priv(ndev);
+	u8 queue_num;
 
 	netdev_info(ndev, "%s: removing driver\n", __func__);
 
+	SXGBE_FOR_EACH_QUEUE(SXGBE_RX_QUEUES, queue_num) {
+		priv->hw->mac->disable_rxqueue(priv->ioaddr, queue_num);
+	}
+
 	priv->hw->dma->stop_rx(priv->ioaddr, SXGBE_RX_QUEUES);
 	priv->hw->dma->stop_tx(priv->ioaddr, SXGBE_TX_QUEUES);
 
diff --git a/drivers/net/ethernet/samsung/sxgbe/sxgbe_mdio.c b/drivers/net/ethernet/samsung/sxgbe/sxgbe_mdio.c
index 01af2cbb479..43ccb4a6de1 100644
--- a/drivers/net/ethernet/samsung/sxgbe/sxgbe_mdio.c
+++ b/drivers/net/ethernet/samsung/sxgbe/sxgbe_mdio.c
@@ -27,7 +27,7 @@
 #define SXGBE_SMA_PREAD_CMD	0x02 /* post read  increament address */
 #define SXGBE_SMA_READ_CMD	0x03 /* read command */
 #define SXGBE_SMA_SKIP_ADDRFRM	0x00040000 /* skip the address frame */
-#define SXGBE_MII_BUSY		0x00800000 /* mii busy */
+#define SXGBE_MII_BUSY		0x00400000 /* mii busy */
 
 static int sxgbe_mdio_busy_wait(void __iomem *ioaddr, unsigned int mii_data)
 {
@@ -147,6 +147,7 @@ int sxgbe_mdio_register(struct net_device *ndev)
 	struct sxgbe_mdio_bus_data *mdio_data = priv->plat->mdio_bus_data;
 	int err, phy_addr;
 	int *irqlist;
+	bool phy_found = false;
 	bool act;
 
 	/* allocate the new mdio bus */
@@ -162,7 +163,7 @@ int sxgbe_mdio_register(struct net_device *ndev)
 		irqlist = priv->mii_irq;
 
 	/* assign mii bus fields */
-	mdio_bus->name = "samsxgbe";
+	mdio_bus->name = "sxgbe";
 	mdio_bus->read = &sxgbe_mdio_read;
 	mdio_bus->write = &sxgbe_mdio_write;
 	snprintf(mdio_bus->id, MII_BUS_ID_SIZE, "%s-%x",
@@ -216,13 +217,22 @@ int sxgbe_mdio_register(struct net_device *ndev)
 			netdev_info(ndev, "PHY ID %08x at %d IRQ %s (%s)%s\n",
 				    phy->phy_id, phy_addr, irq_str,
 				    dev_name(&phy->dev), act ? " active" : "");
+			phy_found = true;
 		}
 	}
 
+	if (!phy_found) {
+		netdev_err(ndev, "PHY not found\n");
+		goto phyfound_err;
+	}
+
 	priv->mii = mdio_bus;
 
 	return 0;
 
+phyfound_err:
+	err = -ENODEV;
+	mdiobus_unregister(mdio_bus);
 mdiobus_err:
 	mdiobus_free(mdio_bus);
 	return err;
diff --git a/drivers/net/ethernet/samsung/sxgbe/sxgbe_reg.h b/drivers/net/ethernet/samsung/sxgbe/sxgbe_reg.h
index 5a89acb4c50..56f8bf5a3f1 100644
--- a/drivers/net/ethernet/samsung/sxgbe/sxgbe_reg.h
+++ b/drivers/net/ethernet/samsung/sxgbe/sxgbe_reg.h
@@ -52,6 +52,10 @@
 #define SXGBE_CORE_RX_CTL2_REG		0x00A8
 #define SXGBE_CORE_RX_CTL3_REG		0x00AC
 
+#define SXGBE_CORE_RXQ_ENABLE_MASK	0x0003
+#define SXGBE_CORE_RXQ_ENABLE		0x0002
+#define SXGBE_CORE_RXQ_DISABLE		0x0000
+
 /* Interrupt Registers */
 #define SXGBE_CORE_INT_STATUS_REG	0x00B0
 #define SXGBE_CORE_INT_ENABLE_REG	0x00B4
diff --git a/drivers/net/ethernet/sfc/ef10.c b/drivers/net/ethernet/sfc/ef10.c
index 21c20ea0dad..b5ed30a3914 100644
--- a/drivers/net/ethernet/sfc/ef10.c
+++ b/drivers/net/ethernet/sfc/ef10.c
@@ -738,8 +738,11 @@ static int efx_ef10_reset(struct efx_nic *efx, enum reset_type reset_type)
 	/* If it was a port reset, trigger reallocation of MC resources.
 	 * Note that on an MC reset nothing needs to be done now because we'll
 	 * detect the MC reset later and handle it then.
+	 * For an FLR, we never get an MC reset event, but the MC has reset all
+	 * resources assigned to us, so we have to trigger reallocation now.
 	 */
-	if (reset_type == RESET_TYPE_ALL && !rc)
+	if ((reset_type == RESET_TYPE_ALL ||
+	     reset_type == RESET_TYPE_MCDI_TIMEOUT) && !rc)
 		efx_ef10_reset_mc_allocations(efx);
 	return rc;
 }
@@ -2141,6 +2144,11 @@ static int efx_ef10_fini_dmaq(struct efx_nic *efx)
 	return 0;
 }
 
+static void efx_ef10_prepare_flr(struct efx_nic *efx)
+{
+	atomic_set(&efx->active_queues, 0);
+}
+
 static bool efx_ef10_filter_equal(const struct efx_filter_spec *left,
 				  const struct efx_filter_spec *right)
 {
@@ -3603,6 +3611,8 @@ const struct efx_nic_type efx_hunt_a0_nic_type = {
 	.probe_port = efx_mcdi_port_probe,
 	.remove_port = efx_mcdi_port_remove,
 	.fini_dmaq = efx_ef10_fini_dmaq,
+	.prepare_flr = efx_ef10_prepare_flr,
+	.finish_flr = efx_port_dummy_op_void,
 	.describe_stats = efx_ef10_describe_stats,
 	.update_stats = efx_ef10_update_stats,
 	.start_stats = efx_mcdi_mac_start_stats,
diff --git a/drivers/net/ethernet/sfc/efx.c b/drivers/net/ethernet/sfc/efx.c
index 57b971e5e6b..63d595fd3cc 100644
--- a/drivers/net/ethernet/sfc/efx.c
+++ b/drivers/net/ethernet/sfc/efx.c
@@ -76,6 +76,7 @@ const char *const efx_reset_type_names[] = {
 	[RESET_TYPE_RECOVER_OR_ALL]     = "RECOVER_OR_ALL",
 	[RESET_TYPE_WORLD]              = "WORLD",
 	[RESET_TYPE_RECOVER_OR_DISABLE] = "RECOVER_OR_DISABLE",
+	[RESET_TYPE_MC_BIST]		= "MC_BIST",
 	[RESET_TYPE_DISABLE]            = "DISABLE",
 	[RESET_TYPE_TX_WATCHDOG]        = "TX_WATCHDOG",
 	[RESET_TYPE_INT_ERROR]          = "INT_ERROR",
@@ -83,7 +84,7 @@ const char *const efx_reset_type_names[] = {
 	[RESET_TYPE_DMA_ERROR]          = "DMA_ERROR",
 	[RESET_TYPE_TX_SKIP]            = "TX_SKIP",
 	[RESET_TYPE_MC_FAILURE]         = "MC_FAILURE",
-	[RESET_TYPE_MC_BIST]		= "MC_BIST",
+	[RESET_TYPE_MCDI_TIMEOUT]	= "MCDI_TIMEOUT (FLR)",
 };
 
 /* Reset workqueue. If any NIC has a hardware failure then a reset will be
@@ -1739,7 +1740,8 @@ static void efx_start_all(struct efx_nic *efx)
 
 	/* Check that it is appropriate to restart the interface. All
 	 * of these flags are safe to read under just the rtnl lock */
-	if (efx->port_enabled || !netif_running(efx->net_dev))
+	if (efx->port_enabled || !netif_running(efx->net_dev) ||
+	    efx->reset_pending)
 		return;
 
 	efx_start_port(efx);
@@ -2334,6 +2336,9 @@ void efx_reset_down(struct efx_nic *efx, enum reset_type method)
 {
 	EFX_ASSERT_RESET_SERIALISED(efx);
 
+	if (method == RESET_TYPE_MCDI_TIMEOUT)
+		efx->type->prepare_flr(efx);
+
 	efx_stop_all(efx);
 	efx_disable_interrupts(efx);
 
@@ -2354,6 +2359,10 @@ int efx_reset_up(struct efx_nic *efx, enum reset_type method, bool ok)
 
 	EFX_ASSERT_RESET_SERIALISED(efx);
 
+	if (method == RESET_TYPE_MCDI_TIMEOUT)
+		efx->type->finish_flr(efx);
+
+	/* Ensure that SRAM is initialised even if we're disabling the device */
 	rc = efx->type->init(efx);
 	if (rc) {
 		netif_err(efx, drv, efx->net_dev, "failed to initialise NIC\n");
@@ -2417,7 +2426,10 @@ int efx_reset(struct efx_nic *efx, enum reset_type method)
 	/* Clear flags for the scopes we covered.  We assume the NIC and
 	 * driver are now quiescent so that there is no race here.
 	 */
-	efx->reset_pending &= -(1 << (method + 1));
+	if (method < RESET_TYPE_MAX_METHOD)
+		efx->reset_pending &= -(1 << (method + 1));
+	else /* it doesn't fit into the well-ordered scope hierarchy */
+		__clear_bit(method, &efx->reset_pending);
 
 	/* Reinitialise bus-mastering, which may have been turned off before
 	 * the reset was scheduled. This is still appropriate, even in the
@@ -2546,6 +2558,7 @@ void efx_schedule_reset(struct efx_nic *efx, enum reset_type type)
 	case RESET_TYPE_DISABLE:
 	case RESET_TYPE_RECOVER_OR_DISABLE:
 	case RESET_TYPE_MC_BIST:
+	case RESET_TYPE_MCDI_TIMEOUT:
 		method = type;
 		netif_dbg(efx, drv, efx->net_dev, "scheduling %s reset\n",
 			  RESET_TYPE(method));
diff --git a/drivers/net/ethernet/sfc/enum.h b/drivers/net/ethernet/sfc/enum.h
index 75ef7ef6450..d1dbb5fb31b 100644
--- a/drivers/net/ethernet/sfc/enum.h
+++ b/drivers/net/ethernet/sfc/enum.h
@@ -143,6 +143,7 @@ enum efx_loopback_mode {
  * @RESET_TYPE_WORLD: Reset as much as possible
  * @RESET_TYPE_RECOVER_OR_DISABLE: Try to recover. Apply RESET_TYPE_DISABLE if
  * unsuccessful.
+ * @RESET_TYPE_MC_BIST: MC entering BIST mode.
  * @RESET_TYPE_DISABLE: Reset datapath, MAC and PHY; leave NIC disabled
  * @RESET_TYPE_TX_WATCHDOG: reset due to TX watchdog
  * @RESET_TYPE_INT_ERROR: reset due to internal error
@@ -150,14 +151,16 @@ enum efx_loopback_mode {
  * @RESET_TYPE_DMA_ERROR: DMA error
  * @RESET_TYPE_TX_SKIP: hardware completed empty tx descriptors
  * @RESET_TYPE_MC_FAILURE: MC reboot/assertion
+ * @RESET_TYPE_MCDI_TIMEOUT: MCDI timeout.
  */
 enum reset_type {
-	RESET_TYPE_INVISIBLE = 0,
-	RESET_TYPE_RECOVER_OR_ALL = 1,
-	RESET_TYPE_ALL = 2,
-	RESET_TYPE_WORLD = 3,
-	RESET_TYPE_RECOVER_OR_DISABLE = 4,
-	RESET_TYPE_DISABLE = 5,
+	RESET_TYPE_INVISIBLE,
+	RESET_TYPE_RECOVER_OR_ALL,
+	RESET_TYPE_ALL,
+	RESET_TYPE_WORLD,
+	RESET_TYPE_RECOVER_OR_DISABLE,
+	RESET_TYPE_MC_BIST,
+	RESET_TYPE_DISABLE,
 	RESET_TYPE_MAX_METHOD,
 	RESET_TYPE_TX_WATCHDOG,
 	RESET_TYPE_INT_ERROR,
@@ -165,7 +168,13 @@ enum reset_type {
 	RESET_TYPE_DMA_ERROR,
 	RESET_TYPE_TX_SKIP,
 	RESET_TYPE_MC_FAILURE,
-	RESET_TYPE_MC_BIST,
+	/* RESET_TYPE_MCDI_TIMEOUT is actually a method, not just a reason, but
+	 * it doesn't fit the scope hierarchy (not well-ordered by inclusion).
+	 * We encode this by having its enum value be greater than
+	 * RESET_TYPE_MAX_METHOD. This also prevents issuing it with
+	 * efx_ioctl_reset.
+	 */
+	RESET_TYPE_MCDI_TIMEOUT,
 	RESET_TYPE_MAX,
 };
 
diff --git a/drivers/net/ethernet/sfc/falcon.c b/drivers/net/ethernet/sfc/falcon.c
index 8ec20b713cc..fae25a41864 100644
--- a/drivers/net/ethernet/sfc/falcon.c
+++ b/drivers/net/ethernet/sfc/falcon.c
@@ -2696,6 +2696,8 @@ const struct efx_nic_type falcon_a1_nic_type = {
 	.fini_dmaq = efx_farch_fini_dmaq,
 	.prepare_flush = falcon_prepare_flush,
 	.finish_flush = efx_port_dummy_op_void,
+	.prepare_flr = efx_port_dummy_op_void,
+	.finish_flr = efx_farch_finish_flr,
 	.describe_stats = falcon_describe_nic_stats,
 	.update_stats = falcon_update_nic_stats,
 	.start_stats = falcon_start_nic_stats,
@@ -2790,6 +2792,8 @@ const struct efx_nic_type falcon_b0_nic_type = {
 	.fini_dmaq = efx_farch_fini_dmaq,
 	.prepare_flush = falcon_prepare_flush,
 	.finish_flush = efx_port_dummy_op_void,
+	.prepare_flr = efx_port_dummy_op_void,
+	.finish_flr = efx_farch_finish_flr,
 	.describe_stats = falcon_describe_nic_stats,
 	.update_stats = falcon_update_nic_stats,
 	.start_stats = falcon_start_nic_stats,
diff --git a/drivers/net/ethernet/sfc/farch.c b/drivers/net/ethernet/sfc/farch.c
index a08761360cd..0537381cd2f 100644
--- a/drivers/net/ethernet/sfc/farch.c
+++ b/drivers/net/ethernet/sfc/farch.c
@@ -741,6 +741,28 @@ int efx_farch_fini_dmaq(struct efx_nic *efx)
 	return rc;
 }
 
+/* Reset queue and flush accounting after FLR
+ *
+ * One possible cause of FLR recovery is that DMA may be failing (eg. if bus
+ * mastering was disabled), in which case we don't receive (RXQ) flush
+ * completion events.  This means that efx->rxq_flush_outstanding remained at 4
+ * after the FLR; also, efx->active_queues was non-zero (as no flush completion
+ * events were received, and we didn't go through efx_check_tx_flush_complete())
+ * If we don't fix this up, on the next call to efx_realloc_channels() we won't
+ * flush any RX queues because efx->rxq_flush_outstanding is at the limit of 4
+ * for batched flush requests; and the efx->active_queues gets messed up because
+ * we keep incrementing for the newly initialised queues, but it never went to
+ * zero previously.  Then we get a timeout every time we try to restart the
+ * queues, as it doesn't go back to zero when we should be flushing the queues.
+ */
+void efx_farch_finish_flr(struct efx_nic *efx)
+{
+	atomic_set(&efx->rxq_flush_pending, 0);
+	atomic_set(&efx->rxq_flush_outstanding, 0);
+	atomic_set(&efx->active_queues, 0);
+}
+
+
 /**************************************************************************
  *
  * Event queue processing
diff --git a/drivers/net/ethernet/sfc/mcdi.c b/drivers/net/ethernet/sfc/mcdi.c
index 7bd4b14bf3b..5239cf9bdc5 100644
--- a/drivers/net/ethernet/sfc/mcdi.c
+++ b/drivers/net/ethernet/sfc/mcdi.c
@@ -52,12 +52,7 @@ static void efx_mcdi_timeout_async(unsigned long context);
 static int efx_mcdi_drv_attach(struct efx_nic *efx, bool driver_operating,
 			       bool *was_attached_out);
 static bool efx_mcdi_poll_once(struct efx_nic *efx);
-
-static inline struct efx_mcdi_iface *efx_mcdi(struct efx_nic *efx)
-{
-	EFX_BUG_ON_PARANOID(!efx->mcdi);
-	return &efx->mcdi->iface;
-}
+static void efx_mcdi_abandon(struct efx_nic *efx);
 
 int efx_mcdi_init(struct efx_nic *efx)
 {
@@ -558,6 +553,8 @@ static int _efx_mcdi_rpc_finish(struct efx_nic *efx, unsigned cmd, size_t inlen,
 			rc = 0;
 		}
 
+		efx_mcdi_abandon(efx);
+
 		/* Close the race with efx_mcdi_ev_cpl() executing just too late
 		 * and completing a request we've just cancelled, by ensuring
 		 * that the seqno check therein fails.
@@ -672,6 +669,9 @@ int efx_mcdi_rpc_start(struct efx_nic *efx, unsigned cmd,
 	if (efx->mc_bist_for_other_fn)
 		return -ENETDOWN;
 
+	if (mcdi->mode == MCDI_MODE_FAIL)
+		return -ENETDOWN;
+
 	efx_mcdi_acquire_sync(mcdi);
 	efx_mcdi_send_request(efx, cmd, inbuf, inlen);
 	return 0;
@@ -812,7 +812,11 @@ void efx_mcdi_mode_poll(struct efx_nic *efx)
 		return;
 
 	mcdi = efx_mcdi(efx);
-	if (mcdi->mode == MCDI_MODE_POLL)
+	/* If already in polling mode, nothing to do.
+	 * If in fail-fast state, don't switch to polled completion.
+	 * FLR recovery will do that later.
+	 */
+	if (mcdi->mode == MCDI_MODE_POLL || mcdi->mode == MCDI_MODE_FAIL)
 		return;
 
 	/* We can switch from event completion to polled completion, because
@@ -841,8 +845,8 @@ void efx_mcdi_flush_async(struct efx_nic *efx)
 
 	mcdi = efx_mcdi(efx);
 
-	/* We must be in polling mode so no more requests can be queued */
-	BUG_ON(mcdi->mode != MCDI_MODE_POLL);
+	/* We must be in poll or fail mode so no more requests can be queued */
+	BUG_ON(mcdi->mode == MCDI_MODE_EVENTS);
 
 	del_timer_sync(&mcdi->async_timer);
 
@@ -875,8 +879,11 @@ void efx_mcdi_mode_event(struct efx_nic *efx)
 		return;
 
 	mcdi = efx_mcdi(efx);
-
-	if (mcdi->mode == MCDI_MODE_EVENTS)
+	/* If already in event completion mode, nothing to do.
+	 * If in fail-fast state, don't switch to event completion.  FLR
+	 * recovery will do that later.
+	 */
+	if (mcdi->mode == MCDI_MODE_EVENTS || mcdi->mode == MCDI_MODE_FAIL)
 		return;
 
 	/* We can't switch from polled to event completion in the middle of a
@@ -966,6 +973,19 @@ static void efx_mcdi_ev_bist(struct efx_nic *efx)
 	spin_unlock(&mcdi->iface_lock);
 }
 
+/* MCDI timeouts seen, so make all MCDI calls fail-fast and issue an FLR to try
+ * to recover.
+ */
+static void efx_mcdi_abandon(struct efx_nic *efx)
+{
+	struct efx_mcdi_iface *mcdi = efx_mcdi(efx);
+
+	if (xchg(&mcdi->mode, MCDI_MODE_FAIL) == MCDI_MODE_FAIL)
+		return; /* it had already been done */
+	netif_dbg(efx, hw, efx->net_dev, "MCDI is timing out; trying to recover\n");
+	efx_schedule_reset(efx, RESET_TYPE_MCDI_TIMEOUT);
+}
+
 /* Called from  falcon_process_eventq for MCDI events */
 void efx_mcdi_process_event(struct efx_channel *channel,
 			    efx_qword_t *event)
@@ -1512,6 +1532,19 @@ int efx_mcdi_reset(struct efx_nic *efx, enum reset_type method)
 {
 	int rc;
 
+	/* If MCDI is down, we can't handle_assertion */
+	if (method == RESET_TYPE_MCDI_TIMEOUT) {
+		rc = pci_reset_function(efx->pci_dev);
+		if (rc)
+			return rc;
+		/* Re-enable polled MCDI completion */
+		if (efx->mcdi) {
+			struct efx_mcdi_iface *mcdi = efx_mcdi(efx);
+			mcdi->mode = MCDI_MODE_POLL;
+		}
+		return 0;
+	}
+
 	/* Recover from a failed assertion pre-reset */
 	rc = efx_mcdi_handle_assertion(efx);
 	if (rc)
diff --git a/drivers/net/ethernet/sfc/mcdi.h b/drivers/net/ethernet/sfc/mcdi.h
index 52931aebf3c..56465f7465a 100644
--- a/drivers/net/ethernet/sfc/mcdi.h
+++ b/drivers/net/ethernet/sfc/mcdi.h
@@ -28,9 +28,16 @@ enum efx_mcdi_state {
 	MCDI_STATE_COMPLETED,
 };
 
+/**
+ * enum efx_mcdi_mode - MCDI transaction mode
+ * @MCDI_MODE_POLL: poll for MCDI completion, until timeout
+ * @MCDI_MODE_EVENTS: wait for an mcdi_event.  On timeout, poll once
+ * @MCDI_MODE_FAIL: we think MCDI is dead, so fail-fast all calls
+ */
 enum efx_mcdi_mode {
 	MCDI_MODE_POLL,
 	MCDI_MODE_EVENTS,
+	MCDI_MODE_FAIL,
 };
 
 /**
@@ -104,6 +111,12 @@ struct efx_mcdi_data {
 	u32 fn_flags;
 };
 
+static inline struct efx_mcdi_iface *efx_mcdi(struct efx_nic *efx)
+{
+	EFX_BUG_ON_PARANOID(!efx->mcdi);
+	return &efx->mcdi->iface;
+}
+
 #ifdef CONFIG_SFC_MCDI_MON
 static inline struct efx_mcdi_mon *efx_mcdi_mon(struct efx_nic *efx)
 {
diff --git a/drivers/net/ethernet/sfc/net_driver.h b/drivers/net/ethernet/sfc/net_driver.h
index 8a400a0595e..5bdae8ed7c5 100644
--- a/drivers/net/ethernet/sfc/net_driver.h
+++ b/drivers/net/ethernet/sfc/net_driver.h
@@ -972,6 +972,8 @@ struct efx_mtd_partition {
  *	(for Falcon architecture)
  * @finish_flush: Clean up after flushing the DMA queues (for Falcon
  *	architecture)
+ * @prepare_flr: Prepare for an FLR
+ * @finish_flr: Clean up after an FLR
  * @describe_stats: Describe statistics for ethtool
  * @update_stats: Update statistics not provided by event handling.
  *	Either argument may be %NULL.
@@ -1100,6 +1102,8 @@ struct efx_nic_type {
 	int (*fini_dmaq)(struct efx_nic *efx);
 	void (*prepare_flush)(struct efx_nic *efx);
 	void (*finish_flush)(struct efx_nic *efx);
+	void (*prepare_flr)(struct efx_nic *efx);
+	void (*finish_flr)(struct efx_nic *efx);
 	size_t (*describe_stats)(struct efx_nic *efx, u8 *names);
 	size_t (*update_stats)(struct efx_nic *efx, u64 *full_stats,
 			       struct rtnl_link_stats64 *core_stats);
diff --git a/drivers/net/ethernet/sfc/nic.c b/drivers/net/ethernet/sfc/nic.c
index 32d969e857f..89b83e59e1d 100644
--- a/drivers/net/ethernet/sfc/nic.c
+++ b/drivers/net/ethernet/sfc/nic.c
@@ -156,13 +156,15 @@ void efx_nic_fini_interrupt(struct efx_nic *efx)
 	efx->net_dev->rx_cpu_rmap = NULL;
 #endif
 
-	/* Disable MSI/MSI-X interrupts */
-	efx_for_each_channel(channel, efx)
-		free_irq(channel->irq, &efx->msi_context[channel->channel]);
-
-	/* Disable legacy interrupt */
-	if (efx->legacy_irq)
+	if (EFX_INT_MODE_USE_MSI(efx)) {
+		/* Disable MSI/MSI-X interrupts */
+		efx_for_each_channel(channel, efx)
+			free_irq(channel->irq,
+				 &efx->msi_context[channel->channel]);
+	} else {
+		/* Disable legacy interrupt */
 		free_irq(efx->legacy_irq, efx);
+	}
 }
 
 /* Register dump */
diff --git a/drivers/net/ethernet/sfc/nic.h b/drivers/net/ethernet/sfc/nic.h
index a001fae1a8d..d3ad8ed8d90 100644
--- a/drivers/net/ethernet/sfc/nic.h
+++ b/drivers/net/ethernet/sfc/nic.h
@@ -757,6 +757,7 @@ static inline int efx_nic_irq_test_irq_cpu(struct efx_nic *efx)
 int efx_nic_flush_queues(struct efx_nic *efx);
 void siena_prepare_flush(struct efx_nic *efx);
 int efx_farch_fini_dmaq(struct efx_nic *efx);
+void efx_farch_finish_flr(struct efx_nic *efx);
 void siena_finish_flush(struct efx_nic *efx);
 void falcon_start_nic_stats(struct efx_nic *efx);
 void falcon_stop_nic_stats(struct efx_nic *efx);
diff --git a/drivers/net/ethernet/sfc/siena.c b/drivers/net/ethernet/sfc/siena.c
index 23f3a6f7737..50ffefed492 100644
--- a/drivers/net/ethernet/sfc/siena.c
+++ b/drivers/net/ethernet/sfc/siena.c
@@ -921,6 +921,8 @@ const struct efx_nic_type siena_a0_nic_type = {
 	.fini_dmaq = efx_farch_fini_dmaq,
 	.prepare_flush = siena_prepare_flush,
 	.finish_flush = siena_finish_flush,
+	.prepare_flr = efx_port_dummy_op_void,
+	.finish_flr = efx_farch_finish_flr,
 	.describe_stats = siena_describe_nic_stats,
 	.update_stats = siena_update_nic_stats,
 	.start_stats = efx_mcdi_mac_start_stats,
diff --git a/drivers/net/ethernet/smsc/smc91x.c b/drivers/net/ethernet/smsc/smc91x.c
index d1b4dca53a9..bcaa41af1e6 100644
--- a/drivers/net/ethernet/smsc/smc91x.c
+++ b/drivers/net/ethernet/smsc/smc91x.c
@@ -147,18 +147,19 @@ MODULE_ALIAS("platform:smc91x");
  */
 #define MII_DELAY		1
 
-#if SMC_DEBUG > 0
-#define DBG(n, dev, args...)				\
-	do {						\
-		if (SMC_DEBUG >= (n))			\
-			netdev_dbg(dev, args);		\
+#define DBG(n, dev, fmt, ...)					\
+	do {							\
+		if (SMC_DEBUG >= (n))				\
+			netdev_dbg(dev, fmt, ##__VA_ARGS__);	\
 	} while (0)
 
-#define PRINTK(dev, args...)   netdev_info(dev, args)
-#else
-#define DBG(n, dev, args...)   do { } while (0)
-#define PRINTK(dev, args...)   netdev_dbg(dev, args)
-#endif
+#define PRINTK(dev, fmt, ...)					\
+	do {							\
+		if (SMC_DEBUG > 0)				\
+			netdev_info(dev, fmt, ##__VA_ARGS__);	\
+		else						\
+			netdev_dbg(dev, fmt, ##__VA_ARGS__);	\
+	} while (0)
 
 #if SMC_DEBUG > 3
 static void PRINT_PKT(u_char *buf, int length)
@@ -191,7 +192,7 @@ static void PRINT_PKT(u_char *buf, int length)
 	pr_cont("\n");
 }
 #else
-#define PRINT_PKT(x...)  do { } while (0)
+static inline void PRINT_PKT(u_char *buf, int length) { }
 #endif
 
 
@@ -1781,7 +1782,7 @@ static int smc_findirq(struct smc_local *lp)
 	int timeout = 20;
 	unsigned long cookie;
 
-	DBG(2, dev, "%s: %s\n", CARDNAME, __func__);
+	DBG(2, lp->dev, "%s: %s\n", CARDNAME, __func__);
 
 	cookie = probe_irq_on();
 
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index d940034acdd..0f4841d2e8d 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -1704,7 +1704,7 @@ static int stmmac_open(struct net_device *dev)
 		if (ret) {
 			pr_err("%s: Cannot attach to PHY (error: %d)\n",
 			       __func__, ret);
-			goto phy_error;
+			return ret;
 		}
 	}
 
@@ -1779,8 +1779,6 @@ init_error:
 dma_desc_error:
 	if (priv->phydev)
 		phy_disconnect(priv->phydev);
-phy_error:
-	clk_disable_unprepare(priv->stmmac_clk);
 
 	return ret;
 }
diff --git a/drivers/net/ethernet/sun/cassini.c b/drivers/net/ethernet/sun/cassini.c
index df8d383acf4..b9ac20f4265 100644
--- a/drivers/net/ethernet/sun/cassini.c
+++ b/drivers/net/ethernet/sun/cassini.c
@@ -246,7 +246,7 @@ static inline void cas_lock_tx(struct cas *cp)
 	int i;
 
 	for (i = 0; i < N_TX_RINGS; i++)
-		spin_lock(&cp->tx_lock[i]);
+		spin_lock_nested(&cp->tx_lock[i], i);
 }
 
 static inline void cas_lock_all(struct cas *cp)
diff --git a/drivers/net/ethernet/ti/cpsw.c b/drivers/net/ethernet/ti/cpsw.c
index 36aa109416c..c331b7ebc81 100644
--- a/drivers/net/ethernet/ti/cpsw.c
+++ b/drivers/net/ethernet/ti/cpsw.c
@@ -1871,18 +1871,13 @@ static int cpsw_probe_dt(struct cpsw_platform_data *data,
 		mdio_node = of_find_node_by_phandle(be32_to_cpup(parp));
 		phyid = be32_to_cpup(parp+1);
 		mdio = of_find_device_by_node(mdio_node);
-
-		if (strncmp(mdio->name, "gpio", 4) == 0) {
-			/* GPIO bitbang MDIO driver attached */
-			struct mii_bus *bus = dev_get_drvdata(&mdio->dev);
-
-			snprintf(slave_data->phy_id, sizeof(slave_data->phy_id),
-				 PHY_ID_FMT, bus->id, phyid);
-		} else {
-			/* davinci MDIO driver attached */
-			snprintf(slave_data->phy_id, sizeof(slave_data->phy_id),
-				 PHY_ID_FMT, mdio->name, phyid);
+		of_node_put(mdio_node);
+		if (!mdio) {
+			pr_err("Missing mdio platform device\n");
+			return -EINVAL;
 		}
+		snprintf(slave_data->phy_id, sizeof(slave_data->phy_id),
+			 PHY_ID_FMT, mdio->name, phyid);
 
 		mac_addr = of_get_mac_address(slave_node);
 		if (mac_addr)
diff --git a/drivers/net/hyperv/netvsc_drv.c b/drivers/net/hyperv/netvsc_drv.c
index 31e55fba7ca..7918d5132c1 100644
--- a/drivers/net/hyperv/netvsc_drv.c
+++ b/drivers/net/hyperv/netvsc_drv.c
@@ -382,6 +382,10 @@ static int netvsc_start_xmit(struct sk_buff *skb, struct net_device *net)
 	if (skb_is_gso(skb))
 		goto do_lso;
 
+	if ((skb->ip_summed == CHECKSUM_NONE) ||
+	    (skb->ip_summed == CHECKSUM_UNNECESSARY))
+		goto do_send;
+
 	rndis_msg_size += NDIS_CSUM_PPI_SIZE;
 	ppi = init_ppi_data(rndis_msg, NDIS_CSUM_PPI_SIZE,
 			    TCPIP_CHKSUM_PKTINFO);
diff --git a/drivers/net/macvlan.c b/drivers/net/macvlan.c
index 753a8c23d15..d53e299ae1d 100644
--- a/drivers/net/macvlan.c
+++ b/drivers/net/macvlan.c
@@ -263,11 +263,9 @@ static int macvlan_queue_xmit(struct sk_buff *skb, struct net_device *dev)
 	const struct macvlan_dev *vlan = netdev_priv(dev);
 	const struct macvlan_port *port = vlan->port;
 	const struct macvlan_dev *dest;
-	__u8 ip_summed = skb->ip_summed;
 
 	if (vlan->mode == MACVLAN_MODE_BRIDGE) {
 		const struct ethhdr *eth = (void *)skb->data;
-		skb->ip_summed = CHECKSUM_UNNECESSARY;
 
 		/* send to other bridge ports directly */
 		if (is_multicast_ether_addr(eth->h_dest)) {
@@ -285,7 +283,6 @@ static int macvlan_queue_xmit(struct sk_buff *skb, struct net_device *dev)
 	}
 
 xmit_world:
-	skb->ip_summed = ip_summed;
 	skb->dev = vlan->lowerdev;
 	return dev_queue_xmit(skb);
 }
@@ -461,8 +458,10 @@ static void macvlan_change_rx_flags(struct net_device *dev, int change)
 	struct macvlan_dev *vlan = netdev_priv(dev);
 	struct net_device *lowerdev = vlan->lowerdev;
 
-	if (change & IFF_ALLMULTI)
-		dev_set_allmulti(lowerdev, dev->flags & IFF_ALLMULTI ? 1 : -1);
+	if (dev->flags & IFF_UP) {
+		if (change & IFF_ALLMULTI)
+			dev_set_allmulti(lowerdev, dev->flags & IFF_ALLMULTI ? 1 : -1);
+	}
 }
 
 static void macvlan_set_mac_lists(struct net_device *dev)
@@ -518,6 +517,11 @@ static struct lock_class_key macvlan_netdev_addr_lock_key;
 #define MACVLAN_STATE_MASK \
 	((1<<__LINK_STATE_NOCARRIER) | (1<<__LINK_STATE_DORMANT))
 
+static int macvlan_get_nest_level(struct net_device *dev)
+{
+	return ((struct macvlan_dev *)netdev_priv(dev))->nest_level;
+}
+
 static void macvlan_set_lockdep_class_one(struct net_device *dev,
 					  struct netdev_queue *txq,
 					  void *_unused)
@@ -528,8 +532,9 @@ static void macvlan_set_lockdep_class_one(struct net_device *dev,
 
 static void macvlan_set_lockdep_class(struct net_device *dev)
 {
-	lockdep_set_class(&dev->addr_list_lock,
-			  &macvlan_netdev_addr_lock_key);
+	lockdep_set_class_and_subclass(&dev->addr_list_lock,
+				       &macvlan_netdev_addr_lock_key,
+				       macvlan_get_nest_level(dev));
 	netdev_for_each_tx_queue(dev, macvlan_set_lockdep_class_one, NULL);
 }
 
@@ -724,6 +729,7 @@ static const struct net_device_ops macvlan_netdev_ops = {
 	.ndo_fdb_add		= macvlan_fdb_add,
 	.ndo_fdb_del		= macvlan_fdb_del,
 	.ndo_fdb_dump		= ndo_dflt_fdb_dump,
+	.ndo_get_lock_subclass  = macvlan_get_nest_level,
 };
 
 void macvlan_common_setup(struct net_device *dev)
@@ -852,6 +858,7 @@ int macvlan_common_newlink(struct net *src_net, struct net_device *dev,
 	vlan->dev      = dev;
 	vlan->port     = port;
 	vlan->set_features = MACVLAN_FEATURES;
+	vlan->nest_level = dev_get_nest_level(lowerdev, netif_is_macvlan) + 1;
 
 	vlan->mode     = MACVLAN_MODE_VEPA;
 	if (data && data[IFLA_MACVLAN_MODE])
diff --git a/drivers/net/macvtap.c b/drivers/net/macvtap.c
index ff111a89e17..3381c4f91a8 100644
--- a/drivers/net/macvtap.c
+++ b/drivers/net/macvtap.c
@@ -322,6 +322,15 @@ static rx_handler_result_t macvtap_handle_frame(struct sk_buff **pskb)
 			segs = nskb;
 		}
 	} else {
+		/* If we receive a partial checksum and the tap side
+		 * doesn't support checksum offload, compute the checksum.
+		 * Note: it doesn't matter which checksum feature to
+		 *        check, we either support them all or none.
+		 */
+		if (skb->ip_summed == CHECKSUM_PARTIAL &&
+		    !(features & NETIF_F_ALL_CSUM) &&
+		    skb_checksum_help(skb))
+			goto drop;
 		skb_queue_tail(&q->sk.sk_receive_queue, skb);
 	}
 
diff --git a/drivers/net/phy/mdio-gpio.c b/drivers/net/phy/mdio-gpio.c
index e701433bf52..5f1a2250018 100644
--- a/drivers/net/phy/mdio-gpio.c
+++ b/drivers/net/phy/mdio-gpio.c
@@ -32,29 +32,39 @@
 
 struct mdio_gpio_info {
 	struct mdiobb_ctrl ctrl;
-	int mdc, mdio;
+	int mdc, mdio, mdo;
+	int mdc_active_low, mdio_active_low, mdo_active_low;
 };
 
 static void *mdio_gpio_of_get_data(struct platform_device *pdev)
 {
 	struct device_node *np = pdev->dev.of_node;
 	struct mdio_gpio_platform_data *pdata;
+	enum of_gpio_flags flags;
 	int ret;
 
 	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
 	if (!pdata)
 		return NULL;
 
-	ret = of_get_gpio(np, 0);
+	ret = of_get_gpio_flags(np, 0, &flags);
 	if (ret < 0)
 		return NULL;
 
 	pdata->mdc = ret;
+	pdata->mdc_active_low = flags & OF_GPIO_ACTIVE_LOW;
 
-	ret = of_get_gpio(np, 1);
+	ret = of_get_gpio_flags(np, 1, &flags);
 	if (ret < 0)
 		return NULL;
 	pdata->mdio = ret;
+	pdata->mdio_active_low = flags & OF_GPIO_ACTIVE_LOW;
+
+	ret = of_get_gpio_flags(np, 2, &flags);
+	if (ret > 0) {
+		pdata->mdo = ret;
+		pdata->mdo_active_low = flags & OF_GPIO_ACTIVE_LOW;
+	}
 
 	return pdata;
 }
@@ -64,8 +74,19 @@ static void mdio_dir(struct mdiobb_ctrl *ctrl, int dir)
 	struct mdio_gpio_info *bitbang =
 		container_of(ctrl, struct mdio_gpio_info, ctrl);
 
+	if (bitbang->mdo) {
+		/* Separate output pin. Always set its value to high
+		 * when changing direction. If direction is input,
+		 * assume the pin serves as pull-up. If direction is
+		 * output, the default value is high.
+		 */
+		gpio_set_value(bitbang->mdo, 1 ^ bitbang->mdo_active_low);
+		return;
+	}
+
 	if (dir)
-		gpio_direction_output(bitbang->mdio, 1);
+		gpio_direction_output(bitbang->mdio,
+				      1 ^ bitbang->mdio_active_low);
 	else
 		gpio_direction_input(bitbang->mdio);
 }
@@ -75,7 +96,7 @@ static int mdio_get(struct mdiobb_ctrl *ctrl)
 	struct mdio_gpio_info *bitbang =
 		container_of(ctrl, struct mdio_gpio_info, ctrl);
 
-	return gpio_get_value(bitbang->mdio);
+	return gpio_get_value(bitbang->mdio) ^ bitbang->mdio_active_low;
 }
 
 static void mdio_set(struct mdiobb_ctrl *ctrl, int what)
@@ -83,7 +104,10 @@ static void mdio_set(struct mdiobb_ctrl *ctrl, int what)
 	struct mdio_gpio_info *bitbang =
 		container_of(ctrl, struct mdio_gpio_info, ctrl);
 
-	gpio_set_value(bitbang->mdio, what);
+	if (bitbang->mdo)
+		gpio_set_value(bitbang->mdo, what ^ bitbang->mdo_active_low);
+	else
+		gpio_set_value(bitbang->mdio, what ^ bitbang->mdio_active_low);
 }
 
 static void mdc_set(struct mdiobb_ctrl *ctrl, int what)
@@ -91,7 +115,7 @@ static void mdc_set(struct mdiobb_ctrl *ctrl, int what)
 	struct mdio_gpio_info *bitbang =
 		container_of(ctrl, struct mdio_gpio_info, ctrl);
 
-	gpio_set_value(bitbang->mdc, what);
+	gpio_set_value(bitbang->mdc, what ^ bitbang->mdc_active_low);
 }
 
 static struct mdiobb_ops mdio_gpio_ops = {
@@ -110,18 +134,22 @@ static struct mii_bus *mdio_gpio_bus_init(struct device *dev,
 	struct mdio_gpio_info *bitbang;
 	int i;
 
-	bitbang = kzalloc(sizeof(*bitbang), GFP_KERNEL);
+	bitbang = devm_kzalloc(dev, sizeof(*bitbang), GFP_KERNEL);
 	if (!bitbang)
 		goto out;
 
 	bitbang->ctrl.ops = &mdio_gpio_ops;
 	bitbang->ctrl.reset = pdata->reset;
 	bitbang->mdc = pdata->mdc;
+	bitbang->mdc_active_low = pdata->mdc_active_low;
 	bitbang->mdio = pdata->mdio;
+	bitbang->mdio_active_low = pdata->mdio_active_low;
+	bitbang->mdo = pdata->mdo;
+	bitbang->mdo_active_low = pdata->mdo_active_low;
 
 	new_bus = alloc_mdio_bitbang(&bitbang->ctrl);
 	if (!new_bus)
-		goto out_free_bitbang;
+		goto out;
 
 	new_bus->name = "GPIO Bitbanged MDIO",
 
@@ -138,11 +166,18 @@ static struct mii_bus *mdio_gpio_bus_init(struct device *dev,
 
 	snprintf(new_bus->id, MII_BUS_ID_SIZE, "gpio-%x", bus_id);
 
-	if (gpio_request(bitbang->mdc, "mdc"))
+	if (devm_gpio_request(dev, bitbang->mdc, "mdc"))
+		goto out_free_bus;
+
+	if (devm_gpio_request(dev, bitbang->mdio, "mdio"))
 		goto out_free_bus;
 
-	if (gpio_request(bitbang->mdio, "mdio"))
-		goto out_free_mdc;
+	if (bitbang->mdo) {
+		if (devm_gpio_request(dev, bitbang->mdo, "mdo"))
+			goto out_free_bus;
+		gpio_direction_output(bitbang->mdo, 1);
+		gpio_direction_input(bitbang->mdio);
+	}
 
 	gpio_direction_output(bitbang->mdc, 0);
 
@@ -150,12 +185,8 @@ static struct mii_bus *mdio_gpio_bus_init(struct device *dev,
 
 	return new_bus;
 
-out_free_mdc:
-	gpio_free(bitbang->mdc);
 out_free_bus:
 	free_mdio_bitbang(new_bus);
-out_free_bitbang:
-	kfree(bitbang);
 out:
 	return NULL;
 }
@@ -163,13 +194,8 @@ out:
 static void mdio_gpio_bus_deinit(struct device *dev)
 {
 	struct mii_bus *bus = dev_get_drvdata(dev);
-	struct mdio_gpio_info *bitbang = bus->priv;
 
-	dev_set_drvdata(dev, NULL);
-	gpio_free(bitbang->mdio);
-	gpio_free(bitbang->mdc);
 	free_mdio_bitbang(bus);
-	kfree(bitbang);
 }
 
 static void mdio_gpio_bus_destroy(struct device *dev)
@@ -189,6 +215,10 @@ static int mdio_gpio_probe(struct platform_device *pdev)
 	if (pdev->dev.of_node) {
 		pdata = mdio_gpio_of_get_data(pdev);
 		bus_id = of_alias_get_id(pdev->dev.of_node, "mdio-gpio");
+		if (bus_id < 0) {
+			dev_warn(&pdev->dev, "failed to get alias id\n");
+			bus_id = 0;
+		}
 	} else {
 		pdata = dev_get_platdata(&pdev->dev);
 		bus_id = pdev->id;
diff --git a/drivers/net/phy/micrel.c b/drivers/net/phy/micrel.c
index 5ad971a55c5..d849684231c 100644
--- a/drivers/net/phy/micrel.c
+++ b/drivers/net/phy/micrel.c
@@ -246,13 +246,13 @@ static int ksz9021_load_values_from_of(struct phy_device *phydev,
 	if (val1 != -1)
 		newval = ((newval & 0xfff0) | ((val1 / PS_TO_REG) & 0xf) << 0);
 
-	if (val2 != -1)
+	if (val2 != -2)
 		newval = ((newval & 0xff0f) | ((val2 / PS_TO_REG) & 0xf) << 4);
 
-	if (val3 != -1)
+	if (val3 != -3)
 		newval = ((newval & 0xf0ff) | ((val3 / PS_TO_REG) & 0xf) << 8);
 
-	if (val4 != -1)
+	if (val4 != -4)
 		newval = ((newval & 0x0fff) | ((val4 / PS_TO_REG) & 0xf) << 12);
 
 	return kszphy_extended_write(phydev, reg, newval);
diff --git a/drivers/net/phy/phy.c b/drivers/net/phy/phy.c
index 1b6d09aef42..3bc079a67a3 100644
--- a/drivers/net/phy/phy.c
+++ b/drivers/net/phy/phy.c
@@ -715,7 +715,7 @@ void phy_state_machine(struct work_struct *work)
 	struct delayed_work *dwork = to_delayed_work(work);
 	struct phy_device *phydev =
 			container_of(dwork, struct phy_device, state_queue);
-	int needs_aneg = 0, do_suspend = 0;
+	bool needs_aneg = false, do_suspend = false, do_resume = false;
 	int err = 0;
 
 	mutex_lock(&phydev->lock);
@@ -727,7 +727,7 @@ void phy_state_machine(struct work_struct *work)
 	case PHY_PENDING:
 		break;
 	case PHY_UP:
-		needs_aneg = 1;
+		needs_aneg = true;
 
 		phydev->link_timeout = PHY_AN_TIMEOUT;
 
@@ -757,7 +757,7 @@ void phy_state_machine(struct work_struct *work)
 			phydev->adjust_link(phydev->attached_dev);
 
 		} else if (0 == phydev->link_timeout--)
-			needs_aneg = 1;
+			needs_aneg = true;
 		break;
 	case PHY_NOLINK:
 		err = phy_read_status(phydev);
@@ -765,6 +765,17 @@ void phy_state_machine(struct work_struct *work)
 			break;
 
 		if (phydev->link) {
+			if (AUTONEG_ENABLE == phydev->autoneg) {
+				err = phy_aneg_done(phydev);
+				if (err < 0)
+					break;
+
+				if (!err) {
+					phydev->state = PHY_AN;
+					phydev->link_timeout = PHY_AN_TIMEOUT;
+					break;
+				}
+			}
 			phydev->state = PHY_RUNNING;
 			netif_carrier_on(phydev->attached_dev);
 			phydev->adjust_link(phydev->attached_dev);
@@ -780,7 +791,7 @@ void phy_state_machine(struct work_struct *work)
 			netif_carrier_on(phydev->attached_dev);
 		} else {
 			if (0 == phydev->link_timeout--)
-				needs_aneg = 1;
+				needs_aneg = true;
 		}
 
 		phydev->adjust_link(phydev->attached_dev);
@@ -816,7 +827,7 @@ void phy_state_machine(struct work_struct *work)
 			phydev->link = 0;
 			netif_carrier_off(phydev->attached_dev);
 			phydev->adjust_link(phydev->attached_dev);
-			do_suspend = 1;
+			do_suspend = true;
 		}
 		break;
 	case PHY_RESUMING:
@@ -865,6 +876,7 @@ void phy_state_machine(struct work_struct *work)
 			}
 			phydev->adjust_link(phydev->attached_dev);
 		}
+		do_resume = true;
 		break;
 	}
 
@@ -872,9 +884,10 @@ void phy_state_machine(struct work_struct *work)
 
 	if (needs_aneg)
 		err = phy_start_aneg(phydev);
-
-	if (do_suspend)
+	else if (do_suspend)
 		phy_suspend(phydev);
+	else if (do_resume)
+		phy_resume(phydev);
 
 	if (err < 0)
 		phy_error(phydev);
diff --git a/drivers/net/phy/phy_device.c b/drivers/net/phy/phy_device.c
index 0ce60662429..4987a1c6dc5 100644
--- a/drivers/net/phy/phy_device.c
+++ b/drivers/net/phy/phy_device.c
@@ -614,8 +614,8 @@ int phy_attach_direct(struct net_device *dev, struct phy_device *phydev,
 	err = phy_init_hw(phydev);
 	if (err)
 		phy_detach(phydev);
-
-	phy_resume(phydev);
+	else
+		phy_resume(phydev);
 
 	return err;
 }
diff --git a/drivers/net/slip/slip.c b/drivers/net/slip/slip.c
index cc70ecfc706..ad4a94e9ff5 100644
--- a/drivers/net/slip/slip.c
+++ b/drivers/net/slip/slip.c
@@ -429,13 +429,13 @@ static void slip_write_wakeup(struct tty_struct *tty)
 	if (!sl || sl->magic != SLIP_MAGIC || !netif_running(sl->dev))
 		return;
 
-	spin_lock(&sl->lock);
+	spin_lock_bh(&sl->lock);
 	if (sl->xleft <= 0)  {
 		/* Now serial buffer is almost free & we can start
 		 * transmission of another packet */
 		sl->dev->stats.tx_packets++;
 		clear_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
-		spin_unlock(&sl->lock);
+		spin_unlock_bh(&sl->lock);
 		sl_unlock(sl);
 		return;
 	}
@@ -443,7 +443,7 @@ static void slip_write_wakeup(struct tty_struct *tty)
 	actual = tty->ops->write(tty, sl->xhead, sl->xleft);
 	sl->xleft -= actual;
 	sl->xhead += actual;
-	spin_unlock(&sl->lock);
+	spin_unlock_bh(&sl->lock);
 }
 
 static void sl_tx_timeout(struct net_device *dev)
diff --git a/drivers/net/team/team.c b/drivers/net/team/team.c
index 33008c1d1d6..767fe61b5ac 100644
--- a/drivers/net/team/team.c
+++ b/drivers/net/team/team.c
@@ -2834,8 +2834,10 @@ static int team_device_event(struct notifier_block *unused,
 	case NETDEV_UP:
 		if (netif_carrier_ok(dev))
 			team_port_change_check(port, true);
+		break;
 	case NETDEV_DOWN:
 		team_port_change_check(port, false);
+		break;
 	case NETDEV_CHANGE:
 		if (netif_running(port->dev))
 			team_port_change_check(port,
diff --git a/drivers/net/usb/cdc_mbim.c b/drivers/net/usb/cdc_mbim.c
index c9f3281506a..2e025ddcef2 100644
--- a/drivers/net/usb/cdc_mbim.c
+++ b/drivers/net/usb/cdc_mbim.c
@@ -120,6 +120,16 @@ static void cdc_mbim_unbind(struct usbnet *dev, struct usb_interface *intf)
 	cdc_ncm_unbind(dev, intf);
 }
 
+/* verify that the ethernet protocol is IPv4 or IPv6 */
+static bool is_ip_proto(__be16 proto)
+{
+	switch (proto) {
+	case htons(ETH_P_IP):
+	case htons(ETH_P_IPV6):
+		return true;
+	}
+	return false;
+}
 
 static struct sk_buff *cdc_mbim_tx_fixup(struct usbnet *dev, struct sk_buff *skb, gfp_t flags)
 {
@@ -128,6 +138,7 @@ static struct sk_buff *cdc_mbim_tx_fixup(struct usbnet *dev, struct sk_buff *skb
 	struct cdc_ncm_ctx *ctx = info->ctx;
 	__le32 sign = cpu_to_le32(USB_CDC_MBIM_NDP16_IPS_SIGN);
 	u16 tci = 0;
+	bool is_ip;
 	u8 *c;
 
 	if (!ctx)
@@ -137,25 +148,32 @@ static struct sk_buff *cdc_mbim_tx_fixup(struct usbnet *dev, struct sk_buff *skb
 		if (skb->len <= ETH_HLEN)
 			goto error;
 
+		/* Some applications using e.g. packet sockets will
+		 * bypass the VLAN acceleration and create tagged
+		 * ethernet frames directly.  We primarily look for
+		 * the accelerated out-of-band tag, but fall back if
+		 * required
+		 */
+		skb_reset_mac_header(skb);
+		if (vlan_get_tag(skb, &tci) < 0 && skb->len > VLAN_ETH_HLEN &&
+		    __vlan_get_tag(skb, &tci) == 0) {
+			is_ip = is_ip_proto(vlan_eth_hdr(skb)->h_vlan_encapsulated_proto);
+			skb_pull(skb, VLAN_ETH_HLEN);
+		} else {
+			is_ip = is_ip_proto(eth_hdr(skb)->h_proto);
+			skb_pull(skb, ETH_HLEN);
+		}
+
 		/* mapping VLANs to MBIM sessions:
 		 *   no tag     => IPS session <0>
 		 *   1 - 255    => IPS session <vlanid>
 		 *   256 - 511  => DSS session <vlanid - 256>
 		 *   512 - 4095 => unsupported, drop
 		 */
-		vlan_get_tag(skb, &tci);
-
 		switch (tci & 0x0f00) {
 		case 0x0000: /* VLAN ID 0 - 255 */
-			/* verify that datagram is IPv4 or IPv6 */
-			skb_reset_mac_header(skb);
-			switch (eth_hdr(skb)->h_proto) {
-			case htons(ETH_P_IP):
-			case htons(ETH_P_IPV6):
-				break;
-			default:
+			if (!is_ip)
 				goto error;
-			}
 			c = (u8 *)&sign;
 			c[3] = tci;
 			break;
@@ -169,7 +187,6 @@ static struct sk_buff *cdc_mbim_tx_fixup(struct usbnet *dev, struct sk_buff *skb
 				  "unsupported tci=0x%04x\n", tci);
 			goto error;
 		}
-		skb_pull(skb, ETH_HLEN);
 	}
 
 	spin_lock_bh(&ctx->mtx);
@@ -204,17 +221,23 @@ static void do_neigh_solicit(struct usbnet *dev, u8 *buf, u16 tci)
 		return;
 
 	/* need to send the NA on the VLAN dev, if any */
-	if (tci)
+	rcu_read_lock();
+	if (tci) {
 		netdev = __vlan_find_dev_deep(dev->net, htons(ETH_P_8021Q),
 					      tci);
-	else
+		if (!netdev) {
+			rcu_read_unlock();
+			return;
+		}
+	} else {
 		netdev = dev->net;
-	if (!netdev)
-		return;
+	}
+	dev_hold(netdev);
+	rcu_read_unlock();
 
 	in6_dev = in6_dev_get(netdev);
 	if (!in6_dev)
-		return;
+		goto out;
 	is_router = !!in6_dev->cnf.forwarding;
 	in6_dev_put(in6_dev);
 
@@ -224,6 +247,8 @@ static void do_neigh_solicit(struct usbnet *dev, u8 *buf, u16 tci)
 				 true /* solicited */,
 				 false /* override */,
 				 true /* inc_opt */);
+out:
+	dev_put(netdev);
 }
 
 static bool is_neigh_solicit(u8 *buf, size_t len)
diff --git a/drivers/net/usb/cdc_ncm.c b/drivers/net/usb/cdc_ncm.c
index 549dbac710e..9a2bd11943e 100644
--- a/drivers/net/usb/cdc_ncm.c
+++ b/drivers/net/usb/cdc_ncm.c
@@ -785,7 +785,7 @@ cdc_ncm_fill_tx_frame(struct usbnet *dev, struct sk_buff *skb, __le32 sign)
 	    skb_out->len > CDC_NCM_MIN_TX_PKT)
 		memset(skb_put(skb_out, ctx->tx_max - skb_out->len), 0,
 		       ctx->tx_max - skb_out->len);
-	else if ((skb_out->len % dev->maxpacket) == 0)
+	else if (skb_out->len < ctx->tx_max && (skb_out->len % dev->maxpacket) == 0)
 		*skb_put(skb_out, 1) = 0;	/* force short packet */
 
 	/* set final frame length */
diff --git a/drivers/net/usb/qmi_wwan.c b/drivers/net/usb/qmi_wwan.c
index e3458e3c44f..83208d4fdc5 100644
--- a/drivers/net/usb/qmi_wwan.c
+++ b/drivers/net/usb/qmi_wwan.c
@@ -669,6 +669,22 @@ static const struct usb_device_id products[] = {
 	{QMI_FIXED_INTF(0x05c6, 0x920d, 5)},
 	{QMI_FIXED_INTF(0x12d1, 0x140c, 1)},	/* Huawei E173 */
 	{QMI_FIXED_INTF(0x12d1, 0x14ac, 1)},	/* Huawei E1820 */
+	{QMI_FIXED_INTF(0x16d8, 0x6003, 0)},	/* CMOTech 6003 */
+	{QMI_FIXED_INTF(0x16d8, 0x6007, 0)},	/* CMOTech CHE-628S */
+	{QMI_FIXED_INTF(0x16d8, 0x6008, 0)},	/* CMOTech CMU-301 */
+	{QMI_FIXED_INTF(0x16d8, 0x6280, 0)},	/* CMOTech CHU-628 */
+	{QMI_FIXED_INTF(0x16d8, 0x7001, 0)},	/* CMOTech CHU-720S */
+	{QMI_FIXED_INTF(0x16d8, 0x7002, 0)},	/* CMOTech 7002 */
+	{QMI_FIXED_INTF(0x16d8, 0x7003, 4)},	/* CMOTech CHU-629K */
+	{QMI_FIXED_INTF(0x16d8, 0x7004, 3)},	/* CMOTech 7004 */
+	{QMI_FIXED_INTF(0x16d8, 0x7006, 5)},	/* CMOTech CGU-629 */
+	{QMI_FIXED_INTF(0x16d8, 0x700a, 4)},	/* CMOTech CHU-629S */
+	{QMI_FIXED_INTF(0x16d8, 0x7211, 0)},	/* CMOTech CHU-720I */
+	{QMI_FIXED_INTF(0x16d8, 0x7212, 0)},	/* CMOTech 7212 */
+	{QMI_FIXED_INTF(0x16d8, 0x7213, 0)},	/* CMOTech 7213 */
+	{QMI_FIXED_INTF(0x16d8, 0x7251, 1)},	/* CMOTech 7251 */
+	{QMI_FIXED_INTF(0x16d8, 0x7252, 1)},	/* CMOTech 7252 */
+	{QMI_FIXED_INTF(0x16d8, 0x7253, 1)},	/* CMOTech 7253 */
 	{QMI_FIXED_INTF(0x19d2, 0x0002, 1)},
 	{QMI_FIXED_INTF(0x19d2, 0x0012, 1)},
 	{QMI_FIXED_INTF(0x19d2, 0x0017, 3)},
@@ -730,16 +746,28 @@ static const struct usb_device_id products[] = {
 	{QMI_FIXED_INTF(0x114f, 0x68a2, 8)},    /* Sierra Wireless MC7750 */
 	{QMI_FIXED_INTF(0x1199, 0x68a2, 8)},	/* Sierra Wireless MC7710 in QMI mode */
 	{QMI_FIXED_INTF(0x1199, 0x68a2, 19)},	/* Sierra Wireless MC7710 in QMI mode */
+	{QMI_FIXED_INTF(0x1199, 0x68c0, 8)},	/* Sierra Wireless MC73xx */
+	{QMI_FIXED_INTF(0x1199, 0x68c0, 10)},	/* Sierra Wireless MC73xx */
+	{QMI_FIXED_INTF(0x1199, 0x68c0, 11)},	/* Sierra Wireless MC73xx */
 	{QMI_FIXED_INTF(0x1199, 0x901c, 8)},    /* Sierra Wireless EM7700 */
+	{QMI_FIXED_INTF(0x1199, 0x901f, 8)},    /* Sierra Wireless EM7355 */
+	{QMI_FIXED_INTF(0x1199, 0x9041, 8)},	/* Sierra Wireless MC7305/MC7355 */
 	{QMI_FIXED_INTF(0x1199, 0x9051, 8)},	/* Netgear AirCard 340U */
 	{QMI_FIXED_INTF(0x1bbb, 0x011e, 4)},	/* Telekom Speedstick LTE II (Alcatel One Touch L100V LTE) */
+	{QMI_FIXED_INTF(0x1bbb, 0x0203, 2)},	/* Alcatel L800MA */
 	{QMI_FIXED_INTF(0x2357, 0x0201, 4)},	/* TP-LINK HSUPA Modem MA180 */
 	{QMI_FIXED_INTF(0x2357, 0x9000, 4)},	/* TP-LINK MA260 */
 	{QMI_FIXED_INTF(0x1bc7, 0x1200, 5)},	/* Telit LE920 */
 	{QMI_FIXED_INTF(0x1bc7, 0x1201, 2)},	/* Telit LE920 */
 	{QMI_FIXED_INTF(0x0b3c, 0xc005, 6)},    /* Olivetti Olicard 200 */
+	{QMI_FIXED_INTF(0x0b3c, 0xc00b, 4)},	/* Olivetti Olicard 500 */
 	{QMI_FIXED_INTF(0x1e2d, 0x0060, 4)},	/* Cinterion PLxx */
 	{QMI_FIXED_INTF(0x1e2d, 0x0053, 4)},	/* Cinterion PHxx,PXxx */
+	{QMI_FIXED_INTF(0x413c, 0x81a2, 8)},	/* Dell Wireless 5806 Gobi(TM) 4G LTE Mobile Broadband Card */
+	{QMI_FIXED_INTF(0x413c, 0x81a3, 8)},	/* Dell Wireless 5570 HSPA+ (42Mbps) Mobile Broadband Card */
+	{QMI_FIXED_INTF(0x413c, 0x81a4, 8)},	/* Dell Wireless 5570e HSPA+ (42Mbps) Mobile Broadband Card */
+	{QMI_FIXED_INTF(0x413c, 0x81a8, 8)},	/* Dell Wireless 5808 Gobi(TM) 4G LTE Mobile Broadband Card */
+	{QMI_FIXED_INTF(0x413c, 0x81a9, 8)},	/* Dell Wireless 5808e Gobi(TM) 4G LTE Mobile Broadband Card */
 
 	/* 4. Gobi 1000 devices */
 	{QMI_GOBI1K_DEVICE(0x05c6, 0x9212)},	/* Acer Gobi Modem Device */
diff --git a/drivers/net/virtio_net.c b/drivers/net/virtio_net.c
index 7b687469199..8a852b5f215 100644
--- a/drivers/net/virtio_net.c
+++ b/drivers/net/virtio_net.c
@@ -1285,7 +1285,7 @@ static int virtnet_set_channels(struct net_device *dev,
 	if (channels->rx_count || channels->tx_count || channels->other_count)
 		return -EINVAL;
 
-	if (queue_pairs > vi->max_queue_pairs)
+	if (queue_pairs > vi->max_queue_pairs || queue_pairs == 0)
 		return -EINVAL;
 
 	get_online_cpus();
diff --git a/drivers/net/vxlan.c b/drivers/net/vxlan.c
index 82355d5d155..4dbb2ed85b9 100644
--- a/drivers/net/vxlan.c
+++ b/drivers/net/vxlan.c
@@ -389,8 +389,8 @@ static inline size_t vxlan_nlmsg_size(void)
 		+ nla_total_size(sizeof(struct nda_cacheinfo));
 }
 
-static void vxlan_fdb_notify(struct vxlan_dev *vxlan,
-			     struct vxlan_fdb *fdb, int type)
+static void vxlan_fdb_notify(struct vxlan_dev *vxlan, struct vxlan_fdb *fdb,
+			     struct vxlan_rdst *rd, int type)
 {
 	struct net *net = dev_net(vxlan->dev);
 	struct sk_buff *skb;
@@ -400,8 +400,7 @@ static void vxlan_fdb_notify(struct vxlan_dev *vxlan,
 	if (skb == NULL)
 		goto errout;
 
-	err = vxlan_fdb_info(skb, vxlan, fdb, 0, 0, type, 0,
-			     first_remote_rtnl(fdb));
+	err = vxlan_fdb_info(skb, vxlan, fdb, 0, 0, type, 0, rd);
 	if (err < 0) {
 		/* -EMSGSIZE implies BUG in vxlan_nlmsg_size() */
 		WARN_ON(err == -EMSGSIZE);
@@ -427,10 +426,7 @@ static void vxlan_ip_miss(struct net_device *dev, union vxlan_addr *ipa)
 		.remote_vni = VXLAN_N_VID,
 	};
 
-	INIT_LIST_HEAD(&f.remotes);
-	list_add_rcu(&remote.list, &f.remotes);
-
-	vxlan_fdb_notify(vxlan, &f, RTM_GETNEIGH);
+	vxlan_fdb_notify(vxlan, &f, &remote, RTM_GETNEIGH);
 }
 
 static void vxlan_fdb_miss(struct vxlan_dev *vxlan, const u8 eth_addr[ETH_ALEN])
@@ -438,11 +434,11 @@ static void vxlan_fdb_miss(struct vxlan_dev *vxlan, const u8 eth_addr[ETH_ALEN])
 	struct vxlan_fdb f = {
 		.state = NUD_STALE,
 	};
+	struct vxlan_rdst remote = { };
 
-	INIT_LIST_HEAD(&f.remotes);
 	memcpy(f.eth_addr, eth_addr, ETH_ALEN);
 
-	vxlan_fdb_notify(vxlan, &f, RTM_GETNEIGH);
+	vxlan_fdb_notify(vxlan, &f, &remote, RTM_GETNEIGH);
 }
 
 /* Hash Ethernet address */
@@ -533,7 +529,8 @@ static int vxlan_fdb_replace(struct vxlan_fdb *f,
 
 /* Add/update destinations for multicast */
 static int vxlan_fdb_append(struct vxlan_fdb *f,
-			    union vxlan_addr *ip, __be16 port, __u32 vni, __u32 ifindex)
+			    union vxlan_addr *ip, __be16 port, __u32 vni,
+			    __u32 ifindex, struct vxlan_rdst **rdp)
 {
 	struct vxlan_rdst *rd;
 
@@ -551,6 +548,7 @@ static int vxlan_fdb_append(struct vxlan_fdb *f,
 
 	list_add_tail_rcu(&rd->list, &f->remotes);
 
+	*rdp = rd;
 	return 1;
 }
 
@@ -690,6 +688,7 @@ static int vxlan_fdb_create(struct vxlan_dev *vxlan,
 			    __be16 port, __u32 vni, __u32 ifindex,
 			    __u8 ndm_flags)
 {
+	struct vxlan_rdst *rd = NULL;
 	struct vxlan_fdb *f;
 	int notify = 0;
 
@@ -726,7 +725,8 @@ static int vxlan_fdb_create(struct vxlan_dev *vxlan,
 		if ((flags & NLM_F_APPEND) &&
 		    (is_multicast_ether_addr(f->eth_addr) ||
 		     is_zero_ether_addr(f->eth_addr))) {
-			int rc = vxlan_fdb_append(f, ip, port, vni, ifindex);
+			int rc = vxlan_fdb_append(f, ip, port, vni, ifindex,
+						  &rd);
 
 			if (rc < 0)
 				return rc;
@@ -756,15 +756,18 @@ static int vxlan_fdb_create(struct vxlan_dev *vxlan,
 		INIT_LIST_HEAD(&f->remotes);
 		memcpy(f->eth_addr, mac, ETH_ALEN);
 
-		vxlan_fdb_append(f, ip, port, vni, ifindex);
+		vxlan_fdb_append(f, ip, port, vni, ifindex, &rd);
 
 		++vxlan->addrcnt;
 		hlist_add_head_rcu(&f->hlist,
 				   vxlan_fdb_head(vxlan, mac));
 	}
 
-	if (notify)
-		vxlan_fdb_notify(vxlan, f, RTM_NEWNEIGH);
+	if (notify) {
+		if (rd == NULL)
+			rd = first_remote_rtnl(f);
+		vxlan_fdb_notify(vxlan, f, rd, RTM_NEWNEIGH);
+	}
 
 	return 0;
 }
@@ -785,7 +788,7 @@ static void vxlan_fdb_destroy(struct vxlan_dev *vxlan, struct vxlan_fdb *f)
 		    "delete %pM\n", f->eth_addr);
 
 	--vxlan->addrcnt;
-	vxlan_fdb_notify(vxlan, f, RTM_DELNEIGH);
+	vxlan_fdb_notify(vxlan, f, first_remote_rtnl(f), RTM_DELNEIGH);
 
 	hlist_del_rcu(&f->hlist);
 	call_rcu(&f->rcu, vxlan_fdb_free);
@@ -919,6 +922,7 @@ static int vxlan_fdb_delete(struct ndmsg *ndm, struct nlattr *tb[],
 	 */
 	if (rd && !list_is_singular(&f->remotes)) {
 		list_del_rcu(&rd->list);
+		vxlan_fdb_notify(vxlan, f, rd, RTM_DELNEIGH);
 		kfree_rcu(rd, rcu);
 		goto out;
 	}
@@ -993,7 +997,7 @@ static bool vxlan_snoop(struct net_device *dev,
 
 		rdst->remote_ip = *src_ip;
 		f->updated = jiffies;
-		vxlan_fdb_notify(vxlan, f, RTM_NEWNEIGH);
+		vxlan_fdb_notify(vxlan, f, rdst, RTM_NEWNEIGH);
 	} else {
 		/* learned new entry */
 		spin_lock(&vxlan->hash_lock);
diff --git a/drivers/net/wireless/ath/ath9k/ahb.c b/drivers/net/wireless/ath/ath9k/ahb.c
index a0398fe3eb2..be3eb2a8d60 100644
--- a/drivers/net/wireless/ath/ath9k/ahb.c
+++ b/drivers/net/wireless/ath/ath9k/ahb.c
@@ -86,7 +86,6 @@ static int ath_ahb_probe(struct platform_device *pdev)
 	int irq;
 	int ret = 0;
 	struct ath_hw *ah;
-	struct ath_common *common;
 	char hw_name[64];
 
 	if (!dev_get_platdata(&pdev->dev)) {
@@ -146,9 +145,6 @@ static int ath_ahb_probe(struct platform_device *pdev)
 	wiphy_info(hw->wiphy, "%s mem=0x%lx, irq=%d\n",
 		   hw_name, (unsigned long)mem, irq);
 
-	common = ath9k_hw_common(sc->sc_ah);
-	/* Will be cleared in ath9k_start() */
-	set_bit(ATH_OP_INVALID, &common->op_flags);
 	return 0;
 
  err_irq:
diff --git a/drivers/net/wireless/ath/ath9k/ani.c b/drivers/net/wireless/ath/ath9k/ani.c
index 6d47783f2e5..ba502a2d199 100644
--- a/drivers/net/wireless/ath/ath9k/ani.c
+++ b/drivers/net/wireless/ath/ath9k/ani.c
@@ -155,6 +155,9 @@ static void ath9k_hw_set_ofdm_nil(struct ath_hw *ah, u8 immunityLevel,
 		ATH9K_ANI_RSSI_THR_LOW,
 		ATH9K_ANI_RSSI_THR_HIGH);
 
+	if (AR_SREV_9100(ah) && immunityLevel < ATH9K_ANI_OFDM_DEF_LEVEL)
+		immunityLevel = ATH9K_ANI_OFDM_DEF_LEVEL;
+
 	if (!scan)
 		aniState->ofdmNoiseImmunityLevel = immunityLevel;
 
@@ -235,6 +238,9 @@ static void ath9k_hw_set_cck_nil(struct ath_hw *ah, u_int8_t immunityLevel,
 		BEACON_RSSI(ah), ATH9K_ANI_RSSI_THR_LOW,
 		ATH9K_ANI_RSSI_THR_HIGH);
 
+	if (AR_SREV_9100(ah) && immunityLevel < ATH9K_ANI_CCK_DEF_LEVEL)
+		immunityLevel = ATH9K_ANI_CCK_DEF_LEVEL;
+
 	if (ah->opmode == NL80211_IFTYPE_STATION &&
 	    BEACON_RSSI(ah) <= ATH9K_ANI_RSSI_THR_LOW &&
 	    immunityLevel > ATH9K_ANI_CCK_MAX_LEVEL_LOW_RSSI)
diff --git a/drivers/net/wireless/ath/ath9k/ath9k.h b/drivers/net/wireless/ath/ath9k/ath9k.h
index 44d74495c4d..3ba03dde421 100644
--- a/drivers/net/wireless/ath/ath9k/ath9k.h
+++ b/drivers/net/wireless/ath/ath9k/ath9k.h
@@ -251,7 +251,6 @@ struct ath_atx_tid {
 
 	s8 bar_index;
 	bool sched;
-	bool paused;
 	bool active;
 };
 
diff --git a/drivers/net/wireless/ath/ath9k/debug_sta.c b/drivers/net/wireless/ath/ath9k/debug_sta.c
index d76e6e0120d..ffca918ff16 100644
--- a/drivers/net/wireless/ath/ath9k/debug_sta.c
+++ b/drivers/net/wireless/ath/ath9k/debug_sta.c
@@ -72,7 +72,7 @@ static ssize_t read_file_node_aggr(struct file *file, char __user *user_buf,
 		ath_txq_lock(sc, txq);
 		if (tid->active) {
 			len += scnprintf(buf + len, size - len,
-					 "%3d%11d%10d%10d%10d%10d%9d%6d%8d\n",
+					 "%3d%11d%10d%10d%10d%10d%9d%6d\n",
 					 tid->tidno,
 					 tid->seq_start,
 					 tid->seq_next,
@@ -80,8 +80,7 @@ static ssize_t read_file_node_aggr(struct file *file, char __user *user_buf,
 					 tid->baw_head,
 					 tid->baw_tail,
 					 tid->bar_index,
-					 tid->sched,
-					 tid->paused);
+					 tid->sched);
 		}
 		ath_txq_unlock(sc, txq);
 	}
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_main.c b/drivers/net/wireless/ath/ath9k/htc_drv_main.c
index f46cd0250e4..5627917c5ff 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_main.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_main.c
@@ -95,8 +95,10 @@ static void ath9k_htc_vif_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
 
 	if ((vif->type == NL80211_IFTYPE_AP ||
 	     vif->type == NL80211_IFTYPE_MESH_POINT) &&
-	    bss_conf->enable_beacon)
+	    bss_conf->enable_beacon) {
 		priv->reconfig_beacon = true;
+		priv->rearm_ani = true;
+	}
 
 	if (bss_conf->assoc) {
 		priv->rearm_ani = true;
@@ -257,6 +259,7 @@ static int ath9k_htc_set_channel(struct ath9k_htc_priv *priv,
 
 	ath9k_htc_ps_wakeup(priv);
 
+	ath9k_htc_stop_ani(priv);
 	del_timer_sync(&priv->tx.cleanup_timer);
 	ath9k_htc_tx_drain(priv);
 
diff --git a/drivers/net/wireless/ath/ath9k/init.c b/drivers/net/wireless/ath/ath9k/init.c
index cbbb02a6b13..36ae6490e55 100644
--- a/drivers/net/wireless/ath/ath9k/init.c
+++ b/drivers/net/wireless/ath/ath9k/init.c
@@ -783,6 +783,9 @@ int ath9k_init_device(u16 devid, struct ath_softc *sc,
 	common = ath9k_hw_common(ah);
 	ath9k_set_hw_capab(sc, hw);
 
+	/* Will be cleared in ath9k_start() */
+	set_bit(ATH_OP_INVALID, &common->op_flags);
+
 	/* Initialize regulatory */
 	error = ath_regd_init(&common->regulatory, sc->hw->wiphy,
 			      ath9k_reg_notifier);
diff --git a/drivers/net/wireless/ath/ath9k/pci.c b/drivers/net/wireless/ath/ath9k/pci.c
index 25304adece5..914dbc6b172 100644
--- a/drivers/net/wireless/ath/ath9k/pci.c
+++ b/drivers/net/wireless/ath/ath9k/pci.c
@@ -784,7 +784,6 @@ static int ath_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 {
 	struct ath_softc *sc;
 	struct ieee80211_hw *hw;
-	struct ath_common *common;
 	u8 csz;
 	u32 val;
 	int ret = 0;
@@ -877,10 +876,6 @@ static int ath_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	wiphy_info(hw->wiphy, "%s mem=0x%lx, irq=%d\n",
 		   hw_name, (unsigned long)sc->mem, pdev->irq);
 
-	/* Will be cleared in ath9k_start() */
-	common = ath9k_hw_common(sc->sc_ah);
-	set_bit(ATH_OP_INVALID, &common->op_flags);
-
 	return 0;
 
 err_init:
diff --git a/drivers/net/wireless/ath/ath9k/recv.c b/drivers/net/wireless/ath/ath9k/recv.c
index 6c9accdb52e..19df969ec90 100644
--- a/drivers/net/wireless/ath/ath9k/recv.c
+++ b/drivers/net/wireless/ath/ath9k/recv.c
@@ -975,6 +975,7 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
 	u64 tsf = 0;
 	unsigned long flags;
 	dma_addr_t new_buf_addr;
+	unsigned int budget = 512;
 
 	if (edma)
 		dma_type = DMA_BIDIRECTIONAL;
@@ -1113,15 +1114,17 @@ requeue_drop_frag:
 		}
 requeue:
 		list_add_tail(&bf->list, &sc->rx.rxbuf);
-		if (flush)
-			continue;
 
 		if (edma) {
 			ath_rx_edma_buf_link(sc, qtype);
 		} else {
 			ath_rx_buf_relink(sc, bf);
-			ath9k_hw_rxena(ah);
+			if (!flush)
+				ath9k_hw_rxena(ah);
 		}
+
+		if (!budget--)
+			break;
 	} while (1);
 
 	if (!(ah->imask & ATH9K_INT_RXEOL)) {
diff --git a/drivers/net/wireless/ath/ath9k/xmit.c b/drivers/net/wireless/ath/ath9k/xmit.c
index 87cbec47fb4..66acb2cbd9d 100644
--- a/drivers/net/wireless/ath/ath9k/xmit.c
+++ b/drivers/net/wireless/ath/ath9k/xmit.c
@@ -107,9 +107,6 @@ static void ath_tx_queue_tid(struct ath_txq *txq, struct ath_atx_tid *tid)
 {
 	struct ath_atx_ac *ac = tid->ac;
 
-	if (tid->paused)
-		return;
-
 	if (tid->sched)
 		return;
 
@@ -1407,7 +1404,6 @@ int ath_tx_aggr_start(struct ath_softc *sc, struct ieee80211_sta *sta,
 	ath_tx_tid_change_state(sc, txtid);
 
 	txtid->active = true;
-	txtid->paused = true;
 	*ssn = txtid->seq_start = txtid->seq_next;
 	txtid->bar_index = -1;
 
@@ -1427,7 +1423,6 @@ void ath_tx_aggr_stop(struct ath_softc *sc, struct ieee80211_sta *sta, u16 tid)
 
 	ath_txq_lock(sc, txq);
 	txtid->active = false;
-	txtid->paused = false;
 	ath_tx_flush_tid(sc, txtid);
 	ath_tx_tid_change_state(sc, txtid);
 	ath_txq_unlock_complete(sc, txq);
@@ -1487,7 +1482,7 @@ void ath_tx_aggr_wakeup(struct ath_softc *sc, struct ath_node *an)
 		ath_txq_lock(sc, txq);
 		ac->clear_ps_filter = true;
 
-		if (!tid->paused && ath_tid_has_buffered(tid)) {
+		if (ath_tid_has_buffered(tid)) {
 			ath_tx_queue_tid(txq, tid);
 			ath_txq_schedule(sc, txq);
 		}
@@ -1510,7 +1505,6 @@ void ath_tx_aggr_resume(struct ath_softc *sc, struct ieee80211_sta *sta,
 	ath_txq_lock(sc, txq);
 
 	tid->baw_size = IEEE80211_MIN_AMPDU_BUF << sta->ht_cap.ampdu_factor;
-	tid->paused = false;
 
 	if (ath_tid_has_buffered(tid)) {
 		ath_tx_queue_tid(txq, tid);
@@ -1544,8 +1538,6 @@ void ath9k_release_buffered_frames(struct ieee80211_hw *hw,
 			continue;
 
 		tid = ATH_AN_2_TID(an, i);
-		if (tid->paused)
-			continue;
 
 		ath_txq_lock(sc, tid->ac->txq);
 		while (nframes > 0) {
@@ -1844,9 +1836,6 @@ void ath_txq_schedule(struct ath_softc *sc, struct ath_txq *txq)
 			list_del(&tid->list);
 			tid->sched = false;
 
-			if (tid->paused)
-				continue;
-
 			if (ath_tx_sched_aggr(sc, txq, tid, &stop))
 				sent = true;
 
@@ -2698,7 +2687,6 @@ void ath_tx_node_init(struct ath_softc *sc, struct ath_node *an)
 		tid->baw_size  = WME_MAX_BA;
 		tid->baw_head  = tid->baw_tail = 0;
 		tid->sched     = false;
-		tid->paused    = false;
 		tid->active	   = false;
 		__skb_queue_head_init(&tid->buf_q);
 		__skb_queue_head_init(&tid->retry_q);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/chip.c b/drivers/net/wireless/brcm80211/brcmfmac/chip.c
index df130ef53d1..c7c9f15c0fe 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/chip.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/chip.c
@@ -303,10 +303,10 @@ static void brcmf_chip_ai_coredisable(struct brcmf_core_priv *core,
 
 	ci = core->chip;
 
-	/* if core is already in reset, just return */
+	/* if core is already in reset, skip reset */
 	regdata = ci->ops->read32(ci->ctx, core->wrapbase + BCMA_RESET_CTL);
 	if ((regdata & BCMA_RESET_CTL_RESET) != 0)
-		return;
+		goto in_reset_configure;
 
 	/* configure reset */
 	ci->ops->write32(ci->ctx, core->wrapbase + BCMA_IOCTL,
@@ -322,6 +322,7 @@ static void brcmf_chip_ai_coredisable(struct brcmf_core_priv *core,
 	SPINWAIT(ci->ops->read32(ci->ctx, core->wrapbase + BCMA_RESET_CTL) !=
 		 BCMA_RESET_CTL_RESET, 300);
 
+in_reset_configure:
 	/* in-reset configure */
 	ci->ops->write32(ci->ctx, core->wrapbase + BCMA_IOCTL,
 			 reset | BCMA_IOCTL_FGC | BCMA_IOCTL_CLK);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c b/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
index afb3d15e38f..be1985296bd 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
@@ -4948,7 +4948,7 @@ static int brcmf_enable_bw40_2g(struct brcmf_if *ifp)
 	if (!err) {
 		/* only set 2G bandwidth using bw_cap command */
 		band_bwcap.band = cpu_to_le32(WLC_BAND_2G);
-		band_bwcap.bw_cap = cpu_to_le32(WLC_BW_40MHZ_BIT);
+		band_bwcap.bw_cap = cpu_to_le32(WLC_BW_CAP_40MHZ);
 		err = brcmf_fil_iovar_data_set(ifp, "bw_cap", &band_bwcap,
 					       sizeof(band_bwcap));
 	} else {
diff --git a/drivers/net/wireless/cw1200/debug.c b/drivers/net/wireless/cw1200/debug.c
index e323b4d5433..34f97c31eec 100644
--- a/drivers/net/wireless/cw1200/debug.c
+++ b/drivers/net/wireless/cw1200/debug.c
@@ -41,6 +41,8 @@ static const char * const cw1200_debug_link_id[] = {
 	"REQ",
 	"SOFT",
 	"HARD",
+	"RESET",
+	"RESET_REMAP",
 };
 
 static const char *cw1200_debug_mode(int mode)
diff --git a/drivers/net/wireless/iwlwifi/iwl-7000.c b/drivers/net/wireless/iwlwifi/iwl-7000.c
index 003a546571d..4c2d4ef28b2 100644
--- a/drivers/net/wireless/iwlwifi/iwl-7000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-7000.c
@@ -67,8 +67,8 @@
 #include "iwl-agn-hw.h"
 
 /* Highest firmware API version supported */
-#define IWL7260_UCODE_API_MAX	8
-#define IWL3160_UCODE_API_MAX	8
+#define IWL7260_UCODE_API_MAX	9
+#define IWL3160_UCODE_API_MAX	9
 
 /* Oldest version we won't warn about */
 #define IWL7260_UCODE_API_OK	8
@@ -244,3 +244,4 @@ const struct iwl_cfg iwl7265_n_cfg = {
 
 MODULE_FIRMWARE(IWL7260_MODULE_FIRMWARE(IWL7260_UCODE_API_OK));
 MODULE_FIRMWARE(IWL3160_MODULE_FIRMWARE(IWL3160_UCODE_API_OK));
+MODULE_FIRMWARE(IWL7265_MODULE_FIRMWARE(IWL7260_UCODE_API_OK));
diff --git a/drivers/net/wireless/iwlwifi/mvm/coex.c b/drivers/net/wireless/iwlwifi/mvm/coex.c
index 685f7e8e694..0489314425c 100644
--- a/drivers/net/wireless/iwlwifi/mvm/coex.c
+++ b/drivers/net/wireless/iwlwifi/mvm/coex.c
@@ -190,7 +190,7 @@ static const __le32 iwl_combined_lookup[BT_COEX_MAX_LUT][BT_COEX_LUT_SIZE] = {
 		cpu_to_le32(0xcc00aaaa),
 		cpu_to_le32(0x0000aaaa),
 		cpu_to_le32(0xc0004000),
-		cpu_to_le32(0x00000000),
+		cpu_to_le32(0x00004000),
 		cpu_to_le32(0xf0005000),
 		cpu_to_le32(0xf0005000),
 	},
@@ -213,16 +213,16 @@ static const __le32 iwl_combined_lookup[BT_COEX_MAX_LUT][BT_COEX_LUT_SIZE] = {
 		/* Tx Tx disabled */
 		cpu_to_le32(0xaaaaaaaa),
 		cpu_to_le32(0xaaaaaaaa),
-		cpu_to_le32(0xaaaaaaaa),
+		cpu_to_le32(0xeeaaaaaa),
 		cpu_to_le32(0xaaaaaaaa),
 		cpu_to_le32(0xcc00ff28),
 		cpu_to_le32(0x0000aaaa),
 		cpu_to_le32(0xcc00aaaa),
 		cpu_to_le32(0x0000aaaa),
-		cpu_to_le32(0xC0004000),
-		cpu_to_le32(0xC0004000),
-		cpu_to_le32(0xF0005000),
-		cpu_to_le32(0xF0005000),
+		cpu_to_le32(0xc0004000),
+		cpu_to_le32(0xc0004000),
+		cpu_to_le32(0xf0005000),
+		cpu_to_le32(0xf0005000),
 	},
 };
 
@@ -611,14 +611,14 @@ int iwl_send_bt_init_conf(struct iwl_mvm *mvm)
 		bt_cmd->flags |= cpu_to_le32(BT_COEX_SYNC2SCO);
 
 	if (IWL_MVM_BT_COEX_CORUNNING) {
-		bt_cmd->valid_bit_msk = cpu_to_le32(BT_VALID_CORUN_LUT_20 |
-						    BT_VALID_CORUN_LUT_40);
+		bt_cmd->valid_bit_msk |= cpu_to_le32(BT_VALID_CORUN_LUT_20 |
+						     BT_VALID_CORUN_LUT_40);
 		bt_cmd->flags |= cpu_to_le32(BT_COEX_CORUNNING);
 	}
 
 	if (IWL_MVM_BT_COEX_MPLUT) {
 		bt_cmd->flags |= cpu_to_le32(BT_COEX_MPLUT);
-		bt_cmd->valid_bit_msk = cpu_to_le32(BT_VALID_MULTI_PRIO_LUT);
+		bt_cmd->valid_bit_msk |= cpu_to_le32(BT_VALID_MULTI_PRIO_LUT);
 	}
 
 	if (mvm->cfg->bt_shared_single_ant)
@@ -1262,6 +1262,7 @@ int iwl_mvm_rx_ant_coupling_notif(struct iwl_mvm *mvm,
 	struct iwl_rx_packet *pkt = rxb_addr(rxb);
 	u32 ant_isolation = le32_to_cpup((void *)pkt->data);
 	u8 __maybe_unused lower_bound, upper_bound;
+	int ret;
 	u8 lut;
 
 	struct iwl_bt_coex_cmd *bt_cmd;
@@ -1318,5 +1319,8 @@ int iwl_mvm_rx_ant_coupling_notif(struct iwl_mvm *mvm,
 	memcpy(bt_cmd->bt4_corun_lut40, antenna_coupling_ranges[lut].lut20,
 	       sizeof(bt_cmd->bt4_corun_lut40));
 
-	return 0;
+	ret = iwl_mvm_send_cmd(mvm, &cmd);
+
+	kfree(bt_cmd);
+	return ret;
 }
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h
index 9426905de6b..d73a89ecd78 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h
@@ -183,9 +183,9 @@ enum iwl_scan_type {
  *	this number of packets were received (typically 1)
  * @passive2active: is auto switching from passive to active during scan allowed
  * @rxchain_sel_flags: RXON_RX_CHAIN_*
- * @max_out_time: in usecs, max out of serving channel time
+ * @max_out_time: in TUs, max out of serving channel time
  * @suspend_time: how long to pause scan when returning to service channel:
- *	bits 0-19: beacon interal in usecs (suspend before executing)
+ *	bits 0-19: beacon interal in TUs (suspend before executing)
  *	bits 20-23: reserved
  *	bits 24-31: number of beacons (suspend between channels)
  * @rxon_flags: RXON_FLG_*
@@ -383,8 +383,8 @@ enum scan_framework_client {
  * @quiet_plcp_th:	quiet channel num of packets threshold
  * @good_CRC_th:	passive to active promotion threshold
  * @rx_chain:		RXON rx chain.
- * @max_out_time:	max uSec to be out of assoceated channel
- * @suspend_time:	pause scan this long when returning to service channel
+ * @max_out_time:	max TUs to be out of assoceated channel
+ * @suspend_time:	pause scan this TUs when returning to service channel
  * @flags:		RXON flags
  * @filter_flags:	RXONfilter
  * @tx_cmd:		tx command for active scan; for 2GHz and for 5GHz.
diff --git a/drivers/net/wireless/iwlwifi/mvm/mac80211.c b/drivers/net/wireless/iwlwifi/mvm/mac80211.c
index 4dd9ff43b8b..b41dc84e943 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mac80211.c
+++ b/drivers/net/wireless/iwlwifi/mvm/mac80211.c
@@ -1007,7 +1007,7 @@ static void iwl_mvm_mc_iface_iterator(void *_data, u8 *mac,
 	memcpy(cmd->bssid, vif->bss_conf.bssid, ETH_ALEN);
 	len = roundup(sizeof(*cmd) + cmd->count * ETH_ALEN, 4);
 
-	ret = iwl_mvm_send_cmd_pdu(mvm, MCAST_FILTER_CMD, CMD_SYNC, len, cmd);
+	ret = iwl_mvm_send_cmd_pdu(mvm, MCAST_FILTER_CMD, CMD_ASYNC, len, cmd);
 	if (ret)
 		IWL_ERR(mvm, "mcast filter cmd error. ret=%d\n", ret);
 }
@@ -1023,7 +1023,7 @@ static void iwl_mvm_recalc_multicast(struct iwl_mvm *mvm)
 	if (WARN_ON_ONCE(!mvm->mcast_filter_cmd))
 		return;
 
-	ieee80211_iterate_active_interfaces(
+	ieee80211_iterate_active_interfaces_atomic(
 		mvm->hw, IEEE80211_IFACE_ITER_NORMAL,
 		iwl_mvm_mc_iface_iterator, &iter_data);
 }
@@ -1332,6 +1332,7 @@ static void iwl_mvm_bss_info_changed_station(struct iwl_mvm *mvm,
 		 */
 		iwl_mvm_remove_time_event(mvm, mvmvif,
 					  &mvmvif->time_event_data);
+		iwl_mvm_sf_update(mvm, vif, false);
 		WARN_ON(iwl_mvm_enable_beacon_filter(mvm, vif, CMD_SYNC));
 	} else if (changes & (BSS_CHANGED_PS | BSS_CHANGED_P2P_PS |
 			      BSS_CHANGED_QOS)) {
@@ -1806,6 +1807,11 @@ static int iwl_mvm_mac_sched_scan_start(struct ieee80211_hw *hw,
 
 	mutex_lock(&mvm->mutex);
 
+	if (!iwl_mvm_is_idle(mvm)) {
+		ret = -EBUSY;
+		goto out;
+	}
+
 	switch (mvm->scan_status) {
 	case IWL_MVM_SCAN_OS:
 		IWL_DEBUG_SCAN(mvm, "Stopping previous scan for sched_scan\n");
diff --git a/drivers/net/wireless/iwlwifi/mvm/mvm.h b/drivers/net/wireless/iwlwifi/mvm/mvm.h
index d564233a65d..f1ec0986c3c 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mvm.h
+++ b/drivers/net/wireless/iwlwifi/mvm/mvm.h
@@ -1003,6 +1003,9 @@ static inline bool iwl_mvm_vif_low_latency(struct iwl_mvm_vif *mvmvif)
 	return mvmvif->low_latency;
 }
 
+/* Assoc status */
+bool iwl_mvm_is_idle(struct iwl_mvm *mvm);
+
 /* Thermal management and CT-kill */
 void iwl_mvm_tt_tx_backoff(struct iwl_mvm *mvm, u32 backoff);
 void iwl_mvm_tt_handler(struct iwl_mvm *mvm);
diff --git a/drivers/net/wireless/iwlwifi/mvm/rs.c b/drivers/net/wireless/iwlwifi/mvm/rs.c
index 568abd61b14..e1c83889936 100644
--- a/drivers/net/wireless/iwlwifi/mvm/rs.c
+++ b/drivers/net/wireless/iwlwifi/mvm/rs.c
@@ -59,7 +59,7 @@
 /* max allowed rate miss before sync LQ cmd */
 #define IWL_MISSED_RATE_MAX		15
 #define RS_STAY_IN_COLUMN_TIMEOUT       (5*HZ)
-
+#define RS_IDLE_TIMEOUT                 (5*HZ)
 
 static u8 rs_ht_to_legacy[] = {
 	[IWL_RATE_MCS_0_INDEX] = IWL_RATE_6M_INDEX,
@@ -142,7 +142,7 @@ enum rs_column_mode {
 	RS_MIMO2,
 };
 
-#define MAX_NEXT_COLUMNS 5
+#define MAX_NEXT_COLUMNS 7
 #define MAX_COLUMN_CHECKS 3
 
 typedef bool (*allow_column_func_t) (struct iwl_mvm *mvm,
@@ -212,8 +212,10 @@ static const struct rs_tx_column rs_tx_columns[] = {
 			RS_COLUMN_LEGACY_ANT_B,
 			RS_COLUMN_SISO_ANT_A,
 			RS_COLUMN_SISO_ANT_B,
-			RS_COLUMN_MIMO2,
-			RS_COLUMN_MIMO2_SGI,
+			RS_COLUMN_INVALID,
+			RS_COLUMN_INVALID,
+			RS_COLUMN_INVALID,
+			RS_COLUMN_INVALID,
 		},
 	},
 	[RS_COLUMN_LEGACY_ANT_B] = {
@@ -223,8 +225,10 @@ static const struct rs_tx_column rs_tx_columns[] = {
 			RS_COLUMN_LEGACY_ANT_A,
 			RS_COLUMN_SISO_ANT_A,
 			RS_COLUMN_SISO_ANT_B,
-			RS_COLUMN_MIMO2,
-			RS_COLUMN_MIMO2_SGI,
+			RS_COLUMN_INVALID,
+			RS_COLUMN_INVALID,
+			RS_COLUMN_INVALID,
+			RS_COLUMN_INVALID,
 		},
 	},
 	[RS_COLUMN_SISO_ANT_A] = {
@@ -235,7 +239,9 @@ static const struct rs_tx_column rs_tx_columns[] = {
 			RS_COLUMN_MIMO2,
 			RS_COLUMN_SISO_ANT_A_SGI,
 			RS_COLUMN_SISO_ANT_B_SGI,
-			RS_COLUMN_MIMO2_SGI,
+			RS_COLUMN_LEGACY_ANT_A,
+			RS_COLUMN_LEGACY_ANT_B,
+			RS_COLUMN_INVALID,
 		},
 		.checks = {
 			rs_siso_allow,
@@ -249,7 +255,9 @@ static const struct rs_tx_column rs_tx_columns[] = {
 			RS_COLUMN_MIMO2,
 			RS_COLUMN_SISO_ANT_B_SGI,
 			RS_COLUMN_SISO_ANT_A_SGI,
-			RS_COLUMN_MIMO2_SGI,
+			RS_COLUMN_LEGACY_ANT_A,
+			RS_COLUMN_LEGACY_ANT_B,
+			RS_COLUMN_INVALID,
 		},
 		.checks = {
 			rs_siso_allow,
@@ -265,6 +273,8 @@ static const struct rs_tx_column rs_tx_columns[] = {
 			RS_COLUMN_SISO_ANT_A,
 			RS_COLUMN_SISO_ANT_B,
 			RS_COLUMN_MIMO2,
+			RS_COLUMN_LEGACY_ANT_A,
+			RS_COLUMN_LEGACY_ANT_B,
 		},
 		.checks = {
 			rs_siso_allow,
@@ -281,6 +291,8 @@ static const struct rs_tx_column rs_tx_columns[] = {
 			RS_COLUMN_SISO_ANT_B,
 			RS_COLUMN_SISO_ANT_A,
 			RS_COLUMN_MIMO2,
+			RS_COLUMN_LEGACY_ANT_A,
+			RS_COLUMN_LEGACY_ANT_B,
 		},
 		.checks = {
 			rs_siso_allow,
@@ -296,6 +308,8 @@ static const struct rs_tx_column rs_tx_columns[] = {
 			RS_COLUMN_SISO_ANT_A_SGI,
 			RS_COLUMN_SISO_ANT_B_SGI,
 			RS_COLUMN_MIMO2_SGI,
+			RS_COLUMN_LEGACY_ANT_A,
+			RS_COLUMN_LEGACY_ANT_B,
 		},
 		.checks = {
 			rs_mimo_allow,
@@ -311,6 +325,8 @@ static const struct rs_tx_column rs_tx_columns[] = {
 			RS_COLUMN_SISO_ANT_A,
 			RS_COLUMN_SISO_ANT_B,
 			RS_COLUMN_MIMO2,
+			RS_COLUMN_LEGACY_ANT_A,
+			RS_COLUMN_LEGACY_ANT_B,
 		},
 		.checks = {
 			rs_mimo_allow,
@@ -503,10 +519,12 @@ static void rs_rate_scale_clear_window(struct iwl_rate_scale_data *window)
 	window->average_tpt = IWL_INVALID_VALUE;
 }
 
-static void rs_rate_scale_clear_tbl_windows(struct iwl_scale_tbl_info *tbl)
+static void rs_rate_scale_clear_tbl_windows(struct iwl_mvm *mvm,
+					    struct iwl_scale_tbl_info *tbl)
 {
 	int i;
 
+	IWL_DEBUG_RATE(mvm, "Clearing up window stats\n");
 	for (i = 0; i < IWL_RATE_COUNT; i++)
 		rs_rate_scale_clear_window(&tbl->win[i]);
 }
@@ -992,6 +1010,13 @@ static void rs_tx_status(void *mvm_r, struct ieee80211_supported_band *sband,
 		return;
 	}
 
+#ifdef CONFIG_MAC80211_DEBUGFS
+	/* Disable last tx check if we are debugging with fixed rate */
+	if (lq_sta->dbg_fixed_rate) {
+		IWL_DEBUG_RATE(mvm, "Fixed rate. avoid rate scaling\n");
+		return;
+	}
+#endif
 	if (!ieee80211_is_data(hdr->frame_control) ||
 	    info->flags & IEEE80211_TX_CTL_NO_ACK)
 		return;
@@ -1034,6 +1059,18 @@ static void rs_tx_status(void *mvm_r, struct ieee80211_supported_band *sband,
 			mac_index++;
 	}
 
+	if (time_after(jiffies,
+		       (unsigned long)(lq_sta->last_tx + RS_IDLE_TIMEOUT))) {
+		int tid;
+		IWL_DEBUG_RATE(mvm, "Tx idle for too long. reinit rs\n");
+		for (tid = 0; tid < IWL_MAX_TID_COUNT; tid++)
+			ieee80211_stop_tx_ba_session(sta, tid);
+
+		iwl_mvm_rs_rate_init(mvm, sta, sband->band, false);
+		return;
+	}
+	lq_sta->last_tx = jiffies;
+
 	/* Here we actually compare this rate to the latest LQ command */
 	if ((mac_index < 0) ||
 	    (rate.sgi != !!(mac_flags & IEEE80211_TX_RC_SHORT_GI)) ||
@@ -1186,9 +1223,26 @@ static void rs_set_stay_in_table(struct iwl_mvm *mvm, u8 is_legacy,
 	lq_sta->visited_columns = 0;
 }
 
+static int rs_get_max_allowed_rate(struct iwl_lq_sta *lq_sta,
+				   const struct rs_tx_column *column)
+{
+	switch (column->mode) {
+	case RS_LEGACY:
+		return lq_sta->max_legacy_rate_idx;
+	case RS_SISO:
+		return lq_sta->max_siso_rate_idx;
+	case RS_MIMO2:
+		return lq_sta->max_mimo2_rate_idx;
+	default:
+		WARN_ON_ONCE(1);
+	}
+
+	return lq_sta->max_legacy_rate_idx;
+}
+
 static const u16 *rs_get_expected_tpt_table(struct iwl_lq_sta *lq_sta,
-				      const struct rs_tx_column *column,
-				      u32 bw)
+					    const struct rs_tx_column *column,
+					    u32 bw)
 {
 	/* Used to choose among HT tables */
 	const u16 (*ht_tbl_pointer)[IWL_RATE_COUNT];
@@ -1438,7 +1492,7 @@ static void rs_stay_in_table(struct iwl_lq_sta *lq_sta, bool force_search)
 
 				IWL_DEBUG_RATE(mvm,
 					       "LQ: stay in table clear win\n");
-				rs_rate_scale_clear_tbl_windows(tbl);
+				rs_rate_scale_clear_tbl_windows(mvm, tbl);
 			}
 		}
 
@@ -1446,8 +1500,7 @@ static void rs_stay_in_table(struct iwl_lq_sta *lq_sta, bool force_search)
 		 * bitmaps and stats in active table (this will become the new
 		 * "search" table). */
 		if (lq_sta->rs_state == RS_STATE_SEARCH_CYCLE_STARTED) {
-			IWL_DEBUG_RATE(mvm, "Clearing up window stats\n");
-			rs_rate_scale_clear_tbl_windows(tbl);
+			rs_rate_scale_clear_tbl_windows(mvm, tbl);
 		}
 	}
 }
@@ -1485,14 +1538,14 @@ static enum rs_column rs_get_next_column(struct iwl_mvm *mvm,
 					 struct ieee80211_sta *sta,
 					 struct iwl_scale_tbl_info *tbl)
 {
-	int i, j, n;
+	int i, j, max_rate;
 	enum rs_column next_col_id;
 	const struct rs_tx_column *curr_col = &rs_tx_columns[tbl->column];
 	const struct rs_tx_column *next_col;
 	allow_column_func_t allow_func;
 	u8 valid_ants = mvm->fw->valid_tx_ant;
 	const u16 *expected_tpt_tbl;
-	s32 tpt, max_expected_tpt;
+	u16 tpt, max_expected_tpt;
 
 	for (i = 0; i < MAX_NEXT_COLUMNS; i++) {
 		next_col_id = curr_col->next_columns[i];
@@ -1535,11 +1588,11 @@ static enum rs_column rs_get_next_column(struct iwl_mvm *mvm,
 		if (WARN_ON_ONCE(!expected_tpt_tbl))
 			continue;
 
-		max_expected_tpt = 0;
-		for (n = 0; n < IWL_RATE_COUNT; n++)
-			if (expected_tpt_tbl[n] > max_expected_tpt)
-				max_expected_tpt = expected_tpt_tbl[n];
+		max_rate = rs_get_max_allowed_rate(lq_sta, next_col);
+		if (WARN_ON_ONCE(max_rate == IWL_RATE_INVALID))
+			continue;
 
+		max_expected_tpt = expected_tpt_tbl[max_rate];
 		if (tpt >= max_expected_tpt) {
 			IWL_DEBUG_RATE(mvm,
 				       "Skip column %d: can't beat current TPT. Max expected %d current %d\n",
@@ -1547,14 +1600,15 @@ static enum rs_column rs_get_next_column(struct iwl_mvm *mvm,
 			continue;
 		}
 
+		IWL_DEBUG_RATE(mvm,
+			       "Found potential column %d. Max expected %d current %d\n",
+			       next_col_id, max_expected_tpt, tpt);
 		break;
 	}
 
 	if (i == MAX_NEXT_COLUMNS)
 		return RS_COLUMN_INVALID;
 
-	IWL_DEBUG_RATE(mvm, "Found potential column %d\n", next_col_id);
-
 	return next_col_id;
 }
 
@@ -1640,85 +1694,76 @@ static enum rs_action rs_get_rate_action(struct iwl_mvm *mvm,
 {
 	enum rs_action action = RS_ACTION_STAY;
 
-	/* Too many failures, decrease rate */
 	if ((sr <= RS_SR_FORCE_DECREASE) || (current_tpt == 0)) {
 		IWL_DEBUG_RATE(mvm,
-			       "decrease rate because of low SR\n");
-		action = RS_ACTION_DOWNSCALE;
-	/* No throughput measured yet for adjacent rates; try increase. */
-	} else if ((low_tpt == IWL_INVALID_VALUE) &&
-		   (high_tpt == IWL_INVALID_VALUE)) {
-		if (high != IWL_RATE_INVALID && sr >= IWL_RATE_INCREASE_TH) {
-			IWL_DEBUG_RATE(mvm,
-				       "Good SR and no high rate measurement. "
-				       "Increase rate\n");
-			action = RS_ACTION_UPSCALE;
-		} else if (low != IWL_RATE_INVALID) {
-			IWL_DEBUG_RATE(mvm,
-				       "Remain in current rate\n");
-			action = RS_ACTION_STAY;
-		}
+			       "Decrease rate because of low SR\n");
+		return RS_ACTION_DOWNSCALE;
 	}
 
-	/* Both adjacent throughputs are measured, but neither one has better
-	 * throughput; we're using the best rate, don't change it!
-	 */
-	else if ((low_tpt != IWL_INVALID_VALUE) &&
-		 (high_tpt != IWL_INVALID_VALUE) &&
-		 (low_tpt < current_tpt) &&
-		 (high_tpt < current_tpt)) {
+	if ((low_tpt == IWL_INVALID_VALUE) &&
+	    (high_tpt == IWL_INVALID_VALUE) &&
+	    (high != IWL_RATE_INVALID)) {
 		IWL_DEBUG_RATE(mvm,
-			       "Both high and low are worse. "
-			       "Maintain rate\n");
-		action = RS_ACTION_STAY;
+			       "No data about high/low rates. Increase rate\n");
+		return RS_ACTION_UPSCALE;
 	}
 
-	/* At least one adjacent rate's throughput is measured,
-	 * and may have better performance.
-	 */
-	else {
-		/* Higher adjacent rate's throughput is measured */
-		if (high_tpt != IWL_INVALID_VALUE) {
-			/* Higher rate has better throughput */
-			if (high_tpt > current_tpt &&
-			    sr >= IWL_RATE_INCREASE_TH) {
-				IWL_DEBUG_RATE(mvm,
-					       "Higher rate is better and good "
-					       "SR. Increate rate\n");
-				action = RS_ACTION_UPSCALE;
-			} else {
-				IWL_DEBUG_RATE(mvm,
-					       "Higher rate isn't better OR "
-					       "no good SR. Maintain rate\n");
-				action = RS_ACTION_STAY;
-			}
+	if ((high_tpt == IWL_INVALID_VALUE) &&
+	    (high != IWL_RATE_INVALID) &&
+	    (low_tpt != IWL_INVALID_VALUE) &&
+	    (low_tpt < current_tpt)) {
+		IWL_DEBUG_RATE(mvm,
+			       "No data about high rate and low rate is worse. Increase rate\n");
+		return RS_ACTION_UPSCALE;
+	}
 
-		/* Lower adjacent rate's throughput is measured */
-		} else if (low_tpt != IWL_INVALID_VALUE) {
-			/* Lower rate has better throughput */
-			if (low_tpt > current_tpt) {
-				IWL_DEBUG_RATE(mvm,
-					       "Lower rate is better. "
-					       "Decrease rate\n");
-				action = RS_ACTION_DOWNSCALE;
-			} else if (sr >= IWL_RATE_INCREASE_TH) {
-				IWL_DEBUG_RATE(mvm,
-					       "Lower rate isn't better and "
-					       "good SR. Increase rate\n");
-				action = RS_ACTION_UPSCALE;
-			}
-		}
+	if ((high_tpt != IWL_INVALID_VALUE) &&
+	    (high_tpt > current_tpt)) {
+		IWL_DEBUG_RATE(mvm,
+			       "Higher rate is better. Increate rate\n");
+		return RS_ACTION_UPSCALE;
 	}
 
-	/* Sanity check; asked for decrease, but success rate or throughput
-	 * has been good at old rate.  Don't change it.
-	 */
-	if ((action == RS_ACTION_DOWNSCALE) && (low != IWL_RATE_INVALID) &&
-	    ((sr > IWL_RATE_HIGH_TH) ||
-	     (current_tpt > (100 * tbl->expected_tpt[low])))) {
+	if ((low_tpt != IWL_INVALID_VALUE) &&
+	    (high_tpt != IWL_INVALID_VALUE) &&
+	    (low_tpt < current_tpt) &&
+	    (high_tpt < current_tpt)) {
+		IWL_DEBUG_RATE(mvm,
+			       "Both high and low are worse. Maintain rate\n");
+		return RS_ACTION_STAY;
+	}
+
+	if ((low_tpt != IWL_INVALID_VALUE) &&
+	    (low_tpt > current_tpt)) {
+		IWL_DEBUG_RATE(mvm,
+			       "Lower rate is better\n");
+		action = RS_ACTION_DOWNSCALE;
+		goto out;
+	}
+
+	if ((low_tpt == IWL_INVALID_VALUE) &&
+	    (low != IWL_RATE_INVALID)) {
 		IWL_DEBUG_RATE(mvm,
-			       "Sanity check failed. Maintain rate\n");
-		action = RS_ACTION_STAY;
+			       "No data about lower rate\n");
+		action = RS_ACTION_DOWNSCALE;
+		goto out;
+	}
+
+	IWL_DEBUG_RATE(mvm, "Maintain rate\n");
+
+out:
+	if ((action == RS_ACTION_DOWNSCALE) && (low != IWL_RATE_INVALID)) {
+		if (sr >= RS_SR_NO_DECREASE) {
+			IWL_DEBUG_RATE(mvm,
+				       "SR is above NO DECREASE. Avoid downscale\n");
+			action = RS_ACTION_STAY;
+		} else if (current_tpt > (100 * tbl->expected_tpt[low])) {
+			IWL_DEBUG_RATE(mvm,
+				       "Current TPT is higher than max expected in low rate. Avoid downscale\n");
+			action = RS_ACTION_STAY;
+		} else {
+			IWL_DEBUG_RATE(mvm, "Decrease rate\n");
+		}
 	}
 
 	return action;
@@ -1792,6 +1837,7 @@ static void rs_rate_scale_perform(struct iwl_mvm *mvm,
 			       "Aggregation changed: prev %d current %d. Update expected TPT table\n",
 			       prev_agg, lq_sta->is_agg);
 		rs_set_expected_tpt_table(lq_sta, tbl);
+		rs_rate_scale_clear_tbl_windows(mvm, tbl);
 	}
 
 	/* current tx rate */
@@ -2021,7 +2067,7 @@ lq_update:
 		if (lq_sta->search_better_tbl) {
 			/* Access the "search" table, clear its history. */
 			tbl = &(lq_sta->lq_info[(1 - lq_sta->active_tbl)]);
-			rs_rate_scale_clear_tbl_windows(tbl);
+			rs_rate_scale_clear_tbl_windows(mvm, tbl);
 
 			/* Use new "search" start rate */
 			index = tbl->rate.index;
@@ -2042,8 +2088,18 @@ lq_update:
 		 * stay with best antenna legacy modulation for a while
 		 * before next round of mode comparisons. */
 		tbl1 = &(lq_sta->lq_info[lq_sta->active_tbl]);
-		if (is_legacy(&tbl1->rate) && !sta->ht_cap.ht_supported) {
+		if (is_legacy(&tbl1->rate)) {
 			IWL_DEBUG_RATE(mvm, "LQ: STAY in legacy table\n");
+
+			if (tid != IWL_MAX_TID_COUNT) {
+				tid_data = &sta_priv->tid_data[tid];
+				if (tid_data->state != IWL_AGG_OFF) {
+					IWL_DEBUG_RATE(mvm,
+						       "Stop aggregation on tid %d\n",
+						       tid);
+					ieee80211_stop_tx_ba_session(sta, tid);
+				}
+			}
 			rs_set_stay_in_table(mvm, 1, lq_sta);
 		} else {
 		/* If we're in an HT mode, and all 3 mode switch actions
@@ -2342,9 +2398,10 @@ void iwl_mvm_rs_rate_init(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 	lq_sta->lq.sta_id = sta_priv->sta_id;
 
 	for (j = 0; j < LQ_SIZE; j++)
-		rs_rate_scale_clear_tbl_windows(&lq_sta->lq_info[j]);
+		rs_rate_scale_clear_tbl_windows(mvm, &lq_sta->lq_info[j]);
 
 	lq_sta->flush_timer = 0;
+	lq_sta->last_tx = jiffies;
 
 	IWL_DEBUG_RATE(mvm,
 		       "LQ: *** rate scale station global init for station %d ***\n",
@@ -2388,11 +2445,22 @@ void iwl_mvm_rs_rate_init(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 		lq_sta->is_vht = true;
 	}
 
-	IWL_DEBUG_RATE(mvm,
-		       "SISO-RATE=%X MIMO2-RATE=%X VHT=%d\n",
+	lq_sta->max_legacy_rate_idx = find_last_bit(&lq_sta->active_legacy_rate,
+						    BITS_PER_LONG);
+	lq_sta->max_siso_rate_idx = find_last_bit(&lq_sta->active_siso_rate,
+						  BITS_PER_LONG);
+	lq_sta->max_mimo2_rate_idx = find_last_bit(&lq_sta->active_mimo2_rate,
+						   BITS_PER_LONG);
+
+	IWL_DEBUG_RATE(mvm, "RATE MASK: LEGACY=%lX SISO=%lX MIMO2=%lX VHT=%d\n",
+		       lq_sta->active_legacy_rate,
 		       lq_sta->active_siso_rate,
 		       lq_sta->active_mimo2_rate,
 		       lq_sta->is_vht);
+	IWL_DEBUG_RATE(mvm, "MAX RATE: LEGACY=%d SISO=%d MIMO2=%d\n",
+		       lq_sta->max_legacy_rate_idx,
+		       lq_sta->max_siso_rate_idx,
+		       lq_sta->max_mimo2_rate_idx);
 
 	/* These values will be overridden later */
 	lq_sta->lq.single_stream_ant_msk =
@@ -2547,6 +2615,7 @@ static void rs_build_rates_table(struct iwl_mvm *mvm,
 	if (is_siso(&rate)) {
 		num_rates = RS_SECONDARY_SISO_NUM_RATES;
 		num_retries = RS_SECONDARY_SISO_RETRIES;
+		lq_cmd->mimo_delim = index;
 	} else if (is_legacy(&rate)) {
 		num_rates = RS_SECONDARY_LEGACY_NUM_RATES;
 		num_retries = RS_LEGACY_RETRIES_PER_RATE;
@@ -2749,7 +2818,7 @@ static ssize_t rs_sta_dbgfs_scale_table_read(struct file *file,
 		return -ENOMEM;
 
 	desc += sprintf(buff+desc, "sta_id %d\n", lq_sta->lq.sta_id);
-	desc += sprintf(buff+desc, "failed=%d success=%d rate=0%X\n",
+	desc += sprintf(buff+desc, "failed=%d success=%d rate=0%lX\n",
 			lq_sta->total_failed, lq_sta->total_success,
 			lq_sta->active_legacy_rate);
 	desc += sprintf(buff+desc, "fixed rate 0x%X\n",
diff --git a/drivers/net/wireless/iwlwifi/mvm/rs.h b/drivers/net/wireless/iwlwifi/mvm/rs.h
index 3332b396011..0acfac96a56 100644
--- a/drivers/net/wireless/iwlwifi/mvm/rs.h
+++ b/drivers/net/wireless/iwlwifi/mvm/rs.h
@@ -156,6 +156,7 @@ enum {
 #define IWL_RATE_HIGH_TH		10880	/*  85% */
 #define IWL_RATE_INCREASE_TH		6400	/*  50% */
 #define RS_SR_FORCE_DECREASE		1920	/*  15% */
+#define RS_SR_NO_DECREASE		10880	/*  85% */
 
 #define LINK_QUAL_AGG_TIME_LIMIT_DEF	(4000) /* 4 milliseconds */
 #define LINK_QUAL_AGG_TIME_LIMIT_MAX	(8000)
@@ -310,13 +311,20 @@ struct iwl_lq_sta {
 	u32 visited_columns;    /* Bitmask marking which Tx columns were
 				 * explored during a search cycle
 				 */
+	u64 last_tx;
 	bool is_vht;
 	enum ieee80211_band band;
 
 	/* The following are bitmaps of rates; IWL_RATE_6M_MASK, etc. */
-	u16 active_legacy_rate;
-	u16 active_siso_rate;
-	u16 active_mimo2_rate;
+	unsigned long active_legacy_rate;
+	unsigned long active_siso_rate;
+	unsigned long active_mimo2_rate;
+
+	/* Highest rate per Tx mode */
+	u8 max_legacy_rate_idx;
+	u8 max_siso_rate_idx;
+	u8 max_mimo2_rate_idx;
+
 	s8 max_rate_idx;     /* Max rate set by user */
 	u8 missed_rate_counter;
 
diff --git a/drivers/net/wireless/iwlwifi/mvm/scan.c b/drivers/net/wireless/iwlwifi/mvm/scan.c
index c91dc849885..c28de54c75d 100644
--- a/drivers/net/wireless/iwlwifi/mvm/scan.c
+++ b/drivers/net/wireless/iwlwifi/mvm/scan.c
@@ -277,51 +277,22 @@ static void iwl_mvm_scan_calc_params(struct iwl_mvm *mvm,
 					    IEEE80211_IFACE_ITER_NORMAL,
 					    iwl_mvm_scan_condition_iterator,
 					    &global_bound);
-	/*
-	 * Under low latency traffic passive scan is fragmented meaning
-	 * that dwell on a particular channel will be fragmented. Each fragment
-	 * dwell time is 20ms and fragments period is 105ms. Skipping to next
-	 * channel will be delayed by the same period - 105ms. So suspend_time
-	 * parameter describing both fragments and channels skipping periods is
-	 * set to 105ms. This value is chosen so that overall passive scan
-	 * duration will not be too long. Max_out_time in this case is set to
-	 * 70ms, so for active scanning operating channel will be left for 70ms
-	 * while for passive still for 20ms (fragment dwell).
-	 */
-	if (global_bound) {
-		if (!iwl_mvm_low_latency(mvm)) {
-			params->suspend_time = ieee80211_tu_to_usec(100);
-			params->max_out_time = ieee80211_tu_to_usec(600);
-		} else {
-			params->suspend_time = ieee80211_tu_to_usec(105);
-			/* P2P doesn't support fragmented passive scan, so
-			 * configure max_out_time to be at least longest dwell
-			 * time for passive scan.
-			 */
-			if (vif->type == NL80211_IFTYPE_STATION && !vif->p2p) {
-				params->max_out_time = ieee80211_tu_to_usec(70);
-				params->passive_fragmented = true;
-			} else {
-				u32 passive_dwell;
 
-				/*
-				 * Use band G so that passive channel dwell time
-				 * will be assigned with maximum value.
-				 */
-				band = IEEE80211_BAND_2GHZ;
-				passive_dwell = iwl_mvm_get_passive_dwell(band);
-				params->max_out_time =
-					ieee80211_tu_to_usec(passive_dwell);
-			}
-		}
+	if (!global_bound)
+		goto not_bound;
+
+	params->suspend_time = 100;
+	params->max_out_time = 600;
+
+	if (iwl_mvm_low_latency(mvm)) {
+		params->suspend_time = 250;
+		params->max_out_time = 250;
 	}
 
+not_bound:
+
 	for (band = IEEE80211_BAND_2GHZ; band < IEEE80211_NUM_BANDS; band++) {
-		if (params->passive_fragmented)
-			params->dwell[band].passive = 20;
-		else
-			params->dwell[band].passive =
-				iwl_mvm_get_passive_dwell(band);
+		params->dwell[band].passive = iwl_mvm_get_passive_dwell(band);
 		params->dwell[band].active = iwl_mvm_get_active_dwell(band,
 								      n_ssids);
 	}
@@ -761,7 +732,7 @@ int iwl_mvm_config_sched_scan(struct iwl_mvm *mvm,
 	int band_2ghz = mvm->nvm_data->bands[IEEE80211_BAND_2GHZ].n_channels;
 	int band_5ghz = mvm->nvm_data->bands[IEEE80211_BAND_5GHZ].n_channels;
 	int head = 0;
-	int tail = band_2ghz + band_5ghz;
+	int tail = band_2ghz + band_5ghz - 1;
 	u32 ssid_bitmap;
 	int cmd_len;
 	int ret;
diff --git a/drivers/net/wireless/iwlwifi/mvm/sf.c b/drivers/net/wireless/iwlwifi/mvm/sf.c
index 8401627c003..88809b2d165 100644
--- a/drivers/net/wireless/iwlwifi/mvm/sf.c
+++ b/drivers/net/wireless/iwlwifi/mvm/sf.c
@@ -274,7 +274,8 @@ int iwl_mvm_sf_update(struct iwl_mvm *mvm, struct ieee80211_vif *changed_vif,
 				return -EINVAL;
 			if (changed_vif->type != NL80211_IFTYPE_STATION) {
 				new_state = SF_UNINIT;
-			} else if (changed_vif->bss_conf.assoc) {
+			} else if (changed_vif->bss_conf.assoc &&
+				   changed_vif->bss_conf.dtim_period) {
 				mvmvif = iwl_mvm_vif_from_mac80211(changed_vif);
 				sta_id = mvmvif->ap_sta_id;
 				new_state = SF_FULL_ON;
diff --git a/drivers/net/wireless/iwlwifi/mvm/utils.c b/drivers/net/wireless/iwlwifi/mvm/utils.c
index d619851745a..2180902266a 100644
--- a/drivers/net/wireless/iwlwifi/mvm/utils.c
+++ b/drivers/net/wireless/iwlwifi/mvm/utils.c
@@ -644,3 +644,22 @@ bool iwl_mvm_low_latency(struct iwl_mvm *mvm)
 
 	return result;
 }
+
+static void iwl_mvm_idle_iter(void *_data, u8 *mac, struct ieee80211_vif *vif)
+{
+	bool *idle = _data;
+
+	if (!vif->bss_conf.idle)
+		*idle = false;
+}
+
+bool iwl_mvm_is_idle(struct iwl_mvm *mvm)
+{
+	bool idle = true;
+
+	ieee80211_iterate_active_interfaces_atomic(
+			mvm->hw, IEEE80211_IFACE_ITER_NORMAL,
+			iwl_mvm_idle_iter, &idle);
+
+	return idle;
+}
diff --git a/drivers/net/wireless/iwlwifi/pcie/drv.c b/drivers/net/wireless/iwlwifi/pcie/drv.c
index edb015c9904..3d1d57f9f5b 100644
--- a/drivers/net/wireless/iwlwifi/pcie/drv.c
+++ b/drivers/net/wireless/iwlwifi/pcie/drv.c
@@ -373,12 +373,14 @@ static DEFINE_PCI_DEVICE_TABLE(iwl_hw_card_ids) = {
 	{IWL_PCI_DEVICE(0x095A, 0x500A, iwl7265_2n_cfg)},
 	{IWL_PCI_DEVICE(0x095B, 0x5200, iwl7265_2n_cfg)},
 	{IWL_PCI_DEVICE(0x095A, 0x5002, iwl7265_n_cfg)},
+	{IWL_PCI_DEVICE(0x095A, 0x5102, iwl7265_n_cfg)},
 	{IWL_PCI_DEVICE(0x095B, 0x5202, iwl7265_n_cfg)},
 	{IWL_PCI_DEVICE(0x095A, 0x9010, iwl7265_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x095A, 0x9012, iwl7265_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x095A, 0x9110, iwl7265_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x095A, 0x9112, iwl7265_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x095A, 0x9210, iwl7265_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x095A, 0x9200, iwl7265_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x095A, 0x9510, iwl7265_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x095A, 0x9310, iwl7265_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x095A, 0x9410, iwl7265_2ac_cfg)},
diff --git a/drivers/net/wireless/iwlwifi/pcie/trans.c b/drivers/net/wireless/iwlwifi/pcie/trans.c
index dcfd6d866d0..2365553f1ef 100644
--- a/drivers/net/wireless/iwlwifi/pcie/trans.c
+++ b/drivers/net/wireless/iwlwifi/pcie/trans.c
@@ -1749,6 +1749,10 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
 	 * PCI Tx retries from interfering with C3 CPU state */
 	pci_write_config_byte(pdev, PCI_CFG_RETRY_TIMEOUT, 0x00);
 
+	trans->dev = &pdev->dev;
+	trans_pcie->pci_dev = pdev;
+	iwl_disable_interrupts(trans);
+
 	err = pci_enable_msi(pdev);
 	if (err) {
 		dev_err(&pdev->dev, "pci_enable_msi failed(0X%x)\n", err);
@@ -1760,8 +1764,6 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
 		}
 	}
 
-	trans->dev = &pdev->dev;
-	trans_pcie->pci_dev = pdev;
 	trans->hw_rev = iwl_read32(trans, CSR_HW_REV);
 	trans->hw_id = (pdev->device << 16) + pdev->subsystem_device;
 	snprintf(trans->hw_id_str, sizeof(trans->hw_id_str),
@@ -1787,8 +1789,6 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
 		goto out_pci_disable_msi;
 	}
 
-	trans_pcie->inta_mask = CSR_INI_SET_MASK;
-
 	if (iwl_pcie_alloc_ict(trans))
 		goto out_free_cmd_pool;
 
@@ -1800,6 +1800,8 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
 		goto out_free_ict;
 	}
 
+	trans_pcie->inta_mask = CSR_INI_SET_MASK;
+
 	return trans;
 
 out_free_ict:
diff --git a/drivers/net/wireless/mwifiex/main.c b/drivers/net/wireless/mwifiex/main.c
index 77db0886c6e..9c771b3e991 100644
--- a/drivers/net/wireless/mwifiex/main.c
+++ b/drivers/net/wireless/mwifiex/main.c
@@ -292,6 +292,12 @@ process_start:
 			while ((skb = skb_dequeue(&adapter->usb_rx_data_q)))
 				mwifiex_handle_rx_packet(adapter, skb);
 
+		/* Check for event */
+		if (adapter->event_received) {
+			adapter->event_received = false;
+			mwifiex_process_event(adapter);
+		}
+
 		/* Check for Cmd Resp */
 		if (adapter->cmd_resp_received) {
 			adapter->cmd_resp_received = false;
@@ -304,12 +310,6 @@ process_start:
 			}
 		}
 
-		/* Check for event */
-		if (adapter->event_received) {
-			adapter->event_received = false;
-			mwifiex_process_event(adapter);
-		}
-
 		/* Check if we need to confirm Sleep Request
 		   received previously */
 		if (adapter->ps_state == PS_STATE_PRE_SLEEP) {
diff --git a/drivers/net/wireless/mwifiex/sta_ioctl.c b/drivers/net/wireless/mwifiex/sta_ioctl.c
index 894270611f2..536c14aa71f 100644
--- a/drivers/net/wireless/mwifiex/sta_ioctl.c
+++ b/drivers/net/wireless/mwifiex/sta_ioctl.c
@@ -60,9 +60,10 @@ int mwifiex_wait_queue_complete(struct mwifiex_adapter *adapter,
 	int status;
 
 	/* Wait for completion */
-	status = wait_event_interruptible(adapter->cmd_wait_q.wait,
-					  *(cmd_queued->condition));
-	if (status) {
+	status = wait_event_interruptible_timeout(adapter->cmd_wait_q.wait,
+						  *(cmd_queued->condition),
+						  (12 * HZ));
+	if (status <= 0) {
 		dev_err(adapter->dev, "cmd_wait_q terminated: %d\n", status);
 		mwifiex_cancel_all_pending_cmd(adapter);
 		return status;
diff --git a/drivers/net/wireless/orinoco/orinoco_usb.c b/drivers/net/wireless/orinoco/orinoco_usb.c
index f9805c9353d..3ac71339d04 100644
--- a/drivers/net/wireless/orinoco/orinoco_usb.c
+++ b/drivers/net/wireless/orinoco/orinoco_usb.c
@@ -100,25 +100,11 @@ static struct ez_usb_fw firmware = {
 	.code = NULL,
 };
 
-#ifdef CONFIG_USB_DEBUG
-static int debug = 1;
-#else
-static int debug;
-#endif
-
 /* Debugging macros */
-#undef dbg
-#define dbg(format, arg...) \
-	do { if (debug) printk(KERN_DEBUG PFX "%s: " format "\n", \
-			       __func__ , ## arg); } while (0)
 #undef err
 #define err(format, arg...) \
 	do { printk(KERN_ERR PFX format "\n", ## arg); } while (0)
 
-/* Module paramaters */
-module_param(debug, int, 0644);
-MODULE_PARM_DESC(debug, "Debug enabled or not");
-
 MODULE_FIRMWARE("orinoco_ezusb_fw");
 
 /*
@@ -341,7 +327,7 @@ static void ezusb_request_timerfn(u_long _ctx)
 		ctx->state = EZUSB_CTX_REQ_TIMEOUT;
 	} else {
 		ctx->state = EZUSB_CTX_RESP_TIMEOUT;
-		dbg("couldn't unlink");
+		dev_dbg(&ctx->outurb->dev->dev, "couldn't unlink\n");
 		atomic_inc(&ctx->refcount);
 		ctx->killed = 1;
 		ezusb_ctx_complete(ctx);
@@ -634,9 +620,9 @@ static void ezusb_request_in_callback(struct ezusb_priv *upriv,
 				ctx = c;
 				break;
 			}
-			dbg("Skipped (0x%x/0x%x) (%d/%d)",
-			    le16_to_cpu(ans->hermes_rid),
-			    c->in_rid, ans->ans_reply_count, reply_count);
+			netdev_dbg(upriv->dev, "Skipped (0x%x/0x%x) (%d/%d)\n",
+				   le16_to_cpu(ans->hermes_rid), c->in_rid,
+				   ans->ans_reply_count, reply_count);
 		}
 	}
 
@@ -768,7 +754,7 @@ static int ezusb_submit_in_urb(struct ezusb_priv *upriv)
 	void *cur_buf = upriv->read_urb->transfer_buffer;
 
 	if (upriv->read_urb->status == -EINPROGRESS) {
-		dbg("urb busy, not resubmiting");
+		netdev_dbg(upriv->dev, "urb busy, not resubmiting\n");
 		retval = -EBUSY;
 		goto exit;
 	}
@@ -838,8 +824,9 @@ static int ezusb_firmware_download(struct ezusb_priv *upriv,
 		memcpy(fw_buffer, &fw->code[addr], FW_BUF_SIZE);
 		if (variant_offset >= addr &&
 		    variant_offset < addr + FW_BUF_SIZE) {
-			dbg("Patching card_variant byte at 0x%04X",
-			    variant_offset);
+			netdev_dbg(upriv->dev,
+				   "Patching card_variant byte at 0x%04X\n",
+				   variant_offset);
 			fw_buffer[variant_offset - addr] = FW_VAR_VALUE;
 		}
 		retval = usb_control_msg(upriv->udev,
@@ -879,7 +866,6 @@ static int ezusb_access_ltv(struct ezusb_priv *upriv,
 	BUG_ON(in_irq());
 
 	if (!upriv->udev) {
-		dbg("Device disconnected");
 		retval = -ENODEV;
 		goto exit;
 	}
@@ -1023,8 +1009,9 @@ static int ezusb_doicmd_wait(struct hermes *hw, u16 cmd, u16 parm0, u16 parm1,
 		cpu_to_le16(parm1),
 		cpu_to_le16(parm2),
 	};
-	dbg("0x%04X, parm0 0x%04X, parm1 0x%04X, parm2 0x%04X",
-	    cmd, parm0, parm1, parm2);
+	netdev_dbg(upriv->dev,
+		   "0x%04X, parm0 0x%04X, parm1 0x%04X, parm2 0x%04X\n", cmd,
+		   parm0, parm1, parm2);
 	ctx = ezusb_alloc_ctx(upriv, EZUSB_RID_DOCMD, EZUSB_RID_ACK);
 	if (!ctx)
 		return -ENOMEM;
@@ -1045,7 +1032,7 @@ static int ezusb_docmd_wait(struct hermes *hw, u16 cmd, u16 parm0,
 		0,
 		0,
 	};
-	dbg("0x%04X, parm0 0x%04X", cmd, parm0);
+	netdev_dbg(upriv->dev, "0x%04X, parm0 0x%04X\n", cmd, parm0);
 	ctx = ezusb_alloc_ctx(upriv, EZUSB_RID_DOCMD, EZUSB_RID_ACK);
 	if (!ctx)
 		return -ENOMEM;
@@ -1332,7 +1319,7 @@ static int ezusb_hard_reset(struct orinoco_private *priv)
 		return retval;
 	}
 
-	dbg("sending control message");
+	netdev_dbg(upriv->dev, "sending control message\n");
 	retval = usb_control_msg(upriv->udev,
 				 usb_sndctrlpipe(upriv->udev, 0),
 				 EZUSB_REQUEST_TRIGER,
@@ -1401,10 +1388,8 @@ static void ezusb_bulk_in_callback(struct urb *urb)
 	u16 crc;
 	u16 hermes_rid;
 
-	if (upriv->udev == NULL) {
-		dbg("disconnected");
+	if (upriv->udev == NULL)
 		return;
-	}
 
 	if (urb->status == -ETIMEDOUT) {
 		/* When a device gets unplugged we get this every time
@@ -1421,12 +1406,13 @@ static void ezusb_bulk_in_callback(struct urb *urb)
 	if ((urb->status == -EILSEQ)
 	    || (urb->status == -ENOENT)
 	    || (urb->status == -ECONNRESET)) {
-		dbg("status %d, not resubmiting", urb->status);
+		netdev_dbg(upriv->dev, "status %d, not resubmiting\n",
+			   urb->status);
 		return;
 	}
 	if (urb->status)
-		dbg("status: %d length: %d",
-		    urb->status, urb->actual_length);
+		netdev_dbg(upriv->dev, "status: %d length: %d\n",
+			   urb->status, urb->actual_length);
 	if (urb->actual_length < sizeof(*ans)) {
 		err("%s: short read, ignoring", __func__);
 		goto resubmit;
diff --git a/drivers/net/wireless/rsi/rsi_91x_core.c b/drivers/net/wireless/rsi/rsi_91x_core.c
index 1a8d3213859..cf61d6e3eaa 100644
--- a/drivers/net/wireless/rsi/rsi_91x_core.c
+++ b/drivers/net/wireless/rsi/rsi_91x_core.c
@@ -88,7 +88,7 @@ static u8 rsi_core_determine_hal_queue(struct rsi_common *common)
 	bool recontend_queue = false;
 	u32 q_len = 0;
 	u8 q_num = INVALID_QUEUE;
-	u8 ii, min = 0;
+	u8 ii = 0, min = 0;
 
 	if (skb_queue_len(&common->tx_queue[MGMT_SOFT_Q])) {
 		if (!common->mgmt_q_block)
diff --git a/drivers/net/wireless/rsi/rsi_91x_mgmt.c b/drivers/net/wireless/rsi/rsi_91x_mgmt.c
index 73694295648..1b28cda6ca8 100644
--- a/drivers/net/wireless/rsi/rsi_91x_mgmt.c
+++ b/drivers/net/wireless/rsi/rsi_91x_mgmt.c
@@ -841,16 +841,6 @@ int rsi_set_channel(struct rsi_common *common, u16 channel)
 	rsi_dbg(MGMT_TX_ZONE,
 		"%s: Sending scan req frame\n", __func__);
 
-	skb = dev_alloc_skb(FRAME_DESC_SZ);
-	if (!skb) {
-		rsi_dbg(ERR_ZONE, "%s: Failed in allocation of skb\n",
-			__func__);
-		return -ENOMEM;
-	}
-
-	memset(skb->data, 0, FRAME_DESC_SZ);
-	mgmt_frame = (struct rsi_mac_frame *)skb->data;
-
 	if (common->band == IEEE80211_BAND_5GHZ) {
 		if ((channel >= 36) && (channel <= 64))
 			channel = ((channel - 32) / 4);
@@ -868,6 +858,16 @@ int rsi_set_channel(struct rsi_common *common, u16 channel)
 		}
 	}
 
+	skb = dev_alloc_skb(FRAME_DESC_SZ);
+	if (!skb) {
+		rsi_dbg(ERR_ZONE, "%s: Failed in allocation of skb\n",
+			__func__);
+		return -ENOMEM;
+	}
+
+	memset(skb->data, 0, FRAME_DESC_SZ);
+	mgmt_frame = (struct rsi_mac_frame *)skb->data;
+
 	mgmt_frame->desc_word[0] = cpu_to_le16(RSI_WIFI_MGMT_Q << 12);
 	mgmt_frame->desc_word[1] = cpu_to_le16(SCAN_REQUEST);
 	mgmt_frame->desc_word[4] = cpu_to_le16(channel);
@@ -966,6 +966,7 @@ static int rsi_send_auto_rate_request(struct rsi_common *common)
 	if (!selected_rates) {
 		rsi_dbg(ERR_ZONE, "%s: Failed in allocation of mem\n",
 			__func__);
+		dev_kfree_skb(skb);
 		return -ENOMEM;
 	}
 
diff --git a/drivers/net/wireless/rt2x00/rt2x00mac.c b/drivers/net/wireless/rt2x00/rt2x00mac.c
index ddeb5a709aa..a87ee9b6585 100644
--- a/drivers/net/wireless/rt2x00/rt2x00mac.c
+++ b/drivers/net/wireless/rt2x00/rt2x00mac.c
@@ -621,20 +621,18 @@ void rt2x00mac_bss_info_changed(struct ieee80211_hw *hw,
 				      bss_conf->bssid);
 
 	/*
-	 * Update the beacon. This is only required on USB devices. PCI
-	 * devices fetch beacons periodically.
-	 */
-	if (changes & BSS_CHANGED_BEACON && rt2x00_is_usb(rt2x00dev))
-		rt2x00queue_update_beacon(rt2x00dev, vif);
-
-	/*
 	 * Start/stop beaconing.
 	 */
 	if (changes & BSS_CHANGED_BEACON_ENABLED) {
 		if (!bss_conf->enable_beacon && intf->enable_beacon) {
-			rt2x00queue_clear_beacon(rt2x00dev, vif);
 			rt2x00dev->intf_beaconing--;
 			intf->enable_beacon = false;
+			/*
+			 * Clear beacon in the H/W for this vif. This is needed
+			 * to disable beaconing on this particular interface
+			 * and keep it running on other interfaces.
+			 */
+			rt2x00queue_clear_beacon(rt2x00dev, vif);
 
 			if (rt2x00dev->intf_beaconing == 0) {
 				/*
@@ -645,11 +643,15 @@ void rt2x00mac_bss_info_changed(struct ieee80211_hw *hw,
 				rt2x00queue_stop_queue(rt2x00dev->bcn);
 				mutex_unlock(&intf->beacon_skb_mutex);
 			}
-
-
 		} else if (bss_conf->enable_beacon && !intf->enable_beacon) {
 			rt2x00dev->intf_beaconing++;
 			intf->enable_beacon = true;
+			/*
+			 * Upload beacon to the H/W. This is only required on
+			 * USB devices. PCI devices fetch beacons periodically.
+			 */
+			if (rt2x00_is_usb(rt2x00dev))
+				rt2x00queue_update_beacon(rt2x00dev, vif);
 
 			if (rt2x00dev->intf_beaconing == 1) {
 				/*
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/trx.c b/drivers/net/wireless/rtlwifi/rtl8188ee/trx.c
index 06ef47cd620..5b4c225396f 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/trx.c
@@ -293,7 +293,7 @@ static void _rtl88ee_translate_rx_signal_stuff(struct ieee80211_hw *hw,
 	u8 *psaddr;
 	__le16 fc;
 	u16 type, ufc;
-	bool match_bssid, packet_toself, packet_beacon, addr;
+	bool match_bssid, packet_toself, packet_beacon = false, addr;
 
 	tmp_buf = skb->data + pstatus->rx_drvinfo_size + pstatus->rx_bufshift;
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c b/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
index 68b5c7e92cf..07cb06da672 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
@@ -1001,7 +1001,7 @@ int rtl92cu_hw_init(struct ieee80211_hw *hw)
 	err = _rtl92cu_init_mac(hw);
 	if (err) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "init mac failed!\n");
-		return err;
+		goto exit;
 	}
 	err = rtl92c_download_fw(hw);
 	if (err) {
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/trx.c b/drivers/net/wireless/rtlwifi/rtl8192se/trx.c
index 36b48be8329..2b3c78baa9f 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/trx.c
@@ -49,6 +49,12 @@ static u8 _rtl92se_map_hwqueue_to_fwqueue(struct sk_buff *skb,	u8 skb_queue)
 	if (ieee80211_is_nullfunc(fc))
 		return QSLT_HIGH;
 
+	/* Kernel commit 1bf4bbb4024dcdab changed EAPOL packets to use
+	 * queue V0 at priority 7; however, the RTL8192SE appears to have
+	 * that queue at priority 6
+	 */
+	if (skb->priority == 7)
+		return QSLT_VO;
 	return skb->priority;
 }
 
diff --git a/drivers/net/wireless/ti/wl18xx/event.h b/drivers/net/wireless/ti/wl18xx/event.h
index 398f3d2c0a6..a76e98eb837 100644
--- a/drivers/net/wireless/ti/wl18xx/event.h
+++ b/drivers/net/wireless/ti/wl18xx/event.h
@@ -68,6 +68,26 @@ struct wl18xx_event_mailbox {
 
 	/* bitmap of inactive stations (by HLID) */
 	__le32 inactive_sta_bitmap;
+
+	/* rx BA win size indicated by RX_BA_WIN_SIZE_CHANGE_EVENT_ID */
+	u8 rx_ba_role_id;
+	u8 rx_ba_link_id;
+	u8 rx_ba_win_size;
+	u8 padding;
+
+	/* smart config */
+	u8 sc_ssid_len;
+	u8 sc_pwd_len;
+	u8 sc_token_len;
+	u8 padding1;
+	u8 sc_ssid[32];
+	u8 sc_pwd[32];
+	u8 sc_token[32];
+
+	/* smart config sync channel */
+	u8 sc_sync_channel;
+	u8 sc_sync_band;
+	u8 padding2[2];
 } __packed;
 
 int wl18xx_wait_for_event(struct wl1271 *wl, enum wlcore_wait_event event,
diff --git a/drivers/net/wireless/ti/wlcore/event.c b/drivers/net/wireless/ti/wlcore/event.c
index 1f9a36031b0..16d10281798 100644
--- a/drivers/net/wireless/ti/wlcore/event.c
+++ b/drivers/net/wireless/ti/wlcore/event.c
@@ -158,6 +158,11 @@ EXPORT_SYMBOL_GPL(wlcore_event_channel_switch);
 
 void wlcore_event_dummy_packet(struct wl1271 *wl)
 {
+	if (wl->plt) {
+		wl1271_info("Got DUMMY_PACKET event in PLT mode.  FW bug, ignoring.");
+		return;
+	}
+
 	wl1271_debug(DEBUG_EVENT, "DUMMY_PACKET_ID_EVENT_ID");
 	wl1271_tx_dummy_packet(wl);
 }
diff --git a/drivers/net/xen-netback/common.h b/drivers/net/xen-netback/common.h
index 630a3fcf65b..0d4a285cbd7 100644
--- a/drivers/net/xen-netback/common.h
+++ b/drivers/net/xen-netback/common.h
@@ -226,7 +226,7 @@ int xenvif_map_frontend_rings(struct xenvif *vif,
 			      grant_ref_t rx_ring_ref);
 
 /* Check for SKBs from frontend and schedule backend processing */
-void xenvif_check_rx_xenvif(struct xenvif *vif);
+void xenvif_napi_schedule_or_enable_events(struct xenvif *vif);
 
 /* Prevent the device from generating any further traffic. */
 void xenvif_carrier_off(struct xenvif *vif);
diff --git a/drivers/net/xen-netback/interface.c b/drivers/net/xen-netback/interface.c
index ef05c5c49d4..20e9defa106 100644
--- a/drivers/net/xen-netback/interface.c
+++ b/drivers/net/xen-netback/interface.c
@@ -75,32 +75,8 @@ static int xenvif_poll(struct napi_struct *napi, int budget)
 	work_done = xenvif_tx_action(vif, budget);
 
 	if (work_done < budget) {
-		int more_to_do = 0;
-		unsigned long flags;
-
-		/* It is necessary to disable IRQ before calling
-		 * RING_HAS_UNCONSUMED_REQUESTS. Otherwise we might
-		 * lose event from the frontend.
-		 *
-		 * Consider:
-		 *   RING_HAS_UNCONSUMED_REQUESTS
-		 *   <frontend generates event to trigger napi_schedule>
-		 *   __napi_complete
-		 *
-		 * This handler is still in scheduled state so the
-		 * event has no effect at all. After __napi_complete
-		 * this handler is descheduled and cannot get
-		 * scheduled again. We lose event in this case and the ring
-		 * will be completely stalled.
-		 */
-
-		local_irq_save(flags);
-
-		RING_FINAL_CHECK_FOR_REQUESTS(&vif->tx, more_to_do);
-		if (!more_to_do)
-			__napi_complete(napi);
-
-		local_irq_restore(flags);
+		napi_complete(napi);
+		xenvif_napi_schedule_or_enable_events(vif);
 	}
 
 	return work_done;
@@ -194,7 +170,7 @@ static void xenvif_up(struct xenvif *vif)
 	enable_irq(vif->tx_irq);
 	if (vif->tx_irq != vif->rx_irq)
 		enable_irq(vif->rx_irq);
-	xenvif_check_rx_xenvif(vif);
+	xenvif_napi_schedule_or_enable_events(vif);
 }
 
 static void xenvif_down(struct xenvif *vif)
diff --git a/drivers/net/xen-netback/netback.c b/drivers/net/xen-netback/netback.c
index 76665405c5a..7367208ee8c 100644
--- a/drivers/net/xen-netback/netback.c
+++ b/drivers/net/xen-netback/netback.c
@@ -104,7 +104,7 @@ static inline unsigned long idx_to_kaddr(struct xenvif *vif,
 
 /* Find the containing VIF's structure from a pointer in pending_tx_info array
  */
-static inline struct xenvif* ubuf_to_vif(struct ubuf_info *ubuf)
+static inline struct xenvif *ubuf_to_vif(const struct ubuf_info *ubuf)
 {
 	u16 pending_idx = ubuf->desc;
 	struct pending_tx_info *temp =
@@ -323,6 +323,35 @@ static void xenvif_gop_frag_copy(struct xenvif *vif, struct sk_buff *skb,
 }
 
 /*
+ * Find the grant ref for a given frag in a chain of struct ubuf_info's
+ * skb: the skb itself
+ * i: the frag's number
+ * ubuf: a pointer to an element in the chain. It should not be NULL
+ *
+ * Returns a pointer to the element in the chain where the page were found. If
+ * not found, returns NULL.
+ * See the definition of callback_struct in common.h for more details about
+ * the chain.
+ */
+static const struct ubuf_info *xenvif_find_gref(const struct sk_buff *const skb,
+						const int i,
+						const struct ubuf_info *ubuf)
+{
+	struct xenvif *foreign_vif = ubuf_to_vif(ubuf);
+
+	do {
+		u16 pending_idx = ubuf->desc;
+
+		if (skb_shinfo(skb)->frags[i].page.p ==
+		    foreign_vif->mmap_pages[pending_idx])
+			break;
+		ubuf = (struct ubuf_info *) ubuf->ctx;
+	} while (ubuf);
+
+	return ubuf;
+}
+
+/*
  * Prepare an SKB to be transmitted to the frontend.
  *
  * This function is responsible for allocating grant operations, meta
@@ -346,9 +375,8 @@ static int xenvif_gop_skb(struct sk_buff *skb,
 	int head = 1;
 	int old_meta_prod;
 	int gso_type;
-	struct ubuf_info *ubuf = skb_shinfo(skb)->destructor_arg;
-	grant_ref_t foreign_grefs[MAX_SKB_FRAGS];
-	struct xenvif *foreign_vif = NULL;
+	const struct ubuf_info *ubuf = skb_shinfo(skb)->destructor_arg;
+	const struct ubuf_info *const head_ubuf = ubuf;
 
 	old_meta_prod = npo->meta_prod;
 
@@ -386,19 +414,6 @@ static int xenvif_gop_skb(struct sk_buff *skb,
 	npo->copy_off = 0;
 	npo->copy_gref = req->gref;
 
-	if ((skb_shinfo(skb)->tx_flags & SKBTX_DEV_ZEROCOPY) &&
-		 (ubuf->callback == &xenvif_zerocopy_callback)) {
-		int i = 0;
-		foreign_vif = ubuf_to_vif(ubuf);
-
-		do {
-			u16 pending_idx = ubuf->desc;
-			foreign_grefs[i++] =
-				foreign_vif->pending_tx_info[pending_idx].req.gref;
-			ubuf = (struct ubuf_info *) ubuf->ctx;
-		} while (ubuf);
-	}
-
 	data = skb->data;
 	while (data < skb_tail_pointer(skb)) {
 		unsigned int offset = offset_in_page(data);
@@ -415,13 +430,60 @@ static int xenvif_gop_skb(struct sk_buff *skb,
 	}
 
 	for (i = 0; i < nr_frags; i++) {
+		/* This variable also signals whether foreign_gref has a real
+		 * value or not.
+		 */
+		struct xenvif *foreign_vif = NULL;
+		grant_ref_t foreign_gref;
+
+		if ((skb_shinfo(skb)->tx_flags & SKBTX_DEV_ZEROCOPY) &&
+			(ubuf->callback == &xenvif_zerocopy_callback)) {
+			const struct ubuf_info *const startpoint = ubuf;
+
+			/* Ideally ubuf points to the chain element which
+			 * belongs to this frag. Or if frags were removed from
+			 * the beginning, then shortly before it.
+			 */
+			ubuf = xenvif_find_gref(skb, i, ubuf);
+
+			/* Try again from the beginning of the list, if we
+			 * haven't tried from there. This only makes sense in
+			 * the unlikely event of reordering the original frags.
+			 * For injected local pages it's an unnecessary second
+			 * run.
+			 */
+			if (unlikely(!ubuf) && startpoint != head_ubuf)
+				ubuf = xenvif_find_gref(skb, i, head_ubuf);
+
+			if (likely(ubuf)) {
+				u16 pending_idx = ubuf->desc;
+
+				foreign_vif = ubuf_to_vif(ubuf);
+				foreign_gref = foreign_vif->pending_tx_info[pending_idx].req.gref;
+				/* Just a safety measure. If this was the last
+				 * element on the list, the for loop will
+				 * iterate again if a local page were added to
+				 * the end. Using head_ubuf here prevents the
+				 * second search on the chain. Or the original
+				 * frags changed order, but that's less likely.
+				 * In any way, ubuf shouldn't be NULL.
+				 */
+				ubuf = ubuf->ctx ?
+					(struct ubuf_info *) ubuf->ctx :
+					head_ubuf;
+			} else
+				/* This frag was a local page, added to the
+				 * array after the skb left netback.
+				 */
+				ubuf = head_ubuf;
+		}
 		xenvif_gop_frag_copy(vif, skb, npo,
 				     skb_frag_page(&skb_shinfo(skb)->frags[i]),
 				     skb_frag_size(&skb_shinfo(skb)->frags[i]),
 				     skb_shinfo(skb)->frags[i].page_offset,
 				     &head,
 				     foreign_vif,
-				     foreign_grefs[i]);
+				     foreign_vif ? foreign_gref : UINT_MAX);
 	}
 
 	return npo->meta_prod - old_meta_prod;
@@ -654,7 +716,7 @@ done:
 		notify_remote_via_irq(vif->rx_irq);
 }
 
-void xenvif_check_rx_xenvif(struct xenvif *vif)
+void xenvif_napi_schedule_or_enable_events(struct xenvif *vif)
 {
 	int more_to_do;
 
@@ -688,7 +750,7 @@ static void tx_credit_callback(unsigned long data)
 {
 	struct xenvif *vif = (struct xenvif *)data;
 	tx_add_credit(vif);
-	xenvif_check_rx_xenvif(vif);
+	xenvif_napi_schedule_or_enable_events(vif);
 }
 
 static void xenvif_tx_err(struct xenvif *vif,
diff --git a/drivers/of/base.c b/drivers/of/base.c
index f72d19b7e5d..32e969d9531 100644
--- a/drivers/of/base.c
+++ b/drivers/of/base.c
@@ -1828,17 +1828,17 @@ int of_update_property(struct device_node *np, struct property *newprop)
 		next = &(*next)->next;
 	}
 	raw_spin_unlock_irqrestore(&devtree_lock, flags);
-	if (rc)
-		return rc;
+	if (!found)
+		return -ENODEV;
+
+	/* At early boot, bail out and defer setup to of_init() */
+	if (!of_kset)
+		return found ? 0 : -ENODEV;
 
 	/* Update the sysfs attribute */
-	if (oldprop)
-		sysfs_remove_bin_file(&np->kobj, &oldprop->attr);
+	sysfs_remove_bin_file(&np->kobj, &oldprop->attr);
 	__of_add_property_sysfs(np, newprop);
 
-	if (!found)
-		return -ENODEV;
-
 	return 0;
 }
 
diff --git a/drivers/of/fdt.c b/drivers/of/fdt.c
index fa16a912a92..7a2ef7bb802 100644
--- a/drivers/of/fdt.c
+++ b/drivers/of/fdt.c
@@ -491,7 +491,7 @@ static int __init __reserved_mem_reserve_reg(unsigned long node,
  * in /reserved-memory matches the values supported by the current implementation,
  * also check if ranges property has been provided
  */
-static int __reserved_mem_check_root(unsigned long node)
+static int __init __reserved_mem_check_root(unsigned long node)
 {
 	__be32 *prop;
 
diff --git a/drivers/of/irq.c b/drivers/of/irq.c
index 9bcf2cf1935..5aeb8941135 100644
--- a/drivers/of/irq.c
+++ b/drivers/of/irq.c
@@ -364,7 +364,7 @@ int of_irq_to_resource(struct device_node *dev, int index, struct resource *r)
 
 		memset(r, 0, sizeof(*r));
 		/*
-		 * Get optional "interrupts-names" property to add a name
+		 * Get optional "interrupt-names" property to add a name
 		 * to the resource.
 		 */
 		of_property_read_string_index(dev, "interrupt-names", index,
@@ -380,6 +380,32 @@ int of_irq_to_resource(struct device_node *dev, int index, struct resource *r)
 EXPORT_SYMBOL_GPL(of_irq_to_resource);
 
 /**
+ * of_irq_get - Decode a node's IRQ and return it as a Linux irq number
+ * @dev: pointer to device tree node
+ * @index: zero-based index of the irq
+ *
+ * Returns Linux irq number on success, or -EPROBE_DEFER if the irq domain
+ * is not yet created.
+ *
+ */
+int of_irq_get(struct device_node *dev, int index)
+{
+	int rc;
+	struct of_phandle_args oirq;
+	struct irq_domain *domain;
+
+	rc = of_irq_parse_one(dev, index, &oirq);
+	if (rc)
+		return rc;
+
+	domain = irq_find_host(oirq.np);
+	if (!domain)
+		return -EPROBE_DEFER;
+
+	return irq_create_of_mapping(&oirq);
+}
+
+/**
  * of_irq_count - Count the number of IRQs a node uses
  * @dev: pointer to device tree node
  */
diff --git a/drivers/of/platform.c b/drivers/of/platform.c
index 404d1daebef..e8376d646d9 100644
--- a/drivers/of/platform.c
+++ b/drivers/of/platform.c
@@ -168,7 +168,9 @@ struct platform_device *of_device_alloc(struct device_node *np,
 			rc = of_address_to_resource(np, i, res);
 			WARN_ON(rc);
 		}
-		WARN_ON(of_irq_to_resource_table(np, res, num_irq) != num_irq);
+		if (of_irq_to_resource_table(np, res, num_irq) != num_irq)
+			pr_debug("not all legacy IRQ resources mapped for %s\n",
+				 np->name);
 	}
 
 	dev->dev.of_node = of_node_get(np);
@@ -204,12 +206,13 @@ static struct platform_device *of_platform_device_create_pdata(
 {
 	struct platform_device *dev;
 
-	if (!of_device_is_available(np))
+	if (!of_device_is_available(np) ||
+	    of_node_test_and_set_flag(np, OF_POPULATED))
 		return NULL;
 
 	dev = of_device_alloc(np, bus_id, parent);
 	if (!dev)
-		return NULL;
+		goto err_clear_flag;
 
 #if defined(CONFIG_MICROBLAZE)
 	dev->archdata.dma_mask = 0xffffffffUL;
@@ -227,10 +230,14 @@ static struct platform_device *of_platform_device_create_pdata(
 
 	if (of_device_add(dev) != 0) {
 		platform_device_put(dev);
-		return NULL;
+		goto err_clear_flag;
 	}
 
 	return dev;
+
+err_clear_flag:
+	of_node_clear_flag(np, OF_POPULATED);
+	return NULL;
 }
 
 /**
@@ -262,14 +269,15 @@ static struct amba_device *of_amba_device_create(struct device_node *node,
 
 	pr_debug("Creating amba device %s\n", node->full_name);
 
-	if (!of_device_is_available(node))
+	if (!of_device_is_available(node) ||
+	    of_node_test_and_set_flag(node, OF_POPULATED))
 		return NULL;
 
 	dev = amba_device_alloc(NULL, 0, 0);
 	if (!dev) {
 		pr_err("%s(): amba_device_alloc() failed for %s\n",
 		       __func__, node->full_name);
-		return NULL;
+		goto err_clear_flag;
 	}
 
 	/* setup generic device info */
@@ -309,6 +317,8 @@ static struct amba_device *of_amba_device_create(struct device_node *node,
 
 err_free:
 	amba_device_put(dev);
+err_clear_flag:
+	of_node_clear_flag(node, OF_POPULATED);
 	return NULL;
 }
 #else /* CONFIG_ARM_AMBA */
@@ -485,4 +495,60 @@ int of_platform_populate(struct device_node *root,
 	return rc;
 }
 EXPORT_SYMBOL_GPL(of_platform_populate);
+
+static int of_platform_device_destroy(struct device *dev, void *data)
+{
+	bool *children_left = data;
+
+	/* Do not touch devices not populated from the device tree */
+	if (!dev->of_node || !of_node_check_flag(dev->of_node, OF_POPULATED)) {
+		*children_left = true;
+		return 0;
+	}
+
+	/* Recurse, but don't touch this device if it has any children left */
+	if (of_platform_depopulate(dev) != 0) {
+		*children_left = true;
+		return 0;
+	}
+
+	if (dev->bus == &platform_bus_type)
+		platform_device_unregister(to_platform_device(dev));
+#ifdef CONFIG_ARM_AMBA
+	else if (dev->bus == &amba_bustype)
+		amba_device_unregister(to_amba_device(dev));
+#endif
+	else {
+		*children_left = true;
+		return 0;
+	}
+
+	of_node_clear_flag(dev->of_node, OF_POPULATED);
+
+	return 0;
+}
+
+/**
+ * of_platform_depopulate() - Remove devices populated from device tree
+ * @parent: device which childred will be removed
+ *
+ * Complementary to of_platform_populate(), this function removes children
+ * of the given device (and, recurrently, their children) that have been
+ * created from their respective device tree nodes (and only those,
+ * leaving others - eg. manually created - unharmed).
+ *
+ * Returns 0 when all children devices have been removed or
+ * -EBUSY when some children remained.
+ */
+int of_platform_depopulate(struct device *parent)
+{
+	bool children_left = false;
+
+	device_for_each_child(parent, &children_left,
+			      of_platform_device_destroy);
+
+	return children_left ? -EBUSY : 0;
+}
+EXPORT_SYMBOL_GPL(of_platform_depopulate);
+
 #endif /* CONFIG_OF_ADDRESS */
diff --git a/drivers/of/selftest.c b/drivers/of/selftest.c
index ae445007050..fe70b86bcff 100644
--- a/drivers/of/selftest.c
+++ b/drivers/of/selftest.c
@@ -10,6 +10,7 @@
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/of_irq.h>
+#include <linux/of_platform.h>
 #include <linux/list.h>
 #include <linux/mutex.h>
 #include <linux/slab.h>
@@ -427,6 +428,36 @@ static void __init of_selftest_match_node(void)
 	}
 }
 
+static void __init of_selftest_platform_populate(void)
+{
+	int irq;
+	struct device_node *np;
+	struct platform_device *pdev;
+
+	np = of_find_node_by_path("/testcase-data");
+	of_platform_populate(np, of_default_bus_match_table, NULL, NULL);
+
+	/* Test that a missing irq domain returns -EPROBE_DEFER */
+	np = of_find_node_by_path("/testcase-data/testcase-device1");
+	pdev = of_find_device_by_node(np);
+	if (!pdev)
+		selftest(0, "device 1 creation failed\n");
+	irq = platform_get_irq(pdev, 0);
+	if (irq != -EPROBE_DEFER)
+		selftest(0, "device deferred probe failed - %d\n", irq);
+
+	/* Test that a parsing failure does not return -EPROBE_DEFER */
+	np = of_find_node_by_path("/testcase-data/testcase-device2");
+	pdev = of_find_device_by_node(np);
+	if (!pdev)
+		selftest(0, "device 2 creation failed\n");
+	irq = platform_get_irq(pdev, 0);
+	if (irq >= 0 || irq == -EPROBE_DEFER)
+		selftest(0, "device parsing error failed - %d\n", irq);
+
+	selftest(1, "passed");
+}
+
 static int __init of_selftest(void)
 {
 	struct device_node *np;
@@ -445,6 +476,7 @@ static int __init of_selftest(void)
 	of_selftest_parse_interrupts();
 	of_selftest_parse_interrupts_extended();
 	of_selftest_match_node();
+	of_selftest_platform_populate();
 	pr_info("end of selftest - %i passed, %i failed\n",
 		selftest_results.passed, selftest_results.failed);
 	return 0;
diff --git a/drivers/of/testcase-data/tests-interrupts.dtsi b/drivers/of/testcase-data/tests-interrupts.dtsi
index c843720bd3e..da4695f6035 100644
--- a/drivers/of/testcase-data/tests-interrupts.dtsi
+++ b/drivers/of/testcase-data/tests-interrupts.dtsi
@@ -54,5 +54,18 @@
 						      <&test_intmap1 1 2>;
 			};
 		};
+
+		testcase-device1 {
+			compatible = "testcase-device";
+			interrupt-parent = <&test_intc0>;
+			interrupts = <1>;
+		};
+
+		testcase-device2 {
+			compatible = "testcase-device";
+			interrupt-parent = <&test_intc2>;
+			interrupts = <1>; /* invalid specifier - too short */
+		};
 	};
+
 };
diff --git a/drivers/parport/parport_serial.c b/drivers/parport/parport_serial.c
index ff53314100f..ee932004724 100644
--- a/drivers/parport/parport_serial.c
+++ b/drivers/parport/parport_serial.c
@@ -62,6 +62,7 @@ enum parport_pc_pci_cards {
 	timedia_9079a,
 	timedia_9079b,
 	timedia_9079c,
+	wch_ch353_1s1p,
 	wch_ch353_2s1p,
 	sunix_2s1p,
 };
@@ -148,6 +149,7 @@ static struct parport_pc_pci cards[] = {
 	/* timedia_9079a */             { 1, { { 2, 3 }, } },
 	/* timedia_9079b */             { 1, { { 2, 3 }, } },
 	/* timedia_9079c */             { 1, { { 2, 3 }, } },
+	/* wch_ch353_1s1p*/             { 1, { { 1, -1}, } },
 	/* wch_ch353_2s1p*/             { 1, { { 2, -1}, } },
 	/* sunix_2s1p */                { 1, { { 3, -1 }, } },
 };
@@ -253,6 +255,7 @@ static struct pci_device_id parport_serial_pci_tbl[] = {
 	{ 0x1409, 0x7168, 0x1409, 0xd079, 0, 0, timedia_9079c },
 
 	/* WCH CARDS */
+	{ 0x4348, 0x5053, PCI_ANY_ID, PCI_ANY_ID, 0, 0, wch_ch353_1s1p},
 	{ 0x4348, 0x7053, 0x4348, 0x3253, 0, 0, wch_ch353_2s1p},
 
 	/*
@@ -479,6 +482,12 @@ static struct pciserial_board pci_parport_serial_boards[] = {
 		.base_baud	= 921600,
 		.uart_offset	= 8,
 	},
+	[wch_ch353_1s1p] = {
+		.flags          = FL_BASE0|FL_BASE_BARS,
+		.num_ports      = 1,
+		.base_baud      = 115200,
+		.uart_offset    = 8,
+	},
 	[wch_ch353_2s1p] = {
 		.flags          = FL_BASE0|FL_BASE_BARS,
 		.num_ports      = 2,
diff --git a/drivers/pci/access.c b/drivers/pci/access.c
index 7f8b78c0887..8c148f39e8d 100644
--- a/drivers/pci/access.c
+++ b/drivers/pci/access.c
@@ -148,7 +148,7 @@ static noinline void pci_wait_cfg(struct pci_dev *dev)
 int pci_user_read_config_##size						\
 	(struct pci_dev *dev, int pos, type *val)			\
 {									\
-	int ret = 0;							\
+	int ret = PCIBIOS_SUCCESSFUL;					\
 	u32 data = -1;							\
 	if (PCI_##size##_BAD)						\
 		return -EINVAL;						\
@@ -159,9 +159,7 @@ int pci_user_read_config_##size						\
 					pos, sizeof(type), &data);	\
 	raw_spin_unlock_irq(&pci_lock);				\
 	*val = (type)data;						\
-	if (ret > 0)							\
-		ret = -EINVAL;						\
-	return ret;							\
+	return pcibios_err_to_errno(ret);				\
 }									\
 EXPORT_SYMBOL_GPL(pci_user_read_config_##size);
 
@@ -170,7 +168,7 @@ EXPORT_SYMBOL_GPL(pci_user_read_config_##size);
 int pci_user_write_config_##size					\
 	(struct pci_dev *dev, int pos, type val)			\
 {									\
-	int ret = -EIO;							\
+	int ret = PCIBIOS_SUCCESSFUL;					\
 	if (PCI_##size##_BAD)						\
 		return -EINVAL;						\
 	raw_spin_lock_irq(&pci_lock);				\
@@ -179,9 +177,7 @@ int pci_user_write_config_##size					\
 	ret = dev->bus->ops->write(dev->bus, dev->devfn,		\
 					pos, sizeof(type), val);	\
 	raw_spin_unlock_irq(&pci_lock);				\
-	if (ret > 0)							\
-		ret = -EINVAL;						\
-	return ret;							\
+	return pcibios_err_to_errno(ret);				\
 }									\
 EXPORT_SYMBOL_GPL(pci_user_write_config_##size);
 
diff --git a/drivers/pci/bus.c b/drivers/pci/bus.c
index fb8aed307c2..447d393725e 100644
--- a/drivers/pci/bus.c
+++ b/drivers/pci/bus.c
@@ -13,7 +13,6 @@
 #include <linux/errno.h>
 #include <linux/ioport.h>
 #include <linux/proc_fs.h>
-#include <linux/init.h>
 #include <linux/slab.h>
 
 #include "pci.h"
@@ -236,7 +235,7 @@ void __weak pcibios_resource_survey_bus(struct pci_bus *bus) { }
  *
  * This adds add sysfs entries and start device drivers
  */
-int pci_bus_add_device(struct pci_dev *dev)
+void pci_bus_add_device(struct pci_dev *dev)
 {
 	int retval;
 
@@ -253,8 +252,6 @@ int pci_bus_add_device(struct pci_dev *dev)
 	WARN_ON(retval < 0);
 
 	dev->is_added = 1;
-
-	return 0;
 }
 
 /**
@@ -267,16 +264,12 @@ void pci_bus_add_devices(const struct pci_bus *bus)
 {
 	struct pci_dev *dev;
 	struct pci_bus *child;
-	int retval;
 
 	list_for_each_entry(dev, &bus->devices, bus_list) {
 		/* Skip already-added devices */
 		if (dev->is_added)
 			continue;
-		retval = pci_bus_add_device(dev);
-		if (retval)
-			dev_err(&dev->dev, "Error adding device (%d)\n",
-				retval);
+		pci_bus_add_device(dev);
 	}
 
 	list_for_each_entry(dev, &bus->devices, bus_list) {
diff --git a/drivers/pci/host-bridge.c b/drivers/pci/host-bridge.c
index 47aaf22d814..0e5f3c95af5 100644
--- a/drivers/pci/host-bridge.c
+++ b/drivers/pci/host-bridge.c
@@ -3,7 +3,6 @@
  */
 
 #include <linux/kernel.h>
-#include <linux/init.h>
 #include <linux/pci.h>
 #include <linux/module.h>
 
diff --git a/drivers/pci/host/Kconfig b/drivers/pci/host/Kconfig
index a6f67ec8882..21df477be0c 100644
--- a/drivers/pci/host/Kconfig
+++ b/drivers/pci/host/Kconfig
@@ -33,4 +33,17 @@ config PCI_RCAR_GEN2
 	  There are 3 internal PCI controllers available with a single
 	  built-in EHCI/OHCI host controller present on each one.
 
+config PCI_RCAR_GEN2_PCIE
+	bool "Renesas R-Car PCIe controller"
+	depends on ARCH_SHMOBILE || (ARM && COMPILE_TEST)
+	help
+	  Say Y here if you want PCIe controller support on R-Car Gen2 SoCs.
+
+config PCI_HOST_GENERIC
+	bool "Generic PCI host controller"
+	depends on ARM && OF
+	help
+	  Say Y here if you want to support a simple generic PCI host
+	  controller, such as the one emulated by kvmtool.
+
 endmenu
diff --git a/drivers/pci/host/Makefile b/drivers/pci/host/Makefile
index 13fb3333aa0..611ba4b48c9 100644
--- a/drivers/pci/host/Makefile
+++ b/drivers/pci/host/Makefile
@@ -4,3 +4,5 @@ obj-$(CONFIG_PCI_IMX6) += pci-imx6.o
 obj-$(CONFIG_PCI_MVEBU) += pci-mvebu.o
 obj-$(CONFIG_PCI_TEGRA) += pci-tegra.o
 obj-$(CONFIG_PCI_RCAR_GEN2) += pci-rcar-gen2.o
+obj-$(CONFIG_PCI_RCAR_GEN2_PCIE) += pcie-rcar.o
+obj-$(CONFIG_PCI_HOST_GENERIC) += pci-host-generic.o
diff --git a/drivers/pci/host/pci-exynos.c b/drivers/pci/host/pci-exynos.c
index 3de6bfbbe8e..1632661c5b7 100644
--- a/drivers/pci/host/pci-exynos.c
+++ b/drivers/pci/host/pci-exynos.c
@@ -415,9 +415,7 @@ static irqreturn_t exynos_pcie_msi_irq_handler(int irq, void *arg)
 {
 	struct pcie_port *pp = arg;
 
-	dw_handle_msi_irq(pp);
-
-	return IRQ_HANDLED;
+	return dw_handle_msi_irq(pp);
 }
 
 static void exynos_pcie_msi_init(struct pcie_port *pp)
@@ -511,7 +509,8 @@ static struct pcie_host_ops exynos_pcie_host_ops = {
 	.host_init = exynos_pcie_host_init,
 };
 
-static int add_pcie_port(struct pcie_port *pp, struct platform_device *pdev)
+static int __init add_pcie_port(struct pcie_port *pp,
+				struct platform_device *pdev)
 {
 	int ret;
 
@@ -568,10 +567,8 @@ static int __init exynos_pcie_probe(struct platform_device *pdev)
 
 	exynos_pcie = devm_kzalloc(&pdev->dev, sizeof(*exynos_pcie),
 				GFP_KERNEL);
-	if (!exynos_pcie) {
-		dev_err(&pdev->dev, "no memory for exynos pcie\n");
+	if (!exynos_pcie)
 		return -ENOMEM;
-	}
 
 	pp = &exynos_pcie->pp;
 
diff --git a/drivers/pci/host/pci-host-generic.c b/drivers/pci/host/pci-host-generic.c
new file mode 100644
index 00000000000..44fe6aa6a43
--- /dev/null
+++ b/drivers/pci/host/pci-host-generic.c
@@ -0,0 +1,388 @@
+/*
+ * Simple, generic PCI host controller driver targetting firmware-initialised
+ * systems and virtual machines (e.g. the PCI emulation provided by kvmtool).
+ *
+ * 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/>.
+ *
+ * Copyright (C) 2014 ARM Limited
+ *
+ * Author: Will Deacon <will.deacon@arm.com>
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of_address.h>
+#include <linux/of_pci.h>
+#include <linux/platform_device.h>
+
+struct gen_pci_cfg_bus_ops {
+	u32 bus_shift;
+	void __iomem *(*map_bus)(struct pci_bus *, unsigned int, int);
+};
+
+struct gen_pci_cfg_windows {
+	struct resource				res;
+	struct resource				bus_range;
+	void __iomem				**win;
+
+	const struct gen_pci_cfg_bus_ops	*ops;
+};
+
+struct gen_pci {
+	struct pci_host_bridge			host;
+	struct gen_pci_cfg_windows		cfg;
+	struct list_head			resources;
+};
+
+static void __iomem *gen_pci_map_cfg_bus_cam(struct pci_bus *bus,
+					     unsigned int devfn,
+					     int where)
+{
+	struct pci_sys_data *sys = bus->sysdata;
+	struct gen_pci *pci = sys->private_data;
+	resource_size_t idx = bus->number - pci->cfg.bus_range.start;
+
+	return pci->cfg.win[idx] + ((devfn << 8) | where);
+}
+
+static struct gen_pci_cfg_bus_ops gen_pci_cfg_cam_bus_ops = {
+	.bus_shift	= 16,
+	.map_bus	= gen_pci_map_cfg_bus_cam,
+};
+
+static void __iomem *gen_pci_map_cfg_bus_ecam(struct pci_bus *bus,
+					      unsigned int devfn,
+					      int where)
+{
+	struct pci_sys_data *sys = bus->sysdata;
+	struct gen_pci *pci = sys->private_data;
+	resource_size_t idx = bus->number - pci->cfg.bus_range.start;
+
+	return pci->cfg.win[idx] + ((devfn << 12) | where);
+}
+
+static struct gen_pci_cfg_bus_ops gen_pci_cfg_ecam_bus_ops = {
+	.bus_shift	= 20,
+	.map_bus	= gen_pci_map_cfg_bus_ecam,
+};
+
+static int gen_pci_config_read(struct pci_bus *bus, unsigned int devfn,
+				int where, int size, u32 *val)
+{
+	void __iomem *addr;
+	struct pci_sys_data *sys = bus->sysdata;
+	struct gen_pci *pci = sys->private_data;
+
+	addr = pci->cfg.ops->map_bus(bus, devfn, where);
+
+	switch (size) {
+	case 1:
+		*val = readb(addr);
+		break;
+	case 2:
+		*val = readw(addr);
+		break;
+	default:
+		*val = readl(addr);
+	}
+
+	return PCIBIOS_SUCCESSFUL;
+}
+
+static int gen_pci_config_write(struct pci_bus *bus, unsigned int devfn,
+				 int where, int size, u32 val)
+{
+	void __iomem *addr;
+	struct pci_sys_data *sys = bus->sysdata;
+	struct gen_pci *pci = sys->private_data;
+
+	addr = pci->cfg.ops->map_bus(bus, devfn, where);
+
+	switch (size) {
+	case 1:
+		writeb(val, addr);
+		break;
+	case 2:
+		writew(val, addr);
+		break;
+	default:
+		writel(val, addr);
+	}
+
+	return PCIBIOS_SUCCESSFUL;
+}
+
+static struct pci_ops gen_pci_ops = {
+	.read	= gen_pci_config_read,
+	.write	= gen_pci_config_write,
+};
+
+static const struct of_device_id gen_pci_of_match[] = {
+	{ .compatible = "pci-host-cam-generic",
+	  .data = &gen_pci_cfg_cam_bus_ops },
+
+	{ .compatible = "pci-host-ecam-generic",
+	  .data = &gen_pci_cfg_ecam_bus_ops },
+
+	{ },
+};
+MODULE_DEVICE_TABLE(of, gen_pci_of_match);
+
+static int gen_pci_calc_io_offset(struct device *dev,
+				  struct of_pci_range *range,
+				  struct resource *res,
+				  resource_size_t *offset)
+{
+	static atomic_t wins = ATOMIC_INIT(0);
+	int err, idx, max_win;
+	unsigned int window;
+
+	if (!PAGE_ALIGNED(range->cpu_addr))
+		return -EINVAL;
+
+	max_win = (IO_SPACE_LIMIT + 1) / SZ_64K;
+	idx = atomic_inc_return(&wins);
+	if (idx > max_win)
+		return -ENOSPC;
+
+	window = (idx - 1) * SZ_64K;
+	err = pci_ioremap_io(window, range->cpu_addr);
+	if (err)
+		return err;
+
+	of_pci_range_to_resource(range, dev->of_node, res);
+	res->start = window;
+	res->end = res->start + range->size - 1;
+	*offset = window - range->pci_addr;
+	return 0;
+}
+
+static int gen_pci_calc_mem_offset(struct device *dev,
+				   struct of_pci_range *range,
+				   struct resource *res,
+				   resource_size_t *offset)
+{
+	of_pci_range_to_resource(range, dev->of_node, res);
+	*offset = range->cpu_addr - range->pci_addr;
+	return 0;
+}
+
+static void gen_pci_release_of_pci_ranges(struct gen_pci *pci)
+{
+	struct pci_host_bridge_window *win;
+
+	list_for_each_entry(win, &pci->resources, list)
+		release_resource(win->res);
+
+	pci_free_resource_list(&pci->resources);
+}
+
+static int gen_pci_parse_request_of_pci_ranges(struct gen_pci *pci)
+{
+	struct of_pci_range range;
+	struct of_pci_range_parser parser;
+	int err, res_valid = 0;
+	struct device *dev = pci->host.dev.parent;
+	struct device_node *np = dev->of_node;
+
+	if (of_pci_range_parser_init(&parser, np)) {
+		dev_err(dev, "missing \"ranges\" property\n");
+		return -EINVAL;
+	}
+
+	for_each_of_pci_range(&parser, &range) {
+		struct resource *parent, *res;
+		resource_size_t offset;
+		u32 restype = range.flags & IORESOURCE_TYPE_BITS;
+
+		res = devm_kmalloc(dev, sizeof(*res), GFP_KERNEL);
+		if (!res) {
+			err = -ENOMEM;
+			goto out_release_res;
+		}
+
+		switch (restype) {
+		case IORESOURCE_IO:
+			parent = &ioport_resource;
+			err = gen_pci_calc_io_offset(dev, &range, res, &offset);
+			break;
+		case IORESOURCE_MEM:
+			parent = &iomem_resource;
+			err = gen_pci_calc_mem_offset(dev, &range, res, &offset);
+			res_valid |= !(res->flags & IORESOURCE_PREFETCH || err);
+			break;
+		default:
+			err = -EINVAL;
+			continue;
+		}
+
+		if (err) {
+			dev_warn(dev,
+				 "error %d: failed to add resource [type 0x%x, %lld bytes]\n",
+				 err, restype, range.size);
+			continue;
+		}
+
+		err = request_resource(parent, res);
+		if (err)
+			goto out_release_res;
+
+		pci_add_resource_offset(&pci->resources, res, offset);
+	}
+
+	if (!res_valid) {
+		dev_err(dev, "non-prefetchable memory resource required\n");
+		err = -EINVAL;
+		goto out_release_res;
+	}
+
+	return 0;
+
+out_release_res:
+	gen_pci_release_of_pci_ranges(pci);
+	return err;
+}
+
+static int gen_pci_parse_map_cfg_windows(struct gen_pci *pci)
+{
+	int err;
+	u8 bus_max;
+	resource_size_t busn;
+	struct resource *bus_range;
+	struct device *dev = pci->host.dev.parent;
+	struct device_node *np = dev->of_node;
+
+	if (of_pci_parse_bus_range(np, &pci->cfg.bus_range))
+		pci->cfg.bus_range = (struct resource) {
+			.name	= np->name,
+			.start	= 0,
+			.end	= 0xff,
+			.flags	= IORESOURCE_BUS,
+		};
+
+	err = of_address_to_resource(np, 0, &pci->cfg.res);
+	if (err) {
+		dev_err(dev, "missing \"reg\" property\n");
+		return err;
+	}
+
+	pci->cfg.win = devm_kcalloc(dev, resource_size(&pci->cfg.bus_range),
+				    sizeof(*pci->cfg.win), GFP_KERNEL);
+	if (!pci->cfg.win)
+		return -ENOMEM;
+
+	/* Limit the bus-range to fit within reg */
+	bus_max = pci->cfg.bus_range.start +
+		  (resource_size(&pci->cfg.res) >> pci->cfg.ops->bus_shift) - 1;
+	pci->cfg.bus_range.end = min_t(resource_size_t, pci->cfg.bus_range.end,
+				       bus_max);
+
+	/* Map our Configuration Space windows */
+	if (!devm_request_mem_region(dev, pci->cfg.res.start,
+				     resource_size(&pci->cfg.res),
+				     "Configuration Space"))
+		return -ENOMEM;
+
+	bus_range = &pci->cfg.bus_range;
+	for (busn = bus_range->start; busn <= bus_range->end; ++busn) {
+		u32 idx = busn - bus_range->start;
+		u32 sz = 1 << pci->cfg.ops->bus_shift;
+
+		pci->cfg.win[idx] = devm_ioremap(dev,
+						 pci->cfg.res.start + busn * sz,
+						 sz);
+		if (!pci->cfg.win[idx])
+			return -ENOMEM;
+	}
+
+	/* Register bus resource */
+	pci_add_resource(&pci->resources, bus_range);
+	return 0;
+}
+
+static int gen_pci_setup(int nr, struct pci_sys_data *sys)
+{
+	struct gen_pci *pci = sys->private_data;
+	list_splice_init(&pci->resources, &sys->resources);
+	return 1;
+}
+
+static int gen_pci_probe(struct platform_device *pdev)
+{
+	int err;
+	const char *type;
+	const struct of_device_id *of_id;
+	const int *prop;
+	struct device *dev = &pdev->dev;
+	struct device_node *np = dev->of_node;
+	struct gen_pci *pci = devm_kzalloc(dev, sizeof(*pci), GFP_KERNEL);
+	struct hw_pci hw = {
+		.nr_controllers	= 1,
+		.private_data	= (void **)&pci,
+		.setup		= gen_pci_setup,
+		.map_irq	= of_irq_parse_and_map_pci,
+		.ops		= &gen_pci_ops,
+	};
+
+	if (!pci)
+		return -ENOMEM;
+
+	type = of_get_property(np, "device_type", NULL);
+	if (!type || strcmp(type, "pci")) {
+		dev_err(dev, "invalid \"device_type\" %s\n", type);
+		return -EINVAL;
+	}
+
+	prop = of_get_property(of_chosen, "linux,pci-probe-only", NULL);
+	if (prop) {
+		if (*prop)
+			pci_add_flags(PCI_PROBE_ONLY);
+		else
+			pci_clear_flags(PCI_PROBE_ONLY);
+	}
+
+	of_id = of_match_node(gen_pci_of_match, np);
+	pci->cfg.ops = of_id->data;
+	pci->host.dev.parent = dev;
+	INIT_LIST_HEAD(&pci->host.windows);
+	INIT_LIST_HEAD(&pci->resources);
+
+	/* Parse our PCI ranges and request their resources */
+	err = gen_pci_parse_request_of_pci_ranges(pci);
+	if (err)
+		return err;
+
+	/* Parse and map our Configuration Space windows */
+	err = gen_pci_parse_map_cfg_windows(pci);
+	if (err) {
+		gen_pci_release_of_pci_ranges(pci);
+		return err;
+	}
+
+	pci_common_init_dev(dev, &hw);
+	return 0;
+}
+
+static struct platform_driver gen_pci_driver = {
+	.driver = {
+		.name = "pci-host-generic",
+		.owner = THIS_MODULE,
+		.of_match_table = gen_pci_of_match,
+	},
+	.probe = gen_pci_probe,
+};
+module_platform_driver(gen_pci_driver);
+
+MODULE_DESCRIPTION("Generic PCI host driver");
+MODULE_AUTHOR("Will Deacon <will.deacon@arm.com>");
+MODULE_LICENSE("GPLv2");
diff --git a/drivers/pci/host/pci-imx6.c b/drivers/pci/host/pci-imx6.c
index ee082509b0b..a5645ae4aef 100644
--- a/drivers/pci/host/pci-imx6.c
+++ b/drivers/pci/host/pci-imx6.c
@@ -25,6 +25,7 @@
 #include <linux/resource.h>
 #include <linux/signal.h>
 #include <linux/types.h>
+#include <linux/interrupt.h>
 
 #include "pcie-designware.h"
 
@@ -32,13 +33,9 @@
 
 struct imx6_pcie {
 	int			reset_gpio;
-	int			power_on_gpio;
-	int			wake_up_gpio;
-	int			disable_gpio;
-	struct clk		*lvds_gate;
-	struct clk		*sata_ref_100m;
-	struct clk		*pcie_ref_125m;
-	struct clk		*pcie_axi;
+	struct clk		*pcie_bus;
+	struct clk		*pcie_phy;
+	struct clk		*pcie;
 	struct pcie_port	pp;
 	struct regmap		*iomuxc_gpr;
 	void __iomem		*mem_base;
@@ -231,36 +228,27 @@ static int imx6_pcie_deassert_core_reset(struct pcie_port *pp)
 	struct imx6_pcie *imx6_pcie = to_imx6_pcie(pp);
 	int ret;
 
-	if (gpio_is_valid(imx6_pcie->power_on_gpio))
-		gpio_set_value(imx6_pcie->power_on_gpio, 1);
-
 	regmap_update_bits(imx6_pcie->iomuxc_gpr, IOMUXC_GPR1,
 			IMX6Q_GPR1_PCIE_TEST_PD, 0 << 18);
 	regmap_update_bits(imx6_pcie->iomuxc_gpr, IOMUXC_GPR1,
 			IMX6Q_GPR1_PCIE_REF_CLK_EN, 1 << 16);
 
-	ret = clk_prepare_enable(imx6_pcie->sata_ref_100m);
-	if (ret) {
-		dev_err(pp->dev, "unable to enable sata_ref_100m\n");
-		goto err_sata_ref;
-	}
-
-	ret = clk_prepare_enable(imx6_pcie->pcie_ref_125m);
+	ret = clk_prepare_enable(imx6_pcie->pcie_phy);
 	if (ret) {
-		dev_err(pp->dev, "unable to enable pcie_ref_125m\n");
-		goto err_pcie_ref;
+		dev_err(pp->dev, "unable to enable pcie_phy clock\n");
+		goto err_pcie_phy;
 	}
 
-	ret = clk_prepare_enable(imx6_pcie->lvds_gate);
+	ret = clk_prepare_enable(imx6_pcie->pcie_bus);
 	if (ret) {
-		dev_err(pp->dev, "unable to enable lvds_gate\n");
-		goto err_lvds_gate;
+		dev_err(pp->dev, "unable to enable pcie_bus clock\n");
+		goto err_pcie_bus;
 	}
 
-	ret = clk_prepare_enable(imx6_pcie->pcie_axi);
+	ret = clk_prepare_enable(imx6_pcie->pcie);
 	if (ret) {
-		dev_err(pp->dev, "unable to enable pcie_axi\n");
-		goto err_pcie_axi;
+		dev_err(pp->dev, "unable to enable pcie clock\n");
+		goto err_pcie;
 	}
 
 	/* allow the clocks to stabilize */
@@ -274,13 +262,11 @@ static int imx6_pcie_deassert_core_reset(struct pcie_port *pp)
 	}
 	return 0;
 
-err_pcie_axi:
-	clk_disable_unprepare(imx6_pcie->lvds_gate);
-err_lvds_gate:
-	clk_disable_unprepare(imx6_pcie->pcie_ref_125m);
-err_pcie_ref:
-	clk_disable_unprepare(imx6_pcie->sata_ref_100m);
-err_sata_ref:
+err_pcie:
+	clk_disable_unprepare(imx6_pcie->pcie_bus);
+err_pcie_bus:
+	clk_disable_unprepare(imx6_pcie->pcie_phy);
+err_pcie_phy:
 	return ret;
 
 }
@@ -329,6 +315,13 @@ static int imx6_pcie_wait_for_link(struct pcie_port *pp)
 	return 0;
 }
 
+static irqreturn_t imx6_pcie_msi_handler(int irq, void *arg)
+{
+	struct pcie_port *pp = arg;
+
+	return dw_handle_msi_irq(pp);
+}
+
 static int imx6_pcie_start_link(struct pcie_port *pp)
 {
 	struct imx6_pcie *imx6_pcie = to_imx6_pcie(pp);
@@ -403,6 +396,9 @@ static void imx6_pcie_host_init(struct pcie_port *pp)
 	dw_pcie_setup_rc(pp);
 
 	imx6_pcie_start_link(pp);
+
+	if (IS_ENABLED(CONFIG_PCI_MSI))
+		dw_pcie_msi_init(pp);
 }
 
 static void imx6_pcie_reset_phy(struct pcie_port *pp)
@@ -487,15 +483,25 @@ static struct pcie_host_ops imx6_pcie_host_ops = {
 	.host_init = imx6_pcie_host_init,
 };
 
-static int imx6_add_pcie_port(struct pcie_port *pp,
+static int __init imx6_add_pcie_port(struct pcie_port *pp,
 			struct platform_device *pdev)
 {
 	int ret;
 
-	pp->irq = platform_get_irq(pdev, 0);
-	if (!pp->irq) {
-		dev_err(&pdev->dev, "failed to get irq\n");
-		return -ENODEV;
+	if (IS_ENABLED(CONFIG_PCI_MSI)) {
+		pp->msi_irq = platform_get_irq_byname(pdev, "msi");
+		if (pp->msi_irq <= 0) {
+			dev_err(&pdev->dev, "failed to get MSI irq\n");
+			return -ENODEV;
+		}
+
+		ret = devm_request_irq(&pdev->dev, pp->msi_irq,
+		                       imx6_pcie_msi_handler,
+		                       IRQF_SHARED, "mx6-pcie-msi", pp);
+		if (ret) {
+			dev_err(&pdev->dev, "failed to request MSI irq\n");
+			return -ENODEV;
+		}
 	}
 
 	pp->root_bus_nr = -1;
@@ -546,69 +552,26 @@ static int __init imx6_pcie_probe(struct platform_device *pdev)
 		}
 	}
 
-	imx6_pcie->power_on_gpio = of_get_named_gpio(np, "power-on-gpio", 0);
-	if (gpio_is_valid(imx6_pcie->power_on_gpio)) {
-		ret = devm_gpio_request_one(&pdev->dev,
-					imx6_pcie->power_on_gpio,
-					GPIOF_OUT_INIT_LOW,
-					"PCIe power enable");
-		if (ret) {
-			dev_err(&pdev->dev, "unable to get power-on gpio\n");
-			return ret;
-		}
-	}
-
-	imx6_pcie->wake_up_gpio = of_get_named_gpio(np, "wake-up-gpio", 0);
-	if (gpio_is_valid(imx6_pcie->wake_up_gpio)) {
-		ret = devm_gpio_request_one(&pdev->dev,
-					imx6_pcie->wake_up_gpio,
-					GPIOF_IN,
-					"PCIe wake up");
-		if (ret) {
-			dev_err(&pdev->dev, "unable to get wake-up gpio\n");
-			return ret;
-		}
-	}
-
-	imx6_pcie->disable_gpio = of_get_named_gpio(np, "disable-gpio", 0);
-	if (gpio_is_valid(imx6_pcie->disable_gpio)) {
-		ret = devm_gpio_request_one(&pdev->dev,
-					imx6_pcie->disable_gpio,
-					GPIOF_OUT_INIT_HIGH,
-					"PCIe disable endpoint");
-		if (ret) {
-			dev_err(&pdev->dev, "unable to get disable-ep gpio\n");
-			return ret;
-		}
-	}
-
 	/* Fetch clocks */
-	imx6_pcie->lvds_gate = devm_clk_get(&pdev->dev, "lvds_gate");
-	if (IS_ERR(imx6_pcie->lvds_gate)) {
-		dev_err(&pdev->dev,
-			"lvds_gate clock select missing or invalid\n");
-		return PTR_ERR(imx6_pcie->lvds_gate);
-	}
-
-	imx6_pcie->sata_ref_100m = devm_clk_get(&pdev->dev, "sata_ref_100m");
-	if (IS_ERR(imx6_pcie->sata_ref_100m)) {
+	imx6_pcie->pcie_phy = devm_clk_get(&pdev->dev, "pcie_phy");
+	if (IS_ERR(imx6_pcie->pcie_phy)) {
 		dev_err(&pdev->dev,
-			"sata_ref_100m clock source missing or invalid\n");
-		return PTR_ERR(imx6_pcie->sata_ref_100m);
+			"pcie_phy clock source missing or invalid\n");
+		return PTR_ERR(imx6_pcie->pcie_phy);
 	}
 
-	imx6_pcie->pcie_ref_125m = devm_clk_get(&pdev->dev, "pcie_ref_125m");
-	if (IS_ERR(imx6_pcie->pcie_ref_125m)) {
+	imx6_pcie->pcie_bus = devm_clk_get(&pdev->dev, "pcie_bus");
+	if (IS_ERR(imx6_pcie->pcie_bus)) {
 		dev_err(&pdev->dev,
-			"pcie_ref_125m clock source missing or invalid\n");
-		return PTR_ERR(imx6_pcie->pcie_ref_125m);
+			"pcie_bus clock source missing or invalid\n");
+		return PTR_ERR(imx6_pcie->pcie_bus);
 	}
 
-	imx6_pcie->pcie_axi = devm_clk_get(&pdev->dev, "pcie_axi");
-	if (IS_ERR(imx6_pcie->pcie_axi)) {
+	imx6_pcie->pcie = devm_clk_get(&pdev->dev, "pcie");
+	if (IS_ERR(imx6_pcie->pcie)) {
 		dev_err(&pdev->dev,
-			"pcie_axi clock source missing or invalid\n");
-		return PTR_ERR(imx6_pcie->pcie_axi);
+			"pcie clock source missing or invalid\n");
+		return PTR_ERR(imx6_pcie->pcie);
 	}
 
 	/* Grab GPR config register range */
diff --git a/drivers/pci/host/pci-mvebu.c b/drivers/pci/host/pci-mvebu.c
index d3d1cfd51e0..e384e253459 100644
--- a/drivers/pci/host/pci-mvebu.c
+++ b/drivers/pci/host/pci-mvebu.c
@@ -293,6 +293,58 @@ static int mvebu_pcie_hw_wr_conf(struct mvebu_pcie_port *port,
 	return PCIBIOS_SUCCESSFUL;
 }
 
+/*
+ * Remove windows, starting from the largest ones to the smallest
+ * ones.
+ */
+static void mvebu_pcie_del_windows(struct mvebu_pcie_port *port,
+				   phys_addr_t base, size_t size)
+{
+	while (size) {
+		size_t sz = 1 << (fls(size) - 1);
+
+		mvebu_mbus_del_window(base, sz);
+		base += sz;
+		size -= sz;
+	}
+}
+
+/*
+ * MBus windows can only have a power of two size, but PCI BARs do not
+ * have this constraint. Therefore, we have to split the PCI BAR into
+ * areas each having a power of two size. We start from the largest
+ * one (i.e highest order bit set in the size).
+ */
+static void mvebu_pcie_add_windows(struct mvebu_pcie_port *port,
+				   unsigned int target, unsigned int attribute,
+				   phys_addr_t base, size_t size,
+				   phys_addr_t remap)
+{
+	size_t size_mapped = 0;
+
+	while (size) {
+		size_t sz = 1 << (fls(size) - 1);
+		int ret;
+
+		ret = mvebu_mbus_add_window_remap_by_id(target, attribute, base,
+							sz, remap);
+		if (ret) {
+			dev_err(&port->pcie->pdev->dev,
+				"Could not create MBus window at 0x%x, size 0x%x: %d\n",
+				base, sz, ret);
+			mvebu_pcie_del_windows(port, base - size_mapped,
+					       size_mapped);
+			return;
+		}
+
+		size -= sz;
+		size_mapped += sz;
+		base += sz;
+		if (remap != MVEBU_MBUS_NO_REMAP)
+			remap += sz;
+	}
+}
+
 static void mvebu_pcie_handle_iobase_change(struct mvebu_pcie_port *port)
 {
 	phys_addr_t iobase;
@@ -304,8 +356,8 @@ static void mvebu_pcie_handle_iobase_change(struct mvebu_pcie_port *port)
 
 		/* If a window was configured, remove it */
 		if (port->iowin_base) {
-			mvebu_mbus_del_window(port->iowin_base,
-					      port->iowin_size);
+			mvebu_pcie_del_windows(port, port->iowin_base,
+					       port->iowin_size);
 			port->iowin_base = 0;
 			port->iowin_size = 0;
 		}
@@ -331,11 +383,11 @@ static void mvebu_pcie_handle_iobase_change(struct mvebu_pcie_port *port)
 	port->iowin_base = port->pcie->io.start + iobase;
 	port->iowin_size = ((0xFFF | ((port->bridge.iolimit & 0xF0) << 8) |
 			    (port->bridge.iolimitupper << 16)) -
-			    iobase);
+			    iobase) + 1;
 
-	mvebu_mbus_add_window_remap_by_id(port->io_target, port->io_attr,
-					  port->iowin_base, port->iowin_size,
-					  iobase);
+	mvebu_pcie_add_windows(port, port->io_target, port->io_attr,
+			       port->iowin_base, port->iowin_size,
+			       iobase);
 }
 
 static void mvebu_pcie_handle_membase_change(struct mvebu_pcie_port *port)
@@ -346,8 +398,8 @@ static void mvebu_pcie_handle_membase_change(struct mvebu_pcie_port *port)
 
 		/* If a window was configured, remove it */
 		if (port->memwin_base) {
-			mvebu_mbus_del_window(port->memwin_base,
-					      port->memwin_size);
+			mvebu_pcie_del_windows(port, port->memwin_base,
+					       port->memwin_size);
 			port->memwin_base = 0;
 			port->memwin_size = 0;
 		}
@@ -364,10 +416,11 @@ static void mvebu_pcie_handle_membase_change(struct mvebu_pcie_port *port)
 	port->memwin_base  = ((port->bridge.membase & 0xFFF0) << 16);
 	port->memwin_size  =
 		(((port->bridge.memlimit & 0xFFF0) << 16) | 0xFFFFF) -
-		port->memwin_base;
+		port->memwin_base + 1;
 
-	mvebu_mbus_add_window_by_id(port->mem_target, port->mem_attr,
-				    port->memwin_base, port->memwin_size);
+	mvebu_pcie_add_windows(port, port->mem_target, port->mem_attr,
+			       port->memwin_base, port->memwin_size,
+			       MVEBU_MBUS_NO_REMAP);
 }
 
 /*
@@ -743,14 +796,21 @@ static resource_size_t mvebu_pcie_align_resource(struct pci_dev *dev,
 
 	/*
 	 * On the PCI-to-PCI bridge side, the I/O windows must have at
-	 * least a 64 KB size and be aligned on their size, and the
-	 * memory windows must have at least a 1 MB size and be
-	 * aligned on their size
+	 * least a 64 KB size and the memory windows must have at
+	 * least a 1 MB size. Moreover, MBus windows need to have a
+	 * base address aligned on their size, and their size must be
+	 * a power of two. This means that if the BAR doesn't have a
+	 * power of two size, several MBus windows will actually be
+	 * created. We need to ensure that the biggest MBus window
+	 * (which will be the first one) is aligned on its size, which
+	 * explains the rounddown_pow_of_two() being done here.
 	 */
 	if (res->flags & IORESOURCE_IO)
-		return round_up(start, max_t(resource_size_t, SZ_64K, size));
+		return round_up(start, max_t(resource_size_t, SZ_64K,
+					     rounddown_pow_of_two(size)));
 	else if (res->flags & IORESOURCE_MEM)
-		return round_up(start, max_t(resource_size_t, SZ_1M, size));
+		return round_up(start, max_t(resource_size_t, SZ_1M,
+					     rounddown_pow_of_two(size)));
 	else
 		return start;
 }
diff --git a/drivers/pci/host/pci-rcar-gen2.c b/drivers/pci/host/pci-rcar-gen2.c
index fd3e3ab5650..3ef854f5a5b 100644
--- a/drivers/pci/host/pci-rcar-gen2.c
+++ b/drivers/pci/host/pci-rcar-gen2.c
@@ -15,6 +15,7 @@
 #include <linux/io.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/of_pci.h>
 #include <linux/pci.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
@@ -98,6 +99,7 @@ struct rcar_pci_priv {
 	struct resource io_res;
 	struct resource mem_res;
 	struct resource *cfg_res;
+	unsigned busnr;
 	int irq;
 	unsigned long window_size;
 };
@@ -180,8 +182,13 @@ static int rcar_pci_map_irq(const struct pci_dev *dev, u8 slot, u8 pin)
 {
 	struct pci_sys_data *sys = dev->bus->sysdata;
 	struct rcar_pci_priv *priv = sys->private_data;
+	int irq;
+
+	irq = of_irq_parse_and_map_pci(dev, slot, pin);
+	if (!irq)
+		irq = priv->irq;
 
-	return priv->irq;
+	return irq;
 }
 
 #ifdef CONFIG_PCI_DEBUG
@@ -312,8 +319,8 @@ static int rcar_pci_setup(int nr, struct pci_sys_data *sys)
 	pci_add_resource(&sys->resources, &priv->io_res);
 	pci_add_resource(&sys->resources, &priv->mem_res);
 
-	/* Setup bus number based on platform device id */
-	sys->busnr = to_platform_device(priv->dev)->id;
+	/* Setup bus number based on platform device id / of bus-range */
+	sys->busnr = priv->busnr;
 	return 1;
 }
 
@@ -366,6 +373,23 @@ static int rcar_pci_probe(struct platform_device *pdev)
 
 	priv->window_size = SZ_1G;
 
+	if (pdev->dev.of_node) {
+		struct resource busnr;
+		int ret;
+
+		ret = of_pci_parse_bus_range(pdev->dev.of_node, &busnr);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "failed to parse bus-range\n");
+			return ret;
+		}
+
+		priv->busnr = busnr.start;
+		if (busnr.end != busnr.start)
+			dev_warn(&pdev->dev, "only one bus number supported\n");
+	} else {
+		priv->busnr = pdev->id;
+	}
+
 	hw_private[0] = priv;
 	memset(&hw, 0, sizeof(hw));
 	hw.nr_controllers = ARRAY_SIZE(hw_private);
@@ -377,11 +401,20 @@ static int rcar_pci_probe(struct platform_device *pdev)
 	return 0;
 }
 
+static struct of_device_id rcar_pci_of_match[] = {
+	{ .compatible = "renesas,pci-r8a7790", },
+	{ .compatible = "renesas,pci-r8a7791", },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(of, rcar_pci_of_match);
+
 static struct platform_driver rcar_pci_driver = {
 	.driver = {
 		.name = "pci-rcar-gen2",
 		.owner = THIS_MODULE,
 		.suppress_bind_attrs = true,
+		.of_match_table = rcar_pci_of_match,
 	},
 	.probe = rcar_pci_probe,
 };
diff --git a/drivers/pci/host/pci-tegra.c b/drivers/pci/host/pci-tegra.c
index 330f7e3a32d..083cf37ca04 100644
--- a/drivers/pci/host/pci-tegra.c
+++ b/drivers/pci/host/pci-tegra.c
@@ -639,10 +639,15 @@ static int tegra_pcie_setup(int nr, struct pci_sys_data *sys)
 static int tegra_pcie_map_irq(const struct pci_dev *pdev, u8 slot, u8 pin)
 {
 	struct tegra_pcie *pcie = sys_to_pcie(pdev->bus->sysdata);
+	int irq;
 
 	tegra_cpuidle_pcie_irqs_in_use();
 
-	return pcie->irq;
+	irq = of_irq_parse_and_map_pci(pdev, slot, pin);
+	if (!irq)
+		irq = pcie->irq;
+
+	return irq;
 }
 
 static void tegra_pcie_add_bus(struct pci_bus *bus)
diff --git a/drivers/pci/host/pcie-designware.c b/drivers/pci/host/pcie-designware.c
index 509a29d8450..e3bf9e6d5d9 100644
--- a/drivers/pci/host/pcie-designware.c
+++ b/drivers/pci/host/pcie-designware.c
@@ -17,6 +17,7 @@
 #include <linux/module.h>
 #include <linux/msi.h>
 #include <linux/of_address.h>
+#include <linux/of_pci.h>
 #include <linux/pci.h>
 #include <linux/pci_regs.h>
 #include <linux/types.h>
@@ -155,15 +156,17 @@ static struct irq_chip dw_msi_irq_chip = {
 };
 
 /* MSI int handler */
-void dw_handle_msi_irq(struct pcie_port *pp)
+irqreturn_t dw_handle_msi_irq(struct pcie_port *pp)
 {
 	unsigned long val;
 	int i, pos, irq;
+	irqreturn_t ret = IRQ_NONE;
 
 	for (i = 0; i < MAX_MSI_CTRLS; i++) {
 		dw_pcie_rd_own_conf(pp, PCIE_MSI_INTR0_STATUS + i * 12, 4,
 				(u32 *)&val);
 		if (val) {
+			ret = IRQ_HANDLED;
 			pos = 0;
 			while ((pos = find_next_bit(&val, 32, pos)) != 32) {
 				irq = irq_find_mapping(pp->irq_domain,
@@ -176,6 +179,8 @@ void dw_handle_msi_irq(struct pcie_port *pp)
 			}
 		}
 	}
+
+	return ret;
 }
 
 void dw_pcie_msi_init(struct pcie_port *pp)
@@ -490,7 +495,7 @@ int __init dw_pcie_host_init(struct pcie_port *pp)
 	dw_pci.nr_controllers = 1;
 	dw_pci.private_data = (void **)&pp;
 
-	pci_common_init(&dw_pci);
+	pci_common_init_dev(pp->dev, &dw_pci);
 	pci_assign_unassigned_resources();
 #ifdef CONFIG_PCI_DOMAINS
 	dw_pci.domain++;
@@ -520,13 +525,13 @@ static void dw_pcie_prog_viewport_cfg1(struct pcie_port *pp, u32 busdev)
 	dw_pcie_writel_rc(pp, PCIE_ATU_REGION_OUTBOUND | PCIE_ATU_REGION_INDEX1,
 			  PCIE_ATU_VIEWPORT);
 	dw_pcie_writel_rc(pp, PCIE_ATU_TYPE_CFG1, PCIE_ATU_CR1);
-	dw_pcie_writel_rc(pp, PCIE_ATU_ENABLE, PCIE_ATU_CR2);
 	dw_pcie_writel_rc(pp, pp->cfg1_base, PCIE_ATU_LOWER_BASE);
 	dw_pcie_writel_rc(pp, (pp->cfg1_base >> 32), PCIE_ATU_UPPER_BASE);
 	dw_pcie_writel_rc(pp, pp->cfg1_base + pp->config.cfg1_size - 1,
 			  PCIE_ATU_LIMIT);
 	dw_pcie_writel_rc(pp, busdev, PCIE_ATU_LOWER_TARGET);
 	dw_pcie_writel_rc(pp, 0, PCIE_ATU_UPPER_TARGET);
+	dw_pcie_writel_rc(pp, PCIE_ATU_ENABLE, PCIE_ATU_CR2);
 }
 
 static void dw_pcie_prog_viewport_mem_outbound(struct pcie_port *pp)
@@ -535,7 +540,6 @@ static void dw_pcie_prog_viewport_mem_outbound(struct pcie_port *pp)
 	dw_pcie_writel_rc(pp, PCIE_ATU_REGION_OUTBOUND | PCIE_ATU_REGION_INDEX0,
 			  PCIE_ATU_VIEWPORT);
 	dw_pcie_writel_rc(pp, PCIE_ATU_TYPE_MEM, PCIE_ATU_CR1);
-	dw_pcie_writel_rc(pp, PCIE_ATU_ENABLE, PCIE_ATU_CR2);
 	dw_pcie_writel_rc(pp, pp->mem_base, PCIE_ATU_LOWER_BASE);
 	dw_pcie_writel_rc(pp, (pp->mem_base >> 32), PCIE_ATU_UPPER_BASE);
 	dw_pcie_writel_rc(pp, pp->mem_base + pp->config.mem_size - 1,
@@ -543,6 +547,7 @@ static void dw_pcie_prog_viewport_mem_outbound(struct pcie_port *pp)
 	dw_pcie_writel_rc(pp, pp->config.mem_bus_addr, PCIE_ATU_LOWER_TARGET);
 	dw_pcie_writel_rc(pp, upper_32_bits(pp->config.mem_bus_addr),
 			  PCIE_ATU_UPPER_TARGET);
+	dw_pcie_writel_rc(pp, PCIE_ATU_ENABLE, PCIE_ATU_CR2);
 }
 
 static void dw_pcie_prog_viewport_io_outbound(struct pcie_port *pp)
@@ -551,7 +556,6 @@ static void dw_pcie_prog_viewport_io_outbound(struct pcie_port *pp)
 	dw_pcie_writel_rc(pp, PCIE_ATU_REGION_OUTBOUND | PCIE_ATU_REGION_INDEX1,
 			  PCIE_ATU_VIEWPORT);
 	dw_pcie_writel_rc(pp, PCIE_ATU_TYPE_IO, PCIE_ATU_CR1);
-	dw_pcie_writel_rc(pp, PCIE_ATU_ENABLE, PCIE_ATU_CR2);
 	dw_pcie_writel_rc(pp, pp->io_base, PCIE_ATU_LOWER_BASE);
 	dw_pcie_writel_rc(pp, (pp->io_base >> 32), PCIE_ATU_UPPER_BASE);
 	dw_pcie_writel_rc(pp, pp->io_base + pp->config.io_size - 1,
@@ -559,6 +563,7 @@ static void dw_pcie_prog_viewport_io_outbound(struct pcie_port *pp)
 	dw_pcie_writel_rc(pp, pp->config.io_bus_addr, PCIE_ATU_LOWER_TARGET);
 	dw_pcie_writel_rc(pp, upper_32_bits(pp->config.io_bus_addr),
 			  PCIE_ATU_UPPER_TARGET);
+	dw_pcie_writel_rc(pp, PCIE_ATU_ENABLE, PCIE_ATU_CR2);
 }
 
 static int dw_pcie_rd_other_conf(struct pcie_port *pp, struct pci_bus *bus,
@@ -723,7 +728,7 @@ static struct pci_bus *dw_pcie_scan_bus(int nr, struct pci_sys_data *sys)
 
 	if (pp) {
 		pp->root_bus_nr = sys->busnr;
-		bus = pci_scan_root_bus(NULL, sys->busnr, &dw_pcie_ops,
+		bus = pci_scan_root_bus(pp->dev, sys->busnr, &dw_pcie_ops,
 					sys, &sys->resources);
 	} else {
 		bus = NULL;
@@ -736,8 +741,13 @@ static struct pci_bus *dw_pcie_scan_bus(int nr, struct pci_sys_data *sys)
 static int dw_pcie_map_irq(const struct pci_dev *dev, u8 slot, u8 pin)
 {
 	struct pcie_port *pp = sys_to_pcie(dev->bus->sysdata);
+	int irq;
 
-	return pp->irq;
+	irq = of_irq_parse_and_map_pci(dev, slot, pin);
+	if (!irq)
+		irq = pp->irq;
+
+	return irq;
 }
 
 static void dw_pcie_add_bus(struct pci_bus *bus)
@@ -764,7 +774,7 @@ void dw_pcie_setup_rc(struct pcie_port *pp)
 	u32 membase;
 	u32 memlimit;
 
-	/* set the number of lines as 4 */
+	/* set the number of lanes */
 	dw_pcie_readl_rc(pp, PCIE_PORT_LINK_CONTROL, &val);
 	val &= ~PORT_LINK_MODE_MASK;
 	switch (pp->lanes) {
diff --git a/drivers/pci/host/pcie-designware.h b/drivers/pci/host/pcie-designware.h
index 3063b3594d8..a169d22d517 100644
--- a/drivers/pci/host/pcie-designware.h
+++ b/drivers/pci/host/pcie-designware.h
@@ -68,7 +68,7 @@ struct pcie_host_ops {
 
 int dw_pcie_cfg_read(void __iomem *addr, int where, int size, u32 *val);
 int dw_pcie_cfg_write(void __iomem *addr, int where, int size, u32 val);
-void dw_handle_msi_irq(struct pcie_port *pp);
+irqreturn_t dw_handle_msi_irq(struct pcie_port *pp);
 void dw_pcie_msi_init(struct pcie_port *pp);
 int dw_pcie_link_up(struct pcie_port *pp);
 void dw_pcie_setup_rc(struct pcie_port *pp);
diff --git a/drivers/pci/host/pcie-rcar.c b/drivers/pci/host/pcie-rcar.c
new file mode 100644
index 00000000000..8e06124aa80
--- /dev/null
+++ b/drivers/pci/host/pcie-rcar.c
@@ -0,0 +1,1008 @@
+/*
+ * PCIe driver for Renesas R-Car SoCs
+ *  Copyright (C) 2014 Renesas Electronics Europe Ltd
+ *
+ * Based on:
+ *  arch/sh/drivers/pci/pcie-sh7786.c
+ *  arch/sh/drivers/pci/ops-sh7786.c
+ *  Copyright (C) 2009 - 2011  Paul Mundt
+ *
+ * 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.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/irqdomain.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/msi.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/of_pci.h>
+#include <linux/of_platform.h>
+#include <linux/pci.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#define DRV_NAME "rcar-pcie"
+
+#define PCIECAR			0x000010
+#define PCIECCTLR		0x000018
+#define  CONFIG_SEND_ENABLE	(1 << 31)
+#define  TYPE0			(0 << 8)
+#define  TYPE1			(1 << 8)
+#define PCIECDR			0x000020
+#define PCIEMSR			0x000028
+#define PCIEINTXR		0x000400
+#define PCIEMSITXR		0x000840
+
+/* Transfer control */
+#define PCIETCTLR		0x02000
+#define  CFINIT			1
+#define PCIETSTR		0x02004
+#define  DATA_LINK_ACTIVE	1
+#define PCIEERRFR		0x02020
+#define  UNSUPPORTED_REQUEST	(1 << 4)
+#define PCIEMSIFR		0x02044
+#define PCIEMSIALR		0x02048
+#define  MSIFE			1
+#define PCIEMSIAUR		0x0204c
+#define PCIEMSIIER		0x02050
+
+/* root port address */
+#define PCIEPRAR(x)		(0x02080 + ((x) * 0x4))
+
+/* local address reg & mask */
+#define PCIELAR(x)		(0x02200 + ((x) * 0x20))
+#define PCIELAMR(x)		(0x02208 + ((x) * 0x20))
+#define  LAM_PREFETCH		(1 << 3)
+#define  LAM_64BIT		(1 << 2)
+#define  LAR_ENABLE		(1 << 1)
+
+/* PCIe address reg & mask */
+#define PCIEPARL(x)		(0x03400 + ((x) * 0x20))
+#define PCIEPARH(x)		(0x03404 + ((x) * 0x20))
+#define PCIEPAMR(x)		(0x03408 + ((x) * 0x20))
+#define PCIEPTCTLR(x)		(0x0340c + ((x) * 0x20))
+#define  PAR_ENABLE		(1 << 31)
+#define  IO_SPACE		(1 << 8)
+
+/* Configuration */
+#define PCICONF(x)		(0x010000 + ((x) * 0x4))
+#define PMCAP(x)		(0x010040 + ((x) * 0x4))
+#define EXPCAP(x)		(0x010070 + ((x) * 0x4))
+#define VCCAP(x)		(0x010100 + ((x) * 0x4))
+
+/* link layer */
+#define IDSETR1			0x011004
+#define TLCTLR			0x011048
+#define MACSR			0x011054
+#define MACCTLR			0x011058
+#define  SCRAMBLE_DISABLE	(1 << 27)
+
+/* R-Car H1 PHY */
+#define H1_PCIEPHYADRR		0x04000c
+#define  WRITE_CMD		(1 << 16)
+#define  PHY_ACK		(1 << 24)
+#define  RATE_POS		12
+#define  LANE_POS		8
+#define  ADR_POS		0
+#define H1_PCIEPHYDOUTR		0x040014
+#define H1_PCIEPHYSR		0x040018
+
+#define INT_PCI_MSI_NR	32
+
+#define RCONF(x)	(PCICONF(0)+(x))
+#define RPMCAP(x)	(PMCAP(0)+(x))
+#define REXPCAP(x)	(EXPCAP(0)+(x))
+#define RVCCAP(x)	(VCCAP(0)+(x))
+
+#define  PCIE_CONF_BUS(b)	(((b) & 0xff) << 24)
+#define  PCIE_CONF_DEV(d)	(((d) & 0x1f) << 19)
+#define  PCIE_CONF_FUNC(f)	(((f) & 0x7) << 16)
+
+#define PCI_MAX_RESOURCES 4
+#define MAX_NR_INBOUND_MAPS 6
+
+struct rcar_msi {
+	DECLARE_BITMAP(used, INT_PCI_MSI_NR);
+	struct irq_domain *domain;
+	struct msi_chip chip;
+	unsigned long pages;
+	struct mutex lock;
+	int irq1;
+	int irq2;
+};
+
+static inline struct rcar_msi *to_rcar_msi(struct msi_chip *chip)
+{
+	return container_of(chip, struct rcar_msi, chip);
+}
+
+/* Structure representing the PCIe interface */
+struct rcar_pcie {
+	struct device		*dev;
+	void __iomem		*base;
+	struct resource		res[PCI_MAX_RESOURCES];
+	struct resource		busn;
+	int			root_bus_nr;
+	struct clk		*clk;
+	struct clk		*bus_clk;
+	struct			rcar_msi msi;
+};
+
+static inline struct rcar_pcie *sys_to_pcie(struct pci_sys_data *sys)
+{
+	return sys->private_data;
+}
+
+static void pci_write_reg(struct rcar_pcie *pcie, unsigned long val,
+			  unsigned long reg)
+{
+	writel(val, pcie->base + reg);
+}
+
+static unsigned long pci_read_reg(struct rcar_pcie *pcie, unsigned long reg)
+{
+	return readl(pcie->base + reg);
+}
+
+enum {
+	PCI_ACCESS_READ,
+	PCI_ACCESS_WRITE,
+};
+
+static void rcar_rmw32(struct rcar_pcie *pcie, int where, u32 mask, u32 data)
+{
+	int shift = 8 * (where & 3);
+	u32 val = pci_read_reg(pcie, where & ~3);
+
+	val &= ~(mask << shift);
+	val |= data << shift;
+	pci_write_reg(pcie, val, where & ~3);
+}
+
+static u32 rcar_read_conf(struct rcar_pcie *pcie, int where)
+{
+	int shift = 8 * (where & 3);
+	u32 val = pci_read_reg(pcie, where & ~3);
+
+	return val >> shift;
+}
+
+/* Serialization is provided by 'pci_lock' in drivers/pci/access.c */
+static int rcar_pcie_config_access(struct rcar_pcie *pcie,
+		unsigned char access_type, struct pci_bus *bus,
+		unsigned int devfn, int where, u32 *data)
+{
+	int dev, func, reg, index;
+
+	dev = PCI_SLOT(devfn);
+	func = PCI_FUNC(devfn);
+	reg = where & ~3;
+	index = reg / 4;
+
+	/*
+	 * While each channel has its own memory-mapped extended config
+	 * space, it's generally only accessible when in endpoint mode.
+	 * When in root complex mode, the controller is unable to target
+	 * itself with either type 0 or type 1 accesses, and indeed, any
+	 * controller initiated target transfer to its own config space
+	 * result in a completer abort.
+	 *
+	 * Each channel effectively only supports a single device, but as
+	 * the same channel <-> device access works for any PCI_SLOT()
+	 * value, we cheat a bit here and bind the controller's config
+	 * space to devfn 0 in order to enable self-enumeration. In this
+	 * case the regular ECAR/ECDR path is sidelined and the mangled
+	 * config access itself is initiated as an internal bus transaction.
+	 */
+	if (pci_is_root_bus(bus)) {
+		if (dev != 0)
+			return PCIBIOS_DEVICE_NOT_FOUND;
+
+		if (access_type == PCI_ACCESS_READ) {
+			*data = pci_read_reg(pcie, PCICONF(index));
+		} else {
+			/* Keep an eye out for changes to the root bus number */
+			if (pci_is_root_bus(bus) && (reg == PCI_PRIMARY_BUS))
+				pcie->root_bus_nr = *data & 0xff;
+
+			pci_write_reg(pcie, *data, PCICONF(index));
+		}
+
+		return PCIBIOS_SUCCESSFUL;
+	}
+
+	if (pcie->root_bus_nr < 0)
+		return PCIBIOS_DEVICE_NOT_FOUND;
+
+	/* Clear errors */
+	pci_write_reg(pcie, pci_read_reg(pcie, PCIEERRFR), PCIEERRFR);
+
+	/* Set the PIO address */
+	pci_write_reg(pcie, PCIE_CONF_BUS(bus->number) | PCIE_CONF_DEV(dev) |
+				PCIE_CONF_FUNC(func) | reg, PCIECAR);
+
+	/* Enable the configuration access */
+	if (bus->parent->number == pcie->root_bus_nr)
+		pci_write_reg(pcie, CONFIG_SEND_ENABLE | TYPE0, PCIECCTLR);
+	else
+		pci_write_reg(pcie, CONFIG_SEND_ENABLE | TYPE1, PCIECCTLR);
+
+	/* Check for errors */
+	if (pci_read_reg(pcie, PCIEERRFR) & UNSUPPORTED_REQUEST)
+		return PCIBIOS_DEVICE_NOT_FOUND;
+
+	/* Check for master and target aborts */
+	if (rcar_read_conf(pcie, RCONF(PCI_STATUS)) &
+		(PCI_STATUS_REC_MASTER_ABORT | PCI_STATUS_REC_TARGET_ABORT))
+		return PCIBIOS_DEVICE_NOT_FOUND;
+
+	if (access_type == PCI_ACCESS_READ)
+		*data = pci_read_reg(pcie, PCIECDR);
+	else
+		pci_write_reg(pcie, *data, PCIECDR);
+
+	/* Disable the configuration access */
+	pci_write_reg(pcie, 0, PCIECCTLR);
+
+	return PCIBIOS_SUCCESSFUL;
+}
+
+static int rcar_pcie_read_conf(struct pci_bus *bus, unsigned int devfn,
+			       int where, int size, u32 *val)
+{
+	struct rcar_pcie *pcie = sys_to_pcie(bus->sysdata);
+	int ret;
+
+	if ((size == 2) && (where & 1))
+		return PCIBIOS_BAD_REGISTER_NUMBER;
+	else if ((size == 4) && (where & 3))
+		return PCIBIOS_BAD_REGISTER_NUMBER;
+
+	ret = rcar_pcie_config_access(pcie, PCI_ACCESS_READ,
+				      bus, devfn, where, val);
+	if (ret != PCIBIOS_SUCCESSFUL) {
+		*val = 0xffffffff;
+		return ret;
+	}
+
+	if (size == 1)
+		*val = (*val >> (8 * (where & 3))) & 0xff;
+	else if (size == 2)
+		*val = (*val >> (8 * (where & 2))) & 0xffff;
+
+	dev_dbg(&bus->dev, "pcie-config-read: bus=%3d devfn=0x%04x "
+		"where=0x%04x size=%d val=0x%08lx\n", bus->number,
+		devfn, where, size, (unsigned long)*val);
+
+	return ret;
+}
+
+/* Serialization is provided by 'pci_lock' in drivers/pci/access.c */
+static int rcar_pcie_write_conf(struct pci_bus *bus, unsigned int devfn,
+				int where, int size, u32 val)
+{
+	struct rcar_pcie *pcie = sys_to_pcie(bus->sysdata);
+	int shift, ret;
+	u32 data;
+
+	if ((size == 2) && (where & 1))
+		return PCIBIOS_BAD_REGISTER_NUMBER;
+	else if ((size == 4) && (where & 3))
+		return PCIBIOS_BAD_REGISTER_NUMBER;
+
+	ret = rcar_pcie_config_access(pcie, PCI_ACCESS_READ,
+				      bus, devfn, where, &data);
+	if (ret != PCIBIOS_SUCCESSFUL)
+		return ret;
+
+	dev_dbg(&bus->dev, "pcie-config-write: bus=%3d devfn=0x%04x "
+		"where=0x%04x size=%d val=0x%08lx\n", bus->number,
+		devfn, where, size, (unsigned long)val);
+
+	if (size == 1) {
+		shift = 8 * (where & 3);
+		data &= ~(0xff << shift);
+		data |= ((val & 0xff) << shift);
+	} else if (size == 2) {
+		shift = 8 * (where & 2);
+		data &= ~(0xffff << shift);
+		data |= ((val & 0xffff) << shift);
+	} else
+		data = val;
+
+	ret = rcar_pcie_config_access(pcie, PCI_ACCESS_WRITE,
+				      bus, devfn, where, &data);
+
+	return ret;
+}
+
+static struct pci_ops rcar_pcie_ops = {
+	.read	= rcar_pcie_read_conf,
+	.write	= rcar_pcie_write_conf,
+};
+
+static void rcar_pcie_setup_window(int win, struct resource *res,
+				   struct rcar_pcie *pcie)
+{
+	/* Setup PCIe address space mappings for each resource */
+	resource_size_t size;
+	u32 mask;
+
+	pci_write_reg(pcie, 0x00000000, PCIEPTCTLR(win));
+
+	/*
+	 * The PAMR mask is calculated in units of 128Bytes, which
+	 * keeps things pretty simple.
+	 */
+	size = resource_size(res);
+	mask = (roundup_pow_of_two(size) / SZ_128) - 1;
+	pci_write_reg(pcie, mask << 7, PCIEPAMR(win));
+
+	pci_write_reg(pcie, upper_32_bits(res->start), PCIEPARH(win));
+	pci_write_reg(pcie, lower_32_bits(res->start), PCIEPARL(win));
+
+	/* First resource is for IO */
+	mask = PAR_ENABLE;
+	if (res->flags & IORESOURCE_IO)
+		mask |= IO_SPACE;
+
+	pci_write_reg(pcie, mask, PCIEPTCTLR(win));
+}
+
+static int rcar_pcie_setup(int nr, struct pci_sys_data *sys)
+{
+	struct rcar_pcie *pcie = sys_to_pcie(sys);
+	struct resource *res;
+	int i;
+
+	pcie->root_bus_nr = -1;
+
+	/* Setup PCI resources */
+	for (i = 0; i < PCI_MAX_RESOURCES; i++) {
+
+		res = &pcie->res[i];
+		if (!res->flags)
+			continue;
+
+		rcar_pcie_setup_window(i, res, pcie);
+
+		if (res->flags & IORESOURCE_IO)
+			pci_ioremap_io(nr * SZ_64K, res->start);
+		else
+			pci_add_resource(&sys->resources, res);
+	}
+	pci_add_resource(&sys->resources, &pcie->busn);
+
+	return 1;
+}
+
+static void rcar_pcie_add_bus(struct pci_bus *bus)
+{
+	if (IS_ENABLED(CONFIG_PCI_MSI)) {
+		struct rcar_pcie *pcie = sys_to_pcie(bus->sysdata);
+
+		bus->msi = &pcie->msi.chip;
+	}
+}
+
+struct hw_pci rcar_pci = {
+	.setup          = rcar_pcie_setup,
+	.map_irq        = of_irq_parse_and_map_pci,
+	.ops            = &rcar_pcie_ops,
+	.add_bus        = rcar_pcie_add_bus,
+};
+
+static void rcar_pcie_enable(struct rcar_pcie *pcie)
+{
+	struct platform_device *pdev = to_platform_device(pcie->dev);
+
+	rcar_pci.nr_controllers = 1;
+	rcar_pci.private_data = (void **)&pcie;
+
+	pci_common_init_dev(&pdev->dev, &rcar_pci);
+#ifdef CONFIG_PCI_DOMAINS
+	rcar_pci.domain++;
+#endif
+}
+
+static int phy_wait_for_ack(struct rcar_pcie *pcie)
+{
+	unsigned int timeout = 100;
+
+	while (timeout--) {
+		if (pci_read_reg(pcie, H1_PCIEPHYADRR) & PHY_ACK)
+			return 0;
+
+		udelay(100);
+	}
+
+	dev_err(pcie->dev, "Access to PCIe phy timed out\n");
+
+	return -ETIMEDOUT;
+}
+
+static void phy_write_reg(struct rcar_pcie *pcie,
+				 unsigned int rate, unsigned int addr,
+				 unsigned int lane, unsigned int data)
+{
+	unsigned long phyaddr;
+
+	phyaddr = WRITE_CMD |
+		((rate & 1) << RATE_POS) |
+		((lane & 0xf) << LANE_POS) |
+		((addr & 0xff) << ADR_POS);
+
+	/* Set write data */
+	pci_write_reg(pcie, data, H1_PCIEPHYDOUTR);
+	pci_write_reg(pcie, phyaddr, H1_PCIEPHYADRR);
+
+	/* Ignore errors as they will be dealt with if the data link is down */
+	phy_wait_for_ack(pcie);
+
+	/* Clear command */
+	pci_write_reg(pcie, 0, H1_PCIEPHYDOUTR);
+	pci_write_reg(pcie, 0, H1_PCIEPHYADRR);
+
+	/* Ignore errors as they will be dealt with if the data link is down */
+	phy_wait_for_ack(pcie);
+}
+
+static int rcar_pcie_wait_for_dl(struct rcar_pcie *pcie)
+{
+	unsigned int timeout = 10;
+
+	while (timeout--) {
+		if ((pci_read_reg(pcie, PCIETSTR) & DATA_LINK_ACTIVE))
+			return 0;
+
+		msleep(5);
+	}
+
+	return -ETIMEDOUT;
+}
+
+static int rcar_pcie_hw_init(struct rcar_pcie *pcie)
+{
+	int err;
+
+	/* Begin initialization */
+	pci_write_reg(pcie, 0, PCIETCTLR);
+
+	/* Set mode */
+	pci_write_reg(pcie, 1, PCIEMSR);
+
+	/*
+	 * Initial header for port config space is type 1, set the device
+	 * class to match. Hardware takes care of propagating the IDSETR
+	 * settings, so there is no need to bother with a quirk.
+	 */
+	pci_write_reg(pcie, PCI_CLASS_BRIDGE_PCI << 16, IDSETR1);
+
+	/*
+	 * Setup Secondary Bus Number & Subordinate Bus Number, even though
+	 * they aren't used, to avoid bridge being detected as broken.
+	 */
+	rcar_rmw32(pcie, RCONF(PCI_SECONDARY_BUS), 0xff, 1);
+	rcar_rmw32(pcie, RCONF(PCI_SUBORDINATE_BUS), 0xff, 1);
+
+	/* Initialize default capabilities. */
+	rcar_rmw32(pcie, REXPCAP(0), 0, PCI_CAP_ID_EXP);
+	rcar_rmw32(pcie, REXPCAP(PCI_EXP_FLAGS),
+		PCI_EXP_FLAGS_TYPE, PCI_EXP_TYPE_ROOT_PORT << 4);
+	rcar_rmw32(pcie, RCONF(PCI_HEADER_TYPE), 0x7f,
+		PCI_HEADER_TYPE_BRIDGE);
+
+	/* Enable data link layer active state reporting */
+	rcar_rmw32(pcie, REXPCAP(PCI_EXP_LNKCAP), 0, PCI_EXP_LNKCAP_DLLLARC);
+
+	/* Write out the physical slot number = 0 */
+	rcar_rmw32(pcie, REXPCAP(PCI_EXP_SLTCAP), PCI_EXP_SLTCAP_PSN, 0);
+
+	/* Set the completion timer timeout to the maximum 50ms. */
+	rcar_rmw32(pcie, TLCTLR+1, 0x3f, 50);
+
+	/* Terminate list of capabilities (Next Capability Offset=0) */
+	rcar_rmw32(pcie, RVCCAP(0), 0xfff0, 0);
+
+	/* Enable MAC data scrambling. */
+	rcar_rmw32(pcie, MACCTLR, SCRAMBLE_DISABLE, 0);
+
+	/* Enable MSI */
+	if (IS_ENABLED(CONFIG_PCI_MSI))
+		pci_write_reg(pcie, 0x101f0000, PCIEMSITXR);
+
+	/* Finish initialization - establish a PCI Express link */
+	pci_write_reg(pcie, CFINIT, PCIETCTLR);
+
+	/* This will timeout if we don't have a link. */
+	err = rcar_pcie_wait_for_dl(pcie);
+	if (err)
+		return err;
+
+	/* Enable INTx interrupts */
+	rcar_rmw32(pcie, PCIEINTXR, 0, 0xF << 8);
+
+	/* Enable slave Bus Mastering */
+	rcar_rmw32(pcie, RCONF(PCI_STATUS), PCI_STATUS_DEVSEL_MASK,
+		PCI_COMMAND_IO | PCI_COMMAND_MEMORY | PCI_COMMAND_MASTER |
+		PCI_STATUS_CAP_LIST | PCI_STATUS_DEVSEL_FAST);
+
+	wmb();
+
+	return 0;
+}
+
+static int rcar_pcie_hw_init_h1(struct rcar_pcie *pcie)
+{
+	unsigned int timeout = 10;
+
+	/* Initialize the phy */
+	phy_write_reg(pcie, 0, 0x42, 0x1, 0x0EC34191);
+	phy_write_reg(pcie, 1, 0x42, 0x1, 0x0EC34180);
+	phy_write_reg(pcie, 0, 0x43, 0x1, 0x00210188);
+	phy_write_reg(pcie, 1, 0x43, 0x1, 0x00210188);
+	phy_write_reg(pcie, 0, 0x44, 0x1, 0x015C0014);
+	phy_write_reg(pcie, 1, 0x44, 0x1, 0x015C0014);
+	phy_write_reg(pcie, 1, 0x4C, 0x1, 0x786174A0);
+	phy_write_reg(pcie, 1, 0x4D, 0x1, 0x048000BB);
+	phy_write_reg(pcie, 0, 0x51, 0x1, 0x079EC062);
+	phy_write_reg(pcie, 0, 0x52, 0x1, 0x20000000);
+	phy_write_reg(pcie, 1, 0x52, 0x1, 0x20000000);
+	phy_write_reg(pcie, 1, 0x56, 0x1, 0x00003806);
+
+	phy_write_reg(pcie, 0, 0x60, 0x1, 0x004B03A5);
+	phy_write_reg(pcie, 0, 0x64, 0x1, 0x3F0F1F0F);
+	phy_write_reg(pcie, 0, 0x66, 0x1, 0x00008000);
+
+	while (timeout--) {
+		if (pci_read_reg(pcie, H1_PCIEPHYSR))
+			return rcar_pcie_hw_init(pcie);
+
+		msleep(5);
+	}
+
+	return -ETIMEDOUT;
+}
+
+static int rcar_msi_alloc(struct rcar_msi *chip)
+{
+	int msi;
+
+	mutex_lock(&chip->lock);
+
+	msi = find_first_zero_bit(chip->used, INT_PCI_MSI_NR);
+	if (msi < INT_PCI_MSI_NR)
+		set_bit(msi, chip->used);
+	else
+		msi = -ENOSPC;
+
+	mutex_unlock(&chip->lock);
+
+	return msi;
+}
+
+static void rcar_msi_free(struct rcar_msi *chip, unsigned long irq)
+{
+	mutex_lock(&chip->lock);
+	clear_bit(irq, chip->used);
+	mutex_unlock(&chip->lock);
+}
+
+static irqreturn_t rcar_pcie_msi_irq(int irq, void *data)
+{
+	struct rcar_pcie *pcie = data;
+	struct rcar_msi *msi = &pcie->msi;
+	unsigned long reg;
+
+	reg = pci_read_reg(pcie, PCIEMSIFR);
+
+	/* MSI & INTx share an interrupt - we only handle MSI here */
+	if (!reg)
+		return IRQ_NONE;
+
+	while (reg) {
+		unsigned int index = find_first_bit(&reg, 32);
+		unsigned int irq;
+
+		/* clear the interrupt */
+		pci_write_reg(pcie, 1 << index, PCIEMSIFR);
+
+		irq = irq_find_mapping(msi->domain, index);
+		if (irq) {
+			if (test_bit(index, msi->used))
+				generic_handle_irq(irq);
+			else
+				dev_info(pcie->dev, "unhandled MSI\n");
+		} else {
+			/* Unknown MSI, just clear it */
+			dev_dbg(pcie->dev, "unexpected MSI\n");
+		}
+
+		/* see if there's any more pending in this vector */
+		reg = pci_read_reg(pcie, PCIEMSIFR);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int rcar_msi_setup_irq(struct msi_chip *chip, struct pci_dev *pdev,
+			      struct msi_desc *desc)
+{
+	struct rcar_msi *msi = to_rcar_msi(chip);
+	struct rcar_pcie *pcie = container_of(chip, struct rcar_pcie, msi.chip);
+	struct msi_msg msg;
+	unsigned int irq;
+	int hwirq;
+
+	hwirq = rcar_msi_alloc(msi);
+	if (hwirq < 0)
+		return hwirq;
+
+	irq = irq_create_mapping(msi->domain, hwirq);
+	if (!irq) {
+		rcar_msi_free(msi, hwirq);
+		return -EINVAL;
+	}
+
+	irq_set_msi_desc(irq, desc);
+
+	msg.address_lo = pci_read_reg(pcie, PCIEMSIALR) & ~MSIFE;
+	msg.address_hi = pci_read_reg(pcie, PCIEMSIAUR);
+	msg.data = hwirq;
+
+	write_msi_msg(irq, &msg);
+
+	return 0;
+}
+
+static void rcar_msi_teardown_irq(struct msi_chip *chip, unsigned int irq)
+{
+	struct rcar_msi *msi = to_rcar_msi(chip);
+	struct irq_data *d = irq_get_irq_data(irq);
+
+	rcar_msi_free(msi, d->hwirq);
+}
+
+static struct irq_chip rcar_msi_irq_chip = {
+	.name = "R-Car PCIe MSI",
+	.irq_enable = unmask_msi_irq,
+	.irq_disable = mask_msi_irq,
+	.irq_mask = mask_msi_irq,
+	.irq_unmask = unmask_msi_irq,
+};
+
+static int rcar_msi_map(struct irq_domain *domain, unsigned int irq,
+			irq_hw_number_t hwirq)
+{
+	irq_set_chip_and_handler(irq, &rcar_msi_irq_chip, handle_simple_irq);
+	irq_set_chip_data(irq, domain->host_data);
+	set_irq_flags(irq, IRQF_VALID);
+
+	return 0;
+}
+
+static const struct irq_domain_ops msi_domain_ops = {
+	.map = rcar_msi_map,
+};
+
+static int rcar_pcie_enable_msi(struct rcar_pcie *pcie)
+{
+	struct platform_device *pdev = to_platform_device(pcie->dev);
+	struct rcar_msi *msi = &pcie->msi;
+	unsigned long base;
+	int err;
+
+	mutex_init(&msi->lock);
+
+	msi->chip.dev = pcie->dev;
+	msi->chip.setup_irq = rcar_msi_setup_irq;
+	msi->chip.teardown_irq = rcar_msi_teardown_irq;
+
+	msi->domain = irq_domain_add_linear(pcie->dev->of_node, INT_PCI_MSI_NR,
+					    &msi_domain_ops, &msi->chip);
+	if (!msi->domain) {
+		dev_err(&pdev->dev, "failed to create IRQ domain\n");
+		return -ENOMEM;
+	}
+
+	/* Two irqs are for MSI, but they are also used for non-MSI irqs */
+	err = devm_request_irq(&pdev->dev, msi->irq1, rcar_pcie_msi_irq,
+			       IRQF_SHARED, rcar_msi_irq_chip.name, pcie);
+	if (err < 0) {
+		dev_err(&pdev->dev, "failed to request IRQ: %d\n", err);
+		goto err;
+	}
+
+	err = devm_request_irq(&pdev->dev, msi->irq2, rcar_pcie_msi_irq,
+			       IRQF_SHARED, rcar_msi_irq_chip.name, pcie);
+	if (err < 0) {
+		dev_err(&pdev->dev, "failed to request IRQ: %d\n", err);
+		goto err;
+	}
+
+	/* setup MSI data target */
+	msi->pages = __get_free_pages(GFP_KERNEL, 0);
+	base = virt_to_phys((void *)msi->pages);
+
+	pci_write_reg(pcie, base | MSIFE, PCIEMSIALR);
+	pci_write_reg(pcie, 0, PCIEMSIAUR);
+
+	/* enable all MSI interrupts */
+	pci_write_reg(pcie, 0xffffffff, PCIEMSIIER);
+
+	return 0;
+
+err:
+	irq_domain_remove(msi->domain);
+	return err;
+}
+
+static int rcar_pcie_get_resources(struct platform_device *pdev,
+				   struct rcar_pcie *pcie)
+{
+	struct resource res;
+	int err, i;
+
+	err = of_address_to_resource(pdev->dev.of_node, 0, &res);
+	if (err)
+		return err;
+
+	pcie->clk = devm_clk_get(&pdev->dev, "pcie");
+	if (IS_ERR(pcie->clk)) {
+		dev_err(pcie->dev, "cannot get platform clock\n");
+		return PTR_ERR(pcie->clk);
+	}
+	err = clk_prepare_enable(pcie->clk);
+	if (err)
+		goto fail_clk;
+
+	pcie->bus_clk = devm_clk_get(&pdev->dev, "pcie_bus");
+	if (IS_ERR(pcie->bus_clk)) {
+		dev_err(pcie->dev, "cannot get pcie bus clock\n");
+		err = PTR_ERR(pcie->bus_clk);
+		goto fail_clk;
+	}
+	err = clk_prepare_enable(pcie->bus_clk);
+	if (err)
+		goto err_map_reg;
+
+	i = irq_of_parse_and_map(pdev->dev.of_node, 0);
+	if (i < 0) {
+		dev_err(pcie->dev, "cannot get platform resources for msi interrupt\n");
+		err = -ENOENT;
+		goto err_map_reg;
+	}
+	pcie->msi.irq1 = i;
+
+	i = irq_of_parse_and_map(pdev->dev.of_node, 1);
+	if (i < 0) {
+		dev_err(pcie->dev, "cannot get platform resources for msi interrupt\n");
+		err = -ENOENT;
+		goto err_map_reg;
+	}
+	pcie->msi.irq2 = i;
+
+	pcie->base = devm_ioremap_resource(&pdev->dev, &res);
+	if (IS_ERR(pcie->base)) {
+		err = PTR_ERR(pcie->base);
+		goto err_map_reg;
+	}
+
+	return 0;
+
+err_map_reg:
+	clk_disable_unprepare(pcie->bus_clk);
+fail_clk:
+	clk_disable_unprepare(pcie->clk);
+
+	return err;
+}
+
+static int rcar_pcie_inbound_ranges(struct rcar_pcie *pcie,
+				    struct of_pci_range *range,
+				    int *index)
+{
+	u64 restype = range->flags;
+	u64 cpu_addr = range->cpu_addr;
+	u64 cpu_end = range->cpu_addr + range->size;
+	u64 pci_addr = range->pci_addr;
+	u32 flags = LAM_64BIT | LAR_ENABLE;
+	u64 mask;
+	u64 size;
+	int idx = *index;
+
+	if (restype & IORESOURCE_PREFETCH)
+		flags |= LAM_PREFETCH;
+
+	/*
+	 * If the size of the range is larger than the alignment of the start
+	 * address, we have to use multiple entries to perform the mapping.
+	 */
+	if (cpu_addr > 0) {
+		unsigned long nr_zeros = __ffs64(cpu_addr);
+		u64 alignment = 1ULL << nr_zeros;
+		size = min(range->size, alignment);
+	} else {
+		size = range->size;
+	}
+	/* Hardware supports max 4GiB inbound region */
+	size = min(size, 1ULL << 32);
+
+	mask = roundup_pow_of_two(size) - 1;
+	mask &= ~0xf;
+
+	while (cpu_addr < cpu_end) {
+		/*
+		 * Set up 64-bit inbound regions as the range parser doesn't
+		 * distinguish between 32 and 64-bit types.
+		 */
+		pci_write_reg(pcie, lower_32_bits(pci_addr), PCIEPRAR(idx));
+		pci_write_reg(pcie, lower_32_bits(cpu_addr), PCIELAR(idx));
+		pci_write_reg(pcie, lower_32_bits(mask) | flags, PCIELAMR(idx));
+
+		pci_write_reg(pcie, upper_32_bits(pci_addr), PCIEPRAR(idx+1));
+		pci_write_reg(pcie, upper_32_bits(cpu_addr), PCIELAR(idx+1));
+		pci_write_reg(pcie, 0, PCIELAMR(idx+1));
+
+		pci_addr += size;
+		cpu_addr += size;
+		idx += 2;
+
+		if (idx > MAX_NR_INBOUND_MAPS) {
+			dev_err(pcie->dev, "Failed to map inbound regions!\n");
+			return -EINVAL;
+		}
+	}
+	*index = idx;
+
+	return 0;
+}
+
+static int pci_dma_range_parser_init(struct of_pci_range_parser *parser,
+				     struct device_node *node)
+{
+	const int na = 3, ns = 2;
+	int rlen;
+
+	parser->node = node;
+	parser->pna = of_n_addr_cells(node);
+	parser->np = parser->pna + na + ns;
+
+	parser->range = of_get_property(node, "dma-ranges", &rlen);
+	if (!parser->range)
+		return -ENOENT;
+
+	parser->end = parser->range + rlen / sizeof(__be32);
+	return 0;
+}
+
+static int rcar_pcie_parse_map_dma_ranges(struct rcar_pcie *pcie,
+					  struct device_node *np)
+{
+	struct of_pci_range range;
+	struct of_pci_range_parser parser;
+	int index = 0;
+	int err;
+
+	if (pci_dma_range_parser_init(&parser, np))
+		return -EINVAL;
+
+	/* Get the dma-ranges from DT */
+	for_each_of_pci_range(&parser, &range) {
+		u64 end = range.cpu_addr + range.size - 1;
+		dev_dbg(pcie->dev, "0x%08x 0x%016llx..0x%016llx -> 0x%016llx\n",
+			range.flags, range.cpu_addr, end, range.pci_addr);
+
+		err = rcar_pcie_inbound_ranges(pcie, &range, &index);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
+static const struct of_device_id rcar_pcie_of_match[] = {
+	{ .compatible = "renesas,pcie-r8a7779", .data = rcar_pcie_hw_init_h1 },
+	{ .compatible = "renesas,pcie-r8a7790", .data = rcar_pcie_hw_init },
+	{ .compatible = "renesas,pcie-r8a7791", .data = rcar_pcie_hw_init },
+	{},
+};
+MODULE_DEVICE_TABLE(of, rcar_pcie_of_match);
+
+static int rcar_pcie_probe(struct platform_device *pdev)
+{
+	struct rcar_pcie *pcie;
+	unsigned int data;
+	struct of_pci_range range;
+	struct of_pci_range_parser parser;
+	const struct of_device_id *of_id;
+	int err, win = 0;
+	int (*hw_init_fn)(struct rcar_pcie *);
+
+	pcie = devm_kzalloc(&pdev->dev, sizeof(*pcie), GFP_KERNEL);
+	if (!pcie)
+		return -ENOMEM;
+
+	pcie->dev = &pdev->dev;
+	platform_set_drvdata(pdev, pcie);
+
+	/* Get the bus range */
+	if (of_pci_parse_bus_range(pdev->dev.of_node, &pcie->busn)) {
+		dev_err(&pdev->dev, "failed to parse bus-range property\n");
+		return -EINVAL;
+	}
+
+	if (of_pci_range_parser_init(&parser, pdev->dev.of_node)) {
+		dev_err(&pdev->dev, "missing ranges property\n");
+		return -EINVAL;
+	}
+
+	err = rcar_pcie_get_resources(pdev, pcie);
+	if (err < 0) {
+		dev_err(&pdev->dev, "failed to request resources: %d\n", err);
+		return err;
+	}
+
+	for_each_of_pci_range(&parser, &range) {
+		of_pci_range_to_resource(&range, pdev->dev.of_node,
+						&pcie->res[win++]);
+
+		if (win > PCI_MAX_RESOURCES)
+			break;
+	}
+
+	 err = rcar_pcie_parse_map_dma_ranges(pcie, pdev->dev.of_node);
+	 if (err)
+		return err;
+
+	if (IS_ENABLED(CONFIG_PCI_MSI)) {
+		err = rcar_pcie_enable_msi(pcie);
+		if (err < 0) {
+			dev_err(&pdev->dev,
+				"failed to enable MSI support: %d\n",
+				err);
+			return err;
+		}
+	}
+
+	of_id = of_match_device(rcar_pcie_of_match, pcie->dev);
+	if (!of_id || !of_id->data)
+		return -EINVAL;
+	hw_init_fn = of_id->data;
+
+	/* Failure to get a link might just be that no cards are inserted */
+	err = hw_init_fn(pcie);
+	if (err) {
+		dev_info(&pdev->dev, "PCIe link down\n");
+		return 0;
+	}
+
+	data = pci_read_reg(pcie, MACSR);
+	dev_info(&pdev->dev, "PCIe x%d: link up\n", (data >> 20) & 0x3f);
+
+	rcar_pcie_enable(pcie);
+
+	return 0;
+}
+
+static struct platform_driver rcar_pcie_driver = {
+	.driver = {
+		.name = DRV_NAME,
+		.owner = THIS_MODULE,
+		.of_match_table = rcar_pcie_of_match,
+		.suppress_bind_attrs = true,
+	},
+	.probe = rcar_pcie_probe,
+};
+module_platform_driver(rcar_pcie_driver);
+
+MODULE_AUTHOR("Phil Edworthy <phil.edworthy@renesas.com>");
+MODULE_DESCRIPTION("Renesas R-Car PCIe driver");
+MODULE_LICENSE("GPLv2");
diff --git a/drivers/pci/hotplug-pci.c b/drivers/pci/hotplug-pci.c
index 6258dc260d9..c68366cee6b 100644
--- a/drivers/pci/hotplug-pci.c
+++ b/drivers/pci/hotplug-pci.c
@@ -4,7 +4,7 @@
 #include <linux/export.h>
 #include "pci.h"
 
-int __ref pci_hp_add_bridge(struct pci_dev *dev)
+int pci_hp_add_bridge(struct pci_dev *dev)
 {
 	struct pci_bus *parent = dev->bus;
 	int pass, busnr, start = parent->busn_res.start;
diff --git a/drivers/pci/hotplug/acpiphp_glue.c b/drivers/pci/hotplug/acpiphp_glue.c
index bccc27ee103..75e17833021 100644
--- a/drivers/pci/hotplug/acpiphp_glue.c
+++ b/drivers/pci/hotplug/acpiphp_glue.c
@@ -41,7 +41,6 @@
 
 #define pr_fmt(fmt) "acpiphp_glue: " fmt
 
-#include <linux/init.h>
 #include <linux/module.h>
 
 #include <linux/kernel.h>
@@ -501,7 +500,7 @@ static int acpiphp_rescan_slot(struct acpiphp_slot *slot)
  * This function should be called per *physical slot*,
  * not per each slot object in ACPI namespace.
  */
-static void __ref enable_slot(struct acpiphp_slot *slot)
+static void enable_slot(struct acpiphp_slot *slot)
 {
 	struct pci_dev *dev;
 	struct pci_bus *bus = slot->bus;
@@ -516,8 +515,7 @@ static void __ref enable_slot(struct acpiphp_slot *slot)
 			if (PCI_SLOT(dev->devfn) != slot->device)
 				continue;
 
-			if (dev->hdr_type == PCI_HEADER_TYPE_BRIDGE ||
-			    dev->hdr_type == PCI_HEADER_TYPE_CARDBUS) {
+			if (pci_is_bridge(dev)) {
 				max = pci_scan_bridge(bus, dev, max, pass);
 				if (pass && dev->subordinate) {
 					check_hotplug_bridge(slot, dev);
diff --git a/drivers/pci/hotplug/cpci_hotplug_pci.c b/drivers/pci/hotplug/cpci_hotplug_pci.c
index 8c146485176..f6ef64c2ccb 100644
--- a/drivers/pci/hotplug/cpci_hotplug_pci.c
+++ b/drivers/pci/hotplug/cpci_hotplug_pci.c
@@ -250,7 +250,7 @@ int cpci_led_off(struct slot* slot)
  * Device configuration functions
  */
 
-int __ref cpci_configure_slot(struct slot *slot)
+int cpci_configure_slot(struct slot *slot)
 {
 	struct pci_dev *dev;
 	struct pci_bus *parent;
@@ -289,8 +289,7 @@ int __ref cpci_configure_slot(struct slot *slot)
 	list_for_each_entry(dev, &parent->devices, bus_list)
 		if (PCI_SLOT(dev->devfn) != PCI_SLOT(slot->devfn))
 			continue;
-		if ((dev->hdr_type == PCI_HEADER_TYPE_BRIDGE) ||
-		    (dev->hdr_type == PCI_HEADER_TYPE_CARDBUS))
+		if (pci_is_bridge(dev))
 			pci_hp_add_bridge(dev);
 
 
diff --git a/drivers/pci/hotplug/cpqphp_ctrl.c b/drivers/pci/hotplug/cpqphp_ctrl.c
index 11845b79679..f593585f278 100644
--- a/drivers/pci/hotplug/cpqphp_ctrl.c
+++ b/drivers/pci/hotplug/cpqphp_ctrl.c
@@ -709,7 +709,8 @@ static struct pci_resource *get_max_resource(struct pci_resource **head, u32 siz
 				temp = temp->next;
 			}
 
-			temp->next = max->next;
+			if (temp)
+				temp->next = max->next;
 		}
 
 		max->next = NULL;
diff --git a/drivers/pci/hotplug/cpqphp_nvram.c b/drivers/pci/hotplug/cpqphp_nvram.c
index 76ba8a1c774..9600a392eaa 100644
--- a/drivers/pci/hotplug/cpqphp_nvram.c
+++ b/drivers/pci/hotplug/cpqphp_nvram.c
@@ -34,7 +34,6 @@
 #include <linux/workqueue.h>
 #include <linux/pci.h>
 #include <linux/pci_hotplug.h>
-#include <linux/init.h>
 #include <asm/uaccess.h>
 #include "cpqphp.h"
 #include "cpqphp_nvram.h"
diff --git a/drivers/pci/hotplug/pciehp.h b/drivers/pci/hotplug/pciehp.h
index 8a66866b8cf..8e9012dca45 100644
--- a/drivers/pci/hotplug/pciehp.h
+++ b/drivers/pci/hotplug/pciehp.h
@@ -127,7 +127,7 @@ struct controller {
 #define HP_SUPR_RM(ctrl)	((ctrl)->slot_cap & PCI_EXP_SLTCAP_HPS)
 #define EMI(ctrl)		((ctrl)->slot_cap & PCI_EXP_SLTCAP_EIP)
 #define NO_CMD_CMPL(ctrl)	((ctrl)->slot_cap & PCI_EXP_SLTCAP_NCCS)
-#define PSN(ctrl)		((ctrl)->slot_cap >> 19)
+#define PSN(ctrl)		(((ctrl)->slot_cap & PCI_EXP_SLTCAP_PSN) >> 19)
 
 int pciehp_sysfs_enable_slot(struct slot *slot);
 int pciehp_sysfs_disable_slot(struct slot *slot);
diff --git a/drivers/pci/hotplug/pciehp_hpc.c b/drivers/pci/hotplug/pciehp_hpc.c
index d7d058fa19a..1463412cf7f 100644
--- a/drivers/pci/hotplug/pciehp_hpc.c
+++ b/drivers/pci/hotplug/pciehp_hpc.c
@@ -159,6 +159,8 @@ static void pcie_write_cmd(struct controller *ctrl, u16 cmd, u16 mask)
 
 	pcie_capability_read_word(pdev, PCI_EXP_SLTSTA, &slot_status);
 	if (slot_status & PCI_EXP_SLTSTA_CC) {
+		pcie_capability_write_word(pdev, PCI_EXP_SLTSTA,
+					   PCI_EXP_SLTSTA_CC);
 		if (!ctrl->no_cmd_complete) {
 			/*
 			 * After 1 sec and CMD_COMPLETED still not set, just
diff --git a/drivers/pci/hotplug/pciehp_pci.c b/drivers/pci/hotplug/pciehp_pci.c
index 1b533060ce6..b6cb1df6709 100644
--- a/drivers/pci/hotplug/pciehp_pci.c
+++ b/drivers/pci/hotplug/pciehp_pci.c
@@ -62,8 +62,7 @@ int pciehp_configure_device(struct slot *p_slot)
 	}
 
 	list_for_each_entry(dev, &parent->devices, bus_list)
-		if ((dev->hdr_type == PCI_HEADER_TYPE_BRIDGE) ||
-				(dev->hdr_type == PCI_HEADER_TYPE_CARDBUS))
+		if (pci_is_bridge(dev))
 			pci_hp_add_bridge(dev);
 
 	pci_assign_unassigned_bridge_resources(bridge);
diff --git a/drivers/pci/hotplug/pcihp_slot.c b/drivers/pci/hotplug/pcihp_slot.c
index 16f92035231..e246a10a0d2 100644
--- a/drivers/pci/hotplug/pcihp_slot.c
+++ b/drivers/pci/hotplug/pcihp_slot.c
@@ -160,8 +160,7 @@ void pci_configure_slot(struct pci_dev *dev)
 			(dev->class >> 8) == PCI_CLASS_BRIDGE_PCI)))
 		return;
 
-	if (dev->bus)
-		pcie_bus_configure_settings(dev->bus);
+	pcie_bus_configure_settings(dev->bus);
 
 	memset(&hpp, 0, sizeof(hpp));
 	ret = pci_get_hp_params(dev, &hpp);
diff --git a/drivers/pci/hotplug/rpadlpar_core.c b/drivers/pci/hotplug/rpadlpar_core.c
index 4fcdeedda31..7660232ef46 100644
--- a/drivers/pci/hotplug/rpadlpar_core.c
+++ b/drivers/pci/hotplug/rpadlpar_core.c
@@ -157,8 +157,7 @@ static void dlpar_pci_add_bus(struct device_node *dn)
 	}
 
 	/* Scan below the new bridge */
-	if (dev->hdr_type == PCI_HEADER_TYPE_BRIDGE ||
-	    dev->hdr_type == PCI_HEADER_TYPE_CARDBUS)
+	if (pci_is_bridge(dev))
 		of_scan_pci_bridge(dev);
 
 	/* Map IO space for child bus, which may or may not succeed */
diff --git a/drivers/pci/hotplug/rpaphp_core.c b/drivers/pci/hotplug/rpaphp_core.c
index 4796c15fba9..984d708552f 100644
--- a/drivers/pci/hotplug/rpaphp_core.c
+++ b/drivers/pci/hotplug/rpaphp_core.c
@@ -223,16 +223,16 @@ int rpaphp_get_drc_props(struct device_node *dn, int *drc_index,
 	type_tmp = (char *) &types[1];
 
 	/* Iterate through parent properties, looking for my-drc-index */
-	for (i = 0; i < indexes[0]; i++) {
+	for (i = 0; i < be32_to_cpu(indexes[0]); i++) {
 		if ((unsigned int) indexes[i + 1] == *my_index) {
 			if (drc_name)
 				*drc_name = name_tmp;
 			if (drc_type)
 				*drc_type = type_tmp;
 			if (drc_index)
-				*drc_index = *my_index;
+				*drc_index = be32_to_cpu(*my_index);
 			if (drc_power_domain)
-				*drc_power_domain = domains[i+1];
+				*drc_power_domain = be32_to_cpu(domains[i+1]);
 			return 0;
 		}
 		name_tmp += (strlen(name_tmp) + 1);
@@ -321,16 +321,19 @@ int rpaphp_add_slot(struct device_node *dn)
 	/* register PCI devices */
 	name = (char *) &names[1];
 	type = (char *) &types[1];
-	for (i = 0; i < indexes[0]; i++) {
+	for (i = 0; i < be32_to_cpu(indexes[0]); i++) {
+		int index;
 
-		slot = alloc_slot_struct(dn, indexes[i + 1], name, power_domains[i + 1]);
+		index = be32_to_cpu(indexes[i + 1]);
+		slot = alloc_slot_struct(dn, index, name,
+					 be32_to_cpu(power_domains[i + 1]));
 		if (!slot)
 			return -ENOMEM;
 
 		slot->type = simple_strtoul(type, NULL, 10);
 
 		dbg("Found drc-index:0x%x drc-name:%s drc-type:%s\n",
-				indexes[i + 1], name, type);
+				index, name, type);
 
 		retval = rpaphp_enable_slot(slot);
 		if (!retval)
diff --git a/drivers/pci/hotplug/s390_pci_hpc.c b/drivers/pci/hotplug/s390_pci_hpc.c
index 8d2ce22151e..d1332d2f873 100644
--- a/drivers/pci/hotplug/s390_pci_hpc.c
+++ b/drivers/pci/hotplug/s390_pci_hpc.c
@@ -15,7 +15,6 @@
 #include <linux/slab.h>
 #include <linux/pci.h>
 #include <linux/pci_hotplug.h>
-#include <linux/init.h>
 #include <asm/pci_debug.h>
 #include <asm/sclp.h>
 
diff --git a/drivers/pci/hotplug/shpchp_ctrl.c b/drivers/pci/hotplug/shpchp_ctrl.c
index 58499277903..6efc2ec5e4d 100644
--- a/drivers/pci/hotplug/shpchp_ctrl.c
+++ b/drivers/pci/hotplug/shpchp_ctrl.c
@@ -282,8 +282,8 @@ static int board_added(struct slot *p_slot)
 		return WRONG_BUS_FREQUENCY;
 	}
 
-	bsp = ctrl->pci_dev->bus->cur_bus_speed;
-	msp = ctrl->pci_dev->bus->max_bus_speed;
+	bsp = ctrl->pci_dev->subordinate->cur_bus_speed;
+	msp = ctrl->pci_dev->subordinate->max_bus_speed;
 
 	/* Check if there are other slots or devices on the same bus */
 	if (!list_empty(&ctrl->pci_dev->subordinate->devices))
diff --git a/drivers/pci/hotplug/shpchp_pci.c b/drivers/pci/hotplug/shpchp_pci.c
index 2bf69fe1926..9202d133485 100644
--- a/drivers/pci/hotplug/shpchp_pci.c
+++ b/drivers/pci/hotplug/shpchp_pci.c
@@ -34,7 +34,7 @@
 #include "../pci.h"
 #include "shpchp.h"
 
-int __ref shpchp_configure_device(struct slot *p_slot)
+int shpchp_configure_device(struct slot *p_slot)
 {
 	struct pci_dev *dev;
 	struct controller *ctrl = p_slot->ctrl;
@@ -64,8 +64,7 @@ int __ref shpchp_configure_device(struct slot *p_slot)
 	list_for_each_entry(dev, &parent->devices, bus_list) {
 		if (PCI_SLOT(dev->devfn) != p_slot->device)
 			continue;
-		if ((dev->hdr_type == PCI_HEADER_TYPE_BRIDGE) ||
-		    (dev->hdr_type == PCI_HEADER_TYPE_CARDBUS))
+		if (pci_is_bridge(dev))
 			pci_hp_add_bridge(dev);
 	}
 
diff --git a/drivers/pci/iov.c b/drivers/pci/iov.c
index de7a74782f9..cb6f24740ee 100644
--- a/drivers/pci/iov.c
+++ b/drivers/pci/iov.c
@@ -106,7 +106,7 @@ static int virtfn_add(struct pci_dev *dev, int id, int reset)
 	pci_device_add(virtfn, virtfn->bus);
 	mutex_unlock(&iov->dev->sriov->lock);
 
-	rc = pci_bus_add_device(virtfn);
+	pci_bus_add_device(virtfn);
 	sprintf(buf, "virtfn%u", id);
 	rc = sysfs_create_link(&dev->dev.kobj, &virtfn->dev.kobj, buf);
 	if (rc)
diff --git a/drivers/pci/msi.c b/drivers/pci/msi.c
index 955ab7990c5..27a7e67ddfe 100644
--- a/drivers/pci/msi.c
+++ b/drivers/pci/msi.c
@@ -10,7 +10,6 @@
 #include <linux/mm.h>
 #include <linux/irq.h>
 #include <linux/interrupt.h>
-#include <linux/init.h>
 #include <linux/export.h>
 #include <linux/ioport.h>
 #include <linux/pci.h>
@@ -544,22 +543,18 @@ static int populate_msi_sysfs(struct pci_dev *pdev)
 	if (!msi_attrs)
 		return -ENOMEM;
 	list_for_each_entry(entry, &pdev->msi_list, list) {
-		char *name = kmalloc(20, GFP_KERNEL);
-		if (!name)
-			goto error_attrs;
-
 		msi_dev_attr = kzalloc(sizeof(*msi_dev_attr), GFP_KERNEL);
-		if (!msi_dev_attr) {
-			kfree(name);
+		if (!msi_dev_attr)
 			goto error_attrs;
-		}
+		msi_attrs[count] = &msi_dev_attr->attr;
 
-		sprintf(name, "%d", entry->irq);
 		sysfs_attr_init(&msi_dev_attr->attr);
-		msi_dev_attr->attr.name = name;
+		msi_dev_attr->attr.name = kasprintf(GFP_KERNEL, "%d",
+						    entry->irq);
+		if (!msi_dev_attr->attr.name)
+			goto error_attrs;
 		msi_dev_attr->attr.mode = S_IRUGO;
 		msi_dev_attr->show = msi_mode_show;
-		msi_attrs[count] = &msi_dev_attr->attr;
 		++count;
 	}
 
@@ -883,50 +878,6 @@ int pci_msi_vec_count(struct pci_dev *dev)
 }
 EXPORT_SYMBOL(pci_msi_vec_count);
 
-/**
- * pci_enable_msi_block - configure device's MSI capability structure
- * @dev: device to configure
- * @nvec: number of interrupts to configure
- *
- * Allocate IRQs for a device with the MSI capability.
- * This function returns a negative errno if an error occurs.  If it
- * is unable to allocate the number of interrupts requested, it returns
- * the number of interrupts it might be able to allocate.  If it successfully
- * allocates at least the number of interrupts requested, it returns 0 and
- * updates the @dev's irq member to the lowest new interrupt number; the
- * other interrupt numbers allocated to this device are consecutive.
- */
-int pci_enable_msi_block(struct pci_dev *dev, int nvec)
-{
-	int status, maxvec;
-
-	if (dev->current_state != PCI_D0)
-		return -EINVAL;
-
-	maxvec = pci_msi_vec_count(dev);
-	if (maxvec < 0)
-		return maxvec;
-	if (nvec > maxvec)
-		return maxvec;
-
-	status = pci_msi_check_device(dev, nvec, PCI_CAP_ID_MSI);
-	if (status)
-		return status;
-
-	WARN_ON(!!dev->msi_enabled);
-
-	/* Check whether driver already requested MSI-X irqs */
-	if (dev->msix_enabled) {
-		dev_info(&dev->dev, "can't enable MSI "
-			 "(MSI-X already enabled)\n");
-		return -EINVAL;
-	}
-
-	status = msi_capability_init(dev, nvec);
-	return status;
-}
-EXPORT_SYMBOL(pci_enable_msi_block);
-
 void pci_msi_shutdown(struct pci_dev *dev)
 {
 	struct msi_desc *desc;
@@ -1132,14 +1083,45 @@ void pci_msi_init_pci_dev(struct pci_dev *dev)
  **/
 int pci_enable_msi_range(struct pci_dev *dev, int minvec, int maxvec)
 {
-	int nvec = maxvec;
+	int nvec;
 	int rc;
 
+	if (dev->current_state != PCI_D0)
+		return -EINVAL;
+
+	WARN_ON(!!dev->msi_enabled);
+
+	/* Check whether driver already requested MSI-X irqs */
+	if (dev->msix_enabled) {
+		dev_info(&dev->dev,
+			 "can't enable MSI (MSI-X already enabled)\n");
+		return -EINVAL;
+	}
+
 	if (maxvec < minvec)
 		return -ERANGE;
 
+	nvec = pci_msi_vec_count(dev);
+	if (nvec < 0)
+		return nvec;
+	else if (nvec < minvec)
+		return -EINVAL;
+	else if (nvec > maxvec)
+		nvec = maxvec;
+
+	do {
+		rc = pci_msi_check_device(dev, nvec, PCI_CAP_ID_MSI);
+		if (rc < 0) {
+			return rc;
+		} else if (rc > 0) {
+			if (rc < minvec)
+				return -ENOSPC;
+			nvec = rc;
+		}
+	} while (rc);
+
 	do {
-		rc = pci_enable_msi_block(dev, nvec);
+		rc = msi_capability_init(dev, nvec);
 		if (rc < 0) {
 			return rc;
 		} else if (rc > 0) {
diff --git a/drivers/pci/pci-acpi.c b/drivers/pci/pci-acpi.c
index f49abef8848..ca4927ba843 100644
--- a/drivers/pci/pci-acpi.c
+++ b/drivers/pci/pci-acpi.c
@@ -309,13 +309,7 @@ static struct acpi_device *acpi_pci_find_companion(struct device *dev)
 	bool check_children;
 	u64 addr;
 
-	/*
-	 * pci_is_bridge() is not suitable here, because pci_dev->subordinate
-	 * is set only after acpi_pci_find_device() has been called for the
-	 * given device.
-	 */
-	check_children = pci_dev->hdr_type == PCI_HEADER_TYPE_BRIDGE
-			|| pci_dev->hdr_type == PCI_HEADER_TYPE_CARDBUS;
+	check_children = pci_is_bridge(pci_dev);
 	/* Please ref to ACPI spec for the syntax of _ADR */
 	addr = (PCI_SLOT(pci_dev->devfn) << 16) | PCI_FUNC(pci_dev->devfn);
 	return acpi_find_child_device(ACPI_COMPANION(dev->parent), addr,
diff --git a/drivers/pci/pci-driver.c b/drivers/pci/pci-driver.c
index d911e0c1f35..837d71f5390 100644
--- a/drivers/pci/pci-driver.c
+++ b/drivers/pci/pci-driver.c
@@ -107,7 +107,7 @@ store_new_id(struct device_driver *driver, const char *buf, size_t count)
 		subdevice=PCI_ANY_ID, class=0, class_mask=0;
 	unsigned long driver_data=0;
 	int fields=0;
-	int retval;
+	int retval = 0;
 
 	fields = sscanf(buf, "%x %x %x %x %x %x %lx",
 			&vendor, &device, &subvendor, &subdevice,
@@ -115,6 +115,26 @@ store_new_id(struct device_driver *driver, const char *buf, size_t count)
 	if (fields < 2)
 		return -EINVAL;
 
+	if (fields != 7) {
+		struct pci_dev *pdev = kzalloc(sizeof(*pdev), GFP_KERNEL);
+		if (!pdev)
+			return -ENOMEM;
+
+		pdev->vendor = vendor;
+		pdev->device = device;
+		pdev->subsystem_vendor = subvendor;
+		pdev->subsystem_device = subdevice;
+		pdev->class = class;
+
+		if (pci_match_id(pdrv->id_table, pdev))
+			retval = -EEXIST;
+
+		kfree(pdev);
+
+		if (retval)
+			return retval;
+	}
+
 	/* Only accept driver_data values that match an existing id_table
 	   entry */
 	if (ids) {
@@ -216,6 +236,13 @@ const struct pci_device_id *pci_match_id(const struct pci_device_id *ids,
 	return NULL;
 }
 
+static const struct pci_device_id pci_device_id_any = {
+	.vendor = PCI_ANY_ID,
+	.device = PCI_ANY_ID,
+	.subvendor = PCI_ANY_ID,
+	.subdevice = PCI_ANY_ID,
+};
+
 /**
  * pci_match_device - Tell if a PCI device structure has a matching PCI device id structure
  * @drv: the PCI driver to match against
@@ -229,18 +256,30 @@ static const struct pci_device_id *pci_match_device(struct pci_driver *drv,
 						    struct pci_dev *dev)
 {
 	struct pci_dynid *dynid;
+	const struct pci_device_id *found_id = NULL;
+
+	/* When driver_override is set, only bind to the matching driver */
+	if (dev->driver_override && strcmp(dev->driver_override, drv->name))
+		return NULL;
 
 	/* Look at the dynamic ids first, before the static ones */
 	spin_lock(&drv->dynids.lock);
 	list_for_each_entry(dynid, &drv->dynids.list, node) {
 		if (pci_match_one_device(&dynid->id, dev)) {
-			spin_unlock(&drv->dynids.lock);
-			return &dynid->id;
+			found_id = &dynid->id;
+			break;
 		}
 	}
 	spin_unlock(&drv->dynids.lock);
 
-	return pci_match_id(drv->id_table, dev);
+	if (!found_id)
+		found_id = pci_match_id(drv->id_table, dev);
+
+	/* driver_override will always match, send a dummy id */
+	if (!found_id && dev->driver_override)
+		found_id = &pci_device_id_any;
+
+	return found_id;
 }
 
 struct drv_dev_and_id {
@@ -580,14 +619,14 @@ static void pci_pm_default_resume(struct pci_dev *pci_dev)
 {
 	pci_fixup_device(pci_fixup_resume, pci_dev);
 
-	if (!pci_is_bridge(pci_dev))
+	if (!pci_has_subordinate(pci_dev))
 		pci_enable_wake(pci_dev, PCI_D0, false);
 }
 
 static void pci_pm_default_suspend(struct pci_dev *pci_dev)
 {
 	/* Disable non-bridge devices without PM support */
-	if (!pci_is_bridge(pci_dev))
+	if (!pci_has_subordinate(pci_dev))
 		pci_disable_enabled_device(pci_dev);
 }
 
@@ -717,7 +756,7 @@ static int pci_pm_suspend_noirq(struct device *dev)
 
 	if (!pci_dev->state_saved) {
 		pci_save_state(pci_dev);
-		if (!pci_is_bridge(pci_dev))
+		if (!pci_has_subordinate(pci_dev))
 			pci_prepare_to_sleep(pci_dev);
 	}
 
@@ -971,7 +1010,7 @@ static int pci_pm_poweroff_noirq(struct device *dev)
 			return error;
 	}
 
-	if (!pci_dev->state_saved && !pci_is_bridge(pci_dev))
+	if (!pci_dev->state_saved && !pci_has_subordinate(pci_dev))
 		pci_prepare_to_sleep(pci_dev);
 
 	/*
@@ -1325,8 +1364,6 @@ static int pci_uevent(struct device *dev, struct kobj_uevent_env *env)
 		return -ENODEV;
 
 	pdev = to_pci_dev(dev);
-	if (!pdev)
-		return -ENODEV;
 
 	if (add_uevent_var(env, "PCI_CLASS=%04X", pdev->class))
 		return -ENOMEM;
@@ -1347,6 +1384,7 @@ static int pci_uevent(struct device *dev, struct kobj_uevent_env *env)
 			   (u8)(pdev->class >> 16), (u8)(pdev->class >> 8),
 			   (u8)(pdev->class)))
 		return -ENOMEM;
+
 	return 0;
 }
 
diff --git a/drivers/pci/pci-sysfs.c b/drivers/pci/pci-sysfs.c
index 4e0acefb756..84c350994b0 100644
--- a/drivers/pci/pci-sysfs.c
+++ b/drivers/pci/pci-sysfs.c
@@ -29,6 +29,7 @@
 #include <linux/slab.h>
 #include <linux/vgaarb.h>
 #include <linux/pm_runtime.h>
+#include <linux/of.h>
 #include "pci.h"
 
 static int sysfs_initialized;	/* = 0 */
@@ -416,6 +417,20 @@ static ssize_t d3cold_allowed_show(struct device *dev,
 static DEVICE_ATTR_RW(d3cold_allowed);
 #endif
 
+#ifdef CONFIG_OF
+static ssize_t devspec_show(struct device *dev,
+			    struct device_attribute *attr, char *buf)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct device_node *np = pci_device_to_OF_node(pdev);
+
+	if (np == NULL || np->full_name == NULL)
+		return 0;
+	return sprintf(buf, "%s", np->full_name);
+}
+static DEVICE_ATTR_RO(devspec);
+#endif
+
 #ifdef CONFIG_PCI_IOV
 static ssize_t sriov_totalvfs_show(struct device *dev,
 				   struct device_attribute *attr,
@@ -499,6 +514,45 @@ static struct device_attribute sriov_numvfs_attr =
 		       sriov_numvfs_show, sriov_numvfs_store);
 #endif /* CONFIG_PCI_IOV */
 
+static ssize_t driver_override_store(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t count)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	char *driver_override, *old = pdev->driver_override, *cp;
+
+	if (count > PATH_MAX)
+		return -EINVAL;
+
+	driver_override = kstrndup(buf, count, GFP_KERNEL);
+	if (!driver_override)
+		return -ENOMEM;
+
+	cp = strchr(driver_override, '\n');
+	if (cp)
+		*cp = '\0';
+
+	if (strlen(driver_override)) {
+		pdev->driver_override = driver_override;
+	} else {
+		kfree(driver_override);
+		pdev->driver_override = NULL;
+	}
+
+	kfree(old);
+
+	return count;
+}
+
+static ssize_t driver_override_show(struct device *dev,
+				    struct device_attribute *attr, char *buf)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+
+	return sprintf(buf, "%s\n", pdev->driver_override);
+}
+static DEVICE_ATTR_RW(driver_override);
+
 static struct attribute *pci_dev_attrs[] = {
 	&dev_attr_resource.attr,
 	&dev_attr_vendor.attr,
@@ -521,6 +575,10 @@ static struct attribute *pci_dev_attrs[] = {
 #if defined(CONFIG_PM_RUNTIME) && defined(CONFIG_ACPI)
 	&dev_attr_d3cold_allowed.attr,
 #endif
+#ifdef CONFIG_OF
+	&dev_attr_devspec.attr,
+#endif
+	&dev_attr_driver_override.attr,
 	NULL,
 };
 
@@ -1255,11 +1313,6 @@ static struct bin_attribute pcie_config_attr = {
 	.write = pci_write_config,
 };
 
-int __weak pcibios_add_platform_entries(struct pci_dev *dev)
-{
-	return 0;
-}
-
 static ssize_t reset_store(struct device *dev,
 			   struct device_attribute *attr, const char *buf,
 			   size_t count)
@@ -1375,11 +1428,6 @@ int __must_check pci_create_sysfs_dev_files (struct pci_dev *pdev)
 		pdev->rom_attr = attr;
 	}
 
-	/* add platform-specific attributes */
-	retval = pcibios_add_platform_entries(pdev);
-	if (retval)
-		goto err_rom_file;
-
 	/* add sysfs entries for various capabilities */
 	retval = pci_create_capabilities_sysfs(pdev);
 	if (retval)
diff --git a/drivers/pci/pci.c b/drivers/pci/pci.c
index 7325d43bf03..7ae7aa0166b 100644
--- a/drivers/pci/pci.c
+++ b/drivers/pci/pci.c
@@ -1468,6 +1468,17 @@ void __weak pcibios_release_device(struct pci_dev *dev) {}
  */
 void __weak pcibios_disable_device (struct pci_dev *dev) {}
 
+/**
+ * pcibios_penalize_isa_irq - penalize an ISA IRQ
+ * @irq: ISA IRQ to penalize
+ * @active: IRQ active or not
+ *
+ * Permits the platform to provide architecture-specific functionality when
+ * penalizing ISA IRQs. This is the default implementation. Architecture
+ * implementations can override this.
+ */
+void __weak pcibios_penalize_isa_irq(int irq, int active) {}
+
 static void do_pci_disable_device(struct pci_dev *dev)
 {
 	u16 pci_command;
@@ -3067,7 +3078,8 @@ int pci_wait_for_pending_transaction(struct pci_dev *dev)
 	if (!pci_is_pcie(dev))
 		return 1;
 
-	return pci_wait_for_pending(dev, PCI_EXP_DEVSTA, PCI_EXP_DEVSTA_TRPND);
+	return pci_wait_for_pending(dev, pci_pcie_cap(dev) + PCI_EXP_DEVSTA,
+				    PCI_EXP_DEVSTA_TRPND);
 }
 EXPORT_SYMBOL(pci_wait_for_pending_transaction);
 
@@ -3109,7 +3121,7 @@ static int pci_af_flr(struct pci_dev *dev, int probe)
 		return 0;
 
 	/* Wait for Transaction Pending bit clean */
-	if (pci_wait_for_pending(dev, PCI_AF_STATUS, PCI_AF_STATUS_TP))
+	if (pci_wait_for_pending(dev, pos + PCI_AF_STATUS, PCI_AF_STATUS_TP))
 		goto clear;
 
 	dev_err(&dev->dev, "transaction is not cleared; "
@@ -3305,8 +3317,27 @@ static void pci_dev_unlock(struct pci_dev *dev)
 	pci_cfg_access_unlock(dev);
 }
 
+/**
+ * pci_reset_notify - notify device driver of reset
+ * @dev: device to be notified of reset
+ * @prepare: 'true' if device is about to be reset; 'false' if reset attempt
+ *           completed
+ *
+ * Must be called prior to device access being disabled and after device
+ * access is restored.
+ */
+static void pci_reset_notify(struct pci_dev *dev, bool prepare)
+{
+	const struct pci_error_handlers *err_handler =
+			dev->driver ? dev->driver->err_handler : NULL;
+	if (err_handler && err_handler->reset_notify)
+		err_handler->reset_notify(dev, prepare);
+}
+
 static void pci_dev_save_and_disable(struct pci_dev *dev)
 {
+	pci_reset_notify(dev, true);
+
 	/*
 	 * Wake-up device prior to save.  PM registers default to D0 after
 	 * reset and a simple register restore doesn't reliably return
@@ -3328,6 +3359,7 @@ static void pci_dev_save_and_disable(struct pci_dev *dev)
 static void pci_dev_restore(struct pci_dev *dev)
 {
 	pci_restore_state(dev);
+	pci_reset_notify(dev, false);
 }
 
 static int pci_dev_reset(struct pci_dev *dev, int probe)
@@ -3344,6 +3376,7 @@ static int pci_dev_reset(struct pci_dev *dev, int probe)
 
 	return rc;
 }
+
 /**
  * __pci_reset_function - reset a PCI device function
  * @dev: PCI device to reset
@@ -4125,7 +4158,7 @@ int pci_set_vga_state(struct pci_dev *dev, bool decode,
 	u16 cmd;
 	int rc;
 
-	WARN_ON((flags & PCI_VGA_STATE_CHANGE_DECODES) & (command_bits & ~(PCI_COMMAND_IO|PCI_COMMAND_MEMORY)));
+	WARN_ON((flags & PCI_VGA_STATE_CHANGE_DECODES) && (command_bits & ~(PCI_COMMAND_IO|PCI_COMMAND_MEMORY)));
 
 	/* ARCH specific VGA enables */
 	rc = pci_set_vga_state_arch(dev, decode, command_bits, flags);
diff --git a/drivers/pci/pci.h b/drivers/pci/pci.h
index 6bd082299e3..0601890db22 100644
--- a/drivers/pci/pci.h
+++ b/drivers/pci/pci.h
@@ -77,7 +77,7 @@ static inline void pci_wakeup_event(struct pci_dev *dev)
 	pm_wakeup_event(&dev->dev, 100);
 }
 
-static inline bool pci_is_bridge(struct pci_dev *pci_dev)
+static inline bool pci_has_subordinate(struct pci_dev *pci_dev)
 {
 	return !!(pci_dev->subordinate);
 }
@@ -201,11 +201,11 @@ 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);
-void __ref __pci_bus_size_bridges(struct pci_bus *bus,
+void __pci_bus_size_bridges(struct pci_bus *bus,
 			struct list_head *realloc_head);
-void __ref __pci_bus_assign_resources(const struct pci_bus *bus,
-				      struct list_head *realloc_head,
-				      struct list_head *fail_head);
+void __pci_bus_assign_resources(const struct pci_bus *bus,
+				struct list_head *realloc_head,
+				struct list_head *fail_head);
 
 /**
  * pci_ari_enabled - query ARI forwarding status
diff --git a/drivers/pci/pcie/portdrv_core.c b/drivers/pci/pcie/portdrv_core.c
index 986f8eadfd3..2f0ce668a77 100644
--- a/drivers/pci/pcie/portdrv_core.c
+++ b/drivers/pci/pcie/portdrv_core.c
@@ -99,7 +99,7 @@ static int pcie_port_enable_msix(struct pci_dev *dev, int *vectors, int mask)
 	for (i = 0; i < nr_entries; i++)
 		msix_entries[i].entry = i;
 
-	status = pci_enable_msix(dev, msix_entries, nr_entries);
+	status = pci_enable_msix_exact(dev, msix_entries, nr_entries);
 	if (status)
 		goto Exit;
 
@@ -171,7 +171,7 @@ static int pcie_port_enable_msix(struct pci_dev *dev, int *vectors, int mask)
 		pci_disable_msix(dev);
 
 		/* Now allocate the MSI-X vectors for real */
-		status = pci_enable_msix(dev, msix_entries, nvec);
+		status = pci_enable_msix_exact(dev, msix_entries, nvec);
 		if (status)
 			goto Exit;
 	}
@@ -379,10 +379,13 @@ int pcie_port_device_register(struct pci_dev *dev)
 	/*
 	 * Initialize service irqs. Don't use service devices that
 	 * require interrupts if there is no way to generate them.
+	 * However, some drivers may have a polling mode (e.g. pciehp_poll_mode)
+	 * that can be used in the absence of irqs.  Allow them to determine
+	 * if that is to be used.
 	 */
 	status = init_service_irqs(dev, irqs, capabilities);
 	if (status) {
-		capabilities &= PCIE_PORT_SERVICE_VC;
+		capabilities &= PCIE_PORT_SERVICE_VC | PCIE_PORT_SERVICE_HP;
 		if (!capabilities)
 			goto error_disable;
 	}
diff --git a/drivers/pci/probe.c b/drivers/pci/probe.c
index ef09f5f2fe6..2bbf5221afb 100644
--- a/drivers/pci/probe.c
+++ b/drivers/pci/probe.c
@@ -171,9 +171,10 @@ int __pci_read_base(struct pci_dev *dev, enum pci_bar_type type,
 			struct resource *res, unsigned int pos)
 {
 	u32 l, sz, mask;
+	u64 l64, sz64, mask64;
 	u16 orig_cmd;
 	struct pci_bus_region region, inverted_region;
-	bool bar_too_big = false, bar_disabled = false;
+	bool bar_too_big = false, bar_too_high = false, bar_invalid = false;
 
 	mask = type ? PCI_ROM_ADDRESS_MASK : ~0;
 
@@ -226,9 +227,9 @@ int __pci_read_base(struct pci_dev *dev, enum pci_bar_type type,
 	}
 
 	if (res->flags & IORESOURCE_MEM_64) {
-		u64 l64 = l;
-		u64 sz64 = sz;
-		u64 mask64 = mask | (u64)~0 << 32;
+		l64 = l;
+		sz64 = sz;
+		mask64 = mask | (u64)~0 << 32;
 
 		pci_read_config_dword(dev, pos + 4, &l);
 		pci_write_config_dword(dev, pos + 4, ~0);
@@ -243,19 +244,22 @@ int __pci_read_base(struct pci_dev *dev, enum pci_bar_type type,
 		if (!sz64)
 			goto fail;
 
-		if ((sizeof(resource_size_t) < 8) && (sz64 > 0x100000000ULL)) {
+		if ((sizeof(dma_addr_t) < 8 || sizeof(resource_size_t) < 8) &&
+		    sz64 > 0x100000000ULL) {
+			res->flags |= IORESOURCE_UNSET | IORESOURCE_DISABLED;
+			res->start = 0;
+			res->end = 0;
 			bar_too_big = true;
-			goto fail;
+			goto out;
 		}
 
-		if ((sizeof(resource_size_t) < 8) && l) {
-			/* Address above 32-bit boundary; disable the BAR */
-			pci_write_config_dword(dev, pos, 0);
-			pci_write_config_dword(dev, pos + 4, 0);
+		if ((sizeof(dma_addr_t) < 8) && l) {
+			/* Above 32-bit boundary; try to reallocate */
 			res->flags |= IORESOURCE_UNSET;
-			region.start = 0;
-			region.end = sz64;
-			bar_disabled = true;
+			res->start = 0;
+			res->end = sz64;
+			bar_too_high = true;
+			goto out;
 		} else {
 			region.start = l64;
 			region.end = l64 + sz64;
@@ -285,11 +289,10 @@ int __pci_read_base(struct pci_dev *dev, enum pci_bar_type type,
 	 * be claimed by the device.
 	 */
 	if (inverted_region.start != region.start) {
-		dev_info(&dev->dev, "reg 0x%x: initial BAR value %pa invalid; forcing reassignment\n",
-			 pos, &region.start);
 		res->flags |= IORESOURCE_UNSET;
-		res->end -= res->start;
 		res->start = 0;
+		res->end = region.end - region.start;
+		bar_invalid = true;
 	}
 
 	goto out;
@@ -303,8 +306,15 @@ out:
 		pci_write_config_word(dev, PCI_COMMAND, orig_cmd);
 
 	if (bar_too_big)
-		dev_err(&dev->dev, "reg 0x%x: can't handle 64-bit BAR\n", pos);
-	if (res->flags && !bar_disabled)
+		dev_err(&dev->dev, "reg 0x%x: can't handle BAR larger than 4GB (size %#010llx)\n",
+			pos, (unsigned long long) sz64);
+	if (bar_too_high)
+		dev_info(&dev->dev, "reg 0x%x: can't handle BAR above 4G (bus address %#010llx)\n",
+			 pos, (unsigned long long) l64);
+	if (bar_invalid)
+		dev_info(&dev->dev, "reg 0x%x: initial BAR value %#010llx invalid\n",
+			 pos, (unsigned long long) region.start);
+	if (res->flags)
 		dev_printk(KERN_DEBUG, &dev->dev, "reg 0x%x: %pR\n", pos, res);
 
 	return (res->flags & IORESOURCE_MEM_64) ? 1 : 0;
@@ -465,7 +475,7 @@ void pci_read_bridge_bases(struct pci_bus *child)
 
 	if (dev->transparent) {
 		pci_bus_for_each_resource(child->parent, res, i) {
-			if (res) {
+			if (res && res->flags) {
 				pci_bus_add_resource(child, res,
 						     PCI_SUBTRACTIVE_DECODE);
 				dev_printk(KERN_DEBUG, &dev->dev,
@@ -719,7 +729,7 @@ add_dev:
 	return child;
 }
 
-struct pci_bus *__ref pci_add_new_bus(struct pci_bus *parent, struct pci_dev *dev, int busnr)
+struct pci_bus *pci_add_new_bus(struct pci_bus *parent, struct pci_dev *dev, int busnr)
 {
 	struct pci_bus *child;
 
@@ -984,6 +994,43 @@ void set_pcie_hotplug_bridge(struct pci_dev *pdev)
 
 
 /**
+ * pci_ext_cfg_is_aliased - is ext config space just an alias of std config?
+ * @dev: PCI device
+ *
+ * PCI Express to PCI/PCI-X Bridge Specification, rev 1.0, 4.1.4 says that
+ * when forwarding a type1 configuration request the bridge must check that
+ * the extended register address field is zero.  The bridge is not permitted
+ * to forward the transactions and must handle it as an Unsupported Request.
+ * Some bridges do not follow this rule and simply drop the extended register
+ * bits, resulting in the standard config space being aliased, every 256
+ * bytes across the entire configuration space.  Test for this condition by
+ * comparing the first dword of each potential alias to the vendor/device ID.
+ * Known offenders:
+ *   ASM1083/1085 PCIe-to-PCI Reversible Bridge (1b21:1080, rev 01 & 03)
+ *   AMD/ATI SBx00 PCI to PCI Bridge (1002:4384, rev 40)
+ */
+static bool pci_ext_cfg_is_aliased(struct pci_dev *dev)
+{
+#ifdef CONFIG_PCI_QUIRKS
+	int pos;
+	u32 header, tmp;
+
+	pci_read_config_dword(dev, PCI_VENDOR_ID, &header);
+
+	for (pos = PCI_CFG_SPACE_SIZE;
+	     pos < PCI_CFG_SPACE_EXP_SIZE; pos += PCI_CFG_SPACE_SIZE) {
+		if (pci_read_config_dword(dev, pos, &tmp) != PCIBIOS_SUCCESSFUL
+		    || header != tmp)
+			return false;
+	}
+
+	return true;
+#else
+	return false;
+#endif
+}
+
+/**
  * pci_cfg_space_size - get the configuration space size of the PCI device.
  * @dev: PCI device
  *
@@ -1001,7 +1048,7 @@ static int pci_cfg_space_size_ext(struct pci_dev *dev)
 
 	if (pci_read_config_dword(dev, pos, &status) != PCIBIOS_SUCCESSFUL)
 		goto fail;
-	if (status == 0xffffffff)
+	if (status == 0xffffffff || pci_ext_cfg_is_aliased(dev))
 		goto fail;
 
 	return PCI_CFG_SPACE_EXP_SIZE;
@@ -1215,6 +1262,7 @@ static void pci_release_dev(struct device *dev)
 	pci_release_of_node(pci_dev);
 	pcibios_release_device(pci_dev);
 	pci_bus_put(pci_dev->bus);
+	kfree(pci_dev->driver_override);
 	kfree(pci_dev);
 }
 
@@ -1369,7 +1417,7 @@ void pci_device_add(struct pci_dev *dev, struct pci_bus *bus)
 	WARN_ON(ret < 0);
 }
 
-struct pci_dev *__ref pci_scan_single_device(struct pci_bus *bus, int devfn)
+struct pci_dev *pci_scan_single_device(struct pci_bus *bus, int devfn)
 {
 	struct pci_dev *dev;
 
@@ -1617,7 +1665,7 @@ static int pcie_bus_configure_set(struct pci_dev *dev, void *data)
  */
 void pcie_bus_configure_settings(struct pci_bus *bus)
 {
-	u8 smpss;
+	u8 smpss = 0;
 
 	if (!bus->self)
 		return;
@@ -1670,8 +1718,7 @@ unsigned int pci_scan_child_bus(struct pci_bus *bus)
 
 	for (pass=0; pass < 2; pass++)
 		list_for_each_entry(dev, &bus->devices, bus_list) {
-			if (dev->hdr_type == PCI_HEADER_TYPE_BRIDGE ||
-			    dev->hdr_type == PCI_HEADER_TYPE_CARDBUS)
+			if (pci_is_bridge(dev))
 				max = pci_scan_bridge(bus, dev, max, pass);
 		}
 
@@ -1958,7 +2005,7 @@ EXPORT_SYMBOL(pci_scan_bus);
  *
  * Returns the max number of subordinate bus discovered.
  */
-unsigned int __ref pci_rescan_bus_bridge_resize(struct pci_dev *bridge)
+unsigned int pci_rescan_bus_bridge_resize(struct pci_dev *bridge)
 {
 	unsigned int max;
 	struct pci_bus *bus = bridge->subordinate;
@@ -1981,7 +2028,7 @@ unsigned int __ref pci_rescan_bus_bridge_resize(struct pci_dev *bridge)
  *
  * Returns the max number of subordinate bus discovered.
  */
-unsigned int __ref pci_rescan_bus(struct pci_bus *bus)
+unsigned int pci_rescan_bus(struct pci_bus *bus)
 {
 	unsigned int max;
 
diff --git a/drivers/pci/quirks.c b/drivers/pci/quirks.c
index e7292065a1b..92e68c7747f 100644
--- a/drivers/pci/quirks.c
+++ b/drivers/pci/quirks.c
@@ -2954,6 +2954,7 @@ static void disable_igfx_irq(struct pci_dev *dev)
 }
 DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_INTEL, 0x0102, disable_igfx_irq);
 DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_INTEL, 0x010a, disable_igfx_irq);
+DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_INTEL, 0x0152, disable_igfx_irq);
 
 /*
  * PCI devices which are on Intel chips can skip the 10ms delay
@@ -2991,6 +2992,14 @@ DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_CHELSIO, 0x0030,
 			 quirk_broken_intx_masking);
 DECLARE_PCI_FIXUP_HEADER(0x1814, 0x0601, /* Ralink RT2800 802.11n PCI */
 			 quirk_broken_intx_masking);
+/*
+ * Realtek RTL8169 PCI Gigabit Ethernet Controller (rev 10)
+ * Subsystem: Realtek RTL8169/8110 Family PCI Gigabit Ethernet NIC
+ *
+ * RTL8110SC - Fails under PCI device assignment using DisINTx masking.
+ */
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_REALTEK, 0x8169,
+			 quirk_broken_intx_masking);
 
 static void pci_do_fixups(struct pci_dev *dev, struct pci_fixup *f,
 			  struct pci_fixup *end)
@@ -3453,6 +3462,8 @@ static const u16 pci_quirk_intel_pch_acs_ids[] = {
 	/* Wildcat PCH */
 	0x9c90, 0x9c91, 0x9c92, 0x9c93, 0x9c94, 0x9c95, 0x9c96, 0x9c97,
 	0x9c98, 0x9c99, 0x9c9a, 0x9c9b,
+	/* Patsburg (X79) PCH */
+	0x1d10, 0x1d12, 0x1d14, 0x1d16, 0x1d18, 0x1d1a, 0x1d1c, 0x1d1e,
 };
 
 static bool pci_quirk_intel_pch_acs_match(struct pci_dev *dev)
diff --git a/drivers/pci/search.c b/drivers/pci/search.c
index 4a1b972efe7..8e495bda678 100644
--- a/drivers/pci/search.c
+++ b/drivers/pci/search.c
@@ -7,7 +7,6 @@
  *	Copyright (C) 2003 -- 2004 Greg Kroah-Hartman <greg@kroah.com>
  */
 
-#include <linux/init.h>
 #include <linux/pci.h>
 #include <linux/slab.h>
 #include <linux/module.h>
diff --git a/drivers/pci/setup-bus.c b/drivers/pci/setup-bus.c
index 138bdd6393b..fd9b545c3cf 100644
--- a/drivers/pci/setup-bus.c
+++ b/drivers/pci/setup-bus.c
@@ -713,12 +713,11 @@ static void pci_bridge_check_ranges(struct pci_bus *bus)
    bus resource of a given type. Note: we intentionally skip
    the bus resources which have already been assigned (that is,
    have non-NULL parent resource). */
-static struct resource *find_free_bus_resource(struct pci_bus *bus, unsigned long type)
+static struct resource *find_free_bus_resource(struct pci_bus *bus,
+			 unsigned long type_mask, unsigned long type)
 {
 	int i;
 	struct resource *r;
-	unsigned long type_mask = IORESOURCE_IO | IORESOURCE_MEM |
-				  IORESOURCE_PREFETCH;
 
 	pci_bus_for_each_resource(bus, r, i) {
 		if (r == &ioport_resource || r == &iomem_resource)
@@ -815,7 +814,8 @@ static void pbus_size_io(struct pci_bus *bus, resource_size_t min_size,
 		resource_size_t add_size, struct list_head *realloc_head)
 {
 	struct pci_dev *dev;
-	struct resource *b_res = find_free_bus_resource(bus, IORESOURCE_IO);
+	struct resource *b_res = find_free_bus_resource(bus, IORESOURCE_IO,
+							IORESOURCE_IO);
 	resource_size_t size = 0, size0 = 0, size1 = 0;
 	resource_size_t children_add_size = 0;
 	resource_size_t min_align, align;
@@ -907,36 +907,40 @@ static inline resource_size_t calculate_mem_align(resource_size_t *aligns,
  * @bus : the bus
  * @mask: mask the resource flag, then compare it with type
  * @type: the type of free resource from bridge
+ * @type2: second match type
+ * @type3: third match type
  * @min_size : the minimum memory window that must to be allocated
  * @add_size : additional optional memory window
  * @realloc_head : track the additional memory window on this list
  *
  * Calculate the size of the bus and minimal alignment which
  * guarantees that all child resources fit in this size.
+ *
+ * Returns -ENOSPC if there's no available bus resource of the desired type.
+ * Otherwise, sets the bus resource start/end to indicate the required
+ * size, adds things to realloc_head (if supplied), and returns 0.
  */
 static int pbus_size_mem(struct pci_bus *bus, unsigned long mask,
-			 unsigned long type, resource_size_t min_size,
-			resource_size_t add_size,
-			struct list_head *realloc_head)
+			 unsigned long type, unsigned long type2,
+			 unsigned long type3,
+			 resource_size_t min_size, resource_size_t add_size,
+			 struct list_head *realloc_head)
 {
 	struct pci_dev *dev;
 	resource_size_t min_align, align, size, size0, size1;
-	resource_size_t aligns[12];	/* Alignments from 1Mb to 2Gb */
+	resource_size_t aligns[14];	/* Alignments from 1Mb to 8Gb */
 	int order, max_order;
-	struct resource *b_res = find_free_bus_resource(bus, type);
-	unsigned int mem64_mask = 0;
+	struct resource *b_res = find_free_bus_resource(bus,
+					mask | IORESOURCE_PREFETCH, type);
 	resource_size_t children_add_size = 0;
 
 	if (!b_res)
-		return 0;
+		return -ENOSPC;
 
 	memset(aligns, 0, sizeof(aligns));
 	max_order = 0;
 	size = 0;
 
-	mem64_mask = b_res->flags & IORESOURCE_MEM_64;
-	b_res->flags &= ~IORESOURCE_MEM_64;
-
 	list_for_each_entry(dev, &bus->devices, bus_list) {
 		int i;
 
@@ -944,7 +948,9 @@ static int pbus_size_mem(struct pci_bus *bus, unsigned long mask,
 			struct resource *r = &dev->resource[i];
 			resource_size_t r_size;
 
-			if (r->parent || (r->flags & mask) != type)
+			if (r->parent || ((r->flags & mask) != type &&
+					  (r->flags & mask) != type2 &&
+					  (r->flags & mask) != type3))
 				continue;
 			r_size = resource_size(r);
 #ifdef CONFIG_PCI_IOV
@@ -957,10 +963,17 @@ static int pbus_size_mem(struct pci_bus *bus, unsigned long mask,
 				continue;
 			}
 #endif
-			/* For bridges size != alignment */
+			/*
+			 * aligns[0] is for 1MB (since bridge memory
+			 * windows are always at least 1MB aligned), so
+			 * keep "order" from being negative for smaller
+			 * resources.
+			 */
 			align = pci_resource_alignment(dev, r);
 			order = __ffs(align) - 20;
-			if (order > 11) {
+			if (order < 0)
+				order = 0;
+			if (order >= ARRAY_SIZE(aligns)) {
 				dev_warn(&dev->dev, "disabling BAR %d: %pR "
 					 "(bad alignment %#llx)\n", i, r,
 					 (unsigned long long) align);
@@ -968,15 +981,12 @@ static int pbus_size_mem(struct pci_bus *bus, unsigned long mask,
 				continue;
 			}
 			size += r_size;
-			if (order < 0)
-				order = 0;
 			/* Exclude ranges with size > align from
 			   calculation of the alignment. */
 			if (r_size == align)
 				aligns[order] += align;
 			if (order > max_order)
 				max_order = order;
-			mem64_mask &= r->flags & IORESOURCE_MEM_64;
 
 			if (realloc_head)
 				children_add_size += get_res_add_size(realloc_head, r);
@@ -997,18 +1007,18 @@ static int pbus_size_mem(struct pci_bus *bus, unsigned long mask,
 				 "%pR to %pR (unused)\n", b_res,
 				 &bus->busn_res);
 		b_res->flags = 0;
-		return 1;
+		return 0;
 	}
 	b_res->start = min_align;
 	b_res->end = size0 + min_align - 1;
-	b_res->flags |= IORESOURCE_STARTALIGN | mem64_mask;
+	b_res->flags |= IORESOURCE_STARTALIGN;
 	if (size1 > size0 && realloc_head) {
 		add_to_list(realloc_head, bus->self, b_res, size1-size0, min_align);
 		dev_printk(KERN_DEBUG, &bus->self->dev, "bridge window "
 				 "%pR to %pR add_size %llx\n", b_res,
 				 &bus->busn_res, (unsigned long long)size1-size0);
 	}
-	return 1;
+	return 0;
 }
 
 unsigned long pci_cardbus_resource_alignment(struct resource *res)
@@ -1113,12 +1123,13 @@ handle_done:
 	;
 }
 
-void __ref __pci_bus_size_bridges(struct pci_bus *bus,
-			struct list_head *realloc_head)
+void __pci_bus_size_bridges(struct pci_bus *bus, struct list_head *realloc_head)
 {
 	struct pci_dev *dev;
-	unsigned long mask, prefmask;
+	unsigned long mask, prefmask, type2 = 0, type3 = 0;
 	resource_size_t additional_mem_size = 0, additional_io_size = 0;
+	struct resource *b_res;
+	int ret;
 
 	list_for_each_entry(dev, &bus->devices, bus_list) {
 		struct pci_bus *b = dev->subordinate;
@@ -1152,41 +1163,93 @@ void __ref __pci_bus_size_bridges(struct pci_bus *bus,
 			additional_io_size  = pci_hotplug_io_size;
 			additional_mem_size = pci_hotplug_mem_size;
 		}
-		/*
-		 * Follow thru
-		 */
+		/* Fall through */
 	default:
 		pbus_size_io(bus, realloc_head ? 0 : additional_io_size,
 			     additional_io_size, realloc_head);
-		/* If the bridge supports prefetchable range, size it
-		   separately. If it doesn't, or its prefetchable window
-		   has already been allocated by arch code, try
-		   non-prefetchable range for both types of PCI memory
-		   resources. */
+
+		/*
+		 * If there's a 64-bit prefetchable MMIO window, compute
+		 * the size required to put all 64-bit prefetchable
+		 * resources in it.
+		 */
+		b_res = &bus->self->resource[PCI_BRIDGE_RESOURCES];
 		mask = IORESOURCE_MEM;
 		prefmask = IORESOURCE_MEM | IORESOURCE_PREFETCH;
-		if (pbus_size_mem(bus, prefmask, prefmask,
+		if (b_res[2].flags & IORESOURCE_MEM_64) {
+			prefmask |= IORESOURCE_MEM_64;
+			ret = pbus_size_mem(bus, prefmask, prefmask,
+				  prefmask, prefmask,
 				  realloc_head ? 0 : additional_mem_size,
-				  additional_mem_size, realloc_head))
-			mask = prefmask; /* Success, size non-prefetch only. */
-		else
-			additional_mem_size += additional_mem_size;
-		pbus_size_mem(bus, mask, IORESOURCE_MEM,
+				  additional_mem_size, realloc_head);
+
+			/*
+			 * If successful, all non-prefetchable resources
+			 * and any 32-bit prefetchable resources will go in
+			 * the non-prefetchable window.
+			 */
+			if (ret == 0) {
+				mask = prefmask;
+				type2 = prefmask & ~IORESOURCE_MEM_64;
+				type3 = prefmask & ~IORESOURCE_PREFETCH;
+			}
+		}
+
+		/*
+		 * If there is no 64-bit prefetchable window, compute the
+		 * size required to put all prefetchable resources in the
+		 * 32-bit prefetchable window (if there is one).
+		 */
+		if (!type2) {
+			prefmask &= ~IORESOURCE_MEM_64;
+			ret = pbus_size_mem(bus, prefmask, prefmask,
+					 prefmask, prefmask,
+					 realloc_head ? 0 : additional_mem_size,
+					 additional_mem_size, realloc_head);
+
+			/*
+			 * If successful, only non-prefetchable resources
+			 * will go in the non-prefetchable window.
+			 */
+			if (ret == 0)
+				mask = prefmask;
+			else
+				additional_mem_size += additional_mem_size;
+
+			type2 = type3 = IORESOURCE_MEM;
+		}
+
+		/*
+		 * Compute the size required to put everything else in the
+		 * non-prefetchable window.  This includes:
+		 *
+		 *   - all non-prefetchable resources
+		 *   - 32-bit prefetchable resources if there's a 64-bit
+		 *     prefetchable window or no prefetchable window at all
+		 *   - 64-bit prefetchable resources if there's no
+		 *     prefetchable window at all
+		 *
+		 * Note that the strategy in __pci_assign_resource() must
+		 * match that used here.  Specifically, we cannot put a
+		 * 32-bit prefetchable resource in a 64-bit prefetchable
+		 * window.
+		 */
+		pbus_size_mem(bus, mask, IORESOURCE_MEM, type2, type3,
 				realloc_head ? 0 : additional_mem_size,
 				additional_mem_size, realloc_head);
 		break;
 	}
 }
 
-void __ref pci_bus_size_bridges(struct pci_bus *bus)
+void pci_bus_size_bridges(struct pci_bus *bus)
 {
 	__pci_bus_size_bridges(bus, NULL);
 }
 EXPORT_SYMBOL(pci_bus_size_bridges);
 
-void __ref __pci_bus_assign_resources(const struct pci_bus *bus,
-				      struct list_head *realloc_head,
-				      struct list_head *fail_head)
+void __pci_bus_assign_resources(const struct pci_bus *bus,
+				struct list_head *realloc_head,
+				struct list_head *fail_head)
 {
 	struct pci_bus *b;
 	struct pci_dev *dev;
@@ -1218,15 +1281,15 @@ void __ref __pci_bus_assign_resources(const struct pci_bus *bus,
 	}
 }
 
-void __ref pci_bus_assign_resources(const struct pci_bus *bus)
+void pci_bus_assign_resources(const struct pci_bus *bus)
 {
 	__pci_bus_assign_resources(bus, NULL, NULL);
 }
 EXPORT_SYMBOL(pci_bus_assign_resources);
 
-static void __ref __pci_bridge_assign_resources(const struct pci_dev *bridge,
-					 struct list_head *add_head,
-					 struct list_head *fail_head)
+static void __pci_bridge_assign_resources(const struct pci_dev *bridge,
+					  struct list_head *add_head,
+					  struct list_head *fail_head)
 {
 	struct pci_bus *b;
 
@@ -1257,42 +1320,66 @@ static void __ref __pci_bridge_assign_resources(const struct pci_dev *bridge,
 static void pci_bridge_release_resources(struct pci_bus *bus,
 					  unsigned long type)
 {
-	int idx;
-	bool changed = false;
-	struct pci_dev *dev;
+	struct pci_dev *dev = bus->self;
 	struct resource *r;
 	unsigned long type_mask = IORESOURCE_IO | IORESOURCE_MEM |
-				  IORESOURCE_PREFETCH;
+				  IORESOURCE_PREFETCH | IORESOURCE_MEM_64;
+	unsigned old_flags = 0;
+	struct resource *b_res;
+	int idx = 1;
 
-	dev = bus->self;
-	for (idx = PCI_BRIDGE_RESOURCES; idx <= PCI_BRIDGE_RESOURCE_END;
-	     idx++) {
-		r = &dev->resource[idx];
-		if ((r->flags & type_mask) != type)
-			continue;
-		if (!r->parent)
-			continue;
-		/*
-		 * if there are children under that, we should release them
-		 *  all
-		 */
-		release_child_resources(r);
-		if (!release_resource(r)) {
-			dev_printk(KERN_DEBUG, &dev->dev,
-				 "resource %d %pR released\n", idx, r);
-			/* keep the old size */
-			r->end = resource_size(r) - 1;
-			r->start = 0;
-			r->flags = 0;
-			changed = true;
-		}
-	}
+	b_res = &dev->resource[PCI_BRIDGE_RESOURCES];
+
+	/*
+	 *     1. if there is io port assign fail, will release bridge
+	 *	  io port.
+	 *     2. if there is non pref mmio assign fail, release bridge
+	 *	  nonpref mmio.
+	 *     3. if there is 64bit pref mmio assign fail, and bridge pref
+	 *	  is 64bit, release bridge pref mmio.
+	 *     4. if there is pref mmio assign fail, and bridge pref is
+	 *	  32bit mmio, release bridge pref mmio
+	 *     5. if there is pref mmio assign fail, and bridge pref is not
+	 *	  assigned, release bridge nonpref mmio.
+	 */
+	if (type & IORESOURCE_IO)
+		idx = 0;
+	else if (!(type & IORESOURCE_PREFETCH))
+		idx = 1;
+	else if ((type & IORESOURCE_MEM_64) &&
+		 (b_res[2].flags & IORESOURCE_MEM_64))
+		idx = 2;
+	else if (!(b_res[2].flags & IORESOURCE_MEM_64) &&
+		 (b_res[2].flags & IORESOURCE_PREFETCH))
+		idx = 2;
+	else
+		idx = 1;
+
+	r = &b_res[idx];
+
+	if (!r->parent)
+		return;
+
+	/*
+	 * if there are children under that, we should release them
+	 *  all
+	 */
+	release_child_resources(r);
+	if (!release_resource(r)) {
+		type = old_flags = r->flags & type_mask;
+		dev_printk(KERN_DEBUG, &dev->dev, "resource %d %pR released\n",
+					PCI_BRIDGE_RESOURCES + idx, r);
+		/* keep the old size */
+		r->end = resource_size(r) - 1;
+		r->start = 0;
+		r->flags = 0;
 
-	if (changed) {
 		/* avoiding touch the one without PREF */
 		if (type & IORESOURCE_PREFETCH)
 			type = IORESOURCE_PREFETCH;
 		__pci_setup_bridge(bus, type);
+		/* for next child res under same bridge */
+		r->flags = old_flags;
 	}
 }
 
@@ -1304,9 +1391,9 @@ enum release_type {
  * try to release pci bridge resources that is from leaf bridge,
  * so we can allocate big new one later
  */
-static void __ref pci_bus_release_bridge_resources(struct pci_bus *bus,
-						   unsigned long type,
-						   enum release_type rel_type)
+static void pci_bus_release_bridge_resources(struct pci_bus *bus,
+					     unsigned long type,
+					     enum release_type rel_type)
 {
 	struct pci_dev *dev;
 	bool is_leaf_bridge = true;
@@ -1471,7 +1558,7 @@ void pci_assign_unassigned_root_bus_resources(struct pci_bus *bus)
 	LIST_HEAD(fail_head);
 	struct pci_dev_resource *fail_res;
 	unsigned long type_mask = IORESOURCE_IO | IORESOURCE_MEM |
-				  IORESOURCE_PREFETCH;
+				  IORESOURCE_PREFETCH | IORESOURCE_MEM_64;
 	int pci_try_num = 1;
 	enum enable_type enable_local;
 
@@ -1629,9 +1716,7 @@ void pci_assign_unassigned_bus_resources(struct pci_bus *bus)
 
 	down_read(&pci_bus_sem);
 	list_for_each_entry(dev, &bus->devices, bus_list)
-		if (dev->hdr_type == PCI_HEADER_TYPE_BRIDGE ||
-		    dev->hdr_type == PCI_HEADER_TYPE_CARDBUS)
-			if (dev->subordinate)
+		if (pci_is_bridge(dev) && pci_has_subordinate(dev))
 				__pci_bus_size_bridges(dev->subordinate,
 							 &add_list);
 	up_read(&pci_bus_sem);
diff --git a/drivers/pci/setup-irq.c b/drivers/pci/setup-irq.c
index 9bd6864ec5d..dbc4ffcf42d 100644
--- a/drivers/pci/setup-irq.c
+++ b/drivers/pci/setup-irq.c
@@ -10,7 +10,6 @@
  */
 
 
-#include <linux/init.h>
 #include <linux/kernel.h>
 #include <linux/pci.h>
 #include <linux/errno.h>
diff --git a/drivers/pci/setup-res.c b/drivers/pci/setup-res.c
index 7eed671d558..33f9e32d94d 100644
--- a/drivers/pci/setup-res.c
+++ b/drivers/pci/setup-res.c
@@ -16,7 +16,6 @@
  *	     Resource sorting
  */
 
-#include <linux/init.h>
 #include <linux/kernel.h>
 #include <linux/export.h>
 #include <linux/pci.h>
@@ -209,21 +208,42 @@ static int __pci_assign_resource(struct pci_bus *bus, struct pci_dev *dev,
 
 	min = (res->flags & IORESOURCE_IO) ? PCIBIOS_MIN_IO : PCIBIOS_MIN_MEM;
 
-	/* First, try exact prefetching match.. */
+	/*
+	 * First, try exact prefetching match.  Even if a 64-bit
+	 * prefetchable bridge window is below 4GB, we can't put a 32-bit
+	 * prefetchable resource in it because pbus_size_mem() assumes a
+	 * 64-bit window will contain no 32-bit resources.  If we assign
+	 * things differently than they were sized, not everything will fit.
+	 */
 	ret = pci_bus_alloc_resource(bus, res, size, align, min,
-				     IORESOURCE_PREFETCH,
+				     IORESOURCE_PREFETCH | IORESOURCE_MEM_64,
 				     pcibios_align_resource, dev);
+	if (ret == 0)
+		return 0;
 
-	if (ret < 0 && (res->flags & IORESOURCE_PREFETCH)) {
-		/*
-		 * That failed.
-		 *
-		 * But a prefetching area can handle a non-prefetching
-		 * window (it will just not perform as well).
-		 */
-		ret = pci_bus_alloc_resource(bus, res, size, align, min, 0,
+	/*
+	 * If the prefetchable window is only 32 bits wide, we can put
+	 * 64-bit prefetchable resources in it.
+	 */
+	if ((res->flags & (IORESOURCE_PREFETCH | IORESOURCE_MEM_64)) ==
+	     (IORESOURCE_PREFETCH | IORESOURCE_MEM_64)) {
+		ret = pci_bus_alloc_resource(bus, res, size, align, min,
+					     IORESOURCE_PREFETCH,
 					     pcibios_align_resource, dev);
+		if (ret == 0)
+			return 0;
 	}
+
+	/*
+	 * If we didn't find a better match, we can put any memory resource
+	 * in a non-prefetchable window.  If this resource is 32 bits and
+	 * non-prefetchable, the first call already tried the only possibility
+	 * so we don't need to try again.
+	 */
+	if (res->flags & (IORESOURCE_PREFETCH | IORESOURCE_MEM_64))
+		ret = pci_bus_alloc_resource(bus, res, size, align, min, 0,
+					     pcibios_align_resource, dev);
+
 	return ret;
 }
 
diff --git a/drivers/pcmcia/cardbus.c b/drivers/pcmcia/cardbus.c
index 8bde61952d2..4fe4cc4ae19 100644
--- a/drivers/pcmcia/cardbus.c
+++ b/drivers/pcmcia/cardbus.c
@@ -78,8 +78,7 @@ int __ref cb_alloc(struct pcmcia_socket *s)
 	max = bus->busn_res.start;
 	for (pass = 0; pass < 2; pass++)
 		list_for_each_entry(dev, &bus->devices, bus_list)
-			if (dev->hdr_type == PCI_HEADER_TYPE_BRIDGE ||
-			    dev->hdr_type == PCI_HEADER_TYPE_CARDBUS)
+			if (pci_is_bridge(dev))
 				max = pci_scan_bridge(bus, dev, max, pass);
 
 	/*
diff --git a/drivers/phy/Kconfig b/drivers/phy/Kconfig
index 3bb05f17b9b..16a2f067c24 100644
--- a/drivers/phy/Kconfig
+++ b/drivers/phy/Kconfig
@@ -33,6 +33,7 @@ config PHY_MVEBU_SATA
 
 config OMAP_CONTROL_PHY
 	tristate "OMAP CONTROL PHY Driver"
+	depends on ARCH_OMAP2PLUS || COMPILE_TEST
 	help
 	  Enable this to add support for the PHY part present in the control
 	  module. This driver has API to power on the USB2 PHY and to write to
@@ -88,8 +89,8 @@ config PHY_EXYNOS_DP_VIDEO
 
 config BCM_KONA_USB2_PHY
 	tristate "Broadcom Kona USB2 PHY Driver"
-	depends on GENERIC_PHY
 	depends on HAS_IOMEM
+	select GENERIC_PHY
 	help
 	  Enable this to support the Broadcom Kona USB 2.0 PHY.
 
@@ -159,6 +160,17 @@ config PHY_EXYNOS5250_USB2
 	  particular SoC is compiled in the driver. In case of Exynos 5250 four
 	  phys are available - device, host, HSIC0 and HSIC.
 
+config PHY_EXYNOS5_USBDRD
+	tristate "Exynos5 SoC series USB DRD PHY driver"
+	depends on ARCH_EXYNOS5 && OF
+	depends on HAS_IOMEM
+	select GENERIC_PHY
+	select MFD_SYSCON
+	help
+	  Enable USB DRD PHY support for Exynos 5 SoC series.
+	  This driver provides PHY interface for USB 3.0 DRD controller
+	  present on Exynos5 SoC series.
+
 config PHY_XGENE
 	tristate "APM X-Gene 15Gbps PHY support"
 	depends on HAS_IOMEM && OF && (ARM64 || COMPILE_TEST)
diff --git a/drivers/phy/Makefile b/drivers/phy/Makefile
index 2faf78edc86..b4f1d577060 100644
--- a/drivers/phy/Makefile
+++ b/drivers/phy/Makefile
@@ -13,8 +13,10 @@ obj-$(CONFIG_TI_PIPE3)			+= phy-ti-pipe3.o
 obj-$(CONFIG_TWL4030_USB)		+= phy-twl4030-usb.o
 obj-$(CONFIG_PHY_EXYNOS5250_SATA)	+= phy-exynos5250-sata.o
 obj-$(CONFIG_PHY_SUN4I_USB)		+= phy-sun4i-usb.o
-obj-$(CONFIG_PHY_SAMSUNG_USB2)		+= phy-samsung-usb2.o
-obj-$(CONFIG_PHY_EXYNOS4210_USB2)	+= phy-exynos4210-usb2.o
-obj-$(CONFIG_PHY_EXYNOS4X12_USB2)	+= phy-exynos4x12-usb2.o
-obj-$(CONFIG_PHY_EXYNOS5250_USB2)	+= phy-exynos5250-usb2.o
+obj-$(CONFIG_PHY_SAMSUNG_USB2)		+= phy-exynos-usb2.o
+phy-exynos-usb2-y			+= phy-samsung-usb2.o
+phy-exynos-usb2-$(CONFIG_PHY_EXYNOS4210_USB2)	+= phy-exynos4210-usb2.o
+phy-exynos-usb2-$(CONFIG_PHY_EXYNOS4X12_USB2)	+= phy-exynos4x12-usb2.o
+phy-exynos-usb2-$(CONFIG_PHY_EXYNOS5250_USB2)	+= phy-exynos5250-usb2.o
+obj-$(CONFIG_PHY_EXYNOS5_USBDRD)	+= phy-exynos5-usbdrd.o
 obj-$(CONFIG_PHY_XGENE)			+= phy-xgene.o
diff --git a/drivers/phy/phy-core.c b/drivers/phy/phy-core.c
index 623b71c54b3..c64a2f3b2d6 100644
--- a/drivers/phy/phy-core.c
+++ b/drivers/phy/phy-core.c
@@ -64,6 +64,9 @@ static struct phy *phy_lookup(struct device *device, const char *port)
 	class_dev_iter_init(&iter, phy_class, NULL, NULL);
 	while ((dev = class_dev_iter_next(&iter))) {
 		phy = to_phy(dev);
+
+		if (!phy->init_data)
+			continue;
 		count = phy->init_data->num_consumers;
 		consumers = phy->init_data->consumers;
 		while (count--) {
diff --git a/drivers/phy/phy-exynos-mipi-video.c b/drivers/phy/phy-exynos-mipi-video.c
index 7f139326a64..ff026689358 100644
--- a/drivers/phy/phy-exynos-mipi-video.c
+++ b/drivers/phy/phy-exynos-mipi-video.c
@@ -101,7 +101,7 @@ static struct phy *exynos_mipi_video_phy_xlate(struct device *dev,
 {
 	struct exynos_mipi_video_phy *state = dev_get_drvdata(dev);
 
-	if (WARN_ON(args->args[0] > EXYNOS_MIPI_PHYS_NUM))
+	if (WARN_ON(args->args[0] >= EXYNOS_MIPI_PHYS_NUM))
 		return ERR_PTR(-ENODEV);
 
 	return state->phys[args->args[0]].phy;
diff --git a/drivers/phy/phy-exynos5-usbdrd.c b/drivers/phy/phy-exynos5-usbdrd.c
new file mode 100644
index 00000000000..76d862b2202
--- /dev/null
+++ b/drivers/phy/phy-exynos5-usbdrd.c
@@ -0,0 +1,676 @@
+/*
+ * Samsung EXYNOS5 SoC series USB DRD PHY driver
+ *
+ * Phy provider for USB 3.0 DRD controller on Exynos5 SoC series
+ *
+ * Copyright (C) 2014 Samsung Electronics Co., Ltd.
+ * Author: Vivek Gautam <gautam.vivek@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/delay.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/phy/phy.h>
+#include <linux/platform_device.h>
+#include <linux/mutex.h>
+#include <linux/mfd/syscon.h>
+#include <linux/mfd/syscon/exynos5-pmu.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
+
+/* Exynos USB PHY registers */
+#define EXYNOS5_FSEL_9MHZ6		0x0
+#define EXYNOS5_FSEL_10MHZ		0x1
+#define EXYNOS5_FSEL_12MHZ		0x2
+#define EXYNOS5_FSEL_19MHZ2		0x3
+#define EXYNOS5_FSEL_20MHZ		0x4
+#define EXYNOS5_FSEL_24MHZ		0x5
+#define EXYNOS5_FSEL_50MHZ		0x7
+
+/* EXYNOS5: USB 3.0 DRD PHY registers */
+#define EXYNOS5_DRD_LINKSYSTEM			0x04
+
+#define LINKSYSTEM_FLADJ_MASK			(0x3f << 1)
+#define LINKSYSTEM_FLADJ(_x)			((_x) << 1)
+#define LINKSYSTEM_XHCI_VERSION_CONTROL		BIT(27)
+
+#define EXYNOS5_DRD_PHYUTMI			0x08
+
+#define PHYUTMI_OTGDISABLE			BIT(6)
+#define PHYUTMI_FORCESUSPEND			BIT(1)
+#define PHYUTMI_FORCESLEEP			BIT(0)
+
+#define EXYNOS5_DRD_PHYPIPE			0x0c
+
+#define EXYNOS5_DRD_PHYCLKRST			0x10
+
+#define PHYCLKRST_EN_UTMISUSPEND		BIT(31)
+
+#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			BIT(20)
+#define PHYCLKRST_REF_SSP_EN			BIT(19)
+#define PHYCLKRST_REF_CLKDIV2			BIT(18)
+
+#define PHYCLKRST_MPLL_MULTIPLIER_MASK		(0x7f << 11)
+#define PHYCLKRST_MPLL_MULTIPLIER_100MHZ_REF	(0x19 << 11)
+#define PHYCLKRST_MPLL_MULTIPLIER_50M_REF	(0x32 << 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_UTMI_MASK		(0x7 << 5)
+#define PHYCLKRST_FSEL_PIPE_MASK		(0x7 << 8)
+#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			BIT(4)
+
+#define PHYCLKRST_REFCLKSEL_MASK		(0x03 << 2)
+#define PHYCLKRST_REFCLKSEL_PAD_REFCLK		(0x2 << 2)
+#define PHYCLKRST_REFCLKSEL_EXT_REFCLK		(0x3 << 2)
+
+#define PHYCLKRST_PORTRESET			BIT(1)
+#define PHYCLKRST_COMMONONN			BIT(0)
+
+#define EXYNOS5_DRD_PHYREG0			0x14
+#define EXYNOS5_DRD_PHYREG1			0x18
+
+#define EXYNOS5_DRD_PHYPARAM0			0x1c
+
+#define PHYPARAM0_REF_USE_PAD			BIT(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			BIT(3)
+#define PHYTEST_POWERDOWN_HSP			BIT(2)
+
+#define EXYNOS5_DRD_PHYADP			0x2c
+
+#define EXYNOS5_DRD_PHYUTMICLKSEL		0x30
+
+#define PHYUTMICLKSEL_UTMI_CLKSEL		BIT(2)
+
+#define EXYNOS5_DRD_PHYRESUME			0x34
+#define EXYNOS5_DRD_LINKPORT			0x44
+
+#define KHZ	1000
+#define MHZ	(KHZ * KHZ)
+
+enum exynos5_usbdrd_phy_id {
+	EXYNOS5_DRDPHY_UTMI,
+	EXYNOS5_DRDPHY_PIPE3,
+	EXYNOS5_DRDPHYS_NUM,
+};
+
+struct phy_usb_instance;
+struct exynos5_usbdrd_phy;
+
+struct exynos5_usbdrd_phy_config {
+	u32 id;
+	void (*phy_isol)(struct phy_usb_instance *inst, u32 on);
+	void (*phy_init)(struct exynos5_usbdrd_phy *phy_drd);
+	unsigned int (*set_refclk)(struct phy_usb_instance *inst);
+};
+
+struct exynos5_usbdrd_phy_drvdata {
+	const struct exynos5_usbdrd_phy_config *phy_cfg;
+	u32 pmu_offset_usbdrd0_phy;
+	u32 pmu_offset_usbdrd1_phy;
+};
+
+/**
+ * struct exynos5_usbdrd_phy - driver data for USB 3.0 PHY
+ * @dev: pointer to device instance of this platform device
+ * @reg_phy: usb phy controller register memory base
+ * @clk: phy clock for register access
+ * @drv_data: pointer to SoC level driver data structure
+ * @phys[]: array for 'EXYNOS5_DRDPHYS_NUM' number of PHY
+ *	    instances each with its 'phy' and 'phy_cfg'.
+ * @extrefclk: frequency select settings when using 'separate
+ *	       reference clocks' for SS and HS operations
+ * @ref_clk: reference clock to PHY block from which PHY's
+ *	     operational clocks are derived
+ * @ref_rate: rate of above reference clock
+ */
+struct exynos5_usbdrd_phy {
+	struct device *dev;
+	void __iomem *reg_phy;
+	struct clk *clk;
+	const struct exynos5_usbdrd_phy_drvdata *drv_data;
+	struct phy_usb_instance {
+		struct phy *phy;
+		u32 index;
+		struct regmap *reg_pmu;
+		u32 pmu_offset;
+		const struct exynos5_usbdrd_phy_config *phy_cfg;
+	} phys[EXYNOS5_DRDPHYS_NUM];
+	u32 extrefclk;
+	struct clk *ref_clk;
+	struct regulator *vbus;
+};
+
+static inline
+struct exynos5_usbdrd_phy *to_usbdrd_phy(struct phy_usb_instance *inst)
+{
+	return container_of((inst), struct exynos5_usbdrd_phy,
+			    phys[(inst)->index]);
+}
+
+/*
+ * exynos5_rate_to_clk() converts the supplied clock rate to the value that
+ * can be written to the phy register.
+ */
+static unsigned int exynos5_rate_to_clk(unsigned long rate, u32 *reg)
+{
+	/* EXYNOS5_FSEL_MASK */
+
+	switch (rate) {
+	case 9600 * KHZ:
+		*reg = EXYNOS5_FSEL_9MHZ6;
+		break;
+	case 10 * MHZ:
+		*reg = EXYNOS5_FSEL_10MHZ;
+		break;
+	case 12 * MHZ:
+		*reg = EXYNOS5_FSEL_12MHZ;
+		break;
+	case 19200 * KHZ:
+		*reg = EXYNOS5_FSEL_19MHZ2;
+		break;
+	case 20 * MHZ:
+		*reg = EXYNOS5_FSEL_20MHZ;
+		break;
+	case 24 * MHZ:
+		*reg = EXYNOS5_FSEL_24MHZ;
+		break;
+	case 50 * MHZ:
+		*reg = EXYNOS5_FSEL_50MHZ;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void exynos5_usbdrd_phy_isol(struct phy_usb_instance *inst,
+						unsigned int on)
+{
+	unsigned int val;
+
+	if (!inst->reg_pmu)
+		return;
+
+	val = on ? 0 : EXYNOS5_PHY_ENABLE;
+
+	regmap_update_bits(inst->reg_pmu, inst->pmu_offset,
+			   EXYNOS5_PHY_ENABLE, val);
+}
+
+/*
+ * Sets the pipe3 phy's clk as EXTREFCLK (XXTI) which is internal clock
+ * from clock core. Further sets multiplier values and spread spectrum
+ * clock settings for SuperSpeed operations.
+ */
+static unsigned int
+exynos5_usbdrd_pipe3_set_refclk(struct phy_usb_instance *inst)
+{
+	static u32 reg;
+	struct exynos5_usbdrd_phy *phy_drd = to_usbdrd_phy(inst);
+
+	/* restore any previous reference clock settings */
+	reg = readl(phy_drd->reg_phy + EXYNOS5_DRD_PHYCLKRST);
+
+	/* Use EXTREFCLK as ref clock */
+	reg &= ~PHYCLKRST_REFCLKSEL_MASK;
+	reg |=	PHYCLKRST_REFCLKSEL_EXT_REFCLK;
+
+	/* FSEL settings corresponding to reference clock */
+	reg &= ~PHYCLKRST_FSEL_PIPE_MASK |
+		PHYCLKRST_MPLL_MULTIPLIER_MASK |
+		PHYCLKRST_SSC_REFCLKSEL_MASK;
+	switch (phy_drd->extrefclk) {
+	case EXYNOS5_FSEL_50MHZ:
+		reg |= (PHYCLKRST_MPLL_MULTIPLIER_50M_REF |
+			PHYCLKRST_SSC_REFCLKSEL(0x00));
+		break;
+	case EXYNOS5_FSEL_24MHZ:
+		reg |= (PHYCLKRST_MPLL_MULTIPLIER_24MHZ_REF |
+			PHYCLKRST_SSC_REFCLKSEL(0x88));
+		break;
+	case EXYNOS5_FSEL_20MHZ:
+		reg |= (PHYCLKRST_MPLL_MULTIPLIER_20MHZ_REF |
+			PHYCLKRST_SSC_REFCLKSEL(0x00));
+		break;
+	case EXYNOS5_FSEL_19MHZ2:
+		reg |= (PHYCLKRST_MPLL_MULTIPLIER_19200KHZ_REF |
+			PHYCLKRST_SSC_REFCLKSEL(0x88));
+		break;
+	default:
+		dev_dbg(phy_drd->dev, "unsupported ref clk\n");
+		break;
+	}
+
+	return reg;
+}
+
+/*
+ * Sets the utmi phy's clk as EXTREFCLK (XXTI) which is internal clock
+ * from clock core. Further sets the FSEL values for HighSpeed operations.
+ */
+static unsigned int
+exynos5_usbdrd_utmi_set_refclk(struct phy_usb_instance *inst)
+{
+	static u32 reg;
+	struct exynos5_usbdrd_phy *phy_drd = to_usbdrd_phy(inst);
+
+	/* restore any previous reference clock settings */
+	reg = readl(phy_drd->reg_phy + EXYNOS5_DRD_PHYCLKRST);
+
+	reg &= ~PHYCLKRST_REFCLKSEL_MASK;
+	reg |=	PHYCLKRST_REFCLKSEL_EXT_REFCLK;
+
+	reg &= ~PHYCLKRST_FSEL_UTMI_MASK |
+		PHYCLKRST_MPLL_MULTIPLIER_MASK |
+		PHYCLKRST_SSC_REFCLKSEL_MASK;
+	reg |= PHYCLKRST_FSEL(phy_drd->extrefclk);
+
+	return reg;
+}
+
+static void exynos5_usbdrd_pipe3_init(struct exynos5_usbdrd_phy *phy_drd)
+{
+	u32 reg;
+
+	reg = readl(phy_drd->reg_phy + EXYNOS5_DRD_PHYPARAM1);
+	/* Set Tx De-Emphasis level */
+	reg &= ~PHYPARAM1_PCS_TXDEEMPH_MASK;
+	reg |=	PHYPARAM1_PCS_TXDEEMPH;
+	writel(reg, phy_drd->reg_phy + EXYNOS5_DRD_PHYPARAM1);
+
+	reg = readl(phy_drd->reg_phy + EXYNOS5_DRD_PHYTEST);
+	reg &= ~PHYTEST_POWERDOWN_SSP;
+	writel(reg, phy_drd->reg_phy + EXYNOS5_DRD_PHYTEST);
+}
+
+static void exynos5_usbdrd_utmi_init(struct exynos5_usbdrd_phy *phy_drd)
+{
+	u32 reg;
+
+	reg = readl(phy_drd->reg_phy + EXYNOS5_DRD_PHYPARAM0);
+	/* Set Loss-of-Signal Detector sensitivity */
+	reg &= ~PHYPARAM0_REF_LOSLEVEL_MASK;
+	reg |=	PHYPARAM0_REF_LOSLEVEL;
+	writel(reg, phy_drd->reg_phy + EXYNOS5_DRD_PHYPARAM0);
+
+	reg = readl(phy_drd->reg_phy + EXYNOS5_DRD_PHYPARAM1);
+	/* Set Tx De-Emphasis level */
+	reg &= ~PHYPARAM1_PCS_TXDEEMPH_MASK;
+	reg |=	PHYPARAM1_PCS_TXDEEMPH;
+	writel(reg, phy_drd->reg_phy + EXYNOS5_DRD_PHYPARAM1);
+
+	/* UTMI Power Control */
+	writel(PHYUTMI_OTGDISABLE, phy_drd->reg_phy + EXYNOS5_DRD_PHYUTMI);
+
+	reg = readl(phy_drd->reg_phy + EXYNOS5_DRD_PHYTEST);
+	reg &= ~PHYTEST_POWERDOWN_HSP;
+	writel(reg, phy_drd->reg_phy + EXYNOS5_DRD_PHYTEST);
+}
+
+static int exynos5_usbdrd_phy_init(struct phy *phy)
+{
+	int ret;
+	u32 reg;
+	struct phy_usb_instance *inst = phy_get_drvdata(phy);
+	struct exynos5_usbdrd_phy *phy_drd = to_usbdrd_phy(inst);
+
+	ret = clk_prepare_enable(phy_drd->clk);
+	if (ret)
+		return ret;
+
+	/* Reset USB 3.0 PHY */
+	writel(0x0, phy_drd->reg_phy + EXYNOS5_DRD_PHYREG0);
+	writel(0x0, phy_drd->reg_phy + EXYNOS5_DRD_PHYRESUME);
+
+	/*
+	 * Setting the Frame length Adj value[6:1] to default 0x20
+	 * See xHCI 1.0 spec, 5.2.4
+	 */
+	reg =	LINKSYSTEM_XHCI_VERSION_CONTROL |
+		LINKSYSTEM_FLADJ(0x20);
+	writel(reg, phy_drd->reg_phy + EXYNOS5_DRD_LINKSYSTEM);
+
+	reg = readl(phy_drd->reg_phy + EXYNOS5_DRD_PHYPARAM0);
+	/* Select PHY CLK source */
+	reg &= ~PHYPARAM0_REF_USE_PAD;
+	writel(reg, phy_drd->reg_phy + EXYNOS5_DRD_PHYPARAM0);
+
+	/* This bit must be set for both HS and SS operations */
+	reg = readl(phy_drd->reg_phy + EXYNOS5_DRD_PHYUTMICLKSEL);
+	reg |= PHYUTMICLKSEL_UTMI_CLKSEL;
+	writel(reg, phy_drd->reg_phy + EXYNOS5_DRD_PHYUTMICLKSEL);
+
+	/* UTMI or PIPE3 specific init */
+	inst->phy_cfg->phy_init(phy_drd);
+
+	/* reference clock settings */
+	reg = inst->phy_cfg->set_refclk(inst);
+
+		/* Digital power supply in normal operating mode */
+	reg |=	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 |
+		/* Reset the port */
+		PHYCLKRST_PORTRESET;
+
+	writel(reg, phy_drd->reg_phy + EXYNOS5_DRD_PHYCLKRST);
+
+	udelay(10);
+
+	reg &= ~PHYCLKRST_PORTRESET;
+	writel(reg, phy_drd->reg_phy + EXYNOS5_DRD_PHYCLKRST);
+
+	clk_disable_unprepare(phy_drd->clk);
+
+	return 0;
+}
+
+static int exynos5_usbdrd_phy_exit(struct phy *phy)
+{
+	int ret;
+	u32 reg;
+	struct phy_usb_instance *inst = phy_get_drvdata(phy);
+	struct exynos5_usbdrd_phy *phy_drd = to_usbdrd_phy(inst);
+
+	ret = clk_prepare_enable(phy_drd->clk);
+	if (ret)
+		return ret;
+
+	reg =	PHYUTMI_OTGDISABLE |
+		PHYUTMI_FORCESUSPEND |
+		PHYUTMI_FORCESLEEP;
+	writel(reg, phy_drd->reg_phy + EXYNOS5_DRD_PHYUTMI);
+
+	/* Resetting the PHYCLKRST enable bits to reduce leakage current */
+	reg = readl(phy_drd->reg_phy + EXYNOS5_DRD_PHYCLKRST);
+	reg &= ~(PHYCLKRST_REF_SSP_EN |
+		 PHYCLKRST_SSC_EN |
+		 PHYCLKRST_COMMONONN);
+	writel(reg, phy_drd->reg_phy + EXYNOS5_DRD_PHYCLKRST);
+
+	/* Control PHYTEST to remove leakage current */
+	reg = readl(phy_drd->reg_phy + EXYNOS5_DRD_PHYTEST);
+	reg |=	PHYTEST_POWERDOWN_SSP |
+		PHYTEST_POWERDOWN_HSP;
+	writel(reg, phy_drd->reg_phy + EXYNOS5_DRD_PHYTEST);
+
+	clk_disable_unprepare(phy_drd->clk);
+
+	return 0;
+}
+
+static int exynos5_usbdrd_phy_power_on(struct phy *phy)
+{
+	int ret;
+	struct phy_usb_instance *inst = phy_get_drvdata(phy);
+	struct exynos5_usbdrd_phy *phy_drd = to_usbdrd_phy(inst);
+
+	dev_dbg(phy_drd->dev, "Request to power_on usbdrd_phy phy\n");
+
+	clk_prepare_enable(phy_drd->ref_clk);
+
+	/* Enable VBUS supply */
+	if (phy_drd->vbus) {
+		ret = regulator_enable(phy_drd->vbus);
+		if (ret) {
+			dev_err(phy_drd->dev, "Failed to enable VBUS supply\n");
+			goto fail_vbus;
+		}
+	}
+
+	/* Power-on PHY*/
+	inst->phy_cfg->phy_isol(inst, 0);
+
+	return 0;
+
+fail_vbus:
+	clk_disable_unprepare(phy_drd->ref_clk);
+
+	return ret;
+}
+
+static int exynos5_usbdrd_phy_power_off(struct phy *phy)
+{
+	struct phy_usb_instance *inst = phy_get_drvdata(phy);
+	struct exynos5_usbdrd_phy *phy_drd = to_usbdrd_phy(inst);
+
+	dev_dbg(phy_drd->dev, "Request to power_off usbdrd_phy phy\n");
+
+	/* Power-off the PHY */
+	inst->phy_cfg->phy_isol(inst, 1);
+
+	/* Disable VBUS supply */
+	if (phy_drd->vbus)
+		regulator_disable(phy_drd->vbus);
+
+	clk_disable_unprepare(phy_drd->ref_clk);
+
+	return 0;
+}
+
+static struct phy *exynos5_usbdrd_phy_xlate(struct device *dev,
+					struct of_phandle_args *args)
+{
+	struct exynos5_usbdrd_phy *phy_drd = dev_get_drvdata(dev);
+
+	if (WARN_ON(args->args[0] > EXYNOS5_DRDPHYS_NUM))
+		return ERR_PTR(-ENODEV);
+
+	return phy_drd->phys[args->args[0]].phy;
+}
+
+static struct phy_ops exynos5_usbdrd_phy_ops = {
+	.init		= exynos5_usbdrd_phy_init,
+	.exit		= exynos5_usbdrd_phy_exit,
+	.power_on	= exynos5_usbdrd_phy_power_on,
+	.power_off	= exynos5_usbdrd_phy_power_off,
+	.owner		= THIS_MODULE,
+};
+
+const struct exynos5_usbdrd_phy_config phy_cfg_exynos5[] = {
+	{
+		.id		= EXYNOS5_DRDPHY_UTMI,
+		.phy_isol	= exynos5_usbdrd_phy_isol,
+		.phy_init	= exynos5_usbdrd_utmi_init,
+		.set_refclk	= exynos5_usbdrd_utmi_set_refclk,
+	},
+	{
+		.id		= EXYNOS5_DRDPHY_PIPE3,
+		.phy_isol	= exynos5_usbdrd_phy_isol,
+		.phy_init	= exynos5_usbdrd_pipe3_init,
+		.set_refclk	= exynos5_usbdrd_pipe3_set_refclk,
+	},
+};
+
+const struct exynos5_usbdrd_phy_drvdata exynos5420_usbdrd_phy = {
+	.phy_cfg		= phy_cfg_exynos5,
+	.pmu_offset_usbdrd0_phy	= EXYNOS5_USBDRD_PHY_CONTROL,
+	.pmu_offset_usbdrd1_phy	= EXYNOS5420_USBDRD1_PHY_CONTROL,
+};
+
+const struct exynos5_usbdrd_phy_drvdata exynos5250_usbdrd_phy = {
+	.phy_cfg		= phy_cfg_exynos5,
+	.pmu_offset_usbdrd0_phy	= EXYNOS5_USBDRD_PHY_CONTROL,
+};
+
+static const struct of_device_id exynos5_usbdrd_phy_of_match[] = {
+	{
+		.compatible = "samsung,exynos5250-usbdrd-phy",
+		.data = &exynos5250_usbdrd_phy
+	}, {
+		.compatible = "samsung,exynos5420-usbdrd-phy",
+		.data = &exynos5420_usbdrd_phy
+	},
+	{ },
+};
+
+static int exynos5_usbdrd_phy_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *node = dev->of_node;
+	struct exynos5_usbdrd_phy *phy_drd;
+	struct phy_provider *phy_provider;
+	struct resource *res;
+	const struct of_device_id *match;
+	const struct exynos5_usbdrd_phy_drvdata *drv_data;
+	struct regmap *reg_pmu;
+	u32 pmu_offset;
+	unsigned long ref_rate;
+	int i, ret;
+	int channel;
+
+	phy_drd = devm_kzalloc(dev, sizeof(*phy_drd), GFP_KERNEL);
+	if (!phy_drd)
+		return -ENOMEM;
+
+	dev_set_drvdata(dev, phy_drd);
+	phy_drd->dev = dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	phy_drd->reg_phy = devm_ioremap_resource(dev, res);
+	if (IS_ERR(phy_drd->reg_phy))
+		return PTR_ERR(phy_drd->reg_phy);
+
+	match = of_match_node(exynos5_usbdrd_phy_of_match, pdev->dev.of_node);
+
+	drv_data = match->data;
+	phy_drd->drv_data = drv_data;
+
+	phy_drd->clk = devm_clk_get(dev, "phy");
+	if (IS_ERR(phy_drd->clk)) {
+		dev_err(dev, "Failed to get clock of phy controller\n");
+		return PTR_ERR(phy_drd->clk);
+	}
+
+	phy_drd->ref_clk = devm_clk_get(dev, "ref");
+	if (IS_ERR(phy_drd->ref_clk)) {
+		dev_err(dev, "Failed to get reference clock of usbdrd phy\n");
+		return PTR_ERR(phy_drd->ref_clk);
+	}
+	ref_rate = clk_get_rate(phy_drd->ref_clk);
+
+	ret = exynos5_rate_to_clk(ref_rate, &phy_drd->extrefclk);
+	if (ret) {
+		dev_err(phy_drd->dev, "Clock rate (%ld) not supported\n",
+			ref_rate);
+		return ret;
+	}
+
+	reg_pmu = syscon_regmap_lookup_by_phandle(dev->of_node,
+						   "samsung,pmu-syscon");
+	if (IS_ERR(reg_pmu)) {
+		dev_err(dev, "Failed to lookup PMU regmap\n");
+		return PTR_ERR(reg_pmu);
+	}
+
+	/*
+	 * Exynos5420 SoC has multiple channels for USB 3.0 PHY, with
+	 * each having separate power control registers.
+	 * 'channel' facilitates to set such registers.
+	 */
+	channel = of_alias_get_id(node, "usbdrdphy");
+	if (channel < 0)
+		dev_dbg(dev, "Not a multi-controller usbdrd phy\n");
+
+	switch (channel) {
+	case 1:
+		pmu_offset = phy_drd->drv_data->pmu_offset_usbdrd1_phy;
+		break;
+	case 0:
+	default:
+		pmu_offset = phy_drd->drv_data->pmu_offset_usbdrd0_phy;
+		break;
+	}
+
+	/* Get Vbus regulator */
+	phy_drd->vbus = devm_regulator_get(dev, "vbus");
+	if (IS_ERR(phy_drd->vbus)) {
+		ret = PTR_ERR(phy_drd->vbus);
+		if (ret == -EPROBE_DEFER)
+			return ret;
+
+		dev_warn(dev, "Failed to get VBUS supply regulator\n");
+		phy_drd->vbus = NULL;
+	}
+
+	dev_vdbg(dev, "Creating usbdrd_phy phy\n");
+
+	for (i = 0; i < EXYNOS5_DRDPHYS_NUM; i++) {
+		struct phy *phy = devm_phy_create(dev, &exynos5_usbdrd_phy_ops,
+						  NULL);
+		if (IS_ERR(phy)) {
+			dev_err(dev, "Failed to create usbdrd_phy phy\n");
+			return PTR_ERR(phy);
+		}
+
+		phy_drd->phys[i].phy = phy;
+		phy_drd->phys[i].index = i;
+		phy_drd->phys[i].reg_pmu = reg_pmu;
+		phy_drd->phys[i].pmu_offset = pmu_offset;
+		phy_drd->phys[i].phy_cfg = &drv_data->phy_cfg[i];
+		phy_set_drvdata(phy, &phy_drd->phys[i]);
+	}
+
+	phy_provider = devm_of_phy_provider_register(dev,
+						     exynos5_usbdrd_phy_xlate);
+	if (IS_ERR(phy_provider)) {
+		dev_err(phy_drd->dev, "Failed to register phy provider\n");
+		return PTR_ERR(phy_provider);
+	}
+
+	return 0;
+}
+
+static struct platform_driver exynos5_usb3drd_phy = {
+	.probe	= exynos5_usbdrd_phy_probe,
+	.driver = {
+		.of_match_table	= exynos5_usbdrd_phy_of_match,
+		.name		= "exynos5_usb3drd_phy",
+		.owner		= THIS_MODULE,
+	}
+};
+
+module_platform_driver(exynos5_usb3drd_phy);
+MODULE_DESCRIPTION("Samsung EXYNOS5 SoCs USB 3.0 DRD controller PHY driver");
+MODULE_AUTHOR("Vivek Gautam <gautam.vivek@samsung.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:exynos5_usb3drd_phy");
diff --git a/drivers/phy/phy-exynos5250-sata.c b/drivers/phy/phy-exynos5250-sata.c
index c9361b731fa..05689450f93 100644
--- a/drivers/phy/phy-exynos5250-sata.c
+++ b/drivers/phy/phy-exynos5250-sata.c
@@ -246,6 +246,6 @@ static struct platform_driver exynos_sata_phy_driver = {
 module_platform_driver(exynos_sata_phy_driver);
 
 MODULE_DESCRIPTION("Samsung SerDes PHY driver");
-MODULE_LICENSE("GPL V2");
+MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Girish K S <ks.giri@samsung.com>");
 MODULE_AUTHOR("Yuvaraj C D <yuvaraj.cd@samsung.com>");
diff --git a/drivers/phy/phy-omap-usb2.c b/drivers/phy/phy-omap-usb2.c
index a2205a841e5..7007c11fe07 100644
--- a/drivers/phy/phy-omap-usb2.c
+++ b/drivers/phy/phy-omap-usb2.c
@@ -275,18 +275,34 @@ static int omap_usb2_probe(struct platform_device *pdev)
 	if (IS_ERR(phy_provider))
 		return PTR_ERR(phy_provider);
 
-	phy->wkupclk = devm_clk_get(phy->dev, "usb_phy_cm_clk32k");
+	phy->wkupclk = devm_clk_get(phy->dev, "wkupclk");
 	if (IS_ERR(phy->wkupclk)) {
-		dev_err(&pdev->dev, "unable to get usb_phy_cm_clk32k\n");
-		return PTR_ERR(phy->wkupclk);
+		dev_warn(&pdev->dev, "unable to get wkupclk, trying old name\n");
+		phy->wkupclk = devm_clk_get(phy->dev, "usb_phy_cm_clk32k");
+		if (IS_ERR(phy->wkupclk)) {
+			dev_err(&pdev->dev, "unable to get usb_phy_cm_clk32k\n");
+			return PTR_ERR(phy->wkupclk);
+		} else {
+			dev_warn(&pdev->dev,
+				 "found usb_phy_cm_clk32k, please fix DTS\n");
+		}
 	}
 	clk_prepare(phy->wkupclk);
 
-	phy->optclk = devm_clk_get(phy->dev, "usb_otg_ss_refclk960m");
-	if (IS_ERR(phy->optclk))
-		dev_vdbg(&pdev->dev, "unable to get refclk960m\n");
-	else
+	phy->optclk = devm_clk_get(phy->dev, "refclk");
+	if (IS_ERR(phy->optclk)) {
+		dev_dbg(&pdev->dev, "unable to get refclk, trying old name\n");
+		phy->optclk = devm_clk_get(phy->dev, "usb_otg_ss_refclk960m");
+		if (IS_ERR(phy->optclk)) {
+			dev_dbg(&pdev->dev,
+				"unable to get usb_otg_ss_refclk960m\n");
+		} else {
+			dev_warn(&pdev->dev,
+				 "found usb_otg_ss_refclk960m, please fix DTS\n");
+		}
+	} else {
 		clk_prepare(phy->optclk);
+	}
 
 	usb_add_phy_dev(&phy->phy);
 
diff --git a/drivers/phy/phy-sun4i-usb.c b/drivers/phy/phy-sun4i-usb.c
index e6e6c4ba714..115d8d5190d 100644
--- a/drivers/phy/phy-sun4i-usb.c
+++ b/drivers/phy/phy-sun4i-usb.c
@@ -61,7 +61,6 @@
 #define MAX_PHYS			3
 
 struct sun4i_usb_phy_data {
-	struct clk *clk;
 	void __iomem *base;
 	struct mutex mutex;
 	int num_phys;
@@ -71,6 +70,7 @@ struct sun4i_usb_phy_data {
 		void __iomem *pmu;
 		struct regulator *vbus;
 		struct reset_control *reset;
+		struct clk *clk;
 		int index;
 	} phys[MAX_PHYS];
 };
@@ -146,13 +146,13 @@ static int sun4i_usb_phy_init(struct phy *_phy)
 	struct sun4i_usb_phy_data *data = to_sun4i_usb_phy_data(phy);
 	int ret;
 
-	ret = clk_prepare_enable(data->clk);
+	ret = clk_prepare_enable(phy->clk);
 	if (ret)
 		return ret;
 
 	ret = reset_control_deassert(phy->reset);
 	if (ret) {
-		clk_disable_unprepare(data->clk);
+		clk_disable_unprepare(phy->clk);
 		return ret;
 	}
 
@@ -170,11 +170,10 @@ static int sun4i_usb_phy_init(struct phy *_phy)
 static int sun4i_usb_phy_exit(struct phy *_phy)
 {
 	struct sun4i_usb_phy *phy = phy_get_drvdata(_phy);
-	struct sun4i_usb_phy_data *data = to_sun4i_usb_phy_data(phy);
 
 	sun4i_usb_phy_passby(phy, 0);
 	reset_control_assert(phy->reset);
-	clk_disable_unprepare(data->clk);
+	clk_disable_unprepare(phy->clk);
 
 	return 0;
 }
@@ -224,13 +223,9 @@ static int sun4i_usb_phy_probe(struct platform_device *pdev)
 	struct sun4i_usb_phy_data *data;
 	struct device *dev = &pdev->dev;
 	struct device_node *np = dev->of_node;
-	void __iomem *pmu = NULL;
 	struct phy_provider *phy_provider;
-	struct reset_control *reset;
-	struct regulator *vbus;
+	bool dedicated_clocks;
 	struct resource *res;
-	struct phy *phy;
-	char name[16];
 	int i;
 
 	data = devm_kzalloc(dev, sizeof(*data), GFP_KERNEL);
@@ -249,55 +244,64 @@ static int sun4i_usb_phy_probe(struct platform_device *pdev)
 	else
 		data->disc_thresh = 2;
 
+	if (of_device_is_compatible(np, "allwinner,sun6i-a31-usb-phy"))
+		dedicated_clocks = true;
+	else
+		dedicated_clocks = false;
+
 	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "phy_ctrl");
 	data->base = devm_ioremap_resource(dev, res);
 	if (IS_ERR(data->base))
 		return PTR_ERR(data->base);
 
-	data->clk = devm_clk_get(dev, "usb_phy");
-	if (IS_ERR(data->clk)) {
-		dev_err(dev, "could not get usb_phy clock\n");
-		return PTR_ERR(data->clk);
-	}
-
 	/* Skip 0, 0 is the phy for otg which is not yet supported. */
 	for (i = 1; i < data->num_phys; i++) {
+		struct sun4i_usb_phy *phy = data->phys + i;
+		char name[16];
+
 		snprintf(name, sizeof(name), "usb%d_vbus", i);
-		vbus = devm_regulator_get_optional(dev, name);
-		if (IS_ERR(vbus)) {
-			if (PTR_ERR(vbus) == -EPROBE_DEFER)
+		phy->vbus = devm_regulator_get_optional(dev, name);
+		if (IS_ERR(phy->vbus)) {
+			if (PTR_ERR(phy->vbus) == -EPROBE_DEFER)
 				return -EPROBE_DEFER;
-			vbus = NULL;
+			phy->vbus = NULL;
+		}
+
+		if (dedicated_clocks)
+			snprintf(name, sizeof(name), "usb%d_phy", i);
+		else
+			strlcpy(name, "usb_phy", sizeof(name));
+
+		phy->clk = devm_clk_get(dev, name);
+		if (IS_ERR(phy->clk)) {
+			dev_err(dev, "failed to get clock %s\n", name);
+			return PTR_ERR(phy->clk);
 		}
 
 		snprintf(name, sizeof(name), "usb%d_reset", i);
-		reset = devm_reset_control_get(dev, name);
-		if (IS_ERR(reset)) {
+		phy->reset = devm_reset_control_get(dev, name);
+		if (IS_ERR(phy->reset)) {
 			dev_err(dev, "failed to get reset %s\n", name);
-			return PTR_ERR(reset);
+			return PTR_ERR(phy->reset);
 		}
 
 		if (i) { /* No pmu for usbc0 */
 			snprintf(name, sizeof(name), "pmu%d", i);
 			res = platform_get_resource_byname(pdev,
 							IORESOURCE_MEM, name);
-			pmu = devm_ioremap_resource(dev, res);
-			if (IS_ERR(pmu))
-				return PTR_ERR(pmu);
+			phy->pmu = devm_ioremap_resource(dev, res);
+			if (IS_ERR(phy->pmu))
+				return PTR_ERR(phy->pmu);
 		}
 
-		phy = devm_phy_create(dev, &sun4i_usb_phy_ops, NULL);
-		if (IS_ERR(phy)) {
+		phy->phy = devm_phy_create(dev, &sun4i_usb_phy_ops, NULL);
+		if (IS_ERR(phy->phy)) {
 			dev_err(dev, "failed to create PHY %d\n", i);
-			return PTR_ERR(phy);
+			return PTR_ERR(phy->phy);
 		}
 
-		data->phys[i].phy = phy;
-		data->phys[i].pmu = pmu;
-		data->phys[i].vbus = vbus;
-		data->phys[i].reset = reset;
-		data->phys[i].index = i;
-		phy_set_drvdata(phy, &data->phys[i]);
+		phy->index = i;
+		phy_set_drvdata(phy->phy, &data->phys[i]);
 	}
 
 	dev_set_drvdata(dev, data);
@@ -311,6 +315,7 @@ static int sun4i_usb_phy_probe(struct platform_device *pdev)
 static const struct of_device_id sun4i_usb_phy_of_match[] = {
 	{ .compatible = "allwinner,sun4i-a10-usb-phy" },
 	{ .compatible = "allwinner,sun5i-a13-usb-phy" },
+	{ .compatible = "allwinner,sun6i-a31-usb-phy" },
 	{ .compatible = "allwinner,sun7i-a20-usb-phy" },
 	{ },
 };
diff --git a/drivers/pinctrl/Kconfig b/drivers/pinctrl/Kconfig
index e00c02d0a09..0042ccb46b9 100644
--- a/drivers/pinctrl/Kconfig
+++ b/drivers/pinctrl/Kconfig
@@ -84,6 +84,9 @@ config PINCTRL_AT91
 	depends on ARCH_AT91
 	select PINMUX
 	select PINCONF
+	select GPIOLIB
+	select OF_GPIO
+	select GPIOLIB_IRQCHIP
 	help
 	  Say Y here to enable the at91 pinctrl driver
 
@@ -185,6 +188,13 @@ config PINCTRL_IMX6SL
 	help
 	  Say Y here to enable the imx6sl pinctrl driver
 
+config PINCTRL_IMX6SX
+	bool "IMX6SX pinctrl driver"
+	depends on SOC_IMX6SX
+	select PINCTRL_IMX
+	help
+	  Say Y here to enable the imx6sx pinctrl driver
+
 config PINCTRL_VF610
 	bool "Freescale Vybrid VF610 pinctrl driver"
 	depends on SOC_VF610
@@ -221,10 +231,27 @@ config PINCTRL_MSM
 	select PINMUX
 	select PINCONF
 	select GENERIC_PINCONF
+	select GPIOLIB_IRQCHIP
+
+config PINCTRL_APQ8064
+	tristate "Qualcomm APQ8064 pin controller driver"
+	depends on GPIOLIB && OF
+	select PINCTRL_MSM
+	help
+	  This is the pinctrl, pinmux, pinconf and gpiolib driver for the
+	  Qualcomm TLMM block found in the Qualcomm APQ8064 platform.
+
+config PINCTRL_IPQ8064
+	tristate "Qualcomm IPQ8064 pin controller driver"
+	depends on GPIOLIB && OF
+	select PINCTRL_MSM
+	help
+	  This is the pinctrl, pinmux, pinconf and gpiolib driver for the
+	  Qualcomm TLMM block found in the Qualcomm IPQ8064 platform.
 
 config PINCTRL_MSM8X74
 	tristate "Qualcomm 8x74 pin controller driver"
-	depends on GPIOLIB && OF
+	depends on GPIOLIB && OF && (ARCH_QCOM || COMPILE_TEST)
 	select PINCTRL_MSM
 	help
 	  This is the pinctrl, pinmux, pinconf and gpiolib driver for the
@@ -256,6 +283,7 @@ config PINCTRL_ROCKCHIP
 	select PINMUX
 	select GENERIC_PINCONF
 	select GENERIC_IRQ_CHIP
+	select MFD_SYSCON
 
 config PINCTRL_SINGLE
 	tristate "One-register-per-pin type device tree based pinctrl driver"
@@ -270,17 +298,14 @@ config PINCTRL_SIRF
 	bool "CSR SiRFprimaII/SiRFmarco pin controller driver"
 	depends on ARCH_SIRF
 	select PINMUX
-
-config PINCTRL_SUNXI
-	bool
-	select PINMUX
-	select GENERIC_PINCONF
+	select GPIOLIB_IRQCHIP
 
 config PINCTRL_ST
 	bool
 	depends on OF
 	select PINMUX
 	select PINCONF
+	select GPIOLIB_IRQCHIP
 
 config PINCTRL_TEGRA
 	bool
@@ -368,9 +393,11 @@ config PINCTRL_S3C64XX
 	depends on ARCH_S3C64XX
 	select PINCTRL_SAMSUNG
 
+source "drivers/pinctrl/berlin/Kconfig"
 source "drivers/pinctrl/mvebu/Kconfig"
 source "drivers/pinctrl/sh-pfc/Kconfig"
 source "drivers/pinctrl/spear/Kconfig"
+source "drivers/pinctrl/sunxi/Kconfig"
 source "drivers/pinctrl/vt8500/Kconfig"
 
 config PINCTRL_XWAY
diff --git a/drivers/pinctrl/Makefile b/drivers/pinctrl/Makefile
index 6d3fd62b9ae..c4b5d405b8f 100644
--- a/drivers/pinctrl/Makefile
+++ b/drivers/pinctrl/Makefile
@@ -32,12 +32,15 @@ obj-$(CONFIG_PINCTRL_IMX53)	+= pinctrl-imx53.o
 obj-$(CONFIG_PINCTRL_IMX6Q)	+= pinctrl-imx6q.o
 obj-$(CONFIG_PINCTRL_IMX6Q)	+= pinctrl-imx6dl.o
 obj-$(CONFIG_PINCTRL_IMX6SL)	+= pinctrl-imx6sl.o
+obj-$(CONFIG_PINCTRL_IMX6SX)	+= pinctrl-imx6sx.o
 obj-$(CONFIG_PINCTRL_FALCON)	+= pinctrl-falcon.o
 obj-$(CONFIG_PINCTRL_MXS)	+= pinctrl-mxs.o
 obj-$(CONFIG_PINCTRL_IMX23)	+= pinctrl-imx23.o
 obj-$(CONFIG_PINCTRL_IMX25)	+= pinctrl-imx25.o
 obj-$(CONFIG_PINCTRL_IMX28)	+= pinctrl-imx28.o
 obj-$(CONFIG_PINCTRL_MSM)	+= pinctrl-msm.o
+obj-$(CONFIG_PINCTRL_APQ8064)	+= pinctrl-apq8064.o
+obj-$(CONFIG_PINCTRL_IPQ8064)	+= pinctrl-ipq8064.o
 obj-$(CONFIG_PINCTRL_MSM8X74)	+= pinctrl-msm8x74.o
 obj-$(CONFIG_PINCTRL_NOMADIK)	+= pinctrl-nomadik.o
 obj-$(CONFIG_PINCTRL_STN8815)	+= pinctrl-nomadik-stn8815.o
@@ -47,7 +50,6 @@ obj-$(CONFIG_PINCTRL_PALMAS)	+= pinctrl-palmas.o
 obj-$(CONFIG_PINCTRL_ROCKCHIP)	+= pinctrl-rockchip.o
 obj-$(CONFIG_PINCTRL_SINGLE)	+= pinctrl-single.o
 obj-$(CONFIG_PINCTRL_SIRF)	+= sirf/
-obj-$(CONFIG_PINCTRL_SUNXI)	+= pinctrl-sunxi.o
 obj-$(CONFIG_PINCTRL_TEGRA)	+= pinctrl-tegra.o
 obj-$(CONFIG_PINCTRL_TEGRA20)	+= pinctrl-tegra20.o
 obj-$(CONFIG_PINCTRL_TEGRA30)	+= pinctrl-tegra30.o
@@ -68,8 +70,10 @@ obj-$(CONFIG_PINCTRL_TB10X)	+= pinctrl-tb10x.o
 obj-$(CONFIG_PINCTRL_ST) 	+= pinctrl-st.o
 obj-$(CONFIG_PINCTRL_VF610)	+= pinctrl-vf610.o
 
+obj-$(CONFIG_ARCH_BERLIN)	+= berlin/
 obj-$(CONFIG_PLAT_ORION)        += mvebu/
 obj-$(CONFIG_ARCH_SHMOBILE)	+= sh-pfc/
 obj-$(CONFIG_SUPERH)		+= sh-pfc/
 obj-$(CONFIG_PLAT_SPEAR)	+= spear/
 obj-$(CONFIG_ARCH_VT8500)	+= vt8500/
+obj-$(CONFIG_ARCH_SUNXI)	+= sunxi/
diff --git a/drivers/pinctrl/berlin/Kconfig b/drivers/pinctrl/berlin/Kconfig
new file mode 100644
index 00000000000..b18322bc7bf
--- /dev/null
+++ b/drivers/pinctrl/berlin/Kconfig
@@ -0,0 +1,20 @@
+if ARCH_BERLIN
+
+config PINCTRL_BERLIN
+	bool
+	select PINMUX
+	select REGMAP_MMIO
+
+config PINCTRL_BERLIN_BG2
+	bool
+	select PINCTRL_BERLIN
+
+config PINCTRL_BERLIN_BG2CD
+	bool
+	select PINCTRL_BERLIN
+
+config PINCTRL_BERLIN_BG2Q
+	bool
+	select PINCTRL_BERLIN
+
+endif
diff --git a/drivers/pinctrl/berlin/Makefile b/drivers/pinctrl/berlin/Makefile
new file mode 100644
index 00000000000..deb0c6baf31
--- /dev/null
+++ b/drivers/pinctrl/berlin/Makefile
@@ -0,0 +1,4 @@
+obj-$(CONFIG_PINCTRL_BERLIN)		+= berlin.o
+obj-$(CONFIG_PINCTRL_BERLIN_BG2)	+= berlin-bg2.o
+obj-$(CONFIG_PINCTRL_BERLIN_BG2CD)	+= berlin-bg2cd.o
+obj-$(CONFIG_PINCTRL_BERLIN_BG2Q)	+= berlin-bg2q.o
diff --git a/drivers/pinctrl/berlin/berlin-bg2.c b/drivers/pinctrl/berlin/berlin-bg2.c
new file mode 100644
index 00000000000..dcd4f6a4fc5
--- /dev/null
+++ b/drivers/pinctrl/berlin/berlin-bg2.c
@@ -0,0 +1,274 @@
+/*
+ * Marvell Berlin BG2 pinctrl driver.
+ *
+ * Copyright (C) 2014 Marvell Technology Group Ltd.
+ *
+ * Antoine Ténart <antoine.tenart@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.
+ */
+
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+
+#include "berlin.h"
+
+static const struct berlin_desc_group berlin2_soc_pinctrl_groups[] = {
+	/* G */
+	BERLIN_PINCTRL_GROUP("G0", 0x00, 0x1, 0x00,
+		BERLIN_PINCTRL_FUNCTION(0x0, "spi1"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "gpio")),
+	BERLIN_PINCTRL_GROUP("G1", 0x00, 0x2, 0x01,
+		BERLIN_PINCTRL_FUNCTION(0x0, "spi1"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "usb1")),
+	BERLIN_PINCTRL_GROUP("G2", 0x00, 0x2, 0x02,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "spi1"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "pwm"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "i2s0")),
+	BERLIN_PINCTRL_GROUP("G3", 0x00, 0x2, 0x04,
+		BERLIN_PINCTRL_FUNCTION(0x0, "soc"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "spi1"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "i2s1")),
+	BERLIN_PINCTRL_GROUP("G4", 0x00, 0x2, 0x06,
+		BERLIN_PINCTRL_FUNCTION(0x0, "spi1"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "pwm")),
+	BERLIN_PINCTRL_GROUP("G5", 0x00, 0x3, 0x08,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "sts1"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "et"),
+		/*
+		 * Mode 0x3 mux i2s2 mclk *and* i2s3 mclk:
+		 * add two functions so it can be used with other groups
+		 * within the same subnode in the device tree
+		 */
+		BERLIN_PINCTRL_FUNCTION(0x3, "i2s2"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "i2s3")),
+	BERLIN_PINCTRL_GROUP("G6", 0x00, 0x2, 0x0b,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "sts0"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "et")),
+	BERLIN_PINCTRL_GROUP("G7", 0x00, 0x3, 0x0d,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "sts0"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "et"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "vdac")),
+	BERLIN_PINCTRL_GROUP("G8", 0x00, 0x3, 0x10,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "sd0"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "et"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "usb0_dbg"),
+		BERLIN_PINCTRL_FUNCTION(0x4, "sata_dbg"),
+		BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G9", 0x00, 0x3, 0x13,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "sd0"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "et"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "usb0_dbg"),
+		BERLIN_PINCTRL_FUNCTION(0x4, "sata_dbg"),
+		BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G10", 0x00, 0x2, 0x16,
+		BERLIN_PINCTRL_FUNCTION(0x0, "soc"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "twsi0"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "ptp")),
+	BERLIN_PINCTRL_GROUP("G11", 0x00, 0x2, 0x18,
+		BERLIN_PINCTRL_FUNCTION(0x0, "soc"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "twsi1"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "eddc")),
+	BERLIN_PINCTRL_GROUP("G12", 0x00, 0x3, 0x1a,
+		BERLIN_PINCTRL_FUNCTION(0x0, "sts2"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "sata"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "sd1"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "usb0_dbg"),
+		BERLIN_PINCTRL_FUNCTION(0x4, "sts1"),
+		BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G13", 0x04, 0x3, 0x00,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "sata"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "sd1"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "usb0_dbg"),
+		BERLIN_PINCTRL_FUNCTION(0x4, "sts1"),
+		BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G14", 0x04, 0x1, 0x03,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("G15", 0x04, 0x2, 0x04,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "et"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "osco")),
+	BERLIN_PINCTRL_GROUP("G16", 0x04, 0x3, 0x06,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "dvio"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "fp")),
+	BERLIN_PINCTRL_GROUP("G17", 0x04, 0x3, 0x09,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "dvio"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "fp")),
+	BERLIN_PINCTRL_GROUP("G18", 0x04, 0x1, 0x0c,
+		BERLIN_PINCTRL_FUNCTION(0x0, "pll"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "i2s0")),
+	BERLIN_PINCTRL_GROUP("G19", 0x04, 0x1, 0x0d,
+		BERLIN_PINCTRL_FUNCTION(0x0, "i2s0"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "pwm")),
+	BERLIN_PINCTRL_GROUP("G20", 0x04, 0x1, 0x0e,
+		BERLIN_PINCTRL_FUNCTION(0x0, "spdif"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "arc")),
+	BERLIN_PINCTRL_GROUP("G21", 0x04, 0x3, 0x0f,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "dvio"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "adac_dbg"),
+		BERLIN_PINCTRL_FUNCTION(0x4, "pdm_a"),	/* gpio17..19,pdm */
+		BERLIN_PINCTRL_FUNCTION(0x7, "pdm_b")),	/* gpio12..14,pdm */
+	BERLIN_PINCTRL_GROUP("G22", 0x04, 0x3, 0x12,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "dv0"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "twsi0"),
+		BERLIN_PINCTRL_FUNCTION(0x4, "pwm")),
+	BERLIN_PINCTRL_GROUP("G23", 0x04, 0x3, 0x15,
+		BERLIN_PINCTRL_FUNCTION(0x0, "vclki"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "dv0"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "i2s0"),
+		BERLIN_PINCTRL_FUNCTION(0x4, "pwm"),
+		BERLIN_PINCTRL_FUNCTION(0x7, "pdm")),
+	BERLIN_PINCTRL_GROUP("G24", 0x04, 0x2, 0x18,
+		BERLIN_PINCTRL_FUNCTION(0x0, "i2s2"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "i2s1")),
+	BERLIN_PINCTRL_GROUP("G25", 0x04, 0x2, 0x1a,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "nand"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "i2s2")),
+	BERLIN_PINCTRL_GROUP("G26", 0x04, 0x1, 0x1c,
+		BERLIN_PINCTRL_FUNCTION(0x0, "nand"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "emmc")),
+	BERLIN_PINCTRL_GROUP("G27", 0x04, 0x1, 0x1d,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "nand")),
+	BERLIN_PINCTRL_GROUP("G28", 0x04, 0x2, 0x1e,
+		BERLIN_PINCTRL_FUNCTION(0x0, "dvo"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "sp")),
+};
+
+static const struct berlin_desc_group berlin2_sysmgr_pinctrl_groups[] = {
+	/* GSM */
+	BERLIN_PINCTRL_GROUP("GSM0", 0x40, 0x2, 0x00,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "spi2"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "eth1")),
+	BERLIN_PINCTRL_GROUP("GSM1", 0x40, 0x2, 0x02,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "spi2"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "eth1")),
+	BERLIN_PINCTRL_GROUP("GSM2", 0x40, 0x2, 0x04,
+		BERLIN_PINCTRL_FUNCTION(0x0, "twsi2"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "spi2")),
+	BERLIN_PINCTRL_GROUP("GSM3", 0x40, 0x2, 0x06,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "uart0"),	/* CTS/RTS */
+		BERLIN_PINCTRL_FUNCTION(0x2, "uart2"),	/* RX/TX */
+		BERLIN_PINCTRL_FUNCTION(0x3, "twsi2")),
+	BERLIN_PINCTRL_GROUP("GSM4", 0x40, 0x2, 0x08,
+		BERLIN_PINCTRL_FUNCTION(0x0, "uart0"),	/* RX/TX */
+		BERLIN_PINCTRL_FUNCTION(0x1, "irda0")),
+	BERLIN_PINCTRL_GROUP("GSM5", 0x40, 0x2, 0x0a,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "uart1"),	/* RX/TX */
+		BERLIN_PINCTRL_FUNCTION(0x2, "irda1"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "twsi3")),
+	BERLIN_PINCTRL_GROUP("GSM6", 0x40, 0x2, 0x0c,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "spi2"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "clki")),
+	BERLIN_PINCTRL_GROUP("GSM7", 0x40, 0x1, 0x0e,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "hdmi")),
+	BERLIN_PINCTRL_GROUP("GSM8", 0x40, 0x1, 0x0f,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "hdmi")),
+	BERLIN_PINCTRL_GROUP("GSM9", 0x40, 0x1, 0x10,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "led")),
+	BERLIN_PINCTRL_GROUP("GSM10", 0x40, 0x1, 0x11,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "led")),
+	BERLIN_PINCTRL_GROUP("GSM11", 0x40, 0x1, 0x12,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "led")),
+};
+
+static const struct berlin_pinctrl_desc berlin2_soc_pinctrl_data = {
+	.groups = berlin2_soc_pinctrl_groups,
+	.ngroups = ARRAY_SIZE(berlin2_soc_pinctrl_groups),
+};
+
+static const struct berlin_pinctrl_desc berlin2_sysmgr_pinctrl_data = {
+	.groups = berlin2_sysmgr_pinctrl_groups,
+	.ngroups = ARRAY_SIZE(berlin2_sysmgr_pinctrl_groups),
+};
+
+static const struct of_device_id berlin2_pinctrl_match[] = {
+	{
+		.compatible = "marvell,berlin2-chip-ctrl",
+		.data = &berlin2_soc_pinctrl_data
+	},
+	{
+		.compatible = "marvell,berlin2-system-ctrl",
+		.data = &berlin2_sysmgr_pinctrl_data
+	},
+	{}
+};
+MODULE_DEVICE_TABLE(of, berlin2_pinctrl_match);
+
+static int berlin2_pinctrl_probe(struct platform_device *pdev)
+{
+	const struct of_device_id *match =
+		of_match_device(berlin2_pinctrl_match, &pdev->dev);
+	struct regmap_config *rmconfig;
+	struct regmap *regmap;
+	struct resource *res;
+	void __iomem *base;
+
+	rmconfig = devm_kzalloc(&pdev->dev, sizeof(*rmconfig), GFP_KERNEL);
+	if (!rmconfig)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	rmconfig->reg_bits = 32,
+	rmconfig->val_bits = 32,
+	rmconfig->reg_stride = 4,
+	rmconfig->max_register = resource_size(res);
+
+	regmap = devm_regmap_init_mmio(&pdev->dev, base, rmconfig);
+	if (IS_ERR(regmap))
+		return PTR_ERR(regmap);
+
+	return berlin_pinctrl_probe(pdev, match->data);
+}
+
+static struct platform_driver berlin2_pinctrl_driver = {
+	.probe	= berlin2_pinctrl_probe,
+	.driver	= {
+		.name = "berlin-bg2-pinctrl",
+		.owner = THIS_MODULE,
+		.of_match_table = berlin2_pinctrl_match,
+	},
+};
+module_platform_driver(berlin2_pinctrl_driver);
+
+MODULE_AUTHOR("Antoine Ténart <antoine.tenart@free-electrons.com>");
+MODULE_DESCRIPTION("Marvell Berlin BG2 pinctrl driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/pinctrl/berlin/berlin-bg2cd.c b/drivers/pinctrl/berlin/berlin-bg2cd.c
new file mode 100644
index 00000000000..89d585ef7da
--- /dev/null
+++ b/drivers/pinctrl/berlin/berlin-bg2cd.c
@@ -0,0 +1,217 @@
+/*
+ * Marvell Berlin BG2CD pinctrl driver.
+ *
+ * Copyright (C) 2014 Marvell Technology Group Ltd.
+ *
+ * Antoine Ténart <antoine.tenart@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.
+ */
+
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+
+#include "berlin.h"
+
+static const struct berlin_desc_group berlin2cd_soc_pinctrl_groups[] = {
+	/* G */
+	BERLIN_PINCTRL_GROUP("G0", 0x00, 0x1, 0x00,
+		BERLIN_PINCTRL_FUNCTION(0x0, "jtag"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "led"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "pwm")),
+	BERLIN_PINCTRL_GROUP("G1", 0x00, 0x2, 0x01,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "sd0"),
+		BERLIN_PINCTRL_FUNCTION(0x6, "usb0_dbg"),
+		BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G2", 0x00, 0x2, 0x02,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "sd0"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "fe"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "pll"),
+		BERLIN_PINCTRL_FUNCTION(0x6, "usb0_dbg"),
+		BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G3", 0x00, 0x2, 0x04,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "sd0"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "twsi2"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "pll"),
+		BERLIN_PINCTRL_FUNCTION(0x4, "fe"),
+		BERLIN_PINCTRL_FUNCTION(0x6, "usb0_dbg"),
+		BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G4", 0x00, 0x2, 0x06,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "sd0"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "twsi3"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "pll"),
+		BERLIN_PINCTRL_FUNCTION(0x4, "pwm"),
+		BERLIN_PINCTRL_FUNCTION(0x6, "usb0_dbg"),
+		BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G5", 0x00, 0x3, 0x08,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "sd0"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "twsi3"),
+		BERLIN_PINCTRL_FUNCTION(0x3, "arc"),
+		BERLIN_PINCTRL_FUNCTION(0x4, "pwm"),
+		BERLIN_PINCTRL_FUNCTION(0x6, "usb0_dbg"),
+		BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G6", 0x00, 0x2, 0x0b,
+		BERLIN_PINCTRL_FUNCTION(0x0, "uart0"),	/* RX/TX */
+		BERLIN_PINCTRL_FUNCTION(0x1, "gpio")),
+	BERLIN_PINCTRL_GROUP("G7", 0x00, 0x3, 0x0d,
+		BERLIN_PINCTRL_FUNCTION(0x0, "eddc"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "twsi1"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "gpio")),
+	BERLIN_PINCTRL_GROUP("G8", 0x00, 0x3, 0x10,
+		BERLIN_PINCTRL_FUNCTION(0x0, "ss0"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "gpio")),
+	BERLIN_PINCTRL_GROUP("G9", 0x00, 0x3, 0x13,
+		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "spi1"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "twsi0")),
+	BERLIN_PINCTRL_GROUP("G10", 0x00, 0x2, 0x16,
+		BERLIN_PINCTRL_FUNCTION(0x0, "spi1"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "gpio")),
+	BERLIN_PINCTRL_GROUP("G11", 0x00, 0x2, 0x18,
+		BERLIN_PINCTRL_FUNCTION(0x0, "spi1"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "gpio")),
+	BERLIN_PINCTRL_GROUP("G12", 0x00, 0x3, 0x1a,
+		BERLIN_PINCTRL_FUNCTION(0x0, "usb1"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "gpio")),
+	BERLIN_PINCTRL_GROUP("G13", 0x04, 0x3, 0x00,
+		BERLIN_PINCTRL_FUNCTION(0x0, "nand"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "usb0_dbg"),
+		BERLIN_PINCTRL_FUNCTION(0x2, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G14", 0x04, 0x1, 0x03,
+		BERLIN_PINCTRL_FUNCTION(0x0, "nand"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "gpio")),
+	BERLIN_PINCTRL_GROUP("G15", 0x04, 0x2, 0x04,
+		BERLIN_PINCTRL_FUNCTION(0x0, "jtag"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "gpio")),
+	BERLIN_PINCTRL_GROUP("G16", 0x04, 0x3, 0x06,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("G17", 0x04, 0x3, 0x09,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("G18", 0x04, 0x1, 0x0c,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("G19", 0x04, 0x1, 0x0d,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("G20", 0x04, 0x1, 0x0e,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("G21", 0x04, 0x3, 0x0f,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("G22", 0x04, 0x3, 0x12,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("G23", 0x04, 0x3, 0x15,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("G24", 0x04, 0x2, 0x18,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("G25", 0x04, 0x2, 0x1a,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("G26", 0x04, 0x1, 0x1c,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("G27", 0x04, 0x1, 0x1d,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("G28", 0x04, 0x2, 0x1e,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+};
+
+static const struct berlin_desc_group berlin2cd_sysmgr_pinctrl_groups[] = {
+	/* GSM */
+	BERLIN_PINCTRL_GROUP("GSM0", 0x40, 0x2, 0x00,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("GSM1", 0x40, 0x2, 0x02,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("GSM2", 0x40, 0x2, 0x04,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("GSM3", 0x40, 0x2, 0x06,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("GSM4", 0x40, 0x2, 0x08,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("GSM5", 0x40, 0x2, 0x0a,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("GSM6", 0x40, 0x2, 0x0c,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("GSM7", 0x40, 0x1, 0x0e,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("GSM8", 0x40, 0x1, 0x0f,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("GSM9", 0x40, 0x1, 0x10,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("GSM10", 0x40, 0x1, 0x11,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+	BERLIN_PINCTRL_GROUP("GSM11", 0x40, 0x1, 0x12,
+		BERLIN_PINCTRL_FUNCTION_UNKNOWN),
+};
+
+static const struct berlin_pinctrl_desc berlin2cd_soc_pinctrl_data = {
+	.groups = berlin2cd_soc_pinctrl_groups,
+	.ngroups = ARRAY_SIZE(berlin2cd_soc_pinctrl_groups),
+};
+
+static const struct berlin_pinctrl_desc berlin2cd_sysmgr_pinctrl_data = {
+	.groups = berlin2cd_sysmgr_pinctrl_groups,
+	.ngroups = ARRAY_SIZE(berlin2cd_sysmgr_pinctrl_groups),
+};
+
+static const struct of_device_id berlin2cd_pinctrl_match[] = {
+	{
+		.compatible = "marvell,berlin2cd-chip-ctrl",
+		.data = &berlin2cd_soc_pinctrl_data
+	},
+	{
+		.compatible = "marvell,berlin2cd-system-ctrl",
+		.data = &berlin2cd_sysmgr_pinctrl_data
+	},
+	{}
+};
+MODULE_DEVICE_TABLE(of, berlin2cd_pinctrl_match);
+
+static int berlin2cd_pinctrl_probe(struct platform_device *pdev)
+{
+	const struct of_device_id *match =
+		of_match_device(berlin2cd_pinctrl_match, &pdev->dev);
+	struct regmap_config *rmconfig;
+	struct regmap *regmap;
+	struct resource *res;
+	void __iomem *base;
+
+	rmconfig = devm_kzalloc(&pdev->dev, sizeof(*rmconfig), GFP_KERNEL);
+	if (!rmconfig)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	rmconfig->reg_bits = 32,
+	rmconfig->val_bits = 32,
+	rmconfig->reg_stride = 4,
+	rmconfig->max_register = resource_size(res);
+
+	regmap = devm_regmap_init_mmio(&pdev->dev, base, rmconfig);
+	if (IS_ERR(regmap))
+		return PTR_ERR(regmap);
+
+	return berlin_pinctrl_probe(pdev, match->data);
+}
+
+static struct platform_driver berlin2cd_pinctrl_driver = {
+	.probe	= berlin2cd_pinctrl_probe,
+	.driver	= {
+		.name = "berlin-bg2cd-pinctrl",
+		.owner = THIS_MODULE,
+		.of_match_table = berlin2cd_pinctrl_match,
+	},
+};
+module_platform_driver(berlin2cd_pinctrl_driver);
+
+MODULE_AUTHOR("Antoine Ténart <antoine.tenart@free-electrons.com>");
+MODULE_DESCRIPTION("Marvell Berlin BG2CD pinctrl driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/pinctrl/berlin/berlin-bg2q.c b/drivers/pinctrl/berlin/berlin-bg2q.c
new file mode 100644
index 00000000000..9fcf9836045
--- /dev/null
+++ b/drivers/pinctrl/berlin/berlin-bg2q.c
@@ -0,0 +1,436 @@
+/*
+ * Marvell Berlin BG2Q pinctrl driver
+ *
+ * Copyright (C) 2014 Marvell Technology Group Ltd.
+ *
+ * Antoine Ténart <antoine.tenart@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.
+ */
+
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+
+#include "berlin.h"
+
+static const struct berlin_desc_group berlin2q_soc_pinctrl_groups[] = {
+	/* G */
+	BERLIN_PINCTRL_GROUP("G0", 0x18, 0x3, 0x00,
+			BERLIN_PINCTRL_FUNCTION(0x0, "nand"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "mmc"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio")),
+	BERLIN_PINCTRL_GROUP("G1", 0x18, 0x3, 0x03,
+			BERLIN_PINCTRL_FUNCTION(0x0, "nand"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio")),
+	BERLIN_PINCTRL_GROUP("G2", 0x18, 0x3, 0x06,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "arc"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "lvds")),
+	BERLIN_PINCTRL_GROUP("G3", 0x18, 0x3, 0x09,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "i2s2"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "lvds")),
+	BERLIN_PINCTRL_GROUP("G4", 0x18, 0x3, 0x0c,
+			BERLIN_PINCTRL_FUNCTION(0x0, "pll"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "sd0"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "rgmii"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x5, "sata_dbg"),
+			BERLIN_PINCTRL_FUNCTION(0x6, "usb0_dbg"),
+			BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G5", 0x18, 0x3, 0x0f,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "sd0"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "rgmii"),
+			BERLIN_PINCTRL_FUNCTION(0x5, "sata_dbg"),
+			BERLIN_PINCTRL_FUNCTION(0x6, "usb0_dbg"),
+			BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G6", 0x18, 0x3, 0x12,
+			BERLIN_PINCTRL_FUNCTION(0x0, "jtag"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "twsi0"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio")),
+	BERLIN_PINCTRL_GROUP("G7", 0x18, 0x3, 0x15,
+			BERLIN_PINCTRL_FUNCTION(0x0, "jtag"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "twsi1"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "eddc")),
+	BERLIN_PINCTRL_GROUP("G8", 0x18, 0x3, 0x18,
+			BERLIN_PINCTRL_FUNCTION(0x0, "spi1"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "gpio")),
+	BERLIN_PINCTRL_GROUP("G9", 0x18, 0x3, 0x1b,
+			BERLIN_PINCTRL_FUNCTION(0x0, "spi1"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x5, "sata")),
+	BERLIN_PINCTRL_GROUP("G10", 0x1c, 0x3, 0x00,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "spi1"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "i2s0"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "pwm"),
+			BERLIN_PINCTRL_FUNCTION(0x5, "sata")),
+	BERLIN_PINCTRL_GROUP("G11", 0x1c, 0x3, 0x03,
+			BERLIN_PINCTRL_FUNCTION(0x0, "jtag"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "spi1"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "i2s1"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "pwm"),
+			BERLIN_PINCTRL_FUNCTION(0x5, "sata")),
+	BERLIN_PINCTRL_GROUP("G12", 0x1c, 0x3, 0x06,
+			BERLIN_PINCTRL_FUNCTION(0x0, "agc"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "gpio")),
+	BERLIN_PINCTRL_GROUP("G13", 0x1c, 0x3, 0x09,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "sts1"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "rgmii"),
+			BERLIN_PINCTRL_FUNCTION(0x6, "usb0_dbg"),
+			BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G14", 0x1c, 0x3, 0x0c,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "sts0"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "sts1"),
+			BERLIN_PINCTRL_FUNCTION(0x6, "usb0_dbg"),
+			BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G15", 0x1c, 0x3, 0x0f,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "sts0"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "sts1"),
+			BERLIN_PINCTRL_FUNCTION(0x5, "vdac"),
+			BERLIN_PINCTRL_FUNCTION(0x6, "usb0_dbg"),
+			BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G16", 0x1c, 0x3, 0x12,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "sts0"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "sts1"),
+			BERLIN_PINCTRL_FUNCTION(0x5, "osco"),
+			BERLIN_PINCTRL_FUNCTION(0x6, "usb0_dbg"),
+			BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G17", 0x1c, 0x3, 0x15,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "rgmii"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "spdif"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "sts1"),
+			BERLIN_PINCTRL_FUNCTION(0x6, "usb0_dbg"),
+			BERLIN_PINCTRL_FUNCTION(0x7, "usb1_dbg")),
+	BERLIN_PINCTRL_GROUP("G18", 0x1c, 0x3, 0x18,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "rgmii"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "i2s2"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "sts1")),
+	BERLIN_PINCTRL_GROUP("G19", 0x1c, 0x3, 0x1b,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "rgmii"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "i2s3"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "sts1"),
+			BERLIN_PINCTRL_FUNCTION(0x5, "osco")),
+	BERLIN_PINCTRL_GROUP("G20", 0x20, 0x3, 0x00,
+			BERLIN_PINCTRL_FUNCTION(0x0, "cam"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "demod"),
+			/*
+			 * Mode 0x4 mux usb2_dbg *and* usb3_dbg:
+			 * add two functions so it can be used with other groups
+			 * within the same subnode in the device tree
+			 */
+			BERLIN_PINCTRL_FUNCTION(0x4, "usb2_dbg"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "usb3_dbg")),
+	BERLIN_PINCTRL_GROUP("G21", 0x20, 0x3, 0x03,
+			BERLIN_PINCTRL_FUNCTION(0x0, "cam"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "sts2"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "demod")),
+	BERLIN_PINCTRL_GROUP("G22", 0x20, 0x3, 0x06,
+			BERLIN_PINCTRL_FUNCTION(0x0, "cam"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio")),
+	BERLIN_PINCTRL_GROUP("G23", 0x20, 0x3, 0x09,
+			BERLIN_PINCTRL_FUNCTION(0x0, "cam"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "avif"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "usb2_dbg")),
+	BERLIN_PINCTRL_GROUP("G24", 0x20, 0x3, 0x0c,
+			BERLIN_PINCTRL_FUNCTION(0x0, "cam"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "demod"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "usb2_dbg")),
+	BERLIN_PINCTRL_GROUP("G25", 0x20, 0x3, 0x0f,
+			BERLIN_PINCTRL_FUNCTION(0x0, "cam"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "vga"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "avif"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "usb2_dbg")),
+	BERLIN_PINCTRL_GROUP("G26", 0x20, 0x3, 0x12,
+			BERLIN_PINCTRL_FUNCTION(0x0, "cam"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "lvds"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio")),
+	BERLIN_PINCTRL_GROUP("G27", 0x20, 0x3, 0x15,
+			BERLIN_PINCTRL_FUNCTION(0x0, "cam"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "agc"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio")),
+	BERLIN_PINCTRL_GROUP("G28", 0x20, 0x3, 0x18,
+			BERLIN_PINCTRL_FUNCTION(0x0, "cam"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "avif"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "usb2_dbg")),
+	BERLIN_PINCTRL_GROUP("G29", 0x20, 0x3, 0x1b,
+			BERLIN_PINCTRL_FUNCTION(0x0, "cam"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "scrd0"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio")),
+	BERLIN_PINCTRL_GROUP("G30", 0x24, 0x3, 0x00,
+			BERLIN_PINCTRL_FUNCTION(0x0, "cam"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "scrd1"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio")),
+	BERLIN_PINCTRL_GROUP("G31", 0x24, 0x3, 0x03,
+			BERLIN_PINCTRL_FUNCTION(0x0, "cam"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "sd1"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio")),
+	BERLIN_PINCTRL_GROUP("G32", 0x24, 0x3, 0x06,
+			BERLIN_PINCTRL_FUNCTION(0x0, "cam"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "sd1"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio")),
+	/* GAV */
+	BERLIN_PINCTRL_GROUP("GAV0", 0x24, 0x3, 0x09,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "dvio"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "lvds")),
+	BERLIN_PINCTRL_GROUP("GAV1", 0x24, 0x3, 0x0c,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "dvio"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "vga")),
+	BERLIN_PINCTRL_GROUP("GAV2", 0x24, 0x3, 0x0f,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "dvio"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "i2s3"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "pdm"),
+			BERLIN_PINCTRL_FUNCTION(0x6, "adac")),
+	BERLIN_PINCTRL_GROUP("GAV3", 0x24, 0x3, 0x12,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "dvio"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "i2s3"),
+			BERLIN_PINCTRL_FUNCTION(0x6, "adac")),
+	BERLIN_PINCTRL_GROUP("GAV4", 0x24, 0x3, 0x15,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "dvio"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "i2s1"),
+			BERLIN_PINCTRL_FUNCTION(0x6, "adac")),
+	BERLIN_PINCTRL_GROUP("GAV5", 0x24, 0x3, 0x18,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "dvio"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "spdif")),
+	BERLIN_PINCTRL_GROUP("GAV6", 0x24, 0x3, 0x1b,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "dvio"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "i2s2")),
+	BERLIN_PINCTRL_GROUP("GAV7", 0x28, 0x3, 0x00,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "dvio"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "i2s3")),
+	BERLIN_PINCTRL_GROUP("GAV8", 0x28, 0x3, 0x03,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "dv0"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "pwm")),
+	BERLIN_PINCTRL_GROUP("GAV9", 0x28, 0x3, 0x06,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "dv0"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "pwm")),
+	BERLIN_PINCTRL_GROUP("GAV10", 0x28, 0x3, 0x09,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "dv0"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "agc")),
+	BERLIN_PINCTRL_GROUP("GAV11", 0x28, 0x3, 0x0c,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "dv0"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "fp"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "i2s0"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "pwm"),
+			BERLIN_PINCTRL_FUNCTION(0x5, "vclki")),
+	BERLIN_PINCTRL_GROUP("GAV12", 0x28, 0x3, 0x0f,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "i2s2"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "i2s1")),
+	BERLIN_PINCTRL_GROUP("GAV13", 0x28, 0x3, 0x12,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "i2s2")),
+	BERLIN_PINCTRL_GROUP("GAV14", 0x28, 0x3, 0x15,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "i2s2"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "i2s1")),
+	BERLIN_PINCTRL_GROUP("GAV15", 0x28, 0x3, 0x18,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "i2s1"),
+			BERLIN_PINCTRL_FUNCTION(0x6, "dac_dbg")),
+	BERLIN_PINCTRL_GROUP("GAV16", 0x28, 0x3, 0x1b,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "i2s0"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "i2s1"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "i2s3"),
+			BERLIN_PINCTRL_FUNCTION(0x5, "pdm"),
+			BERLIN_PINCTRL_FUNCTION(0x6, "dac_dbg")),
+	BERLIN_PINCTRL_GROUP("GAV17", 0x2c, 0x3, 0x00,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "i2s0"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "i2s1"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "pwm"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "i2s3"),
+			BERLIN_PINCTRL_FUNCTION(0x5, "pdm"),
+			BERLIN_PINCTRL_FUNCTION(0x6, "dac_dbg")),
+	BERLIN_PINCTRL_GROUP("GAV18", 0x2c, 0x3, 0x03,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "spdif"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "arc")),
+	BERLIN_PINCTRL_GROUP("GAV19", 0x2c, 0x3, 0x06,
+			BERLIN_PINCTRL_FUNCTION(0x0, "avio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "spdif"),
+			BERLIN_PINCTRL_FUNCTION(0x4, "i2s3"),
+			BERLIN_PINCTRL_FUNCTION(0x5, "pdm")),
+};
+
+static const struct berlin_desc_group berlin2q_sysmgr_pinctrl_groups[] = {
+	/* GSM */
+	BERLIN_PINCTRL_GROUP("GSM0", 0x40, 0x2, 0x00,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "spi2"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "eth1")),
+	BERLIN_PINCTRL_GROUP("GSM1", 0x40, 0x2, 0x02,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "spi2"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "eth1")),
+	BERLIN_PINCTRL_GROUP("GSM2", 0x40, 0x2, 0x04,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "spi2"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "eddc")),
+	BERLIN_PINCTRL_GROUP("GSM3", 0x40, 0x2, 0x06,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "spi2"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "eddc")),
+	BERLIN_PINCTRL_GROUP("GSM4", 0x40, 0x1, 0x08,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "hdmi")),
+	BERLIN_PINCTRL_GROUP("GSM5", 0x40, 0x1, 0x09,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "hdmi")),
+	BERLIN_PINCTRL_GROUP("GSM6", 0x40, 0x1, 0x0a,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "hdmi")),
+	BERLIN_PINCTRL_GROUP("GSM7", 0x40, 0x1, 0x0b,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "hdmi")),
+	BERLIN_PINCTRL_GROUP("GSM8", 0x40, 0x1, 0x0c,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "hdmi")),
+	BERLIN_PINCTRL_GROUP("GSM9", 0x40, 0x1, 0x0d,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "hdmi")),
+	BERLIN_PINCTRL_GROUP("GSM10", 0x40, 0x1, 0x0e,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "led")),
+	BERLIN_PINCTRL_GROUP("GSM11", 0x40, 0x1, 0x0f,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "led")),
+	BERLIN_PINCTRL_GROUP("GSM12", 0x40, 0x2, 0x10,
+			BERLIN_PINCTRL_FUNCTION(0x0, "uart0"), /* RX/TX */
+			BERLIN_PINCTRL_FUNCTION(0x1, "irda0"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio")),
+	BERLIN_PINCTRL_GROUP("GSM13", 0x40, 0x2, 0x12,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "uart0"), /* CTS/RTS */
+			BERLIN_PINCTRL_FUNCTION(0x2, "uart1"), /* RX/TX */
+			BERLIN_PINCTRL_FUNCTION(0x3, "twsi2")),
+	BERLIN_PINCTRL_GROUP("GSM14", 0x40, 0x2, 0x14,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "uart1"), /* RX/TX */
+			BERLIN_PINCTRL_FUNCTION(0x2, "irda1"),
+			BERLIN_PINCTRL_FUNCTION(0x3, "twsi3")),
+	BERLIN_PINCTRL_GROUP("GSM15", 0x40, 0x2, 0x16,
+			BERLIN_PINCTRL_FUNCTION(0x0, "pwr"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "led"),
+			BERLIN_PINCTRL_FUNCTION(0x2, "gpio")),
+	BERLIN_PINCTRL_GROUP("GSM16", 0x40, 0x1, 0x18,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "eddc")),
+	BERLIN_PINCTRL_GROUP("GSM17", 0x40, 0x1, 0x19,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "eddc")),
+	BERLIN_PINCTRL_GROUP("GSM18", 0x40, 0x1, 0x1a,
+			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "eddc")),
+};
+
+static const struct berlin_pinctrl_desc berlin2q_soc_pinctrl_data = {
+	.groups = berlin2q_soc_pinctrl_groups,
+	.ngroups = ARRAY_SIZE(berlin2q_soc_pinctrl_groups),
+};
+
+static const struct berlin_pinctrl_desc berlin2q_sysmgr_pinctrl_data = {
+	.groups = berlin2q_sysmgr_pinctrl_groups,
+	.ngroups = ARRAY_SIZE(berlin2q_sysmgr_pinctrl_groups),
+};
+
+static const struct of_device_id berlin2q_pinctrl_match[] = {
+	{
+		.compatible = "marvell,berlin2q-chip-ctrl",
+		.data = &berlin2q_soc_pinctrl_data,
+	},
+	{
+		.compatible = "marvell,berlin2q-system-ctrl",
+		.data = &berlin2q_sysmgr_pinctrl_data,
+	},
+	{}
+};
+MODULE_DEVICE_TABLE(of, berlin2q_pinctrl_match);
+
+static int berlin2q_pinctrl_probe(struct platform_device *pdev)
+{
+	const struct of_device_id *match =
+		of_match_device(berlin2q_pinctrl_match, &pdev->dev);
+	struct regmap_config *rmconfig;
+	struct regmap *regmap;
+	struct resource *res;
+	void __iomem *base;
+
+	rmconfig = devm_kzalloc(&pdev->dev, sizeof(*rmconfig), GFP_KERNEL);
+	if (!rmconfig)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	rmconfig->reg_bits = 32,
+	rmconfig->val_bits = 32,
+	rmconfig->reg_stride = 4,
+	rmconfig->max_register = resource_size(res);
+
+	regmap = devm_regmap_init_mmio(&pdev->dev, base, rmconfig);
+	if (IS_ERR(regmap))
+		return PTR_ERR(regmap);
+
+	return berlin_pinctrl_probe(pdev, match->data);
+}
+
+static struct platform_driver berlin2q_pinctrl_driver = {
+	.probe	= berlin2q_pinctrl_probe,
+	.driver	= {
+		.name = "berlin-bg2q-pinctrl",
+		.owner = THIS_MODULE,
+		.of_match_table = berlin2q_pinctrl_match,
+	},
+};
+module_platform_driver(berlin2q_pinctrl_driver);
+
+MODULE_AUTHOR("Antoine Ténart <antoine.tenart@free-electrons.com>");
+MODULE_DESCRIPTION("Marvell Berlin BG2Q pinctrl driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/pinctrl/berlin/berlin.c b/drivers/pinctrl/berlin/berlin.c
new file mode 100644
index 00000000000..edf5d2fd2b2
--- /dev/null
+++ b/drivers/pinctrl/berlin/berlin.c
@@ -0,0 +1,348 @@
+/*
+ * Marvell Berlin SoC pinctrl core driver
+ *
+ * Copyright (C) 2014 Marvell Technology Group Ltd.
+ *
+ * Antoine Ténart <antoine.tenart@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.
+ */
+
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/pinctrl/pinmux.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+
+#include "../core.h"
+#include "../pinctrl-utils.h"
+#include "berlin.h"
+
+struct berlin_pinctrl {
+	struct regmap *regmap;
+	struct device *dev;
+	const struct berlin_pinctrl_desc *desc;
+	struct berlin_pinctrl_function *functions;
+	unsigned nfunctions;
+	struct pinctrl_dev *pctrl_dev;
+};
+
+static int berlin_pinctrl_get_group_count(struct pinctrl_dev *pctrl_dev)
+{
+	struct berlin_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctrl_dev);
+
+	return pctrl->desc->ngroups;
+}
+
+static const char *berlin_pinctrl_get_group_name(struct pinctrl_dev *pctrl_dev,
+						 unsigned group)
+{
+	struct berlin_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctrl_dev);
+
+	return pctrl->desc->groups[group].name;
+}
+
+static int berlin_pinctrl_dt_node_to_map(struct pinctrl_dev *pctrl_dev,
+					 struct device_node *node,
+					 struct pinctrl_map **map,
+					 unsigned *num_maps)
+{
+	struct berlin_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctrl_dev);
+	struct property *prop;
+	const char *function_name, *group_name;
+	unsigned reserved_maps = 0;
+	int ret, ngroups;
+
+	*map = NULL;
+	*num_maps = 0;
+
+	ret = of_property_read_string(node, "function", &function_name);
+	if (ret) {
+		dev_err(pctrl->dev,
+			"missing function property in node %s\n",
+			node->name);
+		return -EINVAL;
+	}
+
+	ngroups = of_property_count_strings(node, "groups");
+	if (ngroups < 0) {
+		dev_err(pctrl->dev,
+			"missing groups property in node %s\n",
+			node->name);
+		return -EINVAL;
+	}
+
+	ret = pinctrl_utils_reserve_map(pctrl_dev, map, &reserved_maps,
+					num_maps, ngroups);
+	if (ret) {
+		dev_err(pctrl->dev, "can't reserve map: %d\n", ret);
+		return ret;
+	}
+
+	of_property_for_each_string(node, "groups", prop, group_name) {
+		ret = pinctrl_utils_add_map_mux(pctrl_dev, map, &reserved_maps,
+						num_maps, group_name,
+						function_name);
+		if (ret) {
+			dev_err(pctrl->dev, "can't add map: %d\n", ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static void berlin_pinctrl_dt_free_map(struct pinctrl_dev *pctrl_dev,
+				       struct pinctrl_map *map,
+				       unsigned nmaps)
+{
+	int i;
+
+	for (i = 0; i < nmaps; i++) {
+		if (map[i].type == PIN_MAP_TYPE_MUX_GROUP) {
+			kfree(map[i].data.mux.group);
+
+			/* a function can be applied to multiple groups */
+			if (i == 0)
+				kfree(map[i].data.mux.function);
+		}
+	}
+
+	kfree(map);
+}
+
+static const struct pinctrl_ops berlin_pinctrl_ops = {
+	.get_groups_count	= &berlin_pinctrl_get_group_count,
+	.get_group_name		= &berlin_pinctrl_get_group_name,
+	.dt_node_to_map		= &berlin_pinctrl_dt_node_to_map,
+	.dt_free_map		= &berlin_pinctrl_dt_free_map,
+};
+
+static int berlin_pinmux_get_functions_count(struct pinctrl_dev *pctrl_dev)
+{
+	struct berlin_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctrl_dev);
+
+	return pctrl->nfunctions;
+}
+
+static const char *berlin_pinmux_get_function_name(struct pinctrl_dev *pctrl_dev,
+						   unsigned function)
+{
+	struct berlin_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctrl_dev);
+
+	return pctrl->functions[function].name;
+}
+
+static int berlin_pinmux_get_function_groups(struct pinctrl_dev *pctrl_dev,
+					     unsigned function,
+					     const char * const **groups,
+					     unsigned * const num_groups)
+{
+	struct berlin_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctrl_dev);
+
+	*groups = pctrl->functions[function].groups;
+	*num_groups = pctrl->functions[function].ngroups;
+
+	return 0;
+}
+
+static struct berlin_desc_function *
+berlin_pinctrl_find_function_by_name(struct berlin_pinctrl *pctrl,
+				     const struct berlin_desc_group *group,
+				     const char *fname)
+{
+	struct berlin_desc_function *function = group->functions;
+
+	while (function->name) {
+		if (!strcmp(function->name, fname))
+			return function;
+
+		function++;
+	}
+
+	return NULL;
+}
+
+static int berlin_pinmux_enable(struct pinctrl_dev *pctrl_dev,
+				unsigned function,
+				unsigned group)
+{
+	struct berlin_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctrl_dev);
+	const struct berlin_desc_group *group_desc = pctrl->desc->groups + group;
+	struct berlin_pinctrl_function *func = pctrl->functions + function;
+	struct berlin_desc_function *function_desc =
+		berlin_pinctrl_find_function_by_name(pctrl, group_desc,
+						     func->name);
+	u32 mask, val;
+
+	if (!function_desc)
+		return -EINVAL;
+
+	mask = GENMASK(group_desc->lsb + group_desc->bit_width - 1,
+		       group_desc->lsb);
+	val = function_desc->muxval << group_desc->lsb;
+	regmap_update_bits(pctrl->regmap, group_desc->offset, mask, val);
+
+	return 0;
+}
+
+static const struct pinmux_ops berlin_pinmux_ops = {
+	.get_functions_count	= &berlin_pinmux_get_functions_count,
+	.get_function_name	= &berlin_pinmux_get_function_name,
+	.get_function_groups	= &berlin_pinmux_get_function_groups,
+	.enable			= &berlin_pinmux_enable,
+};
+
+static int berlin_pinctrl_add_function(struct berlin_pinctrl *pctrl,
+				       const char *name)
+{
+	struct berlin_pinctrl_function *function = pctrl->functions;
+
+	while (function->name) {
+		if (!strcmp(function->name, name)) {
+			function->ngroups++;
+			return -EEXIST;
+		}
+		function++;
+	}
+
+	function->name = name;
+	function->ngroups = 1;
+
+	pctrl->nfunctions++;
+
+	return 0;
+}
+
+static int berlin_pinctrl_build_state(struct platform_device *pdev)
+{
+	struct berlin_pinctrl *pctrl = platform_get_drvdata(pdev);
+	struct berlin_desc_group const *desc_group;
+	struct berlin_desc_function const *desc_function;
+	int i, max_functions = 0;
+
+	pctrl->nfunctions = 0;
+
+	for (i = 0; i < pctrl->desc->ngroups; i++) {
+		desc_group = pctrl->desc->groups + i;
+		/* compute the maxiumum number of functions a group can have */
+		max_functions += 1 << (desc_group->bit_width + 1);
+	}
+
+	/* we will reallocate later */
+	pctrl->functions = devm_kzalloc(&pdev->dev,
+					max_functions * sizeof(*pctrl->functions),
+					GFP_KERNEL);
+	if (!pctrl->functions)
+		return -ENOMEM;
+
+	/* register all functions */
+	for (i = 0; i < pctrl->desc->ngroups; i++) {
+		desc_group = pctrl->desc->groups + i;
+		desc_function = desc_group->functions;
+
+		while (desc_function->name) {
+			berlin_pinctrl_add_function(pctrl, desc_function->name);
+			desc_function++;
+		}
+	}
+
+	pctrl->functions = krealloc(pctrl->functions,
+				    pctrl->nfunctions * sizeof(*pctrl->functions),
+				    GFP_KERNEL);
+
+	/* map functions to theirs groups */
+	for (i = 0; i < pctrl->desc->ngroups; i++) {
+		desc_group = pctrl->desc->groups + i;
+		desc_function = desc_group->functions;
+
+		while (desc_function->name) {
+			struct berlin_pinctrl_function
+				*function = pctrl->functions;
+			const char **groups;
+			bool found = false;
+
+			while (function->name) {
+				if (!strcmp(desc_function->name, function->name)) {
+					found = true;
+					break;
+				}
+				function++;
+			}
+
+			if (!found)
+				return -EINVAL;
+
+			if (!function->groups) {
+				function->groups =
+					devm_kzalloc(&pdev->dev,
+						     function->ngroups * sizeof(char *),
+						     GFP_KERNEL);
+
+				if (!function->groups)
+					return -ENOMEM;
+			}
+
+			groups = function->groups;
+			while (*groups)
+				groups++;
+
+			*groups = desc_group->name;
+
+			desc_function++;
+		}
+	}
+
+	return 0;
+}
+
+static struct pinctrl_desc berlin_pctrl_desc = {
+	.name		= "berlin-pinctrl",
+	.pctlops	= &berlin_pinctrl_ops,
+	.pmxops		= &berlin_pinmux_ops,
+	.owner		= THIS_MODULE,
+};
+
+int berlin_pinctrl_probe(struct platform_device *pdev,
+			 const struct berlin_pinctrl_desc *desc)
+{
+	struct device *dev = &pdev->dev;
+	struct berlin_pinctrl *pctrl;
+	struct regmap *regmap;
+	int ret;
+
+	regmap = dev_get_regmap(&pdev->dev, NULL);
+	if (!regmap)
+		return PTR_ERR(regmap);
+
+	pctrl = devm_kzalloc(dev, sizeof(*pctrl), GFP_KERNEL);
+	if (!pctrl)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, pctrl);
+
+	pctrl->regmap = regmap;
+	pctrl->dev = &pdev->dev;
+	pctrl->desc = desc;
+
+	ret = berlin_pinctrl_build_state(pdev);
+	if (ret) {
+		dev_err(dev, "cannot build driver state: %d\n", ret);
+		return ret;
+	}
+
+	pctrl->pctrl_dev = pinctrl_register(&berlin_pctrl_desc, dev, pctrl);
+	if (!pctrl->pctrl_dev) {
+		dev_err(dev, "failed to register pinctrl driver\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
diff --git a/drivers/pinctrl/berlin/berlin.h b/drivers/pinctrl/berlin/berlin.h
new file mode 100644
index 00000000000..e1aa8414519
--- /dev/null
+++ b/drivers/pinctrl/berlin/berlin.h
@@ -0,0 +1,61 @@
+/*
+ * Marvell Berlin SoC pinctrl driver.
+ *
+ * Copyright (C) 2014 Marvell Technology Group Ltd.
+ *
+ * Antoine Ténart <antoine.tenart@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 __PINCTRL_BERLIN_H
+#define __PINCTRL_BERLIN_H
+
+struct berlin_desc_function {
+	const char	*name;
+	u8		muxval;
+};
+
+struct berlin_desc_group {
+	const char			*name;
+	u8				offset;
+	u8				bit_width;
+	u8				lsb;
+	struct berlin_desc_function	*functions;
+};
+
+struct berlin_pinctrl_desc {
+	const struct berlin_desc_group	*groups;
+	unsigned			ngroups;
+};
+
+struct berlin_pinctrl_function {
+	const char	*name;
+	const char	**groups;
+	unsigned	ngroups;
+};
+
+#define BERLIN_PINCTRL_GROUP(_name, _offset, _width, _lsb, ...)		\
+	{								\
+		.name = _name,						\
+		.offset = _offset,					\
+		.bit_width = _width,					\
+		.lsb = _lsb,						\
+		.functions = (struct berlin_desc_function[]){		\
+			__VA_ARGS__, { } },				\
+	}
+
+#define BERLIN_PINCTRL_FUNCTION(_muxval, _name)		\
+	{						\
+		.name = _name,				\
+		.muxval = _muxval,			\
+	}
+
+#define BERLIN_PINCTRL_FUNCTION_UNKNOWN		{}
+
+int berlin_pinctrl_probe(struct platform_device *pdev,
+			 const struct berlin_pinctrl_desc *desc);
+
+#endif /* __PINCTRL_BERLIN_H */
diff --git a/drivers/pinctrl/core.c b/drivers/pinctrl/core.c
index c0fe6091566..e09474ecde2 100644
--- a/drivers/pinctrl/core.c
+++ b/drivers/pinctrl/core.c
@@ -468,6 +468,9 @@ int pinctrl_get_group_pins(struct pinctrl_dev *pctldev, const char *pin_group,
 	const struct pinctrl_ops *pctlops = pctldev->desc->pctlops;
 	int gs;
 
+	if (!pctlops->get_group_pins)
+		return -EINVAL;
+
 	gs = pinctrl_get_group_selector(pctldev, pin_group);
 	if (gs < 0)
 		return gs;
@@ -1362,15 +1365,16 @@ static int pinctrl_groups_show(struct seq_file *s, void *what)
 
 	seq_puts(s, "registered pin groups:\n");
 	while (selector < ngroups) {
-		const unsigned *pins;
-		unsigned num_pins;
+		const unsigned *pins = NULL;
+		unsigned num_pins = 0;
 		const char *gname = ops->get_group_name(pctldev, selector);
 		const char *pname;
-		int ret;
+		int ret = 0;
 		int i;
 
-		ret = ops->get_group_pins(pctldev, selector,
-					  &pins, &num_pins);
+		if (ops->get_group_pins)
+			ret = ops->get_group_pins(pctldev, selector,
+						  &pins, &num_pins);
 		if (ret)
 			seq_printf(s, "%s [ERROR GETTING PINS]\n",
 				   gname);
@@ -1694,8 +1698,7 @@ static int pinctrl_check_ops(struct pinctrl_dev *pctldev)
 
 	if (!ops ||
 	    !ops->get_groups_count ||
-	    !ops->get_group_name ||
-	    !ops->get_group_pins)
+	    !ops->get_group_name)
 		return -EINVAL;
 
 	if (ops->dt_node_to_map && !ops->dt_free_map)
diff --git a/drivers/pinctrl/mvebu/Kconfig b/drivers/pinctrl/mvebu/Kconfig
index cc298fade93..d6dd8358a6f 100644
--- a/drivers/pinctrl/mvebu/Kconfig
+++ b/drivers/pinctrl/mvebu/Kconfig
@@ -30,4 +30,8 @@ config PINCTRL_ARMADA_XP
 	bool
 	select PINCTRL_MVEBU
 
+config PINCTRL_ORION
+	bool
+	select PINCTRL_MVEBU
+
 endif
diff --git a/drivers/pinctrl/mvebu/Makefile b/drivers/pinctrl/mvebu/Makefile
index bc1b9f14f53..a0818e96374 100644
--- a/drivers/pinctrl/mvebu/Makefile
+++ b/drivers/pinctrl/mvebu/Makefile
@@ -5,3 +5,4 @@ obj-$(CONFIG_PINCTRL_ARMADA_370) += pinctrl-armada-370.o
 obj-$(CONFIG_PINCTRL_ARMADA_375) += pinctrl-armada-375.o
 obj-$(CONFIG_PINCTRL_ARMADA_38X) += pinctrl-armada-38x.o
 obj-$(CONFIG_PINCTRL_ARMADA_XP)  += pinctrl-armada-xp.o
+obj-$(CONFIG_PINCTRL_ORION)  += pinctrl-orion.o
diff --git a/drivers/pinctrl/mvebu/pinctrl-orion.c b/drivers/pinctrl/mvebu/pinctrl-orion.c
new file mode 100644
index 00000000000..dda1e7254e1
--- /dev/null
+++ b/drivers/pinctrl/mvebu/pinctrl-orion.c
@@ -0,0 +1,261 @@
+/*
+ * Marvell Orion pinctrl driver based on mvebu pinctrl core
+ *
+ * Author: Thomas Petazzoni <thomas.petazzoni@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.
+ *
+ * The first 16 MPP pins on Orion are easy to handle: they are
+ * configured through 2 consecutive registers, located at the base
+ * address of the MPP device.
+ *
+ * However the last 4 MPP pins are handled by a register at offset
+ * 0x50 from the base address, so it is not consecutive with the first
+ * two registers.
+ */
+
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-mvebu.h"
+
+static void __iomem *mpp_base;
+static void __iomem *high_mpp_base;
+
+static int orion_mpp_ctrl_get(unsigned pid, unsigned long *config)
+{
+	unsigned shift = (pid % MVEBU_MPPS_PER_REG) * MVEBU_MPP_BITS;
+
+	if (pid < 16) {
+		unsigned off = (pid / MVEBU_MPPS_PER_REG) * MVEBU_MPP_BITS;
+		*config = (readl(mpp_base + off) >> shift) & MVEBU_MPP_MASK;
+	}
+	else {
+		*config = (readl(high_mpp_base) >> shift) & MVEBU_MPP_MASK;
+	}
+
+	return 0;
+}
+
+static int orion_mpp_ctrl_set(unsigned pid, unsigned long config)
+{
+	unsigned shift = (pid % MVEBU_MPPS_PER_REG) * MVEBU_MPP_BITS;
+
+	if (pid < 16) {
+		unsigned off = (pid / MVEBU_MPPS_PER_REG) * MVEBU_MPP_BITS;
+		u32 reg = readl(mpp_base + off) & ~(MVEBU_MPP_MASK << shift);
+		writel(reg | (config << shift), mpp_base + off);
+	}
+	else {
+		u32 reg = readl(high_mpp_base) & ~(MVEBU_MPP_MASK << shift);
+		writel(reg | (config << shift), high_mpp_base);
+	}
+
+	return 0;
+}
+
+#define V(f5181l, f5182, f5281) \
+	((f5181l << 0) | (f5182 << 1) | (f5281 << 2))
+
+enum orion_variant {
+	V_5181L = V(1, 0, 0),
+	V_5182  = V(0, 1, 0),
+	V_5281  = V(0, 0, 1),
+	V_ALL   = V(1, 1, 1),
+};
+
+static struct mvebu_mpp_mode orion_mpp_modes[] = {
+	MPP_MODE(0,
+		 MPP_VAR_FUNCTION(0x0, "pcie", "rstout",    V_ALL),
+		 MPP_VAR_FUNCTION(0x2, "pci", "req2",       V_ALL),
+		 MPP_VAR_FUNCTION(0x3, "gpio", NULL,        V_ALL)),
+	MPP_MODE(1,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x2, "pci", "gnt2",       V_ALL)),
+	MPP_MODE(2,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x2, "pci", "req3",       V_ALL),
+		 MPP_VAR_FUNCTION(0x3, "pci-1", "pme",      V_ALL)),
+	MPP_MODE(3,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x2, "pci", "gnt3",       V_ALL)),
+	MPP_MODE(4,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x2, "pci", "req4",       V_ALL),
+		 MPP_VAR_FUNCTION(0x4, "bootnand", "re",    V_5182 | V_5281),
+		 MPP_VAR_FUNCTION(0x5, "sata0", "prsnt",    V_5182)),
+	MPP_MODE(5,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x2, "pci", "gnt4",       V_ALL),
+		 MPP_VAR_FUNCTION(0x4, "bootnand", "we",    V_5182 | V_5281),
+		 MPP_VAR_FUNCTION(0x5, "sata1", "prsnt",    V_5182)),
+	MPP_MODE(6,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x2, "pci", "req5",       V_ALL),
+		 MPP_VAR_FUNCTION(0x4, "nand", "re0",       V_5182 | V_5281),
+		 MPP_VAR_FUNCTION(0x5, "pci-1", "clk",      V_5181L),
+		 MPP_VAR_FUNCTION(0x5, "sata0", "act",      V_5182)),
+	MPP_MODE(7,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x2, "pci", "gnt5",       V_ALL),
+		 MPP_VAR_FUNCTION(0x4, "nand", "we0",       V_5182 | V_5281),
+		 MPP_VAR_FUNCTION(0x5, "pci-1", "clk",      V_5181L),
+		 MPP_VAR_FUNCTION(0x5, "sata1", "act",      V_5182)),
+	MPP_MODE(8,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x1, "ge", "col",         V_ALL)),
+	MPP_MODE(9,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x1, "ge", "rxerr",       V_ALL)),
+	MPP_MODE(10,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x1, "ge", "crs",         V_ALL)),
+	MPP_MODE(11,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x1, "ge", "txerr",       V_ALL)),
+	MPP_MODE(12,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x1, "ge", "txd4",        V_ALL),
+		 MPP_VAR_FUNCTION(0x4, "nand", "re1",       V_5182 | V_5281),
+		 MPP_VAR_FUNCTION(0x5, "sata0", "ledprsnt", V_5182)),
+	MPP_MODE(13,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x1, "ge", "txd5",        V_ALL),
+		 MPP_VAR_FUNCTION(0x4, "nand", "we1",       V_5182 | V_5281),
+		 MPP_VAR_FUNCTION(0x5, "sata1", "ledprsnt", V_5182)),
+	MPP_MODE(14,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x1, "ge", "txd6",        V_ALL),
+		 MPP_VAR_FUNCTION(0x4, "nand", "re2",       V_5182 | V_5281),
+		 MPP_VAR_FUNCTION(0x5, "sata0", "ledact",   V_5182)),
+	MPP_MODE(15,
+		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_ALL),
+		 MPP_VAR_FUNCTION(0x1, "ge", "txd7",        V_ALL),
+		 MPP_VAR_FUNCTION(0x4, "nand", "we2",       V_5182 | V_5281),
+		 MPP_VAR_FUNCTION(0x5, "sata1", "ledact",   V_5182)),
+	MPP_MODE(16,
+		 MPP_VAR_FUNCTION(0x0, "uart1", "rxd",      V_5182 | V_5281),
+		 MPP_VAR_FUNCTION(0x1, "ge", "rxd4",        V_ALL),
+		 MPP_VAR_FUNCTION(0x5, "gpio", NULL,        V_5182)),
+	MPP_MODE(17,
+		 MPP_VAR_FUNCTION(0x0, "uart1", "txd",      V_5182 | V_5281),
+		 MPP_VAR_FUNCTION(0x1, "ge", "rxd5",        V_ALL),
+		 MPP_VAR_FUNCTION(0x5, "gpio", NULL,        V_5182)),
+	MPP_MODE(18,
+		 MPP_VAR_FUNCTION(0x0, "uart1", "cts",      V_5182 | V_5281),
+		 MPP_VAR_FUNCTION(0x1, "ge", "rxd6",        V_ALL),
+		 MPP_VAR_FUNCTION(0x5, "gpio", NULL,        V_5182)),
+	MPP_MODE(19,
+		 MPP_VAR_FUNCTION(0x0, "uart1", "rts",      V_5182 | V_5281),
+		 MPP_VAR_FUNCTION(0x1, "ge", "rxd7",        V_ALL),
+		 MPP_VAR_FUNCTION(0x5, "gpio", NULL,        V_5182)),
+};
+
+static struct mvebu_mpp_ctrl orion_mpp_controls[] = {
+	MPP_FUNC_CTRL(0, 19, NULL, orion_mpp_ctrl),
+};
+
+static struct pinctrl_gpio_range mv88f5181l_gpio_ranges[] = {
+	MPP_GPIO_RANGE(0, 0, 0, 16),
+};
+
+static struct pinctrl_gpio_range mv88f5182_gpio_ranges[] = {
+	MPP_GPIO_RANGE(0, 0, 0, 19),
+};
+
+static struct pinctrl_gpio_range mv88f5281_gpio_ranges[] = {
+	MPP_GPIO_RANGE(0, 0, 0, 16),
+};
+
+static struct mvebu_pinctrl_soc_info mv88f5181l_info = {
+	.variant = V_5181L,
+	.controls = orion_mpp_controls,
+	.ncontrols = ARRAY_SIZE(orion_mpp_controls),
+	.modes = orion_mpp_modes,
+	.nmodes = ARRAY_SIZE(orion_mpp_modes),
+	.gpioranges = mv88f5181l_gpio_ranges,
+	.ngpioranges = ARRAY_SIZE(mv88f5181l_gpio_ranges),
+};
+
+static struct mvebu_pinctrl_soc_info mv88f5182_info = {
+	.variant = V_5182,
+	.controls = orion_mpp_controls,
+	.ncontrols = ARRAY_SIZE(orion_mpp_controls),
+	.modes = orion_mpp_modes,
+	.nmodes = ARRAY_SIZE(orion_mpp_modes),
+	.gpioranges = mv88f5182_gpio_ranges,
+	.ngpioranges = ARRAY_SIZE(mv88f5182_gpio_ranges),
+};
+
+static struct mvebu_pinctrl_soc_info mv88f5281_info = {
+	.variant = V_5281,
+	.controls = orion_mpp_controls,
+	.ncontrols = ARRAY_SIZE(orion_mpp_controls),
+	.modes = orion_mpp_modes,
+	.nmodes = ARRAY_SIZE(orion_mpp_modes),
+	.gpioranges = mv88f5281_gpio_ranges,
+	.ngpioranges = ARRAY_SIZE(mv88f5281_gpio_ranges),
+};
+
+/*
+ * There are multiple variants of the Orion SoCs, but in terms of pin
+ * muxing, they are identical.
+ */
+static struct of_device_id orion_pinctrl_of_match[] = {
+	{ .compatible = "marvell,88f5181l-pinctrl", .data = &mv88f5181l_info },
+	{ .compatible = "marvell,88f5182-pinctrl", .data = &mv88f5182_info },
+	{ .compatible = "marvell,88f5281-pinctrl", .data = &mv88f5281_info },
+	{ }
+};
+
+static int orion_pinctrl_probe(struct platform_device *pdev)
+{
+	const struct of_device_id *match =
+		of_match_device(orion_pinctrl_of_match, &pdev->dev);
+	struct resource *res;
+
+	pdev->dev.platform_data = (void*)match->data;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	mpp_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(mpp_base))
+		return PTR_ERR(mpp_base);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	high_mpp_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(high_mpp_base))
+		return PTR_ERR(high_mpp_base);
+
+	return mvebu_pinctrl_probe(pdev);
+}
+
+static int orion_pinctrl_remove(struct platform_device *pdev)
+{
+	return mvebu_pinctrl_remove(pdev);
+}
+
+static struct platform_driver orion_pinctrl_driver = {
+	.driver = {
+		.name = "orion-pinctrl",
+		.owner = THIS_MODULE,
+		.of_match_table = of_match_ptr(orion_pinctrl_of_match),
+	},
+	.probe = orion_pinctrl_probe,
+	.remove = orion_pinctrl_remove,
+};
+
+module_platform_driver(orion_pinctrl_driver);
+
+MODULE_AUTHOR("Thomas Petazzoni <thomas.petazzoni@free-electrons.com>");
+MODULE_DESCRIPTION("Marvell Orion pinctrl driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pinctrl/pinconf-generic.c b/drivers/pinctrl/pinconf-generic.c
index 3d9a999fb69..29ff77f90fc 100644
--- a/drivers/pinctrl/pinconf-generic.c
+++ b/drivers/pinctrl/pinconf-generic.c
@@ -166,6 +166,7 @@ static struct pinconf_generic_dt_params dt_params[] = {
 	{ "input-schmitt-enable", PIN_CONFIG_INPUT_SCHMITT_ENABLE, 1 },
 	{ "input-schmitt-disable", PIN_CONFIG_INPUT_SCHMITT_ENABLE, 0 },
 	{ "input-debounce", PIN_CONFIG_INPUT_DEBOUNCE, 0 },
+	{ "power-source", PIN_CONFIG_POWER_SOURCE, 0 },
 	{ "low-power-enable", PIN_CONFIG_LOW_POWER_MODE, 1 },
 	{ "low-power-disable", PIN_CONFIG_LOW_POWER_MODE, 0 },
 	{ "output-low", PIN_CONFIG_OUTPUT, 0, },
@@ -228,13 +229,12 @@ int pinconf_generic_parse_dt_config(struct device_node *np,
 	 * Now limit the number of configs to the real number of
 	 * found properties.
 	 */
-	*configs = kzalloc(ncfg * sizeof(unsigned long), GFP_KERNEL);
+	*configs = kmemdup(cfg, ncfg * sizeof(unsigned long), GFP_KERNEL);
 	if (!*configs) {
 		ret = -ENOMEM;
 		goto out;
 	}
 
-	memcpy(*configs, cfg, ncfg * sizeof(unsigned long));
 	*nconfigs = ncfg;
 
 out:
diff --git a/drivers/pinctrl/pinctrl-adi2.c b/drivers/pinctrl/pinctrl-adi2.c
index 0cc0eec8339..5c44feb54eb 100644
--- a/drivers/pinctrl/pinctrl-adi2.c
+++ b/drivers/pinctrl/pinctrl-adi2.c
@@ -927,7 +927,7 @@ static int adi_gpio_irq_map(struct irq_domain *d, unsigned int irq,
 	return 0;
 }
 
-const struct irq_domain_ops adi_gpio_irq_domain_ops = {
+static const struct irq_domain_ops adi_gpio_irq_domain_ops = {
 	.map = adi_gpio_irq_map,
 	.xlate = irq_domain_xlate_onecell,
 };
diff --git a/drivers/pinctrl/pinctrl-apq8064.c b/drivers/pinctrl/pinctrl-apq8064.c
new file mode 100644
index 00000000000..519f7886b0f
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-apq8064.c
@@ -0,0 +1,613 @@
+/*
+ * Copyright (c) 2014, Sony Mobile Communications AB.
+ *
+ * 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.
+ */
+
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-msm.h"
+
+static const struct pinctrl_pin_desc apq8064_pins[] = {
+	PINCTRL_PIN(0, "GPIO_0"),
+	PINCTRL_PIN(1, "GPIO_1"),
+	PINCTRL_PIN(2, "GPIO_2"),
+	PINCTRL_PIN(3, "GPIO_3"),
+	PINCTRL_PIN(4, "GPIO_4"),
+	PINCTRL_PIN(5, "GPIO_5"),
+	PINCTRL_PIN(6, "GPIO_6"),
+	PINCTRL_PIN(7, "GPIO_7"),
+	PINCTRL_PIN(8, "GPIO_8"),
+	PINCTRL_PIN(9, "GPIO_9"),
+	PINCTRL_PIN(10, "GPIO_10"),
+	PINCTRL_PIN(11, "GPIO_11"),
+	PINCTRL_PIN(12, "GPIO_12"),
+	PINCTRL_PIN(13, "GPIO_13"),
+	PINCTRL_PIN(14, "GPIO_14"),
+	PINCTRL_PIN(15, "GPIO_15"),
+	PINCTRL_PIN(16, "GPIO_16"),
+	PINCTRL_PIN(17, "GPIO_17"),
+	PINCTRL_PIN(18, "GPIO_18"),
+	PINCTRL_PIN(19, "GPIO_19"),
+	PINCTRL_PIN(20, "GPIO_20"),
+	PINCTRL_PIN(21, "GPIO_21"),
+	PINCTRL_PIN(22, "GPIO_22"),
+	PINCTRL_PIN(23, "GPIO_23"),
+	PINCTRL_PIN(24, "GPIO_24"),
+	PINCTRL_PIN(25, "GPIO_25"),
+	PINCTRL_PIN(26, "GPIO_26"),
+	PINCTRL_PIN(27, "GPIO_27"),
+	PINCTRL_PIN(28, "GPIO_28"),
+	PINCTRL_PIN(29, "GPIO_29"),
+	PINCTRL_PIN(30, "GPIO_30"),
+	PINCTRL_PIN(31, "GPIO_31"),
+	PINCTRL_PIN(32, "GPIO_32"),
+	PINCTRL_PIN(33, "GPIO_33"),
+	PINCTRL_PIN(34, "GPIO_34"),
+	PINCTRL_PIN(35, "GPIO_35"),
+	PINCTRL_PIN(36, "GPIO_36"),
+	PINCTRL_PIN(37, "GPIO_37"),
+	PINCTRL_PIN(38, "GPIO_38"),
+	PINCTRL_PIN(39, "GPIO_39"),
+	PINCTRL_PIN(40, "GPIO_40"),
+	PINCTRL_PIN(41, "GPIO_41"),
+	PINCTRL_PIN(42, "GPIO_42"),
+	PINCTRL_PIN(43, "GPIO_43"),
+	PINCTRL_PIN(44, "GPIO_44"),
+	PINCTRL_PIN(45, "GPIO_45"),
+	PINCTRL_PIN(46, "GPIO_46"),
+	PINCTRL_PIN(47, "GPIO_47"),
+	PINCTRL_PIN(48, "GPIO_48"),
+	PINCTRL_PIN(49, "GPIO_49"),
+	PINCTRL_PIN(50, "GPIO_50"),
+	PINCTRL_PIN(51, "GPIO_51"),
+	PINCTRL_PIN(52, "GPIO_52"),
+	PINCTRL_PIN(53, "GPIO_53"),
+	PINCTRL_PIN(54, "GPIO_54"),
+	PINCTRL_PIN(55, "GPIO_55"),
+	PINCTRL_PIN(56, "GPIO_56"),
+	PINCTRL_PIN(57, "GPIO_57"),
+	PINCTRL_PIN(58, "GPIO_58"),
+	PINCTRL_PIN(59, "GPIO_59"),
+	PINCTRL_PIN(60, "GPIO_60"),
+	PINCTRL_PIN(61, "GPIO_61"),
+	PINCTRL_PIN(62, "GPIO_62"),
+	PINCTRL_PIN(63, "GPIO_63"),
+	PINCTRL_PIN(64, "GPIO_64"),
+	PINCTRL_PIN(65, "GPIO_65"),
+	PINCTRL_PIN(66, "GPIO_66"),
+	PINCTRL_PIN(67, "GPIO_67"),
+	PINCTRL_PIN(68, "GPIO_68"),
+	PINCTRL_PIN(69, "GPIO_69"),
+	PINCTRL_PIN(70, "GPIO_70"),
+	PINCTRL_PIN(71, "GPIO_71"),
+	PINCTRL_PIN(72, "GPIO_72"),
+	PINCTRL_PIN(73, "GPIO_73"),
+	PINCTRL_PIN(74, "GPIO_74"),
+	PINCTRL_PIN(75, "GPIO_75"),
+	PINCTRL_PIN(76, "GPIO_76"),
+	PINCTRL_PIN(77, "GPIO_77"),
+	PINCTRL_PIN(78, "GPIO_78"),
+	PINCTRL_PIN(79, "GPIO_79"),
+	PINCTRL_PIN(80, "GPIO_80"),
+	PINCTRL_PIN(81, "GPIO_81"),
+	PINCTRL_PIN(82, "GPIO_82"),
+	PINCTRL_PIN(83, "GPIO_83"),
+	PINCTRL_PIN(84, "GPIO_84"),
+	PINCTRL_PIN(85, "GPIO_85"),
+	PINCTRL_PIN(86, "GPIO_86"),
+	PINCTRL_PIN(87, "GPIO_87"),
+	PINCTRL_PIN(88, "GPIO_88"),
+	PINCTRL_PIN(89, "GPIO_89"),
+
+	PINCTRL_PIN(90, "SDC1_CLK"),
+	PINCTRL_PIN(91, "SDC1_CMD"),
+	PINCTRL_PIN(92, "SDC1_DATA"),
+	PINCTRL_PIN(93, "SDC3_CLK"),
+	PINCTRL_PIN(94, "SDC3_CMD"),
+	PINCTRL_PIN(95, "SDC3_DATA"),
+};
+
+#define DECLARE_APQ_GPIO_PINS(pin) static const unsigned int gpio##pin##_pins[] = { pin }
+DECLARE_APQ_GPIO_PINS(0);
+DECLARE_APQ_GPIO_PINS(1);
+DECLARE_APQ_GPIO_PINS(2);
+DECLARE_APQ_GPIO_PINS(3);
+DECLARE_APQ_GPIO_PINS(4);
+DECLARE_APQ_GPIO_PINS(5);
+DECLARE_APQ_GPIO_PINS(6);
+DECLARE_APQ_GPIO_PINS(7);
+DECLARE_APQ_GPIO_PINS(8);
+DECLARE_APQ_GPIO_PINS(9);
+DECLARE_APQ_GPIO_PINS(10);
+DECLARE_APQ_GPIO_PINS(11);
+DECLARE_APQ_GPIO_PINS(12);
+DECLARE_APQ_GPIO_PINS(13);
+DECLARE_APQ_GPIO_PINS(14);
+DECLARE_APQ_GPIO_PINS(15);
+DECLARE_APQ_GPIO_PINS(16);
+DECLARE_APQ_GPIO_PINS(17);
+DECLARE_APQ_GPIO_PINS(18);
+DECLARE_APQ_GPIO_PINS(19);
+DECLARE_APQ_GPIO_PINS(20);
+DECLARE_APQ_GPIO_PINS(21);
+DECLARE_APQ_GPIO_PINS(22);
+DECLARE_APQ_GPIO_PINS(23);
+DECLARE_APQ_GPIO_PINS(24);
+DECLARE_APQ_GPIO_PINS(25);
+DECLARE_APQ_GPIO_PINS(26);
+DECLARE_APQ_GPIO_PINS(27);
+DECLARE_APQ_GPIO_PINS(28);
+DECLARE_APQ_GPIO_PINS(29);
+DECLARE_APQ_GPIO_PINS(30);
+DECLARE_APQ_GPIO_PINS(31);
+DECLARE_APQ_GPIO_PINS(32);
+DECLARE_APQ_GPIO_PINS(33);
+DECLARE_APQ_GPIO_PINS(34);
+DECLARE_APQ_GPIO_PINS(35);
+DECLARE_APQ_GPIO_PINS(36);
+DECLARE_APQ_GPIO_PINS(37);
+DECLARE_APQ_GPIO_PINS(38);
+DECLARE_APQ_GPIO_PINS(39);
+DECLARE_APQ_GPIO_PINS(40);
+DECLARE_APQ_GPIO_PINS(41);
+DECLARE_APQ_GPIO_PINS(42);
+DECLARE_APQ_GPIO_PINS(43);
+DECLARE_APQ_GPIO_PINS(44);
+DECLARE_APQ_GPIO_PINS(45);
+DECLARE_APQ_GPIO_PINS(46);
+DECLARE_APQ_GPIO_PINS(47);
+DECLARE_APQ_GPIO_PINS(48);
+DECLARE_APQ_GPIO_PINS(49);
+DECLARE_APQ_GPIO_PINS(50);
+DECLARE_APQ_GPIO_PINS(51);
+DECLARE_APQ_GPIO_PINS(52);
+DECLARE_APQ_GPIO_PINS(53);
+DECLARE_APQ_GPIO_PINS(54);
+DECLARE_APQ_GPIO_PINS(55);
+DECLARE_APQ_GPIO_PINS(56);
+DECLARE_APQ_GPIO_PINS(57);
+DECLARE_APQ_GPIO_PINS(58);
+DECLARE_APQ_GPIO_PINS(59);
+DECLARE_APQ_GPIO_PINS(60);
+DECLARE_APQ_GPIO_PINS(61);
+DECLARE_APQ_GPIO_PINS(62);
+DECLARE_APQ_GPIO_PINS(63);
+DECLARE_APQ_GPIO_PINS(64);
+DECLARE_APQ_GPIO_PINS(65);
+DECLARE_APQ_GPIO_PINS(66);
+DECLARE_APQ_GPIO_PINS(67);
+DECLARE_APQ_GPIO_PINS(68);
+DECLARE_APQ_GPIO_PINS(69);
+DECLARE_APQ_GPIO_PINS(70);
+DECLARE_APQ_GPIO_PINS(71);
+DECLARE_APQ_GPIO_PINS(72);
+DECLARE_APQ_GPIO_PINS(73);
+DECLARE_APQ_GPIO_PINS(74);
+DECLARE_APQ_GPIO_PINS(75);
+DECLARE_APQ_GPIO_PINS(76);
+DECLARE_APQ_GPIO_PINS(77);
+DECLARE_APQ_GPIO_PINS(78);
+DECLARE_APQ_GPIO_PINS(79);
+DECLARE_APQ_GPIO_PINS(80);
+DECLARE_APQ_GPIO_PINS(81);
+DECLARE_APQ_GPIO_PINS(82);
+DECLARE_APQ_GPIO_PINS(83);
+DECLARE_APQ_GPIO_PINS(84);
+DECLARE_APQ_GPIO_PINS(85);
+DECLARE_APQ_GPIO_PINS(86);
+DECLARE_APQ_GPIO_PINS(87);
+DECLARE_APQ_GPIO_PINS(88);
+DECLARE_APQ_GPIO_PINS(89);
+
+static const unsigned int sdc1_clk_pins[] = { 90 };
+static const unsigned int sdc1_cmd_pins[] = { 91 };
+static const unsigned int sdc1_data_pins[] = { 92 };
+static const unsigned int sdc3_clk_pins[] = { 93 };
+static const unsigned int sdc3_cmd_pins[] = { 94 };
+static const unsigned int sdc3_data_pins[] = { 95 };
+
+#define FUNCTION(fname)					\
+	[APQ_MUX_##fname] = {				\
+		.name = #fname,				\
+		.groups = fname##_groups,		\
+		.ngroups = ARRAY_SIZE(fname##_groups),	\
+	}
+
+#define PINGROUP(id, f1, f2, f3, f4, f5, f6, f7, f8, f9, f10) \
+	{						\
+		.name = "gpio" #id,			\
+		.pins = gpio##id##_pins,		\
+		.npins = ARRAY_SIZE(gpio##id##_pins),	\
+		.funcs = (int[]){			\
+			APQ_MUX_NA, /* gpio mode */	\
+			APQ_MUX_##f1,			\
+			APQ_MUX_##f2,			\
+			APQ_MUX_##f3,			\
+			APQ_MUX_##f4,			\
+			APQ_MUX_##f5,			\
+			APQ_MUX_##f6,			\
+			APQ_MUX_##f7,			\
+			APQ_MUX_##f8,			\
+			APQ_MUX_##f9,			\
+			APQ_MUX_##f10,			\
+		},					\
+		.nfuncs = 11,				\
+		.ctl_reg = 0x1000 + 0x10 * id,		\
+		.io_reg = 0x1004 + 0x10 * id,		\
+		.intr_cfg_reg = 0x1008 + 0x10 * id,	\
+		.intr_status_reg = 0x100c + 0x10 * id,	\
+		.intr_target_reg = 0x400 + 0x4 * id,	\
+		.mux_bit = 2,				\
+		.pull_bit = 0,				\
+		.drv_bit = 6,				\
+		.oe_bit = 9,				\
+		.in_bit = 0,				\
+		.out_bit = 1,				\
+		.intr_enable_bit = 0,			\
+		.intr_status_bit = 0,			\
+		.intr_ack_high = 1,			\
+		.intr_target_bit = 0,			\
+		.intr_raw_status_bit = 3,		\
+		.intr_polarity_bit = 1,			\
+		.intr_detection_bit = 2,		\
+		.intr_detection_width = 1,		\
+	}
+
+#define SDC_PINGROUP(pg_name, ctl, pull, drv)		\
+	{						\
+		.name = #pg_name,			\
+		.pins = pg_name##_pins,			\
+		.npins = ARRAY_SIZE(pg_name##_pins),	\
+		.ctl_reg = ctl,				\
+		.io_reg = 0,				\
+		.intr_cfg_reg = 0,			\
+		.intr_status_reg = 0,			\
+		.intr_target_reg = 0,			\
+		.mux_bit = -1,				\
+		.pull_bit = pull,			\
+		.drv_bit = drv,				\
+		.oe_bit = -1,				\
+		.in_bit = -1,				\
+		.out_bit = -1,				\
+		.intr_enable_bit = -1,			\
+		.intr_status_bit = -1,			\
+		.intr_target_bit = -1,			\
+		.intr_raw_status_bit = -1,		\
+		.intr_polarity_bit = -1,		\
+		.intr_detection_bit = -1,		\
+		.intr_detection_width = -1,		\
+	}
+
+enum apq8064_functions {
+	APQ_MUX_cam_mclk,
+	APQ_MUX_codec_mic_i2s,
+	APQ_MUX_codec_spkr_i2s,
+	APQ_MUX_gsbi1,
+	APQ_MUX_gsbi2,
+	APQ_MUX_gsbi3,
+	APQ_MUX_gsbi4,
+	APQ_MUX_gsbi4_cam_i2c,
+	APQ_MUX_gsbi5,
+	APQ_MUX_gsbi5_spi_cs1,
+	APQ_MUX_gsbi5_spi_cs2,
+	APQ_MUX_gsbi5_spi_cs3,
+	APQ_MUX_gsbi6,
+	APQ_MUX_gsbi6_spi_cs1,
+	APQ_MUX_gsbi6_spi_cs2,
+	APQ_MUX_gsbi6_spi_cs3,
+	APQ_MUX_gsbi7,
+	APQ_MUX_gsbi7_spi_cs1,
+	APQ_MUX_gsbi7_spi_cs2,
+	APQ_MUX_gsbi7_spi_cs3,
+	APQ_MUX_gsbi_cam_i2c,
+	APQ_MUX_hdmi,
+	APQ_MUX_mi2s,
+	APQ_MUX_riva_bt,
+	APQ_MUX_riva_fm,
+	APQ_MUX_riva_wlan,
+	APQ_MUX_sdc2,
+	APQ_MUX_sdc4,
+	APQ_MUX_slimbus,
+	APQ_MUX_spkr_i2s,
+	APQ_MUX_tsif1,
+	APQ_MUX_tsif2,
+	APQ_MUX_usb2_hsic,
+	APQ_MUX_NA,
+};
+
+static const char * const cam_mclk_groups[] = {
+	"gpio4" "gpio5"
+};
+static const char * const codec_mic_i2s_groups[] = {
+	"gpio34", "gpio35", "gpio36", "gpio37", "gpio38"
+};
+static const char * const codec_spkr_i2s_groups[] = {
+	"gpio39", "gpio40", "gpio41", "gpio42"
+};
+static const char * const gsbi1_groups[] = {
+	"gpio18", "gpio19", "gpio20", "gpio21"
+};
+static const char * const gsbi2_groups[] = {
+	"gpio22", "gpio23", "gpio24", "gpio25"
+};
+static const char * const gsbi3_groups[] = {
+	"gpio6", "gpio7", "gpio8", "gpio9"
+};
+static const char * const gsbi4_groups[] = {
+	"gpio10", "gpio11", "gpio12", "gpio13"
+};
+static const char * const gsbi4_cam_i2c_groups[] = {
+	"gpio10", "gpio11", "gpio12", "gpio13"
+};
+static const char * const gsbi5_groups[] = {
+	"gpio51", "gpio52", "gpio53", "gpio54"
+};
+static const char * const gsbi5_spi_cs1_groups[] = {
+	"gpio47"
+};
+static const char * const gsbi5_spi_cs2_groups[] = {
+	"gpio31"
+};
+static const char * const gsbi5_spi_cs3_groups[] = {
+	"gpio32"
+};
+static const char * const gsbi6_groups[] = {
+	"gpio14", "gpio15", "gpio16", "gpio17"
+};
+static const char * const gsbi6_spi_cs1_groups[] = {
+	"gpio47"
+};
+static const char * const gsbi6_spi_cs2_groups[] = {
+	"gpio31"
+};
+static const char * const gsbi6_spi_cs3_groups[] = {
+	"gpio32"
+};
+static const char * const gsbi7_groups[] = {
+	"gpio82", "gpio83", "gpio84", "gpio85"
+};
+static const char * const gsbi7_spi_cs1_groups[] = {
+	"gpio47"
+};
+static const char * const gsbi7_spi_cs2_groups[] = {
+	"gpio31"
+};
+static const char * const gsbi7_spi_cs3_groups[] = {
+	"gpio32"
+};
+static const char * const gsbi_cam_i2c_groups[] = {
+	"gpio10", "gpio11", "gpio12", "gpio13"
+};
+static const char * const hdmi_groups[] = {
+	"gpio69", "gpio70", "gpio71", "gpio72"
+};
+static const char * const mi2s_groups[] = {
+	"gpio27", "gpio28", "gpio29", "gpio30", "gpio31", "gpio32", "gpio33"
+};
+static const char * const riva_bt_groups[] = {
+	"gpio16", "gpio17"
+};
+static const char * const riva_fm_groups[] = {
+	"gpio14", "gpio15"
+};
+static const char * const riva_wlan_groups[] = {
+	"gpio64", "gpio65", "gpio66", "gpio67", "gpio68"
+};
+static const char * const sdc2_groups[] = {
+	"gpio57", "gpio58", "gpio59", "gpio60", "gpio61", "gpio62"
+};
+static const char * const sdc4_groups[] = {
+	"gpio63", "gpio64", "gpio65", "gpio66", "gpio67", "gpio68"
+};
+static const char * const slimbus_groups[] = {
+	"gpio40", "gpio41"
+};
+static const char * const spkr_i2s_groups[] = {
+	"gpio47", "gpio48", "gpio49", "gpio50"
+};
+static const char * const tsif1_groups[] = {
+	"gpio55", "gpio56", "gpio57"
+};
+static const char * const tsif2_groups[] = {
+	"gpio58", "gpio59", "gpio60"
+};
+static const char * const usb2_hsic_groups[] = {
+	"gpio88", "gpio89"
+};
+
+static const struct msm_function apq8064_functions[] = {
+	FUNCTION(cam_mclk),
+	FUNCTION(codec_mic_i2s),
+	FUNCTION(codec_spkr_i2s),
+	FUNCTION(gsbi1),
+	FUNCTION(gsbi2),
+	FUNCTION(gsbi3),
+	FUNCTION(gsbi4),
+	FUNCTION(gsbi4_cam_i2c),
+	FUNCTION(gsbi5),
+	FUNCTION(gsbi5_spi_cs1),
+	FUNCTION(gsbi5_spi_cs2),
+	FUNCTION(gsbi5_spi_cs3),
+	FUNCTION(gsbi6),
+	FUNCTION(gsbi6_spi_cs1),
+	FUNCTION(gsbi6_spi_cs2),
+	FUNCTION(gsbi6_spi_cs3),
+	FUNCTION(gsbi7),
+	FUNCTION(gsbi7_spi_cs1),
+	FUNCTION(gsbi7_spi_cs2),
+	FUNCTION(gsbi7_spi_cs3),
+	FUNCTION(gsbi_cam_i2c),
+	FUNCTION(hdmi),
+	FUNCTION(mi2s),
+	FUNCTION(riva_bt),
+	FUNCTION(riva_fm),
+	FUNCTION(riva_wlan),
+	FUNCTION(sdc2),
+	FUNCTION(sdc4),
+	FUNCTION(slimbus),
+	FUNCTION(spkr_i2s),
+	FUNCTION(tsif1),
+	FUNCTION(tsif2),
+	FUNCTION(usb2_hsic),
+};
+
+static const struct msm_pingroup apq8064_groups[] = {
+	PINGROUP(0, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(1, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(2, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(3, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(4, NA, NA, cam_mclk, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(5, NA, cam_mclk, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(6, gsbi3, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(7, gsbi3, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(8, gsbi3, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(9, gsbi3, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(10, gsbi4, NA, NA, NA, NA, NA, NA, NA, gsbi4_cam_i2c, NA),
+	PINGROUP(11, gsbi4, NA, NA, NA, NA, NA, NA, NA, NA, gsbi4_cam_i2c),
+	PINGROUP(12, gsbi4, NA, NA, NA, NA, gsbi4_cam_i2c, NA, NA, NA, NA),
+	PINGROUP(13, gsbi4, NA, NA, NA, NA, gsbi4_cam_i2c, NA, NA, NA, NA),
+	PINGROUP(14, riva_fm, gsbi6, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(15, riva_fm, gsbi6, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(16, riva_bt, gsbi6, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(17, riva_bt, gsbi6, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(18, gsbi1, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(19, gsbi1, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(20, gsbi1, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(21, gsbi1, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(22, gsbi2, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(23, gsbi2, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(24, gsbi2, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(25, gsbi2, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(26, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(27, mi2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(28, mi2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(29, mi2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(30, mi2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(31, mi2s, NA, gsbi5_spi_cs2, gsbi6_spi_cs2, gsbi7_spi_cs2, NA, NA, NA, NA, NA),
+	PINGROUP(32, mi2s, NA, NA, NA, NA, gsbi5_spi_cs3, gsbi6_spi_cs3, gsbi7_spi_cs3, NA, NA),
+	PINGROUP(33, mi2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(34, codec_mic_i2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(35, codec_mic_i2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(36, codec_mic_i2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(37, codec_mic_i2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(38, codec_mic_i2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(39, codec_spkr_i2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(40, slimbus, codec_spkr_i2s, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(41, slimbus, codec_spkr_i2s, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(42, codec_spkr_i2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(43, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(44, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(45, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(46, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(47, spkr_i2s, gsbi5_spi_cs1, gsbi6_spi_cs1, gsbi7_spi_cs1, NA, NA, NA, NA, NA, NA),
+	PINGROUP(48, spkr_i2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(49, spkr_i2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(50, spkr_i2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(51, NA, gsbi5, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(52, NA, gsbi5, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(53, NA, gsbi5, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(54, NA, gsbi5, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(55, tsif1, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(56, tsif1, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(57, tsif1, sdc2, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(58, tsif2, sdc2, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(59, tsif2, sdc2, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(60, tsif2, sdc2, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(61, NA, sdc2, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(62, NA, sdc2, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(63, NA, sdc4, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(64, riva_wlan, sdc4, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(65, riva_wlan, sdc4, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(66, riva_wlan, sdc4, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(67, riva_wlan, sdc4, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(68, riva_wlan, sdc4, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(69, hdmi, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(70, hdmi, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(71, hdmi, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(72, hdmi, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(73, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(74, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(75, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(76, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(77, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(78, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(79, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(80, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(81, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(82, NA, gsbi7, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(83, gsbi7, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(84, NA, gsbi7, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(85, NA, NA, gsbi7, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(86, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(87, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(88, usb2_hsic, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(89, usb2_hsic, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+
+	SDC_PINGROUP(sdc1_clk, 0x20a0, 13, 6),
+	SDC_PINGROUP(sdc1_cmd, 0x20a0, 11, 3),
+	SDC_PINGROUP(sdc1_data, 0x20a0, 9, 0),
+
+	SDC_PINGROUP(sdc3_clk, 0x20a4, 14, 6),
+	SDC_PINGROUP(sdc3_cmd, 0x20a4, 11, 3),
+	SDC_PINGROUP(sdc3_data, 0x20a4, 9, 0),
+};
+
+#define NUM_GPIO_PINGROUPS 90
+
+static const struct msm_pinctrl_soc_data apq8064_pinctrl = {
+	.pins = apq8064_pins,
+	.npins = ARRAY_SIZE(apq8064_pins),
+	.functions = apq8064_functions,
+	.nfunctions = ARRAY_SIZE(apq8064_functions),
+	.groups = apq8064_groups,
+	.ngroups = ARRAY_SIZE(apq8064_groups),
+	.ngpios = NUM_GPIO_PINGROUPS,
+};
+
+static int apq8064_pinctrl_probe(struct platform_device *pdev)
+{
+	return msm_pinctrl_probe(pdev, &apq8064_pinctrl);
+}
+
+static const struct of_device_id apq8064_pinctrl_of_match[] = {
+	{ .compatible = "qcom,apq8064-pinctrl", },
+	{ },
+};
+
+static struct platform_driver apq8064_pinctrl_driver = {
+	.driver = {
+		.name = "apq8064-pinctrl",
+		.owner = THIS_MODULE,
+		.of_match_table = apq8064_pinctrl_of_match,
+	},
+	.probe = apq8064_pinctrl_probe,
+	.remove = msm_pinctrl_remove,
+};
+
+static int __init apq8064_pinctrl_init(void)
+{
+	return platform_driver_register(&apq8064_pinctrl_driver);
+}
+arch_initcall(apq8064_pinctrl_init);
+
+static void __exit apq8064_pinctrl_exit(void)
+{
+	platform_driver_unregister(&apq8064_pinctrl_driver);
+}
+module_exit(apq8064_pinctrl_exit);
+
+MODULE_AUTHOR("Bjorn Andersson <bjorn.andersson@sonymobile.com>");
+MODULE_DESCRIPTION("Qualcomm APQ8064 pinctrl driver");
+MODULE_LICENSE("GPL v2");
+MODULE_DEVICE_TABLE(of, apq8064_pinctrl_of_match);
diff --git a/drivers/pinctrl/pinctrl-as3722.c b/drivers/pinctrl/pinctrl-as3722.c
index 92ed4b2e3c0..c862f9c0e9c 100644
--- a/drivers/pinctrl/pinctrl-as3722.c
+++ b/drivers/pinctrl/pinctrl-as3722.c
@@ -64,7 +64,6 @@ struct as3722_pin_function {
 };
 
 struct as3722_gpio_pin_control {
-	bool enable_gpio_invert;
 	unsigned mode_prop;
 	int io_function;
 };
@@ -320,10 +319,8 @@ static int as3722_pinctrl_gpio_set_direction(struct pinctrl_dev *pctldev,
 		return mode;
 	}
 
-	if (as_pci->gpio_control[offset].enable_gpio_invert)
-		mode |= AS3722_GPIO_INV;
-
-	return as3722_write(as3722, AS3722_GPIOn_CONTROL_REG(offset), mode);
+	return as3722_update_bits(as3722, AS3722_GPIOn_CONTROL_REG(offset),
+				AS3722_GPIO_MODE_MASK, mode);
 }
 
 static const struct pinmux_ops as3722_pinmux_ops = {
@@ -496,10 +493,18 @@ static void as3722_gpio_set(struct gpio_chip *chip, unsigned offset,
 {
 	struct as3722_pctrl_info *as_pci = to_as_pci(chip);
 	struct as3722 *as3722 = as_pci->as3722;
-	int en_invert = as_pci->gpio_control[offset].enable_gpio_invert;
+	int en_invert;
 	u32 val;
 	int ret;
 
+	ret = as3722_read(as3722, AS3722_GPIOn_CONTROL_REG(offset), &val);
+	if (ret < 0) {
+		dev_err(as_pci->dev,
+			"GPIO_CONTROL%d_REG read failed: %d\n", offset, ret);
+		return;
+	}
+	en_invert = !!(val & AS3722_GPIO_INV);
+
 	if (value)
 		val = (en_invert) ? 0 : AS3722_GPIOn_SIGNAL(offset);
 	else
diff --git a/drivers/pinctrl/pinctrl-at91.c b/drivers/pinctrl/pinctrl-at91.c
index 5d24aaec5db..421493cb490 100644
--- a/drivers/pinctrl/pinctrl-at91.c
+++ b/drivers/pinctrl/pinctrl-at91.c
@@ -16,9 +16,6 @@
 #include <linux/of_irq.h>
 #include <linux/slab.h>
 #include <linux/interrupt.h>
-#include <linux/irq.h>
-#include <linux/irqdomain.h>
-#include <linux/irqchip/chained_irq.h>
 #include <linux/io.h>
 #include <linux/gpio.h>
 #include <linux/pinctrl/machine.h>
@@ -47,7 +44,6 @@ struct at91_gpio_chip {
 	int			pioc_idx;	/* PIO bank index */
 	void __iomem		*regbase;	/* PIO bank virtual address */
 	struct clk		*clock;		/* associated clock */
-	struct irq_domain	*domain;	/* associated irq domain */
 	struct at91_pinctrl_mux_ops *ops;	/* ops */
 };
 
@@ -1192,21 +1188,6 @@ static int at91_gpio_direction_output(struct gpio_chip *chip, unsigned offset,
 	return 0;
 }
 
-static int at91_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
-{
-	struct at91_gpio_chip *at91_gpio = to_at91_gpio_chip(chip);
-	int virq;
-
-	if (offset < chip->ngpio)
-		virq = irq_create_mapping(at91_gpio->domain, offset);
-	else
-		virq = -ENXIO;
-
-	dev_dbg(chip->dev, "%s: request IRQ for GPIO %d, return %d\n",
-				chip->label, offset + chip->base, virq);
-	return virq;
-}
-
 #ifdef CONFIG_DEBUG_FS
 static void at91_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 {
@@ -1216,8 +1197,7 @@ static void at91_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 	void __iomem *pio = at91_gpio->regbase;
 
 	for (i = 0; i < chip->ngpio; i++) {
-		unsigned pin = chip->base + i;
-		unsigned mask = pin_to_mask(pin);
+		unsigned mask = pin_to_mask(i);
 		const char *gpio_label;
 		u32 pdsr;
 
@@ -1336,6 +1316,11 @@ static int alt_gpio_irq_type(struct irq_data *d, unsigned type)
 	return 0;
 }
 
+static void gpio_irq_ack(struct irq_data *d)
+{
+	/* the interrupt is already cleared before by reading ISR */
+}
+
 static unsigned int gpio_irq_startup(struct irq_data *d)
 {
 	struct at91_gpio_chip *at91_gpio = irq_data_get_irq_chip_data(d);
@@ -1435,6 +1420,7 @@ void at91_pinctrl_gpio_resume(void)
 
 static struct irq_chip gpio_irqchip = {
 	.name		= "GPIO",
+	.irq_ack	= gpio_irq_ack,
 	.irq_startup	= gpio_irq_startup,
 	.irq_shutdown	= gpio_irq_shutdown,
 	.irq_disable	= gpio_irq_mask,
@@ -1446,9 +1432,11 @@ static struct irq_chip gpio_irqchip = {
 
 static void gpio_irq_handler(unsigned irq, struct irq_desc *desc)
 {
-	struct irq_chip *chip = irq_desc_get_chip(desc);
-	struct irq_data *idata = irq_desc_get_irq_data(desc);
-	struct at91_gpio_chip *at91_gpio = irq_data_get_irq_chip_data(idata);
+	struct irq_chip *chip = irq_get_chip(irq);
+	struct gpio_chip *gpio_chip = irq_desc_get_handler_data(desc);
+	struct at91_gpio_chip *at91_gpio = container_of(gpio_chip,
+					   struct at91_gpio_chip, chip);
+
 	void __iomem	*pio = at91_gpio->regbase;
 	unsigned long	isr;
 	int		n;
@@ -1465,85 +1453,25 @@ static void gpio_irq_handler(unsigned irq, struct irq_desc *desc)
 				break;
 			at91_gpio = at91_gpio->next;
 			pio = at91_gpio->regbase;
+			gpio_chip = &at91_gpio->chip;
 			continue;
 		}
 
 		for_each_set_bit(n, &isr, BITS_PER_LONG) {
-			generic_handle_irq(irq_find_mapping(at91_gpio->domain, n));
+			generic_handle_irq(irq_find_mapping(
+					   gpio_chip->irqdomain, n));
 		}
 	}
 	chained_irq_exit(chip, desc);
 	/* now it may re-trigger */
 }
 
-/*
- * This lock class tells lockdep that GPIO irqs are in a different
- * category than their parents, so it won't report false recursion.
- */
-static struct lock_class_key gpio_lock_class;
-
-static int at91_gpio_irq_map(struct irq_domain *h, unsigned int virq,
-							irq_hw_number_t hw)
-{
-	struct at91_gpio_chip	*at91_gpio = h->host_data;
-	void __iomem		*pio = at91_gpio->regbase;
-	u32			mask = 1 << hw;
-
-	irq_set_lockdep_class(virq, &gpio_lock_class);
-
-	/*
-	 * Can use the "simple" and not "edge" handler since it's
-	 * shorter, and the AIC handles interrupts sanely.
-	 */
-	irq_set_chip(virq, &gpio_irqchip);
-	if ((at91_gpio->ops == &at91sam9x5_ops) &&
-	    (readl_relaxed(pio + PIO_AIMMR) & mask) &&
-	    (readl_relaxed(pio + PIO_ELSR) & mask))
-		irq_set_handler(virq, handle_level_irq);
-	else
-		irq_set_handler(virq, handle_simple_irq);
-	set_irq_flags(virq, IRQF_VALID);
-	irq_set_chip_data(virq, at91_gpio);
-
-	return 0;
-}
-
-static int at91_gpio_irq_domain_xlate(struct irq_domain *d,
-				      struct device_node *ctrlr,
-				      const u32 *intspec, unsigned int intsize,
-				      irq_hw_number_t *out_hwirq,
-				      unsigned int *out_type)
-{
-	struct at91_gpio_chip *at91_gpio = d->host_data;
-	int ret;
-	int pin = at91_gpio->chip.base + intspec[0];
-
-	if (WARN_ON(intsize < 2))
-		return -EINVAL;
-	*out_hwirq = intspec[0];
-	*out_type = intspec[1] & IRQ_TYPE_SENSE_MASK;
-
-	ret = gpio_request(pin, ctrlr->full_name);
-	if (ret)
-		return ret;
-
-	ret = gpio_direction_input(pin);
-	if (ret)
-		return ret;
-
-	return 0;
-}
-
-static struct irq_domain_ops at91_gpio_ops = {
-	.map	= at91_gpio_irq_map,
-	.xlate	= at91_gpio_irq_domain_xlate,
-};
-
 static int at91_gpio_of_irq_setup(struct device_node *node,
 				  struct at91_gpio_chip *at91_gpio)
 {
-	struct at91_gpio_chip	*prev = NULL;
+	struct at91_gpio_chip   *prev = NULL;
 	struct irq_data		*d = irq_get_irq_data(at91_gpio->pioc_virq);
+	int ret;
 
 	at91_gpio->pioc_hwirq = irqd_to_hwirq(d);
 
@@ -1553,10 +1481,17 @@ static int at91_gpio_of_irq_setup(struct device_node *node,
 	/* Disable irqs of this PIO controller */
 	writel_relaxed(~0, at91_gpio->regbase + PIO_IDR);
 
-	/* Setup irq domain */
-	at91_gpio->domain = irq_domain_add_linear(node, at91_gpio->chip.ngpio,
-						&at91_gpio_ops, at91_gpio);
-	if (!at91_gpio->domain)
+	/*
+	 * Let the generic code handle this edge IRQ, the the chained
+	 * handler will perform the actual work of handling the parent
+	 * interrupt.
+	 */
+	ret = gpiochip_irqchip_add(&at91_gpio->chip,
+				   &gpio_irqchip,
+				   0,
+				   handle_edge_irq,
+				   IRQ_TYPE_EDGE_BOTH);
+	if (ret)
 		panic("at91_gpio.%d: couldn't allocate irq domain (DT).\n",
 			at91_gpio->pioc_idx);
 
@@ -1571,8 +1506,11 @@ static int at91_gpio_of_irq_setup(struct device_node *node,
 	if (prev && prev->next == at91_gpio)
 		return 0;
 
-	irq_set_chip_data(at91_gpio->pioc_virq, at91_gpio);
-	irq_set_chained_handler(at91_gpio->pioc_virq, gpio_irq_handler);
+	/* Then register the chain on the parent IRQ */
+	gpiochip_set_chained_irqchip(&at91_gpio->chip,
+				     &gpio_irqchip,
+				     at91_gpio->pioc_virq,
+				     gpio_irq_handler);
 
 	return 0;
 }
@@ -1586,7 +1524,6 @@ static struct gpio_chip at91_gpio_template = {
 	.get			= at91_gpio_get,
 	.direction_output	= at91_gpio_direction_output,
 	.set			= at91_gpio_set,
-	.to_irq			= at91_gpio_to_irq,
 	.dbg_show		= at91_gpio_dbg_show,
 	.can_sleep		= false,
 	.ngpio			= MAX_NB_GPIO_PER_BANK,
diff --git a/drivers/pinctrl/pinctrl-baytrail.c b/drivers/pinctrl/pinctrl-baytrail.c
index 6e8301f7718..975572e2f26 100644
--- a/drivers/pinctrl/pinctrl-baytrail.c
+++ b/drivers/pinctrl/pinctrl-baytrail.c
@@ -43,9 +43,20 @@
 #define BYT_INT_STAT_REG	0x800
 
 /* BYT_CONF0_REG register bits */
+#define BYT_IODEN		BIT(31)
 #define BYT_TRIG_NEG		BIT(26)
 #define BYT_TRIG_POS		BIT(25)
 #define BYT_TRIG_LVL		BIT(24)
+#define BYT_PULL_STR_SHIFT	9
+#define BYT_PULL_STR_MASK	(3 << BYT_PULL_STR_SHIFT)
+#define BYT_PULL_STR_2K		(0 << BYT_PULL_STR_SHIFT)
+#define BYT_PULL_STR_10K	(1 << BYT_PULL_STR_SHIFT)
+#define BYT_PULL_STR_20K	(2 << BYT_PULL_STR_SHIFT)
+#define BYT_PULL_STR_40K	(3 << BYT_PULL_STR_SHIFT)
+#define BYT_PULL_ASSIGN_SHIFT	7
+#define BYT_PULL_ASSIGN_MASK	(3 << BYT_PULL_ASSIGN_SHIFT)
+#define BYT_PULL_ASSIGN_UP	(1 << BYT_PULL_ASSIGN_SHIFT)
+#define BYT_PULL_ASSIGN_DOWN	(2 << BYT_PULL_ASSIGN_SHIFT)
 #define BYT_PIN_MUX		0x07
 
 /* BYT_VAL_REG register bits */
@@ -321,6 +332,8 @@ static void byt_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 	spin_lock_irqsave(&vg->lock, flags);
 
 	for (i = 0; i < vg->chip.ngpio; i++) {
+		const char *pull_str = NULL;
+		const char *pull = NULL;
 		const char *label;
 		offs = vg->range->pins[i] * 16;
 		conf0 = readl(vg->reg_base + offs + BYT_CONF0_REG);
@@ -330,8 +343,32 @@ static void byt_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 		if (!label)
 			label = "Unrequested";
 
+		switch (conf0 & BYT_PULL_ASSIGN_MASK) {
+		case BYT_PULL_ASSIGN_UP:
+			pull = "up";
+			break;
+		case BYT_PULL_ASSIGN_DOWN:
+			pull = "down";
+			break;
+		}
+
+		switch (conf0 & BYT_PULL_STR_MASK) {
+		case BYT_PULL_STR_2K:
+			pull_str = "2k";
+			break;
+		case BYT_PULL_STR_10K:
+			pull_str = "10k";
+			break;
+		case BYT_PULL_STR_20K:
+			pull_str = "20k";
+			break;
+		case BYT_PULL_STR_40K:
+			pull_str = "40k";
+			break;
+		}
+
 		seq_printf(s,
-			   " gpio-%-3d (%-20.20s) %s %s %s pad-%-3d offset:0x%03x mux:%d %s%s%s\n",
+			   " gpio-%-3d (%-20.20s) %s %s %s pad-%-3d offset:0x%03x mux:%d %s%s%s",
 			   i,
 			   label,
 			   val & BYT_INPUT_EN ? "  " : "in",
@@ -339,9 +376,19 @@ static void byt_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 			   val & BYT_LEVEL ? "hi" : "lo",
 			   vg->range->pins[i], offs,
 			   conf0 & 0x7,
-			   conf0 & BYT_TRIG_NEG ? " fall" : "",
-			   conf0 & BYT_TRIG_POS ? " rise" : "",
-			   conf0 & BYT_TRIG_LVL ? " level" : "");
+			   conf0 & BYT_TRIG_NEG ? " fall" : "     ",
+			   conf0 & BYT_TRIG_POS ? " rise" : "     ",
+			   conf0 & BYT_TRIG_LVL ? " level" : "      ");
+
+		if (pull && pull_str)
+			seq_printf(s, " %-4s %-3s", pull, pull_str);
+		else
+			seq_puts(s, "          ");
+
+		if (conf0 & BYT_IODEN)
+			seq_puts(s, " open-drain");
+
+		seq_puts(s, "\n");
 	}
 	spin_unlock_irqrestore(&vg->lock, flags);
 }
@@ -527,12 +574,6 @@ static int byt_gpio_probe(struct platform_device *pdev)
 	gc->can_sleep = false;
 	gc->dev = dev;
 
-	ret = gpiochip_add(gc);
-	if (ret) {
-		dev_err(&pdev->dev, "failed adding byt-gpio chip\n");
-		return ret;
-	}
-
 	/* set up interrupts  */
 	irq_rc = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 	if (irq_rc && irq_rc->start) {
@@ -550,6 +591,12 @@ static int byt_gpio_probe(struct platform_device *pdev)
 		irq_set_chained_handler(hwirq, byt_gpio_irq_handler);
 	}
 
+	ret = gpiochip_add(gc);
+	if (ret) {
+		dev_err(&pdev->dev, "failed adding byt-gpio chip\n");
+		return ret;
+	}
+
 	pm_runtime_enable(dev);
 
 	return 0;
@@ -572,6 +619,7 @@ static const struct dev_pm_ops byt_gpio_pm_ops = {
 
 static const struct acpi_device_id byt_gpio_acpi_match[] = {
 	{ "INT33B2", 0 },
+	{ "INT33FC", 0 },
 	{ }
 };
 MODULE_DEVICE_TABLE(acpi, byt_gpio_acpi_match);
diff --git a/drivers/pinctrl/pinctrl-exynos.c b/drivers/pinctrl/pinctrl-exynos.c
index 07c81306f2f..9609c23834c 100644
--- a/drivers/pinctrl/pinctrl-exynos.c
+++ b/drivers/pinctrl/pinctrl-exynos.c
@@ -718,6 +718,73 @@ struct samsung_pin_ctrl s5pv210_pin_ctrl[] = {
 	},
 };
 
+/* pin banks of exynos3250 pin-controller 0 */
+static struct samsung_pin_bank exynos3250_pin_banks0[] = {
+	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpa0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(6, 0x020, "gpa1", 0x04),
+	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpb",  0x08),
+	EXYNOS_PIN_BANK_EINTG(5, 0x060, "gpc0", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(5, 0x080, "gpc1", 0x10),
+	EXYNOS_PIN_BANK_EINTG(4, 0x0a0, "gpd0", 0x14),
+	EXYNOS_PIN_BANK_EINTG(4, 0x0c0, "gpd1", 0x18),
+};
+
+/* pin banks of exynos3250 pin-controller 1 */
+static struct samsung_pin_bank exynos3250_pin_banks1[] = {
+	EXYNOS_PIN_BANK_EINTN(8, 0x120, "gpe0"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x140, "gpe1"),
+	EXYNOS_PIN_BANK_EINTN(3, 0x180, "gpe2"),
+	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpk0", 0x08),
+	EXYNOS_PIN_BANK_EINTG(7, 0x060, "gpk1", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(7, 0x080, "gpk2", 0x10),
+	EXYNOS_PIN_BANK_EINTG(4, 0x0c0, "gpl0", 0x18),
+	EXYNOS_PIN_BANK_EINTG(8, 0x260, "gpm0", 0x24),
+	EXYNOS_PIN_BANK_EINTG(7, 0x280, "gpm1", 0x28),
+	EXYNOS_PIN_BANK_EINTG(5, 0x2a0, "gpm2", 0x2c),
+	EXYNOS_PIN_BANK_EINTG(8, 0x2c0, "gpm3", 0x30),
+	EXYNOS_PIN_BANK_EINTG(8, 0x2e0, "gpm4", 0x34),
+	EXYNOS_PIN_BANK_EINTW(8, 0xc00, "gpx0", 0x00),
+	EXYNOS_PIN_BANK_EINTW(8, 0xc20, "gpx1", 0x04),
+	EXYNOS_PIN_BANK_EINTW(8, 0xc40, "gpx2", 0x08),
+	EXYNOS_PIN_BANK_EINTW(8, 0xc60, "gpx3", 0x0c),
+};
+
+/*
+ * Samsung pinctrl driver data for Exynos3250 SoC. Exynos3250 SoC includes
+ * two gpio/pin-mux/pinconfig controllers.
+ */
+struct samsung_pin_ctrl exynos3250_pin_ctrl[] = {
+	{
+		/* pin-controller instance 0 data */
+		.pin_banks	= exynos3250_pin_banks0,
+		.nr_banks	= ARRAY_SIZE(exynos3250_pin_banks0),
+		.geint_con	= EXYNOS_GPIO_ECON_OFFSET,
+		.geint_mask	= EXYNOS_GPIO_EMASK_OFFSET,
+		.geint_pend	= EXYNOS_GPIO_EPEND_OFFSET,
+		.svc		= EXYNOS_SVC_OFFSET,
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+		.label		= "exynos3250-gpio-ctrl0",
+	}, {
+		/* pin-controller instance 1 data */
+		.pin_banks	= exynos3250_pin_banks1,
+		.nr_banks	= ARRAY_SIZE(exynos3250_pin_banks1),
+		.geint_con	= EXYNOS_GPIO_ECON_OFFSET,
+		.geint_mask	= EXYNOS_GPIO_EMASK_OFFSET,
+		.geint_pend	= EXYNOS_GPIO_EPEND_OFFSET,
+		.weint_con	= EXYNOS_WKUP_ECON_OFFSET,
+		.weint_mask	= EXYNOS_WKUP_EMASK_OFFSET,
+		.weint_pend	= EXYNOS_WKUP_EPEND_OFFSET,
+		.svc		= EXYNOS_SVC_OFFSET,
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.eint_wkup_init = exynos_eint_wkup_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+		.label		= "exynos3250-gpio-ctrl1",
+	},
+};
+
 /* pin banks of exynos4210 pin-controller 0 */
 static struct samsung_pin_bank exynos4210_pin_banks0[] = {
 	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpa0", 0x00),
diff --git a/drivers/pinctrl/pinctrl-imx.c b/drivers/pinctrl/pinctrl-imx.c
index e118fb121e0..a24448e5d39 100644
--- a/drivers/pinctrl/pinctrl-imx.c
+++ b/drivers/pinctrl/pinctrl-imx.c
@@ -491,7 +491,7 @@ static int imx_pinctrl_parse_groups(struct device_node *np,
 			pin->mux_mode |= IOMUXC_CONFIG_SION;
 		pin->config = config & ~IMX_PAD_SION;
 
-		dev_dbg(info->dev, "%s: %d 0x%08lx", info->pins[pin_id].name,
+		dev_dbg(info->dev, "%s: 0x%x 0x%08lx", info->pins[pin_id].name,
 				pin->mux_mode, pin->config);
 	}
 
diff --git a/drivers/pinctrl/pinctrl-imx6sx.c b/drivers/pinctrl/pinctrl-imx6sx.c
new file mode 100644
index 00000000000..09758a56b9d
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-imx6sx.c
@@ -0,0 +1,407 @@
+/*
+ * Copyright (C) 2014 Freescale Semiconductor, 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.
+ */
+
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-imx.h"
+
+enum imx6sx_pads {
+	MX6Sx_PAD_RESERVE0 = 0,
+	MX6Sx_PAD_RESERVE1 = 1,
+	MX6Sx_PAD_RESERVE2 = 2,
+	MX6Sx_PAD_RESERVE3 = 3,
+	MX6Sx_PAD_RESERVE4 = 4,
+	MX6SX_PAD_GPIO1_IO00 = 5,
+	MX6SX_PAD_GPIO1_IO01 = 6,
+	MX6SX_PAD_GPIO1_IO02 = 7,
+	MX6SX_PAD_GPIO1_IO03 = 8,
+	MX6SX_PAD_GPIO1_IO04 = 9,
+	MX6SX_PAD_GPIO1_IO05 = 10,
+	MX6SX_PAD_GPIO1_IO06 = 11,
+	MX6SX_PAD_GPIO1_IO07 = 12,
+	MX6SX_PAD_GPIO1_IO08 = 13,
+	MX6SX_PAD_GPIO1_IO09 = 14,
+	MX6SX_PAD_GPIO1_IO10 = 15,
+	MX6SX_PAD_GPIO1_IO11 = 16,
+	MX6SX_PAD_GPIO1_IO12 = 17,
+	MX6SX_PAD_GPIO1_IO13 = 18,
+	MX6SX_PAD_CSI_DATA00 = 19,
+	MX6SX_PAD_CSI_DATA01 = 20,
+	MX6SX_PAD_CSI_DATA02 = 21,
+	MX6SX_PAD_CSI_DATA03 = 22,
+	MX6SX_PAD_CSI_DATA04 = 23,
+	MX6SX_PAD_CSI_DATA05 = 24,
+	MX6SX_PAD_CSI_DATA06 = 25,
+	MX6SX_PAD_CSI_DATA07 = 26,
+	MX6SX_PAD_CSI_HSYNC = 27,
+	MX6SX_PAD_CSI_MCLK = 28,
+	MX6SX_PAD_CSI_PIXCLK = 29,
+	MX6SX_PAD_CSI_VSYNC = 30,
+	MX6SX_PAD_ENET1_COL = 31,
+	MX6SX_PAD_ENET1_CRS = 32,
+	MX6SX_PAD_ENET1_MDC = 33,
+	MX6SX_PAD_ENET1_MDIO = 34,
+	MX6SX_PAD_ENET1_RX_CLK = 35,
+	MX6SX_PAD_ENET1_TX_CLK = 36,
+	MX6SX_PAD_ENET2_COL = 37,
+	MX6SX_PAD_ENET2_CRS = 38,
+	MX6SX_PAD_ENET2_RX_CLK = 39,
+	MX6SX_PAD_ENET2_TX_CLK = 40,
+	MX6SX_PAD_KEY_COL0 = 41,
+	MX6SX_PAD_KEY_COL1 = 42,
+	MX6SX_PAD_KEY_COL2 = 43,
+	MX6SX_PAD_KEY_COL3 = 44,
+	MX6SX_PAD_KEY_COL4 = 45,
+	MX6SX_PAD_KEY_ROW0 = 46,
+	MX6SX_PAD_KEY_ROW1 = 47,
+	MX6SX_PAD_KEY_ROW2 = 48,
+	MX6SX_PAD_KEY_ROW3 = 49,
+	MX6SX_PAD_KEY_ROW4 = 50,
+	MX6SX_PAD_LCD1_CLK = 51,
+	MX6SX_PAD_LCD1_DATA00 = 52,
+	MX6SX_PAD_LCD1_DATA01 = 53,
+	MX6SX_PAD_LCD1_DATA02 = 54,
+	MX6SX_PAD_LCD1_DATA03 = 55,
+	MX6SX_PAD_LCD1_DATA04 = 56,
+	MX6SX_PAD_LCD1_DATA05 = 57,
+	MX6SX_PAD_LCD1_DATA06 = 58,
+	MX6SX_PAD_LCD1_DATA07 = 59,
+	MX6SX_PAD_LCD1_DATA08 = 60,
+	MX6SX_PAD_LCD1_DATA09 = 61,
+	MX6SX_PAD_LCD1_DATA10 = 62,
+	MX6SX_PAD_LCD1_DATA11 = 63,
+	MX6SX_PAD_LCD1_DATA12 = 64,
+	MX6SX_PAD_LCD1_DATA13 = 65,
+	MX6SX_PAD_LCD1_DATA14 = 66,
+	MX6SX_PAD_LCD1_DATA15 = 67,
+	MX6SX_PAD_LCD1_DATA16 = 68,
+	MX6SX_PAD_LCD1_DATA17 = 69,
+	MX6SX_PAD_LCD1_DATA18 = 70,
+	MX6SX_PAD_LCD1_DATA19 = 71,
+	MX6SX_PAD_LCD1_DATA20 = 72,
+	MX6SX_PAD_LCD1_DATA21 = 73,
+	MX6SX_PAD_LCD1_DATA22 = 74,
+	MX6SX_PAD_LCD1_DATA23 = 75,
+	MX6SX_PAD_LCD1_ENABLE = 76,
+	MX6SX_PAD_LCD1_HSYNC = 77,
+	MX6SX_PAD_LCD1_RESET = 78,
+	MX6SX_PAD_LCD1_VSYNC = 79,
+	MX6SX_PAD_NAND_ALE = 80,
+	MX6SX_PAD_NAND_CE0_B = 81,
+	MX6SX_PAD_NAND_CE1_B = 82,
+	MX6SX_PAD_NAND_CLE = 83,
+	MX6SX_PAD_NAND_DATA00 = 84 ,
+	MX6SX_PAD_NAND_DATA01 = 85,
+	MX6SX_PAD_NAND_DATA02 = 86,
+	MX6SX_PAD_NAND_DATA03 = 87,
+	MX6SX_PAD_NAND_DATA04 = 88,
+	MX6SX_PAD_NAND_DATA05 = 89,
+	MX6SX_PAD_NAND_DATA06 = 90,
+	MX6SX_PAD_NAND_DATA07 = 91,
+	MX6SX_PAD_NAND_RE_B = 92,
+	MX6SX_PAD_NAND_READY_B = 93,
+	MX6SX_PAD_NAND_WE_B = 94,
+	MX6SX_PAD_NAND_WP_B = 95,
+	MX6SX_PAD_QSPI1A_DATA0 = 96,
+	MX6SX_PAD_QSPI1A_DATA1 = 97,
+	MX6SX_PAD_QSPI1A_DATA2 = 98,
+	MX6SX_PAD_QSPI1A_DATA3 = 99,
+	MX6SX_PAD_QSPI1A_DQS = 100,
+	MX6SX_PAD_QSPI1A_SCLK = 101,
+	MX6SX_PAD_QSPI1A_SS0_B = 102,
+	MX6SX_PAD_QSPI1A_SS1_B = 103,
+	MX6SX_PAD_QSPI1B_DATA0 = 104,
+	MX6SX_PAD_QSPI1B_DATA1 = 105,
+	MX6SX_PAD_QSPI1B_DATA2 = 106,
+	MX6SX_PAD_QSPI1B_DATA3 = 107,
+	MX6SX_PAD_QSPI1B_DQS = 108,
+	MX6SX_PAD_QSPI1B_SCLK = 109,
+	MX6SX_PAD_QSPI1B_SS0_B = 110,
+	MX6SX_PAD_QSPI1B_SS1_B = 111,
+	MX6SX_PAD_RGMII1_RD0 = 112,
+	MX6SX_PAD_RGMII1_RD1 = 113,
+	MX6SX_PAD_RGMII1_RD2 = 114,
+	MX6SX_PAD_RGMII1_RD3 = 115,
+	MX6SX_PAD_RGMII1_RX_CTL = 116,
+	MX6SX_PAD_RGMII1_RXC = 117,
+	MX6SX_PAD_RGMII1_TD0 = 118,
+	MX6SX_PAD_RGMII1_TD1 = 119,
+	MX6SX_PAD_RGMII1_TD2 = 120,
+	MX6SX_PAD_RGMII1_TD3 = 121,
+	MX6SX_PAD_RGMII1_TX_CTL = 122,
+	MX6SX_PAD_RGMII1_TXC = 123,
+	MX6SX_PAD_RGMII2_RD0 = 124,
+	MX6SX_PAD_RGMII2_RD1 = 125,
+	MX6SX_PAD_RGMII2_RD2 = 126,
+	MX6SX_PAD_RGMII2_RD3 = 127,
+	MX6SX_PAD_RGMII2_RX_CTL = 128,
+	MX6SX_PAD_RGMII2_RXC = 129,
+	MX6SX_PAD_RGMII2_TD0 = 130,
+	MX6SX_PAD_RGMII2_TD1 = 131,
+	MX6SX_PAD_RGMII2_TD2 = 132,
+	MX6SX_PAD_RGMII2_TD3 = 133,
+	MX6SX_PAD_RGMII2_TX_CTL = 134,
+	MX6SX_PAD_RGMII2_TXC = 135,
+	MX6SX_PAD_SD1_CLK = 136,
+	MX6SX_PAD_SD1_CMD = 137,
+	MX6SX_PAD_SD1_DATA0 = 138,
+	MX6SX_PAD_SD1_DATA1 = 139,
+	MX6SX_PAD_SD1_DATA2 = 140,
+	MX6SX_PAD_SD1_DATA3 = 141,
+	MX6SX_PAD_SD2_CLK = 142,
+	MX6SX_PAD_SD2_CMD = 143,
+	MX6SX_PAD_SD2_DATA0 = 144,
+	MX6SX_PAD_SD2_DATA1 = 145,
+	MX6SX_PAD_SD2_DATA2 = 146,
+	MX6SX_PAD_SD2_DATA3 = 147,
+	MX6SX_PAD_SD3_CLK = 148,
+	MX6SX_PAD_SD3_CMD = 149,
+	MX6SX_PAD_SD3_DATA0 = 150,
+	MX6SX_PAD_SD3_DATA1 = 151,
+	MX6SX_PAD_SD3_DATA2 = 152,
+	MX6SX_PAD_SD3_DATA3 = 153,
+	MX6SX_PAD_SD3_DATA4 = 154,
+	MX6SX_PAD_SD3_DATA5 = 155,
+	MX6SX_PAD_SD3_DATA6 = 156,
+	MX6SX_PAD_SD3_DATA7 = 157,
+	MX6SX_PAD_SD4_CLK = 158,
+	MX6SX_PAD_SD4_CMD = 159,
+	MX6SX_PAD_SD4_DATA0 = 160,
+	MX6SX_PAD_SD4_DATA1 = 161,
+	MX6SX_PAD_SD4_DATA2 = 162,
+	MX6SX_PAD_SD4_DATA3 = 163,
+	MX6SX_PAD_SD4_DATA4 = 164,
+	MX6SX_PAD_SD4_DATA5 = 165,
+	MX6SX_PAD_SD4_DATA6 = 166,
+	MX6SX_PAD_SD4_DATA7 = 167,
+	MX6SX_PAD_SD4_RESET_B = 168,
+	MX6SX_PAD_USB_H_DATA = 169,
+	MX6SX_PAD_USB_H_STROBE = 170,
+};
+
+/* Pad names for the pinmux subsystem */
+static const struct pinctrl_pin_desc imx6sx_pinctrl_pads[] = {
+	IMX_PINCTRL_PIN(MX6Sx_PAD_RESERVE0),
+	IMX_PINCTRL_PIN(MX6Sx_PAD_RESERVE1),
+	IMX_PINCTRL_PIN(MX6Sx_PAD_RESERVE2),
+	IMX_PINCTRL_PIN(MX6Sx_PAD_RESERVE3),
+	IMX_PINCTRL_PIN(MX6Sx_PAD_RESERVE4),
+	IMX_PINCTRL_PIN(MX6SX_PAD_GPIO1_IO00),
+	IMX_PINCTRL_PIN(MX6SX_PAD_GPIO1_IO01),
+	IMX_PINCTRL_PIN(MX6SX_PAD_GPIO1_IO02),
+	IMX_PINCTRL_PIN(MX6SX_PAD_GPIO1_IO03),
+	IMX_PINCTRL_PIN(MX6SX_PAD_GPIO1_IO04),
+	IMX_PINCTRL_PIN(MX6SX_PAD_GPIO1_IO05),
+	IMX_PINCTRL_PIN(MX6SX_PAD_GPIO1_IO06),
+	IMX_PINCTRL_PIN(MX6SX_PAD_GPIO1_IO07),
+	IMX_PINCTRL_PIN(MX6SX_PAD_GPIO1_IO08),
+	IMX_PINCTRL_PIN(MX6SX_PAD_GPIO1_IO09),
+	IMX_PINCTRL_PIN(MX6SX_PAD_GPIO1_IO10),
+	IMX_PINCTRL_PIN(MX6SX_PAD_GPIO1_IO11),
+	IMX_PINCTRL_PIN(MX6SX_PAD_GPIO1_IO12),
+	IMX_PINCTRL_PIN(MX6SX_PAD_GPIO1_IO13),
+	IMX_PINCTRL_PIN(MX6SX_PAD_CSI_DATA00),
+	IMX_PINCTRL_PIN(MX6SX_PAD_CSI_DATA01),
+	IMX_PINCTRL_PIN(MX6SX_PAD_CSI_DATA02),
+	IMX_PINCTRL_PIN(MX6SX_PAD_CSI_DATA03),
+	IMX_PINCTRL_PIN(MX6SX_PAD_CSI_DATA04),
+	IMX_PINCTRL_PIN(MX6SX_PAD_CSI_DATA05),
+	IMX_PINCTRL_PIN(MX6SX_PAD_CSI_DATA06),
+	IMX_PINCTRL_PIN(MX6SX_PAD_CSI_DATA07),
+	IMX_PINCTRL_PIN(MX6SX_PAD_CSI_HSYNC),
+	IMX_PINCTRL_PIN(MX6SX_PAD_CSI_MCLK),
+	IMX_PINCTRL_PIN(MX6SX_PAD_CSI_PIXCLK),
+	IMX_PINCTRL_PIN(MX6SX_PAD_CSI_VSYNC),
+	IMX_PINCTRL_PIN(MX6SX_PAD_ENET1_COL),
+	IMX_PINCTRL_PIN(MX6SX_PAD_ENET1_CRS),
+	IMX_PINCTRL_PIN(MX6SX_PAD_ENET1_MDC),
+	IMX_PINCTRL_PIN(MX6SX_PAD_ENET1_MDIO),
+	IMX_PINCTRL_PIN(MX6SX_PAD_ENET1_RX_CLK),
+	IMX_PINCTRL_PIN(MX6SX_PAD_ENET1_TX_CLK),
+	IMX_PINCTRL_PIN(MX6SX_PAD_ENET2_COL),
+	IMX_PINCTRL_PIN(MX6SX_PAD_ENET2_CRS),
+	IMX_PINCTRL_PIN(MX6SX_PAD_ENET2_RX_CLK),
+	IMX_PINCTRL_PIN(MX6SX_PAD_ENET2_TX_CLK),
+	IMX_PINCTRL_PIN(MX6SX_PAD_KEY_COL0),
+	IMX_PINCTRL_PIN(MX6SX_PAD_KEY_COL1),
+	IMX_PINCTRL_PIN(MX6SX_PAD_KEY_COL2),
+	IMX_PINCTRL_PIN(MX6SX_PAD_KEY_COL3),
+	IMX_PINCTRL_PIN(MX6SX_PAD_KEY_COL4),
+	IMX_PINCTRL_PIN(MX6SX_PAD_KEY_ROW0),
+	IMX_PINCTRL_PIN(MX6SX_PAD_KEY_ROW1),
+	IMX_PINCTRL_PIN(MX6SX_PAD_KEY_ROW2),
+	IMX_PINCTRL_PIN(MX6SX_PAD_KEY_ROW3),
+	IMX_PINCTRL_PIN(MX6SX_PAD_KEY_ROW4),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_CLK),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA00),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA01),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA02),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA03),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA04),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA05),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA06),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA07),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA08),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA09),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA10),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA11),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA12),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA13),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA14),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA15),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA16),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA17),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA18),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA19),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA20),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA21),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA22),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_DATA23),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_ENABLE),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_HSYNC),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_RESET),
+	IMX_PINCTRL_PIN(MX6SX_PAD_LCD1_VSYNC),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_ALE),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_CE0_B),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_CE1_B),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_CLE),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_DATA00),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_DATA01),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_DATA02),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_DATA03),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_DATA04),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_DATA05),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_DATA06),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_DATA07),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_RE_B),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_READY_B),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_WE_B),
+	IMX_PINCTRL_PIN(MX6SX_PAD_NAND_WP_B),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1A_DATA0),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1A_DATA1),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1A_DATA2),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1A_DATA3),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1A_DQS),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1A_SCLK),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1A_SS0_B),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1A_SS1_B),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1B_DATA0),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1B_DATA1),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1B_DATA2),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1B_DATA3),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1B_DQS),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1B_SCLK),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1B_SS0_B),
+	IMX_PINCTRL_PIN(MX6SX_PAD_QSPI1B_SS1_B),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII1_RD0),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII1_RD1),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII1_RD2),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII1_RD3),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII1_RX_CTL),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII1_RXC),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII1_TD0),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII1_TD1),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII1_TD2),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII1_TD3),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII1_TX_CTL),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII1_TXC),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII2_RD0),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII2_RD1),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII2_RD2),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII2_RD3),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII2_RX_CTL),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII2_RXC),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII2_TD0),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII2_TD1),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII2_TD2),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII2_TD3),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII2_TX_CTL),
+	IMX_PINCTRL_PIN(MX6SX_PAD_RGMII2_TXC),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD1_CLK),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD1_CMD),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD1_DATA0),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD1_DATA1),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD1_DATA2),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD1_DATA3),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD2_CLK),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD2_CMD),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD2_DATA0),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD2_DATA1),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD2_DATA2),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD2_DATA3),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD3_CLK),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD3_CMD),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD3_DATA0),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD3_DATA1),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD3_DATA2),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD3_DATA3),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD3_DATA4),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD3_DATA5),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD3_DATA6),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD3_DATA7),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD4_CLK),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD4_CMD),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD4_DATA0),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD4_DATA1),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD4_DATA2),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD4_DATA3),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD4_DATA4),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD4_DATA5),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD4_DATA6),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD4_DATA7),
+	IMX_PINCTRL_PIN(MX6SX_PAD_SD4_RESET_B),
+	IMX_PINCTRL_PIN(MX6SX_PAD_USB_H_DATA),
+	IMX_PINCTRL_PIN(MX6SX_PAD_USB_H_STROBE),
+};
+
+static struct imx_pinctrl_soc_info imx6sx_pinctrl_info = {
+	.pins = imx6sx_pinctrl_pads,
+	.npins = ARRAY_SIZE(imx6sx_pinctrl_pads),
+};
+
+static struct of_device_id imx6sx_pinctrl_of_match[] = {
+	{ .compatible = "fsl,imx6sx-iomuxc", },
+	{ /* sentinel */ }
+};
+
+static int imx6sx_pinctrl_probe(struct platform_device *pdev)
+{
+	return imx_pinctrl_probe(pdev, &imx6sx_pinctrl_info);
+}
+
+static struct platform_driver imx6sx_pinctrl_driver = {
+	.driver = {
+		.name = "imx6sx-pinctrl",
+		.owner = THIS_MODULE,
+		.of_match_table = of_match_ptr(imx6sx_pinctrl_of_match),
+	},
+	.probe = imx6sx_pinctrl_probe,
+	.remove = imx_pinctrl_remove,
+};
+
+static int __init imx6sx_pinctrl_init(void)
+{
+	return platform_driver_register(&imx6sx_pinctrl_driver);
+}
+arch_initcall(imx6sx_pinctrl_init);
+
+static void __exit imx6sx_pinctrl_exit(void)
+{
+	platform_driver_unregister(&imx6sx_pinctrl_driver);
+}
+module_exit(imx6sx_pinctrl_exit);
+
+MODULE_AUTHOR("Anson Huang <Anson.Huang@freescale.com>");
+MODULE_DESCRIPTION("Freescale imx6sx pinctrl driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pinctrl/pinctrl-ipq8064.c b/drivers/pinctrl/pinctrl-ipq8064.c
new file mode 100644
index 00000000000..acafea4c3a3
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-ipq8064.c
@@ -0,0 +1,653 @@
+/*
+ * Copyright (c) 2014, The Linux Foundation. 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 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.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-msm.h"
+
+static const struct pinctrl_pin_desc ipq8064_pins[] = {
+	PINCTRL_PIN(0, "GPIO_0"),
+	PINCTRL_PIN(1, "GPIO_1"),
+	PINCTRL_PIN(2, "GPIO_2"),
+	PINCTRL_PIN(3, "GPIO_3"),
+	PINCTRL_PIN(4, "GPIO_4"),
+	PINCTRL_PIN(5, "GPIO_5"),
+	PINCTRL_PIN(6, "GPIO_6"),
+	PINCTRL_PIN(7, "GPIO_7"),
+	PINCTRL_PIN(8, "GPIO_8"),
+	PINCTRL_PIN(9, "GPIO_9"),
+	PINCTRL_PIN(10, "GPIO_10"),
+	PINCTRL_PIN(11, "GPIO_11"),
+	PINCTRL_PIN(12, "GPIO_12"),
+	PINCTRL_PIN(13, "GPIO_13"),
+	PINCTRL_PIN(14, "GPIO_14"),
+	PINCTRL_PIN(15, "GPIO_15"),
+	PINCTRL_PIN(16, "GPIO_16"),
+	PINCTRL_PIN(17, "GPIO_17"),
+	PINCTRL_PIN(18, "GPIO_18"),
+	PINCTRL_PIN(19, "GPIO_19"),
+	PINCTRL_PIN(20, "GPIO_20"),
+	PINCTRL_PIN(21, "GPIO_21"),
+	PINCTRL_PIN(22, "GPIO_22"),
+	PINCTRL_PIN(23, "GPIO_23"),
+	PINCTRL_PIN(24, "GPIO_24"),
+	PINCTRL_PIN(25, "GPIO_25"),
+	PINCTRL_PIN(26, "GPIO_26"),
+	PINCTRL_PIN(27, "GPIO_27"),
+	PINCTRL_PIN(28, "GPIO_28"),
+	PINCTRL_PIN(29, "GPIO_29"),
+	PINCTRL_PIN(30, "GPIO_30"),
+	PINCTRL_PIN(31, "GPIO_31"),
+	PINCTRL_PIN(32, "GPIO_32"),
+	PINCTRL_PIN(33, "GPIO_33"),
+	PINCTRL_PIN(34, "GPIO_34"),
+	PINCTRL_PIN(35, "GPIO_35"),
+	PINCTRL_PIN(36, "GPIO_36"),
+	PINCTRL_PIN(37, "GPIO_37"),
+	PINCTRL_PIN(38, "GPIO_38"),
+	PINCTRL_PIN(39, "GPIO_39"),
+	PINCTRL_PIN(40, "GPIO_40"),
+	PINCTRL_PIN(41, "GPIO_41"),
+	PINCTRL_PIN(42, "GPIO_42"),
+	PINCTRL_PIN(43, "GPIO_43"),
+	PINCTRL_PIN(44, "GPIO_44"),
+	PINCTRL_PIN(45, "GPIO_45"),
+	PINCTRL_PIN(46, "GPIO_46"),
+	PINCTRL_PIN(47, "GPIO_47"),
+	PINCTRL_PIN(48, "GPIO_48"),
+	PINCTRL_PIN(49, "GPIO_49"),
+	PINCTRL_PIN(50, "GPIO_50"),
+	PINCTRL_PIN(51, "GPIO_51"),
+	PINCTRL_PIN(52, "GPIO_52"),
+	PINCTRL_PIN(53, "GPIO_53"),
+	PINCTRL_PIN(54, "GPIO_54"),
+	PINCTRL_PIN(55, "GPIO_55"),
+	PINCTRL_PIN(56, "GPIO_56"),
+	PINCTRL_PIN(57, "GPIO_57"),
+	PINCTRL_PIN(58, "GPIO_58"),
+	PINCTRL_PIN(59, "GPIO_59"),
+	PINCTRL_PIN(60, "GPIO_60"),
+	PINCTRL_PIN(61, "GPIO_61"),
+	PINCTRL_PIN(62, "GPIO_62"),
+	PINCTRL_PIN(63, "GPIO_63"),
+	PINCTRL_PIN(64, "GPIO_64"),
+	PINCTRL_PIN(65, "GPIO_65"),
+	PINCTRL_PIN(66, "GPIO_66"),
+	PINCTRL_PIN(67, "GPIO_67"),
+	PINCTRL_PIN(68, "GPIO_68"),
+
+	PINCTRL_PIN(69, "SDC3_CLK"),
+	PINCTRL_PIN(70, "SDC3_CMD"),
+	PINCTRL_PIN(71, "SDC3_DATA"),
+};
+
+#define DECLARE_IPQ_GPIO_PINS(pin) static const unsigned int gpio##pin##_pins[] = { pin }
+DECLARE_IPQ_GPIO_PINS(0);
+DECLARE_IPQ_GPIO_PINS(1);
+DECLARE_IPQ_GPIO_PINS(2);
+DECLARE_IPQ_GPIO_PINS(3);
+DECLARE_IPQ_GPIO_PINS(4);
+DECLARE_IPQ_GPIO_PINS(5);
+DECLARE_IPQ_GPIO_PINS(6);
+DECLARE_IPQ_GPIO_PINS(7);
+DECLARE_IPQ_GPIO_PINS(8);
+DECLARE_IPQ_GPIO_PINS(9);
+DECLARE_IPQ_GPIO_PINS(10);
+DECLARE_IPQ_GPIO_PINS(11);
+DECLARE_IPQ_GPIO_PINS(12);
+DECLARE_IPQ_GPIO_PINS(13);
+DECLARE_IPQ_GPIO_PINS(14);
+DECLARE_IPQ_GPIO_PINS(15);
+DECLARE_IPQ_GPIO_PINS(16);
+DECLARE_IPQ_GPIO_PINS(17);
+DECLARE_IPQ_GPIO_PINS(18);
+DECLARE_IPQ_GPIO_PINS(19);
+DECLARE_IPQ_GPIO_PINS(20);
+DECLARE_IPQ_GPIO_PINS(21);
+DECLARE_IPQ_GPIO_PINS(22);
+DECLARE_IPQ_GPIO_PINS(23);
+DECLARE_IPQ_GPIO_PINS(24);
+DECLARE_IPQ_GPIO_PINS(25);
+DECLARE_IPQ_GPIO_PINS(26);
+DECLARE_IPQ_GPIO_PINS(27);
+DECLARE_IPQ_GPIO_PINS(28);
+DECLARE_IPQ_GPIO_PINS(29);
+DECLARE_IPQ_GPIO_PINS(30);
+DECLARE_IPQ_GPIO_PINS(31);
+DECLARE_IPQ_GPIO_PINS(32);
+DECLARE_IPQ_GPIO_PINS(33);
+DECLARE_IPQ_GPIO_PINS(34);
+DECLARE_IPQ_GPIO_PINS(35);
+DECLARE_IPQ_GPIO_PINS(36);
+DECLARE_IPQ_GPIO_PINS(37);
+DECLARE_IPQ_GPIO_PINS(38);
+DECLARE_IPQ_GPIO_PINS(39);
+DECLARE_IPQ_GPIO_PINS(40);
+DECLARE_IPQ_GPIO_PINS(41);
+DECLARE_IPQ_GPIO_PINS(42);
+DECLARE_IPQ_GPIO_PINS(43);
+DECLARE_IPQ_GPIO_PINS(44);
+DECLARE_IPQ_GPIO_PINS(45);
+DECLARE_IPQ_GPIO_PINS(46);
+DECLARE_IPQ_GPIO_PINS(47);
+DECLARE_IPQ_GPIO_PINS(48);
+DECLARE_IPQ_GPIO_PINS(49);
+DECLARE_IPQ_GPIO_PINS(50);
+DECLARE_IPQ_GPIO_PINS(51);
+DECLARE_IPQ_GPIO_PINS(52);
+DECLARE_IPQ_GPIO_PINS(53);
+DECLARE_IPQ_GPIO_PINS(54);
+DECLARE_IPQ_GPIO_PINS(55);
+DECLARE_IPQ_GPIO_PINS(56);
+DECLARE_IPQ_GPIO_PINS(57);
+DECLARE_IPQ_GPIO_PINS(58);
+DECLARE_IPQ_GPIO_PINS(59);
+DECLARE_IPQ_GPIO_PINS(60);
+DECLARE_IPQ_GPIO_PINS(61);
+DECLARE_IPQ_GPIO_PINS(62);
+DECLARE_IPQ_GPIO_PINS(63);
+DECLARE_IPQ_GPIO_PINS(64);
+DECLARE_IPQ_GPIO_PINS(65);
+DECLARE_IPQ_GPIO_PINS(66);
+DECLARE_IPQ_GPIO_PINS(67);
+DECLARE_IPQ_GPIO_PINS(68);
+
+static const unsigned int sdc3_clk_pins[] = { 69 };
+static const unsigned int sdc3_cmd_pins[] = { 70 };
+static const unsigned int sdc3_data_pins[] = { 71 };
+
+#define FUNCTION(fname)					\
+	[IPQ_MUX_##fname] = {				\
+		.name = #fname,				\
+		.groups = fname##_groups,		\
+		.ngroups = ARRAY_SIZE(fname##_groups),	\
+	}
+
+#define PINGROUP(id, f1, f2, f3, f4, f5, f6, f7, f8, f9, f10) \
+	{						\
+		.name = "gpio" #id,			\
+		.pins = gpio##id##_pins,		\
+		.npins = ARRAY_SIZE(gpio##id##_pins),	\
+		.funcs = (int[]){			\
+			IPQ_MUX_NA, /* gpio mode */	\
+			IPQ_MUX_##f1,			\
+			IPQ_MUX_##f2,			\
+			IPQ_MUX_##f3,			\
+			IPQ_MUX_##f4,			\
+			IPQ_MUX_##f5,			\
+			IPQ_MUX_##f6,			\
+			IPQ_MUX_##f7,			\
+			IPQ_MUX_##f8,			\
+			IPQ_MUX_##f9,			\
+			IPQ_MUX_##f10,			\
+		},					\
+		.nfuncs = 11,				\
+		.ctl_reg = 0x1000 + 0x10 * id,		\
+		.io_reg = 0x1004 + 0x10 * id,		\
+		.intr_cfg_reg = 0x1008 + 0x10 * id,	\
+		.intr_status_reg = 0x100c + 0x10 * id,	\
+		.intr_target_reg = 0x400 + 0x4 * id,	\
+		.mux_bit = 2,				\
+		.pull_bit = 0,				\
+		.drv_bit = 6,				\
+		.oe_bit = 9,				\
+		.in_bit = 0,				\
+		.out_bit = 1,				\
+		.intr_enable_bit = 0,			\
+		.intr_status_bit = 0,			\
+		.intr_ack_high = 1,			\
+		.intr_target_bit = 0,			\
+		.intr_raw_status_bit = 3,		\
+		.intr_polarity_bit = 1,			\
+		.intr_detection_bit = 2,		\
+		.intr_detection_width = 1,		\
+	}
+
+#define SDC_PINGROUP(pg_name, ctl, pull, drv)		\
+	{						\
+		.name = #pg_name,	                \
+		.pins = pg_name##_pins,                 \
+		.npins = ARRAY_SIZE(pg_name##_pins),    \
+		.ctl_reg = ctl,                         \
+		.io_reg = 0,                            \
+		.intr_cfg_reg = 0,                      \
+		.intr_status_reg = 0,                   \
+		.intr_target_reg = 0,                   \
+		.mux_bit = -1,                          \
+		.pull_bit = pull,                       \
+		.drv_bit = drv,                         \
+		.oe_bit = -1,                           \
+		.in_bit = -1,                           \
+		.out_bit = -1,                          \
+		.intr_enable_bit = -1,                  \
+		.intr_status_bit = -1,                  \
+		.intr_target_bit = -1,                  \
+		.intr_raw_status_bit = -1,              \
+		.intr_polarity_bit = -1,                \
+		.intr_detection_bit = -1,               \
+		.intr_detection_width = -1,             \
+	}
+
+enum ipq8064_functions {
+	IPQ_MUX_mdio,
+	IPQ_MUX_mi2s,
+	IPQ_MUX_pdm,
+	IPQ_MUX_ssbi,
+	IPQ_MUX_spmi,
+	IPQ_MUX_audio_pcm,
+	IPQ_MUX_gsbi1,
+	IPQ_MUX_gsbi2,
+	IPQ_MUX_gsbi4,
+	IPQ_MUX_gsbi5,
+	IPQ_MUX_gsbi5_spi_cs1,
+	IPQ_MUX_gsbi5_spi_cs2,
+	IPQ_MUX_gsbi5_spi_cs3,
+	IPQ_MUX_gsbi6,
+	IPQ_MUX_gsbi7,
+	IPQ_MUX_nss_spi,
+	IPQ_MUX_sdc1,
+	IPQ_MUX_spdif,
+	IPQ_MUX_nand,
+	IPQ_MUX_tsif1,
+	IPQ_MUX_tsif2,
+	IPQ_MUX_usb_fs_n,
+	IPQ_MUX_usb_fs,
+	IPQ_MUX_usb2_hsic,
+	IPQ_MUX_rgmii2,
+	IPQ_MUX_sata,
+	IPQ_MUX_pcie1_rst,
+	IPQ_MUX_pcie1_prsnt,
+	IPQ_MUX_pcie1_pwrflt,
+	IPQ_MUX_pcie1_pwren_n,
+	IPQ_MUX_pcie1_pwren,
+	IPQ_MUX_pcie1_clk_req,
+	IPQ_MUX_pcie2_rst,
+	IPQ_MUX_pcie2_prsnt,
+	IPQ_MUX_pcie2_pwrflt,
+	IPQ_MUX_pcie2_pwren_n,
+	IPQ_MUX_pcie2_pwren,
+	IPQ_MUX_pcie2_clk_req,
+	IPQ_MUX_pcie3_rst,
+	IPQ_MUX_pcie3_prsnt,
+	IPQ_MUX_pcie3_pwrflt,
+	IPQ_MUX_pcie3_pwren_n,
+	IPQ_MUX_pcie3_pwren,
+	IPQ_MUX_pcie3_clk_req,
+	IPQ_MUX_ps_hold,
+	IPQ_MUX_NA,
+};
+
+static const char * const mdio_groups[] = {
+	"gpio0", "gpio1", "gpio10", "gpio11",
+};
+
+static const char * const mi2s_groups[] = {
+	"gpio27", "gpio28", "gpio29", "gpio30", "gpio31", "gpio32",
+	"gpio33", "gpio55", "gpio56", "gpio57", "gpio58",
+};
+
+static const char * const pdm_groups[] = {
+	"gpio3", "gpio16", "gpio17", "gpio22", "gpio30", "gpio31",
+	"gpio34", "gpio35", "gpio52", "gpio55", "gpio56", "gpio58",
+	"gpio59",
+};
+
+static const char * const ssbi_groups[] = {
+	"gpio10", "gpio11",
+};
+
+static const char * const spmi_groups[] = {
+	"gpio10", "gpio11",
+};
+
+static const char * const audio_pcm_groups[] = {
+	"gpio14", "gpio15", "gpio16", "gpio17",
+};
+
+static const char * const gsbi1_groups[] = {
+	"gpio51", "gpio52", "gpio53", "gpio54",
+};
+
+static const char * const gsbi2_groups[] = {
+	"gpio22", "gpio23", "gpio24", "gpio25",
+};
+
+static const char * const gsbi4_groups[] = {
+	"gpio10", "gpio11", "gpio12", "gpio13",
+};
+
+static const char * const gsbi5_groups[] = {
+	"gpio18", "gpio19", "gpio20", "gpio21",
+};
+
+static const char * const gsbi5_spi_cs1_groups[] = {
+	"gpio6", "gpio61",
+};
+
+static const char * const gsbi5_spi_cs2_groups[] = {
+	"gpio7", "gpio62",
+};
+
+static const char * const gsbi5_spi_cs3_groups[] = {
+	"gpio2",
+};
+
+static const char * const gsbi6_groups[] = {
+	"gpio27", "gpio28", "gpio29", "gpio30", "gpio55", "gpio56",
+	"gpio57", "gpio58",
+};
+
+static const char * const gsbi7_groups[] = {
+	"gpio6", "gpio7", "gpio8", "gpio9",
+};
+
+static const char * const nss_spi_groups[] = {
+	"gpio14", "gpio15", "gpio16", "gpio17", "gpio55", "gpio56",
+	"gpio57", "gpio58",
+};
+
+static const char * const sdc1_groups[] = {
+	"gpio38", "gpio39", "gpio40", "gpio41", "gpio42", "gpio43",
+	"gpio44", "gpio45", "gpio46", "gpio47",
+};
+
+static const char * const spdif_groups[] = {
+	"gpio10", "gpio48",
+};
+
+static const char * const nand_groups[] = {
+	"gpio34", "gpio35", "gpio36", "gpio37", "gpio38", "gpio39",
+	"gpio40", "gpio41", "gpio42", "gpio43", "gpio44", "gpio45",
+	"gpio46", "gpio47",
+};
+
+static const char * const tsif1_groups[] = {
+	"gpio55", "gpio56", "gpio57", "gpio58",
+};
+
+static const char * const tsif2_groups[] = {
+	"gpio59", "gpio60", "gpio61", "gpio62",
+};
+
+static const char * const usb_fs_n_groups[] = {
+	"gpio6",
+};
+
+static const char * const usb_fs_groups[] = {
+	"gpio6", "gpio7", "gpio8",
+};
+
+static const char * const usb2_hsic_groups[] = {
+	"gpio67", "gpio68",
+};
+
+static const char * const rgmii2_groups[] = {
+	"gpio27", "gpio28", "gpio29", "gpio30", "gpio31", "gpio32",
+	"gpio51", "gpio52", "gpio59", "gpio60", "gpio61", "gpio62",
+};
+
+static const char * const sata_groups[] = {
+	"gpio10",
+};
+
+static const char * const pcie1_rst_groups[] = {
+	"gpio3",
+};
+
+static const char * const pcie1_prsnt_groups[] = {
+	"gpio3", "gpio11",
+};
+
+static const char * const pcie1_pwren_n_groups[] = {
+	"gpio4", "gpio12",
+};
+
+static const char * const pcie1_pwren_groups[] = {
+	"gpio4", "gpio12",
+};
+
+static const char * const pcie1_pwrflt_groups[] = {
+	"gpio5", "gpio13",
+};
+
+static const char * const pcie1_clk_req_groups[] = {
+	"gpio5",
+};
+
+static const char * const pcie2_rst_groups[] = {
+	"gpio48",
+};
+
+static const char * const pcie2_prsnt_groups[] = {
+	"gpio11", "gpio48",
+};
+
+static const char * const pcie2_pwren_n_groups[] = {
+	"gpio12", "gpio49",
+};
+
+static const char * const pcie2_pwren_groups[] = {
+	"gpio12", "gpio49",
+};
+
+static const char * const pcie2_pwrflt_groups[] = {
+	"gpio13", "gpio50",
+};
+
+static const char * const pcie2_clk_req_groups[] = {
+	"gpio50",
+};
+
+static const char * const pcie3_rst_groups[] = {
+	"gpio63",
+};
+
+static const char * const pcie3_prsnt_groups[] = {
+	"gpio11",
+};
+
+static const char * const pcie3_pwren_n_groups[] = {
+	"gpio12",
+};
+
+static const char * const pcie3_pwren_groups[] = {
+	"gpio12",
+};
+
+static const char * const pcie3_pwrflt_groups[] = {
+	"gpio13",
+};
+
+static const char * const pcie3_clk_req_groups[] = {
+	"gpio65",
+};
+
+static const char * const ps_hold_groups[] = {
+	"gpio26",
+};
+
+static const struct msm_function ipq8064_functions[] = {
+	FUNCTION(mdio),
+	FUNCTION(ssbi),
+	FUNCTION(spmi),
+	FUNCTION(mi2s),
+	FUNCTION(pdm),
+	FUNCTION(audio_pcm),
+	FUNCTION(gsbi1),
+	FUNCTION(gsbi2),
+	FUNCTION(gsbi4),
+	FUNCTION(gsbi5),
+	FUNCTION(gsbi5_spi_cs1),
+	FUNCTION(gsbi5_spi_cs2),
+	FUNCTION(gsbi5_spi_cs3),
+	FUNCTION(gsbi6),
+	FUNCTION(gsbi7),
+	FUNCTION(nss_spi),
+	FUNCTION(sdc1),
+	FUNCTION(spdif),
+	FUNCTION(nand),
+	FUNCTION(tsif1),
+	FUNCTION(tsif2),
+	FUNCTION(usb_fs_n),
+	FUNCTION(usb_fs),
+	FUNCTION(usb2_hsic),
+	FUNCTION(rgmii2),
+	FUNCTION(sata),
+	FUNCTION(pcie1_rst),
+	FUNCTION(pcie1_prsnt),
+	FUNCTION(pcie1_pwren_n),
+	FUNCTION(pcie1_pwren),
+	FUNCTION(pcie1_pwrflt),
+	FUNCTION(pcie1_clk_req),
+	FUNCTION(pcie2_rst),
+	FUNCTION(pcie2_prsnt),
+	FUNCTION(pcie2_pwren_n),
+	FUNCTION(pcie2_pwren),
+	FUNCTION(pcie2_pwrflt),
+	FUNCTION(pcie2_clk_req),
+	FUNCTION(pcie3_rst),
+	FUNCTION(pcie3_prsnt),
+	FUNCTION(pcie3_pwren_n),
+	FUNCTION(pcie3_pwren),
+	FUNCTION(pcie3_pwrflt),
+	FUNCTION(pcie3_clk_req),
+	FUNCTION(ps_hold),
+};
+
+static const struct msm_pingroup ipq8064_groups[] = {
+	PINGROUP(0, mdio, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(1, mdio, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(2, gsbi5_spi_cs3, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(3, pcie1_rst, pcie1_prsnt, pdm, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(4, pcie1_pwren_n, pcie1_pwren, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(5, pcie1_clk_req, pcie1_pwrflt, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(6, gsbi7, usb_fs, gsbi5_spi_cs1, usb_fs_n, NA, NA, NA, NA, NA, NA),
+	PINGROUP(7, gsbi7, usb_fs, gsbi5_spi_cs2, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(8, gsbi7, usb_fs, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(9, gsbi7, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(10, gsbi4, spdif, sata, ssbi, mdio, spmi, NA, NA, NA, NA),
+	PINGROUP(11, gsbi4, pcie2_prsnt, pcie1_prsnt, pcie3_prsnt, ssbi, mdio, spmi, NA, NA, NA),
+	PINGROUP(12, gsbi4, pcie2_pwren_n, pcie1_pwren_n, pcie3_pwren_n, pcie2_pwren, pcie1_pwren, pcie3_pwren, NA, NA, NA),
+	PINGROUP(13, gsbi4, pcie2_pwrflt, pcie1_pwrflt, pcie3_pwrflt, NA, NA, NA, NA, NA, NA),
+	PINGROUP(14, audio_pcm, nss_spi, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(15, audio_pcm, nss_spi, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(16, audio_pcm, nss_spi, pdm, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(17, audio_pcm, nss_spi, pdm, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(18, gsbi5, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(19, gsbi5, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(20, gsbi5, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(21, gsbi5, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(22, gsbi2, pdm, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(23, gsbi2, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(24, gsbi2, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(25, gsbi2, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(26, ps_hold, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(27, mi2s, rgmii2, gsbi6, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(28, mi2s, rgmii2, gsbi6, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(29, mi2s, rgmii2, gsbi6, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(30, mi2s, rgmii2, gsbi6, pdm, NA, NA, NA, NA, NA, NA),
+	PINGROUP(31, mi2s, rgmii2, pdm, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(32, mi2s, rgmii2, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(33, mi2s, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(34, nand, pdm, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(35, nand, pdm, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(36, nand, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(37, nand, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(38, nand, sdc1, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(39, nand, sdc1, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(40, nand, sdc1, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(41, nand, sdc1, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(42, nand, sdc1, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(43, nand, sdc1, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(44, nand, sdc1, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(45, nand, sdc1, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(46, nand, sdc1, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(47, nand, sdc1, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(48, pcie2_rst, spdif, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(49, pcie2_pwren_n, pcie2_pwren, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(50, pcie2_clk_req, pcie2_pwrflt, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(51, gsbi1, rgmii2, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(52, gsbi1, rgmii2, pdm, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(53, gsbi1, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(54, gsbi1, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(55, tsif1, mi2s, gsbi6, pdm, nss_spi, NA, NA, NA, NA, NA),
+	PINGROUP(56, tsif1, mi2s, gsbi6, pdm, nss_spi, NA, NA, NA, NA, NA),
+	PINGROUP(57, tsif1, mi2s, gsbi6, nss_spi, NA, NA, NA, NA, NA, NA),
+	PINGROUP(58, tsif1, mi2s, gsbi6, pdm, nss_spi, NA, NA, NA, NA, NA),
+	PINGROUP(59, tsif2, rgmii2, pdm, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(60, tsif2, rgmii2, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(61, tsif2, rgmii2, gsbi5_spi_cs1, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(62, tsif2, rgmii2, gsbi5_spi_cs2, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(63, pcie3_rst, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(64, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(65, pcie3_clk_req, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(66, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(67, usb2_hsic, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(68, usb2_hsic, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	SDC_PINGROUP(sdc3_clk, 0x204a, 14, 6),
+	SDC_PINGROUP(sdc3_cmd, 0x204a, 11, 3),
+	SDC_PINGROUP(sdc3_data, 0x204a, 9, 0),
+};
+
+#define NUM_GPIO_PINGROUPS 69
+
+static const struct msm_pinctrl_soc_data ipq8064_pinctrl = {
+	.pins = ipq8064_pins,
+	.npins = ARRAY_SIZE(ipq8064_pins),
+	.functions = ipq8064_functions,
+	.nfunctions = ARRAY_SIZE(ipq8064_functions),
+	.groups = ipq8064_groups,
+	.ngroups = ARRAY_SIZE(ipq8064_groups),
+	.ngpios = NUM_GPIO_PINGROUPS,
+};
+
+static int ipq8064_pinctrl_probe(struct platform_device *pdev)
+{
+	return msm_pinctrl_probe(pdev, &ipq8064_pinctrl);
+}
+
+static const struct of_device_id ipq8064_pinctrl_of_match[] = {
+	{ .compatible = "qcom,ipq8064-pinctrl", },
+	{ },
+};
+
+static struct platform_driver ipq8064_pinctrl_driver = {
+	.driver = {
+		.name = "ipq8064-pinctrl",
+		.owner = THIS_MODULE,
+		.of_match_table = ipq8064_pinctrl_of_match,
+	},
+	.probe = ipq8064_pinctrl_probe,
+	.remove = msm_pinctrl_remove,
+};
+
+static int __init ipq8064_pinctrl_init(void)
+{
+	return platform_driver_register(&ipq8064_pinctrl_driver);
+}
+arch_initcall(ipq8064_pinctrl_init);
+
+static void __exit ipq8064_pinctrl_exit(void)
+{
+	platform_driver_unregister(&ipq8064_pinctrl_driver);
+}
+module_exit(ipq8064_pinctrl_exit);
+
+MODULE_AUTHOR("Andy Gross <agross@codeaurora.org>");
+MODULE_DESCRIPTION("Qualcomm IPQ8064 pinctrl driver");
+MODULE_LICENSE("GPL v2");
+MODULE_DEVICE_TABLE(of, ipq8064_pinctrl_of_match);
diff --git a/drivers/pinctrl/pinctrl-lantiq.h b/drivers/pinctrl/pinctrl-lantiq.h
index 6d07f023853..c7cfad5527d 100644
--- a/drivers/pinctrl/pinctrl-lantiq.h
+++ b/drivers/pinctrl/pinctrl-lantiq.h
@@ -10,6 +10,7 @@
  */
 
 #ifndef __PINCTRL_LANTIQ_H
+#define __PINCTRL_LANTIQ_H
 
 #include <linux/clkdev.h>
 #include <linux/pinctrl/pinctrl.h>
diff --git a/drivers/pinctrl/pinctrl-msm.c b/drivers/pinctrl/pinctrl-msm.c
index e43fbce5659..df6dda4ce80 100644
--- a/drivers/pinctrl/pinctrl-msm.c
+++ b/drivers/pinctrl/pinctrl-msm.c
@@ -13,7 +13,6 @@
  */
 
 #include <linux/err.h>
-#include <linux/irqdomain.h>
 #include <linux/io.h>
 #include <linux/module.h>
 #include <linux/of.h>
@@ -26,8 +25,6 @@
 #include <linux/slab.h>
 #include <linux/gpio.h>
 #include <linux/interrupt.h>
-#include <linux/irq.h>
-#include <linux/irqchip/chained_irq.h>
 #include <linux/spinlock.h>
 
 #include "core.h"
@@ -41,7 +38,6 @@
  * struct msm_pinctrl - state for a pinctrl-msm device
  * @dev:            device handle.
  * @pctrl:          pinctrl handle.
- * @domain:         irqdomain handle.
  * @chip:           gpiochip handle.
  * @irq:            parent irq for the TLMM irq_chip.
  * @lock:           Spinlock to protect register resources as well
@@ -55,7 +51,6 @@
 struct msm_pinctrl {
 	struct device *dev;
 	struct pinctrl_dev *pctrl;
-	struct irq_domain *domain;
 	struct gpio_chip chip;
 	int irq;
 
@@ -68,6 +63,11 @@ struct msm_pinctrl {
 	void __iomem *regs;
 };
 
+static inline struct msm_pinctrl *to_msm_pinctrl(struct gpio_chip *gc)
+{
+	return container_of(gc, struct msm_pinctrl, chip);
+}
+
 static int msm_get_groups_count(struct pinctrl_dev *pctldev)
 {
 	struct msm_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctldev);
@@ -145,12 +145,12 @@ static int msm_pinmux_enable(struct pinctrl_dev *pctldev,
 	if (WARN_ON(g->mux_bit < 0))
 		return -EINVAL;
 
-	for (i = 0; i < ARRAY_SIZE(g->funcs); i++) {
+	for (i = 0; i < g->nfuncs; i++) {
 		if (g->funcs[i] == function)
 			break;
 	}
 
-	if (WARN_ON(i == ARRAY_SIZE(g->funcs)))
+	if (WARN_ON(i == g->nfuncs))
 		return -EINVAL;
 
 	spin_lock_irqsave(&pctrl->lock, flags);
@@ -480,13 +480,6 @@ static void msm_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 	spin_unlock_irqrestore(&pctrl->lock, flags);
 }
 
-static int msm_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
-{
-	struct msm_pinctrl *pctrl = container_of(chip, struct msm_pinctrl, chip);
-
-	return irq_find_mapping(pctrl->domain, offset);
-}
-
 static int msm_gpio_request(struct gpio_chip *chip, unsigned offset)
 {
 	int gpio = chip->base + offset;
@@ -556,7 +549,6 @@ static struct gpio_chip msm_gpio_template = {
 	.direction_output = msm_gpio_direction_output,
 	.get              = msm_gpio_get,
 	.set              = msm_gpio_set,
-	.to_irq           = msm_gpio_to_irq,
 	.request          = msm_gpio_request,
 	.free             = msm_gpio_free,
 	.dbg_show         = msm_gpio_dbg_show,
@@ -608,12 +600,12 @@ static void msm_gpio_update_dual_edge_pos(struct msm_pinctrl *pctrl,
 
 static void msm_gpio_irq_mask(struct irq_data *d)
 {
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct msm_pinctrl *pctrl = to_msm_pinctrl(gc);
 	const struct msm_pingroup *g;
-	struct msm_pinctrl *pctrl;
 	unsigned long flags;
 	u32 val;
 
-	pctrl = irq_data_get_irq_chip_data(d);
 	g = &pctrl->soc->groups[d->hwirq];
 
 	spin_lock_irqsave(&pctrl->lock, flags);
@@ -629,12 +621,12 @@ static void msm_gpio_irq_mask(struct irq_data *d)
 
 static void msm_gpio_irq_unmask(struct irq_data *d)
 {
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct msm_pinctrl *pctrl = to_msm_pinctrl(gc);
 	const struct msm_pingroup *g;
-	struct msm_pinctrl *pctrl;
 	unsigned long flags;
 	u32 val;
 
-	pctrl = irq_data_get_irq_chip_data(d);
 	g = &pctrl->soc->groups[d->hwirq];
 
 	spin_lock_irqsave(&pctrl->lock, flags);
@@ -654,12 +646,12 @@ static void msm_gpio_irq_unmask(struct irq_data *d)
 
 static void msm_gpio_irq_ack(struct irq_data *d)
 {
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct msm_pinctrl *pctrl = to_msm_pinctrl(gc);
 	const struct msm_pingroup *g;
-	struct msm_pinctrl *pctrl;
 	unsigned long flags;
 	u32 val;
 
-	pctrl = irq_data_get_irq_chip_data(d);
 	g = &pctrl->soc->groups[d->hwirq];
 
 	spin_lock_irqsave(&pctrl->lock, flags);
@@ -681,12 +673,12 @@ static void msm_gpio_irq_ack(struct irq_data *d)
 
 static int msm_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 {
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct msm_pinctrl *pctrl = to_msm_pinctrl(gc);
 	const struct msm_pingroup *g;
-	struct msm_pinctrl *pctrl;
 	unsigned long flags;
 	u32 val;
 
-	pctrl = irq_data_get_irq_chip_data(d);
 	g = &pctrl->soc->groups[d->hwirq];
 
 	spin_lock_irqsave(&pctrl->lock, flags);
@@ -775,11 +767,10 @@ static int msm_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 
 static int msm_gpio_irq_set_wake(struct irq_data *d, unsigned int on)
 {
-	struct msm_pinctrl *pctrl;
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct msm_pinctrl *pctrl = to_msm_pinctrl(gc);
 	unsigned long flags;
 
-	pctrl = irq_data_get_irq_chip_data(d);
-
 	spin_lock_irqsave(&pctrl->lock, flags);
 
 	irq_set_irq_wake(pctrl->irq, on);
@@ -789,25 +780,6 @@ static int msm_gpio_irq_set_wake(struct irq_data *d, unsigned int on)
 	return 0;
 }
 
-static int msm_gpio_irq_reqres(struct irq_data *d)
-{
-	struct msm_pinctrl *pctrl = irq_data_get_irq_chip_data(d);
-
-	if (gpio_lock_as_irq(&pctrl->chip, d->hwirq)) {
-		dev_err(pctrl->dev, "unable to lock HW IRQ %lu for IRQ\n",
-			d->hwirq);
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static void msm_gpio_irq_relres(struct irq_data *d)
-{
-	struct msm_pinctrl *pctrl = irq_data_get_irq_chip_data(d);
-
-	gpio_unlock_as_irq(&pctrl->chip, d->hwirq);
-}
-
 static struct irq_chip msm_gpio_irq_chip = {
 	.name           = "msmgpio",
 	.irq_mask       = msm_gpio_irq_mask,
@@ -815,14 +787,13 @@ static struct irq_chip msm_gpio_irq_chip = {
 	.irq_ack        = msm_gpio_irq_ack,
 	.irq_set_type   = msm_gpio_irq_set_type,
 	.irq_set_wake   = msm_gpio_irq_set_wake,
-	.irq_request_resources = msm_gpio_irq_reqres,
-	.irq_release_resources = msm_gpio_irq_relres,
 };
 
 static void msm_gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
 {
+	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
 	const struct msm_pingroup *g;
-	struct msm_pinctrl *pctrl = irq_desc_get_handler_data(desc);
+	struct msm_pinctrl *pctrl = to_msm_pinctrl(gc);
 	struct irq_chip *chip = irq_get_chip(irq);
 	int irq_pin;
 	int handled = 0;
@@ -839,7 +810,7 @@ static void msm_gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
 		g = &pctrl->soc->groups[i];
 		val = readl(pctrl->regs + g->intr_status_reg);
 		if (val & BIT(g->intr_status_bit)) {
-			irq_pin = irq_find_mapping(pctrl->domain, i);
+			irq_pin = irq_find_mapping(gc->irqdomain, i);
 			generic_handle_irq(irq_pin);
 			handled++;
 		}
@@ -852,19 +823,10 @@ static void msm_gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
 	chained_irq_exit(chip, desc);
 }
 
-/*
- * This lock class tells lockdep that GPIO irqs are in a different
- * category than their parents, so it won't report false recursion.
- */
-static struct lock_class_key gpio_lock_class;
-
 static int msm_gpio_init(struct msm_pinctrl *pctrl)
 {
 	struct gpio_chip *chip;
-	int irq;
 	int ret;
-	int i;
-	int r;
 	unsigned ngpio = pctrl->soc->ngpios;
 
 	if (WARN_ON(ngpio > MAX_NR_GPIO))
@@ -890,23 +852,18 @@ static int msm_gpio_init(struct msm_pinctrl *pctrl)
 		return ret;
 	}
 
-	pctrl->domain = irq_domain_add_linear(pctrl->dev->of_node, chip->ngpio,
-					      &irq_domain_simple_ops, NULL);
-	if (!pctrl->domain) {
-		dev_err(pctrl->dev, "Failed to register irq domain\n");
-		r = gpiochip_remove(&pctrl->chip);
+	ret = gpiochip_irqchip_add(chip,
+				   &msm_gpio_irq_chip,
+				   0,
+				   handle_edge_irq,
+				   IRQ_TYPE_NONE);
+	if (ret) {
+		dev_err(pctrl->dev, "Failed to add irqchip to gpiochip\n");
 		return -ENOSYS;
 	}
 
-	for (i = 0; i < chip->ngpio; i++) {
-		irq = irq_create_mapping(pctrl->domain, i);
-		irq_set_lockdep_class(irq, &gpio_lock_class);
-		irq_set_chip_and_handler(irq, &msm_gpio_irq_chip, handle_edge_irq);
-		irq_set_chip_data(irq, pctrl);
-	}
-
-	irq_set_handler_data(pctrl->irq, pctrl);
-	irq_set_chained_handler(pctrl->irq, msm_gpio_irq_handler);
+	gpiochip_set_chained_irqchip(chip, &msm_gpio_irq_chip, pctrl->irq,
+				     msm_gpio_irq_handler);
 
 	return 0;
 }
@@ -974,8 +931,6 @@ int msm_pinctrl_remove(struct platform_device *pdev)
 		return ret;
 	}
 
-	irq_set_chained_handler(pctrl->irq, NULL);
-	irq_domain_remove(pctrl->domain);
 	pinctrl_unregister(pctrl->pctrl);
 
 	return 0;
diff --git a/drivers/pinctrl/pinctrl-msm.h b/drivers/pinctrl/pinctrl-msm.h
index 6e26f1b676d..7b2a227a590 100644
--- a/drivers/pinctrl/pinctrl-msm.h
+++ b/drivers/pinctrl/pinctrl-msm.h
@@ -65,7 +65,8 @@ struct msm_pingroup {
 	const unsigned *pins;
 	unsigned npins;
 
-	unsigned funcs[8];
+	unsigned *funcs;
+	unsigned nfuncs;
 
 	s16 ctl_reg;
 	s16 io_reg;
diff --git a/drivers/pinctrl/pinctrl-msm8x74.c b/drivers/pinctrl/pinctrl-msm8x74.c
index dde5529807a..418306911a6 100644
--- a/drivers/pinctrl/pinctrl-msm8x74.c
+++ b/drivers/pinctrl/pinctrl-msm8x74.c
@@ -341,7 +341,7 @@ static const unsigned int sdc2_data_pins[] = { 151 };
 		.name = "gpio" #id,			\
 		.pins = gpio##id##_pins,		\
 		.npins = ARRAY_SIZE(gpio##id##_pins),	\
-		.funcs = {				\
+		.funcs = (int[]){			\
 			MSM_MUX_NA, /* gpio mode */	\
 			MSM_MUX_##f1,			\
 			MSM_MUX_##f2,			\
@@ -351,6 +351,7 @@ static const unsigned int sdc2_data_pins[] = { 151 };
 			MSM_MUX_##f6,			\
 			MSM_MUX_##f7			\
 		},					\
+		.nfuncs = 8,				\
 		.ctl_reg = 0x1000 + 0x10 * id,		\
 		.io_reg = 0x1004 + 0x10 * id,		\
 		.intr_cfg_reg = 0x1008 + 0x10 * id,	\
@@ -401,169 +402,568 @@ static const unsigned int sdc2_data_pins[] = { 151 };
  * the pingroup table below.
  */
 enum msm8x74_functions {
+	MSM_MUX_cci_i2c0,
+	MSM_MUX_cci_i2c1,
+	MSM_MUX_blsp_i2c1,
 	MSM_MUX_blsp_i2c2,
+	MSM_MUX_blsp_i2c3,
+	MSM_MUX_blsp_i2c4,
+	MSM_MUX_blsp_i2c5,
 	MSM_MUX_blsp_i2c6,
+	MSM_MUX_blsp_i2c7,
+	MSM_MUX_blsp_i2c8,
+	MSM_MUX_blsp_i2c9,
+	MSM_MUX_blsp_i2c10,
 	MSM_MUX_blsp_i2c11,
+	MSM_MUX_blsp_i2c12,
 	MSM_MUX_blsp_spi1,
+	MSM_MUX_blsp_spi1_cs1,
+	MSM_MUX_blsp_spi1_cs2,
+	MSM_MUX_blsp_spi1_cs3,
+	MSM_MUX_blsp_spi2,
+	MSM_MUX_blsp_spi2_cs1,
+	MSM_MUX_blsp_spi2_cs2,
+	MSM_MUX_blsp_spi2_cs3,
+	MSM_MUX_blsp_spi3,
+	MSM_MUX_blsp_spi4,
+	MSM_MUX_blsp_spi5,
+	MSM_MUX_blsp_spi6,
+	MSM_MUX_blsp_spi7,
 	MSM_MUX_blsp_spi8,
+	MSM_MUX_blsp_spi9,
+	MSM_MUX_blsp_spi10,
+	MSM_MUX_blsp_spi10_cs1,
+	MSM_MUX_blsp_spi10_cs2,
+	MSM_MUX_blsp_spi10_cs3,
+	MSM_MUX_blsp_spi11,
+	MSM_MUX_blsp_spi12,
+	MSM_MUX_blsp_uart1,
 	MSM_MUX_blsp_uart2,
+	MSM_MUX_blsp_uart3,
+	MSM_MUX_blsp_uart4,
+	MSM_MUX_blsp_uart5,
+	MSM_MUX_blsp_uart6,
+	MSM_MUX_blsp_uart7,
 	MSM_MUX_blsp_uart8,
+	MSM_MUX_blsp_uart9,
+	MSM_MUX_blsp_uart10,
+	MSM_MUX_blsp_uart11,
+	MSM_MUX_blsp_uart12,
+	MSM_MUX_blsp_uim1,
+	MSM_MUX_blsp_uim2,
+	MSM_MUX_blsp_uim3,
+	MSM_MUX_blsp_uim4,
+	MSM_MUX_blsp_uim5,
+	MSM_MUX_blsp_uim6,
+	MSM_MUX_blsp_uim7,
+	MSM_MUX_blsp_uim8,
+	MSM_MUX_blsp_uim9,
+	MSM_MUX_blsp_uim10,
+	MSM_MUX_blsp_uim11,
+	MSM_MUX_blsp_uim12,
+	MSM_MUX_uim1,
+	MSM_MUX_uim2,
+	MSM_MUX_uim_batt_alarm,
+	MSM_MUX_sdc3,
+	MSM_MUX_sdc4,
+	MSM_MUX_gcc_gp_clk1,
+	MSM_MUX_gcc_gp_clk2,
+	MSM_MUX_gcc_gp_clk3,
+	MSM_MUX_qua_mi2s,
+	MSM_MUX_pri_mi2s,
+	MSM_MUX_spkr_mi2s,
+	MSM_MUX_ter_mi2s,
+	MSM_MUX_sec_mi2s,
+	MSM_MUX_hdmi_cec,
+	MSM_MUX_hdmi_ddc,
+	MSM_MUX_hdmi_hpd,
+	MSM_MUX_edp_hpd,
+	MSM_MUX_mdp_vsync,
+	MSM_MUX_cam_mclk0,
+	MSM_MUX_cam_mclk1,
+	MSM_MUX_cam_mclk2,
+	MSM_MUX_cam_mclk3,
+	MSM_MUX_cci_timer0,
+	MSM_MUX_cci_timer1,
+	MSM_MUX_cci_timer2,
+	MSM_MUX_cci_timer3,
+	MSM_MUX_cci_timer4,
+	MSM_MUX_cci_async_in0,
+	MSM_MUX_cci_async_in1,
+	MSM_MUX_cci_async_in2,
+	MSM_MUX_gp_pdm0,
+	MSM_MUX_gp_pdm1,
+	MSM_MUX_gp_pdm2,
+	MSM_MUX_gp0_clk,
+	MSM_MUX_gp1_clk,
+	MSM_MUX_gp_mn,
+	MSM_MUX_tsif1,
+	MSM_MUX_tsif2,
+	MSM_MUX_hsic,
+	MSM_MUX_grfc,
+	MSM_MUX_audio_ref_clk,
+	MSM_MUX_bt,
+	MSM_MUX_fm,
+	MSM_MUX_wlan,
 	MSM_MUX_slimbus,
 	MSM_MUX_NA,
 };
 
+static const char * const blsp_uart1_groups[] = {
+	"gpio0", "gpio1", "gpio2", "gpio3"
+};
+static const char * const blsp_uim1_groups[] = { "gpio0", "gpio1" };
+static const char * const blsp_i2c1_groups[] = { "gpio2", "gpio3" };
+static const char * const blsp_spi1_groups[] = {
+	"gpio0", "gpio1", "gpio2", "gpio3"
+};
+static const char * const blsp_spi1_cs1_groups[] = { "gpio8" };
+static const char * const blsp_spi1_cs2_groups[] = { "gpio9", "gpio11" };
+static const char * const blsp_spi1_cs3_groups[] = { "gpio10" };
+
+static const char * const blsp_uart2_groups[] = {
+	"gpio4", "gpio5", "gpio6", "gpio7"
+};
+static const char * const blsp_uim2_groups[] = { "gpio4", "gpio5" };
 static const char * const blsp_i2c2_groups[] = { "gpio6", "gpio7" };
+static const char * const blsp_spi2_groups[] = {
+	"gpio4", "gpio5", "gpio6", "gpio7"
+};
+static const char * const blsp_spi2_cs1_groups[] = { "gpio53", "gpio62" };
+static const char * const blsp_spi2_cs2_groups[] = { "gpio54", "gpio63" };
+static const char * const blsp_spi2_cs3_groups[] = { "gpio66" };
+
+static const char * const blsp_uart3_groups[] = {
+	"gpio8", "gpio9", "gpio10", "gpio11"
+};
+static const char * const blsp_uim3_groups[] = { "gpio8", "gpio9" };
+static const char * const blsp_i2c3_groups[] = { "gpio10", "gpio11" };
+static const char * const blsp_spi3_groups[] = {
+	"gpio8", "gpio9", "gpio10", "gpio11"
+};
+
+static const char * const cci_i2c0_groups[] = { "gpio19", "gpio20" };
+static const char * const cci_i2c1_groups[] = { "gpio21", "gpio22" };
+
+static const char * const blsp_uart4_groups[] = {
+	"gpio19", "gpio20", "gpio21", "gpio22"
+};
+static const char * const blsp_uim4_groups[] = { "gpio19", "gpio20" };
+static const char * const blsp_i2c4_groups[] = { "gpio21", "gpio22" };
+static const char * const blsp_spi4_groups[] = {
+	"gpio19", "gpio20", "gpio21", "gpio22"
+};
+
+static const char * const blsp_uart5_groups[] = {
+	"gpio23", "gpio24", "gpio25", "gpio26"
+};
+static const char * const blsp_uim5_groups[] = { "gpio23", "gpio24" };
+static const char * const blsp_i2c5_groups[] = { "gpio25", "gpio26" };
+static const char * const blsp_spi5_groups[] = {
+	"gpio23", "gpio24", "gpio25", "gpio26"
+};
+
+static const char * const blsp_uart6_groups[] = {
+	"gpio27", "gpio28", "gpio29", "gpio30"
+};
+static const char * const blsp_uim6_groups[] = { "gpio27", "gpio28" };
 static const char * const blsp_i2c6_groups[] = { "gpio29", "gpio30" };
-static const char * const blsp_i2c11_groups[] = { "gpio83", "gpio84" };
-static const char * const blsp_spi1_groups[] = { "gpio0", "gpio1", "gpio2", "gpio3" };
+static const char * const blsp_spi6_groups[] = {
+	"gpio27", "gpio28", "gpio29", "gpio30"
+};
+
+static const char * const blsp_uart7_groups[] = {
+	"gpio41", "gpio42", "gpio43", "gpio44"
+};
+static const char * const blsp_uim7_groups[] = { "gpio41", "gpio42" };
+static const char * const blsp_i2c7_groups[] = { "gpio43", "gpio44" };
+static const char * const blsp_spi7_groups[] = {
+	"gpio41", "gpio42", "gpio43", "gpio44"
+};
+
+static const char * const blsp_uart8_groups[] = {
+	"gpio45", "gpio46", "gpio47", "gpio48"
+};
+static const char * const blsp_uim8_groups[] = { "gpio45", "gpio46" };
+static const char * const blsp_i2c8_groups[] = { "gpio47", "gpio48" };
 static const char * const blsp_spi8_groups[] = {
 	"gpio45", "gpio46", "gpio47", "gpio48"
 };
-static const char * const blsp_uart2_groups[] = { "gpio4", "gpio5" };
-static const char * const blsp_uart8_groups[] = { "gpio45", "gpio46" };
+
+static const char * const blsp_uart9_groups[] = {
+	"gpio49", "gpio50", "gpio51", "gpio52"
+};
+static const char * const blsp_uim9_groups[] = { "gpio49", "gpio50" };
+static const char * const blsp_i2c9_groups[] = { "gpio51", "gpio52" };
+static const char * const blsp_spi9_groups[] = {
+	"gpio49", "gpio50", "gpio51", "gpio52"
+};
+
+static const char * const blsp_uart10_groups[] = {
+	"gpio53", "gpio54", "gpio55", "gpio56"
+};
+static const char * const blsp_uim10_groups[] = { "gpio53", "gpio54" };
+static const char * const blsp_i2c10_groups[] = { "gpio55", "gpio56" };
+static const char * const blsp_spi10_groups[] = {
+	"gpio53", "gpio54", "gpio55", "gpio56"
+};
+static const char * const blsp_spi10_cs1_groups[] = { "gpio47", "gpio67" };
+static const char * const blsp_spi10_cs2_groups[] = { "gpio48", "gpio68" };
+static const char * const blsp_spi10_cs3_groups[] = { "gpio90" };
+
+static const char * const blsp_uart11_groups[] = {
+	"gpio81", "gpio82", "gpio83", "gpio84"
+};
+static const char * const blsp_uim11_groups[] = { "gpio81", "gpio82" };
+static const char * const blsp_i2c11_groups[] = { "gpio83", "gpio84" };
+static const char * const blsp_spi11_groups[] = {
+	"gpio81", "gpio82", "gpio83", "gpio84"
+};
+
+static const char * const blsp_uart12_groups[] = {
+	"gpio85", "gpio86", "gpio87", "gpio88"
+};
+static const char * const blsp_uim12_groups[] = { "gpio85", "gpio86" };
+static const char * const blsp_i2c12_groups[] = { "gpio87", "gpio88" };
+static const char * const blsp_spi12_groups[] = {
+	"gpio85", "gpio86", "gpio87", "gpio88"
+};
+
+static const char * const uim1_groups[] = {
+	"gpio97", "gpio98", "gpio99", "gpio100"
+};
+
+static const char * const uim2_groups[] = {
+	"gpio49", "gpio50", "gpio51", "gpio52"
+};
+
+static const char * const uim_batt_alarm_groups[] = { "gpio101" };
+
+static const char * const sdc3_groups[] = {
+	"gpio35", "gpio36", "gpio37", "gpio38", "gpio39", "gpio40"
+};
+
+static const char * const sdc4_groups[] = {
+	"gpio91", "gpio92", "gpio93", "gpio94", "gpio95", "gpio96"
+};
+
+static const char * const gp0_clk_groups[] = { "gpio26" };
+static const char * const gp1_clk_groups[] = { "gpio27", "gpio57", "gpio78" };
+static const char * const gp_mn_groups[] = { "gpio29" };
+static const char * const gcc_gp_clk1_groups[] = { "gpio57", "gpio78" };
+static const char * const gcc_gp_clk2_groups[] = { "gpio58", "gpio81" };
+static const char * const gcc_gp_clk3_groups[] = { "gpio59", "gpio82" };
+
+static const char * const qua_mi2s_groups[] = {
+	"gpio57", "gpio58", "gpio59", "gpio60", "gpio61", "gpio62", "gpio63",
+};
+
+static const char * const pri_mi2s_groups[] = {
+	"gpio64", "gpio65", "gpio66", "gpio67", "gpio68"
+};
+
+static const char * const spkr_mi2s_groups[] = {
+	"gpio69", "gpio70", "gpio71", "gpio72"
+};
+
+static const char * const ter_mi2s_groups[] = {
+	"gpio73", "gpio74", "gpio75", "gpio76", "gpio77"
+};
+
+static const char * const sec_mi2s_groups[] = {
+	"gpio78", "gpio79", "gpio80", "gpio81", "gpio82"
+};
+
+static const char * const hdmi_cec_groups[] = { "gpio31" };
+static const char * const hdmi_ddc_groups[] = { "gpio32", "gpio33" };
+static const char * const hdmi_hpd_groups[] = { "gpio34" };
+static const char * const edp_hpd_groups[] = { "gpio102" };
+
+static const char * const mdp_vsync_groups[] = { "gpio12", "gpio13", "gpio14" };
+static const char * const cam_mclk0_groups[] = { "gpio15" };
+static const char * const cam_mclk1_groups[] = { "gpio16" };
+static const char * const cam_mclk2_groups[] = { "gpio17" };
+static const char * const cam_mclk3_groups[] = { "gpio18" };
+
+static const char * const cci_timer0_groups[] = { "gpio23" };
+static const char * const cci_timer1_groups[] = { "gpio24" };
+static const char * const cci_timer2_groups[] = { "gpio25" };
+static const char * const cci_timer3_groups[] = { "gpio26" };
+static const char * const cci_timer4_groups[] = { "gpio27" };
+static const char * const cci_async_in0_groups[] = { "gpio28" };
+static const char * const cci_async_in1_groups[] = { "gpio26" };
+static const char * const cci_async_in2_groups[] = { "gpio27" };
+
+static const char * const gp_pdm0_groups[] = { "gpio54", "gpio68" };
+static const char * const gp_pdm1_groups[] = { "gpio74", "gpio86" };
+static const char * const gp_pdm2_groups[] = { "gpio63", "gpio79" };
+
+static const char * const tsif1_groups[] = {
+	"gpio89", "gpio90", "gpio91", "gpio92"
+};
+
+static const char * const tsif2_groups[] = {
+	"gpio93", "gpio94", "gpio95", "gpio96"
+};
+
+static const char * const hsic_groups[] = { "gpio144", "gpio145" };
+static const char * const grfc_groups[] = {
+	"gpio104", "gpio105", "gpio106", "gpio107", "gpio108", "gpio109",
+	"gpio110", "gpio111", "gpio112", "gpio113", "gpio114", "gpio115",
+	"gpio116", "gpio117", "gpio118", "gpio119", "gpio120", "gpio121",
+	"gpio122", "gpio123", "gpio124", "gpio125", "gpio126", "gpio127",
+	"gpio128", "gpio136", "gpio137", "gpio141", "gpio143"
+};
+
+static const char * const audio_ref_clk_groups[] = { "gpio69" };
+
+static const char * const bt_groups[] = { "gpio35", "gpio43", "gpio44" };
+
+static const char * const fm_groups[] = { "gpio41", "gpio42" };
+
+static const char * const wlan_groups[] = {
+	"gpio36", "gpio37", "gpio38", "gpio39", "gpio40"
+};
+
 static const char * const slimbus_groups[] = { "gpio70", "gpio71" };
 
 static const struct msm_function msm8x74_functions[] = {
+	FUNCTION(cci_i2c0),
+	FUNCTION(cci_i2c1),
+	FUNCTION(uim1),
+	FUNCTION(uim2),
+	FUNCTION(uim_batt_alarm),
+	FUNCTION(blsp_uim1),
+	FUNCTION(blsp_uim2),
+	FUNCTION(blsp_uim3),
+	FUNCTION(blsp_uim4),
+	FUNCTION(blsp_uim5),
+	FUNCTION(blsp_uim6),
+	FUNCTION(blsp_uim7),
+	FUNCTION(blsp_uim8),
+	FUNCTION(blsp_uim9),
+	FUNCTION(blsp_uim10),
+	FUNCTION(blsp_uim11),
+	FUNCTION(blsp_uim12),
+	FUNCTION(blsp_i2c1),
 	FUNCTION(blsp_i2c2),
+	FUNCTION(blsp_i2c3),
+	FUNCTION(blsp_i2c4),
+	FUNCTION(blsp_i2c5),
 	FUNCTION(blsp_i2c6),
+	FUNCTION(blsp_i2c7),
+	FUNCTION(blsp_i2c8),
+	FUNCTION(blsp_i2c9),
+	FUNCTION(blsp_i2c10),
 	FUNCTION(blsp_i2c11),
+	FUNCTION(blsp_i2c12),
 	FUNCTION(blsp_spi1),
+	FUNCTION(blsp_spi1_cs1),
+	FUNCTION(blsp_spi1_cs2),
+	FUNCTION(blsp_spi1_cs3),
+	FUNCTION(blsp_spi2),
+	FUNCTION(blsp_spi2_cs1),
+	FUNCTION(blsp_spi2_cs2),
+	FUNCTION(blsp_spi2_cs3),
+	FUNCTION(blsp_spi3),
+	FUNCTION(blsp_spi4),
+	FUNCTION(blsp_spi5),
+	FUNCTION(blsp_spi6),
+	FUNCTION(blsp_spi7),
 	FUNCTION(blsp_spi8),
+	FUNCTION(blsp_spi9),
+	FUNCTION(blsp_spi10),
+	FUNCTION(blsp_spi10_cs1),
+	FUNCTION(blsp_spi10_cs2),
+	FUNCTION(blsp_spi10_cs3),
+	FUNCTION(blsp_spi11),
+	FUNCTION(blsp_spi12),
+	FUNCTION(blsp_uart1),
 	FUNCTION(blsp_uart2),
+	FUNCTION(blsp_uart3),
+	FUNCTION(blsp_uart4),
+	FUNCTION(blsp_uart5),
+	FUNCTION(blsp_uart6),
+	FUNCTION(blsp_uart7),
 	FUNCTION(blsp_uart8),
+	FUNCTION(blsp_uart9),
+	FUNCTION(blsp_uart10),
+	FUNCTION(blsp_uart11),
+	FUNCTION(blsp_uart12),
+	FUNCTION(sdc3),
+	FUNCTION(sdc4),
+	FUNCTION(gcc_gp_clk1),
+	FUNCTION(gcc_gp_clk2),
+	FUNCTION(gcc_gp_clk3),
+	FUNCTION(qua_mi2s),
+	FUNCTION(pri_mi2s),
+	FUNCTION(spkr_mi2s),
+	FUNCTION(ter_mi2s),
+	FUNCTION(sec_mi2s),
+	FUNCTION(mdp_vsync),
+	FUNCTION(cam_mclk0),
+	FUNCTION(cam_mclk1),
+	FUNCTION(cam_mclk2),
+	FUNCTION(cam_mclk3),
+	FUNCTION(cci_timer0),
+	FUNCTION(cci_timer1),
+	FUNCTION(cci_timer2),
+	FUNCTION(cci_timer3),
+	FUNCTION(cci_timer4),
+	FUNCTION(cci_async_in0),
+	FUNCTION(cci_async_in1),
+	FUNCTION(cci_async_in2),
+	FUNCTION(hdmi_cec),
+	FUNCTION(hdmi_ddc),
+	FUNCTION(hdmi_hpd),
+	FUNCTION(edp_hpd),
+	FUNCTION(gp_pdm0),
+	FUNCTION(gp_pdm1),
+	FUNCTION(gp_pdm2),
+	FUNCTION(gp0_clk),
+	FUNCTION(gp1_clk),
+	FUNCTION(gp_mn),
+	FUNCTION(tsif1),
+	FUNCTION(tsif2),
+	FUNCTION(hsic),
+	FUNCTION(grfc),
+	FUNCTION(audio_ref_clk),
+	FUNCTION(bt),
+	FUNCTION(fm),
+	FUNCTION(wlan),
 	FUNCTION(slimbus),
 };
 
 static const struct msm_pingroup msm8x74_groups[] = {
-	PINGROUP(0,   blsp_spi1, NA, NA, NA, NA, NA, NA),
-	PINGROUP(1,   blsp_spi1, NA, NA, NA, NA, NA, NA),
-	PINGROUP(2,   blsp_spi1, NA, NA, NA, NA, NA, NA),
-	PINGROUP(3,   blsp_spi1, NA, NA, NA, NA, NA, NA),
-	PINGROUP(4,   NA, blsp_uart2, NA, NA, NA, NA, NA),
-	PINGROUP(5,   NA, blsp_uart2, NA, NA, NA, NA, NA),
-	PINGROUP(6,   NA, NA, blsp_i2c2, NA, NA, NA, NA),
-	PINGROUP(7,   NA, NA, blsp_i2c2, NA, NA, NA, NA),
-	PINGROUP(8,   NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(9,   NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(10,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(11,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(12,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(13,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(14,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(15,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(16,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(17,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(18,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(19,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(20,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(21,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(22,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(23,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(24,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(25,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(26,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(27,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(28,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(29,  NA, NA, blsp_i2c6, NA, NA, NA, NA),
-	PINGROUP(30,  NA, NA, blsp_i2c6, NA, NA, NA, NA),
-	PINGROUP(31,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(32,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(33,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(34,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(35,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(36,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(37,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(38,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(39,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(40,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(41,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(42,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(43,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(44,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(45,  blsp_spi8, blsp_uart8, NA, NA, NA, NA, NA),
-	PINGROUP(46,  blsp_spi8, blsp_uart8, NA, NA, NA, NA, NA),
-	PINGROUP(47,  blsp_spi8, NA, NA, NA, NA, NA, NA),
-	PINGROUP(48,  blsp_spi8, NA, NA, NA, NA, NA, NA),
-	PINGROUP(49,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(50,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(51,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(52,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(53,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(54,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(55,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(56,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(57,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(58,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(59,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(60,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(61,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(62,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(63,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(64,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(65,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(66,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(67,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(68,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(69,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(70,  slimbus, NA, NA, NA, NA, NA, NA),
-	PINGROUP(71,  slimbus, NA, NA, NA, NA, NA, NA),
-	PINGROUP(72,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(73,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(74,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(75,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(76,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(77,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(78,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(79,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(80,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(81,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(82,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(83,  NA, NA, blsp_i2c11, NA, NA, NA, NA),
-	PINGROUP(84,  NA, NA, blsp_i2c11, NA, NA, NA, NA),
-	PINGROUP(85,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(86,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(87,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(88,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(89,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(90,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(91,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(92,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(93,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(94,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(95,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(96,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(97,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(98,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(99,  NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(100, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(101, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(102, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(0,   blsp_spi1, blsp_uart1, blsp_uim1, NA, NA, NA, NA),
+	PINGROUP(1,   blsp_spi1, blsp_uart1, blsp_uim1, NA, NA, NA, NA),
+	PINGROUP(2,   blsp_spi1, blsp_uart1, blsp_i2c1, NA, NA, NA, NA),
+	PINGROUP(3,   blsp_spi1, blsp_uart1, blsp_i2c1, NA, NA, NA, NA),
+	PINGROUP(4,   blsp_spi2, blsp_uart2, blsp_uim2, NA, NA, NA, NA),
+	PINGROUP(5,   blsp_spi2, blsp_uart2, blsp_uim2, NA, NA, NA, NA),
+	PINGROUP(6,   blsp_spi2, blsp_uart2, blsp_i2c2, NA, NA, NA, NA),
+	PINGROUP(7,   blsp_spi2, blsp_uart2, blsp_i2c2, NA, NA, NA, NA),
+	PINGROUP(8,   blsp_spi3, blsp_uart3, blsp_uim3, blsp_spi1_cs1, NA, NA, NA),
+	PINGROUP(9,   blsp_spi3, blsp_uart3, blsp_uim3, blsp_spi1_cs2, NA, NA, NA),
+	PINGROUP(10,  blsp_spi3, blsp_uart3, blsp_i2c3, blsp_spi1_cs3, NA, NA, NA),
+	PINGROUP(11,  blsp_spi3, blsp_uart3, blsp_i2c3, blsp_spi1_cs2, NA, NA, NA),
+	PINGROUP(12,  mdp_vsync, NA, NA, NA, NA, NA, NA),
+	PINGROUP(13,  mdp_vsync, NA, NA, NA, NA, NA, NA),
+	PINGROUP(14,  mdp_vsync, NA, NA, NA, NA, NA, NA),
+	PINGROUP(15,  cam_mclk0, NA, NA, NA, NA, NA, NA),
+	PINGROUP(16,  cam_mclk1, NA, NA, NA, NA, NA, NA),
+	PINGROUP(17,  cam_mclk2, NA, NA, NA, NA, NA, NA),
+	PINGROUP(18,  cam_mclk3, NA, NA, NA, NA, NA, NA),
+	PINGROUP(19,  cci_i2c0, blsp_spi4, blsp_uart4, blsp_uim4, NA, NA, NA),
+	PINGROUP(20,  cci_i2c0, blsp_spi4, blsp_uart4, blsp_uim4, NA, NA, NA),
+	PINGROUP(21,  cci_i2c1, blsp_spi4, blsp_uart4, blsp_i2c4, NA, NA, NA),
+	PINGROUP(22,  cci_i2c1, blsp_spi4, blsp_uart4, blsp_i2c4, NA, NA, NA),
+	PINGROUP(23,  cci_timer0, blsp_spi5, blsp_uart5, blsp_uim5, NA, NA, NA),
+	PINGROUP(24,  cci_timer1, blsp_spi5, blsp_uart5, blsp_uim5, NA, NA, NA),
+	PINGROUP(25,  cci_timer2, blsp_spi5, blsp_uart5, blsp_i2c5, NA, NA, NA),
+	PINGROUP(26,  cci_timer3, cci_async_in1, blsp_spi5, blsp_uart5, blsp_i2c5, gp0_clk, NA),
+	PINGROUP(27,  cci_timer4, cci_async_in2, blsp_spi6, blsp_uart6, blsp_i2c6, gp1_clk, NA),
+	PINGROUP(28,  cci_async_in0, blsp_spi6, blsp_uart6, blsp_uim6, NA, NA, NA),
+	PINGROUP(29,  blsp_spi6, blsp_uart6, blsp_i2c6, gp_mn, NA, NA, NA),
+	PINGROUP(30,  blsp_spi6, blsp_uart6, blsp_i2c6, NA, NA, NA, NA),
+	PINGROUP(31,  hdmi_cec, NA, NA, NA, NA, NA, NA),
+	PINGROUP(32,  hdmi_ddc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(33,  hdmi_ddc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(34,  hdmi_hpd, NA, NA, NA, NA, NA, NA),
+	PINGROUP(35,  bt, sdc3, NA, NA, NA, NA, NA),
+	PINGROUP(36,  wlan, sdc3, NA, NA, NA, NA, NA),
+	PINGROUP(37,  wlan, sdc3, NA, NA, NA, NA, NA),
+	PINGROUP(38,  wlan, sdc3, NA, NA, NA, NA, NA),
+	PINGROUP(39,  wlan, sdc3, NA, NA, NA, NA, NA),
+	PINGROUP(40,  wlan, sdc3, NA, NA, NA, NA, NA),
+	PINGROUP(41,  fm, blsp_spi7, blsp_uart7, blsp_uim7, NA, NA, NA),
+	PINGROUP(42,  fm, blsp_spi7, blsp_uart7, blsp_uim7, NA, NA, NA),
+	PINGROUP(43,  bt, blsp_spi7, blsp_uart7, blsp_i2c7, NA, NA, NA),
+	PINGROUP(44,  bt, blsp_spi7, blsp_uart7, blsp_i2c7, NA, NA, NA),
+	PINGROUP(45,  blsp_spi8, blsp_uart8, blsp_uim8, NA, NA, NA, NA),
+	PINGROUP(46,  blsp_spi8, blsp_uart8, blsp_uim8, NA, NA, NA, NA),
+	PINGROUP(47,  blsp_spi8, blsp_uart8, blsp_i2c8, blsp_spi10_cs1, NA, NA, NA),
+	PINGROUP(48,  blsp_spi8, blsp_uart8, blsp_i2c8, blsp_spi10_cs2, NA, NA, NA),
+	PINGROUP(49,  uim2, blsp_spi9, blsp_uart9, blsp_uim9, NA, NA, NA),
+	PINGROUP(50,  uim2, blsp_spi9, blsp_uart9, blsp_uim9, NA, NA, NA),
+	PINGROUP(51,  uim2, blsp_spi9, blsp_uart9, blsp_i2c9, NA, NA, NA),
+	PINGROUP(52,  uim2, blsp_spi9, blsp_uart9, blsp_i2c9, NA, NA, NA),
+	PINGROUP(53,  blsp_spi10, blsp_uart10, blsp_uim10, blsp_spi2_cs1, NA, NA, NA),
+	PINGROUP(54,  blsp_spi10, blsp_uart10, blsp_uim10, blsp_spi2_cs2, gp_pdm0, NA, NA),
+	PINGROUP(55,  blsp_spi10, blsp_uart10, blsp_i2c10, NA, NA, NA, NA),
+	PINGROUP(56,  blsp_spi10, blsp_uart10, blsp_i2c10, NA, NA, NA, NA),
+	PINGROUP(57,  qua_mi2s, gcc_gp_clk1, NA, NA, NA, NA, NA),
+	PINGROUP(58,  qua_mi2s, gcc_gp_clk2, NA, NA, NA, NA, NA),
+	PINGROUP(59,  qua_mi2s, gcc_gp_clk3, NA, NA, NA, NA, NA),
+	PINGROUP(60,  qua_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(61,  qua_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(62,  qua_mi2s, blsp_spi2_cs1, NA, NA, NA, NA, NA),
+	PINGROUP(63,  qua_mi2s, blsp_spi2_cs2, gp_pdm2, NA, NA, NA, NA),
+	PINGROUP(64,  pri_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(65,  pri_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(66,  pri_mi2s, blsp_spi2_cs3, NA, NA, NA, NA, NA),
+	PINGROUP(67,  pri_mi2s, blsp_spi10_cs1, NA, NA, NA, NA, NA),
+	PINGROUP(68,  pri_mi2s, blsp_spi10_cs2, gp_pdm0, NA, NA, NA, NA),
+	PINGROUP(69,  spkr_mi2s, audio_ref_clk, NA, NA, NA, NA, NA),
+	PINGROUP(70,  slimbus, spkr_mi2s, NA, NA, NA, NA, NA),
+	PINGROUP(71,  slimbus, spkr_mi2s, NA, NA, NA, NA, NA),
+	PINGROUP(72,  spkr_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(73,  ter_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(74,  ter_mi2s, gp_pdm1, NA, NA, NA, NA, NA),
+	PINGROUP(75,  ter_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(76,  ter_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(77,  ter_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(78,  sec_mi2s, gcc_gp_clk1, NA, NA, NA, NA, NA),
+	PINGROUP(79,  sec_mi2s, gp_pdm2, NA, NA, NA, NA, NA),
+	PINGROUP(80,  sec_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(81,  sec_mi2s, blsp_spi11, blsp_uart11, blsp_uim11, gcc_gp_clk2, NA, NA),
+	PINGROUP(82,  sec_mi2s, blsp_spi11, blsp_uart11, blsp_uim11, gcc_gp_clk3, NA, NA),
+	PINGROUP(83,  blsp_spi11, blsp_uart11, blsp_i2c11, NA, NA, NA, NA),
+	PINGROUP(84,  blsp_spi11, blsp_uart11, blsp_i2c11, NA, NA, NA, NA),
+	PINGROUP(85,  blsp_spi12, blsp_uart12, blsp_uim12, NA, NA, NA, NA),
+	PINGROUP(86,  blsp_spi12, blsp_uart12, blsp_uim12, gp_pdm1, NA, NA, NA),
+	PINGROUP(87,  blsp_spi12, blsp_uart12, blsp_i2c12, NA, NA, NA, NA),
+	PINGROUP(88,  blsp_spi12, blsp_uart12, blsp_i2c12, NA, NA, NA, NA),
+	PINGROUP(89,  tsif1, NA, NA, NA, NA, NA, NA),
+	PINGROUP(90,  tsif1, blsp_spi10_cs3, NA, NA, NA, NA, NA),
+	PINGROUP(91,  tsif1, sdc4, NA, NA, NA, NA, NA),
+	PINGROUP(92,  tsif1, sdc4, NA, NA, NA, NA, NA),
+	PINGROUP(93,  tsif2, sdc4, NA, NA, NA, NA, NA),
+	PINGROUP(94,  tsif2, sdc4, NA, NA, NA, NA, NA),
+	PINGROUP(95,  tsif2, sdc4, NA, NA, NA, NA, NA),
+	PINGROUP(96,  tsif2, sdc4, NA, NA, NA, NA, NA),
+	PINGROUP(97,  uim1, NA, NA, NA, NA, NA, NA),
+	PINGROUP(98,  uim1, NA, NA, NA, NA, NA, NA),
+	PINGROUP(99,  uim1, NA, NA, NA, NA, NA, NA),
+	PINGROUP(100, uim1, NA, NA, NA, NA, NA, NA),
+	PINGROUP(101, uim_batt_alarm, NA, NA, NA, NA, NA, NA),
+	PINGROUP(102, edp_hpd, NA, NA, NA, NA, NA, NA),
 	PINGROUP(103, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(104, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(105, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(106, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(107, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(108, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(109, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(110, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(111, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(112, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(113, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(114, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(115, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(116, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(117, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(118, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(119, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(120, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(121, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(122, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(123, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(124, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(125, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(126, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(127, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(128, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(104, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(105, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(106, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(107, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(108, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(109, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(110, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(111, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(112, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(113, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(114, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(115, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(116, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(117, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(118, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(119, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(120, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(121, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(122, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(123, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(124, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(125, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(126, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(127, grfc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(128, NA, grfc, NA, NA, NA, NA, NA),
 	PINGROUP(129, NA, NA, NA, NA, NA, NA, NA),
 	PINGROUP(130, NA, NA, NA, NA, NA, NA, NA),
 	PINGROUP(131, NA, NA, NA, NA, NA, NA, NA),
@@ -571,16 +971,16 @@ static const struct msm_pingroup msm8x74_groups[] = {
 	PINGROUP(133, NA, NA, NA, NA, NA, NA, NA),
 	PINGROUP(134, NA, NA, NA, NA, NA, NA, NA),
 	PINGROUP(135, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(136, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(137, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(136, NA, grfc, NA, NA, NA, NA, NA),
+	PINGROUP(137, NA, grfc, NA, NA, NA, NA, NA),
 	PINGROUP(138, NA, NA, NA, NA, NA, NA, NA),
 	PINGROUP(139, NA, NA, NA, NA, NA, NA, NA),
 	PINGROUP(140, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(141, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(143, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(143, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(144, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(145, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(141, NA, grfc, NA, NA, NA, NA, NA),
+	PINGROUP(142, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(143, NA, grfc, NA, NA, NA, NA, NA),
+	PINGROUP(144, hsic, NA, NA, NA, NA, NA, NA),
+	PINGROUP(145, hsic, NA, NA, NA, NA, NA, NA),
 	SDC_PINGROUP(sdc1_clk, 0x2044, 13, 6),
 	SDC_PINGROUP(sdc1_cmd, 0x2044, 11, 3),
 	SDC_PINGROUP(sdc1_data, 0x2044, 9, 0),
diff --git a/drivers/pinctrl/pinctrl-rockchip.c b/drivers/pinctrl/pinctrl-rockchip.c
index 96c60d230c1..bb805d5e9ff 100644
--- a/drivers/pinctrl/pinctrl-rockchip.c
+++ b/drivers/pinctrl/pinctrl-rockchip.c
@@ -37,6 +37,8 @@
 #include <linux/pinctrl/pinconf-generic.h>
 #include <linux/irqchip/chained_irq.h>
 #include <linux/clk.h>
+#include <linux/regmap.h>
+#include <linux/mfd/syscon.h>
 #include <dt-bindings/pinctrl/rockchip.h>
 
 #include "core.h"
@@ -86,7 +88,7 @@ enum rockchip_pin_bank_type {
  */
 struct rockchip_pin_bank {
 	void __iomem			*reg_base;
-	void __iomem			*reg_pull;
+	struct regmap			*regmap_pull;
 	struct clk			*clk;
 	int				irq;
 	u32				pin_base;
@@ -120,8 +122,9 @@ struct rockchip_pin_ctrl {
 	char				*label;
 	enum rockchip_pinctrl_type	type;
 	int				mux_offset;
-	void	(*pull_calc_reg)(struct rockchip_pin_bank *bank, int pin_num,
-				 void __iomem **reg, u8 *bit);
+	void	(*pull_calc_reg)(struct rockchip_pin_bank *bank,
+				    int pin_num, struct regmap **regmap,
+				    int *reg, u8 *bit);
 };
 
 struct rockchip_pin_config {
@@ -159,8 +162,10 @@ struct rockchip_pmx_func {
 };
 
 struct rockchip_pinctrl {
-	void __iomem			*reg_base;
-	void __iomem			*reg_pull;
+	struct regmap			*regmap_base;
+	int				reg_size;
+	struct regmap			*regmap_pull;
+	struct regmap			*regmap_pmu;
 	struct device			*dev;
 	struct rockchip_pin_ctrl	*ctrl;
 	struct pinctrl_desc		pctl;
@@ -171,6 +176,12 @@ struct rockchip_pinctrl {
 	unsigned int			nfunctions;
 };
 
+static struct regmap_config rockchip_regmap_config = {
+	.reg_bits = 32,
+	.val_bits = 32,
+	.reg_stride = 4,
+};
+
 static inline struct rockchip_pin_bank *gc_to_pin_bank(struct gpio_chip *gc)
 {
 	return container_of(gc, struct rockchip_pin_bank, gpio_chip);
@@ -329,6 +340,29 @@ static const struct pinctrl_ops rockchip_pctrl_ops = {
  * Hardware access
  */
 
+static int rockchip_get_mux(struct rockchip_pin_bank *bank, int pin)
+{
+	struct rockchip_pinctrl *info = bank->drvdata;
+	unsigned int val;
+	int reg, ret;
+	u8 bit;
+
+	if (bank->bank_type == RK3188_BANK0 && pin < 16)
+		return RK_FUNC_GPIO;
+
+	/* get basic quadrupel of mux registers and the correct reg inside */
+	reg = info->ctrl->mux_offset;
+	reg += bank->bank_num * 0x10;
+	reg += (pin / 8) * 4;
+	bit = (pin % 8) * 2;
+
+	ret = regmap_read(info->regmap_base, reg, &val);
+	if (ret)
+		return ret;
+
+	return ((val >> bit) & 3);
+}
+
 /*
  * Set a new mux function for a pin.
  *
@@ -345,7 +379,7 @@ static const struct pinctrl_ops rockchip_pctrl_ops = {
 static int rockchip_set_mux(struct rockchip_pin_bank *bank, int pin, int mux)
 {
 	struct rockchip_pinctrl *info = bank->drvdata;
-	void __iomem *reg = info->reg_base + info->ctrl->mux_offset;
+	int reg, ret;
 	unsigned long flags;
 	u8 bit;
 	u32 data;
@@ -368,6 +402,7 @@ static int rockchip_set_mux(struct rockchip_pin_bank *bank, int pin, int mux)
 						bank->bank_num, pin, mux);
 
 	/* get basic quadrupel of mux registers and the correct reg inside */
+	reg = info->ctrl->mux_offset;
 	reg += bank->bank_num * 0x10;
 	reg += (pin / 8) * 4;
 	bit = (pin % 8) * 2;
@@ -376,11 +411,11 @@ static int rockchip_set_mux(struct rockchip_pin_bank *bank, int pin, int mux)
 
 	data = (3 << (bit + 16));
 	data |= (mux & 3) << bit;
-	writel(data, reg);
+	ret = regmap_write(info->regmap_base, reg, data);
 
 	spin_unlock_irqrestore(&bank->slock, flags);
 
-	return 0;
+	return ret;
 }
 
 #define RK2928_PULL_OFFSET		0x118
@@ -388,34 +423,46 @@ static int rockchip_set_mux(struct rockchip_pin_bank *bank, int pin, int mux)
 #define RK2928_PULL_BANK_STRIDE		8
 
 static void rk2928_calc_pull_reg_and_bit(struct rockchip_pin_bank *bank,
-				    int pin_num, void __iomem **reg, u8 *bit)
+				    int pin_num, struct regmap **regmap,
+				    int *reg, u8 *bit)
 {
 	struct rockchip_pinctrl *info = bank->drvdata;
 
-	*reg = info->reg_base + RK2928_PULL_OFFSET;
+	*regmap = info->regmap_base;
+	*reg = RK2928_PULL_OFFSET;
 	*reg += bank->bank_num * RK2928_PULL_BANK_STRIDE;
 	*reg += (pin_num / RK2928_PULL_PINS_PER_REG) * 4;
 
 	*bit = pin_num % RK2928_PULL_PINS_PER_REG;
 };
 
+#define RK3188_PULL_OFFSET		0x164
 #define RK3188_PULL_BITS_PER_PIN	2
 #define RK3188_PULL_PINS_PER_REG	8
 #define RK3188_PULL_BANK_STRIDE		16
+#define RK3188_PULL_PMU_OFFSET		0x64
 
 static void rk3188_calc_pull_reg_and_bit(struct rockchip_pin_bank *bank,
-				    int pin_num, void __iomem **reg, u8 *bit)
+				    int pin_num, struct regmap **regmap,
+				    int *reg, u8 *bit)
 {
 	struct rockchip_pinctrl *info = bank->drvdata;
 
 	/* The first 12 pins of the first bank are located elsewhere */
 	if (bank->bank_type == RK3188_BANK0 && pin_num < 12) {
-		*reg = bank->reg_pull +
-				((pin_num / RK3188_PULL_PINS_PER_REG) * 4);
+		*regmap = info->regmap_pmu ? info->regmap_pmu
+					   : bank->regmap_pull;
+		*reg = info->regmap_pmu ? RK3188_PULL_PMU_OFFSET : 0;
+		*reg += ((pin_num / RK3188_PULL_PINS_PER_REG) * 4);
 		*bit = pin_num % RK3188_PULL_PINS_PER_REG;
 		*bit *= RK3188_PULL_BITS_PER_PIN;
 	} else {
-		*reg = info->reg_pull - 4;
+		*regmap = info->regmap_pull ? info->regmap_pull
+					    : info->regmap_base;
+		*reg = info->regmap_pull ? 0 : RK3188_PULL_OFFSET;
+
+		/* correct the offset, as it is the 2nd pull register */
+		*reg -= 4;
 		*reg += bank->bank_num * RK3188_PULL_BANK_STRIDE;
 		*reg += ((pin_num / RK3188_PULL_PINS_PER_REG) * 4);
 
@@ -433,7 +480,8 @@ static int rockchip_get_pull(struct rockchip_pin_bank *bank, int pin_num)
 {
 	struct rockchip_pinctrl *info = bank->drvdata;
 	struct rockchip_pin_ctrl *ctrl = info->ctrl;
-	void __iomem *reg;
+	struct regmap *regmap;
+	int reg, ret;
 	u8 bit;
 	u32 data;
 
@@ -441,15 +489,19 @@ static int rockchip_get_pull(struct rockchip_pin_bank *bank, int pin_num)
 	if (ctrl->type == RK3066B)
 		return PIN_CONFIG_BIAS_DISABLE;
 
-	ctrl->pull_calc_reg(bank, pin_num, &reg, &bit);
+	ctrl->pull_calc_reg(bank, pin_num, &regmap, &reg, &bit);
+
+	ret = regmap_read(regmap, reg, &data);
+	if (ret)
+		return ret;
 
 	switch (ctrl->type) {
 	case RK2928:
-		return !(readl_relaxed(reg) & BIT(bit))
+		return !(data & BIT(bit))
 				? PIN_CONFIG_BIAS_PULL_PIN_DEFAULT
 				: PIN_CONFIG_BIAS_DISABLE;
 	case RK3188:
-		data = readl_relaxed(reg) >> bit;
+		data >>= bit;
 		data &= (1 << RK3188_PULL_BITS_PER_PIN) - 1;
 
 		switch (data) {
@@ -476,7 +528,8 @@ static int rockchip_set_pull(struct rockchip_pin_bank *bank,
 {
 	struct rockchip_pinctrl *info = bank->drvdata;
 	struct rockchip_pin_ctrl *ctrl = info->ctrl;
-	void __iomem *reg;
+	struct regmap *regmap;
+	int reg, ret;
 	unsigned long flags;
 	u8 bit;
 	u32 data;
@@ -488,7 +541,7 @@ static int rockchip_set_pull(struct rockchip_pin_bank *bank,
 	if (ctrl->type == RK3066B)
 		return pull ? -EINVAL : 0;
 
-	ctrl->pull_calc_reg(bank, pin_num, &reg, &bit);
+	ctrl->pull_calc_reg(bank, pin_num, &regmap, &reg, &bit);
 
 	switch (ctrl->type) {
 	case RK2928:
@@ -497,7 +550,7 @@ static int rockchip_set_pull(struct rockchip_pin_bank *bank,
 		data = BIT(bit + 16);
 		if (pull == PIN_CONFIG_BIAS_DISABLE)
 			data |= BIT(bit);
-		writel(data, reg);
+		ret = regmap_write(regmap, reg, data);
 
 		spin_unlock_irqrestore(&bank->slock, flags);
 		break;
@@ -526,7 +579,7 @@ static int rockchip_set_pull(struct rockchip_pin_bank *bank,
 			return -EINVAL;
 		}
 
-		writel(data, reg);
+		ret = regmap_write(regmap, reg, data);
 
 		spin_unlock_irqrestore(&bank->slock, flags);
 		break;
@@ -535,7 +588,7 @@ static int rockchip_set_pull(struct rockchip_pin_bank *bank,
 		return -EINVAL;
 	}
 
-	return 0;
+	return ret;
 }
 
 /*
@@ -687,6 +740,10 @@ static bool rockchip_pinconf_pull_valid(struct rockchip_pin_ctrl *ctrl,
 	return false;
 }
 
+static int rockchip_gpio_direction_output(struct gpio_chip *gc,
+					  unsigned offset, int value);
+static int rockchip_gpio_get(struct gpio_chip *gc, unsigned offset);
+
 /* set the pin config settings for a specified pin */
 static int rockchip_pinconf_set(struct pinctrl_dev *pctldev, unsigned int pin,
 				unsigned long *configs, unsigned num_configs)
@@ -724,6 +781,13 @@ static int rockchip_pinconf_set(struct pinctrl_dev *pctldev, unsigned int pin,
 			if (rc)
 				return rc;
 			break;
+		case PIN_CONFIG_OUTPUT:
+			rc = rockchip_gpio_direction_output(&bank->gpio_chip,
+							    pin - bank->pin_base,
+							    arg);
+			if (rc)
+				return rc;
+			break;
 		default:
 			return -ENOTSUPP;
 			break;
@@ -740,13 +804,15 @@ static int rockchip_pinconf_get(struct pinctrl_dev *pctldev, unsigned int pin,
 	struct rockchip_pinctrl *info = pinctrl_dev_get_drvdata(pctldev);
 	struct rockchip_pin_bank *bank = pin_to_bank(info, pin);
 	enum pin_config_param param = pinconf_to_config_param(*config);
+	u16 arg;
+	int rc;
 
 	switch (param) {
 	case PIN_CONFIG_BIAS_DISABLE:
 		if (rockchip_get_pull(bank, pin - bank->pin_base) != param)
 			return -EINVAL;
 
-		*config = 0;
+		arg = 0;
 		break;
 	case PIN_CONFIG_BIAS_PULL_UP:
 	case PIN_CONFIG_BIAS_PULL_DOWN:
@@ -758,13 +824,26 @@ static int rockchip_pinconf_get(struct pinctrl_dev *pctldev, unsigned int pin,
 		if (rockchip_get_pull(bank, pin - bank->pin_base) != param)
 			return -EINVAL;
 
-		*config = 1;
+		arg = 1;
+		break;
+	case PIN_CONFIG_OUTPUT:
+		rc = rockchip_get_mux(bank, pin - bank->pin_base);
+		if (rc != RK_FUNC_GPIO)
+			return -EINVAL;
+
+		rc = rockchip_gpio_get(&bank->gpio_chip, pin - bank->pin_base);
+		if (rc < 0)
+			return rc;
+
+		arg = rc ? 1 : 0;
 		break;
 	default:
 		return -ENOTSUPP;
 		break;
 	}
 
+	*config = pinconf_to_config_packed(param, arg);
+
 	return 0;
 }
 
@@ -1365,16 +1444,17 @@ static int rockchip_gpiolib_unregister(struct platform_device *pdev,
 }
 
 static int rockchip_get_bank_data(struct rockchip_pin_bank *bank,
-				  struct device *dev)
+				  struct rockchip_pinctrl *info)
 {
 	struct resource res;
+	void __iomem *base;
 
 	if (of_address_to_resource(bank->of_node, 0, &res)) {
-		dev_err(dev, "cannot find IO resource for bank\n");
+		dev_err(info->dev, "cannot find IO resource for bank\n");
 		return -ENOENT;
 	}
 
-	bank->reg_base = devm_ioremap_resource(dev, &res);
+	bank->reg_base = devm_ioremap_resource(info->dev, &res);
 	if (IS_ERR(bank->reg_base))
 		return PTR_ERR(bank->reg_base);
 
@@ -1384,16 +1464,30 @@ static int rockchip_get_bank_data(struct rockchip_pin_bank *bank,
 	 */
 	if (of_device_is_compatible(bank->of_node,
 				    "rockchip,rk3188-gpio-bank0")) {
+		struct device_node *node;
+
 		bank->bank_type = RK3188_BANK0;
 
-		if (of_address_to_resource(bank->of_node, 1, &res)) {
-			dev_err(dev, "cannot find IO resource for bank\n");
-			return -ENOENT;
+		node = of_parse_phandle(bank->of_node->parent,
+					"rockchip,pmu", 0);
+		if (!node) {
+			if (of_address_to_resource(bank->of_node, 1, &res)) {
+				dev_err(info->dev, "cannot find IO resource for bank\n");
+				return -ENOENT;
+			}
+
+			base = devm_ioremap_resource(info->dev, &res);
+			if (IS_ERR(base))
+				return PTR_ERR(base);
+			rockchip_regmap_config.max_register =
+						    resource_size(&res) - 4;
+			rockchip_regmap_config.name =
+					    "rockchip,rk3188-gpio-bank0-pull";
+			bank->regmap_pull = devm_regmap_init_mmio(info->dev,
+						    base,
+						    &rockchip_regmap_config);
 		}
 
-		bank->reg_pull = devm_ioremap_resource(dev, &res);
-		if (IS_ERR(bank->reg_pull))
-			return PTR_ERR(bank->reg_pull);
 	} else {
 		bank->bank_type = COMMON_BANK;
 	}
@@ -1433,7 +1527,7 @@ static struct rockchip_pin_ctrl *rockchip_pinctrl_get_soc_data(
 			if (!strcmp(bank->name, np->name)) {
 				bank->of_node = np;
 
-				if (!rockchip_get_bank_data(bank, &pdev->dev))
+				if (!rockchip_get_bank_data(bank, d))
 					bank->valid = true;
 
 				break;
@@ -1457,7 +1551,9 @@ static int rockchip_pinctrl_probe(struct platform_device *pdev)
 	struct rockchip_pinctrl *info;
 	struct device *dev = &pdev->dev;
 	struct rockchip_pin_ctrl *ctrl;
+	struct device_node *np = pdev->dev.of_node, *node;
 	struct resource *res;
+	void __iomem *base;
 	int ret;
 
 	if (!dev->of_node) {
@@ -1469,25 +1565,56 @@ static int rockchip_pinctrl_probe(struct platform_device *pdev)
 	if (!info)
 		return -ENOMEM;
 
+	info->dev = dev;
+
 	ctrl = rockchip_pinctrl_get_soc_data(info, pdev);
 	if (!ctrl) {
 		dev_err(dev, "driver data not available\n");
 		return -EINVAL;
 	}
 	info->ctrl = ctrl;
-	info->dev = dev;
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	info->reg_base = devm_ioremap_resource(&pdev->dev, res);
-	if (IS_ERR(info->reg_base))
-		return PTR_ERR(info->reg_base);
-
-	/* The RK3188 has its pull registers in a separate place */
-	if (ctrl->type == RK3188) {
-		res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-		info->reg_pull = devm_ioremap_resource(&pdev->dev, res);
-		if (IS_ERR(info->reg_pull))
-			return PTR_ERR(info->reg_pull);
+	node = of_parse_phandle(np, "rockchip,grf", 0);
+	if (node) {
+		info->regmap_base = syscon_node_to_regmap(node);
+		if (IS_ERR(info->regmap_base))
+			return PTR_ERR(info->regmap_base);
+	} else {
+		res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+		base = devm_ioremap_resource(&pdev->dev, res);
+		if (IS_ERR(base))
+			return PTR_ERR(base);
+
+		rockchip_regmap_config.max_register = resource_size(res) - 4;
+		rockchip_regmap_config.name = "rockchip,pinctrl";
+		info->regmap_base = devm_regmap_init_mmio(&pdev->dev, base,
+						    &rockchip_regmap_config);
+
+		/* to check for the old dt-bindings */
+		info->reg_size = resource_size(res);
+
+		/* Honor the old binding, with pull registers as 2nd resource */
+		if (ctrl->type == RK3188 && info->reg_size < 0x200) {
+			res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+			base = devm_ioremap_resource(&pdev->dev, res);
+			if (IS_ERR(base))
+				return PTR_ERR(base);
+
+			rockchip_regmap_config.max_register =
+							resource_size(res) - 4;
+			rockchip_regmap_config.name = "rockchip,pinctrl-pull";
+			info->regmap_pull = devm_regmap_init_mmio(&pdev->dev,
+						    base,
+						    &rockchip_regmap_config);
+		}
+	}
+
+	/* try to find the optional reference to the pmu syscon */
+	node = of_parse_phandle(np, "rockchip,pmu", 0);
+	if (node) {
+		info->regmap_pmu = syscon_node_to_regmap(node);
+		if (IS_ERR(info->regmap_pmu))
+			return PTR_ERR(info->regmap_pmu);
 	}
 
 	ret = rockchip_gpiolib_register(pdev, info);
diff --git a/drivers/pinctrl/pinctrl-samsung.c b/drivers/pinctrl/pinctrl-samsung.c
index 0324d4cb19b..3e61d0f8f14 100644
--- a/drivers/pinctrl/pinctrl-samsung.c
+++ b/drivers/pinctrl/pinctrl-samsung.c
@@ -1114,6 +1114,8 @@ static struct syscore_ops samsung_pinctrl_syscore_ops = {
 
 static const struct of_device_id samsung_pinctrl_dt_match[] = {
 #ifdef CONFIG_PINCTRL_EXYNOS
+	{ .compatible = "samsung,exynos3250-pinctrl",
+		.data = (void *)exynos3250_pin_ctrl },
 	{ .compatible = "samsung,exynos4210-pinctrl",
 		.data = (void *)exynos4210_pin_ctrl },
 	{ .compatible = "samsung,exynos4x12-pinctrl",
diff --git a/drivers/pinctrl/pinctrl-samsung.h b/drivers/pinctrl/pinctrl-samsung.h
index bab9c212255..b3e41fa5798 100644
--- a/drivers/pinctrl/pinctrl-samsung.h
+++ b/drivers/pinctrl/pinctrl-samsung.h
@@ -251,6 +251,7 @@ struct samsung_pmx_func {
 };
 
 /* list of all exported SoC specific data */
+extern struct samsung_pin_ctrl exynos3250_pin_ctrl[];
 extern struct samsung_pin_ctrl exynos4210_pin_ctrl[];
 extern struct samsung_pin_ctrl exynos4x12_pin_ctrl[];
 extern struct samsung_pin_ctrl exynos5250_pin_ctrl[];
diff --git a/drivers/pinctrl/pinctrl-single.c b/drivers/pinctrl/pinctrl-single.c
index 81075f2a1d3..2960557bfed 100644
--- a/drivers/pinctrl/pinctrl-single.c
+++ b/drivers/pinctrl/pinctrl-single.c
@@ -810,6 +810,7 @@ static const struct pinconf_ops pcs_pinconf_ops = {
 static int pcs_add_pin(struct pcs_device *pcs, unsigned offset,
 		unsigned pin_pos)
 {
+	struct pcs_soc_data *pcs_soc = &pcs->socdata;
 	struct pinctrl_pin_desc *pin;
 	struct pcs_name *pn;
 	int i;
@@ -821,6 +822,18 @@ static int pcs_add_pin(struct pcs_device *pcs, unsigned offset,
 		return -ENOMEM;
 	}
 
+	if (pcs_soc->irq_enable_mask) {
+		unsigned val;
+
+		val = pcs->read(pcs->base + offset);
+		if (val & pcs_soc->irq_enable_mask) {
+			dev_dbg(pcs->dev, "irq enabled at boot for pin at %lx (%x), clearing\n",
+				(unsigned long)pcs->res->start + offset, val);
+			val &= ~pcs_soc->irq_enable_mask;
+			pcs->write(val, pcs->base + offset);
+		}
+	}
+
 	pin = &pcs->pins.pa[i];
 	pn = &pcs->names[i];
 	sprintf(pn->name, "%lx.%d",
diff --git a/drivers/pinctrl/pinctrl-st.c b/drivers/pinctrl/pinctrl-st.c
index bd725b0a434..1bd6363bc95 100644
--- a/drivers/pinctrl/pinctrl-st.c
+++ b/drivers/pinctrl/pinctrl-st.c
@@ -13,10 +13,6 @@
 #include <linux/slab.h>
 #include <linux/err.h>
 #include <linux/io.h>
-#include <linux/irq.h>
-#include <linux/irqdesc.h>
-#include <linux/irqdomain.h>
-#include <linux/irqchip/chained_irq.h>
 #include <linux/of.h>
 #include <linux/of_irq.h>
 #include <linux/of_gpio.h>
@@ -242,13 +238,13 @@ struct st_pio_control {
 };
 
 struct st_pctl_data {
-	enum st_retime_style rt_style;
-	unsigned int	*input_delays;
-	int		ninput_delays;
-	unsigned int	*output_delays;
-	int		noutput_delays;
+	const enum st_retime_style	rt_style;
+	const unsigned int		*input_delays;
+	const int			ninput_delays;
+	const unsigned int		*output_delays;
+	const int			noutput_delays;
 	/* register offset information */
-	int alt, oe, pu, od, rt;
+	const int alt, oe, pu, od, rt;
 };
 
 struct st_pinconf {
@@ -321,7 +317,6 @@ struct st_gpio_bank {
 	struct pinctrl_gpio_range	range;
 	void __iomem			*base;
 	struct st_pio_control		pc;
-	struct	irq_domain		*domain;
 	unsigned long			irq_edge_conf;
 	spinlock_t                      lock;
 };
@@ -342,15 +337,15 @@ struct st_pinctrl {
 
 /* SOC specific data */
 /* STiH415 data */
-static unsigned int stih415_input_delays[] = {0, 500, 1000, 1500};
-static unsigned int stih415_output_delays[] = {0, 1000, 2000, 3000};
+static const unsigned int stih415_input_delays[] = {0, 500, 1000, 1500};
+static const unsigned int stih415_output_delays[] = {0, 1000, 2000, 3000};
 
 #define STIH415_PCTRL_COMMON_DATA				\
 	.rt_style	= st_retime_style_packed,		\
 	.input_delays	= stih415_input_delays,			\
-	.ninput_delays	= 4,					\
+	.ninput_delays	= ARRAY_SIZE(stih415_input_delays),	\
 	.output_delays = stih415_output_delays,			\
-	.noutput_delays = 4
+	.noutput_delays = ARRAY_SIZE(stih415_output_delays)
 
 static const struct st_pctl_data  stih415_sbc_data = {
 	STIH415_PCTRL_COMMON_DATA,
@@ -378,8 +373,8 @@ static const struct st_pctl_data  stih415_right_data = {
 };
 
 /* STiH416 data */
-static unsigned int stih416_delays[] = {0, 300, 500, 750, 1000, 1250, 1500,
-			1750, 2000, 2250, 2500, 2750, 3000, 3250 };
+static const unsigned int stih416_delays[] = {0, 300, 500, 750, 1000, 1250,
+			1500, 1750, 2000, 2250, 2500, 2750, 3000, 3250 };
 
 static const struct st_pctl_data  stih416_data = {
 	.rt_style	= st_retime_style_dedicated,
@@ -468,7 +463,7 @@ static void st_pctl_set_function(struct st_pio_control *pc,
 static unsigned long st_pinconf_delay_to_bit(unsigned int delay,
 	const struct st_pctl_data *data, unsigned long config)
 {
-	unsigned int *delay_times;
+	const unsigned int *delay_times;
 	int num_delay_times, i, closest_index = -1;
 	unsigned int closest_divergence = UINT_MAX;
 
@@ -501,7 +496,7 @@ static unsigned long st_pinconf_delay_to_bit(unsigned int delay,
 static unsigned long st_pinconf_bit_to_delay(unsigned int index,
 	const struct st_pctl_data *data, unsigned long output)
 {
-	unsigned int *delay_times;
+	const unsigned int *delay_times;
 	int num_delay_times;
 
 	if (output) {
@@ -1285,58 +1280,26 @@ static int st_pctl_parse_functions(struct device_node *np,
 	return 0;
 }
 
-static int st_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
-{
-	struct st_gpio_bank *bank = gpio_chip_to_bank(chip);
-	int irq = -ENXIO;
-
-	if (offset < chip->ngpio)
-		irq = irq_find_mapping(bank->domain, offset);
-
-	dev_info(chip->dev, "%s: request IRQ for GPIO %d, return %d\n",
-				chip->label, offset + chip->base, irq);
-	return irq;
-}
-
 static void st_gpio_irq_mask(struct irq_data *d)
 {
-	struct st_gpio_bank *bank = irq_data_get_irq_chip_data(d);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct st_gpio_bank *bank = gpio_chip_to_bank(gc);
 
 	writel(BIT(d->hwirq), bank->base + REG_PIO_CLR_PMASK);
 }
 
 static void st_gpio_irq_unmask(struct irq_data *d)
 {
-	struct st_gpio_bank *bank = irq_data_get_irq_chip_data(d);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct st_gpio_bank *bank = gpio_chip_to_bank(gc);
 
 	writel(BIT(d->hwirq), bank->base + REG_PIO_SET_PMASK);
 }
 
-static unsigned int st_gpio_irq_startup(struct irq_data *d)
-{
-	struct st_gpio_bank *bank = irq_data_get_irq_chip_data(d);
-
-	if (gpio_lock_as_irq(&bank->gpio_chip, d->hwirq))
-		dev_err(bank->gpio_chip.dev,
-			"unable to lock HW IRQ %lu for IRQ\n",
-			d->hwirq);
-
-	st_gpio_irq_unmask(d);
-
-	return 0;
-}
-
-static void st_gpio_irq_shutdown(struct irq_data *d)
-{
-	struct st_gpio_bank *bank = irq_data_get_irq_chip_data(d);
-
-	st_gpio_irq_mask(d);
-	gpio_unlock_as_irq(&bank->gpio_chip, d->hwirq);
-}
-
 static int st_gpio_irq_set_type(struct irq_data *d, unsigned type)
 {
-	struct st_gpio_bank *bank = irq_data_get_irq_chip_data(d);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct st_gpio_bank *bank = gpio_chip_to_bank(gc);
 	unsigned long flags;
 	int comp, pin = d->hwirq;
 	u32 val;
@@ -1440,7 +1403,7 @@ static void __gpio_irq_handler(struct st_gpio_bank *bank)
 					continue;
 			}
 
-			generic_handle_irq(irq_find_mapping(bank->domain, n));
+			generic_handle_irq(irq_find_mapping(bank->gpio_chip.irqdomain, n));
 		}
 	}
 }
@@ -1449,7 +1412,8 @@ static void st_gpio_irq_handler(unsigned irq, struct irq_desc *desc)
 {
 	/* interrupt dedicated per bank */
 	struct irq_chip *chip = irq_get_chip(irq);
-	struct st_gpio_bank *bank = irq_get_handler_data(irq);
+	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
+	struct st_gpio_bank *bank = gpio_chip_to_bank(gc);
 
 	chained_irq_enter(chip, desc);
 	__gpio_irq_handler(bank);
@@ -1483,7 +1447,6 @@ static struct gpio_chip st_gpio_template = {
 	.ngpio			= ST_GPIO_PINS_PER_BANK,
 	.of_gpio_n_cells	= 1,
 	.of_xlate		= st_gpio_xlate,
-	.to_irq			= st_gpio_to_irq,
 };
 
 static struct irq_chip st_gpio_irqchip = {
@@ -1491,26 +1454,6 @@ static struct irq_chip st_gpio_irqchip = {
 	.irq_mask	= st_gpio_irq_mask,
 	.irq_unmask	= st_gpio_irq_unmask,
 	.irq_set_type	= st_gpio_irq_set_type,
-	.irq_startup	= st_gpio_irq_startup,
-	.irq_shutdown	= st_gpio_irq_shutdown,
-};
-
-static int st_gpio_irq_domain_map(struct irq_domain *h,
-			unsigned int virq, irq_hw_number_t hw)
-{
-	struct st_gpio_bank *bank = h->host_data;
-
-	irq_set_chip(virq, &st_gpio_irqchip);
-	irq_set_handler(virq, handle_simple_irq);
-	set_irq_flags(virq, IRQF_VALID);
-	irq_set_chip_data(virq, bank);
-
-	return 0;
-}
-
-static struct irq_domain_ops st_gpio_irq_ops = {
-	.map	= st_gpio_irq_domain_map,
-	.xlate	= irq_domain_xlate_twocell,
 };
 
 static int st_gpiolib_register_bank(struct st_pinctrl *info,
@@ -1521,7 +1464,7 @@ static int st_gpiolib_register_bank(struct st_pinctrl *info,
 	struct device *dev = info->dev;
 	int bank_num = of_alias_get_id(np, "gpio");
 	struct resource res, irq_res;
-	int gpio_irq = 0, err, i;
+	int gpio_irq = 0, err;
 
 	if (of_address_to_resource(np, 0, &res))
 		return -ENODEV;
@@ -1534,6 +1477,7 @@ static int st_gpiolib_register_bank(struct st_pinctrl *info,
 	bank->gpio_chip.base = bank_num * ST_GPIO_PINS_PER_BANK;
 	bank->gpio_chip.ngpio = ST_GPIO_PINS_PER_BANK;
 	bank->gpio_chip.of_node = np;
+	bank->gpio_chip.dev = dev;
 	spin_lock_init(&bank->lock);
 
 	of_property_read_string(np, "st,bank-name", &range->name);
@@ -1571,26 +1515,18 @@ static int st_gpiolib_register_bank(struct st_pinctrl *info,
 
 	if (of_irq_to_resource(np, 0, &irq_res)) {
 		gpio_irq = irq_res.start;
-		irq_set_chained_handler(gpio_irq, st_gpio_irq_handler);
-		irq_set_handler_data(gpio_irq, bank);
+		gpiochip_set_chained_irqchip(&bank->gpio_chip, &st_gpio_irqchip,
+					     gpio_irq, st_gpio_irq_handler);
 	}
 
 	if (info->irqmux_base > 0 || gpio_irq > 0) {
-		/* Setup IRQ domain */
-		bank->domain  = irq_domain_add_linear(np,
-						ST_GPIO_PINS_PER_BANK,
-						&st_gpio_irq_ops, bank);
-		if (!bank->domain) {
-			dev_err(dev, "Failed to add irq domain for %s\n",
-				np->full_name);
-		} else  {
-			for (i = 0; i < ST_GPIO_PINS_PER_BANK; i++) {
-				if (irq_create_mapping(bank->domain, i) < 0)
-					dev_err(dev,
-						"Failed to map IRQ %i\n", i);
-			}
+		err = gpiochip_irqchip_add(&bank->gpio_chip, &st_gpio_irqchip,
+					   0, handle_simple_irq,
+					   IRQ_TYPE_LEVEL_LOW);
+		if (err) {
+			dev_info(dev, "could not add irqchip\n");
+			return err;
 		}
-
 	} else {
 		dev_info(dev, "No IRQ support for %s bank\n", np->full_name);
 	}
diff --git a/drivers/pinctrl/pinctrl-sunxi-pins.h b/drivers/pinctrl/pinctrl-sunxi-pins.h
deleted file mode 100644
index 3d6066988a7..00000000000
--- a/drivers/pinctrl/pinctrl-sunxi-pins.h
+++ /dev/null
@@ -1,3863 +0,0 @@
-/*
- * Allwinner A1X SoCs pinctrl driver.
- *
- * Copyright (C) 2012 Maxime Ripard
- *
- * Maxime Ripard <maxime.ripard@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 __PINCTRL_SUNXI_PINS_H
-#define __PINCTRL_SUNXI_PINS_H
-
-#include "pinctrl-sunxi.h"
-
-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(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(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(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(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(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(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(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(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(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(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(0x2, "i2c0")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "pwm")),		/* PWM0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "ir0")),		/* RX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(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(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(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(0x2, "i2s")),		/* DO1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "i2s")),		/* DO3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "spi2")),		/* CS1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(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(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(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(0x2, "i2c1")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "i2c2")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB21,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(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(0x3, "ir1")),		/* RX */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(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(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(0x2, "nand0")),	/* NCE1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "nand0")),	/* NRE# */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(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(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(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(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(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(0x2, "nand0")),	/* NDQ4 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "nand0")),	/* NDQ6 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "nand0")),	/* NWP */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "nand0")),	/* NCE3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(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(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(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(0x3, "spi0")),		/* CS0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC24,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(0x2, "lcd1"),		/* D0 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAA0 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 0),		/* EINT0 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D1 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAA1 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 1),		/* EINT1 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D2 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAA2 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* RTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 2),		/* EINT2 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D3 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAIRQ */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* CTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 3),		/* EINT3 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D4 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD0 */
-		  SUNXI_FUNCTION(0x4, "uart4"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 4),		/* EINT4 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D4 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D5 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD1 */
-		  SUNXI_FUNCTION(0x4, "uart4"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 5),		/* EINT5 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D5 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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_IRQ(0x6, 6),		/* EINT6 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D6 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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_IRQ(0x6, 7),		/* EINT7 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D7 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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_IRQ(0x6, 8),		/* EINT8 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D8 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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_IRQ(0x6, 9),		/* EINT9 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D9 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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_IRQ(0x6, 10),		/* EINT10 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D10 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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_IRQ(0x6, 11),		/* EINT11 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D11 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D12 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD8 */
-		  SUNXI_FUNCTION(0x4, "ps2"),		/* SCK1 */
-		  SUNXI_FUNCTION_IRQ(0x6, 12),		/* EINT12 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D12 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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_IRQ(0x6, 13),		/* EINT13 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D13 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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_IRQ(0x6, 14),		/* EINT14 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D14 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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_IRQ(0x6, 15),		/* EINT15 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D15 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D16 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD12 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN6 */
-		  SUNXI_FUNCTION_IRQ(0x6, 16),		/* EINT16 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D16 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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_IRQ(0x6, 17),		/* EINT17 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D17 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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_IRQ(0x6, 18),		/* EINT18 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D18 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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_IRQ(0x6, 19),		/* EINT19 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D19 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH20,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D20 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAOE */
-		  SUNXI_FUNCTION(0x4, "can"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 20),		/* EINT20 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D20 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH21,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D21 */
-		  SUNXI_FUNCTION(0x3, "pata"),		/* ATADREQ */
-		  SUNXI_FUNCTION(0x4, "can"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 21),		/* EINT21 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D21 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH22,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(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(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(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(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(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"),
-		  SUNXI_FUNCTION(0x1, "gpio_out")),
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out")),
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out")),
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "mmc3")),		/* CMD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "mmc3")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "mmc3")),		/* D2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "spi0"),		/* CS0 */
-		  SUNXI_FUNCTION(0x3, "uart5"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 22)),		/* EINT22 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi0"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "uart5"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 23)),		/* EINT23 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi0"),		/* MOSI */
-		  SUNXI_FUNCTION(0x3, "uart6"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 24)),		/* EINT24 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi0"),		/* MISO */
-		  SUNXI_FUNCTION(0x3, "uart6"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 25)),		/* EINT25 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi0"),		/* CS1 */
-		  SUNXI_FUNCTION(0x3, "ps2"),		/* SCK1 */
-		  SUNXI_FUNCTION(0x4, "timer4"),	/* TCLKIN0 */
-		  SUNXI_FUNCTION_IRQ(0x6, 26)),		/* EINT26 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS1 */
-		  SUNXI_FUNCTION(0x3, "ps2"),		/* SDA1 */
-		  SUNXI_FUNCTION(0x4, "timer5"),	/* TCLKIN1 */
-		  SUNXI_FUNCTION_IRQ(0x6, 27)),		/* EINT27 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS0 */
-		  SUNXI_FUNCTION(0x3, "uart2"),		/* RTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 28)),		/* EINT28 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "uart2"),		/* CTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 29)),		/* EINT29 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* MOSI */
-		  SUNXI_FUNCTION(0x3, "uart2"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 30)),		/* EINT30 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* MISO */
-		  SUNXI_FUNCTION(0x3, "uart2"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 31)),		/* EINT31 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI20,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "ps2"),		/* SDA0 */
-		  SUNXI_FUNCTION(0x3, "uart7"),		/* RX */
-		  SUNXI_FUNCTION(0x4, "hdmi")),		/* HSDA */
-};
-
-static const struct sunxi_desc_pin sun5i_a10s_pins[] = {
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD3 */
-		  SUNXI_FUNCTION(0x3, "ts0"),		/* CLK */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD2 */
-		  SUNXI_FUNCTION(0x3, "ts0"),		/* ERR */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD1 */
-		  SUNXI_FUNCTION(0x3, "ts0"),		/* SYNC */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD0 */
-		  SUNXI_FUNCTION(0x3, "ts0"),		/* DLVD */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD3 */
-		  SUNXI_FUNCTION(0x3, "ts0"),		/* D0 */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN4 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD2 */
-		  SUNXI_FUNCTION(0x3, "ts0"),		/* D1 */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN5 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD1 */
-		  SUNXI_FUNCTION(0x3, "ts0"),		/* D2 */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN6 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD0 */
-		  SUNXI_FUNCTION(0x3, "ts0"),		/* D3 */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN7 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXCK */
-		  SUNXI_FUNCTION(0x3, "ts0"),		/* D4 */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* DTR */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXERR */
-		  SUNXI_FUNCTION(0x3, "ts0"),		/* D5 */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* DSR */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXDV */
-		  SUNXI_FUNCTION(0x3, "ts0"),		/* D6 */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* DCD */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* EMDC */
-		  SUNXI_FUNCTION(0x3, "ts0"),		/* D7 */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* RING */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* EMDIO */
-		  SUNXI_FUNCTION(0x3, "uart1"),		/* TX */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT4 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXEN */
-		  SUNXI_FUNCTION(0x3, "uart1"),		/* RX */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT5 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXCK */
-		  SUNXI_FUNCTION(0x3, "uart1"),		/* CTS */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* TX */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT6 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ECRS */
-		  SUNXI_FUNCTION(0x3, "uart1"),		/* RTS */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* RX */
-		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT7 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ECOL */
-		  SUNXI_FUNCTION(0x3, "uart2")),	/* TX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXERR */
-		  SUNXI_FUNCTION(0x3, "uart2"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 31)),		/* EINT31 */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "i2c0")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "pwm"),		/* PWM0 */
-		  SUNXI_FUNCTION_IRQ(0x6, 16)),		/* EINT16 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ir0"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 17)),		/* EINT17 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ir0"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 18)),		/* EINT18 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s"),		/* MCLK */
-		  SUNXI_FUNCTION_IRQ(0x6, 19)),		/* EINT19 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s"),		/* BCLK */
-		  SUNXI_FUNCTION_IRQ(0x6, 20)),		/* EINT20 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s"),		/* LRCK */
-		  SUNXI_FUNCTION_IRQ(0x6, 21)),		/* EINT21 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s"),		/* DO */
-		  SUNXI_FUNCTION_IRQ(0x6, 22)),		/* EINT22 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s"),		/* DI */
-		  SUNXI_FUNCTION_IRQ(0x6, 23)),		/* EINT23 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* CS1 */
-		  SUNXI_FUNCTION_IRQ(0x6, 24)),		/* EINT24 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* CS0 */
-		  SUNXI_FUNCTION(0x3, "jtag"),		/* MS0 */
-		  SUNXI_FUNCTION_IRQ(0x6, 25)),		/* EINT25 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "jtag"),		/* CK0 */
-		  SUNXI_FUNCTION_IRQ(0x6, 26)),		/* EINT26 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* MOSI */
-		  SUNXI_FUNCTION(0x3, "jtag"),		/* DO0 */
-		  SUNXI_FUNCTION_IRQ(0x6, 27)),		/* EINT27 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* MISO */
-		  SUNXI_FUNCTION(0x3, "jtag"),		/* DI0 */
-		  SUNXI_FUNCTION_IRQ(0x6, 28)),		/* EINT28 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "i2c1")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "i2c2")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "uart0"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 29)),		/* EINT29 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB20,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "uart0"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 30)),		/* EINT30 */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(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(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(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(0x2, "nand0")),	/* NCE0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(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(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(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(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(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(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(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(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(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(0x2, "nand0"),		/* NDQ7 */
-		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D7 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NWP */
-		  SUNXI_FUNCTION(0x4, "uart3")),	/* TX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE2 */
-		  SUNXI_FUNCTION(0x4, "uart3")),	/* RX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE3 */
-		  SUNXI_FUNCTION(0x3, "uart2"),		/* TX */
-		  SUNXI_FUNCTION(0x4, "uart3")),	/* CTS */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE4 */
-		  SUNXI_FUNCTION(0x3, "uart2"),		/* RX */
-		  SUNXI_FUNCTION(0x4, "uart3")),	/* RTS */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D2 */
-		  SUNXI_FUNCTION(0x3, "uart2")),	/* TX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D3 */
-		  SUNXI_FUNCTION(0x3, "uart2")),	/* RX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D4 */
-		  SUNXI_FUNCTION(0x3, "uart2")),	/* CTS */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D5 */
-		  SUNXI_FUNCTION(0x3, "uart2")),	/* RTS */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D6 */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ECRS */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D7 */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ECOL */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D8 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D9 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D10 */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ERXD0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D11 */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ERXD1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D12 */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ERXD2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D13 */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ERXD3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D14 */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ERXCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D15 */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ERXERR */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D16 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D17 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D18 */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ERXDV */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D19 */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ETXD0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD20,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D20 */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ETXD1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD21,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D21 */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ETXD2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD22,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D22 */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ETXD3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD23,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D23 */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ETXEN */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD24,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ETXCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD25,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* DE */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* ETXERR */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD26,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* HSYNC */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* EMDC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD27,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0"),		/* VSYNC */
-		  SUNXI_FUNCTION(0x3, "emac")),		/* EMDIO */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* PCK */
-		  SUNXI_FUNCTION(0x4, "spi2"),		/* CS0 */
-		  SUNXI_FUNCTION_IRQ(0x6, 14)),		/* EINT14 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* ERR */
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* CK */
-		  SUNXI_FUNCTION(0x4, "spi2"),		/* CLK */
-		  SUNXI_FUNCTION_IRQ(0x6, 15)),		/* EINT15 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x2, "ts0"),		/* SYNC */
-		  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(0x2, "ts0"),		/* DVLD */
-		  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(0x2, "ts0"),		/* D0 */
-		  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(0x2, "ts0"),		/* D1 */
-		  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(0x2, "ts0"),		/* D2 */
-		  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(0x2, "ts0"),		/* D3 */
-		  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(0x2, "ts0"),		/* D4 */
-		  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(0x2, "ts0"),		/* D5 */
-		  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(0x2, "ts0"),		/* D6 */
-		  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(0x2, "ts0"),		/* D7 */
-		  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(0x2, "mmc0"),		/* D1 */
-		  SUNXI_FUNCTION(0x4, "jtag")),		/* MS1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(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(0x2, "mmc0"),		/* D2 */
-		  SUNXI_FUNCTION(0x4, "jtag")),		/* CK1 */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x2, "gps"),		/* CLK */
-		  SUNXI_FUNCTION_IRQ(0x6, 0)),		/* EINT0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x2, "gps"),		/* SIGN */
-		  SUNXI_FUNCTION_IRQ(0x6, 1)),		/* EINT1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x2, "gps"),		/* MAG */
-		  SUNXI_FUNCTION_IRQ(0x6, 2)),		/* EINT2 */
-	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_FUNCTION_IRQ(0x6, 3)),		/* EINT3 */
-	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 */
-		  SUNXI_FUNCTION_IRQ(0x6, 4)),		/* EINT4 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc1"),		/* DO */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* CTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 5)),		/* EINT5 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc1"),		/* D1 */
-		  SUNXI_FUNCTION(0x4, "uart1"),		/* RTS */
-		  SUNXI_FUNCTION(0x5, "uart2"),		/* RTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 6)),		/* EINT6 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc1"),		/* D2 */
-		  SUNXI_FUNCTION(0x5, "uart2"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 7)),		/* EINT7 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc1"),		/* D3 */
-		  SUNXI_FUNCTION(0x5, "uart2"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 8)),		/* EINT8 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS0 */
-		  SUNXI_FUNCTION(0x3, "uart3"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 9)),		/* EINT9 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "uart3"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 10)),		/* EINT10 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* MOSI */
-		  SUNXI_FUNCTION(0x3, "uart3"),		/* CTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 11)),		/* EINT11 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* MISO */
-		  SUNXI_FUNCTION(0x3, "uart3"),		/* RTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 12)),		/* EINT12 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS1 */
-		  SUNXI_FUNCTION(0x3, "uart3"),		/* PWM1 */
-		  SUNXI_FUNCTION(0x5, "uart2"),		/* CTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 13)),		/* EINT13 */
-};
-
-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(0x2, "i2c0")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "pwm"),
-		  SUNXI_FUNCTION_IRQ(0x6, 16)),		/* EINT16 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ir0"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 17)),		/* EINT17 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "ir0"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 18)),		/* EINT18 */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* CS1 */
-		  SUNXI_FUNCTION_IRQ(0x6, 24)),		/* EINT24 */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "i2c1")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "i2c2")),		/* SDA */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(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(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(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(0x2, "nand0")),	/* NCE0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(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(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(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(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(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(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(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(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(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(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(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(0x2, "lcd0")),		/* D2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "lcd0")),		/* D4 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "lcd0")),		/* D6 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "lcd0")),		/* D10 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "lcd0")),		/* D12 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "lcd0")),		/* D14 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "lcd0")),		/* D18 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "lcd0")),		/* D20 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD21,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "lcd0")),		/* D22 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD23,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "lcd0")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD25,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "lcd0")),		/* HSYNC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD27,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd0")),		/* VSYNC */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* PCLK */
-		  SUNXI_FUNCTION(0x4, "spi2"),		/* CS0 */
-		  SUNXI_FUNCTION_IRQ(0x6, 14)),		/* EINT14 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x3, "csi0"),		/* MCLK */
-		  SUNXI_FUNCTION(0x4, "spi2"),		/* CLK */
-		  SUNXI_FUNCTION_IRQ(0x6, 15)),		/* EINT15 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(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(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(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(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(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(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(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(0x2, "mmc0")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0")),		/* CMD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0")),		/* D2 */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION_IRQ(0x6, 0)),		/* EINT0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION_IRQ(0x6, 1)),		/* EINT1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION_IRQ(0x6, 2)),		/* EINT2 */
-	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_FUNCTION_IRQ(0x6, 3)),		/* EINT3 */
-	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 */
-		  SUNXI_FUNCTION_IRQ(0x6, 4)),		/* EINT4 */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS0 */
-		  SUNXI_FUNCTION(0x3, "uart3"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 9)),		/* EINT9 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "uart3"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 10)),		/* EINT10 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* MOSI */
-		  SUNXI_FUNCTION(0x3, "uart3"),		/* CTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 11)),		/* EINT11 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* MISO */
-		  SUNXI_FUNCTION(0x3, "uart3"),		/* RTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 12)),		/* EINT12 */
-};
-
-static const struct sunxi_desc_pin sun6i_a31_pins[] = {
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD0 */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D0 */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* DTR */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD1 */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D1 */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* DSR */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD2 */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D2 */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* DCD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD3 */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D3 */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* RING */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD4 */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D4 */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* TX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD5 */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D5 */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* RX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD6 */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D6 */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* RTS */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD7 */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D7 */
-		  SUNXI_FUNCTION(0x4, "uart1")),	/* CTS */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXCLK */
-		  SUNXI_FUNCTION(0x3, "lcd1")),		/* D8 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXEN */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D9 */
-		  SUNXI_FUNCTION(0x4, "mmc3"),		/* CMD */
-		  SUNXI_FUNCTION(0x5, "mmc2")),		/* CMD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* GTXCLK */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D10 */
-		  SUNXI_FUNCTION(0x4, "mmc3"),		/* CLK */
-		  SUNXI_FUNCTION(0x5, "mmc2")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD0 */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D11 */
-		  SUNXI_FUNCTION(0x4, "mmc3"),		/* D0 */
-		  SUNXI_FUNCTION(0x5, "mmc2")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD1 */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D12 */
-		  SUNXI_FUNCTION(0x4, "mmc3"),		/* D1 */
-		  SUNXI_FUNCTION(0x5, "mmc2")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD2 */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D13 */
-		  SUNXI_FUNCTION(0x4, "mmc3"),		/* D2 */
-		  SUNXI_FUNCTION(0x5, "mmc2")),		/* D2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD3 */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D14 */
-		  SUNXI_FUNCTION(0x4, "mmc3"),		/* D3 */
-		  SUNXI_FUNCTION(0x5, "mmc2")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD4 */
-		  SUNXI_FUNCTION(0x3, "lcd1")),		/* D15 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD5 */
-		  SUNXI_FUNCTION(0x3, "lcd1")),		/* D16 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD6 */
-		  SUNXI_FUNCTION(0x3, "lcd1")),		/* D17 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD7 */
-		  SUNXI_FUNCTION(0x3, "lcd1")),		/* D18 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXDV */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D19 */
-		  SUNXI_FUNCTION(0x4, "pwm3")),		/* Positive */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA20,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXCLK */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D20 */
-		  SUNXI_FUNCTION(0x4, "pwm3")),		/* Negative */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA21,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXERR */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D21 */
-		  SUNXI_FUNCTION(0x4, "spi3")),		/* CS0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA22,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXERR */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D22 */
-		  SUNXI_FUNCTION(0x4, "spi3")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA23,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* COL */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D23 */
-		  SUNXI_FUNCTION(0x4, "spi3")),		/* MOSI */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA24,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* CRS */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* CLK */
-		  SUNXI_FUNCTION(0x4, "spi3")),		/* MISO */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA25,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* CLKIN */
-		  SUNXI_FUNCTION(0x3, "lcd1"),		/* DE */
-		  SUNXI_FUNCTION(0x4, "spi3")),		/* CS1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA26,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* MDC */
-		  SUNXI_FUNCTION(0x3, "lcd1")),		/* HSYNC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA27,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "gmac"),		/* MDIO */
-		  SUNXI_FUNCTION(0x3, "lcd1")),		/* VSYNC */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0"),		/* MCLK */
-		  SUNXI_FUNCTION(0x3, "uart3"),		/* CTS */
-		  SUNXI_FUNCTION(0x4, "csi")),		/* MCLK1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0")),		/* BCLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0")),		/* LRCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0")),		/* DO0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0"),		/* DO1 */
-		  SUNXI_FUNCTION(0x3, "uart3")),	/* RTS */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0"),		/* DO2 */
-		  SUNXI_FUNCTION(0x3, "uart3"),		/* TX */
-		  SUNXI_FUNCTION(0x4, "i2c3")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0"),		/* DO3 */
-		  SUNXI_FUNCTION(0x3, "uart3"),		/* RX */
-		  SUNXI_FUNCTION(0x4, "i2c3")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x3, "i2s0")),		/* DI */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* WE */
-		  SUNXI_FUNCTION(0x3, "spi0")),		/* MOSI */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* ALE */
-		  SUNXI_FUNCTION(0x3, "spi0")),		/* MISO */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* CLE */
-		  SUNXI_FUNCTION(0x3, "spi0")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* CE1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* CE0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* RE */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* RB0 */
-		  SUNXI_FUNCTION(0x3, "mmc2"),		/* CMD */
-		  SUNXI_FUNCTION(0x4, "mmc3")),		/* CMD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* RB1 */
-		  SUNXI_FUNCTION(0x3, "mmc2"),		/* CLK */
-		  SUNXI_FUNCTION(0x4, "mmc3")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ0 */
-		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D0 */
-		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ1 */
-		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D1 */
-		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ2 */
-		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D2 */
-		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ3 */
-		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D3 */
-		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ4 */
-		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D4 */
-		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D4 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ5 */
-		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D5 */
-		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D5 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ6 */
-		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D6 */
-		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D6 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ7 */
-		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D7 */
-		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D7 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ8 */
-		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ9 */
-		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ10 */
-		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ11 */
-		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC20,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ12 */
-		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ4 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC21,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ13 */
-		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ5 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC22,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ14 */
-		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ6 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC23,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ15 */
-		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ7 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC24,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQS */
-		  SUNXI_FUNCTION(0x3, "mmc2"),		/* RST */
-		  SUNXI_FUNCTION(0x4, "mmc3")),		/* RST */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC25,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* CE2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC26,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0")),	/* CE3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC27,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x3, "spi0")),		/* CS0 */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(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(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(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(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(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(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(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(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(0x2, "lcd0"),		/* D9 */
-		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(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(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(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(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(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(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(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(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(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(0x2, "lcd0")),		/* D20 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD21,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "lcd0")),		/* D22 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD23,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "lcd0")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD25,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "lcd0")),		/* HSYNC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD27,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "csi"),		/* PCLK */
-		  SUNXI_FUNCTION(0x3, "ts")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* MCLK */
-		  SUNXI_FUNCTION(0x3, "ts")),		/* ERR */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* HSYNC */
-		  SUNXI_FUNCTION(0x3, "ts")),		/* SYNC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* VSYNC */
-		  SUNXI_FUNCTION(0x3, "ts")),		/* DVLD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* D0 */
-		  SUNXI_FUNCTION(0x3, "uart5")),	/* TX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* D1 */
-		  SUNXI_FUNCTION(0x3, "uart5")),	/* RX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* D2 */
-		  SUNXI_FUNCTION(0x3, "uart5")),	/* RTS */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* D3 */
-		  SUNXI_FUNCTION(0x3, "uart5")),	/* CTS */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* D4 */
-		  SUNXI_FUNCTION(0x3, "ts")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* D5 */
-		  SUNXI_FUNCTION(0x3, "ts")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* D6 */
-		  SUNXI_FUNCTION(0x3, "ts")),		/* D2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* D7 */
-		  SUNXI_FUNCTION(0x3, "ts")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* D8 */
-		  SUNXI_FUNCTION(0x3, "ts")),		/* D4 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* D9 */
-		  SUNXI_FUNCTION(0x3, "ts")),		/* D5 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* D10 */
-		  SUNXI_FUNCTION(0x3, "ts")),		/* D6 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi"),		/* D11 */
-		  SUNXI_FUNCTION(0x3, "ts")),		/* D7 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PE16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "csi")),		/* MIPI CSI MCLK */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D1 */
-		  SUNXI_FUNCTION(0x4, "jtag")),		/* MS1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PF1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(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(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(0x2, "mmc1")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc1")),		/* CMD */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc1")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc1")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc1")),		/* D2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "mmc1")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "uart2")),	/* TX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "uart2")),	/* RX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "uart2")),	/* RTS */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "uart2")),	/* CTS */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c3"),		/* SCK */
-		  SUNXI_FUNCTION(0x3, "usb")),		/* DP3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c3"),		/* SDA */
-		  SUNXI_FUNCTION(0x3, "usb")),		/* DM3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS1 */
-		  SUNXI_FUNCTION(0x3, "i2s1")),		/* MCLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS0 */
-		  SUNXI_FUNCTION(0x3, "i2s1")),		/* BCLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "i2s1")),		/* LRCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* MOSI */
-		  SUNXI_FUNCTION(0x3, "i2s1")),		/* DIN */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* MISO */
-		  SUNXI_FUNCTION(0x3, "i2s1")),		/* DOUT */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "uart4")),	/* TX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PG18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "uart4")),	/* RX */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand1")),	/* WE */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand1")),	/* ALE */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand1")),	/* CLE */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand1")),	/* CE1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand1")),	/* CE0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand1")),	/* RE */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand1")),	/* RB0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand1")),	/* RB1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand1")),	/* DQS */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* CS0 */
-		  SUNXI_FUNCTION(0x3, "jtag"),		/* MS0 */
-		  SUNXI_FUNCTION(0x4, "pwm1")),		/* Positive */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "jtag"),		/* CK0 */
-		  SUNXI_FUNCTION(0x4, "pwm1")),		/* Negative */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* MOSI */
-		  SUNXI_FUNCTION(0x3, "jtag"),		/* DO0 */
-		  SUNXI_FUNCTION(0x4, "pwm2")),		/* Positive */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* MISO */
-		  SUNXI_FUNCTION(0x3, "jtag"),		/* DI0 */
-		  SUNXI_FUNCTION(0x4, "pwm2")),		/* Negative */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "pwm0")),
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH20,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "uart0")),	/* TX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH21,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "uart0")),	/* RX */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH22,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out")),
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH23,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out")),
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH24,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out")),
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH25,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out")),
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH26,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out")),
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH27,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out")),
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH28,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out")),
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH29,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand1")),	/* CE2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH30,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand1")),	/* CE3 */
-};
-
-static const struct sunxi_desc_pin sun7i_a20_pins[] = {
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD3 */
-		  SUNXI_FUNCTION(0x3, "spi1"),		/* CS0 */
-		  SUNXI_FUNCTION(0x4, "uart2"),		/* RTS */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GRXD3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD2 */
-		  SUNXI_FUNCTION(0x3, "spi1"),		/* CLK */
-		  SUNXI_FUNCTION(0x4, "uart2"),		/* CTS */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GRXD2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD1 */
-		  SUNXI_FUNCTION(0x3, "spi1"),		/* MOSI */
-		  SUNXI_FUNCTION(0x4, "uart2"),		/* TX */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GRXD1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD0 */
-		  SUNXI_FUNCTION(0x3, "spi1"),		/* MISO */
-		  SUNXI_FUNCTION(0x4, "uart2"),		/* RX */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GRXD0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD3 */
-		  SUNXI_FUNCTION(0x3, "spi1"),		/* CS1 */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GTXD3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD2 */
-		  SUNXI_FUNCTION(0x3, "spi3"),		/* CS0 */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GTXD2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD1 */
-		  SUNXI_FUNCTION(0x3, "spi3"),		/* CLK */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GTXD1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD0 */
-		  SUNXI_FUNCTION(0x3, "spi3"),		/* MOSI */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GTXD0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXCK */
-		  SUNXI_FUNCTION(0x3, "spi3"),		/* MISO */
-		  SUNXI_FUNCTION(0x5, "gmac")),		/* GRXCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PA9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXERR */
-		  SUNXI_FUNCTION(0x3, "spi3"),		/* CS1 */
-		  SUNXI_FUNCTION(0x5, "gmac"),		/* GNULL / ERXERR */
-		  SUNXI_FUNCTION(0x6, "i2s1")),		/* MCLK */
-	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_FUNCTION(0x5, "gmac")),		/* GRXCTL / ERXDV */
-	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_FUNCTION(0x5, "gmac")),		/* EMDC */
-	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_FUNCTION(0x5, "gmac")),		/* EMDIO */
-	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_FUNCTION(0x5, "gmac")),		/* GTXCTL / ETXEN */
-	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_FUNCTION(0x5, "gmac"),		/* GNULL / ETXCK */
-		  SUNXI_FUNCTION(0x6, "i2s1")),		/* BCLK */
-	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_FUNCTION(0x5, "gmac"),		/* GTXCK / ECRS */
-		  SUNXI_FUNCTION(0x6, "i2s1")),		/* LRCK */
-	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_FUNCTION(0x5, "gmac"),		/* GCLKIN / ECOL */
-		  SUNXI_FUNCTION(0x6, "i2s1")),		/* DO */
-	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 */
-		  SUNXI_FUNCTION(0x5, "gmac"),		/* GNULL / ETXERR */
-		  SUNXI_FUNCTION(0x6, "i2s1")),		/* LRCK */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "i2c0")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "ir0"),		/* TX */
-		  SUNXI_FUNCTION(0x4, "spdif")),	/* MCLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "i2s0"),		/* MCLK */
-		  SUNXI_FUNCTION(0x3, "ac97")),		/* MCLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0"),		/* BCLK */
-		  SUNXI_FUNCTION(0x3, "ac97")),		/* BCLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0"),		/* LRCK */
-		  SUNXI_FUNCTION(0x3, "ac97")),		/* SYNC */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0"),		/* DO0 */
-		  SUNXI_FUNCTION(0x3, "ac97")),		/* DO */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0")),		/* DO1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0")),		/* DO2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0")),		/* DO3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "i2s0"),		/* DI */
-		  SUNXI_FUNCTION(0x3, "ac97"),		/* DI */
-		  SUNXI_FUNCTION(0x4, "spdif")),	/* DI */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi2"),		/* CS1 */
-		  SUNXI_FUNCTION(0x4, "spdif")),	/* DO */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(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(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(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(0x2, "i2c1")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "i2c2")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PB21,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(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(0x3, "ir1")),		/* RX */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(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(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(0x2, "nand0")),	/* NCE1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "nand0")),	/* NRE# */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(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(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(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(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(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(0x2, "nand0")),	/* NDQ4 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "nand0")),	/* NDQ6 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "nand0")),	/* NWP */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "nand0")),	/* NCE3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE4 */
-		  SUNXI_FUNCTION(0x3, "spi2"),		/* CS0 */
-		  SUNXI_FUNCTION_IRQ(0x6, 12)),		/* EINT12 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC20,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE5 */
-		  SUNXI_FUNCTION(0x3, "spi2"),		/* CLK */
-		  SUNXI_FUNCTION_IRQ(0x6, 13)),		/* EINT13 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC21,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE6 */
-		  SUNXI_FUNCTION(0x3, "spi2"),		/* MOSI */
-		  SUNXI_FUNCTION_IRQ(0x6, 14)),		/* EINT14 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC22,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE7 */
-		  SUNXI_FUNCTION(0x3, "spi2"),		/* MISO */
-		  SUNXI_FUNCTION_IRQ(0x6, 15)),		/* EINT15 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PC23,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "nand0")),	/* NDQS */
-	/* Hole */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PD0,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(0x2, "lcd1"),		/* D0 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 0),		/* EINT0 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D0 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D1 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 1),		/* EINT1 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D2 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* RTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 2),		/* EINT2 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D2 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D3 */
-		  SUNXI_FUNCTION(0x4, "uart3"),		/* CTS */
-		  SUNXI_FUNCTION_IRQ(0x6, 3),		/* EINT3 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D4 */
-		  SUNXI_FUNCTION(0x4, "uart4"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 4),		/* EINT4 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D4 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH5,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D5 */
-		  SUNXI_FUNCTION(0x4, "uart4"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 5),		/* EINT5 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D5 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D6 */
-		  SUNXI_FUNCTION(0x4, "uart5"),		/* TX */
-		  SUNXI_FUNCTION(0x5, "ms"),		/* BS */
-		  SUNXI_FUNCTION_IRQ(0x6, 6),		/* EINT6 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D6 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH7,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D7 */
-		  SUNXI_FUNCTION(0x4, "uart5"),		/* RX */
-		  SUNXI_FUNCTION(0x5, "ms"),		/* CLK */
-		  SUNXI_FUNCTION_IRQ(0x6, 7),		/* EINT7 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D7 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D8 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXD3 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN0 */
-		  SUNXI_FUNCTION(0x5, "ms"),		/* D0 */
-		  SUNXI_FUNCTION_IRQ(0x6, 8),		/* EINT8 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D8 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH9,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D9 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXD2 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN1 */
-		  SUNXI_FUNCTION(0x5, "ms"),		/* D1 */
-		  SUNXI_FUNCTION_IRQ(0x6, 9),		/* EINT9 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D9 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D10 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXD1 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN2 */
-		  SUNXI_FUNCTION(0x5, "ms"),		/* D2 */
-		  SUNXI_FUNCTION_IRQ(0x6, 10),		/* EINT10 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D10 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D11 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXD0 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN3 */
-		  SUNXI_FUNCTION(0x5, "ms"),		/* D3 */
-		  SUNXI_FUNCTION_IRQ(0x6, 11),		/* EINT11 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D11 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D12 */
-		  SUNXI_FUNCTION(0x4, "ps2"),		/* SCK1 */
-		  SUNXI_FUNCTION_IRQ(0x6, 12),		/* EINT12 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D12 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D13 */
-		  SUNXI_FUNCTION(0x4, "ps2"),		/* SDA1 */
-		  SUNXI_FUNCTION(0x5, "sim"),		/* RST */
-		  SUNXI_FUNCTION_IRQ(0x6, 13),		/* EINT13 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D13 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D14 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXD3 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN4 */
-		  SUNXI_FUNCTION(0x5, "sim"),		/* VPPEN */
-		  SUNXI_FUNCTION_IRQ(0x6, 14),		/* EINT14 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D14 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D15 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXD3 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN5 */
-		  SUNXI_FUNCTION(0x5, "sim"),		/* VPPPP */
-		  SUNXI_FUNCTION_IRQ(0x6, 15),		/* EINT15 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D15 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D16 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXD2 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN6 */
-		  SUNXI_FUNCTION_IRQ(0x6, 16),		/* EINT16 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D16 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D17 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXD1 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN7 */
-		  SUNXI_FUNCTION(0x5, "sim"),		/* VCCEN */
-		  SUNXI_FUNCTION_IRQ(0x6, 17),		/* EINT17 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D17 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D18 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXD0 */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT0 */
-		  SUNXI_FUNCTION(0x5, "sim"),		/* SCK */
-		  SUNXI_FUNCTION_IRQ(0x6, 18),		/* EINT18 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D18 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D19 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXERR */
-		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT1 */
-		  SUNXI_FUNCTION(0x5, "sim"),		/* SDA */
-		  SUNXI_FUNCTION_IRQ(0x6, 19),		/* EINT19 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D19 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH20,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D20 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXDV */
-		  SUNXI_FUNCTION(0x4, "can"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x6, 20),		/* EINT20 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D20 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH21,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D21 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* EMDC */
-		  SUNXI_FUNCTION(0x4, "can"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x6, 21),		/* EINT21 */
-		  SUNXI_FUNCTION(0x7, "csi1")),		/* D21 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PH22,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D22 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* EMDIO */
-		  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(0x2, "lcd1"),		/* D23 */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXEN */
-		  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(0x2, "lcd1"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXCK */
-		  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(0x2, "lcd1"),		/* DE */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ECRS */
-		  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(0x2, "lcd1"),		/* HSYNC */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ECOL */
-		  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(0x2, "lcd1"),		/* VSYNC */
-		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXERR */
-		  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"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x3, "i2c3")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI1,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x3, "i2c3")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI2,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x3, "i2c4")),		/* SCK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI3,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "pwm"),		/* PWM1 */
-		  SUNXI_FUNCTION(0x3, "i2c4")),		/* SDA */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI4,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "mmc3")),		/* CLK */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI6,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "mmc3")),		/* D1 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI8,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "mmc3")),		/* D3 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI10,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi0"),		/* CS0 */
-		  SUNXI_FUNCTION(0x3, "uart5"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x5, 22)),		/* EINT22 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI11,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi0"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "uart5"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x5, 23)),		/* EINT23 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI12,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi0"),		/* MOSI */
-		  SUNXI_FUNCTION(0x3, "uart6"),		/* TX */
-		  SUNXI_FUNCTION(0x4, "clk_out_a"),	/* CLK_OUT_A */
-		  SUNXI_FUNCTION_IRQ(0x5, 24)),		/* EINT24 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI13,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi0"),		/* MISO */
-		  SUNXI_FUNCTION(0x3, "uart6"),		/* RX */
-		  SUNXI_FUNCTION(0x4, "clk_out_b"),	/* CLK_OUT_B */
-		  SUNXI_FUNCTION_IRQ(0x5, 25)),		/* EINT25 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI14,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi0"),		/* CS1 */
-		  SUNXI_FUNCTION(0x3, "ps2"),		/* SCK1 */
-		  SUNXI_FUNCTION(0x4, "timer4"),	/* TCLKIN0 */
-		  SUNXI_FUNCTION_IRQ(0x5, 26)),		/* EINT26 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI15,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS1 */
-		  SUNXI_FUNCTION(0x3, "ps2"),		/* SDA1 */
-		  SUNXI_FUNCTION(0x4, "timer5"),	/* TCLKIN1 */
-		  SUNXI_FUNCTION_IRQ(0x5, 27)),		/* EINT27 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI16,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS0 */
-		  SUNXI_FUNCTION(0x3, "uart2"),		/* RTS */
-		  SUNXI_FUNCTION_IRQ(0x5, 28)),		/* EINT28 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI17,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* CLK */
-		  SUNXI_FUNCTION(0x3, "uart2"),		/* CTS */
-		  SUNXI_FUNCTION_IRQ(0x5, 29)),		/* EINT29 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI18,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* MOSI */
-		  SUNXI_FUNCTION(0x3, "uart2"),		/* TX */
-		  SUNXI_FUNCTION_IRQ(0x5, 30)),		/* EINT30 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI19,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x2, "spi1"),		/* MISO */
-		  SUNXI_FUNCTION(0x3, "uart2"),		/* RX */
-		  SUNXI_FUNCTION_IRQ(0x5, 31)),		/* EINT31 */
-	SUNXI_PIN(SUNXI_PINCTRL_PIN_PI20,
-		  SUNXI_FUNCTION(0x0, "gpio_in"),
-		  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(0x2, "ps2"),		/* SDA0 */
-		  SUNXI_FUNCTION(0x3, "uart7"),		/* RX */
-		  SUNXI_FUNCTION(0x4, "hdmi")),		/* HSDA */
-};
-
-static const struct sunxi_pinctrl_desc sun4i_a10_pinctrl_data = {
-	.pins = sun4i_a10_pins,
-	.npins = ARRAY_SIZE(sun4i_a10_pins),
-};
-
-static const struct sunxi_pinctrl_desc sun5i_a10s_pinctrl_data = {
-	.pins = sun5i_a10s_pins,
-	.npins = ARRAY_SIZE(sun5i_a10s_pins),
-};
-
-static const struct sunxi_pinctrl_desc sun5i_a13_pinctrl_data = {
-	.pins = sun5i_a13_pins,
-	.npins = ARRAY_SIZE(sun5i_a13_pins),
-};
-
-static const struct sunxi_pinctrl_desc sun6i_a31_pinctrl_data = {
-	.pins = sun6i_a31_pins,
-	.npins = ARRAY_SIZE(sun6i_a31_pins),
-};
-
-static const struct sunxi_pinctrl_desc sun7i_a20_pinctrl_data = {
-	.pins = sun7i_a20_pins,
-	.npins = ARRAY_SIZE(sun7i_a20_pins),
-};
-
-#endif /* __PINCTRL_SUNXI_PINS_H */
diff --git a/drivers/pinctrl/pinctrl-sunxi.h b/drivers/pinctrl/pinctrl-sunxi.h
deleted file mode 100644
index 552b0e97077..00000000000
--- a/drivers/pinctrl/pinctrl-sunxi.h
+++ /dev/null
@@ -1,548 +0,0 @@
-/*
- * Allwinner A1X SoCs pinctrl driver.
- *
- * Copyright (C) 2012 Maxime Ripard
- *
- * Maxime Ripard <maxime.ripard@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 __PINCTRL_SUNXI_H
-#define __PINCTRL_SUNXI_H
-
-#include <linux/kernel.h>
-#include <linux/spinlock.h>
-
-#define PA_BASE	0
-#define PB_BASE	32
-#define PC_BASE	64
-#define PD_BASE	96
-#define PE_BASE	128
-#define PF_BASE	160
-#define PG_BASE	192
-#define PH_BASE	224
-#define PI_BASE	256
-
-#define SUNXI_PINCTRL_PIN_PA0	PINCTRL_PIN(PA_BASE + 0, "PA0")
-#define SUNXI_PINCTRL_PIN_PA1	PINCTRL_PIN(PA_BASE + 1, "PA1")
-#define SUNXI_PINCTRL_PIN_PA2	PINCTRL_PIN(PA_BASE + 2, "PA2")
-#define SUNXI_PINCTRL_PIN_PA3	PINCTRL_PIN(PA_BASE + 3, "PA3")
-#define SUNXI_PINCTRL_PIN_PA4	PINCTRL_PIN(PA_BASE + 4, "PA4")
-#define SUNXI_PINCTRL_PIN_PA5	PINCTRL_PIN(PA_BASE + 5, "PA5")
-#define SUNXI_PINCTRL_PIN_PA6	PINCTRL_PIN(PA_BASE + 6, "PA6")
-#define SUNXI_PINCTRL_PIN_PA7	PINCTRL_PIN(PA_BASE + 7, "PA7")
-#define SUNXI_PINCTRL_PIN_PA8	PINCTRL_PIN(PA_BASE + 8, "PA8")
-#define SUNXI_PINCTRL_PIN_PA9	PINCTRL_PIN(PA_BASE + 9, "PA9")
-#define SUNXI_PINCTRL_PIN_PA10	PINCTRL_PIN(PA_BASE + 10, "PA10")
-#define SUNXI_PINCTRL_PIN_PA11	PINCTRL_PIN(PA_BASE + 11, "PA11")
-#define SUNXI_PINCTRL_PIN_PA12	PINCTRL_PIN(PA_BASE + 12, "PA12")
-#define SUNXI_PINCTRL_PIN_PA13	PINCTRL_PIN(PA_BASE + 13, "PA13")
-#define SUNXI_PINCTRL_PIN_PA14	PINCTRL_PIN(PA_BASE + 14, "PA14")
-#define SUNXI_PINCTRL_PIN_PA15	PINCTRL_PIN(PA_BASE + 15, "PA15")
-#define SUNXI_PINCTRL_PIN_PA16	PINCTRL_PIN(PA_BASE + 16, "PA16")
-#define SUNXI_PINCTRL_PIN_PA17	PINCTRL_PIN(PA_BASE + 17, "PA17")
-#define SUNXI_PINCTRL_PIN_PA18	PINCTRL_PIN(PA_BASE + 18, "PA18")
-#define SUNXI_PINCTRL_PIN_PA19	PINCTRL_PIN(PA_BASE + 19, "PA19")
-#define SUNXI_PINCTRL_PIN_PA20	PINCTRL_PIN(PA_BASE + 20, "PA20")
-#define SUNXI_PINCTRL_PIN_PA21	PINCTRL_PIN(PA_BASE + 21, "PA21")
-#define SUNXI_PINCTRL_PIN_PA22	PINCTRL_PIN(PA_BASE + 22, "PA22")
-#define SUNXI_PINCTRL_PIN_PA23	PINCTRL_PIN(PA_BASE + 23, "PA23")
-#define SUNXI_PINCTRL_PIN_PA24	PINCTRL_PIN(PA_BASE + 24, "PA24")
-#define SUNXI_PINCTRL_PIN_PA25	PINCTRL_PIN(PA_BASE + 25, "PA25")
-#define SUNXI_PINCTRL_PIN_PA26	PINCTRL_PIN(PA_BASE + 26, "PA26")
-#define SUNXI_PINCTRL_PIN_PA27	PINCTRL_PIN(PA_BASE + 27, "PA27")
-#define SUNXI_PINCTRL_PIN_PA28	PINCTRL_PIN(PA_BASE + 28, "PA28")
-#define SUNXI_PINCTRL_PIN_PA29	PINCTRL_PIN(PA_BASE + 29, "PA29")
-#define SUNXI_PINCTRL_PIN_PA30	PINCTRL_PIN(PA_BASE + 30, "PA30")
-#define SUNXI_PINCTRL_PIN_PA31	PINCTRL_PIN(PA_BASE + 31, "PA31")
-
-#define SUNXI_PINCTRL_PIN_PB0	PINCTRL_PIN(PB_BASE + 0, "PB0")
-#define SUNXI_PINCTRL_PIN_PB1	PINCTRL_PIN(PB_BASE + 1, "PB1")
-#define SUNXI_PINCTRL_PIN_PB2	PINCTRL_PIN(PB_BASE + 2, "PB2")
-#define SUNXI_PINCTRL_PIN_PB3	PINCTRL_PIN(PB_BASE + 3, "PB3")
-#define SUNXI_PINCTRL_PIN_PB4	PINCTRL_PIN(PB_BASE + 4, "PB4")
-#define SUNXI_PINCTRL_PIN_PB5	PINCTRL_PIN(PB_BASE + 5, "PB5")
-#define SUNXI_PINCTRL_PIN_PB6	PINCTRL_PIN(PB_BASE + 6, "PB6")
-#define SUNXI_PINCTRL_PIN_PB7	PINCTRL_PIN(PB_BASE + 7, "PB7")
-#define SUNXI_PINCTRL_PIN_PB8	PINCTRL_PIN(PB_BASE + 8, "PB8")
-#define SUNXI_PINCTRL_PIN_PB9	PINCTRL_PIN(PB_BASE + 9, "PB9")
-#define SUNXI_PINCTRL_PIN_PB10	PINCTRL_PIN(PB_BASE + 10, "PB10")
-#define SUNXI_PINCTRL_PIN_PB11	PINCTRL_PIN(PB_BASE + 11, "PB11")
-#define SUNXI_PINCTRL_PIN_PB12	PINCTRL_PIN(PB_BASE + 12, "PB12")
-#define SUNXI_PINCTRL_PIN_PB13	PINCTRL_PIN(PB_BASE + 13, "PB13")
-#define SUNXI_PINCTRL_PIN_PB14	PINCTRL_PIN(PB_BASE + 14, "PB14")
-#define SUNXI_PINCTRL_PIN_PB15	PINCTRL_PIN(PB_BASE + 15, "PB15")
-#define SUNXI_PINCTRL_PIN_PB16	PINCTRL_PIN(PB_BASE + 16, "PB16")
-#define SUNXI_PINCTRL_PIN_PB17	PINCTRL_PIN(PB_BASE + 17, "PB17")
-#define SUNXI_PINCTRL_PIN_PB18	PINCTRL_PIN(PB_BASE + 18, "PB18")
-#define SUNXI_PINCTRL_PIN_PB19	PINCTRL_PIN(PB_BASE + 19, "PB19")
-#define SUNXI_PINCTRL_PIN_PB20	PINCTRL_PIN(PB_BASE + 20, "PB20")
-#define SUNXI_PINCTRL_PIN_PB21	PINCTRL_PIN(PB_BASE + 21, "PB21")
-#define SUNXI_PINCTRL_PIN_PB22	PINCTRL_PIN(PB_BASE + 22, "PB22")
-#define SUNXI_PINCTRL_PIN_PB23	PINCTRL_PIN(PB_BASE + 23, "PB23")
-#define SUNXI_PINCTRL_PIN_PB24	PINCTRL_PIN(PB_BASE + 24, "PB24")
-#define SUNXI_PINCTRL_PIN_PB25	PINCTRL_PIN(PB_BASE + 25, "PB25")
-#define SUNXI_PINCTRL_PIN_PB26	PINCTRL_PIN(PB_BASE + 26, "PB26")
-#define SUNXI_PINCTRL_PIN_PB27	PINCTRL_PIN(PB_BASE + 27, "PB27")
-#define SUNXI_PINCTRL_PIN_PB28	PINCTRL_PIN(PB_BASE + 28, "PB28")
-#define SUNXI_PINCTRL_PIN_PB29	PINCTRL_PIN(PB_BASE + 29, "PB29")
-#define SUNXI_PINCTRL_PIN_PB30	PINCTRL_PIN(PB_BASE + 30, "PB30")
-#define SUNXI_PINCTRL_PIN_PB31	PINCTRL_PIN(PB_BASE + 31, "PB31")
-
-#define SUNXI_PINCTRL_PIN_PC0	PINCTRL_PIN(PC_BASE + 0, "PC0")
-#define SUNXI_PINCTRL_PIN_PC1	PINCTRL_PIN(PC_BASE + 1, "PC1")
-#define SUNXI_PINCTRL_PIN_PC2	PINCTRL_PIN(PC_BASE + 2, "PC2")
-#define SUNXI_PINCTRL_PIN_PC3	PINCTRL_PIN(PC_BASE + 3, "PC3")
-#define SUNXI_PINCTRL_PIN_PC4	PINCTRL_PIN(PC_BASE + 4, "PC4")
-#define SUNXI_PINCTRL_PIN_PC5	PINCTRL_PIN(PC_BASE + 5, "PC5")
-#define SUNXI_PINCTRL_PIN_PC6	PINCTRL_PIN(PC_BASE + 6, "PC6")
-#define SUNXI_PINCTRL_PIN_PC7	PINCTRL_PIN(PC_BASE + 7, "PC7")
-#define SUNXI_PINCTRL_PIN_PC8	PINCTRL_PIN(PC_BASE + 8, "PC8")
-#define SUNXI_PINCTRL_PIN_PC9	PINCTRL_PIN(PC_BASE + 9, "PC9")
-#define SUNXI_PINCTRL_PIN_PC10	PINCTRL_PIN(PC_BASE + 10, "PC10")
-#define SUNXI_PINCTRL_PIN_PC11	PINCTRL_PIN(PC_BASE + 11, "PC11")
-#define SUNXI_PINCTRL_PIN_PC12	PINCTRL_PIN(PC_BASE + 12, "PC12")
-#define SUNXI_PINCTRL_PIN_PC13	PINCTRL_PIN(PC_BASE + 13, "PC13")
-#define SUNXI_PINCTRL_PIN_PC14	PINCTRL_PIN(PC_BASE + 14, "PC14")
-#define SUNXI_PINCTRL_PIN_PC15	PINCTRL_PIN(PC_BASE + 15, "PC15")
-#define SUNXI_PINCTRL_PIN_PC16	PINCTRL_PIN(PC_BASE + 16, "PC16")
-#define SUNXI_PINCTRL_PIN_PC17	PINCTRL_PIN(PC_BASE + 17, "PC17")
-#define SUNXI_PINCTRL_PIN_PC18	PINCTRL_PIN(PC_BASE + 18, "PC18")
-#define SUNXI_PINCTRL_PIN_PC19	PINCTRL_PIN(PC_BASE + 19, "PC19")
-#define SUNXI_PINCTRL_PIN_PC20	PINCTRL_PIN(PC_BASE + 20, "PC20")
-#define SUNXI_PINCTRL_PIN_PC21	PINCTRL_PIN(PC_BASE + 21, "PC21")
-#define SUNXI_PINCTRL_PIN_PC22	PINCTRL_PIN(PC_BASE + 22, "PC22")
-#define SUNXI_PINCTRL_PIN_PC23	PINCTRL_PIN(PC_BASE + 23, "PC23")
-#define SUNXI_PINCTRL_PIN_PC24	PINCTRL_PIN(PC_BASE + 24, "PC24")
-#define SUNXI_PINCTRL_PIN_PC25	PINCTRL_PIN(PC_BASE + 25, "PC25")
-#define SUNXI_PINCTRL_PIN_PC26	PINCTRL_PIN(PC_BASE + 26, "PC26")
-#define SUNXI_PINCTRL_PIN_PC27	PINCTRL_PIN(PC_BASE + 27, "PC27")
-#define SUNXI_PINCTRL_PIN_PC28	PINCTRL_PIN(PC_BASE + 28, "PC28")
-#define SUNXI_PINCTRL_PIN_PC29	PINCTRL_PIN(PC_BASE + 29, "PC29")
-#define SUNXI_PINCTRL_PIN_PC30	PINCTRL_PIN(PC_BASE + 30, "PC30")
-#define SUNXI_PINCTRL_PIN_PC31	PINCTRL_PIN(PC_BASE + 31, "PC31")
-
-#define SUNXI_PINCTRL_PIN_PD0	PINCTRL_PIN(PD_BASE + 0, "PD0")
-#define SUNXI_PINCTRL_PIN_PD1	PINCTRL_PIN(PD_BASE + 1, "PD1")
-#define SUNXI_PINCTRL_PIN_PD2	PINCTRL_PIN(PD_BASE + 2, "PD2")
-#define SUNXI_PINCTRL_PIN_PD3	PINCTRL_PIN(PD_BASE + 3, "PD3")
-#define SUNXI_PINCTRL_PIN_PD4	PINCTRL_PIN(PD_BASE + 4, "PD4")
-#define SUNXI_PINCTRL_PIN_PD5	PINCTRL_PIN(PD_BASE + 5, "PD5")
-#define SUNXI_PINCTRL_PIN_PD6	PINCTRL_PIN(PD_BASE + 6, "PD6")
-#define SUNXI_PINCTRL_PIN_PD7	PINCTRL_PIN(PD_BASE + 7, "PD7")
-#define SUNXI_PINCTRL_PIN_PD8	PINCTRL_PIN(PD_BASE + 8, "PD8")
-#define SUNXI_PINCTRL_PIN_PD9	PINCTRL_PIN(PD_BASE + 9, "PD9")
-#define SUNXI_PINCTRL_PIN_PD10	PINCTRL_PIN(PD_BASE + 10, "PD10")
-#define SUNXI_PINCTRL_PIN_PD11	PINCTRL_PIN(PD_BASE + 11, "PD11")
-#define SUNXI_PINCTRL_PIN_PD12	PINCTRL_PIN(PD_BASE + 12, "PD12")
-#define SUNXI_PINCTRL_PIN_PD13	PINCTRL_PIN(PD_BASE + 13, "PD13")
-#define SUNXI_PINCTRL_PIN_PD14	PINCTRL_PIN(PD_BASE + 14, "PD14")
-#define SUNXI_PINCTRL_PIN_PD15	PINCTRL_PIN(PD_BASE + 15, "PD15")
-#define SUNXI_PINCTRL_PIN_PD16	PINCTRL_PIN(PD_BASE + 16, "PD16")
-#define SUNXI_PINCTRL_PIN_PD17	PINCTRL_PIN(PD_BASE + 17, "PD17")
-#define SUNXI_PINCTRL_PIN_PD18	PINCTRL_PIN(PD_BASE + 18, "PD18")
-#define SUNXI_PINCTRL_PIN_PD19	PINCTRL_PIN(PD_BASE + 19, "PD19")
-#define SUNXI_PINCTRL_PIN_PD20	PINCTRL_PIN(PD_BASE + 20, "PD20")
-#define SUNXI_PINCTRL_PIN_PD21	PINCTRL_PIN(PD_BASE + 21, "PD21")
-#define SUNXI_PINCTRL_PIN_PD22	PINCTRL_PIN(PD_BASE + 22, "PD22")
-#define SUNXI_PINCTRL_PIN_PD23	PINCTRL_PIN(PD_BASE + 23, "PD23")
-#define SUNXI_PINCTRL_PIN_PD24	PINCTRL_PIN(PD_BASE + 24, "PD24")
-#define SUNXI_PINCTRL_PIN_PD25	PINCTRL_PIN(PD_BASE + 25, "PD25")
-#define SUNXI_PINCTRL_PIN_PD26	PINCTRL_PIN(PD_BASE + 26, "PD26")
-#define SUNXI_PINCTRL_PIN_PD27	PINCTRL_PIN(PD_BASE + 27, "PD27")
-#define SUNXI_PINCTRL_PIN_PD28	PINCTRL_PIN(PD_BASE + 28, "PD28")
-#define SUNXI_PINCTRL_PIN_PD29	PINCTRL_PIN(PD_BASE + 29, "PD29")
-#define SUNXI_PINCTRL_PIN_PD30	PINCTRL_PIN(PD_BASE + 30, "PD30")
-#define SUNXI_PINCTRL_PIN_PD31	PINCTRL_PIN(PD_BASE + 31, "PD31")
-
-#define SUNXI_PINCTRL_PIN_PE0	PINCTRL_PIN(PE_BASE + 0, "PE0")
-#define SUNXI_PINCTRL_PIN_PE1	PINCTRL_PIN(PE_BASE + 1, "PE1")
-#define SUNXI_PINCTRL_PIN_PE2	PINCTRL_PIN(PE_BASE + 2, "PE2")
-#define SUNXI_PINCTRL_PIN_PE3	PINCTRL_PIN(PE_BASE + 3, "PE3")
-#define SUNXI_PINCTRL_PIN_PE4	PINCTRL_PIN(PE_BASE + 4, "PE4")
-#define SUNXI_PINCTRL_PIN_PE5	PINCTRL_PIN(PE_BASE + 5, "PE5")
-#define SUNXI_PINCTRL_PIN_PE6	PINCTRL_PIN(PE_BASE + 6, "PE6")
-#define SUNXI_PINCTRL_PIN_PE7	PINCTRL_PIN(PE_BASE + 7, "PE7")
-#define SUNXI_PINCTRL_PIN_PE8	PINCTRL_PIN(PE_BASE + 8, "PE8")
-#define SUNXI_PINCTRL_PIN_PE9	PINCTRL_PIN(PE_BASE + 9, "PE9")
-#define SUNXI_PINCTRL_PIN_PE10	PINCTRL_PIN(PE_BASE + 10, "PE10")
-#define SUNXI_PINCTRL_PIN_PE11	PINCTRL_PIN(PE_BASE + 11, "PE11")
-#define SUNXI_PINCTRL_PIN_PE12	PINCTRL_PIN(PE_BASE + 12, "PE12")
-#define SUNXI_PINCTRL_PIN_PE13	PINCTRL_PIN(PE_BASE + 13, "PE13")
-#define SUNXI_PINCTRL_PIN_PE14	PINCTRL_PIN(PE_BASE + 14, "PE14")
-#define SUNXI_PINCTRL_PIN_PE15	PINCTRL_PIN(PE_BASE + 15, "PE15")
-#define SUNXI_PINCTRL_PIN_PE16	PINCTRL_PIN(PE_BASE + 16, "PE16")
-#define SUNXI_PINCTRL_PIN_PE17	PINCTRL_PIN(PE_BASE + 17, "PE17")
-#define SUNXI_PINCTRL_PIN_PE18	PINCTRL_PIN(PE_BASE + 18, "PE18")
-#define SUNXI_PINCTRL_PIN_PE19	PINCTRL_PIN(PE_BASE + 19, "PE19")
-#define SUNXI_PINCTRL_PIN_PE20	PINCTRL_PIN(PE_BASE + 20, "PE20")
-#define SUNXI_PINCTRL_PIN_PE21	PINCTRL_PIN(PE_BASE + 21, "PE21")
-#define SUNXI_PINCTRL_PIN_PE22	PINCTRL_PIN(PE_BASE + 22, "PE22")
-#define SUNXI_PINCTRL_PIN_PE23	PINCTRL_PIN(PE_BASE + 23, "PE23")
-#define SUNXI_PINCTRL_PIN_PE24	PINCTRL_PIN(PE_BASE + 24, "PE24")
-#define SUNXI_PINCTRL_PIN_PE25	PINCTRL_PIN(PE_BASE + 25, "PE25")
-#define SUNXI_PINCTRL_PIN_PE26	PINCTRL_PIN(PE_BASE + 26, "PE26")
-#define SUNXI_PINCTRL_PIN_PE27	PINCTRL_PIN(PE_BASE + 27, "PE27")
-#define SUNXI_PINCTRL_PIN_PE28	PINCTRL_PIN(PE_BASE + 28, "PE28")
-#define SUNXI_PINCTRL_PIN_PE29	PINCTRL_PIN(PE_BASE + 29, "PE29")
-#define SUNXI_PINCTRL_PIN_PE30	PINCTRL_PIN(PE_BASE + 30, "PE30")
-#define SUNXI_PINCTRL_PIN_PE31	PINCTRL_PIN(PE_BASE + 31, "PE31")
-
-#define SUNXI_PINCTRL_PIN_PF0	PINCTRL_PIN(PF_BASE + 0, "PF0")
-#define SUNXI_PINCTRL_PIN_PF1	PINCTRL_PIN(PF_BASE + 1, "PF1")
-#define SUNXI_PINCTRL_PIN_PF2	PINCTRL_PIN(PF_BASE + 2, "PF2")
-#define SUNXI_PINCTRL_PIN_PF3	PINCTRL_PIN(PF_BASE + 3, "PF3")
-#define SUNXI_PINCTRL_PIN_PF4	PINCTRL_PIN(PF_BASE + 4, "PF4")
-#define SUNXI_PINCTRL_PIN_PF5	PINCTRL_PIN(PF_BASE + 5, "PF5")
-#define SUNXI_PINCTRL_PIN_PF6	PINCTRL_PIN(PF_BASE + 6, "PF6")
-#define SUNXI_PINCTRL_PIN_PF7	PINCTRL_PIN(PF_BASE + 7, "PF7")
-#define SUNXI_PINCTRL_PIN_PF8	PINCTRL_PIN(PF_BASE + 8, "PF8")
-#define SUNXI_PINCTRL_PIN_PF9	PINCTRL_PIN(PF_BASE + 9, "PF9")
-#define SUNXI_PINCTRL_PIN_PF10	PINCTRL_PIN(PF_BASE + 10, "PF10")
-#define SUNXI_PINCTRL_PIN_PF11	PINCTRL_PIN(PF_BASE + 11, "PF11")
-#define SUNXI_PINCTRL_PIN_PF12	PINCTRL_PIN(PF_BASE + 12, "PF12")
-#define SUNXI_PINCTRL_PIN_PF13	PINCTRL_PIN(PF_BASE + 13, "PF13")
-#define SUNXI_PINCTRL_PIN_PF14	PINCTRL_PIN(PF_BASE + 14, "PF14")
-#define SUNXI_PINCTRL_PIN_PF15	PINCTRL_PIN(PF_BASE + 15, "PF15")
-#define SUNXI_PINCTRL_PIN_PF16	PINCTRL_PIN(PF_BASE + 16, "PF16")
-#define SUNXI_PINCTRL_PIN_PF17	PINCTRL_PIN(PF_BASE + 17, "PF17")
-#define SUNXI_PINCTRL_PIN_PF18	PINCTRL_PIN(PF_BASE + 18, "PF18")
-#define SUNXI_PINCTRL_PIN_PF19	PINCTRL_PIN(PF_BASE + 19, "PF19")
-#define SUNXI_PINCTRL_PIN_PF20	PINCTRL_PIN(PF_BASE + 20, "PF20")
-#define SUNXI_PINCTRL_PIN_PF21	PINCTRL_PIN(PF_BASE + 21, "PF21")
-#define SUNXI_PINCTRL_PIN_PF22	PINCTRL_PIN(PF_BASE + 22, "PF22")
-#define SUNXI_PINCTRL_PIN_PF23	PINCTRL_PIN(PF_BASE + 23, "PF23")
-#define SUNXI_PINCTRL_PIN_PF24	PINCTRL_PIN(PF_BASE + 24, "PF24")
-#define SUNXI_PINCTRL_PIN_PF25	PINCTRL_PIN(PF_BASE + 25, "PF25")
-#define SUNXI_PINCTRL_PIN_PF26	PINCTRL_PIN(PF_BASE + 26, "PF26")
-#define SUNXI_PINCTRL_PIN_PF27	PINCTRL_PIN(PF_BASE + 27, "PF27")
-#define SUNXI_PINCTRL_PIN_PF28	PINCTRL_PIN(PF_BASE + 28, "PF28")
-#define SUNXI_PINCTRL_PIN_PF29	PINCTRL_PIN(PF_BASE + 29, "PF29")
-#define SUNXI_PINCTRL_PIN_PF30	PINCTRL_PIN(PF_BASE + 30, "PF30")
-#define SUNXI_PINCTRL_PIN_PF31	PINCTRL_PIN(PF_BASE + 31, "PF31")
-
-#define SUNXI_PINCTRL_PIN_PG0	PINCTRL_PIN(PG_BASE + 0, "PG0")
-#define SUNXI_PINCTRL_PIN_PG1	PINCTRL_PIN(PG_BASE + 1, "PG1")
-#define SUNXI_PINCTRL_PIN_PG2	PINCTRL_PIN(PG_BASE + 2, "PG2")
-#define SUNXI_PINCTRL_PIN_PG3	PINCTRL_PIN(PG_BASE + 3, "PG3")
-#define SUNXI_PINCTRL_PIN_PG4	PINCTRL_PIN(PG_BASE + 4, "PG4")
-#define SUNXI_PINCTRL_PIN_PG5	PINCTRL_PIN(PG_BASE + 5, "PG5")
-#define SUNXI_PINCTRL_PIN_PG6	PINCTRL_PIN(PG_BASE + 6, "PG6")
-#define SUNXI_PINCTRL_PIN_PG7	PINCTRL_PIN(PG_BASE + 7, "PG7")
-#define SUNXI_PINCTRL_PIN_PG8	PINCTRL_PIN(PG_BASE + 8, "PG8")
-#define SUNXI_PINCTRL_PIN_PG9	PINCTRL_PIN(PG_BASE + 9, "PG9")
-#define SUNXI_PINCTRL_PIN_PG10	PINCTRL_PIN(PG_BASE + 10, "PG10")
-#define SUNXI_PINCTRL_PIN_PG11	PINCTRL_PIN(PG_BASE + 11, "PG11")
-#define SUNXI_PINCTRL_PIN_PG12	PINCTRL_PIN(PG_BASE + 12, "PG12")
-#define SUNXI_PINCTRL_PIN_PG13	PINCTRL_PIN(PG_BASE + 13, "PG13")
-#define SUNXI_PINCTRL_PIN_PG14	PINCTRL_PIN(PG_BASE + 14, "PG14")
-#define SUNXI_PINCTRL_PIN_PG15	PINCTRL_PIN(PG_BASE + 15, "PG15")
-#define SUNXI_PINCTRL_PIN_PG16	PINCTRL_PIN(PG_BASE + 16, "PG16")
-#define SUNXI_PINCTRL_PIN_PG17	PINCTRL_PIN(PG_BASE + 17, "PG17")
-#define SUNXI_PINCTRL_PIN_PG18	PINCTRL_PIN(PG_BASE + 18, "PG18")
-#define SUNXI_PINCTRL_PIN_PG19	PINCTRL_PIN(PG_BASE + 19, "PG19")
-#define SUNXI_PINCTRL_PIN_PG20	PINCTRL_PIN(PG_BASE + 20, "PG20")
-#define SUNXI_PINCTRL_PIN_PG21	PINCTRL_PIN(PG_BASE + 21, "PG21")
-#define SUNXI_PINCTRL_PIN_PG22	PINCTRL_PIN(PG_BASE + 22, "PG22")
-#define SUNXI_PINCTRL_PIN_PG23	PINCTRL_PIN(PG_BASE + 23, "PG23")
-#define SUNXI_PINCTRL_PIN_PG24	PINCTRL_PIN(PG_BASE + 24, "PG24")
-#define SUNXI_PINCTRL_PIN_PG25	PINCTRL_PIN(PG_BASE + 25, "PG25")
-#define SUNXI_PINCTRL_PIN_PG26	PINCTRL_PIN(PG_BASE + 26, "PG26")
-#define SUNXI_PINCTRL_PIN_PG27	PINCTRL_PIN(PG_BASE + 27, "PG27")
-#define SUNXI_PINCTRL_PIN_PG28	PINCTRL_PIN(PG_BASE + 28, "PG28")
-#define SUNXI_PINCTRL_PIN_PG29	PINCTRL_PIN(PG_BASE + 29, "PG29")
-#define SUNXI_PINCTRL_PIN_PG30	PINCTRL_PIN(PG_BASE + 30, "PG30")
-#define SUNXI_PINCTRL_PIN_PG31	PINCTRL_PIN(PG_BASE + 31, "PG31")
-
-#define SUNXI_PINCTRL_PIN_PH0	PINCTRL_PIN(PH_BASE + 0, "PH0")
-#define SUNXI_PINCTRL_PIN_PH1	PINCTRL_PIN(PH_BASE + 1, "PH1")
-#define SUNXI_PINCTRL_PIN_PH2	PINCTRL_PIN(PH_BASE + 2, "PH2")
-#define SUNXI_PINCTRL_PIN_PH3	PINCTRL_PIN(PH_BASE + 3, "PH3")
-#define SUNXI_PINCTRL_PIN_PH4	PINCTRL_PIN(PH_BASE + 4, "PH4")
-#define SUNXI_PINCTRL_PIN_PH5	PINCTRL_PIN(PH_BASE + 5, "PH5")
-#define SUNXI_PINCTRL_PIN_PH6	PINCTRL_PIN(PH_BASE + 6, "PH6")
-#define SUNXI_PINCTRL_PIN_PH7	PINCTRL_PIN(PH_BASE + 7, "PH7")
-#define SUNXI_PINCTRL_PIN_PH8	PINCTRL_PIN(PH_BASE + 8, "PH8")
-#define SUNXI_PINCTRL_PIN_PH9	PINCTRL_PIN(PH_BASE + 9, "PH9")
-#define SUNXI_PINCTRL_PIN_PH10	PINCTRL_PIN(PH_BASE + 10, "PH10")
-#define SUNXI_PINCTRL_PIN_PH11	PINCTRL_PIN(PH_BASE + 11, "PH11")
-#define SUNXI_PINCTRL_PIN_PH12	PINCTRL_PIN(PH_BASE + 12, "PH12")
-#define SUNXI_PINCTRL_PIN_PH13	PINCTRL_PIN(PH_BASE + 13, "PH13")
-#define SUNXI_PINCTRL_PIN_PH14	PINCTRL_PIN(PH_BASE + 14, "PH14")
-#define SUNXI_PINCTRL_PIN_PH15	PINCTRL_PIN(PH_BASE + 15, "PH15")
-#define SUNXI_PINCTRL_PIN_PH16	PINCTRL_PIN(PH_BASE + 16, "PH16")
-#define SUNXI_PINCTRL_PIN_PH17	PINCTRL_PIN(PH_BASE + 17, "PH17")
-#define SUNXI_PINCTRL_PIN_PH18	PINCTRL_PIN(PH_BASE + 18, "PH18")
-#define SUNXI_PINCTRL_PIN_PH19	PINCTRL_PIN(PH_BASE + 19, "PH19")
-#define SUNXI_PINCTRL_PIN_PH20	PINCTRL_PIN(PH_BASE + 20, "PH20")
-#define SUNXI_PINCTRL_PIN_PH21	PINCTRL_PIN(PH_BASE + 21, "PH21")
-#define SUNXI_PINCTRL_PIN_PH22	PINCTRL_PIN(PH_BASE + 22, "PH22")
-#define SUNXI_PINCTRL_PIN_PH23	PINCTRL_PIN(PH_BASE + 23, "PH23")
-#define SUNXI_PINCTRL_PIN_PH24	PINCTRL_PIN(PH_BASE + 24, "PH24")
-#define SUNXI_PINCTRL_PIN_PH25	PINCTRL_PIN(PH_BASE + 25, "PH25")
-#define SUNXI_PINCTRL_PIN_PH26	PINCTRL_PIN(PH_BASE + 26, "PH26")
-#define SUNXI_PINCTRL_PIN_PH27	PINCTRL_PIN(PH_BASE + 27, "PH27")
-#define SUNXI_PINCTRL_PIN_PH28	PINCTRL_PIN(PH_BASE + 28, "PH28")
-#define SUNXI_PINCTRL_PIN_PH29	PINCTRL_PIN(PH_BASE + 29, "PH29")
-#define SUNXI_PINCTRL_PIN_PH30	PINCTRL_PIN(PH_BASE + 30, "PH30")
-#define SUNXI_PINCTRL_PIN_PH31	PINCTRL_PIN(PH_BASE + 31, "PH31")
-
-#define SUNXI_PINCTRL_PIN_PI0	PINCTRL_PIN(PI_BASE + 0, "PI0")
-#define SUNXI_PINCTRL_PIN_PI1	PINCTRL_PIN(PI_BASE + 1, "PI1")
-#define SUNXI_PINCTRL_PIN_PI2	PINCTRL_PIN(PI_BASE + 2, "PI2")
-#define SUNXI_PINCTRL_PIN_PI3	PINCTRL_PIN(PI_BASE + 3, "PI3")
-#define SUNXI_PINCTRL_PIN_PI4	PINCTRL_PIN(PI_BASE + 4, "PI4")
-#define SUNXI_PINCTRL_PIN_PI5	PINCTRL_PIN(PI_BASE + 5, "PI5")
-#define SUNXI_PINCTRL_PIN_PI6	PINCTRL_PIN(PI_BASE + 6, "PI6")
-#define SUNXI_PINCTRL_PIN_PI7	PINCTRL_PIN(PI_BASE + 7, "PI7")
-#define SUNXI_PINCTRL_PIN_PI8	PINCTRL_PIN(PI_BASE + 8, "PI8")
-#define SUNXI_PINCTRL_PIN_PI9	PINCTRL_PIN(PI_BASE + 9, "PI9")
-#define SUNXI_PINCTRL_PIN_PI10	PINCTRL_PIN(PI_BASE + 10, "PI10")
-#define SUNXI_PINCTRL_PIN_PI11	PINCTRL_PIN(PI_BASE + 11, "PI11")
-#define SUNXI_PINCTRL_PIN_PI12	PINCTRL_PIN(PI_BASE + 12, "PI12")
-#define SUNXI_PINCTRL_PIN_PI13	PINCTRL_PIN(PI_BASE + 13, "PI13")
-#define SUNXI_PINCTRL_PIN_PI14	PINCTRL_PIN(PI_BASE + 14, "PI14")
-#define SUNXI_PINCTRL_PIN_PI15	PINCTRL_PIN(PI_BASE + 15, "PI15")
-#define SUNXI_PINCTRL_PIN_PI16	PINCTRL_PIN(PI_BASE + 16, "PI16")
-#define SUNXI_PINCTRL_PIN_PI17	PINCTRL_PIN(PI_BASE + 17, "PI17")
-#define SUNXI_PINCTRL_PIN_PI18	PINCTRL_PIN(PI_BASE + 18, "PI18")
-#define SUNXI_PINCTRL_PIN_PI19	PINCTRL_PIN(PI_BASE + 19, "PI19")
-#define SUNXI_PINCTRL_PIN_PI20	PINCTRL_PIN(PI_BASE + 20, "PI20")
-#define SUNXI_PINCTRL_PIN_PI21	PINCTRL_PIN(PI_BASE + 21, "PI21")
-#define SUNXI_PINCTRL_PIN_PI22	PINCTRL_PIN(PI_BASE + 22, "PI22")
-#define SUNXI_PINCTRL_PIN_PI23	PINCTRL_PIN(PI_BASE + 23, "PI23")
-#define SUNXI_PINCTRL_PIN_PI24	PINCTRL_PIN(PI_BASE + 24, "PI24")
-#define SUNXI_PINCTRL_PIN_PI25	PINCTRL_PIN(PI_BASE + 25, "PI25")
-#define SUNXI_PINCTRL_PIN_PI26	PINCTRL_PIN(PI_BASE + 26, "PI26")
-#define SUNXI_PINCTRL_PIN_PI27	PINCTRL_PIN(PI_BASE + 27, "PI27")
-#define SUNXI_PINCTRL_PIN_PI28	PINCTRL_PIN(PI_BASE + 28, "PI28")
-#define SUNXI_PINCTRL_PIN_PI29	PINCTRL_PIN(PI_BASE + 29, "PI29")
-#define SUNXI_PINCTRL_PIN_PI30	PINCTRL_PIN(PI_BASE + 30, "PI30")
-#define SUNXI_PINCTRL_PIN_PI31	PINCTRL_PIN(PI_BASE + 31, "PI31")
-
-#define SUNXI_PIN_NAME_MAX_LEN	5
-
-#define BANK_MEM_SIZE		0x24
-#define MUX_REGS_OFFSET		0x0
-#define DATA_REGS_OFFSET	0x10
-#define DLEVEL_REGS_OFFSET	0x14
-#define PULL_REGS_OFFSET	0x1c
-
-#define PINS_PER_BANK		32
-#define MUX_PINS_PER_REG	8
-#define MUX_PINS_BITS		4
-#define MUX_PINS_MASK		0x0f
-#define DATA_PINS_PER_REG	32
-#define DATA_PINS_BITS		1
-#define DATA_PINS_MASK		0x01
-#define DLEVEL_PINS_PER_REG	16
-#define DLEVEL_PINS_BITS	2
-#define DLEVEL_PINS_MASK	0x03
-#define PULL_PINS_PER_REG	16
-#define PULL_PINS_BITS		2
-#define PULL_PINS_MASK		0x03
-
-#define SUNXI_IRQ_NUMBER	32
-
-#define IRQ_CFG_REG		0x200
-#define IRQ_CFG_IRQ_PER_REG		8
-#define IRQ_CFG_IRQ_BITS		4
-#define IRQ_CFG_IRQ_MASK		((1 << IRQ_CFG_IRQ_BITS) - 1)
-#define IRQ_CTRL_REG		0x210
-#define IRQ_CTRL_IRQ_PER_REG		32
-#define IRQ_CTRL_IRQ_BITS		1
-#define IRQ_CTRL_IRQ_MASK		((1 << IRQ_CTRL_IRQ_BITS) - 1)
-#define IRQ_STATUS_REG		0x214
-#define IRQ_STATUS_IRQ_PER_REG		32
-#define IRQ_STATUS_IRQ_BITS		1
-#define IRQ_STATUS_IRQ_MASK		((1 << IRQ_STATUS_IRQ_BITS) - 1)
-
-#define IRQ_EDGE_RISING		0x00
-#define IRQ_EDGE_FALLING	0x01
-#define IRQ_LEVEL_HIGH		0x02
-#define IRQ_LEVEL_LOW		0x03
-#define IRQ_EDGE_BOTH		0x04
-
-struct sunxi_desc_function {
-	const char	*name;
-	u8		muxval;
-	u8		irqnum;
-};
-
-struct sunxi_desc_pin {
-	struct pinctrl_pin_desc		pin;
-	struct sunxi_desc_function	*functions;
-};
-
-struct sunxi_pinctrl_desc {
-	const struct sunxi_desc_pin	*pins;
-	int				npins;
-	struct pinctrl_gpio_range	*ranges;
-	int				nranges;
-};
-
-struct sunxi_pinctrl_function {
-	const char	*name;
-	const char	**groups;
-	unsigned	ngroups;
-};
-
-struct sunxi_pinctrl_group {
-	const char	*name;
-	unsigned long	config;
-	unsigned	pin;
-};
-
-struct sunxi_pinctrl {
-	void __iomem			*membase;
-	struct gpio_chip		*chip;
-	struct sunxi_pinctrl_desc	*desc;
-	struct device			*dev;
-	struct irq_domain		*domain;
-	struct sunxi_pinctrl_function	*functions;
-	unsigned			nfunctions;
-	struct sunxi_pinctrl_group	*groups;
-	unsigned			ngroups;
-	int				irq;
-	int				irq_array[SUNXI_IRQ_NUMBER];
-	spinlock_t			lock;
-	struct pinctrl_dev		*pctl_dev;
-};
-
-#define SUNXI_PIN(_pin, ...)					\
-	{							\
-		.pin = _pin,					\
-		.functions = (struct sunxi_desc_function[]){	\
-			__VA_ARGS__, { } },			\
-	}
-
-#define SUNXI_FUNCTION(_val, _name)				\
-	{							\
-		.name = _name,					\
-		.muxval = _val,					\
-	}
-
-#define SUNXI_FUNCTION_IRQ(_val, _irq)				\
-	{							\
-		.name = "irq",					\
-		.muxval = _val,					\
-		.irqnum = _irq,					\
-	}
-
-/*
- * The sunXi PIO registers are organized as is:
- * 0x00 - 0x0c	Muxing values.
- *		8 pins per register, each pin having a 4bits value
- * 0x10		Pin values
- *		32 bits per register, each pin corresponding to one bit
- * 0x14 - 0x18	Drive level
- *		16 pins per register, each pin having a 2bits value
- * 0x1c - 0x20	Pull-Up values
- *		16 pins per register, each pin having a 2bits value
- *
- * This is for the first bank. Each bank will have the same layout,
- * with an offset being a multiple of 0x24.
- *
- * The following functions calculate from the pin number the register
- * and the bit offset that we should access.
- */
-static inline u32 sunxi_mux_reg(u16 pin)
-{
-	u8 bank = pin / PINS_PER_BANK;
-	u32 offset = bank * BANK_MEM_SIZE;
-	offset += MUX_REGS_OFFSET;
-	offset += pin % PINS_PER_BANK / MUX_PINS_PER_REG * 0x04;
-	return round_down(offset, 4);
-}
-
-static inline u32 sunxi_mux_offset(u16 pin)
-{
-	u32 pin_num = pin % MUX_PINS_PER_REG;
-	return pin_num * MUX_PINS_BITS;
-}
-
-static inline u32 sunxi_data_reg(u16 pin)
-{
-	u8 bank = pin / PINS_PER_BANK;
-	u32 offset = bank * BANK_MEM_SIZE;
-	offset += DATA_REGS_OFFSET;
-	offset += pin % PINS_PER_BANK / DATA_PINS_PER_REG * 0x04;
-	return round_down(offset, 4);
-}
-
-static inline u32 sunxi_data_offset(u16 pin)
-{
-	u32 pin_num = pin % DATA_PINS_PER_REG;
-	return pin_num * DATA_PINS_BITS;
-}
-
-static inline u32 sunxi_dlevel_reg(u16 pin)
-{
-	u8 bank = pin / PINS_PER_BANK;
-	u32 offset = bank * BANK_MEM_SIZE;
-	offset += DLEVEL_REGS_OFFSET;
-	offset += pin % PINS_PER_BANK / DLEVEL_PINS_PER_REG * 0x04;
-	return round_down(offset, 4);
-}
-
-static inline u32 sunxi_dlevel_offset(u16 pin)
-{
-	u32 pin_num = pin % DLEVEL_PINS_PER_REG;
-	return pin_num * DLEVEL_PINS_BITS;
-}
-
-static inline u32 sunxi_pull_reg(u16 pin)
-{
-	u8 bank = pin / PINS_PER_BANK;
-	u32 offset = bank * BANK_MEM_SIZE;
-	offset += PULL_REGS_OFFSET;
-	offset += pin % PINS_PER_BANK / PULL_PINS_PER_REG * 0x04;
-	return round_down(offset, 4);
-}
-
-static inline u32 sunxi_pull_offset(u16 pin)
-{
-	u32 pin_num = pin % PULL_PINS_PER_REG;
-	return pin_num * PULL_PINS_BITS;
-}
-
-static inline u32 sunxi_irq_cfg_reg(u16 irq)
-{
-	u8 reg = irq / IRQ_CFG_IRQ_PER_REG * 0x04;
-	return reg + IRQ_CFG_REG;
-}
-
-static inline u32 sunxi_irq_cfg_offset(u16 irq)
-{
-	u32 irq_num = irq % IRQ_CFG_IRQ_PER_REG;
-	return irq_num * IRQ_CFG_IRQ_BITS;
-}
-
-static inline u32 sunxi_irq_ctrl_reg(u16 irq)
-{
-	u8 reg = irq / IRQ_CTRL_IRQ_PER_REG * 0x04;
-	return reg + IRQ_CTRL_REG;
-}
-
-static inline u32 sunxi_irq_ctrl_offset(u16 irq)
-{
-	u32 irq_num = irq % IRQ_CTRL_IRQ_PER_REG;
-	return irq_num * IRQ_CTRL_IRQ_BITS;
-}
-
-static inline u32 sunxi_irq_status_reg(u16 irq)
-{
-	u8 reg = irq / IRQ_STATUS_IRQ_PER_REG * 0x04;
-	return reg + IRQ_STATUS_REG;
-}
-
-static inline u32 sunxi_irq_status_offset(u16 irq)
-{
-	u32 irq_num = irq % IRQ_STATUS_IRQ_PER_REG;
-	return irq_num * IRQ_STATUS_IRQ_BITS;
-}
-
-#endif /* __PINCTRL_SUNXI_H */
diff --git a/drivers/pinctrl/pinctrl-tb10x.c b/drivers/pinctrl/pinctrl-tb10x.c
index c5e0f6973a3..26ca6855f47 100644
--- a/drivers/pinctrl/pinctrl-tb10x.c
+++ b/drivers/pinctrl/pinctrl-tb10x.c
@@ -629,9 +629,8 @@ static int tb10x_gpio_request_enable(struct pinctrl_dev *pctl,
 	 */
 	for (i = 0; i < state->pinfuncgrpcnt; i++) {
 		const struct tb10x_pinfuncgrp *pfg = &state->pingroups[i];
-		unsigned int port = pfg->port;
 		unsigned int mode = pfg->mode;
-		int j;
+		int j, port = pfg->port;
 
 		/*
 		 * Skip pin groups which are always mapped and don't need
diff --git a/drivers/pinctrl/pinctrl-tegra.c b/drivers/pinctrl/pinctrl-tegra.c
index 65458096f41..2d43bff74f5 100644
--- a/drivers/pinctrl/pinctrl-tegra.c
+++ b/drivers/pinctrl/pinctrl-tegra.c
@@ -295,17 +295,11 @@ static void tegra_pinctrl_disable(struct pinctrl_dev *pctldev,
 {
 	struct tegra_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
 	const struct tegra_pingroup *g;
-	u32 val;
 
 	g = &pmx->soc->groups[group];
 
 	if (WARN_ON(g->mux_reg < 0))
 		return;
-
-	val = pmx_readl(pmx, g->mux_bank, g->mux_reg);
-	val &= ~(0x3 << g->mux_bit);
-	val |= g->func_safe << g->mux_bit;
-	pmx_writel(pmx, val, g->mux_bank, g->mux_reg);
 }
 
 static const struct pinmux_ops tegra_pinmux_ops = {
@@ -336,32 +330,32 @@ static int tegra_pinconf_reg(struct tegra_pmx *pmx,
 		*width = 1;
 		break;
 	case TEGRA_PINCONF_PARAM_ENABLE_INPUT:
-		*bank = g->einput_bank;
-		*reg = g->einput_reg;
+		*bank = g->mux_bank;
+		*reg = g->mux_reg;
 		*bit = g->einput_bit;
 		*width = 1;
 		break;
 	case TEGRA_PINCONF_PARAM_OPEN_DRAIN:
-		*bank = g->odrain_bank;
-		*reg = g->odrain_reg;
+		*bank = g->mux_bank;
+		*reg = g->mux_reg;
 		*bit = g->odrain_bit;
 		*width = 1;
 		break;
 	case TEGRA_PINCONF_PARAM_LOCK:
-		*bank = g->lock_bank;
-		*reg = g->lock_reg;
+		*bank = g->mux_bank;
+		*reg = g->mux_reg;
 		*bit = g->lock_bit;
 		*width = 1;
 		break;
 	case TEGRA_PINCONF_PARAM_IORESET:
-		*bank = g->ioreset_bank;
-		*reg = g->ioreset_reg;
+		*bank = g->mux_bank;
+		*reg = g->mux_reg;
 		*bit = g->ioreset_bit;
 		*width = 1;
 		break;
 	case TEGRA_PINCONF_PARAM_RCV_SEL:
-		*bank = g->rcv_sel_bank;
-		*reg = g->rcv_sel_reg;
+		*bank = g->mux_bank;
+		*reg = g->mux_reg;
 		*bit = g->rcv_sel_bit;
 		*width = 1;
 		break;
@@ -408,8 +402,8 @@ static int tegra_pinconf_reg(struct tegra_pmx *pmx,
 		*width = g->slwr_width;
 		break;
 	case TEGRA_PINCONF_PARAM_DRIVE_TYPE:
-		*bank = g->drvtype_bank;
-		*reg = g->drvtype_reg;
+		*bank = g->drv_bank;
+		*reg = g->drv_reg;
 		*bit = g->drvtype_bit;
 		*width = 2;
 		break;
@@ -418,11 +412,22 @@ static int tegra_pinconf_reg(struct tegra_pmx *pmx,
 		return -ENOTSUPP;
 	}
 
-	if (*reg < 0) {
-		if (report_err)
+	if (*reg < 0 || *bit > 31) {
+		if (report_err) {
+			const char *prop = "unknown";
+			int i;
+
+			for (i = 0; i < ARRAY_SIZE(cfg_params); i++) {
+				if (cfg_params[i].param == param) {
+					prop = cfg_params[i].property;
+					break;
+				}
+			}
+
 			dev_err(pmx->dev,
-				"Config param %04x not supported on group %s\n",
-				param, g->name);
+				"Config param %04x (%s) not supported on group %s\n",
+				param, prop, g->name);
+		}
 		return -ENOTSUPP;
 	}
 
diff --git a/drivers/pinctrl/pinctrl-tegra.h b/drivers/pinctrl/pinctrl-tegra.h
index 6053832d433..8d94d1332e7 100644
--- a/drivers/pinctrl/pinctrl-tegra.h
+++ b/drivers/pinctrl/pinctrl-tegra.h
@@ -78,98 +78,83 @@ struct tegra_function {
 
 /**
  * struct tegra_pingroup - Tegra pin group
- * @mux_reg:		Mux register offset. -1 if unsupported.
- * @mux_bank:		Mux register bank. 0 if unsupported.
- * @mux_bit:		Mux register bit. 0 if unsupported.
- * @pupd_reg:		Pull-up/down register offset. -1 if unsupported.
- * @pupd_bank:		Pull-up/down register bank. 0 if unsupported.
- * @pupd_bit:		Pull-up/down register bit. 0 if unsupported.
- * @tri_reg:		Tri-state register offset. -1 if unsupported.
- * @tri_bank:		Tri-state register bank. 0 if unsupported.
- * @tri_bit:		Tri-state register bit. 0 if unsupported.
- * @einput_reg:		Enable-input register offset. -1 if unsupported.
- * @einput_bank:	Enable-input register bank. 0 if unsupported.
- * @einput_bit:		Enable-input register bit. 0 if unsupported.
- * @odrain_reg:		Open-drain register offset. -1 if unsupported.
- * @odrain_bank:	Open-drain register bank. 0 if unsupported.
- * @odrain_bit:		Open-drain register bit. 0 if unsupported.
- * @lock_reg:		Lock register offset. -1 if unsupported.
- * @lock_bank:		Lock register bank. 0 if unsupported.
- * @lock_bit:		Lock register bit. 0 if unsupported.
- * @ioreset_reg:	IO reset register offset. -1 if unsupported.
- * @ioreset_bank:	IO reset register bank. 0 if unsupported.
- * @ioreset_bit:	IO reset register bit. 0 if unsupported.
- * @rcv_sel_reg:	Receiver select offset. -1 if unsupported.
- * @rcv_sel_bank:	Receiver select bank. 0 if unsupported.
- * @rcv_sel_bit:	Receiver select bit. 0 if unsupported.
- * @drv_reg:		Drive fields register offset. -1 if unsupported.
- *			This register contains the hsm, schmitt, lpmd, drvdn,
- *			drvup, slwr, and slwf parameters.
- * @drv_bank:		Drive fields register bank. 0 if unsupported.
- * @hsm_bit:		High Speed Mode register bit. 0 if unsupported.
- * @schmitt_bit:	Scmitt register bit. 0 if unsupported.
- * @lpmd_bit:		Low Power Mode register bit. 0 if unsupported.
- * @drvdn_bit:		Drive Down register bit. 0 if unsupported.
- * @drvdn_width:	Drive Down field width. 0 if unsupported.
- * @drvup_bit:		Drive Up register bit. 0 if unsupported.
- * @drvup_width:	Drive Up field width. 0 if unsupported.
- * @slwr_bit:		Slew Rising register bit. 0 if unsupported.
- * @slwr_width:		Slew Rising field width. 0 if unsupported.
- * @slwf_bit:		Slew Falling register bit. 0 if unsupported.
- * @slwf_width:		Slew Falling field width. 0 if unsupported.
- * @drvtype_reg:	Drive type fields register offset. -1 if unsupported.
- * @drvtype_bank:	Drive type fields register bank. 0 if unsupported.
- * @drvtype_bit:	Drive type register bit. 0 if unsupported.
+ * @name		The name of the pin group.
+ * @pins		An array of pin IDs included in this pin group.
+ * @npins		The number of entries in @pins.
+ * @funcs		The mux functions which can be muxed onto this group.
+ * @mux_reg:		Mux register offset.
+ *			This register contains the mux, einput, odrain, lock,
+ *			ioreset, rcv_sel parameters.
+ * @mux_bank:		Mux register bank.
+ * @mux_bit:		Mux register bit.
+ * @pupd_reg:		Pull-up/down register offset.
+ * @pupd_bank:		Pull-up/down register bank.
+ * @pupd_bit:		Pull-up/down register bit.
+ * @tri_reg:		Tri-state register offset.
+ * @tri_bank:		Tri-state register bank.
+ * @tri_bit:		Tri-state register bit.
+ * @einput_bit:		Enable-input register bit.
+ * @odrain_bit:		Open-drain register bit.
+ * @lock_bit:		Lock register bit.
+ * @ioreset_bit:	IO reset register bit.
+ * @rcv_sel_bit:	Receiver select bit.
+ * @drv_reg:		Drive fields register offset.
+ *			This register contains hsm, schmitt, lpmd, drvdn,
+ *			drvup, slwr, slwf, and drvtype parameters.
+ * @drv_bank:		Drive fields register bank.
+ * @hsm_bit:		High Speed Mode register bit.
+ * @schmitt_bit:	Scmitt register bit.
+ * @lpmd_bit:		Low Power Mode register bit.
+ * @drvdn_bit:		Drive Down register bit.
+ * @drvdn_width:	Drive Down field width.
+ * @drvup_bit:		Drive Up register bit.
+ * @drvup_width:	Drive Up field width.
+ * @slwr_bit:		Slew Rising register bit.
+ * @slwr_width:		Slew Rising field width.
+ * @slwf_bit:		Slew Falling register bit.
+ * @slwf_width:		Slew Falling field width.
+ * @drvtype_bit:	Drive type register bit.
+ *
+ * -1 in a *_reg field means that feature is unsupported for this group.
+ * *_bank and *_reg values are irrelevant when *_reg is -1.
+ * When *_reg is valid, *_bit may be -1 to indicate an unsupported feature.
  *
  * A representation of a group of pins (possibly just one pin) in the Tegra
  * pin controller. Each group allows some parameter or parameters to be
  * configured. The most common is mux function selection. Many others exist
  * such as pull-up/down, tri-state, etc. Tegra's pin controller is complex;
  * certain groups may only support configuring certain parameters, hence
- * each parameter is optional, represented by a -1 "reg" value.
+ * each parameter is optional.
  */
 struct tegra_pingroup {
 	const char *name;
 	const unsigned *pins;
-	unsigned npins;
-	unsigned funcs[4];
-	unsigned func_safe;
+	u8 npins;
+	u8 funcs[4];
 	s16 mux_reg;
 	s16 pupd_reg;
 	s16 tri_reg;
-	s16 einput_reg;
-	s16 odrain_reg;
-	s16 lock_reg;
-	s16 ioreset_reg;
-	s16 rcv_sel_reg;
 	s16 drv_reg;
-	s16 drvtype_reg;
 	u32 mux_bank:2;
 	u32 pupd_bank:2;
 	u32 tri_bank:2;
-	u32 einput_bank:2;
-	u32 odrain_bank:2;
-	u32 ioreset_bank:2;
-	u32 rcv_sel_bank:2;
-	u32 lock_bank:2;
 	u32 drv_bank:2;
-	u32 drvtype_bank:2;
-	u32 mux_bit:5;
-	u32 pupd_bit:5;
-	u32 tri_bit:5;
-	u32 einput_bit:5;
-	u32 odrain_bit:5;
-	u32 lock_bit:5;
-	u32 ioreset_bit:5;
-	u32 rcv_sel_bit:5;
-	u32 hsm_bit:5;
-	u32 schmitt_bit:5;
-	u32 lpmd_bit:5;
-	u32 drvdn_bit:5;
-	u32 drvup_bit:5;
-	u32 slwr_bit:5;
-	u32 slwf_bit:5;
-	u32 drvtype_bit:5;
+	u32 mux_bit:6;
+	u32 pupd_bit:6;
+	u32 tri_bit:6;
+	u32 einput_bit:6;
+	u32 odrain_bit:6;
+	u32 lock_bit:6;
+	u32 ioreset_bit:6;
+	u32 rcv_sel_bit:6;
+	u32 hsm_bit:6;
+	u32 schmitt_bit:6;
+	u32 lpmd_bit:6;
+	u32 drvdn_bit:6;
+	u32 drvup_bit:6;
+	u32 slwr_bit:6;
+	u32 slwf_bit:6;
+	u32 drvtype_bit:6;
 	u32 drvdn_width:6;
 	u32 drvup_width:6;
 	u32 slwr_width:6;
diff --git a/drivers/pinctrl/pinctrl-tegra114.c b/drivers/pinctrl/pinctrl-tegra114.c
index 63fe7619d3f..33614baab4c 100644
--- a/drivers/pinctrl/pinctrl-tegra114.c
+++ b/drivers/pinctrl/pinctrl-tegra114.c
@@ -1547,10 +1547,12 @@ static struct tegra_function tegra114_functions[] = {
 #define DRV_PINGROUP_REG_A		0x868	/* bank 0 */
 #define PINGROUP_REG_A			0x3000	/* bank 1 */
 
-#define PINGROUP_REG_Y(r)		((r) - PINGROUP_REG_A)
-#define PINGROUP_REG_N(r)		-1
+#define PINGROUP_REG(r)			((r) - PINGROUP_REG_A)
 
-#define PINGROUP(pg_name, f0, f1, f2, f3, f_safe, r, od, ior, rcv_sel)	\
+#define PINGROUP_BIT_Y(b)		(b)
+#define PINGROUP_BIT_N(b)		(-1)
+
+#define PINGROUP(pg_name, f0, f1, f2, f3, r, od, ior, rcv_sel)		\
 	{								\
 		.name = #pg_name,					\
 		.pins = pg_name##_pins,					\
@@ -1561,38 +1563,24 @@ static struct tegra_function tegra114_functions[] = {
 			TEGRA_MUX_##f2,					\
 			TEGRA_MUX_##f3,					\
 		},							\
-		.func_safe = TEGRA_MUX_##f_safe,			\
-		.mux_reg = PINGROUP_REG_Y(r),				\
+		.mux_reg = PINGROUP_REG(r),				\
 		.mux_bank = 1,						\
 		.mux_bit = 0,						\
-		.pupd_reg = PINGROUP_REG_Y(r),				\
+		.pupd_reg = PINGROUP_REG(r),				\
 		.pupd_bank = 1,						\
 		.pupd_bit = 2,						\
-		.tri_reg = PINGROUP_REG_Y(r),				\
+		.tri_reg = PINGROUP_REG(r),				\
 		.tri_bank = 1,						\
 		.tri_bit = 4,						\
-		.einput_reg = PINGROUP_REG_Y(r),			\
-		.einput_bank = 1,					\
-		.einput_bit = 5,					\
-		.odrain_reg = PINGROUP_REG_##od(r),			\
-		.odrain_bank = 1,					\
-		.odrain_bit = 6,					\
-		.lock_reg = PINGROUP_REG_Y(r),				\
-		.lock_bank = 1,						\
-		.lock_bit = 7,						\
-		.ioreset_reg = PINGROUP_REG_##ior(r),			\
-		.ioreset_bank = 1,					\
-		.ioreset_bit = 8,					\
-		.rcv_sel_reg = PINGROUP_REG_##rcv_sel(r),		\
-		.rcv_sel_bank = 1,					\
-		.rcv_sel_bit = 9,					\
+		.einput_bit = PINGROUP_BIT_Y(5),			\
+		.odrain_bit = PINGROUP_BIT_##od(6),			\
+		.lock_bit = PINGROUP_BIT_Y(7),				\
+		.ioreset_bit = PINGROUP_BIT_##ior(8),			\
+		.rcv_sel_bit = PINGROUP_BIT_##rcv_sel(9),		\
 		.drv_reg = -1,						\
-		.drvtype_reg = -1,					\
 	}
 
-#define DRV_PINGROUP_REG_Y(r)		((r) - DRV_PINGROUP_REG_A)
-#define DRV_PINGROUP_REG_N(r)		-1
-
+#define DRV_PINGROUP_REG(r)		((r) - DRV_PINGROUP_REG_A)
 
 #define DRV_PINGROUP(pg_name, r, hsm_b, schmitt_b, lpmd_b,		\
 		     drvdn_b, drvdn_w, drvup_b, drvup_w,		\
@@ -1605,12 +1593,12 @@ static struct tegra_function tegra114_functions[] = {
 		.mux_reg = -1,						\
 		.pupd_reg = -1,						\
 		.tri_reg = -1,						\
-		.einput_reg = -1,					\
-		.odrain_reg = -1,					\
-		.lock_reg = -1,						\
-		.ioreset_reg = -1,					\
-		.rcv_sel_reg = -1,					\
-		.drv_reg = DRV_PINGROUP_REG_Y(r),			\
+		.einput_bit = -1,					\
+		.odrain_bit = -1,					\
+		.lock_bit = -1,						\
+		.ioreset_bit = -1,					\
+		.rcv_sel_bit = -1,					\
+		.drv_reg = DRV_PINGROUP_REG(r),				\
 		.drv_bank = 0,						\
 		.hsm_bit = hsm_b,					\
 		.schmitt_bit = schmitt_b,				\
@@ -1623,190 +1611,188 @@ static struct tegra_function tegra114_functions[] = {
 		.slwr_width = slwr_w,					\
 		.slwf_bit = slwf_b,					\
 		.slwf_width = slwf_w,					\
-		.drvtype_reg = DRV_PINGROUP_REG_##drvtype(r),		\
-		.drvtype_bank = 0,					\
-		.drvtype_bit = 6,					\
+		.drvtype_bit = PINGROUP_BIT_##drvtype(6),		\
 	}
 
 static const struct tegra_pingroup tegra114_groups[] = {
-	/*       pg_name,                f0,         f1,         f2,           f3,          safe,     r,      od, ior, rcv_sel */
-	PINGROUP(ulpi_data0_po1,         SPI3,       HSI,        UARTA,        ULPI,        ULPI,     0x3000,  N,  N,  N),
-	PINGROUP(ulpi_data1_po2,         SPI3,       HSI,        UARTA,        ULPI,        ULPI,     0x3004,  N,  N,  N),
-	PINGROUP(ulpi_data2_po3,         SPI3,       HSI,        UARTA,        ULPI,        ULPI,     0x3008,  N,  N,  N),
-	PINGROUP(ulpi_data3_po4,         SPI3,       HSI,        UARTA,        ULPI,        ULPI,     0x300c,  N,  N,  N),
-	PINGROUP(ulpi_data4_po5,         SPI2,       HSI,        UARTA,        ULPI,        ULPI,     0x3010,  N,  N,  N),
-	PINGROUP(ulpi_data5_po6,         SPI2,       HSI,        UARTA,        ULPI,        ULPI,     0x3014,  N,  N,  N),
-	PINGROUP(ulpi_data6_po7,         SPI2,       HSI,        UARTA,        ULPI,        ULPI,     0x3018,  N,  N,  N),
-	PINGROUP(ulpi_data7_po0,         SPI2,       HSI,        UARTA,        ULPI,        ULPI,     0x301c,  N,  N,  N),
-	PINGROUP(ulpi_clk_py0,           SPI1,       SPI5,       UARTD,        ULPI,        ULPI,     0x3020,  N,  N,  N),
-	PINGROUP(ulpi_dir_py1,           SPI1,       SPI5,       UARTD,        ULPI,        ULPI,     0x3024,  N,  N,  N),
-	PINGROUP(ulpi_nxt_py2,           SPI1,       SPI5,       UARTD,        ULPI,        ULPI,     0x3028,  N,  N,  N),
-	PINGROUP(ulpi_stp_py3,           SPI1,       SPI5,       UARTD,        ULPI,        ULPI,     0x302c,  N,  N,  N),
-	PINGROUP(dap3_fs_pp0,            I2S2,       SPI5,       DISPLAYA,     DISPLAYB,    I2S2,     0x3030,  N,  N,  N),
-	PINGROUP(dap3_din_pp1,           I2S2,       SPI5,       DISPLAYA,     DISPLAYB,    I2S2,     0x3034,  N,  N,  N),
-	PINGROUP(dap3_dout_pp2,          I2S2,       SPI5,       DISPLAYA,     DISPLAYB,    I2S2,     0x3038,  N,  N,  N),
-	PINGROUP(dap3_sclk_pp3,          I2S2,       SPI5,       DISPLAYA,     DISPLAYB,    I2S2,     0x303c,  N,  N,  N),
-	PINGROUP(pv0,                    USB,        RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x3040,  N,  N,  N),
-	PINGROUP(pv1,                    RSVD1,      RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x3044,  N,  N,  N),
-	PINGROUP(sdmmc1_clk_pz0,         SDMMC1,     CLK12,      RSVD3,        RSVD4,       RSVD4,    0x3048,  N,  N,  N),
-	PINGROUP(sdmmc1_cmd_pz1,         SDMMC1,     SPDIF,      SPI4,         UARTA,       SDMMC1,   0x304c,  N,  N,  N),
-	PINGROUP(sdmmc1_dat3_py4,        SDMMC1,     SPDIF,      SPI4,         UARTA,       SDMMC1,   0x3050,  N,  N,  N),
-	PINGROUP(sdmmc1_dat2_py5,        SDMMC1,     PWM0,       SPI4,         UARTA,       SDMMC1,   0x3054,  N,  N,  N),
-	PINGROUP(sdmmc1_dat1_py6,        SDMMC1,     PWM1,       SPI4,         UARTA,       SDMMC1,   0x3058,  N,  N,  N),
-	PINGROUP(sdmmc1_dat0_py7,        SDMMC1,     RSVD2,      SPI4,         UARTA,       RSVD2,    0x305c,  N,  N,  N),
-	PINGROUP(clk2_out_pw5,           EXTPERIPH2, RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x3068,  N,  N,  N),
-	PINGROUP(clk2_req_pcc5,          DAP,        RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x306c,  N,  N,  N),
-	PINGROUP(hdmi_int_pn7,           RSVD1,      RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x3110,  N,  N,  Y),
-	PINGROUP(ddc_scl_pv4,            I2C4,       RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x3114,  N,  N,  Y),
-	PINGROUP(ddc_sda_pv5,            I2C4,       RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x3118,  N,  N,  Y),
-	PINGROUP(uart2_rxd_pc3,          IRDA,       SPDIF,      UARTA,        SPI4,        IRDA,     0x3164,  N,  N,  N),
-	PINGROUP(uart2_txd_pc2,          IRDA,       SPDIF,      UARTA,        SPI4,        IRDA,     0x3168,  N,  N,  N),
-	PINGROUP(uart2_rts_n_pj6,        UARTA,      UARTB,      RSVD3,        SPI4,        RSVD3,    0x316c,  N,  N,  N),
-	PINGROUP(uart2_cts_n_pj5,        UARTA,      UARTB,      RSVD3,        SPI4,        RSVD3,    0x3170,  N,  N,  N),
-	PINGROUP(uart3_txd_pw6,          UARTC,      RSVD2,      RSVD3,        SPI4,        RSVD3,    0x3174,  N,  N,  N),
-	PINGROUP(uart3_rxd_pw7,          UARTC,      RSVD2,      RSVD3,        SPI4,        RSVD3,    0x3178,  N,  N,  N),
-	PINGROUP(uart3_cts_n_pa1,        UARTC,      SDMMC1,     DTV,          SPI4,        UARTC,    0x317c,  N,  N,  N),
-	PINGROUP(uart3_rts_n_pc0,        UARTC,      PWM0,       DTV,          DISPLAYA,    UARTC,    0x3180,  N,  N,  N),
-	PINGROUP(pu0,                    OWR,        UARTA,      RSVD3,        RSVD4,       RSVD4,    0x3184,  N,  N,  N),
-	PINGROUP(pu1,                    RSVD1,      UARTA,      RSVD3,        RSVD4,       RSVD4,    0x3188,  N,  N,  N),
-	PINGROUP(pu2,                    RSVD1,      UARTA,      RSVD3,        RSVD4,       RSVD4,    0x318c,  N,  N,  N),
-	PINGROUP(pu3,                    PWM0,       UARTA,      DISPLAYA,     DISPLAYB,    PWM0,     0x3190,  N,  N,  N),
-	PINGROUP(pu4,                    PWM1,       UARTA,      DISPLAYA,     DISPLAYB,    PWM1,     0x3194,  N,  N,  N),
-	PINGROUP(pu5,                    PWM2,       UARTA,      DISPLAYA,     DISPLAYB,    PWM2,     0x3198,  N,  N,  N),
-	PINGROUP(pu6,                    PWM3,       UARTA,      USB,          DISPLAYB,    PWM3,     0x319c,  N,  N,  N),
-	PINGROUP(gen1_i2c_sda_pc5,       I2C1,       RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x31a0,  Y,  N,  N),
-	PINGROUP(gen1_i2c_scl_pc4,       I2C1,       RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x31a4,  Y,  N,  N),
-	PINGROUP(dap4_fs_pp4,            I2S3,       RSVD2,      DTV,          RSVD4,       RSVD4,    0x31a8,  N,  N,  N),
-	PINGROUP(dap4_din_pp5,           I2S3,       RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x31ac,  N,  N,  N),
-	PINGROUP(dap4_dout_pp6,          I2S3,       RSVD2,      DTV,          RSVD4,       RSVD4,    0x31b0,  N,  N,  N),
-	PINGROUP(dap4_sclk_pp7,          I2S3,       RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x31b4,  N,  N,  N),
-	PINGROUP(clk3_out_pee0,          EXTPERIPH3, RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x31b8,  N,  N,  N),
-	PINGROUP(clk3_req_pee1,          DEV3,       RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x31bc,  N,  N,  N),
-	PINGROUP(gmi_wp_n_pc7,           RSVD1,      NAND,       GMI,          GMI_ALT,     RSVD1,    0x31c0,  N,  N,  N),
-	PINGROUP(gmi_iordy_pi5,          SDMMC2,     RSVD2,      GMI,          TRACE,       RSVD2,    0x31c4,  N,  N,  N),
-	PINGROUP(gmi_wait_pi7,           SPI4,       NAND,       GMI,          DTV,         NAND,     0x31c8,  N,  N,  N),
-	PINGROUP(gmi_adv_n_pk0,          RSVD1,      NAND,       GMI,          TRACE,       RSVD1,    0x31cc,  N,  N,  N),
-	PINGROUP(gmi_clk_pk1,            SDMMC2,     NAND,       GMI,          TRACE,       GMI,      0x31d0,  N,  N,  N),
-	PINGROUP(gmi_cs0_n_pj0,          RSVD1,      NAND,       GMI,          USB,         RSVD1,    0x31d4,  N,  N,  N),
-	PINGROUP(gmi_cs1_n_pj2,          RSVD1,      NAND,       GMI,          SOC,         RSVD1,    0x31d8,  N,  N,  N),
-	PINGROUP(gmi_cs2_n_pk3,          SDMMC2,     NAND,       GMI,          TRACE,       GMI,      0x31dc,  N,  N,  N),
-	PINGROUP(gmi_cs3_n_pk4,          SDMMC2,     NAND,       GMI,          GMI_ALT,     GMI,      0x31e0,  N,  N,  N),
-	PINGROUP(gmi_cs4_n_pk2,          USB,        NAND,       GMI,          TRACE,       GMI,      0x31e4,  N,  N,  N),
-	PINGROUP(gmi_cs6_n_pi3,          NAND,       NAND_ALT,   GMI,          SPI4,        NAND,     0x31e8,  N,  N,  N),
-	PINGROUP(gmi_cs7_n_pi6,          NAND,       NAND_ALT,   GMI,          SDMMC2,      NAND,     0x31ec,  N,  N,  N),
-	PINGROUP(gmi_ad0_pg0,            RSVD1,      NAND,       GMI,          RSVD4,       RSVD4,    0x31f0,  N,  N,  N),
-	PINGROUP(gmi_ad1_pg1,            RSVD1,      NAND,       GMI,          RSVD4,       RSVD4,    0x31f4,  N,  N,  N),
-	PINGROUP(gmi_ad2_pg2,            RSVD1,      NAND,       GMI,          RSVD4,       RSVD4,    0x31f8,  N,  N,  N),
-	PINGROUP(gmi_ad3_pg3,            RSVD1,      NAND,       GMI,          RSVD4,       RSVD4,    0x31fc,  N,  N,  N),
-	PINGROUP(gmi_ad4_pg4,            RSVD1,      NAND,       GMI,          RSVD4,       RSVD4,    0x3200,  N,  N,  N),
-	PINGROUP(gmi_ad5_pg5,            RSVD1,      NAND,       GMI,          SPI4,        RSVD1,    0x3204,  N,  N,  N),
-	PINGROUP(gmi_ad6_pg6,            RSVD1,      NAND,       GMI,          SPI4,        RSVD1,    0x3208,  N,  N,  N),
-	PINGROUP(gmi_ad7_pg7,            RSVD1,      NAND,       GMI,          SPI4,        RSVD1,    0x320c,  N,  N,  N),
-	PINGROUP(gmi_ad8_ph0,            PWM0,       NAND,       GMI,          DTV,         GMI,      0x3210,  N,  N,  N),
-	PINGROUP(gmi_ad9_ph1,            PWM1,       NAND,       GMI,          CLDVFS,      GMI,      0x3214,  N,  N,  N),
-	PINGROUP(gmi_ad10_ph2,           PWM2,       NAND,       GMI,          CLDVFS,      GMI,      0x3218,  N,  N,  N),
-	PINGROUP(gmi_ad11_ph3,           PWM3,       NAND,       GMI,          USB,         GMI,      0x321c,  N,  N,  N),
-	PINGROUP(gmi_ad12_ph4,           SDMMC2,     NAND,       GMI,          RSVD4,       RSVD4,    0x3220,  N,  N,  N),
-	PINGROUP(gmi_ad13_ph5,           SDMMC2,     NAND,       GMI,          RSVD4,       RSVD4,    0x3224,  N,  N,  N),
-	PINGROUP(gmi_ad14_ph6,           SDMMC2,     NAND,       GMI,          DTV,         GMI,      0x3228,  N,  N,  N),
-	PINGROUP(gmi_ad15_ph7,           SDMMC2,     NAND,       GMI,          DTV,         GMI,      0x322c,  N,  N,  N),
-	PINGROUP(gmi_a16_pj7,            UARTD,      TRACE,      GMI,          GMI_ALT,     GMI,      0x3230,  N,  N,  N),
-	PINGROUP(gmi_a17_pb0,            UARTD,      RSVD2,      GMI,          TRACE,       RSVD2,    0x3234,  N,  N,  N),
-	PINGROUP(gmi_a18_pb1,            UARTD,      RSVD2,      GMI,          TRACE,       RSVD2,    0x3238,  N,  N,  N),
-	PINGROUP(gmi_a19_pk7,            UARTD,      SPI4,       GMI,          TRACE,       GMI,      0x323c,  N,  N,  N),
-	PINGROUP(gmi_wr_n_pi0,           RSVD1,      NAND,       GMI,          SPI4,        RSVD1,    0x3240,  N,  N,  N),
-	PINGROUP(gmi_oe_n_pi1,           RSVD1,      NAND,       GMI,          SOC,         RSVD1,    0x3244,  N,  N,  N),
-	PINGROUP(gmi_dqs_p_pj3,          SDMMC2,     NAND,       GMI,          TRACE,       NAND,     0x3248,  N,  N,  N),
-	PINGROUP(gmi_rst_n_pi4,          NAND,       NAND_ALT,   GMI,          RSVD4,       RSVD4,    0x324c,  N,  N,  N),
-	PINGROUP(gen2_i2c_scl_pt5,       I2C2,       RSVD2,      GMI,          RSVD4,       RSVD4,    0x3250,  Y,  N,  N),
-	PINGROUP(gen2_i2c_sda_pt6,       I2C2,       RSVD2,      GMI,          RSVD4,       RSVD4,    0x3254,  Y,  N,  N),
-	PINGROUP(sdmmc4_clk_pcc4,        SDMMC4,     RSVD2,      GMI,          RSVD4,       RSVD4,    0x3258,  N,  Y,  N),
-	PINGROUP(sdmmc4_cmd_pt7,         SDMMC4,     RSVD2,      GMI,          RSVD4,       RSVD4,    0x325c,  N,  Y,  N),
-	PINGROUP(sdmmc4_dat0_paa0,       SDMMC4,     SPI3,       GMI,          RSVD4,       RSVD4,    0x3260,  N,  Y,  N),
-	PINGROUP(sdmmc4_dat1_paa1,       SDMMC4,     SPI3,       GMI,          RSVD4,       RSVD4,    0x3264,  N,  Y,  N),
-	PINGROUP(sdmmc4_dat2_paa2,       SDMMC4,     SPI3,       GMI,          RSVD4,       RSVD4,    0x3268,  N,  Y,  N),
-	PINGROUP(sdmmc4_dat3_paa3,       SDMMC4,     SPI3,       GMI,          RSVD4,       RSVD4,    0x326c,  N,  Y,  N),
-	PINGROUP(sdmmc4_dat4_paa4,       SDMMC4,     SPI3,       GMI,          RSVD4,       RSVD4,    0x3270,  N,  Y,  N),
-	PINGROUP(sdmmc4_dat5_paa5,       SDMMC4,     SPI3,       GMI,          RSVD4,       RSVD4,    0x3274,  N,  Y,  N),
-	PINGROUP(sdmmc4_dat6_paa6,       SDMMC4,     SPI3,       GMI,          RSVD4,       RSVD4,    0x3278,  N,  Y,  N),
-	PINGROUP(sdmmc4_dat7_paa7,       SDMMC4,     RSVD2,      GMI,          RSVD4,       RSVD4,    0x327c,  N,  Y,  N),
-	PINGROUP(cam_mclk_pcc0,          VI,         VI_ALT1,    VI_ALT3,      RSVD4,       RSVD4,    0x3284,  N,  N,  N),
-	PINGROUP(pcc1,                   I2S4,       RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x3288,  N,  N,  N),
-	PINGROUP(pbb0,                   I2S4,       VI,         VI_ALT1,      VI_ALT3,     I2S4,     0x328c,  N,  N,  N),
-	PINGROUP(cam_i2c_scl_pbb1,       VGP1,       I2C3,       RSVD3,        RSVD4,       RSVD4,    0x3290,  Y,  N,  N),
-	PINGROUP(cam_i2c_sda_pbb2,       VGP2,       I2C3,       RSVD3,        RSVD4,       RSVD4,    0x3294,  Y,  N,  N),
-	PINGROUP(pbb3,                   VGP3,       DISPLAYA,   DISPLAYB,     RSVD4,       RSVD4,    0x3298,  N,  N,  N),
-	PINGROUP(pbb4,                   VGP4,       DISPLAYA,   DISPLAYB,     RSVD4,       RSVD4,    0x329c,  N,  N,  N),
-	PINGROUP(pbb5,                   VGP5,       DISPLAYA,   DISPLAYB,     RSVD4,       RSVD4,    0x32a0,  N,  N,  N),
-	PINGROUP(pbb6,                   VGP6,       DISPLAYA,   DISPLAYB,     RSVD4,       RSVD4,    0x32a4,  N,  N,  N),
-	PINGROUP(pbb7,                   I2S4,       RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x32a8,  N,  N,  N),
-	PINGROUP(pcc2,                   I2S4,       RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x32ac,  N,  N,  N),
-	PINGROUP(jtag_rtck,              RTCK,       RSVD2,      RSVD3,        RSVD4,       RTCK,     0x32b0,  N,  N,  N),
-	PINGROUP(pwr_i2c_scl_pz6,        I2CPWR,     RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x32b4,  Y,  N,  N),
-	PINGROUP(pwr_i2c_sda_pz7,        I2CPWR,     RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x32b8,  Y,  N,  N),
-	PINGROUP(kb_row0_pr0,            KBC,        RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x32bc,  N,  N,  N),
-	PINGROUP(kb_row1_pr1,            KBC,        RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x32c0,  N,  N,  N),
-	PINGROUP(kb_row2_pr2,            KBC,        RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x32c4,  N,  N,  N),
-	PINGROUP(kb_row3_pr3,            KBC,        DISPLAYA,   RSVD3,        DISPLAYB,    RSVD3,    0x32c8,  N,  N,  N),
-	PINGROUP(kb_row4_pr4,            KBC,        DISPLAYA,   SPI2,         DISPLAYB,    KBC,      0x32cc,  N,  N,  N),
-	PINGROUP(kb_row5_pr5,            KBC,        DISPLAYA,   SPI2,         DISPLAYB,    KBC,      0x32d0,  N,  N,  N),
-	PINGROUP(kb_row6_pr6,            KBC,        DISPLAYA,   DISPLAYA_ALT, DISPLAYB,    KBC,      0x32d4,  N,  N,  N),
-	PINGROUP(kb_row7_pr7,            KBC,        RSVD2,      CLDVFS,       UARTA,       RSVD2,    0x32d8,  N,  N,  N),
-	PINGROUP(kb_row8_ps0,            KBC,        RSVD2,      CLDVFS,       UARTA,       RSVD2,    0x32dc,  N,  N,  N),
-	PINGROUP(kb_row9_ps1,            KBC,        RSVD2,      RSVD3,        UARTA,       RSVD3,    0x32e0,  N,  N,  N),
-	PINGROUP(kb_row10_ps2,           KBC,        RSVD2,      RSVD3,        UARTA,       RSVD3,    0x32e4,  N,  N,  N),
-	PINGROUP(kb_col0_pq0,            KBC,        USB,        SPI2,         EMC_DLL,     KBC,      0x32fc,  N,  N,  N),
-	PINGROUP(kb_col1_pq1,            KBC,        RSVD2,      SPI2,         EMC_DLL,     RSVD2,    0x3300,  N,  N,  N),
-	PINGROUP(kb_col2_pq2,            KBC,        RSVD2,      SPI2,         RSVD4,       RSVD2,    0x3304,  N,  N,  N),
-	PINGROUP(kb_col3_pq3,            KBC,        DISPLAYA,   PWM2,         UARTA,       KBC,      0x3308,  N,  N,  N),
-	PINGROUP(kb_col4_pq4,            KBC,        OWR,        SDMMC3,       UARTA,       KBC,      0x330c,  N,  N,  N),
-	PINGROUP(kb_col5_pq5,            KBC,        RSVD2,      SDMMC1,       RSVD4,       RSVD4,    0x3310,  N,  N,  N),
-	PINGROUP(kb_col6_pq6,            KBC,        RSVD2,      SPI2,         RSVD4,       RSVD4,    0x3314,  N,  N,  N),
-	PINGROUP(kb_col7_pq7,            KBC,        RSVD2,      SPI2,         RSVD4,       RSVD4,    0x3318,  N,  N,  N),
-	PINGROUP(clk_32k_out_pa0,        BLINK,      SOC,        RSVD3,        RSVD4,       RSVD4,    0x331c,  N,  N,  N),
-	PINGROUP(sys_clk_req_pz5,        SYSCLK,     RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x3320,  N,  N,  N),
-	PINGROUP(core_pwr_req,           PWRON,      RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x3324,  N,  N,  N),
-	PINGROUP(cpu_pwr_req,            CPU,        RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x3328,  N,  N,  N),
-	PINGROUP(pwr_int_n,              PMI,        RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x332c,  N,  N,  N),
-	PINGROUP(clk_32k_in,             CLK,        RSVD2,      RSVD3,        RSVD4,       CLK,      0x3330,  N,  N,  N),
-	PINGROUP(owr,                    OWR,        RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x3334,  N,  N,  Y),
-	PINGROUP(dap1_fs_pn0,            I2S0,       HDA,        GMI,          RSVD4,       RSVD4,    0x3338,  N,  N,  N),
-	PINGROUP(dap1_din_pn1,           I2S0,       HDA,        GMI,          RSVD4,       RSVD4,    0x333c,  N,  N,  N),
-	PINGROUP(dap1_dout_pn2,          I2S0,       HDA,        GMI,          RSVD4,       RSVD4,    0x3340,  N,  N,  N),
-	PINGROUP(dap1_sclk_pn3,          I2S0,       HDA,        GMI,          RSVD4,       RSVD4,    0x3344,  N,  N,  N),
-	PINGROUP(clk1_req_pee2,          DAP,        DAP1,       RSVD3,        RSVD4,       RSVD4,    0x3348,  N,  N,  N),
-	PINGROUP(clk1_out_pw4,           EXTPERIPH1, DAP2,       RSVD3,        RSVD4,       RSVD4,    0x334c,  N,  N,  N),
-	PINGROUP(spdif_in_pk6,           SPDIF,      USB,        RSVD3,        RSVD4,       RSVD4,    0x3350,  N,  N,  N),
-	PINGROUP(spdif_out_pk5,          SPDIF,      RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x3354,  N,  N,  N),
-	PINGROUP(dap2_fs_pa2,            I2S1,       HDA,        RSVD3,        RSVD4,       RSVD4,    0x3358,  N,  N,  N),
-	PINGROUP(dap2_din_pa4,           I2S1,       HDA,        RSVD3,        RSVD4,       RSVD4,    0x335c,  N,  N,  N),
-	PINGROUP(dap2_dout_pa5,          I2S1,       HDA,        RSVD3,        RSVD4,       RSVD4,    0x3360,  N,  N,  N),
-	PINGROUP(dap2_sclk_pa3,          I2S1,       HDA,        RSVD3,        RSVD4,       RSVD4,    0x3364,  N,  N,  N),
-	PINGROUP(dvfs_pwm_px0,           SPI6,       CLDVFS,     RSVD3,        RSVD4,       RSVD4,    0x3368,  N,  N,  N),
-	PINGROUP(gpio_x1_aud_px1,        SPI6,       RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x336c,  N,  N,  N),
-	PINGROUP(gpio_x3_aud_px3,        SPI6,       SPI1,       RSVD3,        RSVD4,       RSVD4,    0x3370,  N,  N,  N),
-	PINGROUP(dvfs_clk_px2,           SPI6,       CLDVFS,     RSVD3,        RSVD4,       RSVD4,    0x3374,  N,  N,  N),
-	PINGROUP(gpio_x4_aud_px4,        RSVD1,      SPI1,       SPI2,         DAP2,        RSVD1,    0x3378,  N,  N,  N),
-	PINGROUP(gpio_x5_aud_px5,        RSVD1,      SPI1,       SPI2,         RSVD4,       RSVD1,    0x337c,  N,  N,  N),
-	PINGROUP(gpio_x6_aud_px6,        SPI6,       SPI1,       SPI2,         RSVD4,       RSVD4,    0x3380,  N,  N,  N),
-	PINGROUP(gpio_x7_aud_px7,        RSVD1,      SPI1,       SPI2,         RSVD4,       RSVD4,    0x3384,  N,  N,  N),
-	PINGROUP(sdmmc3_clk_pa6,         SDMMC3,     RSVD2,      RSVD3,        SPI3,        RSVD3,    0x3390,  N,  N,  N),
-	PINGROUP(sdmmc3_cmd_pa7,         SDMMC3,     PWM3,       UARTA,        SPI3,        SDMMC3,   0x3394,  N,  N,  N),
-	PINGROUP(sdmmc3_dat0_pb7,        SDMMC3,     RSVD2,      RSVD3,        SPI3,        RSVD3,    0x3398,  N,  N,  N),
-	PINGROUP(sdmmc3_dat1_pb6,        SDMMC3,     PWM2,       UARTA,        SPI3,        SDMMC3,   0x339c,  N,  N,  N),
-	PINGROUP(sdmmc3_dat2_pb5,        SDMMC3,     PWM1,       DISPLAYA,     SPI3,        SDMMC3,   0x33a0,  N,  N,  N),
-	PINGROUP(sdmmc3_dat3_pb4,        SDMMC3,     PWM0,       DISPLAYB,     SPI3,        SDMMC3,   0x33a4,  N,  N,  N),
-	PINGROUP(hdmi_cec_pee3,          CEC,        SDMMC3,     RSVD3,        SOC,         RSVD3,    0x33e0,  Y,  N,  N),
-	PINGROUP(sdmmc1_wp_n_pv3,        SDMMC1,     CLK12,      SPI4,         UARTA,       SDMMC1,   0x33e4,  N,  N,  N),
-	PINGROUP(sdmmc3_cd_n_pv2,        SDMMC3,     OWR,        RSVD3,        RSVD4,       RSVD4,    0x33e8,  N,  N,  N),
-	PINGROUP(gpio_w2_aud_pw2,        SPI6,       RSVD2,      SPI2,         I2C1,        RSVD2,    0x33ec,  N,  N,  N),
-	PINGROUP(gpio_w3_aud_pw3,        SPI6,       SPI1,       SPI2,         I2C1,        SPI6,     0x33f0,  N,  N,  N),
-	PINGROUP(usb_vbus_en0_pn4,       USB,        RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x33f4,  Y,  N,  N),
-	PINGROUP(usb_vbus_en1_pn5,       USB,        RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x33f8,  Y,  N,  N),
-	PINGROUP(sdmmc3_clk_lb_in_pee5,  SDMMC3,     RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x33fc,  N,  N,  N),
-	PINGROUP(sdmmc3_clk_lb_out_pee4, SDMMC3,     RSVD2,      RSVD3,        RSVD4,       RSVD4,    0x3400,  N,  N,  N),
-	PINGROUP(gmi_clk_lb,             SDMMC2,     NAND,       GMI,          RSVD4,       GMI,      0x3404,  N,  N,  N),
-	PINGROUP(reset_out_n,            RSVD1,      RSVD2,      RSVD3,        RESET_OUT_N, RSVD3,    0x3408,  N,  N,  N),
+	/*       pg_name,                f0,         f1,         f2,           f3,          r,      od, ior, rcv_sel */
+	PINGROUP(ulpi_data0_po1,         SPI3,       HSI,        UARTA,        ULPI,        0x3000, N,   N,  N),
+	PINGROUP(ulpi_data1_po2,         SPI3,       HSI,        UARTA,        ULPI,        0x3004, N,   N,  N),
+	PINGROUP(ulpi_data2_po3,         SPI3,       HSI,        UARTA,        ULPI,        0x3008, N,   N,  N),
+	PINGROUP(ulpi_data3_po4,         SPI3,       HSI,        UARTA,        ULPI,        0x300c, N,   N,  N),
+	PINGROUP(ulpi_data4_po5,         SPI2,       HSI,        UARTA,        ULPI,        0x3010, N,   N,  N),
+	PINGROUP(ulpi_data5_po6,         SPI2,       HSI,        UARTA,        ULPI,        0x3014, N,   N,  N),
+	PINGROUP(ulpi_data6_po7,         SPI2,       HSI,        UARTA,        ULPI,        0x3018, N,   N,  N),
+	PINGROUP(ulpi_data7_po0,         SPI2,       HSI,        UARTA,        ULPI,        0x301c, N,   N,  N),
+	PINGROUP(ulpi_clk_py0,           SPI1,       SPI5,       UARTD,        ULPI,        0x3020, N,   N,  N),
+	PINGROUP(ulpi_dir_py1,           SPI1,       SPI5,       UARTD,        ULPI,        0x3024, N,   N,  N),
+	PINGROUP(ulpi_nxt_py2,           SPI1,       SPI5,       UARTD,        ULPI,        0x3028, N,   N,  N),
+	PINGROUP(ulpi_stp_py3,           SPI1,       SPI5,       UARTD,        ULPI,        0x302c, N,   N,  N),
+	PINGROUP(dap3_fs_pp0,            I2S2,       SPI5,       DISPLAYA,     DISPLAYB,    0x3030, N,   N,  N),
+	PINGROUP(dap3_din_pp1,           I2S2,       SPI5,       DISPLAYA,     DISPLAYB,    0x3034, N,   N,  N),
+	PINGROUP(dap3_dout_pp2,          I2S2,       SPI5,       DISPLAYA,     DISPLAYB,    0x3038, N,   N,  N),
+	PINGROUP(dap3_sclk_pp3,          I2S2,       SPI5,       DISPLAYA,     DISPLAYB,    0x303c, N,   N,  N),
+	PINGROUP(pv0,                    USB,        RSVD2,      RSVD3,        RSVD4,       0x3040, N,   N,  N),
+	PINGROUP(pv1,                    RSVD1,      RSVD2,      RSVD3,        RSVD4,       0x3044, N,   N,  N),
+	PINGROUP(sdmmc1_clk_pz0,         SDMMC1,     CLK12,      RSVD3,        RSVD4,       0x3048, N,   N,  N),
+	PINGROUP(sdmmc1_cmd_pz1,         SDMMC1,     SPDIF,      SPI4,         UARTA,       0x304c, N,   N,  N),
+	PINGROUP(sdmmc1_dat3_py4,        SDMMC1,     SPDIF,      SPI4,         UARTA,       0x3050, N,   N,  N),
+	PINGROUP(sdmmc1_dat2_py5,        SDMMC1,     PWM0,       SPI4,         UARTA,       0x3054, N,   N,  N),
+	PINGROUP(sdmmc1_dat1_py6,        SDMMC1,     PWM1,       SPI4,         UARTA,       0x3058, N,   N,  N),
+	PINGROUP(sdmmc1_dat0_py7,        SDMMC1,     RSVD2,      SPI4,         UARTA,       0x305c, N,   N,  N),
+	PINGROUP(clk2_out_pw5,           EXTPERIPH2, RSVD2,      RSVD3,        RSVD4,       0x3068, N,   N,  N),
+	PINGROUP(clk2_req_pcc5,          DAP,        RSVD2,      RSVD3,        RSVD4,       0x306c, N,   N,  N),
+	PINGROUP(hdmi_int_pn7,           RSVD1,      RSVD2,      RSVD3,        RSVD4,       0x3110, N,   N,  Y),
+	PINGROUP(ddc_scl_pv4,            I2C4,       RSVD2,      RSVD3,        RSVD4,       0x3114, N,   N,  Y),
+	PINGROUP(ddc_sda_pv5,            I2C4,       RSVD2,      RSVD3,        RSVD4,       0x3118, N,   N,  Y),
+	PINGROUP(uart2_rxd_pc3,          IRDA,       SPDIF,      UARTA,        SPI4,        0x3164, N,   N,  N),
+	PINGROUP(uart2_txd_pc2,          IRDA,       SPDIF,      UARTA,        SPI4,        0x3168, N,   N,  N),
+	PINGROUP(uart2_rts_n_pj6,        UARTA,      UARTB,      RSVD3,        SPI4,        0x316c, N,   N,  N),
+	PINGROUP(uart2_cts_n_pj5,        UARTA,      UARTB,      RSVD3,        SPI4,        0x3170, N,   N,  N),
+	PINGROUP(uart3_txd_pw6,          UARTC,      RSVD2,      RSVD3,        SPI4,        0x3174, N,   N,  N),
+	PINGROUP(uart3_rxd_pw7,          UARTC,      RSVD2,      RSVD3,        SPI4,        0x3178, N,   N,  N),
+	PINGROUP(uart3_cts_n_pa1,        UARTC,      SDMMC1,     DTV,          SPI4,        0x317c, N,   N,  N),
+	PINGROUP(uart3_rts_n_pc0,        UARTC,      PWM0,       DTV,          DISPLAYA,    0x3180, N,   N,  N),
+	PINGROUP(pu0,                    OWR,        UARTA,      RSVD3,        RSVD4,       0x3184, N,   N,  N),
+	PINGROUP(pu1,                    RSVD1,      UARTA,      RSVD3,        RSVD4,       0x3188, N,   N,  N),
+	PINGROUP(pu2,                    RSVD1,      UARTA,      RSVD3,        RSVD4,       0x318c, N,   N,  N),
+	PINGROUP(pu3,                    PWM0,       UARTA,      DISPLAYA,     DISPLAYB,    0x3190, N,   N,  N),
+	PINGROUP(pu4,                    PWM1,       UARTA,      DISPLAYA,     DISPLAYB,    0x3194, N,   N,  N),
+	PINGROUP(pu5,                    PWM2,       UARTA,      DISPLAYA,     DISPLAYB,    0x3198, N,   N,  N),
+	PINGROUP(pu6,                    PWM3,       UARTA,      USB,          DISPLAYB,    0x319c, N,   N,  N),
+	PINGROUP(gen1_i2c_sda_pc5,       I2C1,       RSVD2,      RSVD3,        RSVD4,       0x31a0, Y,   N,  N),
+	PINGROUP(gen1_i2c_scl_pc4,       I2C1,       RSVD2,      RSVD3,        RSVD4,       0x31a4, Y,   N,  N),
+	PINGROUP(dap4_fs_pp4,            I2S3,       RSVD2,      DTV,          RSVD4,       0x31a8, N,   N,  N),
+	PINGROUP(dap4_din_pp5,           I2S3,       RSVD2,      RSVD3,        RSVD4,       0x31ac, N,   N,  N),
+	PINGROUP(dap4_dout_pp6,          I2S3,       RSVD2,      DTV,          RSVD4,       0x31b0, N,   N,  N),
+	PINGROUP(dap4_sclk_pp7,          I2S3,       RSVD2,      RSVD3,        RSVD4,       0x31b4, N,   N,  N),
+	PINGROUP(clk3_out_pee0,          EXTPERIPH3, RSVD2,      RSVD3,        RSVD4,       0x31b8, N,   N,  N),
+	PINGROUP(clk3_req_pee1,          DEV3,       RSVD2,      RSVD3,        RSVD4,       0x31bc, N,   N,  N),
+	PINGROUP(gmi_wp_n_pc7,           RSVD1,      NAND,       GMI,          GMI_ALT,     0x31c0, N,   N,  N),
+	PINGROUP(gmi_iordy_pi5,          SDMMC2,     RSVD2,      GMI,          TRACE,       0x31c4, N,   N,  N),
+	PINGROUP(gmi_wait_pi7,           SPI4,       NAND,       GMI,          DTV,         0x31c8, N,   N,  N),
+	PINGROUP(gmi_adv_n_pk0,          RSVD1,      NAND,       GMI,          TRACE,       0x31cc, N,   N,  N),
+	PINGROUP(gmi_clk_pk1,            SDMMC2,     NAND,       GMI,          TRACE,       0x31d0, N,   N,  N),
+	PINGROUP(gmi_cs0_n_pj0,          RSVD1,      NAND,       GMI,          USB,         0x31d4, N,   N,  N),
+	PINGROUP(gmi_cs1_n_pj2,          RSVD1,      NAND,       GMI,          SOC,         0x31d8, N,   N,  N),
+	PINGROUP(gmi_cs2_n_pk3,          SDMMC2,     NAND,       GMI,          TRACE,       0x31dc, N,   N,  N),
+	PINGROUP(gmi_cs3_n_pk4,          SDMMC2,     NAND,       GMI,          GMI_ALT,     0x31e0, N,   N,  N),
+	PINGROUP(gmi_cs4_n_pk2,          USB,        NAND,       GMI,          TRACE,       0x31e4, N,   N,  N),
+	PINGROUP(gmi_cs6_n_pi3,          NAND,       NAND_ALT,   GMI,          SPI4,        0x31e8, N,   N,  N),
+	PINGROUP(gmi_cs7_n_pi6,          NAND,       NAND_ALT,   GMI,          SDMMC2,      0x31ec, N,   N,  N),
+	PINGROUP(gmi_ad0_pg0,            RSVD1,      NAND,       GMI,          RSVD4,       0x31f0, N,   N,  N),
+	PINGROUP(gmi_ad1_pg1,            RSVD1,      NAND,       GMI,          RSVD4,       0x31f4, N,   N,  N),
+	PINGROUP(gmi_ad2_pg2,            RSVD1,      NAND,       GMI,          RSVD4,       0x31f8, N,   N,  N),
+	PINGROUP(gmi_ad3_pg3,            RSVD1,      NAND,       GMI,          RSVD4,       0x31fc, N,   N,  N),
+	PINGROUP(gmi_ad4_pg4,            RSVD1,      NAND,       GMI,          RSVD4,       0x3200, N,   N,  N),
+	PINGROUP(gmi_ad5_pg5,            RSVD1,      NAND,       GMI,          SPI4,        0x3204, N,   N,  N),
+	PINGROUP(gmi_ad6_pg6,            RSVD1,      NAND,       GMI,          SPI4,        0x3208, N,   N,  N),
+	PINGROUP(gmi_ad7_pg7,            RSVD1,      NAND,       GMI,          SPI4,        0x320c, N,   N,  N),
+	PINGROUP(gmi_ad8_ph0,            PWM0,       NAND,       GMI,          DTV,         0x3210, N,   N,  N),
+	PINGROUP(gmi_ad9_ph1,            PWM1,       NAND,       GMI,          CLDVFS,      0x3214, N,   N,  N),
+	PINGROUP(gmi_ad10_ph2,           PWM2,       NAND,       GMI,          CLDVFS,      0x3218, N,   N,  N),
+	PINGROUP(gmi_ad11_ph3,           PWM3,       NAND,       GMI,          USB,         0x321c, N,   N,  N),
+	PINGROUP(gmi_ad12_ph4,           SDMMC2,     NAND,       GMI,          RSVD4,       0x3220, N,   N,  N),
+	PINGROUP(gmi_ad13_ph5,           SDMMC2,     NAND,       GMI,          RSVD4,       0x3224, N,   N,  N),
+	PINGROUP(gmi_ad14_ph6,           SDMMC2,     NAND,       GMI,          DTV,         0x3228, N,   N,  N),
+	PINGROUP(gmi_ad15_ph7,           SDMMC2,     NAND,       GMI,          DTV,         0x322c, N,   N,  N),
+	PINGROUP(gmi_a16_pj7,            UARTD,      TRACE,      GMI,          GMI_ALT,     0x3230, N,   N,  N),
+	PINGROUP(gmi_a17_pb0,            UARTD,      RSVD2,      GMI,          TRACE,       0x3234, N,   N,  N),
+	PINGROUP(gmi_a18_pb1,            UARTD,      RSVD2,      GMI,          TRACE,       0x3238, N,   N,  N),
+	PINGROUP(gmi_a19_pk7,            UARTD,      SPI4,       GMI,          TRACE,       0x323c, N,   N,  N),
+	PINGROUP(gmi_wr_n_pi0,           RSVD1,      NAND,       GMI,          SPI4,        0x3240, N,   N,  N),
+	PINGROUP(gmi_oe_n_pi1,           RSVD1,      NAND,       GMI,          SOC,         0x3244, N,   N,  N),
+	PINGROUP(gmi_dqs_p_pj3,          SDMMC2,     NAND,       GMI,          TRACE,       0x3248, N,   N,  N),
+	PINGROUP(gmi_rst_n_pi4,          NAND,       NAND_ALT,   GMI,          RSVD4,       0x324c, N,   N,  N),
+	PINGROUP(gen2_i2c_scl_pt5,       I2C2,       RSVD2,      GMI,          RSVD4,       0x3250, Y,   N,  N),
+	PINGROUP(gen2_i2c_sda_pt6,       I2C2,       RSVD2,      GMI,          RSVD4,       0x3254, Y,   N,  N),
+	PINGROUP(sdmmc4_clk_pcc4,        SDMMC4,     RSVD2,      GMI,          RSVD4,       0x3258, N,   Y,  N),
+	PINGROUP(sdmmc4_cmd_pt7,         SDMMC4,     RSVD2,      GMI,          RSVD4,       0x325c, N,   Y,  N),
+	PINGROUP(sdmmc4_dat0_paa0,       SDMMC4,     SPI3,       GMI,          RSVD4,       0x3260, N,   Y,  N),
+	PINGROUP(sdmmc4_dat1_paa1,       SDMMC4,     SPI3,       GMI,          RSVD4,       0x3264, N,   Y,  N),
+	PINGROUP(sdmmc4_dat2_paa2,       SDMMC4,     SPI3,       GMI,          RSVD4,       0x3268, N,   Y,  N),
+	PINGROUP(sdmmc4_dat3_paa3,       SDMMC4,     SPI3,       GMI,          RSVD4,       0x326c, N,   Y,  N),
+	PINGROUP(sdmmc4_dat4_paa4,       SDMMC4,     SPI3,       GMI,          RSVD4,       0x3270, N,   Y,  N),
+	PINGROUP(sdmmc4_dat5_paa5,       SDMMC4,     SPI3,       GMI,          RSVD4,       0x3274, N,   Y,  N),
+	PINGROUP(sdmmc4_dat6_paa6,       SDMMC4,     SPI3,       GMI,          RSVD4,       0x3278, N,   Y,  N),
+	PINGROUP(sdmmc4_dat7_paa7,       SDMMC4,     RSVD2,      GMI,          RSVD4,       0x327c, N,   Y,  N),
+	PINGROUP(cam_mclk_pcc0,          VI,         VI_ALT1,    VI_ALT3,      RSVD4,       0x3284, N,   N,  N),
+	PINGROUP(pcc1,                   I2S4,       RSVD2,      RSVD3,        RSVD4,       0x3288, N,   N,  N),
+	PINGROUP(pbb0,                   I2S4,       VI,         VI_ALT1,      VI_ALT3,     0x328c, N,   N,  N),
+	PINGROUP(cam_i2c_scl_pbb1,       VGP1,       I2C3,       RSVD3,        RSVD4,       0x3290, Y,   N,  N),
+	PINGROUP(cam_i2c_sda_pbb2,       VGP2,       I2C3,       RSVD3,        RSVD4,       0x3294, Y,   N,  N),
+	PINGROUP(pbb3,                   VGP3,       DISPLAYA,   DISPLAYB,     RSVD4,       0x3298, N,   N,  N),
+	PINGROUP(pbb4,                   VGP4,       DISPLAYA,   DISPLAYB,     RSVD4,       0x329c, N,   N,  N),
+	PINGROUP(pbb5,                   VGP5,       DISPLAYA,   DISPLAYB,     RSVD4,       0x32a0, N,   N,  N),
+	PINGROUP(pbb6,                   VGP6,       DISPLAYA,   DISPLAYB,     RSVD4,       0x32a4, N,   N,  N),
+	PINGROUP(pbb7,                   I2S4,       RSVD2,      RSVD3,        RSVD4,       0x32a8, N,   N,  N),
+	PINGROUP(pcc2,                   I2S4,       RSVD2,      RSVD3,        RSVD4,       0x32ac, N,   N,  N),
+	PINGROUP(jtag_rtck,              RTCK,       RSVD2,      RSVD3,        RSVD4,       0x32b0, N,   N,  N),
+	PINGROUP(pwr_i2c_scl_pz6,        I2CPWR,     RSVD2,      RSVD3,        RSVD4,       0x32b4, Y,   N,  N),
+	PINGROUP(pwr_i2c_sda_pz7,        I2CPWR,     RSVD2,      RSVD3,        RSVD4,       0x32b8, Y,   N,  N),
+	PINGROUP(kb_row0_pr0,            KBC,        RSVD2,      RSVD3,        RSVD4,       0x32bc, N,   N,  N),
+	PINGROUP(kb_row1_pr1,            KBC,        RSVD2,      RSVD3,        RSVD4,       0x32c0, N,   N,  N),
+	PINGROUP(kb_row2_pr2,            KBC,        RSVD2,      RSVD3,        RSVD4,       0x32c4, N,   N,  N),
+	PINGROUP(kb_row3_pr3,            KBC,        DISPLAYA,   RSVD3,        DISPLAYB,    0x32c8, N,   N,  N),
+	PINGROUP(kb_row4_pr4,            KBC,        DISPLAYA,   SPI2,         DISPLAYB,    0x32cc, N,   N,  N),
+	PINGROUP(kb_row5_pr5,            KBC,        DISPLAYA,   SPI2,         DISPLAYB,    0x32d0, N,   N,  N),
+	PINGROUP(kb_row6_pr6,            KBC,        DISPLAYA,   DISPLAYA_ALT, DISPLAYB,    0x32d4, N,   N,  N),
+	PINGROUP(kb_row7_pr7,            KBC,        RSVD2,      CLDVFS,       UARTA,       0x32d8, N,   N,  N),
+	PINGROUP(kb_row8_ps0,            KBC,        RSVD2,      CLDVFS,       UARTA,       0x32dc, N,   N,  N),
+	PINGROUP(kb_row9_ps1,            KBC,        RSVD2,      RSVD3,        UARTA,       0x32e0, N,   N,  N),
+	PINGROUP(kb_row10_ps2,           KBC,        RSVD2,      RSVD3,        UARTA,       0x32e4, N,   N,  N),
+	PINGROUP(kb_col0_pq0,            KBC,        USB,        SPI2,         EMC_DLL,     0x32fc, N,   N,  N),
+	PINGROUP(kb_col1_pq1,            KBC,        RSVD2,      SPI2,         EMC_DLL,     0x3300, N,   N,  N),
+	PINGROUP(kb_col2_pq2,            KBC,        RSVD2,      SPI2,         RSVD4,       0x3304, N,   N,  N),
+	PINGROUP(kb_col3_pq3,            KBC,        DISPLAYA,   PWM2,         UARTA,       0x3308, N,   N,  N),
+	PINGROUP(kb_col4_pq4,            KBC,        OWR,        SDMMC3,       UARTA,       0x330c, N,   N,  N),
+	PINGROUP(kb_col5_pq5,            KBC,        RSVD2,      SDMMC1,       RSVD4,       0x3310, N,   N,  N),
+	PINGROUP(kb_col6_pq6,            KBC,        RSVD2,      SPI2,         RSVD4,       0x3314, N,   N,  N),
+	PINGROUP(kb_col7_pq7,            KBC,        RSVD2,      SPI2,         RSVD4,       0x3318, N,   N,  N),
+	PINGROUP(clk_32k_out_pa0,        BLINK,      SOC,        RSVD3,        RSVD4,       0x331c, N,   N,  N),
+	PINGROUP(sys_clk_req_pz5,        SYSCLK,     RSVD2,      RSVD3,        RSVD4,       0x3320, N,   N,  N),
+	PINGROUP(core_pwr_req,           PWRON,      RSVD2,      RSVD3,        RSVD4,       0x3324, N,   N,  N),
+	PINGROUP(cpu_pwr_req,            CPU,        RSVD2,      RSVD3,        RSVD4,       0x3328, N,   N,  N),
+	PINGROUP(pwr_int_n,              PMI,        RSVD2,      RSVD3,        RSVD4,       0x332c, N,   N,  N),
+	PINGROUP(clk_32k_in,             CLK,        RSVD2,      RSVD3,        RSVD4,       0x3330, N,   N,  N),
+	PINGROUP(owr,                    OWR,        RSVD2,      RSVD3,        RSVD4,       0x3334, N,   N,  Y),
+	PINGROUP(dap1_fs_pn0,            I2S0,       HDA,        GMI,          RSVD4,       0x3338, N,   N,  N),
+	PINGROUP(dap1_din_pn1,           I2S0,       HDA,        GMI,          RSVD4,       0x333c, N,   N,  N),
+	PINGROUP(dap1_dout_pn2,          I2S0,       HDA,        GMI,          RSVD4,       0x3340, N,   N,  N),
+	PINGROUP(dap1_sclk_pn3,          I2S0,       HDA,        GMI,          RSVD4,       0x3344, N,   N,  N),
+	PINGROUP(clk1_req_pee2,          DAP,        DAP1,       RSVD3,        RSVD4,       0x3348, N,   N,  N),
+	PINGROUP(clk1_out_pw4,           EXTPERIPH1, DAP2,       RSVD3,        RSVD4,       0x334c, N,   N,  N),
+	PINGROUP(spdif_in_pk6,           SPDIF,      USB,        RSVD3,        RSVD4,       0x3350, N,   N,  N),
+	PINGROUP(spdif_out_pk5,          SPDIF,      RSVD2,      RSVD3,        RSVD4,       0x3354, N,   N,  N),
+	PINGROUP(dap2_fs_pa2,            I2S1,       HDA,        RSVD3,        RSVD4,       0x3358, N,   N,  N),
+	PINGROUP(dap2_din_pa4,           I2S1,       HDA,        RSVD3,        RSVD4,       0x335c, N,   N,  N),
+	PINGROUP(dap2_dout_pa5,          I2S1,       HDA,        RSVD3,        RSVD4,       0x3360, N,   N,  N),
+	PINGROUP(dap2_sclk_pa3,          I2S1,       HDA,        RSVD3,        RSVD4,       0x3364, N,   N,  N),
+	PINGROUP(dvfs_pwm_px0,           SPI6,       CLDVFS,     RSVD3,        RSVD4,       0x3368, N,   N,  N),
+	PINGROUP(gpio_x1_aud_px1,        SPI6,       RSVD2,      RSVD3,        RSVD4,       0x336c, N,   N,  N),
+	PINGROUP(gpio_x3_aud_px3,        SPI6,       SPI1,       RSVD3,        RSVD4,       0x3370, N,   N,  N),
+	PINGROUP(dvfs_clk_px2,           SPI6,       CLDVFS,     RSVD3,        RSVD4,       0x3374, N,   N,  N),
+	PINGROUP(gpio_x4_aud_px4,        RSVD1,      SPI1,       SPI2,         DAP2,        0x3378, N,   N,  N),
+	PINGROUP(gpio_x5_aud_px5,        RSVD1,      SPI1,       SPI2,         RSVD4,       0x337c, N,   N,  N),
+	PINGROUP(gpio_x6_aud_px6,        SPI6,       SPI1,       SPI2,         RSVD4,       0x3380, N,   N,  N),
+	PINGROUP(gpio_x7_aud_px7,        RSVD1,      SPI1,       SPI2,         RSVD4,       0x3384, N,   N,  N),
+	PINGROUP(sdmmc3_clk_pa6,         SDMMC3,     RSVD2,      RSVD3,        SPI3,        0x3390, N,   N,  N),
+	PINGROUP(sdmmc3_cmd_pa7,         SDMMC3,     PWM3,       UARTA,        SPI3,        0x3394, N,   N,  N),
+	PINGROUP(sdmmc3_dat0_pb7,        SDMMC3,     RSVD2,      RSVD3,        SPI3,        0x3398, N,   N,  N),
+	PINGROUP(sdmmc3_dat1_pb6,        SDMMC3,     PWM2,       UARTA,        SPI3,        0x339c, N,   N,  N),
+	PINGROUP(sdmmc3_dat2_pb5,        SDMMC3,     PWM1,       DISPLAYA,     SPI3,        0x33a0, N,   N,  N),
+	PINGROUP(sdmmc3_dat3_pb4,        SDMMC3,     PWM0,       DISPLAYB,     SPI3,        0x33a4, N,   N,  N),
+	PINGROUP(hdmi_cec_pee3,          CEC,        SDMMC3,     RSVD3,        SOC,         0x33e0, Y,   N,  N),
+	PINGROUP(sdmmc1_wp_n_pv3,        SDMMC1,     CLK12,      SPI4,         UARTA,       0x33e4, N,   N,  N),
+	PINGROUP(sdmmc3_cd_n_pv2,        SDMMC3,     OWR,        RSVD3,        RSVD4,       0x33e8, N,   N,  N),
+	PINGROUP(gpio_w2_aud_pw2,        SPI6,       RSVD2,      SPI2,         I2C1,        0x33ec, N,   N,  N),
+	PINGROUP(gpio_w3_aud_pw3,        SPI6,       SPI1,       SPI2,         I2C1,        0x33f0, N,   N,  N),
+	PINGROUP(usb_vbus_en0_pn4,       USB,        RSVD2,      RSVD3,        RSVD4,       0x33f4, Y,   N,  N),
+	PINGROUP(usb_vbus_en1_pn5,       USB,        RSVD2,      RSVD3,        RSVD4,       0x33f8, Y,   N,  N),
+	PINGROUP(sdmmc3_clk_lb_in_pee5,  SDMMC3,     RSVD2,      RSVD3,        RSVD4,       0x33fc, N,   N,  N),
+	PINGROUP(sdmmc3_clk_lb_out_pee4, SDMMC3,     RSVD2,      RSVD3,        RSVD4,       0x3400, N,   N,  N),
+	PINGROUP(gmi_clk_lb,             SDMMC2,     NAND,       GMI,          RSVD4,       0x3404, N,   N,  N),
+	PINGROUP(reset_out_n,            RSVD1,      RSVD2,      RSVD3,        RESET_OUT_N, 0x3408, N,   N,  N),
 
 	/* pg_name, r, hsm_b, schmitt_b, lpmd_b, drvdn_b, drvdn_w, drvup_b, drvup_w, slwr_b, slwr_w, slwf_b, slwf_w, drvtype */
 	DRV_PINGROUP(ao1,         0x868,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
diff --git a/drivers/pinctrl/pinctrl-tegra124.c b/drivers/pinctrl/pinctrl-tegra124.c
index 73773706755..e80797e2001 100644
--- a/drivers/pinctrl/pinctrl-tegra124.c
+++ b/drivers/pinctrl/pinctrl-tegra124.c
@@ -1677,10 +1677,12 @@ static struct tegra_function tegra124_functions[] = {
 #define DRV_PINGROUP_REG_A		0x868	/* bank 0 */
 #define PINGROUP_REG_A			0x3000	/* bank 1 */
 
-#define PINGROUP_REG_Y(r)		((r) - PINGROUP_REG_A)
-#define PINGROUP_REG_N(r)		-1
+#define PINGROUP_REG(r)			((r) - PINGROUP_REG_A)
 
-#define PINGROUP(pg_name, f0, f1, f2, f3, f_safe, r, od, ior, rcv_sel)	\
+#define PINGROUP_BIT_Y(b)		(b)
+#define PINGROUP_BIT_N(b)		(-1)
+
+#define PINGROUP(pg_name, f0, f1, f2, f3, r, od, ior, rcv_sel)		\
 	{								\
 		.name = #pg_name,					\
 		.pins = pg_name##_pins,					\
@@ -1691,38 +1693,24 @@ static struct tegra_function tegra124_functions[] = {
 			TEGRA_MUX_##f2,					\
 			TEGRA_MUX_##f3,					\
 		},							\
-		.func_safe = TEGRA_MUX_##f_safe,			\
-		.mux_reg = PINGROUP_REG_Y(r),				\
+		.mux_reg = PINGROUP_REG(r),				\
 		.mux_bank = 1,						\
 		.mux_bit = 0,						\
-		.pupd_reg = PINGROUP_REG_Y(r),				\
+		.pupd_reg = PINGROUP_REG(r),				\
 		.pupd_bank = 1,						\
 		.pupd_bit = 2,						\
-		.tri_reg = PINGROUP_REG_Y(r),				\
+		.tri_reg = PINGROUP_REG(r),				\
 		.tri_bank = 1,						\
 		.tri_bit = 4,						\
-		.einput_reg = PINGROUP_REG_Y(r),			\
-		.einput_bank = 1,					\
-		.einput_bit = 5,					\
-		.odrain_reg = PINGROUP_REG_##od(r),			\
-		.odrain_bank = 1,					\
-		.odrain_bit = 6,					\
-		.lock_reg = PINGROUP_REG_Y(r),				\
-		.lock_bank = 1,						\
-		.lock_bit = 7,						\
-		.ioreset_reg = PINGROUP_REG_##ior(r),			\
-		.ioreset_bank = 1,					\
-		.ioreset_bit = 8,					\
-		.rcv_sel_reg = PINGROUP_REG_##rcv_sel(r),		\
-		.rcv_sel_bank = 1,					\
-		.rcv_sel_bit = 9,					\
+		.einput_bit = PINGROUP_BIT_Y(5),			\
+		.odrain_bit = PINGROUP_BIT_##od(6),			\
+		.lock_bit = PINGROUP_BIT_Y(7),				\
+		.ioreset_bit = PINGROUP_BIT_##ior(8),			\
+		.rcv_sel_bit = PINGROUP_BIT_##rcv_sel(9),		\
 		.drv_reg = -1,						\
-		.drvtype_reg = -1,					\
 	}
 
-#define DRV_PINGROUP_REG_Y(r)		((r) - DRV_PINGROUP_REG_A)
-#define DRV_PINGROUP_REG_N(r)		-1
-
+#define DRV_PINGROUP_REG(r)		((r) - DRV_PINGROUP_REG_A)
 
 #define DRV_PINGROUP(pg_name, r, hsm_b, schmitt_b, lpmd_b,		\
 		     drvdn_b, drvdn_w, drvup_b, drvup_w,		\
@@ -1735,12 +1723,12 @@ static struct tegra_function tegra124_functions[] = {
 		.mux_reg = -1,						\
 		.pupd_reg = -1,						\
 		.tri_reg = -1,						\
-		.einput_reg = -1,					\
-		.odrain_reg = -1,					\
-		.lock_reg = -1,						\
-		.ioreset_reg = -1,					\
-		.rcv_sel_reg = -1,					\
-		.drv_reg = DRV_PINGROUP_REG_Y(r),			\
+		.einput_bit = -1,					\
+		.odrain_bit = -1,					\
+		.lock_bit = -1,						\
+		.ioreset_bit = -1,					\
+		.rcv_sel_bit = -1,					\
+		.drv_reg = DRV_PINGROUP_REG(r),				\
 		.drv_bank = 0,						\
 		.hsm_bit = hsm_b,					\
 		.schmitt_bit = schmitt_b,				\
@@ -1753,246 +1741,244 @@ static struct tegra_function tegra124_functions[] = {
 		.slwr_width = slwr_w,					\
 		.slwf_bit = slwf_b,					\
 		.slwf_width = slwf_w,					\
-		.drvtype_reg = DRV_PINGROUP_REG_##drvtype(r),		\
-		.drvtype_bank = 0,					\
-		.drvtype_bit = 6,					\
+		.drvtype_bit = PINGROUP_BIT_##drvtype(6),		\
 	}
 
 static const struct tegra_pingroup tegra124_groups[] = {
-       /*       pg_name,                f0,         f1,         f2,           f3,          safe,       r,      od, ior, rcv_sel */
-       PINGROUP(ulpi_data0_po1,         SPI3,       HSI,        UARTA,        ULPI,        SPI3,       0x3000,  N,  N,  N),
-       PINGROUP(ulpi_data1_po2,         SPI3,       HSI,        UARTA,        ULPI,        SPI3,       0x3004,  N,  N,  N),
-       PINGROUP(ulpi_data2_po3,         SPI3,       HSI,        UARTA,        ULPI,        SPI3,       0x3008,  N,  N,  N),
-       PINGROUP(ulpi_data3_po4,         SPI3,       HSI,        UARTA,        ULPI,        SPI3,       0x300c,  N,  N,  N),
-       PINGROUP(ulpi_data4_po5,         SPI2,       HSI,        UARTA,        ULPI,        SPI2,       0x3010,  N,  N,  N),
-       PINGROUP(ulpi_data5_po6,         SPI2,       HSI,        UARTA,        ULPI,        SPI2,       0x3014,  N,  N,  N),
-       PINGROUP(ulpi_data6_po7,         SPI2,       HSI,        UARTA,        ULPI,        SPI2,       0x3018,  N,  N,  N),
-       PINGROUP(ulpi_data7_po0,         SPI2,       HSI,        UARTA,        ULPI,        SPI2,       0x301c,  N,  N,  N),
-       PINGROUP(ulpi_clk_py0,           SPI1,       SPI5,       UARTD,        ULPI,        SPI1,       0x3020,  N,  N,  N),
-       PINGROUP(ulpi_dir_py1,           SPI1,       SPI5,       UARTD,        ULPI,        SPI1,       0x3024,  N,  N,  N),
-       PINGROUP(ulpi_nxt_py2,           SPI1,       SPI5,       UARTD,        ULPI,        SPI1,       0x3028,  N,  N,  N),
-       PINGROUP(ulpi_stp_py3,           SPI1,       SPI5,       UARTD,        ULPI,        SPI1,       0x302c,  N,  N,  N),
-       PINGROUP(dap3_fs_pp0,            I2S2,       SPI5,       DISPLAYA,     DISPLAYB,    I2S2,       0x3030,  N,  N,  N),
-       PINGROUP(dap3_din_pp1,           I2S2,       SPI5,       DISPLAYA,     DISPLAYB,    I2S2,       0x3034,  N,  N,  N),
-       PINGROUP(dap3_dout_pp2,          I2S2,       SPI5,       DISPLAYA,     RSVD4,       I2S2,       0x3038,  N,  N,  N),
-       PINGROUP(dap3_sclk_pp3,          I2S2,       SPI5,       RSVD3,        DISPLAYB,    I2S2,       0x303c,  N,  N,  N),
-       PINGROUP(pv0,                    RSVD1,      RSVD2,      RSVD3,        RSVD4,       RSVD1,      0x3040,  N,  N,  N),
-       PINGROUP(pv1,                    RSVD1,      RSVD2,      RSVD3,        RSVD4,       RSVD1,      0x3044,  N,  N,  N),
-       PINGROUP(sdmmc1_clk_pz0,         SDMMC1,     CLK12,      RSVD3,        RSVD4,       RSVD3,      0x3048,  N,  N,  N),
-       PINGROUP(sdmmc1_cmd_pz1,         SDMMC1,     SPDIF,      SPI4,         UARTA,       SDMMC1,     0x304c,  N,  N,  N),
-       PINGROUP(sdmmc1_dat3_py4,        SDMMC1,     SPDIF,      SPI4,         UARTA,       SDMMC1,     0x3050,  N,  N,  N),
-       PINGROUP(sdmmc1_dat2_py5,        SDMMC1,     PWM0,       SPI4,         UARTA,       SDMMC1,     0x3054,  N,  N,  N),
-       PINGROUP(sdmmc1_dat1_py6,        SDMMC1,     PWM1,       SPI4,         UARTA,       SDMMC1,     0x3058,  N,  N,  N),
-       PINGROUP(sdmmc1_dat0_py7,        SDMMC1,     RSVD2,      SPI4,         UARTA,       SDMMC1,     0x305c,  N,  N,  N),
-       PINGROUP(clk2_out_pw5,           EXTPERIPH2, RSVD2,      RSVD3,        RSVD4,       EXTPERIPH2, 0x3068,  N,  N,  N),
-       PINGROUP(clk2_req_pcc5,          DAP,        RSVD2,      RSVD3,        RSVD4,       DAP,        0x306c,  N,  N,  N),
-       PINGROUP(hdmi_int_pn7,           RSVD1,      RSVD2,      RSVD3,        RSVD4,       RSVD1,      0x3110,  N,  N,  Y),
-       PINGROUP(ddc_scl_pv4,            I2C4,       RSVD2,      RSVD3,        RSVD4,       I2C4,       0x3114,  N,  N,  Y),
-       PINGROUP(ddc_sda_pv5,            I2C4,       RSVD2,      RSVD3,        RSVD4,       I2C4,       0x3118,  N,  N,  Y),
-       PINGROUP(uart2_rxd_pc3,          IRDA,       SPDIF,      UARTA,        SPI4,        IRDA,       0x3164,  N,  N,  N),
-       PINGROUP(uart2_txd_pc2,          IRDA,       SPDIF,      UARTA,        SPI4,        IRDA,       0x3168,  N,  N,  N),
-       PINGROUP(uart2_rts_n_pj6,        UARTA,      UARTB,      GMI,          SPI4,        UARTA,      0x316c,  N,  N,  N),
-       PINGROUP(uart2_cts_n_pj5,        UARTA,      UARTB,      GMI,          SPI4,        UARTA,      0x3170,  N,  N,  N),
-       PINGROUP(uart3_txd_pw6,          UARTC,      RSVD2,      GMI,          SPI4,        UARTC,      0x3174,  N,  N,  N),
-       PINGROUP(uart3_rxd_pw7,          UARTC,      RSVD2,      GMI,          SPI4,        UARTC,      0x3178,  N,  N,  N),
-       PINGROUP(uart3_cts_n_pa1,        UARTC,      SDMMC1,     DTV,          GMI,         UARTC,      0x317c,  N,  N,  N),
-       PINGROUP(uart3_rts_n_pc0,        UARTC,      PWM0,       DTV,          GMI,         UARTC,      0x3180,  N,  N,  N),
-       PINGROUP(pu0,                    OWR,        UARTA,      GMI,          RSVD4,       RSVD4,      0x3184,  N,  N,  N),
-       PINGROUP(pu1,                    RSVD1,      UARTA,      GMI,          RSVD4,       RSVD4,      0x3188,  N,  N,  N),
-       PINGROUP(pu2,                    RSVD1,      UARTA,      GMI,          RSVD4,       RSVD4,      0x318c,  N,  N,  N),
-       PINGROUP(pu3,                    PWM0,       UARTA,      GMI,          DISPLAYB,    PWM0,       0x3190,  N,  N,  N),
-       PINGROUP(pu4,                    PWM1,       UARTA,      GMI,          DISPLAYB,    PWM1,       0x3194,  N,  N,  N),
-       PINGROUP(pu5,                    PWM2,       UARTA,      GMI,          DISPLAYB,    PWM2,       0x3198,  N,  N,  N),
-       PINGROUP(pu6,                    PWM3,       UARTA,      RSVD3,        GMI,         RSVD3,      0x319c,  N,  N,  N),
-       PINGROUP(gen1_i2c_sda_pc5,       I2C1,       RSVD2,      RSVD3,        RSVD4,       I2C1,       0x31a0,  Y,  N,  N),
-       PINGROUP(gen1_i2c_scl_pc4,       I2C1,       RSVD2,      RSVD3,        RSVD4,       I2C1,       0x31a4,  Y,  N,  N),
-       PINGROUP(dap4_fs_pp4,            I2S3,       GMI,        DTV,          RSVD4,       I2S3,       0x31a8,  N,  N,  N),
-       PINGROUP(dap4_din_pp5,           I2S3,       GMI,        RSVD3,        RSVD4,       I2S3,       0x31ac,  N,  N,  N),
-       PINGROUP(dap4_dout_pp6,          I2S3,       GMI,        DTV,          RSVD4,       I2S3,       0x31b0,  N,  N,  N),
-       PINGROUP(dap4_sclk_pp7,          I2S3,       GMI,        RSVD3,        RSVD4,       I2S3,       0x31b4,  N,  N,  N),
-       PINGROUP(clk3_out_pee0,          EXTPERIPH3, RSVD2,      RSVD3,        RSVD4,       RSVD3,      0x31b8,  N,  N,  N),
-       PINGROUP(clk3_req_pee1,          DEV3,       RSVD2,      RSVD3,        RSVD4,       RSVD4,      0x31bc,  N,  N,  N),
-       PINGROUP(pc7,                    RSVD1,      RSVD2,      GMI,          GMI_ALT,     RSVD1,      0x31c0,  N,  N,  N),
-       PINGROUP(pi5,                    SDMMC2,     RSVD2,      GMI,          RSVD4,       GMI,        0x31c4,  N,  N,  N),
-       PINGROUP(pi7,                    RSVD1,      TRACE,      GMI,          DTV,         RSVD1,      0x31c8,  N,  N,  N),
-       PINGROUP(pk0,                    RSVD1,      SDMMC3,     GMI,          SOC,         RSVD1,      0x31cc,  N,  N,  N),
-       PINGROUP(pk1,                    SDMMC2,     TRACE,      GMI,          RSVD4,       GMI,        0x31d0,  N,  N,  N),
-       PINGROUP(pj0,                    RSVD1,      RSVD2,      GMI,          USB,         RSVD1,      0x31d4,  N,  N,  N),
-       PINGROUP(pj2,                    RSVD1,      RSVD2,      GMI,          SOC,         RSVD1,      0x31d8,  N,  N,  N),
-       PINGROUP(pk3,                    SDMMC2,     TRACE,      GMI,          CCLA,        GMI,        0x31dc,  N,  N,  N),
-       PINGROUP(pk4,                    SDMMC2,     RSVD2,      GMI,          GMI_ALT,     GMI,        0x31e0,  N,  N,  N),
-       PINGROUP(pk2,                    RSVD1,      RSVD2,      GMI,          RSVD4,       RSVD4,      0x31e4,  N,  N,  N),
-       PINGROUP(pi3,                    RSVD1,      RSVD2,      GMI,          SPI4,        RSVD1,      0x31e8,  N,  N,  N),
-       PINGROUP(pi6,                    RSVD1,      RSVD2,      GMI,          SDMMC2,      RSVD1,      0x31ec,  N,  N,  N),
-       PINGROUP(pg0,                    RSVD1,      RSVD2,      GMI,          RSVD4,       RSVD4,      0x31f0,  N,  N,  N),
-       PINGROUP(pg1,                    RSVD1,      RSVD2,      GMI,          RSVD4,       RSVD4,      0x31f4,  N,  N,  N),
-       PINGROUP(pg2,                    RSVD1,      TRACE,      GMI,          RSVD4,       RSVD4,      0x31f8,  N,  N,  N),
-       PINGROUP(pg3,                    RSVD1,      TRACE,      GMI,          RSVD4,       RSVD4,      0x31fc,  N,  N,  N),
-       PINGROUP(pg4,                    RSVD1,      TMDS,       GMI,          SPI4,        RSVD1,      0x3200,  N,  N,  N),
-       PINGROUP(pg5,                    RSVD1,      RSVD2,      GMI,          SPI4,        RSVD1,      0x3204,  N,  N,  N),
-       PINGROUP(pg6,                    RSVD1,      RSVD2,      GMI,          SPI4,        RSVD1,      0x3208,  N,  N,  N),
-       PINGROUP(pg7,                    RSVD1,      RSVD2,      GMI,          SPI4,        RSVD1,      0x320c,  N,  N,  N),
-       PINGROUP(ph0,                    PWM0,       TRACE,      GMI,          DTV,         GMI,        0x3210,  N,  N,  N),
-       PINGROUP(ph1,                    PWM1,       TMDS,       GMI,          DISPLAYA,    GMI,        0x3214,  N,  N,  N),
-       PINGROUP(ph2,                    PWM2,       TMDS,       GMI,          CLDVFS,      GMI,        0x3218,  N,  N,  N),
-       PINGROUP(ph3,                    PWM3,       SPI4,       GMI,          CLDVFS,      GMI,        0x321c,  N,  N,  N),
-       PINGROUP(ph4,                    SDMMC2,     RSVD2,      GMI,          RSVD4,       GMI,        0x3220,  N,  N,  N),
-       PINGROUP(ph5,                    SDMMC2,     RSVD2,      GMI,          RSVD4,       GMI,        0x3224,  N,  N,  N),
-       PINGROUP(ph6,                    SDMMC2,     TRACE,      GMI,          DTV,         GMI,        0x3228,  N,  N,  N),
-       PINGROUP(ph7,                    SDMMC2,     TRACE,      GMI,          DTV,         GMI,        0x322c,  N,  N,  N),
-       PINGROUP(pj7,                    UARTD,      RSVD2,      GMI,          GMI_ALT,     RSVD2,      0x3230,  N,  N,  N),
-       PINGROUP(pb0,                    UARTD,      RSVD2,      GMI,          RSVD4,       RSVD2,      0x3234,  N,  N,  N),
-       PINGROUP(pb1,                    UARTD,      RSVD2,      GMI,          RSVD4,       RSVD2,      0x3238,  N,  N,  N),
-       PINGROUP(pk7,                    UARTD,      RSVD2,      GMI,          RSVD4,       RSVD2,      0x323c,  N,  N,  N),
-       PINGROUP(pi0,                    RSVD1,      RSVD2,      GMI,          RSVD4,       RSVD4,      0x3240,  N,  N,  N),
-       PINGROUP(pi1,                    RSVD1,      RSVD2,      GMI,          RSVD4,       RSVD1,      0x3244,  N,  N,  N),
-       PINGROUP(pi2,                    SDMMC2,     TRACE,      GMI,          RSVD4,       GMI,        0x3248,  N,  N,  N),
-       PINGROUP(pi4,                    SPI4,       TRACE,      GMI,          DISPLAYA,    GMI,        0x324c,  N,  N,  N),
-       PINGROUP(gen2_i2c_scl_pt5,       I2C2,       RSVD2,      GMI,          RSVD4,       RSVD2,      0x3250,  Y,  N,  N),
-       PINGROUP(gen2_i2c_sda_pt6,       I2C2,       RSVD2,      GMI,          RSVD4,       RSVD2,      0x3254,  Y,  N,  N),
-       PINGROUP(sdmmc4_clk_pcc4,        SDMMC4,     RSVD2,      GMI,          RSVD4,       RSVD2,      0x3258,  N,  Y,  N),
-       PINGROUP(sdmmc4_cmd_pt7,         SDMMC4,     RSVD2,      GMI,          RSVD4,       RSVD2,      0x325c,  N,  Y,  N),
-       PINGROUP(sdmmc4_dat0_paa0,       SDMMC4,     SPI3,       GMI,          RSVD4,       SDMMC4,     0x3260,  N,  Y,  N),
-       PINGROUP(sdmmc4_dat1_paa1,       SDMMC4,     SPI3,       GMI,          RSVD4,       SDMMC4,     0x3264,  N,  Y,  N),
-       PINGROUP(sdmmc4_dat2_paa2,       SDMMC4,     SPI3,       GMI,          RSVD4,       SDMMC4,     0x3268,  N,  Y,  N),
-       PINGROUP(sdmmc4_dat3_paa3,       SDMMC4,     SPI3,       GMI,          RSVD4,       SDMMC4,     0x326c,  N,  Y,  N),
-       PINGROUP(sdmmc4_dat4_paa4,       SDMMC4,     SPI3,       GMI,          RSVD4,       SDMMC4,     0x3270,  N,  Y,  N),
-       PINGROUP(sdmmc4_dat5_paa5,       SDMMC4,     SPI3,       RSVD3,        RSVD4,       SDMMC4,     0x3274,  N,  Y,  N),
-       PINGROUP(sdmmc4_dat6_paa6,       SDMMC4,     SPI3,       GMI,          RSVD4,       SDMMC4,     0x3278,  N,  Y,  N),
-       PINGROUP(sdmmc4_dat7_paa7,       SDMMC4,     RSVD2,      GMI,          RSVD4,       SDMMC4,     0x327c,  N,  Y,  N),
-       PINGROUP(cam_mclk_pcc0,          VI,         VI_ALT1,    VI_ALT3,      SDMMC2,      VI,         0x3284,  N,  N,  N),
-       PINGROUP(pcc1,                   I2S4,       RSVD2,      RSVD3,        SDMMC2,      I2S4,       0x3288,  N,  N,  N),
-       PINGROUP(pbb0,                   VGP6,       VIMCLK2,    SDMMC2,       VIMCLK2_ALT, VGP6,       0x328c,  N,  N,  N),
-       PINGROUP(cam_i2c_scl_pbb1,       VGP1,       I2C3,       RSVD3,        SDMMC2,      VGP1,       0x3290,  Y,  N,  N),
-       PINGROUP(cam_i2c_sda_pbb2,       VGP2,       I2C3,       RSVD3,        SDMMC2,      VGP2,       0x3294,  Y,  N,  N),
-       PINGROUP(pbb3,                   VGP3,       DISPLAYA,   DISPLAYB,     SDMMC2,      VGP3,       0x3298,  N,  N,  N),
-       PINGROUP(pbb4,                   VGP4,       DISPLAYA,   DISPLAYB,     SDMMC2,      VGP4,       0x329c,  N,  N,  N),
-       PINGROUP(pbb5,                   VGP5,       DISPLAYA,   RSVD3,        SDMMC2,      VGP5,       0x32a0,  N,  N,  N),
-       PINGROUP(pbb6,                   I2S4,       RSVD2,      DISPLAYB,     SDMMC2,      I2S4,       0x32a4,  N,  N,  N),
-       PINGROUP(pbb7,                   I2S4,       RSVD2,      RSVD3,        SDMMC2,      I2S4,       0x32a8,  N,  N,  N),
-       PINGROUP(pcc2,                   I2S4,       RSVD2,      SDMMC3,       SDMMC2,      I2S4,       0x32ac,  N,  N,  N),
-       PINGROUP(jtag_rtck,              RTCK,       RSVD2,      RSVD3,        RSVD4,       RTCK,       0x32b0,  N,  N,  N),
-       PINGROUP(pwr_i2c_scl_pz6,        I2CPWR,     RSVD2,      RSVD3,        RSVD4,       RSVD2,      0x32b4,  Y,  N,  N),
-       PINGROUP(pwr_i2c_sda_pz7,        I2CPWR,     RSVD2,      RSVD3,        RSVD4,       RSVD2,      0x32b8,  Y,  N,  N),
-       PINGROUP(kb_row0_pr0,            KBC,        RSVD2,      RSVD3,        RSVD4,       RSVD4,      0x32bc,  N,  N,  N),
-       PINGROUP(kb_row1_pr1,            KBC,        RSVD2,      RSVD3,        RSVD4,       RSVD4,      0x32c0,  N,  N,  N),
-       PINGROUP(kb_row2_pr2,            KBC,        RSVD2,      RSVD3,        RSVD4,       RSVD4,      0x32c4,  N,  N,  N),
-       PINGROUP(kb_row3_pr3,            KBC,        DISPLAYA,   SYS,          DISPLAYB,    KBC,        0x32c8,  N,  N,  N),
-       PINGROUP(kb_row4_pr4,            KBC,        DISPLAYA,   RSVD3,        DISPLAYB,    RSVD3,      0x32cc,  N,  N,  N),
-       PINGROUP(kb_row5_pr5,            KBC,        DISPLAYA,   RSVD3,        DISPLAYB,    RSVD3,      0x32d0,  N,  N,  N),
-       PINGROUP(kb_row6_pr6,            KBC,        DISPLAYA,   DISPLAYA_ALT, DISPLAYB,    KBC,        0x32d4,  N,  N,  N),
-       PINGROUP(kb_row7_pr7,            KBC,        RSVD2,      CLDVFS,       UARTA,       RSVD2,      0x32d8,  N,  N,  N),
-       PINGROUP(kb_row8_ps0,            KBC,        RSVD2,      CLDVFS,       UARTA,       RSVD2,      0x32dc,  N,  N,  N),
-       PINGROUP(kb_row9_ps1,            KBC,        RSVD2,      RSVD3,        UARTA,       KBC,        0x32e0,  N,  N,  N),
-       PINGROUP(kb_row10_ps2,           KBC,        RSVD2,      RSVD3,        UARTA,       KBC,        0x32e4,  N,  N,  N),
-       PINGROUP(kb_row11_ps3,           KBC,        RSVD2,      RSVD3,        IRDA,        RSVD3,      0x32e8,  N,  N,  N),
-       PINGROUP(kb_row12_ps4,           KBC,        RSVD2,      RSVD3,        IRDA,        RSVD3,      0x32ec,  N,  N,  N),
-       PINGROUP(kb_row13_ps5,           KBC,        RSVD2,      SPI2,         RSVD4,       RSVD4,      0x32f0,  N,  N,  N),
-       PINGROUP(kb_row14_ps6,           KBC,        RSVD2,      SPI2,         RSVD4,       RSVD4,      0x32f4,  N,  N,  N),
-       PINGROUP(kb_row15_ps7,           KBC,        SOC,        RSVD3,        RSVD4,       KBC,        0x32f8,  N,  N,  N),
-       PINGROUP(kb_col0_pq0,            KBC,        RSVD2,      SPI2,         RSVD4,       RSVD4,      0x32fc,  N,  N,  N),
-       PINGROUP(kb_col1_pq1,            KBC,        RSVD2,      SPI2,         RSVD4,       RSVD4,      0x3300,  N,  N,  N),
-       PINGROUP(kb_col2_pq2,            KBC,        RSVD2,      SPI2,         RSVD4,       RSVD4,      0x3304,  N,  N,  N),
-       PINGROUP(kb_col3_pq3,            KBC,        DISPLAYA,   PWM2,         UARTA,       KBC,        0x3308,  N,  N,  N),
-       PINGROUP(kb_col4_pq4,            KBC,        OWR,        SDMMC3,       UARTA,       KBC,        0x330c,  N,  N,  N),
-       PINGROUP(kb_col5_pq5,            KBC,        RSVD2,      SDMMC3,       RSVD4,       RSVD4,      0x3310,  N,  N,  N),
-       PINGROUP(kb_col6_pq6,            KBC,        RSVD2,      SPI2,         UARTD,       RSVD2,      0x3314,  N,  N,  N),
-       PINGROUP(kb_col7_pq7,            KBC,        RSVD2,      SPI2,         UARTD,       RSVD2,      0x3318,  N,  N,  N),
-       PINGROUP(clk_32k_out_pa0,        BLINK,      SOC,        RSVD3,        RSVD4,       RSVD3,      0x331c,  N,  N,  N),
-       PINGROUP(core_pwr_req,           PWRON,      RSVD2,      RSVD3,        RSVD4,       RSVD2,      0x3324,  N,  N,  N),
-       PINGROUP(cpu_pwr_req,            CPU,        RSVD2,      RSVD3,        RSVD4,       RSVD2,      0x3328,  N,  N,  N),
-       PINGROUP(pwr_int_n,              PMI,        RSVD2,      RSVD3,        RSVD4,       RSVD2,      0x332c,  N,  N,  N),
-       PINGROUP(clk_32k_in,             CLK,        RSVD2,      RSVD3,        RSVD4,       RSVD2,      0x3330,  N,  N,  N),
-       PINGROUP(owr,                    OWR,        RSVD2,      RSVD3,        RSVD4,       RSVD2,      0x3334,  N,  N,  Y),
-       PINGROUP(dap1_fs_pn0,            I2S0,       HDA,        GMI,          RSVD4,       RSVD4,      0x3338,  N,  N,  N),
-       PINGROUP(dap1_din_pn1,           I2S0,       HDA,        GMI,          RSVD4,       RSVD4,      0x333c,  N,  N,  N),
-       PINGROUP(dap1_dout_pn2,          I2S0,       HDA,        GMI,          SATA,        I2S0,       0x3340,  N,  N,  N),
-       PINGROUP(dap1_sclk_pn3,          I2S0,       HDA,        GMI,          RSVD4,       I2S0,       0x3344,  N,  N,  N),
-       PINGROUP(dap_mclk1_req_pee2,     DAP,        DAP1,       SATA,         RSVD4,       DAP,        0x3348,  N,  N,  N),
-       PINGROUP(dap_mclk1_pw4,          EXTPERIPH1, DAP2,       RSVD3,        RSVD4,       RSVD3,      0x334c,  N,  N,  N),
-       PINGROUP(spdif_in_pk6,           SPDIF,      RSVD2,      RSVD3,        I2C3,        RSVD3,      0x3350,  N,  N,  N),
-       PINGROUP(spdif_out_pk5,          SPDIF,      RSVD2,      RSVD3,        I2C3,        RSVD3,      0x3354,  N,  N,  N),
-       PINGROUP(dap2_fs_pa2,            I2S1,       HDA,        GMI,          RSVD4,       I2S1,       0x3358,  N,  N,  N),
-       PINGROUP(dap2_din_pa4,           I2S1,       HDA,        GMI,          RSVD4,       I2S1,       0x335c,  N,  N,  N),
-       PINGROUP(dap2_dout_pa5,          I2S1,       HDA,        GMI,          RSVD4,       I2S1,       0x3360,  N,  N,  N),
-       PINGROUP(dap2_sclk_pa3,          I2S1,       HDA,        GMI,          RSVD4,       I2S1,       0x3364,  N,  N,  N),
-       PINGROUP(dvfs_pwm_px0,           SPI6,       CLDVFS,     GMI,          RSVD4,       SPI6,       0x3368,  N,  N,  N),
-       PINGROUP(gpio_x1_aud_px1,        SPI6,       RSVD2,      GMI,          RSVD4,       SPI6,       0x336c,  N,  N,  N),
-       PINGROUP(gpio_x3_aud_px3,        SPI6,       SPI1,       GMI,          RSVD4,       SPI6,       0x3370,  N,  N,  N),
-       PINGROUP(dvfs_clk_px2,           SPI6,       CLDVFS,     GMI,          RSVD4,       SPI6,       0x3374,  N,  N,  N),
-       PINGROUP(gpio_x4_aud_px4,        GMI,        SPI1,       SPI2,         DAP2,        SPI1,       0x3378,  N,  N,  N),
-       PINGROUP(gpio_x5_aud_px5,        GMI,        SPI1,       SPI2,         RSVD4,       SPI1,       0x337c,  N,  N,  N),
-       PINGROUP(gpio_x6_aud_px6,        SPI6,       SPI1,       SPI2,         GMI,         SPI1,       0x3380,  N,  N,  N),
-       PINGROUP(gpio_x7_aud_px7,        RSVD1,      SPI1,       SPI2,         RSVD4,       SPI1,       0x3384,  N,  N,  N),
-       PINGROUP(sdmmc3_clk_pa6,         SDMMC3,     RSVD2,      RSVD3,        SPI3,        SDMMC3,     0x3390,  N,  N,  N),
-       PINGROUP(sdmmc3_cmd_pa7,         SDMMC3,     PWM3,       UARTA,        SPI3,        SDMMC3,     0x3394,  N,  N,  N),
-       PINGROUP(sdmmc3_dat0_pb7,        SDMMC3,     RSVD2,      RSVD3,        SPI3,        SDMMC3,     0x3398,  N,  N,  N),
-       PINGROUP(sdmmc3_dat1_pb6,        SDMMC3,     PWM2,       UARTA,        SPI3,        SDMMC3,     0x339c,  N,  N,  N),
-       PINGROUP(sdmmc3_dat2_pb5,        SDMMC3,     PWM1,       DISPLAYA,     SPI3,        SDMMC3,     0x33a0,  N,  N,  N),
-       PINGROUP(sdmmc3_dat3_pb4,        SDMMC3,     PWM0,       DISPLAYB,     SPI3,        SDMMC3,     0x33a4,  N,  N,  N),
-       PINGROUP(pex_l0_rst_n_pdd1,      PE0,        RSVD2,      RSVD3,        RSVD4,       PE0,        0x33bc,  N,  N,  N),
-       PINGROUP(pex_l0_clkreq_n_pdd2,   PE0,        RSVD2,      RSVD3,        RSVD4,       PE0,        0x33c0,  N,  N,  N),
-       PINGROUP(pex_wake_n_pdd3,        PE,         RSVD2,      RSVD3,        RSVD4,       PE,         0x33c4,  N,  N,  N),
-       PINGROUP(pex_l1_rst_n_pdd5,      PE1,        RSVD2,      RSVD3,        RSVD4,       PE1,        0x33cc,  N,  N,  N),
-       PINGROUP(pex_l1_clkreq_n_pdd6,   PE1,        RSVD2,      RSVD3,        RSVD4,       PE1,        0x33d0,  N,  N,  N),
-       PINGROUP(hdmi_cec_pee3,          CEC,        RSVD2,      RSVD3,        RSVD4,       CEC,        0x33e0,  Y,  N,  N),
-       PINGROUP(sdmmc1_wp_n_pv3,        SDMMC1,     CLK12,      SPI4,         UARTA,       SDMMC1,     0x33e4,  N,  N,  N),
-       PINGROUP(sdmmc3_cd_n_pv2,        SDMMC3,     OWR,        RSVD3,        RSVD4,       SDMMC3,     0x33e8,  N,  N,  N),
-       PINGROUP(gpio_w2_aud_pw2,        SPI6,       RSVD2,      SPI2,         I2C1,        RSVD2,      0x33ec,  N,  N,  N),
-       PINGROUP(gpio_w3_aud_pw3,        SPI6,       SPI1,       SPI2,         I2C1,        SPI1,       0x33f0,  N,  N,  N),
-       PINGROUP(usb_vbus_en0_pn4,       USB,        RSVD2,      RSVD3,        RSVD4,       USB,        0x33f4,  Y,  N,  N),
-       PINGROUP(usb_vbus_en1_pn5,       USB,        RSVD2,      RSVD3,        RSVD4,       USB,        0x33f8,  Y,  N,  N),
-       PINGROUP(sdmmc3_clk_lb_in_pee5,  SDMMC3,     RSVD2,      RSVD3,        RSVD4,       SDMMC3,     0x33fc,  N,  N,  N),
-       PINGROUP(sdmmc3_clk_lb_out_pee4, SDMMC3,     RSVD2,      RSVD3,        RSVD4,       SDMMC3,     0x3400,  N,  N,  N),
-       PINGROUP(gmi_clk_lb,             SDMMC2,     RSVD2,      GMI,          RSVD4,       SDMMC2,     0x3404,  N,  N,  N),
-       PINGROUP(reset_out_n,            RSVD1,      RSVD2,      RSVD3,        RESET_OUT_N, RSVD1,      0x3408,  N,  N,  N),
-       PINGROUP(kb_row16_pt0,           KBC,        RSVD2,      RSVD3,        UARTC,       KBC,        0x340c,  N,  N,  N),
-       PINGROUP(kb_row17_pt1,           KBC,        RSVD2,      RSVD3,        UARTC,       KBC,        0x3410,  N,  N,  N),
-       PINGROUP(usb_vbus_en2_pff1,      USB,        RSVD2,      RSVD3,        RSVD4,       USB,        0x3414,  Y,  N,  N),
-       PINGROUP(pff2,                   SATA,       RSVD2,      RSVD3,        RSVD4,       RSVD2,      0x3418,  Y,  N,  N),
-       PINGROUP(dp_hpd_pff0,            DP,         RSVD2,      RSVD3,        RSVD4,       DP,         0x3430,  N,  N,  N),
+	/*       pg_name,                f0,         f1,         f2,           f3,          r,      od, ior, rcv_sel */
+	PINGROUP(ulpi_data0_po1,         SPI3,       HSI,        UARTA,        ULPI,        0x3000, N,   N,  N),
+	PINGROUP(ulpi_data1_po2,         SPI3,       HSI,        UARTA,        ULPI,        0x3004, N,   N,  N),
+	PINGROUP(ulpi_data2_po3,         SPI3,       HSI,        UARTA,        ULPI,        0x3008, N,   N,  N),
+	PINGROUP(ulpi_data3_po4,         SPI3,       HSI,        UARTA,        ULPI,        0x300c, N,   N,  N),
+	PINGROUP(ulpi_data4_po5,         SPI2,       HSI,        UARTA,        ULPI,        0x3010, N,   N,  N),
+	PINGROUP(ulpi_data5_po6,         SPI2,       HSI,        UARTA,        ULPI,        0x3014, N,   N,  N),
+	PINGROUP(ulpi_data6_po7,         SPI2,       HSI,        UARTA,        ULPI,        0x3018, N,   N,  N),
+	PINGROUP(ulpi_data7_po0,         SPI2,       HSI,        UARTA,        ULPI,        0x301c, N,   N,  N),
+	PINGROUP(ulpi_clk_py0,           SPI1,       SPI5,       UARTD,        ULPI,        0x3020, N,   N,  N),
+	PINGROUP(ulpi_dir_py1,           SPI1,       SPI5,       UARTD,        ULPI,        0x3024, N,   N,  N),
+	PINGROUP(ulpi_nxt_py2,           SPI1,       SPI5,       UARTD,        ULPI,        0x3028, N,   N,  N),
+	PINGROUP(ulpi_stp_py3,           SPI1,       SPI5,       UARTD,        ULPI,        0x302c, N,   N,  N),
+	PINGROUP(dap3_fs_pp0,            I2S2,       SPI5,       DISPLAYA,     DISPLAYB,    0x3030, N,   N,  N),
+	PINGROUP(dap3_din_pp1,           I2S2,       SPI5,       DISPLAYA,     DISPLAYB,    0x3034, N,   N,  N),
+	PINGROUP(dap3_dout_pp2,          I2S2,       SPI5,       DISPLAYA,     RSVD4,       0x3038, N,   N,  N),
+	PINGROUP(dap3_sclk_pp3,          I2S2,       SPI5,       RSVD3,        DISPLAYB,    0x303c, N,   N,  N),
+	PINGROUP(pv0,                    RSVD1,      RSVD2,      RSVD3,        RSVD4,       0x3040, N,   N,  N),
+	PINGROUP(pv1,                    RSVD1,      RSVD2,      RSVD3,        RSVD4,       0x3044, N,   N,  N),
+	PINGROUP(sdmmc1_clk_pz0,         SDMMC1,     CLK12,      RSVD3,        RSVD4,       0x3048, N,   N,  N),
+	PINGROUP(sdmmc1_cmd_pz1,         SDMMC1,     SPDIF,      SPI4,         UARTA,       0x304c, N,   N,  N),
+	PINGROUP(sdmmc1_dat3_py4,        SDMMC1,     SPDIF,      SPI4,         UARTA,       0x3050, N,   N,  N),
+	PINGROUP(sdmmc1_dat2_py5,        SDMMC1,     PWM0,       SPI4,         UARTA,       0x3054, N,   N,  N),
+	PINGROUP(sdmmc1_dat1_py6,        SDMMC1,     PWM1,       SPI4,         UARTA,       0x3058, N,   N,  N),
+	PINGROUP(sdmmc1_dat0_py7,        SDMMC1,     RSVD2,      SPI4,         UARTA,       0x305c, N,   N,  N),
+	PINGROUP(clk2_out_pw5,           EXTPERIPH2, RSVD2,      RSVD3,        RSVD4,       0x3068, N,   N,  N),
+	PINGROUP(clk2_req_pcc5,          DAP,        RSVD2,      RSVD3,        RSVD4,       0x306c, N,   N,  N),
+	PINGROUP(hdmi_int_pn7,           RSVD1,      RSVD2,      RSVD3,        RSVD4,       0x3110, N,   N,  Y),
+	PINGROUP(ddc_scl_pv4,            I2C4,       RSVD2,      RSVD3,        RSVD4,       0x3114, N,   N,  Y),
+	PINGROUP(ddc_sda_pv5,            I2C4,       RSVD2,      RSVD3,        RSVD4,       0x3118, N,   N,  Y),
+	PINGROUP(uart2_rxd_pc3,          IRDA,       SPDIF,      UARTA,        SPI4,        0x3164, N,   N,  N),
+	PINGROUP(uart2_txd_pc2,          IRDA,       SPDIF,      UARTA,        SPI4,        0x3168, N,   N,  N),
+	PINGROUP(uart2_rts_n_pj6,        UARTA,      UARTB,      GMI,          SPI4,        0x316c, N,   N,  N),
+	PINGROUP(uart2_cts_n_pj5,        UARTA,      UARTB,      GMI,          SPI4,        0x3170, N,   N,  N),
+	PINGROUP(uart3_txd_pw6,          UARTC,      RSVD2,      GMI,          SPI4,        0x3174, N,   N,  N),
+	PINGROUP(uart3_rxd_pw7,          UARTC,      RSVD2,      GMI,          SPI4,        0x3178, N,   N,  N),
+	PINGROUP(uart3_cts_n_pa1,        UARTC,      SDMMC1,     DTV,          GMI,         0x317c, N,   N,  N),
+	PINGROUP(uart3_rts_n_pc0,        UARTC,      PWM0,       DTV,          GMI,         0x3180, N,   N,  N),
+	PINGROUP(pu0,                    OWR,        UARTA,      GMI,          RSVD4,       0x3184, N,   N,  N),
+	PINGROUP(pu1,                    RSVD1,      UARTA,      GMI,          RSVD4,       0x3188, N,   N,  N),
+	PINGROUP(pu2,                    RSVD1,      UARTA,      GMI,          RSVD4,       0x318c, N,   N,  N),
+	PINGROUP(pu3,                    PWM0,       UARTA,      GMI,          DISPLAYB,    0x3190, N,   N,  N),
+	PINGROUP(pu4,                    PWM1,       UARTA,      GMI,          DISPLAYB,    0x3194, N,   N,  N),
+	PINGROUP(pu5,                    PWM2,       UARTA,      GMI,          DISPLAYB,    0x3198, N,   N,  N),
+	PINGROUP(pu6,                    PWM3,       UARTA,      RSVD3,        GMI,         0x319c, N,   N,  N),
+	PINGROUP(gen1_i2c_sda_pc5,       I2C1,       RSVD2,      RSVD3,        RSVD4,       0x31a0, Y,   N,  N),
+	PINGROUP(gen1_i2c_scl_pc4,       I2C1,       RSVD2,      RSVD3,        RSVD4,       0x31a4, Y,   N,  N),
+	PINGROUP(dap4_fs_pp4,            I2S3,       GMI,        DTV,          RSVD4,       0x31a8, N,   N,  N),
+	PINGROUP(dap4_din_pp5,           I2S3,       GMI,        RSVD3,        RSVD4,       0x31ac, N,   N,  N),
+	PINGROUP(dap4_dout_pp6,          I2S3,       GMI,        DTV,          RSVD4,       0x31b0, N,   N,  N),
+	PINGROUP(dap4_sclk_pp7,          I2S3,       GMI,        RSVD3,        RSVD4,       0x31b4, N,   N,  N),
+	PINGROUP(clk3_out_pee0,          EXTPERIPH3, RSVD2,      RSVD3,        RSVD4,       0x31b8, N,   N,  N),
+	PINGROUP(clk3_req_pee1,          DEV3,       RSVD2,      RSVD3,        RSVD4,       0x31bc, N,   N,  N),
+	PINGROUP(pc7,                    RSVD1,      RSVD2,      GMI,          GMI_ALT,     0x31c0, N,   N,  N),
+	PINGROUP(pi5,                    SDMMC2,     RSVD2,      GMI,          RSVD4,       0x31c4, N,   N,  N),
+	PINGROUP(pi7,                    RSVD1,      TRACE,      GMI,          DTV,         0x31c8, N,   N,  N),
+	PINGROUP(pk0,                    RSVD1,      SDMMC3,     GMI,          SOC,         0x31cc, N,   N,  N),
+	PINGROUP(pk1,                    SDMMC2,     TRACE,      GMI,          RSVD4,       0x31d0, N,   N,  N),
+	PINGROUP(pj0,                    RSVD1,      RSVD2,      GMI,          USB,         0x31d4, N,   N,  N),
+	PINGROUP(pj2,                    RSVD1,      RSVD2,      GMI,          SOC,         0x31d8, N,   N,  N),
+	PINGROUP(pk3,                    SDMMC2,     TRACE,      GMI,          CCLA,        0x31dc, N,   N,  N),
+	PINGROUP(pk4,                    SDMMC2,     RSVD2,      GMI,          GMI_ALT,     0x31e0, N,   N,  N),
+	PINGROUP(pk2,                    RSVD1,      RSVD2,      GMI,          RSVD4,       0x31e4, N,   N,  N),
+	PINGROUP(pi3,                    RSVD1,      RSVD2,      GMI,          SPI4,        0x31e8, N,   N,  N),
+	PINGROUP(pi6,                    RSVD1,      RSVD2,      GMI,          SDMMC2,      0x31ec, N,   N,  N),
+	PINGROUP(pg0,                    RSVD1,      RSVD2,      GMI,          RSVD4,       0x31f0, N,   N,  N),
+	PINGROUP(pg1,                    RSVD1,      RSVD2,      GMI,          RSVD4,       0x31f4, N,   N,  N),
+	PINGROUP(pg2,                    RSVD1,      TRACE,      GMI,          RSVD4,       0x31f8, N,   N,  N),
+	PINGROUP(pg3,                    RSVD1,      TRACE,      GMI,          RSVD4,       0x31fc, N,   N,  N),
+	PINGROUP(pg4,                    RSVD1,      TMDS,       GMI,          SPI4,        0x3200, N,   N,  N),
+	PINGROUP(pg5,                    RSVD1,      RSVD2,      GMI,          SPI4,        0x3204, N,   N,  N),
+	PINGROUP(pg6,                    RSVD1,      RSVD2,      GMI,          SPI4,        0x3208, N,   N,  N),
+	PINGROUP(pg7,                    RSVD1,      RSVD2,      GMI,          SPI4,        0x320c, N,   N,  N),
+	PINGROUP(ph0,                    PWM0,       TRACE,      GMI,          DTV,         0x3210, N,   N,  N),
+	PINGROUP(ph1,                    PWM1,       TMDS,       GMI,          DISPLAYA,    0x3214, N,   N,  N),
+	PINGROUP(ph2,                    PWM2,       TMDS,       GMI,          CLDVFS,      0x3218, N,   N,  N),
+	PINGROUP(ph3,                    PWM3,       SPI4,       GMI,          CLDVFS,      0x321c, N,   N,  N),
+	PINGROUP(ph4,                    SDMMC2,     RSVD2,      GMI,          RSVD4,       0x3220, N,   N,  N),
+	PINGROUP(ph5,                    SDMMC2,     RSVD2,      GMI,          RSVD4,       0x3224, N,   N,  N),
+	PINGROUP(ph6,                    SDMMC2,     TRACE,      GMI,          DTV,         0x3228, N,   N,  N),
+	PINGROUP(ph7,                    SDMMC2,     TRACE,      GMI,          DTV,         0x322c, N,   N,  N),
+	PINGROUP(pj7,                    UARTD,      RSVD2,      GMI,          GMI_ALT,     0x3230, N,   N,  N),
+	PINGROUP(pb0,                    UARTD,      RSVD2,      GMI,          RSVD4,       0x3234, N,   N,  N),
+	PINGROUP(pb1,                    UARTD,      RSVD2,      GMI,          RSVD4,       0x3238, N,   N,  N),
+	PINGROUP(pk7,                    UARTD,      RSVD2,      GMI,          RSVD4,       0x323c, N,   N,  N),
+	PINGROUP(pi0,                    RSVD1,      RSVD2,      GMI,          RSVD4,       0x3240, N,   N,  N),
+	PINGROUP(pi1,                    RSVD1,      RSVD2,      GMI,          RSVD4,       0x3244, N,   N,  N),
+	PINGROUP(pi2,                    SDMMC2,     TRACE,      GMI,          RSVD4,       0x3248, N,   N,  N),
+	PINGROUP(pi4,                    SPI4,       TRACE,      GMI,          DISPLAYA,    0x324c, N,   N,  N),
+	PINGROUP(gen2_i2c_scl_pt5,       I2C2,       RSVD2,      GMI,          RSVD4,       0x3250, Y,   N,  N),
+	PINGROUP(gen2_i2c_sda_pt6,       I2C2,       RSVD2,      GMI,          RSVD4,       0x3254, Y,   N,  N),
+	PINGROUP(sdmmc4_clk_pcc4,        SDMMC4,     RSVD2,      GMI,          RSVD4,       0x3258, N,   Y,  N),
+	PINGROUP(sdmmc4_cmd_pt7,         SDMMC4,     RSVD2,      GMI,          RSVD4,       0x325c, N,   Y,  N),
+	PINGROUP(sdmmc4_dat0_paa0,       SDMMC4,     SPI3,       GMI,          RSVD4,       0x3260, N,   Y,  N),
+	PINGROUP(sdmmc4_dat1_paa1,       SDMMC4,     SPI3,       GMI,          RSVD4,       0x3264, N,   Y,  N),
+	PINGROUP(sdmmc4_dat2_paa2,       SDMMC4,     SPI3,       GMI,          RSVD4,       0x3268, N,   Y,  N),
+	PINGROUP(sdmmc4_dat3_paa3,       SDMMC4,     SPI3,       GMI,          RSVD4,       0x326c, N,   Y,  N),
+	PINGROUP(sdmmc4_dat4_paa4,       SDMMC4,     SPI3,       GMI,          RSVD4,       0x3270, N,   Y,  N),
+	PINGROUP(sdmmc4_dat5_paa5,       SDMMC4,     SPI3,       RSVD3,        RSVD4,       0x3274, N,   Y,  N),
+	PINGROUP(sdmmc4_dat6_paa6,       SDMMC4,     SPI3,       GMI,          RSVD4,       0x3278, N,   Y,  N),
+	PINGROUP(sdmmc4_dat7_paa7,       SDMMC4,     RSVD2,      GMI,          RSVD4,       0x327c, N,   Y,  N),
+	PINGROUP(cam_mclk_pcc0,          VI,         VI_ALT1,    VI_ALT3,      SDMMC2,      0x3284, N,   N,  N),
+	PINGROUP(pcc1,                   I2S4,       RSVD2,      RSVD3,        SDMMC2,      0x3288, N,   N,  N),
+	PINGROUP(pbb0,                   VGP6,       VIMCLK2,    SDMMC2,       VIMCLK2_ALT, 0x328c, N,   N,  N),
+	PINGROUP(cam_i2c_scl_pbb1,       VGP1,       I2C3,       RSVD3,        SDMMC2,      0x3290, Y,   N,  N),
+	PINGROUP(cam_i2c_sda_pbb2,       VGP2,       I2C3,       RSVD3,        SDMMC2,      0x3294, Y,   N,  N),
+	PINGROUP(pbb3,                   VGP3,       DISPLAYA,   DISPLAYB,     SDMMC2,      0x3298, N,   N,  N),
+	PINGROUP(pbb4,                   VGP4,       DISPLAYA,   DISPLAYB,     SDMMC2,      0x329c, N,   N,  N),
+	PINGROUP(pbb5,                   VGP5,       DISPLAYA,   RSVD3,        SDMMC2,      0x32a0, N,   N,  N),
+	PINGROUP(pbb6,                   I2S4,       RSVD2,      DISPLAYB,     SDMMC2,      0x32a4, N,   N,  N),
+	PINGROUP(pbb7,                   I2S4,       RSVD2,      RSVD3,        SDMMC2,      0x32a8, N,   N,  N),
+	PINGROUP(pcc2,                   I2S4,       RSVD2,      SDMMC3,       SDMMC2,      0x32ac, N,   N,  N),
+	PINGROUP(jtag_rtck,              RTCK,       RSVD2,      RSVD3,        RSVD4,       0x32b0, N,   N,  N),
+	PINGROUP(pwr_i2c_scl_pz6,        I2CPWR,     RSVD2,      RSVD3,        RSVD4,       0x32b4, Y,   N,  N),
+	PINGROUP(pwr_i2c_sda_pz7,        I2CPWR,     RSVD2,      RSVD3,        RSVD4,       0x32b8, Y,   N,  N),
+	PINGROUP(kb_row0_pr0,            KBC,        RSVD2,      RSVD3,        RSVD4,       0x32bc, N,   N,  N),
+	PINGROUP(kb_row1_pr1,            KBC,        RSVD2,      RSVD3,        RSVD4,       0x32c0, N,   N,  N),
+	PINGROUP(kb_row2_pr2,            KBC,        RSVD2,      RSVD3,        RSVD4,       0x32c4, N,   N,  N),
+	PINGROUP(kb_row3_pr3,            KBC,        DISPLAYA,   SYS,          DISPLAYB,    0x32c8, N,   N,  N),
+	PINGROUP(kb_row4_pr4,            KBC,        DISPLAYA,   RSVD3,        DISPLAYB,    0x32cc, N,   N,  N),
+	PINGROUP(kb_row5_pr5,            KBC,        DISPLAYA,   RSVD3,        DISPLAYB,    0x32d0, N,   N,  N),
+	PINGROUP(kb_row6_pr6,            KBC,        DISPLAYA,   DISPLAYA_ALT, DISPLAYB,    0x32d4, N,   N,  N),
+	PINGROUP(kb_row7_pr7,            KBC,        RSVD2,      CLDVFS,       UARTA,       0x32d8, N,   N,  N),
+	PINGROUP(kb_row8_ps0,            KBC,        RSVD2,      CLDVFS,       UARTA,       0x32dc, N,   N,  N),
+	PINGROUP(kb_row9_ps1,            KBC,        RSVD2,      RSVD3,        UARTA,       0x32e0, N,   N,  N),
+	PINGROUP(kb_row10_ps2,           KBC,        RSVD2,      RSVD3,        UARTA,       0x32e4, N,   N,  N),
+	PINGROUP(kb_row11_ps3,           KBC,        RSVD2,      RSVD3,        IRDA,        0x32e8, N,   N,  N),
+	PINGROUP(kb_row12_ps4,           KBC,        RSVD2,      RSVD3,        IRDA,        0x32ec, N,   N,  N),
+	PINGROUP(kb_row13_ps5,           KBC,        RSVD2,      SPI2,         RSVD4,       0x32f0, N,   N,  N),
+	PINGROUP(kb_row14_ps6,           KBC,        RSVD2,      SPI2,         RSVD4,       0x32f4, N,   N,  N),
+	PINGROUP(kb_row15_ps7,           KBC,        SOC,        RSVD3,        RSVD4,       0x32f8, N,   N,  N),
+	PINGROUP(kb_col0_pq0,            KBC,        RSVD2,      SPI2,         RSVD4,       0x32fc, N,   N,  N),
+	PINGROUP(kb_col1_pq1,            KBC,        RSVD2,      SPI2,         RSVD4,       0x3300, N,   N,  N),
+	PINGROUP(kb_col2_pq2,            KBC,        RSVD2,      SPI2,         RSVD4,       0x3304, N,   N,  N),
+	PINGROUP(kb_col3_pq3,            KBC,        DISPLAYA,   PWM2,         UARTA,       0x3308, N,   N,  N),
+	PINGROUP(kb_col4_pq4,            KBC,        OWR,        SDMMC3,       UARTA,       0x330c, N,   N,  N),
+	PINGROUP(kb_col5_pq5,            KBC,        RSVD2,      SDMMC3,       RSVD4,       0x3310, N,   N,  N),
+	PINGROUP(kb_col6_pq6,            KBC,        RSVD2,      SPI2,         UARTD,       0x3314, N,   N,  N),
+	PINGROUP(kb_col7_pq7,            KBC,        RSVD2,      SPI2,         UARTD,       0x3318, N,   N,  N),
+	PINGROUP(clk_32k_out_pa0,        BLINK,      SOC,        RSVD3,        RSVD4,       0x331c, N,   N,  N),
+	PINGROUP(core_pwr_req,           PWRON,      RSVD2,      RSVD3,        RSVD4,       0x3324, N,   N,  N),
+	PINGROUP(cpu_pwr_req,            CPU,        RSVD2,      RSVD3,        RSVD4,       0x3328, N,   N,  N),
+	PINGROUP(pwr_int_n,              PMI,        RSVD2,      RSVD3,        RSVD4,       0x332c, N,   N,  N),
+	PINGROUP(clk_32k_in,             CLK,        RSVD2,      RSVD3,        RSVD4,       0x3330, N,   N,  N),
+	PINGROUP(owr,                    OWR,        RSVD2,      RSVD3,        RSVD4,       0x3334, N,   N,  Y),
+	PINGROUP(dap1_fs_pn0,            I2S0,       HDA,        GMI,          RSVD4,       0x3338, N,   N,  N),
+	PINGROUP(dap1_din_pn1,           I2S0,       HDA,        GMI,          RSVD4,       0x333c, N,   N,  N),
+	PINGROUP(dap1_dout_pn2,          I2S0,       HDA,        GMI,          SATA,        0x3340, N,   N,  N),
+	PINGROUP(dap1_sclk_pn3,          I2S0,       HDA,        GMI,          RSVD4,       0x3344, N,   N,  N),
+	PINGROUP(dap_mclk1_req_pee2,     DAP,        DAP1,       SATA,         RSVD4,       0x3348, N,   N,  N),
+	PINGROUP(dap_mclk1_pw4,          EXTPERIPH1, DAP2,       RSVD3,        RSVD4,       0x334c, N,   N,  N),
+	PINGROUP(spdif_in_pk6,           SPDIF,      RSVD2,      RSVD3,        I2C3,        0x3350, N,   N,  N),
+	PINGROUP(spdif_out_pk5,          SPDIF,      RSVD2,      RSVD3,        I2C3,        0x3354, N,   N,  N),
+	PINGROUP(dap2_fs_pa2,            I2S1,       HDA,        GMI,          RSVD4,       0x3358, N,   N,  N),
+	PINGROUP(dap2_din_pa4,           I2S1,       HDA,        GMI,          RSVD4,       0x335c, N,   N,  N),
+	PINGROUP(dap2_dout_pa5,          I2S1,       HDA,        GMI,          RSVD4,       0x3360, N,   N,  N),
+	PINGROUP(dap2_sclk_pa3,          I2S1,       HDA,        GMI,          RSVD4,       0x3364, N,   N,  N),
+	PINGROUP(dvfs_pwm_px0,           SPI6,       CLDVFS,     GMI,          RSVD4,       0x3368, N,   N,  N),
+	PINGROUP(gpio_x1_aud_px1,        SPI6,       RSVD2,      GMI,          RSVD4,       0x336c, N,   N,  N),
+	PINGROUP(gpio_x3_aud_px3,        SPI6,       SPI1,       GMI,          RSVD4,       0x3370, N,   N,  N),
+	PINGROUP(dvfs_clk_px2,           SPI6,       CLDVFS,     GMI,          RSVD4,       0x3374, N,   N,  N),
+	PINGROUP(gpio_x4_aud_px4,        GMI,        SPI1,       SPI2,         DAP2,        0x3378, N,   N,  N),
+	PINGROUP(gpio_x5_aud_px5,        GMI,        SPI1,       SPI2,         RSVD4,       0x337c, N,   N,  N),
+	PINGROUP(gpio_x6_aud_px6,        SPI6,       SPI1,       SPI2,         GMI,         0x3380, N,   N,  N),
+	PINGROUP(gpio_x7_aud_px7,        RSVD1,      SPI1,       SPI2,         RSVD4,       0x3384, N,   N,  N),
+	PINGROUP(sdmmc3_clk_pa6,         SDMMC3,     RSVD2,      RSVD3,        SPI3,        0x3390, N,   N,  N),
+	PINGROUP(sdmmc3_cmd_pa7,         SDMMC3,     PWM3,       UARTA,        SPI3,        0x3394, N,   N,  N),
+	PINGROUP(sdmmc3_dat0_pb7,        SDMMC3,     RSVD2,      RSVD3,        SPI3,        0x3398, N,   N,  N),
+	PINGROUP(sdmmc3_dat1_pb6,        SDMMC3,     PWM2,       UARTA,        SPI3,        0x339c, N,   N,  N),
+	PINGROUP(sdmmc3_dat2_pb5,        SDMMC3,     PWM1,       DISPLAYA,     SPI3,        0x33a0, N,   N,  N),
+	PINGROUP(sdmmc3_dat3_pb4,        SDMMC3,     PWM0,       DISPLAYB,     SPI3,        0x33a4, N,   N,  N),
+	PINGROUP(pex_l0_rst_n_pdd1,      PE0,        RSVD2,      RSVD3,        RSVD4,       0x33bc, N,   N,  N),
+	PINGROUP(pex_l0_clkreq_n_pdd2,   PE0,        RSVD2,      RSVD3,        RSVD4,       0x33c0, N,   N,  N),
+	PINGROUP(pex_wake_n_pdd3,        PE,         RSVD2,      RSVD3,        RSVD4,       0x33c4, N,   N,  N),
+	PINGROUP(pex_l1_rst_n_pdd5,      PE1,        RSVD2,      RSVD3,        RSVD4,       0x33cc, N,   N,  N),
+	PINGROUP(pex_l1_clkreq_n_pdd6,   PE1,        RSVD2,      RSVD3,        RSVD4,       0x33d0, N,   N,  N),
+	PINGROUP(hdmi_cec_pee3,          CEC,        RSVD2,      RSVD3,        RSVD4,       0x33e0, Y,   N,  N),
+	PINGROUP(sdmmc1_wp_n_pv3,        SDMMC1,     CLK12,      SPI4,         UARTA,       0x33e4, N,   N,  N),
+	PINGROUP(sdmmc3_cd_n_pv2,        SDMMC3,     OWR,        RSVD3,        RSVD4,       0x33e8, N,   N,  N),
+	PINGROUP(gpio_w2_aud_pw2,        SPI6,       RSVD2,      SPI2,         I2C1,        0x33ec, N,   N,  N),
+	PINGROUP(gpio_w3_aud_pw3,        SPI6,       SPI1,       SPI2,         I2C1,        0x33f0, N,   N,  N),
+	PINGROUP(usb_vbus_en0_pn4,       USB,        RSVD2,      RSVD3,        RSVD4,       0x33f4, Y,   N,  N),
+	PINGROUP(usb_vbus_en1_pn5,       USB,        RSVD2,      RSVD3,        RSVD4,       0x33f8, Y,   N,  N),
+	PINGROUP(sdmmc3_clk_lb_in_pee5,  SDMMC3,     RSVD2,      RSVD3,        RSVD4,       0x33fc, N,   N,  N),
+	PINGROUP(sdmmc3_clk_lb_out_pee4, SDMMC3,     RSVD2,      RSVD3,        RSVD4,       0x3400, N,   N,  N),
+	PINGROUP(gmi_clk_lb,             SDMMC2,     RSVD2,      GMI,          RSVD4,       0x3404, N,   N,  N),
+	PINGROUP(reset_out_n,            RSVD1,      RSVD2,      RSVD3,        RESET_OUT_N, 0x3408, N,   N,  N),
+	PINGROUP(kb_row16_pt0,           KBC,        RSVD2,      RSVD3,        UARTC,       0x340c, N,   N,  N),
+	PINGROUP(kb_row17_pt1,           KBC,        RSVD2,      RSVD3,        UARTC,       0x3410, N,   N,  N),
+	PINGROUP(usb_vbus_en2_pff1,      USB,        RSVD2,      RSVD3,        RSVD4,       0x3414, Y,   N,  N),
+	PINGROUP(pff2,                   SATA,       RSVD2,      RSVD3,        RSVD4,       0x3418, Y,   N,  N),
+	PINGROUP(dp_hpd_pff0,            DP,         RSVD2,      RSVD3,        RSVD4,       0x3430, N,   N,  N),
 
 	/* pg_name, r, hsm_b, schmitt_b, lpmd_b, drvdn_b, drvdn_w, drvup_b, drvup_w, slwr_b, slwr_w, slwf_b, slwf_w, drvtype */
-       DRV_PINGROUP(ao1,         0x868,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(ao2,         0x86c,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(at1,         0x870,  2,  3,  4,  12,  7,  20,  7,  28,  2,  30,  2,  Y),
-       DRV_PINGROUP(at2,         0x874,  2,  3,  4,  12,  7,  20,  7,  28,  2,  30,  2,  Y),
-       DRV_PINGROUP(at3,         0x878,  2,  3,  4,  12,  7,  20,  7,  28,  2,  30,  2,  Y),
-       DRV_PINGROUP(at4,         0x87c,  2,  3,  4,  12,  7,  20,  7,  28,  2,  30,  2,  Y),
-       DRV_PINGROUP(at5,         0x880,  2,  3,  4,  14,  5,  19,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(cdev1,       0x884,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(cdev2,       0x888,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(dap1,        0x890,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(dap2,        0x894,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(dap3,        0x898,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(dap4,        0x89c,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(dbg,         0x8a0,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(sdio3,       0x8b0,  2,  3, -1,  12,  7,  20,  7,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(spi,         0x8b4,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(uaa,         0x8b8,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(uab,         0x8bc,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(uart2,       0x8c0,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(uart3,       0x8c4,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(sdio1,       0x8ec,  2,  3, -1,  12,  7,  20,  7,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(ddc,         0x8fc,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(gma,         0x900,  2,  3,  4,  14,  5,  20,  5,  28,  2,  30,  2,  Y),
-       DRV_PINGROUP(gme,         0x910,  2,  3,  4,  14,  5,  19,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(gmf,         0x914,  2,  3,  4,  14,  5,  19,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(gmg,         0x918,  2,  3,  4,  14,  5,  19,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(gmh,         0x91c,  2,  3,  4,  14,  5,  19,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(owr,         0x920,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(uda,         0x924,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(gpv,         0x928,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(dev3,        0x92c,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(cec,         0x938,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(at6,         0x994,  2,  3,  4,  12,  7,  20,  7,  28,  2,  30,  2,  Y),
-       DRV_PINGROUP(dap5,        0x998,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(usb_vbus_en, 0x99c,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(ao3,         0x9a8,  2,  3,  4,  12,  5,  -1, -1,  28,  2,  -1, -1,  N),
-       DRV_PINGROUP(ao0,         0x9b0,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(hv0,         0x9b4,  2,  3,  4,  12,  5,  -1, -1,  28,  2,  -1, -1,  N),
-       DRV_PINGROUP(sdio4,       0x9c4,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
-       DRV_PINGROUP(ao4,         0x9c8,  2,  3,  4,  12,  7,  20,  7,  28,  2,  30,  2,  Y),
+	DRV_PINGROUP(ao1,         0x868,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(ao2,         0x86c,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(at1,         0x870,  2,  3,  4,  12,  7,  20,  7,  28,  2,  30,  2,  Y),
+	DRV_PINGROUP(at2,         0x874,  2,  3,  4,  12,  7,  20,  7,  28,  2,  30,  2,  Y),
+	DRV_PINGROUP(at3,         0x878,  2,  3,  4,  12,  7,  20,  7,  28,  2,  30,  2,  Y),
+	DRV_PINGROUP(at4,         0x87c,  2,  3,  4,  12,  7,  20,  7,  28,  2,  30,  2,  Y),
+	DRV_PINGROUP(at5,         0x880,  2,  3,  4,  14,  5,  19,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(cdev1,       0x884,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(cdev2,       0x888,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(dap1,        0x890,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(dap2,        0x894,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(dap3,        0x898,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(dap4,        0x89c,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(dbg,         0x8a0,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(sdio3,       0x8b0,  2,  3, -1,  12,  7,  20,  7,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(spi,         0x8b4,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(uaa,         0x8b8,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(uab,         0x8bc,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(uart2,       0x8c0,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(uart3,       0x8c4,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(sdio1,       0x8ec,  2,  3, -1,  12,  7,  20,  7,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(ddc,         0x8fc,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(gma,         0x900,  2,  3,  4,  14,  5,  20,  5,  28,  2,  30,  2,  Y),
+	DRV_PINGROUP(gme,         0x910,  2,  3,  4,  14,  5,  19,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(gmf,         0x914,  2,  3,  4,  14,  5,  19,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(gmg,         0x918,  2,  3,  4,  14,  5,  19,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(gmh,         0x91c,  2,  3,  4,  14,  5,  19,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(owr,         0x920,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(uda,         0x924,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(gpv,         0x928,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(dev3,        0x92c,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(cec,         0x938,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(at6,         0x994,  2,  3,  4,  12,  7,  20,  7,  28,  2,  30,  2,  Y),
+	DRV_PINGROUP(dap5,        0x998,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(usb_vbus_en, 0x99c,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(ao3,         0x9a8,  2,  3,  4,  12,  5,  -1, -1,  28,  2,  -1, -1,  N),
+	DRV_PINGROUP(ao0,         0x9b0,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(hv0,         0x9b4,  2,  3,  4,  12,  5,  -1, -1,  28,  2,  -1, -1,  N),
+	DRV_PINGROUP(sdio4,       0x9c4,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
+	DRV_PINGROUP(ao4,         0x9c8,  2,  3,  4,  12,  7,  20,  7,  28,  2,  30,  2,  Y),
 };
 
 static const struct tegra_pinctrl_soc_data tegra124_pinctrl = {
diff --git a/drivers/pinctrl/pinctrl-tegra20.c b/drivers/pinctrl/pinctrl-tegra20.c
index e0b50408838..7563ebc9c79 100644
--- a/drivers/pinctrl/pinctrl-tegra20.c
+++ b/drivers/pinctrl/pinctrl-tegra20.c
@@ -1973,7 +1973,7 @@ static struct tegra_function tegra20_functions[] = {
 #define PINGROUP_REG_A		0x868
 
 /* Pin group with mux control, and typically tri-state and pull-up/down too */
-#define MUX_PG(pg_name, f0, f1, f2, f3, f_safe,			\
+#define MUX_PG(pg_name, f0, f1, f2, f3,				\
 	       tri_r, tri_b, mux_r, mux_b, pupd_r, pupd_b)	\
 	{							\
 		.name = #pg_name,				\
@@ -1985,7 +1985,6 @@ static struct tegra_function tegra20_functions[] = {
 			TEGRA_MUX_ ## f2,			\
 			TEGRA_MUX_ ## f3,			\
 		},						\
-		.func_safe = TEGRA_MUX_ ## f_safe,		\
 		.mux_reg = ((mux_r) - PIN_MUX_CTL_REG_A),	\
 		.mux_bank = 1,					\
 		.mux_bit = mux_b,				\
@@ -1995,13 +1994,12 @@ static struct tegra_function tegra20_functions[] = {
 		.tri_reg = ((tri_r) - TRISTATE_REG_A),		\
 		.tri_bank = 0,					\
 		.tri_bit = tri_b,				\
-		.einput_reg = -1,				\
-		.odrain_reg = -1,				\
-		.lock_reg = -1,					\
-		.ioreset_reg = -1,				\
-		.rcv_sel_reg = -1,				\
+		.einput_bit = -1,				\
+		.odrain_bit = -1,				\
+		.lock_bit = -1,					\
+		.ioreset_bit = -1,				\
+		.rcv_sel_bit = -1,				\
 		.drv_reg = -1,					\
-		.drvtype_reg = -1,				\
 	}
 
 /* Pin groups with only pull up and pull down control */
@@ -2014,14 +2012,7 @@ static struct tegra_function tegra20_functions[] = {
 		.pupd_reg = ((pupd_r) - PULLUPDOWN_REG_A),	\
 		.pupd_bank = 2,					\
 		.pupd_bit = pupd_b,				\
-		.tri_reg = -1,					\
-		.einput_reg = -1,				\
-		.odrain_reg = -1,				\
-		.lock_reg = -1,					\
-		.ioreset_reg = -1,				\
-		.rcv_sel_reg = -1,				\
 		.drv_reg = -1,					\
-		.drvtype_reg = -1,				\
 	}
 
 /* Pin groups for drive strength registers (configurable version) */
@@ -2035,11 +2026,6 @@ static struct tegra_function tegra20_functions[] = {
 		.mux_reg = -1,					\
 		.pupd_reg = -1,					\
 		.tri_reg = -1,					\
-		.einput_reg = -1,				\
-		.odrain_reg = -1,				\
-		.lock_reg = -1,					\
-		.ioreset_reg = -1,				\
-		.rcv_sel_reg = -1,				\
 		.drv_reg = ((r) - PINGROUP_REG_A),		\
 		.drv_bank = 3,					\
 		.hsm_bit = hsm_b,				\
@@ -2053,7 +2039,7 @@ static struct tegra_function tegra20_functions[] = {
 		.slwr_width = slwr_w,				\
 		.slwf_bit = slwf_b,				\
 		.slwf_width = slwf_w,				\
-		.drvtype_reg = -1,				\
+		.drvtype_bit = -1,				\
 	}
 
 /* Pin groups for drive strength registers (simple version) */
@@ -2061,114 +2047,114 @@ static struct tegra_function tegra20_functions[] = {
 	DRV_PG_EXT(pg_name, r, 2,  3,  4, 12, 20, 28, 2, 30, 2)
 
 static const struct tegra_pingroup tegra20_groups[] = {
-	/*     name,   f0,        f1,        f2,        f3,            f_safe,    tri r/b,  mux r/b,  pupd r/b */
-	MUX_PG(ata,    IDE,       NAND,      GMI,       RSVD4,         IDE,       0x14, 0,  0x80, 24, 0xa0, 0),
-	MUX_PG(atb,    IDE,       NAND,      GMI,       SDIO4,         IDE,       0x14, 1,  0x80, 16, 0xa0, 2),
-	MUX_PG(atc,    IDE,       NAND,      GMI,       SDIO4,         IDE,       0x14, 2,  0x80, 22, 0xa0, 4),
-	MUX_PG(atd,    IDE,       NAND,      GMI,       SDIO4,         IDE,       0x14, 3,  0x80, 20, 0xa0, 6),
-	MUX_PG(ate,    IDE,       NAND,      GMI,       RSVD4,         IDE,       0x18, 25, 0x80, 12, 0xa0, 8),
-	MUX_PG(cdev1,  OSC,       PLLA_OUT,  PLLM_OUT1, AUDIO_SYNC,    OSC,       0x14, 4,  0x88, 2,  0xa8, 0),
-	MUX_PG(cdev2,  OSC,       AHB_CLK,   APB_CLK,   PLLP_OUT4,     OSC,       0x14, 5,  0x88, 4,  0xa8, 2),
-	MUX_PG(crtp,   CRT,       RSVD2,     RSVD3,     RSVD4,         RSVD2,     0x20, 14, 0x98, 20, 0xa4, 24),
-	MUX_PG(csus,   PLLC_OUT1, PLLP_OUT2, PLLP_OUT3, VI_SENSOR_CLK, PLLC_OUT1, 0x14, 6,  0x88, 6,  0xac, 24),
-	MUX_PG(dap1,   DAP1,      RSVD2,     GMI,       SDIO2,         DAP1,      0x14, 7,  0x88, 20, 0xa0, 10),
-	MUX_PG(dap2,   DAP2,      TWC,       RSVD3,     GMI,           DAP2,      0x14, 8,  0x88, 22, 0xa0, 12),
-	MUX_PG(dap3,   DAP3,      RSVD2,     RSVD3,     RSVD4,         DAP3,      0x14, 9,  0x88, 24, 0xa0, 14),
-	MUX_PG(dap4,   DAP4,      RSVD2,     GMI,       RSVD4,         DAP4,      0x14, 10, 0x88, 26, 0xa0, 16),
-	MUX_PG(ddc,    I2C2,      RSVD2,     RSVD3,     RSVD4,         RSVD4,     0x18, 31, 0x88, 0,  0xb0, 28),
-	MUX_PG(dta,    RSVD1,     SDIO2,     VI,        RSVD4,         RSVD4,     0x14, 11, 0x84, 20, 0xa0, 18),
-	MUX_PG(dtb,    RSVD1,     RSVD2,     VI,        SPI1,          RSVD1,     0x14, 12, 0x84, 22, 0xa0, 20),
-	MUX_PG(dtc,    RSVD1,     RSVD2,     VI,        RSVD4,         RSVD1,     0x14, 13, 0x84, 26, 0xa0, 22),
-	MUX_PG(dtd,    RSVD1,     SDIO2,     VI,        RSVD4,         RSVD1,     0x14, 14, 0x84, 28, 0xa0, 24),
-	MUX_PG(dte,    RSVD1,     RSVD2,     VI,        SPI1,          RSVD1,     0x14, 15, 0x84, 30, 0xa0, 26),
-	MUX_PG(dtf,    I2C3,      RSVD2,     VI,        RSVD4,         RSVD4,     0x20, 12, 0x98, 30, 0xa0, 28),
-	MUX_PG(gma,    UARTE,     SPI3,      GMI,       SDIO4,         SPI3,      0x14, 28, 0x84, 0,  0xb0, 20),
-	MUX_PG(gmb,    IDE,       NAND,      GMI,       GMI_INT,       GMI,       0x18, 29, 0x88, 28, 0xb0, 22),
-	MUX_PG(gmc,    UARTD,     SPI4,      GMI,       SFLASH,        SPI4,      0x14, 29, 0x84, 2,  0xb0, 24),
-	MUX_PG(gmd,    RSVD1,     NAND,      GMI,       SFLASH,        GMI,       0x18, 30, 0x88, 30, 0xb0, 26),
-	MUX_PG(gme,    RSVD1,     DAP5,      GMI,       SDIO4,         GMI,       0x18, 0,  0x8c, 0,  0xa8, 24),
-	MUX_PG(gpu,    PWM,       UARTA,     GMI,       RSVD4,         RSVD4,     0x14, 16, 0x8c, 4,  0xa4, 20),
-	MUX_PG(gpu7,   RTCK,      RSVD2,     RSVD3,     RSVD4,         RTCK,      0x20, 11, 0x98, 28, 0xa4, 6),
-	MUX_PG(gpv,    PCIE,      RSVD2,     RSVD3,     RSVD4,         PCIE,      0x14, 17, 0x8c, 2,  0xa0, 30),
-	MUX_PG(hdint,  HDMI,      RSVD2,     RSVD3,     RSVD4,         HDMI,      0x1c, 23, 0x84, 4,  -1,   -1),
-	MUX_PG(i2cp,   I2CP,      RSVD2,     RSVD3,     RSVD4,         RSVD4,     0x14, 18, 0x88, 8,  0xa4, 2),
-	MUX_PG(irrx,   UARTA,     UARTB,     GMI,       SPI4,          UARTB,     0x14, 20, 0x88, 18, 0xa8, 22),
-	MUX_PG(irtx,   UARTA,     UARTB,     GMI,       SPI4,          UARTB,     0x14, 19, 0x88, 16, 0xa8, 20),
-	MUX_PG(kbca,   KBC,       NAND,      SDIO2,     EMC_TEST0_DLL, KBC,       0x14, 22, 0x88, 10, 0xa4, 8),
-	MUX_PG(kbcb,   KBC,       NAND,      SDIO2,     MIO,           KBC,       0x14, 21, 0x88, 12, 0xa4, 10),
-	MUX_PG(kbcc,   KBC,       NAND,      TRACE,     EMC_TEST1_DLL, KBC,       0x18, 26, 0x88, 14, 0xa4, 12),
-	MUX_PG(kbcd,   KBC,       NAND,      SDIO2,     MIO,           KBC,       0x20, 10, 0x98, 26, 0xa4, 14),
-	MUX_PG(kbce,   KBC,       NAND,      OWR,       RSVD4,         KBC,       0x14, 26, 0x80, 28, 0xb0, 2),
-	MUX_PG(kbcf,   KBC,       NAND,      TRACE,     MIO,           KBC,       0x14, 27, 0x80, 26, 0xb0, 0),
-	MUX_PG(lcsn,   DISPLAYA,  DISPLAYB,  SPI3,      RSVD4,         RSVD4,     0x1c, 31, 0x90, 12, -1,   -1),
-	MUX_PG(ld0,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 0,  0x94, 0,  -1,   -1),
-	MUX_PG(ld1,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 1,  0x94, 2,  -1,   -1),
-	MUX_PG(ld2,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 2,  0x94, 4,  -1,   -1),
-	MUX_PG(ld3,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 3,  0x94, 6,  -1,   -1),
-	MUX_PG(ld4,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 4,  0x94, 8,  -1,   -1),
-	MUX_PG(ld5,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 5,  0x94, 10, -1,   -1),
-	MUX_PG(ld6,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 6,  0x94, 12, -1,   -1),
-	MUX_PG(ld7,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 7,  0x94, 14, -1,   -1),
-	MUX_PG(ld8,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 8,  0x94, 16, -1,   -1),
-	MUX_PG(ld9,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 9,  0x94, 18, -1,   -1),
-	MUX_PG(ld10,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 10, 0x94, 20, -1,   -1),
-	MUX_PG(ld11,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 11, 0x94, 22, -1,   -1),
-	MUX_PG(ld12,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 12, 0x94, 24, -1,   -1),
-	MUX_PG(ld13,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 13, 0x94, 26, -1,   -1),
-	MUX_PG(ld14,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 14, 0x94, 28, -1,   -1),
-	MUX_PG(ld15,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 15, 0x94, 30, -1,   -1),
-	MUX_PG(ld16,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 16, 0x98, 0,  -1,   -1),
-	MUX_PG(ld17,   DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         RSVD4,     0x1c, 17, 0x98, 2,  -1,   -1),
-	MUX_PG(ldc,    DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         RSVD4,     0x1c, 30, 0x90, 14, -1,   -1),
-	MUX_PG(ldi,    DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         RSVD4,     0x20, 6,  0x98, 16, -1,   -1),
-	MUX_PG(lhp0,   DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         RSVD4,     0x1c, 18, 0x98, 10, -1,   -1),
-	MUX_PG(lhp1,   DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         RSVD4,     0x1c, 19, 0x98, 4,  -1,   -1),
-	MUX_PG(lhp2,   DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         RSVD4,     0x1c, 20, 0x98, 6,  -1,   -1),
-	MUX_PG(lhs,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x20, 7,  0x90, 22, -1,   -1),
-	MUX_PG(lm0,    DISPLAYA,  DISPLAYB,  SPI3,      RSVD4,         RSVD4,     0x1c, 24, 0x90, 26, -1,   -1),
-	MUX_PG(lm1,    DISPLAYA,  DISPLAYB,  RSVD3,     CRT,           RSVD3,     0x1c, 25, 0x90, 28, -1,   -1),
-	MUX_PG(lpp,    DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         RSVD4,     0x20, 8,  0x98, 14, -1,   -1),
-	MUX_PG(lpw0,   DISPLAYA,  DISPLAYB,  SPI3,      HDMI,          DISPLAYA,  0x20, 3,  0x90, 0,  -1,   -1),
-	MUX_PG(lpw1,   DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         RSVD4,     0x20, 4,  0x90, 2,  -1,   -1),
-	MUX_PG(lpw2,   DISPLAYA,  DISPLAYB,  SPI3,      HDMI,          DISPLAYA,  0x20, 5,  0x90, 4,  -1,   -1),
-	MUX_PG(lsc0,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 27, 0x90, 18, -1,   -1),
-	MUX_PG(lsc1,   DISPLAYA,  DISPLAYB,  SPI3,      HDMI,          DISPLAYA,  0x1c, 28, 0x90, 20, -1,   -1),
-	MUX_PG(lsck,   DISPLAYA,  DISPLAYB,  SPI3,      HDMI,          DISPLAYA,  0x1c, 29, 0x90, 16, -1,   -1),
-	MUX_PG(lsda,   DISPLAYA,  DISPLAYB,  SPI3,      HDMI,          DISPLAYA,  0x20, 1,  0x90, 8,  -1,   -1),
-	MUX_PG(lsdi,   DISPLAYA,  DISPLAYB,  SPI3,      RSVD4,         DISPLAYA,  0x20, 2,  0x90, 6,  -1,   -1),
-	MUX_PG(lspi,   DISPLAYA,  DISPLAYB,  XIO,       HDMI,          DISPLAYA,  0x20, 0,  0x90, 10, -1,   -1),
-	MUX_PG(lvp0,   DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         RSVD4,     0x1c, 21, 0x90, 30, -1,   -1),
-	MUX_PG(lvp1,   DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         RSVD4,     0x1c, 22, 0x98, 8,  -1,   -1),
-	MUX_PG(lvs,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         RSVD4,     0x1c, 26, 0x90, 24, -1,   -1),
-	MUX_PG(owc,    OWR,       RSVD2,     RSVD3,     RSVD4,         OWR,       0x14, 31, 0x84, 8,  0xb0, 30),
-	MUX_PG(pmc,    PWR_ON,    PWR_INTR,  RSVD3,     RSVD4,         PWR_ON,    0x14, 23, 0x98, 18, -1,   -1),
-	MUX_PG(pta,    I2C2,      HDMI,      GMI,       RSVD4,         RSVD4,     0x14, 24, 0x98, 22, 0xa4, 4),
-	MUX_PG(rm,     I2C1,      RSVD2,     RSVD3,     RSVD4,         RSVD4,     0x14, 25, 0x80, 14, 0xa4, 0),
-	MUX_PG(sdb,    UARTA,     PWM,       SDIO3,     SPI2,          PWM,       0x20, 15, 0x8c, 10, -1,   -1),
-	MUX_PG(sdc,    PWM,       TWC,       SDIO3,     SPI3,          TWC,       0x18, 1,  0x8c, 12, 0xac, 28),
-	MUX_PG(sdd,    UARTA,     PWM,       SDIO3,     SPI3,          PWM,       0x18, 2,  0x8c, 14, 0xac, 30),
-	MUX_PG(sdio1,  SDIO1,     RSVD2,     UARTE,     UARTA,         RSVD2,     0x14, 30, 0x80, 30, 0xb0, 18),
-	MUX_PG(slxa,   PCIE,      SPI4,      SDIO3,     SPI2,          PCIE,      0x18, 3,  0x84, 6,  0xa4, 22),
-	MUX_PG(slxc,   SPDIF,     SPI4,      SDIO3,     SPI2,          SPI4,      0x18, 5,  0x84, 10, 0xa4, 26),
-	MUX_PG(slxd,   SPDIF,     SPI4,      SDIO3,     SPI2,          SPI4,      0x18, 6,  0x84, 12, 0xa4, 28),
-	MUX_PG(slxk,   PCIE,      SPI4,      SDIO3,     SPI2,          PCIE,      0x18, 7,  0x84, 14, 0xa4, 30),
-	MUX_PG(spdi,   SPDIF,     RSVD2,     I2C1,      SDIO2,         RSVD2,     0x18, 8,  0x8c, 8,  0xa4, 16),
-	MUX_PG(spdo,   SPDIF,     RSVD2,     I2C1,      SDIO2,         RSVD2,     0x18, 9,  0x8c, 6,  0xa4, 18),
-	MUX_PG(spia,   SPI1,      SPI2,      SPI3,      GMI,           GMI,       0x18, 10, 0x8c, 30, 0xa8, 4),
-	MUX_PG(spib,   SPI1,      SPI2,      SPI3,      GMI,           GMI,       0x18, 11, 0x8c, 28, 0xa8, 6),
-	MUX_PG(spic,   SPI1,      SPI2,      SPI3,      GMI,           GMI,       0x18, 12, 0x8c, 26, 0xa8, 8),
-	MUX_PG(spid,   SPI2,      SPI1,      SPI2_ALT,  GMI,           GMI,       0x18, 13, 0x8c, 24, 0xa8, 10),
-	MUX_PG(spie,   SPI2,      SPI1,      SPI2_ALT,  GMI,           GMI,       0x18, 14, 0x8c, 22, 0xa8, 12),
-	MUX_PG(spif,   SPI3,      SPI1,      SPI2,      RSVD4,         RSVD4,     0x18, 15, 0x8c, 20, 0xa8, 14),
-	MUX_PG(spig,   SPI3,      SPI2,      SPI2_ALT,  I2C1,          SPI2_ALT,  0x18, 16, 0x8c, 18, 0xa8, 16),
-	MUX_PG(spih,   SPI3,      SPI2,      SPI2_ALT,  I2C1,          SPI2_ALT,  0x18, 17, 0x8c, 16, 0xa8, 18),
-	MUX_PG(uaa,    SPI3,      MIPI_HS,   UARTA,     ULPI,          MIPI_HS,   0x18, 18, 0x80, 0,  0xac, 0),
-	MUX_PG(uab,    SPI2,      MIPI_HS,   UARTA,     ULPI,          MIPI_HS,   0x18, 19, 0x80, 2,  0xac, 2),
-	MUX_PG(uac,    OWR,       RSVD2,     RSVD3,     RSVD4,         RSVD4,     0x18, 20, 0x80, 4,  0xac, 4),
-	MUX_PG(uad,    IRDA,      SPDIF,     UARTA,     SPI4,          SPDIF,     0x18, 21, 0x80, 6,  0xac, 6),
-	MUX_PG(uca,    UARTC,     RSVD2,     GMI,       RSVD4,         RSVD4,     0x18, 22, 0x84, 16, 0xac, 8),
-	MUX_PG(ucb,    UARTC,     PWM,       GMI,       RSVD4,         RSVD4,     0x18, 23, 0x84, 18, 0xac, 10),
-	MUX_PG(uda,    SPI1,      RSVD2,     UARTD,     ULPI,          RSVD2,     0x20, 13, 0x80, 8,  0xb0, 16),
+	/*     name,   f0,        f1,        f2,        f3,            tri r/b,  mux r/b,  pupd r/b */
+	MUX_PG(ata,    IDE,       NAND,      GMI,       RSVD4,         0x14, 0,  0x80, 24, 0xa0, 0),
+	MUX_PG(atb,    IDE,       NAND,      GMI,       SDIO4,         0x14, 1,  0x80, 16, 0xa0, 2),
+	MUX_PG(atc,    IDE,       NAND,      GMI,       SDIO4,         0x14, 2,  0x80, 22, 0xa0, 4),
+	MUX_PG(atd,    IDE,       NAND,      GMI,       SDIO4,         0x14, 3,  0x80, 20, 0xa0, 6),
+	MUX_PG(ate,    IDE,       NAND,      GMI,       RSVD4,         0x18, 25, 0x80, 12, 0xa0, 8),
+	MUX_PG(cdev1,  OSC,       PLLA_OUT,  PLLM_OUT1, AUDIO_SYNC,    0x14, 4,  0x88, 2,  0xa8, 0),
+	MUX_PG(cdev2,  OSC,       AHB_CLK,   APB_CLK,   PLLP_OUT4,     0x14, 5,  0x88, 4,  0xa8, 2),
+	MUX_PG(crtp,   CRT,       RSVD2,     RSVD3,     RSVD4,         0x20, 14, 0x98, 20, 0xa4, 24),
+	MUX_PG(csus,   PLLC_OUT1, PLLP_OUT2, PLLP_OUT3, VI_SENSOR_CLK, 0x14, 6,  0x88, 6,  0xac, 24),
+	MUX_PG(dap1,   DAP1,      RSVD2,     GMI,       SDIO2,         0x14, 7,  0x88, 20, 0xa0, 10),
+	MUX_PG(dap2,   DAP2,      TWC,       RSVD3,     GMI,           0x14, 8,  0x88, 22, 0xa0, 12),
+	MUX_PG(dap3,   DAP3,      RSVD2,     RSVD3,     RSVD4,         0x14, 9,  0x88, 24, 0xa0, 14),
+	MUX_PG(dap4,   DAP4,      RSVD2,     GMI,       RSVD4,         0x14, 10, 0x88, 26, 0xa0, 16),
+	MUX_PG(ddc,    I2C2,      RSVD2,     RSVD3,     RSVD4,         0x18, 31, 0x88, 0,  0xb0, 28),
+	MUX_PG(dta,    RSVD1,     SDIO2,     VI,        RSVD4,         0x14, 11, 0x84, 20, 0xa0, 18),
+	MUX_PG(dtb,    RSVD1,     RSVD2,     VI,        SPI1,          0x14, 12, 0x84, 22, 0xa0, 20),
+	MUX_PG(dtc,    RSVD1,     RSVD2,     VI,        RSVD4,         0x14, 13, 0x84, 26, 0xa0, 22),
+	MUX_PG(dtd,    RSVD1,     SDIO2,     VI,        RSVD4,         0x14, 14, 0x84, 28, 0xa0, 24),
+	MUX_PG(dte,    RSVD1,     RSVD2,     VI,        SPI1,          0x14, 15, 0x84, 30, 0xa0, 26),
+	MUX_PG(dtf,    I2C3,      RSVD2,     VI,        RSVD4,         0x20, 12, 0x98, 30, 0xa0, 28),
+	MUX_PG(gma,    UARTE,     SPI3,      GMI,       SDIO4,         0x14, 28, 0x84, 0,  0xb0, 20),
+	MUX_PG(gmb,    IDE,       NAND,      GMI,       GMI_INT,       0x18, 29, 0x88, 28, 0xb0, 22),
+	MUX_PG(gmc,    UARTD,     SPI4,      GMI,       SFLASH,        0x14, 29, 0x84, 2,  0xb0, 24),
+	MUX_PG(gmd,    RSVD1,     NAND,      GMI,       SFLASH,        0x18, 30, 0x88, 30, 0xb0, 26),
+	MUX_PG(gme,    RSVD1,     DAP5,      GMI,       SDIO4,         0x18, 0,  0x8c, 0,  0xa8, 24),
+	MUX_PG(gpu,    PWM,       UARTA,     GMI,       RSVD4,         0x14, 16, 0x8c, 4,  0xa4, 20),
+	MUX_PG(gpu7,   RTCK,      RSVD2,     RSVD3,     RSVD4,         0x20, 11, 0x98, 28, 0xa4, 6),
+	MUX_PG(gpv,    PCIE,      RSVD2,     RSVD3,     RSVD4,         0x14, 17, 0x8c, 2,  0xa0, 30),
+	MUX_PG(hdint,  HDMI,      RSVD2,     RSVD3,     RSVD4,         0x1c, 23, 0x84, 4,  -1,   -1),
+	MUX_PG(i2cp,   I2CP,      RSVD2,     RSVD3,     RSVD4,         0x14, 18, 0x88, 8,  0xa4, 2),
+	MUX_PG(irrx,   UARTA,     UARTB,     GMI,       SPI4,          0x14, 20, 0x88, 18, 0xa8, 22),
+	MUX_PG(irtx,   UARTA,     UARTB,     GMI,       SPI4,          0x14, 19, 0x88, 16, 0xa8, 20),
+	MUX_PG(kbca,   KBC,       NAND,      SDIO2,     EMC_TEST0_DLL, 0x14, 22, 0x88, 10, 0xa4, 8),
+	MUX_PG(kbcb,   KBC,       NAND,      SDIO2,     MIO,           0x14, 21, 0x88, 12, 0xa4, 10),
+	MUX_PG(kbcc,   KBC,       NAND,      TRACE,     EMC_TEST1_DLL, 0x18, 26, 0x88, 14, 0xa4, 12),
+	MUX_PG(kbcd,   KBC,       NAND,      SDIO2,     MIO,           0x20, 10, 0x98, 26, 0xa4, 14),
+	MUX_PG(kbce,   KBC,       NAND,      OWR,       RSVD4,         0x14, 26, 0x80, 28, 0xb0, 2),
+	MUX_PG(kbcf,   KBC,       NAND,      TRACE,     MIO,           0x14, 27, 0x80, 26, 0xb0, 0),
+	MUX_PG(lcsn,   DISPLAYA,  DISPLAYB,  SPI3,      RSVD4,         0x1c, 31, 0x90, 12, -1,   -1),
+	MUX_PG(ld0,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 0,  0x94, 0,  -1,   -1),
+	MUX_PG(ld1,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 1,  0x94, 2,  -1,   -1),
+	MUX_PG(ld2,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 2,  0x94, 4,  -1,   -1),
+	MUX_PG(ld3,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 3,  0x94, 6,  -1,   -1),
+	MUX_PG(ld4,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 4,  0x94, 8,  -1,   -1),
+	MUX_PG(ld5,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 5,  0x94, 10, -1,   -1),
+	MUX_PG(ld6,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 6,  0x94, 12, -1,   -1),
+	MUX_PG(ld7,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 7,  0x94, 14, -1,   -1),
+	MUX_PG(ld8,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 8,  0x94, 16, -1,   -1),
+	MUX_PG(ld9,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 9,  0x94, 18, -1,   -1),
+	MUX_PG(ld10,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 10, 0x94, 20, -1,   -1),
+	MUX_PG(ld11,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 11, 0x94, 22, -1,   -1),
+	MUX_PG(ld12,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 12, 0x94, 24, -1,   -1),
+	MUX_PG(ld13,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 13, 0x94, 26, -1,   -1),
+	MUX_PG(ld14,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 14, 0x94, 28, -1,   -1),
+	MUX_PG(ld15,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 15, 0x94, 30, -1,   -1),
+	MUX_PG(ld16,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 16, 0x98, 0,  -1,   -1),
+	MUX_PG(ld17,   DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         0x1c, 17, 0x98, 2,  -1,   -1),
+	MUX_PG(ldc,    DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         0x1c, 30, 0x90, 14, -1,   -1),
+	MUX_PG(ldi,    DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         0x20, 6,  0x98, 16, -1,   -1),
+	MUX_PG(lhp0,   DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         0x1c, 18, 0x98, 10, -1,   -1),
+	MUX_PG(lhp1,   DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         0x1c, 19, 0x98, 4,  -1,   -1),
+	MUX_PG(lhp2,   DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         0x1c, 20, 0x98, 6,  -1,   -1),
+	MUX_PG(lhs,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x20, 7,  0x90, 22, -1,   -1),
+	MUX_PG(lm0,    DISPLAYA,  DISPLAYB,  SPI3,      RSVD4,         0x1c, 24, 0x90, 26, -1,   -1),
+	MUX_PG(lm1,    DISPLAYA,  DISPLAYB,  RSVD3,     CRT,           0x1c, 25, 0x90, 28, -1,   -1),
+	MUX_PG(lpp,    DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         0x20, 8,  0x98, 14, -1,   -1),
+	MUX_PG(lpw0,   DISPLAYA,  DISPLAYB,  SPI3,      HDMI,          0x20, 3,  0x90, 0,  -1,   -1),
+	MUX_PG(lpw1,   DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         0x20, 4,  0x90, 2,  -1,   -1),
+	MUX_PG(lpw2,   DISPLAYA,  DISPLAYB,  SPI3,      HDMI,          0x20, 5,  0x90, 4,  -1,   -1),
+	MUX_PG(lsc0,   DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 27, 0x90, 18, -1,   -1),
+	MUX_PG(lsc1,   DISPLAYA,  DISPLAYB,  SPI3,      HDMI,          0x1c, 28, 0x90, 20, -1,   -1),
+	MUX_PG(lsck,   DISPLAYA,  DISPLAYB,  SPI3,      HDMI,          0x1c, 29, 0x90, 16, -1,   -1),
+	MUX_PG(lsda,   DISPLAYA,  DISPLAYB,  SPI3,      HDMI,          0x20, 1,  0x90, 8,  -1,   -1),
+	MUX_PG(lsdi,   DISPLAYA,  DISPLAYB,  SPI3,      RSVD4,         0x20, 2,  0x90, 6,  -1,   -1),
+	MUX_PG(lspi,   DISPLAYA,  DISPLAYB,  XIO,       HDMI,          0x20, 0,  0x90, 10, -1,   -1),
+	MUX_PG(lvp0,   DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         0x1c, 21, 0x90, 30, -1,   -1),
+	MUX_PG(lvp1,   DISPLAYA,  DISPLAYB,  RSVD3,     RSVD4,         0x1c, 22, 0x98, 8,  -1,   -1),
+	MUX_PG(lvs,    DISPLAYA,  DISPLAYB,  XIO,       RSVD4,         0x1c, 26, 0x90, 24, -1,   -1),
+	MUX_PG(owc,    OWR,       RSVD2,     RSVD3,     RSVD4,         0x14, 31, 0x84, 8,  0xb0, 30),
+	MUX_PG(pmc,    PWR_ON,    PWR_INTR,  RSVD3,     RSVD4,         0x14, 23, 0x98, 18, -1,   -1),
+	MUX_PG(pta,    I2C2,      HDMI,      GMI,       RSVD4,         0x14, 24, 0x98, 22, 0xa4, 4),
+	MUX_PG(rm,     I2C1,      RSVD2,     RSVD3,     RSVD4,         0x14, 25, 0x80, 14, 0xa4, 0),
+	MUX_PG(sdb,    UARTA,     PWM,       SDIO3,     SPI2,          0x20, 15, 0x8c, 10, -1,   -1),
+	MUX_PG(sdc,    PWM,       TWC,       SDIO3,     SPI3,          0x18, 1,  0x8c, 12, 0xac, 28),
+	MUX_PG(sdd,    UARTA,     PWM,       SDIO3,     SPI3,          0x18, 2,  0x8c, 14, 0xac, 30),
+	MUX_PG(sdio1,  SDIO1,     RSVD2,     UARTE,     UARTA,         0x14, 30, 0x80, 30, 0xb0, 18),
+	MUX_PG(slxa,   PCIE,      SPI4,      SDIO3,     SPI2,          0x18, 3,  0x84, 6,  0xa4, 22),
+	MUX_PG(slxc,   SPDIF,     SPI4,      SDIO3,     SPI2,          0x18, 5,  0x84, 10, 0xa4, 26),
+	MUX_PG(slxd,   SPDIF,     SPI4,      SDIO3,     SPI2,          0x18, 6,  0x84, 12, 0xa4, 28),
+	MUX_PG(slxk,   PCIE,      SPI4,      SDIO3,     SPI2,          0x18, 7,  0x84, 14, 0xa4, 30),
+	MUX_PG(spdi,   SPDIF,     RSVD2,     I2C1,      SDIO2,         0x18, 8,  0x8c, 8,  0xa4, 16),
+	MUX_PG(spdo,   SPDIF,     RSVD2,     I2C1,      SDIO2,         0x18, 9,  0x8c, 6,  0xa4, 18),
+	MUX_PG(spia,   SPI1,      SPI2,      SPI3,      GMI,           0x18, 10, 0x8c, 30, 0xa8, 4),
+	MUX_PG(spib,   SPI1,      SPI2,      SPI3,      GMI,           0x18, 11, 0x8c, 28, 0xa8, 6),
+	MUX_PG(spic,   SPI1,      SPI2,      SPI3,      GMI,           0x18, 12, 0x8c, 26, 0xa8, 8),
+	MUX_PG(spid,   SPI2,      SPI1,      SPI2_ALT,  GMI,           0x18, 13, 0x8c, 24, 0xa8, 10),
+	MUX_PG(spie,   SPI2,      SPI1,      SPI2_ALT,  GMI,           0x18, 14, 0x8c, 22, 0xa8, 12),
+	MUX_PG(spif,   SPI3,      SPI1,      SPI2,      RSVD4,         0x18, 15, 0x8c, 20, 0xa8, 14),
+	MUX_PG(spig,   SPI3,      SPI2,      SPI2_ALT,  I2C1,          0x18, 16, 0x8c, 18, 0xa8, 16),
+	MUX_PG(spih,   SPI3,      SPI2,      SPI2_ALT,  I2C1,          0x18, 17, 0x8c, 16, 0xa8, 18),
+	MUX_PG(uaa,    SPI3,      MIPI_HS,   UARTA,     ULPI,          0x18, 18, 0x80, 0,  0xac, 0),
+	MUX_PG(uab,    SPI2,      MIPI_HS,   UARTA,     ULPI,          0x18, 19, 0x80, 2,  0xac, 2),
+	MUX_PG(uac,    OWR,       RSVD2,     RSVD3,     RSVD4,         0x18, 20, 0x80, 4,  0xac, 4),
+	MUX_PG(uad,    IRDA,      SPDIF,     UARTA,     SPI4,          0x18, 21, 0x80, 6,  0xac, 6),
+	MUX_PG(uca,    UARTC,     RSVD2,     GMI,       RSVD4,         0x18, 22, 0x84, 16, 0xac, 8),
+	MUX_PG(ucb,    UARTC,     PWM,       GMI,       RSVD4,         0x18, 23, 0x84, 18, 0xac, 10),
+	MUX_PG(uda,    SPI1,      RSVD2,     UARTD,     ULPI,          0x20, 13, 0x80, 8,  0xb0, 16),
 	/*      pg_name, pupd_r/b */
 	PULL_PG(ck32,    0xb0, 14),
 	PULL_PG(ddrc,    0xac, 26),
diff --git a/drivers/pinctrl/pinctrl-tegra30.c b/drivers/pinctrl/pinctrl-tegra30.c
index 41d24f5c285..fe2d2cf78ad 100644
--- a/drivers/pinctrl/pinctrl-tegra30.c
+++ b/drivers/pinctrl/pinctrl-tegra30.c
@@ -2108,10 +2108,12 @@ static struct tegra_function tegra30_functions[] = {
 #define DRV_PINGROUP_REG_A		0x868	/* bank 0 */
 #define PINGROUP_REG_A			0x3000	/* bank 1 */
 
-#define PINGROUP_REG_Y(r)		((r) - PINGROUP_REG_A)
-#define PINGROUP_REG_N(r)		-1
+#define PINGROUP_REG(r)			((r) - PINGROUP_REG_A)
 
-#define PINGROUP(pg_name, f0, f1, f2, f3, f_safe, r, od, ior)	\
+#define PINGROUP_BIT_Y(b)		(b)
+#define PINGROUP_BIT_N(b)		(-1)
+
+#define PINGROUP(pg_name, f0, f1, f2, f3, r, od, ior)		\
 	{							\
 		.name = #pg_name,				\
 		.pins = pg_name##_pins,				\
@@ -2122,35 +2124,24 @@ static struct tegra_function tegra30_functions[] = {
 			TEGRA_MUX_##f2,				\
 			TEGRA_MUX_##f3,				\
 		},						\
-		.func_safe = TEGRA_MUX_##f_safe,		\
-		.mux_reg = PINGROUP_REG_Y(r),			\
+		.mux_reg = PINGROUP_REG(r),			\
 		.mux_bank = 1,					\
 		.mux_bit = 0,					\
-		.pupd_reg = PINGROUP_REG_Y(r),			\
+		.pupd_reg = PINGROUP_REG(r),			\
 		.pupd_bank = 1,					\
 		.pupd_bit = 2,					\
-		.tri_reg = PINGROUP_REG_Y(r),			\
+		.tri_reg = PINGROUP_REG(r),			\
 		.tri_bank = 1,					\
 		.tri_bit = 4,					\
-		.einput_reg = PINGROUP_REG_Y(r),		\
-		.einput_bank = 1,				\
-		.einput_bit = 5,				\
-		.odrain_reg = PINGROUP_REG_##od(r),		\
-		.odrain_bank = 1,				\
-		.odrain_bit = 6,				\
-		.lock_reg = PINGROUP_REG_Y(r),			\
-		.lock_bank = 1,					\
-		.lock_bit = 7,					\
-		.ioreset_reg = PINGROUP_REG_##ior(r),		\
-		.ioreset_bank = 1,				\
-		.ioreset_bit = 8,				\
-		.rcv_sel_reg = -1,				\
+		.einput_bit = PINGROUP_BIT_Y(5),		\
+		.odrain_bit = PINGROUP_BIT_##od(6),		\
+		.lock_bit = PINGROUP_BIT_Y(7),			\
+		.ioreset_bit = PINGROUP_BIT_##ior(8),		\
+		.rcv_sel_bit = -1,				\
 		.drv_reg = -1,					\
-		.drvtype_reg = -1,				\
 	}
 
-#define DRV_PINGROUP_REG_Y(r)		((r) - DRV_PINGROUP_REG_A)
-#define DRV_PINGROUP_REG_N(r)		-1
+#define DRV_PINGROUP_REG(r)		((r) - DRV_PINGROUP_REG_A)
 
 #define DRV_PINGROUP(pg_name, r, hsm_b, schmitt_b, lpmd_b,	\
 		     drvdn_b, drvdn_w, drvup_b, drvup_w,	\
@@ -2162,12 +2153,12 @@ static struct tegra_function tegra30_functions[] = {
 		.mux_reg = -1,					\
 		.pupd_reg = -1,					\
 		.tri_reg = -1,					\
-		.einput_reg = -1,				\
-		.odrain_reg = -1,				\
-		.lock_reg = -1,					\
-		.ioreset_reg = -1,				\
-		.rcv_sel_reg = -1,				\
-		.drv_reg = DRV_PINGROUP_REG_Y(r),		\
+		.einput_bit = -1,				\
+		.odrain_bit = -1,				\
+		.lock_bit = -1,					\
+		.ioreset_bit = -1,				\
+		.rcv_sel_bit = -1,				\
+		.drv_reg = DRV_PINGROUP_REG(r),			\
 		.drv_bank = 0,					\
 		.hsm_bit = hsm_b,				\
 		.schmitt_bit = schmitt_b,			\
@@ -2180,260 +2171,260 @@ static struct tegra_function tegra30_functions[] = {
 		.slwr_width = slwr_w,				\
 		.slwf_bit = slwf_b,				\
 		.slwf_width = slwf_w,				\
-		.drvtype_reg = -1,				\
+		.drvtype_bit = -1,				\
 	}
 
 static const struct tegra_pingroup tegra30_groups[] = {
-	/*       pg_name,              f0,           f1,           f2,           f3,           safe,         r,      od, ior */
-	PINGROUP(clk_32k_out_pa0,      BLINK,        RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x331c, N, N),
-	PINGROUP(uart3_cts_n_pa1,      UARTC,        RSVD2,        GMI,          RSVD4,        RSVD4,        0x317c, N, N),
-	PINGROUP(dap2_fs_pa2,          I2S1,         HDA,          RSVD3,        GMI,          RSVD3,        0x3358, N, N),
-	PINGROUP(dap2_sclk_pa3,        I2S1,         HDA,          RSVD3,        GMI,          RSVD3,        0x3364, N, N),
-	PINGROUP(dap2_din_pa4,         I2S1,         HDA,          RSVD3,        GMI,          RSVD3,        0x335c, N, N),
-	PINGROUP(dap2_dout_pa5,        I2S1,         HDA,          RSVD3,        GMI,          RSVD3,        0x3360, N, N),
-	PINGROUP(sdmmc3_clk_pa6,       UARTA,        PWM2,         SDMMC3,       SPI3,         SPI3,         0x3390, N, N),
-	PINGROUP(sdmmc3_cmd_pa7,       UARTA,        PWM3,         SDMMC3,       SPI2,         SPI2,         0x3394, N, N),
-	PINGROUP(gmi_a17_pb0,          UARTD,        SPI4,         GMI,          DTV,          DTV,          0x3234, N, N),
-	PINGROUP(gmi_a18_pb1,          UARTD,        SPI4,         GMI,          DTV,          DTV,          0x3238, N, N),
-	PINGROUP(lcd_pwr0_pb2,         DISPLAYA,     DISPLAYB,     SPI5,         HDCP,         HDCP,         0x3090, N, N),
-	PINGROUP(lcd_pclk_pb3,         DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x3094, N, N),
-	PINGROUP(sdmmc3_dat3_pb4,      RSVD1,        PWM0,         SDMMC3,       SPI3,         RSVD1,        0x33a4, N, N),
-	PINGROUP(sdmmc3_dat2_pb5,      RSVD1,        PWM1,         SDMMC3,       SPI3,         RSVD1,        0x33a0, N, N),
-	PINGROUP(sdmmc3_dat1_pb6,      RSVD1,        RSVD2,        SDMMC3,       SPI3,         RSVD2,        0x339c, N, N),
-	PINGROUP(sdmmc3_dat0_pb7,      RSVD1,        RSVD2,        SDMMC3,       SPI3,         RSVD2,        0x3398, N, N),
-	PINGROUP(uart3_rts_n_pc0,      UARTC,        PWM0,         GMI,          RSVD4,        RSVD4,        0x3180, N, N),
-	PINGROUP(lcd_pwr1_pc1,         DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x3070, N, N),
-	PINGROUP(uart2_txd_pc2,        UARTB,        SPDIF,        UARTA,        SPI4,         SPI4,         0x3168, N, N),
-	PINGROUP(uart2_rxd_pc3,        UARTB,        SPDIF,        UARTA,        SPI4,         SPI4,         0x3164, N, N),
-	PINGROUP(gen1_i2c_scl_pc4,     I2C1,         RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x31a4, Y, N),
-	PINGROUP(gen1_i2c_sda_pc5,     I2C1,         RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x31a0, Y, N),
-	PINGROUP(lcd_pwr2_pc6,         DISPLAYA,     DISPLAYB,     SPI5,         HDCP,         HDCP,         0x3074, N, N),
-	PINGROUP(gmi_wp_n_pc7,         RSVD1,        NAND,         GMI,          GMI_ALT,      RSVD1,        0x31c0, N, N),
-	PINGROUP(sdmmc3_dat5_pd0,      PWM0,         SPI4,         SDMMC3,       SPI2,         SPI2,         0x33ac, N, N),
-	PINGROUP(sdmmc3_dat4_pd1,      PWM1,         SPI4,         SDMMC3,       SPI2,         SPI2,         0x33a8, N, N),
-	PINGROUP(lcd_dc1_pd2,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x310c, N, N),
-	PINGROUP(sdmmc3_dat6_pd3,      SPDIF,        SPI4,         SDMMC3,       SPI2,         SPI2,         0x33b0, N, N),
-	PINGROUP(sdmmc3_dat7_pd4,      SPDIF,        SPI4,         SDMMC3,       SPI2,         SPI2,         0x33b4, N, N),
-	PINGROUP(vi_d1_pd5,            DDR,          SDMMC2,       VI,           RSVD4,        RSVD4,        0x3128, N, Y),
-	PINGROUP(vi_vsync_pd6,         DDR,          RSVD2,        VI,           RSVD4,        RSVD4,        0x315c, N, Y),
-	PINGROUP(vi_hsync_pd7,         DDR,          RSVD2,        VI,           RSVD4,        RSVD4,        0x3160, N, Y),
-	PINGROUP(lcd_d0_pe0,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30a4, N, N),
-	PINGROUP(lcd_d1_pe1,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30a8, N, N),
-	PINGROUP(lcd_d2_pe2,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30ac, N, N),
-	PINGROUP(lcd_d3_pe3,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30b0, N, N),
-	PINGROUP(lcd_d4_pe4,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30b4, N, N),
-	PINGROUP(lcd_d5_pe5,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30b8, N, N),
-	PINGROUP(lcd_d6_pe6,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30bc, N, N),
-	PINGROUP(lcd_d7_pe7,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30c0, N, N),
-	PINGROUP(lcd_d8_pf0,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30c4, N, N),
-	PINGROUP(lcd_d9_pf1,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30c8, N, N),
-	PINGROUP(lcd_d10_pf2,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30cc, N, N),
-	PINGROUP(lcd_d11_pf3,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30d0, N, N),
-	PINGROUP(lcd_d12_pf4,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30d4, N, N),
-	PINGROUP(lcd_d13_pf5,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30d8, N, N),
-	PINGROUP(lcd_d14_pf6,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30dc, N, N),
-	PINGROUP(lcd_d15_pf7,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30e0, N, N),
-	PINGROUP(gmi_ad0_pg0,          RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x31f0, N, N),
-	PINGROUP(gmi_ad1_pg1,          RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x31f4, N, N),
-	PINGROUP(gmi_ad2_pg2,          RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x31f8, N, N),
-	PINGROUP(gmi_ad3_pg3,          RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x31fc, N, N),
-	PINGROUP(gmi_ad4_pg4,          RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x3200, N, N),
-	PINGROUP(gmi_ad5_pg5,          RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x3204, N, N),
-	PINGROUP(gmi_ad6_pg6,          RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x3208, N, N),
-	PINGROUP(gmi_ad7_pg7,          RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x320c, N, N),
-	PINGROUP(gmi_ad8_ph0,          PWM0,         NAND,         GMI,          RSVD4,        RSVD4,        0x3210, N, N),
-	PINGROUP(gmi_ad9_ph1,          PWM1,         NAND,         GMI,          RSVD4,        RSVD4,        0x3214, N, N),
-	PINGROUP(gmi_ad10_ph2,         PWM2,         NAND,         GMI,          RSVD4,        RSVD4,        0x3218, N, N),
-	PINGROUP(gmi_ad11_ph3,         PWM3,         NAND,         GMI,          RSVD4,        RSVD4,        0x321c, N, N),
-	PINGROUP(gmi_ad12_ph4,         RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x3220, N, N),
-	PINGROUP(gmi_ad13_ph5,         RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x3224, N, N),
-	PINGROUP(gmi_ad14_ph6,         RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x3228, N, N),
-	PINGROUP(gmi_ad15_ph7,         RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x322c, N, N),
-	PINGROUP(gmi_wr_n_pi0,         RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x3240, N, N),
-	PINGROUP(gmi_oe_n_pi1,         RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x3244, N, N),
-	PINGROUP(gmi_dqs_pi2,          RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x3248, N, N),
-	PINGROUP(gmi_cs6_n_pi3,        NAND,         NAND_ALT,     GMI,          SATA,         SATA,         0x31e8, N, N),
-	PINGROUP(gmi_rst_n_pi4,        NAND,         NAND_ALT,     GMI,          RSVD4,        RSVD4,        0x324c, N, N),
-	PINGROUP(gmi_iordy_pi5,        RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x31c4, N, N),
-	PINGROUP(gmi_cs7_n_pi6,        NAND,         NAND_ALT,     GMI,          GMI_ALT,      GMI_ALT,      0x31ec, N, N),
-	PINGROUP(gmi_wait_pi7,         RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x31c8, N, N),
-	PINGROUP(gmi_cs0_n_pj0,        RSVD1,        NAND,         GMI,          DTV,          RSVD1,        0x31d4, N, N),
-	PINGROUP(lcd_de_pj1,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x3098, N, N),
-	PINGROUP(gmi_cs1_n_pj2,        RSVD1,        NAND,         GMI,          DTV,          RSVD1,        0x31d8, N, N),
-	PINGROUP(lcd_hsync_pj3,        DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x309c, N, N),
-	PINGROUP(lcd_vsync_pj4,        DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30a0, N, N),
-	PINGROUP(uart2_cts_n_pj5,      UARTA,        UARTB,        GMI,          SPI4,         SPI4,         0x3170, N, N),
-	PINGROUP(uart2_rts_n_pj6,      UARTA,        UARTB,        GMI,          SPI4,         SPI4,         0x316c, N, N),
-	PINGROUP(gmi_a16_pj7,          UARTD,        SPI4,         GMI,          GMI_ALT,      GMI_ALT,      0x3230, N, N),
-	PINGROUP(gmi_adv_n_pk0,        RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x31cc, N, N),
-	PINGROUP(gmi_clk_pk1,          RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x31d0, N, N),
-	PINGROUP(gmi_cs4_n_pk2,        RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x31e4, N, N),
-	PINGROUP(gmi_cs2_n_pk3,        RSVD1,        NAND,         GMI,          RSVD4,        RSVD4,        0x31dc, N, N),
-	PINGROUP(gmi_cs3_n_pk4,        RSVD1,        NAND,         GMI,          GMI_ALT,      RSVD1,        0x31e0, N, N),
-	PINGROUP(spdif_out_pk5,        SPDIF,        RSVD2,        I2C1,         SDMMC2,       RSVD2,        0x3354, N, N),
-	PINGROUP(spdif_in_pk6,         SPDIF,        HDA,          I2C1,         SDMMC2,       SDMMC2,       0x3350, N, N),
-	PINGROUP(gmi_a19_pk7,          UARTD,        SPI4,         GMI,          RSVD4,        RSVD4,        0x323c, N, N),
-	PINGROUP(vi_d2_pl0,            DDR,          SDMMC2,       VI,           RSVD4,        RSVD4,        0x312c, N, Y),
-	PINGROUP(vi_d3_pl1,            DDR,          SDMMC2,       VI,           RSVD4,        RSVD4,        0x3130, N, Y),
-	PINGROUP(vi_d4_pl2,            DDR,          SDMMC2,       VI,           RSVD4,        RSVD4,        0x3134, N, Y),
-	PINGROUP(vi_d5_pl3,            DDR,          SDMMC2,       VI,           RSVD4,        RSVD4,        0x3138, N, Y),
-	PINGROUP(vi_d6_pl4,            DDR,          SDMMC2,       VI,           RSVD4,        RSVD4,        0x313c, N, Y),
-	PINGROUP(vi_d7_pl5,            DDR,          SDMMC2,       VI,           RSVD4,        RSVD4,        0x3140, N, Y),
-	PINGROUP(vi_d8_pl6,            DDR,          SDMMC2,       VI,           RSVD4,        RSVD4,        0x3144, N, Y),
-	PINGROUP(vi_d9_pl7,            DDR,          SDMMC2,       VI,           RSVD4,        RSVD4,        0x3148, N, Y),
-	PINGROUP(lcd_d16_pm0,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30e4, N, N),
-	PINGROUP(lcd_d17_pm1,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30e8, N, N),
-	PINGROUP(lcd_d18_pm2,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30ec, N, N),
-	PINGROUP(lcd_d19_pm3,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30f0, N, N),
-	PINGROUP(lcd_d20_pm4,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30f4, N, N),
-	PINGROUP(lcd_d21_pm5,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30f8, N, N),
-	PINGROUP(lcd_d22_pm6,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x30fc, N, N),
-	PINGROUP(lcd_d23_pm7,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x3100, N, N),
-	PINGROUP(dap1_fs_pn0,          I2S0,         HDA,          GMI,          SDMMC2,       SDMMC2,       0x3338, N, N),
-	PINGROUP(dap1_din_pn1,         I2S0,         HDA,          GMI,          SDMMC2,       SDMMC2,       0x333c, N, N),
-	PINGROUP(dap1_dout_pn2,        I2S0,         HDA,          GMI,          SDMMC2,       SDMMC2,       0x3340, N, N),
-	PINGROUP(dap1_sclk_pn3,        I2S0,         HDA,          GMI,          SDMMC2,       SDMMC2,       0x3344, N, N),
-	PINGROUP(lcd_cs0_n_pn4,        DISPLAYA,     DISPLAYB,     SPI5,         RSVD4,        RSVD4,        0x3084, N, N),
-	PINGROUP(lcd_sdout_pn5,        DISPLAYA,     DISPLAYB,     SPI5,         HDCP,         HDCP,         0x307c, N, N),
-	PINGROUP(lcd_dc0_pn6,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x3088, N, N),
-	PINGROUP(hdmi_int_pn7,         HDMI,         RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x3110, N, N),
-	PINGROUP(ulpi_data7_po0,       SPI2,         HSI,          UARTA,        ULPI,         ULPI,         0x301c, N, N),
-	PINGROUP(ulpi_data0_po1,       SPI3,         HSI,          UARTA,        ULPI,         ULPI,         0x3000, N, N),
-	PINGROUP(ulpi_data1_po2,       SPI3,         HSI,          UARTA,        ULPI,         ULPI,         0x3004, N, N),
-	PINGROUP(ulpi_data2_po3,       SPI3,         HSI,          UARTA,        ULPI,         ULPI,         0x3008, N, N),
-	PINGROUP(ulpi_data3_po4,       SPI3,         HSI,          UARTA,        ULPI,         ULPI,         0x300c, N, N),
-	PINGROUP(ulpi_data4_po5,       SPI2,         HSI,          UARTA,        ULPI,         ULPI,         0x3010, N, N),
-	PINGROUP(ulpi_data5_po6,       SPI2,         HSI,          UARTA,        ULPI,         ULPI,         0x3014, N, N),
-	PINGROUP(ulpi_data6_po7,       SPI2,         HSI,          UARTA,        ULPI,         ULPI,         0x3018, N, N),
-	PINGROUP(dap3_fs_pp0,          I2S2,         RSVD2,        DISPLAYA,     DISPLAYB,     RSVD2,        0x3030, N, N),
-	PINGROUP(dap3_din_pp1,         I2S2,         RSVD2,        DISPLAYA,     DISPLAYB,     RSVD2,        0x3034, N, N),
-	PINGROUP(dap3_dout_pp2,        I2S2,         RSVD2,        DISPLAYA,     DISPLAYB,     RSVD2,        0x3038, N, N),
-	PINGROUP(dap3_sclk_pp3,        I2S2,         RSVD2,        DISPLAYA,     DISPLAYB,     RSVD2,        0x303c, N, N),
-	PINGROUP(dap4_fs_pp4,          I2S3,         RSVD2,        GMI,          RSVD4,        RSVD4,        0x31a8, N, N),
-	PINGROUP(dap4_din_pp5,         I2S3,         RSVD2,        GMI,          RSVD4,        RSVD4,        0x31ac, N, N),
-	PINGROUP(dap4_dout_pp6,        I2S3,         RSVD2,        GMI,          RSVD4,        RSVD4,        0x31b0, N, N),
-	PINGROUP(dap4_sclk_pp7,        I2S3,         RSVD2,        GMI,          RSVD4,        RSVD4,        0x31b4, N, N),
-	PINGROUP(kb_col0_pq0,          KBC,          NAND,         TRACE,        TEST,         TEST,         0x32fc, N, N),
-	PINGROUP(kb_col1_pq1,          KBC,          NAND,         TRACE,        TEST,         TEST,         0x3300, N, N),
-	PINGROUP(kb_col2_pq2,          KBC,          NAND,         TRACE,        RSVD4,        RSVD4,        0x3304, N, N),
-	PINGROUP(kb_col3_pq3,          KBC,          NAND,         TRACE,        RSVD4,        RSVD4,        0x3308, N, N),
-	PINGROUP(kb_col4_pq4,          KBC,          NAND,         TRACE,        RSVD4,        RSVD4,        0x330c, N, N),
-	PINGROUP(kb_col5_pq5,          KBC,          NAND,         TRACE,        RSVD4,        RSVD4,        0x3310, N, N),
-	PINGROUP(kb_col6_pq6,          KBC,          NAND,         TRACE,        MIO,          MIO,          0x3314, N, N),
-	PINGROUP(kb_col7_pq7,          KBC,          NAND,         TRACE,        MIO,          MIO,          0x3318, N, N),
-	PINGROUP(kb_row0_pr0,          KBC,          NAND,         RSVD3,        RSVD4,        RSVD4,        0x32bc, N, N),
-	PINGROUP(kb_row1_pr1,          KBC,          NAND,         RSVD3,        RSVD4,        RSVD4,        0x32c0, N, N),
-	PINGROUP(kb_row2_pr2,          KBC,          NAND,         RSVD3,        RSVD4,        RSVD4,        0x32c4, N, N),
-	PINGROUP(kb_row3_pr3,          KBC,          NAND,         RSVD3,        INVALID,      RSVD3,        0x32c8, N, N),
-	PINGROUP(kb_row4_pr4,          KBC,          NAND,         TRACE,        RSVD4,        RSVD4,        0x32cc, N, N),
-	PINGROUP(kb_row5_pr5,          KBC,          NAND,         TRACE,        OWR,          OWR,          0x32d0, N, N),
-	PINGROUP(kb_row6_pr6,          KBC,          NAND,         SDMMC2,       MIO,          MIO,          0x32d4, N, N),
-	PINGROUP(kb_row7_pr7,          KBC,          NAND,         SDMMC2,       MIO,          MIO,          0x32d8, N, N),
-	PINGROUP(kb_row8_ps0,          KBC,          NAND,         SDMMC2,       MIO,          MIO,          0x32dc, N, N),
-	PINGROUP(kb_row9_ps1,          KBC,          NAND,         SDMMC2,       MIO,          MIO,          0x32e0, N, N),
-	PINGROUP(kb_row10_ps2,         KBC,          NAND,         SDMMC2,       MIO,          MIO,          0x32e4, N, N),
-	PINGROUP(kb_row11_ps3,         KBC,          NAND,         SDMMC2,       MIO,          MIO,          0x32e8, N, N),
-	PINGROUP(kb_row12_ps4,         KBC,          NAND,         SDMMC2,       MIO,          MIO,          0x32ec, N, N),
-	PINGROUP(kb_row13_ps5,         KBC,          NAND,         SDMMC2,       MIO,          MIO,          0x32f0, N, N),
-	PINGROUP(kb_row14_ps6,         KBC,          NAND,         SDMMC2,       MIO,          MIO,          0x32f4, N, N),
-	PINGROUP(kb_row15_ps7,         KBC,          NAND,         SDMMC2,       MIO,          MIO,          0x32f8, N, N),
-	PINGROUP(vi_pclk_pt0,          RSVD1,        SDMMC2,       VI,           RSVD4,        RSVD4,        0x3154, N, Y),
-	PINGROUP(vi_mclk_pt1,          VI,           VI_ALT1,      VI_ALT2,      VI_ALT3,      VI_ALT3,      0x3158, N, Y),
-	PINGROUP(vi_d10_pt2,           DDR,          RSVD2,        VI,           RSVD4,        RSVD4,        0x314c, N, Y),
-	PINGROUP(vi_d11_pt3,           DDR,          RSVD2,        VI,           RSVD4,        RSVD4,        0x3150, N, Y),
-	PINGROUP(vi_d0_pt4,            DDR,          RSVD2,        VI,           RSVD4,        RSVD4,        0x3124, N, Y),
-	PINGROUP(gen2_i2c_scl_pt5,     I2C2,         HDCP,         GMI,          RSVD4,        RSVD4,        0x3250, Y, N),
-	PINGROUP(gen2_i2c_sda_pt6,     I2C2,         HDCP,         GMI,          RSVD4,        RSVD4,        0x3254, Y, N),
-	PINGROUP(sdmmc4_cmd_pt7,       I2C3,         NAND,         GMI,          SDMMC4,       SDMMC4,       0x325c, N, Y),
-	PINGROUP(pu0,                  OWR,          UARTA,        GMI,          RSVD4,        RSVD4,        0x3184, N, N),
-	PINGROUP(pu1,                  RSVD1,        UARTA,        GMI,          RSVD4,        RSVD4,        0x3188, N, N),
-	PINGROUP(pu2,                  RSVD1,        UARTA,        GMI,          RSVD4,        RSVD4,        0x318c, N, N),
-	PINGROUP(pu3,                  PWM0,         UARTA,        GMI,          RSVD4,        RSVD4,        0x3190, N, N),
-	PINGROUP(pu4,                  PWM1,         UARTA,        GMI,          RSVD4,        RSVD4,        0x3194, N, N),
-	PINGROUP(pu5,                  PWM2,         UARTA,        GMI,          RSVD4,        RSVD4,        0x3198, N, N),
-	PINGROUP(pu6,                  PWM3,         UARTA,        GMI,          RSVD4,        RSVD4,        0x319c, N, N),
-	PINGROUP(jtag_rtck_pu7,        RTCK,         RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x32b0, N, N),
-	PINGROUP(pv0,                  RSVD1,        RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x3040, N, N),
-	PINGROUP(pv1,                  RSVD1,        RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x3044, N, N),
-	PINGROUP(pv2,                  OWR,          RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x3060, N, N),
-	PINGROUP(pv3,                  CLK_12M_OUT,  RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x3064, N, N),
-	PINGROUP(ddc_scl_pv4,          I2C4,         RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x3114, N, N),
-	PINGROUP(ddc_sda_pv5,          I2C4,         RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x3118, N, N),
-	PINGROUP(crt_hsync_pv6,        CRT,          RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x311c, N, N),
-	PINGROUP(crt_vsync_pv7,        CRT,          RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x3120, N, N),
-	PINGROUP(lcd_cs1_n_pw0,        DISPLAYA,     DISPLAYB,     SPI5,         RSVD4,        RSVD4,        0x3104, N, N),
-	PINGROUP(lcd_m1_pw1,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        RSVD4,        0x3108, N, N),
-	PINGROUP(spi2_cs1_n_pw2,       SPI3,         SPI2,         SPI2_ALT,     I2C1,         I2C1,         0x3388, N, N),
-	PINGROUP(spi2_cs2_n_pw3,       SPI3,         SPI2,         SPI2_ALT,     I2C1,         I2C1,         0x338c, N, N),
-	PINGROUP(clk1_out_pw4,         EXTPERIPH1,   RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x334c, N, N),
-	PINGROUP(clk2_out_pw5,         EXTPERIPH2,   RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x3068, N, N),
-	PINGROUP(uart3_txd_pw6,        UARTC,        RSVD2,        GMI,          RSVD4,        RSVD4,        0x3174, N, N),
-	PINGROUP(uart3_rxd_pw7,        UARTC,        RSVD2,        GMI,          RSVD4,        RSVD4,        0x3178, N, N),
-	PINGROUP(spi2_mosi_px0,        SPI6,         SPI2,         SPI3,         GMI,          GMI,          0x3368, N, N),
-	PINGROUP(spi2_miso_px1,        SPI6,         SPI2,         SPI3,         GMI,          GMI,          0x336c, N, N),
-	PINGROUP(spi2_sck_px2,         SPI6,         SPI2,         SPI3,         GMI,          GMI,          0x3374, N, N),
-	PINGROUP(spi2_cs0_n_px3,       SPI6,         SPI2,         SPI3,         GMI,          GMI,          0x3370, N, N),
-	PINGROUP(spi1_mosi_px4,        SPI2,         SPI1,         SPI2_ALT,     GMI,          GMI,          0x3378, N, N),
-	PINGROUP(spi1_sck_px5,         SPI2,         SPI1,         SPI2_ALT,     GMI,          GMI,          0x337c, N, N),
-	PINGROUP(spi1_cs0_n_px6,       SPI2,         SPI1,         SPI2_ALT,     GMI,          GMI,          0x3380, N, N),
-	PINGROUP(spi1_miso_px7,        SPI3,         SPI1,         SPI2_ALT,     RSVD4,        RSVD4,        0x3384, N, N),
-	PINGROUP(ulpi_clk_py0,         SPI1,         RSVD2,        UARTD,        ULPI,         RSVD2,        0x3020, N, N),
-	PINGROUP(ulpi_dir_py1,         SPI1,         RSVD2,        UARTD,        ULPI,         RSVD2,        0x3024, N, N),
-	PINGROUP(ulpi_nxt_py2,         SPI1,         RSVD2,        UARTD,        ULPI,         RSVD2,        0x3028, N, N),
-	PINGROUP(ulpi_stp_py3,         SPI1,         RSVD2,        UARTD,        ULPI,         RSVD2,        0x302c, N, N),
-	PINGROUP(sdmmc1_dat3_py4,      SDMMC1,       RSVD2,        UARTE,        UARTA,        RSVD2,        0x3050, N, N),
-	PINGROUP(sdmmc1_dat2_py5,      SDMMC1,       RSVD2,        UARTE,        UARTA,        RSVD2,        0x3054, N, N),
-	PINGROUP(sdmmc1_dat1_py6,      SDMMC1,       RSVD2,        UARTE,        UARTA,        RSVD2,        0x3058, N, N),
-	PINGROUP(sdmmc1_dat0_py7,      SDMMC1,       RSVD2,        UARTE,        UARTA,        RSVD2,        0x305c, N, N),
-	PINGROUP(sdmmc1_clk_pz0,       SDMMC1,       RSVD2,        RSVD3,        UARTA,        RSVD3,        0x3048, N, N),
-	PINGROUP(sdmmc1_cmd_pz1,       SDMMC1,       RSVD2,        RSVD3,        UARTA,        RSVD3,        0x304c, N, N),
-	PINGROUP(lcd_sdin_pz2,         DISPLAYA,     DISPLAYB,     SPI5,         RSVD4,        RSVD4,        0x3078, N, N),
-	PINGROUP(lcd_wr_n_pz3,         DISPLAYA,     DISPLAYB,     SPI5,         HDCP,         HDCP,         0x3080, N, N),
-	PINGROUP(lcd_sck_pz4,          DISPLAYA,     DISPLAYB,     SPI5,         HDCP,         HDCP,         0x308c, N, N),
-	PINGROUP(sys_clk_req_pz5,      SYSCLK,       RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x3320, N, N),
-	PINGROUP(pwr_i2c_scl_pz6,      I2CPWR,       RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x32b4, Y, N),
-	PINGROUP(pwr_i2c_sda_pz7,      I2CPWR,       RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x32b8, Y, N),
-	PINGROUP(sdmmc4_dat0_paa0,     UARTE,        SPI3,         GMI,          SDMMC4,       SDMMC4,       0x3260, N, Y),
-	PINGROUP(sdmmc4_dat1_paa1,     UARTE,        SPI3,         GMI,          SDMMC4,       SDMMC4,       0x3264, N, Y),
-	PINGROUP(sdmmc4_dat2_paa2,     UARTE,        SPI3,         GMI,          SDMMC4,       SDMMC4,       0x3268, N, Y),
-	PINGROUP(sdmmc4_dat3_paa3,     UARTE,        SPI3,         GMI,          SDMMC4,       SDMMC4,       0x326c, N, Y),
-	PINGROUP(sdmmc4_dat4_paa4,     I2C3,         I2S4,         GMI,          SDMMC4,       SDMMC4,       0x3270, N, Y),
-	PINGROUP(sdmmc4_dat5_paa5,     VGP3,         I2S4,         GMI,          SDMMC4,       SDMMC4,       0x3274, N, Y),
-	PINGROUP(sdmmc4_dat6_paa6,     VGP4,         I2S4,         GMI,          SDMMC4,       SDMMC4,       0x3278, N, Y),
-	PINGROUP(sdmmc4_dat7_paa7,     VGP5,         I2S4,         GMI,          SDMMC4,       SDMMC4,       0x327c, N, Y),
-	PINGROUP(pbb0,                 I2S4,         RSVD2,        RSVD3,        SDMMC4,       RSVD3,        0x328c, N, N),
-	PINGROUP(cam_i2c_scl_pbb1,     VGP1,         I2C3,         RSVD3,        SDMMC4,       RSVD3,        0x3290, Y, N),
-	PINGROUP(cam_i2c_sda_pbb2,     VGP2,         I2C3,         RSVD3,        SDMMC4,       RSVD3,        0x3294, Y, N),
-	PINGROUP(pbb3,                 VGP3,         DISPLAYA,     DISPLAYB,     SDMMC4,       SDMMC4,       0x3298, N, N),
-	PINGROUP(pbb4,                 VGP4,         DISPLAYA,     DISPLAYB,     SDMMC4,       SDMMC4,       0x329c, N, N),
-	PINGROUP(pbb5,                 VGP5,         DISPLAYA,     DISPLAYB,     SDMMC4,       SDMMC4,       0x32a0, N, N),
-	PINGROUP(pbb6,                 VGP6,         DISPLAYA,     DISPLAYB,     SDMMC4,       SDMMC4,       0x32a4, N, N),
-	PINGROUP(pbb7,                 I2S4,         RSVD2,        RSVD3,        SDMMC4,       RSVD3,        0x32a8, N, N),
-	PINGROUP(cam_mclk_pcc0,        VI,           VI_ALT1,      VI_ALT3,      SDMMC4,       SDMMC4,       0x3284, N, N),
-	PINGROUP(pcc1,                 I2S4,         RSVD2,        RSVD3,        SDMMC4,       RSVD3,        0x3288, N, N),
-	PINGROUP(pcc2,                 I2S4,         RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x32ac, N, N),
-	PINGROUP(sdmmc4_rst_n_pcc3,    VGP6,         RSVD2,        RSVD3,        SDMMC4,       RSVD3,        0x3280, N, Y),
-	PINGROUP(sdmmc4_clk_pcc4,      INVALID,      NAND,         GMI,          SDMMC4,       SDMMC4,       0x3258, N, Y),
-	PINGROUP(clk2_req_pcc5,        DAP,          RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x306c, N, N),
-	PINGROUP(pex_l2_rst_n_pcc6,    PCIE,         HDA,          RSVD3,        RSVD4,        RSVD4,        0x33d8, N, N),
-	PINGROUP(pex_l2_clkreq_n_pcc7, PCIE,         HDA,          RSVD3,        RSVD4,        RSVD4,        0x33dc, N, N),
-	PINGROUP(pex_l0_prsnt_n_pdd0,  PCIE,         HDA,          RSVD3,        RSVD4,        RSVD4,        0x33b8, N, N),
-	PINGROUP(pex_l0_rst_n_pdd1,    PCIE,         HDA,          RSVD3,        RSVD4,        RSVD4,        0x33bc, N, N),
-	PINGROUP(pex_l0_clkreq_n_pdd2, PCIE,         HDA,          RSVD3,        RSVD4,        RSVD4,        0x33c0, N, N),
-	PINGROUP(pex_wake_n_pdd3,      PCIE,         HDA,          RSVD3,        RSVD4,        RSVD4,        0x33c4, N, N),
-	PINGROUP(pex_l1_prsnt_n_pdd4,  PCIE,         HDA,          RSVD3,        RSVD4,        RSVD4,        0x33c8, N, N),
-	PINGROUP(pex_l1_rst_n_pdd5,    PCIE,         HDA,          RSVD3,        RSVD4,        RSVD4,        0x33cc, N, N),
-	PINGROUP(pex_l1_clkreq_n_pdd6, PCIE,         HDA,          RSVD3,        RSVD4,        RSVD4,        0x33d0, N, N),
-	PINGROUP(pex_l2_prsnt_n_pdd7,  PCIE,         HDA,          RSVD3,        RSVD4,        RSVD4,        0x33d4, N, N),
-	PINGROUP(clk3_out_pee0,        EXTPERIPH3,   RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x31b8, N, N),
-	PINGROUP(clk3_req_pee1,        DEV3,         RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x31bc, N, N),
-	PINGROUP(clk1_req_pee2,        DAP,          HDA,          RSVD3,        RSVD4,        RSVD4,        0x3348, N, N),
-	PINGROUP(hdmi_cec_pee3,        CEC,          RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x33e0, Y, N),
-	PINGROUP(clk_32k_in,           CLK_32K_IN,   RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x3330, N, N),
-	PINGROUP(core_pwr_req,         CORE_PWR_REQ, RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x3324, N, N),
-	PINGROUP(cpu_pwr_req,          CPU_PWR_REQ,  RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x3328, N, N),
-	PINGROUP(owr,                  OWR,          CEC,          RSVD3,        RSVD4,        RSVD4,        0x3334, N, N),
-	PINGROUP(pwr_int_n,            PWR_INT_N,    RSVD2,        RSVD3,        RSVD4,        RSVD4,        0x332c, N, N),
+	/*       pg_name,              f0,           f1,           f2,           f3,           r,      od, ior */
+	PINGROUP(clk_32k_out_pa0,      BLINK,        RSVD2,        RSVD3,        RSVD4,        0x331c, N, N),
+	PINGROUP(uart3_cts_n_pa1,      UARTC,        RSVD2,        GMI,          RSVD4,        0x317c, N, N),
+	PINGROUP(dap2_fs_pa2,          I2S1,         HDA,          RSVD3,        GMI,          0x3358, N, N),
+	PINGROUP(dap2_sclk_pa3,        I2S1,         HDA,          RSVD3,        GMI,          0x3364, N, N),
+	PINGROUP(dap2_din_pa4,         I2S1,         HDA,          RSVD3,        GMI,          0x335c, N, N),
+	PINGROUP(dap2_dout_pa5,        I2S1,         HDA,          RSVD3,        GMI,          0x3360, N, N),
+	PINGROUP(sdmmc3_clk_pa6,       UARTA,        PWM2,         SDMMC3,       SPI3,         0x3390, N, N),
+	PINGROUP(sdmmc3_cmd_pa7,       UARTA,        PWM3,         SDMMC3,       SPI2,         0x3394, N, N),
+	PINGROUP(gmi_a17_pb0,          UARTD,        SPI4,         GMI,          DTV,          0x3234, N, N),
+	PINGROUP(gmi_a18_pb1,          UARTD,        SPI4,         GMI,          DTV,          0x3238, N, N),
+	PINGROUP(lcd_pwr0_pb2,         DISPLAYA,     DISPLAYB,     SPI5,         HDCP,         0x3090, N, N),
+	PINGROUP(lcd_pclk_pb3,         DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x3094, N, N),
+	PINGROUP(sdmmc3_dat3_pb4,      RSVD1,        PWM0,         SDMMC3,       SPI3,         0x33a4, N, N),
+	PINGROUP(sdmmc3_dat2_pb5,      RSVD1,        PWM1,         SDMMC3,       SPI3,         0x33a0, N, N),
+	PINGROUP(sdmmc3_dat1_pb6,      RSVD1,        RSVD2,        SDMMC3,       SPI3,         0x339c, N, N),
+	PINGROUP(sdmmc3_dat0_pb7,      RSVD1,        RSVD2,        SDMMC3,       SPI3,         0x3398, N, N),
+	PINGROUP(uart3_rts_n_pc0,      UARTC,        PWM0,         GMI,          RSVD4,        0x3180, N, N),
+	PINGROUP(lcd_pwr1_pc1,         DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x3070, N, N),
+	PINGROUP(uart2_txd_pc2,        UARTB,        SPDIF,        UARTA,        SPI4,         0x3168, N, N),
+	PINGROUP(uart2_rxd_pc3,        UARTB,        SPDIF,        UARTA,        SPI4,         0x3164, N, N),
+	PINGROUP(gen1_i2c_scl_pc4,     I2C1,         RSVD2,        RSVD3,        RSVD4,        0x31a4, Y, N),
+	PINGROUP(gen1_i2c_sda_pc5,     I2C1,         RSVD2,        RSVD3,        RSVD4,        0x31a0, Y, N),
+	PINGROUP(lcd_pwr2_pc6,         DISPLAYA,     DISPLAYB,     SPI5,         HDCP,         0x3074, N, N),
+	PINGROUP(gmi_wp_n_pc7,         RSVD1,        NAND,         GMI,          GMI_ALT,      0x31c0, N, N),
+	PINGROUP(sdmmc3_dat5_pd0,      PWM0,         SPI4,         SDMMC3,       SPI2,         0x33ac, N, N),
+	PINGROUP(sdmmc3_dat4_pd1,      PWM1,         SPI4,         SDMMC3,       SPI2,         0x33a8, N, N),
+	PINGROUP(lcd_dc1_pd2,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x310c, N, N),
+	PINGROUP(sdmmc3_dat6_pd3,      SPDIF,        SPI4,         SDMMC3,       SPI2,         0x33b0, N, N),
+	PINGROUP(sdmmc3_dat7_pd4,      SPDIF,        SPI4,         SDMMC3,       SPI2,         0x33b4, N, N),
+	PINGROUP(vi_d1_pd5,            DDR,          SDMMC2,       VI,           RSVD4,        0x3128, N, Y),
+	PINGROUP(vi_vsync_pd6,         DDR,          RSVD2,        VI,           RSVD4,        0x315c, N, Y),
+	PINGROUP(vi_hsync_pd7,         DDR,          RSVD2,        VI,           RSVD4,        0x3160, N, Y),
+	PINGROUP(lcd_d0_pe0,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30a4, N, N),
+	PINGROUP(lcd_d1_pe1,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30a8, N, N),
+	PINGROUP(lcd_d2_pe2,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30ac, N, N),
+	PINGROUP(lcd_d3_pe3,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30b0, N, N),
+	PINGROUP(lcd_d4_pe4,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30b4, N, N),
+	PINGROUP(lcd_d5_pe5,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30b8, N, N),
+	PINGROUP(lcd_d6_pe6,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30bc, N, N),
+	PINGROUP(lcd_d7_pe7,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30c0, N, N),
+	PINGROUP(lcd_d8_pf0,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30c4, N, N),
+	PINGROUP(lcd_d9_pf1,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30c8, N, N),
+	PINGROUP(lcd_d10_pf2,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30cc, N, N),
+	PINGROUP(lcd_d11_pf3,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30d0, N, N),
+	PINGROUP(lcd_d12_pf4,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30d4, N, N),
+	PINGROUP(lcd_d13_pf5,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30d8, N, N),
+	PINGROUP(lcd_d14_pf6,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30dc, N, N),
+	PINGROUP(lcd_d15_pf7,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30e0, N, N),
+	PINGROUP(gmi_ad0_pg0,          RSVD1,        NAND,         GMI,          RSVD4,        0x31f0, N, N),
+	PINGROUP(gmi_ad1_pg1,          RSVD1,        NAND,         GMI,          RSVD4,        0x31f4, N, N),
+	PINGROUP(gmi_ad2_pg2,          RSVD1,        NAND,         GMI,          RSVD4,        0x31f8, N, N),
+	PINGROUP(gmi_ad3_pg3,          RSVD1,        NAND,         GMI,          RSVD4,        0x31fc, N, N),
+	PINGROUP(gmi_ad4_pg4,          RSVD1,        NAND,         GMI,          RSVD4,        0x3200, N, N),
+	PINGROUP(gmi_ad5_pg5,          RSVD1,        NAND,         GMI,          RSVD4,        0x3204, N, N),
+	PINGROUP(gmi_ad6_pg6,          RSVD1,        NAND,         GMI,          RSVD4,        0x3208, N, N),
+	PINGROUP(gmi_ad7_pg7,          RSVD1,        NAND,         GMI,          RSVD4,        0x320c, N, N),
+	PINGROUP(gmi_ad8_ph0,          PWM0,         NAND,         GMI,          RSVD4,        0x3210, N, N),
+	PINGROUP(gmi_ad9_ph1,          PWM1,         NAND,         GMI,          RSVD4,        0x3214, N, N),
+	PINGROUP(gmi_ad10_ph2,         PWM2,         NAND,         GMI,          RSVD4,        0x3218, N, N),
+	PINGROUP(gmi_ad11_ph3,         PWM3,         NAND,         GMI,          RSVD4,        0x321c, N, N),
+	PINGROUP(gmi_ad12_ph4,         RSVD1,        NAND,         GMI,          RSVD4,        0x3220, N, N),
+	PINGROUP(gmi_ad13_ph5,         RSVD1,        NAND,         GMI,          RSVD4,        0x3224, N, N),
+	PINGROUP(gmi_ad14_ph6,         RSVD1,        NAND,         GMI,          RSVD4,        0x3228, N, N),
+	PINGROUP(gmi_ad15_ph7,         RSVD1,        NAND,         GMI,          RSVD4,        0x322c, N, N),
+	PINGROUP(gmi_wr_n_pi0,         RSVD1,        NAND,         GMI,          RSVD4,        0x3240, N, N),
+	PINGROUP(gmi_oe_n_pi1,         RSVD1,        NAND,         GMI,          RSVD4,        0x3244, N, N),
+	PINGROUP(gmi_dqs_pi2,          RSVD1,        NAND,         GMI,          RSVD4,        0x3248, N, N),
+	PINGROUP(gmi_cs6_n_pi3,        NAND,         NAND_ALT,     GMI,          SATA,         0x31e8, N, N),
+	PINGROUP(gmi_rst_n_pi4,        NAND,         NAND_ALT,     GMI,          RSVD4,        0x324c, N, N),
+	PINGROUP(gmi_iordy_pi5,        RSVD1,        NAND,         GMI,          RSVD4,        0x31c4, N, N),
+	PINGROUP(gmi_cs7_n_pi6,        NAND,         NAND_ALT,     GMI,          GMI_ALT,      0x31ec, N, N),
+	PINGROUP(gmi_wait_pi7,         RSVD1,        NAND,         GMI,          RSVD4,        0x31c8, N, N),
+	PINGROUP(gmi_cs0_n_pj0,        RSVD1,        NAND,         GMI,          DTV,          0x31d4, N, N),
+	PINGROUP(lcd_de_pj1,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x3098, N, N),
+	PINGROUP(gmi_cs1_n_pj2,        RSVD1,        NAND,         GMI,          DTV,          0x31d8, N, N),
+	PINGROUP(lcd_hsync_pj3,        DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x309c, N, N),
+	PINGROUP(lcd_vsync_pj4,        DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30a0, N, N),
+	PINGROUP(uart2_cts_n_pj5,      UARTA,        UARTB,        GMI,          SPI4,         0x3170, N, N),
+	PINGROUP(uart2_rts_n_pj6,      UARTA,        UARTB,        GMI,          SPI4,         0x316c, N, N),
+	PINGROUP(gmi_a16_pj7,          UARTD,        SPI4,         GMI,          GMI_ALT,      0x3230, N, N),
+	PINGROUP(gmi_adv_n_pk0,        RSVD1,        NAND,         GMI,          RSVD4,        0x31cc, N, N),
+	PINGROUP(gmi_clk_pk1,          RSVD1,        NAND,         GMI,          RSVD4,        0x31d0, N, N),
+	PINGROUP(gmi_cs4_n_pk2,        RSVD1,        NAND,         GMI,          RSVD4,        0x31e4, N, N),
+	PINGROUP(gmi_cs2_n_pk3,        RSVD1,        NAND,         GMI,          RSVD4,        0x31dc, N, N),
+	PINGROUP(gmi_cs3_n_pk4,        RSVD1,        NAND,         GMI,          GMI_ALT,      0x31e0, N, N),
+	PINGROUP(spdif_out_pk5,        SPDIF,        RSVD2,        I2C1,         SDMMC2,       0x3354, N, N),
+	PINGROUP(spdif_in_pk6,         SPDIF,        HDA,          I2C1,         SDMMC2,       0x3350, N, N),
+	PINGROUP(gmi_a19_pk7,          UARTD,        SPI4,         GMI,          RSVD4,        0x323c, N, N),
+	PINGROUP(vi_d2_pl0,            DDR,          SDMMC2,       VI,           RSVD4,        0x312c, N, Y),
+	PINGROUP(vi_d3_pl1,            DDR,          SDMMC2,       VI,           RSVD4,        0x3130, N, Y),
+	PINGROUP(vi_d4_pl2,            DDR,          SDMMC2,       VI,           RSVD4,        0x3134, N, Y),
+	PINGROUP(vi_d5_pl3,            DDR,          SDMMC2,       VI,           RSVD4,        0x3138, N, Y),
+	PINGROUP(vi_d6_pl4,            DDR,          SDMMC2,       VI,           RSVD4,        0x313c, N, Y),
+	PINGROUP(vi_d7_pl5,            DDR,          SDMMC2,       VI,           RSVD4,        0x3140, N, Y),
+	PINGROUP(vi_d8_pl6,            DDR,          SDMMC2,       VI,           RSVD4,        0x3144, N, Y),
+	PINGROUP(vi_d9_pl7,            DDR,          SDMMC2,       VI,           RSVD4,        0x3148, N, Y),
+	PINGROUP(lcd_d16_pm0,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30e4, N, N),
+	PINGROUP(lcd_d17_pm1,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30e8, N, N),
+	PINGROUP(lcd_d18_pm2,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30ec, N, N),
+	PINGROUP(lcd_d19_pm3,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30f0, N, N),
+	PINGROUP(lcd_d20_pm4,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30f4, N, N),
+	PINGROUP(lcd_d21_pm5,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30f8, N, N),
+	PINGROUP(lcd_d22_pm6,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x30fc, N, N),
+	PINGROUP(lcd_d23_pm7,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x3100, N, N),
+	PINGROUP(dap1_fs_pn0,          I2S0,         HDA,          GMI,          SDMMC2,       0x3338, N, N),
+	PINGROUP(dap1_din_pn1,         I2S0,         HDA,          GMI,          SDMMC2,       0x333c, N, N),
+	PINGROUP(dap1_dout_pn2,        I2S0,         HDA,          GMI,          SDMMC2,       0x3340, N, N),
+	PINGROUP(dap1_sclk_pn3,        I2S0,         HDA,          GMI,          SDMMC2,       0x3344, N, N),
+	PINGROUP(lcd_cs0_n_pn4,        DISPLAYA,     DISPLAYB,     SPI5,         RSVD4,        0x3084, N, N),
+	PINGROUP(lcd_sdout_pn5,        DISPLAYA,     DISPLAYB,     SPI5,         HDCP,         0x307c, N, N),
+	PINGROUP(lcd_dc0_pn6,          DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x3088, N, N),
+	PINGROUP(hdmi_int_pn7,         HDMI,         RSVD2,        RSVD3,        RSVD4,        0x3110, N, N),
+	PINGROUP(ulpi_data7_po0,       SPI2,         HSI,          UARTA,        ULPI,         0x301c, N, N),
+	PINGROUP(ulpi_data0_po1,       SPI3,         HSI,          UARTA,        ULPI,         0x3000, N, N),
+	PINGROUP(ulpi_data1_po2,       SPI3,         HSI,          UARTA,        ULPI,         0x3004, N, N),
+	PINGROUP(ulpi_data2_po3,       SPI3,         HSI,          UARTA,        ULPI,         0x3008, N, N),
+	PINGROUP(ulpi_data3_po4,       SPI3,         HSI,          UARTA,        ULPI,         0x300c, N, N),
+	PINGROUP(ulpi_data4_po5,       SPI2,         HSI,          UARTA,        ULPI,         0x3010, N, N),
+	PINGROUP(ulpi_data5_po6,       SPI2,         HSI,          UARTA,        ULPI,         0x3014, N, N),
+	PINGROUP(ulpi_data6_po7,       SPI2,         HSI,          UARTA,        ULPI,         0x3018, N, N),
+	PINGROUP(dap3_fs_pp0,          I2S2,         RSVD2,        DISPLAYA,     DISPLAYB,     0x3030, N, N),
+	PINGROUP(dap3_din_pp1,         I2S2,         RSVD2,        DISPLAYA,     DISPLAYB,     0x3034, N, N),
+	PINGROUP(dap3_dout_pp2,        I2S2,         RSVD2,        DISPLAYA,     DISPLAYB,     0x3038, N, N),
+	PINGROUP(dap3_sclk_pp3,        I2S2,         RSVD2,        DISPLAYA,     DISPLAYB,     0x303c, N, N),
+	PINGROUP(dap4_fs_pp4,          I2S3,         RSVD2,        GMI,          RSVD4,        0x31a8, N, N),
+	PINGROUP(dap4_din_pp5,         I2S3,         RSVD2,        GMI,          RSVD4,        0x31ac, N, N),
+	PINGROUP(dap4_dout_pp6,        I2S3,         RSVD2,        GMI,          RSVD4,        0x31b0, N, N),
+	PINGROUP(dap4_sclk_pp7,        I2S3,         RSVD2,        GMI,          RSVD4,        0x31b4, N, N),
+	PINGROUP(kb_col0_pq0,          KBC,          NAND,         TRACE,        TEST,         0x32fc, N, N),
+	PINGROUP(kb_col1_pq1,          KBC,          NAND,         TRACE,        TEST,         0x3300, N, N),
+	PINGROUP(kb_col2_pq2,          KBC,          NAND,         TRACE,        RSVD4,        0x3304, N, N),
+	PINGROUP(kb_col3_pq3,          KBC,          NAND,         TRACE,        RSVD4,        0x3308, N, N),
+	PINGROUP(kb_col4_pq4,          KBC,          NAND,         TRACE,        RSVD4,        0x330c, N, N),
+	PINGROUP(kb_col5_pq5,          KBC,          NAND,         TRACE,        RSVD4,        0x3310, N, N),
+	PINGROUP(kb_col6_pq6,          KBC,          NAND,         TRACE,        MIO,          0x3314, N, N),
+	PINGROUP(kb_col7_pq7,          KBC,          NAND,         TRACE,        MIO,          0x3318, N, N),
+	PINGROUP(kb_row0_pr0,          KBC,          NAND,         RSVD3,        RSVD4,        0x32bc, N, N),
+	PINGROUP(kb_row1_pr1,          KBC,          NAND,         RSVD3,        RSVD4,        0x32c0, N, N),
+	PINGROUP(kb_row2_pr2,          KBC,          NAND,         RSVD3,        RSVD4,        0x32c4, N, N),
+	PINGROUP(kb_row3_pr3,          KBC,          NAND,         RSVD3,        INVALID,      0x32c8, N, N),
+	PINGROUP(kb_row4_pr4,          KBC,          NAND,         TRACE,        RSVD4,        0x32cc, N, N),
+	PINGROUP(kb_row5_pr5,          KBC,          NAND,         TRACE,        OWR,          0x32d0, N, N),
+	PINGROUP(kb_row6_pr6,          KBC,          NAND,         SDMMC2,       MIO,          0x32d4, N, N),
+	PINGROUP(kb_row7_pr7,          KBC,          NAND,         SDMMC2,       MIO,          0x32d8, N, N),
+	PINGROUP(kb_row8_ps0,          KBC,          NAND,         SDMMC2,       MIO,          0x32dc, N, N),
+	PINGROUP(kb_row9_ps1,          KBC,          NAND,         SDMMC2,       MIO,          0x32e0, N, N),
+	PINGROUP(kb_row10_ps2,         KBC,          NAND,         SDMMC2,       MIO,          0x32e4, N, N),
+	PINGROUP(kb_row11_ps3,         KBC,          NAND,         SDMMC2,       MIO,          0x32e8, N, N),
+	PINGROUP(kb_row12_ps4,         KBC,          NAND,         SDMMC2,       MIO,          0x32ec, N, N),
+	PINGROUP(kb_row13_ps5,         KBC,          NAND,         SDMMC2,       MIO,          0x32f0, N, N),
+	PINGROUP(kb_row14_ps6,         KBC,          NAND,         SDMMC2,       MIO,          0x32f4, N, N),
+	PINGROUP(kb_row15_ps7,         KBC,          NAND,         SDMMC2,       MIO,          0x32f8, N, N),
+	PINGROUP(vi_pclk_pt0,          RSVD1,        SDMMC2,       VI,           RSVD4,        0x3154, N, Y),
+	PINGROUP(vi_mclk_pt1,          VI,           VI_ALT1,      VI_ALT2,      VI_ALT3,      0x3158, N, Y),
+	PINGROUP(vi_d10_pt2,           DDR,          RSVD2,        VI,           RSVD4,        0x314c, N, Y),
+	PINGROUP(vi_d11_pt3,           DDR,          RSVD2,        VI,           RSVD4,        0x3150, N, Y),
+	PINGROUP(vi_d0_pt4,            DDR,          RSVD2,        VI,           RSVD4,        0x3124, N, Y),
+	PINGROUP(gen2_i2c_scl_pt5,     I2C2,         HDCP,         GMI,          RSVD4,        0x3250, Y, N),
+	PINGROUP(gen2_i2c_sda_pt6,     I2C2,         HDCP,         GMI,          RSVD4,        0x3254, Y, N),
+	PINGROUP(sdmmc4_cmd_pt7,       I2C3,         NAND,         GMI,          SDMMC4,       0x325c, N, Y),
+	PINGROUP(pu0,                  OWR,          UARTA,        GMI,          RSVD4,        0x3184, N, N),
+	PINGROUP(pu1,                  RSVD1,        UARTA,        GMI,          RSVD4,        0x3188, N, N),
+	PINGROUP(pu2,                  RSVD1,        UARTA,        GMI,          RSVD4,        0x318c, N, N),
+	PINGROUP(pu3,                  PWM0,         UARTA,        GMI,          RSVD4,        0x3190, N, N),
+	PINGROUP(pu4,                  PWM1,         UARTA,        GMI,          RSVD4,        0x3194, N, N),
+	PINGROUP(pu5,                  PWM2,         UARTA,        GMI,          RSVD4,        0x3198, N, N),
+	PINGROUP(pu6,                  PWM3,         UARTA,        GMI,          RSVD4,        0x319c, N, N),
+	PINGROUP(jtag_rtck_pu7,        RTCK,         RSVD2,        RSVD3,        RSVD4,        0x32b0, N, N),
+	PINGROUP(pv0,                  RSVD1,        RSVD2,        RSVD3,        RSVD4,        0x3040, N, N),
+	PINGROUP(pv1,                  RSVD1,        RSVD2,        RSVD3,        RSVD4,        0x3044, N, N),
+	PINGROUP(pv2,                  OWR,          RSVD2,        RSVD3,        RSVD4,        0x3060, N, N),
+	PINGROUP(pv3,                  CLK_12M_OUT,  RSVD2,        RSVD3,        RSVD4,        0x3064, N, N),
+	PINGROUP(ddc_scl_pv4,          I2C4,         RSVD2,        RSVD3,        RSVD4,        0x3114, N, N),
+	PINGROUP(ddc_sda_pv5,          I2C4,         RSVD2,        RSVD3,        RSVD4,        0x3118, N, N),
+	PINGROUP(crt_hsync_pv6,        CRT,          RSVD2,        RSVD3,        RSVD4,        0x311c, N, N),
+	PINGROUP(crt_vsync_pv7,        CRT,          RSVD2,        RSVD3,        RSVD4,        0x3120, N, N),
+	PINGROUP(lcd_cs1_n_pw0,        DISPLAYA,     DISPLAYB,     SPI5,         RSVD4,        0x3104, N, N),
+	PINGROUP(lcd_m1_pw1,           DISPLAYA,     DISPLAYB,     RSVD3,        RSVD4,        0x3108, N, N),
+	PINGROUP(spi2_cs1_n_pw2,       SPI3,         SPI2,         SPI2_ALT,     I2C1,         0x3388, N, N),
+	PINGROUP(spi2_cs2_n_pw3,       SPI3,         SPI2,         SPI2_ALT,     I2C1,         0x338c, N, N),
+	PINGROUP(clk1_out_pw4,         EXTPERIPH1,   RSVD2,        RSVD3,        RSVD4,        0x334c, N, N),
+	PINGROUP(clk2_out_pw5,         EXTPERIPH2,   RSVD2,        RSVD3,        RSVD4,        0x3068, N, N),
+	PINGROUP(uart3_txd_pw6,        UARTC,        RSVD2,        GMI,          RSVD4,        0x3174, N, N),
+	PINGROUP(uart3_rxd_pw7,        UARTC,        RSVD2,        GMI,          RSVD4,        0x3178, N, N),
+	PINGROUP(spi2_mosi_px0,        SPI6,         SPI2,         SPI3,         GMI,          0x3368, N, N),
+	PINGROUP(spi2_miso_px1,        SPI6,         SPI2,         SPI3,         GMI,          0x336c, N, N),
+	PINGROUP(spi2_sck_px2,         SPI6,         SPI2,         SPI3,         GMI,          0x3374, N, N),
+	PINGROUP(spi2_cs0_n_px3,       SPI6,         SPI2,         SPI3,         GMI,          0x3370, N, N),
+	PINGROUP(spi1_mosi_px4,        SPI2,         SPI1,         SPI2_ALT,     GMI,          0x3378, N, N),
+	PINGROUP(spi1_sck_px5,         SPI2,         SPI1,         SPI2_ALT,     GMI,          0x337c, N, N),
+	PINGROUP(spi1_cs0_n_px6,       SPI2,         SPI1,         SPI2_ALT,     GMI,          0x3380, N, N),
+	PINGROUP(spi1_miso_px7,        SPI3,         SPI1,         SPI2_ALT,     RSVD4,        0x3384, N, N),
+	PINGROUP(ulpi_clk_py0,         SPI1,         RSVD2,        UARTD,        ULPI,         0x3020, N, N),
+	PINGROUP(ulpi_dir_py1,         SPI1,         RSVD2,        UARTD,        ULPI,         0x3024, N, N),
+	PINGROUP(ulpi_nxt_py2,         SPI1,         RSVD2,        UARTD,        ULPI,         0x3028, N, N),
+	PINGROUP(ulpi_stp_py3,         SPI1,         RSVD2,        UARTD,        ULPI,         0x302c, N, N),
+	PINGROUP(sdmmc1_dat3_py4,      SDMMC1,       RSVD2,        UARTE,        UARTA,        0x3050, N, N),
+	PINGROUP(sdmmc1_dat2_py5,      SDMMC1,       RSVD2,        UARTE,        UARTA,        0x3054, N, N),
+	PINGROUP(sdmmc1_dat1_py6,      SDMMC1,       RSVD2,        UARTE,        UARTA,        0x3058, N, N),
+	PINGROUP(sdmmc1_dat0_py7,      SDMMC1,       RSVD2,        UARTE,        UARTA,        0x305c, N, N),
+	PINGROUP(sdmmc1_clk_pz0,       SDMMC1,       RSVD2,        RSVD3,        UARTA,        0x3048, N, N),
+	PINGROUP(sdmmc1_cmd_pz1,       SDMMC1,       RSVD2,        RSVD3,        UARTA,        0x304c, N, N),
+	PINGROUP(lcd_sdin_pz2,         DISPLAYA,     DISPLAYB,     SPI5,         RSVD4,        0x3078, N, N),
+	PINGROUP(lcd_wr_n_pz3,         DISPLAYA,     DISPLAYB,     SPI5,         HDCP,         0x3080, N, N),
+	PINGROUP(lcd_sck_pz4,          DISPLAYA,     DISPLAYB,     SPI5,         HDCP,         0x308c, N, N),
+	PINGROUP(sys_clk_req_pz5,      SYSCLK,       RSVD2,        RSVD3,        RSVD4,        0x3320, N, N),
+	PINGROUP(pwr_i2c_scl_pz6,      I2CPWR,       RSVD2,        RSVD3,        RSVD4,        0x32b4, Y, N),
+	PINGROUP(pwr_i2c_sda_pz7,      I2CPWR,       RSVD2,        RSVD3,        RSVD4,        0x32b8, Y, N),
+	PINGROUP(sdmmc4_dat0_paa0,     UARTE,        SPI3,         GMI,          SDMMC4,       0x3260, N, Y),
+	PINGROUP(sdmmc4_dat1_paa1,     UARTE,        SPI3,         GMI,          SDMMC4,       0x3264, N, Y),
+	PINGROUP(sdmmc4_dat2_paa2,     UARTE,        SPI3,         GMI,          SDMMC4,       0x3268, N, Y),
+	PINGROUP(sdmmc4_dat3_paa3,     UARTE,        SPI3,         GMI,          SDMMC4,       0x326c, N, Y),
+	PINGROUP(sdmmc4_dat4_paa4,     I2C3,         I2S4,         GMI,          SDMMC4,       0x3270, N, Y),
+	PINGROUP(sdmmc4_dat5_paa5,     VGP3,         I2S4,         GMI,          SDMMC4,       0x3274, N, Y),
+	PINGROUP(sdmmc4_dat6_paa6,     VGP4,         I2S4,         GMI,          SDMMC4,       0x3278, N, Y),
+	PINGROUP(sdmmc4_dat7_paa7,     VGP5,         I2S4,         GMI,          SDMMC4,       0x327c, N, Y),
+	PINGROUP(pbb0,                 I2S4,         RSVD2,        RSVD3,        SDMMC4,       0x328c, N, N),
+	PINGROUP(cam_i2c_scl_pbb1,     VGP1,         I2C3,         RSVD3,        SDMMC4,       0x3290, Y, N),
+	PINGROUP(cam_i2c_sda_pbb2,     VGP2,         I2C3,         RSVD3,        SDMMC4,       0x3294, Y, N),
+	PINGROUP(pbb3,                 VGP3,         DISPLAYA,     DISPLAYB,     SDMMC4,       0x3298, N, N),
+	PINGROUP(pbb4,                 VGP4,         DISPLAYA,     DISPLAYB,     SDMMC4,       0x329c, N, N),
+	PINGROUP(pbb5,                 VGP5,         DISPLAYA,     DISPLAYB,     SDMMC4,       0x32a0, N, N),
+	PINGROUP(pbb6,                 VGP6,         DISPLAYA,     DISPLAYB,     SDMMC4,       0x32a4, N, N),
+	PINGROUP(pbb7,                 I2S4,         RSVD2,        RSVD3,        SDMMC4,       0x32a8, N, N),
+	PINGROUP(cam_mclk_pcc0,        VI,           VI_ALT1,      VI_ALT3,      SDMMC4,       0x3284, N, N),
+	PINGROUP(pcc1,                 I2S4,         RSVD2,        RSVD3,        SDMMC4,       0x3288, N, N),
+	PINGROUP(pcc2,                 I2S4,         RSVD2,        RSVD3,        RSVD4,        0x32ac, N, N),
+	PINGROUP(sdmmc4_rst_n_pcc3,    VGP6,         RSVD2,        RSVD3,        SDMMC4,       0x3280, N, Y),
+	PINGROUP(sdmmc4_clk_pcc4,      INVALID,      NAND,         GMI,          SDMMC4,       0x3258, N, Y),
+	PINGROUP(clk2_req_pcc5,        DAP,          RSVD2,        RSVD3,        RSVD4,        0x306c, N, N),
+	PINGROUP(pex_l2_rst_n_pcc6,    PCIE,         HDA,          RSVD3,        RSVD4,        0x33d8, N, N),
+	PINGROUP(pex_l2_clkreq_n_pcc7, PCIE,         HDA,          RSVD3,        RSVD4,        0x33dc, N, N),
+	PINGROUP(pex_l0_prsnt_n_pdd0,  PCIE,         HDA,          RSVD3,        RSVD4,        0x33b8, N, N),
+	PINGROUP(pex_l0_rst_n_pdd1,    PCIE,         HDA,          RSVD3,        RSVD4,        0x33bc, N, N),
+	PINGROUP(pex_l0_clkreq_n_pdd2, PCIE,         HDA,          RSVD3,        RSVD4,        0x33c0, N, N),
+	PINGROUP(pex_wake_n_pdd3,      PCIE,         HDA,          RSVD3,        RSVD4,        0x33c4, N, N),
+	PINGROUP(pex_l1_prsnt_n_pdd4,  PCIE,         HDA,          RSVD3,        RSVD4,        0x33c8, N, N),
+	PINGROUP(pex_l1_rst_n_pdd5,    PCIE,         HDA,          RSVD3,        RSVD4,        0x33cc, N, N),
+	PINGROUP(pex_l1_clkreq_n_pdd6, PCIE,         HDA,          RSVD3,        RSVD4,        0x33d0, N, N),
+	PINGROUP(pex_l2_prsnt_n_pdd7,  PCIE,         HDA,          RSVD3,        RSVD4,        0x33d4, N, N),
+	PINGROUP(clk3_out_pee0,        EXTPERIPH3,   RSVD2,        RSVD3,        RSVD4,        0x31b8, N, N),
+	PINGROUP(clk3_req_pee1,        DEV3,         RSVD2,        RSVD3,        RSVD4,        0x31bc, N, N),
+	PINGROUP(clk1_req_pee2,        DAP,          HDA,          RSVD3,        RSVD4,        0x3348, N, N),
+	PINGROUP(hdmi_cec_pee3,        CEC,          RSVD2,        RSVD3,        RSVD4,        0x33e0, Y, N),
+	PINGROUP(clk_32k_in,           CLK_32K_IN,   RSVD2,        RSVD3,        RSVD4,        0x3330, N, N),
+	PINGROUP(core_pwr_req,         CORE_PWR_REQ, RSVD2,        RSVD3,        RSVD4,        0x3324, N, N),
+	PINGROUP(cpu_pwr_req,          CPU_PWR_REQ,  RSVD2,        RSVD3,        RSVD4,        0x3328, N, N),
+	PINGROUP(owr,                  OWR,          CEC,          RSVD3,        RSVD4,        0x3334, N, N),
+	PINGROUP(pwr_int_n,            PWR_INT_N,    RSVD2,        RSVD3,        RSVD4,        0x332c, N, N),
 	/* pg_name, r, hsm_b, schmitt_b, lpmd_b, drvdn_b, drvdn_w, drvup_b, drvup_w, slwr_b, slwr_w, slwf_b, slwf_w */
 	DRV_PINGROUP(ao1,   0x868,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2),
 	DRV_PINGROUP(ao2,   0x86c,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2),
diff --git a/drivers/pinctrl/pinmux.c b/drivers/pinctrl/pinmux.c
index 9248ce4efed..051e8592990 100644
--- a/drivers/pinctrl/pinmux.c
+++ b/drivers/pinctrl/pinmux.c
@@ -391,14 +391,16 @@ int pinmux_enable_setting(struct pinctrl_setting const *setting)
 	struct pinctrl_dev *pctldev = setting->pctldev;
 	const struct pinctrl_ops *pctlops = pctldev->desc->pctlops;
 	const struct pinmux_ops *ops = pctldev->desc->pmxops;
-	int ret;
-	const unsigned *pins;
-	unsigned num_pins;
+	int ret = 0;
+	const unsigned *pins = NULL;
+	unsigned num_pins = 0;
 	int i;
 	struct pin_desc *desc;
 
-	ret = pctlops->get_group_pins(pctldev, setting->data.mux.group,
-				      &pins, &num_pins);
+	if (pctlops->get_group_pins)
+		ret = pctlops->get_group_pins(pctldev, setting->data.mux.group,
+					      &pins, &num_pins);
+
 	if (ret) {
 		const char *gname;
 
@@ -470,14 +472,15 @@ void pinmux_disable_setting(struct pinctrl_setting const *setting)
 	struct pinctrl_dev *pctldev = setting->pctldev;
 	const struct pinctrl_ops *pctlops = pctldev->desc->pctlops;
 	const struct pinmux_ops *ops = pctldev->desc->pmxops;
-	int ret;
-	const unsigned *pins;
-	unsigned num_pins;
+	int ret = 0;
+	const unsigned *pins = NULL;
+	unsigned num_pins = 0;
 	int i;
 	struct pin_desc *desc;
 
-	ret = pctlops->get_group_pins(pctldev, setting->data.mux.group,
-				      &pins, &num_pins);
+	if (pctlops->get_group_pins)
+		ret = pctlops->get_group_pins(pctldev, setting->data.mux.group,
+					      &pins, &num_pins);
 	if (ret) {
 		const char *gname;
 
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a73a4.c b/drivers/pinctrl/sh-pfc/pfc-r8a73a4.c
index d39ca87353e..ce9fb7aa8ba 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a73a4.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a73a4.c
@@ -20,7 +20,10 @@
 #include <linux/io.h>
 #include <linux/kernel.h>
 #include <linux/pinctrl/pinconf-generic.h>
+
+#ifndef CONFIG_ARCH_MULTIPLATFORM
 #include <mach/irqs.h>
+#endif
 
 #include "core.h"
 #include "sh_pfc.h"
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a7740.c b/drivers/pinctrl/sh-pfc/pfc-r8a7740.c
index 6c83ce43a94..e4c1ef47705 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a7740.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a7740.c
@@ -22,7 +22,9 @@
 #include <linux/kernel.h>
 #include <linux/pinctrl/pinconf-generic.h>
 
+#ifndef CONFIG_ARCH_MULTIPLATFORM
 #include <mach/irqs.h>
+#endif
 
 #include "core.h"
 #include "sh_pfc.h"
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a7790.c b/drivers/pinctrl/sh-pfc/pfc-r8a7790.c
index 48093719167..9a179c94b4d 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a7790.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a7790.c
@@ -782,7 +782,8 @@ enum {
 	USB1_PWEN_MARK, AUDIO_CLKOUT_D_MARK, USB1_OVC_MARK,
 	TCLK1_B_MARK,
 
-	I2C3_SCL_MARK, I2C3_SDA_MARK,
+	IIC0_SCL_MARK, IIC0_SDA_MARK, I2C0_SCL_MARK, I2C0_SDA_MARK,
+	IIC3_SCL_MARK, IIC3_SDA_MARK, I2C3_SCL_MARK, I2C3_SDA_MARK,
 	PINMUX_MARK_END,
 };
 
@@ -1722,6 +1723,13 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_DATA(IP16_7, USB1_OVC),
 	PINMUX_IPSR_MODSEL_DATA(IP16_7, TCLK1_B, SEL_TMU1_1),
 
+	PINMUX_DATA(IIC0_SCL_MARK, FN_SEL_IIC0_0),
+	PINMUX_DATA(IIC0_SDA_MARK, FN_SEL_IIC0_0),
+	PINMUX_DATA(I2C0_SCL_MARK, FN_SEL_IIC0_1),
+	PINMUX_DATA(I2C0_SDA_MARK, FN_SEL_IIC0_1),
+
+	PINMUX_DATA(IIC3_SCL_MARK, FN_SEL_IICDVFS_0),
+	PINMUX_DATA(IIC3_SDA_MARK, FN_SEL_IICDVFS_0),
 	PINMUX_DATA(I2C3_SCL_MARK, FN_SEL_IICDVFS_1),
 	PINMUX_DATA(I2C3_SDA_MARK, FN_SEL_IICDVFS_1),
 };
@@ -1735,8 +1743,10 @@ static const struct sh_pfc_pin pinmux_pins[] = {
 	PINMUX_GPIO_GP_ALL(),
 
 	/* Pins not associated with a GPIO port */
-	SH_PFC_PIN_NAMED(ROW_GROUP_A('J'), 15, AJ15),
+	SH_PFC_PIN_NAMED(ROW_GROUP_A('F'), 15, AF15),
+	SH_PFC_PIN_NAMED(ROW_GROUP_A('G'), 15, AG15),
 	SH_PFC_PIN_NAMED(ROW_GROUP_A('H'), 15, AH15),
+	SH_PFC_PIN_NAMED(ROW_GROUP_A('J'), 15, AJ15),
 };
 
 /* - AUDIO CLOCK ------------------------------------------------------------ */
@@ -2054,6 +2064,14 @@ static const unsigned int hscif1_ctrl_b_pins[] = {
 static const unsigned int hscif1_ctrl_b_mux[] = {
 	HRTS1_N_B_MARK, HCTS1_N_B_MARK,
 };
+/* - I2C0 ------------------------------------------------------------------- */
+static const unsigned int i2c0_pins[] = {
+	/* SCL, SDA */
+	PIN_A_NUMBER('G', 15), PIN_A_NUMBER('F', 15),
+};
+static const unsigned int i2c0_mux[] = {
+	I2C0_SCL_MARK, I2C0_SDA_MARK,
+};
 /* - I2C1 ------------------------------------------------------------------- */
 static const unsigned int i2c1_pins[] = {
 	/* SCL, SDA */
@@ -2120,6 +2138,80 @@ static const unsigned int i2c3_pins[] = {
 static const unsigned int i2c3_mux[] = {
 	I2C3_SCL_MARK, I2C3_SDA_MARK,
 };
+/* - IIC0 (I2C4) ------------------------------------------------------------ */
+static const unsigned int iic0_pins[] = {
+	/* SCL, SDA */
+	PIN_A_NUMBER('G', 15), PIN_A_NUMBER('F', 15),
+};
+static const unsigned int iic0_mux[] = {
+	IIC0_SCL_MARK, IIC0_SDA_MARK,
+};
+/* - IIC1 (I2C5) ------------------------------------------------------------ */
+static const unsigned int iic1_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(1, 16), RCAR_GP_PIN(1, 17),
+};
+static const unsigned int iic1_mux[] = {
+	IIC1_SCL_MARK, IIC1_SDA_MARK,
+};
+static const unsigned int iic1_b_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(3, 6), RCAR_GP_PIN(3, 7),
+};
+static const unsigned int iic1_b_mux[] = {
+	IIC1_SCL_B_MARK, IIC1_SDA_B_MARK,
+};
+static const unsigned int iic1_c_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(4, 30), RCAR_GP_PIN(4, 27),
+};
+static const unsigned int iic1_c_mux[] = {
+	IIC1_SCL_C_MARK, IIC1_SDA_C_MARK,
+};
+/* - IIC2 (I2C6) ------------------------------------------------------------ */
+static const unsigned int iic2_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(5, 5), RCAR_GP_PIN(5, 6),
+};
+static const unsigned int iic2_mux[] = {
+	IIC2_SCL_MARK, IIC2_SDA_MARK,
+};
+static const unsigned int iic2_b_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(4, 0), RCAR_GP_PIN(4, 1),
+};
+static const unsigned int iic2_b_mux[] = {
+	IIC2_SCL_B_MARK, IIC2_SDA_B_MARK,
+};
+static const unsigned int iic2_c_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(0, 6), RCAR_GP_PIN(0, 7),
+};
+static const unsigned int iic2_c_mux[] = {
+	IIC2_SCL_C_MARK, IIC2_SDA_C_MARK,
+};
+static const unsigned int iic2_d_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(3, 14), RCAR_GP_PIN(3, 15),
+};
+static const unsigned int iic2_d_mux[] = {
+	IIC2_SCL_D_MARK, IIC2_SDA_D_MARK,
+};
+static const unsigned int iic2_e_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(2, 18), RCAR_GP_PIN(2, 19),
+};
+static const unsigned int iic2_e_mux[] = {
+	IIC2_SCL_E_MARK, IIC2_SDA_E_MARK,
+};
+/* - IIC3 (I2C7) ------------------------------------------------------------ */
+static const unsigned int iic3_pins[] = {
+/* SCL, SDA */
+	PIN_A_NUMBER('J', 15), PIN_A_NUMBER('H', 15),
+};
+static const unsigned int iic3_mux[] = {
+	IIC3_SCL_MARK, IIC3_SDA_MARK,
+};
 /* - INTC ------------------------------------------------------------------- */
 static const unsigned int intc_irq0_pins[] = {
 	/* IRQ */
@@ -3757,6 +3849,7 @@ static const struct sh_pfc_pin_group pinmux_groups[] = {
 	SH_PFC_PIN_GROUP(hscif1_data_b),
 	SH_PFC_PIN_GROUP(hscif1_clk_b),
 	SH_PFC_PIN_GROUP(hscif1_ctrl_b),
+	SH_PFC_PIN_GROUP(i2c0),
 	SH_PFC_PIN_GROUP(i2c1),
 	SH_PFC_PIN_GROUP(i2c1_b),
 	SH_PFC_PIN_GROUP(i2c1_c),
@@ -3766,6 +3859,16 @@ static const struct sh_pfc_pin_group pinmux_groups[] = {
 	SH_PFC_PIN_GROUP(i2c2_d),
 	SH_PFC_PIN_GROUP(i2c2_e),
 	SH_PFC_PIN_GROUP(i2c3),
+	SH_PFC_PIN_GROUP(iic0),
+	SH_PFC_PIN_GROUP(iic1),
+	SH_PFC_PIN_GROUP(iic1_b),
+	SH_PFC_PIN_GROUP(iic1_c),
+	SH_PFC_PIN_GROUP(iic2),
+	SH_PFC_PIN_GROUP(iic2_b),
+	SH_PFC_PIN_GROUP(iic2_c),
+	SH_PFC_PIN_GROUP(iic2_d),
+	SH_PFC_PIN_GROUP(iic2_e),
+	SH_PFC_PIN_GROUP(iic3),
 	SH_PFC_PIN_GROUP(intc_irq0),
 	SH_PFC_PIN_GROUP(intc_irq1),
 	SH_PFC_PIN_GROUP(intc_irq2),
@@ -4044,6 +4147,10 @@ static const char * const hscif1_groups[] = {
 	"hscif1_ctrl_b",
 };
 
+static const char * const i2c0_groups[] = {
+	"i2c0",
+};
+
 static const char * const i2c1_groups[] = {
 	"i2c1",
 	"i2c1_b",
@@ -4062,6 +4169,28 @@ static const char * const i2c3_groups[] = {
 	"i2c3",
 };
 
+static const char * const iic0_groups[] = {
+	"iic0",
+};
+
+static const char * const iic1_groups[] = {
+	"iic1",
+	"iic1_b",
+	"iic1_c",
+};
+
+static const char * const iic2_groups[] = {
+	"iic2",
+	"iic2_b",
+	"iic2_c",
+	"iic2_d",
+	"iic2_e",
+};
+
+static const char * const iic3_groups[] = {
+	"iic3",
+};
+
 static const char * const intc_groups[] = {
 	"intc_irq0",
 	"intc_irq1",
@@ -4373,9 +4502,14 @@ static const struct sh_pfc_function pinmux_functions[] = {
 	SH_PFC_FUNCTION(eth),
 	SH_PFC_FUNCTION(hscif0),
 	SH_PFC_FUNCTION(hscif1),
+	SH_PFC_FUNCTION(i2c0),
 	SH_PFC_FUNCTION(i2c1),
 	SH_PFC_FUNCTION(i2c2),
 	SH_PFC_FUNCTION(i2c3),
+	SH_PFC_FUNCTION(iic0),
+	SH_PFC_FUNCTION(iic1),
+	SH_PFC_FUNCTION(iic2),
+	SH_PFC_FUNCTION(iic3),
 	SH_PFC_FUNCTION(intc),
 	SH_PFC_FUNCTION(mmc0),
 	SH_PFC_FUNCTION(mmc1),
@@ -4794,8 +4928,7 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		FN_MSIOF0_SCK_B, 0,
 		/* IP5_23_21 [3] */
 		FN_WE1_N, FN_IERX, FN_CAN1_RX, FN_VI1_G4,
-		FN_VI1_G4_B, FN_VI2_R6, FN_SCIFA0_CTS_N_B,
-		FN_IERX_C, 0,
+		FN_VI1_G4_B, FN_VI2_R6, FN_SCIFA0_CTS_N_B, FN_IERX_C,
 		/* IP5_20_18 [3] */
 		FN_WE0_N, FN_IECLK, FN_CAN_CLK,
 		FN_VI2_VSYNC_N, FN_SCIFA0_TXD_B, FN_VI2_VSYNC_N_B, 0, 0,
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a7791.c b/drivers/pinctrl/sh-pfc/pfc-r8a7791.c
index 5186d70c49d..2e688dc4a3c 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a7791.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a7791.c
@@ -1680,6 +1680,53 @@ static const struct sh_pfc_pin pinmux_pins[] = {
 	PINMUX_GPIO_GP_ALL(),
 };
 
+/* - Audio Clock ------------------------------------------------------------ */
+static const unsigned int audio_clk_a_pins[] = {
+	/* CLK */
+	RCAR_GP_PIN(2, 28),
+};
+
+static const unsigned int audio_clk_a_mux[] = {
+	AUDIO_CLKA_MARK,
+};
+
+static const unsigned int audio_clk_b_pins[] = {
+	/* CLK */
+	RCAR_GP_PIN(2, 29),
+};
+
+static const unsigned int audio_clk_b_mux[] = {
+	AUDIO_CLKB_MARK,
+};
+
+static const unsigned int audio_clk_b_b_pins[] = {
+	/* CLK */
+	RCAR_GP_PIN(7, 20),
+};
+
+static const unsigned int audio_clk_b_b_mux[] = {
+	AUDIO_CLKB_B_MARK,
+};
+
+static const unsigned int audio_clk_c_pins[] = {
+	/* CLK */
+	RCAR_GP_PIN(2, 30),
+};
+
+static const unsigned int audio_clk_c_mux[] = {
+	AUDIO_CLKC_MARK,
+};
+
+static const unsigned int audio_clkout_pins[] = {
+	/* CLK */
+	RCAR_GP_PIN(2, 31),
+};
+
+static const unsigned int audio_clkout_mux[] = {
+	AUDIO_CLKOUT_MARK,
+};
+
+
 /* - DU --------------------------------------------------------------------- */
 static const unsigned int du_rgb666_pins[] = {
 	/* R[7:2], G[7:2], B[7:2] */
@@ -1733,19 +1780,32 @@ static const unsigned int du_clk_out_1_mux[] = {
 	DU1_DOTCLKOUT1_MARK
 };
 static const unsigned int du_sync_pins[] = {
-	/* EXVSYNC/VSYNC, EXHSYNC/HSYNC, EXDISP/EXODDF/EXCDE */
-	RCAR_GP_PIN(3, 29), RCAR_GP_PIN(3, 28), RCAR_GP_PIN(3, 27),
+	/* EXVSYNC/VSYNC, EXHSYNC/HSYNC */
+	RCAR_GP_PIN(3, 28), RCAR_GP_PIN(3, 27),
 };
 static const unsigned int du_sync_mux[] = {
-	DU1_EXODDF_DU1_ODDF_DISP_CDE_MARK,
 	DU1_EXVSYNC_DU1_VSYNC_MARK, DU1_EXHSYNC_DU1_HSYNC_MARK
 };
-static const unsigned int du_cde_disp_pins[] = {
-	/* CDE DISP */
-	RCAR_GP_PIN(3, 31), RCAR_GP_PIN(3, 30),
+static const unsigned int du_oddf_pins[] = {
+	/* EXDISP/EXODDF/EXCDE */
+	RCAR_GP_PIN(3, 29),
+};
+static const unsigned int du_oddf_mux[] = {
+	DU1_EXODDF_DU1_ODDF_DISP_CDE_MARK,
+};
+static const unsigned int du_cde_pins[] = {
+	/* CDE */
+	RCAR_GP_PIN(3, 31),
 };
-static const unsigned int du_cde_disp_mux[] = {
-	DU1_CDE_MARK, DU1_DISP_MARK
+static const unsigned int du_cde_mux[] = {
+	DU1_CDE_MARK,
+};
+static const unsigned int du_disp_pins[] = {
+	/* DISP */
+	RCAR_GP_PIN(3, 30),
+};
+static const unsigned int du_disp_mux[] = {
+	DU1_DISP_MARK,
 };
 static const unsigned int du0_clk_in_pins[] = {
 	/* CLKIN */
@@ -3246,6 +3306,260 @@ static const unsigned int sdhi2_wp_pins[] = {
 static const unsigned int sdhi2_wp_mux[] = {
 	SD2_WP_MARK,
 };
+
+/* - SSI -------------------------------------------------------------------- */
+static const unsigned int ssi0_data_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(2, 2),
+};
+
+static const unsigned int ssi0_data_mux[] = {
+	SSI_SDATA0_MARK,
+};
+
+static const unsigned int ssi0_data_b_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(3, 4),
+};
+
+static const unsigned int ssi0_data_b_mux[] = {
+	SSI_SDATA0_B_MARK,
+};
+
+static const unsigned int ssi0129_ctrl_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(2, 0), RCAR_GP_PIN(2, 1),
+};
+
+static const unsigned int ssi0129_ctrl_mux[] = {
+	SSI_SCK0129_MARK, SSI_WS0129_MARK,
+};
+
+static const unsigned int ssi0129_ctrl_b_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(3, 2), RCAR_GP_PIN(3, 3),
+};
+
+static const unsigned int ssi0129_ctrl_b_mux[] = {
+	SSI_SCK0129_B_MARK, SSI_WS0129_B_MARK,
+};
+
+static const unsigned int ssi1_data_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(2, 5),
+};
+
+static const unsigned int ssi1_data_mux[] = {
+	SSI_SDATA1_MARK,
+};
+
+static const unsigned int ssi1_data_b_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(3, 7),
+};
+
+static const unsigned int ssi1_data_b_mux[] = {
+	SSI_SDATA1_B_MARK,
+};
+
+static const unsigned int ssi1_ctrl_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(2, 3), RCAR_GP_PIN(2, 4),
+};
+
+static const unsigned int ssi1_ctrl_mux[] = {
+	SSI_SCK1_MARK, SSI_WS1_MARK,
+};
+
+static const unsigned int ssi1_ctrl_b_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(3, 5), RCAR_GP_PIN(3, 6),
+};
+
+static const unsigned int ssi1_ctrl_b_mux[] = {
+	SSI_SCK1_B_MARK, SSI_WS1_B_MARK,
+};
+
+static const unsigned int ssi2_data_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(2, 8),
+};
+
+static const unsigned int ssi2_data_mux[] = {
+	SSI_SDATA2_MARK,
+};
+
+static const unsigned int ssi2_ctrl_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(2, 6), RCAR_GP_PIN(2, 7),
+};
+
+static const unsigned int ssi2_ctrl_mux[] = {
+	SSI_SCK2_MARK, SSI_WS2_MARK,
+};
+
+static const unsigned int ssi3_data_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(2, 11),
+};
+
+static const unsigned int ssi3_data_mux[] = {
+	SSI_SDATA3_MARK,
+};
+
+static const unsigned int ssi34_ctrl_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(2, 9), RCAR_GP_PIN(2, 10),
+};
+
+static const unsigned int ssi34_ctrl_mux[] = {
+	SSI_SCK34_MARK, SSI_WS34_MARK,
+};
+
+static const unsigned int ssi4_data_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(2, 14),
+};
+
+static const unsigned int ssi4_data_mux[] = {
+	SSI_SDATA4_MARK,
+};
+
+static const unsigned int ssi4_ctrl_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(2, 12), RCAR_GP_PIN(2, 13),
+};
+
+static const unsigned int ssi4_ctrl_mux[] = {
+	SSI_SCK4_MARK, SSI_WS4_MARK,
+};
+
+static const unsigned int ssi5_data_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(2, 17),
+};
+
+static const unsigned int ssi5_data_mux[] = {
+	SSI_SDATA5_MARK,
+};
+
+static const unsigned int ssi5_ctrl_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(2, 15), RCAR_GP_PIN(2, 16),
+};
+
+static const unsigned int ssi5_ctrl_mux[] = {
+	SSI_SCK5_MARK, SSI_WS5_MARK,
+};
+
+static const unsigned int ssi6_data_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(2, 20),
+};
+
+static const unsigned int ssi6_data_mux[] = {
+	SSI_SDATA6_MARK,
+};
+
+static const unsigned int ssi6_ctrl_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(2, 18), RCAR_GP_PIN(2, 19),
+};
+
+static const unsigned int ssi6_ctrl_mux[] = {
+	SSI_SCK6_MARK, SSI_WS6_MARK,
+};
+
+static const unsigned int ssi7_data_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(2, 23),
+};
+
+static const unsigned int ssi7_data_mux[] = {
+	SSI_SDATA7_MARK,
+};
+
+static const unsigned int ssi7_data_b_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(3, 12),
+};
+
+static const unsigned int ssi7_data_b_mux[] = {
+	SSI_SDATA7_B_MARK,
+};
+
+static const unsigned int ssi78_ctrl_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(2, 21), RCAR_GP_PIN(2, 22),
+};
+
+static const unsigned int ssi78_ctrl_mux[] = {
+	SSI_SCK78_MARK, SSI_WS78_MARK,
+};
+
+static const unsigned int ssi78_ctrl_b_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(3, 10), RCAR_GP_PIN(3, 11),
+};
+
+static const unsigned int ssi78_ctrl_b_mux[] = {
+	SSI_SCK78_B_MARK, SSI_WS78_B_MARK,
+};
+
+static const unsigned int ssi8_data_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(2, 24),
+};
+
+static const unsigned int ssi8_data_mux[] = {
+	SSI_SDATA8_MARK,
+};
+
+static const unsigned int ssi8_data_b_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(3, 13),
+};
+
+static const unsigned int ssi8_data_b_mux[] = {
+	SSI_SDATA8_B_MARK,
+};
+
+static const unsigned int ssi9_data_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(2, 27),
+};
+
+static const unsigned int ssi9_data_mux[] = {
+	SSI_SDATA9_MARK,
+};
+
+static const unsigned int ssi9_data_b_pins[] = {
+	/* SDATA */
+	RCAR_GP_PIN(3, 18),
+};
+
+static const unsigned int ssi9_data_b_mux[] = {
+	SSI_SDATA9_B_MARK,
+};
+
+static const unsigned int ssi9_ctrl_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(2, 25), RCAR_GP_PIN(2, 26),
+};
+
+static const unsigned int ssi9_ctrl_mux[] = {
+	SSI_SCK9_MARK, SSI_WS9_MARK,
+};
+
+static const unsigned int ssi9_ctrl_b_pins[] = {
+	/* SCK, WS */
+	RCAR_GP_PIN(3, 14), RCAR_GP_PIN(3, 15),
+};
+
+static const unsigned int ssi9_ctrl_b_mux[] = {
+	SSI_SCK9_B_MARK, SSI_WS9_B_MARK,
+};
+
 /* - USB0 ------------------------------------------------------------------- */
 static const unsigned int usb0_pins[] = {
 	RCAR_GP_PIN(7, 23), /* PWEN */
@@ -3550,12 +3864,19 @@ static const unsigned int vin2_clk_mux[] = {
 };
 
 static const struct sh_pfc_pin_group pinmux_groups[] = {
+	SH_PFC_PIN_GROUP(audio_clk_a),
+	SH_PFC_PIN_GROUP(audio_clk_b),
+	SH_PFC_PIN_GROUP(audio_clk_b_b),
+	SH_PFC_PIN_GROUP(audio_clk_c),
+	SH_PFC_PIN_GROUP(audio_clkout),
 	SH_PFC_PIN_GROUP(du_rgb666),
 	SH_PFC_PIN_GROUP(du_rgb888),
 	SH_PFC_PIN_GROUP(du_clk_out_0),
 	SH_PFC_PIN_GROUP(du_clk_out_1),
 	SH_PFC_PIN_GROUP(du_sync),
-	SH_PFC_PIN_GROUP(du_cde_disp),
+	SH_PFC_PIN_GROUP(du_oddf),
+	SH_PFC_PIN_GROUP(du_cde),
+	SH_PFC_PIN_GROUP(du_disp),
 	SH_PFC_PIN_GROUP(du0_clk_in),
 	SH_PFC_PIN_GROUP(du1_clk_in),
 	SH_PFC_PIN_GROUP(du1_clk_in_b),
@@ -3762,6 +4083,34 @@ static const struct sh_pfc_pin_group pinmux_groups[] = {
 	SH_PFC_PIN_GROUP(sdhi2_ctrl),
 	SH_PFC_PIN_GROUP(sdhi2_cd),
 	SH_PFC_PIN_GROUP(sdhi2_wp),
+	SH_PFC_PIN_GROUP(ssi0_data),
+	SH_PFC_PIN_GROUP(ssi0_data_b),
+	SH_PFC_PIN_GROUP(ssi0129_ctrl),
+	SH_PFC_PIN_GROUP(ssi0129_ctrl_b),
+	SH_PFC_PIN_GROUP(ssi1_data),
+	SH_PFC_PIN_GROUP(ssi1_data_b),
+	SH_PFC_PIN_GROUP(ssi1_ctrl),
+	SH_PFC_PIN_GROUP(ssi1_ctrl_b),
+	SH_PFC_PIN_GROUP(ssi2_data),
+	SH_PFC_PIN_GROUP(ssi2_ctrl),
+	SH_PFC_PIN_GROUP(ssi3_data),
+	SH_PFC_PIN_GROUP(ssi34_ctrl),
+	SH_PFC_PIN_GROUP(ssi4_data),
+	SH_PFC_PIN_GROUP(ssi4_ctrl),
+	SH_PFC_PIN_GROUP(ssi5_data),
+	SH_PFC_PIN_GROUP(ssi5_ctrl),
+	SH_PFC_PIN_GROUP(ssi6_data),
+	SH_PFC_PIN_GROUP(ssi6_ctrl),
+	SH_PFC_PIN_GROUP(ssi7_data),
+	SH_PFC_PIN_GROUP(ssi7_data_b),
+	SH_PFC_PIN_GROUP(ssi78_ctrl),
+	SH_PFC_PIN_GROUP(ssi78_ctrl_b),
+	SH_PFC_PIN_GROUP(ssi8_data),
+	SH_PFC_PIN_GROUP(ssi8_data_b),
+	SH_PFC_PIN_GROUP(ssi9_data),
+	SH_PFC_PIN_GROUP(ssi9_data_b),
+	SH_PFC_PIN_GROUP(ssi9_ctrl),
+	SH_PFC_PIN_GROUP(ssi9_ctrl_b),
 	SH_PFC_PIN_GROUP(usb0),
 	SH_PFC_PIN_GROUP(usb1),
 	VIN_DATA_PIN_GROUP(vin0_data, 24),
@@ -3798,13 +4147,23 @@ static const struct sh_pfc_pin_group pinmux_groups[] = {
 	SH_PFC_PIN_GROUP(vin2_clk),
 };
 
+static const char * const audio_clk_groups[] = {
+	"audio_clk_a",
+	"audio_clk_b",
+	"audio_clk_b_b",
+	"audio_clk_c",
+	"audio_clkout",
+};
+
 static const char * const du_groups[] = {
 	"du_rgb666",
 	"du_rgb888",
 	"du_clk_out_0",
 	"du_clk_out_1",
 	"du_sync",
-	"du_cde_disp",
+	"du_oddf",
+	"du_cde",
+	"du_disp",
 };
 
 static const char * const du0_groups[] = {
@@ -4103,6 +4462,37 @@ static const char * const sdhi2_groups[] = {
 	"sdhi2_wp",
 };
 
+static const char * const ssi_groups[] = {
+	"ssi0_data",
+	"ssi0_data_b",
+	"ssi0129_ctrl",
+	"ssi0129_ctrl_b",
+	"ssi1_data",
+	"ssi1_data_b",
+	"ssi1_ctrl",
+	"ssi1_ctrl_b",
+	"ssi2_data",
+	"ssi2_ctrl",
+	"ssi3_data",
+	"ssi34_ctrl",
+	"ssi4_data",
+	"ssi4_ctrl",
+	"ssi5_data",
+	"ssi5_ctrl",
+	"ssi6_data",
+	"ssi6_ctrl",
+	"ssi7_data",
+	"ssi7_data_b",
+	"ssi78_ctrl",
+	"ssi78_ctrl_b",
+	"ssi8_data",
+	"ssi8_data_b",
+	"ssi9_data",
+	"ssi9_data_b",
+	"ssi9_ctrl",
+	"ssi9_ctrl_b",
+};
+
 static const char * const usb0_groups[] = {
 	"usb0",
 };
@@ -4152,6 +4542,7 @@ static const char * const vin2_groups[] = {
 };
 
 static const struct sh_pfc_function pinmux_functions[] = {
+	SH_PFC_FUNCTION(audio_clk),
 	SH_PFC_FUNCTION(du),
 	SH_PFC_FUNCTION(du0),
 	SH_PFC_FUNCTION(du1),
@@ -4187,6 +4578,7 @@ static const struct sh_pfc_function pinmux_functions[] = {
 	SH_PFC_FUNCTION(sdhi0),
 	SH_PFC_FUNCTION(sdhi1),
 	SH_PFC_FUNCTION(sdhi2),
+	SH_PFC_FUNCTION(ssi),
 	SH_PFC_FUNCTION(usb0),
 	SH_PFC_FUNCTION(usb1),
 	SH_PFC_FUNCTION(vin0),
@@ -5288,7 +5680,7 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		/* SEL_SCIF3 [2] */
 		FN_SEL_SCIF3_0, FN_SEL_SCIF3_1, FN_SEL_SCIF3_2, FN_SEL_SCIF3_3,
 		/* SEL_IEB [2] */
-		FN_SEL_IEB_0, FN_SEL_IEB_1, FN_SEL_IEB_2,
+		FN_SEL_IEB_0, FN_SEL_IEB_1, FN_SEL_IEB_2, 0,
 		/* SEL_MMC [1] */
 		FN_SEL_MMC_0, FN_SEL_MMC_1,
 		/* SEL_SCIF5 [1] */
diff --git a/drivers/pinctrl/sh-pfc/pfc-sh73a0.c b/drivers/pinctrl/sh-pfc/pfc-sh73a0.c
index 6f6ba100994..ee370de4609 100644
--- a/drivers/pinctrl/sh-pfc/pfc-sh73a0.c
+++ b/drivers/pinctrl/sh-pfc/pfc-sh73a0.c
@@ -26,7 +26,9 @@
 #include <linux/regulator/machine.h>
 #include <linux/slab.h>
 
+#ifndef CONFIG_ARCH_MULTIPLATFORM
 #include <mach/irqs.h>
+#endif
 
 #include "core.h"
 #include "sh_pfc.h"
diff --git a/drivers/pinctrl/sh-pfc/sh_pfc.h b/drivers/pinctrl/sh-pfc/sh_pfc.h
index ab8fd258d9e..d482c40b012 100644
--- a/drivers/pinctrl/sh-pfc/sh_pfc.h
+++ b/drivers/pinctrl/sh-pfc/sh_pfc.h
@@ -98,8 +98,13 @@ struct pinmux_irq {
 	const short *gpios;
 };
 
+#ifdef CONFIG_ARCH_MULTIPLATFORM
+#define PINMUX_IRQ(irq_nr, ids...)			   \
+	{ .gpios = (const short []) { ids, -1 } }
+#else
 #define PINMUX_IRQ(irq_nr, ids...)			   \
 	{ .irq = irq_nr, .gpios = (const short []) { ids, -1 } }
+#endif
 
 struct pinmux_range {
 	u16 begin;
diff --git a/drivers/pinctrl/sirf/pinctrl-sirf.c b/drivers/pinctrl/sirf/pinctrl-sirf.c
index 76502aab2cb..014f5b1fee5 100644
--- a/drivers/pinctrl/sirf/pinctrl-sirf.c
+++ b/drivers/pinctrl/sirf/pinctrl-sirf.c
@@ -14,8 +14,6 @@
 #include <linux/io.h>
 #include <linux/slab.h>
 #include <linux/err.h>
-#include <linux/irqdomain.h>
-#include <linux/irqchip/chained_irq.h>
 #include <linux/pinctrl/pinctrl.h>
 #include <linux/pinctrl/pinmux.h>
 #include <linux/pinctrl/consumer.h>
@@ -27,22 +25,23 @@
 #include <linux/bitops.h>
 #include <linux/gpio.h>
 #include <linux/of_gpio.h>
-#include <asm/mach/irq.h>
 
 #include "pinctrl-sirf.h"
 
 #define DRIVER_NAME "pinmux-sirf"
 
 struct sirfsoc_gpio_bank {
-	struct of_mm_gpio_chip chip;
-	struct irq_domain *domain;
 	int id;
 	int parent_irq;
 	spinlock_t lock;
+};
+
+struct sirfsoc_gpio_chip {
+	struct of_mm_gpio_chip chip;
 	bool is_marco; /* for marco, some registers are different with prima2 */
+	struct sirfsoc_gpio_bank sgpio_bank[SIRFSOC_GPIO_NO_OF_BANKS];
 };
 
-static struct sirfsoc_gpio_bank sgpio_bank[SIRFSOC_GPIO_NO_OF_BANKS];
 static DEFINE_SPINLOCK(sgpio_lock);
 
 static struct sirfsoc_pin_group *sirfsoc_pin_groups;
@@ -255,37 +254,6 @@ static struct pinctrl_desc sirfsoc_pinmux_desc = {
 	.owner = THIS_MODULE,
 };
 
-/*
- * Todo: bind irq_chip to every pinctrl_gpio_range
- */
-static struct pinctrl_gpio_range sirfsoc_gpio_ranges[] = {
-	{
-		.name = "sirfsoc-gpio*",
-		.id = 0,
-		.base = 0,
-		.pin_base = 0,
-		.npins = 32,
-	}, {
-		.name = "sirfsoc-gpio*",
-		.id = 1,
-		.base = 32,
-		.pin_base = 32,
-		.npins = 32,
-	}, {
-		.name = "sirfsoc-gpio*",
-		.id = 2,
-		.base = 64,
-		.pin_base = 64,
-		.npins = 32,
-	}, {
-		.name = "sirfsoc-gpio*",
-		.id = 3,
-		.base = 96,
-		.pin_base = 96,
-		.npins = 19,
-	},
-};
-
 static void __iomem *sirfsoc_rsc_of_iomap(void)
 {
 	const struct of_device_id rsc_ids[]  = {
@@ -303,19 +271,16 @@ static void __iomem *sirfsoc_rsc_of_iomap(void)
 }
 
 static int sirfsoc_gpio_of_xlate(struct gpio_chip *gc,
-       const struct of_phandle_args *gpiospec,
-       u32 *flags)
+	const struct of_phandle_args *gpiospec,
+	u32 *flags)
 {
-       if (gpiospec->args[0] > SIRFSOC_GPIO_NO_OF_BANKS * SIRFSOC_GPIO_BANK_SIZE)
-		return -EINVAL;
-
-       if (gc != &sgpio_bank[gpiospec->args[0] / SIRFSOC_GPIO_BANK_SIZE].chip.gc)
+	if (gpiospec->args[0] > SIRFSOC_GPIO_NO_OF_BANKS * SIRFSOC_GPIO_BANK_SIZE)
 		return -EINVAL;
 
-       if (flags)
+	if (flags)
 		*flags = gpiospec->args[1];
 
-       return gpiospec->args[0] % SIRFSOC_GPIO_BANK_SIZE;
+	return gpiospec->args[0];
 }
 
 static const struct of_device_id pinmux_ids[] = {
@@ -331,7 +296,6 @@ static int sirfsoc_pinmux_probe(struct platform_device *pdev)
 	struct sirfsoc_pmx *spmx;
 	struct device_node *np = pdev->dev.of_node;
 	const struct sirfsoc_pinctrl_data *pdata;
-	int i;
 
 	/* Create state holders etc for this driver */
 	spmx = devm_kzalloc(&pdev->dev, sizeof(*spmx), GFP_KERNEL);
@@ -375,11 +339,6 @@ static int sirfsoc_pinmux_probe(struct platform_device *pdev)
 		goto out_no_pmx;
 	}
 
-	for (i = 0; i < ARRAY_SIZE(sirfsoc_gpio_ranges); i++) {
-		sirfsoc_gpio_ranges[i].gc = &sgpio_bank[i].chip.gc;
-		pinctrl_add_gpio_range(spmx->pmx, &sirfsoc_gpio_ranges[i]);
-	}
-
 	dev_info(&pdev->dev, "initialized SIRFSOC pinmux driver\n");
 
 	return 0;
@@ -464,34 +423,28 @@ static int __init sirfsoc_pinmux_init(void)
 }
 arch_initcall(sirfsoc_pinmux_init);
 
-static inline int sirfsoc_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
+static inline struct sirfsoc_gpio_chip *to_sirfsoc_gpio(struct gpio_chip *gc)
 {
-	struct sirfsoc_gpio_bank *bank = container_of(to_of_mm_gpio_chip(chip),
-		struct sirfsoc_gpio_bank, chip);
-
-	return irq_create_mapping(bank->domain, offset + bank->id *
-		SIRFSOC_GPIO_BANK_SIZE);
-}
-
-static inline int sirfsoc_gpio_to_offset(unsigned int gpio)
-{
-	return gpio % SIRFSOC_GPIO_BANK_SIZE;
+	return container_of(gc, struct sirfsoc_gpio_chip, chip.gc);
 }
 
-static inline struct sirfsoc_gpio_bank *sirfsoc_gpio_to_bank(unsigned int gpio)
+static inline struct sirfsoc_gpio_bank *
+sirfsoc_gpio_to_bank(struct sirfsoc_gpio_chip *sgpio, unsigned int offset)
 {
-	return &sgpio_bank[gpio / SIRFSOC_GPIO_BANK_SIZE];
+	return &sgpio->sgpio_bank[offset / SIRFSOC_GPIO_BANK_SIZE];
 }
 
-static inline struct sirfsoc_gpio_bank *sirfsoc_irqchip_to_bank(struct gpio_chip *chip)
+static inline int sirfsoc_gpio_to_bankoff(unsigned int offset)
 {
-	return container_of(to_of_mm_gpio_chip(chip), struct sirfsoc_gpio_bank, chip);
+	return offset % SIRFSOC_GPIO_BANK_SIZE;
 }
 
 static void sirfsoc_gpio_irq_ack(struct irq_data *d)
 {
-	struct sirfsoc_gpio_bank *bank = irq_data_get_irq_chip_data(d);
-	int idx = d->hwirq % SIRFSOC_GPIO_BANK_SIZE;
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(gc);
+	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, d->hwirq);
+	int idx = sirfsoc_gpio_to_bankoff(d->hwirq);
 	u32 val, offset;
 	unsigned long flags;
 
@@ -499,14 +452,16 @@ static void sirfsoc_gpio_irq_ack(struct irq_data *d)
 
 	spin_lock_irqsave(&sgpio_lock, flags);
 
-	val = readl(bank->chip.regs + offset);
+	val = readl(sgpio->chip.regs + offset);
 
-	writel(val, bank->chip.regs + offset);
+	writel(val, sgpio->chip.regs + offset);
 
 	spin_unlock_irqrestore(&sgpio_lock, flags);
 }
 
-static void __sirfsoc_gpio_irq_mask(struct sirfsoc_gpio_bank *bank, int idx)
+static void __sirfsoc_gpio_irq_mask(struct sirfsoc_gpio_chip *sgpio,
+				    struct sirfsoc_gpio_bank *bank,
+				    int idx)
 {
 	u32 val, offset;
 	unsigned long flags;
@@ -515,25 +470,29 @@ static void __sirfsoc_gpio_irq_mask(struct sirfsoc_gpio_bank *bank, int idx)
 
 	spin_lock_irqsave(&sgpio_lock, flags);
 
-	val = readl(bank->chip.regs + offset);
+	val = readl(sgpio->chip.regs + offset);
 	val &= ~SIRFSOC_GPIO_CTL_INTR_EN_MASK;
 	val &= ~SIRFSOC_GPIO_CTL_INTR_STS_MASK;
-	writel(val, bank->chip.regs + offset);
+	writel(val, sgpio->chip.regs + offset);
 
 	spin_unlock_irqrestore(&sgpio_lock, flags);
 }
 
 static void sirfsoc_gpio_irq_mask(struct irq_data *d)
 {
-	struct sirfsoc_gpio_bank *bank = irq_data_get_irq_chip_data(d);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(gc);
+	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, d->hwirq);
 
-	__sirfsoc_gpio_irq_mask(bank, d->hwirq % SIRFSOC_GPIO_BANK_SIZE);
+	__sirfsoc_gpio_irq_mask(sgpio, bank, d->hwirq % SIRFSOC_GPIO_BANK_SIZE);
 }
 
 static void sirfsoc_gpio_irq_unmask(struct irq_data *d)
 {
-	struct sirfsoc_gpio_bank *bank = irq_data_get_irq_chip_data(d);
-	int idx = d->hwirq % SIRFSOC_GPIO_BANK_SIZE;
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(gc);
+	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, d->hwirq);
+	int idx = sirfsoc_gpio_to_bankoff(d->hwirq);
 	u32 val, offset;
 	unsigned long flags;
 
@@ -541,18 +500,20 @@ static void sirfsoc_gpio_irq_unmask(struct irq_data *d)
 
 	spin_lock_irqsave(&sgpio_lock, flags);
 
-	val = readl(bank->chip.regs + offset);
+	val = readl(sgpio->chip.regs + offset);
 	val &= ~SIRFSOC_GPIO_CTL_INTR_STS_MASK;
 	val |= SIRFSOC_GPIO_CTL_INTR_EN_MASK;
-	writel(val, bank->chip.regs + offset);
+	writel(val, sgpio->chip.regs + offset);
 
 	spin_unlock_irqrestore(&sgpio_lock, flags);
 }
 
 static int sirfsoc_gpio_irq_type(struct irq_data *d, unsigned type)
 {
-	struct sirfsoc_gpio_bank *bank = irq_data_get_irq_chip_data(d);
-	int idx = d->hwirq % SIRFSOC_GPIO_BANK_SIZE;
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(gc);
+	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, d->hwirq);
+	int idx = sirfsoc_gpio_to_bankoff(d->hwirq);
 	u32 val, offset;
 	unsigned long flags;
 
@@ -560,7 +521,7 @@ static int sirfsoc_gpio_irq_type(struct irq_data *d, unsigned type)
 
 	spin_lock_irqsave(&sgpio_lock, flags);
 
-	val = readl(bank->chip.regs + offset);
+	val = readl(sgpio->chip.regs + offset);
 	val &= ~(SIRFSOC_GPIO_CTL_INTR_STS_MASK | SIRFSOC_GPIO_CTL_OUT_EN_MASK);
 
 	switch (type) {
@@ -588,53 +549,41 @@ static int sirfsoc_gpio_irq_type(struct irq_data *d, unsigned type)
 		break;
 	}
 
-	writel(val, bank->chip.regs + offset);
+	writel(val, sgpio->chip.regs + offset);
 
 	spin_unlock_irqrestore(&sgpio_lock, flags);
 
 	return 0;
 }
 
-static int sirfsoc_gpio_irq_reqres(struct irq_data *d)
-{
-	struct sirfsoc_gpio_bank *bank = irq_data_get_irq_chip_data(d);
-
-	if (gpio_lock_as_irq(&bank->chip.gc, d->hwirq % SIRFSOC_GPIO_BANK_SIZE)) {
-		dev_err(bank->chip.gc.dev,
-			"unable to lock HW IRQ %lu for IRQ\n",
-			d->hwirq);
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static void sirfsoc_gpio_irq_relres(struct irq_data *d)
-{
-	struct sirfsoc_gpio_bank *bank = irq_data_get_irq_chip_data(d);
-
-	gpio_unlock_as_irq(&bank->chip.gc, d->hwirq % SIRFSOC_GPIO_BANK_SIZE);
-}
-
 static struct irq_chip sirfsoc_irq_chip = {
 	.name = "sirf-gpio-irq",
 	.irq_ack = sirfsoc_gpio_irq_ack,
 	.irq_mask = sirfsoc_gpio_irq_mask,
 	.irq_unmask = sirfsoc_gpio_irq_unmask,
 	.irq_set_type = sirfsoc_gpio_irq_type,
-	.irq_request_resources = sirfsoc_gpio_irq_reqres,
-	.irq_release_resources = sirfsoc_gpio_irq_relres,
 };
 
 static void sirfsoc_gpio_handle_irq(unsigned int irq, struct irq_desc *desc)
 {
-	struct sirfsoc_gpio_bank *bank = irq_get_handler_data(irq);
+	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
+	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(gc);
+	struct sirfsoc_gpio_bank *bank;
 	u32 status, ctrl;
 	int idx = 0;
 	struct irq_chip *chip = irq_get_chip(irq);
+	int i;
+
+	for (i = 0; i < SIRFSOC_GPIO_NO_OF_BANKS; i++) {
+		bank = &sgpio->sgpio_bank[i];
+		if (bank->parent_irq == irq)
+			break;
+	}
+	BUG_ON(i == SIRFSOC_GPIO_NO_OF_BANKS);
 
 	chained_irq_enter(chip, desc);
 
-	status = readl(bank->chip.regs + SIRFSOC_GPIO_INT_STATUS(bank->id));
+	status = readl(sgpio->chip.regs + SIRFSOC_GPIO_INT_STATUS(bank->id));
 	if (!status) {
 		printk(KERN_WARNING
 			"%s: gpio id %d status %#x no interrupt is flaged\n",
@@ -644,7 +593,7 @@ static void sirfsoc_gpio_handle_irq(unsigned int irq, struct irq_desc *desc)
 	}
 
 	while (status) {
-		ctrl = readl(bank->chip.regs + SIRFSOC_GPIO_CTRL(bank->id, idx));
+		ctrl = readl(sgpio->chip.regs + SIRFSOC_GPIO_CTRL(bank->id, idx));
 
 		/*
 		 * Here we must check whether the corresponding GPIO's interrupt
@@ -653,7 +602,7 @@ static void sirfsoc_gpio_handle_irq(unsigned int irq, struct irq_desc *desc)
 		if ((status & 0x1) && (ctrl & SIRFSOC_GPIO_CTL_INTR_EN_MASK)) {
 			pr_debug("%s: gpio id %d idx %d happens\n",
 				__func__, bank->id, idx);
-			generic_handle_irq(irq_find_mapping(bank->domain, idx +
+			generic_handle_irq(irq_find_mapping(gc->irqdomain, idx +
 					bank->id * SIRFSOC_GPIO_BANK_SIZE));
 		}
 
@@ -664,18 +613,20 @@ static void sirfsoc_gpio_handle_irq(unsigned int irq, struct irq_desc *desc)
 	chained_irq_exit(chip, desc);
 }
 
-static inline void sirfsoc_gpio_set_input(struct sirfsoc_gpio_bank *bank, unsigned ctrl_offset)
+static inline void sirfsoc_gpio_set_input(struct sirfsoc_gpio_chip *sgpio,
+					  unsigned ctrl_offset)
 {
 	u32 val;
 
-	val = readl(bank->chip.regs + ctrl_offset);
+	val = readl(sgpio->chip.regs + ctrl_offset);
 	val &= ~SIRFSOC_GPIO_CTL_OUT_EN_MASK;
-	writel(val, bank->chip.regs + ctrl_offset);
+	writel(val, sgpio->chip.regs + ctrl_offset);
 }
 
 static int sirfsoc_gpio_request(struct gpio_chip *chip, unsigned offset)
 {
-	struct sirfsoc_gpio_bank *bank = sirfsoc_irqchip_to_bank(chip);
+	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(chip);
+	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, offset);
 	unsigned long flags;
 
 	if (pinctrl_request_gpio(chip->base + offset))
@@ -687,8 +638,8 @@ static int sirfsoc_gpio_request(struct gpio_chip *chip, unsigned offset)
 	 * default status:
 	 * set direction as input and mask irq
 	 */
-	sirfsoc_gpio_set_input(bank, SIRFSOC_GPIO_CTRL(bank->id, offset));
-	__sirfsoc_gpio_irq_mask(bank, offset);
+	sirfsoc_gpio_set_input(sgpio, SIRFSOC_GPIO_CTRL(bank->id, offset));
+	__sirfsoc_gpio_irq_mask(sgpio, bank, offset);
 
 	spin_unlock_irqrestore(&bank->lock, flags);
 
@@ -697,13 +648,14 @@ static int sirfsoc_gpio_request(struct gpio_chip *chip, unsigned offset)
 
 static void sirfsoc_gpio_free(struct gpio_chip *chip, unsigned offset)
 {
-	struct sirfsoc_gpio_bank *bank = sirfsoc_irqchip_to_bank(chip);
+	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(chip);
+	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, offset);
 	unsigned long flags;
 
 	spin_lock_irqsave(&bank->lock, flags);
 
-	__sirfsoc_gpio_irq_mask(bank, offset);
-	sirfsoc_gpio_set_input(bank, SIRFSOC_GPIO_CTRL(bank->id, offset));
+	__sirfsoc_gpio_irq_mask(sgpio, bank, offset);
+	sirfsoc_gpio_set_input(sgpio, SIRFSOC_GPIO_CTRL(bank->id, offset));
 
 	spin_unlock_irqrestore(&bank->lock, flags);
 
@@ -712,8 +664,9 @@ static void sirfsoc_gpio_free(struct gpio_chip *chip, unsigned offset)
 
 static int sirfsoc_gpio_direction_input(struct gpio_chip *chip, unsigned gpio)
 {
-	struct sirfsoc_gpio_bank *bank = sirfsoc_irqchip_to_bank(chip);
-	int idx = sirfsoc_gpio_to_offset(gpio);
+	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(chip);
+	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, gpio);
+	int idx = sirfsoc_gpio_to_bankoff(gpio);
 	unsigned long flags;
 	unsigned offset;
 
@@ -721,22 +674,24 @@ static int sirfsoc_gpio_direction_input(struct gpio_chip *chip, unsigned gpio)
 
 	spin_lock_irqsave(&bank->lock, flags);
 
-	sirfsoc_gpio_set_input(bank, offset);
+	sirfsoc_gpio_set_input(sgpio, offset);
 
 	spin_unlock_irqrestore(&bank->lock, flags);
 
 	return 0;
 }
 
-static inline void sirfsoc_gpio_set_output(struct sirfsoc_gpio_bank *bank, unsigned offset,
-	int value)
+static inline void sirfsoc_gpio_set_output(struct sirfsoc_gpio_chip *sgpio,
+					   struct sirfsoc_gpio_bank *bank,
+					   unsigned offset,
+					   int value)
 {
 	u32 out_ctrl;
 	unsigned long flags;
 
 	spin_lock_irqsave(&bank->lock, flags);
 
-	out_ctrl = readl(bank->chip.regs + offset);
+	out_ctrl = readl(sgpio->chip.regs + offset);
 	if (value)
 		out_ctrl |= SIRFSOC_GPIO_CTL_DATAOUT_MASK;
 	else
@@ -744,15 +699,16 @@ static inline void sirfsoc_gpio_set_output(struct sirfsoc_gpio_bank *bank, unsig
 
 	out_ctrl &= ~SIRFSOC_GPIO_CTL_INTR_EN_MASK;
 	out_ctrl |= SIRFSOC_GPIO_CTL_OUT_EN_MASK;
-	writel(out_ctrl, bank->chip.regs + offset);
+	writel(out_ctrl, sgpio->chip.regs + offset);
 
 	spin_unlock_irqrestore(&bank->lock, flags);
 }
 
 static int sirfsoc_gpio_direction_output(struct gpio_chip *chip, unsigned gpio, int value)
 {
-	struct sirfsoc_gpio_bank *bank = sirfsoc_irqchip_to_bank(chip);
-	int idx = sirfsoc_gpio_to_offset(gpio);
+	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(chip);
+	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, gpio);
+	int idx = sirfsoc_gpio_to_bankoff(gpio);
 	u32 offset;
 	unsigned long flags;
 
@@ -760,7 +716,7 @@ static int sirfsoc_gpio_direction_output(struct gpio_chip *chip, unsigned gpio,
 
 	spin_lock_irqsave(&sgpio_lock, flags);
 
-	sirfsoc_gpio_set_output(bank, offset, value);
+	sirfsoc_gpio_set_output(sgpio, bank, offset, value);
 
 	spin_unlock_irqrestore(&sgpio_lock, flags);
 
@@ -769,13 +725,14 @@ static int sirfsoc_gpio_direction_output(struct gpio_chip *chip, unsigned gpio,
 
 static int sirfsoc_gpio_get_value(struct gpio_chip *chip, unsigned offset)
 {
-	struct sirfsoc_gpio_bank *bank = sirfsoc_irqchip_to_bank(chip);
+	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(chip);
+	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, offset);
 	u32 val;
 	unsigned long flags;
 
 	spin_lock_irqsave(&bank->lock, flags);
 
-	val = readl(bank->chip.regs + SIRFSOC_GPIO_CTRL(bank->id, offset));
+	val = readl(sgpio->chip.regs + SIRFSOC_GPIO_CTRL(bank->id, offset));
 
 	spin_unlock_irqrestore(&bank->lock, flags);
 
@@ -785,44 +742,25 @@ static int sirfsoc_gpio_get_value(struct gpio_chip *chip, unsigned offset)
 static void sirfsoc_gpio_set_value(struct gpio_chip *chip, unsigned offset,
 	int value)
 {
-	struct sirfsoc_gpio_bank *bank = sirfsoc_irqchip_to_bank(chip);
+	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(chip);
+	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, offset);
 	u32 ctrl;
 	unsigned long flags;
 
 	spin_lock_irqsave(&bank->lock, flags);
 
-	ctrl = readl(bank->chip.regs + SIRFSOC_GPIO_CTRL(bank->id, offset));
+	ctrl = readl(sgpio->chip.regs + SIRFSOC_GPIO_CTRL(bank->id, offset));
 	if (value)
 		ctrl |= SIRFSOC_GPIO_CTL_DATAOUT_MASK;
 	else
 		ctrl &= ~SIRFSOC_GPIO_CTL_DATAOUT_MASK;
-	writel(ctrl, bank->chip.regs + SIRFSOC_GPIO_CTRL(bank->id, offset));
+	writel(ctrl, sgpio->chip.regs + SIRFSOC_GPIO_CTRL(bank->id, offset));
 
 	spin_unlock_irqrestore(&bank->lock, flags);
 }
 
-static int sirfsoc_gpio_irq_map(struct irq_domain *d, unsigned int irq,
-				irq_hw_number_t hwirq)
-{
-	struct sirfsoc_gpio_bank *bank = d->host_data;
-
-	if (!bank)
-		return -EINVAL;
-
-	irq_set_chip(irq, &sirfsoc_irq_chip);
-	irq_set_handler(irq, handle_level_irq);
-	irq_set_chip_data(irq, bank + hwirq / SIRFSOC_GPIO_BANK_SIZE);
-	set_irq_flags(irq, IRQF_VALID);
-
-	return 0;
-}
-
-static const struct irq_domain_ops sirfsoc_gpio_irq_simple_ops = {
-	.map = sirfsoc_gpio_irq_map,
-	.xlate = irq_domain_xlate_twocell,
-};
-
-static void sirfsoc_gpio_set_pullup(const u32 *pullups)
+static void sirfsoc_gpio_set_pullup(struct sirfsoc_gpio_chip *sgpio,
+				    const u32 *pullups)
 {
 	int i, n;
 	const unsigned long *p = (const unsigned long *)pullups;
@@ -830,15 +768,16 @@ static void sirfsoc_gpio_set_pullup(const u32 *pullups)
 	for (i = 0; i < SIRFSOC_GPIO_NO_OF_BANKS; i++) {
 		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);
+			u32 val = readl(sgpio->chip.regs + offset);
 			val |= SIRFSOC_GPIO_CTL_PULL_MASK;
 			val |= SIRFSOC_GPIO_CTL_PULL_HIGH;
-			writel(val, sgpio_bank[i].chip.regs + offset);
+			writel(val, sgpio->chip.regs + offset);
 		}
 	}
 }
 
-static void sirfsoc_gpio_set_pulldown(const u32 *pulldowns)
+static void sirfsoc_gpio_set_pulldown(struct sirfsoc_gpio_chip *sgpio,
+				      const u32 *pulldowns)
 {
 	int i, n;
 	const unsigned long *p = (const unsigned long *)pulldowns;
@@ -846,10 +785,10 @@ static void sirfsoc_gpio_set_pulldown(const u32 *pulldowns)
 	for (i = 0; i < SIRFSOC_GPIO_NO_OF_BANKS; i++) {
 		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);
+			u32 val = readl(sgpio->chip.regs + offset);
 			val |= SIRFSOC_GPIO_CTL_PULL_MASK;
 			val &= ~SIRFSOC_GPIO_CTL_PULL_HIGH;
-			writel(val, sgpio_bank[i].chip.regs + offset);
+			writel(val, sgpio->chip.regs + offset);
 		}
 	}
 }
@@ -857,10 +796,10 @@ static void sirfsoc_gpio_set_pulldown(const u32 *pulldowns)
 static int sirfsoc_gpio_probe(struct device_node *np)
 {
 	int i, err = 0;
+	static struct sirfsoc_gpio_chip *sgpio;
 	struct sirfsoc_gpio_bank *bank;
 	void __iomem *regs;
 	struct platform_device *pdev;
-	struct irq_domain *domain;
 	bool is_marco = false;
 
 	u32 pullups[SIRFSOC_GPIO_NO_OF_BANKS], pulldowns[SIRFSOC_GPIO_NO_OF_BANKS];
@@ -869,6 +808,10 @@ static int sirfsoc_gpio_probe(struct device_node *np)
 	if (!pdev)
 		return -ENODEV;
 
+	sgpio = devm_kzalloc(&pdev->dev, sizeof(*sgpio), GFP_KERNEL);
+	if (!sgpio)
+		return -ENOMEM;
+
 	regs = of_iomap(np, 0);
 	if (!regs)
 		return -ENOMEM;
@@ -876,63 +819,76 @@ static int sirfsoc_gpio_probe(struct device_node *np)
 	if (of_device_is_compatible(np, "sirf,marco-pinctrl"))
 		is_marco = 1;
 
-	domain = irq_domain_add_linear(np, SIRFSOC_GPIO_BANK_SIZE * SIRFSOC_GPIO_NO_OF_BANKS,
-		&sirfsoc_gpio_irq_simple_ops, sgpio_bank);
-	if (!domain) {
-		pr_err("%s: Failed to create irqdomain\n", np->full_name);
-			err = -ENOSYS;
+	sgpio->chip.gc.request = sirfsoc_gpio_request;
+	sgpio->chip.gc.free = sirfsoc_gpio_free;
+	sgpio->chip.gc.direction_input = sirfsoc_gpio_direction_input;
+	sgpio->chip.gc.get = sirfsoc_gpio_get_value;
+	sgpio->chip.gc.direction_output = sirfsoc_gpio_direction_output;
+	sgpio->chip.gc.set = sirfsoc_gpio_set_value;
+	sgpio->chip.gc.base = 0;
+	sgpio->chip.gc.ngpio = SIRFSOC_GPIO_BANK_SIZE * SIRFSOC_GPIO_NO_OF_BANKS;
+	sgpio->chip.gc.label = kstrdup(np->full_name, GFP_KERNEL);
+	sgpio->chip.gc.of_node = np;
+	sgpio->chip.gc.of_xlate = sirfsoc_gpio_of_xlate;
+	sgpio->chip.gc.of_gpio_n_cells = 2;
+	sgpio->chip.gc.dev = &pdev->dev;
+	sgpio->chip.regs = regs;
+	sgpio->is_marco = is_marco;
+
+	err = gpiochip_add(&sgpio->chip.gc);
+	if (err) {
+		dev_err(&pdev->dev, "%s: error in probe function with status %d\n",
+			np->full_name, err);
+		goto out;
+	}
+
+	err =  gpiochip_irqchip_add(&sgpio->chip.gc,
+		&sirfsoc_irq_chip,
+		0, handle_level_irq,
+		IRQ_TYPE_NONE);
+	if (err) {
+		dev_err(&pdev->dev,
+			"could not connect irqchip to gpiochip\n");
 		goto out;
 	}
 
 	for (i = 0; i < SIRFSOC_GPIO_NO_OF_BANKS; i++) {
-		bank = &sgpio_bank[i];
+		bank = &sgpio->sgpio_bank[i];
 		spin_lock_init(&bank->lock);
-		bank->chip.gc.request = sirfsoc_gpio_request;
-		bank->chip.gc.free = sirfsoc_gpio_free;
-		bank->chip.gc.direction_input = sirfsoc_gpio_direction_input;
-		bank->chip.gc.get = sirfsoc_gpio_get_value;
-		bank->chip.gc.direction_output = sirfsoc_gpio_direction_output;
-		bank->chip.gc.set = sirfsoc_gpio_set_value;
-		bank->chip.gc.to_irq = sirfsoc_gpio_to_irq;
-		bank->chip.gc.base = i * SIRFSOC_GPIO_BANK_SIZE;
-		bank->chip.gc.ngpio = SIRFSOC_GPIO_BANK_SIZE;
-		bank->chip.gc.label = kstrdup(np->full_name, GFP_KERNEL);
-		bank->chip.gc.of_node = np;
-		bank->chip.gc.of_xlate = sirfsoc_gpio_of_xlate;
-		bank->chip.gc.of_gpio_n_cells = 2;
-		bank->chip.gc.dev = &pdev->dev;
-		bank->chip.regs = regs;
-		bank->id = i;
-		bank->is_marco = is_marco;
 		bank->parent_irq = platform_get_irq(pdev, i);
 		if (bank->parent_irq < 0) {
 			err = bank->parent_irq;
-			goto out;
+			goto out_banks;
 		}
 
-		err = gpiochip_add(&bank->chip.gc);
-		if (err) {
-			pr_err("%s: error in probe function with status %d\n",
-				np->full_name, err);
-			goto out;
-		}
-
-		bank->domain = domain;
+		gpiochip_set_chained_irqchip(&sgpio->chip.gc,
+			&sirfsoc_irq_chip,
+			bank->parent_irq,
+			sirfsoc_gpio_handle_irq);
+	}
 
-		irq_set_chained_handler(bank->parent_irq, sirfsoc_gpio_handle_irq);
-		irq_set_handler_data(bank->parent_irq, bank);
+	err = gpiochip_add_pin_range(&sgpio->chip.gc, dev_name(&pdev->dev),
+		0, 0, SIRFSOC_GPIO_BANK_SIZE * SIRFSOC_GPIO_NO_OF_BANKS);
+	if (err) {
+		dev_err(&pdev->dev,
+			"could not add gpiochip pin range\n");
+		goto out_no_range;
 	}
 
 	if (!of_property_read_u32_array(np, "sirf,pullups", pullups,
 		SIRFSOC_GPIO_NO_OF_BANKS))
-		sirfsoc_gpio_set_pullup(pullups);
+		sirfsoc_gpio_set_pullup(sgpio, pullups);
 
 	if (!of_property_read_u32_array(np, "sirf,pulldowns", pulldowns,
 		SIRFSOC_GPIO_NO_OF_BANKS))
-		sirfsoc_gpio_set_pulldown(pulldowns);
+		sirfsoc_gpio_set_pulldown(sgpio, pulldowns);
 
 	return 0;
 
+out_no_range:
+out_banks:
+	if (gpiochip_remove(&sgpio->chip.gc))
+		dev_err(&pdev->dev, "could not remove gpio chip\n");
 out:
 	iounmap(regs);
 	return err;
diff --git a/drivers/pinctrl/sunxi/Kconfig b/drivers/pinctrl/sunxi/Kconfig
new file mode 100644
index 00000000000..73e0a305ea1
--- /dev/null
+++ b/drivers/pinctrl/sunxi/Kconfig
@@ -0,0 +1,36 @@
+if ARCH_SUNXI
+
+config PINCTRL_SUNXI
+	bool
+
+config PINCTRL_SUNXI_COMMON
+	bool
+	select PINMUX
+	select GENERIC_PINCONF
+
+config PINCTRL_SUN4I_A10
+	def_bool PINCTRL_SUNXI || MACH_SUN4I
+	select PINCTRL_SUNXI_COMMON
+
+config PINCTRL_SUN5I_A10S
+	def_bool PINCTRL_SUNXI || MACH_SUN5I
+	select PINCTRL_SUNXI_COMMON
+
+config PINCTRL_SUN5I_A13
+	def_bool PINCTRL_SUNXI || MACH_SUN5I
+	select PINCTRL_SUNXI_COMMON
+
+config PINCTRL_SUN6I_A31
+	def_bool PINCTRL_SUNXI || MACH_SUN6I
+	select PINCTRL_SUNXI_COMMON
+
+config PINCTRL_SUN6I_A31_R
+	def_bool PINCTRL_SUNXI || MACH_SUN6I
+	depends on RESET_CONTROLLER
+	select PINCTRL_SUNXI_COMMON
+
+config PINCTRL_SUN7I_A20
+	def_bool PINCTRL_SUNXI || MACH_SUN7I
+	select PINCTRL_SUNXI_COMMON
+
+endif
diff --git a/drivers/pinctrl/sunxi/Makefile b/drivers/pinctrl/sunxi/Makefile
new file mode 100644
index 00000000000..0f4461cbe11
--- /dev/null
+++ b/drivers/pinctrl/sunxi/Makefile
@@ -0,0 +1,10 @@
+# Core
+obj-$(CONFIG_PINCTRL_SUNXI_COMMON)	+= pinctrl-sunxi.o
+
+# SoC Drivers
+obj-$(CONFIG_PINCTRL_SUN4I_A10)		+= pinctrl-sun4i-a10.o
+obj-$(CONFIG_PINCTRL_SUN5I_A10S)	+= pinctrl-sun5i-a10s.o
+obj-$(CONFIG_PINCTRL_SUN5I_A13)		+= pinctrl-sun5i-a13.o
+obj-$(CONFIG_PINCTRL_SUN6I_A31)		+= pinctrl-sun6i-a31.o
+obj-$(CONFIG_PINCTRL_SUN6I_A31_R)	+= pinctrl-sun6i-a31-r.o
+obj-$(CONFIG_PINCTRL_SUN7I_A20)		+= pinctrl-sun7i-a20.o
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun4i-a10.c b/drivers/pinctrl/sunxi/pinctrl-sun4i-a10.c
new file mode 100644
index 00000000000..fa1ff7c7e35
--- /dev/null
+++ b/drivers/pinctrl/sunxi/pinctrl-sun4i-a10.c
@@ -0,0 +1,1039 @@
+/*
+ * Allwinner A10 SoCs pinctrl driver.
+ *
+ * Copyright (C) 2014 Maxime Ripard
+ *
+ * Maxime Ripard <maxime.ripard@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.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-sunxi.h"
+
+static const struct sunxi_desc_pin sun4i_a10_pins[] = {
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(A, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(A, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(A, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(A, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD3 */
+		  SUNXI_FUNCTION(0x3, "spi1")),		/* CS1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD2 */
+		  SUNXI_FUNCTION(0x3, "spi3")),		/* CS0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD1 */
+		  SUNXI_FUNCTION(0x3, "spi3")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD0 */
+		  SUNXI_FUNCTION(0x3, "spi3")),		/* MOSI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXCK */
+		  SUNXI_FUNCTION(0x3, "spi3")),		/* MISO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXERR */
+		  SUNXI_FUNCTION(0x3, "spi3")),		/* CS1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 10),
+		  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(A, 11),
+		  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(A, 12),
+		  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(A, 13),
+		  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(A, 14),
+		  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(A, 15),
+		  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(A, 16),
+		  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(A, 17),
+		  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(B, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "pwm")),		/* PWM0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ir0")),		/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ir0")),		/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s"),		/* MCLK */
+		  SUNXI_FUNCTION(0x3, "ac97")),		/* MCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s"),		/* BCLK */
+		  SUNXI_FUNCTION(0x3, "ac97")),		/* BCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s"),		/* LRCK */
+		  SUNXI_FUNCTION(0x3, "ac97")),		/* SYNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s"),		/* DO0 */
+		  SUNXI_FUNCTION(0x3, "ac97")),		/* DO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s")),		/* DO1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s")),		/* DO2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s")),		/* DO3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s"),		/* DI */
+		  SUNXI_FUNCTION(0x3, "ac97")),		/* DI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2")),		/* CS1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* CS0 */
+		  SUNXI_FUNCTION(0x3, "jtag")),		/* MS0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "jtag")),		/* CK0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* MOSI */
+		  SUNXI_FUNCTION(0x3, "jtag")),		/* DO0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* MISO */
+		  SUNXI_FUNCTION(0x3, "jtag")),		/* DI0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 22),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart0"),		/* TX */
+		  SUNXI_FUNCTION(0x3, "ir1")),		/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 23),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart0"),		/* RX */
+		  SUNXI_FUNCTION(0x3, "ir1")),		/* RX */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NWE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* MOSI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NALE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* MISO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCLE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NRE# */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NRB0 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CMD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NRB1 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ0 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ1 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ2 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ3 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQ4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQ5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQ6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQ7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NWP */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE4 */
+		  SUNXI_FUNCTION(0x3, "spi2")),		/* CS0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE5 */
+		  SUNXI_FUNCTION(0x3, "spi2")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE6 */
+		  SUNXI_FUNCTION(0x3, "spi2")),		/* MOSI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 22),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE7 */
+		  SUNXI_FUNCTION(0x3, "spi2")),		/* MISO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 23),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* CS0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 24),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQS */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D0 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D1 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D2 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D3 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D4 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D5 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D6 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VPC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D7 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D8 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D9 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VM3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D10 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D11 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D12 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D13 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D14 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D15 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D16 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VPC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D17 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D18 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D19 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D20 */
+		  SUNXI_FUNCTION(0x3, "csi1")),		/* MCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D21 */
+		  SUNXI_FUNCTION(0x3, "sim")),		/* VPPEN */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 22),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D22 */
+		  SUNXI_FUNCTION(0x3, "sim")),		/* VPPPP */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 23),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D23 */
+		  SUNXI_FUNCTION(0x3, "sim")),		/* DET */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 24),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "sim")),		/* VCCEN */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 25),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* DE */
+		  SUNXI_FUNCTION(0x3, "sim")),		/* RST */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 26),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* HSYNC */
+		  SUNXI_FUNCTION(0x3, "sim")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 27),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* VSYNC */
+		  SUNXI_FUNCTION(0x3, "sim")),		/* SDA */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* PCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* ERR */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* CK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* SYNC */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* HSYNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* DVLD */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* VSYNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D0 */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(E, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D2 */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D3 */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D4 */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* D4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D5 */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* D5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D6 */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* D6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D7 */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* D7 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D1 */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* MSI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D0 */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* DI1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 2),
+		  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(F, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* CMD */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* DO1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 4),
+		  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(F, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D2 */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* CK1 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(G, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(G, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(G, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(G, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(G, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(G, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(G, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(G, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(G, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(G, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(G, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(H, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D0 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAA0 */
+		  SUNXI_FUNCTION(0x4, "uart3"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 0),		/* EINT0 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D1 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAA1 */
+		  SUNXI_FUNCTION(0x4, "uart3"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 1),		/* EINT1 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D2 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAA2 */
+		  SUNXI_FUNCTION(0x4, "uart3"),		/* RTS */
+		  SUNXI_FUNCTION_IRQ(0x6, 2),		/* EINT2 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D3 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAIRQ */
+		  SUNXI_FUNCTION(0x4, "uart3"),		/* CTS */
+		  SUNXI_FUNCTION_IRQ(0x6, 3),		/* EINT3 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D4 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD0 */
+		  SUNXI_FUNCTION(0x4, "uart4"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 4),		/* EINT4 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D5 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD1 */
+		  SUNXI_FUNCTION(0x4, "uart4"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 5),		/* EINT5 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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_IRQ(0x6, 6),		/* EINT6 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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_IRQ(0x6, 7),		/* EINT7 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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_IRQ(0x6, 8),		/* EINT8 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D8 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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_IRQ(0x6, 9),		/* EINT9 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D9 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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_IRQ(0x6, 10),		/* EINT10 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D10 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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_IRQ(0x6, 11),		/* EINT11 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D11 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D12 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD8 */
+		  SUNXI_FUNCTION(0x4, "ps2"),		/* SCK1 */
+		  SUNXI_FUNCTION_IRQ(0x6, 12),		/* EINT12 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D12 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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_IRQ(0x6, 13),		/* EINT13 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D13 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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_IRQ(0x6, 14),		/* EINT14 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D14 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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_IRQ(0x6, 15),		/* EINT15 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D15 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D16 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAD12 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN6 */
+		  SUNXI_FUNCTION_IRQ(0x6, 16),		/* EINT16 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D16 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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_IRQ(0x6, 17),		/* EINT17 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D17 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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_IRQ(0x6, 18),		/* EINT18 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D18 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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_IRQ(0x6, 19),		/* EINT19 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D19 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D20 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATAOE */
+		  SUNXI_FUNCTION(0x4, "can"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 20),		/* EINT20 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D20 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D21 */
+		  SUNXI_FUNCTION(0x3, "pata"),		/* ATADREQ */
+		  SUNXI_FUNCTION(0x4, "can"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 21),		/* EINT21 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D21 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 22),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(H, 23),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(H, 24),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(H, 25),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(H, 26),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(H, 27),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(I, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "pwm")),		/* PWM1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc3")),		/* CMD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc3")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc3")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc3")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc3")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc3")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi0"),		/* CS0 */
+		  SUNXI_FUNCTION(0x3, "uart5"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 22)),		/* EINT22 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi0"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "uart5"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 23)),		/* EINT23 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi0"),		/* MOSI */
+		  SUNXI_FUNCTION(0x3, "uart6"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 24)),		/* EINT24 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi0"),		/* MISO */
+		  SUNXI_FUNCTION(0x3, "uart6"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 25)),		/* EINT25 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi0"),		/* CS1 */
+		  SUNXI_FUNCTION(0x3, "ps2"),		/* SCK1 */
+		  SUNXI_FUNCTION(0x4, "timer4"),	/* TCLKIN0 */
+		  SUNXI_FUNCTION_IRQ(0x6, 26)),		/* EINT26 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS1 */
+		  SUNXI_FUNCTION(0x3, "ps2"),		/* SDA1 */
+		  SUNXI_FUNCTION(0x4, "timer5"),	/* TCLKIN1 */
+		  SUNXI_FUNCTION_IRQ(0x6, 27)),		/* EINT27 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS0 */
+		  SUNXI_FUNCTION(0x3, "uart2"),		/* RTS */
+		  SUNXI_FUNCTION_IRQ(0x6, 28)),		/* EINT28 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "uart2"),		/* CTS */
+		  SUNXI_FUNCTION_IRQ(0x6, 29)),		/* EINT29 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* MOSI */
+		  SUNXI_FUNCTION(0x3, "uart2"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 30)),		/* EINT30 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* MISO */
+		  SUNXI_FUNCTION(0x3, "uart2"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 31)),		/* EINT31 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(I, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ps2"),		/* SDA0 */
+		  SUNXI_FUNCTION(0x3, "uart7"),		/* RX */
+		  SUNXI_FUNCTION(0x4, "hdmi")),		/* HSDA */
+};
+
+static const struct sunxi_pinctrl_desc sun4i_a10_pinctrl_data = {
+	.pins = sun4i_a10_pins,
+	.npins = ARRAY_SIZE(sun4i_a10_pins),
+};
+
+static int sun4i_a10_pinctrl_probe(struct platform_device *pdev)
+{
+	return sunxi_pinctrl_init(pdev,
+				  &sun4i_a10_pinctrl_data);
+}
+
+static struct of_device_id sun4i_a10_pinctrl_match[] = {
+	{ .compatible = "allwinner,sun4i-a10-pinctrl", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, sun4i_a10_pinctrl_match);
+
+static struct platform_driver sun4i_a10_pinctrl_driver = {
+	.probe	= sun4i_a10_pinctrl_probe,
+	.driver	= {
+		.name		= "sun4i-pinctrl",
+		.owner		= THIS_MODULE,
+		.of_match_table	= sun4i_a10_pinctrl_match,
+	},
+};
+module_platform_driver(sun4i_a10_pinctrl_driver);
+
+MODULE_AUTHOR("Maxime Ripard <maxime.ripard@free-electrons.com");
+MODULE_DESCRIPTION("Allwinner A10 pinctrl driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun5i-a10s.c b/drivers/pinctrl/sunxi/pinctrl-sun5i-a10s.c
new file mode 100644
index 00000000000..164d743f526
--- /dev/null
+++ b/drivers/pinctrl/sunxi/pinctrl-sun5i-a10s.c
@@ -0,0 +1,690 @@
+/*
+ * Allwinner A10s SoCs pinctrl driver.
+ *
+ * Copyright (C) 2014 Maxime Ripard
+ *
+ * Maxime Ripard <maxime.ripard@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.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-sunxi.h"
+
+static const struct sunxi_desc_pin sun5i_a10s_pins[] = {
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD3 */
+		  SUNXI_FUNCTION(0x3, "ts0"),		/* CLK */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD2 */
+		  SUNXI_FUNCTION(0x3, "ts0"),		/* ERR */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD1 */
+		  SUNXI_FUNCTION(0x3, "ts0"),		/* SYNC */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD0 */
+		  SUNXI_FUNCTION(0x3, "ts0"),		/* DLVD */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD3 */
+		  SUNXI_FUNCTION(0x3, "ts0"),		/* D0 */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD2 */
+		  SUNXI_FUNCTION(0x3, "ts0"),		/* D1 */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD1 */
+		  SUNXI_FUNCTION(0x3, "ts0"),		/* D2 */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD0 */
+		  SUNXI_FUNCTION(0x3, "ts0"),		/* D3 */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* IN7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXCK */
+		  SUNXI_FUNCTION(0x3, "ts0"),		/* D4 */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* DTR */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXERR */
+		  SUNXI_FUNCTION(0x3, "ts0"),		/* D5 */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* DSR */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXDV */
+		  SUNXI_FUNCTION(0x3, "ts0"),		/* D6 */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* DCD */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* EMDC */
+		  SUNXI_FUNCTION(0x3, "ts0"),		/* D7 */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* RING */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* EMDIO */
+		  SUNXI_FUNCTION(0x3, "uart1"),		/* TX */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXEN */
+		  SUNXI_FUNCTION(0x3, "uart1"),		/* RX */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXCK */
+		  SUNXI_FUNCTION(0x3, "uart1"),		/* CTS */
+		  SUNXI_FUNCTION(0x4, "uart3"),		/* TX */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ECRS */
+		  SUNXI_FUNCTION(0x3, "uart1"),		/* RTS */
+		  SUNXI_FUNCTION(0x4, "uart3"),		/* RX */
+		  SUNXI_FUNCTION(0x5, "keypad")),	/* OUT7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ECOL */
+		  SUNXI_FUNCTION(0x3, "uart2")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXERR */
+		  SUNXI_FUNCTION(0x3, "uart2"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 31)),		/* EINT31 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "pwm"),		/* PWM0 */
+		  SUNXI_FUNCTION_IRQ(0x6, 16)),		/* EINT16 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ir0"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 17)),		/* EINT17 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ir0"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 18)),		/* EINT18 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s"),		/* MCLK */
+		  SUNXI_FUNCTION_IRQ(0x6, 19)),		/* EINT19 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s"),		/* BCLK */
+		  SUNXI_FUNCTION_IRQ(0x6, 20)),		/* EINT20 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s"),		/* LRCK */
+		  SUNXI_FUNCTION_IRQ(0x6, 21)),		/* EINT21 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s"),		/* DO */
+		  SUNXI_FUNCTION_IRQ(0x6, 22)),		/* EINT22 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s"),		/* DI */
+		  SUNXI_FUNCTION_IRQ(0x6, 23)),		/* EINT23 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* CS1 */
+		  SUNXI_FUNCTION_IRQ(0x6, 24)),		/* EINT24 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* CS0 */
+		  SUNXI_FUNCTION(0x3, "jtag"),		/* MS0 */
+		  SUNXI_FUNCTION_IRQ(0x6, 25)),		/* EINT25 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "jtag"),		/* CK0 */
+		  SUNXI_FUNCTION_IRQ(0x6, 26)),		/* EINT26 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* MOSI */
+		  SUNXI_FUNCTION(0x3, "jtag"),		/* DO0 */
+		  SUNXI_FUNCTION_IRQ(0x6, 27)),		/* EINT27 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* MISO */
+		  SUNXI_FUNCTION(0x3, "jtag"),		/* DI0 */
+		  SUNXI_FUNCTION_IRQ(0x6, 28)),		/* EINT28 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart0"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 29)),		/* EINT29 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart0"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 30)),		/* EINT30 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NWE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* MOSI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NALE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* MISO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCLE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE1 */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* CS0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NRE */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NRB0 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CMD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NRB1 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ0 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ1 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ2 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ3 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ4 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ5 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ6 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ7 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NWP */
+		  SUNXI_FUNCTION(0x4, "uart3")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE2 */
+		  SUNXI_FUNCTION(0x4, "uart3")),	/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE3 */
+		  SUNXI_FUNCTION(0x3, "uart2"),		/* TX */
+		  SUNXI_FUNCTION(0x4, "uart3")),	/* CTS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE4 */
+		  SUNXI_FUNCTION(0x3, "uart2"),		/* RX */
+		  SUNXI_FUNCTION(0x4, "uart3")),	/* RTS */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D2 */
+		  SUNXI_FUNCTION(0x3, "uart2")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D3 */
+		  SUNXI_FUNCTION(0x3, "uart2")),	/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D4 */
+		  SUNXI_FUNCTION(0x3, "uart2")),	/* CTS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D5 */
+		  SUNXI_FUNCTION(0x3, "uart2")),	/* RTS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D6 */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ECRS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D7 */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ECOL */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D8 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D9 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D10 */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ERXD0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D11 */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ERXD1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D12 */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ERXD2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D13 */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ERXD3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D14 */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ERXCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D15 */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ERXERR */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D16 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D17 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D18 */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ERXDV */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D19 */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ETXD0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D20 */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ETXD1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D21 */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ETXD2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 22),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D22 */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ETXD3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 23),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D23 */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ETXEN */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 24),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ETXCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 25),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* DE */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* ETXERR */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 26),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* HSYNC */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* EMDC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 27),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* VSYNC */
+		  SUNXI_FUNCTION(0x3, "emac")),		/* EMDIO */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* PCK */
+		  SUNXI_FUNCTION(0x4, "spi2"),		/* CS0 */
+		  SUNXI_FUNCTION_IRQ(0x6, 14)),		/* EINT14 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* ERR */
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* CK */
+		  SUNXI_FUNCTION(0x4, "spi2"),		/* CLK */
+		  SUNXI_FUNCTION_IRQ(0x6, 15)),		/* EINT15 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* SYNC */
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* HSYNC */
+		  SUNXI_FUNCTION(0x4, "spi2")),		/* MOSI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* DVLD */
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* VSYNC */
+		  SUNXI_FUNCTION(0x4, "spi2")),		/* MISO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D0 */
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D0 */
+		  SUNXI_FUNCTION(0x4, "mmc2")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D1 */
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D1 */
+		  SUNXI_FUNCTION(0x4, "mmc2")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D2 */
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D2 */
+		  SUNXI_FUNCTION(0x4, "mmc2")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D3 */
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D3 */
+		  SUNXI_FUNCTION(0x4, "mmc2")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D4 */
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D4 */
+		  SUNXI_FUNCTION(0x4, "mmc2")),		/* CMD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D5 */
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D5 */
+		  SUNXI_FUNCTION(0x4, "mmc2")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D6 */
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D6 */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D7 */
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D7 */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* RX */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D1 */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* MS1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D0 */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* DI1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 2),
+		  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(F, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* CMD */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* DO1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 4),
+		  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(F, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D2 */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* CK1 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x2, "gps"),		/* CLK */
+		  SUNXI_FUNCTION_IRQ(0x6, 0)),		/* EINT0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x2, "gps"),		/* SIGN */
+		  SUNXI_FUNCTION_IRQ(0x6, 1)),		/* EINT1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x2, "gps"),		/* MAG */
+		  SUNXI_FUNCTION_IRQ(0x6, 2)),		/* EINT2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1"),		/* CMD */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 3)),		/* EINT3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1"),		/* CLK */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 4)),		/* EINT4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1"),		/* DO */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* CTS */
+		  SUNXI_FUNCTION_IRQ(0x6, 5)),		/* EINT5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1"),		/* D1 */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* RTS */
+		  SUNXI_FUNCTION(0x5, "uart2"),		/* RTS */
+		  SUNXI_FUNCTION_IRQ(0x6, 6)),		/* EINT6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1"),		/* D2 */
+		  SUNXI_FUNCTION(0x5, "uart2"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 7)),		/* EINT7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1"),		/* D3 */
+		  SUNXI_FUNCTION(0x5, "uart2"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 8)),		/* EINT8 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS0 */
+		  SUNXI_FUNCTION(0x3, "uart3"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 9)),		/* EINT9 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "uart3"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 10)),		/* EINT10 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* MOSI */
+		  SUNXI_FUNCTION(0x3, "uart3"),		/* CTS */
+		  SUNXI_FUNCTION_IRQ(0x6, 11)),		/* EINT11 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* MISO */
+		  SUNXI_FUNCTION(0x3, "uart3"),		/* RTS */
+		  SUNXI_FUNCTION_IRQ(0x6, 12)),		/* EINT12 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS1 */
+		  SUNXI_FUNCTION(0x3, "uart3"),		/* PWM1 */
+		  SUNXI_FUNCTION(0x5, "uart2"),		/* CTS */
+		  SUNXI_FUNCTION_IRQ(0x6, 13)),		/* EINT13 */
+};
+
+static const struct sunxi_pinctrl_desc sun5i_a10s_pinctrl_data = {
+	.pins = sun5i_a10s_pins,
+	.npins = ARRAY_SIZE(sun5i_a10s_pins),
+};
+
+static int sun5i_a10s_pinctrl_probe(struct platform_device *pdev)
+{
+	return sunxi_pinctrl_init(pdev,
+				  &sun5i_a10s_pinctrl_data);
+}
+
+static struct of_device_id sun5i_a10s_pinctrl_match[] = {
+	{ .compatible = "allwinner,sun5i-a10s-pinctrl", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, sun5i_a10s_pinctrl_match);
+
+static struct platform_driver sun5i_a10s_pinctrl_driver = {
+	.probe	= sun5i_a10s_pinctrl_probe,
+	.driver	= {
+		.name		= "sun5i-a10s-pinctrl",
+		.owner		= THIS_MODULE,
+		.of_match_table	= sun5i_a10s_pinctrl_match,
+	},
+};
+module_platform_driver(sun5i_a10s_pinctrl_driver);
+
+MODULE_AUTHOR("Maxime Ripard <maxime.ripard@free-electrons.com");
+MODULE_DESCRIPTION("Allwinner A10s pinctrl driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun5i-a13.c b/drivers/pinctrl/sunxi/pinctrl-sun5i-a13.c
new file mode 100644
index 00000000000..1188a2b7b98
--- /dev/null
+++ b/drivers/pinctrl/sunxi/pinctrl-sun5i-a13.c
@@ -0,0 +1,411 @@
+/*
+ * Allwinner A13 SoCs pinctrl driver.
+ *
+ * Copyright (C) 2014 Maxime Ripard
+ *
+ * Maxime Ripard <maxime.ripard@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.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-sunxi.h"
+
+static const struct sunxi_desc_pin sun5i_a13_pins[] = {
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "pwm"),
+		  SUNXI_FUNCTION_IRQ(0x6, 16)),		/* EINT16 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ir0"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 17)),		/* EINT17 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ir0"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 18)),		/* EINT18 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* CS1 */
+		  SUNXI_FUNCTION_IRQ(0x6, 24)),		/* EINT24 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SDA */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NWE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* MOSI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NALE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* MISO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCLE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE1 */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* CS0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NRE */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NRB0 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CMD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NRB1 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ0 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ1 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ2 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ3 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ4 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ5 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ6 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ7 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D7 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQS */
+		  SUNXI_FUNCTION(0x4, "uart3")),	/* RTS */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D7 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D10 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D11 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D12 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D13 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D14 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D15 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D18 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D19 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D20 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D21 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 22),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D22 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 23),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D23 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 24),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 25),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* DE */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 26),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* HSYNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 27),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* VSYNC */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* PCLK */
+		  SUNXI_FUNCTION(0x4, "spi2"),		/* CS0 */
+		  SUNXI_FUNCTION_IRQ(0x6, 14)),		/* EINT14 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* MCLK */
+		  SUNXI_FUNCTION(0x4, "spi2"),		/* CLK */
+		  SUNXI_FUNCTION_IRQ(0x6, 15)),		/* EINT15 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* HSYNC */
+		  SUNXI_FUNCTION(0x4, "spi2")),		/* MOSI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* VSYNC */
+		  SUNXI_FUNCTION(0x4, "spi2")),		/* MISO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D0 */
+		  SUNXI_FUNCTION(0x4, "mmc2")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D1 */
+		  SUNXI_FUNCTION(0x4, "mmc2")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D2 */
+		  SUNXI_FUNCTION(0x4, "mmc2")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D3 */
+		  SUNXI_FUNCTION(0x4, "mmc2")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D4 */
+		  SUNXI_FUNCTION(0x4, "mmc2")),		/* CMD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "csi0"),		/* D5 */
+		  SUNXI_FUNCTION(0x4, "mmc2")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 10),
+		  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(E, 11),
+		  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(F, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0")),		/* CMD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0")),		/* D2 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION_IRQ(0x6, 0)),		/* EINT0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION_IRQ(0x6, 1)),		/* EINT1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION_IRQ(0x6, 2)),		/* EINT2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1"),		/* CMD */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 3)),		/* EINT3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1"),		/* CLK */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 4)),		/* EINT4 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS0 */
+		  SUNXI_FUNCTION(0x3, "uart3"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 9)),		/* EINT9 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "uart3"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 10)),		/* EINT10 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* MOSI */
+		  SUNXI_FUNCTION(0x3, "uart3"),		/* CTS */
+		  SUNXI_FUNCTION_IRQ(0x6, 11)),		/* EINT11 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* MISO */
+		  SUNXI_FUNCTION(0x3, "uart3"),		/* RTS */
+		  SUNXI_FUNCTION_IRQ(0x6, 12)),		/* EINT12 */
+};
+
+static const struct sunxi_pinctrl_desc sun5i_a13_pinctrl_data = {
+	.pins = sun5i_a13_pins,
+	.npins = ARRAY_SIZE(sun5i_a13_pins),
+};
+
+static int sun5i_a13_pinctrl_probe(struct platform_device *pdev)
+{
+	return sunxi_pinctrl_init(pdev,
+				  &sun5i_a13_pinctrl_data);
+}
+
+static struct of_device_id sun5i_a13_pinctrl_match[] = {
+	{ .compatible = "allwinner,sun5i-a13-pinctrl", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, sun5i_a13_pinctrl_match);
+
+static struct platform_driver sun5i_a13_pinctrl_driver = {
+	.probe	= sun5i_a13_pinctrl_probe,
+	.driver	= {
+		.name		= "sun5i-a13-pinctrl",
+		.owner		= THIS_MODULE,
+		.of_match_table	= sun5i_a13_pinctrl_match,
+	},
+};
+module_platform_driver(sun5i_a13_pinctrl_driver);
+
+MODULE_AUTHOR("Maxime Ripard <maxime.ripard@free-electrons.com");
+MODULE_DESCRIPTION("Allwinner A13 pinctrl driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun6i-a31-r.c b/drivers/pinctrl/sunxi/pinctrl-sun6i-a31-r.c
new file mode 100644
index 00000000000..8fcba48e0a4
--- /dev/null
+++ b/drivers/pinctrl/sunxi/pinctrl-sun6i-a31-r.c
@@ -0,0 +1,141 @@
+/*
+ * Allwinner A31 SoCs special pins pinctrl driver.
+ *
+ * Copyright (C) 2014 Boris Brezillon
+ * Boris Brezillon <boris.brezillon@free-electrons.com>
+ *
+ * Copyright (C) 2014 Maxime Ripard
+ * Maxime Ripard <maxime.ripard@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.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/reset.h>
+
+#include "pinctrl-sunxi.h"
+
+static const struct sunxi_desc_pin sun6i_a31_r_pins[] = {
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_twi"),		/* SCK */
+		  SUNXI_FUNCTION(0x3, "s_p2wi")),	/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_twi"),		/* SDA */
+		  SUNXI_FUNCTION(0x3, "s_p2wi")),	/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_uart")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_uart")),	/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_ir")),		/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "s_jtag")),	/* MS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "s_jtag")),	/* CK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "s_jtag")),	/* DO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "s_jtag")),	/* DI */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(M, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(M, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(M, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "1wire")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(M, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(M, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(M, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(M, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(M, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "rtc")),		/* CLKO */
+};
+
+static const struct sunxi_pinctrl_desc sun6i_a31_r_pinctrl_data = {
+	.pins = sun6i_a31_r_pins,
+	.npins = ARRAY_SIZE(sun6i_a31_r_pins),
+	.pin_base = PL_BASE,
+};
+
+static int sun6i_a31_r_pinctrl_probe(struct platform_device *pdev)
+{
+	struct reset_control *rstc;
+	int ret;
+
+	rstc = devm_reset_control_get(&pdev->dev, NULL);
+	if (IS_ERR(rstc)) {
+		dev_err(&pdev->dev, "Reset controller missing\n");
+		return PTR_ERR(rstc);
+	}
+
+	ret = reset_control_deassert(rstc);
+	if (ret)
+		return ret;
+
+	ret = sunxi_pinctrl_init(pdev,
+				 &sun6i_a31_r_pinctrl_data);
+
+	if (ret)
+		reset_control_assert(rstc);
+
+	return ret;
+}
+
+static struct of_device_id sun6i_a31_r_pinctrl_match[] = {
+	{ .compatible = "allwinner,sun6i-a31-r-pinctrl", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, sun6i_a31_r_pinctrl_match);
+
+static struct platform_driver sun6i_a31_r_pinctrl_driver = {
+	.probe	= sun6i_a31_r_pinctrl_probe,
+	.driver	= {
+		.name		= "sun6i-a31-r-pinctrl",
+		.owner		= THIS_MODULE,
+		.of_match_table	= sun6i_a31_r_pinctrl_match,
+	},
+};
+module_platform_driver(sun6i_a31_r_pinctrl_driver);
+
+MODULE_AUTHOR("Boris Brezillon <boris.brezillon@free-electrons.com");
+MODULE_AUTHOR("Maxime Ripard <maxime.ripard@free-electrons.com");
+MODULE_DESCRIPTION("Allwinner A31 R_PIO pinctrl driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun6i-a31.c b/drivers/pinctrl/sunxi/pinctrl-sun6i-a31.c
new file mode 100644
index 00000000000..8dea5856458
--- /dev/null
+++ b/drivers/pinctrl/sunxi/pinctrl-sun6i-a31.c
@@ -0,0 +1,865 @@
+/*
+ * Allwinner A31 SoCs pinctrl driver.
+ *
+ * Copyright (C) 2014 Maxime Ripard
+ *
+ * Maxime Ripard <maxime.ripard@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.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-sunxi.h"
+
+static const struct sunxi_desc_pin sun6i_a31_pins[] = {
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD0 */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D0 */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* DTR */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD1 */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D1 */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* DSR */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD2 */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D2 */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* DCD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD3 */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D3 */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* RING */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD4 */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D4 */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD5 */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D5 */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD6 */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D6 */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* RTS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXD7 */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D7 */
+		  SUNXI_FUNCTION(0x4, "uart1")),	/* CTS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXCLK */
+		  SUNXI_FUNCTION(0x3, "lcd1")),		/* D8 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXEN */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D9 */
+		  SUNXI_FUNCTION(0x4, "mmc3"),		/* CMD */
+		  SUNXI_FUNCTION(0x5, "mmc2")),		/* CMD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* GTXCLK */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D10 */
+		  SUNXI_FUNCTION(0x4, "mmc3"),		/* CLK */
+		  SUNXI_FUNCTION(0x5, "mmc2")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD0 */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D11 */
+		  SUNXI_FUNCTION(0x4, "mmc3"),		/* D0 */
+		  SUNXI_FUNCTION(0x5, "mmc2")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD1 */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D12 */
+		  SUNXI_FUNCTION(0x4, "mmc3"),		/* D1 */
+		  SUNXI_FUNCTION(0x5, "mmc2")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD2 */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D13 */
+		  SUNXI_FUNCTION(0x4, "mmc3"),		/* D2 */
+		  SUNXI_FUNCTION(0x5, "mmc2")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD3 */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D14 */
+		  SUNXI_FUNCTION(0x4, "mmc3"),		/* D3 */
+		  SUNXI_FUNCTION(0x5, "mmc2")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD4 */
+		  SUNXI_FUNCTION(0x3, "lcd1")),		/* D15 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD5 */
+		  SUNXI_FUNCTION(0x3, "lcd1")),		/* D16 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD6 */
+		  SUNXI_FUNCTION(0x3, "lcd1")),		/* D17 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXD7 */
+		  SUNXI_FUNCTION(0x3, "lcd1")),		/* D18 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXDV */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D19 */
+		  SUNXI_FUNCTION(0x4, "pwm3")),		/* Positive */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXCLK */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D20 */
+		  SUNXI_FUNCTION(0x4, "pwm3")),		/* Negative */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* TXERR */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D21 */
+		  SUNXI_FUNCTION(0x4, "spi3")),		/* CS0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 22),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* RXERR */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D22 */
+		  SUNXI_FUNCTION(0x4, "spi3")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 23),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* COL */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* D23 */
+		  SUNXI_FUNCTION(0x4, "spi3")),		/* MOSI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 24),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* CRS */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* CLK */
+		  SUNXI_FUNCTION(0x4, "spi3")),		/* MISO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 25),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* CLKIN */
+		  SUNXI_FUNCTION(0x3, "lcd1"),		/* DE */
+		  SUNXI_FUNCTION(0x4, "spi3")),		/* CS1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 26),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* MDC */
+		  SUNXI_FUNCTION(0x3, "lcd1")),		/* HSYNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 27),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "gmac"),		/* MDIO */
+		  SUNXI_FUNCTION(0x3, "lcd1")),		/* VSYNC */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0"),		/* MCLK */
+		  SUNXI_FUNCTION(0x3, "uart3"),		/* CTS */
+		  SUNXI_FUNCTION(0x4, "csi")),		/* MCLK1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0")),		/* BCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0")),		/* LRCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0")),		/* DO0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0"),		/* DO1 */
+		  SUNXI_FUNCTION(0x3, "uart3")),	/* RTS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0"),		/* DO2 */
+		  SUNXI_FUNCTION(0x3, "uart3"),		/* TX */
+		  SUNXI_FUNCTION(0x4, "i2c3")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0"),		/* DO3 */
+		  SUNXI_FUNCTION(0x3, "uart3"),		/* RX */
+		  SUNXI_FUNCTION(0x4, "i2c3")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "i2s0")),		/* DI */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* WE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* MOSI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* ALE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* MISO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* CLE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* CE1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* CE0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* RE */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* RB0 */
+		  SUNXI_FUNCTION(0x3, "mmc2"),		/* CMD */
+		  SUNXI_FUNCTION(0x4, "mmc3")),		/* CMD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* RB1 */
+		  SUNXI_FUNCTION(0x3, "mmc2"),		/* CLK */
+		  SUNXI_FUNCTION(0x4, "mmc3")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ0 */
+		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D0 */
+		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ1 */
+		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D1 */
+		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ2 */
+		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D2 */
+		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ3 */
+		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D3 */
+		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ4 */
+		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D4 */
+		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ5 */
+		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D5 */
+		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ6 */
+		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D6 */
+		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ7 */
+		  SUNXI_FUNCTION(0x3, "mmc2"),		/* D7 */
+		  SUNXI_FUNCTION(0x4, "mmc3")),		/* D7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ8 */
+		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ9 */
+		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ10 */
+		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ11 */
+		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ12 */
+		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ13 */
+		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 22),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ14 */
+		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 23),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ15 */
+		  SUNXI_FUNCTION(0x3, "nand1")),	/* DQ7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 24),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQS */
+		  SUNXI_FUNCTION(0x3, "mmc2"),		/* RST */
+		  SUNXI_FUNCTION(0x4, "mmc3")),		/* RST */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 25),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* CE2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 26),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* CE3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 27),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* CS0 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D0 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D1 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D2 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D3 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D4 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D5 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D6 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VPC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D7 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D8 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D9 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D10 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D11 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D12 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D13 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D14 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D15 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D16 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VPC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D17 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D18 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D19 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D20 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D21 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 22),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D22 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 23),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D23 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 24),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 25),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* DE */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 26),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* HSYNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 27),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* VSYNC */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* PCLK */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* MCLK */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* ERR */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* HSYNC */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* SYNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* VSYNC */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* DVLD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D0 */
+		  SUNXI_FUNCTION(0x3, "uart5")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D1 */
+		  SUNXI_FUNCTION(0x3, "uart5")),	/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D2 */
+		  SUNXI_FUNCTION(0x3, "uart5")),	/* RTS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D3 */
+		  SUNXI_FUNCTION(0x3, "uart5")),	/* CTS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D4 */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D5 */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D6 */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D7 */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D8 */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* D4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D9 */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* D5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D10 */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* D6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D11 */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* D7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi")),		/* MIPI CSI MCLK */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D1 */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* MS1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D0 */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* DI1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 2),
+		  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(F, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* CMD */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* DO1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 4),
+		  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(F, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D2 */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* CK1 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1")),		/* CMD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart2")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart2")),	/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart2")),	/* RTS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart2")),	/* CTS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c3"),		/* SCK */
+		  SUNXI_FUNCTION(0x3, "usb")),		/* DP3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c3"),		/* SDA */
+		  SUNXI_FUNCTION(0x3, "usb")),		/* DM3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS1 */
+		  SUNXI_FUNCTION(0x3, "i2s1")),		/* MCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS0 */
+		  SUNXI_FUNCTION(0x3, "i2s1")),		/* BCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "i2s1")),		/* LRCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* MOSI */
+		  SUNXI_FUNCTION(0x3, "i2s1")),		/* DIN */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* MISO */
+		  SUNXI_FUNCTION(0x3, "i2s1")),		/* DOUT */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart4")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart4")),	/* RX */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand1")),	/* WE */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand1")),	/* ALE */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand1")),	/* CLE */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand1")),	/* CE1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand1")),	/* CE0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand1")),	/* RE */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand1")),	/* RB0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand1")),	/* RB1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand1")),	/* DQS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* CS0 */
+		  SUNXI_FUNCTION(0x3, "jtag"),		/* MS0 */
+		  SUNXI_FUNCTION(0x4, "pwm1")),		/* Positive */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "jtag"),		/* CK0 */
+		  SUNXI_FUNCTION(0x4, "pwm1")),		/* Negative */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* MOSI */
+		  SUNXI_FUNCTION(0x3, "jtag"),		/* DO0 */
+		  SUNXI_FUNCTION(0x4, "pwm2")),		/* Positive */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* MISO */
+		  SUNXI_FUNCTION(0x3, "jtag"),		/* DI0 */
+		  SUNXI_FUNCTION(0x4, "pwm2")),		/* Negative */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "pwm0")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart0")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart0")),	/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 22),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 23),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 24),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 25),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 26),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 27),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 28),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 29),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand1")),	/* CE2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 30),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand1")),	/* CE3 */
+};
+
+static const struct sunxi_pinctrl_desc sun6i_a31_pinctrl_data = {
+	.pins = sun6i_a31_pins,
+	.npins = ARRAY_SIZE(sun6i_a31_pins),
+};
+
+static int sun6i_a31_pinctrl_probe(struct platform_device *pdev)
+{
+	return sunxi_pinctrl_init(pdev,
+				  &sun6i_a31_pinctrl_data);
+}
+
+static struct of_device_id sun6i_a31_pinctrl_match[] = {
+	{ .compatible = "allwinner,sun6i-a31-pinctrl", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, sun6i_a31_pinctrl_match);
+
+static struct platform_driver sun6i_a31_pinctrl_driver = {
+	.probe	= sun6i_a31_pinctrl_probe,
+	.driver	= {
+		.name		= "sun6i-a31-pinctrl",
+		.owner		= THIS_MODULE,
+		.of_match_table	= sun6i_a31_pinctrl_match,
+	},
+};
+module_platform_driver(sun6i_a31_pinctrl_driver);
+
+MODULE_AUTHOR("Maxime Ripard <maxime.ripard@free-electrons.com");
+MODULE_DESCRIPTION("Allwinner A31 pinctrl driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun7i-a20.c b/drivers/pinctrl/sunxi/pinctrl-sun7i-a20.c
new file mode 100644
index 00000000000..d8577ce5f1a
--- /dev/null
+++ b/drivers/pinctrl/sunxi/pinctrl-sun7i-a20.c
@@ -0,0 +1,1065 @@
+/*
+ * Allwinner A20 SoCs pinctrl driver.
+ *
+ * Copyright (C) 2014 Maxime Ripard
+ *
+ * Maxime Ripard <maxime.ripard@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.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-sunxi.h"
+
+static const struct sunxi_desc_pin sun7i_a20_pins[] = {
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD3 */
+		  SUNXI_FUNCTION(0x3, "spi1"),		/* CS0 */
+		  SUNXI_FUNCTION(0x4, "uart2"),		/* RTS */
+		  SUNXI_FUNCTION(0x5, "gmac")),		/* GRXD3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD2 */
+		  SUNXI_FUNCTION(0x3, "spi1"),		/* CLK */
+		  SUNXI_FUNCTION(0x4, "uart2"),		/* CTS */
+		  SUNXI_FUNCTION(0x5, "gmac")),		/* GRXD2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD1 */
+		  SUNXI_FUNCTION(0x3, "spi1"),		/* MOSI */
+		  SUNXI_FUNCTION(0x4, "uart2"),		/* TX */
+		  SUNXI_FUNCTION(0x5, "gmac")),		/* GRXD1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXD0 */
+		  SUNXI_FUNCTION(0x3, "spi1"),		/* MISO */
+		  SUNXI_FUNCTION(0x4, "uart2"),		/* RX */
+		  SUNXI_FUNCTION(0x5, "gmac")),		/* GRXD0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD3 */
+		  SUNXI_FUNCTION(0x3, "spi1"),		/* CS1 */
+		  SUNXI_FUNCTION(0x5, "gmac")),		/* GTXD3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD2 */
+		  SUNXI_FUNCTION(0x3, "spi3"),		/* CS0 */
+		  SUNXI_FUNCTION(0x5, "gmac")),		/* GTXD2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD1 */
+		  SUNXI_FUNCTION(0x3, "spi3"),		/* CLK */
+		  SUNXI_FUNCTION(0x5, "gmac")),		/* GTXD1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ETXD0 */
+		  SUNXI_FUNCTION(0x3, "spi3"),		/* MOSI */
+		  SUNXI_FUNCTION(0x5, "gmac")),		/* GTXD0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXCK */
+		  SUNXI_FUNCTION(0x3, "spi3"),		/* MISO */
+		  SUNXI_FUNCTION(0x5, "gmac")),		/* GRXCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXERR */
+		  SUNXI_FUNCTION(0x3, "spi3"),		/* CS1 */
+		  SUNXI_FUNCTION(0x5, "gmac"),		/* GNULL / ERXERR */
+		  SUNXI_FUNCTION(0x6, "i2s1")),		/* MCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* ERXDV */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* TX */
+		  SUNXI_FUNCTION(0x5, "gmac")),		/* GRXCTL / ERXDV */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac"),		/* EMDC */
+		  SUNXI_FUNCTION(0x4, "uart1"),		/* RX */
+		  SUNXI_FUNCTION(0x5, "gmac")),		/* EMDC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 12),
+		  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_FUNCTION(0x5, "gmac")),		/* EMDIO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 13),
+		  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_FUNCTION(0x5, "gmac")),		/* GTXCTL / ETXEN */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 14),
+		  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_FUNCTION(0x5, "gmac"),		/* GNULL / ETXCK */
+		  SUNXI_FUNCTION(0x6, "i2s1")),		/* BCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 15),
+		  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_FUNCTION(0x5, "gmac"),		/* GTXCK / ECRS */
+		  SUNXI_FUNCTION(0x6, "i2s1")),		/* LRCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 16),
+		  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_FUNCTION(0x5, "gmac"),		/* GCLKIN / ECOL */
+		  SUNXI_FUNCTION(0x6, "i2s1")),		/* DO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 17),
+		  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 */
+		  SUNXI_FUNCTION(0x5, "gmac"),		/* GNULL / ETXERR */
+		  SUNXI_FUNCTION(0x6, "i2s1")),		/* LRCK */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "pwm")),		/* PWM0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ir0"),		/* TX */
+		  SUNXI_FUNCTION(0x4, "spdif")),	/* MCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ir0")),		/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0"),		/* MCLK */
+		  SUNXI_FUNCTION(0x3, "ac97")),		/* MCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0"),		/* BCLK */
+		  SUNXI_FUNCTION(0x3, "ac97")),		/* BCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0"),		/* LRCK */
+		  SUNXI_FUNCTION(0x3, "ac97")),		/* SYNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0"),		/* DO0 */
+		  SUNXI_FUNCTION(0x3, "ac97")),		/* DO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0")),		/* DO1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0")),		/* DO2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0")),		/* DO3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0"),		/* DI */
+		  SUNXI_FUNCTION(0x3, "ac97"),		/* DI */
+		  SUNXI_FUNCTION(0x4, "spdif")),	/* DI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* CS1 */
+		  SUNXI_FUNCTION(0x4, "spdif")),	/* DO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* CS0 */
+		  SUNXI_FUNCTION(0x3, "jtag")),		/* MS0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "jtag")),		/* CK0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* MOSI */
+		  SUNXI_FUNCTION(0x3, "jtag")),		/* DO0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi2"),		/* MISO */
+		  SUNXI_FUNCTION(0x3, "jtag")),		/* DI0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c2")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 22),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart0"),		/* TX */
+		  SUNXI_FUNCTION(0x3, "ir1")),		/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 23),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart0"),		/* RX */
+		  SUNXI_FUNCTION(0x3, "ir1")),		/* RX */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NWE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* MOSI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NALE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* MISO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCLE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NRE# */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NRB0 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CMD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NRB1 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ0 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ1 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ2 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NDQ3 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQ4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQ5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQ6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQ7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NWP */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NCE3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE4 */
+		  SUNXI_FUNCTION(0x3, "spi2"),		/* CS0 */
+		  SUNXI_FUNCTION_IRQ(0x6, 12)),		/* EINT12 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE5 */
+		  SUNXI_FUNCTION(0x3, "spi2"),		/* CLK */
+		  SUNXI_FUNCTION_IRQ(0x6, 13)),		/* EINT13 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE6 */
+		  SUNXI_FUNCTION(0x3, "spi2"),		/* MOSI */
+		  SUNXI_FUNCTION_IRQ(0x6, 14)),		/* EINT14 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 22),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* NCE7 */
+		  SUNXI_FUNCTION(0x3, "spi2"),		/* MISO */
+		  SUNXI_FUNCTION_IRQ(0x6, 15)),		/* EINT15 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 23),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* CS0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 24),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* NDQS */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D0 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D1 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D2 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D3 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D4 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D5 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D6 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VPC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D7 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D8 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D9 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VM3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D10 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D11 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D12 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D13 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D14 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D15 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D16 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VPC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D17 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D18 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VP3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D19 */
+		  SUNXI_FUNCTION(0x3, "lvds1")),	/* VN3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D20 */
+		  SUNXI_FUNCTION(0x3, "csi1")),		/* MCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D21 */
+		  SUNXI_FUNCTION(0x3, "sim")),		/* VPPEN */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 22),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D22 */
+		  SUNXI_FUNCTION(0x3, "sim")),		/* VPPPP */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 23),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D23 */
+		  SUNXI_FUNCTION(0x3, "sim")),		/* DET */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 24),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "sim")),		/* VCCEN */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 25),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* DE */
+		  SUNXI_FUNCTION(0x3, "sim")),		/* RST */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 26),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* HSYNC */
+		  SUNXI_FUNCTION(0x3, "sim")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 27),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* VSYNC */
+		  SUNXI_FUNCTION(0x3, "sim")),		/* SDA */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* PCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* ERR */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* CK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* SYNC */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* HSYNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* DVLD */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* VSYNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D0 */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(E, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D2 */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D3 */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D4 */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* D4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D5 */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* D5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D6 */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* D6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ts0"),		/* D7 */
+		  SUNXI_FUNCTION(0x3, "csi0")),		/* D7 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D1 */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* MSI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D0 */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* DI1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 2),
+		  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(F, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* CMD */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* DO1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 4),
+		  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(F, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D2 */
+		  SUNXI_FUNCTION(0x4, "jtag")),		/* CK1 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(G, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(G, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(G, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(G, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(G, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(G, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(G, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(G, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(G, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(G, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(G, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(H, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D0 */
+		  SUNXI_FUNCTION(0x4, "uart3"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 0),		/* EINT0 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D1 */
+		  SUNXI_FUNCTION(0x4, "uart3"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 1),		/* EINT1 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D2 */
+		  SUNXI_FUNCTION(0x4, "uart3"),		/* RTS */
+		  SUNXI_FUNCTION_IRQ(0x6, 2),		/* EINT2 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D3 */
+		  SUNXI_FUNCTION(0x4, "uart3"),		/* CTS */
+		  SUNXI_FUNCTION_IRQ(0x6, 3),		/* EINT3 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D4 */
+		  SUNXI_FUNCTION(0x4, "uart4"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 4),		/* EINT4 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D5 */
+		  SUNXI_FUNCTION(0x4, "uart4"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 5),		/* EINT5 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D6 */
+		  SUNXI_FUNCTION(0x4, "uart5"),		/* TX */
+		  SUNXI_FUNCTION(0x5, "ms"),		/* BS */
+		  SUNXI_FUNCTION_IRQ(0x6, 6),		/* EINT6 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D7 */
+		  SUNXI_FUNCTION(0x4, "uart5"),		/* RX */
+		  SUNXI_FUNCTION(0x5, "ms"),		/* CLK */
+		  SUNXI_FUNCTION_IRQ(0x6, 7),		/* EINT7 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D8 */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXD3 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN0 */
+		  SUNXI_FUNCTION(0x5, "ms"),		/* D0 */
+		  SUNXI_FUNCTION_IRQ(0x6, 8),		/* EINT8 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D8 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D9 */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXD2 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN1 */
+		  SUNXI_FUNCTION(0x5, "ms"),		/* D1 */
+		  SUNXI_FUNCTION_IRQ(0x6, 9),		/* EINT9 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D9 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D10 */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXD1 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN2 */
+		  SUNXI_FUNCTION(0x5, "ms"),		/* D2 */
+		  SUNXI_FUNCTION_IRQ(0x6, 10),		/* EINT10 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D10 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D11 */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXD0 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN3 */
+		  SUNXI_FUNCTION(0x5, "ms"),		/* D3 */
+		  SUNXI_FUNCTION_IRQ(0x6, 11),		/* EINT11 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D11 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D12 */
+		  SUNXI_FUNCTION(0x4, "ps2"),		/* SCK1 */
+		  SUNXI_FUNCTION_IRQ(0x6, 12),		/* EINT12 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D12 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D13 */
+		  SUNXI_FUNCTION(0x4, "ps2"),		/* SDA1 */
+		  SUNXI_FUNCTION(0x5, "sim"),		/* RST */
+		  SUNXI_FUNCTION_IRQ(0x6, 13),		/* EINT13 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D13 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D14 */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXD3 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN4 */
+		  SUNXI_FUNCTION(0x5, "sim"),		/* VPPEN */
+		  SUNXI_FUNCTION_IRQ(0x6, 14),		/* EINT14 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D14 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D15 */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXD3 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN5 */
+		  SUNXI_FUNCTION(0x5, "sim"),		/* VPPPP */
+		  SUNXI_FUNCTION_IRQ(0x6, 15),		/* EINT15 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D15 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D16 */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXD2 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN6 */
+		  SUNXI_FUNCTION_IRQ(0x6, 16),		/* EINT16 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D16 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D17 */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXD1 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* IN7 */
+		  SUNXI_FUNCTION(0x5, "sim"),		/* VCCEN */
+		  SUNXI_FUNCTION_IRQ(0x6, 17),		/* EINT17 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D17 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D18 */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXD0 */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT0 */
+		  SUNXI_FUNCTION(0x5, "sim"),		/* SCK */
+		  SUNXI_FUNCTION_IRQ(0x6, 18),		/* EINT18 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D18 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D19 */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXERR */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT1 */
+		  SUNXI_FUNCTION(0x5, "sim"),		/* SDA */
+		  SUNXI_FUNCTION_IRQ(0x6, 19),		/* EINT19 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D19 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D20 */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ERXDV */
+		  SUNXI_FUNCTION(0x4, "can"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x6, 20),		/* EINT20 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D20 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D21 */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* EMDC */
+		  SUNXI_FUNCTION(0x4, "can"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x6, 21),		/* EINT21 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D21 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 22),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D22 */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* EMDIO */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT2 */
+		  SUNXI_FUNCTION(0x5, "mmc1"),		/* CMD */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D22 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 23),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* D23 */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXEN */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT3 */
+		  SUNXI_FUNCTION(0x5, "mmc1"),		/* CLK */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* D23 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 24),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXCK */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT4 */
+		  SUNXI_FUNCTION(0x5, "mmc1"),		/* D0 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* PCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 25),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* DE */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ECRS */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT5 */
+		  SUNXI_FUNCTION(0x5, "mmc1"),		/* D1 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* FIELD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 26),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* HSYNC */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ECOL */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT6 */
+		  SUNXI_FUNCTION(0x5, "mmc1"),		/* D2 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* HSYNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 27),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd1"),		/* VSYNC */
+		  SUNXI_FUNCTION(0x3, "emac"),		/* ETXERR */
+		  SUNXI_FUNCTION(0x4, "keypad"),	/* OUT7 */
+		  SUNXI_FUNCTION(0x5, "mmc1"),		/* D3 */
+		  SUNXI_FUNCTION(0x7, "csi1")),		/* VSYNC */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "i2c3")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "i2c3")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "i2c4")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "pwm"),		/* PWM1 */
+		  SUNXI_FUNCTION(0x3, "i2c4")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc3")),		/* CMD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc3")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc3")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc3")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc3")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc3")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi0"),		/* CS0 */
+		  SUNXI_FUNCTION(0x3, "uart5"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x5, 22)),		/* EINT22 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi0"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "uart5"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x5, 23)),		/* EINT23 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi0"),		/* MOSI */
+		  SUNXI_FUNCTION(0x3, "uart6"),		/* TX */
+		  SUNXI_FUNCTION(0x4, "clk_out_a"),	/* CLK_OUT_A */
+		  SUNXI_FUNCTION_IRQ(0x5, 24)),		/* EINT24 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi0"),		/* MISO */
+		  SUNXI_FUNCTION(0x3, "uart6"),		/* RX */
+		  SUNXI_FUNCTION(0x4, "clk_out_b"),	/* CLK_OUT_B */
+		  SUNXI_FUNCTION_IRQ(0x5, 25)),		/* EINT25 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi0"),		/* CS1 */
+		  SUNXI_FUNCTION(0x3, "ps2"),		/* SCK1 */
+		  SUNXI_FUNCTION(0x4, "timer4"),	/* TCLKIN0 */
+		  SUNXI_FUNCTION_IRQ(0x5, 26)),		/* EINT26 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS1 */
+		  SUNXI_FUNCTION(0x3, "ps2"),		/* SDA1 */
+		  SUNXI_FUNCTION(0x4, "timer5"),	/* TCLKIN1 */
+		  SUNXI_FUNCTION_IRQ(0x5, 27)),		/* EINT27 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS0 */
+		  SUNXI_FUNCTION(0x3, "uart2"),		/* RTS */
+		  SUNXI_FUNCTION_IRQ(0x5, 28)),		/* EINT28 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "uart2"),		/* CTS */
+		  SUNXI_FUNCTION_IRQ(0x5, 29)),		/* EINT29 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* MOSI */
+		  SUNXI_FUNCTION(0x3, "uart2"),		/* TX */
+		  SUNXI_FUNCTION_IRQ(0x5, 30)),		/* EINT30 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* MISO */
+		  SUNXI_FUNCTION(0x3, "uart2"),		/* RX */
+		  SUNXI_FUNCTION_IRQ(0x5, 31)),		/* EINT31 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(I, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  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(I, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "ps2"),		/* SDA0 */
+		  SUNXI_FUNCTION(0x3, "uart7"),		/* RX */
+		  SUNXI_FUNCTION(0x4, "hdmi")),		/* HSDA */
+};
+
+static const struct sunxi_pinctrl_desc sun7i_a20_pinctrl_data = {
+	.pins = sun7i_a20_pins,
+	.npins = ARRAY_SIZE(sun7i_a20_pins),
+};
+
+static int sun7i_a20_pinctrl_probe(struct platform_device *pdev)
+{
+	return sunxi_pinctrl_init(pdev,
+				  &sun7i_a20_pinctrl_data);
+}
+
+static struct of_device_id sun7i_a20_pinctrl_match[] = {
+	{ .compatible = "allwinner,sun7i-a20-pinctrl", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, sun7i_a20_pinctrl_match);
+
+static struct platform_driver sun7i_a20_pinctrl_driver = {
+	.probe	= sun7i_a20_pinctrl_probe,
+	.driver	= {
+		.name		= "sun7i-a20-pinctrl",
+		.owner		= THIS_MODULE,
+		.of_match_table	= sun7i_a20_pinctrl_match,
+	},
+};
+module_platform_driver(sun7i_a20_pinctrl_driver);
+
+MODULE_AUTHOR("Maxime Ripard <maxime.ripard@free-electrons.com");
+MODULE_DESCRIPTION("Allwinner A20 pinctrl driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/pinctrl/pinctrl-sunxi.c b/drivers/pinctrl/sunxi/pinctrl-sunxi.c
index f9fabe9bf47..f1ca75e6d7b 100644
--- a/drivers/pinctrl/pinctrl-sunxi.c
+++ b/drivers/pinctrl/sunxi/pinctrl-sunxi.c
@@ -28,9 +28,8 @@
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 
-#include "core.h"
+#include "../core.h"
 #include "pinctrl-sunxi.h"
-#include "pinctrl-sunxi-pins.h"
 
 static struct sunxi_pinctrl_group *
 sunxi_pinctrl_find_group_by_name(struct sunxi_pinctrl *pctl, const char *group)
@@ -281,6 +280,7 @@ static int sunxi_pconf_group_set(struct pinctrl_dev *pctldev,
 	struct sunxi_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
 	struct sunxi_pinctrl_group *g = &pctl->groups[group];
 	unsigned long flags;
+	unsigned pin = g->pin - pctl->desc->pin_base;
 	u32 val, mask;
 	u16 strength;
 	u8 dlevel;
@@ -304,23 +304,23 @@ static int sunxi_pconf_group_set(struct pinctrl_dev *pctldev,
 			 *   3: 40mA
 			 */
 			dlevel = strength / 10 - 1;
-			val = readl(pctl->membase + sunxi_dlevel_reg(g->pin));
-			mask = DLEVEL_PINS_MASK << sunxi_dlevel_offset(g->pin);
+			val = readl(pctl->membase + sunxi_dlevel_reg(pin));
+			mask = DLEVEL_PINS_MASK << sunxi_dlevel_offset(pin);
 			writel((val & ~mask)
-				| dlevel << sunxi_dlevel_offset(g->pin),
-				pctl->membase + sunxi_dlevel_reg(g->pin));
+				| dlevel << sunxi_dlevel_offset(pin),
+				pctl->membase + sunxi_dlevel_reg(pin));
 			break;
 		case PIN_CONFIG_BIAS_PULL_UP:
-			val = readl(pctl->membase + sunxi_pull_reg(g->pin));
-			mask = PULL_PINS_MASK << sunxi_pull_offset(g->pin);
-			writel((val & ~mask) | 1 << sunxi_pull_offset(g->pin),
-				pctl->membase + sunxi_pull_reg(g->pin));
+			val = readl(pctl->membase + sunxi_pull_reg(pin));
+			mask = PULL_PINS_MASK << sunxi_pull_offset(pin);
+			writel((val & ~mask) | 1 << sunxi_pull_offset(pin),
+				pctl->membase + sunxi_pull_reg(pin));
 			break;
 		case PIN_CONFIG_BIAS_PULL_DOWN:
-			val = readl(pctl->membase + sunxi_pull_reg(g->pin));
-			mask = PULL_PINS_MASK << sunxi_pull_offset(g->pin);
-			writel((val & ~mask) | 2 << sunxi_pull_offset(g->pin),
-				pctl->membase + sunxi_pull_reg(g->pin));
+			val = readl(pctl->membase + sunxi_pull_reg(pin));
+			mask = PULL_PINS_MASK << sunxi_pull_offset(pin);
+			writel((val & ~mask) | 2 << sunxi_pull_offset(pin),
+				pctl->membase + sunxi_pull_reg(pin));
 			break;
 		default:
 			break;
@@ -377,6 +377,7 @@ static void sunxi_pmx_set(struct pinctrl_dev *pctldev,
 
 	spin_lock_irqsave(&pctl->lock, flags);
 
+	pin -= pctl->desc->pin_base;
 	val = readl(pctl->membase + sunxi_mux_reg(pin));
 	mask = MUX_PINS_MASK << sunxi_mux_offset(pin);
 	writel((val & ~mask) | config << sunxi_mux_offset(pin),
@@ -437,12 +438,6 @@ static const struct pinmux_ops sunxi_pmx_ops = {
 	.gpio_set_direction	= sunxi_pmx_gpio_set_direction,
 };
 
-static struct pinctrl_desc sunxi_pctrl_desc = {
-	.confops	= &sunxi_pconf_ops,
-	.pctlops	= &sunxi_pctrl_ops,
-	.pmxops		= &sunxi_pmx_ops,
-};
-
 static int sunxi_pinctrl_gpio_request(struct gpio_chip *chip, unsigned offset)
 {
 	return pinctrl_request_gpio(chip->base + offset);
@@ -530,27 +525,12 @@ static int sunxi_pinctrl_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 	if (!desc)
 		return -EINVAL;
 
-	pctl->irq_array[desc->irqnum] = offset;
-
 	dev_dbg(chip->dev, "%s: request IRQ for GPIO %d, return %d\n",
 		chip->label, offset + chip->base, desc->irqnum);
 
 	return irq_find_mapping(pctl->domain, desc->irqnum);
 }
 
-static struct gpio_chip sunxi_pinctrl_gpio_chip = {
-	.owner			= THIS_MODULE,
-	.request		= sunxi_pinctrl_gpio_request,
-	.free			= sunxi_pinctrl_gpio_free,
-	.direction_input	= sunxi_pinctrl_gpio_direction_input,
-	.direction_output	= sunxi_pinctrl_gpio_direction_output,
-	.get			= sunxi_pinctrl_gpio_get,
-	.set			= sunxi_pinctrl_gpio_set,
-	.of_xlate		= sunxi_pinctrl_gpio_of_xlate,
-	.to_irq			= sunxi_pinctrl_gpio_to_irq,
-	.of_gpio_n_cells	= 3,
-	.can_sleep		= false,
-};
 
 static int sunxi_pinctrl_irq_set_type(struct irq_data *d,
 				      unsigned int type)
@@ -685,16 +665,6 @@ static void sunxi_pinctrl_irq_handler(unsigned irq, struct irq_desc *desc)
 	}
 }
 
-static struct of_device_id sunxi_pinctrl_match[] = {
-	{ .compatible = "allwinner,sun4i-a10-pinctrl", .data = (void *)&sun4i_a10_pinctrl_data },
-	{ .compatible = "allwinner,sun5i-a10s-pinctrl", .data = (void *)&sun5i_a10s_pinctrl_data },
-	{ .compatible = "allwinner,sun5i-a13-pinctrl", .data = (void *)&sun5i_a13_pinctrl_data },
-	{ .compatible = "allwinner,sun6i-a31-pinctrl", .data = (void *)&sun6i_a31_pinctrl_data },
-	{ .compatible = "allwinner,sun7i-a20-pinctrl", .data = (void *)&sun7i_a20_pinctrl_data },
-	{}
-};
-MODULE_DEVICE_TABLE(of, sunxi_pinctrl_match);
-
 static int sunxi_pinctrl_add_function(struct sunxi_pinctrl *pctl,
 					const char *name)
 {
@@ -755,6 +725,9 @@ static int sunxi_pinctrl_build_state(struct platform_device *pdev)
 		struct sunxi_desc_function *func = pin->functions;
 
 		while (func->name) {
+			/* Create interrupt mapping while we're at it */
+			if (!strcmp(func->name, "irq"))
+				pctl->irq_array[func->irqnum] = pin->pin.number;
 			sunxi_pinctrl_add_function(pctl, func->name);
 			func++;
 		}
@@ -798,12 +771,14 @@ static int sunxi_pinctrl_build_state(struct platform_device *pdev)
 	return 0;
 }
 
-static int sunxi_pinctrl_probe(struct platform_device *pdev)
+int sunxi_pinctrl_init(struct platform_device *pdev,
+		       const struct sunxi_pinctrl_desc *desc)
 {
 	struct device_node *node = pdev->dev.of_node;
-	const struct of_device_id *device;
+	struct pinctrl_desc *pctrl_desc;
 	struct pinctrl_pin_desc *pins;
 	struct sunxi_pinctrl *pctl;
+	struct resource *res;
 	int i, ret, last_pin;
 	struct clk *clk;
 
@@ -814,15 +789,13 @@ static int sunxi_pinctrl_probe(struct platform_device *pdev)
 
 	spin_lock_init(&pctl->lock);
 
-	pctl->membase = of_iomap(node, 0);
-	if (!pctl->membase)
-		return -ENOMEM;
-
-	device = of_match_device(sunxi_pinctrl_match, &pdev->dev);
-	if (!device)
-		return -ENODEV;
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	pctl->membase = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(pctl->membase))
+		return PTR_ERR(pctl->membase);
 
-	pctl->desc = (struct sunxi_pinctrl_desc *)device->data;
+	pctl->dev = &pdev->dev;
+	pctl->desc = desc;
 
 	ret = sunxi_pinctrl_build_state(pdev);
 	if (ret) {
@@ -839,12 +812,21 @@ static int sunxi_pinctrl_probe(struct platform_device *pdev)
 	for (i = 0; i < pctl->desc->npins; i++)
 		pins[i] = pctl->desc->pins[i].pin;
 
-	sunxi_pctrl_desc.name = dev_name(&pdev->dev);
-	sunxi_pctrl_desc.owner = THIS_MODULE;
-	sunxi_pctrl_desc.pins = pins;
-	sunxi_pctrl_desc.npins = pctl->desc->npins;
-	pctl->dev = &pdev->dev;
-	pctl->pctl_dev = pinctrl_register(&sunxi_pctrl_desc,
+	pctrl_desc = devm_kzalloc(&pdev->dev,
+				  sizeof(*pctrl_desc),
+				  GFP_KERNEL);
+	if (!pctrl_desc)
+		return -ENOMEM;
+
+	pctrl_desc->name = dev_name(&pdev->dev);
+	pctrl_desc->owner = THIS_MODULE;
+	pctrl_desc->pins = pins;
+	pctrl_desc->npins = pctl->desc->npins;
+	pctrl_desc->confops = &sunxi_pconf_ops;
+	pctrl_desc->pctlops = &sunxi_pctrl_ops;
+	pctrl_desc->pmxops =  &sunxi_pmx_ops;
+
+	pctl->pctl_dev = pinctrl_register(pctrl_desc,
 					  &pdev->dev, pctl);
 	if (!pctl->pctl_dev) {
 		dev_err(&pdev->dev, "couldn't register pinctrl driver\n");
@@ -858,11 +840,22 @@ static int sunxi_pinctrl_probe(struct platform_device *pdev)
 	}
 
 	last_pin = pctl->desc->pins[pctl->desc->npins - 1].pin.number;
-	pctl->chip = &sunxi_pinctrl_gpio_chip;
-	pctl->chip->ngpio = round_up(last_pin, PINS_PER_BANK);
+	pctl->chip->owner = THIS_MODULE;
+	pctl->chip->request = sunxi_pinctrl_gpio_request,
+	pctl->chip->free = sunxi_pinctrl_gpio_free,
+	pctl->chip->direction_input = sunxi_pinctrl_gpio_direction_input,
+	pctl->chip->direction_output = sunxi_pinctrl_gpio_direction_output,
+	pctl->chip->get = sunxi_pinctrl_gpio_get,
+	pctl->chip->set = sunxi_pinctrl_gpio_set,
+	pctl->chip->of_xlate = sunxi_pinctrl_gpio_of_xlate,
+	pctl->chip->to_irq = sunxi_pinctrl_gpio_to_irq,
+	pctl->chip->of_gpio_n_cells = 3,
+	pctl->chip->can_sleep = false,
+	pctl->chip->ngpio = round_up(last_pin, PINS_PER_BANK) -
+			    pctl->desc->pin_base;
 	pctl->chip->label = dev_name(&pdev->dev);
 	pctl->chip->dev = &pdev->dev;
-	pctl->chip->base = 0;
+	pctl->chip->base = pctl->desc->pin_base;
 
 	ret = gpiochip_add(pctl->chip);
 	if (ret)
@@ -884,12 +877,14 @@ static int sunxi_pinctrl_probe(struct platform_device *pdev)
 		goto gpiochip_error;
 	}
 
-	clk_prepare_enable(clk);
+	ret = clk_prepare_enable(clk);
+	if (ret)
+		goto gpiochip_error;
 
 	pctl->irq = irq_of_parse_and_map(node, 0);
 	if (!pctl->irq) {
 		ret = -EINVAL;
-		goto gpiochip_error;
+		goto clk_error;
 	}
 
 	pctl->domain = irq_domain_add_linear(node, SUNXI_IRQ_NUMBER,
@@ -897,7 +892,7 @@ static int sunxi_pinctrl_probe(struct platform_device *pdev)
 	if (!pctl->domain) {
 		dev_err(&pdev->dev, "Couldn't register IRQ domain\n");
 		ret = -ENOMEM;
-		goto gpiochip_error;
+		goto clk_error;
 	}
 
 	for (i = 0; i < SUNXI_IRQ_NUMBER; i++) {
@@ -915,6 +910,8 @@ static int sunxi_pinctrl_probe(struct platform_device *pdev)
 
 	return 0;
 
+clk_error:
+	clk_disable_unprepare(clk);
 gpiochip_error:
 	if (gpiochip_remove(pctl->chip))
 		dev_err(&pdev->dev, "failed to remove gpio chip\n");
@@ -922,17 +919,3 @@ pinctrl_error:
 	pinctrl_unregister(pctl->pctl_dev);
 	return ret;
 }
-
-static struct platform_driver sunxi_pinctrl_driver = {
-	.probe = sunxi_pinctrl_probe,
-	.driver = {
-		.name = "sunxi-pinctrl",
-		.owner = THIS_MODULE,
-		.of_match_table = sunxi_pinctrl_match,
-	},
-};
-module_platform_driver(sunxi_pinctrl_driver);
-
-MODULE_AUTHOR("Maxime Ripard <maxime.ripard@free-electrons.com");
-MODULE_DESCRIPTION("Allwinner A1X pinctrl driver");
-MODULE_LICENSE("GPL");
diff --git a/drivers/pinctrl/sunxi/pinctrl-sunxi.h b/drivers/pinctrl/sunxi/pinctrl-sunxi.h
new file mode 100644
index 00000000000..8169ba59887
--- /dev/null
+++ b/drivers/pinctrl/sunxi/pinctrl-sunxi.h
@@ -0,0 +1,258 @@
+/*
+ * Allwinner A1X SoCs pinctrl driver.
+ *
+ * Copyright (C) 2012 Maxime Ripard
+ *
+ * Maxime Ripard <maxime.ripard@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 __PINCTRL_SUNXI_H
+#define __PINCTRL_SUNXI_H
+
+#include <linux/kernel.h>
+#include <linux/spinlock.h>
+
+#define PA_BASE	0
+#define PB_BASE	32
+#define PC_BASE	64
+#define PD_BASE	96
+#define PE_BASE	128
+#define PF_BASE	160
+#define PG_BASE	192
+#define PH_BASE	224
+#define PI_BASE	256
+#define PL_BASE	352
+#define PM_BASE	384
+
+#define SUNXI_PINCTRL_PIN(bank, pin)		\
+	PINCTRL_PIN(P ## bank ## _BASE + (pin), "P" #bank #pin)
+
+#define SUNXI_PIN_NAME_MAX_LEN	5
+
+#define BANK_MEM_SIZE		0x24
+#define MUX_REGS_OFFSET		0x0
+#define DATA_REGS_OFFSET	0x10
+#define DLEVEL_REGS_OFFSET	0x14
+#define PULL_REGS_OFFSET	0x1c
+
+#define PINS_PER_BANK		32
+#define MUX_PINS_PER_REG	8
+#define MUX_PINS_BITS		4
+#define MUX_PINS_MASK		0x0f
+#define DATA_PINS_PER_REG	32
+#define DATA_PINS_BITS		1
+#define DATA_PINS_MASK		0x01
+#define DLEVEL_PINS_PER_REG	16
+#define DLEVEL_PINS_BITS	2
+#define DLEVEL_PINS_MASK	0x03
+#define PULL_PINS_PER_REG	16
+#define PULL_PINS_BITS		2
+#define PULL_PINS_MASK		0x03
+
+#define SUNXI_IRQ_NUMBER	32
+
+#define IRQ_CFG_REG		0x200
+#define IRQ_CFG_IRQ_PER_REG		8
+#define IRQ_CFG_IRQ_BITS		4
+#define IRQ_CFG_IRQ_MASK		((1 << IRQ_CFG_IRQ_BITS) - 1)
+#define IRQ_CTRL_REG		0x210
+#define IRQ_CTRL_IRQ_PER_REG		32
+#define IRQ_CTRL_IRQ_BITS		1
+#define IRQ_CTRL_IRQ_MASK		((1 << IRQ_CTRL_IRQ_BITS) - 1)
+#define IRQ_STATUS_REG		0x214
+#define IRQ_STATUS_IRQ_PER_REG		32
+#define IRQ_STATUS_IRQ_BITS		1
+#define IRQ_STATUS_IRQ_MASK		((1 << IRQ_STATUS_IRQ_BITS) - 1)
+
+#define IRQ_EDGE_RISING		0x00
+#define IRQ_EDGE_FALLING	0x01
+#define IRQ_LEVEL_HIGH		0x02
+#define IRQ_LEVEL_LOW		0x03
+#define IRQ_EDGE_BOTH		0x04
+
+struct sunxi_desc_function {
+	const char	*name;
+	u8		muxval;
+	u8		irqnum;
+};
+
+struct sunxi_desc_pin {
+	struct pinctrl_pin_desc		pin;
+	struct sunxi_desc_function	*functions;
+};
+
+struct sunxi_pinctrl_desc {
+	const struct sunxi_desc_pin	*pins;
+	int				npins;
+	unsigned			pin_base;
+};
+
+struct sunxi_pinctrl_function {
+	const char	*name;
+	const char	**groups;
+	unsigned	ngroups;
+};
+
+struct sunxi_pinctrl_group {
+	const char	*name;
+	unsigned long	config;
+	unsigned	pin;
+};
+
+struct sunxi_pinctrl {
+	void __iomem			*membase;
+	struct gpio_chip		*chip;
+	const struct sunxi_pinctrl_desc	*desc;
+	struct device			*dev;
+	struct irq_domain		*domain;
+	struct sunxi_pinctrl_function	*functions;
+	unsigned			nfunctions;
+	struct sunxi_pinctrl_group	*groups;
+	unsigned			ngroups;
+	int				irq;
+	int				irq_array[SUNXI_IRQ_NUMBER];
+	spinlock_t			lock;
+	struct pinctrl_dev		*pctl_dev;
+};
+
+#define SUNXI_PIN(_pin, ...)					\
+	{							\
+		.pin = _pin,					\
+		.functions = (struct sunxi_desc_function[]){	\
+			__VA_ARGS__, { } },			\
+	}
+
+#define SUNXI_FUNCTION(_val, _name)				\
+	{							\
+		.name = _name,					\
+		.muxval = _val,					\
+	}
+
+#define SUNXI_FUNCTION_IRQ(_val, _irq)				\
+	{							\
+		.name = "irq",					\
+		.muxval = _val,					\
+		.irqnum = _irq,					\
+	}
+
+/*
+ * The sunXi PIO registers are organized as is:
+ * 0x00 - 0x0c	Muxing values.
+ *		8 pins per register, each pin having a 4bits value
+ * 0x10		Pin values
+ *		32 bits per register, each pin corresponding to one bit
+ * 0x14 - 0x18	Drive level
+ *		16 pins per register, each pin having a 2bits value
+ * 0x1c - 0x20	Pull-Up values
+ *		16 pins per register, each pin having a 2bits value
+ *
+ * This is for the first bank. Each bank will have the same layout,
+ * with an offset being a multiple of 0x24.
+ *
+ * The following functions calculate from the pin number the register
+ * and the bit offset that we should access.
+ */
+static inline u32 sunxi_mux_reg(u16 pin)
+{
+	u8 bank = pin / PINS_PER_BANK;
+	u32 offset = bank * BANK_MEM_SIZE;
+	offset += MUX_REGS_OFFSET;
+	offset += pin % PINS_PER_BANK / MUX_PINS_PER_REG * 0x04;
+	return round_down(offset, 4);
+}
+
+static inline u32 sunxi_mux_offset(u16 pin)
+{
+	u32 pin_num = pin % MUX_PINS_PER_REG;
+	return pin_num * MUX_PINS_BITS;
+}
+
+static inline u32 sunxi_data_reg(u16 pin)
+{
+	u8 bank = pin / PINS_PER_BANK;
+	u32 offset = bank * BANK_MEM_SIZE;
+	offset += DATA_REGS_OFFSET;
+	offset += pin % PINS_PER_BANK / DATA_PINS_PER_REG * 0x04;
+	return round_down(offset, 4);
+}
+
+static inline u32 sunxi_data_offset(u16 pin)
+{
+	u32 pin_num = pin % DATA_PINS_PER_REG;
+	return pin_num * DATA_PINS_BITS;
+}
+
+static inline u32 sunxi_dlevel_reg(u16 pin)
+{
+	u8 bank = pin / PINS_PER_BANK;
+	u32 offset = bank * BANK_MEM_SIZE;
+	offset += DLEVEL_REGS_OFFSET;
+	offset += pin % PINS_PER_BANK / DLEVEL_PINS_PER_REG * 0x04;
+	return round_down(offset, 4);
+}
+
+static inline u32 sunxi_dlevel_offset(u16 pin)
+{
+	u32 pin_num = pin % DLEVEL_PINS_PER_REG;
+	return pin_num * DLEVEL_PINS_BITS;
+}
+
+static inline u32 sunxi_pull_reg(u16 pin)
+{
+	u8 bank = pin / PINS_PER_BANK;
+	u32 offset = bank * BANK_MEM_SIZE;
+	offset += PULL_REGS_OFFSET;
+	offset += pin % PINS_PER_BANK / PULL_PINS_PER_REG * 0x04;
+	return round_down(offset, 4);
+}
+
+static inline u32 sunxi_pull_offset(u16 pin)
+{
+	u32 pin_num = pin % PULL_PINS_PER_REG;
+	return pin_num * PULL_PINS_BITS;
+}
+
+static inline u32 sunxi_irq_cfg_reg(u16 irq)
+{
+	u8 reg = irq / IRQ_CFG_IRQ_PER_REG * 0x04;
+	return reg + IRQ_CFG_REG;
+}
+
+static inline u32 sunxi_irq_cfg_offset(u16 irq)
+{
+	u32 irq_num = irq % IRQ_CFG_IRQ_PER_REG;
+	return irq_num * IRQ_CFG_IRQ_BITS;
+}
+
+static inline u32 sunxi_irq_ctrl_reg(u16 irq)
+{
+	u8 reg = irq / IRQ_CTRL_IRQ_PER_REG * 0x04;
+	return reg + IRQ_CTRL_REG;
+}
+
+static inline u32 sunxi_irq_ctrl_offset(u16 irq)
+{
+	u32 irq_num = irq % IRQ_CTRL_IRQ_PER_REG;
+	return irq_num * IRQ_CTRL_IRQ_BITS;
+}
+
+static inline u32 sunxi_irq_status_reg(u16 irq)
+{
+	u8 reg = irq / IRQ_STATUS_IRQ_PER_REG * 0x04;
+	return reg + IRQ_STATUS_REG;
+}
+
+static inline u32 sunxi_irq_status_offset(u16 irq)
+{
+	u32 irq_num = irq % IRQ_STATUS_IRQ_PER_REG;
+	return irq_num * IRQ_STATUS_IRQ_BITS;
+}
+
+int sunxi_pinctrl_init(struct platform_device *pdev,
+		       const struct sunxi_pinctrl_desc *desc);
+
+#endif /* __PINCTRL_SUNXI_H */
diff --git a/drivers/pinctrl/vt8500/pinctrl-wmt.c b/drivers/pinctrl/vt8500/pinctrl-wmt.c
index 9802b67040c..2c61281bebd 100644
--- a/drivers/pinctrl/vt8500/pinctrl-wmt.c
+++ b/drivers/pinctrl/vt8500/pinctrl-wmt.c
@@ -523,17 +523,6 @@ static int wmt_gpio_get_direction(struct gpio_chip *chip, unsigned offset)
 		return GPIOF_DIR_IN;
 }
 
-static int wmt_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
-{
-	return pinctrl_gpio_direction_input(chip->base + offset);
-}
-
-static int wmt_gpio_direction_output(struct gpio_chip *chip, unsigned offset,
-				     int value)
-{
-	return pinctrl_gpio_direction_output(chip->base + offset);
-}
-
 static int wmt_gpio_get_value(struct gpio_chip *chip, unsigned offset)
 {
 	struct wmt_pinctrl_data *data = dev_get_drvdata(chip->dev);
@@ -568,6 +557,18 @@ static void wmt_gpio_set_value(struct gpio_chip *chip, unsigned offset,
 		wmt_clearbits(data, reg_data_out, BIT(bit));
 }
 
+static int wmt_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
+{
+	return pinctrl_gpio_direction_input(chip->base + offset);
+}
+
+static int wmt_gpio_direction_output(struct gpio_chip *chip, unsigned offset,
+				     int value)
+{
+	wmt_gpio_set_value(chip, offset, value);
+	return pinctrl_gpio_direction_output(chip->base + offset);
+}
+
 static struct gpio_chip wmt_gpio_chip = {
 	.label = "gpio-wmt",
 	.owner = THIS_MODULE,
diff --git a/drivers/platform/goldfish/goldfish_pipe.c b/drivers/platform/goldfish/goldfish_pipe.c
index 4f5aa831f54..d9a09d9637d 100644
--- a/drivers/platform/goldfish/goldfish_pipe.c
+++ b/drivers/platform/goldfish/goldfish_pipe.c
@@ -56,6 +56,7 @@
 #include <linux/bitops.h>
 #include <linux/slab.h>
 #include <linux/io.h>
+#include <linux/goldfish.h>
 
 /*
  * IMPORTANT: The following constants must match the ones used and defined
@@ -66,8 +67,10 @@
 #define PIPE_REG_COMMAND		0x00  /* write: value = command */
 #define PIPE_REG_STATUS			0x04  /* read */
 #define PIPE_REG_CHANNEL		0x08  /* read/write: channel id */
+#define PIPE_REG_CHANNEL_HIGH	        0x30  /* read/write: channel id */
 #define PIPE_REG_SIZE			0x0c  /* read/write: buffer size */
 #define PIPE_REG_ADDRESS		0x10  /* write: physical address */
+#define PIPE_REG_ADDRESS_HIGH	        0x34  /* write: physical address */
 #define PIPE_REG_WAKES			0x14  /* read: wake flags */
 #define PIPE_REG_PARAMS_ADDR_LOW	0x18  /* read/write: batch data address */
 #define PIPE_REG_PARAMS_ADDR_HIGH	0x1c  /* read/write: batch data address */
@@ -109,9 +112,9 @@
 #define PIPE_WAKE_WRITE        (1 << 2)  /* pipe can now be written to */
 
 struct access_params {
-	u32 channel;
+	unsigned long channel;
 	u32 size;
-	u32 address;
+	unsigned long address;
 	u32 cmd;
 	u32 result;
 	/* reserved for future extension */
@@ -149,13 +152,14 @@ enum {
 
 
 static u32 goldfish_cmd_status(struct goldfish_pipe *pipe, u32 cmd)
-{ 
+{
 	unsigned long flags;
 	u32 status;
 	struct goldfish_pipe_dev *dev = pipe->dev;
 
 	spin_lock_irqsave(&dev->lock, flags);
-	writel((u32)pipe, dev->base + PIPE_REG_CHANNEL);
+	gf_write64((u64)(unsigned long)pipe, dev->base + PIPE_REG_CHANNEL,
+				dev->base + PIPE_REG_CHANNEL_HIGH);
 	writel(cmd, dev->base + PIPE_REG_COMMAND);
 	status = readl(dev->base + PIPE_REG_STATUS);
 	spin_unlock_irqrestore(&dev->lock, flags);
@@ -163,12 +167,13 @@ static u32 goldfish_cmd_status(struct goldfish_pipe *pipe, u32 cmd)
 }
 
 static void goldfish_cmd(struct goldfish_pipe *pipe, u32 cmd)
-{ 
+{
 	unsigned long flags;
 	struct goldfish_pipe_dev *dev = pipe->dev;
 
 	spin_lock_irqsave(&dev->lock, flags);
-	writel((u32)pipe, dev->base + PIPE_REG_CHANNEL);
+	gf_write64((u64)(unsigned long)pipe, dev->base + PIPE_REG_CHANNEL,
+				dev->base + PIPE_REG_CHANNEL_HIGH);
 	writel(cmd, dev->base + PIPE_REG_COMMAND);
 	spin_unlock_irqrestore(&dev->lock, flags);
 }
@@ -322,9 +327,12 @@ static ssize_t goldfish_pipe_read_write(struct file *filp, char __user *buffer,
 		spin_lock_irqsave(&dev->lock, irq_flags);
 		if (access_with_param(dev, CMD_WRITE_BUFFER + cmd_offset,
 				address, avail, pipe, &status)) {
-			writel((u32)pipe, dev->base + PIPE_REG_CHANNEL);
+			gf_write64((u64)(unsigned long)pipe,
+				   dev->base + PIPE_REG_CHANNEL,
+				   dev->base + PIPE_REG_CHANNEL_HIGH);
 			writel(avail, dev->base + PIPE_REG_SIZE);
-			writel(address, dev->base + PIPE_REG_ADDRESS);
+			gf_write64(address, dev->base + PIPE_REG_ADDRESS,
+				dev->base + PIPE_REG_ADDRESS_HIGH);
 			writel(CMD_WRITE_BUFFER + cmd_offset,
 					dev->base + PIPE_REG_COMMAND);
 			status = readl(dev->base + PIPE_REG_STATUS);
@@ -447,7 +455,15 @@ static irqreturn_t goldfish_pipe_interrupt(int irq, void *dev_id)
 		/* First read the channel, 0 means the end of the list */
 		struct goldfish_pipe *pipe;
 		unsigned long wakes;
-		unsigned long channel = readl(dev->base + PIPE_REG_CHANNEL);
+		unsigned long channel = 0;
+
+#ifdef CONFIG_64BIT
+		channel = (u64)readl(dev->base + PIPE_REG_CHANNEL_HIGH) << 32;
+
+		if (channel == 0)
+			break;
+#endif
+		channel |= readl(dev->base + PIPE_REG_CHANNEL);
 
 		if (channel == 0)
 			break;
diff --git a/drivers/platform/goldfish/pdev_bus.c b/drivers/platform/goldfish/pdev_bus.c
index 92cc4cfafde..8c43589c3ed 100644
--- a/drivers/platform/goldfish/pdev_bus.c
+++ b/drivers/platform/goldfish/pdev_bus.c
@@ -36,6 +36,7 @@
 #define PDEV_BUS_IO_SIZE        (0x14)
 #define PDEV_BUS_IRQ            (0x18)
 #define PDEV_BUS_IRQ_COUNT      (0x1c)
+#define PDEV_BUS_GET_NAME_HIGH  (0x20)
 
 struct pdev_bus_dev {
 	struct list_head list;
@@ -129,7 +130,10 @@ static int goldfish_new_pdev(void)
 	dev->pdev.dev.dma_mask = (void *)(dev->pdev.name + name_len + 1);
 	*dev->pdev.dev.dma_mask = ~0;
 
-	writel((unsigned long)name, pdev_bus_base + PDEV_BUS_GET_NAME);
+#ifdef CONFIG_64BIT
+	writel((u32)((u64)name>>32), pdev_bus_base + PDEV_BUS_GET_NAME_HIGH);
+#endif
+	writel((u32)(unsigned long)name, pdev_bus_base + PDEV_BUS_GET_NAME);
 	name[name_len] = '\0';
 	dev->pdev.id = readl(pdev_bus_base + PDEV_BUS_ID);
 	dev->pdev.resource[0].start = base;
@@ -184,11 +188,6 @@ static int goldfish_pdev_bus_probe(struct platform_device *pdev)
 	pdev_bus_addr = r->start;
 	pdev_bus_len = resource_size(r);
 
-	if (request_mem_region(pdev_bus_addr, pdev_bus_len, "goldfish")) {
-		dev_err(&pdev->dev, "unable to reserve Goldfish MMIO.\n");
-		return -EBUSY;
-	}
-
 	pdev_bus_base = ioremap(pdev_bus_addr, pdev_bus_len);
 	if (pdev_bus_base == NULL) {
 		ret = -ENOMEM;
diff --git a/drivers/platform/x86/asus-wmi.c b/drivers/platform/x86/asus-wmi.c
index c5e082fb82f..91ef69a5226 100644
--- a/drivers/platform/x86/asus-wmi.c
+++ b/drivers/platform/x86/asus-wmi.c
@@ -642,8 +642,7 @@ static void asus_rfkill_hotplug(struct asus_wmi *asus)
 			dev = pci_scan_single_device(bus, 0);
 			if (dev) {
 				pci_bus_assign_resources(bus);
-				if (pci_bus_add_device(dev))
-					pr_err("Unable to hotplug wifi\n");
+				pci_bus_add_device(dev);
 			}
 		} else {
 			dev = pci_get_slot(bus, 0);
diff --git a/drivers/platform/x86/eeepc-laptop.c b/drivers/platform/x86/eeepc-laptop.c
index 399e8c56219..9b0c57cd1d4 100644
--- a/drivers/platform/x86/eeepc-laptop.c
+++ b/drivers/platform/x86/eeepc-laptop.c
@@ -633,8 +633,7 @@ static void eeepc_rfkill_hotplug(struct eeepc_laptop *eeepc, acpi_handle handle)
 			dev = pci_scan_single_device(bus, 0);
 			if (dev) {
 				pci_bus_assign_resources(bus);
-				if (pci_bus_add_device(dev))
-					pr_err("Unable to hotplug wifi\n");
+				pci_bus_add_device(dev);
 			}
 		} else {
 			dev = pci_get_slot(bus, 0);
diff --git a/drivers/pnp/pnpacpi/core.c b/drivers/pnp/pnpacpi/core.c
index 9f611cbbc29..c31aa07b3ba 100644
--- a/drivers/pnp/pnpacpi/core.c
+++ b/drivers/pnp/pnpacpi/core.c
@@ -83,8 +83,7 @@ static int pnpacpi_set_resources(struct pnp_dev *dev)
 {
 	struct acpi_device *acpi_dev;
 	acpi_handle handle;
-	struct acpi_buffer buffer;
-	int ret;
+	int ret = 0;
 
 	pnp_dbg(&dev->dev, "set resources\n");
 
@@ -97,19 +96,26 @@ static int pnpacpi_set_resources(struct pnp_dev *dev)
 	if (WARN_ON_ONCE(acpi_dev != dev->data))
 		dev->data = acpi_dev;
 
-	ret = pnpacpi_build_resource_template(dev, &buffer);
-	if (ret)
-		return ret;
-	ret = pnpacpi_encode_resources(dev, &buffer);
-	if (ret) {
+	if (acpi_has_method(handle, METHOD_NAME__SRS)) {
+		struct acpi_buffer buffer;
+
+		ret = pnpacpi_build_resource_template(dev, &buffer);
+		if (ret)
+			return ret;
+
+		ret = pnpacpi_encode_resources(dev, &buffer);
+		if (!ret) {
+			acpi_status status;
+
+			status = acpi_set_current_resources(handle, &buffer);
+			if (ACPI_FAILURE(status))
+				ret = -EIO;
+		}
 		kfree(buffer.pointer);
-		return ret;
 	}
-	if (ACPI_FAILURE(acpi_set_current_resources(handle, &buffer)))
-		ret = -EINVAL;
-	else if (acpi_bus_power_manageable(handle))
+	if (!ret && acpi_bus_power_manageable(handle))
 		ret = acpi_bus_set_power(handle, ACPI_STATE_D0);
-	kfree(buffer.pointer);
+
 	return ret;
 }
 
@@ -117,7 +123,7 @@ static int pnpacpi_disable_resources(struct pnp_dev *dev)
 {
 	struct acpi_device *acpi_dev;
 	acpi_handle handle;
-	int ret;
+	acpi_status status;
 
 	dev_dbg(&dev->dev, "disable resources\n");
 
@@ -128,13 +134,15 @@ static int pnpacpi_disable_resources(struct pnp_dev *dev)
 	}
 
 	/* acpi_unregister_gsi(pnp_irq(dev, 0)); */
-	ret = 0;
 	if (acpi_bus_power_manageable(handle))
 		acpi_bus_set_power(handle, ACPI_STATE_D3_COLD);
-		/* continue even if acpi_bus_set_power() fails */
-	if (ACPI_FAILURE(acpi_evaluate_object(handle, "_DIS", NULL, NULL)))
-		ret = -ENODEV;
-	return ret;
+
+	/* continue even if acpi_bus_set_power() fails */
+	status = acpi_evaluate_object(handle, "_DIS", NULL, NULL);
+	if (ACPI_FAILURE(status) && status != AE_NOT_FOUND)
+		return -ENODEV;
+
+	return 0;
 }
 
 #ifdef CONFIG_ACPI_SLEEP
diff --git a/drivers/pnp/pnpbios/bioscalls.c b/drivers/pnp/pnpbios/bioscalls.c
index deb7f4bcdb7..438d4c72c7b 100644
--- a/drivers/pnp/pnpbios/bioscalls.c
+++ b/drivers/pnp/pnpbios/bioscalls.c
@@ -37,7 +37,7 @@ __visible struct {
  * kernel begins at offset 3GB...
  */
 
-asmlinkage void pnp_bios_callfunc(void);
+asmlinkage __visible void pnp_bios_callfunc(void);
 
 __asm__(".text			\n"
 	__ALIGN_STR "\n"
diff --git a/drivers/pnp/quirks.c b/drivers/pnp/quirks.c
index 258fef272ea..ebf0d6710b5 100644
--- a/drivers/pnp/quirks.c
+++ b/drivers/pnp/quirks.c
@@ -15,6 +15,7 @@
 
 #include <linux/types.h>
 #include <linux/kernel.h>
+#include <linux/pci.h>
 #include <linux/string.h>
 #include <linux/slab.h>
 #include <linux/pnp.h>
@@ -334,6 +335,81 @@ static void quirk_amd_mmconfig_area(struct pnp_dev *dev)
 }
 #endif
 
+#ifdef CONFIG_PCI
+/* Device IDs of parts that have 32KB MCH space */
+static const unsigned int mch_quirk_devices[] = {
+	0x0154,	/* Ivy Bridge */
+	0x0c00,	/* Haswell */
+};
+
+static struct pci_dev *get_intel_host(void)
+{
+	int i;
+	struct pci_dev *host;
+
+	for (i = 0; i < ARRAY_SIZE(mch_quirk_devices); i++) {
+		host = pci_get_device(PCI_VENDOR_ID_INTEL, mch_quirk_devices[i],
+				      NULL);
+		if (host)
+			return host;
+	}
+	return NULL;
+}
+
+static void quirk_intel_mch(struct pnp_dev *dev)
+{
+	struct pci_dev *host;
+	u32 addr_lo, addr_hi;
+	struct pci_bus_region region;
+	struct resource mch;
+	struct pnp_resource *pnp_res;
+	struct resource *res;
+
+	host = get_intel_host();
+	if (!host)
+		return;
+
+	/*
+	 * MCHBAR is not an architected PCI BAR, so MCH space is usually
+	 * reported as a PNP0C02 resource.  The MCH space was originally
+	 * 16KB, but is 32KB in newer parts.  Some BIOSes still report a
+	 * PNP0C02 resource that is only 16KB, which means the rest of the
+	 * MCH space is consumed but unreported.
+	 */
+
+	/*
+	 * Read MCHBAR for Host Member Mapped Register Range Base
+	 * https://www-ssl.intel.com/content/www/us/en/processors/core/4th-gen-core-family-desktop-vol-2-datasheet
+	 * Sec 3.1.12.
+	 */
+	pci_read_config_dword(host, 0x48, &addr_lo);
+	region.start = addr_lo & ~0x7fff;
+	pci_read_config_dword(host, 0x4c, &addr_hi);
+	region.start |= (u64) addr_hi << 32;
+	region.end = region.start + 32*1024 - 1;
+
+	memset(&mch, 0, sizeof(mch));
+	mch.flags = IORESOURCE_MEM;
+	pcibios_bus_to_resource(host->bus, &mch, &region);
+
+	list_for_each_entry(pnp_res, &dev->resources, list) {
+		res = &pnp_res->res;
+		if (res->end < mch.start || res->start > mch.end)
+			continue;	/* no overlap */
+		if (res->start == mch.start && res->end == mch.end)
+			continue;	/* exact match */
+
+		dev_info(&dev->dev, FW_BUG "PNP resource %pR covers only part of %s Intel MCH; extending to %pR\n",
+			 res, pci_name(host), &mch);
+		res->start = mch.start;
+		res->end = mch.end;
+		break;
+	}
+
+	pci_dev_put(host);
+}
+#endif
+
 /*
  *  PnP Quirks
  *  Cards or devices that need some tweaking due to incomplete resource info
@@ -364,6 +440,9 @@ static struct pnp_fixup pnp_fixups[] = {
 #ifdef CONFIG_AMD_NB
 	{"PNP0c01", quirk_amd_mmconfig_area},
 #endif
+#ifdef CONFIG_PCI
+	{"PNP0c02", quirk_intel_mch},
+#endif
 	{""}
 };
 
diff --git a/drivers/power/reset/Kconfig b/drivers/power/reset/Kconfig
index fa0e4e057b9..bdcf5173e37 100644
--- a/drivers/power/reset/Kconfig
+++ b/drivers/power/reset/Kconfig
@@ -12,6 +12,14 @@ config POWER_RESET_AS3722
 	help
 	  This driver supports turning off board via a ams AS3722 power-off.
 
+config POWER_RESET_AXXIA
+	bool "LSI Axxia reset driver"
+	depends on POWER_RESET && ARCH_AXXIA
+	help
+	  This driver supports restart for Axxia SoC.
+
+	  Say Y if you have an Axxia family SoC.
+
 config POWER_RESET_GPIO
 	bool "GPIO power-off driver"
 	depends on OF_GPIO && POWER_RESET
@@ -43,6 +51,13 @@ config POWER_RESET_RESTART
 	  Instead they restart, and u-boot holds the SoC until the
 	  user presses a key. u-boot then boots into Linux.
 
+config POWER_RESET_SUN6I
+	bool "Allwinner A31 SoC reset driver"
+	depends on ARCH_SUNXI
+	depends on POWER_RESET
+	help
+	  Reboot support for the Allwinner A31 SoCs.
+
 config POWER_RESET_VEXPRESS
 	bool "ARM Versatile Express power-off and reset driver"
 	depends on ARM || ARM64
@@ -57,3 +72,11 @@ config POWER_RESET_XGENE
 	depends on POWER_RESET
 	help
 	  Reboot support for the APM SoC X-Gene Eval boards.
+
+config POWER_RESET_KEYSTONE
+	bool "Keystone reset driver"
+	depends on ARCH_KEYSTONE
+	select MFD_SYSCON
+	help
+	  Reboot support for the KEYSTONE SoCs.
+
diff --git a/drivers/power/reset/Makefile b/drivers/power/reset/Makefile
index a5b4a77d1a4..dde2e8bbac5 100644
--- a/drivers/power/reset/Makefile
+++ b/drivers/power/reset/Makefile
@@ -1,7 +1,10 @@
 obj-$(CONFIG_POWER_RESET_AS3722) += as3722-poweroff.o
+obj-$(CONFIG_POWER_RESET_AXXIA) += axxia-reset.o
 obj-$(CONFIG_POWER_RESET_GPIO) += gpio-poweroff.o
 obj-$(CONFIG_POWER_RESET_MSM) += msm-poweroff.o
 obj-$(CONFIG_POWER_RESET_QNAP) += qnap-poweroff.o
 obj-$(CONFIG_POWER_RESET_RESTART) += restart-poweroff.o
+obj-$(CONFIG_POWER_RESET_SUN6I) += sun6i-reboot.o
 obj-$(CONFIG_POWER_RESET_VEXPRESS) += vexpress-poweroff.o
 obj-$(CONFIG_POWER_RESET_XGENE) += xgene-reboot.o
+obj-$(CONFIG_POWER_RESET_KEYSTONE) += keystone-reset.o
diff --git a/drivers/power/reset/axxia-reset.c b/drivers/power/reset/axxia-reset.c
new file mode 100644
index 00000000000..3b1f8d60178
--- /dev/null
+++ b/drivers/power/reset/axxia-reset.c
@@ -0,0 +1,88 @@
+/*
+ * Reset driver for Axxia devices
+ *
+ * Copyright (C) 2014 LSI
+ *
+ * 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.
+ *
+ */
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/reboot.h>
+#include <linux/regmap.h>
+
+#include <asm/system_misc.h>
+
+
+#define SC_CRIT_WRITE_KEY	0x1000
+#define SC_LATCH_ON_RESET	0x1004
+#define SC_RESET_CONTROL	0x1008
+#define   RSTCTL_RST_ZERO	(1<<3)
+#define   RSTCTL_RST_FAB	(1<<2)
+#define   RSTCTL_RST_CHIP	(1<<1)
+#define   RSTCTL_RST_SYS	(1<<0)
+#define SC_EFUSE_INT_STATUS	0x180c
+#define   EFUSE_READ_DONE	(1<<31)
+
+static struct regmap *syscon;
+
+static void do_axxia_restart(enum reboot_mode reboot_mode, const char *cmd)
+{
+	/* Access Key (0xab) */
+	regmap_write(syscon, SC_CRIT_WRITE_KEY, 0xab);
+	/* Select internal boot from 0xffff0000 */
+	regmap_write(syscon, SC_LATCH_ON_RESET, 0x00000040);
+	/* Assert ResetReadDone (to avoid hanging in boot ROM) */
+	regmap_write(syscon, SC_EFUSE_INT_STATUS, EFUSE_READ_DONE);
+	/* Assert chip reset */
+	regmap_update_bits(syscon, SC_RESET_CONTROL,
+			   RSTCTL_RST_CHIP, RSTCTL_RST_CHIP);
+}
+
+static int axxia_reset_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+
+	syscon = syscon_regmap_lookup_by_phandle(dev->of_node, "syscon");
+	if (IS_ERR(syscon)) {
+		pr_err("%s: syscon lookup failed\n", dev->of_node->name);
+		return PTR_ERR(syscon);
+	}
+
+	arm_pm_restart = do_axxia_restart;
+
+	return 0;
+}
+
+static const struct of_device_id of_axxia_reset_match[] = {
+	{ .compatible = "lsi,axm55xx-reset", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, of_axxia_reset_match);
+
+static struct platform_driver axxia_reset_driver = {
+	.probe = axxia_reset_probe,
+	.driver = {
+		.name = "axxia-reset",
+		.of_match_table = of_match_ptr(of_axxia_reset_match),
+	},
+};
+
+static int __init axxia_reset_init(void)
+{
+	return platform_driver_register(&axxia_reset_driver);
+}
+device_initcall(axxia_reset_init);
diff --git a/drivers/power/reset/keystone-reset.c b/drivers/power/reset/keystone-reset.c
new file mode 100644
index 00000000000..408a18fd91c
--- /dev/null
+++ b/drivers/power/reset/keystone-reset.c
@@ -0,0 +1,166 @@
+/*
+ * TI keystone reboot driver
+ *
+ * Copyright (C) 2014 Texas Instruments Incorporated. http://www.ti.com/
+ *
+ * Author: Ivan Khoronzhuk <ivan.khoronzhuk@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.
+ */
+
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/reboot.h>
+#include <linux/regmap.h>
+#include <asm/system_misc.h>
+#include <linux/mfd/syscon.h>
+#include <linux/of_platform.h>
+
+#define RSTYPE_RG			0x0
+#define RSCTRL_RG			0x4
+#define RSCFG_RG			0x8
+#define RSISO_RG			0xc
+
+#define RSCTRL_KEY_MASK			0x0000ffff
+#define RSCTRL_RESET_MASK		BIT(16)
+#define RSCTRL_KEY			0x5a69
+
+#define RSMUX_OMODE_MASK		0xe
+#define RSMUX_OMODE_RESET_ON		0xa
+#define RSMUX_OMODE_RESET_OFF		0x0
+#define RSMUX_LOCK_MASK			0x1
+#define RSMUX_LOCK_SET			0x1
+
+#define RSCFG_RSTYPE_SOFT		0x300f
+#define RSCFG_RSTYPE_HARD		0x0
+
+#define WDT_MUX_NUMBER			0x4
+
+static int rspll_offset;
+static struct regmap *pllctrl_regs;
+
+/**
+ * rsctrl_enable_rspll_write - enable access to RSCTRL, RSCFG
+ * To be able to access to RSCTRL, RSCFG registers
+ * we have to write a key before
+ */
+static inline int rsctrl_enable_rspll_write(void)
+{
+	return regmap_update_bits(pllctrl_regs, rspll_offset + RSCTRL_RG,
+				  RSCTRL_KEY_MASK, RSCTRL_KEY);
+}
+
+static void rsctrl_restart(enum reboot_mode mode, const char *cmd)
+{
+	/* enable write access to RSTCTRL */
+	rsctrl_enable_rspll_write();
+
+	/* reset the SOC */
+	regmap_update_bits(pllctrl_regs, rspll_offset + RSCTRL_RG,
+			   RSCTRL_RESET_MASK, 0);
+}
+
+static struct of_device_id rsctrl_of_match[] = {
+	{.compatible = "ti,keystone-reset", },
+	{},
+};
+
+static int rsctrl_probe(struct platform_device *pdev)
+{
+	int i;
+	int ret;
+	u32 val;
+	unsigned int rg;
+	u32 rsmux_offset;
+	struct regmap *devctrl_regs;
+	struct device *dev = &pdev->dev;
+	struct device_node *np = dev->of_node;
+
+	if (!np)
+		return -ENODEV;
+
+	/* get regmaps */
+	pllctrl_regs = syscon_regmap_lookup_by_phandle(np, "ti,syscon-pll");
+	if (IS_ERR(pllctrl_regs))
+		return PTR_ERR(pllctrl_regs);
+
+	devctrl_regs = syscon_regmap_lookup_by_phandle(np, "ti,syscon-dev");
+	if (IS_ERR(devctrl_regs))
+		return PTR_ERR(devctrl_regs);
+
+	ret = of_property_read_u32_index(np, "ti,syscon-pll", 1, &rspll_offset);
+	if (ret) {
+		dev_err(dev, "couldn't read the reset pll offset!\n");
+		return -EINVAL;
+	}
+
+	ret = of_property_read_u32_index(np, "ti,syscon-dev", 1, &rsmux_offset);
+	if (ret) {
+		dev_err(dev, "couldn't read the rsmux offset!\n");
+		return -EINVAL;
+	}
+
+	/* set soft/hard reset */
+	val = of_property_read_bool(np, "ti,soft-reset");
+	val = val ? RSCFG_RSTYPE_SOFT : RSCFG_RSTYPE_HARD;
+
+	ret = rsctrl_enable_rspll_write();
+	if (ret)
+		return ret;
+
+	ret = regmap_write(pllctrl_regs, rspll_offset + RSCFG_RG, val);
+	if (ret)
+		return ret;
+
+	arm_pm_restart = rsctrl_restart;
+
+	/* disable a reset isolation for all module clocks */
+	ret = regmap_write(pllctrl_regs, rspll_offset + RSISO_RG, 0);
+	if (ret)
+		return ret;
+
+	/* enable a reset for watchdogs from wdt-list */
+	for (i = 0; i < WDT_MUX_NUMBER; i++) {
+		ret = of_property_read_u32_index(np, "ti,wdt-list", i, &val);
+		if (ret == -EOVERFLOW && !i) {
+			dev_err(dev, "ti,wdt-list property has to contain at"
+				"least one entry\n");
+			return -EINVAL;
+		} else if (ret) {
+			break;
+		}
+
+		if (val >= WDT_MUX_NUMBER) {
+			dev_err(dev, "ti,wdt-list property can contain"
+				"only numbers < 4\n");
+			return -EINVAL;
+		}
+
+		rg = rsmux_offset + val * 4;
+
+		ret = regmap_update_bits(devctrl_regs, rg, RSMUX_OMODE_MASK,
+					 RSMUX_OMODE_RESET_ON |
+					 RSMUX_LOCK_SET);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static struct platform_driver rsctrl_driver = {
+	.probe = rsctrl_probe,
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = KBUILD_MODNAME,
+		.of_match_table = rsctrl_of_match,
+	},
+};
+module_platform_driver(rsctrl_driver);
+
+MODULE_AUTHOR("Ivan Khoronzhuk <ivan.khoronzhuk@ti.com>");
+MODULE_DESCRIPTION("Texas Instruments keystone reset driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:" KBUILD_MODNAME);
diff --git a/drivers/power/reset/sun6i-reboot.c b/drivers/power/reset/sun6i-reboot.c
new file mode 100644
index 00000000000..af2cd7ff2fe
--- /dev/null
+++ b/drivers/power/reset/sun6i-reboot.c
@@ -0,0 +1,85 @@
+/*
+ * Allwinner A31 SoCs reset code
+ *
+ * Copyright (C) 2012-2014 Maxime Ripard
+ *
+ * Maxime Ripard <maxime.ripard@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.
+ */
+
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of_address.h>
+#include <linux/platform_device.h>
+#include <linux/reboot.h>
+
+#include <asm/system_misc.h>
+
+#define SUN6I_WATCHDOG1_IRQ_REG		0x00
+#define SUN6I_WATCHDOG1_CTRL_REG	0x10
+#define SUN6I_WATCHDOG1_CTRL_RESTART		BIT(0)
+#define SUN6I_WATCHDOG1_CONFIG_REG	0x14
+#define SUN6I_WATCHDOG1_CONFIG_RESTART		BIT(0)
+#define SUN6I_WATCHDOG1_CONFIG_IRQ		BIT(1)
+#define SUN6I_WATCHDOG1_MODE_REG	0x18
+#define SUN6I_WATCHDOG1_MODE_ENABLE		BIT(0)
+
+static void __iomem *wdt_base;
+
+static void sun6i_wdt_restart(enum reboot_mode mode, const char *cmd)
+{
+	if (!wdt_base)
+		return;
+
+	/* Disable interrupts */
+	writel(0, wdt_base + SUN6I_WATCHDOG1_IRQ_REG);
+
+	/* We want to disable the IRQ and just reset the whole system */
+	writel(SUN6I_WATCHDOG1_CONFIG_RESTART,
+		wdt_base + SUN6I_WATCHDOG1_CONFIG_REG);
+
+	/* Enable timer. The default and lowest interval value is 0.5s */
+	writel(SUN6I_WATCHDOG1_MODE_ENABLE,
+		wdt_base + SUN6I_WATCHDOG1_MODE_REG);
+
+	/* Restart the watchdog. */
+	writel(SUN6I_WATCHDOG1_CTRL_RESTART,
+		wdt_base + SUN6I_WATCHDOG1_CTRL_REG);
+
+	while (1) {
+		mdelay(5);
+		writel(SUN6I_WATCHDOG1_MODE_ENABLE,
+			wdt_base + SUN6I_WATCHDOG1_MODE_REG);
+	}
+}
+
+static int sun6i_reboot_probe(struct platform_device *pdev)
+{
+	wdt_base = of_iomap(pdev->dev.of_node, 0);
+	if (!wdt_base) {
+		WARN(1, "failed to map watchdog base address");
+		return -ENODEV;
+	}
+
+	arm_pm_restart = sun6i_wdt_restart;
+
+	return 0;
+}
+
+static struct of_device_id sun6i_reboot_of_match[] = {
+	{ .compatible = "allwinner,sun6i-a31-wdt" },
+	{}
+};
+
+static struct platform_driver sun6i_reboot_driver = {
+	.probe = sun6i_reboot_probe,
+	.driver = {
+		.name = "sun6i-reboot",
+		.of_match_table = sun6i_reboot_of_match,
+	},
+};
+module_platform_driver(sun6i_reboot_driver);
diff --git a/drivers/power/reset/vexpress-poweroff.c b/drivers/power/reset/vexpress-poweroff.c
index 476aa495c11..4dc102e2b23 100644
--- a/drivers/power/reset/vexpress-poweroff.c
+++ b/drivers/power/reset/vexpress-poweroff.c
@@ -11,7 +11,7 @@
  * Copyright (C) 2012 ARM Limited
  */
 
-#include <linux/jiffies.h>
+#include <linux/delay.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
 #include <linux/platform_device.h>
@@ -23,17 +23,12 @@
 static void vexpress_reset_do(struct device *dev, const char *what)
 {
 	int err = -ENOENT;
-	struct vexpress_config_func *func =
-			vexpress_config_func_get_by_dev(dev);
+	struct regmap *reg = dev_get_drvdata(dev);
 
-	if (func) {
-		unsigned long timeout;
-
-		err = vexpress_config_write(func, 0, 0);
-
-		timeout = jiffies + HZ;
-		while (time_before(jiffies, timeout))
-			cpu_relax();
+	if (reg) {
+		err = regmap_write(reg, 0, 0);
+		if (!err)
+			mdelay(1000);
 	}
 
 	dev_emerg(dev, "Unable to %s (%d)\n", what, err);
@@ -96,12 +91,18 @@ static int vexpress_reset_probe(struct platform_device *pdev)
 	enum vexpress_reset_func func;
 	const struct of_device_id *match =
 			of_match_device(vexpress_reset_of_match, &pdev->dev);
+	struct regmap *regmap;
 
 	if (match)
 		func = (enum vexpress_reset_func)match->data;
 	else
 		func = pdev->id_entry->driver_data;
 
+	regmap = devm_regmap_init_vexpress_config(&pdev->dev);
+	if (IS_ERR(regmap))
+		return PTR_ERR(regmap);
+	dev_set_drvdata(&pdev->dev, regmap);
+
 	switch (func) {
 	case FUNC_SHUTDOWN:
 		vexpress_power_off_device = &pdev->dev;
diff --git a/drivers/power/tps65090-charger.c b/drivers/power/tps65090-charger.c
index 8fc9d6df87f..1685f63b9e5 100644
--- a/drivers/power/tps65090-charger.c
+++ b/drivers/power/tps65090-charger.c
@@ -28,17 +28,6 @@
 
 #include <linux/mfd/tps65090.h>
 
-#define TPS65090_REG_INTR_STS	0x00
-#define TPS65090_REG_INTR_MASK	0x02
-#define TPS65090_REG_CG_CTRL0	0x04
-#define TPS65090_REG_CG_CTRL1	0x05
-#define TPS65090_REG_CG_CTRL2	0x06
-#define TPS65090_REG_CG_CTRL3	0x07
-#define TPS65090_REG_CG_CTRL4	0x08
-#define TPS65090_REG_CG_CTRL5	0x09
-#define TPS65090_REG_CG_STATUS1	0x0a
-#define TPS65090_REG_CG_STATUS2	0x0b
-
 #define TPS65090_CHARGER_ENABLE	BIT(0)
 #define TPS65090_VACG		BIT(1)
 #define TPS65090_NOITERM	BIT(5)
diff --git a/drivers/ptp/Kconfig b/drivers/ptp/Kconfig
index 6963bdf5417..6aea373547f 100644
--- a/drivers/ptp/Kconfig
+++ b/drivers/ptp/Kconfig
@@ -6,6 +6,7 @@ menu "PTP clock support"
 
 config PTP_1588_CLOCK
 	tristate "PTP clock support"
+	depends on NET
 	select PPS
 	select NET_PTP_CLASSIFY
 	help
@@ -74,7 +75,7 @@ config DP83640_PHY
 config PTP_1588_CLOCK_PCH
 	tristate "Intel PCH EG20T as PTP clock"
 	depends on X86 || COMPILE_TEST
-	depends on HAS_IOMEM
+	depends on HAS_IOMEM && NET
 	select PTP_1588_CLOCK
 	help
 	  This driver adds support for using the PCH EG20T as a PTP
diff --git a/drivers/pwm/pwm-spear.c b/drivers/pwm/pwm-spear.c
index 8ad26b8bf41..cb2d4f0f971 100644
--- a/drivers/pwm/pwm-spear.c
+++ b/drivers/pwm/pwm-spear.c
@@ -2,7 +2,7 @@
  * ST Microelectronics SPEAr Pulse Width Modulator driver
  *
  * Copyright (C) 2012 ST Microelectronics
- * Shiraz Hashim <shiraz.hashim@st.com>
+ * Shiraz Hashim <shiraz.linux.kernel@gmail.com>
  *
  * This file is licensed under the terms of the GNU General Public
  * License version 2. This program is licensed "as is" without any
@@ -264,6 +264,6 @@ static struct platform_driver spear_pwm_driver = {
 module_platform_driver(spear_pwm_driver);
 
 MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Shiraz Hashim <shiraz.hashim@st.com>");
+MODULE_AUTHOR("Shiraz Hashim <shiraz.linux.kernel@gmail.com>");
 MODULE_AUTHOR("Viresh Kumar <viresh.kumar@linaro.com>");
 MODULE_ALIAS("platform:spear-pwm");
diff --git a/drivers/regulator/Kconfig b/drivers/regulator/Kconfig
index 903eb37f047..789eb46090e 100644
--- a/drivers/regulator/Kconfig
+++ b/drivers/regulator/Kconfig
@@ -139,6 +139,13 @@ config REGULATOR_AS3722
 	  AS3722 PMIC. This will enable support for all the software
 	  controllable DCDC/LDO regulators.
 
+config REGULATOR_AXP20X
+	tristate "X-POWERS AXP20X PMIC Regulators"
+	depends on MFD_AXP20X
+	help
+	  This driver provides support for the voltage regulators on the
+	  AXP20X PMIC.
+
 config REGULATOR_BCM590XX
 	tristate "Broadcom BCM590xx PMU Regulators"
 	depends on MFD_BCM590XX
@@ -265,12 +272,21 @@ config REGULATOR_LP8788
 	help
 	  This driver supports LP8788 voltage regulator chip.
 
+config REGULATOR_LTC3589
+	tristate "LTC3589 8-output voltage regulator"
+	depends on I2C
+	select REGMAP_I2C
+	help
+	  This enables support for the LTC3589, LTC3589-1, and LTC3589-2
+	  8-output regulators controlled via I2C.
+
 config REGULATOR_MAX14577
-	tristate "Maxim 14577 regulator"
+	tristate "Maxim 14577/77836 regulator"
 	depends on MFD_MAX14577
 	help
-	  This driver controls a Maxim 14577 regulator via I2C bus.
-	  The regulators include safeout LDO and current regulator 'CHARGER'.
+	  This driver controls a Maxim MAX14577/77836 regulator via I2C bus.
+	  The MAX14577 regulators include safeout LDO and charger current
+	  regulator. The MAX77836 has two additional LDOs.
 
 config REGULATOR_MAX1586
 	tristate "Maxim 1586/1587 voltage regulator"
diff --git a/drivers/regulator/Makefile b/drivers/regulator/Makefile
index 12ef277a48b..d461110f446 100644
--- a/drivers/regulator/Makefile
+++ b/drivers/regulator/Makefile
@@ -20,6 +20,7 @@ obj-$(CONFIG_REGULATOR_ANATOP) += anatop-regulator.o
 obj-$(CONFIG_REGULATOR_ARIZONA) += arizona-micsupp.o arizona-ldo1.o
 obj-$(CONFIG_REGULATOR_AS3711) += as3711-regulator.o
 obj-$(CONFIG_REGULATOR_AS3722) += as3722-regulator.o
+obj-$(CONFIG_REGULATOR_AXP20X) += axp20x-regulator.o
 obj-$(CONFIG_REGULATOR_BCM590XX) += bcm590xx-regulator.o
 obj-$(CONFIG_REGULATOR_DA903X)	+= da903x.o
 obj-$(CONFIG_REGULATOR_DA9052)	+= da9052-regulator.o
@@ -37,6 +38,7 @@ obj-$(CONFIG_REGULATOR_LP872X) += lp872x.o
 obj-$(CONFIG_REGULATOR_LP8788) += lp8788-buck.o
 obj-$(CONFIG_REGULATOR_LP8788) += lp8788-ldo.o
 obj-$(CONFIG_REGULATOR_LP8755) += lp8755.o
+obj-$(CONFIG_REGULATOR_LTC3589) += ltc3589.o
 obj-$(CONFIG_REGULATOR_MAX14577) += max14577.o
 obj-$(CONFIG_REGULATOR_MAX1586) += max1586.o
 obj-$(CONFIG_REGULATOR_MAX8649)	+= max8649.o
diff --git a/drivers/regulator/anatop-regulator.c b/drivers/regulator/anatop-regulator.c
index 7c397bb81e0..4f730af70e7 100644
--- a/drivers/regulator/anatop-regulator.c
+++ b/drivers/regulator/anatop-regulator.c
@@ -300,7 +300,7 @@ static int anatop_regulator_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id of_anatop_regulator_match_tbl[] = {
+static const struct of_device_id of_anatop_regulator_match_tbl[] = {
 	{ .compatible = "fsl,anatop-regulator", },
 	{ /* end */ }
 };
diff --git a/drivers/regulator/arizona-ldo1.c b/drivers/regulator/arizona-ldo1.c
index b1033d30b50..04f262a836b 100644
--- a/drivers/regulator/arizona-ldo1.c
+++ b/drivers/regulator/arizona-ldo1.c
@@ -16,9 +16,11 @@
 #include <linux/init.h>
 #include <linux/bitops.h>
 #include <linux/err.h>
+#include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
+#include <linux/regulator/of_regulator.h>
 #include <linux/gpio.h>
 #include <linux/slab.h>
 
@@ -178,6 +180,42 @@ static const struct regulator_init_data arizona_ldo1_default = {
 	.num_consumer_supplies = 1,
 };
 
+static int arizona_ldo1_of_get_pdata(struct arizona *arizona,
+				     struct regulator_config *config)
+{
+	struct arizona_pdata *pdata = &arizona->pdata;
+	struct arizona_ldo1 *ldo1 = config->driver_data;
+	struct device_node *init_node, *dcvdd_node;
+	struct regulator_init_data *init_data;
+
+	pdata->ldoena = arizona_of_get_named_gpio(arizona, "wlf,ldoena", true);
+
+	init_node = of_get_child_by_name(arizona->dev->of_node, "ldo1");
+	dcvdd_node = of_parse_phandle(arizona->dev->of_node, "DCVDD-supply", 0);
+
+	if (init_node) {
+		config->of_node = init_node;
+
+		init_data = of_get_regulator_init_data(arizona->dev, init_node);
+
+		if (init_data) {
+			init_data->consumer_supplies = &ldo1->supply;
+			init_data->num_consumer_supplies = 1;
+
+			if (dcvdd_node && dcvdd_node != init_node)
+				arizona->external_dcvdd = true;
+
+			pdata->ldo1 = init_data;
+		}
+	} else if (dcvdd_node) {
+		arizona->external_dcvdd = true;
+	}
+
+	of_node_put(dcvdd_node);
+
+	return 0;
+}
+
 static int arizona_ldo1_probe(struct platform_device *pdev)
 {
 	struct arizona *arizona = dev_get_drvdata(pdev->dev.parent);
@@ -186,6 +224,8 @@ static int arizona_ldo1_probe(struct platform_device *pdev)
 	struct arizona_ldo1 *ldo1;
 	int ret;
 
+	arizona->external_dcvdd = false;
+
 	ldo1 = devm_kzalloc(&pdev->dev, sizeof(*ldo1), GFP_KERNEL);
 	if (!ldo1)
 		return -ENOMEM;
@@ -216,6 +256,15 @@ static int arizona_ldo1_probe(struct platform_device *pdev)
 	config.dev = arizona->dev;
 	config.driver_data = ldo1;
 	config.regmap = arizona->regmap;
+
+	if (IS_ENABLED(CONFIG_OF)) {
+		if (!dev_get_platdata(arizona->dev)) {
+			ret = arizona_ldo1_of_get_pdata(arizona, &config);
+			if (ret < 0)
+				return ret;
+		}
+	}
+
 	config.ena_gpio = arizona->pdata.ldoena;
 
 	if (arizona->pdata.ldo1)
@@ -223,6 +272,13 @@ static int arizona_ldo1_probe(struct platform_device *pdev)
 	else
 		config.init_data = &ldo1->init_data;
 
+	/*
+	 * LDO1 can only be used to supply DCVDD so if it has no
+	 * consumers then DCVDD is supplied externally.
+	 */
+	if (config.init_data->num_consumer_supplies == 0)
+		arizona->external_dcvdd = true;
+
 	ldo1->regulator = devm_regulator_register(&pdev->dev, desc, &config);
 	if (IS_ERR(ldo1->regulator)) {
 		ret = PTR_ERR(ldo1->regulator);
@@ -231,6 +287,8 @@ static int arizona_ldo1_probe(struct platform_device *pdev)
 		return ret;
 	}
 
+	of_node_put(config.of_node);
+
 	platform_set_drvdata(pdev, ldo1);
 
 	return 0;
diff --git a/drivers/regulator/arizona-micsupp.c b/drivers/regulator/arizona-micsupp.c
index 6fdd9bf6927..ce9aca5f8ee 100644
--- a/drivers/regulator/arizona-micsupp.c
+++ b/drivers/regulator/arizona-micsupp.c
@@ -16,9 +16,11 @@
 #include <linux/init.h>
 #include <linux/bitops.h>
 #include <linux/err.h>
+#include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
+#include <linux/regulator/of_regulator.h>
 #include <linux/gpio.h>
 #include <linux/slab.h>
 #include <linux/workqueue.h>
@@ -195,6 +197,32 @@ static const struct regulator_init_data arizona_micsupp_ext_default = {
 	.num_consumer_supplies = 1,
 };
 
+static int arizona_micsupp_of_get_pdata(struct arizona *arizona,
+					struct regulator_config *config)
+{
+	struct arizona_pdata *pdata = &arizona->pdata;
+	struct arizona_micsupp *micsupp = config->driver_data;
+	struct device_node *np;
+	struct regulator_init_data *init_data;
+
+	np = of_get_child_by_name(arizona->dev->of_node, "micvdd");
+
+	if (np) {
+		config->of_node = np;
+
+		init_data = of_get_regulator_init_data(arizona->dev, np);
+
+		if (init_data) {
+			init_data->consumer_supplies = &micsupp->supply;
+			init_data->num_consumer_supplies = 1;
+
+			pdata->micvdd = init_data;
+		}
+	}
+
+	return 0;
+}
+
 static int arizona_micsupp_probe(struct platform_device *pdev)
 {
 	struct arizona *arizona = dev_get_drvdata(pdev->dev.parent);
@@ -234,6 +262,14 @@ static int arizona_micsupp_probe(struct platform_device *pdev)
 	config.driver_data = micsupp;
 	config.regmap = arizona->regmap;
 
+	if (IS_ENABLED(CONFIG_OF)) {
+		if (!dev_get_platdata(arizona->dev)) {
+			ret = arizona_micsupp_of_get_pdata(arizona, &config);
+			if (ret < 0)
+				return ret;
+		}
+	}
+
 	if (arizona->pdata.micvdd)
 		config.init_data = arizona->pdata.micvdd;
 	else
@@ -253,6 +289,8 @@ static int arizona_micsupp_probe(struct platform_device *pdev)
 		return ret;
 	}
 
+	of_node_put(config.of_node);
+
 	platform_set_drvdata(pdev, micsupp);
 
 	return 0;
diff --git a/drivers/regulator/axp20x-regulator.c b/drivers/regulator/axp20x-regulator.c
new file mode 100644
index 00000000000..004aadb7bcc
--- /dev/null
+++ b/drivers/regulator/axp20x-regulator.c
@@ -0,0 +1,286 @@
+/*
+ * AXP20x regulators driver.
+ *
+ * Copyright (C) 2013 Carlo Caione <carlo@caione.org>
+ *
+ * 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.
+ *
+ * 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/init.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/mfd/axp20x.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/of_regulator.h>
+
+#define AXP20X_IO_ENABLED		0x03
+#define AXP20X_IO_DISABLED		0x07
+
+#define AXP20X_WORKMODE_DCDC2_MASK	BIT(2)
+#define AXP20X_WORKMODE_DCDC3_MASK	BIT(1)
+
+#define AXP20X_FREQ_DCDC_MASK		0x0f
+
+#define AXP20X_DESC_IO(_id, _supply, _min, _max, _step, _vreg, _vmask, _ereg,   \
+		       _emask, _enable_val, _disable_val)			\
+	[AXP20X_##_id] = {							\
+		.name		= #_id,						\
+		.supply_name	= (_supply),					\
+		.type		= REGULATOR_VOLTAGE,				\
+		.id		= AXP20X_##_id,					\
+		.n_voltages	= (((_max) - (_min)) / (_step) + 1),		\
+		.owner		= THIS_MODULE,					\
+		.min_uV		= (_min) * 1000,				\
+		.uV_step	= (_step) * 1000,				\
+		.vsel_reg	= (_vreg),					\
+		.vsel_mask	= (_vmask),					\
+		.enable_reg	= (_ereg),					\
+		.enable_mask	= (_emask),					\
+		.enable_val	= (_enable_val),				\
+		.disable_val	= (_disable_val),				\
+		.ops		= &axp20x_ops,					\
+	}
+
+#define AXP20X_DESC(_id, _supply, _min, _max, _step, _vreg, _vmask, _ereg,	\
+		    _emask) 							\
+	[AXP20X_##_id] = {							\
+		.name		= #_id,						\
+		.supply_name	= (_supply),					\
+		.type		= REGULATOR_VOLTAGE,				\
+		.id		= AXP20X_##_id,					\
+		.n_voltages	= (((_max) - (_min)) / (_step) + 1),		\
+		.owner		= THIS_MODULE,					\
+		.min_uV		= (_min) * 1000,				\
+		.uV_step	= (_step) * 1000,				\
+		.vsel_reg	= (_vreg),					\
+		.vsel_mask	= (_vmask),					\
+		.enable_reg	= (_ereg),					\
+		.enable_mask	= (_emask),					\
+		.ops		= &axp20x_ops,					\
+	}
+
+#define AXP20X_DESC_FIXED(_id, _supply, _volt)					\
+	[AXP20X_##_id] = {							\
+		.name		= #_id,						\
+		.supply_name	= (_supply),					\
+		.type		= REGULATOR_VOLTAGE,				\
+		.id		= AXP20X_##_id,					\
+		.n_voltages	= 1,						\
+		.owner		= THIS_MODULE,					\
+		.min_uV		= (_volt) * 1000,				\
+		.ops		= &axp20x_ops_fixed				\
+	}
+
+#define AXP20X_DESC_TABLE(_id, _supply, _table, _vreg, _vmask, _ereg, _emask)	\
+	[AXP20X_##_id] = {							\
+		.name		= #_id,						\
+		.supply_name	= (_supply),					\
+		.type		= REGULATOR_VOLTAGE,				\
+		.id		= AXP20X_##_id,					\
+		.n_voltages	= ARRAY_SIZE(_table),				\
+		.owner		= THIS_MODULE,					\
+		.vsel_reg	= (_vreg),					\
+		.vsel_mask	= (_vmask),					\
+		.enable_reg	= (_ereg),					\
+		.enable_mask	= (_emask),					\
+		.volt_table	= (_table),					\
+		.ops		= &axp20x_ops_table,				\
+	}
+
+static const int axp20x_ldo4_data[] = { 1250000, 1300000, 1400000, 1500000, 1600000,
+					1700000, 1800000, 1900000, 2000000, 2500000,
+					2700000, 2800000, 3000000, 3100000, 3200000,
+					3300000 };
+
+static struct regulator_ops axp20x_ops_fixed = {
+	.list_voltage		= regulator_list_voltage_linear,
+};
+
+static struct regulator_ops axp20x_ops_table = {
+	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
+	.list_voltage		= regulator_list_voltage_table,
+	.map_voltage		= regulator_map_voltage_ascend,
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
+	.is_enabled		= regulator_is_enabled_regmap,
+};
+
+static struct regulator_ops axp20x_ops = {
+	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
+	.list_voltage		= regulator_list_voltage_linear,
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
+	.is_enabled		= regulator_is_enabled_regmap,
+};
+
+static const struct regulator_desc axp20x_regulators[] = {
+	AXP20X_DESC(DCDC2, "vin2", 700, 2275, 25, AXP20X_DCDC2_V_OUT, 0x3f,
+		    AXP20X_PWR_OUT_CTRL, 0x10),
+	AXP20X_DESC(DCDC3, "vin3", 700, 3500, 25, AXP20X_DCDC3_V_OUT, 0x7f,
+		    AXP20X_PWR_OUT_CTRL, 0x02),
+	AXP20X_DESC_FIXED(LDO1, "acin", 1300),
+	AXP20X_DESC(LDO2, "ldo24in", 1800, 3300, 100, AXP20X_LDO24_V_OUT, 0xf0,
+		    AXP20X_PWR_OUT_CTRL, 0x04),
+	AXP20X_DESC(LDO3, "ldo3in", 700, 3500, 25, AXP20X_LDO3_V_OUT, 0x7f,
+		    AXP20X_PWR_OUT_CTRL, 0x40),
+	AXP20X_DESC_TABLE(LDO4, "ldo24in", axp20x_ldo4_data, AXP20X_LDO24_V_OUT, 0x0f,
+			  AXP20X_PWR_OUT_CTRL, 0x08),
+	AXP20X_DESC_IO(LDO5, "ldo5in", 1800, 3300, 100, AXP20X_LDO5_V_OUT, 0xf0,
+		       AXP20X_GPIO0_CTRL, 0x07, AXP20X_IO_ENABLED,
+		       AXP20X_IO_DISABLED),
+};
+
+#define AXP_MATCH(_name, _id) \
+	[AXP20X_##_id] = { \
+		.name		= #_name, \
+		.driver_data	= (void *) &axp20x_regulators[AXP20X_##_id], \
+	}
+
+static struct of_regulator_match axp20x_matches[] = {
+	AXP_MATCH(dcdc2, DCDC2),
+	AXP_MATCH(dcdc3, DCDC3),
+	AXP_MATCH(ldo1, LDO1),
+	AXP_MATCH(ldo2, LDO2),
+	AXP_MATCH(ldo3, LDO3),
+	AXP_MATCH(ldo4, LDO4),
+	AXP_MATCH(ldo5, LDO5),
+};
+
+static int axp20x_set_dcdc_freq(struct platform_device *pdev, u32 dcdcfreq)
+{
+	struct axp20x_dev *axp20x = dev_get_drvdata(pdev->dev.parent);
+
+	if (dcdcfreq < 750) {
+		dcdcfreq = 750;
+		dev_warn(&pdev->dev, "DCDC frequency too low. Set to 750kHz\n");
+	}
+
+	if (dcdcfreq > 1875) {
+		dcdcfreq = 1875;
+		dev_warn(&pdev->dev, "DCDC frequency too high. Set to 1875kHz\n");
+	}
+
+	dcdcfreq = (dcdcfreq - 750) / 75;
+
+	return regmap_update_bits(axp20x->regmap, AXP20X_DCDC_FREQ,
+				  AXP20X_FREQ_DCDC_MASK, dcdcfreq);
+}
+
+static int axp20x_regulator_parse_dt(struct platform_device *pdev)
+{
+	struct device_node *np, *regulators;
+	int ret;
+	u32 dcdcfreq;
+
+	np = of_node_get(pdev->dev.parent->of_node);
+	if (!np)
+		return 0;
+
+	regulators = of_get_child_by_name(np, "regulators");
+	if (!regulators) {
+		dev_warn(&pdev->dev, "regulators node not found\n");
+	} else {
+		ret = of_regulator_match(&pdev->dev, regulators, axp20x_matches,
+					 ARRAY_SIZE(axp20x_matches));
+		if (ret < 0) {
+			dev_err(&pdev->dev, "Error parsing regulator init data: %d\n", ret);
+			return ret;
+		}
+
+		dcdcfreq = 1500;
+		of_property_read_u32(regulators, "x-powers,dcdc-freq", &dcdcfreq);
+		ret = axp20x_set_dcdc_freq(pdev, dcdcfreq);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "Error setting dcdc frequency: %d\n", ret);
+			return ret;
+		}
+
+		of_node_put(regulators);
+	}
+
+	return 0;
+}
+
+static int axp20x_set_dcdc_workmode(struct regulator_dev *rdev, int id, u32 workmode)
+{
+	unsigned int mask = AXP20X_WORKMODE_DCDC2_MASK;
+
+	if ((id != AXP20X_DCDC2) && (id != AXP20X_DCDC3))
+		return -EINVAL;
+
+	if (id == AXP20X_DCDC3)
+		mask = AXP20X_WORKMODE_DCDC3_MASK;
+
+	workmode <<= ffs(mask) - 1;
+
+	return regmap_update_bits(rdev->regmap, AXP20X_DCDC_MODE, mask, workmode);
+}
+
+static int axp20x_regulator_probe(struct platform_device *pdev)
+{
+	struct regulator_dev *rdev;
+	struct axp20x_dev *axp20x = dev_get_drvdata(pdev->dev.parent);
+	struct regulator_config config = { };
+	struct regulator_init_data *init_data;
+	int ret, i;
+	u32 workmode;
+
+	ret = axp20x_regulator_parse_dt(pdev);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < AXP20X_REG_ID_MAX; i++) {
+		init_data = axp20x_matches[i].init_data;
+
+		config.dev = &pdev->dev;
+		config.init_data = init_data;
+		config.regmap = axp20x->regmap;
+		config.of_node = axp20x_matches[i].of_node;
+
+		rdev = devm_regulator_register(&pdev->dev, &axp20x_regulators[i],
+					       &config);
+		if (IS_ERR(rdev)) {
+			dev_err(&pdev->dev, "Failed to register %s\n",
+				axp20x_regulators[i].name);
+
+			return PTR_ERR(rdev);
+		}
+
+		ret = of_property_read_u32(axp20x_matches[i].of_node, "x-powers,dcdc-workmode",
+					   &workmode);
+		if (!ret) {
+			if (axp20x_set_dcdc_workmode(rdev, i, workmode))
+				dev_err(&pdev->dev, "Failed to set workmode on %s\n",
+					axp20x_regulators[i].name);
+		}
+	}
+
+	return 0;
+}
+
+static struct platform_driver axp20x_regulator_driver = {
+	.probe	= axp20x_regulator_probe,
+	.driver	= {
+		.name		= "axp20x-regulator",
+		.owner		= THIS_MODULE,
+	},
+};
+
+module_platform_driver(axp20x_regulator_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Carlo Caione <carlo@caione.org>");
+MODULE_DESCRIPTION("Regulator Driver for AXP20X PMIC");
diff --git a/drivers/regulator/bcm590xx-regulator.c b/drivers/regulator/bcm590xx-regulator.c
index c3750c5b382..57544e254a7 100644
--- a/drivers/regulator/bcm590xx-regulator.c
+++ b/drivers/regulator/bcm590xx-regulator.c
@@ -22,7 +22,7 @@
 #include <linux/regulator/of_regulator.h>
 #include <linux/slab.h>
 
-/* Register defs */
+/* I2C slave 0 registers */
 #define BCM590XX_RFLDOPMCTRL1	0x60
 #define BCM590XX_IOSR1PMCTRL1	0x7a
 #define BCM590XX_IOSR2PMCTRL1	0x7c
@@ -31,13 +31,34 @@
 #define BCM590XX_SDSR2PMCTRL1	0x86
 #define BCM590XX_MSRPMCTRL1	0x8a
 #define BCM590XX_VSRPMCTRL1	0x8e
-#define BCM590XX_REG_ENABLE	BIT(7)
-
 #define BCM590XX_RFLDOCTRL	0x96
 #define BCM590XX_CSRVOUT1	0xc0
+
+/* I2C slave 1 registers */
+#define BCM590XX_GPLDO5PMCTRL1	0x16
+#define BCM590XX_GPLDO6PMCTRL1	0x18
+#define BCM590XX_GPLDO1CTRL	0x1a
+#define BCM590XX_GPLDO2CTRL	0x1b
+#define BCM590XX_GPLDO3CTRL	0x1c
+#define BCM590XX_GPLDO4CTRL	0x1d
+#define BCM590XX_GPLDO5CTRL	0x1e
+#define BCM590XX_GPLDO6CTRL	0x1f
+#define BCM590XX_OTG_CTRL	0x40
+#define BCM590XX_GPLDO1PMCTRL1	0x57
+#define BCM590XX_GPLDO2PMCTRL1	0x59
+#define BCM590XX_GPLDO3PMCTRL1	0x5b
+#define BCM590XX_GPLDO4PMCTRL1	0x5d
+
+#define BCM590XX_REG_ENABLE	BIT(7)
+#define BCM590XX_VBUS_ENABLE	BIT(2)
 #define BCM590XX_LDO_VSEL_MASK	GENMASK(5, 3)
 #define BCM590XX_SR_VSEL_MASK	GENMASK(5, 0)
 
+/*
+ * RFLDO to VSR regulators are
+ * accessed via I2C slave 0
+ */
+
 /* LDO regulator IDs */
 #define BCM590XX_REG_RFLDO	0
 #define BCM590XX_REG_CAMLDO1	1
@@ -62,9 +83,25 @@
 #define BCM590XX_REG_SDSR2	18
 #define BCM590XX_REG_VSR	19
 
-#define BCM590XX_NUM_REGS	20
+/*
+ * GPLDO1 to VBUS regulators are
+ * accessed via I2C slave 1
+ */
+
+#define BCM590XX_REG_GPLDO1	20
+#define BCM590XX_REG_GPLDO2	21
+#define BCM590XX_REG_GPLDO3	22
+#define BCM590XX_REG_GPLDO4	23
+#define BCM590XX_REG_GPLDO5	24
+#define BCM590XX_REG_GPLDO6	25
+#define BCM590XX_REG_VBUS	26
+
+#define BCM590XX_NUM_REGS	27
 
 #define BCM590XX_REG_IS_LDO(n)	(n < BCM590XX_REG_CSR)
+#define BCM590XX_REG_IS_GPLDO(n) \
+	((n > BCM590XX_REG_VSR) && (n < BCM590XX_REG_VBUS))
+#define BCM590XX_REG_IS_VBUS(n)	(n == BCM590XX_REG_VBUS)
 
 struct bcm590xx_board {
 	struct regulator_init_data *bcm590xx_pmu_init_data[BCM590XX_NUM_REGS];
@@ -149,6 +186,12 @@ static struct bcm590xx_info bcm590xx_regs[] = {
 	BCM590XX_REG_RANGES(sdsr1, dcdc_sdsr1_ranges),
 	BCM590XX_REG_RANGES(sdsr2, dcdc_iosr1_ranges),
 	BCM590XX_REG_RANGES(vsr, dcdc_iosr1_ranges),
+	BCM590XX_REG_TABLE(gpldo1, ldo_a_table),
+	BCM590XX_REG_TABLE(gpldo2, ldo_a_table),
+	BCM590XX_REG_TABLE(gpldo3, ldo_a_table),
+	BCM590XX_REG_TABLE(gpldo4, ldo_a_table),
+	BCM590XX_REG_TABLE(gpldo5, ldo_a_table),
+	BCM590XX_REG_TABLE(gpldo6, ldo_a_table),
 };
 
 struct bcm590xx_reg {
@@ -161,6 +204,8 @@ static int bcm590xx_get_vsel_register(int id)
 {
 	if (BCM590XX_REG_IS_LDO(id))
 		return BCM590XX_RFLDOCTRL + id;
+	else if (BCM590XX_REG_IS_GPLDO(id))
+		return BCM590XX_GPLDO1CTRL + id;
 	else
 		return BCM590XX_CSRVOUT1 + (id - BCM590XX_REG_CSR) * 3;
 }
@@ -171,6 +216,8 @@ static int bcm590xx_get_enable_register(int id)
 
 	if (BCM590XX_REG_IS_LDO(id))
 		reg = BCM590XX_RFLDOPMCTRL1 + id * 2;
+	else if (BCM590XX_REG_IS_GPLDO(id))
+		reg = BCM590XX_GPLDO1PMCTRL1 + id * 2;
 	else
 		switch (id) {
 		case BCM590XX_REG_CSR:
@@ -191,8 +238,11 @@ static int bcm590xx_get_enable_register(int id)
 		case BCM590XX_REG_SDSR2:
 			reg = BCM590XX_SDSR2PMCTRL1;
 			break;
+		case BCM590XX_REG_VBUS:
+			reg = BCM590XX_OTG_CTRL;
 		};
 
+
 	return reg;
 }
 
@@ -216,6 +266,12 @@ static struct regulator_ops bcm590xx_ops_dcdc = {
 	.map_voltage		= regulator_map_voltage_linear_range,
 };
 
+static struct regulator_ops bcm590xx_ops_vbus = {
+	.is_enabled		= regulator_is_enabled_regmap,
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
+};
+
 #define BCM590XX_MATCH(_name, _id) \
 	{ \
 		.name = #_name, \
@@ -243,6 +299,13 @@ static struct of_regulator_match bcm590xx_matches[] = {
 	BCM590XX_MATCH(sdsr1, SDSR1),
 	BCM590XX_MATCH(sdsr2, SDSR2),
 	BCM590XX_MATCH(vsr, VSR),
+	BCM590XX_MATCH(gpldo1, GPLDO1),
+	BCM590XX_MATCH(gpldo2, GPLDO2),
+	BCM590XX_MATCH(gpldo3, GPLDO3),
+	BCM590XX_MATCH(gpldo4, GPLDO4),
+	BCM590XX_MATCH(gpldo5, GPLDO5),
+	BCM590XX_MATCH(gpldo6, GPLDO6),
+	BCM590XX_MATCH(vbus, VBUS),
 };
 
 static struct bcm590xx_board *bcm590xx_parse_dt_reg_data(
@@ -353,17 +416,23 @@ static int bcm590xx_probe(struct platform_device *pdev)
 		pmu->desc[i].linear_ranges = info->linear_ranges;
 		pmu->desc[i].n_linear_ranges = info->n_linear_ranges;
 
-		if (BCM590XX_REG_IS_LDO(i)) {
+		if ((BCM590XX_REG_IS_LDO(i)) || (BCM590XX_REG_IS_GPLDO(i))) {
 			pmu->desc[i].ops = &bcm590xx_ops_ldo;
 			pmu->desc[i].vsel_mask = BCM590XX_LDO_VSEL_MASK;
-		} else {
+		} else if (BCM590XX_REG_IS_VBUS(i))
+			pmu->desc[i].ops = &bcm590xx_ops_vbus;
+		else {
 			pmu->desc[i].ops = &bcm590xx_ops_dcdc;
 			pmu->desc[i].vsel_mask = BCM590XX_SR_VSEL_MASK;
 		}
 
-		pmu->desc[i].vsel_reg = bcm590xx_get_vsel_register(i);
-		pmu->desc[i].enable_is_inverted = true;
-		pmu->desc[i].enable_mask = BCM590XX_REG_ENABLE;
+		if (BCM590XX_REG_IS_VBUS(i))
+			pmu->desc[i].enable_mask = BCM590XX_VBUS_ENABLE;
+		else {
+			pmu->desc[i].vsel_reg = bcm590xx_get_vsel_register(i);
+			pmu->desc[i].enable_is_inverted = true;
+			pmu->desc[i].enable_mask = BCM590XX_REG_ENABLE;
+		}
 		pmu->desc[i].enable_reg = bcm590xx_get_enable_register(i);
 		pmu->desc[i].type = REGULATOR_VOLTAGE;
 		pmu->desc[i].owner = THIS_MODULE;
@@ -371,7 +440,10 @@ static int bcm590xx_probe(struct platform_device *pdev)
 		config.dev = bcm590xx->dev;
 		config.init_data = reg_data;
 		config.driver_data = pmu;
-		config.regmap = bcm590xx->regmap;
+		if (BCM590XX_REG_IS_GPLDO(i) || BCM590XX_REG_IS_VBUS(i))
+			config.regmap = bcm590xx->regmap_sec;
+		else
+			config.regmap = bcm590xx->regmap_pri;
 
 		if (bcm590xx_reg_matches)
 			config.of_node = bcm590xx_reg_matches[i].of_node;
diff --git a/drivers/regulator/core.c b/drivers/regulator/core.c
index 9a09f3cdbab..4c1f999041d 100644
--- a/drivers/regulator/core.c
+++ b/drivers/regulator/core.c
@@ -844,13 +844,22 @@ static int machine_constraints_voltage(struct regulator_dev *rdev,
 	/* do we need to apply the constraint voltage */
 	if (rdev->constraints->apply_uV &&
 	    rdev->constraints->min_uV == rdev->constraints->max_uV) {
-		ret = _regulator_do_set_voltage(rdev,
-						rdev->constraints->min_uV,
-						rdev->constraints->max_uV);
-		if (ret < 0) {
-			rdev_err(rdev, "failed to apply %duV constraint\n",
-				 rdev->constraints->min_uV);
-			return ret;
+		int current_uV = _regulator_get_voltage(rdev);
+		if (current_uV < 0) {
+			rdev_err(rdev, "failed to get the current voltage\n");
+			return current_uV;
+		}
+		if (current_uV < rdev->constraints->min_uV ||
+		    current_uV > rdev->constraints->max_uV) {
+			ret = _regulator_do_set_voltage(
+				rdev, rdev->constraints->min_uV,
+				rdev->constraints->max_uV);
+			if (ret < 0) {
+				rdev_err(rdev,
+					"failed to apply %duV constraint\n",
+					rdev->constraints->min_uV);
+				return ret;
+			}
 		}
 	}
 
@@ -1430,9 +1439,9 @@ EXPORT_SYMBOL_GPL(regulator_get);
  *
  * Returns a struct regulator corresponding to the regulator producer,
  * or IS_ERR() condition containing errno.  Other consumers will be
- * unable to obtain this reference is held and the use count for the
- * regulator will be initialised to reflect the current state of the
- * regulator.
+ * unable to obtain this regulator while this reference is held and the
+ * use count for the regulator will be initialised to reflect the current
+ * state of the regulator.
  *
  * This is intended for use by consumers which cannot tolerate shared
  * use of the regulator such as those which need to force the
@@ -1456,10 +1465,7 @@ EXPORT_SYMBOL_GPL(regulator_get_exclusive);
  * @id: Supply name or regulator ID.
  *
  * Returns a struct regulator corresponding to the regulator producer,
- * or IS_ERR() condition containing errno.  Other consumers will be
- * unable to obtain this reference is held and the use count for the
- * regulator will be initialised to reflect the current state of the
- * regulator.
+ * or IS_ERR() condition containing errno.
  *
  * This is intended for use by consumers for devices which can have
  * some supplies unconnected in normal use, such as some MMC devices.
@@ -1597,9 +1603,10 @@ EXPORT_SYMBOL_GPL(regulator_unregister_supply_alias);
  * registered any aliases that were registered will be removed
  * before returning to the caller.
  */
-int regulator_bulk_register_supply_alias(struct device *dev, const char **id,
+int regulator_bulk_register_supply_alias(struct device *dev,
+					 const char *const *id,
 					 struct device *alias_dev,
-					 const char **alias_id,
+					 const char *const *alias_id,
 					 int num_id)
 {
 	int i;
@@ -1637,7 +1644,7 @@ EXPORT_SYMBOL_GPL(regulator_bulk_register_supply_alias);
  * aliases in one operation.
  */
 void regulator_bulk_unregister_supply_alias(struct device *dev,
-					    const char **id,
+					    const char *const *id,
 					    int num_id)
 {
 	int i;
@@ -2321,6 +2328,10 @@ static int _regulator_do_set_voltage(struct regulator_dev *rdev,
 			    regulator_list_voltage_linear)
 				ret = regulator_map_voltage_linear(rdev,
 								min_uV, max_uV);
+			else if (rdev->desc->ops->list_voltage ==
+				 regulator_list_voltage_linear_range)
+				ret = regulator_map_voltage_linear_range(rdev,
+								min_uV, max_uV);
 			else
 				ret = regulator_map_voltage_iterate(rdev,
 								min_uV, max_uV);
@@ -3447,7 +3458,7 @@ regulator_register(const struct regulator_desc *regulator_desc,
 
 	/* register with sysfs */
 	rdev->dev.class = &regulator_class;
-	rdev->dev.of_node = config->of_node;
+	rdev->dev.of_node = of_node_get(config->of_node);
 	rdev->dev.parent = dev;
 	dev_set_name(&rdev->dev, "regulator.%d",
 		     atomic_inc_return(&regulator_no) - 1);
@@ -3589,6 +3600,7 @@ void regulator_unregister(struct regulator_dev *rdev)
 	list_del(&rdev->list);
 	kfree(rdev->constraints);
 	regulator_ena_gpio_free(rdev);
+	of_node_put(rdev->dev.of_node);
 	device_unregister(&rdev->dev);
 	mutex_unlock(&regulator_list_mutex);
 }
@@ -3819,8 +3831,9 @@ static int __init regulator_init_complete(void)
 	mutex_lock(&regulator_list_mutex);
 
 	/* If we have a full configuration then disable any regulators
-	 * which are not in use or always_on.  This will become the
-	 * default behaviour in the future.
+	 * we have permission to change the status for and which are
+	 * not in use or always_on.  This is effectively the default
+	 * for DT and ACPI as they have full constraints.
 	 */
 	list_for_each_entry(rdev, &regulator_list, list) {
 		ops = rdev->desc->ops;
@@ -3829,6 +3842,9 @@ static int __init regulator_init_complete(void)
 		if (c && c->always_on)
 			continue;
 
+		if (c && !(c->valid_ops_mask & REGULATOR_CHANGE_STATUS))
+			continue;
+
 		mutex_lock(&rdev->mutex);
 
 		if (rdev->use_count)
@@ -3867,4 +3883,4 @@ unlock:
 
 	return 0;
 }
-late_initcall(regulator_init_complete);
+late_initcall_sync(regulator_init_complete);
diff --git a/drivers/regulator/devres.c b/drivers/regulator/devres.c
index f44818b838d..8f785bc9e51 100644
--- a/drivers/regulator/devres.c
+++ b/drivers/regulator/devres.c
@@ -360,9 +360,9 @@ EXPORT_SYMBOL_GPL(devm_regulator_unregister_supply_alias);
  * will be removed before returning to the caller.
  */
 int devm_regulator_bulk_register_supply_alias(struct device *dev,
-					      const char **id,
+					      const char *const *id,
 					      struct device *alias_dev,
-					      const char **alias_id,
+					      const char *const *alias_id,
 					      int num_id)
 {
 	int i;
@@ -404,7 +404,7 @@ EXPORT_SYMBOL_GPL(devm_regulator_bulk_register_supply_alias);
  * will ensure that the resource is freed.
  */
 void devm_regulator_bulk_unregister_supply_alias(struct device *dev,
-						 const char **id,
+						 const char *const *id,
 						 int num_id)
 {
 	int i;
diff --git a/drivers/regulator/fixed.c b/drivers/regulator/fixed.c
index c61f7e97e4f..354105eff1f 100644
--- a/drivers/regulator/fixed.c
+++ b/drivers/regulator/fixed.c
@@ -50,7 +50,6 @@ of_get_fixed_voltage_config(struct device *dev)
 {
 	struct fixed_voltage_config *config;
 	struct device_node *np = dev->of_node;
-	const __be32 *delay;
 	struct regulator_init_data *init_data;
 
 	config = devm_kzalloc(dev, sizeof(struct fixed_voltage_config),
@@ -91,15 +90,11 @@ of_get_fixed_voltage_config(struct device *dev)
 	if ((config->gpio == -ENODEV) || (config->gpio == -EPROBE_DEFER))
 		return ERR_PTR(-EPROBE_DEFER);
 
-	delay = of_get_property(np, "startup-delay-us", NULL);
-	if (delay)
-		config->startup_delay = be32_to_cpu(*delay);
+	of_property_read_u32(np, "startup-delay-us", &config->startup_delay);
 
-	if (of_find_property(np, "enable-active-high", NULL))
-		config->enable_high = true;
-
-	if (of_find_property(np, "gpio-open-drain", NULL))
-		config->gpio_is_open_drain = true;
+	config->enable_high = of_property_read_bool(np, "enable-active-high");
+	config->gpio_is_open_drain = of_property_read_bool(np,
+							   "gpio-open-drain");
 
 	if (of_find_property(np, "vin-supply", NULL))
 		config->input_supply = "vin";
diff --git a/drivers/regulator/ltc3589.c b/drivers/regulator/ltc3589.c
new file mode 100644
index 00000000000..110a99ee116
--- /dev/null
+++ b/drivers/regulator/ltc3589.c
@@ -0,0 +1,554 @@
+/*
+ * Linear Technology LTC3589,LTC3589-1 regulator support
+ *
+ * Copyright (c) 2014 Philipp Zabel <p.zabel@pengutronix.de>, Pengutronix
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * 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/i2c.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/of.h>
+#include <linux/regmap.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/of_regulator.h>
+
+#define DRIVER_NAME		"ltc3589"
+
+#define LTC3589_IRQSTAT		0x02
+#define LTC3589_SCR1		0x07
+#define LTC3589_OVEN		0x10
+#define LTC3589_SCR2		0x12
+#define LTC3589_PGSTAT		0x13
+#define LTC3589_VCCR		0x20
+#define LTC3589_CLIRQ		0x21
+#define LTC3589_B1DTV1		0x23
+#define LTC3589_B1DTV2		0x24
+#define LTC3589_VRRCR		0x25
+#define LTC3589_B2DTV1		0x26
+#define LTC3589_B2DTV2		0x27
+#define LTC3589_B3DTV1		0x29
+#define LTC3589_B3DTV2		0x2a
+#define LTC3589_L2DTV1		0x32
+#define LTC3589_L2DTV2		0x33
+
+#define LTC3589_IRQSTAT_PGOOD_TIMEOUT	BIT(3)
+#define LTC3589_IRQSTAT_UNDERVOLT_WARN	BIT(4)
+#define LTC3589_IRQSTAT_UNDERVOLT_FAULT	BIT(5)
+#define LTC3589_IRQSTAT_THERMAL_WARN	BIT(6)
+#define LTC3589_IRQSTAT_THERMAL_FAULT	BIT(7)
+
+#define LTC3589_OVEN_SW1		BIT(0)
+#define LTC3589_OVEN_SW2		BIT(1)
+#define LTC3589_OVEN_SW3		BIT(2)
+#define LTC3589_OVEN_BB_OUT		BIT(3)
+#define LTC3589_OVEN_LDO2		BIT(4)
+#define LTC3589_OVEN_LDO3		BIT(5)
+#define LTC3589_OVEN_LDO4		BIT(6)
+#define LTC3589_OVEN_SW_CTRL		BIT(7)
+
+#define LTC3589_VCCR_SW1_GO		BIT(0)
+#define LTC3589_VCCR_SW2_GO		BIT(2)
+#define LTC3589_VCCR_SW3_GO		BIT(4)
+#define LTC3589_VCCR_LDO2_GO		BIT(6)
+
+enum ltc3589_variant {
+	LTC3589,
+	LTC3589_1,
+	LTC3589_2,
+};
+
+enum ltc3589_reg {
+	LTC3589_SW1,
+	LTC3589_SW2,
+	LTC3589_SW3,
+	LTC3589_BB_OUT,
+	LTC3589_LDO1,
+	LTC3589_LDO2,
+	LTC3589_LDO3,
+	LTC3589_LDO4,
+	LTC3589_NUM_REGULATORS,
+};
+
+struct ltc3589_regulator {
+	struct regulator_desc desc;
+
+	/* External feedback voltage divider */
+	unsigned int r1;
+	unsigned int r2;
+};
+
+struct ltc3589 {
+	struct regmap *regmap;
+	struct device *dev;
+	enum ltc3589_variant variant;
+	struct ltc3589_regulator regulator_descs[LTC3589_NUM_REGULATORS];
+	struct regulator_dev *regulators[LTC3589_NUM_REGULATORS];
+};
+
+static const int ltc3589_ldo4[] = {
+	2800000, 2500000, 1800000, 3300000,
+};
+
+static const int ltc3589_12_ldo4[] = {
+	1200000, 1800000, 2500000, 3200000,
+};
+
+static int ltc3589_set_ramp_delay(struct regulator_dev *rdev, int ramp_delay)
+{
+	struct ltc3589 *ltc3589 = rdev_get_drvdata(rdev);
+	int sel, shift;
+
+	if (unlikely(ramp_delay <= 0))
+		return -EINVAL;
+
+	/* VRRCR slew rate offsets are the same as VCCR go bit offsets */
+	shift = ffs(rdev->desc->apply_bit) - 1;
+
+	/* The slew rate can be set to 0.88, 1.75, 3.5, or 7 mV/uS */
+	for (sel = 0; sel < 4; sel++) {
+		if ((880 << sel) >= ramp_delay) {
+			return regmap_update_bits(ltc3589->regmap,
+						  LTC3589_VRRCR,
+						  0x3 << shift, sel << shift);
+		}
+	}
+	return -EINVAL;
+}
+
+static int ltc3589_set_suspend_voltage(struct regulator_dev *rdev, int uV)
+{
+	struct ltc3589 *ltc3589 = rdev_get_drvdata(rdev);
+	int sel;
+
+	sel = regulator_map_voltage_linear(rdev, uV, uV);
+	if (sel < 0)
+		return sel;
+
+	/* DTV2 register follows right after the corresponding DTV1 register */
+	return regmap_update_bits(ltc3589->regmap, rdev->desc->vsel_reg + 1,
+				  rdev->desc->vsel_mask, sel);
+}
+
+static int ltc3589_set_suspend_mode(struct regulator_dev *rdev,
+				    unsigned int mode)
+{
+	struct ltc3589 *ltc3589 = rdev_get_drvdata(rdev);
+	int mask, bit = 0;
+
+	/* VCCR reference selects are right next to the VCCR go bits */
+	mask = rdev->desc->apply_bit << 1;
+
+	if (mode == REGULATOR_MODE_STANDBY)
+		bit = mask;	/* Select DTV2 */
+
+	mask |= rdev->desc->apply_bit;
+	bit |= rdev->desc->apply_bit;
+	return regmap_update_bits(ltc3589->regmap, LTC3589_VCCR, mask, bit);
+}
+
+/* SW1, SW2, SW3, LDO2 */
+static struct regulator_ops ltc3589_linear_regulator_ops = {
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
+	.list_voltage = regulator_list_voltage_linear,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.set_ramp_delay = ltc3589_set_ramp_delay,
+	.set_voltage_time_sel = regulator_set_voltage_time_sel,
+	.set_suspend_voltage = ltc3589_set_suspend_voltage,
+	.set_suspend_mode = ltc3589_set_suspend_mode,
+};
+
+/* BB_OUT, LDO3 */
+static struct regulator_ops ltc3589_fixed_regulator_ops = {
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
+};
+
+/* LDO1 */
+static struct regulator_ops ltc3589_fixed_standby_regulator_ops = {
+};
+
+/* LDO4 */
+static struct regulator_ops ltc3589_table_regulator_ops = {
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
+	.list_voltage = regulator_list_voltage_table,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+};
+
+
+#define LTC3589_REG(_name, _ops, en_bit, dtv1_reg, dtv_mask, go_bit)	\
+	[LTC3589_ ## _name] = {						\
+		.desc = {						\
+			.name = #_name,					\
+			.n_voltages = (dtv_mask) + 1,			\
+			.min_uV = (go_bit) ? 362500 : 0,		\
+			.uV_step = (go_bit) ? 12500 : 0,		\
+			.ramp_delay = (go_bit) ? 1750 : 0,		\
+			.fixed_uV = (dtv_mask) ? 0 : 800000,		\
+			.ops = &ltc3589_ ## _ops ## _regulator_ops,	\
+			.type = REGULATOR_VOLTAGE,			\
+			.id = LTC3589_ ## _name,			\
+			.owner = THIS_MODULE,				\
+			.vsel_reg = (dtv1_reg),			\
+			.vsel_mask = (dtv_mask),			\
+			.apply_reg = (go_bit) ? LTC3589_VCCR : 0,	\
+			.apply_bit = (go_bit),				\
+			.enable_reg = (en_bit) ? LTC3589_OVEN : 0,	\
+			.enable_mask = (en_bit),			\
+		},							\
+	}
+
+#define LTC3589_LINEAR_REG(_name, _dtv1)				\
+	LTC3589_REG(_name, linear, LTC3589_OVEN_ ## _name,		\
+		    LTC3589_ ## _dtv1, 0x1f,				\
+		    LTC3589_VCCR_ ## _name ## _GO)
+
+#define LTC3589_FIXED_REG(_name) \
+	LTC3589_REG(_name, fixed, LTC3589_OVEN_ ## _name, 0, 0, 0)
+
+static struct ltc3589_regulator ltc3589_regulators[LTC3589_NUM_REGULATORS] = {
+	LTC3589_LINEAR_REG(SW1, B1DTV1),
+	LTC3589_LINEAR_REG(SW2, B2DTV1),
+	LTC3589_LINEAR_REG(SW3, B3DTV1),
+	LTC3589_FIXED_REG(BB_OUT),
+	LTC3589_REG(LDO1, fixed_standby, 0, 0, 0, 0),
+	LTC3589_LINEAR_REG(LDO2, L2DTV1),
+	LTC3589_FIXED_REG(LDO3),
+	LTC3589_REG(LDO4, table, LTC3589_OVEN_LDO4, LTC3589_L2DTV2, 0x60, 0),
+};
+
+#ifdef CONFIG_OF
+static struct of_regulator_match ltc3589_matches[LTC3589_NUM_REGULATORS] = {
+	{ .name = "sw1",    },
+	{ .name = "sw2",    },
+	{ .name = "sw3",    },
+	{ .name = "bb-out", },
+	{ .name = "ldo1",   }, /* standby */
+	{ .name = "ldo2",   },
+	{ .name = "ldo3",   },
+	{ .name = "ldo4",   },
+};
+
+static int ltc3589_parse_regulators_dt(struct ltc3589 *ltc3589)
+{
+	struct device *dev = ltc3589->dev;
+	struct device_node *node;
+	int i, ret;
+
+	node = of_find_node_by_name(dev->of_node, "regulators");
+	if (!node) {
+		dev_err(dev, "regulators node not found\n");
+		return -EINVAL;
+	}
+
+	ret = of_regulator_match(dev, node, ltc3589_matches,
+				 ARRAY_SIZE(ltc3589_matches));
+	of_node_put(node);
+	if (ret < 0) {
+		dev_err(dev, "Error parsing regulator init data: %d\n", ret);
+		return ret;
+	}
+	if (ret != LTC3589_NUM_REGULATORS) {
+		dev_err(dev, "Only %d regulators described in device tree\n",
+			ret);
+		return -EINVAL;
+	}
+
+	/* Parse feedback voltage dividers. LDO3 and LDO4 don't have them */
+	for (i = 0; i < LTC3589_LDO3; i++) {
+		struct ltc3589_regulator *desc = &ltc3589->regulator_descs[i];
+		struct device_node *np = ltc3589_matches[i].of_node;
+		u32 vdiv[2];
+
+		ret = of_property_read_u32_array(np, "lltc,fb-voltage-divider",
+						 vdiv, 2);
+		if (ret) {
+			dev_err(dev, "Failed to parse voltage divider: %d\n",
+				ret);
+			return ret;
+		}
+
+		desc->r1 = vdiv[0];
+		desc->r2 = vdiv[1];
+	}
+
+	return 0;
+}
+
+static inline struct regulator_init_data *match_init_data(int index)
+{
+	return ltc3589_matches[index].init_data;
+}
+
+static inline struct device_node *match_of_node(int index)
+{
+	return ltc3589_matches[index].of_node;
+}
+#else
+static inline int ltc3589_parse_regulators_dt(struct ltc3589 *ltc3589)
+{
+	return 0;
+}
+
+static inline struct regulator_init_data *match_init_data(int index)
+{
+	return NULL;
+}
+
+static inline struct device_node *match_of_node(int index)
+{
+	return NULL;
+}
+#endif
+
+static bool ltc3589_writeable_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case LTC3589_IRQSTAT:
+	case LTC3589_SCR1:
+	case LTC3589_OVEN:
+	case LTC3589_SCR2:
+	case LTC3589_VCCR:
+	case LTC3589_CLIRQ:
+	case LTC3589_B1DTV1:
+	case LTC3589_B1DTV2:
+	case LTC3589_VRRCR:
+	case LTC3589_B2DTV1:
+	case LTC3589_B2DTV2:
+	case LTC3589_B3DTV1:
+	case LTC3589_B3DTV2:
+	case LTC3589_L2DTV1:
+	case LTC3589_L2DTV2:
+		return true;
+	}
+	return false;
+}
+
+static bool ltc3589_readable_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case LTC3589_IRQSTAT:
+	case LTC3589_SCR1:
+	case LTC3589_OVEN:
+	case LTC3589_SCR2:
+	case LTC3589_PGSTAT:
+	case LTC3589_VCCR:
+	case LTC3589_B1DTV1:
+	case LTC3589_B1DTV2:
+	case LTC3589_VRRCR:
+	case LTC3589_B2DTV1:
+	case LTC3589_B2DTV2:
+	case LTC3589_B3DTV1:
+	case LTC3589_B3DTV2:
+	case LTC3589_L2DTV1:
+	case LTC3589_L2DTV2:
+		return true;
+	}
+	return false;
+}
+
+static bool ltc3589_volatile_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case LTC3589_IRQSTAT:
+	case LTC3589_PGSTAT:
+		return true;
+	}
+	return false;
+}
+
+struct reg_default ltc3589_reg_defaults[] = {
+	{ LTC3589_SCR1,   0x00 },
+	{ LTC3589_OVEN,   0x00 },
+	{ LTC3589_SCR2,   0x00 },
+	{ LTC3589_VCCR,   0x00 },
+	{ LTC3589_B1DTV1, 0x19 },
+	{ LTC3589_B1DTV2, 0x19 },
+	{ LTC3589_VRRCR,  0xff },
+	{ LTC3589_B2DTV1, 0x19 },
+	{ LTC3589_B2DTV2, 0x19 },
+	{ LTC3589_B3DTV1, 0x19 },
+	{ LTC3589_B3DTV2, 0x19 },
+	{ LTC3589_L2DTV1, 0x19 },
+	{ LTC3589_L2DTV2, 0x19 },
+};
+
+static const struct regmap_config ltc3589_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.writeable_reg = ltc3589_writeable_reg,
+	.readable_reg = ltc3589_readable_reg,
+	.volatile_reg = ltc3589_volatile_reg,
+	.max_register = LTC3589_L2DTV2,
+	.reg_defaults = ltc3589_reg_defaults,
+	.num_reg_defaults = ARRAY_SIZE(ltc3589_reg_defaults),
+	.use_single_rw = true,
+	.cache_type = REGCACHE_RBTREE,
+};
+
+
+static irqreturn_t ltc3589_isr(int irq, void *dev_id)
+{
+	struct ltc3589 *ltc3589 = dev_id;
+	unsigned int i, irqstat, event;
+
+	regmap_read(ltc3589->regmap, LTC3589_IRQSTAT, &irqstat);
+
+	if (irqstat & LTC3589_IRQSTAT_THERMAL_WARN) {
+		event = REGULATOR_EVENT_OVER_TEMP;
+		for (i = 0; i < LTC3589_NUM_REGULATORS; i++)
+			regulator_notifier_call_chain(ltc3589->regulators[i],
+						      event, NULL);
+	}
+
+	if (irqstat & LTC3589_IRQSTAT_UNDERVOLT_WARN) {
+		event = REGULATOR_EVENT_UNDER_VOLTAGE;
+		for (i = 0; i < LTC3589_NUM_REGULATORS; i++)
+			regulator_notifier_call_chain(ltc3589->regulators[i],
+						      event, NULL);
+	}
+
+	/* Clear warning condition */
+	regmap_write(ltc3589->regmap, LTC3589_CLIRQ, 0);
+
+	return IRQ_HANDLED;
+}
+
+static inline unsigned int ltc3589_scale(unsigned int uV, u32 r1, u32 r2)
+{
+	uint64_t tmp;
+	if (uV == 0)
+		return 0;
+	tmp = (uint64_t)uV * r1;
+	do_div(tmp, r2);
+	return uV + (unsigned int)tmp;
+}
+
+static void ltc3589_apply_fb_voltage_divider(struct ltc3589_regulator *rdesc)
+{
+	struct regulator_desc *desc = &rdesc->desc;
+
+	if (!rdesc->r1 || !rdesc->r2)
+		return;
+
+	desc->min_uV = ltc3589_scale(desc->min_uV, rdesc->r1, rdesc->r2);
+	desc->uV_step = ltc3589_scale(desc->uV_step, rdesc->r1, rdesc->r2);
+	desc->fixed_uV = ltc3589_scale(desc->fixed_uV, rdesc->r1, rdesc->r2);
+}
+
+static int ltc3589_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct device *dev = &client->dev;
+	struct ltc3589_regulator *descs;
+	struct ltc3589 *ltc3589;
+	int i, ret;
+
+	ltc3589 = devm_kzalloc(dev, sizeof(*ltc3589), GFP_KERNEL);
+	if (!ltc3589)
+		return -ENOMEM;
+
+	i2c_set_clientdata(client, ltc3589);
+	ltc3589->variant = id->driver_data;
+	ltc3589->dev = dev;
+
+	descs = ltc3589->regulator_descs;
+	memcpy(descs, ltc3589_regulators, sizeof(ltc3589_regulators));
+	if (ltc3589->variant == LTC3589) {
+		descs[LTC3589_LDO3].desc.fixed_uV = 1800000;
+		descs[LTC3589_LDO4].desc.volt_table = ltc3589_ldo4;
+	} else {
+		descs[LTC3589_LDO3].desc.fixed_uV = 2800000;
+		descs[LTC3589_LDO4].desc.volt_table = ltc3589_12_ldo4;
+	}
+
+	ltc3589->regmap = devm_regmap_init_i2c(client, &ltc3589_regmap_config);
+	if (IS_ERR(ltc3589->regmap)) {
+		ret = PTR_ERR(ltc3589->regmap);
+		dev_err(dev, "failed to initialize regmap: %d\n", ret);
+		return ret;
+	}
+
+	ret = ltc3589_parse_regulators_dt(ltc3589);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < LTC3589_NUM_REGULATORS; i++) {
+		struct ltc3589_regulator *rdesc = &ltc3589->regulator_descs[i];
+		struct regulator_desc *desc = &rdesc->desc;
+		struct regulator_init_data *init_data;
+		struct regulator_config config = { };
+
+		init_data = match_init_data(i);
+
+		if (i < LTC3589_LDO3)
+			ltc3589_apply_fb_voltage_divider(rdesc);
+
+		config.dev = dev;
+		config.init_data = init_data;
+		config.driver_data = ltc3589;
+		config.of_node = match_of_node(i);
+
+		ltc3589->regulators[i] = devm_regulator_register(dev, desc,
+								 &config);
+		if (IS_ERR(ltc3589->regulators[i])) {
+			ret = PTR_ERR(ltc3589->regulators[i]);
+			dev_err(dev, "failed to register regulator %s: %d\n",
+				desc->name, ret);
+			return ret;
+		}
+	}
+
+	ret = devm_request_threaded_irq(dev, client->irq, NULL, ltc3589_isr,
+					IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+					client->name, ltc3589);
+	if (ret) {
+		dev_err(dev, "Failed to request IRQ: %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static struct i2c_device_id ltc3589_i2c_id[] = {
+	{ "ltc3589",   LTC3589   },
+	{ "ltc3589-1", LTC3589_1 },
+	{ "ltc3589-2", LTC3589_2 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ltc3589_i2c_id);
+
+static struct i2c_driver ltc3589_driver = {
+	.driver = {
+		.name = DRIVER_NAME,
+		.owner = THIS_MODULE,
+	},
+	.probe = ltc3589_probe,
+	.id_table = ltc3589_i2c_id,
+};
+module_i2c_driver(ltc3589_driver);
+
+MODULE_AUTHOR("Philipp Zabel <p.zabel@pengutronix.de>");
+MODULE_DESCRIPTION("Regulator driver for Linear Technology LTC3589(-1,2)");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("i2c:ltc3589");
diff --git a/drivers/regulator/max14577.c b/drivers/regulator/max14577.c
index ed60baaecee..5d9c605cf53 100644
--- a/drivers/regulator/max14577.c
+++ b/drivers/regulator/max14577.c
@@ -1,5 +1,5 @@
 /*
- * max14577.c - Regulator driver for the Maxim 14577
+ * max14577.c - Regulator driver for the Maxim 14577/77836
  *
  * Copyright (C) 2013,2014 Samsung Electronics
  * Krzysztof Kozlowski <k.kozlowski@samsung.com>
@@ -22,6 +22,42 @@
 #include <linux/mfd/max14577-private.h>
 #include <linux/regulator/of_regulator.h>
 
+/*
+ * Valid limits of current for max14577 and max77836 chargers.
+ * They must correspond to MBCICHWRCL and MBCICHWRCH fields in CHGCTRL4
+ * register for given chipset.
+ */
+struct maxim_charger_current {
+	/* Minimal current, set in CHGCTRL4/MBCICHWRCL, uA */
+	unsigned int min;
+	/*
+	 * Minimal current when high setting is active,
+	 * set in CHGCTRL4/MBCICHWRCH, uA
+	 */
+	unsigned int high_start;
+	/* Value of one step in high setting, uA */
+	unsigned int high_step;
+	/* Maximum current of high setting, uA */
+	unsigned int max;
+};
+
+/* Table of valid charger currents for different Maxim chipsets */
+static const struct maxim_charger_current maxim_charger_currents[] = {
+	[MAXIM_DEVICE_TYPE_UNKNOWN] = { 0, 0, 0, 0 },
+	[MAXIM_DEVICE_TYPE_MAX14577] = {
+		.min		= MAX14577_REGULATOR_CURRENT_LIMIT_MIN,
+		.high_start	= MAX14577_REGULATOR_CURRENT_LIMIT_HIGH_START,
+		.high_step	= MAX14577_REGULATOR_CURRENT_LIMIT_HIGH_STEP,
+		.max		= MAX14577_REGULATOR_CURRENT_LIMIT_MAX,
+	},
+	[MAXIM_DEVICE_TYPE_MAX77836] = {
+		.min		= MAX77836_REGULATOR_CURRENT_LIMIT_MIN,
+		.high_start	= MAX77836_REGULATOR_CURRENT_LIMIT_HIGH_START,
+		.high_step	= MAX77836_REGULATOR_CURRENT_LIMIT_HIGH_STEP,
+		.max		= MAX77836_REGULATOR_CURRENT_LIMIT_MAX,
+	},
+};
+
 static int max14577_reg_is_enabled(struct regulator_dev *rdev)
 {
 	int rid = rdev_get_id(rdev);
@@ -47,6 +83,9 @@ static int max14577_reg_get_current_limit(struct regulator_dev *rdev)
 {
 	u8 reg_data;
 	struct regmap *rmap = rdev->regmap;
+	struct max14577 *max14577 = rdev_get_drvdata(rdev);
+	const struct maxim_charger_current *limits =
+		&maxim_charger_currents[max14577->dev_type];
 
 	if (rdev_get_id(rdev) != MAX14577_CHARGER)
 		return -EINVAL;
@@ -54,12 +93,11 @@ static int max14577_reg_get_current_limit(struct regulator_dev *rdev)
 	max14577_read_reg(rmap, MAX14577_CHG_REG_CHG_CTRL4, &reg_data);
 
 	if ((reg_data & CHGCTRL4_MBCICHWRCL_MASK) == 0)
-		return MAX14577_REGULATOR_CURRENT_LIMIT_MIN;
+		return limits->min;
 
 	reg_data = ((reg_data & CHGCTRL4_MBCICHWRCH_MASK) >>
 			CHGCTRL4_MBCICHWRCH_SHIFT);
-	return MAX14577_REGULATOR_CURRENT_LIMIT_HIGH_START +
-		reg_data * MAX14577_REGULATOR_CURRENT_LIMIT_HIGH_STEP;
+	return limits->high_start + reg_data * limits->high_step;
 }
 
 static int max14577_reg_set_current_limit(struct regulator_dev *rdev,
@@ -67,33 +105,39 @@ static int max14577_reg_set_current_limit(struct regulator_dev *rdev,
 {
 	int i, current_bits = 0xf;
 	u8 reg_data;
+	struct max14577 *max14577 = rdev_get_drvdata(rdev);
+	const struct maxim_charger_current *limits =
+		&maxim_charger_currents[max14577->dev_type];
 
 	if (rdev_get_id(rdev) != MAX14577_CHARGER)
 		return -EINVAL;
 
-	if (min_uA > MAX14577_REGULATOR_CURRENT_LIMIT_MAX ||
-			max_uA < MAX14577_REGULATOR_CURRENT_LIMIT_MIN)
+	if (min_uA > limits->max || max_uA < limits->min)
 		return -EINVAL;
 
-	if (max_uA < MAX14577_REGULATOR_CURRENT_LIMIT_HIGH_START) {
-		/* Less than 200 mA, so set 90mA (turn only Low Bit off) */
+	if (max_uA < limits->high_start) {
+		/*
+		 * Less than high_start,
+		 * so set the minimal current (turn only Low Bit off)
+		 */
 		u8 reg_data = 0x0 << CHGCTRL4_MBCICHWRCL_SHIFT;
 		return max14577_update_reg(rdev->regmap,
 				MAX14577_CHG_REG_CHG_CTRL4,
 				CHGCTRL4_MBCICHWRCL_MASK, reg_data);
 	}
 
-	/* max_uA is in range: <LIMIT_HIGH_START, inifinite>, so search for
-	 * valid current starting from LIMIT_MAX. */
-	for (i = MAX14577_REGULATOR_CURRENT_LIMIT_MAX;
-			i >= MAX14577_REGULATOR_CURRENT_LIMIT_HIGH_START;
-			i -= MAX14577_REGULATOR_CURRENT_LIMIT_HIGH_STEP) {
+	/*
+	 * max_uA is in range: <high_start, inifinite>, so search for
+	 * valid current starting from maximum current.
+	 */
+	for (i = limits->max; i >= limits->high_start; i -= limits->high_step) {
 		if (i <= max_uA)
 			break;
 		current_bits--;
 	}
 	BUG_ON(current_bits < 0); /* Cannot happen */
-	/* Turn Low Bit on (use range 200mA-950 mA) */
+
+	/* Turn Low Bit on (use range high_start-max)... */
 	reg_data = 0x1 << CHGCTRL4_MBCICHWRCL_SHIFT;
 	/* and set proper High Bits */
 	reg_data |= current_bits << CHGCTRL4_MBCICHWRCH_SHIFT;
@@ -118,7 +162,7 @@ static struct regulator_ops max14577_charger_ops = {
 	.set_current_limit	= max14577_reg_set_current_limit,
 };
 
-static const struct regulator_desc supported_regulators[] = {
+static const struct regulator_desc max14577_supported_regulators[] = {
 	[MAX14577_SAFEOUT] = {
 		.name		= "SAFEOUT",
 		.id		= MAX14577_SAFEOUT,
@@ -141,16 +185,88 @@ static const struct regulator_desc supported_regulators[] = {
 	},
 };
 
+static struct regulator_ops max77836_ldo_ops = {
+	.is_enabled		= regulator_is_enabled_regmap,
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
+	.list_voltage		= regulator_list_voltage_linear,
+	.map_voltage		= regulator_map_voltage_linear,
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
+	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
+	/* TODO: add .set_suspend_mode */
+};
+
+static const struct regulator_desc max77836_supported_regulators[] = {
+	[MAX14577_SAFEOUT] = {
+		.name		= "SAFEOUT",
+		.id		= MAX14577_SAFEOUT,
+		.ops		= &max14577_safeout_ops,
+		.type		= REGULATOR_VOLTAGE,
+		.owner		= THIS_MODULE,
+		.n_voltages	= 1,
+		.min_uV		= MAX14577_REGULATOR_SAFEOUT_VOLTAGE,
+		.enable_reg	= MAX14577_REG_CONTROL2,
+		.enable_mask	= CTRL2_SFOUTORD_MASK,
+	},
+	[MAX14577_CHARGER] = {
+		.name		= "CHARGER",
+		.id		= MAX14577_CHARGER,
+		.ops		= &max14577_charger_ops,
+		.type		= REGULATOR_CURRENT,
+		.owner		= THIS_MODULE,
+		.enable_reg	= MAX14577_CHG_REG_CHG_CTRL2,
+		.enable_mask	= CHGCTRL2_MBCHOSTEN_MASK,
+	},
+	[MAX77836_LDO1] = {
+		.name		= "LDO1",
+		.id		= MAX77836_LDO1,
+		.ops		= &max77836_ldo_ops,
+		.type		= REGULATOR_VOLTAGE,
+		.owner		= THIS_MODULE,
+		.n_voltages	= MAX77836_REGULATOR_LDO_VOLTAGE_STEPS_NUM,
+		.min_uV		= MAX77836_REGULATOR_LDO_VOLTAGE_MIN,
+		.uV_step	= MAX77836_REGULATOR_LDO_VOLTAGE_STEP,
+		.enable_reg	= MAX77836_LDO_REG_CNFG1_LDO1,
+		.enable_mask	= MAX77836_CNFG1_LDO_PWRMD_MASK,
+		.vsel_reg	= MAX77836_LDO_REG_CNFG1_LDO1,
+		.vsel_mask	= MAX77836_CNFG1_LDO_TV_MASK,
+	},
+	[MAX77836_LDO2] = {
+		.name		= "LDO2",
+		.id		= MAX77836_LDO2,
+		.ops		= &max77836_ldo_ops,
+		.type		= REGULATOR_VOLTAGE,
+		.owner		= THIS_MODULE,
+		.n_voltages	= MAX77836_REGULATOR_LDO_VOLTAGE_STEPS_NUM,
+		.min_uV		= MAX77836_REGULATOR_LDO_VOLTAGE_MIN,
+		.uV_step	= MAX77836_REGULATOR_LDO_VOLTAGE_STEP,
+		.enable_reg	= MAX77836_LDO_REG_CNFG1_LDO2,
+		.enable_mask	= MAX77836_CNFG1_LDO_PWRMD_MASK,
+		.vsel_reg	= MAX77836_LDO_REG_CNFG1_LDO2,
+		.vsel_mask	= MAX77836_CNFG1_LDO_TV_MASK,
+	},
+};
+
 #ifdef CONFIG_OF
 static struct of_regulator_match max14577_regulator_matches[] = {
 	{ .name	= "SAFEOUT", },
 	{ .name = "CHARGER", },
 };
 
-static int max14577_regulator_dt_parse_pdata(struct platform_device *pdev)
+static struct of_regulator_match max77836_regulator_matches[] = {
+	{ .name	= "SAFEOUT", },
+	{ .name = "CHARGER", },
+	{ .name = "LDO1", },
+	{ .name = "LDO2", },
+};
+
+static int max14577_regulator_dt_parse_pdata(struct platform_device *pdev,
+		enum maxim_device_type dev_type)
 {
 	int ret;
 	struct device_node *np;
+	struct of_regulator_match *regulator_matches;
+	unsigned int regulator_matches_size;
 
 	np = of_get_child_by_name(pdev->dev.parent->of_node, "regulators");
 	if (!np) {
@@ -158,8 +274,19 @@ static int max14577_regulator_dt_parse_pdata(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	ret = of_regulator_match(&pdev->dev, np, max14577_regulator_matches,
-			MAX14577_REG_MAX);
+	switch (dev_type) {
+	case MAXIM_DEVICE_TYPE_MAX77836:
+		regulator_matches = max77836_regulator_matches;
+		regulator_matches_size = ARRAY_SIZE(max77836_regulator_matches);
+		break;
+	case MAXIM_DEVICE_TYPE_MAX14577:
+	default:
+		regulator_matches = max14577_regulator_matches;
+		regulator_matches_size = ARRAY_SIZE(max14577_regulator_matches);
+	}
+
+	ret = of_regulator_match(&pdev->dev, np, regulator_matches,
+			regulator_matches_size);
 	if (ret < 0)
 		dev_err(&pdev->dev, "Error parsing regulator init data: %d\n", ret);
 	else
@@ -170,31 +297,74 @@ static int max14577_regulator_dt_parse_pdata(struct platform_device *pdev)
 	return ret;
 }
 
-static inline struct regulator_init_data *match_init_data(int index)
+static inline struct regulator_init_data *match_init_data(int index,
+		enum maxim_device_type dev_type)
 {
-	return max14577_regulator_matches[index].init_data;
+	switch (dev_type) {
+	case MAXIM_DEVICE_TYPE_MAX77836:
+		return max77836_regulator_matches[index].init_data;
+
+	case MAXIM_DEVICE_TYPE_MAX14577:
+	default:
+		return max14577_regulator_matches[index].init_data;
+	}
 }
 
-static inline struct device_node *match_of_node(int index)
+static inline struct device_node *match_of_node(int index,
+		enum maxim_device_type dev_type)
 {
-	return max14577_regulator_matches[index].of_node;
+	switch (dev_type) {
+	case MAXIM_DEVICE_TYPE_MAX77836:
+		return max77836_regulator_matches[index].of_node;
+
+	case MAXIM_DEVICE_TYPE_MAX14577:
+	default:
+		return max14577_regulator_matches[index].of_node;
+	}
 }
 #else /* CONFIG_OF */
-static int max14577_regulator_dt_parse_pdata(struct platform_device *pdev)
+static int max14577_regulator_dt_parse_pdata(struct platform_device *pdev,
+		enum maxim_device_type dev_type)
 {
 	return 0;
 }
-static inline struct regulator_init_data *match_init_data(int index)
+static inline struct regulator_init_data *match_init_data(int index,
+		enum maxim_device_type dev_type)
 {
 	return NULL;
 }
 
-static inline struct device_node *match_of_node(int index)
+static inline struct device_node *match_of_node(int index,
+		enum maxim_device_type dev_type)
 {
 	return NULL;
 }
 #endif /* CONFIG_OF */
 
+/**
+ * Registers for regulators of max77836 use different I2C slave addresses so
+ * different regmaps must be used for them.
+ *
+ * Returns proper regmap for accessing regulator passed by id.
+ */
+static struct regmap *max14577_get_regmap(struct max14577 *max14577,
+		int reg_id)
+{
+	switch (max14577->dev_type) {
+	case MAXIM_DEVICE_TYPE_MAX77836:
+		switch (reg_id) {
+		case MAX77836_SAFEOUT ... MAX77836_CHARGER:
+			return max14577->regmap;
+		default:
+			/* MAX77836_LDO1 ... MAX77836_LDO2 */
+			return max14577->regmap_pmic;
+		}
+
+	case MAXIM_DEVICE_TYPE_MAX14577:
+	default:
+		return max14577->regmap;
+	}
+}
 
 static int max14577_regulator_probe(struct platform_device *pdev)
 {
@@ -202,15 +372,29 @@ static int max14577_regulator_probe(struct platform_device *pdev)
 	struct max14577_platform_data *pdata = dev_get_platdata(max14577->dev);
 	int i, ret;
 	struct regulator_config config = {};
+	const struct regulator_desc *supported_regulators;
+	unsigned int supported_regulators_size;
+	enum maxim_device_type dev_type = max14577->dev_type;
 
-	ret = max14577_regulator_dt_parse_pdata(pdev);
+	ret = max14577_regulator_dt_parse_pdata(pdev, dev_type);
 	if (ret)
 		return ret;
 
+	switch (dev_type) {
+	case MAXIM_DEVICE_TYPE_MAX77836:
+		supported_regulators = max77836_supported_regulators;
+		supported_regulators_size = ARRAY_SIZE(max77836_supported_regulators);
+		break;
+	case MAXIM_DEVICE_TYPE_MAX14577:
+	default:
+		supported_regulators = max14577_supported_regulators;
+		supported_regulators_size = ARRAY_SIZE(max14577_supported_regulators);
+	}
+
 	config.dev = &pdev->dev;
-	config.regmap = max14577->regmap;
+	config.driver_data = max14577;
 
-	for (i = 0; i < ARRAY_SIZE(supported_regulators); i++) {
+	for (i = 0; i < supported_regulators_size; i++) {
 		struct regulator_dev *regulator;
 		/*
 		 * Index of supported_regulators[] is also the id and must
@@ -220,17 +404,19 @@ static int max14577_regulator_probe(struct platform_device *pdev)
 			config.init_data = pdata->regulators[i].initdata;
 			config.of_node = pdata->regulators[i].of_node;
 		} else {
-			config.init_data = match_init_data(i);
-			config.of_node = match_of_node(i);
+			config.init_data = match_init_data(i, dev_type);
+			config.of_node = match_of_node(i, dev_type);
 		}
+		config.regmap = max14577_get_regmap(max14577,
+				supported_regulators[i].id);
 
 		regulator = devm_regulator_register(&pdev->dev,
 				&supported_regulators[i], &config);
 		if (IS_ERR(regulator)) {
 			ret = PTR_ERR(regulator);
 			dev_err(&pdev->dev,
-					"Regulator init failed for ID %d with error: %d\n",
-					i, ret);
+					"Regulator init failed for %d/%s with error: %d\n",
+					i, supported_regulators[i].name, ret);
 			return ret;
 		}
 	}
@@ -238,20 +424,41 @@ static int max14577_regulator_probe(struct platform_device *pdev)
 	return ret;
 }
 
+static const struct platform_device_id max14577_regulator_id[] = {
+	{ "max14577-regulator", MAXIM_DEVICE_TYPE_MAX14577, },
+	{ "max77836-regulator", MAXIM_DEVICE_TYPE_MAX77836, },
+	{ }
+};
+MODULE_DEVICE_TABLE(platform, max14577_regulator_id);
+
 static struct platform_driver max14577_regulator_driver = {
 	.driver = {
 		   .owner = THIS_MODULE,
 		   .name = "max14577-regulator",
 		   },
-	.probe	= max14577_regulator_probe,
+	.probe		= max14577_regulator_probe,
+	.id_table	= max14577_regulator_id,
 };
 
 static int __init max14577_regulator_init(void)
 {
+	/* Check for valid values for charger */
 	BUILD_BUG_ON(MAX14577_REGULATOR_CURRENT_LIMIT_HIGH_START +
 			MAX14577_REGULATOR_CURRENT_LIMIT_HIGH_STEP * 0xf !=
 			MAX14577_REGULATOR_CURRENT_LIMIT_MAX);
-	BUILD_BUG_ON(ARRAY_SIZE(supported_regulators) != MAX14577_REG_MAX);
+	BUILD_BUG_ON(MAX77836_REGULATOR_CURRENT_LIMIT_HIGH_START +
+			MAX77836_REGULATOR_CURRENT_LIMIT_HIGH_STEP * 0xf !=
+			MAX77836_REGULATOR_CURRENT_LIMIT_MAX);
+	/* Valid charger current values must be provided for each chipset */
+	BUILD_BUG_ON(ARRAY_SIZE(maxim_charger_currents) != MAXIM_DEVICE_TYPE_NUM);
+
+	BUILD_BUG_ON(ARRAY_SIZE(max14577_supported_regulators) != MAX14577_REGULATOR_NUM);
+	BUILD_BUG_ON(ARRAY_SIZE(max77836_supported_regulators) != MAX77836_REGULATOR_NUM);
+
+	BUILD_BUG_ON(MAX77836_REGULATOR_LDO_VOLTAGE_MIN +
+			(MAX77836_REGULATOR_LDO_VOLTAGE_STEP *
+			  (MAX77836_REGULATOR_LDO_VOLTAGE_STEPS_NUM - 1)) !=
+			MAX77836_REGULATOR_LDO_VOLTAGE_MAX);
 
 	return platform_driver_register(&max14577_regulator_driver);
 }
@@ -264,6 +471,6 @@ static void __exit max14577_regulator_exit(void)
 module_exit(max14577_regulator_exit);
 
 MODULE_AUTHOR("Krzysztof Kozlowski <k.kozlowski@samsung.com>");
-MODULE_DESCRIPTION("MAXIM 14577 regulator driver");
+MODULE_DESCRIPTION("Maxim 14577/77836 regulator driver");
 MODULE_LICENSE("GPL");
 MODULE_ALIAS("platform:max14577-regulator");
diff --git a/drivers/regulator/max8649.c b/drivers/regulator/max8649.c
index 3172da847d2..c8bddcc8f91 100644
--- a/drivers/regulator/max8649.c
+++ b/drivers/regulator/max8649.c
@@ -161,10 +161,8 @@ static int max8649_regulator_probe(struct i2c_client *client,
 
 	info = devm_kzalloc(&client->dev, sizeof(struct max8649_regulator_info),
 			    GFP_KERNEL);
-	if (!info) {
-		dev_err(&client->dev, "No enough memory\n");
+	if (!info)
 		return -ENOMEM;
-	}
 
 	info->regmap = devm_regmap_init_i2c(client, &max8649_regmap_config);
 	if (IS_ERR(info->regmap)) {
diff --git a/drivers/regulator/max8952.c b/drivers/regulator/max8952.c
index d920f5a32ec..c2792f0271a 100644
--- a/drivers/regulator/max8952.c
+++ b/drivers/regulator/max8952.c
@@ -129,7 +129,7 @@ static const struct regulator_desc regulator = {
 };
 
 #ifdef CONFIG_OF
-static struct of_device_id max8952_dt_match[] = {
+static const struct of_device_id max8952_dt_match[] = {
 	{ .compatible = "maxim,max8952" },
 	{},
 };
diff --git a/drivers/regulator/of_regulator.c b/drivers/regulator/of_regulator.c
index ea4f36f2cbe..ee5e67bc8d5 100644
--- a/drivers/regulator/of_regulator.c
+++ b/drivers/regulator/of_regulator.c
@@ -19,9 +19,7 @@
 static void of_get_regulation_constraints(struct device_node *np,
 					struct regulator_init_data **init_data)
 {
-	const __be32 *min_uV, *max_uV, *uV_offset;
-	const __be32 *min_uA, *max_uA, *ramp_delay;
-	struct property *prop;
+	const __be32 *min_uV, *max_uV;
 	struct regulation_constraints *constraints = &(*init_data)->constraints;
 	int ret;
 	u32 pval;
@@ -42,36 +40,29 @@ static void of_get_regulation_constraints(struct device_node *np,
 	if (min_uV && max_uV && constraints->min_uV == constraints->max_uV)
 		constraints->apply_uV = true;
 
-	uV_offset = of_get_property(np, "regulator-microvolt-offset", NULL);
-	if (uV_offset)
-		constraints->uV_offset = be32_to_cpu(*uV_offset);
-	min_uA = of_get_property(np, "regulator-min-microamp", NULL);
-	if (min_uA)
-		constraints->min_uA = be32_to_cpu(*min_uA);
-	max_uA = of_get_property(np, "regulator-max-microamp", NULL);
-	if (max_uA)
-		constraints->max_uA = be32_to_cpu(*max_uA);
+	if (!of_property_read_u32(np, "regulator-microvolt-offset", &pval))
+		constraints->uV_offset = pval;
+	if (!of_property_read_u32(np, "regulator-min-microamp", &pval))
+		constraints->min_uA = pval;
+	if (!of_property_read_u32(np, "regulator-max-microamp", &pval))
+		constraints->max_uA = pval;
 
 	/* Current change possible? */
 	if (constraints->min_uA != constraints->max_uA)
 		constraints->valid_ops_mask |= REGULATOR_CHANGE_CURRENT;
 
-	if (of_find_property(np, "regulator-boot-on", NULL))
-		constraints->boot_on = true;
-
-	if (of_find_property(np, "regulator-always-on", NULL))
-		constraints->always_on = true;
-	else /* status change should be possible if not always on. */
+	constraints->boot_on = of_property_read_bool(np, "regulator-boot-on");
+	constraints->always_on = of_property_read_bool(np, "regulator-always-on");
+	if (!constraints->always_on) /* status change should be possible. */
 		constraints->valid_ops_mask |= REGULATOR_CHANGE_STATUS;
 
 	if (of_property_read_bool(np, "regulator-allow-bypass"))
 		constraints->valid_ops_mask |= REGULATOR_CHANGE_BYPASS;
 
-	prop = of_find_property(np, "regulator-ramp-delay", NULL);
-	if (prop && prop->value) {
-		ramp_delay = prop->value;
-		if (*ramp_delay)
-			constraints->ramp_delay = be32_to_cpu(*ramp_delay);
+	ret = of_property_read_u32(np, "regulator-ramp-delay", &pval);
+	if (!ret) {
+		if (pval)
+			constraints->ramp_delay = pval;
 		else
 			constraints->ramp_disable = true;
 	}
@@ -106,6 +97,20 @@ struct regulator_init_data *of_get_regulator_init_data(struct device *dev,
 }
 EXPORT_SYMBOL_GPL(of_get_regulator_init_data);
 
+struct devm_of_regulator_matches {
+	struct of_regulator_match *matches;
+	unsigned int num_matches;
+};
+
+static void devm_of_regulator_put_matches(struct device *dev, void *res)
+{
+	struct devm_of_regulator_matches *devm_matches = res;
+	int i;
+
+	for (i = 0; i < devm_matches->num_matches; i++)
+		of_node_put(devm_matches->matches[i].of_node);
+}
+
 /**
  * of_regulator_match - extract multiple regulator init data from device tree.
  * @dev: device requesting the data
@@ -119,7 +124,8 @@ EXPORT_SYMBOL_GPL(of_get_regulator_init_data);
  * regulator. The data parsed from a child node will be matched to a regulator
  * based on either the deprecated property regulator-compatible if present,
  * or otherwise the child node's name. Note that the match table is modified
- * in place.
+ * in place and an additional of_node reference is taken for each matched
+ * regulator.
  *
  * Returns the number of matches found or a negative error code on failure.
  */
@@ -131,10 +137,22 @@ int of_regulator_match(struct device *dev, struct device_node *node,
 	unsigned int i;
 	const char *name;
 	struct device_node *child;
+	struct devm_of_regulator_matches *devm_matches;
 
 	if (!dev || !node)
 		return -EINVAL;
 
+	devm_matches = devres_alloc(devm_of_regulator_put_matches,
+				    sizeof(struct devm_of_regulator_matches),
+				    GFP_KERNEL);
+	if (!devm_matches)
+		return -ENOMEM;
+
+	devm_matches->matches = matches;
+	devm_matches->num_matches = num_matches;
+
+	devres_add(dev, devm_matches);
+
 	for (i = 0; i < num_matches; i++) {
 		struct of_regulator_match *match = &matches[i];
 		match->init_data = NULL;
@@ -162,7 +180,7 @@ int of_regulator_match(struct device *dev, struct device_node *node,
 					child->name);
 				return -EINVAL;
 			}
-			match->of_node = child;
+			match->of_node = of_node_get(child);
 			count++;
 			break;
 		}
diff --git a/drivers/regulator/palmas-regulator.c b/drivers/regulator/palmas-regulator.c
index 9c62b1d3468..864ed02ce4b 100644
--- a/drivers/regulator/palmas-regulator.c
+++ b/drivers/regulator/palmas-regulator.c
@@ -36,6 +36,18 @@ struct regs_info {
 	int	sleep_id;
 };
 
+static const struct regulator_linear_range smps_low_ranges[] = {
+	REGULATOR_LINEAR_RANGE(500000, 0x1, 0x6, 0),
+	REGULATOR_LINEAR_RANGE(510000, 0x7, 0x79, 10000),
+	REGULATOR_LINEAR_RANGE(1650000, 0x7A, 0x7f, 0),
+};
+
+static const struct regulator_linear_range smps_high_ranges[] = {
+	REGULATOR_LINEAR_RANGE(1000000, 0x1, 0x6, 0),
+	REGULATOR_LINEAR_RANGE(1020000, 0x7, 0x79, 20000),
+	REGULATOR_LINEAR_RANGE(3300000, 0x7A, 0x7f, 0),
+};
+
 static const struct regs_info palmas_regs_info[] = {
 	{
 		.name		= "SMPS12",
@@ -280,54 +292,6 @@ static int palmas_ldo_write(struct palmas *palmas, unsigned int reg,
 	return regmap_write(palmas->regmap[REGULATOR_SLAVE], addr, value);
 }
 
-static int palmas_is_enabled_smps(struct regulator_dev *dev)
-{
-	struct palmas_pmic *pmic = rdev_get_drvdata(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;
-
-	return !!(reg);
-}
-
-static int palmas_enable_smps(struct regulator_dev *dev)
-{
-	struct palmas_pmic *pmic = rdev_get_drvdata(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_MODE_ACTIVE_MASK;
-	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);
-
-	return 0;
-}
-
-static int palmas_disable_smps(struct regulator_dev *dev)
-{
-	struct palmas_pmic *pmic = rdev_get_drvdata(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_MODE_ACTIVE_MASK;
-
-	palmas_smps_write(pmic->palmas, palmas_regs_info[id].ctrl_addr, reg);
-
-	return 0;
-}
-
 static int palmas_set_mode_smps(struct regulator_dev *dev, unsigned int mode)
 {
 	struct palmas_pmic *pmic = rdev_get_drvdata(dev);
@@ -382,81 +346,6 @@ static unsigned int palmas_get_mode_smps(struct regulator_dev *dev)
 	return 0;
 }
 
-static int palmas_list_voltage_smps(struct regulator_dev *dev,
-					unsigned selector)
-{
-	struct palmas_pmic *pmic = rdev_get_drvdata(dev);
-	int id = rdev_get_id(dev);
-	int mult = 1;
-
-	/* Read the multiplier set in VSEL register to return
-	 * the correct voltage.
-	 */
-	if (pmic->range[id])
-		mult = 2;
-
-	if (selector == 0)
-		return 0;
-	else if (selector < 6)
-		return 500000 * mult;
-	else
-		/* Voltage is linear mapping starting from selector 6,
-		 * volt = (0.49V + ((selector - 5) * 0.01V)) * RANGE
-		 * RANGE is either x1 or x2
-		 */
-		return (490000 + ((selector - 5) * 10000)) * mult;
-}
-
-static int palmas_map_voltage_smps(struct regulator_dev *rdev,
-		int min_uV, int max_uV)
-{
-	struct palmas_pmic *pmic = rdev_get_drvdata(rdev);
-	int id = rdev_get_id(rdev);
-	int ret, voltage;
-
-	if (min_uV == 0)
-		return 0;
-
-	if (pmic->range[id]) { /* RANGE is x2 */
-		if (min_uV < 1000000)
-			min_uV = 1000000;
-		ret = DIV_ROUND_UP(min_uV - 1000000, 20000) + 6;
-	} else {		/* RANGE is x1 */
-		if (min_uV < 500000)
-			min_uV = 500000;
-		ret = DIV_ROUND_UP(min_uV - 500000, 10000) + 6;
-	}
-
-	/* Map back into a voltage to verify we're still in bounds */
-	voltage = palmas_list_voltage_smps(rdev, ret);
-	if (voltage < min_uV || voltage > max_uV)
-		return -EINVAL;
-
-	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)
 {
@@ -493,16 +382,16 @@ static int palmas_smps_set_ramp_delay(struct regulator_dev *rdev,
 }
 
 static struct regulator_ops palmas_ops_smps = {
-	.is_enabled		= palmas_is_enabled_smps,
-	.enable			= palmas_enable_smps,
-	.disable		= palmas_disable_smps,
+	.is_enabled		= regulator_is_enabled_regmap,
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
 	.set_mode		= palmas_set_mode_smps,
 	.get_mode		= palmas_get_mode_smps,
 	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
 	.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,
+	.list_voltage		= regulator_list_voltage_linear_range,
+	.map_voltage		= regulator_map_voltage_linear_range,
+	.set_voltage_time_sel	= regulator_set_voltage_time_sel,
 	.set_ramp_delay		= palmas_smps_set_ramp_delay,
 };
 
@@ -511,9 +400,9 @@ static struct regulator_ops palmas_ops_ext_control_smps = {
 	.get_mode		= palmas_get_mode_smps,
 	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
 	.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,
+	.list_voltage		= regulator_list_voltage_linear_range,
+	.map_voltage		= regulator_map_voltage_linear_range,
+	.set_voltage_time_sel	= regulator_set_voltage_time_sel,
 	.set_ramp_delay		= palmas_smps_set_ramp_delay,
 };
 
@@ -1042,12 +931,17 @@ static int palmas_regulators_probe(struct platform_device *pdev)
 			 * ranges. Read the current smps mode for later use.
 			 */
 			addr = palmas_regs_info[id].vsel_addr;
+			pmic->desc[id].n_linear_ranges = 3;
 
 			ret = palmas_smps_read(pmic->palmas, addr, &reg);
 			if (ret)
 				return ret;
 			if (reg & PALMAS_SMPS12_VOLTAGE_RANGE)
 				pmic->range[id] = 1;
+			if (pmic->range[id])
+				pmic->desc[id].linear_ranges = smps_high_ranges;
+			else
+				pmic->desc[id].linear_ranges = smps_low_ranges;
 
 			if (reg_init && reg_init->roof_floor)
 				pmic->desc[id].ops =
@@ -1199,7 +1093,7 @@ static int palmas_regulators_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id of_palmas_match_tbl[] = {
+static const struct of_device_id of_palmas_match_tbl[] = {
 	{ .compatible = "ti,palmas-pmic", },
 	{ .compatible = "ti,twl6035-pmic", },
 	{ .compatible = "ti,twl6036-pmic", },
diff --git a/drivers/regulator/pbias-regulator.c b/drivers/regulator/pbias-regulator.c
index ded3b357420..6d02d68dfb4 100644
--- a/drivers/regulator/pbias-regulator.c
+++ b/drivers/regulator/pbias-regulator.c
@@ -38,85 +38,24 @@ struct pbias_reg_info {
 struct pbias_regulator_data {
 	struct regulator_desc desc;
 	void __iomem *pbias_addr;
-	unsigned int pbias_reg;
 	struct regulator_dev *dev;
 	struct regmap *syscon;
 	const struct pbias_reg_info *info;
 	int voltage;
 };
 
-static int pbias_regulator_set_voltage(struct regulator_dev *dev,
-			int min_uV, int max_uV, unsigned *selector)
-{
-	struct pbias_regulator_data *data = rdev_get_drvdata(dev);
-	const struct pbias_reg_info *info = data->info;
-	int ret, vmode;
-
-	if (min_uV <= 1800000)
-		vmode = 0;
-	else if (min_uV > 1800000)
-		vmode = info->vmode;
-
-	ret = regmap_update_bits(data->syscon, data->pbias_reg,
-						info->vmode, vmode);
-
-	return ret;
-}
-
-static int pbias_regulator_get_voltage(struct regulator_dev *rdev)
-{
-	struct pbias_regulator_data *data = rdev_get_drvdata(rdev);
-	const struct pbias_reg_info *info = data->info;
-	int value, voltage;
-
-	regmap_read(data->syscon, data->pbias_reg, &value);
-	value &= info->vmode;
-
-	voltage = value ? 3000000 : 1800000;
-
-	return voltage;
-}
-
-static int pbias_regulator_enable(struct regulator_dev *rdev)
-{
-	struct pbias_regulator_data *data = rdev_get_drvdata(rdev);
-	const struct pbias_reg_info *info = data->info;
-	int ret;
-
-	ret = regmap_update_bits(data->syscon, data->pbias_reg,
-					info->enable_mask, info->enable);
-
-	return ret;
-}
-
-static int pbias_regulator_disable(struct regulator_dev *rdev)
-{
-	struct pbias_regulator_data *data = rdev_get_drvdata(rdev);
-	const struct pbias_reg_info *info = data->info;
-	int ret;
-
-	ret = regmap_update_bits(data->syscon, data->pbias_reg,
-						info->enable_mask, 0);
-	return ret;
-}
-
-static int pbias_regulator_is_enable(struct regulator_dev *rdev)
-{
-	struct pbias_regulator_data *data = rdev_get_drvdata(rdev);
-	const struct pbias_reg_info *info = data->info;
-	int value;
-
-	regmap_read(data->syscon, data->pbias_reg, &value);
-
-	return (value & info->enable_mask) == info->enable_mask;
-}
+static const unsigned int pbias_volt_table[] = {
+	1800000,
+	3000000
+};
 
 static struct regulator_ops pbias_regulator_voltage_ops = {
-	.set_voltage	= pbias_regulator_set_voltage,
-	.get_voltage	= pbias_regulator_get_voltage,
-	.enable		= pbias_regulator_enable,
-	.disable	= pbias_regulator_disable,
-	.is_enabled	= pbias_regulator_is_enable,
+	.list_voltage = regulator_list_voltage_table,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
 };
 
 static const struct pbias_reg_info pbias_mmc_omap2430 = {
@@ -183,15 +122,14 @@ static int pbias_regulator_probe(struct platform_device *pdev)
 
 	drvdata = devm_kzalloc(&pdev->dev, sizeof(struct pbias_regulator_data)
 			       * count, GFP_KERNEL);
-	if (drvdata == NULL) {
-		dev_err(&pdev->dev, "Failed to allocate device data\n");
+	if (!drvdata)
 		return -ENOMEM;
-	}
 
 	syscon = syscon_regmap_lookup_by_phandle(np, "syscon");
 	if (IS_ERR(syscon))
 		return PTR_ERR(syscon);
 
+	cfg.regmap = syscon;
 	cfg.dev = &pdev->dev;
 
 	for (idx = 0; idx < PBIAS_NUM_REGS && data_idx < count; idx++) {
@@ -207,15 +145,20 @@ static int pbias_regulator_probe(struct platform_device *pdev)
 		if (!res)
 			return -EINVAL;
 
-		drvdata[data_idx].pbias_reg = res->start;
 		drvdata[data_idx].syscon = syscon;
 		drvdata[data_idx].info = info;
 		drvdata[data_idx].desc.name = info->name;
 		drvdata[data_idx].desc.owner = THIS_MODULE;
 		drvdata[data_idx].desc.type = REGULATOR_VOLTAGE;
 		drvdata[data_idx].desc.ops = &pbias_regulator_voltage_ops;
+		drvdata[data_idx].desc.volt_table = pbias_volt_table;
 		drvdata[data_idx].desc.n_voltages = 2;
 		drvdata[data_idx].desc.enable_time = info->enable_time;
+		drvdata[data_idx].desc.vsel_reg = res->start;
+		drvdata[data_idx].desc.vsel_mask = info->vmode;
+		drvdata[data_idx].desc.enable_reg = res->start;
+		drvdata[data_idx].desc.enable_mask = info->enable_mask;
+		drvdata[data_idx].desc.enable_val = info->enable;
 
 		cfg.init_data = pbias_matches[idx].init_data;
 		cfg.driver_data = &drvdata[data_idx];
diff --git a/drivers/regulator/pfuze100-regulator.c b/drivers/regulator/pfuze100-regulator.c
index 67e678c4301..c879dff597e 100644
--- a/drivers/regulator/pfuze100-regulator.c
+++ b/drivers/regulator/pfuze100-regulator.c
@@ -125,6 +125,9 @@ static struct regulator_ops pfuze100_ldo_regulator_ops = {
 };
 
 static struct regulator_ops pfuze100_fixed_regulator_ops = {
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
 	.list_voltage = regulator_list_voltage_linear,
 };
 
@@ -137,6 +140,8 @@ static struct regulator_ops pfuze100_sw_regulator_ops = {
 };
 
 static struct regulator_ops pfuze100_swb_regulator_ops = {
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
 	.list_voltage = regulator_list_voltage_table,
 	.map_voltage = regulator_map_voltage_ascend,
 	.set_voltage_sel = regulator_set_voltage_sel_regmap,
@@ -189,6 +194,8 @@ static struct regulator_ops pfuze100_swb_regulator_ops = {
 			.volt_table = voltages,	\
 			.vsel_reg = (base),	\
 			.vsel_mask = (mask),	\
+			.enable_reg = (base),	\
+			.enable_mask = 0x48,	\
 		},	\
 	}
 
@@ -502,6 +509,7 @@ static int pfuze100_regulator_probe(struct i2c_client *client,
 		config.init_data = init_data;
 		config.driver_data = pfuze_chip;
 		config.of_node = match_of_node(i);
+		config.ena_gpio = -EINVAL;
 
 		pfuze_chip->regulators[i] =
 			devm_regulator_register(&client->dev, desc, &config);
diff --git a/drivers/regulator/s2mpa01.c b/drivers/regulator/s2mpa01.c
index f19a30f0fb4..ee83b487642 100644
--- a/drivers/regulator/s2mpa01.c
+++ b/drivers/regulator/s2mpa01.c
@@ -61,7 +61,7 @@ static int s2mpa01_regulator_set_voltage_time_sel(struct regulator_dev *rdev,
 	unsigned int ramp_delay = 0;
 	int old_volt, new_volt;
 
-	switch (rdev->desc->id) {
+	switch (rdev_get_id(rdev)) {
 	case S2MPA01_BUCK2:
 	case S2MPA01_BUCK4:
 		ramp_delay = s2mpa01->ramp_delay24;
@@ -102,7 +102,7 @@ static int s2mpa01_set_ramp_delay(struct regulator_dev *rdev, int ramp_delay)
 	unsigned int ramp_enable = 1, enable_shift = 0;
 	int ret;
 
-	switch (rdev->desc->id) {
+	switch (rdev_get_id(rdev)) {
 	case S2MPA01_BUCK1:
 		enable_shift = S2MPA01_BUCK1_RAMP_EN_SHIFT;
 		if (!ramp_delay) {
@@ -116,7 +116,6 @@ static int s2mpa01_set_ramp_delay(struct regulator_dev *rdev, int ramp_delay)
 			ramp_delay = s2mpa01->ramp_delay16;
 
 		ramp_shift = S2MPA01_BUCK16_RAMP_SHIFT;
-		ramp_reg = S2MPA01_REG_RAMP1;
 		break;
 	case S2MPA01_BUCK2:
 		enable_shift = S2MPA01_BUCK2_RAMP_EN_SHIFT;
@@ -192,11 +191,15 @@ static int s2mpa01_set_ramp_delay(struct regulator_dev *rdev, int ramp_delay)
 	if (!ramp_enable)
 		goto ramp_disable;
 
-	ret = regmap_update_bits(rdev->regmap, S2MPA01_REG_RAMP1,
-				 1 << enable_shift, 1 << enable_shift);
-	if (ret) {
-		dev_err(&rdev->dev, "failed to enable ramp rate\n");
-		return ret;
+	/* Ramp delay can be enabled/disabled only for buck[1234] */
+	if (rdev_get_id(rdev) >= S2MPA01_BUCK1 &&
+			rdev_get_id(rdev) <= S2MPA01_BUCK4) {
+		ret = regmap_update_bits(rdev->regmap, S2MPA01_REG_RAMP1,
+					 1 << enable_shift, 1 << enable_shift);
+		if (ret) {
+			dev_err(&rdev->dev, "failed to enable ramp rate\n");
+			return ret;
+		}
 	}
 
 	ramp_val = get_ramp_delay(ramp_delay);
diff --git a/drivers/regulator/s2mps11.c b/drivers/regulator/s2mps11.c
index e713c162fbd..02e2fb2fca6 100644
--- a/drivers/regulator/s2mps11.c
+++ b/drivers/regulator/s2mps11.c
@@ -27,6 +27,7 @@
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
 #include <linux/regulator/of_regulator.h>
+#include <linux/of_gpio.h>
 #include <linux/mfd/samsung/core.h>
 #include <linux/mfd/samsung/s2mps11.h>
 #include <linux/mfd/samsung/s2mps14.h>
@@ -44,6 +45,8 @@ struct s2mps11_info {
 	 * was enabled.
 	 */
 	unsigned int s2mps14_suspend_state:30;
+	/* Array of size rdev_num with GPIO-s for external sleep control */
+	int *ext_control_gpio;
 };
 
 static int get_ramp_delay(int ramp_delay)
@@ -202,11 +205,16 @@ static int s2mps11_set_ramp_delay(struct regulator_dev *rdev, int ramp_delay)
 	if (!ramp_enable)
 		goto ramp_disable;
 
-	ret = regmap_update_bits(rdev->regmap, S2MPS11_REG_RAMP,
-				 1 << enable_shift, 1 << enable_shift);
-	if (ret) {
-		dev_err(&rdev->dev, "failed to enable ramp rate\n");
-		return ret;
+	/* Ramp delay can be enabled/disabled only for buck[2346] */
+	if ((rdev_get_id(rdev) >= S2MPS11_BUCK2 &&
+			rdev_get_id(rdev) <= S2MPS11_BUCK4) ||
+			rdev_get_id(rdev) == S2MPS11_BUCK6)  {
+		ret = regmap_update_bits(rdev->regmap, S2MPS11_REG_RAMP,
+					 1 << enable_shift, 1 << enable_shift);
+		if (ret) {
+			dev_err(&rdev->dev, "failed to enable ramp rate\n");
+			return ret;
+		}
 	}
 
 	ramp_val = get_ramp_delay(ramp_delay);
@@ -409,6 +417,8 @@ static int s2mps14_regulator_enable(struct regulator_dev *rdev)
 
 	if (s2mps11->s2mps14_suspend_state & (1 << rdev_get_id(rdev)))
 		val = S2MPS14_ENABLE_SUSPEND;
+	else if (gpio_is_valid(s2mps11->ext_control_gpio[rdev_get_id(rdev)]))
+		val = S2MPS14_ENABLE_EXT_CONTROL;
 	else
 		val = rdev->desc->enable_mask;
 
@@ -565,12 +575,61 @@ static const struct regulator_desc s2mps14_regulators[] = {
 	regulator_desc_s2mps14_buck1235(5),
 };
 
+static int s2mps14_pmic_enable_ext_control(struct s2mps11_info *s2mps11,
+		struct regulator_dev *rdev)
+{
+	return regmap_update_bits(rdev->regmap, rdev->desc->enable_reg,
+			rdev->desc->enable_mask, S2MPS14_ENABLE_EXT_CONTROL);
+}
+
+static void s2mps14_pmic_dt_parse_ext_control_gpio(struct platform_device *pdev,
+		struct of_regulator_match *rdata, struct s2mps11_info *s2mps11)
+{
+	int *gpio = s2mps11->ext_control_gpio;
+	unsigned int i;
+	unsigned int valid_regulators[3] = { S2MPS14_LDO10, S2MPS14_LDO11,
+		S2MPS14_LDO12 };
+
+	for (i = 0; i < ARRAY_SIZE(valid_regulators); i++) {
+		unsigned int reg = valid_regulators[i];
+
+		if (!rdata[reg].init_data || !rdata[reg].of_node)
+			continue;
+
+		gpio[reg] = of_get_named_gpio(rdata[reg].of_node,
+				"samsung,ext-control-gpios", 0);
+		if (gpio_is_valid(gpio[reg]))
+			dev_dbg(&pdev->dev, "Using GPIO %d for ext-control over %d/%s\n",
+					gpio[reg], reg, rdata[reg].name);
+	}
+}
+
+static int s2mps11_pmic_dt_parse(struct platform_device *pdev,
+		struct of_regulator_match *rdata, struct s2mps11_info *s2mps11,
+		enum sec_device_type dev_type)
+{
+	struct device_node *reg_np;
+
+	reg_np = of_get_child_by_name(pdev->dev.parent->of_node, "regulators");
+	if (!reg_np) {
+		dev_err(&pdev->dev, "could not find regulators sub-node\n");
+		return -EINVAL;
+	}
+
+	of_regulator_match(&pdev->dev, reg_np, rdata, s2mps11->rdev_num);
+	if (dev_type == S2MPS14X)
+		s2mps14_pmic_dt_parse_ext_control_gpio(pdev, rdata, s2mps11);
+
+	of_node_put(reg_np);
+
+	return 0;
+}
+
 static int s2mps11_pmic_probe(struct platform_device *pdev)
 {
 	struct sec_pmic_dev *iodev = dev_get_drvdata(pdev->dev.parent);
-	struct sec_platform_data *pdata = iodev->pdata;
+	struct sec_platform_data *pdata = NULL;
 	struct of_regulator_match *rdata = NULL;
-	struct device_node *reg_np = NULL;
 	struct regulator_config config = { };
 	struct s2mps11_info *s2mps11;
 	int i, ret = 0;
@@ -597,8 +656,21 @@ static int s2mps11_pmic_probe(struct platform_device *pdev)
 		return -EINVAL;
 	};
 
+	s2mps11->ext_control_gpio = devm_kzalloc(&pdev->dev,
+			sizeof(*s2mps11->ext_control_gpio) * s2mps11->rdev_num,
+			GFP_KERNEL);
+	if (!s2mps11->ext_control_gpio)
+		return -ENOMEM;
+	/*
+	 * 0 is a valid GPIO so initialize all GPIO-s to negative value
+	 * to indicate that external control won't be used for this regulator.
+	 */
+	for (i = 0; i < s2mps11->rdev_num; i++)
+		s2mps11->ext_control_gpio[i] = -EINVAL;
+
 	if (!iodev->dev->of_node) {
-		if (pdata) {
+		if (iodev->pdata) {
+			pdata = iodev->pdata;
 			goto common_reg;
 		} else {
 			dev_err(pdev->dev.parent,
@@ -614,15 +686,9 @@ static int s2mps11_pmic_probe(struct platform_device *pdev)
 	for (i = 0; i < s2mps11->rdev_num; i++)
 		rdata[i].name = regulators[i].name;
 
-	reg_np = of_get_child_by_name(iodev->dev->of_node, "regulators");
-	if (!reg_np) {
-		dev_err(&pdev->dev, "could not find regulators sub-node\n");
-		ret = -EINVAL;
+	ret = s2mps11_pmic_dt_parse(pdev, rdata, s2mps11, dev_type);
+	if (ret)
 		goto out;
-	}
-
-	of_regulator_match(&pdev->dev, reg_np, rdata, s2mps11->rdev_num);
-	of_node_put(reg_np);
 
 common_reg:
 	platform_set_drvdata(pdev, s2mps11);
@@ -630,16 +696,18 @@ common_reg:
 	config.dev = &pdev->dev;
 	config.regmap = iodev->regmap_pmic;
 	config.driver_data = s2mps11;
+	config.ena_gpio_flags = GPIOF_OUT_INIT_HIGH;
 	for (i = 0; i < s2mps11->rdev_num; i++) {
 		struct regulator_dev *regulator;
 
-		if (!reg_np) {
+		if (pdata) {
 			config.init_data = pdata->regulators[i].initdata;
 			config.of_node = pdata->regulators[i].reg_node;
 		} else {
 			config.init_data = rdata[i].init_data;
 			config.of_node = rdata[i].of_node;
 		}
+		config.ena_gpio = s2mps11->ext_control_gpio[i];
 
 		regulator = devm_regulator_register(&pdev->dev,
 						&regulators[i], &config);
@@ -649,6 +717,17 @@ common_reg:
 				i);
 			goto out;
 		}
+
+		if (gpio_is_valid(s2mps11->ext_control_gpio[i])) {
+			ret = s2mps14_pmic_enable_ext_control(s2mps11,
+					regulator);
+			if (ret < 0) {
+				dev_err(&pdev->dev,
+						"failed to enable GPIO control over %s: %d\n",
+						regulator->desc->name, ret);
+				goto out;
+			}
+		}
 	}
 
 out:
diff --git a/drivers/regulator/s5m8767.c b/drivers/regulator/s5m8767.c
index 92f19a005dc..c79af943a5c 100644
--- a/drivers/regulator/s5m8767.c
+++ b/drivers/regulator/s5m8767.c
@@ -28,7 +28,6 @@ struct s5m8767_info {
 	struct device *dev;
 	struct sec_pmic_dev *iodev;
 	int num_regulators;
-	struct regulator_dev **rdev;
 	struct sec_opmode_data *opmode;
 
 	int ramp_delay;
@@ -529,16 +528,6 @@ static int s5m8767_pmic_dt_parse_ds_gpio(struct sec_pmic_dev *iodev,
 	return 0;
 }
 
-static void s5m8767_pmic_dt_parse_ext_control_gpio(struct sec_pmic_dev *iodev,
-		struct sec_regulator_data *rdata,
-		struct device_node *reg_np)
-{
-	rdata->ext_control_gpio = of_get_named_gpio(reg_np,
-			"s5m8767,pmic-ext-control-gpios", 0);
-	if (!gpio_is_valid(rdata->ext_control_gpio))
-		rdata->ext_control_gpio = 0;
-}
-
 static int s5m8767_pmic_dt_parse_pdata(struct platform_device *pdev,
 					struct sec_platform_data *pdata)
 {
@@ -587,7 +576,8 @@ static int s5m8767_pmic_dt_parse_pdata(struct platform_device *pdev,
 			continue;
 		}
 
-		s5m8767_pmic_dt_parse_ext_control_gpio(iodev, rdata, reg_np);
+		rdata->ext_control_gpio = of_get_named_gpio(reg_np,
+			"s5m8767,pmic-ext-control-gpios", 0);
 
 		rdata->id = i;
 		rdata->initdata = of_get_regulator_init_data(
@@ -695,7 +685,6 @@ static int s5m8767_pmic_probe(struct platform_device *pdev)
 	struct sec_pmic_dev *iodev = dev_get_drvdata(pdev->dev.parent);
 	struct sec_platform_data *pdata = iodev->pdata;
 	struct regulator_config config = { };
-	struct regulator_dev **rdev;
 	struct s5m8767_info *s5m8767;
 	int i, ret, size, buck_init;
 
@@ -737,11 +726,7 @@ static int s5m8767_pmic_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	size = sizeof(struct regulator_dev *) * (S5M8767_REG_MAX - 2);
-	s5m8767->rdev = devm_kzalloc(&pdev->dev, size, GFP_KERNEL);
-	if (!s5m8767->rdev)
-		return -ENOMEM;
 
-	rdev = s5m8767->rdev;
 	s5m8767->dev = &pdev->dev;
 	s5m8767->iodev = iodev;
 	s5m8767->num_regulators = pdata->num_regulators;
@@ -938,6 +923,7 @@ static int s5m8767_pmic_probe(struct platform_device *pdev)
 		const struct sec_voltage_desc *desc;
 		int id = pdata->regulators[i].id;
 		int enable_reg, enable_val;
+		struct regulator_dev *rdev;
 
 		desc = reg_voltage_map[id];
 		if (desc) {
@@ -964,26 +950,27 @@ static int s5m8767_pmic_probe(struct platform_device *pdev)
 		config.driver_data = s5m8767;
 		config.regmap = iodev->regmap_pmic;
 		config.of_node = pdata->regulators[i].reg_node;
-		config.ena_gpio = config.ena_gpio_flags = 0;
-		if (pdata->regulators[i].ext_control_gpio)
+		config.ena_gpio = -EINVAL;
+		config.ena_gpio_flags = 0;
+		if (gpio_is_valid(pdata->regulators[i].ext_control_gpio))
 			s5m8767_regulator_config_ext_control(s5m8767,
 					&pdata->regulators[i], &config);
 
-		rdev[i] = devm_regulator_register(&pdev->dev, &regulators[id],
+		rdev = devm_regulator_register(&pdev->dev, &regulators[id],
 						  &config);
-		if (IS_ERR(rdev[i])) {
-			ret = PTR_ERR(rdev[i]);
+		if (IS_ERR(rdev)) {
+			ret = PTR_ERR(rdev);
 			dev_err(s5m8767->dev, "regulator init failed for %d\n",
 					id);
 			return ret;
 		}
 
-		if (pdata->regulators[i].ext_control_gpio) {
-			ret = s5m8767_enable_ext_control(s5m8767, rdev[i]);
+		if (gpio_is_valid(pdata->regulators[i].ext_control_gpio)) {
+			ret = s5m8767_enable_ext_control(s5m8767, rdev);
 			if (ret < 0) {
 				dev_err(s5m8767->dev,
 						"failed to enable gpio control over %s: %d\n",
-						rdev[i]->desc->name, ret);
+						rdev->desc->name, ret);
 				return ret;
 			}
 		}
diff --git a/drivers/regulator/st-pwm.c b/drivers/regulator/st-pwm.c
index e367af1c5f9..5ea78df449f 100644
--- a/drivers/regulator/st-pwm.c
+++ b/drivers/regulator/st-pwm.c
@@ -118,7 +118,7 @@ static const struct st_pwm_regulator_pdata b2105_info = {
 	.duty_cycle_table = b2105_duty_cycle_table,
 };
 
-static struct of_device_id st_pwm_of_match[] = {
+static const struct of_device_id st_pwm_of_match[] = {
 	{ .compatible = "st,b2105-pwm-regulator", .data = &b2105_info, },
 	{ },
 };
diff --git a/drivers/regulator/tps65090-regulator.c b/drivers/regulator/tps65090-regulator.c
index 2e92ef68574..2064b3fd45f 100644
--- a/drivers/regulator/tps65090-regulator.c
+++ b/drivers/regulator/tps65090-regulator.c
@@ -17,6 +17,7 @@
  */
 
 #include <linux/module.h>
+#include <linux/delay.h>
 #include <linux/init.h>
 #include <linux/gpio.h>
 #include <linux/of_gpio.h>
@@ -28,49 +29,216 @@
 #include <linux/regulator/of_regulator.h>
 #include <linux/mfd/tps65090.h>
 
+#define MAX_CTRL_READ_TRIES	5
+#define MAX_FET_ENABLE_TRIES	1000
+
+#define CTRL_EN_BIT		0 /* Regulator enable bit, active high */
+#define CTRL_WT_BIT		2 /* Regulator wait time 0 bit */
+#define CTRL_PG_BIT		4 /* Regulator power good bit, 1=good */
+#define CTRL_TO_BIT		7 /* Regulator timeout bit, 1=wait */
+
+#define MAX_OVERCURRENT_WAIT	3 /* Overcurrent wait must be <= this */
+
+/**
+ * struct tps65090_regulator - Per-regulator data for a tps65090 regulator
+ *
+ * @dev: Pointer to our device.
+ * @desc: The struct regulator_desc for the regulator.
+ * @rdev: The struct regulator_dev for the regulator.
+ * @overcurrent_wait_valid: True if overcurrent_wait is valid.
+ * @overcurrent_wait: For FETs, the value to put in the WTFET bitfield.
+ */
+
 struct tps65090_regulator {
 	struct device		*dev;
 	struct regulator_desc	*desc;
 	struct regulator_dev	*rdev;
+	bool			overcurrent_wait_valid;
+	int			overcurrent_wait;
 };
 
 static struct regulator_ops tps65090_ext_control_ops = {
 };
 
-static struct regulator_ops tps65090_reg_contol_ops = {
+/**
+ * tps65090_reg_set_overcurrent_wait - Setup overcurrent wait
+ *
+ * This will set the overcurrent wait time based on what's in the regulator
+ * info.
+ *
+ * @ri:		Overall regulator data
+ * @rdev:	Regulator device
+ *
+ * Return: 0 if no error, non-zero if there was an error writing the register.
+ */
+static int tps65090_reg_set_overcurrent_wait(struct tps65090_regulator *ri,
+					     struct regulator_dev *rdev)
+{
+	int ret;
+
+	ret = regmap_update_bits(rdev->regmap, rdev->desc->enable_reg,
+				 MAX_OVERCURRENT_WAIT << CTRL_WT_BIT,
+				 ri->overcurrent_wait << CTRL_WT_BIT);
+	if (ret) {
+		dev_err(&rdev->dev, "Error updating overcurrent wait %#x\n",
+			rdev->desc->enable_reg);
+	}
+
+	return ret;
+}
+
+/**
+ * tps65090_try_enable_fet - Try to enable a FET
+ *
+ * @rdev:	Regulator device
+ *
+ * Return: 0 if ok, -ENOTRECOVERABLE if the FET power good bit did not get
+ * set, or some other -ve value if another error occurred (e.g. i2c error)
+ */
+static int tps65090_try_enable_fet(struct regulator_dev *rdev)
+{
+	unsigned int control;
+	int ret, i;
+
+	ret = regmap_update_bits(rdev->regmap, rdev->desc->enable_reg,
+				 rdev->desc->enable_mask,
+				 rdev->desc->enable_mask);
+	if (ret < 0) {
+		dev_err(&rdev->dev, "Error in updating reg %#x\n",
+			rdev->desc->enable_reg);
+		return ret;
+	}
+
+	for (i = 0; i < MAX_CTRL_READ_TRIES; i++) {
+		ret = regmap_read(rdev->regmap, rdev->desc->enable_reg,
+				  &control);
+		if (ret < 0)
+			return ret;
+
+		if (!(control & BIT(CTRL_TO_BIT)))
+			break;
+
+		usleep_range(1000, 1500);
+	}
+	if (!(control & BIT(CTRL_PG_BIT)))
+		return -ENOTRECOVERABLE;
+
+	return 0;
+}
+
+/**
+ * tps65090_fet_enable - Enable a FET, trying a few times if it fails
+ *
+ * Some versions of the tps65090 have issues when turning on the FETs.
+ * This function goes through several steps to ensure the best chance of the
+ * FET going on.  Specifically:
+ * - We'll make sure that we bump the "overcurrent wait" to the maximum, which
+ *   increases the chances that we'll turn on properly.
+ * - We'll retry turning the FET on multiple times (turning off in between).
+ *
+ * @rdev:	Regulator device
+ *
+ * Return: 0 if ok, non-zero if it fails.
+ */
+static int tps65090_fet_enable(struct regulator_dev *rdev)
+{
+	int ret, tries;
+
+	/*
+	 * Try enabling multiple times until we succeed since sometimes the
+	 * first try times out.
+	 */
+	tries = 0;
+	while (true) {
+		ret = tps65090_try_enable_fet(rdev);
+		if (!ret)
+			break;
+		if (ret != -ENOTRECOVERABLE || tries == MAX_FET_ENABLE_TRIES)
+			goto err;
+
+		/* Try turning the FET off (and then on again) */
+		ret = regmap_update_bits(rdev->regmap, rdev->desc->enable_reg,
+					 rdev->desc->enable_mask, 0);
+		if (ret)
+			goto err;
+
+		tries++;
+	}
+
+	if (tries)
+		dev_warn(&rdev->dev, "reg %#x enable ok after %d tries\n",
+			 rdev->desc->enable_reg, tries);
+
+	return 0;
+err:
+	dev_warn(&rdev->dev, "reg %#x enable failed\n", rdev->desc->enable_reg);
+	WARN_ON(1);
+
+	return ret;
+}
+
+static struct regulator_ops tps65090_reg_control_ops = {
 	.enable		= regulator_enable_regmap,
 	.disable	= regulator_disable_regmap,
 	.is_enabled	= regulator_is_enabled_regmap,
 };
 
+static struct regulator_ops tps65090_fet_control_ops = {
+	.enable		= tps65090_fet_enable,
+	.disable	= regulator_disable_regmap,
+	.is_enabled	= regulator_is_enabled_regmap,
+};
+
 static struct regulator_ops tps65090_ldo_ops = {
 };
 
-#define tps65090_REG_DESC(_id, _sname, _en_reg, _ops)	\
+#define tps65090_REG_DESC(_id, _sname, _en_reg, _en_bits, _ops)	\
 {							\
 	.name = "TPS65090_RAILS"#_id,			\
 	.supply_name = _sname,				\
 	.id = TPS65090_REGULATOR_##_id,			\
 	.ops = &_ops,					\
 	.enable_reg = _en_reg,				\
-	.enable_mask = BIT(0),				\
+	.enable_val = _en_bits,				\
+	.enable_mask = _en_bits,			\
 	.type = REGULATOR_VOLTAGE,			\
 	.owner = THIS_MODULE,				\
 }
 
 static struct regulator_desc tps65090_regulator_desc[] = {
-	tps65090_REG_DESC(DCDC1, "vsys1",   0x0C, tps65090_reg_contol_ops),
-	tps65090_REG_DESC(DCDC2, "vsys2",   0x0D, tps65090_reg_contol_ops),
-	tps65090_REG_DESC(DCDC3, "vsys3",   0x0E, tps65090_reg_contol_ops),
-	tps65090_REG_DESC(FET1,  "infet1",  0x0F, tps65090_reg_contol_ops),
-	tps65090_REG_DESC(FET2,  "infet2",  0x10, tps65090_reg_contol_ops),
-	tps65090_REG_DESC(FET3,  "infet3",  0x11, tps65090_reg_contol_ops),
-	tps65090_REG_DESC(FET4,  "infet4",  0x12, tps65090_reg_contol_ops),
-	tps65090_REG_DESC(FET5,  "infet5",  0x13, tps65090_reg_contol_ops),
-	tps65090_REG_DESC(FET6,  "infet6",  0x14, tps65090_reg_contol_ops),
-	tps65090_REG_DESC(FET7,  "infet7",  0x15, tps65090_reg_contol_ops),
-	tps65090_REG_DESC(LDO1,  "vsys-l1", 0,    tps65090_ldo_ops),
-	tps65090_REG_DESC(LDO2,  "vsys-l2", 0,    tps65090_ldo_ops),
+	tps65090_REG_DESC(DCDC1, "vsys1",   0x0C, BIT(CTRL_EN_BIT),
+			  tps65090_reg_control_ops),
+	tps65090_REG_DESC(DCDC2, "vsys2",   0x0D, BIT(CTRL_EN_BIT),
+			  tps65090_reg_control_ops),
+	tps65090_REG_DESC(DCDC3, "vsys3",   0x0E, BIT(CTRL_EN_BIT),
+			  tps65090_reg_control_ops),
+
+	tps65090_REG_DESC(FET1,  "infet1",  0x0F,
+			  BIT(CTRL_EN_BIT) | BIT(CTRL_PG_BIT),
+			  tps65090_fet_control_ops),
+	tps65090_REG_DESC(FET2,  "infet2",  0x10,
+			  BIT(CTRL_EN_BIT) | BIT(CTRL_PG_BIT),
+			  tps65090_fet_control_ops),
+	tps65090_REG_DESC(FET3,  "infet3",  0x11,
+			  BIT(CTRL_EN_BIT) | BIT(CTRL_PG_BIT),
+			  tps65090_fet_control_ops),
+	tps65090_REG_DESC(FET4,  "infet4",  0x12,
+			  BIT(CTRL_EN_BIT) | BIT(CTRL_PG_BIT),
+			  tps65090_fet_control_ops),
+	tps65090_REG_DESC(FET5,  "infet5",  0x13,
+			  BIT(CTRL_EN_BIT) | BIT(CTRL_PG_BIT),
+			  tps65090_fet_control_ops),
+	tps65090_REG_DESC(FET6,  "infet6",  0x14,
+			  BIT(CTRL_EN_BIT) | BIT(CTRL_PG_BIT),
+			  tps65090_fet_control_ops),
+	tps65090_REG_DESC(FET7,  "infet7",  0x15,
+			  BIT(CTRL_EN_BIT) | BIT(CTRL_PG_BIT),
+			  tps65090_fet_control_ops),
+
+	tps65090_REG_DESC(LDO1,  "vsys-l1", 0, 0,
+			  tps65090_ldo_ops),
+	tps65090_REG_DESC(LDO2,  "vsys-l2", 0, 0,
+			  tps65090_ldo_ops),
 };
 
 static inline bool is_dcdc(int id)
@@ -209,6 +377,11 @@ static struct tps65090_platform_data *tps65090_parse_dt_reg_data(
 			rpdata->gpio = of_get_named_gpio(np,
 					"dcdc-ext-control-gpios", 0);
 
+		if (of_property_read_u32(tps65090_matches[idx].of_node,
+					 "ti,overcurrent-wait",
+					 &rpdata->overcurrent_wait) == 0)
+			rpdata->overcurrent_wait_valid = true;
+
 		tps65090_pdata->reg_pdata[idx] = rpdata;
 	}
 	return tps65090_pdata;
@@ -258,6 +431,11 @@ static int tps65090_regulator_probe(struct platform_device *pdev)
 		ri = &pmic[num];
 		ri->dev = &pdev->dev;
 		ri->desc = &tps65090_regulator_desc[num];
+		if (tps_pdata) {
+			ri->overcurrent_wait_valid =
+				tps_pdata->overcurrent_wait_valid;
+			ri->overcurrent_wait = tps_pdata->overcurrent_wait;
+		}
 
 		/*
 		 * TPS5090 DCDC support the control from external digital input.
@@ -299,6 +477,12 @@ static int tps65090_regulator_probe(struct platform_device *pdev)
 		}
 		ri->rdev = rdev;
 
+		if (ri->overcurrent_wait_valid) {
+			ret = tps65090_reg_set_overcurrent_wait(ri, rdev);
+			if (ret < 0)
+				return ret;
+		}
+
 		/* Enable external control if it is require */
 		if (tps_pdata && is_dcdc(num) && tps_pdata->reg_init_data &&
 				tps_pdata->enable_ext_control) {
diff --git a/drivers/regulator/tps65217-regulator.c b/drivers/regulator/tps65217-regulator.c
index 10b78d2b766..f7ed20a5a8b 100644
--- a/drivers/regulator/tps65217-regulator.c
+++ b/drivers/regulator/tps65217-regulator.c
@@ -134,6 +134,7 @@ static struct regulator_ops tps65217_pmic_ldo1_ops = {
 	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
 	.set_voltage_sel	= tps65217_pmic_set_voltage_sel,
 	.list_voltage		= regulator_list_voltage_table,
+	.map_voltage		= regulator_map_voltage_ascend,
 };
 
 static const struct regulator_desc regulators[] = {
@@ -257,9 +258,6 @@ static int tps65217_regulator_probe(struct platform_device *pdev)
 				pdev->name);
 			return PTR_ERR(rdev);
 		}
-
-		/* Save regulator for cleanup */
-		tps->rdev[i] = rdev;
 	}
 	return 0;
 }
diff --git a/drivers/regulator/tps65218-regulator.c b/drivers/regulator/tps65218-regulator.c
index cec72fa71d1..69b4b775041 100644
--- a/drivers/regulator/tps65218-regulator.c
+++ b/drivers/regulator/tps65218-regulator.c
@@ -27,12 +27,10 @@
 #include <linux/regulator/machine.h>
 #include <linux/mfd/tps65218.h>
 
-static unsigned int tps65218_ramp_delay = 4000;
-
 enum tps65218_regulators { DCDC1, DCDC2, DCDC3, DCDC4, DCDC5, DCDC6, LDO1 };
 
 #define TPS65218_REGULATOR(_name, _id, _ops, _n, _vr, _vm, _er, _em, _t, \
-			    _lr, _nlr)				\
+			    _lr, _nlr, _delay)			\
 	{							\
 		.name			= _name,		\
 		.id			= _id,			\
@@ -47,6 +45,7 @@ enum tps65218_regulators { DCDC1, DCDC2, DCDC3, DCDC4, DCDC5, DCDC6, LDO1 };
 		.volt_table		= _t,			\
 		.linear_ranges		= _lr,			\
 		.n_linear_ranges	= _nlr,			\
+		.ramp_delay		= _delay,		\
 	}							\
 
 #define TPS65218_INFO(_id, _nm, _min, _max)	\
@@ -152,22 +151,6 @@ static int tps65218_pmic_disable(struct regulator_dev *dev)
 				   dev->desc->enable_mask, TPS65218_PROTECT_L1);
 }
 
-static int tps65218_set_voltage_time_sel(struct regulator_dev *rdev,
-	unsigned int old_selector, unsigned int new_selector)
-{
-	int old_uv, new_uv;
-
-	old_uv = regulator_list_voltage_linear_range(rdev, old_selector);
-	if (old_uv < 0)
-		return old_uv;
-
-	new_uv = regulator_list_voltage_linear_range(rdev, new_selector);
-	if (new_uv < 0)
-		return new_uv;
-
-	return DIV_ROUND_UP(abs(old_uv - new_uv), tps65218_ramp_delay);
-}
-
 /* Operations permitted on DCDC1, DCDC2 */
 static struct regulator_ops tps65218_dcdc12_ops = {
 	.is_enabled		= regulator_is_enabled_regmap,
@@ -177,7 +160,7 @@ static struct regulator_ops tps65218_dcdc12_ops = {
 	.set_voltage_sel	= tps65218_pmic_set_voltage_sel,
 	.list_voltage		= regulator_list_voltage_linear_range,
 	.map_voltage		= regulator_map_voltage_linear_range,
-	.set_voltage_time_sel	= tps65218_set_voltage_time_sel,
+	.set_voltage_time_sel	= regulator_set_voltage_time_sel,
 };
 
 /* Operations permitted on DCDC3, DCDC4 and LDO1 */
@@ -203,33 +186,33 @@ static const struct regulator_desc regulators[] = {
 			   TPS65218_REG_CONTROL_DCDC1,
 			   TPS65218_CONTROL_DCDC1_MASK,
 			   TPS65218_REG_ENABLE1, TPS65218_ENABLE1_DC1_EN, NULL,
-			   dcdc1_dcdc2_ranges, 2),
+			   dcdc1_dcdc2_ranges, 2, 4000),
 	TPS65218_REGULATOR("DCDC2", TPS65218_DCDC_2, tps65218_dcdc12_ops, 64,
 			   TPS65218_REG_CONTROL_DCDC2,
 			   TPS65218_CONTROL_DCDC2_MASK,
 			   TPS65218_REG_ENABLE1, TPS65218_ENABLE1_DC2_EN, NULL,
-			   dcdc1_dcdc2_ranges, 2),
+			   dcdc1_dcdc2_ranges, 2, 4000),
 	TPS65218_REGULATOR("DCDC3", TPS65218_DCDC_3, tps65218_ldo1_dcdc34_ops,
 			   64, TPS65218_REG_CONTROL_DCDC3,
 			   TPS65218_CONTROL_DCDC3_MASK, TPS65218_REG_ENABLE1,
 			   TPS65218_ENABLE1_DC3_EN, NULL,
-			   ldo1_dcdc3_ranges, 2),
+			   ldo1_dcdc3_ranges, 2, 0),
 	TPS65218_REGULATOR("DCDC4", TPS65218_DCDC_4, tps65218_ldo1_dcdc34_ops,
 			   53, TPS65218_REG_CONTROL_DCDC4,
 			   TPS65218_CONTROL_DCDC4_MASK,
 			   TPS65218_REG_ENABLE1, TPS65218_ENABLE1_DC4_EN, NULL,
-			   dcdc4_ranges, 2),
+			   dcdc4_ranges, 2, 0),
 	TPS65218_REGULATOR("DCDC5", TPS65218_DCDC_5, tps65218_dcdc56_pmic_ops,
 			   1, -1, -1, TPS65218_REG_ENABLE1,
-			   TPS65218_ENABLE1_DC5_EN, NULL, NULL, 0),
+			   TPS65218_ENABLE1_DC5_EN, NULL, NULL, 0, 0),
 	TPS65218_REGULATOR("DCDC6", TPS65218_DCDC_6, tps65218_dcdc56_pmic_ops,
 			   1, -1, -1, TPS65218_REG_ENABLE1,
-			   TPS65218_ENABLE1_DC6_EN, NULL, NULL, 0),
+			   TPS65218_ENABLE1_DC6_EN, NULL, NULL, 0, 0),
 	TPS65218_REGULATOR("LDO1", TPS65218_LDO_1, tps65218_ldo1_dcdc34_ops, 64,
 			   TPS65218_REG_CONTROL_DCDC4,
 			   TPS65218_CONTROL_LDO1_MASK, TPS65218_REG_ENABLE2,
 			   TPS65218_ENABLE2_LDO1_EN, NULL, ldo1_dcdc3_ranges,
-			   2),
+			   2, 0),
 };
 
 static int tps65218_regulator_probe(struct platform_device *pdev)
diff --git a/drivers/regulator/tps6586x-regulator.c b/drivers/regulator/tps6586x-regulator.c
index 32f38a63d94..0a3bb3aecd9 100644
--- a/drivers/regulator/tps6586x-regulator.c
+++ b/drivers/regulator/tps6586x-regulator.c
@@ -63,12 +63,7 @@ struct tps6586x_regulator {
 	int enable_reg[2];
 };
 
-static inline struct device *to_tps6586x_dev(struct regulator_dev *rdev)
-{
-	return rdev_get_dev(rdev)->parent;
-}
-
-static struct regulator_ops tps6586x_regulator_ops = {
+static struct regulator_ops tps6586x_rw_regulator_ops = {
 	.list_voltage = regulator_list_voltage_table,
 	.map_voltage = regulator_map_voltage_ascend,
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
@@ -79,6 +74,16 @@ static struct regulator_ops tps6586x_regulator_ops = {
 	.disable = regulator_disable_regmap,
 };
 
+static struct regulator_ops tps6586x_ro_regulator_ops = {
+	.list_voltage = regulator_list_voltage_table,
+	.map_voltage = regulator_map_voltage_ascend,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+
+	.is_enabled = regulator_is_enabled_regmap,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+};
+
 static struct regulator_ops tps6586x_sys_regulator_ops = {
 };
 
@@ -106,6 +111,13 @@ static const unsigned int tps6586x_sm2_voltages[] = {
 	4200000, 4250000, 4300000, 4350000, 4400000, 4450000, 4500000, 4550000,
 };
 
+static int tps658640_sm2_voltages[] = {
+	2150000, 2200000, 2250000, 2300000, 2350000, 2400000, 2450000, 2500000,
+	2550000, 2600000, 2650000, 2700000, 2750000, 2800000, 2850000, 2900000,
+	2950000, 3000000, 3050000, 3100000, 3150000, 3200000, 3250000, 3300000,
+	3350000, 3400000, 3450000, 3500000, 3550000, 3600000, 3650000, 3700000,
+};
+
 static const unsigned int tps658643_sm2_voltages[] = {
 	1025000, 1050000, 1075000, 1100000, 1125000, 1150000, 1175000, 1200000,
 	1225000, 1250000, 1275000, 1300000, 1325000, 1350000, 1375000, 1400000,
@@ -120,12 +132,16 @@ static const unsigned int tps6586x_dvm_voltages[] = {
 	1325000, 1350000, 1375000, 1400000, 1425000, 1450000, 1475000, 1500000,
 };
 
-#define TPS6586X_REGULATOR(_id, _pin_name, vdata, vreg, shift, nbits,	\
+static int tps658640_rtc_voltages[] = {
+	2500000, 2850000, 3100000, 3300000,
+};
+
+#define TPS6586X_REGULATOR(_id, _ops, _pin_name, vdata, vreg, shift, nbits, \
 			   ereg0, ebit0, ereg1, ebit1, goreg, gobit)	\
 	.desc	= {							\
 		.supply_name = _pin_name,				\
 		.name	= "REG-" #_id,					\
-		.ops	= &tps6586x_regulator_ops,			\
+		.ops	= &tps6586x_## _ops ## _regulator_ops,		\
 		.type	= REGULATOR_VOLTAGE,				\
 		.id	= TPS6586X_ID_##_id,				\
 		.n_voltages = ARRAY_SIZE(vdata##_voltages),		\
@@ -146,14 +162,21 @@ static const unsigned int tps6586x_dvm_voltages[] = {
 #define TPS6586X_LDO(_id, _pname, vdata, vreg, shift, nbits,		\
 		     ereg0, ebit0, ereg1, ebit1)			\
 {									\
-	TPS6586X_REGULATOR(_id, _pname, vdata, vreg, shift, nbits,	\
+	TPS6586X_REGULATOR(_id, rw, _pname, vdata, vreg, shift, nbits,	\
+			   ereg0, ebit0, ereg1, ebit1, 0, 0)		\
+}
+
+#define TPS6586X_FIXED_LDO(_id, _pname, vdata, vreg, shift, nbits,	\
+			  ereg0, ebit0, ereg1, ebit1)			\
+{									\
+	TPS6586X_REGULATOR(_id, ro, _pname, vdata, vreg, shift, nbits,	\
 			   ereg0, ebit0, ereg1, ebit1, 0, 0)		\
 }
 
 #define TPS6586X_DVM(_id, _pname, vdata, vreg, shift, nbits,		\
 		     ereg0, ebit0, ereg1, ebit1, goreg, gobit)		\
 {									\
-	TPS6586X_REGULATOR(_id, _pname, vdata, vreg, shift, nbits,	\
+	TPS6586X_REGULATOR(_id, rw, _pname, vdata, vreg, shift, nbits,	\
 			   ereg0, ebit0, ereg1, ebit1, goreg, gobit)	\
 }
 
@@ -207,6 +230,26 @@ static struct tps6586x_regulator tps658623_regulator[] = {
 					END, 7),
 };
 
+static struct tps6586x_regulator tps658640_regulator[] = {
+	TPS6586X_LDO(LDO_3, "vinldo23", tps6586x_ldo0, SUPPLYV4, 0, 3,
+					ENC, 2, END, 2),
+	TPS6586X_LDO(LDO_5, "REG-SYS", tps6586x_ldo0, SUPPLYV6, 0, 3,
+					ENE, 6, ENE, 6),
+	TPS6586X_LDO(LDO_6, "vinldo678", tps6586x_ldo0, SUPPLYV3, 0, 3,
+					ENC, 4, END, 4),
+	TPS6586X_LDO(LDO_7, "vinldo678", tps6586x_ldo0, SUPPLYV3, 3, 3,
+					ENC, 5, END, 5),
+	TPS6586X_LDO(LDO_8, "vinldo678", tps6586x_ldo0, SUPPLYV2, 5, 3,
+					ENC, 6, END, 6),
+	TPS6586X_LDO(LDO_9, "vinldo9", tps6586x_ldo0, SUPPLYV6, 3, 3,
+					ENE, 7, ENE, 7),
+	TPS6586X_LDO(SM_2, "vin-sm2", tps658640_sm2, SUPPLYV2, 0, 5,
+					ENC, 7, END, 7),
+
+	TPS6586X_FIXED_LDO(LDO_RTC, "REG-SYS", tps658640_rtc, SUPPLYV4, 3, 2,
+					V4, 7, V4, 7),
+};
+
 static struct tps6586x_regulator tps658643_regulator[] = {
 	TPS6586X_LDO(SM_2, "vin-sm2", tps658643_sm2, SUPPLYV2, 0, 5, ENC, 7,
 					END, 7),
@@ -295,6 +338,11 @@ static struct tps6586x_regulator *find_regulator_info(int id, int version)
 		table = tps658623_regulator;
 		num = ARRAY_SIZE(tps658623_regulator);
 		break;
+	case TPS658640:
+	case TPS658640v2:
+		table = tps658640_regulator;
+		num = ARRAY_SIZE(tps658640_regulator);
+		break;
 	case TPS658643:
 		table = tps658643_regulator;
 		num = ARRAY_SIZE(tps658643_regulator);
diff --git a/drivers/regulator/vexpress.c b/drivers/regulator/vexpress.c
index f3ae28a7e66..02e7267ccf9 100644
--- a/drivers/regulator/vexpress.c
+++ b/drivers/regulator/vexpress.c
@@ -26,14 +26,14 @@
 struct vexpress_regulator {
 	struct regulator_desc desc;
 	struct regulator_dev *regdev;
-	struct vexpress_config_func *func;
+	struct regmap *regmap;
 };
 
 static int vexpress_regulator_get_voltage(struct regulator_dev *regdev)
 {
 	struct vexpress_regulator *reg = rdev_get_drvdata(regdev);
 	u32 uV;
-	int err = vexpress_config_read(reg->func, 0, &uV);
+	int err = regmap_read(reg->regmap, 0, &uV);
 
 	return err ? err : uV;
 }
@@ -43,7 +43,7 @@ static int vexpress_regulator_set_voltage(struct regulator_dev *regdev,
 {
 	struct vexpress_regulator *reg = rdev_get_drvdata(regdev);
 
-	return vexpress_config_write(reg->func, 0, min_uV);
+	return regmap_write(reg->regmap, 0, min_uV);
 }
 
 static struct regulator_ops vexpress_regulator_ops_ro = {
@@ -57,22 +57,17 @@ static struct regulator_ops vexpress_regulator_ops = {
 
 static int vexpress_regulator_probe(struct platform_device *pdev)
 {
-	int err;
 	struct vexpress_regulator *reg;
 	struct regulator_init_data *init_data;
 	struct regulator_config config = { };
 
 	reg = devm_kzalloc(&pdev->dev, sizeof(*reg), GFP_KERNEL);
-	if (!reg) {
-		err = -ENOMEM;
-		goto error_kzalloc;
-	}
+	if (!reg)
+		return -ENOMEM;
 
-	reg->func = vexpress_config_func_get_by_dev(&pdev->dev);
-	if (!reg->func) {
-		err = -ENXIO;
-		goto error_get_func;
-	}
+	reg->regmap = devm_regmap_init_vexpress_config(&pdev->dev);
+	if (IS_ERR(reg->regmap))
+		return PTR_ERR(reg->regmap);
 
 	reg->desc.name = dev_name(&pdev->dev);
 	reg->desc.type = REGULATOR_VOLTAGE;
@@ -80,10 +75,8 @@ static int vexpress_regulator_probe(struct platform_device *pdev)
 	reg->desc.continuous_voltage_range = true;
 
 	init_data = of_get_regulator_init_data(&pdev->dev, pdev->dev.of_node);
-	if (!init_data) {
-		err = -EINVAL;
-		goto error_get_regulator_init_data;
-	}
+	if (!init_data)
+		return -EINVAL;
 
 	init_data->constraints.apply_uV = 0;
 	if (init_data->constraints.min_uV && init_data->constraints.max_uV)
@@ -97,40 +90,21 @@ static int vexpress_regulator_probe(struct platform_device *pdev)
 	config.of_node = pdev->dev.of_node;
 
 	reg->regdev = devm_regulator_register(&pdev->dev, &reg->desc, &config);
-	if (IS_ERR(reg->regdev)) {
-		err = PTR_ERR(reg->regdev);
-		goto error_regulator_register;
-	}
+	if (IS_ERR(reg->regdev))
+		return PTR_ERR(reg->regdev);
 
 	platform_set_drvdata(pdev, reg);
 
 	return 0;
-
-error_regulator_register:
-error_get_regulator_init_data:
-	vexpress_config_func_put(reg->func);
-error_get_func:
-error_kzalloc:
-	return err;
-}
-
-static int vexpress_regulator_remove(struct platform_device *pdev)
-{
-	struct vexpress_regulator *reg = platform_get_drvdata(pdev);
-
-	vexpress_config_func_put(reg->func);
-
-	return 0;
 }
 
-static struct of_device_id vexpress_regulator_of_match[] = {
+static const struct of_device_id vexpress_regulator_of_match[] = {
 	{ .compatible = "arm,vexpress-volt", },
 	{ }
 };
 
 static struct platform_driver vexpress_regulator_driver = {
 	.probe = vexpress_regulator_probe,
-	.remove = vexpress_regulator_remove,
 	.driver	= {
 		.name = DRVNAME,
 		.owner = THIS_MODULE,
diff --git a/drivers/reset/Makefile b/drivers/reset/Makefile
index 4f60caf750c..60fed3d7820 100644
--- a/drivers/reset/Makefile
+++ b/drivers/reset/Makefile
@@ -1,3 +1,4 @@
 obj-$(CONFIG_RESET_CONTROLLER) += core.o
+obj-$(CONFIG_ARCH_SOCFPGA) += reset-socfpga.o
 obj-$(CONFIG_ARCH_SUNXI) += reset-sunxi.o
 obj-$(CONFIG_ARCH_STI) += sti/
diff --git a/drivers/reset/reset-socfpga.c b/drivers/reset/reset-socfpga.c
new file mode 100644
index 00000000000..79c32ca84ef
--- /dev/null
+++ b/drivers/reset/reset-socfpga.c
@@ -0,0 +1,146 @@
+/*
+ * Copyright 2014 Steffen Trumtrar <s.trumtrar@pengutronix.de>
+ *
+ * based on
+ * Allwinner SoCs Reset Controller driver
+ *
+ * Copyright 2013 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.
+ */
+
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/reset-controller.h>
+#include <linux/spinlock.h>
+#include <linux/types.h>
+
+#define NR_BANKS		4
+#define OFFSET_MODRST		0x10
+
+struct socfpga_reset_data {
+	spinlock_t			lock;
+	void __iomem			*membase;
+	struct reset_controller_dev	rcdev;
+};
+
+static int socfpga_reset_assert(struct reset_controller_dev *rcdev,
+				unsigned long id)
+{
+	struct socfpga_reset_data *data = container_of(rcdev,
+						     struct socfpga_reset_data,
+						     rcdev);
+	int bank = id / BITS_PER_LONG;
+	int offset = id % BITS_PER_LONG;
+	unsigned long flags;
+	u32 reg;
+
+	spin_lock_irqsave(&data->lock, flags);
+
+	reg = readl(data->membase + OFFSET_MODRST + (bank * NR_BANKS));
+	writel(reg | BIT(offset), data->membase + OFFSET_MODRST +
+				 (bank * NR_BANKS));
+	spin_unlock_irqrestore(&data->lock, flags);
+
+	return 0;
+}
+
+static int socfpga_reset_deassert(struct reset_controller_dev *rcdev,
+				  unsigned long id)
+{
+	struct socfpga_reset_data *data = container_of(rcdev,
+						     struct socfpga_reset_data,
+						     rcdev);
+
+	int bank = id / BITS_PER_LONG;
+	int offset = id % BITS_PER_LONG;
+	unsigned long flags;
+	u32 reg;
+
+	spin_lock_irqsave(&data->lock, flags);
+
+	reg = readl(data->membase + OFFSET_MODRST + (bank * NR_BANKS));
+	writel(reg & ~BIT(offset), data->membase + OFFSET_MODRST +
+				  (bank * NR_BANKS));
+
+	spin_unlock_irqrestore(&data->lock, flags);
+
+	return 0;
+}
+
+static struct reset_control_ops socfpga_reset_ops = {
+	.assert		= socfpga_reset_assert,
+	.deassert	= socfpga_reset_deassert,
+};
+
+static int socfpga_reset_probe(struct platform_device *pdev)
+{
+	struct socfpga_reset_data *data;
+	struct resource *res;
+
+	/*
+	 * The binding was mainlined without the required property.
+	 * Do not continue, when we encounter an old DT.
+	 */
+	if (!of_find_property(pdev->dev.of_node, "#reset-cells", NULL)) {
+		dev_err(&pdev->dev, "%s missing #reset-cells property\n",
+			pdev->dev.of_node->full_name);
+		return -EINVAL;
+	}
+
+	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	data->membase = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(data->membase))
+		return PTR_ERR(data->membase);
+
+	spin_lock_init(&data->lock);
+
+	data->rcdev.owner = THIS_MODULE;
+	data->rcdev.nr_resets = NR_BANKS * BITS_PER_LONG;
+	data->rcdev.ops = &socfpga_reset_ops;
+	data->rcdev.of_node = pdev->dev.of_node;
+	reset_controller_register(&data->rcdev);
+
+	return 0;
+}
+
+static int socfpga_reset_remove(struct platform_device *pdev)
+{
+	struct socfpga_reset_data *data = platform_get_drvdata(pdev);
+
+	reset_controller_unregister(&data->rcdev);
+
+	return 0;
+}
+
+static const struct of_device_id socfpga_reset_dt_ids[] = {
+	{ .compatible = "altr,rst-mgr", },
+	{ /* sentinel */ },
+};
+
+static struct platform_driver socfpga_reset_driver = {
+	.probe	= socfpga_reset_probe,
+	.remove	= socfpga_reset_remove,
+	.driver = {
+		.name		= "socfpga-reset",
+		.owner		= THIS_MODULE,
+		.of_match_table	= socfpga_reset_dt_ids,
+	},
+};
+module_platform_driver(socfpga_reset_driver);
+
+MODULE_AUTHOR("Steffen Trumtrar <s.trumtrar@pengutronix.de");
+MODULE_DESCRIPTION("Socfpga Reset Controller Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/reset/reset-sunxi.c b/drivers/reset/reset-sunxi.c
index 695bd3496eb..a94e7a7820b 100644
--- a/drivers/reset/reset-sunxi.c
+++ b/drivers/reset/reset-sunxi.c
@@ -145,7 +145,24 @@ MODULE_DEVICE_TABLE(of, sunxi_reset_dt_ids);
 
 static int sunxi_reset_probe(struct platform_device *pdev)
 {
-	return sunxi_reset_init(pdev->dev.of_node);
+	struct sunxi_reset_data *data;
+	struct resource *res;
+
+	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	data->membase = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(data->membase))
+		return PTR_ERR(data->membase);
+
+	data->rcdev.owner = THIS_MODULE;
+	data->rcdev.nr_resets = resource_size(res) * 32;
+	data->rcdev.ops = &sunxi_reset_ops;
+	data->rcdev.of_node = pdev->dev.of_node;
+
+	return reset_controller_register(&data->rcdev);
 }
 
 static int sunxi_reset_remove(struct platform_device *pdev)
@@ -153,8 +170,6 @@ static int sunxi_reset_remove(struct platform_device *pdev)
 	struct sunxi_reset_data *data = platform_get_drvdata(pdev);
 
 	reset_controller_unregister(&data->rcdev);
-	iounmap(data->membase);
-	kfree(data);
 
 	return 0;
 }
diff --git a/drivers/reset/sti/reset-stih415.c b/drivers/reset/sti/reset-stih415.c
index e6f6c41abe1..c93fd260447 100644
--- a/drivers/reset/sti/reset-stih415.c
+++ b/drivers/reset/sti/reset-stih415.c
@@ -73,6 +73,7 @@ static const struct syscfg_reset_channel_data stih415_softresets[] = {
 	[STIH415_USB0_SOFTRESET] = STIH415_SRST_REAR(SYSCFG_376, 9),
 	[STIH415_USB1_SOFTRESET] = STIH415_SRST_REAR(SYSCFG_376, 10),
 	[STIH415_USB2_SOFTRESET] = STIH415_SRST_REAR(SYSCFG_376, 11),
+	[STIH415_KEYSCAN_SOFTRESET] = STIH415_SRST_LPM(LPM_SYSCFG_1, 8),
 };
 
 static struct syscfg_reset_controller_data stih415_powerdown_controller = {
diff --git a/drivers/reset/sti/reset-stih416.c b/drivers/reset/sti/reset-stih416.c
index fe3bf02bdc8..5fc987076a9 100644
--- a/drivers/reset/sti/reset-stih416.c
+++ b/drivers/reset/sti/reset-stih416.c
@@ -104,6 +104,7 @@ static const struct syscfg_reset_channel_data stih416_softresets[] = {
 	[STIH416_COMPO_A_SOFTRESET] = STIH416_SRST_CPU(SYSCFG_7564, 4),
 	[STIH416_VP8_DEC_SOFTRESET] = STIH416_SRST_CPU(SYSCFG_7564, 10),
 	[STIH416_VTG_MAIN_SOFTRESET] = STIH416_SRST_CPU(SYSCFG_7564, 16),
+	[STIH416_KEYSCAN_SOFTRESET] = STIH416_SRST_LPM(LPM_SYSCFG_1, 8),
 };
 
 static struct syscfg_reset_controller_data stih416_powerdown_controller = {
diff --git a/drivers/rtc/rtc-hym8563.c b/drivers/rtc/rtc-hym8563.c
index bd628a6f981..e5f13c4310f 100644
--- a/drivers/rtc/rtc-hym8563.c
+++ b/drivers/rtc/rtc-hym8563.c
@@ -569,6 +569,9 @@ static int hym8563_probe(struct i2c_client *client,
 	if (IS_ERR(hym8563->rtc))
 		return PTR_ERR(hym8563->rtc);
 
+	/* the hym8563 alarm only supports a minute accuracy */
+	hym8563->rtc->uie_unsupported = 1;
+
 #ifdef CONFIG_COMMON_CLK
 	hym8563_clkout_register_clk(hym8563);
 #endif
diff --git a/drivers/rtc/rtc-pcf8523.c b/drivers/rtc/rtc-pcf8523.c
index 5c8f8226c84..4cdb64be061 100644
--- a/drivers/rtc/rtc-pcf8523.c
+++ b/drivers/rtc/rtc-pcf8523.c
@@ -206,7 +206,7 @@ static int pcf8523_rtc_read_time(struct device *dev, struct rtc_time *tm)
 	tm->tm_hour = bcd2bin(regs[2] & 0x3f);
 	tm->tm_mday = bcd2bin(regs[3] & 0x3f);
 	tm->tm_wday = regs[4] & 0x7;
-	tm->tm_mon = bcd2bin(regs[5] & 0x1f);
+	tm->tm_mon = bcd2bin(regs[5] & 0x1f) - 1;
 	tm->tm_year = bcd2bin(regs[6]) + 100;
 
 	return rtc_valid_tm(tm);
@@ -229,7 +229,7 @@ static int pcf8523_rtc_set_time(struct device *dev, struct rtc_time *tm)
 	regs[3] = bin2bcd(tm->tm_hour);
 	regs[4] = bin2bcd(tm->tm_mday);
 	regs[5] = tm->tm_wday;
-	regs[6] = bin2bcd(tm->tm_mon);
+	regs[6] = bin2bcd(tm->tm_mon + 1);
 	regs[7] = bin2bcd(tm->tm_year - 100);
 
 	msg.addr = client->addr;
diff --git a/drivers/s390/char/Makefile b/drivers/s390/char/Makefile
index b69ab17f13f..629fcc275e9 100644
--- a/drivers/s390/char/Makefile
+++ b/drivers/s390/char/Makefile
@@ -33,4 +33,4 @@ obj-$(CONFIG_MONWRITER) += monwriter.o
 obj-$(CONFIG_S390_VMUR) += vmur.o
 
 zcore_mod-objs := sclp_sdias.o zcore.o
-obj-$(CONFIG_ZFCPDUMP) += zcore_mod.o
+obj-$(CONFIG_CRASH_DUMP) += zcore_mod.o
diff --git a/drivers/s390/char/zcore.c b/drivers/s390/char/zcore.c
index 3d8e4d63f51..1884653e447 100644
--- a/drivers/s390/char/zcore.c
+++ b/drivers/s390/char/zcore.c
@@ -17,6 +17,8 @@
 #include <linux/miscdevice.h>
 #include <linux/debugfs.h>
 #include <linux/module.h>
+#include <linux/memblock.h>
+
 #include <asm/asm-offsets.h>
 #include <asm/ipl.h>
 #include <asm/sclp.h>
@@ -411,33 +413,24 @@ static ssize_t zcore_memmap_read(struct file *filp, char __user *buf,
 				 size_t count, loff_t *ppos)
 {
 	return simple_read_from_buffer(buf, count, ppos, filp->private_data,
-				       MEMORY_CHUNKS * CHUNK_INFO_SIZE);
+				       memblock.memory.cnt * CHUNK_INFO_SIZE);
 }
 
 static int zcore_memmap_open(struct inode *inode, struct file *filp)
 {
-	int i;
+	struct memblock_region *reg;
 	char *buf;
-	struct mem_chunk *chunk_array;
+	int i = 0;
 
-	chunk_array = kzalloc(MEMORY_CHUNKS * sizeof(struct mem_chunk),
-			      GFP_KERNEL);
-	if (!chunk_array)
-		return -ENOMEM;
-	detect_memory_layout(chunk_array, 0);
-	buf = kzalloc(MEMORY_CHUNKS * CHUNK_INFO_SIZE, GFP_KERNEL);
+	buf = kzalloc(memblock.memory.cnt * CHUNK_INFO_SIZE, GFP_KERNEL);
 	if (!buf) {
-		kfree(chunk_array);
 		return -ENOMEM;
 	}
-	for (i = 0; i < MEMORY_CHUNKS; i++) {
-		sprintf(buf + (i * CHUNK_INFO_SIZE), "%016llx %016llx ",
-			(unsigned long long) chunk_array[i].addr,
-			(unsigned long long) chunk_array[i].size);
-		if (chunk_array[i].size == 0)
-			break;
+	for_each_memblock(memory, reg) {
+		sprintf(buf + (i++ * CHUNK_INFO_SIZE), "%016llx %016llx ",
+			(unsigned long long) reg->base,
+			(unsigned long long) reg->size);
 	}
-	kfree(chunk_array);
 	filp->private_data = buf;
 	return nonseekable_open(inode, filp);
 }
@@ -593,21 +586,12 @@ static int __init check_sdias(void)
 
 static int __init get_mem_info(unsigned long *mem, unsigned long *end)
 {
-	int i;
-	struct mem_chunk *chunk_array;
+	struct memblock_region *reg;
 
-	chunk_array = kzalloc(MEMORY_CHUNKS * sizeof(struct mem_chunk),
-			      GFP_KERNEL);
-	if (!chunk_array)
-		return -ENOMEM;
-	detect_memory_layout(chunk_array, 0);
-	for (i = 0; i < MEMORY_CHUNKS; i++) {
-		if (chunk_array[i].size == 0)
-			break;
-		*mem += chunk_array[i].size;
-		*end = max(*end, chunk_array[i].addr + chunk_array[i].size);
+	for_each_memblock(memory, reg) {
+		*mem += reg->size;
+		*end = max_t(unsigned long, *end, reg->base + reg->size);
 	}
-	kfree(chunk_array);
 	return 0;
 }
 
diff --git a/drivers/s390/cio/ccwreq.c b/drivers/s390/cio/ccwreq.c
index 5156264d0c7..07676c22d51 100644
--- a/drivers/s390/cio/ccwreq.c
+++ b/drivers/s390/cio/ccwreq.c
@@ -46,7 +46,7 @@ static u16 ccwreq_next_path(struct ccw_device *cdev)
 		goto out;
 	}
 	req->retries	= req->maxretries;
-	req->mask	= lpm_adjust(req->mask >>= 1, req->lpm);
+	req->mask	= lpm_adjust(req->mask >> 1, req->lpm);
 out:
 	return req->mask;
 }
@@ -252,7 +252,7 @@ static void ccwreq_log_status(struct ccw_device *cdev, enum io_status status)
  */
 void ccw_request_handler(struct ccw_device *cdev)
 {
-	struct irb *irb = (struct irb *)&S390_lowcore.irb;
+	struct irb *irb = &__get_cpu_var(cio_irb);
 	struct ccw_request *req = &cdev->private->req;
 	enum io_status status;
 	int rc = -EOPNOTSUPP;
diff --git a/drivers/s390/cio/chp.c b/drivers/s390/cio/chp.c
index 6c440d4349d..d497aa05a72 100644
--- a/drivers/s390/cio/chp.c
+++ b/drivers/s390/cio/chp.c
@@ -509,7 +509,7 @@ out:
  * On success return a newly allocated copy of the channel-path description
  * data associated with the given channel-path ID. Return %NULL on error.
  */
-void *chp_get_chp_desc(struct chp_id chpid)
+struct channel_path_desc *chp_get_chp_desc(struct chp_id chpid)
 {
 	struct channel_path *chp;
 	struct channel_path_desc *desc;
diff --git a/drivers/s390/cio/chp.h b/drivers/s390/cio/chp.h
index 9284b785a06..4efd5b867cc 100644
--- a/drivers/s390/cio/chp.h
+++ b/drivers/s390/cio/chp.h
@@ -60,7 +60,7 @@ static inline struct channel_path *chpid_to_chp(struct chp_id chpid)
 int chp_get_status(struct chp_id chpid);
 u8 chp_get_sch_opm(struct subchannel *sch);
 int chp_is_registered(struct chp_id chpid);
-void *chp_get_chp_desc(struct chp_id chpid);
+struct channel_path_desc *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);
diff --git a/drivers/s390/cio/chsc.c b/drivers/s390/cio/chsc.c
index 9f0ea6cb692..e3bf885f4a6 100644
--- a/drivers/s390/cio/chsc.c
+++ b/drivers/s390/cio/chsc.c
@@ -541,18 +541,27 @@ static void chsc_process_sei_nt0(struct chsc_sei_nt0_area *sei_area)
 
 static void chsc_process_event_information(struct chsc_sei *sei, u64 ntsm)
 {
-	do {
+	static int ntsm_unsupported;
+
+	while (true) {
 		memset(sei, 0, sizeof(*sei));
 		sei->request.length = 0x0010;
 		sei->request.code = 0x000e;
-		sei->ntsm = ntsm;
+		if (!ntsm_unsupported)
+			sei->ntsm = ntsm;
 
 		if (chsc(sei))
 			break;
 
 		if (sei->response.code != 0x0001) {
-			CIO_CRW_EVENT(2, "chsc: sei failed (rc=%04x)\n",
-				      sei->response.code);
+			CIO_CRW_EVENT(2, "chsc: sei failed (rc=%04x, ntsm=%llx)\n",
+				      sei->response.code, sei->ntsm);
+
+			if (sei->response.code == 3 && sei->ntsm) {
+				/* Fallback for old firmware. */
+				ntsm_unsupported = 1;
+				continue;
+			}
 			break;
 		}
 
@@ -568,7 +577,10 @@ static void chsc_process_event_information(struct chsc_sei *sei, u64 ntsm)
 			CIO_CRW_EVENT(2, "chsc: unhandled nt: %d\n", sei->nt);
 			break;
 		}
-	} while (sei->u.nt0_area.flags & 0x80);
+
+		if (!(sei->u.nt0_area.flags & 0x80))
+			break;
+	}
 }
 
 /*
diff --git a/drivers/s390/cio/chsc.h b/drivers/s390/cio/chsc.h
index 7e53a9c8b0b..76c9b50700b 100644
--- a/drivers/s390/cio/chsc.h
+++ b/drivers/s390/cio/chsc.h
@@ -21,17 +21,6 @@ struct cmg_entry {
 	u32 values[NR_MEASUREMENT_ENTRIES];
 } __attribute__ ((packed));
 
-struct channel_path_desc {
-	u8 flags;
-	u8 lsn;
-	u8 desc;
-	u8 chpid;
-	u8 swla;
-	u8 zeroes;
-	u8 chla;
-	u8 chpp;
-} __attribute__ ((packed));
-
 struct channel_path_desc_fmt1 {
 	u8 flags;
 	u8 lsn;
diff --git a/drivers/s390/cio/chsc_sch.c b/drivers/s390/cio/chsc_sch.c
index 1d3661af7bd..3d22d2a4ce1 100644
--- a/drivers/s390/cio/chsc_sch.c
+++ b/drivers/s390/cio/chsc_sch.c
@@ -58,7 +58,7 @@ static void chsc_subchannel_irq(struct subchannel *sch)
 {
 	struct chsc_private *private = dev_get_drvdata(&sch->dev);
 	struct chsc_request *request = private->request;
-	struct irb *irb = (struct irb *)&S390_lowcore.irb;
+	struct irb *irb = &__get_cpu_var(cio_irb);
 
 	CHSC_LOG(4, "irb");
 	CHSC_LOG_HEX(4, irb, sizeof(*irb));
diff --git a/drivers/s390/cio/cio.c b/drivers/s390/cio/cio.c
index 9e058c4657a..77f9c92df4b 100644
--- a/drivers/s390/cio/cio.c
+++ b/drivers/s390/cio/cio.c
@@ -46,6 +46,9 @@ debug_info_t *cio_debug_msg_id;
 debug_info_t *cio_debug_trace_id;
 debug_info_t *cio_debug_crw_id;
 
+DEFINE_PER_CPU_ALIGNED(struct irb, cio_irb);
+EXPORT_PER_CPU_SYMBOL(cio_irb);
+
 /*
  * Function: cio_debug_init
  * Initializes three debug logs for common I/O:
@@ -560,7 +563,7 @@ static irqreturn_t do_cio_interrupt(int irq, void *dummy)
 
 	__this_cpu_write(s390_idle.nohz_delay, 1);
 	tpi_info = (struct tpi_info *) &get_irq_regs()->int_code;
-	irb = (struct irb *) &S390_lowcore.irb;
+	irb = &__get_cpu_var(cio_irb);
 	sch = (struct subchannel *)(unsigned long) tpi_info->intparm;
 	if (!sch) {
 		/* Clear pending interrupt condition. */
@@ -609,7 +612,7 @@ void cio_tsch(struct subchannel *sch)
 	struct irb *irb;
 	int irq_context;
 
-	irb = (struct irb *)&S390_lowcore.irb;
+	irb = &__get_cpu_var(cio_irb);
 	/* Store interrupt response block to lowcore. */
 	if (tsch(sch->schid, irb) != 0)
 		/* Not status pending or not operational. */
@@ -746,7 +749,7 @@ __clear_io_subchannel_easy(struct subchannel_id schid)
 		struct tpi_info ti;
 
 		if (tpi(&ti)) {
-			tsch(ti.schid, (struct irb *)&S390_lowcore.irb);
+			tsch(ti.schid, &__get_cpu_var(cio_irb));
 			if (schid_equal(&ti.schid, &schid))
 				return 0;
 		}
diff --git a/drivers/s390/cio/cio.h b/drivers/s390/cio/cio.h
index d42f67412bd..a01376ae174 100644
--- a/drivers/s390/cio/cio.h
+++ b/drivers/s390/cio/cio.h
@@ -102,6 +102,8 @@ struct subchannel {
 	struct schib_config config;
 } __attribute__ ((aligned(8)));
 
+DECLARE_PER_CPU(struct irb, cio_irb);
+
 #define to_subchannel(n) container_of(n, struct subchannel, dev)
 
 extern int cio_validate_subchannel (struct subchannel *, struct subchannel_id);
diff --git a/drivers/s390/cio/device_fsm.c b/drivers/s390/cio/device_fsm.c
index c7638c54325..0bc902b3cd8 100644
--- a/drivers/s390/cio/device_fsm.c
+++ b/drivers/s390/cio/device_fsm.c
@@ -739,7 +739,7 @@ ccw_device_irq(struct ccw_device *cdev, enum dev_event dev_event)
 	struct irb *irb;
 	int is_cmd;
 
-	irb = (struct irb *)&S390_lowcore.irb;
+	irb = &__get_cpu_var(cio_irb);
 	is_cmd = !scsw_is_tm(&irb->scsw);
 	/* Check for unsolicited interrupt. */
 	if (!scsw_is_solicited(&irb->scsw)) {
@@ -805,7 +805,7 @@ ccw_device_w4sense(struct ccw_device *cdev, enum dev_event dev_event)
 {
 	struct irb *irb;
 
-	irb = (struct irb *)&S390_lowcore.irb;
+	irb = &__get_cpu_var(cio_irb);
 	/* Check for unsolicited interrupt. */
 	if (scsw_stctl(&irb->scsw) ==
 	    (SCSW_STCTL_STATUS_PEND | SCSW_STCTL_ALERT_STATUS)) {
diff --git a/drivers/s390/cio/device_ops.c b/drivers/s390/cio/device_ops.c
index 4845d64f284..f3c417943da 100644
--- a/drivers/s390/cio/device_ops.c
+++ b/drivers/s390/cio/device_ops.c
@@ -563,14 +563,23 @@ out_unlock:
 	return rc;
 }
 
-void *ccw_device_get_chp_desc(struct ccw_device *cdev, int chp_no)
+/**
+ * chp_get_chp_desc - return newly allocated channel-path descriptor
+ * @cdev: device to obtain the descriptor for
+ * @chp_idx: index of the channel path
+ *
+ * On success return a newly allocated copy of the channel-path description
+ * data associated with the given channel path. Return %NULL on error.
+ */
+struct channel_path_desc *ccw_device_get_chp_desc(struct ccw_device *cdev,
+						  int chp_idx)
 {
 	struct subchannel *sch;
 	struct chp_id chpid;
 
 	sch = to_subchannel(cdev->dev.parent);
 	chp_id_init(&chpid);
-	chpid.id = sch->schib.pmcw.chpid[chp_no];
+	chpid.id = sch->schib.pmcw.chpid[chp_idx];
 	return chp_get_chp_desc(chpid);
 }
 
diff --git a/drivers/s390/cio/eadm_sch.c b/drivers/s390/cio/eadm_sch.c
index 3a2ee4a740b..c4f7bf3e24c 100644
--- a/drivers/s390/cio/eadm_sch.c
+++ b/drivers/s390/cio/eadm_sch.c
@@ -134,7 +134,7 @@ static void eadm_subchannel_irq(struct subchannel *sch)
 {
 	struct eadm_private *private = get_eadm_private(sch);
 	struct eadm_scsw *scsw = &sch->schib.scsw.eadm;
-	struct irb *irb = (struct irb *)&S390_lowcore.irb;
+	struct irb *irb = &__get_cpu_var(cio_irb);
 	int error = 0;
 
 	EADM_LOG(6, "irq");
diff --git a/drivers/s390/net/qeth_core_main.c b/drivers/s390/net/qeth_core_main.c
index 22470a3b182..e89f38c3117 100644
--- a/drivers/s390/net/qeth_core_main.c
+++ b/drivers/s390/net/qeth_core_main.c
@@ -22,6 +22,7 @@
 #include <net/iucv/af_iucv.h>
 
 #include <asm/ebcdic.h>
+#include <asm/chpid.h>
 #include <asm/io.h>
 #include <asm/sysinfo.h>
 #include <asm/compat.h>
@@ -1344,16 +1345,7 @@ static void qeth_set_multiple_write_queues(struct qeth_card *card)
 static void qeth_update_from_chp_desc(struct qeth_card *card)
 {
 	struct ccw_device *ccwdev;
-	struct channelPath_dsc {
-		u8 flags;
-		u8 lsn;
-		u8 desc;
-		u8 chpid;
-		u8 swla;
-		u8 zeroes;
-		u8 chla;
-		u8 chpp;
-	} *chp_dsc;
+	struct channel_path_desc *chp_dsc;
 
 	QETH_DBF_TEXT(SETUP, 2, "chp_desc");
 
diff --git a/drivers/sbus/char/jsflash.c b/drivers/sbus/char/jsflash.c
index 4ccb5d86938..a40ee1e3748 100644
--- a/drivers/sbus/char/jsflash.c
+++ b/drivers/sbus/char/jsflash.c
@@ -207,7 +207,7 @@ static void jsfd_do_request(struct request_queue *q)
 			goto end;
 		}
 
-		jsfd_read(req->buffer, jdp->dbase + offset, len);
+		jsfd_read(bio_data(req->bio), jdp->dbase + offset, len);
 		err = 0;
 	end:
 		if (!__blk_end_request_cur(req, err))
diff --git a/drivers/scsi/atari_scsi.c b/drivers/scsi/atari_scsi.c
index 296c936cc03..a8d721ff19e 100644
--- a/drivers/scsi/atari_scsi.c
+++ b/drivers/scsi/atari_scsi.c
@@ -639,7 +639,7 @@ static int __init atari_scsi_detect(struct scsi_host_template *host)
 					"double buffer\n");
 			return 0;
 		}
-		atari_dma_phys_buffer = virt_to_phys(atari_dma_buffer);
+		atari_dma_phys_buffer = atari_stram_to_phys(atari_dma_buffer);
 		atari_dma_orig_addr = 0;
 	}
 #endif
diff --git a/drivers/scsi/hpsa.c b/drivers/scsi/hpsa.c
index 8cf4a0c69ba..9a6e4a2cd07 100644
--- a/drivers/scsi/hpsa.c
+++ b/drivers/scsi/hpsa.c
@@ -7463,6 +7463,10 @@ static void hpsa_put_ctlr_into_performant_mode(struct ctlr_info *h)
 	if (hpsa_simple_mode)
 		return;
 
+	trans_support = readl(&(h->cfgtable->TransportSupport));
+	if (!(trans_support & PERFORMANT_MODE))
+		return;
+
 	/* Check for I/O accelerator mode support */
 	if (trans_support & CFGTBL_Trans_io_accel1) {
 		transMethod |= CFGTBL_Trans_io_accel1 |
@@ -7479,10 +7483,6 @@ static void hpsa_put_ctlr_into_performant_mode(struct ctlr_info *h)
 	}
 
 	/* TODO, check that this next line h->nreply_queues is correct */
-	trans_support = readl(&(h->cfgtable->TransportSupport));
-	if (!(trans_support & PERFORMANT_MODE))
-		return;
-
 	h->nreply_queues = h->msix_vector > 0 ? h->msix_vector : 1;
 	hpsa_get_max_perf_mode_cmds(h);
 	/* Performant mode ring buffer and supporting data structures */
diff --git a/drivers/scsi/mpt2sas/mpt2sas_scsih.c b/drivers/scsi/mpt2sas/mpt2sas_scsih.c
index 7f0af4fcc00..6fd7d40b2c4 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_scsih.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_scsih.c
@@ -8293,7 +8293,6 @@ _scsih_suspend(struct pci_dev *pdev, pm_message_t state)
 
 	mpt2sas_base_free_resources(ioc);
 	pci_save_state(pdev);
-	pci_disable_device(pdev);
 	pci_set_power_state(pdev, device_state);
 	return 0;
 }
diff --git a/drivers/scsi/scsi_error.c b/drivers/scsi/scsi_error.c
index 771c16bfdba..f17aa7aa787 100644
--- a/drivers/scsi/scsi_error.c
+++ b/drivers/scsi/scsi_error.c
@@ -189,6 +189,7 @@ scsi_abort_command(struct scsi_cmnd *scmd)
 		/*
 		 * Retry after abort failed, escalate to next level.
 		 */
+		scmd->eh_eflags &= ~SCSI_EH_ABORT_SCHEDULED;
 		SCSI_LOG_ERROR_RECOVERY(3,
 			scmd_printk(KERN_INFO, scmd,
 				    "scmd %p previous abort failed\n", scmd));
@@ -920,10 +921,12 @@ void scsi_eh_prep_cmnd(struct scsi_cmnd *scmd, struct scsi_eh_save *ses,
 	ses->prot_op = scmd->prot_op;
 
 	scmd->prot_op = SCSI_PROT_NORMAL;
+	scmd->eh_eflags = 0;
 	scmd->cmnd = ses->eh_cmnd;
 	memset(scmd->cmnd, 0, BLK_MAX_CDB);
 	memset(&scmd->sdb, 0, sizeof(scmd->sdb));
 	scmd->request->next_rq = NULL;
+	scmd->result = 0;
 
 	if (sense_bytes) {
 		scmd->sdb.length = min_t(unsigned, SCSI_SENSE_BUFFERSIZE,
@@ -1157,6 +1160,15 @@ int scsi_eh_get_sense(struct list_head *work_q,
 					     __func__));
 			break;
 		}
+		if (status_byte(scmd->result) != CHECK_CONDITION)
+			/*
+			 * don't request sense if there's no check condition
+			 * status because the error we're processing isn't one
+			 * that has a sense code (and some devices get
+			 * confused by sense requests out of the blue)
+			 */
+			continue;
+
 		SCSI_LOG_ERROR_RECOVERY(2, scmd_printk(KERN_INFO, scmd,
 						  "%s: requesting sense\n",
 						  current->comm));
diff --git a/drivers/scsi/scsi_lib.c b/drivers/scsi/scsi_lib.c
index 65a123d9c67..a0c95cac91f 100644
--- a/drivers/scsi/scsi_lib.c
+++ b/drivers/scsi/scsi_lib.c
@@ -137,9 +137,10 @@ static void __scsi_queue_insert(struct scsi_cmnd *cmd, int reason, int unbusy)
 	 * lock such that the kblockd_schedule_work() call happens
 	 * before blk_cleanup_queue() finishes.
 	 */
+	cmd->result = 0;
 	spin_lock_irqsave(q->queue_lock, flags);
 	blk_requeue_request(q, cmd->request);
-	kblockd_schedule_work(q, &device->requeue_work);
+	kblockd_schedule_work(&device->requeue_work);
 	spin_unlock_irqrestore(q->queue_lock, flags);
 }
 
@@ -1018,8 +1019,6 @@ static int scsi_init_sgtable(struct request *req, struct scsi_data_buffer *sdb,
 		return BLKPREP_DEFER;
 	}
 
-	req->buffer = NULL;
-
 	/* 
 	 * Next, walk the list, and fill in the addresses and sizes of
 	 * each segment.
@@ -1044,6 +1043,7 @@ static int scsi_init_sgtable(struct request *req, struct scsi_data_buffer *sdb,
  */
 int scsi_init_io(struct scsi_cmnd *cmd, gfp_t gfp_mask)
 {
+	struct scsi_device *sdev = cmd->device;
 	struct request *rq = cmd->request;
 
 	int error = scsi_init_sgtable(rq, &cmd->sdb, gfp_mask);
@@ -1091,7 +1091,7 @@ err_exit:
 	scsi_release_buffers(cmd);
 	cmd->request->special = NULL;
 	scsi_put_command(cmd);
-	put_device(&cmd->device->sdev_gendev);
+	put_device(&sdev->sdev_gendev);
 	return error;
 }
 EXPORT_SYMBOL(scsi_init_io);
@@ -1156,7 +1156,6 @@ int scsi_setup_blk_pc_cmnd(struct scsi_device *sdev, struct request *req)
 		BUG_ON(blk_rq_bytes(req));
 
 		memset(&cmd->sdb, 0, sizeof(cmd->sdb));
-		req->buffer = NULL;
 	}
 
 	cmd->cmd_len = req->cmd_len;
@@ -1273,7 +1272,7 @@ int scsi_prep_return(struct request_queue *q, struct request *req, int ret)
 			struct scsi_cmnd *cmd = req->special;
 			scsi_release_buffers(cmd);
 			scsi_put_command(cmd);
-			put_device(&cmd->device->sdev_gendev);
+			put_device(&sdev->sdev_gendev);
 			req->special = NULL;
 		}
 		break;
diff --git a/drivers/scsi/scsi_netlink.c b/drivers/scsi/scsi_netlink.c
index fe30ea94ffe..109802f776e 100644
--- a/drivers/scsi/scsi_netlink.c
+++ b/drivers/scsi/scsi_netlink.c
@@ -77,7 +77,7 @@ scsi_nl_rcv_msg(struct sk_buff *skb)
 			goto next_msg;
 		}
 
-		if (!capable(CAP_SYS_ADMIN)) {
+		if (!netlink_capable(skb, CAP_SYS_ADMIN)) {
 			err = -EPERM;
 			goto next_msg;
 		}
diff --git a/drivers/scsi/scsi_transport_sas.c b/drivers/scsi/scsi_transport_sas.c
index 1b681427dde..c341f855fad 100644
--- a/drivers/scsi/scsi_transport_sas.c
+++ b/drivers/scsi/scsi_transport_sas.c
@@ -1621,8 +1621,6 @@ void sas_rphy_free(struct sas_rphy *rphy)
 	list_del(&rphy->list);
 	mutex_unlock(&sas_host->lock);
 
-	sas_bsg_remove(shost, rphy);
-
 	transport_destroy_device(dev);
 
 	put_device(dev);
@@ -1681,6 +1679,7 @@ sas_rphy_remove(struct sas_rphy *rphy)
 	}
 
 	sas_rphy_unlink(rphy);
+	sas_bsg_remove(NULL, rphy);
 	transport_remove_device(dev);
 	device_del(dev);
 }
diff --git a/drivers/scsi/sd.c b/drivers/scsi/sd.c
index efcbcd18286..96af195224f 100644
--- a/drivers/scsi/sd.c
+++ b/drivers/scsi/sd.c
@@ -737,16 +737,14 @@ static int sd_setup_discard_cmnd(struct scsi_device *sdp, struct request *rq)
 		goto out;
 	}
 
+	rq->completion_data = page;
 	blk_add_request_payload(rq, page, len);
 	ret = scsi_setup_blk_pc_cmnd(sdp, rq);
-	rq->buffer = page_address(page);
 	rq->__data_len = nr_bytes;
 
 out:
-	if (ret != BLKPREP_OK) {
+	if (ret != BLKPREP_OK)
 		__free_page(page);
-		rq->buffer = NULL;
-	}
 	return ret;
 }
 
@@ -842,10 +840,9 @@ static void sd_unprep_fn(struct request_queue *q, struct request *rq)
 {
 	struct scsi_cmnd *SCpnt = rq->special;
 
-	if (rq->cmd_flags & REQ_DISCARD) {
-		free_page((unsigned long)rq->buffer);
-		rq->buffer = NULL;
-	}
+	if (rq->cmd_flags & REQ_DISCARD)
+		__free_page(rq->completion_data);
+
 	if (SCpnt->cmnd != rq->cmd) {
 		mempool_free(SCpnt->cmnd, sd_cdb_pool);
 		SCpnt->cmnd = NULL;
diff --git a/drivers/scsi/virtio_scsi.c b/drivers/scsi/virtio_scsi.c
index 16bfd50cd3f..db3b494e592 100644
--- a/drivers/scsi/virtio_scsi.c
+++ b/drivers/scsi/virtio_scsi.c
@@ -750,8 +750,12 @@ static void __virtscsi_set_affinity(struct virtio_scsi *vscsi, bool affinity)
 
 		vscsi->affinity_hint_set = true;
 	} else {
-		for (i = 0; i < vscsi->num_queues; i++)
+		for (i = 0; i < vscsi->num_queues; i++) {
+			if (!vscsi->req_vqs[i].vq)
+				continue;
+
 			virtqueue_set_affinity(vscsi->req_vqs[i].vq, -1);
+		}
 
 		vscsi->affinity_hint_set = false;
 	}
diff --git a/drivers/sh/Makefile b/drivers/sh/Makefile
index fc67f564f02..788ed9b59b4 100644
--- a/drivers/sh/Makefile
+++ b/drivers/sh/Makefile
@@ -1,10 +1,12 @@
 #
 # Makefile for the SuperH specific drivers.
 #
-obj-y	:= intc/
+obj-$(CONFIG_SUPERH)			+= intc/
+obj-$(CONFIG_ARCH_SHMOBILE_LEGACY)	+= intc/
+ifneq ($(CONFIG_COMMON_CLK),y)
+obj-$(CONFIG_HAVE_CLK)			+= clk/
+endif
+obj-$(CONFIG_MAPLE)			+= maple/
+obj-$(CONFIG_SUPERHYWAY)		+= superhyway/
 
-obj-$(CONFIG_HAVE_CLK)		+= clk/
-obj-$(CONFIG_MAPLE)		+= maple/
-obj-$(CONFIG_SUPERHYWAY)	+= superhyway/
-
-obj-y				+= pm_runtime.o
+obj-y					+= pm_runtime.o
diff --git a/drivers/sh/pm_runtime.c b/drivers/sh/pm_runtime.c
index 8afa5a4589f..10c65eb51f8 100644
--- a/drivers/sh/pm_runtime.c
+++ b/drivers/sh/pm_runtime.c
@@ -50,8 +50,25 @@ static struct pm_clk_notifier_block platform_bus_notifier = {
 	.con_ids = { NULL, },
 };
 
+static bool default_pm_on;
+
 static int __init sh_pm_runtime_init(void)
 {
+	if (IS_ENABLED(CONFIG_ARCH_SHMOBILE_MULTI)) {
+		if (!of_machine_is_compatible("renesas,emev2") &&
+		    !of_machine_is_compatible("renesas,r7s72100") &&
+		    !of_machine_is_compatible("renesas,r8a73a4") &&
+		    !of_machine_is_compatible("renesas,r8a7740") &&
+		    !of_machine_is_compatible("renesas,r8a7778") &&
+		    !of_machine_is_compatible("renesas,r8a7779") &&
+		    !of_machine_is_compatible("renesas,r8a7790") &&
+		    !of_machine_is_compatible("renesas,r8a7791") &&
+		    !of_machine_is_compatible("renesas,sh7372") &&
+		    !of_machine_is_compatible("renesas,sh73a0"))
+			return 0;
+	}
+
+	default_pm_on = true;
 	pm_clk_add_notifier(&platform_bus_type, &platform_bus_notifier);
 	return 0;
 }
@@ -59,7 +76,8 @@ core_initcall(sh_pm_runtime_init);
 
 static int __init sh_pm_runtime_late_init(void)
 {
-	pm_genpd_poweroff_unused();
+	if (default_pm_on)
+		pm_genpd_poweroff_unused();
 	return 0;
 }
 late_initcall(sh_pm_runtime_late_init);
diff --git a/drivers/soc/Kconfig b/drivers/soc/Kconfig
new file mode 100644
index 00000000000..c8543855aa8
--- /dev/null
+++ b/drivers/soc/Kconfig
@@ -0,0 +1,5 @@
+menu "SOC (System On Chip) specific Drivers"
+
+source "drivers/soc/qcom/Kconfig"
+
+endmenu
diff --git a/drivers/soc/Makefile b/drivers/soc/Makefile
new file mode 100644
index 00000000000..0f7c44793b2
--- /dev/null
+++ b/drivers/soc/Makefile
@@ -0,0 +1,5 @@
+#
+# Makefile for the Linux Kernel SOC specific device drivers.
+#
+
+obj-$(CONFIG_ARCH_QCOM)		+= qcom/
diff --git a/drivers/soc/qcom/Kconfig b/drivers/soc/qcom/Kconfig
new file mode 100644
index 00000000000..7bd2c94f54a
--- /dev/null
+++ b/drivers/soc/qcom/Kconfig
@@ -0,0 +1,11 @@
+#
+# QCOM Soc drivers
+#
+config QCOM_GSBI
+        tristate "QCOM General Serial Bus Interface"
+        depends on ARCH_QCOM
+        help
+          Say y here to enable GSBI support.  The GSBI provides control
+          functions for connecting the underlying serial UART, SPI, and I2C
+          devices to the output pins.
+
diff --git a/drivers/soc/qcom/Makefile b/drivers/soc/qcom/Makefile
new file mode 100644
index 00000000000..438901257ac
--- /dev/null
+++ b/drivers/soc/qcom/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_QCOM_GSBI)	+=	qcom_gsbi.o
diff --git a/drivers/soc/qcom/qcom_gsbi.c b/drivers/soc/qcom/qcom_gsbi.c
new file mode 100644
index 00000000000..447458e696a
--- /dev/null
+++ b/drivers/soc/qcom/qcom_gsbi.c
@@ -0,0 +1,85 @@
+/*
+ * Copyright (c) 2014, The Linux foundation. 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 rev 2 and
+ * only rev 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/clk.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+
+#define GSBI_CTRL_REG		0x0000
+#define GSBI_PROTOCOL_SHIFT	4
+
+static int gsbi_probe(struct platform_device *pdev)
+{
+	struct device_node *node = pdev->dev.of_node;
+	struct resource *res;
+	void __iomem *base;
+	struct clk *hclk;
+	u32 mode, crci = 0;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	if (of_property_read_u32(node, "qcom,mode", &mode)) {
+		dev_err(&pdev->dev, "missing mode configuration\n");
+		return -EINVAL;
+	}
+
+	/* not required, so default to 0 if not present */
+	of_property_read_u32(node, "qcom,crci", &crci);
+
+	dev_info(&pdev->dev, "GSBI port protocol: %d crci: %d\n", mode, crci);
+
+	hclk = devm_clk_get(&pdev->dev, "iface");
+	if (IS_ERR(hclk))
+		return PTR_ERR(hclk);
+
+	clk_prepare_enable(hclk);
+
+	writel_relaxed((mode << GSBI_PROTOCOL_SHIFT) | crci,
+				base + GSBI_CTRL_REG);
+
+	/* make sure the gsbi control write is not reordered */
+	wmb();
+
+	clk_disable_unprepare(hclk);
+
+	return of_platform_populate(pdev->dev.of_node, NULL, NULL, &pdev->dev);
+}
+
+static const struct of_device_id gsbi_dt_match[] = {
+	{ .compatible = "qcom,gsbi-v1.0.0", },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(of, gsbi_dt_match);
+
+static struct platform_driver gsbi_driver = {
+	.driver = {
+		.name		= "gsbi",
+		.owner		= THIS_MODULE,
+		.of_match_table	= gsbi_dt_match,
+	},
+	.probe = gsbi_probe,
+};
+
+module_platform_driver(gsbi_driver);
+
+MODULE_AUTHOR("Andy Gross <agross@codeaurora.org>");
+MODULE_DESCRIPTION("QCOM GSBI driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/spi/Kconfig b/drivers/spi/Kconfig
index 60f2b41c731..213b5cbb9dc 100644
--- a/drivers/spi/Kconfig
+++ b/drivers/spi/Kconfig
@@ -91,8 +91,8 @@ config SPI_BFIN5XX
 	help
 	  This is the SPI controller master driver for Blackfin 5xx processor.
 
-config SPI_BFIN_V3
-	tristate "SPI controller v3 for Blackfin"
+config SPI_ADI_V3
+	tristate "SPI controller v3 for ADI"
 	depends on BF60x
 	help
 	  This is the SPI controller v3 master driver
@@ -148,6 +148,13 @@ config SPI_BUTTERFLY
 	  inexpensive battery powered microcontroller evaluation board.
 	  This same cable can be used to flash new firmware.
 
+config SPI_CADENCE
+	tristate "Cadence SPI controller"
+	depends on ARM
+	help
+	  This selects the Cadence SPI controller master driver
+	  used by Xilinx Zynq.
+
 config SPI_CLPS711X
 	tristate "CLPS711X host SPI controller"
 	depends on ARCH_CLPS711X || COMPILE_TEST
@@ -505,7 +512,7 @@ config SPI_TEGRA20_SLINK
 
 config SPI_TOPCLIFF_PCH
 	tristate "Intel EG20T PCH/LAPIS Semicon IOH(ML7213/ML7223/ML7831) SPI"
-	depends on PCI
+	depends on PCI && (X86_32 || COMPILE_TEST)
 	help
 	  SPI driver for the Topcliff PCH (Platform Controller Hub) SPI bus
 	  used in some x86 embedded processors.
diff --git a/drivers/spi/Makefile b/drivers/spi/Makefile
index bd792669e56..929c9f5eac0 100644
--- a/drivers/spi/Makefile
+++ b/drivers/spi/Makefile
@@ -18,10 +18,11 @@ obj-$(CONFIG_SPI_BCM2835)		+= spi-bcm2835.o
 obj-$(CONFIG_SPI_BCM63XX)		+= spi-bcm63xx.o
 obj-$(CONFIG_SPI_BCM63XX_HSSPI)		+= spi-bcm63xx-hsspi.o
 obj-$(CONFIG_SPI_BFIN5XX)		+= spi-bfin5xx.o
-obj-$(CONFIG_SPI_BFIN_V3)               += spi-bfin-v3.o
+obj-$(CONFIG_SPI_ADI_V3)                += spi-adi-v3.o
 obj-$(CONFIG_SPI_BFIN_SPORT)		+= spi-bfin-sport.o
 obj-$(CONFIG_SPI_BITBANG)		+= spi-bitbang.o
 obj-$(CONFIG_SPI_BUTTERFLY)		+= spi-butterfly.o
+obj-$(CONFIG_SPI_CADENCE)		+= spi-cadence.o
 obj-$(CONFIG_SPI_CLPS711X)		+= spi-clps711x.o
 obj-$(CONFIG_SPI_COLDFIRE_QSPI)		+= spi-coldfire-qspi.o
 obj-$(CONFIG_SPI_DAVINCI)		+= spi-davinci.o
diff --git a/drivers/spi/spi-bfin-v3.c b/drivers/spi/spi-adi-v3.c
index 4089d0e0d84..dcb2287c7f8 100644
--- a/drivers/spi/spi-bfin-v3.c
+++ b/drivers/spi/spi-adi-v3.c
@@ -1,7 +1,7 @@
 /*
  * Analog Devices SPI3 controller driver
  *
- * Copyright (c) 2013 Analog Devices Inc.
+ * Copyright (c) 2014 Analog Devices 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
@@ -13,6 +13,7 @@
  * GNU General Public License for more details.
  */
 
+#include <linux/clk.h>
 #include <linux/delay.h>
 #include <linux/device.h>
 #include <linux/dma-mapping.h>
@@ -26,35 +27,34 @@
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 #include <linux/spi/spi.h>
+#include <linux/spi/adi_spi3.h>
 #include <linux/types.h>
 
-#include <asm/bfin_spi3.h>
-#include <asm/cacheflush.h>
 #include <asm/dma.h>
 #include <asm/portmux.h>
 
-enum bfin_spi_state {
+enum adi_spi_state {
 	START_STATE,
 	RUNNING_STATE,
 	DONE_STATE,
 	ERROR_STATE
 };
 
-struct bfin_spi_master;
+struct adi_spi_master;
 
-struct bfin_spi_transfer_ops {
-	void (*write) (struct bfin_spi_master *);
-	void (*read) (struct bfin_spi_master *);
-	void (*duplex) (struct bfin_spi_master *);
+struct adi_spi_transfer_ops {
+	void (*write) (struct adi_spi_master *);
+	void (*read) (struct adi_spi_master *);
+	void (*duplex) (struct adi_spi_master *);
 };
 
 /* runtime info for spi master */
-struct bfin_spi_master {
+struct adi_spi_master {
 	/* SPI framework hookup */
 	struct spi_master *master;
 
 	/* Regs base of SPI controller */
-	struct bfin_spi_regs __iomem *regs;
+	struct adi_spi_regs __iomem *regs;
 
 	/* Pin request list */
 	u16 *pin_req;
@@ -65,7 +65,7 @@ struct bfin_spi_master {
 	/* Current message transfer state info */
 	struct spi_message *cur_msg;
 	struct spi_transfer *cur_transfer;
-	struct bfin_spi_device *cur_chip;
+	struct adi_spi_device *cur_chip;
 	unsigned transfer_len;
 
 	/* transfer buffer */
@@ -90,12 +90,12 @@ struct bfin_spi_master {
 	u32 ssel;
 
 	unsigned long sclk;
-	enum bfin_spi_state state;
+	enum adi_spi_state state;
 
-	const struct bfin_spi_transfer_ops *ops;
+	const struct adi_spi_transfer_ops *ops;
 };
 
-struct bfin_spi_device {
+struct adi_spi_device {
 	u32 control;
 	u32 clock;
 	u32 ssel;
@@ -105,17 +105,25 @@ struct bfin_spi_device {
 	u32 cs_gpio;
 	u32 tx_dummy_val; /* tx value for rx only transfer */
 	bool enable_dma;
-	const struct bfin_spi_transfer_ops *ops;
+	const struct adi_spi_transfer_ops *ops;
 };
 
-static void bfin_spi_enable(struct bfin_spi_master *drv_data)
+static void adi_spi_enable(struct adi_spi_master *drv_data)
 {
-	bfin_write_or(&drv_data->regs->control, SPI_CTL_EN);
+	u32 ctl;
+
+	ctl = ioread32(&drv_data->regs->control);
+	ctl |= SPI_CTL_EN;
+	iowrite32(ctl, &drv_data->regs->control);
 }
 
-static void bfin_spi_disable(struct bfin_spi_master *drv_data)
+static void adi_spi_disable(struct adi_spi_master *drv_data)
 {
-	bfin_write_and(&drv_data->regs->control, ~SPI_CTL_EN);
+	u32 ctl;
+
+	ctl = ioread32(&drv_data->regs->control);
+	ctl &= ~SPI_CTL_EN;
+	iowrite32(ctl, &drv_data->regs->control);
 }
 
 /* Caculate the SPI_CLOCK register value based on input HZ */
@@ -128,35 +136,43 @@ static u32 hz_to_spi_clock(u32 sclk, u32 speed_hz)
 	return spi_clock;
 }
 
-static int bfin_spi_flush(struct bfin_spi_master *drv_data)
+static int adi_spi_flush(struct adi_spi_master *drv_data)
 {
 	unsigned long limit = loops_per_jiffy << 1;
 
 	/* wait for stop and clear stat */
-	while (!(bfin_read(&drv_data->regs->status) & SPI_STAT_SPIF) && --limit)
+	while (!(ioread32(&drv_data->regs->status) & SPI_STAT_SPIF) && --limit)
 		cpu_relax();
 
-	bfin_write(&drv_data->regs->status, 0xFFFFFFFF);
+	iowrite32(0xFFFFFFFF, &drv_data->regs->status);
 
 	return limit;
 }
 
 /* Chip select operation functions for cs_change flag */
-static void bfin_spi_cs_active(struct bfin_spi_master *drv_data, struct bfin_spi_device *chip)
+static void adi_spi_cs_active(struct adi_spi_master *drv_data, struct adi_spi_device *chip)
 {
-	if (likely(chip->cs < MAX_CTRL_CS))
-		bfin_write_and(&drv_data->regs->ssel, ~chip->ssel);
-	else
+	if (likely(chip->cs < MAX_CTRL_CS)) {
+		u32 reg;
+		reg = ioread32(&drv_data->regs->ssel);
+		reg &= ~chip->ssel;
+		iowrite32(reg, &drv_data->regs->ssel);
+	} else {
 		gpio_set_value(chip->cs_gpio, 0);
+	}
 }
 
-static void bfin_spi_cs_deactive(struct bfin_spi_master *drv_data,
-				struct bfin_spi_device *chip)
+static void adi_spi_cs_deactive(struct adi_spi_master *drv_data,
+				struct adi_spi_device *chip)
 {
-	if (likely(chip->cs < MAX_CTRL_CS))
-		bfin_write_or(&drv_data->regs->ssel, chip->ssel);
-	else
+	if (likely(chip->cs < MAX_CTRL_CS)) {
+		u32 reg;
+		reg = ioread32(&drv_data->regs->ssel);
+		reg |= chip->ssel;
+		iowrite32(reg, &drv_data->regs->ssel);
+	} else {
 		gpio_set_value(chip->cs_gpio, 1);
+	}
 
 	/* Move delay here for consistency */
 	if (chip->cs_chg_udelay)
@@ -164,187 +180,192 @@ static void bfin_spi_cs_deactive(struct bfin_spi_master *drv_data,
 }
 
 /* enable or disable the pin muxed by GPIO and SPI CS to work as SPI CS */
-static inline void bfin_spi_cs_enable(struct bfin_spi_master *drv_data,
-					struct bfin_spi_device *chip)
+static inline void adi_spi_cs_enable(struct adi_spi_master *drv_data,
+					struct adi_spi_device *chip)
 {
-	if (chip->cs < MAX_CTRL_CS)
-		bfin_write_or(&drv_data->regs->ssel, chip->ssel >> 8);
+	if (chip->cs < MAX_CTRL_CS) {
+		u32 reg;
+		reg = ioread32(&drv_data->regs->ssel);
+		reg |= chip->ssel >> 8;
+		iowrite32(reg, &drv_data->regs->ssel);
+	}
 }
 
-static inline void bfin_spi_cs_disable(struct bfin_spi_master *drv_data,
-					struct bfin_spi_device *chip)
+static inline void adi_spi_cs_disable(struct adi_spi_master *drv_data,
+					struct adi_spi_device *chip)
 {
-	if (chip->cs < MAX_CTRL_CS)
-		bfin_write_and(&drv_data->regs->ssel, ~(chip->ssel >> 8));
+	if (chip->cs < MAX_CTRL_CS) {
+		u32 reg;
+		reg = ioread32(&drv_data->regs->ssel);
+		reg &= ~(chip->ssel >> 8);
+		iowrite32(reg, &drv_data->regs->ssel);
+	}
 }
 
 /* stop controller and re-config current chip*/
-static void bfin_spi_restore_state(struct bfin_spi_master *drv_data)
+static void adi_spi_restore_state(struct adi_spi_master *drv_data)
 {
-	struct bfin_spi_device *chip = drv_data->cur_chip;
+	struct adi_spi_device *chip = drv_data->cur_chip;
 
 	/* Clear status and disable clock */
-	bfin_write(&drv_data->regs->status, 0xFFFFFFFF);
-	bfin_write(&drv_data->regs->rx_control, 0x0);
-	bfin_write(&drv_data->regs->tx_control, 0x0);
-	bfin_spi_disable(drv_data);
-
-	SSYNC();
+	iowrite32(0xFFFFFFFF, &drv_data->regs->status);
+	iowrite32(0x0, &drv_data->regs->rx_control);
+	iowrite32(0x0, &drv_data->regs->tx_control);
+	adi_spi_disable(drv_data);
 
 	/* Load the registers */
-	bfin_write(&drv_data->regs->control, chip->control);
-	bfin_write(&drv_data->regs->clock, chip->clock);
+	iowrite32(chip->control, &drv_data->regs->control);
+	iowrite32(chip->clock, &drv_data->regs->clock);
 
-	bfin_spi_enable(drv_data);
+	adi_spi_enable(drv_data);
 	drv_data->tx_num = drv_data->rx_num = 0;
 	/* we always choose tx transfer initiate */
-	bfin_write(&drv_data->regs->rx_control, SPI_RXCTL_REN);
-	bfin_write(&drv_data->regs->tx_control,
-			SPI_TXCTL_TEN | SPI_TXCTL_TTI);
-	bfin_spi_cs_active(drv_data, chip);
+	iowrite32(SPI_RXCTL_REN, &drv_data->regs->rx_control);
+	iowrite32(SPI_TXCTL_TEN | SPI_TXCTL_TTI, &drv_data->regs->tx_control);
+	adi_spi_cs_active(drv_data, chip);
 }
 
 /* discard invalid rx data and empty rfifo */
-static inline void dummy_read(struct bfin_spi_master *drv_data)
+static inline void dummy_read(struct adi_spi_master *drv_data)
 {
-	while (!(bfin_read(&drv_data->regs->status) & SPI_STAT_RFE))
-		bfin_read(&drv_data->regs->rfifo);
+	while (!(ioread32(&drv_data->regs->status) & SPI_STAT_RFE))
+		ioread32(&drv_data->regs->rfifo);
 }
 
-static void bfin_spi_u8_write(struct bfin_spi_master *drv_data)
+static void adi_spi_u8_write(struct adi_spi_master *drv_data)
 {
 	dummy_read(drv_data);
 	while (drv_data->tx < drv_data->tx_end) {
-		bfin_write(&drv_data->regs->tfifo, (*(u8 *)(drv_data->tx++)));
-		while (bfin_read(&drv_data->regs->status) & SPI_STAT_RFE)
+		iowrite32(*(u8 *)(drv_data->tx++), &drv_data->regs->tfifo);
+		while (ioread32(&drv_data->regs->status) & SPI_STAT_RFE)
 			cpu_relax();
-		bfin_read(&drv_data->regs->rfifo);
+		ioread32(&drv_data->regs->rfifo);
 	}
 }
 
-static void bfin_spi_u8_read(struct bfin_spi_master *drv_data)
+static void adi_spi_u8_read(struct adi_spi_master *drv_data)
 {
 	u32 tx_val = drv_data->cur_chip->tx_dummy_val;
 
 	dummy_read(drv_data);
 	while (drv_data->rx < drv_data->rx_end) {
-		bfin_write(&drv_data->regs->tfifo, tx_val);
-		while (bfin_read(&drv_data->regs->status) & SPI_STAT_RFE)
+		iowrite32(tx_val, &drv_data->regs->tfifo);
+		while (ioread32(&drv_data->regs->status) & SPI_STAT_RFE)
 			cpu_relax();
-		*(u8 *)(drv_data->rx++) = bfin_read(&drv_data->regs->rfifo);
+		*(u8 *)(drv_data->rx++) = ioread32(&drv_data->regs->rfifo);
 	}
 }
 
-static void bfin_spi_u8_duplex(struct bfin_spi_master *drv_data)
+static void adi_spi_u8_duplex(struct adi_spi_master *drv_data)
 {
 	dummy_read(drv_data);
 	while (drv_data->rx < drv_data->rx_end) {
-		bfin_write(&drv_data->regs->tfifo, (*(u8 *)(drv_data->tx++)));
-		while (bfin_read(&drv_data->regs->status) & SPI_STAT_RFE)
+		iowrite32(*(u8 *)(drv_data->tx++), &drv_data->regs->tfifo);
+		while (ioread32(&drv_data->regs->status) & SPI_STAT_RFE)
 			cpu_relax();
-		*(u8 *)(drv_data->rx++) = bfin_read(&drv_data->regs->rfifo);
+		*(u8 *)(drv_data->rx++) = ioread32(&drv_data->regs->rfifo);
 	}
 }
 
-static const struct bfin_spi_transfer_ops bfin_bfin_spi_transfer_ops_u8 = {
-	.write  = bfin_spi_u8_write,
-	.read   = bfin_spi_u8_read,
-	.duplex = bfin_spi_u8_duplex,
+static const struct adi_spi_transfer_ops adi_spi_transfer_ops_u8 = {
+	.write  = adi_spi_u8_write,
+	.read   = adi_spi_u8_read,
+	.duplex = adi_spi_u8_duplex,
 };
 
-static void bfin_spi_u16_write(struct bfin_spi_master *drv_data)
+static void adi_spi_u16_write(struct adi_spi_master *drv_data)
 {
 	dummy_read(drv_data);
 	while (drv_data->tx < drv_data->tx_end) {
-		bfin_write(&drv_data->regs->tfifo, (*(u16 *)drv_data->tx));
+		iowrite32(*(u16 *)drv_data->tx, &drv_data->regs->tfifo);
 		drv_data->tx += 2;
-		while (bfin_read(&drv_data->regs->status) & SPI_STAT_RFE)
+		while (ioread32(&drv_data->regs->status) & SPI_STAT_RFE)
 			cpu_relax();
-		bfin_read(&drv_data->regs->rfifo);
+		ioread32(&drv_data->regs->rfifo);
 	}
 }
 
-static void bfin_spi_u16_read(struct bfin_spi_master *drv_data)
+static void adi_spi_u16_read(struct adi_spi_master *drv_data)
 {
 	u32 tx_val = drv_data->cur_chip->tx_dummy_val;
 
 	dummy_read(drv_data);
 	while (drv_data->rx < drv_data->rx_end) {
-		bfin_write(&drv_data->regs->tfifo, tx_val);
-		while (bfin_read(&drv_data->regs->status) & SPI_STAT_RFE)
+		iowrite32(tx_val, &drv_data->regs->tfifo);
+		while (ioread32(&drv_data->regs->status) & SPI_STAT_RFE)
 			cpu_relax();
-		*(u16 *)drv_data->rx = bfin_read(&drv_data->regs->rfifo);
+		*(u16 *)drv_data->rx = ioread32(&drv_data->regs->rfifo);
 		drv_data->rx += 2;
 	}
 }
 
-static void bfin_spi_u16_duplex(struct bfin_spi_master *drv_data)
+static void adi_spi_u16_duplex(struct adi_spi_master *drv_data)
 {
 	dummy_read(drv_data);
 	while (drv_data->rx < drv_data->rx_end) {
-		bfin_write(&drv_data->regs->tfifo, (*(u16 *)drv_data->tx));
+		iowrite32(*(u16 *)drv_data->tx, &drv_data->regs->tfifo);
 		drv_data->tx += 2;
-		while (bfin_read(&drv_data->regs->status) & SPI_STAT_RFE)
+		while (ioread32(&drv_data->regs->status) & SPI_STAT_RFE)
 			cpu_relax();
-		*(u16 *)drv_data->rx = bfin_read(&drv_data->regs->rfifo);
+		*(u16 *)drv_data->rx = ioread32(&drv_data->regs->rfifo);
 		drv_data->rx += 2;
 	}
 }
 
-static const struct bfin_spi_transfer_ops bfin_bfin_spi_transfer_ops_u16 = {
-	.write  = bfin_spi_u16_write,
-	.read   = bfin_spi_u16_read,
-	.duplex = bfin_spi_u16_duplex,
+static const struct adi_spi_transfer_ops adi_spi_transfer_ops_u16 = {
+	.write  = adi_spi_u16_write,
+	.read   = adi_spi_u16_read,
+	.duplex = adi_spi_u16_duplex,
 };
 
-static void bfin_spi_u32_write(struct bfin_spi_master *drv_data)
+static void adi_spi_u32_write(struct adi_spi_master *drv_data)
 {
 	dummy_read(drv_data);
 	while (drv_data->tx < drv_data->tx_end) {
-		bfin_write(&drv_data->regs->tfifo, (*(u32 *)drv_data->tx));
+		iowrite32(*(u32 *)drv_data->tx, &drv_data->regs->tfifo);
 		drv_data->tx += 4;
-		while (bfin_read(&drv_data->regs->status) & SPI_STAT_RFE)
+		while (ioread32(&drv_data->regs->status) & SPI_STAT_RFE)
 			cpu_relax();
-		bfin_read(&drv_data->regs->rfifo);
+		ioread32(&drv_data->regs->rfifo);
 	}
 }
 
-static void bfin_spi_u32_read(struct bfin_spi_master *drv_data)
+static void adi_spi_u32_read(struct adi_spi_master *drv_data)
 {
 	u32 tx_val = drv_data->cur_chip->tx_dummy_val;
 
 	dummy_read(drv_data);
 	while (drv_data->rx < drv_data->rx_end) {
-		bfin_write(&drv_data->regs->tfifo, tx_val);
-		while (bfin_read(&drv_data->regs->status) & SPI_STAT_RFE)
+		iowrite32(tx_val, &drv_data->regs->tfifo);
+		while (ioread32(&drv_data->regs->status) & SPI_STAT_RFE)
 			cpu_relax();
-		*(u32 *)drv_data->rx = bfin_read(&drv_data->regs->rfifo);
+		*(u32 *)drv_data->rx = ioread32(&drv_data->regs->rfifo);
 		drv_data->rx += 4;
 	}
 }
 
-static void bfin_spi_u32_duplex(struct bfin_spi_master *drv_data)
+static void adi_spi_u32_duplex(struct adi_spi_master *drv_data)
 {
 	dummy_read(drv_data);
 	while (drv_data->rx < drv_data->rx_end) {
-		bfin_write(&drv_data->regs->tfifo, (*(u32 *)drv_data->tx));
+		iowrite32(*(u32 *)drv_data->tx, &drv_data->regs->tfifo);
 		drv_data->tx += 4;
-		while (bfin_read(&drv_data->regs->status) & SPI_STAT_RFE)
+		while (ioread32(&drv_data->regs->status) & SPI_STAT_RFE)
 			cpu_relax();
-		*(u32 *)drv_data->rx = bfin_read(&drv_data->regs->rfifo);
+		*(u32 *)drv_data->rx = ioread32(&drv_data->regs->rfifo);
 		drv_data->rx += 4;
 	}
 }
 
-static const struct bfin_spi_transfer_ops bfin_bfin_spi_transfer_ops_u32 = {
-	.write  = bfin_spi_u32_write,
-	.read   = bfin_spi_u32_read,
-	.duplex = bfin_spi_u32_duplex,
+static const struct adi_spi_transfer_ops adi_spi_transfer_ops_u32 = {
+	.write  = adi_spi_u32_write,
+	.read   = adi_spi_u32_read,
+	.duplex = adi_spi_u32_duplex,
 };
 
 
 /* test if there is more transfer to be done */
-static void bfin_spi_next_transfer(struct bfin_spi_master *drv)
+static void adi_spi_next_transfer(struct adi_spi_master *drv)
 {
 	struct spi_message *msg = drv->cur_msg;
 	struct spi_transfer *t = drv->cur_transfer;
@@ -360,15 +381,15 @@ static void bfin_spi_next_transfer(struct bfin_spi_master *drv)
 	}
 }
 
-static void bfin_spi_giveback(struct bfin_spi_master *drv_data)
+static void adi_spi_giveback(struct adi_spi_master *drv_data)
 {
-	struct bfin_spi_device *chip = drv_data->cur_chip;
+	struct adi_spi_device *chip = drv_data->cur_chip;
 
-	bfin_spi_cs_deactive(drv_data, chip);
+	adi_spi_cs_deactive(drv_data, chip);
 	spi_finalize_current_message(drv_data->master);
 }
 
-static int bfin_spi_setup_transfer(struct bfin_spi_master *drv)
+static int adi_spi_setup_transfer(struct adi_spi_master *drv)
 {
 	struct spi_transfer *t = drv->cur_transfer;
 	u32 cr, cr_width;
@@ -393,34 +414,33 @@ static int bfin_spi_setup_transfer(struct bfin_spi_master *drv)
 	switch (t->bits_per_word) {
 	case 8:
 		cr_width = SPI_CTL_SIZE08;
-		drv->ops = &bfin_bfin_spi_transfer_ops_u8;
+		drv->ops = &adi_spi_transfer_ops_u8;
 		break;
 	case 16:
 		cr_width = SPI_CTL_SIZE16;
-		drv->ops = &bfin_bfin_spi_transfer_ops_u16;
+		drv->ops = &adi_spi_transfer_ops_u16;
 		break;
 	case 32:
 		cr_width = SPI_CTL_SIZE32;
-		drv->ops = &bfin_bfin_spi_transfer_ops_u32;
+		drv->ops = &adi_spi_transfer_ops_u32;
 		break;
 	default:
 		return -EINVAL;
 	}
-	cr = bfin_read(&drv->regs->control) & ~SPI_CTL_SIZE;
+	cr = ioread32(&drv->regs->control) & ~SPI_CTL_SIZE;
 	cr |= cr_width;
-	bfin_write(&drv->regs->control, cr);
+	iowrite32(cr, &drv->regs->control);
 
 	/* speed setup */
-	bfin_write(&drv->regs->clock,
-			hz_to_spi_clock(drv->sclk, t->speed_hz));
+	iowrite32(hz_to_spi_clock(drv->sclk, t->speed_hz), &drv->regs->clock);
 	return 0;
 }
 
-static int bfin_spi_dma_xfer(struct bfin_spi_master *drv_data)
+static int adi_spi_dma_xfer(struct adi_spi_master *drv_data)
 {
 	struct spi_transfer *t = drv_data->cur_transfer;
 	struct spi_message *msg = drv_data->cur_msg;
-	struct bfin_spi_device *chip = drv_data->cur_chip;
+	struct adi_spi_device *chip = drv_data->cur_chip;
 	u32 dma_config;
 	unsigned long word_count, word_size;
 	void *tx_buf, *rx_buf;
@@ -498,17 +518,16 @@ static int bfin_spi_dma_xfer(struct bfin_spi_master *drv_data)
 	set_dma_config(drv_data->rx_dma, dma_config | WNR);
 	enable_dma(drv_data->tx_dma);
 	enable_dma(drv_data->rx_dma);
-	SSYNC();
 
-	bfin_write(&drv_data->regs->rx_control, SPI_RXCTL_REN | SPI_RXCTL_RDR_NE);
-	SSYNC();
-	bfin_write(&drv_data->regs->tx_control,
-			SPI_TXCTL_TEN | SPI_TXCTL_TTI | SPI_TXCTL_TDR_NF);
+	iowrite32(SPI_RXCTL_REN | SPI_RXCTL_RDR_NE,
+			&drv_data->regs->rx_control);
+	iowrite32(SPI_TXCTL_TEN | SPI_TXCTL_TTI | SPI_TXCTL_TDR_NF,
+			&drv_data->regs->tx_control);
 
 	return 0;
 }
 
-static int bfin_spi_pio_xfer(struct bfin_spi_master *drv_data)
+static int adi_spi_pio_xfer(struct adi_spi_master *drv_data)
 {
 	struct spi_message *msg = drv_data->cur_msg;
 
@@ -529,19 +548,19 @@ static int bfin_spi_pio_xfer(struct bfin_spi_master *drv_data)
 			return -EIO;
 	}
 
-	if (!bfin_spi_flush(drv_data))
+	if (!adi_spi_flush(drv_data))
 		return -EIO;
 	msg->actual_length += drv_data->transfer_len;
 	tasklet_schedule(&drv_data->pump_transfers);
 	return 0;
 }
 
-static void bfin_spi_pump_transfers(unsigned long data)
+static void adi_spi_pump_transfers(unsigned long data)
 {
-	struct bfin_spi_master *drv_data = (struct bfin_spi_master *)data;
+	struct adi_spi_master *drv_data = (struct adi_spi_master *)data;
 	struct spi_message *msg = NULL;
 	struct spi_transfer *t = NULL;
-	struct bfin_spi_device *chip = NULL;
+	struct adi_spi_device *chip = NULL;
 	int ret;
 
 	/* Get current state information */
@@ -552,7 +571,7 @@ static void bfin_spi_pump_transfers(unsigned long data)
 	/* Handle for abort */
 	if (drv_data->state == ERROR_STATE) {
 		msg->status = -EIO;
-		bfin_spi_giveback(drv_data);
+		adi_spi_giveback(drv_data);
 		return;
 	}
 
@@ -560,14 +579,14 @@ static void bfin_spi_pump_transfers(unsigned long data)
 		if (t->delay_usecs)
 			udelay(t->delay_usecs);
 		if (t->cs_change)
-			bfin_spi_cs_deactive(drv_data, chip);
-		bfin_spi_next_transfer(drv_data);
+			adi_spi_cs_deactive(drv_data, chip);
+		adi_spi_next_transfer(drv_data);
 		t = drv_data->cur_transfer;
 	}
 	/* Handle end of message */
 	if (drv_data->state == DONE_STATE) {
 		msg->status = 0;
-		bfin_spi_giveback(drv_data);
+		adi_spi_giveback(drv_data);
 		return;
 	}
 
@@ -577,34 +596,34 @@ static void bfin_spi_pump_transfers(unsigned long data)
 		return;
 	}
 
-	ret = bfin_spi_setup_transfer(drv_data);
+	ret = adi_spi_setup_transfer(drv_data);
 	if (ret) {
 		msg->status = ret;
-		bfin_spi_giveback(drv_data);
+		adi_spi_giveback(drv_data);
 	}
 
-	bfin_write(&drv_data->regs->status, 0xFFFFFFFF);
-	bfin_spi_cs_active(drv_data, chip);
+	iowrite32(0xFFFFFFFF, &drv_data->regs->status);
+	adi_spi_cs_active(drv_data, chip);
 	drv_data->state = RUNNING_STATE;
 
 	if (chip->enable_dma)
-		ret = bfin_spi_dma_xfer(drv_data);
+		ret = adi_spi_dma_xfer(drv_data);
 	else
-		ret = bfin_spi_pio_xfer(drv_data);
+		ret = adi_spi_pio_xfer(drv_data);
 	if (ret) {
 		msg->status = ret;
-		bfin_spi_giveback(drv_data);
+		adi_spi_giveback(drv_data);
 	}
 }
 
-static int bfin_spi_transfer_one_message(struct spi_master *master,
+static int adi_spi_transfer_one_message(struct spi_master *master,
 					struct spi_message *m)
 {
-	struct bfin_spi_master *drv_data = spi_master_get_devdata(master);
+	struct adi_spi_master *drv_data = spi_master_get_devdata(master);
 
 	drv_data->cur_msg = m;
 	drv_data->cur_chip = spi_get_ctldata(drv_data->cur_msg->spi);
-	bfin_spi_restore_state(drv_data);
+	adi_spi_restore_state(drv_data);
 
 	drv_data->state = START_STATE;
 	drv_data->cur_transfer = list_entry(drv_data->cur_msg->transfers.next,
@@ -630,15 +649,15 @@ static const u16 ssel[][MAX_SPI_SSEL] = {
 	P_SPI2_SSEL6, P_SPI2_SSEL7},
 };
 
-static int bfin_spi_setup(struct spi_device *spi)
+static int adi_spi_setup(struct spi_device *spi)
 {
-	struct bfin_spi_master *drv_data = spi_master_get_devdata(spi->master);
-	struct bfin_spi_device *chip = spi_get_ctldata(spi);
-	u32 bfin_ctl_reg = SPI_CTL_ODM | SPI_CTL_PSSE;
+	struct adi_spi_master *drv_data = spi_master_get_devdata(spi->master);
+	struct adi_spi_device *chip = spi_get_ctldata(spi);
+	u32 ctl_reg = SPI_CTL_ODM | SPI_CTL_PSSE;
 	int ret = -EINVAL;
 
 	if (!chip) {
-		struct bfin_spi3_chip *chip_info = spi->controller_data;
+		struct adi_spi3_chip *chip_info = spi->controller_data;
 
 		chip = kzalloc(sizeof(*chip), GFP_KERNEL);
 		if (!chip) {
@@ -646,7 +665,7 @@ static int bfin_spi_setup(struct spi_device *spi)
 			return -ENOMEM;
 		}
 		if (chip_info) {
-			if (chip_info->control & ~bfin_ctl_reg) {
+			if (chip_info->control & ~ctl_reg) {
 				dev_err(&spi->dev,
 					"do not set bits that the SPI framework manages\n");
 				goto error;
@@ -657,6 +676,7 @@ static int bfin_spi_setup(struct spi_device *spi)
 			chip->enable_dma = chip_info->enable_dma;
 		}
 		chip->cs = spi->chip_select;
+
 		if (chip->cs < MAX_CTRL_CS) {
 			chip->ssel = (1 << chip->cs) << 8;
 			ret = peripheral_request(ssel[spi->master->bus_num]
@@ -678,7 +698,7 @@ static int bfin_spi_setup(struct spi_device *spi)
 	}
 
 	/* force a default base state */
-	chip->control &= bfin_ctl_reg;
+	chip->control &= ctl_reg;
 
 	if (spi->mode & SPI_CPOL)
 		chip->control |= SPI_CTL_CPOL;
@@ -692,8 +712,8 @@ static int bfin_spi_setup(struct spi_device *spi)
 
 	chip->clock = hz_to_spi_clock(drv_data->sclk, spi->max_speed_hz);
 
-	bfin_spi_cs_enable(drv_data, chip);
-	bfin_spi_cs_deactive(drv_data, chip);
+	adi_spi_cs_enable(drv_data, chip);
+	adi_spi_cs_deactive(drv_data, chip);
 
 	return 0;
 error:
@@ -705,10 +725,10 @@ error:
 	return ret;
 }
 
-static void bfin_spi_cleanup(struct spi_device *spi)
+static void adi_spi_cleanup(struct spi_device *spi)
 {
-	struct bfin_spi_device *chip = spi_get_ctldata(spi);
-	struct bfin_spi_master *drv_data = spi_master_get_devdata(spi->master);
+	struct adi_spi_device *chip = spi_get_ctldata(spi);
+	struct adi_spi_master *drv_data = spi_master_get_devdata(spi->master);
 
 	if (!chip)
 		return;
@@ -716,7 +736,7 @@ static void bfin_spi_cleanup(struct spi_device *spi)
 	if (chip->cs < MAX_CTRL_CS) {
 		peripheral_free(ssel[spi->master->bus_num]
 					[chip->cs-1]);
-		bfin_spi_cs_disable(drv_data, chip);
+		adi_spi_cs_disable(drv_data, chip);
 	} else {
 		gpio_free(chip->cs_gpio);
 	}
@@ -725,10 +745,11 @@ static void bfin_spi_cleanup(struct spi_device *spi)
 	spi_set_ctldata(spi, NULL);
 }
 
-static irqreturn_t bfin_spi_tx_dma_isr(int irq, void *dev_id)
+static irqreturn_t adi_spi_tx_dma_isr(int irq, void *dev_id)
 {
-	struct bfin_spi_master *drv_data = dev_id;
+	struct adi_spi_master *drv_data = dev_id;
 	u32 dma_stat = get_dma_curr_irqstat(drv_data->tx_dma);
+	u32 tx_ctl;
 
 	clear_dma_irqstat(drv_data->tx_dma);
 	if (dma_stat & DMA_DONE) {
@@ -739,13 +760,15 @@ static irqreturn_t bfin_spi_tx_dma_isr(int irq, void *dev_id)
 		if (drv_data->tx)
 			drv_data->state = ERROR_STATE;
 	}
-	bfin_write_and(&drv_data->regs->tx_control, ~SPI_TXCTL_TDR_NF);
+	tx_ctl = ioread32(&drv_data->regs->tx_control);
+	tx_ctl &= ~SPI_TXCTL_TDR_NF;
+	iowrite32(tx_ctl, &drv_data->regs->tx_control);
 	return IRQ_HANDLED;
 }
 
-static irqreturn_t bfin_spi_rx_dma_isr(int irq, void *dev_id)
+static irqreturn_t adi_spi_rx_dma_isr(int irq, void *dev_id)
 {
-	struct bfin_spi_master *drv_data = dev_id;
+	struct adi_spi_master *drv_data = dev_id;
 	struct spi_message *msg = drv_data->cur_msg;
 	u32 dma_stat = get_dma_curr_irqstat(drv_data->rx_dma);
 
@@ -760,8 +783,8 @@ static irqreturn_t bfin_spi_rx_dma_isr(int irq, void *dev_id)
 		dev_err(&drv_data->master->dev,
 				"spi rx dma error: %d\n", dma_stat);
 	}
-	bfin_write(&drv_data->regs->tx_control, 0);
-	bfin_write(&drv_data->regs->rx_control, 0);
+	iowrite32(0, &drv_data->regs->tx_control);
+	iowrite32(0, &drv_data->regs->rx_control);
 	if (drv_data->rx_num != drv_data->tx_num)
 		dev_dbg(&drv_data->master->dev,
 				"dma interrupt missing: tx=%d,rx=%d\n",
@@ -770,15 +793,15 @@ static irqreturn_t bfin_spi_rx_dma_isr(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static int bfin_spi_probe(struct platform_device *pdev)
+static int adi_spi_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
-	struct bfin_spi3_master *info = dev_get_platdata(dev);
+	struct adi_spi3_master *info = dev_get_platdata(dev);
 	struct spi_master *master;
-	struct bfin_spi_master *drv_data;
+	struct adi_spi_master *drv_data;
 	struct resource *mem, *res;
 	unsigned int tx_dma, rx_dma;
-	unsigned long sclk;
+	struct clk *sclk;
 	int ret;
 
 	if (!info) {
@@ -786,10 +809,10 @@ static int bfin_spi_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	sclk = get_sclk1();
-	if (!sclk) {
-		dev_err(dev, "can not get sclk1\n");
-		return -ENXIO;
+	sclk = devm_clk_get(dev, "spi");
+	if (IS_ERR(sclk)) {
+		dev_err(dev, "can not get spi clock\n");
+		return PTR_ERR(sclk);
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_DMA, 0);
@@ -819,9 +842,9 @@ static int bfin_spi_probe(struct platform_device *pdev)
 
 	master->bus_num = pdev->id;
 	master->num_chipselect = info->num_chipselect;
-	master->cleanup = bfin_spi_cleanup;
-	master->setup = bfin_spi_setup;
-	master->transfer_one_message = bfin_spi_transfer_one_message;
+	master->cleanup = adi_spi_cleanup;
+	master->setup = adi_spi_setup;
+	master->transfer_one_message = adi_spi_transfer_one_message;
 	master->bits_per_word_mask = SPI_BPW_MASK(32) | SPI_BPW_MASK(16) |
 				     SPI_BPW_MASK(8);
 
@@ -830,7 +853,7 @@ static int bfin_spi_probe(struct platform_device *pdev)
 	drv_data->tx_dma = tx_dma;
 	drv_data->rx_dma = rx_dma;
 	drv_data->pin_req = info->pin_req;
-	drv_data->sclk = sclk;
+	drv_data->sclk = clk_get_rate(sclk);
 
 	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	drv_data->regs = devm_ioremap_resource(dev, mem);
@@ -845,28 +868,28 @@ static int bfin_spi_probe(struct platform_device *pdev)
 		dev_err(dev, "can not request SPI TX DMA channel\n");
 		goto err_put_master;
 	}
-	set_dma_callback(tx_dma, bfin_spi_tx_dma_isr, drv_data);
+	set_dma_callback(tx_dma, adi_spi_tx_dma_isr, drv_data);
 
 	ret = request_dma(rx_dma, "SPI_RX_DMA");
 	if (ret) {
 		dev_err(dev, "can not request SPI RX DMA channel\n");
 		goto err_free_tx_dma;
 	}
-	set_dma_callback(drv_data->rx_dma, bfin_spi_rx_dma_isr, drv_data);
+	set_dma_callback(drv_data->rx_dma, adi_spi_rx_dma_isr, drv_data);
 
 	/* request CLK, MOSI and MISO */
-	ret = peripheral_request_list(drv_data->pin_req, "bfin-spi3");
+	ret = peripheral_request_list(drv_data->pin_req, "adi-spi3");
 	if (ret < 0) {
 		dev_err(dev, "can not request spi pins\n");
 		goto err_free_rx_dma;
 	}
 
-	bfin_write(&drv_data->regs->control, SPI_CTL_MSTR | SPI_CTL_CPHA);
-	bfin_write(&drv_data->regs->ssel, 0x0000FE00);
-	bfin_write(&drv_data->regs->delay, 0x0);
+	iowrite32(SPI_CTL_MSTR | SPI_CTL_CPHA, &drv_data->regs->control);
+	iowrite32(0x0000FE00, &drv_data->regs->ssel);
+	iowrite32(0x0, &drv_data->regs->delay);
 
 	tasklet_init(&drv_data->pump_transfers,
-			bfin_spi_pump_transfers, (unsigned long)drv_data);
+			adi_spi_pump_transfers, (unsigned long)drv_data);
 	/* register with the SPI framework */
 	ret = devm_spi_register_master(dev, master);
 	if (ret) {
@@ -888,43 +911,41 @@ err_put_master:
 	return ret;
 }
 
-static int bfin_spi_remove(struct platform_device *pdev)
+static int adi_spi_remove(struct platform_device *pdev)
 {
 	struct spi_master *master = platform_get_drvdata(pdev);
-	struct bfin_spi_master *drv_data = spi_master_get_devdata(master);
-
-	bfin_spi_disable(drv_data);
+	struct adi_spi_master *drv_data = spi_master_get_devdata(master);
 
+	adi_spi_disable(drv_data);
 	peripheral_free_list(drv_data->pin_req);
 	free_dma(drv_data->rx_dma);
 	free_dma(drv_data->tx_dma);
-
 	return 0;
 }
 
 #ifdef CONFIG_PM
-static int bfin_spi_suspend(struct device *dev)
+static int adi_spi_suspend(struct device *dev)
 {
 	struct spi_master *master = dev_get_drvdata(dev);
-	struct bfin_spi_master *drv_data = spi_master_get_devdata(master);
+	struct adi_spi_master *drv_data = spi_master_get_devdata(master);
 
 	spi_master_suspend(master);
 
-	drv_data->control = bfin_read(&drv_data->regs->control);
-	drv_data->ssel = bfin_read(&drv_data->regs->ssel);
+	drv_data->control = ioread32(&drv_data->regs->control);
+	drv_data->ssel = ioread32(&drv_data->regs->ssel);
 
-	bfin_write(&drv_data->regs->control, SPI_CTL_MSTR | SPI_CTL_CPHA);
-	bfin_write(&drv_data->regs->ssel, 0x0000FE00);
+	iowrite32(SPI_CTL_MSTR | SPI_CTL_CPHA, &drv_data->regs->control);
+	iowrite32(0x0000FE00, &drv_data->regs->ssel);
 	dma_disable_irq(drv_data->rx_dma);
 	dma_disable_irq(drv_data->tx_dma);
 
 	return 0;
 }
 
-static int bfin_spi_resume(struct device *dev)
+static int adi_spi_resume(struct device *dev)
 {
 	struct spi_master *master = dev_get_drvdata(dev);
-	struct bfin_spi_master *drv_data = spi_master_get_devdata(master);
+	struct adi_spi_master *drv_data = spi_master_get_devdata(master);
 	int ret = 0;
 
 	/* bootrom may modify spi and dma status when resume in spi boot mode */
@@ -932,8 +953,8 @@ static int bfin_spi_resume(struct device *dev)
 
 	dma_enable_irq(drv_data->rx_dma);
 	dma_enable_irq(drv_data->tx_dma);
-	bfin_write(&drv_data->regs->control, drv_data->control);
-	bfin_write(&drv_data->regs->ssel, drv_data->ssel);
+	iowrite32(drv_data->control, &drv_data->regs->control);
+	iowrite32(drv_data->ssel, &drv_data->regs->ssel);
 
 	ret = spi_master_resume(master);
 	if (ret) {
@@ -944,21 +965,21 @@ static int bfin_spi_resume(struct device *dev)
 	return ret;
 }
 #endif
-static const struct dev_pm_ops bfin_spi_pm_ops = {
-	SET_SYSTEM_SLEEP_PM_OPS(bfin_spi_suspend, bfin_spi_resume)
+static const struct dev_pm_ops adi_spi_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(adi_spi_suspend, adi_spi_resume)
 };
 
-MODULE_ALIAS("platform:bfin-spi3");
-static struct platform_driver bfin_spi_driver = {
+MODULE_ALIAS("platform:adi-spi3");
+static struct platform_driver adi_spi_driver = {
 	.driver	= {
-		.name	= "bfin-spi3",
+		.name	= "adi-spi3",
 		.owner	= THIS_MODULE,
-		.pm     = &bfin_spi_pm_ops,
+		.pm     = &adi_spi_pm_ops,
 	},
-	.remove		= bfin_spi_remove,
+	.remove		= adi_spi_remove,
 };
 
-module_platform_driver_probe(bfin_spi_driver, bfin_spi_probe);
+module_platform_driver_probe(adi_spi_driver, adi_spi_probe);
 
 MODULE_DESCRIPTION("Analog Devices SPI3 controller driver");
 MODULE_AUTHOR("Scott Jiang <Scott.Jiang.Linux@gmail.com>");
diff --git a/drivers/spi/spi-ath79.c b/drivers/spi/spi-ath79.c
index 3898b0b9ee7..058db0fe8dc 100644
--- a/drivers/spi/spi-ath79.c
+++ b/drivers/spi/spi-ath79.c
@@ -16,7 +16,6 @@
 #include <linux/module.h>
 #include <linux/delay.h>
 #include <linux/spinlock.h>
-#include <linux/workqueue.h>
 #include <linux/platform_device.h>
 #include <linux/io.h>
 #include <linux/spi/spi.h>
diff --git a/drivers/spi/spi-atmel.c b/drivers/spi/spi-atmel.c
index 8005f986948..92a6f0d9323 100644
--- a/drivers/spi/spi-atmel.c
+++ b/drivers/spi/spi-atmel.c
@@ -224,7 +224,7 @@ struct atmel_spi {
 	struct platform_device	*pdev;
 
 	struct spi_transfer	*current_transfer;
-	unsigned long		current_remaining_bytes;
+	int			current_remaining_bytes;
 	int			done_status;
 
 	struct completion	xfer_completion;
@@ -874,8 +874,9 @@ atmel_spi_pump_pio_data(struct atmel_spi *as, struct spi_transfer *xfer)
 		spi_readl(as, RDR);
 	}
 	if (xfer->bits_per_word > 8) {
-		as->current_remaining_bytes -= 2;
-		if (as->current_remaining_bytes < 0)
+		if (as->current_remaining_bytes > 2)
+			as->current_remaining_bytes -= 2;
+		else
 			as->current_remaining_bytes = 0;
 	} else {
 		as->current_remaining_bytes--;
@@ -1110,13 +1111,18 @@ static int atmel_spi_one_transfer(struct spi_master *master,
 				atmel_spi_next_xfer_pio(master, xfer);
 			} else {
 				as->current_remaining_bytes -= len;
+				if (as->current_remaining_bytes < 0)
+					as->current_remaining_bytes = 0;
 			}
 		} else {
 			atmel_spi_next_xfer_pio(master, xfer);
 		}
 
+		/* interrupts are disabled, so free the lock for schedule */
+		atmel_spi_unlock(as);
 		ret = wait_for_completion_timeout(&as->xfer_completion,
 							SPI_DMA_TIMEOUT);
+		atmel_spi_lock(as);
 		if (WARN_ON(ret == 0)) {
 			dev_err(&spi->dev,
 				"spi trasfer timeout, err %d\n", ret);
diff --git a/drivers/spi/spi-bcm63xx-hsspi.c b/drivers/spi/spi-bcm63xx-hsspi.c
index 5a211e98383..86f5a98aa7a 100644
--- a/drivers/spi/spi-bcm63xx-hsspi.c
+++ b/drivers/spi/spi-bcm63xx-hsspi.c
@@ -18,7 +18,6 @@
 #include <linux/err.h>
 #include <linux/interrupt.h>
 #include <linux/spi/spi.h>
-#include <linux/workqueue.h>
 #include <linux/mutex.h>
 
 #define HSSPI_GLOBAL_CTRL_REG			0x0
diff --git a/drivers/spi/spi-bcm63xx.c b/drivers/spi/spi-bcm63xx.c
index 0250fa721ce..8510400e786 100644
--- a/drivers/spi/spi-bcm63xx.c
+++ b/drivers/spi/spi-bcm63xx.c
@@ -29,7 +29,6 @@
 #include <linux/spi/spi.h>
 #include <linux/completion.h>
 #include <linux/err.h>
-#include <linux/workqueue.h>
 #include <linux/pm_runtime.h>
 
 #include <bcm63xx_dev_spi.h>
diff --git a/drivers/spi/spi-bfin5xx.c b/drivers/spi/spi-bfin5xx.c
index 55e57c3eb9b..ebf720b88a2 100644
--- a/drivers/spi/spi-bfin5xx.c
+++ b/drivers/spi/spi-bfin5xx.c
@@ -12,6 +12,7 @@
 #include <linux/module.h>
 #include <linux/delay.h>
 #include <linux/device.h>
+#include <linux/gpio.h>
 #include <linux/slab.h>
 #include <linux/io.h>
 #include <linux/ioport.h>
diff --git a/drivers/spi/spi-cadence.c b/drivers/spi/spi-cadence.c
new file mode 100644
index 00000000000..bb758978465
--- /dev/null
+++ b/drivers/spi/spi-cadence.c
@@ -0,0 +1,673 @@
+/*
+ * Cadence SPI controller driver (master mode only)
+ *
+ * Copyright (C) 2008 - 2014 Xilinx, Inc.
+ *
+ * based on Blackfin On-Chip SPI Driver (spi_bfin5xx.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 published by the
+ * Free Software Foundation; either version 2 of the License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of_irq.h>
+#include <linux/of_address.h>
+#include <linux/platform_device.h>
+#include <linux/spi/spi.h>
+
+/* Name of this driver */
+#define CDNS_SPI_NAME		"cdns-spi"
+
+/* Register offset definitions */
+#define CDNS_SPI_CR_OFFSET	0x00 /* Configuration  Register, RW */
+#define CDNS_SPI_ISR_OFFSET	0x04 /* Interrupt Status Register, RO */
+#define CDNS_SPI_IER_OFFSET	0x08 /* Interrupt Enable Register, WO */
+#define CDNS_SPI_IDR_OFFSET	0x0c /* Interrupt Disable Register, WO */
+#define CDNS_SPI_IMR_OFFSET	0x10 /* Interrupt Enabled Mask Register, RO */
+#define CDNS_SPI_ER_OFFSET	0x14 /* Enable/Disable Register, RW */
+#define CDNS_SPI_DR_OFFSET	0x18 /* Delay Register, RW */
+#define CDNS_SPI_TXD_OFFSET	0x1C /* Data Transmit Register, WO */
+#define CDNS_SPI_RXD_OFFSET	0x20 /* Data Receive Register, RO */
+#define CDNS_SPI_SICR_OFFSET	0x24 /* Slave Idle Count Register, RW */
+#define CDNS_SPI_THLD_OFFSET	0x28 /* Transmit FIFO Watermark Register,RW */
+
+/*
+ * SPI Configuration Register bit Masks
+ *
+ * This register contains various control bits that affect the operation
+ * of the SPI controller
+ */
+#define CDNS_SPI_CR_MANSTRT_MASK	0x00010000 /* Manual TX Start */
+#define CDNS_SPI_CR_CPHA_MASK		0x00000004 /* Clock Phase Control */
+#define CDNS_SPI_CR_CPOL_MASK		0x00000002 /* Clock Polarity Control */
+#define CDNS_SPI_CR_SSCTRL_MASK		0x00003C00 /* Slave Select Mask */
+#define CDNS_SPI_CR_BAUD_DIV_MASK	0x00000038 /* Baud Rate Divisor Mask */
+#define CDNS_SPI_CR_MSTREN_MASK		0x00000001 /* Master Enable Mask */
+#define CDNS_SPI_CR_MANSTRTEN_MASK	0x00008000 /* Manual TX Enable Mask */
+#define CDNS_SPI_CR_SSFORCE_MASK	0x00004000 /* Manual SS Enable Mask */
+#define CDNS_SPI_CR_BAUD_DIV_4_MASK	0x00000008 /* Default Baud Div Mask */
+#define CDNS_SPI_CR_DEFAULT_MASK	(CDNS_SPI_CR_MSTREN_MASK | \
+					CDNS_SPI_CR_SSCTRL_MASK | \
+					CDNS_SPI_CR_SSFORCE_MASK | \
+					CDNS_SPI_CR_BAUD_DIV_4_MASK)
+
+/*
+ * SPI Configuration Register - Baud rate and slave select
+ *
+ * These are the values used in the calculation of baud rate divisor and
+ * setting the slave select.
+ */
+
+#define CDNS_SPI_BAUD_DIV_MAX		7 /* Baud rate divisor maximum */
+#define CDNS_SPI_BAUD_DIV_MIN		1 /* Baud rate divisor minimum */
+#define CDNS_SPI_BAUD_DIV_SHIFT		3 /* Baud rate divisor shift in CR */
+#define CDNS_SPI_SS_SHIFT		10 /* Slave Select field shift in CR */
+#define CDNS_SPI_SS0			0x1 /* Slave Select zero */
+
+/*
+ * SPI Interrupt Registers bit Masks
+ *
+ * All the four interrupt registers (Status/Mask/Enable/Disable) have the same
+ * bit definitions.
+ */
+#define CDNS_SPI_IXR_TXOW_MASK	0x00000004 /* SPI TX FIFO Overwater */
+#define CDNS_SPI_IXR_MODF_MASK	0x00000002 /* SPI Mode Fault */
+#define CDNS_SPI_IXR_RXNEMTY_MASK 0x00000010 /* SPI RX FIFO Not Empty */
+#define CDNS_SPI_IXR_DEFAULT_MASK	(CDNS_SPI_IXR_TXOW_MASK | \
+					CDNS_SPI_IXR_MODF_MASK)
+#define CDNS_SPI_IXR_TXFULL_MASK	0x00000008 /* SPI TX Full */
+#define CDNS_SPI_IXR_ALL_MASK	0x0000007F /* SPI all interrupts */
+
+/*
+ * SPI Enable Register bit Masks
+ *
+ * This register is used to enable or disable the SPI controller
+ */
+#define CDNS_SPI_ER_ENABLE_MASK	0x00000001 /* SPI Enable Bit Mask */
+#define CDNS_SPI_ER_DISABLE_MASK	0x0 /* SPI Disable Bit Mask */
+
+/* SPI FIFO depth in bytes */
+#define CDNS_SPI_FIFO_DEPTH	128
+
+/* Default number of chip select lines */
+#define CDNS_SPI_DEFAULT_NUM_CS		4
+
+/**
+ * struct cdns_spi - This definition defines spi driver instance
+ * @regs:		Virtual address of the SPI controller registers
+ * @ref_clk:		Pointer to the peripheral clock
+ * @pclk:		Pointer to the APB clock
+ * @speed_hz:		Current SPI bus clock speed in Hz
+ * @txbuf:		Pointer	to the TX buffer
+ * @rxbuf:		Pointer to the RX buffer
+ * @tx_bytes:		Number of bytes left to transfer
+ * @rx_bytes:		Number of bytes requested
+ * @dev_busy:		Device busy flag
+ * @is_decoded_cs:	Flag for decoder property set or not
+ */
+struct cdns_spi {
+	void __iomem *regs;
+	struct clk *ref_clk;
+	struct clk *pclk;
+	u32 speed_hz;
+	const u8 *txbuf;
+	u8 *rxbuf;
+	int tx_bytes;
+	int rx_bytes;
+	u8 dev_busy;
+	u32 is_decoded_cs;
+};
+
+/* Macros for the SPI controller read/write */
+static inline u32 cdns_spi_read(struct cdns_spi *xspi, u32 offset)
+{
+	return readl_relaxed(xspi->regs + offset);
+}
+
+static inline void cdns_spi_write(struct cdns_spi *xspi, u32 offset, u32 val)
+{
+	writel_relaxed(val, xspi->regs + offset);
+}
+
+/**
+ * cdns_spi_init_hw - Initialize the hardware and configure the SPI controller
+ * @xspi:	Pointer to the cdns_spi structure
+ *
+ * On reset the SPI controller is configured to be in master mode, baud rate
+ * divisor is set to 4, threshold value for TX FIFO not full interrupt is set
+ * to 1 and size of the word to be transferred as 8 bit.
+ * This function initializes the SPI controller to disable and clear all the
+ * interrupts, enable manual slave select and manual start, deselect all the
+ * chip select lines, and enable the SPI controller.
+ */
+static void cdns_spi_init_hw(struct cdns_spi *xspi)
+{
+	cdns_spi_write(xspi, CDNS_SPI_ER_OFFSET,
+		       CDNS_SPI_ER_DISABLE_MASK);
+	cdns_spi_write(xspi, CDNS_SPI_IDR_OFFSET,
+		       CDNS_SPI_IXR_ALL_MASK);
+
+	/* Clear the RX FIFO */
+	while (cdns_spi_read(xspi, CDNS_SPI_ISR_OFFSET) &
+	       CDNS_SPI_IXR_RXNEMTY_MASK)
+		cdns_spi_read(xspi, CDNS_SPI_RXD_OFFSET);
+
+	cdns_spi_write(xspi, CDNS_SPI_ISR_OFFSET,
+		       CDNS_SPI_IXR_ALL_MASK);
+	cdns_spi_write(xspi, CDNS_SPI_CR_OFFSET,
+		       CDNS_SPI_CR_DEFAULT_MASK);
+	cdns_spi_write(xspi, CDNS_SPI_ER_OFFSET,
+		       CDNS_SPI_ER_ENABLE_MASK);
+}
+
+/**
+ * cdns_spi_chipselect - Select or deselect the chip select line
+ * @spi:	Pointer to the spi_device structure
+ * @is_on:	Select(0) or deselect (1) the chip select line
+ */
+static void cdns_spi_chipselect(struct spi_device *spi, bool is_high)
+{
+	struct cdns_spi *xspi = spi_master_get_devdata(spi->master);
+	u32 ctrl_reg;
+
+	ctrl_reg = cdns_spi_read(xspi, CDNS_SPI_CR_OFFSET);
+
+	if (is_high) {
+		/* Deselect the slave */
+		ctrl_reg |= CDNS_SPI_CR_SSCTRL_MASK;
+	} else {
+		/* Select the slave */
+		ctrl_reg &= ~CDNS_SPI_CR_SSCTRL_MASK;
+		if (!(xspi->is_decoded_cs))
+			ctrl_reg |= ((~(CDNS_SPI_SS0 << spi->chip_select)) <<
+				     CDNS_SPI_SS_SHIFT) &
+				     CDNS_SPI_CR_SSCTRL_MASK;
+		else
+			ctrl_reg |= (spi->chip_select << CDNS_SPI_SS_SHIFT) &
+				     CDNS_SPI_CR_SSCTRL_MASK;
+	}
+
+	cdns_spi_write(xspi, CDNS_SPI_CR_OFFSET, ctrl_reg);
+}
+
+/**
+ * cdns_spi_config_clock_mode - Sets clock polarity and phase
+ * @spi:	Pointer to the spi_device structure
+ *
+ * Sets the requested clock polarity and phase.
+ */
+static void cdns_spi_config_clock_mode(struct spi_device *spi)
+{
+	struct cdns_spi *xspi = spi_master_get_devdata(spi->master);
+	u32 ctrl_reg;
+
+	ctrl_reg = cdns_spi_read(xspi, CDNS_SPI_CR_OFFSET);
+
+	/* Set the SPI clock phase and clock polarity */
+	ctrl_reg &= ~(CDNS_SPI_CR_CPHA_MASK | CDNS_SPI_CR_CPOL_MASK);
+	if (spi->mode & SPI_CPHA)
+		ctrl_reg |= CDNS_SPI_CR_CPHA_MASK;
+	if (spi->mode & SPI_CPOL)
+		ctrl_reg |= CDNS_SPI_CR_CPOL_MASK;
+
+	cdns_spi_write(xspi, CDNS_SPI_CR_OFFSET, ctrl_reg);
+}
+
+/**
+ * cdns_spi_config_clock_freq - Sets clock frequency
+ * @spi:	Pointer to the spi_device structure
+ * @transfer:	Pointer to the spi_transfer structure which provides
+ *		information about next transfer setup parameters
+ *
+ * Sets the requested clock frequency.
+ * Note: If the requested frequency is not an exact match with what can be
+ * obtained using the prescalar value the driver sets the clock frequency which
+ * is lower than the requested frequency (maximum lower) for the transfer. If
+ * the requested frequency is higher or lower than that is supported by the SPI
+ * controller the driver will set the highest or lowest frequency supported by
+ * controller.
+ */
+static void cdns_spi_config_clock_freq(struct spi_device *spi,
+				  struct spi_transfer *transfer)
+{
+	struct cdns_spi *xspi = spi_master_get_devdata(spi->master);
+	u32 ctrl_reg, baud_rate_val;
+	unsigned long frequency;
+
+	frequency = clk_get_rate(xspi->ref_clk);
+
+	ctrl_reg = cdns_spi_read(xspi, CDNS_SPI_CR_OFFSET);
+
+	/* Set the clock frequency */
+	if (xspi->speed_hz != transfer->speed_hz) {
+		/* first valid value is 1 */
+		baud_rate_val = CDNS_SPI_BAUD_DIV_MIN;
+		while ((baud_rate_val < CDNS_SPI_BAUD_DIV_MAX) &&
+		       (frequency / (2 << baud_rate_val)) > transfer->speed_hz)
+			baud_rate_val++;
+
+		ctrl_reg &= ~CDNS_SPI_CR_BAUD_DIV_MASK;
+		ctrl_reg |= baud_rate_val << CDNS_SPI_BAUD_DIV_SHIFT;
+
+		xspi->speed_hz = frequency / (2 << baud_rate_val);
+	}
+	cdns_spi_write(xspi, CDNS_SPI_CR_OFFSET, ctrl_reg);
+}
+
+/**
+ * cdns_spi_setup_transfer - Configure SPI controller for specified transfer
+ * @spi:	Pointer to the spi_device structure
+ * @transfer:	Pointer to the spi_transfer structure which provides
+ *		information about next transfer setup parameters
+ *
+ * Sets the operational mode of SPI controller for the next SPI transfer and
+ * sets the requested clock frequency.
+ *
+ * Return:	Always 0
+ */
+static int cdns_spi_setup_transfer(struct spi_device *spi,
+				   struct spi_transfer *transfer)
+{
+	struct cdns_spi *xspi = spi_master_get_devdata(spi->master);
+
+	cdns_spi_config_clock_freq(spi, transfer);
+
+	dev_dbg(&spi->dev, "%s, mode %d, %u bits/w, %u clock speed\n",
+		__func__, spi->mode, spi->bits_per_word,
+		xspi->speed_hz);
+
+	return 0;
+}
+
+/**
+ * cdns_spi_fill_tx_fifo - Fills the TX FIFO with as many bytes as possible
+ * @xspi:	Pointer to the cdns_spi structure
+ */
+static void cdns_spi_fill_tx_fifo(struct cdns_spi *xspi)
+{
+	unsigned long trans_cnt = 0;
+
+	while ((trans_cnt < CDNS_SPI_FIFO_DEPTH) &&
+	       (xspi->tx_bytes > 0)) {
+		if (xspi->txbuf)
+			cdns_spi_write(xspi, CDNS_SPI_TXD_OFFSET,
+				       *xspi->txbuf++);
+		else
+			cdns_spi_write(xspi, CDNS_SPI_TXD_OFFSET, 0);
+
+		xspi->tx_bytes--;
+		trans_cnt++;
+	}
+}
+
+/**
+ * cdns_spi_irq - Interrupt service routine of the SPI controller
+ * @irq:	IRQ number
+ * @dev_id:	Pointer to the xspi structure
+ *
+ * This function handles TX empty and Mode Fault interrupts only.
+ * On TX empty interrupt this function reads the received data from RX FIFO and
+ * fills the TX FIFO if there is any data remaining to be transferred.
+ * On Mode Fault interrupt this function indicates that transfer is completed,
+ * the SPI subsystem will identify the error as the remaining bytes to be
+ * transferred is non-zero.
+ *
+ * Return:	IRQ_HANDLED when handled; IRQ_NONE otherwise.
+ */
+static irqreturn_t cdns_spi_irq(int irq, void *dev_id)
+{
+	struct spi_master *master = dev_id;
+	struct cdns_spi *xspi = spi_master_get_devdata(master);
+	u32 intr_status, status;
+
+	status = IRQ_NONE;
+	intr_status = cdns_spi_read(xspi, CDNS_SPI_ISR_OFFSET);
+	cdns_spi_write(xspi, CDNS_SPI_ISR_OFFSET, intr_status);
+
+	if (intr_status & CDNS_SPI_IXR_MODF_MASK) {
+		/* Indicate that transfer is completed, the SPI subsystem will
+		 * identify the error as the remaining bytes to be
+		 * transferred is non-zero
+		 */
+		cdns_spi_write(xspi, CDNS_SPI_IDR_OFFSET,
+			       CDNS_SPI_IXR_DEFAULT_MASK);
+		spi_finalize_current_transfer(master);
+		status = IRQ_HANDLED;
+	} else if (intr_status & CDNS_SPI_IXR_TXOW_MASK) {
+		unsigned long trans_cnt;
+
+		trans_cnt = xspi->rx_bytes - xspi->tx_bytes;
+
+		/* Read out the data from the RX FIFO */
+		while (trans_cnt) {
+			u8 data;
+
+			data = cdns_spi_read(xspi, CDNS_SPI_RXD_OFFSET);
+			if (xspi->rxbuf)
+				*xspi->rxbuf++ = data;
+
+			xspi->rx_bytes--;
+			trans_cnt--;
+		}
+
+		if (xspi->tx_bytes) {
+			/* There is more data to send */
+			cdns_spi_fill_tx_fifo(xspi);
+		} else {
+			/* Transfer is completed */
+			cdns_spi_write(xspi, CDNS_SPI_IDR_OFFSET,
+				       CDNS_SPI_IXR_DEFAULT_MASK);
+			spi_finalize_current_transfer(master);
+		}
+		status = IRQ_HANDLED;
+	}
+
+	return status;
+}
+
+/**
+ * cdns_transfer_one - Initiates the SPI transfer
+ * @master:	Pointer to spi_master structure
+ * @spi:	Pointer to the spi_device structure
+ * @transfer:	Pointer to the spi_transfer structure which provides
+ *		information about next transfer parameters
+ *
+ * This function fills the TX FIFO, starts the SPI transfer and
+ * returns a positive transfer count so that core will wait for completion.
+ *
+ * Return:	Number of bytes transferred in the last transfer
+ */
+static int cdns_transfer_one(struct spi_master *master,
+			     struct spi_device *spi,
+			     struct spi_transfer *transfer)
+{
+	struct cdns_spi *xspi = spi_master_get_devdata(master);
+
+	xspi->txbuf = transfer->tx_buf;
+	xspi->rxbuf = transfer->rx_buf;
+	xspi->tx_bytes = transfer->len;
+	xspi->rx_bytes = transfer->len;
+
+	cdns_spi_setup_transfer(spi, transfer);
+
+	cdns_spi_fill_tx_fifo(xspi);
+
+	cdns_spi_write(xspi, CDNS_SPI_IER_OFFSET,
+		       CDNS_SPI_IXR_DEFAULT_MASK);
+	return transfer->len;
+}
+
+/**
+ * cdns_prepare_transfer_hardware - Prepares hardware for transfer.
+ * @master:	Pointer to the spi_master structure which provides
+ *		information about the controller.
+ *
+ * This function enables SPI master controller.
+ *
+ * Return:	0 always
+ */
+static int cdns_prepare_transfer_hardware(struct spi_master *master)
+{
+	struct cdns_spi *xspi = spi_master_get_devdata(master);
+
+	cdns_spi_config_clock_mode(master->cur_msg->spi);
+
+	cdns_spi_write(xspi, CDNS_SPI_ER_OFFSET,
+		       CDNS_SPI_ER_ENABLE_MASK);
+
+	return 0;
+}
+
+/**
+ * cdns_unprepare_transfer_hardware - Relaxes hardware after transfer
+ * @master:	Pointer to the spi_master structure which provides
+ *		information about the controller.
+ *
+ * This function disables the SPI master controller.
+ *
+ * Return:	0 always
+ */
+static int cdns_unprepare_transfer_hardware(struct spi_master *master)
+{
+	struct cdns_spi *xspi = spi_master_get_devdata(master);
+
+	cdns_spi_write(xspi, CDNS_SPI_ER_OFFSET,
+		       CDNS_SPI_ER_DISABLE_MASK);
+
+	return 0;
+}
+
+/**
+ * cdns_spi_probe - Probe method for the SPI driver
+ * @pdev:	Pointer to the platform_device structure
+ *
+ * This function initializes the driver data structures and the hardware.
+ *
+ * Return:	0 on success and error value on error
+ */
+static int cdns_spi_probe(struct platform_device *pdev)
+{
+	int ret = 0, irq;
+	struct spi_master *master;
+	struct cdns_spi *xspi;
+	struct resource *res;
+	u32 num_cs;
+
+	master = spi_alloc_master(&pdev->dev, sizeof(*xspi));
+	if (master == NULL)
+		return -ENOMEM;
+
+	xspi = spi_master_get_devdata(master);
+	master->dev.of_node = pdev->dev.of_node;
+	platform_set_drvdata(pdev, master);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	xspi->regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(xspi->regs)) {
+		ret = PTR_ERR(xspi->regs);
+		goto remove_master;
+	}
+
+	xspi->pclk = devm_clk_get(&pdev->dev, "pclk");
+	if (IS_ERR(xspi->pclk)) {
+		dev_err(&pdev->dev, "pclk clock not found.\n");
+		ret = PTR_ERR(xspi->pclk);
+		goto remove_master;
+	}
+
+	xspi->ref_clk = devm_clk_get(&pdev->dev, "ref_clk");
+	if (IS_ERR(xspi->ref_clk)) {
+		dev_err(&pdev->dev, "ref_clk clock not found.\n");
+		ret = PTR_ERR(xspi->ref_clk);
+		goto remove_master;
+	}
+
+	ret = clk_prepare_enable(xspi->pclk);
+	if (ret) {
+		dev_err(&pdev->dev, "Unable to enable APB clock.\n");
+		goto remove_master;
+	}
+
+	ret = clk_prepare_enable(xspi->ref_clk);
+	if (ret) {
+		dev_err(&pdev->dev, "Unable to enable device clock.\n");
+		goto clk_dis_apb;
+	}
+
+	/* SPI controller initializations */
+	cdns_spi_init_hw(xspi);
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq <= 0) {
+		ret = -ENXIO;
+		dev_err(&pdev->dev, "irq number is invalid\n");
+		goto remove_master;
+	}
+
+	ret = devm_request_irq(&pdev->dev, irq, cdns_spi_irq,
+			       0, pdev->name, master);
+	if (ret != 0) {
+		ret = -ENXIO;
+		dev_err(&pdev->dev, "request_irq failed\n");
+		goto remove_master;
+	}
+
+	ret = of_property_read_u32(pdev->dev.of_node, "num-cs", &num_cs);
+
+	if (ret < 0)
+		master->num_chipselect = CDNS_SPI_DEFAULT_NUM_CS;
+	else
+		master->num_chipselect = num_cs;
+
+	ret = of_property_read_u32(pdev->dev.of_node, "is-decoded-cs",
+				   &xspi->is_decoded_cs);
+
+	if (ret < 0)
+		xspi->is_decoded_cs = 0;
+
+	master->prepare_transfer_hardware = cdns_prepare_transfer_hardware;
+	master->transfer_one = cdns_transfer_one;
+	master->unprepare_transfer_hardware = cdns_unprepare_transfer_hardware;
+	master->set_cs = cdns_spi_chipselect;
+	master->mode_bits = SPI_CPOL | SPI_CPHA;
+
+	/* Set to default valid value */
+	master->max_speed_hz = clk_get_rate(xspi->ref_clk) / 4;
+	xspi->speed_hz = master->max_speed_hz;
+
+	master->bits_per_word_mask = SPI_BPW_MASK(8);
+
+	ret = spi_register_master(master);
+	if (ret) {
+		dev_err(&pdev->dev, "spi_register_master failed\n");
+		goto clk_dis_all;
+	}
+
+	return ret;
+
+clk_dis_all:
+	clk_disable_unprepare(xspi->ref_clk);
+clk_dis_apb:
+	clk_disable_unprepare(xspi->pclk);
+remove_master:
+	spi_master_put(master);
+	return ret;
+}
+
+/**
+ * cdns_spi_remove - Remove method for the SPI driver
+ * @pdev:	Pointer to the platform_device structure
+ *
+ * This function is called if a device is physically removed from the system or
+ * if the driver module is being unloaded. It frees all resources allocated to
+ * the device.
+ *
+ * Return:	0 on success and error value on error
+ */
+static int cdns_spi_remove(struct platform_device *pdev)
+{
+	struct spi_master *master = platform_get_drvdata(pdev);
+	struct cdns_spi *xspi = spi_master_get_devdata(master);
+
+	cdns_spi_write(xspi, CDNS_SPI_ER_OFFSET,
+		       CDNS_SPI_ER_DISABLE_MASK);
+
+	clk_disable_unprepare(xspi->ref_clk);
+	clk_disable_unprepare(xspi->pclk);
+
+	spi_unregister_master(master);
+
+	return 0;
+}
+
+/**
+ * cdns_spi_suspend - Suspend method for the SPI driver
+ * @dev:	Address of the platform_device structure
+ *
+ * This function disables the SPI controller and
+ * changes the driver state to "suspend"
+ *
+ * Return:	Always 0
+ */
+static int __maybe_unused cdns_spi_suspend(struct device *dev)
+{
+	struct platform_device *pdev = container_of(dev,
+			struct platform_device, dev);
+	struct spi_master *master = platform_get_drvdata(pdev);
+	struct cdns_spi *xspi = spi_master_get_devdata(master);
+
+	spi_master_suspend(master);
+
+	clk_disable_unprepare(xspi->ref_clk);
+
+	clk_disable_unprepare(xspi->pclk);
+
+	return 0;
+}
+
+/**
+ * cdns_spi_resume - Resume method for the SPI driver
+ * @dev:	Address of the platform_device structure
+ *
+ * This function changes the driver state to "ready"
+ *
+ * Return:	0 on success and error value on error
+ */
+static int __maybe_unused cdns_spi_resume(struct device *dev)
+{
+	struct platform_device *pdev = container_of(dev,
+			struct platform_device, dev);
+	struct spi_master *master = platform_get_drvdata(pdev);
+	struct cdns_spi *xspi = spi_master_get_devdata(master);
+	int ret = 0;
+
+	ret = clk_prepare_enable(xspi->pclk);
+	if (ret) {
+		dev_err(dev, "Cannot enable APB clock.\n");
+		return ret;
+	}
+
+	ret = clk_prepare_enable(xspi->ref_clk);
+	if (ret) {
+		dev_err(dev, "Cannot enable device clock.\n");
+		clk_disable(xspi->pclk);
+		return ret;
+	}
+	spi_master_resume(master);
+
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(cdns_spi_dev_pm_ops, cdns_spi_suspend,
+			 cdns_spi_resume);
+
+static struct of_device_id cdns_spi_of_match[] = {
+	{ .compatible = "xlnx,zynq-spi-r1p6" },
+	{ .compatible = "cdns,spi-r1p6" },
+	{ /* end of table */ }
+};
+MODULE_DEVICE_TABLE(of, cdns_spi_of_match);
+
+/* cdns_spi_driver - This structure defines the SPI subsystem platform driver */
+static struct platform_driver cdns_spi_driver = {
+	.probe	= cdns_spi_probe,
+	.remove	= cdns_spi_remove,
+	.driver = {
+		.name = CDNS_SPI_NAME,
+		.owner = THIS_MODULE,
+		.of_match_table = cdns_spi_of_match,
+		.pm = &cdns_spi_dev_pm_ops,
+	},
+};
+
+module_platform_driver(cdns_spi_driver);
+
+MODULE_AUTHOR("Xilinx, Inc.");
+MODULE_DESCRIPTION("Cadence SPI driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/spi/spi-dw-mmio.c b/drivers/spi/spi-dw-mmio.c
index 1492f5ee9aa..a5cba14ac3d 100644
--- a/drivers/spi/spi-dw-mmio.c
+++ b/drivers/spi/spi-dw-mmio.c
@@ -16,6 +16,7 @@
 #include <linux/spi/spi.h>
 #include <linux/scatterlist.h>
 #include <linux/module.h>
+#include <linux/of_gpio.h>
 
 #include "spi-dw.h"
 
@@ -70,6 +71,27 @@ static int dw_spi_mmio_probe(struct platform_device *pdev)
 	dws->num_cs = 4;
 	dws->max_freq = clk_get_rate(dwsmmio->clk);
 
+	if (pdev->dev.of_node) {
+		int i;
+
+		for (i = 0; i < dws->num_cs; i++) {
+			int cs_gpio = of_get_named_gpio(pdev->dev.of_node,
+					"cs-gpios", i);
+
+			if (cs_gpio == -EPROBE_DEFER) {
+				ret = cs_gpio;
+				goto out;
+			}
+
+			if (gpio_is_valid(cs_gpio)) {
+				ret = devm_gpio_request(&pdev->dev, cs_gpio,
+						dev_name(&pdev->dev));
+				if (ret)
+					goto out;
+			}
+		}
+	}
+
 	ret = dw_spi_add_host(&pdev->dev, dws);
 	if (ret)
 		goto out;
diff --git a/drivers/spi/spi-dw.c b/drivers/spi/spi-dw.c
index 712ac5629cd..29f33143b79 100644
--- a/drivers/spi/spi-dw.c
+++ b/drivers/spi/spi-dw.c
@@ -24,6 +24,7 @@
 #include <linux/delay.h>
 #include <linux/slab.h>
 #include <linux/spi/spi.h>
+#include <linux/gpio.h>
 
 #include "spi-dw.h"
 
@@ -36,12 +37,6 @@
 #define DONE_STATE	((void *)2)
 #define ERROR_STATE	((void *)-1)
 
-#define QUEUE_RUNNING	0
-#define QUEUE_STOPPED	1
-
-#define MRST_SPI_DEASSERT	0
-#define MRST_SPI_ASSERT		1
-
 /* Slave spi_dev related */
 struct chip_data {
 	u16 cr0;
@@ -263,28 +258,22 @@ static int map_dma_buffers(struct dw_spi *dws)
 static void giveback(struct dw_spi *dws)
 {
 	struct spi_transfer *last_transfer;
-	unsigned long flags;
 	struct spi_message *msg;
 
-	spin_lock_irqsave(&dws->lock, flags);
 	msg = dws->cur_msg;
 	dws->cur_msg = NULL;
 	dws->cur_transfer = NULL;
 	dws->prev_chip = dws->cur_chip;
 	dws->cur_chip = NULL;
 	dws->dma_mapped = 0;
-	queue_work(dws->workqueue, &dws->pump_messages);
-	spin_unlock_irqrestore(&dws->lock, flags);
 
 	last_transfer = list_last_entry(&msg->transfers, struct spi_transfer,
 					transfer_list);
 
-	if (!last_transfer->cs_change && dws->cs_control)
-		dws->cs_control(MRST_SPI_DEASSERT);
+	if (!last_transfer->cs_change)
+		spi_chip_sel(dws, dws->cur_msg->spi, 0);
 
-	msg->state = NULL;
-	if (msg->complete)
-		msg->complete(msg->context);
+	spi_finalize_current_message(dws->master);
 }
 
 static void int_error_stop(struct dw_spi *dws, const char *msg)
@@ -502,7 +491,7 @@ static void pump_transfers(unsigned long data)
 			dw_writew(dws, DW_SPI_CTRL0, cr0);
 
 		spi_set_clk(dws, clk_div ? clk_div : chip->clk_div);
-		spi_chip_sel(dws, spi->chip_select);
+		spi_chip_sel(dws, spi, 1);
 
 		/* Set the interrupt mask, for poll mode just disable all int */
 		spi_mask_intr(dws, 0xff);
@@ -529,30 +518,12 @@ early_exit:
 	return;
 }
 
-static void pump_messages(struct work_struct *work)
+static int dw_spi_transfer_one_message(struct spi_master *master,
+		struct spi_message *msg)
 {
-	struct dw_spi *dws =
-		container_of(work, struct dw_spi, pump_messages);
-	unsigned long flags;
-
-	/* Lock queue and check for queue work */
-	spin_lock_irqsave(&dws->lock, flags);
-	if (list_empty(&dws->queue) || dws->run == QUEUE_STOPPED) {
-		dws->busy = 0;
-		spin_unlock_irqrestore(&dws->lock, flags);
-		return;
-	}
-
-	/* Make sure we are not already running a message */
-	if (dws->cur_msg) {
-		spin_unlock_irqrestore(&dws->lock, flags);
-		return;
-	}
-
-	/* Extract head of queue */
-	dws->cur_msg = list_entry(dws->queue.next, struct spi_message, queue);
-	list_del_init(&dws->cur_msg->queue);
+	struct dw_spi *dws = spi_master_get_devdata(master);
 
+	dws->cur_msg = msg;
 	/* Initial message state*/
 	dws->cur_msg->state = START_STATE;
 	dws->cur_transfer = list_entry(dws->cur_msg->transfers.next,
@@ -560,46 +531,9 @@ static void pump_messages(struct work_struct *work)
 						transfer_list);
 	dws->cur_chip = spi_get_ctldata(dws->cur_msg->spi);
 
-	/* Mark as busy and launch transfers */
+	/* Launch transfers */
 	tasklet_schedule(&dws->pump_transfers);
 
-	dws->busy = 1;
-	spin_unlock_irqrestore(&dws->lock, flags);
-}
-
-/* spi_device use this to queue in their spi_msg */
-static int dw_spi_transfer(struct spi_device *spi, struct spi_message *msg)
-{
-	struct dw_spi *dws = spi_master_get_devdata(spi->master);
-	unsigned long flags;
-
-	spin_lock_irqsave(&dws->lock, flags);
-
-	if (dws->run == QUEUE_STOPPED) {
-		spin_unlock_irqrestore(&dws->lock, flags);
-		return -ESHUTDOWN;
-	}
-
-	msg->actual_length = 0;
-	msg->status = -EINPROGRESS;
-	msg->state = START_STATE;
-
-	list_add_tail(&msg->queue, &dws->queue);
-
-	if (dws->run == QUEUE_RUNNING && !dws->busy) {
-
-		if (dws->cur_transfer || dws->cur_msg)
-			queue_work(dws->workqueue,
-					&dws->pump_messages);
-		else {
-			/* If no other data transaction in air, just go */
-			spin_unlock_irqrestore(&dws->lock, flags);
-			pump_messages(&dws->pump_messages);
-			return 0;
-		}
-	}
-
-	spin_unlock_irqrestore(&dws->lock, flags);
 	return 0;
 }
 
@@ -608,6 +542,7 @@ static int dw_spi_setup(struct spi_device *spi)
 {
 	struct dw_spi_chip *chip_info = NULL;
 	struct chip_data *chip;
+	int ret;
 
 	/* Only alloc on first setup */
 	chip = spi_get_ctldata(spi);
@@ -661,81 +596,13 @@ static int dw_spi_setup(struct spi_device *spi)
 			| (spi->mode  << SPI_MODE_OFFSET)
 			| (chip->tmode << SPI_TMOD_OFFSET);
 
-	return 0;
-}
-
-static int init_queue(struct dw_spi *dws)
-{
-	INIT_LIST_HEAD(&dws->queue);
-	spin_lock_init(&dws->lock);
-
-	dws->run = QUEUE_STOPPED;
-	dws->busy = 0;
-
-	tasklet_init(&dws->pump_transfers,
-			pump_transfers,	(unsigned long)dws);
-
-	INIT_WORK(&dws->pump_messages, pump_messages);
-	dws->workqueue = create_singlethread_workqueue(
-					dev_name(dws->master->dev.parent));
-	if (dws->workqueue == NULL)
-		return -EBUSY;
-
-	return 0;
-}
-
-static int start_queue(struct dw_spi *dws)
-{
-	unsigned long flags;
-
-	spin_lock_irqsave(&dws->lock, flags);
-
-	if (dws->run == QUEUE_RUNNING || dws->busy) {
-		spin_unlock_irqrestore(&dws->lock, flags);
-		return -EBUSY;
+	if (gpio_is_valid(spi->cs_gpio)) {
+		ret = gpio_direction_output(spi->cs_gpio,
+				!(spi->mode & SPI_CS_HIGH));
+		if (ret)
+			return ret;
 	}
 
-	dws->run = QUEUE_RUNNING;
-	dws->cur_msg = NULL;
-	dws->cur_transfer = NULL;
-	dws->cur_chip = NULL;
-	dws->prev_chip = NULL;
-	spin_unlock_irqrestore(&dws->lock, flags);
-
-	queue_work(dws->workqueue, &dws->pump_messages);
-
-	return 0;
-}
-
-static int stop_queue(struct dw_spi *dws)
-{
-	unsigned long flags;
-	unsigned limit = 50;
-	int status = 0;
-
-	spin_lock_irqsave(&dws->lock, flags);
-	dws->run = QUEUE_STOPPED;
-	while ((!list_empty(&dws->queue) || dws->busy) && limit--) {
-		spin_unlock_irqrestore(&dws->lock, flags);
-		msleep(10);
-		spin_lock_irqsave(&dws->lock, flags);
-	}
-
-	if (!list_empty(&dws->queue) || dws->busy)
-		status = -EBUSY;
-	spin_unlock_irqrestore(&dws->lock, flags);
-
-	return status;
-}
-
-static int destroy_queue(struct dw_spi *dws)
-{
-	int status;
-
-	status = stop_queue(dws);
-	if (status != 0)
-		return status;
-	destroy_workqueue(dws->workqueue);
 	return 0;
 }
 
@@ -794,7 +661,7 @@ int dw_spi_add_host(struct device *dev, struct dw_spi *dws)
 	master->bus_num = dws->bus_num;
 	master->num_chipselect = dws->num_cs;
 	master->setup = dw_spi_setup;
-	master->transfer = dw_spi_transfer;
+	master->transfer_one_message = dw_spi_transfer_one_message;
 	master->max_speed_hz = dws->max_freq;
 
 	/* Basic HW init */
@@ -808,33 +675,21 @@ int dw_spi_add_host(struct device *dev, struct dw_spi *dws)
 		}
 	}
 
-	/* Initial and start queue */
-	ret = init_queue(dws);
-	if (ret) {
-		dev_err(&master->dev, "problem initializing queue\n");
-		goto err_diable_hw;
-	}
-	ret = start_queue(dws);
-	if (ret) {
-		dev_err(&master->dev, "problem starting queue\n");
-		goto err_diable_hw;
-	}
+	tasklet_init(&dws->pump_transfers, pump_transfers, (unsigned long)dws);
 
 	spi_master_set_devdata(master, dws);
 	ret = devm_spi_register_master(dev, master);
 	if (ret) {
 		dev_err(&master->dev, "problem registering spi master\n");
-		goto err_queue_alloc;
+		goto err_dma_exit;
 	}
 
 	mrst_spi_debugfs_init(dws);
 	return 0;
 
-err_queue_alloc:
-	destroy_queue(dws);
+err_dma_exit:
 	if (dws->dma_ops && dws->dma_ops->dma_exit)
 		dws->dma_ops->dma_exit(dws);
-err_diable_hw:
 	spi_enable_chip(dws, 0);
 err_free_master:
 	spi_master_put(master);
@@ -844,18 +699,10 @@ EXPORT_SYMBOL_GPL(dw_spi_add_host);
 
 void dw_spi_remove_host(struct dw_spi *dws)
 {
-	int status = 0;
-
 	if (!dws)
 		return;
 	mrst_spi_debugfs_remove(dws);
 
-	/* Remove the queue */
-	status = destroy_queue(dws);
-	if (status != 0)
-		dev_err(&dws->master->dev,
-			"dw_spi_remove: workqueue will not complete, message memory not freed\n");
-
 	if (dws->dma_ops && dws->dma_ops->dma_exit)
 		dws->dma_ops->dma_exit(dws);
 	spi_enable_chip(dws, 0);
@@ -868,7 +715,7 @@ int dw_spi_suspend_host(struct dw_spi *dws)
 {
 	int ret = 0;
 
-	ret = stop_queue(dws);
+	ret = spi_master_suspend(dws->master);
 	if (ret)
 		return ret;
 	spi_enable_chip(dws, 0);
@@ -882,7 +729,7 @@ int dw_spi_resume_host(struct dw_spi *dws)
 	int ret;
 
 	spi_hw_init(dws);
-	ret = start_queue(dws);
+	ret = spi_master_resume(dws->master);
 	if (ret)
 		dev_err(&dws->master->dev, "fail to start queue (%d)\n", ret);
 	return ret;
diff --git a/drivers/spi/spi-dw.h b/drivers/spi/spi-dw.h
index 587643dae11..6d2acad34f6 100644
--- a/drivers/spi/spi-dw.h
+++ b/drivers/spi/spi-dw.h
@@ -3,6 +3,7 @@
 
 #include <linux/io.h>
 #include <linux/scatterlist.h>
+#include <linux/gpio.h>
 
 /* Register offsets */
 #define DW_SPI_CTRL0			0x00
@@ -104,14 +105,6 @@ struct dw_spi {
 	u16			bus_num;
 	u16			num_cs;		/* supported slave numbers */
 
-	/* Driver message queue */
-	struct workqueue_struct	*workqueue;
-	struct work_struct	pump_messages;
-	spinlock_t		lock;
-	struct list_head	queue;
-	int			busy;
-	int			run;
-
 	/* Message Transfer pump */
 	struct tasklet_struct	pump_transfers;
 
@@ -186,15 +179,20 @@ static inline void spi_set_clk(struct dw_spi *dws, u16 div)
 	dw_writel(dws, DW_SPI_BAUDR, div);
 }
 
-static inline void spi_chip_sel(struct dw_spi *dws, u16 cs)
+static inline void spi_chip_sel(struct dw_spi *dws, struct spi_device *spi,
+		int active)
 {
-	if (cs > dws->num_cs)
-		return;
+	u16 cs = spi->chip_select;
+	int gpio_val = active ? (spi->mode & SPI_CS_HIGH) :
+		!(spi->mode & SPI_CS_HIGH);
 
 	if (dws->cs_control)
-		dws->cs_control(1);
+		dws->cs_control(active);
+	if (gpio_is_valid(spi->cs_gpio))
+		gpio_set_value(spi->cs_gpio, gpio_val);
 
-	dw_writel(dws, DW_SPI_SER, 1 << cs);
+	if (active)
+		dw_writel(dws, DW_SPI_SER, 1 << cs);
 }
 
 /* Disable IRQ bits */
diff --git a/drivers/spi/spi-falcon.c b/drivers/spi/spi-falcon.c
index 09965f069a1..ba441ad9a00 100644
--- a/drivers/spi/spi-falcon.c
+++ b/drivers/spi/spi-falcon.c
@@ -11,7 +11,6 @@
 #include <linux/platform_device.h>
 #include <linux/spi/spi.h>
 #include <linux/delay.h>
-#include <linux/workqueue.h>
 #include <linux/of.h>
 #include <linux/of_platform.h>
 
diff --git a/drivers/spi/spi-fsl-dspi.c b/drivers/spi/spi-fsl-dspi.c
index d565eeee3bd..5021ddf03f6 100644
--- a/drivers/spi/spi-fsl-dspi.c
+++ b/drivers/spi/spi-fsl-dspi.c
@@ -406,7 +406,7 @@ static irqreturn_t dspi_interrupt(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static struct of_device_id fsl_dspi_dt_ids[] = {
+static const struct of_device_id fsl_dspi_dt_ids[] = {
 	{ .compatible = "fsl,vf610-dspi", .data = NULL, },
 	{ /* sentinel */ }
 };
diff --git a/drivers/spi/spi-fsl-espi.c b/drivers/spi/spi-fsl-espi.c
index e767f5831b9..8ebd724e4c5 100644
--- a/drivers/spi/spi-fsl-espi.c
+++ b/drivers/spi/spi-fsl-espi.c
@@ -348,7 +348,7 @@ static void fsl_espi_cmd_trans(struct spi_message *m,
 	}
 
 	espi_trans->tx_buf = local_buf;
-	espi_trans->rx_buf = local_buf + espi_trans->n_tx;
+	espi_trans->rx_buf = local_buf;
 	fsl_espi_do_trans(m, espi_trans);
 
 	espi_trans->actual_length = espi_trans->len;
@@ -397,7 +397,7 @@ static void fsl_espi_rw_trans(struct spi_message *m,
 		espi_trans->n_rx = trans_len;
 		espi_trans->len = trans_len + n_tx;
 		espi_trans->tx_buf = local_buf;
-		espi_trans->rx_buf = local_buf + n_tx;
+		espi_trans->rx_buf = local_buf;
 		fsl_espi_do_trans(m, espi_trans);
 
 		memcpy(rx_buf + pos, espi_trans->rx_buf + n_tx, trans_len);
@@ -458,7 +458,7 @@ static int fsl_espi_setup(struct spi_device *spi)
 		return -EINVAL;
 
 	if (!cs) {
-		cs = kzalloc(sizeof *cs, GFP_KERNEL);
+		cs = devm_kzalloc(&spi->dev, sizeof(*cs), GFP_KERNEL);
 		if (!cs)
 			return -ENOMEM;
 		spi->controller_state = cs;
@@ -586,8 +586,10 @@ static struct spi_master * fsl_espi_probe(struct device *dev,
 	struct spi_master *master;
 	struct mpc8xxx_spi *mpc8xxx_spi;
 	struct fsl_espi_reg *reg_base;
-	u32 regval;
-	int i, ret = 0;
+	struct device_node *nc;
+	const __be32 *prop;
+	u32 regval, csmode;
+	int i, len, ret = 0;
 
 	master = spi_alloc_master(dev, sizeof(struct mpc8xxx_spi));
 	if (!master) {
@@ -634,8 +636,32 @@ static struct spi_master * fsl_espi_probe(struct device *dev,
 	mpc8xxx_spi_write_reg(&reg_base->event, 0xffffffff);
 
 	/* Init eSPI CS mode register */
-	for (i = 0; i < pdata->max_chipselect; i++)
-		mpc8xxx_spi_write_reg(&reg_base->csmode[i], CSMODE_INIT_VAL);
+	for_each_available_child_of_node(master->dev.of_node, nc) {
+		/* get chip select */
+		prop = of_get_property(nc, "reg", &len);
+		if (!prop || len < sizeof(*prop))
+			continue;
+		i = be32_to_cpup(prop);
+		if (i < 0 || i >= pdata->max_chipselect)
+			continue;
+
+		csmode = CSMODE_INIT_VAL;
+		/* check if CSBEF is set in device tree */
+		prop = of_get_property(nc, "fsl,csbef", &len);
+		if (prop && len >= sizeof(*prop)) {
+			csmode &= ~(CSMODE_BEF(0xf));
+			csmode |= CSMODE_BEF(be32_to_cpup(prop));
+		}
+		/* check if CSAFT is set in device tree */
+		prop = of_get_property(nc, "fsl,csaft", &len);
+		if (prop && len >= sizeof(*prop)) {
+			csmode &= ~(CSMODE_AFT(0xf));
+			csmode |= CSMODE_AFT(be32_to_cpup(prop));
+		}
+		mpc8xxx_spi_write_reg(&reg_base->csmode[i], csmode);
+
+		dev_info(dev, "cs=%d, init_csmode=0x%x\n", i, csmode);
+	}
 
 	/* Enable SPI interface */
 	regval = pdata->initial_spmode | SPMODE_INIT_VAL | SPMODE_ENABLE;
diff --git a/drivers/spi/spi-fsl-lib.c b/drivers/spi/spi-fsl-lib.c
index e5d45fca355..95212ea96c8 100644
--- a/drivers/spi/spi-fsl-lib.c
+++ b/drivers/spi/spi-fsl-lib.c
@@ -99,11 +99,6 @@ int mpc8xxx_spi_transfer(struct spi_device *spi,
 	return 0;
 }
 
-void mpc8xxx_spi_cleanup(struct spi_device *spi)
-{
-	kfree(spi->controller_state);
-}
-
 const char *mpc8xxx_spi_strmode(unsigned int flags)
 {
 	if (flags & SPI_QE_CPU_MODE) {
@@ -134,7 +129,6 @@ int mpc8xxx_spi_probe(struct device *dev, struct resource *mem,
 			| SPI_LSB_FIRST | SPI_LOOP;
 
 	master->transfer = mpc8xxx_spi_transfer;
-	master->cleanup = mpc8xxx_spi_cleanup;
 	master->dev.of_node = dev->of_node;
 
 	mpc8xxx_spi = spi_master_get_devdata(master);
diff --git a/drivers/spi/spi-fsl-lib.h b/drivers/spi/spi-fsl-lib.h
index 52db6936778..2fcbfd01d10 100644
--- a/drivers/spi/spi-fsl-lib.h
+++ b/drivers/spi/spi-fsl-lib.h
@@ -124,7 +124,6 @@ extern struct mpc8xxx_spi_probe_info *to_of_pinfo(
 extern int mpc8xxx_spi_bufs(struct mpc8xxx_spi *mspi,
 		struct spi_transfer *t, unsigned int len);
 extern int mpc8xxx_spi_transfer(struct spi_device *spi, struct spi_message *m);
-extern void mpc8xxx_spi_cleanup(struct spi_device *spi);
 extern const char *mpc8xxx_spi_strmode(unsigned int flags);
 extern int mpc8xxx_spi_probe(struct device *dev, struct resource *mem,
 		unsigned int irq);
diff --git a/drivers/spi/spi-fsl-spi.c b/drivers/spi/spi-fsl-spi.c
index b3e7775034d..98ccd231bf0 100644
--- a/drivers/spi/spi-fsl-spi.c
+++ b/drivers/spi/spi-fsl-spi.c
@@ -431,7 +431,7 @@ static int fsl_spi_setup(struct spi_device *spi)
 		return -EINVAL;
 
 	if (!cs) {
-		cs = kzalloc(sizeof *cs, GFP_KERNEL);
+		cs = devm_kzalloc(&spi->dev, sizeof(*cs), GFP_KERNEL);
 		if (!cs)
 			return -ENOMEM;
 		spi->controller_state = cs;
diff --git a/drivers/spi/spi-gpio.c b/drivers/spi/spi-gpio.c
index 09823076df8..9f595535cf2 100644
--- a/drivers/spi/spi-gpio.c
+++ b/drivers/spi/spi-gpio.c
@@ -340,7 +340,7 @@ done:
 }
 
 #ifdef CONFIG_OF
-static struct of_device_id spi_gpio_dt_ids[] = {
+static const struct of_device_id spi_gpio_dt_ids[] = {
 	{ .compatible = "spi-gpio" },
 	{}
 };
diff --git a/drivers/spi/spi-nuc900.c b/drivers/spi/spi-nuc900.c
index 16e30de650b..73e91d5a43d 100644
--- a/drivers/spi/spi-nuc900.c
+++ b/drivers/spi/spi-nuc900.c
@@ -10,7 +10,6 @@
 
 #include <linux/module.h>
 #include <linux/spinlock.h>
-#include <linux/workqueue.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
 #include <linux/errno.h>
diff --git a/drivers/spi/spi-omap-uwire.c b/drivers/spi/spi-omap-uwire.c
index be2a2e108e2..0f5a0aa3b87 100644
--- a/drivers/spi/spi-omap-uwire.c
+++ b/drivers/spi/spi-omap-uwire.c
@@ -37,7 +37,6 @@
 #include <linux/init.h>
 #include <linux/delay.h>
 #include <linux/platform_device.h>
-#include <linux/workqueue.h>
 #include <linux/interrupt.h>
 #include <linux/err.h>
 #include <linux/clk.h>
diff --git a/drivers/spi/spi-pl022.c b/drivers/spi/spi-pl022.c
index 51d99779682..66d2ae21e78 100644
--- a/drivers/spi/spi-pl022.c
+++ b/drivers/spi/spi-pl022.c
@@ -1111,10 +1111,8 @@ static int pl022_dma_probe(struct pl022 *pl022)
 	}
 
 	pl022->dummypage = kmalloc(PAGE_SIZE, GFP_KERNEL);
-	if (!pl022->dummypage) {
-		dev_dbg(&pl022->adev->dev, "no DMA dummypage!\n");
+	if (!pl022->dummypage)
 		goto err_no_dummypage;
-	}
 
 	dev_info(&pl022->adev->dev, "setup for DMA on RX %s, TX %s\n",
 		 dma_chan_name(pl022->dma_rx_channel),
@@ -1809,11 +1807,8 @@ static int pl022_setup(struct spi_device *spi)
 
 	if (chip == NULL) {
 		chip = kzalloc(sizeof(struct chip_data), GFP_KERNEL);
-		if (!chip) {
-			dev_err(&spi->dev,
-				"cannot allocate controller state\n");
+		if (!chip)
 			return -ENOMEM;
-		}
 		dev_dbg(&spi->dev,
 			"allocated memory for controller's runtime state\n");
 	}
@@ -2050,10 +2045,8 @@ pl022_platform_data_dt_get(struct device *dev)
 	}
 
 	pd = devm_kzalloc(dev, sizeof(struct pl022_ssp_controller), GFP_KERNEL);
-	if (!pd) {
-		dev_err(dev, "cannot allocate platform data memory\n");
+	if (!pd)
 		return NULL;
-	}
 
 	pd->bus_id = -1;
 	pd->enable_dma = 1;
diff --git a/drivers/spi/spi-pxa2xx-dma.c b/drivers/spi/spi-pxa2xx-dma.c
index 713af4806f2..f6759dc0153 100644
--- a/drivers/spi/spi-pxa2xx-dma.c
+++ b/drivers/spi/spi-pxa2xx-dma.c
@@ -29,18 +29,6 @@ static int pxa2xx_spi_map_dma_buffer(struct driver_data *drv_data,
 	struct sg_table *sgt;
 	void *buf, *pbuf;
 
-	/*
-	 * Some DMA controllers have problems transferring buffers that are
-	 * not multiple of 4 bytes. So we truncate the transfer so that it
-	 * is suitable for such controllers, and handle the trailing bytes
-	 * manually after the DMA completes.
-	 *
-	 * REVISIT: It would be better if this information could be
-	 * retrieved directly from the DMA device in a similar way than
-	 * ->copy_align etc. is done.
-	 */
-	len = ALIGN(drv_data->len, 4);
-
 	if (dir == DMA_TO_DEVICE) {
 		dmadev = drv_data->tx_chan->device->dev;
 		sgt = &drv_data->tx_sgt;
@@ -144,12 +132,8 @@ static void pxa2xx_spi_dma_transfer_complete(struct driver_data *drv_data,
 		if (!error) {
 			pxa2xx_spi_unmap_dma_buffers(drv_data);
 
-			/* Handle the last bytes of unaligned transfer */
 			drv_data->tx += drv_data->tx_map_len;
-			drv_data->write(drv_data);
-
 			drv_data->rx += drv_data->rx_map_len;
-			drv_data->read(drv_data);
 
 			msg->actual_length += drv_data->len;
 			msg->state = pxa2xx_spi_next_transfer(drv_data);
diff --git a/drivers/spi/spi-pxa2xx-pci.c b/drivers/spi/spi-pxa2xx-pci.c
index 3f006d3ed2a..c1865c92ccb 100644
--- a/drivers/spi/spi-pxa2xx-pci.c
+++ b/drivers/spi/spi-pxa2xx-pci.c
@@ -8,7 +8,43 @@
 #include <linux/module.h>
 #include <linux/spi/pxa2xx_spi.h>
 
-static int ce4100_spi_probe(struct pci_dev *dev,
+enum {
+	PORT_CE4100,
+	PORT_BYT,
+};
+
+struct pxa_spi_info {
+	enum pxa_ssp_type type;
+	int port_id;
+	int num_chipselect;
+	int tx_slave_id;
+	int tx_chan_id;
+	int rx_slave_id;
+	int rx_chan_id;
+};
+
+static struct pxa_spi_info spi_info_configs[] = {
+	[PORT_CE4100] = {
+		.type = PXA25x_SSP,
+		.port_id =  -1,
+		.num_chipselect = -1,
+		.tx_slave_id = -1,
+		.tx_chan_id = -1,
+		.rx_slave_id = -1,
+		.rx_chan_id = -1,
+	},
+	[PORT_BYT] = {
+		.type = LPSS_SSP,
+		.port_id = 0,
+		.num_chipselect = 1,
+		.tx_slave_id = 0,
+		.tx_chan_id = 0,
+		.rx_slave_id = 1,
+		.rx_chan_id = 1,
+	},
+};
+
+static int pxa2xx_spi_pci_probe(struct pci_dev *dev,
 		const struct pci_device_id *ent)
 {
 	struct platform_device_info pi;
@@ -16,6 +52,7 @@ static int ce4100_spi_probe(struct pci_dev *dev,
 	struct platform_device *pdev;
 	struct pxa2xx_spi_master spi_pdata;
 	struct ssp_device *ssp;
+	struct pxa_spi_info *c;
 
 	ret = pcim_enable_device(dev);
 	if (ret)
@@ -25,8 +62,16 @@ static int ce4100_spi_probe(struct pci_dev *dev,
 	if (ret)
 		return ret;
 
+	c = &spi_info_configs[ent->driver_data];
+
 	memset(&spi_pdata, 0, sizeof(spi_pdata));
-	spi_pdata.num_chipselect = dev->devfn;
+	spi_pdata.num_chipselect = (c->num_chipselect > 0) ?
+					c->num_chipselect : dev->devfn;
+	spi_pdata.tx_slave_id = c->tx_slave_id;
+	spi_pdata.tx_chan_id = c->tx_chan_id;
+	spi_pdata.rx_slave_id = c->rx_slave_id;
+	spi_pdata.rx_chan_id = c->rx_chan_id;
+	spi_pdata.enable_dma = c->rx_slave_id >= 0 && c->tx_slave_id >= 0;
 
 	ssp = &spi_pdata.ssp;
 	ssp->phys_base = pci_resource_start(dev, 0);
@@ -36,8 +81,8 @@ static int ce4100_spi_probe(struct pci_dev *dev,
 		return -EIO;
 	}
 	ssp->irq = dev->irq;
-	ssp->port_id = dev->devfn;
-	ssp->type = PXA25x_SSP;
+	ssp->port_id = (c->port_id >= 0) ? c->port_id : dev->devfn;
+	ssp->type = c->type;
 
 	memset(&pi, 0, sizeof(pi));
 	pi.parent = &dev->dev;
@@ -55,28 +100,29 @@ static int ce4100_spi_probe(struct pci_dev *dev,
 	return 0;
 }
 
-static void ce4100_spi_remove(struct pci_dev *dev)
+static void pxa2xx_spi_pci_remove(struct pci_dev *dev)
 {
 	struct platform_device *pdev = pci_get_drvdata(dev);
 
 	platform_device_unregister(pdev);
 }
 
-static const struct pci_device_id ce4100_spi_devices[] = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x2e6a) },
+static const struct pci_device_id pxa2xx_spi_pci_devices[] = {
+	{ PCI_VDEVICE(INTEL, 0x2e6a), PORT_CE4100 },
+	{ PCI_VDEVICE(INTEL, 0x0f0e), PORT_BYT },
 	{ },
 };
-MODULE_DEVICE_TABLE(pci, ce4100_spi_devices);
+MODULE_DEVICE_TABLE(pci, pxa2xx_spi_pci_devices);
 
-static struct pci_driver ce4100_spi_driver = {
-	.name           = "ce4100_spi",
-	.id_table       = ce4100_spi_devices,
-	.probe          = ce4100_spi_probe,
-	.remove         = ce4100_spi_remove,
+static struct pci_driver pxa2xx_spi_pci_driver = {
+	.name           = "pxa2xx_spi_pci",
+	.id_table       = pxa2xx_spi_pci_devices,
+	.probe          = pxa2xx_spi_pci_probe,
+	.remove         = pxa2xx_spi_pci_remove,
 };
 
-module_pci_driver(ce4100_spi_driver);
+module_pci_driver(pxa2xx_spi_pci_driver);
 
-MODULE_DESCRIPTION("CE4100 PCI-SPI glue code for PXA's driver");
+MODULE_DESCRIPTION("CE4100/LPSS PCI-SPI glue code for PXA's driver");
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Sebastian Andrzej Siewior <bigeasy@linutronix.de>");
diff --git a/drivers/spi/spi-pxa2xx.c b/drivers/spi/spi-pxa2xx.c
index 41185d0557f..a98df7eeb42 100644
--- a/drivers/spi/spi-pxa2xx.c
+++ b/drivers/spi/spi-pxa2xx.c
@@ -27,7 +27,6 @@
 #include <linux/platform_device.h>
 #include <linux/spi/pxa2xx_spi.h>
 #include <linux/spi/spi.h>
-#include <linux/workqueue.h>
 #include <linux/delay.h>
 #include <linux/gpio.h>
 #include <linux/slab.h>
@@ -886,11 +885,8 @@ static int setup(struct spi_device *spi)
 	chip = spi_get_ctldata(spi);
 	if (!chip) {
 		chip = kzalloc(sizeof(struct chip_data), GFP_KERNEL);
-		if (!chip) {
-			dev_err(&spi->dev,
-				"failed setup: can't allocate chip data\n");
+		if (!chip)
 			return -ENOMEM;
-		}
 
 		if (drv_data->ssp_type == CE4100_SSP) {
 			if (spi->chip_select > 4) {
@@ -1037,11 +1033,8 @@ pxa2xx_spi_acpi_get_pdata(struct platform_device *pdev)
 		return NULL;
 
 	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
-	if (!pdata) {
-		dev_err(&pdev->dev,
-			"failed to allocate memory for platform data\n");
+	if (!pdata)
 		return NULL;
-	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res)
@@ -1202,6 +1195,11 @@ static int pxa2xx_spi_probe(struct platform_device *pdev)
 	tasklet_init(&drv_data->pump_transfers, pump_transfers,
 		     (unsigned long)drv_data);
 
+	pm_runtime_set_autosuspend_delay(&pdev->dev, 50);
+	pm_runtime_use_autosuspend(&pdev->dev);
+	pm_runtime_set_active(&pdev->dev);
+	pm_runtime_enable(&pdev->dev);
+
 	/* Register with the SPI framework */
 	platform_set_drvdata(pdev, drv_data);
 	status = devm_spi_register_master(&pdev->dev, master);
@@ -1210,11 +1208,6 @@ static int pxa2xx_spi_probe(struct platform_device *pdev)
 		goto out_error_clock_enabled;
 	}
 
-	pm_runtime_set_autosuspend_delay(&pdev->dev, 50);
-	pm_runtime_use_autosuspend(&pdev->dev);
-	pm_runtime_set_active(&pdev->dev);
-	pm_runtime_enable(&pdev->dev);
-
 	return status;
 
 out_error_clock_enabled:
diff --git a/drivers/spi/spi-qup.c b/drivers/spi/spi-qup.c
index b032e8885e2..fc1de86d3c8 100644
--- a/drivers/spi/spi-qup.c
+++ b/drivers/spi/spi-qup.c
@@ -287,7 +287,7 @@ static irqreturn_t spi_qup_qup_irq(int irq, void *dev_id)
 	writel_relaxed(opflags, controller->base + QUP_OPERATIONAL);
 
 	if (!xfer) {
-		dev_err_ratelimited(controller->dev, "unexpected irq %x08 %x08 %x08\n",
+		dev_err_ratelimited(controller->dev, "unexpected irq %08x %08x %08x\n",
 				    qup_err, spi_err, opflags);
 		return IRQ_HANDLED;
 	}
@@ -366,7 +366,7 @@ static int spi_qup_io_config(struct spi_device *spi, struct spi_transfer *xfer)
 	n_words = xfer->len / w_size;
 	controller->w_size = w_size;
 
-	if (n_words <= controller->in_fifo_sz) {
+	if (n_words <= (controller->in_fifo_sz / sizeof(u32))) {
 		mode = QUP_IO_M_MODE_FIFO;
 		writel_relaxed(n_words, controller->base + QUP_MX_READ_CNT);
 		writel_relaxed(n_words, controller->base + QUP_MX_WRITE_CNT);
@@ -734,7 +734,7 @@ static int spi_qup_remove(struct platform_device *pdev)
 	int ret;
 
 	ret = pm_runtime_get_sync(&pdev->dev);
-	if (ret)
+	if (ret < 0)
 		return ret;
 
 	ret = spi_qup_set_state(controller, QUP_STATE_RESET);
@@ -749,7 +749,7 @@ static int spi_qup_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id spi_qup_dt_match[] = {
+static const struct of_device_id spi_qup_dt_match[] = {
 	{ .compatible = "qcom,spi-qup-v2.1.1", },
 	{ .compatible = "qcom,spi-qup-v2.2.1", },
 	{ }
diff --git a/drivers/spi/spi-rspi.c b/drivers/spi/spi-rspi.c
index 1fb0ad21332..10112745bb1 100644
--- a/drivers/spi/spi-rspi.c
+++ b/drivers/spi/spi-rspi.c
@@ -183,8 +183,6 @@
 #define SPBFCR_TXTRG_MASK	0x30	/* Transmit Buffer Data Triggering Number */
 #define SPBFCR_RXTRG_MASK	0x07	/* Receive Buffer Data Triggering Number */
 
-#define DUMMY_DATA		0x00
-
 struct rspi_data {
 	void __iomem *addr;
 	u32 max_speed_hz;
@@ -197,11 +195,6 @@ struct rspi_data {
 	int rx_irq, tx_irq;
 	const struct spi_ops *ops;
 
-	/* for dmaengine */
-	struct dma_chan *chan_tx;
-	struct dma_chan *chan_rx;
-
-	unsigned dma_width_16bit:1;
 	unsigned dma_callbacked:1;
 	unsigned byte_access:1;
 };
@@ -253,6 +246,8 @@ struct spi_ops {
 	int (*transfer_one)(struct spi_master *master, struct spi_device *spi,
 			    struct spi_transfer *xfer);
 	u16 mode_bits;
+	u16 flags;
+	u16 fifo_size;
 };
 
 /*
@@ -266,7 +261,8 @@ static int rspi_set_config_register(struct rspi_data *rspi, int access_size)
 	rspi_write8(rspi, rspi->sppcr, RSPI_SPPCR);
 
 	/* Sets transfer bit rate */
-	spbr = clk_get_rate(rspi->clk) / (2 * rspi->max_speed_hz) - 1;
+	spbr = DIV_ROUND_UP(clk_get_rate(rspi->clk),
+			    2 * rspi->max_speed_hz) - 1;
 	rspi_write8(rspi, clamp(spbr, 0, 255), RSPI_SPBR);
 
 	/* Disable dummy transmission, set 16-bit word access, 1 frame */
@@ -302,7 +298,8 @@ static int rspi_rz_set_config_register(struct rspi_data *rspi, int access_size)
 	rspi_write8(rspi, rspi->sppcr, RSPI_SPPCR);
 
 	/* Sets transfer bit rate */
-	spbr = clk_get_rate(rspi->clk) / (2 * rspi->max_speed_hz) - 1;
+	spbr = DIV_ROUND_UP(clk_get_rate(rspi->clk),
+			    2 * rspi->max_speed_hz) - 1;
 	rspi_write8(rspi, clamp(spbr, 0, 255), RSPI_SPBR);
 
 	/* Disable dummy transmission, set byte access */
@@ -335,7 +332,7 @@ static int qspi_set_config_register(struct rspi_data *rspi, int access_size)
 	rspi_write8(rspi, rspi->sppcr, RSPI_SPPCR);
 
 	/* Sets transfer bit rate */
-	spbr = clk_get_rate(rspi->clk) / (2 * rspi->max_speed_hz);
+	spbr = DIV_ROUND_UP(clk_get_rate(rspi->clk), 2 * rspi->max_speed_hz);
 	rspi_write8(rspi, clamp(spbr, 0, 255), RSPI_SPBR);
 
 	/* Disable dummy transmission, set byte access */
@@ -403,11 +400,22 @@ static int rspi_wait_for_interrupt(struct rspi_data *rspi, u8 wait_mask,
 	return 0;
 }
 
+static inline int rspi_wait_for_tx_empty(struct rspi_data *rspi)
+{
+	return rspi_wait_for_interrupt(rspi, SPSR_SPTEF, SPCR_SPTIE);
+}
+
+static inline int rspi_wait_for_rx_full(struct rspi_data *rspi)
+{
+	return rspi_wait_for_interrupt(rspi, SPSR_SPRF, SPCR_SPRIE);
+}
+
 static int rspi_data_out(struct rspi_data *rspi, u8 data)
 {
-	if (rspi_wait_for_interrupt(rspi, SPSR_SPTEF, SPCR_SPTIE) < 0) {
+	int error = rspi_wait_for_tx_empty(rspi);
+	if (error < 0) {
 		dev_err(&rspi->master->dev, "transmit timeout\n");
-		return -ETIMEDOUT;
+		return error;
 	}
 	rspi_write_data(rspi, data);
 	return 0;
@@ -415,25 +423,36 @@ static int rspi_data_out(struct rspi_data *rspi, u8 data)
 
 static int rspi_data_in(struct rspi_data *rspi)
 {
+	int error;
 	u8 data;
 
-	if (rspi_wait_for_interrupt(rspi, SPSR_SPRF, SPCR_SPRIE) < 0) {
+	error = rspi_wait_for_rx_full(rspi);
+	if (error < 0) {
 		dev_err(&rspi->master->dev, "receive timeout\n");
-		return -ETIMEDOUT;
+		return error;
 	}
 	data = rspi_read_data(rspi);
 	return data;
 }
 
-static int rspi_data_out_in(struct rspi_data *rspi, u8 data)
+static int rspi_pio_transfer(struct rspi_data *rspi, const u8 *tx, u8 *rx,
+			     unsigned int n)
 {
-	int ret;
-
-	ret = rspi_data_out(rspi, data);
-	if (ret < 0)
-		return ret;
+	while (n-- > 0) {
+		if (tx) {
+			int ret = rspi_data_out(rspi, *tx++);
+			if (ret < 0)
+				return ret;
+		}
+		if (rx) {
+			int ret = rspi_data_in(rspi);
+			if (ret < 0)
+				return ret;
+			*rx++ = ret;
+		}
+	}
 
-	return rspi_data_in(rspi);
+	return 0;
 }
 
 static void rspi_dma_complete(void *arg)
@@ -444,97 +463,67 @@ static void rspi_dma_complete(void *arg)
 	wake_up_interruptible(&rspi->wait);
 }
 
-static int rspi_dma_map_sg(struct scatterlist *sg, const void *buf,
-			   unsigned len, struct dma_chan *chan,
-			   enum dma_transfer_direction dir)
-{
-	sg_init_table(sg, 1);
-	sg_set_buf(sg, buf, len);
-	sg_dma_len(sg) = len;
-	return dma_map_sg(chan->device->dev, sg, 1, dir);
-}
-
-static void rspi_dma_unmap_sg(struct scatterlist *sg, struct dma_chan *chan,
-			      enum dma_transfer_direction dir)
+static int rspi_dma_transfer(struct rspi_data *rspi, struct sg_table *tx,
+			     struct sg_table *rx)
 {
-	dma_unmap_sg(chan->device->dev, sg, 1, dir);
-}
-
-static void rspi_memory_to_8bit(void *buf, const void *data, unsigned len)
-{
-	u16 *dst = buf;
-	const u8 *src = data;
-
-	while (len) {
-		*dst++ = (u16)(*src++);
-		len--;
-	}
-}
-
-static void rspi_memory_from_8bit(void *buf, const void *data, unsigned len)
-{
-	u8 *dst = buf;
-	const u16 *src = data;
-
-	while (len) {
-		*dst++ = (u8)*src++;
-		len--;
-	}
-}
+	struct dma_async_tx_descriptor *desc_tx = NULL, *desc_rx = NULL;
+	u8 irq_mask = 0;
+	unsigned int other_irq = 0;
+	dma_cookie_t cookie;
+	int ret;
 
-static int rspi_send_dma(struct rspi_data *rspi, struct spi_transfer *t)
-{
-	struct scatterlist sg;
-	const void *buf = NULL;
-	struct dma_async_tx_descriptor *desc;
-	unsigned int len;
-	int ret = 0;
-
-	if (rspi->dma_width_16bit) {
-		void *tmp;
-		/*
-		 * If DMAC bus width is 16-bit, the driver allocates a dummy
-		 * buffer. And, the driver converts original data into the
-		 * DMAC data as the following format:
-		 *  original data: 1st byte, 2nd byte ...
-		 *  DMAC data:     1st byte, dummy, 2nd byte, dummy ...
-		 */
-		len = t->len * 2;
-		tmp = kmalloc(len, GFP_KERNEL);
-		if (!tmp)
-			return -ENOMEM;
-		rspi_memory_to_8bit(tmp, t->tx_buf, t->len);
-		buf = tmp;
-	} else {
-		len = t->len;
-		buf = t->tx_buf;
-	}
+	if (tx) {
+		desc_tx = dmaengine_prep_slave_sg(rspi->master->dma_tx,
+					tx->sgl, tx->nents, DMA_TO_DEVICE,
+					DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+		if (!desc_tx)
+			return -EIO;
 
-	if (!rspi_dma_map_sg(&sg, buf, len, rspi->chan_tx, DMA_TO_DEVICE)) {
-		ret = -EFAULT;
-		goto end_nomap;
+		irq_mask |= SPCR_SPTIE;
 	}
-	desc = dmaengine_prep_slave_sg(rspi->chan_tx, &sg, 1, DMA_TO_DEVICE,
-				       DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
-	if (!desc) {
-		ret = -EIO;
-		goto end;
+	if (rx) {
+		desc_rx = dmaengine_prep_slave_sg(rspi->master->dma_rx,
+					rx->sgl, rx->nents, DMA_FROM_DEVICE,
+					DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+		if (!desc_rx)
+			return -EIO;
+
+		irq_mask |= SPCR_SPRIE;
 	}
 
 	/*
-	 * DMAC needs SPTIE, but if SPTIE is set, this IRQ routine will be
+	 * DMAC needs SPxIE, but if SPxIE is set, the IRQ routine will be
 	 * called. So, this driver disables the IRQ while DMA transfer.
 	 */
-	disable_irq(rspi->tx_irq);
+	if (tx)
+		disable_irq(other_irq = rspi->tx_irq);
+	if (rx && rspi->rx_irq != other_irq)
+		disable_irq(rspi->rx_irq);
 
-	rspi_write8(rspi, rspi_read8(rspi, RSPI_SPCR) | SPCR_TXMD, RSPI_SPCR);
-	rspi_enable_irq(rspi, SPCR_SPTIE);
+	rspi_enable_irq(rspi, irq_mask);
 	rspi->dma_callbacked = 0;
 
-	desc->callback = rspi_dma_complete;
-	desc->callback_param = rspi;
-	dmaengine_submit(desc);
-	dma_async_issue_pending(rspi->chan_tx);
+	if (rx) {
+		desc_rx->callback = rspi_dma_complete;
+		desc_rx->callback_param = rspi;
+		cookie = dmaengine_submit(desc_rx);
+		if (dma_submit_error(cookie))
+			return cookie;
+		dma_async_issue_pending(rspi->master->dma_rx);
+	}
+	if (tx) {
+		if (rx) {
+			/* No callback */
+			desc_tx->callback = NULL;
+		} else {
+			desc_tx->callback = rspi_dma_complete;
+			desc_tx->callback_param = rspi;
+		}
+		cookie = dmaengine_submit(desc_tx);
+		if (dma_submit_error(cookie))
+			return cookie;
+		dma_async_issue_pending(rspi->master->dma_tx);
+	}
 
 	ret = wait_event_interruptible_timeout(rspi->wait,
 					       rspi->dma_callbacked, HZ);
@@ -542,15 +531,13 @@ static int rspi_send_dma(struct rspi_data *rspi, struct spi_transfer *t)
 		ret = 0;
 	else if (!ret)
 		ret = -ETIMEDOUT;
-	rspi_disable_irq(rspi, SPCR_SPTIE);
 
-	enable_irq(rspi->tx_irq);
+	rspi_disable_irq(rspi, irq_mask);
 
-end:
-	rspi_dma_unmap_sg(&sg, rspi->chan_tx, DMA_TO_DEVICE);
-end_nomap:
-	if (rspi->dma_width_16bit)
-		kfree(buf);
+	if (tx)
+		enable_irq(rspi->tx_irq);
+	if (rx && rspi->rx_irq != other_irq)
+		enable_irq(rspi->rx_irq);
 
 	return ret;
 }
@@ -585,157 +572,37 @@ static void qspi_receive_init(const struct rspi_data *rspi)
 	rspi_write8(rspi, 0, QSPI_SPBFCR);
 }
 
-static int rspi_receive_dma(struct rspi_data *rspi, struct spi_transfer *t)
+static bool __rspi_can_dma(const struct rspi_data *rspi,
+			   const struct spi_transfer *xfer)
 {
-	struct scatterlist sg, sg_dummy;
-	void *dummy = NULL, *rx_buf = NULL;
-	struct dma_async_tx_descriptor *desc, *desc_dummy;
-	unsigned int len;
-	int ret = 0;
-
-	if (rspi->dma_width_16bit) {
-		/*
-		 * If DMAC bus width is 16-bit, the driver allocates a dummy
-		 * buffer. And, finally the driver converts the DMAC data into
-		 * actual data as the following format:
-		 *  DMAC data:   1st byte, dummy, 2nd byte, dummy ...
-		 *  actual data: 1st byte, 2nd byte ...
-		 */
-		len = t->len * 2;
-		rx_buf = kmalloc(len, GFP_KERNEL);
-		if (!rx_buf)
-			return -ENOMEM;
-	 } else {
-		len = t->len;
-		rx_buf = t->rx_buf;
-	}
-
-	/* prepare dummy transfer to generate SPI clocks */
-	dummy = kzalloc(len, GFP_KERNEL);
-	if (!dummy) {
-		ret = -ENOMEM;
-		goto end_nomap;
-	}
-	if (!rspi_dma_map_sg(&sg_dummy, dummy, len, rspi->chan_tx,
-			     DMA_TO_DEVICE)) {
-		ret = -EFAULT;
-		goto end_nomap;
-	}
-	desc_dummy = dmaengine_prep_slave_sg(rspi->chan_tx, &sg_dummy, 1,
-			DMA_TO_DEVICE, DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
-	if (!desc_dummy) {
-		ret = -EIO;
-		goto end_dummy_mapped;
-	}
-
-	/* prepare receive transfer */
-	if (!rspi_dma_map_sg(&sg, rx_buf, len, rspi->chan_rx,
-			     DMA_FROM_DEVICE)) {
-		ret = -EFAULT;
-		goto end_dummy_mapped;
-
-	}
-	desc = dmaengine_prep_slave_sg(rspi->chan_rx, &sg, 1, DMA_FROM_DEVICE,
-				       DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
-	if (!desc) {
-		ret = -EIO;
-		goto end;
-	}
-
-	rspi_receive_init(rspi);
-
-	/*
-	 * DMAC needs SPTIE, but if SPTIE is set, this IRQ routine will be
-	 * called. So, this driver disables the IRQ while DMA transfer.
-	 */
-	disable_irq(rspi->tx_irq);
-	if (rspi->rx_irq != rspi->tx_irq)
-		disable_irq(rspi->rx_irq);
-
-	rspi_write8(rspi, rspi_read8(rspi, RSPI_SPCR) & ~SPCR_TXMD, RSPI_SPCR);
-	rspi_enable_irq(rspi, SPCR_SPTIE | SPCR_SPRIE);
-	rspi->dma_callbacked = 0;
-
-	desc->callback = rspi_dma_complete;
-	desc->callback_param = rspi;
-	dmaengine_submit(desc);
-	dma_async_issue_pending(rspi->chan_rx);
-
-	desc_dummy->callback = NULL;	/* No callback */
-	dmaengine_submit(desc_dummy);
-	dma_async_issue_pending(rspi->chan_tx);
-
-	ret = wait_event_interruptible_timeout(rspi->wait,
-					       rspi->dma_callbacked, HZ);
-	if (ret > 0 && rspi->dma_callbacked)
-		ret = 0;
-	else if (!ret)
-		ret = -ETIMEDOUT;
-	rspi_disable_irq(rspi, SPCR_SPTIE | SPCR_SPRIE);
-
-	enable_irq(rspi->tx_irq);
-	if (rspi->rx_irq != rspi->tx_irq)
-		enable_irq(rspi->rx_irq);
-
-end:
-	rspi_dma_unmap_sg(&sg, rspi->chan_rx, DMA_FROM_DEVICE);
-end_dummy_mapped:
-	rspi_dma_unmap_sg(&sg_dummy, rspi->chan_tx, DMA_TO_DEVICE);
-end_nomap:
-	if (rspi->dma_width_16bit) {
-		if (!ret)
-			rspi_memory_from_8bit(t->rx_buf, rx_buf, t->len);
-		kfree(rx_buf);
-	}
-	kfree(dummy);
-
-	return ret;
+	return xfer->len > rspi->ops->fifo_size;
 }
 
-static int rspi_is_dma(const struct rspi_data *rspi, struct spi_transfer *t)
+static bool rspi_can_dma(struct spi_master *master, struct spi_device *spi,
+			 struct spi_transfer *xfer)
 {
-	if (t->tx_buf && rspi->chan_tx)
-		return 1;
-	/* If the module receives data by DMAC, it also needs TX DMAC */
-	if (t->rx_buf && rspi->chan_tx && rspi->chan_rx)
-		return 1;
+	struct rspi_data *rspi = spi_master_get_devdata(master);
 
-	return 0;
+	return __rspi_can_dma(rspi, xfer);
 }
 
-static int rspi_transfer_out_in(struct rspi_data *rspi,
+static int rspi_common_transfer(struct rspi_data *rspi,
 				struct spi_transfer *xfer)
 {
-	int remain = xfer->len, ret;
-	const u8 *tx_buf = xfer->tx_buf;
-	u8 *rx_buf = xfer->rx_buf;
-	u8 spcr, data;
-
-	rspi_receive_init(rspi);
-
-	spcr = rspi_read8(rspi, RSPI_SPCR);
-	if (rx_buf)
-		spcr &= ~SPCR_TXMD;
-	else
-		spcr |= SPCR_TXMD;
-	rspi_write8(rspi, spcr, RSPI_SPCR);
+	int ret;
 
-	while (remain > 0) {
-		data = tx_buf ? *tx_buf++ : DUMMY_DATA;
-		ret = rspi_data_out(rspi, data);
-		if (ret < 0)
-			return ret;
-		if (rx_buf) {
-			ret = rspi_data_in(rspi);
-			if (ret < 0)
-				return ret;
-			*rx_buf++ = ret;
-		}
-		remain--;
+	if (rspi->master->can_dma && __rspi_can_dma(rspi, xfer)) {
+		/* rx_buf can be NULL on RSPI on SH in TX-only Mode */
+		return rspi_dma_transfer(rspi, &xfer->tx_sg,
+					 xfer->rx_buf ? &xfer->rx_sg : NULL);
 	}
 
+	ret = rspi_pio_transfer(rspi, xfer->tx_buf, xfer->rx_buf, xfer->len);
+	if (ret < 0)
+		return ret;
+
 	/* Wait for the last transmission */
-	rspi_wait_for_interrupt(rspi, SPSR_SPTEF, SPCR_SPTIE);
+	rspi_wait_for_tx_empty(rspi);
 
 	return 0;
 }
@@ -744,46 +611,18 @@ static int rspi_transfer_one(struct spi_master *master, struct spi_device *spi,
 			     struct spi_transfer *xfer)
 {
 	struct rspi_data *rspi = spi_master_get_devdata(master);
-	int ret;
+	u8 spcr;
 
-	if (!rspi_is_dma(rspi, xfer))
-		return rspi_transfer_out_in(rspi, xfer);
-
-	if (xfer->tx_buf) {
-		ret = rspi_send_dma(rspi, xfer);
-		if (ret < 0)
-			return ret;
-	}
-	if (xfer->rx_buf)
-		return rspi_receive_dma(rspi, xfer);
-
-	return 0;
-}
-
-static int rspi_rz_transfer_out_in(struct rspi_data *rspi,
-				   struct spi_transfer *xfer)
-{
-	int remain = xfer->len, ret;
-	const u8 *tx_buf = xfer->tx_buf;
-	u8 *rx_buf = xfer->rx_buf;
-	u8 data;
-
-	rspi_rz_receive_init(rspi);
-
-	while (remain > 0) {
-		data = tx_buf ? *tx_buf++ : DUMMY_DATA;
-		ret = rspi_data_out_in(rspi, data);
-		if (ret < 0)
-			return ret;
-		if (rx_buf)
-			*rx_buf++ = ret;
-		remain--;
+	spcr = rspi_read8(rspi, RSPI_SPCR);
+	if (xfer->rx_buf) {
+		rspi_receive_init(rspi);
+		spcr &= ~SPCR_TXMD;
+	} else {
+		spcr |= SPCR_TXMD;
 	}
+	rspi_write8(rspi, spcr, RSPI_SPCR);
 
-	/* Wait for the last transmission */
-	rspi_wait_for_interrupt(rspi, SPSR_SPTEF, SPCR_SPTIE);
-
-	return 0;
+	return rspi_common_transfer(rspi, xfer);
 }
 
 static int rspi_rz_transfer_one(struct spi_master *master,
@@ -791,68 +630,44 @@ static int rspi_rz_transfer_one(struct spi_master *master,
 				struct spi_transfer *xfer)
 {
 	struct rspi_data *rspi = spi_master_get_devdata(master);
+	int ret;
 
-	return rspi_rz_transfer_out_in(rspi, xfer);
+	rspi_rz_receive_init(rspi);
+
+	return rspi_common_transfer(rspi, xfer);
 }
 
 static int qspi_transfer_out_in(struct rspi_data *rspi,
 				struct spi_transfer *xfer)
 {
-	int remain = xfer->len, ret;
-	const u8 *tx_buf = xfer->tx_buf;
-	u8 *rx_buf = xfer->rx_buf;
-	u8 data;
-
 	qspi_receive_init(rspi);
 
-	while (remain > 0) {
-		data = tx_buf ? *tx_buf++ : DUMMY_DATA;
-		ret = rspi_data_out_in(rspi, data);
-		if (ret < 0)
-			return ret;
-		if (rx_buf)
-			*rx_buf++ = ret;
-		remain--;
-	}
-
-	/* Wait for the last transmission */
-	rspi_wait_for_interrupt(rspi, SPSR_SPTEF, SPCR_SPTIE);
-
-	return 0;
+	return rspi_common_transfer(rspi, xfer);
 }
 
 static int qspi_transfer_out(struct rspi_data *rspi, struct spi_transfer *xfer)
 {
-	const u8 *buf = xfer->tx_buf;
-	unsigned int i;
 	int ret;
 
-	for (i = 0; i < xfer->len; i++) {
-		ret = rspi_data_out(rspi, *buf++);
-		if (ret < 0)
-			return ret;
-	}
+	if (rspi->master->can_dma && __rspi_can_dma(rspi, xfer))
+		return rspi_dma_transfer(rspi, &xfer->tx_sg, NULL);
+
+	ret = rspi_pio_transfer(rspi, xfer->tx_buf, NULL, xfer->len);
+	if (ret < 0)
+		return ret;
 
 	/* Wait for the last transmission */
-	rspi_wait_for_interrupt(rspi, SPSR_SPTEF, SPCR_SPTIE);
+	rspi_wait_for_tx_empty(rspi);
 
 	return 0;
 }
 
 static int qspi_transfer_in(struct rspi_data *rspi, struct spi_transfer *xfer)
 {
-	u8 *buf = xfer->rx_buf;
-	unsigned int i;
-	int ret;
-
-	for (i = 0; i < xfer->len; i++) {
-		ret = rspi_data_in(rspi);
-		if (ret < 0)
-			return ret;
-		*buf++ = ret;
-	}
+	if (rspi->master->can_dma && __rspi_can_dma(rspi, xfer))
+		return rspi_dma_transfer(rspi, NULL, &xfer->rx_sg);
 
-	return 0;
+	return rspi_pio_transfer(rspi, NULL, xfer->rx_buf, xfer->len);
 }
 
 static int qspi_transfer_one(struct spi_master *master, struct spi_device *spi,
@@ -862,10 +677,10 @@ static int qspi_transfer_one(struct spi_master *master, struct spi_device *spi,
 
 	if (spi->mode & SPI_LOOP) {
 		return qspi_transfer_out_in(rspi, xfer);
-	} else if (xfer->tx_buf && xfer->tx_nbits > SPI_NBITS_SINGLE) {
+	} else if (xfer->tx_nbits > SPI_NBITS_SINGLE) {
 		/* Quad or Dual SPI Write */
 		return qspi_transfer_out(rspi, xfer);
-	} else if (xfer->rx_buf && xfer->rx_nbits > SPI_NBITS_SINGLE) {
+	} else if (xfer->rx_nbits > SPI_NBITS_SINGLE) {
 		/* Quad or Dual SPI Read */
 		return qspi_transfer_in(rspi, xfer);
 	} else {
@@ -1046,65 +861,78 @@ static irqreturn_t rspi_irq_tx(int irq, void *_sr)
 	return 0;
 }
 
-static int rspi_request_dma(struct rspi_data *rspi,
-				      struct platform_device *pdev)
+static struct dma_chan *rspi_request_dma_chan(struct device *dev,
+					      enum dma_transfer_direction dir,
+					      unsigned int id,
+					      dma_addr_t port_addr)
 {
-	const struct rspi_plat_data *rspi_pd = dev_get_platdata(&pdev->dev);
-	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	dma_cap_mask_t mask;
+	struct dma_chan *chan;
 	struct dma_slave_config cfg;
 	int ret;
 
-	if (!res || !rspi_pd)
-		return 0;	/* The driver assumes no error. */
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_SLAVE, mask);
 
-	rspi->dma_width_16bit = rspi_pd->dma_width_16bit;
-
-	/* If the module receives data by DMAC, it also needs TX DMAC */
-	if (rspi_pd->dma_rx_id && rspi_pd->dma_tx_id) {
-		dma_cap_zero(mask);
-		dma_cap_set(DMA_SLAVE, mask);
-		rspi->chan_rx = dma_request_channel(mask, shdma_chan_filter,
-						    (void *)rspi_pd->dma_rx_id);
-		if (rspi->chan_rx) {
-			cfg.slave_id = rspi_pd->dma_rx_id;
-			cfg.direction = DMA_DEV_TO_MEM;
-			cfg.dst_addr = 0;
-			cfg.src_addr = res->start + RSPI_SPDR;
-			ret = dmaengine_slave_config(rspi->chan_rx, &cfg);
-			if (!ret)
-				dev_info(&pdev->dev, "Use DMA when rx.\n");
-			else
-				return ret;
-		}
+	chan = dma_request_channel(mask, shdma_chan_filter,
+				   (void *)(unsigned long)id);
+	if (!chan) {
+		dev_warn(dev, "dma_request_channel failed\n");
+		return NULL;
 	}
-	if (rspi_pd->dma_tx_id) {
-		dma_cap_zero(mask);
-		dma_cap_set(DMA_SLAVE, mask);
-		rspi->chan_tx = dma_request_channel(mask, shdma_chan_filter,
-						    (void *)rspi_pd->dma_tx_id);
-		if (rspi->chan_tx) {
-			cfg.slave_id = rspi_pd->dma_tx_id;
-			cfg.direction = DMA_MEM_TO_DEV;
-			cfg.dst_addr = res->start + RSPI_SPDR;
-			cfg.src_addr = 0;
-			ret = dmaengine_slave_config(rspi->chan_tx, &cfg);
-			if (!ret)
-				dev_info(&pdev->dev, "Use DMA when tx\n");
-			else
-				return ret;
-		}
+
+	memset(&cfg, 0, sizeof(cfg));
+	cfg.slave_id = id;
+	cfg.direction = dir;
+	if (dir == DMA_MEM_TO_DEV)
+		cfg.dst_addr = port_addr;
+	else
+		cfg.src_addr = port_addr;
+
+	ret = dmaengine_slave_config(chan, &cfg);
+	if (ret) {
+		dev_warn(dev, "dmaengine_slave_config failed %d\n", ret);
+		dma_release_channel(chan);
+		return NULL;
 	}
 
+	return chan;
+}
+
+static int rspi_request_dma(struct device *dev, struct spi_master *master,
+			    const struct resource *res)
+{
+	const struct rspi_plat_data *rspi_pd = dev_get_platdata(dev);
+
+	if (!rspi_pd || !rspi_pd->dma_rx_id || !rspi_pd->dma_tx_id)
+		return 0;	/* The driver assumes no error. */
+
+	master->dma_rx = rspi_request_dma_chan(dev, DMA_DEV_TO_MEM,
+					       rspi_pd->dma_rx_id,
+					       res->start + RSPI_SPDR);
+	if (!master->dma_rx)
+		return -ENODEV;
+
+	master->dma_tx = rspi_request_dma_chan(dev, DMA_MEM_TO_DEV,
+					       rspi_pd->dma_tx_id,
+					       res->start + RSPI_SPDR);
+	if (!master->dma_tx) {
+		dma_release_channel(master->dma_rx);
+		master->dma_rx = NULL;
+		return -ENODEV;
+	}
+
+	master->can_dma = rspi_can_dma;
+	dev_info(dev, "DMA available");
 	return 0;
 }
 
 static void rspi_release_dma(struct rspi_data *rspi)
 {
-	if (rspi->chan_tx)
-		dma_release_channel(rspi->chan_tx);
-	if (rspi->chan_rx)
-		dma_release_channel(rspi->chan_rx);
+	if (rspi->master->dma_tx)
+		dma_release_channel(rspi->master->dma_tx);
+	if (rspi->master->dma_rx)
+		dma_release_channel(rspi->master->dma_rx);
 }
 
 static int rspi_remove(struct platform_device *pdev)
@@ -1118,23 +946,29 @@ static int rspi_remove(struct platform_device *pdev)
 }
 
 static const struct spi_ops rspi_ops = {
-	.set_config_register =		rspi_set_config_register,
-	.transfer_one =			rspi_transfer_one,
-	.mode_bits =			SPI_CPHA | SPI_CPOL | SPI_LOOP,
+	.set_config_register =	rspi_set_config_register,
+	.transfer_one =		rspi_transfer_one,
+	.mode_bits =		SPI_CPHA | SPI_CPOL | SPI_LOOP,
+	.flags =		SPI_MASTER_MUST_TX,
+	.fifo_size =		8,
 };
 
 static const struct spi_ops rspi_rz_ops = {
-	.set_config_register =		rspi_rz_set_config_register,
-	.transfer_one =			rspi_rz_transfer_one,
-	.mode_bits =			SPI_CPHA | SPI_CPOL | SPI_LOOP,
+	.set_config_register =	rspi_rz_set_config_register,
+	.transfer_one =		rspi_rz_transfer_one,
+	.mode_bits =		SPI_CPHA | SPI_CPOL | SPI_LOOP,
+	.flags =		SPI_MASTER_MUST_RX | SPI_MASTER_MUST_TX,
+	.fifo_size =		8,	/* 8 for TX, 32 for RX */
 };
 
 static const struct spi_ops qspi_ops = {
-	.set_config_register =		qspi_set_config_register,
-	.transfer_one =			qspi_transfer_one,
-	.mode_bits =			SPI_CPHA | SPI_CPOL | SPI_LOOP |
-					SPI_TX_DUAL | SPI_TX_QUAD |
-					SPI_RX_DUAL | SPI_RX_QUAD,
+	.set_config_register =	qspi_set_config_register,
+	.transfer_one =		qspi_transfer_one,
+	.mode_bits =		SPI_CPHA | SPI_CPOL | SPI_LOOP |
+				SPI_TX_DUAL | SPI_TX_QUAD |
+				SPI_RX_DUAL | SPI_RX_QUAD,
+	.flags =		SPI_MASTER_MUST_RX | SPI_MASTER_MUST_TX,
+	.fifo_size =		32,
 };
 
 #ifdef CONFIG_OF
@@ -1254,6 +1088,7 @@ static int rspi_probe(struct platform_device *pdev)
 	master->prepare_message = rspi_prepare_message;
 	master->unprepare_message = rspi_unprepare_message;
 	master->mode_bits = ops->mode_bits;
+	master->flags = ops->flags;
 	master->dev.of_node = pdev->dev.of_node;
 
 	ret = platform_get_irq_byname(pdev, "rx");
@@ -1291,11 +1126,9 @@ static int rspi_probe(struct platform_device *pdev)
 		goto error2;
 	}
 
-	ret = rspi_request_dma(rspi, pdev);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "rspi_request_dma failed.\n");
-		goto error3;
-	}
+	ret = rspi_request_dma(&pdev->dev, master, res);
+	if (ret < 0)
+		dev_warn(&pdev->dev, "DMA not available, using PIO\n");
 
 	ret = devm_spi_register_master(&pdev->dev, master);
 	if (ret < 0) {
diff --git a/drivers/spi/spi-s3c24xx.c b/drivers/spi/spi-s3c24xx.c
index bed23384dfa..e713737d784 100644
--- a/drivers/spi/spi-s3c24xx.c
+++ b/drivers/spi/spi-s3c24xx.c
@@ -10,7 +10,6 @@
 */
 
 #include <linux/spinlock.h>
-#include <linux/workqueue.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
 #include <linux/errno.h>
@@ -183,11 +182,11 @@ static int s3c24xx_spi_setup(struct spi_device *spi)
 
 	/* allocate settings on the first call */
 	if (!cs) {
-		cs = kzalloc(sizeof(struct s3c24xx_spi_devstate), GFP_KERNEL);
-		if (!cs) {
-			dev_err(&spi->dev, "no memory for controller state\n");
+		cs = devm_kzalloc(&spi->dev,
+				  sizeof(struct s3c24xx_spi_devstate),
+				  GFP_KERNEL);
+		if (!cs)
 			return -ENOMEM;
-		}
 
 		cs->spcon = SPCON_DEFAULT;
 		cs->hz = -1;
@@ -209,11 +208,6 @@ static int s3c24xx_spi_setup(struct spi_device *spi)
 	return 0;
 }
 
-static void s3c24xx_spi_cleanup(struct spi_device *spi)
-{
-	kfree(spi->controller_state);
-}
-
 static inline unsigned int hw_txbyte(struct s3c24xx_spi *hw, int count)
 {
 	return hw->tx ? hw->tx[count] : 0;
@@ -543,7 +537,6 @@ static int s3c24xx_spi_probe(struct platform_device *pdev)
 	hw->bitbang.txrx_bufs      = s3c24xx_spi_txrx;
 
 	hw->master->setup  = s3c24xx_spi_setup;
-	hw->master->cleanup = s3c24xx_spi_cleanup;
 
 	dev_dbg(hw->dev, "bitbang at %p\n", &hw->bitbang);
 
diff --git a/drivers/spi/spi-s3c64xx.c b/drivers/spi/spi-s3c64xx.c
index f19cd97855e..75a56968b14 100644
--- a/drivers/spi/spi-s3c64xx.c
+++ b/drivers/spi/spi-s3c64xx.c
@@ -19,7 +19,6 @@
 
 #include <linux/init.h>
 #include <linux/module.h>
-#include <linux/workqueue.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
 #include <linux/clk.h>
@@ -773,7 +772,6 @@ static struct s3c64xx_spi_csinfo *s3c64xx_get_slave_ctrldata(
 
 	cs = kzalloc(sizeof(*cs), GFP_KERNEL);
 	if (!cs) {
-		dev_err(&spi->dev, "could not allocate memory for controller data\n");
 		of_node_put(data_np);
 		return ERR_PTR(-ENOMEM);
 	}
@@ -987,10 +985,8 @@ static struct s3c64xx_spi_info *s3c64xx_spi_parse_dt(struct device *dev)
 	u32 temp;
 
 	sci = devm_kzalloc(dev, sizeof(*sci), GFP_KERNEL);
-	if (!sci) {
-		dev_err(dev, "memory allocation for spi_info failed\n");
+	if (!sci)
 		return ERR_PTR(-ENOMEM);
-	}
 
 	if (of_property_read_u32(dev->of_node, "samsung,spi-src-clk", &temp)) {
 		dev_warn(dev, "spi bus clock parent not specified, using clock at index 0 as parent\n");
diff --git a/drivers/spi/spi-sh-hspi.c b/drivers/spi/spi-sh-hspi.c
index 9009456bdf4..c8e795ef2e1 100644
--- a/drivers/spi/spi-sh-hspi.c
+++ b/drivers/spi/spi-sh-hspi.c
@@ -244,9 +244,9 @@ static int hspi_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
-	clk = clk_get(NULL, "shyway_clk");
+	clk = clk_get(&pdev->dev, NULL);
 	if (IS_ERR(clk)) {
-		dev_err(&pdev->dev, "shyway_clk is required\n");
+		dev_err(&pdev->dev, "couldn't get clock\n");
 		ret = -EINVAL;
 		goto error0;
 	}
diff --git a/drivers/spi/spi-sh-msiof.c b/drivers/spi/spi-sh-msiof.c
index e850d03e719..45b09142afe 100644
--- a/drivers/spi/spi-sh-msiof.c
+++ b/drivers/spi/spi-sh-msiof.c
@@ -642,10 +642,8 @@ static struct sh_msiof_spi_info *sh_msiof_spi_parse_dt(struct device *dev)
 	u32 num_cs = 1;
 
 	info = devm_kzalloc(dev, sizeof(struct sh_msiof_spi_info), GFP_KERNEL);
-	if (!info) {
-		dev_err(dev, "failed to allocate setup data\n");
+	if (!info)
 		return NULL;
-	}
 
 	/* Parse the MSIOF properties */
 	of_property_read_u32(np, "num-cs", &num_cs);
diff --git a/drivers/spi/spi-sh-sci.c b/drivers/spi/spi-sh-sci.c
index 8b44b71f502..1f56ef651d1 100644
--- a/drivers/spi/spi-sh-sci.c
+++ b/drivers/spi/spi-sh-sci.c
@@ -16,7 +16,6 @@
 #include <linux/kernel.h>
 #include <linux/delay.h>
 #include <linux/spinlock.h>
-#include <linux/workqueue.h>
 #include <linux/platform_device.h>
 
 #include <linux/spi/spi.h>
diff --git a/drivers/spi/spi-sirf.c b/drivers/spi/spi-sirf.c
index 1a77ad52812..95ac276eaaf 100644
--- a/drivers/spi/spi-sirf.c
+++ b/drivers/spi/spi-sirf.c
@@ -10,6 +10,7 @@
 #include <linux/kernel.h>
 #include <linux/slab.h>
 #include <linux/clk.h>
+#include <linux/completion.h>
 #include <linux/interrupt.h>
 #include <linux/io.h>
 #include <linux/of.h>
@@ -85,6 +86,7 @@
 #define SIRFSOC_SPI_TX_DONE		BIT(1)
 #define SIRFSOC_SPI_RX_OFLOW		BIT(2)
 #define SIRFSOC_SPI_TX_UFLOW		BIT(3)
+#define SIRFSOC_SPI_RX_IO_DMA		BIT(4)
 #define SIRFSOC_SPI_RX_FIFO_FULL	BIT(6)
 #define SIRFSOC_SPI_TXFIFO_EMPTY	BIT(7)
 #define SIRFSOC_SPI_RXFIFO_THD_REACH	BIT(8)
@@ -264,41 +266,34 @@ static irqreturn_t spi_sirfsoc_irq(int irq, void *dev_id)
 {
 	struct sirfsoc_spi *sspi = dev_id;
 	u32 spi_stat = readl(sspi->base + SIRFSOC_SPI_INT_STATUS);
-
-	writel(spi_stat, sspi->base + SIRFSOC_SPI_INT_STATUS);
-
 	if (sspi->tx_by_cmd && (spi_stat & SIRFSOC_SPI_FRM_END)) {
 		complete(&sspi->tx_done);
 		writel(0x0, sspi->base + SIRFSOC_SPI_INT_EN);
+		writel(SIRFSOC_SPI_INT_MASK_ALL,
+				sspi->base + SIRFSOC_SPI_INT_STATUS);
 		return IRQ_HANDLED;
 	}
 
 	/* Error Conditions */
 	if (spi_stat & SIRFSOC_SPI_RX_OFLOW ||
 			spi_stat & SIRFSOC_SPI_TX_UFLOW) {
+		complete(&sspi->tx_done);
 		complete(&sspi->rx_done);
 		writel(0x0, sspi->base + SIRFSOC_SPI_INT_EN);
+		writel(SIRFSOC_SPI_INT_MASK_ALL,
+				sspi->base + SIRFSOC_SPI_INT_STATUS);
+		return IRQ_HANDLED;
 	}
+	if (spi_stat & SIRFSOC_SPI_TXFIFO_EMPTY)
+		complete(&sspi->tx_done);
+	while (!(readl(sspi->base + SIRFSOC_SPI_INT_STATUS) &
+		SIRFSOC_SPI_RX_IO_DMA))
+		cpu_relax();
+	complete(&sspi->rx_done);
+	writel(0x0, sspi->base + SIRFSOC_SPI_INT_EN);
+	writel(SIRFSOC_SPI_INT_MASK_ALL,
+			sspi->base + SIRFSOC_SPI_INT_STATUS);
 
-	if (spi_stat & (SIRFSOC_SPI_FRM_END
-			| SIRFSOC_SPI_RXFIFO_THD_REACH))
-		while (!((readl(sspi->base + SIRFSOC_SPI_RXFIFO_STATUS)
-				& SIRFSOC_SPI_FIFO_EMPTY)) &&
-				sspi->left_rx_word)
-			sspi->rx_word(sspi);
-
-	if (spi_stat & (SIRFSOC_SPI_FIFO_EMPTY
-			| SIRFSOC_SPI_TXFIFO_THD_REACH))
-		while (!((readl(sspi->base + SIRFSOC_SPI_TXFIFO_STATUS)
-				& SIRFSOC_SPI_FIFO_FULL)) &&
-				sspi->left_tx_word)
-			sspi->tx_word(sspi);
-
-	/* Received all words */
-	if ((sspi->left_rx_word == 0) && (sspi->left_tx_word == 0)) {
-		complete(&sspi->rx_done);
-		writel(0x0, sspi->base + SIRFSOC_SPI_INT_EN);
-	}
 	return IRQ_HANDLED;
 }
 
@@ -309,59 +304,51 @@ static void spi_sirfsoc_dma_fini_callback(void *data)
 	complete(dma_complete);
 }
 
-static int spi_sirfsoc_transfer(struct spi_device *spi, struct spi_transfer *t)
+static int spi_sirfsoc_cmd_transfer(struct spi_device *spi,
+	struct spi_transfer *t)
 {
 	struct sirfsoc_spi *sspi;
 	int timeout = t->len * 10;
-	sspi = spi_master_get_devdata(spi->master);
-
-	sspi->tx = t->tx_buf ? t->tx_buf : sspi->dummypage;
-	sspi->rx = t->rx_buf ? t->rx_buf : sspi->dummypage;
-	sspi->left_tx_word = sspi->left_rx_word = t->len / sspi->word_width;
-	reinit_completion(&sspi->rx_done);
-	reinit_completion(&sspi->tx_done);
+	u32 cmd;
 
-	writel(SIRFSOC_SPI_INT_MASK_ALL, sspi->base + SIRFSOC_SPI_INT_STATUS);
-
-	/*
-	 * fill tx_buf into command register and wait for its completion
-	 */
-	if (sspi->tx_by_cmd) {
-		u32 cmd;
-		memcpy(&cmd, sspi->tx, t->len);
-
-		if (sspi->word_width == 1 && !(spi->mode & SPI_LSB_FIRST))
-			cmd = cpu_to_be32(cmd) >>
-				((SIRFSOC_MAX_CMD_BYTES - t->len) * 8);
-		if (sspi->word_width == 2 && t->len == 4 &&
-				(!(spi->mode & SPI_LSB_FIRST)))
-			cmd = ((cmd & 0xffff) << 16) | (cmd >> 16);
-
-		writel(cmd, sspi->base + SIRFSOC_SPI_CMD);
-		writel(SIRFSOC_SPI_FRM_END_INT_EN,
-			sspi->base + SIRFSOC_SPI_INT_EN);
-		writel(SIRFSOC_SPI_CMD_TX_EN,
-			sspi->base + SIRFSOC_SPI_TX_RX_EN);
+	sspi = spi_master_get_devdata(spi->master);
+	memcpy(&cmd, sspi->tx, t->len);
+	if (sspi->word_width == 1 && !(spi->mode & SPI_LSB_FIRST))
+		cmd = cpu_to_be32(cmd) >>
+			((SIRFSOC_MAX_CMD_BYTES - t->len) * 8);
+	if (sspi->word_width == 2 && t->len == 4 &&
+			(!(spi->mode & SPI_LSB_FIRST)))
+		cmd = ((cmd & 0xffff) << 16) | (cmd >> 16);
+	writel(cmd, sspi->base + SIRFSOC_SPI_CMD);
+	writel(SIRFSOC_SPI_FRM_END_INT_EN,
+		sspi->base + SIRFSOC_SPI_INT_EN);
+	writel(SIRFSOC_SPI_CMD_TX_EN,
+		sspi->base + SIRFSOC_SPI_TX_RX_EN);
+	if (wait_for_completion_timeout(&sspi->tx_done, timeout) == 0) {
+		dev_err(&spi->dev, "cmd transfer timeout\n");
+		return 0;
+	}
 
-		if (wait_for_completion_timeout(&sspi->tx_done, timeout) == 0) {
-			dev_err(&spi->dev, "transfer timeout\n");
-			return 0;
-		}
+	return t->len;
+}
 
-		return t->len;
-	}
+static void spi_sirfsoc_dma_transfer(struct spi_device *spi,
+	struct spi_transfer *t)
+{
+	struct sirfsoc_spi *sspi;
+	struct dma_async_tx_descriptor *rx_desc, *tx_desc;
+	int timeout = t->len * 10;
 
-	if (sspi->left_tx_word == 1) {
-		writel(readl(sspi->base + SIRFSOC_SPI_CTRL) |
-			SIRFSOC_SPI_ENA_AUTO_CLR,
-			sspi->base + SIRFSOC_SPI_CTRL);
-		writel(0, sspi->base + SIRFSOC_SPI_TX_DMA_IO_LEN);
-		writel(0, sspi->base + SIRFSOC_SPI_RX_DMA_IO_LEN);
-	} else if ((sspi->left_tx_word > 1) && (sspi->left_tx_word <
-				SIRFSOC_SPI_DAT_FRM_LEN_MAX)) {
+	sspi = spi_master_get_devdata(spi->master);
+	writel(SIRFSOC_SPI_FIFO_RESET, sspi->base + SIRFSOC_SPI_RXFIFO_OP);
+	writel(SIRFSOC_SPI_FIFO_RESET, sspi->base + SIRFSOC_SPI_TXFIFO_OP);
+	writel(SIRFSOC_SPI_FIFO_START, sspi->base + SIRFSOC_SPI_RXFIFO_OP);
+	writel(SIRFSOC_SPI_FIFO_START, sspi->base + SIRFSOC_SPI_TXFIFO_OP);
+	writel(0, sspi->base + SIRFSOC_SPI_INT_EN);
+	writel(SIRFSOC_SPI_INT_MASK_ALL, sspi->base + SIRFSOC_SPI_INT_STATUS);
+	if (sspi->left_tx_word < SIRFSOC_SPI_DAT_FRM_LEN_MAX) {
 		writel(readl(sspi->base + SIRFSOC_SPI_CTRL) |
-				SIRFSOC_SPI_MUL_DAT_MODE |
-				SIRFSOC_SPI_ENA_AUTO_CLR,
+			SIRFSOC_SPI_ENA_AUTO_CLR | SIRFSOC_SPI_MUL_DAT_MODE,
 			sspi->base + SIRFSOC_SPI_CTRL);
 		writel(sspi->left_tx_word - 1,
 				sspi->base + SIRFSOC_SPI_TX_DMA_IO_LEN);
@@ -373,76 +360,122 @@ static int spi_sirfsoc_transfer(struct spi_device *spi, struct spi_transfer *t)
 		writel(0, sspi->base + SIRFSOC_SPI_TX_DMA_IO_LEN);
 		writel(0, sspi->base + SIRFSOC_SPI_RX_DMA_IO_LEN);
 	}
-
-	writel(SIRFSOC_SPI_FIFO_RESET, sspi->base + SIRFSOC_SPI_RXFIFO_OP);
-	writel(SIRFSOC_SPI_FIFO_RESET, sspi->base + SIRFSOC_SPI_TXFIFO_OP);
-	writel(SIRFSOC_SPI_FIFO_START, sspi->base + SIRFSOC_SPI_RXFIFO_OP);
-	writel(SIRFSOC_SPI_FIFO_START, sspi->base + SIRFSOC_SPI_TXFIFO_OP);
-
-	if (IS_DMA_VALID(t)) {
-		struct dma_async_tx_descriptor *rx_desc, *tx_desc;
-
-		sspi->dst_start = dma_map_single(&spi->dev, sspi->rx, t->len, DMA_FROM_DEVICE);
-		rx_desc = dmaengine_prep_slave_single(sspi->rx_chan,
-			sspi->dst_start, t->len, DMA_DEV_TO_MEM,
-			DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
-		rx_desc->callback = spi_sirfsoc_dma_fini_callback;
-		rx_desc->callback_param = &sspi->rx_done;
-
-		sspi->src_start = dma_map_single(&spi->dev, (void *)sspi->tx, t->len, DMA_TO_DEVICE);
-		tx_desc = dmaengine_prep_slave_single(sspi->tx_chan,
-			sspi->src_start, t->len, DMA_MEM_TO_DEV,
-			DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
-		tx_desc->callback = spi_sirfsoc_dma_fini_callback;
-		tx_desc->callback_param = &sspi->tx_done;
-
-		dmaengine_submit(tx_desc);
-		dmaengine_submit(rx_desc);
-		dma_async_issue_pending(sspi->tx_chan);
-		dma_async_issue_pending(sspi->rx_chan);
-	} else {
-		/* Send the first word to trigger the whole tx/rx process */
-		sspi->tx_word(sspi);
-
-		writel(SIRFSOC_SPI_RX_OFLOW_INT_EN | SIRFSOC_SPI_TX_UFLOW_INT_EN |
-			SIRFSOC_SPI_RXFIFO_THD_INT_EN | SIRFSOC_SPI_TXFIFO_THD_INT_EN |
-			SIRFSOC_SPI_FRM_END_INT_EN | SIRFSOC_SPI_RXFIFO_FULL_INT_EN |
-			SIRFSOC_SPI_TXFIFO_EMPTY_INT_EN, sspi->base + SIRFSOC_SPI_INT_EN);
-	}
-
-	writel(SIRFSOC_SPI_RX_EN | SIRFSOC_SPI_TX_EN, sspi->base + SIRFSOC_SPI_TX_RX_EN);
-
-	if (!IS_DMA_VALID(t)) { /* for PIO */
-		if (wait_for_completion_timeout(&sspi->rx_done, timeout) == 0)
-			dev_err(&spi->dev, "transfer timeout\n");
-	} else if (wait_for_completion_timeout(&sspi->rx_done, timeout) == 0) {
+	sspi->dst_start = dma_map_single(&spi->dev, sspi->rx, t->len,
+					(t->tx_buf != t->rx_buf) ?
+					DMA_FROM_DEVICE : DMA_BIDIRECTIONAL);
+	rx_desc = dmaengine_prep_slave_single(sspi->rx_chan,
+		sspi->dst_start, t->len, DMA_DEV_TO_MEM,
+		DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+	rx_desc->callback = spi_sirfsoc_dma_fini_callback;
+	rx_desc->callback_param = &sspi->rx_done;
+
+	sspi->src_start = dma_map_single(&spi->dev, (void *)sspi->tx, t->len,
+					(t->tx_buf != t->rx_buf) ?
+					DMA_TO_DEVICE : DMA_BIDIRECTIONAL);
+	tx_desc = dmaengine_prep_slave_single(sspi->tx_chan,
+		sspi->src_start, t->len, DMA_MEM_TO_DEV,
+		DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+	tx_desc->callback = spi_sirfsoc_dma_fini_callback;
+	tx_desc->callback_param = &sspi->tx_done;
+
+	dmaengine_submit(tx_desc);
+	dmaengine_submit(rx_desc);
+	dma_async_issue_pending(sspi->tx_chan);
+	dma_async_issue_pending(sspi->rx_chan);
+	writel(SIRFSOC_SPI_RX_EN | SIRFSOC_SPI_TX_EN,
+			sspi->base + SIRFSOC_SPI_TX_RX_EN);
+	if (wait_for_completion_timeout(&sspi->rx_done, timeout) == 0) {
 		dev_err(&spi->dev, "transfer timeout\n");
 		dmaengine_terminate_all(sspi->rx_chan);
 	} else
 		sspi->left_rx_word = 0;
-
 	/*
 	 * we only wait tx-done event if transferring by DMA. for PIO,
 	 * we get rx data by writing tx data, so if rx is done, tx has
 	 * done earlier
 	 */
-	if (IS_DMA_VALID(t)) {
-		if (wait_for_completion_timeout(&sspi->tx_done, timeout) == 0) {
-			dev_err(&spi->dev, "transfer timeout\n");
-			dmaengine_terminate_all(sspi->tx_chan);
-		}
-	}
-
-	if (IS_DMA_VALID(t)) {
-		dma_unmap_single(&spi->dev, sspi->src_start, t->len, DMA_TO_DEVICE);
-		dma_unmap_single(&spi->dev, sspi->dst_start, t->len, DMA_FROM_DEVICE);
+	if (wait_for_completion_timeout(&sspi->tx_done, timeout) == 0) {
+		dev_err(&spi->dev, "transfer timeout\n");
+		dmaengine_terminate_all(sspi->tx_chan);
 	}
-
+	dma_unmap_single(&spi->dev, sspi->src_start, t->len, DMA_TO_DEVICE);
+	dma_unmap_single(&spi->dev, sspi->dst_start, t->len, DMA_FROM_DEVICE);
 	/* TX, RX FIFO stop */
 	writel(0, sspi->base + SIRFSOC_SPI_RXFIFO_OP);
 	writel(0, sspi->base + SIRFSOC_SPI_TXFIFO_OP);
-	writel(0, sspi->base + SIRFSOC_SPI_TX_RX_EN);
-	writel(0, sspi->base + SIRFSOC_SPI_INT_EN);
+	if (sspi->left_tx_word >= SIRFSOC_SPI_DAT_FRM_LEN_MAX)
+		writel(0, sspi->base + SIRFSOC_SPI_TX_RX_EN);
+}
+
+static void spi_sirfsoc_pio_transfer(struct spi_device *spi,
+		struct spi_transfer *t)
+{
+	struct sirfsoc_spi *sspi;
+	int timeout = t->len * 10;
+
+	sspi = spi_master_get_devdata(spi->master);
+	do {
+		writel(SIRFSOC_SPI_FIFO_RESET,
+			sspi->base + SIRFSOC_SPI_RXFIFO_OP);
+		writel(SIRFSOC_SPI_FIFO_RESET,
+			sspi->base + SIRFSOC_SPI_TXFIFO_OP);
+		writel(SIRFSOC_SPI_FIFO_START,
+			sspi->base + SIRFSOC_SPI_RXFIFO_OP);
+		writel(SIRFSOC_SPI_FIFO_START,
+			sspi->base + SIRFSOC_SPI_TXFIFO_OP);
+		writel(0, sspi->base + SIRFSOC_SPI_INT_EN);
+		writel(SIRFSOC_SPI_INT_MASK_ALL,
+			sspi->base + SIRFSOC_SPI_INT_STATUS);
+		writel(readl(sspi->base + SIRFSOC_SPI_CTRL) |
+			SIRFSOC_SPI_MUL_DAT_MODE | SIRFSOC_SPI_ENA_AUTO_CLR,
+			sspi->base + SIRFSOC_SPI_CTRL);
+		writel(min(sspi->left_tx_word, (u32)(256 / sspi->word_width))
+				- 1, sspi->base + SIRFSOC_SPI_TX_DMA_IO_LEN);
+		writel(min(sspi->left_rx_word, (u32)(256 / sspi->word_width))
+				- 1, sspi->base + SIRFSOC_SPI_RX_DMA_IO_LEN);
+		while (!((readl(sspi->base + SIRFSOC_SPI_TXFIFO_STATUS)
+			& SIRFSOC_SPI_FIFO_FULL)) && sspi->left_tx_word)
+			sspi->tx_word(sspi);
+		writel(SIRFSOC_SPI_TXFIFO_EMPTY_INT_EN |
+			SIRFSOC_SPI_TX_UFLOW_INT_EN |
+			SIRFSOC_SPI_RX_OFLOW_INT_EN,
+			sspi->base + SIRFSOC_SPI_INT_EN);
+		writel(SIRFSOC_SPI_RX_EN | SIRFSOC_SPI_TX_EN,
+			sspi->base + SIRFSOC_SPI_TX_RX_EN);
+		if (!wait_for_completion_timeout(&sspi->tx_done, timeout) ||
+			!wait_for_completion_timeout(&sspi->rx_done, timeout)) {
+			dev_err(&spi->dev, "transfer timeout\n");
+			break;
+		}
+		while (!((readl(sspi->base + SIRFSOC_SPI_RXFIFO_STATUS)
+			& SIRFSOC_SPI_FIFO_EMPTY)) && sspi->left_rx_word)
+			sspi->rx_word(sspi);
+		writel(0, sspi->base + SIRFSOC_SPI_RXFIFO_OP);
+		writel(0, sspi->base + SIRFSOC_SPI_TXFIFO_OP);
+	} while (sspi->left_tx_word != 0 || sspi->left_rx_word != 0);
+}
+
+static int spi_sirfsoc_transfer(struct spi_device *spi, struct spi_transfer *t)
+{
+	struct sirfsoc_spi *sspi;
+	sspi = spi_master_get_devdata(spi->master);
+
+	sspi->tx = t->tx_buf ? t->tx_buf : sspi->dummypage;
+	sspi->rx = t->rx_buf ? t->rx_buf : sspi->dummypage;
+	sspi->left_tx_word = sspi->left_rx_word = t->len / sspi->word_width;
+	reinit_completion(&sspi->rx_done);
+	reinit_completion(&sspi->tx_done);
+	/*
+	 * in the transfer, if transfer data using command register with rx_buf
+	 * null, just fill command data into command register and wait for its
+	 * completion.
+	 */
+	if (sspi->tx_by_cmd)
+		spi_sirfsoc_cmd_transfer(spi, t);
+	else if (IS_DMA_VALID(t))
+		spi_sirfsoc_dma_transfer(spi, t);
+	else
+		spi_sirfsoc_pio_transfer(spi, t);
 
 	return t->len - sspi->left_rx_word * sspi->word_width;
 }
@@ -470,7 +503,16 @@ static void spi_sirfsoc_chipselect(struct spi_device *spi, int value)
 		writel(regval, sspi->base + SIRFSOC_SPI_CTRL);
 	} else {
 		int gpio = sspi->chipselect[spi->chip_select];
-		gpio_direction_output(gpio, spi->mode & SPI_CS_HIGH ? 0 : 1);
+		switch (value) {
+		case BITBANG_CS_ACTIVE:
+			gpio_direction_output(gpio,
+					spi->mode & SPI_CS_HIGH ? 1 : 0);
+			break;
+		case BITBANG_CS_INACTIVE:
+			gpio_direction_output(gpio,
+					spi->mode & SPI_CS_HIGH ? 0 : 1);
+			break;
+		}
 	}
 }
 
@@ -503,7 +545,8 @@ spi_sirfsoc_setup_transfer(struct spi_device *spi, struct spi_transfer *t)
 		break;
 	case 12:
 	case 16:
-		regval |= (bits_per_word ==  12) ? SIRFSOC_SPI_TRAN_DAT_FORMAT_12 :
+		regval |= (bits_per_word ==  12) ?
+			SIRFSOC_SPI_TRAN_DAT_FORMAT_12 :
 			SIRFSOC_SPI_TRAN_DAT_FORMAT_16;
 		sspi->rx_word = spi_sirfsoc_rx_word_u16;
 		sspi->tx_word = spi_sirfsoc_tx_word_u16;
@@ -531,8 +574,8 @@ spi_sirfsoc_setup_transfer(struct spi_device *spi, struct spi_transfer *t)
 		regval |= SIRFSOC_SPI_CLK_IDLE_STAT;
 
 	/*
-	 * Data should be driven at least 1/2 cycle before the fetch edge to make
-	 * sure that data gets stable at the fetch edge.
+	 * Data should be driven at least 1/2 cycle before the fetch edge
+	 * to make sure that data gets stable at the fetch edge.
 	 */
 	if (((spi->mode & SPI_CPOL) && (spi->mode & SPI_CPHA)) ||
 	    (!(spi->mode & SPI_CPOL) && !(spi->mode & SPI_CPHA)))
@@ -559,16 +602,24 @@ spi_sirfsoc_setup_transfer(struct spi_device *spi, struct spi_transfer *t)
 		regval &= ~SIRFSOC_SPI_CMD_MODE;
 		sspi->tx_by_cmd = false;
 	}
+	/*
+	 * set spi controller in RISC chipselect mode, we are controlling CS by
+	 * software BITBANG_CS_ACTIVE and BITBANG_CS_INACTIVE.
+	 */
+	regval |= SIRFSOC_SPI_CS_IO_MODE;
 	writel(regval, sspi->base + SIRFSOC_SPI_CTRL);
 
 	if (IS_DMA_VALID(t)) {
 		/* Enable DMA mode for RX, TX */
 		writel(0, sspi->base + SIRFSOC_SPI_TX_DMA_IO_CTRL);
-		writel(SIRFSOC_SPI_RX_DMA_FLUSH, sspi->base + SIRFSOC_SPI_RX_DMA_IO_CTRL);
+		writel(SIRFSOC_SPI_RX_DMA_FLUSH,
+			sspi->base + SIRFSOC_SPI_RX_DMA_IO_CTRL);
 	} else {
 		/* Enable IO mode for RX, TX */
-		writel(SIRFSOC_SPI_IO_MODE_SEL, sspi->base + SIRFSOC_SPI_TX_DMA_IO_CTRL);
-		writel(SIRFSOC_SPI_IO_MODE_SEL, sspi->base + SIRFSOC_SPI_RX_DMA_IO_CTRL);
+		writel(SIRFSOC_SPI_IO_MODE_SEL,
+			sspi->base + SIRFSOC_SPI_TX_DMA_IO_CTRL);
+		writel(SIRFSOC_SPI_IO_MODE_SEL,
+			sspi->base + SIRFSOC_SPI_RX_DMA_IO_CTRL);
 	}
 
 	return 0;
@@ -598,7 +649,8 @@ static int spi_sirfsoc_probe(struct platform_device *pdev)
 		goto err_cs;
 	}
 
-	master = spi_alloc_master(&pdev->dev, sizeof(*sspi) + sizeof(int) * num_cs);
+	master = spi_alloc_master(&pdev->dev,
+			sizeof(*sspi) + sizeof(int) * num_cs);
 	if (!master) {
 		dev_err(&pdev->dev, "Unable to allocate SPI master\n");
 		return -ENOMEM;
@@ -794,8 +846,7 @@ static struct platform_driver spi_sirfsoc_driver = {
 	.remove = spi_sirfsoc_remove,
 };
 module_platform_driver(spi_sirfsoc_driver);
-
 MODULE_DESCRIPTION("SiRF SoC SPI master driver");
-MODULE_AUTHOR("Zhiwu Song <Zhiwu.Song@csr.com>, "
-		"Barry Song <Baohua.Song@csr.com>");
+MODULE_AUTHOR("Zhiwu Song <Zhiwu.Song@csr.com>");
+MODULE_AUTHOR("Barry Song <Baohua.Song@csr.com>");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/spi/spi-sun4i.c b/drivers/spi/spi-sun4i.c
index d266a870206..85204c93f3d 100644
--- a/drivers/spi/spi-sun4i.c
+++ b/drivers/spi/spi-sun4i.c
@@ -19,7 +19,6 @@
 #include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
-#include <linux/workqueue.h>
 
 #include <linux/spi/spi.h>
 
diff --git a/drivers/spi/spi-sun6i.c b/drivers/spi/spi-sun6i.c
index b3e3498a7e6..bd24093f403 100644
--- a/drivers/spi/spi-sun6i.c
+++ b/drivers/spi/spi-sun6i.c
@@ -20,7 +20,6 @@
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/reset.h>
-#include <linux/workqueue.h>
 
 #include <linux/spi/spi.h>
 
diff --git a/drivers/spi/spi-tegra114.c b/drivers/spi/spi-tegra114.c
index 40064959550..e4a85ada861 100644
--- a/drivers/spi/spi-tegra114.c
+++ b/drivers/spi/spi-tegra114.c
@@ -1012,7 +1012,7 @@ static irqreturn_t tegra_spi_isr(int irq, void *context_data)
 	return IRQ_WAKE_THREAD;
 }
 
-static struct of_device_id tegra_spi_of_match[] = {
+static const struct of_device_id tegra_spi_of_match[] = {
 	{ .compatible = "nvidia,tegra114-spi", },
 	{}
 };
diff --git a/drivers/spi/spi-tegra20-sflash.c b/drivers/spi/spi-tegra20-sflash.c
index 47869ea636e..3548ce25c08 100644
--- a/drivers/spi/spi-tegra20-sflash.c
+++ b/drivers/spi/spi-tegra20-sflash.c
@@ -419,7 +419,7 @@ static irqreturn_t tegra_sflash_isr(int irq, void *context_data)
 	return handle_cpu_based_xfer(tsd);
 }
 
-static struct of_device_id tegra_sflash_of_match[] = {
+static const struct of_device_id tegra_sflash_of_match[] = {
 	{ .compatible = "nvidia,tegra20-sflash", },
 	{}
 };
diff --git a/drivers/spi/spi-tegra20-slink.c b/drivers/spi/spi-tegra20-slink.c
index e3c1b93e45d..0b9e32e9f49 100644
--- a/drivers/spi/spi-tegra20-slink.c
+++ b/drivers/spi/spi-tegra20-slink.c
@@ -1001,7 +1001,7 @@ static const struct tegra_slink_chip_data tegra20_spi_cdata = {
 	.cs_hold_time = false,
 };
 
-static struct of_device_id tegra_slink_of_match[] = {
+static const struct of_device_id tegra_slink_of_match[] = {
 	{ .compatible = "nvidia,tegra30-slink", .data = &tegra30_spi_cdata, },
 	{ .compatible = "nvidia,tegra20-slink", .data = &tegra20_spi_cdata, },
 	{}
diff --git a/drivers/spi/spi-tle62x0.c b/drivers/spi/spi-tle62x0.c
index 2d4010d8082..daf5aa1c24c 100644
--- a/drivers/spi/spi-tle62x0.c
+++ b/drivers/spi/spi-tle62x0.c
@@ -253,10 +253,8 @@ static int tle62x0_probe(struct spi_device *spi)
 	}
 
 	st = kzalloc(sizeof(struct tle62x0_state), GFP_KERNEL);
-	if (st == NULL) {
-		dev_err(&spi->dev, "no memory for device state\n");
+	if (st == NULL)
 		return -ENOMEM;
-	}
 
 	st->us = spi;
 	st->nr_gpio = pdata->gpio_count;
diff --git a/drivers/spi/spi-topcliff-pch.c b/drivers/spi/spi-topcliff-pch.c
index f406b30af96..f05abf89c06 100644
--- a/drivers/spi/spi-topcliff-pch.c
+++ b/drivers/spi/spi-topcliff-pch.c
@@ -1578,14 +1578,11 @@ static int pch_spi_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	struct pch_pd_dev_save *pd_dev_save;
 
 	pd_dev_save = kzalloc(sizeof(struct pch_pd_dev_save), GFP_KERNEL);
-	if (!pd_dev_save) {
-		dev_err(&pdev->dev, "%s Can't allocate pd_dev_sav\n", __func__);
+	if (!pd_dev_save)
 		return -ENOMEM;
-	}
 
 	board_dat = kzalloc(sizeof(struct pch_spi_board_data), GFP_KERNEL);
 	if (!board_dat) {
-		dev_err(&pdev->dev, "%s Can't allocate board_dat\n", __func__);
 		retval = -ENOMEM;
 		goto err_no_mem;
 	}
diff --git a/drivers/spi/spi.c b/drivers/spi/spi.c
index 4eb9bf02996..d4f9670b51b 100644
--- a/drivers/spi/spi.c
+++ b/drivers/spi/spi.c
@@ -580,6 +580,7 @@ static void spi_set_cs(struct spi_device *spi, bool enable)
 		spi->master->set_cs(spi, !enable);
 }
 
+#ifdef CONFIG_HAS_DMA
 static int spi_map_buf(struct spi_master *master, struct device *dev,
 		       struct sg_table *sgt, void *buf, size_t len,
 		       enum dma_data_direction dir)
@@ -637,55 +638,12 @@ static void spi_unmap_buf(struct spi_master *master, struct device *dev,
 	}
 }
 
-static int spi_map_msg(struct spi_master *master, struct spi_message *msg)
+static int __spi_map_msg(struct spi_master *master, struct spi_message *msg)
 {
 	struct device *tx_dev, *rx_dev;
 	struct spi_transfer *xfer;
-	void *tmp;
-	unsigned int max_tx, max_rx;
 	int ret;
 
-	if (master->flags & (SPI_MASTER_MUST_RX | SPI_MASTER_MUST_TX)) {
-		max_tx = 0;
-		max_rx = 0;
-
-		list_for_each_entry(xfer, &msg->transfers, transfer_list) {
-			if ((master->flags & SPI_MASTER_MUST_TX) &&
-			    !xfer->tx_buf)
-				max_tx = max(xfer->len, max_tx);
-			if ((master->flags & SPI_MASTER_MUST_RX) &&
-			    !xfer->rx_buf)
-				max_rx = max(xfer->len, max_rx);
-		}
-
-		if (max_tx) {
-			tmp = krealloc(master->dummy_tx, max_tx,
-				       GFP_KERNEL | GFP_DMA);
-			if (!tmp)
-				return -ENOMEM;
-			master->dummy_tx = tmp;
-			memset(tmp, 0, max_tx);
-		}
-
-		if (max_rx) {
-			tmp = krealloc(master->dummy_rx, max_rx,
-				       GFP_KERNEL | GFP_DMA);
-			if (!tmp)
-				return -ENOMEM;
-			master->dummy_rx = tmp;
-		}
-
-		if (max_tx || max_rx) {
-			list_for_each_entry(xfer, &msg->transfers,
-					    transfer_list) {
-				if (!xfer->tx_buf)
-					xfer->tx_buf = master->dummy_tx;
-				if (!xfer->rx_buf)
-					xfer->rx_buf = master->dummy_rx;
-			}
-		}
-	}
-
 	if (!master->can_dma)
 		return 0;
 
@@ -742,6 +700,69 @@ static int spi_unmap_msg(struct spi_master *master, struct spi_message *msg)
 
 	return 0;
 }
+#else /* !CONFIG_HAS_DMA */
+static inline int __spi_map_msg(struct spi_master *master,
+				struct spi_message *msg)
+{
+	return 0;
+}
+
+static inline int spi_unmap_msg(struct spi_master *master,
+				struct spi_message *msg)
+{
+	return 0;
+}
+#endif /* !CONFIG_HAS_DMA */
+
+static int spi_map_msg(struct spi_master *master, struct spi_message *msg)
+{
+	struct spi_transfer *xfer;
+	void *tmp;
+	unsigned int max_tx, max_rx;
+
+	if (master->flags & (SPI_MASTER_MUST_RX | SPI_MASTER_MUST_TX)) {
+		max_tx = 0;
+		max_rx = 0;
+
+		list_for_each_entry(xfer, &msg->transfers, transfer_list) {
+			if ((master->flags & SPI_MASTER_MUST_TX) &&
+			    !xfer->tx_buf)
+				max_tx = max(xfer->len, max_tx);
+			if ((master->flags & SPI_MASTER_MUST_RX) &&
+			    !xfer->rx_buf)
+				max_rx = max(xfer->len, max_rx);
+		}
+
+		if (max_tx) {
+			tmp = krealloc(master->dummy_tx, max_tx,
+				       GFP_KERNEL | GFP_DMA);
+			if (!tmp)
+				return -ENOMEM;
+			master->dummy_tx = tmp;
+			memset(tmp, 0, max_tx);
+		}
+
+		if (max_rx) {
+			tmp = krealloc(master->dummy_rx, max_rx,
+				       GFP_KERNEL | GFP_DMA);
+			if (!tmp)
+				return -ENOMEM;
+			master->dummy_rx = tmp;
+		}
+
+		if (max_tx || max_rx) {
+			list_for_each_entry(xfer, &msg->transfers,
+					    transfer_list) {
+				if (!xfer->tx_buf)
+					xfer->tx_buf = master->dummy_tx;
+				if (!xfer->rx_buf)
+					xfer->rx_buf = master->dummy_rx;
+			}
+		}
+	}
+
+	return __spi_map_msg(master, msg);
+}
 
 /*
  * spi_transfer_one_message - Default implementation of transfer_one_message()
@@ -775,7 +796,7 @@ static int spi_transfer_one_message(struct spi_master *master,
 		if (ret > 0) {
 			ret = 0;
 			ms = xfer->len * 8 * 1000 / xfer->speed_hz;
-			ms += 10; /* some tolerance */
+			ms += ms + 100; /* some tolerance */
 
 			ms = wait_for_completion_timeout(&master->xfer_completion,
 							 msecs_to_jiffies(ms));
@@ -1151,7 +1172,6 @@ static int spi_master_initialize_queue(struct spi_master *master)
 {
 	int ret;
 
-	master->queued = true;
 	master->transfer = spi_queued_transfer;
 	if (!master->transfer_one_message)
 		master->transfer_one_message = spi_transfer_one_message;
@@ -1162,6 +1182,7 @@ static int spi_master_initialize_queue(struct spi_master *master)
 		dev_err(&master->dev, "problem initializing queue\n");
 		goto err_init_queue;
 	}
+	master->queued = true;
 	ret = spi_start_queue(master);
 	if (ret) {
 		dev_err(&master->dev, "problem starting queue\n");
@@ -1171,8 +1192,8 @@ static int spi_master_initialize_queue(struct spi_master *master)
 	return 0;
 
 err_start_queue:
-err_init_queue:
 	spi_destroy_queue(master);
+err_init_queue:
 	return ret;
 }
 
@@ -1234,6 +1255,8 @@ static void of_register_spi_devices(struct spi_master *master)
 			spi->mode |= SPI_CS_HIGH;
 		if (of_find_property(nc, "spi-3wire", NULL))
 			spi->mode |= SPI_3WIRE;
+		if (of_find_property(nc, "spi-lsb-first", NULL))
+			spi->mode |= SPI_LSB_FIRST;
 
 		/* Device DUAL/QUAD mode */
 		if (!of_property_read_u32(nc, "spi-tx-bus-width", &value)) {
@@ -1247,11 +1270,10 @@ static void of_register_spi_devices(struct spi_master *master)
 				spi->mode |= SPI_TX_QUAD;
 				break;
 			default:
-				dev_err(&master->dev,
-					"spi-tx-bus-width %d not supported\n",
-					value);
-				spi_dev_put(spi);
-				continue;
+				dev_warn(&master->dev,
+					 "spi-tx-bus-width %d not supported\n",
+					 value);
+				break;
 			}
 		}
 
@@ -1266,11 +1288,10 @@ static void of_register_spi_devices(struct spi_master *master)
 				spi->mode |= SPI_RX_QUAD;
 				break;
 			default:
-				dev_err(&master->dev,
-					"spi-rx-bus-width %d not supported\n",
-					value);
-				spi_dev_put(spi);
-				continue;
+				dev_warn(&master->dev,
+					 "spi-rx-bus-width %d not supported\n",
+					 value);
+				break;
 			}
 		}
 
@@ -1756,7 +1777,7 @@ EXPORT_SYMBOL_GPL(spi_busnum_to_master);
  */
 int spi_setup(struct spi_device *spi)
 {
-	unsigned	bad_bits;
+	unsigned	bad_bits, ugly_bits;
 	int		status = 0;
 
 	/* check mode to prevent that DUAL and QUAD set at the same time
@@ -1776,6 +1797,15 @@ int spi_setup(struct spi_device *spi)
 	 * that aren't supported with their current master
 	 */
 	bad_bits = spi->mode & ~spi->master->mode_bits;
+	ugly_bits = bad_bits &
+		    (SPI_TX_DUAL | SPI_TX_QUAD | SPI_RX_DUAL | SPI_RX_QUAD);
+	if (ugly_bits) {
+		dev_warn(&spi->dev,
+			 "setup: ignoring unsupported mode bits %x\n",
+			 ugly_bits);
+		spi->mode &= ~ugly_bits;
+		bad_bits &= ~ugly_bits;
+	}
 	if (bad_bits) {
 		dev_err(&spi->dev, "setup: unsupported mode bits %x\n",
 			bad_bits);
diff --git a/drivers/staging/Kconfig b/drivers/staging/Kconfig
index ea5efb426f7..4f38fc000a3 100644
--- a/drivers/staging/Kconfig
+++ b/drivers/staging/Kconfig
@@ -40,8 +40,6 @@ source "drivers/staging/olpc_dcon/Kconfig"
 
 source "drivers/staging/panel/Kconfig"
 
-source "drivers/staging/rtl8187se/Kconfig"
-
 source "drivers/staging/rtl8192u/Kconfig"
 
 source "drivers/staging/rtl8192e/Kconfig"
@@ -50,12 +48,12 @@ source "drivers/staging/rtl8712/Kconfig"
 
 source "drivers/staging/rtl8188eu/Kconfig"
 
+source "drivers/staging/rtl8192ee/Kconfig"
+
 source "drivers/staging/rtl8723au/Kconfig"
 
 source "drivers/staging/rtl8821ae/Kconfig"
 
-source "drivers/staging/rts5139/Kconfig"
-
 source "drivers/staging/rts5208/Kconfig"
 
 source "drivers/staging/frontier/Kconfig"
@@ -92,8 +90,6 @@ source "drivers/staging/tidspbridge/Kconfig"
 
 source "drivers/staging/quickstart/Kconfig"
 
-source "drivers/staging/sbe-2t3e3/Kconfig"
-
 source "drivers/staging/keucr/Kconfig"
 
 source "drivers/staging/bcm/Kconfig"
@@ -146,6 +142,8 @@ source "drivers/staging/gs_fpgaboot/Kconfig"
 
 source "drivers/staging/nokia_h4p/Kconfig"
 
+source "drivers/staging/skein/Kconfig"
+
 source "drivers/staging/unisys/Kconfig"
 
 endif # STAGING
diff --git a/drivers/staging/Makefile b/drivers/staging/Makefile
index 86e020c2ad0..1e97ad2177d 100644
--- a/drivers/staging/Makefile
+++ b/drivers/staging/Makefile
@@ -12,14 +12,13 @@ obj-$(CONFIG_PRISM2_USB)	+= wlan-ng/
 obj-$(CONFIG_COMEDI)		+= comedi/
 obj-$(CONFIG_FB_OLPC_DCON)	+= olpc_dcon/
 obj-$(CONFIG_PANEL)		+= panel/
-obj-$(CONFIG_R8187SE)		+= rtl8187se/
 obj-$(CONFIG_RTL8192U)		+= rtl8192u/
 obj-$(CONFIG_RTL8192E)		+= rtl8192e/
 obj-$(CONFIG_R8712U)		+= rtl8712/
 obj-$(CONFIG_R8188EU)		+= rtl8188eu/
+obj-$(CONFIG_R8192EE)		+= rtl8192ee/
 obj-$(CONFIG_R8723AU)		+= rtl8723au/
 obj-$(CONFIG_R8821AE)		+= rtl8821ae/
-obj-$(CONFIG_RTS5139)		+= rts5139/
 obj-$(CONFIG_RTS5208)		+= rts5208/
 obj-$(CONFIG_TRANZPORT)		+= frontier/
 obj-$(CONFIG_IDE_PHISON)	+= phison/
@@ -40,7 +39,6 @@ obj-$(CONFIG_CXT1E1)		+= cxt1e1/
 obj-$(CONFIG_FB_XGI)		+= xgifb/
 obj-$(CONFIG_TIDSPBRIDGE)	+= tidspbridge/
 obj-$(CONFIG_ACPI_QUICKSTART)	+= quickstart/
-obj-$(CONFIG_SBE_2T3E3)		+= sbe-2t3e3/
 obj-$(CONFIG_USB_ENESTORAGE)	+= keucr/
 obj-$(CONFIG_BCM_WIMAX)		+= bcm/
 obj-$(CONFIG_FT1000)		+= ft1000/
@@ -65,4 +63,5 @@ obj-$(CONFIG_DGAP)			+= dgap/
 obj-$(CONFIG_MTD_SPINAND_MT29F)	+= mt29f_spinand/
 obj-$(CONFIG_GS_FPGABOOT)	+= gs_fpgaboot/
 obj-$(CONFIG_BT_NOKIA_H4P)	+= nokia_h4p/
+obj-$(CONFIG_CRYPTO_SKEIN)	+= skein/
 obj-$(CONFIG_UNISYSSPAR)	+= unisys/
diff --git a/drivers/staging/android/alarm-dev.c b/drivers/staging/android/alarm-dev.c
index 2fc7cdd4c4e..f200e8a8432 100644
--- a/drivers/staging/android/alarm-dev.c
+++ b/drivers/staging/android/alarm-dev.c
@@ -329,6 +329,7 @@ static int alarm_release(struct inode *inode, struct file *file)
 	if (file->private_data) {
 		for (i = 0; i < ANDROID_ALARM_TYPE_COUNT; i++) {
 			uint32_t alarm_type_mask = 1U << i;
+
 			if (alarm_enabled & alarm_type_mask) {
 				alarm_dbg(INFO,
 					  "%s: clear alarm, pending %d\n",
diff --git a/drivers/staging/android/binder.c b/drivers/staging/android/binder.c
index cfe4bc8f05c..989f809f323 100644
--- a/drivers/staging/android/binder.c
+++ b/drivers/staging/android/binder.c
@@ -118,6 +118,7 @@ static int binder_set_stop_on_user_error(const char *val,
 					 struct kernel_param *kp)
 {
 	int ret;
+
 	ret = param_set_int(val, kp);
 	if (binder_stop_on_user_error < 2)
 		wake_up(&binder_user_error_wait);
@@ -194,6 +195,7 @@ static struct binder_transaction_log_entry *binder_transaction_log_add(
 	struct binder_transaction_log *log)
 {
 	struct binder_transaction_log_entry *e;
+
 	e = &log->entry[log->next];
 	memset(e, 0, sizeof(*e));
 	log->next++;
@@ -432,6 +434,7 @@ static inline void binder_unlock(const char *tag)
 static void binder_set_nice(long nice)
 {
 	long min_nice;
+
 	if (can_nice(current, nice)) {
 		set_user_nice(current, nice);
 		return;
@@ -584,6 +587,7 @@ static int binder_update_page_range(struct binder_proc *proc, int allocate,
 	for (page_addr = start; page_addr < end; page_addr += PAGE_SIZE) {
 		int ret;
 		struct page **page_array_ptr;
+
 		page = &proc->pages[(page_addr - proc->buffer) / PAGE_SIZE];
 
 		BUG_ON(*page);
@@ -726,6 +730,7 @@ static struct binder_buffer *binder_alloc_buf(struct binder_proc *proc,
 	binder_insert_allocated_buffer(proc, buffer);
 	if (buffer_size != size) {
 		struct binder_buffer *new_buffer = (void *)buffer->data + size;
+
 		list_add(&new_buffer->entry, &buffer->entry);
 		new_buffer->free = 1;
 		binder_insert_free_buffer(proc, new_buffer);
@@ -838,6 +843,7 @@ static void binder_free_buf(struct binder_proc *proc,
 	if (!list_is_last(&buffer->entry, &proc->buffers)) {
 		struct binder_buffer *next = list_entry(buffer->entry.next,
 						struct binder_buffer, entry);
+
 		if (next->free) {
 			rb_erase(&next->rb_node, &proc->free_buffers);
 			binder_delete_free_buffer(proc, next);
@@ -846,6 +852,7 @@ static void binder_free_buf(struct binder_proc *proc,
 	if (proc->buffers.next != &buffer->entry) {
 		struct binder_buffer *prev = list_entry(buffer->entry.prev,
 						struct binder_buffer, entry);
+
 		if (prev->free) {
 			binder_delete_free_buffer(proc, buffer);
 			rb_erase(&prev->rb_node, &proc->free_buffers);
@@ -1107,6 +1114,7 @@ static int binder_inc_ref(struct binder_ref *ref, int strong,
 			  struct list_head *target_list)
 {
 	int ret;
+
 	if (strong) {
 		if (ref->strong == 0) {
 			ret = binder_inc_node(ref->node, 1, 1, target_list);
@@ -1138,6 +1146,7 @@ static int binder_dec_ref(struct binder_ref *ref, int strong)
 		ref->strong--;
 		if (ref->strong == 0) {
 			int ret;
+
 			ret = binder_dec_node(ref->node, strong, 1);
 			if (ret)
 				return ret;
@@ -1177,6 +1186,7 @@ static void binder_send_failed_reply(struct binder_transaction *t,
 				     uint32_t error_code)
 {
 	struct binder_thread *target_thread;
+
 	BUG_ON(t->flags & TF_ONE_WAY);
 	while (1) {
 		target_thread = t->from;
@@ -1247,6 +1257,7 @@ static void binder_transaction_buffer_release(struct binder_proc *proc,
 		off_end = (void *)offp + buffer->offsets_size;
 	for (; offp < off_end; offp++) {
 		struct flat_binder_object *fp;
+
 		if (*offp > buffer->data_size - sizeof(*fp) ||
 		    buffer->data_size < sizeof(*fp) ||
 		    !IS_ALIGNED(*offp, sizeof(u32))) {
@@ -1259,6 +1270,7 @@ static void binder_transaction_buffer_release(struct binder_proc *proc,
 		case BINDER_TYPE_BINDER:
 		case BINDER_TYPE_WEAK_BINDER: {
 			struct binder_node *node = binder_get_node(proc, fp->binder);
+
 			if (node == NULL) {
 				pr_err("transaction release %d bad node %016llx\n",
 				       debug_id, (u64)fp->binder);
@@ -1272,6 +1284,7 @@ static void binder_transaction_buffer_release(struct binder_proc *proc,
 		case BINDER_TYPE_HANDLE:
 		case BINDER_TYPE_WEAK_HANDLE: {
 			struct binder_ref *ref = binder_get_ref(proc, fp->handle);
+
 			if (ref == NULL) {
 				pr_err("transaction release %d bad handle %d\n",
 				 debug_id, fp->handle);
@@ -1363,6 +1376,7 @@ static void binder_transaction(struct binder_proc *proc,
 	} else {
 		if (tr->target.handle) {
 			struct binder_ref *ref;
+
 			ref = binder_get_ref(proc, tr->target.handle);
 			if (ref == NULL) {
 				binder_user_error("%d:%d got transaction to invalid handle\n",
@@ -1386,6 +1400,7 @@ static void binder_transaction(struct binder_proc *proc,
 		}
 		if (!(tr->flags & TF_ONE_WAY) && thread->transaction_stack) {
 			struct binder_transaction *tmp;
+
 			tmp = thread->transaction_stack;
 			if (tmp->to_thread != thread) {
 				binder_user_error("%d:%d got new transaction with bad transaction stack, transaction %d has target %d:%d\n",
@@ -1452,7 +1467,7 @@ static void binder_transaction(struct binder_proc *proc,
 		t->from = thread;
 	else
 		t->from = NULL;
-	t->sender_euid = proc->tsk->cred->euid;
+	t->sender_euid = task_euid(proc->tsk);
 	t->to_proc = target_proc;
 	t->to_thread = target_thread;
 	t->code = tr->code;
@@ -1501,6 +1516,7 @@ static void binder_transaction(struct binder_proc *proc,
 	off_end = (void *)offp + tr->offsets_size;
 	for (; offp < off_end; offp++) {
 		struct flat_binder_object *fp;
+
 		if (*offp > t->buffer->data_size - sizeof(*fp) ||
 		    t->buffer->data_size < sizeof(*fp) ||
 		    !IS_ALIGNED(*offp, sizeof(u32))) {
@@ -1515,6 +1531,7 @@ static void binder_transaction(struct binder_proc *proc,
 		case BINDER_TYPE_WEAK_BINDER: {
 			struct binder_ref *ref;
 			struct binder_node *node = binder_get_node(proc, fp->binder);
+
 			if (node == NULL) {
 				node = binder_new_node(proc, fp->binder, fp->cookie);
 				if (node == NULL) {
@@ -1529,6 +1546,7 @@ static void binder_transaction(struct binder_proc *proc,
 					proc->pid, thread->pid,
 					(u64)fp->binder, node->debug_id,
 					(u64)fp->cookie, (u64)node->cookie);
+				return_error = BR_FAILED_REPLY;
 				goto err_binder_get_ref_for_node_failed;
 			}
 			ref = binder_get_ref_for_node(target_proc, node);
@@ -1553,6 +1571,7 @@ static void binder_transaction(struct binder_proc *proc,
 		case BINDER_TYPE_HANDLE:
 		case BINDER_TYPE_WEAK_HANDLE: {
 			struct binder_ref *ref = binder_get_ref(proc, fp->handle);
+
 			if (ref == NULL) {
 				binder_user_error("%d:%d got transaction with invalid handle, %d\n",
 						proc->pid,
@@ -1575,6 +1594,7 @@ static void binder_transaction(struct binder_proc *proc,
 					     (u64)ref->node->ptr);
 			} else {
 				struct binder_ref *new_ref;
+
 				new_ref = binder_get_ref_for_node(target_proc, ref->node);
 				if (new_ref == NULL) {
 					return_error = BR_FAILED_REPLY;
@@ -1694,6 +1714,7 @@ err_no_context_mgr_node:
 
 	{
 		struct binder_transaction_log_entry *fe;
+
 		fe = binder_transaction_log_add(&binder_transaction_log_failed);
 		*fe = *e;
 	}
@@ -2024,12 +2045,14 @@ static int binder_thread_write(struct binder_proc *proc,
 			struct binder_work *w;
 			binder_uintptr_t cookie;
 			struct binder_ref_death *death = NULL;
+
 			if (get_user(cookie, (binder_uintptr_t __user *)ptr))
 				return -EFAULT;
 
 			ptr += sizeof(void *);
 			list_for_each_entry(w, &proc->delivered_death, entry) {
 				struct binder_ref_death *tmp_death = container_of(w, struct binder_ref_death, work);
+
 				if (tmp_death->cookie == cookie) {
 					death = tmp_death;
 					break;
@@ -2216,6 +2239,7 @@ retry:
 			const char *cmd_name;
 			int strong = node->internal_strong_refs || node->local_strong_refs;
 			int weak = !hlist_empty(&node->refs) || node->local_weak_refs || strong;
+
 			if (weak && !node->has_weak_ref) {
 				cmd = BR_INCREFS;
 				cmd_name = "BR_INCREFS";
@@ -2322,6 +2346,7 @@ retry:
 		BUG_ON(t->buffer == NULL);
 		if (t->buffer->target_node) {
 			struct binder_node *target_node = t->buffer->target_node;
+
 			tr.target.ptr = target_node->ptr;
 			tr.cookie =  target_node->cookie;
 			t->saved_priority = task_nice(current);
@@ -2343,6 +2368,7 @@ retry:
 
 		if (t->from) {
 			struct task_struct *sender = t->from->proc->tsk;
+
 			tr.sender_pid = task_tgid_nr_ns(sender,
 							task_active_pid_ns(current));
 		} else {
@@ -2413,6 +2439,7 @@ done:
 static void binder_release_work(struct list_head *list)
 {
 	struct binder_work *w;
+
 	while (!list_empty(list)) {
 		w = list_first_entry(list, struct binder_work, entry);
 		list_del_init(&w->entry);
@@ -2574,6 +2601,7 @@ static long binder_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 	struct binder_thread *thread;
 	unsigned int size = _IOC_SIZE(cmd);
 	void __user *ubuf = (void __user *)arg;
+	kuid_t curr_euid = current_euid();
 
 	/*pr_info("binder_ioctl: %d:%d %x %lx\n", proc->pid, current->pid, cmd, arg);*/
 
@@ -2593,6 +2621,7 @@ static long binder_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 	switch (cmd) {
 	case BINDER_WRITE_READ: {
 		struct binder_write_read bwr;
+
 		if (size != sizeof(struct binder_write_read)) {
 			ret = -EINVAL;
 			goto err;
@@ -2658,15 +2687,16 @@ static long binder_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 			goto err;
 		}
 		if (uid_valid(binder_context_mgr_uid)) {
-			if (!uid_eq(binder_context_mgr_uid, current->cred->euid)) {
+			if (!uid_eq(binder_context_mgr_uid, curr_euid)) {
 				pr_err("BINDER_SET_CONTEXT_MGR bad uid %d != %d\n",
-				       from_kuid(&init_user_ns, current->cred->euid),
+				       from_kuid(&init_user_ns, curr_euid),
 				       from_kuid(&init_user_ns, binder_context_mgr_uid));
 				ret = -EPERM;
 				goto err;
 			}
-		} else
-			binder_context_mgr_uid = current->cred->euid;
+		} else {
+			binder_context_mgr_uid = curr_euid;
+		}
 		binder_context_mgr_node = binder_new_node(proc, 0, 0);
 		if (binder_context_mgr_node == NULL) {
 			ret = -ENOMEM;
@@ -2683,16 +2713,20 @@ static long binder_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 		binder_free_thread(proc, thread);
 		thread = NULL;
 		break;
-	case BINDER_VERSION:
+	case BINDER_VERSION: {
+		struct binder_version __user *ver = ubuf;
+
 		if (size != sizeof(struct binder_version)) {
 			ret = -EINVAL;
 			goto err;
 		}
-		if (put_user(BINDER_CURRENT_PROTOCOL_VERSION, &((struct binder_version *)ubuf)->protocol_version)) {
+		if (put_user(BINDER_CURRENT_PROTOCOL_VERSION,
+			     &ver->protocol_version)) {
 			ret = -EINVAL;
 			goto err;
 		}
 		break;
+	}
 	default:
 		ret = -EINVAL;
 		goto err;
@@ -2713,6 +2747,7 @@ err_unlocked:
 static void binder_vma_open(struct vm_area_struct *vma)
 {
 	struct binder_proc *proc = vma->vm_private_data;
+
 	binder_debug(BINDER_DEBUG_OPEN_CLOSE,
 		     "%d open vm area %lx-%lx (%ld K) vma %lx pagep %lx\n",
 		     proc->pid, vma->vm_start, vma->vm_end,
@@ -2723,6 +2758,7 @@ static void binder_vma_open(struct vm_area_struct *vma)
 static void binder_vma_close(struct vm_area_struct *vma)
 {
 	struct binder_proc *proc = vma->vm_private_data;
+
 	binder_debug(BINDER_DEBUG_OPEN_CLOSE,
 		     "%d close vm area %lx-%lx (%ld K) vma %lx pagep %lx\n",
 		     proc->pid, vma->vm_start, vma->vm_end,
@@ -2865,6 +2901,7 @@ static int binder_open(struct inode *nodp, struct file *filp)
 
 	if (binder_debugfs_dir_entry_proc) {
 		char strbuf[11];
+
 		snprintf(strbuf, sizeof(strbuf), "%u", proc->pid);
 		proc->debugfs_entry = debugfs_create_file(strbuf, S_IRUGO,
 			binder_debugfs_dir_entry_proc, proc, &binder_proc_fops);
@@ -2886,8 +2923,10 @@ static void binder_deferred_flush(struct binder_proc *proc)
 {
 	struct rb_node *n;
 	int wake_count = 0;
+
 	for (n = rb_first(&proc->threads); n != NULL; n = rb_next(n)) {
 		struct binder_thread *thread = rb_entry(n, struct binder_thread, rb_node);
+
 		thread->looper |= BINDER_LOOPER_STATE_NEED_RETURN;
 		if (thread->looper & BINDER_LOOPER_STATE_WAITING) {
 			wake_up_interruptible(&thread->wait);
@@ -2904,6 +2943,7 @@ static void binder_deferred_flush(struct binder_proc *proc)
 static int binder_release(struct inode *nodp, struct file *filp)
 {
 	struct binder_proc *proc = filp->private_data;
+
 	debugfs_remove(proc->debugfs_entry);
 	binder_defer_work(proc, BINDER_DEFERRED_RELEASE);
 
@@ -3065,6 +3105,7 @@ static void binder_deferred_func(struct work_struct *work)
 	struct files_struct *files;
 
 	int defer;
+
 	do {
 		binder_lock(__func__);
 		mutex_lock(&binder_deferred_lock);
diff --git a/drivers/staging/android/ion/ion.c b/drivers/staging/android/ion/ion.c
index 3d5bf147223..389b8f67a2e 100644
--- a/drivers/staging/android/ion/ion.c
+++ b/drivers/staging/android/ion/ion.c
@@ -408,6 +408,7 @@ static struct ion_handle *ion_handle_lookup(struct ion_client *client,
 
 	while (n) {
 		struct ion_handle *entry = rb_entry(n, struct ion_handle, node);
+
 		if (buffer < entry->buffer)
 			n = n->rb_left;
 		else if (buffer > entry->buffer)
@@ -626,6 +627,10 @@ static void ion_handle_kmap_put(struct ion_handle *handle)
 {
 	struct ion_buffer *buffer = handle->buffer;
 
+	if (!handle->kmap_cnt) {
+		WARN(1, "%s: Double unmap detected! bailing...\n", __func__);
+		return;
+	}
 	handle->kmap_cnt--;
 	if (!handle->kmap_cnt)
 		ion_buffer_kmap_put(buffer);
@@ -720,9 +725,11 @@ static int ion_get_client_serial(const struct rb_root *root,
 {
 	int serial = -1;
 	struct rb_node *node;
+
 	for (node = rb_first(root); node; node = rb_next(node)) {
 		struct ion_client *client = rb_entry(node, struct ion_client,
 						node);
+
 		if (strcmp(client->name, name))
 			continue;
 		serial = max(serial, client->display_serial);
@@ -1035,12 +1042,14 @@ static int ion_mmap(struct dma_buf *dmabuf, struct vm_area_struct *vma)
 static void ion_dma_buf_release(struct dma_buf *dmabuf)
 {
 	struct ion_buffer *buffer = dmabuf->priv;
+
 	ion_buffer_put(buffer);
 }
 
 static void *ion_dma_buf_kmap(struct dma_buf *dmabuf, unsigned long offset)
 {
 	struct ion_buffer *buffer = dmabuf->priv;
+
 	return buffer->vaddr + offset * PAGE_SIZE;
 }
 
@@ -1292,6 +1301,7 @@ static long ion_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 	case ION_IOC_IMPORT:
 	{
 		struct ion_handle *handle;
+
 		handle = ion_import_dma_buf(client, data.fd.fd);
 		if (IS_ERR(handle))
 			ret = PTR_ERR(handle);
@@ -1393,6 +1403,7 @@ static int ion_debug_heap_show(struct seq_file *s, void *unused)
 		struct ion_client *client = rb_entry(n, struct ion_client,
 						     node);
 		size_t size = ion_debug_heap_total(client, heap->id);
+
 		if (!size)
 			continue;
 		if (client->task) {
@@ -1516,6 +1527,7 @@ void ion_device_add_heap(struct ion_device *dev, struct ion_heap *heap)
 
 	if (!debug_file) {
 		char buf[256], *path;
+
 		path = dentry_path(dev->heaps_debug_root, buf, 256);
 		pr_err("Failed to create heap debugfs at %s/%s\n",
 			path, heap->name);
@@ -1531,6 +1543,7 @@ void ion_device_add_heap(struct ion_device *dev, struct ion_heap *heap)
 			&debug_shrink_fops);
 		if (!debug_file) {
 			char buf[256], *path;
+
 			path = dentry_path(dev->heaps_debug_root, buf, 256);
 			pr_err("Failed to create heap shrinker debugfs at %s/%s\n",
 				path, debug_name);
@@ -1606,6 +1619,7 @@ void __init ion_reserve(struct ion_platform_data *data)
 
 		if (data->heaps[i].base == 0) {
 			phys_addr_t paddr;
+
 			paddr = memblock_alloc_base(data->heaps[i].size,
 						    data->heaps[i].align,
 						    MEMBLOCK_ALLOC_ANYWHERE);
diff --git a/drivers/staging/android/ion/ion_carveout_heap.c b/drivers/staging/android/ion/ion_carveout_heap.c
index 3cb05b9b0e9..dcb6f2196c8 100644
--- a/drivers/staging/android/ion/ion_carveout_heap.c
+++ b/drivers/staging/android/ion/ion_carveout_heap.c
@@ -81,7 +81,7 @@ static int ion_carveout_heap_allocate(struct ion_heap *heap,
 	if (align > PAGE_SIZE)
 		return -EINVAL;
 
-	table = kzalloc(sizeof(struct sg_table), GFP_KERNEL);
+	table = kmalloc(sizeof(struct sg_table), GFP_KERNEL);
 	if (!table)
 		return -ENOMEM;
 	ret = sg_alloc_table(table, 1, GFP_KERNEL);
diff --git a/drivers/staging/android/ion/ion_chunk_heap.c b/drivers/staging/android/ion/ion_chunk_heap.c
index d40f5f83180..3f2c12ba4d1 100644
--- a/drivers/staging/android/ion/ion_chunk_heap.c
+++ b/drivers/staging/android/ion/ion_chunk_heap.c
@@ -55,7 +55,7 @@ static int ion_chunk_heap_allocate(struct ion_heap *heap,
 	if (allocated_size > chunk_heap->size - chunk_heap->allocated)
 		return -ENOMEM;
 
-	table = kzalloc(sizeof(struct sg_table), GFP_KERNEL);
+	table = kmalloc(sizeof(struct sg_table), GFP_KERNEL);
 	if (!table)
 		return -ENOMEM;
 	ret = sg_alloc_table(table, num_chunks, GFP_KERNEL);
diff --git a/drivers/staging/android/ion/ion_heap.c b/drivers/staging/android/ion/ion_heap.c
index bdc6a28ba8c..4605e04712a 100644
--- a/drivers/staging/android/ion/ion_heap.c
+++ b/drivers/staging/android/ion/ion_heap.c
@@ -48,6 +48,7 @@ void *ion_heap_map_kernel(struct ion_heap *heap,
 	for_each_sg(table->sgl, sg, table->nents, i) {
 		int npages_this_entry = PAGE_ALIGN(sg->length) / PAGE_SIZE;
 		struct page *page = sg_page(sg);
+
 		BUG_ON(i >= npages);
 		for (j = 0; j < npages_this_entry; j++)
 			*(tmp++) = page++;
@@ -105,6 +106,7 @@ int ion_heap_map_user(struct ion_heap *heap, struct ion_buffer *buffer,
 static int ion_heap_clear_pages(struct page **pages, int num, pgprot_t pgprot)
 {
 	void *addr = vm_map_ram(pages, num, -1, pgprot);
+
 	if (!addr)
 		return -ENOMEM;
 	memset(addr, 0, PAGE_SIZE * num);
diff --git a/drivers/staging/android/ion/ion_page_pool.c b/drivers/staging/android/ion/ion_page_pool.c
index ecb5fc34ec5..5864f3dfcbc 100644
--- a/drivers/staging/android/ion/ion_page_pool.c
+++ b/drivers/staging/android/ion/ion_page_pool.c
@@ -21,13 +21,9 @@
 #include <linux/list.h>
 #include <linux/module.h>
 #include <linux/slab.h>
+#include <linux/swap.h>
 #include "ion_priv.h"
 
-struct ion_page_pool_item {
-	struct page *page;
-	struct list_head list;
-};
-
 static void *ion_page_pool_alloc_pages(struct ion_page_pool *pool)
 {
 	struct page *page = alloc_pages(pool->gfp_mask, pool->order);
@@ -47,19 +43,12 @@ static void ion_page_pool_free_pages(struct ion_page_pool *pool,
 
 static int ion_page_pool_add(struct ion_page_pool *pool, struct page *page)
 {
-	struct ion_page_pool_item *item;
-
-	item = kmalloc(sizeof(struct ion_page_pool_item), GFP_KERNEL);
-	if (!item)
-		return -ENOMEM;
-
 	mutex_lock(&pool->mutex);
-	item->page = page;
 	if (PageHighMem(page)) {
-		list_add_tail(&item->list, &pool->high_items);
+		list_add_tail(&page->lru, &pool->high_items);
 		pool->high_count++;
 	} else {
-		list_add_tail(&item->list, &pool->low_items);
+		list_add_tail(&page->lru, &pool->low_items);
 		pool->low_count++;
 	}
 	mutex_unlock(&pool->mutex);
@@ -68,28 +57,23 @@ static int ion_page_pool_add(struct ion_page_pool *pool, struct page *page)
 
 static struct page *ion_page_pool_remove(struct ion_page_pool *pool, bool high)
 {
-	struct ion_page_pool_item *item;
 	struct page *page;
 
 	if (high) {
 		BUG_ON(!pool->high_count);
-		item = list_first_entry(&pool->high_items,
-					struct ion_page_pool_item, list);
+		page = list_first_entry(&pool->high_items, struct page, lru);
 		pool->high_count--;
 	} else {
 		BUG_ON(!pool->low_count);
-		item = list_first_entry(&pool->low_items,
-					struct ion_page_pool_item, list);
+		page = list_first_entry(&pool->low_items, struct page, lru);
 		pool->low_count--;
 	}
 
-	list_del(&item->list);
-	page = item->page;
-	kfree(item);
+	list_del(&page->lru);
 	return page;
 }
 
-void *ion_page_pool_alloc(struct ion_page_pool *pool)
+struct page *ion_page_pool_alloc(struct ion_page_pool *pool)
 {
 	struct page *page = NULL;
 
@@ -112,6 +96,8 @@ void ion_page_pool_free(struct ion_page_pool *pool, struct page *page)
 {
 	int ret;
 
+	BUG_ON(pool->order != compound_order(page));
+
 	ret = ion_page_pool_add(pool, page);
 	if (ret)
 		ion_page_pool_free_pages(pool, page);
@@ -119,12 +105,12 @@ void ion_page_pool_free(struct ion_page_pool *pool, struct page *page)
 
 static int ion_page_pool_total(struct ion_page_pool *pool, bool high)
 {
-	int total = 0;
+	int count = pool->low_count;
+
+	if (high)
+		count += pool->high_count;
 
-	total += high ? (pool->high_count + pool->low_count) *
-		(1 << pool->order) :
-			pool->low_count * (1 << pool->order);
-	return total;
+	return count << pool->order;
 }
 
 int ion_page_pool_shrink(struct ion_page_pool *pool, gfp_t gfp_mask,
@@ -133,7 +119,10 @@ int ion_page_pool_shrink(struct ion_page_pool *pool, gfp_t gfp_mask,
 	int freed;
 	bool high;
 
-	high = !!(gfp_mask & __GFP_HIGHMEM);
+	if (current_is_kswapd())
+		high = 1;
+	else
+		high = !!(gfp_mask & __GFP_HIGHMEM);
 
 	if (nr_to_scan == 0)
 		return ion_page_pool_total(pool, high);
@@ -167,7 +156,7 @@ struct ion_page_pool *ion_page_pool_create(gfp_t gfp_mask, unsigned int order)
 	pool->low_count = 0;
 	INIT_LIST_HEAD(&pool->low_items);
 	INIT_LIST_HEAD(&pool->high_items);
-	pool->gfp_mask = gfp_mask;
+	pool->gfp_mask = gfp_mask | __GFP_COMP;
 	pool->order = order;
 	mutex_init(&pool->mutex);
 	plist_node_init(&pool->list, order);
diff --git a/drivers/staging/android/ion/ion_priv.h b/drivers/staging/android/ion/ion_priv.h
index 1eba3f2076a..c8f01757abf 100644
--- a/drivers/staging/android/ion/ion_priv.h
+++ b/drivers/staging/android/ion/ion_priv.h
@@ -178,6 +178,7 @@ struct ion_heap {
 	spinlock_t free_lock;
 	wait_queue_head_t waitqueue;
 	struct task_struct *task;
+
 	int (*debug_show)(struct ion_heap *heap, struct seq_file *, void *);
 };
 
@@ -377,7 +378,7 @@ struct ion_page_pool {
 
 struct ion_page_pool *ion_page_pool_create(gfp_t gfp_mask, unsigned int order);
 void ion_page_pool_destroy(struct ion_page_pool *);
-void *ion_page_pool_alloc(struct ion_page_pool *);
+struct page *ion_page_pool_alloc(struct ion_page_pool *);
 void ion_page_pool_free(struct ion_page_pool *, struct page *);
 
 /** ion_page_pool_shrink - shrinks the size of the memory cached in the pool
diff --git a/drivers/staging/android/ion/ion_system_heap.c b/drivers/staging/android/ion/ion_system_heap.c
index c92363356ae..cb7ae08a5e2 100644
--- a/drivers/staging/android/ion/ion_system_heap.c
+++ b/drivers/staging/android/ion/ion_system_heap.c
@@ -34,6 +34,7 @@ static const int num_orders = ARRAY_SIZE(orders);
 static int order_to_index(unsigned int order)
 {
 	int i;
+
 	for (i = 0; i < num_orders; i++)
 		if (order == orders[i])
 			return i;
@@ -41,7 +42,7 @@ static int order_to_index(unsigned int order)
 	return -1;
 }
 
-static unsigned int order_to_size(int order)
+static inline unsigned int order_to_size(int order)
 {
 	return PAGE_SIZE << order;
 }
@@ -72,14 +73,12 @@ static struct page *alloc_buffer_page(struct ion_system_heap *heap,
 
 		if (order > 4)
 			gfp_flags = high_order_gfp_flags;
-		page = alloc_pages(gfp_flags, order);
+		page = alloc_pages(gfp_flags | __GFP_COMP, order);
 		if (!page)
 			return NULL;
 		ion_pages_sync_for_device(NULL, page, PAGE_SIZE << order,
 						DMA_BIDIRECTIONAL);
 	}
-	if (!page)
-		return NULL;
 
 	return page;
 }
@@ -92,6 +91,7 @@ static void free_buffer_page(struct ion_system_heap *heap,
 
 	if (!cached && !(buffer->private_flags & ION_PRIV_FLAG_SHRINKER_FREE)) {
 		struct ion_page_pool *pool = heap->pools[order_to_index(order)];
+
 		ion_page_pool_free(pool, page);
 	} else {
 		__free_pages(page, order);
@@ -124,7 +124,6 @@ static struct page_info *alloc_largest_available(struct ion_system_heap *heap,
 
 		info->page = page;
 		info->order = orders[i];
-		INIT_LIST_HEAD(&info->list);
 		return info;
 	}
 	kfree(info);
@@ -142,7 +141,6 @@ static int ion_system_heap_allocate(struct ion_heap *heap,
 							heap);
 	struct sg_table *table;
 	struct scatterlist *sg;
-	int ret;
 	struct list_head pages;
 	struct page_info *info, *tmp_info;
 	int i = 0;
@@ -160,24 +158,23 @@ static int ion_system_heap_allocate(struct ion_heap *heap,
 		info = alloc_largest_available(sys_heap, buffer, size_remaining,
 						max_order);
 		if (!info)
-			goto err;
+			goto free_pages;
 		list_add_tail(&info->list, &pages);
-		size_remaining -= (1 << info->order) * PAGE_SIZE;
+		size_remaining -= PAGE_SIZE << info->order;
 		max_order = info->order;
 		i++;
 	}
-	table = kzalloc(sizeof(struct sg_table), GFP_KERNEL);
+	table = kmalloc(sizeof(struct sg_table), GFP_KERNEL);
 	if (!table)
-		goto err;
+		goto free_pages;
 
-	ret = sg_alloc_table(table, i, GFP_KERNEL);
-	if (ret)
-		goto err1;
+	if (sg_alloc_table(table, i, GFP_KERNEL))
+		goto free_table;
 
 	sg = table->sgl;
 	list_for_each_entry_safe(info, tmp_info, &pages, list) {
 		struct page *page = info->page;
-		sg_set_page(sg, page, (1 << info->order) * PAGE_SIZE, 0);
+		sg_set_page(sg, page, PAGE_SIZE << info->order, 0);
 		sg = sg_next(sg);
 		list_del(&info->list);
 		kfree(info);
@@ -185,9 +182,10 @@ static int ion_system_heap_allocate(struct ion_heap *heap,
 
 	buffer->priv_virt = table;
 	return 0;
-err1:
+
+free_table:
 	kfree(table);
-err:
+free_pages:
 	list_for_each_entry_safe(info, tmp_info, &pages, list) {
 		free_buffer_page(sys_heap, buffer, info->page, info->order);
 		kfree(info);
@@ -197,14 +195,12 @@ err:
 
 static void ion_system_heap_free(struct ion_buffer *buffer)
 {
-	struct ion_heap *heap = buffer->heap;
-	struct ion_system_heap *sys_heap = container_of(heap,
+	struct ion_system_heap *sys_heap = container_of(buffer->heap,
 							struct ion_system_heap,
 							heap);
 	struct sg_table *table = buffer->sg_table;
 	bool cached = ion_buffer_cached(buffer);
 	struct scatterlist *sg;
-	LIST_HEAD(pages);
 	int i;
 
 	/* uncached pages come from the page pools, zero them before returning
@@ -242,6 +238,7 @@ static int ion_system_heap_shrink(struct ion_heap *heap, gfp_t gfp_mask,
 
 	for (i = 0; i < num_orders; i++) {
 		struct ion_page_pool *pool = sys_heap->pools[i];
+
 		nr_total += ion_page_pool_shrink(pool, gfp_mask, nr_to_scan);
 	}
 
@@ -267,14 +264,16 @@ static int ion_system_heap_debug_show(struct ion_heap *heap, struct seq_file *s,
 							struct ion_system_heap,
 							heap);
 	int i;
+
 	for (i = 0; i < num_orders; i++) {
 		struct ion_page_pool *pool = sys_heap->pools[i];
+
 		seq_printf(s, "%d order %u highmem pages in pool = %lu total\n",
 			   pool->high_count, pool->order,
-			   (1 << pool->order) * PAGE_SIZE * pool->high_count);
+			   (PAGE_SIZE << pool->order) * pool->high_count);
 		seq_printf(s, "%d order %u lowmem pages in pool = %lu total\n",
 			   pool->low_count, pool->order,
-			   (1 << pool->order) * PAGE_SIZE * pool->low_count);
+			   (PAGE_SIZE << pool->order) * pool->low_count);
 	}
 	return 0;
 }
@@ -293,7 +292,7 @@ struct ion_heap *ion_system_heap_create(struct ion_platform_heap *unused)
 	heap->pools = kzalloc(sizeof(struct ion_page_pool *) * num_orders,
 			      GFP_KERNEL);
 	if (!heap->pools)
-		goto err_alloc_pools;
+		goto free_heap;
 	for (i = 0; i < num_orders; i++) {
 		struct ion_page_pool *pool;
 		gfp_t gfp_flags = low_order_gfp_flags;
@@ -302,18 +301,18 @@ struct ion_heap *ion_system_heap_create(struct ion_platform_heap *unused)
 			gfp_flags = high_order_gfp_flags;
 		pool = ion_page_pool_create(gfp_flags, orders[i]);
 		if (!pool)
-			goto err_create_pool;
+			goto destroy_pools;
 		heap->pools[i] = pool;
 	}
 
 	heap->heap.debug_show = ion_system_heap_debug_show;
 	return &heap->heap;
-err_create_pool:
-	for (i = 0; i < num_orders; i++)
-		if (heap->pools[i])
-			ion_page_pool_destroy(heap->pools[i]);
+
+destroy_pools:
+	while (i--)
+		ion_page_pool_destroy(heap->pools[i]);
 	kfree(heap->pools);
-err_alloc_pools:
+free_heap:
 	kfree(heap);
 	return ERR_PTR(-ENOMEM);
 }
@@ -356,15 +355,15 @@ static int ion_system_contig_heap_allocate(struct ion_heap *heap,
 	for (i = len >> PAGE_SHIFT; i < (1 << order); i++)
 		__free_page(page + i);
 
-	table = kzalloc(sizeof(struct sg_table), GFP_KERNEL);
+	table = kmalloc(sizeof(struct sg_table), GFP_KERNEL);
 	if (!table) {
 		ret = -ENOMEM;
-		goto out;
+		goto free_pages;
 	}
 
 	ret = sg_alloc_table(table, 1, GFP_KERNEL);
 	if (ret)
-		goto out;
+		goto free_table;
 
 	sg_set_page(table->sgl, page, len, 0);
 
@@ -374,10 +373,12 @@ static int ion_system_contig_heap_allocate(struct ion_heap *heap,
 
 	return 0;
 
-out:
+free_table:
+	kfree(table);
+free_pages:
 	for (i = 0; i < len >> PAGE_SHIFT; i++)
 		__free_page(page + i);
-	kfree(table);
+
 	return ret;
 }
 
@@ -443,4 +444,3 @@ void ion_system_contig_heap_destroy(struct ion_heap *heap)
 {
 	kfree(heap);
 }
-
diff --git a/drivers/staging/android/logger.c b/drivers/staging/android/logger.c
index d42f5785f09..2772e01b37f 100644
--- a/drivers/staging/android/logger.c
+++ b/drivers/staging/android/logger.c
@@ -108,6 +108,7 @@ static inline struct logger_log *file_get_log(struct file *file)
 {
 	if (file->f_mode & FMODE_READ) {
 		struct logger_reader *reader = file->private_data;
+
 		return reader->log;
 	} else
 		return file->private_data;
@@ -124,6 +125,7 @@ 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,
@@ -642,6 +644,7 @@ static unsigned int logger_poll(struct file *file, poll_table *wait)
 static long logger_set_version(struct logger_reader *reader, void __user *arg)
 {
 	int version;
+
 	if (copy_from_user(&version, arg, sizeof(int)))
 		return -EFAULT;
 
diff --git a/drivers/staging/android/ram_console.h b/drivers/staging/android/ram_console.h
deleted file mode 100644
index 9f1125c1106..00000000000
--- a/drivers/staging/android/ram_console.h
+++ /dev/null
@@ -1,22 +0,0 @@
-/*
- * Copyright (C) 2010 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 _INCLUDE_LINUX_PLATFORM_DATA_RAM_CONSOLE_H_
-#define _INCLUDE_LINUX_PLATFORM_DATA_RAM_CONSOLE_H_
-
-struct ram_console_platform_data {
-	const char *bootinfo;
-};
-
-#endif /* _INCLUDE_LINUX_PLATFORM_DATA_RAM_CONSOLE_H_ */
diff --git a/drivers/staging/android/sw_sync.c b/drivers/staging/android/sw_sync.c
index f24493ac65e..12a136ec1ce 100644
--- a/drivers/staging/android/sw_sync.c
+++ b/drivers/staging/android/sw_sync.c
@@ -97,6 +97,7 @@ 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);
 }
 
@@ -156,6 +157,7 @@ static int sw_sync_open(struct inode *inode, struct file *file)
 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;
 }
diff --git a/drivers/staging/android/sync.c b/drivers/staging/android/sync.c
index 3d05f662110..18174f7c871 100644
--- a/drivers/staging/android/sync.c
+++ b/drivers/staging/android/sync.c
@@ -92,6 +92,10 @@ static void sync_timeline_free(struct kref *kref)
 void sync_timeline_destroy(struct sync_timeline *obj)
 {
 	obj->destroyed = true;
+	/*
+	 * Ensure timeline is marked as destroyed before
+	 * changing timeline's fences status.
+	 */
 	smp_wmb();
 
 	/*
@@ -384,6 +388,7 @@ static void sync_fence_detach_pts(struct sync_fence *fence)
 
 	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);
 	}
 }
@@ -394,6 +399,7 @@ static void sync_fence_free_pts(struct sync_fence *fence)
 
 	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);
 	}
 }
@@ -827,6 +833,7 @@ 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);
@@ -856,18 +863,21 @@ static const char *sync_status_str(int status)
 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) {
@@ -892,6 +902,7 @@ static void sync_print_obj(struct seq_file *s, struct sync_timeline *obj)
 
 	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) {
@@ -1001,6 +1012,7 @@ static void sync_dump(void)
 	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;
diff --git a/drivers/staging/android/timed_gpio.c b/drivers/staging/android/timed_gpio.c
index 0c7fdc83b33..180c209a009 100644
--- a/drivers/staging/android/timed_gpio.c
+++ b/drivers/staging/android/timed_gpio.c
@@ -51,6 +51,7 @@ static int gpio_get_time(struct timed_output_dev *dev)
 	if (hrtimer_active(&data->timer)) {
 		ktime_t r = hrtimer_get_remaining(&data->timer);
 		struct timeval t = ktime_to_timeval(r);
+
 		return t.tv_sec * 1000 + t.tv_usec / 1000;
 	} else
 		return 0;
@@ -91,8 +92,8 @@ static int timed_gpio_probe(struct platform_device *pdev)
 		return -EBUSY;
 
 	gpio_data = devm_kzalloc(&pdev->dev,
-				sizeof(struct timed_gpio_data) * pdata->num_gpios,
-				GFP_KERNEL);
+			sizeof(struct timed_gpio_data) * pdata->num_gpios,
+			GFP_KERNEL);
 	if (!gpio_data)
 		return -ENOMEM;
 
diff --git a/drivers/staging/android/uapi/ion.h b/drivers/staging/android/uapi/ion.h
index f09e7c154d6..6aa49567337 100644
--- a/drivers/staging/android/uapi/ion.h
+++ b/drivers/staging/android/uapi/ion.h
@@ -27,12 +27,12 @@ typedef int ion_user_handle_t;
  * @ION_HEAP_TYPE_SYSTEM:	 memory allocated via vmalloc
  * @ION_HEAP_TYPE_SYSTEM_CONTIG: memory allocated via kmalloc
  * @ION_HEAP_TYPE_CARVEOUT:	 memory allocated from a prereserved
- * 				 carveout heap, allocations are physically
- * 				 contiguous
+ *				 carveout heap, allocations are physically
+ *				 contiguous
  * @ION_HEAP_TYPE_DMA:		 memory allocated via DMA API
  * @ION_NUM_HEAPS:		 helper for iterating over heaps, a bit mask
- * 				 is used to identify the heaps, so only 32
- * 				 total heap types are supported
+ *				 is used to identify the heaps, so only 32
+ *				 total heap types are supported
  */
 enum ion_heap_type {
 	ION_HEAP_TYPE_SYSTEM,
@@ -50,7 +50,7 @@ enum ion_heap_type {
 #define ION_HEAP_CARVEOUT_MASK		(1 << ION_HEAP_TYPE_CARVEOUT)
 #define ION_HEAP_TYPE_DMA_MASK		(1 << ION_HEAP_TYPE_DMA)
 
-#define ION_NUM_HEAP_IDS		sizeof(unsigned int) * 8
+#define ION_NUM_HEAP_IDS		(sizeof(unsigned int) * 8)
 
 /**
  * allocation flags - the lower 16 bits are used by core ion, the upper 16
@@ -78,7 +78,7 @@ enum ion_heap_type {
  * @align:		required alignment of the allocation
  * @heap_id_mask:	mask of heap ids to allocate from
  * @flags:		flags passed to heap
- * @handle:		pointer that will be populated with a cookie to use to 
+ * @handle:		pointer that will be populated with a cookie to use to
  *			refer to this allocation
  *
  * Provided by userspace as an argument to the ioctl
diff --git a/drivers/staging/bcm/Bcmchar.c b/drivers/staging/bcm/Bcmchar.c
index ae7490b4d70..606d5f5e921 100644
--- a/drivers/staging/bcm/Bcmchar.c
+++ b/drivers/staging/bcm/Bcmchar.c
@@ -1,6 +1,94 @@
 #include <linux/fs.h>
 
 #include "headers.h"
+
+static int bcm_handle_nvm_read_cmd(struct bcm_mini_adapter *Adapter,
+	PUCHAR pReadData, struct bcm_nvm_readwrite *stNVMReadWrite)
+{
+	INT Status = STATUS_FAILURE;
+
+	down(&Adapter->NVMRdmWrmLock);
+
+	if ((Adapter->IdleMode == TRUE) || (Adapter->bShutStatus == TRUE) ||
+			(Adapter->bPreparingForLowPowerMode == TRUE)) {
+
+		BCM_DEBUG_PRINT(Adapter,
+			DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+			"Device is in Idle/Shutdown Mode\n");
+		up(&Adapter->NVMRdmWrmLock);
+		kfree(pReadData);
+		return -EACCES;
+	}
+
+	Status = BeceemNVMRead(Adapter, (PUINT)pReadData,
+			       stNVMReadWrite->uiOffset,
+			       stNVMReadWrite->uiNumBytes);
+	up(&Adapter->NVMRdmWrmLock);
+
+	if (Status != STATUS_SUCCESS) {
+		kfree(pReadData);
+		return Status;
+	}
+
+	if (copy_to_user(stNVMReadWrite->pBuffer, pReadData,
+			stNVMReadWrite->uiNumBytes)) {
+		kfree(pReadData);
+		return -EFAULT;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+static int handle_flash2x_adapter(struct bcm_mini_adapter *Adapter,
+	PUCHAR pReadData, struct bcm_nvm_readwrite *stNVMReadWrite)
+{
+	/*
+	 * New Requirement:-
+	 * DSD section updation will be allowed in two case:-
+	 * 1.  if DSD sig is present in DSD header means dongle
+	 * is ok and updation is fruitfull
+	 * 2.  if point 1 failes then user buff should have
+	 * DSD sig. this point ensures that if dongle is
+	 * corrupted then user space program first modify
+	 * the DSD header with valid DSD sig so that this
+	 * as well as further write may be worthwhile.
+	 *
+	 * This restriction has been put assuming that
+	 * if DSD sig is corrupted, DSD data won't be
+	 * considered valid.
+	 */
+	INT Status;
+	ULONG ulDSDMagicNumInUsrBuff = 0;
+
+	Status = BcmFlash2xCorruptSig(Adapter, Adapter->eActiveDSD);
+	if (Status == STATUS_SUCCESS)
+		return STATUS_SUCCESS;
+
+	if (((stNVMReadWrite->uiOffset + stNVMReadWrite->uiNumBytes) !=
+			Adapter->uiNVMDSDSize) ||
+			(stNVMReadWrite->uiNumBytes < SIGNATURE_SIZE)) {
+
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+				"DSD Sig is present neither in Flash nor User provided Input..");
+		up(&Adapter->NVMRdmWrmLock);
+		kfree(pReadData);
+		return Status;
+	}
+
+	ulDSDMagicNumInUsrBuff =
+		ntohl(*(PUINT)(pReadData + stNVMReadWrite->uiNumBytes -
+		      SIGNATURE_SIZE));
+	if (ulDSDMagicNumInUsrBuff != DSD_IMAGE_MAGIC_NUMBER) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+				"DSD Sig is present neither in Flash nor User provided Input..");
+		up(&Adapter->NVMRdmWrmLock);
+		kfree(pReadData);
+		return Status;
+	}
+
+	return STATUS_SUCCESS;
+}
+
 /***************************************************************
 * Function	  - bcm_char_open()
 *
@@ -101,9 +189,11 @@ static ssize_t bcm_char_read(struct file *filp, char __user *buf, size_t size,
 	int wait_ret_val = 0;
 	unsigned long ret = 0;
 
-	wait_ret_val = wait_event_interruptible(Adapter->process_read_wait_queue,
-						(pTarang->RxAppControlHead ||
-						 Adapter->device_removed));
+	wait_ret_val = wait_event_interruptible(
+				Adapter->process_read_wait_queue,
+				(pTarang->RxAppControlHead ||
+				Adapter->device_removed));
+
 	if ((wait_ret_val == -ERESTARTSYS)) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 				"Exiting as i've been asked to exit!!!\n");
@@ -151,7 +241,7 @@ static ssize_t bcm_char_read(struct file *filp, char __user *buf, size_t size,
 }
 
 static int bcm_char_ioctl_reg_read_private(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+					   struct bcm_mini_adapter *Adapter)
 {
 	struct bcm_rdm_buffer sRdmBuffer = {0};
 	struct bcm_ioctl_buffer IoBuffer;
@@ -202,7 +292,7 @@ static int bcm_char_ioctl_reg_read_private(void __user *argp,
 }
 
 static int bcm_char_ioctl_reg_write_private(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+					    struct bcm_mini_adapter *Adapter)
 {
 	struct bcm_wrm_buffer sWrmBuffer = {0};
 	struct bcm_ioctl_buffer IoBuffer;
@@ -249,7 +339,7 @@ static int bcm_char_ioctl_reg_write_private(void __user *argp,
 }
 
 static int bcm_char_ioctl_eeprom_reg_read(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+					  struct bcm_mini_adapter *Adapter)
 {
 	struct bcm_rdm_buffer sRdmBuffer = {0};
 	struct bcm_ioctl_buffer IoBuffer;
@@ -317,7 +407,8 @@ static int bcm_char_ioctl_eeprom_reg_read(void __user *argp,
 }
 
 static int bcm_char_ioctl_eeprom_reg_write(void __user *argp,
-	struct bcm_mini_adapter *Adapter, UINT cmd)
+					   struct bcm_mini_adapter *Adapter,
+					   UINT cmd)
 {
 	struct bcm_wrm_buffer sWrmBuffer = {0};
 	struct bcm_ioctl_buffer IoBuffer;
@@ -383,7 +474,7 @@ static int bcm_char_ioctl_eeprom_reg_write(void __user *argp,
 }
 
 static int bcm_char_ioctl_gpio_set_request(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+					   struct bcm_mini_adapter *Adapter)
 {
 	struct bcm_gpio_info gpio_info = {0};
 	struct bcm_ioctl_buffer IoBuffer;
@@ -410,7 +501,8 @@ static int bcm_char_ioctl_gpio_set_request(void __user *argp,
 	if (IoBuffer.InputLength > sizeof(gpio_info))
 		return -EINVAL;
 
-	if (copy_from_user(&gpio_info, IoBuffer.InputBuffer, IoBuffer.InputLength))
+	if (copy_from_user(&gpio_info, IoBuffer.InputBuffer,
+			   IoBuffer.InputLength))
 		return -EFAULT;
 
 	uiBit  = gpio_info.uiGpioNumber;
@@ -492,7 +584,7 @@ static int bcm_char_ioctl_gpio_set_request(void __user *argp,
 }
 
 static int bcm_char_ioctl_led_thread_state_change_req(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+		struct bcm_mini_adapter *Adapter)
 {
 	struct bcm_user_thread_req threadReq = {0};
 	struct bcm_ioctl_buffer IoBuffer;
@@ -516,7 +608,8 @@ static int bcm_char_ioctl_led_thread_state_change_req(void __user *argp,
 	if (IoBuffer.InputLength > sizeof(threadReq))
 		return -EINVAL;
 
-	if (copy_from_user(&threadReq, IoBuffer.InputBuffer, IoBuffer.InputLength))
+	if (copy_from_user(&threadReq, IoBuffer.InputBuffer,
+			   IoBuffer.InputLength))
 		return -EFAULT;
 
 	/* if LED thread is running(Actively or Inactively)
@@ -542,7 +635,7 @@ static int bcm_char_ioctl_led_thread_state_change_req(void __user *argp,
 }
 
 static int bcm_char_ioctl_gpio_status_request(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+		struct bcm_mini_adapter *Adapter)
 {
 	struct bcm_gpio_info gpio_info = {0};
 	struct bcm_ioctl_buffer IoBuffer;
@@ -584,7 +677,7 @@ static int bcm_char_ioctl_gpio_status_request(void __user *argp,
 }
 
 static int bcm_char_ioctl_gpio_multi_request(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+		struct bcm_mini_adapter *Adapter)
 {
 	struct bcm_gpio_multi_info gpio_multi_info[MAX_IDX];
 	struct bcm_gpio_multi_info *pgpio_multi_info =
@@ -594,7 +687,8 @@ static int bcm_char_ioctl_gpio_multi_request(void __user *argp,
 	INT Status = STATUS_FAILURE;
 	int bytes;
 
-	memset(pgpio_multi_info, 0, MAX_IDX * sizeof(struct bcm_gpio_multi_info));
+	memset(pgpio_multi_info, 0,
+	       MAX_IDX * sizeof(struct bcm_gpio_multi_info));
 
 	if ((Adapter->IdleMode == TRUE) ||
 		(Adapter->bShutStatus == TRUE) ||
@@ -610,11 +704,11 @@ static int bcm_char_ioctl_gpio_multi_request(void __user *argp,
 		IoBuffer.OutputLength = sizeof(gpio_multi_info);
 
 	if (copy_from_user(&gpio_multi_info, IoBuffer.InputBuffer,
-		IoBuffer.InputLength))
+			   IoBuffer.InputLength))
 		return -EFAULT;
 
-	if (IsReqGpioIsLedInNVM(Adapter,
-		pgpio_multi_info[WIMAX_IDX].uiGPIOMask) == false) {
+	if (IsReqGpioIsLedInNVM(Adapter, pgpio_multi_info[WIMAX_IDX].uiGPIOMask)
+			== false) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG,
 				DBG_LVL_ALL,
 				"Sorry, Requested GPIO<0x%X> is not correspond to NVM LED bit map<0x%X>!!!",
@@ -627,7 +721,7 @@ static int bcm_char_ioctl_gpio_multi_request(void __user *argp,
 	if ((pgpio_multi_info[WIMAX_IDX].uiGPIOMask) &
 		(pgpio_multi_info[WIMAX_IDX].uiGPIOCommand)) {
 		/* Set 1's in GPIO OUTPUT REGISTER */
-		*(UINT *)ucResetValue =  pgpio_multi_info[WIMAX_IDX].uiGPIOMask &
+		*(UINT *)ucResetValue = pgpio_multi_info[WIMAX_IDX].uiGPIOMask &
 			pgpio_multi_info[WIMAX_IDX].uiGPIOCommand &
 			pgpio_multi_info[WIMAX_IDX].uiGPIOValue;
 
@@ -643,7 +737,8 @@ static int bcm_char_ioctl_gpio_multi_request(void __user *argp,
 		}
 
 		/* Clear to 0's in GPIO OUTPUT REGISTER */
-		*(UINT *)ucResetValue = (pgpio_multi_info[WIMAX_IDX].uiGPIOMask &
+		*(UINT *)ucResetValue =
+			(pgpio_multi_info[WIMAX_IDX].uiGPIOMask &
 			pgpio_multi_info[WIMAX_IDX].uiGPIOCommand &
 			(~(pgpio_multi_info[WIMAX_IDX].uiGPIOValue)));
 
@@ -661,7 +756,7 @@ static int bcm_char_ioctl_gpio_multi_request(void __user *argp,
 
 	if (pgpio_multi_info[WIMAX_IDX].uiGPIOMask) {
 		bytes = rdmaltWithLock(Adapter, (UINT)GPIO_PIN_STATE_REGISTER,
-			(PUINT)ucResetValue, sizeof(UINT));
+				       (PUINT)ucResetValue, sizeof(UINT));
 
 		if (bytes < 0) {
 			Status = bytes;
@@ -672,7 +767,8 @@ static int bcm_char_ioctl_gpio_multi_request(void __user *argp,
 			Status = STATUS_SUCCESS;
 		}
 
-		pgpio_multi_info[WIMAX_IDX].uiGPIOValue = (*(UINT *)ucResetValue &
+		pgpio_multi_info[WIMAX_IDX].uiGPIOValue =
+			(*(UINT *)ucResetValue &
 			pgpio_multi_info[WIMAX_IDX].uiGPIOMask);
 	}
 
@@ -688,7 +784,7 @@ static int bcm_char_ioctl_gpio_multi_request(void __user *argp,
 }
 
 static int bcm_char_ioctl_gpio_mode_request(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+		struct bcm_mini_adapter *Adapter)
 {
 	struct bcm_gpio_multi_mode gpio_multi_mode[MAX_IDX];
 	struct bcm_gpio_multi_mode *pgpio_multi_mode =
@@ -728,8 +824,8 @@ static int bcm_char_ioctl_gpio_mode_request(void __user *argp,
 	}
 
 	/* Validating the request */
-	if (IsReqGpioIsLedInNVM(Adapter,
-		pgpio_multi_mode[WIMAX_IDX].uiGPIOMask) == false) {
+	if (IsReqGpioIsLedInNVM(Adapter, pgpio_multi_mode[WIMAX_IDX].uiGPIOMask)
+			== false) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 				"Sorry, Requested GPIO<0x%X> is not correspond to NVM LED bit map<0x%X>!!!",
 				pgpio_multi_mode[WIMAX_IDX].uiGPIOMask,
@@ -739,11 +835,13 @@ static int bcm_char_ioctl_gpio_mode_request(void __user *argp,
 
 	if (pgpio_multi_mode[WIMAX_IDX].uiGPIOMask) {
 		/* write all OUT's (1's) */
-		*(UINT *) ucResetValue |= (pgpio_multi_mode[WIMAX_IDX].uiGPIOMode &
+		*(UINT *) ucResetValue |=
+			(pgpio_multi_mode[WIMAX_IDX].uiGPIOMode &
 					pgpio_multi_mode[WIMAX_IDX].uiGPIOMask);
 
 		/* write all IN's (0's) */
-		*(UINT *) ucResetValue &= ~((~pgpio_multi_mode[WIMAX_IDX].uiGPIOMode) &
+		*(UINT *) ucResetValue &=
+			~((~pgpio_multi_mode[WIMAX_IDX].uiGPIOMode) &
 					pgpio_multi_mode[WIMAX_IDX].uiGPIOMask);
 
 		/* Currently implemented return the modes of all GPIO's
@@ -779,7 +877,7 @@ static int bcm_char_ioctl_gpio_mode_request(void __user *argp,
 }
 
 static int bcm_char_ioctl_misc_request(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+		struct bcm_mini_adapter *Adapter)
 {
 	struct bcm_ioctl_buffer IoBuffer;
 	PVOID pvBuffer = NULL;
@@ -801,9 +899,11 @@ static int bcm_char_ioctl_misc_request(void __user *argp,
 		return PTR_ERR(pvBuffer);
 
 	down(&Adapter->LowPowerModeSync);
-	Status = wait_event_interruptible_timeout(Adapter->lowpower_mode_wait_queue,
-		!Adapter->bPreparingForLowPowerMode,
-		(1 * HZ));
+	Status = wait_event_interruptible_timeout(
+			Adapter->lowpower_mode_wait_queue,
+			!Adapter->bPreparingForLowPowerMode,
+			(1 * HZ));
+
 	if (Status == -ERESTARTSYS)
 		goto cntrlEnd;
 
@@ -822,7 +922,7 @@ cntrlEnd:
 }
 
 static int bcm_char_ioctl_buffer_download_start(
-	struct bcm_mini_adapter *Adapter)
+		struct bcm_mini_adapter *Adapter)
 {
 	INT Status;
 
@@ -833,7 +933,8 @@ static int bcm_char_ioctl_buffer_download_start(
 	}
 
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-		"Starting the firmware download PID =0x%x!!!!\n", current->pid);
+			"Starting the firmware download PID =0x%x!!!!\n",
+			current->pid);
 
 	if (down_trylock(&Adapter->fw_download_sema))
 		return -EBUSY;
@@ -858,7 +959,7 @@ static int bcm_char_ioctl_buffer_download_start(
 }
 
 static int bcm_char_ioctl_buffer_download(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+		struct bcm_mini_adapter *Adapter)
 {
 	struct bcm_firmware_info *psFwInfo = NULL;
 	struct bcm_ioctl_buffer IoBuffer;
@@ -943,7 +1044,7 @@ static int bcm_char_ioctl_buffer_download(void __user *argp,
 }
 
 static int bcm_char_ioctl_buffer_download_stop(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+		struct bcm_mini_adapter *Adapter)
 {
 	INT Status;
 	int timeout = 0;
@@ -1036,7 +1137,7 @@ static int bcm_char_ioctl_chip_reset(struct bcm_mini_adapter *Adapter)
 }
 
 static int bcm_char_ioctl_qos_threshold(ULONG arg,
-	struct bcm_mini_adapter *Adapter)
+					struct bcm_mini_adapter *Adapter)
 {
 	USHORT uiLoopIndex;
 
@@ -1388,7 +1489,6 @@ static int bcm_char_ioctl_nvm_rw(void __user *argp,
 	struct timeval tv0, tv1;
 	struct bcm_ioctl_buffer IoBuffer;
 	PUCHAR pReadData = NULL;
-	ULONG ulDSDMagicNumInUsrBuff = 0;
 	INT Status = STATUS_FAILURE;
 
 	memset(&tv0, 0, sizeof(struct timeval));
@@ -1438,34 +1538,10 @@ static int bcm_char_ioctl_nvm_rw(void __user *argp,
 
 	do_gettimeofday(&tv0);
 	if (IOCTL_BCM_NVM_READ == cmd) {
-		down(&Adapter->NVMRdmWrmLock);
-
-		if ((Adapter->IdleMode == TRUE) ||
-			(Adapter->bShutStatus == TRUE) ||
-			(Adapter->bPreparingForLowPowerMode == TRUE)) {
-
-			BCM_DEBUG_PRINT(Adapter,
-				DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-				"Device is in Idle/Shutdown Mode\n");
-			up(&Adapter->NVMRdmWrmLock);
-			kfree(pReadData);
-			return -EACCES;
-		}
-
-		Status = BeceemNVMRead(Adapter, (PUINT)pReadData,
-			stNVMReadWrite.uiOffset, stNVMReadWrite.uiNumBytes);
-		up(&Adapter->NVMRdmWrmLock);
-
-		if (Status != STATUS_SUCCESS) {
-			kfree(pReadData);
-			return Status;
-		}
-
-		if (copy_to_user(stNVMReadWrite.pBuffer, pReadData,
-			stNVMReadWrite.uiNumBytes)) {
-			kfree(pReadData);
-			return -EFAULT;
-		}
+		int ret = bcm_handle_nvm_read_cmd(Adapter, pReadData,
+				&stNVMReadWrite);
+		if (ret != STATUS_SUCCESS)
+			return ret;
 	} else {
 		down(&Adapter->NVMRdmWrmLock);
 
@@ -1483,48 +1559,11 @@ static int bcm_char_ioctl_nvm_rw(void __user *argp,
 
 		Adapter->bHeaderChangeAllowed = TRUE;
 		if (IsFlash2x(Adapter)) {
-			/*
-			 * New Requirement:-
-			 * DSD section updation will be allowed in two case:-
-			 * 1.  if DSD sig is present in DSD header means dongle
-			 * is ok and updation is fruitfull
-			 * 2.  if point 1 failes then user buff should have
-			 * DSD sig. this point ensures that if dongle is
-			 * corrupted then user space program first modify
-			 * the DSD header with valid DSD sig so that this
-			 * as well as further write may be worthwhile.
-			 *
-			 * This restriction has been put assuming that
-			 * if DSD sig is corrupted, DSD data won't be
-			 * considered valid.
-			 */
-
-			Status = BcmFlash2xCorruptSig(Adapter,
-				Adapter->eActiveDSD);
-			if (Status != STATUS_SUCCESS) {
-				if (((stNVMReadWrite.uiOffset + stNVMReadWrite.uiNumBytes) !=
-					Adapter->uiNVMDSDSize) ||
-					(stNVMReadWrite.uiNumBytes < SIGNATURE_SIZE)) {
-
-					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
-						OSAL_DBG, DBG_LVL_ALL,
-						"DSD Sig is present neither in Flash nor User provided Input..");
-					up(&Adapter->NVMRdmWrmLock);
-					kfree(pReadData);
-					return Status;
-				}
-
-				ulDSDMagicNumInUsrBuff = ntohl(*(PUINT)(pReadData + stNVMReadWrite.uiNumBytes - SIGNATURE_SIZE));
-				if (ulDSDMagicNumInUsrBuff !=
-					DSD_IMAGE_MAGIC_NUMBER) {
-					BCM_DEBUG_PRINT(Adapter,
-					DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-						"DSD Sig is present neither in Flash nor User provided Input..");
-					up(&Adapter->NVMRdmWrmLock);
-					kfree(pReadData);
-					return Status;
-				}
-			}
+			int ret = handle_flash2x_adapter(Adapter,
+							pReadData,
+							&stNVMReadWrite);
+			if (ret != STATUS_SUCCESS)
+				return ret;
 		}
 
 		Status = BeceemNVMWrite(Adapter, (PUINT)pReadData,
@@ -1546,7 +1585,8 @@ static int bcm_char_ioctl_nvm_rw(void __user *argp,
 	do_gettimeofday(&tv1);
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 		" timetaken by Write/read :%ld msec\n",
-		(tv1.tv_sec - tv0.tv_sec)*1000 + (tv1.tv_usec - tv0.tv_usec)/1000);
+		(tv1.tv_sec - tv0.tv_sec)*1000 +
+		(tv1.tv_usec - tv0.tv_usec)/1000);
 
 	kfree(pReadData);
 	return STATUS_SUCCESS;
@@ -1582,13 +1622,17 @@ static int bcm_char_ioctl_flash2x_section_read(void __user *argp,
 		return -EFAULT;
 
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"\nsFlash2xRead.Section :%x", sFlash2xRead.Section);
+			"\nsFlash2xRead.Section :%x",
+			sFlash2xRead.Section);
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"\nsFlash2xRead.offset :%x", sFlash2xRead.offset);
+			"\nsFlash2xRead.offset :%x",
+			sFlash2xRead.offset);
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"\nsFlash2xRead.numOfBytes :%x", sFlash2xRead.numOfBytes);
+			"\nsFlash2xRead.numOfBytes :%x",
+			sFlash2xRead.numOfBytes);
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"\nsFlash2xRead.bVerify :%x\n", sFlash2xRead.bVerify);
+			"\nsFlash2xRead.bVerify :%x\n",
+			sFlash2xRead.bVerify);
 
 	/* This was internal to driver for raw read.
 	 * now it has ben exposed to user space app.
@@ -1608,7 +1652,7 @@ static int bcm_char_ioctl_flash2x_section_read(void __user *argp,
 
 	if (pReadBuff == NULL) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Memory allocation failed for Flash 2.x Read Structure");
+				"Memory allocation failed for Flash 2.x Read Structure");
 		return -ENOMEM;
 	}
 	down(&Adapter->NVMRdmWrmLock);
@@ -1618,7 +1662,8 @@ static int bcm_char_ioctl_flash2x_section_read(void __user *argp,
 		(Adapter->bPreparingForLowPowerMode == TRUE)) {
 
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG,
-			DBG_LVL_ALL, "Device is in Idle/Shutdown Mode\n");
+				DBG_LVL_ALL,
+				"Device is in Idle/Shutdown Mode\n");
 		up(&Adapter->NVMRdmWrmLock);
 		kfree(pReadBuff);
 		return -EACCES;
@@ -1735,8 +1780,10 @@ static int bcm_char_ioctl_flash2x_section_write(void __user *argp,
 
 	/* extracting the remainder of the given offset. */
 	WriteBytes = Adapter->uiSectorSize;
-	if (WriteOffset % Adapter->uiSectorSize)
-		WriteBytes = Adapter->uiSectorSize - (WriteOffset % Adapter->uiSectorSize);
+	if (WriteOffset % Adapter->uiSectorSize) {
+		WriteBytes = Adapter->uiSectorSize -
+			(WriteOffset % Adapter->uiSectorSize);
+	}
 
 	if (NOB < WriteBytes)
 		WriteBytes = NOB;
@@ -1769,8 +1816,10 @@ static int bcm_char_ioctl_flash2x_section_write(void __user *argp,
 
 		/* Writing the data from Flash 2.x */
 		Status = BcmFlash2xBulkWrite(Adapter, (PUINT)pWriteBuff,
-			sFlash2xWrite.Section, WriteOffset, WriteBytes,
-			sFlash2xWrite.bVerify);
+					     sFlash2xWrite.Section,
+					     WriteOffset,
+					     WriteBytes,
+					     sFlash2xWrite.bVerify);
 
 		if (Status) {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
@@ -1800,7 +1849,6 @@ static int bcm_char_ioctl_flash2x_section_bitmap(void __user *argp,
 {
 	struct bcm_flash2x_bitmap *psFlash2xBitMap;
 	struct bcm_ioctl_buffer IoBuffer;
-	INT Status = STATUS_FAILURE;
 
 BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 	"IOCTL_BCM_GET_FLASH2X_SECTION_BITMAP Called");
@@ -1811,7 +1859,9 @@ BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 	if (IoBuffer.OutputLength != sizeof(struct bcm_flash2x_bitmap))
 		return -EINVAL;
 
-	psFlash2xBitMap = kzalloc(sizeof(struct bcm_flash2x_bitmap), GFP_KERNEL);
+	psFlash2xBitMap = kzalloc(sizeof(struct bcm_flash2x_bitmap),
+			GFP_KERNEL);
+
 	if (psFlash2xBitMap == NULL) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
 			"Memory is not available");
@@ -1841,7 +1891,7 @@ BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 	}
 
 	kfree(psFlash2xBitMap);
-	return Status;
+	return STATUS_FAILURE;
 }
 
 static int bcm_char_ioctl_set_active_section(void __user *argp,
@@ -1852,23 +1902,24 @@ static int bcm_char_ioctl_set_active_section(void __user *argp,
 	struct bcm_ioctl_buffer IoBuffer;
 
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"IOCTL_BCM_SET_ACTIVE_SECTION Called");
+			"IOCTL_BCM_SET_ACTIVE_SECTION Called");
 
 	if (IsFlash2x(Adapter) != TRUE) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Flash Does not have 2.x map");
+				"Flash Does not have 2.x map");
 		return -EINVAL;
 	}
 
-	Status = copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer));
+	Status = copy_from_user(&IoBuffer, argp,
+				sizeof(struct bcm_ioctl_buffer));
 	if (Status) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Copy of IOCTL BUFFER failed");
+				"Copy of IOCTL BUFFER failed");
 		return -EFAULT;
 	}
 
 	Status = copy_from_user(&eFlash2xSectionVal,
-		IoBuffer.InputBuffer, sizeof(INT));
+				IoBuffer.InputBuffer, sizeof(INT));
 	if (Status) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
 			"Copy of flash section val failed");
@@ -1882,7 +1933,7 @@ static int bcm_char_ioctl_set_active_section(void __user *argp,
 		(Adapter->bPreparingForLowPowerMode == TRUE)) {
 
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"Device is in Idle/Shutdown Mode\n");
+				"Device is in Idle/Shutdown Mode\n");
 		up(&Adapter->NVMRdmWrmLock);
 		return -EACCES;
 	}
@@ -1890,8 +1941,8 @@ static int bcm_char_ioctl_set_active_section(void __user *argp,
 	Status = BcmSetActiveSection(Adapter, eFlash2xSectionVal);
 	if (Status)
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Failed to make it's priority Highest. Status %d",
-			Status);
+				"Failed to make it's priority Highest. Status %d",
+				Status);
 
 	up(&Adapter->NVMRdmWrmLock);
 
@@ -1906,57 +1957,59 @@ static int bcm_char_ioctl_copy_section(void __user *argp,
 	INT Status = STATUS_SUCCESS;
 
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"IOCTL_BCM_COPY_SECTION  Called");
+			"IOCTL_BCM_COPY_SECTION  Called");
 
 	Adapter->bAllDSDWriteAllow = false;
 	if (IsFlash2x(Adapter) != TRUE) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Flash Does not have 2.x map");
+				"Flash Does not have 2.x map");
 		return -EINVAL;
 	}
 
-	Status = copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer));
+	Status = copy_from_user(&IoBuffer, argp,
+				sizeof(struct bcm_ioctl_buffer));
 	if (Status) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Copy of IOCTL BUFFER failed Status :%d", Status);
+				"Copy of IOCTL BUFFER failed Status :%d",
+				Status);
 		return -EFAULT;
 	}
 
 	Status = copy_from_user(&sCopySectStrut, IoBuffer.InputBuffer,
-			sizeof(struct bcm_flash2x_copy_section));
+				sizeof(struct bcm_flash2x_copy_section));
 	if (Status) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Copy of Copy_Section_Struct failed with Status :%d",
-			Status);
+				"Copy of Copy_Section_Struct failed with Status :%d",
+				Status);
 		return -EFAULT;
 	}
 
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"Source SEction :%x", sCopySectStrut.SrcSection);
+			"Source SEction :%x", sCopySectStrut.SrcSection);
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"Destination SEction :%x", sCopySectStrut.DstSection);
+			"Destination SEction :%x", sCopySectStrut.DstSection);
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"offset :%x", sCopySectStrut.offset);
+			"offset :%x", sCopySectStrut.offset);
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"NOB :%x", sCopySectStrut.numOfBytes);
+			"NOB :%x", sCopySectStrut.numOfBytes);
 
 	if (IsSectionExistInFlash(Adapter, sCopySectStrut.SrcSection) == false) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Source Section<%x> does not exist in Flash ",
-			sCopySectStrut.SrcSection);
+				"Source Section<%x> does not exist in Flash ",
+				sCopySectStrut.SrcSection);
 		return -EINVAL;
 	}
 
 	if (IsSectionExistInFlash(Adapter, sCopySectStrut.DstSection) == false) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Destinatio Section<%x> does not exist in Flash ",
-			sCopySectStrut.DstSection);
+				"Destinatio Section<%x> does not exist in Flash ",
+				sCopySectStrut.DstSection);
 		return -EINVAL;
 	}
 
 	if (sCopySectStrut.SrcSection == sCopySectStrut.DstSection) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"Source and Destination section should be different");
+				"Source and Destination section should be different");
 		return -EINVAL;
 	}
 
@@ -1967,7 +2020,7 @@ static int bcm_char_ioctl_copy_section(void __user *argp,
 		(Adapter->bPreparingForLowPowerMode == TRUE)) {
 
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"Device is in Idle/Shutdown Mode\n");
+				"Device is in Idle/Shutdown Mode\n");
 		up(&Adapter->NVMRdmWrmLock);
 		return -EACCES;
 	}
@@ -1976,13 +2029,13 @@ static int bcm_char_ioctl_copy_section(void __user *argp,
 		sCopySectStrut.SrcSection == ISO_IMAGE2) {
 		if (IsNonCDLessDevice(Adapter)) {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-				"Device is Non-CDLess hence won't have ISO !!");
+					"Device is Non-CDLess hence won't have ISO !!");
 			Status = -EINVAL;
 		} else if (sCopySectStrut.numOfBytes == 0) {
 			Status = BcmCopyISO(Adapter, sCopySectStrut);
 		} else {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-				"Partial Copy of ISO section is not Allowed..");
+					"Partial Copy of ISO section is not Allowed..");
 			Status = STATUS_FAILURE;
 		}
 		up(&Adapter->NVMRdmWrmLock);
@@ -2004,18 +2057,19 @@ static int bcm_char_ioctl_get_flash_cs_info(void __user *argp,
 	INT Status = STATUS_SUCCESS;
 
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		" IOCTL_BCM_GET_FLASH_CS_INFO Called");
+			" IOCTL_BCM_GET_FLASH_CS_INFO Called");
 
-	Status = copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer));
+	Status = copy_from_user(&IoBuffer, argp,
+			sizeof(struct bcm_ioctl_buffer));
 	if (Status) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Copy of IOCTL BUFFER failed");
+				"Copy of IOCTL BUFFER failed");
 		return -EFAULT;
 	}
 
 	if (Adapter->eNVMType != NVM_FLASH) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Connected device does not have flash");
+				"Connected device does not have flash");
 		return -EINVAL;
 	}
 
@@ -2024,15 +2078,15 @@ static int bcm_char_ioctl_get_flash_cs_info(void __user *argp,
 			return -EINVAL;
 
 		if (copy_to_user(IoBuffer.OutputBuffer,
-			Adapter->psFlash2xCSInfo,
-			sizeof(struct bcm_flash2x_cs_info)))
+				 Adapter->psFlash2xCSInfo,
+				 sizeof(struct bcm_flash2x_cs_info)))
 			return -EFAULT;
 	} else {
 		if (IoBuffer.OutputLength < sizeof(struct bcm_flash_cs_info))
 			return -EINVAL;
 
 		if (copy_to_user(IoBuffer.OutputBuffer, Adapter->psFlashCSInfo,
-			sizeof(struct bcm_flash_cs_info)))
+				 sizeof(struct bcm_flash_cs_info)))
 			return -EFAULT;
 	}
 	return Status;
@@ -2048,45 +2102,46 @@ static int bcm_char_ioctl_select_dsd(void __user *argp,
 
 	eFlash2xSectionVal = NO_SECTION_VAL;
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"IOCTL_BCM_SELECT_DSD Called");
+			"IOCTL_BCM_SELECT_DSD Called");
 
 	if (IsFlash2x(Adapter) != TRUE) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Flash Does not have 2.x map");
+				"Flash Does not have 2.x map");
 		return -EINVAL;
 	}
 
-	Status = copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer));
+	Status = copy_from_user(&IoBuffer, argp,
+				sizeof(struct bcm_ioctl_buffer));
 	if (Status) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Copy of IOCTL BUFFER failed");
+				"Copy of IOCTL BUFFER failed");
 		return -EFAULT;
 	}
 	Status = copy_from_user(&eFlash2xSectionVal, IoBuffer.InputBuffer,
 		sizeof(INT));
 	if (Status) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Copy of flash section val failed");
+				"Copy of flash section val failed");
 		return -EFAULT;
 	}
 
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"Read Section :%d", eFlash2xSectionVal);
+			"Read Section :%d", eFlash2xSectionVal);
 	if ((eFlash2xSectionVal != DSD0) &&
 		(eFlash2xSectionVal != DSD1) &&
 		(eFlash2xSectionVal != DSD2)) {
 
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Passed section<%x> is not DSD section",
-			eFlash2xSectionVal);
+				"Passed section<%x> is not DSD section",
+				eFlash2xSectionVal);
 		return STATUS_FAILURE;
 	}
 
 	SectOfset = BcmGetSectionValStartOffset(Adapter, eFlash2xSectionVal);
 	if (SectOfset == INVALID_OFFSET) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Provided Section val <%d> does not exist in Flash 2.x",
-			eFlash2xSectionVal);
+				"Provided Section val <%d> does not exist in Flash 2.x",
+				eFlash2xSectionVal);
 		return -EINVAL;
 	}
 
@@ -2112,14 +2167,14 @@ static int bcm_char_ioctl_nvm_raw_read(void __user *argp,
 
 	if (Adapter->eNVMType != NVM_FLASH) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"NVM TYPE is not Flash");
+				"NVM TYPE is not Flash");
 		return -EINVAL;
 	}
 
 	/* Copy Ioctl Buffer structure */
 	if (copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer))) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"copy_from_user 1 failed\n");
+				"copy_from_user 1 failed\n");
 		return -EFAULT;
 	}
 
@@ -2141,7 +2196,7 @@ static int bcm_char_ioctl_nvm_raw_read(void __user *argp,
 	pReadBuff = kzalloc(BuffSize , GFP_KERNEL);
 	if (pReadBuff == NULL) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Memory allocation failed for Flash 2.x Read Structure");
+				"Memory allocation failed for Flash 2.x Read Structure");
 		return -ENOMEM;
 	}
 	down(&Adapter->NVMRdmWrmLock);
@@ -2150,8 +2205,8 @@ static int bcm_char_ioctl_nvm_raw_read(void __user *argp,
 		(Adapter->bShutStatus == TRUE) ||
 		(Adapter->bPreparingForLowPowerMode == TRUE)) {
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG,
-			DBG_LVL_ALL, "Device is in Idle/Shutdown Mode\n");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+				"Device is in Idle/Shutdown Mode\n");
 		kfree(pReadBuff);
 		up(&Adapter->NVMRdmWrmLock);
 		return -EACCES;
@@ -2170,17 +2225,19 @@ static int bcm_char_ioctl_nvm_raw_read(void __user *argp,
 			ReadOffset, ReadBytes);
 		if (Status) {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-				"Flash 2x read err with Status :%d", Status);
+					"Flash 2x read err with Status :%d",
+					Status);
 			break;
 		}
 
 		BCM_DEBUG_PRINT_BUFFER(Adapter, DBG_TYPE_OTHERS, OSAL_DBG,
-			DBG_LVL_ALL, pReadBuff, ReadBytes);
+				       DBG_LVL_ALL, pReadBuff, ReadBytes);
 
 		Status = copy_to_user(OutPutBuff, pReadBuff, ReadBytes);
 		if (Status) {
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-				"Copy to use failed with status :%d", Status);
+					"Copy to use failed with status :%d",
+					Status);
 			up(&Adapter->NVMRdmWrmLock);
 			kfree(pReadBuff);
 			return -EFAULT;
@@ -2205,26 +2262,27 @@ static int bcm_char_ioctl_cntrlmsg_mask(void __user *argp,
 	ULONG RxCntrlMsgBitMask = 0;
 
 	/* Copy Ioctl Buffer structure */
-	Status = copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer));
+	Status = copy_from_user(&IoBuffer, argp,
+			sizeof(struct bcm_ioctl_buffer));
 	if (Status) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"copy of Ioctl buffer is failed from user space");
+				"copy of Ioctl buffer is failed from user space");
 		return -EFAULT;
 	}
 
 	if (IoBuffer.InputLength != sizeof(unsigned long))
 		return -EINVAL;
 
-	Status = copy_from_user(&RxCntrlMsgBitMask,
-		IoBuffer.InputBuffer, IoBuffer.InputLength);
+	Status = copy_from_user(&RxCntrlMsgBitMask, IoBuffer.InputBuffer,
+				IoBuffer.InputLength);
 	if (Status) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"copy of control bit mask failed from user space");
+				"copy of control bit mask failed from user space");
 		return -EFAULT;
 	}
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"\n Got user defined cntrl msg bit mask :%lx",
-		RxCntrlMsgBitMask);
+			"\n Got user defined cntrl msg bit mask :%lx",
+			RxCntrlMsgBitMask);
 	pTarang->RxCntrlMsgBitMask = RxCntrlMsgBitMask;
 
 	return Status;
@@ -2236,8 +2294,8 @@ static int bcm_char_ioctl_get_device_driver_info(void __user *argp,
 	struct bcm_driver_info DevInfo;
 	struct bcm_ioctl_buffer IoBuffer;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG,
-		DBG_LVL_ALL, "Called IOCTL_BCM_GET_DEVICE_DRIVER_INFO\n");
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+			"Called IOCTL_BCM_GET_DEVICE_DRIVER_INFO\n");
 
 	memset(&DevInfo, 0, sizeof(DevInfo));
 	DevInfo.MaxRDMBufferSize = BUFFER_4K;
@@ -2265,7 +2323,7 @@ static int bcm_char_ioctl_time_since_net_entry(void __user *argp,
 	struct bcm_ioctl_buffer IoBuffer;
 
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"IOCTL_BCM_TIME_SINCE_NET_ENTRY called");
+			"IOCTL_BCM_TIME_SINCE_NET_ENTRY called");
 
 	if (copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer)))
 		return -EFAULT;
@@ -2277,7 +2335,7 @@ static int bcm_char_ioctl_time_since_net_entry(void __user *argp,
 		get_seconds() - Adapter->liTimeSinceLastNetEntry;
 
 	if (copy_to_user(IoBuffer.OutputBuffer, &stTimeElapsedSinceNetEntry,
-		sizeof(struct bcm_time_elapsed)))
+			 sizeof(struct bcm_time_elapsed)))
 		return -EFAULT;
 
 	return STATUS_SUCCESS;
@@ -2355,7 +2413,8 @@ static long bcm_char_ioctl(struct file *filp, UINT cmd, ULONG arg)
 		return Status;
 
 	case BCM_LED_THREAD_STATE_CHANGE_REQ:
-		Status = bcm_char_ioctl_led_thread_state_change_req(argp, Adapter);
+		Status = bcm_char_ioctl_led_thread_state_change_req(argp,
+								    Adapter);
 		return Status;
 
 	case IOCTL_BCM_GPIO_STATUS_REQUEST:
@@ -2394,13 +2453,15 @@ static long bcm_char_ioctl(struct file *filp, UINT cmd, ULONG arg)
 
 	case IOCTL_BE_BUCKET_SIZE:
 		Status = 0;
-		if (get_user(Adapter->BEBucketSize, (unsigned long __user *)arg))
+		if (get_user(Adapter->BEBucketSize,
+			     (unsigned long __user *)arg))
 			Status = -EFAULT;
 		break;
 
 	case IOCTL_RTPS_BUCKET_SIZE:
 		Status = 0;
-		if (get_user(Adapter->rtPSBucketSize, (unsigned long __user *)arg))
+		if (get_user(Adapter->rtPSBucketSize,
+			     (unsigned long __user *)arg))
 			Status = -EFAULT;
 		break;
 
@@ -2420,7 +2481,7 @@ static long bcm_char_ioctl(struct file *filp, UINT cmd, ULONG arg)
 
 	case IOCTL_GET_PACK_INFO:
 		if (copy_to_user(argp, &Adapter->PackInfo,
-			sizeof(struct bcm_packet_info)*NO_OF_QUEUES))
+				 sizeof(struct bcm_packet_info)*NO_OF_QUEUES))
 			return -EFAULT;
 		Status = STATUS_SUCCESS;
 		break;
@@ -2451,7 +2512,7 @@ static long bcm_char_ioctl(struct file *filp, UINT cmd, ULONG arg)
 
 	case IOCTL_BCM_WAKE_UP_DEVICE_FROM_IDLE:
 		if ((false == Adapter->bTriedToWakeUpFromlowPowerMode) &&
-			(TRUE == Adapter->IdleMode)) {
+				(TRUE == Adapter->IdleMode)) {
 			Adapter->usIdleModePattern = ABORT_IDLE_MODE;
 			Adapter->bWakeUpDevice = TRUE;
 			wake_up(&Adapter->process_rx_cntrlpkt);
@@ -2501,7 +2562,7 @@ static long bcm_char_ioctl(struct file *filp, UINT cmd, ULONG arg)
 		/* Right Now we are taking care of only DSD */
 		Adapter->bAllDSDWriteAllow = false;
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"IOCTL_BCM_IDENTIFY_ACTIVE_SECTION called");
+				"IOCTL_BCM_IDENTIFY_ACTIVE_SECTION called");
 		Status = STATUS_SUCCESS;
 		break;
 
@@ -2534,8 +2595,8 @@ static long bcm_char_ioctl(struct file *filp, UINT cmd, ULONG arg)
 		return Status;
 
 	case IOCTL_CLOSE_NOTIFICATION:
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG,
-			DBG_LVL_ALL, "IOCTL_CLOSE_NOTIFICATION");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+				"IOCTL_CLOSE_NOTIFICATION");
 		break;
 
 	default:
@@ -2569,8 +2630,8 @@ int register_control_device_interface(struct bcm_mini_adapter *Adapter)
 	}
 
 	Adapter->pstCreatedClassDevice = device_create(bcm_class, NULL,
-						MKDEV(Adapter->major, 0),
-						Adapter, DEV_NAME);
+						       MKDEV(Adapter->major, 0),
+						       Adapter, DEV_NAME);
 
 	if (IS_ERR(Adapter->pstCreatedClassDevice)) {
 		pr_err(DRV_NAME ": class device create failed\n");
diff --git a/drivers/staging/bcm/CmHost.c b/drivers/staging/bcm/CmHost.c
index 632f81a7c63..fb1d932c5d7 100644
--- a/drivers/staging/bcm/CmHost.c
+++ b/drivers/staging/bcm/CmHost.c
@@ -13,7 +13,19 @@ enum E_CLASSIFIER_ACTION {
 	eDeleteClassifier
 };
 
-static ULONG GetNextTargetBufferLocation(struct bcm_mini_adapter *Adapter, B_UINT16 tid);
+static ULONG GetNextTargetBufferLocation(struct bcm_mini_adapter *Adapter,
+		B_UINT16 tid);
+static void restore_endianess_of_pstClassifierEntry(
+		struct bcm_classifier_rule *pstClassifierEntry,
+		enum bcm_ipaddr_context eIpAddrContext);
+
+static void apply_phs_rule_to_all_classifiers(
+		register struct bcm_mini_adapter *Adapter,
+		register UINT uiSearchRuleIndex,
+		USHORT uVCID,
+		struct bcm_phs_rule *sPhsRule,
+		struct bcm_phs_rules *cPhsRule,
+		struct bcm_add_indication_alt *pstAddIndication);
 
 /************************************************************
  * Function - SearchSfid
@@ -67,13 +79,16 @@ static int SearchFreeSfid(struct bcm_mini_adapter *Adapter)
  *  B_UINT16  uiClassifierID - The classifier ID to be searched
  * Return: int :Classifier table index of matching entry
  */
-static int SearchClsid(struct bcm_mini_adapter *Adapter, ULONG ulSFID, B_UINT16  uiClassifierID)
+static int SearchClsid(struct bcm_mini_adapter *Adapter,
+		ULONG ulSFID,
+		B_UINT16 uiClassifierID)
 {
 	int i;
 
 	for (i = 0; i < MAX_CLASSIFIERS; i++) {
 		if ((Adapter->astClassifierTable[i].bUsed) &&
-			(Adapter->astClassifierTable[i].uiClassifierRuleIndex == uiClassifierID) &&
+			(Adapter->astClassifierTable[i].uiClassifierRuleIndex
+				== uiClassifierID) &&
 			(Adapter->astClassifierTable[i].ulSFID == ulSFID))
 			return i;
 	}
@@ -98,7 +113,8 @@ static int SearchFreeClsid(struct bcm_mini_adapter *Adapter /**Adapter Context*/
 	return MAX_CLASSIFIERS+1;
 }
 
-static VOID deleteSFBySfid(struct bcm_mini_adapter *Adapter, UINT uiSearchRuleIndex)
+static VOID deleteSFBySfid(struct bcm_mini_adapter *Adapter,
+		UINT uiSearchRuleIndex)
 {
 	/* deleting all the packet held in the SF */
 	flush_queue(Adapter, uiSearchRuleIndex);
@@ -107,7 +123,8 @@ static VOID deleteSFBySfid(struct bcm_mini_adapter *Adapter, UINT uiSearchRuleIn
 	DeleteAllClassifiersForSF(Adapter, uiSearchRuleIndex);
 
 	/* Resetting only MIBS related entries in the SF */
-	memset((PVOID)&Adapter->PackInfo[uiSearchRuleIndex], 0, sizeof(struct bcm_mibs_table));
+	memset((PVOID)&Adapter->PackInfo[uiSearchRuleIndex], 0,
+			sizeof(struct bcm_mibs_table));
 }
 
 static inline VOID
@@ -125,70 +142,109 @@ CopyIpAddrToClassifier(struct bcm_classifier_rule *pstClassifierEntry,
 		nSizeOfIPAddressInBytes = IPV6_ADDRESS_SIZEINBYTES;
 
 	/* Destination Ip Address */
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Ip Address Range Length:0x%X ", u8IpAddressLen);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"Ip Address Range Length:0x%X ", u8IpAddressLen);
 	if ((bIpVersion6 ? (IPV6_ADDRESS_SIZEINBYTES * MAX_IP_RANGE_LENGTH * 2) :
 			(TOTAL_MASKED_ADDRESS_IN_BYTES)) >= u8IpAddressLen) {
+
+		union u_ip_address *st_dest_ip =
+			&pstClassifierEntry->stDestIpAddress;
+
+		union u_ip_address *st_src_ip =
+			&pstClassifierEntry->stSrcIpAddress;
+
 		/*
 		 * checking both the mask and address togethor in Classification.
 		 * So length will be : TotalLengthInBytes/nSizeOfIPAddressInBytes * 2
 		 * (nSizeOfIPAddressInBytes for address and nSizeOfIPAddressInBytes for mask)
 		 */
 		if (eIpAddrContext == eDestIpAddress) {
-			pstClassifierEntry->ucIPDestinationAddressLength = u8IpAddressLen/(nSizeOfIPAddressInBytes * 2);
+			pstClassifierEntry->ucIPDestinationAddressLength =
+				u8IpAddressLen/(nSizeOfIPAddressInBytes * 2);
 			if (bIpVersion6) {
-				ptrClassifierIpAddress = pstClassifierEntry->stDestIpAddress.ucIpv6Address;
-				ptrClassifierIpMask = pstClassifierEntry->stDestIpAddress.ucIpv6Mask;
+				ptrClassifierIpAddress =
+					st_dest_ip->ucIpv6Address;
+				ptrClassifierIpMask =
+					st_dest_ip->ucIpv6Mask;
 			} else {
-				ptrClassifierIpAddress = pstClassifierEntry->stDestIpAddress.ucIpv4Address;
-				ptrClassifierIpMask = pstClassifierEntry->stDestIpAddress.ucIpv4Mask;
+				ptrClassifierIpAddress =
+					st_dest_ip->ucIpv4Address;
+				ptrClassifierIpMask =
+					st_dest_ip->ucIpv4Mask;
 			}
 		} else if (eIpAddrContext == eSrcIpAddress) {
-			pstClassifierEntry->ucIPSourceAddressLength = u8IpAddressLen/(nSizeOfIPAddressInBytes * 2);
+			pstClassifierEntry->ucIPSourceAddressLength =
+				u8IpAddressLen/(nSizeOfIPAddressInBytes * 2);
 			if (bIpVersion6) {
-				ptrClassifierIpAddress = pstClassifierEntry->stSrcIpAddress.ucIpv6Address;
-				ptrClassifierIpMask = pstClassifierEntry->stSrcIpAddress.ucIpv6Mask;
+				ptrClassifierIpAddress =
+					st_src_ip->ucIpv6Address;
+				ptrClassifierIpMask = st_src_ip->ucIpv6Mask;
 			} else {
-				ptrClassifierIpAddress = pstClassifierEntry->stSrcIpAddress.ucIpv4Address;
-				ptrClassifierIpMask = pstClassifierEntry->stSrcIpAddress.ucIpv4Mask;
+				ptrClassifierIpAddress =
+					st_src_ip->ucIpv4Address;
+				ptrClassifierIpMask = st_src_ip->ucIpv4Mask;
 			}
 		}
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Address Length:0x%X\n", pstClassifierEntry->ucIPDestinationAddressLength);
-		while ((u8IpAddressLen >= nSizeOfIPAddressInBytes) && (i < MAX_IP_RANGE_LENGTH)) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"Address Length:0x%X\n",
+				pstClassifierEntry->ucIPDestinationAddressLength);
+		while ((u8IpAddressLen >= nSizeOfIPAddressInBytes)
+				&& (i < MAX_IP_RANGE_LENGTH)) {
 			memcpy(ptrClassifierIpAddress +
 				(i * nSizeOfIPAddressInBytes),
-				(pu8IpAddressMaskSrc+(i*nSizeOfIPAddressInBytes*2)),
+				(pu8IpAddressMaskSrc
+					+ (i * nSizeOfIPAddressInBytes * 2)),
 				nSizeOfIPAddressInBytes);
 
 			if (!bIpVersion6) {
 				if (eIpAddrContext == eSrcIpAddress) {
-					pstClassifierEntry->stSrcIpAddress.ulIpv4Addr[i] = ntohl(pstClassifierEntry->stSrcIpAddress.ulIpv4Addr[i]);
-					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Src Ip Address:0x%luX ",
-							pstClassifierEntry->stSrcIpAddress.ulIpv4Addr[i]);
+					st_src_ip->ulIpv4Addr[i] =
+						ntohl(st_src_ip->ulIpv4Addr[i]);
+					BCM_DEBUG_PRINT(Adapter,
+							DBG_TYPE_OTHERS,
+							CONN_MSG,
+							DBG_LVL_ALL,
+							"Src Ip Address:0x%luX ",
+							st_src_ip->ulIpv4Addr[i]);
 				} else if (eIpAddrContext == eDestIpAddress) {
-					pstClassifierEntry->stDestIpAddress.ulIpv4Addr[i] = ntohl(pstClassifierEntry->stDestIpAddress.ulIpv4Addr[i]);
-					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Dest Ip Address:0x%luX ",
-							pstClassifierEntry->stDestIpAddress.ulIpv4Addr[i]);
+					st_dest_ip->ulIpv4Addr[i] =
+						ntohl(st_dest_ip->ulIpv4Addr[i]);
+					BCM_DEBUG_PRINT(Adapter,
+							DBG_TYPE_OTHERS,
+							CONN_MSG,
+							DBG_LVL_ALL,
+							"Dest Ip Address:0x%luX ",
+							st_dest_ip->ulIpv4Addr[i]);
 				}
 			}
 			u8IpAddressLen -= nSizeOfIPAddressInBytes;
 			if (u8IpAddressLen >= nSizeOfIPAddressInBytes) {
 				memcpy(ptrClassifierIpMask +
 					(i * nSizeOfIPAddressInBytes),
-					(pu8IpAddressMaskSrc+nSizeOfIPAddressInBytes +
-						(i*nSizeOfIPAddressInBytes*2)),
+					(pu8IpAddressMaskSrc
+						+ nSizeOfIPAddressInBytes
+						+ (i * nSizeOfIPAddressInBytes * 2)),
 					nSizeOfIPAddressInBytes);
 
 				if (!bIpVersion6) {
 					if (eIpAddrContext == eSrcIpAddress) {
-						pstClassifierEntry->stSrcIpAddress.ulIpv4Mask[i] =
-							ntohl(pstClassifierEntry->stSrcIpAddress.ulIpv4Mask[i]);
-						BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Src Ip Mask Address:0x%luX ",
-								pstClassifierEntry->stSrcIpAddress.ulIpv4Mask[i]);
+						st_src_ip->ulIpv4Mask[i] =
+							ntohl(st_src_ip->ulIpv4Mask[i]);
+						BCM_DEBUG_PRINT(Adapter,
+								DBG_TYPE_OTHERS,
+								CONN_MSG,
+								DBG_LVL_ALL,
+								"Src Ip Mask Address:0x%luX ",
+								st_src_ip->ulIpv4Mask[i]);
 					} else if (eIpAddrContext == eDestIpAddress) {
-						pstClassifierEntry->stDestIpAddress.ulIpv4Mask[i] =
-							ntohl(pstClassifierEntry->stDestIpAddress.ulIpv4Mask[i]);
-						BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Dest Ip Mask Address:0x%luX ",
-								pstClassifierEntry->stDestIpAddress.ulIpv4Mask[i]);
+						st_dest_ip->ulIpv4Mask[i] =
+							ntohl(st_dest_ip->ulIpv4Mask[i]);
+						BCM_DEBUG_PRINT(Adapter,
+								DBG_TYPE_OTHERS,
+								CONN_MSG,
+								DBG_LVL_ALL,
+								"Dest Ip Mask Address:0x%luX ",
+								st_dest_ip->ulIpv4Mask[i]);
 					}
 				}
 				u8IpAddressLen -= nSizeOfIPAddressInBytes;
@@ -200,15 +256,10 @@ CopyIpAddrToClassifier(struct bcm_classifier_rule *pstClassifierEntry,
 		}
 		if (bIpVersion6) {
 			/* Restore EndianNess of Struct */
-			for (i = 0; i < MAX_IP_RANGE_LENGTH * 4; i++) {
-				if (eIpAddrContext == eSrcIpAddress) {
-					pstClassifierEntry->stSrcIpAddress.ulIpv6Addr[i] = ntohl(pstClassifierEntry->stSrcIpAddress.ulIpv6Addr[i]);
-					pstClassifierEntry->stSrcIpAddress.ulIpv6Mask[i] = ntohl(pstClassifierEntry->stSrcIpAddress.ulIpv6Mask[i]);
-				} else if (eIpAddrContext == eDestIpAddress) {
-					pstClassifierEntry->stDestIpAddress.ulIpv6Addr[i] = ntohl(pstClassifierEntry->stDestIpAddress.ulIpv6Addr[i]);
-					pstClassifierEntry->stDestIpAddress.ulIpv6Mask[i] = ntohl(pstClassifierEntry->stDestIpAddress.ulIpv6Mask[i]);
-				}
-			}
+			restore_endianess_of_pstClassifierEntry(
+					pstClassifierEntry,
+					eIpAddrContext
+					);
 		}
 	}
 }
@@ -216,16 +267,20 @@ CopyIpAddrToClassifier(struct bcm_classifier_rule *pstClassifierEntry,
 void ClearTargetDSXBuffer(struct bcm_mini_adapter *Adapter, B_UINT16 TID, bool bFreeAll)
 {
 	int i;
+	struct bcm_targetdsx_buffer *curr_buf;
 
 	for (i = 0; i < Adapter->ulTotalTargetBuffersAvailable; i++) {
-		if (Adapter->astTargetDsxBuffer[i].valid)
+		curr_buf = &Adapter->astTargetDsxBuffer[i];
+
+		if (curr_buf->valid)
 			continue;
 
-		if ((bFreeAll) || (Adapter->astTargetDsxBuffer[i].tid == TID)) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "ClearTargetDSXBuffer: found tid %d buffer cleared %lx\n",
-					TID, Adapter->astTargetDsxBuffer[i].ulTargetDsxBuffer);
-			Adapter->astTargetDsxBuffer[i].valid = 1;
-			Adapter->astTargetDsxBuffer[i].tid = 0;
+		if ((bFreeAll) || (curr_buf->tid == TID)) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+					"ClearTargetDSXBuffer: found tid %d buffer cleared %lx\n",
+					TID, curr_buf->ulTargetDsxBuffer);
+			curr_buf->valid = 1;
+			curr_buf->tid = 0;
 			Adapter->ulFreeTargetBufferCnt++;
 		}
 	}
@@ -235,7 +290,10 @@ void ClearTargetDSXBuffer(struct bcm_mini_adapter *Adapter, B_UINT16 TID, bool b
  * @ingroup ctrl_pkt_functions
  * copy classifier rule into the specified SF index
  */
-static inline VOID CopyClassifierRuleToSF(struct bcm_mini_adapter *Adapter, struct bcm_convergence_types *psfCSType, UINT uiSearchRuleIndex, UINT nClassifierIndex)
+static inline VOID CopyClassifierRuleToSF(struct bcm_mini_adapter *Adapter,
+		struct bcm_convergence_types *psfCSType,
+		UINT uiSearchRuleIndex,
+		UINT nClassifierIndex)
 {
 	struct bcm_classifier_rule *pstClassifierEntry = NULL;
 	/* VOID *pvPhsContext = NULL; */
@@ -243,12 +301,16 @@ static inline VOID CopyClassifierRuleToSF(struct bcm_mini_adapter *Adapter, stru
 	/* UCHAR ucProtocolLength=0; */
 	/* ULONG ulPhsStatus; */
 
+	struct bcm_packet_class_rules *pack_class_rule =
+		&psfCSType->cCPacketClassificationRule;
+
 	if (Adapter->PackInfo[uiSearchRuleIndex].usVCID_Value == 0 ||
 		nClassifierIndex > (MAX_CLASSIFIERS-1))
 		return;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Storing Classifier Rule Index : %X",
-			ntohs(psfCSType->cCPacketClassificationRule.u16PacketClassificationRuleIndex));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"Storing Classifier Rule Index : %X",
+			ntohs(pack_class_rule->u16PacketClassificationRuleIndex));
 
 	if (nClassifierIndex > MAX_CLASSIFIERS-1)
 		return;
@@ -256,106 +318,152 @@ static inline VOID CopyClassifierRuleToSF(struct bcm_mini_adapter *Adapter, stru
 	pstClassifierEntry = &Adapter->astClassifierTable[nClassifierIndex];
 	if (pstClassifierEntry) {
 		/* Store if Ipv6 */
-		pstClassifierEntry->bIpv6Protocol = (Adapter->PackInfo[uiSearchRuleIndex].ucIpVersion == IPV6) ? TRUE : false;
+		pstClassifierEntry->bIpv6Protocol =
+			(Adapter->PackInfo[uiSearchRuleIndex].ucIpVersion == IPV6) ? TRUE : false;
 
 		/* Destinaiton Port */
-		pstClassifierEntry->ucDestPortRangeLength = psfCSType->cCPacketClassificationRule.u8ProtocolDestPortRangeLength / 4;
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Destination Port Range Length:0x%X ", pstClassifierEntry->ucDestPortRangeLength);
+		pstClassifierEntry->ucDestPortRangeLength =
+			pack_class_rule->u8ProtocolDestPortRangeLength / 4;
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"Destination Port Range Length:0x%X ",
+				pstClassifierEntry->ucDestPortRangeLength);
 
-		if (psfCSType->cCPacketClassificationRule.u8ProtocolDestPortRangeLength <= MAX_PORT_RANGE) {
+		if (pack_class_rule->u8ProtocolDestPortRangeLength <= MAX_PORT_RANGE) {
 			for (i = 0; i < (pstClassifierEntry->ucDestPortRangeLength); i++) {
-				pstClassifierEntry->usDestPortRangeLo[i] = *((PUSHORT)(psfCSType->cCPacketClassificationRule.u8ProtocolDestPortRange+i));
+				pstClassifierEntry->usDestPortRangeLo[i] =
+					*((PUSHORT)(pack_class_rule->u8ProtocolDestPortRange+i));
 				pstClassifierEntry->usDestPortRangeHi[i] =
-					*((PUSHORT)(psfCSType->cCPacketClassificationRule.u8ProtocolDestPortRange+2+i));
-				pstClassifierEntry->usDestPortRangeLo[i] = ntohs(pstClassifierEntry->usDestPortRangeLo[i]);
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Destination Port Range Lo:0x%X ",
+					*((PUSHORT)(pack_class_rule->u8ProtocolDestPortRange+2+i));
+				pstClassifierEntry->usDestPortRangeLo[i] =
+					ntohs(pstClassifierEntry->usDestPortRangeLo[i]);
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
+						CONN_MSG, DBG_LVL_ALL,
+						"Destination Port Range Lo:0x%X ",
 						pstClassifierEntry->usDestPortRangeLo[i]);
-				pstClassifierEntry->usDestPortRangeHi[i] = ntohs(pstClassifierEntry->usDestPortRangeHi[i]);
+				pstClassifierEntry->usDestPortRangeHi[i] =
+					ntohs(pstClassifierEntry->usDestPortRangeHi[i]);
 			}
 		} else {
 			pstClassifierEntry->ucDestPortRangeLength = 0;
 		}
 
 		/* Source Port */
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Source Port Range Length:0x%X ",
-				psfCSType->cCPacketClassificationRule.u8ProtocolSourcePortRangeLength);
-		if (psfCSType->cCPacketClassificationRule.u8ProtocolSourcePortRangeLength <= MAX_PORT_RANGE) {
-			pstClassifierEntry->ucSrcPortRangeLength = psfCSType->cCPacketClassificationRule.u8ProtocolSourcePortRangeLength/4;
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"Source Port Range Length:0x%X ",
+				pack_class_rule->u8ProtocolSourcePortRangeLength);
+		if (pack_class_rule->u8ProtocolSourcePortRangeLength <= MAX_PORT_RANGE) {
+			pstClassifierEntry->ucSrcPortRangeLength =
+				pack_class_rule->u8ProtocolSourcePortRangeLength/4;
 			for (i = 0; i < (pstClassifierEntry->ucSrcPortRangeLength); i++) {
 				pstClassifierEntry->usSrcPortRangeLo[i] =
-					*((PUSHORT)(psfCSType->cCPacketClassificationRule.
+					*((PUSHORT)(pack_class_rule->
 							u8ProtocolSourcePortRange+i));
 				pstClassifierEntry->usSrcPortRangeHi[i] =
-					*((PUSHORT)(psfCSType->cCPacketClassificationRule.
+					*((PUSHORT)(pack_class_rule->
 							u8ProtocolSourcePortRange+2+i));
 				pstClassifierEntry->usSrcPortRangeLo[i] =
 					ntohs(pstClassifierEntry->usSrcPortRangeLo[i]);
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Source Port Range Lo:0x%X ",
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
+						CONN_MSG, DBG_LVL_ALL,
+						"Source Port Range Lo:0x%X ",
 						pstClassifierEntry->usSrcPortRangeLo[i]);
-				pstClassifierEntry->usSrcPortRangeHi[i] = ntohs(pstClassifierEntry->usSrcPortRangeHi[i]);
+				pstClassifierEntry->usSrcPortRangeHi[i] =
+					ntohs(pstClassifierEntry->usSrcPortRangeHi[i]);
 			}
 		}
 		/* Destination Ip Address and Mask */
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Ip Destination Parameters : ");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"Ip Destination Parameters : ");
 		CopyIpAddrToClassifier(pstClassifierEntry,
-				psfCSType->cCPacketClassificationRule.u8IPDestinationAddressLength,
-				psfCSType->cCPacketClassificationRule.u8IPDestinationAddress,
+				pack_class_rule->u8IPDestinationAddressLength,
+				pack_class_rule->u8IPDestinationAddress,
 				(Adapter->PackInfo[uiSearchRuleIndex].ucIpVersion == IPV6) ?
 			TRUE : false, eDestIpAddress);
 
 		/* Source Ip Address and Mask */
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Ip Source Parameters : ");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"Ip Source Parameters : ");
 
 		CopyIpAddrToClassifier(pstClassifierEntry,
-				psfCSType->cCPacketClassificationRule.u8IPMaskedSourceAddressLength,
-				psfCSType->cCPacketClassificationRule.u8IPMaskedSourceAddress,
+				pack_class_rule->u8IPMaskedSourceAddressLength,
+				pack_class_rule->u8IPMaskedSourceAddress,
 				(Adapter->PackInfo[uiSearchRuleIndex].ucIpVersion == IPV6) ? TRUE : false,
 				eSrcIpAddress);
 
 		/* TOS */
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "TOS Length:0x%X ", psfCSType->cCPacketClassificationRule.u8IPTypeOfServiceLength);
-		if (psfCSType->cCPacketClassificationRule.u8IPTypeOfServiceLength == 3) {
-			pstClassifierEntry->ucIPTypeOfServiceLength = psfCSType->cCPacketClassificationRule.u8IPTypeOfServiceLength;
-			pstClassifierEntry->ucTosLow = psfCSType->cCPacketClassificationRule.u8IPTypeOfService[0];
-			pstClassifierEntry->ucTosHigh = psfCSType->cCPacketClassificationRule.u8IPTypeOfService[1];
-			pstClassifierEntry->ucTosMask = psfCSType->cCPacketClassificationRule.u8IPTypeOfService[2];
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"TOS Length:0x%X ",
+				pack_class_rule->u8IPTypeOfServiceLength);
+		if (pack_class_rule->u8IPTypeOfServiceLength == 3) {
+			pstClassifierEntry->ucIPTypeOfServiceLength =
+				pack_class_rule->u8IPTypeOfServiceLength;
+			pstClassifierEntry->ucTosLow =
+				pack_class_rule->u8IPTypeOfService[0];
+			pstClassifierEntry->ucTosHigh =
+				pack_class_rule->u8IPTypeOfService[1];
+			pstClassifierEntry->ucTosMask =
+				pack_class_rule->u8IPTypeOfService[2];
 			pstClassifierEntry->bTOSValid = TRUE;
 		}
-		if (psfCSType->cCPacketClassificationRule.u8Protocol == 0) {
+		if (pack_class_rule->u8Protocol == 0) {
 			/* we didn't get protocol field filled in by the BS */
 			pstClassifierEntry->ucProtocolLength = 0;
 		} else {
 			pstClassifierEntry->ucProtocolLength = 1; /* 1 valid protocol */
 		}
 
-		pstClassifierEntry->ucProtocol[0] = psfCSType->cCPacketClassificationRule.u8Protocol;
-		pstClassifierEntry->u8ClassifierRulePriority = psfCSType->cCPacketClassificationRule.u8ClassifierRulePriority;
+		pstClassifierEntry->ucProtocol[0] = pack_class_rule->u8Protocol;
+		pstClassifierEntry->u8ClassifierRulePriority =
+			pack_class_rule->u8ClassifierRulePriority;
 
 		/* store the classifier rule ID and set this classifier entry as valid */
-		pstClassifierEntry->ucDirection = Adapter->PackInfo[uiSearchRuleIndex].ucDirection;
-		pstClassifierEntry->uiClassifierRuleIndex = ntohs(psfCSType->cCPacketClassificationRule.u16PacketClassificationRuleIndex);
-		pstClassifierEntry->usVCID_Value = Adapter->PackInfo[uiSearchRuleIndex].usVCID_Value;
-		pstClassifierEntry->ulSFID = Adapter->PackInfo[uiSearchRuleIndex].ulSFID;
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Search Index %d Dir: %d, Index: %d, Vcid: %d\n",
-				uiSearchRuleIndex, pstClassifierEntry->ucDirection,
+		pstClassifierEntry->ucDirection =
+			Adapter->PackInfo[uiSearchRuleIndex].ucDirection;
+		pstClassifierEntry->uiClassifierRuleIndex =
+			ntohs(pack_class_rule->u16PacketClassificationRuleIndex);
+		pstClassifierEntry->usVCID_Value =
+			Adapter->PackInfo[uiSearchRuleIndex].usVCID_Value;
+		pstClassifierEntry->ulSFID =
+			Adapter->PackInfo[uiSearchRuleIndex].ulSFID;
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"Search Index %d Dir: %d, Index: %d, Vcid: %d\n",
+				uiSearchRuleIndex,
+				pstClassifierEntry->ucDirection,
 				pstClassifierEntry->uiClassifierRuleIndex,
 				pstClassifierEntry->usVCID_Value);
 
-		if (psfCSType->cCPacketClassificationRule.u8AssociatedPHSI)
-			pstClassifierEntry->u8AssociatedPHSI = psfCSType->cCPacketClassificationRule.u8AssociatedPHSI;
+		if (pack_class_rule->u8AssociatedPHSI)
+			pstClassifierEntry->u8AssociatedPHSI =
+				pack_class_rule->u8AssociatedPHSI;
 
 		/* Copy ETH CS Parameters */
-		pstClassifierEntry->ucEthCSSrcMACLen = (psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddressLength);
-		memcpy(pstClassifierEntry->au8EThCSSrcMAC, psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddress, MAC_ADDRESS_SIZE);
-		memcpy(pstClassifierEntry->au8EThCSSrcMACMask, psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddress + MAC_ADDRESS_SIZE, MAC_ADDRESS_SIZE);
-		pstClassifierEntry->ucEthCSDestMACLen = (psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddressLength);
-		memcpy(pstClassifierEntry->au8EThCSDestMAC, psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddress, MAC_ADDRESS_SIZE);
-		memcpy(pstClassifierEntry->au8EThCSDestMACMask, psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddress + MAC_ADDRESS_SIZE, MAC_ADDRESS_SIZE);
-		pstClassifierEntry->ucEtherTypeLen = (psfCSType->cCPacketClassificationRule.u8EthertypeLength);
-		memcpy(pstClassifierEntry->au8EthCSEtherType, psfCSType->cCPacketClassificationRule.u8Ethertype, NUM_ETHERTYPE_BYTES);
-		memcpy(pstClassifierEntry->usUserPriority, &psfCSType->cCPacketClassificationRule.u16UserPriority, 2);
-		pstClassifierEntry->usVLANID = ntohs(psfCSType->cCPacketClassificationRule.u16VLANID);
-		pstClassifierEntry->usValidityBitMap = ntohs(psfCSType->cCPacketClassificationRule.u16ValidityBitMap);
+		pstClassifierEntry->ucEthCSSrcMACLen =
+			(pack_class_rule->u8EthernetSourceMACAddressLength);
+		memcpy(pstClassifierEntry->au8EThCSSrcMAC,
+				pack_class_rule->u8EthernetSourceMACAddress,
+				MAC_ADDRESS_SIZE);
+		memcpy(pstClassifierEntry->au8EThCSSrcMACMask,
+				pack_class_rule->u8EthernetSourceMACAddress
+				+ MAC_ADDRESS_SIZE, MAC_ADDRESS_SIZE);
+		pstClassifierEntry->ucEthCSDestMACLen =
+			(pack_class_rule->u8EthernetDestMacAddressLength);
+		memcpy(pstClassifierEntry->au8EThCSDestMAC,
+				pack_class_rule->u8EthernetDestMacAddress,
+				MAC_ADDRESS_SIZE);
+		memcpy(pstClassifierEntry->au8EThCSDestMACMask,
+				pack_class_rule->u8EthernetDestMacAddress
+				+ MAC_ADDRESS_SIZE, MAC_ADDRESS_SIZE);
+		pstClassifierEntry->ucEtherTypeLen =
+			(pack_class_rule->u8EthertypeLength);
+		memcpy(pstClassifierEntry->au8EthCSEtherType,
+				pack_class_rule->u8Ethertype,
+				NUM_ETHERTYPE_BYTES);
+		memcpy(pstClassifierEntry->usUserPriority,
+				&pack_class_rule->u16UserPriority, 2);
+		pstClassifierEntry->usVLANID =
+			ntohs(pack_class_rule->u16VLANID);
+		pstClassifierEntry->usValidityBitMap =
+			ntohs(pack_class_rule->u16ValidityBitMap);
 
 		pstClassifierEntry->bUsed = TRUE;
 	}
@@ -364,7 +472,8 @@ static inline VOID CopyClassifierRuleToSF(struct bcm_mini_adapter *Adapter, stru
 /*
  * @ingroup ctrl_pkt_functions
  */
-static inline VOID DeleteClassifierRuleFromSF(struct bcm_mini_adapter *Adapter, UINT uiSearchRuleIndex, UINT nClassifierIndex)
+static inline VOID DeleteClassifierRuleFromSF(struct bcm_mini_adapter *Adapter,
+		UINT uiSearchRuleIndex, UINT nClassifierIndex)
 {
 	struct bcm_classifier_rule *pstClassifierEntry = NULL;
 	B_UINT16 u16PacketClassificationRuleIndex;
@@ -380,22 +489,26 @@ static inline VOID DeleteClassifierRuleFromSF(struct bcm_mini_adapter *Adapter,
 	if (usVCID == 0)
 		return;
 
-	u16PacketClassificationRuleIndex = Adapter->astClassifierTable[nClassifierIndex].uiClassifierRuleIndex;
+	u16PacketClassificationRuleIndex =
+		Adapter->astClassifierTable[nClassifierIndex].uiClassifierRuleIndex;
 	pstClassifierEntry = &Adapter->astClassifierTable[nClassifierIndex];
 	if (pstClassifierEntry) {
 		pstClassifierEntry->bUsed = false;
 		pstClassifierEntry->uiClassifierRuleIndex = 0;
-		memset(pstClassifierEntry, 0, sizeof(struct bcm_classifier_rule));
+		memset(pstClassifierEntry, 0,
+				sizeof(struct bcm_classifier_rule));
 
 		/* Delete the PHS Rule for this classifier */
-		PhsDeleteClassifierRule(&Adapter->stBCMPhsContext, usVCID, u16PacketClassificationRuleIndex);
+		PhsDeleteClassifierRule(&Adapter->stBCMPhsContext, usVCID,
+				u16PacketClassificationRuleIndex);
 	}
 }
 
 /*
  * @ingroup ctrl_pkt_functions
  */
-VOID DeleteAllClassifiersForSF(struct bcm_mini_adapter *Adapter, UINT uiSearchRuleIndex)
+VOID DeleteAllClassifiersForSF(struct bcm_mini_adapter *Adapter,
+		UINT uiSearchRuleIndex)
 {
 	struct bcm_classifier_rule *pstClassifierEntry = NULL;
 	int i;
@@ -414,7 +527,8 @@ VOID DeleteAllClassifiersForSF(struct bcm_mini_adapter *Adapter, UINT uiSearchRu
 			pstClassifierEntry = &Adapter->astClassifierTable[i];
 
 			if (pstClassifierEntry->bUsed)
-				DeleteClassifierRuleFromSF(Adapter, uiSearchRuleIndex, i);
+				DeleteClassifierRuleFromSF(Adapter,
+						uiSearchRuleIndex, i);
 		}
 	}
 
@@ -441,324 +555,262 @@ static VOID CopyToAdapter(register struct bcm_mini_adapter *Adapter, /* <Pointer
 	int i;
 	struct bcm_convergence_types *psfCSType = NULL;
 	struct bcm_phs_rule sPhsRule;
-	USHORT uVCID = Adapter->PackInfo[uiSearchRuleIndex].usVCID_Value;
+	struct bcm_packet_info *curr_packinfo =
+		&Adapter->PackInfo[uiSearchRuleIndex];
+	USHORT uVCID = curr_packinfo->usVCID_Value;
 	UINT UGIValue = 0;
 
-	Adapter->PackInfo[uiSearchRuleIndex].bValid = TRUE;
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Search Rule Index = %d\n", uiSearchRuleIndex);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "%s: SFID= %x ", __func__, ntohl(psfLocalSet->u32SFID));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Updating Queue %d", uiSearchRuleIndex);
+	curr_packinfo->bValid = TRUE;
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"Search Rule Index = %d\n", uiSearchRuleIndex);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"%s: SFID= %x ", __func__, ntohl(psfLocalSet->u32SFID));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"Updating Queue %d", uiSearchRuleIndex);
 
 	ulSFID = ntohl(psfLocalSet->u32SFID);
 	/* Store IP Version used */
 	/* Get The Version Of IP used (IPv6 or IPv4) from CSSpecification field of SF */
 
-	Adapter->PackInfo[uiSearchRuleIndex].bIPCSSupport = 0;
-	Adapter->PackInfo[uiSearchRuleIndex].bEthCSSupport = 0;
+	curr_packinfo->bIPCSSupport = 0;
+	curr_packinfo->bEthCSSupport = 0;
 
 	/* Enable IP/ETh CS Support As Required */
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "CopyToAdapter : u8CSSpecification : %X\n", psfLocalSet->u8CSSpecification);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"CopyToAdapter : u8CSSpecification : %X\n",
+			psfLocalSet->u8CSSpecification);
 	switch (psfLocalSet->u8CSSpecification) {
 	case eCSPacketIPV4:
-	{
-		Adapter->PackInfo[uiSearchRuleIndex].bIPCSSupport = IPV4_CS;
+		curr_packinfo->bIPCSSupport = IPV4_CS;
 		break;
-	}
 	case eCSPacketIPV6:
-	{
-		Adapter->PackInfo[uiSearchRuleIndex].bIPCSSupport = IPV6_CS;
+		curr_packinfo->bIPCSSupport = IPV6_CS;
 		break;
-	}
 	case eCS802_3PacketEthernet:
 	case eCS802_1QPacketVLAN:
-	{
-		Adapter->PackInfo[uiSearchRuleIndex].bEthCSSupport = ETH_CS_802_3;
+		curr_packinfo->bEthCSSupport = ETH_CS_802_3;
 		break;
-	}
 	case eCSPacketIPV4Over802_1QVLAN:
 	case eCSPacketIPV4Over802_3Ethernet:
-	{
-		Adapter->PackInfo[uiSearchRuleIndex].bIPCSSupport = IPV4_CS;
-		Adapter->PackInfo[uiSearchRuleIndex].bEthCSSupport = ETH_CS_802_3;
+		curr_packinfo->bIPCSSupport = IPV4_CS;
+		curr_packinfo->bEthCSSupport = ETH_CS_802_3;
 		break;
-	}
 	case eCSPacketIPV6Over802_1QVLAN:
 	case eCSPacketIPV6Over802_3Ethernet:
-	{
-		Adapter->PackInfo[uiSearchRuleIndex].bIPCSSupport = IPV6_CS;
-		Adapter->PackInfo[uiSearchRuleIndex].bEthCSSupport = ETH_CS_802_3;
+		curr_packinfo->bIPCSSupport = IPV6_CS;
+		curr_packinfo->bEthCSSupport = ETH_CS_802_3;
 		break;
-	}
 	default:
-	{
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Error in value of CS Classification.. setting default to IP CS\n");
-		Adapter->PackInfo[uiSearchRuleIndex].bIPCSSupport = IPV4_CS;
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"Error in value of CS Classification.. setting default to IP CS\n");
+		curr_packinfo->bIPCSSupport = IPV4_CS;
 		break;
 	}
-	}
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "CopyToAdapter : Queue No : %X ETH CS Support :  %X  , IP CS Support : %X\n",
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"CopyToAdapter : Queue No : %X ETH CS Support :  %X  , IP CS Support : %X\n",
 			uiSearchRuleIndex,
-			Adapter->PackInfo[uiSearchRuleIndex].bEthCSSupport,
-			Adapter->PackInfo[uiSearchRuleIndex].bIPCSSupport);
+			curr_packinfo->bEthCSSupport,
+			curr_packinfo->bIPCSSupport);
 
 	/* Store IP Version used */
 	/* Get The Version Of IP used (IPv6 or IPv4) from CSSpecification field of SF */
-	if (Adapter->PackInfo[uiSearchRuleIndex].bIPCSSupport == IPV6_CS)
-		Adapter->PackInfo[uiSearchRuleIndex].ucIpVersion = IPV6;
+	if (curr_packinfo->bIPCSSupport == IPV6_CS)
+		curr_packinfo->ucIpVersion = IPV6;
 	else
-		Adapter->PackInfo[uiSearchRuleIndex].ucIpVersion = IPV4;
+		curr_packinfo->ucIpVersion = IPV4;
 
 	/* To ensure that the ETH CS code doesn't gets executed if the BS doesn't supports ETH CS */
 	if (!Adapter->bETHCSEnabled)
-		Adapter->PackInfo[uiSearchRuleIndex].bEthCSSupport = 0;
+		curr_packinfo->bEthCSSupport = 0;
 
 	if (psfLocalSet->u8ServiceClassNameLength > 0 && psfLocalSet->u8ServiceClassNameLength < 32)
-		memcpy(Adapter->PackInfo[uiSearchRuleIndex].ucServiceClassName,	psfLocalSet->u8ServiceClassName, psfLocalSet->u8ServiceClassNameLength);
+		memcpy(curr_packinfo->ucServiceClassName,
+				psfLocalSet->u8ServiceClassName,
+				psfLocalSet->u8ServiceClassNameLength);
 
-	Adapter->PackInfo[uiSearchRuleIndex].u8QueueType = psfLocalSet->u8ServiceFlowSchedulingType;
+	curr_packinfo->u8QueueType = psfLocalSet->u8ServiceFlowSchedulingType;
 
-	if (Adapter->PackInfo[uiSearchRuleIndex].u8QueueType == BE && Adapter->PackInfo[uiSearchRuleIndex].ucDirection)
+	if (curr_packinfo->u8QueueType == BE && curr_packinfo->ucDirection)
 		Adapter->usBestEffortQueueIndex = uiSearchRuleIndex;
 
-	Adapter->PackInfo[uiSearchRuleIndex].ulSFID = ntohl(psfLocalSet->u32SFID);
+	curr_packinfo->ulSFID = ntohl(psfLocalSet->u32SFID);
 
-	Adapter->PackInfo[uiSearchRuleIndex].u8TrafficPriority = psfLocalSet->u8TrafficPriority;
+	curr_packinfo->u8TrafficPriority = psfLocalSet->u8TrafficPriority;
 
 	/* copy all the classifier in the Service Flow param  structure */
 	for (i = 0; i < psfLocalSet->u8TotalClassifiers; i++) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Classifier index =%d", i);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"Classifier index =%d", i);
 		psfCSType = &psfLocalSet->cConvergenceSLTypes[i];
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Classifier index =%d", i);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"Classifier index =%d", i);
 
 		if (psfCSType->cCPacketClassificationRule.u8ClassifierRulePriority)
-			Adapter->PackInfo[uiSearchRuleIndex].bClassifierPriority = TRUE;
+			curr_packinfo->bClassifierPriority = TRUE;
 
 		if (psfCSType->cCPacketClassificationRule.u8ClassifierRulePriority)
-			Adapter->PackInfo[uiSearchRuleIndex].bClassifierPriority = TRUE;
+			curr_packinfo->bClassifierPriority = TRUE;
 
 		if (ucDsxType == DSA_ACK) {
 			eClassifierAction = eAddClassifier;
 		} else if (ucDsxType == DSC_ACK) {
 			switch (psfCSType->u8ClassfierDSCAction) {
 			case 0: /* DSC Add Classifier */
-			{
 				eClassifierAction = eAddClassifier;
-			}
-			break;
+				break;
 			case 1: /* DSC Replace Classifier */
-			{
 				eClassifierAction = eReplaceClassifier;
-			}
-			break;
+				break;
 			case 2: /* DSC Delete Classifier */
-			{
 				eClassifierAction = eDeleteClassifier;
-			}
-			break;
+				break;
 			default:
-			{
 				eClassifierAction = eInvalidClassifierAction;
 			}
-			}
 		}
 
 		u16PacketClassificationRuleIndex = ntohs(psfCSType->cCPacketClassificationRule.u16PacketClassificationRuleIndex);
 
 		switch (eClassifierAction) {
 		case eAddClassifier:
-		{
 			/* Get a Free Classifier Index From Classifier table for this SF to add the Classifier */
 			/* Contained in this message */
-			nClassifierIndex = SearchClsid(Adapter, ulSFID, u16PacketClassificationRuleIndex);
+			nClassifierIndex = SearchClsid(Adapter,
+					ulSFID,
+					u16PacketClassificationRuleIndex);
 
 			if (nClassifierIndex > MAX_CLASSIFIERS) {
 				nClassifierIndex = SearchFreeClsid(Adapter);
 				if (nClassifierIndex > MAX_CLASSIFIERS) {
 					/* Failed To get a free Entry */
-					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Error Failed To get a free Classifier Entry");
+					BCM_DEBUG_PRINT(Adapter,
+							DBG_TYPE_OTHERS,
+							CONN_MSG,
+							DBG_LVL_ALL,
+							"Error Failed To get a free Classifier Entry");
 					break;
 				}
 				/* Copy the Classifier Rule for this service flow into our Classifier table maintained per SF. */
-				CopyClassifierRuleToSF(Adapter, psfCSType, uiSearchRuleIndex, nClassifierIndex);
+				CopyClassifierRuleToSF(Adapter, psfCSType,
+						uiSearchRuleIndex,
+						nClassifierIndex);
 			} else {
 				/* This Classifier Already Exists and it is invalid to Add Classifier with existing PCRI */
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
+						CONN_MSG,
+						DBG_LVL_ALL,
 						"CopyToAdapter: Error The Specified Classifier Already Exists and attempted To Add Classifier with Same PCRI : 0x%x\n",
 						u16PacketClassificationRuleIndex);
 			}
-		}
-		break;
+			break;
 		case eReplaceClassifier:
-		{
 			/* Get the Classifier Index From Classifier table for this SF and replace existing  Classifier */
 			/* with the new classifier Contained in this message */
-			nClassifierIndex = SearchClsid(Adapter, ulSFID, u16PacketClassificationRuleIndex);
+			nClassifierIndex = SearchClsid(Adapter, ulSFID,
+					u16PacketClassificationRuleIndex);
 			if (nClassifierIndex > MAX_CLASSIFIERS) {
 				/* Failed To search the classifier */
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Error Search for Classifier To be replaced failed");
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
+						CONN_MSG, DBG_LVL_ALL,
+						"Error Search for Classifier To be replaced failed");
 				break;
 			}
 			/* Copy the Classifier Rule for this service flow into our Classifier table maintained per SF. */
-			CopyClassifierRuleToSF(Adapter, psfCSType, uiSearchRuleIndex, nClassifierIndex);
-		}
-		break;
+			CopyClassifierRuleToSF(Adapter, psfCSType,
+					uiSearchRuleIndex, nClassifierIndex);
+			break;
 		case eDeleteClassifier:
-		{
 			/* Get the Classifier Index From Classifier table for this SF and replace existing  Classifier */
 			/* with the new classifier Contained in this message */
-			nClassifierIndex = SearchClsid(Adapter, ulSFID, u16PacketClassificationRuleIndex);
+			nClassifierIndex = SearchClsid(Adapter, ulSFID,
+					u16PacketClassificationRuleIndex);
 			if (nClassifierIndex > MAX_CLASSIFIERS)	{
 				/* Failed To search the classifier */
-				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Error Search for Classifier To be deleted failed");
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
+						CONN_MSG, DBG_LVL_ALL,
+						"Error Search for Classifier To be deleted failed");
 				break;
 			}
 
 			/* Delete This classifier */
-			DeleteClassifierRuleFromSF(Adapter, uiSearchRuleIndex, nClassifierIndex);
-		}
-		break;
+			DeleteClassifierRuleFromSF(Adapter, uiSearchRuleIndex,
+					nClassifierIndex);
+			break;
 		default:
-		{
 			/* Invalid Action for classifier */
 			break;
 		}
-		}
 	}
 
 	/* Repeat parsing Classification Entries to process PHS Rules */
 	for (i = 0; i < psfLocalSet->u8TotalClassifiers; i++) {
 		psfCSType = &psfLocalSet->cConvergenceSLTypes[i];
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "psfCSType->u8PhsDSCAction : 0x%x\n", psfCSType->u8PhsDSCAction);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"psfCSType->u8PhsDSCAction : 0x%x\n",
+				psfCSType->u8PhsDSCAction);
 
 		switch (psfCSType->u8PhsDSCAction) {
 		case eDeleteAllPHSRules:
-		{
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Deleting All PHS Rules For VCID: 0x%X\n", uVCID);
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG,
+					DBG_LVL_ALL,
+					"Deleting All PHS Rules For VCID: 0x%X\n",
+					uVCID);
 
 			/* Delete All the PHS rules for this Service flow */
 			PhsDeleteSFRules(&Adapter->stBCMPhsContext, uVCID);
 			break;
-		}
 		case eDeletePHSRule:
-		{
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "PHS DSC Action = Delete PHS Rule\n");
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG,
+					DBG_LVL_ALL,
+					"PHS DSC Action = Delete PHS Rule\n");
 
 			if (psfCSType->cPhsRule.u8PHSI)
-				PhsDeletePHSRule(&Adapter->stBCMPhsContext, uVCID, psfCSType->cCPacketClassificationRule.u8AssociatedPHSI);
+				PhsDeletePHSRule(&Adapter->stBCMPhsContext,
+						uVCID,
+						psfCSType->cCPacketClassificationRule.u8AssociatedPHSI);
 
 			break;
-		}
 		default:
-		{
 			if (ucDsxType == DSC_ACK) {
 				/* BCM_DEBUG_PRINT(CONN_MSG,("Invalid PHS DSC Action For DSC\n",psfCSType->cPhsRule.u8PHSI)); */
 				break; /* FOr DSC ACK Case PHS DSC Action must be in valid set */
 			}
-		}
 		/* Proceed To Add PHS rule for DSA_ACK case even if PHS DSC action is unspecified */
 		/* No Break Here . Intentionally! */
 
 		case eAddPHSRule:
 		case eSetPHSRule:
-		{
 			if (psfCSType->cPhsRule.u8PHSI)	{
 				/* Apply This PHS Rule to all classifiers whose Associated PHSI Match */
-				unsigned int uiClassifierIndex = 0;
-				if (pstAddIndication->u8Direction == UPLINK_DIR) {
-					for (uiClassifierIndex = 0; uiClassifierIndex < MAX_CLASSIFIERS; uiClassifierIndex++) {
-						if ((Adapter->astClassifierTable[uiClassifierIndex].bUsed) &&
-							(Adapter->astClassifierTable[uiClassifierIndex].ulSFID == Adapter->PackInfo[uiSearchRuleIndex].ulSFID) &&
-							(Adapter->astClassifierTable[uiClassifierIndex].u8AssociatedPHSI == psfCSType->cPhsRule.u8PHSI)) {
-							BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
-									"Adding PHS Rule For Classifier: 0x%x cPhsRule.u8PHSI: 0x%x\n",
-									Adapter->astClassifierTable[uiClassifierIndex].uiClassifierRuleIndex,
-									psfCSType->cPhsRule.u8PHSI);
-							/* Update The PHS Rule for this classifier as Associated PHSI id defined */
-
-							/* Copy the PHS Rule */
-							sPhsRule.u8PHSI = psfCSType->cPhsRule.u8PHSI;
-							sPhsRule.u8PHSFLength = psfCSType->cPhsRule.u8PHSFLength;
-							sPhsRule.u8PHSMLength = psfCSType->cPhsRule.u8PHSMLength;
-							sPhsRule.u8PHSS = psfCSType->cPhsRule.u8PHSS;
-							sPhsRule.u8PHSV = psfCSType->cPhsRule.u8PHSV;
-							memcpy(sPhsRule.u8PHSF, psfCSType->cPhsRule.u8PHSF, MAX_PHS_LENGTHS);
-							memcpy(sPhsRule.u8PHSM, psfCSType->cPhsRule.u8PHSM, MAX_PHS_LENGTHS);
-							sPhsRule.u8RefCnt = 0;
-							sPhsRule.bUnclassifiedPHSRule = false;
-							sPhsRule.PHSModifiedBytes = 0;
-							sPhsRule.PHSModifiedNumPackets = 0;
-							sPhsRule.PHSErrorNumPackets = 0;
-
-							/* bPHSRuleAssociated = TRUE; */
-							/* Store The PHS Rule for this classifier */
-
-							PhsUpdateClassifierRule(
-								&Adapter->stBCMPhsContext,
-								uVCID,
-								Adapter->astClassifierTable[uiClassifierIndex].uiClassifierRuleIndex,
-								&sPhsRule,
-								Adapter->astClassifierTable[uiClassifierIndex].u8AssociatedPHSI);
-
-							/* Update PHS Rule For the Classifier */
-							if (sPhsRule.u8PHSI) {
-								Adapter->astClassifierTable[uiClassifierIndex].u32PHSRuleID = sPhsRule.u8PHSI;
-								memcpy(&Adapter->astClassifierTable[uiClassifierIndex].sPhsRule, &sPhsRule, sizeof(struct bcm_phs_rule));
-							}
-						}
-					}
-				} else {
-					/* Error PHS Rule specified in signaling could not be applied to any classifier */
-
-					/* Copy the PHS Rule */
-					sPhsRule.u8PHSI = psfCSType->cPhsRule.u8PHSI;
-					sPhsRule.u8PHSFLength = psfCSType->cPhsRule.u8PHSFLength;
-					sPhsRule.u8PHSMLength = psfCSType->cPhsRule.u8PHSMLength;
-					sPhsRule.u8PHSS = psfCSType->cPhsRule.u8PHSS;
-					sPhsRule.u8PHSV = psfCSType->cPhsRule.u8PHSV;
-					memcpy(sPhsRule.u8PHSF, psfCSType->cPhsRule.u8PHSF, MAX_PHS_LENGTHS);
-					memcpy(sPhsRule.u8PHSM, psfCSType->cPhsRule.u8PHSM, MAX_PHS_LENGTHS);
-					sPhsRule.u8RefCnt = 0;
-					sPhsRule.bUnclassifiedPHSRule = TRUE;
-					sPhsRule.PHSModifiedBytes = 0;
-					sPhsRule.PHSModifiedNumPackets = 0;
-					sPhsRule.PHSErrorNumPackets = 0;
-					/* Store The PHS Rule for this classifier */
-
-					/*
-					 * Passing the argument u8PHSI instead of clsid. Because for DL with no classifier rule,
-					 * clsid will be zero hence we can't have multiple PHS rules for the same SF.
-					 * To support multiple PHS rule, passing u8PHSI.
-					 */
-					PhsUpdateClassifierRule(
-						&Adapter->stBCMPhsContext,
+				apply_phs_rule_to_all_classifiers(Adapter,
+						uiSearchRuleIndex,
 						uVCID,
-						sPhsRule.u8PHSI,
 						&sPhsRule,
-						sPhsRule.u8PHSI);
-				}
+						&psfCSType->cPhsRule,
+						pstAddIndication);
 			}
-		}
-		break;
+			break;
 		}
 	}
 
 	if (psfLocalSet->u32MaxSustainedTrafficRate == 0) {
 		/* No Rate Limit . Set Max Sustained Traffic Rate to Maximum */
-		Adapter->PackInfo[uiSearchRuleIndex].uiMaxAllowedRate = WIMAX_MAX_ALLOWED_RATE;
+		curr_packinfo->uiMaxAllowedRate = WIMAX_MAX_ALLOWED_RATE;
 	} else if (ntohl(psfLocalSet->u32MaxSustainedTrafficRate) > WIMAX_MAX_ALLOWED_RATE) {
 		/* Too large Allowed Rate specified. Limiting to Wi Max  Allowed rate */
-		Adapter->PackInfo[uiSearchRuleIndex].uiMaxAllowedRate = WIMAX_MAX_ALLOWED_RATE;
+		curr_packinfo->uiMaxAllowedRate = WIMAX_MAX_ALLOWED_RATE;
 	} else {
-		Adapter->PackInfo[uiSearchRuleIndex].uiMaxAllowedRate =  ntohl(psfLocalSet->u32MaxSustainedTrafficRate);
+		curr_packinfo->uiMaxAllowedRate =
+			ntohl(psfLocalSet->u32MaxSustainedTrafficRate);
 	}
 
-	Adapter->PackInfo[uiSearchRuleIndex].uiMaxLatency = ntohl(psfLocalSet->u32MaximumLatency);
-	if (Adapter->PackInfo[uiSearchRuleIndex].uiMaxLatency == 0) /* 0 should be treated as infinite */
-		Adapter->PackInfo[uiSearchRuleIndex].uiMaxLatency = MAX_LATENCY_ALLOWED;
+	curr_packinfo->uiMaxLatency = ntohl(psfLocalSet->u32MaximumLatency);
+	if (curr_packinfo->uiMaxLatency == 0) /* 0 should be treated as infinite */
+		curr_packinfo->uiMaxLatency = MAX_LATENCY_ALLOWED;
 
-	if ((Adapter->PackInfo[uiSearchRuleIndex].u8QueueType == ERTPS ||
-			Adapter->PackInfo[uiSearchRuleIndex].u8QueueType == UGS))
+	if ((curr_packinfo->u8QueueType == ERTPS ||
+			curr_packinfo->u8QueueType == UGS))
 		UGIValue = ntohs(psfLocalSet->u16UnsolicitedGrantInterval);
 
 	if (UGIValue == 0)
@@ -770,42 +822,45 @@ static VOID CopyToAdapter(register struct bcm_mini_adapter *Adapter, /* <Pointer
 	 * The extra amount of token is to ensure that a large amount of jitter won't have loss in throughput...
 	 * In case of non-UGI based connection, 200 frames worth of data is the max token count at host...
 	 */
-	Adapter->PackInfo[uiSearchRuleIndex].uiMaxBucketSize =
-		(DEFAULT_UGI_FACTOR*Adapter->PackInfo[uiSearchRuleIndex].uiMaxAllowedRate*UGIValue)/1000;
+	curr_packinfo->uiMaxBucketSize =
+		(DEFAULT_UGI_FACTOR*curr_packinfo->uiMaxAllowedRate*UGIValue)/1000;
 
-	if (Adapter->PackInfo[uiSearchRuleIndex].uiMaxBucketSize < WIMAX_MAX_MTU*8) {
+	if (curr_packinfo->uiMaxBucketSize < WIMAX_MAX_MTU*8) {
 		UINT UGIFactor = 0;
 		/* Special Handling to ensure the biggest size of packet can go out from host to FW as follows:
 		 * 1. Any packet from Host to FW can go out in different packet size.
 		 * 2. So in case the Bucket count is smaller than MTU, the packets of size (Size > TokenCount), will get dropped.
 		 * 3. We can allow packets of MaxSize from Host->FW that can go out from FW in multiple SDUs by fragmentation at Wimax Layer
 		 */
-		UGIFactor = (Adapter->PackInfo[uiSearchRuleIndex].uiMaxLatency/UGIValue + 1);
+		UGIFactor = (curr_packinfo->uiMaxLatency/UGIValue + 1);
 
 		if (UGIFactor > DEFAULT_UGI_FACTOR)
-			Adapter->PackInfo[uiSearchRuleIndex].uiMaxBucketSize =
-				(UGIFactor*Adapter->PackInfo[uiSearchRuleIndex].uiMaxAllowedRate*UGIValue)/1000;
+			curr_packinfo->uiMaxBucketSize =
+				(UGIFactor*curr_packinfo->uiMaxAllowedRate*UGIValue)/1000;
 
-		if (Adapter->PackInfo[uiSearchRuleIndex].uiMaxBucketSize > WIMAX_MAX_MTU*8)
-			Adapter->PackInfo[uiSearchRuleIndex].uiMaxBucketSize = WIMAX_MAX_MTU*8;
+		if (curr_packinfo->uiMaxBucketSize > WIMAX_MAX_MTU*8)
+			curr_packinfo->uiMaxBucketSize = WIMAX_MAX_MTU*8;
 	}
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "LAT: %d, UGI: %d\n", Adapter->PackInfo[uiSearchRuleIndex].uiMaxLatency, UGIValue);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "uiMaxAllowedRate: 0x%x, u32MaxSustainedTrafficRate: 0x%x ,uiMaxBucketSize: 0x%x",
-			Adapter->PackInfo[uiSearchRuleIndex].uiMaxAllowedRate,
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"LAT: %d, UGI: %d\n", curr_packinfo->uiMaxLatency,
+			UGIValue);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"uiMaxAllowedRate: 0x%x, u32MaxSustainedTrafficRate: 0x%x ,uiMaxBucketSize: 0x%x",
+			curr_packinfo->uiMaxAllowedRate,
 			ntohl(psfLocalSet->u32MaxSustainedTrafficRate),
-			Adapter->PackInfo[uiSearchRuleIndex].uiMaxBucketSize);
+			curr_packinfo->uiMaxBucketSize);
 
 	/* copy the extended SF Parameters to Support MIBS */
 	CopyMIBSExtendedSFParameters(Adapter, psfLocalSet, uiSearchRuleIndex);
 
 	/* store header suppression enabled flag per SF */
-	Adapter->PackInfo[uiSearchRuleIndex].bHeaderSuppressionEnabled =
+	curr_packinfo->bHeaderSuppressionEnabled =
 		!(psfLocalSet->u8RequesttransmissionPolicy &
 			MASK_DISABLE_HEADER_SUPPRESSION);
 
-	kfree(Adapter->PackInfo[uiSearchRuleIndex].pstSFIndication);
-	Adapter->PackInfo[uiSearchRuleIndex].pstSFIndication = pstAddIndication;
+	kfree(curr_packinfo->pstSFIndication);
+	curr_packinfo->pstSFIndication = pstAddIndication;
 
 	/* Re Sort the SF list in PackInfo according to Traffic Priority */
 	SortPackInfo(Adapter);
@@ -815,7 +870,8 @@ static VOID CopyToAdapter(register struct bcm_mini_adapter *Adapter, /* <Pointer
 	 */
 	SortClassifiers(Adapter);
 	DumpPhsRules(&Adapter->stBCMPhsContext);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "%s <=====", __func__);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"%s <=====", __func__);
 }
 
 /***********************************************************************
@@ -1197,123 +1253,179 @@ static VOID DumpCmControlPacket(PVOID pvBuffer)
 
 	for (nIndex = 0; nIndex < nCurClassifierCnt; nIndex++)	{
 		struct bcm_convergence_types *psfCSType = NULL;
+		struct bcm_packet_class_rules *clsRule = NULL;
 
-		psfCSType =  &pstAddIndication->sfActiveSet.cConvergenceSLTypes[nIndex];
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " CCPacketClassificationRuleSI====>");
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8ClassifierRulePriority: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u8ClassifierRulePriority);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8IPTypeOfServiceLength: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u8IPTypeOfServiceLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8IPTypeOfService[3]: 0x%X ,0x%X ,0x%X ",
-				psfCSType->cCPacketClassificationRule.u8IPTypeOfService[0],
-				psfCSType->cCPacketClassificationRule.u8IPTypeOfService[1],
-				psfCSType->cCPacketClassificationRule.u8IPTypeOfService[2]);
+		psfCSType = &pstAddIndication->sfActiveSet.cConvergenceSLTypes[nIndex];
+		clsRule	= &psfCSType->cCPacketClassificationRule;
+
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL, " CCPacketClassificationRuleSI====>");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL, " u8ClassifierRulePriority: 0x%X ",
+				clsRule->u8ClassifierRulePriority);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL, " u8IPTypeOfServiceLength: 0x%X ",
+				clsRule->u8IPTypeOfServiceLength);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				" u8IPTypeOfService[3]: 0x%X ,0x%X ,0x%X ",
+				clsRule->u8IPTypeOfService[0],
+				clsRule->u8IPTypeOfService[1],
+				clsRule->u8IPTypeOfService[2]);
 
 		for (uiLoopIndex = 0; uiLoopIndex < 1; uiLoopIndex++)
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8Protocol: 0x%X ", psfCSType->cCPacketClassificationRule.u8Protocol);
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+					DBG_LVL_ALL,
+					" u8Protocol: 0x%X ",
+					clsRule->u8Protocol);
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8IPMaskedSourceAddressLength: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u8IPMaskedSourceAddressLength);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				"u8IPMaskedSourceAddressLength: 0x%X ",
+				clsRule->u8IPMaskedSourceAddressLength);
 
 		for (uiLoopIndex = 0; uiLoopIndex < 32; uiLoopIndex++)
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8IPMaskedSourceAddress[32]: 0x%X ",
-					psfCSType->cCPacketClassificationRule.u8IPMaskedSourceAddress[uiLoopIndex]);
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+					DBG_LVL_ALL,
+					"u8IPMaskedSourceAddress[32]: 0x%X ",
+					clsRule->u8IPMaskedSourceAddress[uiLoopIndex]);
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8IPDestinationAddressLength: 0x%02X ",
-				psfCSType->cCPacketClassificationRule.u8IPDestinationAddressLength);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				"u8IPDestinationAddressLength: 0x%02X ",
+				clsRule->u8IPDestinationAddressLength);
 
 		for (uiLoopIndex = 0; uiLoopIndex < 32; uiLoopIndex++)
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8IPDestinationAddress[32]:0x%X ",
-					psfCSType->cCPacketClassificationRule.u8IPDestinationAddress[uiLoopIndex]);
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+					DBG_LVL_ALL,
+					" u8IPDestinationAddress[32]:0x%X ",
+					clsRule->u8IPDestinationAddress[uiLoopIndex]);
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8ProtocolSourcePortRangeLength: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u8ProtocolSourcePortRangeLength);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				" u8ProtocolSourcePortRangeLength: 0x%X ",
+				clsRule->u8ProtocolSourcePortRangeLength);
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8ProtocolSourcePortRange[4]: 0x%X ,0x%X ,0x%X ,0x%X ",
-				psfCSType->cCPacketClassificationRule.u8ProtocolSourcePortRange[0],
-				psfCSType->cCPacketClassificationRule.u8ProtocolSourcePortRange[1],
-				psfCSType->cCPacketClassificationRule.u8ProtocolSourcePortRange[2],
-				psfCSType->cCPacketClassificationRule.u8ProtocolSourcePortRange[3]);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				" u8ProtocolSourcePortRange[4]: 0x%X ,0x%X ,0x%X ,0x%X ",
+				clsRule->u8ProtocolSourcePortRange[0],
+				clsRule->u8ProtocolSourcePortRange[1],
+				clsRule->u8ProtocolSourcePortRange[2],
+				clsRule->u8ProtocolSourcePortRange[3]);
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8ProtocolDestPortRangeLength: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u8ProtocolDestPortRangeLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8ProtocolDestPortRange[4]: 0x%X ,0x%X ,0x%X ,0x%X ",
-				psfCSType->cCPacketClassificationRule.u8ProtocolDestPortRange[0],
-				psfCSType->cCPacketClassificationRule.u8ProtocolDestPortRange[1],
-				psfCSType->cCPacketClassificationRule.u8ProtocolDestPortRange[2],
-				psfCSType->cCPacketClassificationRule.u8ProtocolDestPortRange[3]);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				" u8ProtocolDestPortRangeLength: 0x%X ",
+				clsRule->u8ProtocolDestPortRangeLength);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				" u8ProtocolDestPortRange[4]: 0x%X ,0x%X ,0x%X ,0x%X ",
+				clsRule->u8ProtocolDestPortRange[0],
+				clsRule->u8ProtocolDestPortRange[1],
+				clsRule->u8ProtocolDestPortRange[2],
+				clsRule->u8ProtocolDestPortRange[3]);
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8EthernetDestMacAddressLength: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddressLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8EthernetDestMacAddress[6]: 0x%X ,0x%X ,0x%X ,0x%X ,0x%X ,0x%X",
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddress[0],
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddress[1],
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddress[2],
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddress[3],
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddress[4],
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddress[5]);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8EthernetSourceMACAddressLength: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u8EthernetDestMacAddressLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, "u8EthernetSourceMACAddress[6]: 0x%X ,0x%X ,0x%X ,0x%X ,0x%X ,0x%X",
-				psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddress[0],
-				psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddress[1],
-				psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddress[2],
-				psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddress[3],
-				psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddress[4],
-				psfCSType->cCPacketClassificationRule.u8EthernetSourceMACAddress[5]);
-
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8EthertypeLength: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u8EthertypeLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8Ethertype[3]: 0x%X ,0x%X ,0x%X ",
-				psfCSType->cCPacketClassificationRule.u8Ethertype[0],
-				psfCSType->cCPacketClassificationRule.u8Ethertype[1],
-				psfCSType->cCPacketClassificationRule.u8Ethertype[2]);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u16UserPriority: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u16UserPriority);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u16VLANID: 0x%X ", psfCSType->cCPacketClassificationRule.u16VLANID);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8AssociatedPHSI: 0x%X ", psfCSType->cCPacketClassificationRule.u8AssociatedPHSI);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u16PacketClassificationRuleIndex:0x%X ",
-				psfCSType->cCPacketClassificationRule.u16PacketClassificationRuleIndex);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				" u8EthernetDestMacAddressLength: 0x%X ",
+				clsRule->u8EthernetDestMacAddressLength);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				" u8EthernetDestMacAddress[6]: 0x%X ,0x%X ,0x%X ,0x%X ,0x%X ,0x%X",
+				clsRule->u8EthernetDestMacAddress[0],
+				clsRule->u8EthernetDestMacAddress[1],
+				clsRule->u8EthernetDestMacAddress[2],
+				clsRule->u8EthernetDestMacAddress[3],
+				clsRule->u8EthernetDestMacAddress[4],
+				clsRule->u8EthernetDestMacAddress[5]);
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8VendorSpecificClassifierParamLength:0x%X ",
-				psfCSType->cCPacketClassificationRule.u8VendorSpecificClassifierParamLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8VendorSpecificClassifierParam[1]:0x%X ",
-				psfCSType->cCPacketClassificationRule.u8VendorSpecificClassifierParam[0]);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				" u8EthernetSourceMACAddressLength: 0x%X ",
+				clsRule->u8EthernetDestMacAddressLength);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				"u8EthernetSourceMACAddress[6]: 0x%X ,0x%X ,0x%X ,0x%X ,0x%X ,0x%X",
+				clsRule->u8EthernetSourceMACAddress[0],
+				clsRule->u8EthernetSourceMACAddress[1],
+				clsRule->u8EthernetSourceMACAddress[2],
+				clsRule->u8EthernetSourceMACAddress[3],
+				clsRule->u8EthernetSourceMACAddress[4],
+				clsRule->u8EthernetSourceMACAddress[5]);
+
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL, " u8EthertypeLength: 0x%X ",
+				clsRule->u8EthertypeLength);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				" u8Ethertype[3]: 0x%X ,0x%X ,0x%X ",
+				clsRule->u8Ethertype[0],
+				clsRule->u8Ethertype[1],
+				clsRule->u8Ethertype[2]);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL, " u16UserPriority: 0x%X ",
+				clsRule->u16UserPriority);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL, " u16VLANID: 0x%X ",
+				clsRule->u16VLANID);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL, " u8AssociatedPHSI: 0x%X ",
+				clsRule->u8AssociatedPHSI);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				" u16PacketClassificationRuleIndex:0x%X ",
+				clsRule->u16PacketClassificationRuleIndex);
+
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				" u8VendorSpecificClassifierParamLength:0x%X ",
+				clsRule->u8VendorSpecificClassifierParamLength);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				" u8VendorSpecificClassifierParam[1]:0x%X ",
+				clsRule->u8VendorSpecificClassifierParam[0]);
 #ifdef VERSION_D5
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8IPv6FlowLableLength: 0x%X ",
-				psfCSType->cCPacketClassificationRule.u8IPv6FlowLableLength);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " u8IPv6FlowLable[6]: 0x%X ,0x%X ,0x%X ,0x%X ,0x%X ,0x%X ",
-				psfCSType->cCPacketClassificationRule.u8IPv6FlowLable[0],
-				psfCSType->cCPacketClassificationRule.u8IPv6FlowLable[1],
-				psfCSType->cCPacketClassificationRule.u8IPv6FlowLable[2],
-				psfCSType->cCPacketClassificationRule.u8IPv6FlowLable[3],
-				psfCSType->cCPacketClassificationRule.u8IPv6FlowLable[4],
-				psfCSType->cCPacketClassificationRule.u8IPv6FlowLable[5]);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL, " u8IPv6FlowLableLength: 0x%X ",
+				clsRule->u8IPv6FlowLableLength);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL,
+				DBG_LVL_ALL,
+				" u8IPv6FlowLable[6]: 0x%X ,0x%X ,0x%X ,0x%X ,0x%X ,0x%X ",
+				clsRule->u8IPv6FlowLable[0],
+				clsRule->u8IPv6FlowLable[1],
+				clsRule->u8IPv6FlowLable[2],
+				clsRule->u8IPv6FlowLable[3],
+				clsRule->u8IPv6FlowLable[4],
+				clsRule->u8IPv6FlowLable[5]);
 #endif
 	}
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL, " bValid: 0x%X", pstAddIndication->sfActiveSet.bValid);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_CONTROL, DBG_LVL_ALL,
+			" bValid: 0x%X", pstAddIndication->sfActiveSet.bValid);
 }
 
-static inline ULONG RestoreSFParam(struct bcm_mini_adapter *Adapter, ULONG ulAddrSFParamSet, PUCHAR pucDestBuffer)
+static inline ULONG RestoreSFParam(struct bcm_mini_adapter *Adapter,
+		ULONG ulAddrSFParamSet, PUCHAR pucDestBuffer)
 {
 	UINT  nBytesToRead = sizeof(struct bcm_connect_mgr_params);
 
 	if (ulAddrSFParamSet == 0 || NULL == pucDestBuffer) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Got Param address as 0!!");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"Got Param address as 0!!");
 		return 0;
 	}
 	ulAddrSFParamSet = ntohl(ulAddrSFParamSet);
 
 	/* Read out the SF Param Set At the indicated Location */
-	if (rdm(Adapter, ulAddrSFParamSet, (PUCHAR)pucDestBuffer, nBytesToRead) < 0)
+	if (rdm(Adapter, ulAddrSFParamSet,(PUCHAR)pucDestBuffer, nBytesToRead) < 0)
 		return STATUS_FAILURE;
 
 	return 1;
 }
 
-static ULONG StoreSFParam(struct bcm_mini_adapter *Adapter, PUCHAR pucSrcBuffer, ULONG ulAddrSFParamSet)
+static ULONG StoreSFParam(struct bcm_mini_adapter *Adapter, PUCHAR pucSrcBuffer,
+		ULONG ulAddrSFParamSet)
 {
 	UINT nBytesToWrite = sizeof(struct bcm_connect_mgr_params);
 	int ret = 0;
@@ -1323,13 +1435,15 @@ static ULONG StoreSFParam(struct bcm_mini_adapter *Adapter, PUCHAR pucSrcBuffer,
 
 	ret = wrm(Adapter, ulAddrSFParamSet, (u8 *)pucSrcBuffer, nBytesToWrite);
 	if (ret < 0) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "%s:%d WRM failed", __func__, __LINE__);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"%s:%d WRM failed", __func__, __LINE__);
 		return ret;
 	}
 	return 1;
 }
 
-ULONG StoreCmControlResponseMessage(struct bcm_mini_adapter *Adapter, PVOID pvBuffer, UINT *puBufferLength)
+ULONG StoreCmControlResponseMessage(struct bcm_mini_adapter *Adapter,
+		PVOID pvBuffer, UINT *puBufferLength)
 {
 	struct bcm_add_indication_alt *pstAddIndicationAlt = NULL;
 	struct bcm_add_indication *pstAddIndication = NULL;
@@ -1363,13 +1477,15 @@ ULONG StoreCmControlResponseMessage(struct bcm_mini_adapter *Adapter, PVOID pvBu
 	}
 	/* For DSA_REQ, only up to "psfAuthorizedSet" parameter should be accessed by driver! */
 
-	pstAddIndication = kmalloc(sizeof(struct bcm_add_indication), GFP_KERNEL);
+	pstAddIndication = kmalloc(sizeof(struct bcm_add_indication),
+			GFP_KERNEL);
 	if (pstAddIndication == NULL)
 		return 0;
 
 	/* AUTHORIZED SET */
 	pstAddIndication->psfAuthorizedSet = (struct bcm_connect_mgr_params *)
-			GetNextTargetBufferLocation(Adapter, pstAddIndicationAlt->u16TID);
+			GetNextTargetBufferLocation(Adapter,
+					pstAddIndicationAlt->u16TID);
 	if (!pstAddIndication->psfAuthorizedSet) {
 		kfree(pstAddIndication);
 		return 0;
@@ -1382,7 +1498,9 @@ ULONG StoreCmControlResponseMessage(struct bcm_mini_adapter *Adapter, PVOID pvBu
 	}
 
 	/* this can't possibly be right */
-	pstAddIndication->psfAuthorizedSet = (struct bcm_connect_mgr_params *)ntohl((ULONG)pstAddIndication->psfAuthorizedSet);
+	pstAddIndication->psfAuthorizedSet =
+		(struct bcm_connect_mgr_params *) ntohl(
+				(ULONG)pstAddIndication->psfAuthorizedSet);
 
 	if (pstAddIndicationAlt->u8Type == DSA_REQ) {
 		struct bcm_add_request AddRequest;
@@ -1411,31 +1529,39 @@ ULONG StoreCmControlResponseMessage(struct bcm_mini_adapter *Adapter, PVOID pvBu
 
 	/* ADMITTED SET */
 	pstAddIndication->psfAdmittedSet = (struct bcm_connect_mgr_params *)
-		GetNextTargetBufferLocation(Adapter, pstAddIndicationAlt->u16TID);
+		GetNextTargetBufferLocation(Adapter,
+				pstAddIndicationAlt->u16TID);
 	if (!pstAddIndication->psfAdmittedSet) {
 		kfree(pstAddIndication);
 		return 0;
 	}
-	if (StoreSFParam(Adapter, (PUCHAR)&pstAddIndicationAlt->sfAdmittedSet, (ULONG)pstAddIndication->psfAdmittedSet) != 1) {
+	if (StoreSFParam(Adapter, (PUCHAR)&pstAddIndicationAlt->sfAdmittedSet,
+				(ULONG)pstAddIndication->psfAdmittedSet) != 1) {
 		kfree(pstAddIndication);
 		return 0;
 	}
 
-	pstAddIndication->psfAdmittedSet = (struct bcm_connect_mgr_params *)ntohl((ULONG)pstAddIndication->psfAdmittedSet);
+	pstAddIndication->psfAdmittedSet =
+		(struct bcm_connect_mgr_params *) ntohl(
+				(ULONG) pstAddIndication->psfAdmittedSet);
 
 	/* ACTIVE SET */
 	pstAddIndication->psfActiveSet = (struct bcm_connect_mgr_params *)
-		GetNextTargetBufferLocation(Adapter, pstAddIndicationAlt->u16TID);
+		GetNextTargetBufferLocation(Adapter,
+				pstAddIndicationAlt->u16TID);
 	if (!pstAddIndication->psfActiveSet) {
 		kfree(pstAddIndication);
 		return 0;
 	}
-	if (StoreSFParam(Adapter, (PUCHAR)&pstAddIndicationAlt->sfActiveSet, (ULONG)pstAddIndication->psfActiveSet) != 1) {
+	if (StoreSFParam(Adapter, (PUCHAR)&pstAddIndicationAlt->sfActiveSet,
+				(ULONG)pstAddIndication->psfActiveSet) != 1) {
 		kfree(pstAddIndication);
 		return 0;
 	}
 
-	pstAddIndication->psfActiveSet = (struct bcm_connect_mgr_params *)ntohl((ULONG)pstAddIndication->psfActiveSet);
+	pstAddIndication->psfActiveSet =
+		(struct bcm_connect_mgr_params *) ntohl(
+				(ULONG)pstAddIndication->psfActiveSet);
 
 	(*puBufferLength) = sizeof(struct bcm_add_indication);
 	*(struct bcm_add_indication *)pvBuffer = *pstAddIndication;
@@ -1444,40 +1570,63 @@ ULONG StoreCmControlResponseMessage(struct bcm_mini_adapter *Adapter, PVOID pvBu
 }
 
 static inline struct bcm_add_indication_alt
-*RestoreCmControlResponseMessage(register struct bcm_mini_adapter *Adapter, register PVOID pvBuffer)
+*RestoreCmControlResponseMessage(register struct bcm_mini_adapter *Adapter,
+		register PVOID pvBuffer)
 {
 	ULONG ulStatus = 0;
 	struct bcm_add_indication *pstAddIndication = NULL;
 	struct bcm_add_indication_alt *pstAddIndicationDest = NULL;
 
 	pstAddIndication = pvBuffer;
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "=====>");
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"=====>");
 	if ((pstAddIndication->u8Type == DSD_REQ) ||
 		(pstAddIndication->u8Type == DSD_RSP) ||
 		(pstAddIndication->u8Type == DSD_ACK))
 		return pvBuffer;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Inside RestoreCmControlResponseMessage ");
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"Inside RestoreCmControlResponseMessage ");
 	/*
 	 * Need to Allocate memory to contain the SUPER Large structures
 	 * Our driver can't create these structures on Stack :(
 	 */
-	pstAddIndicationDest = kmalloc(sizeof(struct bcm_add_indication_alt), GFP_KERNEL);
+	pstAddIndicationDest = kmalloc(sizeof(struct bcm_add_indication_alt),
+			GFP_KERNEL);
 
 	if (pstAddIndicationDest) {
-		memset(pstAddIndicationDest, 0, sizeof(struct bcm_add_indication_alt));
+		memset(pstAddIndicationDest, 0,
+				sizeof(struct bcm_add_indication_alt));
 	} else {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Failed to allocate memory for SF Add Indication Structure ");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG,
+				DBG_LVL_ALL,
+				"Failed to allocate memory for SF Add Indication Structure ");
 		return NULL;
 	}
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "AddIndication-u8Type : 0x%X", pstAddIndication->u8Type);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "AddIndication-u8Direction : 0x%X", pstAddIndication->eConnectionDir);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "AddIndication-u8TID : 0x%X", ntohs(pstAddIndication->u16TID));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "AddIndication-u8CID : 0x%X", ntohs(pstAddIndication->u16CID));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "AddIndication-u16VCID : 0x%X", ntohs(pstAddIndication->u16VCID));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "AddIndication-autorized set loc : %p", pstAddIndication->psfAuthorizedSet);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "AddIndication-admitted set loc : %p", pstAddIndication->psfAdmittedSet);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "AddIndication-Active set loc : %p", pstAddIndication->psfActiveSet);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"AddIndication-u8Type : 0x%X",
+			pstAddIndication->u8Type);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"AddIndication-u8Direction : 0x%X",
+			pstAddIndication->eConnectionDir);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"AddIndication-u8TID : 0x%X",
+			ntohs(pstAddIndication->u16TID));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"AddIndication-u8CID : 0x%X",
+			ntohs(pstAddIndication->u16CID));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"AddIndication-u16VCID : 0x%X",
+			ntohs(pstAddIndication->u16VCID));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"AddIndication-autorized set loc : %p",
+			pstAddIndication->psfAuthorizedSet);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"AddIndication-admitted set loc : %p",
+			pstAddIndication->psfAdmittedSet);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"AddIndication-Active set loc : %p",
+			pstAddIndication->psfActiveSet);
 
 	pstAddIndicationDest->u8Type = pstAddIndication->u8Type;
 	pstAddIndicationDest->u8Direction = pstAddIndication->eConnectionDir;
@@ -1486,39 +1635,60 @@ static inline struct bcm_add_indication_alt
 	pstAddIndicationDest->u16VCID = pstAddIndication->u16VCID;
 	pstAddIndicationDest->u8CC = pstAddIndication->u8CC;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,  "Restoring Active Set ");
-	ulStatus = RestoreSFParam(Adapter, (ULONG)pstAddIndication->psfActiveSet, (PUCHAR)&pstAddIndicationDest->sfActiveSet);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"Restoring Active Set ");
+	ulStatus = RestoreSFParam(Adapter,
+			(ULONG)pstAddIndication->psfActiveSet,
+			(PUCHAR)&pstAddIndicationDest->sfActiveSet);
 	if (ulStatus != 1)
 		goto failed_restore_sf_param;
 
 	if (pstAddIndicationDest->sfActiveSet.u8TotalClassifiers > MAX_CLASSIFIERS_IN_SF)
-		pstAddIndicationDest->sfActiveSet.u8TotalClassifiers = MAX_CLASSIFIERS_IN_SF;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,  "Restoring Admitted Set ");
-	ulStatus = RestoreSFParam(Adapter, (ULONG)pstAddIndication->psfAdmittedSet, (PUCHAR)&pstAddIndicationDest->sfAdmittedSet);
+		pstAddIndicationDest->sfActiveSet.u8TotalClassifiers =
+			MAX_CLASSIFIERS_IN_SF;
+
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"Restoring Admitted Set ");
+	ulStatus = RestoreSFParam(Adapter,
+			(ULONG)pstAddIndication->psfAdmittedSet,
+			(PUCHAR)&pstAddIndicationDest->sfAdmittedSet);
 	if (ulStatus != 1)
 		goto failed_restore_sf_param;
 
 	if (pstAddIndicationDest->sfAdmittedSet.u8TotalClassifiers > MAX_CLASSIFIERS_IN_SF)
-		pstAddIndicationDest->sfAdmittedSet.u8TotalClassifiers = MAX_CLASSIFIERS_IN_SF;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,  "Restoring Authorized Set ");
-	ulStatus = RestoreSFParam(Adapter, (ULONG)pstAddIndication->psfAuthorizedSet, (PUCHAR)&pstAddIndicationDest->sfAuthorizedSet);
+		pstAddIndicationDest->sfAdmittedSet.u8TotalClassifiers =
+			MAX_CLASSIFIERS_IN_SF;
+
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"Restoring Authorized Set ");
+	ulStatus = RestoreSFParam(Adapter,
+			(ULONG)pstAddIndication->psfAuthorizedSet,
+			(PUCHAR)&pstAddIndicationDest->sfAuthorizedSet);
 	if (ulStatus != 1)
 		goto failed_restore_sf_param;
 
 	if (pstAddIndicationDest->sfAuthorizedSet.u8TotalClassifiers > MAX_CLASSIFIERS_IN_SF)
-		pstAddIndicationDest->sfAuthorizedSet.u8TotalClassifiers = MAX_CLASSIFIERS_IN_SF;
-
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Dumping the whole raw packet");
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "============================================================");
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, " pstAddIndicationDest->sfActiveSet size  %zx %p", sizeof(*pstAddIndicationDest), pstAddIndicationDest);
-	/* BCM_DEBUG_PRINT_BUFFER(Adapter,DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, (unsigned char *)pstAddIndicationDest, sizeof(*pstAddIndicationDest)); */
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "============================================================");
+		pstAddIndicationDest->sfAuthorizedSet.u8TotalClassifiers =
+			MAX_CLASSIFIERS_IN_SF;
+
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"Dumping the whole raw packet");
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+		"============================================================");
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			" pstAddIndicationDest->sfActiveSet size  %zx %p",
+			sizeof(*pstAddIndicationDest), pstAddIndicationDest);
+	/* BCM_DEBUG_PRINT_BUFFER(Adapter,DBG_TYPE_OTHERS, CONN_MSG,
+	 *		DBG_LVL_ALL, (unsigned char *)pstAddIndicationDest,
+	 *		sizeof(*pstAddIndicationDest));
+	 */
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"============================================================");
 	return pstAddIndicationDest;
 failed_restore_sf_param:
 	kfree(pstAddIndicationDest);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "<=====");
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"<=====");
 	return NULL;
 }
 
@@ -1530,31 +1700,44 @@ ULONG SetUpTargetDsxBuffers(struct bcm_mini_adapter *Adapter)
 	int Status;
 
 	if (!Adapter) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Adapter was NULL!!!");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"Adapter was NULL!!!");
 		return 0;
 	}
 
 	if (Adapter->astTargetDsxBuffer[0].ulTargetDsxBuffer)
 		return 1;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Size of Each DSX Buffer(Also size of connection manager parameters): %zx ", sizeof(struct bcm_connect_mgr_params));
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Reading DSX buffer From Target location %x ", DSX_MESSAGE_EXCHANGE_BUFFER);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"Size of Each DSX Buffer(Also size of connection manager parameters): %zx ",
+			sizeof(struct bcm_connect_mgr_params));
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"Reading DSX buffer From Target location %x ",
+			DSX_MESSAGE_EXCHANGE_BUFFER);
 
-	Status = rdmalt(Adapter, DSX_MESSAGE_EXCHANGE_BUFFER, (PUINT)&ulTargetDsxBuffersBase, sizeof(UINT));
+	Status = rdmalt(Adapter, DSX_MESSAGE_EXCHANGE_BUFFER,
+			(PUINT)&ulTargetDsxBuffersBase, sizeof(UINT));
 	if (Status < 0) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "RDM failed!!");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"RDM failed!!");
 		return 0;
 	}
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Base Address Of DSX  Target Buffer : 0x%lx", ulTargetDsxBuffersBase);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,  "Tgt Buffer is Now %lx :", ulTargetDsxBuffersBase);
-	ulCntTargetBuffers = DSX_MESSAGE_EXCHANGE_BUFFER_SIZE / sizeof(struct bcm_connect_mgr_params);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"Base Address Of DSX  Target Buffer : 0x%lx",
+			ulTargetDsxBuffersBase);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"Tgt Buffer is Now %lx :", ulTargetDsxBuffersBase);
+	ulCntTargetBuffers = DSX_MESSAGE_EXCHANGE_BUFFER_SIZE /
+		sizeof(struct bcm_connect_mgr_params);
 
 	Adapter->ulTotalTargetBuffersAvailable =
 		ulCntTargetBuffers > MAX_TARGET_DSX_BUFFERS ?
 		MAX_TARGET_DSX_BUFFERS : ulCntTargetBuffers;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, " Total Target DSX Buffer setup %lx ", Adapter->ulTotalTargetBuffersAvailable);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			" Total Target DSX Buffer setup %lx ",
+			Adapter->ulTotalTargetBuffersAvailable);
 
 	for (i = 0; i < Adapter->ulTotalTargetBuffersAvailable; i++) {
 		Adapter->astTargetDsxBuffer[i].ulTargetDsxBuffer = ulTargetDsxBuffersBase;
@@ -1569,12 +1752,14 @@ ULONG SetUpTargetDsxBuffers(struct bcm_mini_adapter *Adapter)
 	return 1;
 }
 
-static ULONG GetNextTargetBufferLocation(struct bcm_mini_adapter *Adapter, B_UINT16 tid)
+static ULONG GetNextTargetBufferLocation(struct bcm_mini_adapter *Adapter,
+		B_UINT16 tid)
 {
 	ULONG dsx_buf;
 	ULONG idx, max_try;
 
-	if ((Adapter->ulTotalTargetBuffersAvailable == 0) || (Adapter->ulFreeTargetBufferCnt == 0)) {
+	if ((Adapter->ulTotalTargetBuffersAvailable == 0)
+			|| (Adapter->ulFreeTargetBufferCnt == 0)) {
 		ClearTargetDSXBuffer(Adapter, tid, false);
 		return 0;
 	}
@@ -1587,7 +1772,9 @@ static ULONG GetNextTargetBufferLocation(struct bcm_mini_adapter *Adapter, B_UIN
 	}
 
 	if (max_try == 0) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "\n GetNextTargetBufferLocation : Error No Free Target DSX Buffers FreeCnt : %lx ", Adapter->ulFreeTargetBufferCnt);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+				"\n GetNextTargetBufferLocation : Error No Free Target DSX Buffers FreeCnt : %lx ",
+				Adapter->ulFreeTargetBufferCnt);
 		ClearTargetDSXBuffer(Adapter, tid, false);
 		return 0;
 	}
@@ -1598,7 +1785,9 @@ static ULONG GetNextTargetBufferLocation(struct bcm_mini_adapter *Adapter, B_UIN
 	Adapter->ulFreeTargetBufferCnt--;
 	idx = (idx+1)%Adapter->ulTotalTargetBuffersAvailable;
 	Adapter->ulCurrentTargetBuffer = idx;
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "GetNextTargetBufferLocation :Returning address %lx tid %d\n", dsx_buf, tid);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+			"GetNextTargetBufferLocation :Returning address %lx tid %d\n",
+			dsx_buf, tid);
 
 	return dsx_buf;
 }
@@ -1609,7 +1798,8 @@ int AllocAdapterDsxBuffer(struct bcm_mini_adapter *Adapter)
 	 * Need to Allocate memory to contain the SUPER Large structures
 	 * Our driver can't create these structures on Stack
 	 */
-	Adapter->caDsxReqResp = kmalloc(sizeof(struct bcm_add_indication_alt)+LEADER_SIZE, GFP_KERNEL);
+	Adapter->caDsxReqResp = kmalloc(sizeof(struct bcm_add_indication_alt)
+			+ LEADER_SIZE, GFP_KERNEL);
 	if (!Adapter->caDsxReqResp)
 		return -ENOMEM;
 
@@ -1867,29 +2057,37 @@ bool CmControlResponseMessage(struct bcm_mini_adapter *Adapter,  /* <Pointer to
 	return TRUE;
 }
 
-int get_dsx_sf_data_to_application(struct bcm_mini_adapter *Adapter, UINT uiSFId, void __user *user_buffer)
+int get_dsx_sf_data_to_application(struct bcm_mini_adapter *Adapter,
+		UINT uiSFId, void __user *user_buffer)
 {
 	int status = 0;
 	struct bcm_packet_info *psSfInfo = NULL;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "status =%d", status);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"status =%d", status);
 	status = SearchSfid(Adapter, uiSFId);
 	if (status >= NO_OF_QUEUES) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "SFID %d not present in queue !!!", uiSFId);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"SFID %d not present in queue !!!", uiSFId);
 		return -EINVAL;
 	}
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "status =%d", status);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"status =%d", status);
 	psSfInfo = &Adapter->PackInfo[status];
-	if (psSfInfo->pstSFIndication && copy_to_user(user_buffer,
-							psSfInfo->pstSFIndication, sizeof(struct bcm_add_indication_alt))) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "copy to user failed SFID %d, present in queue !!!", uiSFId);
+	if (psSfInfo->pstSFIndication
+			&& copy_to_user(user_buffer, psSfInfo->pstSFIndication,
+				sizeof(struct bcm_add_indication_alt))) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+				"copy to user failed SFID %d, present in queue !!!",
+				uiSFId);
 		status = -EFAULT;
 		return status;
 	}
 	return STATUS_SUCCESS;
 }
 
-VOID OverrideServiceFlowParams(struct bcm_mini_adapter *Adapter, PUINT puiBuffer)
+VOID OverrideServiceFlowParams(struct bcm_mini_adapter *Adapter,
+		PUINT puiBuffer)
 {
 	B_UINT32 u32NumofSFsinMsg = ntohl(*(puiBuffer + 1));
 	struct bcm_stim_sfhostnotify *pHostInfo = NULL;
@@ -1897,7 +2095,8 @@ VOID OverrideServiceFlowParams(struct bcm_mini_adapter *Adapter, PUINT puiBuffer
 	ULONG ulSFID = 0;
 
 	puiBuffer += 2;
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "u32NumofSFsinMsg: 0x%x\n", u32NumofSFsinMsg);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+			"u32NumofSFsinMsg: 0x%x\n", u32NumofSFsinMsg);
 
 	while (u32NumofSFsinMsg != 0 && u32NumofSFsinMsg < NO_OF_QUEUES) {
 		u32NumofSFsinMsg--;
@@ -1906,31 +2105,149 @@ VOID OverrideServiceFlowParams(struct bcm_mini_adapter *Adapter, PUINT puiBuffer
 
 		ulSFID = ntohl(pHostInfo->SFID);
 		uiSearchRuleIndex = SearchSfid(Adapter, ulSFID);
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "SFID: 0x%lx\n", ulSFID);
-
-		if (uiSearchRuleIndex >= NO_OF_QUEUES || uiSearchRuleIndex == HiPriority) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "The SFID <%lx> doesn't exist in host entry or is Invalid\n", ulSFID);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+				"SFID: 0x%lx\n", ulSFID);
+
+		if (uiSearchRuleIndex >= NO_OF_QUEUES
+				|| uiSearchRuleIndex == HiPriority) {
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG,
+					DBG_LVL_ALL,
+					"The SFID <%lx> doesn't exist in host entry or is Invalid\n",
+					ulSFID);
 			continue;
 		}
 
 		if (pHostInfo->RetainSF == false) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "Going to Delete SF");
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG,
+					DBG_LVL_ALL, "Going to Delete SF");
 			deleteSFBySfid(Adapter, uiSearchRuleIndex);
 		} else {
-			Adapter->PackInfo[uiSearchRuleIndex].usVCID_Value = ntohs(pHostInfo->VCID);
-			Adapter->PackInfo[uiSearchRuleIndex].usCID = ntohs(pHostInfo->newCID);
-			Adapter->PackInfo[uiSearchRuleIndex].bActive = false;
+			struct bcm_packet_info *packinfo =
+				&Adapter->PackInfo[uiSearchRuleIndex];
 
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL, "pHostInfo->QoSParamSet: 0x%x\n", pHostInfo->QoSParamSet);
+			packinfo->usVCID_Value = ntohs(pHostInfo->VCID);
+			packinfo->usCID = ntohs(pHostInfo->newCID);
+			packinfo->bActive = false;
+
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG,
+					DBG_LVL_ALL,
+					"pHostInfo->QoSParamSet: 0x%x\n",
+					pHostInfo->QoSParamSet);
 
 			if (pHostInfo->QoSParamSet & 0x1)
-				Adapter->PackInfo[uiSearchRuleIndex].bAuthorizedSet = TRUE;
+				packinfo->bAuthorizedSet = TRUE;
 			if (pHostInfo->QoSParamSet & 0x2)
-				Adapter->PackInfo[uiSearchRuleIndex].bAdmittedSet = TRUE;
+				packinfo->bAdmittedSet = TRUE;
 			if (pHostInfo->QoSParamSet & 0x4) {
-				Adapter->PackInfo[uiSearchRuleIndex].bActiveSet = TRUE;
-				Adapter->PackInfo[uiSearchRuleIndex].bActive = TRUE;
+				packinfo->bActiveSet = TRUE;
+				packinfo->bActive = TRUE;
+			}
+		}
+	}
+}
+
+static void restore_endianess_of_pstClassifierEntry(
+		struct bcm_classifier_rule *pstClassifierEntry,
+		enum bcm_ipaddr_context eIpAddrContext)
+{
+	int i;
+	union u_ip_address *stSrc  = &pstClassifierEntry->stSrcIpAddress;
+	union u_ip_address *stDest = &pstClassifierEntry->stDestIpAddress;
+
+	for (i = 0; i < MAX_IP_RANGE_LENGTH * 4; i++) {
+		if (eIpAddrContext == eSrcIpAddress) {
+			stSrc->ulIpv6Addr[i] = ntohl(stSrc->ulIpv6Addr[i]);
+			stSrc->ulIpv6Mask[i] = ntohl(stSrc->ulIpv6Mask[i]);
+		} else if (eIpAddrContext == eDestIpAddress) {
+			stDest->ulIpv6Addr[i] = ntohl(stDest->ulIpv6Addr[i]);
+			stDest->ulIpv6Mask[i] = ntohl(stDest->ulIpv6Mask[i]);
+		}
+	}
+}
+
+static void apply_phs_rule_to_all_classifiers(
+		register struct bcm_mini_adapter *Adapter,		/* <Pointer to the Adapter structure */
+		register UINT uiSearchRuleIndex,			/* <Index of Queue, to which this data belongs */
+		USHORT uVCID,
+		struct bcm_phs_rule *sPhsRule,
+		struct bcm_phs_rules *cPhsRule,
+		struct bcm_add_indication_alt *pstAddIndication)
+{
+	unsigned int uiClassifierIndex = 0;
+	struct bcm_classifier_rule *curr_classifier = NULL;
+
+	if (pstAddIndication->u8Direction == UPLINK_DIR) {
+		for (uiClassifierIndex = 0; uiClassifierIndex < MAX_CLASSIFIERS; uiClassifierIndex++) {
+			curr_classifier =
+				&Adapter->astClassifierTable[uiClassifierIndex];
+			if ((curr_classifier->bUsed) &&
+				(curr_classifier->ulSFID == Adapter->PackInfo[uiSearchRuleIndex].ulSFID) &&
+				(curr_classifier->u8AssociatedPHSI == cPhsRule->u8PHSI)) {
+				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, CONN_MSG, DBG_LVL_ALL,
+						"Adding PHS Rule For Classifier: 0x%x cPhsRule.u8PHSI: 0x%x\n",
+						curr_classifier->uiClassifierRuleIndex,
+						cPhsRule->u8PHSI);
+				/* Update The PHS Rule for this classifier as Associated PHSI id defined */
+
+				/* Copy the PHS Rule */
+				sPhsRule->u8PHSI = cPhsRule->u8PHSI;
+				sPhsRule->u8PHSFLength = cPhsRule->u8PHSFLength;
+				sPhsRule->u8PHSMLength = cPhsRule->u8PHSMLength;
+				sPhsRule->u8PHSS = cPhsRule->u8PHSS;
+				sPhsRule->u8PHSV = cPhsRule->u8PHSV;
+				memcpy(sPhsRule->u8PHSF, cPhsRule->u8PHSF, MAX_PHS_LENGTHS);
+				memcpy(sPhsRule->u8PHSM, cPhsRule->u8PHSM, MAX_PHS_LENGTHS);
+				sPhsRule->u8RefCnt = 0;
+				sPhsRule->bUnclassifiedPHSRule = false;
+				sPhsRule->PHSModifiedBytes = 0;
+				sPhsRule->PHSModifiedNumPackets = 0;
+				sPhsRule->PHSErrorNumPackets = 0;
+
+				/* bPHSRuleAssociated = TRUE; */
+				/* Store The PHS Rule for this classifier */
+
+				PhsUpdateClassifierRule(
+					&Adapter->stBCMPhsContext,
+					uVCID,
+					curr_classifier->uiClassifierRuleIndex,
+					sPhsRule,
+					curr_classifier->u8AssociatedPHSI);
+
+				/* Update PHS Rule For the Classifier */
+				if (sPhsRule->u8PHSI) {
+					curr_classifier->u32PHSRuleID = sPhsRule->u8PHSI;
+					memcpy(&curr_classifier->sPhsRule, sPhsRule, sizeof(struct bcm_phs_rule));
+				}
 			}
 		}
+	} else {
+		/* Error PHS Rule specified in signaling could not be applied to any classifier */
+
+		/* Copy the PHS Rule */
+		sPhsRule->u8PHSI = cPhsRule->u8PHSI;
+		sPhsRule->u8PHSFLength = cPhsRule->u8PHSFLength;
+		sPhsRule->u8PHSMLength = cPhsRule->u8PHSMLength;
+		sPhsRule->u8PHSS = cPhsRule->u8PHSS;
+		sPhsRule->u8PHSV = cPhsRule->u8PHSV;
+		memcpy(sPhsRule->u8PHSF, cPhsRule->u8PHSF, MAX_PHS_LENGTHS);
+		memcpy(sPhsRule->u8PHSM, cPhsRule->u8PHSM, MAX_PHS_LENGTHS);
+		sPhsRule->u8RefCnt = 0;
+		sPhsRule->bUnclassifiedPHSRule = TRUE;
+		sPhsRule->PHSModifiedBytes = 0;
+		sPhsRule->PHSModifiedNumPackets = 0;
+		sPhsRule->PHSErrorNumPackets = 0;
+		/* Store The PHS Rule for this classifier */
+
+		/*
+		 * Passing the argument u8PHSI instead of clsid. Because for DL with no classifier rule,
+		 * clsid will be zero hence we can't have multiple PHS rules for the same SF.
+		 * To support multiple PHS rule, passing u8PHSI.
+		 */
+		PhsUpdateClassifierRule(
+			&Adapter->stBCMPhsContext,
+			uVCID,
+			sPhsRule->u8PHSI,
+			sPhsRule,
+			sPhsRule->u8PHSI);
 	}
 }
diff --git a/drivers/staging/bcm/InterfaceDld.c b/drivers/staging/bcm/InterfaceDld.c
index 005e4607b26..e1925bdc127 100644
--- a/drivers/staging/bcm/InterfaceDld.c
+++ b/drivers/staging/bcm/InterfaceDld.c
@@ -53,8 +53,8 @@ int InterfaceFileReadbackFromChip(PVOID arg, struct file *flp,
 	struct bcm_interface_adapter *psIntfAdapter = arg;
 	int bytes;
 
-	buff = kmalloc(MAX_TRANSFER_CTRL_BYTE_USB, GFP_DMA);
-	buff_readback = kmalloc(MAX_TRANSFER_CTRL_BYTE_USB , GFP_DMA);
+	buff = kzalloc(MAX_TRANSFER_CTRL_BYTE_USB, GFP_DMA);
+	buff_readback = kzalloc(MAX_TRANSFER_CTRL_BYTE_USB , GFP_DMA);
 	if (!buff || !buff_readback) {
 		kfree(buff);
 		kfree(buff_readback);
@@ -64,8 +64,6 @@ int InterfaceFileReadbackFromChip(PVOID arg, struct file *flp,
 
 	is_config_file = (on_chip_loc == CONFIG_BEGIN_ADDR) ? 1 : 0;
 
-	memset(buff_readback, 0, MAX_TRANSFER_CTRL_BYTE_USB);
-	memset(buff, 0, MAX_TRANSFER_CTRL_BYTE_USB);
 	while (1) {
 		oldfs = get_fs();
 		set_fs(get_ds());
diff --git a/drivers/staging/bcm/InterfaceIdleMode.c b/drivers/staging/bcm/InterfaceIdleMode.c
index fecf81ffe06..c84ee494f55 100644
--- a/drivers/staging/bcm/InterfaceIdleMode.c
+++ b/drivers/staging/bcm/InterfaceIdleMode.c
@@ -223,7 +223,6 @@ static int InterfaceAbortIdlemode(struct bcm_mini_adapter *Adapter,
 }
 int InterfaceIdleModeWakeup(struct bcm_mini_adapter *Adapter)
 {
-	ULONG	Status = 0;
 	if (Adapter->bTriedToWakeUpFromlowPowerMode) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
 		IDLE_MODE, DBG_LVL_ALL,
@@ -233,7 +232,7 @@ int InterfaceIdleModeWakeup(struct bcm_mini_adapter *Adapter)
 		InterfaceAbortIdlemode(Adapter, Adapter->usIdleModePattern);
 
 	}
-	return Status;
+	return 0;
 }
 
 void InterfaceHandleShutdownModeWakeup(struct bcm_mini_adapter *Adapter)
diff --git a/drivers/staging/bcm/InterfaceMisc.h b/drivers/staging/bcm/InterfaceMisc.h
index bce6869a747..efb68604615 100644
--- a/drivers/staging/bcm/InterfaceMisc.h
+++ b/drivers/staging/bcm/InterfaceMisc.h
@@ -14,13 +14,13 @@ InterfaceWRM(struct bcm_interface_adapter *psIntfAdapter,
 			INT len);
 
 
-int InterfaceFileDownload( PVOID psIntfAdapter,
-                        struct file *flp,
-                        unsigned int on_chip_loc);
+int InterfaceFileDownload(PVOID psIntfAdapter,
+			struct file *flp,
+			unsigned int on_chip_loc);
 
-int InterfaceFileReadbackFromChip( PVOID psIntfAdapter,
-                        struct file *flp,
-                        unsigned int on_chip_loc);
+int InterfaceFileReadbackFromChip(PVOID psIntfAdapter,
+			struct file *flp,
+			unsigned int on_chip_loc);
 
 
 int BcmRDM(PVOID arg,
diff --git a/drivers/staging/bcm/PHSModule.c b/drivers/staging/bcm/PHSModule.c
index afc7bcc3e54..07c5a0bae1e 100644
--- a/drivers/staging/bcm/PHSModule.c
+++ b/drivers/staging/bcm/PHSModule.c
@@ -409,7 +409,6 @@ ULONG PhsUpdateClassifierRule(IN void *pvContext,
  */
 ULONG PhsDeletePHSRule(IN void *pvContext, IN B_UINT16 uiVcid, IN B_UINT8 u8PHSI)
 {
-	ULONG lStatus = 0;
 	UINT nSFIndex = 0, nClsidIndex = 0;
 	struct bcm_phs_entry *pstServiceFlowEntry = NULL;
 	struct bcm_phs_classifier_table *pstClassifierRulesTable = NULL;
@@ -446,7 +445,7 @@ ULONG PhsDeletePHSRule(IN void *pvContext, IN B_UINT16 uiVcid, IN B_UINT8 u8PHSI
 			}
 		}
 	}
-	return lStatus;
+	return 0;
 }
 
 /*
@@ -467,7 +466,6 @@ ULONG PhsDeletePHSRule(IN void *pvContext, IN B_UINT16 uiVcid, IN B_UINT8 u8PHSI
  */
 ULONG PhsDeleteClassifierRule(IN void *pvContext, IN B_UINT16 uiVcid, IN B_UINT16 uiClsId)
 {
-	ULONG lStatus = 0;
 	UINT nSFIndex = 0, nClsidIndex = 0;
 	struct bcm_phs_entry *pstServiceFlowEntry = NULL;
 	struct bcm_phs_classifier_entry *pstClassifierEntry = NULL;
@@ -504,7 +502,7 @@ ULONG PhsDeleteClassifierRule(IN void *pvContext, IN B_UINT16 uiVcid, IN B_UINT1
 			memset(pstClassifierEntry, 0, sizeof(struct bcm_phs_classifier_entry));
 		}
 	}
-	return lStatus;
+	return 0;
 }
 
 /*
@@ -524,7 +522,6 @@ ULONG PhsDeleteClassifierRule(IN void *pvContext, IN B_UINT16 uiVcid, IN B_UINT1
  */
 ULONG PhsDeleteSFRules(IN void *pvContext, IN B_UINT16 uiVcid)
 {
-	ULONG lStatus = 0;
 	UINT nSFIndex = 0, nClsidIndex = 0;
 	struct bcm_phs_entry *pstServiceFlowEntry = NULL;
 	struct bcm_phs_classifier_table *pstClassifierRulesTable = NULL;
@@ -573,7 +570,7 @@ ULONG PhsDeleteSFRules(IN void *pvContext, IN B_UINT16 uiVcid)
 		pstServiceFlowEntry->uiVcid = 0;
 	}
 
-	return lStatus;
+	return 0;
 }
 
 /*
diff --git a/drivers/staging/bcm/Qos.c b/drivers/staging/bcm/Qos.c
index 4f315835ddf..0c742da8c6b 100644
--- a/drivers/staging/bcm/Qos.c
+++ b/drivers/staging/bcm/Qos.c
@@ -4,11 +4,18 @@ This file contains the routines related to Quality of Service.
 */
 #include "headers.h"
 
-static void EThCSGetPktInfo(struct bcm_mini_adapter *Adapter, PVOID pvEthPayload, struct bcm_eth_packet_info *pstEthCsPktInfo);
-static bool EThCSClassifyPkt(struct bcm_mini_adapter *Adapter, struct sk_buff* skb, struct bcm_eth_packet_info *pstEthCsPktInfo, struct bcm_classifier_rule *pstClassifierRule, B_UINT8 EthCSCupport);
+static void EThCSGetPktInfo(struct bcm_mini_adapter *Adapter,
+			    PVOID pvEthPayload,
+			    struct bcm_eth_packet_info *pstEthCsPktInfo);
 
-static USHORT	IpVersion4(struct bcm_mini_adapter *Adapter, struct iphdr *iphd,
-			   struct bcm_classifier_rule *pstClassifierRule);
+static bool EThCSClassifyPkt(struct bcm_mini_adapter *Adapter,
+			     struct sk_buff *skb,
+			     struct bcm_eth_packet_info *pstEthCsPktInfo,
+			     struct bcm_classifier_rule *pstClassifierRule,
+			     B_UINT8 EthCSCupport);
+
+static USHORT IpVersion4(struct bcm_mini_adapter *Adapter, struct iphdr *iphd,
+			 struct bcm_classifier_rule *pstClassifierRule);
 
 static VOID PruneQueue(struct bcm_mini_adapter *Adapter, INT iIndex);
 
@@ -33,14 +40,11 @@ static bool MatchSrcIpAddress(struct bcm_classifier_rule *pstClassifierRule, ULO
 	ulSrcIP = ntohl(ulSrcIP);
 	if (0 == pstClassifierRule->ucIPSourceAddressLength)
 		return TRUE;
-	for (ucLoopIndex = 0; ucLoopIndex < (pstClassifierRule->ucIPSourceAddressLength); ucLoopIndex++)
-	{
+	for (ucLoopIndex = 0; ucLoopIndex < (pstClassifierRule->ucIPSourceAddressLength); ucLoopIndex++) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Src Ip Address Mask:0x%x PacketIp:0x%x and Classification:0x%x", (UINT)pstClassifierRule->stSrcIpAddress.ulIpv4Mask[ucLoopIndex], (UINT)ulSrcIP, (UINT)pstClassifierRule->stSrcIpAddress.ulIpv6Addr[ucLoopIndex]);
 		if ((pstClassifierRule->stSrcIpAddress.ulIpv4Mask[ucLoopIndex] & ulSrcIP) ==
 				(pstClassifierRule->stSrcIpAddress.ulIpv4Addr[ucLoopIndex] & pstClassifierRule->stSrcIpAddress.ulIpv4Mask[ucLoopIndex]))
-		{
 			return TRUE;
-		}
 	}
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Src Ip Address Not Matched");
 	return false;
@@ -68,13 +72,10 @@ static bool MatchDestIpAddress(struct bcm_classifier_rule *pstClassifierRule, UL
 		return TRUE;
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Destination Ip Address 0x%x 0x%x 0x%x  ", (UINT)ulDestIP, (UINT)pstClassifierRule->stDestIpAddress.ulIpv4Mask[ucLoopIndex], (UINT)pstClassifierRule->stDestIpAddress.ulIpv4Addr[ucLoopIndex]);
 
-	for (ucLoopIndex = 0; ucLoopIndex < (pstClassifierRule->ucIPDestinationAddressLength); ucLoopIndex++)
-	{
+	for (ucLoopIndex = 0; ucLoopIndex < (pstClassifierRule->ucIPDestinationAddressLength); ucLoopIndex++) {
 		if ((pstClassifierRule->stDestIpAddress.ulIpv4Mask[ucLoopIndex] & ulDestIP) ==
 				(pstClassifierRule->stDestIpAddress.ulIpv4Addr[ucLoopIndex] & pstClassifierRule->stDestIpAddress.ulIpv4Mask[ucLoopIndex]))
-		{
 			return TRUE;
-		}
 	}
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Destination Ip Address Not Matched");
 	return false;
@@ -99,9 +100,8 @@ static bool MatchTos(struct bcm_classifier_rule *pstClassifierRule, UCHAR ucType
 		return TRUE;
 
 	if (((pstClassifierRule->ucTosMask & ucTypeOfService) <= pstClassifierRule->ucTosHigh) && ((pstClassifierRule->ucTosMask & ucTypeOfService) >= pstClassifierRule->ucTosLow))
-	{
 		return TRUE;
-	}
+
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Type Of Service Not Matched");
 	return false;
 }
@@ -123,13 +123,10 @@ bool MatchProtocol(struct bcm_classifier_rule *pstClassifierRule, UCHAR ucProtoc
 	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 	if (0 == pstClassifierRule->ucProtocolLength)
 		return TRUE;
-	for (ucLoopIndex = 0; ucLoopIndex < pstClassifierRule->ucProtocolLength; ucLoopIndex++)
-	{
+	for (ucLoopIndex = 0; ucLoopIndex < pstClassifierRule->ucProtocolLength; ucLoopIndex++) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Protocol:0x%X Classification Protocol:0x%X", ucProtocol, pstClassifierRule->ucProtocol[ucLoopIndex]);
 		if (pstClassifierRule->ucProtocol[ucLoopIndex] == ucProtocol)
-		{
 			return TRUE;
-		}
 	}
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Protocol Not Matched");
 	return false;
@@ -155,13 +152,10 @@ bool MatchSrcPort(struct bcm_classifier_rule *pstClassifierRule, USHORT ushSrcPo
 
 	if (0 == pstClassifierRule->ucSrcPortRangeLength)
 		return TRUE;
-	for (ucLoopIndex = 0; ucLoopIndex < pstClassifierRule->ucSrcPortRangeLength; ucLoopIndex++)
-	{
+	for (ucLoopIndex = 0; ucLoopIndex < pstClassifierRule->ucSrcPortRangeLength; ucLoopIndex++) {
 		if (ushSrcPort <= pstClassifierRule->usSrcPortRangeHi[ucLoopIndex] &&
 			ushSrcPort >= pstClassifierRule->usSrcPortRangeLo[ucLoopIndex])
-		{
 			return TRUE;
-		}
 	}
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Src Port: %x Not Matched ", ushSrcPort);
 	return false;
@@ -186,15 +180,12 @@ bool MatchDestPort(struct bcm_classifier_rule *pstClassifierRule, USHORT ushDest
 	if (0 == pstClassifierRule->ucDestPortRangeLength)
 		return TRUE;
 
-	for (ucLoopIndex = 0; ucLoopIndex < pstClassifierRule->ucDestPortRangeLength; ucLoopIndex++)
-	{
+	for (ucLoopIndex = 0; ucLoopIndex < pstClassifierRule->ucDestPortRangeLength; ucLoopIndex++) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Matching Port:0x%X   0x%X  0x%X", ushDestPort, pstClassifierRule->usDestPortRangeLo[ucLoopIndex], pstClassifierRule->usDestPortRangeHi[ucLoopIndex]);
 
 		if (ushDestPort <= pstClassifierRule->usDestPortRangeHi[ucLoopIndex] &&
 			ushDestPort >= pstClassifierRule->usDestPortRangeLo[ucLoopIndex])
-		{
 			return TRUE;
-		}
 	}
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "Dest Port: %x Not Matched", ushDestPort);
 	return false;
@@ -273,21 +264,13 @@ static USHORT	IpVersion4(struct bcm_mini_adapter *Adapter,
 		bClassificationSucceed = TRUE;
 	} while (0);
 
-	if (TRUE == bClassificationSucceed)
-	{
+	if (TRUE == bClassificationSucceed) {
 		INT iMatchedSFQueueIndex = 0;
 		iMatchedSFQueueIndex = SearchSfid(Adapter, pstClassifierRule->ulSFID);
 		if (iMatchedSFQueueIndex >= NO_OF_QUEUES)
-		{
 			bClassificationSucceed = false;
-		}
-		else
-		{
-			if (false == Adapter->PackInfo[iMatchedSFQueueIndex].bActive)
-			{
-				bClassificationSucceed = false;
-			}
-		}
+		else if (false == Adapter->PackInfo[iMatchedSFQueueIndex].bActive)
+			bClassificationSucceed = false;
 	}
 
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "IpVersion4 <==========");
@@ -299,8 +282,7 @@ VOID PruneQueueAllSF(struct bcm_mini_adapter *Adapter)
 {
 	UINT iIndex = 0;
 
-	for (iIndex = 0; iIndex < HiPriority; iIndex++)
-	{
+	for (iIndex = 0; iIndex < HiPriority; iIndex++) {
 		if (!Adapter->PackInfo[iIndex].bValid)
 			continue;
 
@@ -334,10 +316,10 @@ static VOID PruneQueue(struct bcm_mini_adapter *Adapter, INT iIndex)
 
 	spin_lock_bh(&Adapter->PackInfo[iIndex].SFQueueLock);
 
-	while (1)
+	while (1) {
 //	while((UINT)Adapter->PackInfo[iIndex].uiCurrentPacketsOnHost >
-//		SF_MAX_ALLOWED_PACKETS_TO_BACKUP)
-	{
+//		SF_MAX_ALLOWED_PACKETS_TO_BACKUP) {
+
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, PRUNE_QUEUE, DBG_LVL_ALL, "uiCurrentBytesOnHost:%x uiMaxBucketSize :%x",
 		Adapter->PackInfo[iIndex].uiCurrentBytesOnHost,
 		Adapter->PackInfo[iIndex].uiMaxBucketSize);
@@ -350,8 +332,7 @@ static VOID PruneQueue(struct bcm_mini_adapter *Adapter, INT iIndex)
 			((1000*(jiffies - *((B_UINT32 *)(PacketToDrop->cb)+SKB_CB_LATENCY_OFFSET))/HZ) <= Adapter->PackInfo[iIndex].uiMaxLatency))
 			break;
 
-		if (PacketToDrop)
-		{
+		if (PacketToDrop) {
 			if (netif_msg_tx_err(Adapter))
 				pr_info(PFX "%s: tx queue %d overlimit\n",
 					Adapter->dev->name, iIndex);
@@ -394,20 +375,16 @@ VOID flush_all_queues(struct bcm_mini_adapter *Adapter)
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, DUMP_INFO, DBG_LVL_ALL, "=====>");
 
 //	down(&Adapter->data_packet_queue_lock);
-	for (iQIndex = LowPriority; iQIndex < HiPriority; iQIndex++)
-	{
+	for (iQIndex = LowPriority; iQIndex < HiPriority; iQIndex++) {
 		struct net_device_stats *netstats = &Adapter->dev->stats;
 
 		spin_lock_bh(&Adapter->PackInfo[iQIndex].SFQueueLock);
-		while (Adapter->PackInfo[iQIndex].FirstTxQueue)
-		{
+		while (Adapter->PackInfo[iQIndex].FirstTxQueue) {
 			PacketToDrop = Adapter->PackInfo[iQIndex].FirstTxQueue;
-			if (PacketToDrop)
-			{
+			if (PacketToDrop) {
 				uiTotalPacketLength = PacketToDrop->len;
 				netstats->tx_dropped++;
-			}
-			else
+			} else
 				uiTotalPacketLength = 0;
 
 			DEQUEUEPACKET(Adapter->PackInfo[iQIndex].FirstTxQueue,
@@ -455,58 +432,42 @@ USHORT ClassifyPacket(struct bcm_mini_adapter *Adapter, struct sk_buff* skb)
 	*((UINT32*) (skb->cb) +SKB_CB_TCPACK_OFFSET) = 0;
 	EThCSGetPktInfo(Adapter, pvEThPayload, &stEthCsPktInfo);
 
-	switch (stEthCsPktInfo.eNwpktEthFrameType)
-	{
+	switch (stEthCsPktInfo.eNwpktEthFrameType) {
 		case eEth802LLCFrame:
-		{
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "ClassifyPacket : 802LLCFrame\n");
 			pIpHeader = pvEThPayload + sizeof(struct bcm_eth_llc_frame);
 			break;
-		}
-
 		case eEth802LLCSNAPFrame:
-		{
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "ClassifyPacket : 802LLC SNAP Frame\n");
 			pIpHeader = pvEThPayload + sizeof(struct bcm_eth_llc_snap_frame);
 			break;
-		}
 		case eEth802QVLANFrame:
-		{
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "ClassifyPacket : 802.1Q VLANFrame\n");
 			pIpHeader = pvEThPayload + sizeof(struct bcm_eth_q_frame);
 			break;
-		}
 		case eEthOtherFrame:
-		{
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "ClassifyPacket : ETH Other Frame\n");
 			pIpHeader = pvEThPayload + sizeof(struct bcm_ethernet2_frame);
 			break;
-		}
 		default:
-		{
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "ClassifyPacket : Unrecognized ETH Frame\n");
 			pIpHeader = pvEThPayload + sizeof(struct bcm_ethernet2_frame);
 			break;
-		}
 	}
 
-	if (stEthCsPktInfo.eNwpktIPFrameType == eIPv4Packet)
-	{
+	if (stEthCsPktInfo.eNwpktIPFrameType == eIPv4Packet) {
 		usCurrFragment = (ntohs(pIpHeader->frag_off) & IP_OFFSET);
 		if ((ntohs(pIpHeader->frag_off) & IP_MF) || usCurrFragment)
 			bFragmentedPkt = TRUE;
 
-		if (bFragmentedPkt)
-		{
+		if (bFragmentedPkt) {
 				//Fragmented  Packet. Get Frag Classifier Entry.
 			pstClassifierRule = GetFragIPClsEntry(Adapter, pIpHeader->id, pIpHeader->saddr);
-			if (pstClassifierRule)
-			{
+			if (pstClassifierRule) {
 					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "It is next Fragmented pkt");
 					bClassificationSucceed = TRUE;
 			}
-			if (!(ntohs(pIpHeader->frag_off) & IP_MF))
-			{
+			if (!(ntohs(pIpHeader->frag_off) & IP_MF)) {
 				//Fragmented Last packet . Remove Frag Classifier Entry
 				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "This is the last fragmented Pkt");
 				DelFragIPClsEntry(Adapter, pIpHeader->id, pIpHeader->saddr);
@@ -514,23 +475,19 @@ USHORT ClassifyPacket(struct bcm_mini_adapter *Adapter, struct sk_buff* skb)
 		}
 	}
 
-	for (uiLoopIndex = MAX_CLASSIFIERS - 1; uiLoopIndex >= 0; uiLoopIndex--)
-	{
+	for (uiLoopIndex = MAX_CLASSIFIERS - 1; uiLoopIndex >= 0; uiLoopIndex--) {
 		if (bClassificationSucceed)
 			break;
 		//Iterate through all classifiers which are already in order of priority
 		//to classify the packet until match found
-		do
-		{
-			if (false == Adapter->astClassifierTable[uiLoopIndex].bUsed)
-			{
+		do {
+			if (false == Adapter->astClassifierTable[uiLoopIndex].bUsed) {
 				bClassificationSucceed = false;
 				break;
 			}
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "Adapter->PackInfo[%d].bvalid=True\n", uiLoopIndex);
 
-			if (0 == Adapter->astClassifierTable[uiLoopIndex].ucDirection)
-			{
+			if (0 == Adapter->astClassifierTable[uiLoopIndex].ucDirection) {
 				bClassificationSucceed = false;//cannot be processed for classification.
 				break;						// it is a down link connection
 			}
@@ -543,11 +500,9 @@ USHORT ClassifyPacket(struct bcm_mini_adapter *Adapter, struct sk_buff* skb)
 				break;
 			}
 
-			if (Adapter->PackInfo[uiSfIndex].bEthCSSupport)
-			{
+			if (Adapter->PackInfo[uiSfIndex].bEthCSSupport) {
 
-				if (eEthUnsupportedFrame == stEthCsPktInfo.eNwpktEthFrameType)
-				{
+				if (eEthUnsupportedFrame == stEthCsPktInfo.eNwpktEthFrameType) {
 					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, " ClassifyPacket : Packet Not a Valid Supported Ethernet Frame\n");
 					bClassificationSucceed = false;
 					break;
@@ -558,17 +513,12 @@ USHORT ClassifyPacket(struct bcm_mini_adapter *Adapter, struct sk_buff* skb)
 				BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "Performing ETH CS Classification on Classifier Rule ID : %x Service Flow ID : %lx\n", pstClassifierRule->uiClassifierRuleIndex, Adapter->PackInfo[uiSfIndex].ulSFID);
 				bClassificationSucceed = EThCSClassifyPkt(Adapter, skb, &stEthCsPktInfo, pstClassifierRule, Adapter->PackInfo[uiSfIndex].bEthCSSupport);
 
-				if (!bClassificationSucceed)
-				{
+				if (!bClassificationSucceed) {
 					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "ClassifyPacket : Ethernet CS Classification Failed\n");
 					break;
 				}
-			}
-
-			else // No ETH Supported on this SF
-			{
-				if (eEthOtherFrame != stEthCsPktInfo.eNwpktEthFrameType)
-				{
+			} else {	// No ETH Supported on this SF
+				if (eEthOtherFrame != stEthCsPktInfo.eNwpktEthFrameType) {
 					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, " ClassifyPacket : Packet Not a 802.3 Ethernet Frame... hence not allowed over non-ETH CS SF\n");
 					bClassificationSucceed = false;
 					break;
@@ -577,11 +527,9 @@ USHORT ClassifyPacket(struct bcm_mini_adapter *Adapter, struct sk_buff* skb)
 
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "Proceeding to IP CS Clasification");
 
-			if (Adapter->PackInfo[uiSfIndex].bIPCSSupport)
-			{
+			if (Adapter->PackInfo[uiSfIndex].bIPCSSupport) {
 
-				if (stEthCsPktInfo.eNwpktIPFrameType == eNonIPPacket)
-				{
+				if (stEthCsPktInfo.eNwpktIPFrameType == eNonIPPacket) {
 					BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, " ClassifyPacket : Packet is Not an IP Packet\n");
 					bClassificationSucceed = false;
 					break;
@@ -598,31 +546,26 @@ USHORT ClassifyPacket(struct bcm_mini_adapter *Adapter, struct sk_buff* skb)
 		} while (0);
 	}
 
-	if (bClassificationSucceed == TRUE)
-	{
+	if (bClassificationSucceed == TRUE) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "CF id : %d, SF ID is =%lu", pstClassifierRule->uiClassifierRuleIndex, pstClassifierRule->ulSFID);
 
 		//Store The matched Classifier in SKB
 		*((UINT32*)(skb->cb)+SKB_CB_CLASSIFICATION_OFFSET) = pstClassifierRule->uiClassifierRuleIndex;
-		if ((TCP == pIpHeader->protocol) && !bFragmentedPkt && (ETH_AND_IP_HEADER_LEN + TCP_HEADER_LEN <= skb->len))
-		{
+		if ((TCP == pIpHeader->protocol) && !bFragmentedPkt && (ETH_AND_IP_HEADER_LEN + TCP_HEADER_LEN <= skb->len)) {
 			 IpHeaderLength   = pIpHeader->ihl;
 			 pTcpHeader = (struct bcm_tcp_header *)(((PUCHAR)pIpHeader)+(IpHeaderLength*4));
 			 TcpHeaderLength  = GET_TCP_HEADER_LEN(pTcpHeader->HeaderLength);
 
 			if ((pTcpHeader->ucFlags & TCP_ACK) &&
 			   (ntohs(pIpHeader->tot_len) == (IpHeaderLength*4)+(TcpHeaderLength*4)))
-			{
 				*((UINT32*) (skb->cb) + SKB_CB_TCPACK_OFFSET) = TCP_ACK;
-			}
 		}
 
 		usIndex = SearchSfid(Adapter, pstClassifierRule->ulSFID);
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "index is	=%d", usIndex);
 
 		//If this is the first fragment of a Fragmented pkt, add this CF. Only This CF should be used for all other fragment of this Pkt.
-		if (bFragmentedPkt && (usCurrFragment == 0))
-		{
+		if (bFragmentedPkt && (usCurrFragment == 0)) {
 			//First Fragment of Fragmented Packet. Create Frag CLS Entry
 			struct bcm_fragmented_packet_info stFragPktInfo;
 			stFragPktInfo.bUsed = TRUE;
@@ -648,9 +591,8 @@ static bool EthCSMatchSrcMACAddress(struct bcm_classifier_rule *pstClassifierRul
 	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 	if (pstClassifierRule->ucEthCSSrcMACLen == 0)
 		return TRUE;
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s\n", __FUNCTION__);
-	for (i = 0; i < MAC_ADDRESS_SIZE; i++)
-	{
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s\n", __func__);
+	for (i = 0; i < MAC_ADDRESS_SIZE; i++) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "SRC MAC[%x] = %x ClassifierRuleSrcMAC = %x Mask : %x\n", i, Mac[i], pstClassifierRule->au8EThCSSrcMAC[i], pstClassifierRule->au8EThCSSrcMACMask[i]);
 		if ((pstClassifierRule->au8EThCSSrcMAC[i] & pstClassifierRule->au8EThCSSrcMACMask[i]) !=
 			(Mac[i] & pstClassifierRule->au8EThCSSrcMACMask[i]))
@@ -665,9 +607,8 @@ static bool EthCSMatchDestMACAddress(struct bcm_classifier_rule *pstClassifierRu
 	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 	if (pstClassifierRule->ucEthCSDestMACLen == 0)
 		return TRUE;
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "%s\n", __FUNCTION__);
-	for (i = 0; i < MAC_ADDRESS_SIZE; i++)
-	{
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "%s\n", __func__);
+	for (i = 0; i < MAC_ADDRESS_SIZE; i++) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "SRC MAC[%x] = %x ClassifierRuleSrcMAC = %x Mask : %x\n", i, Mac[i], pstClassifierRule->au8EThCSDestMAC[i], pstClassifierRule->au8EThCSDestMACMask[i]);
 		if ((pstClassifierRule->au8EThCSDestMAC[i] & pstClassifierRule->au8EThCSDestMACMask[i]) !=
 			(Mac[i] & pstClassifierRule->au8EThCSDestMACMask[i]))
@@ -683,10 +624,9 @@ static bool EthCSMatchEThTypeSAP(struct bcm_classifier_rule *pstClassifierRule,
 		(pstClassifierRule->au8EthCSEtherType[0] == 0))
 		return TRUE;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s SrcEtherType:%x CLS EtherType[0]:%x\n", __FUNCTION__, pstEthCsPktInfo->usEtherType, pstClassifierRule->au8EthCSEtherType[0]);
-	if (pstClassifierRule->au8EthCSEtherType[0] == 1)
-	{
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s  CLS EtherType[1]:%x EtherType[2]:%x\n", __FUNCTION__, pstClassifierRule->au8EthCSEtherType[1], pstClassifierRule->au8EthCSEtherType[2]);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s SrcEtherType:%x CLS EtherType[0]:%x\n", __func__, pstEthCsPktInfo->usEtherType, pstClassifierRule->au8EthCSEtherType[0]);
+	if (pstClassifierRule->au8EthCSEtherType[0] == 1) {
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s  CLS EtherType[1]:%x EtherType[2]:%x\n", __func__, pstClassifierRule->au8EthCSEtherType[1], pstClassifierRule->au8EthCSEtherType[2]);
 
 		if (memcmp(&pstEthCsPktInfo->usEtherType, &pstClassifierRule->au8EthCSEtherType[1], 2) == 0)
 			return TRUE;
@@ -694,12 +634,11 @@ static bool EthCSMatchEThTypeSAP(struct bcm_classifier_rule *pstClassifierRule,
 			return false;
 	}
 
-	if (pstClassifierRule->au8EthCSEtherType[0] == 2)
-	{
+	if (pstClassifierRule->au8EthCSEtherType[0] == 2) {
 		if (eEth802LLCFrame != pstEthCsPktInfo->eNwpktEthFrameType)
 			return false;
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s  EthCS DSAP:%x EtherType[2]:%x\n", __FUNCTION__, pstEthCsPktInfo->ucDSAP, pstClassifierRule->au8EthCSEtherType[2]);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s  EthCS DSAP:%x EtherType[2]:%x\n", __func__, pstEthCsPktInfo->ucDSAP, pstClassifierRule->au8EthCSEtherType[2]);
 		if (pstEthCsPktInfo->ucDSAP == pstClassifierRule->au8EthCSEtherType[2])
 			return TRUE;
 		else
@@ -718,11 +657,10 @@ static bool EthCSMatchVLANRules(struct bcm_classifier_rule *pstClassifierRule, s
 	B_UINT8 uPriority = 0;
 	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(gblpnetdev);
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s  CLS UserPrio:%x CLS VLANID:%x\n", __FUNCTION__, ntohs(*((USHORT *)pstClassifierRule->usUserPriority)), pstClassifierRule->usVLANID);
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s  CLS UserPrio:%x CLS VLANID:%x\n", __func__, ntohs(*((USHORT *)pstClassifierRule->usUserPriority)), pstClassifierRule->usVLANID);
 
 	/* In case FW didn't receive the TLV, the priority field should be ignored */
-	if (pstClassifierRule->usValidityBitMap & (1<<PKT_CLASSIFICATION_USER_PRIORITY_VALID))
-	{
+	if (pstClassifierRule->usValidityBitMap & (1<<PKT_CLASSIFICATION_USER_PRIORITY_VALID)) {
 		if (pstEthCsPktInfo->eNwpktEthFrameType != eEth802QVLANFrame)
 				return false;
 
@@ -739,14 +677,13 @@ static bool EthCSMatchVLANRules(struct bcm_classifier_rule *pstClassifierRule, s
 
 	bClassificationSucceed = false;
 
-	if (pstClassifierRule->usValidityBitMap & (1<<PKT_CLASSIFICATION_VLANID_VALID))
-	{
+	if (pstClassifierRule->usValidityBitMap & (1<<PKT_CLASSIFICATION_VLANID_VALID)) {
 		if (pstEthCsPktInfo->eNwpktEthFrameType != eEth802QVLANFrame)
 				return false;
 
 		usVLANID = ntohs(*(USHORT *)(skb->data + sizeof(struct bcm_eth_header))) & 0xFFF;
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s  Pkt VLANID %x Priority: %d\n", __FUNCTION__, usVLANID, uPriority);
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "%s  Pkt VLANID %x Priority: %d\n", __func__, usVLANID, uPriority);
 
 		if (usVLANID == ((pstClassifierRule->usVLANID & 0xFFF0) >> 4))
 			bClassificationSucceed = TRUE;
@@ -800,32 +737,24 @@ static void EThCSGetPktInfo(struct bcm_mini_adapter *Adapter, PVOID pvEthPayload
 	USHORT u16Etype = ntohs(((struct bcm_eth_header *)pvEthPayload)->u16Etype);
 
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL,  "EthCSGetPktInfo : Eth Hdr Type : %X\n", u16Etype);
-	if (u16Etype > 0x5dc)
-	{
+	if (u16Etype > 0x5dc) {
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "EthCSGetPktInfo : ETH2 Frame\n");
 		//ETH2 Frame
-		if (u16Etype == ETHERNET_FRAMETYPE_802QVLAN)
-		{
+		if (u16Etype == ETHERNET_FRAMETYPE_802QVLAN) {
 			//802.1Q VLAN Header
 			pstEthCsPktInfo->eNwpktEthFrameType = eEth802QVLANFrame;
 			u16Etype = ((struct bcm_eth_q_frame *)pvEthPayload)->EthType;
 			//((ETH_CS_802_Q_FRAME*)pvEthPayload)->UserPriority
-		}
-		else
-		{
+		} else {
 			pstEthCsPktInfo->eNwpktEthFrameType = eEthOtherFrame;
 			u16Etype = ntohs(u16Etype);
 		}
-
-	}
-	else
-	{
+	} else {
 		//802.2 LLC
 		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, IPV4_DBG, DBG_LVL_ALL, "802.2 LLC Frame\n");
 		pstEthCsPktInfo->eNwpktEthFrameType = eEth802LLCFrame;
 		pstEthCsPktInfo->ucDSAP = ((struct bcm_eth_llc_frame *)pvEthPayload)->DSAP;
-		if (pstEthCsPktInfo->ucDSAP == 0xAA && ((struct bcm_eth_llc_frame *)pvEthPayload)->SSAP == 0xAA)
-		{
+		if (pstEthCsPktInfo->ucDSAP == 0xAA && ((struct bcm_eth_llc_frame *)pvEthPayload)->SSAP == 0xAA) {
 			//SNAP Frame
 			pstEthCsPktInfo->eNwpktEthFrameType = eEth802LLCSNAPFrame;
 			u16Etype = ((struct bcm_eth_llc_snap_frame *)pvEthPayload)->usEtherType;
diff --git a/drivers/staging/bcm/Transmit.c b/drivers/staging/bcm/Transmit.c
index 2ed4836b965..49767468ac2 100644
--- a/drivers/staging/bcm/Transmit.c
+++ b/drivers/staging/bcm/Transmit.c
@@ -46,12 +46,14 @@ int SendControlPacket(struct bcm_mini_adapter *Adapter, char *pControlPacket)
 
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "Tx");
 	if (!pControlPacket || !Adapter) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "Got NULL Control Packet or Adapter");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL,
+				"Got NULL Control Packet or Adapter");
 		return STATUS_FAILURE;
 	}
 	if ((atomic_read(&Adapter->CurrNumFreeTxDesc) <
 			((PLeader->PLength-1)/MAX_DEVICE_DESC_SIZE)+1))	{
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL, "NO FREE DESCRIPTORS TO SEND CONTROL PACKET");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_CONTROL, DBG_LVL_ALL,
+				"NO FREE DESCRIPTORS TO SEND CONTROL PACKET");
 		return STATUS_FAILURE;
 	}
 
@@ -109,7 +111,8 @@ int SetupNextSend(struct bcm_mini_adapter *Adapter,  struct sk_buff *Packet, USH
 			(UINT *)&Packet->len, Adapter->PackInfo[QueueIndex].bEthCSSupport);
 
 	if (status != STATUS_SUCCESS) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, NEXT_SEND, DBG_LVL_ALL, "PHS Transmit failed..\n");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, NEXT_SEND, DBG_LVL_ALL,
+				"PHS Transmit failed..\n");
 		goto errExit;
 	}
 
@@ -217,12 +220,15 @@ int tx_pkt_handler(struct bcm_mini_adapter *Adapter /**< pointer to adapter obje
 			Adapter->LinkStatus == SYNC_UP_REQUEST &&
 			!Adapter->bSyncUpRequestSent) {
 
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_PACKETS, DBG_LVL_ALL, "Calling LinkMessage");
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_PACKETS,
+					DBG_LVL_ALL, "Calling LinkMessage");
 			LinkMessage(Adapter);
 		}
 
 		if ((Adapter->IdleMode || Adapter->bShutStatus) && atomic_read(&Adapter->TotalPacketCount)) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_PACKETS, DBG_LVL_ALL, "Device in Low Power mode...waking up");
+			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX,
+					TX_PACKETS, DBG_LVL_ALL,
+					"Device in Low Power mode...waking up");
 			Adapter->usIdleModePattern = ABORT_IDLE_MODE;
 			Adapter->bWakeUpDevice = TRUE;
 			wake_up(&Adapter->process_rx_cntrlpkt);
@@ -232,7 +238,8 @@ int tx_pkt_handler(struct bcm_mini_adapter *Adapter /**< pointer to adapter obje
 		atomic_set(&Adapter->TxPktAvail, 0);
 	}
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_PACKETS, DBG_LVL_ALL, "Exiting the tx thread..\n");
+	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TX_PACKETS, DBG_LVL_ALL,
+			"Exiting the tx thread..\n");
 	Adapter->transmit_packet_thread = NULL;
 	return 0;
 }
diff --git a/drivers/staging/bcm/hostmibs.c b/drivers/staging/bcm/hostmibs.c
index 39ace5510c4..42d9004e357 100644
--- a/drivers/staging/bcm/hostmibs.c
+++ b/drivers/staging/bcm/hostmibs.c
@@ -9,37 +9,40 @@
 
 #include "headers.h"
 
-INT ProcessGetHostMibs(struct bcm_mini_adapter *Adapter, struct bcm_host_stats_mibs *pstHostMibs)
+INT ProcessGetHostMibs(struct bcm_mini_adapter *Adapter,
+		       struct bcm_host_stats_mibs *pstHostMibs)
 {
 	struct bcm_phs_entry *pstServiceFlowEntry = NULL;
 	struct bcm_phs_rule *pstPhsRule = NULL;
 	struct bcm_phs_classifier_table *pstClassifierTable = NULL;
 	struct bcm_phs_classifier_entry *pstClassifierRule = NULL;
-	struct bcm_phs_extension *pDeviceExtension = (struct bcm_phs_extension *) &Adapter->stBCMPhsContext;
-
-	UINT nClassifierIndex = 0, nPhsTableIndex = 0, nSfIndex = 0, uiIndex = 0;
+	struct bcm_phs_extension *pDeviceExtension = &Adapter->stBCMPhsContext;
+	UINT nClassifierIndex = 0;
+	UINT nPhsTableIndex = 0;
+	UINT nSfIndex = 0;
+	UINT uiIndex = 0;
 
 	if (pDeviceExtension == NULL) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, HOST_MIBS, DBG_LVL_ALL, "Invalid Device Extension\n");
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, HOST_MIBS,
+				DBG_LVL_ALL, "Invalid Device Extension\n");
 		return STATUS_FAILURE;
 	}
 
 	/* Copy the classifier Table */
-	for (nClassifierIndex = 0; nClassifierIndex < MAX_CLASSIFIERS; nClassifierIndex++) {
+	for (nClassifierIndex = 0; nClassifierIndex < MAX_CLASSIFIERS;
+							nClassifierIndex++) {
 		if (Adapter->astClassifierTable[nClassifierIndex].bUsed == TRUE)
-			memcpy((PVOID) &pstHostMibs->
-			       astClassifierTable[nClassifierIndex],
-			       (PVOID) &Adapter->
-			       astClassifierTable[nClassifierIndex],
+			memcpy(&pstHostMibs->astClassifierTable[nClassifierIndex],
+			       &Adapter->astClassifierTable[nClassifierIndex],
 			       sizeof(struct bcm_mibs_classifier_rule));
 	}
 
 	/* Copy the SF Table */
 	for (nSfIndex = 0; nSfIndex < NO_OF_QUEUES; nSfIndex++) {
 		if (Adapter->PackInfo[nSfIndex].bValid) {
-			memcpy((PVOID) &pstHostMibs->astSFtable[nSfIndex],
-			       (PVOID) &Adapter->PackInfo[nSfIndex],
-				sizeof(struct bcm_mibs_table));
+			memcpy(&pstHostMibs->astSFtable[nSfIndex],
+			       &Adapter->PackInfo[nSfIndex],
+			       sizeof(struct bcm_mibs_table));
 		} else {
 			/* If index in not valid,
 			 * don't process this for the PHS table.
@@ -68,9 +71,9 @@ INT ProcessGetHostMibs(struct bcm_mini_adapter *Adapter, struct bcm_host_stats_m
 				pstHostMibs->astPhsRulesTable[nPhsTableIndex].
 				    ulSFID = Adapter->PackInfo[nSfIndex].ulSFID;
 
-				memcpy(&pstHostMibs->
-				       astPhsRulesTable[nPhsTableIndex].u8PHSI,
-				       &pstPhsRule->u8PHSI, sizeof(struct bcm_phs_rule));
+				memcpy(&pstHostMibs->astPhsRulesTable[nPhsTableIndex].u8PHSI,
+				       &pstPhsRule->u8PHSI,
+				       sizeof(struct bcm_phs_rule));
 				nPhsTableIndex++;
 
 			}
@@ -82,26 +85,32 @@ INT ProcessGetHostMibs(struct bcm_mini_adapter *Adapter, struct bcm_host_stats_m
 	/* Copy other Host Statistics parameters */
 	pstHostMibs->stHostInfo.GoodTransmits = Adapter->dev->stats.tx_packets;
 	pstHostMibs->stHostInfo.GoodReceives = Adapter->dev->stats.rx_packets;
-	pstHostMibs->stHostInfo.CurrNumFreeDesc = atomic_read(&Adapter->CurrNumFreeTxDesc);
+	pstHostMibs->stHostInfo.CurrNumFreeDesc =
+				atomic_read(&Adapter->CurrNumFreeTxDesc);
 	pstHostMibs->stHostInfo.BEBucketSize = Adapter->BEBucketSize;
 	pstHostMibs->stHostInfo.rtPSBucketSize = Adapter->rtPSBucketSize;
 	pstHostMibs->stHostInfo.TimerActive = Adapter->TimerActive;
 	pstHostMibs->stHostInfo.u32TotalDSD = Adapter->u32TotalDSD;
 
-	memcpy(pstHostMibs->stHostInfo.aTxPktSizeHist, Adapter->aTxPktSizeHist, sizeof(UINT32) * MIBS_MAX_HIST_ENTRIES);
-	memcpy(pstHostMibs->stHostInfo.aRxPktSizeHist, Adapter->aRxPktSizeHist, sizeof(UINT32) * MIBS_MAX_HIST_ENTRIES);
+	memcpy(pstHostMibs->stHostInfo.aTxPktSizeHist, Adapter->aTxPktSizeHist,
+	       sizeof(UINT32) * MIBS_MAX_HIST_ENTRIES);
+	memcpy(pstHostMibs->stHostInfo.aRxPktSizeHist, Adapter->aRxPktSizeHist,
+	       sizeof(UINT32) * MIBS_MAX_HIST_ENTRIES);
 
 	return STATUS_SUCCESS;
 }
 
-VOID GetDroppedAppCntrlPktMibs(struct bcm_host_stats_mibs *pstHostMibs, struct bcm_tarang_data *pTarang)
+VOID GetDroppedAppCntrlPktMibs(struct bcm_host_stats_mibs *pstHostMibs,
+			       struct bcm_tarang_data *pTarang)
 {
 	memcpy(&(pstHostMibs->stDroppedAppCntrlMsgs),
 	       &(pTarang->stDroppedAppCntrlMsgs),
 	       sizeof(struct bcm_mibs_dropped_cntrl_msg));
 }
 
-VOID CopyMIBSExtendedSFParameters(struct bcm_mini_adapter *Adapter, struct bcm_connect_mgr_params *psfLocalSet, UINT uiSearchRuleIndex)
+VOID CopyMIBSExtendedSFParameters(struct bcm_mini_adapter *Adapter,
+				  struct bcm_connect_mgr_params *psfLocalSet,
+				  UINT uiSearchRuleIndex)
 {
 	struct bcm_mibs_parameters *t = &Adapter->PackInfo[uiSearchRuleIndex].stMibsExtServiceFlowTable;
 
diff --git a/drivers/staging/comedi/Kconfig b/drivers/staging/comedi/Kconfig
index 703c5d40ae5..5d56428d508 100644
--- a/drivers/staging/comedi/Kconfig
+++ b/drivers/staging/comedi/Kconfig
@@ -160,16 +160,18 @@ config COMEDI_PCL730
 	  Enable support for various simple ISA or PC/104 Digital I/O boards.
 	  These boards all use 8-bit I/O ports.
 
-	  Advantech PCL-730   isolated - 16 in/16 out  ttl - 16 in/16 out
-	  ICP ISO-730         isolated - 16 in/16 out  ttl - 16 in/16 out
-	  ADlink ACL-7130     isolated - 16 in/16 out  ttl - 16 in/16 out
-	  Advantech PCM-3730  isolated - 8 in/8 out    ttl - 16 in/16 out
-	  Advantech PCL-725   isolated - 8 in/8 out
-	  ICP P8R8-DIO        isolated - 8 in/8 out
-	  ADlink ACL-7225b    isolated - 16 in/16 out
-	  ICP P16R16-DIO      isolated - 16 in/16 out
-	  Advantech PCL-733   isolated - 32 in
-	  Advantech PCL-734   isolated - 32 out
+	  Advantech PCL-730             iso - 16 in/16 out  ttl - 16 in/16 out
+	  ICP ISO-730                   iso - 16 in/16 out  ttl - 16 in/16 out
+	  ADlink ACL-7130               iso - 16 in/16 out  ttl - 16 in/16 out
+	  Advantech PCM-3730            iso - 8 in/8 out    ttl - 16 in/16 out
+	  Advantech PCL-725             iso - 8 in/8 out
+	  ICP P8R8-DIO                  iso - 8 in/8 out
+	  ADlink ACL-7225b              iso - 16 in/16 out
+	  ICP P16R16-DIO                iso - 16 in/16 out
+	  Advantech PCL-733             iso - 32 in
+	  Advantech PCL-734             iso - 32 out
+	  Diamond Systems OPMM-1616-XT  iso - 16 in/16 out
+	  Diamond Systems PEARL-MM-P    iso - 16 out
 
 	  To compile this driver as a module, choose M here: the module will be
 	  called pcl730.
diff --git a/drivers/staging/comedi/TODO b/drivers/staging/comedi/TODO
index fa8da9aada3..b68fbdb5eeb 100644
--- a/drivers/staging/comedi/TODO
+++ b/drivers/staging/comedi/TODO
@@ -3,7 +3,6 @@ TODO:
 	- Lindent
 	- remove all wrappers
 	- audit userspace interface
-	- reserve major number
 	- cleanup the individual comedi drivers as well
 
 Please send patches to Greg Kroah-Hartman <greg@kroah.com> and
diff --git a/drivers/staging/comedi/comedi_buf.c b/drivers/staging/comedi/comedi_buf.c
index 924fce97798..df4a9c4bca3 100644
--- a/drivers/staging/comedi/comedi_buf.c
+++ b/drivers/staging/comedi/comedi_buf.c
@@ -61,6 +61,8 @@ static void __comedi_buf_free(struct comedi_device *dev,
 			      struct comedi_subdevice *s)
 {
 	struct comedi_async *async = s->async;
+	struct comedi_buf_map *bm;
+	unsigned long flags;
 
 	if (async->prealloc_buf) {
 		vunmap(async->prealloc_buf);
@@ -68,8 +70,11 @@ static void __comedi_buf_free(struct comedi_device *dev,
 		async->prealloc_bufsz = 0;
 	}
 
-	comedi_buf_map_put(async->buf_map);
+	spin_lock_irqsave(&s->spin_lock, flags);
+	bm = async->buf_map;
 	async->buf_map = NULL;
+	spin_unlock_irqrestore(&s->spin_lock, flags);
+	comedi_buf_map_put(bm);
 }
 
 static void __comedi_buf_alloc(struct comedi_device *dev,
@@ -80,6 +85,7 @@ static void __comedi_buf_alloc(struct comedi_device *dev,
 	struct page **pages = NULL;
 	struct comedi_buf_map *bm;
 	struct comedi_buf_page *buf;
+	unsigned long flags;
 	unsigned i;
 
 	if (!IS_ENABLED(CONFIG_HAS_DMA) && s->async_dma_dir != DMA_NONE) {
@@ -92,8 +98,10 @@ static void __comedi_buf_alloc(struct comedi_device *dev,
 	if (!bm)
 		return;
 
-	async->buf_map = bm;
 	kref_init(&bm->refcount);
+	spin_lock_irqsave(&s->spin_lock, flags);
+	async->buf_map = bm;
+	spin_unlock_irqrestore(&s->spin_lock, flags);
 	bm->dma_dir = s->async_dma_dir;
 	if (bm->dma_dir != DMA_NONE)
 		/* Need ref to hardware device to free buffer later. */
@@ -127,7 +135,9 @@ static void __comedi_buf_alloc(struct comedi_device *dev,
 
 		pages[i] = virt_to_page(buf->virt_addr);
 	}
+	spin_lock_irqsave(&s->spin_lock, flags);
 	bm->n_pages = i;
+	spin_unlock_irqrestore(&s->spin_lock, flags);
 
 	/* vmap the prealloc_buf if all the pages were allocated */
 	if (i == n_pages)
@@ -150,9 +160,32 @@ int comedi_buf_map_put(struct comedi_buf_map *bm)
 	return 1;
 }
 
-bool comedi_buf_is_mmapped(struct comedi_async *async)
+/* returns s->async->buf_map and increments its kref refcount */
+struct comedi_buf_map *
+comedi_buf_map_from_subdev_get(struct comedi_subdevice *s)
+{
+	struct comedi_async *async = s->async;
+	struct comedi_buf_map *bm = NULL;
+	unsigned long flags;
+
+	if (!async)
+		return NULL;
+
+	spin_lock_irqsave(&s->spin_lock, flags);
+	bm = async->buf_map;
+	/* only want it if buffer pages allocated */
+	if (bm && bm->n_pages)
+		comedi_buf_map_get(bm);
+	else
+		bm = NULL;
+	spin_unlock_irqrestore(&s->spin_lock, flags);
+
+	return bm;
+}
+
+bool comedi_buf_is_mmapped(struct comedi_subdevice *s)
 {
-	struct comedi_buf_map *bm = async->buf_map;
+	struct comedi_buf_map *bm = s->async->buf_map;
 
 	return bm && (atomic_read(&bm->refcount.refcount) > 1);
 }
@@ -189,8 +222,10 @@ int comedi_buf_alloc(struct comedi_device *dev, struct comedi_subdevice *s,
 	return 0;
 }
 
-void comedi_buf_reset(struct comedi_async *async)
+void comedi_buf_reset(struct comedi_subdevice *s)
 {
+	struct comedi_async *async = s->async;
+
 	async->buf_write_alloc_count = 0;
 	async->buf_write_count = 0;
 	async->buf_read_alloc_count = 0;
@@ -208,18 +243,20 @@ void comedi_buf_reset(struct comedi_async *async)
 	async->events = 0;
 }
 
-static unsigned int comedi_buf_write_n_available(struct comedi_async *async)
+static unsigned int comedi_buf_write_n_available(struct comedi_subdevice *s)
 {
+	struct comedi_async *async = s->async;
 	unsigned int free_end = async->buf_read_count + async->prealloc_bufsz;
 
 	return free_end - async->buf_write_alloc_count;
 }
 
-static unsigned int __comedi_buf_write_alloc(struct comedi_async *async,
+static unsigned int __comedi_buf_write_alloc(struct comedi_subdevice *s,
 					     unsigned int nbytes,
 					     int strict)
 {
-	unsigned int available = comedi_buf_write_n_available(async);
+	struct comedi_async *async = s->async;
+	unsigned int available = comedi_buf_write_n_available(s);
 
 	if (nbytes > available)
 		nbytes = strict ? 0 : available;
@@ -236,10 +273,10 @@ static unsigned int __comedi_buf_write_alloc(struct comedi_async *async,
 }
 
 /* allocates chunk for the writer from free buffer space */
-unsigned int comedi_buf_write_alloc(struct comedi_async *async,
+unsigned int comedi_buf_write_alloc(struct comedi_subdevice *s,
 				    unsigned int nbytes)
 {
-	return __comedi_buf_write_alloc(async, nbytes, 0);
+	return __comedi_buf_write_alloc(s, nbytes, 0);
 }
 EXPORT_SYMBOL_GPL(comedi_buf_write_alloc);
 
@@ -247,10 +284,10 @@ EXPORT_SYMBOL_GPL(comedi_buf_write_alloc);
  * munging is applied to data by core as it passes between user
  * and kernel space
  */
-static unsigned int comedi_buf_munge(struct comedi_async *async,
+static unsigned int comedi_buf_munge(struct comedi_subdevice *s,
 				     unsigned int num_bytes)
 {
-	struct comedi_subdevice *s = async->subdevice;
+	struct comedi_async *async = s->async;
 	unsigned int count = 0;
 	const unsigned num_sample_bytes = bytes_per_sample(s);
 
@@ -290,23 +327,26 @@ static unsigned int comedi_buf_munge(struct comedi_async *async,
 	return count;
 }
 
-unsigned int comedi_buf_write_n_allocated(struct comedi_async *async)
+unsigned int comedi_buf_write_n_allocated(struct comedi_subdevice *s)
 {
+	struct comedi_async *async = s->async;
+
 	return async->buf_write_alloc_count - async->buf_write_count;
 }
 
 /* transfers a chunk from writer to filled buffer space */
-unsigned int comedi_buf_write_free(struct comedi_async *async,
+unsigned int comedi_buf_write_free(struct comedi_subdevice *s,
 				   unsigned int nbytes)
 {
-	unsigned int allocated = comedi_buf_write_n_allocated(async);
+	struct comedi_async *async = s->async;
+	unsigned int allocated = comedi_buf_write_n_allocated(s);
 
 	if (nbytes > allocated)
 		nbytes = allocated;
 
 	async->buf_write_count += nbytes;
 	async->buf_write_ptr += nbytes;
-	comedi_buf_munge(async, async->buf_write_count - async->munge_count);
+	comedi_buf_munge(s, async->buf_write_count - async->munge_count);
 	if (async->buf_write_ptr >= async->prealloc_bufsz)
 		async->buf_write_ptr %= async->prealloc_bufsz;
 
@@ -314,8 +354,9 @@ unsigned int comedi_buf_write_free(struct comedi_async *async,
 }
 EXPORT_SYMBOL_GPL(comedi_buf_write_free);
 
-unsigned int comedi_buf_read_n_available(struct comedi_async *async)
+unsigned int comedi_buf_read_n_available(struct comedi_subdevice *s)
 {
+	struct comedi_async *async = s->async;
 	unsigned num_bytes;
 
 	if (!async)
@@ -334,9 +375,10 @@ unsigned int comedi_buf_read_n_available(struct comedi_async *async)
 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,
+unsigned int comedi_buf_read_alloc(struct comedi_subdevice *s,
 				   unsigned int nbytes)
 {
+	struct comedi_async *async = s->async;
 	unsigned int available;
 
 	available = async->munge_count - async->buf_read_alloc_count;
@@ -361,9 +403,10 @@ static unsigned int comedi_buf_read_n_allocated(struct comedi_async *async)
 }
 
 /* transfers control of a chunk from reader to free buffer space */
-unsigned int comedi_buf_read_free(struct comedi_async *async,
+unsigned int comedi_buf_read_free(struct comedi_subdevice *s,
 				  unsigned int nbytes)
 {
+	struct comedi_async *async = s->async;
 	unsigned int allocated;
 
 	/*
@@ -383,36 +426,39 @@ unsigned int comedi_buf_read_free(struct comedi_async *async,
 }
 EXPORT_SYMBOL_GPL(comedi_buf_read_free);
 
-int comedi_buf_put(struct comedi_async *async, unsigned short x)
+int comedi_buf_put(struct comedi_subdevice *s, unsigned short x)
 {
-	unsigned int n = __comedi_buf_write_alloc(async, sizeof(short), 1);
+	struct comedi_async *async = s->async;
+	unsigned int n = __comedi_buf_write_alloc(s, sizeof(short), 1);
 
 	if (n < sizeof(short)) {
 		async->events |= COMEDI_CB_ERROR;
 		return 0;
 	}
 	*(unsigned short *)(async->prealloc_buf + async->buf_write_ptr) = x;
-	comedi_buf_write_free(async, sizeof(short));
+	comedi_buf_write_free(s, sizeof(short));
 	return 1;
 }
 EXPORT_SYMBOL_GPL(comedi_buf_put);
 
-int comedi_buf_get(struct comedi_async *async, unsigned short *x)
+int comedi_buf_get(struct comedi_subdevice *s, unsigned short *x)
 {
-	unsigned int n = comedi_buf_read_n_available(async);
+	struct comedi_async *async = s->async;
+	unsigned int n = comedi_buf_read_n_available(s);
 
 	if (n < sizeof(short))
 		return 0;
-	comedi_buf_read_alloc(async, sizeof(short));
+	comedi_buf_read_alloc(s, sizeof(short));
 	*x = *(unsigned short *)(async->prealloc_buf + async->buf_read_ptr);
-	comedi_buf_read_free(async, sizeof(short));
+	comedi_buf_read_free(s, sizeof(short));
 	return 1;
 }
 EXPORT_SYMBOL_GPL(comedi_buf_get);
 
-void comedi_buf_memcpy_to(struct comedi_async *async, unsigned int offset,
+void comedi_buf_memcpy_to(struct comedi_subdevice *s, unsigned int offset,
 			  const void *data, unsigned int num_bytes)
 {
+	struct comedi_async *async = s->async;
 	unsigned int write_ptr = async->buf_write_ptr + offset;
 
 	if (write_ptr >= async->prealloc_bufsz)
@@ -436,10 +482,11 @@ void comedi_buf_memcpy_to(struct comedi_async *async, unsigned int offset,
 }
 EXPORT_SYMBOL_GPL(comedi_buf_memcpy_to);
 
-void comedi_buf_memcpy_from(struct comedi_async *async, unsigned int offset,
+void comedi_buf_memcpy_from(struct comedi_subdevice *s, unsigned int offset,
 			    void *dest, unsigned int nbytes)
 {
 	void *src;
+	struct comedi_async *async = s->async;
 	unsigned int read_ptr = async->buf_read_ptr + offset;
 
 	if (read_ptr >= async->prealloc_bufsz)
diff --git a/drivers/staging/comedi/comedi_fops.c b/drivers/staging/comedi/comedi_fops.c
index ea6dc36d753..9d99fb3c18a 100644
--- a/drivers/staging/comedi/comedi_fops.c
+++ b/drivers/staging/comedi/comedi_fops.c
@@ -238,9 +238,9 @@ comedi_write_subdevice(const struct comedi_device *dev, unsigned int minor)
 }
 
 static int resize_async_buffer(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_async *async, unsigned new_size)
+			       struct comedi_subdevice *s, unsigned new_size)
 {
+	struct comedi_async *async = s->async;
 	int retval;
 
 	if (new_size > async->max_bufsize)
@@ -251,7 +251,7 @@ static int resize_async_buffer(struct comedi_device *dev,
 			"subdevice is busy, cannot resize buffer\n");
 		return -EBUSY;
 	}
-	if (comedi_buf_is_mmapped(async)) {
+	if (comedi_buf_is_mmapped(s)) {
 		dev_dbg(dev->class_dev,
 			"subdevice is mmapped, cannot resize buffer\n");
 		return -EBUSY;
@@ -380,7 +380,7 @@ static ssize_t read_buffer_kb_store(struct device *csdev,
 	mutex_lock(&dev->mutex);
 	s = comedi_read_subdevice(dev, minor);
 	if (s && (s->subdev_flags & SDF_CMD_READ) && s->async)
-		err = resize_async_buffer(dev, s, s->async, size);
+		err = resize_async_buffer(dev, s, size);
 	else
 		err = -EINVAL;
 	mutex_unlock(&dev->mutex);
@@ -493,7 +493,7 @@ static ssize_t write_buffer_kb_store(struct device *csdev,
 	mutex_lock(&dev->mutex);
 	s = comedi_write_subdevice(dev, minor);
 	if (s && (s->subdev_flags & SDF_CMD_WRITE) && s->async)
-		err = resize_async_buffer(dev, s, s->async, size);
+		err = resize_async_buffer(dev, s, size);
 	else
 		err = -EINVAL;
 	mutex_unlock(&dev->mutex);
@@ -583,7 +583,7 @@ static void do_become_nonbusy(struct comedi_device *dev,
 
 	comedi_set_subdevice_runflags(s, SRF_RUNNING, 0);
 	if (async) {
-		comedi_buf_reset(async);
+		comedi_buf_reset(s);
 		async->inttrig = NULL;
 		kfree(async->cmd.chanlist);
 		async->cmd.chanlist = NULL;
@@ -635,7 +635,7 @@ static int is_device_busy(struct comedi_device *dev)
 		s = &dev->subdevices[i];
 		if (s->busy)
 			return 1;
-		if (s->async && comedi_buf_is_mmapped(s->async))
+		if (s->async && comedi_buf_is_mmapped(s))
 			return 1;
 	}
 
@@ -668,6 +668,7 @@ static int do_devconfig_ioctl(struct comedi_device *dev,
 			return -EBUSY;
 		if (dev->attached) {
 			struct module *driver_module = dev->driver->module;
+
 			comedi_device_detach(dev);
 			module_put(driver_module);
 		}
@@ -740,7 +741,7 @@ static int do_bufconfig_ioctl(struct comedi_device *dev,
 	}
 
 	if (bc.size) {
-		retval = resize_async_buffer(dev, s, async, bc.size);
+		retval = resize_async_buffer(dev, s, bc.size);
 		if (retval < 0)
 			return retval;
 	}
@@ -992,8 +993,8 @@ static int do_bufinfo_ioctl(struct comedi_device *dev,
 		return -EACCES;
 
 	if (bi.bytes_read && (s->subdev_flags & SDF_CMD_READ)) {
-		bi.bytes_read = comedi_buf_read_alloc(async, bi.bytes_read);
-		comedi_buf_read_free(async, bi.bytes_read);
+		bi.bytes_read = comedi_buf_read_alloc(s, bi.bytes_read);
+		comedi_buf_read_free(s, bi.bytes_read);
 
 		if (comedi_is_subdevice_idle(s) &&
 		    async->buf_write_count == async->buf_read_count) {
@@ -1003,8 +1004,8 @@ static int do_bufinfo_ioctl(struct comedi_device *dev,
 
 	if (bi.bytes_written && (s->subdev_flags & SDF_CMD_WRITE)) {
 		bi.bytes_written =
-		    comedi_buf_write_alloc(async, bi.bytes_written);
-		comedi_buf_write_free(async, bi.bytes_written);
+		    comedi_buf_write_alloc(s, bi.bytes_written);
+		comedi_buf_write_free(s, bi.bytes_written);
 	}
 
 copyback_position:
@@ -1435,13 +1436,15 @@ static int __comedi_get_user_cmd(struct comedi_device *dev,
 	s = &dev->subdevices[cmd->subdev];
 
 	if (s->type == COMEDI_SUBD_UNUSED) {
-		dev_dbg(dev->class_dev, "%d not valid subdevice\n", cmd->subdev);
+		dev_dbg(dev->class_dev, "%d not valid subdevice\n",
+			cmd->subdev);
 		return -EIO;
 	}
 
 	if (!s->do_cmd || !s->do_cmdtest || !s->async) {
 		dev_dbg(dev->class_dev,
-			"subdevice %d does not support commands\n", cmd->subdev);
+			"subdevice %d does not support commands\n",
+			cmd->subdev);
 		return -EIO;
 	}
 
@@ -1554,7 +1557,7 @@ static int do_cmd_ioctl(struct comedi_device *dev,
 		goto cleanup;
 	}
 
-	comedi_buf_reset(async);
+	comedi_buf_reset(s);
 
 	async->cb_mask =
 	    COMEDI_CB_EOA | COMEDI_CB_BLOCK | COMEDI_CB_ERROR |
@@ -1597,7 +1600,6 @@ static int do_cmdtest_ioctl(struct comedi_device *dev,
 {
 	struct comedi_cmd cmd;
 	struct comedi_subdevice *s;
-	unsigned int *chanlist = NULL;
 	unsigned int __user *user_chanlist;
 	int ret;
 
@@ -1626,8 +1628,6 @@ static int do_cmdtest_ioctl(struct comedi_device *dev,
 		ret = -EFAULT;
 	}
 
-	kfree(chanlist);
-
 	return ret;
 }
 
@@ -1926,14 +1926,21 @@ static int comedi_mmap(struct file *file, struct vm_area_struct *vma)
 	struct comedi_device *dev = file->private_data;
 	struct comedi_subdevice *s;
 	struct comedi_async *async;
-	struct comedi_buf_map *bm;
+	struct comedi_buf_map *bm = NULL;
 	unsigned long start = vma->vm_start;
 	unsigned long size;
 	int n_pages;
 	int i;
 	int retval;
 
-	mutex_lock(&dev->mutex);
+	/*
+	 * 'trylock' avoids circular dependency with current->mm->mmap_sem
+	 * and down-reading &dev->attach_lock should normally succeed without
+	 * contention unless the device is in the process of being attached
+	 * or detached.
+	 */
+	if (!down_read_trylock(&dev->attach_lock))
+		return -EAGAIN;
 
 	if (!dev->attached) {
 		dev_dbg(dev->class_dev, "no driver attached\n");
@@ -1973,7 +1980,9 @@ static int comedi_mmap(struct file *file, struct vm_area_struct *vma)
 	}
 
 	n_pages = size >> PAGE_SHIFT;
-	bm = async->buf_map;
+
+	/* get reference to current buf map (if any) */
+	bm = comedi_buf_map_from_subdev_get(s);
 	if (!bm || n_pages > bm->n_pages) {
 		retval = -EINVAL;
 		goto done;
@@ -1997,7 +2006,8 @@ static int comedi_mmap(struct file *file, struct vm_area_struct *vma)
 
 	retval = 0;
 done:
-	mutex_unlock(&dev->mutex);
+	up_read(&dev->attach_lock);
+	comedi_buf_map_put(bm);	/* put reference to buf map - okay if NULL */
 	return retval;
 }
 
@@ -2019,18 +2029,18 @@ static unsigned int comedi_poll(struct file *file, poll_table *wait)
 	if (s && s->async) {
 		poll_wait(file, &s->async->wait_head, wait);
 		if (!s->busy || !comedi_is_subdevice_running(s) ||
-		    comedi_buf_read_n_available(s->async) > 0)
+		    comedi_buf_read_n_available(s) > 0)
 			mask |= POLLIN | POLLRDNORM;
 	}
 
 	s = comedi_write_subdevice(dev, minor);
 	if (s && s->async) {
-		unsigned int bps = bytes_per_sample(s->async->subdevice);
+		unsigned int bps = bytes_per_sample(s);
 
 		poll_wait(file, &s->async->wait_head, wait);
-		comedi_buf_write_alloc(s->async, s->async->prealloc_bufsz);
+		comedi_buf_write_alloc(s, s->async->prealloc_bufsz);
 		if (!s->busy || !comedi_is_subdevice_running(s) ||
-		    comedi_buf_write_n_allocated(s->async) >= bps)
+		    comedi_buf_write_n_allocated(s) >= bps)
 			mask |= POLLOUT | POLLWRNORM;
 	}
 
@@ -2126,9 +2136,9 @@ static ssize_t comedi_write(struct file *file, const char __user *buf,
 		m = n;
 		if (async->buf_write_ptr + m > async->prealloc_bufsz)
 			m = async->prealloc_bufsz - async->buf_write_ptr;
-		comedi_buf_write_alloc(async, async->prealloc_bufsz);
-		if (m > comedi_buf_write_n_allocated(async))
-			m = comedi_buf_write_n_allocated(async);
+		comedi_buf_write_alloc(s, async->prealloc_bufsz);
+		if (m > comedi_buf_write_n_allocated(s))
+			m = comedi_buf_write_n_allocated(s);
 		if (m < n)
 			n = m;
 
@@ -2157,7 +2167,7 @@ static ssize_t comedi_write(struct file *file, const char __user *buf,
 			n -= m;
 			retval = -EFAULT;
 		}
-		comedi_buf_write_free(async, n);
+		comedi_buf_write_free(s, n);
 
 		count += n;
 		nbytes -= n;
@@ -2219,7 +2229,7 @@ static ssize_t comedi_read(struct file *file, char __user *buf, size_t nbytes,
 
 		n = nbytes;
 
-		m = comedi_buf_read_n_available(async);
+		m = comedi_buf_read_n_available(s);
 		/* printk("%d available\n",m); */
 		if (async->buf_read_ptr + m > async->prealloc_bufsz)
 			m = async->prealloc_bufsz - async->buf_read_ptr;
@@ -2262,8 +2272,8 @@ static ssize_t comedi_read(struct file *file, char __user *buf, size_t nbytes,
 			retval = -EFAULT;
 		}
 
-		comedi_buf_read_alloc(async, n);
-		comedi_buf_read_free(async, n);
+		comedi_buf_read_alloc(s, n);
+		comedi_buf_read_free(s, n);
 
 		count += n;
 		nbytes -= n;
@@ -2317,46 +2327,12 @@ static int comedi_open(struct inode *inode, struct file *file)
 		return -ENODEV;
 	}
 
-	/* 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 -> autoload
-	 * case: user opens device, unattached, in_request_module -> fail
-	 * case: root opens device, attached -> ok
-	 * case: root opens device, unattached, in_request_module -> ok
-	 *   (typically called from modprobe)
-	 * case: root opens device, unattached, !in_request_module -> autoload
-	 *
-	 * The last could be changed to "-> ok", which would deny root
-	 * autoloading.
-	 */
 	mutex_lock(&dev->mutex);
-	if (dev->attached)
-		goto ok;
-	if (!capable(CAP_NET_ADMIN) && dev->in_request_module) {
-		dev_dbg(dev->class_dev, "in request module\n");
-		rc = -ENODEV;
-		goto out;
-	}
-	if (capable(CAP_NET_ADMIN) && dev->in_request_module)
-		goto ok;
-
-	dev->in_request_module = true;
-
-#ifdef CONFIG_KMOD
-	mutex_unlock(&dev->mutex);
-	request_module("char-major-%i-%i", COMEDI_MAJOR, dev->minor);
-	mutex_lock(&dev->mutex);
-#endif
-
-	dev->in_request_module = false;
-
 	if (!dev->attached && !capable(CAP_NET_ADMIN)) {
 		dev_dbg(dev->class_dev, "not attached and not CAP_NET_ADMIN\n");
 		rc = -ENODEV;
 		goto out;
 	}
-ok:
 	if (dev->attached && dev->use_count == 0) {
 		if (!try_module_get(dev->driver->module)) {
 			rc = -ENOSYS;
@@ -2644,6 +2620,7 @@ static int __init comedi_init(void)
 	/* create devices files for legacy/manual use */
 	for (i = 0; i < comedi_num_legacy_minors; i++) {
 		struct comedi_device *dev;
+
 		dev = comedi_alloc_board_minor(NULL);
 		if (IS_ERR(dev)) {
 			comedi_cleanup_board_minors();
diff --git a/drivers/staging/comedi/comedi_internal.h b/drivers/staging/comedi/comedi_internal.h
index 9a746570f16..e978c223f5b 100644
--- a/drivers/staging/comedi/comedi_internal.h
+++ b/drivers/staging/comedi/comedi_internal.h
@@ -15,11 +15,13 @@ void comedi_free_subdevice_minor(struct comedi_subdevice *s);
 
 int comedi_buf_alloc(struct comedi_device *dev, struct comedi_subdevice *s,
 		     unsigned long new_size);
-void comedi_buf_reset(struct comedi_async *async);
-bool comedi_buf_is_mmapped(struct comedi_async *async);
+void comedi_buf_reset(struct comedi_subdevice *s);
+bool comedi_buf_is_mmapped(struct comedi_subdevice *s);
 void comedi_buf_map_get(struct comedi_buf_map *bm);
 int comedi_buf_map_put(struct comedi_buf_map *bm);
-unsigned int comedi_buf_write_n_allocated(struct comedi_async *async);
+struct comedi_buf_map *comedi_buf_map_from_subdev_get(
+		struct comedi_subdevice *s);
+unsigned int comedi_buf_write_n_allocated(struct comedi_subdevice *s);
 void comedi_device_cancel_all(struct comedi_device *dev);
 
 extern unsigned int comedi_default_buf_size_kb;
diff --git a/drivers/staging/comedi/comedidev.h b/drivers/staging/comedi/comedidev.h
index d46123a9758..8f4e44bfbe0 100644
--- a/drivers/staging/comedi/comedidev.h
+++ b/drivers/staging/comedi/comedidev.h
@@ -108,8 +108,6 @@ struct comedi_buf_map {
 };
 
 struct comedi_async {
-	struct comedi_subdevice *subdevice;
-
 	void *prealloc_buf;	/* pre-allocated buffer */
 	unsigned int prealloc_bufsz;	/* buffer size, in bytes */
 	struct comedi_buf_map *buf_map;	/* map of buffer pages */
@@ -182,7 +180,6 @@ struct comedi_device {
 	const char *board_name;
 	const void *board_ptr;
 	bool attached:1;
-	bool in_request_module:1;
 	bool ioenabled:1;
 	spinlock_t spinlock;
 	struct mutex mutex;
@@ -336,19 +333,19 @@ static inline unsigned int bytes_per_sample(const struct comedi_subdevice *subd)
  */
 int comedi_set_hw_dev(struct comedi_device *dev, struct device *hw_dev);
 
-unsigned int comedi_buf_write_alloc(struct comedi_async *, unsigned int);
-unsigned int comedi_buf_write_free(struct comedi_async *, unsigned int);
+unsigned int comedi_buf_write_alloc(struct comedi_subdevice *s, unsigned int n);
+unsigned int comedi_buf_write_free(struct comedi_subdevice *s, unsigned int n);
 
-unsigned int comedi_buf_read_n_available(struct comedi_async *);
-unsigned int comedi_buf_read_alloc(struct comedi_async *, unsigned int);
-unsigned int comedi_buf_read_free(struct comedi_async *, unsigned int);
+unsigned int comedi_buf_read_n_available(struct comedi_subdevice *s);
+unsigned int comedi_buf_read_alloc(struct comedi_subdevice *s, unsigned int n);
+unsigned int comedi_buf_read_free(struct comedi_subdevice *s, unsigned int n);
 
-int comedi_buf_put(struct comedi_async *, unsigned short);
-int comedi_buf_get(struct comedi_async *, unsigned short *);
+int comedi_buf_put(struct comedi_subdevice *s, unsigned short x);
+int comedi_buf_get(struct comedi_subdevice *s, unsigned short *x);
 
-void comedi_buf_memcpy_to(struct comedi_async *async, unsigned int offset,
+void comedi_buf_memcpy_to(struct comedi_subdevice *s, unsigned int offset,
 			  const void *source, unsigned int num_bytes);
-void comedi_buf_memcpy_from(struct comedi_async *async, unsigned int offset,
+void comedi_buf_memcpy_from(struct comedi_subdevice *s, unsigned int offset,
 			    void *destination, unsigned int num_bytes);
 
 /* drivers.c - general comedi driver functions */
diff --git a/drivers/staging/comedi/drivers.c b/drivers/staging/comedi/drivers.c
index ab0e8ed4729..299726f39e2 100644
--- a/drivers/staging/comedi/drivers.c
+++ b/drivers/staging/comedi/drivers.c
@@ -258,6 +258,7 @@ static int insn_rw_emulate_bits(struct comedi_device *dev,
 	const unsigned base_bitfield_channel =
 	    (chan < channels_per_bitfield) ? 0 : chan;
 	unsigned int new_data[2];
+
 	memset(new_data, 0, sizeof(new_data));
 	memset(&new_insn, 0, sizeof(new_insn));
 	new_insn.insn = INSN_BITS;
@@ -306,7 +307,6 @@ static int __comedi_device_postconfig_async(struct comedi_device *dev,
 		return -ENOMEM;
 
 	init_waitqueue_head(&async->wait_head);
-	async->subdevice = s;
 	s->async = async;
 
 	async->max_bufsize = comedi_default_buf_maxsize_kb * 1024;
diff --git a/drivers/staging/comedi/drivers/8253.h b/drivers/staging/comedi/drivers/8253.h
index e3d737cf730..5829b46b757 100644
--- a/drivers/staging/comedi/drivers/8253.h
+++ b/drivers/staging/comedi/drivers/8253.h
@@ -30,106 +30,11 @@
 #define I8254_OSC_BASE_2MHZ		500
 #define I8254_OSC_BASE_1MHZ		1000
 
-#define i8253_cascade_ns_to_timer i8253_cascade_ns_to_timer_2div
-
-static inline void i8253_cascade_ns_to_timer_2div_old(int i8253_osc_base,
-						      unsigned int *d1,
-						      unsigned int *d2,
-						      unsigned int *nanosec,
-						      int round_mode)
-{
-	int divider;
-	int div1, div2;
-	int div1_glb, div2_glb, ns_glb;
-	int div1_lub, div2_lub, ns_lub;
-	int ns;
-
-	divider = (*nanosec + i8253_osc_base / 2) / i8253_osc_base;
-
-	/* find 2 integers 1<={x,y}<=65536 such that x*y is
-	   close to divider */
-
-	div1_lub = div2_lub = 0;
-	div1_glb = div2_glb = 0;
-
-	ns_glb = 0;
-	ns_lub = 0xffffffff;
-
-	div2 = 0x10000;
-	for (div1 = divider / 65536 + 1; div1 < div2; div1++) {
-		div2 = divider / div1;
-
-		ns = i8253_osc_base * div1 * div2;
-		if (ns <= *nanosec && ns > ns_glb) {
-			ns_glb = ns;
-			div1_glb = div1;
-			div2_glb = div2;
-		}
-
-		div2++;
-		if (div2 <= 65536) {
-			ns = i8253_osc_base * div1 * div2;
-			if (ns > *nanosec && ns < ns_lub) {
-				ns_lub = ns;
-				div1_lub = div1;
-				div2_lub = div2;
-			}
-		}
-	}
-
-	*nanosec = div1_lub * div2_lub * i8253_osc_base;
-	*d1 = div1_lub & 0xffff;
-	*d2 = div2_lub & 0xffff;
-	return;
-}
-
-static inline void i8253_cascade_ns_to_timer_power(int i8253_osc_base,
-						   unsigned int *d1,
-						   unsigned int *d2,
-						   unsigned int *nanosec,
-						   int round_mode)
-{
-	int div1, div2;
-	int base;
-
-	for (div1 = 2; div1 <= (1 << 16); div1 <<= 1) {
-		base = i8253_osc_base * div1;
-		round_mode &= TRIG_ROUND_MASK;
-		switch (round_mode) {
-		case TRIG_ROUND_NEAREST:
-		default:
-			div2 = (*nanosec + base / 2) / base;
-			break;
-		case TRIG_ROUND_DOWN:
-			div2 = (*nanosec) / base;
-			break;
-		case TRIG_ROUND_UP:
-			div2 = (*nanosec + base - 1) / base;
-			break;
-		}
-		if (div2 < 2)
-			div2 = 2;
-		if (div2 <= 65536) {
-			*nanosec = div2 * base;
-			*d1 = div1 & 0xffff;
-			*d2 = div2 & 0xffff;
-			return;
-		}
-	}
-
-	/* shouldn't get here */
-	div1 = 0x10000;
-	div2 = 0x10000;
-	*nanosec = div1 * div2 * i8253_osc_base;
-	*d1 = div1 & 0xffff;
-	*d2 = div2 & 0xffff;
-}
-
-static inline void i8253_cascade_ns_to_timer_2div(int i8253_osc_base,
-						  unsigned int *d1,
-						  unsigned int *d2,
-						  unsigned int *nanosec,
-						  int round_mode)
+static inline void i8253_cascade_ns_to_timer(int i8253_osc_base,
+					     unsigned int *d1,
+					     unsigned int *d2,
+					     unsigned int *nanosec,
+					     int round_mode)
 {
 	unsigned int divider;
 	unsigned int div1, div2;
@@ -386,7 +291,7 @@ static inline int i8254_set_mode(unsigned long base_address,
 
 	if (counter_number > 2)
 		return -1;
-	if (mode > (I8254_MODE5 | I8254_BINARY))
+	if (mode > (I8254_MODE5 | I8254_BCD))
 		return -1;
 
 	byte = counter_number << 6;
@@ -406,7 +311,7 @@ static inline int i8254_mm_set_mode(void __iomem *base_address,
 
 	if (counter_number > 2)
 		return -1;
-	if (mode > (I8254_MODE5 | I8254_BINARY))
+	if (mode > (I8254_MODE5 | I8254_BCD))
 		return -1;
 
 	byte = counter_number << 6;
diff --git a/drivers/staging/comedi/drivers/8255.c b/drivers/staging/comedi/drivers/8255.c
index 48817f087d9..46113a37413 100644
--- a/drivers/staging/comedi/drivers/8255.c
+++ b/drivers/staging/comedi/drivers/8255.c
@@ -117,7 +117,7 @@ void subdev_8255_interrupt(struct comedi_device *dev,
 	d = spriv->io(0, _8255_DATA, 0, iobase);
 	d |= (spriv->io(0, _8255_DATA + 1, 0, iobase) << 8);
 
-	comedi_buf_put(s->async, d);
+	comedi_buf_put(s, d);
 	s->async->events |= COMEDI_CB_EOS;
 
 	comedi_event(dev, s);
@@ -231,7 +231,7 @@ static int subdev_8255_cmdtest(struct comedi_device *dev,
 	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, 1);
+	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 	err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
@@ -298,6 +298,7 @@ int subdev_8255_init_irq(struct comedi_device *dev, struct comedi_subdevice *s,
 	if (ret)
 		return ret;
 
+	s->len_chanlist	= 1;
 	s->do_cmdtest	= subdev_8255_cmdtest;
 	s->do_cmd	= subdev_8255_cmd;
 	s->cancel	= subdev_8255_cancel;
diff --git a/drivers/staging/comedi/drivers/8255.h b/drivers/staging/comedi/drivers/8255.h
index 4f16ea78f86..795d232a6c0 100644
--- a/drivers/staging/comedi/drivers/8255.h
+++ b/drivers/staging/comedi/drivers/8255.h
@@ -22,10 +22,10 @@
 #include "../comedidev.h"
 
 int subdev_8255_init(struct comedi_device *dev, struct comedi_subdevice *s,
-		     int (*io) (int, int, int, unsigned long),
+		     int (*io)(int, int, int, unsigned long),
 		     unsigned long iobase);
 int subdev_8255_init_irq(struct comedi_device *dev, struct comedi_subdevice *s,
-			 int (*io) (int, int, int, unsigned long),
+			 int (*io)(int, int, int, unsigned long),
 			 unsigned long iobase);
 void subdev_8255_interrupt(struct comedi_device *dev,
 			   struct comedi_subdevice *s);
diff --git a/drivers/staging/comedi/drivers/addi-data/addi_common.h b/drivers/staging/comedi/drivers/addi-data/addi_common.h
index 2ed2da3499f..5c6a11c35de 100644
--- a/drivers/staging/comedi/drivers/addi-data/addi_common.h
+++ b/drivers/staging/comedi/drivers/addi-data/addi_common.h
@@ -118,18 +118,10 @@ struct addi_private {
 	int i_IobaseAmcc;	/*  base+size for AMCC chip */
 	int i_IobaseAddon;	/* addon base address */
 	int i_IobaseReserved;
-	unsigned char b_AiContinuous;	/*  we do unlimited AI */
 	unsigned int ui_AiActualScan;	/* how many scans we finished */
 	unsigned int ui_AiNbrofChannels;	/*  how many channels is measured */
-	unsigned int ui_AiScanLength;	/*  Length of actual scanlist */
-	unsigned int *pui_AiChannelList;	/*  actual chanlist */
 	unsigned int ui_AiChannelList[32];	/*  actual chanlist */
 	unsigned int ui_AiReadData[32];
-	unsigned int ui_AiTimer0;	/* Timer Constant for Timer0 */
-	unsigned int ui_AiTimer1;	/* Timer constant for Timer1 */
-	unsigned int ui_AiFlags;
-	unsigned int ui_AiDataLength;
-	unsigned int ui_AiNbrofScans;	/*  number of scans to do */
 	unsigned short us_UseDma;	/*  To use Dma or not */
 	unsigned char b_DmaDoubleBuffer;	/*  we can use double buffering */
 	unsigned int ui_DmaActualBuffer;	/*  which buffer is used now */
@@ -145,7 +137,7 @@ struct addi_private {
 	unsigned short us_OutputRegister;	/*  Contain data written at iobase + 0 */
 	unsigned char b_Timer2Mode;	/*  Specify the timer 2 mode */
 	unsigned char b_Timer2Interrupt;	/* Timer2  interrupt enable or disable */
-	unsigned char b_AiCyclicAcquisition;	/*  indicate cyclic acquisition */
+	unsigned int ai_running:1;
 	unsigned char b_InterruptMode;	/*  eoc eos or dma */
 	unsigned char b_EocEosInterrupt;	/*  Enable disable eoc eos interrupt */
 	unsigned int ui_EocEosConversionTime;
diff --git a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci1564.c b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci1564.c
index 9c86b02eb6d..0ba5385226a 100644
--- a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci1564.c
+++ b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci1564.c
@@ -21,6 +21,8 @@
  *
  */
 
+#include "../addi_watchdog.h"
+
 #define APCI1564_ADDRESS_RANGE				128
 
 /* Digital Input IRQ Function Selection */
@@ -76,7 +78,7 @@
 #define APCI1564_TIMER_WARN_TIMEBASE_REG		0x64
 
 /*
- * devpriv->iobase Register Map
+ * dev>iobase Register Map
  */
 #define APCI1564_TCW_REG(x)				(0x00 + ((x) * 0x20))
 #define APCI1564_TCW_RELOAD_REG(x)			(0x04 + ((x) * 0x20))
@@ -127,18 +129,6 @@ static int apci1564_di_config(struct comedi_device *dev,
 	return insn->n;
 }
 
-static int apci1564_di_insn_bits(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn,
-				 unsigned int *data)
-{
-	struct addi_private *devpriv = dev->private;
-
-	data[1] = inl(devpriv->i_IobaseAmcc + APCI1564_DI_REG);
-
-	return insn->n;
-}
-
 /*
  * Configures The Digital Output Subdevice.
  *
@@ -180,23 +170,6 @@ static int apci1564_do_config(struct comedi_device *dev,
 	return insn->n;
 }
 
-static int apci1564_do_insn_bits(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn,
-				 unsigned int *data)
-{
-	struct addi_private *devpriv = dev->private;
-
-	s->state = inl(devpriv->i_IobaseAmcc + APCI1564_DO_REG);
-
-	if (comedi_dio_update_state(s, data))
-		outl(s->state, devpriv->i_IobaseAmcc + APCI1564_DO_REG);
-
-	data[1] = s->state;
-
-	return insn->n;
-}
-
 /*
  * Configures The Timer, Counter or Watchdog
  *
@@ -239,13 +212,13 @@ static int apci1564_timer_config(struct comedi_device *dev,
 			outl(0x0, devpriv->i_IobaseAmcc + APCI1564_DO_IRQ_REG);
 			outl(0x0, devpriv->i_IobaseAmcc + APCI1564_WDOG_IRQ_REG);
 			outl(0x0,
-				devpriv->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER1));
+			     dev->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER1));
 			outl(0x0,
-				devpriv->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER2));
+			     dev->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER2));
 			outl(0x0,
-				devpriv->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER3));
+			     dev->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER3));
 			outl(0x0,
-				devpriv->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER4));
+			     dev->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER4));
 		} else {
 			/* disable Timer interrupt */
 			outl(0x0, devpriv->i_IobaseAmcc + APCI1564_TIMER_CTRL_REG);
@@ -266,13 +239,13 @@ static int apci1564_timer_config(struct comedi_device *dev,
 		devpriv->b_ModeSelectRegister = data[5];
 
 		/* First Stop The Counter */
-		ul_Command1 = inl(devpriv->iobase + APCI1564_TCW_CTRL_REG(data[5] - 1));
+		ul_Command1 = inl(dev->iobase + APCI1564_TCW_CTRL_REG(data[5] - 1));
 		ul_Command1 = ul_Command1 & 0xFFFFF9FEUL;
 		/* Stop The Timer */
-		outl(ul_Command1, devpriv->iobase + APCI1564_TCW_CTRL_REG(data[5] - 1));
+		outl(ul_Command1, dev->iobase + APCI1564_TCW_CTRL_REG(data[5] - 1));
 
 		/* Set the reload value */
-		outl(data[3], devpriv->iobase + APCI1564_TCW_RELOAD_REG(data[5] - 1));
+		outl(data[3], dev->iobase + APCI1564_TCW_RELOAD_REG(data[5] - 1));
 
 		/* Set the mode :             */
 		/* - Disable the hardware     */
@@ -285,15 +258,15 @@ static int apci1564_timer_config(struct comedi_device *dev,
 		ul_Command1 =
 			(ul_Command1 & 0xFFFC19E2UL) | 0x80000UL |
 			(unsigned int) ((unsigned int) data[4] << 16UL);
-		outl(ul_Command1, devpriv->iobase + APCI1564_TCW_CTRL_REG(data[5] - 1));
+		outl(ul_Command1, dev->iobase + APCI1564_TCW_CTRL_REG(data[5] - 1));
 
 		/*  Enable or Disable Interrupt */
 		ul_Command1 = (ul_Command1 & 0xFFFFF9FD) | (data[1] << 1);
-		outl(ul_Command1, devpriv->iobase + APCI1564_TCW_CTRL_REG(data[5] - 1));
+		outl(ul_Command1, dev->iobase + APCI1564_TCW_CTRL_REG(data[5] - 1));
 
 		/* Set the Up/Down selection */
 		ul_Command1 = (ul_Command1 & 0xFFFBF9FFUL) | (data[6] << 18);
-		outl(ul_Command1, devpriv->iobase + APCI1564_TCW_CTRL_REG(data[5] - 1));
+		outl(ul_Command1, dev->iobase + APCI1564_TCW_CTRL_REG(data[5] - 1));
 	} else {
 		dev_err(dev->class_dev, "Invalid subdevice.\n");
 	}
@@ -349,8 +322,8 @@ static int apci1564_timer_write(struct comedi_device *dev,
 	}
 	if (devpriv->b_TimerSelectMode == ADDIDATA_COUNTER) {
 		ul_Command1 =
-			inl(devpriv->iobase +
-				APCI1564_TCW_CTRL_REG(devpriv->b_ModeSelectRegister - 1));
+			inl(dev->iobase +
+			    APCI1564_TCW_CTRL_REG(devpriv->b_ModeSelectRegister - 1));
 		if (data[1] == 1) {
 			/* Start the Counter subdevice */
 			ul_Command1 = (ul_Command1 & 0xFFFFF9FFUL) | 0x1UL;
@@ -362,9 +335,8 @@ static int apci1564_timer_write(struct comedi_device *dev,
 			/*  Clears the Counter subdevice */
 			ul_Command1 = (ul_Command1 & 0xFFFFF9FFUL) | 0x400;
 		}
-		outl(ul_Command1,
-			devpriv->iobase +
-			APCI1564_TCW_CTRL_REG(devpriv->b_ModeSelectRegister - 1));
+		outl(ul_Command1, dev->iobase +
+		     APCI1564_TCW_CTRL_REG(devpriv->b_ModeSelectRegister - 1));
 	}
 	return insn->n;
 }
@@ -393,11 +365,11 @@ static int apci1564_timer_read(struct comedi_device *dev,
 	} else if (devpriv->b_TimerSelectMode == ADDIDATA_COUNTER) {
 		/*  Read the Counter Actual Value. */
 		data[0] =
-			inl(devpriv->iobase +
-				APCI1564_TCW_REG(devpriv->b_ModeSelectRegister - 1));
+			inl(dev->iobase +
+			    APCI1564_TCW_REG(devpriv->b_ModeSelectRegister - 1));
 		ul_Command1 =
-			inl(devpriv->iobase +
-				APCI1564_TCW_STATUS_REG(devpriv->b_ModeSelectRegister - 1));
+			inl(dev->iobase +
+			    APCI1564_TCW_STATUS_REG(devpriv->b_ModeSelectRegister - 1));
 
 		/* Get the software trigger status */
 		data[1] = (unsigned char) ((ul_Command1 >> 1) & 1);
@@ -446,13 +418,13 @@ static void apci1564_interrupt(int irq, void *d)
 	ui_DO = inl(devpriv->i_IobaseAmcc + APCI1564_DO_IRQ_REG) & 0x01;
 	ui_Timer = inl(devpriv->i_IobaseAmcc + APCI1564_TIMER_IRQ_REG) & 0x01;
 	ui_C1 =
-		inl(devpriv->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER1)) & 0x1;
+		inl(dev->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER1)) & 0x1;
 	ui_C2 =
-		inl(devpriv->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER2)) & 0x1;
+		inl(dev->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER2)) & 0x1;
 	ui_C3 =
-		inl(devpriv->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER3)) & 0x1;
+		inl(dev->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER3)) & 0x1;
 	ui_C4 =
-		inl(devpriv->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER4)) & 0x1;
+		inl(dev->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER4)) & 0x1;
 	if (ui_DI == 0 && ui_DO == 0 && ui_Timer == 0 && ui_C1 == 0
 		&& ui_C2 == 0 && ui_C3 == 0 && ui_C4 == 0) {
 		dev_err(dev->class_dev, "Interrupt from unknown source.\n");
@@ -506,16 +478,16 @@ static void apci1564_interrupt(int irq, void *d)
 
 			/*  Disable Counter Interrupt */
 			ul_Command2 =
-				inl(devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER1));
+				inl(dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER1));
 			outl(0x0,
-			     devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER1));
+			     dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER1));
 
 			/* Send a signal to from kernel to user space */
 			send_sig(SIGIO, devpriv->tsk_Current, 0);
 
 			/*  Enable Counter Interrupt */
 			outl(ul_Command2,
-			     devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER1));
+			     dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER1));
 		}
 	}
 
@@ -525,16 +497,16 @@ static void apci1564_interrupt(int irq, void *d)
 
 			/*  Disable Counter Interrupt */
 			ul_Command2 =
-				inl(devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER2));
+				inl(dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER2));
 			outl(0x0,
-			     devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER2));
+			     dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER2));
 
 			/* Send a signal to from kernel to user space */
 			send_sig(SIGIO, devpriv->tsk_Current, 0);
 
 			/*  Enable Counter Interrupt */
 			outl(ul_Command2,
-			     devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER2));
+			     dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER2));
 		}
 	}
 
@@ -544,16 +516,16 @@ static void apci1564_interrupt(int irq, void *d)
 
 			/*  Disable Counter Interrupt */
 			ul_Command2 =
-				inl(devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER3));
+				inl(dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER3));
 			outl(0x0,
-			     devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER3));
+			     dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER3));
 
 			/* Send a signal to from kernel to user space */
 			send_sig(SIGIO, devpriv->tsk_Current, 0);
 
 			/*  Enable Counter Interrupt */
 			outl(ul_Command2,
-			     devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER3));
+			     dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER3));
 		}
 	}
 
@@ -563,45 +535,17 @@ static void apci1564_interrupt(int irq, void *d)
 
 			/*  Disable Counter Interrupt */
 			ul_Command2 =
-				inl(devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER4));
+				inl(dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER4));
 			outl(0x0,
-			     devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER4));
+			     dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER4));
 
 			/* Send a signal to from kernel to user space */
 			send_sig(SIGIO, devpriv->tsk_Current, 0);
 
 			/*  Enable Counter Interrupt */
 			outl(ul_Command2,
-			     devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER4));
+			     dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER4));
 		}
 	}
 	return;
 }
-
-static int apci1564_reset(struct comedi_device *dev)
-{
-	struct addi_private *devpriv = dev->private;
-
-	/* disable the interrupts */
-	outl(0x0, devpriv->i_IobaseAmcc + APCI1564_DI_IRQ_REG);
-	/* Reset the interrupt status register */
-	inl(devpriv->i_IobaseAmcc + APCI1564_DI_INT_STATUS_REG);
-	/* Disable the and/or interrupt */
-	outl(0x0, devpriv->i_IobaseAmcc + APCI1564_DI_INT_MODE1_REG);
-	outl(0x0, devpriv->i_IobaseAmcc + APCI1564_DI_INT_MODE2_REG);
-	devpriv->b_DigitalOutputRegister = 0;
-	ui_Type = 0;
-	/* Resets the output channels */
-	outl(0x0, devpriv->i_IobaseAmcc + APCI1564_DO_REG);
-	/* Disables the interrupt. */
-	outl(0x0, devpriv->i_IobaseAmcc + APCI1564_DO_INT_CTRL_REG);
-	outl(0x0, devpriv->i_IobaseAmcc + APCI1564_WDOG_RELOAD_REG);
-	outl(0x0, devpriv->i_IobaseAmcc + APCI1564_TIMER_REG);
-	outl(0x0, devpriv->i_IobaseAmcc + APCI1564_TIMER_CTRL_REG);
-
-	outl(0x0, devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER1));
-	outl(0x0, devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER2));
-	outl(0x0, devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER3));
-	outl(0x0, devpriv->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER4));
-	return 0;
-}
diff --git a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3120.c b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3120.c
index 70e8f426285..764c8f17f8f 100644
--- a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3120.c
+++ b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3120.c
@@ -609,11 +609,10 @@ static int apci3120_reset(struct comedi_device *dev)
 	unsigned int i;
 	unsigned short us_TmpValue;
 
-	devpriv->b_AiCyclicAcquisition = APCI3120_DISABLE;
+	devpriv->ai_running = 0;
 	devpriv->b_EocEosInterrupt = APCI3120_DISABLE;
 	devpriv->b_InterruptMode = APCI3120_EOC_MODE;
 	devpriv->ui_EocEosConversionTime = 0;	/*  set eoc eos conv time to 0 */
-	devpriv->b_OutputMemoryStatus = 0;
 
 	/*  variables used in timer subdevice */
 	devpriv->b_Timer2Mode = 0;
@@ -720,10 +719,9 @@ static int apci3120_cancel(struct comedi_device *dev,
 	inw(dev->iobase + APCI3120_RD_STATUS);
 	devpriv->ui_AiActualScan = 0;
 	s->async->cur_chan = 0;
-	devpriv->b_AiContinuous = 0;
 	devpriv->ui_DmaActualBuffer = 0;
 
-	devpriv->b_AiCyclicAcquisition = APCI3120_DISABLE;
+	devpriv->ai_running = 0;
 	devpriv->b_InterruptMode = APCI3120_EOC_MODE;
 	devpriv->b_EocEosInterrupt = APCI3120_DISABLE;
 	apci3120_reset(dev);
@@ -734,7 +732,6 @@ static int apci3120_ai_cmdtest(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_cmd *cmd)
 {
-	const struct addi_board *this_board = comedi_board(dev);
 	int err = 0;
 
 	/* Step 1 : check if triggers are trivially valid */
@@ -767,20 +764,16 @@ static int apci3120_ai_cmdtest(struct comedi_device *dev,
 	if (cmd->scan_begin_src == TRIG_TIMER)	/* Test Delay timing */
 		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg, 100000);
 
-	if (cmd->convert_src == TRIG_TIMER) {	/*  Test Acquisition timing */
-		if (cmd->scan_begin_src == TRIG_TIMER) {
-			if (cmd->convert_arg)
-				err |= cfc_check_trigger_arg_min(
-						&cmd->convert_arg, 10000);
-		} else {
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		if (cmd->convert_arg)
 			err |= cfc_check_trigger_arg_min(&cmd->convert_arg,
-							10000);
-		}
+							 10000);
+	} else {
+		err |= cfc_check_trigger_arg_min(&cmd->convert_arg, 10000);
 	}
 
 	err |= cfc_check_trigger_arg_min(&cmd->chanlist_len, 1);
-	err |= cfc_check_trigger_arg_max(&cmd->chanlist_len,
-					 this_board->i_AiChannelList);
+	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
 		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
@@ -792,15 +785,10 @@ static int apci3120_ai_cmdtest(struct comedi_device *dev,
 
 	/*  step 4: fix up any arguments */
 
-	if (cmd->convert_src == TRIG_TIMER) {
-
-		if (cmd->scan_begin_src == TRIG_TIMER &&
-			cmd->scan_begin_arg <
-			cmd->convert_arg * cmd->scan_end_arg) {
-			cmd->scan_begin_arg =
-				cmd->convert_arg * cmd->scan_end_arg;
-			err++;
-		}
+	if (cmd->scan_begin_src == TRIG_TIMER &&
+	    cmd->scan_begin_arg < cmd->convert_arg * cmd->scan_end_arg) {
+		cmd->scan_begin_arg = cmd->convert_arg * cmd->scan_end_arg;
+		err |= -EINVAL;
 	}
 
 	if (err)
@@ -821,16 +809,13 @@ static int apci3120_cyclic_ai(int mode,
 {
 	const struct addi_board *this_board = comedi_board(dev);
 	struct addi_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned char b_Tmp;
 	unsigned int ui_Tmp, ui_DelayTiming = 0, ui_TimerValue1 = 0, dmalen0 =
 		0, dmalen1 = 0, ui_TimerValue2 =
 		0, ui_TimerValue0, ui_ConvertTiming;
 	unsigned short us_TmpValue;
 
-	/* BEGIN JK 07.05.04: Comparison between WIN32 and Linux driver */
-	/* devpriv->b_AiCyclicAcquisition=APCI3120_ENABLE; */
-	/* END JK 07.05.04: Comparison between WIN32 and Linux driver */
-
 	/*******************/
 	/* Resets the FIFO */
 	/*******************/
@@ -840,12 +825,7 @@ static int apci3120_cyclic_ai(int mode,
 	/* inw(dev->iobase+APCI3120_RD_STATUS); */
 	/* END JK 07.05.04: Comparison between WIN32 and Linux driver */
 
-	/***************************/
-	/* Acquisition initialized */
-	/***************************/
-	/* BEGIN JK 07.05.04: Comparison between WIN32 and Linux driver */
-	devpriv->b_AiCyclicAcquisition = APCI3120_ENABLE;
-	/* END JK 07.05.04: Comparison between WIN32 and Linux driver */
+	devpriv->ai_running = 1;
 
 	/*  clear software  registers */
 	devpriv->b_TimerSelectMode = 0;
@@ -892,17 +872,17 @@ static int apci3120_cyclic_ai(int mode,
 	devpriv->ui_DmaActualBuffer = 0;
 
 	/*  value for timer2  minus -2 has to be done .....dunno y?? */
-	ui_TimerValue2 = devpriv->ui_AiNbrofScans - 2;
-	ui_ConvertTiming = devpriv->ui_AiTimer0;
+	ui_TimerValue2 = cmd->stop_arg - 2;
+	ui_ConvertTiming = cmd->convert_arg;
 
 	if (mode == 2)
-		ui_DelayTiming = devpriv->ui_AiTimer1;
+		ui_DelayTiming = cmd->scan_begin_arg;
 
    /**********************************/
 	/* Initializes the sequence array */
    /**********************************/
 	if (!apci3120_setup_chan_list(dev, s, devpriv->ui_AiNbrofChannels,
-			devpriv->pui_AiChannelList, 0))
+			cmd->chanlist, 0))
 		return -EINVAL;
 
 	us_TmpValue = (unsigned short) inw(dev->iobase + APCI3120_RD_STATUS);
@@ -1039,7 +1019,7 @@ static int apci3120_cyclic_ai(int mode,
 		outb(devpriv->b_ModeSelectRegister,
 			dev->iobase + APCI3120_WRITE_MODE_SELECT);
 
-		if (!devpriv->b_AiContinuous) {
+		if (cmd->stop_src == TRIG_COUNT) {
 /*
  * configure Timer2 For counting EOS Reset gate 2 of Timer 2 to
  * disable it (Set Bit D14 to 0)
@@ -1107,6 +1087,7 @@ static int apci3120_cyclic_ai(int mode,
 		}
 	} else {
 		/* If DMA Enabled */
+		unsigned int scan_bytes = cmd->scan_end_arg * sizeof(short);
 
 		/* BEGIN JK 07.05.04: Comparison between WIN32 and Linux driver */
 		/* inw(dev->iobase+0); reset EOC bit */
@@ -1125,37 +1106,38 @@ static int apci3120_cyclic_ai(int mode,
 		dmalen0 = devpriv->ui_DmaBufferSize[0];
 		dmalen1 = devpriv->ui_DmaBufferSize[1];
 
-		if (!devpriv->b_AiContinuous) {
-
-			if (dmalen0 > (devpriv->ui_AiNbrofScans * devpriv->ui_AiScanLength * 2)) {	/*  must we fill full first buffer? */
-				dmalen0 =
-					devpriv->ui_AiNbrofScans *
-					devpriv->ui_AiScanLength * 2;
-			} else if (dmalen1 > (devpriv->ui_AiNbrofScans * devpriv->ui_AiScanLength * 2 - dmalen0))	/*  and must we fill full second buffer when first is once filled? */
-				dmalen1 =
-					devpriv->ui_AiNbrofScans *
-					devpriv->ui_AiScanLength * 2 - dmalen0;
+		if (cmd->stop_src == TRIG_COUNT) {
+			/*
+			 * Must we fill full first buffer? And must we fill
+			 * full second buffer when first is once filled?
+			 */
+			if (dmalen0 > (cmd->stop_arg * scan_bytes)) {
+				dmalen0 = cmd->stop_arg * scan_bytes;
+			} else if (dmalen1 > (cmd->stop_arg * scan_bytes -
+					      dmalen0))
+				dmalen1 = cmd->stop_arg * scan_bytes -
+					  dmalen0;
 		}
 
-		if (devpriv->ui_AiFlags & TRIG_WAKE_EOS) {
+		if (cmd->flags & TRIG_WAKE_EOS) {
 			/*  don't we want wake up every scan? */
-			if (dmalen0 > (devpriv->ui_AiScanLength * 2)) {
-				dmalen0 = devpriv->ui_AiScanLength * 2;
-				if (devpriv->ui_AiScanLength & 1)
+			if (dmalen0 > scan_bytes) {
+				dmalen0 = scan_bytes;
+				if (cmd->scan_end_arg & 1)
 					dmalen0 += 2;
 			}
-			if (dmalen1 > (devpriv->ui_AiScanLength * 2)) {
-				dmalen1 = devpriv->ui_AiScanLength * 2;
-				if (devpriv->ui_AiScanLength & 1)
+			if (dmalen1 > scan_bytes) {
+				dmalen1 = scan_bytes;
+				if (cmd->scan_end_arg & 1)
 					dmalen1 -= 2;
 				if (dmalen1 < 4)
 					dmalen1 = 4;
 			}
 		} else {	/*  isn't output buff smaller that our DMA buff? */
-			if (dmalen0 > (devpriv->ui_AiDataLength))
-				dmalen0 = devpriv->ui_AiDataLength;
-			if (dmalen1 > (devpriv->ui_AiDataLength))
-				dmalen1 = devpriv->ui_AiDataLength;
+			if (dmalen0 > s->async->prealloc_bufsz)
+				dmalen0 = s->async->prealloc_bufsz;
+			if (dmalen1 > s->async->prealloc_bufsz)
+				dmalen1 = s->async->prealloc_bufsz;
 		}
 		devpriv->ui_DmaBufferUsesize[0] = dmalen0;
 		devpriv->ui_DmaBufferUsesize[1] = dmalen1;
@@ -1293,8 +1275,8 @@ static int apci3120_cyclic_ai(int mode,
 		/* END JK 07.05.04: Comparison between WIN32 and Linux driver */
 	}
 
-	if ((devpriv->us_UseDma == APCI3120_DISABLE)
-		&& !devpriv->b_AiContinuous) {
+	if (devpriv->us_UseDma == APCI3120_DISABLE &&
+	    cmd->stop_src == TRIG_COUNT) {
 		/*  set gate 2   to start conversion */
 		devpriv->us_OutputRegister =
 			devpriv->us_OutputRegister | APCI3120_ENABLE_TIMER2;
@@ -1337,50 +1319,17 @@ static int apci3120_ai_cmd(struct comedi_device *dev,
 	struct comedi_cmd *cmd = &s->async->cmd;
 
 	/* loading private structure with cmd structure inputs */
-	devpriv->ui_AiFlags = cmd->flags;
 	devpriv->ui_AiNbrofChannels = cmd->chanlist_len;
-	devpriv->ui_AiScanLength = cmd->scan_end_arg;
-	devpriv->pui_AiChannelList = cmd->chanlist;
-
-	/* UPDATE-0.7.57->0.7.68devpriv->ui_AiDataLength=s->async->data_len; */
-	devpriv->ui_AiDataLength = s->async->prealloc_bufsz;
-
-	if (cmd->stop_src == TRIG_COUNT)
-		devpriv->ui_AiNbrofScans = cmd->stop_arg;
-	else
-		devpriv->ui_AiNbrofScans = 0;
-
-	devpriv->ui_AiTimer0 = 0;	/*  variables changed to timer0,timer1 */
-	devpriv->ui_AiTimer1 = 0;
-	if ((devpriv->ui_AiNbrofScans == 0) || (devpriv->ui_AiNbrofScans == -1))
-		devpriv->b_AiContinuous = 1;	/*  user want neverending analog acquisition */
-	/*  stopped using cancel */
 
 	if (cmd->start_src == TRIG_EXT)
 		devpriv->b_ExttrigEnable = APCI3120_ENABLE;
 	else
 		devpriv->b_ExttrigEnable = APCI3120_DISABLE;
 
-	if (cmd->scan_begin_src == TRIG_FOLLOW) {
-		/*  mode 1 or 3 */
-		if (cmd->convert_src == TRIG_TIMER) {
-			/*  mode 1 */
-
-			devpriv->ui_AiTimer0 = cmd->convert_arg;	/*  timer constant in nano seconds */
-			/* return this_board->ai_cmd(1,dev,s); */
-			return apci3120_cyclic_ai(1, dev, s);
-		}
-
-	}
-	if ((cmd->scan_begin_src == TRIG_TIMER)
-		&& (cmd->convert_src == TRIG_TIMER)) {
-		/*  mode 2 */
-		devpriv->ui_AiTimer1 = cmd->scan_begin_arg;
-		devpriv->ui_AiTimer0 = cmd->convert_arg;	/*  variable changed timer2 to timer0 */
-		/* return this_board->ai_cmd(2,dev,s); */
+	if (cmd->scan_begin_src == TRIG_FOLLOW)
+		return apci3120_cyclic_ai(1, dev, s);
+	else	/* TRIG_TIMER */
 		return apci3120_cyclic_ai(2, dev, s);
-	}
-	return -1;
 }
 
 /*
@@ -1392,11 +1341,12 @@ static void v_APCI3120_InterruptDmaMoveBlock16bit(struct comedi_device *dev,
 						  unsigned int num_samples)
 {
 	struct addi_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 
 	devpriv->ui_AiActualScan +=
-		(s->async->cur_chan + num_samples) / devpriv->ui_AiScanLength;
+		(s->async->cur_chan + num_samples) / cmd->scan_end_arg;
 	s->async->cur_chan += num_samples;
-	s->async->cur_chan %= devpriv->ui_AiScanLength;
+	s->async->cur_chan %= cmd->scan_end_arg;
 
 	cfc_write_array_to_buffer(s, dma_buffer, num_samples * sizeof(short));
 }
@@ -1412,6 +1362,7 @@ static void apci3120_interrupt_dma(int irq, void *d)
 	struct comedi_device *dev = d;
 	struct addi_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->read_subdev;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned int next_dma_buf, samplesinbuf;
 	unsigned long low_word, high_word, var;
 	unsigned int ui_Tmp;
@@ -1427,8 +1378,6 @@ static void apci3120_interrupt_dma(int irq, void *d)
 	if (samplesinbuf & 1) {
 		comedi_error(dev, "Odd count of bytes in DMA ring!");
 		apci3120_cancel(dev, s);
-		devpriv->b_AiCyclicAcquisition = APCI3120_DISABLE;
-
 		return;
 	}
 	samplesinbuf = samplesinbuf >> 1;	/*  number of received samples */
@@ -1489,16 +1438,15 @@ static void apci3120_interrupt_dma(int irq, void *d)
 			devpriv->ul_DmaBufferVirtual[devpriv->
 				ui_DmaActualBuffer], samplesinbuf);
 
-		if (!(devpriv->ui_AiFlags & TRIG_WAKE_EOS)) {
+		if (!(cmd->flags & TRIG_WAKE_EOS)) {
 			s->async->events |= COMEDI_CB_EOS;
 			comedi_event(dev, s);
 		}
 	}
-	if (!devpriv->b_AiContinuous)
-		if (devpriv->ui_AiActualScan >= devpriv->ui_AiNbrofScans) {
+	if (cmd->stop_src == TRIG_COUNT)
+		if (devpriv->ui_AiActualScan >= cmd->stop_arg) {
 			/*  all data sampled */
 			apci3120_cancel(dev, s);
-			devpriv->b_AiCyclicAcquisition = APCI3120_DISABLE;
 			s->async->events |= COMEDI_CB_EOA;
 			comedi_event(dev, s);
 			return;
@@ -1572,7 +1520,7 @@ static int apci3120_interrupt_handle_eos(struct comedi_device *dev)
 	n_chan = devpriv->ui_AiNbrofChannels;
 
 	for (i = 0; i < n_chan; i++)
-		err &= comedi_buf_put(s->async, inw(dev->iobase + 0));
+		err &= comedi_buf_put(s, inw(dev->iobase + 0));
 
 	s->async->events |= COMEDI_CB_EOS;
 
@@ -1648,7 +1596,7 @@ static void apci3120_interrupt(int irq, void *d)
 
 		if (devpriv->b_EocEosInterrupt == APCI3120_ENABLE) {	/*  enable this in without DMA ??? */
 
-			if (devpriv->b_AiCyclicAcquisition == APCI3120_ENABLE) {
+			if (devpriv->ai_running) {
 				ui_Check = 0;
 				apci3120_interrupt_handle_eos(dev);
 				devpriv->ui_AiActualScan++;
@@ -1690,8 +1638,6 @@ static void apci3120_interrupt(int irq, void *d)
 
 		switch (devpriv->b_Timer2Mode) {
 		case APCI3120_COUNTER:
-
-			devpriv->b_AiCyclicAcquisition = APCI3120_DISABLE;
 			devpriv->b_ModeSelectRegister =
 				devpriv->
 				b_ModeSelectRegister & APCI3120_DISABLE_EOS_INT;
@@ -1707,7 +1653,6 @@ static void apci3120_interrupt(int irq, void *d)
 
 			/* stop timer 0 and timer 1 */
 			apci3120_cancel(dev, s);
-			devpriv->b_AiCyclicAcquisition = APCI3120_DISABLE;
 
 			/* UPDATE-0.7.57->0.7.68comedi_done(dev,s); */
 			s->async->events |= COMEDI_CB_EOA;
@@ -1746,7 +1691,7 @@ static void apci3120_interrupt(int irq, void *d)
 	}
 
 	if ((int_daq & 0x4) && (devpriv->b_InterruptMode == APCI3120_DMA_MODE)) {
-		if (devpriv->b_AiCyclicAcquisition == APCI3120_ENABLE) {
+		if (devpriv->ai_running) {
 
 			/****************************/
 			/* Clear Timer Write TC int */
diff --git a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3200.c b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3200.c
index 0536d837386..f540394d17b 100644
--- a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3200.c
+++ b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3200.c
@@ -2221,12 +2221,11 @@ static int apci3200_ai_cmdtest(struct comedi_device *dev,
 	unsigned int ui_ConvertTimeBase = 0;
 	unsigned int ui_DelayTime = 0;
 	unsigned int ui_DelayTimeBase = 0;
-	int i_Triggermode = 0;
-	int i_TriggerEdge = 0;
 	int i_NbrOfChannel = 0;
 	int i_Cpt = 0;
 	double d_ConversionTimeForAllChannels = 0.0;
 	double d_SCANTimeNewUnit = 0.0;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -2253,23 +2252,37 @@ static int apci3200_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 2b : and mutually compatible */
 
-	if (cmd->start_src == TRIG_EXT) {
-		i_TriggerEdge = cmd->start_arg & 0xFFFF;
-		i_Triggermode = cmd->start_arg >> 16;
-		if (i_TriggerEdge < 1 || i_TriggerEdge > 3) {
-			err++;
-			printk("\nThe trigger edge selection is in error\n");
-		}
-		if (i_Triggermode != 2) {
-			err++;
-			printk("\nThe trigger mode selection is in error\n");
-		}
-	}
-
 	if (err) {
 		apci3200_reset(dev);
 		return 2;
 	}
+
+	/* Step 3: check if arguments are trivially valid */
+
+	switch (cmd->start_src) {
+	case TRIG_NOW:
+		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+		break;
+	case TRIG_EXT:
+		/* validate the trigger edge selection */
+		arg = cmd->start_arg & 0xffff;
+		if (arg < 1 || arg > 3) {
+			cmd->start_arg &= ~0xffff;
+			cmd->start_arg |= 1;
+			err |= -EINVAL;
+		}
+		/* validate the trigger mode selection */
+		arg = cmd->start_arg >> 16;
+		if (arg != 2) {
+			cmd->start_arg &= ~(0xffff << 16);
+			cmd->start_arg |= (2 << 16);
+			err |= -EINVAL;
+		}
+		break;
+	}
+
+	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+
 	/* i_FirstChannel=cmd->chanlist[0]; */
 	s_BoardInfos[dev->minor].i_FirstChannel = cmd->chanlist[0];
 	/* i_LastChannel=cmd->chanlist[1]; */
@@ -2689,8 +2702,7 @@ static int i_APCI3200_InterruptHandleEos(struct comedi_device *dev)
 			s->async->events |= COMEDI_CB_EOS;
 
 			/*  Test if enougth memory is available and allocate it for 7 values */
-			/* n = comedi_buf_write_alloc(s->async, 7*sizeof(unsigned int)); */
-			n = comedi_buf_write_alloc(s->async,
+			n = comedi_buf_write_alloc(s,
 				(7 + 12) * sizeof(unsigned int));
 
 			/*  If not enough memory available, event is set to Comedi Buffer Error */
@@ -2699,12 +2711,12 @@ static int i_APCI3200_InterruptHandleEos(struct comedi_device *dev)
 				s->async->events |= COMEDI_CB_ERROR;
 			}
 			/*  Write all 7 scan values in the comedi buffer */
-			comedi_buf_memcpy_to(s->async, 0,
+			comedi_buf_memcpy_to(s, 0,
 				(unsigned int *) s_BoardInfos[dev->minor].
 				ui_ScanValueArray, (7 + 12) * sizeof(unsigned int));
 
 			/*  Update comedi buffer pinters indexes */
-			comedi_buf_write_free(s->async,
+			comedi_buf_write_free(s,
 				(7 + 12) * sizeof(unsigned int));
 
 			/*  Send events */
diff --git a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3501.c b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3501.c
index 20e89b0bdc4..e82c3fcd048 100644
--- a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3501.c
+++ b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3501.c
@@ -102,11 +102,7 @@ static int apci3501_write_insn_timer(struct comedi_device *dev,
 			ul_Command1 = (ul_Command1 & 0xFFFFF9FFUL) | 0x1UL;
 			/* Enable the Watchdog */
 			outl(ul_Command1, dev->iobase + APCI3501_TIMER_CTRL_REG);
-		}
-
-		else if (data[1] == 0)	/* Stop The Watchdog */
-		{
-			/* Stop The Watchdog */
+		} else if (data[1] == 0) { /* Stop The Watchdog */
 			ul_Command1 = inl(dev->iobase + APCI3501_TIMER_CTRL_REG);
 			ul_Command1 = ul_Command1 & 0xFFFFF9FEUL;
 			outl(0x0, dev->iobase + APCI3501_TIMER_CTRL_REG);
diff --git a/drivers/staging/comedi/drivers/addi_apci_1032.c b/drivers/staging/comedi/drivers/addi_apci_1032.c
index 0daa0ea63b5..1b2e7c040c9 100644
--- a/drivers/staging/comedi/drivers/addi_apci_1032.c
+++ b/drivers/staging/comedi/drivers/addi_apci_1032.c
@@ -198,7 +198,7 @@ static int apci1032_cos_cmdtest(struct comedi_device *dev,
 	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, 1);
+	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 	err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
@@ -262,7 +262,7 @@ static irqreturn_t apci1032_interrupt(int irq, void *d)
 	outl(ctrl & ~APCI1032_CTRL_INT_ENA, dev->iobase + APCI1032_CTRL_REG);
 
 	s->state = inl(dev->iobase + APCI1032_STATUS_REG) & 0xffff;
-	comedi_buf_put(s->async, s->state);
+	comedi_buf_put(s, s->state);
 	s->async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOS;
 	comedi_event(dev, s);
 
@@ -332,6 +332,7 @@ static int apci1032_auto_attach(struct comedi_device *dev,
 		s->range_table	= &range_digital;
 		s->insn_config	= apci1032_cos_insn_config;
 		s->insn_bits	= apci1032_cos_insn_bits;
+		s->len_chanlist	= 1;
 		s->do_cmdtest	= apci1032_cos_cmdtest;
 		s->do_cmd	= apci1032_cos_cmd;
 		s->cancel	= apci1032_cos_cancel;
@@ -379,5 +380,5 @@ static struct pci_driver apci1032_pci_driver = {
 module_comedi_pci_driver(apci1032_driver, apci1032_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
+MODULE_DESCRIPTION("ADDI-DATA APCI-1032, 32 channel DI boards");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/addi_apci_1500.c b/drivers/staging/comedi/drivers/addi_apci_1500.c
index bd8e08ca14c..eab75eb2647 100644
--- a/drivers/staging/comedi/drivers/addi_apci_1500.c
+++ b/drivers/staging/comedi/drivers/addi_apci_1500.c
@@ -72,5 +72,5 @@ static struct pci_driver apci1500_pci_driver = {
 module_comedi_pci_driver(apci1500_driver, apci1500_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
+MODULE_DESCRIPTION("ADDI-DATA APCI-1500, 16 channel DI / 16 channel DO boards");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/addi_apci_1564.c b/drivers/staging/comedi/drivers/addi_apci_1564.c
index 27aa9ae1bdd..13d9962b47e 100644
--- a/drivers/staging/comedi/drivers/addi_apci_1564.c
+++ b/drivers/staging/comedi/drivers/addi_apci_1564.c
@@ -3,50 +3,168 @@
 
 #include "../comedidev.h"
 #include "comedi_fc.h"
-#include "amcc_s5933.h"
 
 #include "addi-data/addi_common.h"
 
-#include "addi-data/addi_eeprom.c"
 #include "addi-data/hwdrv_apci1564.c"
-#include "addi-data/addi_common.c"
-
-static const struct addi_board apci1564_boardtypes[] = {
-	{
-		.pc_DriverName		= "apci1564",
-		.i_IorangeBase1		= APCI1564_ADDRESS_RANGE,
-		.i_PCIEeprom		= ADDIDATA_EEPROM,
-		.pc_EepromChip		= ADDIDATA_93C76,
-		.i_NbrDiChannel		= 32,
-		.i_NbrDoChannel		= 32,
-		.i_DoMaxdata		= 0xffffffff,
-		.i_Timer		= 1,
-		.interrupt		= apci1564_interrupt,
-		.reset			= apci1564_reset,
-		.di_config		= apci1564_di_config,
-		.di_bits		= apci1564_di_insn_bits,
-		.do_config		= apci1564_do_config,
-		.do_bits		= apci1564_do_insn_bits,
-		.do_read		= apci1564_do_read,
-		.timer_config		= apci1564_timer_config,
-		.timer_write		= apci1564_timer_write,
-		.timer_read		= apci1564_timer_read,
-	},
-};
+
+static irqreturn_t v_ADDI_Interrupt(int irq, void *d)
+{
+	apci1564_interrupt(irq, d);
+	return IRQ_RETVAL(1);
+}
+
+static int apci1564_di_insn_bits(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 struct comedi_insn *insn,
+				 unsigned int *data)
+{
+	struct addi_private *devpriv = dev->private;
+
+	data[1] = inl(devpriv->i_IobaseAmcc + APCI1564_DI_REG);
+
+	return insn->n;
+}
+
+static int apci1564_do_insn_bits(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 struct comedi_insn *insn,
+				 unsigned int *data)
+{
+	struct addi_private *devpriv = dev->private;
+
+	s->state = inl(devpriv->i_IobaseAmcc + APCI1564_DO_REG);
+
+	if (comedi_dio_update_state(s, data))
+		outl(s->state, devpriv->i_IobaseAmcc + APCI1564_DO_REG);
+
+	data[1] = s->state;
+
+	return insn->n;
+}
+
+static int apci1564_reset(struct comedi_device *dev)
+{
+	struct addi_private *devpriv = dev->private;
+
+	ui_Type = 0;
+
+	/* Disable the input interrupts and reset status register */
+	outl(0x0, devpriv->i_IobaseAmcc + APCI1564_DI_IRQ_REG);
+	inl(devpriv->i_IobaseAmcc + APCI1564_DI_INT_STATUS_REG);
+	outl(0x0, devpriv->i_IobaseAmcc + APCI1564_DI_INT_MODE1_REG);
+	outl(0x0, devpriv->i_IobaseAmcc + APCI1564_DI_INT_MODE2_REG);
+
+	/* Reset the output channels and disable interrupts */
+	outl(0x0, devpriv->i_IobaseAmcc + APCI1564_DO_REG);
+	outl(0x0, devpriv->i_IobaseAmcc + APCI1564_DO_INT_CTRL_REG);
+
+	/* Reset the watchdog registers */
+	addi_watchdog_reset(devpriv->i_IobaseAmcc + APCI1564_WDOG_REG);
+
+	/* Reset the timer registers */
+	outl(0x0, devpriv->i_IobaseAmcc + APCI1564_TIMER_CTRL_REG);
+	outl(0x0, devpriv->i_IobaseAmcc + APCI1564_TIMER_RELOAD_REG);
+
+	/* Reset the counter registers */
+	outl(0x0, dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER1));
+	outl(0x0, dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER2));
+	outl(0x0, dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER3));
+	outl(0x0, dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER4));
+
+	return 0;
+}
 
 static int apci1564_auto_attach(struct comedi_device *dev,
-				unsigned long context)
+				      unsigned long context_unused)
+{
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	struct addi_private *devpriv;
+	struct comedi_subdevice *s;
+	int ret;
+
+	dev->board_name = dev->driver->driver_name;
+
+	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
+	if (!devpriv)
+		return -ENOMEM;
+
+	ret = comedi_pci_enable(dev);
+	if (ret)
+		return ret;
+
+	dev->iobase = pci_resource_start(pcidev, 1);
+	devpriv->i_IobaseAmcc = pci_resource_start(pcidev, 0);
+
+	apci1564_reset(dev);
+
+	if (pcidev->irq > 0) {
+		ret = request_irq(pcidev->irq, v_ADDI_Interrupt, IRQF_SHARED,
+				  dev->board_name, dev);
+		if (ret == 0)
+			dev->irq = pcidev->irq;
+	}
+
+	ret = comedi_alloc_subdevices(dev, 3);
+	if (ret)
+		return ret;
+
+	/*  Allocate and Initialise DI Subdevice Structures */
+	s = &dev->subdevices[0];
+	s->type = COMEDI_SUBD_DI;
+	s->subdev_flags = SDF_READABLE;
+	s->n_chan = 32;
+	s->maxdata = 1;
+	s->len_chanlist = 32;
+	s->range_table = &range_digital;
+	s->insn_config = apci1564_di_config;
+	s->insn_bits = apci1564_di_insn_bits;
+
+	/*  Allocate and Initialise DO Subdevice Structures */
+	s = &dev->subdevices[1];
+	s->type = COMEDI_SUBD_DO;
+	s->subdev_flags = SDF_WRITEABLE;
+	s->n_chan = 32;
+	s->maxdata = 0xffffffff;
+	s->len_chanlist = 32;
+	s->range_table = &range_digital;
+	s->insn_config = apci1564_do_config;
+	s->insn_bits = apci1564_do_insn_bits;
+	s->insn_read = apci1564_do_read;
+
+	/*  Allocate and Initialise Timer Subdevice Structures */
+	s = &dev->subdevices[2];
+	s->type = COMEDI_SUBD_TIMER;
+	s->subdev_flags = SDF_WRITEABLE;
+	s->n_chan = 1;
+	s->maxdata = 0;
+	s->len_chanlist = 1;
+	s->range_table = &range_digital;
+	s->insn_write = apci1564_timer_write;
+	s->insn_read = apci1564_timer_read;
+	s->insn_config = apci1564_timer_config;
+
+	return 0;
+}
+
+static void apci1564_detach(struct comedi_device *dev)
 {
-	dev->board_ptr = &apci1564_boardtypes[0];
+	struct addi_private *devpriv = dev->private;
 
-	return addi_auto_attach(dev, context);
+	if (devpriv) {
+		if (dev->iobase)
+			apci1564_reset(dev);
+		if (dev->irq)
+			free_irq(dev->irq, dev);
+	}
+	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver apci1564_driver = {
 	.driver_name	= "addi_apci_1564",
 	.module		= THIS_MODULE,
 	.auto_attach	= apci1564_auto_attach,
-	.detach		= i_ADDI_Detach,
+	.detach		= apci1564_detach,
 };
 
 static int apci1564_pci_probe(struct pci_dev *dev,
@@ -70,5 +188,5 @@ static struct pci_driver apci1564_pci_driver = {
 module_comedi_pci_driver(apci1564_driver, apci1564_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
+MODULE_DESCRIPTION("ADDI-DATA APCI-1564, 32 channel DI / 32 channel DO boards");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/addi_apci_2032.c b/drivers/staging/comedi/drivers/addi_apci_2032.c
index c9b933cb598..be0a8a7bd3b 100644
--- a/drivers/staging/comedi/drivers/addi_apci_2032.c
+++ b/drivers/staging/comedi/drivers/addi_apci_2032.c
@@ -197,6 +197,7 @@ static irqreturn_t apci2032_interrupt(int irq, void *d)
 {
 	struct comedi_device *dev = d;
 	struct comedi_subdevice *s = dev->read_subdev;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	struct apci2032_int_private *subpriv;
 	unsigned int val;
 	bool do_event = false;
@@ -222,21 +223,20 @@ static irqreturn_t apci2032_interrupt(int irq, void *d)
 	 */
 
 	if (subpriv->active && (val & subpriv->enabled_isns) != 0) {
-		unsigned short bits;
-		unsigned int n, len;
-		unsigned int *chanlist;
+		unsigned short bits = 0;
+		int i;
 
 		/* Bits in scan data correspond to indices in channel list. */
-		bits = 0;
-		len = s->async->cmd.chanlist_len;
-		chanlist = &s->async->cmd.chanlist[0];
-		for (n = 0; n < len; n++)
-			if ((val & (1U << CR_CHAN(chanlist[n]))) != 0)
-				bits |= 1U << n;
-
-		if (comedi_buf_put(s->async, bits)) {
+		for (i = 0; i < cmd->chanlist_len; i++) {
+			unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+
+			if (val & (1 << chan))
+				bits |= (1 << i);
+		}
+
+		if (comedi_buf_put(s, bits)) {
 			s->async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOS;
-			if (s->async->cmd.stop_src == TRIG_COUNT &&
+			if (cmd->stop_src == TRIG_COUNT &&
 			    subpriv->stop_count > 0) {
 				subpriv->stop_count--;
 				if (subpriv->stop_count == 0) {
@@ -374,5 +374,5 @@ static struct pci_driver apci2032_pci_driver = {
 module_comedi_pci_driver(apci2032_driver, apci2032_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
+MODULE_DESCRIPTION("ADDI-DATA APCI-2032, 32 channel DO boards");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/addi_apci_3120.c b/drivers/staging/comedi/drivers/addi_apci_3120.c
index 57ee6e5c763..0cfb12fa1cb 100644
--- a/drivers/staging/comedi/drivers/addi_apci_3120.c
+++ b/drivers/staging/comedi/drivers/addi_apci_3120.c
@@ -246,5 +246,5 @@ static struct pci_driver apci3120_pci_driver = {
 module_comedi_pci_driver(apci3120_driver, apci3120_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
+MODULE_DESCRIPTION("ADDI-DATA APCI-3120, Analog input board");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/addi_apci_3xxx.c b/drivers/staging/comedi/drivers/addi_apci_3xxx.c
index 6dc11c407f5..0532b6cc40e 100644
--- a/drivers/staging/comedi/drivers/addi_apci_3xxx.c
+++ b/drivers/staging/comedi/drivers/addi_apci_3xxx.c
@@ -373,7 +373,7 @@ static irqreturn_t apci3xxx_irq_handler(int irq, void *d)
 		writel(status, devpriv->mmio + 16);
 
 		val = readl(devpriv->mmio + 28);
-		comedi_buf_put(s->async, val);
+		comedi_buf_put(s, val);
 
 		s->async->events |= COMEDI_CB_EOA;
 		comedi_event(dev, s);
@@ -533,7 +533,7 @@ static int apci3xxx_ai_cmdtest(struct comedi_device *dev,
 {
 	const struct apci3xxx_boardinfo *board = comedi_board(dev);
 	int err = 0;
-	unsigned int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -573,31 +573,9 @@ static int apci3xxx_ai_cmdtest(struct comedi_device *dev,
 
 	/* step 4: fix up any arguments */
 
-	/*
-	 * FIXME: The hardware supports multiple scan modes but the original
-	 * addi-data driver only supported reading a single channel with
-	 * interrupts. Need a proper datasheet to fix this.
-	 *
-	 * The following scan modes are supported by the hardware:
-	 * 1) Single software scan
-	 * 2) Single hardware triggered scan
-	 * 3) Continuous software scan
-	 * 4) Continuous software scan with timer delay
-	 * 5) Continuous hardware triggered scan
-	 * 6) Continuous hardware triggered scan with timer delay
-	 *
-	 * For now, limit the chanlist to a single channel.
-	 */
-	if (cmd->chanlist_len > 1) {
-		cmd->chanlist_len = 1;
-		err |= -EINVAL;
-	}
-
-	tmp = cmd->convert_arg;
-	err |= apci3xxx_ai_ns_to_timer(dev, &cmd->convert_arg,
-				       cmd->flags & TRIG_ROUND_MASK);
-	if (tmp != cmd->convert_arg)
-		err |= -EINVAL;
+	arg = cmd->convert_arg;
+	err |= apci3xxx_ai_ns_to_timer(dev, &arg, cmd->flags & TRIG_ROUND_MASK);
+	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
 
 	if (err)
 		return 4;
@@ -719,7 +697,8 @@ static int apci3xxx_dio_insn_config(struct comedi_device *dev,
 		if (chan < 16)
 			return -EINVAL;
 		else
-			/* changing any channel in port 2 changes the entire port */
+			/* changing any channel in port 2 */
+			/* changes the entire port        */
 			mask = 0xff0000;
 	}
 
@@ -842,12 +821,30 @@ static int apci3xxx_auto_attach(struct comedi_device *dev,
 		s->subdev_flags	= SDF_READABLE | board->ai_subdev_flags;
 		s->n_chan	= board->ai_n_chan;
 		s->maxdata	= board->ai_maxdata;
-		s->len_chanlist	= s->n_chan;
 		s->range_table	= &apci3xxx_ai_range;
 		s->insn_read	= apci3xxx_ai_insn_read;
 		if (dev->irq) {
+			/*
+			 * FIXME: The hardware supports multiple scan modes
+			 * but the original addi-data driver only supported
+			 * reading a single channel with interrupts. Need a
+			 * proper datasheet to fix this.
+			 *
+			 * The following scan modes are supported by the
+			 * hardware:
+			 *   1) Single software scan
+			 *   2) Single hardware triggered scan
+			 *   3) Continuous software scan
+			 *   4) Continuous software scan with timer delay
+			 *   5) Continuous hardware triggered scan
+			 *   6) Continuous hardware triggered scan with timer
+			 *      delay
+			 *
+			 * For now, limit the chanlist to a single channel.
+			 */
 			dev->read_subdev = s;
 			s->subdev_flags	|= SDF_CMD_READ;
+			s->len_chanlist	= 1;
 			s->do_cmdtest	= apci3xxx_ai_cmdtest;
 			s->do_cmd	= apci3xxx_ai_cmd;
 			s->cancel	= apci3xxx_ai_cancel;
diff --git a/drivers/staging/comedi/drivers/adl_pci9111.c b/drivers/staging/comedi/drivers/adl_pci9111.c
index a29ceacb966..584fd57ecb7 100644
--- a/drivers/staging/comedi/drivers/adl_pci9111.c
+++ b/drivers/staging/comedi/drivers/adl_pci9111.c
@@ -137,10 +137,8 @@ struct pci9111_private_data {
 	unsigned long lcr_io_base;
 
 	int stop_counter;
-	int stop_is_none;
 
 	unsigned int scan_delay;
-	unsigned int chanlist_len;
 	unsigned int chunk_counter;
 	unsigned int chunk_num_samples;
 
@@ -314,148 +312,137 @@ static int pci9111_ai_cancel(struct comedi_device *dev,
 	return 0;
 }
 
+static int pci9111_ai_check_chanlist(struct comedi_device *dev,
+				     struct comedi_subdevice *s,
+				     struct comedi_cmd *cmd)
+{
+	unsigned int range0 = CR_RANGE(cmd->chanlist[0]);
+	unsigned int aref0 = CR_AREF(cmd->chanlist[0]);
+	int i;
+
+	for (i = 1; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+		unsigned int range = CR_RANGE(cmd->chanlist[i]);
+		unsigned int aref = CR_AREF(cmd->chanlist[i]);
+
+		if (chan != i) {
+			dev_dbg(dev->class_dev,
+				"entries in chanlist must be consecutive channels,counting upwards from 0\n");
+			return -EINVAL;
+		}
+
+		if (range != range0) {
+			dev_dbg(dev->class_dev,
+				"entries in chanlist must all have the same gain\n");
+			return -EINVAL;
+		}
+
+		if (aref != aref0) {
+			dev_dbg(dev->class_dev,
+				"entries in chanlist must all have the same reference\n");
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
 static int pci9111_ai_do_cmd_test(struct comedi_device *dev,
 				  struct comedi_subdevice *s,
 				  struct comedi_cmd *cmd)
 {
 	struct pci9111_private_data *dev_private = dev->private;
-	int tmp;
-	int error = 0;
-	int range, reference;
-	int i;
+	int err = 0;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
-	error |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	error |= cfc_check_trigger_src(&cmd->scan_begin_src,
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
 					TRIG_TIMER | TRIG_FOLLOW | TRIG_EXT);
-	error |= cfc_check_trigger_src(&cmd->convert_src,
+	err |= cfc_check_trigger_src(&cmd->convert_src,
 					TRIG_TIMER | TRIG_EXT);
-	error |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	error |= cfc_check_trigger_src(&cmd->stop_src,
+	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= cfc_check_trigger_src(&cmd->stop_src,
 					TRIG_COUNT | TRIG_NONE);
 
-	if (error)
+	if (err)
 		return 1;
 
 	/* Step 2a : make sure trigger sources are unique */
 
-	error |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	error |= cfc_check_trigger_is_unique(cmd->convert_src);
-	error |= cfc_check_trigger_is_unique(cmd->stop_src);
+	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= cfc_check_trigger_is_unique(cmd->convert_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
 
 	/* Step 2b : and mutually compatible */
 
-	if ((cmd->convert_src == TRIG_TIMER) &&
-	    !((cmd->scan_begin_src == TRIG_TIMER) ||
-	      (cmd->scan_begin_src == TRIG_FOLLOW)))
-		error |= -EINVAL;
-	if ((cmd->convert_src == TRIG_EXT) &&
-	    !((cmd->scan_begin_src == TRIG_EXT) ||
-	      (cmd->scan_begin_src == TRIG_FOLLOW)))
-		error |= -EINVAL;
+	if (cmd->scan_begin_src != TRIG_FOLLOW) {
+		if (cmd->scan_begin_src != cmd->convert_src)
+			err |= -EINVAL;
+	}
 
-	if (error)
+	if (err)
 		return 2;
 
 	/* Step 3: check if arguments are trivially valid */
 
-	error |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
 
 	if (cmd->convert_src == TRIG_TIMER)
-		error |= cfc_check_trigger_arg_min(&cmd->convert_arg,
+		err |= cfc_check_trigger_arg_min(&cmd->convert_arg,
 					PCI9111_AI_ACQUISITION_PERIOD_MIN_NS);
 	else	/* TRIG_EXT */
-		error |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
 
 	if (cmd->scan_begin_src == TRIG_TIMER)
-		error |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
+		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
 					PCI9111_AI_ACQUISITION_PERIOD_MIN_NS);
 	else	/* TRIG_FOLLOW || TRIG_EXT */
-		error |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
 
-	error |= cfc_check_trigger_arg_is(&cmd->scan_end_arg,
-					  cmd->chanlist_len);
+	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		error |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
-		error |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
 
-	if (error)
+	if (err)
 		return 3;
 
-	/*  Step 4 : fix up any arguments */
+	/* Step 4: fix up any arguments */
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		tmp = cmd->convert_arg;
+		arg = cmd->convert_arg;
 		i8253_cascade_ns_to_timer(I8254_OSC_BASE_2MHZ,
 					  &dev_private->div1,
 					  &dev_private->div2,
-					  &cmd->convert_arg, cmd->flags);
-		if (tmp != cmd->convert_arg)
-			error++;
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
-	/*  There's only one timer on this card, so the scan_begin timer must */
-	/*  be a multiple of chanlist_len*convert_arg */
 
+	/*
+	 * There's only one timer on this card, so the scan_begin timer
+	 * must be a multiple of chanlist_len*convert_arg
+	 */
 	if (cmd->scan_begin_src == TRIG_TIMER) {
+		arg = cmd->chanlist_len * cmd->convert_arg;
 
-		unsigned int scan_begin_min;
-		unsigned int scan_begin_arg;
-		unsigned int scan_factor;
-
-		scan_begin_min = cmd->chanlist_len * cmd->convert_arg;
-
-		if (cmd->scan_begin_arg != scan_begin_min) {
-			if (scan_begin_min < cmd->scan_begin_arg) {
-				scan_factor =
-				    cmd->scan_begin_arg / scan_begin_min;
-				scan_begin_arg = scan_factor * scan_begin_min;
-				if (cmd->scan_begin_arg != scan_begin_arg) {
-					cmd->scan_begin_arg = scan_begin_arg;
-					error++;
-				}
-			} else {
-				cmd->scan_begin_arg = scan_begin_min;
-				error++;
-			}
-		}
+		if (arg < cmd->scan_begin_arg)
+			arg *= (cmd->scan_begin_arg / arg);
+
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 
-	if (error)
+	if (err)
 		return 4;
 
-	/*  Step 5 : check channel list */
-
-	if (cmd->chanlist) {
-
-		range = CR_RANGE(cmd->chanlist[0]);
-		reference = CR_AREF(cmd->chanlist[0]);
-
-		if (cmd->chanlist_len > 1) {
-			for (i = 0; i < cmd->chanlist_len; i++) {
-				if (CR_CHAN(cmd->chanlist[i]) != i) {
-					comedi_error(dev,
-						     "entries in chanlist must be consecutive "
-						     "channels,counting upwards from 0\n");
-					error++;
-				}
-				if (CR_RANGE(cmd->chanlist[i]) != range) {
-					comedi_error(dev,
-						     "entries in chanlist must all have the same gain\n");
-					error++;
-				}
-				if (CR_AREF(cmd->chanlist[i]) != reference) {
-					comedi_error(dev,
-						     "entries in chanlist must all have the same reference\n");
-					error++;
-				}
-			}
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= pci9111_ai_check_chanlist(dev, s, cmd);
 
-	if (error)
+	if (err)
 		return 5;
 
 	return 0;
@@ -466,18 +453,18 @@ static int pci9111_ai_do_cmd(struct comedi_device *dev,
 			     struct comedi_subdevice *s)
 {
 	struct pci9111_private_data *dev_private = dev->private;
-	struct comedi_cmd *async_cmd = &s->async->cmd;
+	struct comedi_cmd *cmd = &s->async->cmd;
 
 	/*  Set channel scan limit */
 	/*  PCI9111 allows only scanning from channel 0 to channel n */
 	/*  TODO: handle the case of an external multiplexer */
 
-	if (async_cmd->chanlist_len > 1) {
-		outb(async_cmd->chanlist_len - 1,
+	if (cmd->chanlist_len > 1) {
+		outb(cmd->chanlist_len - 1,
 			dev->iobase + PCI9111_AI_CHANNEL_REG);
 		pci9111_autoscan_set(dev, true);
 	} else {
-		outb(CR_CHAN(async_cmd->chanlist[0]),
+		outb(CR_CHAN(cmd->chanlist[0]),
 			dev->iobase + PCI9111_AI_CHANNEL_REG);
 		pci9111_autoscan_set(dev, false);
 	}
@@ -485,22 +472,18 @@ static int pci9111_ai_do_cmd(struct comedi_device *dev,
 	/*  Set gain */
 	/*  This is the same gain on every channel */
 
-	outb(CR_RANGE(async_cmd->chanlist[0]) & PCI9111_AI_RANGE_MASK,
+	outb(CR_RANGE(cmd->chanlist[0]) & PCI9111_AI_RANGE_MASK,
 		dev->iobase + PCI9111_AI_RANGE_STAT_REG);
 
 	/* Set counter */
-	if (async_cmd->stop_src == TRIG_COUNT) {
-		dev_private->stop_counter =
-		    async_cmd->stop_arg * async_cmd->chanlist_len;
-		dev_private->stop_is_none = 0;
-	} else {	/* TRIG_NONE */
+	if (cmd->stop_src == TRIG_COUNT)
+		dev_private->stop_counter = cmd->stop_arg * cmd->chanlist_len;
+	else	/* TRIG_NONE */
 		dev_private->stop_counter = 0;
-		dev_private->stop_is_none = 1;
-	}
 
 	/*  Set timer pacer */
 	dev_private->scan_delay = 0;
-	if (async_cmd->convert_src == TRIG_TIMER) {
+	if (cmd->convert_src == TRIG_TIMER) {
 		pci9111_trigger_source_set(dev, software);
 		pci9111_timer_set(dev);
 		pci9111_fifo_reset(dev);
@@ -510,11 +493,9 @@ static int pci9111_ai_do_cmd(struct comedi_device *dev,
 		plx9050_interrupt_control(dev_private->lcr_io_base, true, true,
 					  false, true, true);
 
-		if (async_cmd->scan_begin_src == TRIG_TIMER) {
-			dev_private->scan_delay =
-				(async_cmd->scan_begin_arg /
-				 (async_cmd->convert_arg *
-				  async_cmd->chanlist_len)) - 1;
+		if (cmd->scan_begin_src == TRIG_TIMER) {
+			dev_private->scan_delay = (cmd->scan_begin_arg /
+				(cmd->convert_arg * cmd->chanlist_len)) - 1;
 		}
 	} else {	/* TRIG_EXT */
 		pci9111_trigger_source_set(dev, external);
@@ -527,10 +508,9 @@ static int pci9111_ai_do_cmd(struct comedi_device *dev,
 	}
 
 	dev_private->stop_counter *= (1 + dev_private->scan_delay);
-	dev_private->chanlist_len = async_cmd->chanlist_len;
 	dev_private->chunk_counter = 0;
-	dev_private->chunk_num_samples =
-	    dev_private->chanlist_len * (1 + dev_private->scan_delay);
+	dev_private->chunk_num_samples = cmd->chanlist_len *
+					 (1 + dev_private->scan_delay);
 
 	return 0;
 }
@@ -551,12 +531,71 @@ static void pci9111_ai_munge(struct comedi_device *dev,
 		array[i] = ((array[i] >> shift) & maxdata) ^ invert;
 }
 
+static void pci9111_handle_fifo_half_full(struct comedi_device *dev,
+					  struct comedi_subdevice *s)
+{
+	struct pci9111_private_data *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
+	unsigned int total = 0;
+	unsigned int samples;
+
+	if (cmd->stop_src == TRIG_COUNT &&
+	    PCI9111_FIFO_HALF_SIZE > devpriv->stop_counter)
+		samples = devpriv->stop_counter;
+	else
+		samples = PCI9111_FIFO_HALF_SIZE;
+
+	insw(dev->iobase + PCI9111_AI_FIFO_REG,
+	     devpriv->ai_bounce_buffer, samples);
+
+	if (devpriv->scan_delay < 1) {
+		total = cfc_write_array_to_buffer(s,
+						  devpriv->ai_bounce_buffer,
+						  samples * sizeof(short));
+	} else {
+		unsigned int pos = 0;
+		unsigned int to_read;
+
+		while (pos < samples) {
+			if (devpriv->chunk_counter < cmd->chanlist_len) {
+				to_read = cmd->chanlist_len -
+					  devpriv->chunk_counter;
+
+				if (to_read > samples - pos)
+					to_read = samples - pos;
+
+				total += cfc_write_array_to_buffer(s,
+						devpriv->ai_bounce_buffer + pos,
+						to_read * sizeof(short));
+			} else {
+				to_read = devpriv->chunk_num_samples -
+					  devpriv->chunk_counter;
+
+				if (to_read > samples - pos)
+					to_read = samples - pos;
+
+				total += to_read * sizeof(short);
+			}
+
+			pos += to_read;
+			devpriv->chunk_counter += to_read;
+
+			if (devpriv->chunk_counter >=
+			    devpriv->chunk_num_samples)
+				devpriv->chunk_counter = 0;
+		}
+	}
+
+	devpriv->stop_counter -= total / sizeof(short);
+}
+
 static irqreturn_t pci9111_interrupt(int irq, void *p_device)
 {
 	struct comedi_device *dev = p_device;
 	struct pci9111_private_data *dev_private = dev->private;
 	struct comedi_subdevice *s = dev->read_subdev;
 	struct comedi_async *async;
+	struct comedi_cmd *cmd;
 	unsigned int status;
 	unsigned long irq_flags;
 	unsigned char intcsr;
@@ -568,6 +607,7 @@ static irqreturn_t pci9111_interrupt(int irq, void *p_device)
 	}
 
 	async = s->async;
+	cmd = &async->cmd;
 
 	spin_lock_irqsave(&dev->spinlock, irq_flags);
 
@@ -599,79 +639,11 @@ static irqreturn_t pci9111_interrupt(int irq, void *p_device)
 		}
 
 		/* '0' means FIFO is half-full */
-		if (!(status & PCI9111_AI_STAT_FF_HF)) {
-			unsigned int num_samples;
-			unsigned int bytes_written = 0;
-
-			num_samples =
-			    PCI9111_FIFO_HALF_SIZE >
-			    dev_private->stop_counter
-			    && !dev_private->
-			    stop_is_none ? dev_private->stop_counter :
-			    PCI9111_FIFO_HALF_SIZE;
-			insw(dev->iobase + PCI9111_AI_FIFO_REG,
-			     dev_private->ai_bounce_buffer, num_samples);
-
-			if (dev_private->scan_delay < 1) {
-				bytes_written =
-				    cfc_write_array_to_buffer(s,
-							      dev_private->
-							      ai_bounce_buffer,
-							      num_samples *
-							      sizeof(short));
-			} else {
-				int position = 0;
-				int to_read;
-
-				while (position < num_samples) {
-					if (dev_private->chunk_counter <
-					    dev_private->chanlist_len) {
-						to_read =
-						    dev_private->chanlist_len -
-						    dev_private->chunk_counter;
-
-						if (to_read >
-						    num_samples - position)
-							to_read =
-							    num_samples -
-							    position;
-
-						bytes_written +=
-						    cfc_write_array_to_buffer
-						    (s,
-						     dev_private->ai_bounce_buffer
-						     + position,
-						     to_read * sizeof(short));
-					} else {
-						to_read =
-						    dev_private->chunk_num_samples
-						    -
-						    dev_private->chunk_counter;
-						if (to_read >
-						    num_samples - position)
-							to_read =
-							    num_samples -
-							    position;
-
-						bytes_written +=
-						    sizeof(short) * to_read;
-					}
-
-					position += to_read;
-					dev_private->chunk_counter += to_read;
-
-					if (dev_private->chunk_counter >=
-					    dev_private->chunk_num_samples)
-						dev_private->chunk_counter = 0;
-				}
-			}
-
-			dev_private->stop_counter -=
-			    bytes_written / sizeof(short);
-		}
+		if (!(status & PCI9111_AI_STAT_FF_HF))
+			pci9111_handle_fifo_half_full(dev, s);
 	}
 
-	if (dev_private->stop_counter == 0 && !dev_private->stop_is_none)
+	if (cmd->stop_src == TRIG_COUNT && dev_private->stop_counter == 0)
 		async->events |= COMEDI_CB_EOA;
 
 	outb(0, dev->iobase + PCI9111_INT_CLR_REG);
diff --git a/drivers/staging/comedi/drivers/adl_pci9118.c b/drivers/staging/comedi/drivers/adl_pci9118.c
index 3cfa1756fa6..59a65cbc6db 100644
--- a/drivers/staging/comedi/drivers/adl_pci9118.c
+++ b/drivers/staging/comedi/drivers/adl_pci9118.c
@@ -96,7 +96,7 @@ Configuration options:
 				 * correct channel number on every 12 bit sample
 				 */
 
-#define IORANGE_9118 	64	/* I hope */
+#define IORANGE_9118	64	/* I hope */
 #define PCI9118_CHANLEN	255	/*
 				 * len of chanlist, some source say 256,
 				 * but reality looks like 255 :-(
@@ -320,14 +320,9 @@ struct pci9118_private {
 	unsigned char AdControlReg;	/* A/D control register */
 	unsigned char IntControlReg;	/* Interrupt control register */
 	unsigned char AdFunctionReg;	/* A/D function register */
-	char valid;			/* driver is ok */
 	char ai_neverending;		/* we do unlimited AI */
-	unsigned int i8254_osc_base;	/* frequence of onboard oscilator */
 	unsigned int ai_do;		/* what do AI? 0=nothing, 1 to 4 mode */
 	unsigned int ai_act_scan;	/* how many scans we finished */
-	unsigned int ai_buf_ptr;	/* data buffer ptr in samples */
-	unsigned int ai_n_chan;		/* how many channels is measured */
-	unsigned int ai_n_scanlen;	/* len of actual scanlist */
 	unsigned int ai_n_realscanlen;	/*
 					 * what we must transfer for one
 					 * outgoing scan include front/back adds
@@ -341,9 +336,6 @@ struct pci9118_private {
 					 * how many channels we must add
 					 * before scan to satisfy DMA?
 					 */
-	unsigned int *ai_chanlist;	/* actual chanlist */
-	unsigned int ai_timer1;
-	unsigned int ai_timer2;
 	unsigned int ai_flags;
 	char ai12_startstop;		/*
 					 * measure can start/stop
@@ -353,9 +345,7 @@ struct pci9118_private {
 						 * divisors for start of measure
 						 * on external start
 						 */
-	unsigned int ai_data_len;
 	unsigned short ao_data[2];		/* data output buffer */
-	unsigned int ai_scans;			/* number of scans to do */
 	char dma_doublebuf;			/* we can use double buffering */
 	unsigned int dma_actbuf;		/* which buffer is used now */
 	unsigned short *dmabuf_virt[2];		/*
@@ -372,31 +362,12 @@ struct pci9118_private {
 						 */
 	unsigned int dmabuf_used_size[2];	/* which size was truly used */
 	unsigned int dmabuf_panic_size[2];
-	unsigned int dmabuf_samples[2];		/* size in samples */
 	int dmabuf_pages[2];			/* number of pages in buffer */
-	unsigned char cnt0_users;		/*
-						 * bit field of 8254 CNT0 users
-						 * (0-unused, 1-AO, 2-DI, 3-DO)
-						 */
 	unsigned char exttrg_users;		/*
 						 * bit field of external trigger
 						 * users(0-AI, 1-AO, 2-DI, 3-DO)
 						 */
-	unsigned int cnt0_divisor;		/* actual CNT0 divisor */
-	void (*int_ai_func) (struct comedi_device *, struct comedi_subdevice *,
-		unsigned short,
-		unsigned int,
-		unsigned short);	/*
-					 * ptr to actual interrupt
-					 * AI function
-					 */
-	unsigned char ai16bits;		/* =1 16 bit card */
 	unsigned char usedma;		/* =1 use DMA transfer and not INT */
-	unsigned char useeoshandle;	/*
-					 * =1 change WAKE_EOS DMA transfer
-					 * to fit on every second
-					 */
-	unsigned char usessh;		/* =1 turn on S&H support */
 	int softsshdelay;		/*
 					 * >0 use software S&H,
 					 * numer is requested delay in ns
@@ -411,7 +382,6 @@ struct pci9118_private {
 					 */
 	unsigned int ai_maskerr;	/* which warning was printed */
 	unsigned int ai_maskharderr;	/* on which error bits stops */
-	unsigned int ai_inttrig_start;	/* TRIG_INT for start */
 };
 
 static int check_channel_list(struct comedi_device *dev,
@@ -468,7 +438,7 @@ static int check_channel_list(struct comedi_device *dev,
 static int setup_channel_list(struct comedi_device *dev,
 			      struct comedi_subdevice *s, int n_chan,
 			      unsigned int *chanlist, int rot, int frontadd,
-			      int backadd, int usedma, char useeos)
+			      int backadd, int usedma)
 {
 	struct pci9118_private *devpriv = dev->private;
 	unsigned int i, differencial = 0, bipolar = 0;
@@ -552,18 +522,6 @@ static int setup_channel_list(struct comedi_device *dev,
 #ifdef PCI9118_PARANOIDCHECK
 	devpriv->chanlist[n_chan ^ usedma] = devpriv->chanlist[0 ^ usedma];
 						/* for 32bit operations */
-	if (useeos) {
-		for (i = 1; i < n_chan; i++) {	/* store range list to card */
-			devpriv->chanlist[(n_chan + i) ^ usedma] =
-			    (CR_CHAN(chanlist[i]) & 0xf) << rot;
-		}
-		devpriv->chanlist[(2 * n_chan) ^ usedma] =
-						devpriv->chanlist[0 ^ usedma];
-						/* for 32bit operations */
-		useeos = 2;
-	} else {
-		useeos = 1;
-	}
 #endif
 	outl(0, dev->iobase + PCI9118_SCANMOD);	/* close scan queue */
 	/* udelay(100); important delay, or first sample will be crippled */
@@ -603,7 +561,7 @@ static int pci9118_insn_read_ai(struct comedi_device *dev,
 						 * trigger stop
 						 */
 
-	if (!setup_channel_list(dev, s, 1, &insn->chanspec, 0, 0, 0, 0, 0))
+	if (!setup_channel_list(dev, s, 1, &insn->chanspec, 0, 0, 0, 0))
 		return -EINVAL;
 
 	outl(0, dev->iobase + PCI9118_DELFIFO);	/* flush FIFO */
@@ -618,7 +576,7 @@ static int pci9118_insn_read_ai(struct comedi_device *dev,
 			return ret;
 		}
 
-		if (devpriv->ai16bits) {
+		if (s->maxdata == 0xffff) {
 			data[n] =
 			    (inl(dev->iobase +
 				 PCI9118_AD_DATA) & 0xffff) ^ 0x8000;
@@ -713,10 +671,11 @@ static unsigned int defragment_dma_buffer(struct comedi_device *dev,
 					  unsigned int num_samples)
 {
 	struct pci9118_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned int i = 0, j = 0;
 	unsigned int start_pos = devpriv->ai_add_front,
-	    stop_pos = devpriv->ai_add_front + devpriv->ai_n_chan;
-	unsigned int raw_scanlen = devpriv->ai_add_front + devpriv->ai_n_chan +
+	    stop_pos = devpriv->ai_add_front + cmd->chanlist_len;
+	unsigned int raw_scanlen = devpriv->ai_add_front + cmd->chanlist_len +
 	    devpriv->ai_add_back;
 
 	for (i = 0; i < num_samples; i++) {
@@ -737,13 +696,14 @@ static int move_block_from_dma(struct comedi_device *dev,
 					unsigned int num_samples)
 {
 	struct pci9118_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned int num_bytes;
 
 	num_samples = defragment_dma_buffer(dev, s, dma_buffer, num_samples);
 	devpriv->ai_act_scan +=
-	    (s->async->cur_chan + num_samples) / devpriv->ai_n_scanlen;
+	    (s->async->cur_chan + num_samples) / cmd->scan_end_arg;
 	s->async->cur_chan += num_samples;
-	s->async->cur_chan %= devpriv->ai_n_scanlen;
+	s->async->cur_chan %= cmd->scan_end_arg;
 	num_bytes =
 	    cfc_write_array_to_buffer(s, dma_buffer,
 				      num_samples * sizeof(short));
@@ -791,47 +751,51 @@ static void pci9118_calc_divisors(char mode, struct comedi_device *dev,
 				  unsigned int *tim1, unsigned int *tim2,
 				  unsigned int flags, int chans,
 				  unsigned int *div1, unsigned int *div2,
-				  char usessh, unsigned int chnsshfront)
+				  unsigned int chnsshfront)
 {
 	const struct boardtype *this_board = comedi_board(dev);
-	struct pci9118_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 
 	switch (mode) {
 	case 1:
 	case 4:
 		if (*tim2 < this_board->ai_ns_min)
 			*tim2 = this_board->ai_ns_min;
-		i8253_cascade_ns_to_timer(devpriv->i8254_osc_base,
+		i8253_cascade_ns_to_timer(I8254_OSC_BASE_4MHZ,
 					  div1, div2,
 					  tim2, flags & TRIG_ROUND_NEAREST);
 		break;
 	case 2:
 		if (*tim2 < this_board->ai_ns_min)
 			*tim2 = this_board->ai_ns_min;
-		*div1 = *tim2 / devpriv->i8254_osc_base;
+		*div1 = *tim2 / I8254_OSC_BASE_4MHZ;
 						/* convert timer (burst) */
 		if (*div1 < this_board->ai_pacer_min)
 			*div1 = this_board->ai_pacer_min;
-		*div2 = *tim1 / devpriv->i8254_osc_base;	/* scan timer */
+		*div2 = *tim1 / I8254_OSC_BASE_4MHZ;	/* scan timer */
 		*div2 = *div2 / *div1;		/* major timer is c1*c2 */
 		if (*div2 < chans)
 			*div2 = chans;
 
-		*tim2 = *div1 * devpriv->i8254_osc_base;
-							/* real convert timer */
+		*tim2 = *div1 * I8254_OSC_BASE_4MHZ;	/* real convert timer */
 
-		if (usessh && (chnsshfront == 0))	/* use BSSH signal */
+		if (cmd->convert_src == TRIG_NOW && !chnsshfront) {
+			/* use BSSH signal */
 			if (*div2 < (chans + 2))
 				*div2 = chans + 2;
+		}
 
-		*tim1 = *div1 * *div2 * devpriv->i8254_osc_base;
+		*tim1 = *div1 * *div2 * I8254_OSC_BASE_4MHZ;
 		break;
 	}
 }
 
-static void start_pacer(struct comedi_device *dev, int mode,
-			unsigned int divisor1, unsigned int divisor2)
+static void pci9118_start_pacer(struct comedi_device *dev, int mode)
 {
+	struct pci9118_private *devpriv = dev->private;
+	unsigned int divisor1 = devpriv->ai_divisor1;
+	unsigned int divisor2 = devpriv->ai_divisor2;
+
 	outl(0x74, dev->iobase + PCI9118_CNTCTRL);
 	outl(0xb4, dev->iobase + PCI9118_CNTCTRL);
 /* outl(0x30, dev->iobase + PCI9118_CNTCTRL); */
@@ -855,7 +819,7 @@ static int pci9118_ai_cancel(struct comedi_device *dev,
 			(~EN_A2P_TRANSFERS),
 			devpriv->iobase_a + AMCC_OP_REG_MCSR);	/* stop DMA */
 	pci9118_exttrg_del(dev, EXTTRG_AI);
-	start_pacer(dev, 0, 0, 0);	/* stop 8254 counters */
+	pci9118_start_pacer(dev, 0);	/* stop 8254 counters */
 	devpriv->AdFunctionReg = AdFunction_PDTrg | AdFunction_PETrg;
 	outl(devpriv->AdFunctionReg, dev->iobase + PCI9118_ADFUNC);
 					/*
@@ -882,7 +846,6 @@ static int pci9118_ai_cancel(struct comedi_device *dev,
 	devpriv->ai_act_dmapos = 0;
 	s->async->cur_chan = 0;
 	s->async->inttrig = NULL;
-	devpriv->ai_buf_ptr = 0;
 	devpriv->ai_neverending = 0;
 	devpriv->dma_actbuf = 0;
 
@@ -938,7 +901,7 @@ static void pci9118_ai_munge(struct comedi_device *dev,
 	for (i = 0; i < num_samples; i++) {
 		if (devpriv->usedma)
 			array[i] = be16_to_cpu(array[i]);
-		if (devpriv->ai16bits)
+		if (s->maxdata == 0xffff)
 			array[i] ^= 0x8000;
 		else
 			array[i] = (array[i] >> 4) & 0x0fff;
@@ -953,6 +916,7 @@ static void interrupt_pci9118_ai_onesample(struct comedi_device *dev,
 					   unsigned short int_daq)
 {
 	struct pci9118_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned short sampl;
 
 	if (int_adstat & devpriv->ai_maskerr)
@@ -962,7 +926,7 @@ static void interrupt_pci9118_ai_onesample(struct comedi_device *dev,
 	sampl = inw(dev->iobase + PCI9118_AD_DATA);
 
 #ifdef PCI9118_PARANOIDCHECK
-	if (devpriv->ai16bits == 0) {
+	if (s->maxdata != 0xffff) {
 		if ((sampl & 0x000f) != devpriv->chanlist[s->async->cur_chan]) {
 							/* data dropout! */
 			dev_info(dev->class_dev,
@@ -977,13 +941,13 @@ static void interrupt_pci9118_ai_onesample(struct comedi_device *dev,
 #endif
 	cfc_write_to_buffer(s, sampl);
 	s->async->cur_chan++;
-	if (s->async->cur_chan >= devpriv->ai_n_scanlen) {
+	if (s->async->cur_chan >= cmd->scan_end_arg) {
 							/* one scan done */
-		s->async->cur_chan %= devpriv->ai_n_scanlen;
+		s->async->cur_chan %= cmd->scan_end_arg;
 		devpriv->ai_act_scan++;
 		if (!devpriv->ai_neverending) {
 			/* all data sampled? */
-			if (devpriv->ai_act_scan >= devpriv->ai_scans)
+			if (devpriv->ai_act_scan >= cmd->stop_arg)
 				s->async->events |= COMEDI_CB_EOA;
 		}
 	}
@@ -998,6 +962,7 @@ static void interrupt_pci9118_ai_dma(struct comedi_device *dev,
 				     unsigned short int_daq)
 {
 	struct pci9118_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned int next_dma_buf, samplesinbuf, sampls, m;
 
 	if (int_amcc & MASTER_ABORT_INT) {
@@ -1037,20 +1002,18 @@ static void interrupt_pci9118_ai_dma(struct comedi_device *dev,
 	}
 
 	if (samplesinbuf) {
-		m = devpriv->ai_data_len >> 1;	/*
-						 * how many samples is to
-						 * end of buffer
-						 */
+		/* how many samples is to end of buffer */
+		m = s->async->prealloc_bufsz >> 1;
 		sampls = m;
 		move_block_from_dma(dev, s,
 				    devpriv->dmabuf_virt[devpriv->dma_actbuf],
 				    samplesinbuf);
-		m = m - sampls;		/* m= how many samples was transferred */
+		m = m - sampls;		/* m=how many samples was transferred */
 	}
 
 	if (!devpriv->ai_neverending) {
 		/* all data sampled? */
-		if (devpriv->ai_act_scan >= devpriv->ai_scans)
+		if (devpriv->ai_act_scan >= cmd->stop_arg)
 			s->async->events |= COMEDI_CB_EOA;
 	}
 
@@ -1068,78 +1031,71 @@ static void interrupt_pci9118_ai_dma(struct comedi_device *dev,
 	cfc_handle_events(dev, s);
 }
 
-static irqreturn_t interrupt_pci9118(int irq, void *d)
+static irqreturn_t pci9118_interrupt(int irq, void *d)
 {
 	struct comedi_device *dev = d;
+	struct comedi_subdevice *s = dev->read_subdev;
 	struct pci9118_private *devpriv = dev->private;
-	unsigned int int_daq = 0, int_amcc, int_adstat;
+	unsigned int intsrc;	/* IRQ reasons from card */
+	unsigned int intcsr;	/* INT register from AMCC chip */
+	unsigned int adstat;	/* STATUS register */
 
 	if (!dev->attached)
-		return IRQ_NONE;	/* not fully initialized */
-
-	int_daq = inl(dev->iobase + PCI9118_INTSRC) & 0xf;
-					/* get IRQ reasons from card */
-	int_amcc = inl(devpriv->iobase_a + AMCC_OP_REG_INTCSR);
-					/* get INT register from AMCC chip */
-
-	if ((!int_daq) && (!(int_amcc & ANY_S593X_INT)))
-		return IRQ_NONE;	/* interrupt from other source */
-
-	outl(int_amcc | 0x00ff0000, devpriv->iobase_a + AMCC_OP_REG_INTCSR);
-					/* shutdown IRQ reasons in AMCC */
-
-	int_adstat = inw(dev->iobase + PCI9118_ADSTAT) & 0x1ff;
-					/* get STATUS register */
-
-	if (devpriv->ai_do) {
-		if (devpriv->ai12_startstop)
-			if ((int_adstat & AdStatus_DTH) &&
-							(int_daq & Int_DTrg)) {
-						/* start stop of measure */
-				if (devpriv->ai12_startstop & START_AI_EXT) {
-					devpriv->ai12_startstop &=
-					    ~START_AI_EXT;
-					if (!(devpriv->ai12_startstop &
-							STOP_AI_EXT))
-							pci9118_exttrg_del
-							(dev, EXTTRG_AI);
-						/* deactivate EXT trigger */
-					start_pacer(dev, devpriv->ai_do,
-						devpriv->ai_divisor1,
-						devpriv->ai_divisor2);
-						/* start pacer */
-					outl(devpriv->AdControlReg,
-						dev->iobase + PCI9118_ADCNTRL);
-				} else {
-					if (devpriv->ai12_startstop &
-						STOP_AI_EXT) {
-						devpriv->ai12_startstop &=
-							~STOP_AI_EXT;
-						pci9118_exttrg_del
-							(dev, EXTTRG_AI);
-						/* deactivate EXT trigger */
-						devpriv->ai_neverending = 0;
-						/*
-						 * well, on next interrupt from
-						 * DMA/EOC measure will stop
-						 */
-					}
-				}
-			}
+		return IRQ_NONE;
+
+	intsrc = inl(dev->iobase + PCI9118_INTSRC) & 0xf;
+	intcsr = inl(devpriv->iobase_a + AMCC_OP_REG_INTCSR);
+
+	if (!intsrc && !(intcsr & ANY_S593X_INT))
+		return IRQ_NONE;
+
+	outl(intcsr | 0x00ff0000, devpriv->iobase_a + AMCC_OP_REG_INTCSR);
+
+	adstat = inw(dev->iobase + PCI9118_ADSTAT) & 0x1ff;
 
-		(devpriv->int_ai_func) (dev, dev->read_subdev, int_adstat,
-					int_amcc, int_daq);
+	if (!devpriv->ai_do)
+		return IRQ_HANDLED;
 
+	if (devpriv->ai12_startstop) {
+		if ((adstat & AdStatus_DTH) && (intsrc & Int_DTrg)) {
+			/* start/stop of measure */
+			if (devpriv->ai12_startstop & START_AI_EXT) {
+				/* deactivate EXT trigger */
+				devpriv->ai12_startstop &= ~START_AI_EXT;
+				if (!(devpriv->ai12_startstop & STOP_AI_EXT))
+					pci9118_exttrg_del(dev, EXTTRG_AI);
+
+				/* start pacer */
+				pci9118_start_pacer(dev, devpriv->ai_do);
+				outl(devpriv->AdControlReg,
+				     dev->iobase + PCI9118_ADCNTRL);
+			} else if (devpriv->ai12_startstop & STOP_AI_EXT) {
+				/* deactivate EXT trigger */
+				devpriv->ai12_startstop &= ~STOP_AI_EXT;
+				pci9118_exttrg_del(dev, EXTTRG_AI);
+
+				/* on next interrupt measure will stop */
+				devpriv->ai_neverending = 0;
+			}
+		}
 	}
+
+	if (devpriv->usedma)
+		interrupt_pci9118_ai_dma(dev, s, adstat, intcsr, intsrc);
+	else
+		interrupt_pci9118_ai_onesample(dev, s, adstat, intcsr, intsrc);
+
 	return IRQ_HANDLED;
 }
 
 static int pci9118_ai_inttrig(struct comedi_device *dev,
-			      struct comedi_subdevice *s, unsigned int trignum)
+			      struct comedi_subdevice *s,
+			      unsigned int trig_num)
 {
 	struct pci9118_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 
-	if (trignum != devpriv->ai_inttrig_start)
+	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
 	devpriv->ai12_startstop &= ~START_AI_INT;
@@ -1148,8 +1104,7 @@ static int pci9118_ai_inttrig(struct comedi_device *dev,
 	outl(devpriv->IntControlReg, dev->iobase + PCI9118_INTCTRL);
 	outl(devpriv->AdFunctionReg, dev->iobase + PCI9118_ADFUNC);
 	if (devpriv->ai_do != 3) {
-		start_pacer(dev, devpriv->ai_do, devpriv->ai_divisor1,
-			    devpriv->ai_divisor2);
+		pci9118_start_pacer(dev, devpriv->ai_do);
 		devpriv->AdControlReg |= AdControl_SoftG;
 	}
 	outl(devpriv->AdControlReg, dev->iobase + PCI9118_ADCNTRL);
@@ -1165,7 +1120,7 @@ static int pci9118_ai_cmdtest(struct comedi_device *dev,
 	struct pci9118_private *devpriv = dev->private;
 	int err = 0;
 	unsigned int flags;
-	int tmp;
+	unsigned int arg;
 	unsigned int divisor1 = 0, divisor2 = 0;
 
 	/* Step 1 : check if triggers are trivially valid */
@@ -1221,8 +1176,15 @@ static int pci9118_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	if (cmd->start_src & (TRIG_NOW | TRIG_EXT))
+	switch (cmd->start_src) {
+	case TRIG_NOW:
+	case TRIG_EXT:
 		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+		break;
+	case TRIG_INT:
+		/* start_arg is the internal trigger (any value) */
+		break;
+	}
 
 	if (cmd->scan_begin_src & (TRIG_FOLLOW | TRIG_EXT))
 		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
@@ -1259,8 +1221,6 @@ static int pci9118_ai_cmdtest(struct comedi_device *dev,
 		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	err |= cfc_check_trigger_arg_min(&cmd->chanlist_len, 1);
-	err |= cfc_check_trigger_arg_max(&cmd->chanlist_len,
-					 this_board->n_aichanlist);
 
 	err |= cfc_check_trigger_arg_min(&cmd->scan_end_arg,
 					 cmd->chanlist_len);
@@ -1277,45 +1237,30 @@ static int pci9118_ai_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		tmp = cmd->scan_begin_arg;
-		i8253_cascade_ns_to_timer(devpriv->i8254_osc_base,
+		arg = cmd->scan_begin_arg;
+		i8253_cascade_ns_to_timer(I8254_OSC_BASE_4MHZ,
 					  &divisor1, &divisor2,
-					  &cmd->scan_begin_arg, cmd->flags);
-		if (cmd->scan_begin_arg < this_board->ai_ns_min)
-			cmd->scan_begin_arg = this_board->ai_ns_min;
-		if (tmp != cmd->scan_begin_arg)
-			err++;
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 
 	if (cmd->convert_src & (TRIG_TIMER | TRIG_NOW)) {
-		tmp = cmd->convert_arg;
-		i8253_cascade_ns_to_timer(devpriv->i8254_osc_base,
+		arg = cmd->convert_arg;
+		i8253_cascade_ns_to_timer(I8254_OSC_BASE_4MHZ,
 					  &divisor1, &divisor2,
-					  &cmd->convert_arg, cmd->flags);
-		if (cmd->convert_arg < this_board->ai_ns_min)
-			cmd->convert_arg = this_board->ai_ns_min;
-		if (tmp != cmd->convert_arg)
-			err++;
-		if (cmd->scan_begin_src == TRIG_TIMER
-		    && cmd->convert_src == TRIG_NOW) {
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
+
+		if (cmd->scan_begin_src == TRIG_TIMER &&
+		    cmd->convert_src == TRIG_NOW) {
 			if (cmd->convert_arg == 0) {
-				if (cmd->scan_begin_arg <
-				    this_board->ai_ns_min *
-				    (cmd->scan_end_arg + 2)) {
-					cmd->scan_begin_arg =
-					    this_board->ai_ns_min *
-					    (cmd->scan_end_arg + 2);
-					err++;
-				}
+				arg = this_board->ai_ns_min *
+				      (cmd->scan_end_arg + 2);
 			} else {
-				if (cmd->scan_begin_arg <
-				    cmd->convert_arg * cmd->chanlist_len) {
-					cmd->scan_begin_arg =
-					    cmd->convert_arg *
-					    cmd->chanlist_len;
-					err++;
-				}
+				arg = cmd->convert_arg * cmd->chanlist_len;
 			}
+			err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
+							 arg);
 		}
 	}
 
@@ -1330,23 +1275,23 @@ static int pci9118_ai_cmdtest(struct comedi_device *dev,
 	return 0;
 }
 
-static int Compute_and_setup_dma(struct comedi_device *dev)
+static int Compute_and_setup_dma(struct comedi_device *dev,
+				 struct comedi_subdevice *s)
 {
 	struct pci9118_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned int dmalen0, dmalen1, i;
 
 	dmalen0 = devpriv->dmabuf_size[0];
 	dmalen1 = devpriv->dmabuf_size[1];
 	/* isn't output buff smaller that our DMA buff? */
-	if (dmalen0 > (devpriv->ai_data_len)) {
-		dmalen0 = devpriv->ai_data_len & ~3L;	/*
-							 * align to 32bit down
-							 */
+	if (dmalen0 > s->async->prealloc_bufsz) {
+		/* align to 32bit down */
+		dmalen0 = s->async->prealloc_bufsz & ~3L;
 	}
-	if (dmalen1 > (devpriv->ai_data_len)) {
-		dmalen1 = devpriv->ai_data_len & ~3L;	/*
-							 * align to 32bit down
-							 */
+	if (dmalen1 > s->async->prealloc_bufsz) {
+		/* align to 32bit down */
+		dmalen1 = s->async->prealloc_bufsz & ~3L;
 	}
 
 	/* we want wake up every scan? */
@@ -1360,8 +1305,6 @@ static int Compute_and_setup_dma(struct comedi_device *dev)
 		} else {
 			/* short first DMA buffer to one scan */
 			dmalen0 = devpriv->ai_n_realscanlen << 1;
-			if (devpriv->useeoshandle)
-				dmalen0 += 2;
 			if (dmalen0 < 4) {
 				dev_info(dev->class_dev,
 					 "ERR: DMA0 buf len bug? (%d<4)\n",
@@ -1380,8 +1323,6 @@ static int Compute_and_setup_dma(struct comedi_device *dev)
 		} else {
 			/* short second DMA buffer to one scan */
 			dmalen1 = devpriv->ai_n_realscanlen << 1;
-			if (devpriv->useeoshandle)
-				dmalen1 -= 2;
 			if (dmalen1 < 4) {
 				dev_info(dev->class_dev,
 					 "ERR: DMA1 buf len bug? (%d<4)\n",
@@ -1416,10 +1357,10 @@ static int Compute_and_setup_dma(struct comedi_device *dev)
 			/* fits whole measure into one DMA buffer? */
 			if (dmalen0 >
 			    ((devpriv->ai_n_realscanlen << 1) *
-			     devpriv->ai_scans)) {
+			     cmd->stop_arg)) {
 				dmalen0 =
 				    (devpriv->ai_n_realscanlen << 1) *
-				    devpriv->ai_scans;
+				    cmd->stop_arg;
 				dmalen0 &= ~3L;
 			} else {	/*
 					 * fits whole measure into
@@ -1427,10 +1368,10 @@ static int Compute_and_setup_dma(struct comedi_device *dev)
 					 */
 				if (dmalen1 >
 				    ((devpriv->ai_n_realscanlen << 1) *
-				     devpriv->ai_scans - dmalen0))
+				     cmd->stop_arg - dmalen0))
 					dmalen1 =
 					    (devpriv->ai_n_realscanlen << 1) *
-					    devpriv->ai_scans - dmalen0;
+					    cmd->stop_arg - dmalen0;
 				dmalen1 &= ~3L;
 			}
 		}
@@ -1442,18 +1383,18 @@ static int Compute_and_setup_dma(struct comedi_device *dev)
 	devpriv->dmabuf_use_size[1] = dmalen1;
 
 #if 0
-	if (devpriv->ai_n_scanlen < this_board->half_fifo_size) {
+	if (cmd->scan_end_arg < this_board->half_fifo_size) {
 		devpriv->dmabuf_panic_size[0] =
-		    (this_board->half_fifo_size / devpriv->ai_n_scanlen +
-		     1) * devpriv->ai_n_scanlen * sizeof(short);
+		    (this_board->half_fifo_size / cmd->scan_end_arg +
+		     1) * cmd->scan_end_arg * sizeof(short);
 		devpriv->dmabuf_panic_size[1] =
-		    (this_board->half_fifo_size / devpriv->ai_n_scanlen +
-		     1) * devpriv->ai_n_scanlen * sizeof(short);
+		    (this_board->half_fifo_size / cmd->scan_end_arg +
+		     1) * cmd->scan_end_arg * sizeof(short);
 	} else {
 		devpriv->dmabuf_panic_size[0] =
-		    (devpriv->ai_n_scanlen << 1) % devpriv->dmabuf_size[0];
+		    (cmd->scan_end_arg << 1) % devpriv->dmabuf_size[0];
 		devpriv->dmabuf_panic_size[1] =
-		    (devpriv->ai_n_scanlen << 1) % devpriv->dmabuf_size[1];
+		    (cmd->scan_end_arg << 1) % devpriv->dmabuf_size[1];
 	}
 #endif
 
@@ -1500,9 +1441,6 @@ static int pci9118_ai_docmd_sampl(struct comedi_device *dev,
 		return -EIO;
 	}
 
-	devpriv->int_ai_func = interrupt_pci9118_ai_onesample;
-						/* transfer function */
-
 	if (devpriv->ai12_startstop)
 		pci9118_exttrg_add(dev, EXTTRG_AI);
 						/* activate EXT trigger */
@@ -1520,8 +1458,7 @@ static int pci9118_ai_docmd_sampl(struct comedi_device *dev,
 		outl(devpriv->IntControlReg, dev->iobase + PCI9118_INTCTRL);
 		outl(devpriv->AdFunctionReg, dev->iobase + PCI9118_ADFUNC);
 		if (devpriv->ai_do != 3) {
-			start_pacer(dev, devpriv->ai_do, devpriv->ai_divisor1,
-				    devpriv->ai_divisor2);
+			pci9118_start_pacer(dev, devpriv->ai_do);
 			devpriv->AdControlReg |= AdControl_SoftG;
 		}
 		outl(devpriv->IntControlReg, dev->iobase + PCI9118_INTCTRL);
@@ -1534,8 +1471,9 @@ static int pci9118_ai_docmd_dma(struct comedi_device *dev,
 				struct comedi_subdevice *s)
 {
 	struct pci9118_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 
-	Compute_and_setup_dma(dev);
+	Compute_and_setup_dma(dev, s);
 
 	switch (devpriv->ai_do) {
 	case 1:
@@ -1548,7 +1486,7 @@ static int pci9118_ai_docmd_dma(struct comedi_device *dev,
 		devpriv->AdFunctionReg =
 		    AdFunction_PDTrg | AdFunction_PETrg | AdFunction_BM |
 		    AdFunction_BS;
-		if (devpriv->usessh && (!devpriv->softsshdelay))
+		if (cmd->convert_src == TRIG_NOW && !devpriv->softsshdelay)
 			devpriv->AdFunctionReg |= AdFunction_BSSH;
 		outl(devpriv->ai_n_realscanlen, dev->iobase + PCI9118_BURST);
 		break;
@@ -1580,9 +1518,6 @@ static int pci9118_ai_docmd_dma(struct comedi_device *dev,
 						/* activate EXT trigger */
 	}
 
-	devpriv->int_ai_func = interrupt_pci9118_ai_dma;
-						/* transfer function */
-
 	outl(0x02000000 | AINT_WRITE_COMPL,
 	     devpriv->iobase_a + AMCC_OP_REG_INTCSR);
 
@@ -1590,8 +1525,7 @@ static int pci9118_ai_docmd_dma(struct comedi_device *dev,
 		outl(devpriv->AdFunctionReg, dev->iobase + PCI9118_ADFUNC);
 		outl(devpriv->IntControlReg, dev->iobase + PCI9118_INTCTRL);
 		if (devpriv->ai_do != 3) {
-			start_pacer(dev, devpriv->ai_do, devpriv->ai_divisor1,
-				    devpriv->ai_divisor2);
+			pci9118_start_pacer(dev, devpriv->ai_do);
 			devpriv->AdControlReg |= AdControl_SoftG;
 		}
 		outl(devpriv->AdControlReg, dev->iobase + PCI9118_ADCNTRL);
@@ -1610,12 +1544,6 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 
 	devpriv->ai12_startstop = 0;
 	devpriv->ai_flags = cmd->flags;
-	devpriv->ai_n_chan = cmd->chanlist_len;
-	devpriv->ai_n_scanlen = cmd->scan_end_arg;
-	devpriv->ai_chanlist = cmd->chanlist;
-	devpriv->ai_data_len = s->async->prealloc_bufsz;
-	devpriv->ai_timer1 = 0;
-	devpriv->ai_timer2 = 0;
 	devpriv->ai_add_front = 0;
 	devpriv->ai_add_back = 0;
 	devpriv->ai_maskerr = 0x10e;
@@ -1629,31 +1557,12 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	}
 	if (cmd->start_src == TRIG_INT) {
 		devpriv->ai12_startstop |= START_AI_INT;
-		devpriv->ai_inttrig_start = cmd->start_arg;
 		s->async->inttrig = pci9118_ai_inttrig;
 	}
-#if 0
-	if (cmd->stop_src == TRIG_INT) {
-		devpriv->ai_neverending = 1;
-		devpriv->ai12_startstop |= STOP_AI_INT;
-	}
-#endif
 	if (cmd->stop_src == TRIG_NONE)
 		devpriv->ai_neverending = 1;
-	if (cmd->stop_src == TRIG_COUNT) {
-		devpriv->ai_scans = cmd->stop_arg;
+	if (cmd->stop_src == TRIG_COUNT)
 		devpriv->ai_neverending = 0;
-	} else {
-		devpriv->ai_scans = 0;
-	}
-
-	/* use sample&hold signal? */
-	if (cmd->convert_src == TRIG_NOW)
-		devpriv->usessh = 1;
-	/* yes */
-	else
-		devpriv->usessh = 0;
-				/*  no */
 
 	/*
 	 * use additional sample at end of every scan
@@ -1661,11 +1570,10 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	 */
 	devpriv->ai_add_front = 0;
 	devpriv->ai_add_back = 0;
-	devpriv->useeoshandle = 0;
 	if (devpriv->master) {
 		devpriv->usedma = 1;
 		if ((cmd->flags & TRIG_WAKE_EOS) &&
-		    (devpriv->ai_n_scanlen == 1)) {
+		    (cmd->scan_end_arg == 1)) {
 			if (cmd->convert_src == TRIG_NOW)
 				devpriv->ai_add_back = 1;
 			if (cmd->convert_src == TRIG_TIMER) {
@@ -1677,13 +1585,9 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 			}
 		}
 		if ((cmd->flags & TRIG_WAKE_EOS) &&
-		    (devpriv->ai_n_scanlen & 1) &&
-		    (devpriv->ai_n_scanlen > 1)) {
+		    (cmd->scan_end_arg & 1) &&
+		    (cmd->scan_end_arg > 1)) {
 			if (cmd->scan_begin_src == TRIG_FOLLOW) {
-				/*
-				 * vpriv->useeoshandle=1; // change DMA transfer
-				 * block to fit EOS on every second call
-				 */
 				devpriv->usedma = 0;
 				/*
 				 * XXX maybe can be corrected to use 16 bit DMA
@@ -1703,7 +1607,7 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	 * we need software S&H signal?
 	 * It adds two samples before every scan as minimum
 	 */
-	if (devpriv->usessh && devpriv->softsshdelay) {
+	if (cmd->convert_src == TRIG_NOW && devpriv->softsshdelay) {
 		devpriv->ai_add_front = 2;
 		if ((devpriv->usedma == 1) && (devpriv->ai_add_back == 1)) {
 							/* move it to front */
@@ -1720,7 +1624,7 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 			devpriv->ai_add_front = addchans + 1;
 			if (devpriv->usedma == 1)
 				if ((devpriv->ai_add_front +
-				     devpriv->ai_n_chan +
+				     cmd->chanlist_len +
 				     devpriv->ai_add_back) & 1)
 					devpriv->ai_add_front++;
 							/* round up to 32 bit */
@@ -1729,21 +1633,20 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	/* well, we now know what must be all added */
 	devpriv->ai_n_realscanlen =	/*
 					 * what we must take from card in real
-					 * to have ai_n_scanlen on output?
+					 * to have cmd->scan_end_arg on output?
 					 */
-	    (devpriv->ai_add_front + devpriv->ai_n_chan +
-	     devpriv->ai_add_back) * (devpriv->ai_n_scanlen /
-				      devpriv->ai_n_chan);
+	    (devpriv->ai_add_front + cmd->chanlist_len +
+	     devpriv->ai_add_back) * (cmd->scan_end_arg /
+				      cmd->chanlist_len);
 
 	/* check and setup channel list */
-	if (!check_channel_list(dev, s, devpriv->ai_n_chan,
-				devpriv->ai_chanlist, devpriv->ai_add_front,
+	if (!check_channel_list(dev, s, cmd->chanlist_len,
+				cmd->chanlist, devpriv->ai_add_front,
 				devpriv->ai_add_back))
 		return -EINVAL;
-	if (!setup_channel_list(dev, s, devpriv->ai_n_chan,
-				devpriv->ai_chanlist, 0, devpriv->ai_add_front,
-				devpriv->ai_add_back, devpriv->usedma,
-				devpriv->useeoshandle))
+	if (!setup_channel_list(dev, s, cmd->chanlist_len,
+				cmd->chanlist, 0, devpriv->ai_add_front,
+				devpriv->ai_add_back, devpriv->usedma))
 		return -EINVAL;
 
 	/* compute timers settings */
@@ -1765,9 +1668,8 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 				      devpriv->ai_flags,
 				      devpriv->ai_n_realscanlen,
 				      &devpriv->ai_divisor1,
-				      &devpriv->ai_divisor2, devpriv->usessh,
+				      &devpriv->ai_divisor2,
 				      devpriv->ai_add_front);
-		devpriv->ai_timer2 = cmd->convert_arg;
 	}
 
 	if ((cmd->scan_begin_src == TRIG_TIMER) &&
@@ -1787,10 +1689,8 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 				      devpriv->ai_flags,
 				      devpriv->ai_n_realscanlen,
 				      &devpriv->ai_divisor1,
-				      &devpriv->ai_divisor2, devpriv->usessh,
+				      &devpriv->ai_divisor2,
 				      devpriv->ai_add_front);
-		devpriv->ai_timer1 = cmd->scan_begin_arg;
-		devpriv->ai_timer2 = cmd->convert_arg;
 	}
 
 	if ((cmd->scan_begin_src == TRIG_FOLLOW)
@@ -1798,7 +1698,7 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		devpriv->ai_do = 3;
 	}
 
-	start_pacer(dev, -1, 0, 0);	/* stop pacer */
+	pci9118_start_pacer(dev, -1);	/* stop pacer */
 
 	devpriv->AdControlReg = 0;	/*
 					 * bipolar, S.E., use 8254, stop 8354,
@@ -1824,7 +1724,6 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	devpriv->ai_act_scan = 0;
 	devpriv->ai_act_dmapos = 0;
 	s->async->cur_chan = 0;
-	devpriv->ai_buf_ptr = 0;
 
 	if (devpriv->usedma)
 		ret = pci9118_ai_docmd_dma(dev, s);
@@ -1845,7 +1744,7 @@ static int pci9118_reset(struct comedi_device *dev)
 						/* disable interrupts source */
 	outl(0x30, dev->iobase + PCI9118_CNTCTRL);
 /* outl(0xb4, dev->iobase + PCI9118_CNTCTRL); */
-	start_pacer(dev, 0, 0, 0);		/* stop 8254 counters */
+	pci9118_start_pacer(dev, 0);		/* stop 8254 counters */
 	devpriv->AdControlReg = 0;
 	outl(devpriv->AdControlReg, dev->iobase + PCI9118_ADCNTRL);
 						/*
@@ -1888,7 +1787,6 @@ static int pci9118_reset(struct comedi_device *dev)
 						 * disable INT and DMA
 						 */
 
-	devpriv->cnt0_users = 0;
 	devpriv->exttrg_users = 0;
 
 	return 0;
@@ -1971,8 +1869,6 @@ static int pci9118_common_attach(struct comedi_device *dev, int disable_irq,
 			if (devpriv->dmabuf_virt[i]) {
 				devpriv->dmabuf_pages[i] = pages;
 				devpriv->dmabuf_size[i] = PAGE_SIZE * pages;
-				devpriv->dmabuf_samples[i] =
-				    devpriv->dmabuf_size[i] >> 1;
 				devpriv->dmabuf_hw[i] =
 				    virt_to_bus((void *)
 						devpriv->dmabuf_virt[i]);
@@ -2015,7 +1911,7 @@ static int pci9118_common_attach(struct comedi_device *dev, int disable_irq,
 				/* Enable parity check for parity error */
 
 	if (!disable_irq && pcidev->irq) {
-		ret = request_irq(pcidev->irq, interrupt_pci9118, IRQF_SHARED,
+		ret = request_irq(pcidev->irq, pci9118_interrupt, IRQF_SHARED,
 				  dev->board_name, dev);
 		if (ret == 0)
 			dev->irq = pcidev->irq;
@@ -2074,22 +1970,11 @@ static int pci9118_common_attach(struct comedi_device *dev, int disable_irq,
 	s->range_table = &range_digital;
 	s->insn_bits = pci9118_insn_bits_do;
 
-	devpriv->valid = 1;
-	devpriv->i8254_osc_base = I8254_OSC_BASE_4MHZ;
 	devpriv->ai_maskharderr = 0x10a;
 					/* default measure crash condition */
 	if (hw_err_mask)		/* disable some requested */
 		devpriv->ai_maskharderr &= ~hw_err_mask;
 
-	switch (this_board->ai_maxdata) {
-	case 0xffff:
-		devpriv->ai16bits = 1;
-		break;
-	default:
-		devpriv->ai16bits = 0;
-		break;
-	}
-
 	return 0;
 }
 
@@ -2152,7 +2037,7 @@ static void pci9118_detach(struct comedi_device *dev)
 	struct pci9118_private *devpriv = dev->private;
 
 	if (devpriv) {
-		if (devpriv->valid)
+		if (dev->iobase)
 			pci9118_reset(dev);
 		if (dev->irq)
 			free_irq(dev->irq, dev);
diff --git a/drivers/staging/comedi/drivers/adv_pci1710.c b/drivers/staging/comedi/drivers/adv_pci1710.c
index 28ec4854831..602b7a1e40e 100644
--- a/drivers/staging/comedi/drivers/adv_pci1710.c
+++ b/drivers/staging/comedi/drivers/adv_pci1710.c
@@ -51,10 +51,6 @@ Configuration options:
 #include "8253.h"
 #include "amcc_s5933.h"
 
-#define PCI171x_PARANOIDCHECK	/* if defined, then is used code which control
-				 * correct channel number on every 12 bit
-				 * sample */
-
 /* hardware types of the cards */
 #define TYPE_PCI171X	0
 #define TYPE_PCI1713	2
@@ -73,6 +69,9 @@ Configuration options:
 #define PCI171x_DAREF	14	/* W:   D/A reference control */
 #define PCI171x_DI	16	/* R:   digi inputs */
 #define PCI171x_DO	16	/* R:   digi inputs */
+
+#define PCI171X_TIMER_BASE	0x18
+
 #define PCI171x_CNT0	24	/* R/W: 8254 counter 0 */
 #define PCI171x_CNT1	26	/* R/W: 8254 counter 1 */
 #define PCI171x_CNT2	28	/* R/W: 8254 counter 2 */
@@ -298,29 +297,18 @@ static const struct boardtype boardtypes[] = {
 };
 
 struct pci1710_private {
-	char neverending_ai;	/*  we do unlimited AI */
 	unsigned int CntrlReg;	/*  Control register */
-	unsigned int i8254_osc_base;	/*  frequence of onboard oscilator */
-	unsigned int ai_do;	/*  what do AI? 0=nothing, 1 to 4 mode */
 	unsigned int ai_act_scan;	/*  how many scans we finished */
-	unsigned int ai_act_chan;	/*  actual position in actual scan */
-	unsigned int ai_buf_ptr;	/*  data buffer ptr in samples */
-	unsigned char ai_eos;	/*  1=EOS wake up */
 	unsigned char ai_et;
 	unsigned int ai_et_CntrlReg;
 	unsigned int ai_et_MuxVal;
-	unsigned int ai_et_div1, ai_et_div2;
+	unsigned int next_divisor1;
+	unsigned int next_divisor2;
+	unsigned int divisor1;
+	unsigned int divisor2;
 	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 saved_seglen;	/* len of the non-repeating chanlist */
 	unsigned char da_ranges;	/*  copy of D/A outpit range register */
-	unsigned int ai_scans;	/*  len of scanlist */
-	unsigned int ai_n_chan;	/*  how many channels is measured */
-	unsigned int *ai_chanlist;	/*  actaul chanlist */
-	unsigned int ai_flags;	/*  flaglist */
-	unsigned int ai_data_len;	/*  len of data buffer */
-	unsigned int ai_timer1;	/*  timers */
-	unsigned int ai_timer2;
 	unsigned short ao_data[4];	/*  data output buffer */
 	unsigned int cnt0_write_wait;	/* after a write, wait for update of the
 					 * internal state */
@@ -334,62 +322,90 @@ static const unsigned int muxonechan[] = {
 	0x1818, 0x1919, 0x1a1a, 0x1b1b, 0x1c1c, 0x1d1d, 0x1e1e, 0x1f1f
 };
 
-/*
-==============================================================================
- Check if channel list from user is built correctly
- If it's ok, then program scan/gain logic.
- This works for all cards.
-*/
-static int check_channel_list(struct comedi_device *dev,
+static int pci171x_ai_dropout(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
-			      unsigned int *chanlist, unsigned int n_chan)
+			      unsigned int chan,
+			      unsigned int val)
+{
+	const struct boardtype *board = comedi_board(dev);
+	struct pci1710_private *devpriv = dev->private;
+
+	if (board->cardtype != TYPE_PCI1713) {
+		if ((val & 0xf000) != devpriv->act_chanlist[chan]) {
+			dev_err(dev->class_dev,
+				"A/D data droput: received from channel %d, expected %d\n",
+				(val >> 12) & 0xf,
+				(devpriv->act_chanlist[chan] >> 12) & 0xf);
+			return -ENODATA;
+		}
+	}
+	return 0;
+}
+
+static int pci171x_ai_check_chanlist(struct comedi_device *dev,
+				     struct comedi_subdevice *s,
+				     struct comedi_cmd *cmd)
 {
+	struct pci1710_private *devpriv = dev->private;
+	unsigned int chan0 = CR_CHAN(cmd->chanlist[0]);
+	unsigned int last_aref = CR_AREF(cmd->chanlist[0]);
+	unsigned int next_chan = (chan0 + 1) % s->n_chan;
 	unsigned int chansegment[32];
-	unsigned int i, nowmustbechan, seglen, segpos;
+	unsigned int seglen;
+	int i;
 
-	/* correct channel and range number check itself comedi/range.c */
-	if (n_chan < 1) {
-		comedi_error(dev, "range/channel list is empty!");
+	if (cmd->chanlist_len == 1) {
+		devpriv->saved_seglen = cmd->chanlist_len;
 		return 0;
 	}
 
-	if (n_chan == 1)
-		return 1; /* seglen=1 */
+	/* first channel is always ok */
+	chansegment[0] = cmd->chanlist[0];
 
-	chansegment[0] = chanlist[0]; /*  first channel is every time ok */
-	for (i = 1, seglen = 1; i < n_chan; i++, seglen++) {
-		if (chanlist[0] == chanlist[i])
+	for (i = 1; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+		unsigned int aref = CR_AREF(cmd->chanlist[i]);
+
+		if (cmd->chanlist[0] == cmd->chanlist[i])
 			break;	/*  we detected a loop, stop */
-		if ((CR_CHAN(chanlist[i]) & 1) &&
-		    (CR_AREF(chanlist[i]) == AREF_DIFF)) {
-			comedi_error(dev, "Odd channel cannot be differential input!\n");
-			return 0;
+
+		if (aref == AREF_DIFF && (chan & 1)) {
+			dev_err(dev->class_dev,
+				"Odd channel cannot be differential input!\n");
+			return -EINVAL;
 		}
-		nowmustbechan = (CR_CHAN(chansegment[i - 1]) + 1) % s->n_chan;
-		if (CR_AREF(chansegment[i - 1]) == AREF_DIFF)
-			nowmustbechan = (nowmustbechan + 1) % s->n_chan;
-		if (nowmustbechan != CR_CHAN(chanlist[i])) {
-			printk("channel list must be continuous! chanlist[%i]=%d but must be %d or %d!\n",
-			       i, CR_CHAN(chanlist[i]), nowmustbechan,
-			       CR_CHAN(chanlist[0]));
-			return 0;
+
+		if (last_aref == AREF_DIFF)
+			next_chan = (next_chan + 1) % s->n_chan;
+		if (chan != next_chan) {
+			dev_err(dev->class_dev,
+				"channel list must be continuous! chanlist[%i]=%d but must be %d or %d!\n",
+				i, chan, next_chan, chan0);
+			return -EINVAL;
 		}
-		chansegment[i] = chanlist[i]; /* next correct channel in list */
-	}
 
-	for (i = 0, segpos = 0; i < n_chan; i++) {
-		if (chanlist[i] != chansegment[i % seglen]) {
-			printk("bad channel, reference or range number! chanlist[%i]=%d,%d,%d and not %d,%d,%d!\n",
-			       i, CR_CHAN(chansegment[i]),
-			       CR_RANGE(chansegment[i]),
-			       CR_AREF(chansegment[i]),
-			       CR_CHAN(chanlist[i % seglen]),
-			       CR_RANGE(chanlist[i % seglen]),
-			       CR_AREF(chansegment[i % seglen]));
-			return 0;
+		/* next correct channel in list */
+		chansegment[i] = cmd->chanlist[i];
+		last_aref = aref;
+	}
+	seglen = i;
+
+	for (i = 0; i < cmd->chanlist_len; i++) {
+		if (cmd->chanlist[i] != chansegment[i % seglen]) {
+			dev_err(dev->class_dev,
+				"bad channel, reference or range number! chanlist[%i]=%d,%d,%d and not %d,%d,%d!\n",
+				i, CR_CHAN(chansegment[i]),
+				CR_RANGE(chansegment[i]),
+				CR_AREF(chansegment[i]),
+				CR_CHAN(cmd->chanlist[i % seglen]),
+				CR_RANGE(cmd->chanlist[i % seglen]),
+				CR_AREF(chansegment[i % seglen]));
+			return -EINVAL;
 		}
 	}
-	return seglen;
+	devpriv->saved_seglen = seglen;
+
+	return 0;
 }
 
 static void setup_channel_list(struct comedi_device *dev,
@@ -401,9 +417,6 @@ static void setup_channel_list(struct comedi_device *dev,
 	struct pci1710_private *devpriv = dev->private;
 	unsigned int i, range, chanprog;
 
-	devpriv->act_chanlist_len = seglen;
-	devpriv->act_chanlist_pos = 0;
-
 	for (i = 0; i < seglen; i++) {	/*  store range list to card */
 		chanprog = muxonechan[CR_CHAN(chanlist[i])];
 		outw(chanprog, dev->iobase + PCI171x_MUX); /* select channel */
@@ -411,17 +424,13 @@ static void setup_channel_list(struct comedi_device *dev,
 		if (CR_AREF(chanlist[i]) == AREF_DIFF)
 			range |= 0x0020;
 		outw(range, dev->iobase + PCI171x_RANGE); /* select gain */
-#ifdef PCI171x_PARANOIDCHECK
 		devpriv->act_chanlist[i] =
 			(CR_CHAN(chanlist[i]) << 12) & 0xf000;
-#endif
 	}
-#ifdef PCI171x_PARANOIDCHECK
 	for ( ; i < n_chan; i++) { /* store remainder of channel list */
 		devpriv->act_chanlist[i] =
 			(CR_CHAN(chanlist[i]) << 12) & 0xf000;
 	}
-#endif
 
 	devpriv->ai_et_MuxVal =
 		CR_CHAN(chanlist[0]) | (CR_CHAN(chanlist[seglen - 1]) << 8);
@@ -447,12 +456,9 @@ static int pci171x_insn_read_ai(struct comedi_device *dev,
 				struct comedi_insn *insn, unsigned int *data)
 {
 	struct pci1710_private *devpriv = dev->private;
-	int ret;
-	int n;
-#ifdef PCI171x_PARANOIDCHECK
-	const struct boardtype *this_board = comedi_board(dev);
-	unsigned int idata;
-#endif
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	int ret = 0;
+	int i;
 
 	devpriv->CntrlReg &= Control_CNT0;
 	devpriv->CntrlReg |= Control_SW;	/*  set software trigger */
@@ -462,34 +468,27 @@ static int pci171x_insn_read_ai(struct comedi_device *dev,
 
 	setup_channel_list(dev, s, &insn->chanspec, 1, 1);
 
-	for (n = 0; n < insn->n; n++) {
+	for (i = 0; i < insn->n; i++) {
+		unsigned int val;
+
 		outw(0, dev->iobase + PCI171x_SOFTTRG);	/* start conversion */
 
 		ret = comedi_timeout(dev, s, insn, pci171x_ai_eoc, 0);
-		if (ret) {
-			outb(0, dev->iobase + PCI171x_CLRFIFO);
-			outb(0, dev->iobase + PCI171x_CLRINT);
-			return ret;
-		}
+		if (ret)
+			break;
 
-#ifdef PCI171x_PARANOIDCHECK
-		idata = inw(dev->iobase + PCI171x_AD_DATA);
-		if (this_board->cardtype != TYPE_PCI1713)
-			if ((idata & 0xf000) != devpriv->act_chanlist[0]) {
-				comedi_error(dev, "A/D insn data droput!");
-				return -ETIME;
-			}
-		data[n] = idata & 0x0fff;
-#else
-		data[n] = inw(dev->iobase + PCI171x_AD_DATA) & 0x0fff;
-#endif
+		val = inw(dev->iobase + PCI171x_AD_DATA);
+		ret = pci171x_ai_dropout(dev, s, chan, val);
+		if (ret)
+			break;
 
+		data[i] = val & s->maxdata;
 	}
 
 	outb(0, dev->iobase + PCI171x_CLRFIFO);
 	outb(0, dev->iobase + PCI171x_CLRINT);
 
-	return n;
+	return ret ? ret : insn->n;
 }
 
 /*
@@ -571,20 +570,18 @@ static int pci171x_insn_bits_do(struct comedi_device *dev,
 	return insn->n;
 }
 
-/*
-==============================================================================
-*/
-static void start_pacer(struct comedi_device *dev, int mode,
-			unsigned int divisor1, unsigned int divisor2)
+static void pci171x_start_pacer(struct comedi_device *dev,
+				bool load_counters)
 {
-	outw(0xb4, dev->iobase + PCI171x_CNTCTRL);
-	outw(0x74, dev->iobase + PCI171x_CNTCTRL);
-
-	if (mode == 1) {
-		outw(divisor2 & 0xff, dev->iobase + PCI171x_CNT2);
-		outw((divisor2 >> 8) & 0xff, dev->iobase + PCI171x_CNT2);
-		outw(divisor1 & 0xff, dev->iobase + PCI171x_CNT1);
-		outw((divisor1 >> 8) & 0xff, dev->iobase + PCI171x_CNT1);
+	struct pci1710_private *devpriv = dev->private;
+	unsigned long timer_base = dev->iobase + PCI171X_TIMER_BASE;
+
+	i8254_set_mode(timer_base, 1, 2, I8254_MODE2 | I8254_BINARY);
+	i8254_set_mode(timer_base, 1, 1, I8254_MODE2 | I8254_BINARY);
+
+	if (load_counters) {
+		i8254_write(timer_base, 1, 2, devpriv->divisor2);
+		i8254_write(timer_base, 1, 1, devpriv->divisor1);
 	}
 }
 
@@ -727,45 +724,37 @@ static int pci171x_ai_cancel(struct comedi_device *dev,
 		devpriv->CntrlReg |= Control_SW;
 
 		outw(devpriv->CntrlReg, dev->iobase + PCI171x_CONTROL);	/*  reset any operations */
-		start_pacer(dev, -1, 0, 0);
+		pci171x_start_pacer(dev, false);
 		outb(0, dev->iobase + PCI171x_CLRFIFO);
 		outb(0, dev->iobase + PCI171x_CLRINT);
 		break;
 	}
 
-	devpriv->ai_do = 0;
 	devpriv->ai_act_scan = 0;
 	s->async->cur_chan = 0;
-	devpriv->ai_buf_ptr = 0;
-	devpriv->neverending_ai = 0;
 
 	return 0;
 }
 
-/*
-==============================================================================
-*/
-static void interrupt_pci1710_every_sample(void *d)
+static void pci1710_handle_every_sample(struct comedi_device *dev,
+					struct comedi_subdevice *s)
 {
-	struct comedi_device *dev = d;
 	struct pci1710_private *devpriv = dev->private;
-	struct comedi_subdevice *s = dev->read_subdev;
-	int m;
-#ifdef PCI171x_PARANOIDCHECK
-	const struct boardtype *this_board = comedi_board(dev);
-	unsigned short sampl;
-#endif
+	struct comedi_cmd *cmd = &s->async->cmd;
+	unsigned int status;
+	unsigned int val;
+	int ret;
 
-	m = inw(dev->iobase + PCI171x_STATUS);
-	if (m & Status_FE) {
-		dev_dbg(dev->class_dev, "A/D FIFO empty (%4x)\n", m);
+	status = inw(dev->iobase + PCI171x_STATUS);
+	if (status & Status_FE) {
+		dev_dbg(dev->class_dev, "A/D FIFO empty (%4x)\n", status);
 		s->async->events |= COMEDI_CB_EOA | COMEDI_CB_ERROR;
 		cfc_handle_events(dev, s);
 		return;
 	}
-	if (m & Status_FF) {
+	if (status & Status_FF) {
 		dev_dbg(dev->class_dev,
-			"A/D FIFO Full status (Fatal Error!) (%4x)\n", m);
+			"A/D FIFO Full status (Fatal Error!) (%4x)\n", status);
 		s->async->events |= COMEDI_CB_EOA | COMEDI_CB_ERROR;
 		cfc_handle_events(dev, s);
 		return;
@@ -774,42 +763,27 @@ static void interrupt_pci1710_every_sample(void *d)
 	outb(0, dev->iobase + PCI171x_CLRINT);	/*  clear our INT request */
 
 	for (; !(inw(dev->iobase + PCI171x_STATUS) & Status_FE);) {
-#ifdef PCI171x_PARANOIDCHECK
-		sampl = inw(dev->iobase + PCI171x_AD_DATA);
-		if (this_board->cardtype != TYPE_PCI1713)
-			if ((sampl & 0xf000) !=
-			    devpriv->act_chanlist[s->async->cur_chan]) {
-				printk
-				    ("comedi: A/D data dropout: received data from channel %d, expected %d!\n",
-				     (sampl & 0xf000) >> 12,
-				     (devpriv->
-				      act_chanlist[s->
-						   async->cur_chan] & 0xf000) >>
-				     12);
-				s->async->events |=
-				    COMEDI_CB_EOA | COMEDI_CB_ERROR;
-				cfc_handle_events(dev, s);
-				return;
-			}
-		comedi_buf_put(s->async, sampl & 0x0fff);
-#else
-		comedi_buf_put(s->async,
-			       inw(dev->iobase + PCI171x_AD_DATA) & 0x0fff);
-#endif
-		++s->async->cur_chan;
+		val = inw(dev->iobase + PCI171x_AD_DATA);
+		ret = pci171x_ai_dropout(dev, s, s->async->cur_chan, val);
+		if (ret) {
+			s->async->events |= COMEDI_CB_EOA | COMEDI_CB_ERROR;
+			break;
+		}
 
-		if (s->async->cur_chan >= devpriv->ai_n_chan)
+		comedi_buf_put(s, val & s->maxdata);
+
+		s->async->cur_chan++;
+		if (s->async->cur_chan >= cmd->chanlist_len)
 			s->async->cur_chan = 0;
 
 
 		if (s->async->cur_chan == 0) {	/*  one scan done */
 			devpriv->ai_act_scan++;
-			if ((!devpriv->neverending_ai) &&
-			    (devpriv->ai_act_scan >= devpriv->ai_scans)) {
+			if (cmd->stop_src == TRIG_COUNT &&
+			    devpriv->ai_act_scan >= cmd->stop_arg) {
 				/*  all data sampled */
 				s->async->events |= COMEDI_CB_EOA;
-				cfc_handle_events(dev, s);
-				return;
+				break;
 			}
 		}
 	}
@@ -826,53 +800,37 @@ static int move_block_from_fifo(struct comedi_device *dev,
 				struct comedi_subdevice *s, int n, int turn)
 {
 	struct pci1710_private *devpriv = dev->private;
-	int i, j;
-#ifdef PCI171x_PARANOIDCHECK
-	const struct boardtype *this_board = comedi_board(dev);
-	unsigned short sampl;
-#endif
+	struct comedi_cmd *cmd = &s->async->cmd;
+	unsigned int val;
+	int ret;
+	int i;
 
-	j = s->async->cur_chan;
 	for (i = 0; i < n; i++) {
-#ifdef PCI171x_PARANOIDCHECK
-		sampl = inw(dev->iobase + PCI171x_AD_DATA);
-		if (this_board->cardtype != TYPE_PCI1713)
-			if ((sampl & 0xf000) != devpriv->act_chanlist[j]) {
-				dev_dbg(dev->class_dev,
-					"A/D FIFO data dropout: received data from channel %d, expected %d! (%d/%d/%d/%d/%d/%4x)\n",
-					(sampl & 0xf000) >> 12,
-					(devpriv->act_chanlist[j] & 0xf000) >> 12,
-					i, j, devpriv->ai_act_scan, n, turn,
-					sampl);
-				s->async->events |=
-				    COMEDI_CB_EOA | COMEDI_CB_ERROR;
-				cfc_handle_events(dev, s);
-				return 1;
-			}
-		comedi_buf_put(s->async, sampl & 0x0fff);
-#else
-		comedi_buf_put(s->async,
-			       inw(dev->iobase + PCI171x_AD_DATA) & 0x0fff);
-#endif
-		j++;
-		if (j >= devpriv->ai_n_chan) {
-			j = 0;
+		val = inw(dev->iobase + PCI171x_AD_DATA);
+
+		ret = pci171x_ai_dropout(dev, s, s->async->cur_chan, val);
+		if (ret) {
+			s->async->events |= COMEDI_CB_EOA | COMEDI_CB_ERROR;
+			return ret;
+		}
+
+		comedi_buf_put(s, val & s->maxdata);
+
+		s->async->cur_chan++;
+		if (s->async->cur_chan >= cmd->chanlist_len) {
+			s->async->cur_chan = 0;
 			devpriv->ai_act_scan++;
 		}
 	}
-	s->async->cur_chan = j;
 	return 0;
 }
 
-/*
-==============================================================================
-*/
-static void interrupt_pci1710_half_fifo(void *d)
+static void pci1710_handle_fifo(struct comedi_device *dev,
+				struct comedi_subdevice *s)
 {
-	struct comedi_device *dev = d;
 	const struct boardtype *this_board = comedi_board(dev);
 	struct pci1710_private *devpriv = dev->private;
-	struct comedi_subdevice *s = dev->read_subdev;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	int m, samplesinbuf;
 
 	m = inw(dev->iobase + PCI171x_STATUS);
@@ -891,8 +849,8 @@ static void interrupt_pci1710_half_fifo(void *d)
 	}
 
 	samplesinbuf = this_board->fifo_half_size;
-	if (samplesinbuf * sizeof(short) >= devpriv->ai_data_len) {
-		m = devpriv->ai_data_len / sizeof(short);
+	if (samplesinbuf * sizeof(short) >= s->async->prealloc_bufsz) {
+		m = s->async->prealloc_bufsz / sizeof(short);
 		if (move_block_from_fifo(dev, s, m, 0))
 			return;
 		samplesinbuf -= m;
@@ -903,13 +861,13 @@ static void interrupt_pci1710_half_fifo(void *d)
 			return;
 	}
 
-	if (!devpriv->neverending_ai)
-		if (devpriv->ai_act_scan >= devpriv->ai_scans) { /* all data
-								    sampled */
-			s->async->events |= COMEDI_CB_EOA;
-			cfc_handle_events(dev, s);
-			return;
-		}
+	if (cmd->stop_src == TRIG_COUNT &&
+	    devpriv->ai_act_scan >= cmd->stop_arg) {
+		/* all data sampled */
+		s->async->events |= COMEDI_CB_EOA;
+		cfc_handle_events(dev, s);
+		return;
+	}
 	outb(0, dev->iobase + PCI171x_CLRINT);	/*  clear our INT request */
 
 	cfc_handle_events(dev, s);
@@ -922,9 +880,15 @@ static irqreturn_t interrupt_service_pci1710(int irq, void *d)
 {
 	struct comedi_device *dev = d;
 	struct pci1710_private *devpriv = dev->private;
+	struct comedi_subdevice *s;
+	struct comedi_cmd *cmd;
 
 	if (!dev->attached)	/*  is device attached? */
 		return IRQ_NONE;	/*  no, exit */
+
+	s = dev->read_subdev;
+	cmd = &s->async->cmd;
+
 	/*  is this interrupt from our board? */
 	if (!(inw(dev->iobase + PCI171x_STATUS) & Status_IRQ))
 		return IRQ_NONE;	/*  no, exit */
@@ -939,95 +903,59 @@ static irqreturn_t interrupt_service_pci1710(int irq, void *d)
 		outb(0, dev->iobase + PCI171x_CLRINT);
 		outw(devpriv->ai_et_MuxVal, dev->iobase + PCI171x_MUX);
 		outw(devpriv->CntrlReg, dev->iobase + PCI171x_CONTROL);
-		/*  start pacer */
-		start_pacer(dev, 1, devpriv->ai_et_div1, devpriv->ai_et_div2);
+		pci171x_start_pacer(dev, true);
 		return IRQ_HANDLED;
 	}
-	if (devpriv->ai_eos) {	/*  We use FIFO half full INT or not? */
-		interrupt_pci1710_every_sample(d);
-	} else {
-		interrupt_pci1710_half_fifo(d);
-	}
+
+	if (cmd->flags & TRIG_WAKE_EOS)
+		pci1710_handle_every_sample(dev, s);
+	else
+		pci1710_handle_fifo(dev, s);
+
 	return IRQ_HANDLED;
 }
 
-/*
-==============================================================================
-*/
-static int pci171x_ai_docmd_and_mode(int mode, struct comedi_device *dev,
-				     struct comedi_subdevice *s)
+static int pci171x_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
-	const struct boardtype *this_board = comedi_board(dev);
 	struct pci1710_private *devpriv = dev->private;
-	unsigned int divisor1 = 0, divisor2 = 0;
-	unsigned int seglen;
+	struct comedi_cmd *cmd = &s->async->cmd;
 
-	start_pacer(dev, -1, 0, 0);	/*  stop pacer */
+	pci171x_start_pacer(dev, false);
 
-	seglen = check_channel_list(dev, s, devpriv->ai_chanlist,
-				    devpriv->ai_n_chan);
-	if (seglen < 1)
-		return -EINVAL;
-	setup_channel_list(dev, s, devpriv->ai_chanlist,
-			   devpriv->ai_n_chan, seglen);
+	setup_channel_list(dev, s, cmd->chanlist, cmd->chanlist_len,
+			   devpriv->saved_seglen);
 
 	outb(0, dev->iobase + PCI171x_CLRFIFO);
 	outb(0, dev->iobase + PCI171x_CLRINT);
 
-	devpriv->ai_do = mode;
-
 	devpriv->ai_act_scan = 0;
 	s->async->cur_chan = 0;
-	devpriv->ai_buf_ptr = 0;
-	devpriv->neverending_ai = 0;
 
 	devpriv->CntrlReg &= Control_CNT0;
-	/*  don't we want wake up every scan?  devpriv->ai_eos=1; */
-	if ((devpriv->ai_flags & TRIG_WAKE_EOS)) {
-		devpriv->ai_eos = 1;
-	} else {
+	if ((cmd->flags & TRIG_WAKE_EOS) == 0)
 		devpriv->CntrlReg |= Control_ONEFH;
-		devpriv->ai_eos = 0;
-	}
 
-	if ((devpriv->ai_scans == 0) || (devpriv->ai_scans == -1))
-		devpriv->neverending_ai = 1;
-	/* well, user want neverending */
-	else
-		devpriv->neverending_ai = 0;
+	devpriv->divisor1 = devpriv->next_divisor1;
+	devpriv->divisor2 = devpriv->next_divisor2;
 
-	switch (mode) {
-	case 1:
-	case 2:
-		if (devpriv->ai_timer1 < this_board->ai_ns_min)
-			devpriv->ai_timer1 = this_board->ai_ns_min;
+	if (cmd->convert_src == TRIG_TIMER) {
 		devpriv->CntrlReg |= Control_PACER | Control_IRQEN;
-		if (mode == 2) {
+		if (cmd->start_src == TRIG_EXT) {
 			devpriv->ai_et_CntrlReg = devpriv->CntrlReg;
 			devpriv->CntrlReg &=
 			    ~(Control_PACER | Control_ONEFH | Control_GATE);
 			devpriv->CntrlReg |= Control_EXT;
 			devpriv->ai_et = 1;
-		} else {
+		} else {	/* TRIG_NOW */
 			devpriv->ai_et = 0;
 		}
-		i8253_cascade_ns_to_timer(devpriv->i8254_osc_base,
-					  &divisor1, &divisor2,
-					  &devpriv->ai_timer1,
-					  devpriv->ai_flags);
 		outw(devpriv->CntrlReg, dev->iobase + PCI171x_CONTROL);
-		if (mode != 2) {
-			/*  start pacer */
-			start_pacer(dev, mode, divisor1, divisor2);
-		} else {
-			devpriv->ai_et_div1 = divisor1;
-			devpriv->ai_et_div2 = divisor2;
-		}
-		break;
-	case 3:
+
+		if (cmd->start_src == TRIG_NOW)
+			pci171x_start_pacer(dev, true);
+	} else {	/* TRIG_EXT */
 		devpriv->CntrlReg |= Control_EXT | Control_IRQEN;
 		outw(devpriv->CntrlReg, dev->iobase + PCI171x_CONTROL);
-		break;
 	}
 
 	return 0;
@@ -1043,8 +971,7 @@ static int pci171x_ai_cmdtest(struct comedi_device *dev,
 	const struct boardtype *this_board = comedi_board(dev);
 	struct pci1710_private *devpriv = dev->private;
 	int err = 0;
-	int tmp;
-	unsigned int divisor1 = 0, divisor2 = 0;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -1092,64 +1019,25 @@ static int pci171x_ai_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		tmp = cmd->convert_arg;
-		i8253_cascade_ns_to_timer(devpriv->i8254_osc_base,
-					  &divisor1, &divisor2,
-					  &cmd->convert_arg, cmd->flags);
-		if (cmd->convert_arg < this_board->ai_ns_min)
-			cmd->convert_arg = this_board->ai_ns_min;
-		if (tmp != cmd->convert_arg)
-			err++;
+		arg = cmd->convert_arg;
+		i8253_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
+					  &devpriv->next_divisor1,
+					  &devpriv->next_divisor2,
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 
 	if (err)
 		return 4;
 
-	/* step 5: complain about special chanlist considerations */
+	/* Step 5: check channel list */
 
-	if (cmd->chanlist) {
-		if (!check_channel_list(dev, s, cmd->chanlist,
-					cmd->chanlist_len))
-			return 5;	/*  incorrect channels list */
-	}
+	err |= pci171x_ai_check_chanlist(dev, s, cmd);
 
-	return 0;
-}
-
-/*
-==============================================================================
-*/
-static int pci171x_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
-{
-	struct pci1710_private *devpriv = dev->private;
-	struct comedi_cmd *cmd = &s->async->cmd;
-
-	devpriv->ai_n_chan = cmd->chanlist_len;
-	devpriv->ai_chanlist = cmd->chanlist;
-	devpriv->ai_flags = cmd->flags;
-	devpriv->ai_data_len = s->async->prealloc_bufsz;
-	devpriv->ai_timer1 = 0;
-	devpriv->ai_timer2 = 0;
-
-	if (cmd->stop_src == TRIG_COUNT)
-		devpriv->ai_scans = cmd->stop_arg;
-	else
-		devpriv->ai_scans = 0;
-
-
-	if (cmd->scan_begin_src == TRIG_FOLLOW) {	/*  mode 1, 2, 3 */
-		if (cmd->convert_src == TRIG_TIMER) {	/*  mode 1 and 2 */
-			devpriv->ai_timer1 = cmd->convert_arg;
-			return pci171x_ai_docmd_and_mode(cmd->start_src ==
-							 TRIG_EXT ? 2 : 1, dev,
-							 s);
-		}
-		if (cmd->convert_src == TRIG_EXT) {	/*  mode 3 */
-			return pci171x_ai_docmd_and_mode(3, dev, s);
-		}
-	}
+	if (err)
+		return 5;
 
-	return -1;
+	return 0;
 }
 
 /*
@@ -1165,7 +1053,7 @@ static int pci171x_reset(struct comedi_device *dev)
 	outw(devpriv->CntrlReg, dev->iobase + PCI171x_CONTROL);	/*  reset any operations */
 	outb(0, dev->iobase + PCI171x_CLRFIFO);	/*  clear FIFO */
 	outb(0, dev->iobase + PCI171x_CLRINT);	/*  clear INT request */
-	start_pacer(dev, -1, 0, 0);	/*  stop 8254 */
+	pci171x_start_pacer(dev, false);
 	devpriv->da_ranges = 0;
 	if (this_board->n_aochan) {
 		outb(devpriv->da_ranges, dev->iobase + PCI171x_DAREF);	/*  set DACs to 0..5V */
@@ -1290,7 +1178,6 @@ static int pci1710_auto_attach(struct comedi_device *dev,
 			s->do_cmd = pci171x_ai_cmd;
 			s->cancel = pci171x_ai_cancel;
 		}
-		devpriv->i8254_osc_base = I8254_OSC_BASE_10MHZ;
 		subdev++;
 	}
 
diff --git a/drivers/staging/comedi/drivers/amplc_dio200_common.c b/drivers/staging/comedi/drivers/amplc_dio200_common.c
index 818a0d7e3d5..3edaa4028da 100644
--- a/drivers/staging/comedi/drivers/amplc_dio200_common.c
+++ b/drivers/staging/comedi/drivers/amplc_dio200_common.c
@@ -130,7 +130,6 @@ struct dio200_subdev_intr {
 	unsigned int enabled_isns;
 	unsigned int stopcount;
 	bool active:1;
-	bool continuous:1;
 };
 
 static inline const struct dio200_layout *
@@ -259,7 +258,7 @@ static int dio200_start_intr(struct comedi_device *dev,
 	struct comedi_cmd *cmd = &s->async->cmd;
 	int retval = 0;
 
-	if (!subpriv->continuous && subpriv->stopcount == 0) {
+	if (cmd->stop_src == TRIG_COUNT && subpriv->stopcount == 0) {
 		/* An empty acquisition! */
 		s->async->events |= COMEDI_CB_EOA;
 		subpriv->active = false;
@@ -281,22 +280,18 @@ static int dio200_start_intr(struct comedi_device *dev,
 	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)
+static int dio200_inttrig_start_intr(struct comedi_device *dev,
+				     struct comedi_subdevice *s,
+				     unsigned int trig_num)
 {
-	struct dio200_subdev_intr *subpriv;
+	struct dio200_subdev_intr *subpriv = s->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned long flags;
 	int event = 0;
 
-	if (trignum != 0)
+	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
-	subpriv = s->private;
-
 	spin_lock_irqsave(&subpriv->spinlock, flags);
 	s->async->inttrig = NULL;
 	if (subpriv->active)
@@ -315,18 +310,18 @@ static void dio200_read_scan_intr(struct comedi_device *dev,
 				  unsigned int triggered)
 {
 	struct dio200_subdev_intr *subpriv = s->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned short val;
-	unsigned int n, ch, len;
+	unsigned int n, ch;
 
 	val = 0;
-	len = s->async->cmd.chanlist_len;
-	for (n = 0; n < len; n++) {
-		ch = CR_CHAN(s->async->cmd.chanlist[n]);
+	for (n = 0; n < cmd->chanlist_len; n++) {
+		ch = CR_CHAN(cmd->chanlist[n]);
 		if (triggered & (1U << ch))
 			val |= (1U << n);
 	}
 	/* Write the scan to the buffer. */
-	if (comedi_buf_put(s->async, val)) {
+	if (comedi_buf_put(s, val)) {
 		s->async->events |= (COMEDI_CB_BLOCK | COMEDI_CB_EOS);
 	} else {
 		/* Error!  Stop acquisition.  */
@@ -336,8 +331,7 @@ static void dio200_read_scan_intr(struct comedi_device *dev,
 	}
 
 	/* Check for end of acquisition. */
-	if (!subpriv->continuous) {
-		/* stop_src == TRIG_COUNT */
+	if (cmd->stop_src == TRIG_COUNT) {
 		if (subpriv->stopcount > 0) {
 			subpriv->stopcount--;
 			if (subpriv->stopcount == 0) {
@@ -516,28 +510,16 @@ static int dio200_subdev_intr_cmd(struct comedi_device *dev,
 	subpriv->active = true;
 
 	/* Set up end of acquisition. */
-	switch (cmd->stop_src) {
-	case TRIG_COUNT:
-		subpriv->continuous = false;
+	if (cmd->stop_src == TRIG_COUNT)
 		subpriv->stopcount = cmd->stop_arg;
-		break;
-	default:
-		/* TRIG_NONE */
-		subpriv->continuous = true;
+	else	/* TRIG_NONE */
 		subpriv->stopcount = 0;
-		break;
-	}
 
-	/* Set up start of acquisition. */
-	switch (cmd->start_src) {
-	case TRIG_INT:
+	if (cmd->start_src == TRIG_INT)
 		s->async->inttrig = dio200_inttrig_start_intr;
-		break;
-	default:
-		/* TRIG_NOW */
+	else	/* TRIG_NOW */
 		event = dio200_start_intr(dev, s);
-		break;
-	}
+
 	spin_unlock_irqrestore(&subpriv->spinlock, flags);
 
 	if (event)
@@ -829,7 +811,7 @@ dio200_subdev_8254_config(struct comedi_device *dev, struct comedi_subdevice *s,
 	spin_lock_irqsave(&subpriv->spinlock, flags);
 	switch (data[0]) {
 	case INSN_CONFIG_SET_COUNTER_MODE:
-		if (data[1] > (I8254_MODE5 | I8254_BINARY))
+		if (data[1] > (I8254_MODE5 | I8254_BCD))
 			ret = -EINVAL;
 		else
 			dio200_subdev_8254_set_mode(dev, s, chan, data[1]);
diff --git a/drivers/staging/comedi/drivers/amplc_pc236.c b/drivers/staging/comedi/drivers/amplc_pc236.c
index b21d7b48f1d..c9a96ad0055 100644
--- a/drivers/staging/comedi/drivers/amplc_pc236.c
+++ b/drivers/staging/comedi/drivers/amplc_pc236.c
@@ -314,7 +314,7 @@ static int pc236_intr_cmdtest(struct comedi_device *dev,
 	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, 1);
+	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 	err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
@@ -361,7 +361,7 @@ static irqreturn_t pc236_interrupt(int irq, void *d)
 
 	handled = pc236_intr_check(dev);
 	if (dev->attached && handled) {
-		comedi_buf_put(s->async, 0);
+		comedi_buf_put(s, 0);
 		s->async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOS;
 		comedi_event(dev, s);
 	}
@@ -402,6 +402,7 @@ static int pc236_common_attach(struct comedi_device *dev, unsigned long iobase,
 			s->maxdata = 1;
 			s->range_table = &range_digital;
 			s->insn_bits = pc236_intr_insn;
+			s->len_chanlist	= 1;
 			s->do_cmdtest = pc236_intr_cmdtest;
 			s->do_cmd = pc236_intr_cmd;
 			s->cancel = pc236_intr_cancel;
diff --git a/drivers/staging/comedi/drivers/amplc_pci224.c b/drivers/staging/comedi/drivers/amplc_pci224.c
index 29e01e28003..339c47c1eb9 100644
--- a/drivers/staging/comedi/drivers/amplc_pci224.c
+++ b/drivers/staging/comedi/drivers/amplc_pci224.c
@@ -377,7 +377,6 @@ struct pci224_private {
 	unsigned int cached_div1;
 	unsigned int cached_div2;
 	unsigned int ao_stop_count;
-	short ao_stop_continuous;
 	unsigned short ao_enab;	/* max 16 channels so 'short' will do */
 	unsigned char intsce;
 };
@@ -467,16 +466,6 @@ pci224_ao_insn_read(struct comedi_device *dev, struct comedi_subdevice *s,
 }
 
 /*
- * Just a wrapper for the inline function 'i8253_cascade_ns_to_timer'.
- */
-static void
-pci224_cascade_ns_to_timer(int osc_base, unsigned int *d1, unsigned int *d2,
-			   unsigned int *nanosec, int round_mode)
-{
-	i8253_cascade_ns_to_timer(osc_base, d1, d2, nanosec, round_mode);
-}
-
-/*
  * Kills a command running on the AO subdevice.
  */
 static void pci224_ao_stop(struct comedi_device *dev,
@@ -531,7 +520,7 @@ static void pci224_ao_start(struct comedi_device *dev,
 	unsigned long flags;
 
 	set_bit(AO_CMD_STARTED, &devpriv->state);
-	if (!devpriv->ao_stop_continuous && devpriv->ao_stop_count == 0) {
+	if (cmd->stop_src == TRIG_COUNT && devpriv->ao_stop_count == 0) {
 		/* An empty acquisition! */
 		s->async->events |= COMEDI_CB_EOA;
 		cfc_handle_events(dev, s);
@@ -556,21 +545,15 @@ static void pci224_ao_handle_fifo(struct comedi_device *dev,
 {
 	struct pci224_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
+	unsigned int bytes_per_scan = cfc_bytes_per_scan(s);
 	unsigned int num_scans;
 	unsigned int room;
 	unsigned short dacstat;
 	unsigned int i, n;
-	unsigned int bytes_per_scan;
 
-	if (cmd->chanlist_len) {
-		bytes_per_scan = cmd->chanlist_len * sizeof(short);
-	} else {
-		/* Shouldn't get here! */
-		bytes_per_scan = sizeof(short);
-	}
 	/* Determine number of scans available in buffer. */
-	num_scans = comedi_buf_read_n_available(s->async) / bytes_per_scan;
-	if (!devpriv->ao_stop_continuous) {
+	num_scans = comedi_buf_read_n_available(s) / bytes_per_scan;
+	if (cmd->stop_src == TRIG_COUNT) {
 		/* Fixed number of scans. */
 		if (num_scans > devpriv->ao_stop_count)
 			num_scans = devpriv->ao_stop_count;
@@ -582,7 +565,7 @@ static void pci224_ao_handle_fifo(struct comedi_device *dev,
 	switch (dacstat & PCI224_DACCON_FIFOFL_MASK) {
 	case PCI224_DACCON_FIFOFL_EMPTY:
 		room = PCI224_FIFO_ROOM_EMPTY;
-		if (!devpriv->ao_stop_continuous && devpriv->ao_stop_count == 0) {
+		if (cmd->stop_src == TRIG_COUNT && devpriv->ao_stop_count == 0) {
 			/* FIFO empty at end of counted acquisition. */
 			s->async->events |= COMEDI_CB_EOA;
 			cfc_handle_events(dev, s);
@@ -608,8 +591,7 @@ static void pci224_ao_handle_fifo(struct comedi_device *dev,
 		}
 	}
 	/* Determine how many new scans can be put in the FIFO. */
-	if (cmd->chanlist_len)
-		room /= cmd->chanlist_len;
+	room /= cmd->chanlist_len;
 
 	/* Determine how many scans to process. */
 	if (num_scans > room)
@@ -624,7 +606,7 @@ static void pci224_ao_handle_fifo(struct comedi_device *dev,
 			     dev->iobase + PCI224_DACDATA);
 		}
 	}
-	if (!devpriv->ao_stop_continuous) {
+	if (cmd->stop_src == TRIG_COUNT) {
 		devpriv->ao_stop_count -= num_scans;
 		if (devpriv->ao_stop_count == 0) {
 			/*
@@ -671,14 +653,13 @@ static void pci224_ao_handle_fifo(struct comedi_device *dev,
 	cfc_handle_events(dev, s);
 }
 
-/*
- * Internal trigger function to start acquisition on AO subdevice.
- */
-static int
-pci224_ao_inttrig_start(struct comedi_device *dev, struct comedi_subdevice *s,
-			unsigned int trignum)
+static int pci224_ao_inttrig_start(struct comedi_device *dev,
+				   struct comedi_subdevice *s,
+				   unsigned int trig_num)
 {
-	if (trignum != 0)
+	struct comedi_cmd *cmd = &s->async->cmd;
+
+	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
 	s->async->inttrig = NULL;
@@ -687,6 +668,37 @@ pci224_ao_inttrig_start(struct comedi_device *dev, struct comedi_subdevice *s,
 	return 1;
 }
 
+static int pci224_ao_check_chanlist(struct comedi_device *dev,
+				    struct comedi_subdevice *s,
+				    struct comedi_cmd *cmd)
+{
+	unsigned int range0 = CR_RANGE(cmd->chanlist[0]);
+	unsigned int chan_mask = 0;
+	int i;
+
+	for (i = 0; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+		unsigned int range = CR_RANGE(cmd->chanlist[i]);
+
+		if (chan_mask & (1 << chan)) {
+			dev_dbg(dev->class_dev,
+				"%s: entries in chanlist must contain no duplicate channels\n",
+				__func__);
+			return -EINVAL;
+		}
+		chan_mask |= (1 << chan);
+
+		if (range != range0) {
+			dev_dbg(dev->class_dev,
+				"%s: entries in chanlist must all have the same range index\n",
+				__func__);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
 #define MAX_SCAN_PERIOD		0xFFFFFFFFU
 #define MIN_SCAN_PERIOD		2500
 #define CONVERT_PERIOD		625
@@ -700,7 +712,7 @@ pci224_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 {
 	struct pci224_private *devpriv = dev->private;
 	int err = 0;
-	unsigned int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -727,14 +739,14 @@ pci224_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	 * There's only one external trigger signal (which makes these
 	 * tests easier).  Only one thing can use it.
 	 */
-	tmp = 0;
+	arg = 0;
 	if (cmd->start_src & TRIG_EXT)
-		tmp++;
+		arg++;
 	if (cmd->scan_begin_src & TRIG_EXT)
-		tmp++;
+		arg++;
 	if (cmd->stop_src & TRIG_EXT)
-		tmp++;
-	if (tmp > 1)
+		arg++;
+	if (arg > 1)
 		err |= -EINVAL;
 
 	if (err)
@@ -767,10 +779,10 @@ pci224_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 		err |= cfc_check_trigger_arg_max(&cmd->scan_begin_arg,
 						 MAX_SCAN_PERIOD);
 
-		tmp = cmd->chanlist_len * CONVERT_PERIOD;
-		if (tmp < MIN_SCAN_PERIOD)
-			tmp = MIN_SCAN_PERIOD;
-		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg, tmp);
+		arg = cmd->chanlist_len * CONVERT_PERIOD;
+		if (arg < MIN_SCAN_PERIOD)
+			arg = MIN_SCAN_PERIOD;
+		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg, arg);
 		break;
 	case TRIG_EXT:
 		/* Force to external trigger 0. */
@@ -821,98 +833,21 @@ pci224_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	/* Step 4: fix up any arguments. */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		unsigned int div1, div2, round;
-		int round_mode = cmd->flags & TRIG_ROUND_MASK;
-
-		tmp = cmd->scan_begin_arg;
-		/* Check whether to use a single timer. */
-		switch (round_mode) {
-		case TRIG_ROUND_NEAREST:
-		default:
-			round = I8254_OSC_BASE_10MHZ / 2;
-			break;
-		case TRIG_ROUND_DOWN:
-			round = 0;
-			break;
-		case TRIG_ROUND_UP:
-			round = I8254_OSC_BASE_10MHZ - 1;
-			break;
-		}
-		/* Be careful to avoid overflow! */
-		div2 = cmd->scan_begin_arg / I8254_OSC_BASE_10MHZ;
-		div2 += (round + cmd->scan_begin_arg % I8254_OSC_BASE_10MHZ) /
-			I8254_OSC_BASE_10MHZ;
-		if (div2 <= 0x10000) {
-			/* A single timer will suffice. */
-			if (div2 < 2)
-				div2 = 2;
-			cmd->scan_begin_arg = div2 * I8254_OSC_BASE_10MHZ;
-			if (cmd->scan_begin_arg < div2 ||
-			    cmd->scan_begin_arg < I8254_OSC_BASE_10MHZ) {
-				/* Overflow! */
-				cmd->scan_begin_arg = MAX_SCAN_PERIOD;
-			}
-		} else {
-			/* Use two timers. */
-			div1 = devpriv->cached_div1;
-			div2 = devpriv->cached_div2;
-			pci224_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
-						   &div1, &div2,
-						   &cmd->scan_begin_arg,
-						   round_mode);
-			devpriv->cached_div1 = div1;
-			devpriv->cached_div2 = div2;
-		}
-		if (tmp != cmd->scan_begin_arg)
-			err++;
-
+		arg = cmd->scan_begin_arg;
+		/* Use two timers. */
+		i8253_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
+					  &devpriv->cached_div1,
+					  &devpriv->cached_div2,
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 
 	if (err)
 		return 4;
 
-	/* Step 5: check channel list. */
-
-	if (cmd->chanlist && (cmd->chanlist_len > 0)) {
-		unsigned int range;
-		enum { range_err = 1, dupchan_err = 2, };
-		unsigned errors;
-		unsigned int n;
-		unsigned int ch;
-
-		/*
-		 * Check all channels have the same range index.  Don't care
-		 * about analogue reference, as we can't configure it.
-		 *
-		 * Check the list has no duplicate channels.
-		 */
-		range = CR_RANGE(cmd->chanlist[0]);
-		errors = 0;
-		tmp = 0;
-		for (n = 0; n < cmd->chanlist_len; n++) {
-			ch = CR_CHAN(cmd->chanlist[n]);
-			if (tmp & (1U << ch))
-				errors |= dupchan_err;
-
-			tmp |= (1U << ch);
-			if (CR_RANGE(cmd->chanlist[n]) != range)
-				errors |= range_err;
-
-		}
-		if (errors) {
-			if (errors & dupchan_err) {
-				dev_dbg(dev->class_dev,
-					"%s: entries in chanlist must contain no duplicate channels\n",
-					__func__);
-			}
-			if (errors & range_err) {
-				dev_dbg(dev->class_dev,
-					"%s: entries in chanlist must all have the same range index\n",
-					__func__);
-			}
-			err++;
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= pci224_ao_check_chanlist(dev, s, cmd);
 
 	if (err)
 		return 5;
@@ -920,9 +855,33 @@ pci224_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	return 0;
 }
 
-/*
- * 'do_cmd' function for AO subdevice.
- */
+static void pci224_ao_start_pacer(struct comedi_device *dev,
+				  struct comedi_subdevice *s)
+{
+	struct pci224_private *devpriv = dev->private;
+	unsigned long timer_base = devpriv->iobase1 + PCI224_Z2_CT0;
+
+	/*
+	 * The output of timer Z2-0 will be used as the scan trigger
+	 * source.
+	 */
+	/* Make sure Z2-0 is gated on.  */
+	outb(GAT_CONFIG(0, GAT_VCC), devpriv->iobase1 + PCI224_ZGAT_SCE);
+	/* Cascading with Z2-2. */
+	/* Make sure Z2-2 is gated on.  */
+	outb(GAT_CONFIG(2, GAT_VCC), devpriv->iobase1 + PCI224_ZGAT_SCE);
+	/* Z2-2 needs 10 MHz clock. */
+	outb(CLK_CONFIG(2, CLK_10MHZ), devpriv->iobase1 + PCI224_ZCLK_SCE);
+	/* Load Z2-2 mode (2) and counter (div1). */
+	i8254_set_mode(timer_base, 0, 2, I8254_MODE2 | I8254_BINARY);
+	i8254_write(timer_base, 0, 2, devpriv->cached_div1);
+	/* Z2-0 is clocked from Z2-2's output. */
+	outb(CLK_CONFIG(0, CLK_OUTNM1), devpriv->iobase1 + PCI224_ZCLK_SCE);
+	/* Load Z2-0 mode (2) and counter (div2). */
+	i8254_set_mode(timer_base, 0, 0, I8254_MODE2 | I8254_BINARY);
+	i8254_write(timer_base, 0, 0, devpriv->cached_div2);
+}
+
 static int pci224_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
 	struct pci224_private *devpriv = dev->private;
@@ -978,106 +937,26 @@ static int pci224_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	outw(devpriv->daccon | PCI224_DACCON_FIFORESET,
 	     dev->iobase + PCI224_DACCON);
 
-	if (cmd->scan_begin_src == TRIG_TIMER) {
-		unsigned int div1, div2, round;
-		unsigned int ns = cmd->scan_begin_arg;
-		int round_mode = cmd->flags & TRIG_ROUND_MASK;
-
-		/* Check whether to use a single timer. */
-		switch (round_mode) {
-		case TRIG_ROUND_NEAREST:
-		default:
-			round = I8254_OSC_BASE_10MHZ / 2;
-			break;
-		case TRIG_ROUND_DOWN:
-			round = 0;
-			break;
-		case TRIG_ROUND_UP:
-			round = I8254_OSC_BASE_10MHZ - 1;
-			break;
-		}
-		/* Be careful to avoid overflow! */
-		div2 = cmd->scan_begin_arg / I8254_OSC_BASE_10MHZ;
-		div2 += (round + cmd->scan_begin_arg % I8254_OSC_BASE_10MHZ) /
-			I8254_OSC_BASE_10MHZ;
-		if (div2 <= 0x10000) {
-			/* A single timer will suffice. */
-			if (div2 < 2)
-				div2 = 2;
-			div2 &= 0xffff;
-			div1 = 1;	/* Flag that single timer to be used. */
-		} else {
-			/* Use two timers. */
-			div1 = devpriv->cached_div1;
-			div2 = devpriv->cached_div2;
-			pci224_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
-						   &div1, &div2,
-						   &ns, round_mode);
-		}
-
-		/*
-		 * The output of timer Z2-0 will be used as the scan trigger
-		 * source.
-		 */
-		/* Make sure Z2-0 is gated on.  */
-		outb(GAT_CONFIG(0, GAT_VCC),
-		     devpriv->iobase1 + PCI224_ZGAT_SCE);
-		if (div1 == 1) {
-			/* Not cascading.  Z2-0 needs 10 MHz clock. */
-			outb(CLK_CONFIG(0, CLK_10MHZ),
-			     devpriv->iobase1 + PCI224_ZCLK_SCE);
-		} else {
-			/* Cascading with Z2-2. */
-			/* Make sure Z2-2 is gated on.  */
-			outb(GAT_CONFIG(2, GAT_VCC),
-			     devpriv->iobase1 + PCI224_ZGAT_SCE);
-			/* Z2-2 needs 10 MHz clock. */
-			outb(CLK_CONFIG(2, CLK_10MHZ),
-			     devpriv->iobase1 + PCI224_ZCLK_SCE);
-			/* Load Z2-2 mode (2) and counter (div1). */
-			i8254_load(devpriv->iobase1 + PCI224_Z2_CT0, 0,
-				   2, div1, 2);
-			/* Z2-0 is clocked from Z2-2's output. */
-			outb(CLK_CONFIG(0, CLK_OUTNM1),
-			     devpriv->iobase1 + PCI224_ZCLK_SCE);
-		}
-		/* Load Z2-0 mode (2) and counter (div2). */
-		i8254_load(devpriv->iobase1 + PCI224_Z2_CT0, 0, 0, div2, 2);
-	}
+	if (cmd->scan_begin_src == TRIG_TIMER)
+		pci224_ao_start_pacer(dev, s);
 
 	/*
 	 * Sort out end of acquisition.
 	 */
-	switch (cmd->stop_src) {
-	case TRIG_COUNT:
-		/* Fixed number of scans.  */
-		devpriv->ao_stop_continuous = 0;
+	if (cmd->stop_src == TRIG_COUNT)
 		devpriv->ao_stop_count = cmd->stop_arg;
-		break;
-	default:
-		/* Continuous scans. */
-		devpriv->ao_stop_continuous = 1;
+	else	/* TRIG_EXT | TRIG_NONE */
 		devpriv->ao_stop_count = 0;
-		break;
-	}
 
-	/*
-	 * Sort out start of acquisition.
-	 */
-	switch (cmd->start_src) {
-	case TRIG_INT:
-		spin_lock_irqsave(&devpriv->ao_spinlock, flags);
-		s->async->inttrig = &pci224_ao_inttrig_start;
-		spin_unlock_irqrestore(&devpriv->ao_spinlock, flags);
-		break;
-	case TRIG_EXT:
+	spin_lock_irqsave(&devpriv->ao_spinlock, flags);
+	if (cmd->start_src == TRIG_INT) {
+		s->async->inttrig = pci224_ao_inttrig_start;
+	} else {	/* TRIG_EXT */
 		/* Enable external interrupt trigger to start acquisition. */
-		spin_lock_irqsave(&devpriv->ao_spinlock, flags);
 		devpriv->intsce |= PCI224_INTR_EXT;
 		outb(devpriv->intsce, devpriv->iobase1 + PCI224_INT_SCE);
-		spin_unlock_irqrestore(&devpriv->ao_spinlock, flags);
-		break;
 	}
+	spin_unlock_irqrestore(&devpriv->ao_spinlock, flags);
 
 	return 0;
 }
@@ -1101,7 +980,7 @@ pci224_ao_munge(struct comedi_device *dev, struct comedi_subdevice *s,
 {
 	const struct pci224_board *thisboard = comedi_board(dev);
 	struct pci224_private *devpriv = dev->private;
-	struct comedi_async *async = s->async;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned short *array = data;
 	unsigned int length = num_bytes / sizeof(*array);
 	unsigned int offset;
@@ -1111,7 +990,7 @@ pci224_ao_munge(struct comedi_device *dev, struct comedi_subdevice *s,
 	/* The hardware expects 16-bit numbers. */
 	shift = 16 - thisboard->ao_bits;
 	/* Channels will be all bipolar or all unipolar. */
-	if ((devpriv->hwrange[CR_RANGE(async->cmd.chanlist[0])] &
+	if ((devpriv->hwrange[CR_RANGE(cmd->chanlist[0])] &
 	     PCI224_DACCON_POLAR_MASK) == PCI224_DACCON_POLAR_UNI) {
 		/* Unipolar */
 		offset = 0;
diff --git a/drivers/staging/comedi/drivers/amplc_pci230.c b/drivers/staging/comedi/drivers/amplc_pci230.c
index 99e60835dc4..3895bc7cb3e 100644
--- a/drivers/staging/comedi/drivers/amplc_pci230.c
+++ b/drivers/staging/comedi/drivers/amplc_pci230.c
@@ -519,14 +519,6 @@ struct pci230_private {
 					 * level threshold (PCI230+/260+). */
 	unsigned short adcg;	/* ADCG register value. */
 	unsigned char int_en;	/* Interrupt enables bits. */
-	unsigned char ai_continuous;	/* Flag set when cmd->stop_src ==
-					 * TRIG_NONE - user chooses to stop
-					 * continuous conversion by
-					 * cancelation. */
-	unsigned char ao_continuous;	/* Flag set when cmd->stop_src ==
-					 * TRIG_NONE - user chooses to stop
-					 * continuous conversion by
-					 * cancelation. */
 	unsigned char ai_bipolar;	/* Set if bipolar input range so we
 					 * know to mangle it. */
 	unsigned char ao_bipolar;	/* Set if bipolar output range so we
@@ -953,6 +945,38 @@ static int pci230_ao_rinsn(struct comedi_device *dev,
 	return i;
 }
 
+static int pci230_ao_check_chanlist(struct comedi_device *dev,
+				    struct comedi_subdevice *s,
+				    struct comedi_cmd *cmd)
+{
+	unsigned int prev_chan = CR_CHAN(cmd->chanlist[0]);
+	unsigned int range0 = CR_RANGE(cmd->chanlist[0]);
+	int i;
+
+	for (i = 1; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+		unsigned int range = CR_RANGE(cmd->chanlist[i]);
+
+		if (chan < prev_chan) {
+			dev_dbg(dev->class_dev,
+				"%s: channel numbers must increase\n",
+				__func__);
+			return -EINVAL;
+		}
+
+		if (range != range0) {
+			dev_dbg(dev->class_dev,
+				"%s: channels must have the same range\n",
+				__func__);
+			return -EINVAL;
+		}
+
+		prev_chan = chan;
+	}
+
+	return 0;
+}
+
 static int pci230_ao_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
@@ -1065,48 +1089,9 @@ static int pci230_ao_cmdtest(struct comedi_device *dev,
 	if (err)
 		return 4;
 
-	/* Step 5: check channel list if it exists. */
-
-	if (cmd->chanlist && cmd->chanlist_len > 0) {
-		enum {
-			seq_err = (1 << 0),
-			range_err = (1 << 1)
-		};
-		unsigned int errors;
-		unsigned int n;
-		unsigned int chan, prev_chan;
-		unsigned int range, first_range;
-
-		prev_chan = CR_CHAN(cmd->chanlist[0]);
-		first_range = CR_RANGE(cmd->chanlist[0]);
-		errors = 0;
-		for (n = 1; n < cmd->chanlist_len; n++) {
-			chan = CR_CHAN(cmd->chanlist[n]);
-			range = CR_RANGE(cmd->chanlist[n]);
-			/* Channel numbers must strictly increase. */
-			if (chan < prev_chan)
-				errors |= seq_err;
-
-			/* Ranges must be the same. */
-			if (range != first_range)
-				errors |= range_err;
-
-			prev_chan = chan;
-		}
-		if (errors != 0) {
-			err++;
-			if ((errors & seq_err) != 0) {
-				dev_dbg(dev->class_dev,
-					"%s: channel numbers must increase\n",
-					__func__);
-			}
-			if ((errors & range_err) != 0) {
-				dev_dbg(dev->class_dev,
-					"%s: channels must have the same range\n",
-					__func__);
-			}
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= pci230_ao_check_chanlist(dev, s, cmd);
 
 	if (err)
 		return 5;
@@ -1175,11 +1160,11 @@ static void pci230_handle_ao_nofifo(struct comedi_device *dev,
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
 
-	if (!devpriv->ao_continuous && (devpriv->ao_scan_count == 0))
+	if (cmd->stop_src == TRIG_COUNT && devpriv->ao_scan_count == 0)
 		return;
 	for (i = 0; i < cmd->chanlist_len; i++) {
 		/* Read sample from Comedi's circular buffer. */
-		ret = comedi_buf_get(s->async, &data);
+		ret = comedi_buf_get(s, &data);
 		if (ret == 0) {
 			s->async->events |= COMEDI_CB_OVERFLOW;
 			pci230_ao_stop(dev, s);
@@ -1190,7 +1175,7 @@ static void pci230_handle_ao_nofifo(struct comedi_device *dev,
 		pci230_ao_write_nofifo(dev, data, CR_CHAN(cmd->chanlist[i]));
 	}
 	async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOS;
-	if (!devpriv->ao_continuous) {
+	if (cmd->stop_src == TRIG_COUNT) {
 		devpriv->ao_scan_count--;
 		if (devpriv->ao_scan_count == 0) {
 			/* End of acquisition. */
@@ -1212,16 +1197,14 @@ static int pci230_handle_ao_fifo(struct comedi_device *dev,
 	unsigned int room;
 	unsigned short dacstat;
 	unsigned int i, n;
-	unsigned int bytes_per_scan;
 	unsigned int events = 0;
 	int running;
 
 	/* Get DAC FIFO status. */
 	dacstat = inw(dev->iobase + PCI230_DACCON);
 	/* Determine number of scans available in buffer. */
-	bytes_per_scan = cmd->chanlist_len * sizeof(short);
-	num_scans = comedi_buf_read_n_available(async) / bytes_per_scan;
-	if (!devpriv->ao_continuous) {
+	num_scans = comedi_buf_read_n_available(s) / cfc_bytes_per_scan(s);
+	if (cmd->stop_src == TRIG_COUNT) {
 		/* Fixed number of scans. */
 		if (num_scans > devpriv->ao_scan_count)
 			num_scans = devpriv->ao_scan_count;
@@ -1265,13 +1248,13 @@ static int pci230_handle_ao_fifo(struct comedi_device *dev,
 			for (i = 0; i < cmd->chanlist_len; i++) {
 				unsigned short datum;
 
-				comedi_buf_get(async, &datum);
+				comedi_buf_get(s, &datum);
 				pci230_ao_write_fifo(dev, datum,
 						     CR_CHAN(cmd->chanlist[i]));
 			}
 		}
 		events |= COMEDI_CB_EOS | COMEDI_CB_BLOCK;
-		if (!devpriv->ao_continuous) {
+		if (cmd->stop_src == TRIG_COUNT) {
 			devpriv->ao_scan_count -= num_scans;
 			if (devpriv->ao_scan_count == 0) {
 				/* All data for the command has been written
@@ -1349,7 +1332,7 @@ static void pci230_ao_start(struct comedi_device *dev,
 	unsigned long irqflags;
 
 	set_bit(AO_CMD_STARTED, &devpriv->state);
-	if (!devpriv->ao_continuous && (devpriv->ao_scan_count == 0)) {
+	if (cmd->stop_src == TRIG_COUNT && devpriv->ao_scan_count == 0) {
 		/* An empty acquisition! */
 		async->events |= COMEDI_CB_EOA;
 		pci230_ao_stop(dev, s);
@@ -1434,7 +1417,9 @@ static int pci230_ao_inttrig_start(struct comedi_device *dev,
 				   struct comedi_subdevice *s,
 				   unsigned int trig_num)
 {
-	if (trig_num != 0)
+	struct comedi_cmd *cmd = &s->async->cmd;
+
+	if (trig_num != cmd->start_src)
 		return -EINVAL;
 
 	s->async->inttrig = NULL;
@@ -1460,14 +1445,10 @@ static int pci230_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	}
 
 	/* Get number of scans required. */
-	if (cmd->stop_src == TRIG_COUNT) {
+	if (cmd->stop_src == TRIG_COUNT)
 		devpriv->ao_scan_count = cmd->stop_arg;
-		devpriv->ao_continuous = 0;
-	} else {
-		/* TRIG_NONE, user calls cancel. */
+	else	/* TRIG_NONE, user calls cancel */
 		devpriv->ao_scan_count = 0;
-		devpriv->ao_continuous = 1;
-	}
 
 	/* Set range - see analogue output range table; 0 => unipolar 10V,
 	 * 1 => bipolar +/-10V range scale */
@@ -1552,6 +1533,109 @@ static int pci230_ai_check_scan_period(struct comedi_cmd *cmd)
 	return !err;
 }
 
+static int pci230_ai_check_chanlist(struct comedi_device *dev,
+				    struct comedi_subdevice *s,
+				    struct comedi_cmd *cmd)
+{
+	struct pci230_private *devpriv = dev->private;
+	unsigned int max_diff_chan = (s->n_chan / 2) - 1;
+	unsigned int prev_chan = 0;
+	unsigned int prev_range = 0;
+	unsigned int prev_aref = 0;
+	unsigned int prev_polarity = 0;
+	unsigned int subseq_len = 0;
+	int i;
+
+	for (i = 0; i < cmd->chanlist_len; i++) {
+		unsigned int chanspec = cmd->chanlist[i];
+		unsigned int chan = CR_CHAN(chanspec);
+		unsigned int range = CR_RANGE(chanspec);
+		unsigned int aref = CR_AREF(chanspec);
+		unsigned int polarity = pci230_ai_bipolar[range];
+
+		if (aref == AREF_DIFF && chan >= max_diff_chan) {
+			dev_dbg(dev->class_dev,
+				"%s: differential channel number out of range 0 to %u\n",
+				__func__, max_diff_chan);
+			return -EINVAL;
+		}
+
+		if (i > 0) {
+			/*
+			 * Channel numbers must strictly increase or
+			 * subsequence must repeat exactly.
+			 */
+			if (chan <= prev_chan && subseq_len == 0)
+				subseq_len = i;
+
+			if (subseq_len > 0 &&
+			    cmd->chanlist[i % subseq_len] != chanspec) {
+					dev_dbg(dev->class_dev,
+						"%s: channel numbers must increase or sequence must repeat exactly\n",
+						__func__);
+					return -EINVAL;
+			}
+
+			if (aref != prev_aref) {
+				dev_dbg(dev->class_dev,
+					"%s: channel sequence analogue references must be all the same (single-ended or differential)\n",
+					__func__);
+				return -EINVAL;
+			}
+
+			if (polarity != prev_polarity) {
+				dev_dbg(dev->class_dev,
+					"%s: channel sequence ranges must be all bipolar or all unipolar\n",
+					__func__);
+				return -EINVAL;
+			}
+
+			if (aref != AREF_DIFF && range != prev_range &&
+			    ((chan ^ prev_chan) & ~1) == 0) {
+				dev_dbg(dev->class_dev,
+					"%s: single-ended channel pairs must have the same range\n",
+					__func__);
+				return -EINVAL;
+			}
+		}
+		prev_chan = chan;
+		prev_range = range;
+		prev_aref = aref;
+		prev_polarity = polarity;
+	}
+
+	if (subseq_len == 0)
+		subseq_len = cmd->chanlist_len;
+
+	if ((cmd->chanlist_len % subseq_len) != 0) {
+		dev_dbg(dev->class_dev,
+			"%s: sequence must repeat exactly\n", __func__);
+		return -EINVAL;
+	}
+
+	/*
+	 * Buggy PCI230+ or PCI260+ requires channel 0 to be (first) in the
+	 * sequence if the sequence contains more than one channel. Hardware
+	 * versions 1 and 2 have the bug. There is no hardware version 3.
+	 *
+	 * Actually, there are two firmwares that report themselves as
+	 * hardware version 1 (the boards have different ADC chips with
+	 * slightly different timing requirements, which was supposed to
+	 * be invisible to software). The first one doesn't seem to have
+	 * the bug, but the second one does, and we can't tell them apart!
+	 */
+	if (devpriv->hwver > 0 && devpriv->hwver < 4) {
+		if (subseq_len > 1 && CR_CHAN(cmd->chanlist[0]) != 0) {
+			dev_info(dev->class_dev,
+				 "amplc_pci230: ai_cmdtest: Buggy PCI230+/260+ h/w version %u requires first channel of multi-channel sequence to be 0 (corrected in h/w version 4)\n",
+				 devpriv->hwver);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
 static int pci230_ai_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
@@ -1740,136 +1824,9 @@ static int pci230_ai_cmdtest(struct comedi_device *dev,
 	if (err)
 		return 4;
 
-	/* Step 5: check channel list if it exists. */
-
-	if (cmd->chanlist && cmd->chanlist_len > 0) {
-		enum {
-			seq_err = 1 << 0,
-			rangepair_err = 1 << 1,
-			polarity_err = 1 << 2,
-			aref_err = 1 << 3,
-			diffchan_err = 1 << 4,
-			buggy_chan0_err = 1 << 5
-		};
-		unsigned int errors;
-		unsigned int chan, prev_chan;
-		unsigned int range, prev_range;
-		unsigned int polarity, prev_polarity;
-		unsigned int aref, prev_aref;
-		unsigned int subseq_len;
-		unsigned int n;
-
-		subseq_len = 0;
-		errors = 0;
-		prev_chan = prev_aref = prev_range = prev_polarity = 0;
-		for (n = 0; n < cmd->chanlist_len; n++) {
-			chan = CR_CHAN(cmd->chanlist[n]);
-			range = CR_RANGE(cmd->chanlist[n]);
-			aref = CR_AREF(cmd->chanlist[n]);
-			polarity = pci230_ai_bipolar[range];
-			/* Only the first half of the channels are available if
-			 * differential.  (These are remapped in software.  In
-			 * hardware, only the even channels are available.) */
-			if ((aref == AREF_DIFF)
-			    && (chan >= (s->n_chan / 2))) {
-				errors |= diffchan_err;
-			}
-			if (n > 0) {
-				/* Channel numbers must strictly increase or
-				 * subsequence must repeat exactly. */
-				if ((chan <= prev_chan)
-				    && (subseq_len == 0)) {
-					subseq_len = n;
-				}
-				if ((subseq_len > 0)
-				    && (cmd->chanlist[n] !=
-					cmd->chanlist[n % subseq_len])) {
-					errors |= seq_err;
-				}
-				/* Channels must have same AREF. */
-				if (aref != prev_aref)
-					errors |= aref_err;
-
-				/* Channel ranges must have same polarity. */
-				if (polarity != prev_polarity)
-					errors |= polarity_err;
-
-				/* Single-ended channel pairs must have same
-				 * range.  */
-				if ((aref != AREF_DIFF)
-				    && (((chan ^ prev_chan) & ~1) == 0)
-				    && (range != prev_range)) {
-					errors |= rangepair_err;
-				}
-			}
-			prev_chan = chan;
-			prev_range = range;
-			prev_aref = aref;
-			prev_polarity = polarity;
-		}
-		if (subseq_len == 0) {
-			/* Subsequence is whole sequence. */
-			subseq_len = n;
-		}
-		/* If channel list is a repeating subsequence, need a whole
-		 * number of repeats. */
-		if ((n % subseq_len) != 0)
-			errors |= seq_err;
-
-		if ((devpriv->hwver > 0) && (devpriv->hwver < 4)) {
-			/*
-			 * Buggy PCI230+ or PCI260+ requires channel 0 to be
-			 * (first) in the sequence if the sequence contains
-			 * more than one channel.  Hardware versions 1 and 2
-			 * have the bug.  There is no hardware version 3.
-			 *
-			 * Actually, there are two firmwares that report
-			 * themselves as hardware version 1 (the boards
-			 * have different ADC chips with slightly different
-			 * timing requirements, which was supposed to be
-			 * invisible to software).  The first one doesn't
-			 * seem to have the bug, but the second one
-			 * does, and we can't tell them apart!
-			 */
-			if ((subseq_len > 1)
-			    && (CR_CHAN(cmd->chanlist[0]) != 0)) {
-				errors |= buggy_chan0_err;
-			}
-		}
-		if (errors != 0) {
-			err++;
-			if ((errors & seq_err) != 0) {
-				dev_dbg(dev->class_dev,
-					"%s: channel numbers must increase or sequence must repeat exactly\n",
-					__func__);
-			}
-			if ((errors & rangepair_err) != 0) {
-				dev_dbg(dev->class_dev,
-					"%s: single-ended channel pairs must have the same range\n",
-					__func__);
-			}
-			if ((errors & polarity_err) != 0) {
-				dev_dbg(dev->class_dev,
-					"%s: channel sequence ranges must be all bipolar or all unipolar\n",
-					__func__);
-			}
-			if ((errors & aref_err) != 0) {
-				dev_dbg(dev->class_dev,
-					"%s: channel sequence analogue references must be all the same (single-ended or differential)\n",
-					__func__);
-			}
-			if ((errors & diffchan_err) != 0) {
-				dev_dbg(dev->class_dev,
-					"%s: differential channel number out of range 0 to %u\n",
-					__func__, (s->n_chan / 2) - 1);
-			}
-			if ((errors & buggy_chan0_err) != 0) {
-				dev_info(dev->class_dev,
-					 "amplc_pci230: ai_cmdtest: Buggy PCI230+/260+ h/w version %u requires first channel of multi-channel sequence to be 0 (corrected in h/w version 4)\n",
-					 devpriv->hwver);
-			}
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= pci230_ai_check_chanlist(dev, s, cmd);
 
 	if (err)
 		return 5;
@@ -1891,9 +1848,9 @@ static void pci230_ai_update_fifo_trigger_level(struct comedi_device *dev,
 		/* Wake at end of scan. */
 		wake = scanlen - devpriv->ai_scan_pos;
 	} else {
-		if (devpriv->ai_continuous
-		    || (devpriv->ai_scan_count >= PCI230_ADC_FIFOLEVEL_HALFFULL)
-		    || (scanlen >= PCI230_ADC_FIFOLEVEL_HALFFULL)) {
+		if (cmd->stop_src != TRIG_COUNT ||
+		    devpriv->ai_scan_count >= PCI230_ADC_FIFOLEVEL_HALFFULL ||
+		    scanlen >= PCI230_ADC_FIFOLEVEL_HALFFULL) {
 			wake = PCI230_ADC_FIFOLEVEL_HALFFULL;
 		} else {
 			wake = (devpriv->ai_scan_count * scanlen)
@@ -2044,7 +2001,7 @@ static void pci230_ai_start(struct comedi_device *dev,
 	struct comedi_cmd *cmd = &async->cmd;
 
 	set_bit(AI_CMD_STARTED, &devpriv->state);
-	if (!devpriv->ai_continuous && (devpriv->ai_scan_count == 0)) {
+	if (cmd->stop_src == TRIG_COUNT && devpriv->ai_scan_count == 0) {
 		/* An empty acquisition! */
 		async->events |= COMEDI_CB_EOA;
 		pci230_ai_stop(dev, s);
@@ -2177,7 +2134,9 @@ static int pci230_ai_inttrig_start(struct comedi_device *dev,
 				   struct comedi_subdevice *s,
 				   unsigned int trig_num)
 {
-	if (trig_num != 0)
+	struct comedi_cmd *cmd = &s->async->cmd;
+
+	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
 	s->async->inttrig = NULL;
@@ -2190,16 +2149,17 @@ static void pci230_handle_ai(struct comedi_device *dev,
 			     struct comedi_subdevice *s)
 {
 	struct pci230_private *devpriv = dev->private;
+	struct comedi_async *async = s->async;
+	struct comedi_cmd *cmd = &async->cmd;
+	unsigned int scanlen = cmd->scan_end_arg;
 	unsigned int events = 0;
 	unsigned int status_fifo;
 	unsigned int i;
 	unsigned int todo;
 	unsigned int fifoamount;
-	struct comedi_async *async = s->async;
-	unsigned int scanlen = async->cmd.scan_end_arg;
 
 	/* Determine number of samples to read. */
-	if (devpriv->ai_continuous) {
+	if (cmd->stop_src != TRIG_COUNT) {
 		todo = PCI230_ADC_FIFOLEVEL_HALFFULL;
 	} else if (devpriv->ai_scan_count == 0) {
 		todo = 0;
@@ -2247,7 +2207,7 @@ static void pci230_handle_ai(struct comedi_device *dev,
 			}
 		}
 		/* Read sample and store in Comedi's circular buffer. */
-		if (comedi_buf_put(async, pci230_ai_read(dev)) == 0) {
+		if (comedi_buf_put(s, pci230_ai_read(dev)) == 0) {
 			events |= COMEDI_CB_ERROR | COMEDI_CB_OVERFLOW;
 			comedi_error(dev, "AI buffer overflow");
 			break;
@@ -2261,7 +2221,7 @@ static void pci230_handle_ai(struct comedi_device *dev,
 			async->events |= COMEDI_CB_EOS;
 		}
 	}
-	if (!devpriv->ai_continuous && (devpriv->ai_scan_count == 0)) {
+	if (cmd->stop_src == TRIG_COUNT && devpriv->ai_scan_count == 0) {
 		/* End of acquisition. */
 		events |= COMEDI_CB_EOA;
 	} else {
@@ -2312,14 +2272,10 @@ static int pci230_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 
 
 	/* Get number of scans required. */
-	if (cmd->stop_src == TRIG_COUNT) {
+	if (cmd->stop_src == TRIG_COUNT)
 		devpriv->ai_scan_count = cmd->stop_arg;
-		devpriv->ai_continuous = 0;
-	} else {
-		/* TRIG_NONE, user calls cancel. */
+	else	/* TRIG_NONE, user calls cancel */
 		devpriv->ai_scan_count = 0;
-		devpriv->ai_continuous = 1;
-	}
 	devpriv->ai_scan_pos = 0;	/* Position within scan. */
 
 	/* Steps;
@@ -2463,12 +2419,10 @@ static int pci230_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		}
 	}
 
-	if (cmd->start_src == TRIG_INT) {
+	if (cmd->start_src == TRIG_INT)
 		s->async->inttrig = pci230_ai_inttrig_start;
-	} else {
-		/* TRIG_NOW */
+	else	/* TRIG_NOW */
 		pci230_ai_start(dev, s);
-	}
 
 	return 0;
 }
diff --git a/drivers/staging/comedi/drivers/cb_das16_cs.c b/drivers/staging/comedi/drivers/cb_das16_cs.c
index 645fcb0cf17..eb1b92d72e8 100644
--- a/drivers/staging/comedi/drivers/cb_das16_cs.c
+++ b/drivers/staging/comedi/drivers/cb_das16_cs.c
@@ -184,6 +184,7 @@ static int das16cs_ao_winsn(struct comedi_device *dev,
 
 		for (bit = 15; bit >= 0; bit--) {
 			int b = (d >> bit) & 0x1;
+
 			b <<= 1;
 			outw(status1 | b | 0x0000, dev->iobase + DAS16CS_MISC1);
 			udelay(1);
diff --git a/drivers/staging/comedi/drivers/cb_pcidas.c b/drivers/staging/comedi/drivers/cb_pcidas.c
index 83a265f3408..7377da1aff7 100644
--- a/drivers/staging/comedi/drivers/cb_pcidas.c
+++ b/drivers/staging/comedi/drivers/cb_pcidas.c
@@ -803,6 +803,33 @@ static int trimpot_read_insn(struct comedi_device *dev,
 	return 1;
 }
 
+static int cb_pcidas_ai_check_chanlist(struct comedi_device *dev,
+				       struct comedi_subdevice *s,
+				       struct comedi_cmd *cmd)
+{
+	unsigned int chan0 = CR_CHAN(cmd->chanlist[0]);
+	unsigned int range0 = CR_RANGE(cmd->chanlist[0]);
+	int i;
+
+	for (i = 1; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+		unsigned int range = CR_RANGE(cmd->chanlist[i]);
+
+		if (chan != (chan0 + i) % s->n_chan) {
+			dev_dbg(dev->class_dev,
+				"entries in chanlist must be consecutive channels, counting upwards\n");
+			return -EINVAL;
+		}
+
+		if (range != range0) {
+			dev_dbg(dev->class_dev,
+				"entries in chanlist must all have the same gain\n");
+			return -EINVAL;
+		}
+	}
+	return 0;
+}
+
 static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_cmd *cmd)
@@ -810,8 +837,7 @@ static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 	const struct cb_pcidas_board *thisboard = comedi_board(dev);
 	struct cb_pcidas_private *devpriv = dev->private;
 	int err = 0;
-	int tmp;
-	int i, gain, start_chan;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -846,9 +872,12 @@ static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 	if (err)
 		return 2;
 
-	/* step 3: arguments are trivially compatible */
+	/* Step 3: check if arguments are trivially valid */
 
 	switch (cmd->start_src) {
+	case TRIG_NOW:
+		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+		break;
 	case TRIG_EXT:
 		/* External trigger, only CR_EDGE and CR_INVERT flags allowed */
 		if ((cmd->start_arg
@@ -862,9 +891,6 @@ static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 			err |= -EINVAL;
 		}
 		break;
-	default:
-		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-		break;
 	}
 
 	if (cmd->scan_begin_src == TRIG_TIMER)
@@ -886,45 +912,28 @@ static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		tmp = cmd->scan_begin_arg;
+		arg = cmd->scan_begin_arg;
 		i8253_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
 					  &devpriv->divisor1,
 					  &devpriv->divisor2,
-					  &cmd->scan_begin_arg, cmd->flags);
-		if (tmp != cmd->scan_begin_arg)
-			err++;
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 	if (cmd->convert_src == TRIG_TIMER) {
-		tmp = cmd->convert_arg;
+		arg = cmd->convert_arg;
 		i8253_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
 					  &devpriv->divisor1,
 					  &devpriv->divisor2,
-					  &cmd->convert_arg, cmd->flags);
-		if (tmp != cmd->convert_arg)
-			err++;
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 
 	if (err)
 		return 4;
 
-	/*  check channel/gain list against card's limitations */
-	if (cmd->chanlist) {
-		gain = CR_RANGE(cmd->chanlist[0]);
-		start_chan = CR_CHAN(cmd->chanlist[0]);
-		for (i = 1; i < cmd->chanlist_len; i++) {
-			if (CR_CHAN(cmd->chanlist[i]) !=
-			    (start_chan + i) % s->n_chan) {
-				comedi_error(dev,
-					     "entries in chanlist must be consecutive channels, counting upwards\n");
-				err++;
-			}
-			if (CR_RANGE(cmd->chanlist[i]) != gain) {
-				comedi_error(dev,
-					     "entries in chanlist must all have the same gain\n");
-				err++;
-			}
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= cb_pcidas_ai_check_chanlist(dev, s, cmd);
 
 	if (err)
 		return 5;
@@ -932,20 +941,16 @@ static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 	return 0;
 }
 
-static void cb_pcidas_load_counters(struct comedi_device *dev, unsigned int *ns,
-				    int rounding_flags)
+static void cb_pcidas_ai_load_counters(struct comedi_device *dev)
 {
 	struct cb_pcidas_private *devpriv = dev->private;
+	unsigned long timer_base = devpriv->pacer_counter_dio + ADC8254;
 
-	i8253_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
-				  &devpriv->divisor1, &devpriv->divisor2,
-				  ns, rounding_flags);
+	i8254_set_mode(timer_base, 0, 1, I8254_MODE2 | I8254_BINARY);
+	i8254_set_mode(timer_base, 0, 2, I8254_MODE2 | I8254_BINARY);
 
-	/* Write the values of ctr1 and ctr2 into counters 1 and 2 */
-	i8254_load(devpriv->pacer_counter_dio + ADC8254, 0, 1,
-		   devpriv->divisor1, 2);
-	i8254_load(devpriv->pacer_counter_dio + ADC8254, 0, 2,
-		   devpriv->divisor2, 2);
+	i8254_write(timer_base, 0, 1, devpriv->divisor1);
+	i8254_write(timer_base, 0, 2, devpriv->divisor2);
 }
 
 static int cb_pcidas_ai_cmd(struct comedi_device *dev,
@@ -983,12 +988,8 @@ static int cb_pcidas_ai_cmd(struct comedi_device *dev,
 	outw(bits, devpriv->control_status + ADCMUX_CONT);
 
 	/*  load counters */
-	if (cmd->convert_src == TRIG_TIMER)
-		cb_pcidas_load_counters(dev, &cmd->convert_arg,
-					cmd->flags & TRIG_ROUND_MASK);
-	else if (cmd->scan_begin_src == TRIG_TIMER)
-		cb_pcidas_load_counters(dev, &cmd->scan_begin_arg,
-					cmd->flags & TRIG_ROUND_MASK);
+	if (cmd->scan_begin_src == TRIG_TIMER || cmd->convert_src == TRIG_TIMER)
+		cb_pcidas_ai_load_counters(dev);
 
 	/*  set number of conversions */
 	if (cmd->stop_src == TRIG_COUNT)
@@ -1035,6 +1036,25 @@ static int cb_pcidas_ai_cmd(struct comedi_device *dev,
 	return 0;
 }
 
+static int cb_pcidas_ao_check_chanlist(struct comedi_device *dev,
+				       struct comedi_subdevice *s,
+				       struct comedi_cmd *cmd)
+{
+	unsigned int chan0 = CR_CHAN(cmd->chanlist[0]);
+
+	if (cmd->chanlist_len > 1) {
+		unsigned int chan1 = CR_CHAN(cmd->chanlist[1]);
+
+		if (chan0 != 0 || chan1 != 1) {
+			dev_dbg(dev->class_dev,
+				"channels must be ordered channel 0, channel 1 in chanlist\n");
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
 static int cb_pcidas_ao_cmdtest(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_cmd *cmd)
@@ -1042,7 +1062,7 @@ static int cb_pcidas_ao_cmdtest(struct comedi_device *dev,
 	const struct cb_pcidas_board *thisboard = comedi_board(dev);
 	struct cb_pcidas_private *devpriv = dev->private;
 	int err = 0;
-	int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -1085,27 +1105,20 @@ static int cb_pcidas_ao_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		tmp = cmd->scan_begin_arg;
+		arg = cmd->scan_begin_arg;
 		i8253_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
 					  &devpriv->ao_divisor1,
 					  &devpriv->ao_divisor2,
-					  &cmd->scan_begin_arg, cmd->flags);
-		if (tmp != cmd->scan_begin_arg)
-			err++;
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 
 	if (err)
 		return 4;
 
-	/*  check channel/gain list against card's limitations */
-	if (cmd->chanlist && cmd->chanlist_len > 1) {
-		if (CR_CHAN(cmd->chanlist[0]) != 0 ||
-		    CR_CHAN(cmd->chanlist[1]) != 1) {
-			comedi_error(dev,
-				     "channels must be ordered channel 0, channel 1 in chanlist\n");
-			err++;
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= cb_pcidas_ao_check_chanlist(dev, s, cmd);
 
 	if (err)
 		return 5;
@@ -1145,7 +1158,7 @@ static int cb_pcidas_ao_inttrig(struct comedi_device *dev,
 	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned long flags;
 
-	if (trig_num != 0)
+	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
 	/*  load up fifo */
@@ -1180,6 +1193,18 @@ static int cb_pcidas_ao_inttrig(struct comedi_device *dev,
 	return 0;
 }
 
+static void cb_pcidas_ao_load_counters(struct comedi_device *dev)
+{
+	struct cb_pcidas_private *devpriv = dev->private;
+	unsigned long timer_base = devpriv->pacer_counter_dio + DAC8254;
+
+	i8254_set_mode(timer_base, 0, 1, I8254_MODE2 | I8254_BINARY);
+	i8254_set_mode(timer_base, 0, 2, I8254_MODE2 | I8254_BINARY);
+
+	i8254_write(timer_base, 0, 1, devpriv->ao_divisor1);
+	i8254_write(timer_base, 0, 2, devpriv->ao_divisor2);
+}
+
 static int cb_pcidas_ao_cmd(struct comedi_device *dev,
 			    struct comedi_subdevice *s)
 {
@@ -1209,18 +1234,9 @@ static int cb_pcidas_ao_cmd(struct comedi_device *dev,
 	outw(0, devpriv->ao_registers + DACFIFOCLR);
 
 	/*  load counters */
-	if (cmd->scan_begin_src == TRIG_TIMER) {
-		i8253_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
-					  &devpriv->ao_divisor1,
-					  &devpriv->ao_divisor2,
-					  &cmd->scan_begin_arg, cmd->flags);
+	if (cmd->scan_begin_src == TRIG_TIMER)
+		cb_pcidas_ao_load_counters(dev);
 
-		/* Write the values of ctr1 and ctr2 into counters 1 and 2 */
-		i8254_load(devpriv->pacer_counter_dio + DAC8254, 0, 1,
-			   devpriv->ao_divisor1, 2);
-		i8254_load(devpriv->pacer_counter_dio + DAC8254, 0, 2,
-			   devpriv->ao_divisor2, 2);
-	}
 	/*  set number of conversions */
 	if (cmd->stop_src == TRIG_COUNT)
 		devpriv->ao_count = cmd->chanlist_len * cmd->stop_arg;
@@ -1305,7 +1321,7 @@ static void handle_ao_interrupt(struct comedi_device *dev, unsigned int status)
 					       num_points * sizeof(short));
 		num_points = num_bytes / sizeof(short);
 
-		if (async->cmd.stop_src == TRIG_COUNT)
+		if (cmd->stop_src == TRIG_COUNT)
 			devpriv->ao_count -= num_points;
 		/*  write data to board's fifo */
 		outsw(devpriv->ao_registers + DACDATA, devpriv->ao_buffer,
@@ -1327,6 +1343,7 @@ static irqreturn_t cb_pcidas_interrupt(int irq, void *d)
 	struct cb_pcidas_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->read_subdev;
 	struct comedi_async *async;
+	struct comedi_cmd *cmd;
 	int status, s5933_status;
 	int half_fifo = thisboard->fifo_size / 2;
 	unsigned int num_samples, i;
@@ -1337,6 +1354,7 @@ static irqreturn_t cb_pcidas_interrupt(int irq, void *d)
 		return IRQ_NONE;
 
 	async = s->async;
+	cmd = &async->cmd;
 
 	s5933_status = inl(devpriv->s5933_config + AMCC_OP_REG_INTCSR);
 
@@ -1359,7 +1377,7 @@ static irqreturn_t cb_pcidas_interrupt(int irq, void *d)
 	if (status & ADHFI) {
 		/*  read data */
 		num_samples = half_fifo;
-		if (async->cmd.stop_src == TRIG_COUNT &&
+		if (cmd->stop_src == TRIG_COUNT &&
 		    num_samples > devpriv->count) {
 			num_samples = devpriv->count;
 		}
@@ -1368,7 +1386,7 @@ static irqreturn_t cb_pcidas_interrupt(int irq, void *d)
 		cfc_write_array_to_buffer(s, devpriv->ai_buffer,
 					  num_samples * sizeof(short));
 		devpriv->count -= num_samples;
-		if (async->cmd.stop_src == TRIG_COUNT && devpriv->count == 0)
+		if (cmd->stop_src == TRIG_COUNT && devpriv->count == 0)
 			async->events |= COMEDI_CB_EOA;
 		/*  clear half-full interrupt latch */
 		spin_lock_irqsave(&dev->spinlock, flags);
@@ -1383,7 +1401,7 @@ static irqreturn_t cb_pcidas_interrupt(int irq, void *d)
 					INT_ADCFIFO)) == 0)
 				break;
 			cfc_write_to_buffer(s, inw(devpriv->adc_fifo));
-			if (async->cmd.stop_src == TRIG_COUNT &&
+			if (cmd->stop_src == TRIG_COUNT &&
 			    --devpriv->count == 0) {
 				/* end of acquisition */
 				async->events |= COMEDI_CB_EOA;
diff --git a/drivers/staging/comedi/drivers/cb_pcidas64.c b/drivers/staging/comedi/drivers/cb_pcidas64.c
index f9afcbe1da5..035c3a17600 100644
--- a/drivers/staging/comedi/drivers/cb_pcidas64.c
+++ b/drivers/staging/comedi/drivers/cb_pcidas64.c
@@ -1995,14 +1995,52 @@ static void check_adc_timing(struct comedi_device *dev, struct comedi_cmd *cmd)
 	return;
 }
 
+static int cb_pcidas64_ai_check_chanlist(struct comedi_device *dev,
+					 struct comedi_subdevice *s,
+					 struct comedi_cmd *cmd)
+{
+	const struct pcidas64_board *board = comedi_board(dev);
+	unsigned int aref0 = CR_AREF(cmd->chanlist[0]);
+	int i;
+
+	for (i = 1; i < cmd->chanlist_len; i++) {
+		unsigned int aref = CR_AREF(cmd->chanlist[i]);
+
+		if (aref != aref0) {
+			dev_dbg(dev->class_dev,
+				"all elements in chanlist must use the same analog reference\n");
+			return -EINVAL;
+		}
+	}
+
+	if (board->layout == LAYOUT_4020) {
+		unsigned int chan0 = CR_CHAN(cmd->chanlist[0]);
+
+		for (i = 1; i < cmd->chanlist_len; i++) {
+			unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+
+			if (chan != (chan0 + i)) {
+				dev_dbg(dev->class_dev,
+					"chanlist must use consecutive channels\n");
+				return -EINVAL;
+			}
+		}
+		if (cmd->chanlist_len == 3) {
+			dev_dbg(dev->class_dev,
+				"chanlist cannot be 3 channels long, use 1, 2, or 4 channels\n");
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
 static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 		      struct comedi_cmd *cmd)
 {
 	const struct pcidas64_board *thisboard = comedi_board(dev);
 	int err = 0;
 	unsigned int tmp_arg, tmp_arg2;
-	int i;
-	int aref;
 	unsigned int triggers;
 
 	/* Step 1 : check if triggers are trivially valid */
@@ -2040,15 +2078,24 @@ static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	if (cmd->convert_src == TRIG_EXT && cmd->scan_begin_src == TRIG_TIMER)
 		err |= -EINVAL;
-	if (cmd->stop_src != TRIG_COUNT &&
-	    cmd->stop_src != TRIG_NONE && cmd->stop_src != TRIG_EXT)
-		err |= -EINVAL;
 
 	if (err)
 		return 2;
 
 	/* Step 3: check if arguments are trivially valid */
 
+	switch (cmd->start_src) {
+	case TRIG_NOW:
+		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+		break;
+	case TRIG_EXT:
+		/*
+		 * start_arg is the CR_CHAN | CR_INVERT of the
+		 * external trigger.
+		 */
+		break;
+	}
+
 	if (cmd->convert_src == TRIG_TIMER) {
 		if (thisboard->layout == LAYOUT_4020) {
 			err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
@@ -2098,36 +2145,9 @@ static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	if (err)
 		return 4;
 
-	/*  make sure user is doesn't change analog reference mid chanlist */
-	if (cmd->chanlist) {
-		aref = CR_AREF(cmd->chanlist[0]);
-		for (i = 1; i < cmd->chanlist_len; i++) {
-			if (aref != CR_AREF(cmd->chanlist[i])) {
-				comedi_error(dev,
-					     "all elements in chanlist must use the same analog reference");
-				err++;
-				break;
-			}
-		}
-		/*  check 4020 chanlist */
-		if (thisboard->layout == LAYOUT_4020) {
-			unsigned int first_channel = CR_CHAN(cmd->chanlist[0]);
-			for (i = 1; i < cmd->chanlist_len; i++) {
-				if (CR_CHAN(cmd->chanlist[i]) !=
-				    first_channel + i) {
-					comedi_error(dev,
-						     "chanlist must use consecutive channels");
-					err++;
-					break;
-				}
-			}
-			if (cmd->chanlist_len == 3) {
-				comedi_error(dev,
-					     "chanlist cannot be 3 channels long, use 1, 2, or 4 channels");
-				err++;
-			}
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= cb_pcidas64_ai_check_chanlist(dev, s, cmd);
 
 	if (err)
 		return 5;
@@ -2703,6 +2723,7 @@ static void drain_dma_buffers(struct comedi_device *dev, unsigned int channel)
 	const struct pcidas64_board *thisboard = comedi_board(dev);
 	struct pcidas64_private *devpriv = dev->private;
 	struct comedi_async *async = dev->read_subdev->async;
+	struct comedi_cmd *cmd = &async->cmd;
 	uint32_t next_transfer_addr;
 	int j;
 	int num_samples = 0;
@@ -2724,7 +2745,7 @@ static void drain_dma_buffers(struct comedi_device *dev, unsigned int channel)
 	      DMA_BUFFER_SIZE) && j < ai_dma_ring_count(thisboard); j++) {
 		/*  transfer data from dma buffer to comedi buffer */
 		num_samples = dma_transfer_size(dev);
-		if (async->cmd.stop_src == TRIG_COUNT) {
+		if (cmd->stop_src == TRIG_COUNT) {
 			if (num_samples > devpriv->ai_count)
 				num_samples = devpriv->ai_count;
 			devpriv->ai_count -= num_samples;
@@ -2873,7 +2894,7 @@ static unsigned int load_ao_dma_buffer(struct comedi_device *dev,
 	buffer_index = devpriv->ao_dma_index;
 	prev_buffer_index = prev_ao_dma_index(dev);
 
-	num_bytes = comedi_buf_read_n_available(dev->write_subdev->async);
+	num_bytes = comedi_buf_read_n_available(dev->write_subdev);
 	if (num_bytes > DMA_BUFFER_SIZE)
 		num_bytes = DMA_BUFFER_SIZE;
 	if (cmd->stop_src == TRIG_COUNT && num_bytes > devpriv->ao_count)
@@ -3184,15 +3205,17 @@ static int prep_ao_dma(struct comedi_device *dev, const struct comedi_cmd *cmd)
 	return 0;
 }
 
-static inline int external_ai_queue_in_use(struct comedi_device *dev)
+static inline int external_ai_queue_in_use(struct comedi_device *dev,
+					   struct comedi_subdevice *s,
+					   struct comedi_cmd *cmd)
 {
 	const struct pcidas64_board *thisboard = comedi_board(dev);
 
-	if (dev->read_subdev->busy)
+	if (s->busy)
 		return 0;
 	if (thisboard->layout == LAYOUT_4020)
 		return 0;
-	else if (use_internal_queue_6xxx(&dev->read_subdev->async->cmd))
+	else if (use_internal_queue_6xxx(cmd))
 		return 0;
 	return 1;
 }
@@ -3204,7 +3227,7 @@ static int ao_inttrig(struct comedi_device *dev, struct comedi_subdevice *s,
 	struct comedi_cmd *cmd = &s->async->cmd;
 	int retval;
 
-	if (trig_num != 0)
+	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
 	retval = prep_ao_dma(dev, cmd);
@@ -3226,7 +3249,7 @@ static int ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	struct pcidas64_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 
-	if (external_ai_queue_in_use(dev)) {
+	if (external_ai_queue_in_use(dev, s, cmd)) {
 		warn_external_queue(dev);
 		return -EBUSY;
 	}
@@ -3247,13 +3270,32 @@ static int ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	return 0;
 }
 
+static int cb_pcidas64_ao_check_chanlist(struct comedi_device *dev,
+					 struct comedi_subdevice *s,
+					 struct comedi_cmd *cmd)
+{
+	unsigned int chan0 = CR_CHAN(cmd->chanlist[0]);
+	int i;
+
+	for (i = 1; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+
+		if (chan != (chan0 + i)) {
+			dev_dbg(dev->class_dev,
+				"chanlist must use consecutive channels\n");
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
 static int ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 		      struct comedi_cmd *cmd)
 {
 	const struct pcidas64_board *thisboard = comedi_board(dev);
 	int err = 0;
 	unsigned int tmp_arg;
-	int i;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -3285,6 +3327,8 @@ static int ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	/* Step 3: check if arguments are trivially valid */
 
+	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+
 	if (cmd->scan_begin_src == TRIG_TIMER) {
 		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
 						 thisboard->ao_scan_speed);
@@ -3315,17 +3359,9 @@ static int ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	if (err)
 		return 4;
 
-	if (cmd->chanlist) {
-		unsigned int first_channel = CR_CHAN(cmd->chanlist[0]);
-		for (i = 1; i < cmd->chanlist_len; i++) {
-			if (CR_CHAN(cmd->chanlist[i]) != first_channel + i) {
-				comedi_error(dev,
-					     "chanlist must use consecutive channels");
-				err++;
-				break;
-			}
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= cb_pcidas64_ao_check_chanlist(dev, s, cmd);
 
 	if (err)
 		return 5;
diff --git a/drivers/staging/comedi/drivers/cb_pcimdas.c b/drivers/staging/comedi/drivers/cb_pcimdas.c
index d3141c865fe..50e522e6e69 100644
--- a/drivers/staging/comedi/drivers/cb_pcimdas.c
+++ b/drivers/staging/comedi/drivers/cb_pcimdas.c
@@ -31,7 +31,8 @@ Configuration Options:
 
 Developed from cb_pcidas and skel by Richard Bytheway (mocelet@sucs.org).
 Only supports DIO, AO and simple AI in it's present form.
-No interrupts, multi channel or FIFO AI, although the card looks like it could support this.
+No interrupts, multi channel or FIFO AI,
+although the card looks like it could support this.
 See http://www.mccdaq.com/PDFs/Manuals/pcim-das1602-16.pdf for more details.
 */
 
@@ -128,8 +129,12 @@ static int cb_pcimdas_ai_rinsn(struct comedi_device *dev,
 		d = d & 0xfd;
 		outb(d, devpriv->BADR3 + 5);
 	}
-	outb(0x01, devpriv->BADR3 + 6);	/* set bursting off, conversions on */
-	outb(0x00, devpriv->BADR3 + 7);	/* set range to 10V. UP/BP is controlled by a switch on the board */
+
+	/* set bursting off, conversions on */
+	outb(0x01, devpriv->BADR3 + 6);
+
+	/* set range to 10V. UP/BP is controlled by a switch on the board */
+	outb(0x00, devpriv->BADR3 + 7);
 
 	/*
 	 * write channel limits to multiplexer, set Low (bits 0-3) and
diff --git a/drivers/staging/comedi/drivers/comedi_fc.c b/drivers/staging/comedi/drivers/comedi_fc.c
index 9d9b1469e89..c33c3e5680a 100644
--- a/drivers/staging/comedi/drivers/comedi_fc.c
+++ b/drivers/staging/comedi/drivers/comedi_fc.c
@@ -24,7 +24,7 @@
 
 unsigned int cfc_bytes_per_scan(struct comedi_subdevice *s)
 {
-	unsigned int chanlist_len = s->async->cmd.chanlist_len;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned int num_samples;
 	unsigned int bits_per_sample;
 
@@ -33,11 +33,11 @@ unsigned int cfc_bytes_per_scan(struct comedi_subdevice *s)
 	case COMEDI_SUBD_DO:
 	case COMEDI_SUBD_DIO:
 		bits_per_sample = 8 * bytes_per_sample(s);
-		num_samples = (chanlist_len + bits_per_sample - 1) /
+		num_samples = (cmd->chanlist_len + bits_per_sample - 1) /
 				bits_per_sample;
 		break;
 	default:
-		num_samples = chanlist_len;
+		num_samples = cmd->chanlist_len;
 		break;
 	}
 	return num_samples * bytes_per_sample(s);
@@ -67,15 +67,15 @@ unsigned int cfc_write_array_to_buffer(struct comedi_subdevice *s,
 	if (num_bytes == 0)
 		return 0;
 
-	retval = comedi_buf_write_alloc(async, num_bytes);
+	retval = comedi_buf_write_alloc(s, num_bytes);
 	if (retval != num_bytes) {
 		dev_warn(s->device->class_dev, "buffer overrun\n");
 		async->events |= COMEDI_CB_OVERFLOW;
 		return 0;
 	}
 
-	comedi_buf_memcpy_to(async, 0, data, num_bytes);
-	comedi_buf_write_free(async, num_bytes);
+	comedi_buf_memcpy_to(s, 0, data, num_bytes);
+	comedi_buf_write_free(s, num_bytes);
 	cfc_inc_scan_progress(s, num_bytes);
 	async->events |= COMEDI_CB_BLOCK;
 
@@ -86,16 +86,14 @@ EXPORT_SYMBOL_GPL(cfc_write_array_to_buffer);
 unsigned int cfc_read_array_from_buffer(struct comedi_subdevice *s,
 					void *data, unsigned int num_bytes)
 {
-	struct comedi_async *async = s->async;
-
 	if (num_bytes == 0)
 		return 0;
 
-	num_bytes = comedi_buf_read_alloc(async, num_bytes);
-	comedi_buf_memcpy_from(async, 0, data, num_bytes);
-	comedi_buf_read_free(async, num_bytes);
+	num_bytes = comedi_buf_read_alloc(s, num_bytes);
+	comedi_buf_memcpy_from(s, 0, data, num_bytes);
+	comedi_buf_read_free(s, num_bytes);
 	cfc_inc_scan_progress(s, num_bytes);
-	async->events |= COMEDI_CB_BLOCK;
+	s->async->events |= COMEDI_CB_BLOCK;
 
 	return num_bytes;
 }
diff --git a/drivers/staging/comedi/drivers/comedi_parport.c b/drivers/staging/comedi/drivers/comedi_parport.c
index 9de81c7712f..a4274869235 100644
--- a/drivers/staging/comedi/drivers/comedi_parport.c
+++ b/drivers/staging/comedi/drivers/comedi_parport.c
@@ -181,7 +181,7 @@ static int parport_intr_cmdtest(struct comedi_device *dev,
 	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, 1);
+	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 	err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
@@ -229,7 +229,7 @@ static irqreturn_t parport_interrupt(int irq, void *d)
 	if (!(ctrl & PARPORT_CTRL_IRQ_ENA))
 		return IRQ_NONE;
 
-	comedi_buf_put(s->async, 0);
+	comedi_buf_put(s, 0);
 	s->async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOS;
 
 	comedi_event(dev, s);
@@ -295,6 +295,7 @@ static int parport_attach(struct comedi_device *dev,
 		s->maxdata	= 1;
 		s->range_table	= &range_digital;
 		s->insn_bits	= parport_intr_insn_bits;
+		s->len_chanlist	= 1;
 		s->do_cmdtest	= parport_intr_cmdtest;
 		s->do_cmd	= parport_intr_cmd;
 		s->cancel	= parport_intr_cancel;
diff --git a/drivers/staging/comedi/drivers/comedi_test.c b/drivers/staging/comedi/drivers/comedi_test.c
index cd9562556d2..67a09aa6b72 100644
--- a/drivers/staging/comedi/drivers/comedi_test.c
+++ b/drivers/staging/comedi/drivers/comedi_test.c
@@ -188,6 +188,7 @@ static void waveform_ai_interrupt(unsigned long arg)
 
 	if (cmd->stop_src == TRIG_COUNT) {
 		unsigned int remaining = cmd->stop_arg - devpriv->ai_count;
+
 		if (num_scans >= remaining) {
 			/* about to finish */
 			num_scans = remaining;
@@ -198,6 +199,7 @@ static void waveform_ai_interrupt(unsigned long arg)
 	for (i = 0; i < num_scans; i++) {
 		for (j = 0; j < cmd->chanlist_len; j++) {
 			unsigned short sample;
+
 			sample = fake_waveform(dev, CR_CHAN(cmd->chanlist[j]),
 					       CR_RANGE(cmd->chanlist[j]),
 					       devpriv->usec_current +
@@ -224,7 +226,7 @@ static int waveform_ai_cmdtest(struct comedi_device *dev,
 			       struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -276,22 +278,18 @@ static int waveform_ai_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		tmp = cmd->scan_begin_arg;
+		arg = cmd->scan_begin_arg;
 		/* round to nearest microsec */
-		cmd->scan_begin_arg =
-		    nano_per_micro * ((tmp +
-				       (nano_per_micro / 2)) / nano_per_micro);
-		if (tmp != cmd->scan_begin_arg)
-			err++;
+		arg = nano_per_micro *
+		      ((arg + (nano_per_micro / 2)) / nano_per_micro);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 	if (cmd->convert_src == TRIG_TIMER) {
-		tmp = cmd->convert_arg;
+		arg = cmd->convert_arg;
 		/* round to nearest microsec */
-		cmd->convert_arg =
-		    nano_per_micro * ((tmp +
-				       (nano_per_micro / 2)) / nano_per_micro);
-		if (tmp != cmd->convert_arg)
-			err++;
+		arg = nano_per_micro *
+		      ((arg + (nano_per_micro / 2)) / nano_per_micro);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 
 	if (err)
@@ -413,11 +411,7 @@ static int waveform_attach(struct comedi_device *dev,
 	s->n_chan = N_CHANS;
 	s->maxdata = 0xffff;
 	s->range_table = &waveform_ai_ranges;
-	s->len_chanlist = s->n_chan * 2;
 	s->insn_write = waveform_ao_insn_write;
-	s->do_cmd = NULL;
-	s->do_cmdtest = NULL;
-	s->cancel = NULL;
 
 	/* Our default loopback value is just a 0V flatline */
 	for (i = 0; i < s->n_chan; i++)
diff --git a/drivers/staging/comedi/drivers/das08.h b/drivers/staging/comedi/drivers/das08.h
index 46a314c5113..18cc170facd 100644
--- a/drivers/staging/comedi/drivers/das08.h
+++ b/drivers/staging/comedi/drivers/das08.h
@@ -40,7 +40,9 @@ struct das08_board_struct {
 };
 
 struct das08_private_struct {
-	unsigned int do_mux_bits;	/*  bits for do/mux register on boards without separate do register */
+	unsigned int do_mux_bits;	/*  bits for do/mux register on boards
+					 *  without separate do register
+					 */
 	const unsigned int *pg_gainlist;
 	unsigned int ao_readback[2];	/* assume 2 AO channels */
 };
diff --git a/drivers/staging/comedi/drivers/das16.c b/drivers/staging/comedi/drivers/das16.c
index 6a7d652ff56..2feecf199f2 100644
--- a/drivers/staging/comedi/drivers/das16.c
+++ b/drivers/staging/comedi/drivers/das16.c
@@ -600,29 +600,56 @@ static void das16_timer_interrupt(unsigned long arg)
 		mod_timer(&devpriv->timer, jiffies + timer_period());
 }
 
+static int das16_ai_check_chanlist(struct comedi_device *dev,
+				   struct comedi_subdevice *s,
+				   struct comedi_cmd *cmd)
+{
+	unsigned int chan0 = CR_CHAN(cmd->chanlist[0]);
+	unsigned int range0 = CR_RANGE(cmd->chanlist[0]);
+	int i;
+
+	for (i = 1; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+		unsigned int range = CR_RANGE(cmd->chanlist[i]);
+
+		if (chan != ((chan0 + i) % s->n_chan)) {
+			dev_dbg(dev->class_dev,
+				"entries in chanlist must be consecutive channels, counting upwards\n");
+			return -EINVAL;
+		}
+
+		if (range != range0) {
+			dev_dbg(dev->class_dev,
+				"entries in chanlist must all have the same gain\n");
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
 static int das16_cmd_test(struct comedi_device *dev, struct comedi_subdevice *s,
 			  struct comedi_cmd *cmd)
 {
 	const struct das16_board *board = comedi_board(dev);
 	struct das16_private_struct *devpriv = dev->private;
-	int err = 0, tmp;
-	int gain, start_chan, i;
-	int mask;
+	int err = 0;
+	unsigned int trig_mask;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
 	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
 
-	mask = TRIG_FOLLOW;
+	trig_mask = TRIG_FOLLOW;
 	if (devpriv->can_burst)
-		mask |= TRIG_TIMER | TRIG_EXT;
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, mask);
+		trig_mask |= TRIG_TIMER | TRIG_EXT;
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src, trig_mask);
 
-	tmp = cmd->convert_src;
-	mask = TRIG_TIMER | TRIG_EXT;
+	trig_mask = TRIG_TIMER | TRIG_EXT;
 	if (devpriv->can_burst)
-		mask |= TRIG_NOW;
-	err |= cfc_check_trigger_src(&cmd->convert_src, mask);
+		trig_mask |= TRIG_NOW;
+	err |= cfc_check_trigger_src(&cmd->convert_src, trig_mask);
 
 	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
 	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
@@ -673,44 +700,28 @@ static int das16_cmd_test(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	/*  step 4: fix up arguments */
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		unsigned int tmp = cmd->scan_begin_arg;
-		/*  set divisors, correct timing arguments */
+		arg = cmd->scan_begin_arg;
 		i8253_cascade_ns_to_timer(devpriv->clockbase,
 					  &devpriv->divisor1,
 					  &devpriv->divisor2,
-					  &cmd->scan_begin_arg, cmd->flags);
-		err += (tmp != cmd->scan_begin_arg);
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 	if (cmd->convert_src == TRIG_TIMER) {
-		unsigned int tmp = cmd->convert_arg;
-		/*  set divisors, correct timing arguments */
+		arg = cmd->convert_arg;
 		i8253_cascade_ns_to_timer(devpriv->clockbase,
 					  &devpriv->divisor1,
 					  &devpriv->divisor2,
-					  &cmd->convert_arg, cmd->flags);
-		err += (tmp != cmd->convert_arg);
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 	if (err)
 		return 4;
 
-	/*  check channel/gain list against card's limitations */
-	if (cmd->chanlist) {
-		gain = CR_RANGE(cmd->chanlist[0]);
-		start_chan = CR_CHAN(cmd->chanlist[0]);
-		for (i = 1; i < cmd->chanlist_len; i++) {
-			if (CR_CHAN(cmd->chanlist[i]) !=
-			    (start_chan + i) % s->n_chan) {
-				dev_err(dev->class_dev,
-					"entries in chanlist must be consecutive channels, counting upwards\n");
-				err++;
-			}
-			if (CR_RANGE(cmd->chanlist[i]) != gain) {
-				dev_err(dev->class_dev,
-					"entries in chanlist must all have the same gain\n");
-				err++;
-			}
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= das16_ai_check_chanlist(dev, s, cmd);
+
 	if (err)
 		return 5;
 
@@ -727,9 +738,10 @@ static unsigned int das16_set_pacer(struct comedi_device *dev, unsigned int ns,
 				  &devpriv->divisor1, &devpriv->divisor2,
 				  &ns, rounding_flags);
 
-	/* Write the values of ctr1 and ctr2 into counters 1 and 2 */
-	i8254_load(timer_base, 0, 1, devpriv->divisor1, 2);
-	i8254_load(timer_base, 0, 2, devpriv->divisor2, 2);
+	i8254_set_mode(timer_base, 0, 1, I8254_MODE2 | I8254_BINARY);
+	i8254_set_mode(timer_base, 0, 2, I8254_MODE2 | I8254_BINARY);
+	i8254_write(timer_base, 0, 1, devpriv->divisor1);
+	i8254_write(timer_base, 0, 2, devpriv->divisor2);
 
 	return ns;
 }
@@ -750,8 +762,7 @@ static int das16_cmd_exec(struct comedi_device *dev, struct comedi_subdevice *s)
 		return -1;
 	}
 
-	devpriv->adc_byte_count =
-	    cmd->stop_arg * cmd->chanlist_len * sizeof(uint16_t);
+	devpriv->adc_byte_count = cmd->stop_arg * cfc_bytes_per_scan(s);
 
 	if (devpriv->can_burst)
 		outb(DAS1600_CONV_DISABLE, dev->iobase + DAS1600_CONV_REG);
diff --git a/drivers/staging/comedi/drivers/das16m1.c b/drivers/staging/comedi/drivers/das16m1.c
index 779225831dc..ec039fbff0f 100644
--- a/drivers/staging/comedi/drivers/das16m1.c
+++ b/drivers/staging/comedi/drivers/das16m1.c
@@ -150,11 +150,40 @@ static void munge_sample_array(unsigned short *array, unsigned int num_elements)
 		array[i] = munge_sample(array[i]);
 }
 
+static int das16m1_ai_check_chanlist(struct comedi_device *dev,
+				     struct comedi_subdevice *s,
+				     struct comedi_cmd *cmd)
+{
+	int i;
+
+	if (cmd->chanlist_len == 1)
+		return 0;
+
+	if ((cmd->chanlist_len % 2) != 0) {
+		dev_dbg(dev->class_dev,
+			"chanlist must be of even length or length 1\n");
+		return -EINVAL;
+	}
+
+	for (i = 0; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+
+		if ((i % 2) != (chan % 2)) {
+			dev_dbg(dev->class_dev,
+				 "even/odd channels must go have even/odd chanlist indices\n");
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
 static int das16m1_cmd_test(struct comedi_device *dev,
 			    struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
 	struct das16m1_private_struct *devpriv = dev->private;
-	unsigned int err = 0, tmp, i;
+	int err = 0;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -203,35 +232,20 @@ static int das16m1_cmd_test(struct comedi_device *dev,
 	/* step 4: fix up arguments */
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		tmp = cmd->convert_arg;
-		/* calculate counter values that give desired timing */
+		arg = cmd->convert_arg;
 		i8253_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
 					  &devpriv->divisor1,
 					  &devpriv->divisor2,
-					  &cmd->convert_arg, cmd->flags);
-		if (tmp != cmd->convert_arg)
-			err++;
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 
 	if (err)
 		return 4;
 
-	/*  check chanlist against board's peculiarities */
-	if (cmd->chanlist && cmd->chanlist_len > 1) {
-		for (i = 0; i < cmd->chanlist_len; i++) {
-			/*  even/odd channels must go into even/odd queue addresses */
-			if ((i % 2) != (CR_CHAN(cmd->chanlist[i]) % 2)) {
-				comedi_error(dev, "bad chanlist:\n"
-					     " even/odd channels must go have even/odd chanlist indices");
-				err++;
-			}
-		}
-		if ((cmd->chanlist_len % 2) != 0) {
-			comedi_error(dev,
-				     "chanlist must be of even length or length 1");
-			err++;
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= das16m1_ai_check_chanlist(dev, s, cmd);
 
 	if (err)
 		return 5;
@@ -239,26 +253,16 @@ static int das16m1_cmd_test(struct comedi_device *dev,
 	return 0;
 }
 
-/* This function takes a time in nanoseconds and sets the     *
- * 2 pacer clocks to the closest frequency possible. It also  *
- * returns the actual sampling period.                        */
-static unsigned int das16m1_set_pacer(struct comedi_device *dev,
-				      unsigned int ns, int rounding_flags)
+static void das16m1_set_pacer(struct comedi_device *dev)
 {
 	struct das16m1_private_struct *devpriv = dev->private;
+	unsigned long timer_base = dev->iobase + DAS16M1_8254_SECOND;
 
-	i8253_cascade_ns_to_timer_2div(I8254_OSC_BASE_10MHZ,
-				       &devpriv->divisor1,
-				       &devpriv->divisor2,
-				       &ns, rounding_flags);
-
-	/* Write the values of ctr1 and ctr2 into counters 1 and 2 */
-	i8254_load(dev->iobase + DAS16M1_8254_SECOND, 0, 1, devpriv->divisor1,
-		   2);
-	i8254_load(dev->iobase + DAS16M1_8254_SECOND, 0, 2, devpriv->divisor2,
-		   2);
+	i8254_set_mode(timer_base, 0, 1, I8254_MODE2 | I8254_BINARY);
+	i8254_set_mode(timer_base, 0, 2, I8254_MODE2 | I8254_BINARY);
 
-	return ns;
+	i8254_write(timer_base, 0, 1, devpriv->divisor1);
+	i8254_write(timer_base, 0, 2, devpriv->divisor2);
 }
 
 static int das16m1_cmd_exec(struct comedi_device *dev,
@@ -267,6 +271,7 @@ static int das16m1_cmd_exec(struct comedi_device *dev,
 	struct das16m1_private_struct *devpriv = dev->private;
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
+	unsigned long timer_base = dev->iobase + DAS16M1_8254_FIRST;
 	unsigned int byte, i;
 
 	/* disable interrupts and internal pacer */
@@ -278,11 +283,11 @@ static int das16m1_cmd_exec(struct comedi_device *dev,
 	/* Initialize lower half of hardware counter, used to determine how
 	 * many samples are in fifo.  Value doesn't actually load into counter
 	 * until counter's next clock (the next a/d conversion) */
-	i8254_load(dev->iobase + DAS16M1_8254_FIRST, 0, 1, 0, 2);
+	i8254_set_mode(timer_base, 0, 1, I8254_MODE2 | I8254_BINARY);
+	i8254_write(timer_base, 0, 1, 0);
 	/* remember current reading of counter so we know when counter has
 	 * actually been loaded */
-	devpriv->initial_hw_count =
-	    i8254_read(dev->iobase + DAS16M1_8254_FIRST, 0, 1);
+	devpriv->initial_hw_count = i8254_read(timer_base, 0, 1);
 	/* setup channel/gain queue */
 	for (i = 0; i < cmd->chanlist_len; i++) {
 		outb(i, dev->iobase + DAS16M1_QUEUE_ADDR);
@@ -292,10 +297,14 @@ static int das16m1_cmd_exec(struct comedi_device *dev,
 		outb(byte, dev->iobase + DAS16M1_QUEUE_DATA);
 	}
 
-	/* set counter mode and counts */
-	cmd->convert_arg =
-	    das16m1_set_pacer(dev, cmd->convert_arg,
-			      cmd->flags & TRIG_ROUND_MASK);
+	/* enable interrupts and set internal pacer counter mode and counts */
+	devpriv->control_state &= ~PACER_MASK;
+	if (cmd->convert_src == TRIG_TIMER) {
+		das16m1_set_pacer(dev);
+		devpriv->control_state |= INT_PACER;
+	} else {	/* TRIG_EXT */
+		devpriv->control_state |= EXT_PACER;
+	}
 
 	/*  set control & status register */
 	byte = 0;
@@ -308,13 +317,6 @@ static int das16m1_cmd_exec(struct comedi_device *dev,
 	/* clear interrupt bit */
 	outb(0, dev->iobase + DAS16M1_CLEAR_INTR);
 
-	/* enable interrupts and internal pacer */
-	devpriv->control_state &= ~PACER_MASK;
-	if (cmd->convert_src == TRIG_TIMER)
-		devpriv->control_state |= INT_PACER;
-	else
-		devpriv->control_state |= EXT_PACER;
-
 	devpriv->control_state |= INTE;
 	outb(devpriv->control_state, dev->iobase + DAS16M1_INTR_CONTROL);
 
diff --git a/drivers/staging/comedi/drivers/das1800.c b/drivers/staging/comedi/drivers/das1800.c
index 8e975d6b06d..859519026c4 100644
--- a/drivers/staging/comedi/drivers/das1800.c
+++ b/drivers/staging/comedi/drivers/das1800.c
@@ -547,7 +547,7 @@ static void das1800_flush_dma_channel(struct comedi_device *dev,
 
 	munge_data(dev, buffer, num_samples);
 	cfc_write_array_to_buffer(s, buffer, num_bytes);
-	if (s->async->cmd.stop_src == TRIG_COUNT)
+	if (cmd->stop_src == TRIG_COUNT)
 		devpriv->count -= num_samples;
 
 	return;
@@ -731,7 +731,7 @@ static irqreturn_t das1800_interrupt(int irq, void *d)
 /* converts requested conversion timing to timing compatible with
  * hardware, used only when card is in 'burst mode'
  */
-static unsigned int burst_convert_arg(unsigned int convert_arg, int round_mode)
+static unsigned int burst_convert_arg(unsigned int convert_arg, int flags)
 {
 	unsigned int micro_sec;
 
@@ -740,7 +740,7 @@ static unsigned int burst_convert_arg(unsigned int convert_arg, int round_mode)
 		convert_arg = 64000;
 
 	/*  the conversion time must be an integral number of microseconds */
-	switch (round_mode) {
+	switch (flags & TRIG_ROUND_MASK) {
 	case TRIG_ROUND_NEAREST:
 	default:
 		micro_sec = (convert_arg + 500) / 1000;
@@ -757,6 +757,26 @@ static unsigned int burst_convert_arg(unsigned int convert_arg, int round_mode)
 	return micro_sec * 1000;
 }
 
+static int das1800_ai_check_chanlist(struct comedi_device *dev,
+				     struct comedi_subdevice *s,
+				     struct comedi_cmd *cmd)
+{
+	unsigned int unipolar0 = CR_RANGE(cmd->chanlist[0]) & UNIPOLAR;
+	int i;
+
+	for (i = 1; i < cmd->chanlist_len; i++) {
+		unsigned int unipolar = CR_RANGE(cmd->chanlist[i]) & UNIPOLAR;
+
+		if (unipolar != unipolar0) {
+			dev_dbg(dev->class_dev,
+				"unipolar and bipolar ranges cannot be mixed in the chanlist\n");
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
 /* test analog input cmd */
 static int das1800_ai_do_cmdtest(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
@@ -765,9 +785,7 @@ static int das1800_ai_do_cmdtest(struct comedi_device *dev,
 	const struct das1800_board *thisboard = comedi_board(dev);
 	struct das1800_private *devpriv = dev->private;
 	int err = 0;
-	unsigned int tmp_arg;
-	int i;
-	int unipolar;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -825,66 +843,48 @@ static int das1800_ai_do_cmdtest(struct comedi_device *dev,
 
 	/* step 4: fix up any arguments */
 
-	if (cmd->convert_src == TRIG_TIMER) {
-		/*  if we are not in burst mode */
-		if (cmd->scan_begin_src == TRIG_FOLLOW) {
-			tmp_arg = cmd->convert_arg;
-			/* calculate counter values that give desired timing */
+	if (cmd->scan_begin_src == TRIG_FOLLOW &&
+	    cmd->convert_src == TRIG_TIMER) {
+		/* we are not in burst mode */
+		arg = cmd->convert_arg;
+		i8253_cascade_ns_to_timer(I8254_OSC_BASE_5MHZ,
+					  &devpriv->divisor1,
+					  &devpriv->divisor2,
+					  &cmd->convert_arg, cmd->flags);
+		if (arg != cmd->convert_arg)
+			err++;
+	} else if (cmd->convert_src == TRIG_TIMER) {
+		/* we are in burst mode */
+		arg = cmd->convert_arg;
+		cmd->convert_arg = burst_convert_arg(cmd->convert_arg,
+						     cmd->flags);
+		if (arg != cmd->convert_arg)
+			err++;
+
+		if (cmd->scan_begin_src == TRIG_TIMER) {
+			arg = cmd->convert_arg * cmd->chanlist_len;
+			if (arg > cmd->scan_begin_arg) {
+				cmd->scan_begin_arg = arg;
+				err++;
+			}
+
+			arg = cmd->scan_begin_arg;
 			i8253_cascade_ns_to_timer(I8254_OSC_BASE_5MHZ,
 						  &devpriv->divisor1,
 						  &devpriv->divisor2,
-						  &cmd->convert_arg,
+						  &cmd->scan_begin_arg,
 						  cmd->flags);
-			if (tmp_arg != cmd->convert_arg)
-				err++;
-		}
-		/*  if we are in burst mode */
-		else {
-			/*  check that convert_arg is compatible */
-			tmp_arg = cmd->convert_arg;
-			cmd->convert_arg =
-			    burst_convert_arg(cmd->convert_arg,
-					      cmd->flags & TRIG_ROUND_MASK);
-			if (tmp_arg != cmd->convert_arg)
+			if (arg != cmd->scan_begin_arg)
 				err++;
-
-			if (cmd->scan_begin_src == TRIG_TIMER) {
-				/*  if scans are timed faster than conversion rate allows */
-				if (cmd->convert_arg * cmd->chanlist_len >
-				    cmd->scan_begin_arg) {
-					cmd->scan_begin_arg =
-					    cmd->convert_arg *
-					    cmd->chanlist_len;
-					err++;
-				}
-				tmp_arg = cmd->scan_begin_arg;
-				/* calculate counter values that give desired timing */
-				i8253_cascade_ns_to_timer(I8254_OSC_BASE_5MHZ,
-							  &devpriv->divisor1,
-							  &devpriv->divisor2,
-							  &cmd->scan_begin_arg,
-							  cmd->flags);
-				if (tmp_arg != cmd->scan_begin_arg)
-					err++;
-			}
 		}
 	}
 
 	if (err)
 		return 4;
 
-	/*  make sure user is not trying to mix unipolar and bipolar ranges */
-	if (cmd->chanlist) {
-		unipolar = CR_RANGE(cmd->chanlist[0]) & UNIPOLAR;
-		for (i = 1; i < cmd->chanlist_len; i++) {
-			if (unipolar != (CR_RANGE(cmd->chanlist[i]) & UNIPOLAR)) {
-				comedi_error(dev,
-					     "unipolar and bipolar ranges cannot be mixed in the chanlist");
-				err++;
-				break;
-			}
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= das1800_ai_check_chanlist(dev, s, cmd);
 
 	if (err)
 		return 5;
@@ -962,68 +962,29 @@ static int control_c_bits(const struct comedi_cmd *cmd)
 	return control_c;
 }
 
-/* loads counters with divisor1, divisor2 from private structure */
-static int das1800_set_frequency(struct comedi_device *dev)
+static void das1800_setup_counters(struct comedi_device *dev,
+				   const struct comedi_cmd *cmd)
 {
 	struct das1800_private *devpriv = dev->private;
-	int err = 0;
-
-	/*  counter 1, mode 2 */
-	if (i8254_load(dev->iobase + DAS1800_COUNTER, 0, 1, devpriv->divisor1,
-		       2))
-		err++;
-	/*  counter 2, mode 2 */
-	if (i8254_load(dev->iobase + DAS1800_COUNTER, 0, 2, devpriv->divisor2,
-		       2))
-		err++;
-	if (err)
-		return -1;
+	unsigned long timer_base = dev->iobase + DAS1800_COUNTER;
 
-	return 0;
-}
-
-/* sets up counters */
-static int setup_counters(struct comedi_device *dev,
-			  const struct comedi_cmd *cmd)
-{
-	struct das1800_private *devpriv = dev->private;
-	unsigned int period;
+	/* setup cascaded counters for conversion/scan frequency */
+	if ((cmd->scan_begin_src == TRIG_FOLLOW ||
+	     cmd->scan_begin_src == TRIG_TIMER) &&
+	    cmd->convert_src == TRIG_TIMER) {
+		i8254_set_mode(timer_base, 0, 1, I8254_MODE2 | I8254_BINARY);
+		i8254_set_mode(timer_base, 0, 2, I8254_MODE2 | I8254_BINARY);
 
-	/*  setup cascaded counters for conversion/scan frequency */
-	switch (cmd->scan_begin_src) {
-	case TRIG_FOLLOW:	/*  not in burst mode */
-		if (cmd->convert_src == TRIG_TIMER) {
-			/* set conversion frequency */
-			period = cmd->convert_arg;
-			i8253_cascade_ns_to_timer(I8254_OSC_BASE_5MHZ,
-						  &devpriv->divisor1,
-						  &devpriv->divisor2,
-						  &period, cmd->flags);
-			if (das1800_set_frequency(dev) < 0)
-				return -1;
-		}
-		break;
-	case TRIG_TIMER:	/*  in burst mode */
-		/* set scan frequency */
-		period = cmd->scan_begin_arg;
-		i8253_cascade_ns_to_timer(I8254_OSC_BASE_5MHZ,
-					  &devpriv->divisor1,
-					  &devpriv->divisor2,
-					  &period, cmd->flags);
-		if (das1800_set_frequency(dev) < 0)
-			return -1;
-		break;
-	default:
-		break;
+		i8254_write(timer_base, 0, 1, devpriv->divisor1);
+		i8254_write(timer_base, 0, 2, devpriv->divisor2);
 	}
 
-	/*  setup counter 0 for 'about triggering' */
+	/* setup counter 0 for 'about triggering' */
 	if (cmd->stop_src == TRIG_EXT) {
-		/*  load counter 0 in mode 0 */
-		i8254_load(dev->iobase + DAS1800_COUNTER, 0, 0, 1, 0);
-	}
+		i8254_set_mode(timer_base, 0, 0, I8254_MODE0 | I8254_BINARY);
 
-	return 0;
+		i8254_write(timer_base, 0, 0, 1);
+	}
 }
 
 /* utility function that suggests a dma transfer size based on the conversion period 'ns' */
@@ -1136,7 +1097,6 @@ static int das1800_ai_do_cmd(struct comedi_device *dev,
 			     struct comedi_subdevice *s)
 {
 	struct das1800_private *devpriv = dev->private;
-	int ret;
 	int control_a, control_c;
 	struct comedi_async *async = s->async;
 	const struct comedi_cmd *cmd = &async->cmd;
@@ -1167,11 +1127,7 @@ static int das1800_ai_do_cmd(struct comedi_device *dev,
 
 	/* setup card and start */
 	program_chanlist(dev, cmd);
-	ret = setup_counters(dev, cmd);
-	if (ret < 0) {
-		comedi_error(dev, "Error setting up counters");
-		return ret;
-	}
+	das1800_setup_counters(dev, cmd);
 	setup_dma(dev, cmd);
 	outb(control_c, dev->iobase + DAS1800_CONTROL_C);
 	/*  set conversion rate and length for burst mode */
@@ -1470,7 +1426,7 @@ 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);
+	const struct das1800_board *thisboard;
 	struct das1800_private *devpriv;
 	struct comedi_subdevice *s;
 	unsigned int irq = it->options[1];
diff --git a/drivers/staging/comedi/drivers/das6402.c b/drivers/staging/comedi/drivers/das6402.c
index e0cfb6cb547..d18eea6c01a 100644
--- a/drivers/staging/comedi/drivers/das6402.c
+++ b/drivers/staging/comedi/drivers/das6402.c
@@ -125,7 +125,7 @@ struct das6402_boardinfo {
 	unsigned int maxdata;
 };
 
-struct das6402_boardinfo das6402_boards[] = {
+static struct das6402_boardinfo das6402_boards[] = {
 	{
 		.name		= "das6402-12",
 		.maxdata	= 0x0fff,
diff --git a/drivers/staging/comedi/drivers/das800.c b/drivers/staging/comedi/drivers/das800.c
index 3e408370dcf..6f7f8d531dd 100644
--- a/drivers/staging/comedi/drivers/das800.c
+++ b/drivers/staging/comedi/drivers/das800.c
@@ -224,7 +224,6 @@ struct das800_private {
 	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 void das800_ind_write(struct comedi_device *dev,
@@ -275,31 +274,54 @@ static void das800_disable(struct comedi_device *dev)
 	spin_unlock_irqrestore(&dev->spinlock, irq_flags);
 }
 
-static int das800_set_frequency(struct comedi_device *dev)
+static void 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;
+	unsigned long timer_base = dev->iobase + DAS800_8254;
 
-	return 0;
+	i8254_set_mode(timer_base, 0, 1, I8254_MODE2 | I8254_BINARY);
+	i8254_set_mode(timer_base, 0, 2, I8254_MODE2 | I8254_BINARY);
+	i8254_write(timer_base, 0, 1, devpriv->divisor1);
+	i8254_write(timer_base, 0, 2, devpriv->divisor2);
 }
 
 static int das800_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 {
 	struct das800_private *devpriv = dev->private;
 
-	devpriv->forever = false;
 	devpriv->count = 0;
 	das800_disable(dev);
 	return 0;
 }
 
+static int das800_ai_check_chanlist(struct comedi_device *dev,
+				    struct comedi_subdevice *s,
+				    struct comedi_cmd *cmd)
+{
+	unsigned int chan0 = CR_CHAN(cmd->chanlist[0]);
+	unsigned int range0 = CR_RANGE(cmd->chanlist[0]);
+	int i;
+
+	for (i = 1; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+		unsigned int range = CR_RANGE(cmd->chanlist[i]);
+
+		if (chan != (chan0 + i) % s->n_chan) {
+			dev_dbg(dev->class_dev,
+				"chanlist must be consecutive, counting upwards\n");
+			return -EINVAL;
+		}
+
+		if (range != range0) {
+			dev_dbg(dev->class_dev,
+				"chanlist must all have the same gain\n");
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
 static int das800_ai_do_cmdtest(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_cmd *cmd)
@@ -307,6 +329,7 @@ static int das800_ai_do_cmdtest(struct comedi_device *dev,
 	const struct das800_board *thisboard = comedi_board(dev);
 	struct das800_private *devpriv = dev->private;
 	int err = 0;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -352,41 +375,20 @@ static int das800_ai_do_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		int tmp = cmd->convert_arg;
-
-		/* calculate counter values that give desired timing */
+		arg = cmd->convert_arg;
 		i8253_cascade_ns_to_timer(I8254_OSC_BASE_1MHZ,
 					  &devpriv->divisor1,
 					  &devpriv->divisor2,
-					  &cmd->convert_arg, cmd->flags);
-		if (tmp != cmd->convert_arg)
-			err++;
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 
 	if (err)
 		return 4;
 
-	/*  check channel/gain list against card's limitations */
-	if (cmd->chanlist) {
-		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++) {
-			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(next) != range) {
-				dev_err(dev->class_dev,
-					"chanlist must all have the same gain\n");
-				err++;
-			}
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= das800_ai_check_chanlist(dev, s, cmd);
 
 	if (err)
 		return 5;
@@ -400,9 +402,10 @@ static int das800_ai_do_cmd(struct comedi_device *dev,
 	const struct das800_board *thisboard = comedi_board(dev);
 	struct das800_private *devpriv = dev->private;
 	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;
+	struct comedi_cmd *cmd = &async->cmd;
+	unsigned int gain = CR_RANGE(cmd->chanlist[0]);
+	unsigned int start_chan = CR_CHAN(cmd->chanlist[0]);
+	unsigned int end_chan = (start_chan + cmd->chanlist_len - 1) % 8;
 	unsigned int scan_chans = (end_chan << 3) | start_chan;
 	int conv_bits;
 	unsigned long irq_flags;
@@ -420,28 +423,22 @@ static int das800_ai_do_cmd(struct comedi_device *dev,
 	gain &= 0xf;
 	outb(gain, dev->iobase + DAS800_GAIN);
 
-	if (async->cmd.stop_src == TRIG_COUNT) {
-		devpriv->count = async->cmd.stop_arg * async->cmd.chanlist_len;
-		devpriv->forever = false;
-	} else {	/* TRIG_NONE */
-		devpriv->forever = true;
+	if (cmd->stop_src == TRIG_COUNT)
+		devpriv->count = cmd->stop_arg * cmd->chanlist_len;
+	else	/* TRIG_NONE */
 		devpriv->count = 0;
-	}
 
 	/* enable auto channel scan, send interrupts on end of conversion
 	 * and set clock source to internal or external
 	 */
 	conv_bits = 0;
 	conv_bits |= EACS | IEOC;
-	if (async->cmd.start_src == TRIG_EXT)
+	if (cmd->start_src == TRIG_EXT)
 		conv_bits |= DTEN;
-	if (async->cmd.convert_src == TRIG_TIMER) {
+	if (cmd->convert_src == TRIG_TIMER) {
 		conv_bits |= CASC | ITE;
 		/* set conversion frequency */
-		if (das800_set_frequency(dev) < 0) {
-			comedi_error(dev, "Error setting up counters");
-			return -1;
-		}
+		das800_set_frequency(dev);
 	}
 
 	spin_lock_irqsave(&dev->spinlock, irq_flags);
@@ -465,7 +462,8 @@ static irqreturn_t das800_interrupt(int irq, void *d)
 	struct comedi_device *dev = d;
 	struct das800_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->read_subdev;
-	struct comedi_async *async = s ? s->async : NULL;
+	struct comedi_async *async;
+	struct comedi_cmd *cmd;
 	unsigned long irq_flags;
 	unsigned int status;
 	unsigned int val;
@@ -479,6 +477,9 @@ static irqreturn_t das800_interrupt(int irq, void *d)
 	if (!dev->attached)
 		return IRQ_HANDLED;
 
+	async = s->async;
+	cmd = &async->cmd;
+
 	spin_lock_irqsave(&dev->spinlock, irq_flags);
 	status = das800_ind_read(dev, CONTROL1) & STATUS2_HCEN;
 	/*
@@ -510,7 +511,7 @@ static irqreturn_t das800_interrupt(int irq, void *d)
 			val >>= 4;	/* 12-bit sample */
 
 		/* if there are more data points to collect */
-		if (devpriv->count > 0 || devpriv->forever) {
+		if (cmd->stop_src == TRIG_NONE || devpriv->count > 0) {
 			/* write data point to buffer */
 			cfc_write_to_buffer(s, val & s->maxdata);
 			devpriv->count--;
@@ -525,7 +526,7 @@ static irqreturn_t das800_interrupt(int irq, void *d)
 		return IRQ_HANDLED;
 	}
 
-	if (devpriv->count > 0 || devpriv->forever) {
+	if (cmd->stop_src == TRIG_NONE || devpriv->count > 0) {
 		/* Re-enable card's interrupt.
 		 * We already have spinlock, so indirect addressing is safe */
 		das800_ind_write(dev, CONTROL1_INTE | devpriv->do_bits,
@@ -673,7 +674,7 @@ static int das800_probe(struct comedi_device *dev)
 
 static int das800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	const struct das800_board *thisboard = comedi_board(dev);
+	const struct das800_board *thisboard;
 	struct das800_private *devpriv;
 	struct comedi_subdevice *s;
 	unsigned int irq = it->options[1];
diff --git a/drivers/staging/comedi/drivers/dmm32at.c b/drivers/staging/comedi/drivers/dmm32at.c
index c8a36eb5f01..ad7a5d53b97 100644
--- a/drivers/staging/comedi/drivers/dmm32at.c
+++ b/drivers/staging/comedi/drivers/dmm32at.c
@@ -243,13 +243,39 @@ static int dmm32at_ns_to_timer(unsigned int *ns, int round)
 	return *ns;
 }
 
+static int dmm32at_ai_check_chanlist(struct comedi_device *dev,
+				     struct comedi_subdevice *s,
+				     struct comedi_cmd *cmd)
+{
+	unsigned int chan0 = CR_CHAN(cmd->chanlist[0]);
+	unsigned int range0 = CR_RANGE(cmd->chanlist[0]);
+	int i;
+
+	for (i = 1; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+		unsigned int range = CR_RANGE(cmd->chanlist[i]);
+
+		if (chan != (chan0 + i) % s->n_chan) {
+			dev_dbg(dev->class_dev,
+				"entries in chanlist must be consecutive channels, counting upwards\n");
+			return -EINVAL;
+		}
+		if (range != range0) {
+			dev_dbg(dev->class_dev,
+				"entries in chanlist must all have the same gain\n");
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
 static int dmm32at_ai_cmdtest(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
 			      struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int tmp;
-	int start_chan, gain, i;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -325,50 +351,28 @@ static int dmm32at_ai_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		tmp = cmd->scan_begin_arg;
-		dmm32at_ns_to_timer(&cmd->scan_begin_arg,
-				    cmd->flags & TRIG_ROUND_MASK);
-		if (tmp != cmd->scan_begin_arg)
-			err++;
+		arg = cmd->scan_begin_arg;
+		dmm32at_ns_to_timer(&arg, cmd->flags & TRIG_ROUND_MASK);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 	if (cmd->convert_src == TRIG_TIMER) {
-		tmp = cmd->convert_arg;
-		dmm32at_ns_to_timer(&cmd->convert_arg,
-				    cmd->flags & TRIG_ROUND_MASK);
-		if (tmp != cmd->convert_arg)
-			err++;
-		if (cmd->scan_begin_src == TRIG_TIMER &&
-		    cmd->scan_begin_arg <
-		    cmd->convert_arg * cmd->scan_end_arg) {
-			cmd->scan_begin_arg =
-			    cmd->convert_arg * cmd->scan_end_arg;
-			err++;
+		arg = cmd->convert_arg;
+		dmm32at_ns_to_timer(&arg, cmd->flags & TRIG_ROUND_MASK);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
+
+		if (cmd->scan_begin_src == TRIG_TIMER) {
+			arg = cmd->convert_arg * cmd->scan_end_arg;
+			err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
+							 arg);
 		}
 	}
 
 	if (err)
 		return 4;
 
-	/* step 5 check the channel list, the channel list for this
-	   board must be consecutive and gains must be the same */
-
-	if (cmd->chanlist) {
-		gain = CR_RANGE(cmd->chanlist[0]);
-		start_chan = CR_CHAN(cmd->chanlist[0]);
-		for (i = 1; i < cmd->chanlist_len; i++) {
-			if (CR_CHAN(cmd->chanlist[i]) !=
-			    (start_chan + i) % s->n_chan) {
-				comedi_error(dev,
-					     "entries in chanlist must be consecutive channels, counting upwards\n");
-				err++;
-			}
-			if (CR_RANGE(cmd->chanlist[i]) != gain) {
-				comedi_error(dev,
-					     "entries in chanlist must all have the same gain\n");
-				err++;
-			}
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= dmm32at_ai_check_chanlist(dev, s, cmd);
 
 	if (err)
 		return 5;
@@ -464,12 +468,6 @@ static int dmm32at_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		outb(0xff, dev->iobase + DMM32AT_CONV);
 	}
 
-/*	for(i=0;i<cmd->chanlist_len;i++) */
-/*		comedi_buf_put(s->async,i*100); */
-
-/*	s->async->events |= COMEDI_CB_EOA; */
-/*	comedi_event(dev, s); */
-
 	return 0;
 
 }
@@ -510,7 +508,7 @@ static irqreturn_t dmm32at_isr(int irq, void *d)
 
 			/* invert sign bit to make range unsigned */
 			samp = ((msb ^ 0x0080) << 8) + lsb;
-			comedi_buf_put(s->async, samp);
+			comedi_buf_put(s, samp);
 		}
 
 		if (devpriv->ai_scans_left != 0xffffffff) {	/* TRIG_COUNT */
diff --git a/drivers/staging/comedi/drivers/dt2801.c b/drivers/staging/comedi/drivers/dt2801.c
index d4d4e4b497d..4263014426f 100644
--- a/drivers/staging/comedi/drivers/dt2801.c
+++ b/drivers/staging/comedi/drivers/dt2801.c
@@ -545,7 +545,7 @@ 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);
+	const struct dt2801_board *board;
 	struct dt2801_private *devpriv;
 	struct comedi_subdevice *s;
 	int board_code, type;
diff --git a/drivers/staging/comedi/drivers/dt2814.c b/drivers/staging/comedi/drivers/dt2814.c
index 3794b7e5209..904c9f0e4af 100644
--- a/drivers/staging/comedi/drivers/dt2814.c
+++ b/drivers/staging/comedi/drivers/dt2814.c
@@ -128,7 +128,7 @@ static int dt2814_ai_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -170,10 +170,9 @@ static int dt2814_ai_cmdtest(struct comedi_device *dev,
 
 	/* step 4: fix up any arguments */
 
-	tmp = cmd->scan_begin_arg;
-	dt2814_ns_to_timer(&cmd->scan_begin_arg, cmd->flags & TRIG_ROUND_MASK);
-	if (tmp != cmd->scan_begin_arg)
-		err++;
+	arg = cmd->scan_begin_arg;
+	dt2814_ns_to_timer(&arg, cmd->flags & TRIG_ROUND_MASK);
+	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 
 	if (err)
 		return 4;
diff --git a/drivers/staging/comedi/drivers/dt282x.c b/drivers/staging/comedi/drivers/dt282x.c
index 16cc100531e..c2a66dcf99f 100644
--- a/drivers/staging/comedi/drivers/dt282x.c
+++ b/drivers/staging/comedi/drivers/dt282x.c
@@ -466,7 +466,7 @@ static irqreturn_t dt282x_interrupt(int irq, void *d)
 
 		if (devpriv->ad_2scomp)
 			data ^= 1 << (board->adbits - 1);
-		ret = comedi_buf_put(s->async, data);
+		ret = comedi_buf_put(s, data);
 
 		if (ret == 0)
 			s->async->events |= COMEDI_CB_OVERFLOW;
@@ -578,7 +578,7 @@ static int dt282x_ai_cmdtest(struct comedi_device *dev,
 {
 	const struct dt282x_board *board = comedi_board(dev);
 	int err = 0;
-	int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -633,10 +633,9 @@ static int dt282x_ai_cmdtest(struct comedi_device *dev,
 
 	/* step 4: fix up any arguments */
 
-	tmp = cmd->convert_arg;
-	dt282x_ns_to_timer(&cmd->convert_arg, cmd->flags & TRIG_ROUND_MASK);
-	if (tmp != cmd->convert_arg)
-		err++;
+	arg = cmd->convert_arg;
+	dt282x_ns_to_timer(&arg, cmd->flags & TRIG_ROUND_MASK);
+	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
 
 	if (err)
 		return 4;
@@ -825,7 +824,7 @@ static int dt282x_ao_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -852,7 +851,7 @@ static int dt282x_ao_cmdtest(struct comedi_device *dev,
 	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
 	err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg, 5000);
 	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
-	err |= cfc_check_trigger_arg_max(&cmd->scan_end_arg, 2);
+	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT) {
 		/* any count is allowed */
@@ -865,10 +864,9 @@ static int dt282x_ao_cmdtest(struct comedi_device *dev,
 
 	/* step 4: fix up any arguments */
 
-	tmp = cmd->scan_begin_arg;
-	dt282x_ns_to_timer(&cmd->scan_begin_arg, cmd->flags & TRIG_ROUND_MASK);
-	if (tmp != cmd->scan_begin_arg)
-		err++;
+	arg = cmd->scan_begin_arg;
+	dt282x_ns_to_timer(&arg, cmd->flags & TRIG_ROUND_MASK);
+	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 
 	if (err)
 		return 4;
@@ -878,12 +876,14 @@ static int dt282x_ao_cmdtest(struct comedi_device *dev,
 }
 
 static int dt282x_ao_inttrig(struct comedi_device *dev,
-			     struct comedi_subdevice *s, unsigned int x)
+			     struct comedi_subdevice *s,
+			     unsigned int trig_num)
 {
 	struct dt282x_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	int size;
 
-	if (x != 0)
+	if (trig_num != cmd->start_src)
 		return -EINVAL;
 
 	size = cfc_read_array_from_buffer(s, devpriv->dma[0].buf,
diff --git a/drivers/staging/comedi/drivers/dt3000.c b/drivers/staging/comedi/drivers/dt3000.c
index 436e451cadf..4ab4de00592 100644
--- a/drivers/staging/comedi/drivers/dt3000.c
+++ b/drivers/staging/comedi/drivers/dt3000.c
@@ -322,7 +322,7 @@ static void dt3k_ai_empty_fifo(struct comedi_device *dev,
 
 	for (i = 0; i < count; i++) {
 		data = readw(devpriv->io_addr + DPR_ADC_buffer + rear);
-		comedi_buf_put(s->async, data);
+		comedi_buf_put(s, data);
 		rear++;
 		if (rear >= AI_FIFO_DEPTH)
 			rear = 0;
@@ -416,7 +416,7 @@ static int dt3k_ai_cmdtest(struct comedi_device *dev,
 {
 	const struct dt3k_boardtype *this_board = comedi_board(dev);
 	int err = 0;
-	int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -466,25 +466,20 @@ static int dt3k_ai_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		tmp = cmd->scan_begin_arg;
-		dt3k_ns_to_timer(100, &cmd->scan_begin_arg,
-				 cmd->flags & TRIG_ROUND_MASK);
-		if (tmp != cmd->scan_begin_arg)
-			err++;
+		arg = cmd->scan_begin_arg;
+		dt3k_ns_to_timer(100, &arg, cmd->flags & TRIG_ROUND_MASK);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		tmp = cmd->convert_arg;
-		dt3k_ns_to_timer(50, &cmd->convert_arg,
-				 cmd->flags & TRIG_ROUND_MASK);
-		if (tmp != cmd->convert_arg)
-			err++;
-		if (cmd->scan_begin_src == TRIG_TIMER &&
-		    cmd->scan_begin_arg <
-		    cmd->convert_arg * cmd->scan_end_arg) {
-			cmd->scan_begin_arg =
-			    cmd->convert_arg * cmd->scan_end_arg;
-			err++;
+		arg = cmd->convert_arg;
+		dt3k_ns_to_timer(50, &arg, cmd->flags & TRIG_ROUND_MASK);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
+
+		if (cmd->scan_begin_src == TRIG_TIMER) {
+			arg = cmd->convert_arg * cmd->scan_end_arg;
+			err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
+							 arg);
 		}
 	}
 
diff --git a/drivers/staging/comedi/drivers/gsc_hpdi.c b/drivers/staging/comedi/drivers/gsc_hpdi.c
index 08d7655e24e..22333c1ad88 100644
--- a/drivers/staging/comedi/drivers/gsc_hpdi.c
+++ b/drivers/staging/comedi/drivers/gsc_hpdi.c
@@ -360,12 +360,30 @@ static int gsc_hpdi_cmd(struct comedi_device *dev,
 	return 0;
 }
 
+static int gsc_hpdi_check_chanlist(struct comedi_device *dev,
+				   struct comedi_subdevice *s,
+				   struct comedi_cmd *cmd)
+{
+	int i;
+
+	for (i = 0; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+
+		if (chan != i) {
+			dev_dbg(dev->class_dev,
+				"chanlist must be ch 0 to 31 in order\n");
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
 static int gsc_hpdi_cmd_test(struct comedi_device *dev,
 			     struct comedi_subdevice *s,
 			     struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int i;
 
 	if (s->io_bits)
 		return -EINVAL;
@@ -392,6 +410,8 @@ static int gsc_hpdi_cmd_test(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
+	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+
 	if (!cmd->chanlist_len || !cmd->chanlist) {
 		cmd->chanlist_len = 32;
 		err |= -EINVAL;
@@ -411,17 +431,9 @@ static int gsc_hpdi_cmd_test(struct comedi_device *dev,
 	if (err)
 		return 4;
 
-	/* step 5: complain about special chanlist considerations */
-
-	for (i = 0; i < cmd->chanlist_len; i++) {
-		if (CR_CHAN(cmd->chanlist[i]) != i) {
-			/*  XXX could support 8 or 16 channels */
-			dev_err(dev->class_dev,
-				"chanlist must be ch 0 to 31 in order");
-			err |= -EINVAL;
-			break;
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= gsc_hpdi_check_chanlist(dev, s, cmd);
 
 	if (err)
 		return 5;
diff --git a/drivers/staging/comedi/drivers/ii_pci20kc.c b/drivers/staging/comedi/drivers/ii_pci20kc.c
index 3558ab3b6e1..2516ce83483 100644
--- a/drivers/staging/comedi/drivers/ii_pci20kc.c
+++ b/drivers/staging/comedi/drivers/ii_pci20kc.c
@@ -461,6 +461,7 @@ static int ii20k_attach(struct comedi_device *dev,
 	id = readb(devpriv->ioaddr + II20K_ID_REG);
 	switch (id & II20K_ID_MASK) {
 	case II20K_ID_PCI20001C_1A:
+		has_dio = false;
 		break;
 	case II20K_ID_PCI20001C_2A:
 		has_dio = true;
diff --git a/drivers/staging/comedi/drivers/me4000.c b/drivers/staging/comedi/drivers/me4000.c
index f02b31b317e..25ce2f78db8 100644
--- a/drivers/staging/comedi/drivers/me4000.c
+++ b/drivers/staging/comedi/drivers/me4000.c
@@ -598,67 +598,35 @@ static int me4000_ai_cancel(struct comedi_device *dev,
 	return 0;
 }
 
-static int ai_check_chanlist(struct comedi_device *dev,
-			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
+static int me4000_ai_check_chanlist(struct comedi_device *dev,
+				    struct comedi_subdevice *s,
+				    struct comedi_cmd *cmd)
 {
-	const struct me4000_board *thisboard = comedi_board(dev);
-	int aref;
+	const struct me4000_board *board = comedi_board(dev);
+	unsigned int max_diff_chan = board->ai_diff_nchan;
+	unsigned int aref0 = CR_AREF(cmd->chanlist[0]);
 	int i;
 
-	/* Check whether a channel list is available */
-	if (!cmd->chanlist_len) {
-		dev_err(dev->class_dev, "No channel list available\n");
-		return -EINVAL;
-	}
-
-	/* Check the channel list size */
-	if (cmd->chanlist_len > ME4000_AI_CHANNEL_LIST_COUNT) {
-		dev_err(dev->class_dev, "Channel list is to large\n");
-		return -EINVAL;
-	}
-
-	/* Check the pointer */
-	if (!cmd->chanlist) {
-		dev_err(dev->class_dev, "NULL pointer to channel list\n");
-		return -EFAULT;
-	}
-
-	/* Check whether aref is equal for all entries */
-	aref = CR_AREF(cmd->chanlist[0]);
 	for (i = 0; i < cmd->chanlist_len; i++) {
-		if (CR_AREF(cmd->chanlist[i]) != aref) {
-			dev_err(dev->class_dev,
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+		unsigned int range = CR_RANGE(cmd->chanlist[i]);
+		unsigned int aref = CR_AREF(cmd->chanlist[i]);
+
+		if (aref != aref0) {
+			dev_dbg(dev->class_dev,
 				"Mode is not equal for all entries\n");
 			return -EINVAL;
 		}
-	}
 
-	/* Check whether channels are available for this ending */
-	if (aref == SDF_DIFF) {
-		for (i = 0; i < cmd->chanlist_len; i++) {
-			if (CR_CHAN(cmd->chanlist[i]) >=
-			    thisboard->ai_diff_nchan) {
-				dev_err(dev->class_dev,
-					"Channel number to high\n");
-				return -EINVAL;
-			}
-		}
-	} else {
-		for (i = 0; i < cmd->chanlist_len; i++) {
-			if (CR_CHAN(cmd->chanlist[i]) >= thisboard->ai_nchan) {
-				dev_err(dev->class_dev,
+		if (aref == SDF_DIFF) {
+			if (chan >= max_diff_chan) {
+				dev_dbg(dev->class_dev,
 					"Channel number to high\n");
 				return -EINVAL;
 			}
-		}
-	}
 
-	/* Check if bipolar is set for all entries when in differential mode */
-	if (aref == SDF_DIFF) {
-		for (i = 0; i < cmd->chanlist_len; i++) {
-			if (CR_RANGE(cmd->chanlist[i]) != 1 &&
-			    CR_RANGE(cmd->chanlist[i]) != 2) {
-				dev_err(dev->class_dev,
+			if (!comedi_range_is_bipolar(s, range)) {
+				dev_dbg(dev->class_dev,
 				       "Bipolar is not selected in differential mode\n");
 				return -EINVAL;
 			}
@@ -934,22 +902,13 @@ static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 		err |= -EINVAL;
 	}
 
-	if (cmd->stop_src == TRIG_NONE && cmd->scan_end_src == TRIG_NONE) {
-	} else if (cmd->stop_src == TRIG_COUNT &&
-		   cmd->scan_end_src == TRIG_NONE) {
-	} else if (cmd->stop_src == TRIG_NONE &&
-		   cmd->scan_end_src == TRIG_COUNT) {
-	} else if (cmd->stop_src == TRIG_COUNT &&
-		   cmd->scan_end_src == TRIG_COUNT) {
-	} else {
-		err |= -EINVAL;
-	}
-
 	if (err)
 		return 2;
 
 	/* Step 3: check if arguments are trivially valid */
 
+	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+
 	if (cmd->chanlist_len < 1) {
 		cmd->chanlist_len = 1;
 		err |= -EINVAL;
@@ -1091,10 +1050,11 @@ static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 	if (err)
 		return 4;
 
-	/*
-	 * Stage 5. Check the channel list.
-	 */
-	if (ai_check_chanlist(dev, s, cmd))
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= me4000_ai_check_chanlist(dev, s, cmd);
+
+	if (err)
 		return 5;
 
 	return 0;
@@ -1164,7 +1124,7 @@ static irqreturn_t me4000_ai_isr(int irq, void *dev_id)
 			lval = inl(dev->iobase + ME4000_AI_DATA_REG) & 0xFFFF;
 			lval ^= 0x8000;
 
-			if (!comedi_buf_put(s->async, lval)) {
+			if (!comedi_buf_put(s, lval)) {
 				/*
 				 * Buffer overflow, so stop conversion
 				 * and disable all interrupts
@@ -1209,7 +1169,7 @@ static irqreturn_t me4000_ai_isr(int irq, void *dev_id)
 			lval = inl(dev->iobase + ME4000_AI_DATA_REG) & 0xFFFF;
 			lval ^= 0x8000;
 
-			if (!comedi_buf_put(s->async, lval)) {
+			if (!comedi_buf_put(s, lval)) {
 				dev_err(dev->class_dev, "Buffer overflow\n");
 				s->async->events |= COMEDI_CB_OVERFLOW;
 				break;
@@ -1392,6 +1352,7 @@ static int me4000_cnt_insn_config(struct comedi_device *dev,
 				  unsigned int *data)
 {
 	struct me4000_info *info = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
 	int err;
 
 	switch (data[0]) {
@@ -1399,16 +1360,17 @@ static int me4000_cnt_insn_config(struct comedi_device *dev,
 		if (insn->n != 1)
 			return -EINVAL;
 
-		err = i8254_load(info->timer_regbase, 0, insn->chanspec, 0,
-				I8254_MODE0 | I8254_BINARY);
+		err = i8254_set_mode(info->timer_regbase, 0, chan,
+				     I8254_MODE0 | I8254_BINARY);
 		if (err)
 			return err;
+		i8254_write(info->timer_regbase, 0, chan, 0);
 		break;
 	case GPCT_SET_OPERATION:
 		if (insn->n != 2)
 			return -EINVAL;
 
-		err = i8254_set_mode(info->timer_regbase, 0, insn->chanspec,
+		err = i8254_set_mode(info->timer_regbase, 0, chan,
 				(data[1] << 1) | I8254_BINARY);
 		if (err)
 			return err;
diff --git a/drivers/staging/comedi/drivers/mite.c b/drivers/staging/comedi/drivers/mite.c
index 1a572c83f99..19c029acbc9 100644
--- a/drivers/staging/comedi/drivers/mite.c
+++ b/drivers/staging/comedi/drivers/mite.c
@@ -307,8 +307,9 @@ EXPORT_SYMBOL_GPL(mite_dma_arm);
 /**************************************/
 
 int mite_buf_change(struct mite_dma_descriptor_ring *ring,
-		    struct comedi_async *async)
+		    struct comedi_subdevice *s)
 {
+	struct comedi_async *async = s->async;
 	unsigned int n_links;
 	int i;
 
@@ -333,7 +334,7 @@ int mite_buf_change(struct mite_dma_descriptor_ring *ring,
 			       n_links * sizeof(struct mite_dma_descriptor),
 			       &ring->descriptors_dma_addr, GFP_KERNEL);
 	if (!ring->descriptors) {
-		dev_err(async->subdevice->device->class_dev,
+		dev_err(s->device->class_dev,
 			"mite: ring buffer allocation failed\n");
 		return -ENOMEM;
 	}
@@ -525,15 +526,15 @@ void mite_dma_disarm(struct mite_channel *mite_chan)
 EXPORT_SYMBOL_GPL(mite_dma_disarm);
 
 int mite_sync_input_dma(struct mite_channel *mite_chan,
-			struct comedi_async *async)
+			struct comedi_subdevice *s)
 {
-	struct comedi_subdevice *s = async->subdevice;
+	struct comedi_async *async = s->async;
 	int count;
 	unsigned int nbytes, old_alloc_count;
 
 	old_alloc_count = async->buf_write_alloc_count;
 	/* write alloc as much as we can */
-	comedi_buf_write_alloc(async, async->prealloc_bufsz);
+	comedi_buf_write_alloc(s, async->prealloc_bufsz);
 
 	nbytes = mite_bytes_written_to_memory_lb(mite_chan);
 	if ((int)(mite_bytes_written_to_memory_ub(mite_chan) -
@@ -550,7 +551,7 @@ int mite_sync_input_dma(struct mite_channel *mite_chan,
 	if (count <= 0)
 		return 0;
 
-	comedi_buf_write_free(async, count);
+	comedi_buf_write_free(s, count);
 	cfc_inc_scan_progress(s, count);
 	async->events |= COMEDI_CB_BLOCK;
 	return 0;
@@ -558,28 +559,25 @@ int mite_sync_input_dma(struct mite_channel *mite_chan,
 EXPORT_SYMBOL_GPL(mite_sync_input_dma);
 
 int mite_sync_output_dma(struct mite_channel *mite_chan,
-			 struct comedi_async *async)
+			 struct comedi_subdevice *s)
 {
-	int count;
+	struct comedi_async *async = s->async;
+	struct comedi_cmd *cmd = &async->cmd;
+	u32 stop_count = cmd->stop_arg * cfc_bytes_per_scan(s);
+	unsigned int old_alloc_count = async->buf_read_alloc_count;
 	u32 nbytes_ub, nbytes_lb;
-	unsigned int old_alloc_count;
-	u32 stop_count =
-	    async->cmd.stop_arg * cfc_bytes_per_scan(async->subdevice);
+	int count;
 
-	old_alloc_count = async->buf_read_alloc_count;
 	/*  read alloc as much as we can */
-	comedi_buf_read_alloc(async, async->prealloc_bufsz);
+	comedi_buf_read_alloc(s, async->prealloc_bufsz);
 	nbytes_lb = mite_bytes_read_from_memory_lb(mite_chan);
-	if (async->cmd.stop_src == TRIG_COUNT &&
-	    (int)(nbytes_lb - stop_count) > 0)
+	if (cmd->stop_src == TRIG_COUNT && (int)(nbytes_lb - stop_count) > 0)
 		nbytes_lb = stop_count;
 	nbytes_ub = mite_bytes_read_from_memory_ub(mite_chan);
-	if (async->cmd.stop_src == TRIG_COUNT &&
-	    (int)(nbytes_ub - stop_count) > 0)
+	if (cmd->stop_src == TRIG_COUNT && (int)(nbytes_ub - stop_count) > 0)
 		nbytes_ub = stop_count;
 	if ((int)(nbytes_ub - old_alloc_count) > 0) {
-		dev_warn(async->subdevice->device->class_dev,
-			 "mite: DMA underrun\n");
+		dev_warn(s->device->class_dev, "mite: DMA underrun\n");
 		async->events |= COMEDI_CB_OVERFLOW;
 		return -1;
 	}
@@ -588,7 +586,7 @@ int mite_sync_output_dma(struct mite_channel *mite_chan,
 		return 0;
 
 	if (count) {
-		comedi_buf_read_free(async, count);
+		comedi_buf_read_free(s, count);
 		async->events |= COMEDI_CB_BLOCK;
 	}
 	return 0;
diff --git a/drivers/staging/comedi/drivers/mite.h b/drivers/staging/comedi/drivers/mite.h
index 78f23574799..e6e58e989b7 100644
--- a/drivers/staging/comedi/drivers/mite.h
+++ b/drivers/staging/comedi/drivers/mite.h
@@ -106,9 +106,9 @@ unsigned mite_dma_tcr(struct mite_channel *mite_chan);
 void mite_dma_arm(struct mite_channel *mite_chan);
 void mite_dma_disarm(struct mite_channel *mite_chan);
 int mite_sync_input_dma(struct mite_channel *mite_chan,
-			struct comedi_async *async);
+			struct comedi_subdevice *s);
 int mite_sync_output_dma(struct mite_channel *mite_chan,
-			 struct comedi_async *async);
+			 struct comedi_subdevice *s);
 u32 mite_bytes_written_to_memory_lb(struct mite_channel *mite_chan);
 u32 mite_bytes_written_to_memory_ub(struct mite_channel *mite_chan);
 u32 mite_bytes_read_from_memory_lb(struct mite_channel *mite_chan);
@@ -120,7 +120,7 @@ int mite_done(struct mite_channel *mite_chan);
 void mite_prep_dma(struct mite_channel *mite_chan,
 		   unsigned int num_device_bits, unsigned int num_memory_bits);
 int mite_buf_change(struct mite_dma_descriptor_ring *ring,
-		    struct comedi_async *async);
+		    struct comedi_subdevice *s);
 
 static inline int CHAN_OFFSET(int channel)
 {
diff --git a/drivers/staging/comedi/drivers/ni_6527.c b/drivers/staging/comedi/drivers/ni_6527.c
index 860fc81fb11..c8b1fa793a3 100644
--- a/drivers/staging/comedi/drivers/ni_6527.c
+++ b/drivers/staging/comedi/drivers/ni_6527.c
@@ -215,7 +215,7 @@ static irqreturn_t ni6527_interrupt(int irq, void *d)
 		return IRQ_NONE;
 
 	if (status & NI6527_STATUS_EDGE) {
-		comedi_buf_put(s->async, 0);
+		comedi_buf_put(s, 0);
 		s->async->events |= COMEDI_CB_EOS;
 		comedi_event(dev, s);
 	}
@@ -253,7 +253,7 @@ static int ni6527_intr_cmdtest(struct comedi_device *dev,
 	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, 1);
+	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 	err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
@@ -421,6 +421,7 @@ static int ni6527_auto_attach(struct comedi_device *dev,
 		s->range_table	= &range_digital;
 		s->insn_config	= ni6527_intr_insn_config;
 		s->insn_bits	= ni6527_intr_insn_bits;
+		s->len_chanlist	= 1;
 		s->do_cmdtest	= ni6527_intr_cmdtest;
 		s->do_cmd	= ni6527_intr_cmd;
 		s->cancel	= ni6527_intr_cancel;
diff --git a/drivers/staging/comedi/drivers/ni_65xx.c b/drivers/staging/comedi/drivers/ni_65xx.c
index 6e42001f686..9a139d6b8ef 100644
--- a/drivers/staging/comedi/drivers/ni_65xx.c
+++ b/drivers/staging/comedi/drivers/ni_65xx.c
@@ -439,7 +439,7 @@ static irqreturn_t ni_65xx_interrupt(int irq, void *d)
 	writeb(ClrEdge | ClrOverflow,
 	       devpriv->mite->daq_io_addr + Clear_Register);
 
-	comedi_buf_put(s->async, 0);
+	comedi_buf_put(s, 0);
 	s->async->events |= COMEDI_CB_EOS;
 	comedi_event(dev, s);
 	return IRQ_HANDLED;
@@ -473,7 +473,7 @@ static int ni_65xx_intr_cmdtest(struct comedi_device *dev,
 	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, 1);
+	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 	err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
@@ -491,7 +491,6 @@ static int ni_65xx_intr_cmd(struct comedi_device *dev,
 			    struct comedi_subdevice *s)
 {
 	struct ni_65xx_private *devpriv = dev->private;
-	/* struct comedi_cmd *cmd = &s->async->cmd; */
 
 	writeb(ClrEdge | ClrOverflow,
 	       devpriv->mite->daq_io_addr + Clear_Register);
@@ -671,6 +670,7 @@ static int ni_65xx_auto_attach(struct comedi_device *dev,
 	s->n_chan = 1;
 	s->range_table = &range_unknown;
 	s->maxdata = 1;
+	s->len_chanlist	= 1;
 	s->do_cmdtest = ni_65xx_intr_cmdtest;
 	s->do_cmd = ni_65xx_intr_cmd;
 	s->cancel = ni_65xx_intr_cancel;
diff --git a/drivers/staging/comedi/drivers/ni_660x.c b/drivers/staging/comedi/drivers/ni_660x.c
index 0d4b9019f76..634cde83a02 100644
--- a/drivers/staging/comedi/drivers/ni_660x.c
+++ b/drivers/staging/comedi/drivers/ni_660x.c
@@ -823,9 +823,9 @@ static int ni_660x_input_poll(struct comedi_device *dev,
 
 	/* lock to avoid race with comedi_poll */
 	spin_lock_irqsave(&devpriv->interrupt_lock, flags);
-	mite_sync_input_dma(counter->mite_chan, s->async);
+	mite_sync_input_dma(counter->mite_chan, s);
 	spin_unlock_irqrestore(&devpriv->interrupt_lock, flags);
-	return comedi_buf_read_n_available(s->async);
+	return comedi_buf_read_n_available(s);
 }
 
 static int ni_660x_buf_change(struct comedi_device *dev,
@@ -836,7 +836,7 @@ static int ni_660x_buf_change(struct comedi_device *dev,
 	struct ni_gpct *counter = s->private;
 	int ret;
 
-	ret = mite_buf_change(mite_ring(devpriv, counter), s->async);
+	ret = mite_buf_change(mite_ring(devpriv, counter), s);
 	if (ret < 0)
 		return ret;
 
diff --git a/drivers/staging/comedi/drivers/ni_at_a2150.c b/drivers/staging/comedi/drivers/ni_at_a2150.c
index 4e39b1f63d8..5bd19494dbf 100644
--- a/drivers/staging/comedi/drivers/ni_at_a2150.c
+++ b/drivers/staging/comedi/drivers/ni_at_a2150.c
@@ -287,14 +287,54 @@ static int a2150_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 	return 0;
 }
 
+static int a2150_ai_check_chanlist(struct comedi_device *dev,
+				   struct comedi_subdevice *s,
+				   struct comedi_cmd *cmd)
+{
+	unsigned int chan0 = CR_CHAN(cmd->chanlist[0]);
+	unsigned int aref0 = CR_AREF(cmd->chanlist[0]);
+	int i;
+
+	if (cmd->chanlist_len == 2 && (chan0 == 1 || chan0 == 3)) {
+		dev_dbg(dev->class_dev,
+			"length 2 chanlist must be channels 0,1 or channels 2,3\n");
+		return -EINVAL;
+	}
+
+	if (cmd->chanlist_len == 3) {
+		dev_dbg(dev->class_dev,
+			"chanlist must have 1,2 or 4 channels\n");
+		return -EINVAL;
+	}
+
+	for (i = 1; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+		unsigned int aref = CR_AREF(cmd->chanlist[i]);
+
+		if (chan != (chan0 + i)) {
+			dev_dbg(dev->class_dev,
+				"entries in chanlist must be consecutive channels, counting upwards\n");
+			return -EINVAL;
+		}
+
+		if (chan == 2)
+			aref0 = aref;
+		if (aref != aref0) {
+			dev_dbg(dev->class_dev,
+				"channels 0/1 and 2/3 must have the same analog reference\n");
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
 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;
-	int i;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -339,42 +379,17 @@ static int a2150_ai_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		tmp = cmd->scan_begin_arg;
-		a2150_get_timing(dev, &cmd->scan_begin_arg, cmd->flags);
-		if (tmp != cmd->scan_begin_arg)
-			err++;
+		arg = cmd->scan_begin_arg;
+		a2150_get_timing(dev, &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 
 	if (err)
 		return 4;
 
-	/*  check channel/gain list against card's limitations */
-	if (cmd->chanlist) {
-		startChan = CR_CHAN(cmd->chanlist[0]);
-		for (i = 1; i < cmd->chanlist_len; i++) {
-			if (CR_CHAN(cmd->chanlist[i]) != (startChan + i)) {
-				comedi_error(dev,
-					     "entries in chanlist must be consecutive channels, counting upwards\n");
-				err++;
-			}
-		}
-		if (cmd->chanlist_len == 2 && CR_CHAN(cmd->chanlist[0]) == 1) {
-			comedi_error(dev,
-				     "length 2 chanlist must be channels 0,1 or channels 2,3");
-			err++;
-		}
-		if (cmd->chanlist_len == 3) {
-			comedi_error(dev,
-				     "chanlist must have 1,2 or 4 channels");
-			err++;
-		}
-		if (CR_AREF(cmd->chanlist[0]) != CR_AREF(cmd->chanlist[1]) ||
-		    CR_AREF(cmd->chanlist[2]) != CR_AREF(cmd->chanlist[3])) {
-			comedi_error(dev,
-				     "channels 0/1 and 2/3 must have the same analog reference");
-			err++;
-		}
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= a2150_ai_check_chanlist(dev, s, cmd);
 
 	if (err)
 		return 5;
@@ -387,6 +402,7 @@ static int a2150_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	struct a2150_private *devpriv = dev->private;
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
+	unsigned long timer_base = dev->iobase + I8253_BASE_REG;
 	unsigned long lock_flags;
 	unsigned int old_config_bits = devpriv->config_bits;
 	unsigned int trigger_bits;
@@ -454,7 +470,8 @@ static int a2150_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	outw(devpriv->irq_dma_bits, dev->iobase + IRQ_DMA_CNTRL_REG);
 
 	/*  may need to wait 72 sampling periods if timing was changed */
-	i8254_load(dev->iobase + I8253_BASE_REG, 0, 2, 72, 0);
+	i8254_set_mode(timer_base, 0, 2, I8254_MODE0 | I8254_BINARY);
+	i8254_write(timer_base, 0, 2, 72);
 
 	/*  setup start triggering */
 	trigger_bits = 0;
@@ -679,7 +696,7 @@ 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);
+	const struct a2150_board *thisboard;
 	struct a2150_private *devpriv;
 	struct comedi_subdevice *s;
 	unsigned int irq = it->options[1];
diff --git a/drivers/staging/comedi/drivers/ni_at_ao.c b/drivers/staging/comedi/drivers/ni_at_ao.c
index 10e3e9475ee..c93b47bcca5 100644
--- a/drivers/staging/comedi/drivers/ni_at_ao.c
+++ b/drivers/staging/comedi/drivers/ni_at_ao.c
@@ -310,6 +310,7 @@ static int atao_calib_insn_read(struct comedi_device *dev,
 static void atao_reset(struct comedi_device *dev)
 {
 	struct atao_private *devpriv = dev->private;
+	unsigned long timer_base = dev->iobase + ATAO_82C53_BASE;
 
 	/* This is the reset sequence described in the manual */
 
@@ -317,10 +318,9 @@ static void atao_reset(struct comedi_device *dev)
 	outw(devpriv->cfg1, dev->iobase + ATAO_CFG1_REG);
 
 	/* Put outputs of counter 1 and counter 2 in a high state */
-	i8254_load(dev->iobase + ATAO_82C53_BASE, 0,
-		   0, 0x0003, I8254_MODE4 | I8254_BINARY);
-	i8254_set_mode(dev->iobase + ATAO_82C53_BASE, 0,
-		   1, I8254_MODE4 | I8254_BINARY);
+	i8254_set_mode(timer_base, 0, 0, I8254_MODE4 | I8254_BINARY);
+	i8254_set_mode(timer_base, 0, 1, I8254_MODE4 | I8254_BINARY);
+	i8254_write(timer_base, 0, 0, 0x0003);
 
 	outw(ATAO_CFG2_CALLD_NOP, dev->iobase + ATAO_CFG2_REG);
 
diff --git a/drivers/staging/comedi/drivers/ni_atmio16d.c b/drivers/staging/comedi/drivers/ni_atmio16d.c
index 4262385e4f4..6ad27f50c6e 100644
--- a/drivers/staging/comedi/drivers/ni_atmio16d.c
+++ b/drivers/staging/comedi/drivers/ni_atmio16d.c
@@ -221,7 +221,7 @@ static irqreturn_t atmio16d_interrupt(int irq, void *d)
 	struct comedi_device *dev = d;
 	struct comedi_subdevice *s = dev->read_subdev;
 
-	comedi_buf_put(s->async, inw(dev->iobase + AD_FIFO_REG));
+	comedi_buf_put(s, inw(dev->iobase + AD_FIFO_REG));
 
 	comedi_event(dev, s);
 	return IRQ_HANDLED;
diff --git a/drivers/staging/comedi/drivers/ni_labpc.c b/drivers/staging/comedi/drivers/ni_labpc.c
index f4216e825f0..3e3f940fa57 100644
--- a/drivers/staging/comedi/drivers/ni_labpc.c
+++ b/drivers/staging/comedi/drivers/ni_labpc.c
@@ -171,32 +171,39 @@ static const struct labpc_boardinfo labpc_boards[] = {
 };
 #endif
 
-static int labpc_counter_load(struct comedi_device *dev,
-			      unsigned long base_address,
-			      unsigned int counter_number,
-			      unsigned int count, unsigned int mode)
+static void labpc_counter_load(struct comedi_device *dev,
+			       unsigned long base_address,
+			       unsigned int counter_number,
+			       unsigned int count,
+			       unsigned int mode)
 {
 	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);
+	if (board->has_mmio) {
+		void __iomem *mmio_base = (void __iomem *)base_address;
+
+		i8254_mm_set_mode(mmio_base, 0, counter_number, mode);
+		i8254_mm_write(mmio_base, 0, counter_number, count);
+	} else {
+		i8254_set_mode(base_address, 0, counter_number, mode);
+		i8254_write(base_address, 0, counter_number, count);
+	}
 }
 
-static int labpc_counter_set_mode(struct comedi_device *dev,
-				  unsigned long base_address,
-				  unsigned int counter_number,
-				  unsigned int mode)
+static void 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);
+	if (board->has_mmio) {
+		void __iomem *mmio_base = (void __iomem *)base_address;
+
+		i8254_mm_set_mode(mmio_base, 0, counter_number, mode);
+	} else {
+		i8254_set_mode(base_address, 0, counter_number, mode);
+	}
 }
 
 static int labpc_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
@@ -349,10 +356,8 @@ static int labpc_ai_insn_read(struct comedi_device *dev,
 	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;
+	labpc_counter_set_mode(dev, dev->iobase + COUNTER_A_BASE_REG,
+			       0, I8254_MODE2);
 
 	labpc_clear_adc_fifo(dev);
 
@@ -546,72 +551,60 @@ static enum scan_mode labpc_ai_scan_mode(const struct comedi_cmd *cmd)
 	return 0;
 }
 
-static int labpc_ai_chanlist_invalid(const struct comedi_device *dev,
-				     const struct comedi_cmd *cmd,
-				     enum scan_mode mode)
+static int labpc_ai_check_chanlist(struct comedi_device *dev,
+				   struct comedi_subdevice *s,
+				   struct comedi_cmd *cmd)
 {
-	int channel, range, aref, i;
-
-	if (cmd->chanlist == NULL)
-		return 0;
+	enum scan_mode mode = labpc_ai_scan_mode(cmd);
+	unsigned int chan0 = CR_CHAN(cmd->chanlist[0]);
+	unsigned int range0 = CR_RANGE(cmd->chanlist[0]);
+	unsigned int aref0 = CR_AREF(cmd->chanlist[0]);
+	int i;
 
 	if (mode == MODE_SINGLE_CHAN)
 		return 0;
 
-	if (mode == MODE_SINGLE_CHAN_INTERVAL) {
-		if (cmd->chanlist_len > 0xff) {
-			comedi_error(dev,
-				     "ni_labpc: chanlist too long for single channel interval mode\n");
-			return 1;
-		}
-	}
-
-	channel = CR_CHAN(cmd->chanlist[0]);
-	range = CR_RANGE(cmd->chanlist[0]);
-	aref = CR_AREF(cmd->chanlist[0]);
-
 	for (i = 0; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+		unsigned int range = CR_RANGE(cmd->chanlist[i]);
+		unsigned int aref = CR_AREF(cmd->chanlist[i]);
 
 		switch (mode) {
+		case MODE_SINGLE_CHAN:
+			break;
 		case MODE_SINGLE_CHAN_INTERVAL:
-			if (CR_CHAN(cmd->chanlist[i]) != channel) {
-				comedi_error(dev,
-					     "channel scanning order specified in chanlist is not supported by hardware.\n");
-				return 1;
+			if (chan != chan0) {
+				dev_dbg(dev->class_dev,
+					"channel scanning order specified in chanlist is not supported by hardware\n");
+				return -EINVAL;
 			}
 			break;
 		case MODE_MULT_CHAN_UP:
-			if (CR_CHAN(cmd->chanlist[i]) != i) {
-				comedi_error(dev,
-					     "channel scanning order specified in chanlist is not supported by hardware.\n");
-				return 1;
+			if (chan != i) {
+				dev_dbg(dev->class_dev,
+					"channel scanning order specified in chanlist is not supported by hardware\n");
+				return -EINVAL;
 			}
 			break;
 		case MODE_MULT_CHAN_DOWN:
-			if (CR_CHAN(cmd->chanlist[i]) !=
-			    cmd->chanlist_len - i - 1) {
-				comedi_error(dev,
-					     "channel scanning order specified in chanlist is not supported by hardware.\n");
-				return 1;
+			if (chan != (cmd->chanlist_len - i - 1)) {
+				dev_dbg(dev->class_dev,
+					"channel scanning order specified in chanlist is not supported by hardware\n");
+				return -EINVAL;
 			}
 			break;
-		default:
-			dev_err(dev->class_dev,
-				"ni_labpc: bug! in chanlist check\n");
-			return 1;
-			break;
 		}
 
-		if (CR_RANGE(cmd->chanlist[i]) != range) {
-			comedi_error(dev,
-				     "entries in chanlist must all have the same range\n");
-			return 1;
+		if (range != range0) {
+			dev_dbg(dev->class_dev,
+				"entries in chanlist must all have the same range\n");
+			return -EINVAL;
 		}
 
-		if (CR_AREF(cmd->chanlist[i]) != aref) {
-			comedi_error(dev,
-				     "entries in chanlist must all have the same reference\n");
-			return 1;
+		if (aref != aref0) {
+			dev_dbg(dev->class_dev,
+				"entries in chanlist must all have the same reference\n");
+			return -EINVAL;
 		}
 	}
 
@@ -661,8 +654,14 @@ static int labpc_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	if (cmd->start_arg == TRIG_NOW)
+	switch (cmd->start_src) {
+	case TRIG_NOW:
 		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+		break;
+	case TRIG_EXT:
+		/* start_arg value is ignored */
+		break;
+	}
 
 	if (!cmd->chanlist_len)
 		err |= -EINVAL;
@@ -711,7 +710,11 @@ static int labpc_ai_cmdtest(struct comedi_device *dev,
 	if (err)
 		return 4;
 
-	if (labpc_ai_chanlist_invalid(dev, cmd, mode))
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= labpc_ai_check_chanlist(dev, s, cmd);
+
+	if (err)
 		return 5;
 
 	return 0;
@@ -732,7 +735,6 @@ static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	unsigned int aref = CR_AREF(chanspec);
 	enum transfer_type xfer;
 	unsigned long flags;
-	int ret;
 
 	/* make sure board is disabled before setting up acquisition */
 	labpc_cancel(dev, s);
@@ -747,17 +749,12 @@ static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		 * load counter a1 with count of 3
 		 * (pc+ manual says this is minimum allowed) using mode 0
 		 */
-		ret = labpc_counter_load(dev, dev->iobase + COUNTER_A_BASE_REG,
-					 1, 3, I8254_MODE0);
+		labpc_counter_load(dev, dev->iobase + COUNTER_A_BASE_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;
+		labpc_counter_set_mode(dev, dev->iobase + COUNTER_A_BASE_REG,
+				       1, I8254_MODE0);
 	}
 
 	/* figure out what method we will use to transfer data */
@@ -802,38 +799,25 @@ static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		/*  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, I8254_MODE3);
-		if (ret < 0) {
-			comedi_error(dev, "error loading counter b0");
-			return -1;
-		}
+		labpc_counter_load(dev, dev->iobase + COUNTER_B_BASE_REG,
+				   0, devpriv->divisor_b0, I8254_MODE3);
 	}
 	/*  set up conversion pacing */
 	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, I8254_MODE2);
+		labpc_counter_load(dev, dev->iobase + COUNTER_A_BASE_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;
+		labpc_counter_set_mode(dev, dev->iobase + COUNTER_A_BASE_REG,
+				       0, I8254_MODE2);
 	}
 
 	/*  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, I8254_MODE2);
-		if (ret < 0) {
-			comedi_error(dev, "error loading counter b1");
-			return -1;
-		}
+		labpc_counter_load(dev, dev->iobase + COUNTER_B_BASE_REG,
+				   1, devpriv->divisor_b1, I8254_MODE2);
 	}
 
 	labpc_clear_adc_fifo(dev);
@@ -890,8 +874,9 @@ static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 static int labpc_drain_fifo(struct comedi_device *dev)
 {
 	struct labpc_private *devpriv = dev->private;
-	unsigned short data;
 	struct comedi_async *async = dev->read_subdev->async;
+	struct comedi_cmd *cmd = &async->cmd;
+	unsigned short data;
 	const int timeout = 10000;
 	unsigned int i;
 
@@ -900,7 +885,7 @@ static int labpc_drain_fifo(struct comedi_device *dev)
 	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 (cmd->stop_src == TRIG_COUNT) {
 			if (devpriv->count == 0)
 				break;
 			devpriv->count--;
diff --git a/drivers/staging/comedi/drivers/ni_labpc_isadma.c b/drivers/staging/comedi/drivers/ni_labpc_isadma.c
index 2149596830a..d9f25fdbb72 100644
--- a/drivers/staging/comedi/drivers/ni_labpc_isadma.c
+++ b/drivers/staging/comedi/drivers/ni_labpc_isadma.c
@@ -87,6 +87,7 @@ void labpc_drain_dma(struct comedi_device *dev)
 	struct labpc_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->read_subdev;
 	struct comedi_async *async = s->async;
+	struct comedi_cmd *cmd = &async->cmd;
 	int status;
 	unsigned long flags;
 	unsigned int max_points, num_points, residue, leftover;
@@ -108,12 +109,12 @@ void labpc_drain_dma(struct comedi_device *dev)
 	 */
 	residue = get_dma_residue(devpriv->dma_chan) / sample_size;
 	num_points = max_points - residue;
-	if (devpriv->count < num_points && async->cmd.stop_src == TRIG_COUNT)
+	if (cmd->stop_src == TRIG_COUNT && devpriv->count < num_points)
 		num_points = devpriv->count;
 
 	/* figure out how many points will be stored next time */
 	leftover = 0;
-	if (async->cmd.stop_src != TRIG_COUNT) {
+	if (cmd->stop_src != TRIG_COUNT) {
 		leftover = devpriv->dma_transfer_size / sample_size;
 	} else if (devpriv->count > num_points) {
 		leftover = devpriv->count - num_points;
@@ -125,7 +126,7 @@ void labpc_drain_dma(struct comedi_device *dev)
 	for (i = 0; i < num_points; i++)
 		cfc_write_to_buffer(s, devpriv->dma_buffer[i]);
 
-	if (async->cmd.stop_src == TRIG_COUNT)
+	if (cmd->stop_src == TRIG_COUNT)
 		devpriv->count -= num_points;
 
 	/* set address and count for next transfer */
diff --git a/drivers/staging/comedi/drivers/ni_mio_common.c b/drivers/staging/comedi/drivers/ni_mio_common.c
index 8a0e3b7236a..7ffdcc07ef9 100644
--- a/drivers/staging/comedi/drivers/ni_mio_common.c
+++ b/drivers/staging/comedi/drivers/ni_mio_common.c
@@ -864,7 +864,7 @@ static void ni_sync_ai_dma(struct comedi_device *dev)
 
 	spin_lock_irqsave(&devpriv->mite_channel_lock, flags);
 	if (devpriv->ai_mite_chan)
-		mite_sync_input_dma(devpriv->ai_mite_chan, s->async);
+		mite_sync_input_dma(devpriv->ai_mite_chan, s);
 	spin_unlock_irqrestore(&devpriv->mite_channel_lock, flags);
 }
 
@@ -877,7 +877,7 @@ static void mite_handle_b_linkc(struct mite_struct *mite,
 
 	spin_lock_irqsave(&devpriv->mite_channel_lock, flags);
 	if (devpriv->ao_mite_chan)
-		mite_sync_output_dma(devpriv->ao_mite_chan, s->async);
+		mite_sync_output_dma(devpriv->ao_mite_chan, s);
 	spin_unlock_irqrestore(&devpriv->mite_channel_lock, flags);
 }
 
@@ -1149,7 +1149,7 @@ static void ni_ao_fifo_load(struct comedi_device *dev,
 
 	chan = async->cur_chan;
 	for (i = 0; i < n; i++) {
-		err &= comedi_buf_get(async, &d);
+		err &= comedi_buf_get(s, &d);
 		if (err == 0)
 			break;
 
@@ -1159,7 +1159,7 @@ static void ni_ao_fifo_load(struct comedi_device *dev,
 			packed_data = d & 0xffff;
 			/* 6711 only has 16 bit wide ao fifo */
 			if (board->reg_type != ni_reg_6711) {
-				err &= comedi_buf_get(async, &d);
+				err &= comedi_buf_get(s, &d);
 				if (err == 0)
 					break;
 				chan++;
@@ -1200,7 +1200,7 @@ static int ni_ao_fifo_half_empty(struct comedi_device *dev,
 	const struct ni_board_struct *board = comedi_board(dev);
 	int n;
 
-	n = comedi_buf_read_n_available(s->async);
+	n = comedi_buf_read_n_available(s);
 	if (n == 0) {
 		s->async->events |= COMEDI_CB_OVERFLOW;
 		return 0;
@@ -1230,7 +1230,7 @@ static int ni_ao_prep_fifo(struct comedi_device *dev,
 		ni_ao_win_outl(dev, 0x6, AO_FIFO_Offset_Load_611x);
 
 	/* load some data */
-	n = comedi_buf_read_n_available(s->async);
+	n = comedi_buf_read_n_available(s);
 	if (n == 0)
 		return 0;
 
@@ -1468,10 +1468,11 @@ static void ni_ai_munge(struct comedi_device *dev, struct comedi_subdevice *s,
 {
 	struct ni_private *devpriv = dev->private;
 	struct comedi_async *async = s->async;
-	unsigned int i;
+	struct comedi_cmd *cmd = &async->cmd;
 	unsigned int length = num_bytes / bytes_per_sample(s);
 	unsigned short *array = data;
 	unsigned int *larray = data;
+	unsigned int i;
 
 	for (i = 0; i < length; i++) {
 #ifdef PCIDMA
@@ -1485,7 +1486,7 @@ static void ni_ai_munge(struct comedi_device *dev, struct comedi_subdevice *s,
 		else
 			array[i] += devpriv->ai_offset[chan_index];
 		chan_index++;
-		chan_index %= async->cmd.chanlist_len;
+		chan_index %= cmd->chanlist_len;
 	}
 }
 
@@ -1505,7 +1506,7 @@ static int ni_ai_setup_MITE_dma(struct comedi_device *dev)
 /* printk("comedi_debug: using mite channel %i for ai.\n", devpriv->ai_mite_chan->channel); */
 
 	/* write alloc the entire buffer */
-	comedi_buf_write_alloc(s->async, s->async->prealloc_bufsz);
+	comedi_buf_write_alloc(s, s->async->prealloc_bufsz);
 
 	spin_lock_irqsave(&devpriv->mite_channel_lock, flags);
 	if (devpriv->ai_mite_chan == NULL) {
@@ -1545,7 +1546,7 @@ static int ni_ao_setup_MITE_dma(struct comedi_device *dev)
 		return retval;
 
 	/* read alloc the entire buffer */
-	comedi_buf_read_alloc(s->async, s->async->prealloc_bufsz);
+	comedi_buf_read_alloc(s, s->async->prealloc_bufsz);
 
 	spin_lock_irqsave(&devpriv->mite_channel_lock, flags);
 	if (devpriv->ao_mite_chan) {
@@ -2080,7 +2081,7 @@ static int ni_ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 	int err = 0;
-	int tmp;
+	unsigned int tmp;
 	unsigned int sources;
 
 	/* Step 1 : check if triggers are trivially valid */
@@ -2119,17 +2120,19 @@ static int ni_ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	if (cmd->start_src == TRIG_EXT) {
-		/* external trigger */
-		unsigned int tmp = CR_CHAN(cmd->start_arg);
+	switch (cmd->start_src) {
+	case TRIG_NOW:
+	case TRIG_INT:
+		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+		break;
+	case TRIG_EXT:
+		tmp = CR_CHAN(cmd->start_arg);
 
 		if (tmp > 16)
 			tmp = 16;
 		tmp |= (cmd->start_arg & (CR_INVERT | CR_EDGE));
 		err |= cfc_check_trigger_arg_is(&cmd->start_arg, tmp);
-	} else {
-		/* true for both TRIG_NOW and TRIG_INT */
-		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+		break;
 	}
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
@@ -2510,30 +2513,28 @@ static int ni_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	}
 #endif
 
-	switch (cmd->start_src) {
-	case TRIG_NOW:
+	if (cmd->start_src == TRIG_NOW) {
 		/* AI_START1_Pulse */
 		devpriv->stc_writew(dev, AI_START1_Pulse | devpriv->ai_cmd2,
 				    AI_Command_2_Register);
 		s->async->inttrig = NULL;
-		break;
-	case TRIG_EXT:
+	} else if (cmd->start_src == TRIG_EXT) {
 		s->async->inttrig = NULL;
-		break;
-	case TRIG_INT:
-		s->async->inttrig = &ni_ai_inttrig;
-		break;
+	} else {	/* TRIG_INT */
+		s->async->inttrig = ni_ai_inttrig;
 	}
 
 	return 0;
 }
 
-static int ni_ai_inttrig(struct comedi_device *dev, struct comedi_subdevice *s,
-			 unsigned int trignum)
+static int ni_ai_inttrig(struct comedi_device *dev,
+			 struct comedi_subdevice *s,
+			 unsigned int trig_num)
 {
 	struct ni_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 
-	if (trignum != 0)
+	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
 	devpriv->stc_writew(dev, AI_START1_Pulse | devpriv->ai_cmd2,
@@ -2710,22 +2711,22 @@ static void ni_ao_munge(struct comedi_device *dev, struct comedi_subdevice *s,
 {
 	const struct ni_board_struct *board = comedi_board(dev);
 	struct comedi_async *async = s->async;
-	unsigned int range;
-	unsigned int i;
-	unsigned int offset;
+	struct comedi_cmd *cmd = &async->cmd;
 	unsigned int length = num_bytes / sizeof(short);
+	unsigned int offset = 1 << (board->aobits - 1);
 	unsigned short *array = data;
+	unsigned int range;
+	unsigned int i;
 
-	offset = 1 << (board->aobits - 1);
 	for (i = 0; i < length; i++) {
-		range = CR_RANGE(async->cmd.chanlist[chan_index]);
+		range = CR_RANGE(cmd->chanlist[chan_index]);
 		if (board->ao_unipolar == 0 || (range & 1) == 0)
 			array[i] -= offset;
 #ifdef PCIDMA
 		array[i] = cpu_to_le16(array[i]);
 #endif
 		chan_index++;
-		chan_index %= async->cmd.chanlist_len;
+		chan_index %= cmd->chanlist_len;
 	}
 }
 
@@ -2946,17 +2947,19 @@ static int ni_ao_insn_config(struct comedi_device *dev,
 	return -EINVAL;
 }
 
-static int ni_ao_inttrig(struct comedi_device *dev, struct comedi_subdevice *s,
-			 unsigned int trignum)
+static int ni_ao_inttrig(struct comedi_device *dev,
+			 struct comedi_subdevice *s,
+			 unsigned int trig_num)
 {
 	const struct ni_board_struct *board __maybe_unused = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	int ret;
 	int interrupt_b_bits;
 	int i;
 	static const int timeout = 1000;
 
-	if (trignum != 0)
+	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
 	/* Null trig at beginning prevent ao start trigger from executing more than
@@ -3217,7 +3220,7 @@ static int ni_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 			    AO_BC_TC_Interrupt_Enable, 1);
 	}
 
-	s->async->inttrig = &ni_ao_inttrig;
+	s->async->inttrig = ni_ao_inttrig;
 
 	return 0;
 }
@@ -3228,7 +3231,7 @@ static int ni_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	const struct ni_board_struct *board = comedi_board(dev);
 	struct ni_private *devpriv = dev->private;
 	int err = 0;
-	int tmp;
+	unsigned int tmp;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -3258,17 +3261,18 @@ static int ni_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	if (cmd->start_src == TRIG_EXT) {
-		/* external trigger */
-		unsigned int tmp = CR_CHAN(cmd->start_arg);
+	switch (cmd->start_src) {
+	case TRIG_INT:
+		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+		break;
+	case TRIG_EXT:
+		tmp = CR_CHAN(cmd->start_arg);
 
 		if (tmp > 18)
 			tmp = 18;
 		tmp |= (cmd->start_arg & (CR_INVERT | CR_EDGE));
 		err |= cfc_check_trigger_arg_is(&cmd->start_arg, tmp);
-	} else {
-		/* true for both TRIG_NOW and TRIG_INT */
-		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+		break;
 	}
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
@@ -3304,11 +3308,6 @@ static int ni_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	if (err)
 		return 4;
 
-	/* step 5: fix up chanlist */
-
-	if (err)
-		return 5;
-
 	return 0;
 }
 
@@ -3439,12 +3438,27 @@ static int ni_m_series_dio_insn_bits(struct comedi_device *dev,
 	return insn->n;
 }
 
+static int ni_cdio_check_chanlist(struct comedi_device *dev,
+				  struct comedi_subdevice *s,
+				  struct comedi_cmd *cmd)
+{
+	int i;
+
+	for (i = 0; i < cmd->chanlist_len; ++i) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+
+		if (chan != i)
+			return -EINVAL;
+	}
+
+	return 0;
+}
+
 static int ni_cdio_cmdtest(struct comedi_device *dev,
 			   struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
 	int err = 0;
 	int tmp;
-	unsigned i;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -3484,12 +3498,9 @@ static int ni_cdio_cmdtest(struct comedi_device *dev,
 	if (err)
 		return 4;
 
-	/* step 5: check chanlist */
-
-	for (i = 0; i < cmd->chanlist_len; ++i) {
-		if (cmd->chanlist[i] != i)
-			err = 1;
-	}
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= ni_cdio_check_chanlist(dev, s, cmd);
 
 	if (err)
 		return 5;
@@ -3530,25 +3541,32 @@ static int ni_cdio_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	retval = ni_request_cdo_mite_channel(dev);
 	if (retval < 0)
 		return retval;
-	s->async->inttrig = &ni_cdo_inttrig;
+
+	s->async->inttrig = ni_cdo_inttrig;
+
 	return 0;
 }
 
-static int ni_cdo_inttrig(struct comedi_device *dev, struct comedi_subdevice *s,
-			  unsigned int trignum)
+static int ni_cdo_inttrig(struct comedi_device *dev,
+			  struct comedi_subdevice *s,
+			  unsigned int trig_num)
 {
 #ifdef PCIDMA
 	struct ni_private *devpriv = dev->private;
 	unsigned long flags;
 #endif
+	struct comedi_cmd *cmd = &s->async->cmd;
 	int retval = 0;
 	unsigned i;
 	const unsigned timeout = 1000;
 
+	if (trig_num != cmd->start_arg)
+		return -EINVAL;
+
 	s->async->inttrig = NULL;
 
 	/* read alloc the entire buffer */
-	comedi_buf_read_alloc(s->async, s->async->prealloc_bufsz);
+	comedi_buf_read_alloc(s, s->async->prealloc_bufsz);
 
 #ifdef PCIDMA
 	spin_lock_irqsave(&devpriv->mite_channel_lock, flags);
@@ -3623,7 +3641,7 @@ static void handle_cdio_interrupt(struct comedi_device *dev)
 			       devpriv->mite->mite_io_addr +
 			       MITE_CHOR(devpriv->cdo_mite_chan->channel));
 		}
-		mite_sync_output_dma(devpriv->cdo_mite_chan, s->async);
+		mite_sync_output_dma(devpriv->cdo_mite_chan, s);
 	}
 	spin_unlock_irqrestore(&devpriv->mite_channel_lock, flags);
 #endif
diff --git a/drivers/staging/comedi/drivers/ni_pcidio.c b/drivers/staging/comedi/drivers/ni_pcidio.c
index 85ac2d964f5..5fc74d6ff6a 100644
--- a/drivers/staging/comedi/drivers/ni_pcidio.c
+++ b/drivers/staging/comedi/drivers/ni_pcidio.c
@@ -359,7 +359,7 @@ static int ni_pcidio_poll(struct comedi_device *dev, struct comedi_subdevice *s)
 	spin_lock_irqsave(&dev->spinlock, irq_flags);
 	spin_lock(&devpriv->mite_channel_lock);
 	if (devpriv->di_mite_chan)
-		mite_sync_input_dma(devpriv->di_mite_chan, s->async);
+		mite_sync_input_dma(devpriv->di_mite_chan, s);
 	spin_unlock(&devpriv->mite_channel_lock);
 	count = s->async->buf_write_count - s->async->buf_read_count;
 	spin_unlock_irqrestore(&dev->spinlock, irq_flags);
@@ -405,7 +405,7 @@ static irqreturn_t nidio_interrupt(int irq, void *d)
 			writel(CHOR_CLRLC,
 			       mite->mite_io_addr +
 			       MITE_CHOR(devpriv->di_mite_chan->channel));
-			mite_sync_input_dma(devpriv->di_mite_chan, s->async);
+			mite_sync_input_dma(devpriv->di_mite_chan, s);
 			/* XXX need to byteswap */
 		}
 		if (m_status & ~(CHSR_INT | CHSR_LINKC | CHSR_DONE | CHSR_DRDY |
@@ -447,8 +447,8 @@ static irqreturn_t nidio_interrupt(int irq, void *d)
 					  Group_1_FIFO);
 				data1 = auxdata & 0xffff;
 				data2 = (auxdata & 0xffff0000) >> 16;
-				comedi_buf_put(async, data1);
-				comedi_buf_put(async, data2);
+				comedi_buf_put(s, data1);
+				comedi_buf_put(s, data2);
 				flags = readb(devpriv->mite->daq_io_addr +
 					      Group_1_Flags);
 			}
@@ -536,7 +536,7 @@ static int ni_pcidio_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -595,11 +595,9 @@ static int ni_pcidio_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		tmp = cmd->scan_begin_arg;
-		ni_pcidio_ns_to_timer(&cmd->scan_begin_arg,
-				      cmd->flags & TRIG_ROUND_MASK);
-		if (tmp != cmd->scan_begin_arg)
-			err++;
+		arg = cmd->scan_begin_arg;
+		ni_pcidio_ns_to_timer(&arg, cmd->flags & TRIG_ROUND_MASK);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 
 	if (err)
@@ -759,7 +757,7 @@ static int setup_mite_dma(struct comedi_device *dev, struct comedi_subdevice *s)
 		return retval;
 
 	/* write alloc the entire buffer */
-	comedi_buf_write_alloc(s->async, s->async->prealloc_bufsz);
+	comedi_buf_write_alloc(s, s->async->prealloc_bufsz);
 
 	spin_lock_irqsave(&devpriv->mite_channel_lock, flags);
 	if (devpriv->di_mite_chan) {
@@ -773,11 +771,13 @@ static int setup_mite_dma(struct comedi_device *dev, struct comedi_subdevice *s)
 }
 
 static int ni_pcidio_inttrig(struct comedi_device *dev,
-			     struct comedi_subdevice *s, unsigned int trignum)
+			     struct comedi_subdevice *s,
+			     unsigned int trig_num)
 {
 	struct nidio96_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 
-	if (trignum != 0)
+	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
 	writeb(devpriv->OpModeBits, devpriv->mite->daq_io_addr + OpMode);
@@ -804,7 +804,7 @@ static int ni_pcidio_change(struct comedi_device *dev,
 	struct nidio96_private *devpriv = dev->private;
 	int ret;
 
-	ret = mite_buf_change(devpriv->di_mite_ring, s->async);
+	ret = mite_buf_change(devpriv->di_mite_ring, s);
 	if (ret < 0)
 		return ret;
 
diff --git a/drivers/staging/comedi/drivers/ni_pcimio.c b/drivers/staging/comedi/drivers/ni_pcimio.c
index d40df072583..89300dc78e3 100644
--- a/drivers/staging/comedi/drivers/ni_pcimio.c
+++ b/drivers/staging/comedi/drivers/ni_pcimio.c
@@ -1560,7 +1560,7 @@ static int pcimio_ai_change(struct comedi_device *dev,
 	struct ni_private *devpriv = dev->private;
 	int ret;
 
-	ret = mite_buf_change(devpriv->ai_mite_ring, s->async);
+	ret = mite_buf_change(devpriv->ai_mite_ring, s);
 	if (ret < 0)
 		return ret;
 
@@ -1573,7 +1573,7 @@ static int pcimio_ao_change(struct comedi_device *dev,
 	struct ni_private *devpriv = dev->private;
 	int ret;
 
-	ret = mite_buf_change(devpriv->ao_mite_ring, s->async);
+	ret = mite_buf_change(devpriv->ao_mite_ring, s);
 	if (ret < 0)
 		return ret;
 
@@ -1587,7 +1587,7 @@ static int pcimio_gpct0_change(struct comedi_device *dev,
 	struct ni_private *devpriv = dev->private;
 	int ret;
 
-	ret = mite_buf_change(devpriv->gpct_mite_ring[0], s->async);
+	ret = mite_buf_change(devpriv->gpct_mite_ring[0], s);
 	if (ret < 0)
 		return ret;
 
@@ -1601,7 +1601,7 @@ static int pcimio_gpct1_change(struct comedi_device *dev,
 	struct ni_private *devpriv = dev->private;
 	int ret;
 
-	ret = mite_buf_change(devpriv->gpct_mite_ring[1], s->async);
+	ret = mite_buf_change(devpriv->gpct_mite_ring[1], s);
 	if (ret < 0)
 		return ret;
 
@@ -1614,7 +1614,7 @@ static int pcimio_dio_change(struct comedi_device *dev,
 	struct ni_private *devpriv = dev->private;
 	int ret;
 
-	ret = mite_buf_change(devpriv->cdo_mite_ring, s->async);
+	ret = mite_buf_change(devpriv->cdo_mite_ring, s);
 	if (ret < 0)
 		return ret;
 
diff --git a/drivers/staging/comedi/drivers/ni_tiocmd.c b/drivers/staging/comedi/drivers/ni_tiocmd.c
index 7d64f8892f0..2557ab48cb6 100644
--- a/drivers/staging/comedi/drivers/ni_tiocmd.c
+++ b/drivers/staging/comedi/drivers/ni_tiocmd.c
@@ -89,14 +89,16 @@ static void ni_tio_configure_dma(struct ni_gpct *counter, short enable,
 
 static int ni_tio_input_inttrig(struct comedi_device *dev,
 				struct comedi_subdevice *s,
-				unsigned int trignum)
+				unsigned int trig_num)
 {
+	struct ni_gpct *counter = s->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned long flags;
 	int retval = 0;
-	struct ni_gpct *counter = s->private;
 
 	BUG_ON(counter == NULL);
-	if (trignum != 0)
+
+	if (trig_num != cmd->start_src)
 		return -EINVAL;
 
 	spin_lock_irqsave(&counter->lock, flags);
@@ -113,15 +115,17 @@ static int ni_tio_input_inttrig(struct comedi_device *dev,
 	return retval;
 }
 
-static int ni_tio_input_cmd(struct ni_gpct *counter, struct comedi_async *async)
+static int ni_tio_input_cmd(struct comedi_subdevice *s)
 {
+	struct ni_gpct *counter = s->private;
 	struct ni_gpct_device *counter_dev = counter->counter_dev;
 	unsigned cidx = counter->counter_index;
+	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
 	int retval = 0;
 
 	/* write alloc the entire buffer */
-	comedi_buf_write_alloc(async, async->prealloc_bufsz);
+	comedi_buf_write_alloc(s, async->prealloc_bufsz);
 	counter->mite_chan->dir = COMEDI_INPUT;
 	switch (counter_dev->variant) {
 	case ni_gpct_variant_m_series:
@@ -162,9 +166,10 @@ static int ni_tio_input_cmd(struct ni_gpct *counter, struct comedi_async *async)
 	return retval;
 }
 
-static int ni_tio_output_cmd(struct ni_gpct *counter,
-			     struct comedi_async *async)
+static int ni_tio_output_cmd(struct comedi_subdevice *s)
 {
+	struct ni_gpct *counter = s->private;
+
 	dev_err(counter->counter_dev->dev->class_dev,
 		"output commands not yet implemented.\n");
 	return -ENOTSUPP;
@@ -176,9 +181,10 @@ static int ni_tio_output_cmd(struct ni_gpct *counter,
 	return ni_tio_arm(counter, 1, NI_GPCT_ARM_IMMEDIATE);
 }
 
-static int ni_tio_cmd_setup(struct ni_gpct *counter, struct comedi_async *async)
+static int ni_tio_cmd_setup(struct comedi_subdevice *s)
 {
-	struct comedi_cmd *cmd = &async->cmd;
+	struct comedi_cmd *cmd = &s->async->cmd;
+	struct ni_gpct *counter = s->private;
 	unsigned cidx = counter->counter_index;
 	int set_gate_source = 0;
 	unsigned gate_source;
@@ -217,12 +223,12 @@ int ni_tio_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 			"Interrupt-driven commands not yet implemented.\n");
 		retval = -EIO;
 	} else {
-		retval = ni_tio_cmd_setup(counter, async);
+		retval = ni_tio_cmd_setup(s);
 		if (retval == 0) {
 			if (cmd->flags & CMDF_WRITE)
-				retval = ni_tio_output_cmd(counter, async);
+				retval = ni_tio_output_cmd(s);
 			else
-				retval = ni_tio_input_cmd(counter, async);
+				retval = ni_tio_input_cmd(s);
 		}
 	}
 	spin_unlock_irqrestore(&counter->lock, flags);
@@ -271,8 +277,16 @@ int ni_tio_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	if (cmd->start_src != TRIG_EXT)
+	switch (cmd->start_src) {
+	case TRIG_NOW:
+	case TRIG_INT:
+	case TRIG_OTHER:
 		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+		break;
+	case TRIG_EXT:
+		/* start_arg is the start_trigger passed to ni_tio_arm() */
+		break;
+	}
 
 	if (cmd->scan_begin_src != TRIG_EXT)
 		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
@@ -450,7 +464,7 @@ void ni_tio_handle_interrupt(struct ni_gpct *counter,
 		       counter->mite_chan->mite->mite_io_addr +
 		       MITE_CHOR(counter->mite_chan->channel));
 	}
-	mite_sync_input_dma(counter->mite_chan, s->async);
+	mite_sync_input_dma(counter->mite_chan, s);
 	spin_unlock_irqrestore(&counter->lock, flags);
 }
 EXPORT_SYMBOL_GPL(ni_tio_handle_interrupt);
diff --git a/drivers/staging/comedi/drivers/pcl711.c b/drivers/staging/comedi/drivers/pcl711.c
index 7c03a5d17b1..c38d97a9a89 100644
--- a/drivers/staging/comedi/drivers/pcl711.c
+++ b/drivers/staging/comedi/drivers/pcl711.c
@@ -202,6 +202,7 @@ static irqreturn_t pcl711_interrupt(int irq, void *d)
 	struct comedi_device *dev = d;
 	struct pcl711_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->read_subdev;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned int data;
 
 	if (!dev->attached) {
@@ -213,12 +214,11 @@ static irqreturn_t pcl711_interrupt(int irq, void *d)
 
 	outb(PCL711_INT_STAT_CLR, dev->iobase + PCL711_INT_STAT_REG);
 
-	if (comedi_buf_put(s->async, data) == 0) {
+	if (comedi_buf_put(s, data) == 0) {
 		s->async->events |= COMEDI_CB_OVERFLOW | COMEDI_CB_ERROR;
 	} else {
 		s->async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOS;
-		if (s->async->cmd.stop_src == TRIG_COUNT &&
-		    !(--devpriv->ntrig)) {
+		if (cmd->stop_src == TRIG_COUNT && !(--devpriv->ntrig)) {
 			pcl711_ai_set_mode(dev, PCL711_MODE_SOFTTRIG);
 			s->async->events |= COMEDI_CB_EOA;
 		}
@@ -295,8 +295,8 @@ static int pcl711_ai_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
 	struct pcl711_private *devpriv = dev->private;
-	int tmp;
 	int err = 0;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -344,14 +344,12 @@ static int pcl711_ai_cmdtest(struct comedi_device *dev,
 	/* step 4 */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		tmp = cmd->scan_begin_arg;
+		arg = cmd->scan_begin_arg;
 		i8253_cascade_ns_to_timer(I8254_OSC_BASE_2MHZ,
 					  &devpriv->divisor1,
 					  &devpriv->divisor2,
-					  &cmd->scan_begin_arg,
-					  cmd->flags);
-		if (tmp != cmd->scan_begin_arg)
-			err++;
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 
 	if (err)
@@ -360,6 +358,18 @@ static int pcl711_ai_cmdtest(struct comedi_device *dev,
 	return 0;
 }
 
+static void pcl711_ai_load_counters(struct comedi_device *dev)
+{
+	struct pcl711_private *devpriv = dev->private;
+	unsigned long timer_base = dev->iobase + PCL711_TIMER_BASE;
+
+	i8254_set_mode(timer_base, 0, 1, I8254_MODE2 | I8254_BINARY);
+	i8254_set_mode(timer_base, 0, 2, I8254_MODE2 | I8254_BINARY);
+
+	i8254_write(timer_base, 0, 1, devpriv->divisor1);
+	i8254_write(timer_base, 0, 2, devpriv->divisor2);
+}
+
 static int pcl711_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
 	struct pcl711_private *devpriv = dev->private;
@@ -378,13 +388,8 @@ static int pcl711_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	}
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		i8254_load(dev->iobase + PCL711_TIMER_BASE, 0,
-			   1, devpriv->divisor1, I8254_MODE2 | I8254_BINARY);
-		i8254_load(dev->iobase + PCL711_TIMER_BASE, 0,
-			   2, devpriv->divisor2, I8254_MODE2 | I8254_BINARY);
-
+		pcl711_ai_load_counters(dev);
 		outb(PCL711_INT_STAT_CLR, dev->iobase + PCL711_INT_STAT_REG);
-
 		pcl711_ai_set_mode(dev, PCL711_MODE_PACER_IRQ);
 	} else {
 		pcl711_ai_set_mode(dev, PCL711_MODE_EXT_IRQ);
diff --git a/drivers/staging/comedi/drivers/pcl726.c b/drivers/staging/comedi/drivers/pcl726.c
index cf9568ee46e..74f6489bd12 100644
--- a/drivers/staging/comedi/drivers/pcl726.c
+++ b/drivers/staging/comedi/drivers/pcl726.c
@@ -197,7 +197,7 @@ static int pcl726_intr_cmdtest(struct comedi_device *dev,
 	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, 1);
+	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 	err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
@@ -240,7 +240,7 @@ static irqreturn_t pcl726_interrupt(int irq, void *d)
 	if (devpriv->cmd_running) {
 		pcl726_intr_cancel(dev, s);
 
-		comedi_buf_put(s->async, 0);
+		comedi_buf_put(s, 0);
 		s->async->events |= (COMEDI_CB_BLOCK | COMEDI_CB_EOS);
 		comedi_event(dev, s);
 	}
@@ -430,6 +430,7 @@ static int pcl726_attach(struct comedi_device *dev,
 		s->maxdata	= 1;
 		s->range_table	= &range_digital;
 		s->insn_bits	= pcl726_intr_insn_bits;
+		s->len_chanlist	= 1;
 		s->do_cmdtest	= pcl726_intr_cmdtest;
 		s->do_cmd	= pcl726_intr_cmd;
 		s->cancel	= pcl726_intr_cancel;
diff --git a/drivers/staging/comedi/drivers/pcl730.c b/drivers/staging/comedi/drivers/pcl730.c
index 2baaf1db6fb..7fb044ce399 100644
--- a/drivers/staging/comedi/drivers/pcl730.c
+++ b/drivers/staging/comedi/drivers/pcl730.c
@@ -17,6 +17,8 @@
  *	    (ICP) P16R16-DIO [p16r16dio]
  *	    (Advantech) PCL-733 [pcl733]
  *	    (Advantech) PCL-734 [pcl734]
+ *	    (Diamond Systems) OPMM-1616-XT [opmm-1616-xt]
+ *	    (Diamond Systems) PEARL-MM-P [prearl-mm-p]
  * Author: José Luis Sánchez (jsanchezv@teleline.es)
  * Status: untested
  *
@@ -70,6 +72,27 @@
  *     BASE+1  Isolated outputs 8-15 (write) or inputs 8-15 (read)
  *     BASE+2  Isolated outputs 16-23 (write) or inputs 16-23 (read)
  *     BASE+3  Isolated outputs 24-31 (write) or inputs 24-31 (read)
+ *
+ * The opmm-1616-xt board has this register mapping:
+ *
+ *     BASE+0  Isolated outputs 0-7 (write) (read back)
+ *     BASE+1  Isolated outputs 8-15 (write) (read back)
+ *     BASE+2  Isolated inputs 0-7 (read)
+ *     BASE+3  Isolated inputs 8-15 (read)
+ *
+ *     These registers are not currently supported:
+ *
+ *     BASE+2  Relay select register (write)
+ *     BASE+3  Board reset control register (write)
+ *     BASE+4  Interrupt control register (write)
+ *     BASE+4  Change detect 7-0 status register (read)
+ *     BASE+5  LED control register (write)
+ *     BASE+5  Change detect 15-8 status register (read)
+ *
+ * The pearl-mm-p board has this register mapping:
+ *
+ *     BASE+0  Isolated outputs 0-7 (write)
+ *     BASE+1  Isolated outputs 8-15 (write)
  */
 
 struct pcl730_board {
@@ -158,6 +181,19 @@ static const struct pcl730_board pcl730_boards[] = {
 		.io_range	= 0x04,
 		.n_subdevs	= 1,
 		.n_iso_out_chan	= 32,
+	}, {
+		.name		= "opmm-1616-xt",
+		.io_range	= 0x10,
+		.is_acl7225b	= 1,
+		.has_readback	= 1,
+		.n_subdevs	= 2,
+		.n_iso_out_chan	= 16,
+		.n_iso_in_chan	= 16,
+	}, {
+		.name		= "pearl-mm-p",
+		.io_range	= 0x02,
+		.n_subdevs	= 1,
+		.n_iso_out_chan	= 16,
 	},
 };
 
diff --git a/drivers/staging/comedi/drivers/pcl812.c b/drivers/staging/comedi/drivers/pcl812.c
index 160eac8083d..4c1b9470647 100644
--- a/drivers/staging/comedi/drivers/pcl812.c
+++ b/drivers/staging/comedi/drivers/pcl812.c
@@ -557,10 +557,8 @@ static void pcl812_ai_setup_dma(struct comedi_device *dev,
 
 	/*  we use EOS, so adapt DMA buffer to one scan */
 	if (devpriv->ai_eos) {
-		devpriv->dmabytestomove[0] =
-			cmd->chanlist_len * sizeof(short);
-		devpriv->dmabytestomove[1] =
-			cmd->chanlist_len * sizeof(short);
+		devpriv->dmabytestomove[0] = cfc_bytes_per_scan(s);
+		devpriv->dmabytestomove[1] = cfc_bytes_per_scan(s);
 		devpriv->dma_runs_to_end = 1;
 	} else {
 		devpriv->dmabytestomove[0] = devpriv->hwdmasize;
@@ -575,8 +573,7 @@ static void pcl812_ai_setup_dma(struct comedi_device *dev,
 			devpriv->dma_runs_to_end = 1;
 		} else {
 			/*  how many samples we must transfer? */
-			bytes = cmd->chanlist_len *
-				cmd->stop_arg * sizeof(short);
+			bytes = cmd->stop_arg * cfc_bytes_per_scan(s);
 
 			/*  how many DMA pages we must fill */
 			devpriv->dma_runs_to_end =
@@ -721,7 +718,7 @@ static int pcl812_ai_cmdtest(struct comedi_device *dev,
 	struct pcl812_private *devpriv = dev->private;
 	int err = 0;
 	unsigned int flags;
-	int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -761,7 +758,6 @@ static int pcl812_ai_cmdtest(struct comedi_device *dev,
 		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
 
 	err |= cfc_check_trigger_arg_min(&cmd->chanlist_len, 1);
-	err |= cfc_check_trigger_arg_max(&cmd->chanlist_len, MAX_CHANLIST_LEN);
 	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
@@ -775,15 +771,12 @@ static int pcl812_ai_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		tmp = cmd->convert_arg;
+		arg = cmd->convert_arg;
 		i8253_cascade_ns_to_timer(I8254_OSC_BASE_2MHZ,
 					  &devpriv->divisor1,
 					  &devpriv->divisor2,
-					  &cmd->convert_arg, cmd->flags);
-		if (cmd->convert_arg < board->ai_ns_min)
-			cmd->convert_arg = board->ai_ns_min;
-		if (tmp != cmd->convert_arg)
-			err++;
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 
 	if (err)
@@ -811,8 +804,9 @@ static int pcl812_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 				devpriv->ai_dma = 0;
 				break;
 			}
-	} else
+	} else {
 		devpriv->ai_dma = 0;
+	}
 
 	devpriv->ai_act_scan = 0;
 	devpriv->ai_poll_ptr = 0;
@@ -882,7 +876,7 @@ static void pcl812_handle_eoc(struct comedi_device *dev,
 		return;
 	}
 
-	comedi_buf_put(s->async, pcl812_ai_get_sample(dev, s));
+	comedi_buf_put(s, pcl812_ai_get_sample(dev, s));
 
 	/* Set up next channel. Added by abbotti 2010-01-20, but untested. */
 	next_chan = s->async->cur_chan + 1;
@@ -902,7 +896,7 @@ static void transfer_from_dma_buf(struct comedi_device *dev,
 	unsigned int i;
 
 	for (i = len; i; i--) {
-		comedi_buf_put(s->async, ptr[bufptr++]);
+		comedi_buf_put(s, ptr[bufptr++]);
 
 		if (!pcl812_ai_next_chan(dev, s))
 			break;
diff --git a/drivers/staging/comedi/drivers/pcl816.c b/drivers/staging/comedi/drivers/pcl816.c
index 6f276f23fab..d9ca7fe16c9 100644
--- a/drivers/staging/comedi/drivers/pcl816.c
+++ b/drivers/staging/comedi/drivers/pcl816.c
@@ -164,9 +164,7 @@ static void pcl816_ai_setup_dma(struct comedi_device *dev,
 	bytes = devpriv->hwdmasize;
 	if (cmd->stop_src == TRIG_COUNT) {
 		/*  how many */
-		bytes = s->async->cmd.chanlist_len *
-		s->async->cmd.chanlist_len *
-		sizeof(short);
+		bytes = cmd->stop_arg * cfc_bytes_per_scan(s);
 
 		/*  how many DMA pages we must fill */
 		devpriv->dma_runs_to_end = bytes / devpriv->hwdmasize;
@@ -322,7 +320,7 @@ static void transfer_from_dma_buf(struct comedi_device *dev,
 	int i;
 
 	for (i = 0; i < len; i++) {
-		comedi_buf_put(s->async, ptr[bufptr++]);
+		comedi_buf_put(s, ptr[bufptr++]);
 
 		if (!pcl816_ai_next_chan(dev, s))
 			return;
@@ -370,7 +368,7 @@ static int pcl816_ai_cmdtest(struct comedi_device *dev,
 {
 	struct pcl816_private *devpriv = dev->private;
 	int err = 0;
-	int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -417,15 +415,12 @@ static int pcl816_ai_cmdtest(struct comedi_device *dev,
 
 	/* step 4: fix up any arguments */
 	if (cmd->convert_src == TRIG_TIMER) {
-		tmp = cmd->convert_arg;
+		arg = cmd->convert_arg;
 		i8253_cascade_ns_to_timer(I8254_OSC_BASE_10MHZ,
 					  &devpriv->divisor1,
 					  &devpriv->divisor2,
-					  &cmd->convert_arg, cmd->flags);
-		if (cmd->convert_arg < 10000)
-			cmd->convert_arg = 10000;
-		if (tmp != cmd->convert_arg)
-			err++;
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 
 	if (err)
diff --git a/drivers/staging/comedi/drivers/pcl818.c b/drivers/staging/comedi/drivers/pcl818.c
index 6463476ce45..7d00ae639d3 100644
--- a/drivers/staging/comedi/drivers/pcl818.c
+++ b/drivers/staging/comedi/drivers/pcl818.c
@@ -353,7 +353,7 @@ static void pcl818_ai_setup_dma(struct comedi_device *dev,
 	disable_dma(devpriv->dma);	/*  disable dma */
 	bytes = devpriv->hwdmasize;
 	if (cmd->stop_src == TRIG_COUNT) {
-		bytes = cmd->chanlist_len * cmd->stop_arg * sizeof(short);
+		bytes = cmd->stop_arg * cfc_bytes_per_scan(s);
 		devpriv->dma_runs_to_end = bytes / devpriv->hwdmasize;
 		devpriv->last_dma_run = bytes % devpriv->hwdmasize;
 		devpriv->dma_runs_to_end--;
@@ -561,7 +561,7 @@ static void pcl818_handle_eoc(struct comedi_device *dev,
 	if (pcl818_ai_dropout(dev, s, chan))
 		return;
 
-	comedi_buf_put(s->async, val);
+	comedi_buf_put(s, val);
 
 	pcl818_ai_next_chan(dev, s);
 }
@@ -590,7 +590,7 @@ static void pcl818_handle_dma(struct comedi_device *dev,
 		if (pcl818_ai_dropout(dev, s, chan))
 			break;
 
-		comedi_buf_put(s->async, val);
+		comedi_buf_put(s, val);
 
 		if (!pcl818_ai_next_chan(dev, s))
 			break;
@@ -630,7 +630,7 @@ static void pcl818_handle_fifo(struct comedi_device *dev,
 		if (pcl818_ai_dropout(dev, s, chan))
 			break;
 
-		comedi_buf_put(s->async, val);
+		comedi_buf_put(s, val);
 
 		if (!pcl818_ai_next_chan(dev, s))
 			break;
@@ -741,7 +741,7 @@ static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	const struct pcl818_board *board = comedi_board(dev);
 	struct pcl818_private *devpriv = dev->private;
 	int err = 0;
-	int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -788,15 +788,12 @@ static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	/* step 4: fix up any arguments */
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		tmp = cmd->convert_arg;
+		arg = cmd->convert_arg;
 		i8253_cascade_ns_to_timer(devpriv->i8253_osc_base,
 					  &devpriv->divisor1,
 					  &devpriv->divisor2,
-					  &cmd->convert_arg, cmd->flags);
-		if (cmd->convert_arg < board->ns_min)
-			cmd->convert_arg = board->ns_min;
-		if (tmp != cmd->convert_arg)
-			err++;
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 
 	if (err)
diff --git a/drivers/staging/comedi/drivers/pcmmio.c b/drivers/staging/comedi/drivers/pcmmio.c
index e89bca84534..fed7e77e030 100644
--- a/drivers/staging/comedi/drivers/pcmmio.c
+++ b/drivers/staging/comedi/drivers/pcmmio.c
@@ -192,7 +192,6 @@ struct pcmmio_private {
 	unsigned int enabled_mask;
 	unsigned int stop_count;
 	unsigned int active:1;
-	unsigned int continuous:1;
 
 	unsigned int ao_readback[8];
 };
@@ -339,8 +338,8 @@ static void pcmmio_handle_dio_intr(struct comedi_device *dev,
 				   unsigned int triggered)
 {
 	struct pcmmio_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned int oldevents = s->async->events;
-	unsigned int len = s->async->cmd.chanlist_len;
 	unsigned int val = 0;
 	unsigned long flags;
 	int i;
@@ -353,16 +352,16 @@ static void pcmmio_handle_dio_intr(struct comedi_device *dev,
 	if (!(triggered & devpriv->enabled_mask))
 		goto done;
 
-	for (i = 0; i < len; i++) {
-		unsigned int chan = CR_CHAN(s->async->cmd.chanlist[i]);
+	for (i = 0; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
 
 		if (triggered & (1 << chan))
 			val |= (1 << i);
 	}
 
 	/* Write the scan to the buffer. */
-	if (comedi_buf_put(s->async, val) &&
-	    comedi_buf_put(s->async, val >> 16)) {
+	if (comedi_buf_put(s, val) &&
+	    comedi_buf_put(s, val >> 16)) {
 		s->async->events |= (COMEDI_CB_BLOCK | COMEDI_CB_EOS);
 	} else {
 		/* Overflow! Stop acquisition!! */
@@ -371,15 +370,12 @@ static void pcmmio_handle_dio_intr(struct comedi_device *dev,
 	}
 
 	/* Check for end of acquisition. */
-	if (!devpriv->continuous) {
-		/* stop_src == TRIG_COUNT */
-		if (devpriv->stop_count > 0) {
-			devpriv->stop_count--;
-			if (devpriv->stop_count == 0) {
-				s->async->events |= COMEDI_CB_EOA;
-				/* TODO: STOP_ACQUISITION_CALL_HERE!! */
-				pcmmio_stop_intr(dev, s);
-			}
+	if (cmd->stop_src == TRIG_COUNT && devpriv->stop_count > 0) {
+		devpriv->stop_count--;
+		if (devpriv->stop_count == 0) {
+			s->async->events |= COMEDI_CB_EOA;
+			/* TODO: STOP_ACQUISITION_CALL_HERE!! */
+			pcmmio_stop_intr(dev, s);
 		}
 	}
 
@@ -421,7 +417,7 @@ static int pcmmio_start_intr(struct comedi_device *dev,
 	unsigned int pol_bits = 0;
 	int i;
 
-	if (!devpriv->continuous && devpriv->stop_count == 0) {
+	if (cmd->stop_src == TRIG_COUNT && devpriv->stop_count == 0) {
 		/* An empty acquisition! */
 		s->async->events |= COMEDI_CB_EOA;
 		devpriv->active = 0;
@@ -464,18 +460,16 @@ static int pcmmio_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 	return 0;
 }
 
-/*
- * Internal trigger function to start acquisition for an 'INTERRUPT' subdevice.
- */
-static int
-pcmmio_inttrig_start_intr(struct comedi_device *dev, struct comedi_subdevice *s,
-			  unsigned int trignum)
+static int pcmmio_inttrig_start_intr(struct comedi_device *dev,
+				     struct comedi_subdevice *s,
+				     unsigned int trig_num)
 {
 	struct pcmmio_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned long flags;
 	int event = 0;
 
-	if (trignum != 0)
+	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
 	spin_lock_irqsave(&devpriv->spinlock, flags);
@@ -504,28 +498,17 @@ static int pcmmio_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	devpriv->active = 1;
 
 	/* Set up end of acquisition. */
-	switch (cmd->stop_src) {
-	case TRIG_COUNT:
-		devpriv->continuous = 0;
+	if (cmd->stop_src == TRIG_COUNT)
 		devpriv->stop_count = cmd->stop_arg;
-		break;
-	default:
-		/* TRIG_NONE */
-		devpriv->continuous = 1;
+	else	/* TRIG_NONE */
 		devpriv->stop_count = 0;
-		break;
-	}
 
 	/* Set up start of acquisition. */
-	switch (cmd->start_src) {
-	case TRIG_INT:
+	if (cmd->start_src == TRIG_INT)
 		s->async->inttrig = pcmmio_inttrig_start_intr;
-		break;
-	default:
-		/* TRIG_NOW */
+	else	/* TRIG_NOW */
 		event = pcmmio_start_intr(dev, s);
-		break;
-	}
+
 	spin_unlock_irqrestore(&devpriv->spinlock, flags);
 
 	if (event)
diff --git a/drivers/staging/comedi/drivers/pcmuio.c b/drivers/staging/comedi/drivers/pcmuio.c
index a8f390f7a87..62914bb342d 100644
--- a/drivers/staging/comedi/drivers/pcmuio.c
+++ b/drivers/staging/comedi/drivers/pcmuio.c
@@ -317,7 +317,7 @@ static void pcmuio_handle_intr_subdev(struct comedi_device *dev,
 	struct pcmuio_private *devpriv = dev->private;
 	int asic = pcmuio_subdevice_to_asic(s);
 	struct pcmuio_asic *chip = &devpriv->asics[asic];
-	unsigned int len = s->async->cmd.chanlist_len;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned oldevents = s->async->events;
 	unsigned int val = 0;
 	unsigned long flags;
@@ -331,15 +331,15 @@ static void pcmuio_handle_intr_subdev(struct comedi_device *dev,
 	if (!(triggered & chip->enabled_mask))
 		goto done;
 
-	for (i = 0; i < len; i++) {
-		unsigned int chan = CR_CHAN(s->async->cmd.chanlist[i]);
+	for (i = 0; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
 		if (triggered & (1 << chan))
 			val |= (1 << i);
 	}
 
 	/* Write the scan to the buffer. */
-	if (comedi_buf_put(s->async, val) &&
-	    comedi_buf_put(s->async, val >> 16)) {
+	if (comedi_buf_put(s, val) &&
+	    comedi_buf_put(s, val >> 16)) {
 		s->async->events |= (COMEDI_CB_BLOCK | COMEDI_CB_EOS);
 	} else {
 		/* Overflow! Stop acquisition!! */
@@ -460,20 +460,18 @@ static int pcmuio_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 	return 0;
 }
 
-/*
- * Internal trigger function to start acquisition for an 'INTERRUPT' subdevice.
- */
-static int
-pcmuio_inttrig_start_intr(struct comedi_device *dev, struct comedi_subdevice *s,
-			  unsigned int trignum)
+static int pcmuio_inttrig_start_intr(struct comedi_device *dev,
+				     struct comedi_subdevice *s,
+				     unsigned int trig_num)
 {
 	struct pcmuio_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	int asic = pcmuio_subdevice_to_asic(s);
 	struct pcmuio_asic *chip = &devpriv->asics[asic];
 	unsigned long flags;
 	int event = 0;
 
-	if (trignum != 0)
+	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
 	spin_lock_irqsave(&chip->spinlock, flags);
@@ -518,15 +516,11 @@ static int pcmuio_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	}
 
 	/* Set up start of acquisition. */
-	switch (cmd->start_src) {
-	case TRIG_INT:
+	if (cmd->start_src == TRIG_INT)
 		s->async->inttrig = pcmuio_inttrig_start_intr;
-		break;
-	default:
-		/* TRIG_NOW */
+	else	/* TRIG_NOW */
 		event = pcmuio_start_intr(dev, s);
-		break;
-	}
+
 	spin_unlock_irqrestore(&chip->spinlock, flags);
 
 	if (event)
diff --git a/drivers/staging/comedi/drivers/quatech_daqp_cs.c b/drivers/staging/comedi/drivers/quatech_daqp_cs.c
index 298dba03f90..b3bbec0a0d2 100644
--- a/drivers/staging/comedi/drivers/quatech_daqp_cs.c
+++ b/drivers/staging/comedi/drivers/quatech_daqp_cs.c
@@ -221,7 +221,7 @@ static enum irqreturn daqp_interrupt(int irq, void *dev_id)
 			data |= inb(dev->iobase + DAQP_FIFO) << 8;
 			data ^= 0x8000;
 
-			comedi_buf_put(s->async, data);
+			comedi_buf_put(s, data);
 
 			/* If there's a limit, decrement it
 			 * and stop conversion if zero
@@ -373,7 +373,7 @@ static int daqp_ai_cmdtest(struct comedi_device *dev,
 			   struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -435,19 +435,15 @@ static int daqp_ai_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		tmp = cmd->scan_begin_arg;
-		daqp_ns_to_timer(&cmd->scan_begin_arg,
-				 cmd->flags & TRIG_ROUND_MASK);
-		if (tmp != cmd->scan_begin_arg)
-			err++;
+		arg = cmd->scan_begin_arg;
+		daqp_ns_to_timer(&arg, cmd->flags & TRIG_ROUND_MASK);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		tmp = cmd->convert_arg;
-		daqp_ns_to_timer(&cmd->convert_arg,
-				 cmd->flags & TRIG_ROUND_MASK);
-		if (tmp != cmd->convert_arg)
-			err++;
+		arg = cmd->convert_arg;
+		daqp_ns_to_timer(&arg, cmd->flags & TRIG_ROUND_MASK);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
 	}
 
 	if (err)
diff --git a/drivers/staging/comedi/drivers/rtd520.c b/drivers/staging/comedi/drivers/rtd520.c
index cd3fdf973bd..d55c5893203 100644
--- a/drivers/staging/comedi/drivers/rtd520.c
+++ b/drivers/staging/comedi/drivers/rtd520.c
@@ -629,7 +629,7 @@ static int ai_read_n(struct comedi_device *dev, struct comedi_subdevice *s,
 			d = comedi_offset_munge(s, d);
 		d &= s->maxdata;
 
-		if (!comedi_buf_put(s->async, d))
+		if (!comedi_buf_put(s, d))
 			return -1;
 
 		if (devpriv->ai_count > 0)	/* < 0, means read forever */
@@ -658,7 +658,7 @@ static int ai_read_dregs(struct comedi_device *dev, struct comedi_subdevice *s)
 			d = comedi_offset_munge(s, d);
 		d &= s->maxdata;
 
-		if (!comedi_buf_put(s->async, d))
+		if (!comedi_buf_put(s, d))
 			return -1;
 
 		if (devpriv->ai_count > 0)	/* < 0, means read forever */
@@ -779,7 +779,7 @@ static int rtd_ai_cmdtest(struct comedi_device *dev,
 			  struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -878,6 +878,8 @@ static int rtd_ai_cmdtest(struct comedi_device *dev,
 		err |= cfc_check_trigger_arg_max(&cmd->convert_arg, 9);
 	}
 
+	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+
 	if (cmd->stop_src == TRIG_COUNT) {
 		/* TODO check for rounding error due to counter wrap */
 	} else {
@@ -891,31 +893,21 @@ static int rtd_ai_cmdtest(struct comedi_device *dev,
 
 	/* step 4: fix up any arguments */
 
-	if (cmd->chanlist_len > RTD_MAX_CHANLIST) {
-		cmd->chanlist_len = RTD_MAX_CHANLIST;
-		err++;
-	}
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		tmp = cmd->scan_begin_arg;
-		rtd_ns_to_timer(&cmd->scan_begin_arg,
-				cmd->flags & TRIG_ROUND_MASK);
-		if (tmp != cmd->scan_begin_arg)
-			err++;
-
+		arg = cmd->scan_begin_arg;
+		rtd_ns_to_timer(&arg, cmd->flags & TRIG_ROUND_MASK);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
+
 	if (cmd->convert_src == TRIG_TIMER) {
-		tmp = cmd->convert_arg;
-		rtd_ns_to_timer(&cmd->convert_arg,
-				cmd->flags & TRIG_ROUND_MASK);
-		if (tmp != cmd->convert_arg)
-			err++;
-
-		if (cmd->scan_begin_src == TRIG_TIMER
-		    && (cmd->scan_begin_arg
-			< (cmd->convert_arg * cmd->scan_end_arg))) {
-			cmd->scan_begin_arg =
-			    cmd->convert_arg * cmd->scan_end_arg;
-			err++;
+		arg = cmd->convert_arg;
+		rtd_ns_to_timer(&arg, cmd->flags & TRIG_ROUND_MASK);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
+
+		if (cmd->scan_begin_src == TRIG_TIMER) {
+			arg = cmd->convert_arg * cmd->scan_end_arg;
+			err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
+							 arg);
 		}
 	}
 
diff --git a/drivers/staging/comedi/drivers/s626.c b/drivers/staging/comedi/drivers/s626.c
index 95fadf343f2..0838f8aa695 100644
--- a/drivers/staging/comedi/drivers/s626.c
+++ b/drivers/staging/comedi/drivers/s626.c
@@ -295,10 +295,24 @@ static void s626_debi_replace(struct comedi_device *dev, unsigned int addr,
 
 /* **************  EEPROM ACCESS FUNCTIONS  ************** */
 
-static uint32_t s626_i2c_handshake(struct comedi_device *dev, uint32_t val)
+static int s626_i2c_handshake_eoc(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 struct comedi_insn *insn,
+				 unsigned long context)
+{
+	bool status;
+
+	status = s626_mc_test(dev, S626_MC2_UPLD_IIC, S626_P_MC2);
+	if (status)
+		return 0;
+	return -EBUSY;
+}
+
+static int s626_i2c_handshake(struct comedi_device *dev, uint32_t val)
 {
 	struct s626_private *devpriv = dev->private;
 	unsigned int ctrl;
+	int ret;
 
 	/* Write I2C command to I2C Transfer Control shadow register */
 	writel(val, devpriv->mmio + S626_P_I2CCTRL);
@@ -308,8 +322,9 @@ static uint32_t s626_i2c_handshake(struct comedi_device *dev, uint32_t val)
 	 * wait for upload confirmation.
 	 */
 	s626_mc_enable(dev, S626_MC2_UPLD_IIC, S626_P_MC2);
-	while (!s626_mc_test(dev, S626_MC2_UPLD_IIC, S626_P_MC2))
-		;
+	ret = comedi_timeout(dev, NULL, NULL, s626_i2c_handshake_eoc, 0);
+	if (ret)
+		return ret;
 
 	/* Wait until I2C bus transfer is finished or an error occurs */
 	do {
@@ -1613,7 +1628,6 @@ static bool s626_handle_eos_interrupt(struct comedi_device *dev)
 		readaddr++;
 
 		/* put data into read buffer */
-		/* comedi_buf_put(async, tempdata); */
 		cfc_write_to_buffer(s, tempdata);
 	}
 
@@ -2029,8 +2043,9 @@ static int s626_ai_insn_read(struct comedi_device *dev,
 	/* Wait for the data to arrive in FB BUFFER 1 register. */
 
 	/* Wait for ADC done */
-	while (!(readl(devpriv->mmio + S626_P_PSR) & S626_PSR_GPIO2))
-		;
+	ret = comedi_timeout(dev, s, insn, s626_ai_eoc, 0);
+	if (ret)
+		return ret;
 
 	/* Fetch ADC data from audio interface's input shift register. */
 
@@ -2060,9 +2075,12 @@ 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 comedi_subdevice *s,
+			   unsigned int trig_num)
 {
-	if (trignum != 0)
+	struct comedi_cmd *cmd = &s->async->cmd;
+
+	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
 	/* Start executing the RPS program */
@@ -2080,7 +2098,7 @@ static int s626_ai_inttrig(struct comedi_device *dev,
  * Also, it should adjust ns so that it cooresponds to the actual time
  * that the device will use.
  */
-static int s626_ns_to_timer(int *nanosec, int round_mode)
+static int s626_ns_to_timer(unsigned int *nanosec, int round_mode)
 {
 	int divider, base;
 
@@ -2188,7 +2206,7 @@ static int s626_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		 * interval
 		 */
 		k = &s626_enc_chan_info[5];
-		tick = s626_ns_to_timer((int *)&cmd->scan_begin_arg,
+		tick = s626_ns_to_timer(&cmd->scan_begin_arg,
 					cmd->flags & TRIG_ROUND_MASK);
 
 		/* load timer value and enable interrupt */
@@ -2211,7 +2229,7 @@ static int s626_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		 * interval
 		 */
 		k = &s626_enc_chan_info[4];
-		tick = s626_ns_to_timer((int *)&cmd->convert_arg,
+		tick = s626_ns_to_timer(&cmd->convert_arg,
 					cmd->flags & TRIG_ROUND_MASK);
 
 		/* load timer value and enable interrupt */
@@ -2270,7 +2288,7 @@ static int s626_ai_cmdtest(struct comedi_device *dev,
 			   struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -2298,12 +2316,18 @@ static int s626_ai_cmdtest(struct comedi_device *dev,
 	if (err)
 		return 2;
 
-	/* step 3: make sure arguments are trivially compatible */
+	/* Step 3: check if arguments are trivially valid */
 
-	if (cmd->start_src != TRIG_EXT)
+	switch (cmd->start_src) {
+	case TRIG_NOW:
+	case TRIG_INT:
 		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-	if (cmd->start_src == TRIG_EXT)
+		break;
+	case TRIG_EXT:
 		err |= cfc_check_trigger_arg_max(&cmd->start_arg, 39);
+		break;
+	}
+
 	if (cmd->scan_begin_src == TRIG_EXT)
 		err |= cfc_check_trigger_arg_max(&cmd->scan_begin_arg, 39);
 	if (cmd->convert_src == TRIG_EXT)
@@ -2347,24 +2371,20 @@ static int s626_ai_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		tmp = cmd->scan_begin_arg;
-		s626_ns_to_timer((int *)&cmd->scan_begin_arg,
-				 cmd->flags & TRIG_ROUND_MASK);
-		if (tmp != cmd->scan_begin_arg)
-			err++;
+		arg = cmd->scan_begin_arg;
+		s626_ns_to_timer(&arg, cmd->flags & TRIG_ROUND_MASK);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
+
 	if (cmd->convert_src == TRIG_TIMER) {
-		tmp = cmd->convert_arg;
-		s626_ns_to_timer((int *)&cmd->convert_arg,
-				 cmd->flags & TRIG_ROUND_MASK);
-		if (tmp != cmd->convert_arg)
-			err++;
-		if (cmd->scan_begin_src == TRIG_TIMER &&
-		    cmd->scan_begin_arg < cmd->convert_arg *
-					  cmd->scan_end_arg) {
-			cmd->scan_begin_arg = cmd->convert_arg *
-					      cmd->scan_end_arg;
-			err++;
+		arg = cmd->convert_arg;
+		s626_ns_to_timer(&arg, cmd->flags & TRIG_ROUND_MASK);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
+
+		if (cmd->scan_begin_src == TRIG_TIMER) {
+			arg = cmd->convert_arg * cmd->scan_end_arg;
+			err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
+							 arg);
 		}
 	}
 
@@ -2681,8 +2701,9 @@ static int s626_initialize(struct comedi_device *dev)
 	writel(S626_I2C_CLKSEL | S626_I2C_ABORT,
 	       devpriv->mmio + S626_P_I2CSTAT);
 	s626_mc_enable(dev, S626_MC2_UPLD_IIC, S626_P_MC2);
-	while (!(readl(devpriv->mmio + S626_P_MC2) & S626_MC2_UPLD_IIC))
-		;
+	ret = comedi_timeout(dev, NULL, NULL, s626_i2c_handshake_eoc, 0);
+	if (ret)
+		return ret;
 
 	/*
 	 * Per SAA7146 data sheet, write to STATUS
@@ -2691,8 +2712,9 @@ static int s626_initialize(struct comedi_device *dev)
 	for (i = 0; i < 2; i++) {
 		writel(S626_I2C_CLKSEL, devpriv->mmio + S626_P_I2CSTAT);
 		s626_mc_enable(dev, S626_MC2_UPLD_IIC, S626_P_MC2);
-		while (!s626_mc_test(dev, S626_MC2_UPLD_IIC, S626_P_MC2))
-			;
+		ret = comedi_timeout(dev, NULL, NULL, s626_i2c_handshake_eoc, 0);
+		if (ret)
+			return ret;
 	}
 
 	/*
diff --git a/drivers/staging/comedi/drivers/skel.c b/drivers/staging/comedi/drivers/skel.c
index 39008cf30ec..3bfa221faf4 100644
--- a/drivers/staging/comedi/drivers/skel.c
+++ b/drivers/staging/comedi/drivers/skel.c
@@ -220,7 +220,7 @@ static int skel_ai_cmdtest(struct comedi_device *dev,
 			   struct comedi_cmd *cmd)
 {
 	int err = 0;
-	int tmp;
+	unsigned int arg;
 
 	/* Step 1 : check if triggers are trivially valid */
 
@@ -286,24 +286,19 @@ static int skel_ai_cmdtest(struct comedi_device *dev,
 	/* step 4: fix up any arguments */
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
-		tmp = cmd->scan_begin_arg;
-		skel_ns_to_timer(&cmd->scan_begin_arg,
-				 cmd->flags & TRIG_ROUND_MASK);
-		if (tmp != cmd->scan_begin_arg)
-			err++;
+		arg = cmd->scan_begin_arg;
+		skel_ns_to_timer(&arg, cmd->flags & TRIG_ROUND_MASK);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
 	}
 	if (cmd->convert_src == TRIG_TIMER) {
-		tmp = cmd->convert_arg;
-		skel_ns_to_timer(&cmd->convert_arg,
-				 cmd->flags & TRIG_ROUND_MASK);
-		if (tmp != cmd->convert_arg)
-			err++;
-		if (cmd->scan_begin_src == TRIG_TIMER &&
-		    cmd->scan_begin_arg <
-		    cmd->convert_arg * cmd->scan_end_arg) {
-			cmd->scan_begin_arg =
-			    cmd->convert_arg * cmd->scan_end_arg;
-			err++;
+		arg = cmd->convert_arg;
+		skel_ns_to_timer(&arg, cmd->flags & TRIG_ROUND_MASK);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
+
+		if (cmd->scan_begin_src == TRIG_TIMER) {
+			arg = cmd->convert_arg * cmd->scan_end_arg;
+			err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
+							 arg);
 		}
 	}
 
diff --git a/drivers/staging/comedi/drivers/usbdux.c b/drivers/staging/comedi/drivers/usbdux.c
index 71db683098d..5f65e4213c6 100644
--- a/drivers/staging/comedi/drivers/usbdux.c
+++ b/drivers/staging/comedi/drivers/usbdux.c
@@ -202,14 +202,11 @@ struct usbdux_private {
 	/* input buffer for single insn */
 	uint16_t *insn_buf;
 
-	uint8_t ao_chanlist[USBDUX_NUM_AO_CHAN];
 	unsigned int ao_readback[USBDUX_NUM_AO_CHAN];
 
 	unsigned int high_speed:1;
 	unsigned int ai_cmd_running:1;
-	unsigned int ai_continous:1;
 	unsigned int ao_cmd_running:1;
-	unsigned int ao_continous:1;
 	unsigned int pwm_cmd_running:1;
 
 	/* number of samples to acquire */
@@ -266,7 +263,8 @@ static void usbduxsub_ai_isoc_irq(struct urb *urb)
 	struct comedi_device *dev = urb->context;
 	struct comedi_subdevice *s = dev->read_subdev;
 	struct usbdux_private *devpriv = dev->private;
-	int i, err, n;
+	struct comedi_cmd *cmd = &s->async->cmd;
+	int i, err;
 
 	/* first we test if something unusual has just happened */
 	switch (urb->status) {
@@ -349,7 +347,7 @@ static void usbduxsub_ai_isoc_irq(struct urb *urb)
 	devpriv->ai_counter = devpriv->ai_timer;
 
 	/* test, if we transmit only a fixed number of samples */
-	if (!devpriv->ai_continous) {
+	if (cmd->stop_src == TRIG_COUNT) {
 		/* not continuous, fixed number of samples */
 		devpriv->ai_sample_count--;
 		/* all samples received? */
@@ -363,9 +361,8 @@ static void usbduxsub_ai_isoc_irq(struct urb *urb)
 		}
 	}
 	/* get the data from the USB bus and hand it over to comedi */
-	n = s->async->cmd.chanlist_len;
-	for (i = 0; i < n; i++) {
-		unsigned int range = CR_RANGE(s->async->cmd.chanlist[i]);
+	for (i = 0; i < cmd->chanlist_len; i++) {
+		unsigned int range = CR_RANGE(cmd->chanlist[i]);
 		uint16_t val = le16_to_cpu(devpriv->in_buf[i]);
 
 		/* bipolar data is two's-complement */
@@ -373,7 +370,7 @@ static void usbduxsub_ai_isoc_irq(struct urb *urb)
 			val ^= ((s->maxdata + 1) >> 1);
 
 		/* transfer data */
-		err = comedi_buf_put(s->async, val);
+		err = comedi_buf_put(s, val);
 		if (unlikely(err == 0)) {
 			/* buffer overflow */
 			usbdux_ai_stop(dev, 0);
@@ -414,8 +411,8 @@ static void usbduxsub_ao_isoc_irq(struct urb *urb)
 	struct comedi_device *dev = urb->context;
 	struct comedi_subdevice *s = dev->write_subdev;
 	struct usbdux_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	uint8_t *datap;
-	int len;
 	int ret;
 	int i;
 
@@ -463,7 +460,7 @@ static void usbduxsub_ao_isoc_irq(struct urb *urb)
 		devpriv->ao_counter = devpriv->ao_timer;
 
 		/* handle non continous acquisition */
-		if (!devpriv->ao_continous) {
+		if (cmd->stop_src == TRIG_COUNT) {
 			/* fixed number of samples */
 			devpriv->ao_sample_count--;
 			if (devpriv->ao_sample_count < 0) {
@@ -478,13 +475,12 @@ static void usbduxsub_ao_isoc_irq(struct urb *urb)
 
 		/* transmit data to the USB bus */
 		datap = urb->transfer_buffer;
-		len = s->async->cmd.chanlist_len;
-		*datap++ = len;
-		for (i = 0; i < s->async->cmd.chanlist_len; i++) {
-			unsigned int chan = devpriv->ao_chanlist[i];
+		*datap++ = cmd->chanlist_len;
+		for (i = 0; i < cmd->chanlist_len; i++) {
+			unsigned int chan = CR_CHAN(cmd->chanlist[i]);
 			unsigned short val;
 
-			ret = comedi_buf_get(s->async, &val);
+			ret = comedi_buf_get(s, &val);
 			if (ret < 0) {
 				dev_err(dev->class_dev, "buffer underflow\n");
 				s->async->events |= (COMEDI_CB_EOA |
@@ -493,7 +489,7 @@ static void usbduxsub_ao_isoc_irq(struct urb *urb)
 			/* pointer to the DA */
 			*datap++ = val & 0xff;
 			*datap++ = (val >> 8) & 0xff;
-			*datap++ = chan;
+			*datap++ = chan << 6;
 			devpriv->ao_readback[chan] = val;
 
 			s->async->events |= COMEDI_CB_BLOCK;
@@ -692,15 +688,16 @@ static int receive_dux_commands(struct comedi_device *dev, unsigned int command)
 
 static int usbdux_ai_inttrig(struct comedi_device *dev,
 			     struct comedi_subdevice *s,
-			     unsigned int trignum)
+			     unsigned int trig_num)
 {
 	struct usbdux_private *devpriv = dev->private;
-	int ret = -EINVAL;
+	struct comedi_cmd *cmd = &s->async->cmd;
+	int ret;
 
-	down(&devpriv->sem);
+	if (trig_num != cmd->start_arg)
+		return -EINVAL;
 
-	if (trignum != 0)
-		goto ai_trig_exit;
+	down(&devpriv->sem);
 
 	if (!devpriv->ai_cmd_running) {
 		devpriv->ai_cmd_running = 1;
@@ -777,10 +774,8 @@ static int usbdux_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	if (cmd->stop_src == TRIG_COUNT) {
 		/* data arrives as one packet */
 		devpriv->ai_sample_count = cmd->stop_arg;
-		devpriv->ai_continous = 0;
 	} else {
 		/* continous acquisition */
-		devpriv->ai_continous = 1;
 		devpriv->ai_sample_count = 0;
 	}
 
@@ -913,15 +908,16 @@ ao_write_exit:
 
 static int usbdux_ao_inttrig(struct comedi_device *dev,
 			     struct comedi_subdevice *s,
-			     unsigned int trignum)
+			     unsigned int trig_num)
 {
 	struct usbdux_private *devpriv = dev->private;
-	int ret = -EINVAL;
+	struct comedi_cmd *cmd = &s->async->cmd;
+	int ret;
 
-	down(&devpriv->sem);
+	if (trig_num != cmd->start_arg)
+		return -EINVAL;
 
-	if (trignum != 0)
-		goto ao_trig_exit;
+	down(&devpriv->sem);
 
 	if (!devpriv->ao_cmd_running) {
 		devpriv->ao_cmd_running = 1;
@@ -1030,7 +1026,6 @@ static int usbdux_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	struct usbdux_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 	int ret = -EBUSY;
-	int i;
 
 	down(&devpriv->sem);
 
@@ -1040,12 +1035,6 @@ static int usbdux_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	/* set current channel of the running acquisition to zero */
 	s->async->cur_chan = 0;
 
-	for (i = 0; i < cmd->chanlist_len; ++i) {
-		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
-
-		devpriv->ao_chanlist[i] = chan << 6;
-	}
-
 	/* we count in steps of 1ms (125us) */
 	/* 125us mode not used yet */
 	if (0) {		/* (devpriv->high_speed) */
@@ -1077,10 +1066,8 @@ static int usbdux_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 			/* data arrives as one packet */
 			devpriv->ao_sample_count = cmd->stop_arg;
 		}
-		devpriv->ao_continous = 0;
 	} else {
 		/* continous acquisition */
-		devpriv->ao_continous = 1;
 		devpriv->ao_sample_count = 0;
 	}
 
diff --git a/drivers/staging/comedi/drivers/usbduxfast.c b/drivers/staging/comedi/drivers/usbduxfast.c
index d6fae11ee4e..85f9dcf5940 100644
--- a/drivers/staging/comedi/drivers/usbduxfast.c
+++ b/drivers/staging/comedi/drivers/usbduxfast.c
@@ -154,7 +154,6 @@ struct usbduxfast_private {
 	uint8_t *duxbuf;
 	int8_t *inbuf;
 	short int ai_cmd_running;	/* asynchronous command is running */
-	short int ai_continous;	/* continous acquisition */
 	long int ai_sample_count;	/* number of samples to acquire */
 	int ignore;		/* counter which ignores the first
 				   buffers */
@@ -239,6 +238,7 @@ static void usbduxfast_ai_interrupt(struct urb *urb)
 	struct comedi_device *dev = urb->context;
 	struct comedi_subdevice *s = dev->read_subdev;
 	struct comedi_async *async = s->async;
+	struct comedi_cmd *cmd = &async->cmd;
 	struct usb_device *usb = comedi_to_usb_dev(dev);
 	struct usbduxfast_private *devpriv = dev->private;
 	int n, err;
@@ -285,7 +285,7 @@ static void usbduxfast_ai_interrupt(struct urb *urb)
 	}
 
 	if (!devpriv->ignore) {
-		if (!devpriv->ai_continous) {
+		if (cmd->stop_src == TRIG_COUNT) {
 			/* not continuous, fixed number of samples */
 			n = urb->actual_length / sizeof(uint16_t);
 			if (unlikely(devpriv->ai_sample_count < n)) {
@@ -398,8 +398,7 @@ static int usbduxfast_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 3: check if arguments are trivially valid */
 
-	if (cmd->start_src == TRIG_NOW)
-		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
 
 	if (!cmd->chanlist_len)
 		err |= -EINVAL;
@@ -451,21 +450,20 @@ static int usbduxfast_ai_cmdtest(struct comedi_device *dev,
 
 static int usbduxfast_ai_inttrig(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
-				 unsigned int trignum)
+				 unsigned int trig_num)
 {
 	struct usbduxfast_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	int ret;
 
 	if (!devpriv)
 		return -EFAULT;
 
+	if (trig_num != cmd->start_arg)
+		return -EINVAL;
+
 	down(&devpriv->sem);
 
-	if (trignum != 0) {
-		dev_err(dev->class_dev, "invalid trignum\n");
-		up(&devpriv->sem);
-		return -EINVAL;
-	}
 	if (!devpriv->ai_cmd_running) {
 		devpriv->ai_cmd_running = 1;
 		ret = usbduxfast_submit_urb(dev);
@@ -811,20 +809,11 @@ static int usbduxfast_ai_cmd(struct comedi_device *dev,
 		up(&devpriv->sem);
 		return result;
 	}
-	if (cmd->stop_src == TRIG_COUNT) {
+
+	if (cmd->stop_src == TRIG_COUNT)
 		devpriv->ai_sample_count = cmd->stop_arg * cmd->scan_end_arg;
-		if (devpriv->ai_sample_count < 1) {
-			dev_err(dev->class_dev,
-				"(cmd->stop_arg)*(cmd->scan_end_arg)<1, aborting\n");
-			up(&devpriv->sem);
-			return -EFAULT;
-		}
-		devpriv->ai_continous = 0;
-	} else {
-		/* continous acquisition */
-		devpriv->ai_continous = 1;
+	else	/* TRIG_NONE */
 		devpriv->ai_sample_count = 0;
-	}
 
 	if ((cmd->start_src == TRIG_NOW) || (cmd->start_src == TRIG_EXT)) {
 		/* enable this acquisition operation */
@@ -837,12 +826,7 @@ static int usbduxfast_ai_cmd(struct comedi_device *dev,
 			return ret;
 		}
 		s->async->inttrig = NULL;
-	} else {
-		/*
-		 * TRIG_INT
-		 * don't enable the acquision operation
-		 * wait for an internal signal
-		 */
+	} else {	/* TRIG_INT */
 		s->async->inttrig = usbduxfast_ai_inttrig;
 	}
 	up(&devpriv->sem);
diff --git a/drivers/staging/comedi/drivers/usbduxsigma.c b/drivers/staging/comedi/drivers/usbduxsigma.c
index 88c60b6020c..ccc3ef7ba55 100644
--- a/drivers/staging/comedi/drivers/usbduxsigma.c
+++ b/drivers/staging/comedi/drivers/usbduxsigma.c
@@ -161,14 +161,11 @@ struct usbduxsigma_private {
 	/* input buffer for single insn */
 	uint8_t *insn_buf;
 
-	uint8_t ao_chanlist[USBDUXSIGMA_NUM_AO_CHAN];
 	unsigned int ao_readback[USBDUXSIGMA_NUM_AO_CHAN];
 
 	unsigned high_speed:1;
 	unsigned ai_cmd_running:1;
-	unsigned ai_continuous:1;
 	unsigned ao_cmd_running:1;
-	unsigned ao_continuous:1;
 	unsigned pwm_cmd_running:1;
 
 	/* number of samples to acquire */
@@ -223,6 +220,7 @@ static void usbduxsigma_ai_urb_complete(struct urb *urb)
 	struct comedi_device *dev = urb->context;
 	struct usbduxsigma_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->read_subdev;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned int dio_state;
 	uint32_t val;
 	int ret;
@@ -301,7 +299,7 @@ static void usbduxsigma_ai_urb_complete(struct urb *urb)
 	/* timer zero, transfer measurements to comedi */
 	devpriv->ai_counter = devpriv->ai_timer;
 
-	if (!devpriv->ai_continuous) {
+	if (cmd->stop_src == TRIG_COUNT) {
 		/* not continuous, fixed number of samples */
 		devpriv->ai_sample_count--;
 		if (devpriv->ai_sample_count < 0) {
@@ -314,7 +312,7 @@ static void usbduxsigma_ai_urb_complete(struct urb *urb)
 	}
 
 	/* get the data from the USB bus and hand it over to comedi */
-	for (i = 0; i < s->async->cmd.chanlist_len; i++) {
+	for (i = 0; i < cmd->chanlist_len; i++) {
 		/* transfer data, note first byte is the DIO state */
 		val = be32_to_cpu(devpriv->in_buf[i+1]);
 		val &= 0x00ffffff;	/* strip status byte */
@@ -360,8 +358,8 @@ static void usbduxsigma_ao_urb_complete(struct urb *urb)
 	struct comedi_device *dev = urb->context;
 	struct usbduxsigma_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->write_subdev;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	uint8_t *datap;
-	int len;
 	int ret;
 	int i;
 
@@ -403,7 +401,7 @@ static void usbduxsigma_ao_urb_complete(struct urb *urb)
 		/* timer zero, transfer from comedi */
 		devpriv->ao_counter = devpriv->ao_timer;
 
-		if (!devpriv->ao_continuous) {
+		if (cmd->stop_src == TRIG_COUNT) {
 			/* not continuous, fixed number of samples */
 			devpriv->ao_sample_count--;
 			if (devpriv->ao_sample_count < 0) {
@@ -417,13 +415,12 @@ static void usbduxsigma_ao_urb_complete(struct urb *urb)
 
 		/* transmit data to the USB bus */
 		datap = urb->transfer_buffer;
-		len = s->async->cmd.chanlist_len;
-		*datap++ = len;
-		for (i = 0; i < len; i++) {
-			unsigned int chan = devpriv->ao_chanlist[i];
+		*datap++ = cmd->chanlist_len;
+		for (i = 0; i < cmd->chanlist_len; i++) {
+			unsigned int chan = CR_CHAN(cmd->chanlist[i]);
 			unsigned short val;
 
-			ret = comedi_buf_get(s->async, &val);
+			ret = comedi_buf_get(s, &val);
 			if (ret < 0) {
 				dev_err(dev->class_dev, "buffer underflow\n");
 				s->async->events |= (COMEDI_CB_EOA |
@@ -596,10 +593,8 @@ static int usbduxsigma_ai_cmdtest(struct comedi_device *dev,
 	if (cmd->stop_src == TRIG_COUNT) {
 		/* data arrives as one packet */
 		devpriv->ai_sample_count = cmd->stop_arg;
-		devpriv->ai_continuous = 0;
 	} else {
 		/* continuous acquisition */
-		devpriv->ai_continuous = 1;
 		devpriv->ai_sample_count = 0;
 	}
 
@@ -663,12 +658,13 @@ static int usbduxsigma_receive_cmd(struct comedi_device *dev, int command)
 
 static int usbduxsigma_ai_inttrig(struct comedi_device *dev,
 				  struct comedi_subdevice *s,
-				  unsigned int trignum)
+				  unsigned int trig_num)
 {
 	struct usbduxsigma_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	int ret;
 
-	if (trignum != 0)
+	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
 	down(&devpriv->sem);
@@ -738,7 +734,6 @@ static int usbduxsigma_ai_cmd(struct comedi_device *dev,
 		}
 		s->async->inttrig = NULL;
 	} else {	/* TRIG_INT */
-		/* wait for an internal signal and submit the urbs later */
 		s->async->inttrig = usbduxsigma_ai_inttrig;
 	}
 
@@ -856,12 +851,13 @@ static int usbduxsigma_ao_insn_write(struct comedi_device *dev,
 
 static int usbduxsigma_ao_inttrig(struct comedi_device *dev,
 				  struct comedi_subdevice *s,
-				  unsigned int trignum)
+				  unsigned int trig_num)
 {
 	struct usbduxsigma_private *devpriv = dev->private;
+	struct comedi_cmd *cmd = &s->async->cmd;
 	int ret;
 
-	if (trignum != 0)
+	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
 	down(&devpriv->sem);
@@ -985,10 +981,8 @@ static int usbduxsigma_ao_cmdtest(struct comedi_device *dev,
 			 */
 			devpriv->ao_sample_count = cmd->stop_arg;
 		}
-		devpriv->ao_continuous = 0;
 	} else {
 		/* continuous acquisition */
-		devpriv->ao_continuous = 1;
 		devpriv->ao_sample_count = 0;
 	}
 
@@ -1004,14 +998,11 @@ static int usbduxsigma_ao_cmd(struct comedi_device *dev,
 	struct usbduxsigma_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 	int ret;
-	int i;
 
 	down(&devpriv->sem);
 
 	/* set current channel of the running acquisition to zero */
 	s->async->cur_chan = 0;
-	for (i = 0; i < cmd->chanlist_len; ++i)
-		devpriv->ao_chanlist[i] = CR_CHAN(cmd->chanlist[i]);
 
 	devpriv->ao_counter = devpriv->ao_timer;
 
@@ -1027,7 +1018,6 @@ static int usbduxsigma_ao_cmd(struct comedi_device *dev,
 		}
 		s->async->inttrig = NULL;
 	} else {	/* TRIG_INT */
-		/* wait for an internal signal and submit the urbs later */
 		s->async->inttrig = usbduxsigma_ao_inttrig;
 	}
 
diff --git a/drivers/staging/crystalhd/crystalhd_lnx.c b/drivers/staging/crystalhd/crystalhd_lnx.c
index 20be9571990..e6fb331c673 100644
--- a/drivers/staging/crystalhd/crystalhd_lnx.c
+++ b/drivers/staging/crystalhd/crystalhd_lnx.c
@@ -28,7 +28,7 @@ static struct crystalhd_adp *g_adp_info;
 
 static irqreturn_t chd_dec_isr(int irq, void *arg)
 {
-	struct crystalhd_adp *adp = (struct crystalhd_adp *) arg;
+	struct crystalhd_adp *adp = arg;
 	int rc = 0;
 	if (adp)
 		rc = crystalhd_cmd_interrupt(&adp->cmds);
@@ -112,7 +112,7 @@ static void chd_dec_free_iodata(struct crystalhd_adp *adp,
 }
 
 static inline int crystalhd_user_data(void __user *ud, void *dr,
-			 int size, int set)
+				      int size, int set)
 {
 	int rc;
 
@@ -135,7 +135,8 @@ static inline int crystalhd_user_data(void __user *ud, void *dr,
 }
 
 static int chd_dec_fetch_cdata(struct crystalhd_adp *adp,
-	 struct crystalhd_ioctl_data *io, uint32_t m_sz, unsigned long ua)
+			       struct crystalhd_ioctl_data *io, uint32_t m_sz,
+			       unsigned long ua)
 {
 	unsigned long ua_off;
 	int rc = 0;
@@ -154,7 +155,7 @@ static int chd_dec_fetch_cdata(struct crystalhd_adp *adp,
 	io->add_cdata_sz = m_sz;
 	ua_off = ua + sizeof(io->udata);
 	rc = crystalhd_user_data((void __user *)ua_off, io->add_cdata,
-			io->add_cdata_sz, 0);
+				 io->add_cdata_sz, 0);
 	if (rc) {
 		BCMLOG_ERR("failed to pull add_cdata sz:%x ua_off:%x\n",
 			   io->add_cdata_sz, (unsigned int)ua_off);
@@ -167,7 +168,8 @@ static int chd_dec_fetch_cdata(struct crystalhd_adp *adp,
 }
 
 static int chd_dec_release_cdata(struct crystalhd_adp *adp,
-			 struct crystalhd_ioctl_data *io, unsigned long ua)
+				 struct crystalhd_ioctl_data *io,
+				 unsigned long ua)
 {
 	unsigned long ua_off;
 	int rc;
@@ -180,7 +182,7 @@ static int chd_dec_release_cdata(struct crystalhd_adp *adp,
 	if (io->cmd != BCM_IOC_FW_DOWNLOAD) {
 		ua_off = ua + sizeof(io->udata);
 		rc = crystalhd_user_data((void __user *)ua_off, io->add_cdata,
-					io->add_cdata_sz, 1);
+					 io->add_cdata_sz, 1);
 		if (rc) {
 			BCMLOG_ERR(
 				"failed to push add_cdata sz:%x ua_off:%x\n",
@@ -210,7 +212,7 @@ static int chd_dec_proc_user_data(struct crystalhd_adp *adp,
 	}
 
 	rc = crystalhd_user_data((void __user *)ua, &io->udata,
-			sizeof(io->udata), set);
+				 sizeof(io->udata), set);
 	if (rc) {
 		BCMLOG_ERR("failed to %s iodata\n", (set ? "set" : "get"));
 		return rc;
@@ -382,7 +384,7 @@ static int chd_dec_init_chdev(struct crystalhd_adp *adp)
 	}
 
 	dev = device_create(crystalhd_class, NULL,
-			 MKDEV(adp->chd_dec_major, 0), NULL, "crystalhd");
+			    MKDEV(adp->chd_dec_major, 0), NULL, "crystalhd");
 	if (IS_ERR(dev)) {
 		rc = PTR_ERR(dev);
 		BCMLOG_ERR("failed to create device\n");
@@ -397,8 +399,7 @@ static int chd_dec_init_chdev(struct crystalhd_adp *adp)
 
 	/* Allocate general purpose ioctl pool. */
 	for (i = 0; i < CHD_IODATA_POOL_SZ; i++) {
-		temp = kzalloc(sizeof(struct crystalhd_ioctl_data),
-					 GFP_KERNEL);
+		temp = kzalloc(sizeof(*temp), GFP_KERNEL);
 		if (!temp) {
 			BCMLOG_ERR("ioctl data pool kzalloc failed\n");
 			rc = -ENOMEM;
@@ -549,11 +550,11 @@ static int chd_dec_pci_probe(struct pci_dev *pdev,
 	enum BC_STATUS sts = BC_STS_SUCCESS;
 
 	BCMLOG(BCMLOG_DBG,
-		"PCI_INFO: Vendor:0x%04x Device:0x%04x s_vendor:0x%04x s_device: 0x%04x\n",
-		pdev->vendor, pdev->device, pdev->subsystem_vendor,
-		pdev->subsystem_device);
+	       "PCI_INFO: Vendor:0x%04x Device:0x%04x s_vendor:0x%04x s_device: 0x%04x\n",
+	       pdev->vendor, pdev->device, pdev->subsystem_vendor,
+	       pdev->subsystem_device);
 
-	pinfo = kzalloc(sizeof(struct crystalhd_adp), GFP_KERNEL);
+	pinfo = kzalloc(sizeof(*pinfo), GFP_KERNEL);
 	if (!pinfo) {
 		BCMLOG_ERR("Failed to allocate memory\n");
 		return -ENOMEM;
diff --git a/drivers/staging/cxt1e1/functions.c b/drivers/staging/cxt1e1/functions.c
index ee9d39bbd25..65b6fc36edf 100644
--- a/drivers/staging/cxt1e1/functions.c
+++ b/drivers/staging/cxt1e1/functions.c
@@ -24,24 +24,6 @@
 #include "libsbew.h"
 #include "pmcc4.h"
 
-#if defined(CONFIG_SBE_HDLC_V7) || defined(CONFIG_SBE_WAN256T3_HDLC_V7) || \
-defined(CONFIG_SBE_HDLC_V7_MODULE) || \
-defined(CONFIG_SBE_WAN256T3_HDLC_V7_MODULE)
-#define _v7_hdlc_  1
-#else
-#define _v7_hdlc_  0
-#endif
-
-#if _v7_hdlc_
-#define V7(x) (x ## _v7)
-extern int  hdlc_netif_rx_v7(hdlc_device *, struct sk_buff *);
-extern int  register_hdlc_device_v7(hdlc_device *);
-extern int  unregister_hdlc_device_v7(hdlc_device *);
-
-#else
-#define V7(x) x
-#endif
-
 
 #ifndef USE_MAX_INT_DELAY
 static int  dummy = 0;
diff --git a/drivers/staging/cxt1e1/hwprobe.c b/drivers/staging/cxt1e1/hwprobe.c
index 9b4198b1e63..4fa27c8931b 100644
--- a/drivers/staging/cxt1e1/hwprobe.c
+++ b/drivers/staging/cxt1e1/hwprobe.c
@@ -159,8 +159,8 @@ prep_hdw_info(void)
 		hi->ndev = NULL;
 		hi->addr[0] = 0L;
 		hi->addr[1] = 0L;
-		hi->addr_mapped[0] = 0L;
-		hi->addr_mapped[1] = 0L;
+		hi->addr_mapped[0] = NULL;
+		hi->addr_mapped[1] = NULL;
 	}
 }
 
@@ -174,14 +174,14 @@ cleanup_ioremap(void)
 		if (hi->pci_slot == 0xff)
 			break;
 		if (hi->addr_mapped[0]) {
-			iounmap((void *)(hi->addr_mapped[0]));
+			iounmap(hi->addr_mapped[0]);
 			release_mem_region((long) hi->addr[0], hi->len[0]);
-			hi->addr_mapped[0] = 0;
+			hi->addr_mapped[0] = NULL;
 		}
 		if (hi->addr_mapped[1]) {
-			iounmap((void *)(hi->addr_mapped[1]));
+			iounmap(hi->addr_mapped[1]);
 			release_mem_region((long) hi->addr[1], hi->len[1]);
-			hi->addr_mapped[1] = 0;
+			hi->addr_mapped[1] = NULL;
 		}
 	}
 }
@@ -205,7 +205,7 @@ cleanup_devs(void)
 #ifdef CONFIG_SBE_PMCC4_NCOMM
 		free_irq(hi->pdev[1]->irq, hi->ndev);
 #endif
-		OS_kfree(hi->ndev);
+		kfree(hi->ndev);
 	}
 }
 
@@ -329,7 +329,7 @@ c4hw_attach_all(void)
 				return -ENOMEM;
 			}
 
-			hi->addr_mapped[j] = (unsigned long)ioremap(hi->addr[j], hi->len[j]);
+			hi->addr_mapped[j] = ioremap(hi->addr[j], hi->len[j]);
 			if (!hi->addr_mapped[j]) {
 				pr_warning("%s: ioremap fails, addr=0x%lx, len=0x%lx ?\n",
 					   hi->devname, hi->addr[j], hi->len[j]);
diff --git a/drivers/staging/cxt1e1/linux.c b/drivers/staging/cxt1e1/linux.c
index b02f5ade666..09f3d5ca75a 100644
--- a/drivers/staging/cxt1e1/linux.c
+++ b/drivers/staging/cxt1e1/linux.c
@@ -60,7 +60,6 @@ status_t    c4_chan_work_init(mpi_t *, mch_t *);
 void        musycc_wq_chan_restart(void *);
 status_t __init c4_init(ci_t *, u_char *, u_char *);
 status_t __init c4_init2(ci_t *);
-ci_t       *__init c4_new(void *);
 int __init  c4hw_attach_all(void);
 void __init hdw_sn_get(hdw_info_t *, int);
 
@@ -84,23 +83,6 @@ int         musycc_start_xmit(ci_t *, int, void *);
 extern ci_t *CI;
 extern struct s_hdw_info hdw_info[];
 
-#if defined(CONFIG_SBE_HDLC_V7) || defined(CONFIG_SBE_WAN256T3_HDLC_V7) || \
-	defined(CONFIG_SBE_HDLC_V7_MODULE) || defined(CONFIG_SBE_WAN256T3_HDLC_V7_MODULE)
-#define _v7_hdlc_  1
-#else
-#define _v7_hdlc_  0
-#endif
-
-#if _v7_hdlc_
-#define V7(x) (x ## _v7)
-extern int  hdlc_netif_rx_v7(hdlc_device *, struct sk_buff *);
-extern int  register_hdlc_device_v7(hdlc_device *);
-extern int  unregister_hdlc_device_v7(hdlc_device *);
-
-#else
-#define V7(x) x
-#endif
-
 int         error_flag;         /* module load error reporting */
 int         cxt1e1_log_level = LOG_ERROR;
 static int  log_level_default = LOG_ERROR;
@@ -418,7 +400,7 @@ create_chan(struct net_device *ndev, ci_t *ci,
 		struct c4_priv *priv;
 
 		/* allocate then fill in private data structure */
-		priv = OS_kmalloc(sizeof(struct c4_priv));
+		priv = kzalloc(sizeof(struct c4_priv), GFP_KERNEL);
 		if (!priv) {
 			pr_warning("%s: no memory for net_device !\n",
 				   ci->devname);
@@ -428,7 +410,7 @@ create_chan(struct net_device *ndev, ci_t *ci,
 		if (!dev) {
 			pr_warning("%s: no memory for hdlc_device !\n",
 				   ci->devname);
-			OS_kfree(priv);
+			kfree(priv);
 			return NULL;
 		}
 		priv->ci = ci;
@@ -972,8 +954,8 @@ c4_add_dev(hdw_info_t *hi, int brdno, unsigned long f0, unsigned long f1,
 
 	if (register_netdev(ndev) ||
 		(c4_init(ci, (u_char *) f0, (u_char *) f1) != SBE_DRVR_SUCCESS)) {
-		OS_kfree(netdev_priv(ndev));
-		OS_kfree(ndev);
+		kfree(netdev_priv(ndev));
+		kfree(ndev);
 		error_flag = -ENODEV;
 		return NULL;
 	}
@@ -998,8 +980,8 @@ c4_add_dev(hdw_info_t *hi, int brdno, unsigned long f0, unsigned long f1,
 		pr_warning("%s: MUSYCC could not get irq: %d\n",
 			   ndev->name, irq0);
 		unregister_netdev(ndev);
-		OS_kfree(netdev_priv(ndev));
-		OS_kfree(ndev);
+		kfree(netdev_priv(ndev));
+		kfree(ndev);
 		error_flag = -EIO;
 		return NULL;
 	}
@@ -1008,8 +990,8 @@ c4_add_dev(hdw_info_t *hi, int brdno, unsigned long f0, unsigned long f1,
 		pr_warning("%s: EBUS could not get irq: %d\n", hi->devname, irq1);
 		unregister_netdev(ndev);
 		free_irq(irq0, ndev);
-		OS_kfree(netdev_priv(ndev));
-		OS_kfree(ndev);
+		kfree(netdev_priv(ndev));
+		kfree(ndev);
 		error_flag = -EIO;
 		return NULL;
 	}
@@ -1068,8 +1050,8 @@ c4_add_dev(hdw_info_t *hi, int brdno, unsigned long f0, unsigned long f1,
 		unregister_netdev(ndev);
 		free_irq(irq1, ndev);
 		free_irq(irq0, ndev);
-		OS_kfree(netdev_priv(ndev));
-		OS_kfree(ndev);
+		kfree(netdev_priv(ndev));
+		kfree(ndev);
 		/* failure, error_flag is set */
 		return NULL;
 	}
diff --git a/drivers/staging/cxt1e1/musycc.c b/drivers/staging/cxt1e1/musycc.c
index 7b4f6f2108e..0bcbd8a3fc8 100644
--- a/drivers/staging/cxt1e1/musycc.c
+++ b/drivers/staging/cxt1e1/musycc.c
@@ -1,5 +1,5 @@
-static unsigned int max_intcnt = 0;
-static unsigned int max_bh = 0;
+static unsigned int max_intcnt;
+static unsigned int max_bh;
 
 /*-----------------------------------------------------------------------------
  * musycc.c -
@@ -64,132 +64,134 @@ void        musycc_update_timeslots(mpi_t *);
 
 /*******************************************************************/
 
-#if 1
 static int
 musycc_dump_rxbuffer_ring(mch_t *ch, int lockit)
 {
-    struct mdesc *m;
-    unsigned long flags = 0;
+	struct mdesc *m;
+	unsigned long flags = 0;
 
-    u_int32_t status;
-    int         n;
+	u_int32_t status;
+	int         n;
 
-    if (lockit)
-	spin_lock_irqsave(&ch->ch_rxlock, flags);
-    if (ch->rxd_num == 0)
-	pr_info("  ZERO receive buffers allocated for this channel.");
-    else {
-	FLUSH_MEM_READ();
-	m = &ch->mdr[ch->rxix_irq_srv];
-	for (n = ch->rxd_num; n; n--) {
-	    status = le32_to_cpu(m->status);
-	    {
-		pr_info("%c  %08lx[%2d]: sts %08x (%c%c%c%c:%d.) Data [%08x] Next [%08x]\n",
-			(m == &ch->mdr[ch->rxix_irq_srv]) ? 'F' : ' ',
-			(unsigned long) m, n,
-			status,
-			m->data ? (status & HOST_RX_OWNED ? 'H' : 'M') : '-',
-			status & POLL_DISABLED ? 'P' : '-',
-			status & EOBIRQ_ENABLE ? 'b' : '-',
-			status & EOMIRQ_ENABLE ? 'm' : '-',
-			status & LENGTH_MASK,
-			le32_to_cpu(m->data), le32_to_cpu(m->next));
 #ifdef RLD_DUMP_BUFDATA
-		{
-		    u_int32_t  *dp;
-		    int         len = status & LENGTH_MASK;
+	u_int32_t *dp;
+	int len = 0;
+#endif
+	if (lockit)
+		spin_lock_irqsave(&ch->ch_rxlock, flags);
+	if (ch->rxd_num == 0)
+		pr_info("  ZERO receive buffers allocated for this channel.");
+	else {
+		FLUSH_MEM_READ();
+		m = &ch->mdr[ch->rxix_irq_srv];
+		for (n = ch->rxd_num; n; n--) {
+			status = le32_to_cpu(m->status);
+			pr_info("%c  %08lx[%2d]: sts %08x (%c%c%c%c:%d.) Data [%08x] Next [%08x]\n",
+				(m == &ch->mdr[ch->rxix_irq_srv]) ? 'F' : ' ',
+				(unsigned long) m, n,
+				status,
+				m->data ? (status & HOST_RX_OWNED ? 'H' : 'M') : '-',
+				status & POLL_DISABLED ? 'P' : '-',
+				status & EOBIRQ_ENABLE ? 'b' : '-',
+				status & EOMIRQ_ENABLE ? 'm' : '-',
+				status & LENGTH_MASK,
+				le32_to_cpu(m->data), le32_to_cpu(m->next));
+#ifdef RLD_DUMP_BUFDATA
+				len = status & LENGTH_MASK;
 
 #if 1
-		    if (m->data && (status & HOST_RX_OWNED))
+				if (m->data && (status & HOST_RX_OWNED))
 #else
-		    if (m->data)    /* always dump regardless of valid RX
-				     * data */
+				/* always dump regardless of valid RX data */
+				if (m->data)
 #endif
-		    {
-			dp = (u_int32_t *) OS_phystov((void *) (le32_to_cpu(m->data)));
-			if (len >= 0x10)
-			    pr_info("    %x[%x]: %08X %08X %08X %08x\n", (u_int32_t) dp, len,
-				    *dp, *(dp + 1), *(dp + 2), *(dp + 3));
-			else if (len >= 0x08)
-			    pr_info("    %x[%x]: %08X %08X\n", (u_int32_t) dp, len,
-				    *dp, *(dp + 1));
-			else
-			    pr_info("    %x[%x]: %08X\n", (u_int32_t) dp, len, *dp);
-		    }
-		}
+				{
+					dp = (u_int32_t *)OS_phystov((void *)(le32_to_cpu(m->data)));
+					if (len >= 0x10)
+						pr_info("    %x[%x]: %08X %08X %08X %08x\n",
+							(u_int32_t)dp, len,
+							*dp, *(dp + 1),
+							*(dp + 2), *(dp + 3));
+					else if (len >= 0x08)
+						pr_info("    %x[%x]: %08X %08X\n",
+							(u_int32_t)dp, len,
+							*dp, *(dp + 1));
+					else
+						pr_info("    %x[%x]: %08X\n",
+							(u_int32_t)dp,
+							len, *dp);
+				}
 #endif
-	    }
-	    m = m->snext;
+			m = m->snext;
+		}
 	}
-    }                               /* -for- */
-    pr_info("\n");
+	pr_info("\n");
 
-    if (lockit)
-	spin_unlock_irqrestore(&ch->ch_rxlock, flags);
-    return 0;
+	if (lockit)
+		spin_unlock_irqrestore(&ch->ch_rxlock, flags);
+	return 0;
 }
-#endif
 
-#if 1
 static int
 musycc_dump_txbuffer_ring(mch_t *ch, int lockit)
 {
-    struct mdesc *m;
-    unsigned long flags = 0;
-    u_int32_t   status;
-    int         n;
-
-    if (lockit)
-	spin_lock_irqsave(&ch->ch_txlock, flags);
-    if (ch->txd_num == 0)
-	pr_info("  ZERO transmit buffers allocated for this channel.");
-    else {
-	FLUSH_MEM_READ();
-	m = ch->txd_irq_srv;
-	for (n = ch->txd_num; n; n--) {
-	    status = le32_to_cpu(m->status);
-	    {
-		pr_info("%c%c %08lx[%2d]: sts %08x (%c%c%c%c:%d.) Data [%08x] Next [%08x]\n",
-			(m == ch->txd_usr_add) ? 'F' : ' ',
-			(m == ch->txd_irq_srv) ? 'L' : ' ',
-			(unsigned long) m, n,
-			status,
-		     m->data ? (status & MUSYCC_TX_OWNED ? 'M' : 'H') : '-',
-			status & POLL_DISABLED ? 'P' : '-',
-			status & EOBIRQ_ENABLE ? 'b' : '-',
-			status & EOMIRQ_ENABLE ? 'm' : '-',
-			status & LENGTH_MASK,
-			le32_to_cpu(m->data), le32_to_cpu(m->next));
+	struct mdesc *m;
+	unsigned long flags = 0;
+	u_int32_t   status;
+	int         n;
 #ifdef RLD_DUMP_BUFDATA
-		{
-		    u_int32_t  *dp;
-		    int         len = status & LENGTH_MASK;
-
-		    if (m->data) {
-			dp = (u_int32_t *) OS_phystov((void *) (le32_to_cpu(m->data)));
-			if (len >= 0x10)
-			    pr_info("    %x[%x]: %08X %08X %08X %08x\n", (u_int32_t) dp, len,
-				    *dp, *(dp + 1), *(dp + 2), *(dp + 3));
-			else if (len >= 0x08)
-			    pr_info("    %x[%x]: %08X %08X\n", (u_int32_t) dp, len,
-				    *dp, *(dp + 1));
-			else
-			    pr_info("    %x[%x]: %08X\n", (u_int32_t) dp, len, *dp);
-		    }
-		}
+	u_int32_t *dp;
+	int len = 0;
 #endif
-	    }
-	    m = m->snext;
-	}
-    }                               /* -for- */
-    pr_info("\n");
 
-    if (lockit)
-	spin_unlock_irqrestore(&ch->ch_txlock, flags);
-    return 0;
-}
+	if (lockit)
+		spin_lock_irqsave(&ch->ch_txlock, flags);
+	if (ch->txd_num == 0)
+		pr_info("  ZERO transmit buffers allocated for this channel.");
+	else {
+		FLUSH_MEM_READ();
+		m = ch->txd_irq_srv;
+		for (n = ch->txd_num; n; n--) {
+			status = le32_to_cpu(m->status);
+			pr_info("%c%c %08lx[%2d]: sts %08x (%c%c%c%c:%d.) Data [%08x] Next [%08x]\n",
+				(m == ch->txd_usr_add) ? 'F' : ' ',
+				(m == ch->txd_irq_srv) ? 'L' : ' ',
+				(unsigned long) m, n,
+				status,
+				m->data ? (status & MUSYCC_TX_OWNED ? 'M' : 'H') : '-',
+				status & POLL_DISABLED ? 'P' : '-',
+				status & EOBIRQ_ENABLE ? 'b' : '-',
+				status & EOMIRQ_ENABLE ? 'm' : '-',
+				status & LENGTH_MASK,
+				le32_to_cpu(m->data), le32_to_cpu(m->next));
+#ifdef RLD_DUMP_BUFDATA
+			len = status & LENGTH_MASK;
+
+			if (m->data) {
+				dp = (u_int32_t *)OS_phystov((void *)(le32_to_cpu(m->data)));
+				if (len >= 0x10)
+					pr_info("    %x[%x]: %08X %08X %08X %08x\n",
+						(u_int32_t) dp, len,
+						*dp, *(dp + 1),
+						*(dp + 2), *(dp + 3));
+				else if (len >= 0x08)
+					pr_info("    %x[%x]: %08X %08X\n",
+						(u_int32_t)dp, len,
+						*dp, *(dp + 1));
+				else
+					pr_info("    %x[%x]: %08X\n",
+						(u_int32_t)dp, len, *dp);
+			}
 #endif
+			m = m->snext;
+		}
+	}                               /* -for- */
+	pr_info("\n");
 
+	if (lockit)
+		spin_unlock_irqrestore(&ch->ch_txlock, flags);
+	return 0;
+}
 
 /*
  * The following supports a backdoor debug facility which can be used to
@@ -199,12 +201,11 @@ musycc_dump_txbuffer_ring(mch_t *ch, int lockit)
 status_t
 musycc_dump_ring(ci_t *ci, unsigned int chan)
 {
-    mch_t      *ch;
+	mch_t      *ch;
+	int bh;
 
-    if (chan >= MAX_CHANS_USED)
-	return SBE_DRVR_FAIL;       /* E2BIG */
-    {
-	int         bh;
+	if (chan >= MAX_CHANS_USED)
+		return SBE_DRVR_FAIL;       /* E2BIG */
 
 	bh = atomic_read(&ci->bh_pending);
 	pr_info(">> bh_pend %d [%d] ihead %d itail %d [%d] th_cnt %d bh_cnt %d wdcnt %d note %d\n",
@@ -214,40 +215,43 @@ musycc_dump_ring(ci_t *ci, unsigned int chan)
 		ci->wdcount, ci->wd_notify);
 	max_bh = 0;                 /* reset counter */
 	max_intcnt = 0;             /* reset counter */
-    }
-
-    ch = sd_find_chan(dummy, chan);
-    if (!ch) {
-	pr_info(">> musycc_dump_ring: channel %d not up.\n", chan);
-	return ENOENT;
-    }
-    pr_info(">> CI %p CHANNEL %3d @ %p: state %x status/p %x/%x\n", ci, chan, ch, ch->state,
-	    ch->status, ch->p.status);
-    pr_info("--------------------------------\nTX Buffer Ring - Channel %d, txd_num %d. (bd/ch pend %d %d), TXD required %d, txpkt %lu\n",
-	    chan, ch->txd_num,
-	    (u_int32_t) atomic_read(&ci->tx_pending), (u_int32_t) atomic_read(&ch->tx_pending), ch->txd_required, ch->s.tx_packets);
-    pr_info("++ User 0x%p IRQ_SRV 0x%p USR_ADD 0x%p QStopped %x, start_tx %x tx_full %d txd_free %d mode %x\n",
-	    ch->user, ch->txd_irq_srv, ch->txd_usr_add,
-	    sd_queue_stopped(ch->user),
-	    ch->ch_start_tx, ch->tx_full, ch->txd_free, ch->p.chan_mode);
-    musycc_dump_txbuffer_ring(ch, 1);
-    pr_info("RX Buffer Ring - Channel %d, rxd_num %d. IRQ_SRV[%d] 0x%p, start_rx %x rxpkt %lu\n",
-	    chan, ch->rxd_num, ch->rxix_irq_srv,
-	    &ch->mdr[ch->rxix_irq_srv], ch->ch_start_rx, ch->s.rx_packets);
-    musycc_dump_rxbuffer_ring(ch, 1);
-
-    return SBE_DRVR_SUCCESS;
+
+	ch = sd_find_chan(dummy, chan);
+	if (!ch) {
+		pr_info(">> musycc_dump_ring: channel %d not up.\n", chan);
+		return ENOENT;
+	}
+	pr_info(">> CI %p CHANNEL %3d @ %p: state %x status/p %x/%x\n",
+		ci, chan, ch, ch->state,
+		ch->status, ch->p.status);
+	pr_info("--------------------------------\n");
+	pr_info("TX Buffer Ring - Channel %d, txd_num %d. (bd/ch pend %d %d), TXD required %d, txpkt %lu\n",
+		chan, ch->txd_num,
+		(u_int32_t)atomic_read(&ci->tx_pending),
+		(u_int32_t)atomic_read(&ch->tx_pending),
+		ch->txd_required, ch->s.tx_packets);
+	pr_info("++ User 0x%p IRQ_SRV 0x%p USR_ADD 0x%p QStopped %x, start_tx %x tx_full %d txd_free %d mode %x\n",
+		ch->user, ch->txd_irq_srv, ch->txd_usr_add,
+		sd_queue_stopped(ch->user),
+		ch->ch_start_tx, ch->tx_full, ch->txd_free, ch->p.chan_mode);
+	musycc_dump_txbuffer_ring(ch, 1);
+	pr_info("RX Buffer Ring - Channel %d, rxd_num %d. IRQ_SRV[%d] 0x%p, start_rx %x rxpkt %lu\n",
+		chan, ch->rxd_num, ch->rxix_irq_srv,
+		&ch->mdr[ch->rxix_irq_srv], ch->ch_start_rx, ch->s.rx_packets);
+	musycc_dump_rxbuffer_ring(ch, 1);
+
+	return SBE_DRVR_SUCCESS;
 }
 
 
 status_t
 musycc_dump_rings(ci_t *ci, unsigned int start_chan)
 {
-    unsigned int chan;
+	unsigned int chan;
 
-    for (chan = start_chan; chan < (start_chan + 5); chan++)
-	musycc_dump_ring(ci, chan);
-    return SBE_DRVR_SUCCESS;
+	for (chan = start_chan; chan < (start_chan + 5); chan++)
+		musycc_dump_ring(ci, chan);
+	return SBE_DRVR_SUCCESS;
 }
 
 
@@ -259,22 +263,22 @@ musycc_dump_rings(ci_t *ci, unsigned int start_chan)
 void
 musycc_init_mdt(mpi_t *pi)
 {
-    u_int32_t  *addr, cfg;
-    int         i;
-
-    /*
-     * This Idle Code insertion takes effect prior to channel's first
-     * transmitted  message.  After that, each message contains its own Idle
-     * Code information which is to be issued after the message is
-     * transmitted (Ref.MUSYCC 5.2.2.3: MCENBL bit in Group Configuration
-     * Descriptor).
-     */
-
-    addr = (u_int32_t *) ((u_long) pi->reg + MUSYCC_MDT_BASE03_ADDR);
-    cfg = CFG_CH_FLAG_7E << IDLE_CODE;
-
-    for (i = 0; i < 32; addr++, i++)
-	pci_write_32(addr, cfg);
+	u_int32_t  *addr, cfg;
+	int         i;
+
+	/*
+	 * This Idle Code insertion takes effect prior to channel's first
+	 * transmitted  message.  After that, each message contains its own Idle
+	 * Code information which is to be issued after the message is
+	 * transmitted (Ref.MUSYCC 5.2.2.3: MCENBL bit in Group Configuration
+	 * Descriptor).
+	 */
+
+	addr = (u_int32_t *) ((u_long) pi->reg + MUSYCC_MDT_BASE03_ADDR);
+	cfg = CFG_CH_FLAG_7E << IDLE_CODE;
+
+	for (i = 0; i < 32; addr++, i++)
+		pci_write_32(addr, cfg);
 }
 
 
@@ -283,44 +287,45 @@ musycc_init_mdt(mpi_t *pi)
 void
 musycc_update_tx_thp(mch_t *ch)
 {
-    struct mdesc *md;
-    unsigned long flags;
+	struct mdesc *md;
+	unsigned long flags;
 
-    spin_lock_irqsave(&ch->ch_txlock, flags);
-    while (1) {
-	md = ch->txd_irq_srv;
-	FLUSH_MEM_READ();
-	if (!md->data) {
-	    /* No MDs with buffers to process */
-	    spin_unlock_irqrestore(&ch->ch_txlock, flags);
-	    return;
+	spin_lock_irqsave(&ch->ch_txlock, flags);
+	while (1) {
+		md = ch->txd_irq_srv;
+		FLUSH_MEM_READ();
+		if (!md->data) {
+			/* No MDs with buffers to process */
+			spin_unlock_irqrestore(&ch->ch_txlock, flags);
+			return;
+		}
+		if ((le32_to_cpu(md->status)) & MUSYCC_TX_OWNED) {
+			/* this is the MD to restart TX with */
+			break;
+		}
+		/*
+		 * Otherwise, we have a valid, host-owned message descriptor which
+		 * has been successfully transmitted and whose buffer can be freed,
+		 * so... process this MD, it's owned by the host.  (This might give
+		 * as a new, updated txd_irq_srv.)
+		 */
+		musycc_bh_tx_eom(ch->up, ch->gchan);
 	}
-	if ((le32_to_cpu(md->status)) & MUSYCC_TX_OWNED) {
-	    /* this is the MD to restart TX with */
-	    break;
+	md = ch->txd_irq_srv;
+	ch->up->regram->thp[ch->gchan] = cpu_to_le32(OS_vtophys(md));
+	FLUSH_MEM_WRITE();
+
+	if (ch->tx_full) {
+		ch->tx_full = 0;
+		ch->txd_required = 0;
+		sd_enable_xmit(ch->user);  /* re-enable to catch flow controlled
+					    * channel */
 	}
-	/*
-	 * Otherwise, we have a valid, host-owned message descriptor which
-	 * has been successfully transmitted and whose buffer can be freed,
-	 * so... process this MD, it's owned by the host.  (This might give
-	 * as a new, updated txd_irq_srv.)
-	 */
-	musycc_bh_tx_eom(ch->up, ch->gchan);
-    }
-    md = ch->txd_irq_srv;
-    ch->up->regram->thp[ch->gchan] = cpu_to_le32(OS_vtophys(md));
-    FLUSH_MEM_WRITE();
-
-    if (ch->tx_full) {
-	ch->tx_full = 0;
-	ch->txd_required = 0;
-	sd_enable_xmit(ch->user);  /* re-enable to catch flow controlled
-				     * channel */
-    }
-    spin_unlock_irqrestore(&ch->ch_txlock, flags);
+	spin_unlock_irqrestore(&ch->ch_txlock, flags);
 
 #ifdef RLD_TRANS_DEBUG
-    pr_info("++ musycc_update_tx_thp[%d]: setting thp = %p, sts %x\n", ch->channum, md, md->status);
+	pr_info("++ musycc_update_tx_thp[%d]: setting thp = %p, sts %x\n",
+		ch->channum, md, md->status);
 #endif
 }
 
@@ -337,96 +342,88 @@ musycc_update_tx_thp(mch_t *ch)
 void
 musycc_wq_chan_restart(void *arg)      /* channel private structure */
 {
-    mch_t      *ch;
-    mpi_t      *pi;
-    struct mdesc *md;
-#if 0
-    unsigned long flags;
+	mch_t      *ch;
+	mpi_t      *pi;
+	struct mdesc *md;
+
+#if defined(RLD_TRANS_DEBUG) || defined(RLD_RXACT_DEBUG)
+	static int hereb4 = 7;
 #endif
 
-    ch = container_of(arg, struct c4_chan_info, ch_work);
-    pi = ch->up;
+	ch = container_of(arg, struct c4_chan_info, ch_work);
+	pi = ch->up;
 
 #ifdef RLD_TRANS_DEBUG
-    pr_info("wq_chan_restart[%d]: start_RT[%d/%d] status %x\n",
-	    ch->channum, ch->ch_start_rx, ch->ch_start_tx, ch->status);
+	pr_info("wq_chan_restart[%d]: start_RT[%d/%d] status %x\n",
+		ch->channum, ch->ch_start_rx, ch->ch_start_tx, ch->status);
 
 #endif
 
-    /**********************************/
-    /** check for RX restart request **/
-    /**********************************/
+	/**********************************/
+	/** check for RX restart request **/
+	/**********************************/
 
-    if ((ch->ch_start_rx) && (ch->status & RX_ENABLED)) {
+	if ((ch->ch_start_rx) && (ch->status & RX_ENABLED)) {
 
-	ch->ch_start_rx = 0;
+		ch->ch_start_rx = 0;
 #if defined(RLD_TRANS_DEBUG) || defined(RLD_RXACT_DEBUG)
-	{
-	    static int  hereb4 = 7;
-
-	    if (hereb4) {            /* RLD DEBUG */
-		hereb4--;
+		if (hereb4) {            /* RLD DEBUG */
+			hereb4--;
 #ifdef RLD_TRANS_DEBUG
-		md = &ch->mdr[ch->rxix_irq_srv];
-		pr_info("++ musycc_wq_chan_restart[%d] CHAN RX ACTIVATE: rxix_irq_srv %d, md %p sts %x, rxpkt %lu\n",
-		ch->channum, ch->rxix_irq_srv, md, le32_to_cpu(md->status),
-			ch->s.rx_packets);
+			md = &ch->mdr[ch->rxix_irq_srv];
+			pr_info("++ musycc_wq_chan_restart[%d] CHAN RX ACTIVATE: rxix_irq_srv %d, md %p sts %x, rxpkt %lu\n",
+				ch->channum, ch->rxix_irq_srv, md,
+				le32_to_cpu(md->status), ch->s.rx_packets);
 #elif defined(RLD_RXACT_DEBUG)
-		md = &ch->mdr[ch->rxix_irq_srv];
-		pr_info("++ musycc_wq_chan_restart[%d] CHAN RX ACTIVATE: rxix_irq_srv %d, md %p sts %x, rxpkt %lu\n",
-		ch->channum, ch->rxix_irq_srv, md, le32_to_cpu(md->status),
-			ch->s.rx_packets);
-		musycc_dump_rxbuffer_ring(ch, 1);      /* RLD DEBUG */
+			md = &ch->mdr[ch->rxix_irq_srv];
+			pr_info("++ musycc_wq_chan_restart[%d] CHAN RX ACTIVATE: rxix_irq_srv %d, md %p sts %x, rxpkt %lu\n",
+				ch->channum, ch->rxix_irq_srv,
+				md, le32_to_cpu(md->status),
+				ch->s.rx_packets);
+			musycc_dump_rxbuffer_ring(ch, 1);      /* RLD DEBUG */
 #endif
-	    }
-	}
+		}
 #endif
-	musycc_serv_req(pi, SR_CHANNEL_ACTIVATE | SR_RX_DIRECTION | ch->gchan);
-    }
-    /**********************************/
-    /** check for TX restart request **/
-    /**********************************/
+		musycc_serv_req(pi, SR_CHANNEL_ACTIVATE |
+				SR_RX_DIRECTION | ch->gchan);
+	}
+	/**********************************/
+	/** check for TX restart request **/
+	/**********************************/
 
-    if ((ch->ch_start_tx) && (ch->status & TX_ENABLED)) {
-	/* find next unprocessed message, then set TX thp to it */
-	musycc_update_tx_thp(ch);
+	if ((ch->ch_start_tx) && (ch->status & TX_ENABLED)) {
+		/* find next unprocessed message, then set TX thp to it */
+		musycc_update_tx_thp(ch);
 
-#if 0
-	spin_lock_irqsave(&ch->ch_txlock, flags);
-#endif
-	md = ch->txd_irq_srv;
-	if (!md) {
+		md = ch->txd_irq_srv;
+		if (!md) {
 #ifdef RLD_TRANS_DEBUG
-	    pr_info("-- musycc_wq_chan_restart[%d]: WARNING, starting NULL md\n", ch->channum);
-#endif
-#if 0
-	    spin_unlock_irqrestore(&ch->ch_txlock, flags);
-#endif
-	} else if (md->data && ((le32_to_cpu(md->status)) & MUSYCC_TX_OWNED)) {
-	    ch->ch_start_tx = 0;
-#if 0
-	    spin_unlock_irqrestore(&ch->ch_txlock, flags);   /* allow interrupts for service request */
+			pr_info("-- musycc_wq_chan_restart[%d]: WARNING, starting NULL md\n",
+				ch->channum);
 #endif
+		} else if (md->data && ((le32_to_cpu(md->status)) &
+			   MUSYCC_TX_OWNED)) {
+			ch->ch_start_tx = 0;
+
 #ifdef RLD_TRANS_DEBUG
-	    pr_info("++ musycc_wq_chan_restart() CHAN TX ACTIVATE: chan %d txd_irq_srv %p = sts %x, txpkt %lu\n",
-		    ch->channum, ch->txd_irq_srv, ch->txd_irq_srv->status, ch->s.tx_packets);
+			pr_info("++ musycc_wq_chan_restart() CHAN TX ACTIVATE: chan %d txd_irq_srv %p = sts %x, txpkt %lu\n",
+				ch->channum, ch->txd_irq_srv,
+				ch->txd_irq_srv->status, ch->s.tx_packets);
 #endif
-	    musycc_serv_req(pi, SR_CHANNEL_ACTIVATE | SR_TX_DIRECTION | ch->gchan);
-	}
+			musycc_serv_req(pi, SR_CHANNEL_ACTIVATE |
+					SR_TX_DIRECTION | ch->gchan);
+		}
 #ifdef RLD_RESTART_DEBUG
-	else {
-	    /* retain request to start until retried and we have data to xmit */
-	    pr_info("-- musycc_wq_chan_restart[%d]: DELAYED due to md %p sts %x data %x, start_tx %x\n",
-		    ch->channum, md,
-		    le32_to_cpu(md->status),
-		    le32_to_cpu(md->data), ch->ch_start_tx);
-	    musycc_dump_txbuffer_ring(ch, 0);
-#if 0
-	    spin_unlock_irqrestore(&ch->ch_txlock, flags);   /* allow interrupts for service request */
+		else {
+			/* retain request to start until retried and we have data to xmit */
+			pr_info("-- musycc_wq_chan_restart[%d]: DELAYED due to md %p sts %x data %x, start_tx %x\n",
+				ch->channum, md,
+				le32_to_cpu(md->status),
+				le32_to_cpu(md->data), ch->ch_start_tx);
+			musycc_dump_txbuffer_ring(ch, 0);
+		}
 #endif
 	}
-#endif
-    }
 }
 
 
@@ -439,31 +436,32 @@ void
 musycc_chan_restart(mch_t *ch)
 {
 #ifdef RLD_RESTART_DEBUG
-    pr_info("++ musycc_chan_restart[%d]: txd_irq_srv @ %p = sts %x\n",
-	    ch->channum, ch->txd_irq_srv, ch->txd_irq_srv->status);
+	pr_info("++ musycc_chan_restart[%d]: txd_irq_srv @ %p = sts %x\n",
+		ch->channum, ch->txd_irq_srv, ch->txd_irq_srv->status);
 #endif
 
-    /* 2.6 - find next unprocessed message, then set TX thp to it */
+	/* 2.6 - find next unprocessed message, then set TX thp to it */
 #ifdef RLD_RESTART_DEBUG
-    pr_info(">> musycc_chan_restart: scheduling Chan %x workQ @ %p\n", ch->channum, &ch->ch_work);
+	pr_info(">> musycc_chan_restart: scheduling Chan %x workQ @ %p\n",
+		ch->channum, &ch->ch_work);
 #endif
-    c4_wk_chan_restart(ch);        /* work queue mechanism fires off: Ref:
-				     * musycc_wq_chan_restart () */
-
+	c4_wk_chan_restart(ch);        /* work queue mechanism fires off: Ref:
+					* musycc_wq_chan_restart () */
 }
 
 
 void
 rld_put_led(mpi_t *pi, u_int32_t ledval)
 {
-    static u_int32_t led = 0;
+	static u_int32_t led;
 
-    if (ledval == 0)
-	led = 0;
-    else
-	led |= ledval;
+	if (ledval == 0)
+		led = 0;
+	else
+		led |= ledval;
 
-    pci_write_32((u_int32_t *) &pi->up->cpldbase->leds, led);  /* RLD DEBUG TRANHANG */
+	/* RLD DEBUG TRANHANG */
+	pci_write_32((u_int32_t *) &pi->up->cpldbase->leds, led);
 }
 
 
@@ -472,100 +470,110 @@ rld_put_led(mpi_t *pi, u_int32_t ledval)
 void
 musycc_serv_req(mpi_t *pi, u_int32_t req)
 {
-    volatile u_int32_t r;
-    int         rcnt;
+	volatile u_int32_t r;
+	int         rcnt;
 
-    /*
-     * PORT NOTE: Semaphore protect service loop guarantees only a single
-     * operation at a time.  Per MUSYCC Manual - "Issuing service requests to
-     * the same channel group without first receiving ACK from each request
-     * may cause the host to lose track of which service request has been
-     * acknowledged."
-     */
+	/*
+	 * PORT NOTE: Semaphore protect service loop guarantees only a single
+	 * operation at a time.  Per MUSYCC Manual - "Issuing service requests to
+	 * the same channel group without first receiving ACK from each request
+	 * may cause the host to lose track of which service request has been
+	 * acknowledged."
+	 */
 
-    SD_SEM_TAKE(&pi->sr_sem_busy, "serv");     /* only 1 thru here, per
-						 * group */
+	SD_SEM_TAKE(&pi->sr_sem_busy, "serv");     /* only 1 thru here, per
+						    * group */
 
-    if (pi->sr_last == req) {
+	if (pi->sr_last == req) {
 #ifdef RLD_TRANS_DEBUG
-	pr_info(">> same SR, Port %d Req %x\n", pi->portnum, req);
+		pr_info(">> same SR, Port %d Req %x\n", pi->portnum, req);
 #endif
 
-	/*
-	 * The most likely repeated request is the channel activation command
-	 * which follows the occurrence of a Transparent mode TX ONR or a
-	 * BUFF error.  If the previous command was a CHANNEL ACTIVATE,
-	 * precede it with a NOOP command in order maintain coherent control
-	 * of this current (re)ACTIVATE.
-	 */
+		/*
+		 * The most likely repeated request is the channel activation command
+		 * which follows the occurrence of a Transparent mode TX ONR or a
+		 * BUFF error.  If the previous command was a CHANNEL ACTIVATE,
+		 * precede it with a NOOP command in order maintain coherent control
+		 * of this current (re)ACTIVATE.
+		 */
 
-	r = (pi->sr_last & ~SR_GCHANNEL_MASK);
-	if ((r == (SR_CHANNEL_ACTIVATE | SR_TX_DIRECTION)) ||
-	    (r == (SR_CHANNEL_ACTIVATE | SR_RX_DIRECTION))) {
+		r = (pi->sr_last & ~SR_GCHANNEL_MASK);
+		if ((r == (SR_CHANNEL_ACTIVATE | SR_TX_DIRECTION)) ||
+		    (r == (SR_CHANNEL_ACTIVATE | SR_RX_DIRECTION))) {
 #ifdef RLD_TRANS_DEBUG
-	    pr_info(">> same CHAN ACT SR, Port %d Req %x => issue SR_NOOP CMD\n", pi->portnum, req);
+			pr_info(">> same CHAN ACT SR, Port %d Req %x => issue SR_NOOP CMD\n", pi->portnum, req);
 #endif
-	    SD_SEM_GIVE(&pi->sr_sem_busy);     /* allow this next request */
-	    musycc_serv_req(pi, SR_NOOP);
-	    SD_SEM_TAKE(&pi->sr_sem_busy, "serv");     /* relock & continue w/
-							 * original req */
-	} else if (req == SR_NOOP) {
-	    /* no need to issue back-to-back SR_NOOP commands at this time */
+			/* allow this next request */
+			SD_SEM_GIVE(&pi->sr_sem_busy);
+			musycc_serv_req(pi, SR_NOOP);
+			/* relock & continue w/ original req */
+			SD_SEM_TAKE(&pi->sr_sem_busy, "serv");
+		} else if (req == SR_NOOP) {
+			/* no need to issue back-to-back
+			 * SR_NOOP commands at this time
+			 */
 #ifdef RLD_TRANS_DEBUG
-	    pr_info(">> same Port SR_NOOP skipped, Port %d\n", pi->portnum);
+			pr_info(">> same Port SR_NOOP skipped, Port %d\n",
+				pi->portnum);
 #endif
-	    SD_SEM_GIVE(&pi->sr_sem_busy);     /* allow this next request */
-	    return;
+			/* allow this next request */
+			SD_SEM_GIVE(&pi->sr_sem_busy);
+			return;
+		}
 	}
-    }
-    rcnt = 0;
-    pi->sr_last = req;
+	rcnt = 0;
+	pi->sr_last = req;
 rewrite:
-    pci_write_32((u_int32_t *) &pi->reg->srd, req);
-    FLUSH_MEM_WRITE();
-
-    /*
-     * Per MUSYCC Manual, Section 6.1,2 - "When writing an SCR service
-     * request, the host must ensure at least one PCI bus clock cycle has
-     * elapsed before writing another service request.  To meet this minimum
-     * elapsed service request write timing interval, it is recommended that
-     * the host follow any SCR write with another operation which reads from
-     * the same address."
-     */
-    r = pci_read_32((u_int32_t *) &pi->reg->srd);      /* adhere to write
-							 * timing imposition */
-
-
-    if ((r != req) && (req != SR_CHIP_RESET) && (++rcnt <= MUSYCC_SR_RETRY_CNT)) {
-	if (cxt1e1_log_level >= LOG_MONITOR)
-	    pr_info("%s: %d - reissue srv req/last %x/%x (hdw reads %x), Chan %d.\n",
-		    pi->up->devname, rcnt, req, pi->sr_last, r,
-		    (pi->portnum * MUSYCC_NCHANS) + (req & 0x1f));
-	OS_uwait_dummy();          /* this delay helps reduce reissue counts
-				     * (reason not yet researched) */
-	goto rewrite;
-    }
-    if (rcnt > MUSYCC_SR_RETRY_CNT) {
-	pr_warning("%s: failed service request (#%d)= %x, group %d.\n",
-		   pi->up->devname, MUSYCC_SR_RETRY_CNT, req, pi->portnum);
-	SD_SEM_GIVE(&pi->sr_sem_busy); /* allow any next request */
-	return;
-    }
-    if (req == SR_CHIP_RESET) {
+	pci_write_32((u_int32_t *) &pi->reg->srd, req);
+	FLUSH_MEM_WRITE();
+
 	/*
-	 * PORT NOTE: the CHIP_RESET command is NOT ack'd by the MUSYCC, thus
-	 * the upcoming delay is used.  Though the MUSYCC documentation
-	 * suggests a read-after-write would supply the required delay, it's
-	 * unclear what CPU/BUS clock speeds might have been assumed when
-	 * suggesting this 'lack of ACK' workaround.  Thus the use of uwait.
+	 * Per MUSYCC Manual, Section 6.1,2 - "When writing an SCR service
+	 * request, the host must ensure at least one PCI bus clock cycle has
+	 * elapsed before writing another service request.  To meet this minimum
+	 * elapsed service request write timing interval, it is recommended that
+	 * the host follow any SCR write with another operation which reads from
+	 * the same address."
 	 */
-	OS_uwait(100000, "icard"); /* 100ms */
-    } else {
-	FLUSH_MEM_READ();
-	SD_SEM_TAKE(&pi->sr_sem_wait, "sakack");       /* sleep until SACK
-							 * interrupt occurs */
-    }
-    SD_SEM_GIVE(&pi->sr_sem_busy); /* allow any next request */
+
+	/* adhere to write timing imposition */
+	r = pci_read_32((u_int32_t *) &pi->reg->srd);
+
+
+	if ((r != req) && (req != SR_CHIP_RESET) &&
+	    (++rcnt <= MUSYCC_SR_RETRY_CNT)) {
+		if (cxt1e1_log_level >= LOG_MONITOR)
+			pr_info("%s: %d - reissue srv req/last %x/%x (hdw reads %x), Chan %d.\n",
+				pi->up->devname, rcnt, req, pi->sr_last, r,
+				(pi->portnum * MUSYCC_NCHANS) + (req & 0x1f));
+		/* this delay helps reduce reissue counts
+		 * (reason not yet researched)
+		 */
+		OS_uwait_dummy();
+		goto rewrite;
+	}
+	if (rcnt > MUSYCC_SR_RETRY_CNT) {
+		pr_warning("%s: failed service request (#%d)= %x, group %d.\n",
+			   pi->up->devname, MUSYCC_SR_RETRY_CNT,
+			   req, pi->portnum);
+		SD_SEM_GIVE(&pi->sr_sem_busy); /* allow any next request */
+		return;
+	}
+	if (req == SR_CHIP_RESET) {
+		/*
+		 * PORT NOTE: the CHIP_RESET command is NOT ack'd by the MUSYCC, thus
+		 * the upcoming delay is used.  Though the MUSYCC documentation
+		 * suggests a read-after-write would supply the required delay, it's
+		 * unclear what CPU/BUS clock speeds might have been assumed when
+		 * suggesting this 'lack of ACK' workaround.  Thus the use of uwait.
+		 */
+		OS_uwait(100000, "icard"); /* 100ms */
+	} else {
+		FLUSH_MEM_READ();
+		/* sleep until SACK interrupt occurs */
+		SD_SEM_TAKE(&pi->sr_sem_wait, "sakack");
+	}
+	SD_SEM_GIVE(&pi->sr_sem_busy); /* allow any next request */
 }
 
 
@@ -573,94 +581,101 @@ rewrite:
 void
 musycc_update_timeslots(mpi_t *pi)
 {
-    int         i, ch;
-    char        e1mode = IS_FRAME_ANY_E1(pi->p.port_mode);
-
-    for (i = 0; i < 32; i++) {
-	int         usedby = 0, last = 0, ts, j, bits[8];
-
-	u_int8_t lastval = 0;
-
-	if (((i == 0) && e1mode) || /* disable if  E1 mode */
-	    ((i == 16) && ((pi->p.port_mode == CFG_FRAME_E1CRC_CAS) || (pi->p.port_mode == CFG_FRAME_E1CRC_CAS_AMI)))
-	    || ((i > 23) && (!e1mode))) /* disable if T1 mode */
-	    pi->tsm[i] = 0xff;      /* make tslot unavailable for this mode */
-	else
-	    pi->tsm[i] = 0x00;      /* make tslot available for assignment */
-	for (j = 0; j < 8; j++)
-	    bits[j] = -1;
-	for (ch = 0; ch < MUSYCC_NCHANS; ch++) {
-	    if ((pi->chan[ch]->state == UP) && (pi->chan[ch]->p.bitmask[i])) {
-		usedby++;
-		last = ch;
-		lastval = pi->chan[ch]->p.bitmask[i];
+	int         i, ch;
+	char        e1mode = IS_FRAME_ANY_E1(pi->p.port_mode);
+
+	for (i = 0; i < 32; i++) {
+		int         usedby = 0, last = 0, ts, j, bits[8];
+
+		u_int8_t lastval = 0;
+
+		if (((i == 0) && e1mode) || /* disable if  E1 mode */
+		    ((i == 16) && ((pi->p.port_mode == CFG_FRAME_E1CRC_CAS) ||
+		    (pi->p.port_mode == CFG_FRAME_E1CRC_CAS_AMI))) ||
+		    ((i > 23) && (!e1mode))) /* disable if T1 mode */
+			/* make tslot unavailable for this mode */
+			pi->tsm[i] = 0xff;
+		else
+			/* make tslot available for assignment */
+			pi->tsm[i] = 0x00;
 		for (j = 0; j < 8; j++)
-		    if (lastval & (1 << j))
-			bits[j] = ch;
-		pi->tsm[i] |= lastval;
-	    }
-	}
-	if (!usedby)
-	    ts = 0;
-	else if ((usedby == 1) && (lastval == 0xff))
-	    ts = (4 << 5) | last;
-	else if ((usedby == 1) && (lastval == 0x7f))
-	    ts = (5 << 5) | last;
-	else {
-	    int         idx;
-
-	    if (bits[0] < 0)
-		ts = (6 << 5) | (idx = last);
-	    else
-		ts = (7 << 5) | (idx = bits[0]);
-	    for (j = 1; j < 8; j++) {
-		pi->regram->rscm[idx * 8 + j] = (bits[j] < 0) ? 0 : (0x80 | bits[j]);
-		pi->regram->tscm[idx * 8 + j] = (bits[j] < 0) ? 0 : (0x80 | bits[j]);
-	    }
+			bits[j] = -1;
+		for (ch = 0; ch < MUSYCC_NCHANS; ch++) {
+			if ((pi->chan[ch]->state == UP) &&
+			    (pi->chan[ch]->p.bitmask[i])) {
+				usedby++;
+				last = ch;
+				lastval = pi->chan[ch]->p.bitmask[i];
+				for (j = 0; j < 8; j++)
+					if (lastval & (1 << j))
+						bits[j] = ch;
+				pi->tsm[i] |= lastval;
+			}
+		}
+		if (!usedby)
+			ts = 0;
+		else if ((usedby == 1) && (lastval == 0xff))
+			ts = (4 << 5) | last;
+		else if ((usedby == 1) && (lastval == 0x7f))
+			ts = (5 << 5) | last;
+		else {
+			int         idx;
+
+			if (bits[0] < 0)
+				ts = (6 << 5) | (idx = last);
+			else
+				ts = (7 << 5) | (idx = bits[0]);
+			for (j = 1; j < 8; j++) {
+				pi->regram->rscm[idx * 8 + j] =
+					(bits[j] < 0) ? 0 : (0x80 | bits[j]);
+				pi->regram->tscm[idx * 8 + j] =
+					(bits[j] < 0) ? 0 : (0x80 | bits[j]);
+			}
+		}
+		pi->regram->rtsm[i] = ts;
+		pi->regram->ttsm[i] = ts;
 	}
-	pi->regram->rtsm[i] = ts;
-	pi->regram->ttsm[i] = ts;
-    }
-    FLUSH_MEM_WRITE();
-
-    musycc_serv_req(pi, SR_TIMESLOT_MAP | SR_RX_DIRECTION);
-    musycc_serv_req(pi, SR_TIMESLOT_MAP | SR_TX_DIRECTION);
-    musycc_serv_req(pi, SR_SUBCHANNEL_MAP | SR_RX_DIRECTION);
-    musycc_serv_req(pi, SR_SUBCHANNEL_MAP | SR_TX_DIRECTION);
+	FLUSH_MEM_WRITE();
+
+	musycc_serv_req(pi, SR_TIMESLOT_MAP | SR_RX_DIRECTION);
+	musycc_serv_req(pi, SR_TIMESLOT_MAP | SR_TX_DIRECTION);
+	musycc_serv_req(pi, SR_SUBCHANNEL_MAP | SR_RX_DIRECTION);
+	musycc_serv_req(pi, SR_SUBCHANNEL_MAP | SR_TX_DIRECTION);
 }
 #endif
 
 
 #ifdef SBE_WAN256T3_ENABLE
-void
+	void
 musycc_update_timeslots(mpi_t *pi)
 {
-    mch_t      *ch;
+	mch_t      *ch;
 
-    u_int8_t    ts, hmask, tsen;
-    int         gchan;
-    int         i;
+	u_int8_t    ts, hmask, tsen;
+	int         gchan;
+	int         i;
 
 #ifdef SBE_PMCC4_ENABLE
-    hmask = (0x1f << pi->up->p.hypersize) & 0x1f;
+	hmask = (0x1f << pi->up->p.hypersize) & 0x1f;
 #endif
 #ifdef SBE_WAN256T3_ENABLE
-    hmask = (0x1f << hyperdummy) & 0x1f;
+	hmask = (0x1f << hyperdummy) & 0x1f;
 #endif
-    for (i = 0; i < 128; i++) {
-	gchan = ((pi->portnum * MUSYCC_NCHANS) + (i & hmask)) % MUSYCC_NCHANS;
-	ch = pi->chan[gchan];
-	if (ch->p.mode_56k)
-	    tsen = MODE_56KBPS;
-	else
-	    tsen = MODE_64KBPS;     /* also the default */
-	ts = ((pi->portnum % 4) == (i / 32)) ? (tsen << 5) | (i & hmask) : 0;
-	pi->regram->rtsm[i] = ts;
-	pi->regram->ttsm[i] = ts;
-    }
-    FLUSH_MEM_WRITE();
-    musycc_serv_req(pi, SR_TIMESLOT_MAP | SR_RX_DIRECTION);
-    musycc_serv_req(pi, SR_TIMESLOT_MAP | SR_TX_DIRECTION);
+	for (i = 0; i < 128; i++) {
+		gchan = ((pi->portnum * MUSYCC_NCHANS) +
+			(i & hmask)) % MUSYCC_NCHANS;
+		ch = pi->chan[gchan];
+		if (ch->p.mode_56k)
+			tsen = MODE_56KBPS;
+		else
+			tsen = MODE_64KBPS;     /* also the default */
+		ts = ((pi->portnum % 4) == (i / 32)) ? (tsen << 5) | (i & hmask) : 0;
+		pi->regram->rtsm[i] = ts;
+		pi->regram->ttsm[i] = ts;
+	}
+	FLUSH_MEM_WRITE();
+	musycc_serv_req(pi, SR_TIMESLOT_MAP | SR_RX_DIRECTION);
+	musycc_serv_req(pi, SR_TIMESLOT_MAP | SR_TX_DIRECTION);
 }
 #endif
 
@@ -672,59 +687,59 @@ musycc_update_timeslots(mpi_t *pi)
 u_int32_t
 musycc_chan_proto(int proto)
 {
-    int         reg;
-
-    switch (proto) {
-    case CFG_CH_PROTO_TRANS:        /* 0 */
-	reg = MUSYCC_CCD_TRANS;
-	break;
-    case CFG_CH_PROTO_SS7:          /* 1 */
-	reg = MUSYCC_CCD_SS7;
-	break;
-    default:
-    case CFG_CH_PROTO_ISLP_MODE:   /* 4 */
-    case CFG_CH_PROTO_HDLC_FCS16:  /* 2 */
-	reg = MUSYCC_CCD_HDLC_FCS16;
-	break;
-    case CFG_CH_PROTO_HDLC_FCS32:  /* 3 */
-	reg = MUSYCC_CCD_HDLC_FCS32;
-	break;
-    }
-
-    return reg;
+	int         reg;
+
+	switch (proto) {
+	case CFG_CH_PROTO_TRANS:        /* 0 */
+		reg = MUSYCC_CCD_TRANS;
+		break;
+	case CFG_CH_PROTO_SS7:          /* 1 */
+		reg = MUSYCC_CCD_SS7;
+		break;
+	default:
+	case CFG_CH_PROTO_ISLP_MODE:   /* 4 */
+	case CFG_CH_PROTO_HDLC_FCS16:  /* 2 */
+		reg = MUSYCC_CCD_HDLC_FCS16;
+		break;
+	case CFG_CH_PROTO_HDLC_FCS32:  /* 3 */
+		reg = MUSYCC_CCD_HDLC_FCS32;
+		break;
+	}
+
+	return reg;
 }
 
 #ifdef SBE_WAN256T3_ENABLE
 static void __init
 musycc_init_port(mpi_t *pi)
 {
-    pci_write_32((u_int32_t *) &pi->reg->gbp, OS_vtophys(pi->regram));
-
-    pi->regram->grcd =
-	__constant_cpu_to_le32(MUSYCC_GRCD_RX_ENABLE |
-				MUSYCC_GRCD_TX_ENABLE |
-				MUSYCC_GRCD_SF_ALIGN |
-				MUSYCC_GRCD_SUBCHAN_DISABLE |
-				MUSYCC_GRCD_OOFMP_DISABLE |
-				MUSYCC_GRCD_COFAIRQ_DISABLE |
-				MUSYCC_GRCD_MC_ENABLE |
-		       (MUSYCC_GRCD_POLLTH_32 << MUSYCC_GRCD_POLLTH_SHIFT));
-
-    pi->regram->pcd =
-	__constant_cpu_to_le32(MUSYCC_PCD_E1X4_MODE |
-				MUSYCC_PCD_TXDATA_RISING |
-				MUSYCC_PCD_TX_DRIVEN);
-
-    /* Message length descriptor */
-       pi->regram->mld = __constant_cpu_to_le32(cxt1e1_max_mru | (cxt1e1_max_mru << 16));
-    FLUSH_MEM_WRITE();
+	pci_write_32((u_int32_t *) &pi->reg->gbp, OS_vtophys(pi->regram));
+
+	pi->regram->grcd =
+		__constant_cpu_to_le32(MUSYCC_GRCD_RX_ENABLE |
+				       MUSYCC_GRCD_TX_ENABLE |
+				       MUSYCC_GRCD_SF_ALIGN |
+				       MUSYCC_GRCD_SUBCHAN_DISABLE |
+				       MUSYCC_GRCD_OOFMP_DISABLE |
+				       MUSYCC_GRCD_COFAIRQ_DISABLE |
+				       MUSYCC_GRCD_MC_ENABLE |
+				       (MUSYCC_GRCD_POLLTH_32 << MUSYCC_GRCD_POLLTH_SHIFT));
+
+	pi->regram->pcd =
+		__constant_cpu_to_le32(MUSYCC_PCD_E1X4_MODE |
+				       MUSYCC_PCD_TXDATA_RISING |
+				       MUSYCC_PCD_TX_DRIVEN);
+
+	/* Message length descriptor */
+	pi->regram->mld = __constant_cpu_to_le32(cxt1e1_max_mru | (cxt1e1_max_mru << 16));
+	FLUSH_MEM_WRITE();
 
-    musycc_serv_req(pi, SR_GROUP_INIT | SR_RX_DIRECTION);
-    musycc_serv_req(pi, SR_GROUP_INIT | SR_TX_DIRECTION);
+	musycc_serv_req(pi, SR_GROUP_INIT | SR_RX_DIRECTION);
+	musycc_serv_req(pi, SR_GROUP_INIT | SR_TX_DIRECTION);
 
-    musycc_init_mdt(pi);
+	musycc_init_mdt(pi);
 
-    musycc_update_timeslots(pi);
+	musycc_update_timeslots(pi);
 }
 #endif
 
@@ -732,490 +747,456 @@ musycc_init_port(mpi_t *pi)
 status_t    __init
 musycc_init(ci_t *ci)
 {
-    char       *regaddr;        /* temp for address boundary calculations */
-    int         i, gchan;
+	char       *regaddr;        /* temp for address boundary calculations */
+	int         i, gchan;
 
-    OS_sem_init(&ci->sem_wdbusy, SEM_AVAILABLE);       /* watchdog exclusion */
+	OS_sem_init(&ci->sem_wdbusy, SEM_AVAILABLE); /* watchdog exclusion */
 
-    /*
-     * Per MUSYCC manual, Section 6.3.4 - "The host must allocate a dword
-     * aligned memory segment for interrupt queue pointers."
-     */
+	/*
+	 * Per MUSYCC manual, Section 6.3.4 - "The host must allocate a dword
+	 * aligned memory segment for interrupt queue pointers."
+	 */
 
 #define INT_QUEUE_BOUNDARY  4
 
-    regaddr = OS_kmalloc((INT_QUEUE_SIZE + 1) * sizeof(u_int32_t));
-    if (!regaddr)
-	return -ENOMEM;
-    ci->iqd_p_saved = regaddr;      /* save orig value for free's usage */
-    ci->iqd_p = (u_int32_t *) ((unsigned long) (regaddr + INT_QUEUE_BOUNDARY - 1) &
-			       (~(INT_QUEUE_BOUNDARY - 1)));    /* this calculates
-								 * closest boundary */
+	regaddr = kzalloc((INT_QUEUE_SIZE + 1) * sizeof(u_int32_t),
+			  GFP_KERNEL | GFP_DMA);
+	if (!regaddr)
+		return -ENOMEM;
+	ci->iqd_p_saved = regaddr;      /* save orig value for free's usage */
+	/* this calculates closest boundary */
+	ci->iqd_p = (u_int32_t *) ((unsigned long)(regaddr + INT_QUEUE_BOUNDARY - 1) &
+				   (~(INT_QUEUE_BOUNDARY - 1)));
 
-    for (i = 0; i < INT_QUEUE_SIZE; i++)
-	ci->iqd_p[i] = __constant_cpu_to_le32(INT_EMPTY_ENTRY);
+	for (i = 0; i < INT_QUEUE_SIZE; i++)
+		ci->iqd_p[i] = __constant_cpu_to_le32(INT_EMPTY_ENTRY);
 
-    for (i = 0; i < ci->max_port; i++) {
-	mpi_t      *pi = &ci->port[i];
+	for (i = 0; i < ci->max_port; i++) {
+		mpi_t      *pi = &ci->port[i];
 
-	/*
-	 * Per MUSYCC manual, Section 6.3.2 - "The host must allocate a 2KB
-	 * bound memory segment for Channel Group 0."
-	 */
+		/*
+		 * Per MUSYCC manual, Section 6.3.2 - "The host must allocate a 2KB
+		 * bound memory segment for Channel Group 0."
+		 */
 
 #define GROUP_BOUNDARY   0x800
 
-	regaddr = OS_kmalloc(sizeof(struct musycc_groupr) + GROUP_BOUNDARY);
-	if (!regaddr) {
-	    for (gchan = 0; gchan < i; gchan++) {
-		pi = &ci->port[gchan];
-		OS_kfree(pi->reg);
-		pi->reg = NULL;
-	    }
-	    return -ENOMEM;
+		regaddr = kzalloc(sizeof(struct musycc_groupr) + GROUP_BOUNDARY,
+				  GFP_KERNEL | GFP_DMA);
+		if (!regaddr) {
+			for (gchan = 0; gchan < i; gchan++) {
+				pi = &ci->port[gchan];
+				kfree(pi->reg);
+				pi->reg = NULL;
+			}
+			return -ENOMEM;
+		}
+		pi->regram_saved = regaddr; /* save orig value for free's usage */
+		/* this calculates closest boundary */
+		pi->regram = (struct musycc_groupr *) ((unsigned long)(regaddr + GROUP_BOUNDARY - 1) &
+				(~(GROUP_BOUNDARY - 1)));
 	}
-	pi->regram_saved = regaddr; /* save orig value for free's usage */
-	pi->regram = (struct musycc_groupr *) ((unsigned long) (regaddr + GROUP_BOUNDARY - 1) &
-					       (~(GROUP_BOUNDARY - 1)));        /* this calculates
-										 * closest boundary */
-    }
-
-    /* any board centric MUSYCC commands will use group ZERO as its "home" */
-    ci->regram = ci->port[0].regram;
-    musycc_serv_req(&ci->port[0], SR_CHIP_RESET);
-
-    pci_write_32((u_int32_t *) &ci->reg->gbp, OS_vtophys(ci->regram));
-    pci_flush_write(ci);
+
+	/* any board centric MUSYCC commands will use group ZERO as its "home" */
+	ci->regram = ci->port[0].regram;
+	musycc_serv_req(&ci->port[0], SR_CHIP_RESET);
+
+	pci_write_32((u_int32_t *) &ci->reg->gbp, OS_vtophys(ci->regram));
+	pci_flush_write(ci);
 #ifdef CONFIG_SBE_PMCC4_NCOMM
-    ci->regram->__glcd = __constant_cpu_to_le32(GCD_MAGIC);
+	ci->regram->__glcd = __constant_cpu_to_le32(GCD_MAGIC);
 #else
-    /* standard driver POLLS for INTB via CPLD register */
-    ci->regram->__glcd = __constant_cpu_to_le32(GCD_MAGIC | MUSYCC_GCD_INTB_DISABLE);
+	/* standard driver POLLS for INTB via CPLD register */
+	ci->regram->__glcd = __constant_cpu_to_le32(GCD_MAGIC |
+						    MUSYCC_GCD_INTB_DISABLE);
 #endif
 
-    ci->regram->__iqp = cpu_to_le32(OS_vtophys(&ci->iqd_p[0]));
-    ci->regram->__iql = __constant_cpu_to_le32(INT_QUEUE_SIZE - 1);
-    pci_write_32((u_int32_t *) &ci->reg->dacbp, 0);
-    FLUSH_MEM_WRITE();
+	ci->regram->__iqp = cpu_to_le32(OS_vtophys(&ci->iqd_p[0]));
+	ci->regram->__iql = __constant_cpu_to_le32(INT_QUEUE_SIZE - 1);
+	pci_write_32((u_int32_t *) &ci->reg->dacbp, 0);
+	FLUSH_MEM_WRITE();
 
-    ci->state = C_RUNNING;          /* mark as full interrupt processing
-				     * available */
+	ci->state = C_RUNNING;          /* mark as full interrupt processing
+					 * available */
 
-    musycc_serv_req(&ci->port[0], SR_GLOBAL_INIT);     /* FIRST INTERRUPT ! */
+	musycc_serv_req(&ci->port[0], SR_GLOBAL_INIT); /* FIRST INTERRUPT ! */
 
-    /* sanity check settable parameters */
+	/* sanity check settable parameters */
 
-       if (cxt1e1_max_mru > 0xffe) {
-	pr_warning("Maximum allowed MRU exceeded, resetting %d to %d.\n",
-				  cxt1e1_max_mru, 0xffe);
-	       cxt1e1_max_mru = 0xffe;
-    }
-       if (cxt1e1_max_mtu > 0xffe) {
-	pr_warning("Maximum allowed MTU exceeded, resetting %d to %d.\n",
-				  cxt1e1_max_mtu, 0xffe);
-	       cxt1e1_max_mtu = 0xffe;
-    }
+	if (cxt1e1_max_mru > 0xffe) {
+		pr_warning("Maximum allowed MRU exceeded, resetting %d to %d.\n",
+			   cxt1e1_max_mru, 0xffe);
+		cxt1e1_max_mru = 0xffe;
+	}
+	if (cxt1e1_max_mtu > 0xffe) {
+		pr_warning("Maximum allowed MTU exceeded, resetting %d to %d.\n",
+			   cxt1e1_max_mtu, 0xffe);
+		cxt1e1_max_mtu = 0xffe;
+	}
 #ifdef SBE_WAN256T3_ENABLE
-    for (i = 0; i < MUSYCC_NPORTS; i++)
-	musycc_init_port(&ci->port[i]);
+	for (i = 0; i < MUSYCC_NPORTS; i++)
+		musycc_init_port(&ci->port[i]);
 #endif
 
-    return SBE_DRVR_SUCCESS;        /* no error */
+	return SBE_DRVR_SUCCESS;        /* no error */
 }
 
 
 void
 musycc_bh_tx_eom(mpi_t *pi, int gchan)
 {
-    mch_t      *ch;
-    struct mdesc *md;
+	mch_t      *ch;
+	struct mdesc *md;
 
-#if 0
-#ifndef SBE_ISR_INLINE
-    unsigned long flags;
+	volatile u_int32_t status;
 
-#endif
-#endif
-    volatile u_int32_t status;
-
-    ch = pi->chan[gchan];
-    if (!ch || ch->state != UP) {
-	if (cxt1e1_log_level >= LOG_ERROR)
-	    pr_info("%s: intr: xmit EOM on uninitialized channel %d\n",
-		    pi->up->devname, gchan);
-    }
-    if (!ch || !ch->mdt)
-	return;                     /* note: mdt==0 implies a malloc()
-				     * failure w/in chan_up() routine */
-
-#if 0
-#ifdef SBE_ISR_INLINE
-    spin_lock_irq(&ch->ch_txlock);
-#else
-    spin_lock_irqsave(&ch->ch_txlock, flags);
-#endif
-#endif
-    do {
-	FLUSH_MEM_READ();
-	md = ch->txd_irq_srv;
-	status = le32_to_cpu(md->status);
+	ch = pi->chan[gchan];
+	if (!ch || ch->state != UP) {
+		if (cxt1e1_log_level >= LOG_ERROR)
+			pr_info("%s: intr: xmit EOM on uninitialized channel %d\n",
+				pi->up->devname, gchan);
+	}
+	if (!ch || !ch->mdt)
+		return;                     /* note: mdt==0 implies a malloc()
+					     * failure w/in chan_up() routine */
 
-	/*
-	 * Note: Per MUSYCC Ref 6.4.9, the host does not poll a host-owned
-	 * Transmit Buffer Descriptor during Transparent Mode.
-	 */
-	if (status & MUSYCC_TX_OWNED) {
-	    int         readCount, loopCount;
-
-	    /***********************************************************/
-	    /* HW Bug Fix                                              */
-	    /* ----------                                              */
-	    /* Under certain PCI Bus loading conditions, the data      */
-	    /* associated with an update of Shared Memory is delayed   */
-	    /* relative to its PCI Interrupt.  This is caught when     */
-	    /* the host determines it does not yet OWN the descriptor. */
-	    /***********************************************************/
-
-	    readCount = 0;
-	    while (status & MUSYCC_TX_OWNED) {
-		for (loopCount = 0; loopCount < 0x30; loopCount++)
-		    OS_uwait_dummy();  /* use call to avoid optimization
-					 * removal of dummy delay */
+	do {
 		FLUSH_MEM_READ();
+		md = ch->txd_irq_srv;
 		status = le32_to_cpu(md->status);
-		if (readCount++ > 40)
-		    break;          /* don't wait any longer */
-	    }
-	    if (status & MUSYCC_TX_OWNED) {
-		if (cxt1e1_log_level >= LOG_MONITOR) {
-		    pr_info("%s: Port %d Chan %2d - unexpected TX msg ownership intr (md %p sts %x)\n",
-			    pi->up->devname, pi->portnum, ch->channum,
-			    md, status);
-		    pr_info("++ User 0x%p IRQ_SRV 0x%p USR_ADD 0x%p QStopped %x, start_tx %x tx_full %d txd_free %d mode %x\n",
-			    ch->user, ch->txd_irq_srv, ch->txd_usr_add,
-			    sd_queue_stopped(ch->user),
-			    ch->ch_start_tx, ch->tx_full, ch->txd_free, ch->p.chan_mode);
-		    musycc_dump_txbuffer_ring(ch, 0);
+
+		/*
+		 * Note: Per MUSYCC Ref 6.4.9, the host does not poll a host-owned
+		 * Transmit Buffer Descriptor during Transparent Mode.
+		 */
+		if (status & MUSYCC_TX_OWNED) {
+			int         readCount, loopCount;
+
+			/***********************************************************/
+			/* HW Bug Fix                                              */
+			/* ----------                                              */
+			/* Under certain PCI Bus loading conditions, the data      */
+			/* associated with an update of Shared Memory is delayed   */
+			/* relative to its PCI Interrupt.  This is caught when     */
+			/* the host determines it does not yet OWN the descriptor. */
+			/***********************************************************/
+
+			readCount = 0;
+			while (status & MUSYCC_TX_OWNED) {
+				for (loopCount = 0; loopCount < 0x30; loopCount++)
+					/* use call to avoid optimization
+					 * removal of dummy delay */
+					OS_uwait_dummy();
+				FLUSH_MEM_READ();
+				status = le32_to_cpu(md->status);
+				if (readCount++ > 40)
+					break; /* don't wait any longer */
+			}
+			if (status & MUSYCC_TX_OWNED) {
+				if (cxt1e1_log_level >= LOG_MONITOR) {
+					pr_info("%s: Port %d Chan %2d - unexpected TX msg ownership intr (md %p sts %x)\n",
+						pi->up->devname, pi->portnum,
+						ch->channum, md, status);
+					pr_info("++ User 0x%p IRQ_SRV 0x%p USR_ADD 0x%p QStopped %x, start_tx %x tx_full %d txd_free %d mode %x\n",
+						ch->user, ch->txd_irq_srv,
+						ch->txd_usr_add,
+						sd_queue_stopped(ch->user),
+						ch->ch_start_tx, ch->tx_full,
+						ch->txd_free, ch->p.chan_mode);
+					musycc_dump_txbuffer_ring(ch, 0);
+				}
+				break;              /* Not our mdesc, done */
+			} else {
+				if (cxt1e1_log_level >= LOG_MONITOR)
+					pr_info("%s: Port %d Chan %2d - recovered TX msg ownership [%d] (md %p sts %x)\n",
+						pi->up->devname, pi->portnum,
+						ch->channum, readCount,
+						md, status);
+			}
 		}
-		break;              /* Not our mdesc, done */
-	    } else {
-		if (cxt1e1_log_level >= LOG_MONITOR)
-		    pr_info("%s: Port %d Chan %2d - recovered TX msg ownership [%d] (md %p sts %x)\n",
-			    pi->up->devname, pi->portnum, ch->channum, readCount, md, status);
-	    }
-	}
-	ch->txd_irq_srv = md->snext;
-
-	md->data = 0;
-	if (md->mem_token)	{
-	    /* upcount channel */
-	    atomic_sub(OS_mem_token_tlen(md->mem_token), &ch->tx_pending);
-	    /* upcount card */
-	    atomic_sub(OS_mem_token_tlen(md->mem_token), &pi->up->tx_pending);
+		ch->txd_irq_srv = md->snext;
+
+		md->data = 0;
+		if (md->mem_token) {
+			/* upcount channel */
+			atomic_sub(OS_mem_token_tlen(md->mem_token),
+				   &ch->tx_pending);
+			/* upcount card */
+			atomic_sub(OS_mem_token_tlen(md->mem_token),
+				   &pi->up->tx_pending);
 #ifdef SBE_WAN256T3_ENABLE
-	    if (!atomic_read(&pi->up->tx_pending))
-		wan256t3_led(pi->up, LED_TX, 0);
+			if (!atomic_read(&pi->up->tx_pending))
+				wan256t3_led(pi->up, LED_TX, 0);
 #endif
-
-#ifdef CONFIG_SBE_WAN256T3_NCOMM
-	    /* callback that our packet was sent */
-	    {
-		int         hdlcnum = (pi->portnum * 32 + gchan);
-
-		if (hdlcnum >= 228) {
-		    if (nciProcess_TX_complete)
-			(*nciProcess_TX_complete) (hdlcnum,
-						   getuserbychan(gchan));
+			OS_mem_token_free_irq(md->mem_token);
+			md->mem_token = NULL;
 		}
-	    }
-#endif                              /*** CONFIG_SBE_WAN256T3_NCOMM ***/
-
-	    OS_mem_token_free_irq(md->mem_token);
-	    md->mem_token = NULL;
-	}
-	md->status = 0;
+		md->status = 0;
 #ifdef RLD_TXFULL_DEBUG
-	if (cxt1e1_log_level >= LOG_MONITOR2)
-	    pr_info("~~ tx_eom: tx_full %x  txd_free %d -> %d\n",
-		    ch->tx_full, ch->txd_free, ch->txd_free + 1);
+		if (cxt1e1_log_level >= LOG_MONITOR2)
+			pr_info("~~ tx_eom: tx_full %x  txd_free %d -> %d\n",
+				ch->tx_full, ch->txd_free, ch->txd_free + 1);
 #endif
-	++ch->txd_free;
-	FLUSH_MEM_WRITE();
+		++ch->txd_free;
+		FLUSH_MEM_WRITE();
+
+		if ((ch->p.chan_mode != CFG_CH_PROTO_TRANS) &&
+		    (status & EOBIRQ_ENABLE)) {
+			if (cxt1e1_log_level >= LOG_MONITOR)
+				pr_info("%s: Mode (%x) incorrect EOB status (%x)\n",
+					pi->up->devname, ch->p.chan_mode,
+					status);
+			if ((status & EOMIRQ_ENABLE) == 0)
+				break;
+		}
+	} while ((ch->p.chan_mode != CFG_CH_PROTO_TRANS) &&
+		 ((status & EOMIRQ_ENABLE) == 0));
+	/*
+	 * NOTE: (The above 'while' is coupled w/ previous 'do', way above.) Each
+	 * Transparent data buffer has the EOB bit, and NOT the EOM bit, set and
+	 * will furthermore have a separate IQD associated with each messages
+	 * buffer.
+	 */
 
-	if ((ch->p.chan_mode != CFG_CH_PROTO_TRANS) && (status & EOBIRQ_ENABLE)) {
-	    if (cxt1e1_log_level >= LOG_MONITOR)
-		pr_info("%s: Mode (%x) incorrect EOB status (%x)\n",
-			pi->up->devname, ch->p.chan_mode, status);
-	    if ((status & EOMIRQ_ENABLE) == 0)
-		break;
-	}
-    } while ((ch->p.chan_mode != CFG_CH_PROTO_TRANS) && ((status & EOMIRQ_ENABLE) == 0));
-    /*
-     * NOTE: (The above 'while' is coupled w/ previous 'do', way above.) Each
-     * Transparent data buffer has the EOB bit, and NOT the EOM bit, set and
-     * will furthermore have a separate IQD associated with each messages
-     * buffer.
-     */
-
-    FLUSH_MEM_READ();
-    /*
-     * Smooth flow control hysterisis by maintaining task stoppage until half
-     * the available write buffers are available.
-     */
-    if (ch->tx_full && (ch->txd_free >= (ch->txd_num / 2))) {
+	FLUSH_MEM_READ();
 	/*
-	 * Then, only releave task stoppage if we actually have enough
-	 * buffers to service the last requested packet.  It may require MORE
-	 * than half the available!
+	 * Smooth flow control hysterisis by maintaining task stoppage until half
+	 * the available write buffers are available.
 	 */
-	if (ch->txd_free >= ch->txd_required) {
+	if (ch->tx_full && (ch->txd_free >= (ch->txd_num / 2))) {
+		/*
+		 * Then, only releave task stoppage if we actually have enough
+		 * buffers to service the last requested packet.  It may require MORE
+		 * than half the available!
+		 */
+		if (ch->txd_free >= ch->txd_required) {
 
 #ifdef RLD_TXFULL_DEBUG
-	    if (cxt1e1_log_level >= LOG_MONITOR2)
-		pr_info("tx_eom[%d]: enable xmit tx_full no more, txd_free %d txd_num/2 %d\n",
-			ch->channum,
-			ch->txd_free, ch->txd_num / 2);
+			if (cxt1e1_log_level >= LOG_MONITOR2)
+				pr_info("tx_eom[%d]: enable xmit tx_full no more, txd_free %d txd_num/2 %d\n",
+					ch->channum,
+					ch->txd_free, ch->txd_num / 2);
 #endif
-	    ch->tx_full = 0;
-	    ch->txd_required = 0;
-	    sd_enable_xmit(ch->user);  /* re-enable to catch flow controlled
-					 * channel */
+			ch->tx_full = 0;
+			ch->txd_required = 0;
+			/* re-enable to catch flow controlled channel */
+			sd_enable_xmit(ch->user);
+		}
 	}
-    }
 #ifdef RLD_TXFULL_DEBUG
-    else if (ch->tx_full) {
-	if (cxt1e1_log_level >= LOG_MONITOR2)
-	    pr_info("tx_eom[%d]: bypass TX enable though room available? (txd_free %d txd_num/2 %d)\n",
-		    ch->channum,
-		    ch->txd_free, ch->txd_num / 2);
-    }
+	else if (ch->tx_full) {
+		if (cxt1e1_log_level >= LOG_MONITOR2)
+			pr_info("tx_eom[%d]: bypass TX enable though room available? (txd_free %d txd_num/2 %d)\n",
+				ch->channum,
+				ch->txd_free, ch->txd_num / 2);
+	}
 #endif
 
-    FLUSH_MEM_WRITE();
-#if 0
-#ifdef SBE_ISR_INLINE
-    spin_unlock_irq(&ch->ch_txlock);
-#else
-    spin_unlock_irqrestore(&ch->ch_txlock, flags);
-#endif
-#endif
+	FLUSH_MEM_WRITE();
 }
 
 
 static void
 musycc_bh_rx_eom(mpi_t *pi, int gchan)
 {
-    mch_t      *ch;
-    void       *m, *m2;
-    struct mdesc *md;
-    volatile u_int32_t status;
-    u_int32_t   error;
-
-    ch = pi->chan[gchan];
-    if (!ch || ch->state != UP) {
-	if (cxt1e1_log_level > LOG_ERROR)
-	    pr_info("%s: intr: receive EOM on uninitialized channel %d\n",
-		    pi->up->devname, gchan);
-	return;
-    }
-    if (!ch->mdr)
-	return;                     /* can this happen ? */
-
-    for (;;) {
-	FLUSH_MEM_READ();
-	md = &ch->mdr[ch->rxix_irq_srv];
-	status = le32_to_cpu(md->status);
-	if (!(status & HOST_RX_OWNED))
-	    break;                  /* Not our mdesc, done */
-	m = md->mem_token;
-	error = (status >> 16) & 0xf;
-	if (error == 0) {
-#ifdef CONFIG_SBE_WAN256T3_NCOMM
-	    int         hdlcnum = (pi->portnum * 32 + gchan);
-
-	    /*
-	     * if the packet number belongs to NCOMM, then send it to the TMS
-	     * driver
-	     */
-	    if (hdlcnum >= 228) {
-		if (nciProcess_RX_packet)
-		    (*nciProcess_RX_packet) (hdlcnum, status & 0x3fff, m, ch->user);
-	    } else
-#endif                              /*** CONFIG_SBE_WAN256T3_NCOMM ***/
-
-	    {
-		m2 = OS_mem_token_alloc(cxt1e1_max_mru);
-		if (m2) {
-			/* substitute the mbuf+cluster */
-			md->mem_token = m2;
-			md->data = cpu_to_le32(OS_vtophys(
-				OS_mem_token_data(m2)));
-
-			/* pass the received mbuf upward */
-			sd_recv_consume(m, status & LENGTH_MASK, ch->user);
-			ch->s.rx_packets++;
-			ch->s.rx_bytes += status & LENGTH_MASK;
-		} else
-			ch->s.rx_dropped++;
-	    }
-	} else if (error == ERR_FCS)
-	    ch->s.rx_crc_errors++;
-	else if (error == ERR_ALIGN)
-	    ch->s.rx_missed_errors++;
-	else if (error == ERR_ABT)
-	    ch->s.rx_missed_errors++;
-	else if (error == ERR_LNG)
-	    ch->s.rx_length_errors++;
-	else if (error == ERR_SHT)
-	    ch->s.rx_length_errors++;
-	FLUSH_MEM_WRITE();
-	       status = cxt1e1_max_mru;
-	if (ch->p.chan_mode == CFG_CH_PROTO_TRANS)
-	    status |= EOBIRQ_ENABLE;
-	md->status = cpu_to_le32(status);
-
-	/* Check next mdesc in the ring */
-	if (++ch->rxix_irq_srv >= ch->rxd_num)
-	    ch->rxix_irq_srv = 0;
-	FLUSH_MEM_WRITE();
-    }
+	mch_t      *ch;
+	void       *m, *m2;
+	struct mdesc *md;
+	volatile u_int32_t status;
+	u_int32_t   error;
+
+	ch = pi->chan[gchan];
+	if (!ch || ch->state != UP) {
+		if (cxt1e1_log_level > LOG_ERROR)
+			pr_info("%s: intr: receive EOM on uninitialized channel %d\n",
+				pi->up->devname, gchan);
+		return;
+	}
+	if (!ch->mdr)
+		return;                     /* can this happen ? */
+
+	for (;;) {
+		FLUSH_MEM_READ();
+		md = &ch->mdr[ch->rxix_irq_srv];
+		status = le32_to_cpu(md->status);
+		if (!(status & HOST_RX_OWNED))
+			break;                  /* Not our mdesc, done */
+		m = md->mem_token;
+		error = (status >> 16) & 0xf;
+		if (error == 0) {
+			{
+				m2 = OS_mem_token_alloc(cxt1e1_max_mru);
+				if (m2) {
+					/* substitute the mbuf+cluster */
+					md->mem_token = m2;
+					md->data = cpu_to_le32(OS_vtophys(
+							       OS_mem_token_data(m2)));
+
+					/* pass the received mbuf upward */
+					sd_recv_consume(m, status & LENGTH_MASK,
+							ch->user);
+					ch->s.rx_packets++;
+					ch->s.rx_bytes += status & LENGTH_MASK;
+				} else
+					ch->s.rx_dropped++;
+			}
+		} else if (error == ERR_FCS)
+			ch->s.rx_crc_errors++;
+		else if (error == ERR_ALIGN)
+			ch->s.rx_missed_errors++;
+		else if (error == ERR_ABT)
+			ch->s.rx_missed_errors++;
+		else if (error == ERR_LNG)
+			ch->s.rx_length_errors++;
+		else if (error == ERR_SHT)
+			ch->s.rx_length_errors++;
+		FLUSH_MEM_WRITE();
+		status = cxt1e1_max_mru;
+		if (ch->p.chan_mode == CFG_CH_PROTO_TRANS)
+			status |= EOBIRQ_ENABLE;
+		md->status = cpu_to_le32(status);
+
+		/* Check next mdesc in the ring */
+		if (++ch->rxix_irq_srv >= ch->rxd_num)
+			ch->rxix_irq_srv = 0;
+		FLUSH_MEM_WRITE();
+	}
 }
 
 
 irqreturn_t
 musycc_intr_th_handler(void *devp)
 {
-    ci_t       *ci = (ci_t *) devp;
-    volatile u_int32_t status, currInt = 0;
-    u_int32_t   nextInt, intCnt;
-
-    /*
-     * Hardware not available, potential interrupt hang.  But since interrupt
-     * might be shared, just return.
-     */
-    if (ci->state == C_INIT)
-	return IRQ_NONE;
-    /*
-     * Marked as hardware available. Don't service interrupts, just clear the
-     * event.
-     */
-
-    if (ci->state == C_IDLE) {
-	status = pci_read_32((u_int32_t *) &ci->reg->isd);
+	ci_t       *ci = (ci_t *) devp;
+	volatile u_int32_t status, currInt = 0;
+	u_int32_t   nextInt, intCnt;
 
-	/* clear the interrupt but process nothing else */
-	pci_write_32((u_int32_t *) &ci->reg->isd, status);
-	return IRQ_HANDLED;
-    }
-    FLUSH_PCI_READ();
-    FLUSH_MEM_READ();
-
-    status = pci_read_32((u_int32_t *) &ci->reg->isd);
-    nextInt = INTRPTS_NEXTINT(status);
-    intCnt = INTRPTS_INTCNT(status);
-    ci->intlog.drvr_intr_thcount++;
-
-    /*********************************************************/
-    /* HW Bug Fix                                            */
-    /* ----------                                            */
-    /* Under certain PCI Bus loading conditions, the         */
-    /* MUSYCC looses the data associated with an update      */
-    /* of its ISD and erroneously returns the immediately    */
-    /* preceding 'nextInt' value.  However, the 'intCnt'     */
-    /* value appears to be correct.  By not starting service */
-    /* where the 'missing' 'nextInt' SHOULD point causes     */
-    /* the IQD not to be serviced - the 'not serviced'       */
-    /* entries then remain and continue to increase as more  */
-    /* incorrect ISD's are encountered.                      */
-    /*********************************************************/
-
-    if (nextInt != INTRPTS_NEXTINT(ci->intlog.this_status_new)) {
-	if (cxt1e1_log_level >= LOG_MONITOR) {
-	    pr_info("%s: note - updated ISD from %08x to %08x\n",
-		    ci->devname, status,
-	      (status & (~INTRPTS_NEXTINT_M)) | ci->intlog.this_status_new);
-	}
 	/*
-	 * Replace bogus status with software corrected value.
-	 *
-	 * It's not known whether, during this problem occurrence, if the
-	 * INTFULL bit is correctly reported or not.
+	 * Hardware not available, potential interrupt hang.  But since interrupt
+	 * might be shared, just return.
 	 */
-	status = (status & (~INTRPTS_NEXTINT_M)) | (ci->intlog.this_status_new);
-	nextInt = INTRPTS_NEXTINT(status);
-    }
-    /**********************************************/
-    /* Cn847x Bug Fix                             */
-    /* --------------                             */
-    /* Fix for inability to write back same index */
-    /* as read for a full interrupt queue.        */
-    /**********************************************/
-
-    if (intCnt == INT_QUEUE_SIZE)
-	currInt = ((intCnt - 1) + nextInt) & (INT_QUEUE_SIZE - 1);
-    else
-	/************************************************/
-	/* Interrupt Write Location Issues              */
-	/* -------------------------------              */
-	/* When the interrupt status descriptor is      */
-	/* written, the interrupt line is de-asserted   */
-	/* by the Cn847x.  In the case of MIPS          */
-	/* microprocessors, this must occur at the      */
-	/* beginning of the interrupt handler so that   */
-	/* the interrupt handle is not re-entered due   */
-	/* to interrupt dis-assertion latency.          */
-	/* In the case of all other processors, this    */
-	/* action should occur at the end of the        */
-	/* interrupt handler to avoid overwriting the   */
-	/* interrupt queue.                             */
-	/************************************************/
-
-    if (intCnt)
-	currInt = (intCnt + nextInt) & (INT_QUEUE_SIZE - 1);
-    else {
+	if (ci->state == C_INIT)
+		return IRQ_NONE;
 	/*
-	 * NOTE: Servicing an interrupt whose ISD contains a count of ZERO
-	 * can be indicative of a Shared Interrupt chain.  Our driver can be
-	 * called from the system's interrupt handler as a matter of the OS
-	 * walking the chain.  As the chain is walked, the interrupt will
-	 * eventually be serviced by the correct driver/handler.
+	 * Marked as hardware available. Don't service interrupts, just clear the
+	 * event.
 	 */
-#if 0
-	/* chained interrupt = not ours */
-	pr_info(">> %s: intCnt NULL, sts %x, possibly a chained interrupt!\n",
-		ci->devname, status);
-#endif
-	return IRQ_NONE;
-    }
 
-    ci->iqp_tailx = currInt;
+	if (ci->state == C_IDLE) {
+		status = pci_read_32((u_int32_t *) &ci->reg->isd);
+
+		/* clear the interrupt but process nothing else */
+		pci_write_32((u_int32_t *) &ci->reg->isd, status);
+		return IRQ_HANDLED;
+	}
+	FLUSH_PCI_READ();
+	FLUSH_MEM_READ();
+
+	status = pci_read_32((u_int32_t *) &ci->reg->isd);
+	nextInt = INTRPTS_NEXTINT(status);
+	intCnt = INTRPTS_INTCNT(status);
+	ci->intlog.drvr_intr_thcount++;
+
+	/*********************************************************/
+	/* HW Bug Fix                                            */
+	/* ----------                                            */
+	/* Under certain PCI Bus loading conditions, the         */
+	/* MUSYCC looses the data associated with an update      */
+	/* of its ISD and erroneously returns the immediately    */
+	/* preceding 'nextInt' value.  However, the 'intCnt'     */
+	/* value appears to be correct.  By not starting service */
+	/* where the 'missing' 'nextInt' SHOULD point causes     */
+	/* the IQD not to be serviced - the 'not serviced'       */
+	/* entries then remain and continue to increase as more  */
+	/* incorrect ISD's are encountered.                      */
+	/*********************************************************/
+
+	if (nextInt != INTRPTS_NEXTINT(ci->intlog.this_status_new)) {
+		if (cxt1e1_log_level >= LOG_MONITOR) {
+			pr_info("%s: note - updated ISD from %08x to %08x\n",
+				ci->devname, status,
+				(status & (~INTRPTS_NEXTINT_M)) |
+				ci->intlog.this_status_new);
+		}
+		/*
+		 * Replace bogus status with software corrected value.
+		 *
+		 * It's not known whether, during this problem occurrence, if the
+		 * INTFULL bit is correctly reported or not.
+		 */
+		status = (status & (~INTRPTS_NEXTINT_M)) |
+			 (ci->intlog.this_status_new);
+		nextInt = INTRPTS_NEXTINT(status);
+	}
+	/**********************************************/
+	/* Cn847x Bug Fix                             */
+	/* --------------                             */
+	/* Fix for inability to write back same index */
+	/* as read for a full interrupt queue.        */
+	/**********************************************/
+
+	if (intCnt == INT_QUEUE_SIZE)
+		currInt = ((intCnt - 1) + nextInt) & (INT_QUEUE_SIZE - 1);
+	else
+		/************************************************/
+		/* Interrupt Write Location Issues              */
+		/* -------------------------------              */
+		/* When the interrupt status descriptor is      */
+		/* written, the interrupt line is de-asserted   */
+		/* by the Cn847x.  In the case of MIPS          */
+		/* microprocessors, this must occur at the      */
+		/* beginning of the interrupt handler so that   */
+		/* the interrupt handle is not re-entered due   */
+		/* to interrupt dis-assertion latency.          */
+		/* In the case of all other processors, this    */
+		/* action should occur at the end of the        */
+		/* interrupt handler to avoid overwriting the   */
+		/* interrupt queue.                             */
+		/************************************************/
+
+		if (intCnt)
+			currInt = (intCnt + nextInt) & (INT_QUEUE_SIZE - 1);
+		else {
+			/*
+			 * NOTE: Servicing an interrupt whose ISD contains a count of ZERO
+			 * can be indicative of a Shared Interrupt chain.  Our driver can be
+			 * called from the system's interrupt handler as a matter of the OS
+			 * walking the chain.  As the chain is walked, the interrupt will
+			 * eventually be serviced by the correct driver/handler.
+			 */
+			return IRQ_NONE;
+		}
+
+	ci->iqp_tailx = currInt;
 
-    currInt <<= INTRPTS_NEXTINT_S;
-    ci->intlog.last_status_new = ci->intlog.this_status_new;
-    ci->intlog.this_status_new = currInt;
+	currInt <<= INTRPTS_NEXTINT_S;
+	ci->intlog.last_status_new = ci->intlog.this_status_new;
+	ci->intlog.this_status_new = currInt;
 
-    if ((cxt1e1_log_level >= LOG_WARN) && (status & INTRPTS_INTFULL_M))
-	pr_info("%s: Interrupt queue full condition occurred\n", ci->devname);
-    if (cxt1e1_log_level >= LOG_DEBUG)
-	pr_info("%s: interrupts pending, isd @ 0x%p: %x curr %d cnt %d NEXT %d\n",
-		ci->devname, &ci->reg->isd,
-	status, nextInt, intCnt, (intCnt + nextInt) & (INT_QUEUE_SIZE - 1));
+	if ((cxt1e1_log_level >= LOG_WARN) && (status & INTRPTS_INTFULL_M))
+		pr_info("%s: Interrupt queue full condition occurred\n",
+			ci->devname);
+	if (cxt1e1_log_level >= LOG_DEBUG)
+		pr_info("%s: interrupts pending, isd @ 0x%p: %x curr %d cnt %d NEXT %d\n",
+			ci->devname, &ci->reg->isd,
+			status, nextInt, intCnt,
+			(intCnt + nextInt) & (INT_QUEUE_SIZE - 1));
 
-    FLUSH_MEM_WRITE();
+	FLUSH_MEM_WRITE();
 #if defined(SBE_ISR_TASKLET)
-    pci_write_32((u_int32_t *) &ci->reg->isd, currInt);
-    atomic_inc(&ci->bh_pending);
-    tasklet_schedule(&ci->ci_musycc_isr_tasklet);
+	pci_write_32((u_int32_t *) &ci->reg->isd, currInt);
+	atomic_inc(&ci->bh_pending);
+	tasklet_schedule(&ci->ci_musycc_isr_tasklet);
 #elif defined(SBE_ISR_IMMEDIATE)
-    pci_write_32((u_int32_t *) &ci->reg->isd, currInt);
-    atomic_inc(&ci->bh_pending);
-    queue_task(&ci->ci_musycc_isr_tq, &tq_immediate);
-    mark_bh(IMMEDIATE_BH);
+	pci_write_32((u_int32_t *) &ci->reg->isd, currInt);
+	atomic_inc(&ci->bh_pending);
+	queue_task(&ci->ci_musycc_isr_tq, &tq_immediate);
+	mark_bh(IMMEDIATE_BH);
 #elif defined(SBE_ISR_INLINE)
-    (void) musycc_intr_bh_tasklet(ci);
-    pci_write_32((u_int32_t *) &ci->reg->isd, currInt);
+	(void) musycc_intr_bh_tasklet(ci);
+	pci_write_32((u_int32_t *) &ci->reg->isd, currInt);
 #endif
-    return IRQ_HANDLED;
+	return IRQ_HANDLED;
 }
 
 
@@ -1226,574 +1207,513 @@ void
 #endif
 musycc_intr_bh_tasklet(ci_t *ci)
 {
-    mpi_t      *pi;
-    mch_t      *ch;
-    unsigned int intCnt;
-    volatile u_int32_t currInt = 0;
-    volatile unsigned int headx, tailx;
-    int         readCount, loopCount;
-    int         group, gchan, event, err, tx;
-    u_int32_t   badInt = INT_EMPTY_ENTRY;
-    u_int32_t   badInt2 = INT_EMPTY_ENTRY2;
-
-    /*
-     * Hardware not available, potential interrupt hang.  But since interrupt
-     * might be shared, just return.
-     */
-    if ((drvr_state != SBE_DRVR_AVAILABLE) || (ci->state == C_INIT)) {
+	mpi_t      *pi;
+	mch_t      *ch;
+	unsigned int intCnt;
+	volatile u_int32_t currInt = 0;
+	volatile unsigned int headx, tailx;
+	int         readCount, loopCount;
+	int         group, gchan, event, err, tx;
+	u_int32_t   badInt = INT_EMPTY_ENTRY;
+	u_int32_t   badInt2 = INT_EMPTY_ENTRY2;
+
+	/*
+	 * Hardware not available, potential interrupt hang.  But since interrupt
+	 * might be shared, just return.
+	 */
+	if ((drvr_state != SBE_DRVR_AVAILABLE) || (ci->state == C_INIT)) {
 #if defined(SBE_ISR_IMMEDIATE)
-	return 0L;
+		return 0L;
 #else
-	return;
+		return;
 #endif
-    }
+	}
 #if defined(SBE_ISR_TASKLET) || defined(SBE_ISR_IMMEDIATE)
-    if (drvr_state != SBE_DRVR_AVAILABLE) {
+	if (drvr_state != SBE_DRVR_AVAILABLE) {
 #if defined(SBE_ISR_TASKLET)
-	return;
+		return;
 #elif defined(SBE_ISR_IMMEDIATE)
-	return 0L;
+		return 0L;
 #endif
-    }
+	}
 #elif defined(SBE_ISR_INLINE)
-    /* no semaphore taken, no double checks */
+	/* no semaphore taken, no double checks */
 #endif
 
-    ci->intlog.drvr_intr_bhcount++;
-    FLUSH_MEM_READ();
-    {
-	unsigned int bh = atomic_read(&ci->bh_pending);
-
-	max_bh = max(bh, max_bh);
-    }
-    atomic_set(&ci->bh_pending, 0);/* if here, no longer pending */
-    while ((headx = ci->iqp_headx) != (tailx = ci->iqp_tailx)) {
-	intCnt = (tailx >= headx) ? (tailx - headx) : (tailx - headx + INT_QUEUE_SIZE);
-	currInt = le32_to_cpu(ci->iqd_p[headx]);
-
-	max_intcnt = max(intCnt, max_intcnt);  /* RLD DEBUG */
-
-	/**************************************************/
-	/* HW Bug Fix                                     */
-	/* ----------                                     */
-	/* The following code checks for the condition    */
-	/* of interrupt assertion before interrupt        */
-	/* queue update.  This is a problem on several    */
-	/* PCI-Local bridge chips found on some products. */
-	/**************************************************/
-
-	readCount = 0;
-	if ((currInt == badInt) || (currInt == badInt2))
-	    ci->intlog.drvr_int_failure++;
-
-	while ((currInt == badInt) || (currInt == badInt2)) {
-	    for (loopCount = 0; loopCount < 0x30; loopCount++)
-		OS_uwait_dummy();  /* use call to avoid optimization removal
-				     * of dummy delay */
-	    FLUSH_MEM_READ();
-	    currInt = le32_to_cpu(ci->iqd_p[headx]);
-	    if (readCount++ > 20)
-		break;
-	}
+	ci->intlog.drvr_intr_bhcount++;
+	FLUSH_MEM_READ();
+	{
+		unsigned int bh = atomic_read(&ci->bh_pending);
 
-	if ((currInt == badInt) || (currInt == badInt2)) {      /* catch failure of Bug
-								 * Fix checking */
-	    if (cxt1e1_log_level >= LOG_WARN)
-		pr_info("%s: Illegal Interrupt Detected @ 0x%p, mod %d.)\n",
-			ci->devname, &ci->iqd_p[headx], headx);
-
-	    /*
-	     * If the descriptor has not recovered, then leaving the EMPTY
-	     * entry set will not signal to the MUSYCC that this descriptor
-	     * has been serviced. The Interrupt Queue can then start losing
-	     * available descriptors and MUSYCC eventually encounters and
-	     * reports the INTFULL condition.  Per manual, changing any bit
-	     * marks descriptor as available, thus the use of different
-	     * EMPTY_ENTRY values.
-	     */
-
-	    if (currInt == badInt)
-		ci->iqd_p[headx] = __constant_cpu_to_le32(INT_EMPTY_ENTRY2);
-	    else
-		ci->iqd_p[headx] = __constant_cpu_to_le32(INT_EMPTY_ENTRY);
-	    ci->iqp_headx = (headx + 1) & (INT_QUEUE_SIZE - 1); /* insure wrapness */
-	    FLUSH_MEM_WRITE();
-	    FLUSH_MEM_READ();
-	    continue;
+		max_bh = max(bh, max_bh);
 	}
-	group = INTRPT_GRP(currInt);
-	gchan = INTRPT_CH(currInt);
-	event = INTRPT_EVENT(currInt);
-	err = INTRPT_ERROR(currInt);
-	tx = currInt & INTRPT_DIR_M;
-
-	ci->iqd_p[headx] = __constant_cpu_to_le32(INT_EMPTY_ENTRY);
-	FLUSH_MEM_WRITE();
+	atomic_set(&ci->bh_pending, 0);/* if here, no longer pending */
+	while ((headx = ci->iqp_headx) != (tailx = ci->iqp_tailx)) {
+		intCnt = (tailx >= headx) ? (tailx - headx) : (tailx - headx + INT_QUEUE_SIZE);
+		currInt = le32_to_cpu(ci->iqd_p[headx]);
+
+		max_intcnt = max(intCnt, max_intcnt);  /* RLD DEBUG */
+
+		/**************************************************/
+		/* HW Bug Fix                                     */
+		/* ----------                                     */
+		/* The following code checks for the condition    */
+		/* of interrupt assertion before interrupt        */
+		/* queue update.  This is a problem on several    */
+		/* PCI-Local bridge chips found on some products. */
+		/**************************************************/
+
+		readCount = 0;
+		if ((currInt == badInt) || (currInt == badInt2))
+			ci->intlog.drvr_int_failure++;
+
+		while ((currInt == badInt) || (currInt == badInt2)) {
+			for (loopCount = 0; loopCount < 0x30; loopCount++)
+				/* use call to avoid optimization
+				 * removal of dummy delay
+				 */
+				OS_uwait_dummy();
+			FLUSH_MEM_READ();
+			currInt = le32_to_cpu(ci->iqd_p[headx]);
+			if (readCount++ > 20)
+				break;
+		}
 
-	if (cxt1e1_log_level >= LOG_DEBUG) {
-	    if (err != 0)
-		pr_info(" %08x -> err: %2d,", currInt, err);
+		/* catch failure of Bug Fix checking */
+		if ((currInt == badInt) || (currInt == badInt2)) {
+			if (cxt1e1_log_level >= LOG_WARN)
+				pr_info("%s: Illegal Interrupt Detected @ 0x%p, mod %d.)\n",
+					ci->devname, &ci->iqd_p[headx], headx);
+
+			/*
+			 * If the descriptor has not recovered, then leaving the EMPTY
+			 * entry set will not signal to the MUSYCC that this descriptor
+			 * has been serviced. The Interrupt Queue can then start losing
+			 * available descriptors and MUSYCC eventually encounters and
+			 * reports the INTFULL condition.  Per manual, changing any bit
+			 * marks descriptor as available, thus the use of different
+			 * EMPTY_ENTRY values.
+			 */
+
+			if (currInt == badInt)
+				ci->iqd_p[headx] = __constant_cpu_to_le32(INT_EMPTY_ENTRY2);
+			else
+				ci->iqd_p[headx] = __constant_cpu_to_le32(INT_EMPTY_ENTRY);
+			/* insure wrapness */
+			ci->iqp_headx = (headx + 1) & (INT_QUEUE_SIZE - 1);
+			FLUSH_MEM_WRITE();
+			FLUSH_MEM_READ();
+			continue;
+		}
+		group = INTRPT_GRP(currInt);
+		gchan = INTRPT_CH(currInt);
+		event = INTRPT_EVENT(currInt);
+		err = INTRPT_ERROR(currInt);
+		tx = currInt & INTRPT_DIR_M;
 
-	    pr_info("+ interrupt event: %d, grp: %d, chan: %2d, side: %cX\n",
-		    event, group, gchan, tx ? 'T' : 'R');
-	}
-	pi = &ci->port[group];      /* notice that here we assume 1-1 group -
-				     * port mapping */
-	ch = pi->chan[gchan];
-	switch (event) {
-	case EVE_SACK:              /* Service Request Acknowledge */
-	    if (cxt1e1_log_level >= LOG_DEBUG) {
-		volatile u_int32_t r;
-
-		r = pci_read_32((u_int32_t *) &pi->reg->srd);
-		pr_info("- SACK cmd: %08x (hdw= %08x)\n", pi->sr_last, r);
-	    }
-	    SD_SEM_GIVE(&pi->sr_sem_wait);     /* wake up waiting process */
-	    break;
-	case EVE_CHABT:     /* Change To Abort Code (0x7e -> 0xff) */
-	case EVE_CHIC:              /* Change To Idle Code (0xff -> 0x7e) */
-	    break;
-	case EVE_EOM:               /* End Of Message */
-	case EVE_EOB:               /* End Of Buffer (Transparent mode) */
-	    if (tx)
-		musycc_bh_tx_eom(pi, gchan);
-	    else
-		musycc_bh_rx_eom(pi, gchan);
-#if 0
-	    break;
-#else
-	    /*
-	     * MUSYCC Interrupt Descriptor section states that EOB and EOM
-	     * can be combined with the NONE error (as well as others).  So
-	     * drop thru to catch this...
-	     */
-#endif
-	case EVE_NONE:
-	    if (err == ERR_SHT)
-		ch->s.rx_length_errors++;
-	    break;
-	default:
-	    if (cxt1e1_log_level >= LOG_WARN)
-		pr_info("%s: unexpected interrupt event: %d, iqd[%d]: %08x, port: %d\n", ci->devname,
-			event, headx, currInt, group);
-	    break;
-	}                           /* switch on event */
+		ci->iqd_p[headx] = __constant_cpu_to_le32(INT_EMPTY_ENTRY);
+		FLUSH_MEM_WRITE();
 
+		if (cxt1e1_log_level >= LOG_DEBUG) {
+			if (err != 0)
+				pr_info(" %08x -> err: %2d,", currInt, err);
 
-	/*
-	 * Per MUSYCC Manual, Section 6.4.8.3 [Transmit Errors], TX errors
-	 * are service-affecting and require action to resume normal
-	 * bit-level processing.
-	 */
+			pr_info("+ interrupt event: %d, grp: %d, chan: %2d, side: %cX\n",
+				event, group, gchan, tx ? 'T' : 'R');
+		}
+		/* notice that here we assume 1-1 group - port mapping */
+		pi = &ci->port[group];
+		ch = pi->chan[gchan];
+		switch (event) {
+		case EVE_SACK:              /* Service Request Acknowledge */
+			if (cxt1e1_log_level >= LOG_DEBUG) {
+				volatile u_int32_t r;
+
+				r = pci_read_32((u_int32_t *) &pi->reg->srd);
+				pr_info("- SACK cmd: %08x (hdw= %08x)\n",
+					pi->sr_last, r);
+			}
+			/* wake up waiting process */
+			SD_SEM_GIVE(&pi->sr_sem_wait);
+			break;
+		case EVE_CHABT: /* Change To Abort Code (0x7e -> 0xff) */
+		case EVE_CHIC:  /* Change To Idle Code (0xff -> 0x7e) */
+			break;
+		case EVE_EOM:   /* End Of Message */
+		case EVE_EOB:   /* End Of Buffer (Transparent mode) */
+			if (tx)
+				musycc_bh_tx_eom(pi, gchan);
+			else
+				musycc_bh_rx_eom(pi, gchan);
+			/*
+			 * MUSYCC Interrupt Descriptor section states that EOB and EOM
+			 * can be combined with the NONE error (as well as others).  So
+			 * drop thru to catch this...
+			 */
+		case EVE_NONE:
+			if (err == ERR_SHT)
+				ch->s.rx_length_errors++;
+			break;
+		default:
+			if (cxt1e1_log_level >= LOG_WARN)
+				pr_info("%s: unexpected interrupt event: %d, iqd[%d]: %08x, port: %d\n", ci->devname,
+					event, headx, currInt, group);
+			break;
+		}                           /* switch on event */
 
-	switch (err) {
-	case ERR_ONR:
-	    /*
-	     * Per MUSYCC manual, Section  6.4.8.3 [Transmit Errors], this
-	     * error requires Transmit channel reactivation.
-	     *
-	     * Per MUSYCC manual, Section  6.4.8.4 [Receive Errors], this error
-	     * requires Receive channel reactivation.
-	     */
-	    if (tx) {
 
 		/*
-		 * TX ONR Error only occurs when channel is configured for
-		 * Transparent Mode.  However, this code will catch and
-		 * re-activate on ANY TX ONR error.
+		 * Per MUSYCC Manual, Section 6.4.8.3 [Transmit Errors], TX errors
+		 * are service-affecting and require action to resume normal
+		 * bit-level processing.
 		 */
 
-		/*
-		 * Set flag to re-enable on any next transmit attempt.
-		 */
-		ch->ch_start_tx = CH_START_TX_ONR;
+		switch (err) {
+		case ERR_ONR:
+			/*
+			 * Per MUSYCC manual, Section  6.4.8.3 [Transmit Errors], this
+			 * error requires Transmit channel reactivation.
+			 *
+			 * Per MUSYCC manual, Section  6.4.8.4 [Receive Errors], this error
+			 * requires Receive channel reactivation.
+			 */
+			if (tx) {
+
+				/*
+				 * TX ONR Error only occurs when channel is configured for
+				 * Transparent Mode.  However, this code will catch and
+				 * re-activate on ANY TX ONR error.
+				 */
+
+				/*
+				 * Set flag to re-enable on any next transmit attempt.
+				 */
+				ch->ch_start_tx = CH_START_TX_ONR;
 
-		{
 #ifdef RLD_TRANS_DEBUG
-		    if (1 || cxt1e1_log_level >= LOG_MONITOR)
+				if (1 || cxt1e1_log_level >= LOG_MONITOR)
 #else
-		    if (cxt1e1_log_level >= LOG_MONITOR)
+				if (cxt1e1_log_level >= LOG_MONITOR)
 #endif
-		    {
-			pr_info("%s: TX buffer underflow [ONR] on channel %d, mode %x QStopped %x free %d\n",
-				ci->devname, ch->channum, ch->p.chan_mode, sd_queue_stopped(ch->user), ch->txd_free);
+				{
+					pr_info("%s: TX buffer underflow [ONR] on channel %d, mode %x QStopped %x free %d\n",
+						ci->devname, ch->channum,
+						ch->p.chan_mode,
+						sd_queue_stopped(ch->user),
+						ch->txd_free);
 #ifdef RLD_DEBUG
-			if (ch->p.chan_mode == 2) {     /* problem = ONR on HDLC
-							 * mode */
-			    pr_info("++ Failed Last %x Next %x QStopped %x, start_tx %x tx_full %d txd_free %d mode %x\n",
-				    (u_int32_t) ch->txd_irq_srv, (u_int32_t) ch->txd_usr_add,
-				    sd_queue_stopped(ch->user),
-				    ch->ch_start_tx, ch->tx_full, ch->txd_free, ch->p.chan_mode);
-			    musycc_dump_txbuffer_ring(ch, 0);
-			}
+					/* problem = ONR on HDLC mode */
+					if (ch->p.chan_mode == 2) {
+						pr_info("++ Failed Last %x Next %x QStopped %x, start_tx %x tx_full %d txd_free %d mode %x\n",
+							(u_int32_t)ch->txd_irq_srv,
+							(u_int32_t)ch->txd_usr_add,
+							sd_queue_stopped(ch->user),
+							ch->ch_start_tx,
+							ch->tx_full,
+							ch->txd_free,
+							ch->p.chan_mode);
+						musycc_dump_txbuffer_ring(ch, 0);
+					}
 #endif
-		    }
-		}
-	    } else {                 /* RX buffer overrun */
-		/*
-		 * Per MUSYCC manual, Section 6.4.8.4 [Receive Errors],
-		 * channel recovery for this RX ONR error IS required.  It is
-		 * also suggested to increase the number of receive buffers
-		 * for this channel.  Receive channel reactivation IS
-		 * required, and data has been lost.
-		 */
-		ch->s.rx_over_errors++;
-		ch->ch_start_rx = CH_START_RX_ONR;
-
-		if (cxt1e1_log_level >= LOG_WARN) {
-		    pr_info("%s: RX buffer overflow [ONR] on channel %d, mode %x\n",
-			    ci->devname, ch->channum, ch->p.chan_mode);
-		    //musycc_dump_rxbuffer_ring (ch, 0);        /* RLD DEBUG */
-		}
-	    }
-	    musycc_chan_restart(ch);
-	    break;
-	case ERR_BUF:
-	    if (tx) {
-		ch->s.tx_fifo_errors++;
-		ch->ch_start_tx = CH_START_TX_BUF;
-		/*
-		 * Per MUSYCC manual, Section  6.4.8.3 [Transmit Errors],
-		 * this BUFF error requires Transmit channel reactivation.
-		 */
-		if (cxt1e1_log_level >= LOG_MONITOR)
-		    pr_info("%s: TX buffer underrun [BUFF] on channel %d, mode %x\n",
-			    ci->devname, ch->channum, ch->p.chan_mode);
-	    } else {                 /* RX buffer overrun */
-		ch->s.rx_over_errors++;
-		/*
-		 * Per MUSYCC manual, Section 6.4.8.4 [Receive Errors], HDLC
-		 * mode requires NO recovery for this RX BUFF error is
-		 * required.  It is suggested to increase the FIFO buffer
-		 * space for this channel.  Receive channel reactivation is
-		 * not required, but data has been lost.
-		 */
-		if (cxt1e1_log_level >= LOG_WARN)
-		    pr_info("%s: RX buffer overrun [BUFF] on channel %d, mode %x\n",
-			    ci->devname, ch->channum, ch->p.chan_mode);
-		/*
-		 * Per MUSYCC manual, Section 6.4.9.4 [Receive Errors],
-		 * Transparent mode DOES require recovery for the RX BUFF
-		 * error.  It is suggested to increase the FIFO buffer space
-		 * for this channel.  Receive channel reactivation IS
-		 * required and data has been lost.
-		 */
-		if (ch->p.chan_mode == CFG_CH_PROTO_TRANS)
-		    ch->ch_start_rx = CH_START_RX_BUF;
-	    }
-
-	    if (tx || (ch->p.chan_mode == CFG_CH_PROTO_TRANS))
-		musycc_chan_restart(ch);
-	    break;
-	default:
-	    break;
-	}                           /* switch on err */
-
-	/* Check for interrupt lost condition */
-	if ((currInt & INTRPT_ILOST_M) && (cxt1e1_log_level >= LOG_ERROR))
-	    pr_info("%s: Interrupt queue overflow - ILOST asserted\n",
-		    ci->devname);
-	ci->iqp_headx = (headx + 1) & (INT_QUEUE_SIZE - 1);     /* insure wrapness */
-	FLUSH_MEM_WRITE();
-	FLUSH_MEM_READ();
-    }                               /* while */
-    if ((cxt1e1_log_level >= LOG_MONITOR2) && (ci->iqp_headx != ci->iqp_tailx)) {
-	int         bh;
-
-	bh = atomic_read(&CI->bh_pending);
-	pr_info("_bh_: late arrivals, head %d != tail %d, pending %d\n",
-		ci->iqp_headx, ci->iqp_tailx, bh);
-    }
-#if defined(SBE_ISR_IMMEDIATE)
-    return 0L;
+				}
+			} else {                 /* RX buffer overrun */
+				/*
+				 * Per MUSYCC manual, Section 6.4.8.4 [Receive Errors],
+				 * channel recovery for this RX ONR error IS required.  It is
+				 * also suggested to increase the number of receive buffers
+				 * for this channel.  Receive channel reactivation IS
+				 * required, and data has been lost.
+				 */
+				ch->s.rx_over_errors++;
+				ch->ch_start_rx = CH_START_RX_ONR;
+
+				if (cxt1e1_log_level >= LOG_WARN) {
+					pr_info("%s: RX buffer overflow [ONR] on channel %d, mode %x\n",
+						ci->devname, ch->channum,
+						ch->p.chan_mode);
+#ifdef RLD_DEBUG
+					musycc_dump_rxbuffer_ring(ch, 0);
 #endif
-    /* else, nothing returned */
-}
+				}
+			}
+			musycc_chan_restart(ch);
+			break;
+		case ERR_BUF:
+			if (tx) {
+				ch->s.tx_fifo_errors++;
+				ch->ch_start_tx = CH_START_TX_BUF;
+				/*
+				 * Per MUSYCC manual, Section  6.4.8.3 [Transmit Errors],
+				 * this BUFF error requires Transmit channel reactivation.
+				 */
+				if (cxt1e1_log_level >= LOG_MONITOR)
+					pr_info("%s: TX buffer underrun [BUFF] on channel %d, mode %x\n",
+						ci->devname, ch->channum,
+						ch->p.chan_mode);
+			} else {                 /* RX buffer overrun */
+				ch->s.rx_over_errors++;
+				/*
+				 * Per MUSYCC manual, Section 6.4.8.4 [Receive Errors], HDLC
+				 * mode requires NO recovery for this RX BUFF error is
+				 * required.  It is suggested to increase the FIFO buffer
+				 * space for this channel.  Receive channel reactivation is
+				 * not required, but data has been lost.
+				 */
+				if (cxt1e1_log_level >= LOG_WARN)
+					pr_info("%s: RX buffer overrun [BUFF] on channel %d, mode %x\n",
+						ci->devname, ch->channum,
+						ch->p.chan_mode);
+				/*
+				 * Per MUSYCC manual, Section 6.4.9.4 [Receive Errors],
+				 * Transparent mode DOES require recovery for the RX BUFF
+				 * error.  It is suggested to increase the FIFO buffer space
+				 * for this channel.  Receive channel reactivation IS
+				 * required and data has been lost.
+				 */
+				if (ch->p.chan_mode == CFG_CH_PROTO_TRANS)
+					ch->ch_start_rx = CH_START_RX_BUF;
+			}
 
-#if 0
-int         __init
-musycc_new_chan(ci_t *ci, int channum, void *user)
-{
-    mch_t      *ch;
-
-    ch = ci->port[channum / MUSYCC_NCHANS].chan[channum % MUSYCC_NCHANS];
-
-    if (ch->state != UNASSIGNED)
-	return EEXIST;
-    /* NOTE: mch_t already cleared during OS_kmalloc() */
-    ch->state = DOWN;
-    ch->user = user;
-#if 0
-    ch->status = 0;
-    ch->p.status = 0;
-    ch->p.intr_mask = 0;
+			if (tx || (ch->p.chan_mode == CFG_CH_PROTO_TRANS))
+				musycc_chan_restart(ch);
+			break;
+		default:
+			break;
+		}                           /* switch on err */
+
+		/* Check for interrupt lost condition */
+		if ((currInt & INTRPT_ILOST_M) &&
+		    (cxt1e1_log_level >= LOG_ERROR))
+			pr_info("%s: Interrupt queue overflow - ILOST asserted\n",
+				ci->devname);
+		/* insure wrapness */
+		ci->iqp_headx = (headx + 1) & (INT_QUEUE_SIZE - 1);
+		FLUSH_MEM_WRITE();
+		FLUSH_MEM_READ();
+	}                               /* while */
+	if ((cxt1e1_log_level >= LOG_MONITOR2) &&
+	    (ci->iqp_headx != ci->iqp_tailx)) {
+		int         bh;
+
+		bh = atomic_read(&CI->bh_pending);
+		pr_info("_bh_: late arrivals, head %d != tail %d, pending %d\n",
+			ci->iqp_headx, ci->iqp_tailx, bh);
+	}
+#if defined(SBE_ISR_IMMEDIATE)
+	return 0L;
 #endif
-    ch->p.chan_mode = CFG_CH_PROTO_HDLC_FCS16;
-    ch->p.idlecode = CFG_CH_FLAG_7E;
-    ch->p.pad_fill_count = 2;
-    spin_lock_init(&ch->ch_rxlock);
-    spin_lock_init(&ch->ch_txlock);
-
-    return 0;
+	/* else, nothing returned */
 }
-#endif
-
 
 #ifdef SBE_PMCC4_ENABLE
-status_t
+	status_t
 musycc_chan_down(ci_t *dummy, int channum)
 {
-    mpi_t      *pi;
-    mch_t      *ch;
-    int         i, gchan;
-
-    ch = sd_find_chan(dummy, channum);
-    if (!ch)
-	return -EINVAL;
-    pi = ch->up;
-    gchan = ch->gchan;
-
-    /* Deactivate the channel */
-    musycc_serv_req(pi, SR_CHANNEL_DEACTIVATE | SR_RX_DIRECTION | gchan);
-    ch->ch_start_rx = 0;
-    musycc_serv_req(pi, SR_CHANNEL_DEACTIVATE | SR_TX_DIRECTION | gchan);
-    ch->ch_start_tx = 0;
-
-    if (ch->state == DOWN)
-	return 0;
-    ch->state = DOWN;
-
-    pi->regram->thp[gchan] = 0;
-    pi->regram->tmp[gchan] = 0;
-    pi->regram->rhp[gchan] = 0;
-    pi->regram->rmp[gchan] = 0;
-    FLUSH_MEM_WRITE();
-    for (i = 0; i < ch->txd_num; i++)
-	if (ch->mdt[i].mem_token)
-	    OS_mem_token_free(ch->mdt[i].mem_token);
-
-    for (i = 0; i < ch->rxd_num; i++)
-	if (ch->mdr[i].mem_token)
-	    OS_mem_token_free(ch->mdr[i].mem_token);
-
-    OS_kfree(ch->mdr);
-    ch->mdr = NULL;
-    ch->rxd_num = 0;
-    OS_kfree(ch->mdt);
-    ch->mdt = NULL;
-    ch->txd_num = 0;
-
-    musycc_update_timeslots(pi);
-    c4_fifo_free(pi, ch->gchan);
-
-    pi->openchans--;
-    return 0;
-}
-#endif
+	mpi_t      *pi;
+	mch_t      *ch;
+	int         i, gchan;
+
+	ch = sd_find_chan(dummy, channum);
+	if (!ch)
+		return -EINVAL;
+	pi = ch->up;
+	gchan = ch->gchan;
+
+	/* Deactivate the channel */
+	musycc_serv_req(pi, SR_CHANNEL_DEACTIVATE | SR_RX_DIRECTION | gchan);
+	ch->ch_start_rx = 0;
+	musycc_serv_req(pi, SR_CHANNEL_DEACTIVATE | SR_TX_DIRECTION | gchan);
+	ch->ch_start_tx = 0;
 
+	if (ch->state == DOWN)
+		return 0;
+	ch->state = DOWN;
 
-#if 0
-/* TODO: determine if these functions will not be needed and can be removed */
-int
-musycc_del_chan(ci_t *ci, int channum)
-{
-    mch_t      *ch;
-
-    if ((channum < 0) || (channum >= (MUSYCC_NPORTS * MUSYCC_NCHANS)))  /* sanity chk param */
-	return ECHRNG;
-    ch = sd_find_chan(ci, channum);
-    if (!ch)
-	return ENOENT;
-    if (ch->state == UP)
-	musycc_chan_down(ci, channum);
-    ch->state = UNASSIGNED;
-    return 0;
-}
+	pi->regram->thp[gchan] = 0;
+	pi->regram->tmp[gchan] = 0;
+	pi->regram->rhp[gchan] = 0;
+	pi->regram->rmp[gchan] = 0;
+	FLUSH_MEM_WRITE();
+	for (i = 0; i < ch->txd_num; i++)
+		if (ch->mdt[i].mem_token)
+			OS_mem_token_free(ch->mdt[i].mem_token);
 
+	for (i = 0; i < ch->rxd_num; i++)
+		if (ch->mdr[i].mem_token)
+			OS_mem_token_free(ch->mdr[i].mem_token);
 
-int
-musycc_del_chan_stats(ci_t *ci, int channum)
-{
-    mch_t      *ch;
+	kfree(ch->mdr);
+	ch->mdr = NULL;
+	ch->rxd_num = 0;
+	kfree(ch->mdt);
+	ch->mdt = NULL;
+	ch->txd_num = 0;
 
-    if (channum < 0 || channum >= (MUSYCC_NPORTS * MUSYCC_NCHANS))      /* sanity chk param */
-	return ECHRNG;
-    ch = sd_find_chan(ci, channum);
-    if (!ch)
-	return ENOENT;
+	musycc_update_timeslots(pi);
+	c4_fifo_free(pi, ch->gchan);
 
-    memset(&ch->s, 0, sizeof(struct sbecom_chan_stats));
-    return 0;
+	pi->openchans--;
+	return 0;
 }
 #endif
 
-
 int
 musycc_start_xmit(ci_t *ci, int channum, void *mem_token)
 {
-    mch_t      *ch;
-    struct mdesc *md;
-    void       *m2;
-#if 0
-    unsigned long flags;
+	mch_t      *ch;
+	struct mdesc *md;
+	void       *m2;
+	int         txd_need_cnt;
+	u_int32_t   len;
+
+	ch = sd_find_chan(ci, channum);
+	if (!ch)
+		return -ENOENT;
+
+	/* full interrupt processing available */
+	if (ci->state != C_RUNNING)
+		return -EINVAL;
+	if (ch->state != UP)
+		return -EINVAL;
+
+	/* how else to flag unwritable state ? */
+	if (!(ch->status & TX_ENABLED))
+		return -EROFS;
+
+#ifdef RLD_TRANS_DEBUG
+	if (1 || cxt1e1_log_level >= LOG_MONITOR2)
+#else
+	if (cxt1e1_log_level >= LOG_MONITOR2)
 #endif
-    int         txd_need_cnt;
-    u_int32_t   len;
+	{
+		pr_info("++ start_xmt[%d]: state %x start %x full %d free %d required %d stopped %x\n",
+			channum, ch->state, ch->ch_start_tx, ch->tx_full,
+			ch->txd_free, ch->txd_required,
+			sd_queue_stopped(ch->user));
+	}
+	/***********************************************/
+	/** Determine total amount of data to be sent **/
+	/***********************************************/
+	m2 = mem_token;
+	txd_need_cnt = 0;
+	for (len = OS_mem_token_tlen(m2); len > 0;
+	     m2 = (void *) OS_mem_token_next(m2)) {
+		if (!OS_mem_token_len(m2))
+			continue;
+		txd_need_cnt++;
+		len -= OS_mem_token_len(m2);
+	}
 
-    ch = sd_find_chan(ci, channum);
-    if (!ch)
-	return -ENOENT;
+	if (txd_need_cnt == 0) {
+		if (cxt1e1_log_level >= LOG_MONITOR2)
+			pr_info("%s channel %d: no TX data in User buffer\n",
+				ci->devname, channum);
+		OS_mem_token_free(mem_token);
+		return 0;                   /* no data to send */
+	}
+	/*************************************************/
+	/** Are there sufficient descriptors available? **/
+	/*************************************************/
+	if (txd_need_cnt > ch->txd_num) { /* never enough descriptors for this
+					   * large a buffer */
+		if (cxt1e1_log_level >= LOG_DEBUG)
+			pr_info("start_xmit: discarding buffer, insufficient descriptor cnt %d, need %d.\n",
+				ch->txd_num, txd_need_cnt + 1);
+		ch->s.tx_dropped++;
+		OS_mem_token_free(mem_token);
+		return 0;
+	}
 
-    if (ci->state != C_RUNNING)     /* full interrupt processing available */
-	return -EINVAL;
-    if (ch->state != UP)
-	return -EINVAL;
+	/************************************************************/
+	/** flow control the line if not enough descriptors remain **/
+	/************************************************************/
+	if (txd_need_cnt > ch->txd_free) {
+		if (cxt1e1_log_level >= LOG_MONITOR2)
+			pr_info("start_xmit[%d]: EBUSY - need more descriptors, have %d of %d need %d\n",
+				channum, ch->txd_free,
+				ch->txd_num, txd_need_cnt);
+		ch->tx_full = 1;
+		ch->txd_required = txd_need_cnt;
+		sd_disable_xmit(ch->user);
+		return -EBUSY;               /* tell user to try again later */
+	}
+	/**************************************************/
+	/** Put the user data into MUSYCC data buffer(s) **/
+	/**************************************************/
+	m2 = mem_token;
+	md = ch->txd_usr_add;           /* get current available descriptor */
 
-    if (!(ch->status & TX_ENABLED))
-	return -EROFS;               /* how else to flag unwritable state ? */
+	for (len = OS_mem_token_tlen(m2); len > 0; m2 = OS_mem_token_next(m2)) {
+		int         u = OS_mem_token_len(m2);
 
-#ifdef RLD_TRANS_DEBUGx
-    if (1 || cxt1e1_log_level >= LOG_MONITOR2)
-#else
-    if (cxt1e1_log_level >= LOG_MONITOR2)
-#endif
-    {
-	pr_info("++ start_xmt[%d]: state %x start %x full %d free %d required %d stopped %x\n",
-		channum, ch->state, ch->ch_start_tx, ch->tx_full,
-		ch->txd_free, ch->txd_required, sd_queue_stopped(ch->user));
-    }
-    /***********************************************/
-    /** Determine total amount of data to be sent **/
-    /***********************************************/
-    m2 = mem_token;
-    txd_need_cnt = 0;
-    for (len = OS_mem_token_tlen(m2); len > 0;
-	 m2 = (void *) OS_mem_token_next(m2)) {
-	if (!OS_mem_token_len(m2))
-	    continue;
-	txd_need_cnt++;
-	len -= OS_mem_token_len(m2);
-    }
-
-    if (txd_need_cnt == 0) {
-	if (cxt1e1_log_level >= LOG_MONITOR2)
-	    pr_info("%s channel %d: no TX data in User buffer\n", ci->devname, channum);
-	OS_mem_token_free(mem_token);
-	return 0;                   /* no data to send */
-    }
-    /*************************************************/
-    /** Are there sufficient descriptors available? **/
-    /*************************************************/
-    if (txd_need_cnt > ch->txd_num) { /* never enough descriptors for this
-				       * large a buffer */
-	if (cxt1e1_log_level >= LOG_DEBUG)
-	    pr_info("start_xmit: discarding buffer, insufficient descriptor cnt %d, need %d.\n",
-		    ch->txd_num, txd_need_cnt + 1);
-	ch->s.tx_dropped++;
-	OS_mem_token_free(mem_token);
-	return 0;
-    }
-#if 0
-    spin_lock_irqsave(&ch->ch_txlock, flags);
-#endif
-    /************************************************************/
-    /** flow control the line if not enough descriptors remain **/
-    /************************************************************/
-    if (txd_need_cnt > ch->txd_free) {
-	if (cxt1e1_log_level >= LOG_MONITOR2)
-	    pr_info("start_xmit[%d]: EBUSY - need more descriptors, have %d of %d need %d\n",
-		    channum, ch->txd_free, ch->txd_num, txd_need_cnt);
-	ch->tx_full = 1;
-	ch->txd_required = txd_need_cnt;
-	sd_disable_xmit(ch->user);
-#if 0
-	spin_unlock_irqrestore(&ch->ch_txlock, flags);
-#endif
-	return -EBUSY;               /* tell user to try again later */
-    }
-    /**************************************************/
-    /** Put the user data into MUSYCC data buffer(s) **/
-    /**************************************************/
-    m2 = mem_token;
-    md = ch->txd_usr_add;           /* get current available descriptor */
+		if (!u)
+			continue;
+		len -= u;
+
+		/*
+		 * Enable following chunks, yet wait to enable the FIRST chunk until
+		 * after ALL subsequent chunks are setup.
+		 */
+		if (md != ch->txd_usr_add)  /* not first chunk */
+			/* transfer ownership from HOST to MUSYCC */
+			u |= MUSYCC_TX_OWNED;
+
+		if (len)                    /* not last chunk */
+			u |= EOBIRQ_ENABLE;
+		else if (ch->p.chan_mode == CFG_CH_PROTO_TRANS) {
+			/*
+			 * Per MUSYCC Ref 6.4.9 for Transparent Mode, the host must
+			 * always clear EOMIRQ_ENABLE in every Transmit Buffer Descriptor
+			 * (IE. don't set herein).
+			 */
+			u |= EOBIRQ_ENABLE;
+		} else
+			u |= EOMIRQ_ENABLE;     /* EOM, last HDLC chunk */
+
+
+		/* last chunk in hdlc mode */
+		u |= (ch->p.idlecode << IDLE_CODE);
+		if (ch->p.pad_fill_count) {
+			u |= (PADFILL_ENABLE | (ch->p.pad_fill_count << EXTRA_FLAGS));
+		}
+		/* Fill in mds on last segment, others set ZERO
+		 * so that entire token is removed ONLY when ALL
+		 * segments have been transmitted.
+		 */
+		md->mem_token = len ? NULL : mem_token;
 
-    for (len = OS_mem_token_tlen(m2); len > 0; m2 = OS_mem_token_next(m2)) {
-	int         u = OS_mem_token_len(m2);
+		md->data = cpu_to_le32(OS_vtophys(OS_mem_token_data(m2)));
+		FLUSH_MEM_WRITE();
+		md->status = cpu_to_le32(u);
+		--ch->txd_free;
+		md = md->snext;
+	}
+	FLUSH_MEM_WRITE();
 
-	if (!u)
-	    continue;
-	len -= u;
 
 	/*
-	 * Enable following chunks, yet wait to enable the FIRST chunk until
-	 * after ALL subsequent chunks are setup.
+	 * Now transfer ownership of first chunk from HOST to MUSYCC in order to
+	 * fire-off this XMIT.
 	 */
-	if (md != ch->txd_usr_add)  /* not first chunk */
-	    u |= MUSYCC_TX_OWNED;   /* transfer ownership from HOST to MUSYCC */
-
-	if (len)                    /* not last chunk */
-	    u |= EOBIRQ_ENABLE;
-	else if (ch->p.chan_mode == CFG_CH_PROTO_TRANS)	{
-	    /*
-	     * Per MUSYCC Ref 6.4.9 for Transparent Mode, the host must
-	     * always clear EOMIRQ_ENABLE in every Transmit Buffer Descriptor
-	     * (IE. don't set herein).
-	     */
-	    u |= EOBIRQ_ENABLE;
-	} else
-	    u |= EOMIRQ_ENABLE;     /* EOM, last HDLC chunk */
-
-
-	/* last chunk in hdlc mode */
-	u |= (ch->p.idlecode << IDLE_CODE);
-	if (ch->p.pad_fill_count) {
-#if 0
-	    /* NOOP NOTE: u_int8_t cannot be > 0xFF */
-	    /* sanitize pad_fill_count for maximums allowed by hardware */
-	    if (ch->p.pad_fill_count > EXTRA_FLAGS_MASK)
-		ch->p.pad_fill_count = EXTRA_FLAGS_MASK;
-#endif
-	    u |= (PADFILL_ENABLE | (ch->p.pad_fill_count << EXTRA_FLAGS));
-	}
-	md->mem_token = len ? NULL : mem_token;    /* Fill in mds on last
-						 * segment, others set ZERO
-						 * so that entire token is
-						 * removed ONLY when ALL
-						 * segments have been
-						 * transmitted. */
-
-	md->data = cpu_to_le32(OS_vtophys(OS_mem_token_data(m2)));
+	ch->txd_usr_add->status |= __constant_cpu_to_le32(MUSYCC_TX_OWNED);
 	FLUSH_MEM_WRITE();
-	md->status = cpu_to_le32(u);
-	--ch->txd_free;
-	md = md->snext;
-    }
-    FLUSH_MEM_WRITE();
-
-
-    /*
-     * Now transfer ownership of first chunk from HOST to MUSYCC in order to
-     * fire-off this XMIT.
-     */
-    ch->txd_usr_add->status |= __constant_cpu_to_le32(MUSYCC_TX_OWNED);
-    FLUSH_MEM_WRITE();
-    ch->txd_usr_add = md;
-
-    len = OS_mem_token_tlen(mem_token);
-    atomic_add(len, &ch->tx_pending);
-    atomic_add(len, &ci->tx_pending);
-    ch->s.tx_packets++;
-    ch->s.tx_bytes += len;
-    /*
-     * If an ONR was seen, then channel requires poking to restart
-     * transmission.
-     */
-    if (ch->ch_start_tx)
-	musycc_chan_restart(ch);
+	ch->txd_usr_add = md;
+
+	len = OS_mem_token_tlen(mem_token);
+	atomic_add(len, &ch->tx_pending);
+	atomic_add(len, &ci->tx_pending);
+	ch->s.tx_packets++;
+	ch->s.tx_bytes += len;
+	/*
+	 * If an ONR was seen, then channel requires poking to restart
+	 * transmission.
+	 */
+	if (ch->ch_start_tx)
+		musycc_chan_restart(ch);
 #ifdef SBE_WAN256T3_ENABLE
-    wan256t3_led(ci, LED_TX, LEDV_G);
+	wan256t3_led(ci, LED_TX, LEDV_G);
 #endif
-    return 0;
+	return 0;
 }
 
 
diff --git a/drivers/staging/cxt1e1/pmc93x6_eeprom.c b/drivers/staging/cxt1e1/pmc93x6_eeprom.c
index 78cc1709ebd..ba588f1b211 100644
--- a/drivers/staging/cxt1e1/pmc93x6_eeprom.c
+++ b/drivers/staging/cxt1e1/pmc93x6_eeprom.c
@@ -28,6 +28,7 @@
 #include "sbecom_inline_linux.h"
 #include "pmcc4.h"
 #include "sbe_promformat.h"
+#include "pmc93x6_eeprom.h"
 
 #ifndef TRUE
 #define TRUE   1
@@ -43,38 +44,35 @@
  *      using.
  */
 
-#define EE_MFG      (long)0     /* Index to manufacturing record */
-#define EE_FIRST    0x28        /* Index to start testing at */
-#define EE_LIMIT    128         /* Index to end testing at */
-
+#define EE_MFG      (long)0	/* Index to manufacturing record */
+#define EE_FIRST    0x28	/* Index to start testing at */
+#define EE_LIMIT    128		/* Index to end testing at */
 
 /*  Bit Ordering for Instructions
-**
-**  A0, A1, A2, A3, A4, A5, A6, OP0, OP1, SB   (lsb, or 1st bit out)
-**
-*/
-
-#define EPROM_EWEN      0x0019  /* Erase/Write enable (reversed) */
-#define EPROM_EWDS      0x0001  /* Erase/Write disable (reversed) */
-#define EPROM_READ      0x0003  /* Read (reversed) */
-#define EPROM_WRITE     0x0005  /* Write (reversed) */
-#define EPROM_ERASE     0x0007  /* Erase (reversed) */
-#define EPROM_ERAL      0x0009  /* Erase All (reversed) */
-#define EPROM_WRAL      0x0011  /* Write All (reversed) */
-
-#define EPROM_ADR_SZ    7       /* Number of bits in offset address */
-#define EPROM_OP_SZ     3       /* Number of bits in command */
-#define SIZE_ADDR_OP    (EPROM_ADR_SZ + EPROM_OP_SZ)
-#define LC46A_MAX_OPS   10      /* Number of bits in Instruction */
-#define NUM_OF_BITS     8       /* Number of bits in data */
+ *
+ *  A0, A1, A2, A3, A4, A5, A6, OP0, OP1, SB   (lsb, or 1st bit out)
+ *
+ */
 
+#define EPROM_EWEN      0x0019	/* Erase/Write enable (reversed) */
+#define EPROM_EWDS      0x0001	/* Erase/Write disable (reversed) */
+#define EPROM_READ      0x0003	/* Read (reversed) */
+#define EPROM_WRITE     0x0005	/* Write (reversed) */
+#define EPROM_ERASE     0x0007	/* Erase (reversed) */
+#define EPROM_ERAL      0x0009	/* Erase All (reversed) */
+#define EPROM_WRAL      0x0011	/* Write All (reversed) */
 
-/* EEPROM signal bits */
-#define EPROM_ACTIVE_OUT_BIT    0x0001  /* Out data bit */
-#define EPROM_ACTIVE_IN_BIT     0x0002  /* In data bit */
-#define ACTIVE_IN_BIT_SHIFT     0x0001  /* Shift In data bit to LSB */
-#define EPROM_ENCS              0x0004  /* Set EEPROM CS during operation */
+#define EPROM_ADR_SZ    7	/* Number of bits in offset address */
+#define EPROM_OP_SZ     3	/* Number of bits in command */
+#define SIZE_ADDR_OP    (EPROM_ADR_SZ + EPROM_OP_SZ)
+#define LC46A_MAX_OPS   10	/* Number of bits in Instruction */
+#define NUM_OF_BITS     8	/* Number of bits in data */
 
+/* EEPROM signal bits */
+#define EPROM_ACTIVE_OUT_BIT    0x0001	/* Out data bit */
+#define EPROM_ACTIVE_IN_BIT     0x0002	/* In data bit */
+#define ACTIVE_IN_BIT_SHIFT     0x0001	/* Shift In data bit to LSB */
+#define EPROM_ENCS              0x0004	/* Set EEPROM CS during operation */
 
 /*------------------------------------------------------------------------
  *      The ByteReverse table is used to reverses the 8 bits within a byte
@@ -82,29 +80,26 @@
  */
 
 static unsigned char ByteReverse[256];
-static int  ByteReverseBuilt = FALSE;
-
+static int ByteReverseBuilt = FALSE;
 
 /*------------------------------------------------------------------------
  *      mfg_template - initial serial EEPROM data structure
  *------------------------------------------------------------------------
  */
 
-static u8 mfg_template[sizeof(FLD_TYPE2)] =
-{
-    PROM_FORMAT_TYPE2,          /* type; */
-    0x00, 0x1A,                 /* length[2]; */
-    0x00, 0x00, 0x00, 0x00,     /* Crc32[4]; */
-    0x11, 0x76,                 /* Id[2]; */
-    0x07, 0x05,                 /* SubId[2] E1; */
-    0x00, 0xA0, 0xD6, 0x00, 0x00, 0x00, /* Serial[6]; */
-    0x00, 0x00, 0x00, 0x00,     /* CreateTime[4]; */
-    0x00, 0x00, 0x00, 0x00,     /* HeatRunTime[4]; */
-    0x00, 0x00, 0x00, 0x00,     /* HeatRunIterations[4]; */
-    0x00, 0x00, 0x00, 0x00,     /* HeatRunErrors[4]; */
+static u8 mfg_template[sizeof(FLD_TYPE2)] = {
+	PROM_FORMAT_TYPE2,	/* type; */
+	0x00, 0x1A,		/* length[2]; */
+	0x00, 0x00, 0x00, 0x00,	/* Crc32[4]; */
+	0x11, 0x76,		/* Id[2]; */
+	0x07, 0x05,		/* SubId[2] E1; */
+	0x00, 0xA0, 0xD6, 0x00, 0x00, 0x00,	/* Serial[6]; */
+	0x00, 0x00, 0x00, 0x00,	/* CreateTime[4]; */
+	0x00, 0x00, 0x00, 0x00,	/* HeatRunTime[4]; */
+	0x00, 0x00, 0x00, 0x00,	/* HeatRunIterations[4]; */
+	0x00, 0x00, 0x00, 0x00,	/* HeatRunErrors[4]; */
 };
 
-
 /*------------------------------------------------------------------------
  *      BuildByteReverse - build the 8-bit reverse table
  *------------------------------------------------------------------------
@@ -113,39 +108,35 @@ static u8 mfg_template[sizeof(FLD_TYPE2)] =
  *      (the MSB becomes the LSB etc.).
  */
 
-static void
-BuildByteReverse (void)
+static void BuildByteReverse(void)
 {
-    long        half;           /* Used to build by powers to 2 */
-    int         i;
+	/* Used to build by powers to 2 */
+	long half;
+	int i;
 
-    ByteReverse[0] = 0;
+	ByteReverse[0] = 0;
 
-    for (half = 1; half < sizeof (ByteReverse); half <<= 1)
-        for (i = 0; i < half; i++)
-            ByteReverse[half + i] = (char) (ByteReverse[i] | (0x80 / half));
+	for (half = 1; half < sizeof(ByteReverse); half <<= 1)
+		for (i = 0; i < half; i++)
+			ByteReverse[half + i] =
+			    (char)(ByteReverse[i] | (0x80 / half));
 
-    ByteReverseBuilt = TRUE;
+	ByteReverseBuilt = TRUE;
 }
 
-
 /*------------------------------------------------------------------------
  *      eeprom_delay - small delay for EEPROM timing
  *------------------------------------------------------------------------
  */
 
-static void
-eeprom_delay (void)
+static void eeprom_delay(void)
 {
-    int         timeout;
+	int timeout;
 
-    for (timeout = 20; timeout; --timeout)
-    {
-        OS_uwait_dummy ();
-    }
+	for (timeout = 20; timeout; --timeout)
+		OS_uwait_dummy();
 }
 
-
 /*------------------------------------------------------------------------
  *      eeprom_put_byte - Send a byte to the EEPROM serially
  *------------------------------------------------------------------------
@@ -154,23 +145,23 @@ eeprom_delay (void)
  *      the data to the EEPROM.
  */
 
-void
-eeprom_put_byte (long addr, long data, int count)
+static void eeprom_put_byte(long addr, long data, int count)
 {
-    u_int32_t output;
-
-    while (--count >= 0)
-    {
-        output = (data & EPROM_ACTIVE_OUT_BIT) ? 1 : 0; /* Get next data bit */
-        output |= EPROM_ENCS;       /* Add Chip Select */
-        data >>= 1;
-
-        eeprom_delay ();
-        pci_write_32 ((u_int32_t *) addr, output);      /* Output it */
-    }
+	u_int32_t output;
+
+	while (--count >= 0) {
+		/* Get next data bit */
+		output = (data & EPROM_ACTIVE_OUT_BIT) ? 1 : 0;
+		/* Add Chip Select */
+		output |= EPROM_ENCS;
+		data >>= 1;
+
+		eeprom_delay();
+		/* Output it */
+		pci_write_32((u_int32_t *) addr, output);
+	}
 }
 
-
 /*------------------------------------------------------------------------
  *      eeprom_get_byte - Receive a byte from the EEPROM serially
  *------------------------------------------------------------------------
@@ -179,37 +170,35 @@ eeprom_put_byte (long addr, long data, int count)
  *      from the  EEPROM.
  */
 
-u_int32_t
-eeprom_get_byte (long addr)
+static u_int32_t eeprom_get_byte(long addr)
 {
-    u_int32_t   input;
-    u_int32_t   data;
-    int         count;
+	u_int32_t input;
+	u_int32_t data;
+	int count;
 
 /*  Start the Reading of DATA
-**
-**  The first read is a dummy as the data is latched in the
-**  EPLD and read on the next read access to the EEPROM.
-*/
+ *
+ *  The first read is a dummy as the data is latched in the
+ *  EPLD and read on the next read access to the EEPROM.
+ */
 
-    input = pci_read_32 ((u_int32_t *) addr);
+	input = pci_read_32((u_int32_t *) addr);
 
-    data = 0;
-    count = NUM_OF_BITS;
-    while (--count >= 0)
-    {
-        eeprom_delay ();
-        input = pci_read_32 ((u_int32_t *) addr);
+	data = 0;
+	count = NUM_OF_BITS;
+	while (--count >= 0) {
+		eeprom_delay();
+		input = pci_read_32((u_int32_t *) addr);
 
-        data <<= 1;                 /* Shift data over */
-        data |= (input & EPROM_ACTIVE_IN_BIT) ? 1 : 0;
+		/* Shift data over */
+		data <<= 1;
+		data |= (input & EPROM_ACTIVE_IN_BIT) ? 1 : 0;
 
-    }
+	}
 
-    return data;
+	return data;
 }
 
-
 /*------------------------------------------------------------------------
  *      disable_pmc_eeprom - Disable writes to the EEPROM
  *------------------------------------------------------------------------
@@ -217,16 +206,14 @@ eeprom_get_byte (long addr)
  *      Issue the EEPROM command to disable writes.
  */
 
-static void
-disable_pmc_eeprom (long addr)
+static void disable_pmc_eeprom(long addr)
 {
-    eeprom_put_byte (addr, EPROM_EWDS, SIZE_ADDR_OP);
+	eeprom_put_byte(addr, EPROM_EWDS, SIZE_ADDR_OP);
 
-    pci_write_32 ((u_int32_t *) addr, 0);       /* this removes Chip Select
-                                                 * from EEPROM */
+	/* this removes Chip Select from EEPROM */
+	pci_write_32((u_int32_t *) addr, 0);
 }
 
-
 /*------------------------------------------------------------------------
  *      enable_pmc_eeprom - Enable writes to the EEPROM
  *------------------------------------------------------------------------
@@ -234,16 +221,14 @@ disable_pmc_eeprom (long addr)
  *      Issue the EEPROM command to enable writes.
  */
 
-static void
-enable_pmc_eeprom (long addr)
+static void enable_pmc_eeprom(long addr)
 {
-    eeprom_put_byte (addr, EPROM_EWEN, SIZE_ADDR_OP);
+	eeprom_put_byte(addr, EPROM_EWEN, SIZE_ADDR_OP);
 
-    pci_write_32 ((u_int32_t *) addr, 0);       /* this removes Chip Select
-                                                 * from EEPROM */
+	/* this removes Chip Select from EEPROM */
+	pci_write_32((u_int32_t *) addr, 0);
 }
 
-
 /*------------------------------------------------------------------------
  *      pmc_eeprom_read - EEPROM location read
  *------------------------------------------------------------------------
@@ -252,34 +237,40 @@ enable_pmc_eeprom (long addr)
  *      the contents of the specified location to the calling routine.
  */
 
-u_int32_t
-pmc_eeprom_read (long addr, long mem_offset)
+static u_int32_t pmc_eeprom_read(long addr, long mem_offset)
 {
-    u_int32_t   data;           /* Data from chip */
+	/* Data from chip */
+	u_int32_t data;
 
-    if (!ByteReverseBuilt)
-        BuildByteReverse ();
+	if (!ByteReverseBuilt)
+		BuildByteReverse();
 
-    mem_offset = ByteReverse[0x7F & mem_offset];        /* Reverse address */
-    /*
-     * NOTE: The max offset address is 128 or half the reversal table. So the
-     * LSB is always zero and counts as a built in shift of one bit.  So even
-     * though we need to shift 3 bits to make room for the command, we only
-     * need to shift twice more because of the built in shift.
-     */
-    mem_offset <<= 2;               /* Shift for command */
-    mem_offset |= EPROM_READ;       /* Add command */
+	/* Reverse address */
+	mem_offset = ByteReverse[0x7F & mem_offset];
 
-    eeprom_put_byte (addr, mem_offset, SIZE_ADDR_OP);   /* Output chip address */
+	/*
+	 * NOTE: The max offset address is 128 or half the reversal table. So
+	 * the LSB is always zero and counts as a built in shift of one bit.
+	 * So even though we need to shift 3 bits to make room for the command,
+	 * we only need to shift twice more because of the built in shift.
+	 */
 
-    data = eeprom_get_byte (addr);  /* Read chip data */
+	/* Shift for command */
+	mem_offset <<= 2;
+	/* Add command */
+	mem_offset |= EPROM_READ;
 
-    pci_write_32 ((u_int32_t *) addr, 0);       /* Remove Chip Select from
-                                                 * EEPROM */
+	/* Output chip address */
+	eeprom_put_byte(addr, mem_offset, SIZE_ADDR_OP);
 
-    return (data & 0x000000FF);
-}
+	/* Read chip data */
+	data = eeprom_get_byte(addr);
+
+	/* Remove Chip Select from EEPROM */
+	pci_write_32((u_int32_t *) addr, 0);
 
+	return (data & 0x000000FF);
+}
 
 /*------------------------------------------------------------------------
  *      pmc_eeprom_write - EEPROM location write
@@ -292,189 +283,181 @@ pmc_eeprom_read (long addr, long mem_offset)
  *      operation succeeded.
  */
 
-int
-pmc_eeprom_write (long addr, long mem_offset, u_int32_t data)
+static int pmc_eeprom_write(long addr, long mem_offset, u_int32_t data)
 {
-    volatile u_int32_t temp;
-    int         count;
+	u_int32_t temp;
+	int count;
+
+	if (!ByteReverseBuilt)
+		BuildByteReverse();
+
+	/* Reverse address */
+	mem_offset = ByteReverse[0x7F & mem_offset];
 
-    if (!ByteReverseBuilt)
-        BuildByteReverse ();
+	/*
+	 * NOTE: The max offset address is 128 or half the reversal table. So
+	 * the LSB is always zero and counts as a built in shift of one bit.
+	 * So even though we need to shift 3 bits to make room for the command,
+	 * we only need to shift twice more because of the built in shift.
+	 */
 
-    mem_offset = ByteReverse[0x7F & mem_offset];        /* Reverse address */
-    /*
-     * NOTE: The max offset address is 128 or half the reversal table. So the
-     * LSB is always zero and counts as a built in shift of one bit.  So even
-     * though we need to shift 3 bits to make room for the command, we only
-     * need to shift twice more because of the built in shift.
-     */
-    mem_offset <<= 2;               /* Shift for command */
-    mem_offset |= EPROM_WRITE;      /* Add command */
+	/* Shift for command */
+	mem_offset <<= 2;
+	/* Add command */
+	mem_offset |= EPROM_WRITE;
 
-    eeprom_put_byte (addr, mem_offset, SIZE_ADDR_OP);   /* Output chip address */
+	/* Output chip address */
+	eeprom_put_byte(addr, mem_offset, SIZE_ADDR_OP);
 
-    data = ByteReverse[0xFF & data];/* Reverse data */
-    eeprom_put_byte (addr, data, NUM_OF_BITS);  /* Output chip data */
+	/* Reverse data */
+	data = ByteReverse[0xFF & data];
+	/* Output chip data */
+	eeprom_put_byte(addr, data, NUM_OF_BITS);
 
-    pci_write_32 ((u_int32_t *) addr, 0);       /* Remove Chip Select from
-                                                 * EEPROM */
+	/* Remove Chip Select from EEPROM */
+	pci_write_32((u_int32_t *) addr, 0);
 
 /*
-**  Must see Data In at a low state before completing this transaction.
-**
-**  Afterwards, the data bit will return to a high state, ~6 ms, terminating
-**  the operation.
-*/
-    pci_write_32 ((u_int32_t *) addr, EPROM_ENCS);      /* Re-enable Chip Select */
-    temp = pci_read_32 ((u_int32_t *) addr);    /* discard first read */
-    temp = pci_read_32 ((u_int32_t *) addr);
-    if (temp & EPROM_ACTIVE_IN_BIT)
-    {
-        temp = pci_read_32 ((u_int32_t *) addr);
-        if (temp & EPROM_ACTIVE_IN_BIT)
-        {
-            pci_write_32 ((u_int32_t *) addr, 0);       /* Remove Chip Select
-                                                         * from EEPROM */
-            return (1);
-        }
-    }
-    count = 1000;
-    while (count--)
-    {
-        for (temp = 0; temp < 0x10; temp++)
-            OS_uwait_dummy ();
-
-        if (pci_read_32 ((u_int32_t *) addr) & EPROM_ACTIVE_IN_BIT)
-            break;
-    }
-
-    if (count == -1)
-        return (2);
-
-    return (0);
+ *  Must see Data In at a low state before completing this transaction.
+ *
+ *  Afterwards, the data bit will return to a high state, ~6 ms, terminating
+ *  the operation.
+ */
+	/* Re-enable Chip Select */
+	pci_write_32((u_int32_t *) addr, EPROM_ENCS);
+	/* discard first read */
+	temp = pci_read_32((u_int32_t *) addr);
+	temp = pci_read_32((u_int32_t *) addr);
+	if (temp & EPROM_ACTIVE_IN_BIT) {
+		temp = pci_read_32((u_int32_t *) addr);
+		if (temp & EPROM_ACTIVE_IN_BIT) {
+			/* Remove Chip Select from EEPROM */
+			pci_write_32((u_int32_t *) addr, 0);
+			return 1;
+		}
+	}
+	count = 1000;
+	while (count--) {
+		for (temp = 0; temp < 0x10; temp++)
+			OS_uwait_dummy();
+
+		if (pci_read_32((u_int32_t *) addr) & EPROM_ACTIVE_IN_BIT)
+			break;
+	}
+
+	if (count == -1)
+		return 2;
+
+	return 0;
 }
 
-
 /*------------------------------------------------------------------------
  *      pmcGetBuffValue - read the specified value from buffer
  *------------------------------------------------------------------------
  */
 
-long
-pmcGetBuffValue (char *ptr, int size)
+static long pmcGetBuffValue(char *ptr, int size)
 {
-    long        value = 0;
-    int         index;
+	long value = 0;
+	int index;
 
-    for (index = 0; index < size; ++index)
-    {
-        value <<= 8;
-        value |= ptr[index] & 0xFF;
-    }
+	for (index = 0; index < size; ++index) {
+		value <<= 8;
+		value |= ptr[index] & 0xFF;
+	}
 
-    return value;
+	return value;
 }
 
-
 /*------------------------------------------------------------------------
  *      pmcSetBuffValue - save the specified value to buffer
  *------------------------------------------------------------------------
  */
 
-void
-pmcSetBuffValue (char *ptr, long value, int size)
+static void pmcSetBuffValue(char *ptr, long value, int size)
 {
-    int         index = size;
+	int index = size;
 
-    while (--index >= 0)
-    {
-        ptr[index] = (char) (value & 0xFF);
-        value >>= 8;
-    }
+	while (--index >= 0) {
+		ptr[index] = (char)(value & 0xFF);
+		value >>= 8;
+	}
 }
 
-
 /*------------------------------------------------------------------------
  *      pmc_eeprom_read_buffer - read EEPROM data into specified buffer
  *------------------------------------------------------------------------
  */
 
 void
-pmc_eeprom_read_buffer (long addr, long mem_offset, char *dest_ptr, int size)
+pmc_eeprom_read_buffer(long addr, long mem_offset, char *dest_ptr, int size)
 {
-    while (--size >= 0)
-        *dest_ptr++ = (char) pmc_eeprom_read (addr, mem_offset++);
+	while (--size >= 0)
+		*dest_ptr++ = (char)pmc_eeprom_read(addr, mem_offset++);
 }
 
-
 /*------------------------------------------------------------------------
  *      pmc_eeprom_write_buffer - write EEPROM data from specified buffer
  *------------------------------------------------------------------------
  */
 
 void
-pmc_eeprom_write_buffer (long addr, long mem_offset, char *dest_ptr, int size)
+pmc_eeprom_write_buffer(long addr, long mem_offset, char *dest_ptr, int size)
 {
-    enable_pmc_eeprom (addr);
+	enable_pmc_eeprom(addr);
 
-    while (--size >= 0)
-        pmc_eeprom_write (addr, mem_offset++, *dest_ptr++);
+	while (--size >= 0)
+		pmc_eeprom_write(addr, mem_offset++, *dest_ptr++);
 
-    disable_pmc_eeprom (addr);
+	disable_pmc_eeprom(addr);
 }
 
-
 /*------------------------------------------------------------------------
  *      pmcCalcCrc - calculate the CRC for the serial EEPROM structure
  *------------------------------------------------------------------------
  */
 
-u_int32_t
-pmcCalcCrc_T01 (void *bufp)
+static u_int32_t pmcCalcCrc_T01(void *bufp)
 {
-    FLD_TYPE2  *buf = bufp;
-    u_int32_t   crc;            /* CRC of the structure */
+	FLD_TYPE2 *buf = bufp;
+	/* CRC of the structure */
+	u_int32_t crc;
 
-    /* Calc CRC for type and length fields */
-    sbeCrc (
-            (u_int8_t *) &buf->type,
-            (u_int32_t) STRUCT_OFFSET (FLD_TYPE1, Crc32),
-            (u_int32_t) 0,
-            (u_int32_t *) &crc);
+	/* Calc CRC for type and length fields */
+	sbeCrc((u_int8_t *) &buf->type,
+	       (u_int32_t) STRUCT_OFFSET(FLD_TYPE1, Crc32),
+	       (u_int32_t) 0, (u_int32_t *) &crc);
 
 #ifdef EEPROM_TYPE_DEBUG
-    pr_info("sbeCrc: crc 1 calculated as %08x\n", crc); /* RLD DEBUG */
+	/* RLD DEBUG */
+	pr_info("sbeCrc: crc 1 calculated as %08x\n", crc);
 #endif
-    return ~crc;
+	return ~crc;
 }
 
-u_int32_t
-pmcCalcCrc_T02 (void *bufp)
+static u_int32_t pmcCalcCrc_T02(void *bufp)
 {
-    FLD_TYPE2  *buf = bufp;
-    u_int32_t   crc;            /* CRC of the structure */
-
-    /* Calc CRC for type and length fields */
-    sbeCrc (
-            (u_int8_t *) &buf->type,
-            (u_int32_t) STRUCT_OFFSET (FLD_TYPE2, Crc32),
-            (u_int32_t) 0,
-            (u_int32_t *) &crc);
-
-    /* Calc CRC for remaining fields */
-    sbeCrc (
-            (u_int8_t *) &buf->Id[0],
-            (u_int32_t) (sizeof (FLD_TYPE2) - STRUCT_OFFSET (FLD_TYPE2, Id)),
-            (u_int32_t) crc,
-            (u_int32_t *) &crc);
+	FLD_TYPE2 *buf = bufp;
+	/* CRC of the structure */
+	u_int32_t crc;
+
+	/* Calc CRC for type and length fields */
+	sbeCrc((u_int8_t *) &buf->type,
+	       (u_int32_t) STRUCT_OFFSET(FLD_TYPE2, Crc32),
+	       (u_int32_t) 0, (u_int32_t *) &crc);
+
+	/* Calc CRC for remaining fields */
+	sbeCrc((u_int8_t *) &buf->Id[0],
+	       (u_int32_t) (sizeof(FLD_TYPE2) - STRUCT_OFFSET(FLD_TYPE2, Id)),
+	       (u_int32_t) crc, (u_int32_t *) &crc);
 
 #ifdef EEPROM_TYPE_DEBUG
-    pr_info("sbeCrc: crc 2 calculated as %08x\n", crc); /* RLD DEBUG */
+	/* RLD DEBUG */
+	pr_info("sbeCrc: crc 2 calculated as %08x\n", crc);
 #endif
-    return crc;
+	return crc;
 }
 
-
 /*------------------------------------------------------------------------
  *      pmc_init_seeprom - initialize the serial EEPROM structure
  *------------------------------------------------------------------------
@@ -485,64 +468,65 @@ pmcCalcCrc_T02 (void *bufp)
  *      serial number field.
  */
 
-void
-pmc_init_seeprom (u_int32_t addr, u_int32_t serialNum)
+void pmc_init_seeprom(u_int32_t addr, u_int32_t serialNum)
 {
-    PROMFORMAT  buffer;         /* Memory image of structure */
-    u_int32_t   crc;            /* CRC of structure */
-    time_t      createTime;
+	/* Memory image of structure */
+	PROMFORMAT buffer;
+	/* CRC of structure */
+	u_int32_t crc;
+	time_t createTime;
 
-    createTime = get_seconds ();
+	createTime = get_seconds();
 
-    /* use template data */
-    memcpy(&buffer.fldType2, mfg_template, sizeof(buffer.fldType2));
+	/* use template data */
+	memcpy(&buffer.fldType2, mfg_template, sizeof(buffer.fldType2));
 
-    /* Update serial number field in buffer */
-    pmcSetBuffValue (&buffer.fldType2.Serial[3], serialNum, 3);
+	/* Update serial number field in buffer */
+	pmcSetBuffValue(&buffer.fldType2.Serial[3], serialNum, 3);
 
-    /* Update create time field in buffer */
-    pmcSetBuffValue (&buffer.fldType2.CreateTime[0], createTime, 4);
+	/* Update create time field in buffer */
+	pmcSetBuffValue(&buffer.fldType2.CreateTime[0], createTime, 4);
 
-    /* Update CRC field in buffer */
-    crc = pmcCalcCrc_T02 (&buffer);
-    pmcSetBuffValue (&buffer.fldType2.Crc32[0], crc, 4);
+	/* Update CRC field in buffer */
+	crc = pmcCalcCrc_T02(&buffer);
+	pmcSetBuffValue(&buffer.fldType2.Crc32[0], crc, 4);
 
 #ifdef DEBUG
-    for (i = 0; i < sizeof (FLD_TYPE2); ++i)
-        pr_info("[%02X] = %02X\n", i, buffer.bytes[i] & 0xFF);
+	for (i = 0; i < sizeof(FLD_TYPE2); ++i)
+		pr_info("[%02X] = %02X\n", i, buffer.bytes[i] & 0xFF);
 #endif
 
-    /* Write structure to serial EEPROM */
-    pmc_eeprom_write_buffer (addr, EE_MFG, (char *) &buffer, sizeof (FLD_TYPE2));
+	/* Write structure to serial EEPROM */
+	pmc_eeprom_write_buffer(addr, EE_MFG, (char *)&buffer,
+				sizeof(FLD_TYPE2));
 }
 
-
-char
-pmc_verify_cksum (void *bufp)
+char pmc_verify_cksum(void *bufp)
 {
-    FLD_TYPE1  *buf1 = bufp;
-    FLD_TYPE2  *buf2 = bufp;
-    u_int32_t   crc1, crc2;     /* CRC read from EEPROM */
+	FLD_TYPE1 *buf1 = bufp;
+	FLD_TYPE2 *buf2 = bufp;
+	/* CRC read from EEPROM */
+	u_int32_t crc1, crc2;
 
-    /* Retrieve contents of CRC field */
-    crc1 = pmcGetBuffValue (&buf1->Crc32[0], sizeof (buf1->Crc32));
+	/* Retrieve contents of CRC field */
+	crc1 = pmcGetBuffValue(&buf1->Crc32[0], sizeof(buf1->Crc32));
 #ifdef EEPROM_TYPE_DEBUG
-    pr_info("EEPROM: chksum 1 reads   as %08x\n", crc1);        /* RLD DEBUG */
+	/* RLD DEBUG */
+	pr_info("EEPROM: chksum 1 reads   as %08x\n", crc1);
 #endif
-    if ((buf1->type == PROM_FORMAT_TYPE1) &&
-        (pmcCalcCrc_T01 ((void *) buf1) == crc1))
-        return PROM_FORMAT_TYPE1;   /* checksum type 1 verified */
+	if ((buf1->type == PROM_FORMAT_TYPE1) &&
+	    (pmcCalcCrc_T01((void *)buf1) == crc1))
+		return PROM_FORMAT_TYPE1;	/* checksum type 1 verified */
 
-    crc2 = pmcGetBuffValue (&buf2->Crc32[0], sizeof (buf2->Crc32));
+	crc2 = pmcGetBuffValue(&buf2->Crc32[0], sizeof(buf2->Crc32));
 #ifdef EEPROM_TYPE_DEBUG
-    pr_info("EEPROM: chksum 2 reads   as %08x\n", crc2);        /* RLD DEBUG */
+	/* RLD DEBUG */
+	pr_info("EEPROM: chksum 2 reads   as %08x\n", crc2);
 #endif
-    if ((buf2->type == PROM_FORMAT_TYPE2) &&
-        (pmcCalcCrc_T02 ((void *) buf2) == crc2))
-        return PROM_FORMAT_TYPE2;   /* checksum type 2 verified */
+	if ((buf2->type == PROM_FORMAT_TYPE2) &&
+	    (pmcCalcCrc_T02((void *)buf2) == crc2))
+		return PROM_FORMAT_TYPE2;	/* checksum type 2 verified */
 
-    return PROM_FORMAT_Unk;         /* failed to validate */
+	/* failed to validate */
+	return PROM_FORMAT_Unk;
 }
-
-
-/*** End-of-File ***/
diff --git a/drivers/staging/cxt1e1/pmcc4_drv.c b/drivers/staging/cxt1e1/pmcc4_drv.c
index 621a7292647..76bebdd18b3 100644
--- a/drivers/staging/cxt1e1/pmcc4_drv.c
+++ b/drivers/staging/cxt1e1/pmcc4_drv.c
@@ -122,35 +122,6 @@ c4_find_chan (int channum)
     return NULL;
 }
 
-
-ci_t       *__init
-c4_new (void *hi)
-{
-    ci_t       *ci;
-
-#ifdef SBE_MAP_DEBUG
-    pr_warning("c4_new() entered, ci needs %u.\n",
-               (unsigned int) sizeof (ci_t));
-#endif
-
-    ci = (ci_t *) OS_kmalloc (sizeof (ci_t));
-    if (ci)
-    {
-        ci->hdw_info = hi;
-        ci->state = C_INIT;         /* mark as hardware not available */
-        ci->next = c4_list;
-        c4_list = ci;
-        ci->brdno = ci->next ? ci->next->brdno + 1 : 0;
-    } else
-        pr_warning("failed CI malloc, size %u.\n",
-                   (unsigned int) sizeof (ci_t));
-
-    if (!CI)
-        CI = ci;                    /* DEBUG, only board 0 usage */
-    return ci;
-}
-
-
 /***
  * Check port state and set LED states using watchdog or ioctl...
  * also check for in-band SF loopback commands (& cause results if they are there)
@@ -485,12 +456,12 @@ c4_cleanup (void)
             for (j = 0; j < MUSYCC_NCHANS; j++)
             {
                 if (pi->chan[j])
-                    OS_kfree (pi->chan[j]);     /* free mch_t struct */
+                    kfree(pi->chan[j]);     /* free mch_t struct */
             }
-            OS_kfree (pi->regram_saved);
+            kfree(pi->regram_saved);
         }
-        OS_kfree (ci->iqd_p_saved);
-        OS_kfree (ci);
+        kfree(ci->iqd_p_saved);
+        kfree(ci);
         ci = next;                  /* cleanup next board, if any */
     }
 }
@@ -619,7 +590,7 @@ c4_init (ci_t *ci, u_char *func0, u_char *func1)
         /* allocate channel structures for this port */
         for (j = 0; j < MUSYCC_NCHANS; j++)
         {
-            ch = OS_kmalloc (sizeof (mch_t));
+		ch = kzalloc(sizeof(mch_t), GFP_KERNEL | GFP_DMA);
             if (ch)
             {
                 pi->chan[j] = ch;
@@ -1368,8 +1339,8 @@ c4_chan_up (ci_t *ci, int channum)
     ch->txd_num = txnum;
     ch->rxix_irq_srv = 0;
 
-    ch->mdr = OS_kmalloc (sizeof (struct mdesc) * rxnum);
-    ch->mdt = OS_kmalloc (sizeof (struct mdesc) * txnum);
+	ch->mdr = kzalloc(sizeof(struct mdesc) * rxnum, GFP_KERNEL | GFP_DMA);
+	ch->mdt = kzalloc(sizeof(struct mdesc) * txnum, GFP_KERNEL | GFP_DMA);
     if (ch->p.chan_mode == CFG_CH_PROTO_TRANS)
                tmp = __constant_cpu_to_le32 (cxt1e1_max_mru | EOBIRQ_ENABLE);
     else
@@ -1462,10 +1433,10 @@ errfree:
         i--;
         OS_mem_token_free (ch->mdr[i].mem_token);
     }
-    OS_kfree (ch->mdt);
+    kfree(ch->mdt);
     ch->mdt = NULL;
     ch->txd_num = 0;
-    OS_kfree (ch->mdr);
+    kfree(ch->mdr);
     ch->mdr = NULL;
     ch->rxd_num = 0;
     ch->state = DOWN;
diff --git a/drivers/staging/cxt1e1/pmcc4_private.h b/drivers/staging/cxt1e1/pmcc4_private.h
index eb28f095ff8..451f12f5b04 100644
--- a/drivers/staging/cxt1e1/pmcc4_private.h
+++ b/drivers/staging/cxt1e1/pmcc4_private.h
@@ -262,7 +262,7 @@ struct s_hdw_info
     struct pci_dev *pdev[2];
 
     unsigned long addr[2];
-    unsigned long addr_mapped[2];
+    void __iomem *addr_mapped[2];
     unsigned long len[2];
 
     union
diff --git a/drivers/staging/cxt1e1/sbecom_inline_linux.h b/drivers/staging/cxt1e1/sbecom_inline_linux.h
index ba3ff3efe06..f5835c29ef3 100644
--- a/drivers/staging/cxt1e1/sbecom_inline_linux.h
+++ b/drivers/staging/cxt1e1/sbecom_inline_linux.h
@@ -39,27 +39,6 @@ void        pci_write_32 (u_int32_t *p, u_int32_t v);
  * system dependent callbacks
  */
 
-/**********/
-/* malloc */
-/**********/
-
-static inline void *
-OS_kmalloc (size_t size)
-{
-    char       *ptr = kmalloc (size, GFP_KERNEL | GFP_DMA);
-
-    if (ptr)
-        memset (ptr, 0, size);
-    return ptr;
-}
-
-static inline void
-OS_kfree (void *x)
-{
-    kfree (x);
-}
-
-
 /****************/
 /* memory token */
 /****************/
@@ -197,7 +176,7 @@ static inline int
 OS_free_watchdog (struct watchdog *wd)
 {
     OS_stop_watchdog (wd);
-    OS_kfree (wd);
+    kfree(wd);
     return 0;
 }
 
diff --git a/drivers/staging/cxt1e1/sbecrc.c b/drivers/staging/cxt1e1/sbecrc.c
index 81fa8a3a462..a51780f6048 100644
--- a/drivers/staging/cxt1e1/sbecrc.c
+++ b/drivers/staging/cxt1e1/sbecrc.c
@@ -101,7 +101,8 @@ sbeCrc(u_int8_t *buffer,          /* data buffer to crc */
 		tbl = &CRCTable;
 		genCrcTable(tbl);
 #else
-		tbl = (u_int32_t *) OS_kmalloc(CRC_TABLE_ENTRIES * sizeof(u_int32_t));
+		tbl = kzalloc(CRC_TABLE_ENTRIES * sizeof(u_int32_t),
+			      GFP_KERNEL);
 		if (!tbl) {
 			*result = 0;   /* dummy up return value due to malloc
 					* failure */
@@ -125,7 +126,7 @@ sbeCrc(u_int8_t *buffer,          /* data buffer to crc */
 
 #ifndef STATIC_CRC_TABLE
 	crcTableInit = 0;
-	OS_kfree(tbl);
+	kfree(tbl);
 #endif
 }
 
diff --git a/drivers/staging/cxt1e1/sbeproc.c b/drivers/staging/cxt1e1/sbeproc.c
index 840c647fbf4..1c2e52e8b5f 100644
--- a/drivers/staging/cxt1e1/sbeproc.c
+++ b/drivers/staging/cxt1e1/sbeproc.c
@@ -72,7 +72,7 @@ static int sbecom_proc_get_sbe_info(struct seq_file *m, void *v)
 	char       *spd;
 	struct sbe_brd_info *bip;
 
-	bip = OS_kmalloc(sizeof(struct sbe_brd_info));
+	bip = kzalloc(sizeof(struct sbe_brd_info), GFP_KERNEL | GFP_DMA);
 	if (!bip)
 		return -ENOMEM;
 
diff --git a/drivers/staging/dgap/dgap.c b/drivers/staging/dgap/dgap.c
index a5fc3c75ed4..170d6f3e422 100644
--- a/drivers/staging/dgap/dgap.c
+++ b/drivers/staging/dgap/dgap.c
@@ -12,21 +12,6 @@
  * 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.
- *
- *
- *	NOTE TO LINUX KERNEL HACKERS:  DO NOT REFORMAT THIS CODE!
- *
- *	This is shared code between Digi's CVS archive and the
- *	Linux Kernel sources.
- *	Changing the source just for reformatting needlessly breaks
- *	our CVS diff history.
- *
- *	Send any bug fixes/changes to:  Eng.Linux at digi dot com.
- *	Thank you.
- *
  */
 
 /*
@@ -78,21 +63,11 @@
 
 #include "dgap.h"
 
-#define init_MUTEX(sem)         sema_init(sem, 1)
-#define DECLARE_MUTEX(name)     \
-	struct semaphore name = __SEMAPHORE_INITIALIZER(name, 1)
-
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Digi International, http://www.digi.com");
 MODULE_DESCRIPTION("Driver for the Digi International EPCA PCI based product line");
 MODULE_SUPPORTED_DEVICE("dgap");
 
-/**************************************************************************
- *
- * protos for this file
- *
- */
-
 static int dgap_start(void);
 static void dgap_init_globals(void);
 static int dgap_found_board(struct pci_dev *pdev, int id);
@@ -105,7 +80,6 @@ static int dgap_probe1(struct pci_dev *pdev, int card_type);
 static int dgap_do_remap(struct board_t *brd);
 static irqreturn_t dgap_intr(int irq, void *voidbrd);
 
-/* Our function prototypes */
 static int dgap_tty_open(struct tty_struct *tty, struct file *file);
 static void dgap_tty_close(struct tty_struct *tty, struct file *file);
 static int dgap_block_til_ready(struct tty_struct *tty, struct file *file,
@@ -161,9 +135,9 @@ static void dgap_cmdw_ext(struct channel_t *ch, u16 cmd, u16 word, uint ncmds);
 static int dgap_event(struct board_t *bd);
 
 static void dgap_poll_tasklet(unsigned long data);
-static void dgap_cmdb(struct channel_t *ch, uchar cmd, uchar byte1,
-			uchar byte2, uint ncmds);
-static void dgap_cmdw(struct channel_t *ch, uchar cmd, u16 word, uint ncmds);
+static void dgap_cmdb(struct channel_t *ch, u8 cmd, u8 byte1,
+			u8 byte2, uint ncmds);
+static void dgap_cmdw(struct channel_t *ch, u8 cmd, u16 word, uint ncmds);
 static void dgap_wmove(struct channel_t *ch, char *buf, uint cnt);
 static int dgap_param(struct tty_struct *tty);
 static void dgap_parity_scan(struct channel_t *ch, unsigned char *cbuf,
@@ -176,7 +150,6 @@ static void dgap_firmware_reset_port(struct channel_t *ch);
  */
 static int dgap_gettok(char **in, struct cnode *p);
 static char *dgap_getword(char **in);
-static char *dgap_savestring(char *s);
 static struct cnode *dgap_newnode(int t);
 static int dgap_checknode(struct cnode *p);
 static void dgap_err(char *s);
@@ -202,7 +175,7 @@ static void dgap_remove_tty_sysfs(struct device *c);
 /*
  * Function prototypes from dgap_parse.h
  */
-static int dgap_parsefile(char **in, int Remove);
+static int dgap_parsefile(char **in, int remove);
 static struct cnode *dgap_find_config(int type, int bus, int slot);
 static uint dgap_config_get_num_prts(struct board_t *bd);
 static char *dgap_create_config_string(struct board_t *bd, char *string);
@@ -210,22 +183,21 @@ static uint dgap_config_get_useintr(struct board_t *bd);
 static uint dgap_config_get_altpin(struct board_t *bd);
 
 static int dgap_ms_sleep(ulong ms);
-static void dgap_do_bios_load(struct board_t *brd, const uchar *ubios, int len);
-static void dgap_do_fep_load(struct board_t *brd, const uchar *ufep, int len);
+static void dgap_do_bios_load(struct board_t *brd, const u8 *ubios, int len);
+static void dgap_do_fep_load(struct board_t *brd, const u8 *ufep, int len);
 #ifdef DIGI_CONCENTRATORS_SUPPORTED
-static void dgap_do_conc_load(struct board_t *brd, uchar *uaddr, int len);
+static void dgap_do_conc_load(struct board_t *brd, u8 *uaddr, int len);
 #endif
 static int dgap_after_config_loaded(int board);
 static int dgap_finalize_board_init(struct board_t *brd);
 
 static void dgap_get_vpd(struct board_t *brd);
 static void dgap_do_reset_board(struct board_t *brd);
-static int dgap_do_wait_for_bios(struct board_t *brd);
-static int dgap_do_wait_for_fep(struct board_t *brd);
+static int dgap_test_bios(struct board_t *brd);
+static int dgap_test_fep(struct board_t *brd);
 static int dgap_tty_register_ports(struct board_t *brd);
 static int dgap_firmware_load(struct pci_dev *pdev, int card_type);
 
-/* Driver unload function */
 static void dgap_cleanup_module(void);
 
 module_exit(dgap_cleanup_module);
@@ -233,35 +205,27 @@ module_exit(dgap_cleanup_module);
 /*
  * File operations permitted on Control/Management major.
  */
-static const struct file_operations DgapBoardFops = {
+static const struct file_operations dgap_board_fops = {
 	.owner	= THIS_MODULE,
 };
 
-/*
- * Globals
- */
-static uint dgap_NumBoards;
-static struct board_t *dgap_Board[MAXBOARDS];
+static uint dgap_numboards;
+static struct board_t *dgap_board[MAXBOARDS];
 static ulong dgap_poll_counter;
 static char *dgap_config_buf;
 static int dgap_driver_state = DRIVER_INITIALIZED;
-DEFINE_SPINLOCK(dgap_dl_lock);
 static wait_queue_head_t dgap_dl_wait;
-static int dgap_dl_action;
 static int dgap_poll_tick = 20;	/* Poll interval - 20 ms */
 
-/*
- * Static vars.
- */
 static struct class *dgap_class;
 
-static struct board_t *dgap_BoardsByMajor[256];
+static struct board_t *dgap_boards_by_major[256];
 static uint dgap_count = 500;
 
 /*
  * Poller stuff
  */
-DEFINE_SPINLOCK(dgap_poll_lock);	/* Poll scheduling lock */
+static DEFINE_SPINLOCK(dgap_poll_lock);	/* Poll scheduling lock */
 static ulong dgap_poll_time;		/* Time of next poll */
 static uint dgap_poll_stop;		/* Used to tell poller to stop */
 static struct timer_list dgap_poll_timer;
@@ -310,12 +274,12 @@ MODULE_DEVICE_TABLE(pci, dgap_pci_tbl);
  */
 struct board_id {
 	uint config_type;
-	uchar *name;
+	u8 *name;
 	uint maxports;
 	uint dpatype;
 };
 
-static struct board_id dgap_Ids[] = {
+static struct board_id dgap_ids[] = {
 	{ PPCM,        PCI_DEV_XEM_NAME,     64, (T_PCXM|T_PCLITE|T_PCIBUS) },
 	{ PCX,         PCI_DEV_CX_NAME,     128, (T_CX|T_PCIBUS)            },
 	{ PCX,         PCI_DEV_CX_IBM_NAME, 128, (T_CX|T_PCIBUS)            },
@@ -342,33 +306,33 @@ static struct pci_driver dgap_driver = {
 };
 
 struct firmware_info {
-	uchar *conf_name;       /* dgap.conf */
-	uchar *bios_name;	/* BIOS filename */
-	uchar *fep_name;	/* FEP  filename */
-	uchar *con_name;	/* Concentrator filename  FIXME*/
-	int num;                /* sequence number */
+	u8 *conf_name;  /* dgap.conf */
+	u8 *bios_name;	/* BIOS filename */
+	u8 *fep_name;	/* FEP  filename */
+	u8 *con_name;	/* Concentrator filename  FIXME*/
+	int num;        /* sequence number */
 };
 
 /*
  * Firmware - BIOS, FEP, and CONC filenames
  */
 static struct firmware_info fw_info[] = {
-	{ "dgap/dgap.conf", "dgap/sxbios.bin",  "dgap/sxfep.bin",  0, 0 },
-	{ "dgap/dgap.conf", "dgap/cxpbios.bin", "dgap/cxpfep.bin", 0, 1 },
-	{ "dgap/dgap.conf", "dgap/cxpbios.bin", "dgap/cxpfep.bin", 0, 2 },
-	{ "dgap/dgap.conf", "dgap/pcibios.bin", "dgap/pcifep.bin", 0, 3 },
-	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  0, 4 },
-	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  0, 5 },
-	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  0, 6 },
-	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  0, 7 },
-	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  0, 8 },
-	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  0, 9 },
-	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  0, 10 },
-	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  0, 11 },
-	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  0, 12 },
-	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  0, 13 },
-	{ "dgap/dgap.conf", "dgap/sxbios.bin",  "dgap/sxfep.bin",  0, 14 },
-	{0,}
+	{ "dgap/dgap.conf", "dgap/sxbios.bin",  "dgap/sxfep.bin",  NULL, 0 },
+	{ "dgap/dgap.conf", "dgap/cxpbios.bin", "dgap/cxpfep.bin", NULL, 1 },
+	{ "dgap/dgap.conf", "dgap/cxpbios.bin", "dgap/cxpfep.bin", NULL, 2 },
+	{ "dgap/dgap.conf", "dgap/pcibios.bin", "dgap/pcifep.bin", NULL, 3 },
+	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  NULL, 4 },
+	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  NULL, 5 },
+	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  NULL, 6 },
+	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  NULL, 7 },
+	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  NULL, 8 },
+	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  NULL, 9 },
+	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  NULL, 10 },
+	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  NULL, 11 },
+	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  NULL, 12 },
+	{ "dgap/dgap.conf", "dgap/xrbios.bin",  "dgap/xrfep.bin",  NULL, 13 },
+	{ "dgap/dgap.conf", "dgap/sxbios.bin",  "dgap/sxfep.bin",  NULL, 14 },
+	{NULL,}
 };
 
 /*
@@ -394,7 +358,7 @@ static struct digi_t dgap_digi_init = {
  * 1 stop bit.
  */
 
-static struct ktermios DgapDefaultTermios = {
+static struct ktermios dgap_default_termios = {
 	.c_iflag =	(DEFAULT_IFLAGS),	/* iflags */
 	.c_oflag =	(DEFAULT_OFLAGS),	/* oflags */
 	.c_cflag =	(DEFAULT_CFLAGS),	/* cflags */
@@ -434,8 +398,8 @@ static struct cnode dgap_head;
 static char dgap_cword[MAXCWORD];
 
 struct toklist {
-	int	token;
-	char	*string;
+	int token;
+	char *string;
 };
 
 static struct toklist dgap_tlist[] = {
@@ -502,7 +466,7 @@ static struct toklist dgap_tlist[] = {
  */
 static int dgap_init_module(void)
 {
-	int rc = 0;
+	int rc;
 
 	pr_info("%s, Digi International Part Number %s\n", DG_NAME, DG_PART);
 
@@ -535,7 +499,7 @@ module_init(dgap_init_module);
  */
 static int dgap_start(void)
 {
-	int rc = 0;
+	int rc;
 	unsigned long flags;
 	struct device *device;
 
@@ -545,7 +509,7 @@ static int dgap_start(void)
 	 */
 	dgap_init_globals();
 
-	dgap_NumBoards = 0;
+	dgap_numboards = 0;
 
 	pr_info("For the tools package please visit http://www.digi.com\n");
 
@@ -556,7 +520,7 @@ static int dgap_start(void)
 	/*
 	 * Register management/dpa devices
 	 */
-	rc = register_chrdev(DIGI_DGAP_MAJOR, "dgap", &DgapBoardFops);
+	rc = register_chrdev(DIGI_DGAP_MAJOR, "dgap", &dgap_board_fops);
 	if (rc < 0)
 		return rc;
 
@@ -602,24 +566,23 @@ static int dgap_init_pci(void)
 	return pci_register_driver(&dgap_driver);
 }
 
-/* returns count (>= 0), or negative on error */
 static int dgap_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
 	int rc;
 
-	/* wake up and enable device */
+	if (dgap_numboards >= MAXBOARDS)
+		return -EPERM;
+
 	rc = pci_enable_device(pdev);
+	if (rc)
+		return -EIO;
 
-	if (rc < 0) {
-		rc = -EIO;
-	} else {
-		rc = dgap_probe1(pdev, ent->driver_data);
-		if (rc == 0) {
-			dgap_NumBoards++;
-			rc = dgap_firmware_load(pdev, ent->driver_data);
-		}
-	}
-	return rc;
+	rc = dgap_probe1(pdev, ent->driver_data);
+	if (rc)
+		return rc;
+
+	dgap_numboards++;
+	return dgap_firmware_load(pdev, ent->driver_data);
 }
 
 static int dgap_probe1(struct pci_dev *pdev, int card_type)
@@ -655,15 +618,13 @@ static void dgap_cleanup_module(void)
 	class_destroy(dgap_class);
 	unregister_chrdev(DIGI_DGAP_MAJOR, "dgap");
 
-	kfree(dgap_config_buf);
-
-	for (i = 0; i < dgap_NumBoards; ++i) {
-		dgap_remove_ports_sysfiles(dgap_Board[i]);
-		dgap_tty_uninit(dgap_Board[i]);
-		dgap_cleanup_board(dgap_Board[i]);
+	for (i = 0; i < dgap_numboards; ++i) {
+		dgap_remove_ports_sysfiles(dgap_board[i]);
+		dgap_tty_uninit(dgap_board[i]);
+		dgap_cleanup_board(dgap_board[i]);
 	}
 
-	if (dgap_NumBoards)
+	if (dgap_numboards)
 		pci_unregister_driver(&dgap_driver);
 }
 
@@ -674,7 +635,7 @@ static void dgap_cleanup_module(void)
  */
 static void dgap_cleanup_board(struct board_t *brd)
 {
-	int i = 0;
+	int i;
 
 	if (!brd || brd->magic != DGAP_BOARD_MAGIC)
 		return;
@@ -703,7 +664,7 @@ static void dgap_cleanup_board(struct board_t *brd)
 	kfree(brd->flipbuf);
 	kfree(brd->flipflagbuf);
 
-	dgap_Board[brd->boardnum] = NULL;
+	dgap_board[brd->boardnum] = NULL;
 
 	kfree(brd);
 }
@@ -717,28 +678,28 @@ static int dgap_found_board(struct pci_dev *pdev, int id)
 {
 	struct board_t *brd;
 	unsigned int pci_irq;
-	int i = 0;
+	int i;
 
 	/* get the board structure and prep it */
 	brd = kzalloc(sizeof(struct board_t), GFP_KERNEL);
 	if (!brd)
 		return -ENOMEM;
 
-	dgap_Board[dgap_NumBoards] = brd;
+	dgap_board[dgap_numboards] = brd;
 
 	/* store the info for the board we've found */
 	brd->magic = DGAP_BOARD_MAGIC;
-	brd->boardnum = dgap_NumBoards;
+	brd->boardnum = dgap_numboards;
 	brd->firstminor = 0;
 	brd->vendor = dgap_pci_tbl[id].vendor;
 	brd->device = dgap_pci_tbl[id].device;
 	brd->pdev = pdev;
 	brd->pci_bus = pdev->bus->number;
 	brd->pci_slot = PCI_SLOT(pdev->devfn);
-	brd->name = dgap_Ids[id].name;
-	brd->maxports = dgap_Ids[id].maxports;
-	brd->type = dgap_Ids[id].config_type;
-	brd->dpatype = dgap_Ids[id].dpatype;
+	brd->name = dgap_ids[id].name;
+	brd->maxports = dgap_ids[id].maxports;
+	brd->type = dgap_ids[id].config_type;
+	brd->dpatype = dgap_ids[id].dpatype;
 	brd->dpastatus = BD_NOFEP;
 	init_waitqueue_head(&brd->state_wait);
 
@@ -819,9 +780,8 @@ static int dgap_found_board(struct pci_dev *pdev, int id)
 	if (i)
 		brd->state = BOARD_FAILED;
 
-	pr_info("dgap: board %d: %s (rev %d), irq %ld, %s\n",
-		dgap_NumBoards, brd->name, brd->rev, brd->irq,
-		brd->state ? "NOT READY\0" : "READY\0");
+	pr_info("dgap: board %d: %s (rev %d), irq %ld\n",
+		dgap_numboards, brd->name, brd->rev, brd->irq);
 
 	return 0;
 }
@@ -856,14 +816,15 @@ static int dgap_finalize_board_init(struct board_t *brd)
 
 static int dgap_firmware_load(struct pci_dev *pdev, int card_type)
 {
-	struct board_t *brd = dgap_Board[dgap_NumBoards - 1];
+	struct board_t *brd = dgap_board[dgap_numboards - 1];
 	const struct firmware *fw;
+	char *tmp_ptr;
 	int ret;
 
 	dgap_get_vpd(brd);
 	dgap_do_reset_board(brd);
 
-	if (fw_info[card_type].conf_name) {
+	if ((fw_info[card_type].conf_name) && !dgap_config_buf) {
 		ret = request_firmware(&fw, fw_info[card_type].conf_name,
 					 &pdev->dev);
 		if (ret) {
@@ -871,20 +832,28 @@ static int dgap_firmware_load(struct pci_dev *pdev, int card_type)
 				fw_info[card_type].conf_name);
 			return ret;
 		}
+
+		dgap_config_buf = kzalloc(fw->size + 1, GFP_KERNEL);
 		if (!dgap_config_buf) {
-			dgap_config_buf = kmalloc(fw->size + 1, GFP_ATOMIC);
-			if (!dgap_config_buf) {
-				release_firmware(fw);
-				return -ENOMEM;
-			}
+			release_firmware(fw);
+			return -ENOMEM;
 		}
 
 		memcpy(dgap_config_buf, fw->data, fw->size);
 		release_firmware(fw);
-		dgap_config_buf[fw->size + 1] = '\0';
 
-		if (dgap_parsefile(&dgap_config_buf, TRUE) != 0)
+		/*
+		 * preserve dgap_config_buf
+		 * as dgap_parsefile would
+		 * otherwise alter it.
+		 */
+		tmp_ptr = dgap_config_buf;
+
+		if (dgap_parsefile(&tmp_ptr, TRUE) != 0) {
+			kfree(dgap_config_buf);
 			return -EINVAL;
+		}
+		kfree(dgap_config_buf);
 	}
 
 	ret = dgap_after_config_loaded(brd->boardnum);
@@ -911,8 +880,13 @@ static int dgap_firmware_load(struct pci_dev *pdev, int card_type)
 		return -EINVAL;
 	}
 
-	dgap_tty_register(brd);
-	dgap_finalize_board_init(brd);
+	ret = dgap_tty_register(brd);
+	if (ret)
+		return ret;
+
+	ret = dgap_finalize_board_init(brd);
+	if (ret)
+		return ret;
 
 	if (fw_info[card_type].bios_name) {
 		ret = request_firmware(&fw, fw_info[card_type].bios_name,
@@ -926,8 +900,9 @@ static int dgap_firmware_load(struct pci_dev *pdev, int card_type)
 		release_firmware(fw);
 
 		/* Wait for BIOS to test board... */
-		if (!dgap_do_wait_for_bios(brd))
-			return -ENXIO;
+		ret = dgap_test_bios(brd);
+		if (ret)
+			return ret;
 	}
 
 	if (fw_info[card_type].fep_name) {
@@ -942,8 +917,9 @@ static int dgap_firmware_load(struct pci_dev *pdev, int card_type)
 		release_firmware(fw);
 
 		/* Wait for FEP to load on board... */
-		if (!dgap_do_wait_for_fep(brd))
-			return -ENXIO;
+		ret = dgap_test_fep(brd);
+		if (ret)
+			return ret;
 	}
 
 #ifdef DIGI_CONCENTRATORS_SUPPORTED
@@ -999,7 +975,7 @@ static int dgap_firmware_load(struct pci_dev *pdev, int card_type)
 static int dgap_do_remap(struct board_t *brd)
 {
 	if (!brd || brd->magic != DGAP_BOARD_MAGIC)
-		return -ENXIO;
+		return -EIO;
 
 	if (!request_mem_region(brd->membase, 0x200000, "dgap"))
 		return -ENOMEM;
@@ -1077,10 +1053,10 @@ static void dgap_poll_handler(ulong dummy)
 	 * Otherwise, use our new tasklet based poller, which should
 	 * speed things up for multiple boards.
 	 */
-	if ((dgap_NumBoards == 1) || (num_online_cpus() <= 1)) {
-		for (i = 0; i < dgap_NumBoards; i++) {
+	if ((dgap_numboards == 1) || (num_online_cpus() <= 1)) {
+		for (i = 0; i < dgap_numboards; i++) {
 
-			brd = dgap_Board[i];
+			brd = dgap_board[i];
 
 			if (brd->state == BOARD_FAILED)
 				continue;
@@ -1093,8 +1069,8 @@ static void dgap_poll_handler(ulong dummy)
 		 * Go thru each board, kicking off a
 		 * tasklet for each if needed
 		 */
-		for (i = 0; i < dgap_NumBoards; i++) {
-			brd = dgap_Board[i];
+		for (i = 0; i < dgap_numboards; i++) {
+			brd = dgap_board[i];
 
 			/*
 			 * Attempt to grab the board lock.
@@ -1189,15 +1165,14 @@ static irqreturn_t dgap_intr(int irq, void *voidbrd)
  */
 static void dgap_init_globals(void)
 {
-	int i = 0;
+	int i;
 
 	for (i = 0; i < MAXBOARDS; i++)
-		dgap_Board[i] = NULL;
+		dgap_board[i] = NULL;
 
 	init_timer(&dgap_poll_timer);
 
 	init_waitqueue_head(&dgap_dl_wait);
-	dgap_dl_action = 0;
 }
 
 /************************************************************************
@@ -1233,86 +1208,105 @@ static int dgap_ms_sleep(ulong ms)
  */
 static int dgap_tty_register(struct board_t *brd)
 {
-	int rc = 0;
+	int rc;
 
-	brd->SerialDriver = alloc_tty_driver(MAXPORTS);
-
-	snprintf(brd->SerialName, MAXTTYNAMELEN, "tty_dgap_%d_", brd->boardnum);
-	brd->SerialDriver->name = brd->SerialName;
-	brd->SerialDriver->name_base = 0;
-	brd->SerialDriver->major = 0;
-	brd->SerialDriver->minor_start = 0;
-	brd->SerialDriver->type = TTY_DRIVER_TYPE_SERIAL;
-	brd->SerialDriver->subtype = SERIAL_TYPE_NORMAL;
-	brd->SerialDriver->init_termios = DgapDefaultTermios;
-	brd->SerialDriver->driver_name = DRVSTR;
-	brd->SerialDriver->flags = (TTY_DRIVER_REAL_RAW |
+	brd->serial_driver = tty_alloc_driver(MAXPORTS, 0);
+	if (IS_ERR(brd->serial_driver))
+		return PTR_ERR(brd->serial_driver);
+
+	snprintf(brd->serial_name, MAXTTYNAMELEN, "tty_dgap_%d_",
+		 brd->boardnum);
+	brd->serial_driver->name = brd->serial_name;
+	brd->serial_driver->name_base = 0;
+	brd->serial_driver->major = 0;
+	brd->serial_driver->minor_start = 0;
+	brd->serial_driver->type = TTY_DRIVER_TYPE_SERIAL;
+	brd->serial_driver->subtype = SERIAL_TYPE_NORMAL;
+	brd->serial_driver->init_termios = dgap_default_termios;
+	brd->serial_driver->driver_name = DRVSTR;
+	brd->serial_driver->flags = (TTY_DRIVER_REAL_RAW |
 				    TTY_DRIVER_DYNAMIC_DEV |
 				    TTY_DRIVER_HARDWARE_BREAK);
 
 	/* The kernel wants space to store pointers to tty_structs */
-	brd->SerialDriver->ttys =
+	brd->serial_driver->ttys =
 		kzalloc(MAXPORTS * sizeof(struct tty_struct *), GFP_KERNEL);
-	if (!brd->SerialDriver->ttys)
-		return -ENOMEM;
+	if (!brd->serial_driver->ttys) {
+		rc = -ENOMEM;
+		goto free_serial_drv;
+	}
 
 	/*
 	 * Entry points for driver.  Called by the kernel from
 	 * tty_io.c and n_tty.c.
 	 */
-	tty_set_operations(brd->SerialDriver, &dgap_tty_ops);
+	tty_set_operations(brd->serial_driver, &dgap_tty_ops);
 
 	/*
 	 * If we're doing transparent print, we have to do all of the above
 	 * again, separately so we don't get the LD confused about what major
 	 * we are when we get into the dgap_tty_open() routine.
 	 */
-	brd->PrintDriver = alloc_tty_driver(MAXPORTS);
-
-	snprintf(brd->PrintName, MAXTTYNAMELEN, "pr_dgap_%d_", brd->boardnum);
-	brd->PrintDriver->name = brd->PrintName;
-	brd->PrintDriver->name_base = 0;
-	brd->PrintDriver->major = 0;
-	brd->PrintDriver->minor_start = 0;
-	brd->PrintDriver->type = TTY_DRIVER_TYPE_SERIAL;
-	brd->PrintDriver->subtype = SERIAL_TYPE_NORMAL;
-	brd->PrintDriver->init_termios = DgapDefaultTermios;
-	brd->PrintDriver->driver_name = DRVSTR;
-	brd->PrintDriver->flags = (TTY_DRIVER_REAL_RAW |
+	brd->print_driver = tty_alloc_driver(MAXPORTS, 0);
+	if (IS_ERR(brd->print_driver)) {
+		rc = PTR_ERR(brd->print_driver);
+		goto free_serial_drv;
+	}
+
+	snprintf(brd->print_name, MAXTTYNAMELEN, "pr_dgap_%d_",
+		 brd->boardnum);
+	brd->print_driver->name = brd->print_name;
+	brd->print_driver->name_base = 0;
+	brd->print_driver->major = 0;
+	brd->print_driver->minor_start = 0;
+	brd->print_driver->type = TTY_DRIVER_TYPE_SERIAL;
+	brd->print_driver->subtype = SERIAL_TYPE_NORMAL;
+	brd->print_driver->init_termios = dgap_default_termios;
+	brd->print_driver->driver_name = DRVSTR;
+	brd->print_driver->flags = (TTY_DRIVER_REAL_RAW |
 				   TTY_DRIVER_DYNAMIC_DEV |
 				   TTY_DRIVER_HARDWARE_BREAK);
 
 	/* The kernel wants space to store pointers to tty_structs */
-	brd->PrintDriver->ttys =
+	brd->print_driver->ttys =
 		kzalloc(MAXPORTS * sizeof(struct tty_struct *), GFP_KERNEL);
-	if (!brd->PrintDriver->ttys)
-		return -ENOMEM;
+	if (!brd->print_driver->ttys) {
+		rc = -ENOMEM;
+		goto free_print_drv;
+	}
 
 	/*
 	 * Entry points for driver.  Called by the kernel from
 	 * tty_io.c and n_tty.c.
 	 */
-	tty_set_operations(brd->PrintDriver, &dgap_tty_ops);
+	tty_set_operations(brd->print_driver, &dgap_tty_ops);
 
-	if (!brd->dgap_Major_Serial_Registered) {
-		/* Register tty devices */
-		rc = tty_register_driver(brd->SerialDriver);
-		if (rc < 0)
-			return rc;
-		brd->dgap_Major_Serial_Registered = TRUE;
-		dgap_BoardsByMajor[brd->SerialDriver->major] = brd;
-		brd->dgap_Serial_Major = brd->SerialDriver->major;
-	}
+	/* Register tty devices */
+	rc = tty_register_driver(brd->serial_driver);
+	if (rc < 0)
+		goto free_print_drv;
 
-	if (!brd->dgap_Major_TransparentPrint_Registered) {
-		/* Register Transparent Print devices */
-		rc = tty_register_driver(brd->PrintDriver);
-		if (rc < 0)
-			return rc;
-		brd->dgap_Major_TransparentPrint_Registered = TRUE;
-		dgap_BoardsByMajor[brd->PrintDriver->major] = brd;
-		brd->dgap_TransparentPrint_Major = brd->PrintDriver->major;
-	}
+	/* Register Transparent Print devices */
+	rc = tty_register_driver(brd->print_driver);
+	if (rc < 0)
+		goto unregister_serial_drv;
+
+	brd->dgap_major_serial_registered = TRUE;
+	dgap_boards_by_major[brd->serial_driver->major] = brd;
+	brd->dgap_serial_major = brd->serial_driver->major;
+
+	brd->dgap_major_transparent_print_registered = TRUE;
+	dgap_boards_by_major[brd->print_driver->major] = brd;
+	brd->dgap_transparent_print_major = brd->print_driver->major;
+
+	return 0;
+
+unregister_serial_drv:
+	tty_unregister_driver(brd->serial_driver);
+free_print_drv:
+	put_tty_driver(brd->print_driver);
+free_serial_drv:
+	put_tty_driver(brd->serial_driver);
 
 	return rc;
 }
@@ -1327,15 +1321,15 @@ static int dgap_tty_init(struct board_t *brd)
 {
 	int i;
 	int tlw;
-	uint true_count = 0;
-	uchar *vaddr;
-	uchar modem = 0;
+	uint true_count;
+	u8 __iomem *vaddr;
+	u8 modem;
 	struct channel_t *ch;
-	struct bs_t *bs;
-	struct cm_t *cm;
+	struct bs_t __iomem *bs;
+	struct cm_t __iomem *cm;
 
 	if (!brd)
-		return -ENXIO;
+		return -EIO;
 
 	/*
 	 * Initialize board structure elements.
@@ -1373,7 +1367,7 @@ static int dgap_tty_init(struct board_t *brd)
 		if (!brd->nasync) {
 			brd->state = BOARD_FAILED;
 			brd->dpastatus = BD_NOFEP;
-			return -ENXIO;
+			return -EIO;
 		}
 	}
 
@@ -1384,7 +1378,7 @@ static int dgap_tty_init(struct board_t *brd)
 	for (i = 0; i < brd->nasync; i++) {
 		if (!brd->channels[i]) {
 			brd->channels[i] =
-				kzalloc(sizeof(struct channel_t), GFP_ATOMIC);
+				kzalloc(sizeof(struct channel_t), GFP_KERNEL);
 			if (!brd->channels[i])
 				return -ENOMEM;
 		}
@@ -1393,8 +1387,8 @@ static int dgap_tty_init(struct board_t *brd)
 	ch = brd->channels[0];
 	vaddr = brd->re_map_membase;
 
-	bs = (struct bs_t *) ((ulong) vaddr + CHANBUF);
-	cm = (struct cm_t *) ((ulong) vaddr + CMDBUF);
+	bs = (struct bs_t __iomem *) ((ulong) vaddr + CHANBUF);
+	cm = (struct cm_t __iomem *) ((ulong) vaddr + CMDBUF);
 
 	brd->bd_bs = bs;
 
@@ -1437,8 +1431,8 @@ static int dgap_tty_init(struct board_t *brd)
 			ch->ch_dsr	= DM_DSR;
 		}
 
-		ch->ch_taddr = vaddr + ((ch->ch_bs->tx_seg) << 4);
-		ch->ch_raddr = vaddr + ((ch->ch_bs->rx_seg) << 4);
+		ch->ch_taddr = vaddr + (ioread16(&(ch->ch_bs->tx_seg)) << 4);
+		ch->ch_raddr = vaddr + (ioread16(&(ch->ch_bs->rx_seg)) << 4);
 		ch->ch_tx_win = 0;
 		ch->ch_rx_win = 0;
 		ch->ch_tsize = readw(&(ch->ch_bs->tx_max)) + 1;
@@ -1468,7 +1462,6 @@ static int dgap_tty_init(struct board_t *brd)
 		init_waitqueue_head(&ch->ch_flags_wait);
 		init_waitqueue_head(&ch->ch_tun.un_flags_wait);
 		init_waitqueue_head(&ch->ch_pun.un_flags_wait);
-		init_waitqueue_head(&ch->ch_sniff_wait);
 
 		/* Turn on all modem interrupts for now */
 		modem = (DM_CD | DM_DSR | DM_CTS | DM_RI);
@@ -1498,160 +1491,39 @@ static int dgap_tty_init(struct board_t *brd)
 static void dgap_tty_uninit(struct board_t *brd)
 {
 	struct device *dev;
-	int i = 0;
+	int i;
 
-	if (brd->dgap_Major_Serial_Registered) {
-		dgap_BoardsByMajor[brd->SerialDriver->major] = NULL;
-		brd->dgap_Serial_Major = 0;
+	if (brd->dgap_major_serial_registered) {
+		dgap_boards_by_major[brd->serial_driver->major] = NULL;
+		brd->dgap_serial_major = 0;
 		for (i = 0; i < brd->nasync; i++) {
-			tty_port_destroy(&brd->SerialPorts[i]);
+			tty_port_destroy(&brd->serial_ports[i]);
 			dev = brd->channels[i]->ch_tun.un_sysfs;
 			dgap_remove_tty_sysfs(dev);
-			tty_unregister_device(brd->SerialDriver, i);
+			tty_unregister_device(brd->serial_driver, i);
 		}
-		tty_unregister_driver(brd->SerialDriver);
-		kfree(brd->SerialDriver->ttys);
-		brd->SerialDriver->ttys = NULL;
-		put_tty_driver(brd->SerialDriver);
-		kfree(brd->SerialPorts);
-		brd->dgap_Major_Serial_Registered = FALSE;
+		tty_unregister_driver(brd->serial_driver);
+		put_tty_driver(brd->serial_driver);
+		kfree(brd->serial_ports);
+		brd->dgap_major_serial_registered = FALSE;
 	}
 
-	if (brd->dgap_Major_TransparentPrint_Registered) {
-		dgap_BoardsByMajor[brd->PrintDriver->major] = NULL;
-		brd->dgap_TransparentPrint_Major = 0;
+	if (brd->dgap_major_transparent_print_registered) {
+		dgap_boards_by_major[brd->print_driver->major] = NULL;
+		brd->dgap_transparent_print_major = 0;
 		for (i = 0; i < brd->nasync; i++) {
-			tty_port_destroy(&brd->PrinterPorts[i]);
+			tty_port_destroy(&brd->printer_ports[i]);
 			dev = brd->channels[i]->ch_pun.un_sysfs;
 			dgap_remove_tty_sysfs(dev);
-			tty_unregister_device(brd->PrintDriver, i);
+			tty_unregister_device(brd->print_driver, i);
 		}
-		tty_unregister_driver(brd->PrintDriver);
-		kfree(brd->PrintDriver->ttys);
-		brd->PrintDriver->ttys = NULL;
-		put_tty_driver(brd->PrintDriver);
-		kfree(brd->PrinterPorts);
-		brd->dgap_Major_TransparentPrint_Registered = FALSE;
+		tty_unregister_driver(brd->print_driver);
+		put_tty_driver(brd->print_driver);
+		kfree(brd->printer_ports);
+		brd->dgap_major_transparent_print_registered = FALSE;
 	}
 }
 
-#define TMPBUFLEN (1024)
-/*
- * dgap_sniff - Dump data out to the "sniff" buffer if the
- * proc sniff file is opened...
- */
-static void dgap_sniff_nowait_nolock(struct channel_t *ch, uchar *text,
-				     uchar *buf, int len)
-{
-	struct timeval tv;
-	int n;
-	int r;
-	int nbuf;
-	int i;
-	int tmpbuflen;
-	char tmpbuf[TMPBUFLEN];
-	char *p = tmpbuf;
-	int too_much_data;
-
-	/* Leave if sniff not open */
-	if (!(ch->ch_sniff_flags & SNIFF_OPEN))
-		return;
-
-	do_gettimeofday(&tv);
-
-	/* Create our header for data dump */
-	p += sprintf(p, "<%ld %ld><%s><", tv.tv_sec, tv.tv_usec, text);
-	tmpbuflen = p - tmpbuf;
-
-	do {
-		too_much_data = 0;
-
-		for (i = 0; i < len && tmpbuflen < (TMPBUFLEN - 4); i++) {
-			p += sprintf(p, "%02x ", *buf);
-			buf++;
-			tmpbuflen = p - tmpbuf;
-		}
-
-		if (tmpbuflen < (TMPBUFLEN - 4)) {
-			if (i > 0)
-				p += sprintf(p - 1, "%s\n", ">");
-			else
-				p += sprintf(p, "%s\n", ">");
-		} else {
-			too_much_data = 1;
-			len -= i;
-		}
-
-		nbuf = strlen(tmpbuf);
-		p = tmpbuf;
-
-		/*
-		 *  Loop while data remains.
-		 */
-		while (nbuf > 0 && ch->ch_sniff_buf) {
-			/*
-			 *  Determine the amount of available space left in the
-			 *  buffer.  If there's none, wait until some appears.
-			 */
-			n = (ch->ch_sniff_out - ch->ch_sniff_in - 1) &
-			     SNIFF_MASK;
-
-			/*
-			 * If there is no space left to write to in our sniff
-			 * buffer, we have no choice but to drop the data.
-			 * We *cannot* sleep here waiting for space, because
-			 * this function was probably called by the
-			 * interrupt/timer routines!
-			 */
-			if (n == 0)
-				return;
-
-			/*
-			 * Copy as much data as will fit.
-			 */
-
-			if (n > nbuf)
-				n = nbuf;
-
-			r = SNIFF_MAX - ch->ch_sniff_in;
-
-			if (r <= n) {
-				memcpy(ch->ch_sniff_buf +
-				       ch->ch_sniff_in, p, r);
-
-				n -= r;
-				ch->ch_sniff_in = 0;
-				p += r;
-				nbuf -= r;
-			}
-
-			memcpy(ch->ch_sniff_buf + ch->ch_sniff_in, p, n);
-
-			ch->ch_sniff_in += n;
-			p += n;
-			nbuf -= n;
-
-			/*
-			 *  Wakeup any thread waiting for data
-			 */
-			if (ch->ch_sniff_flags & SNIFF_WAIT_DATA) {
-				ch->ch_sniff_flags &= ~SNIFF_WAIT_DATA;
-				wake_up_interruptible(&ch->ch_sniff_wait);
-			}
-		}
-
-		/*
-		 * If the user sent us too much data to push into our tmpbuf,
-		 * we need to keep looping around on all the data.
-		 */
-		if (too_much_data) {
-			p = tmpbuf;
-			tmpbuflen = 0;
-		}
-
-	} while (too_much_data);
-}
-
 /*=======================================================================
  *
  *      dgap_input - Process received data.
@@ -1663,21 +1535,21 @@ static void dgap_sniff_nowait_nolock(struct channel_t *ch, uchar *text,
 static void dgap_input(struct channel_t *ch)
 {
 	struct board_t *bd;
-	struct bs_t	*bs;
+	struct bs_t __iomem *bs;
 	struct tty_struct *tp;
 	struct tty_ldisc *ld;
-	uint	rmask;
-	uint	head;
-	uint	tail;
-	int	data_len;
-	ulong	lock_flags;
-	ulong   lock_flags2;
+	uint rmask;
+	uint head;
+	uint tail;
+	int data_len;
+	ulong lock_flags;
+	ulong lock_flags2;
 	int flip_len;
-	int len = 0;
-	int n = 0;
-	uchar *buf;
-	uchar tmpchar;
-	int s = 0;
+	int len;
+	int n;
+	u8 *buf;
+	u8 tmpchar;
+	int s;
 
 	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
 		return;
@@ -1814,8 +1686,7 @@ static void dgap_input(struct channel_t *ch)
 		if (s <= 0)
 			break;
 
-		memcpy_fromio(buf, (char *) ch->ch_raddr + tail, s);
-		dgap_sniff_nowait_nolock(ch, "USER READ", buf, s);
+		memcpy_fromio(buf, ch->ch_raddr + tail, s);
 
 		tail += s;
 		buf += s;
@@ -1987,29 +1858,27 @@ static void dgap_carrier(struct channel_t *ch)
  */
 static int dgap_tty_open(struct tty_struct *tty, struct file *file)
 {
-	struct board_t	*brd;
+	struct board_t *brd;
 	struct channel_t *ch;
-	struct un_t	*un;
-	struct bs_t	*bs;
-	uint		major = 0;
-	uint		minor = 0;
-	int		rc = 0;
-	ulong		lock_flags;
-	ulong		lock_flags2;
-	u16		head;
-
-	rc = 0;
+	struct un_t *un;
+	struct bs_t __iomem *bs;
+	uint major;
+	uint minor;
+	int rc;
+	ulong lock_flags;
+	ulong lock_flags2;
+	u16 head;
 
 	major = MAJOR(tty_devnum(tty));
 	minor = MINOR(tty_devnum(tty));
 
 	if (major > 255)
-		return -ENXIO;
+		return -EIO;
 
 	/* Get board pointer from our array of majors we have allocated */
-	brd = dgap_BoardsByMajor[major];
+	brd = dgap_boards_by_major[major];
 	if (!brd)
-		return -ENXIO;
+		return -EIO;
 
 	/*
 	 * If board is not yet up to a state of READY, go to
@@ -2026,35 +1895,35 @@ static int dgap_tty_open(struct tty_struct *tty, struct file *file)
 	/* The wait above should guarantee this cannot happen */
 	if (brd->state != BOARD_READY) {
 		spin_unlock_irqrestore(&brd->bd_lock, lock_flags);
-		return -ENXIO;
+		return -EIO;
 	}
 
 	/* If opened device is greater than our number of ports, bail. */
 	if (MINOR(tty_devnum(tty)) > brd->nasync) {
 		spin_unlock_irqrestore(&brd->bd_lock, lock_flags);
-		return -ENXIO;
+		return -EIO;
 	}
 
 	ch = brd->channels[minor];
 	if (!ch) {
 		spin_unlock_irqrestore(&brd->bd_lock, lock_flags);
-		return -ENXIO;
+		return -EIO;
 	}
 
 	/* Grab channel lock */
 	spin_lock_irqsave(&ch->ch_lock, lock_flags2);
 
 	/* Figure out our type */
-	if (major == brd->dgap_Serial_Major) {
+	if (major == brd->dgap_serial_major) {
 		un = &brd->channels[minor]->ch_tun;
 		un->un_type = DGAP_SERIAL;
-	} else if (major == brd->dgap_TransparentPrint_Major) {
+	} else if (major == brd->dgap_transparent_print_major) {
 		un = &brd->channels[minor]->ch_pun;
 		un->un_type = DGAP_PRINT;
 	} else {
 		spin_unlock_irqrestore(&ch->ch_lock, lock_flags2);
 		spin_unlock_irqrestore(&brd->bd_lock, lock_flags);
-		return -ENXIO;
+		return -EIO;
 	}
 
 	/* Store our unit into driver_data, so we always have it available. */
@@ -2067,7 +1936,7 @@ static int dgap_tty_open(struct tty_struct *tty, struct file *file)
 	if (!bs) {
 		spin_unlock_irqrestore(&ch->ch_lock, lock_flags2);
 		spin_unlock_irqrestore(&brd->bd_lock, lock_flags);
-		return -ENXIO;
+		return -EIO;
 	}
 
 	/*
@@ -2145,18 +2014,18 @@ static int dgap_block_til_ready(struct tty_struct *tty, struct file *file,
 				struct channel_t *ch)
 {
 	int retval = 0;
-	struct un_t *un = NULL;
-	ulong   lock_flags;
-	uint	old_flags = 0;
-	int sleep_on_un_flags = 0;
+	struct un_t *un;
+	ulong lock_flags;
+	uint old_flags;
+	int sleep_on_un_flags;
 
 	if (!tty || tty->magic != TTY_MAGIC || !file || !ch ||
 		ch->magic != DGAP_CHANNEL_MAGIC)
-		return -ENXIO;
+		return -EIO;
 
 	un = tty->driver_data;
 	if (!un || un->magic != DGAP_UNIT_MAGIC)
-		return -ENXIO;
+		return -EIO;
 
 	spin_lock_irqsave(&ch->ch_lock, lock_flags);
 
@@ -2172,7 +2041,7 @@ static int dgap_block_til_ready(struct tty_struct *tty, struct file *file,
 		 * bail with error.
 		 */
 		if (ch->ch_bd->state == BOARD_FAILED) {
-			retval = -ENXIO;
+			retval = -EIO;
 			break;
 		}
 
@@ -2277,9 +2146,9 @@ static int dgap_block_til_ready(struct tty_struct *tty, struct file *file,
  */
 static void dgap_tty_hangup(struct tty_struct *tty)
 {
-	struct board_t	*bd;
+	struct board_t *bd;
 	struct channel_t *ch;
-	struct un_t	*un;
+	struct un_t *un;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return;
@@ -2298,7 +2167,6 @@ static void dgap_tty_hangup(struct tty_struct *tty)
 
 	/* flush the transmit queues */
 	dgap_tty_flush_buffer(tty);
-
 }
 
 /*
@@ -2312,7 +2180,6 @@ static void dgap_tty_close(struct tty_struct *tty, struct file *file)
 	struct channel_t *ch;
 	struct un_t *un;
 	ulong lock_flags;
-	int rc = 0;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return;
@@ -2378,7 +2245,7 @@ static void dgap_tty_close(struct tty_struct *tty, struct file *file)
 		/* wait for output to drain */
 		/* This will also return if we take an interrupt */
 
-		rc = dgap_wait_for_drain(tty);
+		dgap_wait_for_drain(tty);
 
 		dgap_tty_flush_buffer(tty);
 		tty_ldisc_flush(tty);
@@ -2441,17 +2308,17 @@ static void dgap_tty_close(struct tty_struct *tty, struct file *file)
  */
 static int dgap_tty_chars_in_buffer(struct tty_struct *tty)
 {
-	struct board_t *bd = NULL;
-	struct channel_t *ch = NULL;
-	struct un_t *un = NULL;
-	struct bs_t *bs = NULL;
-	uchar tbusy;
-	uint chars = 0;
+	struct board_t *bd;
+	struct channel_t *ch;
+	struct un_t *un;
+	struct bs_t __iomem *bs;
+	u8 tbusy;
+	uint chars;
 	u16 thead, ttail, tmask, chead, ctail;
-	ulong   lock_flags = 0;
-	ulong   lock_flags2 = 0;
+	ulong lock_flags = 0;
+	ulong lock_flags2 = 0;
 
-	if (tty == NULL)
+	if (!tty)
 		return 0;
 
 	un = tty->driver_data;
@@ -2535,27 +2402,25 @@ static int dgap_wait_for_drain(struct tty_struct *tty)
 {
 	struct channel_t *ch;
 	struct un_t *un;
-	struct bs_t *bs;
-	int ret = -EIO;
+	struct bs_t __iomem *bs;
+	int ret = 0;
 	uint count = 1;
-	ulong   lock_flags = 0;
+	ulong lock_flags = 0;
 
 	if (!tty || tty->magic != TTY_MAGIC)
-		return ret;
+		return -EIO;
 
 	un = tty->driver_data;
 	if (!un || un->magic != DGAP_UNIT_MAGIC)
-		return ret;
+		return -EIO;
 
 	ch = un->un_ch;
 	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
-		return ret;
+		return -EIO;
 
 	bs = ch->ch_bs;
 	if (!bs)
-		return ret;
-
-	ret = 0;
+		return -EIO;
 
 	/* Loop until data is drained */
 	while (count != 0) {
@@ -2596,10 +2461,10 @@ static int dgap_wait_for_drain(struct tty_struct *tty)
  */
 static int dgap_maxcps_room(struct tty_struct *tty, int bytes_available)
 {
-	struct channel_t *ch = NULL;
-	struct un_t *un = NULL;
+	struct channel_t *ch;
+	struct un_t *un;
 
-	if (tty == NULL)
+	if (!tty)
 		return bytes_available;
 
 	un = tty->driver_data;
@@ -2645,8 +2510,8 @@ static int dgap_maxcps_room(struct tty_struct *tty, int bytes_available)
 
 static inline void dgap_set_firmware_event(struct un_t *un, unsigned int event)
 {
-	struct channel_t *ch = NULL;
-	struct bs_t *bs = NULL;
+	struct channel_t *ch;
+	struct bs_t __iomem *bs;
 
 	if (!un || un->magic != DGAP_UNIT_MAGIC)
 		return;
@@ -2678,12 +2543,12 @@ static inline void dgap_set_firmware_event(struct un_t *un, unsigned int event)
  */
 static int dgap_tty_write_room(struct tty_struct *tty)
 {
-	struct channel_t *ch = NULL;
-	struct un_t *un = NULL;
-	struct bs_t *bs = NULL;
+	struct channel_t *ch;
+	struct un_t *un;
+	struct bs_t __iomem *bs;
 	u16 head, tail, tmask;
-	int ret = 0;
-	ulong   lock_flags = 0;
+	int ret;
+	ulong lock_flags = 0;
 
 	if (!tty)
 		return 0;
@@ -2767,13 +2632,13 @@ static int dgap_tty_put_char(struct tty_struct *tty, unsigned char c)
 static int dgap_tty_write(struct tty_struct *tty, const unsigned char *buf,
 				int count)
 {
-	struct channel_t *ch = NULL;
-	struct un_t *un = NULL;
-	struct bs_t *bs = NULL;
-	char *vaddr = NULL;
+	struct channel_t *ch;
+	struct un_t *un;
+	struct bs_t __iomem *bs;
+	char __iomem *vaddr;
 	u16 head, tail, tmask, remain;
-	int bufcount = 0, n = 0;
-	int orig_count = 0;
+	int bufcount, n;
+	int orig_count;
 	ulong lock_flags;
 
 	if (!tty)
@@ -2855,16 +2720,6 @@ static int dgap_tty_write(struct tty_struct *tty, const unsigned char *buf,
 		ch->ch_flags &= ~CH_PRON;
 	}
 
-	/*
-	 * If there is nothing left to copy, or
-	 * I can't handle any more data, leave.
-	 */
-	if (count <= 0) {
-		dgap_set_firmware_event(un, UN_LOW | UN_EMPTY);
-		spin_unlock_irqrestore(&ch->ch_lock, lock_flags);
-		return 0;
-	}
-
 	n = count;
 
 	/*
@@ -2878,9 +2733,7 @@ static int dgap_tty_write(struct tty_struct *tty, const unsigned char *buf,
 		n -= remain;
 		vaddr = ch->ch_taddr + head;
 
-		memcpy_toio(vaddr, (uchar *) buf, remain);
-		dgap_sniff_nowait_nolock(ch, "USER WRITE", (uchar *) buf,
-					remain);
+		memcpy_toio(vaddr, (u8 *) buf, remain);
 
 		head = ch->ch_tstart;
 		buf += remain;
@@ -2894,10 +2747,7 @@ static int dgap_tty_write(struct tty_struct *tty, const unsigned char *buf,
 		vaddr = ch->ch_taddr + head;
 		remain = n;
 
-		memcpy_toio(vaddr, (uchar *) buf, remain);
-		dgap_sniff_nowait_nolock(ch, "USER WRITE", (uchar *)buf,
-					remain);
-
+		memcpy_toio(vaddr, (u8 *) buf, remain);
 		head += remain;
 
 	}
@@ -2949,20 +2799,20 @@ static int dgap_tty_tiocmget(struct tty_struct *tty)
 {
 	struct channel_t *ch;
 	struct un_t *un;
-	int result = -EIO;
-	uchar mstat = 0;
+	int result;
+	u8 mstat;
 	ulong lock_flags;
 
 	if (!tty || tty->magic != TTY_MAGIC)
-		return result;
+		return -EIO;
 
 	un = tty->driver_data;
 	if (!un || un->magic != DGAP_UNIT_MAGIC)
-		return result;
+		return -EIO;
 
 	ch = un->un_ch;
 	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
-		return result;
+		return -EIO;
 
 	spin_lock_irqsave(&ch->ch_lock, lock_flags);
 
@@ -3001,24 +2851,23 @@ static int dgap_tty_tiocmset(struct tty_struct *tty,
 	struct board_t *bd;
 	struct channel_t *ch;
 	struct un_t *un;
-	int ret = -EIO;
 	ulong lock_flags;
 	ulong lock_flags2;
 
 	if (!tty || tty->magic != TTY_MAGIC)
-		return ret;
+		return -EIO;
 
 	un = tty->driver_data;
 	if (!un || un->magic != DGAP_UNIT_MAGIC)
-		return ret;
+		return -EIO;
 
 	ch = un->un_ch;
 	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
-		return ret;
+		return -EIO;
 
 	bd = ch->ch_bd;
 	if (!bd || bd->magic != DGAP_BOARD_MAGIC)
-		return ret;
+		return -EIO;
 
 	spin_lock_irqsave(&bd->bd_lock, lock_flags);
 	spin_lock_irqsave(&ch->ch_lock, lock_flags2);
@@ -3061,24 +2910,23 @@ static int dgap_tty_send_break(struct tty_struct *tty, int msec)
 	struct board_t *bd;
 	struct channel_t *ch;
 	struct un_t *un;
-	int ret = -EIO;
 	ulong lock_flags;
 	ulong lock_flags2;
 
 	if (!tty || tty->magic != TTY_MAGIC)
-		return ret;
+		return -EIO;
 
 	un = tty->driver_data;
 	if (!un || un->magic != DGAP_UNIT_MAGIC)
-		return ret;
+		return -EIO;
 
 	ch = un->un_ch;
 	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
-		return ret;
+		return -EIO;
 
 	bd = ch->ch_bd;
 	if (!bd || bd->magic != DGAP_BOARD_MAGIC)
-		return ret;
+		return -EIO;
 
 	switch (msec) {
 	case -1:
@@ -3175,13 +3023,13 @@ static void dgap_tty_send_xchar(struct tty_struct *tty, char c)
  */
 static int dgap_get_modem_info(struct channel_t *ch, unsigned int __user *value)
 {
-	int result = 0;
-	uchar mstat = 0;
+	int result;
+	u8 mstat;
 	ulong lock_flags;
-	int rc = 0;
+	int rc;
 
 	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
-		return -ENXIO;
+		return -EIO;
 
 	spin_lock_irqsave(&ch->ch_lock, lock_flags);
 
@@ -3222,25 +3070,25 @@ static int dgap_set_modem_info(struct tty_struct *tty, unsigned int command,
 	struct board_t *bd;
 	struct channel_t *ch;
 	struct un_t *un;
-	int ret = -ENXIO;
-	unsigned int arg = 0;
+	int ret;
+	unsigned int arg;
 	ulong lock_flags;
 	ulong lock_flags2;
 
 	if (!tty || tty->magic != TTY_MAGIC)
-		return ret;
+		return -EIO;
 
 	un = tty->driver_data;
 	if (!un || un->magic != DGAP_UNIT_MAGIC)
-		return ret;
+		return -EIO;
 
 	ch = un->un_ch;
 	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
-		return ret;
+		return -EIO;
 
 	bd = ch->ch_bd;
 	if (!bd || bd->magic != DGAP_BOARD_MAGIC)
-		return ret;
+		return -EIO;
 
 	ret = get_user(arg, value);
 	if (ret)
@@ -3360,7 +3208,7 @@ static int dgap_tty_digiseta(struct tty_struct *tty,
 	struct channel_t *ch;
 	struct un_t *un;
 	struct digi_t new_digi;
-	ulong   lock_flags = 0;
+	ulong lock_flags = 0;
 	unsigned long lock_flags2;
 
 	if (!tty || tty->magic != TTY_MAGIC)
@@ -3640,8 +3488,8 @@ static void dgap_tty_throttle(struct tty_struct *tty)
 	struct board_t *bd;
 	struct channel_t *ch;
 	struct un_t *un;
-	ulong   lock_flags;
-	ulong   lock_flags2;
+	ulong lock_flags;
+	ulong lock_flags2;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return;
@@ -3676,8 +3524,8 @@ static void dgap_tty_unthrottle(struct tty_struct *tty)
 	struct board_t *bd;
 	struct channel_t *ch;
 	struct un_t *un;
-	ulong   lock_flags;
-	ulong   lock_flags2;
+	ulong lock_flags;
+	ulong lock_flags2;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return;
@@ -3712,8 +3560,8 @@ static void dgap_tty_start(struct tty_struct *tty)
 	struct board_t *bd;
 	struct channel_t *ch;
 	struct un_t *un;
-	ulong   lock_flags;
-	ulong   lock_flags2;
+	ulong lock_flags;
+	ulong lock_flags2;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return;
@@ -3737,7 +3585,6 @@ static void dgap_tty_start(struct tty_struct *tty)
 
 	spin_unlock_irqrestore(&ch->ch_lock, lock_flags2);
 	spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
-
 }
 
 static void dgap_tty_stop(struct tty_struct *tty)
@@ -3745,8 +3592,8 @@ static void dgap_tty_stop(struct tty_struct *tty)
 	struct board_t *bd;
 	struct channel_t *ch;
 	struct un_t *un;
-	ulong   lock_flags;
-	ulong   lock_flags2;
+	ulong lock_flags;
+	ulong lock_flags2;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return;
@@ -3770,7 +3617,6 @@ static void dgap_tty_stop(struct tty_struct *tty)
 
 	spin_unlock_irqrestore(&ch->ch_lock, lock_flags2);
 	spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
-
 }
 
 /*
@@ -3791,8 +3637,8 @@ static void dgap_tty_flush_chars(struct tty_struct *tty)
 	struct board_t *bd;
 	struct channel_t *ch;
 	struct un_t *un;
-	ulong   lock_flags;
-	ulong   lock_flags2;
+	ulong lock_flags;
+	ulong lock_flags2;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return;
@@ -3828,9 +3674,9 @@ static void dgap_tty_flush_buffer(struct tty_struct *tty)
 	struct board_t *bd;
 	struct channel_t *ch;
 	struct un_t *un;
-	ulong   lock_flags;
-	ulong   lock_flags2;
-	u16	head = 0;
+	ulong lock_flags;
+	ulong lock_flags2;
+	u16 head;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return;
@@ -3888,9 +3734,9 @@ static int dgap_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 	struct channel_t *ch;
 	struct un_t *un;
 	int rc;
-	u16	head = 0;
-	ulong   lock_flags = 0;
-	ulong   lock_flags2 = 0;
+	u16 head;
+	ulong lock_flags = 0;
+	ulong lock_flags2 = 0;
 	void __user *uarg = (void __user *) arg;
 
 	if (!tty || tty->magic != TTY_MAGIC)
@@ -4266,18 +4112,18 @@ static int dgap_after_config_loaded(int board)
 	/*
 	 * Initialize KME waitqueues...
 	 */
-	init_waitqueue_head(&(dgap_Board[board]->kme_wait));
+	init_waitqueue_head(&(dgap_board[board]->kme_wait));
 
 	/*
 	 * allocate flip buffer for board.
 	 */
-	dgap_Board[board]->flipbuf = kmalloc(MYFLIPLEN, GFP_ATOMIC);
-	if (!dgap_Board[board]->flipbuf)
+	dgap_board[board]->flipbuf = kmalloc(MYFLIPLEN, GFP_KERNEL);
+	if (!dgap_board[board]->flipbuf)
 		return -ENOMEM;
 
-	dgap_Board[board]->flipflagbuf = kmalloc(MYFLIPLEN, GFP_ATOMIC);
-	if (!dgap_Board[board]->flipflagbuf) {
-		kfree(dgap_Board[board]->flipbuf);
+	dgap_board[board]->flipflagbuf = kmalloc(MYFLIPLEN, GFP_KERNEL);
+	if (!dgap_board[board]->flipflagbuf) {
+		kfree(dgap_board[board]->flipbuf);
 		return -ENOMEM;
 	}
 
@@ -4292,36 +4138,37 @@ static int dgap_tty_register_ports(struct board_t *brd)
 	struct channel_t *ch;
 	int i;
 
-	brd->SerialPorts = kcalloc(brd->nasync, sizeof(*brd->SerialPorts),
+	brd->serial_ports = kcalloc(brd->nasync, sizeof(*brd->serial_ports),
 					GFP_KERNEL);
-	if (brd->SerialPorts == NULL)
+	if (!brd->serial_ports)
 		return -ENOMEM;
-	for (i = 0; i < brd->nasync; i++)
-		tty_port_init(&brd->SerialPorts[i]);
 
-	brd->PrinterPorts = kcalloc(brd->nasync, sizeof(*brd->PrinterPorts),
+	brd->printer_ports = kcalloc(brd->nasync, sizeof(*brd->printer_ports),
 					GFP_KERNEL);
-	if (brd->PrinterPorts == NULL) {
-		kfree(brd->SerialPorts);
+	if (!brd->printer_ports) {
+		kfree(brd->serial_ports);
 		return -ENOMEM;
 	}
-	for (i = 0; i < brd->nasync; i++)
-		tty_port_init(&brd->PrinterPorts[i]);
+
+	for (i = 0; i < brd->nasync; i++) {
+		tty_port_init(&brd->serial_ports[i]);
+		tty_port_init(&brd->printer_ports[i]);
+	}
 
 	ch = brd->channels[0];
 	for (i = 0; i < brd->nasync; i++, ch = brd->channels[i]) {
 
 		struct device *classp;
 
-		classp = tty_port_register_device(&brd->SerialPorts[i],
-					brd->SerialDriver,
+		classp = tty_port_register_device(&brd->serial_ports[i],
+					brd->serial_driver,
 					brd->firstminor + i, NULL);
 
 		dgap_create_tty_sysfs(&ch->ch_tun, classp);
 		ch->ch_tun.un_sysfs = classp;
 
-		classp = tty_port_register_device(&brd->PrinterPorts[i],
-					brd->PrintDriver,
+		classp = tty_port_register_device(&brd->printer_ports[i],
+					brd->print_driver,
 					brd->firstminor + i, NULL);
 
 		dgap_create_tty_sysfs(&ch->ch_pun, classp);
@@ -4336,9 +4183,9 @@ static int dgap_tty_register_ports(struct board_t *brd)
  * Copies the BIOS code from the user to the board,
  * and starts the BIOS running.
  */
-static void dgap_do_bios_load(struct board_t *brd, const uchar *ubios, int len)
+static void dgap_do_bios_load(struct board_t *brd, const u8 *ubios, int len)
 {
-	uchar *addr;
+	u8 __iomem *addr;
 	uint offset;
 	int i;
 
@@ -4369,16 +4216,15 @@ static void dgap_do_bios_load(struct board_t *brd, const uchar *ubios, int len)
 /*
  * Checks to see if the BIOS completed running on the card.
  */
-static int dgap_do_wait_for_bios(struct board_t *brd)
+static int dgap_test_bios(struct board_t *brd)
 {
-	uchar *addr;
+	u8 __iomem *addr;
 	u16 word;
 	u16 err1;
 	u16 err2;
-	int ret = 0;
 
 	if (!brd || (brd->magic != DGAP_BOARD_MAGIC) || !brd->re_map_membase)
-		return ret;
+		return -EINVAL;
 
 	addr = brd->re_map_membase;
 	word = readw(addr + POSTAREA);
@@ -4392,7 +4238,7 @@ static int dgap_do_wait_for_bios(struct board_t *brd)
 	while (brd->wait_for_bios < 1000) {
 		/* Check to see if BIOS thinks board is good. (GD). */
 		if (word == *(u16 *) "GD")
-			return 1;
+			return 0;
 		msleep_interruptible(10);
 		brd->wait_for_bios++;
 		word = readw(addr + POSTAREA);
@@ -4406,16 +4252,16 @@ static int dgap_do_wait_for_bios(struct board_t *brd)
 	brd->state = BOARD_FAILED;
 	brd->dpastatus = BD_NOBIOS;
 
-	return ret;
+	return -EIO;
 }
 
 /*
  * Copies the FEP code from the user to the board,
  * and starts the FEP running.
  */
-static void dgap_do_fep_load(struct board_t *brd, const uchar *ufep, int len)
+static void dgap_do_fep_load(struct board_t *brd, const u8 *ufep, int len)
 {
-	uchar *addr;
+	u8 __iomem *addr;
 	uint offset;
 
 	if (!brd || (brd->magic != DGAP_BOARD_MAGIC) || !brd->re_map_membase)
@@ -4434,8 +4280,9 @@ static void dgap_do_fep_load(struct board_t *brd, const uchar *ufep, int len)
 	 * it its config string describing how the concentrators look.
 	 */
 	if ((brd->type == PCX) || (brd->type == PEPC)) {
-		uchar string[100];
-		uchar *config, *xconfig;
+		u8 string[100];
+		u8 __iomem *config;
+		u8 *xconfig;
 		int i = 0;
 
 		xconfig = dgap_create_config_string(brd, string);
@@ -4457,16 +4304,15 @@ static void dgap_do_fep_load(struct board_t *brd, const uchar *ufep, int len)
 /*
  * Waits for the FEP to report thats its ready for us to use.
  */
-static int dgap_do_wait_for_fep(struct board_t *brd)
+static int dgap_test_fep(struct board_t *brd)
 {
-	uchar *addr;
+	u8 __iomem *addr;
 	u16 word;
 	u16 err1;
 	u16 err2;
-	int ret = 0;
 
 	if (!brd || (brd->magic != DGAP_BOARD_MAGIC) || !brd->re_map_membase)
-		return ret;
+		return -EINVAL;
 
 	addr = brd->re_map_membase;
 	word = readw(addr + FEPSTAT);
@@ -4486,7 +4332,7 @@ static int dgap_do_wait_for_fep(struct board_t *brd)
 			if (word == *(u16 *) "5A")
 				brd->bd_flags |= BD_FEP5PLUS;
 
-			return 1;
+			return 0;
 		}
 		msleep_interruptible(10);
 		brd->wait_for_fep++;
@@ -4501,7 +4347,7 @@ static int dgap_do_wait_for_fep(struct board_t *brd)
 	brd->state = BOARD_FAILED;
 	brd->dpastatus = BD_NOFEP;
 
-	return ret;
+	return -EIO;
 }
 
 /*
@@ -4509,10 +4355,10 @@ static int dgap_do_wait_for_fep(struct board_t *brd)
  */
 static void dgap_do_reset_board(struct board_t *brd)
 {
-	uchar check;
+	u8 check;
 	u32 check1;
 	u32 check2;
-	int i = 0;
+	int i;
 
 	if (!brd || (brd->magic != DGAP_BOARD_MAGIC) ||
 	    !brd->re_map_membase || !brd->re_map_port)
@@ -4550,17 +4396,16 @@ static void dgap_do_reset_board(struct board_t *brd)
 		brd->dpastatus = BD_NOFEP;
 		return;
 	}
-
 }
 
 #ifdef DIGI_CONCENTRATORS_SUPPORTED
 /*
  * Sends a concentrator image into the FEP5 board.
  */
-static void dgap_do_conc_load(struct board_t *brd, uchar *uaddr, int len)
+static void dgap_do_conc_load(struct board_t *brd, u8 *uaddr, int len)
 {
-	char *vaddr;
-	u16 offset = 0;
+	char __iomem *vaddr;
+	u16 offset;
 	struct downld_t *to_dp;
 
 	if (!brd || (brd->magic != DGAP_BOARD_MAGIC) || !brd->re_map_membase)
@@ -4590,8 +4435,8 @@ static void dgap_get_vpd(struct board_t *brd)
 	u16 vpd_offset;
 	u16 image_length;
 	u16 i;
-	uchar byte1;
-	uchar byte2;
+	u8 byte1;
+	u8 byte2;
 
 	/*
 	 * Poke the magic number at the PCI Rom Address location.
@@ -4696,8 +4541,8 @@ static void dgap_get_vpd(struct board_t *brd)
 static void dgap_poll_tasklet(unsigned long data)
 {
 	struct board_t *bd = (struct board_t *) data;
-	ulong  lock_flags;
-	char *vaddr;
+	ulong lock_flags;
+	char __iomem *vaddr;
 	u16 head, tail;
 
 	if (!bd || (bd->magic != DGAP_BOARD_MAGIC))
@@ -4715,7 +4560,7 @@ static void dgap_poll_tasklet(unsigned long data)
 	 */
 	if (bd->state == BOARD_READY) {
 
-		struct ev_t *eaddr = NULL;
+		struct ev_t __iomem *eaddr;
 
 		if (!bd->re_map_membase) {
 			spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
@@ -4729,7 +4574,7 @@ static void dgap_poll_tasklet(unsigned long data)
 		if (!bd->nasync)
 			goto out;
 
-		eaddr = (struct ev_t *) (vaddr + EVBUF);
+		eaddr = (struct ev_t __iomem *) (vaddr + EVBUF);
 
 		/* Get our head and tail */
 		head = readw(&(eaddr->ev_head));
@@ -4770,15 +4615,15 @@ out:
  *                        in the cmd buffer before returning.
  *
  *=======================================================================*/
-static void dgap_cmdb(struct channel_t *ch, uchar cmd, uchar byte1,
-			uchar byte2, uint ncmds)
+static void dgap_cmdb(struct channel_t *ch, u8 cmd, u8 byte1,
+			u8 byte2, uint ncmds)
 {
-	char		*vaddr = NULL;
-	struct cm_t	*cm_addr = NULL;
-	uint		count;
-	uint		n;
-	u16		head;
-	u16		tail;
+	char __iomem *vaddr;
+	struct __iomem cm_t *cm_addr;
+	uint count;
+	uint n;
+	u16 head;
+	u16 tail;
 
 	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
 		return;
@@ -4798,7 +4643,7 @@ static void dgap_cmdb(struct channel_t *ch, uchar cmd, uchar byte1,
 	if (!vaddr)
 		return;
 
-	cm_addr = (struct cm_t *) (vaddr + CMDBUF);
+	cm_addr = (struct cm_t __iomem *) (vaddr + CMDBUF);
 	head = readw(&(cm_addr->cm_head));
 
 	/*
@@ -4812,10 +4657,10 @@ static void dgap_cmdb(struct channel_t *ch, uchar cmd, uchar byte1,
 	/*
 	 * Put the data in the circular command buffer.
 	 */
-	writeb(cmd, (char *) (vaddr + head + CMDSTART + 0));
-	writeb((uchar) ch->ch_portnum, (char *) (vaddr + head + CMDSTART + 1));
-	writeb(byte1, (char *) (vaddr + head + CMDSTART + 2));
-	writeb(byte2, (char *) (vaddr + head + CMDSTART + 3));
+	writeb(cmd, (vaddr + head + CMDSTART + 0));
+	writeb((u8) ch->ch_portnum, (vaddr + head + CMDSTART + 1));
+	writeb(byte1, (vaddr + head + CMDSTART + 2));
+	writeb(byte2, (vaddr + head + CMDSTART + 3));
 
 	head = (head + 4) & (CMDMAX - CMDSTART - 4);
 
@@ -4856,14 +4701,14 @@ static void dgap_cmdb(struct channel_t *ch, uchar cmd, uchar byte1,
  *                        in the cmd buffer before returning.
  *
  *=======================================================================*/
-static void dgap_cmdw(struct channel_t *ch, uchar cmd, u16 word, uint ncmds)
+static void dgap_cmdw(struct channel_t *ch, u8 cmd, u16 word, uint ncmds)
 {
-	char		*vaddr = NULL;
-	struct cm_t	*cm_addr = NULL;
-	uint		count;
-	uint		n;
-	u16		head;
-	u16		tail;
+	char __iomem *vaddr;
+	struct __iomem cm_t *cm_addr;
+	uint count;
+	uint n;
+	u16 head;
+	u16 tail;
 
 	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
 		return;
@@ -4882,7 +4727,7 @@ static void dgap_cmdw(struct channel_t *ch, uchar cmd, u16 word, uint ncmds)
 	if (!vaddr)
 		return;
 
-	cm_addr = (struct cm_t *) (vaddr + CMDBUF);
+	cm_addr = (struct cm_t __iomem *) (vaddr + CMDBUF);
 	head = readw(&(cm_addr->cm_head));
 
 	/*
@@ -4896,9 +4741,9 @@ static void dgap_cmdw(struct channel_t *ch, uchar cmd, u16 word, uint ncmds)
 	/*
 	 * Put the data in the circular command buffer.
 	 */
-	writeb(cmd, (char *) (vaddr + head + CMDSTART + 0));
-	writeb((uchar) ch->ch_portnum, (char *) (vaddr + head + CMDSTART + 1));
-	writew((u16) word, (char *) (vaddr + head + CMDSTART + 2));
+	writeb(cmd, (vaddr + head + CMDSTART + 0));
+	writeb((u8) ch->ch_portnum, (vaddr + head + CMDSTART + 1));
+	writew((u16) word, (vaddr + head + CMDSTART + 2));
 
 	head = (head + 4) & (CMDMAX - CMDSTART - 4);
 
@@ -4941,12 +4786,12 @@ static void dgap_cmdw(struct channel_t *ch, uchar cmd, u16 word, uint ncmds)
  *=======================================================================*/
 static void dgap_cmdw_ext(struct channel_t *ch, u16 cmd, u16 word, uint ncmds)
 {
-	char		*vaddr = NULL;
-	struct cm_t	*cm_addr = NULL;
-	uint		count;
-	uint		n;
-	u16		head;
-	u16		tail;
+	char __iomem *vaddr;
+	struct __iomem cm_t *cm_addr;
+	uint count;
+	uint n;
+	u16 head;
+	u16 tail;
 
 	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
 		return;
@@ -4965,7 +4810,7 @@ static void dgap_cmdw_ext(struct channel_t *ch, u16 cmd, u16 word, uint ncmds)
 	if (!vaddr)
 		return;
 
-	cm_addr = (struct cm_t *) (vaddr + CMDBUF);
+	cm_addr = (struct cm_t __iomem *) (vaddr + CMDBUF);
 	head = readw(&(cm_addr->cm_head));
 
 	/*
@@ -4981,19 +4826,19 @@ static void dgap_cmdw_ext(struct channel_t *ch, u16 cmd, u16 word, uint ncmds)
 	 */
 
 	/* Write an FF to tell the FEP that we want an extended command */
-	writeb((uchar) 0xff, (char *) (vaddr + head + CMDSTART + 0));
+	writeb((u8) 0xff, (vaddr + head + CMDSTART + 0));
 
-	writeb((uchar) ch->ch_portnum, (uchar *) (vaddr + head + CMDSTART + 1));
-	writew((u16) cmd, (char *) (vaddr + head + CMDSTART + 2));
+	writeb((u8) ch->ch_portnum, (vaddr + head + CMDSTART + 1));
+	writew((u16) cmd, (vaddr + head + CMDSTART + 2));
 
 	/*
 	 * If the second part of the command won't fit,
 	 * put it at the beginning of the circular buffer.
 	 */
 	if (((head + 4) >= ((CMDMAX - CMDSTART)) || (head & 03)))
-		writew((u16) word, (char *) (vaddr + CMDSTART));
+		writew((u16) word, (vaddr + CMDSTART));
 	else
-		writew((u16) word, (char *) (vaddr + head + CMDSTART + 4));
+		writew((u16) word, (vaddr + head + CMDSTART + 4));
 
 	head = (head + 8) & (CMDMAX - CMDSTART - 4);
 
@@ -5034,10 +4879,10 @@ static void dgap_cmdw_ext(struct channel_t *ch, u16 cmd, u16 word, uint ncmds)
  *=======================================================================*/
 static void dgap_wmove(struct channel_t *ch, char *buf, uint cnt)
 {
-	int    n;
-	char   *taddr;
-	struct bs_t    *bs;
-	u16    head;
+	int n;
+	char __iomem *taddr;
+	struct bs_t __iomem *bs;
+	u16 head;
 
 	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
 		return;
@@ -5088,9 +4933,9 @@ static void dgap_wmove(struct channel_t *ch, char *buf, uint cnt)
  */
 static uint dgap_get_custom_baud(struct channel_t *ch)
 {
-	uchar *vaddr;
-	ulong offset = 0;
-	uint value = 0;
+	u8 __iomem *vaddr;
+	ulong offset;
+	uint value;
 
 	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
 		return 0;
@@ -5110,8 +4955,8 @@ static uint dgap_get_custom_baud(struct channel_t *ch)
 	 * Go get from fep mem, what the fep
 	 * believes the custom baud rate is.
 	 */
-	offset = ((((*(unsigned short *)(vaddr + ECS_SEG)) << 4) +
-		(ch->ch_portnum * 0x28) + LINE_SPEED));
+	offset = (ioread16(vaddr + ECS_SEG) << 4) + (ch->ch_portnum * 0x28)
+	       + LINE_SPEED;
 
 	value = readw(vaddr + offset);
 	return value;
@@ -5155,32 +5000,32 @@ static int dgap_param(struct tty_struct *tty)
 	struct ktermios *ts;
 	struct board_t *bd;
 	struct channel_t *ch;
-	struct bs_t   *bs;
-	struct un_t   *un;
-	u16	head;
-	u16	cflag;
-	u16	iflag;
-	uchar	mval;
-	uchar	hflow;
+	struct bs_t __iomem *bs;
+	struct un_t *un;
+	u16 head;
+	u16 cflag;
+	u16 iflag;
+	u8 mval;
+	u8 hflow;
 
 	if (!tty || tty->magic != TTY_MAGIC)
-		return -ENXIO;
+		return -EIO;
 
 	un = (struct un_t *) tty->driver_data;
 	if (!un || un->magic != DGAP_UNIT_MAGIC)
-		return -ENXIO;
+		return -EIO;
 
 	ch = un->un_ch;
 	if (!ch || ch->magic != DGAP_CHANNEL_MAGIC)
-		return -ENXIO;
+		return -EIO;
 
 	bd = ch->ch_bd;
 	if (!bd || bd->magic != DGAP_BOARD_MAGIC)
-		return -ENXIO;
+		return -EIO;
 
 	bs = ch->ch_bs;
 	if (!bs)
-		return -ENXIO;
+		return -EIO;
 
 	ts = &tty->termios;
 
@@ -5427,10 +5272,9 @@ static int dgap_param(struct tty_struct *tty)
 		ch->ch_hflow = hflow;
 
 		/* Okay to have channel and board locks held calling this */
-		dgap_cmdb(ch, SHFLOW, (uchar) hflow, 0xff, 0);
+		dgap_cmdb(ch, SHFLOW, (u8) hflow, 0xff, 0);
 	}
 
-
 	/*
 	 * Set RTS and/or DTR Toggle if needed,
 	 * but only if product is FEP5+ based.
@@ -5455,7 +5299,7 @@ static int dgap_param(struct tty_struct *tty)
 		ch->ch_mostat = mval;
 
 		/* Okay to have channel and board locks held calling this */
-		dgap_cmdb(ch, SMODEM, (uchar) mval, D_RTS(ch)|D_DTR(ch), 0);
+		dgap_cmdb(ch, SMODEM, (u8) mval, D_RTS(ch)|D_DTR(ch), 0);
 	}
 
 	/*
@@ -5606,21 +5450,21 @@ static void dgap_write_wakeup(struct board_t *bd, struct channel_t *ch,
 static int dgap_event(struct board_t *bd)
 {
 	struct channel_t *ch;
-	ulong		lock_flags;
-	ulong		lock_flags2;
-	struct bs_t	*bs;
-	uchar		*event;
-	uchar		*vaddr = NULL;
-	struct ev_t	*eaddr = NULL;
-	uint		head;
-	uint		tail;
-	int		port;
-	int		reason;
-	int		modem;
-	int		b1;
+	ulong lock_flags;
+	ulong lock_flags2;
+	struct bs_t __iomem *bs;
+	u8 __iomem *event;
+	u8 __iomem *vaddr;
+	struct ev_t __iomem *eaddr;
+	uint head;
+	uint tail;
+	int port;
+	int reason;
+	int modem;
+	int b1;
 
 	if (!bd || bd->magic != DGAP_BOARD_MAGIC)
-		return -ENXIO;
+		return -EIO;
 
 	spin_lock_irqsave(&bd->bd_lock, lock_flags);
 
@@ -5628,10 +5472,10 @@ static int dgap_event(struct board_t *bd)
 
 	if (!vaddr) {
 		spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
-		return -ENXIO;
+		return -EIO;
 	}
 
-	eaddr = (struct ev_t *) (vaddr + EVBUF);
+	eaddr = (struct ev_t __iomem *) (vaddr + EVBUF);
 
 	/* Get our head and tail */
 	head = readw(&(eaddr->ev_head));
@@ -5645,7 +5489,7 @@ static int dgap_event(struct board_t *bd)
 	    (head | tail) & 03) {
 		/* Let go of board lock */
 		spin_unlock_irqrestore(&bd->bd_lock, lock_flags);
-		return -ENXIO;
+		return -EIO;
 	}
 
 	/*
@@ -5659,10 +5503,10 @@ static int dgap_event(struct board_t *bd)
 
 		event = bd->re_map_membase + tail + EVSTART;
 
-		port   = event[0];
-		reason = event[1];
-		modem  = event[2];
-		b1     = event[3];
+		port   = ioread8(event);
+		reason = ioread8(event + 1);
+		modem  = ioread8(event + 2);
+		b1     = ioread8(event + 3);
 
 		/*
 		 * Make sure the interrupt is valid.
@@ -5795,7 +5639,7 @@ static DRIVER_ATTR(version, S_IRUSR, dgap_driver_version_show, NULL);
 
 static ssize_t dgap_driver_boards_show(struct device_driver *ddp, char *buf)
 {
-	return snprintf(buf, PAGE_SIZE, "%d\n", dgap_NumBoards);
+	return snprintf(buf, PAGE_SIZE, "%d\n", dgap_numboards);
 }
 static DRIVER_ATTR(boards, S_IRUSR, dgap_driver_boards_show, NULL);
 
@@ -5873,7 +5717,7 @@ static ssize_t dgap_ports_state_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i = 0;
+	int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -5894,7 +5738,7 @@ static ssize_t dgap_ports_baud_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i = 0;
+	int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -5915,7 +5759,7 @@ static ssize_t dgap_ports_msignals_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i = 0;
+	int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -5952,7 +5796,7 @@ static ssize_t dgap_ports_iflag_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i = 0;
+	int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -5972,7 +5816,7 @@ static ssize_t dgap_ports_cflag_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i = 0;
+	int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -5992,7 +5836,7 @@ static ssize_t dgap_ports_oflag_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i = 0;
+	int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -6012,7 +5856,7 @@ static ssize_t dgap_ports_lflag_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i = 0;
+	int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -6032,7 +5876,7 @@ static ssize_t dgap_ports_digi_flag_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i = 0;
+	int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -6052,7 +5896,7 @@ static ssize_t dgap_ports_rxcount_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i = 0;
+	int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -6072,7 +5916,7 @@ static ssize_t dgap_ports_txcount_show(struct device *p,
 {
 	struct board_t *bd;
 	int count = 0;
-	int i = 0;
+	int i;
 
 	bd = dgap_verify_board(p);
 	if (!bd)
@@ -6396,13 +6240,13 @@ static ssize_t dgap_tty_name_show(struct device *d,
 	struct board_t *bd;
 	struct channel_t *ch;
 	struct un_t *un;
-	int	cn;
-	int	bn;
-	struct cnode *cptr = NULL;
+	int cn;
+	int bn;
+	struct cnode *cptr;
 	int found = FALSE;
 	int ncount = 0;
 	int starto = 0;
-	int i = 0;
+	int i;
 
 	if (!d)
 		return 0;
@@ -6494,7 +6338,6 @@ static ssize_t dgap_tty_name_show(struct device *d,
 
 	return snprintf(buf, PAGE_SIZE, "%s_dgap_%d_%d\n",
 		(un->un_type == DGAP_PRINT) ? "pr" : "tty", bn, cn);
-
 }
 static DEVICE_ATTR(custom_name, S_IRUSR, dgap_tty_name_show, NULL);
 
@@ -6538,18 +6381,18 @@ static void dgap_remove_tty_sysfs(struct device *c)
 /*
  * Parse a configuration file read into memory as a string.
  */
-static int	dgap_parsefile(char **in, int Remove)
+static int dgap_parsefile(char **in, int remove)
 {
 	struct cnode *p, *brd, *line, *conc;
-	int	rc;
-	char	*s = NULL;
-	int	linecnt = 0;
+	int rc;
+	char *s;
+	int linecnt = 0;
 
 	p = &dgap_head;
 	brd = line = conc = NULL;
 
 	/* perhaps we are adding to an existing list? */
-	while (p->next != NULL)
+	while (p->next)
 		p = p->next;
 
 	/* file must start with a BEGIN */
@@ -6589,7 +6432,7 @@ static int	dgap_parsefile(char **in, int Remove)
 			}
 			p = p->next;
 
-			p->u.board.status = dgap_savestring("No");
+			p->u.board.status = kstrdup("No", GFP_KERNEL);
 			line = conc = NULL;
 			brd = p;
 			linecnt = -1;
@@ -6683,11 +6526,11 @@ static int	dgap_parsefile(char **in, int Remove)
 				return -1;
 			}
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
-			p->u.board.portstr = dgap_savestring(s);
+			p->u.board.portstr = kstrdup(s, GFP_KERNEL);
 			if (kstrtol(s, 0, &p->u.board.port)) {
 				dgap_err("bad number for IO port");
 				return -1;
@@ -6701,11 +6544,11 @@ static int	dgap_parsefile(char **in, int Remove)
 				return -1;
 			}
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
-			p->u.board.addrstr = dgap_savestring(s);
+			p->u.board.addrstr = kstrdup(s, GFP_KERNEL);
 			if (kstrtoul(s, 0, &p->u.board.addr)) {
 				dgap_err("bad number for memory address");
 				return -1;
@@ -6719,22 +6562,22 @@ static int	dgap_parsefile(char **in, int Remove)
 				return -1;
 			}
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
-			p->u.board.pcibusstr = dgap_savestring(s);
+			p->u.board.pcibusstr = kstrdup(s, GFP_KERNEL);
 			if (kstrtoul(s, 0, &p->u.board.pcibus)) {
 				dgap_err("bad number for pci bus");
 				return -1;
 			}
 			p->u.board.v_pcibus = 1;
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
-			p->u.board.pcislotstr = dgap_savestring(s);
+			p->u.board.pcislotstr = kstrdup(s, GFP_KERNEL);
 			if (kstrtoul(s, 0, &p->u.board.pcislot)) {
 				dgap_err("bad number for pci slot");
 				return -1;
@@ -6748,11 +6591,11 @@ static int	dgap_parsefile(char **in, int Remove)
 				return -1;
 			}
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
-			p->u.board.method = dgap_savestring(s);
+			p->u.board.method = kstrdup(s, GFP_KERNEL);
 			p->u.board.v_method = 1;
 			break;
 
@@ -6762,17 +6605,17 @@ static int	dgap_parsefile(char **in, int Remove)
 				return -1;
 			}
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
-			p->u.board.status = dgap_savestring(s);
+			p->u.board.status = kstrdup(s, GFP_KERNEL);
 			break;
 
 		case NPORTS:	/* number of ports */
 			if (p->type == BNODE) {
 				s = dgap_getword(in);
-				if (s == NULL) {
+				if (!s) {
 					dgap_err("unexpected end of file");
 					return -1;
 				}
@@ -6783,7 +6626,7 @@ static int	dgap_parsefile(char **in, int Remove)
 				p->u.board.v_nport = 1;
 			} else if (p->type == CNODE) {
 				s = dgap_getword(in);
-				if (s == NULL) {
+				if (!s) {
 					dgap_err("unexpected end of file");
 					return -1;
 				}
@@ -6794,7 +6637,7 @@ static int	dgap_parsefile(char **in, int Remove)
 				p->u.conc.v_nport = 1;
 			} else if (p->type == MNODE) {
 				s = dgap_getword(in);
-				if (s == NULL) {
+				if (!s) {
 					dgap_err("unexpected end of file");
 					return -1;
 				}
@@ -6811,18 +6654,18 @@ static int	dgap_parsefile(char **in, int Remove)
 
 		case ID:	/* letter ID used in tty name */
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
 
-			p->u.board.status = dgap_savestring(s);
+			p->u.board.status = kstrdup(s, GFP_KERNEL);
 
 			if (p->type == CNODE) {
-				p->u.conc.id = dgap_savestring(s);
+				p->u.conc.id = kstrdup(s, GFP_KERNEL);
 				p->u.conc.v_id = 1;
 			} else if (p->type == MNODE) {
-				p->u.module.id = dgap_savestring(s);
+				p->u.module.id = kstrdup(s, GFP_KERNEL);
 				p->u.module.v_id = 1;
 			} else {
 				dgap_err("id only valid for concentrators or modules");
@@ -6833,7 +6676,7 @@ static int	dgap_parsefile(char **in, int Remove)
 		case STARTO:	/* start offset of ID */
 			if (p->type == BNODE) {
 				s = dgap_getword(in);
-				if (s == NULL) {
+				if (!s) {
 					dgap_err("unexpected end of file");
 					return -1;
 				}
@@ -6844,7 +6687,7 @@ static int	dgap_parsefile(char **in, int Remove)
 				p->u.board.v_start = 1;
 			} else if (p->type == CNODE) {
 				s = dgap_getword(in);
-				if (s == NULL) {
+				if (!s) {
 					dgap_err("unexpected end of file");
 					return -1;
 				}
@@ -6855,7 +6698,7 @@ static int	dgap_parsefile(char **in, int Remove)
 				p->u.conc.v_start = 1;
 			} else if (p->type == MNODE) {
 				s = dgap_getword(in);
-				if (s == NULL) {
+				if (!s) {
 					dgap_err("unexpected end of file");
 					return -1;
 				}
@@ -6884,7 +6727,7 @@ static int	dgap_parsefile(char **in, int Remove)
 				dgap_err("unexpeced end of file");
 				return -1;
 			}
-			p->u.ttyname = dgap_savestring(s);
+			p->u.ttyname = kstrdup(s, GFP_KERNEL);
 			if (!p->u.ttyname) {
 				dgap_err("out of memory");
 				return -1;
@@ -6905,7 +6748,7 @@ static int	dgap_parsefile(char **in, int Remove)
 				dgap_err("unexpeced end of file");
 				return -1;
 			}
-			p->u.cuname = dgap_savestring(s);
+			p->u.cuname = kstrdup(s, GFP_KERNEL);
 			if (!p->u.cuname) {
 				dgap_err("out of memory");
 				return -1;
@@ -6915,7 +6758,7 @@ static int	dgap_parsefile(char **in, int Remove)
 		case LINE:	/* line information */
 			if (dgap_checknode(p))
 				return -1;
-			if (brd == NULL) {
+			if (!brd) {
 				dgap_err("must specify board before line info");
 				return -1;
 			}
@@ -6938,7 +6781,7 @@ static int	dgap_parsefile(char **in, int Remove)
 		case CONC:	/* concentrator information */
 			if (dgap_checknode(p))
 				return -1;
-			if (line == NULL) {
+			if (!line) {
 				dgap_err("must specify line info before concentrator");
 				return -1;
 			}
@@ -6977,7 +6820,7 @@ static int	dgap_parsefile(char **in, int Remove)
 		case MOD:	/* EBI module */
 			if (dgap_checknode(p))
 				return -1;
-			if (brd == NULL) {
+			if (!brd) {
 				dgap_err("must specify board info before EBI modules");
 				return -1;
 			}
@@ -6986,7 +6829,7 @@ static int	dgap_parsefile(char **in, int Remove)
 				linecnt = 0;
 				break;
 			default:
-				if (conc == NULL) {
+				if (!conc) {
 					dgap_err("must specify concentrator info before EBI module");
 					return -1;
 				}
@@ -7029,7 +6872,7 @@ static int	dgap_parsefile(char **in, int Remove)
 					dgap_err("unexpected end of file");
 					return -1;
 				}
-				p->u.line.cable = dgap_savestring(s);
+				p->u.line.cable = kstrdup(s, GFP_KERNEL);
 				p->u.line.v_cable = 1;
 			}
 			break;
@@ -7037,7 +6880,7 @@ static int	dgap_parsefile(char **in, int Remove)
 		case SPEED:	/* sync line speed indication */
 			if (p->type == LNODE) {
 				s = dgap_getword(in);
-				if (s == NULL) {
+				if (!s) {
 					dgap_err("unexpected end of file");
 					return -1;
 				}
@@ -7048,7 +6891,7 @@ static int	dgap_parsefile(char **in, int Remove)
 				p->u.line.v_speed = 1;
 			} else if (p->type == CNODE) {
 				s = dgap_getword(in);
-				if (s == NULL) {
+				if (!s) {
 					dgap_err("unexpected end of file");
 					return -1;
 				}
@@ -7070,7 +6913,7 @@ static int	dgap_parsefile(char **in, int Remove)
 					dgap_err("unexpected end of file");
 					return -1;
 				}
-				p->u.conc.connect = dgap_savestring(s);
+				p->u.conc.connect = kstrdup(s, GFP_KERNEL);
 				p->u.conc.v_connect = 1;
 			}
 			break;
@@ -7088,7 +6931,7 @@ static int	dgap_parsefile(char **in, int Remove)
 				dgap_err("unexpeced end of file");
 				return -1;
 			}
-			p->u.printname = dgap_savestring(s);
+			p->u.printname = kstrdup(s, GFP_KERNEL);
 			if (!p->u.printname) {
 				dgap_err("out of memory");
 				return -1;
@@ -7105,7 +6948,7 @@ static int	dgap_parsefile(char **in, int Remove)
 			}
 			p = p->next;
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
@@ -7125,7 +6968,7 @@ static int	dgap_parsefile(char **in, int Remove)
 			}
 			p = p->next;
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
@@ -7145,7 +6988,7 @@ static int	dgap_parsefile(char **in, int Remove)
 			}
 			p = p->next;
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
@@ -7165,7 +7008,7 @@ static int	dgap_parsefile(char **in, int Remove)
 			}
 			p = p->next;
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
@@ -7185,7 +7028,7 @@ static int	dgap_parsefile(char **in, int Remove)
 			}
 			p = p->next;
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
@@ -7205,7 +7048,7 @@ static int	dgap_parsefile(char **in, int Remove)
 			}
 			p = p->next;
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
@@ -7225,7 +7068,7 @@ static int	dgap_parsefile(char **in, int Remove)
 			}
 			p = p->next;
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
@@ -7245,7 +7088,7 @@ static int	dgap_parsefile(char **in, int Remove)
 			}
 			p = p->next;
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
@@ -7265,7 +7108,7 @@ static int	dgap_parsefile(char **in, int Remove)
 			}
 			p = p->next;
 			s = dgap_getword(in);
-			if (s == NULL) {
+			if (!s) {
 				dgap_err("unexpected end of file");
 				return -1;
 			}
@@ -7285,7 +7128,7 @@ static int	dgap_parsefile(char **in, int Remove)
  */
 static char *dgap_sindex(char *string, char *group)
 {
-	char    *ptr;
+	char *ptr;
 
 	if (!string || !group)
 		return (char *) NULL;
@@ -7317,7 +7160,7 @@ static char *dgap_sindex(char *string, char *group)
  */
 static int dgap_gettok(char **in, struct cnode *p)
 {
-	char	*w;
+	char *w;
 	struct toklist *t;
 
 	if (strstr(dgap_cword, "boar")) {
@@ -7386,8 +7229,8 @@ static struct cnode *dgap_newnode(int t)
 {
 	struct cnode *n;
 
-	n = kmalloc(sizeof(struct cnode), GFP_ATOMIC);
-	if (n != NULL) {
+	n = kmalloc(sizeof(struct cnode), GFP_KERNEL);
+	if (n) {
 		memset((char *)n, 0, sizeof(struct cnode));
 		n->type = t;
 	}
@@ -7454,37 +7297,21 @@ static int dgap_checknode(struct cnode *p)
 }
 
 /*
- * save a string somewhere
- */
-static char	*dgap_savestring(char *s)
-{
-	char	*p;
-
-	p = kmalloc(strlen(s) + 1, GFP_ATOMIC);
-	if (p)
-		strcpy(p, s);
-	return p;
-}
-
-/*
  * Given a board pointer, returns whether we should use interrupts or not.
  */
 static uint dgap_config_get_useintr(struct board_t *bd)
 {
-	struct cnode *p = NULL;
+	struct cnode *p;
 
 	if (!bd)
 		return 0;
 
 	for (p = bd->bd_config; p; p = p->next) {
-		switch (p->type) {
-		case INTRNODE:
+		if (p->type == INTRNODE) {
 			/*
 			 * check for pcxr types.
 			 */
 			return p->u.useintr;
-		default:
-			break;
 		}
 	}
 
@@ -7497,20 +7324,17 @@ static uint dgap_config_get_useintr(struct board_t *bd)
  */
 static uint dgap_config_get_altpin(struct board_t *bd)
 {
-	struct cnode *p = NULL;
+	struct cnode *p;
 
 	if (!bd)
 		return 0;
 
 	for (p = bd->bd_config; p; p = p->next) {
-		switch (p->type) {
-		case ANODE:
+		if (p->type == ANODE) {
 			/*
 			 * check for pcxr types.
 			 */
 			return p->u.altpin;
-		default:
-			break;
 		}
 	}
 
@@ -7524,59 +7348,61 @@ static uint dgap_config_get_altpin(struct board_t *bd)
  */
 static struct cnode *dgap_find_config(int type, int bus, int slot)
 {
-	struct cnode *p, *prev = NULL, *prev2 = NULL, *found = NULL;
+	struct cnode *p, *prev, *prev2, *found;
 
 	p = &dgap_head;
 
-	while (p->next != NULL) {
+	while (p->next) {
 		prev = p;
 		p = p->next;
 
-		if (p->type == BNODE) {
+		if (p->type != BNODE)
+			continue;
 
-			if (p->u.board.type == type) {
+		if (p->u.board.type != type)
+			continue;
 
-				if (p->u.board.v_pcibus &&
-				    p->u.board.pcibus != bus)
-					continue;
-				if (p->u.board.v_pcislot &&
-				    p->u.board.pcislot != slot)
-					continue;
+		if (p->u.board.v_pcibus &&
+		    p->u.board.pcibus != bus)
+			continue;
 
-				found = p;
-				/*
-				 * Keep walking thru the list till we
-				 * find the next board.
-				 */
-				while (p->next != NULL) {
-					prev2 = p;
-					p = p->next;
-					if (p->type == BNODE) {
-
-						/*
-						 * Mark the end of our 1 board
-						 * chain of configs.
-						 */
-						prev2->next = NULL;
-
-						/*
-						 * Link the "next" board to the
-						 * previous board, effectively
-						 * "unlinking" our board from
-						 * the main config.
-						 */
-						prev->next = p;
-
-						return found;
-					}
-				}
-				/*
-				 * It must be the last board in the list.
-				 */
-				prev->next = NULL;
-				return found;
-			}
+		if (p->u.board.v_pcislot &&
+		    p->u.board.pcislot != slot)
+			continue;
+
+		found = p;
+		/*
+		 * Keep walking thru the list till we
+		 * find the next board.
+		 */
+		while (p->next) {
+			prev2 = p;
+			p = p->next;
+
+			if (p->type != BNODE)
+				continue;
+
+			/*
+			 * Mark the end of our 1 board
+			 * chain of configs.
+			 */
+			prev2->next = NULL;
+
+			/*
+			 * Link the "next" board to the
+			 * previous board, effectively
+			 * "unlinking" our board from
+			 * the main config.
+			 */
+			prev->next = p;
+
+			return found;
 		}
+		/*
+		 * It must be the last board in the list.
+		 */
+		prev->next = NULL;
+		return found;
 	}
 	return NULL;
 }
@@ -7589,7 +7415,7 @@ static struct cnode *dgap_find_config(int type, int bus, int slot)
 static uint dgap_config_get_num_prts(struct board_t *bd)
 {
 	int count = 0;
-	struct cnode *p = NULL;
+	struct cnode *p;
 
 	if (!bd)
 		return 0;
@@ -7618,8 +7444,8 @@ static uint dgap_config_get_num_prts(struct board_t *bd)
 static char *dgap_create_config_string(struct board_t *bd, char *string)
 {
 	char *ptr = string;
-	struct cnode *p = NULL;
-	struct cnode *q = NULL;
+	struct cnode *p;
+	struct cnode *q;
 	int speed;
 
 	if (!bd) {
@@ -7645,13 +7471,11 @@ static char *dgap_create_config_string(struct board_t *bd, char *string)
 			 */
 			speed = p->u.conc.speed;
 			q = p->next;
-			if ((q != NULL) && (q->type == MNODE)) {
+			if (q && (q->type == MNODE)) {
 				*ptr = (p->u.conc.nport + 0x80);
 				ptr++;
 				p = q;
-				while ((q->next != NULL) &&
-				       (q->next->type) == MNODE) {
-
+				while (q->next && (q->next->type) == MNODE) {
 					*ptr = (q->u.module.nport + 0x80);
 					ptr++;
 					p = q;
diff --git a/drivers/staging/dgap/dgap.h b/drivers/staging/dgap/dgap.h
index 6b8f5f85832..03c020e35f8 100644
--- a/drivers/staging/dgap/dgap.h
+++ b/drivers/staging/dgap/dgap.h
@@ -39,9 +39,6 @@
 # define FALSE 0
 #endif
 
-/* Required for our shared headers! */
-typedef unsigned char		uchar;
-
 #if !defined(TTY_FLIPBUF_SIZE)
 # define TTY_FLIPBUF_SIZE 512
 #endif
@@ -61,7 +58,7 @@ typedef unsigned char		uchar;
 
 /*
  * defines from dgap_pci.h
- */ 
+ */
 #define PCIMAX 32			/* maximum number of PCI boards */
 
 #define DIGI_VID		0x114F
@@ -186,7 +183,7 @@ typedef unsigned char		uchar;
 			ECHOCTL | ECHOKE | IEXTEN)
 
 #ifndef _POSIX_VDISABLE
-#define   _POSIX_VDISABLE '\0'
+#define _POSIX_VDISABLE ('\0')
 #endif
 
 #define SNIFF_MAX	65536		/* Sniff buffer size (2^n) */
@@ -207,9 +204,11 @@ typedef unsigned char		uchar;
 #define EVSTART         0x0800L         /* Start of event buffer        */
 #define EVMAX           0x0c00L         /* End of event buffer          */
 #define FEP5_PLUS       0x0E40          /* ASCII '5' and ASCII 'A' is here  */
-#define ECS_SEG         0x0E44          /* Segment of the extended channel structure */
-#define LINE_SPEED      0x10            /* Offset into ECS_SEG for line speed   */
-                                        /* if the fep has extended capabilities */
+#define ECS_SEG         0x0E44          /* Segment of the extended      */
+					/* channel structure            */
+#define LINE_SPEED      0x10            /* Offset into ECS_SEG for line */
+					/* speed if the fep has extended */
+					/* capabilities                 */
 
 /* BIOS MAGIC SPOTS */
 #define ERROR           0x0C14L		/* BIOS error code              */
@@ -247,12 +246,12 @@ typedef unsigned char		uchar;
 
 #define FEPTIMEOUT 200000
 
-#define ENABLE_INTR		0x0e04		/* Enable interrupts flag */
-#define FEPPOLL_MIN		1		/* minimum of 1 millisecond */
-#define FEPPOLL_MAX		20		/* maximum of 20 milliseconds */
-#define FEPPOLL			0x0c26		/* Fep event poll interval */
+#define ENABLE_INTR	0x0e04		/* Enable interrupts flag */
+#define FEPPOLL_MIN	1		/* minimum of 1 millisecond */
+#define FEPPOLL_MAX	20		/* maximum of 20 milliseconds */
+#define FEPPOLL		0x0c26		/* Fep event poll interval */
 
-#define	IALTPIN			0x0080		/* Input flag to swap DSR <-> DCD */
+#define	IALTPIN		0x0080		/* Input flag to swap DSR <-> DCD */
 
 /************************************************************************
  * FEP supported functions
@@ -280,14 +279,12 @@ typedef unsigned char		uchar;
 #define SPINTFC		0xfc		/* Reserved			*/
 #define SCOMMODE	0xfd		/* Set RS232/422 mode		*/
 
-
 /************************************************************************
  *	Modes for SCOMMODE
  ************************************************************************/
 #define MODE_232	0x00
 #define MODE_422	0x01
 
-
 /************************************************************************
  *      Event flags.
  ************************************************************************/
@@ -362,13 +359,13 @@ typedef unsigned char		uchar;
 #define MC8E8K  36
 
 #define AVANFS	42	/* start of Avanstar family definitions */
-#define A8P 	42
+#define A8P	42
 #define A16P	43
 #define AVANFE	43	/* end of Avanstar family definitions */
 
-#define DA2000FS	44	/* start of AccelePort 2000 family definitions */
-#define DA22 		44 /* AccelePort 2002 */
-#define DA24 		45 /* AccelePort 2004 */
+#define DA2000FS	44 /* start of AccelePort 2000 family definitions */
+#define DA22		44 /* AccelePort 2002 */
+#define DA24		45 /* AccelePort 2004 */
 #define DA28		46 /* AccelePort 2008 */
 #define DA216		47 /* AccelePort 2016 */
 #define DAR4		48 /* AccelePort RAS 4 port */
@@ -492,8 +489,6 @@ enum {
 	REQUESTED_CONCENTRATOR
 };
 
-
-
 /*
  * Modem line constants are defined as macros because DSR and
  * DCD are swapable using the ditty altpin option.
@@ -505,14 +500,12 @@ enum {
 #define D_RI(ch)        DM_RI           /* Ring indicator       */
 #define D_DTR(ch)       DM_DTR          /* Data terminal ready  */
 
-
 /*************************************************************************
  *
  * Structures and closely related defines.
  *
  *************************************************************************/
 
-
 /*
  * A structure to hold a statistics counter.  We also
  * compute moving averages for this counter.
@@ -524,7 +517,6 @@ struct macounter {
 	ulong		ema;	/* Exponential moving average */
 };
 
-
 /************************************************************************
  * Device flag definitions for bd_flags.
  ************************************************************************/
@@ -546,11 +538,11 @@ struct board_t {
 	u16		device;		/* PCI device ID */
 	u16		subvendor;	/* PCI subsystem vendor ID */
 	u16		subdevice;	/* PCI subsystem device ID */
-	uchar		rev;		/* PCI revision ID */
+	u8		rev;		/* PCI revision ID */
 	uint		pci_bus;	/* PCI bus value */
 	uint		pci_slot;	/* PCI slot value */
 	u16		maxports;	/* MAX ports this board can handle */
-	uchar		vpd[VPDSIZE];	/* VPD of board, if found */
+	u8		vpd[VPDSIZE];	/* VPD of board, if found */
 	u32		bd_flags;	/* Board flags */
 
 	spinlock_t	bd_lock;	/* Used to protect board */
@@ -571,48 +563,53 @@ struct board_t {
 	ulong		irq;		/* Interrupt request number */
 	ulong		intr_count;	/* Count of interrupts */
 	u32		intr_used;	/* Non-zero if using interrupts */
-	u32		intr_running;	/* Non-zero if FEP knows its doing interrupts */
+	u32		intr_running;	/* Non-zero if FEP knows its doing */
+					/* interrupts */
 
 	ulong		port;		/* Start of base io port of the card */
 	ulong		port_end;	/* End of base io port of the card */
 	ulong		membase;	/* Start of base memory of the card */
 	ulong		membase_end;	/* End of base memory of the card */
 
-	uchar 		*re_map_port;	/* Remapped io port of the card */
-	uchar		*re_map_membase;/* Remapped memory of the card */
+	u8 __iomem	*re_map_port;	/* Remapped io port of the card */
+	u8 __iomem	*re_map_membase;/* Remapped memory of the card */
 
-	uchar		runwait;	/* # Processes waiting for FEP  */
-	uchar		inhibit_poller; /* Tells  the poller to leave us alone */
+	u8		runwait;	/* # Processes waiting for FEP  */
+	u8		inhibit_poller; /* Tells the poller to leave us alone */
 
-	struct channel_t *channels[MAXPORTS]; /* array of pointers to our channels. */
+	struct channel_t *channels[MAXPORTS]; /* array of pointers to our */
+					      /* channels.                */
 
-	struct tty_driver	*SerialDriver;
-	struct tty_port *SerialPorts;
-	char		SerialName[200];
-	struct tty_driver	*PrintDriver;
-	struct tty_port *PrinterPorts;
-	char		PrintName[200];
+	struct tty_driver	*serial_driver;
+	struct tty_port *serial_ports;
+	char		serial_name[200];
+	struct tty_driver	*print_driver;
+	struct tty_port *printer_ports;
+	char		print_name[200];
 
-	u32		dgap_Major_Serial_Registered;
-	u32		dgap_Major_TransparentPrint_Registered;
+	u32		dgap_major_serial_registered;
+	u32		dgap_major_transparent_print_registered;
 
-	u32		dgap_Serial_Major;
-	u32		dgap_TransparentPrint_Major;
+	u32		dgap_serial_major;
+	u32		dgap_transparent_print_major;
 
-	struct bs_t	*bd_bs;			/* Base structure pointer       */
+	struct bs_t __iomem *bd_bs;	/* Base structure pointer         */
 
-	char	*flipbuf;		/* Our flip buffer, alloced if board is found */
-	char	*flipflagbuf;		/* Our flip flag buffer, alloced if board is found */
+	char	*flipbuf;		/* Our flip buffer, alloced if    */
+					/* board is found                 */
+	char	*flipflagbuf;		/* Our flip flag buffer, alloced  */
+					/* if board is found              */
 
-	u16		dpatype;	/* The board "type", as defined by DPA */
-	u16		dpastatus;	/* The board "status", as defined by DPA */
-	wait_queue_head_t kme_wait;	/* Needed for DPA support */
+	u16		dpatype;	/* The board "type", as defined   */
+					/* by DPA                         */
+	u16		dpastatus;	/* The board "status", as defined */
+					/* by DPA                         */
+	wait_queue_head_t kme_wait;	/* Needed for DPA support         */
 
-	u32		conc_dl_status;	/* Status of any pending conc download */
+	u32		conc_dl_status;	/* Status of any pending conc     */
+					/* download                       */
 };
 
-
-
 /************************************************************************
  * Unit flag definitions for un_flags.
  ************************************************************************/
@@ -640,7 +637,7 @@ struct un_t {
 	struct	channel_t *un_ch;
 	u32	un_time;
 	u32	un_type;
-	u32	un_open_count;	/* Counter of opens to port		*/
+	int	un_open_count;	/* Counter of opens to port		*/
 	struct tty_struct *un_tty;/* Pointer to unit tty structure	*/
 	u32	un_flags;	/* Unit flags				*/
 	wait_queue_head_t un_flags_wait; /* Place to sleep to wait on unit */
@@ -650,7 +647,6 @@ struct un_t {
 	struct device *un_sysfs;
 };
 
-
 /************************************************************************
  * Device flag definitions for ch_flags.
  ************************************************************************/
@@ -677,7 +673,6 @@ struct un_t {
 #define SNIFF_WAIT_DATA	0x2
 #define SNIFF_WAIT_SPACE 0x4
 
-
 /************************************************************************
  ***	Definitions for Digi ditty(1) command.
  ************************************************************************/
@@ -689,8 +684,8 @@ struct un_t {
 
 #if !defined(TIOCMODG)
 
-#define	TIOCMODG	(('d'<<8) | 250)		/* get modem ctrl state	*/
-#define	TIOCMODS	(('d'<<8) | 251)		/* set modem ctrl state	*/
+#define	TIOCMODG	(('d'<<8) | 250)	/* get modem ctrl state	*/
+#define	TIOCMODS	(('d'<<8) | 251)	/* set modem ctrl state	*/
 
 #ifndef TIOCM_LE
 #define		TIOCM_LE	0x01		/* line enable		*/
@@ -709,16 +704,15 @@ struct un_t {
 #endif
 
 #if !defined(TIOCMSET)
-#define	TIOCMSET	(('d'<<8) | 252)		/* set modem ctrl state	*/
-#define	TIOCMGET	(('d'<<8) | 253)		/* set modem ctrl state	*/
+#define	TIOCMSET	(('d'<<8) | 252)	/* set modem ctrl state	*/
+#define	TIOCMGET	(('d'<<8) | 253)	/* set modem ctrl state	*/
 #endif
 
 #if !defined(TIOCMBIC)
-#define	TIOCMBIC	(('d'<<8) | 254)		/* set modem ctrl state */
-#define	TIOCMBIS	(('d'<<8) | 255)		/* set modem ctrl state */
+#define	TIOCMBIC	(('d'<<8) | 254)	/* set modem ctrl state */
+#define	TIOCMBIS	(('d'<<8) | 255)	/* set modem ctrl state */
 #endif
 
-
 #if !defined(TIOCSDTR)
 #define	TIOCSDTR	(('e'<<8) | 0)		/* set DTR		*/
 #define	TIOCCDTR	(('e'<<8) | 1)		/* clear DTR		*/
@@ -737,26 +731,25 @@ struct un_t {
 						/* Adapter Memory	*/
 
 #define	DIGI_GETFLOW	(('e'<<8) | 99)		/* Get startc/stopc flow */
-						/* control characters 	 */
-#define	DIGI_SETFLOW	(('e'<<8) | 100)		/* Set startc/stopc flow */
+						/* control characters    */
+#define	DIGI_SETFLOW	(('e'<<8) | 100)	/* Set startc/stopc flow */
 						/* control characters	 */
-#define	DIGI_GETAFLOW	(('e'<<8) | 101)		/* Get Aux. startc/stopc */
-						/* flow control chars 	 */
-#define	DIGI_SETAFLOW	(('e'<<8) | 102)		/* Set Aux. startc/stopc */
+#define	DIGI_GETAFLOW	(('e'<<8) | 101)	/* Get Aux. startc/stopc */
+						/* flow control chars    */
+#define	DIGI_SETAFLOW	(('e'<<8) | 102)	/* Set Aux. startc/stopc */
 						/* flow control chars	 */
 
-#define DIGI_GEDELAY	(('d'<<8) | 246)		/* Get edelay */
-#define DIGI_SEDELAY	(('d'<<8) | 247)		/* Set edelay */
+#define DIGI_GEDELAY	(('d'<<8) | 246)	/* Get edelay */
+#define DIGI_SEDELAY	(('d'<<8) | 247)	/* Set edelay */
 
 struct	digiflow_t {
-	unsigned char	startc;				/* flow cntl start char	*/
-	unsigned char	stopc;				/* flow cntl stop char	*/
+	unsigned char	startc;			/* flow cntl start char	*/
+	unsigned char	stopc;			/* flow cntl stop char	*/
 };
 
-
 #ifdef	FLOW_2200
-#define	F2200_GETA	(('e'<<8) | 104)		/* Get 2x36 flow cntl flags */
-#define	F2200_SETAW	(('e'<<8) | 105)		/* Set 2x36 flow cntl flags */
+#define	F2200_GETA	(('e'<<8) | 104)	/* Get 2x36 flow cntl flags */
+#define	F2200_SETAW	(('e'<<8) | 105)	/* Set 2x36 flow cntl flags */
 #define		F2200_MASK	0x03		/* 2200 flow cntl bit mask  */
 #define		FCNTL_2200	0x01		/* 2x36 terminal flow cntl  */
 #define		PCNTL_2200	0x02		/* 2x36 printer flow cntl   */
@@ -830,59 +823,6 @@ struct rw_t {
 	unsigned char	rw_data[128];	/* Data to read/write		*/
 };
 
-/***********************************************************************
- * Shrink Buffer and Board Information definitions and structures.
-
- ************************************************************************/
-			/* Board type return codes */
-#define	PCXI_TYPE 1     /* Board type at the designated port is a PC/Xi */
-#define PCXM_TYPE 2     /* Board type at the designated port is a PC/Xm */
-#define	PCXE_TYPE 3     /* Board type at the designated port is a PC/Xe */
-#define	MCXI_TYPE 4     /* Board type at the designated port is a MC/Xi */
-#define COMXI_TYPE 5     /* Board type at the designated port is a COM/Xi */
-
-			 /* Non-Zero Result codes. */
-#define RESULT_NOBDFND 1 /* A Digi product at that port is not config installed */
-#define RESULT_NODESCT 2 /* A memory descriptor was not obtainable */
-#define RESULT_NOOSSIG 3 /* FEP/OS signature was not detected on the board */
-#define RESULT_TOOSML  4 /* Too small an area to shrink.  */
-#define RESULT_NOCHAN  5 /* Channel structure for the board was not found */
-
-struct shrink_buf_struct {
-	unsigned long	shrink_buf_vaddr;	/* Virtual address of board */
-	unsigned long	shrink_buf_phys;	/* Physical address of board */
-	unsigned long	shrink_buf_bseg;	/* Amount of board memory */
-	unsigned long	shrink_buf_hseg;	/* '186 Beginning of Dual-Port */
-
-	unsigned long	shrink_buf_lseg;	/* '186 Beginning of freed memory						*/
-	unsigned long	shrink_buf_mseg;	/* Linear address from start of
-						   dual-port were freed memory
-						   begins, host viewpoint. */
-
-	unsigned long	shrink_buf_bdparam;	/* Parameter for xxmemon and
-						   xxmemoff */
-
-	unsigned long	shrink_buf_reserva;	/* Reserved */
-	unsigned long	shrink_buf_reservb;	/* Reserved */
-	unsigned long	shrink_buf_reservc;	/* Reserved */
-	unsigned long	shrink_buf_reservd;	/* Reserved */
-
-	unsigned char	shrink_buf_result;	/* Reason for call failing
-						   Zero is Good return */
-	unsigned char	shrink_buf_init;	/* Non-Zero if it caused an
-						   xxinit call. */
-
-	unsigned char	shrink_buf_anports;	/* Number of async ports  */
-	unsigned char	shrink_buf_snports; 	/* Number of sync  ports */
-	unsigned char	shrink_buf_type;	/* Board type 1 = PC/Xi,
-							      2 = PC/Xm,
-							      3 = PC/Xe
-							      4 = MC/Xi
-							      5 = COMX/i */
-	unsigned char	shrink_buf_card;	/* Card number */
-
-};
-
 /************************************************************************
  * Structure to get driver status information
  ************************************************************************/
@@ -892,7 +832,7 @@ struct digi_dinfo {
 	char		dinfo_version[16];	/* driver version       */
 };
 
-#define	DIGI_GETDD	(('d'<<8) | 248)		/* get driver info      */
+#define	DIGI_GETDD	(('d'<<8) | 248)	/* get driver info      */
 
 /************************************************************************
  * Structure used with ioctl commands for per-board information
@@ -912,7 +852,7 @@ struct digi_info {
 	char		info_reserved[7];	/* for future expansion    */
 };
 
-#define	DIGI_GETBD	(('d'<<8) | 249)		/* get board info          */
+#define	DIGI_GETBD	(('d'<<8) | 249)	/* get board info          */
 
 struct digi_stat {
 	unsigned int	info_chan;		/* Channel number (0 based)  */
@@ -927,7 +867,7 @@ struct digi_stat {
 	unsigned long	info_reserved[8];	/* for future expansion    */
 };
 
-#define	DIGI_GETSTAT	(('d'<<8) | 244)		/* get board info          */
+#define	DIGI_GETSTAT	(('d'<<8) | 244)	/* get board info          */
 /************************************************************************
  *
  * Structure used with ioctl commands for per-channel information
@@ -936,9 +876,9 @@ struct digi_stat {
 struct digi_ch {
 	unsigned long	info_bdnum;		/* Board number (0 based)  */
 	unsigned long	info_channel;		/* Channel index number    */
-	unsigned long	info_ch_cflag;		/* Channel cflag   	   */
-	unsigned long	info_ch_iflag;		/* Channel iflag   	   */
-	unsigned long	info_ch_oflag;		/* Channel oflag   	   */
+	unsigned long	info_ch_cflag;		/* Channel cflag           */
+	unsigned long	info_ch_iflag;		/* Channel iflag           */
+	unsigned long	info_ch_oflag;		/* Channel oflag           */
 	unsigned long	info_chsize;		/* Channel structure size  */
 	unsigned long	info_sleep_stat;	/* sleep status		   */
 	dev_t		info_dev;		/* device number	   */
@@ -970,7 +910,7 @@ struct digi_cmd {
 #define INFO_CH_WLOW	0x0020
 #define INFO_XXBUF_BUSY 0x0040
 
-#define	DIGI_GETCH	(('d'<<8) | 245)		/* get board info          */
+#define	DIGI_GETCH	(('d'<<8) | 245)	/* get board info          */
 
 /* Board type definitions */
 
@@ -1015,24 +955,25 @@ struct digi_cmd {
 #define BD_TRIBOOT	0x8
 #define	BD_BADKME	0x80
 
-#define DIGI_LOOPBACK	      (('d'<<8) | 252)		/* Enable/disable UART internal loopback */
-#define DIGI_SPOLL            (('d'<<8) | 254)		/* change poller rate   */
+#define DIGI_LOOPBACK	(('d'<<8) | 252)	/* Enable/disable UART  */
+						/* internal loopback    */
+#define DIGI_SPOLL	(('d'<<8) | 254)	/* change poller rate   */
 
-#define DIGI_SETCUSTOMBAUD	_IOW('e', 106, int)	/* Set integer baud rate */
-#define DIGI_GETCUSTOMBAUD	_IOR('e', 107, int)	/* Get integer baud rate */
-#define DIGI_RESET_PORT		(('e'<<8) | 93)		/* Reset port		*/
+#define DIGI_SETCUSTOMBAUD _IOW('e', 106, int)	/* Set integer baud rate */
+#define DIGI_GETCUSTOMBAUD _IOR('e', 107, int)	/* Get integer baud rate */
+#define DIGI_RESET_PORT	   (('e'<<8) | 93)	/* Reset port		 */
 
 /************************************************************************
  * Channel information structure.
  ************************************************************************/
 struct channel_t {
 	int magic;			/* Channel Magic Number		*/
-	struct bs_t	*ch_bs;		/* Base structure pointer       */
-	struct cm_t	*ch_cm;		/* Command queue pointer        */
+	struct bs_t __iomem *ch_bs;	/* Base structure pointer       */
+	struct cm_t __iomem *ch_cm;	/* Command queue pointer        */
 	struct board_t *ch_bd;		/* Board structure pointer      */
-	unsigned char *ch_vaddr;	/* FEP memory origin            */
-	unsigned char *ch_taddr;	/* Write buffer origin          */
-	unsigned char *ch_raddr;	/* Read buffer origin           */
+	u8 __iomem *ch_vaddr;		/* FEP memory origin            */
+	u8 __iomem *ch_taddr;		/* Write buffer origin          */
+	u8 __iomem *ch_raddr;		/* Read buffer origin           */
 	struct digi_t  ch_digi;		/* Transparent Print structure  */
 	struct un_t ch_tun;		/* Terminal unit info           */
 	struct un_t ch_pun;		/* Printer unit info            */
@@ -1041,14 +982,14 @@ struct channel_t {
 	wait_queue_head_t ch_flags_wait;
 
 	u32	pscan_state;
-	uchar	pscan_savechar;
+	u8	pscan_savechar;
 
 	u32 ch_portnum;			/* Port number, 0 offset.	*/
 	u32 ch_open_count;		/* open count			*/
 	u32	ch_flags;		/* Channel flags                */
 
-
-	u32	ch_close_delay;		/* How long we should drop RTS/DTR for */
+	u32	ch_close_delay;		/* How long we should drop      */
+					/* RTS/DTR for                  */
 
 	u32	ch_cpstime;		/* Time for CPS calculations    */
 
@@ -1057,7 +998,7 @@ struct channel_t {
 	tcflag_t ch_c_oflag;		/* channel oflags               */
 	tcflag_t ch_c_lflag;		/* channel lflags               */
 
-	u16  ch_fepiflag;            /* FEP tty iflags               */
+	u16  ch_fepiflag;		/* FEP tty iflags               */
 	u16  ch_fepcflag;		/* FEP tty cflags               */
 	u16  ch_fepoflag;		/* FEP tty oflags               */
 	u16  ch_wopen;			/* Waiting for open process cnt */
@@ -1071,75 +1012,69 @@ struct channel_t {
 
 	u16  ch_cook;			/* Output character mask        */
 
-	uchar   ch_card;		/* Card channel is on           */
-	uchar   ch_stopc;		/* Stop character               */
-	uchar   ch_startc;		/* Start character              */
-
-	uchar   ch_mostat;		/* FEP output modem status      */
-	uchar   ch_mistat;		/* FEP input modem status       */
-	uchar   ch_mforce;		/* Modem values to be forced    */
-	uchar   ch_mval;		/* Force values                 */
-	uchar   ch_fepstopc;		/* FEP stop character           */
-	uchar   ch_fepstartc;		/* FEP start character          */
-
-	uchar   ch_astopc;		/* Auxiliary Stop character     */
-	uchar   ch_astartc;		/* Auxiliary Start character    */
-	uchar   ch_fepastopc;		/* Auxiliary FEP stop char      */
-	uchar   ch_fepastartc;		/* Auxiliary FEP start char     */
-
-	uchar   ch_hflow;		/* FEP hardware handshake       */
-	uchar   ch_dsr;			/* stores real dsr value        */
-	uchar   ch_cd;			/* stores real cd value         */
-	uchar   ch_tx_win;		/* channel tx buffer window     */
-	uchar   ch_rx_win;		/* channel rx buffer window     */
+	u8   ch_card;			/* Card channel is on           */
+	u8   ch_stopc;			/* Stop character               */
+	u8   ch_startc;			/* Start character              */
+
+	u8   ch_mostat;			/* FEP output modem status      */
+	u8   ch_mistat;			/* FEP input modem status       */
+	u8   ch_mforce;			/* Modem values to be forced    */
+	u8   ch_mval;			/* Force values                 */
+	u8   ch_fepstopc;		/* FEP stop character           */
+	u8   ch_fepstartc;		/* FEP start character          */
+
+	u8   ch_astopc;			/* Auxiliary Stop character     */
+	u8   ch_astartc;		/* Auxiliary Start character    */
+	u8   ch_fepastopc;		/* Auxiliary FEP stop char      */
+	u8   ch_fepastartc;		/* Auxiliary FEP start char     */
+
+	u8   ch_hflow;			/* FEP hardware handshake       */
+	u8   ch_dsr;			/* stores real dsr value        */
+	u8   ch_cd;			/* stores real cd value         */
+	u8   ch_tx_win;			/* channel tx buffer window     */
+	u8   ch_rx_win;			/* channel rx buffer window     */
 	uint	ch_custom_speed;	/* Custom baud, if set		*/
-	uint	ch_baud_info;		/* Current baud info for /proc output	*/
-	ulong	ch_rxcount;		/* total of data received so far	*/
-	ulong	ch_txcount;		/* total of data transmitted so far	*/
-	ulong	ch_err_parity;		/* Count of parity errors on channel	*/
-	ulong	ch_err_frame;		/* Count of framing errors on channel	*/
+	uint	ch_baud_info;		/* Current baud info for /proc output */
+	ulong	ch_rxcount;		/* total of data received so far      */
+	ulong	ch_txcount;		/* total of data transmitted so far   */
+	ulong	ch_err_parity;		/* Count of parity errors on channel  */
+	ulong	ch_err_frame;		/* Count of framing errors on channel */
 	ulong	ch_err_break;		/* Count of breaks on channel	*/
 	ulong	ch_err_overrun;		/* Count of overruns on channel	*/
-
-	uint ch_sniff_in;
-	uint ch_sniff_out;
-	char *ch_sniff_buf;		/* Sniff buffer for proc */
-	ulong ch_sniff_flags;		/* Channel flags                */
-	wait_queue_head_t ch_sniff_wait;
 };
 
 /************************************************************************
  * Command structure definition.
  ************************************************************************/
 struct cm_t {
-	volatile unsigned short cm_head;	/* Command buffer head offset	*/
-	volatile unsigned short cm_tail;	/* Command buffer tail offset	*/
-	volatile unsigned short cm_start;	/* start offset of buffer	*/
-	volatile unsigned short cm_max;		/* last offset of buffer	*/
+	unsigned short cm_head;		/* Command buffer head offset */
+	unsigned short cm_tail;		/* Command buffer tail offset */
+	unsigned short cm_start;	/* start offset of buffer     */
+	unsigned short cm_max;		/* last offset of buffer      */
 };
 
 /************************************************************************
  * Event structure definition.
  ************************************************************************/
 struct ev_t {
-	volatile unsigned short ev_head;	/* Command buffer head offset	*/
-	volatile unsigned short ev_tail;	/* Command buffer tail offset	*/
-	volatile unsigned short ev_start;	/* start offset of buffer	*/
-	volatile unsigned short ev_max;		/* last offset of buffer	*/
+	unsigned short ev_head;		/* Command buffer head offset */
+	unsigned short ev_tail;		/* Command buffer tail offset */
+	unsigned short ev_start;	/* start offset of buffer     */
+	unsigned short ev_max;		/* last offset of buffer      */
 };
 
 /************************************************************************
  * Download buffer structure.
  ************************************************************************/
 struct downld_t {
-	uchar	dl_type;		/* Header                       */
-	uchar	dl_seq;			/* Download sequence            */
+	u8	dl_type;		/* Header                       */
+	u8	dl_seq;			/* Download sequence            */
 	ushort	dl_srev;		/* Software revision number     */
 	ushort	dl_lrev;		/* Low revision number          */
 	ushort	dl_hrev;		/* High revision number         */
 	ushort	dl_seg;			/* Start segment address        */
 	ushort	dl_size;		/* Number of bytes to download  */
-	uchar	dl_data[1024];		/* Download data                */
+	u8	dl_data[1024];		/* Download data                */
 };
 
 /************************************************************************
@@ -1152,70 +1087,74 @@ struct downld_t {
  *        U = unknown (may be changed w/o notice)                       *
  ************************************************************************/
 struct bs_t {
-	volatile unsigned short  tp_jmp;	/* Transmit poll jump		 */
-	volatile unsigned short  tc_jmp;	/* Cooked procedure jump	 */
-	volatile unsigned short  ri_jmp;	/* Not currently used		 */
-	volatile unsigned short  rp_jmp;	/* Receive poll jump		 */
-
-	volatile unsigned short  tx_seg;	/* W  Tx segment	 */
-	volatile unsigned short  tx_head;	/* W  Tx buffer head offset	*/
-	volatile unsigned short  tx_tail;	/* R  Tx buffer tail offset	*/
-	volatile unsigned short  tx_max;	/* W  Tx buffer size - 1	 */
-
-	volatile unsigned short  rx_seg;	/* W  Rx segment		*/
-	volatile unsigned short  rx_head;	/* W  Rx buffer head offset	*/
-	volatile unsigned short  rx_tail;	/* R  Rx buffer tail offset	*/
-	volatile unsigned short  rx_max;	/* W  Rx buffer size - 1	 */
-
-	volatile unsigned short  tx_lw;		/* W  Tx buffer low water mark  */
-	volatile unsigned short  rx_lw;		/* W  Rx buffer low water mark  */
-	volatile unsigned short  rx_hw;		/* W  Rx buffer high water mark */
-	volatile unsigned short  incr;		/* W  Increment to next channel */
-
-	volatile unsigned short  fepdev;	/* U  SCC device base address    */
-	volatile unsigned short  edelay;	/* W  Exception delay            */
-	volatile unsigned short  blen;		/* W  Break length              */
-	volatile unsigned short  btime;		/* U  Break complete time       */
-
-	volatile unsigned short  iflag;		/* C  UNIX input flags          */
-	volatile unsigned short  oflag;		/* C  UNIX output flags         */
-	volatile unsigned short  cflag;		/* C  UNIX control flags        */
-	volatile unsigned short  wfill[13];	/* U  Reserved for expansion    */
-
-	volatile unsigned char   num;		/* U  Channel number            */
-	volatile unsigned char   ract;		/* U  Receiver active counter   */
-	volatile unsigned char   bstat;		/* U  Break status bits         */
-	volatile unsigned char   tbusy;		/* W  Transmit busy             */
-	volatile unsigned char   iempty;	/* W  Transmit empty event enable */
-	volatile unsigned char   ilow;		/* W  Transmit low-water event enable */
-	volatile unsigned char   idata;		/* W  Receive data interrupt enable */
-	volatile unsigned char   eflag;		/* U  Host event flags          */
-
-	volatile unsigned char   tflag;		/* U  Transmit flags            */
-	volatile unsigned char   rflag;		/* U  Receive flags             */
-	volatile unsigned char   xmask;		/* U  Transmit ready flags      */
-	volatile unsigned char   xval;		/* U  Transmit ready value      */
-	volatile unsigned char   m_stat;	/* RC Modem status bits          */
-	volatile unsigned char   m_change;	/* U  Modem bits which changed  */
-	volatile unsigned char   m_int;		/* W  Modem interrupt enable bits */
-	volatile unsigned char   m_last;	/* U  Last modem status         */
-
-	volatile unsigned char   mtran;		/* C   Unreported modem trans   */
-	volatile unsigned char   orun;		/* C   Buffer overrun occurred  */
-	volatile unsigned char   astartc;	/* W   Auxiliary Xon char       */
-	volatile unsigned char   astopc;	/* W   Auxiliary Xoff char      */
-	volatile unsigned char   startc;	/* W   Xon character             */
-	volatile unsigned char   stopc;		/* W   Xoff character           */
-	volatile unsigned char   vnextc;	/* W   Vnext character           */
-	volatile unsigned char   hflow;		/* C   Software flow control    */
-	
-	volatile unsigned char   fillc;		/* U   Delay Fill character     */
-	volatile unsigned char   ochar;		/* U   Saved output character   */
-	volatile unsigned char   omask;		/* U   Output character mask    */
-
-	volatile unsigned char   bfill[13];	/* U   Reserved for expansion   */
-
-	volatile unsigned char   scc[16];	/* U   SCC registers            */
+	unsigned short  tp_jmp;		/* Transmit poll jump	 */
+	unsigned short  tc_jmp;		/* Cooked procedure jump */
+	unsigned short  ri_jmp;		/* Not currently used	 */
+	unsigned short  rp_jmp;		/* Receive poll jump	 */
+
+	unsigned short  tx_seg;		/* W Tx segment	 */
+	unsigned short  tx_head;	/* W Tx buffer head offset */
+	unsigned short  tx_tail;	/* R Tx buffer tail offset */
+	unsigned short  tx_max;		/* W Tx buffer size - 1    */
+
+	unsigned short  rx_seg;		/* W Rx segment	    */
+	unsigned short  rx_head;	/* W Rx buffer head offset */
+	unsigned short  rx_tail;	/* R Rx buffer tail offset */
+	unsigned short  rx_max;		/* W Rx buffer size - 1    */
+
+	unsigned short  tx_lw;		/* W Tx buffer low water mark */
+	unsigned short  rx_lw;		/* W Rx buffer low water mark */
+	unsigned short  rx_hw;		/* W Rx buffer high water mark*/
+	unsigned short  incr;		/* W Increment to next channel*/
+
+	unsigned short  fepdev;		/* U SCC device base address  */
+	unsigned short  edelay;		/* W Exception delay          */
+	unsigned short  blen;		/* W Break length             */
+	unsigned short  btime;		/* U Break complete time      */
+
+	unsigned short  iflag;		/* C UNIX input flags         */
+	unsigned short  oflag;		/* C UNIX output flags        */
+	unsigned short  cflag;		/* C UNIX control flags       */
+	unsigned short  wfill[13];	/* U Reserved for expansion   */
+
+	unsigned char   num;		/* U Channel number           */
+	unsigned char   ract;		/* U Receiver active counter  */
+	unsigned char   bstat;		/* U Break status bits        */
+	unsigned char   tbusy;		/* W Transmit busy            */
+	unsigned char   iempty;		/* W Transmit empty event     */
+					/* enable                     */
+	unsigned char   ilow;		/* W Transmit low-water event */
+					/* enable                     */
+	unsigned char   idata;		/* W Receive data interrupt   */
+					/* enable                     */
+	unsigned char   eflag;		/* U Host event flags         */
+
+	unsigned char   tflag;		/* U Transmit flags           */
+	unsigned char   rflag;		/* U Receive flags            */
+	unsigned char   xmask;		/* U Transmit ready flags     */
+	unsigned char   xval;		/* U Transmit ready value     */
+	unsigned char   m_stat;		/* RC Modem status bits       */
+	unsigned char   m_change;	/* U Modem bits which changed */
+	unsigned char   m_int;		/* W Modem interrupt enable   */
+					/* bits                       */
+	unsigned char   m_last;		/* U Last modem status        */
+
+	unsigned char   mtran;		/* C Unreported modem trans   */
+	unsigned char   orun;		/* C Buffer overrun occurred  */
+	unsigned char   astartc;	/* W Auxiliary Xon char       */
+	unsigned char   astopc;		/* W Auxiliary Xoff char      */
+	unsigned char   startc;		/* W Xon character            */
+	unsigned char   stopc;		/* W Xoff character           */
+	unsigned char   vnextc;		/* W Vnext character          */
+	unsigned char   hflow;		/* C Software flow control    */
+
+	unsigned char   fillc;		/* U Delay Fill character     */
+	unsigned char   ochar;		/* U Saved output character   */
+	unsigned char   omask;		/* U Output character mask    */
+
+	unsigned char   bfill[13];	/* U Reserved for expansion   */
+
+	unsigned char   scc[16];	/* U SCC registers            */
 };
 
 struct cnode {
@@ -1225,7 +1164,7 @@ struct cnode {
 
 	union {
 		struct {
-			char  type;	/* Board Type 		*/
+			char  type;	/* Board Type           */
 			long  port;	/* I/O Address		*/
 			char  *portstr; /* I/O Address in string */
 			long  addr;	/* Memory Address	*/
@@ -1294,29 +1233,17 @@ struct cnode {
 		} module;
 
 		char *ttyname;
-
 		char *cuname;
-
 		char *printname;
-
 		long majornumber;
-
 		long altpin;
-
 		long ttysize;
-
 		long chsize;
-
 		long bssize;
-
 		long unsize;
-
 		long f2size;
-
 		long vpixsize;
-
 		long useintr;
 	} u;
 };
-
 #endif
diff --git a/drivers/staging/dgnc/dgnc_cls.c b/drivers/staging/dgnc/dgnc_cls.c
index 60d9b62c6f1..8e265c20db5 100644
--- a/drivers/staging/dgnc/dgnc_cls.c
+++ b/drivers/staging/dgnc/dgnc_cls.c
@@ -453,10 +453,6 @@ static inline void cls_parse_isr(struct dgnc_board *brd, uint port)
 			cls_copy_data_from_queue_to_uart(ch);
 		}
 
-		/* Received Xoff signal/Special character */
-		if (isr & UART_IIR_XOFF)
-			/* Empty */
-
 		/* CTS/RTS change of state */
 		if (isr & UART_IIR_CTSRTS) {
 			brd->intr_modem++;
diff --git a/drivers/staging/dgnc/dgnc_cls.h b/drivers/staging/dgnc/dgnc_cls.h
index ffe8535a84a..465d79a6f75 100644
--- a/drivers/staging/dgnc/dgnc_cls.h
+++ b/drivers/staging/dgnc/dgnc_cls.h
@@ -61,7 +61,6 @@ struct cls_uart_struct {
 #define UART_16654_FCR_RXTRIGGER_56	0x80
 #define UART_16654_FCR_RXTRIGGER_60     0xC0
 
-#define UART_IIR_XOFF			0x10	/* Received Xoff signal/Special character */
 #define UART_IIR_CTSRTS			0x20	/* Received CTS/RTS change of state */
 #define UART_IIR_RDI_TIMEOUT		0x0C    /* Receiver data TIMEOUT */
 
diff --git a/drivers/staging/dgnc/dgnc_driver.c b/drivers/staging/dgnc/dgnc_driver.c
index b1a39b25b41..5af8300dfb0 100644
--- a/drivers/staging/dgnc/dgnc_driver.c
+++ b/drivers/staging/dgnc/dgnc_driver.c
@@ -153,8 +153,7 @@ struct board_id {
 	unsigned int is_pci_express;
 };
 
-static struct board_id dgnc_Ids[] =
-{
+static struct board_id dgnc_Ids[] = {
 	{	PCI_DEVICE_CLASSIC_4_PCI_NAME,		4,	0	},
 	{	PCI_DEVICE_CLASSIC_4_422_PCI_NAME,	4,	0	},
 	{	PCI_DEVICE_CLASSIC_8_PCI_NAME,		8,	0	},
@@ -219,9 +218,8 @@ int dgnc_init_module(void)
 	 */
 	rc = dgnc_start();
 
-	if (rc < 0) {
+	if (rc < 0)
 		return rc;
-	}
 
 	/*
 	 * Find and configure all the cards
@@ -239,8 +237,7 @@ int dgnc_init_module(void)
 			pr_warn("WARNING: dgnc driver load failed.  No Digi Neo or Classic boards found.\n");
 
 		dgnc_cleanup_module();
-	}
-	else {
+	} else {
 		dgnc_create_driver_sysfiles(&dgnc_driver);
 	}
 
@@ -451,20 +448,15 @@ static void dgnc_cleanup_board(struct dgnc_board *brd)
 	/* Free all allocated channels structs */
 	for (i = 0; i < MAXPORTS ; i++) {
 		if (brd->channels[i]) {
-			if (brd->channels[i]->ch_rqueue)
-				kfree(brd->channels[i]->ch_rqueue);
-			if (brd->channels[i]->ch_equeue)
-				kfree(brd->channels[i]->ch_equeue);
-			if (brd->channels[i]->ch_wqueue)
-				kfree(brd->channels[i]->ch_wqueue);
-
+			kfree(brd->channels[i]->ch_rqueue);
+			kfree(brd->channels[i]->ch_equeue);
+			kfree(brd->channels[i]->ch_wqueue);
 			kfree(brd->channels[i]);
 			brd->channels[i] = NULL;
 		}
 	}
 
-	if (brd->flipbuf)
-		kfree(brd->flipbuf);
+	kfree(brd->flipbuf);
 
 	dgnc_Board[brd->boardnum] = NULL;
 
@@ -519,9 +511,8 @@ static int dgnc_found_board(struct pci_dev *pdev, int id)
 
 	brd->state		= BOARD_FOUND;
 
-	for (i = 0; i < MAXPORTS; i++) {
+	for (i = 0; i < MAXPORTS; i++)
 		brd->channels[i] = NULL;
-	}
 
 	/* store which card & revision we have */
 	pci_read_config_word(pdev, PCI_SUBSYSTEM_VENDOR_ID, &brd->subvendor);
@@ -730,15 +721,18 @@ static int dgnc_finalize_board_init(struct dgnc_board *brd) {
 	DPR_INIT(("dgnc_finalize_board_init() - start #2\n"));
 
 	if (brd->irq) {
-		rc = request_irq(brd->irq, brd->bd_ops->intr, IRQF_SHARED, "DGNC", brd);
+		rc = request_irq(brd->irq, brd->bd_ops->intr,
+				 IRQF_SHARED, "DGNC", brd);
 
 		if (rc) {
-			printk("Failed to hook IRQ %d\n",brd->irq);
+			dev_err(&brd->pdev->dev,
+				"Failed to hook IRQ %d\n", brd->irq);
 			brd->state = BOARD_FAILED;
 			brd->dpastatus = BD_NOFEP;
 			rc = -ENODEV;
 		} else {
-			DPR_INIT(("Requested and received usage of IRQ %d\n", brd->irq));
+			DPR_INIT(("Requested and received usage of IRQ %d\n",
+				  brd->irq));
 		}
 	}
 	return rc;
@@ -799,9 +793,8 @@ static void dgnc_poll_handler(ulong dummy)
 	 * driver tells us its up and running, and has
 	 * everything it needs.
 	 */
-	if (dgnc_driver_state != DRIVER_READY) {
+	if (dgnc_driver_state != DRIVER_READY)
 		goto schedule_poller;
-	}
 
 	/* Go thru each board, kicking off a tasklet for each if needed */
 	for (i = 0; i < dgnc_NumBoards; i++) {
@@ -831,9 +824,8 @@ schedule_poller:
 
 	new_time = dgnc_poll_time - jiffies;
 
-	if ((ulong) new_time >= 2 * dgnc_poll_tick) {
+	if ((ulong) new_time >= 2 * dgnc_poll_tick)
 		dgnc_poll_time = jiffies +  dgnc_jiffies_from_ms(dgnc_poll_tick);
-	}
 
 	init_timer(&dgnc_poll_timer);
 	dgnc_poll_timer.function = dgnc_poll_handler;
@@ -860,9 +852,8 @@ static void dgnc_init_globals(void)
 	dgnc_trcbuf_size	= trcbuf_size;
 	dgnc_debug		= debug;
 
-	for (i = 0; i < MAXBOARDS; i++) {
+	for (i = 0; i < MAXBOARDS; i++)
 		dgnc_Board[i] = NULL;
-	}
 
 	init_timer(&dgnc_poll_timer);
 }
diff --git a/drivers/staging/dgnc/dgnc_neo.c b/drivers/staging/dgnc/dgnc_neo.c
index cf22c7b725f..9de988cc892 100644
--- a/drivers/staging/dgnc/dgnc_neo.c
+++ b/drivers/staging/dgnc/dgnc_neo.c
@@ -395,7 +395,8 @@ static inline void neo_clear_break(struct channel_t *ch, int force)
 
 	/* Turn break off, and unset some variables */
 	if (ch->ch_flags & CH_BREAK_SENDING) {
-		if ((jiffies >= ch->ch_stop_sending_break) || force) {
+		if (time_after_eq(jiffies, ch->ch_stop_sending_break)
+		    || force) {
 			uchar temp = readb(&ch->ch_neo_uart->lcr);
 			writeb((temp & ~UART_LCR_SBC), &ch->ch_neo_uart->lcr);
 			neo_pci_posting_flush(ch->ch_bd);
@@ -434,9 +435,8 @@ static inline void neo_parse_isr(struct dgnc_board *brd, uint port)
 		isr = readb(&ch->ch_neo_uart->isr_fcr);
 
 		/* Bail if no pending interrupt */
-		if (isr & UART_IIR_NO_INT)  {
+		if (isr & UART_IIR_NO_INT)
 			break;
-		}
 
 		/*
 		 * Yank off the upper 2 bits, which just show that the FIFO's are enabled.
@@ -485,8 +485,7 @@ static inline void neo_parse_isr(struct dgnc_board *brd, uint port)
 					DGNC_UNLOCK(ch->ch_lock, lock_flags);
 				}
 				DPR_INTR(("Port %d. XON detected in incoming data\n", port));
-			}
-			else if (cause == UART_17158_XOFF_DETECT) {
+			} else if (cause == UART_17158_XOFF_DETECT) {
 				if (!(brd->channels[port]->ch_flags & CH_STOP)) {
 					DGNC_LOCK(ch->ch_lock, lock_flags);
 					ch->ch_flags |= CH_STOP;
@@ -511,8 +510,7 @@ static inline void neo_parse_isr(struct dgnc_board *brd, uint port)
 					DGNC_LOCK(ch->ch_lock, lock_flags);
 					ch->ch_mostat |= UART_MCR_RTS;
 					DGNC_UNLOCK(ch->ch_lock, lock_flags);
-				}
-				else {
+				} else {
 					DGNC_LOCK(ch->ch_lock, lock_flags);
 					ch->ch_mostat &= ~(UART_MCR_RTS);
 					DGNC_UNLOCK(ch->ch_lock, lock_flags);
@@ -522,8 +520,7 @@ static inline void neo_parse_isr(struct dgnc_board *brd, uint port)
 					DGNC_LOCK(ch->ch_lock, lock_flags);
 					ch->ch_mostat |= UART_MCR_DTR;
 					DGNC_UNLOCK(ch->ch_lock, lock_flags);
-				}
-				else {
+				} else {
 					DGNC_LOCK(ch->ch_lock, lock_flags);
 					ch->ch_mostat &= ~(UART_MCR_DTR);
 					DGNC_UNLOCK(ch->ch_lock, lock_flags);
@@ -624,8 +621,7 @@ static inline void neo_parse_lsr(struct dgnc_board *brd, uint port)
 
 		/* Transfer data (if any) from Write Queue -> UART. */
 		neo_copy_data_from_queue_to_uart(ch);
-	}
-	else if (linestatus & UART_17158_TX_AND_FIFO_CLR) {
+	} else if (linestatus & UART_17158_TX_AND_FIFO_CLR) {
 		brd->intr_tx++;
 		ch->ch_intr_tx++;
 		DGNC_LOCK(ch->ch_lock, lock_flags);
@@ -654,24 +650,20 @@ static void neo_param(struct tty_struct *tty)
 	struct channel_t *ch;
 	struct un_t   *un;
 
-	if (!tty || tty->magic != TTY_MAGIC) {
+	if (!tty || tty->magic != TTY_MAGIC)
 		return;
-	}
 
 	un = (struct un_t *) tty->driver_data;
-	if (!un || un->magic != DGNC_UNIT_MAGIC) {
+	if (!un || un->magic != DGNC_UNIT_MAGIC)
 		return;
-	}
 
 	ch = un->un_ch;
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC) {
+	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return;
-	}
 
 	bd = ch->ch_bd;
-	if (!bd || bd->magic != DGNC_BOARD_MAGIC) {
+	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
 		return;
-	}
 
 	DPR_PARAM(("param start: tdev: %x cflags: %x oflags: %x iflags: %x\n",
 		ch->ch_tun.un_dev, ch->ch_c_cflag, ch->ch_c_oflag, ch->ch_c_iflag));
@@ -777,13 +769,11 @@ static void neo_param(struct tty_struct *tty)
 		}
 	}
 
-	if (ch->ch_c_cflag & PARENB) {
+	if (ch->ch_c_cflag & PARENB)
 		lcr |= UART_LCR_PARITY;
-	}
 
-	if (!(ch->ch_c_cflag & PARODD)) {
+	if (!(ch->ch_c_cflag & PARODD))
 		lcr |= UART_LCR_EPAR;
-	}
 
 	/*
 	 * Not all platforms support mark/space parity,
@@ -832,26 +822,23 @@ static void neo_param(struct tty_struct *tty)
 	if (uart_lcr != lcr)
 		writeb(lcr, &ch->ch_neo_uart->lcr);
 
-	if (ch->ch_c_cflag & CREAD) {
+	if (ch->ch_c_cflag & CREAD)
 		ier |= (UART_IER_RDI | UART_IER_RLSI);
-	}
-	else {
+	else
 		ier &= ~(UART_IER_RDI | UART_IER_RLSI);
-	}
 
 	/*
 	 * Have the UART interrupt on modem signal changes ONLY when
 	 * we are in hardware flow control mode, or CLOCAL/FORCEDCD is not set.
 	 */
-	if ((ch->ch_digi.digi_flags & CTSPACE) || (ch->ch_digi.digi_flags & RTSPACE) ||
-		(ch->ch_c_cflag & CRTSCTS) || !(ch->ch_digi.digi_flags & DIGI_FORCEDCD) ||
-		!(ch->ch_c_cflag & CLOCAL))
-	{
+	if ((ch->ch_digi.digi_flags & CTSPACE) ||
+	    (ch->ch_digi.digi_flags & RTSPACE) ||
+	    (ch->ch_c_cflag & CRTSCTS) ||
+	    !(ch->ch_digi.digi_flags & DIGI_FORCEDCD) ||
+	    !(ch->ch_c_cflag & CLOCAL))
 		ier |= UART_IER_MSI;
-	}
-	else {
+	else
 		ier &= ~UART_IER_MSI;
-	}
 
 	ier |= UART_IER_THRI;
 
@@ -863,29 +850,25 @@ static void neo_param(struct tty_struct *tty)
 
 	if (ch->ch_digi.digi_flags & CTSPACE || ch->ch_c_cflag & CRTSCTS) {
 		neo_set_cts_flow_control(ch);
-	}
-	else if (ch->ch_c_iflag & IXON) {
+	} else if (ch->ch_c_iflag & IXON) {
 		/* If start/stop is set to disable, then we should disable flow control */
 		if ((ch->ch_startc == _POSIX_VDISABLE) || (ch->ch_stopc == _POSIX_VDISABLE))
 			neo_set_no_output_flow_control(ch);
 		else
 			neo_set_ixon_flow_control(ch);
-	}
-	else {
+	} else {
 		neo_set_no_output_flow_control(ch);
 	}
 
 	if (ch->ch_digi.digi_flags & RTSPACE || ch->ch_c_cflag & CRTSCTS) {
 		neo_set_rts_flow_control(ch);
-	}
-	else if (ch->ch_c_iflag & IXOFF) {
+	} else if (ch->ch_c_iflag & IXOFF) {
 		/* If start/stop is set to disable, then we should disable flow control */
 		if ((ch->ch_startc == _POSIX_VDISABLE) || (ch->ch_stopc == _POSIX_VDISABLE))
 			neo_set_no_input_flow_control(ch);
 		else
 			neo_set_ixoff_flow_control(ch);
-	}
-	else {
+	} else {
 		neo_set_no_input_flow_control(ch);
 	}
 
@@ -1225,12 +1208,10 @@ static void neo_copy_data_from_uart_to_queue(struct channel_t *ch)
 		 * The count can be any where from 0-3 bytes "off".
 		 * Bizarre, but true.
 		 */
-		if ((ch->ch_bd->dvid & 0xf0) >= UART_XR17E158_DVID) {
+		if ((ch->ch_bd->dvid & 0xf0) >= UART_XR17E158_DVID)
 			total -= 1;
-		}
-		else {
+		else
 			total -= 3;
-		}
 	}
 
 
@@ -1274,9 +1255,8 @@ static void neo_copy_data_from_uart_to_queue(struct channel_t *ch)
 		 * will reset some bits after our read, we need to ensure
 		 * we don't miss our TX FIFO emptys.
 		 */
-		if (linestatus & (UART_LSR_THRE | UART_17158_TX_AND_FIFO_CLR)) {
+		if (linestatus & (UART_LSR_THRE | UART_17158_TX_AND_FIFO_CLR))
 			ch->ch_flags |= (CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM);
-		}
 
 		linestatus = 0;
 
@@ -1404,19 +1384,16 @@ static int neo_drain(struct tty_struct *tty, uint seconds)
 	struct un_t *un;
 	int rc = 0;
 
-	if (!tty || tty->magic != TTY_MAGIC) {
+	if (!tty || tty->magic != TTY_MAGIC)
 		return -ENXIO;
-	}
 
 	un = (struct un_t *) tty->driver_data;
-	if (!un || un->magic != DGNC_UNIT_MAGIC) {
+	if (!un || un->magic != DGNC_UNIT_MAGIC)
 		return -ENXIO;
-	}
 
 	ch = un->un_ch;
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC) {
+	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return -ENXIO;
-	}
 
 	DPR_IOCTL(("%d Drain wait started.\n", __LINE__));
 
@@ -1433,12 +1410,10 @@ static int neo_drain(struct tty_struct *tty, uint seconds)
 	rc = wait_event_interruptible(un->un_flags_wait, ((un->un_flags & UN_EMPTY) == 0));
 
 	/* If ret is non-zero, user ctrl-c'ed us */
-	if (rc) {
+	if (rc)
 		DPR_IOCTL(("%d Drain - User ctrl c'ed\n", __LINE__));
-	}
-	else {
+	else
 		DPR_IOCTL(("%d Drain wait finished.\n", __LINE__));
-	}
 
 	return rc;
 }
@@ -1454,9 +1429,8 @@ static void neo_flush_uart_write(struct channel_t *ch)
 	uchar tmp = 0;
 	int i = 0;
 
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC) {
+	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return;
-	}
 
 	writeb((UART_FCR_ENABLE_FIFO | UART_FCR_CLEAR_XMIT), &ch->ch_neo_uart->isr_fcr);
 	neo_pci_posting_flush(ch->ch_bd);
@@ -1468,8 +1442,7 @@ static void neo_flush_uart_write(struct channel_t *ch)
 		if (tmp & 4) {
 			DPR_IOCTL(("Still flushing TX UART... i: %d\n", i));
 			udelay(10);
-		}
-		else
+		} else
 			break;
 	}
 
@@ -1487,9 +1460,8 @@ static void neo_flush_uart_read(struct channel_t *ch)
 	uchar tmp = 0;
 	int i = 0;
 
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC) {
+	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return;
-	}
 
 	writeb((UART_FCR_ENABLE_FIFO | UART_FCR_CLEAR_RCVR), &ch->ch_neo_uart->isr_fcr);
 	neo_pci_posting_flush(ch->ch_bd);
@@ -1501,8 +1473,7 @@ static void neo_flush_uart_read(struct channel_t *ch)
 		if (tmp & 2) {
 			DPR_IOCTL(("Still flushing RX UART... i: %d\n", i));
 			udelay(10);
-		}
-		else
+		} else
 			break;
 	}
 }
@@ -1598,8 +1569,7 @@ static void neo_copy_data_from_queue_to_uart(struct channel_t *ch)
 		}
 
 		n = UART_17158_TX_FIFOSIZE - ch->ch_t_tlevel;
-	}
-	else {
+	} else {
 		n = UART_17158_TX_FIFOSIZE - readb(&ch->ch_neo_uart->tfifo);
 	}
 
@@ -1963,13 +1933,10 @@ static void neo_vpd(struct dgnc_board *brd)
 		||  (brd->vpd[0x7F] != 0x78))   /* small resource end tag */
 	{
 		memset(brd->vpd, '\0', NEO_VPD_IMAGESIZE);
-	}
-	else {
+	} else {
 		/* Search for the serial number */
-		for (i = 0; i < NEO_VPD_IMAGEBYTES - 3; i++) {
-			if (brd->vpd[i] == 'S' && brd->vpd[i + 1] == 'N') {
+		for (i = 0; i < NEO_VPD_IMAGEBYTES - 3; i++)
+			if (brd->vpd[i] == 'S' && brd->vpd[i + 1] == 'N')
 				strncpy(brd->serial_num, &(brd->vpd[i + 3]), 9);
-			}
-		}
 	}
 }
diff --git a/drivers/staging/dgnc/dgnc_sysfs.c b/drivers/staging/dgnc/dgnc_sysfs.c
index 946230c2348..0f0e8fcb663 100644
--- a/drivers/staging/dgnc/dgnc_sysfs.c
+++ b/drivers/staging/dgnc/dgnc_sysfs.c
@@ -739,7 +739,7 @@ void dgnc_create_tty_sysfs(struct un_t *un, struct device *c)
 
 	ret = sysfs_create_group(&c->kobj, &dgnc_tty_attribute_group);
 	if (ret) {
-		printk(KERN_ERR "dgnc: failed to create sysfs tty device attributes.\n");
+		dev_err(c, "dgnc: failed to create sysfs tty device attributes.\n");
 		sysfs_remove_group(&c->kobj, &dgnc_tty_attribute_group);
 		return;
 	}
diff --git a/drivers/staging/dgnc/dgnc_tty.c b/drivers/staging/dgnc/dgnc_tty.c
index f0b17c36edd..4135cb0ed9f 100644
--- a/drivers/staging/dgnc/dgnc_tty.c
+++ b/drivers/staging/dgnc/dgnc_tty.c
@@ -533,9 +533,8 @@ void dgnc_sniff_nowait_nolock(struct channel_t *ch, uchar *text, uchar *buf, int
 			 * We *cannot* sleep here waiting for space, because this
 			 * function was probably called by the interrupt/timer routines!
 			 */
-			if (n == 0) {
+			if (n == 0)
 				return;
-			}
 
 			/*
 			 * Copy as much data as will fit.
@@ -661,7 +660,7 @@ void dgnc_input(struct channel_t *ch)
 	tp = ch->ch_tun.un_tty;
 
 	bd = ch->ch_bd;
-	if(!bd || bd->magic != DGNC_BOARD_MAGIC)
+	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
 		return;
 
 	DGNC_LOCK(ch->ch_lock, lock_flags);
@@ -803,8 +802,7 @@ void dgnc_input(struct channel_t *ch)
 				else
 					tty_insert_flip_char(tp->port, *(ch->ch_rqueue + tail + i), TTY_NORMAL);
 			}
-		}
-		else {
+		} else {
 			tty_insert_flip_string(tp->port, ch->ch_rqueue + tail, s);
 		}
 
@@ -857,14 +855,11 @@ void dgnc_carrier(struct channel_t *ch)
 		phys_carrier = 1;
 	}
 
-	if (ch->ch_digi.digi_flags & DIGI_FORCEDCD) {
+	if (ch->ch_digi.digi_flags & DIGI_FORCEDCD)
 		virt_carrier = 1;
-	}
 
-	if (ch->ch_c_cflag & CLOCAL) {
+	if (ch->ch_c_cflag & CLOCAL)
 		virt_carrier = 1;
-	}
-
 
 	DPR_CARR(("DCD: physical: %d virt: %d\n", phys_carrier, virt_carrier));
 
@@ -1005,11 +1000,10 @@ static void dgnc_set_custom_speed(struct channel_t *ch, uint newrate)
 			deltahigh = testrate_high - newrate;
 			deltalow = newrate - testrate_low;
 
-			if (deltahigh < deltalow) {
+			if (deltahigh < deltalow)
 				newrate = testrate_high;
-			} else {
+			else
 				newrate = testrate_low;
-			}
 		}
 	}
 
@@ -1044,7 +1038,7 @@ void dgnc_check_queue_flow_control(struct channel_t *ch)
 	if (qleft < 256) {
 		/* HWFLOW */
 		if (ch->ch_digi.digi_flags & CTSPACE || ch->ch_c_cflag & CRTSCTS) {
-			if(!(ch->ch_flags & CH_RECEIVER_OFF)) {
+			if (!(ch->ch_flags & CH_RECEIVER_OFF)) {
 				ch->ch_bd->bd_ops->disable_receiver(ch);
 				ch->ch_flags |= (CH_RECEIVER_OFF);
 				DPR_READ(("Internal queue hit hilevel mark (%d)! Turning off interrupts.\n",
@@ -1222,15 +1216,13 @@ static int dgnc_tty_open(struct tty_struct *tty, struct file *file)
 	major = MAJOR(tty_devnum(tty));
 	minor = MINOR(tty_devnum(tty));
 
-	if (major > 255) {
+	if (major > 255)
 		return -ENXIO;
-	}
 
 	/* Get board pointer from our array of majors we have allocated */
 	brd = dgnc_BoardsByMajor[major];
-	if (!brd) {
+	if (!brd)
 		return -ENXIO;
-	}
 
 	/*
 	 * If board is not yet up to a state of READY, go to
@@ -1239,9 +1231,8 @@ static int dgnc_tty_open(struct tty_struct *tty, struct file *file)
 	rc = wait_event_interruptible(brd->state_wait,
 		(brd->state & BOARD_READY));
 
-	if (rc) {
+	if (rc)
 		return rc;
-	}
 
 	DGNC_LOCK(brd->bd_lock, lock_flags);
 
@@ -1267,12 +1258,10 @@ static int dgnc_tty_open(struct tty_struct *tty, struct file *file)
 	if (!IS_PRINT(minor)) {
 		un = &brd->channels[PORT_NUM(minor)]->ch_tun;
 		un->un_type = DGNC_SERIAL;
-	}
-	else if (IS_PRINT(minor)) {
+	} else if (IS_PRINT(minor)) {
 		un = &brd->channels[PORT_NUM(minor)]->ch_pun;
 		un->un_type = DGNC_PRINT;
-	}
-	else {
+	} else {
 		DGNC_UNLOCK(ch->ch_lock, lock_flags);
 		DPR_OPEN(("%d Unknown TYPE!\n", __LINE__));
 		return -ENXIO;
@@ -1409,10 +1398,9 @@ static int dgnc_tty_open(struct tty_struct *tty, struct file *file)
 
 	rc = dgnc_block_til_ready(tty, file, ch);
 
-	if (rc) {
+	if (rc)
 		DPR_OPEN(("dgnc_tty_open returning after dgnc_block_til_ready "
 			"with %d\n", rc));
-	}
 
 	/* No going back now, increment our unit and channel counters */
 	DGNC_LOCK(ch->ch_lock, lock_flags);
@@ -1444,9 +1432,8 @@ static int dgnc_block_til_ready(struct tty_struct *tty, struct file *file, struc
 	}
 
 	un = tty->driver_data;
-	if (!un || un->magic != DGNC_UNIT_MAGIC) {
+	if (!un || un->magic != DGNC_UNIT_MAGIC)
 		return -ENXIO;
-	}
 
 	DPR_OPEN(("dgnc_block_til_ready - before block.\n"));
 
@@ -1489,9 +1476,8 @@ static int dgnc_block_til_ready(struct tty_struct *tty, struct file *file, struc
 			 * 3) DCD (fake or real) is active.
 			 */
 
-			if (file->f_flags & O_NONBLOCK) {
+			if (file->f_flags & O_NONBLOCK)
 				break;
-			}
 
 			if (tty->flags & (1 << TTY_IO_ERROR)) {
 				retval = -EIO;
@@ -1507,8 +1493,7 @@ static int dgnc_block_til_ready(struct tty_struct *tty, struct file *file, struc
 				DPR_OPEN(("%d: ch_flags: %x\n", __LINE__, ch->ch_flags));
 				break;
 			}
-		}
-		else {
+		} else {
 			sleep_on_un_flags = 1;
 		}
 
@@ -1547,14 +1532,12 @@ static int dgnc_block_til_ready(struct tty_struct *tty, struct file *file, struc
 		/*
 		 * Wait for something in the flags to change from the current value.
 		 */
-		if (sleep_on_un_flags) {
+		if (sleep_on_un_flags)
 			retval = wait_event_interruptible(un->un_flags_wait,
 				(old_flags != (ch->ch_tun.un_flags | ch->ch_pun.un_flags)));
-		}
-		else {
+		else
 			retval = wait_event_interruptible(ch->ch_flags_wait,
 				(old_flags != ch->ch_flags));
-		}
 
 		DPR_OPEN(("After sleep... retval: %x\n", retval));
 
@@ -1693,7 +1676,7 @@ static void dgnc_tty_close(struct tty_struct *tty, struct file *file)
 		/*
 		 * turn off print device when closing print device.
 		 */
-		if ((un->un_type == DGNC_PRINT) && (ch->ch_flags & CH_PRON) ) {
+		if ((un->un_type == DGNC_PRINT) && (ch->ch_flags & CH_PRON)) {
 			dgnc_wmove(ch, ch->ch_digi.digi_offstr,
 				(int) ch->ch_digi.digi_offlen);
 			ch->ch_flags &= ~CH_PRON;
@@ -1708,9 +1691,8 @@ static void dgnc_tty_close(struct tty_struct *tty, struct file *file)
 
 		DPR_CLOSE(("After calling wait_for_drain\n"));
 
-		if (rc) {
+		if (rc)
 			DPR_BASIC(("dgnc_tty_close - bad return: %d ", rc));
-		}
 
 		dgnc_tty_flush_buffer(tty);
 		tty_ldisc_flush(tty);
@@ -1748,12 +1730,11 @@ static void dgnc_tty_close(struct tty_struct *tty, struct file *file)
 
 		/* Turn off UART interrupts for this port */
 		ch->ch_bd->bd_ops->uart_off(ch);
-	}
-	else {
+	} else {
 		/*
 		 * turn off print device when closing print device.
 		 */
-		if ((un->un_type == DGNC_PRINT) && (ch->ch_flags & CH_PRON) ) {
+		if ((un->un_type == DGNC_PRINT) && (ch->ch_flags & CH_PRON)) {
 			dgnc_wmove(ch, ch->ch_digi.digi_offstr,
 				(int) ch->ch_digi.digi_offlen);
 			ch->ch_flags &= ~CH_PRON;
@@ -1857,7 +1838,7 @@ static int dgnc_maxcps_room(struct tty_struct *tty, int bytes_available)
 	if (un->un_type != DGNC_PRINT)
 		return bytes_available;
 
-	if (ch->ch_digi.digi_maxcps > 0 && ch->ch_digi.digi_bufsize > 0 ) {
+	if (ch->ch_digi.digi_maxcps > 0 && ch->ch_digi.digi_bufsize > 0) {
 		int cps_limit = 0;
 		unsigned long current_time = jiffies;
 		unsigned long buffer_time = current_time +
@@ -1867,12 +1848,10 @@ static int dgnc_maxcps_room(struct tty_struct *tty, int bytes_available)
 			/* buffer is empty */
 			ch->ch_cpstime = current_time;	    /* reset ch_cpstime */
 			cps_limit = ch->ch_digi.digi_bufsize;
-		}
-		else if (ch->ch_cpstime < buffer_time) {
+		} else if (ch->ch_cpstime < buffer_time) {
 			/* still room in the buffer */
 			cps_limit = ((buffer_time - ch->ch_cpstime) * ch->ch_digi.digi_maxcps) / HZ;
-		}
-		else {
+		} else {
 			/* no room in the buffer */
 			cps_limit = 0;
 		}
@@ -1931,8 +1910,7 @@ static int dgnc_tty_write_room(struct tty_struct *tty)
 		if (!(ch->ch_flags & CH_PRON))
 			ret -= ch->ch_digi.digi_onlen;
 		ret -= ch->ch_digi.digi_offlen;
-	}
-	else {
+	} else {
 		if (ch->ch_flags & CH_PRON)
 			ret -= ch->ch_digi.digi_offlen;
 	}
@@ -2086,9 +2064,8 @@ static int dgnc_tty_write(struct tty_struct *tty,
 		 * the board.
 		 */
 		/* we're allowed to block if it's from_user */
-		if (down_interruptible(&dgnc_TmpWriteSem)) {
+		if (down_interruptible(&dgnc_TmpWriteSem))
 			return -EINTR;
-		}
 
 		/*
 		 * copy_from_user() returns the number
@@ -2271,21 +2248,17 @@ static int dgnc_tty_tiocmset(struct tty_struct *tty,
 
 	DGNC_LOCK(ch->ch_lock, lock_flags);
 
-	if (set & TIOCM_RTS) {
+	if (set & TIOCM_RTS)
 		ch->ch_mostat |= UART_MCR_RTS;
-	}
 
-	if (set & TIOCM_DTR) {
+	if (set & TIOCM_DTR)
 		ch->ch_mostat |= UART_MCR_DTR;
-	}
 
-	if (clear & TIOCM_RTS) {
+	if (clear & TIOCM_RTS)
 		ch->ch_mostat &= ~(UART_MCR_RTS);
-	}
 
-	if (clear & TIOCM_DTR) {
+	if (clear & TIOCM_DTR)
 		ch->ch_mostat &= ~(UART_MCR_DTR);
-	}
 
 	ch->ch_bd->bd_ops->assert_modem_signals(ch);
 
@@ -2535,42 +2508,34 @@ static int dgnc_set_modem_info(struct tty_struct *tty, unsigned int command, uns
 
 	switch (command) {
 	case TIOCMBIS:
-		if (arg & TIOCM_RTS) {
+		if (arg & TIOCM_RTS)
 			ch->ch_mostat |= UART_MCR_RTS;
-		}
 
-		if (arg & TIOCM_DTR) {
+		if (arg & TIOCM_DTR)
 			ch->ch_mostat |= UART_MCR_DTR;
-		}
 
 		break;
 
 	case TIOCMBIC:
-		if (arg & TIOCM_RTS) {
+		if (arg & TIOCM_RTS)
 			ch->ch_mostat &= ~(UART_MCR_RTS);
-		}
 
-		if (arg & TIOCM_DTR) {
+		if (arg & TIOCM_DTR)
 			ch->ch_mostat &= ~(UART_MCR_DTR);
-		}
 
 		break;
 
 	case TIOCMSET:
 
-		if (arg & TIOCM_RTS) {
+		if (arg & TIOCM_RTS)
 			ch->ch_mostat |= UART_MCR_RTS;
-		}
-		else {
+		else
 			ch->ch_mostat &= ~(UART_MCR_RTS);
-		}
 
-		if (arg & TIOCM_DTR) {
+		if (arg & TIOCM_DTR)
 			ch->ch_mostat |= UART_MCR_DTR;
-		}
-		else {
+		else
 			ch->ch_mostat &= ~(UART_MCR_DTR);
-		}
 
 		break;
 
@@ -3048,9 +3013,8 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 		 */
 		rc = tty_check_change(tty);
 		DGNC_UNLOCK(ch->ch_lock, lock_flags);
-		if (rc) {
+		if (rc)
 			return rc;
-		}
 
 		rc = ch->ch_bd->bd_ops->drain(tty, 0);
 
@@ -3061,7 +3025,7 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 
 		DGNC_LOCK(ch->ch_lock, lock_flags);
 
-		if(((cmd == TCSBRK) && (!arg)) || (cmd == TCSBRKP)) {
+		if (((cmd == TCSBRK) && (!arg)) || (cmd == TCSBRKP)) {
 			ch->ch_bd->bd_ops->send_break(ch, 250);
 		}
 
@@ -3081,9 +3045,8 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 		 */
 		rc = tty_check_change(tty);
 		DGNC_UNLOCK(ch->ch_lock, lock_flags);
-		if (rc) {
+		if (rc)
 			return rc;
-		}
 
 		rc = ch->ch_bd->bd_ops->drain(tty, 0);
 		if (rc) {
@@ -3105,9 +3068,8 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 	case TIOCSBRK:
 		rc = tty_check_change(tty);
 		DGNC_UNLOCK(ch->ch_lock, lock_flags);
-		if (rc) {
+		if (rc)
 			return rc;
-		}
 
 		rc = ch->ch_bd->bd_ops->drain(tty, 0);
 		if (rc) {
@@ -3279,8 +3241,7 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 				return -EINTR;
 			}
 			DGNC_LOCK(ch->ch_lock, lock_flags);
-		}
-		else {
+		} else {
 			tty_ldisc_flush(tty);
 		}
 		/* fall thru */
@@ -3370,9 +3331,9 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 
 		DGNC_UNLOCK(ch->ch_lock, lock_flags);
 
-		if (copy_to_user(uarg, &buf, sizeof(buf))) {
+		if (copy_to_user(uarg, &buf, sizeof(buf)))
 			return -EFAULT;
-		}
+
 		return 0;
 	}
 
@@ -3389,9 +3350,9 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 		/* NOTE: MORE EVENTS NEEDS TO BE ADDED HERE */
 		if (ch->ch_flags & CH_BREAK_SENDING)
 			events |= EV_TXB;
-		if ((ch->ch_flags & CH_STOP) || (ch->ch_flags & CH_FORCED_STOP)) {
+		if ((ch->ch_flags & CH_STOP) || (ch->ch_flags & CH_FORCED_STOP))
 			events |= (EV_OPU | EV_OPS);
-		}
+
 		if ((ch->ch_flags & CH_STOPI) || (ch->ch_flags & CH_FORCED_STOPI)) {
 			events |= (EV_IPU | EV_IPS);
 		}
@@ -3418,9 +3379,8 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 		/*
 		 * Get data from user first.
 		 */
-		if (copy_from_user(&buf, uarg, sizeof(buf))) {
+		if (copy_from_user(&buf, uarg, sizeof(buf)))
 			return -EFAULT;
-		}
 
 		DGNC_LOCK(ch->ch_lock, lock_flags);
 
@@ -3449,24 +3409,22 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 		 * insert more characters into our queue for OPOST processing
 		 * that the RealPort Server doesn't know about.
 		 */
-		if (buf.txbuf > tdist) {
+		if (buf.txbuf > tdist)
 			buf.txbuf = tdist;
-		}
 
 		/*
 		 * Report whether our queue and UART TX are completely empty.
 		 */
-		if (count) {
+		if (count)
 			buf.txdone = 0;
-		} else {
+		else
 			buf.txdone = 1;
-		}
 
 		DGNC_UNLOCK(ch->ch_lock, lock_flags);
 
-		if (copy_to_user(uarg, &buf, sizeof(buf))) {
+		if (copy_to_user(uarg, &buf, sizeof(buf)))
 			return -EFAULT;
-		}
+
 		return 0;
 	}
 	default:
diff --git a/drivers/staging/et131x/Module.symvers b/drivers/staging/et131x/Module.symvers
deleted file mode 100644
index e69de29bb2d..00000000000
--- a/drivers/staging/et131x/Module.symvers
+++ /dev/null
diff --git a/drivers/staging/et131x/et131x.c b/drivers/staging/et131x/et131x.c
index d329cf31436..0901ef5d6e8 100644
--- a/drivers/staging/et131x/et131x.c
+++ b/drivers/staging/et131x/et131x.c
@@ -762,6 +762,7 @@ static int et131x_init_eeprom(struct et131x_adapter *adapter)
 	 */
 	if (eestatus & 0x4C) {
 		int write_failed = 0;
+
 		if (pdev->revision == 0x01) {
 			int	i;
 			static const u8 eedata[4] = { 0xFE, 0x13, 0x10, 0xFF };
@@ -2091,6 +2092,7 @@ static void et1310_disable_phy_coma(struct et131x_adapter *adapter)
 static inline u32 bump_free_buff_ring(u32 *free_buff_ring, u32 limit)
 {
 	u32 tmp_free_buff_ring = *free_buff_ring;
+
 	tmp_free_buff_ring++;
 	/* This works for all cases where limit < 1024. The 1023 case
 	 * works because 1023++ is 1024 which means the if condition is not
@@ -4237,7 +4239,6 @@ static int et131x_ioctl(struct net_device *netdev, struct ifreq *reqbuf,
 static int et131x_set_packet_filter(struct et131x_adapter *adapter)
 {
 	int filter = adapter->packet_filter;
-	int status = 0;
 	u32 ctrl;
 	u32 pf_ctrl;
 
@@ -4288,7 +4289,7 @@ static int et131x_set_packet_filter(struct et131x_adapter *adapter)
 		writel(pf_ctrl, &adapter->regs->rxmac.pf_ctrl);
 		writel(ctrl, &adapter->regs->rxmac.ctrl);
 	}
-	return status;
+	return 0;
 }
 
 /* et131x_multicast - The handler to configure multicasting on the interface */
diff --git a/drivers/staging/frontier/tranzport.c b/drivers/staging/frontier/tranzport.c
index 0571988c58f..2f86163d701 100644
--- a/drivers/staging/frontier/tranzport.c
+++ b/drivers/staging/frontier/tranzport.c
@@ -271,8 +271,8 @@ static void usb_tranzport_interrupt_in_callback(struct urb *urb)
 			 dev->interrupt_in_buffer[6],
 			 dev->interrupt_in_buffer[7]);
 #if SUPPRESS_EXTRA_OFFLINE_EVENTS
-	if (dev->offline == 2 && dev->interrupt_in_buffer[1] == 0xff)
-		goto resubmit;
+		if (dev->offline == 2 && dev->interrupt_in_buffer[1] == 0xff)
+			goto resubmit;
 		if (dev->offline == 1 && dev->interrupt_in_buffer[1] == 0xff) {
 			dev->offline = 2;
 			goto resubmit;
@@ -285,8 +285,8 @@ static void usb_tranzport_interrupt_in_callback(struct urb *urb)
 			dev->offline = 1;
 
 #endif	/* SUPPRESS_EXTRA_OFFLINE_EVENTS */
-	   dbg_info(&dev->intf->dev, "%s: head, tail are %x, %x\n",
-		__func__, dev->ring_head, dev->ring_tail);
+		dbg_info(&dev->intf->dev, "%s: head, tail are %x, %x\n",
+			 __func__, dev->ring_head, dev->ring_tail);
 
 		next_ring_head = (dev->ring_head + 1) % ring_buffer_size;
 
@@ -475,6 +475,7 @@ static unsigned int usb_tranzport_poll(struct file *file, poll_table *wait)
 {
 	struct usb_tranzport *dev;
 	unsigned int mask = 0;
+
 	dev = file->private_data;
 	poll_wait(file, &dev->read_wait, wait);
 	poll_wait(file, &dev->write_wait, wait);
@@ -937,6 +938,7 @@ static void usb_tranzport_disconnect(struct usb_interface *intf)
 {
 	struct usb_tranzport *dev;
 	int minor;
+
 	mutex_lock(&disconnect_mutex);
 	dev = usb_get_intfdata(intf);
 	usb_set_intfdata(intf, NULL);
diff --git a/drivers/staging/ft1000/ft1000-pcmcia/ft1000.h b/drivers/staging/ft1000/ft1000-pcmcia/ft1000.h
index 65f7ab6cb46..0c21ac68003 100644
--- a/drivers/staging/ft1000/ft1000-pcmcia/ft1000.h
+++ b/drivers/staging/ft1000/ft1000-pcmcia/ft1000.h
@@ -25,7 +25,10 @@
 
 #define FT1000_DPRAM_BASE	0x0000	/* Dual Port RAM starting offset */
 
-/* Maximum number of occurrence of pseudo header errors before resetting PC Card. */
+/*
+ * Maximum number of occurrence of pseudo header errors before resetting PC
+ * Card.
+ */
 #define MAX_PH_ERR	300
 
 #define SUCCESS	0x00
@@ -40,16 +43,17 @@ struct ft1000_pcmcia {
 struct pcmcia_device;
 struct net_device;
 extern struct net_device *init_ft1000_card(struct pcmcia_device *link,
-						void *ft1000_reset);
+					   void *ft1000_reset);
 extern void stop_ft1000_card(struct net_device *dev);
 extern int card_download(struct net_device *dev, const u8 *pFileStart,
-			size_t FileLength);
+			 size_t FileLength);
 extern void ft1000InitProc(struct net_device *dev);
 extern void ft1000CleanupProc(struct net_device *dev);
 
 extern u16 ft1000_read_dpram(struct net_device *dev, int offset);
 extern void card_bootload(struct net_device *dev);
-extern u16 ft1000_read_dpram_mag_16(struct net_device *dev, int offset, int Index);
+extern u16 ft1000_read_dpram_mag_16(struct net_device *dev, int offset,
+				    int Index);
 extern u32 ft1000_read_dpram_mag_32(struct net_device *dev, int offset);
 void ft1000_write_dpram_mag_32(struct net_device *dev, int offset, u32 value);
 
@@ -60,7 +64,8 @@ static inline u16 ft1000_read_reg(struct net_device *dev, u16 offset)
 }
 
 /* Set the value of a given ASIC register. */
-static inline void ft1000_write_reg(struct net_device *dev, u16 offset, u16 value)
+static inline void ft1000_write_reg(struct net_device *dev, u16 offset,
+				    u16 value)
 {
 	outw(value, dev->base_addr + offset);
 }
diff --git a/drivers/staging/ft1000/ft1000-pcmcia/ft1000_cs.c b/drivers/staging/ft1000/ft1000-pcmcia/ft1000_cs.c
index f376ca43720..1f8b3ca35c6 100644
--- a/drivers/staging/ft1000/ft1000-pcmcia/ft1000_cs.c
+++ b/drivers/staging/ft1000/ft1000-pcmcia/ft1000_cs.c
@@ -95,20 +95,20 @@ static int ft1000_config(struct pcmcia_device *link)
 	/* setup IO window */
 	ret = pcmcia_loop_config(link, ft1000_confcheck, NULL);
 	if (ret) {
-		printk(KERN_INFO "ft1000: Could not configure pcmcia\n");
+		dev_err(&link->dev, "Could not configure pcmcia\n");
 		return -ENODEV;
 	}
 
 	/* configure device */
 	ret = pcmcia_enable_device(link);
 	if (ret) {
-		printk(KERN_INFO "ft1000: could not enable pcmcia\n");
+		dev_err(&link->dev, "Could not enable pcmcia\n");
 		goto failed;
 	}
 
 	link->priv = init_ft1000_card(link, &ft1000_reset);
 	if (!link->priv) {
-		printk(KERN_INFO "ft1000: Could not register as network device\n");
+		dev_err(&link->dev, "Could not register as network device\n");
 		goto failed;
 	}
 
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c b/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c
index ffdc7f597a9..a8945b78596 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c
@@ -417,8 +417,8 @@ static long ft1000_ioctl(struct file *file, unsigned int command,
     u16 tempword;
     unsigned long flags;
     struct timeval tv;
-    IOCTL_GET_VER get_ver_data;
-    IOCTL_GET_DSP_STAT get_stat_data;
+	struct IOCTL_GET_VER get_ver_data;
+	struct IOCTL_GET_DSP_STAT get_stat_data;
     u8 ConnectionMsg[] = {0x00,0x44,0x10,0x20,0x80,0x00,0x00,0x00,0x00,0x00,0x03,0x00,0x00,0x00,0x93,0x64,
                           0x00,0x00,0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x05,0x00,0x00,0x00,0x0a,
                           0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
@@ -527,8 +527,8 @@ static long ft1000_ioctl(struct file *file, unsigned int command,
         break;
     case IOCTL_SET_DPRAM_CMD:
         {
-            IOCTL_DPRAM_BLK *dpram_data = NULL;
-            /* IOCTL_DPRAM_COMMAND dpram_command; */
+		struct IOCTL_DPRAM_BLK *dpram_data = NULL;
+		/* struct IOCTL_DPRAM_COMMAND dpram_command; */
             u16 qtype;
             u16 msgsz;
 		struct pseudo_hdr *ppseudo_hdr;
@@ -672,7 +672,7 @@ static long ft1000_ioctl(struct file *file, unsigned int command,
     case IOCTL_GET_DPRAM_CMD:
         {
 		struct dpram_blk *pdpram_blk;
-            IOCTL_DPRAM_BLK __user *pioctl_dpram;
+		struct IOCTL_DPRAM_BLK __user *pioctl_dpram;
             int msglen;
 
             /* DEBUG("FT1000:ft1000_ioctl: IOCTL_FT1000_GET_DPRAM called\n"); */
@@ -785,7 +785,7 @@ static int ft1000_release(struct inode *inode, struct file *file)
 
     /* initialize application information */
     ft1000dev->appcnt--;
-    DEBUG("ft1000_chdev:%s:appcnt = %d\n", __FUNCTION__, ft1000dev->appcnt);
+    DEBUG("ft1000_chdev:%s:appcnt = %d\n", __func__, ft1000dev->appcnt);
     ft1000dev->app_info[i].fileobject = NULL;
 
     return 0;
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_download.c b/drivers/staging/ft1000/ft1000-usb/ft1000_download.c
index cab9cdf6273..65f98016713 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_download.c
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_download.c
@@ -522,7 +522,6 @@ static void usb_dnld_complete(struct urb *urb)
 static int write_blk_fifo(struct ft1000_usb *ft1000dev, u16 **pUsFile,
 			  u8 **pUcFile, long word_length)
 {
-	int Status = 0;
 	int byte_length;
 
 	byte_length = word_length * 4;
@@ -547,7 +546,7 @@ static int write_blk_fifo(struct ft1000_usb *ft1000dev, u16 **pUsFile,
 	*pUsFile = *pUsFile + (word_length << 1);
 	*pUcFile = *pUcFile + (word_length << 2);
 
-	return Status;
+	return 0;
 }
 
 static int scram_start_dwnld(struct ft1000_usb *ft1000dev, u16 *hshake,
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_ioctl.h b/drivers/staging/ft1000/ft1000-usb/ft1000_ioctl.h
index 419e534874f..cb644a58d9f 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_ioctl.h
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_ioctl.h
@@ -23,83 +23,101 @@
 * History:
 * 11/5/02    Whc                Created.
 *
-*---------------------------------------------------------------------------//---------------------------------------------------------------------------
+*---------------------------------------------------------------------------
 */
 #ifndef _FT1000IOCTLH_
 #define _FT1000IOCTLH_
 
-typedef struct _IOCTL_GET_VER {
-    unsigned long drv_ver;
-} __packed IOCTL_GET_VER, *PIOCTL_GET_VER;
+struct IOCTL_GET_VER {
+	unsigned long drv_ver;
+} __packed;
 
 /* Data structure for Dsp statistics */
-typedef struct _IOCTL_GET_DSP_STAT {
-    unsigned char DspVer[DSPVERSZ];        /* DSP version number */
-    unsigned char HwSerNum[HWSERNUMSZ];    /* Hardware Serial Number */
-    unsigned char Sku[SKUSZ];              /* SKU */
-    unsigned char eui64[EUISZ];            /* EUI64 */
-    unsigned short ConStat;                /* Connection Status */
-                                /*    Bits 0-3 = Connection Status Field */
-                                /*               0000=Idle (Disconnect) */
-                                /*               0001=Searching */
-                                /*               0010=Active (Connected) */
-                                /*               0011=Waiting for L2 down */
-                                /*               0100=Sleep */
-    unsigned short LedStat;                /* Led Status */
-                                /*    Bits 0-3   = Signal Strength Field */
-                                /*                 0000 = -105dBm to -92dBm */
-                                /*                 0001 = -92dBm to -85dBm */
-                                /*                 0011 = -85dBm to -75dBm */
-                                /*                 0111 = -75dBm to -50dBm */
-                                /*                 1111 = -50dBm to 0dBm */
-                                /*    Bits 4-7   = Reserved */
-                                /*    Bits 8-11  = SNR Field */
-                                /*                 0000 = <2dB */
-                                /*                 0001 = 2dB to 8dB */
-                                /*                 0011 = 8dB to 15dB */
-                                /*                 0111 = 15dB to 22dB */
-                                /*                 1111 = >22dB */
-                                /*    Bits 12-15 = Reserved */
-    unsigned long nTxPkts;                /* Number of packets transmitted from host to dsp */
-    unsigned long nRxPkts;                /* Number of packets received from dsp to host */
-    unsigned long nTxBytes;               /* Number of bytes transmitted from host to dsp */
-    unsigned long nRxBytes;               /* Number of bytes received from dsp to host */
-    unsigned long ConTm;                  /* Current session connection time in seconds */
-    unsigned char CalVer[CALVERSZ];       /* Proprietary Calibration Version */
-    unsigned char CalDate[CALDATESZ];     /* Proprietary Calibration Date */
-} __packed IOCTL_GET_DSP_STAT, *PIOCTL_GET_DSP_STAT;
+struct IOCTL_GET_DSP_STAT {
+	unsigned char DspVer[DSPVERSZ];        /* DSP version number */
+	unsigned char HwSerNum[HWSERNUMSZ];    /* Hardware Serial Number */
+	unsigned char Sku[SKUSZ];              /* SKU */
+	unsigned char eui64[EUISZ];            /* EUI64 */
+	unsigned short ConStat;                /* Connection Status */
+	/*    Bits 0-3 = Connection Status Field */
+	/*               0000=Idle (Disconnect) */
+	/*               0001=Searching */
+	/*               0010=Active (Connected) */
+	/*               0011=Waiting for L2 down */
+	/*               0100=Sleep */
+	unsigned short LedStat;                /* Led Status */
+	/*    Bits 0-3   = Signal Strength Field */
+	/*                 0000 = -105dBm to -92dBm */
+	/*                 0001 = -92dBm to -85dBm */
+	/*                 0011 = -85dBm to -75dBm */
+	/*                 0111 = -75dBm to -50dBm */
+	/*                 1111 = -50dBm to 0dBm */
+	/*    Bits 4-7   = Reserved */
+	/*    Bits 8-11  = SNR Field */
+	/*                 0000 = <2dB */
+	/*                 0001 = 2dB to 8dB */
+	/*                 0011 = 8dB to 15dB */
+	/*                 0111 = 15dB to 22dB */
+	/*                 1111 = >22dB */
+	/*    Bits 12-15 = Reserved */
+	unsigned long nTxPkts;                /* Number of packets transmitted
+					       * from host to dsp
+					       */
+	unsigned long nRxPkts;                /* Number of packets received from
+					       * dsp to host
+					       */
+	unsigned long nTxBytes;               /* Number of bytes transmitted
+					       * from host to dsp
+					       */
+	unsigned long nRxBytes;               /* Number of bytes received from
+					       * dsp to host
+					       */
+	unsigned long ConTm;                  /* Current session connection time
+					       * in seconds
+					       */
+	unsigned char CalVer[CALVERSZ];       /* Proprietary Calibration
+					       * Version
+					       */
+	unsigned char CalDate[CALDATESZ];     /* Proprietary Calibration Date */
+} __packed;
 
 /* Data structure for Dual Ported RAM messaging between Host and Dsp */
-typedef struct _IOCTL_DPRAM_BLK {
-    unsigned short total_len;
+struct IOCTL_DPRAM_BLK {
+	unsigned short total_len;
 	struct pseudo_hdr pseudohdr;
-    unsigned char buffer[1780];
-} __packed IOCTL_DPRAM_BLK, *PIOCTL_DPRAM_BLK;
+	unsigned char buffer[1780];
+} __packed;
 
-typedef struct _IOCTL_DPRAM_COMMAND {
-    unsigned short extra;
-    IOCTL_DPRAM_BLK dpram_blk;
-} __packed IOCTL_DPRAM_COMMAND, *PIOCTL_DPRAM_COMMAND;
+struct IOCTL_DPRAM_COMMAND {
+	unsigned short extra;
+	struct IOCTL_DPRAM_BLK dpram_blk;
+} __packed;
 
 /*
 * Custom IOCTL command codes
 */
 #define FT1000_MAGIC_CODE      'F'
 
-#define IOCTL_REGISTER_CMD					0
-#define IOCTL_SET_DPRAM_CMD					3
-#define IOCTL_GET_DPRAM_CMD					4
-#define IOCTL_GET_DSP_STAT_CMD      6
-#define IOCTL_GET_VER_CMD           7
-#define IOCTL_CONNECT               10
-#define IOCTL_DISCONNECT            11
+#define IOCTL_REGISTER_CMD	0
+#define IOCTL_SET_DPRAM_CMD	3
+#define IOCTL_GET_DPRAM_CMD	4
+#define IOCTL_GET_DSP_STAT_CMD	6
+#define IOCTL_GET_VER_CMD	7
+#define IOCTL_CONNECT		10
+#define IOCTL_DISCONNECT	11
 
-#define IOCTL_FT1000_GET_DSP_STAT _IOR(FT1000_MAGIC_CODE, IOCTL_GET_DSP_STAT_CMD, sizeof(IOCTL_GET_DSP_STAT)
-#define IOCTL_FT1000_GET_VER _IOR(FT1000_MAGIC_CODE, IOCTL_GET_VER_CMD, sizeof(IOCTL_GET_VER)
-#define IOCTL_FT1000_CONNECT _IOW(FT1000_MAGIC_CODE, IOCTL_CONNECT, 0
-#define IOCTL_FT1000_DISCONNECT _IOW(FT1000_MAGIC_CODE, IOCTL_DISCONNECT, 0
-#define IOCTL_FT1000_SET_DPRAM _IOW(FT1000_MAGIC_CODE, IOCTL_SET_DPRAM_CMD, sizeof(IOCTL_DPRAM_BLK)
-#define IOCTL_FT1000_GET_DPRAM _IOR(FT1000_MAGIC_CODE, IOCTL_GET_DPRAM_CMD, sizeof(IOCTL_DPRAM_BLK)
-#define IOCTL_FT1000_REGISTER  _IOW(FT1000_MAGIC_CODE, IOCTL_REGISTER_CMD, sizeof(unsigned short *)
-#endif /* _FT1000IOCTLH_ */
+#define IOCTL_FT1000_GET_DSP_STAT _IOR(FT1000_MAGIC_CODE,      \
+				       IOCTL_GET_DSP_STAT_CMD, \
+				       struct IOCTL_GET_DSP_STAT)
+#define IOCTL_FT1000_GET_VER _IOR(FT1000_MAGIC_CODE, IOCTL_GET_VER_CMD, \
+				  struct IOCTL_GET_VER)
+#define IOCTL_FT1000_CONNECT _IO(FT1000_MAGIC_CODE, IOCTL_CONNECT)
+#define IOCTL_FT1000_DISCONNECT _IO(FT1000_MAGIC_CODE, IOCTL_DISCONNECT)
+#define IOCTL_FT1000_SET_DPRAM _IOW(FT1000_MAGIC_CODE, IOCTL_SET_DPRAM_CMD, \
+				    struct IOCTL_DPRAM_BLK)
+#define IOCTL_FT1000_GET_DPRAM _IOR(FT1000_MAGIC_CODE, IOCTL_GET_DPRAM_CMD, \
+				    struct IOCTL_DPRAM_BLK)
+#define IOCTL_FT1000_REGISTER  _IOW(FT1000_MAGIC_CODE, IOCTL_REGISTER_CMD, \
+				    unsigned short *)
 
+#endif /* _FT1000IOCTLH_ */
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_proc.c b/drivers/staging/ft1000/ft1000-usb/ft1000_proc.c
index 2575d0d6bff..e89b5d2c8e6 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_proc.c
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_proc.c
@@ -32,17 +32,20 @@
 
 
 #define seq_putx(m, message, size, var) \
-	seq_printf(m, message);	\
-	for (i = 0; i < (size - 1); i++) \
-		seq_printf(m, "%02x:", var[i]); \
-	seq_printf(m, "%02x\n", var[i])
+	do { \
+		seq_printf(m, message);	\
+		for (i = 0; i < (size - 1); i++) \
+			seq_printf(m, "%02x:", var[i]); \
+		seq_printf(m, "%02x\n", var[i]);	\
+	} while (0)
 
 #define seq_putd(m, message, size, var) \
-	seq_printf(m, message); \
-	for (i = 0; i < (size - 1); i++) \
-		seq_printf(m, "%d.", var[i]); \
-	seq_printf(m, "%d\n", var[i])
-
+	do { \
+		seq_printf(m, message); \
+		for (i = 0; i < (size - 1); i++) \
+			seq_printf(m, "%d.", var[i]); \
+		seq_printf(m, "%d\n", var[i]);	      \
+	} while (0)
 
 #define FTNET_PROC init_net.proc_net
 
@@ -200,7 +203,7 @@ int ft1000_init_proc(struct net_device *dev)
 
 	info->ft1000_proc_dir = proc_mkdir(FT1000_PROC_DIR, FTNET_PROC);
 	if (info->ft1000_proc_dir == NULL) {
-		printk(KERN_WARNING "Unable to create %s dir.\n",
+		netdev_warn(dev, "Unable to create %s dir.\n",
 			FT1000_PROC_DIR);
 		goto fail;
 	}
@@ -210,7 +213,7 @@ int ft1000_init_proc(struct net_device *dev)
 				 &ft1000_proc_fops, dev);
 
 	if (!ft1000_proc_file) {
-		printk(KERN_WARNING "Unable to create /proc entry.\n");
+		netdev_warn(dev, "Unable to create /proc entry.\n");
 		goto fail_entry;
 	}
 
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_usb.c b/drivers/staging/ft1000/ft1000-usb/ft1000_usb.c
index e40763e60db..0a2544c7829 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_usb.c
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_usb.c
@@ -35,7 +35,7 @@ static struct usb_device_id id_table[] = {
 
 MODULE_DEVICE_TABLE(usb, id_table);
 
-static bool gPollingfailed = false;
+static bool gPollingfailed;
 static int ft1000_poll_thread(void *arg)
 {
 	int ret;
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_usb.h b/drivers/staging/ft1000/ft1000-usb/ft1000_usb.h
index a6fdd524ee6..2d4b02e2382 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_usb.h
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_usb.h
@@ -14,17 +14,21 @@
 struct app_info_block {
 	u32 nTxMsg;                    /* DPRAM msg sent to DSP with app_id */
 	u32 nRxMsg;                    /* DPRAM msg rcv from dsp with app_id */
-	u32 nTxMsgReject;              /* DPRAM msg rejected due to DSP doorbell set */
+	u32 nTxMsgReject;              /* DPRAM msg rejected due to DSP doorbell
+					* set
+					*/
 	u32 nRxMsgMiss;                /* DPRAM msg dropped due to overflow */
 	struct fown_struct *fileobject;/* Application's file object */
 	u16 app_id;                    /* Application id */
 	int DspBCMsgFlag;
 	int NumOfMsg;                   /* number of messages queued up */
 	wait_queue_head_t wait_dpram_msg;
-	struct list_head app_sqlist;   /* link list of msgs for applicaton on slow queue */
+	struct list_head app_sqlist;   /* link list of msgs for applicaton on
+					* slow queue
+					*/
 } __packed;
 
-#define DEBUG(args...) printk(KERN_INFO args)
+#define DEBUG(args...) pr_info(args)
 
 #define FALSE           0
 #define TRUE            1
@@ -41,7 +45,9 @@ struct app_info_block {
 /* MEMORY MAP FOR MAGNEMITE */
 /* the indexes are swapped comparing to PCMCIA - is it OK or a bug? */
 #undef FT1000_MAG_DSP_LED_INDX
-#define FT1000_MAG_DSP_LED_INDX		0x1	/* dsp led status for PAD device */
+#define FT1000_MAG_DSP_LED_INDX		0x1	/* dsp led status for PAD
+						 * device
+						 */
 #undef FT1000_MAG_DSP_CON_STATE_INDX
 #define FT1000_MAG_DSP_CON_STATE_INDX	0x0	/* DSP Connection Status Info */
 
@@ -99,21 +105,21 @@ struct dpram_blk {
 } __packed;
 
 int ft1000_read_register(struct ft1000_usb *ft1000dev,
-			u16 *Data, u16 nRegIndx);
+			 u16 *Data, u16 nRegIndx);
 int ft1000_write_register(struct ft1000_usb *ft1000dev,
-			u16 value, u16 nRegIndx);
+			  u16 value, u16 nRegIndx);
 int ft1000_read_dpram32(struct ft1000_usb *ft1000dev,
 			u16 indx, u8 *buffer, u16 cnt);
 int ft1000_write_dpram32(struct ft1000_usb *ft1000dev,
-			u16 indx, u8 *buffer, u16 cnt);
+			 u16 indx, u8 *buffer, u16 cnt);
 int ft1000_read_dpram16(struct ft1000_usb *ft1000dev,
 			u16 indx, u8 *buffer, u8 highlow);
 int ft1000_write_dpram16(struct ft1000_usb *ft1000dev,
-			u16 indx, u16 value, u8 highlow);
+			 u16 indx, u16 value, u8 highlow);
 int fix_ft1000_read_dpram32(struct ft1000_usb *ft1000dev,
-			u16 indx, u8 *buffer);
+			    u16 indx, u8 *buffer);
 int fix_ft1000_write_dpram32(struct ft1000_usb *ft1000dev,
-			u16 indx, u8 *buffer);
+			     u16 indx, u8 *buffer);
 
 extern void *pFileStart;
 extern size_t FileLength;
@@ -125,12 +131,13 @@ int scram_dnldr(struct ft1000_usb *ft1000dev, void *pFileStart,
 
 extern struct list_head freercvpool;
 
-extern spinlock_t free_buff_lock;   /* lock to arbitrate free buffer list for receive command data */
+/* lock to arbitrate free buffer list for receive command data */
+extern spinlock_t free_buff_lock;
 
 int ft1000_create_dev(struct ft1000_usb *dev);
 void ft1000_destroy_dev(struct net_device *dev);
 extern void card_send_command(struct ft1000_usb *ft1000dev,
-				void *ptempbuffer, int size);
+			      void *ptempbuffer, int size);
 
 struct dpram_blk *ft1000_get_buffer(struct list_head *bufflist);
 void ft1000_free_buffer(struct dpram_blk *pdpram_blk, struct list_head *plist);
@@ -139,12 +146,10 @@ int dsp_reload(struct ft1000_usb *ft1000dev);
 int init_ft1000_netdev(struct ft1000_usb *ft1000dev);
 struct usb_interface;
 int reg_ft1000_netdev(struct ft1000_usb *ft1000dev,
-			struct usb_interface *intf);
+		      struct usb_interface *intf);
 int ft1000_poll(void *dev_id);
 
 int ft1000_init_proc(struct net_device *dev);
 void ft1000_cleanup_proc(struct ft1000_info *info);
 
-
-
-#endif
+#endif  /* _FT1000_USB_H_ */
diff --git a/drivers/staging/ft1000/ft1000.h b/drivers/staging/ft1000/ft1000.h
index ccb821a1f37..db57430f3b9 100644
--- a/drivers/staging/ft1000/ft1000.h
+++ b/drivers/staging/ft1000/ft1000.h
@@ -1,5 +1,6 @@
 /*
- * Common structures and definitions for FT1000 Flarion Flash OFDM PCMCIA and USB devices
+ * Common structures and definitions for FT1000 Flarion Flash OFDM PCMCIA and
+ * USB devices.
  *
  * Originally copyright (c) 2002 Flarion Technologies
  *
@@ -17,7 +18,9 @@
 #define MAGNEMITE_ID	0x1a01	/* ASIC ID for Magnemite */
 
 /* MEMORY MAP common to both ELECTRABUZZ and MAGNEMITE */
-#define	FT1000_REG_DPRAM_ADDR	0x000E	/* DPADR - Dual Port Ram Indirect Address Register */
+#define	FT1000_REG_DPRAM_ADDR	0x000E	/* DPADR - Dual Port Ram Indirect
+					 * Address Register
+					 */
 #define	FT1000_REG_SUP_CTRL	0x0020	/* HCTR - Host Control Register */
 #define	FT1000_REG_SUP_STAT	0x0022	/* HSTAT - Host Status Register */
 #define	FT1000_REG_RESET	0x0024	/* HCTR - Host Control Register */
diff --git a/drivers/staging/fwserial/dma_fifo.c b/drivers/staging/fwserial/dma_fifo.c
index 5e846344550..02790624959 100644
--- a/drivers/staging/fwserial/dma_fifo.c
+++ b/drivers/staging/fwserial/dma_fifo.c
@@ -12,10 +12,6 @@
  * 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>
@@ -169,9 +165,9 @@ int dma_fifo_in(struct dma_fifo *fifo, const void *src, int n)
 	memcpy(fifo->data, src + l, n - l);
 
 	if (FAIL(fifo, addr_check(fifo->done, fifo->in, fifo->in + n) ||
-			fifo->avail < n,
-			"fifo corrupt: in:%u out:%u done:%u n:%d avail:%d",
-			fifo->in, fifo->out, fifo->done, n, fifo->avail))
+		 fifo->avail < n,
+		 "fifo corrupt: in:%u out:%u done:%u n:%d avail:%d",
+		 fifo->in, fifo->out, fifo->done, n, fifo->avail))
 		return -ENXIO;
 
 	fifo->in += n;
@@ -236,12 +232,12 @@ int dma_fifo_out_pend(struct dma_fifo *fifo, struct dma_pending *pended)
 	++fifo->open;
 
 	if (FAIL(fifo, fifo->open > fifo->open_limit,
-			"past open limit:%d (limit:%d)",
-			fifo->open, fifo->open_limit))
+		 "past open limit:%d (limit:%d)",
+		 fifo->open, fifo->open_limit))
 		return -ENXIO;
 	if (FAIL(fifo, fifo->out & (fifo->align - 1),
-			"fifo out unaligned:%u (align:%u)",
-			fifo->out, fifo->align))
+		 "fifo out unaligned:%u (align:%u)",
+		 fifo->out, fifo->align))
 		return -ENXIO;
 
 	return len - n;
@@ -264,8 +260,8 @@ int dma_fifo_out_complete(struct dma_fifo *fifo, struct dma_pending *complete)
 		return -EINVAL;
 
 	if (FAIL(fifo, list_empty(&fifo->pending) != (fifo->open == 0),
-			"pending list disagrees with open count:%d",
-			fifo->open))
+		 "pending list disagrees with open count:%d",
+		 fifo->open))
 		return -ENXIO;
 
 	tmp = complete->data;
@@ -282,10 +278,10 @@ int dma_fifo_out_complete(struct dma_fifo *fifo, struct dma_pending *complete)
 		}
 
 		if (FAIL(fifo, pending->out != fifo->done ||
-				addr_check(fifo->in, fifo->done, pending->next),
-				"in:%u out:%u done:%u saved:%u next:%u",
-				fifo->in, fifo->out, fifo->done, pending->out,
-				pending->next))
+			 addr_check(fifo->in, fifo->done, pending->next),
+			 "in:%u out:%u done:%u saved:%u next:%u",
+			 fifo->in, fifo->out, fifo->done, pending->out,
+			 pending->next))
 			return -ENXIO;
 
 		list_del_init(&pending->link);
@@ -300,7 +296,7 @@ int dma_fifo_out_complete(struct dma_fifo *fifo, struct dma_pending *complete)
 	if (FAIL(fifo, fifo->open < 0, "open dma:%d < 0", fifo->open))
 		return -ENXIO;
 	if (FAIL(fifo, fifo->avail > fifo->size, "fifo avail:%d > size:%d",
-			fifo->avail, fifo->size))
+		 fifo->avail, fifo->size))
 		return -ENXIO;
 
 	return 0;
diff --git a/drivers/staging/fwserial/dma_fifo.h b/drivers/staging/fwserial/dma_fifo.h
index a113fe1e6f1..410988224f8 100644
--- a/drivers/staging/fwserial/dma_fifo.h
+++ b/drivers/staging/fwserial/dma_fifo.h
@@ -12,10 +12,6 @@
  * 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 _DMA_FIFO_H_
@@ -85,15 +81,15 @@ static inline bool dp_is_completed(struct dma_pending *dp)
 	return (unsigned long)dp->data & 1UL;
 }
 
-extern void dma_fifo_init(struct dma_fifo *fifo);
-extern int dma_fifo_alloc(struct dma_fifo *fifo, int size, unsigned align,
-			  int tx_limit, int open_limit, gfp_t gfp_mask);
-extern void dma_fifo_free(struct dma_fifo *fifo);
-extern void dma_fifo_reset(struct dma_fifo *fifo);
-extern int dma_fifo_in(struct dma_fifo *fifo, const void *src, int n);
-extern int dma_fifo_out_pend(struct dma_fifo *fifo, struct dma_pending *pended);
-extern int dma_fifo_out_complete(struct dma_fifo *fifo,
-				 struct dma_pending *complete);
+void dma_fifo_init(struct dma_fifo *fifo);
+int dma_fifo_alloc(struct dma_fifo *fifo, int size, unsigned align,
+		   int tx_limit, int open_limit, gfp_t gfp_mask);
+void dma_fifo_free(struct dma_fifo *fifo);
+void dma_fifo_reset(struct dma_fifo *fifo);
+int dma_fifo_in(struct dma_fifo *fifo, const void *src, int n);
+int dma_fifo_out_pend(struct dma_fifo *fifo, struct dma_pending *pended);
+int dma_fifo_out_complete(struct dma_fifo *fifo,
+			  struct dma_pending *complete);
 
 /* returns the # of used bytes in the fifo */
 static inline int dma_fifo_level(struct dma_fifo *fifo)
diff --git a/drivers/staging/fwserial/fwserial.c b/drivers/staging/fwserial/fwserial.c
index b22142ee526..384758b11e3 100644
--- a/drivers/staging/fwserial/fwserial.c
+++ b/drivers/staging/fwserial/fwserial.c
@@ -12,10 +12,6 @@
  * 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.
  */
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
@@ -530,7 +526,7 @@ static void fwtty_emit_breaks(struct work_struct *work)
 	while (n) {
 		t = min(n, 16);
 		c = tty_insert_flip_string_fixed_flag(&port->port, buf,
-				TTY_BREAK, t);
+						      TTY_BREAK, t);
 		n -= c;
 		brk += c;
 		if (c < t)
@@ -638,9 +634,9 @@ static void fwtty_port_handler(struct fw_card *card,
 
 	switch (tcode) {
 	case TCODE_WRITE_QUADLET_REQUEST:
-		if (addr != port->rx_handler.offset || len != 4)
+		if (addr != port->rx_handler.offset || len != 4) {
 			rcode = RCODE_ADDRESS_ERROR;
-		else {
+		} else {
 			fwtty_update_port_status(port, *(unsigned *)data);
 			rcode = RCODE_COMPLETE;
 		}
@@ -741,7 +737,7 @@ static int fwtty_tx(struct fwtty_port *port, bool drain)
 	/* try to write as many dma transactions out as possible */
 	n = -EAGAIN;
 	while (!tty->stopped && !tty->hw_stopped &&
-			!test_bit(STOP_TX, &port->flags)) {
+	       !test_bit(STOP_TX, &port->flags)) {
 		txn = kmem_cache_alloc(fwtty_txn_cache, GFP_ATOMIC);
 		if (!txn) {
 			n = -ENOMEM;
@@ -756,11 +752,11 @@ static int fwtty_tx(struct fwtty_port *port, bool drain)
 
 		if (n < 0) {
 			kmem_cache_free(fwtty_txn_cache, txn);
-			if (n == -EAGAIN)
+			if (n == -EAGAIN) {
 				++port->stats.tx_stall;
-			else if (n == -ENODATA)
+			} else if (n == -ENODATA) {
 				fwtty_profile_data(port->stats.txns, 0);
-			else {
+			} else {
 				++port->stats.fifo_errs;
 				fwtty_err_ratelimited(port, "fifo err: %d\n",
 						      n);
@@ -884,7 +880,7 @@ static void fwserial_destroy(struct kref *kref)
 	for (j = 0; j < num_ports; ++i, ++j) {
 		port_table_corrupt |= port_table[i] != ports[j];
 		WARN_ONCE(port_table_corrupt, "port_table[%d]: %p != ports[%d]: %p",
-		     i, port_table[i], j, ports[j]);
+			  i, port_table[i], j, ports[j]);
 
 		port_table[i] = NULL;
 	}
@@ -1257,15 +1253,16 @@ static int set_serial_info(struct fwtty_port *port,
 		return -EFAULT;
 
 	if (tmp.irq != 0 || tmp.port != 0 || tmp.custom_divisor != 0 ||
-			tmp.baud_base != 400000000)
+	    tmp.baud_base != 400000000)
 		return -EPERM;
 
 	if (!capable(CAP_SYS_ADMIN)) {
 		if (((tmp.flags & ~ASYNC_USR_MASK) !=
 		     (port->port.flags & ~ASYNC_USR_MASK)))
 			return -EPERM;
-	} else
+	} else {
 		port->port.close_delay = tmp.close_delay * HZ / 100;
+	}
 
 	return 0;
 }
@@ -1308,9 +1305,9 @@ static void fwtty_set_termios(struct tty_struct *tty, struct ktermios *old)
 	spin_lock_bh(&port->lock);
 	baud = set_termios(port, tty);
 
-	if ((baud == 0) && (old->c_cflag & CBAUD))
+	if ((baud == 0) && (old->c_cflag & CBAUD)) {
 		port->mctrl &= ~(TIOCM_DTR | TIOCM_RTS);
-	else if ((baud != 0) && !(old->c_cflag & CBAUD)) {
+	} else if ((baud != 0) && !(old->c_cflag & CBAUD)) {
 		if (C_CRTSCTS(tty) || !test_bit(TTY_THROTTLED, &tty->flags))
 			port->mctrl |= TIOCM_DTR | TIOCM_RTS;
 		else
@@ -1733,8 +1730,9 @@ static inline int fwserial_send_mgmt_sync(struct fwtty_peer *peer,
 		    rcode == RCODE_GENERATION) {
 			fwtty_dbg(&peer->unit, "mgmt write error: %d\n", rcode);
 			continue;
-		} else
+		} else {
 			break;
+		}
 	} while (--tries > 0);
 	return rcode;
 }
@@ -1809,7 +1807,7 @@ static void fwserial_release_port(struct fwtty_port *port, bool reset)
 	port->max_payload = link_speed_to_max_payload(SCODE_100);
 	dma_fifo_change_tx_limit(&port->tx_fifo, port->max_payload);
 
-	rcu_assign_pointer(port->peer, NULL);
+	RCU_INIT_POINTER(port->peer, NULL);
 	spin_unlock_bh(&port->lock);
 
 	if (port->port.console && port->fwcon_ops->notify != NULL)
@@ -1818,7 +1816,7 @@ static void fwserial_release_port(struct fwtty_port *port, bool reset)
 
 static void fwserial_plug_timeout(unsigned long data)
 {
-	struct fwtty_peer *peer = (struct fwtty_peer *) data;
+	struct fwtty_peer *peer = (struct fwtty_peer *)data;
 	struct fwtty_port *port;
 
 	spin_lock_bh(&peer->lock);
@@ -2242,7 +2240,7 @@ static int fwserial_create(struct fw_unit *unit)
 		port->max_payload = link_speed_to_max_payload(SCODE_100);
 		dma_fifo_init(&port->tx_fifo);
 
-		rcu_assign_pointer(port->peer, NULL);
+		RCU_INIT_POINTER(port->peer, NULL);
 		serial->ports[i] = port;
 
 		/* get unique bus addr region for port's status & recv fifo */
@@ -2744,9 +2742,9 @@ static int fwserial_parse_mgmt_write(struct fwtty_peer *peer,
 		break;
 
 	case FWSC_VIRT_CABLE_UNPLUG_RSP:
-		if (peer->state != FWPS_UNPLUG_PENDING)
+		if (peer->state != FWPS_UNPLUG_PENDING) {
 			rcode = RCODE_CONFLICT_ERROR;
-		else {
+		} else {
 			if (be16_to_cpu(pkt->hdr.code) & FWSC_RSP_NACK)
 				fwtty_notice(&peer->unit, "NACK unplug?\n");
 			port = peer_revert_state(peer);
diff --git a/drivers/staging/gdm72xx/TODO b/drivers/staging/gdm72xx/TODO
index 5ab27fb2959..62d0cd6225c 100644
--- a/drivers/staging/gdm72xx/TODO
+++ b/drivers/staging/gdm72xx/TODO
@@ -1,3 +1,2 @@
 TODO:
-- Replace kernel_thread with kthread in gdm_usb.c
 - Clean up coding style to meet kernel standard.
diff --git a/drivers/staging/gdm72xx/gdm_qos.c b/drivers/staging/gdm72xx/gdm_qos.c
index 50d43ada093..df6f000534d 100644
--- a/drivers/staging/gdm72xx/gdm_qos.c
+++ b/drivers/staging/gdm72xx/gdm_qos.c
@@ -24,7 +24,7 @@
 #include "hci.h"
 #include "gdm_qos.h"
 
-#define B2H(x)		__be16_to_cpu(x)
+#define B2H(x)	__be16_to_cpu(x)
 
 #define MAX_FREE_LIST_CNT		32
 static struct {
@@ -48,7 +48,7 @@ static void *alloc_qos_entry(void)
 	spin_lock_irqsave(&qos_free_list.lock, flags);
 	if (qos_free_list.cnt) {
 		entry = list_entry(qos_free_list.head.prev, struct qos_entry_s,
-					list);
+				   list);
 		list_del(&entry->list);
 		qos_free_list.cnt--;
 		spin_unlock_irqrestore(&qos_free_list.lock, flags);
@@ -56,13 +56,13 @@ static void *alloc_qos_entry(void)
 	}
 	spin_unlock_irqrestore(&qos_free_list.lock, flags);
 
-	entry = kmalloc(sizeof(struct qos_entry_s), GFP_ATOMIC);
+	entry = kmalloc(sizeof(*entry), GFP_ATOMIC);
 	return entry;
 }
 
 static void free_qos_entry(void *entry)
 {
-	struct qos_entry_s *qentry = (struct qos_entry_s *) entry;
+	struct qos_entry_s *qentry = (struct qos_entry_s *)entry;
 	unsigned long flags;
 
 	spin_lock_irqsave(&qos_free_list.lock, flags);
@@ -142,24 +142,24 @@ void gdm_qos_release_list(void *nic_ptr)
 	free_qos_entry_list(&free_list);
 }
 
-static u32 chk_ipv4_rule(struct gdm_wimax_csr_s *csr, u8 *Stream, u8 *port)
+static u32 chk_ipv4_rule(struct gdm_wimax_csr_s *csr, u8 *stream, u8 *port)
 {
 	int i;
 
 	if (csr->classifier_rule_en&IPTYPEOFSERVICE) {
-		if (((Stream[1] & csr->ip2s_mask) < csr->ip2s_lo) ||
-		((Stream[1] & csr->ip2s_mask) > csr->ip2s_hi))
+		if (((stream[1] & csr->ip2s_mask) < csr->ip2s_lo) ||
+		    ((stream[1] & csr->ip2s_mask) > csr->ip2s_hi))
 			return 1;
 	}
 
 	if (csr->classifier_rule_en&PROTOCOL) {
-		if (Stream[9] != csr->protocol)
+		if (stream[9] != csr->protocol)
 			return 1;
 	}
 
 	if (csr->classifier_rule_en&IPMASKEDSRCADDRESS) {
 		for (i = 0; i < 4; i++) {
-			if ((Stream[12 + i] & csr->ipsrc_addrmask[i]) !=
+			if ((stream[12 + i] & csr->ipsrc_addrmask[i]) !=
 			(csr->ipsrc_addr[i] & csr->ipsrc_addrmask[i]))
 				return 1;
 		}
@@ -167,7 +167,7 @@ static u32 chk_ipv4_rule(struct gdm_wimax_csr_s *csr, u8 *Stream, u8 *port)
 
 	if (csr->classifier_rule_en&IPMASKEDDSTADDRESS) {
 		for (i = 0; i < 4; i++) {
-			if ((Stream[16 + i] & csr->ipdst_addrmask[i]) !=
+			if ((stream[16 + i] & csr->ipdst_addrmask[i]) !=
 			(csr->ipdst_addr[i] & csr->ipdst_addrmask[i]))
 				return 1;
 		}
@@ -190,25 +190,24 @@ static u32 chk_ipv4_rule(struct gdm_wimax_csr_s *csr, u8 *Stream, u8 *port)
 
 static u32 get_qos_index(struct nic *nic, u8 *iph, u8 *tcpudph)
 {
-	u32	IP_Ver, Header_Len, i;
+	u32 IP_ver, i;
 	struct qos_cb_s *qcb = &nic->qos;
 
 	if (iph == NULL || tcpudph == NULL)
 		return -1;
 
-	IP_Ver = (iph[0]>>4)&0xf;
-	Header_Len = iph[0]&0xf;
+	IP_ver = (iph[0]>>4)&0xf;
 
-	if (IP_Ver == 4) {
-		for (i = 0; i < QOS_MAX; i++) {
-			if (qcb->csr[i].enabled) {
-				if (qcb->csr[i].classifier_rule_en) {
-					if (chk_ipv4_rule(&qcb->csr[i], iph,
-					tcpudph) == 0)
-						return i;
-				}
-			}
-		}
+	if (IP_ver != 4)
+		return -1;
+
+	for (i = 0; i < QOS_MAX; i++) {
+		if (!qcb->csr[i].enabled)
+			continue;
+		if (!qcb->csr[i].classifier_rule_en)
+			continue;
+		if (chk_ipv4_rule(&qcb->csr[i], iph, tcpudph) == 0)
+			return i;
 	}
 
 	return -1;
@@ -223,22 +222,21 @@ static u32 extract_qos_list(struct nic *nic, struct list_head *head)
 	INIT_LIST_HEAD(head);
 
 	for (i = 0; i < QOS_MAX; i++) {
-		if (qcb->csr[i].enabled) {
-			if (qcb->csr[i].qos_buf_count < qcb->qos_limit_size) {
-				if (!list_empty(&qcb->qos_list[i])) {
-					entry = list_entry(
-					qcb->qos_list[i].prev,
-					struct qos_entry_s, list);
-					list_move_tail(&entry->list, head);
-					qcb->csr[i].qos_buf_count++;
-
-					if (!list_empty(&qcb->qos_list[i]))
-						netdev_warn(nic->netdev,
-							    "Index(%d) is piled!!\n",
-							    i);
-				}
-			}
-		}
+		if (!qcb->csr[i].enabled)
+			continue;
+		if (qcb->csr[i].qos_buf_count >= qcb->qos_limit_size)
+			continue;
+		if (list_empty(&qcb->qos_list[i]))
+			continue;
+
+		entry = list_entry(qcb->qos_list[i].prev, struct qos_entry_s,
+				   list);
+
+		list_move_tail(&entry->list, head);
+		qcb->csr[i].qos_buf_count++;
+
+		if (!list_empty(&qcb->qos_list[i]))
+			netdev_warn(nic->netdev, "Index(%d) is piled!!\n", i);
 	}
 
 	return 0;
@@ -261,14 +259,14 @@ int gdm_qos_send_hci_pkt(struct sk_buff *skb, struct net_device *dev)
 	int index;
 	struct qos_cb_s *qcb = &nic->qos;
 	unsigned long flags;
-	struct ethhdr *ethh = (struct ethhdr *) (skb->data + HCI_HEADER_SIZE);
-	struct iphdr *iph = (struct iphdr *) ((char *) ethh + ETH_HLEN);
+	struct ethhdr *ethh = (struct ethhdr *)(skb->data + HCI_HEADER_SIZE);
+	struct iphdr *iph = (struct iphdr *)((char *)ethh + ETH_HLEN);
 	struct tcphdr *tcph;
 	struct qos_entry_s *entry = NULL;
 	struct list_head send_list;
 	int ret = 0;
 
-	tcph = (struct tcphdr *) iph + iph->ihl*4;
+	tcph = (struct tcphdr *)iph + iph->ihl*4;
 
 	if (B2H(ethh->h_proto) == ETH_P_IP) {
 		if (qcb->qos_list_cnt && !qos_free_list.cnt) {
@@ -281,7 +279,7 @@ int gdm_qos_send_hci_pkt(struct sk_buff *skb, struct net_device *dev)
 
 		spin_lock_irqsave(&qcb->qos_lock, flags);
 		if (qcb->qos_list_cnt) {
-			index = get_qos_index(nic, (u8 *)iph, (u8 *) tcph);
+			index = get_qos_index(nic, (u8 *)iph, (u8 *)tcph);
 			if (index == -1)
 				index = qcb->qos_null_idx;
 
@@ -336,16 +334,16 @@ 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 sub_cmd_evt;
 	struct qos_cb_s *qcb = &nic->qos;
 	struct qos_entry_s *entry, *n;
 	struct list_head send_list;
 	struct list_head free_list;
 	unsigned long flags;
 
-	subCmdEvt = (u8)buf[4];
+	sub_cmd_evt = (u8)buf[4];
 
-	if (subCmdEvt == QOS_REPORT) {
+	if (sub_cmd_evt == QOS_REPORT) {
 		spin_lock_irqsave(&qcb->qos_lock, flags);
 		for (i = 0; i < qcb->qos_list_cnt; i++) {
 			SFID = ((buf[(i*5)+6]<<24)&0xff000000);
@@ -367,7 +365,7 @@ void gdm_recv_qos_hci_packet(void *nic_ptr, u8 *buf, int size)
 		return;
 	}
 
-	/* subCmdEvt == QOS_ADD || subCmdEvt == QOS_CHANG_DEL */
+	/* sub_cmd_evt == QOS_ADD || sub_cmd_evt == QOS_CHANG_DEL */
 	pos = 6;
 	SFID = ((buf[pos++]<<24)&0xff000000);
 	SFID += ((buf[pos++]<<16)&0xff0000);
@@ -377,12 +375,12 @@ void gdm_recv_qos_hci_packet(void *nic_ptr, u8 *buf, int size)
 	index = get_csr(qcb, SFID, 1);
 	if (index == -1) {
 		netdev_err(nic->netdev,
-			   "QoS ERROR: csr Update Error / Wrong index (%d) \n",
+			   "QoS ERROR: csr Update Error / Wrong index (%d)\n",
 			   index);
 		return;
 	}
 
-	if (subCmdEvt == QOS_ADD) {
+	if (sub_cmd_evt == QOS_ADD) {
 		netdev_dbg(nic->netdev, "QOS_ADD SFID = 0x%x, index=%d\n",
 			   SFID, index);
 
@@ -423,7 +421,7 @@ 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) {
+	} else if (sub_cmd_evt == QOS_CHANGE_DEL) {
 		netdev_dbg(nic->netdev, "QOS_CHANGE_DEL SFID = 0x%x, index=%d\n",
 			   SFID, index);
 
@@ -435,7 +433,7 @@ void gdm_recv_qos_hci_packet(void *nic_ptr, u8 *buf, int size)
 		qcb->qos_limit_size = 254/qcb->qos_list_cnt;
 
 		list_for_each_entry_safe(entry, n, &qcb->qos_list[index],
-					list) {
+					 list) {
 			list_move_tail(&entry->list, &free_list);
 		}
 		spin_unlock_irqrestore(&qcb->qos_lock, flags);
diff --git a/drivers/staging/gdm72xx/gdm_qos.h b/drivers/staging/gdm72xx/gdm_qos.h
index 8f18119d22a..6543cff2a87 100644
--- a/drivers/staging/gdm72xx/gdm_qos.h
+++ b/drivers/staging/gdm72xx/gdm_qos.h
@@ -34,23 +34,6 @@
 #define	IEEE802_1QVLANID		0x10
 
 struct gdm_wimax_csr_s {
-	/*	union{
-		U16 all;
-		struct _CS_CLASSIFIER_RULE_ENABLE{
-			IPTypeOfService:1,
-			Protocol:1,
-			IPMaskedSrcAddress:1,
-			IPMaskedDstAddress:1,
-			ProtocolSrcPortRange:1,
-			ProtocolDstPortRange:1,
-			DstMacAddr:1,
-			SrcMacAddr:1,
-			Ethertype:1,
-			IEEE802_1DUserPriority:1,
-			IEEE802_1QVLANID:1,
-			Reserved:5;
-		} fields;
-	} */
 	BOOLEAN		enabled;
 	u32		SFID;
 	u8		qos_buf_count;
diff --git a/drivers/staging/gdm72xx/gdm_sdio.c b/drivers/staging/gdm72xx/gdm_sdio.c
index c24653739e1..7398d451ccc 100644
--- a/drivers/staging/gdm72xx/gdm_sdio.c
+++ b/drivers/staging/gdm72xx/gdm_sdio.c
@@ -31,11 +31,11 @@
 #define MAX_NR_RX_BUF	4
 
 #define SDU_TX_BUF_SIZE	2048
-#define TX_BUF_SIZE		2048
+#define TX_BUF_SIZE	2048
 #define TX_CHUNK_SIZE	(2048 - TYPE_A_HEADER_SIZE)
-#define RX_BUF_SIZE		(25*1024)
+#define RX_BUF_SIZE	(25*1024)
 
-#define TX_HZ	2000
+#define TX_HZ		2000
 #define TX_INTERVAL	(1000000/TX_HZ)
 
 static int init_sdio(struct sdiowm_dev *sdev);
@@ -127,10 +127,10 @@ static void put_rx_struct(struct rx_cxt *rx, struct sdio_rx *r)
 static int init_sdio(struct sdiowm_dev *sdev)
 {
 	int ret = 0, i;
-	struct tx_cxt	*tx = &sdev->tx;
-	struct rx_cxt	*rx = &sdev->rx;
-	struct sdio_tx	*t;
-	struct sdio_rx	*r;
+	struct tx_cxt *tx = &sdev->tx;
+	struct rx_cxt *rx = &sdev->rx;
+	struct sdio_tx *t;
+	struct sdio_rx *r;
 
 	INIT_LIST_HEAD(&tx->free_list);
 	INIT_LIST_HEAD(&tx->sdu_list);
@@ -313,7 +313,7 @@ static void send_sdu(struct sdio_func *func, struct tx_cxt *tx)
 }
 
 static void send_hci(struct sdio_func *func, struct tx_cxt *tx,
-			struct sdio_tx *t)
+		     struct sdio_tx *t)
 {
 	unsigned long flags;
 
@@ -380,7 +380,7 @@ static void do_tx(struct work_struct *work)
 }
 
 static int gdm_sdio_send(void *priv_dev, void *data, int len,
-			void (*cb)(void *data), void *cb_data)
+			 void (*cb)(void *data), void *cb_data)
 {
 	struct sdiowm_dev *sdev = priv_dev;
 	struct tx_cxt *tx = &sdev->tx;
@@ -510,6 +510,7 @@ static void gdm_sdio_irq(struct sdio_func *func)
 
 	if (hdr[3] == 1) {	/* Ack */
 		u32 *ack_seq = (u32 *)&hdr[4];
+
 		spin_lock_irqsave(&tx->lock, flags);
 		tx->can_send = 1;
 
@@ -521,7 +522,7 @@ static void gdm_sdio_irq(struct sdio_func *func)
 	}
 
 	memcpy(rx->rx_buf, hdr + TYPE_A_HEADER_SIZE,
-			TYPE_A_LOOKAHEAD_SIZE - TYPE_A_HEADER_SIZE);
+	       TYPE_A_LOOKAHEAD_SIZE - TYPE_A_HEADER_SIZE);
 
 	buf = rx->rx_buf + TYPE_A_LOOKAHEAD_SIZE - TYPE_A_HEADER_SIZE;
 	remain = len - TYPE_A_LOOKAHEAD_SIZE + TYPE_A_HEADER_SIZE;
@@ -577,8 +578,8 @@ done:
 }
 
 static int gdm_sdio_receive(void *priv_dev,
-				void (*cb)(void *cb_data, void *data, int len),
-				void *cb_data)
+			    void (*cb)(void *cb_data, void *data, int len),
+			    void *cb_data)
 {
 	struct sdiowm_dev *sdev = priv_dev;
 	struct rx_cxt *rx = &sdev->rx;
@@ -602,7 +603,7 @@ static int gdm_sdio_receive(void *priv_dev,
 }
 
 static int sdio_wimax_probe(struct sdio_func *func,
-				const struct sdio_device_id *id)
+			    const struct sdio_device_id *id)
 {
 	int ret;
 	struct phy_dev *phy_dev = NULL;
diff --git a/drivers/staging/gdm72xx/gdm_sdio.h b/drivers/staging/gdm72xx/gdm_sdio.h
index 216e98f31ba..0c0e2cbb727 100644
--- a/drivers/staging/gdm72xx/gdm_sdio.h
+++ b/drivers/staging/gdm72xx/gdm_sdio.h
@@ -22,10 +22,8 @@
 struct sdio_tx {
 	struct list_head	list;
 	struct tx_cxt		*tx_cxt;
-
-	u8	*buf;
-	int	len;
-
+	u8			*buf;
+	int			len;
 	void (*callback)(void *cb_data);
 	void *cb_data;
 };
@@ -35,18 +33,15 @@ struct tx_cxt {
 	struct list_head	sdu_list;
 	struct list_head	hci_list;
 	struct timeval		sdu_stamp;
-
-	u8	*sdu_buf;
-
-	spinlock_t			lock;
-	int	can_send;
-	int stop_sdu_tx;
+	u8			*sdu_buf;
+	spinlock_t		lock;
+	int			can_send;
+	int			stop_sdu_tx;
 };
 
 struct sdio_rx {
 	struct list_head	list;
 	struct rx_cxt		*rx_cxt;
-
 	void (*callback)(void *cb_data, void *data, int len);
 	void *cb_data;
 };
@@ -54,18 +49,14 @@ struct sdio_rx {
 struct rx_cxt {
 	struct list_head	free_list;
 	struct list_head	req_list;
-
-	u8		*rx_buf;
-
-	spinlock_t			lock;
+	u8			*rx_buf;
+	spinlock_t		lock;
 };
 
 struct sdiowm_dev {
 	struct sdio_func	*func;
-
-	struct tx_cxt	tx;
-	struct rx_cxt	rx;
-
+	struct tx_cxt		tx;
+	struct rx_cxt		rx;
 	struct work_struct	ws;
 };
 
diff --git a/drivers/staging/gdm72xx/gdm_usb.c b/drivers/staging/gdm72xx/gdm_usb.c
index 20539d80939..78d6667fa0d 100644
--- a/drivers/staging/gdm72xx/gdm_usb.c
+++ b/drivers/staging/gdm72xx/gdm_usb.c
@@ -27,6 +27,7 @@
 MODULE_DEVICE_TABLE(usb, id_table);
 
 #define TX_BUF_SIZE		2048
+
 #if defined(CONFIG_WIMAX_GDM72XX_WIMAX2)
 #define RX_BUF_SIZE		(128*1024)	/* For packet aggregation */
 #else
@@ -166,10 +167,10 @@ static void put_rx_struct(struct rx_cxt *rx, struct usb_rx *r)
 static int init_usb(struct usbwm_dev *udev)
 {
 	int ret = 0, i;
-	struct tx_cxt	*tx = &udev->tx;
-	struct rx_cxt	*rx = &udev->rx;
-	struct usb_tx	*t;
-	struct usb_rx	*r;
+	struct tx_cxt *tx = &udev->tx;
+	struct rx_cxt *rx = &udev->rx;
+	struct usb_tx *t;
+	struct usb_rx *r;
 	unsigned long flags;
 
 	INIT_LIST_HEAD(&tx->free_list);
@@ -215,10 +216,10 @@ fail:
 
 static void release_usb(struct usbwm_dev *udev)
 {
-	struct tx_cxt	*tx = &udev->tx;
-	struct rx_cxt	*rx = &udev->rx;
-	struct usb_tx	*t, *t_next;
-	struct usb_rx	*r, *r_next;
+	struct tx_cxt *tx = &udev->tx;
+	struct rx_cxt *rx = &udev->rx;
+	struct usb_tx *t, *t_next;
+	struct usb_rx *r, *r_next;
 	unsigned long flags;
 
 	spin_lock_irqsave(&tx->lock, flags);
@@ -344,16 +345,11 @@ static int gdm_usb_send(void *priv_dev, void *data, int len,
 	if ((len % 512) == 0)
 		len++;
 
-	usb_fill_bulk_urb(t->urb,
-			usbdev,
-			usb_sndbulkpipe(usbdev, 1),
-			t->buf,
-			len + padding,
-			gdm_usb_send_complete,
-			t);
+	usb_fill_bulk_urb(t->urb, usbdev, usb_sndbulkpipe(usbdev, 1), t->buf,
+			  len + padding, gdm_usb_send_complete, t);
 
-	print_hex_dump_debug("usb_send: ", DUMP_PREFIX_NONE, 16, 1,
-			     t->buf, len + padding, false);
+	print_hex_dump_debug("usb_send: ", DUMP_PREFIX_NONE, 16, 1, t->buf,
+			     len + padding, false);
 #ifdef CONFIG_WIMAX_GDM72XX_USB_PM
 	if (usbdev->state & USB_STATE_SUSPENDED) {
 		list_add_tail(&t->p_list, &tx->pending_list);
@@ -470,8 +466,8 @@ static void gdm_usb_rcv_complete(struct urb *urb)
 }
 
 static int gdm_usb_receive(void *priv_dev,
-			void (*cb)(void *cb_data, void *data, int len),
-			void *cb_data)
+			   void (*cb)(void *cb_data, void *data, int len),
+			   void *cb_data)
 {
 	struct usbwm_dev *udev = priv_dev;
 	struct usb_device *usbdev = udev->usbdev;
@@ -494,13 +490,8 @@ static int gdm_usb_receive(void *priv_dev,
 	r->callback = cb;
 	r->cb_data = cb_data;
 
-	usb_fill_bulk_urb(r->urb,
-			usbdev,
-			usb_rcvbulkpipe(usbdev, 0x82),
-			r->buf,
-			RX_BUF_SIZE,
-			gdm_usb_rcv_complete,
-			r);
+	usb_fill_bulk_urb(r->urb, usbdev, usb_rcvbulkpipe(usbdev, 0x82), r->buf,
+			  RX_BUF_SIZE, gdm_usb_rcv_complete, r);
 
 	return usb_submit_urb(r->urb, GFP_ATOMIC);
 }
@@ -518,8 +509,8 @@ static void do_pm_control(struct work_struct *work)
 		usb_autopm_put_interface(udev->intf);
 
 	spin_lock_irqsave(&tx->lock, flags);
-	if (!(udev->usbdev->state & USB_STATE_SUSPENDED)
-		&& (!list_empty(&tx->hci_list) || !list_empty(&tx->sdu_list))) {
+	if (!(udev->usbdev->state & USB_STATE_SUSPENDED) &&
+	    (!list_empty(&tx->hci_list) || !list_empty(&tx->sdu_list))) {
 		struct usb_tx *t, *temp;
 
 		list_for_each_entry_safe(t, temp, &tx->pending_list, p_list) {
@@ -537,7 +528,7 @@ static void do_pm_control(struct work_struct *work)
 #endif /* CONFIG_WIMAX_GDM72XX_USB_PM */
 
 static int gdm_usb_probe(struct usb_interface *intf,
-				const struct usb_device_id *id)
+			 const struct usb_device_id *id)
 {
 	int ret = 0;
 	u8 bConfigurationValue;
@@ -567,7 +558,7 @@ static int gdm_usb_probe(struct usb_interface *intf,
 
 	/* Support for EEPROM bootloader */
 	if (bConfigurationValue == DOWNLOAD_CONF_VALUE ||
-		idProduct & B_DOWNLOAD) {
+	    idProduct & B_DOWNLOAD) {
 		ret = usb_boot(usbdev, bcdDevice);
 		goto out;
 	}
@@ -639,8 +630,9 @@ static void gdm_usb_disconnect(struct usb_interface *intf)
 	idProduct = L2H(usbdev->descriptor.idProduct);
 
 	if (idProduct != EMERGENCY_PID &&
-			bConfigurationValue != DOWNLOAD_CONF_VALUE &&
-			(idProduct & B_DOWNLOAD) == 0) {
+	    bConfigurationValue != DOWNLOAD_CONF_VALUE &&
+	    (idProduct & B_DOWNLOAD) == 0) {
+
 		udev = phy_dev->priv_dev;
 		udev->usbdev = NULL;
 
@@ -730,7 +722,7 @@ static int k_mode_thread(void *arg)
 			spin_unlock_irqrestore(&k_lock, flags2);
 
 			expire = jiffies + K_WAIT_TIME;
-			while (jiffies < expire)
+			while (time_before(jiffies, expire))
 				schedule_timeout(K_WAIT_TIME);
 
 			spin_lock_irqsave(&rx->lock, flags);
@@ -743,7 +735,7 @@ static int k_mode_thread(void *arg)
 			spin_lock_irqsave(&tx->lock, flags);
 
 			list_for_each_entry_safe(t, temp, &tx->pending_list,
-						p_list) {
+						 p_list) {
 				list_del(&t->p_list);
 				ret = usb_submit_urb(t->urb, GFP_ATOMIC);
 
@@ -759,8 +751,8 @@ static int k_mode_thread(void *arg)
 			spin_lock_irqsave(&k_lock, flags2);
 		}
 		wait_event_interruptible_lock_irq(k_wait,
-						  !list_empty(&k_list) || k_mode_stop,
-						  k_lock);
+						  !list_empty(&k_list) ||
+						  k_mode_stop, k_lock);
 		spin_unlock_irqrestore(&k_lock, flags2);
 	}
 	return 0;
diff --git a/drivers/staging/gdm72xx/gdm_usb.h b/drivers/staging/gdm72xx/gdm_usb.h
index f2c54511bb9..30506529a8c 100644
--- a/drivers/staging/gdm72xx/gdm_usb.h
+++ b/drivers/staging/gdm72xx/gdm_usb.h
@@ -28,12 +28,10 @@ struct usb_tx {
 	struct list_head	p_list;
 #endif
 	struct tx_cxt		*tx_cxt;
-
 	struct urb		*urb;
 	u8			*buf;
-
 	void (*callback)(void *cb_data);
-	void *cb_data;
+	void			*cb_data;
 };
 
 struct tx_cxt {
@@ -43,17 +41,14 @@ struct tx_cxt {
 #if defined(CONFIG_WIMAX_GDM72XX_USB_PM) || defined(CONFIG_WIMAX_GDM72XX_K_MODE)
 	struct list_head	pending_list;
 #endif
-
 	spinlock_t		lock;
 };
 
 struct usb_rx {
 	struct list_head	list;
 	struct rx_cxt		*rx_cxt;
-
 	struct urb		*urb;
 	u8			*buf;
-
 	void (*callback)(void *cb_data, void *data, int len);
 	void *cb_data;
 };
@@ -75,11 +70,9 @@ struct usbwm_dev {
 	int bw_switch;
 	struct list_head	list;
 #endif
-
 	struct tx_cxt		tx;
 	struct rx_cxt		rx;
-
-	int padding;
+	int			padding;
 };
 
 #endif /* __GDM_USB_H__ */
diff --git a/drivers/staging/gdm72xx/gdm_wimax.c b/drivers/staging/gdm72xx/gdm_wimax.c
index 05ce2a22c22..e5e51158512 100644
--- a/drivers/staging/gdm72xx/gdm_wimax.c
+++ b/drivers/staging/gdm72xx/gdm_wimax.c
@@ -35,10 +35,10 @@
 #define EVT_MAX_SIZE	2048
 
 struct evt_entry {
-	struct list_head list;
-	struct net_device *dev;
-	char evt_data[EVT_MAX_SIZE];
-	int	 size;
+	struct	list_head list;
+	struct	net_device *dev;
+	char	evt_data[EVT_MAX_SIZE];
+	int	size;
 };
 
 static void __gdm_wimax_event_send(struct work_struct *work);
@@ -52,7 +52,6 @@ static struct {
 	struct sock *sock;
 	struct list_head evtq;
 	spinlock_t evt_lock;
-
 	struct list_head freeq;
 	struct work_struct ws;
 } wm_event;
@@ -135,24 +134,24 @@ static void dump_eth_packet(struct net_device *dev, const char *title,
 	u16 port = 0;
 
 	protocol = (data[12]<<8) | data[13];
-	ih = (struct iphdr *) (data+ETH_HLEN);
+	ih = (struct iphdr *)(data+ETH_HLEN);
 
 	if (protocol == ETH_P_IP) {
-		uh = (struct udphdr *) ((char *)ih + sizeof(struct iphdr));
+		uh = (struct udphdr *)((char *)ih + sizeof(struct iphdr));
 		ip_protocol = ih->protocol;
 		port = ntohs(uh->dest);
 	} else if (protocol == ETH_P_IPV6) {
-		struct ipv6hdr *i6h = (struct ipv6hdr *) data;
-		uh = (struct udphdr *) ((char *)i6h + sizeof(struct ipv6hdr));
+		struct ipv6hdr *i6h = (struct ipv6hdr *)data;
+
+		uh = (struct udphdr *)((char *)i6h + sizeof(struct ipv6hdr));
 		ip_protocol = i6h->nexthdr;
 		port = ntohs(uh->dest);
 	}
 
-	netdev_dbg(dev, "[%s] len=%d, %s, %s, %s\n",
-		title, len,
-		get_protocol_name(protocol),
-		get_ip_protocol_name(ip_protocol),
-		get_port_name(port));
+	netdev_dbg(dev, "[%s] len=%d, %s, %s, %s\n", title, len,
+		   get_protocol_name(protocol),
+		   get_ip_protocol_name(ip_protocol),
+		   get_port_name(port));
 
 	if (!(data[0] == 0xff && data[1] == 0xff)) {
 		if (protocol == ETH_P_IP)
@@ -168,7 +167,6 @@ static inline int gdm_wimax_header(struct sk_buff **pskb)
 {
 	u16 buf[HCI_HEADER_SIZE / sizeof(u16)];
 	struct sk_buff *skb = *pskb;
-	int ret = 0;
 
 	if (unlikely(skb_headroom(skb) < HCI_HEADER_SIZE)) {
 		struct sk_buff *skb2;
@@ -188,7 +186,7 @@ static inline int gdm_wimax_header(struct sk_buff **pskb)
 	memcpy(skb->data, buf, HCI_HEADER_SIZE);
 
 	*pskb = skb;
-	return ret;
+	return 0;
 }
 
 static void gdm_wimax_event_rcv(struct net_device *dev, u16 type, void *msg,
@@ -196,9 +194,10 @@ static void gdm_wimax_event_rcv(struct net_device *dev, u16 type, void *msg,
 {
 	struct nic *nic = netdev_priv(dev);
 
-	u8 *buf = (u8 *) msg;
+	u8 *buf = (u8 *)msg;
 	u16 hci_cmd =  (buf[0]<<8) | buf[1];
 	u16 hci_len = (buf[2]<<8) | buf[3];
+
 	netdev_dbg(dev, "H=>D: 0x%04x(%d)\n", hci_cmd, hci_len);
 
 	gdm_wimax_send(nic, msg, len);
@@ -263,9 +262,9 @@ static struct evt_entry *get_event_entry(void)
 {
 	struct evt_entry *e;
 
-	if (list_empty(&wm_event.freeq))
+	if (list_empty(&wm_event.freeq)) {
 		e = alloc_event_entry();
-	else {
+	} else {
 		e = list_entry(wm_event.freeq.next, struct evt_entry, list);
 		list_del(&e->list);
 	}
@@ -310,6 +309,7 @@ static int gdm_wimax_event_send(struct net_device *dev, char *buf, int size)
 
 	u16 hci_cmd =  ((u8)buf[0]<<8) | (u8)buf[1];
 	u16 hci_len = ((u8)buf[2]<<8) | (u8)buf[3];
+
 	netdev_dbg(dev, "D=>H: 0x%04x(%d)\n", hci_cmd, hci_len);
 
 	spin_lock_irqsave(&wm_event.evt_lock, flags);
@@ -347,7 +347,7 @@ int gdm_wimax_send_tx(struct sk_buff *skb, struct net_device *dev)
 	struct nic *nic = netdev_priv(dev);
 
 	ret = gdm_wimax_send_with_cb(nic, skb->data, skb->len, tx_complete,
-					nic);
+				     nic);
 	if (ret == -ENOSPC) {
 		netif_stop_queue(dev);
 		ret = 0;
@@ -368,7 +368,7 @@ static int gdm_wimax_tx(struct sk_buff *skb, struct net_device *dev)
 {
 	int ret = 0;
 	struct nic *nic = netdev_priv(dev);
-	struct fsm_s *fsm = (struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf;
+	struct fsm_s *fsm = (struct fsm_s *)nic->sdk_data[SIOC_DATA_FSM].buf;
 
 	dump_eth_packet(dev, "TX", skb->data, skb->len);
 
@@ -379,9 +379,9 @@ static int gdm_wimax_tx(struct sk_buff *skb, struct net_device *dev)
 	}
 
 	#if !defined(LOOPBACK_TEST)
-	if (!fsm)
+	if (!fsm) {
 		netdev_err(dev, "ASSERTION ERROR: fsm is NULL!!\n");
-	else if (fsm->m_status != M_CONNECTED) {
+	} else if (fsm->m_status != M_CONNECTED) {
 		netdev_emerg(dev, "ASSERTION ERROR: Device is NOT ready. status=%d\n",
 			     fsm->m_status);
 		kfree_skb(skb);
@@ -408,7 +408,7 @@ static int gdm_wimax_set_config(struct net_device *dev, struct ifmap *map)
 static void __gdm_wimax_set_mac_addr(struct net_device *dev, char *mac_addr)
 {
 	u16 hci_pkt_buf[32 / sizeof(u16)];
-	u8 *pkt = (u8 *) &hci_pkt_buf[0];
+	u8 *pkt = (u8 *)&hci_pkt_buf[0];
 	struct nic *nic = netdev_priv(dev);
 
 	/* Since dev is registered as a ethernet device,
@@ -454,7 +454,7 @@ static struct net_device_stats *gdm_wimax_stats(struct net_device *dev)
 static int gdm_wimax_open(struct net_device *dev)
 {
 	struct nic *nic = netdev_priv(dev);
-	struct fsm_s *fsm = (struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf;
+	struct fsm_s *fsm = (struct fsm_s *)nic->sdk_data[SIOC_DATA_FSM].buf;
 
 	netif_start_queue(dev);
 
@@ -466,7 +466,7 @@ static int gdm_wimax_open(struct net_device *dev)
 static int gdm_wimax_close(struct net_device *dev)
 {
 	struct nic *nic = netdev_priv(dev);
-	struct fsm_s *fsm = (struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf;
+	struct fsm_s *fsm = (struct fsm_s *)nic->sdk_data[SIOC_DATA_FSM].buf;
 
 	netif_stop_queue(dev);
 
@@ -536,17 +536,17 @@ static void gdm_wimax_cleanup_ioctl(struct net_device *dev)
 static void gdm_update_fsm(struct net_device *dev, struct fsm_s *new_fsm)
 {
 	struct nic *nic = netdev_priv(dev);
-	struct fsm_s *cur_fsm =
-		(struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf;
+	struct fsm_s *cur_fsm = (struct fsm_s *)
+					nic->sdk_data[SIOC_DATA_FSM].buf;
 
 	if (!cur_fsm)
 		return;
 
 	if (cur_fsm->m_status != new_fsm->m_status ||
-		cur_fsm->c_status != new_fsm->c_status) {
-		if (new_fsm->m_status == M_CONNECTED)
+	    cur_fsm->c_status != new_fsm->c_status) {
+		if (new_fsm->m_status == M_CONNECTED) {
 			netif_carrier_on(dev);
-		else if (cur_fsm->m_status == M_CONNECTED) {
+		} else if (cur_fsm->m_status == M_CONNECTED) {
 			netif_carrier_off(dev);
 			#if defined(CONFIG_WIMAX_GDM72XX_QOS)
 			gdm_qos_release_list(nic);
@@ -558,7 +558,7 @@ static void gdm_update_fsm(struct net_device *dev, struct fsm_s *new_fsm)
 
 static int gdm_wimax_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 {
-	struct wm_req_s *req = (struct wm_req_s *) ifr;
+	struct wm_req_s *req = (struct wm_req_s *)ifr;
 	struct nic *nic = netdev_priv(dev);
 	int ret;
 
@@ -574,8 +574,8 @@ static int gdm_wimax_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 			return -EOPNOTSUPP;
 		}
 		if (req->cmd == SIOCG_DATA) {
-			ret = gdm_wimax_ioctl_get_data(&req->data,
-						&nic->sdk_data[req->data_id]);
+			ret = gdm_wimax_ioctl_get_data(
+				&req->data, &nic->sdk_data[req->data_id]);
 			if (ret < 0)
 				return ret;
 		} else if (req->cmd == SIOCS_DATA) {
@@ -583,7 +583,7 @@ static int gdm_wimax_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 				/*NOTE: gdm_update_fsm should be called
 				before gdm_wimax_ioctl_set_data is called*/
 				gdm_update_fsm(dev,
-						(struct fsm_s *) req->data.buf);
+					       (struct fsm_s *)req->data.buf);
 			}
 			ret = gdm_wimax_ioctl_set_data(
 				&nic->sdk_data[req->data_id], &req->data);
@@ -603,7 +603,7 @@ static void gdm_wimax_prepare_device(struct net_device *dev)
 {
 	struct nic *nic = netdev_priv(dev);
 	u16 buf[32 / sizeof(u16)];
-	struct hci_s *hci = (struct hci_s *) buf;
+	struct hci_s *hci = (struct hci_s *)buf;
 	u16 len = 0;
 	u32 val = 0;
 
@@ -661,7 +661,7 @@ static int gdm_wimax_hci_get_tlv(u8 *buf, u8 *T, u16 *L, u8 **V)
 }
 
 static int gdm_wimax_get_prepared_info(struct net_device *dev, char *buf,
-					int len)
+				       int len)
 {
 	u8 T, *V;
 	u16 L;
@@ -740,7 +740,7 @@ static void gdm_wimax_transmit_aggr_pkt(struct net_device *dev, char *buf,
 	int length;
 
 	while (len > 0) {
-		hci = (struct hci_s *) buf;
+		hci = (struct hci_s *)buf;
 
 		if (B2H(hci->cmd_evt) != WIMAX_RX_SDU) {
 			netdev_err(dev, "Wrong cmd_evt(0x%04X)\n",
@@ -786,7 +786,7 @@ static void gdm_wimax_transmit_pkt(struct net_device *dev, char *buf, int len)
 	switch (cmd_evt) {
 	case WIMAX_RX_SDU_AGGR:
 		gdm_wimax_transmit_aggr_pkt(dev, &buf[HCI_HEADER_SIZE],
-						cmd_len);
+					    cmd_len);
 		break;
 	case WIMAX_RX_SDU:
 		gdm_wimax_netif_rx(dev, &buf[HCI_HEADER_SIZE], cmd_len);
@@ -822,13 +822,13 @@ static void gdm_wimax_ind_fsm_update(struct net_device *dev, struct fsm_s *fsm)
 	memcpy(&hci_pkt_buf[HCI_HEADER_SIZE], fsm, sizeof(struct fsm_s));
 
 	gdm_wimax_event_send(dev, hci_pkt_buf,
-				HCI_HEADER_SIZE + sizeof(struct fsm_s));
+			     HCI_HEADER_SIZE + sizeof(struct fsm_s));
 }
 
 static void gdm_wimax_ind_if_updown(struct net_device *dev, int if_up)
 {
 	u16 buf[32 / sizeof(u16)];
-	struct hci_s *hci = (struct hci_s *) buf;
+	struct hci_s *hci = (struct hci_s *)buf;
 	unsigned char up_down;
 
 	up_down = if_up ? WIMAX_IF_UP : WIMAX_IF_DOWN;
@@ -855,17 +855,13 @@ static void prepare_rx_complete(void *arg, void *data, int len)
 	int ret;
 
 	ret = gdm_wimax_get_prepared_info(nic->netdev, data, len);
-	if (ret == 1)
+	if (ret == 1) {
 		gdm_wimax_rcv_with_cb(nic, rx_complete, nic);
-	else {
+	} else {
 		if (ret < 0)
 			netdev_err(nic->netdev,
 				   "get_prepared_info failed(%d)\n", ret);
 		gdm_wimax_rcv_with_cb(nic, prepare_rx_complete, nic);
-		#if 0
-		/* Re-prepare WiMax device */
-		gdm_wimax_prepare_device(nic->netdev);
-		#endif
 	}
 }
 
@@ -875,13 +871,13 @@ static void start_rx_proc(struct nic *nic)
 }
 
 static struct net_device_ops gdm_netdev_ops = {
-	.ndo_open				= gdm_wimax_open,
-	.ndo_stop				= gdm_wimax_close,
-	.ndo_set_config			= gdm_wimax_set_config,
-	.ndo_start_xmit			= gdm_wimax_tx,
-	.ndo_get_stats			= gdm_wimax_stats,
+	.ndo_open		= gdm_wimax_open,
+	.ndo_stop		= gdm_wimax_close,
+	.ndo_set_config		= gdm_wimax_set_config,
+	.ndo_start_xmit		= gdm_wimax_tx,
+	.ndo_get_stats		= gdm_wimax_stats,
 	.ndo_set_mac_address	= gdm_wimax_set_mac_addr,
-	.ndo_do_ioctl			= gdm_wimax_ioctl,
+	.ndo_do_ioctl		= gdm_wimax_ioctl,
 };
 
 int register_wimax_device(struct phy_dev *phy_dev, struct device *pdev)
@@ -948,7 +944,7 @@ cleanup:
 void unregister_wimax_device(struct phy_dev *phy_dev)
 {
 	struct nic *nic = netdev_priv(phy_dev->netdev);
-	struct fsm_s *fsm = (struct fsm_s *) nic->sdk_data[SIOC_DATA_FSM].buf;
+	struct fsm_s *fsm = (struct fsm_s *)nic->sdk_data[SIOC_DATA_FSM].buf;
 
 	if (fsm)
 		fsm->m_status = M_INIT;
diff --git a/drivers/staging/gdm72xx/gdm_wimax.h b/drivers/staging/gdm72xx/gdm_wimax.h
index 1fcfc855541..7e2c88877ed 100644
--- a/drivers/staging/gdm72xx/gdm_wimax.h
+++ b/drivers/staging/gdm72xx/gdm_wimax.h
@@ -23,10 +23,6 @@
 
 #define DRIVER_VERSION		"3.2.3"
 
-/*#define ETH_P_IP	0x0800 */
-/*#define ETH_P_ARP	0x0806 */
-/*#define ETH_P_IPV6	0x86DD */
-
 #define H2L(x)		__cpu_to_le16(x)
 #define L2H(x)		__le16_to_cpu(x)
 #define DH2L(x)		__cpu_to_le32(x)
@@ -38,12 +34,11 @@
 #define DB2H(x)		__be32_to_cpu(x)
 
 struct phy_dev {
-	void	*priv_dev;
+	void			*priv_dev;
 	struct net_device	*netdev;
-
-	int	(*send_func)(void *priv_dev, void *data, int len,
-			void (*cb)(void *cb_data), void *cb_data);
-	int	(*rcv_func)(void *priv_dev,
+	int (*send_func)(void *priv_dev, void *data, int len,
+			 void (*cb)(void *cb_data), void *cb_data);
+	int (*rcv_func)(void *priv_dev,
 			void (*cb)(void *cb_data, void *data, int len),
 			void *cb_data);
 };
@@ -51,21 +46,15 @@ struct phy_dev {
 struct nic {
 	struct net_device	*netdev;
 	struct phy_dev		*phy_dev;
-
 	struct net_device_stats	stats;
-
-	struct data_s	sdk_data[SIOC_DATA_MAX];
-
+	struct data_s		sdk_data[SIOC_DATA_MAX];
 #if defined(CONFIG_WIMAX_GDM72XX_QOS)
-	struct qos_cb_s	qos;
+	struct qos_cb_s		qos;
 #endif
-
 };
 
-/*#define LOOPBACK_TEST */
-
-extern int register_wimax_device(struct phy_dev *phy_dev, struct device *pdev);
-extern int gdm_wimax_send_tx(struct sk_buff *skb, struct net_device *dev);
-extern void unregister_wimax_device(struct phy_dev *phy_dev);
+int register_wimax_device(struct phy_dev *phy_dev, struct device *pdev);
+int gdm_wimax_send_tx(struct sk_buff *skb, struct net_device *dev);
+void unregister_wimax_device(struct phy_dev *phy_dev);
 
 #endif
diff --git a/drivers/staging/gdm72xx/hci.h b/drivers/staging/gdm72xx/hci.h
index 0e0676622f1..2485a379912 100644
--- a/drivers/staging/gdm72xx/hci.h
+++ b/drivers/staging/gdm72xx/hci.h
@@ -18,18 +18,18 @@
 #define HCI_VALUE_OFFS		(HCI_HEADER_SIZE)
 #define HCI_MAX_PACKET		2048
 #define HCI_MAX_PARAM		(HCI_MAX_PACKET-HCI_HEADER_SIZE)
-#define HCI_MAX_TLV			32
+#define HCI_MAX_TLV		32
 
 /* CMD-EVT */
 
 /* Category 0 */
-#define WIMAX_RESET				0x0000
-#define WIMAX_SET_INFO			0x0001
-#define WIMAX_GET_INFO			0x0002
+#define WIMAX_RESET		0x0000
+#define WIMAX_SET_INFO		0x0001
+#define WIMAX_GET_INFO		0x0002
 #define WIMAX_GET_INFO_RESULT	0x8003
-#define WIMAX_RADIO_OFF			0x0004
-#define WIMAX_RADIO_ON			0x0006
-#define WIMAX_WIMAX_RESET		0x0007	/* Is this still here */
+#define WIMAX_RADIO_OFF		0x0004
+#define WIMAX_RADIO_ON		0x0006
+#define WIMAX_WIMAX_RESET	0x0007	/* Is this still here */
 
 /* Category 1 */
 #define WIMAX_NET_ENTRY			0x0100
@@ -40,26 +40,22 @@
 #define WIMAX_EXIT_IDLE			0x0106
 #define WIMAX_MODE_CHANGE		0x8108
 #define WIMAX_HANDOVER			0x8109	/* obsolete */
-
-#define WIMAX_SCAN				0x010d
+#define WIMAX_SCAN			0x010d
 #define WIMAX_SCAN_COMPLETE		0x810e
 #define WIMAX_SCAN_RESULT		0x810f
-
 #define WIMAX_CONNECT			0x0110
 #define WIMAX_CONNECT_START		0x8111
-#define WIMAX_CONNECT_COMPLETE	0x8112
+#define WIMAX_CONNECT_COMPLETE		0x8112
 #define WIMAX_ASSOC_START		0x8113
-#define WIMAX_ASSOC_COMPLETE	0x8114
+#define WIMAX_ASSOC_COMPLETE		0x8114
 #define WIMAX_DISCONN_IND		0x8115
 #define WIMAX_ENTRY_IND			0x8116
 #define WIMAX_HO_START			0x8117
 #define WIMAX_HO_COMPLETE		0x8118
-#define WIMAX_RADIO_STATE_IND	0x8119
+#define WIMAX_RADIO_STATE_IND		0x8119
 #define WIMAX_IP_RENEW_IND		0x811a
-
-#define WIMAX_DISCOVER_NSP			0x011d
+#define WIMAX_DISCOVER_NSP		0x011d
 #define WIMAX_DISCOVER_NSP_RESULT	0x811e
-
 #define WIMAX_SDU_TX_FLOW		0x8125
 
 /* Category 2 */
@@ -71,34 +67,33 @@
 #define WIMAX_TX_SDU_AGGR	0x0205
 
 /* Category 3 */
-#define WIMAX_DM_CMD				0x030a
-#define WIMAX_DM_RSP				0x830b
-
-#define WIMAX_CLI_CMD				0x030c
-#define WIMAX_CLI_RSP				0x830d
+#define WIMAX_DM_CMD		0x030a
+#define WIMAX_DM_RSP		0x830b
 
-#define WIMAX_DL_IMAGE				0x0310
-#define WIMAX_DL_IMAGE_STATUS		0x8311
-#define WIMAX_UL_IMAGE				0x0312
-#define WIMAX_UL_IMAGE_RESULT		0x8313
-#define WIMAX_UL_IMAGE_STATUS		0x0314
+#define WIMAX_CLI_CMD		0x030c
+#define WIMAX_CLI_RSP		0x830d
 
-#define WIMAX_EVT_MODEM_REPORT		0x8325
+#define WIMAX_DL_IMAGE		0x0310
+#define WIMAX_DL_IMAGE_STATUS	0x8311
+#define WIMAX_UL_IMAGE		0x0312
+#define WIMAX_UL_IMAGE_RESULT	0x8313
+#define WIMAX_UL_IMAGE_STATUS	0x0314
+#define WIMAX_EVT_MODEM_REPORT	0x8325
 
 /* Category 0xF */
-#define WIMAX_FSM_UPDATE			0x8F01
-#define WIMAX_IF_UPDOWN				0x8F02
-	#define WIMAX_IF_UP				1
-	#define WIMAX_IF_DOWN			2
+#define WIMAX_FSM_UPDATE	0x8F01
+#define WIMAX_IF_UPDOWN		0x8F02
+#define WIMAX_IF_UP		1
+#define WIMAX_IF_DOWN		2
 
 /* WIMAX mode */
-#define W_NULL				0
-#define W_STANDBY			1
-#define W_OOZ				2
-#define W_AWAKE				3
-#define W_IDLE				4
-#define W_SLEEP				5
-#define W_WAIT				6
+#define W_NULL		0
+#define W_STANDBY	1
+#define W_OOZ		2
+#define W_AWAKE		3
+#define W_IDLE		4
+#define W_SLEEP		5
+#define W_WAIT		6
 
 #define W_NET_ENTRY_RNG		0x80
 #define W_NET_ENTRY_SBC		0x81
@@ -113,8 +108,8 @@
 #define W_NET_ENTRY_DSX_FAIL	0x1104000
 
 /* Scan Type */
-#define W_SCAN_ALL_CHANNEL				0
-#define W_SCAN_ALL_SUBSCRIPTION			1
+#define W_SCAN_ALL_CHANNEL		0
+#define W_SCAN_ALL_SUBSCRIPTION		1
 #define W_SCAN_SPECIFIED_SUBSCRIPTION	2
 
 /*
@@ -126,7 +121,7 @@
  *
  */
 #define TLV_L(x)		(((x) >> 16) & 0xff)
-#define TLV_T(x)			((x) & 0xff)
+#define TLV_T(x)		((x) & 0xff)
 #define TLV_COMPOSITE(x)	((x) >> 31)
 
 /* GENERAL */
@@ -141,7 +136,6 @@
 #define T_OOZ_SCAN_INTERVAL		(0x08	| (4 << 16))
 #define T_IMEI				(0x09	| (8 << 16))
 #define T_PID				(0x0a	| (12 << 16))
-
 #define T_CAPABILITY			(0x1a	| (4 << 16))
 #define T_RELEASE_NUMBER		(0x1b	| (4 << 16))
 #define T_DRIVER_REVISION		(0x1c	| (4 << 16))
@@ -150,19 +144,16 @@
 #define T_PHY_HW_REVISION		(0x1f	| (4 << 16))
 
 /* HANDOVER */
-#define T_SCAN_INTERVAL		(0x20	| (1 << 16))
-
+#define T_SCAN_INTERVAL			(0x20	| (1 << 16))
 #define T_RSC_RETAIN_TIME		(0x2f	| (2 << 16))
 
 /* SLEEP */
 #define T_TYPE1_ISW			(0x40	| (1 << 16))
-
 #define T_SLP_START_TO			(0x4a	| (2 << 16))
 
 /* IDLE */
 #define T_IDLE_MODE_TO			(0x50	| (2 << 16))
-
-#define T_IDLE_START_TO		(0x54	| (2 << 16))
+#define T_IDLE_START_TO			(0x54	| (2 << 16))
 
 /* MONITOR */
 #define T_RSSI				(0x60	| (1 << 16))
@@ -180,7 +171,7 @@
 #define T_CS_TYPE			(0xa6	| (2 << 16))
 #define T_VENDOR_NAME			(0xa7	| (0 << 16))
 #define T_MOD_NAME			(0xa8	| (0 << 16))
-#define T_PACKET_FILTER		(0xa9	| (1 << 16))
+#define T_PACKET_FILTER			(0xa9	| (1 << 16))
 #define T_NSP_CHANGE_COUNT		(0xaa	| (4 << 16))
 #define T_RADIO_STATE			(0xab	| (1 << 16))
 #define T_URI_CONTACT_TYPE		(0xac	| (1 << 16))
diff --git a/drivers/staging/gdm72xx/netlink_k.c b/drivers/staging/gdm72xx/netlink_k.c
index af7f1c1d0b5..9bf00e683f1 100644
--- a/drivers/staging/gdm72xx/netlink_k.c
+++ b/drivers/staging/gdm72xx/netlink_k.c
@@ -56,7 +56,7 @@ static void netlink_rcv_cb(struct sk_buff *skb)
 		nlh = (struct nlmsghdr *)skb->data;
 
 		if (skb->len < nlh->nlmsg_len ||
-		nlh->nlmsg_len > ND_MAX_MSG_LEN) {
+		    nlh->nlmsg_len > ND_MAX_MSG_LEN) {
 			netdev_err(skb->dev, "Invalid length (%d,%d)\n",
 				   skb->len, nlh->nlmsg_len);
 			return;
@@ -75,8 +75,9 @@ static void netlink_rcv_cb(struct sk_buff *skb)
 				netdev_err(skb->dev,
 					   "dev_get_by_index(%d) is not found.\n",
 					   ifindex);
-		} else
+		} else {
 			netdev_err(skb->dev, "Unregistered Callback\n");
+		}
 	}
 }
 
@@ -88,7 +89,7 @@ static void netlink_rcv(struct sk_buff *skb)
 }
 
 struct sock *netlink_init(int unit, void (*cb)(struct net_device *dev, u16 type,
-						void *msg, int len))
+					       void *msg, int len))
 {
 	struct sock *sock;
 	struct netlink_kernel_cfg cfg = {
@@ -144,9 +145,9 @@ int netlink_send(struct sock *sock, int group, u16 type, void *msg, int len)
 
 	ret = netlink_broadcast(sock, skb, 0, group+1, GFP_ATOMIC);
 
-	if (!ret)
+	if (!ret) {
 		return len;
-	else {
+	} else {
 		if (ret != -ESRCH) {
 			pr_err("netlink_broadcast g=%d, t=%d, l=%d, r=%d\n",
 			       group, type, len, ret);
diff --git a/drivers/staging/gdm72xx/netlink_k.h b/drivers/staging/gdm72xx/netlink_k.h
index 1dffaa6156e..b6caac16b3d 100644
--- a/drivers/staging/gdm72xx/netlink_k.h
+++ b/drivers/staging/gdm72xx/netlink_k.h
@@ -16,8 +16,8 @@
 #include <linux/netdevice.h>
 #include <net/sock.h>
 
-struct sock *netlink_init(int unit,
-	void (*cb)(struct net_device *dev, u16 type, void *msg, int len));
+struct sock *netlink_init(int unit, void (*cb)(struct net_device *dev, u16 type,
+					       void *msg, int len));
 void netlink_exit(struct sock *sock);
 int netlink_send(struct sock *sock, int group, u16 type, void *msg, int len);
 
diff --git a/drivers/staging/gdm72xx/sdio_boot.c b/drivers/staging/gdm72xx/sdio_boot.c
index cbe5dcfc2ac..2c02842ac5d 100644
--- a/drivers/staging/gdm72xx/sdio_boot.c
+++ b/drivers/staging/gdm72xx/sdio_boot.c
@@ -41,11 +41,11 @@ static u8 *tx_buf;
 
 static int ack_ready(struct sdio_func *func)
 {
-	unsigned long start = jiffies;
+	unsigned long wait = jiffies + HZ;
 	u8 val;
 	int ret;
 
-	while ((jiffies - start) < HZ) {
+	while (time_before(jiffies, wait)) {
 		val = sdio_readb(func, 0x13, &ret);
 		if (val & 0x01)
 			return 1;
diff --git a/drivers/staging/gdm72xx/sdio_boot.h b/drivers/staging/gdm72xx/sdio_boot.h
index 373ac28063c..045c1f45053 100644
--- a/drivers/staging/gdm72xx/sdio_boot.h
+++ b/drivers/staging/gdm72xx/sdio_boot.h
@@ -16,6 +16,6 @@
 
 struct sdio_func;
 
-extern int sdio_boot(struct sdio_func *func);
+int sdio_boot(struct sdio_func *func);
 
 #endif /* __SDIO_BOOT_H__ */
diff --git a/drivers/staging/gdm72xx/usb_boot.c b/drivers/staging/gdm72xx/usb_boot.c
index 0d45eb680be..d59bac872ff 100644
--- a/drivers/staging/gdm72xx/usb_boot.c
+++ b/drivers/staging/gdm72xx/usb_boot.c
@@ -67,6 +67,7 @@ struct fw_info {
 static void array_le32_to_cpu(u32 *arr, int num)
 {
 	int i;
+
 	for (i = 0; i < num; i++, arr++)
 		*arr = __le32_to_cpu(*arr);
 }
@@ -79,7 +80,7 @@ static int gdm_wibro_send(struct usb_device *usbdev, void *data, int len)
 	int actual;
 
 	ret = usb_bulk_msg(usbdev, usb_sndbulkpipe(usbdev, 1), data, len,
-			&actual, 1000);
+			   &actual, 1000);
 
 	if (ret < 0) {
 		dev_err(&usbdev->dev, "Error : usb_bulk_msg ( result = %d )\n",
@@ -95,7 +96,7 @@ static int gdm_wibro_recv(struct usb_device *usbdev, void *data, int len)
 	int actual;
 
 	ret = usb_bulk_msg(usbdev, usb_rcvbulkpipe(usbdev, 2), data, len,
-			&actual, 5000);
+			   &actual, 5000);
 
 	if (ret < 0) {
 		dev_err(&usbdev->dev,
@@ -106,8 +107,8 @@ static int gdm_wibro_recv(struct usb_device *usbdev, void *data, int len)
 }
 
 static int download_image(struct usb_device *usbdev,
-				const struct firmware *firm,
-				loff_t pos, u32 img_len, u32 magic_num)
+			  const struct firmware *firm,
+			  loff_t pos, u32 img_len, u32 magic_num)
 {
 	struct dn_header h;
 	int ret = 0;
@@ -169,14 +170,7 @@ int usb_boot(struct usb_device *usbdev, u16 pid)
 	memcpy(&hdr, firm->data, sizeof(hdr));
 
 	array_le32_to_cpu((u32 *)&hdr, 19);
-#if 0
-	if (hdr.magic_code != 0x10767fff) {
-		dev_err(&usbdev->dev, "Invalid magic code 0x%08x\n",
-			hdr.magic_code);
-		ret = -EINVAL;
-		goto out;
-	}
-#endif
+
 	if (hdr.count > MAX_IMG_CNT) {
 		dev_err(&usbdev->dev, "Too many images. %d\n", hdr.count);
 		ret = -EINVAL;
@@ -201,14 +195,6 @@ int usb_boot(struct usb_device *usbdev, u16 pid)
 		memcpy(&fw_info, firm->data + pos, sizeof(fw_info));
 
 		array_le32_to_cpu((u32 *)&fw_info, 8);
-#if 0
-		if ((fw_info.id & 0xfffff000) != 0x10767000) {
-			dev_err(&usbdev->dev, "Invalid FW id. 0x%08x\n",
-				fw_info.id);
-			ret = -EIO;
-			goto out;
-		}
-#endif
 
 		if ((fw_info.id & 0xffff) != pid)
 			continue;
@@ -219,8 +205,8 @@ int usb_boot(struct usb_device *usbdev, u16 pid)
 			goto out;
 		}
 
-		ret = download_image(usbdev, firm, pos,
-				fw_info.kernel_len, DN_KERNEL_MAGIC_NUMBER);
+		ret = download_image(usbdev, firm, pos, fw_info.kernel_len,
+				     DN_KERNEL_MAGIC_NUMBER);
 		if (ret < 0)
 			goto out;
 		dev_info(&usbdev->dev, "GCT: Kernel download success.\n");
@@ -231,7 +217,7 @@ int usb_boot(struct usb_device *usbdev, u16 pid)
 			goto out;
 		}
 		ret = download_image(usbdev, firm, pos, fw_info.rootfs_len,
-				DN_ROOTFS_MAGIC_NUMBER);
+				     DN_ROOTFS_MAGIC_NUMBER);
 		if (ret < 0)
 			goto out;
 		dev_info(&usbdev->dev, "GCT: Filesystem download success.\n");
@@ -276,7 +262,7 @@ out:
 }
 
 static int em_download_image(struct usb_device *usbdev, const char *img_name,
-				char *type_string)
+			     char *type_string)
 {
 	char *buf = NULL;
 	loff_t pos = 0;
@@ -347,11 +333,8 @@ out:
 
 static int em_fw_reset(struct usb_device *usbdev)
 {
-	int ret;
-
 	/*Send ZLP*/
-	ret = gdm_wibro_send(usbdev, NULL, 0);
-	return ret;
+	return gdm_wibro_send(usbdev, NULL, 0);
 }
 
 int usb_emergency(struct usb_device *usbdev)
diff --git a/drivers/staging/gdm72xx/usb_boot.h b/drivers/staging/gdm72xx/usb_boot.h
index c715cd3cd30..05308e25305 100644
--- a/drivers/staging/gdm72xx/usb_boot.h
+++ b/drivers/staging/gdm72xx/usb_boot.h
@@ -16,7 +16,7 @@
 
 struct usb_device;
 
-extern int usb_boot(struct usb_device *usbdev, u16 pid);
-extern int usb_emergency(struct usb_device *usbdev);
+int usb_boot(struct usb_device *usbdev, u16 pid);
+int usb_emergency(struct usb_device *usbdev);
 
 #endif /* __USB_BOOT_H__ */
diff --git a/drivers/staging/gdm72xx/usb_ids.h b/drivers/staging/gdm72xx/usb_ids.h
index b34616b7203..1a61b359976 100644
--- a/drivers/staging/gdm72xx/usb_ids.h
+++ b/drivers/staging/gdm72xx/usb_ids.h
@@ -29,7 +29,7 @@
 	.idVendor = (vend), .idProduct = (prod), .bInterfaceClass = (intf)
 
 #define EMERGENCY_PID		0x720f
-#define BL_PID_MASK			0xffc0
+#define BL_PID_MASK		0xffc0
 
 #define USB_DEVICE_BOOTLOADER(vid, pid)	\
 	{USB_DEVICE((vid), ((pid)&BL_PID_MASK)|B_DOWNLOAD)},	\
diff --git a/drivers/staging/gdm72xx/wm_ioctl.h b/drivers/staging/gdm72xx/wm_ioctl.h
index 9f46e06f230..d022c6ca0e2 100644
--- a/drivers/staging/gdm72xx/wm_ioctl.h
+++ b/drivers/staging/gdm72xx/wm_ioctl.h
@@ -19,10 +19,10 @@
 
 #define NETLINK_WIMAX	31
 
-#define SIOCWMIOCTL			SIOCDEVPRIVATE
+#define SIOCWMIOCTL	SIOCDEVPRIVATE
 
-#define SIOCG_DATA			0x8D10
-#define SIOCS_DATA			0x8D11
+#define SIOCG_DATA	0x8D10
+#define SIOCS_DATA	0x8D11
 
 enum {
 	SIOC_DATA_FSM,
@@ -34,7 +34,7 @@ enum {
 	SIOC_DATA_END
 };
 
-#define SIOC_DATA_MAX			16
+#define SIOC_DATA_MAX	16
 
 /* FSM */
 enum {
@@ -67,23 +67,21 @@ enum {
 };
 
 struct fsm_s {
-	int		m_status;	/*main status*/
-	int		c_status;	/*connection status*/
-	int		d_status;	/*oma-dm status*/
+	int	m_status;	/*main status*/
+	int	c_status;	/*connection status*/
+	int	d_status;	/*oma-dm status*/
 };
 
 struct data_s {
-	int		size;
+	int	size;
 	void	*buf;
 };
 
 struct wm_req_s {
 	union {
-		char	ifrn_name[IFNAMSIZ];
+		char ifrn_name[IFNAMSIZ];
 	} ifr_ifrn;
-
 	unsigned short	cmd;
-
 	unsigned short	data_id;
 	struct data_s	data;
 
@@ -91,7 +89,7 @@ struct wm_req_s {
 };
 
 #ifndef ifr_name
-#define ifr_name	ifr_ifrn.ifrn_name
+#define ifr_name ifr_ifrn.ifrn_name
 #endif
 
 #endif
diff --git a/drivers/staging/goldfish/README b/drivers/staging/goldfish/README
index 93d65b0f0f8..183af005323 100644
--- a/drivers/staging/goldfish/README
+++ b/drivers/staging/goldfish/README
@@ -5,7 +5,6 @@ Audio
 
 NAND
 ----
-- Switch from spinlock to mutex
 - Remove excess checking of parameters in calls
 - Use dma coherent memory not kmalloc/__pa for the memory (this is just
   a cleanliness issue not a correctness one)
diff --git a/drivers/staging/goldfish/goldfish_audio.c b/drivers/staging/goldfish/goldfish_audio.c
index f96dcec740a..cbd456770af 100644
--- a/drivers/staging/goldfish/goldfish_audio.c
+++ b/drivers/staging/goldfish/goldfish_audio.c
@@ -26,6 +26,7 @@
 #include <linux/sched.h>
 #include <linux/dma-mapping.h>
 #include <linux/uaccess.h>
+#include <linux/goldfish.h>
 
 MODULE_AUTHOR("Google, Inc.");
 MODULE_DESCRIPTION("Android QEMU Audio Driver");
@@ -60,6 +61,8 @@ struct goldfish_audio {
 
 #define AUDIO_READ(data, addr)		(readl(data->reg_base + addr))
 #define AUDIO_WRITE(data, addr, x)	(writel(x, data->reg_base + addr))
+#define AUDIO_WRITE64(data, addr, addr2, x)	\
+	(gf_write64((u64)(x), data->reg_base + addr, data->reg_base+addr2))
 
 /*
  *  temporary variable used between goldfish_audio_probe() and
@@ -78,11 +81,14 @@ enum {
 	/* set number of bytes in buffer to write */
 	AUDIO_WRITE_BUFFER_1  = 0x10,
 	AUDIO_WRITE_BUFFER_2  = 0x14,
+	AUDIO_SET_WRITE_BUFFER_1_HIGH = 0x28,
+	AUDIO_SET_WRITE_BUFFER_2_HIGH = 0x30,
 
 	/* true if audio input is supported */
 	AUDIO_READ_SUPPORTED = 0x18,
 	/* buffer to use for audio input */
 	AUDIO_SET_READ_BUFFER = 0x1C,
+	AUDIO_SET_READ_BUFFER_HIGH = 0x34,
 
 	/* driver writes number of bytes to read */
 	AUDIO_START_READ  = 0x20,
@@ -147,6 +153,7 @@ static ssize_t goldfish_audio_write(struct file *fp, const char __user *buf,
 
 	while (count > 0) {
 		ssize_t copy = count;
+
 		if (copy > WRITE_BUFFER_SIZE)
 			copy = WRITE_BUFFER_SIZE;
 		wait_event_interruptible(data->wait, (data->buffer_status &
@@ -321,19 +328,25 @@ static int goldfish_audio_probe(struct platform_device *pdev)
 		goto err_misc_register_failed;
 	}
 
-	AUDIO_WRITE(data, AUDIO_SET_WRITE_BUFFER_1, buf_addr);
-	AUDIO_WRITE(data, AUDIO_SET_WRITE_BUFFER_2,
-						buf_addr + WRITE_BUFFER_SIZE);
+	AUDIO_WRITE64(data, AUDIO_SET_WRITE_BUFFER_1,
+				AUDIO_SET_WRITE_BUFFER_1_HIGH, buf_addr);
+	buf_addr += WRITE_BUFFER_SIZE;
+
+	AUDIO_WRITE64(data, AUDIO_SET_WRITE_BUFFER_2,
+				AUDIO_SET_WRITE_BUFFER_2_HIGH, buf_addr);
+
+	buf_addr += WRITE_BUFFER_SIZE;
 
 	data->read_supported = AUDIO_READ(data, AUDIO_READ_SUPPORTED);
 	if (data->read_supported)
-		AUDIO_WRITE(data, AUDIO_SET_READ_BUFFER,
-					buf_addr + 2 * WRITE_BUFFER_SIZE);
+		AUDIO_WRITE64(data, AUDIO_SET_READ_BUFFER,
+				AUDIO_SET_READ_BUFFER_HIGH, buf_addr);
 
 	audio_data = data;
 	return 0;
 
 err_misc_register_failed:
+	free_irq(data->irq, data);
 err_request_irq_failed:
 	dma_free_coherent(&pdev->dev, COMBINED_BUFFER_SIZE,
 					data->buffer_virt, data->buffer_phys);
diff --git a/drivers/staging/goldfish/goldfish_nand.c b/drivers/staging/goldfish/goldfish_nand.c
index eca0873098c..092604c698b 100644
--- a/drivers/staging/goldfish/goldfish_nand.c
+++ b/drivers/staging/goldfish/goldfish_nand.c
@@ -24,13 +24,14 @@
 #include <linux/vmalloc.h>
 #include <linux/mtd/mtd.h>
 #include <linux/platform_device.h>
-
+#include <linux/mutex.h>
+#include <linux/goldfish.h>
 #include <asm/div64.h>
 
 #include "goldfish_nand_reg.h"
 
 struct goldfish_nand {
-	spinlock_t              lock;
+	struct mutex            lock;
 	unsigned char __iomem  *base;
 	struct cmd_params       *cmd_params;
 	size_t                  mtd_count;
@@ -66,7 +67,7 @@ static u32 goldfish_nand_cmd_with_params(struct mtd_info *mtd,
 	cps->addr_high = (u32)(addr >> 32);
 	cps->addr_low = (u32)addr;
 	cps->transfer_size = len;
-	cps->data = (u32)ptr;
+	cps->data = (unsigned long)ptr;
 	writel(cmdp, base + NAND_COMMAND);
 	*rv = cps->result;
 	return 0;
@@ -77,20 +78,19 @@ static u32 goldfish_nand_cmd(struct mtd_info *mtd, enum nand_cmd cmd,
 {
 	struct goldfish_nand *nand = mtd->priv;
 	u32 rv;
-	unsigned long irq_flags;
 	unsigned char __iomem  *base = nand->base;
 
-	spin_lock_irqsave(&nand->lock, irq_flags);
+	mutex_lock(&nand->lock);
 	if (goldfish_nand_cmd_with_params(mtd, cmd, addr, len, ptr, &rv)) {
 		writel(mtd - nand->mtd, base + NAND_DEV);
 		writel((u32)(addr >> 32), base + NAND_ADDR_HIGH);
 		writel((u32)addr, base + NAND_ADDR_LOW);
 		writel(len, base + NAND_TRANSFER_SIZE);
-		writel((u32)ptr, base + NAND_DATA);
+		gf_write64((u64)ptr, base + NAND_DATA, base + NAND_DATA_HIGH);
 		writel(cmd, base + NAND_COMMAND);
 		rv = readl(base + NAND_RESULT);
 	}
-	spin_unlock_irqrestore(&nand->lock, irq_flags);
+	mutex_unlock(&nand->lock);
 	return rv;
 }
 
@@ -199,8 +199,6 @@ static int goldfish_nand_read(struct mtd_info *mtd, loff_t from, size_t len,
 
 	if (from + len > mtd->size)
 		goto invalid_arg;
-	if (len != mtd->writesize)
-		goto invalid_arg;
 
 	rem = do_div(from, mtd->writesize);
 	if (rem)
@@ -223,8 +221,6 @@ static int goldfish_nand_write(struct mtd_info *mtd, loff_t to, size_t len,
 
 	if (to + len > mtd->size)
 		goto invalid_arg;
-	if (len != mtd->writesize)
-		goto invalid_arg;
 
 	rem = do_div(to, mtd->writesize);
 	if (rem)
@@ -307,12 +303,11 @@ static int goldfish_nand_init_device(struct platform_device *pdev,
 	u32 name_len;
 	u32 result;
 	u32 flags;
-	unsigned long irq_flags;
 	unsigned char __iomem  *base = nand->base;
 	struct mtd_info *mtd = &nand->mtd[id];
 	char *name;
 
-	spin_lock_irqsave(&nand->lock, irq_flags);
+	mutex_lock(&nand->lock);
 	writel(id, base + NAND_DEV);
 	flags = readl(base + NAND_DEV_FLAGS);
 	name_len = readl(base + NAND_DEV_NAME_LEN);
@@ -329,7 +324,7 @@ static int goldfish_nand_init_device(struct platform_device *pdev,
 		"goldfish nand dev%d: size %llx, page %d, extra %d, erase %d\n",
 		       id, mtd->size, mtd->writesize,
 		       mtd->oobsize, mtd->erasesize);
-	spin_unlock_irqrestore(&nand->lock, irq_flags);
+	mutex_unlock(&nand->lock);
 
 	mtd->priv = nand;
 
@@ -405,7 +400,7 @@ static int goldfish_nand_probe(struct platform_device *pdev)
 	if (nand == NULL)
 		return -ENOMEM;
 
-	spin_lock_init(&nand->lock);
+	mutex_init(&nand->lock);
 	nand->base = base;
 	nand->mtd_count = num_dev;
 	platform_set_drvdata(pdev, nand);
@@ -425,6 +420,7 @@ static int goldfish_nand_remove(struct platform_device *pdev)
 {
 	struct goldfish_nand *nand = platform_get_drvdata(pdev);
 	int i;
+
 	for (i = 0; i < nand->mtd_count; i++) {
 		if (nand->mtd[i].name)
 			mtd_device_unregister(&nand->mtd[i]);
diff --git a/drivers/staging/goldfish/goldfish_nand_reg.h b/drivers/staging/goldfish/goldfish_nand_reg.h
index ddfda71ab27..fe7f47c7a5c 100644
--- a/drivers/staging/goldfish/goldfish_nand_reg.h
+++ b/drivers/staging/goldfish/goldfish_nand_reg.h
@@ -57,6 +57,7 @@ enum nand_reg {
 	NAND_RESULT         = 0x040,
 	NAND_COMMAND        = 0x044,
 	NAND_DATA           = 0x048,
+	NAND_DATA_HIGH	    = 0x100,
 	NAND_TRANSFER_SIZE  = 0x04c,
 	NAND_ADDR_LOW       = 0x050,
 	NAND_ADDR_HIGH      = 0x054,
@@ -69,7 +70,7 @@ struct cmd_params {
 	uint32_t addr_low;
 	uint32_t addr_high;
 	uint32_t transfer_size;
-	uint32_t data;
+	unsigned long data;
 	uint32_t result;
 };
 #endif
diff --git a/drivers/staging/gs_fpgaboot/Makefile b/drivers/staging/gs_fpgaboot/Makefile
index 34cb606e0e3..d2f0211ba54 100644
--- a/drivers/staging/gs_fpgaboot/Makefile
+++ b/drivers/staging/gs_fpgaboot/Makefile
@@ -1,4 +1,2 @@
 gs_fpga-y	+= gs_fpgaboot.o io.o
 obj-$(CONFIG_GS_FPGABOOT)	+= gs_fpga.o
-
-ccflags-$(CONFIG_GS_FPGA_DEBUG)	:= -DDEBUG
diff --git a/drivers/staging/gs_fpgaboot/gs_fpgaboot.c b/drivers/staging/gs_fpgaboot/gs_fpgaboot.c
index 89bc84d833e..6aa9d7c3013 100644
--- a/drivers/staging/gs_fpgaboot/gs_fpgaboot.c
+++ b/drivers/staging/gs_fpgaboot/gs_fpgaboot.c
@@ -214,8 +214,8 @@ static int gs_download_image(struct fpgaimage *fimage, enum wbus bus_bytes)
 {
 	char *bitdata;
 	int size, i, cnt;
-	cnt = 0;
 
+	cnt = 0;
 	bitdata = (char *)fimage->fpgadata;
 	size = fimage->lendata;
 
@@ -373,7 +373,6 @@ static int __init gs_fpgaboot_init(void)
 	r = -1;
 
 	pr_info("FPGA DOWNLOAD --->\n");
-	pr_info("built at %s UTC\n", __TIMESTAMP__);
 
 	pr_info("FPGA image file name: %s\n", file);
 
diff --git a/drivers/staging/gs_fpgaboot/io.c b/drivers/staging/gs_fpgaboot/io.c
index b7be8e37b8d..23c12f47c8f 100644
--- a/drivers/staging/gs_fpgaboot/io.c
+++ b/drivers/staging/gs_fpgaboot/io.c
@@ -44,6 +44,7 @@ static inline void xl_cclk_b(int32_t i);
 void xl_shift_cclk(int count)
 {
 	int i;
+
 	for (i = 0; i < count; i++) {
 		xl_cclk_b(1);
 		xl_cclk_b(0);
@@ -85,6 +86,7 @@ void xl_shift_bytes_out(enum wbus bus_byte, unsigned char *pdata)
 static inline unsigned char bitswap(unsigned char s)
 {
 	unsigned char d;
+
 	d = (((s&0x80)>>7) | ((s&0x40)>>5) | ((s&0x20)>>3) | ((s&0x10)>>1) |
 		((s&0x08)<<1) | ((s&0x04)<<3) | ((s&0x02)<<5) | ((s&0x01)<<7));
 	return d;
@@ -135,6 +137,7 @@ static inline void mpc85xx_gpio_set_high(int32_t port, uint32_t gpios)
 static inline void gpio_set_value(int32_t port, uint32_t gpio, uint32_t value)
 {
 	int32_t g;
+
 	g = 31 - gpio;
 	if (value)
 		mpc85xx_gpio_set_high(port, 1U << g);
@@ -145,6 +148,7 @@ static inline void gpio_set_value(int32_t port, uint32_t gpio, uint32_t value)
 static inline int gpio_get_value(int32_t port, uint32_t gpio)
 {
 	int32_t g;
+
 	g = 31 - gpio;
 	return !!mpc85xx_gpio_get(port, 1U << g);
 }
@@ -184,6 +188,7 @@ int xl_get_done_b(void)
 static inline uint32_t bit_remap_byte0(uint32_t s)
 {
 	uint32_t d;
+
 	d = (((s&0x80)>>7) | ((s&0x40)>>5) | ((s&0x20)>>3) | ((s&0x10)>>1) |
 		((s&0x08)<<1) | ((s&0x04)<<3) | ((s&0x02)<<6) | ((s&0x01)<<9));
 	return d;
@@ -195,6 +200,7 @@ static inline uint32_t bit_remap_byte0(uint32_t s)
 static inline void byte0_out(unsigned char data)
 {
 	uint32_t swap32;
+
 	swap32 =  bit_remap_byte0((uint32_t) data) << 8;
 
 	mpc85xx_gpio_set(0, 0x0002BF00, (uint32_t) swap32);
diff --git a/drivers/staging/iio/Documentation/iio_utils.h b/drivers/staging/iio/Documentation/iio_utils.h
index 2064839ef2c..a9cfc06edb0 100644
--- a/drivers/staging/iio/Documentation/iio_utils.h
+++ b/drivers/staging/iio/Documentation/iio_utils.h
@@ -318,7 +318,7 @@ inline int build_channel_array(const char *device_dir,
 				free(filename);
 				goto error_close_dir;
 			}
-			fscanf(sysfsfp, "%u", &ret);
+			fscanf(sysfsfp, "%i", &ret);
 			if (ret == 1)
 				(*counter)++;
 			fclose(sysfsfp);
@@ -350,7 +350,7 @@ inline int build_channel_array(const char *device_dir,
 				ret = -errno;
 				goto error_cleanup_array;
 			}
-			fscanf(sysfsfp, "%u", &current_enabled);
+			fscanf(sysfsfp, "%i", &current_enabled);
 			fclose(sysfsfp);
 
 			if (!current_enabled) {
diff --git a/drivers/staging/iio/adc/Kconfig b/drivers/staging/iio/adc/Kconfig
index 363329808a4..b87e382ad76 100644
--- a/drivers/staging/iio/adc/Kconfig
+++ b/drivers/staging/iio/adc/Kconfig
@@ -37,26 +37,6 @@ config AD7606_IFACE_SPI
 	  Say yes here to include parallel interface support on the AD7606
 	  ADC driver.
 
-config AD799X
-	tristate "Analog Devices AD799x ADC driver"
-	depends on I2C
-	select IIO_TRIGGER if IIO_BUFFER
-	select AD799X_RING_BUFFER
-	help
-	  Say yes here to build support for Analog Devices:
-	  ad7991, ad7995, ad7999, ad7992, ad7993, ad7994, ad7997, ad7998
-	  i2c analog to digital converters (ADC). Provides direct access
-	  via sysfs.
-
-config AD799X_RING_BUFFER
-	bool "Analog Devices AD799x: use ring buffer"
-	depends on AD799X
-	select IIO_BUFFER
-	select IIO_TRIGGERED_BUFFER
-	help
-	  Say yes here to include ring buffer support in the AD799X
-	  ADC driver.
-
 config AD7780
 	tristate "Analog Devices AD7780 and similar ADCs driver"
 	depends on SPI
diff --git a/drivers/staging/iio/adc/Makefile b/drivers/staging/iio/adc/Makefile
index 3e9fb143d25..afdcd1ff08f 100644
--- a/drivers/staging/iio/adc/Makefile
+++ b/drivers/staging/iio/adc/Makefile
@@ -8,10 +8,6 @@ ad7606-$(CONFIG_AD7606_IFACE_PARALLEL) += ad7606_par.o
 ad7606-$(CONFIG_AD7606_IFACE_SPI) += ad7606_spi.o
 obj-$(CONFIG_AD7606) += ad7606.o
 
-ad799x-y := ad799x_core.o
-ad799x-$(CONFIG_AD799X_RING_BUFFER) += ad799x_ring.o
-obj-$(CONFIG_AD799X) += ad799x.o
-
 obj-$(CONFIG_AD7291) += ad7291.o
 obj-$(CONFIG_AD7780) += ad7780.o
 obj-$(CONFIG_AD7816) += ad7816.o
diff --git a/drivers/staging/iio/adc/ad7280a.c b/drivers/staging/iio/adc/ad7280a.c
index 1ac11f64827..d215edf66af 100644
--- a/drivers/staging/iio/adc/ad7280a.c
+++ b/drivers/staging/iio/adc/ad7280a.c
@@ -443,7 +443,7 @@ static ssize_t ad7280_show_balance_timer(struct device *dev,
 
 	msecs = (ret >> 3) * 71500;
 
-	return sprintf(buf, "%d\n", msecs);
+	return sprintf(buf, "%u\n", msecs);
 }
 
 static ssize_t ad7280_store_balance_timer(struct device *dev,
@@ -619,7 +619,7 @@ static ssize_t ad7280_read_channel_config(struct device *dev,
 		return -EINVAL;
 	}
 
-	return sprintf(buf, "%d\n", val);
+	return sprintf(buf, "%u\n", val);
 }
 
 static ssize_t ad7280_write_channel_config(struct device *dev,
diff --git a/drivers/staging/iio/adc/ad7606.h b/drivers/staging/iio/adc/ad7606.h
index 93c7299e835..ec89d055cf5 100644
--- a/drivers/staging/iio/adc/ad7606.h
+++ b/drivers/staging/iio/adc/ad7606.h
@@ -14,7 +14,7 @@
  */
 
 /**
- * struct ad7606_platform_data - platform/board specifc information
+ * struct ad7606_platform_data - platform/board specific information
  * @default_os:		default oversampling value {0, 2, 4, 8, 16, 32, 64}
  * @default_range:	default range +/-{5000, 10000} mVolt
  * @gpio_convst:	number of gpio connected to the CONVST pin
@@ -41,7 +41,7 @@ struct ad7606_platform_data {
 };
 
 /**
- * struct ad7606_chip_info - chip specifc information
+ * struct ad7606_chip_info - chip specific information
  * @name:		identification string for chip
  * @int_vref_mv:	the internal reference voltage
  * @channels:		channel specification
diff --git a/drivers/staging/iio/adc/ad7816.c b/drivers/staging/iio/adc/ad7816.c
index 2369cf28412..158d770f961 100644
--- a/drivers/staging/iio/adc/ad7816.c
+++ b/drivers/staging/iio/adc/ad7816.c
@@ -40,7 +40,7 @@
 
 
 /*
- * struct ad7816_chip_info - chip specifc information
+ * struct ad7816_chip_info - chip specific information
  */
 
 struct ad7816_chip_info {
diff --git a/drivers/staging/iio/adc/ad799x.h b/drivers/staging/iio/adc/ad799x.h
deleted file mode 100644
index fc8c85298fe..00000000000
--- a/drivers/staging/iio/adc/ad799x.h
+++ /dev/null
@@ -1,121 +0,0 @@
-/*
- * Copyright (C) 2010-2011 Michael Hennerich, Analog Devices Inc.
- * Copyright (C) 2008-2010 Jonathan Cameron
- *
- * 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.
- *
- * ad799x.h
- */
-
-#ifndef _AD799X_H_
-#define  _AD799X_H_
-
-#define AD799X_CHANNEL_SHIFT			4
-#define AD799X_STORAGEBITS			16
-/*
- * AD7991, AD7995 and AD7999 defines
- */
-
-#define AD7991_REF_SEL				0x08
-#define AD7991_FLTR				0x04
-#define AD7991_BIT_TRIAL_DELAY			0x02
-#define AD7991_SAMPLE_DELAY			0x01
-
-/*
- * AD7992, AD7993, AD7994, AD7997 and AD7998 defines
- */
-
-#define AD7998_FLTR				0x08
-#define AD7998_ALERT_EN				0x04
-#define AD7998_BUSY_ALERT			0x02
-#define AD7998_BUSY_ALERT_POL			0x01
-
-#define AD7998_CONV_RES_REG			0x0
-#define AD7998_ALERT_STAT_REG			0x1
-#define AD7998_CONF_REG				0x2
-#define AD7998_CYCLE_TMR_REG			0x3
-
-#define AD7998_DATALOW_REG(x)			((x) * 3 + 0x4)
-#define AD7998_DATAHIGH_REG(x)			((x) * 3 + 0x5)
-#define AD7998_HYST_REG(x)			((x) * 3 + 0x6)
-
-#define AD7998_CYC_MASK				0x7
-#define AD7998_CYC_DIS				0x0
-#define AD7998_CYC_TCONF_32			0x1
-#define AD7998_CYC_TCONF_64			0x2
-#define AD7998_CYC_TCONF_128			0x3
-#define AD7998_CYC_TCONF_256			0x4
-#define AD7998_CYC_TCONF_512			0x5
-#define AD7998_CYC_TCONF_1024			0x6
-#define AD7998_CYC_TCONF_2048			0x7
-
-#define AD7998_ALERT_STAT_CLEAR			0xFF
-
-/*
- * AD7997 and AD7997 defines
- */
-
-#define AD7997_8_READ_SINGLE			0x80
-#define AD7997_8_READ_SEQUENCE			0x70
-/* TODO: move this into a common header */
-#define RES_MASK(bits)	((1 << (bits)) - 1)
-
-enum {
-	ad7991,
-	ad7995,
-	ad7999,
-	ad7992,
-	ad7993,
-	ad7994,
-	ad7997,
-	ad7998
-};
-
-struct ad799x_state;
-
-/**
- * struct ad799x_chip_info - chip specifc information
- * @channel:		channel specification
- * @num_channels:	number of channels
- * @monitor_mode:	whether the chip supports monitor interrupts
- * @default_config:	device default configuration
- * @event_attrs:	pointer to the monitor event attribute group
- */
-
-struct ad799x_chip_info {
-	struct iio_chan_spec		channel[9];
-	int				num_channels;
-	u16				default_config;
-	const struct iio_info		*info;
-};
-
-struct ad799x_state {
-	struct i2c_client		*client;
-	const struct ad799x_chip_info	*chip_info;
-	struct regulator		*reg;
-	struct regulator		*vref;
-	unsigned			id;
-	u16				config;
-
-	u8				*rx_buf;
-	unsigned int			transfer_size;
-};
-
-#ifdef CONFIG_AD799X_RING_BUFFER
-int ad799x_register_ring_funcs_and_init(struct iio_dev *indio_dev);
-void ad799x_ring_cleanup(struct iio_dev *indio_dev);
-#else /* CONFIG_AD799X_RING_BUFFER */
-
-static inline int
-ad799x_register_ring_funcs_and_init(struct iio_dev *indio_dev)
-{
-	return 0;
-}
-
-static inline void ad799x_ring_cleanup(struct iio_dev *indio_dev)
-{
-}
-#endif /* CONFIG_AD799X_RING_BUFFER */
-#endif /* _AD799X_H_ */
diff --git a/drivers/staging/iio/adc/ad799x_ring.c b/drivers/staging/iio/adc/ad799x_ring.c
deleted file mode 100644
index 0ff6c03a483..00000000000
--- a/drivers/staging/iio/adc/ad799x_ring.c
+++ /dev/null
@@ -1,84 +0,0 @@
-/*
- * Copyright (C) 2010-2012 Michael Hennerich, Analog Devices Inc.
- * Copyright (C) 2008-2010 Jonathan Cameron
- *
- * 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.
- *
- * ad799x_ring.c
- */
-
-#include <linux/interrupt.h>
-#include <linux/slab.h>
-#include <linux/kernel.h>
-#include <linux/list.h>
-#include <linux/i2c.h>
-#include <linux/bitops.h>
-
-#include <linux/iio/iio.h>
-#include <linux/iio/buffer.h>
-#include <linux/iio/trigger_consumer.h>
-#include <linux/iio/triggered_buffer.h>
-
-#include "ad799x.h"
-
-/**
- * ad799x_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 ad799x_trigger_handler(int irq, void *p)
-{
-	struct iio_poll_func *pf = p;
-	struct iio_dev *indio_dev = pf->indio_dev;
-	struct ad799x_state *st = iio_priv(indio_dev);
-	int b_sent;
-	u8 cmd;
-
-	switch (st->id) {
-	case ad7991:
-	case ad7995:
-	case ad7999:
-		cmd = st->config |
-			(*indio_dev->active_scan_mask << AD799X_CHANNEL_SHIFT);
-		break;
-	case ad7992:
-	case ad7993:
-	case ad7994:
-		cmd = (*indio_dev->active_scan_mask << AD799X_CHANNEL_SHIFT) |
-			AD7998_CONV_RES_REG;
-		break;
-	case ad7997:
-	case ad7998:
-		cmd = AD7997_8_READ_SEQUENCE | AD7998_CONV_RES_REG;
-		break;
-	default:
-		cmd = 0;
-	}
-
-	b_sent = i2c_smbus_read_i2c_block_data(st->client,
-			cmd, st->transfer_size, st->rx_buf);
-	if (b_sent < 0)
-		goto out;
-
-	iio_push_to_buffers_with_timestamp(indio_dev, st->rx_buf,
-			iio_get_time_ns());
-out:
-	iio_trigger_notify_done(indio_dev->trig);
-
-	return IRQ_HANDLED;
-}
-
-int ad799x_register_ring_funcs_and_init(struct iio_dev *indio_dev)
-{
-	return iio_triggered_buffer_setup(indio_dev, NULL,
-		&ad799x_trigger_handler, NULL);
-}
-
-void ad799x_ring_cleanup(struct iio_dev *indio_dev)
-{
-	iio_triggered_buffer_cleanup(indio_dev);
-}
diff --git a/drivers/staging/iio/adc/mxs-lradc.c b/drivers/staging/iio/adc/mxs-lradc.c
index 11fb9520154..dae8d1a9038 100644
--- a/drivers/staging/iio/adc/mxs-lradc.c
+++ b/drivers/staging/iio/adc/mxs-lradc.c
@@ -1526,7 +1526,7 @@ static int mxs_lradc_probe(struct platform_device *pdev)
 	struct resource *iores;
 	int ret = 0, touch_ret;
 	int i, s;
-	unsigned int scale_uv;
+	uint64_t scale_uv;
 
 	/* Allocate the IIO device. */
 	iio = devm_iio_device_alloc(dev, sizeof(*lradc));
diff --git a/drivers/staging/iio/adc/spear_adc.c b/drivers/staging/iio/adc/spear_adc.c
index 970d9edc73b..c5492ba5075 100644
--- a/drivers/staging/iio/adc/spear_adc.c
+++ b/drivers/staging/iio/adc/spear_adc.c
@@ -22,39 +22,36 @@
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 
-/*
- * SPEAR registers definitions
- */
-
-#define SCAN_RATE_LO(x)		((x) & 0xFFFF)
-#define SCAN_RATE_HI(x)		(((x) >> 0x10) & 0xFFFF)
-#define CLK_LOW(x)		(((x) & 0xf) << 0)
-#define CLK_HIGH(x)		(((x) & 0xf) << 4)
+/* SPEAR registers definitions */
+#define SPEAR600_ADC_SCAN_RATE_LO(x)	((x) & 0xFFFF)
+#define SPEAR600_ADC_SCAN_RATE_HI(x)	(((x) >> 0x10) & 0xFFFF)
+#define SPEAR_ADC_CLK_LOW(x)		(((x) & 0xf) << 0)
+#define SPEAR_ADC_CLK_HIGH(x)		(((x) & 0xf) << 4)
 
 /* Bit definitions for SPEAR_ADC_STATUS */
-#define START_CONVERSION	(1 << 0)
-#define CHANNEL_NUM(x)		((x) << 1)
-#define ADC_ENABLE		(1 << 4)
-#define AVG_SAMPLE(x)		((x) << 5)
-#define VREF_INTERNAL		(1 << 9)
+#define SPEAR_ADC_STATUS_START_CONVERSION	(1 << 0)
+#define SPEAR_ADC_STATUS_CHANNEL_NUM(x)		((x) << 1)
+#define SPEAR_ADC_STATUS_ADC_ENABLE		(1 << 4)
+#define SPEAR_ADC_STATUS_AVG_SAMPLE(x)		((x) << 5)
+#define SPEAR_ADC_STATUS_VREF_INTERNAL		(1 << 9)
 
-#define DATA_MASK		0x03ff
-#define DATA_BITS		10
+#define SPEAR_ADC_DATA_MASK		0x03ff
+#define SPEAR_ADC_DATA_BITS		10
 
-#define MOD_NAME "spear-adc"
+#define SPEAR_ADC_MOD_NAME "spear-adc"
 
-#define ADC_CHANNEL_NUM		8
+#define SPEAR_ADC_CHANNEL_NUM		8
 
-#define CLK_MIN			2500000
-#define CLK_MAX			20000000
+#define SPEAR_ADC_CLK_MIN			2500000
+#define SPEAR_ADC_CLK_MAX			20000000
 
 struct adc_regs_spear3xx {
 	u32 status;
 	u32 average;
 	u32 scan_rate;
 	u32 clk;	/* Not avail for 1340 & 1310 */
-	u32 ch_ctrl[ADC_CHANNEL_NUM];
-	u32 ch_data[ADC_CHANNEL_NUM];
+	u32 ch_ctrl[SPEAR_ADC_CHANNEL_NUM];
+	u32 ch_data[SPEAR_ADC_CHANNEL_NUM];
 };
 
 struct chan_data {
@@ -66,14 +63,14 @@ struct adc_regs_spear6xx {
 	u32 status;
 	u32 pad[2];
 	u32 clk;
-	u32 ch_ctrl[ADC_CHANNEL_NUM];
-	struct chan_data ch_data[ADC_CHANNEL_NUM];
+	u32 ch_ctrl[SPEAR_ADC_CHANNEL_NUM];
+	struct chan_data ch_data[SPEAR_ADC_CHANNEL_NUM];
 	u32 scan_rate_lo;
 	u32 scan_rate_hi;
 	struct chan_data average;
 };
 
-struct spear_adc_info {
+struct spear_adc_state {
 	struct device_node *np;
 	struct adc_regs_spear3xx __iomem *adc_base_spear3xx;
 	struct adc_regs_spear6xx __iomem *adc_base_spear6xx;
@@ -91,100 +88,129 @@ struct spear_adc_info {
  * static inline functions, because of different register offsets
  * on different SoC variants (SPEAr300 vs SPEAr600 etc).
  */
-static void spear_adc_set_status(struct spear_adc_info *info, u32 val)
+static void spear_adc_set_status(struct spear_adc_state *st, u32 val)
 {
-	__raw_writel(val, &info->adc_base_spear6xx->status);
+	__raw_writel(val, &st->adc_base_spear6xx->status);
 }
 
-static void spear_adc_set_clk(struct spear_adc_info *info, u32 val)
+static void spear_adc_set_clk(struct spear_adc_state *st, u32 val)
 {
 	u32 clk_high, clk_low, count;
-	u32 apb_clk = clk_get_rate(info->clk);
+	u32 apb_clk = clk_get_rate(st->clk);
 
 	count = (apb_clk + val - 1) / val;
 	clk_low = count / 2;
 	clk_high = count - clk_low;
-	info->current_clk = apb_clk / count;
+	st->current_clk = apb_clk / count;
 
-	__raw_writel(CLK_LOW(clk_low) | CLK_HIGH(clk_high),
-		     &info->adc_base_spear6xx->clk);
+	__raw_writel(SPEAR_ADC_CLK_LOW(clk_low) | SPEAR_ADC_CLK_HIGH(clk_high),
+		     &st->adc_base_spear6xx->clk);
 }
 
-static void spear_adc_set_ctrl(struct spear_adc_info *info, int n,
+static void spear_adc_set_ctrl(struct spear_adc_state *st, int n,
 			       u32 val)
 {
-	__raw_writel(val, &info->adc_base_spear6xx->ch_ctrl[n]);
+	__raw_writel(val, &st->adc_base_spear6xx->ch_ctrl[n]);
 }
 
-static u32 spear_adc_get_average(struct spear_adc_info *info)
+static u32 spear_adc_get_average(struct spear_adc_state *st)
 {
-	if (of_device_is_compatible(info->np, "st,spear600-adc")) {
-		return __raw_readl(&info->adc_base_spear6xx->average.msb) &
-			DATA_MASK;
+	if (of_device_is_compatible(st->np, "st,spear600-adc")) {
+		return __raw_readl(&st->adc_base_spear6xx->average.msb) &
+			SPEAR_ADC_DATA_MASK;
 	} else {
-		return __raw_readl(&info->adc_base_spear3xx->average) &
-			DATA_MASK;
+		return __raw_readl(&st->adc_base_spear3xx->average) &
+			SPEAR_ADC_DATA_MASK;
 	}
 }
 
-static void spear_adc_set_scanrate(struct spear_adc_info *info, u32 rate)
+static void spear_adc_set_scanrate(struct spear_adc_state *st, u32 rate)
 {
-	if (of_device_is_compatible(info->np, "st,spear600-adc")) {
-		__raw_writel(SCAN_RATE_LO(rate),
-			     &info->adc_base_spear6xx->scan_rate_lo);
-		__raw_writel(SCAN_RATE_HI(rate),
-			     &info->adc_base_spear6xx->scan_rate_hi);
+	if (of_device_is_compatible(st->np, "st,spear600-adc")) {
+		__raw_writel(SPEAR600_ADC_SCAN_RATE_LO(rate),
+			     &st->adc_base_spear6xx->scan_rate_lo);
+		__raw_writel(SPEAR600_ADC_SCAN_RATE_HI(rate),
+			     &st->adc_base_spear6xx->scan_rate_hi);
 	} else {
-		__raw_writel(rate, &info->adc_base_spear3xx->scan_rate);
+		__raw_writel(rate, &st->adc_base_spear3xx->scan_rate);
 	}
 }
 
-static int spear_read_raw(struct iio_dev *indio_dev,
-			  struct iio_chan_spec const *chan,
-			  int *val,
-			  int *val2,
-			  long mask)
+static int spear_adc_read_raw(struct iio_dev *indio_dev,
+			      struct iio_chan_spec const *chan,
+			      int *val,
+			      int *val2,
+			      long mask)
 {
-	struct spear_adc_info *info = iio_priv(indio_dev);
+	struct spear_adc_state *st = iio_priv(indio_dev);
 	u32 status;
 
 	switch (mask) {
 	case IIO_CHAN_INFO_RAW:
 		mutex_lock(&indio_dev->mlock);
 
-		status = CHANNEL_NUM(chan->channel) |
-			AVG_SAMPLE(info->avg_samples) |
-			START_CONVERSION | ADC_ENABLE;
-		if (info->vref_external == 0)
-			status |= VREF_INTERNAL;
+		status = SPEAR_ADC_STATUS_CHANNEL_NUM(chan->channel) |
+			SPEAR_ADC_STATUS_AVG_SAMPLE(st->avg_samples) |
+			SPEAR_ADC_STATUS_START_CONVERSION |
+			SPEAR_ADC_STATUS_ADC_ENABLE;
+		if (st->vref_external == 0)
+			status |= SPEAR_ADC_STATUS_VREF_INTERNAL;
 
-		spear_adc_set_status(info, status);
-		wait_for_completion(&info->completion); /* set by ISR */
-		*val = info->value;
+		spear_adc_set_status(st, status);
+		wait_for_completion(&st->completion); /* set by ISR */
+		*val = st->value;
 
 		mutex_unlock(&indio_dev->mlock);
 
 		return IIO_VAL_INT;
 
 	case IIO_CHAN_INFO_SCALE:
-		*val = info->vref_external;
-		*val2 = DATA_BITS;
+		*val = st->vref_external;
+		*val2 = SPEAR_ADC_DATA_BITS;
 		return IIO_VAL_FRACTIONAL_LOG2;
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		*val = st->current_clk;
+		return IIO_VAL_INT;
 	}
 
 	return -EINVAL;
 }
 
+static int spear_adc_write_raw(struct iio_dev *indio_dev,
+			       struct iio_chan_spec const *chan,
+			       int val,
+			       int val2,
+			       long mask)
+{
+	struct spear_adc_state *st = iio_priv(indio_dev);
+	int ret = 0;
+
+	if (mask != IIO_CHAN_INFO_SAMP_FREQ)
+		return -EINVAL;
+
+	mutex_lock(&indio_dev->mlock);
+
+	if ((val < SPEAR_ADC_CLK_MIN) ||
+		(val > SPEAR_ADC_CLK_MAX) ||
+		(val2 != 0)) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	spear_adc_set_clk(st, val);
+
+out:
+	mutex_unlock(&indio_dev->mlock);
+	return ret;
+}
+
 #define SPEAR_ADC_CHAN(idx) {				\
 	.type = IIO_VOLTAGE,				\
 	.indexed = 1,					\
 	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),	\
 	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),	\
+	.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ),\
 	.channel = idx,					\
-	.scan_type = {					\
-		.sign = 'u',				\
-		.storagebits = 16,			\
-	},						\
 }
 
 static const struct iio_chan_spec spear_adc_iio_channels[] = {
@@ -200,92 +226,34 @@ static const struct iio_chan_spec spear_adc_iio_channels[] = {
 
 static irqreturn_t spear_adc_isr(int irq, void *dev_id)
 {
-	struct spear_adc_info *info = (struct spear_adc_info *)dev_id;
+	struct spear_adc_state *st = (struct spear_adc_state *)dev_id;
 
 	/* Read value to clear IRQ */
-	info->value = spear_adc_get_average(info);
-	complete(&info->completion);
+	st->value = spear_adc_get_average(st);
+	complete(&st->completion);
 
 	return IRQ_HANDLED;
 }
 
-static int spear_adc_configure(struct spear_adc_info *info)
+static int spear_adc_configure(struct spear_adc_state *st)
 {
 	int i;
 
 	/* Reset ADC core */
-	spear_adc_set_status(info, 0);
-	__raw_writel(0, &info->adc_base_spear6xx->clk);
+	spear_adc_set_status(st, 0);
+	__raw_writel(0, &st->adc_base_spear6xx->clk);
 	for (i = 0; i < 8; i++)
-		spear_adc_set_ctrl(info, i, 0);
-	spear_adc_set_scanrate(info, 0);
+		spear_adc_set_ctrl(st, i, 0);
+	spear_adc_set_scanrate(st, 0);
 
-	spear_adc_set_clk(info, info->sampling_freq);
+	spear_adc_set_clk(st, st->sampling_freq);
 
 	return 0;
 }
 
-static ssize_t spear_adc_read_frequency(struct device *dev,
-					struct device_attribute *attr,
-					char *buf)
-{
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
-	struct spear_adc_info *info = iio_priv(indio_dev);
-
-	return sprintf(buf, "%d\n", info->current_clk);
-}
-
-static ssize_t spear_adc_write_frequency(struct device *dev,
-					 struct device_attribute *attr,
-					 const char *buf,
-					 size_t len)
-{
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
-	struct spear_adc_info *info = iio_priv(indio_dev);
-	u32 clk_high, clk_low, count;
-	u32 apb_clk = clk_get_rate(info->clk);
-	unsigned long lval;
-	int ret;
-
-	ret = kstrtoul(buf, 10, &lval);
-	if (ret)
-		return ret;
-
-	mutex_lock(&indio_dev->mlock);
-
-	if ((lval < CLK_MIN) || (lval > CLK_MAX)) {
-		ret = -EINVAL;
-		goto out;
-	}
-
-	count = (apb_clk + lval - 1) / lval;
-	clk_low = count / 2;
-	clk_high = count - clk_low;
-	info->current_clk = apb_clk / count;
-	spear_adc_set_clk(info, lval);
-
-out:
-	mutex_unlock(&indio_dev->mlock);
-
-	return ret ? ret : len;
-}
-
-static IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO,
-			      spear_adc_read_frequency,
-			      spear_adc_write_frequency);
-
-static struct attribute *spear_attributes[] = {
-	&iio_dev_attr_sampling_frequency.dev_attr.attr,
-	NULL
-};
-
-static const struct attribute_group spear_attribute_group = {
-	.attrs = spear_attributes,
-};
-
-static const struct iio_info spear_adc_iio_info = {
-	.read_raw = &spear_read_raw,
-	.attrs = &spear_attribute_group,
+static const struct iio_info spear_adc_info = {
+	.read_raw = &spear_adc_read_raw,
+	.write_raw = &spear_adc_write_raw,
 	.driver_module = THIS_MODULE,
 };
 
@@ -293,40 +261,40 @@ static int spear_adc_probe(struct platform_device *pdev)
 {
 	struct device_node *np = pdev->dev.of_node;
 	struct device *dev = &pdev->dev;
-	struct spear_adc_info *info;
-	struct iio_dev *iodev = NULL;
+	struct spear_adc_state *st;
+	struct iio_dev *indio_dev = NULL;
 	int ret = -ENODEV;
 	int irq;
 
-	iodev = devm_iio_device_alloc(dev, sizeof(struct spear_adc_info));
-	if (!iodev) {
+	indio_dev = devm_iio_device_alloc(dev, sizeof(struct spear_adc_state));
+	if (!indio_dev) {
 		dev_err(dev, "failed allocating iio device\n");
 		return -ENOMEM;
 	}
 
-	info = iio_priv(iodev);
-	info->np = np;
+	st = iio_priv(indio_dev);
+	st->np = np;
 
 	/*
 	 * SPEAr600 has a different register layout than other SPEAr SoC's
 	 * (e.g. SPEAr3xx). Let's provide two register base addresses
 	 * to support multi-arch kernels.
 	 */
-	info->adc_base_spear6xx = of_iomap(np, 0);
-	if (!info->adc_base_spear6xx) {
+	st->adc_base_spear6xx = of_iomap(np, 0);
+	if (!st->adc_base_spear6xx) {
 		dev_err(dev, "failed mapping memory\n");
 		return -ENOMEM;
 	}
-	info->adc_base_spear3xx =
-		(struct adc_regs_spear3xx __iomem *)info->adc_base_spear6xx;
+	st->adc_base_spear3xx =
+		(struct adc_regs_spear3xx __iomem *)st->adc_base_spear6xx;
 
-	info->clk = clk_get(dev, NULL);
-	if (IS_ERR(info->clk)) {
+	st->clk = clk_get(dev, NULL);
+	if (IS_ERR(st->clk)) {
 		dev_err(dev, "failed getting clock\n");
 		goto errout1;
 	}
 
-	ret = clk_prepare_enable(info->clk);
+	ret = clk_prepare_enable(st->clk);
 	if (ret) {
 		dev_err(dev, "failed enabling clock\n");
 		goto errout2;
@@ -339,14 +307,15 @@ static int spear_adc_probe(struct platform_device *pdev)
 		goto errout3;
 	}
 
-	ret = devm_request_irq(dev, irq, spear_adc_isr, 0, MOD_NAME, info);
+	ret = devm_request_irq(dev, irq, spear_adc_isr, 0, SPEAR_ADC_MOD_NAME,
+			       st);
 	if (ret < 0) {
 		dev_err(dev, "failed requesting interrupt\n");
 		goto errout3;
 	}
 
 	if (of_property_read_u32(np, "sampling-frequency",
-				 &info->sampling_freq)) {
+				 &st->sampling_freq)) {
 		dev_err(dev, "sampling-frequency missing in DT\n");
 		ret = -EINVAL;
 		goto errout3;
@@ -356,28 +325,28 @@ static int spear_adc_probe(struct platform_device *pdev)
 	 * Optional avg_samples defaults to 0, resulting in single data
 	 * conversion
 	 */
-	of_property_read_u32(np, "average-samples", &info->avg_samples);
+	of_property_read_u32(np, "average-samples", &st->avg_samples);
 
 	/*
 	 * Optional vref_external defaults to 0, resulting in internal vref
 	 * selection
 	 */
-	of_property_read_u32(np, "vref-external", &info->vref_external);
+	of_property_read_u32(np, "vref-external", &st->vref_external);
 
-	spear_adc_configure(info);
+	spear_adc_configure(st);
 
-	platform_set_drvdata(pdev, iodev);
+	platform_set_drvdata(pdev, indio_dev);
 
-	init_completion(&info->completion);
+	init_completion(&st->completion);
 
-	iodev->name = MOD_NAME;
-	iodev->dev.parent = dev;
-	iodev->info = &spear_adc_iio_info;
-	iodev->modes = INDIO_DIRECT_MODE;
-	iodev->channels = spear_adc_iio_channels;
-	iodev->num_channels = ARRAY_SIZE(spear_adc_iio_channels);
+	indio_dev->name = SPEAR_ADC_MOD_NAME;
+	indio_dev->dev.parent = dev;
+	indio_dev->info = &spear_adc_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = spear_adc_iio_channels;
+	indio_dev->num_channels = ARRAY_SIZE(spear_adc_iio_channels);
 
-	ret = iio_device_register(iodev);
+	ret = iio_device_register(indio_dev);
 	if (ret)
 		goto errout3;
 
@@ -386,23 +355,23 @@ static int spear_adc_probe(struct platform_device *pdev)
 	return 0;
 
 errout3:
-	clk_disable_unprepare(info->clk);
+	clk_disable_unprepare(st->clk);
 errout2:
-	clk_put(info->clk);
+	clk_put(st->clk);
 errout1:
-	iounmap(info->adc_base_spear6xx);
+	iounmap(st->adc_base_spear6xx);
 	return ret;
 }
 
 static int spear_adc_remove(struct platform_device *pdev)
 {
-	struct iio_dev *iodev = platform_get_drvdata(pdev);
-	struct spear_adc_info *info = iio_priv(iodev);
+	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
+	struct spear_adc_state *st = iio_priv(indio_dev);
 
-	iio_device_unregister(iodev);
-	clk_disable_unprepare(info->clk);
-	clk_put(info->clk);
-	iounmap(info->adc_base_spear6xx);
+	iio_device_unregister(indio_dev);
+	clk_disable_unprepare(st->clk);
+	clk_put(st->clk);
+	iounmap(st->adc_base_spear6xx);
 
 	return 0;
 }
@@ -419,7 +388,7 @@ static struct platform_driver spear_adc_driver = {
 	.probe		= spear_adc_probe,
 	.remove		= spear_adc_remove,
 	.driver		= {
-		.name	= MOD_NAME,
+		.name	= SPEAR_ADC_MOD_NAME,
 		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(spear_adc_dt_ids),
 	},
diff --git a/drivers/staging/iio/addac/adt7316.c b/drivers/staging/iio/addac/adt7316.c
index 9f0ebb32900..5f1770e6f6c 100644
--- a/drivers/staging/iio/addac/adt7316.c
+++ b/drivers/staging/iio/addac/adt7316.c
@@ -172,7 +172,7 @@
 #define ID_ADT75XX		0x10
 
 /*
- * struct adt7316_chip_info - chip specifc information
+ * struct adt7316_chip_info - chip specific information
  */
 
 struct adt7316_chip_info {
@@ -208,7 +208,7 @@ struct adt7316_chip_info {
 	(ADT7316_TEMP_INT_MASK)
 
 /*
- * struct adt7316_chip_info - chip specifc information
+ * struct adt7316_chip_info - chip specific information
  */
 
 struct adt7316_limit_regs {
diff --git a/drivers/staging/iio/cdc/ad7152.c b/drivers/staging/iio/cdc/ad7152.c
index f2c309d1eb5..87110d940e9 100644
--- a/drivers/staging/iio/cdc/ad7152.c
+++ b/drivers/staging/iio/cdc/ad7152.c
@@ -78,7 +78,7 @@ enum {
 };
 
 /*
- * struct ad7152_chip_info - chip specifc information
+ * struct ad7152_chip_info - chip specific information
  */
 
 struct ad7152_chip_info {
diff --git a/drivers/staging/iio/cdc/ad7746.c b/drivers/staging/iio/cdc/ad7746.c
index cbb1588d591..e6e9eaa9eab 100644
--- a/drivers/staging/iio/cdc/ad7746.c
+++ b/drivers/staging/iio/cdc/ad7746.c
@@ -91,7 +91,7 @@
 #define AD7746_CAPDAC_DACP(x)		((x) & 0x7F)
 
 /*
- * struct ad7746_chip_info - chip specifc information
+ * struct ad7746_chip_info - chip specific information
  */
 
 struct ad7746_chip_info {
diff --git a/drivers/staging/iio/light/tsl2583.c b/drivers/staging/iio/light/tsl2583.c
index 0a60def9273..fa964987966 100644
--- a/drivers/staging/iio/light/tsl2583.c
+++ b/drivers/staging/iio/light/tsl2583.c
@@ -165,8 +165,9 @@ taos_i2c_read(struct i2c_client *client, u8 reg, u8 *val, unsigned int len)
 		/* select register to write */
 		ret = i2c_smbus_write_byte(client, (TSL258X_CMD_REG | reg));
 		if (ret < 0) {
-			dev_err(&client->dev, "taos_i2c_read failed to write"
-				" register %x\n", reg);
+			dev_err(&client->dev,
+				"taos_i2c_read failed to write register %x\n",
+				reg);
 			return ret;
 		}
 		/* read the data */
@@ -211,7 +212,7 @@ static int taos_get_lux(struct iio_dev *indio_dev)
 	if (chip->taos_chip_status != TSL258X_CHIP_WORKING) {
 		/* device is not enabled */
 		dev_err(&chip->client->dev, "taos_get_lux device is not enabled\n");
-		ret = -EBUSY ;
+		ret = -EBUSY;
 		goto out_unlock;
 	}
 
@@ -231,8 +232,9 @@ static int taos_get_lux(struct iio_dev *indio_dev)
 		int reg = TSL258X_CMD_REG | (TSL258X_ALS_CHAN0LO + i);
 		ret = taos_i2c_read(chip->client, reg, &buf[i], 1);
 		if (ret < 0) {
-			dev_err(&chip->client->dev, "taos_get_lux failed to read"
-				" register %x\n", reg);
+			dev_err(&chip->client->dev,
+				"taos_get_lux failed to read register %x\n",
+				reg);
 			goto out_unlock;
 		}
 	}
@@ -449,7 +451,7 @@ static int taos_chip_on(struct iio_dev *indio_dev)
 		}
 	}
 
-	msleep(3);
+	usleep_range(3000, 3500);
 	/* NOW enable the ADC
 	 * initialize the desired mode of operation */
 	utmp = TSL258X_CNTL_PWR_ON | TSL258X_CNTL_ADC_ENBL;
@@ -809,9 +811,7 @@ static int taos_probe(struct i2c_client *clientp,
 
 	if (!i2c_check_functionality(clientp->adapter,
 		I2C_FUNC_SMBUS_BYTE_DATA)) {
-		dev_err(&clientp->dev,
-			"taos_probe() - i2c smbus byte data "
-			"functions unsupported\n");
+		dev_err(&clientp->dev, "taos_probe() - i2c smbus byte data func unsupported\n");
 		return -EOPNOTSUPP;
 	}
 
@@ -830,30 +830,32 @@ static int taos_probe(struct i2c_client *clientp,
 		ret = i2c_smbus_write_byte(clientp,
 				(TSL258X_CMD_REG | (TSL258X_CNTRL + i)));
 		if (ret < 0) {
-			dev_err(&clientp->dev, "i2c_smbus_write_bytes() to cmd "
-				"reg failed in taos_probe(), err = %d\n", ret);
+			dev_err(&clientp->dev,
+				"i2c_smbus_write_byte to cmd reg failed in taos_probe(), err = %d\n",
+				ret);
 			return ret;
 		}
 		ret = i2c_smbus_read_byte(clientp);
 		if (ret < 0) {
-			dev_err(&clientp->dev, "i2c_smbus_read_byte from "
-				"reg failed in taos_probe(), err = %d\n", ret);
-
+			dev_err(&clientp->dev,
+				"i2c_smbus_read_byte from reg failed in taos_probe(), err = %d\n",
+				ret);
 			return ret;
 		}
 		buf[i] = ret;
 	}
 
 	if (!taos_tsl258x_device(buf)) {
-		dev_info(&clientp->dev, "i2c device found but does not match "
-			"expected id in taos_probe()\n");
+		dev_info(&clientp->dev,
+			"i2c device found but does not match expected id in taos_probe()\n");
 		return -EINVAL;
 	}
 
 	ret = i2c_smbus_write_byte(clientp, (TSL258X_CMD_REG | TSL258X_CNTRL));
 	if (ret < 0) {
-		dev_err(&clientp->dev, "i2c_smbus_write_byte() to cmd reg "
-			"failed in taos_probe(), err = %d\n", ret);
+		dev_err(&clientp->dev,
+			"i2c_smbus_write_byte() to cmd reg failed in taos_probe(), err = %d\n",
+			ret);
 		return ret;
 	}
 
diff --git a/drivers/staging/iio/resolver/ad2s1200.c b/drivers/staging/iio/resolver/ad2s1200.c
index 36eedd8a0ea..017d2f8379b 100644
--- a/drivers/staging/iio/resolver/ad2s1200.c
+++ b/drivers/staging/iio/resolver/ad2s1200.c
@@ -70,6 +70,7 @@ static int ad2s1200_read_raw(struct iio_dev *indio_dev,
 		vel = (((s16)(st->rx[0])) << 4) | ((st->rx[1] & 0xF0) >> 4);
 		vel = (vel << 4) >> 4;
 		*val = vel;
+		break;
 	default:
 		mutex_unlock(&st->lock);
 		return -EINVAL;
@@ -106,7 +107,7 @@ static int ad2s1200_probe(struct spi_device *spi)
 	int pn, ret = 0;
 	unsigned short *pins = spi->dev.platform_data;
 
-	for (pn = 0; pn < AD2S1200_PN; pn++)
+	for (pn = 0; pn < AD2S1200_PN; pn++) {
 		ret = devm_gpio_request_one(&spi->dev, pins[pn], GPIOF_DIR_OUT,
 					    DRV_NAME);
 		if (ret) {
@@ -114,6 +115,7 @@ static int ad2s1200_probe(struct spi_device *spi)
 							pins[pn]);
 			return ret;
 		}
+	}
 	indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*st));
 	if (!indio_dev)
 		return -ENOMEM;
diff --git a/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c b/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
index 48a6afa8408..38ecb4bb6e4 100644
--- a/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
+++ b/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
@@ -33,7 +33,8 @@ static int iio_trig_periodic_rtc_set_state(struct iio_trigger *trig, bool state)
 	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);
+	dev_info(&trig_info->rtc->dev, "trigger frequency is %d\n",
+			trig_info->frequency);
 	return rtc_irq_set_state(trig_info->rtc, &trig_info->task, state);
 }
 
diff --git a/drivers/staging/imx-drm/imx-drm-core.c b/drivers/staging/imx-drm/imx-drm-core.c
index 4144a75e5f7..c270c9ae6d2 100644
--- a/drivers/staging/imx-drm/imx-drm-core.c
+++ b/drivers/staging/imx-drm/imx-drm-core.c
@@ -517,7 +517,7 @@ int imx_drm_encoder_get_mux_id(struct device_node *node,
 		of_node_put(port);
 		if (port == imx_crtc->port) {
 			ret = of_graph_parse_endpoint(ep, &endpoint);
-			return ret ? ret : endpoint.id;
+			return ret ? ret : endpoint.port;
 		}
 	} while (ep);
 
@@ -675,6 +675,11 @@ static int imx_drm_platform_probe(struct platform_device *pdev)
 			if (!remote || !of_device_is_available(remote)) {
 				of_node_put(remote);
 				continue;
+			} else if (!of_device_is_available(remote->parent)) {
+				dev_warn(&pdev->dev, "parent device of %s is not available\n",
+					 remote->full_name);
+				of_node_put(remote);
+				continue;
 			}
 
 			ret = imx_drm_add_component(&pdev->dev, remote);
diff --git a/drivers/staging/imx-drm/imx-hdmi.c b/drivers/staging/imx-drm/imx-hdmi.c
index d47dedd2cdb..1b440483f28 100644
--- a/drivers/staging/imx-drm/imx-hdmi.c
+++ b/drivers/staging/imx-drm/imx-hdmi.c
@@ -120,8 +120,6 @@ struct imx_hdmi {
 	struct clk *isfr_clk;
 	struct clk *iahb_clk;
 
-	enum drm_connector_status connector_status;
-
 	struct hdmi_data_info hdmi_data;
 	int vic;
 
@@ -159,6 +157,7 @@ static inline u8 hdmi_readb(struct imx_hdmi *hdmi, int offset)
 static void hdmi_modb(struct imx_hdmi *hdmi, u8 data, u8 mask, unsigned reg)
 {
 	u8 val = hdmi_readb(hdmi, reg) & ~mask;
+
 	val |= data & mask;
 	hdmi_writeb(hdmi, val, reg);
 }
@@ -659,13 +658,10 @@ static inline void hdmi_phy_test_dout(struct imx_hdmi *hdmi,
 
 static bool hdmi_phy_wait_i2c_done(struct imx_hdmi *hdmi, int msec)
 {
-	unsigned char val = 0;
-	val = hdmi_readb(hdmi, HDMI_IH_I2CMPHY_STAT0) & 0x3;
-	while (!val) {
-		udelay(1000);
+	while ((hdmi_readb(hdmi, HDMI_IH_I2CMPHY_STAT0) & 0x3) == 0) {
 		if (msec-- == 0)
 			return false;
-		val = hdmi_readb(hdmi, HDMI_IH_I2CMPHY_STAT0) & 0x3;
+		udelay(1000);
 	}
 	return true;
 }
@@ -1382,7 +1378,9 @@ static enum drm_connector_status imx_hdmi_connector_detect(struct drm_connector
 {
 	struct imx_hdmi *hdmi = container_of(connector, struct imx_hdmi,
 					     connector);
-	return hdmi->connector_status;
+
+	return hdmi_readb(hdmi, HDMI_PHY_STAT0) & HDMI_PHY_HPD ?
+		connector_status_connected : connector_status_disconnected;
 }
 
 static int imx_hdmi_connector_get_modes(struct drm_connector *connector)
@@ -1524,7 +1522,6 @@ static irqreturn_t imx_hdmi_irq(int irq, void *dev_id)
 
 			hdmi_modb(hdmi, 0, HDMI_PHY_HPD, HDMI_PHY_POL0);
 
-			hdmi->connector_status = connector_status_connected;
 			imx_hdmi_poweron(hdmi);
 		} else {
 			dev_dbg(hdmi->dev, "EVENT=plugout\n");
@@ -1532,7 +1529,6 @@ static irqreturn_t imx_hdmi_irq(int irq, void *dev_id)
 			hdmi_modb(hdmi, HDMI_PHY_HPD, HDMI_PHY_HPD,
 				HDMI_PHY_POL0);
 
-			hdmi->connector_status = connector_status_disconnected;
 			imx_hdmi_poweroff(hdmi);
 		}
 		drm_helper_hpd_irq_event(hdmi->connector.dev);
@@ -1606,12 +1602,12 @@ static int imx_hdmi_bind(struct device *dev, struct device *master, void *data)
 		return -ENOMEM;
 
 	hdmi->dev = dev;
-	hdmi->connector_status = connector_status_disconnected;
 	hdmi->sample_rate = 48000;
 	hdmi->ratio = 100;
 
 	if (of_id) {
 		const struct platform_device_id *device_id = of_id->data;
+
 		hdmi->dev_type = device_id->driver_data;
 	}
 
@@ -1628,7 +1624,7 @@ static int imx_hdmi_bind(struct device *dev, struct device *master, void *data)
 
 	irq = platform_get_irq(pdev, 0);
 	if (irq < 0)
-		return -EINVAL;
+		return irq;
 
 	ret = devm_request_threaded_irq(dev, irq, imx_hdmi_hardirq,
 					imx_hdmi_irq, IRQF_SHARED,
diff --git a/drivers/staging/imx-drm/imx-tve.c b/drivers/staging/imx-drm/imx-tve.c
index 575533f4fd6..a23f4f77314 100644
--- a/drivers/staging/imx-drm/imx-tve.c
+++ b/drivers/staging/imx-drm/imx-tve.c
@@ -582,7 +582,7 @@ static int imx_tve_bind(struct device *dev, struct device *master, void *data)
 	tve->dev = dev;
 	spin_lock_init(&tve->lock);
 
-	ddc_node = of_parse_phandle(np, "i2c-ddc-bus", 0);
+	ddc_node = of_parse_phandle(np, "ddc-i2c-bus", 0);
 	if (ddc_node) {
 		tve->ddc = of_find_i2c_adapter_by_node(ddc_node);
 		of_node_put(ddc_node);
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 c4d14ead583..c2c6fab05ea 100644
--- a/drivers/staging/imx-drm/ipu-v3/imx-ipu-v3.h
+++ b/drivers/staging/imx-drm/ipu-v3/imx-ipu-v3.h
@@ -76,6 +76,7 @@ enum ipu_channel_irq {
 	IPU_IRQ_EOS = 192,
 };
 
+int ipu_map_irq(struct ipu_soc *ipu, int irq);
 int ipu_idmac_channel_irq(struct ipu_soc *ipu, struct ipuv3_channel *channel,
 		enum ipu_channel_irq irq);
 
@@ -114,8 +115,10 @@ struct ipu_dc *ipu_dc_get(struct ipu_soc *ipu, int channel);
 void ipu_dc_put(struct ipu_dc *dc);
 int ipu_dc_init_sync(struct ipu_dc *dc, struct ipu_di *di, bool interlaced,
 		u32 pixel_fmt, u32 width);
+void ipu_dc_enable(struct ipu_soc *ipu);
 void ipu_dc_enable_channel(struct ipu_dc *dc);
 void ipu_dc_disable_channel(struct ipu_dc *dc);
+void ipu_dc_disable(struct ipu_soc *ipu);
 
 /*
  * IPU Display Interface (di) functions
@@ -152,8 +155,10 @@ void ipu_dmfc_put(struct dmfc_channel *dmfc);
 
 struct ipu_dp *ipu_dp_get(struct ipu_soc *ipu, unsigned int flow);
 void ipu_dp_put(struct ipu_dp *);
+int ipu_dp_enable(struct ipu_soc *ipu);
 int ipu_dp_enable_channel(struct ipu_dp *dp);
 void ipu_dp_disable_channel(struct ipu_dp *dp);
+void ipu_dp_disable(struct ipu_soc *ipu);
 int ipu_dp_setup_channel(struct ipu_dp *dp,
 		enum ipu_color_space in, enum ipu_color_space out);
 int ipu_dp_set_window_pos(struct ipu_dp *, u16 x_pos, u16 y_pos);
diff --git a/drivers/staging/imx-drm/ipu-v3/ipu-common.c b/drivers/staging/imx-drm/ipu-v3/ipu-common.c
index ca85d3d70ae..a1f7b2001c8 100644
--- a/drivers/staging/imx-drm/ipu-v3/ipu-common.c
+++ b/drivers/staging/imx-drm/ipu-v3/ipu-common.c
@@ -697,6 +697,12 @@ int ipu_idmac_enable_channel(struct ipuv3_channel *channel)
 }
 EXPORT_SYMBOL_GPL(ipu_idmac_enable_channel);
 
+bool ipu_idmac_channel_busy(struct ipu_soc *ipu, unsigned int chno)
+{
+	return (ipu_idmac_read(ipu, IDMAC_CHA_BUSY(chno)) & idma_mask(chno));
+}
+EXPORT_SYMBOL_GPL(ipu_idmac_channel_busy);
+
 int ipu_idmac_wait_busy(struct ipuv3_channel *channel, int ms)
 {
 	struct ipu_soc *ipu = channel->ipu;
@@ -714,6 +720,22 @@ int ipu_idmac_wait_busy(struct ipuv3_channel *channel, int ms)
 }
 EXPORT_SYMBOL_GPL(ipu_idmac_wait_busy);
 
+int ipu_wait_interrupt(struct ipu_soc *ipu, int irq, int ms)
+{
+	unsigned long timeout;
+
+	timeout = jiffies + msecs_to_jiffies(ms);
+	ipu_cm_write(ipu, BIT(irq % 32), IPU_INT_STAT(irq / 32));
+	while (!(ipu_cm_read(ipu, IPU_INT_STAT(irq / 32) & BIT(irq % 32)))) {
+		if (time_after(jiffies, timeout))
+			return -ETIMEDOUT;
+		cpu_relax();
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_wait_interrupt);
+
 int ipu_idmac_disable_channel(struct ipuv3_channel *channel)
 {
 	struct ipu_soc *ipu = channel->ipu;
@@ -900,7 +922,8 @@ static void ipu_irq_handle(struct ipu_soc *ipu, const int *regs, int num_regs)
 		status &= ipu_cm_read(ipu, IPU_INT_CTRL(regs[i]));
 
 		for_each_set_bit(bit, &status, 32) {
-			irq = irq_linear_revmap(ipu->domain, regs[i] * 32 + bit);
+			irq = irq_linear_revmap(ipu->domain,
+						regs[i] * 32 + bit);
 			if (irq)
 				generic_handle_irq(irq);
 		}
@@ -933,15 +956,22 @@ static void ipu_err_irq_handler(unsigned int irq, struct irq_desc *desc)
 	chained_irq_exit(chip, desc);
 }
 
-int ipu_idmac_channel_irq(struct ipu_soc *ipu, struct ipuv3_channel *channel,
-		enum ipu_channel_irq irq_type)
+int ipu_map_irq(struct ipu_soc *ipu, int irq)
 {
-	int irq = irq_linear_revmap(ipu->domain, irq_type + channel->num);
+	int virq;
 
-	if (!irq)
-		irq = irq_create_mapping(ipu->domain, irq_type + channel->num);
+	virq = irq_linear_revmap(ipu->domain, irq);
+	if (!virq)
+		virq = irq_create_mapping(ipu->domain, irq);
 
-	return irq;
+	return virq;
+}
+EXPORT_SYMBOL_GPL(ipu_map_irq);
+
+int ipu_idmac_channel_irq(struct ipu_soc *ipu, struct ipuv3_channel *channel,
+		enum ipu_channel_irq irq_type)
+{
+	return ipu_map_irq(ipu, irq_type + channel->num);
 }
 EXPORT_SYMBOL_GPL(ipu_idmac_channel_irq);
 
@@ -1053,7 +1083,8 @@ static int ipu_irq_init(struct ipu_soc *ipu)
 	}
 
 	ret = irq_alloc_domain_generic_chips(ipu->domain, 32, 1, "IPU",
-					     handle_level_irq, 0, IRQF_VALID, 0);
+					     handle_level_irq, 0,
+					     IRQF_VALID, 0);
 	if (ret < 0) {
 		dev_err(ipu->dev, "failed to alloc generic irq chips\n");
 		irq_domain_remove(ipu->domain);
diff --git a/drivers/staging/imx-drm/ipu-v3/ipu-dc.c b/drivers/staging/imx-drm/ipu-v3/ipu-dc.c
index d5de8bb5c80..784a4a13eac 100644
--- a/drivers/staging/imx-drm/ipu-v3/ipu-dc.c
+++ b/drivers/staging/imx-drm/ipu-v3/ipu-dc.c
@@ -18,6 +18,7 @@
 #include <linux/types.h>
 #include <linux/errno.h>
 #include <linux/delay.h>
+#include <linux/interrupt.h>
 #include <linux/io.h>
 
 #include "../imx-drm.h"
@@ -91,6 +92,7 @@ enum ipu_dc_map {
 	IPU_DC_MAP_RGB565,
 	IPU_DC_MAP_GBR24, /* TVEv2 */
 	IPU_DC_MAP_BGR666,
+	IPU_DC_MAP_LVDS666,
 	IPU_DC_MAP_BGR24,
 };
 
@@ -110,6 +112,9 @@ struct ipu_dc_priv {
 	struct device		*dev;
 	struct ipu_dc		channels[IPU_DC_NUM_CHANNELS];
 	struct mutex		mutex;
+	struct completion	comp;
+	int			dc_irq;
+	int			dp_irq;
 };
 
 static void dc_link_event(struct ipu_dc *dc, int event, int addr, int priority)
@@ -153,6 +158,8 @@ static int ipu_pixfmt_to_map(u32 fmt)
 		return IPU_DC_MAP_GBR24;
 	case V4L2_PIX_FMT_BGR666:
 		return IPU_DC_MAP_BGR666;
+	case v4l2_fourcc('L', 'V', 'D', '6'):
+		return IPU_DC_MAP_LVDS666;
 	case V4L2_PIX_FMT_BGR24:
 		return IPU_DC_MAP_BGR24;
 	default:
@@ -220,12 +227,16 @@ int ipu_dc_init_sync(struct ipu_dc *dc, struct ipu_di *di, bool interlaced,
 	writel(0x0, dc->base + DC_WR_CH_ADDR);
 	writel(width, priv->dc_reg + DC_DISP_CONF2(dc->di));
 
-	ipu_module_enable(priv->ipu, IPU_CONF_DC_EN);
-
 	return 0;
 }
 EXPORT_SYMBOL_GPL(ipu_dc_init_sync);
 
+void ipu_dc_enable(struct ipu_soc *ipu)
+{
+	ipu_module_enable(ipu, IPU_CONF_DC_EN);
+}
+EXPORT_SYMBOL_GPL(ipu_dc_enable);
+
 void ipu_dc_enable_channel(struct ipu_dc *dc)
 {
 	int di;
@@ -239,41 +250,55 @@ void ipu_dc_enable_channel(struct ipu_dc *dc)
 }
 EXPORT_SYMBOL_GPL(ipu_dc_enable_channel);
 
+static irqreturn_t dc_irq_handler(int irq, void *dev_id)
+{
+	struct ipu_dc *dc = dev_id;
+	u32 reg;
+
+	reg = readl(dc->base + DC_WR_CH_CONF);
+	reg &= ~DC_WR_CH_CONF_PROG_TYPE_MASK;
+	writel(reg, dc->base + DC_WR_CH_CONF);
+
+	/* The Freescale BSP kernel clears DIx_COUNTER_RELEASE here */
+
+	complete(&dc->priv->comp);
+	return IRQ_HANDLED;
+}
+
 void ipu_dc_disable_channel(struct ipu_dc *dc)
 {
 	struct ipu_dc_priv *priv = dc->priv;
+	int irq, ret;
 	u32 val;
-	int irq = 0, timeout = 50;
 
+	/* TODO: Handle MEM_FG_SYNC differently from MEM_BG_SYNC */
 	if (dc->chno == 1)
-		irq = IPU_IRQ_DC_FC_1;
+		irq = priv->dc_irq;
 	else if (dc->chno == 5)
-		irq = IPU_IRQ_DP_SF_END;
+		irq = priv->dp_irq;
 	else
 		return;
 
-	/* should wait for the interrupt here */
-	mdelay(50);
+	init_completion(&priv->comp);
+	enable_irq(irq);
+	ret = wait_for_completion_timeout(&priv->comp, msecs_to_jiffies(50));
+	disable_irq(irq);
+	if (ret <= 0) {
+		dev_warn(priv->dev, "DC stop timeout after 50 ms\n");
 
-	if (dc->di == 0)
-		val = 0x00000002;
-	else
-		val = 0x00000020;
-
-	/* Wait for DC triple buffer to empty */
-	while ((readl(priv->dc_reg + DC_STAT) & val) != val) {
-		usleep_range(2000, 20000);
-		timeout -= 2;
-		if (timeout <= 0)
-			break;
+		val = readl(dc->base + DC_WR_CH_CONF);
+		val &= ~DC_WR_CH_CONF_PROG_TYPE_MASK;
+		writel(val, dc->base + DC_WR_CH_CONF);
 	}
-
-	val = readl(dc->base + DC_WR_CH_CONF);
-	val &= ~DC_WR_CH_CONF_PROG_TYPE_MASK;
-	writel(val, dc->base + DC_WR_CH_CONF);
 }
 EXPORT_SYMBOL_GPL(ipu_dc_disable_channel);
 
+void ipu_dc_disable(struct ipu_soc *ipu)
+{
+	ipu_module_disable(ipu, IPU_CONF_DC_EN);
+}
+EXPORT_SYMBOL_GPL(ipu_dc_disable);
+
 static void ipu_dc_map_config(struct ipu_dc_priv *priv, enum ipu_dc_map map,
 		int byte_num, int offset, int mask)
 {
@@ -340,7 +365,7 @@ int ipu_dc_init(struct ipu_soc *ipu, struct device *dev,
 	struct ipu_dc_priv *priv;
 	static int channel_offsets[] = { 0, 0x1c, 0x38, 0x54, 0x58, 0x5c,
 		0x78, 0, 0x94, 0xb4};
-	int i;
+	int i, ret;
 
 	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
 	if (!priv)
@@ -361,13 +386,31 @@ int ipu_dc_init(struct ipu_soc *ipu, struct device *dev,
 		priv->channels[i].base = priv->dc_reg + channel_offsets[i];
 	}
 
+	priv->dc_irq = ipu_map_irq(ipu, IPU_IRQ_DC_FC_1);
+	if (!priv->dc_irq)
+		return -EINVAL;
+	ret = devm_request_irq(dev, priv->dc_irq, dc_irq_handler, 0, NULL,
+			       &priv->channels[1]);
+	if (ret < 0)
+		return ret;
+	disable_irq(priv->dc_irq);
+	priv->dp_irq = ipu_map_irq(ipu, IPU_IRQ_DP_SF_END);
+	if (!priv->dp_irq)
+		return -EINVAL;
+	ret = devm_request_irq(dev, priv->dp_irq, dc_irq_handler, 0, NULL,
+			       &priv->channels[5]);
+	if (ret < 0)
+		return ret;
+	disable_irq(priv->dp_irq);
+
 	writel(DC_WR_CH_CONF_WORD_SIZE_24 | DC_WR_CH_CONF_DISP_ID_PARALLEL(1) |
 			DC_WR_CH_CONF_PROG_DI_ID,
 			priv->channels[1].base + DC_WR_CH_CONF);
 	writel(DC_WR_CH_CONF_WORD_SIZE_24 | DC_WR_CH_CONF_DISP_ID_PARALLEL(0),
 			priv->channels[5].base + DC_WR_CH_CONF);
 
-	writel(DC_GEN_SYNC_1_6_SYNC | DC_GEN_SYNC_PRIORITY_1, priv->dc_reg + DC_GEN);
+	writel(DC_GEN_SYNC_1_6_SYNC | DC_GEN_SYNC_PRIORITY_1,
+		priv->dc_reg + DC_GEN);
 
 	ipu->dc_priv = priv;
 
@@ -398,6 +441,12 @@ int ipu_dc_init(struct ipu_soc *ipu, struct device *dev,
 	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 */
 
+	/* lvds666 */
+	ipu_dc_map_clear(priv, IPU_DC_MAP_LVDS666);
+	ipu_dc_map_config(priv, IPU_DC_MAP_LVDS666, 0, 5, 0xfc); /* blue */
+	ipu_dc_map_config(priv, IPU_DC_MAP_LVDS666, 1, 13, 0xfc); /* green */
+	ipu_dc_map_config(priv, IPU_DC_MAP_LVDS666, 2, 21, 0xfc); /* red */
+
 	/* bgr24 */
 	ipu_dc_map_clear(priv, IPU_DC_MAP_BGR24);
 	ipu_dc_map_config(priv, IPU_DC_MAP_BGR24, 2, 7, 0xff); /* red */
diff --git a/drivers/staging/imx-drm/ipu-v3/ipu-di.c b/drivers/staging/imx-drm/ipu-v3/ipu-di.c
index 82a9ebad697..849b3e120ef 100644
--- a/drivers/staging/imx-drm/ipu-v3/ipu-di.c
+++ b/drivers/staging/imx-drm/ipu-v3/ipu-di.c
@@ -595,7 +595,7 @@ int ipu_di_init_sync_panel(struct ipu_di *di, struct ipu_di_signal_cfg *sig)
 		}
 	}
 
-	if (!sig->clk_pol)
+	if (sig->clk_pol)
 		di_gen |= DI_GEN_POLARITY_DISP_CLK;
 
 	ipu_di_write(di, di_gen, DI_GENERAL);
diff --git a/drivers/staging/imx-drm/ipu-v3/ipu-dmfc.c b/drivers/staging/imx-drm/ipu-v3/ipu-dmfc.c
index 45213017fa4..59f182b28fc 100644
--- a/drivers/staging/imx-drm/ipu-v3/ipu-dmfc.c
+++ b/drivers/staging/imx-drm/ipu-v3/ipu-dmfc.c
@@ -28,7 +28,12 @@
 #define DMFC_GENERAL1		0x0014
 #define DMFC_GENERAL2		0x0018
 #define DMFC_IC_CTRL		0x001c
-#define DMFC_STAT		0x0020
+#define DMFC_WR_CHAN_ALT	0x0020
+#define DMFC_WR_CHAN_DEF_ALT	0x0024
+#define DMFC_DP_CHAN_ALT	0x0028
+#define DMFC_DP_CHAN_DEF_ALT	0x002c
+#define DMFC_GENERAL1_ALT	0x0030
+#define DMFC_STAT		0x0034
 
 #define DMFC_WR_CHAN_1_28		0
 #define DMFC_WR_CHAN_2_41		8
@@ -133,6 +138,20 @@ int ipu_dmfc_enable_channel(struct dmfc_channel *dmfc)
 }
 EXPORT_SYMBOL_GPL(ipu_dmfc_enable_channel);
 
+static void ipu_dmfc_wait_fifos(struct ipu_dmfc_priv *priv)
+{
+	unsigned long timeout = jiffies + msecs_to_jiffies(1000);
+
+	while ((readl(priv->base + DMFC_STAT) & 0x02fff000) != 0x02fff000) {
+		if (time_after(jiffies, timeout)) {
+			dev_warn(priv->dev,
+				 "Timeout waiting for DMFC FIFOs to clear\n");
+			break;
+		}
+		cpu_relax();
+	}
+}
+
 void ipu_dmfc_disable_channel(struct dmfc_channel *dmfc)
 {
 	struct ipu_dmfc_priv *priv = dmfc->priv;
@@ -141,8 +160,10 @@ void ipu_dmfc_disable_channel(struct dmfc_channel *dmfc)
 
 	priv->use_count--;
 
-	if (!priv->use_count)
+	if (!priv->use_count) {
+		ipu_dmfc_wait_fifos(priv);
 		ipu_module_disable(priv->ipu, IPU_CONF_DMFC_EN);
+	}
 
 	if (priv->use_count < 0)
 		priv->use_count = 0;
diff --git a/drivers/staging/imx-drm/ipu-v3/ipu-dp.c b/drivers/staging/imx-drm/ipu-v3/ipu-dp.c
index 58f87c8d7c0..d90f82a87d1 100644
--- a/drivers/staging/imx-drm/ipu-v3/ipu-dp.c
+++ b/drivers/staging/imx-drm/ipu-v3/ipu-dp.c
@@ -215,10 +215,9 @@ int ipu_dp_setup_channel(struct ipu_dp *dp,
 }
 EXPORT_SYMBOL_GPL(ipu_dp_setup_channel);
 
-int ipu_dp_enable_channel(struct ipu_dp *dp)
+int ipu_dp_enable(struct ipu_soc *ipu)
 {
-	struct ipu_flow *flow = to_flow(dp);
-	struct ipu_dp_priv *priv = flow->priv;
+	struct ipu_dp_priv *priv = ipu->dp_priv;
 
 	mutex_lock(&priv->mutex);
 
@@ -227,15 +226,28 @@ int ipu_dp_enable_channel(struct ipu_dp *dp)
 
 	priv->use_count++;
 
-	if (dp->foreground) {
-		u32 reg;
+	mutex_unlock(&priv->mutex);
 
-		reg = readl(flow->base + DP_COM_CONF);
-		reg |= DP_COM_CONF_FG_EN;
-		writel(reg, flow->base + DP_COM_CONF);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_dp_enable);
 
-		ipu_srm_dp_sync_update(priv->ipu);
-	}
+int ipu_dp_enable_channel(struct ipu_dp *dp)
+{
+	struct ipu_flow *flow = to_flow(dp);
+	struct ipu_dp_priv *priv = flow->priv;
+	u32 reg;
+
+	if (!dp->foreground)
+		return 0;
+
+	mutex_lock(&priv->mutex);
+
+	reg = readl(flow->base + DP_COM_CONF);
+	reg |= DP_COM_CONF_FG_EN;
+	writel(reg, flow->base + DP_COM_CONF);
+
+	ipu_srm_dp_sync_update(priv->ipu);
 
 	mutex_unlock(&priv->mutex);
 
@@ -247,25 +259,38 @@ void ipu_dp_disable_channel(struct ipu_dp *dp)
 {
 	struct ipu_flow *flow = to_flow(dp);
 	struct ipu_dp_priv *priv = flow->priv;
+	u32 reg, csc;
+
+	if (!dp->foreground)
+		return;
 
 	mutex_lock(&priv->mutex);
 
-	priv->use_count--;
+	reg = readl(flow->base + DP_COM_CONF);
+	csc = reg & DP_COM_CONF_CSC_DEF_MASK;
+	if (csc == DP_COM_CONF_CSC_DEF_FG)
+		reg &= ~DP_COM_CONF_CSC_DEF_MASK;
 
-	if (dp->foreground) {
-		u32 reg, csc;
+	reg &= ~DP_COM_CONF_FG_EN;
+	writel(reg, flow->base + DP_COM_CONF);
 
-		reg = readl(flow->base + DP_COM_CONF);
-		csc = reg & DP_COM_CONF_CSC_DEF_MASK;
-		if (csc == DP_COM_CONF_CSC_DEF_FG)
-			reg &= ~DP_COM_CONF_CSC_DEF_MASK;
+	writel(0, flow->base + DP_FG_POS);
+	ipu_srm_dp_sync_update(priv->ipu);
 
-		reg &= ~DP_COM_CONF_FG_EN;
-		writel(reg, flow->base + DP_COM_CONF);
+	if (ipu_idmac_channel_busy(priv->ipu, IPUV3_CHANNEL_MEM_BG_SYNC))
+		ipu_wait_interrupt(priv->ipu, IPU_IRQ_DP_SF_END, 50);
 
-		writel(0, flow->base + DP_FG_POS);
-		ipu_srm_dp_sync_update(priv->ipu);
-	}
+	mutex_unlock(&priv->mutex);
+}
+EXPORT_SYMBOL_GPL(ipu_dp_disable_channel);
+
+void ipu_dp_disable(struct ipu_soc *ipu)
+{
+	struct ipu_dp_priv *priv = ipu->dp_priv;
+
+	mutex_lock(&priv->mutex);
+
+	priv->use_count--;
 
 	if (!priv->use_count)
 		ipu_module_disable(priv->ipu, IPU_CONF_DP_EN);
@@ -275,7 +300,7 @@ void ipu_dp_disable_channel(struct ipu_dp *dp)
 
 	mutex_unlock(&priv->mutex);
 }
-EXPORT_SYMBOL_GPL(ipu_dp_disable_channel);
+EXPORT_SYMBOL_GPL(ipu_dp_disable);
 
 struct ipu_dp *ipu_dp_get(struct ipu_soc *ipu, unsigned int flow)
 {
diff --git a/drivers/staging/imx-drm/ipu-v3/ipu-prv.h b/drivers/staging/imx-drm/ipu-v3/ipu-prv.h
index 4df00501adc..bfc1b336648 100644
--- a/drivers/staging/imx-drm/ipu-v3/ipu-prv.h
+++ b/drivers/staging/imx-drm/ipu-v3/ipu-prv.h
@@ -185,6 +185,9 @@ void ipu_srm_dp_sync_update(struct ipu_soc *ipu);
 int ipu_module_enable(struct ipu_soc *ipu, u32 mask);
 int ipu_module_disable(struct ipu_soc *ipu, u32 mask);
 
+bool ipu_idmac_channel_busy(struct ipu_soc *ipu, unsigned int chno);
+int ipu_wait_interrupt(struct ipu_soc *ipu, int irq, int ms);
+
 int ipu_di_init(struct ipu_soc *ipu, struct device *dev, int id,
 		unsigned long base, u32 module, struct clk *ipu_clk);
 void ipu_di_exit(struct ipu_soc *ipu, int id);
diff --git a/drivers/staging/imx-drm/ipuv3-crtc.c b/drivers/staging/imx-drm/ipuv3-crtc.c
index c48f640db00..47bec5e1735 100644
--- a/drivers/staging/imx-drm/ipuv3-crtc.c
+++ b/drivers/staging/imx-drm/ipuv3-crtc.c
@@ -60,24 +60,32 @@ struct ipu_crtc {
 
 static void ipu_fb_enable(struct ipu_crtc *ipu_crtc)
 {
+	struct ipu_soc *ipu = dev_get_drvdata(ipu_crtc->dev->parent);
+
 	if (ipu_crtc->enabled)
 		return;
 
-	ipu_di_enable(ipu_crtc->di);
-	ipu_dc_enable_channel(ipu_crtc->dc);
+	ipu_dc_enable(ipu);
 	ipu_plane_enable(ipu_crtc->plane[0]);
+	/* Start DC channel and DI after IDMAC */
+	ipu_dc_enable_channel(ipu_crtc->dc);
+	ipu_di_enable(ipu_crtc->di);
 
 	ipu_crtc->enabled = 1;
 }
 
 static void ipu_fb_disable(struct ipu_crtc *ipu_crtc)
 {
+	struct ipu_soc *ipu = dev_get_drvdata(ipu_crtc->dev->parent);
+
 	if (!ipu_crtc->enabled)
 		return;
 
-	ipu_plane_disable(ipu_crtc->plane[0]);
+	/* Stop DC channel and DI before IDMAC */
 	ipu_dc_disable_channel(ipu_crtc->dc);
 	ipu_di_disable(ipu_crtc->di);
+	ipu_plane_disable(ipu_crtc->plane[0]);
+	ipu_dc_disable(ipu);
 
 	ipu_crtc->enabled = 0;
 }
@@ -158,7 +166,7 @@ static int ipu_crtc_mode_set(struct drm_crtc *crtc,
 		sig_cfg.Vsync_pol = 1;
 
 	sig_cfg.enable_pol = 1;
-	sig_cfg.clk_pol = 1;
+	sig_cfg.clk_pol = 0;
 	sig_cfg.width = mode->hdisplay;
 	sig_cfg.height = mode->vdisplay;
 	sig_cfg.pixel_fmt = out_pixel_fmt;
diff --git a/drivers/staging/imx-drm/ipuv3-plane.c b/drivers/staging/imx-drm/ipuv3-plane.c
index 27a8d735dae..5697e59ddf1 100644
--- a/drivers/staging/imx-drm/ipuv3-plane.c
+++ b/drivers/staging/imx-drm/ipuv3-plane.c
@@ -239,6 +239,8 @@ err_out:
 
 void ipu_plane_enable(struct ipu_plane *ipu_plane)
 {
+	if (ipu_plane->dp)
+		ipu_dp_enable(ipu_plane->ipu);
 	ipu_dmfc_enable_channel(ipu_plane->dmfc);
 	ipu_idmac_enable_channel(ipu_plane->ipu_ch);
 	if (ipu_plane->dp)
@@ -257,6 +259,8 @@ void ipu_plane_disable(struct ipu_plane *ipu_plane)
 		ipu_dp_disable_channel(ipu_plane->dp);
 	ipu_idmac_disable_channel(ipu_plane->ipu_ch);
 	ipu_dmfc_disable_channel(ipu_plane->dmfc);
+	if (ipu_plane->dp)
+		ipu_dp_disable(ipu_plane->ipu);
 }
 
 static void ipu_plane_dpms(struct ipu_plane *ipu_plane, int mode)
diff --git a/drivers/staging/imx-drm/parallel-display.c b/drivers/staging/imx-drm/parallel-display.c
index c60b6c645f4..eaf4dda1a0c 100644
--- a/drivers/staging/imx-drm/parallel-display.c
+++ b/drivers/staging/imx-drm/parallel-display.c
@@ -219,6 +219,8 @@ static int imx_pd_bind(struct device *dev, struct device *master, void *data)
 			imxpd->interface_pix_fmt = V4L2_PIX_FMT_RGB565;
 		else if (!strcmp(fmt, "bgr666"))
 			imxpd->interface_pix_fmt = V4L2_PIX_FMT_BGR666;
+		else if (!strcmp(fmt, "lvds666"))
+			imxpd->interface_pix_fmt = v4l2_fourcc('L', 'V', 'D', '6');
 	}
 
 	panel_node = of_parse_phandle(np, "fsl,panel", 0);
diff --git a/drivers/staging/keucr/init.c b/drivers/staging/keucr/init.c
index e6118390654..1e7449d6d12 100644
--- a/drivers/staging/keucr/init.c
+++ b/drivers/staging/keucr/init.c
@@ -9,6 +9,7 @@
 #include "usb.h"
 #include "scsiglue.h"
 #include "transport.h"
+#include "smil.h"
 #include "init.h"
 
 /*
diff --git a/drivers/staging/keucr/init.h b/drivers/staging/keucr/init.h
index 98d2e3ba854..d1367e726ff 100644
--- a/drivers/staging/keucr/init.h
+++ b/drivers/staging/keucr/init.h
@@ -1,10 +1,5 @@
 #include "common.h"
 
-extern u32 MediaChange;
-extern int Check_D_MediaFmt(struct us_data *);
-
-
-
 static u8 SM_Init[] = {
 0x7B, 0x09, 0x7C, 0xF0, 0x7D, 0x10, 0x7E, 0xE9,
 0x7F, 0xCC, 0x12, 0x2F, 0x71, 0x90, 0xE9, 0xCC,
diff --git a/drivers/staging/keucr/scsiglue.c b/drivers/staging/keucr/scsiglue.c
index ac3d34dcc43..7d8d444910c 100644
--- a/drivers/staging/keucr/scsiglue.c
+++ b/drivers/staging/keucr/scsiglue.c
@@ -106,7 +106,7 @@ static int queuecommand_lck(struct scsi_cmnd *srb,
 	/* check for state-transition errors */
 	if (us->srb != NULL) {
 		/* pr_info("Error in %s: us->srb = %p\n"
-				 __FUNCTION__, us->srb); */
+				 __func__, us->srb); */
 		return SCSI_MLQUEUE_HOST_BUSY;
 	}
 
diff --git a/drivers/staging/keucr/smil.h b/drivers/staging/keucr/smil.h
index 39951738d23..f938759337e 100644
--- a/drivers/staging/keucr/smil.h
+++ b/drivers/staging/keucr/smil.h
@@ -204,6 +204,7 @@ extern struct keucr_media_area    CisArea;
 int         Init_D_SmartMedia(void);
 int         Pwoff_D_SmartMedia(void);
 int         Check_D_SmartMedia(void);
+int         Check_D_MediaFmt(struct us_data *);
 int         Check_D_Parameter(struct us_data *, u16 *, u8 *, u8 *);
 int         Media_D_ReadSector(struct us_data *, u32, u16, u8 *);
 int         Media_D_WriteSector(struct us_data *, u32, u16, u8 *);
@@ -284,6 +285,4 @@ u8 correct_data(u8 *, u8 *, u8,   u8,   u8);
 int  _Correct_D_SwECC(u8 *, u8 *, u8 *);
 void _Calculate_D_SwECC(u8 *, u8 *);
 
-void SM_Init(void);
-
 #endif /* already included */
diff --git a/drivers/staging/keucr/smilmain.c b/drivers/staging/keucr/smilmain.c
index fc7cbc6f8a4..42ec8a669ad 100644
--- a/drivers/staging/keucr/smilmain.c
+++ b/drivers/staging/keucr/smilmain.c
@@ -141,23 +141,6 @@ int Media_D_CopySector(struct us_data *us, u32 start, u16 count, u8 *buf)
 	return NO_ERROR;
 }
 
-/* ----- Release_D_CopySector() ------------------------------------------ */
-static int Release_D_CopySector(struct us_data *us)
-{
-	Log2Phy[Media.Zone][Media.LogBlock] = WriteBlock;
-	Media.PhyBlock = ReadBlock;
-
-	if (Media.PhyBlock == NO_ASSIGN) {
-		Media.PhyBlock = WriteBlock;
-		return SMSUCCESS;
-	}
-
-	Clr_D_Bit(Assign[Media.Zone], Media.PhyBlock);
-	Media.PhyBlock = WriteBlock;
-
-	return SMSUCCESS;
-}
-
 /* SmartMedia Physical Format Test Subroutine */
 /* ----- Check_D_MediaFmt() --------------------------------------------- */
 int Check_D_MediaFmt(struct us_data *us)
@@ -184,6 +167,23 @@ int Check_D_MediaFmt(struct us_data *us)
 	return SMSUCCESS;
 }
 
+/* ----- Release_D_CopySector() ------------------------------------------ */
+static int Release_D_CopySector(struct us_data *us)
+{
+	Log2Phy[Media.Zone][Media.LogBlock] = WriteBlock;
+	Media.PhyBlock = ReadBlock;
+
+	if (Media.PhyBlock == NO_ASSIGN) {
+		Media.PhyBlock = WriteBlock;
+		return SMSUCCESS;
+	}
+
+	Clr_D_Bit(Assign[Media.Zone], Media.PhyBlock);
+	Media.PhyBlock = WriteBlock;
+
+	return SMSUCCESS;
+}
+
 /* SmartMedia Physical Address Control Subroutine */
 /* ----- Conv_D_MediaAddr() --------------------------------------------- */
 static int Conv_D_MediaAddr(struct us_data *us, u32 addr)
diff --git a/drivers/staging/keucr/smilsub.c b/drivers/staging/keucr/smilsub.c
index 44ced826503..e981f14f3bf 100644
--- a/drivers/staging/keucr/smilsub.c
+++ b/drivers/staging/keucr/smilsub.c
@@ -133,6 +133,7 @@ void Set_D_LogBlockAddr(u8 *redundant)
 void Set_D_FailBlock(u8 *redundant)
 {
 	char i;
+
 	for (i = 0; i < REDTSIZE; i++)
 		*redundant++ = (u8)((i == REDT_BLOCK) ? 0xF0 : 0xFF);
 }
diff --git a/drivers/staging/keucr/transport.c b/drivers/staging/keucr/transport.c
index ae9414755d2..5e59525271f 100644
--- a/drivers/staging/keucr/transport.c
+++ b/drivers/staging/keucr/transport.c
@@ -669,6 +669,7 @@ int usb_stor_Bulk_transport(struct scsi_cmnd *srb, struct us_data *us)
 	/*  R/W data */
 	if (transfer_length) {
 		unsigned int pipe;
+
 		if (srb->sc_data_direction == DMA_FROM_DEVICE)
 			pipe = us->recv_bulk_pipe;
 		else
diff --git a/drivers/staging/line6/capture.c b/drivers/staging/line6/capture.c
index 0eda51d2e9a..e6ca631e3f7 100644
--- a/drivers/staging/line6/capture.c
+++ b/drivers/staging/line6/capture.c
@@ -97,6 +97,7 @@ void line6_unlink_audio_in_urbs(struct snd_line6_pcm *line6pcm)
 		if (test_bit(i, &line6pcm->active_urb_in)) {
 			if (!test_and_set_bit(i, &line6pcm->unlink_urb_in)) {
 				struct urb *u = line6pcm->urb_audio_in[i];
+
 				usb_unlink_urb(u);
 			}
 		}
@@ -334,6 +335,7 @@ static int snd_line6_capture_hw_params(struct snd_pcm_substream *substream,
 static int snd_line6_capture_hw_free(struct snd_pcm_substream *substream)
 {
 	struct snd_line6_pcm *line6pcm = snd_pcm_substream_chip(substream);
+
 	line6_pcm_release(line6pcm, LINE6_BIT_PCM_ALSA_CAPTURE_BUFFER);
 	return snd_pcm_lib_free_pages(substream);
 }
@@ -380,6 +382,7 @@ static snd_pcm_uframes_t
 snd_line6_capture_pointer(struct snd_pcm_substream *substream)
 {
 	struct snd_line6_pcm *line6pcm = snd_pcm_substream_chip(substream);
+
 	return line6pcm->pos_in_done;
 }
 
diff --git a/drivers/staging/line6/driver.c b/drivers/staging/line6/driver.c
index 77f1b421e95..ef511c76a6e 100644
--- a/drivers/staging/line6/driver.c
+++ b/drivers/staging/line6/driver.c
@@ -124,6 +124,7 @@ static int line6_send_raw_message_async_part(struct message *msg,
 static int line6_start_listen(struct usb_line6 *line6)
 {
 	int err;
+
 	usb_fill_int_urb(line6->urb_listen, line6->usbdev,
 			 usb_rcvintpipe(line6->usbdev, line6->ep_control_read),
 			 line6->buffer_listen, LINE6_BUFSIZE_LISTEN,
diff --git a/drivers/staging/line6/midi.c b/drivers/staging/line6/midi.c
index 02345fb06e3..1ac343b649c 100644
--- a/drivers/staging/line6/midi.c
+++ b/drivers/staging/line6/midi.c
@@ -183,6 +183,7 @@ static void line6_midi_output_drain(struct snd_rawmidi_substream *substream)
 	struct usb_line6 *line6 =
 	    line6_rawmidi_substream_midi(substream)->line6;
 	struct snd_line6_midi *midi = line6->line6midi;
+
 	wait_event_interruptible(midi->send_wait,
 				 midi->num_active_send_urbs == 0);
 }
@@ -260,6 +261,7 @@ static int snd_line6_new_midi(struct snd_line6_midi *line6midi)
 static int snd_line6_midi_free(struct snd_device *device)
 {
 	struct snd_line6_midi *line6midi = device->device_data;
+
 	line6_midibuf_destroy(&line6midi->midibuf_in);
 	line6_midibuf_destroy(&line6midi->midibuf_out);
 	return 0;
diff --git a/drivers/staging/line6/pcm.c b/drivers/staging/line6/pcm.c
index 661080b3c39..a3136b189ee 100644
--- a/drivers/staging/line6/pcm.c
+++ b/drivers/staging/line6/pcm.c
@@ -475,7 +475,7 @@ int line6_init_pcm(struct usb_line6 *line6,
 		MISSING_CASE;
 	}
 
-	line6pcm = kzalloc(sizeof(struct snd_line6_pcm), GFP_KERNEL);
+	line6pcm = kzalloc(sizeof(*line6pcm), GFP_KERNEL);
 
 	if (line6pcm == NULL)
 		return -ENOMEM;
diff --git a/drivers/staging/line6/playback.c b/drivers/staging/line6/playback.c
index 41869caf19a..2ca8900e68c 100644
--- a/drivers/staging/line6/playback.c
+++ b/drivers/staging/line6/playback.c
@@ -34,6 +34,7 @@ static void change_volume(struct urb *urb_out, int volume[],
 
 	if (bytes_per_frame == 4) {
 		short *p, *buf_end;
+
 		p = (short *)urb_out->transfer_buffer;
 		buf_end = p + urb_out->transfer_buffer_length / sizeof(*p);
 
@@ -43,11 +44,13 @@ static void change_volume(struct urb *urb_out, int volume[],
 		}
 	} else if (bytes_per_frame == 6) {
 		unsigned char *p, *buf_end;
+
 		p = (unsigned char *)urb_out->transfer_buffer;
 		buf_end = p + urb_out->transfer_buffer_length;
 
 		for (; p < buf_end; p += 3) {
 			int val;
+
 			val = p[0] + (p[1] << 8) + ((signed char)p[2] << 16);
 			val = (val * volume[chn & 1]) >> 8;
 			p[0] = val;
@@ -116,6 +119,7 @@ static void add_monitor_signal(struct urb *urb_out, unsigned char *signal,
 
 	if (bytes_per_frame == 4) {
 		short *pi, *po, *buf_end;
+
 		pi = (short *)signal;
 		po = (short *)urb_out->transfer_buffer;
 		buf_end = po + urb_out->transfer_buffer_length / sizeof(*po);
@@ -171,6 +175,7 @@ static int submit_audio_out_urb(struct snd_line6_pcm *line6pcm)
 
 		if (fsize == 0) {
 			int n;
+
 			line6pcm->count_out += frame_increment;
 			n = line6pcm->count_out / frame_factor;
 			line6pcm->count_out -= n * frame_factor;
@@ -207,6 +212,7 @@ static int submit_audio_out_urb(struct snd_line6_pcm *line6pcm)
 			   copy the data to the temp buffer.
 			 */
 			int len;
+
 			len = runtime->buffer_size - line6pcm->pos_out;
 
 			if (len > 0) {
@@ -305,6 +311,7 @@ void line6_unlink_audio_out_urbs(struct snd_line6_pcm *line6pcm)
 		if (test_bit(i, &line6pcm->active_urb_out)) {
 			if (!test_and_set_bit(i, &line6pcm->unlink_urb_out)) {
 				struct urb *u = line6pcm->urb_audio_out[i];
+
 				usb_unlink_urb(u);
 			}
 		}
@@ -358,7 +365,6 @@ static void audio_out_callback(struct urb *urb)
 {
 	int i, index, length = 0, shutdown = 0;
 	unsigned long flags;
-
 	struct snd_line6_pcm *line6pcm = (struct snd_line6_pcm *)urb->context;
 	struct snd_pcm_substream *substream =
 	    get_substream(line6pcm, SNDRV_PCM_STREAM_PLAYBACK);
@@ -384,6 +390,7 @@ static void audio_out_callback(struct urb *urb)
 
 	if (test_bit(LINE6_INDEX_PCM_ALSA_PLAYBACK_STREAM, &line6pcm->flags)) {
 		struct snd_pcm_runtime *runtime = substream->runtime;
+
 		line6pcm->pos_out_done +=
 		    length / line6pcm->properties->bytes_per_frame;
 
@@ -480,6 +487,7 @@ static int snd_line6_playback_hw_params(struct snd_pcm_substream *substream,
 static int snd_line6_playback_hw_free(struct snd_pcm_substream *substream)
 {
 	struct snd_line6_pcm *line6pcm = snd_pcm_substream_chip(substream);
+
 	line6_pcm_release(line6pcm, LINE6_BIT_PCM_ALSA_PLAYBACK_BUFFER);
 	return snd_pcm_lib_free_pages(substream);
 }
@@ -534,6 +542,7 @@ static snd_pcm_uframes_t
 snd_line6_playback_pointer(struct snd_pcm_substream *substream)
 {
 	struct snd_line6_pcm *line6pcm = snd_pcm_substream_chip(substream);
+
 	return line6pcm->pos_out_done;
 }
 
diff --git a/drivers/staging/line6/pod.c b/drivers/staging/line6/pod.c
index f4e95a614e3..44f4b2f9857 100644
--- a/drivers/staging/line6/pod.c
+++ b/drivers/staging/line6/pod.c
@@ -197,6 +197,7 @@ static ssize_t serial_number_show(struct device *dev,
 {
 	struct usb_interface *interface = to_usb_interface(dev);
 	struct usb_line6_pod *pod = usb_get_intfdata(interface);
+
 	return sprintf(buf, "%d\n", pod->serial_number);
 }
 
@@ -208,6 +209,7 @@ static ssize_t firmware_version_show(struct device *dev,
 {
 	struct usb_interface *interface = to_usb_interface(dev);
 	struct usb_line6_pod *pod = usb_get_intfdata(interface);
+
 	return sprintf(buf, "%d.%02d\n", pod->firmware_version / 100,
 		       pod->firmware_version % 100);
 }
@@ -220,6 +222,7 @@ static ssize_t device_id_show(struct device *dev,
 {
 	struct usb_interface *interface = to_usb_interface(dev);
 	struct usb_line6_pod *pod = usb_get_intfdata(interface);
+
 	return sprintf(buf, "%d\n", pod->device_id);
 }
 
@@ -243,6 +246,7 @@ static void pod_startup2(unsigned long data)
 {
 	struct usb_line6_pod *pod = (struct usb_line6_pod *)data;
 	struct usb_line6 *line6 = &pod->line6;
+
 	CHECK_STARTUP_PROGRESS(pod->startup_progress, POD_STARTUP_VERSIONREQ);
 
 	/* request firmware version: */
@@ -294,6 +298,7 @@ static int snd_pod_control_monitor_get(struct snd_kcontrol *kcontrol,
 {
 	struct snd_line6_pcm *line6pcm = snd_kcontrol_chip(kcontrol);
 	struct usb_line6_pod *pod = (struct usb_line6_pod *)line6pcm->line6;
+
 	ucontrol->value.integer.value[0] = pod->monitor_level;
 	return 0;
 }
diff --git a/drivers/staging/line6/toneport.c b/drivers/staging/line6/toneport.c
index af2e7e50c13..69437158d38 100644
--- a/drivers/staging/line6/toneport.c
+++ b/drivers/staging/line6/toneport.c
@@ -185,6 +185,7 @@ static int snd_toneport_monitor_get(struct snd_kcontrol *kcontrol,
 				    struct snd_ctl_elem_value *ucontrol)
 {
 	struct snd_line6_pcm *line6pcm = snd_kcontrol_chip(kcontrol);
+
 	ucontrol->value.integer.value[0] = line6pcm->volume_monitor;
 	return 0;
 }
@@ -213,6 +214,7 @@ static int snd_toneport_source_info(struct snd_kcontrol *kcontrol,
 				    struct snd_ctl_elem_info *uinfo)
 {
 	const int size = ARRAY_SIZE(toneport_source_info);
+
 	uinfo->type = SNDRV_CTL_ELEM_TYPE_ENUMERATED;
 	uinfo->count = 1;
 	uinfo->value.enumerated.items = size;
@@ -262,6 +264,7 @@ static void toneport_start_pcm(unsigned long arg)
 {
 	struct usb_line6_toneport *toneport = (struct usb_line6_toneport *)arg;
 	struct usb_line6 *line6 = &toneport->line6;
+
 	line6_pcm_acquire(line6->line6pcm, LINE6_BITS_PCM_MONITOR);
 }
 
diff --git a/drivers/staging/line6/variax.c b/drivers/staging/line6/variax.c
index bd0f694fa8d..ae2be99f9a9 100644
--- a/drivers/staging/line6/variax.c
+++ b/drivers/staging/line6/variax.c
@@ -95,6 +95,7 @@ static void variax_startup3(struct usb_line6_variax *variax)
 static void variax_startup4(unsigned long data)
 {
 	struct usb_line6_variax *variax = (struct usb_line6_variax *)data;
+
 	CHECK_STARTUP_PROGRESS(variax->startup_progress,
 			       VARIAX_STARTUP_ACTIVATE);
 
@@ -107,6 +108,7 @@ static void variax_startup4(unsigned long data)
 static void variax_startup5(unsigned long data)
 {
 	struct usb_line6_variax *variax = (struct usb_line6_variax *)data;
+
 	CHECK_STARTUP_PROGRESS(variax->startup_progress,
 			       VARIAX_STARTUP_WORKQUEUE);
 
diff --git a/drivers/staging/lustre/TODO b/drivers/staging/lustre/TODO
index 0a2b6cb3775..e325e1e9832 100644
--- a/drivers/staging/lustre/TODO
+++ b/drivers/staging/lustre/TODO
@@ -9,6 +9,5 @@
 * Other minor misc cleanups...
 
 Please send any patches to Greg Kroah-Hartman <greg@kroah.com>, Andreas Dilger
-<andreas.dilger@intel.com>, Oleg Drokin <oleg.drokin@intel.com> and
-Peng Tao <tao.peng@emc.com>. CCing hpdd-discuss <hpdd-discuss@lists.01.org>
-would be great too.
+<andreas.dilger@intel.com>, Oleg Drokin <oleg.drokin@intel.com>. CCing
+hpdd-discuss <hpdd-discuss@lists.01.org> would be great too.
diff --git a/drivers/staging/lustre/include/linux/libcfs/curproc.h b/drivers/staging/lustre/include/linux/libcfs/curproc.h
index 8fd47c98fd3..b314f34d2e6 100644
--- a/drivers/staging/lustre/include/linux/libcfs/curproc.h
+++ b/drivers/staging/lustre/include/linux/libcfs/curproc.h
@@ -56,7 +56,6 @@
 /* check if task is running in compat mode.*/
 #define current_pid()		(current->pid)
 #define current_comm()		(current->comm)
-int cfs_get_environ(const char *key, char *value, int *val_len);
 
 typedef __u32 cfs_cap_t;
 
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs.h b/drivers/staging/lustre/include/linux/libcfs/libcfs.h
index 4a6c7da7217..26b53f6420e 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs.h
@@ -42,6 +42,7 @@
 #endif
 
 #include <linux/libcfs/linux/libcfs.h>
+#include <linux/gfp.h>
 
 #include "curproc.h"
 
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_cpu.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_cpu.h
index c87efb49ebc..a140e5d9186 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_cpu.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_cpu.h
@@ -128,7 +128,7 @@ struct cfs_cpt_table *cfs_cpt_table_alloc(unsigned int ncpt);
 int
 cfs_cpt_number(struct cfs_cpt_table *cptab);
 /**
- * return number of HW cores or hypter-threadings in a CPU partition \a cpt
+ * return number of HW cores or hyper-threadings in a CPU partition \a cpt
  */
 int cfs_cpt_weight(struct cfs_cpt_table *cptab, int cpt);
 /**
@@ -152,7 +152,7 @@ int cfs_cpt_of_cpu(struct cfs_cpt_table *cptab, int cpu);
  */
 int cfs_cpt_bind(struct cfs_cpt_table *cptab, int cpt);
 /**
- * add \a cpu to CPU partion @cpt of \a cptab, return 1 for success,
+ * add \a cpu to CPU partition @cpt of \a cptab, return 1 for success,
  * otherwise 0 is returned
  */
 int cfs_cpt_set_cpu(struct cfs_cpt_table *cptab, int cpt, int cpu);
@@ -203,6 +203,11 @@ void cfs_cpt_clear(struct cfs_cpt_table *cptab, int cpt);
 int cfs_cpt_spread_node(struct cfs_cpt_table *cptab, int cpt);
 
 /**
+ * return number of HTs in the same core of \a cpu
+ */
+int cfs_cpu_ht_nsiblings(int cpu);
+
+/**
  * iterate over all CPU partitions in \a cptab
  */
 #define cfs_cpt_for_each(i, cptab)	\
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_crypto.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_crypto.h
index 776e9c0e48c..9e610a9ce51 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_crypto.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_crypto.h
@@ -31,7 +31,7 @@
 struct cfs_crypto_hash_type {
 	char		*cht_name;      /**< hash algorithm name, equal to
 					 * format name for crypto api */
-	unsigned int    cht_key;	/**< init key by default (vaild for
+	unsigned int    cht_key;	/**< init key by default (valid for
 					 * 4 bytes context like crc32, adler */
 	unsigned int    cht_size;       /**< hash digest size */
 };
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_debug.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_debug.h
index 2bd4885ce06..b270d84def9 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_debug.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_debug.h
@@ -183,7 +183,7 @@ struct libcfs_debug_msg_data {
 do {								\
 	(data)->msg_subsys = DEBUG_SUBSYSTEM;			\
 	(data)->msg_file   = __FILE__;				\
-	(data)->msg_fn     = __FUNCTION__;			\
+	(data)->msg_fn     = __func__;				\
 	(data)->msg_line   = __LINE__;				\
 	(data)->msg_cdls   = (cdls);				\
 	(data)->msg_mask   = (mask);				\
@@ -193,7 +193,7 @@ do {								\
 	static struct libcfs_debug_msg_data dataname = {	\
 	       .msg_subsys = DEBUG_SUBSYSTEM,			\
 	       .msg_file   = __FILE__,				\
-	       .msg_fn     = __FUNCTION__,			\
+	       .msg_fn     = __func__,				\
 	       .msg_line   = __LINE__,				\
 	       .msg_cdls   = (cdls)	 };			\
 	dataname.msg_mask   = (mask);
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_hash.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_hash.h
index e5d5db25562..954164361ca 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_hash.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_hash.h
@@ -59,8 +59,8 @@
 /*
  * Ideally we would use HAVE_HASH_LONG for this, but on linux we configure
  * the linux kernel and user space at the same time, so we need to differentiate
- * between them explicitely. If this is not needed on other architectures, then
- * we'll need to move the functions to archi specific headers.
+ * between them explicitly. If this is not needed on other architectures, then
+ * we'll need to move the functions to architecture specific headers.
  */
 
 #include <linux/hash.h>
@@ -86,7 +86,7 @@ union cfs_hash_lock {
 
 /**
  * cfs_hash_bucket is a container of:
- * - lock, couter ...
+ * - lock, counter ...
  * - array of hash-head starting from hsb_head[0], hash-head can be one of
  *   . cfs_hash_head_t
  *   . cfs_hash_head_dep_t
@@ -136,7 +136,7 @@ enum cfs_hash_tag {
 	CFS_HASH_NO_BKTLOCK     = 1 << 1,
 	/** rwlock to protect bucket */
 	CFS_HASH_RW_BKTLOCK     = 1 << 2,
-	/** spinlcok to protect bucket */
+	/** spinlock to protect bucket */
 	CFS_HASH_SPIN_BKTLOCK   = 1 << 3,
 	/** always add new item to tail */
 	CFS_HASH_ADD_TAIL       = 1 << 4,
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_private.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_private.h
index dddccca120c..740bfcd2f09 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_private.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_private.h
@@ -153,7 +153,7 @@ do {									    \
  * default allocator
  */
 #define LIBCFS_ALLOC(ptr, size) \
-	LIBCFS_ALLOC_GFP(ptr, size, __GFP_IO)
+	LIBCFS_ALLOC_GFP(ptr, size, GFP_NOFS)
 
 /**
  * non-sleeping allocator
@@ -177,7 +177,7 @@ do {									    \
 
 /** default numa allocator */
 #define LIBCFS_CPT_ALLOC(ptr, cptab, cpt, size)				    \
-	LIBCFS_CPT_ALLOC_GFP(ptr, cptab, cpt, size, __GFP_IO)
+	LIBCFS_CPT_ALLOC_GFP(ptr, cptab, cpt, size, GFP_NOFS)
 
 #define LIBCFS_FREE(ptr, size)					  \
 do {								    \
diff --git a/drivers/staging/lustre/include/linux/libcfs/linux/linux-lock.h b/drivers/staging/lustre/include/linux/libcfs/linux/linux-lock.h
index d6e00f92e4a..b75e401d9a9 100644
--- a/drivers/staging/lustre/include/linux/libcfs/linux/linux-lock.h
+++ b/drivers/staging/lustre/include/linux/libcfs/linux/linux-lock.h
@@ -66,7 +66,7 @@
  * - spin_unlock(x)
  * - spin_unlock_bh(x)
  * - spin_trylock(x)
- * - spin_is_locked(x)
+ * - assert_spin_locked(x)
  *
  * - spin_lock_irq(x)
  * - spin_lock_irqsave(x, f)
diff --git a/drivers/staging/lustre/include/linux/libcfs/linux/linux-mem.h b/drivers/staging/lustre/include/linux/libcfs/linux/linux-mem.h
index 2af15d41e77..ccee5c3e414 100644
--- a/drivers/staging/lustre/include/linux/libcfs/linux/linux-mem.h
+++ b/drivers/staging/lustre/include/linux/libcfs/linux/linux-mem.h
@@ -53,6 +53,11 @@
 #include <linux/memcontrol.h>
 #include <linux/mm_inline.h>
 
+#ifndef HAVE_LIBCFS_CPT
+/* Need this for cfs_cpt_table */
+#include <linux/libcfs/libcfs_cpu.h>
+#endif
+
 #define CFS_PAGE_MASK		   (~((__u64)PAGE_CACHE_SIZE-1))
 #define page_index(p)       ((p)->index)
 
diff --git a/drivers/staging/lustre/include/linux/lnet/lib-lnet.h b/drivers/staging/lustre/include/linux/lnet/lib-lnet.h
index 856fcfaafaf..06ff463e4af 100644
--- a/drivers/staging/lustre/include/linux/lnet/lib-lnet.h
+++ b/drivers/staging/lustre/include/linux/lnet/lib-lnet.h
@@ -181,7 +181,7 @@ lnet_net_lock_current(void)
 #define MAX_PORTALS     64
 
 /* these are only used by code with LNET_USE_LIB_FREELIST, but we still
- * exported them to !LNET_USE_LIB_FREELIST for easy implemetation */
+ * exported them to !LNET_USE_LIB_FREELIST for easy implementation */
 #define LNET_FL_MAX_MES		2048
 #define LNET_FL_MAX_MDS		2048
 #define LNET_FL_MAX_EQS		512
diff --git a/drivers/staging/lustre/include/linux/lnet/lib-types.h b/drivers/staging/lustre/include/linux/lnet/lib-types.h
index 1c13ef7df80..a63654b660d 100644
--- a/drivers/staging/lustre/include/linux/lnet/lib-types.h
+++ b/drivers/staging/lustre/include/linux/lnet/lib-types.h
@@ -204,7 +204,7 @@ typedef struct lnet_msg {
 	unsigned int	  msg_receiving:1;    /* being received */
 	unsigned int	  msg_txcredit:1;     /* taken an NI send credit */
 	unsigned int	  msg_peertxcredit:1; /* taken a peer send credit */
-	unsigned int	  msg_rtrcredit:1;    /* taken a globel router credit */
+	unsigned int	  msg_rtrcredit:1;    /* taken a global router credit */
 	unsigned int	  msg_peerrtrcredit:1; /* taken a peer router credit */
 	unsigned int	  msg_onactivelist:1; /* on the activelist */
 
@@ -342,7 +342,7 @@ typedef struct lnet_lnd {
 
 	/* Start receiving 'mlen' bytes of payload data, skipping the following
 	 * 'rlen' - 'mlen' bytes. 'private' is the 'private' passed to
-	 * lnet_parse().  Return non-zero for immedaite failure, otherwise
+	 * lnet_parse().  Return non-zero for immediate failure, otherwise
 	 * complete later with lnet_finalize().  This also gives back a receive
 	 * credit if the LND does flow control. */
 	int (*lnd_recv)(struct lnet_ni *ni, void *private, lnet_msg_t *msg,
@@ -594,7 +594,7 @@ struct lnet_match_table {
 	unsigned int		mt_cpt;
 	unsigned int		mt_portal;      /* portal index */
 	/* match table is set as "enabled" if there's non-exhausted MD
-	 * attached on mt_mhash, it's only valide for wildcard portal */
+	 * attached on mt_mhash, it's only valid for wildcard portal */
 	unsigned int		mt_enabled;
 	/* bitmap to flag whether MEs on mt_hash are exhausted or not */
 	__u64			mt_exhausted[LNET_MT_EXHAUSTED_BMAP];
diff --git a/drivers/staging/lustre/include/linux/lnet/lnetst.h b/drivers/staging/lustre/include/linux/lnet/lnetst.h
index e060599314d..87fe9ac76a6 100644
--- a/drivers/staging/lustre/include/linux/lnet/lnetst.h
+++ b/drivers/staging/lustre/include/linux/lnet/lnetst.h
@@ -402,7 +402,7 @@ typedef struct {
 /* add stat in session */
 typedef struct {
 	int		     lstio_sta_key;	  /* IN: session key */
-	int		     lstio_sta_timeout;      /* IN: timeout for stat requst */
+	int		     lstio_sta_timeout;      /* IN: timeout for stat request */
 	int		     lstio_sta_nmlen;	/* IN: group name length */
 	char		   *lstio_sta_namep;	/* IN: group name */
 	int		     lstio_sta_count;	/* IN: # of pid */
diff --git a/drivers/staging/lustre/include/linux/lnet/ptllnd.h b/drivers/staging/lustre/include/linux/lnet/ptllnd.h
index 564f5d3a9b4..313442a7ed3 100644
--- a/drivers/staging/lustre/include/linux/lnet/ptllnd.h
+++ b/drivers/staging/lustre/include/linux/lnet/ptllnd.h
@@ -66,7 +66,7 @@
 /*#define PTL_MD_LUSTRE_COMPLETION_SEMANTICS */
 
 /* Can compare handles directly on Cray Portals */
-#define PtlHandleIsEqual(a,b) ((a) == (b))
+#define PtlHandleIsEqual(a, b) ((a) == (b))
 
 /* Different error types on Cray Portals*/
 #define ptl_err_t ptl_ni_fail_t
diff --git a/drivers/staging/lustre/include/linux/lnet/ptllnd_wire.h b/drivers/staging/lustre/include/linux/lnet/ptllnd_wire.h
index 7d12b3a23a9..0d3ec5be0a0 100644
--- a/drivers/staging/lustre/include/linux/lnet/ptllnd_wire.h
+++ b/drivers/staging/lustre/include/linux/lnet/ptllnd_wire.h
@@ -64,26 +64,22 @@
 					 * above is for bulk data transfer */
 #define LNET_MSG_MATCHBITS       0      /* the value for the message channel */
 
-typedef struct
-{
+typedef struct {
 	lnet_hdr_t	kptlim_hdr;	     /* portals header */
 	char	      kptlim_payload[0];      /* piggy-backed payload */
 } WIRE_ATTR kptl_immediate_msg_t;
 
-typedef struct
-{
+typedef struct {
 	lnet_hdr_t	kptlrm_hdr;	     /* portals header */
 	__u64	     kptlrm_matchbits;       /* matchbits */
 } WIRE_ATTR kptl_rdma_msg_t;
 
-typedef struct
-{
+typedef struct {
 	__u64	     kptlhm_matchbits;       /* matchbits */
 	__u32	     kptlhm_max_msg_size;    /* max message size */
 } WIRE_ATTR kptl_hello_msg_t;
 
-typedef struct
-{
+typedef struct {
 	/* First 2 fields fixed FOR ALL TIME */
 	__u32	   ptlm_magic;     /* I'm a Portals LND message */
 	__u16	   ptlm_version;   /* this is my version number */
@@ -107,7 +103,7 @@ typedef struct
 } kptl_msg_t;
 
 /* kptl_msg_t::ptlm_credits is only a __u8 */
-#define PTLLND_MSG_MAX_CREDITS ((typeof(((kptl_msg_t*) 0)->ptlm_credits)) -1)
+#define PTLLND_MSG_MAX_CREDITS ((typeof(((kptl_msg_t*) 0)->ptlm_credits)) - 1)
 
 #define PTLLND_MSG_MAGIC		LNET_PROTO_PTL_MAGIC
 #define PTLLND_MSG_VERSION	      0x04
diff --git a/drivers/staging/lustre/include/linux/lnet/types.h b/drivers/staging/lustre/include/linux/lnet/types.h
index c833ce8544d..2add7976b3b 100644
--- a/drivers/staging/lustre/include/linux/lnet/types.h
+++ b/drivers/staging/lustre/include/linux/lnet/types.h
@@ -112,7 +112,7 @@ static inline void LNetInvalidateHandle(lnet_handle_any_t *h)
  */
 static inline int LNetHandleIsEqual (lnet_handle_any_t h1, lnet_handle_any_t h2)
 {
-	return (h1.cookie == h2.cookie);
+	return h1.cookie == h2.cookie;
 }
 
 /**
@@ -122,7 +122,7 @@ static inline int LNetHandleIsEqual (lnet_handle_any_t h1, lnet_handle_any_t h2)
  */
 static inline int LNetHandleIsInvalid(lnet_handle_any_t h)
 {
-	return (LNET_WIRE_HANDLE_COOKIE_NONE == h.cookie);
+	return LNET_WIRE_HANDLE_COOKIE_NONE == h.cookie;
 }
 
 /**
@@ -181,7 +181,7 @@ typedef struct {
 	 * address of an array of lnet_kiov_t and the length field specifies
 	 * the number of entries in the array. The length can't be bigger
 	 * than LNET_MAX_IOV. The lnet_kiov_t is used to describe page-based
-	 * fragments that are not necessarily mapped in virtal memory.
+	 * fragments that are not necessarily mapped in virtual memory.
 	 * - LNET_MD_IOVEC bit set: The start field points to the starting
 	 * address of an array of struct iovec and the length field specifies
 	 * the number of entries in the array. The length can't be bigger
@@ -381,7 +381,7 @@ typedef enum {
 
 #define LNET_SEQ_BASETYPE       long
 typedef unsigned LNET_SEQ_BASETYPE lnet_seq_t;
-#define LNET_SEQ_GT(a,b)	(((signed LNET_SEQ_BASETYPE)((a) - (b))) > 0)
+#define LNET_SEQ_GT(a, b)	(((signed LNET_SEQ_BASETYPE)((a) - (b))) > 0)
 
 /**
  * Information about an event on a MD.
diff --git a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c
index 0061c8afa20..892c41991f8 100644
--- a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c
+++ b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c
@@ -1141,7 +1141,7 @@ kiblnd_alloc_pages(kib_pages_t **pp, int cpt, int npages)
 	for (i = 0; i < npages; i++) {
 		p->ibp_pages[i] = alloc_pages_node(
 				    cfs_cpt_spread_node(lnet_cpt_table(), cpt),
-				    __GFP_IO, 0);
+				    GFP_NOFS, 0);
 		if (p->ibp_pages[i] == NULL) {
 			CERROR("Can't allocate page %d of %d\n", i, npages);
 			kiblnd_free_pages(p);
diff --git a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c
index 6173e74d749..dfd16e7ca8a 100644
--- a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c
+++ b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c
@@ -40,7 +40,7 @@
 
 #include "o2iblnd.h"
 
-void
+static void
 kiblnd_tx_done (lnet_ni_t *ni, kib_tx_t *tx)
 {
 	lnet_msg_t *lntmsg[2];
@@ -99,7 +99,7 @@ kiblnd_txlist_done (lnet_ni_t *ni, struct list_head *txlist, int status)
 	}
 }
 
-kib_tx_t *
+static kib_tx_t *
 kiblnd_get_idle_tx(lnet_ni_t *ni, lnet_nid_t target)
 {
 	kib_net_t		*net = (kib_net_t *)ni->ni_data;
@@ -127,7 +127,7 @@ kiblnd_get_idle_tx(lnet_ni_t *ni, lnet_nid_t target)
 	return tx;
 }
 
-void
+static void
 kiblnd_drop_rx(kib_rx_t *rx)
 {
 	kib_conn_t		*conn	= rx->rx_conn;
@@ -209,7 +209,7 @@ kiblnd_post_rx (kib_rx_t *rx, int credit)
 	return 0;
 }
 
-kib_tx_t *
+static kib_tx_t *
 kiblnd_find_waiting_tx_locked(kib_conn_t *conn, int txtype, __u64 cookie)
 {
 	struct list_head   *tmp;
@@ -234,7 +234,7 @@ kiblnd_find_waiting_tx_locked(kib_conn_t *conn, int txtype, __u64 cookie)
 	return NULL;
 }
 
-void
+static void
 kiblnd_handle_completion(kib_conn_t *conn, int txtype, int status, __u64 cookie)
 {
 	kib_tx_t    *tx;
@@ -273,7 +273,7 @@ kiblnd_handle_completion(kib_conn_t *conn, int txtype, int status, __u64 cookie)
 		kiblnd_tx_done(ni, tx);
 }
 
-void
+static void
 kiblnd_send_completion(kib_conn_t *conn, int type, int status, __u64 cookie)
 {
 	lnet_ni_t   *ni = conn->ibc_peer->ibp_ni;
@@ -292,7 +292,7 @@ kiblnd_send_completion(kib_conn_t *conn, int type, int status, __u64 cookie)
 	kiblnd_queue_tx(tx, conn);
 }
 
-void
+static void
 kiblnd_handle_rx (kib_rx_t *rx)
 {
 	kib_msg_t    *msg = rx->rx_msg;
@@ -453,7 +453,7 @@ kiblnd_handle_rx (kib_rx_t *rx)
 		kiblnd_post_rx(rx, post_credit);
 }
 
-void
+static void
 kiblnd_rx_complete (kib_rx_t *rx, int status, int nob)
 {
 	kib_msg_t    *msg = rx->rx_msg;
@@ -524,7 +524,7 @@ kiblnd_rx_complete (kib_rx_t *rx, int status, int nob)
 	kiblnd_drop_rx(rx);		     /* Don't re-post rx. */
 }
 
-struct page *
+static struct page *
 kiblnd_kvaddr_to_page (unsigned long vaddr)
 {
 	struct page *page;
@@ -695,7 +695,7 @@ kiblnd_map_tx(lnet_ni_t *ni, kib_tx_t *tx,
 }
 
 
-int
+static int
 kiblnd_setup_rd_iov(lnet_ni_t *ni, kib_tx_t *tx, kib_rdma_desc_t *rd,
 		    unsigned int niov, struct iovec *iov, int offset, int nob)
 {
@@ -748,7 +748,7 @@ kiblnd_setup_rd_iov(lnet_ni_t *ni, kib_tx_t *tx, kib_rdma_desc_t *rd,
 	return kiblnd_map_tx(ni, tx, rd, sg - tx->tx_frags);
 }
 
-int
+static int
 kiblnd_setup_rd_kiov (lnet_ni_t *ni, kib_tx_t *tx, kib_rdma_desc_t *rd,
 		      int nkiov, lnet_kiov_t *kiov, int offset, int nob)
 {
@@ -788,8 +788,10 @@ kiblnd_setup_rd_kiov (lnet_ni_t *ni, kib_tx_t *tx, kib_rdma_desc_t *rd,
 	return kiblnd_map_tx(ni, tx, rd, sg - tx->tx_frags);
 }
 
-int
+static int
 kiblnd_post_tx_locked (kib_conn_t *conn, kib_tx_t *tx, int credit)
+	__releases(conn->ibc_lock)
+	__acquires(conn->ibc_lock)
 {
 	kib_msg_t	 *msg = tx->tx_msg;
 	kib_peer_t	*peer = conn->ibc_peer;
@@ -992,7 +994,7 @@ kiblnd_check_sends (kib_conn_t *conn)
 	kiblnd_conn_decref(conn); /* ...until here */
 }
 
-void
+static void
 kiblnd_tx_complete (kib_tx_t *tx, int status)
 {
 	int	   failed = (status != IB_WC_SUCCESS);
@@ -1266,7 +1268,7 @@ static int kiblnd_resolve_addr(struct rdma_cm_id *cmid,
 	return rc;
 }
 
-void
+static void
 kiblnd_connect_peer (kib_peer_t *peer)
 {
 	struct rdma_cm_id *cmid;
@@ -1623,7 +1625,7 @@ kiblnd_send (lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg)
 	return 0;
 }
 
-void
+static void
 kiblnd_reply (lnet_ni_t *ni, kib_rx_t *rx, lnet_msg_t *lntmsg)
 {
 	lnet_process_id_t target = lntmsg->msg_target;
@@ -1810,7 +1812,7 @@ kiblnd_thread_start(int (*fn)(void *arg), void *arg, char *name)
 	return 0;
 }
 
-void
+static void
 kiblnd_thread_fini (void)
 {
 	atomic_dec (&kiblnd_data.kib_nthreads);
@@ -1824,7 +1826,7 @@ kiblnd_peer_alive (kib_peer_t *peer)
 	mb();
 }
 
-void
+static void
 kiblnd_peer_notify (kib_peer_t *peer)
 {
 	int	   error = 0;
@@ -1930,7 +1932,7 @@ kiblnd_close_conn(kib_conn_t *conn, int error)
 	write_unlock_irqrestore(&kiblnd_data.kib_global_lock, flags);
 }
 
-void
+static void
 kiblnd_handle_early_rxs(kib_conn_t *conn)
 {
 	unsigned long    flags;
@@ -1953,7 +1955,7 @@ kiblnd_handle_early_rxs(kib_conn_t *conn)
 	write_unlock_irqrestore(&kiblnd_data.kib_global_lock, flags);
 }
 
-void
+static void
 kiblnd_abort_txs(kib_conn_t *conn, struct list_head *txs)
 {
 	LIST_HEAD       (zombies);
@@ -1989,7 +1991,7 @@ kiblnd_abort_txs(kib_conn_t *conn, struct list_head *txs)
 	kiblnd_txlist_done(conn->ibc_peer->ibp_ni, &zombies, -ECONNABORTED);
 }
 
-void
+static void
 kiblnd_finalise_conn (kib_conn_t *conn)
 {
 	LASSERT (!in_interrupt());
@@ -2163,7 +2165,7 @@ kiblnd_connreq_done(kib_conn_t *conn, int status)
 	kiblnd_handle_early_rxs(conn);
 }
 
-void
+static void
 kiblnd_reject(struct rdma_cm_id *cmid, kib_rej_t *rej)
 {
 	int	  rc;
@@ -2174,7 +2176,7 @@ kiblnd_reject(struct rdma_cm_id *cmid, kib_rej_t *rej)
 		CWARN("Error %d sending reject\n", rc);
 }
 
-int
+static int
 kiblnd_passive_connect (struct rdma_cm_id *cmid, void *priv, int priv_nob)
 {
 	rwlock_t		*g_lock = &kiblnd_data.kib_global_lock;
@@ -2448,7 +2450,7 @@ kiblnd_passive_connect (struct rdma_cm_id *cmid, void *priv, int priv_nob)
 	return -ECONNREFUSED;
 }
 
-void
+static void
 kiblnd_reconnect (kib_conn_t *conn, int version,
 		  __u64 incarnation, int why, kib_connparams_t *cp)
 {
@@ -2512,7 +2514,7 @@ kiblnd_reconnect (kib_conn_t *conn, int version,
 	kiblnd_connect_peer(peer);
 }
 
-void
+static void
 kiblnd_rejected (kib_conn_t *conn, int reason, void *priv, int priv_nob)
 {
 	kib_peer_t    *peer = conn->ibc_peer;
@@ -2609,13 +2611,17 @@ kiblnd_rejected (kib_conn_t *conn, int reason, void *priv, int priv_nob)
 
 			case IBLND_REJECT_MSG_QUEUE_SIZE:
 				CERROR("%s rejected: incompatible message queue depth %d, %d\n",
-				       libcfs_nid2str(peer->ibp_nid), cp->ibcp_queue_depth,
+				       libcfs_nid2str(peer->ibp_nid),
+				       cp != NULL ? cp->ibcp_queue_depth :
+				       IBLND_MSG_QUEUE_SIZE(rej->ibr_version),
 				       IBLND_MSG_QUEUE_SIZE(conn->ibc_version));
 				break;
 
 			case IBLND_REJECT_RDMA_FRAGS:
 				CERROR("%s rejected: incompatible # of RDMA fragments %d, %d\n",
-				       libcfs_nid2str(peer->ibp_nid), cp->ibcp_max_frags,
+				       libcfs_nid2str(peer->ibp_nid),
+				       cp != NULL ? cp->ibcp_max_frags :
+				       IBLND_RDMA_FRAGS(rej->ibr_version),
 				       IBLND_RDMA_FRAGS(conn->ibc_version));
 				break;
 
@@ -2647,7 +2653,7 @@ kiblnd_rejected (kib_conn_t *conn, int reason, void *priv, int priv_nob)
 	kiblnd_connreq_done(conn, -ECONNREFUSED);
 }
 
-void
+static void
 kiblnd_check_connreply (kib_conn_t *conn, void *priv, int priv_nob)
 {
 	kib_peer_t    *peer = conn->ibc_peer;
@@ -2746,7 +2752,7 @@ kiblnd_check_connreply (kib_conn_t *conn, void *priv, int priv_nob)
 	kiblnd_connreq_done(conn, 0);
 }
 
-int
+static int
 kiblnd_active_connect (struct rdma_cm_id *cmid)
 {
 	kib_peer_t	      *peer = (kib_peer_t *)cmid->context;
@@ -3017,7 +3023,7 @@ kiblnd_conn_timed_out_locked(kib_conn_t *conn)
 		kiblnd_check_txs_locked(conn, &conn->ibc_active_txs);
 }
 
-void
+static void
 kiblnd_check_conns (int idx)
 {
 	LIST_HEAD (closes);
@@ -3100,7 +3106,7 @@ kiblnd_check_conns (int idx)
 	}
 }
 
-void
+static void
 kiblnd_disconnect_conn (kib_conn_t *conn)
 {
 	LASSERT (!in_interrupt());
@@ -3239,7 +3245,7 @@ kiblnd_qp_event(struct ib_event *event, void *arg)
 	}
 }
 
-void
+static void
 kiblnd_complete (struct ib_wc *wc)
 {
 	switch (kiblnd_wreqid2type(wc->wr_id)) {
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.c b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.c
index 21d36ee5378..775dcd29c20 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.c
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.c
@@ -58,10 +58,10 @@ ksocknal_ip2iface(lnet_ni_t *ni, __u32 ip)
 		iface = &net->ksnn_interfaces[i];
 
 		if (iface->ksni_ipaddr == ip)
-			return (iface);
+			return iface;
 	}
 
-	return (NULL);
+	return NULL;
 }
 
 ksock_route_t *
@@ -71,7 +71,7 @@ ksocknal_create_route (__u32 ipaddr, int port)
 
 	LIBCFS_ALLOC (route, sizeof (*route));
 	if (route == NULL)
-		return (NULL);
+		return NULL;
 
 	atomic_set (&route->ksnr_refcount, 1);
 	route->ksnr_peer = NULL;
@@ -85,7 +85,7 @@ ksocknal_create_route (__u32 ipaddr, int port)
 	route->ksnr_conn_count = 0;
 	route->ksnr_share_count = 0;
 
-	return (route);
+	return route;
 }
 
 void
@@ -197,9 +197,9 @@ ksocknal_find_peer_locked (lnet_ni_t *ni, lnet_process_id_t id)
 		CDEBUG(D_NET, "got peer [%p] -> %s (%d)\n",
 		       peer, libcfs_id2str(id),
 		       atomic_read(&peer->ksnp_refcount));
-		return (peer);
+		return peer;
 	}
-	return (NULL);
+	return NULL;
 }
 
 ksock_peer_t *
@@ -213,7 +213,7 @@ ksocknal_find_peer (lnet_ni_t *ni, lnet_process_id_t id)
 		ksocknal_peer_addref(peer);
 	read_unlock(&ksocknal_data.ksnd_global_lock);
 
-	return (peer);
+	return peer;
 }
 
 void
@@ -318,7 +318,7 @@ ksocknal_get_peer_info (lnet_ni_t *ni, int index,
 	}
  out:
 	read_unlock(&ksocknal_data.ksnd_global_lock);
-	return (rc);
+	return rc;
 }
 
 void
@@ -459,7 +459,7 @@ ksocknal_add_peer (lnet_ni_t *ni, lnet_process_id_t id, __u32 ipaddr, int port)
 
 	if (id.nid == LNET_NID_ANY ||
 	    id.pid == LNET_PID_ANY)
-		return (-EINVAL);
+		return -EINVAL;
 
 	/* Have a brand new peer ready... */
 	rc = ksocknal_create_peer(&peer, ni, id);
@@ -469,7 +469,7 @@ ksocknal_add_peer (lnet_ni_t *ni, lnet_process_id_t id, __u32 ipaddr, int port)
 	route = ksocknal_create_route (ipaddr, port);
 	if (route == NULL) {
 		ksocknal_peer_decref(peer);
-		return (-ENOMEM);
+		return -ENOMEM;
 	}
 
 	write_lock_bh(&ksocknal_data.ksnd_global_lock);
@@ -506,7 +506,7 @@ ksocknal_add_peer (lnet_ni_t *ni, lnet_process_id_t id, __u32 ipaddr, int port)
 
 	write_unlock_bh(&ksocknal_data.ksnd_global_lock);
 
-	return (0);
+	return 0;
 }
 
 void
@@ -620,7 +620,7 @@ ksocknal_del_peer (lnet_ni_t *ni, lnet_process_id_t id, __u32 ip)
 
 	ksocknal_txlist_done(ni, &zombies, 1);
 
-	return (rc);
+	return rc;
 }
 
 ksock_conn_t *
@@ -650,15 +650,14 @@ ksocknal_get_conn_by_idx (lnet_ni_t *ni, int index)
 				conn = list_entry (ctmp, ksock_conn_t,
 						       ksnc_list);
 				ksocknal_conn_addref(conn);
-				read_unlock(&ksocknal_data. \
-						 ksnd_global_lock);
-				return (conn);
+				read_unlock(&ksocknal_data.ksnd_global_lock);
+				return conn;
 			}
 		}
 	}
 
 	read_unlock(&ksocknal_data.ksnd_global_lock);
-	return (NULL);
+	return NULL;
 }
 
 ksock_sched_t *
@@ -709,7 +708,7 @@ ksocknal_local_ipvec (lnet_ni_t *ni, __u32 *ipaddrs)
 	}
 
 	read_unlock(&ksocknal_data.ksnd_global_lock);
-	return (nip);
+	return nip;
 }
 
 int
@@ -741,7 +740,7 @@ ksocknal_match_peerip (ksock_interface_t *iface, __u32 *ips, int nips)
 	}
 
 	LASSERT (best >= 0);
-	return (best);
+	return best;
 }
 
 int
@@ -793,8 +792,6 @@ ksocknal_select_ips(ksock_peer_t *peer, __u32 *peerips, int n_peerips)
 			ip = peer->ksnp_passive_ips[i];
 			best_iface = ksocknal_ip2iface(peer->ksnp_ni, ip);
 
-			/* peer passive ips are kept up to date */
-			LASSERT(best_iface != NULL);
 		} else {
 			/* choose a new interface */
 			LASSERT (i == peer->ksnp_n_passive_ips);
@@ -835,8 +832,6 @@ ksocknal_select_ips(ksock_peer_t *peer, __u32 *peerips, int n_peerips)
 			peer->ksnp_n_passive_ips = i+1;
 		}
 
-		LASSERT (best_iface != NULL);
-
 		/* mark the best matching peer IP used */
 		j = ksocknal_match_peerip(best_iface, peerips, n_peerips);
 		peerips[j] = 0;
@@ -847,7 +842,7 @@ ksocknal_select_ips(ksock_peer_t *peer, __u32 *peerips, int n_peerips)
 
 	write_unlock_bh(global_lock);
 
-	return (n_ips);
+	return n_ips;
 }
 
 void
@@ -1571,7 +1566,7 @@ ksocknal_terminate_conn (ksock_conn_t *conn)
 	conn->ksnc_tx_ready = 1;
 
 	if (!conn->ksnc_tx_scheduled &&
-	    !list_empty(&conn->ksnc_tx_queue)){
+	    !list_empty(&conn->ksnc_tx_queue)) {
 		list_add_tail (&conn->ksnc_tx_list,
 			       &sched->kss_tx_conns);
 		conn->ksnc_tx_scheduled = 1;
@@ -1709,7 +1704,7 @@ ksocknal_close_peer_conns_locked (ksock_peer_t *peer, __u32 ipaddr, int why)
 		}
 	}
 
-	return (count);
+	return count;
 }
 
 int
@@ -1725,7 +1720,7 @@ ksocknal_close_conn_and_siblings (ksock_conn_t *conn, int why)
 
 	write_unlock_bh(&ksocknal_data.ksnd_global_lock);
 
-	return (count);
+	return count;
 }
 
 int
@@ -1766,7 +1761,7 @@ ksocknal_close_matching_conns (lnet_process_id_t id, __u32 ipaddr)
 
 	/* wildcards always succeed */
 	if (id.nid == LNET_NID_ANY || id.pid == LNET_PID_ANY || ipaddr == 0)
-		return (0);
+		return 0;
 
 	if (count == 0)
 		return -ENOENT;
@@ -1937,7 +1932,7 @@ ksocknal_push (lnet_ni_t *ni, lnet_process_id_t id)
 
 	}
 
-	return (rc);
+	return rc;
 }
 
 int
@@ -1955,7 +1950,7 @@ ksocknal_add_interface(lnet_ni_t *ni, __u32 ipaddress, __u32 netmask)
 
 	if (ipaddress == 0 ||
 	    netmask == 0)
-		return (-EINVAL);
+		return -EINVAL;
 
 	write_lock_bh(&ksocknal_data.ksnd_global_lock);
 
@@ -1999,7 +1994,7 @@ ksocknal_add_interface(lnet_ni_t *ni, __u32 ipaddress, __u32 netmask)
 
 	write_unlock_bh(&ksocknal_data.ksnd_global_lock);
 
-	return (rc);
+	return rc;
 }
 
 void
@@ -2088,7 +2083,7 @@ ksocknal_del_interface(lnet_ni_t *ni, __u32 ipaddress)
 
 	write_unlock_bh(&ksocknal_data.ksnd_global_lock);
 
-	return (rc);
+	return rc;
 }
 
 int
@@ -2098,7 +2093,7 @@ ksocknal_ctl(lnet_ni_t *ni, unsigned int cmd, void *arg)
 	struct libcfs_ioctl_data *data = arg;
 	int rc;
 
-	switch(cmd) {
+	switch (cmd) {
 	case IOC_LIBCFS_GET_INTERFACE: {
 		ksock_net_t       *net = ni->ni_data;
 		ksock_interface_t *iface;
@@ -2299,12 +2294,12 @@ ksocknal_base_shutdown(void)
 				for (j = 0; j < info->ksi_nthreads_max; j++) {
 
 					sched = &info->ksi_scheds[j];
-					LASSERT(list_empty(&sched->\
-							       kss_tx_conns));
-					LASSERT(list_empty(&sched->\
-							       kss_rx_conns));
-					LASSERT(list_empty(&sched-> \
-						  kss_zombie_noop_txs));
+					LASSERT(list_empty(
+						&sched->kss_tx_conns));
+					LASSERT(list_empty(
+						&sched->kss_rx_conns));
+					LASSERT(list_empty(
+						&sched->kss_zombie_noop_txs));
 					LASSERT(sched->kss_nconns == 0);
 				}
 			}
@@ -2514,7 +2509,8 @@ ksocknal_debug_peerhash (lnet_ni_t *ni)
 		list_for_each (tmp, &ksocknal_data.ksnd_peers[i]) {
 			peer = list_entry (tmp, ksock_peer_t, ksnp_list);
 
-			if (peer->ksnp_ni == ni) break;
+			if (peer->ksnp_ni == ni)
+				break;
 
 			peer = NULL;
 		}
@@ -2683,8 +2679,8 @@ ksocknal_search_new_ipif(ksock_net_t *net)
 		list_for_each_entry(tmp, &ksocknal_data.ksnd_nets,
 					ksnn_list) {
 			for (j = 0; !found && j < tmp->ksnn_ninterfaces; j++) {
-				char *ifnam2 = &tmp->ksnn_interfaces[j].\
-					     ksni_name[0];
+				char *ifnam2 =
+					&tmp->ksnn_interfaces[j].ksni_name[0];
 				char *colon2 = strchr(ifnam2, ':');
 
 				if (colon2 != NULL)
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.h b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.h
index df2be7a7f46..109a2393247 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.h
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.h
@@ -418,7 +418,7 @@ ksocknal_nid2peerlist (lnet_nid_t nid)
 {
 	unsigned int hash = ((unsigned int)nid) % ksocknal_data.ksnd_peer_hash_size;
 
-	return (&ksocknal_data.ksnd_peers [hash]);
+	return &ksocknal_data.ksnd_peers[hash];
 }
 
 static inline void
@@ -452,7 +452,7 @@ ksocknal_connsock_addref (ksock_conn_t *conn)
 	}
 	read_unlock(&ksocknal_data.ksnd_global_lock);
 
-	return (rc);
+	return rc;
 }
 
 static inline void
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c
index bdf95ea8a54..75bd6583493 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c
@@ -120,7 +120,7 @@ ksocknal_send_iov (ksock_conn_t *conn, ksock_tx_t *tx)
 	rc = ksocknal_lib_send_iov(conn, tx);
 
 	if (rc <= 0)			    /* sent nothing? */
-		return (rc);
+		return rc;
 
 	nob = rc;
 	LASSERT (nob <= tx->tx_resid);
@@ -133,7 +133,7 @@ ksocknal_send_iov (ksock_conn_t *conn, ksock_tx_t *tx)
 		if (nob < (int) iov->iov_len) {
 			iov->iov_base = (void *)((char *)iov->iov_base + nob);
 			iov->iov_len -= nob;
-			return (rc);
+			return rc;
 		}
 
 		nob -= iov->iov_len;
@@ -141,7 +141,7 @@ ksocknal_send_iov (ksock_conn_t *conn, ksock_tx_t *tx)
 		tx->tx_niov--;
 	} while (nob != 0);
 
-	return (rc);
+	return rc;
 }
 
 int
@@ -158,7 +158,7 @@ ksocknal_send_kiov (ksock_conn_t *conn, ksock_tx_t *tx)
 	rc = ksocknal_lib_send_kiov(conn, tx);
 
 	if (rc <= 0)			    /* sent nothing? */
-		return (rc);
+		return rc;
 
 	nob = rc;
 	LASSERT (nob <= tx->tx_resid);
@@ -179,7 +179,7 @@ ksocknal_send_kiov (ksock_conn_t *conn, ksock_tx_t *tx)
 		tx->tx_nkiov--;
 	} while (nob != 0);
 
-	return (rc);
+	return rc;
 }
 
 int
@@ -198,7 +198,7 @@ ksocknal_transmit (ksock_conn_t *conn, ksock_tx_t *tx)
 	rc = ksocknal_connsock_addref(conn);
 	if (rc != 0) {
 		LASSERT (conn->ksnc_closing);
-		return (-ESHUTDOWN);
+		return -ESHUTDOWN;
 	}
 
 	do {
@@ -245,7 +245,7 @@ ksocknal_transmit (ksock_conn_t *conn, ksock_tx_t *tx)
 	} while (tx->tx_resid != 0);
 
 	ksocknal_connsock_decref(conn);
-	return (rc);
+	return rc;
 }
 
 int
@@ -262,7 +262,7 @@ ksocknal_recv_iov (ksock_conn_t *conn)
 	rc = ksocknal_lib_recv_iov(conn);
 
 	if (rc <= 0)
-		return (rc);
+		return rc;
 
 	/* received something... */
 	nob = rc;
@@ -282,7 +282,7 @@ ksocknal_recv_iov (ksock_conn_t *conn)
 		if (nob < (int)iov->iov_len) {
 			iov->iov_len -= nob;
 			iov->iov_base = (void *)((char *)iov->iov_base + nob);
-			return (-EAGAIN);
+			return -EAGAIN;
 		}
 
 		nob -= iov->iov_len;
@@ -290,7 +290,7 @@ ksocknal_recv_iov (ksock_conn_t *conn)
 		conn->ksnc_rx_niov--;
 	} while (nob != 0);
 
-	return (rc);
+	return rc;
 }
 
 int
@@ -306,7 +306,7 @@ ksocknal_recv_kiov (ksock_conn_t *conn)
 	rc = ksocknal_lib_recv_kiov(conn);
 
 	if (rc <= 0)
-		return (rc);
+		return rc;
 
 	/* received something... */
 	nob = rc;
@@ -353,7 +353,7 @@ ksocknal_receive (ksock_conn_t *conn)
 	rc = ksocknal_connsock_addref(conn);
 	if (rc != 0) {
 		LASSERT (conn->ksnc_closing);
-		return (-ESHUTDOWN);
+		return -ESHUTDOWN;
 	}
 
 	for (;;) {
@@ -515,11 +515,11 @@ ksocknal_process_transmit (ksock_conn_t *conn, ksock_tx_t *tx)
 		/* Sent everything OK */
 		LASSERT (rc == 0);
 
-		return (0);
+		return 0;
 	}
 
 	if (rc == -EAGAIN)
-		return (rc);
+		return rc;
 
 	if (rc == -ENOMEM) {
 		static int counter;
@@ -542,7 +542,7 @@ ksocknal_process_transmit (ksock_conn_t *conn, ksock_tx_t *tx)
 			wake_up (&ksocknal_data.ksnd_reaper_waitq);
 
 		spin_unlock_bh(&ksocknal_data.ksnd_reaper_lock);
-		return (rc);
+		return rc;
 	}
 
 	/* Actual error */
@@ -576,7 +576,7 @@ ksocknal_process_transmit (ksock_conn_t *conn, ksock_tx_t *tx)
 	ksocknal_close_conn_and_siblings (conn,
 					  (conn->ksnc_closing) ? 0 : rc);
 
-	return (rc);
+	return rc;
 }
 
 void
@@ -808,10 +808,10 @@ ksocknal_find_connectable_route_locked (ksock_peer_t *peer)
 			continue;
 		}
 
-		return (route);
+		return route;
 	}
 
-	return (NULL);
+	return NULL;
 }
 
 ksock_route_t *
@@ -826,10 +826,10 @@ ksocknal_find_connecting_route_locked (ksock_peer_t *peer)
 		LASSERT (!route->ksnr_connecting || route->ksnr_scheduled);
 
 		if (route->ksnr_scheduled)
-			return (route);
+			return route;
 	}
 
-	return (NULL);
+	return NULL;
 }
 
 int
@@ -857,7 +857,7 @@ ksocknal_launch_packet (lnet_ni_t *ni, ksock_tx_t *tx, lnet_process_id_t id)
 					 * connection... */
 					ksocknal_queue_tx_locked (tx, conn);
 					read_unlock(g_lock);
-					return (0);
+					return 0;
 				}
 			}
 		}
@@ -901,7 +901,7 @@ ksocknal_launch_packet (lnet_ni_t *ni, ksock_tx_t *tx, lnet_process_id_t id)
 		/* Connection exists; queue message on it */
 		ksocknal_queue_tx_locked (tx, conn);
 		write_unlock_bh(g_lock);
-		return (0);
+		return 0;
 	}
 
 	if (peer->ksnp_accepting > 0 ||
@@ -920,7 +920,7 @@ ksocknal_launch_packet (lnet_ni_t *ni, ksock_tx_t *tx, lnet_process_id_t id)
 
 	/* NB Routes may be ignored if connections to them failed recently */
 	CNETERR("No usable routes to %s\n", libcfs_id2str(id));
-	return (-EHOSTUNREACH);
+	return -EHOSTUNREACH;
 }
 
 int
@@ -965,7 +965,7 @@ ksocknal_send(lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg)
 		       type, desc_size);
 		if (lntmsg->msg_vmflush)
 			cfs_memory_pressure_restore(mpflag);
-		return (-ENOMEM);
+		return -ENOMEM;
 	}
 
 	tx->tx_conn = NULL;		     /* set when assigned a conn */
@@ -999,10 +999,10 @@ ksocknal_send(lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg)
 		cfs_memory_pressure_restore(mpflag);
 
 	if (rc == 0)
-		return (0);
+		return 0;
 
 	ksocknal_free_tx(tx);
-	return (-EIO);
+	return -EIO;
 }
 
 int
@@ -1078,7 +1078,7 @@ ksocknal_new_packet (ksock_conn_t *conn, int nob_to_skip)
 		conn->ksnc_rx_kiov = NULL;
 		conn->ksnc_rx_nkiov = 0;
 		conn->ksnc_rx_csum = ~0;
-		return (1);
+		return 1;
 	}
 
 	/* Set up to skip as much as possible now.  If there's more left
@@ -1106,7 +1106,7 @@ ksocknal_new_packet (ksock_conn_t *conn, int nob_to_skip)
 	conn->ksnc_rx_kiov = NULL;
 	conn->ksnc_rx_nkiov = 0;
 	conn->ksnc_rx_nob_wanted = skipped;
-	return (0);
+	return 0;
 }
 
 int
@@ -1153,7 +1153,7 @@ ksocknal_process_receive (ksock_conn_t *conn)
 
 		if (conn->ksnc_rx_nob_wanted != 0) {
 			/* short read */
-			return (-EAGAIN);
+			return -EAGAIN;
 		}
 	}
 	switch (conn->ksnc_rx_state) {
@@ -1172,7 +1172,7 @@ ksocknal_process_receive (ksock_conn_t *conn)
 			       conn->ksnc_msg.ksm_type);
 			ksocknal_new_packet(conn, 0);
 			ksocknal_close_conn_and_siblings(conn, -EPROTO);
-			return (-EPROTO);
+			return -EPROTO;
 		}
 
 		if (conn->ksnc_msg.ksm_type == KSOCK_MSG_NOOP &&
@@ -1184,7 +1184,7 @@ ksocknal_process_receive (ksock_conn_t *conn)
 			       conn->ksnc_msg.ksm_csum, conn->ksnc_rx_csum);
 			ksocknal_new_packet(conn, 0);
 			ksocknal_close_conn_and_siblings(conn, -EPROTO);
-			return (-EIO);
+			return -EIO;
 		}
 
 		if (conn->ksnc_msg.ksm_zc_cookies[1] != 0) {
@@ -1204,7 +1204,7 @@ ksocknal_process_receive (ksock_conn_t *conn)
 				       cookie, conn->ksnc_msg.ksm_zc_cookies[1]);
 				ksocknal_new_packet(conn, 0);
 				ksocknal_close_conn_and_siblings(conn, -EPROTO);
-				return (rc);
+				return rc;
 			}
 		}
 
@@ -1252,7 +1252,7 @@ ksocknal_process_receive (ksock_conn_t *conn)
 			ksocknal_new_packet(conn, 0);
 			ksocknal_close_conn_and_siblings (conn, rc);
 			ksocknal_conn_decref(conn);
-			return (-EPROTO);
+			return -EPROTO;
 		}
 
 		/* I'm racing with ksocknal_recv() */
@@ -1295,7 +1295,7 @@ ksocknal_process_receive (ksock_conn_t *conn)
 		if (rc != 0) {
 			ksocknal_new_packet(conn, 0);
 			ksocknal_close_conn_and_siblings (conn, rc);
-			return (-EPROTO);
+			return -EPROTO;
 		}
 		/* Fall through */
 
@@ -1311,7 +1311,7 @@ ksocknal_process_receive (ksock_conn_t *conn)
 
 	/* Not Reached */
 	LBUG ();
-	return (-EINVAL);		       /* keep gcc happy */
+	return -EINVAL;		       /* keep gcc happy */
 }
 
 int
@@ -1679,13 +1679,13 @@ ksocknal_invert_type(int type)
 	{
 	case SOCKLND_CONN_ANY:
 	case SOCKLND_CONN_CONTROL:
-		return (type);
+		return type;
 	case SOCKLND_CONN_BULK_IN:
 		return SOCKLND_CONN_BULK_OUT;
 	case SOCKLND_CONN_BULK_OUT:
 		return SOCKLND_CONN_BULK_IN;
 	default:
-		return (SOCKLND_CONN_NONE);
+		return SOCKLND_CONN_NONE;
 	}
 }
 
@@ -2291,7 +2291,7 @@ ksocknal_find_timed_out_conn (ksock_peer_t *peer)
 				break;
 			}
 
-			return (conn);
+			return conn;
 		}
 
 		if (conn->ksnc_rx_started &&
@@ -2307,7 +2307,7 @@ ksocknal_find_timed_out_conn (ksock_peer_t *peer)
 				conn->ksnc_rx_state,
 				conn->ksnc_rx_nob_wanted,
 				conn->ksnc_rx_nob_left);
-			return (conn);
+			return conn;
 		}
 
 		if ((!list_empty(&conn->ksnc_tx_queue) ||
@@ -2322,11 +2322,11 @@ ksocknal_find_timed_out_conn (ksock_peer_t *peer)
 				libcfs_id2str(peer->ksnp_id),
 				&conn->ksnc_ipaddr,
 				conn->ksnc_port);
-			return (conn);
+			return conn;
 		}
 	}
 
-	return (NULL);
+	return NULL;
 }
 
 static inline void
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib-linux.c b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib-linux.c
index 37758d1c8a6..d18bab19cd9 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib-linux.c
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib-linux.c
@@ -188,7 +188,6 @@ void
 ksocknal_lib_eager_ack (ksock_conn_t *conn)
 {
 	int	    opt = 1;
-	mm_segment_t   oldmm = get_fs();
 	struct socket *sock = conn->ksnc_sock;
 
 	/* Remind the socket to ACK eagerly.  If I don't, the socket might
@@ -196,10 +195,8 @@ ksocknal_lib_eager_ack (ksock_conn_t *conn)
 	 * on, introducing delay in completing zero-copy sends in my
 	 * peer. */
 
-	set_fs(KERNEL_DS);
-	sock->ops->setsockopt (sock, SOL_TCP, TCP_QUICKACK,
+	kernel_setsockopt(sock, SOL_TCP, TCP_QUICKACK,
 			       (char *)&opt, sizeof (opt));
-	set_fs(oldmm);
 }
 
 int
@@ -383,7 +380,7 @@ ksocknal_lib_recv_kiov (ksock_conn_t *conn)
 			kunmap(kiov[i].kiov_page);
 	}
 
-	return (rc);
+	return rc;
 }
 
 void
@@ -428,7 +425,6 @@ ksocknal_lib_csum_tx(ksock_tx_t *tx)
 int
 ksocknal_lib_get_conn_tunables (ksock_conn_t *conn, int *txmem, int *rxmem, int *nagle)
 {
-	mm_segment_t   oldmm = get_fs ();
 	struct socket *sock = conn->ksnc_sock;
 	int	    len;
 	int	    rc;
@@ -437,16 +433,14 @@ ksocknal_lib_get_conn_tunables (ksock_conn_t *conn, int *txmem, int *rxmem, int
 	if (rc != 0) {
 		LASSERT (conn->ksnc_closing);
 		*txmem = *rxmem = *nagle = 0;
-		return (-ESHUTDOWN);
+		return -ESHUTDOWN;
 	}
 
 	rc = libcfs_sock_getbuf(sock, txmem, rxmem);
 	if (rc == 0) {
 		len = sizeof(*nagle);
-		set_fs(KERNEL_DS);
-		rc = sock->ops->getsockopt(sock, SOL_TCP, TCP_NODELAY,
+		rc = kernel_getsockopt(sock, SOL_TCP, TCP_NODELAY,
 					   (char *)nagle, &len);
-		set_fs(oldmm);
 	}
 
 	ksocknal_connsock_decref(conn);
@@ -456,13 +450,12 @@ ksocknal_lib_get_conn_tunables (ksock_conn_t *conn, int *txmem, int *rxmem, int
 	else
 		*txmem = *rxmem = *nagle = 0;
 
-	return (rc);
+	return rc;
 }
 
 int
 ksocknal_lib_setup_sock (struct socket *sock)
 {
-	mm_segment_t    oldmm = get_fs ();
 	int	     rc;
 	int	     option;
 	int	     keep_idle;
@@ -479,35 +472,29 @@ ksocknal_lib_setup_sock (struct socket *sock)
 	linger.l_onoff = 0;
 	linger.l_linger = 0;
 
-	set_fs (KERNEL_DS);
-	rc = sock_setsockopt (sock, SOL_SOCKET, SO_LINGER,
+	rc = kernel_setsockopt(sock, SOL_SOCKET, SO_LINGER,
 			      (char *)&linger, sizeof (linger));
-	set_fs (oldmm);
 	if (rc != 0) {
 		CERROR ("Can't set SO_LINGER: %d\n", rc);
-		return (rc);
+		return rc;
 	}
 
 	option = -1;
-	set_fs (KERNEL_DS);
-	rc = sock->ops->setsockopt (sock, SOL_TCP, TCP_LINGER2,
+	rc = kernel_setsockopt(sock, SOL_TCP, TCP_LINGER2,
 				    (char *)&option, sizeof (option));
-	set_fs (oldmm);
 	if (rc != 0) {
 		CERROR ("Can't set SO_LINGER2: %d\n", rc);
-		return (rc);
+		return rc;
 	}
 
 	if (!*ksocknal_tunables.ksnd_nagle) {
 		option = 1;
 
-		set_fs (KERNEL_DS);
-		rc = sock->ops->setsockopt (sock, SOL_TCP, TCP_NODELAY,
+		rc = kernel_setsockopt(sock, SOL_TCP, TCP_NODELAY,
 					    (char *)&option, sizeof (option));
-		set_fs (oldmm);
 		if (rc != 0) {
 			CERROR ("Can't disable nagle: %d\n", rc);
-			return (rc);
+			return rc;
 		}
 	}
 
@@ -518,7 +505,7 @@ ksocknal_lib_setup_sock (struct socket *sock)
 		CERROR ("Can't set buffer tx %d, rx %d buffers: %d\n",
 			*ksocknal_tunables.ksnd_tx_buffer_size,
 			*ksocknal_tunables.ksnd_rx_buffer_size, rc);
-		return (rc);
+		return rc;
 	}
 
 /* TCP_BACKOFF_* sockopt tunables unsupported in stock kernels */
@@ -531,46 +518,38 @@ ksocknal_lib_setup_sock (struct socket *sock)
 	do_keepalive = (keep_idle > 0 && keep_count > 0 && keep_intvl > 0);
 
 	option = (do_keepalive ? 1 : 0);
-	set_fs (KERNEL_DS);
-	rc = sock_setsockopt (sock, SOL_SOCKET, SO_KEEPALIVE,
+	rc = kernel_setsockopt(sock, SOL_SOCKET, SO_KEEPALIVE,
 			      (char *)&option, sizeof (option));
-	set_fs (oldmm);
 	if (rc != 0) {
 		CERROR ("Can't set SO_KEEPALIVE: %d\n", rc);
-		return (rc);
+		return rc;
 	}
 
 	if (!do_keepalive)
-		return (0);
+		return 0;
 
-	set_fs (KERNEL_DS);
-	rc = sock->ops->setsockopt (sock, SOL_TCP, TCP_KEEPIDLE,
+	rc = kernel_setsockopt(sock, SOL_TCP, TCP_KEEPIDLE,
 				    (char *)&keep_idle, sizeof (keep_idle));
-	set_fs (oldmm);
 	if (rc != 0) {
 		CERROR ("Can't set TCP_KEEPIDLE: %d\n", rc);
-		return (rc);
+		return rc;
 	}
 
-	set_fs (KERNEL_DS);
-	rc = sock->ops->setsockopt (sock, SOL_TCP, TCP_KEEPINTVL,
+	rc = kernel_setsockopt(sock, SOL_TCP, TCP_KEEPINTVL,
 				    (char *)&keep_intvl, sizeof (keep_intvl));
-	set_fs (oldmm);
 	if (rc != 0) {
 		CERROR ("Can't set TCP_KEEPINTVL: %d\n", rc);
-		return (rc);
+		return rc;
 	}
 
-	set_fs (KERNEL_DS);
-	rc = sock->ops->setsockopt (sock, SOL_TCP, TCP_KEEPCNT,
+	rc = kernel_setsockopt(sock, SOL_TCP, TCP_KEEPCNT,
 				    (char *)&keep_count, sizeof (keep_count));
-	set_fs (oldmm);
 	if (rc != 0) {
 		CERROR ("Can't set TCP_KEEPCNT: %d\n", rc);
-		return (rc);
+		return rc;
 	}
 
-	return (0);
+	return 0;
 }
 
 void
@@ -581,7 +560,6 @@ ksocknal_lib_push_conn (ksock_conn_t *conn)
 	int	     nonagle;
 	int	     val = 1;
 	int	     rc;
-	mm_segment_t    oldmm;
 
 	rc = ksocknal_connsock_addref(conn);
 	if (rc != 0)			    /* being shut down */
@@ -595,15 +573,10 @@ ksocknal_lib_push_conn (ksock_conn_t *conn)
 	tp->nonagle = 1;
 	release_sock (sk);
 
-	oldmm = get_fs ();
-	set_fs (KERNEL_DS);
-
-	rc = sk->sk_prot->setsockopt (sk, SOL_TCP, TCP_NODELAY,
+	rc = kernel_setsockopt(conn->ksnc_sock, SOL_TCP, TCP_NODELAY,
 				      (char *)&val, sizeof (val));
 	LASSERT (rc == 0);
 
-	set_fs (oldmm);
-
 	lock_sock (sk);
 	tp->nonagle = nonagle;
 	release_sock (sk);
diff --git a/drivers/staging/lustre/lnet/lnet/api-ni.c b/drivers/staging/lustre/lnet/lnet/api-ni.c
index 45c23194081..3f878dee2e4 100644
--- a/drivers/staging/lustre/lnet/lnet/api-ni.c
+++ b/drivers/staging/lustre/lnet/lnet/api-ni.c
@@ -127,8 +127,7 @@ lnet_create_remote_nets_table(void)
 static void
 lnet_destroy_remote_nets_table(void)
 {
-	int		i;
-	struct list_head	*hash;
+	int i;
 
 	if (the_lnet.ln_remote_nets_hash == NULL)
 		return;
@@ -137,7 +136,8 @@ lnet_destroy_remote_nets_table(void)
 		LASSERT(list_empty(&the_lnet.ln_remote_nets_hash[i]));
 
 	LIBCFS_FREE(the_lnet.ln_remote_nets_hash,
-		    LNET_REMOTE_NETS_HASH_SIZE * sizeof(*hash));
+		    LNET_REMOTE_NETS_HASH_SIZE *
+		    sizeof(the_lnet.ln_remote_nets_hash[0]));
 	the_lnet.ln_remote_nets_hash = NULL;
 }
 
@@ -338,7 +338,7 @@ lnet_counters_get(lnet_counters_t *counters)
 		counters->send_count   += ctr->send_count;
 		counters->recv_count   += ctr->recv_count;
 		counters->route_count  += ctr->route_count;
-		counters->drop_length  += ctr->drop_length;
+		counters->drop_count   += ctr->drop_count;
 		counters->send_length  += ctr->send_length;
 		counters->recv_length  += ctr->recv_length;
 		counters->route_length += ctr->route_length;
@@ -986,12 +986,11 @@ lnet_shutdown_lndnis (void)
 			break;
 		}
 
-		while (!list_empty(&ni->ni_list)) {
+		if (!list_empty(&ni->ni_list)) {
 			lnet_net_unlock(LNET_LOCK_EX);
 			++i;
 			if ((i & (-i)) == i) {
-				CDEBUG(D_WARNING,
-				       "Waiting for zombie LNI %s\n",
+				CDEBUG(D_WARNING, "Waiting for zombie LNI %s\n",
 				       libcfs_nid2str(ni->ni_nid));
 			}
 			set_current_state(TASK_UNINTERRUPTIBLE);
@@ -1016,6 +1015,8 @@ lnet_shutdown_lndnis (void)
 			       libcfs_nid2str(ni->ni_nid));
 
 		lnet_ni_free(ni);
+		i = 2;
+
 		lnet_net_lock(LNET_LOCK_EX);
 	}
 
@@ -1926,6 +1927,7 @@ lnet_ping (lnet_process_id_t id, int timeout_ms, lnet_process_id_t *ids, int n_i
 
 	rc = -EFAULT;			   /* If I SEGV... */
 
+	memset(&tmpid, 0, sizeof(tmpid));
 	for (i = 0; i < n_ids; i++) {
 		tmpid.pid = info->pi_pid;
 		tmpid.nid = info->pi_ni[i].ns_nid;
diff --git a/drivers/staging/lustre/lnet/lnet/lib-eq.c b/drivers/staging/lustre/lnet/lnet/lib-eq.c
index 7ce07f61b2e..d25dcd8ba56 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-eq.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-eq.c
@@ -238,7 +238,7 @@ lnet_eq_enqueue_event(lnet_eq_t *eq, lnet_event_t *ev)
 	lnet_eq_wait_unlock();
 }
 
-int
+static int
 lnet_eq_dequeue_event(lnet_eq_t *eq, lnet_event_t *ev)
 {
 	int		new_index = eq->eq_deq_seq & (eq->eq_size - 1);
@@ -325,6 +325,7 @@ EXPORT_SYMBOL(LNetEQWait);
 
 static int
 lnet_eq_wait_locked(int *timeout_ms)
+__must_hold(&the_lnet.ln_eq_wait_lock)
 {
 	int		tms = *timeout_ms;
 	int		wait;
diff --git a/drivers/staging/lustre/lnet/lnet/router.c b/drivers/staging/lustre/lnet/lnet/router.c
index 995f50976c4..926923a104c 100644
--- a/drivers/staging/lustre/lnet/lnet/router.c
+++ b/drivers/staging/lustre/lnet/lnet/router.c
@@ -145,7 +145,7 @@ lnet_ni_notify_locked(lnet_ni_t *ni, lnet_peer_t *lp)
 	 * NB individual events can be missed; the only guarantee is that you
 	 * always get the most recent news */
 
-	if (lp->lp_notifying)
+	if (lp->lp_notifying || ni == NULL)
 		return;
 
 	lp->lp_notifying = 1;
diff --git a/drivers/staging/lustre/lustre/fid/lproc_fid.c b/drivers/staging/lustre/lustre/fid/lproc_fid.c
index ddd813cab50..6f5674d1aa7 100644
--- a/drivers/staging/lustre/lustre/fid/lproc_fid.c
+++ b/drivers/staging/lustre/lustre/fid/lproc_fid.c
@@ -54,32 +54,49 @@
 #include <lustre_fid.h>
 #include "fid_internal.h"
 
+/* Format: [0x64BIT_INT - 0x64BIT_INT] + 32 bytes just in case */
+#define MAX_FID_RANGE_STRLEN (32 + 2 * 2 * sizeof(__u64))
 /*
  * Note: this function is only used for testing, it is no safe for production
  * use.
  */
-static int
-lprocfs_fid_write_common(const char *buffer, unsigned long count,
-			 struct lu_seq_range *range)
+static int lprocfs_fid_write_common(const char __user *buffer, size_t count,
+				    struct lu_seq_range *range)
 {
 	struct lu_seq_range tmp;
 	int rc;
+	char kernbuf[MAX_FID_RANGE_STRLEN];
 
 	LASSERT(range != NULL);
 
-	rc = sscanf(buffer, "[%llx - %llx]\n",
+	if (count >= sizeof(kernbuf))
+		return -EINVAL;
+
+	if (copy_from_user(kernbuf, buffer, count))
+		return -EFAULT;
+
+	kernbuf[count] = 0;
+
+	if (count == 5 && strcmp(kernbuf, "clear") == 0) {
+		memset(range, 0, sizeof(*range));
+		return count;
+	}
+
+	/* of the form "[0x0000000240000400 - 0x000000028000400]" */
+	rc = sscanf(kernbuf, "[%llx - %llx]\n",
 		    (long long unsigned *)&tmp.lsr_start,
 		    (long long unsigned *)&tmp.lsr_end);
-	if (rc != 2 || !range_is_sane(&tmp) || range_is_zero(&tmp))
+	if (!range_is_sane(&tmp) || range_is_zero(&tmp) ||
+	    tmp.lsr_start < range->lsr_start || tmp.lsr_end > range->lsr_end)
 		return -EINVAL;
 	*range = tmp;
-	return 0;
+	return count;
 }
 
 /* Client side procfs stuff */
-static ssize_t
-lprocfs_fid_space_seq_write(struct file *file, const char *buffer,
-			    size_t count, loff_t *off)
+static ssize_t lprocfs_fid_space_seq_write(struct file *file,
+					   const char __user *buffer,
+					   size_t count, loff_t *off)
 {
 	struct lu_client_seq *seq = ((struct seq_file *)file->private_data)->private;
 	int rc;
@@ -114,9 +131,9 @@ lprocfs_fid_space_seq_show(struct seq_file *m, void *unused)
 	return rc;
 }
 
-static ssize_t
-lprocfs_fid_width_seq_write(struct file *file, const char *buffer,
-			    size_t count, loff_t *off)
+static ssize_t lprocfs_fid_width_seq_write(struct file *file,
+					   const char __user *buffer,
+					   size_t count, loff_t *off)
 {
 	struct lu_client_seq *seq = ((struct seq_file *)file->private_data)->private;
 	__u64  max;
diff --git a/drivers/staging/lustre/lustre/include/dt_object.h b/drivers/staging/lustre/lustre/include/dt_object.h
index 9304c269afa..9b7921d1dea 100644
--- a/drivers/staging/lustre/lustre/include/dt_object.h
+++ b/drivers/staging/lustre/lustre/include/dt_object.h
@@ -441,7 +441,8 @@ struct dt_object_operations {
 					struct dt_object *dt,
 					struct lustre_capa *old,
 					__u64 opc);
-	int (*do_object_sync)(const struct lu_env *, struct dt_object *);
+	int (*do_object_sync)(const struct lu_env *env, struct dt_object *obj,
+			      __u64 start, __u64 end);
 	/**
 	 * Get object info of next level. Currently, only get inode from osd.
 	 * This is only used by quota b=16542
@@ -900,13 +901,13 @@ static inline int dt_object_lock(const struct lu_env *env,
 int dt_lookup_dir(const struct lu_env *env, struct dt_object *dir,
 		  const char *name, struct lu_fid *fid);
 
-static inline int dt_object_sync(const struct lu_env *env,
-				 struct dt_object *o)
+static inline int dt_object_sync(const struct lu_env *env, struct dt_object *o,
+				 __u64 start, __u64 end)
 {
 	LASSERT(o);
 	LASSERT(o->do_ops);
 	LASSERT(o->do_ops->do_object_sync);
-	return o->do_ops->do_object_sync(env, o);
+	return o->do_ops->do_object_sync(env, o, start, end);
 }
 
 int dt_declare_version_set(const struct lu_env *env, struct dt_object *o,
diff --git a/drivers/staging/lustre/lustre/include/ioctl.h b/drivers/staging/lustre/lustre/include/ioctl.h
deleted file mode 100644
index b986920926b..00000000000
--- a/drivers/staging/lustre/lustre/include/ioctl.h
+++ /dev/null
@@ -1,106 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * 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 version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- */
-
-#ifndef _IOWR
-
-/* On i386 and x86_64, _ASM_I386_IOCTL_H is defined by the kernel's ioctl.h,
- * and on newer kernels this header is shared as _ASM_GENERIC_IOCTL_H.
- *
- * We can avoid any problems with the kernel header being included again by
- * defining _ASM_I386_IOCTL_H here so that a later occurrence of <asm/ioctl.h>
- * does not include the kernel's ioctl.h after this one. b=14746 */
-#define _ASM_I386_IOCTL_H
-#define _ASM_GENERIC_IOCTL_H
-
-/* ioctl command encoding: 32 bits total, command in lower 16 bits,
- * size of the parameter structure in the lower 14 bits of the
- * upper 16 bits.
- * Encoding the size of the parameter structure in the ioctl request
- * The highest 2 bits are reserved for indicating the ``access mode''.
- * NOTE: This limits the max parameter size to 16kB -1 !
- */
-
-/*
- * The following is for compatibility across the various Linux
- * platforms.  The i386 ioctl numbering scheme doesn't really enforce
- * a type field.  De facto, however, the top 8 bits of the lower 16
- * bits are indeed used as a type field, so we might just as well make
- * this explicit here.  Please be sure to use the decoding macros
- * below from now on.
- */
-#define _IOC_NRBITS     8
-#define _IOC_TYPEBITS   8
-#define _IOC_SIZEBITS   14
-#define _IOC_DIRBITS    2
-
-#define _IOC_NRMASK     ((1 << _IOC_NRBITS)-1)
-#define _IOC_TYPEMASK   ((1 << _IOC_TYPEBITS)-1)
-#define _IOC_SIZEMASK   ((1 << _IOC_SIZEBITS)-1)
-#define _IOC_DIRMASK    ((1 << _IOC_DIRBITS)-1)
-
-#define _IOC_NRSHIFT    0
-#define _IOC_TYPESHIFT  (_IOC_NRSHIFT+_IOC_NRBITS)
-#define _IOC_SIZESHIFT  (_IOC_TYPESHIFT+_IOC_TYPEBITS)
-#define _IOC_DIRSHIFT   (_IOC_SIZESHIFT+_IOC_SIZEBITS)
-
-/*
- * Direction bits.
- */
-#define _IOC_NONE       0U
-#define _IOC_WRITE      1U
-#define _IOC_READ       2U
-
-#define _IOC(dir,type,nr,size) (((dir)  << _IOC_DIRSHIFT) | ((type) << _IOC_TYPESHIFT) | ((nr)   << _IOC_NRSHIFT) | ((size) << _IOC_SIZESHIFT))
-
-/* used to create numbers */
-#define _IO(type,nr)	    _IOC(_IOC_NONE,(type),(nr),0)
-#define _IOR(type,nr,size)      _IOC(_IOC_READ,(type),(nr),sizeof(size))
-#define _IOW(type,nr,size)      _IOC(_IOC_WRITE,(type),(nr),sizeof(size))
-#define _IOWR(type,nr,size)     _IOC(_IOC_READ|_IOC_WRITE,(type),(nr),sizeof(size))
-
-/* used to decode ioctl numbers.. */
-#define _IOC_DIR(nr)	    (((nr) >> _IOC_DIRSHIFT) & _IOC_DIRMASK)
-#define _IOC_TYPE(nr)	   (((nr) >> _IOC_TYPESHIFT) & _IOC_TYPEMASK)
-#define _IOC_NR(nr)	     (((nr) >> _IOC_NRSHIFT) & _IOC_NRMASK)
-#define _IOC_SIZE(nr)	   (((nr) >> _IOC_SIZESHIFT) & _IOC_SIZEMASK)
-
-/* ...and for the drivers/sound files... */
-
-#define IOC_IN	  (_IOC_WRITE << _IOC_DIRSHIFT)
-#define IOC_OUT	 (_IOC_READ << _IOC_DIRSHIFT)
-#define IOC_INOUT       ((_IOC_WRITE|_IOC_READ) << _IOC_DIRSHIFT)
-#define IOCSIZE_MASK    (_IOC_SIZEMASK << _IOC_SIZESHIFT)
-#define IOCSIZE_SHIFT   (_IOC_SIZESHIFT)
-
-#endif /* _IOWR */
diff --git a/drivers/staging/lustre/lustre/include/linux/obd.h b/drivers/staging/lustre/lustre/include/linux/obd.h
index dc36f75eb63..f96f65d5168 100644
--- a/drivers/staging/lustre/lustre/include/linux/obd.h
+++ b/drivers/staging/lustre/lustre/include/linux/obd.h
@@ -80,8 +80,8 @@ static inline void __client_obd_list_lock(client_obd_lock_t *lock,
 			break;
 		}
 
-		if ((jiffies - cur > 5 * HZ) &&
-		    (jiffies - lock->time > 5 * HZ)) {
+		if (time_before(cur + 5 * HZ, jiffies) &&
+		    time_before(lock->time + 5 * HZ, jiffies)) {
 			struct task_struct *task = lock->task;
 
 			if (task == NULL)
@@ -104,7 +104,7 @@ static inline void __client_obd_list_lock(client_obd_lock_t *lock,
 }
 
 #define client_obd_list_lock(lock) \
-	__client_obd_list_lock(lock, __FUNCTION__, __LINE__)
+	__client_obd_list_lock(lock, __func__, __LINE__)
 
 static inline void client_obd_list_unlock(client_obd_lock_t *lock)
 {
diff --git a/drivers/staging/lustre/lustre/include/lprocfs_status.h b/drivers/staging/lustre/lustre/include/lprocfs_status.h
index 428e3e4ce05..1b7f6a9bf62 100644
--- a/drivers/staging/lustre/lustre/include/lprocfs_status.h
+++ b/drivers/staging/lustre/lustre/include/lprocfs_status.h
@@ -369,6 +369,7 @@ static inline void s2dhms(struct dhms *ts, time_t secs)
 #define JOBSTATS_JOBID_VAR_MAX_LEN	20
 #define JOBSTATS_DISABLE		"disable"
 #define JOBSTATS_PROCNAME_UID		"procname_uid"
+#define JOBSTATS_NODELOCAL		"nodelocal"
 
 extern int lprocfs_write_frac_helper(const char *buffer, unsigned long count,
 				     int *val, int mult);
@@ -687,7 +688,7 @@ static int name##_single_open(cfs_inode_t *inode, struct file *file)	\
 {									\
 	return single_open(file, name##_seq_show, PDE_DATA(inode));	\
 }									\
-struct file_operations name##_fops = {				     \
+static struct file_operations name##_fops = {				\
 	.owner   = THIS_MODULE,					    \
 	.open    = name##_single_open,				     \
 	.read    = seq_read,					       \
@@ -730,7 +731,7 @@ struct file_operations name##_fops = {				     \
 	{								\
 		return single_open(file, NULL, PDE_DATA(inode));	\
 	}								\
-	struct file_operations name##_##type##_fops = {			\
+	static struct file_operations name##_##type##_fops = {	\
 		.open	= name##_##type##_open,				\
 		.write	= name##_##type##_write,			\
 		.release = lprocfs_single_release,			\
diff --git a/drivers/staging/lustre/lustre/include/lu_object.h b/drivers/staging/lustre/lustre/include/lu_object.h
index 6773bca1e0d..98149f5da15 100644
--- a/drivers/staging/lustre/lustre/include/lu_object.h
+++ b/drivers/staging/lustre/lustre/include/lu_object.h
@@ -516,6 +516,10 @@ enum lu_object_header_attr {
  */
 struct lu_object_header {
 	/**
+	 * Fid, uniquely identifying this object.
+	 */
+	struct lu_fid		loh_fid;
+	/**
 	 * Object flags from enum lu_object_header_flags. Set and checked
 	 * atomically.
 	 */
@@ -525,10 +529,6 @@ struct lu_object_header {
 	 */
 	atomic_t	   loh_ref;
 	/**
-	 * Fid, uniquely identifying this object.
-	 */
-	struct lu_fid	  loh_fid;
-	/**
 	 * Common object attributes, cached for efficiency. From enum
 	 * lu_object_header_attr.
 	 */
diff --git a/drivers/staging/lustre/lustre/include/lu_ref.h b/drivers/staging/lustre/lustre/include/lu_ref.h
index 50a2a7f786d..b451a888ca3 100644
--- a/drivers/staging/lustre/lustre/include/lu_ref.h
+++ b/drivers/staging/lustre/lustre/include/lu_ref.h
@@ -69,12 +69,12 @@
  *
  *	// current thread acquired a temporary reference to foo.
  *	foo_get(foo);
- *	lu_ref_add(&foo->reference, __FUNCTION__, current);
+ *	lu_ref_add(&foo->reference, __func__, current);
  *
  *	...
  *
  *	// temporary reference is released.
- *	lu_ref_del(&foo->reference, __FUNCTION__, current);
+ *	lu_ref_del(&foo->reference, __func__, current);
  *	foo_put(foo);
  * \endcode
  *
diff --git a/drivers/staging/lustre/lustre/include/lustre/lustre_idl.h b/drivers/staging/lustre/lustre/include/lustre/lustre_idl.h
index 87905bbc68e..83014c9fea6 100644
--- a/drivers/staging/lustre/lustre/include/lustre/lustre_idl.h
+++ b/drivers/staging/lustre/lustre/include/lustre/lustre_idl.h
@@ -265,7 +265,7 @@ static inline __u64 range_space(const struct lu_seq_range *range)
 
 static inline void range_init(struct lu_seq_range *range)
 {
-	range->lsr_start = range->lsr_end = range->lsr_index = 0;
+	memset(range, 0, sizeof(*range));
 }
 
 /**
@@ -1682,6 +1682,30 @@ static inline __u32 lov_mds_md_size(__u16 stripes, __u32 lmm_magic)
 				stripes * sizeof(struct lov_ost_data_v1);
 }
 
+static inline __u32
+lov_mds_md_max_stripe_count(size_t buf_size, __u32 lmm_magic)
+{
+	switch (lmm_magic) {
+	case LOV_MAGIC_V1: {
+		struct lov_mds_md_v1 lmm;
+
+		if (buf_size < sizeof(lmm))
+			return 0;
+
+		return (buf_size - sizeof(lmm)) / sizeof(lmm.lmm_objects[0]);
+	}
+	case LOV_MAGIC_V3: {
+		struct lov_mds_md_v3 lmm;
+
+		if (buf_size < sizeof(lmm))
+			return 0;
+
+		return (buf_size - sizeof(lmm)) / sizeof(lmm.lmm_objects[0]);
+	}
+	default:
+		return 0;
+	}
+}
 
 #define OBD_MD_FLID	(0x00000001ULL) /* object ID */
 #define OBD_MD_FLATIME     (0x00000002ULL) /* access time */
@@ -2681,6 +2705,8 @@ enum seq_op {
  * protocol, this will limit the max number of OSTs per LOV */
 
 #define LOV_DESC_MAGIC 0xB0CCDE5C
+#define LOV_DESC_QOS_MAXAGE_DEFAULT 5  /* Seconds */
+#define LOV_DESC_STRIPE_SIZE_DEFAULT (1 << LNET_MTU_BITS)
 
 /* LOV settings descriptor (should only contain static info) */
 struct lov_desc {
diff --git a/drivers/staging/lustre/lustre/include/lustre/lustre_user.h b/drivers/staging/lustre/lustre/include/lustre/lustre_user.h
index f5f369e603d..95c754f2075 100644
--- a/drivers/staging/lustre/lustre/include/lustre/lustre_user.h
+++ b/drivers/staging/lustre/lustre/include/lustre/lustre_user.h
@@ -1106,7 +1106,8 @@ static inline struct hsm_action_item * hai_zero(struct hsm_action_list *hal)
 {
 	return (struct hsm_action_item *)(hal->hal_fsname +
 					  cfs_size_round(strlen(hal-> \
-								hal_fsname)));
+								hal_fsname)
+							 + 1));
 }
 /* Return pointer to next hai */
 static inline struct hsm_action_item * hai_next(struct hsm_action_item *hai)
@@ -1121,7 +1122,7 @@ static inline int hal_size(struct hsm_action_list *hal)
 	int i, sz;
 	struct hsm_action_item *hai;
 
-	sz = sizeof(*hal) + cfs_size_round(strlen(hal->hal_fsname));
+	sz = sizeof(*hal) + cfs_size_round(strlen(hal->hal_fsname) + 1);
 	hai = hai_zero(hal);
 	for (i = 0; i < hal->hal_count; i++, hai = hai_next(hai))
 		sz += cfs_size_round(hai->hai_len);
diff --git a/drivers/staging/lustre/lustre/include/lustre_debug.h b/drivers/staging/lustre/lustre/include/lustre_debug.h
index 7ec91edd680..6146ccb8cce 100644
--- a/drivers/staging/lustre/lustre/include/lustre_debug.h
+++ b/drivers/staging/lustre/lustre/include/lustre_debug.h
@@ -48,7 +48,6 @@
 /* lib/debug.c */
 void dump_lniobuf(struct niobuf_local *lnb);
 int dump_req(struct ptlrpc_request *req);
-void dump_lsm(int level, struct lov_stripe_md *lsm);
 int block_debug_setup(void *addr, int len, __u64 off, __u64 id);
 int block_debug_check(char *who, void *addr, int len, __u64 off, __u64 id);
 
diff --git a/drivers/staging/lustre/lustre/include/lustre_dlm.h b/drivers/staging/lustre/lustre/include/lustre_dlm.h
index 3e25f001c07..0c6b7841e56 100644
--- a/drivers/staging/lustre/lustre/include/lustre_dlm.h
+++ b/drivers/staging/lustre/lustre/include/lustre_dlm.h
@@ -212,7 +212,7 @@ struct ldlm_pool_ops {
 	int (*po_recalc)(struct ldlm_pool *pl);
 	/** Cancel at least \a nr locks from pool \a pl */
 	int (*po_shrink)(struct ldlm_pool *pl, int nr,
-			 unsigned int gfp_mask);
+			 gfp_t gfp_mask);
 	int (*po_setup)(struct ldlm_pool *pl, int limit);
 };
 
@@ -260,7 +260,7 @@ struct ldlm_pool {
 	/** Recalculation period for pool. */
 	time_t			pl_recalc_period;
 	/** Recalculation and shrink operations. */
-	struct ldlm_pool_ops	*pl_ops;
+	const struct ldlm_pool_ops	*pl_ops;
 	/** Number of planned locks for next period. */
 	int			pl_grant_plan;
 	/** Pool statistics. */
@@ -1312,11 +1312,11 @@ int ldlm_lock_change_resource(struct ldlm_namespace *, struct ldlm_lock *,
 			      const struct ldlm_res_id *);
 
 #define LDLM_RESOURCE_ADDREF(res) do {				  \
-	lu_ref_add_atomic(&(res)->lr_reference, __FUNCTION__, current);  \
+	lu_ref_add_atomic(&(res)->lr_reference, __func__, current);  \
 } while (0)
 
 #define LDLM_RESOURCE_DELREF(res) do {				  \
-	lu_ref_del(&(res)->lr_reference, __FUNCTION__, current);  \
+	lu_ref_del(&(res)->lr_reference, __func__, current);	  \
 } while (0)
 
 /* ldlm_request.c */
@@ -1445,7 +1445,7 @@ static inline void unlock_res(struct ldlm_resource *res)
 /** Check if resource is already locked, assert if not. */
 static inline void check_res_locked(struct ldlm_resource *res)
 {
-	LASSERT(spin_is_locked(&res->lr_lock));
+	assert_spin_locked(&res->lr_lock);
 }
 
 struct ldlm_resource * lock_res_and_lock(struct ldlm_lock *lock);
@@ -1463,7 +1463,7 @@ void ldlm_pools_fini(void);
 int ldlm_pool_init(struct ldlm_pool *pl, struct ldlm_namespace *ns,
 		   int idx, ldlm_side_t client);
 int ldlm_pool_shrink(struct ldlm_pool *pl, int nr,
-		     unsigned int gfp_mask);
+		     gfp_t gfp_mask);
 void ldlm_pool_fini(struct ldlm_pool *pl);
 int ldlm_pool_setup(struct ldlm_pool *pl, int limit);
 int ldlm_pool_recalc(struct ldlm_pool *pl);
diff --git a/drivers/staging/lustre/lustre/include/lustre_lib.h b/drivers/staging/lustre/lustre/include/lustre_lib.h
index 0368ca6ffcc..3c26bbdc44b 100644
--- a/drivers/staging/lustre/lustre/include/lustre_lib.h
+++ b/drivers/staging/lustre/lustre/include/lustre_lib.h
@@ -94,19 +94,6 @@ struct obd_client_handle {
 void statfs_pack(struct obd_statfs *osfs, struct kstatfs *sfs);
 void statfs_unpack(struct kstatfs *sfs, struct obd_statfs *osfs);
 
-/* l_lock.c */
-struct lustre_lock {
-	int			l_depth;
-	struct task_struct	*l_owner;
-	struct semaphore	l_sem;
-	spinlock_t		l_spin;
-};
-
-void l_lock_init(struct lustre_lock *);
-void l_lock(struct lustre_lock *);
-void l_unlock(struct lustre_lock *);
-int l_has_lock(struct lustre_lock *);
-
 /*
  * For md echo client
  */
@@ -192,24 +179,25 @@ static inline int obd_ioctl_packlen(struct obd_ioctl_data *data)
 
 static inline int obd_ioctl_is_invalid(struct obd_ioctl_data *data)
 {
-	if (data->ioc_len > (1<<30)) {
-		CERROR("OBD ioctl: ioc_len larger than 1<<30\n");
+	if (data->ioc_len > OBD_MAX_IOCTL_BUFFER) {
+		CERROR("OBD ioctl: ioc_len larger than %d\n",
+		       OBD_MAX_IOCTL_BUFFER);
 		return 1;
 	}
-	if (data->ioc_inllen1 > (1<<30)) {
-		CERROR("OBD ioctl: ioc_inllen1 larger than 1<<30\n");
+	if (data->ioc_inllen1 > OBD_MAX_IOCTL_BUFFER) {
+		CERROR("OBD ioctl: ioc_inllen1 larger than ioc_len\n");
 		return 1;
 	}
-	if (data->ioc_inllen2 > (1<<30)) {
-		CERROR("OBD ioctl: ioc_inllen2 larger than 1<<30\n");
+	if (data->ioc_inllen2 > OBD_MAX_IOCTL_BUFFER) {
+		CERROR("OBD ioctl: ioc_inllen2 larger than ioc_len\n");
 		return 1;
 	}
-	if (data->ioc_inllen3 > (1<<30)) {
-		CERROR("OBD ioctl: ioc_inllen3 larger than 1<<30\n");
+	if (data->ioc_inllen3 > OBD_MAX_IOCTL_BUFFER) {
+		CERROR("OBD ioctl: ioc_inllen3 larger than ioc_len\n");
 		return 1;
 	}
-	if (data->ioc_inllen4 > (1<<30)) {
-		CERROR("OBD ioctl: ioc_inllen4 larger than 1<<30\n");
+	if (data->ioc_inllen4 > OBD_MAX_IOCTL_BUFFER) {
+		CERROR("OBD ioctl: ioc_inllen4 larger than ioc_len\n");
 		return 1;
 	}
 	if (data->ioc_inlbuf1 && !data->ioc_inllen1) {
diff --git a/drivers/staging/lustre/lustre/include/lustre_log.h b/drivers/staging/lustre/lustre/include/lustre_log.h
index 896c7576aa0..1a9a9228b79 100644
--- a/drivers/staging/lustre/lustre/include/lustre_log.h
+++ b/drivers/staging/lustre/lustre/include/lustre_log.h
@@ -206,11 +206,7 @@ int llog_setup(const struct lu_env *env, struct obd_device *obd,
 int __llog_ctxt_put(const struct lu_env *env, struct llog_ctxt *ctxt);
 int llog_cleanup(const struct lu_env *env, struct llog_ctxt *);
 int llog_sync(struct llog_ctxt *ctxt, struct obd_export *exp, int flags);
-int llog_obd_add(const struct lu_env *env, struct llog_ctxt *ctxt,
-		 struct llog_rec_hdr *rec, struct lov_stripe_md *lsm,
-		 struct llog_cookie *logcookies, int numcookies);
 int llog_cancel(const struct lu_env *env, struct llog_ctxt *ctxt,
-		struct lov_stripe_md *lsm, int count,
 		struct llog_cookie *cookies, int flags);
 
 int obd_llog_init(struct obd_device *obd, struct obd_llog_group *olg,
@@ -242,7 +238,6 @@ struct llog_operations {
 			int flags);
 	int (*lop_cleanup)(const struct lu_env *env, struct llog_ctxt *ctxt);
 	int (*lop_cancel)(const struct lu_env *env, struct llog_ctxt *ctxt,
-			  struct lov_stripe_md *lsm, int count,
 			  struct llog_cookie *cookies, int flags);
 	int (*lop_connect)(struct llog_ctxt *ctxt, struct llog_logid *logid,
 			   struct llog_gen *gen, struct obd_uuid *uuid);
@@ -296,11 +291,6 @@ struct llog_operations {
 	int (*lop_add)(const struct lu_env *env, struct llog_handle *lgh,
 		       struct llog_rec_hdr *rec, struct llog_cookie *cookie,
 		       void *buf, struct thandle *th);
-	/* Old llog_add version, used in MDS-LOV-OSC now and will gone with
-	 * LOD/OSP replacement */
-	int (*lop_obd_add)(const struct lu_env *env, struct llog_ctxt *ctxt,
-			   struct llog_rec_hdr *rec, struct lov_stripe_md *lsm,
-			   struct llog_cookie *logcookies, int numcookies);
 };
 
 /* In-memory descriptor for a log object or log catalog */
diff --git a/drivers/staging/lustre/lustre/include/lustre_mdc.h b/drivers/staging/lustre/lustre/include/lustre_mdc.h
index 468f36344a3..66765d4d201 100644
--- a/drivers/staging/lustre/lustre/include/lustre_mdc.h
+++ b/drivers/staging/lustre/lustre/include/lustre_mdc.h
@@ -140,17 +140,26 @@ static inline void mdc_put_rpc_lock(struct mdc_rpc_lock *lck,
 	mutex_unlock(&lck->rpcl_mutex);
 }
 
+/* Update the maximum observed easize and cookiesize.  The default easize
+ * and cookiesize is initialized to the minimum value but allowed to grow
+ * up to a single page in size if required to handle the common case.
+ */
 static inline void mdc_update_max_ea_from_body(struct obd_export *exp,
 					       struct mdt_body *body)
 {
 	if (body->valid & OBD_MD_FLMODEASIZE) {
-		if (exp->exp_obd->u.cli.cl_max_mds_easize < body->max_mdsize)
-			exp->exp_obd->u.cli.cl_max_mds_easize =
-						body->max_mdsize;
-		if (exp->exp_obd->u.cli.cl_max_mds_cookiesize <
-						body->max_cookiesize)
-			exp->exp_obd->u.cli.cl_max_mds_cookiesize =
-						body->max_cookiesize;
+		struct client_obd *cli = &exp->exp_obd->u.cli;
+
+		if (cli->cl_max_mds_easize < body->max_mdsize) {
+			cli->cl_max_mds_easize = body->max_mdsize;
+			cli->cl_default_mds_easize =
+			    min_t(__u32, body->max_mdsize, PAGE_CACHE_SIZE);
+		}
+		if (cli->cl_max_mds_cookiesize < body->max_cookiesize) {
+			cli->cl_max_mds_cookiesize = body->max_cookiesize;
+			cli->cl_default_mds_cookiesize =
+			    min_t(__u32, body->max_cookiesize, PAGE_CACHE_SIZE);
+		}
 	}
 }
 
diff --git a/drivers/staging/lustre/lustre/include/lustre_net.h b/drivers/staging/lustre/lustre/include/lustre_net.h
index 745adbb74cf..f6b7d10cb78 100644
--- a/drivers/staging/lustre/lustre/include/lustre_net.h
+++ b/drivers/staging/lustre/lustre/include/lustre_net.h
@@ -264,6 +264,8 @@
 #define LDLM_MAXREQSIZE   (5 * 1024)
 #define LDLM_MAXREPSIZE   (1024)
 
+#define MDS_MAXREQSIZE		(5 * 1024)	/* >= 4736 */
+
 #define OST_MAXREQSIZE		(5 * 1024)
 
 /* Macro to hide a typecast. */
@@ -717,7 +719,7 @@ struct ptlrpc_nrs_pol_ops {
 	 *			 \a nrq
 	 * \param[in,out] nrq	 The request
 	 *
-	 * \pre spin_is_locked(&svcpt->scp_req_lock)
+	 * \pre assert_spin_locked(&svcpt->scp_req_lock)
 	 *
 	 * \see ptlrpc_nrs_req_stop_nolock()
 	 */
diff --git a/drivers/staging/lustre/lustre/include/obd.h b/drivers/staging/lustre/lustre/include/obd.h
index 72cf3fe4b0c..d5c4613f182 100644
--- a/drivers/staging/lustre/lustre/include/obd.h
+++ b/drivers/staging/lustre/lustre/include/obd.h
@@ -132,6 +132,13 @@ static inline bool lsm_has_objects(struct lov_stripe_md *lsm)
 	return true;
 }
 
+static inline int lov_stripe_md_size(unsigned int stripe_count)
+{
+	struct lov_stripe_md lsm;
+
+	return sizeof(lsm) + stripe_count * sizeof(lsm.lsm_oinfo[0]);
+}
+
 struct obd_info;
 
 typedef int (*obd_enqueue_update_f)(void *cookie, int rc);
@@ -309,9 +316,10 @@ struct client_obd {
 	int		      cl_conn_count;
 	/* max_mds_easize is purely a performance thing so we don't have to
 	 * call obd_size_diskmd() all the time. */
-	int		      cl_default_mds_easize;
-	int		      cl_max_mds_easize;
-	int		      cl_max_mds_cookiesize;
+	int			 cl_default_mds_easize;
+	int			 cl_max_mds_easize;
+	int			 cl_default_mds_cookiesize;
+	int			 cl_max_mds_cookiesize;
 
 	enum lustre_sec_part     cl_sp_me;
 	enum lustre_sec_part     cl_sp_to;
@@ -398,7 +406,7 @@ struct client_obd {
 	struct mdc_rpc_lock     *cl_close_lock;
 
 	/* mgc datastruct */
-	struct semaphore	 cl_mgc_sem;
+	struct mutex		 cl_mgc_mutex;
 	struct local_oid_storage *cl_mgc_los;
 	struct dt_object	*cl_mgc_configs_dir;
 	atomic_t	     cl_mgc_refcount;
@@ -598,6 +606,7 @@ struct lmv_obd {
 	int			max_easize;
 	int			max_def_easize;
 	int			max_cookiesize;
+	int			max_def_cookiesize;
 	int			server_timeout;
 
 	int			tgts_size; /* size of tgts array */
@@ -989,7 +998,10 @@ enum obd_cleanup_stage {
 #define KEY_LOCK_TO_STRIPE      "lock_to_stripe"
 #define KEY_LOVDESC	     "lovdesc"
 #define KEY_LOV_IDX	     "lov_idx"
-#define KEY_MAX_EASIZE	  "max_easize"
+#define KEY_MAX_EASIZE		"max_easize"
+#define KEY_DEFAULT_EASIZE	"default_easize"
+#define KEY_MAX_COOKIESIZE	"max_cookiesize"
+#define KEY_DEFAULT_COOKIESIZE	"default_cookiesize"
 #define KEY_MDS_CONN	    "mds_conn"
 #define KEY_MGSSEC	      "mgssec"
 #define KEY_NEXT_ID	     "next_id"
@@ -1383,7 +1395,7 @@ struct md_ops {
 			  const char *, int, int, int,
 			  struct ptlrpc_request **);
 
-	int (*m_init_ea_size)(struct obd_export *, int, int, int);
+	int (*m_init_ea_size)(struct obd_export *, int, int, int, int);
 
 	int (*m_get_lustre_md)(struct obd_export *, struct ptlrpc_request *,
 			       struct obd_export *, struct obd_export *,
diff --git a/drivers/staging/lustre/lustre/include/obd_class.h b/drivers/staging/lustre/lustre/include/obd_class.h
index 9d1f266e55e..e265820c009 100644
--- a/drivers/staging/lustre/lustre/include/obd_class.h
+++ b/drivers/staging/lustre/lustre/include/obd_class.h
@@ -711,15 +711,6 @@ static inline int obd_size_diskmd(struct obd_export *exp,
 	return obd_packmd(exp, NULL, mem_src);
 }
 
-/* helper functions */
-static inline int obd_alloc_diskmd(struct obd_export *exp,
-				   struct lov_mds_md **disk_tgt)
-{
-	LASSERT(disk_tgt);
-	LASSERT(*disk_tgt == NULL);
-	return obd_packmd(exp, disk_tgt, NULL);
-}
-
 static inline int obd_free_diskmd(struct obd_export *exp,
 				  struct lov_mds_md **disk_tgt)
 {
@@ -2055,12 +2046,13 @@ static inline ldlm_mode_t md_lock_match(struct obd_export *exp, __u64 flags,
 }
 
 static inline int md_init_ea_size(struct obd_export *exp, int easize,
-				  int def_asize, int cookiesize)
+				  int def_asize, int cookiesize,
+				  int def_cookiesize)
 {
 	EXP_CHECK_MD_OP(exp, init_ea_size);
 	EXP_MD_COUNTER_INCREMENT(exp, init_ea_size);
 	return MDP(exp->exp_obd, init_ea_size)(exp, easize, def_asize,
-					       cookiesize);
+					       cookiesize, def_cookiesize);
 }
 
 static inline int md_get_remote_perm(struct obd_export *exp,
@@ -2133,7 +2125,7 @@ extern struct kmem_cache *obdo_cachep;
 
 #define OBDO_ALLOC(ptr)						       \
 do {									  \
-	OBD_SLAB_ALLOC_PTR_GFP((ptr), obdo_cachep, __GFP_IO);	     \
+	OBD_SLAB_ALLOC_PTR_GFP((ptr), obdo_cachep, GFP_NOFS);             \
 } while(0)
 
 #define OBDO_FREE(ptr)							\
@@ -2190,6 +2182,9 @@ void class_exit_uuidlist(void);
 int mea_name2idx(struct lmv_stripe_md *mea, const char *name, int namelen);
 int raw_name2idx(int hashtype, int count, const char *name, int namelen);
 
+/* class_obd.c */
+extern char obd_jobid_node[];
+
 /* prng.c */
 #define ll_generate_random_uuid(uuid_out) cfs_get_random_bytes(uuid_out, sizeof(class_uuid_t))
 
diff --git a/drivers/staging/lustre/lustre/include/obd_lov.h b/drivers/staging/lustre/lustre/include/obd_lov.h
deleted file mode 100644
index 235718b366d..00000000000
--- a/drivers/staging/lustre/lustre/include/obd_lov.h
+++ /dev/null
@@ -1,116 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * 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 version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- *
- * Copyright (c) 2011, Intel Corporation.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- */
-
-#ifndef _OBD_LOV_H__
-#define _OBD_LOV_H__
-
-#define LOV_DEFAULT_STRIPE_SIZE (1 << LNET_MTU_BITS)
-
-static inline int lov_stripe_md_size(__u16 stripes)
-{
-	return sizeof(struct lov_stripe_md) + stripes*sizeof(struct lov_oinfo*);
-}
-
-struct lov_version_size {
-	__u32   lvs_magic;
-	size_t  lvs_lmm_size;
-	size_t  lvs_lod_size;
-};
-
-static inline __u32 lov_mds_md_stripecnt(int ea_size, __u32 lmm_magic)
-{
-	static const struct lov_version_size lmm_ver_size[] = {
-			{ .lvs_magic = LOV_MAGIC_V3,
-			  .lvs_lmm_size = sizeof(struct lov_mds_md_v3),
-			  .lvs_lod_size = sizeof(struct lov_ost_data_v1) },
-			{ .lvs_magic = LOV_MAGIC_V1,
-			  .lvs_lmm_size = sizeof(struct lov_mds_md_v1),
-			  .lvs_lod_size = sizeof(struct lov_ost_data_v1)} };
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(lmm_ver_size); i++) {
-		if (lmm_magic == lmm_ver_size[i].lvs_magic) {
-			if (ea_size <= lmm_ver_size[i].lvs_lmm_size)
-				return 0;
-			return (ea_size - lmm_ver_size[i].lvs_lmm_size) /
-				lmm_ver_size[i].lvs_lod_size;
-		}
-	}
-
-	/* Invalid LOV magic, so no stripes could fit */
-	return 0;
-}
-
-/* lov_do_div64(a, b) returns a % b, and a = a / b.
- * The 32-bit code is LOV-specific due to knowing about stripe limits in
- * order to reduce the divisor to a 32-bit number.  If the divisor is
- * already a 32-bit value the compiler handles this directly. */
-#if BITS_PER_LONG > 32
-# define lov_do_div64(n,base) ({					\
-	uint64_t __base = (base);					\
-	uint64_t __rem;							\
-	__rem = ((uint64_t)(n)) % __base;				\
-	(n) = ((uint64_t)(n)) / __base;					\
-	__rem;								\
-  })
-#else
-# define lov_do_div64(n,base) ({					\
-	uint64_t __rem;							\
-	if ((sizeof(base) > 4) && (((base) & 0xffffffff00000000ULL) != 0)) {  \
-		int __remainder;					      \
-		LASSERTF(!((base) & (LOV_MIN_STRIPE_SIZE - 1)), "64 bit lov " \
-			 "division %llu / %llu\n", (n), (uint64_t)(base));    \
-		__remainder = (n) & (LOV_MIN_STRIPE_SIZE - 1);		\
-		(n) >>= LOV_MIN_STRIPE_BITS;				\
-		__rem = do_div(n, (base) >> LOV_MIN_STRIPE_BITS);	\
-		__rem <<= LOV_MIN_STRIPE_BITS;				\
-		__rem += __remainder;					\
-	} else {							\
-		__rem = do_div(n, base);				\
-	}								\
-	__rem;								\
-  })
-#endif
-
-#define IOC_LOV_TYPE		   'g'
-#define IOC_LOV_MIN_NR		 50
-#define IOC_LOV_SET_OSC_ACTIVE	 _IOWR('g', 50, long)
-#define IOC_LOV_MAX_NR		 50
-
-#define QOS_DEFAULT_THRESHOLD	   10 /* MB */
-#define QOS_DEFAULT_MAXAGE	      5  /* Seconds */
-
-#endif
diff --git a/drivers/staging/lustre/lustre/include/obd_support.h b/drivers/staging/lustre/lustre/include/obd_support.h
index 5ec336968fc..cc5af509b26 100644
--- a/drivers/staging/lustre/lustre/include/obd_support.h
+++ b/drivers/staging/lustre/lustre/include/obd_support.h
@@ -641,8 +641,8 @@ do {									      \
 #define OBD_ALLOC_GFP(ptr, size, gfp_mask)				      \
 	__OBD_MALLOC_VERBOSE(ptr, NULL, 0, size, gfp_mask)
 
-#define OBD_ALLOC(ptr, size) OBD_ALLOC_GFP(ptr, size, __GFP_IO)
-#define OBD_ALLOC_WAIT(ptr, size) OBD_ALLOC_GFP(ptr, size, GFP_IOFS)
+#define OBD_ALLOC(ptr, size) OBD_ALLOC_GFP(ptr, size, GFP_NOFS)
+#define OBD_ALLOC_WAIT(ptr, size) OBD_ALLOC_GFP(ptr, size, GFP_KERNEL)
 #define OBD_ALLOC_PTR(ptr) OBD_ALLOC(ptr, sizeof(*(ptr)))
 #define OBD_ALLOC_PTR_WAIT(ptr) OBD_ALLOC_WAIT(ptr, sizeof(*(ptr)))
 
@@ -650,7 +650,7 @@ do {									      \
 	__OBD_MALLOC_VERBOSE(ptr, cptab, cpt, size, gfp_mask)
 
 #define OBD_CPT_ALLOC(ptr, cptab, cpt, size)				      \
-	OBD_CPT_ALLOC_GFP(ptr, cptab, cpt, size, __GFP_IO)
+	OBD_CPT_ALLOC_GFP(ptr, cptab, cpt, size, GFP_NOFS)
 
 #define OBD_CPT_ALLOC_PTR(ptr, cptab, cpt)				      \
 	OBD_CPT_ALLOC(ptr, cptab, cpt, sizeof(*(ptr)))
@@ -793,10 +793,10 @@ do {									  \
 } while(0)
 
 #define OBD_SLAB_ALLOC(ptr, slab, size)					      \
-	OBD_SLAB_ALLOC_GFP(ptr, slab, size, __GFP_IO)
+	OBD_SLAB_ALLOC_GFP(ptr, slab, size, GFP_NOFS)
 
 #define OBD_SLAB_CPT_ALLOC(ptr, slab, cptab, cpt, size)			      \
-	OBD_SLAB_CPT_ALLOC_GFP(ptr, slab, cptab, cpt, size, __GFP_IO)
+	OBD_SLAB_CPT_ALLOC_GFP(ptr, slab, cptab, cpt, size, GFP_NOFS)
 
 #define OBD_SLAB_ALLOC_PTR(ptr, slab)					      \
 	OBD_SLAB_ALLOC(ptr, slab, sizeof(*(ptr)))
diff --git a/drivers/staging/lustre/lustre/lclient/lcommon_cl.c b/drivers/staging/lustre/lustre/lclient/lcommon_cl.c
index 6907a16dbbd..dc24cfa5803 100644
--- a/drivers/staging/lustre/lustre/lclient/lcommon_cl.c
+++ b/drivers/staging/lustre/lustre/lclient/lcommon_cl.c
@@ -63,7 +63,7 @@
 
 #include "../llite/llite_internal.h"
 
-const struct cl_req_operations ccc_req_ops;
+static const struct cl_req_operations ccc_req_ops;
 
 /*
  * ccc_ prefix stands for "Common Client Code".
@@ -112,12 +112,11 @@ static struct lu_kmem_descr ccc_caches[] = {
  *
  */
 
-void *ccc_key_init(const struct lu_context *ctx,
-			  struct lu_context_key *key)
+void *ccc_key_init(const struct lu_context *ctx, struct lu_context_key *key)
 {
 	struct ccc_thread_info *info;
 
-	OBD_SLAB_ALLOC_PTR_GFP(info, ccc_thread_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(info, ccc_thread_kmem, GFP_NOFS);
 	if (info == NULL)
 		info = ERR_PTR(-ENOMEM);
 	return info;
@@ -135,7 +134,7 @@ void *ccc_session_key_init(const struct lu_context *ctx,
 {
 	struct ccc_session *session;
 
-	OBD_SLAB_ALLOC_PTR_GFP(session, ccc_session_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(session, ccc_session_kmem, GFP_NOFS);
 	if (session == NULL)
 		session = ERR_PTR(-ENOMEM);
 	return session;
@@ -251,7 +250,7 @@ int ccc_req_init(const struct lu_env *env, struct cl_device *dev,
 	struct ccc_req *vrq;
 	int result;
 
-	OBD_SLAB_ALLOC_PTR_GFP(vrq, ccc_req_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(vrq, ccc_req_kmem, GFP_NOFS);
 	if (vrq != NULL) {
 		cl_req_slice_add(req, &vrq->crq_cl, dev, &ccc_req_ops);
 		result = 0;
@@ -327,7 +326,7 @@ struct lu_object *ccc_object_alloc(const struct lu_env *env,
 	struct ccc_object *vob;
 	struct lu_object  *obj;
 
-	OBD_SLAB_ALLOC_PTR_GFP(vob, ccc_object_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(vob, ccc_object_kmem, GFP_NOFS);
 	if (vob != NULL) {
 		struct cl_object_header *hdr;
 
@@ -396,7 +395,7 @@ int ccc_lock_init(const struct lu_env *env,
 
 	CLOBINVRNT(env, obj, ccc_object_invariant(obj));
 
-	OBD_SLAB_ALLOC_PTR_GFP(clk, ccc_lock_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(clk, ccc_lock_kmem, GFP_NOFS);
 	if (clk != NULL) {
 		cl_lock_slice_add(lock, &clk->clk_cl, obj, lkops);
 		result = 0;
@@ -963,7 +962,7 @@ void ccc_req_attr_set(const struct lu_env *env,
 	       JOBSTATS_JOBID_SIZE);
 }
 
-const struct cl_req_operations ccc_req_ops = {
+static const struct cl_req_operations ccc_req_ops = {
 	.cro_attr_set   = ccc_req_attr_set,
 	.cro_completion = ccc_req_completion
 };
diff --git a/drivers/staging/lustre/lustre/lclient/lcommon_misc.c b/drivers/staging/lustre/lustre/lclient/lcommon_misc.c
index e04c2d37c24..21de1cd2afb 100644
--- a/drivers/staging/lustre/lustre/lclient/lcommon_misc.c
+++ b/drivers/staging/lustre/lustre/lclient/lcommon_misc.c
@@ -56,7 +56,7 @@ int cl_init_ea_size(struct obd_export *md_exp, struct obd_export *dt_exp)
 	__u32 valsize = sizeof(struct lov_desc);
 	int rc, easize, def_easize, cookiesize;
 	struct lov_desc desc;
-	__u16 stripes;
+	__u16 stripes, def_stripes;
 
 	rc = obd_get_info(NULL, dt_exp, sizeof(KEY_LOVDESC), KEY_LOVDESC,
 			  &valsize, &desc, NULL);
@@ -67,15 +67,20 @@ int cl_init_ea_size(struct obd_export *md_exp, struct obd_export *dt_exp)
 	lsm.lsm_stripe_count = stripes;
 	easize = obd_size_diskmd(dt_exp, &lsm);
 
-	lsm.lsm_stripe_count = desc.ld_default_stripe_count;
+	def_stripes = min_t(__u32, desc.ld_default_stripe_count,
+			    LOV_MAX_STRIPE_COUNT);
+	lsm.lsm_stripe_count = def_stripes;
 	def_easize = obd_size_diskmd(dt_exp, &lsm);
 
 	cookiesize = stripes * sizeof(struct llog_cookie);
 
-	CDEBUG(D_HA, "updating max_mdsize/max_cookiesize: %d/%d\n",
-	       easize, cookiesize);
+	/* default cookiesize is 0 because from 2.4 server doesn't send
+	 * llog cookies to client. */
+	CDEBUG(D_HA,
+	       "updating def/max_easize: %d/%d def/max_cookiesize: 0/%d\n",
+	       def_easize, easize, cookiesize);
 
-	rc = md_init_ea_size(md_exp, easize, def_easize, cookiesize);
+	rc = md_init_ea_size(md_exp, easize, def_easize, cookiesize, 0);
 	return rc;
 }
 
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_extent.c b/drivers/staging/lustre/lustre/ldlm/ldlm_extent.c
index ac5d66aa7f0..bde3a82ce0d 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_extent.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_extent.c
@@ -104,7 +104,7 @@ struct ldlm_interval *ldlm_interval_alloc(struct ldlm_lock *lock)
 	struct ldlm_interval *node;
 
 	LASSERT(lock->l_resource->lr_type == LDLM_EXTENT);
-	OBD_SLAB_ALLOC_PTR_GFP(node, ldlm_interval_slab, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(node, ldlm_interval_slab, GFP_NOFS);
 	if (node == NULL)
 		return NULL;
 
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h b/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h
index 8cd79633dea..f997566ef11 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h
@@ -247,7 +247,7 @@ typedef enum ldlm_policy_res ldlm_policy_res_t;
 	struct __##var##__dummy_read {;} /* semicolon catcher */
 
 #define LDLM_POOL_PROC_WRITER(var, type)				    \
-	int lprocfs_wr_##var(struct file *file, const char *buffer,	    \
+	static int lprocfs_wr_##var(struct file *file, const char *buffer,  \
 			     unsigned long count, void *data)		    \
 	{								    \
 		struct ldlm_pool *pl = data;				    \
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_lib.c b/drivers/staging/lustre/lustre/ldlm/ldlm_lib.c
index 1a8c0d7005c..8bb59155968 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_lib.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_lib.c
@@ -325,7 +325,7 @@ int client_obd_setup(struct obd_device *obddev, struct lustre_cfg *lcfg)
 	}
 
 	init_rwsem(&cli->cl_sem);
-	sema_init(&cli->cl_mgc_sem, 1);
+	mutex_init(&cli->cl_mgc_mutex);
 	cli->cl_conn_count = 0;
 	memcpy(server_uuid.uuid, lustre_cfg_buf(lcfg, 2),
 	       min_t(unsigned int, LUSTRE_CFG_BUFLEN(lcfg, 2),
@@ -514,7 +514,7 @@ int client_connect_import(const struct lu_env *env,
 		LASSERT (imp->imp_state == LUSTRE_IMP_DISCON);
 		GOTO(out_ldlm, rc);
 	}
-	LASSERT((*exp)->exp_connection);
+	LASSERT(*exp != NULL && (*exp)->exp_connection);
 
 	if (data) {
 		LASSERTF((ocd->ocd_connect_flags & data->ocd_connect_flags) ==
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c b/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c
index 0548aca45e8..1b3f5c1eb80 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c
@@ -232,8 +232,6 @@ int ldlm_lock_remove_from_lru_nolock(struct ldlm_lock *lock)
 
 		LASSERT(lock->l_resource->lr_type != LDLM_FLOCK);
 		list_del_init(&lock->l_lru);
-		if (lock->l_flags & LDLM_FL_SKIPPED)
-			lock->l_flags &= ~LDLM_FL_SKIPPED;
 		LASSERT(ns->ns_nr_unused > 0);
 		ns->ns_nr_unused--;
 		rc = 1;
@@ -271,6 +269,8 @@ void ldlm_lock_add_to_lru_nolock(struct ldlm_lock *lock)
 	LASSERT(list_empty(&lock->l_lru));
 	LASSERT(lock->l_resource->lr_type != LDLM_FLOCK);
 	list_add_tail(&lock->l_lru, &ns->ns_unused_list);
+	if (lock->l_flags & LDLM_FL_SKIPPED)
+		lock->l_flags &= ~LDLM_FL_SKIPPED;
 	LASSERT(ns->ns_nr_unused >= 0);
 	ns->ns_nr_unused++;
 }
@@ -437,7 +437,7 @@ static struct ldlm_lock *ldlm_lock_new(struct ldlm_resource *resource)
 	if (resource == NULL)
 		LBUG();
 
-	OBD_SLAB_ALLOC_PTR_GFP(lock, ldlm_lock_slab, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(lock, ldlm_lock_slab, GFP_NOFS);
 	if (lock == NULL)
 		return NULL;
 
@@ -1624,7 +1624,7 @@ ldlm_error_t ldlm_lock_enqueue(struct ldlm_namespace *ns,
 	 * have to allocate the interval node early otherwise we can't regrant
 	 * this lock in the future. - jay */
 	if (!local && (*flags & LDLM_FL_REPLAY) && res->lr_type == LDLM_EXTENT)
-		OBD_SLAB_ALLOC_PTR_GFP(node, ldlm_interval_slab, __GFP_IO);
+		OBD_SLAB_ALLOC_PTR_GFP(node, ldlm_interval_slab, GFP_NOFS);
 
 	lock_res_and_lock(lock);
 	if (local && lock->l_req_mode == lock->l_granted_mode) {
@@ -2146,7 +2146,7 @@ struct ldlm_resource *ldlm_lock_convert(struct ldlm_lock *lock, int new_mode,
 
 	/* I can't check the type of lock here because the bitlock of lock
 	 * is not held here, so do the allocation blindly. -jay */
-	OBD_SLAB_ALLOC_PTR_GFP(node, ldlm_interval_slab, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(node, ldlm_interval_slab, GFP_NOFS);
 	if (node == NULL)
 		/* Actually, this causes EDEADLOCK to be returned */
 		return NULL;
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c b/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c
index 6758646f575..2cc698179b6 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c
@@ -378,7 +378,7 @@ static int ldlm_srv_pool_recalc(struct ldlm_pool *pl)
  * locks smaller in next 10h.
  */
 static int ldlm_srv_pool_shrink(struct ldlm_pool *pl,
-				int nr, unsigned int gfp_mask)
+				int nr, gfp_t gfp_mask)
 {
 	__u32 limit;
 
@@ -518,7 +518,7 @@ static int ldlm_cli_pool_recalc(struct ldlm_pool *pl)
  * passed \a pl according to \a nr and \a gfp_mask.
  */
 static int ldlm_cli_pool_shrink(struct ldlm_pool *pl,
-				int nr, unsigned int gfp_mask)
+				int nr, gfp_t gfp_mask)
 {
 	struct ldlm_namespace *ns;
 	int unused;
@@ -546,13 +546,13 @@ static int ldlm_cli_pool_shrink(struct ldlm_pool *pl,
 		return ldlm_cancel_lru(ns, nr, LCF_ASYNC, LDLM_CANCEL_SHRINK);
 }
 
-struct ldlm_pool_ops ldlm_srv_pool_ops = {
+static const struct ldlm_pool_ops ldlm_srv_pool_ops = {
 	.po_recalc = ldlm_srv_pool_recalc,
 	.po_shrink = ldlm_srv_pool_shrink,
 	.po_setup  = ldlm_srv_pool_setup
 };
 
-struct ldlm_pool_ops ldlm_cli_pool_ops = {
+static const struct ldlm_pool_ops ldlm_cli_pool_ops = {
 	.po_recalc = ldlm_cli_pool_recalc,
 	.po_shrink = ldlm_cli_pool_shrink
 };
@@ -603,7 +603,7 @@ int ldlm_pool_recalc(struct ldlm_pool *pl)
  * freeable locks. Otherwise, return the number of canceled locks.
  */
 int ldlm_pool_shrink(struct ldlm_pool *pl, int nr,
-		     unsigned int gfp_mask)
+		     gfp_t gfp_mask)
 {
 	int cancel = 0;
 
@@ -718,7 +718,7 @@ LPROC_SEQ_FOPS_RO(lprocfs_grant_speed);
 		snprintf(var_name, MAX_STRING_SIZE, #name);	\
 		pool_vars[0].data = var;			\
 		pool_vars[0].fops = ops;			\
-		lprocfs_add_vars(pl->pl_proc_dir, pool_vars, 0);\
+		lprocfs_add_vars(pl->pl_proc_dir, pool_vars, NULL);\
 	} while (0)
 
 static int ldlm_pool_proc_init(struct ldlm_pool *pl)
@@ -1029,7 +1029,7 @@ static struct completion ldlm_pools_comp;
  * count locks from all namespaces (if possible). Returns number of
  * cached locks.
  */
-static unsigned long ldlm_pools_count(ldlm_side_t client, unsigned int gfp_mask)
+static unsigned long ldlm_pools_count(ldlm_side_t client, gfp_t gfp_mask)
 {
 	int total = 0, nr_ns;
 	struct ldlm_namespace *ns;
@@ -1082,7 +1082,7 @@ static unsigned long ldlm_pools_count(ldlm_side_t client, unsigned int gfp_mask)
 	return total;
 }
 
-static unsigned long ldlm_pools_scan(ldlm_side_t client, int nr, unsigned int gfp_mask)
+static unsigned long ldlm_pools_scan(ldlm_side_t client, int nr, gfp_t gfp_mask)
 {
 	unsigned long freed = 0;
 	int tmp, nr_ns;
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c b/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c
index 2824d4a9a85..c55d72f79b2 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c
@@ -89,7 +89,7 @@ int ldlm_proc_setup(void)
 {
 	int rc;
 	struct lprocfs_vars list[] = {
-		{ "dump_namespaces", &ldlm_dump_ns_fops, 0, 0222 },
+		{ "dump_namespaces", &ldlm_dump_ns_fops, NULL, 0222 },
 		{ "dump_granted_max", &ldlm_rw_uint_fops,
 		  &ldlm_dump_granted_max },
 		{ "cancel_unused_locks_before_replay", &ldlm_rw_uint_fops,
@@ -322,7 +322,7 @@ void ldlm_namespace_proc_unregister(struct ldlm_namespace *ns)
 		snprintf(lock_name, MAX_STRING_SIZE, name);	\
 		lock_vars[0].data = var;			\
 		lock_vars[0].fops = ops;			\
-		lprocfs_add_vars(ns_pde, lock_vars, 0);		\
+		lprocfs_add_vars(ns_pde, lock_vars, NULL);	\
 	} while (0)
 
 int ldlm_namespace_proc_register(struct ldlm_namespace *ns)
@@ -1014,7 +1014,7 @@ static struct ldlm_resource *ldlm_resource_new(void)
 	struct ldlm_resource *res;
 	int idx;
 
-	OBD_SLAB_ALLOC_PTR_GFP(res, ldlm_resource_slab, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(res, ldlm_resource_slab, GFP_NOFS);
 	if (res == NULL)
 		return NULL;
 
diff --git a/drivers/staging/lustre/lustre/libcfs/libcfs_cpu.c b/drivers/staging/lustre/lustre/libcfs/libcfs_cpu.c
index 1fb37008cda..a1a7bf44ccc 100644
--- a/drivers/staging/lustre/lustre/libcfs/libcfs_cpu.c
+++ b/drivers/staging/lustre/lustre/libcfs/libcfs_cpu.c
@@ -74,6 +74,22 @@ cfs_cpt_table_free(struct cfs_cpt_table *cptab)
 }
 EXPORT_SYMBOL(cfs_cpt_table_free);
 
+#ifdef CONFIG_SMP
+int
+cfs_cpt_table_print(struct cfs_cpt_table *cptab, char *buf, int len)
+{
+	int	rc = 0;
+
+	rc = snprintf(buf, len, "%d\t: %d\n", 0, 0);
+	len -= rc;
+	if (len <= 0)
+		return -EFBIG;
+
+	return rc;
+}
+EXPORT_SYMBOL(cfs_cpt_table_print);
+#endif /* CONFIG_SMP */
+
 int
 cfs_cpt_number(struct cfs_cpt_table *cptab)
 {
@@ -161,6 +177,13 @@ cfs_cpt_spread_node(struct cfs_cpt_table *cptab, int cpt)
 EXPORT_SYMBOL(cfs_cpt_spread_node);
 
 int
+cfs_cpu_ht_nsiblings(int cpu)
+{
+	return 1;
+}
+EXPORT_SYMBOL(cfs_cpu_ht_nsiblings);
+
+int
 cfs_cpt_current(struct cfs_cpt_table *cptab, int remap)
 {
 	return 0;
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-cpu.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-cpu.c
index 77b1ef64ecc..fc21210d77e 100644
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-cpu.c
+++ b/drivers/staging/lustre/lustre/libcfs/linux/linux-cpu.c
@@ -71,7 +71,7 @@ struct cfs_cpt_data {
 	/* reserved for hotplug */
 	unsigned long		cpt_version;
 	/* mutex to protect cpt_cpumask */
-	struct semaphore	cpt_mutex;
+	struct mutex		cpt_mutex;
 	/* scratch buffer for set/unset_node */
 	cpumask_t		*cpt_cpumask;
 };
@@ -420,14 +420,14 @@ cfs_cpt_set_node(struct cfs_cpt_table *cptab, int cpt, int node)
 		return 0;
 	}
 
-	down(&cpt_data.cpt_mutex);
+	mutex_lock(&cpt_data.cpt_mutex);
 
 	mask = cpt_data.cpt_cpumask;
 	cfs_node_to_cpumask(node, mask);
 
 	rc = cfs_cpt_set_cpumask(cptab, cpt, mask);
 
-	up(&cpt_data.cpt_mutex);
+	mutex_unlock(&cpt_data.cpt_mutex);
 
 	return rc;
 }
@@ -444,14 +444,14 @@ cfs_cpt_unset_node(struct cfs_cpt_table *cptab, int cpt, int node)
 		return;
 	}
 
-	down(&cpt_data.cpt_mutex);
+	mutex_lock(&cpt_data.cpt_mutex);
 
 	mask = cpt_data.cpt_cpumask;
 	cfs_node_to_cpumask(node, mask);
 
 	cfs_cpt_unset_cpumask(cptab, cpt, mask);
 
-	up(&cpt_data.cpt_mutex);
+	mutex_unlock(&cpt_data.cpt_mutex);
 }
 EXPORT_SYMBOL(cfs_cpt_unset_node);
 
@@ -881,7 +881,7 @@ cfs_cpt_table_create_pattern(char *pattern)
 			break;
 		}
 
-		if (sscanf(str, "%u%n", &cpt, &n) < 1) {
+		if (sscanf(str, "%d%n", &cpt, &n) < 1) {
 			CERROR("Invalid cpu pattern %s\n", str);
 			goto failed;
 		}
@@ -969,11 +969,11 @@ cfs_cpu_notify(struct notifier_block *self, unsigned long action, void *hcpu)
 			break;
 		}
 
-		down(&cpt_data.cpt_mutex);
+		mutex_lock(&cpt_data.cpt_mutex);
 		/* if all HTs in a core are offline, it may break affinity */
 		cfs_cpu_ht_siblings(cpu, cpt_data.cpt_cpumask);
 		warn = any_online_cpu(*cpt_data.cpt_cpumask) >= nr_cpu_ids;
-		up(&cpt_data.cpt_mutex);
+		mutex_unlock(&cpt_data.cpt_mutex);
 		CDEBUG(warn ? D_WARNING : D_INFO,
 		       "Lustre: can't support CPU plug-out well now, "
 		       "performance and stability could be impacted "
@@ -1017,7 +1017,7 @@ cfs_cpu_init(void)
 	}
 
 	spin_lock_init(&cpt_data.cpt_lock);
-	sema_init(&cpt_data.cpt_mutex, 1);
+	mutex_init(&cpt_data.cpt_mutex);
 
 #ifdef CONFIG_HOTPLUG_CPU
 	register_hotcpu_notifier(&cfs_cpu_notifier);
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-curproc.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-curproc.c
index e74c3e28a97..bd301ce0225 100644
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-curproc.c
+++ b/drivers/staging/lustre/lustre/libcfs/linux/linux-curproc.c
@@ -100,158 +100,6 @@ cfs_cap_t cfs_curproc_cap_pack(void)
 	return cap;
 }
 
-static int cfs_access_process_vm(struct task_struct *tsk, unsigned long addr,
-				 void *buf, int len, int write)
-{
-	/* Just copied from kernel for the kernels which doesn't
-	 * have access_process_vm() exported */
-	struct mm_struct *mm;
-	struct vm_area_struct *vma;
-	struct page *page;
-	void *old_buf = buf;
-
-	mm = get_task_mm(tsk);
-	if (!mm)
-		return 0;
-
-	down_read(&mm->mmap_sem);
-	/* ignore errors, just check how much was successfully transferred */
-	while (len) {
-		int bytes, rc, offset;
-		void *maddr;
-
-		rc = get_user_pages(tsk, mm, addr, 1,
-				     write, 1, &page, &vma);
-		if (rc <= 0)
-			break;
-
-		bytes = len;
-		offset = addr & (PAGE_SIZE-1);
-		if (bytes > PAGE_SIZE-offset)
-			bytes = PAGE_SIZE-offset;
-
-		maddr = kmap(page);
-		if (write) {
-			copy_to_user_page(vma, page, addr,
-					  maddr + offset, buf, bytes);
-			set_page_dirty_lock(page);
-		} else {
-			copy_from_user_page(vma, page, addr,
-					    buf, maddr + offset, bytes);
-		}
-		kunmap(page);
-		page_cache_release(page);
-		len -= bytes;
-		buf += bytes;
-		addr += bytes;
-	}
-	up_read(&mm->mmap_sem);
-	mmput(mm);
-
-	return buf - old_buf;
-}
-
-/* Read the environment variable of current process specified by @key. */
-int cfs_get_environ(const char *key, char *value, int *val_len)
-{
-	struct mm_struct *mm;
-	char *buffer, *tmp_buf = NULL;
-	int buf_len = PAGE_CACHE_SIZE;
-	int key_len = strlen(key);
-	unsigned long addr;
-	int rc;
-
-	buffer = kmalloc(buf_len, GFP_USER);
-	if (!buffer)
-		return -ENOMEM;
-
-	mm = get_task_mm(current);
-	if (!mm) {
-		kfree(buffer);
-		return -EINVAL;
-	}
-
-	/* Avoid deadlocks on mmap_sem if called from sys_mmap_pgoff(),
-	 * which is already holding mmap_sem for writes.  If some other
-	 * thread gets the write lock in the meantime, this thread will
-	 * block, but at least it won't deadlock on itself.  LU-1735 */
-	if (down_read_trylock(&mm->mmap_sem) == 0) {
-		kfree(buffer);
-		return -EDEADLK;
-	}
-	up_read(&mm->mmap_sem);
-
-	addr = mm->env_start;
-	while (addr < mm->env_end) {
-		int this_len, retval, scan_len;
-		char *env_start, *env_end;
-
-		memset(buffer, 0, buf_len);
-
-		this_len = min_t(int, mm->env_end - addr, buf_len);
-		retval = cfs_access_process_vm(current, addr, buffer,
-					       this_len, 0);
-		if (retval != this_len)
-			break;
-
-		addr += retval;
-
-		/* Parse the buffer to find out the specified key/value pair.
-		 * The "key=value" entries are separated by '\0'. */
-		env_start = buffer;
-		scan_len = this_len;
-		while (scan_len) {
-			char *entry;
-			int entry_len;
-
-			env_end = memscan(env_start, '\0', scan_len);
-			LASSERT(env_end >= env_start &&
-				env_end <= env_start + scan_len);
-
-			/* The last entry of this buffer cross the buffer
-			 * boundary, reread it in next cycle. */
-			if (unlikely(env_end - env_start == scan_len)) {
-				/* This entry is too large to fit in buffer */
-				if (unlikely(scan_len == this_len)) {
-					CERROR("Too long env variable.\n");
-					GOTO(out, rc = -EINVAL);
-				}
-				addr -= scan_len;
-				break;
-			}
-
-			entry = env_start;
-			entry_len = env_end - env_start;
-
-			/* Key length + length of '=' */
-			if (entry_len > key_len + 1 &&
-			    !memcmp(entry, key, key_len)) {
-				entry += key_len + 1;
-				entry_len -= key_len + 1;
-				/* The 'value' buffer passed in is too small.*/
-				if (entry_len >= *val_len)
-					GOTO(out, rc = -EOVERFLOW);
-
-				memcpy(value, entry, entry_len);
-				*val_len = entry_len;
-				GOTO(out, rc = 0);
-			}
-
-			scan_len -= (env_end - env_start + 1);
-			env_start = env_end + 1;
-		}
-	}
-	GOTO(out, rc = -ENOENT);
-
-out:
-	mmput(mm);
-	kfree((void *)buffer);
-	if (tmp_buf)
-		kfree((void *)tmp_buf);
-	return rc;
-}
-EXPORT_SYMBOL(cfs_get_environ);
-
 EXPORT_SYMBOL(cfs_cap_raise);
 EXPORT_SYMBOL(cfs_cap_lower);
 EXPORT_SYMBOL(cfs_cap_raised);
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-module.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-module.c
index e6eae0666f0..581b4728c6c 100644
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-module.c
+++ b/drivers/staging/lustre/lustre/libcfs/linux/linux-module.c
@@ -44,14 +44,13 @@ int libcfs_ioctl_getdata(char *buf, char *end, void *arg)
 {
 	struct libcfs_ioctl_hdr   *hdr;
 	struct libcfs_ioctl_data  *data;
-	int err;
+	int orig_len;
 
 	hdr = (struct libcfs_ioctl_hdr *)buf;
 	data = (struct libcfs_ioctl_data *)buf;
 
-	err = copy_from_user(buf, (void *)arg, sizeof(*hdr));
-	if (err)
-		return err;
+	if (copy_from_user(buf, (void *)arg, sizeof(*hdr)))
+		return -EFAULT;
 
 	if (hdr->ioc_version != LIBCFS_IOCTL_VERSION) {
 		CERROR("PORTALS: version mismatch kernel vs application\n");
@@ -69,9 +68,11 @@ int libcfs_ioctl_getdata(char *buf, char *end, void *arg)
 		return -EINVAL;
 	}
 
-	err = copy_from_user(buf, (void *)arg, hdr->ioc_len);
-	if (err)
-		return err;
+	orig_len = hdr->ioc_len;
+	if (copy_from_user(buf, (void *)arg, hdr->ioc_len))
+		return -EFAULT;
+	if (orig_len != data->ioc_len)
+		return -EINVAL;
 
 	if (libcfs_ioctl_is_invalid(data)) {
 		CERROR("PORTALS: ioctl not correctly formatted\n");
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-tcpip.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-tcpip.c
index 7539fe16d76..ac3a444a918 100644
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-tcpip.c
+++ b/drivers/staging/lustre/lustre/libcfs/linux/linux-tcpip.c
@@ -46,31 +46,16 @@
 int
 libcfs_sock_ioctl(int cmd, unsigned long arg)
 {
-	mm_segment_t    oldmm = get_fs();
 	struct socket  *sock;
 	int	     rc;
-	struct file    *sock_filp;
 
 	rc = sock_create (PF_INET, SOCK_STREAM, 0, &sock);
 	if (rc != 0) {
 		CERROR ("Can't create socket: %d\n", rc);
 		return rc;
 	}
-
-	sock_filp = sock_alloc_file(sock, 0, NULL);
-	if (IS_ERR(sock_filp)) {
-		sock_release(sock);
-		rc = PTR_ERR(sock_filp);
-		goto out;
-	}
-
-	set_fs(KERNEL_DS);
-	if (sock_filp->f_op->unlocked_ioctl)
-		rc = sock_filp->f_op->unlocked_ioctl(sock_filp, cmd, arg);
-	set_fs(oldmm);
-
-	fput(sock_filp);
-out:
+	rc = kernel_sock_ioctl(sock, cmd, arg);
+	sock_release(sock);
 	return rc;
 }
 
@@ -255,7 +240,6 @@ int
 libcfs_sock_write (struct socket *sock, void *buffer, int nob, int timeout)
 {
 	int	    rc;
-	mm_segment_t   oldmm = get_fs();
 	long	   ticks = timeout * HZ;
 	unsigned long  then;
 	struct timeval tv;
@@ -279,10 +263,8 @@ libcfs_sock_write (struct socket *sock, void *buffer, int nob, int timeout)
 				.tv_sec = ticks / HZ,
 				.tv_usec = ((ticks % HZ) * 1000000) / HZ
 			};
-			set_fs(KERNEL_DS);
-			rc = sock_setsockopt(sock, SOL_SOCKET, SO_SNDTIMEO,
+			rc = kernel_setsockopt(sock, SOL_SOCKET, SO_SNDTIMEO,
 					     (char *)&tv, sizeof(tv));
-			set_fs(oldmm);
 			if (rc != 0) {
 				CERROR("Can't set socket send timeout "
 				       "%ld.%06d: %d\n",
@@ -321,7 +303,6 @@ int
 libcfs_sock_read (struct socket *sock, void *buffer, int nob, int timeout)
 {
 	int	    rc;
-	mm_segment_t   oldmm = get_fs();
 	long	   ticks = timeout * HZ;
 	unsigned long  then;
 	struct timeval tv;
@@ -343,10 +324,8 @@ libcfs_sock_read (struct socket *sock, void *buffer, int nob, int timeout)
 			.tv_sec = ticks / HZ,
 			.tv_usec = ((ticks % HZ) * 1000000) / HZ
 		};
-		set_fs(KERNEL_DS);
-		rc = sock_setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO,
+		rc = kernel_setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO,
 				     (char *)&tv, sizeof(tv));
-		set_fs(oldmm);
 		if (rc != 0) {
 			CERROR("Can't set socket recv timeout %ld.%06d: %d\n",
 			       (long)tv.tv_sec, (int)tv.tv_usec, rc);
@@ -384,7 +363,6 @@ libcfs_sock_create (struct socket **sockp, int *fatal,
 	struct socket      *sock;
 	int		 rc;
 	int		 option;
-	mm_segment_t	oldmm = get_fs();
 
 	/* All errors are fatal except bind failure if the port is in use */
 	*fatal = 1;
@@ -396,11 +374,9 @@ libcfs_sock_create (struct socket **sockp, int *fatal,
 		return (rc);
 	}
 
-	set_fs (KERNEL_DS);
 	option = 1;
-	rc = sock_setsockopt(sock, SOL_SOCKET, SO_REUSEADDR,
+	rc = kernel_setsockopt(sock, SOL_SOCKET, SO_REUSEADDR,
 			     (char *)&option, sizeof (option));
-	set_fs (oldmm);
 	if (rc != 0) {
 		CERROR("Can't set SO_REUSEADDR for socket: %d\n", rc);
 		goto failed;
@@ -437,16 +413,13 @@ libcfs_sock_create (struct socket **sockp, int *fatal,
 int
 libcfs_sock_setbuf (struct socket *sock, int txbufsize, int rxbufsize)
 {
-	mm_segment_t	oldmm = get_fs();
 	int		 option;
 	int		 rc;
 
 	if (txbufsize != 0) {
 		option = txbufsize;
-		set_fs (KERNEL_DS);
-		rc = sock_setsockopt(sock, SOL_SOCKET, SO_SNDBUF,
+		rc = kernel_setsockopt(sock, SOL_SOCKET, SO_SNDBUF,
 				     (char *)&option, sizeof (option));
-		set_fs (oldmm);
 		if (rc != 0) {
 			CERROR ("Can't set send buffer %d: %d\n",
 				option, rc);
@@ -456,10 +429,8 @@ libcfs_sock_setbuf (struct socket *sock, int txbufsize, int rxbufsize)
 
 	if (rxbufsize != 0) {
 		option = rxbufsize;
-		set_fs (KERNEL_DS);
-		rc = sock_setsockopt (sock, SOL_SOCKET, SO_RCVBUF,
+		rc = kernel_setsockopt(sock, SOL_SOCKET, SO_RCVBUF,
 				      (char *)&option, sizeof (option));
-		set_fs (oldmm);
 		if (rc != 0) {
 			CERROR ("Can't set receive buffer %d: %d\n",
 				option, rc);
diff --git a/drivers/staging/lustre/lustre/libcfs/module.c b/drivers/staging/lustre/lustre/libcfs/module.c
index 24ae26d5def..b16ee08fe74 100644
--- a/drivers/staging/lustre/lustre/libcfs/module.c
+++ b/drivers/staging/lustre/lustre/libcfs/module.c
@@ -87,7 +87,8 @@ kportal_memhog_free (struct libcfs_device_userstate *ldu)
 }
 
 int
-kportal_memhog_alloc (struct libcfs_device_userstate *ldu, int npages, int flags)
+kportal_memhog_alloc(struct libcfs_device_userstate *ldu, int npages,
+		     gfp_t flags)
 {
 	struct page **level0p;
 	struct page **level1p;
diff --git a/drivers/staging/lustre/lustre/libcfs/nidstrings.c b/drivers/staging/lustre/lustre/libcfs/nidstrings.c
index cfb274fc9cb..87705ae4480 100644
--- a/drivers/staging/lustre/lustre/libcfs/nidstrings.c
+++ b/drivers/staging/lustre/lustre/libcfs/nidstrings.c
@@ -227,11 +227,11 @@ libcfs_ip_addr2str(__u32 addr, char *str)
 int
 libcfs_ip_str2addr(const char *str, int nob, __u32 *addr)
 {
-	int   a;
-	int   b;
-	int   c;
-	int   d;
-	int   n = nob;			  /* XscanfX */
+	unsigned int	a;
+	unsigned int	b;
+	unsigned int	c;
+	unsigned int	d;
+	int		n = nob; /* XscanfX */
 
 	/* numeric IP? */
 	if (sscanf(str, "%u.%u.%u.%u%n", &a, &b, &c, &d, &n) >= 4 &&
@@ -419,7 +419,7 @@ libcfs_str2net_internal(const char *str, __u32 *net)
 {
 	struct netstrfns *uninitialized_var(nf);
 	int	       nob;
-	int	       netnum;
+	unsigned int   netnum;
 	int	       i;
 
 	for (i = 0; i < libcfs_nnetstrfns; i++) {
diff --git a/drivers/staging/lustre/lustre/libcfs/tracefile.c b/drivers/staging/lustre/lustre/libcfs/tracefile.c
index c8599eefeb7..07845e84424 100644
--- a/drivers/staging/lustre/lustre/libcfs/tracefile.c
+++ b/drivers/staging/lustre/lustre/libcfs/tracefile.c
@@ -66,7 +66,7 @@ cfs_tage_from_list(struct list_head *list)
 	return list_entry(list, struct cfs_trace_page, linkage);
 }
 
-static struct cfs_trace_page *cfs_tage_alloc(int gfp)
+static struct cfs_trace_page *cfs_tage_alloc(gfp_t gfp)
 {
 	struct page	    *page;
 	struct cfs_trace_page *tage;
@@ -114,7 +114,7 @@ static void cfs_tage_to_tail(struct cfs_trace_page *tage,
 	list_move_tail(&tage->linkage, queue);
 }
 
-int cfs_trace_refill_stock(struct cfs_trace_cpu_data *tcd, int gfp,
+int cfs_trace_refill_stock(struct cfs_trace_cpu_data *tcd, gfp_t gfp,
 			   struct list_head *stock)
 {
 	int i;
@@ -416,13 +416,13 @@ console:
 			cdls->cdls_delay /= libcfs_console_backoff * 4;
 		} else {
 			cdls->cdls_delay *= libcfs_console_backoff;
-
-			if (cdls->cdls_delay < libcfs_console_min_delay)
-				cdls->cdls_delay = libcfs_console_min_delay;
-			else if (cdls->cdls_delay > libcfs_console_max_delay)
-				cdls->cdls_delay = libcfs_console_max_delay;
 		}
 
+		if (cdls->cdls_delay < libcfs_console_min_delay)
+			cdls->cdls_delay = libcfs_console_min_delay;
+		else if (cdls->cdls_delay > libcfs_console_max_delay)
+			cdls->cdls_delay = libcfs_console_max_delay;
+
 		/* ensure cdls_next is never zero after it's been seen */
 		cdls->cdls_next = (cfs_time_current() + cdls->cdls_delay) | 1;
 	}
diff --git a/drivers/staging/lustre/lustre/libcfs/tracefile.h b/drivers/staging/lustre/lustre/libcfs/tracefile.h
index 7e8d17c12b5..55ecfc9f201 100644
--- a/drivers/staging/lustre/lustre/libcfs/tracefile.h
+++ b/drivers/staging/lustre/lustre/libcfs/tracefile.h
@@ -307,7 +307,7 @@ cfs_trace_put_tcd (struct cfs_trace_cpu_data *tcd)
 	put_cpu();
 }
 
-int cfs_trace_refill_stock(struct cfs_trace_cpu_data *tcd, int gfp,
+int cfs_trace_refill_stock(struct cfs_trace_cpu_data *tcd, gfp_t gfp,
 			   struct list_head *stock);
 
 
diff --git a/drivers/staging/lustre/lustre/libcfs/workitem.c b/drivers/staging/lustre/lustre/libcfs/workitem.c
index ba16fd5db70..0a03bf7ba3e 100644
--- a/drivers/staging/lustre/lustre/libcfs/workitem.c
+++ b/drivers/staging/lustre/lustre/libcfs/workitem.c
@@ -75,7 +75,7 @@ typedef struct cfs_wi_sched {
 	char			ws_name[CFS_WS_NAME_LEN];
 } cfs_wi_sched_t;
 
-struct cfs_workitem_data {
+static struct cfs_workitem_data {
 	/** serialize */
 	spinlock_t		wi_glock;
 	/** list of all schedulers */
diff --git a/drivers/staging/lustre/lustre/llite/dcache.c b/drivers/staging/lustre/lustre/llite/dcache.c
index 8b550808617..7d520d8f4a6 100644
--- a/drivers/staging/lustre/lustre/llite/dcache.c
+++ b/drivers/staging/lustre/lustre/llite/dcache.c
@@ -69,8 +69,7 @@ static void ll_release(struct dentry *de)
 		ll_intent_release(lld->lld_it);
 		OBD_FREE(lld->lld_it, sizeof(*lld->lld_it));
 	}
-	LASSERT(lld->lld_cwd_count == 0);
-	LASSERT(lld->lld_mnt_count == 0);
+
 	de->d_fsdata = NULL;
 	call_rcu(&lld->lld_rcu_head, free_dentry_data);
 }
@@ -82,8 +81,9 @@ static void ll_release(struct dentry *de)
  * an AST before calling d_revalidate_it().  The dentry still exists (marked
  * INVALID) so d_lookup() matches it, but we have no lock on it (so
  * lock_match() fails) and we spin around real_lookup(). */
-int ll_dcompare(const struct dentry *parent, const struct dentry *dentry,
-		unsigned int len, const char *str, const struct qstr *name)
+static int ll_dcompare(const struct dentry *parent, const struct dentry *dentry,
+		       unsigned int len, const char *str,
+		       const struct qstr *name)
 {
 	if (len != name->len)
 		return 1;
@@ -238,7 +238,8 @@ void ll_intent_release(struct lookup_intent *it)
 	ll_intent_drop_lock(it);
 	/* We are still holding extra reference on a request, need to free it */
 	if (it_disposition(it, DISP_ENQ_OPEN_REF))
-		 ptlrpc_req_finished(it->d.lustre.it_data); /* ll_file_open */
+		ptlrpc_req_finished(it->d.lustre.it_data); /* ll_file_open */
+
 	if (it_disposition(it, DISP_ENQ_CREATE_REF)) /* create rec */
 		ptlrpc_req_finished(it->d.lustre.it_data);
 
@@ -316,15 +317,6 @@ void ll_lookup_finish_locks(struct lookup_intent *it, struct dentry *dentry)
 	}
 }
 
-void ll_frob_intent(struct lookup_intent **itp, struct lookup_intent *deft)
-{
-	struct lookup_intent *it = *itp;
-
-	if (!it || it->it_op == IT_GETXATTR)
-		it = *itp = deft;
-
-}
-
 static int ll_revalidate_dentry(struct dentry *dentry,
 				unsigned int lookup_flags)
 {
@@ -356,7 +348,7 @@ static int ll_revalidate_dentry(struct dentry *dentry,
 /*
  * Always trust cached dentries. Update statahead window if necessary.
  */
-int ll_revalidate_nd(struct dentry *dentry, unsigned int flags)
+static int ll_revalidate_nd(struct dentry *dentry, unsigned int flags)
 {
 	int rc;
 
@@ -368,7 +360,7 @@ int ll_revalidate_nd(struct dentry *dentry, unsigned int flags)
 }
 
 
-void ll_d_iput(struct dentry *de, struct inode *inode)
+static void ll_d_iput(struct dentry *de, struct inode *inode)
 {
 	LASSERT(inode);
 	if (!find_cbdata(inode))
@@ -376,7 +368,7 @@ void ll_d_iput(struct dentry *de, struct inode *inode)
 	iput(inode);
 }
 
-struct dentry_operations ll_d_ops = {
+const struct dentry_operations ll_d_ops = {
 	.d_revalidate = ll_revalidate_nd,
 	.d_release = ll_release,
 	.d_delete  = ll_ddelete,
diff --git a/drivers/staging/lustre/lustre/llite/dir.c b/drivers/staging/lustre/lustre/llite/dir.c
index 7fbc18e3e65..ae6f61aa4da 100644
--- a/drivers/staging/lustre/lustre/llite/dir.c
+++ b/drivers/staging/lustre/lustre/llite/dir.c
@@ -632,7 +632,7 @@ out:
 	return rc;
 }
 
-int ll_send_mgc_param(struct obd_export *mgc, char *string)
+static int ll_send_mgc_param(struct obd_export *mgc, char *string)
 {
 	struct mgs_send_param *msp;
 	int rc = 0;
@@ -795,7 +795,7 @@ int ll_dir_getstripe(struct inode *inode, struct lov_mds_md **lmmp,
 	int rc, lmmsize;
 	struct md_op_data *op_data;
 
-	rc = ll_get_max_mdsize(sbi, &lmmsize);
+	rc = ll_get_default_mdsize(sbi, &lmmsize);
 	if (rc)
 		return rc;
 
@@ -1804,6 +1804,11 @@ out_rmdir:
 		/* Compute the whole struct size */
 		totalsize = hur_len(hur);
 		OBD_FREE_PTR(hur);
+
+		/* Final size will be more than double totalsize */
+		if (totalsize >= MDS_MAXREQSIZE / 3)
+			return -E2BIG;
+
 		OBD_ALLOC_LARGE(hur, totalsize);
 		if (hur == NULL)
 			return -ENOMEM;
@@ -1959,17 +1964,17 @@ out:
 	return ret;
 }
 
-int ll_dir_open(struct inode *inode, struct file *file)
+static int ll_dir_open(struct inode *inode, struct file *file)
 {
 	return ll_file_open(inode, file);
 }
 
-int ll_dir_release(struct inode *inode, struct file *file)
+static int ll_dir_release(struct inode *inode, struct file *file)
 {
 	return ll_file_release(inode, file);
 }
 
-struct file_operations ll_dir_operations = {
+const struct file_operations ll_dir_operations = {
 	.llseek   = ll_dir_seek,
 	.open     = ll_dir_open,
 	.release  = ll_dir_release,
diff --git a/drivers/staging/lustre/lustre/llite/file.c b/drivers/staging/lustre/lustre/llite/file.c
index 8e844a6371e..d87f96d9e0f 100644
--- a/drivers/staging/lustre/lustre/llite/file.c
+++ b/drivers/staging/lustre/lustre/llite/file.c
@@ -50,11 +50,21 @@
 
 #include "cl_object.h"
 
-struct ll_file_data *ll_file_data_get(void)
+static int
+ll_put_grouplock(struct inode *inode, struct file *file, unsigned long arg);
+
+static int ll_lease_close(struct obd_client_handle *och, struct inode *inode,
+			  bool *lease_broken);
+
+static enum llioc_iter
+ll_iocontrol_call(struct inode *inode, struct file *file,
+		  unsigned int cmd, unsigned long arg, int *rcp);
+
+static struct ll_file_data *ll_file_data_get(void)
 {
 	struct ll_file_data *fd;
 
-	OBD_SLAB_ALLOC_PTR_GFP(fd, ll_file_data_slab, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(fd, ll_file_data_slab, GFP_NOFS);
 	if (fd == NULL)
 		return NULL;
 	fd->fd_write_failed = false;
@@ -247,8 +257,8 @@ int ll_md_real_close(struct inode *inode, fmode_t fmode)
 	return rc;
 }
 
-int ll_md_close(struct obd_export *md_exp, struct inode *inode,
-		struct file *file)
+static int ll_md_close(struct obd_export *md_exp, struct inode *inode,
+		       struct file *file)
 {
 	struct ll_file_data *fd = LUSTRE_FPRIVATE(file);
 	struct ll_inode_info *lli = ll_i2info(inode);
@@ -482,8 +492,8 @@ static int ll_och_fill(struct obd_export *md_exp, struct lookup_intent *it,
 	return md_set_open_replay_data(md_exp, och, it);
 }
 
-int ll_local_open(struct file *file, struct lookup_intent *it,
-		  struct ll_file_data *fd, struct obd_client_handle *och)
+static int ll_local_open(struct file *file, struct lookup_intent *it,
+			 struct ll_file_data *fd, struct obd_client_handle *och)
 {
 	struct inode *inode = file->f_dentry->d_inode;
 	struct ll_inode_info *lli = ll_i2info(inode);
@@ -733,8 +743,9 @@ static int ll_md_blocking_lease_ast(struct ldlm_lock *lock,
 /**
  * Acquire a lease and open the file.
  */
-struct obd_client_handle *ll_lease_open(struct inode *inode, struct file *file,
-					fmode_t fmode, __u64 open_flags)
+static struct obd_client_handle *
+ll_lease_open(struct inode *inode, struct file *file, fmode_t fmode,
+	      __u64 open_flags)
 {
 	struct lookup_intent it = { .it_op = IT_OPEN };
 	struct ll_sb_info *sbi = ll_i2sbi(inode);
@@ -862,14 +873,13 @@ out:
 	OBD_FREE_PTR(och);
 	return ERR_PTR(rc);
 }
-EXPORT_SYMBOL(ll_lease_open);
 
 /**
  * Release lease and close the file.
  * It will check if the lease has ever broken.
  */
-int ll_lease_close(struct obd_client_handle *och, struct inode *inode,
-			bool *lease_broken)
+static int ll_lease_close(struct obd_client_handle *och, struct inode *inode,
+			  bool *lease_broken)
 {
 	struct ldlm_lock *lock;
 	bool cancelled = true;
@@ -895,7 +905,6 @@ int ll_lease_close(struct obd_client_handle *och, struct inode *inode,
 				       NULL);
 	return rc;
 }
-EXPORT_SYMBOL(ll_lease_close);
 
 /* Fills the obdo with the attributes for the lsm */
 static int ll_lsm_getattr(struct lov_stripe_md *lsm, struct obd_export *exp,
@@ -1440,7 +1449,7 @@ int ll_lov_getstripe_ea_info(struct inode *inode, const char *filename,
 	struct md_op_data *op_data;
 	int rc, lmmsize;
 
-	rc = ll_get_max_mdsize(sbi, &lmmsize);
+	rc = ll_get_default_mdsize(sbi, &lmmsize);
 	if (rc)
 		return rc;
 
@@ -1590,7 +1599,8 @@ static int ll_lov_getstripe(struct inode *inode, unsigned long arg)
 	return rc;
 }
 
-int ll_get_grouplock(struct inode *inode, struct file *file, unsigned long arg)
+static int
+ll_get_grouplock(struct inode *inode, struct file *file, unsigned long arg)
 {
 	struct ll_inode_info   *lli = ll_i2info(inode);
 	struct ll_file_data    *fd = LUSTRE_FPRIVATE(file);
@@ -1710,13 +1720,13 @@ out:
  * Get size for inode for which FIEMAP mapping is requested.
  * Make the FIEMAP get_info call and returns the result.
  */
-int ll_do_fiemap(struct inode *inode, struct ll_user_fiemap *fiemap,
-	      int num_bytes)
+static int ll_do_fiemap(struct inode *inode, struct ll_user_fiemap *fiemap,
+			size_t num_bytes)
 {
 	struct obd_export *exp = ll_i2dtexp(inode);
 	struct lov_stripe_md *lsm = NULL;
 	struct ll_fiemap_info_key fm_key = { .name = KEY_FIEMAP, };
-	int vallen = num_bytes;
+	__u32 vallen = num_bytes;
 	int rc;
 
 	/* Checks for fiemap flags */
@@ -1819,6 +1829,10 @@ static int ll_ioctl_fiemap(struct inode *inode, unsigned long arg)
 	if (get_user(extent_count,
 	    &((struct ll_user_fiemap __user *)arg)->fm_extent_count))
 		return -EFAULT;
+
+	if (extent_count >=
+	    (SIZE_MAX - sizeof(*fiemap_s)) / sizeof(struct ll_fiemap_extent))
+		return -EINVAL;
 	num_bytes = sizeof(*fiemap_s) + (extent_count *
 					 sizeof(struct ll_fiemap_extent));
 
@@ -2190,7 +2204,8 @@ out:
 	return rc;
 }
 
-long ll_file_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+static long
+ll_file_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 {
 	struct inode		*inode = file->f_dentry->d_inode;
 	struct ll_file_data	*fd = LUSTRE_FPRIVATE(file);
@@ -2509,7 +2524,7 @@ long ll_file_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 }
 
 
-loff_t ll_file_seek(struct file *file, loff_t offset, int origin)
+static loff_t ll_file_seek(struct file *file, loff_t offset, int origin)
 {
 	struct inode *inode = file->f_dentry->d_inode;
 	loff_t retval, eof = 0;
@@ -2533,7 +2548,7 @@ loff_t ll_file_seek(struct file *file, loff_t offset, int origin)
 	return retval;
 }
 
-int ll_flush(struct file *file, fl_owner_t id)
+static int ll_flush(struct file *file, fl_owner_t id)
 {
 	struct inode *inode = file->f_dentry->d_inode;
 	struct ll_inode_info *lli = ll_i2info(inode);
@@ -2559,7 +2574,7 @@ int ll_flush(struct file *file, fl_owner_t id)
 
 /**
  * Called to make sure a portion of file has been written out.
- * if @local_only is not true, it will send OST_SYNC RPCs to ost.
+ * if @mode is not CL_FSYNC_LOCAL, it will send OST_SYNC RPCs to OST.
  *
  * Return how many pages have been written.
  */
@@ -2653,11 +2668,10 @@ int ll_fsync(struct file *file, loff_t start, loff_t end, int datasync)
 	if (!err)
 		ptlrpc_req_finished(req);
 
-	if (datasync && S_ISREG(inode->i_mode)) {
+	if (S_ISREG(inode->i_mode)) {
 		struct ll_file_data *fd = LUSTRE_FPRIVATE(file);
 
-		err = cl_sync_file_range(inode, 0, OBD_OBJECT_EOF,
-				CL_FSYNC_ALL, 0);
+		err = cl_sync_file_range(inode, start, end, CL_FSYNC_ALL, 0);
 		if (rc == 0 && err < 0)
 			rc = err;
 		if (rc < 0)
@@ -2670,7 +2684,8 @@ int ll_fsync(struct file *file, loff_t start, loff_t end, int datasync)
 	return rc;
 }
 
-int ll_file_flock(struct file *file, int cmd, struct file_lock *file_lock)
+static int
+ll_file_flock(struct file *file, int cmd, struct file_lock *file_lock)
 {
 	struct inode *inode = file->f_dentry->d_inode;
 	struct ll_sb_info *sbi = ll_i2sbi(inode);
@@ -2799,7 +2814,8 @@ int ll_file_flock(struct file *file, int cmd, struct file_lock *file_lock)
 	return rc;
 }
 
-int ll_file_noflock(struct file *file, int cmd, struct file_lock *file_lock)
+static int
+ll_file_noflock(struct file *file, int cmd, struct file_lock *file_lock)
 {
 	return -ENOSYS;
 }
@@ -2882,16 +2898,16 @@ static int ll_inode_revalidate_fini(struct inode *inode, int rc)
 		if (!S_ISREG(inode->i_mode) && !S_ISDIR(inode->i_mode))
 			return 0;
 	} else if (rc != 0) {
-		CERROR("%s: revalidate FID "DFID" error: rc = %d\n",
-		       ll_get_fsname(inode->i_sb, NULL, 0),
-		       PFID(ll_inode2fid(inode)), rc);
+		CDEBUG_LIMIT((rc == -EACCES || rc == -EIDRM) ? D_INFO : D_ERROR,
+			     "%s: revalidate FID "DFID" error: rc = %d\n",
+			     ll_get_fsname(inode->i_sb, NULL, 0),
+			     PFID(ll_inode2fid(inode)), rc);
 	}
 
 	return rc;
 }
 
-int __ll_inode_revalidate_it(struct dentry *dentry, struct lookup_intent *it,
-			     __u64 ibits)
+static int __ll_inode_revalidate(struct dentry *dentry, __u64 ibits)
 {
 	struct inode *inode = dentry->d_inode;
 	struct ptlrpc_request *req = NULL;
@@ -2956,7 +2972,7 @@ int __ll_inode_revalidate_it(struct dentry *dentry, struct lookup_intent *it,
 		int ealen = 0;
 
 		if (S_ISREG(inode->i_mode)) {
-			rc = ll_get_max_mdsize(sbi, &ealen);
+			rc = ll_get_default_mdsize(sbi, &ealen);
 			if (rc)
 				return rc;
 			valid |= OBD_MD_FLEASIZE | OBD_MD_FLMODEASIZE;
@@ -2986,13 +3002,12 @@ out:
 	return rc;
 }
 
-int ll_inode_revalidate_it(struct dentry *dentry, struct lookup_intent *it,
-			   __u64 ibits)
+static int ll_inode_revalidate(struct dentry *dentry, __u64 ibits)
 {
 	struct inode *inode = dentry->d_inode;
 	int rc;
 
-	rc = __ll_inode_revalidate_it(dentry, it, ibits);
+	rc = __ll_inode_revalidate(dentry, ibits);
 	if (rc != 0)
 		return rc;
 
@@ -3015,16 +3030,15 @@ int ll_inode_revalidate_it(struct dentry *dentry, struct lookup_intent *it,
 	return rc;
 }
 
-int ll_getattr_it(struct vfsmount *mnt, struct dentry *de,
-		  struct lookup_intent *it, struct kstat *stat)
+int ll_getattr(struct vfsmount *mnt, struct dentry *de, struct kstat *stat)
 {
 	struct inode *inode = de->d_inode;
 	struct ll_sb_info *sbi = ll_i2sbi(inode);
 	struct ll_inode_info *lli = ll_i2info(inode);
 	int res = 0;
 
-	res = ll_inode_revalidate_it(de, it, MDS_INODELOCK_UPDATE |
-					     MDS_INODELOCK_LOOKUP);
+	res = ll_inode_revalidate(de, MDS_INODELOCK_UPDATE |
+				      MDS_INODELOCK_LOOKUP);
 	ll_stats_ops_tally(sbi, LPROC_LL_GETATTR, 1);
 
 	if (res)
@@ -3050,15 +3064,9 @@ int ll_getattr_it(struct vfsmount *mnt, struct dentry *de,
 
 	return 0;
 }
-int ll_getattr(struct vfsmount *mnt, struct dentry *de, struct kstat *stat)
-{
-	struct lookup_intent it = { .it_op = IT_GETATTR };
-
-	return ll_getattr_it(mnt, de, &it, stat);
-}
 
-int ll_fiemap(struct inode *inode, struct fiemap_extent_info *fieinfo,
-		__u64 start, __u64 len)
+static int ll_fiemap(struct inode *inode, struct fiemap_extent_info *fieinfo,
+		     __u64 start, __u64 len)
 {
 	int rc;
 	size_t num_bytes;
@@ -3076,21 +3084,24 @@ int ll_fiemap(struct inode *inode, struct fiemap_extent_info *fieinfo,
 	fiemap->fm_extent_count = fieinfo->fi_extents_max;
 	fiemap->fm_start = start;
 	fiemap->fm_length = len;
-	memcpy(&fiemap->fm_extents[0], fieinfo->fi_extents_start,
-	       sizeof(struct ll_fiemap_extent));
+	if (extent_count > 0)
+		memcpy(&fiemap->fm_extents[0], fieinfo->fi_extents_start,
+		       sizeof(struct ll_fiemap_extent));
 
 	rc = ll_do_fiemap(inode, fiemap, num_bytes);
 
 	fieinfo->fi_flags = fiemap->fm_flags;
 	fieinfo->fi_extents_mapped = fiemap->fm_mapped_extents;
-	memcpy(fieinfo->fi_extents_start, &fiemap->fm_extents[0],
-	       fiemap->fm_mapped_extents * sizeof(struct ll_fiemap_extent));
+	if (extent_count > 0)
+		memcpy(fieinfo->fi_extents_start, &fiemap->fm_extents[0],
+		       fiemap->fm_mapped_extents *
+		       sizeof(struct ll_fiemap_extent));
 
 	OBD_FREE_LARGE(fiemap, num_bytes);
 	return rc;
 }
 
-struct posix_acl * ll_get_acl(struct inode *inode, int type)
+struct posix_acl *ll_get_acl(struct inode *inode, int type)
 {
 	struct ll_inode_info *lli = ll_i2info(inode);
 	struct posix_acl *acl = NULL;
@@ -3117,10 +3128,8 @@ int ll_inode_permission(struct inode *inode, int mask)
 	* need to do it before permission check. */
 
 	if (inode == inode->i_sb->s_root->d_inode) {
-		struct lookup_intent it = { .it_op = IT_LOOKUP };
-
-		rc = __ll_inode_revalidate_it(inode->i_sb->s_root, &it,
-					      MDS_INODELOCK_LOOKUP);
+		rc = __ll_inode_revalidate(inode->i_sb->s_root,
+					   MDS_INODELOCK_LOOKUP);
 		if (rc)
 			return rc;
 	}
@@ -3272,8 +3281,9 @@ void ll_iocontrol_unregister(void *magic)
 EXPORT_SYMBOL(ll_iocontrol_register);
 EXPORT_SYMBOL(ll_iocontrol_unregister);
 
-enum llioc_iter ll_iocontrol_call(struct inode *inode, struct file *file,
-			unsigned int cmd, unsigned long arg, int *rcp)
+static enum llioc_iter
+ll_iocontrol_call(struct inode *inode, struct file *file,
+		  unsigned int cmd, unsigned long arg, int *rcp)
 {
 	enum llioc_iter ret = LLIOC_CONT;
 	struct llioc_data *data;
@@ -3358,7 +3368,7 @@ static int ll_layout_fetch(struct inode *inode, struct ldlm_lock *lock)
 	 * layout here. Please note that we can't use the LVB buffer in
 	 * completion AST because it doesn't have a large enough buffer */
 	oc = ll_mdscapa_get(inode);
-	rc = ll_get_max_mdsize(sbi, &lmmsize);
+	rc = ll_get_default_mdsize(sbi, &lmmsize);
 	if (rc == 0)
 		rc = md_getxattr(sbi->ll_md_exp, ll_inode2fid(inode), oc,
 				OBD_MD_FLXATTR, XATTR_NAME_LOV, NULL, 0,
@@ -3368,7 +3378,7 @@ static int ll_layout_fetch(struct inode *inode, struct ldlm_lock *lock)
 		return rc;
 
 	body = req_capsule_server_get(&req->rq_pill, &RMF_MDT_BODY);
-	if (body == NULL || body->eadatasize > lmmsize)
+	if (body == NULL)
 		GOTO(out, rc = -EPROTO);
 
 	lmmsize = body->eadatasize;
@@ -3435,7 +3445,7 @@ static int ll_layout_lock_set(struct lustre_handle *lockh, ldlm_mode_t mode,
 		if (lvb_ready) {
 			/* layout_gen must be valid if layout lock is not
 			 * cancelled and stripe has already set */
-			*gen = lli->lli_layout_gen;
+			*gen = ll_layout_version_get(lli);
 			rc = 0;
 		}
 		GOTO(out, rc);
@@ -3533,32 +3543,20 @@ int ll_layout_refresh(struct inode *inode, __u32 *gen)
 	};
 	int rc;
 
-	*gen = lli->lli_layout_gen;
-	if (!(sbi->ll_flags & LL_SBI_LAYOUT_LOCK))
+	*gen = ll_layout_version_get(lli);
+	if (!(sbi->ll_flags & LL_SBI_LAYOUT_LOCK) || *gen != LL_LAYOUT_GEN_NONE)
 		return 0;
 
 	/* sanity checks */
 	LASSERT(fid_is_sane(ll_inode2fid(inode)));
 	LASSERT(S_ISREG(inode->i_mode));
 
-	/* mostly layout lock is caching on the local side, so try to match
-	 * it before grabbing layout lock mutex. */
-	mode = ll_take_md_lock(inode, MDS_INODELOCK_LAYOUT, &lockh, 0,
-			       LCK_CR | LCK_CW | LCK_PR | LCK_PW);
-	if (mode != 0) { /* hit cached lock */
-		rc = ll_layout_lock_set(&lockh, mode, inode, gen, false);
-		if (rc == 0)
-			return 0;
-
-		/* better hold lli_layout_mutex to try again otherwise
-		 * it will have starvation problem. */
-	}
-
 	/* take layout lock mutex to enqueue layout lock exclusively. */
 	mutex_lock(&lli->lli_layout_mutex);
 
 again:
-	/* try again. Maybe somebody else has done this. */
+	/* mostly layout lock is caching on the local side, so try to match
+	 * it before grabbing layout lock mutex. */
 	mode = ll_take_md_lock(inode, MDS_INODELOCK_LAYOUT, &lockh, 0,
 			       LCK_CR | LCK_CW | LCK_PR | LCK_PW);
 	if (mode != 0) { /* hit cached lock */
diff --git a/drivers/staging/lustre/lustre/llite/llite_capa.c b/drivers/staging/lustre/lustre/llite/llite_capa.c
index edd512b20ee..d06d0b1ab08 100644
--- a/drivers/staging/lustre/lustre/llite/llite_capa.c
+++ b/drivers/staging/lustre/lustre/llite/llite_capa.c
@@ -68,6 +68,8 @@ static unsigned long long ll_capa_renewal_noent = 0;
 static unsigned long long ll_capa_renewal_failed = 0;
 static unsigned long long ll_capa_renewal_retries = 0;
 
+static int ll_update_capa(struct obd_capa *ocapa, struct lustre_capa *capa);
+
 static inline void update_capa_timer(struct obd_capa *ocapa, cfs_time_t expiry)
 {
 	if (cfs_time_before(expiry, ll_capa_timer.expires) ||
@@ -515,7 +517,7 @@ static inline void delay_capa_renew(struct obd_capa *oc, cfs_time_t delay)
 	oc->c_expiry = cfs_time_add(oc->c_expiry, cfs_time_seconds(delay));
 }
 
-int ll_update_capa(struct obd_capa *ocapa, struct lustre_capa *capa)
+static int ll_update_capa(struct obd_capa *ocapa, struct lustre_capa *capa)
 {
 	struct inode *inode = ocapa->u.cli.inode;
 	int rc = 0;
diff --git a/drivers/staging/lustre/lustre/llite/llite_internal.h b/drivers/staging/lustre/lustre/llite/llite_internal.h
index 69aba0afca4..dde7632ba01 100644
--- a/drivers/staging/lustre/lustre/llite/llite_internal.h
+++ b/drivers/staging/lustre/lustre/llite/llite_internal.h
@@ -71,10 +71,6 @@
 #define LUSTRE_FPRIVATE(file) ((file)->private_data)
 
 struct ll_dentry_data {
-	int				lld_cwd_count;
-	int				lld_mnt_count;
-	struct obd_client_handle	lld_cwd_och;
-	struct obd_client_handle	lld_mnt_och;
 	struct lookup_intent		*lld_it;
 	unsigned int			lld_sa_generation;
 	unsigned int			lld_invalid:1;
@@ -83,8 +79,6 @@ struct ll_dentry_data {
 
 #define ll_d2d(de) ((struct ll_dentry_data*)((de)->d_fsdata))
 
-extern struct file_operations ll_pgcache_seq_fops;
-
 #define LLI_INODE_MAGIC		 0x111d0de5
 #define LLI_INODE_DEAD		  0xdeadd00d
 
@@ -120,16 +114,12 @@ enum lli_flags {
 	/* Sizeon-on-MDS attributes are changed. An attribute update needs to
 	 * be sent to MDS. */
 	LLIF_SOM_DIRTY	  = (1 << 3),
-	/* File is contented */
-	LLIF_CONTENDED	  = (1 << 4),
-	/* Truncate uses server lock for this file */
-	LLIF_SRVLOCK	    = (1 << 5),
 	/* File data is modified. */
-	LLIF_DATA_MODIFIED      = (1 << 6),
+	LLIF_DATA_MODIFIED      = (1 << 4),
 	/* File is being restored */
-	LLIF_FILE_RESTORING	= (1 << 7),
+	LLIF_FILE_RESTORING	= (1 << 5),
 	/* Xattr cache is attached to the file */
-	LLIF_XATTR_CACHE	= (1 << 8),
+	LLIF_XATTR_CACHE	= (1 << 6),
 };
 
 struct ll_inode_info {
@@ -194,7 +184,6 @@ struct ll_inode_info {
 			 * cleanup the dir readahead. */
 			void			   *d_opendir_key;
 			struct ll_statahead_info       *d_sai;
-			struct posix_acl	       *d_def_acl;
 			/* protect statahead stuff. */
 			spinlock_t			d_sa_lock;
 			/* "opendir_pid" is the token when lookup/revalid
@@ -205,14 +194,12 @@ struct ll_inode_info {
 #define lli_readdir_mutex       u.d.d_readdir_mutex
 #define lli_opendir_key	 u.d.d_opendir_key
 #define lli_sai		 u.d.d_sai
-#define lli_def_acl	     u.d.d_def_acl
 #define lli_sa_lock	     u.d.d_sa_lock
 #define lli_opendir_pid	 u.d.d_opendir_pid
 
 		/* for non-directory */
 		struct {
-			struct semaphore		f_size_sem;
-			void				*f_size_sem_owner;
+			struct mutex			f_size_mutex;
 			char				*f_symlink_name;
 			__u64				f_maxbytes;
 			/*
@@ -233,11 +220,6 @@ struct ll_inode_info {
 			/* for writepage() only to communicate to fsync */
 			int				f_async_rc;
 
-			/* volatile file criteria is based on file name, this
-			 * flag is used to keep the test result, so the strcmp
-			 * is done only once
-			 */
-			bool				f_volatile;
 			/*
 			 * whenever a process try to read/write the file, the
 			 * jobid of the process will be saved here, and it'll
@@ -249,8 +231,7 @@ struct ll_inode_info {
 			char		     f_jobid[JOBSTATS_JOBID_SIZE];
 		} f;
 
-#define lli_size_sem	    u.f.f_size_sem
-#define lli_size_sem_owner      u.f.f_size_sem_owner
+#define lli_size_mutex          u.f.f_size_mutex
 #define lli_symlink_name	u.f.f_symlink_name
 #define lli_maxbytes	    u.f.f_maxbytes
 #define lli_trunc_sem	   u.f.f_trunc_sem
@@ -261,7 +242,6 @@ struct ll_inode_info {
 #define lli_agl_index		u.f.f_agl_index
 #define lli_async_rc		u.f.f_async_rc
 #define lli_jobid		u.f.f_jobid
-#define lli_volatile		u.f.f_volatile
 
 	} u;
 
@@ -280,14 +260,33 @@ struct ll_inode_info {
 
 	/* mutex to request for layout lock exclusively. */
 	struct mutex			lli_layout_mutex;
-	/* valid only inside LAYOUT ibits lock, protected by lli_layout_mutex */
+	/* Layout version, protected by lli_layout_lock */
 	__u32				lli_layout_gen;
+	spinlock_t			lli_layout_lock;
 
 	struct rw_semaphore		lli_xattrs_list_rwsem;
 	struct mutex			lli_xattrs_enq_lock;
 	struct list_head		lli_xattrs;/* ll_xattr_entry->xe_list */
 };
 
+static inline __u32 ll_layout_version_get(struct ll_inode_info *lli)
+{
+	__u32 gen;
+
+	spin_lock(&lli->lli_layout_lock);
+	gen = lli->lli_layout_gen;
+	spin_unlock(&lli->lli_layout_lock);
+
+	return gen;
+}
+
+static inline void ll_layout_version_set(struct ll_inode_info *lli, __u32 gen)
+{
+	spin_lock(&lli->lli_layout_lock);
+	lli->lli_layout_gen = gen;
+	spin_unlock(&lli->lli_layout_lock);
+}
+
 int ll_xattr_cache_destroy(struct inode *inode);
 
 int ll_xattr_cache_get(struct inode *inode,
@@ -300,7 +299,7 @@ int ll_xattr_cache_get(struct inode *inode,
  * Locking to guarantee consistency of non-atomic updates to long long i_size,
  * consistency between file size and KMS.
  *
- * Implemented by ->lli_size_sem and ->lsm_lock, nested in that order.
+ * Implemented by ->lli_size_mutex and ->lsm_lock, nested in that order.
  */
 
 void ll_inode_size_lock(struct inode *inode);
@@ -442,10 +441,6 @@ enum stats_track_type {
 	"xattr",	\
 }
 
-/* default value for ll_sb_info->contention_time */
-#define SBI_DEFAULT_CONTENTION_SECONDS     60
-/* default value for lockless_truncate_enable */
-#define SBI_DEFAULT_LOCKLESS_TRUNCATE_ENABLE 1
 #define RCE_HASHES      32
 
 struct rmtacl_ctl_entry {
@@ -656,12 +651,6 @@ static inline struct inode *ll_info2i(struct ll_inode_info *lli)
 	return &lli->lli_vfs_inode;
 }
 
-struct it_cb_data {
-	struct inode  *icbd_parent;
-	struct dentry **icbd_childp;
-	obd_id	hash;
-};
-
 __u32 ll_i2suppgid(struct inode *i);
 void ll_i2gids(__u32 *suppgids, struct inode *i1,struct inode *i2);
 
@@ -669,21 +658,13 @@ static inline int ll_need_32bit_api(struct ll_sb_info *sbi)
 {
 #if BITS_PER_LONG == 32
 	return 1;
+#elif defined(CONFIG_COMPAT)
+	return unlikely(is_compat_task() || (sbi->ll_flags & LL_SBI_32BIT_API));
 #else
-	return unlikely(
-#ifdef CONFIG_COMPAT
-		is_compat_task() ||
-#endif
-		(sbi->ll_flags & LL_SBI_32BIT_API)
-	);
+	return unlikely(sbi->ll_flags & LL_SBI_32BIT_API);
 #endif
 }
 
-#define LLAP_MAGIC 98764321
-
-extern struct kmem_cache *ll_async_page_slab;
-extern size_t ll_async_page_slab_size;
-
 void ll_ra_read_in(struct file *f, struct ll_ra_read *rar);
 void ll_ra_read_ex(struct file *f, struct ll_ra_read *rar);
 struct ll_ra_read *ll_ra_read_get(struct file *f);
@@ -716,14 +697,16 @@ static inline void ll_rw_stats_tally(struct ll_sb_info *sbi, pid_t pid,
 
 /* llite/dir.c */
 void ll_release_page(struct page *page, int remove);
-extern struct file_operations ll_dir_operations;
-extern struct inode_operations ll_dir_inode_operations;
+extern const struct file_operations ll_dir_operations;
+extern const struct inode_operations ll_dir_inode_operations;
 struct page *ll_get_dir_page(struct inode *dir, __u64 hash,
 			     struct ll_dir_chain *chain);
 int ll_dir_read(struct inode *inode, struct dir_context *ctx);
 
 int ll_get_mdt_idx(struct inode *inode);
 /* llite/namei.c */
+extern const struct inode_operations ll_special_inode_operations;
+
 int ll_objects_destroy(struct ptlrpc_request *request,
 		       struct inode *dir);
 struct inode *ll_iget(struct super_block *sb, ino_t hash,
@@ -738,43 +721,34 @@ int ll_prepare_write(struct file *, struct page *, unsigned from, unsigned to);
 int ll_commit_write(struct file *, struct page *, unsigned from, unsigned to);
 int ll_writepage(struct page *page, struct writeback_control *wbc);
 int ll_writepages(struct address_space *, struct writeback_control *wbc);
-void ll_removepage(struct page *page);
 int ll_readpage(struct file *file, struct page *page);
 void ll_readahead_init(struct inode *inode, struct ll_readahead_state *ras);
-int ll_file_punch(struct inode *, loff_t, int);
-ssize_t ll_file_lockless_io(struct file *, char *, size_t, loff_t *, int);
-void ll_clear_file_contended(struct inode*);
-int ll_sync_page_range(struct inode *, struct address_space *, loff_t, size_t);
 int ll_readahead(const struct lu_env *env, struct cl_io *io,
 		 struct ll_readahead_state *ras, struct address_space *mapping,
 		 struct cl_page_list *queue, int flags);
 
+#ifndef MS_HAS_NEW_AOPS
+extern const struct address_space_operations ll_aops;
+#else
+extern const struct address_space_operations_ext ll_aops;
+#endif
+
 /* llite/file.c */
 extern struct file_operations ll_file_operations;
 extern struct file_operations ll_file_operations_flock;
 extern struct file_operations ll_file_operations_noflock;
 extern struct inode_operations ll_file_inode_operations;
-extern int ll_inode_revalidate_it(struct dentry *, struct lookup_intent *,
-				  __u64);
 extern int ll_have_md_lock(struct inode *inode, __u64 *bits,
 			   ldlm_mode_t l_req_mode);
 extern ldlm_mode_t ll_take_md_lock(struct inode *inode, __u64 bits,
 				   struct lustre_handle *lockh, __u64 flags,
 				   ldlm_mode_t mode);
-int __ll_inode_revalidate_it(struct dentry *, struct lookup_intent *,
-			     __u64 bits);
-int ll_revalidate_nd(struct dentry *dentry, unsigned int flags);
 int ll_file_open(struct inode *inode, struct file *file);
 int ll_file_release(struct inode *inode, struct file *file);
 int ll_glimpse_ioctl(struct ll_sb_info *sbi,
 		     struct lov_stripe_md *lsm, lstat_t *st);
 void ll_ioepoch_open(struct ll_inode_info *lli, __u64 ioepoch);
-int ll_local_open(struct file *file,
-		  struct lookup_intent *it, struct ll_file_data *fd,
-		  struct obd_client_handle *och);
 int ll_release_openhandle(struct dentry *, struct lookup_intent *);
-int ll_md_close(struct obd_export *md_exp, struct inode *inode,
-		struct file *file);
 int ll_md_real_close(struct inode *inode, fmode_t fmode);
 void ll_ioepoch_close(struct inode *inode, struct md_op_data *op_data,
 		      struct obd_client_handle **och, unsigned long flags);
@@ -782,15 +756,10 @@ void ll_done_writing_attr(struct inode *inode, struct md_op_data *op_data);
 int ll_som_update(struct inode *inode, struct md_op_data *op_data);
 int ll_inode_getattr(struct inode *inode, struct obdo *obdo,
 		     __u64 ioepoch, int sync);
-int ll_md_setattr(struct dentry *dentry, struct md_op_data *op_data,
-		  struct md_open_data **mod);
 void ll_pack_inode2opdata(struct inode *inode, struct md_op_data *op_data,
 			  struct lustre_handle *fh);
-int ll_getattr_it(struct vfsmount *mnt, struct dentry *de,
-	       struct lookup_intent *it, struct kstat *stat);
 int ll_getattr(struct vfsmount *mnt, struct dentry *de, struct kstat *stat);
-struct ll_file_data *ll_file_data_get(void);
-struct posix_acl * ll_get_acl(struct inode *inode, int type);
+struct posix_acl *ll_get_acl(struct inode *inode, int type);
 
 int ll_inode_permission(struct inode *inode, int mask);
 
@@ -805,44 +774,30 @@ int ll_dir_setstripe(struct inode *inode, struct lov_user_md *lump,
 int ll_dir_getstripe(struct inode *inode, struct lov_mds_md **lmmp,
 		     int *lmm_size, struct ptlrpc_request **request);
 int ll_fsync(struct file *file, loff_t start, loff_t end, int data);
-int ll_do_fiemap(struct inode *inode, struct ll_user_fiemap *fiemap,
-	      int num_bytes);
 int ll_merge_lvb(const struct lu_env *env, struct inode *inode);
-int ll_get_grouplock(struct inode *inode, struct file *file, unsigned long arg);
-int ll_put_grouplock(struct inode *inode, struct file *file, unsigned long arg);
 int ll_fid2path(struct inode *inode, void *arg);
 int ll_data_version(struct inode *inode, __u64 *data_version, int extent_lock);
 int ll_hsm_release(struct inode *inode);
 
-struct obd_client_handle *ll_lease_open(struct inode *inode, struct file *file,
-					fmode_t mode, __u64 flags);
-int ll_lease_close(struct obd_client_handle *och, struct inode *inode,
-		   bool *lease_broken);
-
 /* llite/dcache.c */
 
 int ll_d_init(struct dentry *de);
-extern struct dentry_operations ll_d_ops;
+extern const struct dentry_operations ll_d_ops;
 void ll_intent_drop_lock(struct lookup_intent *);
 void ll_intent_release(struct lookup_intent *);
 void ll_invalidate_aliases(struct inode *);
-void ll_frob_intent(struct lookup_intent **itp, struct lookup_intent *deft);
 void ll_lookup_finish_locks(struct lookup_intent *it, struct dentry *dentry);
-int ll_dcompare(const struct dentry *parent, const struct dentry *dentry,
-		unsigned int len, const char *str, const struct qstr *d_name);
 int ll_revalidate_it_finish(struct ptlrpc_request *request,
 			    struct lookup_intent *it, struct dentry *de);
 
 /* llite/llite_lib.c */
 extern struct super_operations lustre_super_operations;
 
-char *ll_read_opt(const char *opt, char *data);
 void ll_lli_init(struct ll_inode_info *lli);
 int ll_fill_super(struct super_block *sb, struct vfsmount *mnt);
 void ll_put_super(struct super_block *sb);
 void ll_kill_super(struct super_block *sb);
 struct inode *ll_inode_from_resource_lock(struct ldlm_lock *lock);
-struct inode *ll_inode_from_lock(struct ldlm_lock *lock);
 void ll_clear_inode(struct inode *inode);
 int ll_setattr_raw(struct dentry *dentry, struct iattr *attr, bool hsm_import);
 int ll_setattr(struct dentry *de, struct iattr *attr);
@@ -862,9 +817,11 @@ void ll_dirty_page_discard_warn(struct page *page, int ioret);
 int ll_prep_inode(struct inode **inode, struct ptlrpc_request *req,
 		  struct super_block *, struct lookup_intent *);
 void lustre_dump_dentry(struct dentry *, int recur);
-void lustre_dump_inode(struct inode *);
 int ll_obd_statfs(struct inode *inode, void *arg);
 int ll_get_max_mdsize(struct ll_sb_info *sbi, int *max_mdsize);
+int ll_get_default_mdsize(struct ll_sb_info *sbi, int *default_mdsize);
+int ll_get_max_cookiesize(struct ll_sb_info *sbi, int *max_cookiesize);
+int ll_get_default_cookiesize(struct ll_sb_info *sbi, int *default_cookiesize);
 int ll_process_config(struct lustre_cfg *lcfg);
 struct md_op_data *ll_prep_md_op_data(struct md_op_data *op_data,
 				      struct inode *i1, struct inode *i2,
@@ -881,15 +838,6 @@ void get_uuid2fsid(const char *name, int len, __kernel_fsid_t *fsid);
 struct inode *search_inode_for_lustre(struct super_block *sb,
 				      const struct lu_fid *fid);
 
-/* llite/special.c */
-extern struct inode_operations ll_special_inode_operations;
-extern struct file_operations ll_special_chr_inode_fops;
-extern struct file_operations ll_special_chr_file_fops;
-extern struct file_operations ll_special_blk_inode_fops;
-extern struct file_operations ll_special_fifo_inode_fops;
-extern struct file_operations ll_special_fifo_file_fops;
-extern struct file_operations ll_special_sock_inode_fops;
-
 /* llite/symlink.c */
 extern struct inode_operations ll_fast_symlink_inode_operations;
 
@@ -957,11 +905,6 @@ struct vvp_io {
 	 * Set when cui_bead has been initialized.
 	 */
 	int		  cui_ra_window_set;
-	/**
-	 * Partially truncated page, that vvp_io_trunc_start() keeps locked
-	 * across truncate.
-	 */
-	struct cl_page      *cui_partpage;
 };
 
 /**
@@ -990,12 +933,9 @@ struct ll_cl_context {
 	struct cl_page *lcc_page;
 	struct lu_env  *lcc_env;
 	int	     lcc_refcheck;
-	int	     lcc_created;
 };
 
 struct vvp_thread_info {
-	struct ost_lvb       vti_lvb;
-	struct cl_2queue     vti_queue;
 	struct iovec	 vti_local_iov;
 	struct vvp_io_args   vti_args;
 	struct ra_io_arg     vti_ria;
@@ -1042,25 +982,17 @@ static inline struct vvp_io *vvp_env_io(const struct lu_env *env)
 	return &vvp_env_session(env)->vs_ios;
 }
 
+int vvp_global_init(void);
+void vvp_global_fini(void);
+
 void ll_queue_done_writing(struct inode *inode, unsigned long flags);
 void ll_close_thread_shutdown(struct ll_close_queue *lcq);
 int ll_close_thread_start(struct ll_close_queue **lcq_ret);
 
 /* llite/llite_mmap.c */
-typedef struct rb_root  rb_root_t;
-typedef struct rb_node  rb_node_t;
-
-struct ll_lock_tree_node;
-struct ll_lock_tree {
-	rb_root_t		       lt_root;
-	struct list_head		      lt_locked_list;
-	struct ll_file_data	    *lt_fd;
-};
 
 int ll_teardown_mmaps(struct address_space *mapping, __u64 first, __u64 last);
 int ll_file_mmap(struct file * file, struct vm_area_struct * vma);
-struct ll_lock_tree_node * ll_node_from_inode(struct inode *inode, __u64 start,
-					      __u64 end, ldlm_mode_t mode);
 void policy_from_vma(ldlm_policy_data_t *policy,
 		struct vm_area_struct *vma, unsigned long addr, size_t count);
 struct vm_area_struct *our_vma(struct mm_struct *mm, unsigned long addr,
@@ -1127,11 +1059,6 @@ static inline struct lu_fid *ll_inode2fid(struct inode *inode)
 	return fid;
 }
 
-static inline int ll_mds_max_easize(struct super_block *sb)
-{
-	return sbi2mdc(ll_s2sbi(sb))->cl_max_mds_easize;
-}
-
 static inline __u64 ll_file_maxbytes(struct inode *inode)
 {
 	return ll_i2info(inode)->lli_maxbytes;
@@ -1149,7 +1076,6 @@ int ll_removexattr(struct dentry *dentry, const char *name);
 extern struct kmem_cache *ll_remote_perm_cachep;
 extern struct kmem_cache *ll_rmtperm_hash_cachep;
 
-struct hlist_head *alloc_rmtperm_hash(void);
 void free_rmtperm_hash(struct hlist_head *hash);
 int ll_update_remote_perm(struct inode *inode, struct mdt_remote_perm *perm);
 int lustre_check_remote_perm(struct inode *inode, int mask);
@@ -1162,7 +1088,6 @@ void ll_capa_thread_stop(void);
 void ll_capa_timer_callback(unsigned long unused);
 
 struct obd_capa *ll_add_capa(struct inode *inode, struct obd_capa *ocapa);
-int ll_update_capa(struct obd_capa *ocapa, struct lustre_capa *capa);
 
 void ll_capa_open(struct inode *inode);
 void ll_capa_close(struct inode *inode);
@@ -1182,14 +1107,12 @@ extern struct lu_device_type vvp_device_type;
  */
 int cl_sb_init(struct super_block *sb);
 int cl_sb_fini(struct super_block *sb);
-enum cl_lock_mode  vvp_mode_from_vma(struct vm_area_struct *vma);
 void ll_io_init(struct cl_io *io, const struct file *file, int write);
 
 void ras_update(struct ll_sb_info *sbi, struct inode *inode,
 		struct ll_readahead_state *ras, unsigned long index,
 		unsigned hit);
 void ll_ra_count_put(struct ll_sb_info *sbi, unsigned long len);
-int ll_is_file_contended(struct file *file);
 void ll_ra_stats_inc(struct address_space *mapping, enum ra_stat which);
 
 /* llite/llite_rmtacl.c */
@@ -1262,7 +1185,6 @@ struct ll_statahead_info {
 	unsigned int	    sai_skip_hidden;/* skipped hidden dentry count */
 	unsigned int	    sai_ls_all:1,   /* "ls -al", do stat-ahead for
 						 * hidden entries */
-				sai_in_readpage:1,/* statahead is in readdir()*/
 				sai_agl_valid:1;/* AGL is valid for the dir */
 	wait_queue_head_t	     sai_waitq;      /* stat-ahead wait queue */
 	struct ptlrpc_thread    sai_thread;     /* stat-ahead thread */
@@ -1387,9 +1309,6 @@ typedef enum llioc_iter (*llioc_callback_t)(struct inode *inode,
 		struct file *file, unsigned int cmd, unsigned long arg,
 		void *magic, int *rcp);
 
-enum llioc_iter ll_iocontrol_call(struct inode *inode, struct file *file,
-		unsigned int cmd, unsigned long arg, int *rcp);
-
 /* export functions */
 /* Register ioctl block dynamatically for a regular file.
  *
@@ -1431,7 +1350,7 @@ static inline void cl_isize_unlock(struct inode *inode)
 
 static inline void cl_isize_write_nolock(struct inode *inode, loff_t kms)
 {
-	LASSERT(down_trylock(&ll_i2info(inode)->lli_size_sem) != 0);
+	LASSERT(mutex_is_locked(&ll_i2info(inode)->lli_size_mutex));
 	i_size_write(inode, kms);
 }
 
diff --git a/drivers/staging/lustre/lustre/llite/llite_lib.c b/drivers/staging/lustre/lustre/llite/llite_lib.c
index 7c4fd97a7fa..deca27ea33f 100644
--- a/drivers/staging/lustre/lustre/llite/llite_lib.c
+++ b/drivers/staging/lustre/lustre/llite/llite_lib.c
@@ -58,14 +58,8 @@
 struct kmem_cache *ll_file_data_slab;
 struct proc_dir_entry *proc_lustre_fs_root;
 
-LIST_HEAD(ll_super_blocks);
-DEFINE_SPINLOCK(ll_sb_lock);
-
-#ifndef MS_HAS_NEW_AOPS
-extern struct address_space_operations ll_aops;
-#else
-extern struct address_space_operations_ext ll_aops;
-#endif
+static LIST_HEAD(ll_super_blocks);
+static DEFINE_SPINLOCK(ll_sb_lock);
 
 #ifndef log2
 #define log2(n) ffz(~(n))
@@ -143,7 +137,7 @@ static struct ll_sb_info *ll_init_sbi(void)
 	return sbi;
 }
 
-void ll_free_sbi(struct super_block *sb)
+static void ll_free_sbi(struct super_block *sb)
 {
 	struct ll_sb_info *sbi = ll_s2sbi(sb);
 
@@ -597,8 +591,7 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 
 	return err;
 out_root:
-	if (root)
-		iput(root);
+	iput(root);
 out_lock_cn_cb:
 	obd_fid_fini(sbi->ll_dt_exp->exp_obd);
 out_dt:
@@ -634,7 +627,46 @@ int ll_get_max_mdsize(struct ll_sb_info *sbi, int *lmmsize)
 	return rc;
 }
 
-void ll_dump_inode(struct inode *inode)
+int ll_get_default_mdsize(struct ll_sb_info *sbi, int *lmmsize)
+{
+	int size, rc;
+
+	size = sizeof(int);
+	rc = obd_get_info(NULL, sbi->ll_md_exp, sizeof(KEY_DEFAULT_EASIZE),
+			 KEY_DEFAULT_EASIZE, &size, lmmsize, NULL);
+	if (rc)
+		CERROR("Get default mdsize error rc %d\n", rc);
+
+	return rc;
+}
+
+int ll_get_max_cookiesize(struct ll_sb_info *sbi, int *lmmsize)
+{
+	int size, rc;
+
+	size = sizeof(int);
+	rc = obd_get_info(NULL, sbi->ll_md_exp, sizeof(KEY_MAX_COOKIESIZE),
+			  KEY_MAX_COOKIESIZE, &size, lmmsize, NULL);
+	if (rc)
+		CERROR("Get max cookiesize error rc %d\n", rc);
+
+	return rc;
+}
+
+int ll_get_default_cookiesize(struct ll_sb_info *sbi, int *lmmsize)
+{
+	int size, rc;
+
+	size = sizeof(int);
+	rc = obd_get_info(NULL, sbi->ll_md_exp, sizeof(KEY_DEFAULT_COOKIESIZE),
+			  KEY_DEFAULT_COOKIESIZE, &size, lmmsize, NULL);
+	if (rc)
+		CERROR("Get default cookiesize error rc %d\n", rc);
+
+	return rc;
+}
+
+static void ll_dump_inode(struct inode *inode)
 {
 	struct ll_d_hlist_node *tmp;
 	int dentry_count = 0;
@@ -677,7 +709,7 @@ void lustre_dump_dentry(struct dentry *dentry, int recur)
 	}
 }
 
-void client_common_put_super(struct super_block *sb)
+static void client_common_put_super(struct super_block *sb)
 {
 	struct ll_sb_info *sbi = ll_s2sbi(sb);
 
@@ -726,30 +758,6 @@ void ll_kill_super(struct super_block *sb)
 	}
 }
 
-char *ll_read_opt(const char *opt, char *data)
-{
-	char *value;
-	char *retval;
-
-	CDEBUG(D_SUPER, "option: %s, data %s\n", opt, data);
-	if (strncmp(opt, data, strlen(opt)))
-		return NULL;
-	value = strchr(data, '=');
-	if (value == NULL)
-		return NULL;
-
-	value++;
-	OBD_ALLOC(retval, strlen(value) + 1);
-	if (!retval) {
-		CERROR("out of memory!\n");
-		return NULL;
-	}
-
-	memcpy(retval, value, strlen(value)+1);
-	CDEBUG(D_SUPER, "Assigned option: %s, value %s\n", opt, retval);
-	return retval;
-}
-
 static inline int ll_set_opt(const char *opt, char *data, int fl)
 {
 	if (strncmp(opt, data, strlen(opt)) != 0)
@@ -927,7 +935,8 @@ void ll_lli_init(struct ll_inode_info *lli)
 	mutex_init(&lli->lli_och_mutex);
 	spin_lock_init(&lli->lli_agl_lock);
 	lli->lli_has_smd = false;
-	lli->lli_layout_gen = LL_LAYOUT_GEN_NONE;
+	spin_lock_init(&lli->lli_layout_lock);
+	ll_layout_version_set(lli, LL_LAYOUT_GEN_NONE);
 	lli->lli_clob = NULL;
 
 	init_rwsem(&lli->lli_xattrs_list_rwsem);
@@ -938,12 +947,10 @@ void ll_lli_init(struct ll_inode_info *lli)
 		mutex_init(&lli->lli_readdir_mutex);
 		lli->lli_opendir_key = NULL;
 		lli->lli_sai = NULL;
-		lli->lli_def_acl = NULL;
 		spin_lock_init(&lli->lli_sa_lock);
 		lli->lli_opendir_pid = 0;
 	} else {
-		sema_init(&lli->lli_size_sem, 1);
-		lli->lli_size_sem_owner = NULL;
+		mutex_init(&lli->lli_size_mutex);
 		lli->lli_symlink_name = NULL;
 		init_rwsem(&lli->lli_trunc_sem);
 		mutex_init(&lli->lli_write_mutex);
@@ -952,7 +959,6 @@ void ll_lli_init(struct ll_inode_info *lli)
 		INIT_LIST_HEAD(&lli->lli_agl_list);
 		lli->lli_agl_index = 0;
 		lli->lli_async_rc = 0;
-		lli->lli_volatile = false;
 	}
 	mutex_init(&lli->lli_layout_mutex);
 }
@@ -1151,28 +1157,6 @@ struct inode *ll_inode_from_resource_lock(struct ldlm_lock *lock)
 	return inode;
 }
 
-struct inode *ll_inode_from_lock(struct ldlm_lock *lock)
-{
-	struct inode *inode = NULL;
-	/* NOTE: we depend on atomic igrab() -bzzz */
-	lock_res_and_lock(lock);
-	if (lock->l_ast_data) {
-		struct ll_inode_info *lli = ll_i2info(lock->l_ast_data);
-		if (lli->lli_inode_magic == LLI_INODE_MAGIC) {
-			inode = igrab(lock->l_ast_data);
-		} else {
-			inode = lock->l_ast_data;
-			LDLM_DEBUG_LIMIT(inode->i_state & I_FREEING ?  D_INFO :
-					 D_WARNING, lock, "l_ast_data %p is "
-					 "bogus: magic %08x", lock->l_ast_data,
-					 lli->lli_inode_magic);
-			inode = NULL;
-		}
-	}
-	unlock_res_and_lock(lock);
-	return inode;
-}
-
 void ll_clear_inode(struct inode *inode)
 {
 	struct ll_inode_info *lli = ll_i2info(inode);
@@ -1449,7 +1433,6 @@ int ll_setattr_raw(struct dentry *dentry, struct iattr *attr, bool hsm_import)
 		if (attr->ia_valid & ATTR_SIZE)
 			inode_dio_write_done(inode);
 		mutex_unlock(&inode->i_mutex);
-		down_write(&lli->lli_trunc_sem);
 	}
 
 	memcpy(&op_data->op_attr, attr, sizeof(*attr));
@@ -1513,7 +1496,11 @@ int ll_setattr_raw(struct dentry *dentry, struct iattr *attr, bool hsm_import)
 		 * excessive to send mtime/atime updates to OSTs when not
 		 * setting times to past, but it is necessary due to possible
 		 * time de-synchronization between MDT inode and OST objects */
+		if (attr->ia_valid & ATTR_SIZE)
+			down_write(&lli->lli_trunc_sem);
 		rc = ll_setattr_ost(inode, attr);
+		if (attr->ia_valid & ATTR_SIZE)
+			up_write(&lli->lli_trunc_sem);
 out:
 	if (op_data) {
 		if (op_data->op_ioepoch) {
@@ -1524,7 +1511,6 @@ out:
 		ll_finish_md_op_data(op_data);
 	}
 	if (!S_ISDIR(inode->i_mode)) {
-		up_write(&lli->lli_trunc_sem);
 		mutex_lock(&inode->i_mutex);
 		if ((attr->ia_valid & ATTR_SIZE) && !hsm_import)
 			inode_dio_wait(inode);
@@ -1658,10 +1644,7 @@ void ll_inode_size_lock(struct inode *inode)
 	LASSERT(!S_ISDIR(inode->i_mode));
 
 	lli = ll_i2info(inode);
-	LASSERT(lli->lli_size_sem_owner != current);
-	down(&lli->lli_size_sem);
-	LASSERT(lli->lli_size_sem_owner == NULL);
-	lli->lli_size_sem_owner = current;
+	mutex_lock(&lli->lli_size_mutex);
 }
 
 void ll_inode_size_unlock(struct inode *inode)
@@ -1669,9 +1652,7 @@ void ll_inode_size_unlock(struct inode *inode)
 	struct ll_inode_info *lli;
 
 	lli = ll_i2info(inode);
-	LASSERT(lli->lli_size_sem_owner == current);
-	lli->lli_size_sem_owner = NULL;
-	up(&lli->lli_size_sem);
+	mutex_unlock(&lli->lli_size_mutex);
 }
 
 void ll_update_inode(struct inode *inode, struct lustre_md *md)
@@ -2420,11 +2401,12 @@ void ll_dirty_page_discard_warn(struct page *page, int ioret)
 			path = ll_d_path(dentry, buf, PAGE_SIZE);
 	}
 
-	CWARN("%s: dirty page discard: %s/fid: "DFID"/%s may get corrupted "
-	      "(rc %d)\n", ll_get_fsname(page->mapping->host->i_sb, NULL, 0),
-	      s2lsi(page->mapping->host->i_sb)->lsi_lmd->lmd_dev,
-	      PFID(&obj->cob_header.coh_lu.loh_fid),
-	      (path && !IS_ERR(path)) ? path : "", ioret);
+	CDEBUG(D_WARNING,
+	       "%s: dirty page discard: %s/fid: "DFID"/%s may get corrupted "
+	       "(rc %d)\n", ll_get_fsname(page->mapping->host->i_sb, NULL, 0),
+	       s2lsi(page->mapping->host->i_sb)->lsi_lmd->lmd_dev,
+	       PFID(&obj->cob_header.coh_lu.loh_fid),
+	       (path && !IS_ERR(path)) ? path : "", ioret);
 
 	if (dentry != NULL)
 		dput(dentry);
diff --git a/drivers/staging/lustre/lustre/llite/llite_mmap.c b/drivers/staging/lustre/lustre/llite/llite_mmap.c
index 90b2c0d275f..426c7396166 100644
--- a/drivers/staging/lustre/lustre/llite/llite_mmap.c
+++ b/drivers/staging/lustre/lustre/llite/llite_mmap.c
@@ -51,10 +51,7 @@
 #include "llite_internal.h"
 #include <linux/lustre_compat25.h>
 
-struct page *ll_nopage(struct vm_area_struct *vma, unsigned long address,
-		       int *type);
-
-static struct vm_operations_struct ll_file_vm_ops;
+static const struct vm_operations_struct ll_file_vm_ops;
 
 void policy_from_vma(ldlm_policy_data_t *policy,
 			    struct vm_area_struct *vma, unsigned long addr,
@@ -97,10 +94,10 @@ struct vm_area_struct *our_vma(struct mm_struct *mm, unsigned long addr,
  * \retval EINVAL if env can't allocated
  * \return other error codes from cl_io_init.
  */
-struct cl_io *ll_fault_io_init(struct vm_area_struct *vma,
-			       struct lu_env **env_ret,
-			       struct cl_env_nest *nest,
-			       pgoff_t index, unsigned long *ra_flags)
+static struct cl_io *
+ll_fault_io_init(struct vm_area_struct *vma, struct lu_env **env_ret,
+		 struct cl_env_nest *nest, pgoff_t index,
+		 unsigned long *ra_flags)
 {
 	struct file	       *file = vma->vm_file;
 	struct inode	       *inode = file->f_dentry->d_inode;
@@ -446,14 +443,6 @@ static void ll_vm_close(struct vm_area_struct *vma)
 	LASSERT(atomic_read(&vob->cob_mmap_cnt) >= 0);
 }
 
-
-/* return the user space pointer that maps to a file offset via a vma */
-static inline unsigned long file_to_user(struct vm_area_struct *vma, __u64 byte)
-{
-	return vma->vm_start + (byte - ((__u64)vma->vm_pgoff << PAGE_CACHE_SHIFT));
-
-}
-
 /* XXX put nice comment here.  talk about __free_pte -> dirty pages and
  * nopage's reference passing to the pte */
 int ll_teardown_mmaps(struct address_space *mapping, __u64 first, __u64 last)
@@ -470,7 +459,7 @@ int ll_teardown_mmaps(struct address_space *mapping, __u64 first, __u64 last)
 	return rc;
 }
 
-static struct vm_operations_struct ll_file_vm_ops = {
+static const struct vm_operations_struct ll_file_vm_ops = {
 	.fault			= ll_fault,
 	.page_mkwrite		= ll_page_mkwrite,
 	.open			= ll_vm_open,
diff --git a/drivers/staging/lustre/lustre/llite/llite_nfs.c b/drivers/staging/lustre/lustre/llite/llite_nfs.c
index 3580069789f..a614b913ddc 100644
--- a/drivers/staging/lustre/lustre/llite/llite_nfs.c
+++ b/drivers/staging/lustre/lustre/llite/llite_nfs.c
@@ -51,7 +51,9 @@ __u32 get_uuid2int(const char *name, int len)
 	__u32 key0 = 0x12a3fe2d, key1 = 0x37abe8f9;
 	while (len--) {
 		__u32 key = key1 + (key0 ^ (*name++ * 7152373));
-		if (key & 0x80000000) key -= 0x7fffffff;
+
+		if (key & 0x80000000)
+			key -= 0x7fffffff;
 		key1 = key0;
 		key0 = key;
 	}
@@ -98,7 +100,7 @@ struct inode *search_inode_for_lustre(struct super_block *sb,
 	if (inode)
 		return inode;
 
-	rc = ll_get_max_mdsize(sbi, &eadatalen);
+	rc = ll_get_default_mdsize(sbi, &eadatalen);
 	if (rc)
 		return ERR_PTR(rc);
 
@@ -290,7 +292,7 @@ static struct dentry *ll_get_parent(struct dentry *dchild)
 	CDEBUG(D_INFO, "getting parent for (%lu,"DFID")\n",
 			dir->i_ino, PFID(ll_inode2fid(dir)));
 
-	rc = ll_get_max_mdsize(sbi, &lmmsize);
+	rc = ll_get_default_mdsize(sbi, &lmmsize);
 	if (rc != 0)
 		return ERR_PTR(rc);
 
diff --git a/drivers/staging/lustre/lustre/llite/lloop.c b/drivers/staging/lustre/lustre/llite/lloop.c
index f78eda235c7..b9694b8cb5d 100644
--- a/drivers/staging/lustre/lustre/llite/lloop.c
+++ b/drivers/staging/lustre/lustre/llite/lloop.c
@@ -122,9 +122,6 @@ struct lloop_device {
 	loff_t	       lo_offset;
 	loff_t	       lo_sizelimit;
 	int		  lo_flags;
-	int		(*ioctl)(struct lloop_device *, int cmd,
-				    unsigned long arg);
-
 	struct file	 *lo_backing_file;
 	struct block_device *lo_device;
 	unsigned	     lo_blocksize;
@@ -509,7 +506,6 @@ static int loop_set_fd(struct lloop_device *lo, struct file *unused,
 	lo->lo_device = bdev;
 	lo->lo_flags = lo_flags;
 	lo->lo_backing_file = file;
-	lo->ioctl = NULL;
 	lo->lo_sizelimit = 0;
 	lo->old_gfp_mask = mapping_gfp_mask(mapping);
 	mapping_set_gfp_mask(mapping, lo->old_gfp_mask & ~(__GFP_IO|__GFP_FS));
@@ -568,7 +564,6 @@ static int loop_clr_fd(struct lloop_device *lo, struct block_device *bdev,
 
 	down(&lo->lo_sem);
 	lo->lo_backing_file = NULL;
-	lo->ioctl = NULL;
 	lo->lo_device = NULL;
 	lo->lo_offset = 0;
 	lo->lo_sizelimit = 0;
@@ -624,7 +619,10 @@ static int lo_ioctl(struct block_device *bdev, fmode_t mode,
 	case LL_IOC_LLOOP_INFO: {
 		struct lu_fid fid;
 
-		LASSERT(lo->lo_backing_file != NULL);
+		if (lo->lo_backing_file == NULL) {
+			err = -ENOENT;
+			break;
+		}
 		if (inode == NULL)
 			inode = lo->lo_backing_file->f_dentry->d_inode;
 		if (lo->lo_state == LLOOP_BOUND)
diff --git a/drivers/staging/lustre/lustre/llite/lproc_llite.c b/drivers/staging/lustre/lustre/llite/lproc_llite.c
index a9a104a6a4e..77ee9e58cf8 100644
--- a/drivers/staging/lustre/lustre/llite/lproc_llite.c
+++ b/drivers/staging/lustre/lustre/llite/lproc_llite.c
@@ -41,12 +41,12 @@
 #include <obd_support.h>
 
 #include "llite_internal.h"
+#include "vvp_internal.h"
 
 /* /proc/lustre/llite mount point registration */
-extern struct file_operations vvp_dump_pgcache_file_ops;
-struct file_operations ll_rw_extents_stats_fops;
-struct file_operations ll_rw_extents_stats_pp_fops;
-struct file_operations ll_rw_offset_stats_fops;
+static struct file_operations ll_rw_extents_stats_fops;
+static struct file_operations ll_rw_extents_stats_pp_fops;
+static struct file_operations ll_rw_offset_stats_fops;
 
 static int ll_blksize_seq_show(struct seq_file *m, void *v)
 {
@@ -367,8 +367,9 @@ static int ll_max_cached_mb_seq_show(struct seq_file *m, void *v)
 			cache->ccc_lru_shrinkers);
 }
 
-static ssize_t ll_max_cached_mb_seq_write(struct file *file, const char *buffer,
-				      size_t count, loff_t *off)
+static ssize_t ll_max_cached_mb_seq_write(struct file *file,
+					  const char __user *buffer,
+					  size_t count, loff_t *off)
 {
 	struct super_block *sb = ((struct seq_file *)file->private_data)->private;
 	struct ll_sb_info *sbi = ll_s2sbi(sb);
@@ -376,9 +377,18 @@ static ssize_t ll_max_cached_mb_seq_write(struct file *file, const char *buffer,
 	int mult, rc, pages_number;
 	int diff = 0;
 	int nrpages = 0;
+	char kernbuf[128];
+
+	if (count >= sizeof(kernbuf))
+		return -EINVAL;
+
+	if (copy_from_user(kernbuf, buffer, count))
+		return -EFAULT;
+	kernbuf[count] = 0;
 
 	mult = 1 << (20 - PAGE_CACHE_SHIFT);
-	buffer = lprocfs_find_named_value(buffer, "max_cached_mb:", &count);
+	buffer += lprocfs_find_named_value(kernbuf, "max_cached_mb:", &count) -
+		  kernbuf;
 	rc = lprocfs_write_frac_helper(buffer, count, &pages_number, mult);
 	if (rc)
 		return rc;
@@ -681,7 +691,7 @@ static ssize_t ll_lazystatfs_seq_write(struct file *file, const char *buffer,
 }
 LPROC_SEQ_FOPS(ll_lazystatfs);
 
-static int ll_maxea_size_seq_show(struct seq_file *m, void *v)
+static int ll_max_easize_seq_show(struct seq_file *m, void *v)
 {
 	struct super_block *sb = m->private;
 	struct ll_sb_info *sbi = ll_s2sbi(sb);
@@ -694,7 +704,52 @@ static int ll_maxea_size_seq_show(struct seq_file *m, void *v)
 
 	return seq_printf(m, "%u\n", ealen);
 }
-LPROC_SEQ_FOPS_RO(ll_maxea_size);
+LPROC_SEQ_FOPS_RO(ll_max_easize);
+
+static int ll_defult_easize_seq_show(struct seq_file *m, void *v)
+{
+	struct super_block *sb = m->private;
+	struct ll_sb_info *sbi = ll_s2sbi(sb);
+	unsigned int ealen;
+	int rc;
+
+	rc = ll_get_default_mdsize(sbi, &ealen);
+	if (rc)
+		return rc;
+
+	return seq_printf(m, "%u\n", ealen);
+}
+LPROC_SEQ_FOPS_RO(ll_defult_easize);
+
+static int ll_max_cookiesize_seq_show(struct seq_file *m, void *v)
+{
+	struct super_block *sb = m->private;
+	struct ll_sb_info *sbi = ll_s2sbi(sb);
+	unsigned int cookielen;
+	int rc;
+
+	rc = ll_get_max_cookiesize(sbi, &cookielen);
+	if (rc)
+		return rc;
+
+	return seq_printf(m, "%u\n", cookielen);
+}
+LPROC_SEQ_FOPS_RO(ll_max_cookiesize);
+
+static int ll_defult_cookiesize_seq_show(struct seq_file *m, void *v)
+{
+	struct super_block *sb = m->private;
+	struct ll_sb_info *sbi = ll_s2sbi(sb);
+	unsigned int cookielen;
+	int rc;
+
+	rc = ll_get_default_cookiesize(sbi, &cookielen);
+	if (rc)
+		return rc;
+
+	return seq_printf(m, "%u\n", cookielen);
+}
+LPROC_SEQ_FOPS_RO(ll_defult_cookiesize);
 
 static int ll_sbi_flags_seq_show(struct seq_file *m, void *v)
 {
@@ -781,7 +836,10 @@ static struct lprocfs_vars lprocfs_llite_obd_vars[] = {
 	{ "statahead_agl",    &ll_statahead_agl_fops, 0 },
 	{ "statahead_stats",  &ll_statahead_stats_fops, 0, 0 },
 	{ "lazystatfs",       &ll_lazystatfs_fops, 0 },
-	{ "max_easize",       &ll_maxea_size_fops, 0, 0 },
+	{ "max_easize",       &ll_max_easize_fops, 0, 0 },
+	{ "default_easize",   &ll_defult_easize_fops, 0, 0 },
+	{ "max_cookiesize",   &ll_max_cookiesize_fops, 0, 0 },
+	{ "default_cookiesize", &ll_defult_cookiesize_fops, 0, 0 },
 	{ "sbi_flags",	      &ll_sbi_flags_fops, 0, 0 },
 	{ "xattr_cache",      &ll_xattr_cache_fops, 0, 0 },
 	{ 0 }
@@ -789,7 +847,7 @@ static struct lprocfs_vars lprocfs_llite_obd_vars[] = {
 
 #define MAX_STRING_SIZE 128
 
-struct llite_file_opcode {
+static const struct llite_file_opcode {
 	__u32       opcode;
 	__u32       type;
 	const char *opname;
@@ -1115,7 +1173,8 @@ static int ll_rw_extents_stats_pp_seq_show(struct seq_file *seq, void *v)
 }
 
 static ssize_t ll_rw_extents_stats_pp_seq_write(struct file *file,
-						const char *buf, size_t len,
+						const char __user *buf,
+						size_t len,
 						loff_t *off)
 {
 	struct seq_file *seq = file->private_data;
@@ -1124,10 +1183,24 @@ static ssize_t ll_rw_extents_stats_pp_seq_write(struct file *file,
 	int i;
 	int value = 1, rc = 0;
 
+	if (len == 0)
+		return -EINVAL;
+
 	rc = lprocfs_write_helper(buf, len, &value);
-	if (rc < 0 && (strcmp(buf, "disabled") == 0 ||
-		       strcmp(buf, "Disabled") == 0))
-		value = 0;
+	if (rc < 0 && len < 16) {
+		char kernbuf[16];
+
+		if (copy_from_user(kernbuf, buf, len))
+			return -EFAULT;
+		kernbuf[len] = 0;
+
+		if (kernbuf[len - 1] == '\n')
+			kernbuf[len - 1] = 0;
+
+		if (strcmp(kernbuf, "disabled") == 0 ||
+		    strcmp(kernbuf, "Disabled") == 0)
+			value = 0;
+	}
 
 	if (value == 0)
 		sbi->ll_rw_stats_on = 0;
@@ -1174,8 +1247,9 @@ static int ll_rw_extents_stats_seq_show(struct seq_file *seq, void *v)
 	return 0;
 }
 
-static ssize_t ll_rw_extents_stats_seq_write(struct file *file, const char *buf,
-					size_t len, loff_t *off)
+static ssize_t ll_rw_extents_stats_seq_write(struct file *file,
+					     const char __user *buf,
+					     size_t len, loff_t *off)
 {
 	struct seq_file *seq = file->private_data;
 	struct ll_sb_info *sbi = seq->private;
@@ -1183,15 +1257,30 @@ static ssize_t ll_rw_extents_stats_seq_write(struct file *file, const char *buf,
 	int i;
 	int value = 1, rc = 0;
 
+	if (len == 0)
+		return -EINVAL;
+
 	rc = lprocfs_write_helper(buf, len, &value);
-	if (rc < 0 && (strcmp(buf, "disabled") == 0 ||
-		       strcmp(buf, "Disabled") == 0))
-		value = 0;
+	if (rc < 0 && len < 16) {
+		char kernbuf[16];
+
+		if (copy_from_user(kernbuf, buf, len))
+			return -EFAULT;
+		kernbuf[len] = 0;
+
+		if (kernbuf[len - 1] == '\n')
+			kernbuf[len - 1] = 0;
+
+		if (strcmp(kernbuf, "disabled") == 0 ||
+		    strcmp(kernbuf, "Disabled") == 0)
+			value = 0;
+	}
 
 	if (value == 0)
 		sbi->ll_rw_stats_on = 0;
 	else
 		sbi->ll_rw_stats_on = 1;
+
 	spin_lock(&sbi->ll_pp_extent_lock);
 	for (i = 0; i <= LL_PROCESS_HIST_MAX; i++) {
 		io_extents->pp_extents[i].pid = 0;
@@ -1202,7 +1291,6 @@ static ssize_t ll_rw_extents_stats_seq_write(struct file *file, const char *buf,
 
 	return len;
 }
-
 LPROC_SEQ_FOPS(ll_rw_extents_stats);
 
 void ll_rw_stats_tally(struct ll_sb_info *sbi, pid_t pid,
@@ -1362,8 +1450,9 @@ static int ll_rw_offset_stats_seq_show(struct seq_file *seq, void *v)
 	return 0;
 }
 
-static ssize_t ll_rw_offset_stats_seq_write(struct file *file, const char *buf,
-				       size_t len, loff_t *off)
+static ssize_t ll_rw_offset_stats_seq_write(struct file *file,
+					    const char __user *buf,
+					    size_t len, loff_t *off)
 {
 	struct seq_file *seq = file->private_data;
 	struct ll_sb_info *sbi = seq->private;
@@ -1371,11 +1460,25 @@ static ssize_t ll_rw_offset_stats_seq_write(struct file *file, const char *buf,
 	struct ll_rw_process_info *offset_info = sbi->ll_rw_offset_info;
 	int value = 1, rc = 0;
 
+	if (len == 0)
+		return -EINVAL;
+
 	rc = lprocfs_write_helper(buf, len, &value);
 
-	if (rc < 0 && (strcmp(buf, "disabled") == 0 ||
-			   strcmp(buf, "Disabled") == 0))
-		value = 0;
+	if (rc < 0 && len < 16) {
+		char kernbuf[16];
+
+		if (copy_from_user(kernbuf, buf, len))
+			return -EFAULT;
+		kernbuf[len] = 0;
+
+		if (kernbuf[len - 1] == '\n')
+			kernbuf[len - 1] = 0;
+
+		if (strcmp(kernbuf, "disabled") == 0 ||
+		    strcmp(kernbuf, "Disabled") == 0)
+			value = 0;
+	}
 
 	if (value == 0)
 		sbi->ll_rw_stats_on = 0;
diff --git a/drivers/staging/lustre/lustre/llite/namei.c b/drivers/staging/lustre/lustre/llite/namei.c
index 25a6ea580f0..dfa1e745dfd 100644
--- a/drivers/staging/lustre/lustre/llite/namei.c
+++ b/drivers/staging/lustre/lustre/llite/namei.c
@@ -75,14 +75,6 @@ static int ll_d_mountpoint(struct dentry *dparent, struct dentry *dchild,
 	return mounted;
 }
 
-int ll_unlock(__u32 mode, struct lustre_handle *lockh)
-{
-	ldlm_lock_decref(lockh, mode);
-
-	return 0;
-}
-
-
 /* called from iget5_locked->find_inode() under inode_hash_lock spinlock */
 static int ll_test_inode(struct inode *inode, void *opaque)
 {
@@ -433,12 +425,10 @@ struct dentry *ll_splice_alias(struct inode *inode, struct dentry *de)
 	return de;
 }
 
-int ll_lookup_it_finish(struct ptlrpc_request *request,
-			struct lookup_intent *it, void *data)
+static int ll_lookup_it_finish(struct ptlrpc_request *request,
+			       struct lookup_intent *it,
+			       struct inode *parent, struct dentry **de)
 {
-	struct it_cb_data *icbd = data;
-	struct dentry **de = icbd->icbd_childp;
-	struct inode *parent = icbd->icbd_parent;
 	struct inode *inode = NULL;
 	__u64 bits = 0;
 	int rc;
@@ -513,7 +503,6 @@ static struct dentry *ll_lookup_it(struct inode *parent, struct dentry *dentry,
 	struct dentry *save = dentry, *retval;
 	struct ptlrpc_request *req = NULL;
 	struct md_op_data *op_data;
-	struct it_cb_data icbd;
 	__u32 opc;
 	int rc;
 
@@ -527,7 +516,8 @@ static struct dentry *ll_lookup_it(struct inode *parent, struct dentry *dentry,
 	if (d_mountpoint(dentry))
 		CERROR("Tell Peter, lookup on mtpt, it %s\n", LL_IT2STR(it));
 
-	ll_frob_intent(&it, &lookup_it);
+	if (it == NULL || it->it_op == IT_GETXATTR)
+		it = &lookup_it;
 
 	if (it->it_op == IT_GETATTR) {
 		rc = ll_statahead_enter(parent, &dentry, 0);
@@ -538,9 +528,6 @@ static struct dentry *ll_lookup_it(struct inode *parent, struct dentry *dentry,
 		}
 	}
 
-	icbd.icbd_childp = &dentry;
-	icbd.icbd_parent = parent;
-
 	if (it->it_op & IT_CREAT)
 		opc = LUSTRE_OPC_CREATE;
 	else
@@ -562,7 +549,7 @@ static struct dentry *ll_lookup_it(struct inode *parent, struct dentry *dentry,
 	if (rc < 0)
 		GOTO(out, retval = ERR_PTR(rc));
 
-	rc = ll_lookup_it_finish(req, it, &icbd);
+	rc = ll_lookup_it_finish(req, it, parent, &dentry);
 	if (rc != 0) {
 		ll_intent_release(it);
 		GOTO(out, retval = ERR_PTR(rc));
@@ -697,10 +684,7 @@ out_release:
 
 
 /* We depend on "mode" being set with the proper file type/umask by now */
-static struct inode *ll_create_node(struct inode *dir, const char *name,
-				    int namelen, const void *data, int datalen,
-				    int mode, __u64 extra,
-				    struct lookup_intent *it)
+static struct inode *ll_create_node(struct inode *dir, struct lookup_intent *it)
 {
 	struct inode *inode = NULL;
 	struct ptlrpc_request *request = NULL;
@@ -757,14 +741,10 @@ static int ll_create_it(struct inode *dir, struct dentry *dentry, int mode,
 	if (rc)
 		return rc;
 
-	inode = ll_create_node(dir, dentry->d_name.name, dentry->d_name.len,
-			       NULL, 0, mode, 0, it);
+	inode = ll_create_node(dir, it);
 	if (IS_ERR(inode))
 		return PTR_ERR(inode);
 
-	if (filename_is_volatile(dentry->d_name.name, dentry->d_name.len, NULL))
-		ll_i2info(inode)->lli_volatile = true;
-
 	d_instantiate(dentry, inode);
 	return 0;
 }
@@ -1238,7 +1218,7 @@ static int ll_rename(struct inode *old_dir, struct dentry *old_dentry,
 	return err;
 }
 
-struct inode_operations ll_dir_inode_operations = {
+const struct inode_operations ll_dir_inode_operations = {
 	.mknod	      = ll_mknod,
 	.atomic_open	    = ll_atomic_open,
 	.lookup	     = ll_lookup_nd,
@@ -1260,7 +1240,7 @@ struct inode_operations ll_dir_inode_operations = {
 	.get_acl	    = ll_get_acl,
 };
 
-struct inode_operations ll_special_inode_operations = {
+const struct inode_operations ll_special_inode_operations = {
 	.setattr	= ll_setattr,
 	.getattr	= ll_getattr,
 	.permission     = ll_inode_permission,
diff --git a/drivers/staging/lustre/lustre/llite/remote_perm.c b/drivers/staging/lustre/lustre/llite/remote_perm.c
index dedd56ae1e6..a8b1117b8f6 100644
--- a/drivers/staging/lustre/lustre/llite/remote_perm.c
+++ b/drivers/staging/lustre/lustre/llite/remote_perm.c
@@ -77,7 +77,7 @@ static inline void free_ll_remote_perm(struct ll_remote_perm *lrp)
 	OBD_SLAB_FREE(lrp, ll_remote_perm_cachep, sizeof(*lrp));
 }
 
-struct hlist_head *alloc_rmtperm_hash(void)
+static struct hlist_head *alloc_rmtperm_hash(void)
 {
 	struct hlist_head *hash;
 	int i;
diff --git a/drivers/staging/lustre/lustre/llite/rw.c b/drivers/staging/lustre/lustre/llite/rw.c
index 416f7a094a6..f0122c568a0 100644
--- a/drivers/staging/lustre/lustre/llite/rw.c
+++ b/drivers/staging/lustre/lustre/llite/rw.c
@@ -77,12 +77,6 @@ static void ll_cl_fini(struct ll_cl_context *lcc)
 		cl_page_put(env, page);
 	}
 
-	if (io && lcc->lcc_created) {
-		cl_io_end(env, io);
-		cl_io_unlock(env, io);
-		cl_io_iter_fini(env, io);
-		cl_io_fini(env, io);
-	}
 	cl_env_put(env, &lcc->lcc_refcheck);
 }
 
@@ -167,7 +161,6 @@ static struct ll_cl_context *ll_cl_init(struct file *file,
 			}
 		} else
 			result = io->ci_result;
-		lcc->lcc_created = 1;
 	}
 
 	lcc->lcc_io = io;
diff --git a/drivers/staging/lustre/lustre/llite/rw26.c b/drivers/staging/lustre/lustre/llite/rw26.c
index 7e3e0967993..55ca8d3c3e4 100644
--- a/drivers/staging/lustre/lustre/llite/rw26.c
+++ b/drivers/staging/lustre/lustre/llite/rw26.c
@@ -385,8 +385,8 @@ static ssize_t ll_direct_IO_26(int rw, struct kiocb *iocb,
 	if ((file_offset & ~CFS_PAGE_MASK) || (count & ~CFS_PAGE_MASK))
 		return -EINVAL;
 
-	CDEBUG(D_VFSTRACE, "VFS Op:inode=%lu/%u(%p), size=%lu (max %lu), "
-	       "offset=%lld=%llx, pages %lu (max %lu)\n",
+	CDEBUG(D_VFSTRACE,
+	       "VFS Op:inode=%lu/%u(%p), size=%zd (max %lu), offset=%lld=%llx, pages %zd (max %lu)\n",
 	       inode->i_ino, inode->i_generation, inode, count, MAX_DIO_SIZE,
 	       file_offset, file_offset, count >> PAGE_CACHE_SHIFT,
 	       MAX_DIO_SIZE >> PAGE_CACHE_SHIFT);
@@ -529,9 +529,9 @@ static int ll_write_end(struct file *file, struct address_space *mapping,
 }
 
 #ifdef CONFIG_MIGRATION
-int ll_migratepage(struct address_space *mapping,
-		struct page *newpage, struct page *page
-		, enum migrate_mode mode
+static int ll_migratepage(struct address_space *mapping,
+			 struct page *newpage, struct page *page,
+			 enum migrate_mode mode
 		)
 {
 	/* Always fail page migration until we have a proper implementation */
@@ -540,9 +540,8 @@ int ll_migratepage(struct address_space *mapping,
 #endif
 
 #ifndef MS_HAS_NEW_AOPS
-struct address_space_operations ll_aops = {
-	.readpage       = ll_readpage,
-//	.readpages      = ll_readpages,
+const struct address_space_operations ll_aops = {
+	.readpage	= ll_readpage,
 	.direct_IO      = ll_direct_IO_26,
 	.writepage      = ll_writepage,
 	.writepages     = ll_writepages,
@@ -554,10 +553,9 @@ struct address_space_operations ll_aops = {
 #ifdef CONFIG_MIGRATION
 	.migratepage    = ll_migratepage,
 #endif
-	.bmap	   = NULL
 };
 #else
-struct address_space_operations_ext ll_aops = {
+const struct address_space_operations_ext ll_aops = {
 	.orig_aops.readpage       = ll_readpage,
 //	.orig_aops.readpages      = ll_readpages,
 	.orig_aops.direct_IO      = ll_direct_IO_26,
@@ -571,7 +569,6 @@ struct address_space_operations_ext ll_aops = {
 #ifdef CONFIG_MIGRATION
 	.orig_aops.migratepage    = ll_migratepage,
 #endif
-	.orig_aops.bmap	   = NULL,
 	.write_begin    = ll_write_begin,
 	.write_end      = ll_write_end
 };
diff --git a/drivers/staging/lustre/lustre/llite/statahead.c b/drivers/staging/lustre/lustre/llite/statahead.c
index c8624b5a987..1b47774d744 100644
--- a/drivers/staging/lustre/lustre/llite/statahead.c
+++ b/drivers/staging/lustre/lustre/llite/statahead.c
@@ -958,13 +958,18 @@ static int ll_agl_thread(void *arg)
 	struct ptlrpc_thread     *thread = &sai->sai_agl_thread;
 	struct l_wait_info	lwi    = { 0 };
 
-	CDEBUG(D_READA, "agl thread started: [pid %d] [parent %.*s]\n",
-	       current_pid(), parent->d_name.len, parent->d_name.name);
+	thread->t_pid = current_pid();
+	CDEBUG(D_READA, "agl thread started: sai %p, parent %.*s\n",
+	       sai, parent->d_name.len, parent->d_name.name);
 
 	atomic_inc(&sbi->ll_agl_total);
 	spin_lock(&plli->lli_agl_lock);
 	sai->sai_agl_valid = 1;
-	thread_set_flags(thread, SVC_RUNNING);
+	if (thread_is_init(thread))
+		/* If someone else has changed the thread state
+		 * (e.g. already changed to SVC_STOPPING), we can't just
+		 * blindly overwrite that setting. */
+		thread_set_flags(thread, SVC_RUNNING);
 	spin_unlock(&plli->lli_agl_lock);
 	wake_up(&thread->t_ctl_waitq);
 
@@ -1004,8 +1009,8 @@ static int ll_agl_thread(void *arg)
 	spin_unlock(&plli->lli_agl_lock);
 	wake_up(&thread->t_ctl_waitq);
 	ll_sai_put(sai);
-	CDEBUG(D_READA, "agl thread stopped: [pid %d] [parent %.*s]\n",
-	       current_pid(), parent->d_name.len, parent->d_name.name);
+	CDEBUG(D_READA, "agl thread stopped: sai %p, parent %.*s\n",
+	       sai, parent->d_name.len, parent->d_name.name);
 	return 0;
 }
 
@@ -1016,8 +1021,8 @@ static void ll_start_agl(struct dentry *parent, struct ll_statahead_info *sai)
 	struct ll_inode_info  *plli;
 	struct task_struct *task;
 
-	CDEBUG(D_READA, "start agl thread: [pid %d] [parent %.*s]\n",
-	       current_pid(), parent->d_name.len, parent->d_name.name);
+	CDEBUG(D_READA, "start agl thread: sai %p, parent %.*s\n",
+	       sai, parent->d_name.len, parent->d_name.name);
 
 	plli = ll_i2info(parent->d_inode);
 	task = kthread_run(ll_agl_thread, parent,
@@ -1050,15 +1055,20 @@ static int ll_statahead_thread(void *arg)
 	struct ll_dir_chain       chain;
 	struct l_wait_info	lwi    = { 0 };
 
-	CDEBUG(D_READA, "statahead thread started: [pid %d] [parent %.*s]\n",
-	       current_pid(), parent->d_name.len, parent->d_name.name);
+	thread->t_pid = current_pid();
+	CDEBUG(D_READA, "statahead thread starting: sai %p, parent %.*s\n",
+	       sai, parent->d_name.len, parent->d_name.name);
 
 	if (sbi->ll_flags & LL_SBI_AGL_ENABLED)
 		ll_start_agl(parent, sai);
 
 	atomic_inc(&sbi->ll_sa_total);
 	spin_lock(&plli->lli_sa_lock);
-	thread_set_flags(thread, SVC_RUNNING);
+	if (thread_is_init(thread))
+		/* If someone else has changed the thread state
+		 * (e.g. already changed to SVC_STOPPING), we can't just
+		 * blindly overwrite that setting. */
+		thread_set_flags(thread, SVC_RUNNING);
 	spin_unlock(&plli->lli_sa_lock);
 	wake_up(&thread->t_ctl_waitq);
 
@@ -1220,9 +1230,7 @@ do_it:
 			 */
 			ll_release_page(page, le32_to_cpu(dp->ldp_flags) &
 					      LDF_COLLIDE);
-			sai->sai_in_readpage = 1;
 			page = ll_get_dir_page(dir, pos, &chain);
-			sai->sai_in_readpage = 0;
 		} else {
 			LASSERT(le32_to_cpu(dp->ldp_flags) & LDF_COLLIDE);
 			ll_release_page(page, 1);
@@ -1239,8 +1247,8 @@ out:
 		spin_unlock(&plli->lli_agl_lock);
 		wake_up(&agl_thread->t_ctl_waitq);
 
-		CDEBUG(D_READA, "stop agl thread: [pid %d]\n",
-		       current_pid());
+		CDEBUG(D_READA, "stop agl thread: sai %p pid %u\n",
+		       sai, (unsigned int)agl_thread->t_pid);
 		l_wait_event(agl_thread->t_ctl_waitq,
 			     thread_is_stopped(agl_thread),
 			     &lwi);
@@ -1266,8 +1274,8 @@ out:
 	wake_up(&thread->t_ctl_waitq);
 	ll_sai_put(sai);
 	dput(parent);
-	CDEBUG(D_READA, "statahead thread stopped: [pid %d] [parent %.*s]\n",
-	       current_pid(), parent->d_name.len, parent->d_name.name);
+	CDEBUG(D_READA, "statahead thread stopped: sai %p, parent %.*s\n",
+	       sai, parent->d_name.len, parent->d_name.name);
 	return rc;
 }
 
@@ -1298,8 +1306,8 @@ void ll_stop_statahead(struct inode *dir, void *key)
 			spin_unlock(&lli->lli_sa_lock);
 			wake_up(&thread->t_ctl_waitq);
 
-			CDEBUG(D_READA, "stop statahead thread: [pid %d]\n",
-			       current_pid());
+			CDEBUG(D_READA, "stop statahead thread: sai %p pid %u\n",
+			       lli->lli_sai, (unsigned int)thread->t_pid);
 			l_wait_event(thread->t_ctl_waitq,
 				     thread_is_stopped(thread),
 				     &lwi);
@@ -1473,10 +1481,10 @@ ll_sai_unplug(struct ll_statahead_info *sai, struct ll_sa_entry *entry)
 			CDEBUG(D_READA, "Statahead for dir "DFID" hit "
 			       "ratio too low: hit/miss "LPU64"/"LPU64
 			       ", sent/replied "LPU64"/"LPU64", stopping "
-			       "statahead thread: pid %d\n",
+			       "statahead thread\n",
 			       PFID(&lli->lli_fid), sai->sai_hit,
 			       sai->sai_miss, sai->sai_sent,
-			       sai->sai_replied, current_pid());
+			       sai->sai_replied);
 			spin_lock(&lli->lli_sa_lock);
 			if (!thread_is_stopped(thread))
 				thread_set_flags(thread, SVC_STOPPING);
@@ -1553,12 +1561,6 @@ int do_statahead_enter(struct inode *dir, struct dentry **dentryp,
 			return entry ? 1 : -EAGAIN;
 		}
 
-		/* if statahead is busy in readdir, help it do post-work */
-		while (!ll_sa_entry_stated(entry) &&
-		       sai->sai_in_readpage &&
-		       !sa_received_empty(sai))
-			ll_post_statahead(sai);
-
 		if (!ll_sa_entry_stated(entry)) {
 			sai->sai_index_wait = entry->se_index;
 			lwi = LWI_TIMEOUT_INTR(cfs_time_seconds(30), NULL,
@@ -1655,9 +1657,15 @@ int do_statahead_enter(struct inode *dir, struct dentry **dentryp,
 		GOTO(out, rc = -EAGAIN);
 	}
 
-	CDEBUG(D_READA, "start statahead thread: [pid %d] [parent %.*s]\n",
-	       current_pid(), parent->d_name.len, parent->d_name.name);
+	CDEBUG(D_READA, "start statahead thread: sai %p, parent %.*s\n",
+	       sai, parent->d_name.len, parent->d_name.name);
 
+	/* The sai buffer already has one reference taken at allocation time,
+	 * but as soon as we expose the sai by attaching it to the lli that
+	 * default reference can be dropped by another thread calling
+	 * ll_stop_statahead. We need to take a local reference to protect
+	 * the sai buffer while we intend to access it. */
+	ll_sai_get(sai);
 	lli->lli_sai = sai;
 
 	plli = ll_i2info(parent->d_inode);
@@ -1670,6 +1678,9 @@ int do_statahead_enter(struct inode *dir, struct dentry **dentryp,
 		lli->lli_opendir_key = NULL;
 		thread_set_flags(thread, SVC_STOPPED);
 		thread_set_flags(&sai->sai_agl_thread, SVC_STOPPED);
+		/* Drop both our own local reference and the default
+		 * reference from allocation time. */
+		ll_sai_put(sai);
 		ll_sai_put(sai);
 		LASSERT(lli->lli_sai == NULL);
 		return -EAGAIN;
@@ -1678,6 +1689,7 @@ int do_statahead_enter(struct inode *dir, struct dentry **dentryp,
 	l_wait_event(thread->t_ctl_waitq,
 		     thread_is_running(thread) || thread_is_stopped(thread),
 		     &lwi);
+	ll_sai_put(sai);
 
 	/*
 	 * We don't stat-ahead for the first dirent since we are already in
diff --git a/drivers/staging/lustre/lustre/llite/super25.c b/drivers/staging/lustre/lustre/llite/super25.c
index e21e1c760a8..951fdb1265b 100644
--- a/drivers/staging/lustre/lustre/llite/super25.c
+++ b/drivers/staging/lustre/lustre/llite/super25.c
@@ -52,7 +52,7 @@ static struct inode *ll_alloc_inode(struct super_block *sb)
 {
 	struct ll_inode_info *lli;
 	ll_stats_ops_tally(ll_s2sbi(sb), LPROC_LL_ALLOC_INODE, 1);
-	OBD_SLAB_ALLOC_PTR_GFP(lli, ll_inode_cachep, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(lli, ll_inode_cachep, GFP_NOFS);
 	if (lli == NULL)
 		return NULL;
 
@@ -72,7 +72,7 @@ static void ll_destroy_inode(struct inode *inode)
 	call_rcu(&inode->i_rcu, ll_inode_destroy_callback);
 }
 
-int ll_init_inodecache(void)
+static int ll_init_inodecache(void)
 {
 	ll_inode_cachep = kmem_cache_create("lustre_inode_cache",
 					       sizeof(struct ll_inode_info),
@@ -82,7 +82,7 @@ int ll_init_inodecache(void)
 	return 0;
 }
 
-void ll_destroy_inodecache(void)
+static void ll_destroy_inodecache(void)
 {
 	kmem_cache_destroy(ll_inode_cachep);
 }
@@ -103,9 +103,6 @@ MODULE_ALIAS_FS("lustre");
 
 void lustre_register_client_process_config(int (*cpc)(struct lustre_cfg *lcfg));
 
-int vvp_global_init(void);
-void vvp_global_fini(void);
-
 static int __init init_lustre_lite(void)
 {
 	int i, rc, seed[2];
diff --git a/drivers/staging/lustre/lustre/llite/symlink.c b/drivers/staging/lustre/lustre/llite/symlink.c
index 80d48b5ae24..129d3023dce 100644
--- a/drivers/staging/lustre/lustre/llite/symlink.c
+++ b/drivers/staging/lustre/lustre/llite/symlink.c
@@ -120,7 +120,7 @@ static void *ll_follow_link(struct dentry *dentry, struct nameidata *nd)
 	struct inode *inode = dentry->d_inode;
 	struct ptlrpc_request *request = NULL;
 	int rc;
-	char *symname;
+	char *symname = NULL;
 
 	CDEBUG(D_VFSTRACE, "VFS Op\n");
 	/* Limit the recursive symlink depth to 5 instead of default
diff --git a/drivers/staging/lustre/lustre/llite/vvp_dev.c b/drivers/staging/lustre/lustre/llite/vvp_dev.c
index c4d1580b7be..0f68c16abe3 100644
--- a/drivers/staging/lustre/lustre/llite/vvp_dev.c
+++ b/drivers/staging/lustre/lustre/llite/vvp_dev.c
@@ -43,7 +43,7 @@
 
 #include <obd.h>
 #include <lustre_lite.h>
-
+#include "llite_internal.h"
 #include "vvp_internal.h"
 
 /*****************************************************************************
@@ -57,7 +57,7 @@
  * "llite_" (var. "ll_") prefix.
  */
 
-struct kmem_cache *vvp_thread_kmem;
+static struct kmem_cache *vvp_thread_kmem;
 static struct kmem_cache *vvp_session_kmem;
 static struct lu_kmem_descr vvp_caches[] = {
 	{
@@ -80,7 +80,7 @@ static void *vvp_key_init(const struct lu_context *ctx,
 {
 	struct vvp_thread_info *info;
 
-	OBD_SLAB_ALLOC_PTR_GFP(info, vvp_thread_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(info, vvp_thread_kmem, GFP_NOFS);
 	if (info == NULL)
 		info = ERR_PTR(-ENOMEM);
 	return info;
@@ -98,7 +98,7 @@ static void *vvp_session_key_init(const struct lu_context *ctx,
 {
 	struct vvp_session *session;
 
-	OBD_SLAB_ALLOC_PTR_GFP(session, vvp_session_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(session, vvp_session_kmem, GFP_NOFS);
 	if (session == NULL)
 		session = ERR_PTR(-ENOMEM);
 	return session;
@@ -536,7 +536,7 @@ static int vvp_dump_pgcache_seq_open(struct inode *inode, struct file *filp)
 	return result;
 }
 
-struct file_operations vvp_dump_pgcache_file_ops = {
+const struct file_operations vvp_dump_pgcache_file_ops = {
 	.owner   = THIS_MODULE,
 	.open    = vvp_dump_pgcache_seq_open,
 	.read    = seq_read,
diff --git a/drivers/staging/lustre/lustre/llite/vvp_internal.h b/drivers/staging/lustre/lustre/llite/vvp_internal.h
index c82bf17f55a..3c9a03d9955 100644
--- a/drivers/staging/lustre/lustre/llite/vvp_internal.h
+++ b/drivers/staging/lustre/lustre/llite/vvp_internal.h
@@ -57,6 +57,6 @@ struct lu_object *vvp_object_alloc(const struct lu_env *env,
 
 struct ccc_object *cl_inode2ccc(struct inode *inode);
 
-extern struct kmem_cache *vvp_thread_kmem;
+extern const struct file_operations vvp_dump_pgcache_file_ops;
 
 #endif /* VVP_INTERNAL_H */
diff --git a/drivers/staging/lustre/lustre/llite/vvp_io.c b/drivers/staging/lustre/lustre/llite/vvp_io.c
index c7d70091246..7dd2b4723c5 100644
--- a/drivers/staging/lustre/lustre/llite/vvp_io.c
+++ b/drivers/staging/lustre/lustre/llite/vvp_io.c
@@ -80,7 +80,7 @@ static bool can_populate_pages(const struct lu_env *env, struct cl_io *io,
 	case CIT_WRITE:
 		/* don't need lock here to check lli_layout_gen as we have held
 		 * extent lock and GROUP lock has to hold to swap layout */
-		if (lli->lli_layout_gen != cio->cui_layout_gen) {
+		if (ll_layout_version_get(lli) != cio->cui_layout_gen) {
 			io->ci_need_restart = 1;
 			/* this will return application a short read/write */
 			io->ci_continue = 0;
@@ -190,7 +190,7 @@ static void vvp_io_fault_fini(const struct lu_env *env,
 	vvp_io_fini(env, ios);
 }
 
-enum cl_lock_mode vvp_mode_from_vma(struct vm_area_struct *vma)
+static enum cl_lock_mode vvp_mode_from_vma(struct vm_area_struct *vma)
 {
 	/*
 	 * we only want to hold PW locks if the mmap() can generate
diff --git a/drivers/staging/lustre/lustre/llite/vvp_object.c b/drivers/staging/lustre/lustre/llite/vvp_object.c
index 25973dedd9a..65b6db1b71b 100644
--- a/drivers/staging/lustre/lustre/llite/vvp_object.c
+++ b/drivers/staging/lustre/lustre/llite/vvp_object.c
@@ -120,13 +120,28 @@ static int vvp_attr_set(const struct lu_env *env, struct cl_object *obj,
 	return 0;
 }
 
-int vvp_conf_set(const struct lu_env *env, struct cl_object *obj,
-		const struct cl_object_conf *conf)
+static int vvp_conf_set(const struct lu_env *env, struct cl_object *obj,
+			const struct cl_object_conf *conf)
 {
 	struct ll_inode_info *lli = ll_i2info(conf->coc_inode);
 
 	if (conf->coc_opc == OBJECT_CONF_INVALIDATE) {
-		lli->lli_layout_gen = LL_LAYOUT_GEN_NONE;
+		CDEBUG(D_VFSTRACE, DFID ": losing layout lock\n",
+		       PFID(&lli->lli_fid));
+
+		ll_layout_version_set(lli, LL_LAYOUT_GEN_NONE);
+
+		/* Clean up page mmap for this inode.
+		 * The reason for us to do this is that if the page has
+		 * already been installed into memory space, the process
+		 * can access it without interacting with lustre, so this
+		 * page may be stale due to layout change, and the process
+		 * will never be notified.
+		 * This operation is expensive but mmap processes have to pay
+		 * a price themselves. */
+		unmap_mapping_range(conf->coc_inode->i_mapping,
+				    0, OBD_OBJECT_EOF, 0);
+
 		return 0;
 	}
 
@@ -134,18 +149,18 @@ int vvp_conf_set(const struct lu_env *env, struct cl_object *obj,
 		return 0;
 
 	if (conf->u.coc_md != NULL && conf->u.coc_md->lsm != NULL) {
-		CDEBUG(D_VFSTRACE, "layout lock change: %u -> %u\n",
-			lli->lli_layout_gen,
-			conf->u.coc_md->lsm->lsm_layout_gen);
+		CDEBUG(D_VFSTRACE, DFID ": layout version change: %u -> %u\n",
+		       PFID(&lli->lli_fid), lli->lli_layout_gen,
+		       conf->u.coc_md->lsm->lsm_layout_gen);
 
 		lli->lli_has_smd = lsm_has_objects(conf->u.coc_md->lsm);
-		lli->lli_layout_gen = conf->u.coc_md->lsm->lsm_layout_gen;
+		ll_layout_version_set(lli, conf->u.coc_md->lsm->lsm_layout_gen);
 	} else {
-		CDEBUG(D_VFSTRACE, "layout lock destroyed: %u.\n",
-			lli->lli_layout_gen);
+		CDEBUG(D_VFSTRACE, DFID ": layout nuked: %u.\n",
+		       PFID(&lli->lli_fid), lli->lli_layout_gen);
 
 		lli->lli_has_smd = false;
-		lli->lli_layout_gen = LL_LAYOUT_GEN_EMPTY;
+		ll_layout_version_set(lli, LL_LAYOUT_GEN_EMPTY);
 	}
 	return 0;
 }
diff --git a/drivers/staging/lustre/lustre/llite/xattr.c b/drivers/staging/lustre/lustre/llite/xattr.c
index b1ed4d9ea6b..c6c27bbb43b 100644
--- a/drivers/staging/lustre/lustre/llite/xattr.c
+++ b/drivers/staging/lustre/lustre/llite/xattr.c
@@ -112,8 +112,8 @@ int ll_setxattr_common(struct inode *inode, const char *name,
 	struct ptlrpc_request *req = NULL;
 	int xattr_type, rc;
 	struct obd_capa *oc;
-	struct rmtacl_ctl_entry *rce = NULL;
 #ifdef CONFIG_FS_POSIX_ACL
+	struct rmtacl_ctl_entry *rce = NULL;
 	posix_acl_xattr_header *new_value = NULL;
 	ext_acl_xattr_header *acl = NULL;
 #endif
@@ -124,6 +124,11 @@ int ll_setxattr_common(struct inode *inode, const char *name,
 	if (rc)
 		return rc;
 
+	if ((xattr_type == XATTR_ACL_ACCESS_T ||
+	     xattr_type == XATTR_ACL_DEFAULT_T) &&
+	    !inode_owner_or_capable(inode))
+		return -EPERM;
+
 	/* b10667: ignore lustre special xattr for now */
 	if ((xattr_type == XATTR_TRUSTED_T && strcmp(name, "trusted.lov") == 0) ||
 	    (xattr_type == XATTR_LUSTRE_T && strcmp(name, "lustre.lov") == 0))
diff --git a/drivers/staging/lustre/lustre/llite/xattr_cache.c b/drivers/staging/lustre/lustre/llite/xattr_cache.c
index 4defa2fd83b..4dd83fc0368 100644
--- a/drivers/staging/lustre/lustre/llite/xattr_cache.c
+++ b/drivers/staging/lustre/lustre/llite/xattr_cache.c
@@ -120,7 +120,7 @@ static int ll_xattr_cache_add(struct list_head *cache,
 		return -EPROTO;
 	}
 
-	OBD_SLAB_ALLOC_PTR_GFP(xattr, xattr_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(xattr, xattr_kmem, GFP_NOFS);
 	if (xattr == NULL) {
 		CDEBUG(D_CACHE, "failed to allocate xattr\n");
 		return -ENOMEM;
@@ -232,7 +232,7 @@ static int ll_xattr_cache_list(struct list_head *cache,
  * \retval 0 @cache is not initialized
  * \retval 1 @cache is initialized
  */
-int ll_xattr_cache_valid(struct ll_inode_info *lli)
+static int ll_xattr_cache_valid(struct ll_inode_info *lli)
 {
 	return !!(lli->lli_flags & LLIF_XATTR_CACHE);
 }
diff --git a/drivers/staging/lustre/lustre/lmv/lmv_obd.c b/drivers/staging/lustre/lustre/lmv/lmv_obd.c
index 3ba0a0a1d94..4edf8a31221 100644
--- a/drivers/staging/lustre/lustre/lmv/lmv_obd.c
+++ b/drivers/staging/lustre/lustre/lmv/lmv_obd.c
@@ -280,7 +280,7 @@ static void lmv_set_timeouts(struct obd_device *obd)
 }
 
 static int lmv_init_ea_size(struct obd_export *exp, int easize,
-			    int def_easize, int cookiesize)
+			    int def_easize, int cookiesize, int def_cookiesize)
 {
 	struct obd_device   *obd = exp->exp_obd;
 	struct lmv_obd      *lmv = &obd->u.lmv;
@@ -300,6 +300,10 @@ static int lmv_init_ea_size(struct obd_export *exp, int easize,
 		lmv->max_cookiesize = cookiesize;
 		change = 1;
 	}
+	if (lmv->max_def_cookiesize < def_cookiesize) {
+		lmv->max_def_cookiesize = def_cookiesize;
+		change = 1;
+	}
 	if (change == 0)
 		return 0;
 
@@ -315,7 +319,7 @@ static int lmv_init_ea_size(struct obd_export *exp, int easize,
 		}
 
 		rc = md_init_ea_size(lmv->tgts[i]->ltd_exp, easize, def_easize,
-				     cookiesize);
+				     cookiesize, def_cookiesize);
 		if (rc) {
 			CERROR("%s: obd_init_ea_size() failed on MDT target %d:"
 			       " rc = %d.\n", obd->obd_name, i, rc);
@@ -400,8 +404,8 @@ int lmv_connect_mdc(struct obd_device *obd, struct lmv_tgt_desc *tgt)
 	tgt->ltd_exp = mdc_exp;
 	lmv->desc.ld_active_tgt_count++;
 
-	md_init_ea_size(tgt->ltd_exp, lmv->max_easize,
-			lmv->max_def_easize, lmv->max_cookiesize);
+	md_init_ea_size(tgt->ltd_exp, lmv->max_easize, lmv->max_def_easize,
+			lmv->max_cookiesize, lmv->max_def_cookiesize);
 
 	CDEBUG(D_CONFIG, "Connected to %s(%s) successfully (%d)\n",
 		mdc_obd->obd_name, mdc_obd->obd_uuid.uuid,
@@ -527,9 +531,8 @@ static int lmv_add_target(struct obd_device *obd, struct obd_uuid *uuidp,
 			spin_unlock(&lmv->lmv_lock);
 		} else {
 			int easize = sizeof(struct lmv_stripe_md) +
-				     lmv->desc.ld_tgt_count *
-				     sizeof(struct lu_fid);
-			lmv_init_ea_size(obd->obd_self_export, easize, 0, 0);
+				lmv->desc.ld_tgt_count * sizeof(struct lu_fid);
+			lmv_init_ea_size(obd->obd_self_export, easize, 0, 0, 0);
 		}
 	}
 
@@ -578,7 +581,7 @@ int lmv_check_connect(struct obd_device *obd)
 	class_export_put(lmv->exp);
 	lmv->connected = 1;
 	easize = lmv_get_easize(lmv);
-	lmv_init_ea_size(obd->obd_self_export, easize, 0, 0);
+	lmv_init_ea_size(obd->obd_self_export, easize, 0, 0, 0);
 	lmv_init_unlock(lmv);
 	return 0;
 
@@ -2340,7 +2343,11 @@ static int lmv_get_info(const struct lu_env *env, struct obd_export *exp,
 				return 0;
 		}
 		return -EINVAL;
-	} else if (KEY_IS(KEY_MAX_EASIZE) || KEY_IS(KEY_CONN_DATA)) {
+	} else if (KEY_IS(KEY_MAX_EASIZE) ||
+		   KEY_IS(KEY_DEFAULT_EASIZE) ||
+		   KEY_IS(KEY_MAX_COOKIESIZE) ||
+		   KEY_IS(KEY_DEFAULT_COOKIESIZE) ||
+		   KEY_IS(KEY_CONN_DATA)) {
 		rc = lmv_check_connect(obd);
 		if (rc)
 			return rc;
diff --git a/drivers/staging/lustre/lustre/lmv/lproc_lmv.c b/drivers/staging/lustre/lustre/lmv/lproc_lmv.c
index 5d5c3081c46..ae73c82ce49 100644
--- a/drivers/staging/lustre/lustre/lmv/lproc_lmv.c
+++ b/drivers/staging/lustre/lustre/lmv/lproc_lmv.c
@@ -200,19 +200,19 @@ static int lmv_target_seq_open(struct inode *inode, struct file *file)
 LPROC_SEQ_FOPS_RO_TYPE(lmv, uuid);
 
 struct lprocfs_vars lprocfs_lmv_obd_vars[] = {
-	{ "numobd",	  &lmv_numobd_fops,	  0, 0 },
-	{ "placement",	  &lmv_placement_fops,    0, 0 },
-	{ "activeobd",	  &lmv_activeobd_fops,    0, 0 },
-	{ "uuid",	  &lmv_uuid_fops,	  0, 0 },
-	{ "desc_uuid",	  &lmv_desc_uuid_fops,    0, 0 },
-	{ 0 }
+	{ "numobd",	  &lmv_numobd_fops,	  NULL, 0 },
+	{ "placement",	  &lmv_placement_fops,    NULL, 0 },
+	{ "activeobd",	  &lmv_activeobd_fops,    NULL, 0 },
+	{ "uuid",	  &lmv_uuid_fops,	  NULL, 0 },
+	{ "desc_uuid",	  &lmv_desc_uuid_fops,    NULL, 0 },
+	{ NULL }
 };
 
 LPROC_SEQ_FOPS_RO_TYPE(lmv, numrefs);
 
 static struct lprocfs_vars lprocfs_lmv_module_vars[] = {
-	{ "num_refs",	   &lmv_numrefs_fops, 0, 0 },
-	{ 0 }
+	{ "num_refs",	   &lmv_numrefs_fops, NULL, 0 },
+	{ NULL }
 };
 
 struct file_operations lmv_proc_target_fops = {
diff --git a/drivers/staging/lustre/lustre/lov/Makefile b/drivers/staging/lustre/lustre/lov/Makefile
index 9a5f26d5558..a908edb533d 100644
--- a/drivers/staging/lustre/lustre/lov/Makefile
+++ b/drivers/staging/lustre/lustre/lov/Makefile
@@ -1,5 +1,5 @@
 obj-$(CONFIG_LUSTRE_FS) += lov.o
-lov-y := lov_log.o lov_obd.o lov_pack.o lov_offset.o lov_merge.o \
+lov-y := lov_obd.o lov_pack.o lov_offset.o lov_merge.o \
 	 lov_request.o lov_ea.o lov_dev.o lov_object.o lov_page.o  \
 	 lov_lock.o lov_io.o lovsub_dev.o lovsub_object.o lovsub_page.o      \
 	 lovsub_lock.o lovsub_io.o lov_pool.o
diff --git a/drivers/staging/lustre/lustre/lov/lov_dev.c b/drivers/staging/lustre/lustre/lov/lov_dev.c
index 1f33b04b0c3..53e5781ba1d 100644
--- a/drivers/staging/lustre/lustre/lov/lov_dev.c
+++ b/drivers/staging/lustre/lustre/lov/lov_dev.c
@@ -143,7 +143,7 @@ static void *lov_key_init(const struct lu_context *ctx,
 {
 	struct lov_thread_info *info;
 
-	OBD_SLAB_ALLOC_PTR_GFP(info, lov_thread_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(info, lov_thread_kmem, GFP_NOFS);
 	if (info != NULL)
 		INIT_LIST_HEAD(&info->lti_closure.clc_list);
 	else
@@ -170,7 +170,7 @@ static void *lov_session_key_init(const struct lu_context *ctx,
 {
 	struct lov_session *info;
 
-	OBD_SLAB_ALLOC_PTR_GFP(info, lov_session_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(info, lov_session_kmem, GFP_NOFS);
 	if (info == NULL)
 		info = ERR_PTR(-ENOMEM);
 	return info;
@@ -260,7 +260,7 @@ static int lov_req_init(const struct lu_env *env, struct cl_device *dev,
 	struct lov_req *lr;
 	int result;
 
-	OBD_SLAB_ALLOC_PTR_GFP(lr, lov_req_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(lr, lov_req_kmem, GFP_NOFS);
 	if (lr != NULL) {
 		cl_req_slice_add(req, &lr->lr_cl, dev, &lov_req_ops);
 		result = 0;
diff --git a/drivers/staging/lustre/lustre/lov/lov_ea.c b/drivers/staging/lustre/lustre/lov/lov_ea.c
index 6f356e02554..a0c148e31f6 100644
--- a/drivers/staging/lustre/lustre/lov/lov_ea.c
+++ b/drivers/staging/lustre/lustre/lov/lov_ea.c
@@ -44,7 +44,6 @@
 #include <linux/libcfs/libcfs.h>
 
 #include <obd_class.h>
-#include <obd_lov.h>
 #include <lustre/lustre_idl.h>
 
 #include "lov_internal.h"
@@ -101,7 +100,7 @@ struct lov_stripe_md *lsm_alloc_plain(__u16 stripe_count, int *size)
 		return NULL;
 
 	for (i = 0; i < stripe_count; i++) {
-		OBD_SLAB_ALLOC_PTR_GFP(loi, lov_oinfo_slab, __GFP_IO);
+		OBD_SLAB_ALLOC_PTR_GFP(loi, lov_oinfo_slab, GFP_NOFS);
 		if (loi == NULL)
 			goto err;
 		lsm->lsm_oinfo[i] = loi;
@@ -346,3 +345,14 @@ const struct lsm_operations lsm_v3_ops = {
 	.lsm_lmm_verify	 = lsm_lmm_verify_v3,
 	.lsm_unpackmd	   = lsm_unpackmd_v3,
 };
+
+void dump_lsm(unsigned int level, const struct lov_stripe_md *lsm)
+{
+	CDEBUG(level, "lsm %p, objid "DOSTID", maxbytes "LPX64", magic 0x%08X,"
+	       " stripe_size %u, stripe_count %u, refc: %d,"
+	       " layout_gen %u, pool ["LOV_POOLNAMEF"]\n", lsm,
+	       POSTID(&lsm->lsm_oi), lsm->lsm_maxbytes, lsm->lsm_magic,
+	       lsm->lsm_stripe_size, lsm->lsm_stripe_count,
+	       atomic_read(&lsm->lsm_refc), lsm->lsm_layout_gen,
+	       lsm->lsm_pool_name);
+}
diff --git a/drivers/staging/lustre/lustre/lov/lov_internal.h b/drivers/staging/lustre/lustre/lov/lov_internal.h
index 2b22a03c038..38508a5c827 100644
--- a/drivers/staging/lustre/lustre/lov/lov_internal.h
+++ b/drivers/staging/lustre/lustre/lov/lov_internal.h
@@ -38,9 +38,39 @@
 #define LOV_INTERNAL_H
 
 #include <obd_class.h>
-#include <obd_lov.h>
 #include <lustre/lustre_user.h>
 
+/* lov_do_div64(a, b) returns a % b, and a = a / b.
+ * The 32-bit code is LOV-specific due to knowing about stripe limits in
+ * order to reduce the divisor to a 32-bit number.  If the divisor is
+ * already a 32-bit value the compiler handles this directly. */
+#if BITS_PER_LONG == 64
+# define lov_do_div64(n, base) ({					\
+	uint64_t __base = (base);					\
+	uint64_t __rem;							\
+	__rem = ((uint64_t)(n)) % __base;				\
+	(n) = ((uint64_t)(n)) / __base;					\
+	__rem;								\
+})
+#elif BITS_PER_LONG == 32
+# define lov_do_div64(n, base) ({					\
+	uint64_t __rem;							\
+	if ((sizeof(base) > 4) && (((base) & 0xffffffff00000000ULL) != 0)) {  \
+		int __remainder;					      \
+		LASSERTF(!((base) & (LOV_MIN_STRIPE_SIZE - 1)), "64 bit lov " \
+			 "division %llu / %llu\n", (n), (uint64_t)(base));    \
+		__remainder = (n) & (LOV_MIN_STRIPE_SIZE - 1);		\
+		(n) >>= LOV_MIN_STRIPE_BITS;				\
+		__rem = do_div(n, (base) >> LOV_MIN_STRIPE_BITS);	\
+		__rem <<= LOV_MIN_STRIPE_BITS;				\
+		__rem += __remainder;					\
+	} else {							\
+		__rem = do_div(n, base);				\
+	}								\
+	__rem;								\
+})
+#endif
+
 struct lov_lock_handles {
 	struct portals_handle   llh_handle;
 	atomic_t	    llh_refcount;
@@ -251,10 +281,6 @@ int lov_process_config_base(struct obd_device *obd, struct lustre_cfg *lcfg,
 			    __u32 *indexp, int *genp);
 int lov_del_target(struct obd_device *obd, __u32 index,
 		   struct obd_uuid *uuidp, int gen);
-/* lov_log.c */
-int lov_llog_init(struct obd_device *obd, struct obd_llog_group *olg,
-		  struct obd_device *tgt, int *idx);
-int lov_llog_finish(struct obd_device *obd, int count);
 
 /* lov_pack.c */
 int lov_packmd(struct obd_export *exp, struct lov_mds_md **lmm,
@@ -279,6 +305,7 @@ void lov_dump_lmm(int level, void *lmm);
 /* lov_ea.c */
 struct lov_stripe_md *lsm_alloc_plain(__u16 stripe_count, int *size);
 void lsm_free_plain(struct lov_stripe_md *lsm);
+void dump_lsm(unsigned int level, const struct lov_stripe_md *lsm);
 
 int lovea_destroy_object(struct lov_obd *lov, struct lov_stripe_md *lsm,
 			 struct obdo *oa, void *data);
diff --git a/drivers/staging/lustre/lustre/lov/lov_lock.c b/drivers/staging/lustre/lustre/lov/lov_lock.c
index ed2726e523e..08ac3745f0d 100644
--- a/drivers/staging/lustre/lustre/lov/lov_lock.c
+++ b/drivers/staging/lustre/lustre/lov/lov_lock.c
@@ -144,7 +144,7 @@ static struct cl_lock *lov_sublock_alloc(const struct lu_env *env,
 
 	LASSERT(idx < lck->lls_nr);
 
-	OBD_SLAB_ALLOC_PTR_GFP(link, lov_lock_link_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(link, lov_lock_link_kmem, GFP_NOFS);
 	if (link != NULL) {
 		struct lov_sublock_env *subenv;
 		struct lov_lock_sub  *lls;
@@ -346,41 +346,7 @@ static int lov_lock_sub_init(const struct lu_env *env,
 		}
 	}
 	LASSERT(nr == lck->lls_nr);
-	/*
-	 * Then, create sub-locks. Once at least one sub-lock was created,
-	 * top-lock can be reached by other threads.
-	 */
-	for (i = 0; i < lck->lls_nr; ++i) {
-		struct cl_lock       *sublock;
-		struct lov_lock_link *link;
 
-		if (lck->lls_sub[i].sub_lock == NULL) {
-			sublock = lov_sublock_alloc(env, io, lck, i, &link);
-			if (IS_ERR(sublock)) {
-				result = PTR_ERR(sublock);
-				break;
-			}
-			cl_lock_get_trust(sublock);
-			cl_lock_mutex_get(env, sublock);
-			cl_lock_mutex_get(env, parent);
-			/*
-			 * recheck under mutex that sub-lock wasn't created
-			 * concurrently, and that top-lock is still alive.
-			 */
-			if (lck->lls_sub[i].sub_lock == NULL &&
-			    parent->cll_state < CLS_FREEING) {
-				lov_sublock_adopt(env, lck, sublock, i, link);
-				cl_lock_mutex_put(env, parent);
-			} else {
-				OBD_SLAB_FREE_PTR(link, lov_lock_link_kmem);
-				cl_lock_mutex_put(env, parent);
-				cl_lock_unhold(env, sublock,
-					       "lov-parent", parent);
-			}
-			cl_lock_mutex_put(env, sublock);
-			cl_lock_put(env, sublock);
-		}
-	}
 	/*
 	 * Some sub-locks can be missing at this point. This is not a problem,
 	 * because enqueue will create them anyway. Main duty of this function
@@ -533,7 +499,7 @@ static int lov_lock_enqueue_one(const struct lu_env *env, struct lov_lock *lck,
 static int lov_sublock_fill(const struct lu_env *env, struct cl_lock *parent,
 			    struct cl_io *io, struct lov_lock *lck, int idx)
 {
-	struct lov_lock_link *link;
+	struct lov_lock_link *link = NULL;
 	struct cl_lock       *sublock;
 	int		   result;
 
@@ -1159,7 +1125,7 @@ int lov_lock_init_raid0(const struct lu_env *env, struct cl_object *obj,
 	struct lov_lock *lck;
 	int result;
 
-	OBD_SLAB_ALLOC_PTR_GFP(lck, lov_lock_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(lck, lov_lock_kmem, GFP_NOFS);
 	if (lck != NULL) {
 		cl_lock_slice_add(lock, &lck->lls_cl, obj, &lov_lock_ops);
 		result = lov_lock_sub_init(env, lck, io);
@@ -1194,7 +1160,7 @@ int lov_lock_init_empty(const struct lu_env *env, struct cl_object *obj,
 	struct lov_lock *lck;
 	int result = -ENOMEM;
 
-	OBD_SLAB_ALLOC_PTR_GFP(lck, lov_lock_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(lck, lov_lock_kmem, GFP_NOFS);
 	if (lck != NULL) {
 		cl_lock_slice_add(lock, &lck->lls_cl, obj, &lov_empty_lock_ops);
 		lck->lls_orig = lock->cll_descr;
diff --git a/drivers/staging/lustre/lustre/lov/lov_log.c b/drivers/staging/lustre/lustre/lov/lov_log.c
deleted file mode 100644
index 3eedd935d1b..00000000000
--- a/drivers/staging/lustre/lustre/lov/lov_log.c
+++ /dev/null
@@ -1,272 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * 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 version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- *
- * Copyright (c) 2012, Intel Corporation.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- *
- * lustre/lov/lov_log.c
- *
- * Author: Phil Schwan <phil@clusterfs.com>
- * Author: Peter Braam <braam@clusterfs.com>
- * Author: Mike Shaver <shaver@clusterfs.com>
- */
-
-#define DEBUG_SUBSYSTEM S_LOV
-#include <linux/libcfs/libcfs.h>
-
-#include <obd_support.h>
-#include <lustre_lib.h>
-#include <lustre_net.h>
-#include <lustre/lustre_idl.h>
-#include <lustre_dlm.h>
-#include <lustre_mds.h>
-#include <obd_class.h>
-#include <obd_lov.h>
-#include <obd_ost.h>
-#include <lprocfs_status.h>
-#include <lustre_log.h>
-
-#include "lov_internal.h"
-
-/* Add log records for each OSC that this object is striped over, and return
- * cookies for each one.  We _would_ have nice abstraction here, except that
- * we need to keep cookies in stripe order, even if some are NULL, so that
- * the right cookies are passed back to the right OSTs at the client side.
- * Unset cookies should be all-zero (which will never occur naturally). */
-static int lov_llog_origin_add(const struct lu_env *env,
-			       struct llog_ctxt *ctxt,
-			       struct llog_rec_hdr *rec,
-			       struct lov_stripe_md *lsm,
-			       struct llog_cookie *logcookies, int numcookies)
-{
-	struct obd_device *obd = ctxt->loc_obd;
-	struct lov_obd *lov = &obd->u.lov;
-	int i, rc = 0, cookies = 0;
-
-	LASSERTF(logcookies && numcookies >= lsm->lsm_stripe_count,
-		 "logcookies %p, numcookies %d lsm->lsm_stripe_count %d \n",
-		 logcookies, numcookies, lsm->lsm_stripe_count);
-
-	for (i = 0; i < lsm->lsm_stripe_count; i++) {
-		struct lov_oinfo *loi = lsm->lsm_oinfo[i];
-		struct obd_device *child =
-			lov->lov_tgts[loi->loi_ost_idx]->ltd_exp->exp_obd;
-		struct llog_ctxt *cctxt = llog_get_context(child, ctxt->loc_idx);
-
-		/* fill mds unlink/setattr log record */
-		switch (rec->lrh_type) {
-		case MDS_UNLINK_REC: {
-			struct llog_unlink_rec *lur = (struct llog_unlink_rec *)rec;
-			lur->lur_oid = ostid_id(&loi->loi_oi);
-			lur->lur_oseq = (__u32)ostid_seq(&loi->loi_oi);
-			break;
-		}
-		case MDS_SETATTR64_REC: {
-			struct llog_setattr64_rec *lsr = (struct llog_setattr64_rec *)rec;
-			lsr->lsr_oi = loi->loi_oi;
-			break;
-		}
-		default:
-			break;
-		}
-
-		/* inject error in llog_obd_add() below */
-		if (OBD_FAIL_CHECK(OBD_FAIL_MDS_FAIL_LOV_LOG_ADD)) {
-			llog_ctxt_put(cctxt);
-			cctxt = NULL;
-		}
-		rc = llog_obd_add(env, cctxt, rec, NULL, logcookies + cookies,
-				  numcookies - cookies);
-		llog_ctxt_put(cctxt);
-		if (rc < 0) {
-			CERROR("Can't add llog (rc = %d) for stripe %d\n",
-			       rc, cookies);
-			memset(logcookies + cookies, 0,
-			       sizeof(struct llog_cookie));
-			rc = 1; /* skip this cookie */
-		}
-		/* Note that rc is always 1 if llog_obd_add was successful */
-		cookies += rc;
-	}
-	return cookies;
-}
-
-static int lov_llog_origin_connect(struct llog_ctxt *ctxt,
-				   struct llog_logid *logid,
-				   struct llog_gen *gen,
-				   struct obd_uuid *uuid)
-{
-	struct obd_device *obd = ctxt->loc_obd;
-	struct lov_obd *lov = &obd->u.lov;
-	int i, rc = 0, err = 0;
-
-	obd_getref(obd);
-	for (i = 0; i < lov->desc.ld_tgt_count; i++) {
-		struct obd_device *child;
-		struct llog_ctxt *cctxt;
-
-		if (!lov->lov_tgts[i] || !lov->lov_tgts[i]->ltd_active)
-			continue;
-		if (uuid && !obd_uuid_equals(uuid, &lov->lov_tgts[i]->ltd_uuid))
-			continue;
-		CDEBUG(D_CONFIG, "connect %d/%d\n", i, lov->desc.ld_tgt_count);
-		child = lov->lov_tgts[i]->ltd_exp->exp_obd;
-		cctxt = llog_get_context(child, ctxt->loc_idx);
-		rc = llog_connect(cctxt, logid, gen, uuid);
-		llog_ctxt_put(cctxt);
-
-		if (rc) {
-			CERROR("error osc_llog_connect tgt %d (%d)\n", i, rc);
-			if (!err)
-				err = rc;
-		}
-	}
-	obd_putref(obd);
-
-	return err;
-}
-
-/* the replicators commit callback */
-static int lov_llog_repl_cancel(const struct lu_env *env,
-				struct llog_ctxt *ctxt,
-				struct lov_stripe_md *lsm,
-				int count, struct llog_cookie *cookies,
-				int flags)
-{
-	struct lov_obd *lov;
-	struct obd_device *obd = ctxt->loc_obd;
-	int rc = 0, i;
-
-	LASSERT(lsm != NULL);
-	LASSERT(count == lsm->lsm_stripe_count);
-
-	lov = &obd->u.lov;
-	obd_getref(obd);
-	for (i = 0; i < count; i++, cookies++) {
-		struct lov_oinfo *loi = lsm->lsm_oinfo[i];
-		struct obd_device *child =
-			lov->lov_tgts[loi->loi_ost_idx]->ltd_exp->exp_obd;
-		struct llog_ctxt *cctxt =
-			llog_get_context(child, ctxt->loc_idx);
-		int err;
-
-		err = llog_cancel(env, cctxt, NULL, 1, cookies, flags);
-		llog_ctxt_put(cctxt);
-		if (err && lov->lov_tgts[loi->loi_ost_idx]->ltd_active) {
-			CERROR("%s: objid "DOSTID" subobj "DOSTID
-			       " on OST idx %d: rc = %d\n",
-			       obd->obd_name, POSTID(&lsm->lsm_oi),
-			       POSTID(&loi->loi_oi), loi->loi_ost_idx, err);
-			if (!rc)
-				rc = err;
-		}
-	}
-	obd_putref(obd);
-	return rc;
-}
-
-static struct llog_operations lov_mds_ost_orig_logops = {
-	.lop_obd_add	= lov_llog_origin_add,
-	.lop_connect	= lov_llog_origin_connect,
-};
-
-static struct llog_operations lov_size_repl_logops = {
-	.lop_cancel	= lov_llog_repl_cancel,
-};
-
-int lov_llog_init(struct obd_device *obd, struct obd_llog_group *olg,
-		  struct obd_device *disk_obd, int *index)
-{
-	struct lov_obd *lov = &obd->u.lov;
-	struct obd_device *child;
-	int i, rc = 0;
-
-	LASSERT(olg == &obd->obd_olg);
-	rc = llog_setup(NULL, obd, olg, LLOG_MDS_OST_ORIG_CTXT, disk_obd,
-			&lov_mds_ost_orig_logops);
-	if (rc)
-		return rc;
-
-	rc = llog_setup(NULL, obd, olg, LLOG_SIZE_REPL_CTXT, disk_obd,
-			&lov_size_repl_logops);
-	if (rc)
-		GOTO(err_cleanup, rc);
-
-	obd_getref(obd);
-	/* count may not match lov->desc.ld_tgt_count during dynamic ost add */
-	for (i = 0; i < lov->desc.ld_tgt_count; i++) {
-		if (!lov->lov_tgts[i])
-			continue;
-
-		if (index && i != *index)
-			continue;
-
-		child = lov->lov_tgts[i]->ltd_obd;
-		rc = obd_llog_init(child, &child->obd_olg, disk_obd, &i);
-		if (rc)
-			CERROR("error osc_llog_init idx %d osc '%s' tgt '%s' "
-			       "(rc=%d)\n", i, child->obd_name,
-			       disk_obd->obd_name, rc);
-		rc = 0;
-	}
-	obd_putref(obd);
-	GOTO(err_cleanup, rc);
-err_cleanup:
-	if (rc) {
-		struct llog_ctxt *ctxt =
-			llog_get_context(obd, LLOG_SIZE_REPL_CTXT);
-		if (ctxt)
-			llog_cleanup(NULL, ctxt);
-		ctxt = llog_get_context(obd, LLOG_MDS_OST_ORIG_CTXT);
-		if (ctxt)
-			llog_cleanup(NULL, ctxt);
-	}
-	return rc;
-}
-
-int lov_llog_finish(struct obd_device *obd, int count)
-{
-	struct llog_ctxt *ctxt;
-
-	/* cleanup our llogs only if the ctxts have been setup
-	 * (client lov doesn't setup, mds lov does). */
-	ctxt = llog_get_context(obd, LLOG_MDS_OST_ORIG_CTXT);
-	if (ctxt)
-		llog_cleanup(NULL, ctxt);
-
-	ctxt = llog_get_context(obd, LLOG_SIZE_REPL_CTXT);
-	if (ctxt)
-		llog_cleanup(NULL, ctxt);
-
-	/* lov->tgt llogs are cleaned during osc_cleanup. */
-	return 0;
-}
diff --git a/drivers/staging/lustre/lustre/lov/lov_merge.c b/drivers/staging/lustre/lustre/lov/lov_merge.c
index 9defa55d991..da959e90137 100644
--- a/drivers/staging/lustre/lustre/lov/lov_merge.c
+++ b/drivers/staging/lustre/lustre/lov/lov_merge.c
@@ -39,8 +39,6 @@
 #include <linux/libcfs/libcfs.h>
 
 #include <obd_class.h>
-#include <obd_lov.h>
-
 #include "lov_internal.h"
 
 /** Merge the lock value block(&lvb) attributes and KMS from each of the
@@ -60,7 +58,7 @@ int lov_merge_lvb_kms(struct lov_stripe_md *lsm,
 	int i;
 	int rc = 0;
 
-	LASSERT(spin_is_locked(&lsm->lsm_lock));
+	assert_spin_locked(&lsm->lsm_lock);
 	LASSERT(lsm->lsm_lock_owner == current_pid());
 
 	CDEBUG(D_INODE, "MDT ID "DOSTID" initial value: s="LPU64" m="LPU64
@@ -147,7 +145,7 @@ int lov_adjust_kms(struct obd_export *exp, struct lov_stripe_md *lsm,
 	int stripe = 0;
 	__u64 kms;
 
-	LASSERT(spin_is_locked(&lsm->lsm_lock));
+	assert_spin_locked(&lsm->lsm_lock);
 	LASSERT(lsm->lsm_lock_owner == current_pid());
 
 	if (shrink) {
diff --git a/drivers/staging/lustre/lustre/lov/lov_obd.c b/drivers/staging/lustre/lustre/lov/lov_obd.c
index 02509d0cb10..2d843b1c1de 100644
--- a/drivers/staging/lustre/lustre/lov/lov_obd.c
+++ b/drivers/staging/lustre/lustre/lov/lov_obd.c
@@ -50,16 +50,13 @@
 #include <lustre/lustre_idl.h>
 #include <lustre_dlm.h>
 #include <lustre_mds.h>
-#include <lustre_debug.h>
 #include <obd_class.h>
-#include <obd_lov.h>
 #include <obd_ost.h>
 #include <lprocfs_status.h>
 #include <lustre_param.h>
 #include <cl_object.h>
 #include <lclient.h> /* for cl_client_lru */
 #include <lustre/ll_fiemap.h>
-#include <lustre_log.h>
 #include <lustre_fid.h>
 
 #include "lov_internal.h"
@@ -280,7 +277,7 @@ static int lov_disconnect_obd(struct obd_device *obd, struct lov_tgt_desc *tgt)
 
 	osc_obd = class_exp2obd(tgt->ltd_exp);
 	CDEBUG(D_CONFIG, "%s: disconnecting target %s\n",
-	       obd->obd_name, osc_obd->obd_name);
+		obd->obd_name, osc_obd ? osc_obd->obd_name : "NULL");
 
 	if (tgt->ltd_active) {
 		tgt->ltd_active = 0;
@@ -288,11 +285,11 @@ static int lov_disconnect_obd(struct obd_device *obd, struct lov_tgt_desc *tgt)
 		tgt->ltd_exp->exp_obd->obd_inactive = 1;
 	}
 
-	lov_proc_dir = obd->obd_proc_private;
-	if (lov_proc_dir)
-		lprocfs_remove_proc_entry(osc_obd->obd_name, lov_proc_dir);
-
 	if (osc_obd) {
+		lov_proc_dir = obd->obd_proc_private;
+		if (lov_proc_dir) {
+			lprocfs_remove_proc_entry(osc_obd->obd_name, lov_proc_dir);
+		}
 		/* Pass it on to our clients.
 		 * XXX This should be an argument to disconnect,
 		 * XXX not a back-door flag on the OBD.  Ah well.
@@ -726,8 +723,8 @@ void lov_fix_desc_stripe_size(__u64 *val)
 		if (*val != 0)
 			LCONSOLE_INFO("Increasing default stripe size to "
 				      "minimum %u\n",
-				      LOV_DEFAULT_STRIPE_SIZE);
-		*val = LOV_DEFAULT_STRIPE_SIZE;
+				      LOV_DESC_STRIPE_SIZE_DEFAULT);
+		*val = LOV_DESC_STRIPE_SIZE_DEFAULT;
 	} else if (*val & (LOV_MIN_STRIPE_SIZE - 1)) {
 		*val &= ~(LOV_MIN_STRIPE_SIZE - 1);
 		LCONSOLE_WARN("Changing default stripe size to "LPU64" (a "
@@ -753,9 +750,8 @@ void lov_fix_desc_pattern(__u32 *val)
 
 void lov_fix_desc_qos_maxage(__u32 *val)
 {
-	/* fix qos_maxage */
 	if (*val == 0)
-		*val = QOS_DEFAULT_MAXAGE;
+		*val = LOV_DESC_QOS_MAXAGE_DEFAULT;
 }
 
 void lov_fix_desc(struct lov_desc *desc)
@@ -861,12 +857,10 @@ static int lov_precleanup(struct obd_device *obd, enum obd_cleanup_stage stage)
 		}
 		break;
 	}
-	case OBD_CLEANUP_EXPORTS:
-		rc = obd_llog_finish(obd, 0);
-		if (rc != 0)
-			CERROR("failed to cleanup llogging subsystems\n");
+	default:
 		break;
 	}
+
 	return rc;
 }
 
@@ -2254,11 +2248,12 @@ static int lov_fiemap(struct lov_obd *lov, __u32 keylen, void *key,
 	if (fm_end_offset == -EINVAL)
 		GOTO(out, rc = -EINVAL);
 
+	if (fiemap_count_to_size(fiemap->fm_extent_count) > *vallen)
+		fiemap->fm_extent_count = fiemap_size_to_count(*vallen);
 	if (fiemap->fm_extent_count == 0) {
 		get_num_extents = 1;
 		count_local = 0;
 	}
-
 	/* Check each stripe */
 	for (cur_stripe = start_stripe, i = 0; i < stripe_count;
 	     i++, cur_stripe = (cur_stripe + 1) % lsm->lsm_stripe_count) {
@@ -2808,8 +2803,6 @@ struct obd_ops lov_obd_ops = {
 	.o_get_info	    = lov_get_info,
 	.o_set_info_async      = lov_set_info_async,
 	.o_extent_calc	 = lov_extent_calc,
-	.o_llog_init	   = lov_llog_init,
-	.o_llog_finish	 = lov_llog_finish,
 	.o_notify	      = lov_notify,
 	.o_pool_new	    = lov_pool_new,
 	.o_pool_rem	    = lov_pool_remove,
diff --git a/drivers/staging/lustre/lustre/lov/lov_object.c b/drivers/staging/lustre/lustre/lov/lov_object.c
index d6b2cb45b93..992c80ab3d5 100644
--- a/drivers/staging/lustre/lustre/lov/lov_object.c
+++ b/drivers/staging/lustre/lustre/lov/lov_object.c
@@ -42,7 +42,6 @@
 #define DEBUG_SUBSYSTEM S_LOV
 
 #include "lov_cl_internal.h"
-#include <lustre_debug.h>
 
 /** \addtogroup lov
  *  @{
@@ -885,7 +884,7 @@ struct lu_object *lov_object_alloc(const struct lu_env *env,
 	struct lov_object *lov;
 	struct lu_object  *obj;
 
-	OBD_SLAB_ALLOC_PTR_GFP(lov, lov_object_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(lov, lov_object_kmem, GFP_NOFS);
 	if (lov != NULL) {
 		obj = lov2lu(lov);
 		lu_object_init(obj, NULL, dev);
diff --git a/drivers/staging/lustre/lustre/lov/lov_offset.c b/drivers/staging/lustre/lustre/lov/lov_offset.c
index 04863a7c5e0..379568f8245 100644
--- a/drivers/staging/lustre/lustre/lov/lov_offset.c
+++ b/drivers/staging/lustre/lustre/lov/lov_offset.c
@@ -39,7 +39,6 @@
 #include <linux/libcfs/libcfs.h>
 
 #include <obd_class.h>
-#include <obd_lov.h>
 
 #include "lov_internal.h"
 
diff --git a/drivers/staging/lustre/lustre/lov/lov_pack.c b/drivers/staging/lustre/lustre/lov/lov_pack.c
index 74200cf1b33..59ab7c30ffb 100644
--- a/drivers/staging/lustre/lustre/lov/lov_pack.c
+++ b/drivers/staging/lustre/lustre/lov/lov_pack.c
@@ -44,7 +44,6 @@
 
 #include <lustre_net.h>
 #include <obd.h>
-#include <obd_lov.h>
 #include <obd_class.h>
 #include <obd_support.h>
 #include <lustre/lustre_user.h>
@@ -177,8 +176,9 @@ int lov_packmd(struct obd_export *exp, struct lov_mds_md **lmmp,
 		 * Anyway, this is pretty inaccurate since ld_tgt_count now
 		 * represents max index and we should rely on the actual number
 		 * of OSTs instead */
-		stripe_count = lov_mds_md_stripecnt(lov->lov_ocd.ocd_max_easize,
-						    lmm_magic);
+		stripe_count = lov_mds_md_max_stripe_count(
+			lov->lov_ocd.ocd_max_easize, lmm_magic);
+
 		if (stripe_count > lov->desc.ld_tgt_count)
 			stripe_count = lov->desc.ld_tgt_count;
 	}
@@ -264,8 +264,8 @@ __u16 lov_get_stripecnt(struct lov_obd *lov, __u32 magic, __u16 stripe_count)
 	 * larger EA sizes */
 	if (lov->lov_ocd.ocd_connect_flags & OBD_CONNECT_MAX_EASIZE &&
 	    lov->lov_ocd.ocd_max_easize)
-		max_stripes = lov_mds_md_stripecnt(lov->lov_ocd.ocd_max_easize,
-						   magic);
+		max_stripes = lov_mds_md_max_stripe_count(
+			lov->lov_ocd.ocd_max_easize, magic);
 
 	if (stripe_count > max_stripes)
 		stripe_count = max_stripes;
diff --git a/drivers/staging/lustre/lustre/lov/lov_request.c b/drivers/staging/lustre/lustre/lov/lov_request.c
index a5481d7eb5d..bd6490d0129 100644
--- a/drivers/staging/lustre/lustre/lov/lov_request.c
+++ b/drivers/staging/lustre/lustre/lov/lov_request.c
@@ -39,7 +39,6 @@
 #include <linux/libcfs/libcfs.h>
 
 #include <obd_class.h>
-#include <obd_lov.h>
 #include <lustre/lustre_idl.h>
 
 #include "lov_internal.h"
diff --git a/drivers/staging/lustre/lustre/lov/lovsub_dev.c b/drivers/staging/lustre/lustre/lov/lovsub_dev.c
index 926c35a25ce..52fb6c162ad 100644
--- a/drivers/staging/lustre/lustre/lov/lovsub_dev.c
+++ b/drivers/staging/lustre/lustre/lov/lovsub_dev.c
@@ -146,7 +146,7 @@ static int lovsub_req_init(const struct lu_env *env, struct cl_device *dev,
 	struct lovsub_req *lsr;
 	int result;
 
-	OBD_SLAB_ALLOC_PTR_GFP(lsr, lovsub_req_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(lsr, lovsub_req_kmem, GFP_NOFS);
 	if (lsr != NULL) {
 		cl_req_slice_add(req, &lsr->lsrq_cl, dev, &lovsub_req_ops);
 		result = 0;
diff --git a/drivers/staging/lustre/lustre/lov/lovsub_lock.c b/drivers/staging/lustre/lustre/lov/lovsub_lock.c
index 80305aa6171..62b696d25d8 100644
--- a/drivers/staging/lustre/lustre/lov/lovsub_lock.c
+++ b/drivers/staging/lustre/lustre/lov/lovsub_lock.c
@@ -453,7 +453,7 @@ int lovsub_lock_init(const struct lu_env *env, struct cl_object *obj,
 	struct lovsub_lock *lsk;
 	int result;
 
-	OBD_SLAB_ALLOC_PTR_GFP(lsk, lovsub_lock_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(lsk, lovsub_lock_kmem, GFP_NOFS);
 	if (lsk != NULL) {
 		INIT_LIST_HEAD(&lsk->lss_parents);
 		cl_lock_slice_add(lock, &lsk->lss_cl, obj, &lovsub_lock_ops);
diff --git a/drivers/staging/lustre/lustre/lov/lovsub_object.c b/drivers/staging/lustre/lustre/lov/lovsub_object.c
index 89760b3bf3f..57e3629fcce 100644
--- a/drivers/staging/lustre/lustre/lov/lovsub_object.c
+++ b/drivers/staging/lustre/lustre/lov/lovsub_object.c
@@ -145,7 +145,7 @@ struct lu_object *lovsub_object_alloc(const struct lu_env *env,
 	struct lovsub_object *los;
 	struct lu_object     *obj;
 
-	OBD_SLAB_ALLOC_PTR_GFP(los, lovsub_object_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(los, lovsub_object_kmem, GFP_NOFS);
 	if (los != NULL) {
 		struct cl_object_header *hdr;
 
diff --git a/drivers/staging/lustre/lustre/mdc/mdc_locks.c b/drivers/staging/lustre/lustre/mdc/mdc_locks.c
index 53022ec390f..1a8cd98ad6d 100644
--- a/drivers/staging/lustre/lustre/mdc/mdc_locks.c
+++ b/drivers/staging/lustre/lustre/mdc/mdc_locks.c
@@ -442,9 +442,9 @@ static struct ptlrpc_request *mdc_intent_unlink_pack(struct obd_export *exp,
 	mdc_unlink_pack(req, op_data);
 
 	req_capsule_set_size(&req->rq_pill, &RMF_MDT_MD, RCL_SERVER,
-			     obddev->u.cli.cl_max_mds_easize);
+			     obddev->u.cli.cl_default_mds_easize);
 	req_capsule_set_size(&req->rq_pill, &RMF_ACL, RCL_SERVER,
-			     obddev->u.cli.cl_max_mds_cookiesize);
+			     obddev->u.cli.cl_default_mds_cookiesize);
 	ptlrpc_request_set_replen(req);
 	return req;
 }
@@ -462,6 +462,7 @@ static struct ptlrpc_request *mdc_intent_getattr_pack(struct obd_export *exp,
 					       OBD_MD_FLRMTPERM : OBD_MD_FLACL);
 	struct ldlm_intent    *lit;
 	int		    rc;
+	int		    easize;
 
 	req = ptlrpc_request_alloc(class_exp2cliimp(exp),
 				   &RQF_LDLM_INTENT_GETATTR);
@@ -482,12 +483,15 @@ static struct ptlrpc_request *mdc_intent_getattr_pack(struct obd_export *exp,
 	lit = req_capsule_client_get(&req->rq_pill, &RMF_LDLM_INTENT);
 	lit->opc = (__u64)it->it_op;
 
+	if (obddev->u.cli.cl_default_mds_easize > 0)
+		easize = obddev->u.cli.cl_default_mds_easize;
+	else
+		easize = obddev->u.cli.cl_max_mds_easize;
+
 	/* pack the intended request */
-	mdc_getattr_pack(req, valid, it->it_flags, op_data,
-			 obddev->u.cli.cl_max_mds_easize);
+	mdc_getattr_pack(req, valid, it->it_flags, op_data, easize);
 
-	req_capsule_set_size(&req->rq_pill, &RMF_MDT_MD, RCL_SERVER,
-			     obddev->u.cli.cl_max_mds_easize);
+	req_capsule_set_size(&req->rq_pill, &RMF_MDT_MD, RCL_SERVER, easize);
 	if (client_is_remote(exp))
 		req_capsule_set_size(&req->rq_pill, &RMF_ACL, RCL_SERVER,
 				     sizeof(struct mdt_remote_perm));
@@ -528,7 +532,7 @@ static struct ptlrpc_request *mdc_intent_layout_pack(struct obd_export *exp,
 	layout->li_opc = LAYOUT_INTENT_ACCESS;
 
 	req_capsule_set_size(&req->rq_pill, &RMF_DLM_LVB, RCL_SERVER,
-			obd->u.cli.cl_max_mds_easize);
+			     obd->u.cli.cl_default_mds_easize);
 	ptlrpc_request_set_replen(req);
 	return req;
 }
@@ -893,7 +897,10 @@ resend:
 	mdc_put_rpc_lock(obddev->u.cli.cl_rpc_lock, it);
 
 	if (rc < 0) {
-		CERROR("ldlm_cli_enqueue: %d\n", rc);
+		CDEBUG_LIMIT((rc == -EACCES || rc == -EIDRM) ? D_INFO : D_ERROR,
+			     "%s: ldlm_cli_enqueue failed: rc = %d\n",
+			     obddev->obd_name, rc);
+
 		mdc_clear_replay_flag(req, rc);
 		ptlrpc_req_finished(req);
 		return rc;
diff --git a/drivers/staging/lustre/lustre/mdc/mdc_reint.c b/drivers/staging/lustre/lustre/mdc/mdc_reint.c
index d79aa1641fe..08e80940ee4 100644
--- a/drivers/staging/lustre/lustre/mdc/mdc_reint.c
+++ b/drivers/staging/lustre/lustre/mdc/mdc_reint.c
@@ -199,7 +199,8 @@ int mdc_setattr(struct obd_export *exp, struct md_op_data *op_data,
 	*request = req;
 	if (rc && req->rq_commit_cb) {
 		/* Put an extra reference on \var mod on error case. */
-		obd_mod_put(*mod);
+		if (mod != NULL && *mod != NULL)
+			obd_mod_put(*mod);
 		req->rq_commit_cb(req);
 	}
 	return rc;
@@ -357,9 +358,9 @@ int mdc_unlink(struct obd_export *exp, struct md_op_data *op_data,
 	mdc_unlink_pack(req, op_data);
 
 	req_capsule_set_size(&req->rq_pill, &RMF_MDT_MD, RCL_SERVER,
-			     obd->u.cli.cl_max_mds_easize);
+			     obd->u.cli.cl_default_mds_easize);
 	req_capsule_set_size(&req->rq_pill, &RMF_LOGCOOKIES, RCL_SERVER,
-			     obd->u.cli.cl_max_mds_cookiesize);
+			     obd->u.cli.cl_default_mds_cookiesize);
 	ptlrpc_request_set_replen(req);
 
 	*request = req;
@@ -470,9 +471,9 @@ int mdc_rename(struct obd_export *exp, struct md_op_data *op_data,
 	mdc_rename_pack(req, op_data, old, oldlen, new, newlen);
 
 	req_capsule_set_size(&req->rq_pill, &RMF_MDT_MD, RCL_SERVER,
-			     obd->u.cli.cl_max_mds_easize);
+			     obd->u.cli.cl_default_mds_easize);
 	req_capsule_set_size(&req->rq_pill, &RMF_LOGCOOKIES, RCL_SERVER,
-			     obd->u.cli.cl_max_mds_cookiesize);
+			     obd->u.cli.cl_default_mds_cookiesize);
 	ptlrpc_request_set_replen(req);
 
 	rc = mdc_reint(req, obd->u.cli.cl_rpc_lock, LUSTRE_IMP_FULL);
diff --git a/drivers/staging/lustre/lustre/mdc/mdc_request.c b/drivers/staging/lustre/lustre/mdc/mdc_request.c
index bde9f93c149..fca43cf1d67 100644
--- a/drivers/staging/lustre/lustre/mdc/mdc_request.c
+++ b/drivers/staging/lustre/lustre/mdc/mdc_request.c
@@ -903,9 +903,9 @@ int mdc_close(struct obd_export *exp, struct md_op_data *op_data,
 	mdc_close_pack(req, op_data);
 
 	req_capsule_set_size(&req->rq_pill, &RMF_MDT_MD, RCL_SERVER,
-			     obd->u.cli.cl_max_mds_easize);
+			     obd->u.cli.cl_default_mds_easize);
 	req_capsule_set_size(&req->rq_pill, &RMF_LOGCOOKIES, RCL_SERVER,
-			     obd->u.cli.cl_max_mds_cookiesize);
+			     obd->u.cli.cl_default_mds_cookiesize);
 
 	ptlrpc_request_set_replen(req);
 
@@ -2153,12 +2153,40 @@ int mdc_get_info(const struct lu_env *env, struct obd_export *exp,
 
 		if (*vallen != sizeof(int))
 			return -EINVAL;
-		mdsize = *(int*)val;
+		mdsize = *(int *)val;
 		if (mdsize > exp->exp_obd->u.cli.cl_max_mds_easize)
 			exp->exp_obd->u.cli.cl_max_mds_easize = mdsize;
 		max_easize = val;
 		*max_easize = exp->exp_obd->u.cli.cl_max_mds_easize;
 		return 0;
+	} else if (KEY_IS(KEY_DEFAULT_EASIZE)) {
+		int *default_easize;
+
+		if (*vallen != sizeof(int))
+			return -EINVAL;
+		default_easize = val;
+		*default_easize = exp->exp_obd->u.cli.cl_default_mds_easize;
+		return 0;
+	} else if (KEY_IS(KEY_MAX_COOKIESIZE)) {
+		int mdsize, *max_cookiesize;
+
+		if (*vallen != sizeof(int))
+			return -EINVAL;
+		mdsize = *(int *)val;
+		if (mdsize > exp->exp_obd->u.cli.cl_max_mds_cookiesize)
+			exp->exp_obd->u.cli.cl_max_mds_cookiesize = mdsize;
+		max_cookiesize = val;
+		*max_cookiesize = exp->exp_obd->u.cli.cl_max_mds_cookiesize;
+		return 0;
+	} else if (KEY_IS(KEY_DEFAULT_COOKIESIZE)) {
+		int *default_cookiesize;
+
+		if (*vallen != sizeof(int))
+			return -EINVAL;
+		default_cookiesize = val;
+		*default_cookiesize =
+			exp->exp_obd->u.cli.cl_default_mds_cookiesize;
+		return 0;
 	} else if (KEY_IS(KEY_CONN_DATA)) {
 		struct obd_import *imp = class_exp2cliimp(exp);
 		struct obd_connect_data *data = val;
@@ -2439,11 +2467,15 @@ err_rpc_lock:
 }
 
 /* Initialize the default and maximum LOV EA and cookie sizes.  This allows
- * us to make MDS RPCs with large enough reply buffers to hold the
- * maximum-sized (= maximum striped) EA and cookie without having to
- * calculate this (via a call into the LOV + OSCs) each time we make an RPC. */
+ * us to make MDS RPCs with large enough reply buffers to hold a default
+ * sized EA and cookie without having to calculate this (via a call into the
+ * LOV + OSCs) each time we make an RPC.  The maximum size is also tracked
+ * but not used to avoid wastefully vmalloc()'ing large reply buffers when
+ * a large number of stripes is possible.  If a larger reply buffer is
+ * required it will be reallocated in the ptlrpc layer due to overflow.
+ */
 static int mdc_init_ea_size(struct obd_export *exp, int easize,
-		     int def_easize, int cookiesize)
+			    int def_easize, int cookiesize, int def_cookiesize)
 {
 	struct obd_device *obd = exp->exp_obd;
 	struct client_obd *cli = &obd->u.cli;
@@ -2457,6 +2489,9 @@ static int mdc_init_ea_size(struct obd_export *exp, int easize,
 	if (cli->cl_max_mds_cookiesize < cookiesize)
 		cli->cl_max_mds_cookiesize = cookiesize;
 
+	if (cli->cl_default_mds_cookiesize < def_cookiesize)
+		cli->cl_default_mds_cookiesize = def_cookiesize;
+
 	return 0;
 }
 
diff --git a/drivers/staging/lustre/lustre/mgc/lproc_mgc.c b/drivers/staging/lustre/lustre/mgc/lproc_mgc.c
index 1506af13f1b..6c877c5a6a7 100644
--- a/drivers/staging/lustre/lustre/mgc/lproc_mgc.c
+++ b/drivers/staging/lustre/lustre/mgc/lproc_mgc.c
@@ -56,21 +56,21 @@ static int mgc_ir_state_seq_show(struct seq_file *m, void *v)
 LPROC_SEQ_FOPS_RO(mgc_ir_state);
 
 static struct lprocfs_vars lprocfs_mgc_obd_vars[] = {
-	{ "uuid",	     &mgc_uuid_fops,	  0, 0 },
-	{ "ping",	     &mgc_ping_fops,      0, 0222 },
-	{ "connect_flags",   &mgc_connect_flags_fops, 0, 0 },
-	{ "mgs_server_uuid", &mgc_server_uuid_fops,   0, 0 },
-	{ "mgs_conn_uuid",   &mgc_conn_uuid_fops,     0, 0 },
-	{ "import",	     &mgc_import_fops,	0, 0 },
-	{ "state",	     &mgc_state_fops,	 0, 0 },
-	{ "ir_state",	     &mgc_ir_state_fops,  0, 0 },
-	{ 0 }
+	{ "uuid",	     &mgc_uuid_fops,	  NULL, 0 },
+	{ "ping",	     &mgc_ping_fops,      NULL, 0222 },
+	{ "connect_flags",   &mgc_connect_flags_fops, NULL, 0 },
+	{ "mgs_server_uuid", &mgc_server_uuid_fops,   NULL, 0 },
+	{ "mgs_conn_uuid",   &mgc_conn_uuid_fops,     NULL, 0 },
+	{ "import",	     &mgc_import_fops,	NULL, 0 },
+	{ "state",	     &mgc_state_fops,	 NULL, 0 },
+	{ "ir_state",	     &mgc_ir_state_fops,  NULL, 0 },
+	{ NULL }
 };
 
 LPROC_SEQ_FOPS_RO_TYPE(mgc, numrefs);
 static struct lprocfs_vars lprocfs_mgc_module_vars[] = {
-	{ "num_refs",	&mgc_numrefs_fops,       0, 0 },
-	{ 0 }
+	{ "num_refs",	&mgc_numrefs_fops,       NULL, 0 },
+	{ NULL }
 };
 
 void lprocfs_mgc_init_vars(struct lprocfs_static_vars *lvars)
diff --git a/drivers/staging/lustre/lustre/mgc/mgc_request.c b/drivers/staging/lustre/lustre/mgc/mgc_request.c
index de9fb1433ed..a806aeffe02 100644
--- a/drivers/staging/lustre/lustre/mgc/mgc_request.c
+++ b/drivers/staging/lustre/lustre/mgc/mgc_request.c
@@ -672,8 +672,8 @@ static int mgc_fs_setup(struct obd_device *obd, struct super_block *sb)
 	if (env == NULL)
 		return -ENOMEM;
 
-	/* The mgc fs exclusion sem. Only one fs can be setup at a time. */
-	down(&cli->cl_mgc_sem);
+	/* The mgc fs exclusion mutex. Only one fs can be setup at a time. */
+	mutex_lock(&cli->cl_mgc_mutex);
 
 	cfs_cleanup_group_info();
 
@@ -727,7 +727,7 @@ out_los:
 	if (rc < 0) {
 		local_oid_storage_fini(env, cli->cl_mgc_los);
 		cli->cl_mgc_los = NULL;
-		up(&cli->cl_mgc_sem);
+		mutex_unlock(&cli->cl_mgc_mutex);
 	}
 out_env:
 	lu_env_fini(env);
@@ -759,7 +759,7 @@ static int mgc_fs_cleanup(struct obd_device *obd)
 
 unlock:
 	class_decref(obd, "mgc_fs", obd);
-	up(&cli->cl_mgc_sem);
+	mutex_unlock(&cli->cl_mgc_mutex);
 
 	return 0;
 }
diff --git a/drivers/staging/lustre/lustre/obdclass/cl_lock.c b/drivers/staging/lustre/lustre/obdclass/cl_lock.c
index d795cef3f16..df77c4fc0ea 100644
--- a/drivers/staging/lustre/lustre/obdclass/cl_lock.c
+++ b/drivers/staging/lustre/lustre/obdclass/cl_lock.c
@@ -139,7 +139,7 @@ static void cl_lock_trace0(int level, const struct lu_env *env,
 	       func, line);
 }
 #define cl_lock_trace(level, env, prefix, lock)			 \
-	cl_lock_trace0(level, env, prefix, lock, __FUNCTION__, __LINE__)
+	cl_lock_trace0(level, env, prefix, lock, __func__, __LINE__)
 
 #define RETIP ((unsigned long)__builtin_return_address(0))
 
@@ -360,7 +360,7 @@ static struct cl_lock *cl_lock_alloc(const struct lu_env *env,
 	struct cl_lock	  *lock;
 	struct lu_object_header *head;
 
-	OBD_SLAB_ALLOC_PTR_GFP(lock, cl_lock_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(lock, cl_lock_kmem, GFP_NOFS);
 	if (lock != NULL) {
 		atomic_set(&lock->cll_ref, 1);
 		lock->cll_descr = *descr;
@@ -478,7 +478,7 @@ static struct cl_lock *cl_lock_lookup(const struct lu_env *env,
 	struct cl_object_header *head;
 
 	head = cl_object_header(obj);
-	LINVRNT(spin_is_locked(&head->coh_lock_guard));
+	assert_spin_locked(&head->coh_lock_guard);
 	CS_LOCK_INC(obj, lookup);
 	list_for_each_entry(lock, &head->coh_locks, cll_linkage) {
 		int matched;
@@ -533,6 +533,7 @@ static struct cl_lock *cl_lock_find(const struct lu_env *env,
 			spin_lock(&head->coh_lock_guard);
 			ghost = cl_lock_lookup(env, obj, io, need);
 			if (ghost == NULL) {
+				cl_lock_get_trust(lock);
 				list_add_tail(&lock->cll_linkage,
 						  &head->coh_locks);
 				spin_unlock(&head->coh_lock_guard);
@@ -791,15 +792,22 @@ static void cl_lock_delete0(const struct lu_env *env, struct cl_lock *lock)
 	LINVRNT(cl_lock_invariant(env, lock));
 
 	if (lock->cll_state < CLS_FREEING) {
+		bool in_cache;
+
 		LASSERT(lock->cll_state != CLS_INTRANSIT);
 		cl_lock_state_set(env, lock, CLS_FREEING);
 
 		head = cl_object_header(lock->cll_descr.cld_obj);
 
 		spin_lock(&head->coh_lock_guard);
-		list_del_init(&lock->cll_linkage);
+		in_cache = !list_empty(&lock->cll_linkage);
+		if (in_cache)
+			list_del_init(&lock->cll_linkage);
 		spin_unlock(&head->coh_lock_guard);
 
+		if (in_cache) /* coh_locks cache holds a refcount. */
+			cl_lock_put(env, lock);
+
 		/*
 		 * From now on, no new references to this lock can be acquired
 		 * by cl_lock_lookup().
diff --git a/drivers/staging/lustre/lustre/obdclass/cl_object.c b/drivers/staging/lustre/lustre/obdclass/cl_object.c
index 0fc256f59e9..41cbc95b916 100644
--- a/drivers/staging/lustre/lustre/obdclass/cl_object.c
+++ b/drivers/staging/lustre/lustre/obdclass/cl_object.c
@@ -220,7 +220,7 @@ int cl_object_attr_get(const struct lu_env *env, struct cl_object *obj,
 	struct lu_object_header *top;
 	int result;
 
-	LASSERT(spin_is_locked(cl_object_attr_guard(obj)));
+	assert_spin_locked(cl_object_attr_guard(obj));
 
 	top = obj->co_lu.lo_header;
 	result = 0;
@@ -251,7 +251,7 @@ int cl_object_attr_set(const struct lu_env *env, struct cl_object *obj,
 	struct lu_object_header *top;
 	int result;
 
-	LASSERT(spin_is_locked(cl_object_attr_guard(obj)));
+	assert_spin_locked(cl_object_attr_guard(obj));
 
 	top = obj->co_lu.lo_header;
 	result = 0;
@@ -684,7 +684,7 @@ static struct lu_env *cl_env_new(__u32 ctx_tags, __u32 ses_tags, void *debug)
 	struct lu_env *env;
 	struct cl_env *cle;
 
-	OBD_SLAB_ALLOC_PTR_GFP(cle, cl_env_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(cle, cl_env_kmem, GFP_NOFS);
 	if (cle != NULL) {
 		int rc;
 
diff --git a/drivers/staging/lustre/lustre/obdclass/cl_page.c b/drivers/staging/lustre/lustre/obdclass/cl_page.c
index 2a5ce376e57..1b616e4fe14 100644
--- a/drivers/staging/lustre/lustre/obdclass/cl_page.c
+++ b/drivers/staging/lustre/lustre/obdclass/cl_page.c
@@ -130,7 +130,7 @@ struct cl_page *cl_page_lookup(struct cl_object_header *hdr, pgoff_t index)
 {
 	struct cl_page *page;
 
-	LASSERT(spin_is_locked(&hdr->coh_page_guard));
+	assert_spin_locked(&hdr->coh_page_guard);
 
 	page = radix_tree_lookup(&hdr->coh_tree, index);
 	if (page != NULL)
@@ -292,7 +292,7 @@ static struct cl_page *cl_page_alloc(const struct lu_env *env,
 	struct lu_object_header *head;
 
 	OBD_ALLOC_GFP(page, cl_object_header(o)->coh_page_bufsize,
-			__GFP_IO);
+			GFP_NOFS);
 	if (page != NULL) {
 		int result = 0;
 		atomic_set(&page->cp_ref, 1);
diff --git a/drivers/staging/lustre/lustre/obdclass/class_obd.c b/drivers/staging/lustre/lustre/obdclass/class_obd.c
index c93131e0d2d..dde04b767a6 100644
--- a/drivers/staging/lustre/lustre/obdclass/class_obd.c
+++ b/drivers/staging/lustre/lustre/obdclass/class_obd.c
@@ -102,23 +102,17 @@ EXPORT_SYMBOL(obd_dirty_transit_pages);
 char obd_jobid_var[JOBSTATS_JOBID_VAR_MAX_LEN + 1] = JOBSTATS_DISABLE;
 EXPORT_SYMBOL(obd_jobid_var);
 
-/* Get jobid of current process by reading the environment variable
- * stored in between the "env_start" & "env_end" of task struct.
- *
- * TODO:
- * It's better to cache the jobid for later use if there is any
- * efficient way, the cl_env code probably could be reused for this
- * purpose.
+char obd_jobid_node[JOBSTATS_JOBID_SIZE + 1];
+
+/* Get jobid of current process from stored variable or calculate
+ * it from pid and user_id.
  *
- * If some job scheduler doesn't store jobid in the "env_start/end",
- * then an upcall could be issued here to get the jobid by utilizing
- * the userspace tools/api. Then, the jobid must be cached.
+ * Historically this was also done by reading the environment variable
+ * stored in between the "env_start" & "env_end" of task struct.
+ * This is now deprecated.
  */
 int lustre_get_jobid(char *jobid)
 {
-	int jobid_len = JOBSTATS_JOBID_SIZE;
-	int rc = 0;
-
 	memset(jobid, 0, JOBSTATS_JOBID_SIZE);
 	/* Jobstats isn't enabled */
 	if (strcmp(obd_jobid_var, JOBSTATS_DISABLE) == 0)
@@ -132,31 +126,13 @@ int lustre_get_jobid(char *jobid)
 		return 0;
 	}
 
-	rc = cfs_get_environ(obd_jobid_var, jobid, &jobid_len);
-	if (rc) {
-		if (rc == -EOVERFLOW) {
-			/* For the PBS_JOBID and LOADL_STEP_ID keys (which are
-			 * variable length strings instead of just numbers), it
-			 * might make sense to keep the unique parts for JobID,
-			 * instead of just returning an error.  That means a
-			 * larger temp buffer for cfs_get_environ(), then
-			 * truncating the string at some separator to fit into
-			 * the specified jobid_len.  Fix later if needed. */
-			static bool printed;
-			if (unlikely(!printed)) {
-				LCONSOLE_ERROR_MSG(0x16b, "%s value too large "
-						   "for JobID buffer (%d)\n",
-						   obd_jobid_var, jobid_len);
-				printed = true;
-			}
-		} else {
-			CDEBUG((rc == -ENOENT || rc == -EINVAL ||
-				rc == -EDEADLK) ? D_INFO : D_ERROR,
-			       "Get jobid for (%s) failed: rc = %d\n",
-			       obd_jobid_var, rc);
-		}
+	/* Whole node dedicated to single job */
+	if (strcmp(obd_jobid_var, JOBSTATS_NODELOCAL) == 0) {
+		strcpy(jobid, obd_jobid_node);
+		return 0;
 	}
-	return rc;
+
+	return -ENOENT;
 }
 EXPORT_SYMBOL(lustre_get_jobid);
 
diff --git a/drivers/staging/lustre/lustre/obdclass/debug.c b/drivers/staging/lustre/lustre/obdclass/debug.c
index 15f71bbb727..a4e7e754177 100644
--- a/drivers/staging/lustre/lustre/obdclass/debug.c
+++ b/drivers/staging/lustre/lustre/obdclass/debug.c
@@ -56,18 +56,6 @@ void dump_lniobuf(struct niobuf_local *nb)
 }
 EXPORT_SYMBOL(dump_lniobuf);
 
-void dump_lsm(int level, struct lov_stripe_md *lsm)
-{
-	CDEBUG(level, "lsm %p, objid "DOSTID", maxbytes "LPX64", magic 0x%08X,"
-	       " stripe_size %u, stripe_count %u, refc: %d,"
-	       " layout_gen %u, pool ["LOV_POOLNAMEF"]\n", lsm,
-	       POSTID(&lsm->lsm_oi), lsm->lsm_maxbytes, lsm->lsm_magic,
-	       lsm->lsm_stripe_size, lsm->lsm_stripe_count,
-	       atomic_read(&lsm->lsm_refc), lsm->lsm_layout_gen,
-	       lsm->lsm_pool_name);
-}
-EXPORT_SYMBOL(dump_lsm);
-
 #define LPDS sizeof(__u64)
 int block_debug_setup(void *addr, int len, __u64 off, __u64 id)
 {
diff --git a/drivers/staging/lustre/lustre/obdclass/genops.c b/drivers/staging/lustre/lustre/obdclass/genops.c
index f2bdea33041..3210ad8184b 100644
--- a/drivers/staging/lustre/lustre/obdclass/genops.c
+++ b/drivers/staging/lustre/lustre/obdclass/genops.c
@@ -72,7 +72,7 @@ static struct obd_device *obd_device_alloc(void)
 {
 	struct obd_device *obd;
 
-	OBD_SLAB_ALLOC_PTR_GFP(obd, obd_device_cachep, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(obd, obd_device_cachep, GFP_NOFS);
 	if (obd != NULL) {
 		obd->obd_magic = OBD_DEVICE_MAGIC;
 	}
@@ -615,13 +615,13 @@ int class_notify_sptlrpc_conf(const char *fsname, int namelen)
 		if (strncmp(obd->obd_name, fsname, namelen))
 			continue;
 
-		class_incref(obd, __FUNCTION__, obd);
+		class_incref(obd, __func__, obd);
 		read_unlock(&obd_dev_lock);
 		rc2 = obd_set_info_async(NULL, obd->obd_self_export,
 					 sizeof(KEY_SPTLRPC_CONF),
 					 KEY_SPTLRPC_CONF, 0, NULL, NULL);
 		rc = rc ? rc : rc2;
-		class_decref(obd, __FUNCTION__, obd);
+		class_decref(obd, __func__, obd);
 		read_lock(&obd_dev_lock);
 	}
 	read_unlock(&obd_dev_lock);
diff --git a/drivers/staging/lustre/lustre/obdclass/linux/linux-module.c b/drivers/staging/lustre/lustre/obdclass/linux/linux-module.c
index ba20776ebfa..bdf2eed2952 100644
--- a/drivers/staging/lustre/lustre/obdclass/linux/linux-module.c
+++ b/drivers/staging/lustre/lustre/obdclass/linux/linux-module.c
@@ -122,6 +122,8 @@ int obd_ioctl_getdata(char **buf, int *len, void *arg)
 		OBD_FREE_LARGE(*buf, hdr.ioc_len);
 		return err;
 	}
+	if (hdr.ioc_len != data->ioc_len)
+		return -EINVAL;
 
 	if (obd_ioctl_is_invalid(data)) {
 		CERROR("ioctl not correctly formatted\n");
@@ -244,7 +246,7 @@ static int obd_proc_health_seq_show(struct seq_file *m, void *v)
 		if (obd->obd_stopping)
 			continue;
 
-		class_incref(obd, __FUNCTION__, current);
+		class_incref(obd, __func__, current);
 		read_unlock(&obd_dev_lock);
 
 		if (obd_health_check(NULL, obd)) {
@@ -252,7 +254,7 @@ static int obd_proc_health_seq_show(struct seq_file *m, void *v)
 				      obd->obd_name);
 			rc++;
 		}
-		class_decref(obd, __FUNCTION__, current);
+		class_decref(obd, __func__, current);
 		read_lock(&obd_dev_lock);
 	}
 	read_unlock(&obd_dev_lock);
@@ -277,12 +279,44 @@ static ssize_t obd_proc_jobid_var_seq_write(struct file *file, const char *buffe
 		return -EINVAL;
 
 	memset(obd_jobid_var, 0, JOBSTATS_JOBID_VAR_MAX_LEN + 1);
+
+	/* This might leave the var invalid on error, which is probably fine.*/
+	if (copy_from_user(obd_jobid_var, buffer, count))
+		return -EFAULT;
+
 	/* Trim the trailing '\n' if any */
-	memcpy(obd_jobid_var, buffer, count - (buffer[count - 1] == '\n'));
+	if (obd_jobid_var[count - 1] == '\n')
+		obd_jobid_var[count - 1] = 0;
+
 	return count;
 }
 LPROC_SEQ_FOPS(obd_proc_jobid_var);
 
+static int obd_proc_jobid_name_seq_show(struct seq_file *m, void *v)
+{
+	return seq_printf(m, "%s\n", obd_jobid_var);
+}
+
+static ssize_t obd_proc_jobid_name_seq_write(struct file *file,
+					     const char __user *buffer,
+					     size_t count, loff_t *off)
+{
+	if (!count || count > JOBSTATS_JOBID_SIZE)
+		return -EINVAL;
+
+	if (copy_from_user(obd_jobid_node, buffer, count))
+		return -EFAULT;
+
+	obd_jobid_node[count] = 0;
+
+	/* Trim the trailing '\n' if any */
+	if (obd_jobid_node[count - 1] == '\n')
+		obd_jobid_node[count - 1] = 0;
+
+	return count;
+}
+LPROC_SEQ_FOPS(obd_proc_jobid_name);
+
 /* Root for /proc/fs/lustre */
 struct proc_dir_entry *proc_lustre_root = NULL;
 EXPORT_SYMBOL(proc_lustre_root);
@@ -292,6 +326,8 @@ struct lprocfs_vars lprocfs_base[] = {
 	{ "pinger", &obd_proc_pinger_fops },
 	{ "health_check", &obd_proc_health_fops },
 	{ "jobid_var", &obd_proc_jobid_var_fops },
+	{ .name =	"jobid_name",
+	  .fops =	&obd_proc_jobid_name_fops},
 	{ 0 }
 };
 
diff --git a/drivers/staging/lustre/lustre/obdclass/llog_ioctl.c b/drivers/staging/lustre/lustre/obdclass/llog_ioctl.c
index da558a5dc92..e192aab193b 100644
--- a/drivers/staging/lustre/lustre/obdclass/llog_ioctl.c
+++ b/drivers/staging/lustre/lustre/obdclass/llog_ioctl.c
@@ -385,7 +385,7 @@ int llog_ioctl(const struct lu_env *env, struct llog_ctxt *ctxt, int cmd,
 			GOTO(out_close, rc = -EINVAL);
 		}
 
-		if (data->ioc_inlbuf2 > 0) {
+		if (data->ioc_inllen2 > 0) {
 			/* remove indicate log from the catalog */
 			rc = str2logid(&plain, data->ioc_inlbuf2,
 				       data->ioc_inllen2);
diff --git a/drivers/staging/lustre/lustre/obdclass/llog_obd.c b/drivers/staging/lustre/lustre/obdclass/llog_obd.c
index 71817af7539..2c6d81eb5c6 100644
--- a/drivers/staging/lustre/lustre/obdclass/llog_obd.c
+++ b/drivers/staging/lustre/lustre/obdclass/llog_obd.c
@@ -226,34 +226,7 @@ int llog_sync(struct llog_ctxt *ctxt, struct obd_export *exp, int flags)
 }
 EXPORT_SYMBOL(llog_sync);
 
-int llog_obd_add(const struct lu_env *env, struct llog_ctxt *ctxt,
-		 struct llog_rec_hdr *rec, struct lov_stripe_md *lsm,
-		 struct llog_cookie *logcookies, int numcookies)
-{
-	int raised, rc;
-
-	if (!ctxt) {
-		CERROR("No ctxt\n");
-		return -ENODEV;
-	}
-
-	if (ctxt->loc_flags & LLOG_CTXT_FLAG_UNINITIALIZED)
-		return -ENXIO;
-
-	CTXT_CHECK_OP(ctxt, obd_add, -EOPNOTSUPP);
-	raised = cfs_cap_raised(CFS_CAP_SYS_RESOURCE);
-	if (!raised)
-		cfs_cap_raise(CFS_CAP_SYS_RESOURCE);
-	rc = CTXTP(ctxt, obd_add)(env, ctxt, rec, lsm, logcookies,
-				  numcookies);
-	if (!raised)
-		cfs_cap_lower(CFS_CAP_SYS_RESOURCE);
-	return rc;
-}
-EXPORT_SYMBOL(llog_obd_add);
-
 int llog_cancel(const struct lu_env *env, struct llog_ctxt *ctxt,
-		struct lov_stripe_md *lsm, int count,
 		struct llog_cookie *cookies, int flags)
 {
 	int rc;
@@ -264,7 +237,7 @@ int llog_cancel(const struct lu_env *env, struct llog_ctxt *ctxt,
 	}
 
 	CTXT_CHECK_OP(ctxt, cancel, -EOPNOTSUPP);
-	rc = CTXTP(ctxt, cancel)(env, ctxt, lsm, count, cookies, flags);
+	rc = CTXTP(ctxt, cancel)(env, ctxt, cookies, flags);
 	return rc;
 }
 EXPORT_SYMBOL(llog_cancel);
diff --git a/drivers/staging/lustre/lustre/obdclass/obd_mount.c b/drivers/staging/lustre/lustre/obdclass/obd_mount.c
index 6f8ba54e966..a034aee37fc 100644
--- a/drivers/staging/lustre/lustre/obdclass/obd_mount.c
+++ b/drivers/staging/lustre/lustre/obdclass/obd_mount.c
@@ -187,7 +187,7 @@ int lustre_start_simple(char *obdname, char *type, char *uuid,
 	int rc;
 	CDEBUG(D_MOUNT, "Starting obd %s (typ=%s)\n", obdname, type);
 
-	rc = do_lcfg(obdname, 0, LCFG_ATTACH, type, uuid, 0, 0);
+	rc = do_lcfg(obdname, 0, LCFG_ATTACH, type, uuid, NULL, NULL);
 	if (rc) {
 		CERROR("%s attach error %d\n", obdname, rc);
 		return rc;
@@ -195,7 +195,7 @@ int lustre_start_simple(char *obdname, char *type, char *uuid,
 	rc = do_lcfg(obdname, 0, LCFG_SETUP, s1, s2, s3, s4);
 	if (rc) {
 		CERROR("%s setup error %d\n", obdname, rc);
-		do_lcfg(obdname, 0, LCFG_DETACH, 0, 0, 0, 0);
+		do_lcfg(obdname, 0, LCFG_DETACH, NULL, NULL, NULL, NULL);
 	}
 	return rc;
 }
@@ -337,7 +337,8 @@ int lustre_start_mgc(struct super_block *sb)
 			lnet_process_id_t id;
 			while ((rc = LNetGetId(i++, &id)) != -ENOENT) {
 				rc = do_lcfg(mgcname, id.nid,
-					     LCFG_ADD_UUID, niduuid, 0,0,0);
+					     LCFG_ADD_UUID, niduuid,
+					     NULL, NULL, NULL);
 			}
 		} else {
 			/* Use mgsnode= nids */
@@ -351,7 +352,8 @@ int lustre_start_mgc(struct super_block *sb)
 			}
 			while (class_parse_nid(ptr, &nid, &ptr) == 0) {
 				rc = do_lcfg(mgcname, nid,
-					     LCFG_ADD_UUID, niduuid, 0,0,0);
+					     LCFG_ADD_UUID, niduuid,
+					     NULL, NULL, NULL);
 				i++;
 			}
 		}
@@ -360,7 +362,7 @@ int lustre_start_mgc(struct super_block *sb)
 		ptr = lsi->lsi_lmd->lmd_dev;
 		while (class_parse_nid(ptr, &nid, &ptr) == 0) {
 			rc = do_lcfg(mgcname, nid,
-				     LCFG_ADD_UUID, niduuid, 0,0,0);
+				     LCFG_ADD_UUID, niduuid, NULL, NULL, NULL);
 			i++;
 			/* Stop at the first failover nid */
 			if (*ptr == ':')
@@ -381,7 +383,7 @@ int lustre_start_mgc(struct super_block *sb)
 	/* Start the MGC */
 	rc = lustre_start_simple(mgcname, LUSTRE_MGC_NAME,
 				 (char *)uuid->uuid, LUSTRE_MGS_OBDNAME,
-				 niduuid, 0, 0);
+				 niduuid, NULL, NULL);
 	OBD_FREE_PTR(uuid);
 	if (rc)
 		GOTO(out_free, rc);
@@ -396,13 +398,13 @@ int lustre_start_mgc(struct super_block *sb)
 		while (class_parse_nid_quiet(ptr, &nid, &ptr) == 0) {
 			j++;
 			rc = do_lcfg(mgcname, nid,
-				     LCFG_ADD_UUID, niduuid, 0,0,0);
+				     LCFG_ADD_UUID, niduuid, NULL, NULL, NULL);
 			if (*ptr == ':')
 				break;
 		}
 		if (j > 0) {
 			rc = do_lcfg(mgcname, 0, LCFG_ADD_CONN,
-				     niduuid, 0, 0, 0);
+				     niduuid, NULL, NULL, NULL);
 			i++;
 		} else {
 			/* at ":/fsname" */
@@ -480,7 +482,7 @@ static int lustre_stop_mgc(struct super_block *sb)
 {
 	struct lustre_sb_info *lsi = s2lsi(sb);
 	struct obd_device *obd;
-	char *niduuid = 0, *ptr = 0;
+	char *niduuid = NULL, *ptr = NULL;
 	int i, rc = 0, len = 0;
 
 	if (!lsi)
@@ -532,7 +534,7 @@ static int lustre_stop_mgc(struct super_block *sb)
 	for (i = 0; i < lsi->lsi_lmd->lmd_mgs_failnodes; i++) {
 		sprintf(ptr, "_%x", i);
 		rc = do_lcfg(LUSTRE_MGC_OBDNAME, 0, LCFG_DEL_UUID,
-			     niduuid, 0, 0, 0);
+			     niduuid, NULL, NULL, NULL);
 		if (rc)
 			CERROR("del MDC UUID %s failed: rc = %d\n",
 			       niduuid, rc);
diff --git a/drivers/staging/lustre/lustre/obdecho/echo_client.c b/drivers/staging/lustre/lustre/obdecho/echo_client.c
index 754aa8e5566..cdc46719bbd 100644
--- a/drivers/staging/lustre/lustre/obdecho/echo_client.c
+++ b/drivers/staging/lustre/lustre/obdecho/echo_client.c
@@ -47,7 +47,6 @@
 #include <lustre_fid.h>
 #include <lustre_acl.h>
 #include <lustre_net.h>
-#include <obd_lov.h>
 
 #include "echo_internal.h"
 
@@ -428,7 +427,7 @@ static int echo_lock_init(const struct lu_env *env,
 {
 	struct echo_lock *el;
 
-	OBD_SLAB_ALLOC_PTR_GFP(el, echo_lock_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(el, echo_lock_kmem, GFP_NOFS);
 	if (el != NULL) {
 		cl_lock_slice_add(lock, &el->el_cl, obj, &echo_lock_ops);
 		el->el_object = cl2echo_obj(obj);
@@ -599,7 +598,7 @@ static struct lu_object *echo_object_alloc(const struct lu_env *env,
 
 	/* we're the top dev. */
 	LASSERT(hdr == NULL);
-	OBD_SLAB_ALLOC_PTR_GFP(eco, echo_object_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(eco, echo_object_kmem, GFP_NOFS);
 	if (eco != NULL) {
 		struct cl_object_header *hdr = &eco->eo_hdr;
 
@@ -663,7 +662,7 @@ static void *echo_thread_key_init(const struct lu_context *ctx,
 {
 	struct echo_thread_info *info;
 
-	OBD_SLAB_ALLOC_PTR_GFP(info, echo_thread_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(info, echo_thread_kmem, GFP_NOFS);
 	if (info == NULL)
 		info = ERR_PTR(-ENOMEM);
 	return info;
@@ -693,7 +692,7 @@ static void *echo_session_key_init(const struct lu_context *ctx,
 {
 	struct echo_session_info *session;
 
-	OBD_SLAB_ALLOC_PTR_GFP(session, echo_session_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(session, echo_session_kmem, GFP_NOFS);
 	if (session == NULL)
 		session = ERR_PTR(-ENOMEM);
 	return session;
@@ -2432,7 +2431,7 @@ static int echo_client_kbrw(struct echo_device *ed, int rw, struct obdo *oa,
 	int		     i;
 	int		     rc;
 	int		     verify;
-	int		     gfp_mask;
+	gfp_t		     gfp_mask;
 	int		     brw_flags = 0;
 
 	verify = (ostid_id(&oa->o_oi) != ECHO_PERSISTENT_OBJID &&
diff --git a/drivers/staging/lustre/lustre/obdecho/lproc_echo.c b/drivers/staging/lustre/lustre/obdecho/lproc_echo.c
index b9abac1c4dc..8fe9245a8aa 100644
--- a/drivers/staging/lustre/lustre/obdecho/lproc_echo.c
+++ b/drivers/staging/lustre/lustre/obdecho/lproc_echo.c
@@ -39,13 +39,13 @@
 #ifdef LPROCFS
 LPROC_SEQ_FOPS_RO_TYPE(echo, uuid);
 static struct lprocfs_vars lprocfs_echo_obd_vars[] = {
-	{ "uuid",	 &echo_uuid_fops,	0, 0 },
+	{ "uuid",	 &echo_uuid_fops,	NULL, 0 },
 	{ 0 }
 };
 
 LPROC_SEQ_FOPS_RO_TYPE(echo, numrefs);
 static struct lprocfs_vars lprocfs_echo_module_vars[] = {
-	{ "num_refs",     &echo_numrefs_fops,     0, 0 },
+	{ "num_refs",     &echo_numrefs_fops,     NULL, 0 },
 	{ 0 }
 };
 
diff --git a/drivers/staging/lustre/lustre/osc/lproc_osc.c b/drivers/staging/lustre/lustre/osc/lproc_osc.c
index 0b59fc16c50..0cadfcd9226 100644
--- a/drivers/staging/lustre/lustre/osc/lproc_osc.c
+++ b/drivers/staging/lustre/lustre/osc/lproc_osc.c
@@ -174,15 +174,25 @@ static int osc_cached_mb_seq_show(struct seq_file *m, void *v)
 }
 
 /* shrink the number of caching pages to a specific number */
-static ssize_t osc_cached_mb_seq_write(struct file *file, const char *buffer,
-				   size_t count, loff_t *off)
+static ssize_t osc_cached_mb_seq_write(struct file *file,
+				       const char __user *buffer,
+				       size_t count, loff_t *off)
 {
 	struct obd_device *dev = ((struct seq_file *)file->private_data)->private;
 	struct client_obd *cli = &dev->u.cli;
 	int pages_number, mult, rc;
+	char kernbuf[128];
+
+	if (count >= sizeof(kernbuf))
+		return -EINVAL;
+
+	if (copy_from_user(kernbuf, buffer, count))
+		return -EFAULT;
+	kernbuf[count] = 0;
 
 	mult = 1 << (20 - PAGE_CACHE_SHIFT);
-	buffer = lprocfs_find_named_value(buffer, "used_mb:", &count);
+	buffer += lprocfs_find_named_value(kernbuf, "used_mb:", &count) -
+		  kernbuf;
 	rc = lprocfs_write_frac_helper(buffer, count, &pages_number, mult);
 	if (rc)
 		return rc;
@@ -515,44 +525,44 @@ LPROC_SEQ_FOPS_RW_TYPE(osc, import);
 LPROC_SEQ_FOPS_RW_TYPE(osc, pinger_recov);
 
 static struct lprocfs_vars lprocfs_osc_obd_vars[] = {
-	{ "uuid",	     &osc_uuid_fops,	0, 0 },
-	{ "ping",	     &osc_ping_fops,    0, 0222 },
-	{ "connect_flags",   &osc_connect_flags_fops, 0, 0 },
-	{ "blocksize",       &osc_blksize_fops,     0, 0 },
-	{ "kbytestotal",     &osc_kbytestotal_fops, 0, 0 },
-	{ "kbytesfree",      &osc_kbytesfree_fops,  0, 0 },
-	{ "kbytesavail",     &osc_kbytesavail_fops, 0, 0 },
-	{ "filestotal",      &osc_filestotal_fops,  0, 0 },
-	{ "filesfree",       &osc_filesfree_fops,   0, 0 },
-	//{ "filegroups",      lprocfs_rd_filegroups,  0, 0 },
-	{ "ost_server_uuid", &osc_server_uuid_fops, 0, 0 },
-	{ "ost_conn_uuid",   &osc_conn_uuid_fops, 0, 0 },
-	{ "active",	     &osc_active_fops, 0 },
-	{ "max_pages_per_rpc", &osc_obd_max_pages_per_rpc_fops, 0 },
-	{ "max_rpcs_in_flight", &osc_max_rpcs_in_flight_fops, 0 },
-	{ "destroys_in_flight", &osc_destroys_in_flight_fops, 0, 0 },
-	{ "max_dirty_mb",    &osc_max_dirty_mb_fops, 0 },
-	{ "osc_cached_mb",   &osc_cached_mb_fops, 0 },
-	{ "cur_dirty_bytes", &osc_cur_dirty_bytes_fops, 0, 0 },
-	{ "cur_grant_bytes", &osc_cur_grant_bytes_fops, 0 },
-	{ "cur_lost_grant_bytes", &osc_cur_lost_grant_bytes_fops, 0, 0},
-	{ "grant_shrink_interval", &osc_grant_shrink_interval_fops, 0 },
-	{ "checksums",       &osc_checksum_fops, 0 },
-	{ "checksum_type",   &osc_checksum_type_fops, 0 },
-	{ "resend_count",    &osc_resend_count_fops, 0},
-	{ "timeouts",	     &osc_timeouts_fops, 0, 0 },
-	{ "contention_seconds", &osc_contention_seconds_fops, 0 },
-	{ "lockless_truncate",  &osc_lockless_truncate_fops, 0 },
-	{ "import",		&osc_import_fops, 0 },
-	{ "state",		&osc_state_fops, 0, 0 },
-	{ "pinger_recov",	&osc_pinger_recov_fops, 0 },
-	{ 0 }
+	{ "uuid",	     &osc_uuid_fops,	NULL, 0 },
+	{ "ping",	     &osc_ping_fops,    NULL, 0222 },
+	{ "connect_flags",   &osc_connect_flags_fops, NULL, 0 },
+	{ "blocksize",       &osc_blksize_fops,     NULL, 0 },
+	{ "kbytestotal",     &osc_kbytestotal_fops, NULL, 0 },
+	{ "kbytesfree",      &osc_kbytesfree_fops,  NULL, 0 },
+	{ "kbytesavail",     &osc_kbytesavail_fops, NULL, 0 },
+	{ "filestotal",      &osc_filestotal_fops,  NULL, 0 },
+	{ "filesfree",       &osc_filesfree_fops,   NULL, 0 },
+	/*{ "filegroups",      lprocfs_rd_filegroups,  NULL, 0 },*/
+	{ "ost_server_uuid", &osc_server_uuid_fops, NULL, 0 },
+	{ "ost_conn_uuid",   &osc_conn_uuid_fops, NULL, 0 },
+	{ "active",	     &osc_active_fops, NULL },
+	{ "max_pages_per_rpc", &osc_obd_max_pages_per_rpc_fops, NULL },
+	{ "max_rpcs_in_flight", &osc_max_rpcs_in_flight_fops, NULL },
+	{ "destroys_in_flight", &osc_destroys_in_flight_fops, NULL, 0 },
+	{ "max_dirty_mb",    &osc_max_dirty_mb_fops, NULL },
+	{ "osc_cached_mb",   &osc_cached_mb_fops, NULL },
+	{ "cur_dirty_bytes", &osc_cur_dirty_bytes_fops, NULL, 0 },
+	{ "cur_grant_bytes", &osc_cur_grant_bytes_fops, NULL },
+	{ "cur_lost_grant_bytes", &osc_cur_lost_grant_bytes_fops, NULL, 0},
+	{ "grant_shrink_interval", &osc_grant_shrink_interval_fops, NULL },
+	{ "checksums",       &osc_checksum_fops, NULL },
+	{ "checksum_type",   &osc_checksum_type_fops, NULL },
+	{ "resend_count",    &osc_resend_count_fops, NULL},
+	{ "timeouts",	     &osc_timeouts_fops, NULL, 0 },
+	{ "contention_seconds", &osc_contention_seconds_fops, NULL },
+	{ "lockless_truncate",  &osc_lockless_truncate_fops, NULL },
+	{ "import",		&osc_import_fops, NULL },
+	{ "state",		&osc_state_fops, NULL, 0 },
+	{ "pinger_recov",	&osc_pinger_recov_fops, NULL },
+	{ NULL }
 };
 
 LPROC_SEQ_FOPS_RO_TYPE(osc, numrefs);
 static struct lprocfs_vars lprocfs_osc_module_vars[] = {
-	{ "num_refs",	&osc_numrefs_fops,     0, 0 },
-	{ 0 }
+	{ "num_refs",	&osc_numrefs_fops,     NULL, 0 },
+	{ NULL }
 };
 
 #define pct(a,b) (b ? a * 100 / b : 0)
diff --git a/drivers/staging/lustre/lustre/osc/osc_cache.c b/drivers/staging/lustre/lustre/osc/osc_cache.c
index fe9989a49f8..00f38eeb578 100644
--- a/drivers/staging/lustre/lustre/osc/osc_cache.c
+++ b/drivers/staging/lustre/lustre/osc/osc_cache.c
@@ -1311,7 +1311,7 @@ static int osc_completion(const struct lu_env *env, struct osc_async_page *oap,
 static void osc_consume_write_grant(struct client_obd *cli,
 				    struct brw_page *pga)
 {
-	LASSERT(spin_is_locked(&cli->cl_loi_list_lock.lock));
+	assert_spin_locked(&cli->cl_loi_list_lock.lock);
 	LASSERT(!(pga->flag & OBD_BRW_FROM_GRANT));
 	atomic_inc(&obd_dirty_pages);
 	cli->cl_dirty += PAGE_CACHE_SIZE;
@@ -1326,7 +1326,7 @@ static void osc_consume_write_grant(struct client_obd *cli,
 static void osc_release_write_grant(struct client_obd *cli,
 				    struct brw_page *pga)
 {
-	LASSERT(spin_is_locked(&cli->cl_loi_list_lock.lock));
+	assert_spin_locked(&cli->cl_loi_list_lock.lock);
 	if (!(pga->flag & OBD_BRW_FROM_GRANT)) {
 		return;
 	}
diff --git a/drivers/staging/lustre/lustre/osc/osc_cl_internal.h b/drivers/staging/lustre/lustre/osc/osc_cl_internal.h
index 9e7899fa4cc..e74b7bb9776 100644
--- a/drivers/staging/lustre/lustre/osc/osc_cl_internal.h
+++ b/drivers/staging/lustre/lustre/osc/osc_cl_internal.h
@@ -176,7 +176,16 @@ static inline void osc_object_unlock(struct osc_object *obj)
 
 static inline int osc_object_is_locked(struct osc_object *obj)
 {
+#if defined(CONFIG_SMP) || defined(CONFIG_DEBUG_SPINLOCK)
 	return spin_is_locked(&obj->oo_lock);
+#else
+	/*
+	 * It is not perfect to return true all the time.
+	 * But since this function is only used for assertion
+	 * and checking, it seems OK.
+	 */
+	return 1;
+#endif
 }
 
 /*
diff --git a/drivers/staging/lustre/lustre/osc/osc_dev.c b/drivers/staging/lustre/lustre/osc/osc_dev.c
index 35f25786763..a7c1ec0d56f 100644
--- a/drivers/staging/lustre/lustre/osc/osc_dev.c
+++ b/drivers/staging/lustre/lustre/osc/osc_dev.c
@@ -122,7 +122,7 @@ static void *osc_key_init(const struct lu_context *ctx,
 {
 	struct osc_thread_info *info;
 
-	OBD_SLAB_ALLOC_PTR_GFP(info, osc_thread_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(info, osc_thread_kmem, GFP_NOFS);
 	if (info == NULL)
 		info = ERR_PTR(-ENOMEM);
 	return info;
@@ -146,7 +146,7 @@ static void *osc_session_init(const struct lu_context *ctx,
 {
 	struct osc_session *info;
 
-	OBD_SLAB_ALLOC_PTR_GFP(info, osc_session_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(info, osc_session_kmem, GFP_NOFS);
 	if (info == NULL)
 		info = ERR_PTR(-ENOMEM);
 	return info;
@@ -193,7 +193,7 @@ static int osc_device_init(const struct lu_env *env, struct lu_device *d,
 static struct lu_device *osc_device_fini(const struct lu_env *env,
 					 struct lu_device *d)
 {
-	return 0;
+	return NULL;
 }
 
 static struct lu_device *osc_device_free(const struct lu_env *env,
diff --git a/drivers/staging/lustre/lustre/osc/osc_io.c b/drivers/staging/lustre/lustre/osc/osc_io.c
index 5f3c545418d..09e06eb0853 100644
--- a/drivers/staging/lustre/lustre/osc/osc_io.c
+++ b/drivers/staging/lustre/lustre/osc/osc_io.c
@@ -509,12 +509,11 @@ static void osc_io_setattr_end(const struct lu_env *env,
 static int osc_io_read_start(const struct lu_env *env,
 			     const struct cl_io_slice *slice)
 {
-	struct osc_io    *oio   = cl2osc_io(env, slice);
 	struct cl_object *obj   = slice->cis_obj;
 	struct cl_attr   *attr  = &osc_env_info(env)->oti_attr;
 	int rc = 0;
 
-	if (oio->oi_lockless == 0 && !slice->cis_io->ci_noatime) {
+	if (!slice->cis_io->ci_noatime) {
 		cl_object_attr_lock(obj);
 		attr->cat_atime = LTIME_S(CURRENT_TIME);
 		rc = cl_object_attr_set(env, obj, attr, CAT_ATIME);
@@ -526,24 +525,17 @@ static int osc_io_read_start(const struct lu_env *env,
 static int osc_io_write_start(const struct lu_env *env,
 			      const struct cl_io_slice *slice)
 {
-	struct osc_io    *oio   = cl2osc_io(env, slice);
 	struct cl_object *obj   = slice->cis_obj;
 	struct cl_attr   *attr  = &osc_env_info(env)->oti_attr;
-	int	      result = 0;
+	int rc = 0;
 
-	if (oio->oi_lockless == 0) {
-		OBD_FAIL_TIMEOUT(OBD_FAIL_OSC_DELAY_SETTIME, 1);
-		cl_object_attr_lock(obj);
-		result = cl_object_attr_get(env, obj, attr);
-		if (result == 0) {
-			attr->cat_mtime = attr->cat_ctime =
-				LTIME_S(CURRENT_TIME);
-			result = cl_object_attr_set(env, obj, attr,
-						    CAT_MTIME | CAT_CTIME);
-		}
-		cl_object_attr_unlock(obj);
-	}
-	return result;
+	OBD_FAIL_TIMEOUT(OBD_FAIL_OSC_DELAY_SETTIME, 1);
+	cl_object_attr_lock(obj);
+	attr->cat_mtime = attr->cat_ctime = LTIME_S(CURRENT_TIME);
+	rc = cl_object_attr_set(env, obj, attr, CAT_MTIME | CAT_CTIME);
+	cl_object_attr_unlock(obj);
+
+	return rc;
 }
 
 static int osc_fsync_ost(const struct lu_env *env, struct osc_object *obj,
@@ -812,7 +804,7 @@ int osc_req_init(const struct lu_env *env, struct cl_device *dev,
 	struct osc_req *or;
 	int result;
 
-	OBD_SLAB_ALLOC_PTR_GFP(or, osc_req_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(or, osc_req_kmem, GFP_NOFS);
 	if (or != NULL) {
 		cl_req_slice_add(req, &or->or_cl, dev, &osc_req_ops);
 		result = 0;
diff --git a/drivers/staging/lustre/lustre/osc/osc_lock.c b/drivers/staging/lustre/lustre/osc/osc_lock.c
index ef7b9c2b208..a46129bec17 100644
--- a/drivers/staging/lustre/lustre/osc/osc_lock.c
+++ b/drivers/staging/lustre/lustre/osc/osc_lock.c
@@ -1192,6 +1192,7 @@ static int osc_lock_wait(const struct lu_env *env,
 
 		LASSERT(olck->ols_agl);
 		olck->ols_agl = 0;
+		olck->ols_flags &= ~LDLM_FL_BLOCK_NOWAIT;
 		rc = osc_lock_enqueue(env, slice, NULL, CEF_ASYNC | CEF_MUST);
 		if (rc != 0)
 			return rc;
@@ -1558,7 +1559,7 @@ int osc_lock_init(const struct lu_env *env,
 	struct osc_lock *clk;
 	int result;
 
-	OBD_SLAB_ALLOC_PTR_GFP(clk, osc_lock_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(clk, osc_lock_kmem, GFP_NOFS);
 	if (clk != NULL) {
 		__u32 enqflags = lock->cll_descr.cld_enq_flags;
 
diff --git a/drivers/staging/lustre/lustre/osc/osc_object.c b/drivers/staging/lustre/lustre/osc/osc_object.c
index 9d34de873fa..f9bfdc82012 100644
--- a/drivers/staging/lustre/lustre/osc/osc_object.c
+++ b/drivers/staging/lustre/lustre/osc/osc_object.c
@@ -260,7 +260,7 @@ struct lu_object *osc_object_alloc(const struct lu_env *env,
 	struct osc_object *osc;
 	struct lu_object  *obj;
 
-	OBD_SLAB_ALLOC_PTR_GFP(osc, osc_object_kmem, __GFP_IO);
+	OBD_SLAB_ALLOC_PTR_GFP(osc, osc_object_kmem, GFP_NOFS);
 	if (osc != NULL) {
 		obj = osc2lu(osc);
 		lu_object_init(obj, NULL, dev);
diff --git a/drivers/staging/lustre/lustre/osc/osc_request.c b/drivers/staging/lustre/lustre/osc/osc_request.c
index ee6953ac735..294db843012 100644
--- a/drivers/staging/lustre/lustre/osc/osc_request.c
+++ b/drivers/staging/lustre/lustre/osc/osc_request.c
@@ -44,7 +44,6 @@
 #include <lustre/lustre_user.h>
 #include <obd_cksum.h>
 #include <obd_ost.h>
-#include <obd_lov.h>
 
 #include <lustre_ha.h>
 #include <lprocfs_status.h>
diff --git a/drivers/staging/lustre/lustre/ptlrpc/client.c b/drivers/staging/lustre/lustre/ptlrpc/client.c
index 4c9e0069508..7246e8ce9c1 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/client.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/client.c
@@ -379,6 +379,34 @@ static int ptlrpc_at_recv_early_reply(struct ptlrpc_request *req)
 	return rc;
 }
 
+struct kmem_cache *request_cache;
+
+int ptlrpc_request_cache_init(void)
+{
+	request_cache = kmem_cache_create("ptlrpc_cache",
+					  sizeof(struct ptlrpc_request),
+					  0, SLAB_HWCACHE_ALIGN, NULL);
+	return request_cache == NULL ? -ENOMEM : 0;
+}
+
+void ptlrpc_request_cache_fini(void)
+{
+	kmem_cache_destroy(request_cache);
+}
+
+struct ptlrpc_request *ptlrpc_request_cache_alloc(int flags)
+{
+	struct ptlrpc_request *req;
+
+	OBD_SLAB_ALLOC_PTR_GFP(req, request_cache, flags);
+	return req;
+}
+
+void ptlrpc_request_cache_free(struct ptlrpc_request *req)
+{
+	OBD_SLAB_FREE_PTR(req, request_cache);
+}
+
 /**
  * Wind down request pool \a pool.
  * Frees all requests from the pool too
@@ -397,7 +425,7 @@ void ptlrpc_free_rq_pool(struct ptlrpc_request_pool *pool)
 		LASSERT(req->rq_reqbuf);
 		LASSERT(req->rq_reqbuf_len == pool->prp_rq_size);
 		OBD_FREE_LARGE(req->rq_reqbuf, pool->prp_rq_size);
-		OBD_FREE(req, sizeof(*req));
+		ptlrpc_request_cache_free(req);
 	}
 	spin_unlock(&pool->prp_lock);
 	OBD_FREE(pool, sizeof(*pool));
@@ -427,12 +455,12 @@ void ptlrpc_add_rqs_to_pool(struct ptlrpc_request_pool *pool, int num_rq)
 		struct lustre_msg *msg;
 
 		spin_unlock(&pool->prp_lock);
-		OBD_ALLOC(req, sizeof(struct ptlrpc_request));
+		req = ptlrpc_request_cache_alloc(GFP_NOFS);
 		if (!req)
 			return;
 		OBD_ALLOC_LARGE(msg, size);
 		if (!msg) {
-			OBD_FREE(req, sizeof(struct ptlrpc_request));
+			ptlrpc_request_cache_free(req);
 			return;
 		}
 		req->rq_reqbuf = msg;
@@ -668,7 +696,7 @@ struct ptlrpc_request *__ptlrpc_request_alloc(struct obd_import *imp,
 		request = ptlrpc_prep_req_from_pool(pool);
 
 	if (!request)
-		OBD_ALLOC_PTR(request);
+		request = ptlrpc_request_cache_alloc(GFP_NOFS);
 
 	if (request) {
 		LASSERTF((unsigned long)imp > 0x1000, "%p", imp);
@@ -739,7 +767,7 @@ void ptlrpc_request_free(struct ptlrpc_request *request)
 	if (request->rq_pool)
 		__ptlrpc_free_req_to_pool(request);
 	else
-		OBD_FREE_PTR(request);
+		ptlrpc_request_cache_free(request);
 }
 EXPORT_SYMBOL(ptlrpc_request_free);
 
@@ -2233,7 +2261,7 @@ static void __ptlrpc_free_req(struct ptlrpc_request *request, int locked)
 	if (request->rq_pool)
 		__ptlrpc_free_req_to_pool(request);
 	else
-		OBD_FREE(request, sizeof(*request));
+		ptlrpc_request_cache_free(request);
 }
 
 static int __ptlrpc_req_finished(struct ptlrpc_request *request, int locked);
@@ -2243,7 +2271,7 @@ static int __ptlrpc_req_finished(struct ptlrpc_request *request, int locked);
  */
 void ptlrpc_req_finished_with_imp_lock(struct ptlrpc_request *request)
 {
-	LASSERT(spin_is_locked(&request->rq_import->imp_lock));
+	assert_spin_locked(&request->rq_import->imp_lock);
 	(void)__ptlrpc_req_finished(request, 1);
 }
 EXPORT_SYMBOL(ptlrpc_req_finished_with_imp_lock);
@@ -2424,9 +2452,7 @@ void ptlrpc_free_committed(struct obd_import *imp)
 	bool		       skip_committed_list = true;
 
 	LASSERT(imp != NULL);
-
-	LASSERT(spin_is_locked(&imp->imp_lock));
-
+	assert_spin_locked(&imp->imp_lock);
 
 	if (imp->imp_peer_committed_transno == imp->imp_last_transno_checked &&
 	    imp->imp_generation == imp->imp_last_generation_checked) {
@@ -2557,7 +2583,7 @@ void ptlrpc_retain_replayable_request(struct ptlrpc_request *req,
 {
 	struct list_head *tmp;
 
-	LASSERT(spin_is_locked(&imp->imp_lock));
+	assert_spin_locked(&imp->imp_lock);
 
 	if (req->rq_transno == 0) {
 		DEBUG_REQ(D_EMERG, req, "saving request with zero transno");
@@ -3023,7 +3049,7 @@ void *ptlrpcd_alloc_work(struct obd_import *imp,
 		return ERR_PTR(-EINVAL);
 
 	/* copy some code from deprecated fakereq. */
-	OBD_ALLOC_PTR(req);
+	req = ptlrpc_request_cache_alloc(GFP_NOFS);
 	if (req == NULL) {
 		CERROR("ptlrpc: run out of memory!\n");
 		return ERR_PTR(-ENOMEM);
diff --git a/drivers/staging/lustre/lustre/ptlrpc/events.c b/drivers/staging/lustre/lustre/ptlrpc/events.c
index 6ea0a491cfb..aa85239f6cd 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/events.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/events.c
@@ -307,7 +307,7 @@ void request_in_callback(lnet_event_t *ev)
 			/* We moaned above already... */
 			return;
 		}
-		OBD_ALLOC_GFP(req, sizeof(*req), ALLOC_ATOMIC_TRY);
+		req = ptlrpc_request_cache_alloc(ALLOC_ATOMIC_TRY);
 		if (req == NULL) {
 			CERROR("Can't allocate incoming request descriptor: "
 			       "Dropping %s RPC from %s\n",
diff --git a/drivers/staging/lustre/lustre/ptlrpc/gss/gss_bulk.c b/drivers/staging/lustre/lustre/ptlrpc/gss/gss_bulk.c
index 7852bf30a3a..93794bd928c 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/gss/gss_bulk.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/gss/gss_bulk.c
@@ -176,31 +176,31 @@ int gss_cli_ctx_unwrap_bulk(struct ptlrpc_cli_ctx *ctx,
 	switch (SPTLRPC_FLVR_SVC(req->rq_flvr.sf_rpc)) {
 	case SPTLRPC_SVC_NULL:
 		vmsg = req->rq_repdata;
+		LASSERT(vmsg != NULL && vmsg->lm_bufcount >= 3);
 		voff = vmsg->lm_bufcount - 1;
-		LASSERT(vmsg && vmsg->lm_bufcount >= 3);
 
 		rmsg = req->rq_reqbuf;
+		LASSERT(rmsg != NULL && rmsg->lm_bufcount >= 3);
 		roff = rmsg->lm_bufcount - 1; /* last segment */
-		LASSERT(rmsg && rmsg->lm_bufcount >= 3);
 		break;
 	case SPTLRPC_SVC_AUTH:
 	case SPTLRPC_SVC_INTG:
 		vmsg = req->rq_repdata;
+		LASSERT(vmsg != NULL && vmsg->lm_bufcount >= 4);
 		voff = vmsg->lm_bufcount - 2;
-		LASSERT(vmsg && vmsg->lm_bufcount >= 4);
 
 		rmsg = req->rq_reqbuf;
+		LASSERT(rmsg != NULL && rmsg->lm_bufcount >= 4);
 		roff = rmsg->lm_bufcount - 2; /* second last segment */
-		LASSERT(rmsg && rmsg->lm_bufcount >= 4);
 		break;
 	case SPTLRPC_SVC_PRIV:
 		vmsg = req->rq_repdata;
+		LASSERT(vmsg != NULL && vmsg->lm_bufcount >= 2);
 		voff = vmsg->lm_bufcount - 1;
-		LASSERT(vmsg && vmsg->lm_bufcount >= 2);
 
 		rmsg = req->rq_clrbuf;
+		LASSERT(rmsg != NULL && rmsg->lm_bufcount >= 2);
 		roff = rmsg->lm_bufcount - 1; /* last segment */
-		LASSERT(rmsg && rmsg->lm_bufcount >= 2);
 		break;
 	default:
 		LBUG();
diff --git a/drivers/staging/lustre/lustre/ptlrpc/gss/gss_pipefs.c b/drivers/staging/lustre/lustre/ptlrpc/gss/gss_pipefs.c
index 7a1ff4fecbe..3be5bc14c4e 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/gss/gss_pipefs.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/gss/gss_pipefs.c
@@ -137,7 +137,7 @@ void ctx_enhash_pf(struct ptlrpc_cli_ctx *ctx, struct hlist_head *hash)
 static
 void ctx_unhash_pf(struct ptlrpc_cli_ctx *ctx, struct hlist_head *freelist)
 {
-	LASSERT(spin_is_locked(&ctx->cc_sec->ps_lock));
+	assert_spin_locked(&ctx->cc_sec->ps_lock);
 	LASSERT(atomic_read(&ctx->cc_refcount) > 0);
 	LASSERT(test_bit(PTLRPC_CTX_CACHED_BIT, &ctx->cc_flags));
 	LASSERT(!hlist_unhashed(&ctx->cc_cache));
@@ -719,7 +719,7 @@ void gss_unhash_msg_nolock(struct gss_upcall_msg *gmsg)
 	__u32 idx = gmsg->gum_mechidx;
 
 	LASSERT(idx < MECH_MAX);
-	LASSERT(spin_is_locked(&upcall_locks[idx]));
+	assert_spin_locked(&upcall_locks[idx]);
 
 	if (list_empty(&gmsg->gum_list))
 		return;
diff --git a/drivers/staging/lustre/lustre/ptlrpc/import.c b/drivers/staging/lustre/lustre/ptlrpc/import.c
index 537aa6204a5..8573f328bd2 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/import.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/import.c
@@ -194,7 +194,7 @@ int ptlrpc_set_import_discon(struct obd_import *imp, __u32 conn_cnt)
 /* Must be called with imp_lock held! */
 static void ptlrpc_deactivate_and_unlock_import(struct obd_import *imp)
 {
-	LASSERT(spin_is_locked(&imp->imp_lock));
+	assert_spin_locked(&imp->imp_lock);
 
 	CDEBUG(D_HA, "setting import %s INVALID\n", obd2cli_tgt(imp->imp_obd));
 	imp->imp_invalid = 1;
@@ -381,6 +381,11 @@ void ptlrpc_activate_import(struct obd_import *imp)
 	struct obd_device *obd = imp->imp_obd;
 
 	spin_lock(&imp->imp_lock);
+	if (imp->imp_deactive != 0) {
+		spin_unlock(&imp->imp_lock);
+		return;
+	}
+
 	imp->imp_invalid = 0;
 	spin_unlock(&imp->imp_lock);
 	obd_import_event(obd, imp, IMP_EVENT_ACTIVE);
@@ -640,7 +645,7 @@ int ptlrpc_connect_import(struct obd_import *imp)
 	if (rc)
 		GOTO(out, rc);
 
-	rc = sptlrpc_import_sec_adapt(imp, NULL, 0);
+	rc = sptlrpc_import_sec_adapt(imp, NULL, NULL);
 	if (rc)
 		GOTO(out, rc);
 
@@ -1399,26 +1404,33 @@ int ptlrpc_disconnect_import(struct obd_import *imp, int noclose)
 {
 	struct ptlrpc_request *req;
 	int rq_opc, rc = 0;
-	int nowait = imp->imp_obd->obd_force;
 
-	if (nowait)
+	if (imp->imp_obd->obd_force)
 		GOTO(set_state, rc);
 
 	switch (imp->imp_connect_op) {
-	case OST_CONNECT: rq_opc = OST_DISCONNECT; break;
-	case MDS_CONNECT: rq_opc = MDS_DISCONNECT; break;
-	case MGS_CONNECT: rq_opc = MGS_DISCONNECT; break;
+	case OST_CONNECT:
+		rq_opc = OST_DISCONNECT;
+		break;
+	case MDS_CONNECT:
+		rq_opc = MDS_DISCONNECT;
+		break;
+	case MGS_CONNECT:
+		rq_opc = MGS_DISCONNECT;
+		break;
 	default:
-		CERROR("don't know how to disconnect from %s (connect_op %d)\n",
-		       obd2cli_tgt(imp->imp_obd), imp->imp_connect_op);
-		return -EINVAL;
+		rc = -EINVAL;
+		CERROR("%s: don't know how to disconnect from %s "
+		       "(connect_op %d): rc = %d\n",
+		       imp->imp_obd->obd_name, obd2cli_tgt(imp->imp_obd),
+		       imp->imp_connect_op, rc);
+		return rc;
 	}
 
 	if (ptlrpc_import_in_recovery(imp)) {
 		struct l_wait_info lwi;
 		cfs_duration_t timeout;
 
-
 		if (AT_OFF) {
 			if (imp->imp_server_timeout)
 				timeout = cfs_time_seconds(obd_timeout / 2);
@@ -1441,7 +1453,6 @@ int ptlrpc_disconnect_import(struct obd_import *imp, int noclose)
 	spin_lock(&imp->imp_lock);
 	if (imp->imp_state != LUSTRE_IMP_FULL)
 		GOTO(out, 0);
-
 	spin_unlock(&imp->imp_lock);
 
 	req = ptlrpc_request_alloc_pack(imp, &RQF_MDS_DISCONNECT,
@@ -1474,6 +1485,9 @@ out:
 	memset(&imp->imp_remote_handle, 0, sizeof(imp->imp_remote_handle));
 	spin_unlock(&imp->imp_lock);
 
+	if (rc == -ETIMEDOUT || rc == -ENOTCONN || rc == -ESHUTDOWN)
+		rc = 0;
+
 	return rc;
 }
 EXPORT_SYMBOL(ptlrpc_disconnect_import);
diff --git a/drivers/staging/lustre/lustre/ptlrpc/lproc_ptlrpc.c b/drivers/staging/lustre/lustre/ptlrpc/lproc_ptlrpc.c
index 58f1c8bf25b..6b9c6db1f2d 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/lproc_ptlrpc.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/lproc_ptlrpc.c
@@ -449,7 +449,7 @@ void nrs_policy_get_info_locked(struct ptlrpc_nrs_policy *policy,
 {
 	LASSERT(policy != NULL);
 	LASSERT(info != NULL);
-	LASSERT(spin_is_locked(&policy->pol_nrs->nrs_lock));
+	assert_spin_locked(&policy->pol_nrs->nrs_lock);
 
 	memcpy(info->pi_name, policy->pol_desc->pd_name, NRS_POL_NAME_MAX);
 
@@ -1190,7 +1190,7 @@ int lprocfs_wr_evict_client(struct file *file, const char *buffer,
 	 * the proc entries under the being destroyed export{}, so I have
 	 * to drop the lock at first here.
 	 * - jay, jxiong@clusterfs.com */
-	class_incref(obd, __FUNCTION__, current);
+	class_incref(obd, __func__, current);
 
 	if (strncmp(tmpbuf, "nid:", 4) == 0)
 		obd_export_evict_by_nid(obd, tmpbuf + 4);
@@ -1199,7 +1199,7 @@ int lprocfs_wr_evict_client(struct file *file, const char *buffer,
 	else
 		obd_export_evict_by_uuid(obd, tmpbuf);
 
-	class_decref(obd, __FUNCTION__, current);
+	class_decref(obd, __func__, current);
 
 out:
 	OBD_FREE(kbuf, BUFLEN);
diff --git a/drivers/staging/lustre/lustre/ptlrpc/nrs.c b/drivers/staging/lustre/lustre/ptlrpc/nrs.c
index bcba1c8e869..12151aa2a1e 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/nrs.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/nrs.c
@@ -746,7 +746,7 @@ static int nrs_policy_register(struct ptlrpc_nrs *nrs,
 	LASSERT(desc->pd_compat != NULL);
 
 	OBD_CPT_ALLOC_GFP(policy, svcpt->scp_service->srv_cptable,
-			  svcpt->scp_cpt, sizeof(*policy), __GFP_IO);
+			  svcpt->scp_cpt, sizeof(*policy), GFP_NOFS);
 	if (policy == NULL)
 		return -ENOMEM;
 
diff --git a/drivers/staging/lustre/lustre/ptlrpc/pack_generic.c b/drivers/staging/lustre/lustre/ptlrpc/pack_generic.c
index 45c0b84621e..cddeeb6bb23 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/pack_generic.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/pack_generic.c
@@ -300,6 +300,7 @@ lustre_get_emerg_rs(struct ptlrpc_service_part *svcpt)
 	spin_unlock(&svcpt->scp_rep_lock);
 
 	memset(rs, 0, svcpt->scp_service->srv_max_reply_size);
+	rs->rs_size = svcpt->scp_service->srv_max_reply_size;
 	rs->rs_svcpt = svcpt;
 	rs->rs_prealloc = 1;
 out:
diff --git a/drivers/staging/lustre/lustre/ptlrpc/pinger.c b/drivers/staging/lustre/lustre/ptlrpc/pinger.c
index 6dff502ce23..38099d9dfda 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/pinger.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/pinger.c
@@ -368,7 +368,7 @@ EXPORT_SYMBOL(ptlrpc_pinger_sending_on_import);
 void ptlrpc_pinger_commit_expected(struct obd_import *imp)
 {
 	ptlrpc_update_next_ping(imp, 1);
-	LASSERT(spin_is_locked(&imp->imp_lock));
+	assert_spin_locked(&imp->imp_lock);
 	/*
 	 * Avoid reading stale imp_connect_data.  When not sure if pings are
 	 * expected or not on next connection, we assume they are not and force
diff --git a/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h b/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h
index e3b5a920bca..7c940553059 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h
+++ b/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h
@@ -55,6 +55,10 @@ int ptlrpcd_start(int index, int max, const char *name, struct ptlrpcd_ctl *pc);
 /* client.c */
 struct ptlrpc_bulk_desc *ptlrpc_new_bulk(unsigned npages, unsigned max_brw,
 					 unsigned type, unsigned portal);
+int ptlrpc_request_cache_init(void);
+void ptlrpc_request_cache_fini(void);
+struct ptlrpc_request *ptlrpc_request_cache_alloc(int flags);
+void ptlrpc_request_cache_free(struct ptlrpc_request *req);
 void ptlrpc_init_xid(void);
 
 /* events.c */
diff --git a/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_module.c b/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_module.c
index 0efd35887a1..251ae75c2dd 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_module.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_module.c
@@ -73,29 +73,34 @@ __init int ptlrpc_init(void)
 		return rc;
 
 	cleanup_phase = 1;
+	rc = ptlrpc_request_cache_init();
+	if (rc)
+		GOTO(cleanup, rc);
 
+	cleanup_phase = 2;
 	rc = ptlrpc_init_portals();
 	if (rc)
 		GOTO(cleanup, rc);
-	cleanup_phase = 2;
+
+	cleanup_phase = 3;
 
 	rc = ptlrpc_connection_init();
 	if (rc)
 		GOTO(cleanup, rc);
-	cleanup_phase = 3;
 
+	cleanup_phase = 4;
 	ptlrpc_put_connection_superhack = ptlrpc_connection_put;
 
 	rc = ptlrpc_start_pinger();
 	if (rc)
 		GOTO(cleanup, rc);
-	cleanup_phase = 4;
 
+	cleanup_phase = 5;
 	rc = ldlm_init();
 	if (rc)
 		GOTO(cleanup, rc);
-	cleanup_phase = 5;
 
+	cleanup_phase = 6;
 	rc = sptlrpc_init();
 	if (rc)
 		GOTO(cleanup, rc);
@@ -115,19 +120,29 @@ cleanup:
 	switch (cleanup_phase) {
 	case 8:
 		ptlrpc_nrs_fini();
+		/* Fall through */
 	case 7:
 		sptlrpc_fini();
-	case 5:
+		/* Fall through */
+	case 6:
 		ldlm_exit();
-	case 4:
+		/* Fall through */
+	case 5:
 		ptlrpc_stop_pinger();
-	case 3:
+		/* Fall through */
+	case 4:
 		ptlrpc_connection_fini();
-	case 2:
+		/* Fall through */
+	case 3:
 		ptlrpc_exit_portals();
+		/* Fall through */
+	case 2:
+		ptlrpc_request_cache_fini();
+		/* Fall through */
 	case 1:
 		ptlrpc_hr_fini();
 		req_layout_fini();
+		/* Fall through */
 	default: ;
 	}
 
@@ -142,6 +157,7 @@ static void __exit ptlrpc_exit(void)
 	ldlm_exit();
 	ptlrpc_stop_pinger();
 	ptlrpc_exit_portals();
+	ptlrpc_request_cache_fini();
 	ptlrpc_hr_fini();
 	ptlrpc_connection_fini();
 }
diff --git a/drivers/staging/lustre/lustre/ptlrpc/recover.c b/drivers/staging/lustre/lustre/ptlrpc/recover.c
index 48ae328ce24..9cec8a649dc 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/recover.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/recover.c
@@ -49,7 +49,6 @@
 #include <obd.h>
 #include <obd_ost.h>
 #include <obd_class.h>
-#include <obd_lov.h> /* for IOC_LOV_SET_OSC_ACTIVE */
 #include <linux/list.h>
 
 #include "ptlrpc_internal.h"
@@ -369,11 +368,14 @@ EXPORT_SYMBOL(ptlrpc_recover_import);
 int ptlrpc_import_in_recovery(struct obd_import *imp)
 {
 	int in_recovery = 1;
+
 	spin_lock(&imp->imp_lock);
 	if (imp->imp_state == LUSTRE_IMP_FULL ||
 	    imp->imp_state == LUSTRE_IMP_CLOSED ||
-	    imp->imp_state == LUSTRE_IMP_DISCON)
+	    imp->imp_state == LUSTRE_IMP_DISCON ||
+	    imp->imp_obd->obd_no_recov)
 		in_recovery = 0;
 	spin_unlock(&imp->imp_lock);
+
 	return in_recovery;
 }
diff --git a/drivers/staging/lustre/lustre/ptlrpc/sec.c b/drivers/staging/lustre/lustre/ptlrpc/sec.c
index d80418057f2..28ac824a73f 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/sec.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/sec.c
@@ -354,7 +354,7 @@ static int import_sec_check_expire(struct obd_import *imp)
 		return 0;
 
 	CDEBUG(D_SEC, "found delayed sec adapt expired, do it now\n");
-	return sptlrpc_import_sec_adapt(imp, NULL, 0);
+	return sptlrpc_import_sec_adapt(imp, NULL, NULL);
 }
 
 static int import_sec_validate_get(struct obd_import *imp,
@@ -904,7 +904,7 @@ int sptlrpc_import_check_ctx(struct obd_import *imp)
 		return -EACCES;
 	}
 
-	OBD_ALLOC_PTR(req);
+	req = ptlrpc_request_cache_alloc(GFP_NOFS);
 	if (!req)
 		return -ENOMEM;
 
@@ -920,7 +920,7 @@ int sptlrpc_import_check_ctx(struct obd_import *imp)
 	rc = sptlrpc_req_refresh_ctx(req, 0);
 	LASSERT(list_empty(&req->rq_ctx_chain));
 	sptlrpc_cli_ctx_put(req->rq_cli_ctx, 1);
-	OBD_FREE_PTR(req);
+	ptlrpc_request_cache_free(req);
 
 	return rc;
 }
@@ -1088,7 +1088,7 @@ int sptlrpc_cli_unwrap_early_reply(struct ptlrpc_request *req,
 	int		     early_bufsz, early_size;
 	int		     rc;
 
-	OBD_ALLOC_PTR(early_req);
+	early_req = ptlrpc_request_cache_alloc(GFP_NOFS);
 	if (early_req == NULL)
 		return -ENOMEM;
 
@@ -1160,7 +1160,7 @@ err_ctx:
 err_buf:
 	OBD_FREE_LARGE(early_buf, early_bufsz);
 err_req:
-	OBD_FREE_PTR(early_req);
+	ptlrpc_request_cache_free(early_req);
 	return rc;
 }
 
@@ -1177,7 +1177,7 @@ void sptlrpc_cli_finish_early_reply(struct ptlrpc_request *early_req)
 
 	sptlrpc_cli_ctx_put(early_req->rq_cli_ctx, 1);
 	OBD_FREE_LARGE(early_req->rq_repbuf, early_req->rq_repbuf_len);
-	OBD_FREE_PTR(early_req);
+	ptlrpc_request_cache_free(early_req);
 }
 
 /**************************************************
@@ -2086,8 +2086,18 @@ int sptlrpc_svc_alloc_rs(struct ptlrpc_request *req, int msglen)
 
 	rc = policy->sp_sops->alloc_rs(req, msglen);
 	if (unlikely(rc == -ENOMEM)) {
+		struct ptlrpc_service_part *svcpt = req->rq_rqbd->rqbd_svcpt;
+		if (svcpt->scp_service->srv_max_reply_size <
+		   msglen + sizeof(struct ptlrpc_reply_state)) {
+			/* Just return failure if the size is too big */
+			CERROR("size of message is too big (%zd), %d allowed",
+				msglen + sizeof(struct ptlrpc_reply_state),
+				svcpt->scp_service->srv_max_reply_size);
+			return -ENOMEM;
+		}
+
 		/* failed alloc, try emergency pool */
-		rs = lustre_get_emerg_rs(req->rq_rqbd->rqbd_svcpt);
+		rs = lustre_get_emerg_rs(svcpt);
 		if (rs == NULL)
 			return -ENOMEM;
 
diff --git a/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c b/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c
index 96566813274..9d51badea73 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c
@@ -421,7 +421,7 @@ static int enc_pools_add_pages(int npages)
 			goto out_pools;
 
 		for (j = 0; j < PAGES_PER_POOL && alloced < npages; j++) {
-			pools[i][j] = alloc_page(__GFP_IO |
+			pools[i][j] = alloc_page(GFP_NOFS |
 						     __GFP_HIGHMEM);
 			if (pools[i][j] == NULL)
 				goto out_pools;
@@ -450,7 +450,7 @@ out:
 
 static inline void enc_pools_wakeup(void)
 {
-	LASSERT(spin_is_locked(&page_pools.epp_lock));
+	assert_spin_locked(&page_pools.epp_lock);
 	LASSERT(page_pools.epp_waitqlen >= 0);
 
 	if (unlikely(page_pools.epp_waitqlen)) {
diff --git a/drivers/staging/lustre/lustre/ptlrpc/sec_config.c b/drivers/staging/lustre/lustre/ptlrpc/sec_config.c
index bf56120abfa..231656ed766 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/sec_config.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/sec_config.c
@@ -745,11 +745,13 @@ void sptlrpc_conf_log_update_begin(const char *logname)
 	mutex_lock(&sptlrpc_conf_lock);
 
 	conf = sptlrpc_conf_get(fsname, 0);
-	if (conf && conf->sc_local) {
-		LASSERT(conf->sc_updated == 0);
-		sptlrpc_conf_free_rsets(conf);
+	if (conf) {
+		if(conf->sc_local) {
+			LASSERT(conf->sc_updated == 0);
+			sptlrpc_conf_free_rsets(conf);
+		}
+		conf->sc_modified = 0;
 	}
-	conf->sc_modified = 0;
 
 	mutex_unlock(&sptlrpc_conf_lock);
 }
diff --git a/drivers/staging/lustre/lustre/ptlrpc/service.c b/drivers/staging/lustre/lustre/ptlrpc/service.c
index 192adec5382..d278f2e2180 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/service.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/service.c
@@ -384,8 +384,8 @@ void ptlrpc_dispatch_difficult_reply(struct ptlrpc_reply_state *rs)
 void
 ptlrpc_schedule_difficult_reply(struct ptlrpc_reply_state *rs)
 {
-	LASSERT(spin_is_locked(&rs->rs_svcpt->scp_rep_lock));
-	LASSERT(spin_is_locked(&rs->rs_lock));
+	assert_spin_locked(&rs->rs_svcpt->scp_rep_lock);
+	assert_spin_locked(&rs->rs_lock);
 	LASSERT(rs->rs_difficult);
 	rs->rs_scheduled_ever = 1;  /* flag any notification attempt */
 
@@ -842,7 +842,7 @@ static void ptlrpc_server_free_request(struct ptlrpc_request *req)
 		/* NB request buffers use an embedded
 		 * req if the incoming req unlinked the
 		 * MD; this isn't one of them! */
-		OBD_FREE(req, sizeof(*req));
+		ptlrpc_request_cache_free(req);
 	}
 }
 
@@ -1042,9 +1042,6 @@ static void ptlrpc_update_export_timer(struct obd_export *exp, long extra_delay)
 		return;
 
 	exp->exp_last_request_time = new_time;
-	CDEBUG(D_HA, "updating export %s at "CFS_TIME_T" exp %p\n",
-	       exp->exp_client_uuid.uuid,
-	       exp->exp_last_request_time, exp);
 
 	/* exports may get disconnected from the chain even though the
 	   export has references, so we must keep the spin lock while
@@ -1308,14 +1305,12 @@ static int ptlrpc_at_send_early_reply(struct ptlrpc_request *req)
 	}
 	newdl = cfs_time_current_sec() + at_get(&svcpt->scp_at_estimate);
 
-	OBD_ALLOC(reqcopy, sizeof(*reqcopy));
+	reqcopy = ptlrpc_request_cache_alloc(GFP_NOFS);
 	if (reqcopy == NULL)
 		return -ENOMEM;
 	OBD_ALLOC_LARGE(reqmsg, req->rq_reqlen);
-	if (!reqmsg) {
-		OBD_FREE(reqcopy, sizeof(*reqcopy));
-		return -ENOMEM;
-	}
+	if (!reqmsg)
+		GOTO(out_free, rc = -ENOMEM);
 
 	*reqcopy = *req;
 	reqcopy->rq_reply_state = NULL;
@@ -1372,7 +1367,8 @@ out_put:
 out:
 	sptlrpc_svc_ctx_decref(reqcopy);
 	OBD_FREE_LARGE(reqmsg, req->rq_reqlen);
-	OBD_FREE(reqcopy, sizeof(*reqcopy));
+out_free:
+	ptlrpc_request_cache_free(reqcopy);
 	return rc;
 }
 
diff --git a/drivers/staging/media/davinci_vpfe/vpfe_video.c b/drivers/staging/media/davinci_vpfe/vpfe_video.c
index 8c101cbbee9..acc8184c46c 100644
--- a/drivers/staging/media/davinci_vpfe/vpfe_video.c
+++ b/drivers/staging/media/davinci_vpfe/vpfe_video.c
@@ -1247,9 +1247,18 @@ static int vpfe_stop_streaming(struct vb2_queue *vq)
 	struct vpfe_fh *fh = vb2_get_drv_priv(vq);
 	struct vpfe_video_device *video = fh->video;
 
-	if (!vb2_is_streaming(vq))
-		return 0;
 	/* release all active buffers */
+	if (video->cur_frm == video->next_frm) {
+		vb2_buffer_done(&video->cur_frm->vb, VB2_BUF_STATE_ERROR);
+	} else {
+		if (video->cur_frm != NULL)
+			vb2_buffer_done(&video->cur_frm->vb,
+					VB2_BUF_STATE_ERROR);
+		if (video->next_frm != NULL)
+			vb2_buffer_done(&video->next_frm->vb,
+					VB2_BUF_STATE_ERROR);
+	}
+
 	while (!list_empty(&video->dma_queue)) {
 		video->next_frm = list_entry(video->dma_queue.next,
 						struct vpfe_cap_buffer, list);
diff --git a/drivers/staging/media/lirc/lirc_bt829.c b/drivers/staging/media/lirc/lirc_bt829.c
index 30edc740ac2..fe010542ab4 100644
--- a/drivers/staging/media/lirc/lirc_bt829.c
+++ b/drivers/staging/media/lirc/lirc_bt829.c
@@ -64,7 +64,7 @@ static bool debug;
 
 static int atir_minor;
 static phys_addr_t pci_addr_phys;
-static unsigned char *pci_addr_lin;
+static unsigned char __iomem *pci_addr_lin;
 
 static struct lirc_driver atir_driver;
 
@@ -382,7 +382,7 @@ static unsigned char do_get_bits(void)
 
 static unsigned int read_index(unsigned char index)
 {
-	unsigned char *addr;
+	unsigned char __iomem *addr;
 	unsigned int value;
 	/*  addr = pci_addr_lin + DATA_PCI_OFF + ((index & 0xFF) << 2); */
 	addr = pci_addr_lin + ((index & 0xFF) << 2);
@@ -392,7 +392,7 @@ static unsigned int read_index(unsigned char index)
 
 static void write_index(unsigned char index, unsigned int reg_val)
 {
-	unsigned char *addr;
+	unsigned char __iomem *addr;
 	addr = pci_addr_lin + ((index & 0xFF) << 2);
 	writel(reg_val, addr);
 }
diff --git a/drivers/staging/media/lirc/lirc_igorplugusb.c b/drivers/staging/media/lirc/lirc_igorplugusb.c
index f508a1372ad..44b0d076608 100644
--- a/drivers/staging/media/lirc/lirc_igorplugusb.c
+++ b/drivers/staging/media/lirc/lirc_igorplugusb.c
@@ -60,19 +60,6 @@
 #define DRIVER_DESC		"Igorplug USB remote driver for LIRC"
 #define DRIVER_NAME		"lirc_igorplugusb"
 
-/* debugging support */
-#ifdef CONFIG_USB_DEBUG
-static bool debug = true;
-#else
-static bool debug;
-#endif
-
-#define dprintk(fmt, args...)					\
-	do {							\
-		if (debug)					\
-			printk(KERN_DEBUG fmt, ## args);	\
-	} while (0)
-
 /* One mode2 pulse/space has 4 bytes. */
 #define CODE_LENGTH	     sizeof(int)
 
@@ -237,7 +224,7 @@ static int unregister_from_lirc(struct igorplug *ir)
 		return -EINVAL;
 	}
 
-	dprintk(DRIVER_NAME "[%d]: calling lirc_unregister_driver\n", devnum);
+	dev_dbg(&ir->usbdev->dev, "calling lirc_unregister_driver\n");
 	lirc_unregister_driver(d->minor);
 
 	return devnum;
@@ -252,7 +239,7 @@ static int set_use_inc(void *data)
 		return -EIO;
 	}
 
-	dprintk(DRIVER_NAME "[%d]: set use inc\n", ir->devnum);
+	dev_dbg(&ir->usbdev->dev, "set use inc\n");
 
 	if (!ir->usbdev)
 		return -ENODEV;
@@ -269,7 +256,7 @@ static void set_use_dec(void *data)
 		return;
 	}
 
-	dprintk(DRIVER_NAME "[%d]: set use dec\n", ir->devnum);
+	dev_dbg(&ir->usbdev->dev, "set use dec\n");
 }
 
 static void send_fragment(struct igorplug *ir, struct lirc_buffer *buf,
@@ -321,7 +308,7 @@ static int igorplugusb_remote_poll(void *data, struct lirc_buffer *buf)
 		if (ret < DEVICE_HEADERLEN)
 			return -ENODATA;
 
-		dprintk(DRIVER_NAME ": Got %d bytes. Header: %*ph\n",
+		dev_dbg(&ir->usbdev->dev, "Got %d bytes. Header: %*ph\n",
 			ret, 3, ir->buf_in);
 
 		do_gettimeofday(&now);
@@ -385,7 +372,7 @@ static int igorplugusb_remote_probe(struct usb_interface *intf,
 	char buf[63], name[128] = "";
 	int ret;
 
-	dprintk(DRIVER_NAME ": usb probe called.\n");
+	dev_dbg(&intf->dev, "%s: usb probe called.\n", __func__);
 
 	dev = interface_to_usbdev(intf);
 
@@ -405,8 +392,8 @@ static int igorplugusb_remote_probe(struct usb_interface *intf,
 	devnum = dev->devnum;
 	maxp = usb_maxpacket(dev, pipe, usb_pipeout(pipe));
 
-	dprintk(DRIVER_NAME "[%d]: bytes_in_key=%zu maxp=%d\n",
-		devnum, CODE_LENGTH, maxp);
+	dev_dbg(&intf->dev, "%s: bytes_in_key=%zu maxp=%d\n",
+		__func__, CODE_LENGTH, maxp);
 
 	ir = devm_kzalloc(&intf->dev, sizeof(*ir), GFP_KERNEL);
 	if (!ir)
@@ -525,6 +512,3 @@ MODULE_DEVICE_TABLE(usb, igorplugusb_remote_id_table);
 
 module_param(sample_rate, int, S_IRUGO | S_IWUSR);
 MODULE_PARM_DESC(sample_rate, "Sampling rate in Hz (default: 100)");
-
-module_param(debug, bool, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(debug, "Debug enabled or not");
diff --git a/drivers/staging/media/lirc/lirc_parallel.c b/drivers/staging/media/lirc/lirc_parallel.c
index 62f5137b947..1394f020e46 100644
--- a/drivers/staging/media/lirc/lirc_parallel.c
+++ b/drivers/staging/media/lirc/lirc_parallel.c
@@ -324,7 +324,8 @@ static loff_t lirc_lseek(struct file *filep, loff_t offset, int orig)
 	return -ESPIPE;
 }
 
-static ssize_t lirc_read(struct file *filep, char *buf, size_t n, loff_t *ppos)
+static ssize_t lirc_read(struct file *filep, char __user *buf, size_t n,
+			 loff_t *ppos)
 {
 	int result = 0;
 	int count = 0;
@@ -362,7 +363,7 @@ static ssize_t lirc_read(struct file *filep, char *buf, size_t n, loff_t *ppos)
 	return count ? count : result;
 }
 
-static ssize_t lirc_write(struct file *filep, const char *buf, size_t n,
+static ssize_t lirc_write(struct file *filep, const char __user *buf, size_t n,
 			  loff_t *ppos)
 {
 	int count;
@@ -463,43 +464,44 @@ static unsigned int lirc_poll(struct file *file, poll_table *wait)
 static long lirc_ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 {
 	int result;
-	__u32 features = LIRC_CAN_SET_TRANSMITTER_MASK |
-			 LIRC_CAN_SEND_PULSE | LIRC_CAN_REC_MODE2;
-	__u32 mode;
-	__u32 value;
+	u32 __user *uptr = (u32 __user *)arg;
+	u32 features = LIRC_CAN_SET_TRANSMITTER_MASK |
+		       LIRC_CAN_SEND_PULSE | LIRC_CAN_REC_MODE2;
+	u32 mode;
+	u32 value;
 
 	switch (cmd) {
 	case LIRC_GET_FEATURES:
-		result = put_user(features, (__u32 *) arg);
+		result = put_user(features, uptr);
 		if (result)
 			return result;
 		break;
 	case LIRC_GET_SEND_MODE:
-		result = put_user(LIRC_MODE_PULSE, (__u32 *) arg);
+		result = put_user(LIRC_MODE_PULSE, uptr);
 		if (result)
 			return result;
 		break;
 	case LIRC_GET_REC_MODE:
-		result = put_user(LIRC_MODE_MODE2, (__u32 *) arg);
+		result = put_user(LIRC_MODE_MODE2, uptr);
 		if (result)
 			return result;
 		break;
 	case LIRC_SET_SEND_MODE:
-		result = get_user(mode, (__u32 *) arg);
+		result = get_user(mode, uptr);
 		if (result)
 			return result;
 		if (mode != LIRC_MODE_PULSE)
 			return -EINVAL;
 		break;
 	case LIRC_SET_REC_MODE:
-		result = get_user(mode, (__u32 *) arg);
+		result = get_user(mode, uptr);
 		if (result)
 			return result;
 		if (mode != LIRC_MODE_MODE2)
 			return -ENOSYS;
 		break;
 	case LIRC_SET_TRANSMITTER_MASK:
-		result = get_user(value, (__u32 *) arg);
+		result = get_user(value, uptr);
 		if (result)
 			return result;
 		if ((value & LIRC_PARALLEL_TRANSMITTER_MASK) != value)
diff --git a/drivers/staging/media/lirc/lirc_serial.c b/drivers/staging/media/lirc/lirc_serial.c
index 10c685d5de7..efe561cd093 100644
--- a/drivers/staging/media/lirc/lirc_serial.c
+++ b/drivers/staging/media/lirc/lirc_serial.c
@@ -73,9 +73,6 @@
 #include <linux/fcntl.h>
 #include <linux/spinlock.h>
 
-#ifdef CONFIG_LIRC_SERIAL_NSLU2
-#include <asm/hardware.h>
-#endif
 /* From Intel IXP42X Developer's Manual (#252480-005): */
 /* ftp://download.intel.com/design/network/manuals/25248005.pdf */
 #define UART_IE_IXP42X_UUE   0x40 /* IXP42X UART Unit enable */
@@ -198,33 +195,6 @@ static struct lirc_serial hardware[] = {
 		.features    = LIRC_CAN_REC_MODE2
 #endif
 	},
-
-#ifdef CONFIG_LIRC_SERIAL_NSLU2
-	/*
-	 * Modified Linksys Network Storage Link USB 2.0 (NSLU2):
-	 * We receive on CTS of the 2nd serial port (R142,LHS), we
-	 * transmit with a IR diode between GPIO[1] (green status LED),
-	 * and ground (Matthias Goebl <matthias.goebl@goebl.net>).
-	 * See also http://www.nslu2-linux.org for this device
-	 */
-	[LIRC_NSLU2] = {
-		.lock = __SPIN_LOCK_UNLOCKED(hardware[LIRC_NSLU2].lock),
-		.signal_pin        = UART_MSR_CTS,
-		.signal_pin_change = UART_MSR_DCTS,
-		.on  = (UART_MCR_RTS | UART_MCR_OUT2 | UART_MCR_DTR),
-		.off = (UART_MCR_RTS | UART_MCR_OUT2),
-		.send_pulse = send_pulse_homebrew,
-		.send_space = send_space_homebrew,
-#ifdef CONFIG_LIRC_SERIAL_TRANSMITTER
-		.features    = (LIRC_CAN_SET_SEND_DUTY_CYCLE |
-				LIRC_CAN_SET_SEND_CARRIER |
-				LIRC_CAN_SEND_PULSE | LIRC_CAN_REC_MODE2)
-#else
-		.features    = LIRC_CAN_REC_MODE2
-#endif
-	},
-#endif
-
 };
 
 #define RS_ISR_PASS_LIMIT 256
@@ -315,16 +285,6 @@ static void soutp(int offset, u8 value)
 
 static void on(void)
 {
-#ifdef CONFIG_LIRC_SERIAL_NSLU2
-	/*
-	 * On NSLU2, we put the transmit diode between the output of the green
-	 * status LED and ground
-	 */
-	if (type == LIRC_NSLU2) {
-		gpio_set_value(NSLU2_LED_GRN, 0);
-		return;
-	}
-#endif
 	if (txsense)
 		soutp(UART_MCR, hardware[type].off);
 	else
@@ -333,12 +293,6 @@ static void on(void)
 
 static void off(void)
 {
-#ifdef CONFIG_LIRC_SERIAL_NSLU2
-	if (type == LIRC_NSLU2) {
-		gpio_set_value(NSLU2_LED_GRN, 1);
-		return;
-	}
-#endif
 	if (txsense)
 		soutp(UART_MCR, hardware[type].on);
 	else
@@ -793,20 +747,6 @@ static int hardware_init_port(void)
 	sinp(UART_IIR);
 	sinp(UART_MSR);
 
-#ifdef CONFIG_LIRC_SERIAL_NSLU2
-	if (type == LIRC_NSLU2) {
-		/* Setup NSLU2 UART */
-
-		/* Enable UART */
-		soutp(UART_IER, sinp(UART_IER) | UART_IE_IXP42X_UUE);
-		/* Disable Receiver data Time out interrupt */
-		soutp(UART_IER, sinp(UART_IER) & ~UART_IE_IXP42X_RTOIE);
-		/* set out2 = interrupt unmask; off() doesn't set MCR
-		   on NSLU2 */
-		soutp(UART_MCR, UART_MCR_RTS|UART_MCR_OUT2);
-	}
-#endif
-
 	/* Set line for power source */
 	off();
 
@@ -842,16 +782,6 @@ static int lirc_serial_probe(struct platform_device *dev)
 {
 	int i, nlow, nhigh, result;
 
-#ifdef CONFIG_LIRC_SERIAL_NSLU2
-	/* This GPIO is used for a LED on the NSLU2 */
-	result = devm_gpio_request(dev, NSLU2_LED_GRN, "lirc-serial");
-	if (result)
-		return result;
-	result = gpio_direction_output(NSLU2_LED_GRN, 0);
-	if (result)
-		return result;
-#endif
-
 	result = request_irq(irq, lirc_irq_handler,
 			     (share_irq ? IRQF_SHARED : 0),
 			     LIRC_DRIVER_NAME, (void *)&hardware);
@@ -1011,7 +941,8 @@ static ssize_t lirc_write(struct file *file, const char __user *buf,
 static long lirc_ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 {
 	int result;
-	__u32 value;
+	u32 __user *uptr = (u32 __user *)arg;
+	u32 value;
 
 	switch (cmd) {
 	case LIRC_GET_SEND_MODE:
@@ -1020,7 +951,7 @@ static long lirc_ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 
 		result = put_user(LIRC_SEND2MODE
 				  (hardware[type].features&LIRC_CAN_SEND_MASK),
-				  (__u32 *) arg);
+				  uptr);
 		if (result)
 			return result;
 		break;
@@ -1029,7 +960,7 @@ static long lirc_ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 		if (!(hardware[type].features&LIRC_CAN_SEND_MASK))
 			return -ENOIOCTLCMD;
 
-		result = get_user(value, (__u32 *) arg);
+		result = get_user(value, uptr);
 		if (result)
 			return result;
 		/* only LIRC_MODE_PULSE supported */
@@ -1046,7 +977,7 @@ static long lirc_ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 		if (!(hardware[type].features&LIRC_CAN_SET_SEND_DUTY_CYCLE))
 			return -ENOIOCTLCMD;
 
-		result = get_user(value, (__u32 *) arg);
+		result = get_user(value, uptr);
 		if (result)
 			return result;
 		if (value <= 0 || value > 100)
@@ -1059,7 +990,7 @@ static long lirc_ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 		if (!(hardware[type].features&LIRC_CAN_SET_SEND_CARRIER))
 			return -ENOIOCTLCMD;
 
-		result = get_user(value, (__u32 *) arg);
+		result = get_user(value, uptr);
 		if (result)
 			return result;
 		if (value > 500000 || value < 20000)
@@ -1216,14 +1147,6 @@ static int __init lirc_serial_init_module(void)
 		io = io ? io : 0x3f8;
 		irq = irq ? irq : 4;
 		break;
-#ifdef CONFIG_LIRC_SERIAL_NSLU2
-	case LIRC_NSLU2:
-		io = io ? io : IRQ_IXP4XX_UART2;
-		irq = irq ? irq : (IXP4XX_UART2_BASE_VIRT + REG_OFFSET);
-		iommap = iommap ? iommap : IXP4XX_UART2_BASE_PHYS;
-		ioshift = ioshift ? ioshift : 2;
-		break;
-#endif
 	default:
 		return -EINVAL;
 	}
@@ -1231,9 +1154,6 @@ static int __init lirc_serial_init_module(void)
 		switch (type) {
 		case LIRC_HOMEBREW:
 		case LIRC_IGOR:
-#ifdef CONFIG_LIRC_SERIAL_NSLU2
-		case LIRC_NSLU2:
-#endif
 			hardware[type].features &=
 				~(LIRC_CAN_SET_SEND_DUTY_CYCLE|
 				  LIRC_CAN_SET_SEND_CARRIER);
diff --git a/drivers/staging/media/lirc/lirc_sir.c b/drivers/staging/media/lirc/lirc_sir.c
index f781c532b3c..e31cbb81f05 100644
--- a/drivers/staging/media/lirc/lirc_sir.c
+++ b/drivers/staging/media/lirc/lirc_sir.c
@@ -187,10 +187,10 @@ static bool debug;
 
 /* Communication with user-space */
 static unsigned int lirc_poll(struct file *file, poll_table *wait);
-static ssize_t lirc_read(struct file *file, char *buf, size_t count,
-		loff_t *ppos);
-static ssize_t lirc_write(struct file *file, const char *buf, size_t n,
-		loff_t *pos);
+static ssize_t lirc_read(struct file *file, char __user *buf, size_t count,
+			 loff_t *ppos);
+static ssize_t lirc_write(struct file *file, const char __user *buf, size_t n,
+			  loff_t *pos);
 static long lirc_ioctl(struct file *filep, unsigned int cmd, unsigned long arg);
 static void add_read_queue(int flag, unsigned long val);
 static int init_chrdev(void);
@@ -252,8 +252,8 @@ static unsigned int lirc_poll(struct file *file, poll_table *wait)
 	return 0;
 }
 
-static ssize_t lirc_read(struct file *file, char *buf, size_t count,
-		loff_t *ppos)
+static ssize_t lirc_read(struct file *file, char __user *buf, size_t count,
+			 loff_t *ppos)
 {
 	int n = 0;
 	int retval = 0;
@@ -266,9 +266,9 @@ static ssize_t lirc_read(struct file *file, char *buf, size_t count,
 	set_current_state(TASK_INTERRUPTIBLE);
 	while (n < count) {
 		if (rx_head != rx_tail) {
-			if (copy_to_user((void *) buf + n,
-					(void *) (rx_buf + rx_head),
-					sizeof(int))) {
+			if (copy_to_user(buf + n,
+					 rx_buf + rx_head,
+					 sizeof(int))) {
 				retval = -EFAULT;
 				break;
 			}
@@ -291,8 +291,8 @@ static ssize_t lirc_read(struct file *file, char *buf, size_t count,
 	set_current_state(TASK_RUNNING);
 	return n ? n : retval;
 }
-static ssize_t lirc_write(struct file *file, const char *buf, size_t n,
-				loff_t *pos)
+static ssize_t lirc_write(struct file *file, const char __user *buf, size_t n,
+			  loff_t *pos)
 {
 	unsigned long flags;
 	int i, count;
@@ -338,8 +338,9 @@ static ssize_t lirc_write(struct file *file, const char *buf, size_t n,
 
 static long lirc_ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 {
+	u32 __user *uptr = (u32 __user *)arg;
 	int retval = 0;
-	__u32 value = 0;
+	u32 value = 0;
 #ifdef LIRC_ON_SA1100
 
 	if (cmd == LIRC_GET_FEATURES)
@@ -364,16 +365,16 @@ static long lirc_ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 	case LIRC_GET_FEATURES:
 	case LIRC_GET_SEND_MODE:
 	case LIRC_GET_REC_MODE:
-		retval = put_user(value, (__u32 *) arg);
+		retval = put_user(value, uptr);
 		break;
 
 	case LIRC_SET_SEND_MODE:
 	case LIRC_SET_REC_MODE:
-		retval = get_user(value, (__u32 *) arg);
+		retval = get_user(value, uptr);
 		break;
 #ifdef LIRC_ON_SA1100
 	case LIRC_SET_SEND_DUTY_CYCLE:
-		retval = get_user(value, (__u32 *) arg);
+		retval = get_user(value, uptr);
 		if (retval)
 			return retval;
 		if (value <= 0 || value > 100)
@@ -388,7 +389,7 @@ static long lirc_ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 			space_width -= LIRC_ON_SA1100_TRANSMITTER_LATENCY;
 		break;
 	case LIRC_SET_SEND_CARRIER:
-		retval = get_user(value, (__u32 *) arg);
+		retval = get_user(value, uptr);
 		if (retval)
 			return retval;
 		if (value > 500000 || value < 20000)
diff --git a/drivers/staging/media/lirc/lirc_zilog.c b/drivers/staging/media/lirc/lirc_zilog.c
index e1feb616459..3259aacfd35 100644
--- a/drivers/staging/media/lirc/lirc_zilog.c
+++ b/drivers/staging/media/lirc/lirc_zilog.c
@@ -892,7 +892,8 @@ out:
 }
 
 /* copied from lirc_dev */
-static ssize_t read(struct file *filep, char *outbuf, size_t n, loff_t *ppos)
+static ssize_t read(struct file *filep, char __user *outbuf, size_t n,
+		    loff_t *ppos)
 {
 	struct IR *ir = filep->private_data;
 	struct IR_rx *rx;
@@ -954,7 +955,7 @@ static ssize_t read(struct file *filep, char *outbuf, size_t n, loff_t *ppos)
 			}
 			m = lirc_buffer_read(rbuf, buf);
 			if (m == rbuf->chunk_size) {
-				ret = copy_to_user((void *)outbuf+written, buf,
+				ret = copy_to_user(outbuf + written, buf,
 						   rbuf->chunk_size);
 				written += rbuf->chunk_size;
 			} else {
@@ -1094,8 +1095,8 @@ static int send_code(struct IR_tx *tx, unsigned int code, unsigned int key)
  * sent to the device.  We have a spin lock as per i2c documentation to prevent
  * multiple concurrent sends which would probably cause the device to explode.
  */
-static ssize_t write(struct file *filep, const char *buf, size_t n,
-			  loff_t *ppos)
+static ssize_t write(struct file *filep, const char __user *buf, size_t n,
+		     loff_t *ppos)
 {
 	struct IR *ir = filep->private_data;
 	struct IR_tx *tx;
@@ -1237,6 +1238,7 @@ static unsigned int poll(struct file *filep, poll_table *wait)
 static long ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 {
 	struct IR *ir = filep->private_data;
+	unsigned long __user *uptr = (unsigned long __user *)arg;
 	int result;
 	unsigned long mode, features;
 
@@ -1244,11 +1246,10 @@ static long ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 
 	switch (cmd) {
 	case LIRC_GET_LENGTH:
-		result = put_user((unsigned long)13,
-				  (unsigned long *)arg);
+		result = put_user(13UL, uptr);
 		break;
 	case LIRC_GET_FEATURES:
-		result = put_user(features, (unsigned long *) arg);
+		result = put_user(features, uptr);
 		break;
 	case LIRC_GET_REC_MODE:
 		if (!(features&LIRC_CAN_REC_MASK))
@@ -1256,13 +1257,13 @@ static long ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 
 		result = put_user(LIRC_REC2MODE
 				  (features&LIRC_CAN_REC_MASK),
-				  (unsigned long *)arg);
+				  uptr);
 		break;
 	case LIRC_SET_REC_MODE:
 		if (!(features&LIRC_CAN_REC_MASK))
 			return -ENOSYS;
 
-		result = get_user(mode, (unsigned long *)arg);
+		result = get_user(mode, uptr);
 		if (!result && !(LIRC_MODE2REC(mode) & features))
 			result = -EINVAL;
 		break;
@@ -1270,13 +1271,13 @@ static long ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 		if (!(features&LIRC_CAN_SEND_MASK))
 			return -ENOSYS;
 
-		result = put_user(LIRC_MODE_PULSE, (unsigned long *) arg);
+		result = put_user(LIRC_MODE_PULSE, uptr);
 		break;
 	case LIRC_SET_SEND_MODE:
 		if (!(features&LIRC_CAN_SEND_MASK))
 			return -ENOSYS;
 
-		result = get_user(mode, (unsigned long *) arg);
+		result = get_user(mode, uptr);
 		if (!result && mode != LIRC_MODE_PULSE)
 			return -EINVAL;
 		break;
diff --git a/drivers/staging/media/sn9c102/sn9c102_devtable.h b/drivers/staging/media/sn9c102/sn9c102_devtable.h
index b3d2cc72965..4ba56925849 100644
--- a/drivers/staging/media/sn9c102/sn9c102_devtable.h
+++ b/drivers/staging/media/sn9c102/sn9c102_devtable.h
@@ -48,10 +48,8 @@ static const struct usb_device_id sn9c102_id_table[] = {
 	{ SN9C102_USB_DEVICE(0x0c45, 0x600d, BRIDGE_SN9C102), },
 /*	{ SN9C102_USB_DEVICE(0x0c45, 0x6011, BRIDGE_SN9C102), }, OV6650 */
 	{ SN9C102_USB_DEVICE(0x0c45, 0x6019, BRIDGE_SN9C102), },
-#endif
 	{ SN9C102_USB_DEVICE(0x0c45, 0x6024, BRIDGE_SN9C102), },
 	{ SN9C102_USB_DEVICE(0x0c45, 0x6025, BRIDGE_SN9C102), },
-#if !defined CONFIG_USB_GSPCA_SONIXB && !defined CONFIG_USB_GSPCA_SONIXB_MODULE
 	{ SN9C102_USB_DEVICE(0x0c45, 0x6028, BRIDGE_SN9C102), },
 	{ SN9C102_USB_DEVICE(0x0c45, 0x6029, BRIDGE_SN9C102), },
 	{ SN9C102_USB_DEVICE(0x0c45, 0x602a, BRIDGE_SN9C102), },
diff --git a/drivers/staging/mt29f_spinand/mt29f_spinand.c b/drivers/staging/mt29f_spinand/mt29f_spinand.c
index 51dbc13e757..3464e0c521e 100644
--- a/drivers/staging/mt29f_spinand/mt29f_spinand.c
+++ b/drivers/staging/mt29f_spinand/mt29f_spinand.c
@@ -699,6 +699,7 @@ static void spinand_write_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
 {
 
 	struct spinand_state *state = mtd_to_state(mtd);
+
 	memcpy(state->buf + state->buf_ptr, buf, len);
 	state->buf_ptr += len;
 }
@@ -706,6 +707,7 @@ static void spinand_write_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
 static void spinand_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 {
 	struct spinand_state *state = mtd_to_state(mtd);
+
 	memcpy(buf, state->buf + state->buf_ptr, len);
 	state->buf_ptr += len;
 }
@@ -924,6 +926,7 @@ static int spinand_remove(struct spi_device *spi)
 
 static const struct of_device_id spinand_dt[] = {
 	{ .compatible = "spinand,mt29f", },
+	{}
 };
 
 /*
diff --git a/drivers/staging/netlogic/xlr_net.c b/drivers/staging/netlogic/xlr_net.c
index c83e3375104..75d7c63cb41 100644
--- a/drivers/staging/netlogic/xlr_net.c
+++ b/drivers/staging/netlogic/xlr_net.c
@@ -268,6 +268,7 @@ static void xlr_make_tx_desc(struct nlm_fmn_msg *msg, unsigned long addr,
 	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 */
diff --git a/drivers/staging/nokia_h4p/nokia_fw-bcm.c b/drivers/staging/nokia_h4p/nokia_fw-bcm.c
index 111ae94032d..b55f5ba1134 100644
--- a/drivers/staging/nokia_h4p/nokia_fw-bcm.c
+++ b/drivers/staging/nokia_h4p/nokia_fw-bcm.c
@@ -73,7 +73,8 @@ void hci_h4p_bcm_parse_fw_event(struct hci_h4p_info *info, struct sk_buff *skb)
 		return;
 	}
 
-	if (fw_skb->data[1] == 0x01 && fw_skb->data[2] == 0xfc && fw_skb->len >= 10) {
+	if (fw_skb->data[1] == 0x01 && fw_skb->data[2] == 0xfc &&
+			fw_skb->len >= 10) {
 		BT_DBG("Setting bluetooth address");
 		err = hci_h4p_bcm_set_bdaddr(info, fw_skb);
 		if (err < 0) {
diff --git a/drivers/staging/octeon-usb/octeon-hcd.c b/drivers/staging/octeon-usb/octeon-hcd.c
index 8b8ce7293c5..c4c731f6052 100644
--- a/drivers/staging/octeon-usb/octeon-hcd.c
+++ b/drivers/staging/octeon-usb/octeon-hcd.c
@@ -47,6 +47,7 @@
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/pci.h>
+#include <linux/prefetch.h>
 #include <linux/interrupt.h>
 #include <linux/platform_device.h>
 #include <linux/usb.h>
@@ -245,9 +246,6 @@ enum cvmx_usb_pipe_flags {
 	__CVMX_USB_PIPE_FLAGS_NEED_PING	= 1 << 18,
 };
 
-/* Normal prefetch that use the pref instruction. */
-#define CVMX_PREFETCH(address, offset) asm volatile ("pref %[type], %[off](%[rbase])" : : [rbase] "d" (address), [off] "I" (offset), [type] "n" (0))
-
 /* Maximum number of times to retry failed transactions */
 #define MAX_RETRIES		3
 
@@ -462,7 +460,8 @@ struct octeon_hcd {
 	} while (0)
 
 /* Returns the IO address to push/pop stuff data from the FIFOs */
-#define USB_FIFO_ADDRESS(channel, usb_index) (CVMX_USBCX_GOTGCTL(usb_index) + ((channel)+1)*0x1000)
+#define USB_FIFO_ADDRESS(channel, usb_index) \
+	(CVMX_USBCX_GOTGCTL(usb_index) + ((channel)+1)*0x1000)
 
 /**
  * struct octeon_temp_buffer - a bounce buffer for USB transfers
@@ -892,6 +891,7 @@ static int cvmx_usb_initialize(struct cvmx_usb_state *usb,
 	 */
 	{
 		union cvmx_usbcx_gusbcfg usbcx_gusbcfg;
+
 		usbcx_gusbcfg.u32 = __cvmx_usb_read_csr32(usb,
 				CVMX_USBCX_GUSBCFG(usb->index));
 		usbcx_gusbcfg.s.toutcal = 0;
@@ -949,6 +949,7 @@ static int cvmx_usb_initialize(struct cvmx_usb_state *usb,
 		 */
 		{
 			union cvmx_usbcx_hcfg usbcx_hcfg;
+
 			usbcx_hcfg.u32 = __cvmx_usb_read_csr32(usb,
 					CVMX_USBCX_HCFG(usb->index));
 			usbcx_hcfg.s.fslssupp = 0;
@@ -1076,6 +1077,7 @@ static int cvmx_usb_enable(struct cvmx_usb_state *usb)
 	 */
 	{
 		union cvmx_usbcx_gnptxfsiz siz;
+
 		siz.u32 = __cvmx_usb_read_csr32(usb,
 				CVMX_USBCX_GNPTXFSIZ(usb->index));
 		siz.s.nptxfdep = usbcx_ghwcfg3.s.dfifodepth / 2;
@@ -1090,6 +1092,7 @@ static int cvmx_usb_enable(struct cvmx_usb_state *usb)
 	 */
 	{
 		union cvmx_usbcx_hptxfsiz siz;
+
 		siz.u32 = __cvmx_usb_read_csr32(usb,
 				CVMX_USBCX_HPTXFSIZ(usb->index));
 		siz.s.ptxfsize = usbcx_ghwcfg3.s.dfifodepth / 4;
@@ -1219,8 +1222,8 @@ static struct cvmx_usb_port_status cvmx_usb_get_status(
  * Returns: A non-NULL value is a pipe. NULL means an error.
  */
 static struct cvmx_usb_pipe *cvmx_usb_open_pipe(struct cvmx_usb_state *usb,
-						int device_addr, int
-						endpoint_num,
+						int device_addr,
+						int endpoint_num,
 						enum cvmx_usb_speed
 							device_speed,
 						int max_packet,
@@ -1286,7 +1289,8 @@ static struct cvmx_usb_pipe *cvmx_usb_open_pipe(struct cvmx_usb_state *usb,
 	if (__cvmx_usb_pipe_needs_split(usb, pipe)) {
 		pipe->interval = interval*8;
 		/* Force start splits to be schedule on uFrame 0 */
-		pipe->next_tx_frame = ((usb->frame_number+7)&~7) + pipe->interval;
+		pipe->next_tx_frame = ((usb->frame_number+7)&~7) +
+					pipe->interval;
 	} else {
 		pipe->interval = interval;
 		pipe->next_tx_frame = usb->frame_number + pipe->interval;
@@ -1337,9 +1341,13 @@ static void __cvmx_usb_poll_rx_fifo(struct cvmx_usb_state *usb)
 		return;
 
 	/* Get where the DMA engine would have written this data */
-	address = __cvmx_usb_read_csr64(usb, CVMX_USBNX_DMA0_INB_CHN0(usb->index) + channel*8);
+	address = __cvmx_usb_read_csr64(usb,
+			CVMX_USBNX_DMA0_INB_CHN0(usb->index) + channel*8);
+
 	ptr = cvmx_phys_to_ptr(address);
-	__cvmx_usb_write_csr64(usb, CVMX_USBNX_DMA0_INB_CHN0(usb->index) + channel*8, address + bytes);
+	__cvmx_usb_write_csr64(usb,
+			       CVMX_USBNX_DMA0_INB_CHN0(usb->index) + channel*8,
+			       address + bytes);
 
 	/* Loop writing the FIFO data for this packet into memory */
 	while (bytes > 0) {
@@ -1423,6 +1431,7 @@ static void __cvmx_usb_poll_tx_fifo(struct cvmx_usb_state *usb)
 {
 	if (usb->periodic.head != usb->periodic.tail) {
 		union cvmx_usbcx_hptxsts tx_status;
+
 		tx_status.u32 = __cvmx_usb_read_csr32(usb,
 				CVMX_USBCX_HPTXSTS(usb->index));
 		if (__cvmx_usb_fill_tx_hw(usb, &usb->periodic,
@@ -1438,6 +1447,7 @@ static void __cvmx_usb_poll_tx_fifo(struct cvmx_usb_state *usb)
 
 	if (usb->nonperiodic.head != usb->nonperiodic.tail) {
 		union cvmx_usbcx_gnptxsts tx_status;
+
 		tx_status.u32 = __cvmx_usb_read_csr32(usb,
 				CVMX_USBCX_GNPTXSTS(usb->index));
 		if (__cvmx_usb_fill_tx_hw(usb, &usb->nonperiodic,
@@ -1536,25 +1546,33 @@ static void __cvmx_usb_start_channel_control(struct cvmx_usb_state *usb,
 	switch (transaction->stage) {
 	case CVMX_USB_STAGE_NON_CONTROL:
 	case CVMX_USB_STAGE_NON_CONTROL_SPLIT_COMPLETE:
-		cvmx_dprintf("%s: ERROR - Non control stage\n", __FUNCTION__);
+		cvmx_dprintf("%s: ERROR - Non control stage\n", __func__);
 		break;
 	case CVMX_USB_STAGE_SETUP:
 		usbc_hctsiz.s.pid = 3; /* Setup */
 		bytes_to_transfer = sizeof(*header);
 		/* All Control operations start with a setup going OUT */
-		USB_SET_FIELD32(CVMX_USBCX_HCCHARX(channel, usb->index), union cvmx_usbcx_hccharx, epdir, CVMX_USB_DIRECTION_OUT);
+		USB_SET_FIELD32(CVMX_USBCX_HCCHARX(channel, usb->index),
+				union cvmx_usbcx_hccharx, epdir,
+				CVMX_USB_DIRECTION_OUT);
 		/*
 		 * Setup send the control header instead of the buffer data. The
 		 * buffer data will be used in the next stage
 		 */
-		__cvmx_usb_write_csr64(usb, CVMX_USBNX_DMA0_OUTB_CHN0(usb->index) + channel*8, transaction->control_header);
+		__cvmx_usb_write_csr64(usb,
+			CVMX_USBNX_DMA0_OUTB_CHN0(usb->index) + channel*8,
+			transaction->control_header);
 		break;
 	case CVMX_USB_STAGE_SETUP_SPLIT_COMPLETE:
 		usbc_hctsiz.s.pid = 3; /* Setup */
 		bytes_to_transfer = 0;
 		/* All Control operations start with a setup going OUT */
-		USB_SET_FIELD32(CVMX_USBCX_HCCHARX(channel, usb->index), union cvmx_usbcx_hccharx, epdir, CVMX_USB_DIRECTION_OUT);
-		USB_SET_FIELD32(CVMX_USBCX_HCSPLTX(channel, usb->index), union cvmx_usbcx_hcspltx, compsplt, 1);
+		USB_SET_FIELD32(CVMX_USBCX_HCCHARX(channel, usb->index),
+				union cvmx_usbcx_hccharx, epdir,
+				CVMX_USB_DIRECTION_OUT);
+
+		USB_SET_FIELD32(CVMX_USBCX_HCSPLTX(channel, usb->index),
+				union cvmx_usbcx_hcspltx, compsplt, 1);
 		break;
 	case CVMX_USB_STAGE_DATA:
 		usbc_hctsiz.s.pid = __cvmx_usb_get_data_pid(pipe);
@@ -1684,6 +1702,7 @@ static void __cvmx_usb_start_channel(struct cvmx_usb_state *usb,
 		/* Clear all channel status bits */
 		usbc_hcint.u32 = __cvmx_usb_read_csr32(usb,
 				CVMX_USBCX_HCINTX(channel, usb->index));
+
 		__cvmx_usb_write_csr32(usb,
 				       CVMX_USBCX_HCINTX(channel, usb->index),
 				       usbc_hcint.u32);
@@ -1714,18 +1733,31 @@ static void __cvmx_usb_start_channel(struct cvmx_usb_state *usb,
 		__cvmx_usb_write_csr32(usb, CVMX_USBCX_HCINTMSKX(channel, usb->index), usbc_hcintmsk.u32);
 
 		/* Enable the channel interrupt to propagate */
-		usbc_haintmsk.u32 = __cvmx_usb_read_csr32(usb, CVMX_USBCX_HAINTMSK(usb->index));
+		usbc_haintmsk.u32 = __cvmx_usb_read_csr32(usb,
+					CVMX_USBCX_HAINTMSK(usb->index));
 		usbc_haintmsk.s.haintmsk |= 1<<channel;
-		__cvmx_usb_write_csr32(usb, CVMX_USBCX_HAINTMSK(usb->index), usbc_haintmsk.u32);
+		__cvmx_usb_write_csr32(usb,
+					CVMX_USBCX_HAINTMSK(usb->index),
+					usbc_haintmsk.u32);
 	}
 
 	/* Setup the locations the DMA engines use  */
 	{
-		uint64_t dma_address = transaction->buffer + transaction->actual_bytes;
+		uint64_t dma_address = transaction->buffer +
+					transaction->actual_bytes;
+
 		if (transaction->type == CVMX_USB_TRANSFER_ISOCHRONOUS)
-			dma_address = transaction->buffer + transaction->iso_packets[0].offset + transaction->actual_bytes;
-		__cvmx_usb_write_csr64(usb, CVMX_USBNX_DMA0_OUTB_CHN0(usb->index) + channel*8, dma_address);
-		__cvmx_usb_write_csr64(usb, CVMX_USBNX_DMA0_INB_CHN0(usb->index) + channel*8, dma_address);
+			dma_address = transaction->buffer +
+					transaction->iso_packets[0].offset +
+					transaction->actual_bytes;
+
+		__cvmx_usb_write_csr64(usb,
+			CVMX_USBNX_DMA0_OUTB_CHN0(usb->index) + channel*8,
+			dma_address);
+
+		__cvmx_usb_write_csr64(usb,
+			CVMX_USBNX_DMA0_INB_CHN0(usb->index) + channel*8,
+			dma_address);
 	}
 
 	/* Setup both the size of the transfer and the SPLIT characteristics */
@@ -2008,8 +2040,7 @@ static struct cvmx_usb_pipe *__cvmx_usb_find_ready_pipe(
 			 ((((int)current_frame - (int)pipe->split_sc_frame)
 			   & 0x7f) < 0x40)) &&
 			(!usb->active_split || (usb->active_split == t))) {
-			CVMX_PREFETCH(pipe, 128);
-			CVMX_PREFETCH(t, 0);
+			prefetch(t);
 			return pipe;
 		}
 	}
@@ -2036,8 +2067,16 @@ static void __cvmx_usb_schedule(struct cvmx_usb_state *usb, int is_sof)
 		 * Without DMA we need to be careful to not schedule something
 		 * at the end of a frame and cause an overrun.
 		 */
-		union cvmx_usbcx_hfnum hfnum = {.u32 = __cvmx_usb_read_csr32(usb, CVMX_USBCX_HFNUM(usb->index))};
-		union cvmx_usbcx_hfir hfir = {.u32 = __cvmx_usb_read_csr32(usb, CVMX_USBCX_HFIR(usb->index))};
+		union cvmx_usbcx_hfnum hfnum = {
+			.u32 = __cvmx_usb_read_csr32(usb,
+						CVMX_USBCX_HFNUM(usb->index))
+		};
+
+		union cvmx_usbcx_hfir hfir = {
+			.u32 = __cvmx_usb_read_csr32(usb,
+						CVMX_USBCX_HFIR(usb->index))
+		};
+
 		if (hfnum.s.frrem < hfir.s.frint/4)
 			goto done;
 	}
@@ -2522,6 +2561,7 @@ static int cvmx_usb_cancel_all(struct cvmx_usb_state *usb,
 	/* Simply loop through and attempt to cancel each transaction */
 	list_for_each_entry_safe(transaction, next, &pipe->transactions, node) {
 		int result = cvmx_usb_cancel(usb, pipe, transaction);
+
 		if (unlikely(result != 0))
 			return result;
 	}
@@ -2654,19 +2694,20 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 	}
 
 	/* Disable the channel interrupts now that it is done */
-	__cvmx_usb_write_csr32(usb, CVMX_USBCX_HCINTMSKX(channel, usb->index), 0);
+	__cvmx_usb_write_csr32(usb,
+				CVMX_USBCX_HCINTMSKX(channel, usb->index),
+				0);
 	usb->idle_hardware_channels |= (1<<channel);
 
 	/* Make sure this channel is tied to a valid pipe */
 	pipe = usb->pipe_for_channel[channel];
-	CVMX_PREFETCH(pipe, 0);
-	CVMX_PREFETCH(pipe, 128);
+	prefetch(pipe);
 	if (!pipe)
 		return 0;
 	transaction = list_first_entry(&pipe->transactions,
 				       typeof(*transaction),
 				       node);
-	CVMX_PREFETCH(transaction, 0);
+	prefetch(transaction);
 
 	/*
 	 * Disconnect this pipe from the HW channel. Later the schedule
@@ -3021,7 +3062,8 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 					 */
 					if (!buffer_space_left ||
 						(bytes_this_transfer < 188)) {
-						pipe->next_tx_frame += pipe->interval;
+						pipe->next_tx_frame +=
+							pipe->interval;
 						__cvmx_usb_perform_complete(
 							usb,
 							pipe,
@@ -3083,6 +3125,7 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 				pipe->interval;
 	} else {
 		struct cvmx_usb_port_status port;
+
 		port = cvmx_usb_get_status(usb);
 		if (port.port_enabled) {
 			/* We'll retry the exact same transaction again */
@@ -3123,24 +3166,24 @@ static int cvmx_usb_poll(struct cvmx_usb_state *usb)
 	union cvmx_usbcx_hfnum usbc_hfnum;
 	union cvmx_usbcx_gintsts usbc_gintsts;
 
-	CVMX_PREFETCH(usb, 0);
-	CVMX_PREFETCH(usb, 1*128);
-	CVMX_PREFETCH(usb, 2*128);
-	CVMX_PREFETCH(usb, 3*128);
-	CVMX_PREFETCH(usb, 4*128);
+	prefetch_range(usb, sizeof(*usb));
 
 	/* Update the frame counter */
-	usbc_hfnum.u32 = __cvmx_usb_read_csr32(usb, CVMX_USBCX_HFNUM(usb->index));
+	usbc_hfnum.u32 = __cvmx_usb_read_csr32(usb,
+						CVMX_USBCX_HFNUM(usb->index));
 	if ((usb->frame_number&0x3fff) > usbc_hfnum.s.frnum)
 		usb->frame_number += 0x4000;
 	usb->frame_number &= ~0x3fffull;
 	usb->frame_number |= usbc_hfnum.s.frnum;
 
 	/* Read the pending interrupts */
-	usbc_gintsts.u32 = __cvmx_usb_read_csr32(usb, CVMX_USBCX_GINTSTS(usb->index));
+	usbc_gintsts.u32 = __cvmx_usb_read_csr32(usb,
+						CVMX_USBCX_GINTSTS(usb->index));
 
 	/* Clear the interrupts now that we know about them */
-	__cvmx_usb_write_csr32(usb, CVMX_USBCX_GINTSTS(usb->index), usbc_gintsts.u32);
+	__cvmx_usb_write_csr32(usb,
+				CVMX_USBCX_GINTSTS(usb->index),
+				usbc_gintsts.u32);
 
 	if (usbc_gintsts.s.rxflvl) {
 		/*
@@ -3196,7 +3239,9 @@ static int cvmx_usb_poll(struct cvmx_usb_state *usb)
 		 * to clear this bit.
 		 */
 		union cvmx_usbcx_haint usbc_haint;
-		usbc_haint.u32 = __cvmx_usb_read_csr32(usb, CVMX_USBCX_HAINT(usb->index));
+
+		usbc_haint.u32 = __cvmx_usb_read_csr32(usb,
+					CVMX_USBCX_HAINT(usb->index));
 		while (usbc_haint.u32) {
 			int channel;
 
@@ -3267,6 +3312,7 @@ static int octeon_usb_urb_enqueue(struct usb_hcd *hcd,
 		enum cvmx_usb_speed speed;
 		int split_device = 0;
 		int split_port = 0;
+
 		switch (usb_pipetype(urb->pipe)) {
 		case PIPE_ISOCHRONOUS:
 			transfer_type = CVMX_USB_TRANSFER_ISOCHRONOUS;
@@ -3303,6 +3349,7 @@ static int octeon_usb_urb_enqueue(struct usb_hcd *hcd,
 			 * tree.
 			 */
 			struct usb_device *dev = urb->dev;
+
 			while (dev->parent) {
 				/*
 				 * If our parent is high speed then he'll
@@ -3462,6 +3509,7 @@ static void octeon_usb_endpoint_disable(struct usb_hcd *hcd,
 		struct octeon_hcd *priv = hcd_to_octeon(hcd);
 		struct cvmx_usb_pipe *pipe = ep->hcpriv;
 		unsigned long flags;
+
 		spin_lock_irqsave(&priv->lock, flags);
 		cvmx_usb_cancel_all(&priv->usb, pipe);
 		if (cvmx_usb_close_pipe(&priv->usb, pipe))
@@ -3483,10 +3531,11 @@ static int octeon_usb_hub_status_data(struct usb_hcd *hcd, char *buf)
 	buf[0] = 0;
 	buf[0] = port_status.connect_change << 1;
 
-	return (buf[0] != 0);
+	return buf[0] != 0;
 }
 
-static int octeon_usb_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue, u16 wIndex, char *buf, u16 wLength)
+static int octeon_usb_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
+				u16 wIndex, char *buf, u16 wLength)
 {
 	struct octeon_hcd *priv = hcd_to_octeon(hcd);
 	struct device *dev = hcd->self.controller;
@@ -3778,6 +3827,7 @@ static int octeon_usb_probe(struct platform_device *pdev)
 	if (irq < 0) {
 		/* Defective device tree, but we know how to fix it. */
 		irq_hw_number_t hwirq = usb_num ? (1 << 6) + 17 : 56;
+
 		irq = irq_create_mapping(NULL, hwirq);
 	}
 
diff --git a/drivers/staging/olpc_dcon/olpc_dcon.c b/drivers/staging/olpc_dcon/olpc_dcon.c
index 26b4ec56fd3..eb83b28b8cd 100644
--- a/drivers/staging/olpc_dcon/olpc_dcon.c
+++ b/drivers/staging/olpc_dcon/olpc_dcon.c
@@ -210,6 +210,7 @@ static void dcon_sleep(struct dcon_priv *dcon, bool sleep)
 
 	if (sleep) {
 		u8 pm = 0;
+
 		x = olpc_ec_cmd(EC_DCON_POWER_MODE, &pm, 1, NULL, 0);
 		if (x)
 			pr_warn("unable to force dcon to power down: %d!\n", x);
@@ -240,6 +241,7 @@ static void dcon_sleep(struct dcon_priv *dcon, bool sleep)
 static void dcon_load_holdoff(struct dcon_priv *dcon)
 {
 	struct timespec delta_t, now;
+
 	while (1) {
 		getnstimeofday(&now);
 		delta_t = timespec_sub(now, dcon->load_time);
@@ -399,14 +401,15 @@ static ssize_t dcon_mode_show(struct device *dev,
 	struct device_attribute *attr, char *buf)
 {
 	struct dcon_priv *dcon = dev_get_drvdata(dev);
+
 	return sprintf(buf, "%4.4X\n", dcon->disp_mode);
 }
 
 static ssize_t dcon_sleep_show(struct device *dev,
 	struct device_attribute *attr, char *buf)
 {
-
 	struct dcon_priv *dcon = dev_get_drvdata(dev);
+
 	return sprintf(buf, "%d\n", dcon->asleep);
 }
 
@@ -414,6 +417,7 @@ static ssize_t dcon_freeze_show(struct device *dev,
 	struct device_attribute *attr, char *buf)
 {
 	struct dcon_priv *dcon = dev_get_drvdata(dev);
+
 	return sprintf(buf, "%d\n", dcon->curr_src == DCON_SOURCE_DCON ? 1 : 0);
 }
 
@@ -421,6 +425,7 @@ static ssize_t dcon_mono_show(struct device *dev,
 	struct device_attribute *attr, char *buf)
 {
 	struct dcon_priv *dcon = dev_get_drvdata(dev);
+
 	return sprintf(buf, "%d\n", dcon->mono);
 }
 
@@ -534,6 +539,7 @@ static int dcon_bl_update(struct backlight_device *dev)
 static int dcon_bl_get(struct backlight_device *dev)
 {
 	struct dcon_priv *dcon = bl_get_data(dev);
+
 	return dcon->bl_val;
 }
 
diff --git a/drivers/staging/ozwpan/TODO b/drivers/staging/ozwpan/TODO
index b4febd79a68..f32c1c0bc87 100644
--- a/drivers/staging/ozwpan/TODO
+++ b/drivers/staging/ozwpan/TODO
@@ -10,5 +10,5 @@ TODO:
 	- testing with as many devices as possible.
 
 Please send any patches for this driver to
-Rupesh Gujare <rupesh.gujare@atmel.com>
+Shigekatsu Tateno <shigekatsu.tateno@atmel.com>
 and Greg Kroah-Hartman <gregkh@linuxfoundation.org>.
diff --git a/drivers/staging/ozwpan/ozhcd.c b/drivers/staging/ozwpan/ozhcd.c
index b3d401ae41b..30bd9286d47 100644
--- a/drivers/staging/ozwpan/ozhcd.c
+++ b/drivers/staging/ozwpan/ozhcd.c
@@ -294,6 +294,7 @@ static void oz_free_urb_link(struct oz_urb_link *urbl)
 {
 	if (urbl) {
 		unsigned long irq_state;
+
 		spin_lock_irqsave(&g_link_lock, irq_state);
 		if (g_link_pool_size < OZ_MAX_LINK_POOL_SIZE) {
 			urbl->link.next = g_link_pool;
@@ -424,6 +425,7 @@ static void oz_ep_free(struct oz_port *port, struct oz_endpoint *ep)
 	if (port) {
 		struct list_head list;
 		struct oz_hcd *ozhcd = port->ozhcd;
+
 		INIT_LIST_HEAD(&list);
 		if (ep->flags & OZ_F_EP_HAVE_STREAM)
 			oz_usb_stream_delete(port->hpd, ep->ep_num);
@@ -568,6 +570,7 @@ static int oz_dequeue_ep_urb(struct oz_port *port, u8 ep_addr, int in_dir,
 		ep = port->out_ep[ep_addr];
 	if (ep) {
 		struct list_head *e;
+
 		list_for_each(e, &ep->urb_list) {
 			urbl = container_of(e, struct oz_urb_link, link);
 			if (urbl->urb == urb) {
@@ -599,6 +602,7 @@ static struct urb *oz_find_urb_by_id(struct oz_port *port, int ep_ix,
 	ep = port->out_ep[ep_ix];
 	if (ep) {
 		struct list_head *e;
+
 		list_for_each(e, &ep->urb_list) {
 			urbl = container_of(e, struct oz_urb_link, link);
 			if (urbl->req_id == req_id) {
@@ -815,6 +819,7 @@ void oz_hcd_get_desc_cnf(void *hport, u8 req_id, int status, const u8 *desc,
 	if (status == 0) {
 		int copy_len;
 		int required_size = urb->transfer_buffer_length;
+
 		if (required_size > total_size)
 			required_size = total_size;
 		copy_len = required_size-offset;
@@ -826,6 +831,7 @@ void oz_hcd_get_desc_cnf(void *hport, u8 req_id, int status, const u8 *desc,
 			struct usb_ctrlrequest *setup =
 				(struct usb_ctrlrequest *)urb->setup_packet;
 			unsigned wvalue = le16_to_cpu(setup->wValue);
+
 			if (oz_enqueue_ep_urb(port, 0, 0, urb, req_id))
 				err = -ENOMEM;
 			else if (oz_usb_get_desc_req(port->hpd, req_id,
@@ -919,6 +925,7 @@ static void oz_hcd_complete_set_interface(struct oz_port *port, struct urb *urb,
 	if ((rcode == 0) && (port->config_num > 0)) {
 		struct usb_host_config *config;
 		struct usb_host_interface *intf;
+
 		oz_dbg(ON, "Set interface %d alt %d\n", if_num, alt);
 		oz_clean_endpoints_for_interface(hcd, port, if_num);
 		config = &urb->dev->config[port->config_num-1];
@@ -974,6 +981,7 @@ void oz_hcd_control_cnf(void *hport, u8 req_id, u8 rcode, const u8 *data,
 
 	} else {
 		int copy_len;
+
 		oz_dbg(ON, "VENDOR-CLASS - cnf\n");
 		if (data_len) {
 			if (data_len <= urb->transfer_buffer_length)
@@ -1047,6 +1055,7 @@ void oz_hcd_data_ind(void *hport, u8 endpoint, const u8 *data, int data_len)
 					struct oz_urb_link, link);
 			struct urb *urb;
 			int copy_len;
+
 			list_del_init(&urbl->link);
 			spin_unlock_bh(&ozhcd->hcd_lock);
 			urb = urbl->urb;
@@ -1140,6 +1149,7 @@ int oz_hcd_heartbeat(void *hport)
 	spin_lock_bh(&ozhcd->hcd_lock);
 	list_for_each(e, &port->isoc_in_ep) {
 		struct oz_endpoint *ep = ep_from_link(e);
+
 		if (ep->flags & OZ_F_EP_BUFFERING) {
 			if (ep->buffered_units >= OZ_IN_BUFFERING_UNITS) {
 				ep->flags &= ~OZ_F_EP_BUFFERING;
@@ -1160,6 +1170,7 @@ int oz_hcd_heartbeat(void *hport)
 			int len = 0;
 			int copy_len;
 			int i;
+
 			if (ep->credit  < urb->number_of_packets)
 				break;
 			if (ep->buffered_units < urb->number_of_packets)
@@ -1215,6 +1226,7 @@ int oz_hcd_heartbeat(void *hport)
 	if (ep) {
 		struct list_head *e;
 		struct list_head *n;
+
 		spin_lock_bh(&ozhcd->hcd_lock);
 		list_for_each_safe(e, n, &ep->urb_list) {
 			urbl = container_of(e, struct oz_urb_link, link);
@@ -1557,6 +1569,7 @@ static void oz_process_ep0_urb(struct oz_hcd *ozhcd, struct urb *urb,
 	}
 	if (!rc && !complete) {
 		int data_len = 0;
+
 		if ((setup->bRequestType & USB_DIR_IN) == 0)
 			data_len = wlength;
 		urb->actual_length = data_len;
@@ -1761,6 +1774,7 @@ static void oz_hcd_clear_orphanage(struct oz_hcd *ozhcd, int status)
 {
 	if (ozhcd) {
 		struct oz_urb_link *urbl;
+
 		while (!list_empty(&ozhcd->orphanage)) {
 			urbl = list_first_entry(&ozhcd->orphanage,
 				struct oz_urb_link, link);
@@ -1997,7 +2011,6 @@ static void oz_get_hub_descriptor(struct usb_hcd *hcd,
 static int oz_set_port_feature(struct usb_hcd *hcd, u16 wvalue, u16 windex)
 {
 	struct oz_port *port;
-	int err = 0;
 	u8 port_id = (u8)windex;
 	struct oz_hcd *ozhcd = oz_hcd_private(hcd);
 	unsigned set_bits = 0;
@@ -2064,7 +2077,7 @@ static int oz_set_port_feature(struct usb_hcd *hcd, u16 wvalue, u16 windex)
 		spin_unlock_bh(&port->port_lock);
 	}
 	oz_dbg(HUB, "Port[%d] status = 0x%x\n", port_id, port->status);
-	return err;
+	return 0;
 }
 
 /*
@@ -2073,7 +2086,6 @@ static int oz_set_port_feature(struct usb_hcd *hcd, u16 wvalue, u16 windex)
 static int oz_clear_port_feature(struct usb_hcd *hcd, u16 wvalue, u16 windex)
 {
 	struct oz_port *port;
-	int err = 0;
 	u8 port_id = (u8)windex;
 	struct oz_hcd *ozhcd = oz_hcd_private(hcd);
 	unsigned clear_bits = 0;
@@ -2140,7 +2152,7 @@ static int oz_clear_port_feature(struct usb_hcd *hcd, u16 wvalue, u16 windex)
 	}
 	oz_dbg(HUB, "Port[%d] status = 0x%x\n",
 	       port_id, ozhcd->ports[port_id-1].status);
-	return err;
+	return 0;
 }
 
 /*
@@ -2258,6 +2270,7 @@ static int oz_plat_probe(struct platform_device *dev)
 	spin_lock_init(&ozhcd->hcd_lock);
 	for (i = 0; i < OZ_NB_PORTS; i++) {
 		struct oz_port *port = &ozhcd->ports[i];
+
 		port->ozhcd = ozhcd;
 		port->flags = 0;
 		port->status = 0;
diff --git a/drivers/staging/ozwpan/ozproto.c b/drivers/staging/ozwpan/ozproto.c
index f09acd0bb01..11020559919 100644
--- a/drivers/staging/ozwpan/ozproto.c
+++ b/drivers/staging/ozwpan/ozproto.c
@@ -765,7 +765,7 @@ static char *oz_get_next_device_name(char *s, char *dname, int max_size)
 int oz_protocol_init(char *devs)
 {
 	skb_queue_head_init(&g_rx_queue);
-	if (devs && (devs[0] == '*')) {
+	if (devs[0] == '*') {
 		oz_binding_add(NULL);
 	} else {
 		char d[32];
@@ -792,7 +792,7 @@ int oz_get_pd_list(struct oz_mac_addr *addr, int max_count)
 		if (count >= max_count)
 			break;
 		pd = container_of(e, struct oz_pd, link);
-		memcpy(&addr[count++], pd->mac_addr, ETH_ALEN);
+		ether_addr_copy((u8 *)&addr[count++], pd->mac_addr);
 	}
 	spin_unlock_bh(&g_polling_lock);
 	return count;
diff --git a/drivers/staging/panel/panel.c b/drivers/staging/panel/panel.c
index 08f9a489611..4e9229363c3 100644
--- a/drivers/staging/panel/panel.c
+++ b/drivers/staging/panel/panel.c
@@ -462,7 +462,7 @@ MODULE_PARM_DESC(lcd_type,
 static int lcd_proto = -1;
 module_param(lcd_proto, int, 0000);
 MODULE_PARM_DESC(lcd_proto,
-		"LCD communication: 0=parallel (//), 1=serial, 2=TI LCD Interface");
+		 "LCD communication: 0=parallel (//), 1=serial, 2=TI LCD Interface");
 
 static int lcd_charset = -1;
 module_param(lcd_charset, int, 0000);
@@ -664,8 +664,12 @@ static void pin_to_bits(int pin, unsigned char *d_val, unsigned char *c_val)
 {
 	int d_bit, c_bit, inv;
 
-	d_val[0] = c_val[0] = d_val[1] = c_val[1] = 0;
-	d_val[2] = c_val[2] = 0xFF;
+	d_val[0] = 0;
+	c_val[0] = 0;
+	d_val[1] = 0;
+	c_val[1] = 0;
+	d_val[2] = 0xFF;
+	c_val[2] = 0xFF;
 
 	if (pin == 0)
 		return;
@@ -674,7 +678,8 @@ static void pin_to_bits(int pin, unsigned char *d_val, unsigned char *c_val)
 	if (inv)
 		pin = -pin;
 
-	d_bit = c_bit = 0;
+	d_bit = 0;
+	c_bit = 0;
 
 	switch (pin) {
 	case PIN_STROBE:	/* strobe, inverted */
@@ -711,10 +716,9 @@ static void pin_to_bits(int pin, unsigned char *d_val, unsigned char *c_val)
 /* sleeps that many milliseconds with a reschedule */
 static void long_sleep(int ms)
 {
-
-	if (in_interrupt())
+	if (in_interrupt()) {
 		mdelay(ms);
-	else {
+	} else {
 		current->state = TASK_INTERRUPTIBLE;
 		schedule_timeout((ms * HZ + 999) / 1000);
 	}
@@ -867,7 +871,9 @@ static void lcd_print(char c)
 static void lcd_clear_fast_s(void)
 {
 	int pos;
-	lcd_addr_x = lcd_addr_y = 0;
+
+	lcd_addr_x = 0;
+	lcd_addr_y = 0;
 	lcd_gotoxy();
 
 	spin_lock_irq(&pprt_lock);
@@ -879,7 +885,8 @@ static void lcd_clear_fast_s(void)
 	}
 	spin_unlock_irq(&pprt_lock);
 
-	lcd_addr_x = lcd_addr_y = 0;
+	lcd_addr_x = 0;
+	lcd_addr_y = 0;
 	lcd_gotoxy();
 }
 
@@ -887,7 +894,9 @@ static void lcd_clear_fast_s(void)
 static void lcd_clear_fast_p8(void)
 {
 	int pos;
-	lcd_addr_x = lcd_addr_y = 0;
+
+	lcd_addr_x = 0;
+	lcd_addr_y = 0;
 	lcd_gotoxy();
 
 	spin_lock_irq(&pprt_lock);
@@ -914,7 +923,8 @@ static void lcd_clear_fast_p8(void)
 	}
 	spin_unlock_irq(&pprt_lock);
 
-	lcd_addr_x = lcd_addr_y = 0;
+	lcd_addr_x = 0;
+	lcd_addr_y = 0;
 	lcd_gotoxy();
 }
 
@@ -922,7 +932,9 @@ static void lcd_clear_fast_p8(void)
 static void lcd_clear_fast_tilcd(void)
 {
 	int pos;
-	lcd_addr_x = lcd_addr_y = 0;
+
+	lcd_addr_x = 0;
+	lcd_addr_y = 0;
 	lcd_gotoxy();
 
 	spin_lock_irq(&pprt_lock);
@@ -934,7 +946,8 @@ static void lcd_clear_fast_tilcd(void)
 
 	spin_unlock_irq(&pprt_lock);
 
-	lcd_addr_x = lcd_addr_y = 0;
+	lcd_addr_x = 0;
+	lcd_addr_y = 0;
 	lcd_gotoxy();
 }
 
@@ -942,14 +955,14 @@ static void lcd_clear_fast_tilcd(void)
 static void lcd_clear_display(void)
 {
 	lcd_write_cmd(0x01);	/* clear display */
-	lcd_addr_x = lcd_addr_y = 0;
+	lcd_addr_x = 0;
+	lcd_addr_y = 0;
 	/* we must wait a few milliseconds (15) */
 	long_sleep(15);
 }
 
 static void lcd_init_display(void)
 {
-
 	lcd_flags = ((lcd_height > 1) ? LCD_FLAG_N : 0)
 	    | LCD_FLAG_D | LCD_FLAG_C | LCD_FLAG_B;
 
@@ -1092,6 +1105,7 @@ static inline int handle_lcd_special_code(void)
 		break;
 	case 'k': {	/* kill end of line */
 		int x;
+
 		for (x = lcd_addr_x; x < lcd_bwidth; x++)
 			lcd_write_data(' ');
 
@@ -1137,13 +1151,13 @@ static inline int handle_lcd_special_code(void)
 		value = 0;
 		while (*esc && cgoffset < 8) {
 			shift ^= 4;
-			if (*esc >= '0' && *esc <= '9')
+			if (*esc >= '0' && *esc <= '9') {
 				value |= (*esc - '0') << shift;
-			else if (*esc >= 'A' && *esc <= 'Z')
+			} else if (*esc >= 'A' && *esc <= 'Z') {
 				value |= (*esc - 'A' + 10) << shift;
-			else if (*esc >= 'a' && *esc <= 'z')
+			} else if (*esc >= 'a' && *esc <= 'z') {
 				value |= (*esc - 'a' + 10) << shift;
-			else {
+			} else {
 				esc++;
 				continue;
 			}
@@ -1179,8 +1193,9 @@ static inline int handle_lcd_special_code(void)
 				esc++;
 				if (kstrtoul(esc, 10, &lcd_addr_y) < 0)
 					break;
-			} else
+			} else {
 				break;
+			}
 		}
 
 		lcd_gotoxy();
@@ -1217,111 +1232,114 @@ static inline int handle_lcd_special_code(void)
 	return processed;
 }
 
+static void lcd_write_char(char c)
+{
+	/* first, we'll test if we're in escape mode */
+	if ((c != '\n') && lcd_escape_len >= 0) {
+		/* yes, let's add this char to the buffer */
+		lcd_escape[lcd_escape_len++] = c;
+		lcd_escape[lcd_escape_len] = 0;
+	} else {
+		/* aborts any previous escape sequence */
+		lcd_escape_len = -1;
+
+		switch (c) {
+		case LCD_ESCAPE_CHAR:
+			/* start of an escape sequence */
+			lcd_escape_len = 0;
+			lcd_escape[lcd_escape_len] = 0;
+			break;
+		case '\b':
+			/* go back one char and clear it */
+			if (lcd_addr_x > 0) {
+				/* check if we're not at the
+				   end of the line */
+				if (lcd_addr_x < lcd_bwidth)
+					/* back one char */
+					lcd_write_cmd(0x10);
+				lcd_addr_x--;
+			}
+			/* replace with a space */
+			lcd_write_data(' ');
+			/* back one char again */
+			lcd_write_cmd(0x10);
+			break;
+		case '\014':
+			/* quickly clear the display */
+			lcd_clear_fast();
+			break;
+		case '\n':
+			/* flush the remainder of the current line and
+			   go to the beginning of the next line */
+			for (; lcd_addr_x < lcd_bwidth; lcd_addr_x++)
+				lcd_write_data(' ');
+			lcd_addr_x = 0;
+			lcd_addr_y = (lcd_addr_y + 1) % lcd_height;
+			lcd_gotoxy();
+			break;
+		case '\r':
+			/* go to the beginning of the same line */
+			lcd_addr_x = 0;
+			lcd_gotoxy();
+			break;
+		case '\t':
+			/* print a space instead of the tab */
+			lcd_print(' ');
+			break;
+		default:
+			/* simply print this char */
+			lcd_print(c);
+			break;
+		}
+	}
+
+	/* now we'll see if we're in an escape mode and if the current
+	   escape sequence can be understood. */
+	if (lcd_escape_len >= 2) {
+		int processed = 0;
+
+		if (!strcmp(lcd_escape, "[2J")) {
+			/* clear the display */
+			lcd_clear_fast();
+			processed = 1;
+		} else if (!strcmp(lcd_escape, "[H")) {
+			/* cursor to home */
+			lcd_addr_x = 0;
+			lcd_addr_y = 0;
+			lcd_gotoxy();
+			processed = 1;
+		}
+		/* codes starting with ^[[L */
+		else if ((lcd_escape_len >= 3) &&
+			 (lcd_escape[0] == '[') &&
+			 (lcd_escape[1] == 'L')) {
+			processed = handle_lcd_special_code();
+		}
+
+		/* LCD special escape codes */
+		/* flush the escape sequence if it's been processed
+		   or if it is getting too long. */
+		if (processed || (lcd_escape_len >= LCD_ESCAPE_LEN))
+			lcd_escape_len = -1;
+	} /* escape codes */
+}
+
 static ssize_t lcd_write(struct file *file,
-			 const char *buf, size_t count, loff_t *ppos)
+			 const char __user *buf, size_t count, loff_t *ppos)
 {
-	const char *tmp = buf;
+	const char __user *tmp = buf;
 	char c;
 
-	for (; count-- > 0; (ppos ? (*ppos)++ : 0), ++tmp) {
+	for (; count-- > 0; (*ppos)++, tmp++) {
 		if (!in_interrupt() && (((count + 1) & 0x1f) == 0))
 			/* let's be a little nice with other processes
 			   that need some CPU */
 			schedule();
 
-		if (ppos == NULL && file == NULL)
-			/* let's not use get_user() from the kernel ! */
-			c = *tmp;
-		else if (get_user(c, tmp))
+		if (get_user(c, tmp))
 			return -EFAULT;
 
-		/* first, we'll test if we're in escape mode */
-		if ((c != '\n') && lcd_escape_len >= 0) {
-			/* yes, let's add this char to the buffer */
-			lcd_escape[lcd_escape_len++] = c;
-			lcd_escape[lcd_escape_len] = 0;
-		} else {
-			/* aborts any previous escape sequence */
-			lcd_escape_len = -1;
-
-			switch (c) {
-			case LCD_ESCAPE_CHAR:
-				/* start of an escape sequence */
-				lcd_escape_len = 0;
-				lcd_escape[lcd_escape_len] = 0;
-				break;
-			case '\b':
-				/* go back one char and clear it */
-				if (lcd_addr_x > 0) {
-					/* check if we're not at the
-					   end of the line */
-					if (lcd_addr_x < lcd_bwidth)
-						/* back one char */
-						lcd_write_cmd(0x10);
-					lcd_addr_x--;
-				}
-				/* replace with a space */
-				lcd_write_data(' ');
-				/* back one char again */
-				lcd_write_cmd(0x10);
-				break;
-			case '\014':
-				/* quickly clear the display */
-				lcd_clear_fast();
-				break;
-			case '\n':
-				/* flush the remainder of the current line and
-				   go to the beginning of the next line */
-				for (; lcd_addr_x < lcd_bwidth; lcd_addr_x++)
-					lcd_write_data(' ');
-				lcd_addr_x = 0;
-				lcd_addr_y = (lcd_addr_y + 1) % lcd_height;
-				lcd_gotoxy();
-				break;
-			case '\r':
-				/* go to the beginning of the same line */
-				lcd_addr_x = 0;
-				lcd_gotoxy();
-				break;
-			case '\t':
-				/* print a space instead of the tab */
-				lcd_print(' ');
-				break;
-			default:
-				/* simply print this char */
-				lcd_print(c);
-				break;
-			}
-		}
-
-		/* now we'll see if we're in an escape mode and if the current
-		   escape sequence can be understood. */
-		if (lcd_escape_len >= 2) {
-			int processed = 0;
-
-			if (!strcmp(lcd_escape, "[2J")) {
-				/* clear the display */
-				lcd_clear_fast();
-				processed = 1;
-			} else if (!strcmp(lcd_escape, "[H")) {
-				/* cursor to home */
-				lcd_addr_x = lcd_addr_y = 0;
-				lcd_gotoxy();
-				processed = 1;
-			}
-			/* codes starting with ^[[L */
-			else if ((lcd_escape_len >= 3) &&
-				 (lcd_escape[0] == '[') &&
-				 (lcd_escape[1] == 'L')) {
-				processed = handle_lcd_special_code();
-			}
-
-			/* LCD special escape codes */
-			/* flush the escape sequence if it's been processed
-			   or if it is getting too long. */
-			if (processed || (lcd_escape_len >= LCD_ESCAPE_LEN))
-				lcd_escape_len = -1;
-		} /* escape codes */
+		lcd_write_char(c);
 	}
 
 	return tmp - buf;
@@ -1365,8 +1383,19 @@ static struct miscdevice lcd_dev = {
 /* public function usable from the kernel for any purpose */
 static void panel_lcd_print(const char *s)
 {
-	if (lcd_enabled && lcd_initialized)
-		lcd_write(NULL, s, strlen(s), NULL);
+	const char *tmp = s;
+	int count = strlen(s);
+
+	if (lcd_enabled && lcd_initialized) {
+		for (; count-- > 0; tmp++) {
+			if (!in_interrupt() && (((count + 1) & 0x1f) == 0))
+				/* let's be a little nice with other processes
+				   that need some CPU */
+				schedule();
+
+			lcd_write_char(*tmp);
+		}
+	}
 }
 
 /* initialize the LCD driver */
@@ -1560,7 +1589,8 @@ static void lcd_init(void)
 	panel_lcd_print("\x1b[Lc\x1b[Lb\x1b[L*Linux-" UTS_RELEASE "\nPanel-"
 			PANEL_VERSION);
 #endif
-	lcd_addr_x = lcd_addr_y = 0;
+	lcd_addr_x = 0;
+	lcd_addr_y = 0;
 	/* clear the display on the next device opening */
 	lcd_must_clear = 1;
 	lcd_gotoxy();
@@ -1571,11 +1601,10 @@ static void lcd_init(void)
  */
 
 static ssize_t keypad_read(struct file *file,
-			   char *buf, size_t count, loff_t *ppos)
+			   char __user *buf, size_t count, loff_t *ppos)
 {
-
 	unsigned i = *ppos;
-	char *tmp = buf;
+	char __user *tmp = buf;
 
 	if (keypad_buflen == 0) {
 		if (file->f_flags & O_NONBLOCK)
@@ -1598,7 +1627,6 @@ static ssize_t keypad_read(struct file *file,
 
 static int keypad_open(struct inode *inode, struct file *file)
 {
-
 	if (keypad_open_cnt)
 		return -EBUSY;	/* open only once at a time */
 
@@ -1728,8 +1756,8 @@ static inline int input_state_high(struct logical_input *input)
 	 * release function.
 	 * eg: 0 -(press A)-> A -(press B)-> AB : don't match A's release.
 	 */
-	if (((phys_prev & input->mask) == input->value)
-	    && ((phys_curr & input->mask) > input->value)) {
+	if (((phys_prev & input->mask) == input->value) &&
+	    ((phys_curr & input->mask) >  input->value)) {
 		input->state = INPUT_ST_LOW; /* invalidate */
 		return 1;
 	}
@@ -1747,16 +1775,20 @@ static inline int input_state_high(struct logical_input *input)
 
 			if (input->high_timer == 0) {
 				char *press_str = input->u.kbd.press_str;
+
 				if (press_str[0]) {
 					int s = sizeof(input->u.kbd.press_str);
+
 					keypad_send_key(press_str, s);
 				}
 			}
 
 			if (input->u.kbd.repeat_str[0]) {
 				char *repeat_str = input->u.kbd.repeat_str;
+
 				if (input->high_timer >= KEYPAD_REP_START) {
 					int s = sizeof(input->u.kbd.repeat_str);
+
 					input->high_timer -= KEYPAD_REP_DELAY;
 					keypad_send_key(repeat_str, s);
 				}
@@ -1780,8 +1812,8 @@ static inline void input_state_falling(struct logical_input *input)
 {
 #if 0
 	/* FIXME !!! same comment as in input_state_high */
-	if (((phys_prev & input->mask) == input->value)
-	    && ((phys_curr & input->mask) > input->value)) {
+	if (((phys_prev & input->mask) == input->value) &&
+	    ((phys_curr & input->mask) >  input->value)) {
 		input->state = INPUT_ST_LOW;	/* invalidate */
 		return;
 	}
@@ -1794,8 +1826,10 @@ static inline void input_state_falling(struct logical_input *input)
 
 			if (input->u.kbd.repeat_str[0]) {
 				char *repeat_str = input->u.kbd.repeat_str;
+
 				if (input->high_timer >= KEYPAD_REP_START) {
 					int s = sizeof(input->u.kbd.repeat_str);
+
 					input->high_timer -= KEYPAD_REP_DELAY;
 					keypad_send_key(repeat_str, s);
 				}
@@ -1811,12 +1845,15 @@ static inline void input_state_falling(struct logical_input *input)
 		/* call release event */
 		if (input->type == INPUT_TYPE_STD) {
 			void (*release_fct)(int) = input->u.std.release_fct;
+
 			if (release_fct != NULL)
 				release_fct(input->u.std.release_data);
 		} else if (input->type == INPUT_TYPE_KBD) {
 			char *release_str = input->u.kbd.release_str;
+
 			if (release_str[0]) {
 				int s = sizeof(input->u.kbd.release_str);
+
 				keypad_send_key(release_str, s);
 			}
 		}
@@ -1930,12 +1967,17 @@ static int input_name2mask(const char *name, pmask_t *mask, pmask_t *value,
 	char im, om;
 	pmask_t m, v;
 
-	om = im = m = v = 0ULL;
+	om = 0ULL;
+	im = 0ULL;
+	m = 0ULL;
+	v = 0ULL;
 	while (*name) {
 		int in, out, bit, neg;
-		for (in = 0; (in < sizeof(sigtab)) &&
-			     (sigtab[in] != *name); in++)
+
+		for (in = 0; (in < sizeof(sigtab)) && (sigtab[in] != *name);
+		     in++)
 			;
+
 		if (in >= sizeof(sigtab))
 			return 0;	/* input name not found */
 		neg = (in & 1);	/* odd (lower) names are negated */
@@ -1946,10 +1988,11 @@ static int input_name2mask(const char *name, pmask_t *mask, pmask_t *value,
 		if (isdigit(*name)) {
 			out = *name - '0';
 			om |= (1 << out);
-		} else if (*name == '-')
+		} else if (*name == '-') {
 			out = 8;
-		else
+		} else {
 			return 0;	/* unknown bit name */
+		}
 
 		bit = (out * 5) + in;
 
@@ -1977,7 +2020,7 @@ static struct logical_input *panel_bind_key(const char *name, const char *press,
 {
 	struct logical_input *key;
 
-	key = kzalloc(sizeof(struct logical_input), GFP_KERNEL);
+	key = kzalloc(sizeof(*key), GFP_KERNEL);
 	if (!key)
 		return NULL;
 
@@ -2015,7 +2058,7 @@ static struct logical_input *panel_bind_callback(char *name,
 {
 	struct logical_input *callback;
 
-	callback = kmalloc(sizeof(struct logical_input), GFP_KERNEL);
+	callback = kmalloc(sizeof(*callback), GFP_KERNEL);
 	if (!callback)
 		return NULL;
 
@@ -2040,6 +2083,7 @@ static struct logical_input *panel_bind_callback(char *name,
 static void keypad_init(void)
 {
 	int keynum;
+
 	init_waitqueue_head(&keypad_read_wait);
 	keypad_buflen = 0;	/* flushes any eventual noisy keystroke */
 
@@ -2298,7 +2342,7 @@ static void __exit panel_cleanup_module(void)
 	unregister_reboot_notifier(&panel_notifier);
 
 	if (scan_timer.function != NULL)
-		del_timer(&scan_timer);
+		del_timer_sync(&scan_timer);
 
 	if (pprt != NULL) {
 		if (keypad_enabled) {
diff --git a/drivers/staging/rtl8187se/Kconfig b/drivers/staging/rtl8187se/Kconfig
deleted file mode 100644
index ff8d41ebca3..00000000000
--- a/drivers/staging/rtl8187se/Kconfig
+++ /dev/null
@@ -1,10 +0,0 @@
-config R8187SE
-	tristate "RealTek RTL8187SE Wireless LAN NIC driver"
-	depends on PCI && WLAN
-	depends on m
-	select WIRELESS_EXT
-	select WEXT_PRIV
-	select EEPROM_93CX6
-	select CRYPTO
-	---help---
-	  If built as a module, it will be called r8187se.ko.
diff --git a/drivers/staging/rtl8187se/Makefile b/drivers/staging/rtl8187se/Makefile
deleted file mode 100644
index 91d1aa2830c..00000000000
--- a/drivers/staging/rtl8187se/Makefile
+++ /dev/null
@@ -1,38 +0,0 @@
-
-#ccflags-y += -DCONFIG_IEEE80211_NOWEP=y
-#ccflags-y += -std=gnu89
-#ccflags-y += -O2
-#CC            = gcc
-
-ccflags-y := -DSW_ANTE
-ccflags-y += -DTX_TRACK
-ccflags-y += -DHIGH_POWER
-ccflags-y += -DSW_DIG
-ccflags-y += -DRATE_ADAPT
-
-#enable it for legacy power save, disable it for leisure power save
-ccflags-y += -DENABLE_LPS
-
-
-#ccflags-y := -mhard-float -DCONFIG_FORCE_HARD_FLOAT=y
-
-r8187se-y :=			\
-		r8180_core.o		\
-		r8180_wx.o		\
-		r8180_rtl8225z2.o	\
-		r8185b_init.o		\
-		r8180_dm.o		\
-		ieee80211/dot11d.o			\
-		ieee80211/ieee80211_softmac.o		\
-		ieee80211/ieee80211_rx.o		\
-		ieee80211/ieee80211_tx.o		\
-		ieee80211/ieee80211_wx.o		\
-		ieee80211/ieee80211_module.o		\
-		ieee80211/ieee80211_softmac_wx.o	\
-		ieee80211/ieee80211_crypt.o		\
-		ieee80211/ieee80211_crypt_tkip.o	\
-		ieee80211/ieee80211_crypt_ccmp.o	\
-		ieee80211/ieee80211_crypt_wep.o
-
-obj-$(CONFIG_R8187SE)	+= r8187se.o
-
diff --git a/drivers/staging/rtl8187se/Module.symvers b/drivers/staging/rtl8187se/Module.symvers
deleted file mode 100644
index e69de29bb2d..00000000000
--- a/drivers/staging/rtl8187se/Module.symvers
+++ /dev/null
diff --git a/drivers/staging/rtl8187se/TODO b/drivers/staging/rtl8187se/TODO
deleted file mode 100644
index 704949a9da0..00000000000
--- a/drivers/staging/rtl8187se/TODO
+++ /dev/null
@@ -1,13 +0,0 @@
-TODO:
-- prepare private ieee80211 stack for merge with rtl8192su's version:
-  - add hwsec_active flag to struct ieee80211_device
-  - add bHwSec flag to cb_desc structure
-- switch to use shared "librtl" instead of private ieee80211 stack
-- switch to use LIB80211
-- switch to use MAC80211
-- use kernel coding style
-- checkpatch.pl fixes
-- sparse fixes
-- integrate with drivers/net/wireless/rtl818x
-
-Please send any patches to Greg Kroah-Hartman <greg@kroah.com>.
diff --git a/drivers/staging/rtl8187se/ieee80211/dot11d.c b/drivers/staging/rtl8187se/ieee80211/dot11d.c
deleted file mode 100644
index 4483c2c0307..00000000000
--- a/drivers/staging/rtl8187se/ieee80211/dot11d.c
+++ /dev/null
@@ -1,189 +0,0 @@
-#include "dot11d.h"
-
-void Dot11d_Init(struct ieee80211_device *ieee)
-{
-	PRT_DOT11D_INFO pDot11dInfo = GET_DOT11D_INFO(ieee);
-
-	pDot11dInfo->bEnabled = 0;
-
-	pDot11dInfo->State = DOT11D_STATE_NONE;
-	pDot11dInfo->CountryIeLen = 0;
-	memset(pDot11dInfo->channel_map, 0, MAX_CHANNEL_NUMBER+1);
-	memset(pDot11dInfo->MaxTxPwrDbmList, 0xFF, MAX_CHANNEL_NUMBER+1);
-	RESET_CIE_WATCHDOG(ieee);
-
-	netdev_info(ieee->dev, "Dot11d_Init()\n");
-}
-
-/* Reset to the state as we are just entering a regulatory domain. */
-void Dot11d_Reset(struct ieee80211_device *ieee)
-{
-	u32 i;
-	PRT_DOT11D_INFO pDot11dInfo = GET_DOT11D_INFO(ieee);
-
-	/* Clear old channel map */
-	memset(pDot11dInfo->channel_map, 0, MAX_CHANNEL_NUMBER+1);
-	memset(pDot11dInfo->MaxTxPwrDbmList, 0xFF, MAX_CHANNEL_NUMBER+1);
-	/* Set new channel map */
-	for (i = 1; i <= 11; i++)
-		(pDot11dInfo->channel_map)[i] = 1;
-
-	for (i = 12; i <= 14; i++)
-		(pDot11dInfo->channel_map)[i] = 2;
-
-	pDot11dInfo->State = DOT11D_STATE_NONE;
-	pDot11dInfo->CountryIeLen = 0;
-	RESET_CIE_WATCHDOG(ieee);
-}
-
-/*
- * Description:
- *	Update country IE from Beacon or Probe Response and configure PHY for
- *	operation in the regulatory domain.
- *
- * TODO:
- *	Configure Tx power.
- *
- * Assumption:
- *	1. IS_DOT11D_ENABLE() is TRUE.
- *	2. Input IE is an valid one.
- */
-void Dot11d_UpdateCountryIe(struct ieee80211_device *dev, u8 *pTaddr,
-			    u16 CoutryIeLen, u8 *pCoutryIe)
-{
-	PRT_DOT11D_INFO pDot11dInfo = GET_DOT11D_INFO(dev);
-	u8 i, j, NumTriples, MaxChnlNum;
-	u8 index, MaxTxPowerInDbm;
-	PCHNL_TXPOWER_TRIPLE pTriple;
-
-	if ((CoutryIeLen - 3)%3 != 0) {
-		netdev_info(dev->dev, "Dot11d_UpdateCountryIe(): Invalid country IE, skip it........1\n");
-		Dot11d_Reset(dev);
-		return;
-	}
-
-	memset(pDot11dInfo->channel_map, 0, MAX_CHANNEL_NUMBER+1);
-	memset(pDot11dInfo->MaxTxPwrDbmList, 0xFF, MAX_CHANNEL_NUMBER+1);
-	MaxChnlNum = 0;
-	NumTriples = (CoutryIeLen - 3) / 3; /* skip 3-byte country string. */
-	pTriple = (PCHNL_TXPOWER_TRIPLE)(pCoutryIe + 3);
-	for (i = 0; i < NumTriples; i++) {
-		if (MaxChnlNum >= pTriple->FirstChnl) {
-			/*
-			 * It is not in a monotonically increasing order,
-			 * so stop processing.
-			 */
-			netdev_info(dev->dev,
-				    "Dot11d_UpdateCountryIe(): Invalid country IE, skip it........1\n");
-			Dot11d_Reset(dev);
-			return;
-		}
-		if (MAX_CHANNEL_NUMBER <
-		    (pTriple->FirstChnl + pTriple->NumChnls)) {
-			/*
-			 * It is not a valid set of channel id,
-			 * so stop processing
-			 */
-			netdev_info(dev->dev,
-				    "Dot11d_UpdateCountryIe(): Invalid country IE, skip it........2\n");
-			Dot11d_Reset(dev);
-			return;
-		}
-
-		for (j = 0; j < pTriple->NumChnls; j++) {
-			index = pTriple->FirstChnl + j;
-			pDot11dInfo->channel_map[index] = 1;
-			MaxTxPowerInDbm = pTriple->MaxTxPowerInDbm;
-			pDot11dInfo->MaxTxPwrDbmList[index] = MaxTxPowerInDbm;
-			MaxChnlNum = pTriple->FirstChnl + j;
-		}
-
-		pTriple = (PCHNL_TXPOWER_TRIPLE)((u8 *)pTriple + 3);
-	}
-#if 1
-	netdev_info(dev->dev, "Channel List:");
-	for (i = 1; i <= MAX_CHANNEL_NUMBER; i++)
-		if (pDot11dInfo->channel_map[i] > 0)
-			netdev_info(dev->dev, " %d", i);
-	netdev_info(dev->dev, "\n");
-#endif
-
-	UPDATE_CIE_SRC(dev, pTaddr);
-
-	pDot11dInfo->CountryIeLen = CoutryIeLen;
-	memcpy(pDot11dInfo->CountryIeBuf, pCoutryIe, CoutryIeLen);
-	pDot11dInfo->State = DOT11D_STATE_LEARNED;
-}
-
-u8 DOT11D_GetMaxTxPwrInDbm(struct ieee80211_device *dev, u8 Channel)
-{
-	PRT_DOT11D_INFO pDot11dInfo = GET_DOT11D_INFO(dev);
-	u8 MaxTxPwrInDbm = 255;
-
-	if (MAX_CHANNEL_NUMBER < Channel) {
-		netdev_info(dev->dev, "DOT11D_GetMaxTxPwrInDbm(): Invalid Channel\n");
-		return MaxTxPwrInDbm;
-	}
-	if (pDot11dInfo->channel_map[Channel])
-		MaxTxPwrInDbm = pDot11dInfo->MaxTxPwrDbmList[Channel];
-
-	return MaxTxPwrInDbm;
-}
-
-
-void DOT11D_ScanComplete(struct ieee80211_device *dev)
-{
-	PRT_DOT11D_INFO pDot11dInfo = GET_DOT11D_INFO(dev);
-
-	switch (pDot11dInfo->State) {
-	case DOT11D_STATE_LEARNED:
-		pDot11dInfo->State = DOT11D_STATE_DONE;
-		break;
-
-	case DOT11D_STATE_DONE:
-		if (GET_CIE_WATCHDOG(dev) == 0) {
-			/* Reset country IE if previous one is gone. */
-			Dot11d_Reset(dev);
-		}
-		break;
-	case DOT11D_STATE_NONE:
-		break;
-	}
-}
-
-int IsLegalChannel(struct ieee80211_device *dev, u8 channel)
-{
-	PRT_DOT11D_INFO pDot11dInfo = GET_DOT11D_INFO(dev);
-
-	if (MAX_CHANNEL_NUMBER < channel) {
-		netdev_info(dev->dev, "IsLegalChannel(): Invalid Channel\n");
-		return 0;
-	}
-	if (pDot11dInfo->channel_map[channel] > 0)
-		return 1;
-	return 0;
-}
-
-int ToLegalChannel(struct ieee80211_device *dev, u8 channel)
-{
-	PRT_DOT11D_INFO pDot11dInfo = GET_DOT11D_INFO(dev);
-	u8 default_chn = 0;
-	u32 i = 0;
-
-	for (i = 1; i <= MAX_CHANNEL_NUMBER; i++) {
-		if (pDot11dInfo->channel_map[i] > 0) {
-			default_chn = i;
-			break;
-		}
-	}
-
-	if (MAX_CHANNEL_NUMBER < channel) {
-		netdev_info(dev->dev, "IsLegalChannel(): Invalid Channel\n");
-		return default_chn;
-	}
-
-	if (pDot11dInfo->channel_map[channel] > 0)
-		return channel;
-
-	return default_chn;
-}
diff --git a/drivers/staging/rtl8187se/ieee80211/dot11d.h b/drivers/staging/rtl8187se/ieee80211/dot11d.h
deleted file mode 100644
index f996691307b..00000000000
--- a/drivers/staging/rtl8187se/ieee80211/dot11d.h
+++ /dev/null
@@ -1,71 +0,0 @@
-#ifndef __INC_DOT11D_H
-#define __INC_DOT11D_H
-
-#include "ieee80211.h"
-
-/* #define ENABLE_DOT11D */
-
-/* #define DOT11D_MAX_CHNL_NUM 83 */
-
-typedef struct _CHNL_TXPOWER_TRIPLE {
-	u8 FirstChnl;
-	u8  NumChnls;
-	u8  MaxTxPowerInDbm;
-} CHNL_TXPOWER_TRIPLE, *PCHNL_TXPOWER_TRIPLE;
-
-typedef enum _DOT11D_STATE {
-	DOT11D_STATE_NONE = 0,
-	DOT11D_STATE_LEARNED,
-	DOT11D_STATE_DONE,
-} DOT11D_STATE;
-
-typedef struct _RT_DOT11D_INFO {
-	/* DECLARE_RT_OBJECT(RT_DOT12D_INFO); */
-
-	bool bEnabled; /* dot11MultiDomainCapabilityEnabled */
-
-	u16 CountryIeLen; /* > 0 if CountryIeBuf[] contains valid country information element. */
-	u8  CountryIeBuf[MAX_IE_LEN];
-	u8  CountryIeSrcAddr[6]; /* Source AP of the country IE. */
-	u8  CountryIeWatchdog;
-
-	u8  channel_map[MAX_CHANNEL_NUMBER+1];  /* !!!Value 0: Invalid, 1: Valid (active scan), 2: Valid (passive scan) */
-	/* u8  ChnlListLen; // #Bytes valid in ChnlList[]. */
-	/* u8  ChnlList[DOT11D_MAX_CHNL_NUM]; */
-	u8  MaxTxPwrDbmList[MAX_CHANNEL_NUMBER+1];
-
-	DOT11D_STATE State;
-} RT_DOT11D_INFO, *PRT_DOT11D_INFO;
-
-#define eqMacAddr(a, b) (((a)[0] == (b)[0] && (a)[1] == (b)[1] && (a)[2] == (b)[2] && (a)[3] == (b)[3] && (a)[4] == (b)[4] && (a)[5] == (b)[5]) ? 1:0)
-#define cpMacAddr(des, src) ((des)[0] = (src)[0], (des)[1] = (src)[1], (des)[2] = (src)[2], (des)[3] = (src)[3], (des)[4] = (src)[4], (des)[5] = (src)[5])
-#define GET_DOT11D_INFO(__pIeeeDev) ((PRT_DOT11D_INFO)((__pIeeeDev)->pDot11dInfo))
-
-#define IS_DOT11D_ENABLE(__pIeeeDev) GET_DOT11D_INFO(__pIeeeDev)->bEnabled
-#define IS_COUNTRY_IE_VALID(__pIeeeDev) (GET_DOT11D_INFO(__pIeeeDev)->CountryIeLen > 0)
-
-#define IS_EQUAL_CIE_SRC(__pIeeeDev, __pTa) eqMacAddr(GET_DOT11D_INFO(__pIeeeDev)->CountryIeSrcAddr, __pTa)
-#define UPDATE_CIE_SRC(__pIeeeDev, __pTa) cpMacAddr(GET_DOT11D_INFO(__pIeeeDev)->CountryIeSrcAddr, __pTa)
-
-#define IS_COUNTRY_IE_CHANGED(__pIeeeDev, __Ie) \
-	(((__Ie).Length == 0 || (__Ie).Length != GET_DOT11D_INFO(__pIeeeDev)->CountryIeLen) ? \
-	FALSE : \
-	(!memcmp(GET_DOT11D_INFO(__pIeeeDev)->CountryIeBuf, (__Ie).Octet, (__Ie).Length)))
-
-#define CIE_WATCHDOG_TH 1
-#define GET_CIE_WATCHDOG(__pIeeeDev) GET_DOT11D_INFO(__pIeeeDev)->CountryIeWatchdog
-#define RESET_CIE_WATCHDOG(__pIeeeDev) GET_CIE_WATCHDOG(__pIeeeDev) = 0
-#define UPDATE_CIE_WATCHDOG(__pIeeeDev) ++GET_CIE_WATCHDOG(__pIeeeDev)
-
-#define IS_DOT11D_STATE_DONE(__pIeeeDev) (GET_DOT11D_INFO(__pIeeeDev)->State == DOT11D_STATE_DONE)
-
-void Dot11d_Init(struct ieee80211_device *dev);
-void Dot11d_Reset(struct ieee80211_device *dev);
-void Dot11d_UpdateCountryIe(struct ieee80211_device *dev, u8 *pTaddr,
-			    u16 CoutryIeLen, u8 *pCoutryIe);
-u8 DOT11D_GetMaxTxPwrInDbm(struct ieee80211_device *dev, u8 Channel);
-void DOT11D_ScanComplete(struct ieee80211_device *dev);
-int IsLegalChannel(struct ieee80211_device *dev, u8 channel);
-int ToLegalChannel(struct ieee80211_device *dev, u8 channel);
-
-#endif /*  #ifndef __INC_DOT11D_H */
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211.h b/drivers/staging/rtl8187se/ieee80211/ieee80211.h
deleted file mode 100644
index d1763b7b8f2..00000000000
--- a/drivers/staging/rtl8187se/ieee80211/ieee80211.h
+++ /dev/null
@@ -1,1496 +0,0 @@
-/*
- * Merged with mainline ieee80211.h in Aug 2004.  Original ieee802_11
- * remains copyright by the original authors
- *
- * Portions of the merged code are based on Host AP (software wireless
- * LAN access point) driver for Intersil Prism2/2.5/3.
- *
- * Copyright (c) 2001-2002, SSH Communications Security Corp and Jouni Malinen
- * <jkmaline@cc.hut.fi>
- * Copyright (c) 2002-2003, Jouni Malinen <jkmaline@cc.hut.fi>
- *
- * Adaption to a generic IEEE 802.11 stack by James Ketrenos
- * <jketreno@linux.intel.com>
- * Copyright (c) 2004, Intel Corporation
- *
- * Modified for Realtek's wi-fi cards by Andrea Merello
- * <andrea.merello@gmail.com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation. See README and COPYING for
- * more details.
- */
-#ifndef IEEE80211_H
-#define IEEE80211_H
-#include <linux/if_ether.h> /* ETH_ALEN */
-#include <linux/kernel.h>   /* ARRAY_SIZE */
-#include <linux/jiffies.h>
-#include <linux/timer.h>
-#include <linux/sched.h>
-#include <linux/semaphore.h>
-#include <linux/wireless.h>
-#include <linux/ieee80211.h>
-#include <linux/interrupt.h>
-
-#define KEY_TYPE_NA		0x0
-#define KEY_TYPE_WEP40 		0x1
-#define KEY_TYPE_TKIP		0x2
-#define KEY_TYPE_CCMP		0x4
-#define KEY_TYPE_WEP104		0x5
-
-#define aSifsTime					10
-
-#define MGMT_QUEUE_NUM 5
-
-
-#define IEEE_CMD_SET_WPA_PARAM			1
-#define	IEEE_CMD_SET_WPA_IE			2
-#define IEEE_CMD_SET_ENCRYPTION			3
-#define IEEE_CMD_MLME				4
-
-#define IEEE_PARAM_WPA_ENABLED			1
-#define IEEE_PARAM_TKIP_COUNTERMEASURES		2
-#define IEEE_PARAM_DROP_UNENCRYPTED		3
-#define IEEE_PARAM_PRIVACY_INVOKED		4
-#define IEEE_PARAM_AUTH_ALGS			5
-#define IEEE_PARAM_IEEE_802_1X			6
-//It should consistent with the driver_XXX.c
-//   David, 2006.9.26
-#define IEEE_PARAM_WPAX_SELECT			7
-//Added for notify the encryption type selection
-//   David, 2006.9.26
-#define IEEE_PROTO_WPA				1
-#define IEEE_PROTO_RSN				2
-//Added for notify the encryption type selection
-//   David, 2006.9.26
-#define IEEE_WPAX_USEGROUP			0
-#define IEEE_WPAX_WEP40				1
-#define IEEE_WPAX_TKIP				2
-#define IEEE_WPAX_WRAP   			3
-#define IEEE_WPAX_CCMP				4
-#define IEEE_WPAX_WEP104			5
-
-#define IEEE_KEY_MGMT_IEEE8021X			1
-#define IEEE_KEY_MGMT_PSK			2
-
-
-
-#define IEEE_MLME_STA_DEAUTH			1
-#define IEEE_MLME_STA_DISASSOC			2
-
-
-#define IEEE_CRYPT_ERR_UNKNOWN_ALG		2
-#define IEEE_CRYPT_ERR_UNKNOWN_ADDR		3
-#define IEEE_CRYPT_ERR_CRYPT_INIT_FAILED		4
-#define IEEE_CRYPT_ERR_KEY_SET_FAILED		5
-#define IEEE_CRYPT_ERR_TX_KEY_SET_FAILED		6
-#define IEEE_CRYPT_ERR_CARD_CONF_FAILED		7
-
-
-#define	IEEE_CRYPT_ALG_NAME_LEN			16
-
-extern int ieee80211_crypto_tkip_init(void);
-extern void ieee80211_crypto_tkip_exit(void);
-
-//by amy for ps
-typedef struct ieee_param {
-	u32 cmd;
-	u8 sta_addr[ETH_ALEN];
-        union {
-		struct {
-			u8 name;
-			u32 value;
-		} wpa_param;
-		struct {
-			u32 len;
-			u8 reserved[32];
-			u8 data[0];
-		} wpa_ie;
-	        struct{
-			int command;
-    			int reason_code;
-		} mlme;
-		struct {
-			u8 alg[IEEE_CRYPT_ALG_NAME_LEN];
-			u8 set_tx;
-			u32 err;
-			u8 idx;
-			u8 seq[8]; /* sequence counter (set: RX, get: TX) */
-			u16 key_len;
-			u8 key[0];
-		} crypt;
-
-	} u;
-}ieee_param;
-
-
-#define MSECS(t) msecs_to_jiffies(t)
-#define msleep_interruptible_rtl  msleep_interruptible
-
-#define IEEE80211_DATA_LEN		2304
-/* Maximum size for the MA-UNITDATA primitive, 802.11 standard section
-   6.2.1.1.2.
-
-   The figure in section 7.1.2 suggests a body size of up to 2312
-   bytes is allowed, which is a bit confusing, I suspect this
-   represents the 2304 bytes of real data, plus a possible 8 bytes of
-   WEP IV and ICV. (this interpretation suggested by Ramiro Barreiro) */
-
-#define IEEE80211_3ADDR_LEN 24
-#define IEEE80211_4ADDR_LEN 30
-#define IEEE80211_FCS_LEN    4
-#define IEEE80211_HLEN			IEEE80211_4ADDR_LEN
-#define IEEE80211_FRAME_LEN		(IEEE80211_DATA_LEN + IEEE80211_HLEN)
-#define IEEE80211_MGMT_HDR_LEN 24
-#define IEEE80211_DATA_HDR3_LEN 24
-#define IEEE80211_DATA_HDR4_LEN 30
-
-#define MIN_FRAG_THRESHOLD     256U
-#define	MAX_FRAG_THRESHOLD     2346U
-
-/* Frame control field constants */
-#define IEEE80211_FCTL_DSTODS		0x0300 //added by david
-#define IEEE80211_FCTL_WEP		0x4000
-
-/* debug macros */
-
-#ifdef CONFIG_IEEE80211_DEBUG
-extern u32 ieee80211_debug_level;
-#define IEEE80211_DEBUG(level, fmt, args...) \
-do { if (ieee80211_debug_level & (level)) \
-  printk(KERN_DEBUG "ieee80211: %c %s " fmt, \
-         in_interrupt() ? 'I' : 'U', __func__ , ## args); } while (0)
-#else
-#define IEEE80211_DEBUG(level, fmt, args...) do {} while (0)
-#endif	/* CONFIG_IEEE80211_DEBUG */
-
-/*
- * To use the debug system;
- *
- * If you are defining a new debug classification, simply add it to the #define
- * list here in the form of:
- *
- * #define IEEE80211_DL_xxxx VALUE
- *
- * shifting value to the left one bit from the previous entry.  xxxx should be
- * the name of the classification (for example, WEP)
- *
- * You then need to either add a IEEE80211_xxxx_DEBUG() macro definition for your
- * classification, or use IEEE80211_DEBUG(IEEE80211_DL_xxxx, ...) whenever you want
- * to send output to that classification.
- *
- * To add your debug level to the list of levels seen when you perform
- *
- * % cat /proc/net/ipw/debug_level
- *
- * you simply need to add your entry to the ipw_debug_levels array.
- *
- * If you do not see debug_level in /proc/net/ipw then you do not have
- * CONFIG_IEEE80211_DEBUG defined in your kernel configuration
- *
- */
-
-#define IEEE80211_DL_INFO          (1<<0)
-#define IEEE80211_DL_WX            (1<<1)
-#define IEEE80211_DL_SCAN          (1<<2)
-#define IEEE80211_DL_STATE         (1<<3)
-#define IEEE80211_DL_MGMT          (1<<4)
-#define IEEE80211_DL_FRAG          (1<<5)
-#define IEEE80211_DL_EAP           (1<<6)
-#define IEEE80211_DL_DROP          (1<<7)
-
-#define IEEE80211_DL_TX            (1<<8)
-#define IEEE80211_DL_RX            (1<<9)
-
-#define IEEE80211_ERROR(f, a...) printk(KERN_ERR "ieee80211: " f, ## a)
-#define IEEE80211_WARNING(f, a...) printk(KERN_WARNING "ieee80211: " f, ## a)
-#define IEEE80211_DEBUG_INFO(f, a...)   IEEE80211_DEBUG(IEEE80211_DL_INFO, f, ## a)
-
-#define IEEE80211_DEBUG_WX(f, a...)     IEEE80211_DEBUG(IEEE80211_DL_WX, f, ## a)
-#define IEEE80211_DEBUG_SCAN(f, a...)   IEEE80211_DEBUG(IEEE80211_DL_SCAN, f, ## a)
-//#define IEEE_DEBUG_SCAN  IEEE80211_WARNING
-#define IEEE80211_DEBUG_STATE(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_STATE, f, ## a)
-#define IEEE80211_DEBUG_MGMT(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_MGMT, f, ## a)
-#define IEEE80211_DEBUG_FRAG(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_FRAG, f, ## a)
-#define IEEE80211_DEBUG_EAP(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_EAP, f, ## a)
-#define IEEE80211_DEBUG_DROP(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_DROP, f, ## a)
-#define IEEE80211_DEBUG_TX(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_TX, f, ## a)
-#define IEEE80211_DEBUG_RX(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_RX, f, ## a)
-#include <linux/netdevice.h>
-#include <linux/if_arp.h> /* ARPHRD_ETHER */
-
-#ifndef WIRELESS_SPY
-#define WIRELESS_SPY		// enable iwspy support
-#endif
-#include <net/iw_handler.h>	// new driver API
-
-#ifndef ETH_P_PAE
-#define ETH_P_PAE 0x888E /* Port Access Entity (IEEE 802.1X) */
-#endif /* ETH_P_PAE */
-
-#define ETH_P_PREAUTH 0x88C7 /* IEEE 802.11i pre-authentication */
-
-#ifndef ETH_P_80211_RAW
-#define ETH_P_80211_RAW (ETH_P_ECONET + 1)
-#endif
-
-/* IEEE 802.11 defines */
-
-#define P80211_OUI_LEN 3
-
-struct ieee80211_snap_hdr {
-
-        u8    dsap;   /* always 0xAA */
-        u8    ssap;   /* always 0xAA */
-        u8    ctrl;   /* always 0x03 */
-        u8    oui[P80211_OUI_LEN];    /* organizational universal id */
-
-} __attribute__ ((packed));
-
-#define SNAP_SIZE sizeof(struct ieee80211_snap_hdr)
-
-#define WLAN_FC_GET_TYPE(fc) ((fc) & IEEE80211_FCTL_FTYPE)
-#define WLAN_FC_GET_STYPE(fc) ((fc) & IEEE80211_FCTL_STYPE)
-
-#define WLAN_GET_SEQ_FRAG(seq) ((seq) & IEEE80211_SCTL_FRAG)
-#define WLAN_GET_SEQ_SEQ(seq)  ((seq) & IEEE80211_SCTL_SEQ)
-
-#define WLAN_CAPABILITY_BSS (1<<0)
-#define WLAN_CAPABILITY_SHORT_SLOT (1<<10)
-
-#define IEEE80211_STATMASK_SIGNAL (1<<0)
-#define IEEE80211_STATMASK_RSSI (1<<1)
-#define IEEE80211_STATMASK_NOISE (1<<2)
-#define IEEE80211_STATMASK_RATE (1<<3)
-#define IEEE80211_STATMASK_WEMASK 0x7
-
-
-#define IEEE80211_CCK_MODULATION    (1<<0)
-#define IEEE80211_OFDM_MODULATION   (1<<1)
-
-#define IEEE80211_24GHZ_BAND     (1<<0)
-#define IEEE80211_52GHZ_BAND     (1<<1)
-
-#define IEEE80211_CCK_RATE_LEN  		4
-#define IEEE80211_CCK_RATE_1MB		        0x02
-#define IEEE80211_CCK_RATE_2MB		        0x04
-#define IEEE80211_CCK_RATE_5MB		        0x0B
-#define IEEE80211_CCK_RATE_11MB		        0x16
-#define IEEE80211_OFDM_RATE_LEN 		8
-#define IEEE80211_OFDM_RATE_6MB		        0x0C
-#define IEEE80211_OFDM_RATE_9MB		        0x12
-#define IEEE80211_OFDM_RATE_12MB		0x18
-#define IEEE80211_OFDM_RATE_18MB		0x24
-#define IEEE80211_OFDM_RATE_24MB		0x30
-#define IEEE80211_OFDM_RATE_36MB		0x48
-#define IEEE80211_OFDM_RATE_48MB		0x60
-#define IEEE80211_OFDM_RATE_54MB		0x6C
-#define IEEE80211_BASIC_RATE_MASK		0x80
-
-#define IEEE80211_CCK_RATE_1MB_MASK		(1<<0)
-#define IEEE80211_CCK_RATE_2MB_MASK		(1<<1)
-#define IEEE80211_CCK_RATE_5MB_MASK		(1<<2)
-#define IEEE80211_CCK_RATE_11MB_MASK		(1<<3)
-#define IEEE80211_OFDM_RATE_6MB_MASK		(1<<4)
-#define IEEE80211_OFDM_RATE_9MB_MASK		(1<<5)
-#define IEEE80211_OFDM_RATE_12MB_MASK		(1<<6)
-#define IEEE80211_OFDM_RATE_18MB_MASK		(1<<7)
-#define IEEE80211_OFDM_RATE_24MB_MASK		(1<<8)
-#define IEEE80211_OFDM_RATE_36MB_MASK		(1<<9)
-#define IEEE80211_OFDM_RATE_48MB_MASK		(1<<10)
-#define IEEE80211_OFDM_RATE_54MB_MASK		(1<<11)
-
-#define IEEE80211_CCK_RATES_MASK	        0x0000000F
-#define IEEE80211_CCK_BASIC_RATES_MASK	(IEEE80211_CCK_RATE_1MB_MASK | \
-	IEEE80211_CCK_RATE_2MB_MASK)
-#define IEEE80211_CCK_DEFAULT_RATES_MASK	(IEEE80211_CCK_BASIC_RATES_MASK | \
-        IEEE80211_CCK_RATE_5MB_MASK | \
-        IEEE80211_CCK_RATE_11MB_MASK)
-
-#define IEEE80211_OFDM_RATES_MASK		0x00000FF0
-#define IEEE80211_OFDM_BASIC_RATES_MASK	(IEEE80211_OFDM_RATE_6MB_MASK | \
-	IEEE80211_OFDM_RATE_12MB_MASK | \
-	IEEE80211_OFDM_RATE_24MB_MASK)
-#define IEEE80211_OFDM_DEFAULT_RATES_MASK	(IEEE80211_OFDM_BASIC_RATES_MASK | \
-	IEEE80211_OFDM_RATE_9MB_MASK  | \
-	IEEE80211_OFDM_RATE_18MB_MASK | \
-	IEEE80211_OFDM_RATE_36MB_MASK | \
-	IEEE80211_OFDM_RATE_48MB_MASK | \
-	IEEE80211_OFDM_RATE_54MB_MASK)
-#define IEEE80211_DEFAULT_RATES_MASK (IEEE80211_OFDM_DEFAULT_RATES_MASK | \
-                                IEEE80211_CCK_DEFAULT_RATES_MASK)
-
-#define IEEE80211_NUM_OFDM_RATES	    8
-#define IEEE80211_NUM_CCK_RATES	            4
-#define IEEE80211_OFDM_SHIFT_MASK_A         4
-
-/* this is stolen and modified from the madwifi driver*/
-#define IEEE80211_FC0_TYPE_MASK		0x0c
-#define IEEE80211_FC0_TYPE_DATA		0x08
-#define IEEE80211_FC0_SUBTYPE_MASK	0xB0
-#define IEEE80211_FC0_SUBTYPE_QOS	0x80
-
-#define IEEE80211_QOS_HAS_SEQ(fc) \
-	(((fc) & (IEEE80211_FC0_TYPE_MASK | IEEE80211_FC0_SUBTYPE_MASK)) == \
-	 (IEEE80211_FC0_TYPE_DATA | IEEE80211_FC0_SUBTYPE_QOS))
-
-/* this is stolen from ipw2200 driver */
-#define IEEE_IBSS_MAC_HASH_SIZE 31
-struct ieee_ibss_seq {
-	u8 mac[ETH_ALEN];
-	u16 seq_num[17];
-	u16 frag_num[17];
-	unsigned long packet_time[17];
-	struct list_head list;
-};
-
-/* NOTE: This data is for statistical purposes; not all hardware provides this
- *       information for frames received.  Not setting these will not cause
- *       any adverse affects. */
-struct ieee80211_rx_stats {
-	u32 mac_time[2];
-	u8 signalstrength;
-	s8 rssi;
-	u8 signal;
-	u8 noise;
-	u16 rate; /* in 100 kbps */
-	u8 received_channel;
-	u8 control;
-	u8 mask;
-	u8 freq;
-	u16 len;
-	u8 nic_type;
-};
-
-/* IEEE 802.11 requires that STA supports concurrent reception of at least
- * three fragmented frames. This define can be increased to support more
- * concurrent frames, but it should be noted that each entry can consume about
- * 2 kB of RAM and increasing cache size will slow down frame reassembly. */
-#define IEEE80211_FRAG_CACHE_LEN 4
-
-struct ieee80211_frag_entry {
-	unsigned long first_frag_time;
-	unsigned int seq;
-	unsigned int last_frag;
-	struct sk_buff *skb;
-	u8 src_addr[ETH_ALEN];
-	u8 dst_addr[ETH_ALEN];
-};
-
-struct ieee80211_stats {
-	unsigned int tx_unicast_frames;
-	unsigned int tx_multicast_frames;
-	unsigned int tx_fragments;
-	unsigned int tx_unicast_octets;
-	unsigned int tx_multicast_octets;
-	unsigned int tx_deferred_transmissions;
-	unsigned int tx_single_retry_frames;
-	unsigned int tx_multiple_retry_frames;
-	unsigned int tx_retry_limit_exceeded;
-	unsigned int tx_discards;
-	unsigned int rx_unicast_frames;
-	unsigned int rx_multicast_frames;
-	unsigned int rx_fragments;
-	unsigned int rx_unicast_octets;
-	unsigned int rx_multicast_octets;
-	unsigned int rx_fcs_errors;
-	unsigned int rx_discards_no_buffer;
-	unsigned int tx_discards_wrong_sa;
-	unsigned int rx_discards_undecryptable;
-	unsigned int rx_message_in_msg_fragments;
-	unsigned int rx_message_in_bad_msg_fragments;
-};
-
-struct ieee80211_device;
-
-#include "ieee80211_crypt.h"
-
-#define SEC_KEY_1         (1<<0)
-#define SEC_KEY_2         (1<<1)
-#define SEC_KEY_3         (1<<2)
-#define SEC_KEY_4         (1<<3)
-#define SEC_ACTIVE_KEY    (1<<4)
-#define SEC_AUTH_MODE     (1<<5)
-#define SEC_UNICAST_GROUP (1<<6)
-#define SEC_LEVEL         (1<<7)
-#define SEC_ENABLED       (1<<8)
-
-#define SEC_LEVEL_0      0 /* None */
-#define SEC_LEVEL_1      1 /* WEP 40 and 104 bit */
-#define SEC_LEVEL_2      2 /* Level 1 + TKIP */
-#define SEC_LEVEL_2_CKIP 3 /* Level 1 + CKIP */
-#define SEC_LEVEL_3      4 /* Level 2 + CCMP */
-
-#define WEP_KEYS 4
-#define WEP_KEY_LEN 13
-
-#define WEP_KEY_LEN_MODIF 32
-
-struct ieee80211_security {
-	u16 active_key:2,
-            enabled:1,
-	    auth_mode:2,
-            auth_algo:4,
-            unicast_uses_group:1;
-	u8 key_sizes[WEP_KEYS];
-	u8 keys[WEP_KEYS][WEP_KEY_LEN_MODIF];
-	u8 level;
-	u16 flags;
-} __attribute__ ((packed));
-
-
-/*
-
- 802.11 data frame from AP
-
-      ,-------------------------------------------------------------------.
-Bytes |  2   |  2   |    6    |    6    |    6    |  2   | 0..2312 |   4  |
-      |------|------|---------|---------|---------|------|---------|------|
-Desc. | ctrl | dura |  DA/RA  |   TA    |    SA   | Sequ |  frame  |  fcs |
-      |      | tion | (BSSID) |         |         | ence |  data   |      |
-      `-------------------------------------------------------------------'
-
-Total: 28-2340 bytes
-
-*/
-
-/* Management Frame Information Element Types */
-enum {
-	MFIE_TYPE_SSID = 0,
-	MFIE_TYPE_RATES = 1,
-	MFIE_TYPE_FH_SET = 2,
-	MFIE_TYPE_DS_SET = 3,
-	MFIE_TYPE_CF_SET = 4,
-	MFIE_TYPE_TIM = 5,
-	MFIE_TYPE_IBSS_SET = 6,
-	MFIE_TYPE_COUNTRY = 7,
-	MFIE_TYPE_CHALLENGE = 16,
-	MFIE_TYPE_ERP = 42,
-	MFIE_TYPE_RSN = 48,
-	MFIE_TYPE_RATES_EX = 50,
-	MFIE_TYPE_GENERIC = 221,
-};
-
-struct ieee80211_header_data {
-	__le16 frame_ctl;
-	u16 duration_id;
-	u8 addr1[6];
-	u8 addr2[6];
-	u8 addr3[6];
-	u16 seq_ctrl;
-};
-
-struct ieee80211_hdr_4addr {
-	__le16 frame_ctl;
-	u16 duration_id;
-	u8 addr1[ETH_ALEN];
-	u8 addr2[ETH_ALEN];
-	u8 addr3[ETH_ALEN];
-	u16 seq_ctl;
-	u8 addr4[ETH_ALEN];
-} __attribute__ ((packed));
-
-struct ieee80211_hdr_3addrqos {
-	u16 frame_ctl;
-	u16 duration_id;
-	u8 addr1[ETH_ALEN];
-	u8 addr2[ETH_ALEN];
-	u8 addr3[ETH_ALEN];
-	u16 seq_ctl;
-	u16 qos_ctl;
-} __attribute__ ((packed));
-
-struct ieee80211_hdr_4addrqos {
-	u16 frame_ctl;
-	u16 duration_id;
-	u8 addr1[ETH_ALEN];
-	u8 addr2[ETH_ALEN];
-	u8 addr3[ETH_ALEN];
-	u16 seq_ctl;
-	u8 addr4[ETH_ALEN];
-	u16 qos_ctl;
-} __attribute__ ((packed));
-
-struct ieee80211_info_element_hdr {
-	u8 id;
-	u8 len;
-} __attribute__ ((packed));
-
-struct ieee80211_info_element {
-	u8 id;
-	u8 len;
-	u8 data[0];
-} __attribute__ ((packed));
-
-struct ieee80211_authentication {
-	struct ieee80211_header_data header;
-	u16 algorithm;
-	u16 transaction;
-	u16 status;
-	//struct ieee80211_info_element_hdr info_element;
-} __attribute__ ((packed));
-
-struct ieee80211_disassoc_frame {
-	struct ieee80211_hdr_3addr header;
-	u16    reasoncode;
-} __attribute__ ((packed));
-
-struct ieee80211_probe_request {
-	struct ieee80211_header_data header;
-	/* struct ieee80211_info_element info_element; */
-} __attribute__ ((packed));
-
-struct ieee80211_probe_response {
-	struct ieee80211_header_data header;
-	u32 time_stamp[2];
-	u16 beacon_interval;
-	u16 capability;
-	struct ieee80211_info_element info_element;
-} __attribute__ ((packed));
-
-struct ieee80211_assoc_request_frame {
-	struct ieee80211_hdr_3addr header;
-	u16 capability;
-	u16 listen_interval;
-	//u8 current_ap[ETH_ALEN];
-	struct ieee80211_info_element_hdr info_element;
-} __attribute__ ((packed));
-
-struct ieee80211_assoc_response_frame {
-	struct ieee80211_hdr_3addr header;
-	u16 capability;
-	u16 status;
-	u16 aid;
-	struct ieee80211_info_element info_element; /* supported rates */
-} __attribute__ ((packed));
-
-struct ieee80211_txb {
-	u8 nr_frags;
-	u8 encrypted;
-	u16 reserved;
-	u16 frag_size;
-	u16 payload_size;
-	struct sk_buff *fragments[0];
-};
-
-/* SWEEP TABLE ENTRIES NUMBER */
-#define MAX_SWEEP_TAB_ENTRIES			42
-#define MAX_SWEEP_TAB_ENTRIES_PER_PACKET	7
-
-/* MAX_RATES_LENGTH needs to be 12.  The spec says 8, and many APs
- * only use 8, and then use extended rates for the remaining supported
- * rates.  Other APs, however, stick all of their supported rates on the
- * main rates information element... */
-#define MAX_RATES_LENGTH			((u8)12)
-#define MAX_RATES_EX_LENGTH			((u8)16)
-
-#define MAX_NETWORK_COUNT			128
-
-#define MAX_CHANNEL_NUMBER			165
-
-#define IEEE80211_SOFTMAC_SCAN_TIME		100 /* (HZ / 2) */
-#define IEEE80211_SOFTMAC_ASSOC_RETRY_TIME	(HZ * 2)
-
-#define CRC_LENGTH	4U
-
-#define MAX_WPA_IE_LEN	64
-
-#define NETWORK_EMPTY_ESSID	(1 << 0)
-#define NETWORK_HAS_OFDM	(1 << 1)
-#define NETWORK_HAS_CCK		(1 << 2)
-
-struct ieee80211_wmm_ac_param {
-	u8 ac_aci_acm_aifsn;
-	u8 ac_ecwmin_ecwmax;
-	u16 ac_txop_limit;
-};
-
-struct ieee80211_wmm_ts_info {
-	u8 ac_dir_tid;
-	u8 ac_up_psb;
-	u8 reserved;
-} __attribute__ ((packed));
-
-struct ieee80211_wmm_tspec_elem {
-	struct ieee80211_wmm_ts_info ts_info;
-	u16 norm_msdu_size;
-	u16 max_msdu_size;
-	u32 min_serv_inter;
-	u32 max_serv_inter;
-	u32 inact_inter;
-	u32 suspen_inter;
-	u32 serv_start_time;
-	u32 min_data_rate;
-	u32 mean_data_rate;
-	u32 peak_data_rate;
-	u32 max_burst_size;
-	u32 delay_bound;
-	u32 min_phy_rate;
-	u16 surp_band_allow;
-	u16 medium_time;
-}__attribute__((packed));
-
-enum eap_type {
-	EAP_PACKET = 0,
-	EAPOL_START,
-	EAPOL_LOGOFF,
-	EAPOL_KEY,
-	EAPOL_ENCAP_ASF_ALERT
-};
-
-static const char *eap_types[] = {
-	[EAP_PACKET]		= "EAP-Packet",
-	[EAPOL_START]		= "EAPOL-Start",
-	[EAPOL_LOGOFF]		= "EAPOL-Logoff",
-	[EAPOL_KEY]		= "EAPOL-Key",
-	[EAPOL_ENCAP_ASF_ALERT]	= "EAPOL-Encap-ASF-Alert"
-};
-
-static inline const char *eap_get_type(int type)
-{
-	return (type >= ARRAY_SIZE(eap_types)) ? "Unknown" : eap_types[type];
-}
-
-struct eapol {
-	u8 snap[6];
-	u16 ethertype;
-	u8 version;
-	u8 type;
-	u16 length;
-} __attribute__ ((packed));
-
-struct ieee80211_softmac_stats {
-	unsigned int rx_ass_ok;
-	unsigned int rx_ass_err;
-	unsigned int rx_probe_rq;
-	unsigned int tx_probe_rs;
-	unsigned int tx_beacons;
-	unsigned int rx_auth_rq;
-	unsigned int rx_auth_rs_ok;
-	unsigned int rx_auth_rs_err;
-	unsigned int tx_auth_rq;
-	unsigned int no_auth_rs;
-	unsigned int no_ass_rs;
-	unsigned int tx_ass_rq;
-	unsigned int rx_ass_rq;
-	unsigned int tx_probe_rq;
-	unsigned int reassoc;
-	unsigned int swtxstop;
-	unsigned int swtxawake;
-};
-
-#define BEACON_PROBE_SSID_ID_POSITION 12
-
-/*
- * These are the data types that can make up management packets
- *
-	u16 auth_algorithm;
-	u16 auth_sequence;
-	u16 beacon_interval;
-	u16 capability;
-	u8 current_ap[ETH_ALEN];
-	u16 listen_interval;
-	struct {
-		u16 association_id:14, reserved:2;
-	} __attribute__ ((packed));
-	u32 time_stamp[2];
-	u16 reason;
-	u16 status;
-*/
-
-#define IEEE80211_DEFAULT_TX_ESSID "Penguin"
-#define IEEE80211_DEFAULT_BASIC_RATE 10
-
-enum {WMM_all_frame, WMM_two_frame, WMM_four_frame, WMM_six_frame};
-#define MAX_SP_Len  (WMM_all_frame << 4)
-#define IEEE80211_QOS_TID 0x0f
-#define QOS_CTL_NOTCONTAIN_ACK (0x01 << 5)
-
-#define MAX_IE_LEN						0xFF //+YJ,080625
-
-struct rtl8187se_channel_list {
-	u8	channel[MAX_CHANNEL_NUMBER + 1];
-	u8	len;
-};
-
-//by amy for ps
-#define IEEE80211_WATCH_DOG_TIME    2000
-//by amy for ps
-//by amy for antenna
-#define ANTENNA_DIVERSITY_TIMER_PERIOD		1000 // 1000 m
-//by amy for antenna
-
-#define IEEE80211_DTIM_MBCAST 4
-#define IEEE80211_DTIM_UCAST 2
-#define IEEE80211_DTIM_VALID 1
-#define IEEE80211_DTIM_INVALID 0
-
-#define IEEE80211_PS_DISABLED 0
-#define IEEE80211_PS_UNICAST IEEE80211_DTIM_UCAST
-#define IEEE80211_PS_MBCAST IEEE80211_DTIM_MBCAST
-#define IEEE80211_PS_ENABLE   IEEE80211_DTIM_VALID
-//added by David for QoS 2006/6/30
-//#define WMM_Hang_8187
-#ifdef WMM_Hang_8187
-#undef WMM_Hang_8187
-#endif
-
-#define WME_AC_BE   0x00
-#define WME_AC_BK   0x01
-#define WME_AC_VI   0x02
-#define WME_AC_VO   0x03
-#define WME_ACI_MASK 0x03
-#define WME_AIFSN_MASK 0x03
-#define WME_AC_PRAM_LEN 16
-
-//UP Mapping to AC, using in MgntQuery_SequenceNumber() and maybe for DSCP
-//#define UP2AC(up)	((up<3) ? ((up==0)?1:0) : (up>>1))
-#define UP2AC(up) (		   \
-	((up) < 1) ? WME_AC_BE : \
-	((up) < 3) ? WME_AC_BK : \
-	((up) < 4) ? WME_AC_BE : \
-	((up) < 6) ? WME_AC_VI : \
-	WME_AC_VO)
-//AC Mapping to UP, using in Tx part for selecting the corresponding TX queue
-#define AC2UP(_ac)	(       \
-	((_ac) == WME_AC_VO) ? 6 : \
-	((_ac) == WME_AC_VI) ? 5 : \
-	((_ac) == WME_AC_BK) ? 1 : \
-	0)
-
-#define	ETHER_ADDR_LEN		6	/* length of an Ethernet address */
-struct	ether_header {
-	u8 ether_dhost[ETHER_ADDR_LEN];
-	u8 ether_shost[ETHER_ADDR_LEN];
-	u16 ether_type;
-} __attribute__((packed));
-
-#ifndef ETHERTYPE_PAE
-#define	ETHERTYPE_PAE	0x888e		/* EAPOL PAE/802.1x */
-#endif
-#ifndef ETHERTYPE_IP
-#define	ETHERTYPE_IP	0x0800		/* IP protocol */
-#endif
-
-struct ieee80211_network {
-	/* These entries are used to identify a unique network */
-	u8 bssid[ETH_ALEN];
-	u8 channel;
-	/* Ensure null-terminated for any debug msgs */
-	u8 ssid[IW_ESSID_MAX_SIZE + 1];
-	u8 ssid_len;
-
-	/* These are network statistics */
-	struct ieee80211_rx_stats stats;
-	u16 capability;
-	u8 rates[MAX_RATES_LENGTH];
-	u8 rates_len;
-	u8 rates_ex[MAX_RATES_EX_LENGTH];
-	u8 rates_ex_len;
-	unsigned long last_scanned;
-	u8 mode;
-	u8 flags;
-	u32 last_associate;
-	u32 time_stamp[2];
-	u16 beacon_interval;
-	u16 listen_interval;
-	u16 atim_window;
-	u8 wpa_ie[MAX_WPA_IE_LEN];
-	size_t wpa_ie_len;
-	u8 rsn_ie[MAX_WPA_IE_LEN];
-	size_t rsn_ie_len;
-	u8 dtim_period;
-	u8 dtim_data;
-	u32 last_dtim_sta_time[2];
-	struct list_head list;
-	//appeded for QoS
-	u8 wmm_info;
-	struct ieee80211_wmm_ac_param wmm_param[4];
-	u8 QoS_Enable;
-	u8 SignalStrength;
-//by amy 080312
-	u8 HighestOperaRate;
-//by amy 080312
-	u8 Turbo_Enable;//enable turbo mode, added by thomas
-	u16 CountryIeLen;
-	u8 CountryIeBuf[MAX_IE_LEN];
-};
-
-enum ieee80211_state {
-
-	/* the card is not linked at all */
-	IEEE80211_NOLINK = 0,
-
-	/* IEEE80211_ASSOCIATING* are for BSS client mode
-	 * the driver shall not perform RX filtering unless
-	 * the state is LINKED.
-	 * The driver shall just check for the state LINKED and
-	 * defaults to NOLINK for ALL the other states (including
-	 * LINKED_SCANNING)
-	 */
-
-	/* the association procedure will start (wq scheduling)*/
-	IEEE80211_ASSOCIATING,
-	IEEE80211_ASSOCIATING_RETRY,
-
-	/* the association procedure is sending AUTH request*/
-	IEEE80211_ASSOCIATING_AUTHENTICATING,
-
-	/* the association procedure has successfully authenticated
-	 * and is sending association request
-	 */
-	IEEE80211_ASSOCIATING_AUTHENTICATED,
-
-	/* the link is ok. the card associated to a BSS or linked
-	 * to a ibss cell or acting as an AP and creating the bss
-	 */
-	IEEE80211_LINKED,
-
-	/* same as LINKED, but the driver shall apply RX filter
-	 * rules as we are in NO_LINK mode. As the card is still
-	 * logically linked, but it is doing a syncro site survey
-	 * then it will be back to LINKED state.
-	 */
-	IEEE80211_LINKED_SCANNING,
-
-};
-
-#define DEFAULT_MAX_SCAN_AGE (15 * HZ)
-#define DEFAULT_FTS 2346
-
-#define CFG_IEEE80211_RESERVE_FCS (1<<0)
-#define CFG_IEEE80211_COMPUTE_FCS (1<<1)
-
-typedef struct tx_pending_t{
-	int frag;
-	struct ieee80211_txb *txb;
-}tx_pending_t;
-
-enum {
-	COUNTRY_CODE_FCC = 0,
-	COUNTRY_CODE_IC = 1,
-	COUNTRY_CODE_ETSI = 2,
-	COUNTRY_CODE_SPAIN = 3,
-	COUNTRY_CODE_FRANCE = 4,
-	COUNTRY_CODE_MKK = 5,
-	COUNTRY_CODE_MKK1 = 6,
-	COUNTRY_CODE_ISRAEL = 7,
-	COUNTRY_CODE_TELEC = 8,
-	COUNTRY_CODE_GLOBAL_DOMAIN = 9,
-	COUNTRY_CODE_WORLD_WIDE_13_INDEX = 10
-};
-
-struct ieee80211_device {
-	struct net_device *dev;
-
-	/* Bookkeeping structures */
-	struct net_device_stats stats;
-	struct ieee80211_stats ieee_stats;
-	struct ieee80211_softmac_stats softmac_stats;
-
-	/* Probe / Beacon management */
-	struct list_head network_free_list;
-	struct list_head network_list;
-	struct ieee80211_network *networks;
-	int scans;
-	int scan_age;
-
-	int iw_mode; /* operating mode (IW_MODE_*) */
-
-	spinlock_t lock;
-	spinlock_t wpax_suitlist_lock;
-
-	int tx_headroom; /* Set to size of any additional room needed at front
-			  * of allocated Tx SKBs */
-	u32 config;
-
-	/* WEP and other encryption related settings at the device level */
-	int open_wep; /* Set to 1 to allow unencrypted frames */
-
-	int reset_on_keychange; /* Set to 1 if the HW needs to be reset on
-				 * WEP key changes */
-
-	/* If the host performs {en,de}cryption, then set to 1 */
-	int host_encrypt;
-	int host_decrypt;
-	int ieee802_1x; /* is IEEE 802.1X used */
-
-	/* WPA data */
-	int wpa_enabled;
-	int drop_unencrypted;
-	int tkip_countermeasures;
-	int privacy_invoked;
-	size_t wpa_ie_len;
-	u8 *wpa_ie;
-
-	u8 ap_mac_addr[6];
-	u16 pairwise_key_type;
-	u16 broadcast_key_type;
-
-	struct list_head crypt_deinit_list;
-	struct ieee80211_crypt_data *crypt[WEP_KEYS];
-	int tx_keyidx; /* default TX key index (crypt[tx_keyidx]) */
-	struct timer_list crypt_deinit_timer;
-
-	int bcrx_sta_key; /* use individual keys to override default keys even
-			   * with RX of broad/multicast frames */
-
-	/* Fragmentation structures */
-	/* each stream contains an entry */
-	struct ieee80211_frag_entry frag_cache[17][IEEE80211_FRAG_CACHE_LEN];
-	unsigned int frag_next_idx[17];
-	u16 fts; /* Fragmentation Threshold */
-
-	/* This stores infos for the current network.
-	 * Either the network we are associated in INFRASTRUCTURE
-	 * or the network that we are creating in MASTER mode.
-	 * ad-hoc is a mixture ;-).
-	 * Note that in infrastructure mode, even when not associated,
-	 * fields bssid and essid may be valid (if wpa_set and essid_set
-	 * are true) as thy carry the value set by the user via iwconfig
-	 */
-	struct ieee80211_network current_network;
-
-
-	enum ieee80211_state state;
-
-	int short_slot;
-	int mode;       /* A, B, G */
-	int modulation; /* CCK, OFDM */
-	int freq_band;  /* 2.4Ghz, 5.2Ghz, Mixed */
-	int abg_true;   /* ABG flag              */
-
-	/* used for forcing the ibss workqueue to terminate
-	 * without wait for the syncro scan to terminate
-	 */
-	short sync_scan_hurryup;
-
-	void * pDot11dInfo;
-	bool bGlobalDomain;
-
-	// For Liteon Ch12~13 passive scan
-	u8	MinPassiveChnlNum;
-	u8	IbssStartChnl;
-
-	int rate;       /* current rate */
-	int basic_rate;
-	//FIXME: please callback, see if redundant with softmac_features
-	short active_scan;
-
-	/* this contains flags for selectively enable softmac support */
-	u16 softmac_features;
-
-	/* if the sequence control field is not filled by HW */
-	u16 seq_ctrl[5];
-
-	/* association procedure transaction sequence number */
-	u16 associate_seq;
-
-	/* AID for RTXed association responses */
-	u16 assoc_id;
-
-	/* power save mode related*/
-	short ps;
-	short sta_sleep;
-	int ps_timeout;
-	struct tasklet_struct ps_task;
-	u32 ps_th;
-	u32 ps_tl;
-
-	short raw_tx;
-	/* used if IEEE_SOFTMAC_TX_QUEUE is set */
-	short queue_stop;
-	short scanning;
-	short proto_started;
-
-	struct semaphore wx_sem;
-	struct semaphore scan_sem;
-
-	spinlock_t mgmt_tx_lock;
-	spinlock_t beacon_lock;
-
-	short beacon_txing;
-
-	short wap_set;
-	short ssid_set;
-
-	u8  wpax_type_set;    //{added by David, 2006.9.28}
-	u32 wpax_type_notify; //{added by David, 2006.9.26}
-
-	/* QoS related flag */
-	char init_wmmparam_flag;
-
-	/* for discarding duplicated packets in IBSS */
-	struct list_head ibss_mac_hash[IEEE_IBSS_MAC_HASH_SIZE];
-
-	/* for discarding duplicated packets in BSS */
-	u16 last_rxseq_num[17]; /* rx seq previous per-tid */
-	u16 last_rxfrag_num[17];/* tx frag previous per-tid */
-	unsigned long last_packet_time[17];
-
-	/* for PS mode */
-	unsigned long last_rx_ps_time;
-
-	/* used if IEEE_SOFTMAC_SINGLE_QUEUE is set */
-	struct sk_buff *mgmt_queue_ring[MGMT_QUEUE_NUM];
-	int mgmt_queue_head;
-	int mgmt_queue_tail;
-
-
-	/* used if IEEE_SOFTMAC_TX_QUEUE is set */
-	struct  tx_pending_t tx_pending;
-
-	/* used if IEEE_SOFTMAC_ASSOCIATE is set */
-	struct timer_list associate_timer;
-
-	/* used if IEEE_SOFTMAC_BEACONS is set */
-	struct timer_list beacon_timer;
-
-	struct work_struct associate_complete_wq;
-//	struct work_struct associate_retry_wq;
-	struct work_struct associate_procedure_wq;
-//	struct work_struct softmac_scan_wq;
-	struct work_struct wx_sync_scan_wq;
-	struct work_struct wmm_param_update_wq;
-	struct work_struct ps_request_tx_ack_wq;//for ps
-//	struct work_struct hw_wakeup_wq;
-//	struct work_struct hw_sleep_wq;
-//	struct work_struct watch_dog_wq;
-	bool bInactivePs;
-	bool actscanning;
-	bool beinretry;
-	u16 ListenInterval;
-	unsigned long NumRxDataInPeriod; //YJ,add,080828
-	unsigned long NumRxBcnInPeriod;  //YJ,add,080828
-	unsigned long NumRxOkTotal;
-	unsigned long NumRxUnicast;//YJ,add,080828,for keep alive
-	bool bHwRadioOff;
-        struct delayed_work softmac_scan_wq;
-        struct delayed_work associate_retry_wq;
-	struct delayed_work hw_wakeup_wq;
-	struct delayed_work hw_sleep_wq;//+by amy 080324
-	struct delayed_work watch_dog_wq;
-	struct delayed_work sw_antenna_wq;
-	struct delayed_work  start_ibss_wq;
-//by amy for rate adaptive 080312
-    struct delayed_work rate_adapter_wq;
-//by amy for rate adaptive
-	struct delayed_work hw_dig_wq;
-	struct delayed_work tx_pw_wq;
-
-//Added for RF power on power off by lizhaoming 080512
-	struct delayed_work GPIOChangeRFWorkItem;
-
-	struct workqueue_struct *wq;
-
-	/* Callback functions */
-	void (*set_security)(struct net_device *dev,
-			     struct ieee80211_security *sec);
-
-	/* Used to TX data frame by using txb structs.
-	 * this is not used if in the softmac_features
-	 * is set the flag IEEE_SOFTMAC_TX_QUEUE
-	 */
-	int (*hard_start_xmit)(struct ieee80211_txb *txb,
-			       struct net_device *dev);
-
-	int (*reset_port)(struct net_device *dev);
-
-	/* Softmac-generated frames (management) are TXed via this
-	 * callback if the flag IEEE_SOFTMAC_SINGLE_QUEUE is
-	 * not set. As some cards may have different HW queues that
-	 * one might want to use for data and management frames
-	 * the option to have two callbacks might be useful.
-	 * This function can't sleep.
-	 */
-	int (*softmac_hard_start_xmit)(struct sk_buff *skb,
-			       struct net_device *dev);
-
-	/* used instead of hard_start_xmit (not softmac_hard_start_xmit)
-	 * if the IEEE_SOFTMAC_TX_QUEUE feature is used to TX data
-	 * frames. If the option IEEE_SOFTMAC_SINGLE_QUEUE is also set
-	 * then also management frames are sent via this callback.
-	 * This function can't sleep.
-	 */
-	void (*softmac_data_hard_start_xmit)(struct sk_buff *skb,
-			       struct net_device *dev,int rate);
-
-	/* stops the HW queue for DATA frames. Useful to avoid
-	 * waste time to TX data frame when we are reassociating
-	 * This function can sleep.
-	 */
-	void (*data_hard_stop)(struct net_device *dev);
-
-	/* OK this is complementar to data_poll_hard_stop */
-	void (*data_hard_resume)(struct net_device *dev);
-
-	/* ask to the driver to retune the radio .
-	 * This function can sleep. the driver should ensure
-	 * the radio has been switched before return.
-	 */
-	void (*set_chan)(struct net_device *dev,short ch);
-
-	/* These are not used if the ieee stack takes care of
-	 * scanning (IEEE_SOFTMAC_SCAN feature set).
-	 * In this case only the set_chan is used.
-	 *
-	 * The syncro version is similar to the start_scan but
-	 * does not return until all channels has been scanned.
-	 * this is called in user context and should sleep,
-	 * it is called in a work_queue when switching to ad-hoc mode
-	 * or in behalf of iwlist scan when the card is associated
-	 * and root user ask for a scan.
-	 * the function stop_scan should stop both the syncro and
-	 * background scanning and can sleep.
-	 * The function start_scan should initiate the background
-	 * scanning and can't sleep.
-	 */
-	void (*scan_syncro)(struct net_device *dev);
-	void (*start_scan)(struct net_device *dev);
-	void (*stop_scan)(struct net_device *dev);
-
-	/* indicate the driver that the link state is changed
-	 * for example it may indicate the card is associated now.
-	 * Driver might be interested in this to apply RX filter
-	 * rules or simply light the LINK led
-	 */
-	void (*link_change)(struct net_device *dev);
-
-	/* these two function indicates to the HW when to start
-	 * and stop to send beacons. This is used when the
-	 * IEEE_SOFTMAC_BEACONS is not set. For now the
-	 * stop_send_bacons is NOT guaranteed to be called only
-	 * after start_send_beacons.
-	 */
-	void (*start_send_beacons) (struct net_device *dev);
-	void (*stop_send_beacons) (struct net_device *dev);
-
-	/* power save mode related */
-	void (*sta_wake_up) (struct net_device *dev);
-	void (*ps_request_tx_ack) (struct net_device *dev);
-	void (*enter_sleep_state) (struct net_device *dev, u32 th, u32 tl);
-	short (*ps_is_queue_empty) (struct net_device *dev);
-
-	/* QoS related */
-	//void (*wmm_param_update) (struct net_device *dev, u8 *ac_param);
-	//void (*wmm_param_update) (struct ieee80211_device *ieee);
-
-	/* This must be the last item so that it points to the data
-	 * allocated beyond this structure by alloc_ieee80211 */
-	u8 priv[0];
-};
-
-#define IEEE_A            (1<<0)
-#define IEEE_B            (1<<1)
-#define IEEE_G            (1<<2)
-#define IEEE_MODE_MASK    (IEEE_A|IEEE_B|IEEE_G)
-
-/* Generate a 802.11 header */
-
-/* Uses the channel change callback directly
- * instead of [start/stop] scan callbacks
- */
-#define IEEE_SOFTMAC_SCAN (1<<2)
-
-/* Perform authentication and association handshake */
-#define IEEE_SOFTMAC_ASSOCIATE (1<<3)
-
-/* Generate probe requests */
-#define IEEE_SOFTMAC_PROBERQ (1<<4)
-
-/* Generate response to probe requests */
-#define IEEE_SOFTMAC_PROBERS (1<<5)
-
-/* The ieee802.11 stack will manages the netif queue
- * wake/stop for the driver, taking care of 802.11
- * fragmentation. See softmac.c for details. */
-#define IEEE_SOFTMAC_TX_QUEUE (1<<7)
-
-/* Uses only the softmac_data_hard_start_xmit
- * even for TX management frames.
- */
-#define IEEE_SOFTMAC_SINGLE_QUEUE (1<<8)
-
-/* Generate beacons.  The stack will enqueue beacons
- * to the card
- */
-#define IEEE_SOFTMAC_BEACONS (1<<6)
-
-
-
-static inline void *ieee80211_priv(struct net_device *dev)
-{
-	return ((struct ieee80211_device *)netdev_priv(dev))->priv;
-}
-
-static inline int ieee80211_is_empty_essid(const char *essid, int essid_len)
-{
-	/* Single white space is for Linksys APs */
-	if (essid_len == 1 && essid[0] == ' ')
-		return 1;
-
-	/* Otherwise, if the entire essid is 0, we assume it is hidden */
-	while (essid_len) {
-		essid_len--;
-		if (essid[essid_len] != '\0')
-			return 0;
-	}
-
-	return 1;
-}
-
-static inline int ieee80211_is_valid_mode(struct ieee80211_device *ieee,
-					  int mode)
-{
-	/*
-	 * It is possible for both access points and our device to support
-	 * combinations of modes, so as long as there is one valid combination
-	 * of ap/device supported modes, then return success
-	 *
-	 */
-	if ((mode & IEEE_A) &&
-	    (ieee->modulation & IEEE80211_OFDM_MODULATION) &&
-	    (ieee->freq_band & IEEE80211_52GHZ_BAND))
-		return 1;
-
-	if ((mode & IEEE_G) &&
-	    (ieee->modulation & IEEE80211_OFDM_MODULATION) &&
-	    (ieee->freq_band & IEEE80211_24GHZ_BAND))
-		return 1;
-
-	if ((mode & IEEE_B) &&
-	    (ieee->modulation & IEEE80211_CCK_MODULATION) &&
-	    (ieee->freq_band & IEEE80211_24GHZ_BAND))
-		return 1;
-
-	return 0;
-}
-
-static inline int ieee80211_get_hdrlen(u16 fc)
-{
-	int hdrlen = 24;
-
-	switch (WLAN_FC_GET_TYPE(fc)) {
-	case IEEE80211_FTYPE_DATA:
-		if ((fc & IEEE80211_FCTL_FROMDS) && (fc & IEEE80211_FCTL_TODS))
-			hdrlen = 30; /* Addr4 */
-		if(IEEE80211_QOS_HAS_SEQ(fc))
-			hdrlen += 2; /* QOS ctrl*/
-		break;
-	case IEEE80211_FTYPE_CTL:
-		switch (WLAN_FC_GET_STYPE(fc)) {
-		case IEEE80211_STYPE_CTS:
-		case IEEE80211_STYPE_ACK:
-			hdrlen = 10;
-			break;
-		default:
-			hdrlen = 16;
-			break;
-		}
-		break;
-	}
-
-	return hdrlen;
-}
-
-
-
-/* ieee80211.c */
-extern void free_ieee80211(struct net_device *dev);
-extern struct net_device *alloc_ieee80211(int sizeof_priv);
-
-extern int ieee80211_set_encryption(struct ieee80211_device *ieee);
-
-/* ieee80211_tx.c */
-
-extern int ieee80211_encrypt_fragment(struct ieee80211_device *ieee,
-				      struct sk_buff *frag, int hdr_len);
-
-extern int ieee80211_rtl_xmit(struct sk_buff *skb, struct net_device *dev);
-extern void ieee80211_txb_free(struct ieee80211_txb *);
-
-
-/* ieee80211_rx.c */
-extern int ieee80211_rtl_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
-			    struct ieee80211_rx_stats *rx_stats);
-extern void ieee80211_rx_mgt(struct ieee80211_device *ieee,
-			     struct ieee80211_hdr_4addr *header,
-			     struct ieee80211_rx_stats *stats);
-
-/* ieee80211_wx.c */
-extern int ieee80211_wx_get_scan(struct ieee80211_device *ieee,
-				 struct iw_request_info *info,
-				 union iwreq_data *wrqu, char *key);
-extern int ieee80211_wx_set_encode(struct ieee80211_device *ieee,
-				   struct iw_request_info *info,
-				   union iwreq_data *wrqu, char *key);
-extern int ieee80211_wx_get_encode(struct ieee80211_device *ieee,
-				   struct iw_request_info *info,
-				   union iwreq_data *wrqu, char *key);
-extern int ieee80211_wx_set_encode_ext(struct ieee80211_device *ieee,
-				       struct iw_request_info *info,
-				       union iwreq_data *wrqu, char *extra);
-int ieee80211_wx_set_auth(struct ieee80211_device *ieee,
-			  struct iw_request_info *info,
-			  struct iw_param *data, char *extra);
-int ieee80211_wx_set_mlme(struct ieee80211_device *ieee,
-			  struct iw_request_info *info,
-			  union iwreq_data *wrqu, char *extra);
-
-int ieee80211_wx_set_gen_ie(struct ieee80211_device *ieee, u8 *ie, size_t len);
-/* ieee80211_softmac.c */
-extern short ieee80211_is_54g(const struct ieee80211_network *net);
-extern short ieee80211_is_shortslot(const struct ieee80211_network *net);
-extern int ieee80211_rx_frame_softmac(struct ieee80211_device *ieee,
-				      struct sk_buff *skb,
-				      struct ieee80211_rx_stats *rx_stats,
-				      u16 type, u16 stype);
-extern void ieee80211_softmac_new_net(struct ieee80211_device *ieee,
-				      struct ieee80211_network *net);
-
-extern void ieee80211_softmac_xmit(struct ieee80211_txb *txb,
-				   struct ieee80211_device *ieee);
-extern void ieee80211_softmac_check_all_nets(struct ieee80211_device *ieee);
-extern void ieee80211_start_bss(struct ieee80211_device *ieee);
-extern void ieee80211_start_master_bss(struct ieee80211_device *ieee);
-extern void ieee80211_start_ibss(struct ieee80211_device *ieee);
-extern void ieee80211_softmac_init(struct ieee80211_device *ieee);
-extern void ieee80211_softmac_free(struct ieee80211_device *ieee);
-extern void ieee80211_associate_abort(struct ieee80211_device *ieee);
-extern void ieee80211_disassociate(struct ieee80211_device *ieee);
-extern void ieee80211_stop_scan(struct ieee80211_device *ieee);
-extern void ieee80211_start_scan_syncro(struct ieee80211_device *ieee);
-extern void ieee80211_check_all_nets(struct ieee80211_device *ieee);
-extern void ieee80211_start_protocol(struct ieee80211_device *ieee);
-extern void ieee80211_stop_protocol(struct ieee80211_device *ieee);
-extern void ieee80211_softmac_start_protocol(struct ieee80211_device *ieee);
-extern void ieee80211_softmac_stop_protocol(struct ieee80211_device *ieee);
-extern void ieee80211_reset_queue(struct ieee80211_device *ieee);
-extern void ieee80211_rtl_wake_queue(struct ieee80211_device *ieee);
-extern void ieee80211_rtl_stop_queue(struct ieee80211_device *ieee);
-extern struct sk_buff *ieee80211_get_beacon(struct ieee80211_device *ieee);
-extern void ieee80211_start_send_beacons(struct ieee80211_device *ieee);
-extern void ieee80211_stop_send_beacons(struct ieee80211_device *ieee);
-extern int ieee80211_wpa_supplicant_ioctl(struct ieee80211_device *ieee,
-					  struct iw_point *p);
-extern void notify_wx_assoc_event(struct ieee80211_device *ieee);
-extern void ieee80211_ps_tx_ack(struct ieee80211_device *ieee, short success);
-extern void SendDisassociation(struct ieee80211_device *ieee, u8 *asSta,
-			       u8 asRsn);
-extern void ieee80211_rtl_start_scan(struct ieee80211_device *ieee);
-
-//Add for RF power on power off by lizhaoming 080512
-extern void SendDisassociation(struct ieee80211_device *ieee, u8 *asSta,
-			       u8 asRsn);
-
-/* ieee80211_crypt_ccmp&tkip&wep.c */
-extern void ieee80211_tkip_null(void);
-extern void ieee80211_wep_null(void);
-extern void ieee80211_ccmp_null(void);
-/* ieee80211_softmac_wx.c */
-
-extern int ieee80211_wx_get_wap(struct ieee80211_device *ieee,
-				struct iw_request_info *info,
-				union iwreq_data *wrqu, char *ext);
-
-extern int ieee80211_wx_set_wap(struct ieee80211_device *ieee,
-				struct iw_request_info *info,
-				union iwreq_data *awrq,
-				char *extra);
-
-extern int ieee80211_wx_get_essid(struct ieee80211_device *ieee,
-				  struct iw_request_info *a,
-				  union iwreq_data *wrqu, char *b);
-
-extern int ieee80211_wx_set_rate(struct ieee80211_device *ieee,
-				 struct iw_request_info *info,
-				 union iwreq_data *wrqu, char *extra);
-
-extern int ieee80211_wx_get_rate(struct ieee80211_device *ieee,
-				 struct iw_request_info *info,
-				 union iwreq_data *wrqu, char *extra);
-
-extern int ieee80211_wx_set_mode(struct ieee80211_device *ieee,
-				 struct iw_request_info *a,
-				 union iwreq_data *wrqu, char *b);
-
-extern int ieee80211_wx_set_scan(struct ieee80211_device *ieee,
-				 struct iw_request_info *a,
-				 union iwreq_data *wrqu, char *b);
-
-extern int ieee80211_wx_set_essid(struct ieee80211_device *ieee,
-				  struct iw_request_info *a,
-				  union iwreq_data *wrqu, char *extra);
-
-extern int ieee80211_wx_get_mode(struct ieee80211_device *ieee,
-				 struct iw_request_info *a,
-				 union iwreq_data *wrqu, char *b);
-
-extern int ieee80211_wx_set_freq(struct ieee80211_device *ieee,
-				 struct iw_request_info *a,
-				 union iwreq_data *wrqu, char *b);
-
-extern int ieee80211_wx_get_freq(struct ieee80211_device *ieee,
-				 struct iw_request_info *a,
-				 union iwreq_data *wrqu, char *b);
-
-extern void ieee80211_wx_sync_scan_wq(struct work_struct *work);
-
-extern int ieee80211_wx_set_rawtx(struct ieee80211_device *ieee,
-				  struct iw_request_info *info,
-				  union iwreq_data *wrqu, char *extra);
-
-extern int ieee80211_wx_get_name(struct ieee80211_device *ieee,
-				 struct iw_request_info *info,
-				 union iwreq_data *wrqu, char *extra);
-
-extern int ieee80211_wx_set_power(struct ieee80211_device *ieee,
-				  struct iw_request_info *info,
-				  union iwreq_data *wrqu, char *extra);
-
-extern int ieee80211_wx_get_power(struct ieee80211_device *ieee,
-				  struct iw_request_info *info,
-				  union iwreq_data *wrqu, char *extra);
-
-extern void ieee80211_softmac_ips_scan_syncro(struct ieee80211_device *ieee);
-
-extern void ieee80211_sta_ps_send_null_frame(struct ieee80211_device *ieee,
-					     short pwr);
-
-extern const long ieee80211_wlan_frequencies[];
-
-extern inline void ieee80211_increment_scans(struct ieee80211_device *ieee)
-{
-	ieee->scans++;
-}
-
-extern inline int ieee80211_get_scans(struct ieee80211_device *ieee)
-{
-	return ieee->scans;
-}
-
-static inline const char *escape_essid(const char *essid, u8 essid_len) {
-	static char escaped[IW_ESSID_MAX_SIZE * 2 + 1];
-	const char *s = essid;
-	char *d = escaped;
-
-	if (ieee80211_is_empty_essid(essid, essid_len)) {
-		memcpy(escaped, "<hidden>", sizeof("<hidden>"));
-		return escaped;
-	}
-
-	essid_len = min(essid_len, (u8)IW_ESSID_MAX_SIZE);
-	while (essid_len--) {
-		if (*s == '\0') {
-			*d++ = '\\';
-			*d++ = '0';
-			s++;
-		} else {
-			*d++ = *s++;
-		}
-	}
-	*d = '\0';
-	return escaped;
-}
-#endif /* IEEE80211_H */
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt.c
deleted file mode 100644
index 101f0c0cdb0..00000000000
--- a/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt.c
+++ /dev/null
@@ -1,240 +0,0 @@
-/*
- * Host AP crypto routines
- *
- * Copyright (c) 2002-2003, Jouni Malinen <jkmaline@cc.hut.fi>
- * Portions Copyright (C) 2004, Intel Corporation <jketreno@linux.intel.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. See README and COPYING for
- * more details.
- *
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-//#include <linux/config.h>
-#include <linux/module.h>
-#include <linux/slab.h>
-#include <linux/string.h>
-#include <linux/errno.h>
-
-#include "ieee80211.h"
-
-MODULE_AUTHOR("Jouni Malinen");
-MODULE_DESCRIPTION("HostAP crypto");
-MODULE_LICENSE("GPL");
-
-struct ieee80211_crypto_alg {
-	struct list_head list;
-	struct ieee80211_crypto_ops *ops;
-};
-
-
-struct ieee80211_crypto {
-	struct list_head algs;
-	spinlock_t lock;
-};
-
-static struct ieee80211_crypto *hcrypt;
-
-void ieee80211_crypt_deinit_entries(struct ieee80211_device *ieee, int force)
-{
-	struct list_head *ptr, *n;
-	struct ieee80211_crypt_data *entry;
-
-	for (ptr = ieee->crypt_deinit_list.next, n = ptr->next;
-	     ptr != &ieee->crypt_deinit_list; ptr = n, n = ptr->next) {
-		entry = list_entry(ptr, struct ieee80211_crypt_data, list);
-
-		if (atomic_read(&entry->refcnt) != 0 && !force)
-			continue;
-
-		list_del(ptr);
-
-		if (entry->ops)
-			entry->ops->deinit(entry->priv);
-		kfree(entry);
-	}
-}
-
-void ieee80211_crypt_deinit_handler(unsigned long data)
-{
-	struct ieee80211_device *ieee = (struct ieee80211_device *)data;
-	unsigned long flags;
-
-	spin_lock_irqsave(&ieee->lock, flags);
-	ieee80211_crypt_deinit_entries(ieee, 0);
-	if (!list_empty(&ieee->crypt_deinit_list)) {
-		pr_debug("entries remaining in delayed crypt deletion list\n");
-		ieee->crypt_deinit_timer.expires = jiffies + HZ;
-		add_timer(&ieee->crypt_deinit_timer);
-	}
-	spin_unlock_irqrestore(&ieee->lock, flags);
-
-}
-
-void ieee80211_crypt_delayed_deinit(struct ieee80211_device *ieee,
-				    struct ieee80211_crypt_data **crypt)
-{
-	struct ieee80211_crypt_data *tmp;
-	unsigned long flags;
-
-	if (*crypt == NULL)
-		return;
-
-	tmp = *crypt;
-	*crypt = NULL;
-
-	/* must not run ops->deinit() while there may be pending encrypt or
-	 * decrypt operations. Use a list of delayed deinits to avoid needing
-	 * locking. */
-
-	spin_lock_irqsave(&ieee->lock, flags);
-	list_add(&tmp->list, &ieee->crypt_deinit_list);
-	if (!timer_pending(&ieee->crypt_deinit_timer)) {
-		ieee->crypt_deinit_timer.expires = jiffies + HZ;
-		add_timer(&ieee->crypt_deinit_timer);
-	}
-	spin_unlock_irqrestore(&ieee->lock, flags);
-}
-
-int ieee80211_register_crypto_ops(struct ieee80211_crypto_ops *ops)
-{
-	unsigned long flags;
-	struct ieee80211_crypto_alg *alg;
-
-	if (hcrypt == NULL)
-		return -1;
-
-	alg = kzalloc(sizeof(*alg), GFP_KERNEL);
-	if (alg == NULL)
-		return -ENOMEM;
-
-	alg->ops = ops;
-
-	spin_lock_irqsave(&hcrypt->lock, flags);
-	list_add(&alg->list, &hcrypt->algs);
-	spin_unlock_irqrestore(&hcrypt->lock, flags);
-
-	pr_debug("registered algorithm '%s'\n", ops->name);
-
-	return 0;
-}
-
-int ieee80211_unregister_crypto_ops(struct ieee80211_crypto_ops *ops)
-{
-	unsigned long flags;
-	struct list_head *ptr;
-	struct ieee80211_crypto_alg *del_alg = NULL;
-
-	if (hcrypt == NULL)
-		return -1;
-
-	spin_lock_irqsave(&hcrypt->lock, flags);
-	for (ptr = hcrypt->algs.next; ptr != &hcrypt->algs; ptr = ptr->next) {
-		struct ieee80211_crypto_alg *alg =
-			(struct ieee80211_crypto_alg *) ptr;
-		if (alg->ops == ops) {
-			list_del(&alg->list);
-			del_alg = alg;
-			break;
-		}
-	}
-	spin_unlock_irqrestore(&hcrypt->lock, flags);
-
-	if (del_alg) {
-		pr_debug("unregistered algorithm '%s'\n", ops->name);
-		kfree(del_alg);
-	}
-
-	return del_alg ? 0 : -1;
-}
-
-
-struct ieee80211_crypto_ops *ieee80211_get_crypto_ops(const char *name)
-{
-	unsigned long flags;
-	struct list_head *ptr;
-	struct ieee80211_crypto_alg *found_alg = NULL;
-
-	if (hcrypt == NULL)
-		return NULL;
-
-	spin_lock_irqsave(&hcrypt->lock, flags);
-	for (ptr = hcrypt->algs.next; ptr != &hcrypt->algs; ptr = ptr->next) {
-		struct ieee80211_crypto_alg *alg =
-			(struct ieee80211_crypto_alg *) ptr;
-		if (strcmp(alg->ops->name, name) == 0) {
-			found_alg = alg;
-			break;
-		}
-	}
-	spin_unlock_irqrestore(&hcrypt->lock, flags);
-
-	if (found_alg)
-		return found_alg->ops;
-	else
-		return NULL;
-}
-
-
-static void *ieee80211_crypt_null_init(int keyidx) { return (void *) 1; }
-static void ieee80211_crypt_null_deinit(void *priv) {}
-
-static struct ieee80211_crypto_ops ieee80211_crypt_null = {
-	.name			= "NULL",
-	.init			= ieee80211_crypt_null_init,
-	.deinit			= ieee80211_crypt_null_deinit,
-	.encrypt_mpdu		= NULL,
-	.decrypt_mpdu		= NULL,
-	.encrypt_msdu		= NULL,
-	.decrypt_msdu		= NULL,
-	.set_key		= NULL,
-	.get_key		= NULL,
-	.extra_prefix_len	= 0,
-	.extra_postfix_len	= 0,
-	.owner			= THIS_MODULE,
-};
-
-
-int ieee80211_crypto_init(void)
-{
-	int ret = -ENOMEM;
-
-	hcrypt = kzalloc(sizeof(*hcrypt), GFP_KERNEL);
-	if (!hcrypt)
-		goto out;
-
-	INIT_LIST_HEAD(&hcrypt->algs);
-	spin_lock_init(&hcrypt->lock);
-
-	ret = ieee80211_register_crypto_ops(&ieee80211_crypt_null);
-	if (ret < 0) {
-		kfree(hcrypt);
-		hcrypt = NULL;
-	}
-out:
-	return ret;
-}
-
-
-void ieee80211_crypto_deinit(void)
-{
-	struct list_head *ptr, *n;
-	struct ieee80211_crypto_alg *alg = NULL;
-
-	if (hcrypt == NULL)
-		return;
-
-	list_for_each_safe(ptr, n, &hcrypt->algs) {
-		alg = list_entry(ptr, struct ieee80211_crypto_alg, list);
-		if (alg) {
-			list_del(ptr);
-			pr_debug("unregistered algorithm '%s' (deinit)\n",
-				 alg->ops->name);
-			kfree(alg);
-		}
-	}
-	kfree(hcrypt);
-}
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt.h b/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt.h
deleted file mode 100644
index 0b4ea431982..00000000000
--- a/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt.h
+++ /dev/null
@@ -1,86 +0,0 @@
-/*
- * Original code based on Host AP (software wireless LAN access point) driver
- * for Intersil Prism2/2.5/3.
- *
- * Copyright (c) 2001-2002, SSH Communications Security Corp and Jouni Malinen
- * <jkmaline@cc.hut.fi>
- * Copyright (c) 2002-2003, Jouni Malinen <jkmaline@cc.hut.fi>
- *
- * Adaption to a generic IEEE 802.11 stack by James Ketrenos
- * <jketreno@linux.intel.com>
- *
- * Copyright (c) 2004, Intel Corporation
- *
- * 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. See README and COPYING for
- * more details.
- */
-
-/*
- * This file defines the interface to the ieee80211 crypto module.
- */
-#ifndef IEEE80211_CRYPT_H
-#define IEEE80211_CRYPT_H
-
-#include <linux/skbuff.h>
-
-struct ieee80211_crypto_ops {
-	const char *name;
-
-	/* init new crypto context (e.g., allocate private data space,
-	 * select IV, etc.); returns NULL on failure or pointer to allocated
-	 * private data on success */
-	void * (*init)(int keyidx);
-
-	/* deinitialize crypto context and free allocated private data */
-	void (*deinit)(void *priv);
-
-	/* encrypt/decrypt return < 0 on error or >= 0 on success. The return
-	 * value from decrypt_mpdu is passed as the keyidx value for
-	 * decrypt_msdu. skb must have enough head and tail room for the
-	 * encryption; if not, error will be returned; these functions are
-	 * called for all MPDUs (i.e., fragments).
-	 */
-	int (*encrypt_mpdu)(struct sk_buff *skb, int hdr_len, void *priv);
-	int (*decrypt_mpdu)(struct sk_buff *skb, int hdr_len, void *priv);
-
-	/* These functions are called for full MSDUs, i.e. full frames.
-	 * These can be NULL if full MSDU operations are not needed. */
-	int (*encrypt_msdu)(struct sk_buff *skb, int hdr_len, void *priv);
-	int (*decrypt_msdu)(struct sk_buff *skb, int keyidx, int hdr_len,
-			    void *priv);
-
-	int (*set_key)(void *key, int len, u8 *seq, void *priv);
-	int (*get_key)(void *key, int len, u8 *seq, void *priv);
-
-	/* procfs handler for printing out key information and possible
-	 * statistics */
-	char * (*print_stats)(char *p, void *priv);
-
-	/* maximum number of bytes added by encryption; encrypt buf is
-	 * allocated with extra_prefix_len bytes, copy of in_buf, and
-	 * extra_postfix_len; encrypt need not use all this space, but
-	 * the result must start at the beginning of the buffer and correct
-	 * length must be returned */
-	int extra_prefix_len, extra_postfix_len;
-
-	struct module *owner;
-};
-
-struct ieee80211_crypt_data {
-	struct list_head list; /* delayed deletion list */
-	struct ieee80211_crypto_ops *ops;
-	void *priv;
-	atomic_t refcnt;
-};
-
-int ieee80211_register_crypto_ops(struct ieee80211_crypto_ops *ops);
-int ieee80211_unregister_crypto_ops(struct ieee80211_crypto_ops *ops);
-struct ieee80211_crypto_ops *ieee80211_get_crypto_ops(const char *name);
-void ieee80211_crypt_deinit_entries(struct ieee80211_device *, int);
-void ieee80211_crypt_deinit_handler(unsigned long);
-void ieee80211_crypt_delayed_deinit(struct ieee80211_device *ieee,
-				    struct ieee80211_crypt_data **crypt);
-
-#endif
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt_ccmp.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt_ccmp.c
deleted file mode 100644
index 4fe25381863..00000000000
--- a/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt_ccmp.c
+++ /dev/null
@@ -1,455 +0,0 @@
-/*
- * Host AP crypt: host-based CCMP encryption implementation for Host AP driver
- *
- * Copyright (c) 2003-2004, Jouni Malinen <jkmaline@cc.hut.fi>
- *
- * 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. See README and COPYING for
- * more details.
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/module.h>
-#include <linux/slab.h>
-#include <linux/random.h>
-#include <linux/skbuff.h>
-#include <linux/netdevice.h>
-#include <linux/if_ether.h>
-#include <linux/if_arp.h>
-#include <linux/string.h>
-#include <linux/wireless.h>
-
-#include "ieee80211.h"
-
-#include <linux/crypto.h>
-#include <linux/scatterlist.h>
-
-MODULE_AUTHOR("Jouni Malinen");
-MODULE_DESCRIPTION("Host AP crypt: CCMP");
-MODULE_LICENSE("GPL");
-
-
-#define AES_BLOCK_LEN 16
-#define CCMP_HDR_LEN 8
-#define CCMP_MIC_LEN 8
-#define CCMP_TK_LEN 16
-#define CCMP_PN_LEN 6
-
-struct ieee80211_ccmp_data {
-	u8 key[CCMP_TK_LEN];
-	int key_set;
-
-	u8 tx_pn[CCMP_PN_LEN];
-	u8 rx_pn[CCMP_PN_LEN];
-
-	u32 dot11RSNAStatsCCMPFormatErrors;
-	u32 dot11RSNAStatsCCMPReplays;
-	u32 dot11RSNAStatsCCMPDecryptErrors;
-
-	int key_idx;
-
-	struct crypto_tfm *tfm;
-
-	/* scratch buffers for virt_to_page() (crypto API) */
-	u8 tx_b0[AES_BLOCK_LEN], tx_b[AES_BLOCK_LEN],
-		tx_e[AES_BLOCK_LEN], tx_s0[AES_BLOCK_LEN];
-	u8 rx_b0[AES_BLOCK_LEN], rx_b[AES_BLOCK_LEN], rx_a[AES_BLOCK_LEN];
-};
-
-static void ieee80211_ccmp_aes_encrypt(struct crypto_tfm *tfm,
-				const u8 pt[16], u8 ct[16])
-{
-	crypto_cipher_encrypt_one((void *)tfm, ct, pt);
-}
-
-static void *ieee80211_ccmp_init(int key_idx)
-{
-	struct ieee80211_ccmp_data *priv;
-
-	priv = kzalloc(sizeof(*priv), GFP_ATOMIC);
-	if (priv == NULL)
-		goto fail;
-	priv->key_idx = key_idx;
-
-	priv->tfm = (void *)crypto_alloc_cipher("aes", 0, CRYPTO_ALG_ASYNC);
-	if (IS_ERR(priv->tfm)) {
-		pr_debug("could not allocate crypto API aes\n");
-		priv->tfm = NULL;
-		goto fail;
-	}
-
-	return priv;
-
-fail:
-	if (priv) {
-		if (priv->tfm)
-			crypto_free_cipher((void *)priv->tfm);
-		kfree(priv);
-	}
-
-	return NULL;
-}
-
-
-static void ieee80211_ccmp_deinit(void *priv)
-{
-	struct ieee80211_ccmp_data *_priv = priv;
-
-	if (_priv && _priv->tfm)
-		crypto_free_cipher((void *)_priv->tfm);
-	kfree(priv);
-}
-
-
-static inline void xor_block(u8 *b, u8 *a, size_t len)
-{
-	int i;
-	for (i = 0; i < len; i++)
-		b[i] ^= a[i];
-}
-
-static void ccmp_init_blocks(struct crypto_tfm *tfm,
-			     struct ieee80211_hdr_4addr *hdr,
-			     u8 *pn, size_t dlen, u8 *b0, u8 *auth,
-			     u8 *s0)
-{
-	u8 *pos, qc = 0;
-	size_t aad_len;
-	u16 fc;
-	int a4_included, qc_included;
-	u8 aad[2 * AES_BLOCK_LEN];
-
-	fc = le16_to_cpu(hdr->frame_ctl);
-	a4_included = ((fc & (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) ==
-		       (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS));
-	/*
-	qc_included = ((WLAN_FC_GET_TYPE(fc) == IEEE80211_FTYPE_DATA) &&
-		       (WLAN_FC_GET_STYPE(fc) & 0x08));
-	*/
-	qc_included = ((WLAN_FC_GET_TYPE(fc) == IEEE80211_FTYPE_DATA) &&
-		       (WLAN_FC_GET_STYPE(fc) & 0x80));
-	aad_len = 22;
-	if (a4_included)
-		aad_len += 6;
-	if (qc_included) {
-		pos = (u8 *) &hdr->addr4;
-		if (a4_included)
-			pos += 6;
-		qc = *pos & 0x0f;
-		aad_len += 2;
-	}
-	/* CCM Initial Block:
-	 * Flag (Include authentication header, M=3 (8-octet MIC),
-	 *       L=1 (2-octet Dlen))
-	 * Nonce: 0x00 | A2 | PN
-	 * Dlen */
-	b0[0] = 0x59;
-	b0[1] = qc;
-	memcpy(b0 + 2, hdr->addr2, ETH_ALEN);
-	memcpy(b0 + 8, pn, CCMP_PN_LEN);
-	b0[14] = (dlen >> 8) & 0xff;
-	b0[15] = dlen & 0xff;
-
-	/* AAD:
-	 * FC with bits 4..6 and 11..13 masked to zero; 14 is always one
-	 * A1 | A2 | A3
-	 * SC with bits 4..15 (seq#) masked to zero
-	 * A4 (if present)
-	 * QC (if present)
-	 */
-	pos = (u8 *) hdr;
-	aad[0] = 0; /* aad_len >> 8 */
-	aad[1] = aad_len & 0xff;
-	aad[2] = pos[0] & 0x8f;
-	aad[3] = pos[1] & 0xc7;
-	memcpy(aad + 4, hdr->addr1, 3 * ETH_ALEN);
-	pos = (u8 *) &hdr->seq_ctl;
-	aad[22] = pos[0] & 0x0f;
-	aad[23] = 0; /* all bits masked */
-	memset(aad + 24, 0, 8);
-	if (a4_included)
-		memcpy(aad + 24, hdr->addr4, ETH_ALEN);
-	if (qc_included) {
-		aad[a4_included ? 30 : 24] = qc;
-		/* rest of QC masked */
-	}
-
-	/* Start with the first block and AAD */
-	ieee80211_ccmp_aes_encrypt(tfm, b0, auth);
-	xor_block(auth, aad, AES_BLOCK_LEN);
-	ieee80211_ccmp_aes_encrypt(tfm, auth, auth);
-	xor_block(auth, &aad[AES_BLOCK_LEN], AES_BLOCK_LEN);
-	ieee80211_ccmp_aes_encrypt(tfm, auth, auth);
-	b0[0] &= 0x07;
-	b0[14] = b0[15] = 0;
-	ieee80211_ccmp_aes_encrypt(tfm, b0, s0);
-}
-
-static int ieee80211_ccmp_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
-{
-	struct ieee80211_ccmp_data *key = priv;
-	int data_len, i;
-	u8 *pos;
-	struct ieee80211_hdr_4addr *hdr;
-	int blocks, last, len;
-	u8 *mic;
-	u8 *b0 = key->tx_b0;
-	u8 *b = key->tx_b;
-	u8 *e = key->tx_e;
-	u8 *s0 = key->tx_s0;
-
-	if (skb_headroom(skb) < CCMP_HDR_LEN ||
-	    skb_tailroom(skb) < CCMP_MIC_LEN ||
-	    skb->len < hdr_len)
-		return -1;
-
-	data_len = skb->len - hdr_len;
-	pos = skb_push(skb, CCMP_HDR_LEN);
-	memmove(pos, pos + CCMP_HDR_LEN, hdr_len);
-	pos += hdr_len;
-
-	i = CCMP_PN_LEN - 1;
-	while (i >= 0) {
-		key->tx_pn[i]++;
-		if (key->tx_pn[i] != 0)
-			break;
-		i--;
-	}
-
-	*pos++ = key->tx_pn[5];
-	*pos++ = key->tx_pn[4];
-	*pos++ = 0;
-	*pos++ = (key->key_idx << 6) | (1 << 5) /* Ext IV included */;
-	*pos++ = key->tx_pn[3];
-	*pos++ = key->tx_pn[2];
-	*pos++ = key->tx_pn[1];
-	*pos++ = key->tx_pn[0];
-
-	hdr = (struct ieee80211_hdr_4addr *)skb->data;
-	mic = skb_put(skb, CCMP_MIC_LEN);
-
-	ccmp_init_blocks(key->tfm, hdr, key->tx_pn, data_len, b0, b, s0);
-
-	blocks = (data_len + AES_BLOCK_LEN - 1) / AES_BLOCK_LEN;
-	last = data_len % AES_BLOCK_LEN;
-
-	for (i = 1; i <= blocks; i++) {
-		len = (i == blocks && last) ? last : AES_BLOCK_LEN;
-		/* Authentication */
-		xor_block(b, pos, len);
-		ieee80211_ccmp_aes_encrypt(key->tfm, b, b);
-		/* Encryption, with counter */
-		b0[14] = (i >> 8) & 0xff;
-		b0[15] = i & 0xff;
-		ieee80211_ccmp_aes_encrypt(key->tfm, b0, e);
-		xor_block(pos, e, len);
-		pos += len;
-	}
-
-	for (i = 0; i < CCMP_MIC_LEN; i++)
-		mic[i] = b[i] ^ s0[i];
-
-	return 0;
-}
-
-
-static int ieee80211_ccmp_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
-{
-	struct ieee80211_ccmp_data *key = priv;
-	u8 keyidx, *pos;
-	struct ieee80211_hdr_4addr *hdr;
-	u8 pn[6];
-	size_t data_len = skb->len - hdr_len - CCMP_HDR_LEN - CCMP_MIC_LEN;
-	u8 *mic = skb->data + skb->len - CCMP_MIC_LEN;
-	u8 *b0 = key->rx_b0;
-	u8 *b = key->rx_b;
-	u8 *a = key->rx_a;
-	int i, blocks, last, len;
-
-	if (skb->len < hdr_len + CCMP_HDR_LEN + CCMP_MIC_LEN) {
-		key->dot11RSNAStatsCCMPFormatErrors++;
-		return -1;
-	}
-
-	hdr = (struct ieee80211_hdr_4addr *)skb->data;
-	pos = skb->data + hdr_len;
-	keyidx = pos[3];
-	if (!(keyidx & (1 << 5))) {
-		if (net_ratelimit()) {
-			pr_debug("received packet without ExtIV flag from %pM\n",
-				 hdr->addr2);
-		}
-		key->dot11RSNAStatsCCMPFormatErrors++;
-		return -2;
-	}
-	keyidx >>= 6;
-	if (key->key_idx != keyidx) {
-		pr_debug("RX tkey->key_idx=%d frame keyidx=%d priv=%p\n",
-			 key->key_idx, keyidx, priv);
-		return -6;
-	}
-	if (!key->key_set) {
-		if (net_ratelimit()) {
-			pr_debug("received packet from %pM with keyid=%d that does not have a configured key\n",
-				 hdr->addr2, keyidx);
-		}
-		return -3;
-	}
-
-	pn[0] = pos[7];
-	pn[1] = pos[6];
-	pn[2] = pos[5];
-	pn[3] = pos[4];
-	pn[4] = pos[1];
-	pn[5] = pos[0];
-	pos += 8;
-
-	if (memcmp(pn, key->rx_pn, CCMP_PN_LEN) <= 0) {
-		if (net_ratelimit()) {
-			pr_debug("replay detected: STA=%pM previous PN %pm received PN %pm\n",
-				 hdr->addr2, key->rx_pn, pn);
-		}
-		key->dot11RSNAStatsCCMPReplays++;
-		return -4;
-	}
-
-	ccmp_init_blocks(key->tfm, hdr, pn, data_len, b0, a, b);
-	xor_block(mic, b, CCMP_MIC_LEN);
-
-	blocks = (data_len + AES_BLOCK_LEN - 1) / AES_BLOCK_LEN;
-	last = data_len % AES_BLOCK_LEN;
-
-	for (i = 1; i <= blocks; i++) {
-		len = (i == blocks && last) ? last : AES_BLOCK_LEN;
-		/* Decrypt, with counter */
-		b0[14] = (i >> 8) & 0xff;
-		b0[15] = i & 0xff;
-		ieee80211_ccmp_aes_encrypt(key->tfm, b0, b);
-		xor_block(pos, b, len);
-		/* Authentication */
-		xor_block(a, pos, len);
-		ieee80211_ccmp_aes_encrypt(key->tfm, a, a);
-		pos += len;
-	}
-
-	if (memcmp(mic, a, CCMP_MIC_LEN) != 0) {
-		if (net_ratelimit())
-			pr_debug("decrypt failed: STA=%pM\n", hdr->addr2);
-
-		key->dot11RSNAStatsCCMPDecryptErrors++;
-		return -5;
-	}
-
-	memcpy(key->rx_pn, pn, CCMP_PN_LEN);
-
-	/* Remove hdr and MIC */
-	memmove(skb->data + CCMP_HDR_LEN, skb->data, hdr_len);
-	skb_pull(skb, CCMP_HDR_LEN);
-	skb_trim(skb, skb->len - CCMP_MIC_LEN);
-
-	return keyidx;
-}
-
-
-static int ieee80211_ccmp_set_key(void *key, int len, u8 *seq, void *priv)
-{
-	struct ieee80211_ccmp_data *data = priv;
-	int keyidx;
-	struct crypto_tfm *tfm = data->tfm;
-
-	keyidx = data->key_idx;
-	memset(data, 0, sizeof(*data));
-	data->key_idx = keyidx;
-	data->tfm = tfm;
-	if (len == CCMP_TK_LEN) {
-		memcpy(data->key, key, CCMP_TK_LEN);
-		data->key_set = 1;
-		if (seq) {
-			data->rx_pn[0] = seq[5];
-			data->rx_pn[1] = seq[4];
-			data->rx_pn[2] = seq[3];
-			data->rx_pn[3] = seq[2];
-			data->rx_pn[4] = seq[1];
-			data->rx_pn[5] = seq[0];
-		}
-		crypto_cipher_setkey((void *)data->tfm, data->key, CCMP_TK_LEN);
-	} else if (len == 0)
-		data->key_set = 0;
-	else
-		return -1;
-
-	return 0;
-}
-
-
-static int ieee80211_ccmp_get_key(void *key, int len, u8 *seq, void *priv)
-{
-	struct ieee80211_ccmp_data *data = priv;
-
-	if (len < CCMP_TK_LEN)
-		return -1;
-
-	if (!data->key_set)
-		return 0;
-	memcpy(key, data->key, CCMP_TK_LEN);
-
-	if (seq) {
-		seq[0] = data->tx_pn[5];
-		seq[1] = data->tx_pn[4];
-		seq[2] = data->tx_pn[3];
-		seq[3] = data->tx_pn[2];
-		seq[4] = data->tx_pn[1];
-		seq[5] = data->tx_pn[0];
-	}
-
-	return CCMP_TK_LEN;
-}
-
-
-static char *ieee80211_ccmp_print_stats(char *p, void *priv)
-{
-	struct ieee80211_ccmp_data *ccmp = priv;
-	p += sprintf(p,
-		     "key[%d] alg=CCMP key_set=%d tx_pn=%pm rx_pn=%pm format_errors=%d replays=%d decrypt_errors=%d\n",
-		     ccmp->key_idx, ccmp->key_set,
-		     ccmp->tx_pn, ccmp->rx_pn,
-		     ccmp->dot11RSNAStatsCCMPFormatErrors,
-		     ccmp->dot11RSNAStatsCCMPReplays,
-		     ccmp->dot11RSNAStatsCCMPDecryptErrors);
-
-	return p;
-}
-
-void ieee80211_ccmp_null(void)
-{
-	return;
-}
-static struct ieee80211_crypto_ops ieee80211_crypt_ccmp = {
-	.name			= "CCMP",
-	.init			= ieee80211_ccmp_init,
-	.deinit			= ieee80211_ccmp_deinit,
-	.encrypt_mpdu		= ieee80211_ccmp_encrypt,
-	.decrypt_mpdu		= ieee80211_ccmp_decrypt,
-	.encrypt_msdu		= NULL,
-	.decrypt_msdu		= NULL,
-	.set_key		= ieee80211_ccmp_set_key,
-	.get_key		= ieee80211_ccmp_get_key,
-	.print_stats		= ieee80211_ccmp_print_stats,
-	.extra_prefix_len	= CCMP_HDR_LEN,
-	.extra_postfix_len	= CCMP_MIC_LEN,
-	.owner			= THIS_MODULE,
-};
-
-
-int ieee80211_crypto_ccmp_init(void)
-{
-	return ieee80211_register_crypto_ops(&ieee80211_crypt_ccmp);
-}
-
-
-void ieee80211_crypto_ccmp_exit(void)
-{
-	ieee80211_unregister_crypto_ops(&ieee80211_crypt_ccmp);
-}
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt_tkip.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt_tkip.c
deleted file mode 100644
index 6c1acc5dfba..00000000000
--- a/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt_tkip.c
+++ /dev/null
@@ -1,740 +0,0 @@
-/*
- * Host AP crypt: host-based TKIP encryption implementation for Host AP driver
- *
- * Copyright (c) 2003-2004, Jouni Malinen <jkmaline@cc.hut.fi>
- *
- * 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. See README and COPYING for
- * more details.
- */
-
-#include <linux/module.h>
-#include <linux/slab.h>
-#include <linux/random.h>
-#include <linux/skbuff.h>
-#include <linux/netdevice.h>
-#include <linux/if_ether.h>
-#include <linux/if_arp.h>
-#include <asm/string.h>
-
-#include "ieee80211.h"
-
-#include <linux/crypto.h>
-#include <linux/scatterlist.h>
-#include <linux/crc32.h>
-
-MODULE_AUTHOR("Jouni Malinen");
-MODULE_DESCRIPTION("Host AP crypt: TKIP");
-MODULE_LICENSE("GPL");
-
-
-struct ieee80211_tkip_data {
-#define TKIP_KEY_LEN 32
-	u8 key[TKIP_KEY_LEN];
-	int key_set;
-
-	u32 tx_iv32;
-	u16 tx_iv16;
-	u16 tx_ttak[5];
-	int tx_phase1_done;
-
-	u32 rx_iv32;
-	u16 rx_iv16;
-	u16 rx_ttak[5];
-	int rx_phase1_done;
-	u32 rx_iv32_new;
-	u16 rx_iv16_new;
-
-	u32 dot11RSNAStatsTKIPReplays;
-	u32 dot11RSNAStatsTKIPICVErrors;
-	u32 dot11RSNAStatsTKIPLocalMICFailures;
-
-	int key_idx;
-
-	struct crypto_blkcipher *rx_tfm_arc4;
-	struct crypto_hash *rx_tfm_michael;
-	struct crypto_blkcipher *tx_tfm_arc4;
-	struct crypto_hash *tx_tfm_michael;
-	struct crypto_tfm *tfm_arc4;
-	struct crypto_tfm *tfm_michael;
-
-	/* scratch buffers for virt_to_page() (crypto API) */
-	u8 rx_hdr[16], tx_hdr[16];
-};
-
-static void *ieee80211_tkip_init(int key_idx)
-{
-	struct ieee80211_tkip_data *priv;
-
-	priv = kzalloc(sizeof(*priv), GFP_ATOMIC);
-	if (priv == NULL)
-		goto fail;
-	priv->key_idx = key_idx;
-
-	priv->tx_tfm_arc4 = crypto_alloc_blkcipher("ecb(arc4)", 0,
-						CRYPTO_ALG_ASYNC);
-	if (IS_ERR(priv->tx_tfm_arc4)) {
-		printk(KERN_DEBUG "ieee80211_crypt_tkip: could not allocate "
-		       "crypto API arc4\n");
-		priv->tx_tfm_arc4 = NULL;
-		goto fail;
-	}
-
-	priv->tx_tfm_michael = crypto_alloc_hash("michael_mic", 0,
-						 CRYPTO_ALG_ASYNC);
-	if (IS_ERR(priv->tx_tfm_michael)) {
-		printk(KERN_DEBUG "ieee80211_crypt_tkip: could not allocate "
-		       "crypto API michael_mic\n");
-		priv->tx_tfm_michael = NULL;
-		goto fail;
-	}
-
-	priv->rx_tfm_arc4 = crypto_alloc_blkcipher("ecb(arc4)", 0,
-						CRYPTO_ALG_ASYNC);
-	if (IS_ERR(priv->rx_tfm_arc4)) {
-		printk(KERN_DEBUG "ieee80211_crypt_tkip: could not allocate "
-		       "crypto API arc4\n");
-		priv->rx_tfm_arc4 = NULL;
-		goto fail;
-	}
-
-	priv->rx_tfm_michael = crypto_alloc_hash("michael_mic", 0,
-						 CRYPTO_ALG_ASYNC);
-	if (IS_ERR(priv->rx_tfm_michael)) {
-		printk(KERN_DEBUG "ieee80211_crypt_tkip: could not allocate "
-		       "crypto API michael_mic\n");
-		priv->rx_tfm_michael = NULL;
-		goto fail;
-	}
-
-	return priv;
-
-fail:
-	if (priv) {
-		if (priv->tx_tfm_michael)
-			crypto_free_hash(priv->tx_tfm_michael);
-		if (priv->tx_tfm_arc4)
-			crypto_free_blkcipher(priv->tx_tfm_arc4);
-		if (priv->rx_tfm_michael)
-			crypto_free_hash(priv->rx_tfm_michael);
-		if (priv->rx_tfm_arc4)
-			crypto_free_blkcipher(priv->rx_tfm_arc4);
-		kfree(priv);
-	}
-
-	return NULL;
-}
-
-
-static void ieee80211_tkip_deinit(void *priv)
-{
-	struct ieee80211_tkip_data *_priv = priv;
-
-	if (_priv) {
-		if (_priv->tx_tfm_michael)
-			crypto_free_hash(_priv->tx_tfm_michael);
-		if (_priv->tx_tfm_arc4)
-			crypto_free_blkcipher(_priv->tx_tfm_arc4);
-		if (_priv->rx_tfm_michael)
-			crypto_free_hash(_priv->rx_tfm_michael);
-		if (_priv->rx_tfm_arc4)
-			crypto_free_blkcipher(_priv->rx_tfm_arc4);
-	}
-	kfree(priv);
-}
-
-
-static inline u16 RotR1(u16 val)
-{
-	return (val >> 1) | (val << 15);
-}
-
-
-static inline u8 Lo8(u16 val)
-{
-	return val & 0xff;
-}
-
-
-static inline u8 Hi8(u16 val)
-{
-	return val >> 8;
-}
-
-
-static inline u16 Lo16(u32 val)
-{
-	return val & 0xffff;
-}
-
-
-static inline u16 Hi16(u32 val)
-{
-	return val >> 16;
-}
-
-
-static inline u16 Mk16(u8 hi, u8 lo)
-{
-	return lo | (((u16) hi) << 8);
-}
-
-
-static inline u16 Mk16_le(u16 *v)
-{
-	return le16_to_cpu(*v);
-}
-
-
-static const u16 Sbox[256] = {
-	0xC6A5, 0xF884, 0xEE99, 0xF68D, 0xFF0D, 0xD6BD, 0xDEB1, 0x9154,
-	0x6050, 0x0203, 0xCEA9, 0x567D, 0xE719, 0xB562, 0x4DE6, 0xEC9A,
-	0x8F45, 0x1F9D, 0x8940, 0xFA87, 0xEF15, 0xB2EB, 0x8EC9, 0xFB0B,
-	0x41EC, 0xB367, 0x5FFD, 0x45EA, 0x23BF, 0x53F7, 0xE496, 0x9B5B,
-	0x75C2, 0xE11C, 0x3DAE, 0x4C6A, 0x6C5A, 0x7E41, 0xF502, 0x834F,
-	0x685C, 0x51F4, 0xD134, 0xF908, 0xE293, 0xAB73, 0x6253, 0x2A3F,
-	0x080C, 0x9552, 0x4665, 0x9D5E, 0x3028, 0x37A1, 0x0A0F, 0x2FB5,
-	0x0E09, 0x2436, 0x1B9B, 0xDF3D, 0xCD26, 0x4E69, 0x7FCD, 0xEA9F,
-	0x121B, 0x1D9E, 0x5874, 0x342E, 0x362D, 0xDCB2, 0xB4EE, 0x5BFB,
-	0xA4F6, 0x764D, 0xB761, 0x7DCE, 0x527B, 0xDD3E, 0x5E71, 0x1397,
-	0xA6F5, 0xB968, 0x0000, 0xC12C, 0x4060, 0xE31F, 0x79C8, 0xB6ED,
-	0xD4BE, 0x8D46, 0x67D9, 0x724B, 0x94DE, 0x98D4, 0xB0E8, 0x854A,
-	0xBB6B, 0xC52A, 0x4FE5, 0xED16, 0x86C5, 0x9AD7, 0x6655, 0x1194,
-	0x8ACF, 0xE910, 0x0406, 0xFE81, 0xA0F0, 0x7844, 0x25BA, 0x4BE3,
-	0xA2F3, 0x5DFE, 0x80C0, 0x058A, 0x3FAD, 0x21BC, 0x7048, 0xF104,
-	0x63DF, 0x77C1, 0xAF75, 0x4263, 0x2030, 0xE51A, 0xFD0E, 0xBF6D,
-	0x814C, 0x1814, 0x2635, 0xC32F, 0xBEE1, 0x35A2, 0x88CC, 0x2E39,
-	0x9357, 0x55F2, 0xFC82, 0x7A47, 0xC8AC, 0xBAE7, 0x322B, 0xE695,
-	0xC0A0, 0x1998, 0x9ED1, 0xA37F, 0x4466, 0x547E, 0x3BAB, 0x0B83,
-	0x8CCA, 0xC729, 0x6BD3, 0x283C, 0xA779, 0xBCE2, 0x161D, 0xAD76,
-	0xDB3B, 0x6456, 0x744E, 0x141E, 0x92DB, 0x0C0A, 0x486C, 0xB8E4,
-	0x9F5D, 0xBD6E, 0x43EF, 0xC4A6, 0x39A8, 0x31A4, 0xD337, 0xF28B,
-	0xD532, 0x8B43, 0x6E59, 0xDAB7, 0x018C, 0xB164, 0x9CD2, 0x49E0,
-	0xD8B4, 0xACFA, 0xF307, 0xCF25, 0xCAAF, 0xF48E, 0x47E9, 0x1018,
-	0x6FD5, 0xF088, 0x4A6F, 0x5C72, 0x3824, 0x57F1, 0x73C7, 0x9751,
-	0xCB23, 0xA17C, 0xE89C, 0x3E21, 0x96DD, 0x61DC, 0x0D86, 0x0F85,
-	0xE090, 0x7C42, 0x71C4, 0xCCAA, 0x90D8, 0x0605, 0xF701, 0x1C12,
-	0xC2A3, 0x6A5F, 0xAEF9, 0x69D0, 0x1791, 0x9958, 0x3A27, 0x27B9,
-	0xD938, 0xEB13, 0x2BB3, 0x2233, 0xD2BB, 0xA970, 0x0789, 0x33A7,
-	0x2DB6, 0x3C22, 0x1592, 0xC920, 0x8749, 0xAAFF, 0x5078, 0xA57A,
-	0x038F, 0x59F8, 0x0980, 0x1A17, 0x65DA, 0xD731, 0x84C6, 0xD0B8,
-	0x82C3, 0x29B0, 0x5A77, 0x1E11, 0x7BCB, 0xA8FC, 0x6DD6, 0x2C3A,
-};
-
-
-static inline u16 _S_(u16 v)
-{
-	u16 t = Sbox[Hi8(v)];
-	return Sbox[Lo8(v)] ^ ((t << 8) | (t >> 8));
-}
-
-#define PHASE1_LOOP_COUNT 8
-
-static void tkip_mixing_phase1(u16 *TTAK, const u8 *TK, const u8 *TA, u32 IV32)
-{
-	int i, j;
-
-	/* Initialize the 80-bit TTAK from TSC (IV32) and TA[0..5] */
-	TTAK[0] = Lo16(IV32);
-	TTAK[1] = Hi16(IV32);
-	TTAK[2] = Mk16(TA[1], TA[0]);
-	TTAK[3] = Mk16(TA[3], TA[2]);
-	TTAK[4] = Mk16(TA[5], TA[4]);
-
-	for (i = 0; i < PHASE1_LOOP_COUNT; i++) {
-		j = 2 * (i & 1);
-		TTAK[0] += _S_(TTAK[4] ^ Mk16(TK[1 + j], TK[0 + j]));
-		TTAK[1] += _S_(TTAK[0] ^ Mk16(TK[5 + j], TK[4 + j]));
-		TTAK[2] += _S_(TTAK[1] ^ Mk16(TK[9 + j], TK[8 + j]));
-		TTAK[3] += _S_(TTAK[2] ^ Mk16(TK[13 + j], TK[12 + j]));
-		TTAK[4] += _S_(TTAK[3] ^ Mk16(TK[1 + j], TK[0 + j])) + i;
-	}
-}
-
-
-static void tkip_mixing_phase2(u8 *WEPSeed, const u8 *TK, const u16 *TTAK,
-			       u16 IV16)
-{
-	/* Make temporary area overlap WEP seed so that the final copy can be
-	 * avoided on little endian hosts. */
-	u16 *PPK = (u16 *) &WEPSeed[4];
-
-	/* Step 1 - make copy of TTAK and bring in TSC */
-	PPK[0] = TTAK[0];
-	PPK[1] = TTAK[1];
-	PPK[2] = TTAK[2];
-	PPK[3] = TTAK[3];
-	PPK[4] = TTAK[4];
-	PPK[5] = TTAK[4] + IV16;
-
-	/* Step 2 - 96-bit bijective mixing using S-box */
-	PPK[0] += _S_(PPK[5] ^ Mk16_le((u16 *) &TK[0]));
-	PPK[1] += _S_(PPK[0] ^ Mk16_le((u16 *) &TK[2]));
-	PPK[2] += _S_(PPK[1] ^ Mk16_le((u16 *) &TK[4]));
-	PPK[3] += _S_(PPK[2] ^ Mk16_le((u16 *) &TK[6]));
-	PPK[4] += _S_(PPK[3] ^ Mk16_le((u16 *) &TK[8]));
-	PPK[5] += _S_(PPK[4] ^ Mk16_le((u16 *) &TK[10]));
-
-	PPK[0] += RotR1(PPK[5] ^ Mk16_le((u16 *) &TK[12]));
-	PPK[1] += RotR1(PPK[0] ^ Mk16_le((u16 *) &TK[14]));
-	PPK[2] += RotR1(PPK[1]);
-	PPK[3] += RotR1(PPK[2]);
-	PPK[4] += RotR1(PPK[3]);
-	PPK[5] += RotR1(PPK[4]);
-
-	/* Step 3 - bring in last of TK bits, assign 24-bit WEP IV value
-	 * WEPSeed[0..2] is transmitted as WEP IV */
-	WEPSeed[0] = Hi8(IV16);
-	WEPSeed[1] = (Hi8(IV16) | 0x20) & 0x7F;
-	WEPSeed[2] = Lo8(IV16);
-	WEPSeed[3] = Lo8((PPK[5] ^ Mk16_le((u16 *) &TK[0])) >> 1);
-
-#ifdef __BIG_ENDIAN
-	{
-		int i;
-		for (i = 0; i < 6; i++)
-			PPK[i] = (PPK[i] << 8) | (PPK[i] >> 8);
-	}
-#endif
-}
-
-static int ieee80211_tkip_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
-{
-	struct ieee80211_tkip_data *tkey = priv;
-	struct blkcipher_desc desc = {.tfm = tkey->tx_tfm_arc4};
-	int len;
-	u8  *pos;
-	struct ieee80211_hdr_4addr *hdr;
-	u8 rc4key[16], *icv;
-	u32 crc;
-	struct scatterlist sg;
-	int ret;
-
-	ret = 0;
-	if (skb_headroom(skb) < 8 || skb_tailroom(skb) < 4 ||
-	    skb->len < hdr_len)
-		return -1;
-
-	hdr = (struct ieee80211_hdr_4addr *)skb->data;
-
-	if (!tkey->tx_phase1_done) {
-		tkip_mixing_phase1(tkey->tx_ttak, tkey->key, hdr->addr2,
-				   tkey->tx_iv32);
-		tkey->tx_phase1_done = 1;
-	}
-	tkip_mixing_phase2(rc4key, tkey->key, tkey->tx_ttak, tkey->tx_iv16);
-
-	len = skb->len - hdr_len;
-	pos = skb_push(skb, 8);
-	memmove(pos, pos + 8, hdr_len);
-	pos += hdr_len;
-
-	*pos++ = rc4key[0];
-	*pos++ = rc4key[1];
-	*pos++ = rc4key[2];
-	*pos++ = (tkey->key_idx << 6) | (1 << 5) /* Ext IV included */;
-	*pos++ = tkey->tx_iv32 & 0xff;
-	*pos++ = (tkey->tx_iv32 >> 8) & 0xff;
-	*pos++ = (tkey->tx_iv32 >> 16) & 0xff;
-	*pos++ = (tkey->tx_iv32 >> 24) & 0xff;
-
-	icv = skb_put(skb, 4);
-	crc = ~crc32_le(~0, pos, len);
-	icv[0] = crc;
-	icv[1] = crc >> 8;
-	icv[2] = crc >> 16;
-	icv[3] = crc >> 24;
-	crypto_blkcipher_setkey(tkey->tx_tfm_arc4, rc4key, 16);
-	sg_init_one(&sg, pos, len + 4);
-	ret = crypto_blkcipher_encrypt(&desc, &sg, &sg, len + 4);
-
-	tkey->tx_iv16++;
-	if (tkey->tx_iv16 == 0) {
-		tkey->tx_phase1_done = 0;
-		tkey->tx_iv32++;
-	}
-	   return ret;
-}
-
-static int ieee80211_tkip_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
-{
-	struct ieee80211_tkip_data *tkey = priv;
-	struct blkcipher_desc desc = { .tfm = tkey->rx_tfm_arc4 };
-	u8 keyidx, *pos;
-	u32 iv32;
-	u16 iv16;
-	struct ieee80211_hdr_4addr *hdr;
-	u8 icv[4];
-	u32 crc;
-	struct scatterlist sg;
-	u8 rc4key[16];
-	int plen;
-
-	if (skb->len < hdr_len + 8 + 4)
-		return -1;
-
-	hdr = (struct ieee80211_hdr_4addr *)skb->data;
-	pos = skb->data + hdr_len;
-	keyidx = pos[3];
-	if (!(keyidx & (1 << 5))) {
-		if (net_ratelimit()) {
-			printk(KERN_DEBUG "TKIP: received packet without ExtIV"
-			       " flag from %pM\n", hdr->addr2);
-		}
-		return -2;
-	}
-	keyidx >>= 6;
-	if (tkey->key_idx != keyidx) {
-		printk(KERN_DEBUG "TKIP: RX tkey->key_idx=%d frame "
-		       "keyidx=%d priv=%p\n", tkey->key_idx, keyidx, priv);
-		return -6;
-	}
-	if (!tkey->key_set) {
-		if (net_ratelimit()) {
-			printk(KERN_DEBUG "TKIP: received packet from %pM"
-			       " with keyid=%d that does not have a configured"
-			       " key\n", hdr->addr2, keyidx);
-		}
-		return -3;
-	}
-	iv16 = (pos[0] << 8) | pos[2];
-	iv32 = pos[4] | (pos[5] << 8) | (pos[6] << 16) | (pos[7] << 24);
-	pos += 8;
-
-	if (iv32 < tkey->rx_iv32 ||
-	    (iv32 == tkey->rx_iv32 && iv16 <= tkey->rx_iv16)) {
-		if (net_ratelimit()) {
-			printk(KERN_DEBUG "TKIP: replay detected: STA=%pM"
-			       " previous TSC %08x%04x received TSC "
-			       "%08x%04x\n", hdr->addr2,
-			       tkey->rx_iv32, tkey->rx_iv16, iv32, iv16);
-		}
-		tkey->dot11RSNAStatsTKIPReplays++;
-		return -4;
-	}
-
-	if (iv32 != tkey->rx_iv32 || !tkey->rx_phase1_done) {
-		tkip_mixing_phase1(tkey->rx_ttak, tkey->key, hdr->addr2, iv32);
-		tkey->rx_phase1_done = 1;
-	}
-	tkip_mixing_phase2(rc4key, tkey->key, tkey->rx_ttak, iv16);
-
-	plen = skb->len - hdr_len - 12;
-	crypto_blkcipher_setkey(tkey->rx_tfm_arc4, rc4key, 16);
-	sg_init_one(&sg, pos, plen + 4);
-	if (crypto_blkcipher_decrypt(&desc, &sg, &sg, plen + 4)) {
-		if (net_ratelimit()) {
-			printk(KERN_DEBUG ": TKIP: failed to decrypt "
-			       "received packet from %pM\n",
-			       hdr->addr2);
-		}
-		return -7;
-	}
-
-	crc = ~crc32_le(~0, pos, plen);
-	icv[0] = crc;
-	icv[1] = crc >> 8;
-	icv[2] = crc >> 16;
-	icv[3] = crc >> 24;
-	if (memcmp(icv, pos + plen, 4) != 0) {
-		if (iv32 != tkey->rx_iv32) {
-			/* Previously cached Phase1 result was already lost, so
-			 * it needs to be recalculated for the next packet. */
-			tkey->rx_phase1_done = 0;
-		}
-		if (net_ratelimit()) {
-			printk(KERN_DEBUG "TKIP: ICV error detected: STA="
-			       "%pM\n", hdr->addr2);
-		}
-		tkey->dot11RSNAStatsTKIPICVErrors++;
-		return -5;
-	}
-
-	/* Update real counters only after Michael MIC verification has
-	 * completed */
-	tkey->rx_iv32_new = iv32;
-	tkey->rx_iv16_new = iv16;
-
-	/* Remove IV and ICV */
-	memmove(skb->data + 8, skb->data, hdr_len);
-	skb_pull(skb, 8);
-	skb_trim(skb, skb->len - 4);
-
-	return keyidx;
-}
-
-static int michael_mic(struct crypto_hash *tfm_michael, u8 *key, u8 *hdr,
-			u8 *data, size_t data_len, u8 *mic)
-{
-	struct hash_desc desc;
-	struct scatterlist sg[2];
-
-	if (tfm_michael == NULL) {
-		printk(KERN_WARNING "michael_mic: tfm_michael == NULL\n");
-		return -1;
-	}
-
-	sg_init_table(sg, 2);
-	sg_set_buf(&sg[0], hdr, 16);
-	sg_set_buf(&sg[1], data, data_len);
-
-	if (crypto_hash_setkey(tfm_michael, key, 8))
-		return -1;
-
-	desc.tfm = tfm_michael;
-	desc.flags = 0;
-	return crypto_hash_digest(&desc, sg, data_len + 16, mic);
-}
-
-static void michael_mic_hdr(struct sk_buff *skb, u8 *hdr)
-{
-	struct ieee80211_hdr_4addr *hdr11;
-
-	hdr11 = (struct ieee80211_hdr_4addr *)skb->data;
-	switch (le16_to_cpu(hdr11->frame_ctl) &
-		(IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS)) {
-	case IEEE80211_FCTL_TODS:
-		memcpy(hdr, hdr11->addr3, ETH_ALEN); /* DA */
-		memcpy(hdr + ETH_ALEN, hdr11->addr2, ETH_ALEN); /* SA */
-		break;
-	case IEEE80211_FCTL_FROMDS:
-		memcpy(hdr, hdr11->addr1, ETH_ALEN); /* DA */
-		memcpy(hdr + ETH_ALEN, hdr11->addr3, ETH_ALEN); /* SA */
-		break;
-	case IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS:
-		memcpy(hdr, hdr11->addr3, ETH_ALEN); /* DA */
-		memcpy(hdr + ETH_ALEN, hdr11->addr4, ETH_ALEN); /* SA */
-		break;
-	case 0:
-		memcpy(hdr, hdr11->addr1, ETH_ALEN); /* DA */
-		memcpy(hdr + ETH_ALEN, hdr11->addr2, ETH_ALEN); /* SA */
-		break;
-	}
-
-	hdr[12] = 0; /* priority */
-
-	hdr[13] = hdr[14] = hdr[15] = 0; /* reserved */
-}
-
-
-static int ieee80211_michael_mic_add(struct sk_buff *skb, int hdr_len,
-				     void *priv)
-{
-	struct ieee80211_tkip_data *tkey = priv;
-	u8 *pos;
-	struct ieee80211_hdr_4addr *hdr;
-
-	hdr = (struct ieee80211_hdr_4addr *)skb->data;
-
-	if (skb_tailroom(skb) < 8 || skb->len < hdr_len) {
-		printk(KERN_DEBUG "Invalid packet for Michael MIC add "
-		       "(tailroom=%d hdr_len=%d skb->len=%d)\n",
-		       skb_tailroom(skb), hdr_len, skb->len);
-		return -1;
-	}
-
-	michael_mic_hdr(skb, tkey->tx_hdr);
-
-	if (IEEE80211_QOS_HAS_SEQ(le16_to_cpu(hdr->frame_ctl)))
-		tkey->tx_hdr[12] = *(skb->data + hdr_len - 2) & 0x07;
-
-	pos = skb_put(skb, 8);
-
-	if (michael_mic(tkey->tx_tfm_michael, &tkey->key[16], tkey->tx_hdr,
-			skb->data + hdr_len, skb->len - 8 - hdr_len, pos))
-		return -1;
-
-	return 0;
-}
-
-static void ieee80211_michael_mic_failure(struct net_device *dev,
-					  struct ieee80211_hdr_4addr *hdr,
-					  int keyidx)
-{
-	union iwreq_data wrqu;
-	struct iw_michaelmicfailure ev;
-
-	/* TODO: needed parameters: count, keyid, key type, TSC */
-	memset(&ev, 0, sizeof(ev));
-	ev.flags = keyidx & IW_MICFAILURE_KEY_ID;
-	if (hdr->addr1[0] & 0x01)
-		ev.flags |= IW_MICFAILURE_GROUP;
-	else
-		ev.flags |= IW_MICFAILURE_PAIRWISE;
-	ev.src_addr.sa_family = ARPHRD_ETHER;
-	memcpy(ev.src_addr.sa_data, hdr->addr2, ETH_ALEN);
-	memset(&wrqu, 0, sizeof(wrqu));
-	wrqu.data.length = sizeof(ev);
-	wireless_send_event(dev, IWEVMICHAELMICFAILURE, &wrqu, (char *) &ev);
-}
-
-static int ieee80211_michael_mic_verify(struct sk_buff *skb, int keyidx,
-					int hdr_len, void *priv)
-{
-	struct ieee80211_tkip_data *tkey = priv;
-	u8 mic[8];
-	struct ieee80211_hdr_4addr *hdr;
-
-	hdr = (struct ieee80211_hdr_4addr *)skb->data;
-
-	if (!tkey->key_set)
-		return -1;
-
-	michael_mic_hdr(skb, tkey->rx_hdr);
-	if (IEEE80211_QOS_HAS_SEQ(le16_to_cpu(hdr->frame_ctl)))
-		tkey->rx_hdr[12] = *(skb->data + hdr_len - 2) & 0x07;
-
-	if (michael_mic(tkey->rx_tfm_michael, &tkey->key[24], tkey->rx_hdr,
-			skb->data + hdr_len, skb->len - 8 - hdr_len, mic))
-		return -1;
-
-	if (memcmp(mic, skb->data + skb->len - 8, 8) != 0) {
-		struct ieee80211_hdr_4addr *hdr;
-		hdr = (struct ieee80211_hdr_4addr *)skb->data;
-		printk(KERN_DEBUG "%s: Michael MIC verification failed for "
-		       "MSDU from %pM keyidx=%d\n",
-		       skb->dev ? skb->dev->name : "N/A", hdr->addr2,
-		       keyidx);
-		if (skb->dev)
-			ieee80211_michael_mic_failure(skb->dev, hdr, keyidx);
-		tkey->dot11RSNAStatsTKIPLocalMICFailures++;
-		return -1;
-	}
-
-	/* Update TSC counters for RX now that the packet verification has
-	 * completed. */
-	tkey->rx_iv32 = tkey->rx_iv32_new;
-	tkey->rx_iv16 = tkey->rx_iv16_new;
-
-	skb_trim(skb, skb->len - 8);
-
-	return 0;
-}
-
-
-static int ieee80211_tkip_set_key(void *key, int len, u8 *seq, void *priv)
-{
-	struct ieee80211_tkip_data *tkey = priv;
-	int keyidx;
-	struct crypto_hash *tfm = tkey->tx_tfm_michael;
-	struct crypto_blkcipher *tfm2 = tkey->tx_tfm_arc4;
-	struct crypto_hash *tfm3 = tkey->rx_tfm_michael;
-	struct crypto_blkcipher *tfm4 = tkey->rx_tfm_arc4;
-
-	keyidx = tkey->key_idx;
-	memset(tkey, 0, sizeof(*tkey));
-	tkey->key_idx = keyidx;
-
-	tkey->tx_tfm_michael = tfm;
-	tkey->tx_tfm_arc4 = tfm2;
-	tkey->rx_tfm_michael = tfm3;
-	tkey->rx_tfm_arc4 = tfm4;
-
-	if (len == TKIP_KEY_LEN) {
-		memcpy(tkey->key, key, TKIP_KEY_LEN);
-		tkey->key_set = 1;
-		tkey->tx_iv16 = 1; /* TSC is initialized to 1 */
-		if (seq) {
-			tkey->rx_iv32 = (seq[5] << 24) | (seq[4] << 16) |
-				(seq[3] << 8) | seq[2];
-			tkey->rx_iv16 = (seq[1] << 8) | seq[0];
-		}
-	} else if (len == 0)
-		tkey->key_set = 0;
-	else
-		return -1;
-
-	return 0;
-}
-
-
-static int ieee80211_tkip_get_key(void *key, int len, u8 *seq, void *priv)
-{
-	struct ieee80211_tkip_data *tkey = priv;
-
-	if (len < TKIP_KEY_LEN)
-		return -1;
-
-	if (!tkey->key_set)
-		return 0;
-	memcpy(key, tkey->key, TKIP_KEY_LEN);
-
-	if (seq) {
-		/* Return the sequence number of the last transmitted frame. */
-		u16 iv16 = tkey->tx_iv16;
-		u32 iv32 = tkey->tx_iv32;
-		if (iv16 == 0)
-			iv32--;
-		iv16--;
-		seq[0] = tkey->tx_iv16;
-		seq[1] = tkey->tx_iv16 >> 8;
-		seq[2] = tkey->tx_iv32;
-		seq[3] = tkey->tx_iv32 >> 8;
-		seq[4] = tkey->tx_iv32 >> 16;
-		seq[5] = tkey->tx_iv32 >> 24;
-	}
-
-	return TKIP_KEY_LEN;
-}
-
-
-static char *ieee80211_tkip_print_stats(char *p, void *priv)
-{
-	struct ieee80211_tkip_data *tkip = priv;
-	p += sprintf(p, "key[%d] alg=TKIP key_set=%d "
-		     "tx_pn=%02x%02x%02x%02x%02x%02x "
-		     "rx_pn=%02x%02x%02x%02x%02x%02x "
-		     "replays=%d icv_errors=%d local_mic_failures=%d\n",
-		     tkip->key_idx, tkip->key_set,
-		     (tkip->tx_iv32 >> 24) & 0xff,
-		     (tkip->tx_iv32 >> 16) & 0xff,
-		     (tkip->tx_iv32 >> 8) & 0xff,
-		     tkip->tx_iv32 & 0xff,
-		     (tkip->tx_iv16 >> 8) & 0xff,
-		     tkip->tx_iv16 & 0xff,
-		     (tkip->rx_iv32 >> 24) & 0xff,
-		     (tkip->rx_iv32 >> 16) & 0xff,
-		     (tkip->rx_iv32 >> 8) & 0xff,
-		     tkip->rx_iv32 & 0xff,
-		     (tkip->rx_iv16 >> 8) & 0xff,
-		     tkip->rx_iv16 & 0xff,
-		     tkip->dot11RSNAStatsTKIPReplays,
-		     tkip->dot11RSNAStatsTKIPICVErrors,
-		     tkip->dot11RSNAStatsTKIPLocalMICFailures);
-	return p;
-}
-
-
-static struct ieee80211_crypto_ops ieee80211_crypt_tkip = {
-	.name			= "TKIP",
-	.init			= ieee80211_tkip_init,
-	.deinit			= ieee80211_tkip_deinit,
-	.encrypt_mpdu		= ieee80211_tkip_encrypt,
-	.decrypt_mpdu		= ieee80211_tkip_decrypt,
-	.encrypt_msdu		= ieee80211_michael_mic_add,
-	.decrypt_msdu		= ieee80211_michael_mic_verify,
-	.set_key		= ieee80211_tkip_set_key,
-	.get_key		= ieee80211_tkip_get_key,
-	.print_stats		= ieee80211_tkip_print_stats,
-	.extra_prefix_len	= 4 + 4, /* IV + ExtIV */
-	.extra_postfix_len	= 8 + 4, /* MIC + ICV */
-	.owner		        = THIS_MODULE,
-};
-
-
-int ieee80211_crypto_tkip_init(void)
-{
-	return ieee80211_register_crypto_ops(&ieee80211_crypt_tkip);
-}
-
-
-void ieee80211_crypto_tkip_exit(void)
-{
-	ieee80211_unregister_crypto_ops(&ieee80211_crypt_tkip);
-}
-
-
-void ieee80211_tkip_null(void)
-{
-}
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt_wep.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt_wep.c
deleted file mode 100644
index f2536722494..00000000000
--- a/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt_wep.c
+++ /dev/null
@@ -1,277 +0,0 @@
-/*
- * Host AP crypt: host-based WEP encryption implementation for Host AP driver
- *
- * Copyright (c) 2002-2004, Jouni Malinen <jkmaline@cc.hut.fi>
- *
- * 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. See README and COPYING for
- * more details.
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/module.h>
-#include <linux/slab.h>
-#include <linux/random.h>
-#include <linux/skbuff.h>
-#include <linux/string.h>
-
-#include "ieee80211.h"
-
-#include <linux/crypto.h>
-#include <linux/scatterlist.h>
-#include <linux/crc32.h>
-
-MODULE_AUTHOR("Jouni Malinen");
-MODULE_DESCRIPTION("Host AP crypt: WEP");
-MODULE_LICENSE("GPL");
-
-struct prism2_wep_data {
-	u32 iv;
-#define WEP_KEY_LEN 13
-	u8 key[WEP_KEY_LEN + 1];
-	u8 key_len;
-	u8 key_idx;
-	struct crypto_blkcipher *tx_tfm;
-	struct crypto_blkcipher *rx_tfm;
-};
-
-static void *prism2_wep_init(int keyidx)
-{
-	struct prism2_wep_data *priv;
-
-	priv = kzalloc(sizeof(*priv), GFP_ATOMIC);
-	if (priv == NULL)
-		goto fail;
-	priv->key_idx = keyidx;
-	priv->tx_tfm = crypto_alloc_blkcipher("ecb(arc4)", 0, CRYPTO_ALG_ASYNC);
-	if (IS_ERR(priv->tx_tfm)) {
-		pr_debug("could not allocate crypto API arc4\n");
-		priv->tx_tfm = NULL;
-		goto fail;
-	}
-	priv->rx_tfm = crypto_alloc_blkcipher("ecb(arc4)", 0, CRYPTO_ALG_ASYNC);
-	if (IS_ERR(priv->rx_tfm)) {
-		pr_debug("could not allocate crypto API arc4\n");
-		priv->rx_tfm = NULL;
-		goto fail;
-	}
-
-	/* start WEP IV from a random value */
-	get_random_bytes(&priv->iv, 4);
-
-	return priv;
-
-fail:
-	if (priv) {
-		if (priv->tx_tfm)
-			crypto_free_blkcipher(priv->tx_tfm);
-		if (priv->rx_tfm)
-			crypto_free_blkcipher(priv->rx_tfm);
-		kfree(priv);
-	}
-
-	return NULL;
-}
-
-static void prism2_wep_deinit(void *priv)
-{
-	struct prism2_wep_data *_priv = priv;
-
-	if (_priv) {
-		if (_priv->tx_tfm)
-			crypto_free_blkcipher(_priv->tx_tfm);
-		if (_priv->rx_tfm)
-			crypto_free_blkcipher(_priv->rx_tfm);
-	}
-
-	kfree(priv);
-}
-
-/* Perform WEP encryption on given skb that has at least 4 bytes of headroom
- * for IV and 4 bytes of tailroom for ICV. Both IV and ICV will be transmitted,
- * so the payload length increases with 8 bytes.
- *
- * WEP frame payload: IV + TX key idx, RC4(data), ICV = RC4(CRC32(data))
- */
-static int prism2_wep_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
-{
-	struct prism2_wep_data *wep = priv;
-	struct blkcipher_desc desc = { .tfm = wep->tx_tfm };
-	u32 klen, len;
-	u8 key[WEP_KEY_LEN + 3];
-	u8 *pos;
-	u32 crc;
-	u8 *icv;
-	struct scatterlist sg;
-
-	if (skb_headroom(skb) < 4 || skb_tailroom(skb) < 4 ||
-	    skb->len < hdr_len)
-		return -1;
-
-	len = skb->len - hdr_len;
-	pos = skb_push(skb, 4);
-	memmove(pos, pos + 4, hdr_len);
-	pos += hdr_len;
-
-	klen = 3 + wep->key_len;
-
-	wep->iv++;
-
-	/* Fluhrer, Mantin, and Shamir have reported weaknesses in the key
-	 * scheduling algorithm of RC4. At least IVs (KeyByte + 3, 0xff, N)
-	 * can be used to speedup attacks, so avoid using them. */
-	if ((wep->iv & 0xff00) == 0xff00) {
-		u8 B = (wep->iv >> 16) & 0xff;
-		if (B >= 3 && B < klen)
-			wep->iv += 0x0100;
-	}
-
-	/* Prepend 24-bit IV to RC4 key and TX frame */
-	*pos++ = key[0] = (wep->iv >> 16) & 0xff;
-	*pos++ = key[1] = (wep->iv >> 8) & 0xff;
-	*pos++ = key[2] = wep->iv & 0xff;
-	*pos++ = wep->key_idx << 6;
-
-	/* Copy rest of the WEP key (the secret part) */
-	memcpy(key + 3, wep->key, wep->key_len);
-
-	/* Append little-endian CRC32 and encrypt it to produce ICV */
-	crc = ~crc32_le(~0, pos, len);
-	icv = skb_put(skb, 4);
-	icv[0] = crc;
-	icv[1] = crc >> 8;
-	icv[2] = crc >> 16;
-	icv[3] = crc >> 24;
-
-	crypto_blkcipher_setkey(wep->tx_tfm, key, klen);
-	sg_init_one(&sg, pos, len + 4);
-
-	return crypto_blkcipher_encrypt(&desc, &sg, &sg, len + 4);
-}
-
-/* Perform WEP decryption on given buffer. Buffer includes whole WEP part of
- * the frame: IV (4 bytes), encrypted payload (including SNAP header),
- * ICV (4 bytes). len includes both IV and ICV.
- *
- * Returns 0 if frame was decrypted successfully and ICV was correct and -1 on
- * failure. If frame is OK, IV and ICV will be removed.
- */
-static int prism2_wep_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
-{
-	struct prism2_wep_data *wep = priv;
-	struct blkcipher_desc desc = { .tfm = wep->rx_tfm };
-	u32 klen, plen;
-	u8 key[WEP_KEY_LEN + 3];
-	u8 keyidx, *pos;
-	u32 crc;
-	u8 icv[4];
-	struct scatterlist sg;
-
-	if (skb->len < hdr_len + 8)
-		return -1;
-
-	pos = skb->data + hdr_len;
-	key[0] = *pos++;
-	key[1] = *pos++;
-	key[2] = *pos++;
-	keyidx = *pos++ >> 6;
-	if (keyidx != wep->key_idx)
-		return -1;
-
-	klen = 3 + wep->key_len;
-
-	/* Copy rest of the WEP key (the secret part) */
-	memcpy(key + 3, wep->key, wep->key_len);
-
-	/* Apply RC4 to data and compute CRC32 over decrypted data */
-	plen = skb->len - hdr_len - 8;
-
-	crypto_blkcipher_setkey(wep->rx_tfm, key, klen);
-	sg_init_one(&sg, pos, plen + 4);
-
-	if (crypto_blkcipher_decrypt(&desc, &sg, &sg, plen + 4))
-		return -7;
-
-	crc = ~crc32_le(~0, pos, plen);
-	icv[0] = crc;
-	icv[1] = crc >> 8;
-	icv[2] = crc >> 16;
-	icv[3] = crc >> 24;
-
-	if (memcmp(icv, pos + plen, 4) != 0) {
-		/* ICV mismatch - drop frame */
-		return -2;
-	}
-
-	/* Remove IV and ICV */
-	memmove(skb->data + 4, skb->data, hdr_len);
-	skb_pull(skb, 4);
-	skb_trim(skb, skb->len - 4);
-	return 0;
-}
-
-static int prism2_wep_set_key(void *key, int len, u8 *seq, void *priv)
-{
-	struct prism2_wep_data *wep = priv;
-
-	if (len < 0 || len > WEP_KEY_LEN)
-		return -1;
-
-	memcpy(wep->key, key, len);
-	wep->key_len = len;
-
-	return 0;
-}
-
-static int prism2_wep_get_key(void *key, int len, u8 *seq, void *priv)
-{
-	struct prism2_wep_data *wep = priv;
-
-	if (len < wep->key_len)
-		return -1;
-
-	memcpy(key, wep->key, wep->key_len);
-
-	return wep->key_len;
-}
-
-static char *prism2_wep_print_stats(char *p, void *priv)
-{
-	struct prism2_wep_data *wep = priv;
-	p += sprintf(p, "key[%d] alg=WEP len=%d\n",
-		     wep->key_idx, wep->key_len);
-	return p;
-}
-
-static struct ieee80211_crypto_ops ieee80211_crypt_wep = {
-	.name			= "WEP",
-	.init			= prism2_wep_init,
-	.deinit			= prism2_wep_deinit,
-	.encrypt_mpdu		= prism2_wep_encrypt,
-	.decrypt_mpdu		= prism2_wep_decrypt,
-	.encrypt_msdu		= NULL,
-	.decrypt_msdu		= NULL,
-	.set_key		= prism2_wep_set_key,
-	.get_key		= prism2_wep_get_key,
-	.print_stats		= prism2_wep_print_stats,
-	.extra_prefix_len	= 4, /* IV */
-	.extra_postfix_len	= 4, /* ICV */
-	.owner			= THIS_MODULE,
-};
-
-int ieee80211_crypto_wep_init(void)
-{
-	return ieee80211_register_crypto_ops(&ieee80211_crypt_wep);
-}
-
-void ieee80211_crypto_wep_exit(void)
-{
-	ieee80211_unregister_crypto_ops(&ieee80211_crypt_wep);
-}
-
-void ieee80211_wep_null(void)
-{
-	return;
-}
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_module.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_module.c
deleted file mode 100644
index 07a1fbb6678..00000000000
--- a/drivers/staging/rtl8187se/ieee80211/ieee80211_module.c
+++ /dev/null
@@ -1,203 +0,0 @@
-/*******************************************************************************
-
-  Copyright(c) 2004 Intel Corporation. All rights reserved.
-
-  Portions of this file are based on the WEP enablement code provided by the
-  Host AP project hostap-drivers v0.1.3
-  Copyright (c) 2001-2002, SSH Communications Security Corp and Jouni Malinen
-  <jkmaline@cc.hut.fi>
-  Copyright (c) 2002-2003, Jouni Malinen <jkmaline@cc.hut.fi>
-
-  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.
-
-  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.
-
-  The full GNU General Public License is included in this distribution in the
-  file called LICENSE.
-
-  Contact Information:
-  James P. Ketrenos <ipw2100-admin@linux.intel.com>
-  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
-
-*******************************************************************************/
-
-#include <linux/compiler.h>
-//#include <linux/config.h>
-#include <linux/errno.h>
-#include <linux/if_arp.h>
-#include <linux/in6.h>
-#include <linux/in.h>
-#include <linux/ip.h>
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/netdevice.h>
-#include <linux/pci.h>
-#include <linux/proc_fs.h>
-#include <linux/skbuff.h>
-#include <linux/slab.h>
-#include <linux/tcp.h>
-#include <linux/types.h>
-#include <linux/wireless.h>
-#include <linux/etherdevice.h>
-#include <linux/uaccess.h>
-#include <net/arp.h>
-#include <net/net_namespace.h>
-
-#include "ieee80211.h"
-
-MODULE_DESCRIPTION("802.11 data/management/control stack");
-MODULE_AUTHOR("Copyright (C) 2004 Intel Corporation <jketreno@linux.intel.com>");
-MODULE_LICENSE("GPL");
-
-#define DRV_NAME "ieee80211"
-
-static inline int ieee80211_networks_allocate(struct ieee80211_device *ieee)
-{
-	if (ieee->networks)
-		return 0;
-
-	ieee->networks = kcalloc(
-		MAX_NETWORK_COUNT, sizeof(struct ieee80211_network),
-		GFP_KERNEL);
-	if (!ieee->networks)
-		return -ENOMEM;
-
-	return 0;
-}
-
-static inline void ieee80211_networks_free(struct ieee80211_device *ieee)
-{
-	if (!ieee->networks)
-		return;
-	kfree(ieee->networks);
-	ieee->networks = NULL;
-}
-
-static inline void ieee80211_networks_initialize(struct ieee80211_device *ieee)
-{
-	int i;
-
-	INIT_LIST_HEAD(&ieee->network_free_list);
-	INIT_LIST_HEAD(&ieee->network_list);
-	for (i = 0; i < MAX_NETWORK_COUNT; i++)
-		list_add_tail(&ieee->networks[i].list, &ieee->network_free_list);
-}
-
-
-struct net_device *alloc_ieee80211(int sizeof_priv)
-{
-	struct ieee80211_device *ieee;
-	struct net_device *dev;
-	int i, err;
-
-	IEEE80211_DEBUG_INFO("Initializing...\n");
-
-	dev = alloc_etherdev(sizeof(struct ieee80211_device) + sizeof_priv);
-	if (!dev) {
-		IEEE80211_ERROR("Unable to network device.\n");
-		goto failed;
-	}
-	ieee = netdev_priv(dev);
-
-	ieee->dev = dev;
-
-	err = ieee80211_networks_allocate(ieee);
-	if (err) {
-		IEEE80211_ERROR("Unable to allocate beacon storage: %d\n",
-				err);
-		goto failed;
-	}
-	ieee80211_networks_initialize(ieee);
-
-	/* Default fragmentation threshold is maximum payload size */
-	ieee->fts = DEFAULT_FTS;
-	ieee->scan_age = DEFAULT_MAX_SCAN_AGE;
-	ieee->open_wep = 1;
-
-	/* Default to enabling full open WEP with host based encrypt/decrypt */
-	ieee->host_encrypt = 1;
-	ieee->host_decrypt = 1;
-	ieee->ieee802_1x = 1; /* Default to supporting 802.1x */
-
-	INIT_LIST_HEAD(&ieee->crypt_deinit_list);
-	init_timer(&ieee->crypt_deinit_timer);
-	ieee->crypt_deinit_timer.data = (unsigned long)ieee;
-	ieee->crypt_deinit_timer.function = ieee80211_crypt_deinit_handler;
-
-	spin_lock_init(&ieee->lock);
-	spin_lock_init(&ieee->wpax_suitlist_lock);
-
-	ieee->wpax_type_set = 0;
-	ieee->wpa_enabled = 0;
-	ieee->tkip_countermeasures = 0;
-	ieee->drop_unencrypted = 0;
-	ieee->privacy_invoked = 0;
-	ieee->ieee802_1x = 1;
-	ieee->raw_tx = 0;
-
-	ieee80211_softmac_init(ieee);
-
-	for (i = 0; i < IEEE_IBSS_MAC_HASH_SIZE; i++)
-		INIT_LIST_HEAD(&ieee->ibss_mac_hash[i]);
-
-	for (i = 0; i < 17; i++) {
-		ieee->last_rxseq_num[i] = -1;
-		ieee->last_rxfrag_num[i] = -1;
-		ieee->last_packet_time[i] = 0;
-	}
-//These function were added to load crypte module autoly
-	ieee80211_tkip_null();
-	ieee80211_wep_null();
-	ieee80211_ccmp_null();
-	return dev;
-
- failed:
-	if (dev)
-		free_netdev(dev);
-	return NULL;
-}
-
-
-void free_ieee80211(struct net_device *dev)
-{
-	struct ieee80211_device *ieee = netdev_priv(dev);
-
-	int i;
-	struct list_head *p, *q;
-
-
-	ieee80211_softmac_free(ieee);
-	del_timer_sync(&ieee->crypt_deinit_timer);
-	ieee80211_crypt_deinit_entries(ieee, 1);
-
-	for (i = 0; i < WEP_KEYS; i++) {
-		struct ieee80211_crypt_data *crypt = ieee->crypt[i];
-		if (crypt) {
-			if (crypt->ops)
-				crypt->ops->deinit(crypt->priv);
-			kfree(crypt);
-			ieee->crypt[i] = NULL;
-		}
-	}
-
-	ieee80211_networks_free(ieee);
-
-	for (i = 0; i < IEEE_IBSS_MAC_HASH_SIZE; i++) {
-		list_for_each_safe(p, q, &ieee->ibss_mac_hash[i]) {
-			kfree(list_entry(p, struct ieee_ibss_seq, list));
-			list_del(p);
-		}
-	}
-
-
-	free_netdev(dev);
-}
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_rx.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_rx.c
deleted file mode 100644
index b522b57a269..00000000000
--- a/drivers/staging/rtl8187se/ieee80211/ieee80211_rx.c
+++ /dev/null
@@ -1,1486 +0,0 @@
-/*
- * Original code based Host AP (software wireless LAN access point) driver
- * for Intersil Prism2/2.5/3 - hostap.o module, common routines
- *
- * Copyright (c) 2001-2002, SSH Communications Security Corp and Jouni Malinen
- * <jkmaline@cc.hut.fi>
- * Copyright (c) 2002-2003, Jouni Malinen <jkmaline@cc.hut.fi>
- * Copyright (c) 2004, Intel Corporation
- *
- * 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. See README and COPYING for
- * more details.
- ******************************************************************************
-
-  Few modifications for Realtek's Wi-Fi drivers by
-  Andrea Merello <andrea.merello@gmail.com>
-
-  A special thanks goes to Realtek for their support !
-
-******************************************************************************/
-
-
-#include <linux/compiler.h>
-//#include <linux/config.h>
-#include <linux/errno.h>
-#include <linux/if_arp.h>
-#include <linux/in6.h>
-#include <linux/in.h>
-#include <linux/ip.h>
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/netdevice.h>
-#include <linux/pci.h>
-#include <linux/proc_fs.h>
-#include <linux/skbuff.h>
-#include <linux/slab.h>
-#include <linux/tcp.h>
-#include <linux/types.h>
-#include <linux/wireless.h>
-#include <linux/etherdevice.h>
-#include <linux/uaccess.h>
-#include <linux/ctype.h>
-
-#include "ieee80211.h"
-#include "dot11d.h"
-static inline void ieee80211_monitor_rx(struct ieee80211_device *ieee,
-					struct sk_buff *skb,
-					struct ieee80211_rx_stats *rx_stats)
-{
-	struct ieee80211_hdr_4addr *hdr =
-		(struct ieee80211_hdr_4addr *)skb->data;
-	u16 fc = le16_to_cpu(hdr->frame_ctl);
-
-	skb->dev = ieee->dev;
-	skb_reset_mac_header(skb);
-	skb_pull(skb, ieee80211_get_hdrlen(fc));
-	skb->pkt_type = PACKET_OTHERHOST;
-	skb->protocol = __constant_htons(ETH_P_80211_RAW);
-	memset(skb->cb, 0, sizeof(skb->cb));
-	netif_rx(skb);
-}
-
-
-/* Called only as a tasklet (software IRQ) */
-static struct ieee80211_frag_entry *
-ieee80211_frag_cache_find(struct ieee80211_device *ieee, unsigned int seq,
-			  unsigned int frag, u8 tid, u8 *src, u8 *dst)
-{
-	struct ieee80211_frag_entry *entry;
-	int i;
-
-	for (i = 0; i < IEEE80211_FRAG_CACHE_LEN; i++) {
-		entry = &ieee->frag_cache[tid][i];
-		if (entry->skb != NULL &&
-		    time_after(jiffies, entry->first_frag_time + 2 * HZ)) {
-			IEEE80211_DEBUG_FRAG(
-				"expiring fragment cache entry "
-				"seq=%u last_frag=%u\n",
-				entry->seq, entry->last_frag);
-			dev_kfree_skb_any(entry->skb);
-			entry->skb = NULL;
-		}
-
-		if (entry->skb != NULL && entry->seq == seq &&
-		    (entry->last_frag + 1 == frag || frag == -1) &&
-		    memcmp(entry->src_addr, src, ETH_ALEN) == 0 &&
-		    memcmp(entry->dst_addr, dst, ETH_ALEN) == 0)
-			return entry;
-	}
-
-	return NULL;
-}
-
-/* Called only as a tasklet (software IRQ) */
-static struct sk_buff *
-ieee80211_frag_cache_get(struct ieee80211_device *ieee,
-			 struct ieee80211_hdr_4addr *hdr)
-{
-	struct sk_buff *skb = NULL;
-	u16 fc = le16_to_cpu(hdr->frame_ctl);
-	u16 sc = le16_to_cpu(hdr->seq_ctl);
-	unsigned int frag = WLAN_GET_SEQ_FRAG(sc);
-	unsigned int seq = WLAN_GET_SEQ_SEQ(sc);
-	struct ieee80211_frag_entry *entry;
-	struct ieee80211_hdr_3addrqos *hdr_3addrqos;
-	struct ieee80211_hdr_4addrqos *hdr_4addrqos;
-	u8 tid;
-
-	if (((fc & IEEE80211_FCTL_DSTODS) == IEEE80211_FCTL_DSTODS) && IEEE80211_QOS_HAS_SEQ(fc)) {
-		hdr_4addrqos = (struct ieee80211_hdr_4addrqos *)hdr;
-		tid = le16_to_cpu(hdr_4addrqos->qos_ctl) & IEEE80211_QOS_TID;
-		tid = UP2AC(tid);
-		tid++;
-	} else if (IEEE80211_QOS_HAS_SEQ(fc)) {
-		hdr_3addrqos = (struct ieee80211_hdr_3addrqos *)hdr;
-		tid = le16_to_cpu(hdr_3addrqos->qos_ctl) & IEEE80211_QOS_TID;
-		tid = UP2AC(tid);
-		tid++;
-	} else {
-		tid = 0;
-	}
-
-	if (frag == 0) {
-		/* Reserve enough space to fit maximum frame length */
-		skb = dev_alloc_skb(ieee->dev->mtu +
-				    sizeof(struct ieee80211_hdr_4addr) +
-				    8 /* LLC */ +
-				    2 /* alignment */ +
-				    8 /* WEP */ +
-				    ETH_ALEN /* WDS */ +
-				    (IEEE80211_QOS_HAS_SEQ(fc) ? 2 : 0) /* QOS Control */);
-		if (skb == NULL)
-			return NULL;
-
-		entry = &ieee->frag_cache[tid][ieee->frag_next_idx[tid]];
-		ieee->frag_next_idx[tid]++;
-		if (ieee->frag_next_idx[tid] >= IEEE80211_FRAG_CACHE_LEN)
-			ieee->frag_next_idx[tid] = 0;
-
-		if (entry->skb != NULL)
-			dev_kfree_skb_any(entry->skb);
-
-		entry->first_frag_time = jiffies;
-		entry->seq = seq;
-		entry->last_frag = frag;
-		entry->skb = skb;
-		memcpy(entry->src_addr, hdr->addr2, ETH_ALEN);
-		memcpy(entry->dst_addr, hdr->addr1, ETH_ALEN);
-	} else {
-		/* received a fragment of a frame for which the head fragment
-		 * should have already been received */
-		entry = ieee80211_frag_cache_find(ieee, seq, frag, tid, hdr->addr2,
-						  hdr->addr1);
-		if (entry != NULL) {
-			entry->last_frag = frag;
-			skb = entry->skb;
-		}
-	}
-
-	return skb;
-}
-
-
-/* Called only as a tasklet (software IRQ) */
-static int ieee80211_frag_cache_invalidate(struct ieee80211_device *ieee,
-					   struct ieee80211_hdr_4addr *hdr)
-{
-	u16 fc = le16_to_cpu(hdr->frame_ctl);
-	u16 sc = le16_to_cpu(hdr->seq_ctl);
-	unsigned int seq = WLAN_GET_SEQ_SEQ(sc);
-	struct ieee80211_frag_entry *entry;
-	struct ieee80211_hdr_3addrqos *hdr_3addrqos;
-	struct ieee80211_hdr_4addrqos *hdr_4addrqos;
-	u8 tid;
-
-	if (((fc & IEEE80211_FCTL_DSTODS) == IEEE80211_FCTL_DSTODS) && IEEE80211_QOS_HAS_SEQ(fc)) {
-		hdr_4addrqos = (struct ieee80211_hdr_4addrqos *)hdr;
-		tid = le16_to_cpu(hdr_4addrqos->qos_ctl) & IEEE80211_QOS_TID;
-		tid = UP2AC(tid);
-		tid++;
-	} else if (IEEE80211_QOS_HAS_SEQ(fc)) {
-		hdr_3addrqos = (struct ieee80211_hdr_3addrqos *)hdr;
-		tid = le16_to_cpu(hdr_3addrqos->qos_ctl) & IEEE80211_QOS_TID;
-		tid = UP2AC(tid);
-		tid++;
-	} else {
-		tid = 0;
-	}
-
-	entry = ieee80211_frag_cache_find(ieee, seq, -1, tid, hdr->addr2,
-					  hdr->addr1);
-
-	if (entry == NULL) {
-		IEEE80211_DEBUG_FRAG(
-			"could not invalidate fragment cache "
-			"entry (seq=%u)\n", seq);
-		return -1;
-	}
-
-	entry->skb = NULL;
-	return 0;
-}
-
-
-
-/* ieee80211_rx_frame_mgtmt
- *
- * Responsible for handling management control frames
- *
- * Called by ieee80211_rx */
-static inline int
-ieee80211_rx_frame_mgmt(struct ieee80211_device *ieee, struct sk_buff *skb,
-			struct ieee80211_rx_stats *rx_stats, u16 type,
-			u16 stype)
-{
-	struct ieee80211_hdr_4addr *hdr;
-
-	// cheat the the hdr type
-	hdr = (struct ieee80211_hdr_4addr *)skb->data;
-
-	/* On the struct stats definition there is written that
-	 * this is not mandatory.... but seems that the probe
-	 * response parser uses it
-	 */
-	rx_stats->len = skb->len;
-	ieee80211_rx_mgt(ieee, (struct ieee80211_hdr_4addr *)skb->data,
-			 rx_stats);
-
-	if ((ieee->state == IEEE80211_LINKED) && (memcmp(hdr->addr3, ieee->current_network.bssid, ETH_ALEN))) {
-		dev_kfree_skb_any(skb);
-		return 0;
-	}
-
-	ieee80211_rx_frame_softmac(ieee, skb, rx_stats, type, stype);
-
-	dev_kfree_skb_any(skb);
-
-	return 0;
-
-}
-
-
-
-/* See IEEE 802.1H for LLC/SNAP encapsulation/decapsulation */
-/* Ethernet-II snap header (RFC1042 for most EtherTypes) */
-static unsigned char rfc1042_header[] = { 0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00 };
-/* Bridge-Tunnel header (for EtherTypes ETH_P_AARP and ETH_P_IPX) */
-static unsigned char bridge_tunnel_header[] = { 0xaa, 0xaa, 0x03, 0x00, 0x00, 0xf8 };
-/* No encapsulation header if EtherType < 0x600 (=length) */
-
-/* Called by ieee80211_rx_frame_decrypt */
-static int ieee80211_is_eapol_frame(struct ieee80211_device *ieee,
-				    struct sk_buff *skb, size_t hdrlen)
-{
-	struct net_device *dev = ieee->dev;
-	u16 fc, ethertype;
-	struct ieee80211_hdr_4addr *hdr;
-	u8 *pos;
-
-	if (skb->len < 24)
-		return 0;
-
-	hdr = (struct ieee80211_hdr_4addr *)skb->data;
-	fc = le16_to_cpu(hdr->frame_ctl);
-
-	/* check that the frame is unicast frame to us */
-	if ((fc & (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) ==
-	    IEEE80211_FCTL_TODS &&
-	    memcmp(hdr->addr1, dev->dev_addr, ETH_ALEN) == 0 &&
-	    memcmp(hdr->addr3, dev->dev_addr, ETH_ALEN) == 0) {
-		/* ToDS frame with own addr BSSID and DA */
-	} else if ((fc & (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) ==
-		   IEEE80211_FCTL_FROMDS &&
-		   memcmp(hdr->addr1, dev->dev_addr, ETH_ALEN) == 0) {
-		/* FromDS frame with own addr as DA */
-	} else
-		return 0;
-
-	if (skb->len < 24 + 8)
-		return 0;
-
-	/* check for port access entity Ethernet type */
-//	pos = skb->data + 24;
-	pos = skb->data + hdrlen;
-	ethertype = (pos[6] << 8) | pos[7];
-	if (ethertype == ETH_P_PAE)
-		return 1;
-
-	return 0;
-}
-
-/* Called only as a tasklet (software IRQ), by ieee80211_rx */
-static inline int
-ieee80211_rx_frame_decrypt(struct ieee80211_device *ieee, struct sk_buff *skb,
-			   struct ieee80211_crypt_data *crypt)
-{
-	struct ieee80211_hdr_4addr *hdr;
-	int res, hdrlen;
-
-	if (crypt == NULL || crypt->ops->decrypt_mpdu == NULL)
-		return 0;
-
-	hdr = (struct ieee80211_hdr_4addr *)skb->data;
-	hdrlen = ieee80211_get_hdrlen(le16_to_cpu(hdr->frame_ctl));
-
-#ifdef CONFIG_IEEE80211_CRYPT_TKIP
-	if (ieee->tkip_countermeasures &&
-	    strcmp(crypt->ops->name, "TKIP") == 0) {
-		if (net_ratelimit()) {
-			netdev_dbg(ieee->dev,
-				   "TKIP countermeasures: dropped received packet from %pM\n",
-				   ieee->dev->name, hdr->addr2);
-		}
-		return -1;
-	}
-#endif
-
-	atomic_inc(&crypt->refcnt);
-	res = crypt->ops->decrypt_mpdu(skb, hdrlen, crypt->priv);
-	atomic_dec(&crypt->refcnt);
-	if (res < 0) {
-		IEEE80211_DEBUG_DROP(
-			"decryption failed (SA=%pM"
-			") res=%d\n", hdr->addr2, res);
-		if (res == -2)
-			IEEE80211_DEBUG_DROP("Decryption failed ICV "
-					     "mismatch (key %d)\n",
-					     skb->data[hdrlen + 3] >> 6);
-		ieee->ieee_stats.rx_discards_undecryptable++;
-		return -1;
-	}
-
-	return res;
-}
-
-
-/* Called only as a tasklet (software IRQ), by ieee80211_rx */
-static inline int
-ieee80211_rx_frame_decrypt_msdu(struct ieee80211_device *ieee,
-				struct sk_buff *skb, int keyidx,
-				struct ieee80211_crypt_data *crypt)
-{
-	struct ieee80211_hdr_4addr *hdr;
-	int res, hdrlen;
-
-	if (crypt == NULL || crypt->ops->decrypt_msdu == NULL)
-		return 0;
-
-	hdr = (struct ieee80211_hdr_4addr *)skb->data;
-	hdrlen = ieee80211_get_hdrlen(le16_to_cpu(hdr->frame_ctl));
-
-	atomic_inc(&crypt->refcnt);
-	res = crypt->ops->decrypt_msdu(skb, keyidx, hdrlen, crypt->priv);
-	atomic_dec(&crypt->refcnt);
-	if (res < 0) {
-		netdev_dbg(ieee->dev,
-			   "MSDU decryption/MIC verification failed (SA=%pM keyidx=%d)\n",
-			   hdr->addr2, keyidx);
-		return -1;
-	}
-
-	return 0;
-}
-
-
-/* this function is stolen from ipw2200 driver*/
-#define IEEE_PACKET_RETRY_TIME (5*HZ)
-static int is_duplicate_packet(struct ieee80211_device *ieee,
-			       struct ieee80211_hdr_4addr *header)
-{
-	u16 fc = le16_to_cpu(header->frame_ctl);
-	u16 sc = le16_to_cpu(header->seq_ctl);
-	u16 seq = WLAN_GET_SEQ_SEQ(sc);
-	u16 frag = WLAN_GET_SEQ_FRAG(sc);
-	u16 *last_seq, *last_frag;
-	unsigned long *last_time;
-	struct ieee80211_hdr_3addrqos *hdr_3addrqos;
-	struct ieee80211_hdr_4addrqos *hdr_4addrqos;
-	u8 tid;
-
-	//TO2DS and QoS
-	if (((fc & IEEE80211_FCTL_DSTODS) == IEEE80211_FCTL_DSTODS) && IEEE80211_QOS_HAS_SEQ(fc)) {
-		hdr_4addrqos = (struct ieee80211_hdr_4addrqos *)header;
-		tid = le16_to_cpu(hdr_4addrqos->qos_ctl) & IEEE80211_QOS_TID;
-		tid = UP2AC(tid);
-		tid++;
-	} else if (IEEE80211_QOS_HAS_SEQ(fc)) { //QoS
-		hdr_3addrqos = (struct ieee80211_hdr_3addrqos *)header;
-		tid = le16_to_cpu(hdr_3addrqos->qos_ctl) & IEEE80211_QOS_TID;
-		tid = UP2AC(tid);
-		tid++;
-	} else { // no QoS
-		tid = 0;
-	}
-	switch (ieee->iw_mode) {
-	case IW_MODE_ADHOC:
-	{
-		struct list_head *p;
-		struct ieee_ibss_seq *entry = NULL;
-		u8 *mac = header->addr2;
-		int index = mac[5] % IEEE_IBSS_MAC_HASH_SIZE;
-
-		list_for_each(p, &ieee->ibss_mac_hash[index]) {
-			entry = list_entry(p, struct ieee_ibss_seq, list);
-			if (!memcmp(entry->mac, mac, ETH_ALEN))
-				break;
-		}
-	//	if (memcmp(entry->mac, mac, ETH_ALEN)){
-		if (p == &ieee->ibss_mac_hash[index]) {
-			entry = kmalloc(sizeof(struct ieee_ibss_seq), GFP_ATOMIC);
-			if (!entry)
-				return 0;
-
-			memcpy(entry->mac, mac, ETH_ALEN);
-			entry->seq_num[tid] = seq;
-			entry->frag_num[tid] = frag;
-			entry->packet_time[tid] = jiffies;
-			list_add(&entry->list, &ieee->ibss_mac_hash[index]);
-			return 0;
-		}
-		last_seq = &entry->seq_num[tid];
-		last_frag = &entry->frag_num[tid];
-		last_time = &entry->packet_time[tid];
-		break;
-	}
-
-	case IW_MODE_INFRA:
-		last_seq = &ieee->last_rxseq_num[tid];
-		last_frag = &ieee->last_rxfrag_num[tid];
-		last_time = &ieee->last_packet_time[tid];
-
-		break;
-	default:
-		return 0;
-	}
-
-//	if(tid != 0) {
-//		printk(KERN_WARNING ":)))))))))))%x %x %x, fc(%x)\n", tid, *last_seq, seq, header->frame_ctl);
-//	}
-	if ((*last_seq == seq) &&
-	    time_after(*last_time + IEEE_PACKET_RETRY_TIME, jiffies)) {
-		if (*last_frag == frag) {
-			//printk(KERN_WARNING "[1] go drop!\n");
-			goto drop;
-
-		}
-		if (*last_frag + 1 != frag)
-			/* out-of-order fragment */
-			//printk(KERN_WARNING "[2] go drop!\n");
-			goto drop;
-	} else
-		*last_seq = seq;
-
-	*last_frag = frag;
-	*last_time = jiffies;
-	return 0;
-
-drop:
-//	BUG_ON(!(fc & IEEE80211_FCTL_RETRY));
-//	printk("DUP\n");
-
-	return 1;
-}
-
-
-/* All received frames are sent to this function. @skb contains the frame in
- * IEEE 802.11 format, i.e., in the format it was sent over air.
- * This function is called only as a tasklet (software IRQ). */
-int ieee80211_rtl_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
-		     struct ieee80211_rx_stats *rx_stats)
-{
-	struct net_device *dev = ieee->dev;
-	//struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	struct ieee80211_hdr_4addr *hdr;
-
-	size_t hdrlen;
-	u16 fc, type, stype, sc;
-	struct net_device_stats *stats;
-	unsigned int frag;
-	u8 *payload;
-	u16 ethertype;
-	u8 dst[ETH_ALEN];
-	u8 src[ETH_ALEN];
-	u8 bssid[ETH_ALEN];
-	struct ieee80211_crypt_data *crypt = NULL;
-	int keyidx = 0;
-
-	// cheat the the hdr type
-	hdr = (struct ieee80211_hdr_4addr *)skb->data;
-	stats = &ieee->stats;
-
-	if (skb->len < 10) {
-		netdev_info(ieee->dev, "SKB length < 10\n");
-		goto rx_dropped;
-	}
-
-	fc = le16_to_cpu(hdr->frame_ctl);
-	type = WLAN_FC_GET_TYPE(fc);
-	stype = WLAN_FC_GET_STYPE(fc);
-	sc = le16_to_cpu(hdr->seq_ctl);
-
-	frag = WLAN_GET_SEQ_FRAG(sc);
-
-//YJ,add,080828,for keep alive
-	if ((fc & IEEE80211_FCTL_TODS) != IEEE80211_FCTL_TODS) {
-		if (!memcmp(hdr->addr1, dev->dev_addr, ETH_ALEN))
-			ieee->NumRxUnicast++;
-	} else {
-		if (!memcmp(hdr->addr3, dev->dev_addr, ETH_ALEN))
-			ieee->NumRxUnicast++;
-	}
-//YJ,add,080828,for keep alive,end
-
-	hdrlen = ieee80211_get_hdrlen(fc);
-
-
-	if (ieee->iw_mode == IW_MODE_MONITOR) {
-		ieee80211_monitor_rx(ieee, skb, rx_stats);
-		stats->rx_packets++;
-		stats->rx_bytes += skb->len;
-		return 1;
-	}
-
-	if (ieee->host_decrypt) {
-		int idx = 0;
-		if (skb->len >= hdrlen + 3)
-			idx = skb->data[hdrlen + 3] >> 6;
-		crypt = ieee->crypt[idx];
-
-		/* allow NULL decrypt to indicate an station specific override
-		 * for default encryption */
-		if (crypt && (crypt->ops == NULL ||
-			      crypt->ops->decrypt_mpdu == NULL))
-			crypt = NULL;
-
-		if (!crypt && (fc & IEEE80211_FCTL_WEP)) {
-			/* This seems to be triggered by some (multicast?)
-			 * frames from other than current BSS, so just drop the
-			 * frames silently instead of filling system log with
-			 * these reports. */
-			IEEE80211_DEBUG_DROP("Decryption failed (not set)"
-					     " (SA=%pM)\n",
-					     hdr->addr2);
-			ieee->ieee_stats.rx_discards_undecryptable++;
-			goto rx_dropped;
-		}
-	}
-
-	if (skb->len < IEEE80211_DATA_HDR3_LEN)
-		goto rx_dropped;
-
-	// if QoS enabled, should check the sequence for each of the AC
-	if (is_duplicate_packet(ieee, hdr))
-		goto rx_dropped;
-
-
-	if (type == IEEE80211_FTYPE_MGMT) {
-		if (ieee80211_rx_frame_mgmt(ieee, skb, rx_stats, type, stype))
-			goto rx_dropped;
-		else
-			goto rx_exit;
-	}
-
-	/* Data frame - extract src/dst addresses */
-	switch (fc & (IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS)) {
-	case IEEE80211_FCTL_FROMDS:
-		memcpy(dst, hdr->addr1, ETH_ALEN);
-		memcpy(src, hdr->addr3, ETH_ALEN);
-		memcpy(bssid, hdr->addr2, ETH_ALEN);
-		break;
-	case IEEE80211_FCTL_TODS:
-		memcpy(dst, hdr->addr3, ETH_ALEN);
-		memcpy(src, hdr->addr2, ETH_ALEN);
-		memcpy(bssid, hdr->addr1, ETH_ALEN);
-		break;
-	case IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS:
-		if (skb->len < IEEE80211_DATA_HDR4_LEN)
-			goto rx_dropped;
-		memcpy(dst, hdr->addr3, ETH_ALEN);
-		memcpy(src, hdr->addr4, ETH_ALEN);
-		memcpy(bssid, ieee->current_network.bssid, ETH_ALEN);
-		break;
-	case 0:
-		memcpy(dst, hdr->addr1, ETH_ALEN);
-		memcpy(src, hdr->addr2, ETH_ALEN);
-		memcpy(bssid, hdr->addr3, ETH_ALEN);
-		break;
-	}
-
-
-	dev->last_rx = jiffies;
-
-
-	/* Nullfunc frames may have PS-bit set, so they must be passed to
-	 * hostap_handle_sta_rx() before being dropped here. */
-	if (stype != IEEE80211_STYPE_DATA &&
-	    stype != IEEE80211_STYPE_DATA_CFACK &&
-	    stype != IEEE80211_STYPE_DATA_CFPOLL &&
-	    stype != IEEE80211_STYPE_DATA_CFACKPOLL &&
-	    stype != IEEE80211_STYPE_QOS_DATA//add by David,2006.8.4
-	    ) {
-		if (stype != IEEE80211_STYPE_NULLFUNC)
-			IEEE80211_DEBUG_DROP(
-				"RX: dropped data frame "
-				"with no data (type=0x%02x, "
-				"subtype=0x%02x, len=%d)\n",
-				type, stype, skb->len);
-		goto rx_dropped;
-	}
-	if (memcmp(bssid, ieee->current_network.bssid, ETH_ALEN))
-		goto rx_dropped;
-
-	ieee->NumRxDataInPeriod++;
-	ieee->NumRxOkTotal++;
-	/* skb: hdr + (possibly fragmented, possibly encrypted) payload */
-
-	if (ieee->host_decrypt && (fc & IEEE80211_FCTL_WEP) &&
-	    (keyidx = ieee80211_rx_frame_decrypt(ieee, skb, crypt)) < 0)
-		goto rx_dropped;
-
-	hdr = (struct ieee80211_hdr_4addr *)skb->data;
-
-	/* skb: hdr + (possibly fragmented) plaintext payload */
-	// PR: FIXME: hostap has additional conditions in the "if" below:
-	// ieee->host_decrypt && (fc & IEEE80211_FCTL_WEP) &&
-	if ((frag != 0 || (fc & IEEE80211_FCTL_MOREFRAGS))) {
-		int flen;
-		struct sk_buff *frag_skb = ieee80211_frag_cache_get(ieee, hdr);
-		IEEE80211_DEBUG_FRAG("Rx Fragment received (%u)\n", frag);
-
-		if (!frag_skb) {
-			IEEE80211_DEBUG(IEEE80211_DL_RX | IEEE80211_DL_FRAG,
-					"Rx cannot get skb from fragment "
-					"cache (morefrag=%d seq=%u frag=%u)\n",
-					(fc & IEEE80211_FCTL_MOREFRAGS) != 0,
-					WLAN_GET_SEQ_SEQ(sc), frag);
-			goto rx_dropped;
-		}
-		flen = skb->len;
-		if (frag != 0)
-			flen -= hdrlen;
-
-		if (frag_skb->tail + flen > frag_skb->end) {
-			netdev_warn(ieee->dev,
-				    "host decrypted and reassembled frame did not fit skb\n");
-			ieee80211_frag_cache_invalidate(ieee, hdr);
-			goto rx_dropped;
-		}
-
-		if (frag == 0) {
-			/* copy first fragment (including full headers) into
-			 * beginning of the fragment cache skb */
-			memcpy(skb_put(frag_skb, flen), skb->data, flen);
-		} else {
-			/* append frame payload to the end of the fragment
-			 * cache skb */
-			memcpy(skb_put(frag_skb, flen), skb->data + hdrlen,
-			       flen);
-		}
-		dev_kfree_skb_any(skb);
-		skb = NULL;
-
-		if (fc & IEEE80211_FCTL_MOREFRAGS) {
-			/* more fragments expected - leave the skb in fragment
-			 * cache for now; it will be delivered to upper layers
-			 * after all fragments have been received */
-			goto rx_exit;
-		}
-
-		/* this was the last fragment and the frame will be
-		 * delivered, so remove skb from fragment cache */
-		skb = frag_skb;
-		hdr = (struct ieee80211_hdr_4addr *)skb->data;
-		ieee80211_frag_cache_invalidate(ieee, hdr);
-	}
-
-	/* skb: hdr + (possible reassembled) full MSDU payload; possibly still
-	 * encrypted/authenticated */
-	if (ieee->host_decrypt && (fc & IEEE80211_FCTL_WEP) &&
-	    ieee80211_rx_frame_decrypt_msdu(ieee, skb, keyidx, crypt))
-		goto rx_dropped;
-
-	hdr = (struct ieee80211_hdr_4addr *)skb->data;
-	if (crypt && !(fc & IEEE80211_FCTL_WEP) && !ieee->open_wep) {
-		if (/*ieee->ieee802_1x &&*/
-		    ieee80211_is_eapol_frame(ieee, skb, hdrlen)) {
-
-#ifdef CONFIG_IEEE80211_DEBUG
-			/* pass unencrypted EAPOL frames even if encryption is
-			 * configured */
-			struct eapol *eap = (struct eapol *)(skb->data +
-				24);
-			IEEE80211_DEBUG_EAP("RX: IEEE 802.1X EAPOL frame: %s\n",
-						eap_get_type(eap->type));
-#endif
-		} else {
-			IEEE80211_DEBUG_DROP(
-				"encryption configured, but RX "
-				"frame not encrypted (SA=%pM)\n",
-				hdr->addr2);
-			goto rx_dropped;
-		}
-	}
-
-#ifdef CONFIG_IEEE80211_DEBUG
-	if (crypt && !(fc & IEEE80211_FCTL_WEP) &&
-	    ieee80211_is_eapol_frame(ieee, skb, hdrlen)) {
-			struct eapol *eap = (struct eapol *)(skb->data +
-				24);
-			IEEE80211_DEBUG_EAP("RX: IEEE 802.1X EAPOL frame: %s\n",
-						eap_get_type(eap->type));
-	}
-#endif
-
-	if (crypt && !(fc & IEEE80211_FCTL_WEP) && !ieee->open_wep &&
-	    !ieee80211_is_eapol_frame(ieee, skb, hdrlen)) {
-		IEEE80211_DEBUG_DROP(
-			"dropped unencrypted RX data "
-			"frame from %pM"
-			" (drop_unencrypted=1)\n",
-			hdr->addr2);
-		goto rx_dropped;
-	}
-/*
-	if(ieee80211_is_eapol_frame(ieee, skb, hdrlen)) {
-		printk(KERN_WARNING "RX: IEEE802.1X EPAOL frame!\n");
-	}
-*/
-	/* skb: hdr + (possible reassembled) full plaintext payload */
-	payload = skb->data + hdrlen;
-	ethertype = (payload[6] << 8) | payload[7];
-
-
-	/* convert hdr + possible LLC headers into Ethernet header */
-	if (skb->len - hdrlen >= 8 &&
-	    ((memcmp(payload, rfc1042_header, SNAP_SIZE) == 0 &&
-	      ethertype != ETH_P_AARP && ethertype != ETH_P_IPX) ||
-	     memcmp(payload, bridge_tunnel_header, SNAP_SIZE) == 0)) {
-		/* remove RFC1042 or Bridge-Tunnel encapsulation and
-		 * replace EtherType */
-		skb_pull(skb, hdrlen + SNAP_SIZE);
-		memcpy(skb_push(skb, ETH_ALEN), src, ETH_ALEN);
-		memcpy(skb_push(skb, ETH_ALEN), dst, ETH_ALEN);
-	} else {
-		u16 len;
-		/* Leave Ethernet header part of hdr and full payload */
-		skb_pull(skb, hdrlen);
-		len = htons(skb->len);
-		memcpy(skb_push(skb, 2), &len, 2);
-		memcpy(skb_push(skb, ETH_ALEN), src, ETH_ALEN);
-		memcpy(skb_push(skb, ETH_ALEN), dst, ETH_ALEN);
-	}
-
-
-	stats->rx_packets++;
-	stats->rx_bytes += skb->len;
-
-	if (skb) {
-		skb->protocol = eth_type_trans(skb, dev);
-		memset(skb->cb, 0, sizeof(skb->cb));
-		skb->dev = dev;
-		skb->ip_summed = CHECKSUM_NONE; /* 802.11 crc not sufficient */
-		ieee->last_rx_ps_time = jiffies;
-		netif_rx(skb);
-	}
-
- rx_exit:
-	return 1;
-
- rx_dropped:
-	stats->rx_dropped++;
-
-	/* Returning 0 indicates to caller that we have not handled the SKB--
-	 * so it is still allocated and can be used again by underlying
-	 * hardware as a DMA target */
-	return 0;
-}
-
-#define MGMT_FRAME_FIXED_PART_LENGTH		0x24
-
-static inline int ieee80211_is_ofdm_rate(u8 rate)
-{
-	switch (rate & ~IEEE80211_BASIC_RATE_MASK) {
-	case IEEE80211_OFDM_RATE_6MB:
-	case IEEE80211_OFDM_RATE_9MB:
-	case IEEE80211_OFDM_RATE_12MB:
-	case IEEE80211_OFDM_RATE_18MB:
-	case IEEE80211_OFDM_RATE_24MB:
-	case IEEE80211_OFDM_RATE_36MB:
-	case IEEE80211_OFDM_RATE_48MB:
-	case IEEE80211_OFDM_RATE_54MB:
-		return 1;
-	}
-	return 0;
-}
-
-static inline int ieee80211_SignalStrengthTranslate(int CurrSS)
-{
-	int RetSS;
-
-	// Step 1. Scale mapping.
-	if (CurrSS >= 71 && CurrSS <= 100)
-		RetSS = 90 + ((CurrSS - 70) / 3);
-	else if (CurrSS >= 41 && CurrSS <= 70)
-		RetSS = 78 + ((CurrSS - 40) / 3);
-	else if (CurrSS >= 31 && CurrSS <= 40)
-		RetSS = 66 + (CurrSS - 30);
-	else if (CurrSS >= 21 && CurrSS <= 30)
-		RetSS = 54 + (CurrSS - 20);
-	else if (CurrSS >= 5 && CurrSS <= 20)
-		RetSS = 42 + (((CurrSS - 5) * 2) / 3);
-	else if (CurrSS == 4)
-		RetSS = 36;
-	else if (CurrSS == 3)
-		RetSS = 27;
-	else if (CurrSS == 2)
-		RetSS = 18;
-	else if (CurrSS == 1)
-		RetSS = 9;
-	else
-		RetSS = CurrSS;
-
-	//RT_TRACE(COMP_DBG, DBG_LOUD, ("##### After Mapping:  LastSS: %d, CurrSS: %d, RetSS: %d\n", LastSS, CurrSS, RetSS));
-
-	// Step 2. Smoothing.
-
-	//RT_TRACE(COMP_DBG, DBG_LOUD, ("$$$$$ After Smoothing:  LastSS: %d, CurrSS: %d, RetSS: %d\n", LastSS, CurrSS, RetSS));
-
-	return RetSS;
-}
-
-static inline void
-ieee80211_extract_country_ie(struct ieee80211_device *ieee,
-			     struct ieee80211_info_element *info_element,
-			     struct ieee80211_network *network, u8 *addr2)
-{
-	if (IS_DOT11D_ENABLE(ieee)) {
-		if (info_element->len != 0) {
-			memcpy(network->CountryIeBuf, info_element->data, info_element->len);
-			network->CountryIeLen = info_element->len;
-
-			if (!IS_COUNTRY_IE_VALID(ieee))
-				Dot11d_UpdateCountryIe(ieee, addr2, info_element->len, info_element->data);
-		}
-
-		//
-		// 070305, rcnjko: I update country IE watch dog here because
-		// some AP (e.g. Cisco 1242) don't include country IE in their
-		// probe response frame.
-		//
-		if (IS_EQUAL_CIE_SRC(ieee, addr2))
-			UPDATE_CIE_WATCHDOG(ieee);
-	}
-
-}
-
-/* SignalStrengthIndex is 0-100 */
-static int ieee80211_TranslateToDbm(unsigned char SignalStrengthIndex)
-{
-	unsigned char SignalPower; // in dBm.
-
-	// Translate to dBm (x=0.5y-95).
-	SignalPower = (int)SignalStrengthIndex * 7 / 10;
-	SignalPower -= 95;
-
-	return SignalPower;
-}
-inline int ieee80211_network_init(
-	struct ieee80211_device *ieee,
-	struct ieee80211_probe_response *beacon,
-	struct ieee80211_network *network,
-	struct ieee80211_rx_stats *stats)
-{
-#ifdef CONFIG_IEEE80211_DEBUG
-	char rates_str[64];
-	char *p;
-#endif
-	struct ieee80211_info_element *info_element;
-	u16 left;
-	u8 i;
-	short offset;
-	u8 curRate = 0, hOpRate = 0, curRate_ex = 0;
-
-	/* Pull out fixed field data */
-	memcpy(network->bssid, beacon->header.addr3, ETH_ALEN);
-	network->capability = beacon->capability;
-	network->last_scanned = jiffies;
-	network->time_stamp[0] = beacon->time_stamp[0];
-	network->time_stamp[1] = beacon->time_stamp[1];
-	network->beacon_interval = beacon->beacon_interval;
-	/* Where to pull this? beacon->listen_interval;*/
-	network->listen_interval = 0x0A;
-	network->rates_len = network->rates_ex_len = 0;
-	network->last_associate = 0;
-	network->ssid_len = 0;
-	network->flags = 0;
-	network->atim_window = 0;
-	network->QoS_Enable = 0;
-//by amy 080312
-	network->HighestOperaRate = 0;
-//by amy 080312
-	network->Turbo_Enable = 0;
-	network->CountryIeLen = 0;
-	memset(network->CountryIeBuf, 0, MAX_IE_LEN);
-
-	if (stats->freq == IEEE80211_52GHZ_BAND) {
-		/* for A band (No DS info) */
-		network->channel = stats->received_channel;
-	} else
-		network->flags |= NETWORK_HAS_CCK;
-
-	network->wpa_ie_len = 0;
-	network->rsn_ie_len = 0;
-
-	info_element = &beacon->info_element;
-	left = stats->len - ((void *)info_element - (void *)beacon);
-	while (left >= sizeof(struct ieee80211_info_element_hdr)) {
-		if (sizeof(struct ieee80211_info_element_hdr) + info_element->len > left) {
-			IEEE80211_DEBUG_SCAN("SCAN: parse failed: info_element->len + 2 > left : info_element->len+2=%d left=%d.\n",
-					     info_element->len + sizeof(struct ieee80211_info_element),
-					     left);
-			return 1;
-		}
-
-		switch (info_element->id) {
-		case MFIE_TYPE_SSID:
-			if (ieee80211_is_empty_essid(info_element->data,
-						     info_element->len)) {
-				network->flags |= NETWORK_EMPTY_ESSID;
-				break;
-			}
-
-			network->ssid_len = min(info_element->len,
-						(u8)IW_ESSID_MAX_SIZE);
-			memcpy(network->ssid, info_element->data, network->ssid_len);
-			if (network->ssid_len < IW_ESSID_MAX_SIZE)
-				memset(network->ssid + network->ssid_len, 0,
-				       IW_ESSID_MAX_SIZE - network->ssid_len);
-
-			IEEE80211_DEBUG_SCAN("MFIE_TYPE_SSID: '%s' len=%d.\n",
-					     network->ssid, network->ssid_len);
-			break;
-
-		case MFIE_TYPE_RATES:
-#ifdef CONFIG_IEEE80211_DEBUG
-			p = rates_str;
-#endif
-			network->rates_len = min(info_element->len, MAX_RATES_LENGTH);
-			for (i = 0; i < network->rates_len; i++) {
-				network->rates[i] = info_element->data[i];
-				curRate = network->rates[i] & 0x7f;
-				if (hOpRate < curRate)
-					hOpRate = curRate;
-#ifdef CONFIG_IEEE80211_DEBUG
-				p += snprintf(p, sizeof(rates_str) - (p - rates_str), "%02X ", network->rates[i]);
-#endif
-				if (ieee80211_is_ofdm_rate(info_element->data[i])) {
-					network->flags |= NETWORK_HAS_OFDM;
-					if (info_element->data[i] &
-					    IEEE80211_BASIC_RATE_MASK)
-						network->flags &=
-							~NETWORK_HAS_CCK;
-				}
-			}
-
-			IEEE80211_DEBUG_SCAN("MFIE_TYPE_RATES: '%s' (%d)\n",
-					     rates_str, network->rates_len);
-			break;
-
-		case MFIE_TYPE_RATES_EX:
-#ifdef CONFIG_IEEE80211_DEBUG
-			p = rates_str;
-#endif
-			network->rates_ex_len = min(info_element->len, MAX_RATES_EX_LENGTH);
-			for (i = 0; i < network->rates_ex_len; i++) {
-				network->rates_ex[i] = info_element->data[i];
-				curRate_ex = network->rates_ex[i] & 0x7f;
-				if (hOpRate < curRate_ex)
-					hOpRate = curRate_ex;
-#ifdef CONFIG_IEEE80211_DEBUG
-				p += snprintf(p, sizeof(rates_str) - (p - rates_str), "%02X ", network->rates[i]);
-#endif
-				if (ieee80211_is_ofdm_rate(info_element->data[i])) {
-					network->flags |= NETWORK_HAS_OFDM;
-					if (info_element->data[i] &
-					    IEEE80211_BASIC_RATE_MASK)
-						network->flags &=
-							~NETWORK_HAS_CCK;
-				}
-			}
-
-			IEEE80211_DEBUG_SCAN("MFIE_TYPE_RATES_EX: '%s' (%d)\n",
-					     rates_str, network->rates_ex_len);
-			break;
-
-		case MFIE_TYPE_DS_SET:
-			IEEE80211_DEBUG_SCAN("MFIE_TYPE_DS_SET: %d\n",
-					     info_element->data[0]);
-			if (stats->freq == IEEE80211_24GHZ_BAND)
-				network->channel = info_element->data[0];
-			break;
-
-		case MFIE_TYPE_FH_SET:
-			IEEE80211_DEBUG_SCAN("MFIE_TYPE_FH_SET: ignored\n");
-			break;
-
-		case MFIE_TYPE_CF_SET:
-			IEEE80211_DEBUG_SCAN("MFIE_TYPE_CF_SET: ignored\n");
-			break;
-
-		case MFIE_TYPE_TIM:
-
-			if (info_element->len < 4)
-				break;
-
-			network->dtim_period = info_element->data[1];
-
-			if (ieee->state != IEEE80211_LINKED)
-				break;
-
-			network->last_dtim_sta_time[0] = jiffies;
-			network->last_dtim_sta_time[1] = stats->mac_time[1];
-
-			network->dtim_data = IEEE80211_DTIM_VALID;
-
-			if (info_element->data[0] != 0)
-				break;
-
-			if (info_element->data[2] & 1)
-				network->dtim_data |= IEEE80211_DTIM_MBCAST;
-
-			offset = (info_element->data[2] >> 1)*2;
-
-			//printk("offset1:%x aid:%x\n",offset, ieee->assoc_id);
-
-			/* add and modified for ps 2008.1.22 */
-			if (ieee->assoc_id < 8*offset ||
-				ieee->assoc_id > 8*(offset + info_element->len - 3)) {
-				break;
-			}
-
-			offset = (ieee->assoc_id/8) - offset;// + ((aid % 8)? 0 : 1) ;
-
-		//	printk("offset:%x data:%x, ucast:%d\n", offset,
-			//	info_element->data[3+offset] ,
-			//	info_element->data[3+offset] & (1<<(ieee->assoc_id%8)));
-
-			if (info_element->data[3+offset] & (1<<(ieee->assoc_id%8)))
-				network->dtim_data |= IEEE80211_DTIM_UCAST;
-
-			break;
-
-		case MFIE_TYPE_IBSS_SET:
-			IEEE80211_DEBUG_SCAN("MFIE_TYPE_IBSS_SET: ignored\n");
-			break;
-
-		case MFIE_TYPE_CHALLENGE:
-			IEEE80211_DEBUG_SCAN("MFIE_TYPE_CHALLENGE: ignored\n");
-			break;
-
-		case MFIE_TYPE_GENERIC:
-			//nic is 87B
-			IEEE80211_DEBUG_SCAN("MFIE_TYPE_GENERIC: %d bytes\n",
-					     info_element->len);
-			if (info_element->len >= 4  &&
-			    info_element->data[0] == 0x00 &&
-			    info_element->data[1] == 0x50 &&
-			    info_element->data[2] == 0xf2 &&
-			    info_element->data[3] == 0x01) {
-				network->wpa_ie_len = min(info_element->len + 2,
-							 MAX_WPA_IE_LEN);
-				memcpy(network->wpa_ie, info_element,
-				       network->wpa_ie_len);
-			}
-
-			if (info_element->len == 7 &&
-			    info_element->data[0] == 0x00 &&
-			    info_element->data[1] == 0xe0 &&
-			    info_element->data[2] == 0x4c &&
-			    info_element->data[3] == 0x01 &&
-			    info_element->data[4] == 0x02) {
-				network->Turbo_Enable = 1;
-			}
-			if (1 == stats->nic_type) //nic 87
-				break;
-
-			if (info_element->len >= 5  &&
-			    info_element->data[0] == 0x00 &&
-			    info_element->data[1] == 0x50 &&
-			    info_element->data[2] == 0xf2 &&
-			    info_element->data[3] == 0x02 &&
-			    info_element->data[4] == 0x00) {
-				//printk(KERN_WARNING "wmm info updated: %x\n", info_element->data[6]);
-				//WMM Information Element
-				network->wmm_info = info_element->data[6];
-				network->QoS_Enable = 1;
-			}
-
-			if (info_element->len >= 8  &&
-			    info_element->data[0] == 0x00 &&
-			    info_element->data[1] == 0x50 &&
-			    info_element->data[2] == 0xf2 &&
-			    info_element->data[3] == 0x02 &&
-			    info_element->data[4] == 0x01) {
-				// Not care about version at present.
-				//WMM Information Element
-				//printk(KERN_WARNING "wmm info&param updated: %x\n", info_element->data[6]);
-				network->wmm_info = info_element->data[6];
-				//WMM Parameter Element
-				memcpy(network->wmm_param, (u8 *)(info_element->data + 8), (info_element->len - 8));
-				network->QoS_Enable = 1;
-			}
-			break;
-
-		case MFIE_TYPE_RSN:
-			IEEE80211_DEBUG_SCAN("MFIE_TYPE_RSN: %d bytes\n",
-					     info_element->len);
-			network->rsn_ie_len = min(info_element->len + 2,
-						 MAX_WPA_IE_LEN);
-			memcpy(network->rsn_ie, info_element,
-			       network->rsn_ie_len);
-			break;
-		case MFIE_TYPE_COUNTRY:
-			IEEE80211_DEBUG_SCAN("MFIE_TYPE_COUNTRY: %d bytes\n",
-					     info_element->len);
-//			printk("=====>Receive <%s> Country IE\n",network->ssid);
-			ieee80211_extract_country_ie(ieee, info_element, network, beacon->header.addr2);
-			break;
-		default:
-			IEEE80211_DEBUG_SCAN("unsupported IE %d\n",
-					     info_element->id);
-			break;
-		}
-
-		left -= sizeof(struct ieee80211_info_element_hdr) +
-			info_element->len;
-		info_element = (struct ieee80211_info_element *)
-			&info_element->data[info_element->len];
-	}
-//by amy 080312
-	network->HighestOperaRate = hOpRate;
-//by amy 080312
-	network->mode = 0;
-	if (stats->freq == IEEE80211_52GHZ_BAND)
-		network->mode = IEEE_A;
-	else {
-		if (network->flags & NETWORK_HAS_OFDM)
-			network->mode |= IEEE_G;
-		if (network->flags & NETWORK_HAS_CCK)
-			network->mode |= IEEE_B;
-	}
-
-	if (network->mode == 0) {
-		IEEE80211_DEBUG_SCAN("Filtered out '%s (%pM)' "
-				     "network.\n",
-				     escape_essid(network->ssid,
-						  network->ssid_len),
-				     network->bssid);
-		return 1;
-	}
-
-	if (ieee80211_is_empty_essid(network->ssid, network->ssid_len))
-		network->flags |= NETWORK_EMPTY_ESSID;
-
-	stats->signal = ieee80211_TranslateToDbm(stats->signalstrength);
-	//stats->noise = stats->signal - stats->noise;
-	stats->noise = ieee80211_TranslateToDbm(100 - stats->signalstrength) - 25;
-	memcpy(&network->stats, stats, sizeof(network->stats));
-
-	return 0;
-}
-
-static inline int is_same_network(struct ieee80211_network *src,
-				  struct ieee80211_network *dst,
-				  struct ieee80211_device *ieee)
-{
-	/* A network is only a duplicate if the channel, BSSID, ESSID
-	 * and the capability field (in particular IBSS and BSS) all match.
-	 * We treat all <hidden> with the same BSSID and channel
-	 * as one network */
-	return (((src->ssid_len == dst->ssid_len) || (ieee->iw_mode == IW_MODE_INFRA)) &&  //YJ,mod,080819,for hidden ap
-		//((src->ssid_len == dst->ssid_len) &&
-		(src->channel == dst->channel) &&
-		!memcmp(src->bssid, dst->bssid, ETH_ALEN) &&
-		(!memcmp(src->ssid, dst->ssid, src->ssid_len) || (ieee->iw_mode == IW_MODE_INFRA)) && //YJ,mod,080819,for hidden ap
-		//!memcmp(src->ssid, dst->ssid, src->ssid_len) &&
-		((src->capability & WLAN_CAPABILITY_IBSS) ==
-		(dst->capability & WLAN_CAPABILITY_IBSS)) &&
-		((src->capability & WLAN_CAPABILITY_BSS) ==
-		(dst->capability & WLAN_CAPABILITY_BSS)));
-}
-
-inline void update_network(struct ieee80211_network *dst,
-			   struct ieee80211_network *src)
-{
-	unsigned char quality = src->stats.signalstrength;
-	unsigned char signal = 0;
-	unsigned char noise = 0;
-	if (dst->stats.signalstrength > 0)
-		quality = (dst->stats.signalstrength * 5 + src->stats.signalstrength + 5)/6;
-	signal = ieee80211_TranslateToDbm(quality);
-	//noise = signal - src->stats.noise;
-	if (dst->stats.noise > 0)
-		noise = (dst->stats.noise * 5 + src->stats.noise)/6;
-        //if(strcmp(dst->ssid, "linksys_lzm000") == 0)
-//	printk("ssid:%s, quality:%d, signal:%d\n", dst->ssid, quality, signal);
-	memcpy(&dst->stats, &src->stats, sizeof(struct ieee80211_rx_stats));
-	dst->stats.signalstrength = quality;
-	dst->stats.signal = signal;
-//	printk("==================>stats.signal is %d\n",dst->stats.signal);
-	dst->stats.noise = noise;
-
-
-	dst->capability = src->capability;
-	memcpy(dst->rates, src->rates, src->rates_len);
-	dst->rates_len = src->rates_len;
-	memcpy(dst->rates_ex, src->rates_ex, src->rates_ex_len);
-	dst->rates_ex_len = src->rates_ex_len;
-	dst->HighestOperaRate = src->HighestOperaRate;
-	//printk("==========>in %s: src->ssid is %s,chan is %d\n",__func__,src->ssid,src->channel);
-
-	//YJ,add,080819,for hidden ap
-	if (src->ssid_len > 0) {
-		//if(src->ssid_len == 13)
-		//	printk("=====================>>>>>>>> Dst ssid: %s Src ssid: %s\n", dst->ssid, src->ssid);
-		memset(dst->ssid, 0, dst->ssid_len);
-		dst->ssid_len = src->ssid_len;
-		memcpy(dst->ssid, src->ssid, src->ssid_len);
-	}
-	//YJ,add,080819,for hidden ap,end
-
-	dst->channel = src->channel;
-	dst->mode = src->mode;
-	dst->flags = src->flags;
-	dst->time_stamp[0] = src->time_stamp[0];
-	dst->time_stamp[1] = src->time_stamp[1];
-
-	dst->beacon_interval = src->beacon_interval;
-	dst->listen_interval = src->listen_interval;
-	dst->atim_window = src->atim_window;
-	dst->dtim_period = src->dtim_period;
-	dst->dtim_data = src->dtim_data;
-	dst->last_dtim_sta_time[0] = src->last_dtim_sta_time[0];
-	dst->last_dtim_sta_time[1] = src->last_dtim_sta_time[1];
-//	printk("update:%s, dtim_period:%x, dtim_data:%x\n", src->ssid, src->dtim_period, src->dtim_data);
-	memcpy(dst->wpa_ie, src->wpa_ie, src->wpa_ie_len);
-	dst->wpa_ie_len = src->wpa_ie_len;
-	memcpy(dst->rsn_ie, src->rsn_ie, src->rsn_ie_len);
-	dst->rsn_ie_len = src->rsn_ie_len;
-
-	dst->last_scanned = jiffies;
-	/* dst->last_associate is not overwritten */
-// disable QoS process now, added by David 2006/7/25
-#if 1
-	dst->wmm_info = src->wmm_info; //sure to exist in beacon or probe response frame.
-/*
-	if((dst->wmm_info^src->wmm_info)&0x0f) {//Param Set Count change, update Parameter
-	  memcpy(dst->wmm_param, src->wmm_param, IEEE80211_AC_PRAM_LEN);
-	}
-*/
-	if (src->wmm_param[0].ac_aci_acm_aifsn || \
-	   src->wmm_param[1].ac_aci_acm_aifsn || \
-	   src->wmm_param[2].ac_aci_acm_aifsn || \
-	   src->wmm_param[3].ac_aci_acm_aifsn) {
-		memcpy(dst->wmm_param, src->wmm_param, WME_AC_PRAM_LEN);
-	}
-	dst->QoS_Enable = src->QoS_Enable;
-#else
-	dst->QoS_Enable = 1;//for Rtl8187 simulation
-#endif
-	dst->SignalStrength = src->SignalStrength;
-	dst->Turbo_Enable = src->Turbo_Enable;
-	dst->CountryIeLen = src->CountryIeLen;
-	memcpy(dst->CountryIeBuf, src->CountryIeBuf, src->CountryIeLen);
-}
-
-
-inline void
-ieee80211_process_probe_response(struct ieee80211_device *ieee,
-				 struct ieee80211_probe_response *beacon,
-				 struct ieee80211_rx_stats *stats)
-{
-	struct ieee80211_network network;
-	struct ieee80211_network *target;
-	struct ieee80211_network *oldest = NULL;
-#ifdef CONFIG_IEEE80211_DEBUG
-	struct ieee80211_info_element *info_element = &beacon->info_element;
-#endif
-	unsigned long flags;
-	short renew;
-	u8 wmm_info;
-	u8 is_beacon = (WLAN_FC_GET_STYPE(beacon->header.frame_ctl) == IEEE80211_STYPE_BEACON) ? 1 : 0;  //YJ,add,080819,for hidden ap
-
-	memset(&network, 0, sizeof(struct ieee80211_network));
-
-	IEEE80211_DEBUG_SCAN(
-		"'%s' (%pM): %c%c%c%c %c%c%c%c-%c%c%c%c %c%c%c%c\n",
-		escape_essid(info_element->data, info_element->len),
-		beacon->header.addr3,
-		(beacon->capability & (1<<0xf)) ? '1' : '0',
-		(beacon->capability & (1<<0xe)) ? '1' : '0',
-		(beacon->capability & (1<<0xd)) ? '1' : '0',
-		(beacon->capability & (1<<0xc)) ? '1' : '0',
-		(beacon->capability & (1<<0xb)) ? '1' : '0',
-		(beacon->capability & (1<<0xa)) ? '1' : '0',
-		(beacon->capability & (1<<0x9)) ? '1' : '0',
-		(beacon->capability & (1<<0x8)) ? '1' : '0',
-		(beacon->capability & (1<<0x7)) ? '1' : '0',
-		(beacon->capability & (1<<0x6)) ? '1' : '0',
-		(beacon->capability & (1<<0x5)) ? '1' : '0',
-		(beacon->capability & (1<<0x4)) ? '1' : '0',
-		(beacon->capability & (1<<0x3)) ? '1' : '0',
-		(beacon->capability & (1<<0x2)) ? '1' : '0',
-		(beacon->capability & (1<<0x1)) ? '1' : '0',
-		(beacon->capability & (1<<0x0)) ? '1' : '0');
-
-	if (ieee80211_network_init(ieee, beacon, &network, stats)) {
-		IEEE80211_DEBUG_SCAN("Dropped '%s' (%pM) via %s.\n",
-				     escape_essid(info_element->data,
-						  info_element->len),
-				     beacon->header.addr3,
-				     WLAN_FC_GET_STYPE(beacon->header.frame_ctl) ==
-				     IEEE80211_STYPE_PROBE_RESP ?
-				     "PROBE RESPONSE" : "BEACON");
-		return;
-	}
-
-	// For Asus EeePc request,
-	// (1) if wireless adapter receive get any 802.11d country code in AP beacon,
-	//	   wireless adapter should follow the country code.
-	// (2)  If there is no any country code in beacon,
-	//       then wireless adapter should do active scan from ch1~11 and
-	//       passive scan from ch12~14
-	if (ieee->bGlobalDomain) {
-		if (WLAN_FC_GET_STYPE(beacon->header.frame_ctl) == IEEE80211_STYPE_PROBE_RESP) {
-			// Case 1: Country code
-			if (IS_COUNTRY_IE_VALID(ieee)) {
-				if (!IsLegalChannel(ieee, network.channel)) {
-					printk("GetScanInfo(): For Country code, filter probe response at channel(%d).\n", network.channel);
-					return;
-				}
-			}
-			// Case 2: No any country code.
-			else {
-				// Filter over channel ch12~14
-				if (network.channel > 11) {
-					printk("GetScanInfo(): For Global Domain, filter probe response at channel(%d).\n", network.channel);
-					return;
-				}
-			}
-		} else {
-			// Case 1: Country code
-			if (IS_COUNTRY_IE_VALID(ieee)) {
-				if (!IsLegalChannel(ieee, network.channel)) {
-					printk("GetScanInfo(): For Country code, filter beacon at channel(%d).\n", network.channel);
-					return;
-				}
-			}
-			// Case 2: No any country code.
-			else {
-				// Filter over channel ch12~14
-				if (network.channel > 14) {
-					printk("GetScanInfo(): For Global Domain, filter beacon at channel(%d).\n", network.channel);
-					return;
-				}
-			}
-		}
-	}
-	/* The network parsed correctly -- so now we scan our known networks
-	 * to see if we can find it in our list.
-	 *
-	 * NOTE:  This search is definitely not optimized.  Once its doing
-	 *        the "right thing" we'll optimize it for efficiency if
-	 *        necessary */
-
-	/* Search for this entry in the list and update it if it is
-	 * already there. */
-
-	spin_lock_irqsave(&ieee->lock, flags);
-
-	if (is_same_network(&ieee->current_network, &network, ieee)) {
-		wmm_info = ieee->current_network.wmm_info;
-		//YJ,add,080819,for hidden ap
-		if (is_beacon == 0)
-			network.flags = (~NETWORK_EMPTY_ESSID & network.flags)|(NETWORK_EMPTY_ESSID & ieee->current_network.flags);
-		else if (ieee->state == IEEE80211_LINKED)
-			ieee->NumRxBcnInPeriod++;
-		//YJ,add,080819,for hidden ap,end
-		//printk("====>network.ssid=%s cur_ssid=%s\n", network.ssid, ieee->current_network.ssid);
-		update_network(&ieee->current_network, &network);
-	}
-
-	list_for_each_entry(target, &ieee->network_list, list) {
-		if (is_same_network(target, &network, ieee))
-			break;
-		if ((oldest == NULL) ||
-		    (target->last_scanned < oldest->last_scanned))
-			oldest = target;
-	}
-
-	/* If we didn't find a match, then get a new network slot to initialize
-	 * with this beacon's information */
-	if (&target->list == &ieee->network_list) {
-		if (list_empty(&ieee->network_free_list)) {
-			/* If there are no more slots, expire the oldest */
-			list_del(&oldest->list);
-			target = oldest;
-			IEEE80211_DEBUG_SCAN("Expired '%s' (%pM) from "
-					     "network list.\n",
-					     escape_essid(target->ssid,
-							  target->ssid_len),
-					     target->bssid);
-		} else {
-			/* Otherwise just pull from the free list */
-			target = list_entry(ieee->network_free_list.next,
-					    struct ieee80211_network, list);
-			list_del(ieee->network_free_list.next);
-		}
-
-
-#ifdef CONFIG_IEEE80211_DEBUG
-		IEEE80211_DEBUG_SCAN("Adding '%s' (%pM) via %s.\n",
-				     escape_essid(network.ssid,
-						  network.ssid_len),
-				     network.bssid,
-				     WLAN_FC_GET_STYPE(beacon->header.frame_ctl) ==
-				     IEEE80211_STYPE_PROBE_RESP ?
-				     "PROBE RESPONSE" : "BEACON");
-#endif
-
-		memcpy(target, &network, sizeof(*target));
-		list_add_tail(&target->list, &ieee->network_list);
-	} else {
-		IEEE80211_DEBUG_SCAN("Updating '%s' (%pM) via %s.\n",
-				     escape_essid(target->ssid,
-						  target->ssid_len),
-				     target->bssid,
-				     WLAN_FC_GET_STYPE(beacon->header.frame_ctl) ==
-				     IEEE80211_STYPE_PROBE_RESP ?
-				     "PROBE RESPONSE" : "BEACON");
-
-		/* we have an entry and we are going to update it. But this entry may
-		 * be already expired. In this case we do the same as we found a new
-		 * net and call the new_net handler
-		 */
-		renew = !time_after(target->last_scanned + ieee->scan_age, jiffies);
-		//YJ,add,080819,for hidden ap
-		if (is_beacon == 0)
-			network.flags = (~NETWORK_EMPTY_ESSID & network.flags)|(NETWORK_EMPTY_ESSID & target->flags);
-		//if(strncmp(network.ssid, "linksys-c",9) == 0)
-		//	printk("====>2 network.ssid=%s FLAG=%d target.ssid=%s FLAG=%d\n", network.ssid, network.flags, target->ssid, target->flags);
-		if (((network.flags & NETWORK_EMPTY_ESSID) == NETWORK_EMPTY_ESSID) \
-		    && (((network.ssid_len > 0) && (strncmp(target->ssid, network.ssid, network.ssid_len)))\
-		    || ((ieee->current_network.ssid_len == network.ssid_len) && (strncmp(ieee->current_network.ssid, network.ssid, network.ssid_len) == 0) && (ieee->state == IEEE80211_NOLINK))))
-			renew = 1;
-		//YJ,add,080819,for hidden ap,end
-		update_network(target, &network);
-	}
-
-	spin_unlock_irqrestore(&ieee->lock, flags);
-}
-
-void ieee80211_rx_mgt(struct ieee80211_device *ieee,
-		      struct ieee80211_hdr_4addr *header,
-		      struct ieee80211_rx_stats *stats)
-{
-	switch (WLAN_FC_GET_STYPE(header->frame_ctl)) {
-
-	case IEEE80211_STYPE_BEACON:
-		IEEE80211_DEBUG_MGMT("received BEACON (%d)\n",
-				     WLAN_FC_GET_STYPE(header->frame_ctl));
-		IEEE80211_DEBUG_SCAN("Beacon\n");
-		ieee80211_process_probe_response(
-			ieee, (struct ieee80211_probe_response *)header, stats);
-		break;
-
-	case IEEE80211_STYPE_PROBE_RESP:
-		IEEE80211_DEBUG_MGMT("received PROBE RESPONSE (%d)\n",
-				     WLAN_FC_GET_STYPE(header->frame_ctl));
-		IEEE80211_DEBUG_SCAN("Probe response\n");
-		ieee80211_process_probe_response(
-			ieee, (struct ieee80211_probe_response *)header, stats);
-		break;
-	}
-}
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac.c
deleted file mode 100644
index 03eb164798c..00000000000
--- a/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac.c
+++ /dev/null
@@ -1,2711 +0,0 @@
-/* IEEE 802.11 SoftMAC layer
- * Copyright (c) 2005 Andrea Merello <andrea.merello@gmail.com>
- *
- * Mostly extracted from the rtl8180-sa2400 driver for the
- * in-kernel generic ieee802.11 stack.
- *
- * Few lines might be stolen from other part of the ieee80211
- * stack. Copyright who own it's copyright
- *
- * WPA code stolen from the ipw2200 driver.
- * Copyright who own it's copyright.
- *
- * released under the GPL
- */
-
-#include "ieee80211.h"
-
-#include <linux/random.h>
-#include <linux/delay.h>
-#include <linux/slab.h>
-#include <linux/interrupt.h>
-#include <linux/uaccess.h>
-#include <linux/etherdevice.h>
-
-#include "dot11d.h"
-
-short ieee80211_is_54g(const struct ieee80211_network *net)
-{
-	return (net->rates_ex_len > 0) || (net->rates_len > 4);
-}
-
-short ieee80211_is_shortslot(const struct ieee80211_network *net)
-{
-	return net->capability & WLAN_CAPABILITY_SHORT_SLOT;
-}
-
-/* returns the total length needed for placing the RATE MFIE
- * tag and the EXTENDED RATE MFIE tag if needed.
- * It encludes two bytes per tag for the tag itself and its len
- */
-static unsigned int ieee80211_MFIE_rate_len(struct ieee80211_device *ieee)
-{
-	unsigned int rate_len = 0;
-
-	if (ieee->modulation & IEEE80211_CCK_MODULATION)
-		rate_len = IEEE80211_CCK_RATE_LEN + 2;
-
-	if (ieee->modulation & IEEE80211_OFDM_MODULATION)
-
-		rate_len += IEEE80211_OFDM_RATE_LEN + 2;
-
-	return rate_len;
-}
-
-/* place the MFIE rate, tag to the memory (double) poised.
- * Then it updates the pointer so that it points after the new MFIE tag added.
- */
-static void ieee80211_MFIE_Brate(struct ieee80211_device *ieee, u8 **tag_p)
-{
-	u8 *tag = *tag_p;
-
-	if (ieee->modulation & IEEE80211_CCK_MODULATION) {
-		*tag++ = MFIE_TYPE_RATES;
-		*tag++ = 4;
-		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_CCK_RATE_1MB;
-		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_CCK_RATE_2MB;
-		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_CCK_RATE_5MB;
-		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_CCK_RATE_11MB;
-	}
-
-	/* We may add an option for custom rates that specific HW might support */
-	*tag_p = tag;
-}
-
-static void ieee80211_MFIE_Grate(struct ieee80211_device *ieee, u8 **tag_p)
-{
-	u8 *tag = *tag_p;
-
-		if (ieee->modulation & IEEE80211_OFDM_MODULATION) {
-		*tag++ = MFIE_TYPE_RATES_EX;
-		*tag++ = 8;
-		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_6MB;
-		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_9MB;
-		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_12MB;
-		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_18MB;
-		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_24MB;
-		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_36MB;
-		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_48MB;
-		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_54MB;
-
-	}
-	/* We may add an option for custom rates that specific HW might support */
-	*tag_p = tag;
-}
-
-static void ieee80211_WMM_Info(struct ieee80211_device *ieee, u8 **tag_p)
-{
-	u8 *tag = *tag_p;
-
-	*tag++ = MFIE_TYPE_GENERIC; /* 0 */
-	*tag++ = 7;
-	*tag++ = 0x00;
-	*tag++ = 0x50;
-	*tag++ = 0xf2;
-	*tag++ = 0x02; /* 5 */
-	*tag++ = 0x00;
-	*tag++ = 0x01;
-#ifdef SUPPORT_USPD
-	if (ieee->current_network.wmm_info & 0x80)
-		*tag++ = 0x0f|MAX_SP_Len;
-	else
-		*tag++ = MAX_SP_Len;
-#else
-	*tag++ = MAX_SP_Len;
-#endif
-	*tag_p = tag;
-}
-
-static void ieee80211_TURBO_Info(struct ieee80211_device *ieee, u8 **tag_p)
-{
-	u8 *tag = *tag_p;
-	*tag++ = MFIE_TYPE_GENERIC; /* 0 */
-	*tag++ = 7;
-	*tag++ = 0x00;
-	*tag++ = 0xe0;
-	*tag++ = 0x4c;
-	*tag++ = 0x01; /* 5 */
-	*tag++ = 0x02;
-	*tag++ = 0x11;
-	*tag++ = 0x00;
-	*tag_p = tag;
-	printk(KERN_ALERT "This is enable turbo mode IE process\n");
-}
-
-static void enqueue_mgmt(struct ieee80211_device *ieee, struct sk_buff *skb)
-{
-	int nh;
-	nh = (ieee->mgmt_queue_head + 1) % MGMT_QUEUE_NUM;
-
-	ieee->mgmt_queue_head = nh;
-	ieee->mgmt_queue_ring[nh] = skb;
-}
-
-static struct sk_buff *dequeue_mgmt(struct ieee80211_device *ieee)
-{
-	struct sk_buff *ret;
-
-	if (ieee->mgmt_queue_tail == ieee->mgmt_queue_head)
-		return NULL;
-
-	ret = ieee->mgmt_queue_ring[ieee->mgmt_queue_tail];
-
-	ieee->mgmt_queue_tail =
-		(ieee->mgmt_queue_tail + 1) % MGMT_QUEUE_NUM;
-
-	return ret;
-}
-
-static void init_mgmt_queue(struct ieee80211_device *ieee)
-{
-	ieee->mgmt_queue_tail = ieee->mgmt_queue_head = 0;
-}
-
-void ieee80211_sta_wakeup(struct ieee80211_device *ieee, short nl);
-
-inline void softmac_mgmt_xmit(struct sk_buff *skb,
-			      struct ieee80211_device *ieee)
-{
-	unsigned long flags;
-	short single = ieee->softmac_features & IEEE_SOFTMAC_SINGLE_QUEUE;
-	struct ieee80211_hdr_3addr  *header =
-		(struct ieee80211_hdr_3addr  *) skb->data;
-
-	spin_lock_irqsave(&ieee->lock, flags);
-
-	/* called with 2nd param 0, no mgmt lock required */
-	ieee80211_sta_wakeup(ieee, 0);
-
-	if (single) {
-		if (ieee->queue_stop) {
-			enqueue_mgmt(ieee, skb);
-		} else {
-			header->seq_ctrl = cpu_to_le16(ieee->seq_ctrl[0]<<4);
-
-			if (ieee->seq_ctrl[0] == 0xFFF)
-				ieee->seq_ctrl[0] = 0;
-			else
-				ieee->seq_ctrl[0]++;
-
-			/* avoid watchdog triggers */
-			ieee->dev->trans_start = jiffies;
-			ieee->softmac_data_hard_start_xmit(skb, ieee->dev, ieee->basic_rate);
-		}
-
-		spin_unlock_irqrestore(&ieee->lock, flags);
-	} else {
-		spin_unlock_irqrestore(&ieee->lock, flags);
-		spin_lock_irqsave(&ieee->mgmt_tx_lock, flags);
-
-		header->seq_ctrl = cpu_to_le16(ieee->seq_ctrl[0] << 4);
-
-		if (ieee->seq_ctrl[0] == 0xFFF)
-			ieee->seq_ctrl[0] = 0;
-		else
-			ieee->seq_ctrl[0]++;
-
-		/* avoid watchdog triggers */
-		ieee->dev->trans_start = jiffies;
-		ieee->softmac_hard_start_xmit(skb, ieee->dev);
-
-		spin_unlock_irqrestore(&ieee->mgmt_tx_lock, flags);
-	}
-}
-
-inline void softmac_ps_mgmt_xmit(struct sk_buff *skb,
-				 struct ieee80211_device *ieee)
-{
-	short single = ieee->softmac_features & IEEE_SOFTMAC_SINGLE_QUEUE;
-	struct ieee80211_hdr_3addr  *header =
-		(struct ieee80211_hdr_3addr  *) skb->data;
-
-	if (single) {
-		header->seq_ctrl = cpu_to_le16(ieee->seq_ctrl[0] << 4);
-
-		if (ieee->seq_ctrl[0] == 0xFFF)
-			ieee->seq_ctrl[0] = 0;
-		else
-			ieee->seq_ctrl[0]++;
-
-		/* avoid watchdog triggers */
-		ieee->dev->trans_start = jiffies;
-		ieee->softmac_data_hard_start_xmit(skb, ieee->dev, ieee->basic_rate);
-	} else {
-		header->seq_ctrl = cpu_to_le16(ieee->seq_ctrl[0] << 4);
-
-		if (ieee->seq_ctrl[0] == 0xFFF)
-			ieee->seq_ctrl[0] = 0;
-		else
-			ieee->seq_ctrl[0]++;
-
-		/* avoid watchdog triggers */
-		ieee->dev->trans_start = jiffies;
-		ieee->softmac_hard_start_xmit(skb, ieee->dev);
-	}
-}
-
-inline struct sk_buff *
-ieee80211_disassociate_skb(struct ieee80211_network *beacon,
-			   struct ieee80211_device *ieee, u8 asRsn)
-{
-	struct sk_buff *skb;
-	struct ieee80211_disassoc_frame *disass;
-
-	skb = dev_alloc_skb(sizeof(struct ieee80211_disassoc_frame));
-	if (!skb)
-		return NULL;
-
-	disass = (struct ieee80211_disassoc_frame *) skb_put(skb, sizeof(struct ieee80211_disassoc_frame));
-	disass->header.frame_control = cpu_to_le16(IEEE80211_STYPE_DISASSOC);
-	disass->header.duration_id = 0;
-
-	memcpy(disass->header.addr1, beacon->bssid, ETH_ALEN);
-	memcpy(disass->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
-	memcpy(disass->header.addr3, beacon->bssid, ETH_ALEN);
-
-	disass->reasoncode = asRsn;
-	return skb;
-}
-
-void SendDisassociation(struct ieee80211_device *ieee, u8 *asSta, u8 asRsn)
-{
-	struct ieee80211_network *beacon = &ieee->current_network;
-	struct sk_buff *skb;
-	skb = ieee80211_disassociate_skb(beacon, ieee, asRsn);
-	if (skb)
-		softmac_mgmt_xmit(skb, ieee);
-}
-
-inline struct sk_buff *ieee80211_probe_req(struct ieee80211_device *ieee)
-{
-	unsigned int len, rate_len;
-	u8 *tag;
-	struct sk_buff *skb;
-	struct ieee80211_probe_request *req;
-
-	len = ieee->current_network.ssid_len;
-
-	rate_len = ieee80211_MFIE_rate_len(ieee);
-
-	skb = dev_alloc_skb(sizeof(struct ieee80211_probe_request) +
-			    2 + len + rate_len);
-	if (!skb)
-		return NULL;
-
-	req = (struct ieee80211_probe_request *) skb_put(skb, sizeof(struct ieee80211_probe_request));
-	req->header.frame_ctl = cpu_to_le16(IEEE80211_STYPE_PROBE_REQ);
-	req->header.duration_id = 0; /* FIXME: is this OK ? */
-
-	memset(req->header.addr1, 0xff, ETH_ALEN);
-	memcpy(req->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
-	memset(req->header.addr3, 0xff, ETH_ALEN);
-
-	tag = (u8 *) skb_put(skb, len + 2 + rate_len);
-
-	*tag++ = MFIE_TYPE_SSID;
-	*tag++ = len;
-	memcpy(tag, ieee->current_network.ssid, len);
-	tag += len;
-	ieee80211_MFIE_Brate(ieee, &tag);
-	ieee80211_MFIE_Grate(ieee, &tag);
-
-	return skb;
-}
-
-struct sk_buff *ieee80211_get_beacon_(struct ieee80211_device *ieee);
-
-static void ieee80211_send_beacon(struct ieee80211_device *ieee)
-{
-	struct sk_buff *skb;
-
-	skb = ieee80211_get_beacon_(ieee);
-
-	if (skb) {
-		softmac_mgmt_xmit(skb, ieee);
-		ieee->softmac_stats.tx_beacons++;
-		dev_kfree_skb_any(skb);
-	}
-
-	ieee->beacon_timer.expires = jiffies +
-		(MSECS(ieee->current_network.beacon_interval - 5));
-
-	if (ieee->beacon_txing)
-		add_timer(&ieee->beacon_timer);
-}
-
-
-static void ieee80211_send_beacon_cb(unsigned long _ieee)
-{
-	struct ieee80211_device *ieee =
-		(struct ieee80211_device *) _ieee;
-	unsigned long flags;
-
-	spin_lock_irqsave(&ieee->beacon_lock, flags);
-	ieee80211_send_beacon(ieee);
-	spin_unlock_irqrestore(&ieee->beacon_lock, flags);
-}
-
-static void ieee80211_send_probe(struct ieee80211_device *ieee)
-{
-	struct sk_buff *skb;
-
-	skb = ieee80211_probe_req(ieee);
-	if (skb) {
-		softmac_mgmt_xmit(skb, ieee);
-		ieee->softmac_stats.tx_probe_rq++;
-	}
-}
-
-static void ieee80211_send_probe_requests(struct ieee80211_device *ieee)
-{
-	if (ieee->active_scan && (ieee->softmac_features & IEEE_SOFTMAC_PROBERQ)) {
-		ieee80211_send_probe(ieee);
-		ieee80211_send_probe(ieee);
-	}
-}
-
-/* this performs syncro scan blocking the caller until all channels
- * in the allowed channel map has been checked.
- */
-static void ieee80211_softmac_scan_syncro(struct ieee80211_device *ieee)
-{
-	short ch = 0;
-	u8 channel_map[MAX_CHANNEL_NUMBER+1];
-	memcpy(channel_map, GET_DOT11D_INFO(ieee)->channel_map, MAX_CHANNEL_NUMBER+1);
-	down(&ieee->scan_sem);
-
-	while (1) {
-		do {
-			ch++;
-			if (ch > MAX_CHANNEL_NUMBER)
-				goto out; /* scan completed */
-
-		} while (!channel_map[ch]);
-		/* this function can be called in two situations
-		 * 1- We have switched to ad-hoc mode and we are
-		 *    performing a complete syncro scan before conclude
-		 *    there are no interesting cell and to create a
-		 *    new one. In this case the link state is
-		 *    IEEE80211_NOLINK until we found an interesting cell.
-		 *    If so the ieee8021_new_net, called by the RX path
-		 *    will set the state to IEEE80211_LINKED, so we stop
-		 *    scanning
-		 * 2- We are linked and the root uses run iwlist scan.
-		 *    So we switch to IEEE80211_LINKED_SCANNING to remember
-		 *    that we are still logically linked (not interested in
-		 *    new network events, despite for updating the net list,
-		 *    but we are temporarily 'unlinked' as the driver shall
-		 *    not filter RX frames and the channel is changing.
-		 * So the only situation in witch are interested is to check
-		 * if the state become LINKED because of the #1 situation
-		 */
-
-		if (ieee->state == IEEE80211_LINKED)
-			goto out;
-
-		ieee->set_chan(ieee->dev, ch);
-		if (channel_map[ch] == 1)
-			ieee80211_send_probe_requests(ieee);
-
-		/* this prevent excessive time wait when we
-		 * need to wait for a syncro scan to end..
-		 */
-		if (ieee->sync_scan_hurryup)
-			goto out;
-
-		msleep_interruptible_rtl(IEEE80211_SOFTMAC_SCAN_TIME);
-	}
-out:
-	ieee->sync_scan_hurryup = 0;
-	up(&ieee->scan_sem);
-	if (IS_DOT11D_ENABLE(ieee))
-		DOT11D_ScanComplete(ieee);
-}
-
-void ieee80211_softmac_ips_scan_syncro(struct ieee80211_device *ieee)
-{
-	int ch;
-	unsigned int watch_dog = 0;
-	u8 channel_map[MAX_CHANNEL_NUMBER+1];
-	memcpy(channel_map, GET_DOT11D_INFO(ieee)->channel_map, MAX_CHANNEL_NUMBER+1);
-	down(&ieee->scan_sem);
-	ch = ieee->current_network.channel;
-
-	while (1) {
-		/* this function can be called in two situations
-		 * 1- We have switched to ad-hoc mode and we are
-		 *    performing a complete syncro scan before conclude
-		 *    there are no interesting cell and to create a
-		 *    new one. In this case the link state is
-		 *    IEEE80211_NOLINK until we found an interesting cell.
-		 *    If so the ieee8021_new_net, called by the RX path
-		 *    will set the state to IEEE80211_LINKED, so we stop
-		 *    scanning
-		 * 2- We are linked and the root uses run iwlist scan.
-		 *    So we switch to IEEE80211_LINKED_SCANNING to remember
-		 *    that we are still logically linked (not interested in
-		 *    new network events, despite for updating the net list,
-		 *    but we are temporarily 'unlinked' as the driver shall
-		 *    not filter RX frames and the channel is changing.
-		 * So the only situation in witch are interested is to check
-		 * if the state become LINKED because of the #1 situation
-		 */
-		if (ieee->state == IEEE80211_LINKED)
-			goto out;
-
-		if (channel_map[ieee->current_network.channel] > 0)
-			ieee->set_chan(ieee->dev, ieee->current_network.channel);
-
-		if (channel_map[ieee->current_network.channel] == 1)
-			ieee80211_send_probe_requests(ieee);
-
-		msleep_interruptible_rtl(IEEE80211_SOFTMAC_SCAN_TIME);
-
-		do {
-			if (watch_dog++ >= MAX_CHANNEL_NUMBER)
-				goto out; /* scan completed */
-
-			ieee->current_network.channel = (ieee->current_network.channel + 1)%MAX_CHANNEL_NUMBER;
-		} while (!channel_map[ieee->current_network.channel]);
-	}
-out:
-	ieee->actscanning = false;
-	up(&ieee->scan_sem);
-	if (IS_DOT11D_ENABLE(ieee))
-		DOT11D_ScanComplete(ieee);
-}
-
-static void ieee80211_softmac_scan_wq(struct work_struct *work)
-{
-	struct delayed_work *dwork = to_delayed_work(work);
-	struct ieee80211_device *ieee = container_of(dwork, struct ieee80211_device, softmac_scan_wq);
-	static short watchdog;
-	u8 channel_map[MAX_CHANNEL_NUMBER+1];
-	memcpy(channel_map, GET_DOT11D_INFO(ieee)->channel_map, MAX_CHANNEL_NUMBER+1);
-	down(&ieee->scan_sem);
-
-	do {
-		ieee->current_network.channel =
-			(ieee->current_network.channel + 1) % MAX_CHANNEL_NUMBER;
-		if (watchdog++ > MAX_CHANNEL_NUMBER)
-				goto out; /* no good chans */
-	} while (!channel_map[ieee->current_network.channel]);
-
-	if (ieee->scanning == 0) {
-		printk("error out, scanning = 0\n");
-		goto out;
-	}
-	ieee->set_chan(ieee->dev, ieee->current_network.channel);
-	if (channel_map[ieee->current_network.channel] == 1)
-		ieee80211_send_probe_requests(ieee);
-
-	queue_delayed_work(ieee->wq, &ieee->softmac_scan_wq, IEEE80211_SOFTMAC_SCAN_TIME);
-	up(&ieee->scan_sem);
-	return;
-out:
-	ieee->actscanning = false;
-	watchdog = 0;
-	ieee->scanning = 0;
-	up(&ieee->scan_sem);
-
-	if (IS_DOT11D_ENABLE(ieee))
-		DOT11D_ScanComplete(ieee);
-	return;
-}
-
-static void ieee80211_beacons_start(struct ieee80211_device *ieee)
-{
-	unsigned long flags;
-
-	spin_lock_irqsave(&ieee->beacon_lock, flags);
-
-	ieee->beacon_txing = 1;
-	ieee80211_send_beacon(ieee);
-
-	spin_unlock_irqrestore(&ieee->beacon_lock, flags);
-}
-
-static void ieee80211_beacons_stop(struct ieee80211_device *ieee)
-{
-	unsigned long flags;
-
-	spin_lock_irqsave(&ieee->beacon_lock, flags);
-
-	ieee->beacon_txing = 0;
-	del_timer_sync(&ieee->beacon_timer);
-
-	spin_unlock_irqrestore(&ieee->beacon_lock, flags);
-}
-
-void ieee80211_stop_send_beacons(struct ieee80211_device *ieee)
-{
-	if (ieee->stop_send_beacons)
-		ieee->stop_send_beacons(ieee->dev);
-	if (ieee->softmac_features & IEEE_SOFTMAC_BEACONS)
-		ieee80211_beacons_stop(ieee);
-}
-
-void ieee80211_start_send_beacons(struct ieee80211_device *ieee)
-{
-	if (ieee->start_send_beacons)
-		ieee->start_send_beacons(ieee->dev);
-	if (ieee->softmac_features & IEEE_SOFTMAC_BEACONS)
-		ieee80211_beacons_start(ieee);
-}
-
-static void ieee80211_softmac_stop_scan(struct ieee80211_device *ieee)
-{
-	down(&ieee->scan_sem);
-
-	if (ieee->scanning == 1) {
-		ieee->scanning = 0;
-		cancel_delayed_work(&ieee->softmac_scan_wq);
-	}
-
-	up(&ieee->scan_sem);
-}
-
-void ieee80211_stop_scan(struct ieee80211_device *ieee)
-{
-	if (ieee->softmac_features & IEEE_SOFTMAC_SCAN)
-		ieee80211_softmac_stop_scan(ieee);
-	else
-		ieee->stop_scan(ieee->dev);
-}
-
-/* called with ieee->lock held */
-void ieee80211_rtl_start_scan(struct ieee80211_device *ieee)
-{
-	if (IS_DOT11D_ENABLE(ieee)) {
-		if (IS_COUNTRY_IE_VALID(ieee))
-			RESET_CIE_WATCHDOG(ieee);
-	}
-
-	if (ieee->softmac_features & IEEE_SOFTMAC_SCAN) {
-		if (ieee->scanning == 0) {
-			ieee->scanning = 1;
-#if 1
-			queue_delayed_work(ieee->wq, &ieee->softmac_scan_wq, 0);
-#endif
-		}
-	}else
-		ieee->start_scan(ieee->dev);
-}
-
-/* called with wx_sem held */
-void ieee80211_start_scan_syncro(struct ieee80211_device *ieee)
-{
-	if (IS_DOT11D_ENABLE(ieee)) {
-		if (IS_COUNTRY_IE_VALID(ieee))
-			RESET_CIE_WATCHDOG(ieee);
-	}
-	ieee->sync_scan_hurryup = 0;
-
-	if (ieee->softmac_features & IEEE_SOFTMAC_SCAN)
-		ieee80211_softmac_scan_syncro(ieee);
-	else
-		ieee->scan_syncro(ieee->dev);
-}
-
-inline struct sk_buff *
-ieee80211_authentication_req(struct ieee80211_network *beacon,
-			     struct ieee80211_device *ieee, int challengelen)
-{
-	struct sk_buff *skb;
-	struct ieee80211_authentication *auth;
-
-	skb = dev_alloc_skb(sizeof(struct ieee80211_authentication) + challengelen);
-
-	if (!skb)
-		return NULL;
-
-	auth = (struct ieee80211_authentication *)
-		skb_put(skb, sizeof(struct ieee80211_authentication));
-
-	auth->header.frame_ctl = IEEE80211_STYPE_AUTH;
-	if (challengelen)
-		auth->header.frame_ctl |= IEEE80211_FCTL_WEP;
-
-	auth->header.duration_id = 0x013a; /* FIXME */
-
-	memcpy(auth->header.addr1, beacon->bssid, ETH_ALEN);
-	memcpy(auth->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
-	memcpy(auth->header.addr3, beacon->bssid, ETH_ALEN);
-
-	auth->algorithm = ieee->open_wep ? WLAN_AUTH_OPEN : WLAN_AUTH_SHARED_KEY;
-
-	auth->transaction = cpu_to_le16(ieee->associate_seq);
-	ieee->associate_seq++;
-
-	auth->status = cpu_to_le16(WLAN_STATUS_SUCCESS);
-
-	return skb;
-}
-
-static struct sk_buff *ieee80211_probe_resp(struct ieee80211_device *ieee,
-					    u8 *dest)
-{
-	u8 *tag;
-	int beacon_size;
-	struct ieee80211_probe_response *beacon_buf;
-	struct sk_buff *skb;
-	int encrypt;
-	int atim_len, erp_len;
-	struct ieee80211_crypt_data *crypt;
-
-	char *ssid = ieee->current_network.ssid;
-	int ssid_len = ieee->current_network.ssid_len;
-	int rate_len = ieee->current_network.rates_len+2;
-	int rate_ex_len = ieee->current_network.rates_ex_len;
-	int wpa_ie_len = ieee->wpa_ie_len;
-	if (rate_ex_len > 0)
-		rate_ex_len += 2;
-
-	if (ieee->current_network.capability & WLAN_CAPABILITY_IBSS)
-		atim_len = 4;
-	else
-		atim_len = 0;
-
-	if (ieee80211_is_54g(&ieee->current_network))
-		erp_len = 3;
-	else
-		erp_len = 0;
-
-	beacon_size = sizeof(struct ieee80211_probe_response)+
-		ssid_len
-		+3 /* channel */
-		+rate_len
-		+rate_ex_len
-		+atim_len
-		+wpa_ie_len
-		+erp_len;
-
-	skb = dev_alloc_skb(beacon_size);
-
-	if (!skb)
-		return NULL;
-
-	beacon_buf = (struct ieee80211_probe_response *) skb_put(skb, beacon_size);
-
-	memcpy(beacon_buf->header.addr1, dest, ETH_ALEN);
-	memcpy(beacon_buf->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
-	memcpy(beacon_buf->header.addr3, ieee->current_network.bssid, ETH_ALEN);
-
-	beacon_buf->header.duration_id = 0; /* FIXME */
-	beacon_buf->beacon_interval =
-		cpu_to_le16(ieee->current_network.beacon_interval);
-	beacon_buf->capability =
-		cpu_to_le16(ieee->current_network.capability & WLAN_CAPABILITY_IBSS);
-
-	if (ieee->short_slot && (ieee->current_network.capability & WLAN_CAPABILITY_SHORT_SLOT))
-		beacon_buf->capability |= cpu_to_le16(WLAN_CAPABILITY_SHORT_SLOT);
-
-	crypt = ieee->crypt[ieee->tx_keyidx];
-
-	encrypt = ieee->host_encrypt && crypt && crypt->ops &&
-		((0 == strcmp(crypt->ops->name, "WEP")) || wpa_ie_len);
-
-	if (encrypt)
-		beacon_buf->capability |= cpu_to_le16(WLAN_CAPABILITY_PRIVACY);
-
-
-	beacon_buf->header.frame_ctl = cpu_to_le16(IEEE80211_STYPE_PROBE_RESP);
-
-	beacon_buf->info_element.id = MFIE_TYPE_SSID;
-	beacon_buf->info_element.len = ssid_len;
-
-	tag = (u8 *) beacon_buf->info_element.data;
-
-	memcpy(tag, ssid, ssid_len);
-
-	tag += ssid_len;
-
-	*(tag++) = MFIE_TYPE_RATES;
-	*(tag++) = rate_len - 2;
-	memcpy(tag, ieee->current_network.rates, rate_len-2);
-	tag += rate_len - 2;
-
-	*(tag++) = MFIE_TYPE_DS_SET;
-	*(tag++) = 1;
-	*(tag++) = ieee->current_network.channel;
-
-	if (atim_len) {
-		*(tag++) = MFIE_TYPE_IBSS_SET;
-		*(tag++) = 2;
-		*((u16 *)(tag)) = cpu_to_le16(ieee->current_network.atim_window);
-		tag += 2;
-	}
-
-	if (erp_len) {
-		*(tag++) = MFIE_TYPE_ERP;
-		*(tag++) = 1;
-		*(tag++) = 0;
-	}
-
-	if (rate_ex_len) {
-		*(tag++) = MFIE_TYPE_RATES_EX;
-		*(tag++) = rate_ex_len-2;
-		memcpy(tag, ieee->current_network.rates_ex, rate_ex_len-2);
-		tag += rate_ex_len - 2;
-	}
-
-	if (wpa_ie_len)	{
-		if (ieee->iw_mode == IW_MODE_ADHOC) {
-			/* as Windows will set pairwise key same as the group
-			 * key which is not allowed in Linux, so set this for
-			 * IOT issue.
-			 */
-			memcpy(&ieee->wpa_ie[14], &ieee->wpa_ie[8], 4);
-		}
-
-		memcpy(tag, ieee->wpa_ie, ieee->wpa_ie_len);
-	}
-	skb->dev = ieee->dev;
-	return skb;
-}
-
-static struct sk_buff *ieee80211_assoc_resp(struct ieee80211_device *ieee,
-					    u8 *dest)
-{
-	struct sk_buff *skb;
-	u8 *tag;
-
-	struct ieee80211_crypt_data *crypt;
-	struct ieee80211_assoc_response_frame *assoc;
-	short encrypt;
-
-	unsigned int rate_len = ieee80211_MFIE_rate_len(ieee);
-	int len = sizeof(struct ieee80211_assoc_response_frame) + rate_len;
-
-	skb = dev_alloc_skb(len);
-
-	if (!skb)
-		return NULL;
-
-	assoc = (struct ieee80211_assoc_response_frame *)
-		skb_put(skb, sizeof(struct ieee80211_assoc_response_frame));
-
-	assoc->header.frame_control = cpu_to_le16(IEEE80211_STYPE_ASSOC_RESP);
-	memcpy(assoc->header.addr1, dest, ETH_ALEN);
-	memcpy(assoc->header.addr3, ieee->dev->dev_addr, ETH_ALEN);
-	memcpy(assoc->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
-	assoc->capability = cpu_to_le16(ieee->iw_mode == IW_MODE_MASTER ?
-		WLAN_CAPABILITY_BSS : WLAN_CAPABILITY_IBSS);
-
-	if (ieee->short_slot)
-		assoc->capability |= cpu_to_le16(WLAN_CAPABILITY_SHORT_SLOT);
-
-	if (ieee->host_encrypt)
-		crypt = ieee->crypt[ieee->tx_keyidx];
-	else
-		crypt = NULL;
-
-	encrypt = (crypt && crypt->ops);
-
-	if (encrypt)
-		assoc->capability |= cpu_to_le16(WLAN_CAPABILITY_PRIVACY);
-
-	assoc->status = 0;
-	assoc->aid = cpu_to_le16(ieee->assoc_id);
-	if (ieee->assoc_id == 0x2007)
-		ieee->assoc_id = 0;
-	else
-		ieee->assoc_id++;
-
-	tag = (u8 *) skb_put(skb, rate_len);
-
-	ieee80211_MFIE_Brate(ieee, &tag);
-	ieee80211_MFIE_Grate(ieee, &tag);
-
-	return skb;
-}
-
-static struct sk_buff *ieee80211_auth_resp(struct ieee80211_device *ieee,
-					   int status, u8 *dest)
-{
-	struct sk_buff *skb;
-	struct ieee80211_authentication *auth;
-
-	skb = dev_alloc_skb(sizeof(struct ieee80211_authentication)+1);
-
-	if (!skb)
-		return NULL;
-
-	skb->len = sizeof(struct ieee80211_authentication);
-
-	auth = (struct ieee80211_authentication *)skb->data;
-
-	auth->status = cpu_to_le16(status);
-	auth->transaction = cpu_to_le16(2);
-	auth->algorithm = cpu_to_le16(WLAN_AUTH_OPEN);
-
-	memcpy(auth->header.addr3, ieee->dev->dev_addr, ETH_ALEN);
-	memcpy(auth->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
-	memcpy(auth->header.addr1, dest, ETH_ALEN);
-	auth->header.frame_ctl = cpu_to_le16(IEEE80211_STYPE_AUTH);
-	return skb;
-}
-
-static struct sk_buff *ieee80211_null_func(struct ieee80211_device *ieee, short pwr)
-{
-	struct sk_buff *skb;
-	struct ieee80211_hdr_3addr *hdr;
-
-	skb = dev_alloc_skb(sizeof(struct ieee80211_hdr_3addr));
-
-	if (!skb)
-		return NULL;
-
-	hdr = (struct ieee80211_hdr_3addr *)skb_put(skb, sizeof(struct ieee80211_hdr_3addr));
-
-	memcpy(hdr->addr1, ieee->current_network.bssid, ETH_ALEN);
-	memcpy(hdr->addr2, ieee->dev->dev_addr, ETH_ALEN);
-	memcpy(hdr->addr3, ieee->current_network.bssid, ETH_ALEN);
-
-	hdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_DATA |
-		IEEE80211_STYPE_NULLFUNC | IEEE80211_FCTL_TODS |
-		(pwr ? IEEE80211_FCTL_PM:0));
-
-	return skb;
-}
-
-static void ieee80211_resp_to_assoc_rq(struct ieee80211_device *ieee, u8 *dest)
-{
-	struct sk_buff *buf = ieee80211_assoc_resp(ieee, dest);
-
-	if (buf) {
-		softmac_mgmt_xmit(buf, ieee);
-		dev_kfree_skb_any(buf);
-	}
-}
-
-static void ieee80211_resp_to_auth(struct ieee80211_device *ieee, int s, u8 *dest)
-{
-	struct sk_buff *buf = ieee80211_auth_resp(ieee, s, dest);
-
-	if (buf) {
-		softmac_mgmt_xmit(buf, ieee);
-		dev_kfree_skb_any(buf);
-	}
-}
-
-static void ieee80211_resp_to_probe(struct ieee80211_device *ieee, u8 *dest)
-{
-	struct sk_buff *buf = ieee80211_probe_resp(ieee, dest);
-
-	if (buf) {
-		softmac_mgmt_xmit(buf, ieee);
-		dev_kfree_skb_any(buf);
-	}
-}
-
-inline struct sk_buff *
-ieee80211_association_req(struct ieee80211_network *beacon,
-			  struct ieee80211_device *ieee)
-{
-	struct sk_buff *skb;
-
-	struct ieee80211_assoc_request_frame *hdr;
-	u8 *tag;
-	unsigned int wpa_len = beacon->wpa_ie_len;
-#if 1
-	/* for testing purpose */
-	unsigned int rsn_len = beacon->rsn_ie_len;
-#endif
-	unsigned int rate_len = ieee80211_MFIE_rate_len(ieee);
-	unsigned int wmm_info_len = beacon->QoS_Enable?9:0;
-	unsigned int turbo_info_len = beacon->Turbo_Enable?9:0;
-
-	u8  encry_proto = ieee->wpax_type_notify & 0xff;
-
-	int len = 0;
-
-	/* [0] Notify type of encryption: WPA/WPA2
-	 * [1] pair wise type
-	 * [2] authen type
-	 */
-	if (ieee->wpax_type_set) {
-		if (IEEE_PROTO_WPA == encry_proto) {
-			rsn_len = 0;
-		} else if (IEEE_PROTO_RSN == encry_proto) {
-			wpa_len = 0;
-		}
-	}
-	len = sizeof(struct ieee80211_assoc_request_frame)+
-		+ beacon->ssid_len /* essid tagged val */
-		+ rate_len /* rates tagged val */
-		+ wpa_len
-		+ rsn_len
-		+ wmm_info_len
-		+ turbo_info_len;
-
-	skb = dev_alloc_skb(len);
-
-	if (!skb)
-		return NULL;
-
-	hdr = (struct ieee80211_assoc_request_frame *)
-		skb_put(skb, sizeof(struct ieee80211_assoc_request_frame));
-
-	hdr->header.frame_control = IEEE80211_STYPE_ASSOC_REQ;
-	hdr->header.duration_id = 37; /* FIXME */
-	memcpy(hdr->header.addr1, beacon->bssid, ETH_ALEN);
-	memcpy(hdr->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
-	memcpy(hdr->header.addr3, beacon->bssid, ETH_ALEN);
-	memcpy(ieee->ap_mac_addr, beacon->bssid, ETH_ALEN); /* for HW security */
-
-	hdr->capability = cpu_to_le16(WLAN_CAPABILITY_BSS);
-	if (beacon->capability & WLAN_CAPABILITY_PRIVACY)
-		hdr->capability |= cpu_to_le16(WLAN_CAPABILITY_PRIVACY);
-	if (beacon->capability & WLAN_CAPABILITY_SHORT_PREAMBLE)
-		hdr->capability |= cpu_to_le16(WLAN_CAPABILITY_SHORT_PREAMBLE);
-
-	if (ieee->short_slot)
-		hdr->capability |= cpu_to_le16(WLAN_CAPABILITY_SHORT_SLOT);
-
-	hdr->listen_interval = 0xa; /* FIXME */
-
-	hdr->info_element.id = MFIE_TYPE_SSID;
-
-	hdr->info_element.len = beacon->ssid_len;
-	tag = skb_put(skb, beacon->ssid_len);
-	memcpy(tag, beacon->ssid, beacon->ssid_len);
-
-	tag = skb_put(skb, rate_len);
-
-	ieee80211_MFIE_Brate(ieee, &tag);
-	ieee80211_MFIE_Grate(ieee, &tag);
-
-	/* add rsn==0 condition for ap's mix security mode(wpa+wpa2)
-	 * choose AES encryption as default algorithm while using mixed mode.
-	 */
-
-	tag = skb_put(skb, ieee->wpa_ie_len);
-	memcpy(tag, ieee->wpa_ie, ieee->wpa_ie_len);
-
-	tag = skb_put(skb, wmm_info_len);
-	if (wmm_info_len)
-	  ieee80211_WMM_Info(ieee, &tag);
-
-	tag = skb_put(skb, turbo_info_len);
-	if (turbo_info_len)
-		ieee80211_TURBO_Info(ieee, &tag);
-
-	return skb;
-}
-
-void ieee80211_associate_abort(struct ieee80211_device *ieee)
-{
-	unsigned long flags;
-	spin_lock_irqsave(&ieee->lock, flags);
-
-	ieee->associate_seq++;
-
-	/* don't scan, and avoid to have the RX path possibly
-	 * try again to associate. Even do not react to AUTH or
-	 * ASSOC response. Just wait for the retry wq to be scheduled.
-	 * Here we will check if there are good nets to associate
-	 * with, so we retry or just get back to NO_LINK and scanning
-	 */
-	if (ieee->state == IEEE80211_ASSOCIATING_AUTHENTICATING) {
-		IEEE80211_DEBUG_MGMT("Authentication failed\n");
-		ieee->softmac_stats.no_auth_rs++;
-	} else {
-		IEEE80211_DEBUG_MGMT("Association failed\n");
-		ieee->softmac_stats.no_ass_rs++;
-	}
-
-	ieee->state = IEEE80211_ASSOCIATING_RETRY;
-
-	queue_delayed_work(ieee->wq, &ieee->associate_retry_wq, IEEE80211_SOFTMAC_ASSOC_RETRY_TIME);
-
-	spin_unlock_irqrestore(&ieee->lock, flags);
-}
-
-static void ieee80211_associate_abort_cb(unsigned long dev)
-{
-	ieee80211_associate_abort((struct ieee80211_device *) dev);
-}
-
-static void ieee80211_associate_step1(struct ieee80211_device *ieee)
-{
-	struct ieee80211_network *beacon = &ieee->current_network;
-	struct sk_buff *skb;
-
-	IEEE80211_DEBUG_MGMT("Stopping scan\n");
-	ieee->softmac_stats.tx_auth_rq++;
-	skb = ieee80211_authentication_req(beacon, ieee, 0);
-	if (!skb) {
-		ieee80211_associate_abort(ieee);
-	} else {
-		ieee->state = IEEE80211_ASSOCIATING_AUTHENTICATING;
-		IEEE80211_DEBUG_MGMT("Sending authentication request\n");
-		softmac_mgmt_xmit(skb, ieee);
-		/* BUGON when you try to add_timer twice, using mod_timer may
-		 * be better.
-		 */
-		if (!timer_pending(&ieee->associate_timer)) {
-			ieee->associate_timer.expires = jiffies + (HZ / 2);
-			add_timer(&ieee->associate_timer);
-		}
-		/* If call dev_kfree_skb_any,a warning will ocur....
-		 * KERNEL: assertion (!atomic_read(&skb->users)) failed at
-		 * net/core/dev.c (1708)
-		 */
-	}
-}
-
-static void ieee80211_rtl_auth_challenge(struct ieee80211_device *ieee, u8 *challenge,
-				  int chlen)
-{
-	u8 *c;
-	struct sk_buff *skb;
-	struct ieee80211_network *beacon = &ieee->current_network;
-	del_timer_sync(&ieee->associate_timer);
-	ieee->associate_seq++;
-	ieee->softmac_stats.tx_auth_rq++;
-
-	skb = ieee80211_authentication_req(beacon, ieee, chlen+2);
-	if (!skb)
-		ieee80211_associate_abort(ieee);
-	else {
-		c = skb_put(skb, chlen+2);
-		*(c++) = MFIE_TYPE_CHALLENGE;
-		*(c++) = chlen;
-		memcpy(c, challenge, chlen);
-
-		IEEE80211_DEBUG_MGMT("Sending authentication challenge response\n");
-
-		ieee80211_encrypt_fragment(ieee, skb, sizeof(struct ieee80211_hdr_3addr));
-
-		softmac_mgmt_xmit(skb, ieee);
-		if (!timer_pending(&ieee->associate_timer)) {
-		ieee->associate_timer.expires = jiffies + (HZ / 2);
-		add_timer(&ieee->associate_timer);
-		}
-		dev_kfree_skb_any(skb);
-	}
-	kfree(challenge);
-}
-
-static void ieee80211_associate_step2(struct ieee80211_device *ieee)
-{
-	struct sk_buff *skb;
-	struct ieee80211_network *beacon = &ieee->current_network;
-
-	del_timer_sync(&ieee->associate_timer);
-
-	IEEE80211_DEBUG_MGMT("Sending association request\n");
-	ieee->softmac_stats.tx_ass_rq++;
-	skb = ieee80211_association_req(beacon, ieee);
-	if (!skb)
-		ieee80211_associate_abort(ieee);
-	else {
-		softmac_mgmt_xmit(skb, ieee);
-		if (!timer_pending(&ieee->associate_timer)) {
-		ieee->associate_timer.expires = jiffies + (HZ / 2);
-		add_timer(&ieee->associate_timer);
-		}
-	}
-}
-
-static void ieee80211_associate_complete_wq(struct work_struct *work)
-{
-	struct ieee80211_device *ieee = container_of(work, struct ieee80211_device, associate_complete_wq);
-
-	printk(KERN_INFO "Associated successfully\n");
-	if (ieee80211_is_54g(&ieee->current_network) &&
-		(ieee->modulation & IEEE80211_OFDM_MODULATION)) {
-		ieee->rate = 540;
-		printk(KERN_INFO"Using G rates\n");
-	} else {
-		ieee->rate = 110;
-		printk(KERN_INFO"Using B rates\n");
-	}
-	ieee->link_change(ieee->dev);
-	notify_wx_assoc_event(ieee);
-	if (ieee->data_hard_resume)
-		ieee->data_hard_resume(ieee->dev);
-	netif_carrier_on(ieee->dev);
-}
-
-static void ieee80211_associate_complete(struct ieee80211_device *ieee)
-{
-	del_timer_sync(&ieee->associate_timer);
-
-	ieee->state = IEEE80211_LINKED;
-	IEEE80211_DEBUG_MGMT("Successfully associated\n");
-
-	queue_work(ieee->wq, &ieee->associate_complete_wq);
-}
-
-static void ieee80211_associate_procedure_wq(struct work_struct *work)
-{
-	struct ieee80211_device *ieee = container_of(work, struct ieee80211_device, associate_procedure_wq);
-
-	ieee->sync_scan_hurryup = 1;
-	down(&ieee->wx_sem);
-
-	if (ieee->data_hard_stop)
-		ieee->data_hard_stop(ieee->dev);
-
-	ieee80211_stop_scan(ieee);
-	ieee->set_chan(ieee->dev, ieee->current_network.channel);
-
-	ieee->associate_seq = 1;
-	ieee80211_associate_step1(ieee);
-
-	up(&ieee->wx_sem);
-}
-
-inline void ieee80211_softmac_new_net(struct ieee80211_device *ieee,
-				      struct ieee80211_network *net)
-{
-	u8 tmp_ssid[IW_ESSID_MAX_SIZE+1];
-	int tmp_ssid_len = 0;
-
-	short apset, ssidset, ssidbroad, apmatch, ssidmatch;
-
-	/* we are interested in new new only if we are not associated
-	 * and we are not associating / authenticating
-	 */
-	if (ieee->state != IEEE80211_NOLINK)
-		return;
-
-	if ((ieee->iw_mode == IW_MODE_INFRA) && !(net->capability & WLAN_CAPABILITY_BSS))
-		return;
-
-	if ((ieee->iw_mode == IW_MODE_ADHOC) && !(net->capability & WLAN_CAPABILITY_IBSS))
-		return;
-
-	if (ieee->iw_mode == IW_MODE_INFRA || ieee->iw_mode == IW_MODE_ADHOC) {
-		/* if the user specified the AP MAC, we need also the essid
-		 * This could be obtained by beacons or, if the network does not
-		 * broadcast it, it can be put manually.
-		 */
-		apset = ieee->wap_set;
-		ssidset = ieee->ssid_set;
-		ssidbroad =  !(net->ssid_len == 0 || net->ssid[0] == '\0');
-		apmatch = (memcmp(ieee->current_network.bssid, net->bssid, ETH_ALEN) == 0);
-
-		if (ieee->current_network.ssid_len != net->ssid_len)
-			ssidmatch = 0;
-		else
-			ssidmatch = (0 == strncmp(ieee->current_network.ssid, net->ssid, net->ssid_len));
-
-		/* if the user set the AP check if match.
-		 * if the network does not broadcast essid we check the user
-		 * supplied ANY essid
-		 * if the network does broadcast and the user does not set essid
-		 * it is OK
-		 * if the network does broadcast and the user did set essid
-		 * chech if essid match
-		 * (apset && apmatch && ((ssidset && ssidbroad && ssidmatch) ||
-		 *  (ssidbroad && !ssidset) || (!ssidbroad && ssidset))) ||
-		 * if the ap is not set, check that the user set the bssid and
-		 * the network does broadcast and that those two bssid matches
-		 * (!apset && ssidset && ssidbroad && ssidmatch)
-		 */
-		if ((apset && apmatch && ((ssidset && ssidbroad && ssidmatch) ||
-		     (ssidbroad && !ssidset) || (!ssidbroad && ssidset))) ||
-		    (!apset && ssidset && ssidbroad && ssidmatch)) {
-			/* if the essid is hidden replace it with the
-			 * essid provided by the user.
-			 */
-			if (!ssidbroad) {
-				strncpy(tmp_ssid, ieee->current_network.ssid, IW_ESSID_MAX_SIZE);
-				tmp_ssid_len = ieee->current_network.ssid_len;
-			}
-			memcpy(&ieee->current_network, net, sizeof(struct ieee80211_network));
-
-			if (!ssidbroad) {
-				strncpy(ieee->current_network.ssid, tmp_ssid, IW_ESSID_MAX_SIZE);
-				ieee->current_network.ssid_len = tmp_ssid_len;
-			}
-			printk(KERN_INFO"Linking with %s: channel is %d\n", ieee->current_network.ssid, ieee->current_network.channel);
-
-			if (ieee->iw_mode == IW_MODE_INFRA) {
-				ieee->state = IEEE80211_ASSOCIATING;
-				ieee->beinretry = false;
-				queue_work(ieee->wq, &ieee->associate_procedure_wq);
-			} else {
-				if (ieee80211_is_54g(&ieee->current_network) &&
-						(ieee->modulation & IEEE80211_OFDM_MODULATION)) {
-					ieee->rate = 540;
-					printk(KERN_INFO"Using G rates\n");
-				} else {
-					ieee->rate = 110;
-					printk(KERN_INFO"Using B rates\n");
-				}
-				ieee->state = IEEE80211_LINKED;
-				ieee->beinretry = false;
-			}
-		}
-	}
-}
-
-void ieee80211_softmac_check_all_nets(struct ieee80211_device *ieee)
-{
-	unsigned long flags;
-	struct ieee80211_network *target;
-
-	spin_lock_irqsave(&ieee->lock, flags);
-	list_for_each_entry(target, &ieee->network_list, list) {
-		/* if the state become different that NOLINK means
-		 * we had found what we are searching for
-		 */
-		if (ieee->state != IEEE80211_NOLINK)
-			break;
-
-		if (ieee->scan_age == 0 || time_after(target->last_scanned + ieee->scan_age, jiffies))
-			ieee80211_softmac_new_net(ieee, target);
-	}
-	spin_unlock_irqrestore(&ieee->lock, flags);
-}
-
-static inline u16 auth_parse(struct sk_buff *skb, u8 **challenge, int *chlen)
-{
-	struct ieee80211_authentication *a;
-	u8 *t;
-	if (skb->len < (sizeof(struct ieee80211_authentication) - sizeof(struct ieee80211_info_element))) {
-		IEEE80211_DEBUG_MGMT("invalid len in auth resp: %d\n", skb->len);
-		return 0xcafe;
-	}
-	*challenge = NULL;
-	a = (struct ieee80211_authentication *) skb->data;
-	if (skb->len > (sizeof(struct ieee80211_authentication) + 3)) {
-		t = skb->data + sizeof(struct ieee80211_authentication);
-
-		if (*(t++) == MFIE_TYPE_CHALLENGE) {
-			*chlen = *(t++);
-			*challenge = kmemdup(t, *chlen, GFP_ATOMIC);
-			if (!*challenge)
-				return -ENOMEM;
-		}
-	}
-	return cpu_to_le16(a->status);
-}
-
-static int auth_rq_parse(struct sk_buff *skb, u8 *dest)
-{
-	struct ieee80211_authentication *a;
-
-	if (skb->len < (sizeof(struct ieee80211_authentication) - sizeof(struct ieee80211_info_element))) {
-		IEEE80211_DEBUG_MGMT("invalid len in auth request: %d\n", skb->len);
-		return -1;
-	}
-	a = (struct ieee80211_authentication *) skb->data;
-
-	memcpy(dest, a->header.addr2, ETH_ALEN);
-
-	if (le16_to_cpu(a->algorithm) != WLAN_AUTH_OPEN)
-		return  WLAN_STATUS_NOT_SUPPORTED_AUTH_ALG;
-
-	return WLAN_STATUS_SUCCESS;
-}
-
-static short probe_rq_parse(struct ieee80211_device *ieee, struct sk_buff *skb,
-			    u8 *src)
-{
-	u8 *tag;
-	u8 *skbend;
-	u8 *ssid = NULL;
-	u8 ssidlen = 0;
-
-	struct ieee80211_hdr_3addr   *header =
-		(struct ieee80211_hdr_3addr   *) skb->data;
-
-	if (skb->len < sizeof(struct ieee80211_hdr_3addr))
-		return -1; /* corrupted */
-
-	memcpy(src, header->addr2, ETH_ALEN);
-
-	skbend = (u8 *)skb->data + skb->len;
-
-	tag = skb->data + sizeof(struct ieee80211_hdr_3addr);
-
-	while (tag+1 < skbend) {
-		if (*tag == 0) {
-			ssid = tag+2;
-			ssidlen = *(tag+1);
-			break;
-		}
-		tag++; /* point to the len field */
-		tag = tag + *(tag); /* point to the last data byte of the tag */
-		tag++; /* point to the next tag */
-	}
-
-	if (ssidlen == 0)
-		return 1;
-
-	if (!ssid)
-		 return 1; /* ssid not found in tagged param */
-
-	return (!strncmp(ssid, ieee->current_network.ssid, ssidlen));
-
-}
-
-static int assoc_rq_parse(struct sk_buff *skb, u8 *dest)
-{
-	struct ieee80211_assoc_request_frame *a;
-
-	if (skb->len < (sizeof(struct ieee80211_assoc_request_frame) -
-		sizeof(struct ieee80211_info_element))) {
-
-		IEEE80211_DEBUG_MGMT("invalid len in auth request:%d\n", skb->len);
-		return -1;
-	}
-
-	a = (struct ieee80211_assoc_request_frame *) skb->data;
-
-	memcpy(dest, a->header.addr2, ETH_ALEN);
-
-	return 0;
-}
-
-static inline u16 assoc_parse(struct sk_buff *skb, int *aid)
-{
-	struct ieee80211_assoc_response_frame *a;
-	if (skb->len < sizeof(struct ieee80211_assoc_response_frame)) {
-		IEEE80211_DEBUG_MGMT("invalid len in auth resp: %d\n", skb->len);
-		return 0xcafe;
-	}
-
-	a = (struct ieee80211_assoc_response_frame *) skb->data;
-	*aid = le16_to_cpu(a->aid) & 0x3fff;
-	return le16_to_cpu(a->status);
-}
-
-static inline void ieee80211_rx_probe_rq(struct ieee80211_device *ieee,
-					 struct sk_buff *skb)
-{
-	u8 dest[ETH_ALEN];
-
-	ieee->softmac_stats.rx_probe_rq++;
-	if (probe_rq_parse(ieee, skb, dest)) {
-		ieee->softmac_stats.tx_probe_rs++;
-		ieee80211_resp_to_probe(ieee, dest);
-	}
-}
-
-inline void ieee80211_rx_auth_rq(struct ieee80211_device *ieee,
-				 struct sk_buff *skb)
-{
-	u8 dest[ETH_ALEN];
-	int status;
-	ieee->softmac_stats.rx_auth_rq++;
-
-	status = auth_rq_parse(skb, dest);
-	if (status != -1)
-		ieee80211_resp_to_auth(ieee, status, dest);
-}
-
-inline void
-ieee80211_rx_assoc_rq(struct ieee80211_device *ieee, struct sk_buff *skb)
-{
-
-	u8 dest[ETH_ALEN];
-
-	ieee->softmac_stats.rx_ass_rq++;
-	if (assoc_rq_parse(skb, dest) != -1)
-		ieee80211_resp_to_assoc_rq(ieee, dest);
-
-
-	printk(KERN_INFO"New client associated: %pM\n", dest);
-}
-
-void ieee80211_sta_ps_send_null_frame(struct ieee80211_device *ieee, short pwr)
-{
-	struct sk_buff *buf = ieee80211_null_func(ieee, pwr);
-
-	if (buf)
-		softmac_ps_mgmt_xmit(buf, ieee);
-}
-
-static short ieee80211_sta_ps_sleep(struct ieee80211_device *ieee, u32 *time_h,
-			     u32 *time_l)
-{
-	int timeout = 0;
-
-	u8 dtim;
-	dtim = ieee->current_network.dtim_data;
-
-	if (!(dtim & IEEE80211_DTIM_VALID))
-		return 0;
-	else
-		timeout = ieee->current_network.beacon_interval;
-
-	ieee->current_network.dtim_data = IEEE80211_DTIM_INVALID;
-
-	if (dtim & ((IEEE80211_DTIM_UCAST | IEEE80211_DTIM_MBCAST) & ieee->ps))
-		return 2;
-
-	if (!time_after(jiffies, ieee->dev->trans_start + MSECS(timeout)))
-		return 0;
-
-	if (!time_after(jiffies, ieee->last_rx_ps_time + MSECS(timeout)))
-		return 0;
-
-	if ((ieee->softmac_features & IEEE_SOFTMAC_SINGLE_QUEUE) &&
-		(ieee->mgmt_queue_tail != ieee->mgmt_queue_head))
-		return 0;
-
-	if (time_l) {
-		*time_l = ieee->current_network.last_dtim_sta_time[0]
-			+ MSECS((ieee->current_network.beacon_interval));
-	}
-
-	if (time_h) {
-		*time_h = ieee->current_network.last_dtim_sta_time[1];
-		if (time_l && *time_l < ieee->current_network.last_dtim_sta_time[0])
-			*time_h += 1;
-	}
-
-	return 1;
-}
-
-static inline void ieee80211_sta_ps(struct ieee80211_device *ieee)
-{
-
-	u32 th, tl;
-	short sleep;
-
-	unsigned long flags, flags2;
-
-	spin_lock_irqsave(&ieee->lock, flags);
-
-	if ((ieee->ps == IEEE80211_PS_DISABLED ||
-		ieee->iw_mode != IW_MODE_INFRA ||
-		ieee->state != IEEE80211_LINKED)) {
-
-		/* #warning CHECK_LOCK_HERE */
-		spin_lock_irqsave(&ieee->mgmt_tx_lock, flags2);
-
-		ieee80211_sta_wakeup(ieee, 1);
-
-		spin_unlock_irqrestore(&ieee->mgmt_tx_lock, flags2);
-	}
-
-	sleep = ieee80211_sta_ps_sleep(ieee, &th, &tl);
-	/* 2 wake, 1 sleep, 0 do nothing */
-	if (sleep == 0)
-		goto out;
-
-	if (sleep == 1) {
-		if (ieee->sta_sleep == 1)
-			ieee->enter_sleep_state(ieee->dev, th, tl);
-
-		else if (ieee->sta_sleep == 0) {
-			spin_lock_irqsave(&ieee->mgmt_tx_lock, flags2);
-			if (ieee->ps_is_queue_empty(ieee->dev)) {
-				ieee->sta_sleep = 2;
-
-				ieee->ps_request_tx_ack(ieee->dev);
-
-				ieee80211_sta_ps_send_null_frame(ieee, 1);
-
-				ieee->ps_th = th;
-				ieee->ps_tl = tl;
-			}
-			spin_unlock_irqrestore(&ieee->mgmt_tx_lock, flags2);
-		}
-	} else if (sleep == 2) {
-		/* #warning CHECK_LOCK_HERE */
-		spin_lock_irqsave(&ieee->mgmt_tx_lock, flags2);
-
-		ieee80211_sta_wakeup(ieee, 1);
-
-		spin_unlock_irqrestore(&ieee->mgmt_tx_lock, flags2);
-	}
-out:
-	spin_unlock_irqrestore(&ieee->lock, flags);
-}
-
-void ieee80211_sta_wakeup(struct ieee80211_device *ieee, short nl)
-{
-	if (ieee->sta_sleep == 0) {
-		if (nl) {
-			ieee->ps_request_tx_ack(ieee->dev);
-			ieee80211_sta_ps_send_null_frame(ieee, 0);
-		}
-		return;
-	}
-
-	if (ieee->sta_sleep == 1)
-		ieee->sta_wake_up(ieee->dev);
-
-	ieee->sta_sleep = 0;
-
-	if (nl) {
-		ieee->ps_request_tx_ack(ieee->dev);
-		ieee80211_sta_ps_send_null_frame(ieee, 0);
-	}
-}
-
-void ieee80211_ps_tx_ack(struct ieee80211_device *ieee, short success)
-{
-	unsigned long flags, flags2;
-
-	spin_lock_irqsave(&ieee->lock, flags);
-	if (ieee->sta_sleep == 2) {
-		/* Null frame with PS bit set */
-		if (success) {
-			ieee->sta_sleep = 1;
-			ieee->enter_sleep_state(ieee->dev, ieee->ps_th, ieee->ps_tl);
-		}
-		/* if the card report not success we can't be sure the AP
-		 * has not RXed so we can't assume the AP believe us awake
-		 */
-	} else {
-		if ((ieee->sta_sleep == 0) && !success) {
-			spin_lock_irqsave(&ieee->mgmt_tx_lock, flags2);
-			ieee80211_sta_ps_send_null_frame(ieee, 0);
-			spin_unlock_irqrestore(&ieee->mgmt_tx_lock, flags2);
-		}
-	}
-	spin_unlock_irqrestore(&ieee->lock, flags);
-}
-
-inline int ieee80211_rx_frame_softmac(struct ieee80211_device *ieee,
-				      struct sk_buff *skb,
-				      struct ieee80211_rx_stats *rx_stats,
-				      u16 type,	u16 stype)
-{
-	struct ieee80211_hdr_3addr *header = (struct ieee80211_hdr_3addr *) skb->data;
-	u16 errcode;
-	u8 *challenge = NULL;
-	int chlen = 0;
-	int aid = 0;
-	struct ieee80211_assoc_response_frame *assoc_resp;
-	struct ieee80211_info_element *info_element;
-
-	if (!ieee->proto_started)
-		return 0;
-
-	if (ieee->sta_sleep || (ieee->ps != IEEE80211_PS_DISABLED &&
-		ieee->iw_mode == IW_MODE_INFRA &&
-		ieee->state == IEEE80211_LINKED))
-
-		tasklet_schedule(&ieee->ps_task);
-
-	if (WLAN_FC_GET_STYPE(header->frame_control) != IEEE80211_STYPE_PROBE_RESP &&
-		WLAN_FC_GET_STYPE(header->frame_control) != IEEE80211_STYPE_BEACON)
-		ieee->last_rx_ps_time = jiffies;
-
-	switch (WLAN_FC_GET_STYPE(header->frame_control)) {
-		case IEEE80211_STYPE_ASSOC_RESP:
-		case IEEE80211_STYPE_REASSOC_RESP:
-			IEEE80211_DEBUG_MGMT("received [RE]ASSOCIATION RESPONSE (%d)\n",
-					WLAN_FC_GET_STYPE(header->frame_ctl));
-			if ((ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE) &&
-				ieee->state == IEEE80211_ASSOCIATING_AUTHENTICATED &&
-				ieee->iw_mode == IW_MODE_INFRA) {
-				errcode = assoc_parse(skb, &aid);
-				if (0 == errcode) {
-					u16 left;
-
-					ieee->state = IEEE80211_LINKED;
-					ieee->assoc_id = aid;
-					ieee->softmac_stats.rx_ass_ok++;
-					/* card type is 8187 */
-					if (1 == rx_stats->nic_type)
-						goto associate_complete;
-
-					assoc_resp = (struct ieee80211_assoc_response_frame *)skb->data;
-					info_element = &assoc_resp->info_element;
-					left = skb->len - ((void *)info_element - (void *)assoc_resp);
-
-					while (left >= sizeof(struct ieee80211_info_element_hdr)) {
-						if (sizeof(struct ieee80211_info_element_hdr) + info_element->len > left) {
-							printk(KERN_WARNING "[re]associate response error!");
-							return 1;
-						}
-						switch (info_element->id) {
-						  case MFIE_TYPE_GENERIC:
-							IEEE80211_DEBUG_SCAN("MFIE_TYPE_GENERIC: %d bytes\n", info_element->len);
-							if (info_element->len >= 8  &&
-							    info_element->data[0] == 0x00 &&
-							    info_element->data[1] == 0x50 &&
-							    info_element->data[2] == 0xf2 &&
-							    info_element->data[3] == 0x02 &&
-							    info_element->data[4] == 0x01) {
-								/* Not care about version at present.
-								 * WMM Parameter Element.
-								 */
-								memcpy(ieee->current_network.wmm_param, (u8 *)(info_element->data\
-									+ 8), (info_element->len - 8));
-
-								if (((ieee->current_network.wmm_info^info_element->data[6])& \
-										    0x0f) || (!ieee->init_wmmparam_flag)) {
-									/* refresh parameter element for current network
-									 * update the register parameter for hardware.
-									 */
-									ieee->init_wmmparam_flag = 1;
-									queue_work(ieee->wq, &ieee->wmm_param_update_wq);
-								}
-								/* update info_element for current network */
-								ieee->current_network.wmm_info  = info_element->data[6];
-							}
-							break;
-						  default:
-							/* nothing to do at present!!! */
-							break;
-						}
-
-						left -= sizeof(struct ieee80211_info_element_hdr) +
-							info_element->len;
-						info_element = (struct ieee80211_info_element *)
-							&info_element->data[info_element->len];
-					}
-					/* legacy AP, reset the AC_xx_param register */
-					if (!ieee->init_wmmparam_flag) {
-						queue_work(ieee->wq, &ieee->wmm_param_update_wq);
-						ieee->init_wmmparam_flag = 1; /* indicate AC_xx_param upated since last associate */
-					}
-associate_complete:
-					ieee80211_associate_complete(ieee);
-				} else {
-					ieee->softmac_stats.rx_ass_err++;
-					IEEE80211_DEBUG_MGMT(
-						"Association response status code 0x%x\n",
-						errcode);
-					ieee80211_associate_abort(ieee);
-				}
-			}
-			break;
-		case IEEE80211_STYPE_ASSOC_REQ:
-		case IEEE80211_STYPE_REASSOC_REQ:
-			if ((ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE) &&
-				ieee->iw_mode == IW_MODE_MASTER)
-
-				ieee80211_rx_assoc_rq(ieee, skb);
-			break;
-		case IEEE80211_STYPE_AUTH:
-			if (ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE) {
-				if (ieee->state == IEEE80211_ASSOCIATING_AUTHENTICATING &&
-				ieee->iw_mode == IW_MODE_INFRA){
-						IEEE80211_DEBUG_MGMT("Received authentication response");
-
-						errcode = auth_parse(skb, &challenge, &chlen);
-						if (0 == errcode) {
-							if (ieee->open_wep || !challenge) {
-								ieee->state = IEEE80211_ASSOCIATING_AUTHENTICATED;
-								ieee->softmac_stats.rx_auth_rs_ok++;
-
-								ieee80211_associate_step2(ieee);
-							} else {
-								ieee80211_rtl_auth_challenge(ieee, challenge, chlen);
-							}
-						} else {
-							ieee->softmac_stats.rx_auth_rs_err++;
-							IEEE80211_DEBUG_MGMT("Authentication response status code 0x%x", errcode);
-							ieee80211_associate_abort(ieee);
-						}
-
-					} else if (ieee->iw_mode == IW_MODE_MASTER) {
-						ieee80211_rx_auth_rq(ieee, skb);
-					}
-				}
-			break;
-		case IEEE80211_STYPE_PROBE_REQ:
-			if ((ieee->softmac_features & IEEE_SOFTMAC_PROBERS) &&
-				((ieee->iw_mode == IW_MODE_ADHOC ||
-				ieee->iw_mode == IW_MODE_MASTER) &&
-				ieee->state == IEEE80211_LINKED))
-
-				ieee80211_rx_probe_rq(ieee, skb);
-			break;
-		case IEEE80211_STYPE_DISASSOC:
-		case IEEE80211_STYPE_DEAUTH:
-			/* FIXME for now repeat all the association procedure
-			 * both for disassociation and deauthentication
-			 */
-			if ((ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE) &&
-				(ieee->state == IEEE80211_LINKED) &&
-				(ieee->iw_mode == IW_MODE_INFRA) &&
-				(!memcmp(header->addr2, ieee->current_network.bssid, ETH_ALEN))) {
-				ieee->state = IEEE80211_ASSOCIATING;
-				ieee->softmac_stats.reassoc++;
-
-				queue_work(ieee->wq, &ieee->associate_procedure_wq);
-			}
-			break;
-		default:
-			return -1;
-			break;
-	}
-	return 0;
-}
-
-/* following are for a simpler TX queue management.
- * Instead of using netif_[stop/wake]_queue the driver
- * will uses these two function (plus a reset one), that
- * will internally uses the kernel netif_* and takes
- * care of the ieee802.11 fragmentation.
- * So the driver receives a fragment per time and might
- * call the stop function when it want without take care
- * to have enough room to TX an entire packet.
- * This might be useful if each fragment need it's own
- * descriptor, thus just keep a total free memory > than
- * the max fragmentation threshold is not enough.. If the
- * ieee802.11 stack passed a TXB struct then you needed
- * to keep N free descriptors where
- * N = MAX_PACKET_SIZE / MIN_FRAG_TRESHOLD
- * In this way you need just one and the 802.11 stack
- * will take care of buffering fragments and pass them to
- * to the driver later, when it wakes the queue.
- */
-
-void ieee80211_softmac_xmit(struct ieee80211_txb *txb,
-			    struct ieee80211_device *ieee)
-{
-	unsigned long flags;
-	int  i;
-
-	spin_lock_irqsave(&ieee->lock, flags);
-
-	/* called with 2nd parm 0, no tx mgmt lock required */
-	ieee80211_sta_wakeup(ieee, 0);
-
-	for (i = 0; i < txb->nr_frags; i++) {
-		if (ieee->queue_stop) {
-			ieee->tx_pending.txb = txb;
-			ieee->tx_pending.frag = i;
-			goto exit;
-		} else {
-			ieee->softmac_data_hard_start_xmit(
-				txb->fragments[i],
-				ieee->dev, ieee->rate);
-			ieee->stats.tx_packets++;
-			ieee->stats.tx_bytes += txb->fragments[i]->len;
-			ieee->dev->trans_start = jiffies;
-		}
-	}
-
-	ieee80211_txb_free(txb);
-
-	exit:
-	spin_unlock_irqrestore(&ieee->lock, flags);
-}
-
-/* called with ieee->lock acquired */
-static void ieee80211_resume_tx(struct ieee80211_device *ieee)
-{
-	int i;
-	for (i = ieee->tx_pending.frag; i < ieee->tx_pending.txb->nr_frags; i++) {
-
-		if (ieee->queue_stop) {
-			ieee->tx_pending.frag = i;
-			return;
-		} else {
-			ieee->softmac_data_hard_start_xmit(
-				ieee->tx_pending.txb->fragments[i],
-				ieee->dev, ieee->rate);
-			ieee->stats.tx_packets++;
-			ieee->dev->trans_start = jiffies;
-		}
-	}
-
-	ieee80211_txb_free(ieee->tx_pending.txb);
-	ieee->tx_pending.txb = NULL;
-}
-
-void ieee80211_reset_queue(struct ieee80211_device *ieee)
-{
-	unsigned long flags;
-
-	spin_lock_irqsave(&ieee->lock, flags);
-	init_mgmt_queue(ieee);
-	if (ieee->tx_pending.txb) {
-		ieee80211_txb_free(ieee->tx_pending.txb);
-		ieee->tx_pending.txb = NULL;
-	}
-	ieee->queue_stop = 0;
-	spin_unlock_irqrestore(&ieee->lock, flags);
-}
-
-void ieee80211_rtl_wake_queue(struct ieee80211_device *ieee)
-{
-	unsigned long flags;
-	struct sk_buff *skb;
-	struct ieee80211_hdr_3addr  *header;
-
-	spin_lock_irqsave(&ieee->lock, flags);
-	if (!ieee->queue_stop)
-		goto exit;
-
-	ieee->queue_stop = 0;
-
-	if (ieee->softmac_features & IEEE_SOFTMAC_SINGLE_QUEUE) {
-		while (!ieee->queue_stop && (skb = dequeue_mgmt(ieee))) {
-			header = (struct ieee80211_hdr_3addr  *) skb->data;
-
-			header->seq_ctrl = cpu_to_le16(ieee->seq_ctrl[0] << 4);
-
-			if (ieee->seq_ctrl[0] == 0xFFF)
-				ieee->seq_ctrl[0] = 0;
-			else
-				ieee->seq_ctrl[0]++;
-
-			ieee->softmac_data_hard_start_xmit(skb, ieee->dev, ieee->basic_rate);
-			dev_kfree_skb_any(skb);
-		}
-	}
-	if (!ieee->queue_stop && ieee->tx_pending.txb)
-		ieee80211_resume_tx(ieee);
-
-	if (!ieee->queue_stop && netif_queue_stopped(ieee->dev)) {
-		ieee->softmac_stats.swtxawake++;
-		netif_wake_queue(ieee->dev);
-	}
-exit:
-	spin_unlock_irqrestore(&ieee->lock, flags);
-}
-
-void ieee80211_rtl_stop_queue(struct ieee80211_device *ieee)
-{
-	if (!netif_queue_stopped(ieee->dev)) {
-		netif_stop_queue(ieee->dev);
-		ieee->softmac_stats.swtxstop++;
-	}
-	ieee->queue_stop = 1;
-}
-
-inline void ieee80211_randomize_cell(struct ieee80211_device *ieee)
-{
-	random_ether_addr(ieee->current_network.bssid);
-}
-
-/* called in user context only */
-void ieee80211_start_master_bss(struct ieee80211_device *ieee)
-{
-	ieee->assoc_id = 1;
-
-	if (ieee->current_network.ssid_len == 0) {
-		strncpy(ieee->current_network.ssid,
-			IEEE80211_DEFAULT_TX_ESSID,
-			IW_ESSID_MAX_SIZE);
-
-		ieee->current_network.ssid_len = strlen(IEEE80211_DEFAULT_TX_ESSID);
-		ieee->ssid_set = 1;
-	}
-
-	memcpy(ieee->current_network.bssid, ieee->dev->dev_addr, ETH_ALEN);
-
-	ieee->set_chan(ieee->dev, ieee->current_network.channel);
-	ieee->state = IEEE80211_LINKED;
-	ieee->link_change(ieee->dev);
-	notify_wx_assoc_event(ieee);
-
-	if (ieee->data_hard_resume)
-		ieee->data_hard_resume(ieee->dev);
-
-	netif_carrier_on(ieee->dev);
-}
-
-static void ieee80211_start_monitor_mode(struct ieee80211_device *ieee)
-{
-	if (ieee->raw_tx) {
-
-		if (ieee->data_hard_resume)
-			ieee->data_hard_resume(ieee->dev);
-
-		netif_carrier_on(ieee->dev);
-	}
-}
-
-static void ieee80211_start_ibss_wq(struct work_struct *work)
-{
-	struct delayed_work *dwork = to_delayed_work(work);
-	struct ieee80211_device *ieee = container_of(dwork, struct ieee80211_device, start_ibss_wq);
-
-	/* iwconfig mode ad-hoc will schedule this and return
-	 * on the other hand this will block further iwconfig SET
-	 * operations because of the wx_sem hold.
-	 * Anyway some most set operations set a flag to speed-up
-	 * (abort) this wq (when syncro scanning) before sleeping
-	 * on the semaphore
-	 */
-
-	down(&ieee->wx_sem);
-
-	if (ieee->current_network.ssid_len == 0) {
-		strcpy(ieee->current_network.ssid, IEEE80211_DEFAULT_TX_ESSID);
-		ieee->current_network.ssid_len = strlen(IEEE80211_DEFAULT_TX_ESSID);
-		ieee->ssid_set = 1;
-	}
-
-	/* check if we have this cell in our network list */
-	ieee80211_softmac_check_all_nets(ieee);
-
-	if (ieee->state == IEEE80211_NOLINK)
-		ieee->current_network.channel = 10;
-	/* if not then the state is not linked. Maybe the user switched to
-	 * ad-hoc mode just after being in monitor mode, or just after
-	 * being very few time in managed mode (so the card have had no
-	 * time to scan all the chans..) or we have just run up the iface
-	 * after setting ad-hoc mode. So we have to give another try..
-	 * Here, in ibss mode, should be safe to do this without extra care
-	 * (in bss mode we had to make sure no-one tried to associate when
-	 * we had just checked the ieee->state and we was going to start the
-	 * scan) because in ibss mode the ieee80211_new_net function, when
-	 * finds a good net, just set the ieee->state to IEEE80211_LINKED,
-	 * so, at worst, we waste a bit of time to initiate an unneeded syncro
-	 * scan, that will stop at the first round because it sees the state
-	 * associated.
-	 */
-	if (ieee->state == IEEE80211_NOLINK)
-		ieee80211_start_scan_syncro(ieee);
-
-	/* the network definitively is not here.. create a new cell */
-	if (ieee->state == IEEE80211_NOLINK) {
-		printk("creating new IBSS cell\n");
-		if (!ieee->wap_set)
-			ieee80211_randomize_cell(ieee);
-
-		if (ieee->modulation & IEEE80211_CCK_MODULATION) {
-			ieee->current_network.rates_len = 4;
-
-			ieee->current_network.rates[0] = IEEE80211_BASIC_RATE_MASK | IEEE80211_CCK_RATE_1MB;
-			ieee->current_network.rates[1] = IEEE80211_BASIC_RATE_MASK | IEEE80211_CCK_RATE_2MB;
-			ieee->current_network.rates[2] = IEEE80211_BASIC_RATE_MASK | IEEE80211_CCK_RATE_5MB;
-			ieee->current_network.rates[3] = IEEE80211_BASIC_RATE_MASK | IEEE80211_CCK_RATE_11MB;
-
-		} else
-			ieee->current_network.rates_len = 0;
-
-		if (ieee->modulation & IEEE80211_OFDM_MODULATION) {
-			ieee->current_network.rates_ex_len = 8;
-
-			ieee->current_network.rates_ex[0] = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_6MB;
-			ieee->current_network.rates_ex[1] = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_9MB;
-			ieee->current_network.rates_ex[2] = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_12MB;
-			ieee->current_network.rates_ex[3] = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_18MB;
-			ieee->current_network.rates_ex[4] = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_24MB;
-			ieee->current_network.rates_ex[5] = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_36MB;
-			ieee->current_network.rates_ex[6] = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_48MB;
-			ieee->current_network.rates_ex[7] = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_54MB;
-
-			ieee->rate = 540;
-		} else {
-			ieee->current_network.rates_ex_len = 0;
-			ieee->rate = 110;
-		}
-
-		/* By default, WMM function will be disabled in IBSS mode */
-		ieee->current_network.QoS_Enable = 0;
-
-		ieee->current_network.atim_window = 0;
-		ieee->current_network.capability = WLAN_CAPABILITY_IBSS;
-		if (ieee->short_slot)
-			ieee->current_network.capability |= WLAN_CAPABILITY_SHORT_SLOT;
-	}
-
-	ieee->state = IEEE80211_LINKED;
-	ieee->set_chan(ieee->dev, ieee->current_network.channel);
-	ieee->link_change(ieee->dev);
-
-	notify_wx_assoc_event(ieee);
-
-	ieee80211_start_send_beacons(ieee);
-	printk(KERN_WARNING "after sending beacon packet!\n");
-
-	if (ieee->data_hard_resume)
-		ieee->data_hard_resume(ieee->dev);
-
-	netif_carrier_on(ieee->dev);
-
-	up(&ieee->wx_sem);
-}
-
-inline void ieee80211_start_ibss(struct ieee80211_device *ieee)
-{
-	queue_delayed_work(ieee->wq, &ieee->start_ibss_wq, 100);
-}
-
-/* this is called only in user context, with wx_sem held */
-void ieee80211_start_bss(struct ieee80211_device *ieee)
-{
-	unsigned long flags;
-	/* Ref: 802.11d 11.1.3.3
-	 * STA shall not start a BSS unless properly formed Beacon frame
-	 * including a Country IE.
-	 */
-	if (IS_DOT11D_ENABLE(ieee) && !IS_COUNTRY_IE_VALID(ieee)) {
-		if (!ieee->bGlobalDomain)
-			return;
-	}
-	/* check if we have already found the net we are interested in (if any).
-	 * if not (we are disassociated and we are not
-	 * in associating / authenticating phase) start the background scanning.
-	 */
-	ieee80211_softmac_check_all_nets(ieee);
-
-	/* ensure no-one start an associating process (thus setting
-	 * the ieee->state to ieee80211_ASSOCIATING) while we
-	 * have just cheked it and we are going to enable scan.
-	 * The ieee80211_new_net function is always called with
-	 * lock held (from both ieee80211_softmac_check_all_nets and
-	 * the rx path), so we cannot be in the middle of such function
-	 */
-	spin_lock_irqsave(&ieee->lock, flags);
-
-	if (ieee->state == IEEE80211_NOLINK) {
-		ieee->actscanning = true;
-		ieee80211_rtl_start_scan(ieee);
-	}
-	spin_unlock_irqrestore(&ieee->lock, flags);
-}
-
-/* called only in userspace context */
-void ieee80211_disassociate(struct ieee80211_device *ieee)
-{
-	netif_carrier_off(ieee->dev);
-
-	if (ieee->softmac_features & IEEE_SOFTMAC_TX_QUEUE)
-			ieee80211_reset_queue(ieee);
-
-	if (ieee->data_hard_stop)
-			ieee->data_hard_stop(ieee->dev);
-
-	if (IS_DOT11D_ENABLE(ieee))
-		Dot11d_Reset(ieee);
-
-	ieee->link_change(ieee->dev);
-	if (ieee->state == IEEE80211_LINKED)
-		notify_wx_assoc_event(ieee);
-	ieee->state = IEEE80211_NOLINK;
-
-}
-static void ieee80211_associate_retry_wq(struct work_struct *work)
-{
-	struct delayed_work *dwork = to_delayed_work(work);
-	struct ieee80211_device *ieee = container_of(dwork, struct ieee80211_device, associate_retry_wq);
-	unsigned long flags;
-	down(&ieee->wx_sem);
-	if (!ieee->proto_started)
-		goto exit;
-	if (ieee->state != IEEE80211_ASSOCIATING_RETRY)
-		goto exit;
-	/* until we do not set the state to IEEE80211_NOLINK
-	* there are no possibility to have someone else trying
-	* to start an association procedure (we get here with
-	* ieee->state = IEEE80211_ASSOCIATING).
-	* When we set the state to IEEE80211_NOLINK it is possible
-	* that the RX path run an attempt to associate, but
-	* both ieee80211_softmac_check_all_nets and the
-	* RX path works with ieee->lock held so there are no
-	* problems. If we are still disassociated then start a scan.
-	* the lock here is necessary to ensure no one try to start
-	* an association procedure when we have just checked the
-	* state and we are going to start the scan.
-	*/
-	ieee->state = IEEE80211_NOLINK;
-	ieee->beinretry = true;
-	ieee80211_softmac_check_all_nets(ieee);
-
-	spin_lock_irqsave(&ieee->lock, flags);
-
-	if (ieee->state == IEEE80211_NOLINK) {
-		ieee->beinretry = false;
-		ieee->actscanning = true;
-		ieee80211_rtl_start_scan(ieee);
-	}
-	if (ieee->state == IEEE80211_NOLINK)
-		notify_wx_assoc_event(ieee);
-	spin_unlock_irqrestore(&ieee->lock, flags);
-
-exit:
-	up(&ieee->wx_sem);
-}
-
-struct sk_buff *ieee80211_get_beacon_(struct ieee80211_device *ieee)
-{
-	u8 broadcast_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
-
-	struct sk_buff *skb = NULL;
-	struct ieee80211_probe_response *b;
-
-	skb = ieee80211_probe_resp(ieee, broadcast_addr);
-	if (!skb)
-		return NULL;
-
-	b = (struct ieee80211_probe_response *) skb->data;
-	b->header.frame_ctl = cpu_to_le16(IEEE80211_STYPE_BEACON);
-
-	return skb;
-}
-
-struct sk_buff *ieee80211_get_beacon(struct ieee80211_device *ieee)
-{
-	struct sk_buff *skb;
-	struct ieee80211_probe_response *b;
-
-	skb = ieee80211_get_beacon_(ieee);
-	if (!skb)
-		return NULL;
-
-	b = (struct ieee80211_probe_response *) skb->data;
-	b->header.seq_ctrl = cpu_to_le16(ieee->seq_ctrl[0] << 4);
-
-	if (ieee->seq_ctrl[0] == 0xFFF)
-		ieee->seq_ctrl[0] = 0;
-	else
-		ieee->seq_ctrl[0]++;
-
-	return skb;
-}
-
-void ieee80211_softmac_stop_protocol(struct ieee80211_device *ieee)
-{
-	ieee->sync_scan_hurryup = 1;
-	down(&ieee->wx_sem);
-	ieee80211_stop_protocol(ieee);
-	up(&ieee->wx_sem);
-}
-
-void ieee80211_stop_protocol(struct ieee80211_device *ieee)
-{
-	if (!ieee->proto_started)
-		return;
-
-	ieee->proto_started = 0;
-
-	ieee80211_stop_send_beacons(ieee);
-	if ((ieee->iw_mode == IW_MODE_INFRA) && (ieee->state == IEEE80211_LINKED))
-		SendDisassociation(ieee, NULL, WLAN_REASON_DISASSOC_STA_HAS_LEFT);
-
-	del_timer_sync(&ieee->associate_timer);
-	cancel_delayed_work(&ieee->associate_retry_wq);
-	cancel_delayed_work(&ieee->start_ibss_wq);
-	ieee80211_stop_scan(ieee);
-
-	ieee80211_disassociate(ieee);
-}
-
-void ieee80211_softmac_start_protocol(struct ieee80211_device *ieee)
-{
-	ieee->sync_scan_hurryup = 0;
-	down(&ieee->wx_sem);
-	ieee80211_start_protocol(ieee);
-	up(&ieee->wx_sem);
-}
-
-void ieee80211_start_protocol(struct ieee80211_device *ieee)
-{
-	short ch = 0;
-	int i = 0;
-
-	if (ieee->proto_started)
-		return;
-
-	ieee->proto_started = 1;
-
-	if (ieee->current_network.channel == 0) {
-		do {
-			ch++;
-			if (ch > MAX_CHANNEL_NUMBER)
-				return; /* no channel found */
-
-		} while (!GET_DOT11D_INFO(ieee)->channel_map[ch]);
-
-		ieee->current_network.channel = ch;
-	}
-
-	if (ieee->current_network.beacon_interval == 0)
-		ieee->current_network.beacon_interval = 100;
-	ieee->set_chan(ieee->dev, ieee->current_network.channel);
-
-	for (i = 0; i < 17; i++) {
-		ieee->last_rxseq_num[i] = -1;
-		ieee->last_rxfrag_num[i] = -1;
-		ieee->last_packet_time[i] = 0;
-	}
-
-	ieee->init_wmmparam_flag = 0; /* reinitialize AC_xx_PARAM registers. */
-
-	/* if the user set the MAC of the ad-hoc cell and then
-	 * switch to managed mode, shall we  make sure that association
-	 * attempts does not fail just because the user provide the essid
-	 * and the nic is still checking for the AP MAC ??
-	 */
-	switch (ieee->iw_mode) {
-		case IW_MODE_AUTO:
-			ieee->iw_mode = IW_MODE_INFRA;
-			/* not set break here intentionly */
-		case IW_MODE_INFRA:
-			ieee80211_start_bss(ieee);
-			break;
-
-		case IW_MODE_ADHOC:
-			ieee80211_start_ibss(ieee);
-			break;
-
-		case IW_MODE_MASTER:
-			ieee80211_start_master_bss(ieee);
-		break;
-
-		case IW_MODE_MONITOR:
-			ieee80211_start_monitor_mode(ieee);
-			break;
-
-		default:
-			ieee->iw_mode = IW_MODE_INFRA;
-			ieee80211_start_bss(ieee);
-			break;
-	}
-}
-
-#define DRV_NAME  "Ieee80211"
-void ieee80211_softmac_init(struct ieee80211_device *ieee)
-{
-	int i;
-	memset(&ieee->current_network, 0, sizeof(struct ieee80211_network));
-
-	ieee->state = IEEE80211_NOLINK;
-	ieee->sync_scan_hurryup = 0;
-	for (i = 0; i < 5; i++)
-		ieee->seq_ctrl[i] = 0;
-
-	ieee->assoc_id = 0;
-	ieee->queue_stop = 0;
-	ieee->scanning = 0;
-	ieee->softmac_features = 0; /* so IEEE2100-like driver are happy */
-	ieee->wap_set = 0;
-	ieee->ssid_set = 0;
-	ieee->proto_started = 0;
-	ieee->basic_rate = IEEE80211_DEFAULT_BASIC_RATE;
-	ieee->rate = 3;
-	ieee->ps = IEEE80211_PS_MBCAST|IEEE80211_PS_UNICAST;
-	ieee->sta_sleep = 0;
-	ieee->bInactivePs = false;
-	ieee->actscanning = false;
-	ieee->ListenInterval = 2;
-	ieee->NumRxDataInPeriod = 0;
-	ieee->NumRxBcnInPeriod = 0;
-	ieee->NumRxOkTotal = 0;
-	ieee->NumRxUnicast = 0; /* for keep alive */
-	ieee->beinretry = false;
-	ieee->bHwRadioOff = false;
-
-	init_mgmt_queue(ieee);
-
-	ieee->tx_pending.txb = NULL;
-
-	init_timer(&ieee->associate_timer);
-	ieee->associate_timer.data = (unsigned long)ieee;
-	ieee->associate_timer.function = ieee80211_associate_abort_cb;
-
-	init_timer(&ieee->beacon_timer);
-	ieee->beacon_timer.data = (unsigned long) ieee;
-	ieee->beacon_timer.function = ieee80211_send_beacon_cb;
-
-	ieee->wq = create_workqueue(DRV_NAME);
-
-	INIT_DELAYED_WORK(&ieee->start_ibss_wq, (void *) ieee80211_start_ibss_wq);
-	INIT_WORK(&ieee->associate_complete_wq, (void *) ieee80211_associate_complete_wq);
-	INIT_WORK(&ieee->associate_procedure_wq, (void *) ieee80211_associate_procedure_wq);
-	INIT_DELAYED_WORK(&ieee->softmac_scan_wq, (void *) ieee80211_softmac_scan_wq);
-	INIT_DELAYED_WORK(&ieee->associate_retry_wq, (void *) ieee80211_associate_retry_wq);
-	INIT_WORK(&ieee->wx_sync_scan_wq, (void *) ieee80211_wx_sync_scan_wq);
-
-	sema_init(&ieee->wx_sem, 1);
-	sema_init(&ieee->scan_sem, 1);
-
-	spin_lock_init(&ieee->mgmt_tx_lock);
-	spin_lock_init(&ieee->beacon_lock);
-
-	tasklet_init(&ieee->ps_task,
-	     (void(*)(unsigned long)) ieee80211_sta_ps,
-	     (unsigned long)ieee);
-	ieee->pDot11dInfo = kmalloc(sizeof(RT_DOT11D_INFO), GFP_ATOMIC);
-}
-
-void ieee80211_softmac_free(struct ieee80211_device *ieee)
-{
-	down(&ieee->wx_sem);
-
-	del_timer_sync(&ieee->associate_timer);
-	cancel_delayed_work(&ieee->associate_retry_wq);
-
-	/* add for RF power on power of */
-	cancel_delayed_work(&ieee->GPIOChangeRFWorkItem);
-
-	destroy_workqueue(ieee->wq);
-	kfree(ieee->pDot11dInfo);
-	up(&ieee->wx_sem);
-}
-
-/* Start of WPA code. This is stolen from the ipw2200 driver  */
-static int ieee80211_wpa_enable(struct ieee80211_device *ieee, int value)
-{
-	/* This is called when wpa_supplicant loads and closes the driver
-	 * interface. */
-	printk("%s WPA\n", value ? "enabling" : "disabling");
-	ieee->wpa_enabled = value;
-	return 0;
-}
-
-static void ieee80211_wpa_assoc_frame(struct ieee80211_device *ieee, char *wpa_ie,
-			       int wpa_ie_len)
-{
-	/* make sure WPA is enabled */
-	ieee80211_wpa_enable(ieee, 1);
-
-	ieee80211_disassociate(ieee);
-}
-
-static int ieee80211_wpa_mlme(struct ieee80211_device *ieee, int command,
-			      int reason)
-{
-	int ret = 0;
-
-	switch (command) {
-	case IEEE_MLME_STA_DEAUTH:
-		/* silently ignore */
-		break;
-
-	case IEEE_MLME_STA_DISASSOC:
-		ieee80211_disassociate(ieee);
-		break;
-
-	default:
-		printk("Unknown MLME request: %d\n", command);
-		ret = -EOPNOTSUPP;
-	}
-
-	return ret;
-}
-
-static int ieee80211_wpa_set_wpa_ie(struct ieee80211_device *ieee,
-				    struct ieee_param *param, int plen)
-{
-	u8 *buf;
-
-	if (param->u.wpa_ie.len > MAX_WPA_IE_LEN ||
-	    (param->u.wpa_ie.len && param->u.wpa_ie.data == NULL))
-		return -EINVAL;
-
-	if (param->u.wpa_ie.len) {
-		buf = kmemdup(param->u.wpa_ie.data, param->u.wpa_ie.len,
-			      GFP_KERNEL);
-		if (buf == NULL)
-			return -ENOMEM;
-
-		kfree(ieee->wpa_ie);
-		ieee->wpa_ie = buf;
-		ieee->wpa_ie_len = param->u.wpa_ie.len;
-	} else {
-		kfree(ieee->wpa_ie);
-		ieee->wpa_ie = NULL;
-		ieee->wpa_ie_len = 0;
-	}
-
-	ieee80211_wpa_assoc_frame(ieee, ieee->wpa_ie, ieee->wpa_ie_len);
-	return 0;
-}
-
-#define AUTH_ALG_OPEN_SYSTEM			0x1
-#define AUTH_ALG_SHARED_KEY			0x2
-
-static int ieee80211_wpa_set_auth_algs(struct ieee80211_device *ieee, int value)
-{
-	struct ieee80211_security sec = {
-		.flags = SEC_AUTH_MODE,
-	};
-	int ret = 0;
-
-	if (value & AUTH_ALG_SHARED_KEY) {
-		sec.auth_mode = WLAN_AUTH_SHARED_KEY;
-		ieee->open_wep = 0;
-	} else {
-		sec.auth_mode = WLAN_AUTH_OPEN;
-		ieee->open_wep = 1;
-	}
-
-	if (ieee->set_security)
-		ieee->set_security(ieee->dev, &sec);
-	else
-		ret = -EOPNOTSUPP;
-
-	return ret;
-}
-
-static int ieee80211_wpa_set_param(struct ieee80211_device *ieee, u8 name,
-				   u32 value)
-{
-	int ret = 0;
-	unsigned long flags;
-
-	switch (name) {
-	case IEEE_PARAM_WPA_ENABLED:
-		ret = ieee80211_wpa_enable(ieee, value);
-		break;
-
-	case IEEE_PARAM_TKIP_COUNTERMEASURES:
-		ieee->tkip_countermeasures = value;
-		break;
-
-	case IEEE_PARAM_DROP_UNENCRYPTED: {
-		/* HACK:
-		 *
-		 * wpa_supplicant calls set_wpa_enabled when the driver
-		 * is loaded and unloaded, regardless of if WPA is being
-		 * used.  No other calls are made which can be used to
-		 * determine if encryption will be used or not prior to
-		 * association being expected.  If encryption is not being
-		 * used, drop_unencrypted is set to false, else true -- we
-		 * can use this to determine if the CAP_PRIVACY_ON bit should
-		 * be set.
-		 */
-		struct ieee80211_security sec = {
-			.flags = SEC_ENABLED,
-			.enabled = value,
-		};
-		ieee->drop_unencrypted = value;
-		/* We only change SEC_LEVEL for open mode. Others
-		 * are set by ipw_wpa_set_encryption.
-		 */
-		if (!value) {
-			sec.flags |= SEC_LEVEL;
-			sec.level = SEC_LEVEL_0;
-		} else {
-			sec.flags |= SEC_LEVEL;
-			sec.level = SEC_LEVEL_1;
-		}
-		if (ieee->set_security)
-			ieee->set_security(ieee->dev, &sec);
-		break;
-	}
-
-	case IEEE_PARAM_PRIVACY_INVOKED:
-		ieee->privacy_invoked = value;
-		break;
-	case IEEE_PARAM_AUTH_ALGS:
-		ret = ieee80211_wpa_set_auth_algs(ieee, value);
-		break;
-	case IEEE_PARAM_IEEE_802_1X:
-		ieee->ieee802_1x = value;
-		break;
-	case IEEE_PARAM_WPAX_SELECT:
-		spin_lock_irqsave(&ieee->wpax_suitlist_lock, flags);
-		ieee->wpax_type_set = 1;
-		ieee->wpax_type_notify = value;
-		spin_unlock_irqrestore(&ieee->wpax_suitlist_lock, flags);
-		break;
-	default:
-		printk("Unknown WPA param: %d\n", name);
-		ret = -EOPNOTSUPP;
-	}
-
-	return ret;
-}
-
-/* implementation borrowed from hostap driver */
-
-static int ieee80211_wpa_set_encryption(struct ieee80211_device *ieee,
-					struct ieee_param *param, int param_len)
-{
-	int ret = 0;
-
-	struct ieee80211_crypto_ops *ops;
-	struct ieee80211_crypt_data **crypt;
-
-	struct ieee80211_security sec = {
-		.flags = 0,
-	};
-
-	param->u.crypt.err = 0;
-	param->u.crypt.alg[IEEE_CRYPT_ALG_NAME_LEN - 1] = '\0';
-
-	if (param_len !=
-	    (int) ((char *) param->u.crypt.key - (char *) param) +
-	    param->u.crypt.key_len) {
-		printk("Len mismatch %d, %d\n", param_len,
-			       param->u.crypt.key_len);
-		return -EINVAL;
-	}
-	if (is_broadcast_ether_addr(param->sta_addr)) {
-		if (param->u.crypt.idx >= WEP_KEYS)
-			return -EINVAL;
-		crypt = &ieee->crypt[param->u.crypt.idx];
-	} else {
-		return -EINVAL;
-	}
-
-	if (strcmp(param->u.crypt.alg, "none") == 0) {
-		if (crypt) {
-			sec.enabled = 0;
-			/* FIXME FIXME */
-			sec.level = SEC_LEVEL_0;
-			sec.flags |= SEC_ENABLED | SEC_LEVEL;
-			ieee80211_crypt_delayed_deinit(ieee, crypt);
-		}
-		goto done;
-	}
-	sec.enabled = 1;
-	/* FIXME FIXME */
-	sec.flags |= SEC_ENABLED;
-
-	/* IPW HW cannot build TKIP MIC, host decryption still needed. */
-	if (!(ieee->host_encrypt || ieee->host_decrypt) &&
-	    strcmp(param->u.crypt.alg, "TKIP"))
-		goto skip_host_crypt;
-
-	ops = ieee80211_get_crypto_ops(param->u.crypt.alg);
-	if (ops == NULL && strcmp(param->u.crypt.alg, "WEP") == 0)
-		ops = ieee80211_get_crypto_ops(param->u.crypt.alg);
-	else if (ops == NULL && strcmp(param->u.crypt.alg, "TKIP") == 0)
-		ops = ieee80211_get_crypto_ops(param->u.crypt.alg);
-	else if (ops == NULL && strcmp(param->u.crypt.alg, "CCMP") == 0)
-		ops = ieee80211_get_crypto_ops(param->u.crypt.alg);
-	if (ops == NULL) {
-		printk("unknown crypto alg '%s'\n", param->u.crypt.alg);
-		param->u.crypt.err = IEEE_CRYPT_ERR_UNKNOWN_ALG;
-		ret = -EINVAL;
-		goto done;
-	}
-
-	if (*crypt == NULL || (*crypt)->ops != ops) {
-		struct ieee80211_crypt_data *new_crypt;
-
-		ieee80211_crypt_delayed_deinit(ieee, crypt);
-
-		new_crypt = kmalloc(sizeof(*new_crypt), GFP_KERNEL);
-		if (new_crypt == NULL) {
-			ret = -ENOMEM;
-			goto done;
-		}
-		memset(new_crypt, 0, sizeof(struct ieee80211_crypt_data));
-		new_crypt->ops = ops;
-		if (new_crypt->ops)
-			new_crypt->priv =
-				new_crypt->ops->init(param->u.crypt.idx);
-
-		if (new_crypt->priv == NULL) {
-			kfree(new_crypt);
-			param->u.crypt.err = IEEE_CRYPT_ERR_CRYPT_INIT_FAILED;
-			ret = -EINVAL;
-			goto done;
-		}
-
-		*crypt = new_crypt;
-	}
-
-	if (param->u.crypt.key_len > 0 && (*crypt)->ops->set_key &&
-	    (*crypt)->ops->set_key(param->u.crypt.key,
-				   param->u.crypt.key_len, param->u.crypt.seq,
-				   (*crypt)->priv) < 0) {
-		printk("key setting failed\n");
-		param->u.crypt.err = IEEE_CRYPT_ERR_KEY_SET_FAILED;
-		ret = -EINVAL;
-		goto done;
-	}
-
- skip_host_crypt:
-	if (param->u.crypt.set_tx) {
-		ieee->tx_keyidx = param->u.crypt.idx;
-		sec.active_key = param->u.crypt.idx;
-		sec.flags |= SEC_ACTIVE_KEY;
-	} else
-		sec.flags &= ~SEC_ACTIVE_KEY;
-
-	if (param->u.crypt.alg != NULL) {
-		memcpy(sec.keys[param->u.crypt.idx],
-		       param->u.crypt.key,
-		       param->u.crypt.key_len);
-		sec.key_sizes[param->u.crypt.idx] = param->u.crypt.key_len;
-		sec.flags |= (1 << param->u.crypt.idx);
-
-		if (strcmp(param->u.crypt.alg, "WEP") == 0) {
-			sec.flags |= SEC_LEVEL;
-			sec.level = SEC_LEVEL_1;
-		} else if (strcmp(param->u.crypt.alg, "TKIP") == 0) {
-			sec.flags |= SEC_LEVEL;
-			sec.level = SEC_LEVEL_2;
-		} else if (strcmp(param->u.crypt.alg, "CCMP") == 0) {
-			sec.flags |= SEC_LEVEL;
-			sec.level = SEC_LEVEL_3;
-		}
-	}
- done:
-	if (ieee->set_security)
-		ieee->set_security(ieee->dev, &sec);
-
-	/* Do not reset port if card is in Managed mode since resetting will
-	 * generate new IEEE 802.11 authentication which may end up in looping
-	 * with IEEE 802.1X.  If your hardware requires a reset after WEP
-	 * configuration (for example... Prism2), implement the reset_port in
-	 * the callbacks structures used to initialize the 802.11 stack. */
-	if (ieee->reset_on_keychange &&
-	    ieee->iw_mode != IW_MODE_INFRA &&
-	    ieee->reset_port &&
-	    ieee->reset_port(ieee->dev)) {
-		printk("reset_port failed\n");
-		param->u.crypt.err = IEEE_CRYPT_ERR_CARD_CONF_FAILED;
-		return -EINVAL;
-	}
-
-	return ret;
-}
-
-int ieee80211_wpa_supplicant_ioctl(struct ieee80211_device *ieee,
-				   struct iw_point *p)
-{
-	struct ieee_param *param;
-	int ret = 0;
-
-	down(&ieee->wx_sem);
-
-	if (p->length < sizeof(struct ieee_param) || !p->pointer) {
-		ret = -EINVAL;
-		goto out;
-	}
-
-	param = memdup_user(p->pointer, p->length);
-	if (IS_ERR(param)) {
-		ret = PTR_ERR(param);
-		goto out;
-	}
-
-	switch (param->cmd) {
-	case IEEE_CMD_SET_WPA_PARAM:
-		ret = ieee80211_wpa_set_param(ieee, param->u.wpa_param.name,
-					param->u.wpa_param.value);
-		break;
-	case IEEE_CMD_SET_WPA_IE:
-		ret = ieee80211_wpa_set_wpa_ie(ieee, param, p->length);
-		break;
-	case IEEE_CMD_SET_ENCRYPTION:
-		ret = ieee80211_wpa_set_encryption(ieee, param, p->length);
-		break;
-	case IEEE_CMD_MLME:
-		ret = ieee80211_wpa_mlme(ieee, param->u.mlme.command,
-				   param->u.mlme.reason_code);
-		break;
-	default:
-		printk("Unknown WPA supplicant request: %d\n", param->cmd);
-		ret = -EOPNOTSUPP;
-		break;
-	}
-
-	if (ret == 0 && copy_to_user(p->pointer, param, p->length))
-		ret = -EFAULT;
-
-	kfree(param);
-out:
-	up(&ieee->wx_sem);
-
-	return ret;
-}
-
-void notify_wx_assoc_event(struct ieee80211_device *ieee)
-{
-	union iwreq_data wrqu;
-	wrqu.ap_addr.sa_family = ARPHRD_ETHER;
-	if (ieee->state == IEEE80211_LINKED)
-		memcpy(wrqu.ap_addr.sa_data, ieee->current_network.bssid, ETH_ALEN);
-	else
-		memset(wrqu.ap_addr.sa_data, 0, ETH_ALEN);
-	wireless_send_event(ieee->dev, SIOCGIWAP, &wrqu, NULL);
-}
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac_wx.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac_wx.c
deleted file mode 100644
index 46f35644126..00000000000
--- a/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac_wx.c
+++ /dev/null
@@ -1,567 +0,0 @@
-/* IEEE 802.11 SoftMAC layer
- * Copyright (c) 2005 Andrea Merello <andrea.merello@gmail.com>
- *
- * Mostly extracted from the rtl8180-sa2400 driver for the
- * in-kernel generic ieee802.11 stack.
- *
- * Some pieces of code might be stolen from ipw2100 driver
- * copyright of who own it's copyright ;-)
- *
- * PS wx handler mostly stolen from hostap, copyright who
- * own it's copyright ;-)
- *
- * released under the GPL
- */
-
-
-#include <linux/etherdevice.h>
-
-#include "ieee80211.h"
-
-/* FIXME: add A freqs */
-
-const long ieee80211_wlan_frequencies[] = {
-	2412, 2417, 2422, 2427,
-	2432, 2437, 2442, 2447,
-	2452, 2457, 2462, 2467,
-	2472, 2484
-};
-
-
-int ieee80211_wx_set_freq(struct ieee80211_device *ieee,
-			  struct iw_request_info *a, union iwreq_data *wrqu,
-			  char *b)
-{
-	int ret;
-	struct iw_freq *fwrq = &wrqu->freq;
-//	printk("in %s\n",__func__);
-	down(&ieee->wx_sem);
-
-	if (ieee->iw_mode == IW_MODE_INFRA) {
-		ret = -EOPNOTSUPP;
-		goto out;
-	}
-
-	/* if setting by freq convert to channel */
-	if (fwrq->e == 1) {
-		if ((fwrq->m >= (int) 2.412e8 &&
-		     fwrq->m <= (int) 2.487e8)) {
-			int f = fwrq->m / 100000;
-			int c = 0;
-
-			while ((c < 14) && (f != ieee80211_wlan_frequencies[c]))
-				c++;
-
-			/* hack to fall through */
-			fwrq->e = 0;
-			fwrq->m = c + 1;
-		}
-	}
-
-	if (fwrq->e > 0 || fwrq->m > 14 || fwrq->m < 1) {
-		ret = -EOPNOTSUPP;
-		goto out;
-
-	} else { /* Set the channel */
-
-
-		ieee->current_network.channel = fwrq->m;
-		ieee->set_chan(ieee->dev, ieee->current_network.channel);
-
-		if (ieee->iw_mode == IW_MODE_ADHOC || ieee->iw_mode == IW_MODE_MASTER)
-			if (ieee->state == IEEE80211_LINKED) {
-				ieee80211_stop_send_beacons(ieee);
-				ieee80211_start_send_beacons(ieee);
-			}
-	}
-
-	ret = 0;
-out:
-	up(&ieee->wx_sem);
-	return ret;
-}
-
-
-int ieee80211_wx_get_freq(struct ieee80211_device *ieee,
-			  struct iw_request_info *a, union iwreq_data *wrqu,
-			  char *b)
-{
-	struct iw_freq *fwrq = &wrqu->freq;
-
-	if (ieee->current_network.channel == 0)
-		return -1;
-
-	fwrq->m = ieee->current_network.channel;
-	fwrq->e = 0;
-
-	return 0;
-}
-
-int ieee80211_wx_get_wap(struct ieee80211_device *ieee,
-			 struct iw_request_info *info, union iwreq_data *wrqu,
-			 char *extra)
-{
-	unsigned long flags;
-
-	wrqu->ap_addr.sa_family = ARPHRD_ETHER;
-
-	if (ieee->iw_mode == IW_MODE_MONITOR)
-		return -1;
-
-	/* We want avoid to give to the user inconsistent infos*/
-	spin_lock_irqsave(&ieee->lock, flags);
-
-	if (ieee->state != IEEE80211_LINKED &&
-		ieee->state != IEEE80211_LINKED_SCANNING &&
-		ieee->wap_set == 0)
-
-		memset(wrqu->ap_addr.sa_data, 0, ETH_ALEN);
-	else
-		memcpy(wrqu->ap_addr.sa_data,
-		       ieee->current_network.bssid, ETH_ALEN);
-
-	spin_unlock_irqrestore(&ieee->lock, flags);
-
-	return 0;
-}
-
-
-int ieee80211_wx_set_wap(struct ieee80211_device *ieee,
-			 struct iw_request_info *info, union iwreq_data *awrq,
-			 char *extra)
-{
-
-	int ret = 0;
-	unsigned long flags;
-
-	short ifup = ieee->proto_started;//dev->flags & IFF_UP;
-	struct sockaddr *temp = (struct sockaddr *)awrq;
-
-	//printk("=======Set WAP:");
-	ieee->sync_scan_hurryup = 1;
-
-	down(&ieee->wx_sem);
-	/* use ifconfig hw ether */
-	if (ieee->iw_mode == IW_MODE_MASTER) {
-		ret = -1;
-		goto out;
-	}
-
-	if (temp->sa_family != ARPHRD_ETHER) {
-		ret = -EINVAL;
-		goto out;
-	}
-
-	if (ifup)
-		ieee80211_stop_protocol(ieee);
-
-	/* just to avoid to give inconsistent infos in the
-	 * get wx method. not really needed otherwise
-	 */
-	spin_lock_irqsave(&ieee->lock, flags);
-
-	memcpy(ieee->current_network.bssid, temp->sa_data, ETH_ALEN);
-	ieee->wap_set = !is_zero_ether_addr(temp->sa_data);
-	//printk(" %x:%x:%x:%x:%x:%x\n", ieee->current_network.bssid[0],ieee->current_network.bssid[1],ieee->current_network.bssid[2],ieee->current_network.bssid[3],ieee->current_network.bssid[4],ieee->current_network.bssid[5]);
-
-	spin_unlock_irqrestore(&ieee->lock, flags);
-
-	if (ifup)
-		ieee80211_start_protocol(ieee);
-
-out:
-	up(&ieee->wx_sem);
-	return ret;
-}
-
-int ieee80211_wx_get_essid(struct ieee80211_device *ieee,
-			   struct iw_request_info *a, union iwreq_data *wrqu,
-			   char *b)
-{
-	int len, ret = 0;
-	unsigned long flags;
-
-	if (ieee->iw_mode == IW_MODE_MONITOR)
-		return -1;
-
-	/* We want avoid to give to the user inconsistent infos*/
-	spin_lock_irqsave(&ieee->lock, flags);
-
-	if (ieee->current_network.ssid[0] == '\0' ||
-		ieee->current_network.ssid_len == 0){
-		ret = -1;
-		goto out;
-	}
-
-	if (ieee->state != IEEE80211_LINKED &&
-		ieee->state != IEEE80211_LINKED_SCANNING &&
-		ieee->ssid_set == 0){
-		ret = -1;
-		goto out;
-	}
-	len = ieee->current_network.ssid_len;
-	wrqu->essid.length = len;
-	strncpy(b, ieee->current_network.ssid, len);
-	wrqu->essid.flags = 1;
-
-out:
-	spin_unlock_irqrestore(&ieee->lock, flags);
-
-	return ret;
-
-}
-
-int ieee80211_wx_set_rate(struct ieee80211_device *ieee,
-			  struct iw_request_info *info, union iwreq_data *wrqu,
-			  char *extra)
-{
-
-	u32 target_rate = wrqu->bitrate.value;
-
-	//added by lizhaoming for auto mode
-	if (target_rate == -1)
-		ieee->rate = 110;
-	else
-		ieee->rate = target_rate/100000;
-
-	//FIXME: we might want to limit rate also in management protocols.
-	return 0;
-}
-
-
-
-int ieee80211_wx_get_rate(struct ieee80211_device *ieee,
-			  struct iw_request_info *info, union iwreq_data *wrqu,
-			  char *extra)
-{
-
-	wrqu->bitrate.value = ieee->rate * 100000;
-
-	return 0;
-}
-
-int ieee80211_wx_set_mode(struct ieee80211_device *ieee,
-			  struct iw_request_info *a, union iwreq_data *wrqu,
-			  char *b)
-{
-
-	ieee->sync_scan_hurryup = 1;
-
-	down(&ieee->wx_sem);
-
-	if (wrqu->mode == ieee->iw_mode)
-		goto out;
-
-	if (wrqu->mode == IW_MODE_MONITOR)
-		ieee->dev->type = ARPHRD_IEEE80211;
-	else
-		ieee->dev->type = ARPHRD_ETHER;
-
-	if (!ieee->proto_started) {
-		ieee->iw_mode = wrqu->mode;
-	} else {
-		ieee80211_stop_protocol(ieee);
-		ieee->iw_mode = wrqu->mode;
-		ieee80211_start_protocol(ieee);
-	}
-
-out:
-	up(&ieee->wx_sem);
-	return 0;
-}
-
-
-void ieee80211_wx_sync_scan_wq(struct work_struct *work)
-{
-	struct ieee80211_device *ieee = container_of(work, struct ieee80211_device, wx_sync_scan_wq);
-	short chan;
-
-	chan = ieee->current_network.channel;
-
-	if (ieee->data_hard_stop)
-		ieee->data_hard_stop(ieee->dev);
-
-	ieee80211_stop_send_beacons(ieee);
-
-	ieee->state = IEEE80211_LINKED_SCANNING;
-	ieee->link_change(ieee->dev);
-
-	ieee80211_start_scan_syncro(ieee);
-
-	ieee->set_chan(ieee->dev, chan);
-
-	ieee->state = IEEE80211_LINKED;
-	ieee->link_change(ieee->dev);
-
-	if (ieee->data_hard_resume)
-		ieee->data_hard_resume(ieee->dev);
-
-	if (ieee->iw_mode == IW_MODE_ADHOC || ieee->iw_mode == IW_MODE_MASTER)
-		ieee80211_start_send_beacons(ieee);
-
-	//YJ,add,080828, In prevent of lossing ping packet during scanning
-	//ieee80211_sta_ps_send_null_frame(ieee, false);
-	//YJ,add,080828,end
-
-	up(&ieee->wx_sem);
-
-}
-
-int ieee80211_wx_set_scan(struct ieee80211_device *ieee,
-			  struct iw_request_info *a, union iwreq_data *wrqu,
-			  char *b)
-{
-	int ret = 0;
-
-	down(&ieee->wx_sem);
-
-	if (ieee->iw_mode == IW_MODE_MONITOR || !(ieee->proto_started)) {
-		ret = -1;
-		goto out;
-	}
-	//YJ,add,080828
-	//In prevent of lossing ping packet during scanning
-	//ieee80211_sta_ps_send_null_frame(ieee, true);
-	//YJ,add,080828,end
-
-	if (ieee->state == IEEE80211_LINKED) {
-		queue_work(ieee->wq, &ieee->wx_sync_scan_wq);
-		/* intentionally forget to up sem */
-		return 0;
-	}
-
-out:
-	up(&ieee->wx_sem);
-	return ret;
-}
-
-int ieee80211_wx_set_essid(struct ieee80211_device *ieee,
-			   struct iw_request_info *a, union iwreq_data *wrqu,
-			   char *extra)
-{
-
-	int ret = 0, len;
-	short proto_started;
-	unsigned long flags;
-
-	ieee->sync_scan_hurryup = 1;
-
-	down(&ieee->wx_sem);
-
-	proto_started = ieee->proto_started;
-
-	if (wrqu->essid.length > IW_ESSID_MAX_SIZE) {
-		ret = -E2BIG;
-		goto out;
-	}
-
-	if (ieee->iw_mode == IW_MODE_MONITOR) {
-		ret = -1;
-		goto out;
-	}
-
-	if (proto_started)
-		ieee80211_stop_protocol(ieee);
-
-	/* this is just to be sure that the GET wx callback
-	 * has consistent infos. not needed otherwise
-	 */
-	spin_lock_irqsave(&ieee->lock, flags);
-
-	if (wrqu->essid.flags && wrqu->essid.length) {
-//YJ,modified,080819
-		len = (wrqu->essid.length < IW_ESSID_MAX_SIZE) ? (wrqu->essid.length) : IW_ESSID_MAX_SIZE;
-		memset(ieee->current_network.ssid, 0, ieee->current_network.ssid_len); //YJ,add,080819
-		strncpy(ieee->current_network.ssid, extra, len);
-		ieee->current_network.ssid_len = len;
-		ieee->ssid_set = 1;
-//YJ,modified,080819,end
-
-		//YJ,add,080819,for hidden ap
-		if (len == 0) {
-			memset(ieee->current_network.bssid, 0, ETH_ALEN);
-			ieee->current_network.capability = 0;
-		}
-		//YJ,add,080819,for hidden ap,end
-	} else {
-		ieee->ssid_set = 0;
-		ieee->current_network.ssid[0] = '\0';
-		ieee->current_network.ssid_len = 0;
-	}
-	//printk("==========set essid %s!\n",ieee->current_network.ssid);
-	spin_unlock_irqrestore(&ieee->lock, flags);
-
-	if (proto_started)
-		ieee80211_start_protocol(ieee);
-out:
-	up(&ieee->wx_sem);
-	return ret;
-}
-
-int ieee80211_wx_get_mode(struct ieee80211_device *ieee,
-			  struct iw_request_info *a, union iwreq_data *wrqu,
-			  char *b)
-{
-
-	wrqu->mode = ieee->iw_mode;
-	return 0;
-}
-
-int ieee80211_wx_set_rawtx(struct ieee80211_device *ieee,
-			   struct iw_request_info *info, union iwreq_data *wrqu,
-			   char *extra)
-{
-
-	int *parms = (int *)extra;
-	int enable = (parms[0] > 0);
-	short prev = ieee->raw_tx;
-
-	down(&ieee->wx_sem);
-
-	if (enable)
-		ieee->raw_tx = 1;
-	else
-		ieee->raw_tx = 0;
-
-	netdev_info(ieee->dev, "raw TX is %s\n",
-		    ieee->raw_tx ? "enabled" : "disabled");
-
-	if (ieee->iw_mode == IW_MODE_MONITOR) {
-		if (prev == 0 && ieee->raw_tx) {
-			if (ieee->data_hard_resume)
-				ieee->data_hard_resume(ieee->dev);
-
-			netif_carrier_on(ieee->dev);
-		}
-
-		if (prev && ieee->raw_tx == 1)
-			netif_carrier_off(ieee->dev);
-	}
-
-	up(&ieee->wx_sem);
-
-	return 0;
-}
-
-int ieee80211_wx_get_name(struct ieee80211_device *ieee,
-			  struct iw_request_info *info, union iwreq_data *wrqu,
-			  char *extra)
-{
-	strlcpy(wrqu->name, "802.11", IFNAMSIZ);
-	if (ieee->modulation & IEEE80211_CCK_MODULATION) {
-		strlcat(wrqu->name, "b", IFNAMSIZ);
-		if (ieee->modulation & IEEE80211_OFDM_MODULATION)
-			strlcat(wrqu->name, "/g", IFNAMSIZ);
-	} else if (ieee->modulation & IEEE80211_OFDM_MODULATION)
-		strlcat(wrqu->name, "g", IFNAMSIZ);
-
-	if ((ieee->state == IEEE80211_LINKED) ||
-		(ieee->state == IEEE80211_LINKED_SCANNING))
-		strlcat(wrqu->name, "  link", IFNAMSIZ);
-	else if (ieee->state != IEEE80211_NOLINK)
-		strlcat(wrqu->name, " .....", IFNAMSIZ);
-
-
-	return 0;
-}
-
-
-/* this is mostly stolen from hostap */
-int ieee80211_wx_set_power(struct ieee80211_device *ieee,
-			   struct iw_request_info *info, union iwreq_data *wrqu,
-			   char *extra)
-{
-	int ret = 0;
-
-	if ((!ieee->sta_wake_up) ||
-	    (!ieee->ps_request_tx_ack) ||
-	    (!ieee->enter_sleep_state) ||
-	    (!ieee->ps_is_queue_empty)) {
-
-		printk("ERROR. PS mode tried to be use but driver missed a callback\n\n");
-
-		return -1;
-	}
-
-	down(&ieee->wx_sem);
-
-	if (wrqu->power.disabled) {
-		ieee->ps = IEEE80211_PS_DISABLED;
-
-		goto exit;
-	}
-	switch (wrqu->power.flags & IW_POWER_MODE) {
-	case IW_POWER_UNICAST_R:
-		ieee->ps = IEEE80211_PS_UNICAST;
-
-		break;
-	case IW_POWER_ALL_R:
-		ieee->ps = IEEE80211_PS_UNICAST | IEEE80211_PS_MBCAST;
-		break;
-
-	case IW_POWER_ON:
-		ieee->ps = IEEE80211_PS_DISABLED;
-		break;
-
-	default:
-		ret = -EINVAL;
-		goto exit;
-	}
-
-	if (wrqu->power.flags & IW_POWER_TIMEOUT) {
-
-		ieee->ps_timeout = wrqu->power.value / 1000;
-		printk("Timeout %d\n", ieee->ps_timeout);
-	}
-
-	if (wrqu->power.flags & IW_POWER_PERIOD) {
-
-		ret = -EOPNOTSUPP;
-		goto exit;
-		//wrq->value / 1024;
-
-	}
-exit:
-	up(&ieee->wx_sem);
-	return ret;
-
-}
-
-/* this is stolen from hostap */
-int ieee80211_wx_get_power(struct ieee80211_device *ieee,
-			   struct iw_request_info *info, union iwreq_data *wrqu,
-			   char *extra)
-{
-	int ret = 0;
-
-	down(&ieee->wx_sem);
-
-	if (ieee->ps == IEEE80211_PS_DISABLED) {
-		wrqu->power.disabled = 1;
-		goto exit;
-	}
-
-	wrqu->power.disabled = 0;
-
-//	if ((wrqu->power.flags & IW_POWER_TYPE) == IW_POWER_TIMEOUT) {
-		wrqu->power.flags = IW_POWER_TIMEOUT;
-		wrqu->power.value = ieee->ps_timeout * 1000;
-//	} else {
-//		ret = -EOPNOTSUPP;
-//		goto exit;
-		//wrqu->power.flags = IW_POWER_PERIOD;
-		//wrqu->power.value = ieee->current_network.dtim_period *
-		//	ieee->current_network.beacon_interval * 1024;
-//	}
-
-
-	if (ieee->ps & IEEE80211_PS_MBCAST)
-		wrqu->power.flags |= IW_POWER_ALL_R;
-	else
-		wrqu->power.flags |= IW_POWER_UNICAST_R;
-
-exit:
-	up(&ieee->wx_sem);
-	return ret;
-
-}
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_tx.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_tx.c
deleted file mode 100644
index 0dc5ae41427..00000000000
--- a/drivers/staging/rtl8187se/ieee80211/ieee80211_tx.c
+++ /dev/null
@@ -1,591 +0,0 @@
-/******************************************************************************
-
-  Copyright(c) 2003 - 2004 Intel Corporation. All rights reserved.
-
-  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.
-
-  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.
-
-  The full GNU General Public License is included in this distribution in the
-  file called LICENSE.
-
-  Contact Information:
-  James P. Ketrenos <ipw2100-admin@linux.intel.com>
-  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
-
-******************************************************************************
-
-  Few modifications for Realtek's Wi-Fi drivers by
-  Andrea Merello <andrea.merello@gmail.com>
-
-  A special thanks goes to Realtek for their support !
-
-******************************************************************************/
-
-#include <linux/compiler.h>
-#include <linux/errno.h>
-#include <linux/if_arp.h>
-#include <linux/in6.h>
-#include <linux/in.h>
-#include <linux/ip.h>
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/netdevice.h>
-#include <linux/pci.h>
-#include <linux/proc_fs.h>
-#include <linux/skbuff.h>
-#include <linux/slab.h>
-#include <linux/tcp.h>
-#include <linux/types.h>
-#include <linux/wireless.h>
-#include <linux/etherdevice.h>
-#include <asm/uaccess.h>
-#include <linux/if_vlan.h>
-
-#include "ieee80211.h"
-
-
-/*
-
-
-802.11 Data Frame
-
-
-802.11 frame_contorl for data frames - 2 bytes
-     ,-----------------------------------------------------------------------------------------.
-bits | 0  |  1  |  2  |  3  |  4  |  5  |  6  |  7  |  8  |  9  |  a  |  b  |  c  |  d  |  e   |
-     |----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|------|
-val  | 0  |  0  |  0  |  1  |  x  |  0  |  0  |  0  |  1  |  0  |  x  |  x  |  x  |  x  |  x   |
-     |----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|------|
-desc | ^-ver-^  |  ^type-^  |  ^-----subtype-----^  | to  |from |more |retry| pwr |more |wep   |
-     |          |           | x=0 data,x=1 data+ack | DS  | DS  |frag |     | mgm |data |      |
-     '-----------------------------------------------------------------------------------------'
-		                                    /\
-                                                    |
-802.11 Data Frame                                   |
-           ,--------- 'ctrl' expands to >-----------'
-          |
-      ,--'---,-------------------------------------------------------------.
-Bytes |  2   |  2   |    6    |    6    |    6    |  2   | 0..2312 |   4  |
-      |------|------|---------|---------|---------|------|---------|------|
-Desc. | ctrl | dura |  DA/RA  |   TA    |    SA   | Sequ |  Frame  |  fcs |
-      |      | tion | (BSSID) |         |         | ence |  data   |      |
-      `--------------------------------------------------|         |------'
-Total: 28 non-data bytes                                 `----.----'
-                                                              |
-       .- 'Frame data' expands to <---------------------------'
-       |
-       V
-      ,---------------------------------------------------.
-Bytes |  1   |  1   |    1    |    3     |  2   |  0-2304 |
-      |------|------|---------|----------|------|---------|
-Desc. | SNAP | SNAP | Control |Eth Tunnel| Type | IP      |
-      | DSAP | SSAP |         |          |      | Packet  |
-      | 0xAA | 0xAA |0x03 (UI)|0x00-00-F8|      |         |
-      `-----------------------------------------|         |
-Total: 8 non-data bytes                         `----.----'
-                                                     |
-       .- 'IP Packet' expands, if WEP enabled, to <--'
-       |
-       V
-      ,-----------------------.
-Bytes |  4  |   0-2296  |  4  |
-      |-----|-----------|-----|
-Desc. | IV  | Encrypted | ICV |
-      |     | IP Packet |     |
-      `-----------------------'
-Total: 8 non-data bytes
-
-
-802.3 Ethernet Data Frame
-
-      ,-----------------------------------------.
-Bytes |   6   |   6   |  2   |  Variable |   4  |
-      |-------|-------|------|-----------|------|
-Desc. | Dest. | Source| Type | IP Packet |  fcs |
-      |  MAC  |  MAC  |      |           |      |
-      `-----------------------------------------'
-Total: 18 non-data bytes
-
-In the event that fragmentation is required, the incoming payload is split into
-N parts of size ieee->fts.  The first fragment contains the SNAP header and the
-remaining packets are just data.
-
-If encryption is enabled, each fragment payload size is reduced by enough space
-to add the prefix and postfix (IV and ICV totalling 8 bytes in the case of WEP)
-So if you have 1500 bytes of payload with ieee->fts set to 500 without
-encryption it will take 3 frames.  With WEP it will take 4 frames as the
-payload of each frame is reduced to 492 bytes.
-
-* SKB visualization
-*
-*  ,- skb->data
-* |
-* |    ETHERNET HEADER        ,-<-- PAYLOAD
-* |                           |     14 bytes from skb->data
-* |  2 bytes for Type --> ,T. |     (sizeof ethhdr)
-* |                       | | |
-* |,-Dest.--. ,--Src.---. | | |
-* |  6 bytes| | 6 bytes | | | |
-* v         | |         | | | |
-* 0         | v       1 | v | v           2
-* 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5
-*     ^     | ^         | ^ |
-*     |     | |         | | |
-*     |     | |         | `T' <---- 2 bytes for Type
-*     |     | |         |
-*     |     | '---SNAP--' <-------- 6 bytes for SNAP
-*     |     |
-*     `-IV--' <-------------------- 4 bytes for IV (WEP)
-*
-*      SNAP HEADER
-*
-*/
-
-static u8 P802_1H_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0xf8 };
-static u8 RFC1042_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0x00 };
-
-static inline int ieee80211_put_snap(u8 *data, u16 h_proto)
-{
-	struct ieee80211_snap_hdr *snap;
-	u8 *oui;
-
-	snap = (struct ieee80211_snap_hdr *)data;
-	snap->dsap = 0xaa;
-	snap->ssap = 0xaa;
-	snap->ctrl = 0x03;
-
-	if (h_proto == 0x8137 || h_proto == 0x80f3)
-		oui = P802_1H_OUI;
-	else
-		oui = RFC1042_OUI;
-	snap->oui[0] = oui[0];
-	snap->oui[1] = oui[1];
-	snap->oui[2] = oui[2];
-
-	*(u16 *)(data + SNAP_SIZE) = htons(h_proto);
-
-	return SNAP_SIZE + sizeof(u16);
-}
-
-int ieee80211_encrypt_fragment(struct ieee80211_device *ieee,
-			       struct sk_buff *frag, int hdr_len)
-{
-	struct ieee80211_crypt_data* crypt = ieee->crypt[ieee->tx_keyidx];
-	int res;
-
-	/*
-	 * added to care about null crypt condition, to solve that system hangs
-	 * when shared keys error
-	 */
-	if (!crypt || !crypt->ops)
-		return -1;
-
-#ifdef CONFIG_IEEE80211_CRYPT_TKIP
-	struct ieee80211_hdr_4addr *header;
-
-	if (ieee->tkip_countermeasures &&
-	    crypt && crypt->ops && strcmp(crypt->ops->name, "TKIP") == 0) {
-		header = (struct ieee80211_hdr_4addr *)frag->data;
-		if (net_ratelimit()) {
-			netdev_dbg(ieee->dev, "TKIP countermeasures: dropped "
-			       "TX packet to %pM\n", header->addr1);
-		}
-		return -1;
-	}
-#endif
-	/*
-	 * To encrypt, frame format is:
-	 * IV (4 bytes), clear payload (including SNAP), ICV (4 bytes)
-	 *
-	 * PR: FIXME: Copied from hostap. Check fragmentation/MSDU/MPDU
-	 * encryption.
-	 *
-	 * Host-based IEEE 802.11 fragmentation for TX is not yet supported, so
-	 * call both MSDU and MPDU encryption functions from here.
-	 */
-	atomic_inc(&crypt->refcnt);
-	res = 0;
-	if (crypt->ops->encrypt_msdu)
-		res = crypt->ops->encrypt_msdu(frag, hdr_len, crypt->priv);
-	if (res == 0 && crypt->ops->encrypt_mpdu)
-		res = crypt->ops->encrypt_mpdu(frag, hdr_len, crypt->priv);
-
-	atomic_dec(&crypt->refcnt);
-	if (res < 0) {
-		netdev_info(ieee->dev, "Encryption failed: len=%d.\n", frag->len);
-		ieee->ieee_stats.tx_discards++;
-		return -1;
-	}
-
-	return 0;
-}
-
-
-void ieee80211_txb_free(struct ieee80211_txb *txb)
-{
-	int i;
-	if (unlikely(!txb))
-		return;
-	for (i = 0; i < txb->nr_frags; i++)
-		if (txb->fragments[i])
-			dev_kfree_skb_any(txb->fragments[i]);
-	kfree(txb);
-}
-
-static struct ieee80211_txb *ieee80211_alloc_txb(int nr_frags, int txb_size,
-						 gfp_t gfp_mask)
-{
-	struct ieee80211_txb *txb;
-	int i;
-	txb = kmalloc(
-		sizeof(struct ieee80211_txb) + (sizeof(u8 *) * nr_frags),
-		gfp_mask);
-	if (!txb)
-		return NULL;
-
-	memset(txb, 0, sizeof(struct ieee80211_txb));
-	txb->nr_frags = nr_frags;
-	txb->frag_size = txb_size;
-
-	for (i = 0; i < nr_frags; i++) {
-		txb->fragments[i] = dev_alloc_skb(txb_size);
-		if (unlikely(!txb->fragments[i])) {
-			i--;
-			break;
-		}
-	}
-	if (unlikely(i != nr_frags)) {
-		while (i >= 0)
-			dev_kfree_skb_any(txb->fragments[i--]);
-		kfree(txb);
-		return NULL;
-	}
-	return txb;
-}
-
-/*
- * Classify the to-be send data packet
- * Need to acquire the sent queue index.
- */
-static int ieee80211_classify(struct sk_buff *skb,
-			      struct ieee80211_network *network)
-{
-	struct ether_header *eh = (struct ether_header *)skb->data;
-	unsigned int wme_UP = 0;
-
-	if (!network->QoS_Enable) {
-		skb->priority = 0;
-		return(wme_UP);
-	}
-
-	if (eh->ether_type == __constant_htons(ETHERTYPE_IP)) {
-		const struct iphdr *ih = (struct iphdr *)(skb->data +
-		    sizeof(struct ether_header));
-		wme_UP = (ih->tos >> 5)&0x07;
-	} else if (vlan_tx_tag_present(skb)) {/* vtag packet */
-#ifndef VLAN_PRI_SHIFT
-#define VLAN_PRI_SHIFT  13              /* Shift to find VLAN user priority */
-#define VLAN_PRI_MASK   7               /* Mask for user priority bits in VLAN */
-#endif
-		u32 tag = vlan_tx_tag_get(skb);
-		wme_UP = (tag >> VLAN_PRI_SHIFT) & VLAN_PRI_MASK;
-	} else if (ETH_P_PAE ==  ntohs(((struct ethhdr *)skb->data)->h_proto)) {
-		wme_UP = 7;
-	}
-
-	skb->priority = wme_UP;
-	return(wme_UP);
-}
-
-/* SKBs are added to the ieee->tx_queue. */
-int ieee80211_rtl_xmit(struct sk_buff *skb, struct net_device *dev)
-{
-	struct ieee80211_device *ieee = netdev_priv(dev);
-	struct ieee80211_txb *txb = NULL;
-	struct ieee80211_hdr_3addrqos *frag_hdr;
-	int i, bytes_per_frag, nr_frags, bytes_last_frag, frag_size;
-	unsigned long flags;
-	struct net_device_stats *stats = &ieee->stats;
-	int ether_type, encrypt;
-	int bytes, fc, qos_ctl, hdr_len;
-	struct sk_buff *skb_frag;
-	struct ieee80211_hdr_3addrqos header = { /* Ensure zero initialized */
-		.duration_id = 0,
-		.seq_ctl = 0,
-		.qos_ctl = 0
-	};
-	u8 dest[ETH_ALEN], src[ETH_ALEN];
-
-	struct ieee80211_crypt_data* crypt;
-
-	spin_lock_irqsave(&ieee->lock, flags);
-
-	/*
-	 * If there is no driver handler to take the TXB, don't bother
-	 * creating it...
-	 */
-	if ((!ieee->hard_start_xmit &&
-	     !(ieee->softmac_features & IEEE_SOFTMAC_TX_QUEUE)) ||
-	    ((!ieee->softmac_data_hard_start_xmit &&
-	      (ieee->softmac_features & IEEE_SOFTMAC_TX_QUEUE)))) {
-		netdev_warn(ieee->dev, "No xmit handler.\n");
-		goto success;
-	}
-
-	ieee80211_classify(skb,&ieee->current_network);
-	if (likely(ieee->raw_tx == 0)){
-
-		if (unlikely(skb->len < SNAP_SIZE + sizeof(u16))) {
-			netdev_warn(ieee->dev, "skb too small (%d).\n", skb->len);
-			goto success;
-		}
-
-		ether_type = ntohs(((struct ethhdr *)skb->data)->h_proto);
-
-		crypt = ieee->crypt[ieee->tx_keyidx];
-
-		encrypt = !(ether_type == ETH_P_PAE && ieee->ieee802_1x) &&
-			ieee->host_encrypt && crypt && crypt->ops;
-
-		if (!encrypt && ieee->ieee802_1x &&
-		ieee->drop_unencrypted && ether_type != ETH_P_PAE) {
-			stats->tx_dropped++;
-			goto success;
-		}
-
-	#ifdef CONFIG_IEEE80211_DEBUG
-		if (crypt && !encrypt && ether_type == ETH_P_PAE) {
-			struct eapol *eap = (struct eapol *)(skb->data +
-				sizeof(struct ethhdr) - SNAP_SIZE - sizeof(u16));
-			IEEE80211_DEBUG_EAP("TX: IEEE 802.11 EAPOL frame: %s\n",
-				eap_get_type(eap->type));
-		}
-	#endif
-
-		/* Save source and destination addresses */
-		memcpy(&dest, skb->data, ETH_ALEN);
-		memcpy(&src, skb->data+ETH_ALEN, ETH_ALEN);
-
-		/* Advance the SKB to the start of the payload */
-		skb_pull(skb, sizeof(struct ethhdr));
-
-		/* Determine total amount of storage required for TXB packets */
-		bytes = skb->len + SNAP_SIZE + sizeof(u16);
-
-		if (ieee->current_network.QoS_Enable) {
-			if (encrypt)
-				fc = IEEE80211_FTYPE_DATA | IEEE80211_STYPE_QOS_DATA |
-					IEEE80211_FCTL_WEP;
-			else
-				fc = IEEE80211_FTYPE_DATA | IEEE80211_STYPE_QOS_DATA;
-
-		} else {
-			if (encrypt)
-				fc = IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA |
-					IEEE80211_FCTL_WEP;
-			else
-				fc = IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA;
-		}
-
-		if (ieee->iw_mode == IW_MODE_INFRA) {
-			fc |= IEEE80211_FCTL_TODS;
-			/* To DS: Addr1 = BSSID, Addr2 = SA, Addr3 = DA */
-			memcpy(&header.addr1, ieee->current_network.bssid, ETH_ALEN);
-			memcpy(&header.addr2, &src, ETH_ALEN);
-			memcpy(&header.addr3, &dest, ETH_ALEN);
-		} else if (ieee->iw_mode == IW_MODE_ADHOC) {
-			/*
-			 * not From/To DS: Addr1 = DA, Addr2 = SA,
-			 * Addr3 = BSSID
-			 */
-			memcpy(&header.addr1, dest, ETH_ALEN);
-			memcpy(&header.addr2, src, ETH_ALEN);
-			memcpy(&header.addr3, ieee->current_network.bssid, ETH_ALEN);
-		}
-		header.frame_ctl = cpu_to_le16(fc);
-
-		/*
-		 * Determine fragmentation size based on destination (multicast
-		 * and broadcast are not fragmented)
-		 */
-		if (is_multicast_ether_addr(header.addr1)) {
-			frag_size = MAX_FRAG_THRESHOLD;
-			qos_ctl = QOS_CTL_NOTCONTAIN_ACK;
-		} else {
-			/* default:392 */
-			frag_size = ieee->fts;
-			qos_ctl = 0;
-		}
-
-		if (ieee->current_network.QoS_Enable)	{
-			hdr_len = IEEE80211_3ADDR_LEN + 2;
-			/* skb->priority is set in the ieee80211_classify() */
-			qos_ctl |= skb->priority;
-			header.qos_ctl = cpu_to_le16(qos_ctl);
-		} else {
-			hdr_len = IEEE80211_3ADDR_LEN;
-		}
-
-		/*
-		 * Determine amount of payload per fragment.  Regardless of if
-		 * this stack is providing the full 802.11 header, one will
-		 * eventually be affixed to this fragment -- so we must account
-		 * for it when determining the amount of payload space.
-		 */
-		bytes_per_frag = frag_size - hdr_len;
-		if (ieee->config &
-		(CFG_IEEE80211_COMPUTE_FCS | CFG_IEEE80211_RESERVE_FCS))
-			bytes_per_frag -= IEEE80211_FCS_LEN;
-
-		/* Each fragment may need to have room for encryption pre/postfix */
-		if (encrypt)
-			bytes_per_frag -= crypt->ops->extra_prefix_len +
-				crypt->ops->extra_postfix_len;
-
-		/*
-		 * Number of fragments is the total bytes_per_frag /
-		 * payload_per_fragment
-		 */
-		nr_frags = bytes / bytes_per_frag;
-		bytes_last_frag = bytes % bytes_per_frag;
-		if (bytes_last_frag)
-			nr_frags++;
-		else
-			bytes_last_frag = bytes_per_frag;
-
-		/*
-		 * When we allocate the TXB we allocate enough space for the 
-		 * reserve and full fragment bytes (bytes_per_frag doesn't
-		 * include prefix, postfix, header, FCS, etc.)
-		 */
-		txb = ieee80211_alloc_txb(nr_frags, frag_size, GFP_ATOMIC);
-		if (unlikely(!txb)) {
-			netdev_warn(ieee->dev, "Could not allocate TXB\n");
-			goto failed;
-		}
-		txb->encrypted = encrypt;
-		txb->payload_size = bytes;
-
-		for (i = 0; i < nr_frags; i++) {
-			skb_frag = txb->fragments[i];
-			skb_frag->priority = UP2AC(skb->priority);
-			if (encrypt)
-				skb_reserve(skb_frag, crypt->ops->extra_prefix_len);
-
-			frag_hdr = (struct ieee80211_hdr_3addrqos *)skb_put(
-				skb_frag, hdr_len);
-			memcpy(frag_hdr, &header, hdr_len);
-
-			/*
-			 * If this is not the last fragment, then add the MOREFRAGS
-			 * bit to the frame control
-			 */
-			if (i != nr_frags - 1) {
-				frag_hdr->frame_ctl = cpu_to_le16(
-					fc | IEEE80211_FCTL_MOREFRAGS);
-				bytes = bytes_per_frag;
-
-			} else {
-				/* The last fragment takes the remaining length */
-				bytes = bytes_last_frag;
-			}
-			if (ieee->current_network.QoS_Enable) {
-				/*
-				 * add 1 only indicate to corresponding seq
-				 * number control 2006/7/12
-				 */
-				frag_hdr->seq_ctl = cpu_to_le16(
-					ieee->seq_ctrl[UP2AC(skb->priority)+1]<<4 | i);
-			} else {
-				frag_hdr->seq_ctl = cpu_to_le16(
-					ieee->seq_ctrl[0]<<4 | i);
-			}
-
-			/* Put a SNAP header on the first fragment */
-			if (i == 0) {
-				ieee80211_put_snap(
-					skb_put(skb_frag, SNAP_SIZE + sizeof(u16)),
-					ether_type);
-				bytes -= SNAP_SIZE + sizeof(u16);
-			}
-
-			memcpy(skb_put(skb_frag, bytes), skb->data, bytes);
-
-			/* Advance the SKB... */
-			skb_pull(skb, bytes);
-
-			/*
-			 * Encryption routine will move the header forward in
-			 * order to insert the IV between the header and the
-			 * payload
-			 */
-			if (encrypt)
-				ieee80211_encrypt_fragment(ieee, skb_frag, hdr_len);
-			if (ieee->config &
-			(CFG_IEEE80211_COMPUTE_FCS | CFG_IEEE80211_RESERVE_FCS))
-				skb_put(skb_frag, 4);
-		}
-		/* Advance sequence number in data frame. */
-		if (ieee->current_network.QoS_Enable) {
-			if (ieee->seq_ctrl[UP2AC(skb->priority) + 1] == 0xFFF)
-				ieee->seq_ctrl[UP2AC(skb->priority) + 1] = 0;
-			else
-				ieee->seq_ctrl[UP2AC(skb->priority) + 1]++;
-		} else {
-			if (ieee->seq_ctrl[0] == 0xFFF)
-				ieee->seq_ctrl[0] = 0;
-			else
-				ieee->seq_ctrl[0]++;
-		}
-	} else {
-		if (unlikely(skb->len < sizeof(struct ieee80211_hdr_3addr))) {
-			netdev_warn(ieee->dev, "skb too small (%d).\n", skb->len);
-			goto success;
-		}
-
-		txb = ieee80211_alloc_txb(1, skb->len, GFP_ATOMIC);
-		if (!txb) {
-			netdev_warn(ieee->dev, "Could not allocate TXB\n");
-			goto failed;
-		}
-
-		txb->encrypted = 0;
-		txb->payload_size = skb->len;
-		memcpy(skb_put(txb->fragments[0], skb->len), skb->data, skb->len);
-	}
-
- success:
-	spin_unlock_irqrestore(&ieee->lock, flags);
-		dev_kfree_skb_any(skb);
-	if (txb) {
-		if (ieee->softmac_features & IEEE_SOFTMAC_TX_QUEUE) {
-			ieee80211_softmac_xmit(txb, ieee);
-		} else {
-			if ((*ieee->hard_start_xmit)(txb, dev) == 0) {
-				stats->tx_packets++;
-				stats->tx_bytes += txb->payload_size;
-				return NETDEV_TX_OK;
-			}
-			ieee80211_txb_free(txb);
-		}
-	}
-
-	return NETDEV_TX_OK;
-
- failed:
-	spin_unlock_irqrestore(&ieee->lock, flags);
-	netif_stop_queue(dev);
-	stats->tx_errors++;
-	return NETDEV_TX_BUSY;
-
-}
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_wx.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_wx.c
deleted file mode 100644
index 07c3f715a6f..00000000000
--- a/drivers/staging/rtl8187se/ieee80211/ieee80211_wx.c
+++ /dev/null
@@ -1,713 +0,0 @@
-/*
- *  Copyright(c) 2004 Intel Corporation. All rights reserved.
- *
- * Portions of this file are based on the WEP enablement code provided by the
- * Host AP project hostap-drivers v0.1.3
- * Copyright (c) 2001-2002, SSH Communications Security Corp and Jouni Malinen
- * <jkmaline@cc.hut.fi>
- * Copyright (c) 2002-2003, Jouni Malinen <jkmaline@cc.hut.fi>
- *
- * 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.
- *
- * 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.
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * James P. Ketrenos <ipw2100-admin@linux.intel.com>
- * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
- */
-
-#include <linux/wireless.h>
-#include <linux/kmod.h>
-#include <linux/slab.h>
-#include <linux/module.h>
-#include <linux/etherdevice.h>
-
-#include "ieee80211.h"
-static const char *ieee80211_modes[] = {
-	"?", "a", "b", "ab", "g", "ag", "bg", "abg"
-};
-
-#define MAX_CUSTOM_LEN 64
-static inline char *rtl818x_translate_scan(struct ieee80211_device *ieee,
-					   char *start, char *stop,
-					   struct ieee80211_network *network,
-					   struct iw_request_info *info)
-{
-	char custom[MAX_CUSTOM_LEN];
-	char *p;
-	struct iw_event iwe;
-	int i, j;
-	u8 max_rate, rate;
-
-	/* First entry *MUST* be the AP MAC address */
-	iwe.cmd = SIOCGIWAP;
-	iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
-	ether_addr_copy(iwe.u.ap_addr.sa_data, network->bssid);
-	start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_ADDR_LEN);
-
-	/* Remaining entries will be displayed in the order we provide them */
-
-	/* Add the ESSID */
-	iwe.cmd = SIOCGIWESSID;
-	iwe.u.data.flags = 1;
-	if (network->ssid_len == 0) {
-		iwe.u.data.length = sizeof("<hidden>");
-		start = iwe_stream_add_point(info, start, stop, &iwe, "<hidden>");
-	} else {
-		iwe.u.data.length = min_t(u8, network->ssid_len, 32);
-		start = iwe_stream_add_point(info, start, stop, &iwe, network->ssid);
-	}
-	/* Add the protocol name */
-	iwe.cmd = SIOCGIWNAME;
-	snprintf(iwe.u.name, IFNAMSIZ, "IEEE 802.11%s", ieee80211_modes[network->mode]);
-	start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_CHAR_LEN);
-
-	/* Add mode */
-	iwe.cmd = SIOCGIWMODE;
-	if (network->capability &
-	    (WLAN_CAPABILITY_BSS | WLAN_CAPABILITY_IBSS)) {
-		if (network->capability & WLAN_CAPABILITY_BSS)
-			iwe.u.mode = IW_MODE_MASTER;
-		else
-			iwe.u.mode = IW_MODE_ADHOC;
-
-		start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_UINT_LEN);
-	}
-
-	/* Add frequency/channel */
-	iwe.cmd = SIOCGIWFREQ;
-	iwe.u.freq.m = network->channel;
-	iwe.u.freq.e = 0;
-	iwe.u.freq.i = 0;
-	start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_FREQ_LEN);
-
-	/* Add encryption capability */
-	iwe.cmd = SIOCGIWENCODE;
-	if (network->capability & WLAN_CAPABILITY_PRIVACY)
-		iwe.u.data.flags = IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
-	else
-		iwe.u.data.flags = IW_ENCODE_DISABLED;
-	iwe.u.data.length = 0;
-	start = iwe_stream_add_point(info, start, stop, &iwe, network->ssid);
-
-	/* Add basic and extended rates */
-	max_rate = 0;
-	p = custom;
-	p += snprintf(p, MAX_CUSTOM_LEN - (p - custom), " Rates (Mb/s): ");
-	for (i = 0, j = 0; i < network->rates_len; ) {
-		if (j < network->rates_ex_len &&
-		    ((network->rates_ex[j] & 0x7F) <
-		     (network->rates[i] & 0x7F)))
-			rate = network->rates_ex[j++] & 0x7F;
-		else
-			rate = network->rates[i++] & 0x7F;
-		if (rate > max_rate)
-			max_rate = rate;
-		p += snprintf(p, MAX_CUSTOM_LEN - (p - custom),
-			      "%d%s ", rate >> 1, (rate & 1) ? ".5" : "");
-	}
-	for (; j < network->rates_ex_len; j++) {
-		rate = network->rates_ex[j] & 0x7F;
-		p += snprintf(p, MAX_CUSTOM_LEN - (p - custom),
-			      "%d%s ", rate >> 1, (rate & 1) ? ".5" : "");
-		if (rate > max_rate)
-			max_rate = rate;
-	}
-
-	iwe.cmd = SIOCGIWRATE;
-	iwe.u.bitrate.fixed = iwe.u.bitrate.disabled = 0;
-	iwe.u.bitrate.value = max_rate * 500000;
-	start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_PARAM_LEN);
-
-	iwe.cmd = IWEVCUSTOM;
-	iwe.u.data.length = p - custom;
-	if (iwe.u.data.length)
-		start = iwe_stream_add_point(info, start, stop, &iwe, custom);
-
-	/* Add quality statistics */
-	/* TODO: Fix these values... */
-	if (network->stats.signal == 0 || network->stats.rssi == 0)
-		netdev_info(ieee->dev, "========>signal:%d, rssi:%d\n",
-			    network->stats.signal, network->stats.rssi);
-	iwe.cmd = IWEVQUAL;
-	iwe.u.qual.qual = network->stats.signalstrength;
-	iwe.u.qual.level = network->stats.signal;
-	iwe.u.qual.noise = network->stats.noise;
-	iwe.u.qual.updated = network->stats.mask & IEEE80211_STATMASK_WEMASK;
-	if (!(network->stats.mask & IEEE80211_STATMASK_RSSI))
-		iwe.u.qual.updated |= IW_QUAL_LEVEL_INVALID;
-	if (!(network->stats.mask & IEEE80211_STATMASK_NOISE))
-		iwe.u.qual.updated |= IW_QUAL_NOISE_INVALID;
-	if (!(network->stats.mask & IEEE80211_STATMASK_SIGNAL))
-		iwe.u.qual.updated |= IW_QUAL_QUAL_INVALID;
-	iwe.u.qual.updated = 7;
-	start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_QUAL_LEN);
-
-	iwe.cmd = IWEVCUSTOM;
-	p = custom;
-
-	iwe.u.data.length = p - custom;
-	if (iwe.u.data.length)
-		start = iwe_stream_add_point(info, start, stop, &iwe, custom);
-
-	memset(&iwe, 0, sizeof(iwe));
-	if (network->wpa_ie_len) {
-		char buf[MAX_WPA_IE_LEN];
-		memcpy(buf, network->wpa_ie, network->wpa_ie_len);
-		iwe.cmd = IWEVGENIE;
-		iwe.u.data.length = network->wpa_ie_len;
-		start = iwe_stream_add_point(info, start, stop, &iwe, buf);
-	}
-
-	memset(&iwe, 0, sizeof(iwe));
-	if (network->rsn_ie_len) {
-		char buf[MAX_WPA_IE_LEN];
-		memcpy(buf, network->rsn_ie, network->rsn_ie_len);
-		iwe.cmd = IWEVGENIE;
-		iwe.u.data.length = network->rsn_ie_len;
-		start = iwe_stream_add_point(info, start, stop, &iwe, buf);
-	}
-
-	/* Add EXTRA: Age to display seconds since last beacon/probe response
-	 * for given network.
-	 */
-	iwe.cmd = IWEVCUSTOM;
-	p = custom;
-	p += snprintf(p, MAX_CUSTOM_LEN - (p - custom),
-		      " Last beacon: %lums ago", (jiffies - network->last_scanned) / (HZ / 100));
-	iwe.u.data.length = p - custom;
-	if (iwe.u.data.length)
-		start = iwe_stream_add_point(info, start, stop, &iwe, custom);
-
-	return start;
-}
-
-int ieee80211_wx_get_scan(struct ieee80211_device *ieee,
-			  struct iw_request_info *info,
-			  union iwreq_data *wrqu, char *extra)
-{
-	struct ieee80211_network *network;
-	unsigned long flags;
-	int err = 0;
-	char *ev = extra;
-	char *stop = ev + wrqu->data.length;
-	int i = 0;
-
-	IEEE80211_DEBUG_WX("Getting scan\n");
-	down(&ieee->wx_sem);
-	spin_lock_irqsave(&ieee->lock, flags);
-
-	if (!ieee->bHwRadioOff) {
-		list_for_each_entry(network, &ieee->network_list, list) {
-			i++;
-
-			if ((stop-ev) < 200) {
-				err = -E2BIG;
-				break;
-			}
-			if (ieee->scan_age == 0 ||
-			    time_after(network->last_scanned + ieee->scan_age, jiffies)) {
-				ev = rtl818x_translate_scan(ieee, ev, stop, network, info);
-			} else
-				IEEE80211_DEBUG_SCAN(
-					"Not showing network '%s ("
-					"%pM)' due to age (%lums).\n",
-					escape_essid(network->ssid,
-						     network->ssid_len),
-					network->bssid,
-					(jiffies - network->last_scanned) / (HZ / 100));
-		}
-	}
-	spin_unlock_irqrestore(&ieee->lock, flags);
-	up(&ieee->wx_sem);
-	wrqu->data.length = ev -  extra;
-	wrqu->data.flags = 0;
-	IEEE80211_DEBUG_WX("exit: %d networks returned.\n", i);
-
-	return err;
-}
-
-int ieee80211_wx_set_encode(struct ieee80211_device *ieee,
-			    struct iw_request_info *info,
-			    union iwreq_data *wrqu, char *keybuf)
-{
-	struct iw_point *erq = &(wrqu->encoding);
-	struct net_device *dev = ieee->dev;
-	struct ieee80211_security sec = {
-		.flags = 0
-	};
-	int i, key, key_provided, len;
-	struct ieee80211_crypt_data **crypt;
-
-	IEEE80211_DEBUG_WX("SET_ENCODE\n");
-
-	key = erq->flags & IW_ENCODE_INDEX;
-	if (key) {
-		if (key > WEP_KEYS)
-			return -EINVAL;
-		key--;
-		key_provided = 1;
-	} else {
-		key_provided = 0;
-		key = ieee->tx_keyidx;
-	}
-
-	IEEE80211_DEBUG_WX("Key: %d [%s]\n", key, key_provided ?
-			   "provided" : "default");
-
-	crypt = &ieee->crypt[key];
-
-	if (erq->flags & IW_ENCODE_DISABLED) {
-		if (key_provided && *crypt) {
-			IEEE80211_DEBUG_WX("Disabling encryption on key %d.\n",
-					   key);
-			ieee80211_crypt_delayed_deinit(ieee, crypt);
-		} else
-			IEEE80211_DEBUG_WX("Disabling encryption.\n");
-
-		/* Check all the keys to see if any are still configured,
-		 * and if no key index was provided, de-init them all.
-		 */
-		for (i = 0; i < WEP_KEYS; i++) {
-			if (ieee->crypt[i] != NULL) {
-				if (key_provided)
-					break;
-				ieee80211_crypt_delayed_deinit(
-					ieee, &ieee->crypt[i]);
-			}
-		}
-
-		if (i == WEP_KEYS) {
-			sec.enabled = 0;
-			sec.level = SEC_LEVEL_0;
-			sec.flags |= SEC_ENABLED | SEC_LEVEL;
-		}
-
-		goto done;
-	}
-
-	sec.enabled = 1;
-	sec.flags |= SEC_ENABLED;
-
-	if (*crypt != NULL && (*crypt)->ops != NULL &&
-	    strcmp((*crypt)->ops->name, "WEP") != 0) {
-		/* changing to use WEP; deinit previously used algorithm
-		 * on this key.
-		 */
-		ieee80211_crypt_delayed_deinit(ieee, crypt);
-	}
-
-	if (*crypt == NULL) {
-		struct ieee80211_crypt_data *new_crypt;
-
-		/* take WEP into use */
-		new_crypt = kzalloc(sizeof(struct ieee80211_crypt_data),
-				    GFP_KERNEL);
-		if (new_crypt == NULL)
-			return -ENOMEM;
-		new_crypt->ops = ieee80211_get_crypto_ops("WEP");
-		if (!new_crypt->ops)
-			new_crypt->ops = ieee80211_get_crypto_ops("WEP");
-
-		if (new_crypt->ops)
-			new_crypt->priv = new_crypt->ops->init(key);
-
-		if (!new_crypt->ops || !new_crypt->priv) {
-			kfree(new_crypt);
-			new_crypt = NULL;
-
-			netdev_warn(ieee->dev,
-				    "could not initialize WEP: load module ieee80211_crypt_wep\n");
-			return -EOPNOTSUPP;
-		}
-		*crypt = new_crypt;
-	}
-
-	/* If a new key was provided, set it up */
-	if (erq->length > 0) {
-		len = erq->length <= 5 ? 5 : 13;
-		memcpy(sec.keys[key], keybuf, erq->length);
-		if (len > erq->length)
-			memset(sec.keys[key] + erq->length, 0,
-			       len - erq->length);
-		IEEE80211_DEBUG_WX("Setting key %d to '%s' (%d:%d bytes)\n",
-				   key, escape_essid(sec.keys[key], len),
-				   erq->length, len);
-		sec.key_sizes[key] = len;
-		(*crypt)->ops->set_key(sec.keys[key], len, NULL,
-				       (*crypt)->priv);
-		sec.flags |= (1 << key);
-		/* This ensures a key will be activated if no key is
-		 * explicitly set.
-		 */
-		if (key == sec.active_key)
-			sec.flags |= SEC_ACTIVE_KEY;
-		ieee->tx_keyidx = key;
-	} else {
-		len = (*crypt)->ops->get_key(sec.keys[key], WEP_KEY_LEN,
-					     NULL, (*crypt)->priv);
-		if (len == 0) {
-			/* Set a default key of all 0 */
-			IEEE80211_DEBUG_WX("Setting key %d to all zero.\n",
-					   key);
-			memset(sec.keys[key], 0, 13);
-			(*crypt)->ops->set_key(sec.keys[key], 13, NULL,
-					       (*crypt)->priv);
-			sec.key_sizes[key] = 13;
-			sec.flags |= (1 << key);
-		}
-
-		/* No key data - just set the default TX key index */
-		if (key_provided) {
-			IEEE80211_DEBUG_WX(
-				"Setting key %d to default Tx key.\n", key);
-			ieee->tx_keyidx = key;
-			sec.active_key = key;
-			sec.flags |= SEC_ACTIVE_KEY;
-		}
-	}
-
- done:
-	ieee->open_wep = !(erq->flags & IW_ENCODE_RESTRICTED);
-	sec.auth_mode = ieee->open_wep ? WLAN_AUTH_OPEN : WLAN_AUTH_SHARED_KEY;
-	sec.flags |= SEC_AUTH_MODE;
-	IEEE80211_DEBUG_WX("Auth: %s\n", sec.auth_mode == WLAN_AUTH_OPEN ?
-			   "OPEN" : "SHARED KEY");
-
-	/* For now we just support WEP, so only set that security level...
-	 * TODO: When WPA is added this is one place that needs to change
-	 */
-	sec.flags |= SEC_LEVEL;
-	sec.level = SEC_LEVEL_1; /* 40 and 104 bit WEP */
-
-	if (ieee->set_security)
-		ieee->set_security(dev, &sec);
-
-	/* Do not reset port if card is in Managed mode since resetting will
-	 * generate new IEEE 802.11 authentication which may end up in looping
-	 * with IEEE 802.1X.  If your hardware requires a reset after WEP
-	 * configuration (for example... Prism2), implement the reset_port in
-	 * the callbacks structures used to initialize the 802.11 stack.
-	 */
-	if (ieee->reset_on_keychange &&
-	    ieee->iw_mode != IW_MODE_INFRA &&
-	    ieee->reset_port && ieee->reset_port(dev)) {
-		netdev_dbg(ieee->dev, "reset_port failed\n");
-		return -EINVAL;
-	}
-	return 0;
-}
-
-int ieee80211_wx_get_encode(struct ieee80211_device *ieee,
-			    struct iw_request_info *info,
-			    union iwreq_data *wrqu, char *keybuf)
-{
-	struct iw_point *erq = &(wrqu->encoding);
-	int len, key;
-	struct ieee80211_crypt_data *crypt;
-
-	IEEE80211_DEBUG_WX("GET_ENCODE\n");
-
-	if (ieee->iw_mode == IW_MODE_MONITOR)
-		return -1;
-
-	key = erq->flags & IW_ENCODE_INDEX;
-	if (key) {
-		if (key > WEP_KEYS)
-			return -EINVAL;
-		key--;
-	} else
-		key = ieee->tx_keyidx;
-
-	crypt = ieee->crypt[key];
-	erq->flags = key + 1;
-
-	if (crypt == NULL || crypt->ops == NULL) {
-		erq->length = 0;
-		erq->flags |= IW_ENCODE_DISABLED;
-		return 0;
-	}
-
-	if (strcmp(crypt->ops->name, "WEP") != 0) {
-		/* only WEP is supported with wireless extensions, so just
-		 * report that encryption is used.
-		 */
-		erq->length = 0;
-		erq->flags |= IW_ENCODE_ENABLED;
-		return 0;
-	}
-
-	len = crypt->ops->get_key(keybuf, WEP_KEY_LEN, NULL, crypt->priv);
-	erq->length = (len >= 0 ? len : 0);
-
-	erq->flags |= IW_ENCODE_ENABLED;
-
-	if (ieee->open_wep)
-		erq->flags |= IW_ENCODE_OPEN;
-	else
-		erq->flags |= IW_ENCODE_RESTRICTED;
-
-	return 0;
-}
-
-int ieee80211_wx_set_encode_ext(struct ieee80211_device *ieee,
-				struct iw_request_info *info,
-				union iwreq_data *wrqu, char *extra)
-{
-	struct net_device *dev = ieee->dev;
-	struct iw_point *encoding = &wrqu->encoding;
-	struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
-	int i, idx, ret = 0;
-	int group_key = 0;
-	const char *alg;
-	struct ieee80211_crypto_ops *ops;
-	struct ieee80211_crypt_data **crypt;
-
-	struct ieee80211_security sec = {
-		.flags = 0,
-	};
-	idx = encoding->flags & IW_ENCODE_INDEX;
-	if (idx) {
-		if (idx < 1 || idx > WEP_KEYS)
-			return -EINVAL;
-		idx--;
-	} else
-		idx = ieee->tx_keyidx;
-
-	if (ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY) {
-		crypt = &ieee->crypt[idx];
-		group_key = 1;
-	} else {
-		/* some Cisco APs use idx>0 for unicast in dynamic WEP */
-		if (idx != 0 && ext->alg != IW_ENCODE_ALG_WEP)
-			return -EINVAL;
-		if (ieee->iw_mode == IW_MODE_INFRA)
-			crypt = &ieee->crypt[idx];
-		else
-			return -EINVAL;
-	}
-
-	sec.flags |= SEC_ENABLED;
-	if ((encoding->flags & IW_ENCODE_DISABLED) ||
-	    ext->alg == IW_ENCODE_ALG_NONE) {
-		if (*crypt)
-			ieee80211_crypt_delayed_deinit(ieee, crypt);
-
-		for (i = 0; i < WEP_KEYS; i++)
-			if (ieee->crypt[i] != NULL)
-				break;
-
-		if (i == WEP_KEYS) {
-			sec.enabled = 0;
-			sec.level = SEC_LEVEL_0;
-			sec.flags |= SEC_LEVEL;
-		}
-		goto done;
-	}
-
-	sec.enabled = 1;
-
-	switch (ext->alg) {
-	case IW_ENCODE_ALG_WEP:
-		alg = "WEP";
-		break;
-	case IW_ENCODE_ALG_TKIP:
-		alg = "TKIP";
-		break;
-	case IW_ENCODE_ALG_CCMP:
-		alg = "CCMP";
-		break;
-	default:
-		IEEE80211_DEBUG_WX("%s: unknown crypto alg %d\n",
-				   dev->name, ext->alg);
-		ret = -EINVAL;
-		goto done;
-	}
-
-	ops = ieee80211_get_crypto_ops(alg);
-	if (ops == NULL)
-		ops = ieee80211_get_crypto_ops(alg);
-	if (ops == NULL) {
-		IEEE80211_DEBUG_WX("%s: unknown crypto alg %d\n",
-				   dev->name, ext->alg);
-		netdev_err(ieee->dev, "========>unknown crypto alg %d\n",
-			   ext->alg);
-		ret = -EINVAL;
-		goto done;
-	}
-
-	if (*crypt == NULL || (*crypt)->ops != ops) {
-		struct ieee80211_crypt_data *new_crypt;
-
-		ieee80211_crypt_delayed_deinit(ieee, crypt);
-
-		new_crypt = kzalloc(sizeof(*new_crypt), GFP_KERNEL);
-		if (new_crypt == NULL) {
-			ret = -ENOMEM;
-			goto done;
-		}
-		new_crypt->ops = ops;
-		if (new_crypt->ops)
-			new_crypt->priv = new_crypt->ops->init(idx);
-		if (new_crypt->priv == NULL) {
-			kfree(new_crypt);
-			ret = -EINVAL;
-			goto done;
-		}
-		*crypt = new_crypt;
-
-	}
-
-	if (ext->key_len > 0 && (*crypt)->ops->set_key &&
-	    (*crypt)->ops->set_key(ext->key, ext->key_len, ext->rx_seq,
-				   (*crypt)->priv) < 0) {
-		IEEE80211_DEBUG_WX("%s: key setting failed\n", dev->name);
-		netdev_err(ieee->dev, "key setting failed\n");
-		ret = -EINVAL;
-		goto done;
-	}
-#if 1
-	if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY) {
-		ieee->tx_keyidx = idx;
-		sec.active_key = idx;
-		sec.flags |= SEC_ACTIVE_KEY;
-	}
-
-	if (ext->alg != IW_ENCODE_ALG_NONE) {
-		memcpy(sec.keys[idx], ext->key, ext->key_len);
-		sec.key_sizes[idx] = ext->key_len;
-		sec.flags |= (1 << idx);
-		if (ext->alg == IW_ENCODE_ALG_WEP) {
-			sec.flags |= SEC_LEVEL;
-			sec.level = SEC_LEVEL_1;
-		} else if (ext->alg == IW_ENCODE_ALG_TKIP) {
-			sec.flags |= SEC_LEVEL;
-			sec.level = SEC_LEVEL_2;
-		} else if (ext->alg == IW_ENCODE_ALG_CCMP) {
-			sec.flags |= SEC_LEVEL;
-			sec.level = SEC_LEVEL_3;
-		}
-		/* Don't set sec level for group keys. */
-		if (group_key)
-			sec.flags &= ~SEC_LEVEL;
-	}
-#endif
-done:
-	if (ieee->set_security)
-		ieee->set_security(ieee->dev, &sec);
-
-	if (ieee->reset_on_keychange &&
-	    ieee->iw_mode != IW_MODE_INFRA &&
-	    ieee->reset_port && ieee->reset_port(dev)) {
-		IEEE80211_DEBUG_WX("%s: reset_port failed\n", dev->name);
-		return -EINVAL;
-	}
-
-	return ret;
-}
-
-int ieee80211_wx_set_mlme(struct ieee80211_device *ieee,
-			  struct iw_request_info *info,
-			  union iwreq_data *wrqu, char *extra)
-{
-	struct iw_mlme *mlme = (struct iw_mlme *) extra;
-#if 1
-	switch (mlme->cmd) {
-	case IW_MLME_DEAUTH:
-	case IW_MLME_DISASSOC:
-		ieee80211_disassociate(ieee);
-		break;
-	default:
-		return -EOPNOTSUPP;
-	}
-#endif
-	return 0;
-}
-
-int ieee80211_wx_set_auth(struct ieee80211_device *ieee,
-			  struct iw_request_info *info,
-			  struct iw_param *data, char *extra)
-{
-	switch (data->flags & IW_AUTH_INDEX) {
-	case IW_AUTH_WPA_VERSION:
-		/* need to support wpa2 here */
-		break;
-	case IW_AUTH_CIPHER_PAIRWISE:
-	case IW_AUTH_CIPHER_GROUP:
-	case IW_AUTH_KEY_MGMT:
-		/* Host AP driver does not use these parameters and allows
-		 * wpa_supplicant to control them internally.
-		 */
-		break;
-	case IW_AUTH_TKIP_COUNTERMEASURES:
-		ieee->tkip_countermeasures = data->value;
-		break;
-	case IW_AUTH_DROP_UNENCRYPTED:
-		ieee->drop_unencrypted = data->value;
-		break;
-
-	case IW_AUTH_80211_AUTH_ALG:
-		ieee->open_wep = (data->value&IW_AUTH_ALG_OPEN_SYSTEM) ? 1 : 0;
-		break;
-
-#if 1
-	case IW_AUTH_WPA_ENABLED:
-		ieee->wpa_enabled = (data->value) ? 1 : 0;
-		break;
-
-#endif
-	case IW_AUTH_RX_UNENCRYPTED_EAPOL:
-		ieee->ieee802_1x = data->value;
-		break;
-	case IW_AUTH_PRIVACY_INVOKED:
-		ieee->privacy_invoked = data->value;
-		break;
-	default:
-		return -EOPNOTSUPP;
-	}
-	return 0;
-}
-
-#if 1
-int ieee80211_wx_set_gen_ie(struct ieee80211_device *ieee, u8 *ie, size_t len)
-{
-	u8 *buf = NULL;
-
-	if (len > MAX_WPA_IE_LEN || (len && ie == NULL)) {
-		netdev_err(ieee->dev, "return error out, len:%zu\n", len);
-	return -EINVAL;
-	}
-
-	if (len) {
-		if (len != ie[1]+2) {
-			netdev_err(ieee->dev, "len:%zu, ie:%d\n", len, ie[1]);
-			return -EINVAL;
-		}
-		buf = kmemdup(ie, len, GFP_KERNEL);
-		if (buf == NULL)
-			return -ENOMEM;
-		kfree(ieee->wpa_ie);
-		ieee->wpa_ie = buf;
-		ieee->wpa_ie_len = len;
-	} else {
-		kfree(ieee->wpa_ie);
-		ieee->wpa_ie = NULL;
-		ieee->wpa_ie_len = 0;
-	}
-
-	return 0;
-
-}
-#endif
diff --git a/drivers/staging/rtl8187se/r8180.h b/drivers/staging/rtl8187se/r8180.h
deleted file mode 100644
index 9f931dba1d8..00000000000
--- a/drivers/staging/rtl8187se/r8180.h
+++ /dev/null
@@ -1,640 +0,0 @@
-/*
- * This is part of rtl8180 OpenSource driver.
- * Copyright (C) Andrea Merello 2004-2005  <andrea.merello@gmail.com>
- * Released under the terms of GPL (General Public Licence)
- *
- * Parts of this driver are based on the GPL part of the official realtek driver
- *
- * Parts of this driver are based on the rtl8180 driver skeleton from Patric
- * Schenke & Andres Salomon
- *
- * Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver
- *
- * We want to thanks the Authors of those projects and the Ndiswrapper project
- * Authors.
- */
-
-#ifndef R8180H
-#define R8180H
-
-#include <linux/interrupt.h>
-
-#define RTL8180_MODULE_NAME "r8180"
-#define DMESG(x, a...) printk(KERN_INFO RTL8180_MODULE_NAME ": " x "\n", ## a)
-#define DMESGW(x, a...) printk(KERN_WARNING RTL8180_MODULE_NAME ": WW:" x "\n", ## a)
-#define DMESGE(x, a...) printk(KERN_WARNING RTL8180_MODULE_NAME ": EE:" x "\n", ## a)
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/ioport.h>
-#include <linux/sched.h>
-#include <linux/types.h>
-#include <linux/slab.h>
-#include <linux/netdevice.h>
-#include <linux/pci.h>
-#include <linux/etherdevice.h>
-#include <linux/delay.h>
-#include <linux/rtnetlink.h> /* for rtnl_lock() */
-#include <linux/wireless.h>
-#include <linux/timer.h>
-#include <linux/proc_fs.h> /* Necessary because we use the proc fs. */
-#include <linux/if_arp.h>
-#include "ieee80211/ieee80211.h"
-#include <asm/io.h>
-
-#define EPROM_93c46 0
-#define EPROM_93c56 1
-
-#define RTL_IOCTL_WPA_SUPPLICANT (SIOCIWFIRSTPRIV + 30)
-
-#define DEFAULT_FRAG_THRESHOLD 2342U
-#define MIN_FRAG_THRESHOLD 256U
-#define DEFAULT_RTS_THRESHOLD 2342U
-#define MIN_RTS_THRESHOLD 0U
-#define MAX_RTS_THRESHOLD 2342U
-#define DEFAULT_BEACONINTERVAL 0x64U
-
-#define DEFAULT_RETRY_RTS 7
-#define DEFAULT_RETRY_DATA 7
-
-#define BEACON_QUEUE 6
-
-#define aSifsTime 10
-
-#define sCrcLng 4
-#define sAckCtsLng 112 /* bits in ACK and CTS frames. */
-/* +by amy 080312. */
-#define RATE_ADAPTIVE_TIMER_PERIOD 300
-
-enum wireless_mode {
-	WIRELESS_MODE_UNKNOWN = 0x00,
-	WIRELESS_MODE_A = 0x01,
-	WIRELESS_MODE_B = 0x02,
-	WIRELESS_MODE_G = 0x04,
-	WIRELESS_MODE_AUTO = 0x08,
-};
-
-struct chnl_access_setting {
-	u16 sifs_timer;
-	u16 difs_timer;
-	u16 slot_time_timer;
-	u16 eifs_timer;
-	u16 cwmin_index;
-	u16 cwmax_index;
-};
-
-enum nic_t {
-	NIC_8185 = 1,
-	NIC_8185B
-};
-
-typedef u32 AC_CODING;
-#define AC0_BE	0 /* ACI: 0x00 */ /* Best Effort. */
-#define AC1_BK	1 /* ACI: 0x01 */ /* Background. */
-#define AC2_VI	2 /* ACI: 0x10 */ /* Video. */
-#define AC3_VO	3 /* ACI: 0x11 */ /* Voice. */
-#define AC_MAX	4 /* Max: define total number; Should not to be used as a real
-		   * enum.
-		   */
-
-/*
- * ECWmin/ECWmax field.
- * Ref: WMM spec 2.2.2: WME Parameter Element, p.13.
- */
-typedef union _ECW {
-	u8 charData;
-	struct {
-		u8 ECWmin:4;
-		u8 ECWmax:4;
-	} f;	/* Field */
-} ECW, *PECW;
-
-/*
- * ACI/AIFSN Field. Ref: WMM spec 2.2.2: WME Parameter Element, p.12.
- */
-typedef union _ACI_AIFSN {
-	u8 charData;
-
-	struct {
-		u8 AIFSN:4;
-		u8 ACM:1;
-		u8 ACI:2;
-		u8 Reserved:1;
-	} f;	/* Field */
-} ACI_AIFSN, *PACI_AIFSN;
-
-/*
- * AC Parameters Record Format.
- * Ref: WMM spec 2.2.2: WME Parameter Element, p.12.
- */
-typedef union _AC_PARAM {
-	u32 longData;
-	u8 charData[4];
-
-	struct {
-		ACI_AIFSN AciAifsn;
-		ECW Ecw;
-		u16 TXOPLimit;
-	} f;	/* Field */
-} AC_PARAM, *PAC_PARAM;
-
-struct buffer {
-	struct buffer *next;
-	u32 *buf;
-	dma_addr_t dma;
-};
-
-/* YJ,modified,080828. */
-struct stats {
-	unsigned long txrdu;
-	unsigned long rxrdu;
-	unsigned long rxnolast;
-	unsigned long rxnodata;
-	unsigned long rxnopointer;
-	unsigned long txnperr;
-	unsigned long txresumed;
-	unsigned long rxerr;
-	unsigned long rxoverflow;
-	unsigned long rxint;
-	unsigned long txbkpokint;
-	unsigned long txbepoking;
-	unsigned long txbkperr;
-	unsigned long txbeperr;
-	unsigned long txnpokint;
-	unsigned long txhpokint;
-	unsigned long txhperr;
-	unsigned long ints;
-	unsigned long shints;
-	unsigned long txoverflow;
-	unsigned long rxdmafail;
-	unsigned long txbeacon;
-	unsigned long txbeaconerr;
-	unsigned long txlpokint;
-	unsigned long txlperr;
-	unsigned long txretry; /* retry number tony 20060601 */
-	unsigned long rxcrcerrmin; /* crc error (0-500) */
-	unsigned long rxcrcerrmid; /* crc error (500-1000) */
-	unsigned long rxcrcerrmax; /* crc error (>1000) */
-	unsigned long rxicverr; /* ICV error */
-};
-
-#define MAX_LD_SLOT_NUM 10
-#define KEEP_ALIVE_INTERVAL 20 /* in seconds. */
-#define CHECK_FOR_HANG_PERIOD 2 /* be equal to watchdog check time. */
-#define DEFAULT_KEEP_ALIVE_LEVEL 1
-#define DEFAULT_SLOT_NUM 2
-#define POWER_PROFILE_AC 0
-#define POWER_PROFILE_BATTERY 1
-
-struct link_detect_t {
-	u32 rx_frame_num[MAX_LD_SLOT_NUM]; /* number of Rx Frame.
-					    * CheckForHang_period  to determine
-					    * link status.
-					    */
-	u16 slot_num; /* number of CheckForHang period to determine link status,
-		       * default is 2.
-		       */
-	u16 slot_index;
-	u32 num_tx_ok_in_period; /* number of packet transmitted during
-				  * CheckForHang.
-				  */
-	u32 num_rx_ok_in_period; /* number of packet received during
-				  * CheckForHang.
-				  */
-	u8 idle_count; /* (KEEP_ALIVE_INTERVAL / CHECK_FOR_HANG_PERIOD) */
-	u32 last_num_tx_unicast;
-	u32 last_num_rx_unicast;
-
-	bool b_busy_traffic; /* when it is set to 1, UI cann't scan at will. */
-};
-
-/* YJ,modified,080828,end */
-
-/* by amy for led
- * ==========================================================================
- * LED customization.
- * ==========================================================================
- */
-enum led_strategy_8185 {
-	SW_LED_MODE0,
-	SW_LED_MODE1,
-	HW_LED, /* HW control 2 LEDs, LED0 and LED1 (there are 4 different
-		 * control modes). */
-};
-
-enum rt_rf_power_state {
-	RF_ON,
-	RF_SLEEP,
-	RF_OFF
-};
-
-enum _ReasonCode {
-	unspec_reason = 0x1,
-	auth_not_valid = 0x2,
-	deauth_lv_ss = 0x3,
-	inactivity = 0x4,
-	ap_overload = 0x5,
-	class2_err = 0x6,
-	class3_err = 0x7,
-	disas_lv_ss = 0x8,
-	asoc_not_auth = 0x9,
-
-	/* ----MIC_CHECK */
-	mic_failure = 0xe,
-	/* ----END MIC_CHECK */
-
-	/* Reason code defined in 802.11i D10.0 p.28. */
-	invalid_IE = 0x0d,
-	four_way_tmout = 0x0f,
-	two_way_tmout = 0x10,
-	IE_dismatch = 0x11,
-	invalid_Gcipher	= 0x12,
-	invalid_Pcipher	= 0x13,
-	invalid_AKMP = 0x14,
-	unsup_RSNIEver = 0x15,
-	invalid_RSNIE = 0x16,
-	auth_802_1x_fail = 0x17,
-	ciper_reject = 0x18,
-
-	/* Reason code defined in 7.3.1.7, 802.1e D13.0, p.42. Added by Annie,
-	 * 2005-11-15.
-	 */
-	QoS_unspec = 0x20, /* 32 */
-	QAP_bandwidth = 0x21, /* 33 */
-	poor_condition = 0x22, /* 34 */
-	no_facility = 0x23, /* 35 */
-	/* Where is 36??? */
-	req_declined = 0x25, /* 37 */
-	invalid_param = 0x26, /* 38 */
-	req_not_honored = 0x27, /* 39 */
-	TS_not_created = 0x2F, /* 47 */
-	DL_not_allowed = 0x30, /* 48 */
-	dest_not_exist = 0x31, /* 49 */
-	dest_not_QSTA = 0x32, /* 50 */
-};
-
-enum rt_ps_mode {
-	ACTIVE, /* Active/Continuous access. */
-	MAX_PS,	/* Max power save mode. */
-	FAST_PS /* Fast power save mode. */
-};
-
-/* by amy for power save. */
-struct r8180_priv {
-	struct pci_dev *pdev;
-
-	short epromtype;
-	int irq;
-	struct ieee80211_device *ieee80211;
-
-	short plcp_preamble_mode; /* 0:auto 1:short 2:long */
-
-	spinlock_t irq_th_lock;
-	spinlock_t tx_lock;
-	spinlock_t ps_lock;
-	spinlock_t rf_ps_lock;
-
-	u16 irq_mask;
-	short irq_enabled;
-	struct net_device *dev;
-	short chan;
-	short sens;
-	short max_sens;
-	u8 chtxpwr[15]; /* channels from 1 to 14, 0 not used. */
-	u8 chtxpwr_ofdm[15]; /* channels from 1 to 14, 0 not used. */
-	u8 channel_plan;  /* it's the channel plan index. */
-	short up;
-	short crcmon; /* if 1 allow bad crc frame reception in monitor mode. */
-
-	struct timer_list scan_timer;
-	spinlock_t scan_lock;
-	u8 active_probe;
-	struct semaphore wx_sem;
-	short hw_wep;
-
-	short digphy;
-	short antb;
-	short diversity;
-	u32 key0[4];
-	short (*rf_set_sens)(struct net_device *dev, short sens);
-	void (*rf_set_chan)(struct net_device *dev, short ch);
-	void (*rf_close)(struct net_device *dev);
-	void (*rf_init)(struct net_device *dev);
-	void (*rf_sleep)(struct net_device *dev);
-	void (*rf_wakeup)(struct net_device *dev);
-	/* short rate; */
-	short promisc;
-	/* stats */
-	struct stats stats;
-	struct link_detect_t link_detect; /* YJ,add,080828 */
-	struct iw_statistics wstats;
-
-	/* RX stuff. */
-	u32 *rxring;
-	u32 *rxringtail;
-	dma_addr_t rxringdma;
-	struct buffer *rxbuffer;
-	struct buffer *rxbufferhead;
-	int rxringcount;
-	u16 rxbuffersize;
-
-	struct sk_buff *rx_skb;
-
-	short rx_skb_complete;
-
-	u32 rx_prevlen;
-
-	u32 *txmapring;
-	u32 *txbkpring;
-	u32 *txbepring;
-	u32 *txvipring;
-	u32 *txvopring;
-	u32 *txhpring;
-	dma_addr_t txmapringdma;
-	dma_addr_t txbkpringdma;
-	dma_addr_t txbepringdma;
-	dma_addr_t txvipringdma;
-	dma_addr_t txvopringdma;
-	dma_addr_t txhpringdma;
-	u32 *txmapringtail;
-	u32 *txbkpringtail;
-	u32 *txbepringtail;
-	u32 *txvipringtail;
-	u32 *txvopringtail;
-	u32 *txhpringtail;
-	u32 *txmapringhead;
-	u32 *txbkpringhead;
-	u32 *txbepringhead;
-	u32 *txvipringhead;
-	u32 *txvopringhead;
-	u32 *txhpringhead;
-	struct buffer *txmapbufs;
-	struct buffer *txbkpbufs;
-	struct buffer *txbepbufs;
-	struct buffer *txvipbufs;
-	struct buffer *txvopbufs;
-	struct buffer *txhpbufs;
-	struct buffer *txmapbufstail;
-	struct buffer *txbkpbufstail;
-	struct buffer *txbepbufstail;
-	struct buffer *txvipbufstail;
-	struct buffer *txvopbufstail;
-	struct buffer *txhpbufstail;
-
-	int txringcount;
-	int txbuffsize;
-	struct tasklet_struct irq_rx_tasklet;
-	u8 dma_poll_mask;
-
-	/* adhoc/master mode stuff. */
-	u32 *txbeaconringtail;
-	dma_addr_t txbeaconringdma;
-	u32 *txbeaconring;
-	int txbeaconcount;
-	struct buffer *txbeaconbufs;
-	struct buffer *txbeaconbufstail;
-
-	u8 retry_data;
-	u8 retry_rts;
-	u16 rts;
-
-	/* by amy for led. */
-	enum led_strategy_8185 led_strategy;
-	/* by amy for led. */
-
-	/* by amy for power save. */
-	struct timer_list watch_dog_timer;
-	bool bInactivePs;
-	bool bSwRfProcessing;
-	enum rt_rf_power_state eInactivePowerState;
-	enum rt_rf_power_state eRFPowerState;
-	u32 RfOffReason;
-	bool RFChangeInProgress;
-	bool SetRFPowerStateInProgress;
-	u8 RFProgType;
-	bool bLeisurePs;
-	enum rt_ps_mode dot11PowerSaveMode;
-	u8 TxPollingTimes;
-
-	bool bApBufOurFrame; /* TRUE if AP buffer our unicast data , we will
-			      * keep eAwake until receive data or timeout.
-			      */
-	u8 WaitBufDataBcnCount;
-	u8 WaitBufDataTimeOut;
-
-	/* by amy for power save. */
-	/* by amy for antenna. */
-	u8 EEPROMSwAntennaDiversity;
-	bool EEPROMDefaultAntenna1;
-	u8 RegSwAntennaDiversityMechanism;
-	bool bSwAntennaDiverity;
-	u8 RegDefaultAntenna;
-	bool bDefaultAntenna1;
-	u8 SignalStrength;
-	long Stats_SignalStrength;
-	long LastSignalStrengthInPercent; /* In percentage, used for smoothing,
-					   * e.g. Moving Average.
-					   */
-	u8 SignalQuality; /* in 0-100 index. */
-	long Stats_SignalQuality;
-	long RecvSignalPower; /* in dBm. */
-	long Stats_RecvSignalPower;
-	u8 LastRxPktAntenna; /* +by amy 080312 Antenna which received the lasted
-			      * packet. 0: Aux, 1:Main. Added by Roger,
-			      * 2008.01.25.
-			      */
-	u32 AdRxOkCnt;
-	long AdRxSignalStrength;
-	u8 CurrAntennaIndex; /* Index to current Antenna (both Tx and Rx). */
-	u8 AdTickCount; /* Times of SwAntennaDiversityTimer happened. */
-	u8 AdCheckPeriod; /* # of period SwAntennaDiversityTimer to check Rx
-			   * signal strength for SW Antenna Diversity.
-			   */
-	u8 AdMinCheckPeriod; /* Min value of AdCheckPeriod. */
-	u8 AdMaxCheckPeriod; /* Max value of AdCheckPeriod. */
-	long AdRxSsThreshold; /* Signal strength threshold to switch antenna. */
-	long AdMaxRxSsThreshold; /* Max value of AdRxSsThreshold. */
-	bool bAdSwitchedChecking; /* TRUE if we shall shall check Rx signal
-				   * strength for last time switching antenna.
-				   */
-	long AdRxSsBeforeSwitched; /* Rx signal strength before we switched
-				    * antenna.
-				    */
-	struct timer_list SwAntennaDiversityTimer;
-	/* by amy for antenna {by amy 080312 */
-
-	/* Crystal calibration. Added by Roger, 2007.12.11. */
-
-	bool bXtalCalibration; /* Crystal calibration.*/
-	u8 XtalCal_Xin; /* Crystal calibration for Xin. 0~7.5pF */
-	u8 XtalCal_Xout; /* Crystal calibration for Xout. 0~7.5pF */
-
-	/* Tx power tracking with thermal meter indication.
-	 * Added by Roger, 2007.12.11.
-	 */
-
-	bool bTxPowerTrack; /* Tx Power tracking. */
-	u8 ThermalMeter; /* Thermal meter reference indication. */
-
-	/* Dynamic Initial Gain Adjustment Mechanism. Added by Bruce,
-	 * 2007-02-14.
-	 */
-	bool bDigMechanism; /* TRUE if DIG is enabled, FALSE ow. */
-	bool bRegHighPowerMechanism; /* For High Power Mechanism. 061010,
-				      * by rcnjko.
-				      */
-	u32 FalseAlarmRegValue;
-	u8 RegDigOfdmFaUpTh; /* Upper threshold of OFDM false alarm, which is
-			      * used in DIG.
-			      */
-	u8 DIG_NumberFallbackVote;
-	u8 DIG_NumberUpgradeVote;
-	/* For HW antenna diversity, added by Roger, 2008.01.30. */
-	u32 AdMainAntennaRxOkCnt; /* Main antenna Rx OK count. */
-	u32 AdAuxAntennaRxOkCnt; /* Aux antenna Rx OK count. */
-	bool bHWAdSwitched; /* TRUE if we has switched default antenna by HW
-			     * evaluation.
-			     */
-	/* RF High Power upper/lower threshold. */
-	u8 RegHiPwrUpperTh;
-	u8 RegHiPwrLowerTh;
-	/* RF RSSI High Power upper/lower Threshold. */
-	u8 RegRSSIHiPwrUpperTh;
-	u8 RegRSSIHiPwrLowerTh;
-	/* Current CCK RSSI value to determine CCK high power, asked by SD3 DZ,
-	 * by Bruce, 2007-04-12.
-	 */
-	u8 CurCCKRSSI;
-	bool bCurCCKPkt;
-	/* High Power Mechanism. Added by amy, 080312. */
-	bool bToUpdateTxPwr;
-	long UndecoratedSmoothedSS;
-	long UndecoratedSmoothedRxPower;
-	u8 RSSI;
-	char RxPower;
-	u8 InitialGain;
-	/* For adjust Dig Threshold during Legacy/Leisure Power Save Mode. */
-	u32 DozePeriodInPast2Sec;
-	/* Don't access BB/RF under disable PLL situation. */
-	u8 InitialGainBackUp;
-	u8 RegBModeGainStage;
-	/* by amy for rate adaptive */
-	struct timer_list rateadapter_timer;
-	u32 RateAdaptivePeriod;
-	bool bEnhanceTxPwr;
-	bool bUpdateARFR;
-	int ForcedDataRate; /* Force Data Rate. 0: Auto, 0x02: 1M ~ 0x6C: 54M.)
-			     */
-	u32 NumTxUnicast; /* YJ,add,080828,for keep alive. */
-	u8 keepAliveLevel; /*YJ,add,080828,for KeepAlive. */
-	unsigned long NumTxOkTotal;
-	u16 LastRetryCnt;
-	u16 LastRetryRate;
-	unsigned long LastTxokCnt;
-	unsigned long LastRxokCnt;
-	u16 CurrRetryCnt;
-	unsigned long LastTxOKBytes;
-	unsigned long NumTxOkBytesTotal;
-	u8 LastFailTxRate;
-	long LastFailTxRateSS;
-	u8 FailTxRateCount;
-	u32 LastTxThroughput;
-	/* for up rate. */
-	unsigned short bTryuping;
-	u8 CurrTxRate; /* the rate before up. */
-	u16 CurrRetryRate;
-	u16 TryupingCount;
-	u8 TryDownCountLowData;
-	u8 TryupingCountNoData;
-
-	u8 CurrentOperaRate;
-	struct work_struct reset_wq;
-	struct work_struct watch_dog_wq;
-	short ack_tx_to_ieee;
-
-	u8 dma_poll_stop_mask;
-
-	u16 ShortRetryLimit;
-	u16 LongRetryLimit;
-	u16 EarlyRxThreshold;
-	u32 TransmitConfig;
-	u32 ReceiveConfig;
-	u32 IntrMask;
-
-	struct chnl_access_setting ChannelAccessSetting;
-};
-
-#define MANAGE_PRIORITY 0
-#define BK_PRIORITY 1
-#define BE_PRIORITY 2
-#define VI_PRIORITY 3
-#define VO_PRIORITY 4
-#define HI_PRIORITY 5
-#define BEACON_PRIORITY 6
-
-#define LOW_PRIORITY VI_PRIORITY
-#define NORM_PRIORITY VO_PRIORITY
-/* AC2Queue mapping. */
-#define AC2Q(_ac) (((_ac) == WME_AC_VO) ? VO_PRIORITY : \
-		((_ac) == WME_AC_VI) ? VI_PRIORITY : \
-		((_ac) == WME_AC_BK) ? BK_PRIORITY : \
-		BE_PRIORITY)
-
-short rtl8180_tx(struct net_device *dev, u8 *skbuf, int len, int priority,
-		 bool morefrag, short fragdesc, int rate);
-
-u8 read_nic_byte(struct net_device *dev, int x);
-u32 read_nic_dword(struct net_device *dev, int x);
-u16 read_nic_word(struct net_device *dev, int x);
-void write_nic_byte(struct net_device *dev, int x, u8 y);
-void write_nic_word(struct net_device *dev, int x, u16 y);
-void write_nic_dword(struct net_device *dev, int x, u32 y);
-void force_pci_posting(struct net_device *dev);
-
-void rtl8180_rtx_disable(struct net_device *);
-void rtl8180_set_anaparam(struct net_device *dev, u32 a);
-void rtl8185_set_anaparam2(struct net_device *dev, u32 a);
-void rtl8180_set_hw_wep(struct net_device *dev);
-void rtl8180_no_hw_wep(struct net_device *dev);
-void rtl8180_update_msr(struct net_device *dev);
-void rtl8180_beacon_tx_disable(struct net_device *dev);
-void rtl8180_beacon_rx_disable(struct net_device *dev);
-int rtl8180_down(struct net_device *dev);
-int rtl8180_up(struct net_device *dev);
-void rtl8180_commit(struct net_device *dev);
-void rtl8180_set_chan(struct net_device *dev, short ch);
-void write_phy(struct net_device *dev, u8 adr, u8 data);
-void write_phy_cck(struct net_device *dev, u8 adr, u32 data);
-void write_phy_ofdm(struct net_device *dev, u8 adr, u32 data);
-void rtl8185_tx_antenna(struct net_device *dev, u8 ant);
-void rtl8185_rf_pins_enable(struct net_device *dev);
-void IPSEnter(struct net_device *dev);
-void IPSLeave(struct net_device *dev);
-int get_curr_tx_free_desc(struct net_device *dev, int priority);
-void UpdateInitialGain(struct net_device *dev);
-bool SetAntennaConfig87SE(struct net_device *dev, u8 DefaultAnt,
-			  bool bAntDiversity);
-
-void rtl8185b_adapter_start(struct net_device *dev);
-void rtl8185b_rx_enable(struct net_device *dev);
-void rtl8185b_tx_enable(struct net_device *dev);
-void rtl8180_reset(struct net_device *dev);
-void rtl8185b_irq_enable(struct net_device *dev);
-void fix_rx_fifo(struct net_device *dev);
-void fix_tx_fifo(struct net_device *dev);
-void rtl8225z2_SetTXPowerLevel(struct net_device *dev, short ch);
-void rtl8180_rate_adapter(struct work_struct *work);
-bool MgntActSet_RF_State(struct net_device *dev, enum rt_rf_power_state StateToSet,
-			 u32 ChangeSource);
-
-#endif
-
-/* fun with the built-in ieee80211 stack... */
-extern int ieee80211_crypto_init(void);
-extern void ieee80211_crypto_deinit(void);
-extern int ieee80211_crypto_tkip_init(void);
-extern void ieee80211_crypto_tkip_exit(void);
-extern int ieee80211_crypto_ccmp_init(void);
-extern void ieee80211_crypto_ccmp_exit(void);
-extern int ieee80211_crypto_wep_init(void);
-extern void ieee80211_crypto_wep_exit(void);
diff --git a/drivers/staging/rtl8187se/r8180_93cx6.h b/drivers/staging/rtl8187se/r8180_93cx6.h
deleted file mode 100644
index b52b5b0610a..00000000000
--- a/drivers/staging/rtl8187se/r8180_93cx6.h
+++ /dev/null
@@ -1,54 +0,0 @@
-/*
-	This is part of rtl8180 OpenSource driver
-	Copyright (C) Andrea Merello 2004-2005  <andrea.merello@gmail.com>
-	Released under the terms of GPL (General Public Licence)
-
-	Parts of this driver are based on the GPL part of the official realtek driver
-	Parts of this driver are based on the rtl8180 driver skeleton from Patric Schenke & Andres Salomon
-	Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver
-
-	We want to tanks the Authors of such projects and the Ndiswrapper project Authors.
-*/
-
-/*This files contains card eeprom (93c46 or 93c56) programming routines*/
-/*memory is addressed by WORDS*/
-
-#include "r8180.h"
-#include "r8180_hw.h"
-
-#define EPROM_DELAY 10
-
-#define EPROM_ANAPARAM_ADDRLWORD 0xd
-#define EPROM_ANAPARAM_ADDRHWORD 0xe
-
-#define RFCHIPID 0x6
-#define	RFCHIPID_INTERSIL 1
-#define	RFCHIPID_RFMD 2
-#define	RFCHIPID_PHILIPS 3
-#define	RFCHIPID_MAXIM 4
-#define	RFCHIPID_GCT 5
-#define RFCHIPID_RTL8225 9
-#define RF_ZEBRA2 11
-#define EPROM_TXPW_BASE 0x05
-#define RF_ZEBRA4 12
-#define RFCHIPID_RTL8255 0xa
-#define RF_PARAM 0x19
-#define RF_PARAM_DIGPHY_SHIFT 0
-#define RF_PARAM_ANTBDEFAULT_SHIFT 1
-#define RF_PARAM_CARRIERSENSE_SHIFT 2
-#define RF_PARAM_CARRIERSENSE_MASK (3<<2)
-#define ENERGY_TRESHOLD 0x17
-#define EPROM_VERSION 0x1E
-#define MAC_ADR 0x7
-
-#define CIS 0x18
-
-#define	EPROM_TXPW_OFDM_CH1_2 0x20
-
-#define	EPROM_TXPW_CH1_2 0x30
-
-#define RTL818X_EEPROM_CMD_READ		(1 << 0)
-#define RTL818X_EEPROM_CMD_WRITE	(1 << 1)
-#define RTL818X_EEPROM_CMD_CK		(1 << 2)
-#define RTL818X_EEPROM_CMD_CS		(1 << 3)
-
diff --git a/drivers/staging/rtl8187se/r8180_core.c b/drivers/staging/rtl8187se/r8180_core.c
deleted file mode 100644
index a6022d4e757..00000000000
--- a/drivers/staging/rtl8187se/r8180_core.c
+++ /dev/null
@@ -1,3775 +0,0 @@
-/*
- * This is part of rtl818x pci OpenSource driver - v 0.1
- * Copyright (C) Andrea Merello 2004-2005  <andrea.merello@gmail.com>
- * Released under the terms of GPL (General Public License)
- *
- * Parts of this driver are based on the GPL part of the official
- * Realtek driver.
- *
- * Parts of this driver are based on the rtl8180 driver skeleton
- * from Patric Schenke & Andres Salomon.
- *
- * Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver.
- *
- * Parts of BB/RF code are derived from David Young rtl8180 netbsd driver.
- *
- * RSSI calc function from 'The Deuce'
- *
- * Some ideas borrowed from the 8139too.c driver included in linux kernel.
- *
- * We (I?) want to thanks the Authors of those projecs and also the
- * Ndiswrapper's project Authors.
- *
- * A big big thanks goes also to Realtek corp. for their help in my attempt to
- * add RTL8185 and RTL8225 support, and to David Young also.
- *
- * Power management interface routines.
- * Written by Mariusz Matuszek.
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#undef RX_DONT_PASS_UL
-#undef DUMMY_RX
-
-#include <linux/slab.h>
-#include <linux/syscalls.h>
-#include <linux/eeprom_93cx6.h>
-#include <linux/interrupt.h>
-#include <linux/proc_fs.h>
-#include <linux/seq_file.h>
-
-#include "r8180_hw.h"
-#include "r8180.h"
-#include "r8180_rtl8225.h" /* RTL8225 Radio frontend */
-#include "r8180_93cx6.h"   /* Card EEPROM */
-#include "r8180_wx.h"
-#include "r8180_dm.h"
-
-#include "ieee80211/dot11d.h"
-
-static struct pci_device_id rtl8180_pci_id_tbl[] = {
-	{
-		.vendor = PCI_VENDOR_ID_REALTEK,
-		.device = 0x8199,
-		.subvendor = PCI_ANY_ID,
-		.subdevice = PCI_ANY_ID,
-		.driver_data = 0,
-	},
-	{
-		.vendor = 0,
-		.device = 0,
-		.subvendor = 0,
-		.subdevice = 0,
-		.driver_data = 0,
-	}
-};
-
-static char ifname[IFNAMSIZ] = "wlan%d";
-static int hwwep;
-
-MODULE_LICENSE("GPL");
-MODULE_DEVICE_TABLE(pci, rtl8180_pci_id_tbl);
-MODULE_AUTHOR("Andrea Merello <andrea.merello@gmail.com>");
-MODULE_DESCRIPTION("Linux driver for Realtek RTL8187SE WiFi cards");
-
-module_param_string(ifname, ifname, sizeof(ifname), S_IRUGO|S_IWUSR);
-module_param(hwwep, int, S_IRUGO|S_IWUSR);
-
-MODULE_PARM_DESC(hwwep, " Try to use hardware WEP support. Still broken and not available on all cards");
-
-static int rtl8180_pci_probe(struct pci_dev *pdev,
-			     const struct pci_device_id *id);
-
-static void rtl8180_pci_remove(struct pci_dev *pdev);
-
-static void rtl8180_shutdown(struct pci_dev *pdev)
-{
-	struct net_device *dev = pci_get_drvdata(pdev);
-	if (dev->netdev_ops->ndo_stop)
-		dev->netdev_ops->ndo_stop(dev);
-	pci_disable_device(pdev);
-}
-
-static int rtl8180_suspend(struct pci_dev *pdev, pm_message_t state)
-{
-	struct net_device *dev = pci_get_drvdata(pdev);
-
-	if (!netif_running(dev))
-		goto out_pci_suspend;
-
-	if (dev->netdev_ops->ndo_stop)
-		dev->netdev_ops->ndo_stop(dev);
-
-	netif_device_detach(dev);
-
-out_pci_suspend:
-	pci_save_state(pdev);
-	pci_disable_device(pdev);
-	pci_set_power_state(pdev, pci_choose_state(pdev, state));
-	return 0;
-}
-
-static int rtl8180_resume(struct pci_dev *pdev)
-{
-	struct net_device *dev = pci_get_drvdata(pdev);
-	int err;
-	u32 val;
-
-	pci_set_power_state(pdev, PCI_D0);
-
-	err = pci_enable_device(pdev);
-	if (err) {
-		dev_err(&pdev->dev, "pci_enable_device failed on resume\n");
-
-		return err;
-	}
-
-	pci_restore_state(pdev);
-
-	/*
-	 * Suspend/Resume resets the PCI configuration space, so we have to
-	 * re-disable the RETRY_TIMEOUT register (0x41) to keep PCI Tx retries
-	 * from interfering with C3 CPU state. pci_restore_state won't help
-	 * here since it only restores the first 64 bytes pci config header.
-	 */
-	pci_read_config_dword(pdev, 0x40, &val);
-	if ((val & 0x0000ff00) != 0)
-		pci_write_config_dword(pdev, 0x40, val & 0xffff00ff);
-
-	if (!netif_running(dev))
-		goto out;
-
-	if (dev->netdev_ops->ndo_open)
-		dev->netdev_ops->ndo_open(dev);
-
-	netif_device_attach(dev);
-out:
-	return 0;
-}
-
-static struct pci_driver rtl8180_pci_driver = {
-	.name		= RTL8180_MODULE_NAME,
-	.id_table	= rtl8180_pci_id_tbl,
-	.probe		= rtl8180_pci_probe,
-	.remove		= rtl8180_pci_remove,
-	.suspend	= rtl8180_suspend,
-	.resume		= rtl8180_resume,
-	.shutdown	= rtl8180_shutdown,
-};
-
-u8 read_nic_byte(struct net_device *dev, int x)
-{
-	return 0xff&readb((u8 __iomem *)dev->mem_start + x);
-}
-
-u32 read_nic_dword(struct net_device *dev, int x)
-{
-	return readl((u8 __iomem *)dev->mem_start + x);
-}
-
-u16 read_nic_word(struct net_device *dev, int x)
-{
-	return readw((u8 __iomem *)dev->mem_start + x);
-}
-
-void write_nic_byte(struct net_device *dev, int x, u8 y)
-{
-	writeb(y, (u8 __iomem *)dev->mem_start + x);
-	udelay(20);
-}
-
-void write_nic_dword(struct net_device *dev, int x, u32 y)
-{
-	writel(y, (u8 __iomem *)dev->mem_start + x);
-	udelay(20);
-}
-
-void write_nic_word(struct net_device *dev, int x, u16 y)
-{
-	writew(y, (u8 __iomem *)dev->mem_start + x);
-	udelay(20);
-}
-
-inline void force_pci_posting(struct net_device *dev)
-{
-	read_nic_byte(dev, EPROM_CMD);
-	mb();
-}
-
-static irqreturn_t rtl8180_interrupt(int irq, void *netdev);
-void set_nic_rxring(struct net_device *dev);
-void set_nic_txring(struct net_device *dev);
-static struct net_device_stats *rtl8180_stats(struct net_device *dev);
-void rtl8180_commit(struct net_device *dev);
-void rtl8180_start_tx_beacon(struct net_device *dev);
-
-static struct proc_dir_entry *rtl8180_proc;
-
-static int proc_get_registers(struct seq_file *m, void *v)
-{
-	struct net_device *dev = m->private;
-	int i, n, max = 0xff;
-
-	/* This dump the current register page */
-	for (n = 0; n <= max;) {
-		seq_printf(m, "\nD:  %2x > ", n);
-
-		for (i = 0; i < 16 && n <= max; i++, n++)
-			seq_printf(m, "%2x ", read_nic_byte(dev, n));
-	}
-	seq_putc(m, '\n');
-	return 0;
-}
-
-int get_curr_tx_free_desc(struct net_device *dev, int priority);
-
-static int proc_get_stats_hw(struct seq_file *m, void *v)
-{
-	return 0;
-}
-
-static int proc_get_stats_rx(struct seq_file *m, void *v)
-{
-	struct net_device *dev = m->private;
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-
-	seq_printf(m,
-		"RX OK: %lu\n"
-		"RX Retry: %lu\n"
-		"RX CRC Error(0-500): %lu\n"
-		"RX CRC Error(500-1000): %lu\n"
-		"RX CRC Error(>1000): %lu\n"
-		"RX ICV Error: %lu\n",
-		priv->stats.rxint,
-		priv->stats.rxerr,
-		priv->stats.rxcrcerrmin,
-		priv->stats.rxcrcerrmid,
-		priv->stats.rxcrcerrmax,
-		priv->stats.rxicverr
-		);
-
-	return 0;
-}
-
-static int proc_get_stats_tx(struct seq_file *m, void *v)
-{
-	struct net_device *dev = m->private;
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	unsigned long totalOK;
-
-	totalOK = priv->stats.txnpokint + priv->stats.txhpokint +
-		priv->stats.txlpokint;
-
-	seq_printf(m,
-		"TX OK: %lu\n"
-		"TX Error: %lu\n"
-		"TX Retry: %lu\n"
-		"TX beacon OK: %lu\n"
-		"TX beacon error: %lu\n",
-		totalOK,
-		priv->stats.txnperr+priv->stats.txhperr+priv->stats.txlperr,
-		priv->stats.txretry,
-		priv->stats.txbeacon,
-		priv->stats.txbeaconerr
-	);
-
-	return 0;
-}
-
-static void rtl8180_proc_module_init(void)
-{
-	DMESG("Initializing proc filesystem");
-	rtl8180_proc = proc_mkdir(RTL8180_MODULE_NAME, init_net.proc_net);
-}
-
-static void rtl8180_proc_module_remove(void)
-{
-	remove_proc_entry(RTL8180_MODULE_NAME, init_net.proc_net);
-}
-
-static void rtl8180_proc_remove_one(struct net_device *dev)
-{
-	remove_proc_subtree(dev->name, rtl8180_proc);
-}
-
-/*
- * seq_file wrappers for procfile show routines.
- */
-static int rtl8180_proc_open(struct inode *inode, struct file *file)
-{
-	struct net_device *dev = proc_get_parent_data(inode);
-	int (*show)(struct seq_file *, void *) = PDE_DATA(inode);
-
-	return single_open(file, show, dev);
-}
-
-static const struct file_operations rtl8180_proc_fops = {
-	.open		= rtl8180_proc_open,
-	.read		= seq_read,
-	.llseek		= seq_lseek,
-	.release	= single_release,
-};
-
-/*
- * Table of proc files we need to create.
- */
-struct rtl8180_proc_file {
-	char name[12];
-	int (*show)(struct seq_file *, void *);
-};
-
-static const struct rtl8180_proc_file rtl8180_proc_files[] = {
-	{ "stats-hw",	&proc_get_stats_hw },
-	{ "stats-rx",	&proc_get_stats_rx },
-	{ "stats-tx",	&proc_get_stats_tx },
-	{ "registers",	&proc_get_registers },
-	{ "" }
-};
-
-static void rtl8180_proc_init_one(struct net_device *dev)
-{
-	const struct rtl8180_proc_file *f;
-	struct proc_dir_entry *dir;
-
-	dir = proc_mkdir_data(dev->name, 0, rtl8180_proc, dev);
-	if (!dir) {
-		DMESGE("Unable to initialize /proc/net/r8180/%s\n", dev->name);
-		return;
-	}
-
-	for (f = rtl8180_proc_files; f->name[0]; f++) {
-		if (!proc_create_data(f->name, S_IFREG | S_IRUGO, dir,
-				      &rtl8180_proc_fops, f->show)) {
-			DMESGE("Unable to initialize /proc/net/r8180/%s/%s\n",
-			       dev->name, f->name);
-			return;
-		}
-	}
-}
-
-/*
- * FIXME: check if we can use some standard already-existent
- * data type+functions in kernel.
- */
-
-static short buffer_add(struct buffer **buffer, u32 *buf, dma_addr_t dma,
-			struct buffer **bufferhead)
-{
-	struct buffer *tmp;
-
-	if (!*buffer) {
-
-		*buffer = kmalloc(sizeof(struct buffer), GFP_KERNEL);
-
-		if (*buffer == NULL) {
-			DMESGE("Failed to kmalloc head of TX/RX struct");
-			return -1;
-		}
-		(*buffer)->next = *buffer;
-		(*buffer)->buf = buf;
-		(*buffer)->dma = dma;
-		if (bufferhead != NULL)
-			(*bufferhead) = (*buffer);
-		return 0;
-	}
-	tmp = *buffer;
-
-	while (tmp->next != (*buffer))
-		tmp = tmp->next;
-	tmp->next = kmalloc(sizeof(struct buffer), GFP_KERNEL);
-	if (tmp->next == NULL) {
-		DMESGE("Failed to kmalloc TX/RX struct");
-		return -1;
-	}
-	tmp->next->buf = buf;
-	tmp->next->dma = dma;
-	tmp->next->next = *buffer;
-
-	return 0;
-}
-
-static void buffer_free(struct net_device *dev, struct buffer **buffer, int len,
-		 short consistent)
-{
-
-	struct buffer *tmp, *next;
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	struct pci_dev *pdev = priv->pdev;
-
-	if (!*buffer)
-		return;
-
-	tmp = *buffer;
-
-	do {
-		next = tmp->next;
-		if (consistent) {
-			pci_free_consistent(pdev, len,
-				    tmp->buf, tmp->dma);
-		} else {
-			pci_unmap_single(pdev, tmp->dma,
-			len, PCI_DMA_FROMDEVICE);
-			kfree(tmp->buf);
-		}
-		kfree(tmp);
-		tmp = next;
-	} while (next != *buffer);
-
-	*buffer = NULL;
-}
-
-int get_curr_tx_free_desc(struct net_device *dev, int priority)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	u32 *tail;
-	u32 *head;
-	int ret;
-
-	switch (priority) {
-	case MANAGE_PRIORITY:
-		head = priv->txmapringhead;
-		tail = priv->txmapringtail;
-		break;
-	case BK_PRIORITY:
-		head = priv->txbkpringhead;
-		tail = priv->txbkpringtail;
-		break;
-	case BE_PRIORITY:
-		head = priv->txbepringhead;
-		tail = priv->txbepringtail;
-		break;
-	case VI_PRIORITY:
-		head = priv->txvipringhead;
-		tail = priv->txvipringtail;
-		break;
-	case VO_PRIORITY:
-		head = priv->txvopringhead;
-		tail = priv->txvopringtail;
-		break;
-	case HI_PRIORITY:
-		head = priv->txhpringhead;
-		tail = priv->txhpringtail;
-		break;
-	default:
-		return -1;
-	}
-
-	if (head <= tail)
-		ret = priv->txringcount - (tail - head)/8;
-	else
-		ret = (head - tail)/8;
-
-	if (ret > priv->txringcount)
-		DMESG("BUG");
-
-	return ret;
-}
-
-static short check_nic_enought_desc(struct net_device *dev, int priority)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	struct ieee80211_device *ieee = netdev_priv(dev);
-	int requiredbyte;
-	int required;
-
-	requiredbyte = priv->ieee80211->fts +
-		sizeof(struct ieee80211_header_data);
-
-	if (ieee->current_network.QoS_Enable)
-		requiredbyte += 2;
-
-	required = requiredbyte / (priv->txbuffsize-4);
-
-	if (requiredbyte % priv->txbuffsize)
-		required++;
-
-	/* for now we keep two free descriptor as a safety boundary
-	 * between the tail and the head
-	 */
-
-	return required + 2 < get_curr_tx_free_desc(dev, priority);
-}
-
-void fix_tx_fifo(struct net_device *dev)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	u32 *tmp;
-	int i;
-
-	for (tmp = priv->txmapring, i = 0;
-	     i < priv->txringcount;
-	     tmp += 8, i++) {
-		*tmp = *tmp & ~(1<<31);
-	}
-
-	for (tmp = priv->txbkpring, i = 0;
-	     i < priv->txringcount;
-	     tmp += 8, i++) {
-		*tmp = *tmp & ~(1<<31);
-	}
-
-	for (tmp = priv->txbepring, i = 0;
-	     i < priv->txringcount;
-	     tmp += 8, i++) {
-		*tmp = *tmp & ~(1<<31);
-	}
-	for (tmp = priv->txvipring, i = 0;
-	     i < priv->txringcount;
-	     tmp += 8, i++) {
-		*tmp = *tmp & ~(1<<31);
-	}
-
-	for (tmp = priv->txvopring, i = 0;
-	     i < priv->txringcount;
-	     tmp += 8, i++) {
-		*tmp = *tmp & ~(1<<31);
-	}
-
-	for (tmp = priv->txhpring, i = 0;
-	     i < priv->txringcount;
-	     tmp += 8, i++) {
-		*tmp = *tmp & ~(1<<31);
-	}
-
-	for (tmp = priv->txbeaconring, i = 0;
-	     i < priv->txbeaconcount;
-	     tmp += 8, i++) {
-		*tmp = *tmp & ~(1<<31);
-	}
-
-	priv->txmapringtail = priv->txmapring;
-	priv->txmapringhead = priv->txmapring;
-	priv->txmapbufstail = priv->txmapbufs;
-
-	priv->txbkpringtail = priv->txbkpring;
-	priv->txbkpringhead = priv->txbkpring;
-	priv->txbkpbufstail = priv->txbkpbufs;
-
-	priv->txbepringtail = priv->txbepring;
-	priv->txbepringhead = priv->txbepring;
-	priv->txbepbufstail = priv->txbepbufs;
-
-	priv->txvipringtail = priv->txvipring;
-	priv->txvipringhead = priv->txvipring;
-	priv->txvipbufstail = priv->txvipbufs;
-
-	priv->txvopringtail = priv->txvopring;
-	priv->txvopringhead = priv->txvopring;
-	priv->txvopbufstail = priv->txvopbufs;
-
-	priv->txhpringtail = priv->txhpring;
-	priv->txhpringhead = priv->txhpring;
-	priv->txhpbufstail = priv->txhpbufs;
-
-	priv->txbeaconringtail = priv->txbeaconring;
-	priv->txbeaconbufstail = priv->txbeaconbufs;
-	set_nic_txring(dev);
-
-	ieee80211_reset_queue(priv->ieee80211);
-	priv->ack_tx_to_ieee = 0;
-}
-
-void fix_rx_fifo(struct net_device *dev)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	u32 *tmp;
-	struct buffer *rxbuf;
-	u8 rx_desc_size;
-
-	rx_desc_size = 8; /* 4*8 = 32 bytes */
-
-	for (tmp = priv->rxring, rxbuf = priv->rxbufferhead;
-	     (tmp < (priv->rxring)+(priv->rxringcount)*rx_desc_size);
-	     tmp += rx_desc_size, rxbuf = rxbuf->next) {
-		*(tmp+2) = rxbuf->dma;
-		*tmp = *tmp & ~0xfff;
-		*tmp = *tmp | priv->rxbuffersize;
-		*tmp |= (1<<31);
-	}
-
-	priv->rxringtail = priv->rxring;
-	priv->rxbuffer = priv->rxbufferhead;
-	priv->rx_skb_complete = 1;
-	set_nic_rxring(dev);
-}
-
-static void rtl8180_irq_disable(struct net_device *dev)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-
-	write_nic_dword(dev, IMR, 0);
-	force_pci_posting(dev);
-	priv->irq_enabled = 0;
-}
-
-void rtl8180_set_mode(struct net_device *dev, int mode)
-{
-	u8 ecmd;
-
-	ecmd = read_nic_byte(dev, EPROM_CMD);
-	ecmd = ecmd & ~EPROM_CMD_OPERATING_MODE_MASK;
-	ecmd = ecmd | (mode<<EPROM_CMD_OPERATING_MODE_SHIFT);
-	ecmd = ecmd & ~(1<<EPROM_CS_SHIFT);
-	ecmd = ecmd & ~(1<<EPROM_CK_SHIFT);
-	write_nic_byte(dev, EPROM_CMD, ecmd);
-}
-
-void rtl8180_beacon_tx_enable(struct net_device *dev);
-
-void rtl8180_update_msr(struct net_device *dev)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	u8 msr;
-	u32 rxconf;
-
-	msr  = read_nic_byte(dev, MSR);
-	msr &= ~MSR_LINK_MASK;
-
-	rxconf = read_nic_dword(dev, RX_CONF);
-
-	if (priv->ieee80211->state == IEEE80211_LINKED)	{
-		if (priv->ieee80211->iw_mode == IW_MODE_ADHOC)
-			msr |= (MSR_LINK_ADHOC<<MSR_LINK_SHIFT);
-		else if (priv->ieee80211->iw_mode == IW_MODE_MASTER)
-			msr |= (MSR_LINK_MASTER<<MSR_LINK_SHIFT);
-		else if (priv->ieee80211->iw_mode == IW_MODE_INFRA)
-			msr |= (MSR_LINK_MANAGED<<MSR_LINK_SHIFT);
-		else
-			msr |= (MSR_LINK_NONE<<MSR_LINK_SHIFT);
-		rxconf |= (1<<RX_CHECK_BSSID_SHIFT);
-
-	} else {
-		msr |= (MSR_LINK_NONE<<MSR_LINK_SHIFT);
-		rxconf &= ~(1<<RX_CHECK_BSSID_SHIFT);
-	}
-
-	write_nic_byte(dev, MSR, msr);
-	write_nic_dword(dev, RX_CONF, rxconf);
-}
-
-void rtl8180_set_chan(struct net_device *dev, short ch)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-
-	if ((ch > 14) || (ch < 1)) {
-		netdev_err(dev, "In %s: Invalid channel %d\n", __func__, ch);
-		return;
-	}
-
-	priv->chan = ch;
-	priv->rf_set_chan(dev, priv->chan);
-}
-
-void set_nic_txring(struct net_device *dev)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-
-	write_nic_dword(dev, TX_MANAGEPRIORITY_RING_ADDR, priv->txmapringdma);
-	write_nic_dword(dev, TX_BKPRIORITY_RING_ADDR, priv->txbkpringdma);
-	write_nic_dword(dev, TX_BEPRIORITY_RING_ADDR, priv->txbepringdma);
-	write_nic_dword(dev, TX_VIPRIORITY_RING_ADDR, priv->txvipringdma);
-	write_nic_dword(dev, TX_VOPRIORITY_RING_ADDR, priv->txvopringdma);
-	write_nic_dword(dev, TX_HIGHPRIORITY_RING_ADDR, priv->txhpringdma);
-	write_nic_dword(dev, TX_BEACON_RING_ADDR, priv->txbeaconringdma);
-}
-
-void rtl8180_beacon_tx_enable(struct net_device *dev)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-
-	rtl8180_set_mode(dev, EPROM_CMD_CONFIG);
-	priv->dma_poll_stop_mask &= ~(TPPOLLSTOP_BQ);
-	write_nic_byte(dev, TPPollStop, priv->dma_poll_mask);
-	rtl8180_set_mode(dev, EPROM_CMD_NORMAL);
-}
-
-void rtl8180_beacon_tx_disable(struct net_device *dev)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-
-	rtl8180_set_mode(dev, EPROM_CMD_CONFIG);
-	priv->dma_poll_stop_mask |= TPPOLLSTOP_BQ;
-	write_nic_byte(dev, TPPollStop, priv->dma_poll_stop_mask);
-	rtl8180_set_mode(dev, EPROM_CMD_NORMAL);
-
-}
-
-void rtl8180_rtx_disable(struct net_device *dev)
-{
-	u8 cmd;
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-	cmd = read_nic_byte(dev, CMD);
-	write_nic_byte(dev, CMD, cmd &
-		       ~((1<<CMD_RX_ENABLE_SHIFT)|(1<<CMD_TX_ENABLE_SHIFT)));
-	force_pci_posting(dev);
-	mdelay(10);
-
-	if (!priv->rx_skb_complete)
-		dev_kfree_skb_any(priv->rx_skb);
-}
-
-static short alloc_tx_desc_ring(struct net_device *dev, int bufsize, int count,
-				int addr)
-{
-	int i;
-	u32 *desc;
-	u32 *tmp;
-	dma_addr_t dma_desc, dma_tmp;
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	struct pci_dev *pdev = priv->pdev;
-	void *buf;
-
-	if ((bufsize & 0xfff) != bufsize) {
-		DMESGE("TX buffer allocation too large");
-		return 0;
-	}
-	desc = (u32 *)pci_alloc_consistent(pdev,
-					  sizeof(u32)*8*count+256, &dma_desc);
-	if (desc == NULL)
-		return -1;
-
-	if (dma_desc & 0xff)
-		/*
-		 * descriptor's buffer must be 256 byte aligned
-		 * we shouldn't be here, since we set DMA mask !
-		 */
-		WARN(1, "DMA buffer is not aligned\n");
-
-	tmp = desc;
-
-	for (i = 0; i < count; i++) {
-		buf = (void *)pci_alloc_consistent(pdev, bufsize, &dma_tmp);
-		if (buf == NULL)
-			return -ENOMEM;
-
-		switch (addr) {
-		case TX_MANAGEPRIORITY_RING_ADDR:
-			if (-1 == buffer_add(&priv->txmapbufs,
-				buf, dma_tmp, NULL)) {
-				DMESGE("Unable to allocate mem for buffer NP");
-				return -ENOMEM;
-			}
-			break;
-		case TX_BKPRIORITY_RING_ADDR:
-			if (-1 == buffer_add(&priv->txbkpbufs,
-				buf, dma_tmp, NULL)) {
-				DMESGE("Unable to allocate mem for buffer LP");
-				return -ENOMEM;
-			}
-			break;
-		case TX_BEPRIORITY_RING_ADDR:
-			if (-1 == buffer_add(&priv->txbepbufs,
-				buf, dma_tmp, NULL)) {
-				DMESGE("Unable to allocate mem for buffer NP");
-				return -ENOMEM;
-			}
-			break;
-		case TX_VIPRIORITY_RING_ADDR:
-			if (-1 == buffer_add(&priv->txvipbufs,
-				buf, dma_tmp, NULL)) {
-				DMESGE("Unable to allocate mem for buffer LP");
-				return -ENOMEM;
-			}
-			break;
-		case TX_VOPRIORITY_RING_ADDR:
-			if (-1 == buffer_add(&priv->txvopbufs,
-				buf, dma_tmp, NULL)) {
-				DMESGE("Unable to allocate mem for buffer NP");
-				return -ENOMEM;
-			}
-			break;
-		case TX_HIGHPRIORITY_RING_ADDR:
-			if (-1 == buffer_add(&priv->txhpbufs,
-				buf, dma_tmp, NULL)) {
-				DMESGE("Unable to allocate mem for buffer HP");
-				return -ENOMEM;
-			}
-			break;
-		case TX_BEACON_RING_ADDR:
-			if (-1 == buffer_add(&priv->txbeaconbufs,
-				buf, dma_tmp, NULL)) {
-				DMESGE("Unable to allocate mem for buffer BP");
-				return -ENOMEM;
-			}
-			break;
-		}
-		*tmp = *tmp & ~(1<<31); /* descriptor empty, owned by the drv */
-		*(tmp+2) = (u32)dma_tmp;
-		*(tmp+3) = bufsize;
-
-		if (i+1 < count)
-			*(tmp+4) = (u32)dma_desc+((i+1)*8*4);
-		else
-			*(tmp+4) = (u32)dma_desc;
-
-		tmp = tmp+8;
-	}
-
-	switch (addr) {
-	case TX_MANAGEPRIORITY_RING_ADDR:
-		priv->txmapringdma = dma_desc;
-		priv->txmapring = desc;
-		break;
-	case TX_BKPRIORITY_RING_ADDR:
-		priv->txbkpringdma = dma_desc;
-		priv->txbkpring = desc;
-		break;
-	case TX_BEPRIORITY_RING_ADDR:
-		priv->txbepringdma = dma_desc;
-		priv->txbepring = desc;
-		break;
-	case TX_VIPRIORITY_RING_ADDR:
-		priv->txvipringdma = dma_desc;
-		priv->txvipring = desc;
-		break;
-	case TX_VOPRIORITY_RING_ADDR:
-		priv->txvopringdma = dma_desc;
-		priv->txvopring = desc;
-		break;
-	case TX_HIGHPRIORITY_RING_ADDR:
-		priv->txhpringdma = dma_desc;
-		priv->txhpring = desc;
-		break;
-	case TX_BEACON_RING_ADDR:
-		priv->txbeaconringdma = dma_desc;
-		priv->txbeaconring = desc;
-		break;
-
-	}
-
-	return 0;
-}
-
-static void free_tx_desc_rings(struct net_device *dev)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	struct pci_dev *pdev = priv->pdev;
-	int count = priv->txringcount;
-
-	pci_free_consistent(pdev, sizeof(u32)*8*count+256,
-			    priv->txmapring, priv->txmapringdma);
-	buffer_free(dev, &(priv->txmapbufs), priv->txbuffsize, 1);
-
-	pci_free_consistent(pdev, sizeof(u32)*8*count+256,
-			    priv->txbkpring, priv->txbkpringdma);
-	buffer_free(dev, &(priv->txbkpbufs), priv->txbuffsize, 1);
-
-	pci_free_consistent(pdev, sizeof(u32)*8*count+256,
-			    priv->txbepring, priv->txbepringdma);
-	buffer_free(dev, &(priv->txbepbufs), priv->txbuffsize, 1);
-
-	pci_free_consistent(pdev, sizeof(u32)*8*count+256,
-			    priv->txvipring, priv->txvipringdma);
-	buffer_free(dev, &(priv->txvipbufs), priv->txbuffsize, 1);
-
-	pci_free_consistent(pdev, sizeof(u32)*8*count+256,
-			    priv->txvopring, priv->txvopringdma);
-	buffer_free(dev, &(priv->txvopbufs), priv->txbuffsize, 1);
-
-	pci_free_consistent(pdev, sizeof(u32)*8*count+256,
-			    priv->txhpring, priv->txhpringdma);
-	buffer_free(dev, &(priv->txhpbufs), priv->txbuffsize, 1);
-
-	count = priv->txbeaconcount;
-	pci_free_consistent(pdev, sizeof(u32)*8*count+256,
-			    priv->txbeaconring, priv->txbeaconringdma);
-	buffer_free(dev, &(priv->txbeaconbufs), priv->txbuffsize, 1);
-}
-
-static void free_rx_desc_ring(struct net_device *dev)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	struct pci_dev *pdev = priv->pdev;
-	int count = priv->rxringcount;
-
-	pci_free_consistent(pdev, sizeof(u32)*8*count+256,
-			    priv->rxring, priv->rxringdma);
-
-	buffer_free(dev, &(priv->rxbuffer), priv->rxbuffersize, 0);
-}
-
-static short alloc_rx_desc_ring(struct net_device *dev, u16 bufsize, int count)
-{
-	int i;
-	u32 *desc;
-	u32 *tmp;
-	dma_addr_t dma_desc, dma_tmp;
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	struct pci_dev *pdev = priv->pdev;
-	void *buf;
-	u8 rx_desc_size;
-
-	rx_desc_size = 8; /* 4*8 = 32 bytes */
-
-	if ((bufsize & 0xfff) != bufsize) {
-		DMESGE("RX buffer allocation too large");
-		return -1;
-	}
-
-	desc = (u32 *)pci_alloc_consistent(pdev,
-		sizeof(u32) * rx_desc_size * count + 256, &dma_desc);
-
-	if (dma_desc & 0xff)
-		/*
-		 * descriptor's buffer must be 256 byte aligned
-		 * should never happen since we specify the DMA mask
-		 */
-		WARN(1, "DMA buffer is not aligned\n");
-
-	priv->rxring = desc;
-	priv->rxringdma = dma_desc;
-	tmp = desc;
-
-	for (i = 0; i < count; i++) {
-		buf = kmalloc(bufsize * sizeof(u8), GFP_ATOMIC);
-		if (buf == NULL) {
-			DMESGE("Failed to kmalloc RX buffer");
-			return -1;
-		}
-
-		dma_tmp = pci_map_single(pdev, buf, bufsize * sizeof(u8),
-					 PCI_DMA_FROMDEVICE);
-		if (pci_dma_mapping_error(pdev, dma_tmp))
-			return -1;
-		if (-1 == buffer_add(&(priv->rxbuffer), buf, dma_tmp,
-			   &(priv->rxbufferhead))) {
-			DMESGE("Unable to allocate mem RX buf");
-			return -1;
-		}
-		*tmp = 0; /* zero pads the header of the descriptor */
-		*tmp = *tmp | (bufsize&0xfff);
-		*(tmp+2) = (u32)dma_tmp;
-		*tmp = *tmp | (1<<31); /* descriptor void, owned by the NIC */
-
-		tmp = tmp+rx_desc_size;
-	}
-
-	/* this is the last descriptor */
-	*(tmp - rx_desc_size) = *(tmp - rx_desc_size) | (1 << 30);
-
-	return 0;
-}
-
-
-void set_nic_rxring(struct net_device *dev)
-{
-	u8 pgreg;
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-
-	pgreg = read_nic_byte(dev, PGSELECT);
-	write_nic_byte(dev, PGSELECT, pgreg & ~(1<<PGSELECT_PG_SHIFT));
-
-	write_nic_dword(dev, RXRING_ADDR, priv->rxringdma);
-}
-
-void rtl8180_reset(struct net_device *dev)
-{
-	u8 cr;
-
-	rtl8180_irq_disable(dev);
-
-	cr = read_nic_byte(dev, CMD);
-	cr = cr & 2;
-	cr = cr | (1<<CMD_RST_SHIFT);
-	write_nic_byte(dev, CMD, cr);
-
-	force_pci_posting(dev);
-
-	mdelay(200);
-
-	if (read_nic_byte(dev, CMD) & (1<<CMD_RST_SHIFT))
-		DMESGW("Card reset timeout!");
-	else
-		DMESG("Card successfully reset");
-
-	rtl8180_set_mode(dev, EPROM_CMD_LOAD);
-	force_pci_posting(dev);
-	mdelay(200);
-}
-
-inline u16 ieeerate2rtlrate(int rate)
-{
-	switch (rate) {
-	case 10:
-		return 0;
-	case 20:
-		return 1;
-	case 55:
-		return 2;
-	case 110:
-		return 3;
-	case 60:
-		return 4;
-	case 90:
-		return 5;
-	case 120:
-		return 6;
-	case 180:
-		return 7;
-	case 240:
-		return 8;
-	case 360:
-		return 9;
-	case 480:
-		return 10;
-	case 540:
-		return 11;
-	default:
-		return 3;
-	}
-}
-
-static u16 rtl_rate[] = {10, 20, 55, 110, 60,
-	90, 120, 180, 240, 360, 480, 540, 720};
-
-inline u16 rtl8180_rate2rate(short rate)
-{
-	if (rate > 12)
-		return 10;
-	return rtl_rate[rate];
-}
-
-inline u8 rtl8180_IsWirelessBMode(u16 rate)
-{
-	if (((rate <= 110) && (rate != 60) && (rate != 90)) || (rate == 220))
-		return 1;
-	else
-		return 0;
-}
-
-u16 N_DBPSOfRate(u16 DataRate);
-
-static u16 ComputeTxTime(u16 FrameLength, u16 DataRate, u8 bManagementFrame,
-		  u8 bShortPreamble)
-{
-	u16	FrameTime;
-	u16	N_DBPS;
-	u16	Ceiling;
-
-	if (rtl8180_IsWirelessBMode(DataRate)) {
-		if (bManagementFrame || !bShortPreamble || DataRate == 10)
-			/* long preamble */
-			FrameTime = (u16)(144+48+(FrameLength*8/(DataRate/10)));
-		else
-			/* short preamble */
-			FrameTime = (u16)(72+24+(FrameLength*8/(DataRate/10)));
-
-		if ((FrameLength*8 % (DataRate/10)) != 0) /* get the ceilling */
-			FrameTime++;
-	} else {	/* 802.11g DSSS-OFDM PLCP length field calculation. */
-		N_DBPS = N_DBPSOfRate(DataRate);
-		Ceiling = (16 + 8*FrameLength + 6) / N_DBPS
-				+ (((16 + 8*FrameLength + 6) % N_DBPS) ? 1 : 0);
-		FrameTime = (u16)(16 + 4 + 4*Ceiling + 6);
-	}
-	return FrameTime;
-}
-
-u16 N_DBPSOfRate(u16 DataRate)
-{
-	 u16 N_DBPS = 24;
-
-	switch (DataRate) {
-	case 60:
-		N_DBPS = 24;
-		break;
-	case 90:
-		N_DBPS = 36;
-		break;
-	case 120:
-		N_DBPS = 48;
-		break;
-	case 180:
-		N_DBPS = 72;
-		break;
-	case 240:
-		N_DBPS = 96;
-		break;
-	case 360:
-		N_DBPS = 144;
-		break;
-	case 480:
-		N_DBPS = 192;
-		break;
-	case 540:
-		N_DBPS = 216;
-		break;
-	default:
-		break;
-	}
-
-	return N_DBPS;
-}
-
-/*
- * For Netgear case, they want good-looking signal strength.
- */
-static long NetgearSignalStrengthTranslate(long LastSS, long CurrSS)
-{
-	long RetSS;
-
-	/* Step 1. Scale mapping. */
-	if (CurrSS >= 71 && CurrSS <= 100)
-		RetSS = 90 + ((CurrSS - 70) / 3);
-	else if (CurrSS >= 41 && CurrSS <= 70)
-		RetSS = 78 + ((CurrSS - 40) / 3);
-	else if (CurrSS >= 31 && CurrSS <= 40)
-		RetSS = 66 + (CurrSS - 30);
-	else if (CurrSS >= 21 && CurrSS <= 30)
-		RetSS = 54 + (CurrSS - 20);
-	else if (CurrSS >= 5 && CurrSS <= 20)
-		RetSS = 42 + (((CurrSS - 5) * 2) / 3);
-	else if (CurrSS == 4)
-		RetSS = 36;
-	else if (CurrSS == 3)
-		RetSS = 27;
-	else if (CurrSS == 2)
-		RetSS = 18;
-	else if (CurrSS == 1)
-		RetSS = 9;
-	else
-		RetSS = CurrSS;
-
-	/* Step 2. Smoothing. */
-	if (LastSS > 0)
-		RetSS = ((LastSS * 5) + (RetSS) + 5) / 6;
-
-	return RetSS;
-}
-
-/*
- * Translate 0-100 signal strength index into dBm.
- */
-static long TranslateToDbm8185(u8 SignalStrengthIndex)
-{
-	long SignalPower;
-
-	/* Translate to dBm (x=0.5y-95). */
-	SignalPower = (long)((SignalStrengthIndex + 1) >> 1);
-	SignalPower -= 95;
-
-	return SignalPower;
-}
-
-/*
- * Perform signal smoothing for dynamic mechanism.
- * This is different with PerformSignalSmoothing8185 in smoothing formula.
- * No dramatic adjustment is applied because dynamic mechanism need some
- * degree of correctness. Ported from 8187B.
- */
-static void PerformUndecoratedSignalSmoothing8185(struct r8180_priv *priv,
-						  bool bCckRate)
-{
-	long smoothedSS;
-	long smoothedRx;
-
-	/* Determine the current packet is CCK rate. */
-	priv->bCurCCKPkt = bCckRate;
-
-	smoothedSS = priv->SignalStrength * 10;
-
-	if (priv->UndecoratedSmoothedSS >= 0)
-		smoothedSS = ((priv->UndecoratedSmoothedSS * 5) +
-				smoothedSS) / 6;
-
-	priv->UndecoratedSmoothedSS = smoothedSS;
-
-	smoothedRx = ((priv->UndecoratedSmoothedRxPower * 50) +
-			(priv->RxPower * 11)) / 60;
-
-	priv->UndecoratedSmoothedRxPower = smoothedRx;
-
-	if (bCckRate)
-		priv->CurCCKRSSI = priv->RSSI;
-	else
-		priv->CurCCKRSSI = 0;
-}
-
-
-/*
- * This is rough RX isr handling routine
- */
-static void rtl8180_rx(struct net_device *dev)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	struct sk_buff *tmp_skb;
-	short first, last;
-	u32 len;
-	int lastlen;
-	unsigned char quality, signal;
-	u8 rate;
-	u32 *tmp, *tmp2;
-	u8 rx_desc_size;
-	u8 padding;
-	char rxpower = 0;
-	u32 RXAGC = 0;
-	long RxAGC_dBm = 0;
-	u8	LNA = 0, BB = 0;
-	u8	LNA_gain[4] = {02, 17, 29, 39};
-	u8  Antenna = 0;
-	struct ieee80211_hdr_4addr *hdr;
-	u16 fc, type;
-	u8 bHwError = 0, bCRC = 0, bICV = 0;
-	bool	bCckRate = false;
-	u8     RSSI = 0;
-	long	SignalStrengthIndex = 0;
-	struct ieee80211_rx_stats stats = {
-		.signal = 0,
-		.noise = -98,
-		.rate = 0,
-		.freq = IEEE80211_24GHZ_BAND,
-	};
-
-	stats.nic_type = NIC_8185B;
-	rx_desc_size = 8;
-
-	if ((*(priv->rxringtail)) & (1<<31)) {
-		/* we have got an RX int, but the descriptor. we are pointing
-		 * is empty.
-		 */
-
-		priv->stats.rxnodata++;
-		priv->ieee80211->stats.rx_errors++;
-
-		tmp2 = NULL;
-		tmp = priv->rxringtail;
-		do {
-			if (tmp == priv->rxring)
-				tmp  = priv->rxring + (priv->rxringcount - 1) *
-					rx_desc_size;
-			else
-				tmp -= rx_desc_size;
-
-			if (!(*tmp & (1<<31)))
-				tmp2 = tmp;
-		} while (tmp != priv->rxring);
-
-		if (tmp2)
-			priv->rxringtail = tmp2;
-	}
-
-	/* while there are filled descriptors */
-	while (!(*(priv->rxringtail) & (1<<31))) {
-		if (*(priv->rxringtail) & (1<<26))
-			DMESGW("RX buffer overflow");
-		if (*(priv->rxringtail) & (1<<12))
-			priv->stats.rxicverr++;
-
-		if (*(priv->rxringtail) & (1<<27)) {
-			priv->stats.rxdmafail++;
-			goto drop;
-		}
-
-		pci_dma_sync_single_for_cpu(priv->pdev,
-				    priv->rxbuffer->dma,
-				    priv->rxbuffersize * sizeof(u8),
-				    PCI_DMA_FROMDEVICE);
-
-		first = *(priv->rxringtail) & (1<<29) ? 1 : 0;
-		if (first)
-			priv->rx_prevlen = 0;
-
-		last = *(priv->rxringtail) & (1<<28) ? 1 : 0;
-		if (last) {
-			lastlen = ((*priv->rxringtail) & 0xfff);
-
-			/* if the last descriptor (that should tell us the total
-			 * packet len) tell us something less than the
-			 * descriptors len we had until now, then there is some
-			 * problem..
-			 * workaround to prevent kernel panic
-			 */
-			if (lastlen < priv->rx_prevlen)
-				len = 0;
-			else
-				len = lastlen-priv->rx_prevlen;
-
-			if (*(priv->rxringtail) & (1<<13)) {
-				if ((*(priv->rxringtail) & 0xfff) < 500)
-					priv->stats.rxcrcerrmin++;
-				else if ((*(priv->rxringtail) & 0x0fff) > 1000)
-					priv->stats.rxcrcerrmax++;
-				else
-					priv->stats.rxcrcerrmid++;
-
-			}
-
-		} else {
-			len = priv->rxbuffersize;
-		}
-
-		if (first && last) {
-			padding = ((*(priv->rxringtail+3))&(0x04000000))>>26;
-		} else if (first) {
-			padding = ((*(priv->rxringtail+3))&(0x04000000))>>26;
-			if (padding)
-				len -= 2;
-		} else {
-			padding = 0;
-		}
-		padding = 0;
-		priv->rx_prevlen += len;
-
-		if (priv->rx_prevlen > MAX_FRAG_THRESHOLD + 100) {
-			/* HW is probably passing several buggy frames without
-			 * FD or LD flag set.
-			 * Throw this garbage away to prevent skb memory
-			 * exhausting
-			 */
-			if (!priv->rx_skb_complete)
-				dev_kfree_skb_any(priv->rx_skb);
-			priv->rx_skb_complete = 1;
-		}
-
-		signal = (unsigned char)((*(priv->rxringtail + 3) &
-			0x00ff0000) >> 16);
-		signal = (signal & 0xfe) >> 1;
-
-		quality = (unsigned char)((*(priv->rxringtail+3)) & (0xff));
-
-		stats.mac_time[0] = *(priv->rxringtail+1);
-		stats.mac_time[1] = *(priv->rxringtail+2);
-
-		rxpower = ((char)((*(priv->rxringtail + 4) &
-			0x00ff0000) >> 16)) / 2 - 42;
-
-		RSSI = ((u8)((*(priv->rxringtail + 3) &
-			0x0000ff00) >> 8)) & 0x7f;
-
-		rate = ((*(priv->rxringtail)) &
-			((1<<23)|(1<<22)|(1<<21)|(1<<20)))>>20;
-
-		stats.rate = rtl8180_rate2rate(rate);
-		Antenna = (*(priv->rxringtail + 3) & 0x00008000) == 0 ? 0 : 1;
-		if (!rtl8180_IsWirelessBMode(stats.rate)) { /* OFDM rate. */
-			RxAGC_dBm = rxpower+1;	/* bias */
-		} else { /* CCK rate. */
-			RxAGC_dBm = signal; /* bit 0 discard */
-
-			LNA = (u8) (RxAGC_dBm & 0x60) >> 5; /* bit 6~ bit 5 */
-			BB  = (u8) (RxAGC_dBm & 0x1F); /* bit 4 ~ bit 0 */
-
-			/* Pin_11b=-(LNA_gain+BB_gain) (dBm) */
-			RxAGC_dBm = -(LNA_gain[LNA] + (BB * 2));
-
-			RxAGC_dBm += 4; /* bias */
-		}
-
-		if (RxAGC_dBm & 0x80) /* absolute value */
-			RXAGC = ~(RxAGC_dBm)+1;
-		bCckRate = rtl8180_IsWirelessBMode(stats.rate);
-		/* Translate RXAGC into 1-100. */
-		if (!rtl8180_IsWirelessBMode(stats.rate)) { /* OFDM rate. */
-			if (RXAGC > 90)
-				RXAGC = 90;
-			else if (RXAGC < 25)
-				RXAGC = 25;
-			RXAGC = (90-RXAGC)*100/65;
-		} else { /* CCK rate. */
-			if (RXAGC > 95)
-				RXAGC = 95;
-			else if (RXAGC < 30)
-				RXAGC = 30;
-			RXAGC = (95-RXAGC)*100/65;
-		}
-		priv->SignalStrength = (u8)RXAGC;
-		priv->RecvSignalPower = RxAGC_dBm;
-		priv->RxPower = rxpower;
-		priv->RSSI = RSSI;
-		/* SQ translation formula is provided by SD3 DZ. 2006.06.27 */
-		if (quality >= 127)
-			/* 0 causes epc to show signal zero, walk around now */
-			quality = 1;
-		else if (quality < 27)
-			quality = 100;
-		else
-			quality = 127 - quality;
-		priv->SignalQuality = quality;
-
-		stats.signal = (u8) quality;
-
-		stats.signalstrength = RXAGC;
-		if (stats.signalstrength > 100)
-			stats.signalstrength = 100;
-		stats.signalstrength = (stats.signalstrength * 70) / 100 + 30;
-		stats.rssi = priv->wstats.qual.qual = priv->SignalQuality;
-		stats.noise = priv->wstats.qual.noise =
-			100 - priv->wstats.qual.qual;
-		bHwError = (((*(priv->rxringtail)) & (0x00000fff)) == 4080) |
-			   (((*(priv->rxringtail)) & (0x04000000)) != 0) |
-			   (((*(priv->rxringtail)) & (0x08000000)) != 0) |
-			   (((~(*(priv->rxringtail))) & (0x10000000)) != 0) |
-			   (((~(*(priv->rxringtail))) & (0x20000000)) != 0);
-		bCRC = ((*(priv->rxringtail)) & (0x00002000)) >> 13;
-		bICV = ((*(priv->rxringtail)) & (0x00001000)) >> 12;
-		hdr = (struct ieee80211_hdr_4addr *)priv->rxbuffer->buf;
-		    fc = le16_to_cpu(hdr->frame_ctl);
-		type = WLAN_FC_GET_TYPE(fc);
-
-		if (IEEE80211_FTYPE_CTL != type &&
-		    !bHwError && !bCRC && !bICV &&
-		    eqMacAddr(priv->ieee80211->current_network.bssid,
-			fc & IEEE80211_FCTL_TODS ? hdr->addr1 :
-			fc & IEEE80211_FCTL_FROMDS ? hdr->addr2 :
-			hdr->addr3)) {
-
-			/* Perform signal smoothing for dynamic
-			 * mechanism on demand. This is different
-			 * with PerformSignalSmoothing8185 in smoothing
-			 * fomula. No dramatic adjustion is apply
-			 * because dynamic mechanism need some degree
-			 * of correctness. */
-			PerformUndecoratedSignalSmoothing8185(priv, bCckRate);
-
-			/* For good-looking singal strength. */
-			SignalStrengthIndex = NetgearSignalStrengthTranslate(
-				priv->LastSignalStrengthInPercent,
-				priv->SignalStrength);
-
-			priv->LastSignalStrengthInPercent = SignalStrengthIndex;
-			priv->Stats_SignalStrength =
-				TranslateToDbm8185((u8)SignalStrengthIndex);
-
-			/*
-			 * We need more correct power of received packets and
-			 * the "SignalStrength" of RxStats is beautified, so we
-			 * record the correct power here.
-			 */
-
-			priv->Stats_SignalQuality = (long)(
-				priv->Stats_SignalQuality * 5 +
-				(long)priv->SignalQuality + 5) / 6;
-
-			priv->Stats_RecvSignalPower = (long)(
-				priv->Stats_RecvSignalPower * 5 +
-				priv->RecvSignalPower - 1) / 6;
-
-			/*
-			 * Figure out which antenna received the last packet.
-			 * 0: aux, 1: main
-			 */
-			priv->LastRxPktAntenna = Antenna ? 1 : 0;
-			SwAntennaDiversityRxOk8185(dev, priv->SignalStrength);
-		}
-
-		if (first) {
-			if (!priv->rx_skb_complete) {
-				/* seems that HW sometimes fails to receive and
-				 * doesn't provide the last descriptor.
-				 */
-				dev_kfree_skb_any(priv->rx_skb);
-				priv->stats.rxnolast++;
-			}
-			priv->rx_skb = dev_alloc_skb(len+2);
-			if (!priv->rx_skb)
-				goto drop;
-
-			priv->rx_skb_complete = 0;
-			priv->rx_skb->dev = dev;
-		} else {
-			/* if we are here we should have already RXed the first
-			 * frame.
-			 * If we get here and the skb is not allocated then
-			 * we have just throw out garbage (skb not allocated)
-			 * and we are still rxing garbage....
-			 */
-			if (!priv->rx_skb_complete) {
-
-				tmp_skb = dev_alloc_skb(
-					priv->rx_skb->len + len + 2);
-
-				if (!tmp_skb)
-					goto drop;
-
-				tmp_skb->dev = dev;
-
-				memcpy(skb_put(tmp_skb, priv->rx_skb->len),
-					priv->rx_skb->data,
-					priv->rx_skb->len);
-
-				dev_kfree_skb_any(priv->rx_skb);
-
-				priv->rx_skb = tmp_skb;
-			}
-		}
-
-		if (!priv->rx_skb_complete) {
-			memcpy(skb_put(priv->rx_skb, len), ((unsigned char *)
-				priv->rxbuffer->buf) + (padding ? 2 : 0), len);
-		}
-
-		if (last && !priv->rx_skb_complete) {
-			if (priv->rx_skb->len > 4)
-				skb_trim(priv->rx_skb, priv->rx_skb->len-4);
-			if (!ieee80211_rtl_rx(priv->ieee80211,
-					 priv->rx_skb, &stats))
-				dev_kfree_skb_any(priv->rx_skb);
-			priv->rx_skb_complete = 1;
-		}
-
-		pci_dma_sync_single_for_device(priv->pdev,
-				    priv->rxbuffer->dma,
-				    priv->rxbuffersize * sizeof(u8),
-				    PCI_DMA_FROMDEVICE);
-
-drop: /* this is used when we have not enough mem */
-		/* restore the descriptor */
-		*(priv->rxringtail+2) = priv->rxbuffer->dma;
-		*(priv->rxringtail) = *(priv->rxringtail) & ~0xfff;
-		*(priv->rxringtail) =
-			*(priv->rxringtail) | priv->rxbuffersize;
-
-		*(priv->rxringtail) =
-			*(priv->rxringtail) | (1<<31);
-
-		priv->rxringtail += rx_desc_size;
-		if (priv->rxringtail >=
-		   (priv->rxring)+(priv->rxringcount)*rx_desc_size)
-			priv->rxringtail = priv->rxring;
-
-		priv->rxbuffer = (priv->rxbuffer->next);
-	}
-}
-
-
-static void rtl8180_dma_kick(struct net_device *dev, int priority)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-
-	rtl8180_set_mode(dev, EPROM_CMD_CONFIG);
-	write_nic_byte(dev, TX_DMA_POLLING,
-			(1 << (priority + 1)) | priv->dma_poll_mask);
-	rtl8180_set_mode(dev, EPROM_CMD_NORMAL);
-
-	force_pci_posting(dev);
-}
-
-static void rtl8180_data_hard_stop(struct net_device *dev)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-
-	rtl8180_set_mode(dev, EPROM_CMD_CONFIG);
-	priv->dma_poll_stop_mask |= TPPOLLSTOP_AC_VIQ;
-	write_nic_byte(dev, TPPollStop, priv->dma_poll_stop_mask);
-	rtl8180_set_mode(dev, EPROM_CMD_NORMAL);
-}
-
-static void rtl8180_data_hard_resume(struct net_device *dev)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-
-	rtl8180_set_mode(dev, EPROM_CMD_CONFIG);
-	priv->dma_poll_stop_mask &= ~(TPPOLLSTOP_AC_VIQ);
-	write_nic_byte(dev, TPPollStop, priv->dma_poll_stop_mask);
-	rtl8180_set_mode(dev, EPROM_CMD_NORMAL);
-}
-
-/*
- * This function TX data frames when the ieee80211 stack requires this.
- * It checks also if we need to stop the ieee tx queue, eventually do it
- */
-static void rtl8180_hard_data_xmit(struct sk_buff *skb, struct net_device *dev,
-				   int rate)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	int mode;
-	struct ieee80211_hdr_3addr *h = (struct ieee80211_hdr_3addr *)skb->data;
-	bool morefrag = le16_to_cpu(h->frame_control) & IEEE80211_FCTL_MOREFRAGS;
-	unsigned long flags;
-	int priority;
-
-	mode = priv->ieee80211->iw_mode;
-
-	rate = ieeerate2rtlrate(rate);
-	/*
-	 * This function doesn't require lock because we make sure it's called
-	 * with the tx_lock already acquired.
-	 * This come from the kernel's hard_xmit callback (through the ieee
-	 * stack, or from the try_wake_queue (again through the ieee stack.
-	 */
-	priority = AC2Q(skb->priority);
-	spin_lock_irqsave(&priv->tx_lock, flags);
-
-	if (priv->ieee80211->bHwRadioOff) {
-		spin_unlock_irqrestore(&priv->tx_lock, flags);
-
-		return;
-	}
-
-	if (!check_nic_enought_desc(dev, priority)) {
-		DMESGW("Error: no descriptor left by previous TX (avail %d) ",
-			get_curr_tx_free_desc(dev, priority));
-		ieee80211_rtl_stop_queue(priv->ieee80211);
-	}
-	rtl8180_tx(dev, skb->data, skb->len, priority, morefrag, 0, rate);
-	if (!check_nic_enought_desc(dev, priority))
-		ieee80211_rtl_stop_queue(priv->ieee80211);
-
-	spin_unlock_irqrestore(&priv->tx_lock, flags);
-}
-
-/*
- * This is a rough attempt to TX a frame
- * This is called by the ieee 80211 stack to TX management frames.
- * If the ring is full packets are dropped (for data frame the queue
- * is stopped before this can happen). For this reason it is better
- * if the descriptors are larger than the largest management frame
- * we intend to TX: i'm unsure what the HW does if it will not find
- * the last fragment of a frame because it has been dropped...
- * Since queues for Management and Data frames are different we
- * might use a different lock than tx_lock (for example mgmt_tx_lock)
- */
-/* these function may loop if invoked with 0 descriptors or 0 len buffer */
-static int rtl8180_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	unsigned long flags;
-	int priority;
-
-	priority = MANAGE_PRIORITY;
-
-	spin_lock_irqsave(&priv->tx_lock, flags);
-
-	if (priv->ieee80211->bHwRadioOff) {
-		spin_unlock_irqrestore(&priv->tx_lock, flags);
-		dev_kfree_skb_any(skb);
-		return NETDEV_TX_OK;
-	}
-
-	rtl8180_tx(dev, skb->data, skb->len, priority,
-		0, 0, ieeerate2rtlrate(priv->ieee80211->basic_rate));
-
-	priv->ieee80211->stats.tx_bytes += skb->len;
-	priv->ieee80211->stats.tx_packets++;
-	spin_unlock_irqrestore(&priv->tx_lock, flags);
-
-	dev_kfree_skb_any(skb);
-	return NETDEV_TX_OK;
-}
-
-static void rtl8180_prepare_beacon(struct net_device *dev)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	struct sk_buff *skb;
-
-	u16 word  = read_nic_word(dev, BcnItv);
-	word &= ~BcnItv_BcnItv; /* clear Bcn_Itv */
-
-	/* word |= 0x64; */
-	word |= cpu_to_le16(priv->ieee80211->current_network.beacon_interval);
-
-	write_nic_word(dev, BcnItv, word);
-
-	skb = ieee80211_get_beacon(priv->ieee80211);
-	if (skb) {
-		rtl8180_tx(dev, skb->data, skb->len, BEACON_PRIORITY,
-			0, 0, ieeerate2rtlrate(priv->ieee80211->basic_rate));
-		dev_kfree_skb_any(skb);
-	}
-}
-
-/*
- * This function do the real dirty work: it enqueues a TX command descriptor in
- * the ring buffer, copyes the frame in a TX buffer and kicks the NIC to ensure
- * it does the DMA transfer.
- */
-short rtl8180_tx(struct net_device *dev, u8 *txbuf, int len, int priority,
-		 bool morefrag, short descfrag, int rate)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	u32 *tail, *temp_tail;
-	u32 *begin;
-	u32 *buf;
-	int i;
-	int remain;
-	int buflen;
-	int count;
-	struct buffer *buflist;
-	struct ieee80211_hdr_3addr *frag_hdr =
-		(struct ieee80211_hdr_3addr *)txbuf;
-	u8 dest[ETH_ALEN];
-	u8 bUseShortPreamble = 0;
-	u8 bCTSEnable = 0;
-	u8 bRTSEnable = 0;
-	u16 Duration = 0;
-	u16 RtsDur = 0;
-	u16 ThisFrameTime = 0;
-	u16 TxDescDuration = 0;
-	bool ownbit_flag = false;
-
-	switch (priority) {
-	case MANAGE_PRIORITY:
-		tail = priv->txmapringtail;
-		begin = priv->txmapring;
-		buflist = priv->txmapbufstail;
-		count = priv->txringcount;
-		break;
-	case BK_PRIORITY:
-		tail = priv->txbkpringtail;
-		begin = priv->txbkpring;
-		buflist = priv->txbkpbufstail;
-		count = priv->txringcount;
-		break;
-	case BE_PRIORITY:
-		tail = priv->txbepringtail;
-		begin = priv->txbepring;
-		buflist = priv->txbepbufstail;
-		count = priv->txringcount;
-		break;
-	case VI_PRIORITY:
-		tail = priv->txvipringtail;
-		begin = priv->txvipring;
-		buflist = priv->txvipbufstail;
-		count = priv->txringcount;
-		break;
-	case VO_PRIORITY:
-		tail = priv->txvopringtail;
-		begin = priv->txvopring;
-		buflist = priv->txvopbufstail;
-		count = priv->txringcount;
-		break;
-	case HI_PRIORITY:
-		tail = priv->txhpringtail;
-		begin = priv->txhpring;
-		buflist = priv->txhpbufstail;
-		count = priv->txringcount;
-		break;
-	case BEACON_PRIORITY:
-		tail = priv->txbeaconringtail;
-		begin = priv->txbeaconring;
-		buflist = priv->txbeaconbufstail;
-		count = priv->txbeaconcount;
-		break;
-	default:
-		return -1;
-		break;
-	}
-
-	memcpy(&dest, frag_hdr->addr1, ETH_ALEN);
-	if (is_multicast_ether_addr(dest)) {
-		Duration = 0;
-		RtsDur = 0;
-		bRTSEnable = 0;
-		bCTSEnable = 0;
-
-		ThisFrameTime = ComputeTxTime(len + sCrcLng,
-			rtl8180_rate2rate(rate), 0, bUseShortPreamble);
-		TxDescDuration = ThisFrameTime;
-	} else { /* Unicast packet */
-		u16 AckTime;
-
-		/* for Keep alive */
-		priv->NumTxUnicast++;
-
-		/* Figure out ACK rate according to BSS basic rate
-		 * and Tx rate.
-		 * AckCTSLng = 14 use 1M bps send
-		 */
-		AckTime = ComputeTxTime(14, 10, 0, 0);
-
-		if (((len + sCrcLng) > priv->rts) && priv->rts) { /* RTS/CTS. */
-			u16 RtsTime, CtsTime;
-			bRTSEnable = 1;
-			bCTSEnable = 0;
-
-			/* Rate and time required for RTS. */
-			RtsTime = ComputeTxTime(sAckCtsLng / 8,
-				priv->ieee80211->basic_rate, 0, 0);
-
-			/* Rate and time required for CTS.
-			 * AckCTSLng = 14 use 1M bps send
-			 */
-			CtsTime = ComputeTxTime(14, 10, 0, 0);
-
-			/* Figure out time required to transmit this frame. */
-			ThisFrameTime = ComputeTxTime(len + sCrcLng,
-				rtl8180_rate2rate(rate), 0,
-				bUseShortPreamble);
-
-			/* RTS-CTS-ThisFrame-ACK. */
-			RtsDur = CtsTime + ThisFrameTime +
-				AckTime + 3 * aSifsTime;
-
-			TxDescDuration = RtsTime + RtsDur;
-		} else { /* Normal case. */
-			bCTSEnable = 0;
-			bRTSEnable = 0;
-			RtsDur = 0;
-
-			ThisFrameTime = ComputeTxTime(len + sCrcLng,
-				rtl8180_rate2rate(rate), 0, bUseShortPreamble);
-			TxDescDuration = ThisFrameTime + aSifsTime + AckTime;
-		}
-
-		if (!(le16_to_cpu(frag_hdr->frame_control) & IEEE80211_FCTL_MOREFRAGS)) {
-			/* ThisFrame-ACK. */
-			Duration = aSifsTime + AckTime;
-		} else { /* One or more fragments remained. */
-			u16 NextFragTime;
-
-			/* pretend following packet length = current packet */
-			NextFragTime = ComputeTxTime(len + sCrcLng,
-				rtl8180_rate2rate(rate), 0, bUseShortPreamble);
-
-			/* ThisFrag-ACk-NextFrag-ACK. */
-			Duration = NextFragTime + 3 * aSifsTime + 2 * AckTime;
-		}
-
-	} /* End of Unicast packet */
-
-	frag_hdr->duration_id = Duration;
-
-	buflen = priv->txbuffsize;
-	remain = len;
-	temp_tail = tail;
-
-	while (remain != 0) {
-		mb();
-		if (!buflist) {
-			DMESGE("TX buffer error, cannot TX frames. pri %d.",
-				priority);
-			return -1;
-		}
-		buf = buflist->buf;
-
-		if ((*tail & (1 << 31)) && (priority != BEACON_PRIORITY)) {
-			DMESGW("No more TX desc, returning %x of %x",
-			       remain, len);
-			priv->stats.txrdu++;
-			return remain;
-		}
-
-		*tail = 0; /* zeroes header */
-		*(tail+1) = 0;
-		*(tail+3) = 0;
-		*(tail+5) = 0;
-		*(tail+6) = 0;
-		*(tail+7) = 0;
-
-		/* FIXME: should be triggered by HW encryption parameters.*/
-		*tail |= (1<<15); /* no encrypt */
-
-		if (remain == len && !descfrag) {
-			ownbit_flag = false;
-			*tail = *tail | (1 << 29); /* first segment of packet */
-			*tail = *tail | (len);
-		} else {
-			ownbit_flag = true;
-		}
-
-		for (i = 0; i < buflen && remain > 0; i++, remain--) {
-			/* copy data into descriptor pointed DMAble buffer */
-			((u8 *)buf)[i] = txbuf[i];
-
-			if (remain == 4 && i+4 >= buflen)
-				break;
-			/* ensure the last desc has at least 4 bytes payload */
-		}
-		txbuf = txbuf + i;
-		*(tail+3) = *(tail+3) & ~0xfff;
-		*(tail+3) = *(tail+3) | i; /* buffer length */
-
-		if (bCTSEnable)
-			*tail |= (1<<18);
-
-		if (bRTSEnable) { /* rts enable */
-			/* RTS RATE */
-			*tail |= (ieeerate2rtlrate(
-				priv->ieee80211->basic_rate) << 19);
-
-			*tail |= (1<<23); /* rts enable */
-			*(tail+1) |= (RtsDur&0xffff); /* RTS Duration */
-		}
-		*(tail+3) |= ((TxDescDuration&0xffff)<<16); /* DURATION */
-
-		*(tail + 5) |= (11 << 8); /* retry lim; */
-
-		*tail = *tail | ((rate&0xf) << 24);
-
-		if (morefrag)
-			*tail = (*tail) | (1<<17); /* more fragment */
-		if (!remain)
-			*tail = (*tail) | (1<<28); /* last segment of frame */
-
-		*(tail+5) = *(tail+5)|(2<<27);
-		*(tail+7) = *(tail+7)|(1<<4);
-
-		wmb();
-		if (ownbit_flag)
-			/* descriptor ready to be txed */
-			*tail |= (1 << 31);
-
-		if ((tail - begin)/8 == count-1)
-			tail = begin;
-		else
-			tail = tail+8;
-
-		buflist = buflist->next;
-
-		mb();
-
-		switch (priority) {
-		case MANAGE_PRIORITY:
-			priv->txmapringtail = tail;
-			priv->txmapbufstail = buflist;
-			break;
-		case BK_PRIORITY:
-			priv->txbkpringtail = tail;
-			priv->txbkpbufstail = buflist;
-			break;
-		case BE_PRIORITY:
-			priv->txbepringtail = tail;
-			priv->txbepbufstail = buflist;
-			break;
-		case VI_PRIORITY:
-			priv->txvipringtail = tail;
-			priv->txvipbufstail = buflist;
-			break;
-		case VO_PRIORITY:
-			priv->txvopringtail = tail;
-			priv->txvopbufstail = buflist;
-			break;
-		case HI_PRIORITY:
-			priv->txhpringtail = tail;
-			priv->txhpbufstail = buflist;
-			break;
-		case BEACON_PRIORITY:
-			/*
-			 * The HW seems to be happy with the 1st
-			 * descriptor filled and the 2nd empty...
-			 * So always update descriptor 1 and never
-			 * touch 2nd
-			 */
-			break;
-		}
-	}
-	*temp_tail = *temp_tail | (1<<31); /* descriptor ready to be txed */
-	rtl8180_dma_kick(dev, priority);
-
-	return 0;
-}
-
-void rtl8180_irq_rx_tasklet(struct r8180_priv *priv);
-
-static void rtl8180_link_change(struct net_device *dev)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	u16 beacon_interval;
-	struct ieee80211_network *net = &priv->ieee80211->current_network;
-
-	rtl8180_update_msr(dev);
-
-	rtl8180_set_mode(dev, EPROM_CMD_CONFIG);
-
-	write_nic_dword(dev, BSSID, ((u32 *)net->bssid)[0]);
-	write_nic_word(dev, BSSID+4, ((u16 *)net->bssid)[2]);
-
-	beacon_interval  = read_nic_word(dev, BEACON_INTERVAL);
-	beacon_interval &= ~BEACON_INTERVAL_MASK;
-	beacon_interval |= net->beacon_interval;
-	write_nic_word(dev, BEACON_INTERVAL, beacon_interval);
-
-	rtl8180_set_mode(dev, EPROM_CMD_NORMAL);
-
-	rtl8180_set_chan(dev, priv->chan);
-}
-
-static void rtl8180_rq_tx_ack(struct net_device *dev)
-{
-
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-	write_nic_byte(dev, CONFIG4,
-		read_nic_byte(dev, CONFIG4) | CONFIG4_PWRMGT);
-	priv->ack_tx_to_ieee = 1;
-}
-
-static short rtl8180_is_tx_queue_empty(struct net_device *dev)
-{
-
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	u32 *d;
-
-	for (d = priv->txmapring;
-		d < priv->txmapring + priv->txringcount; d += 8)
-			if (*d & (1<<31))
-				return 0;
-
-	for (d = priv->txbkpring;
-		d < priv->txbkpring + priv->txringcount; d += 8)
-			if (*d & (1<<31))
-				return 0;
-
-	for (d = priv->txbepring;
-		d < priv->txbepring + priv->txringcount; d += 8)
-			if (*d & (1<<31))
-				return 0;
-
-	for (d = priv->txvipring;
-		d < priv->txvipring + priv->txringcount; d += 8)
-			if (*d & (1<<31))
-				return 0;
-
-	for (d = priv->txvopring;
-		d < priv->txvopring + priv->txringcount; d += 8)
-			if (*d & (1<<31))
-				return 0;
-
-	for (d = priv->txhpring;
-		d < priv->txhpring + priv->txringcount; d += 8)
-			if (*d & (1<<31))
-				return 0;
-	return 1;
-}
-
-static void rtl8180_hw_wakeup(struct net_device *dev)
-{
-	unsigned long flags;
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-	spin_lock_irqsave(&priv->ps_lock, flags);
-	write_nic_byte(dev, CONFIG4,
-		read_nic_byte(dev, CONFIG4) & ~CONFIG4_PWRMGT);
-	if (priv->rf_wakeup)
-		priv->rf_wakeup(dev);
-	spin_unlock_irqrestore(&priv->ps_lock, flags);
-}
-
-static void rtl8180_hw_sleep_down(struct net_device *dev)
-{
-	unsigned long flags;
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-	spin_lock_irqsave(&priv->ps_lock, flags);
-	if (priv->rf_sleep)
-		priv->rf_sleep(dev);
-	spin_unlock_irqrestore(&priv->ps_lock, flags);
-}
-
-static void rtl8180_hw_sleep(struct net_device *dev, u32 th, u32 tl)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	u32 rb = jiffies;
-	unsigned long flags;
-
-	spin_lock_irqsave(&priv->ps_lock, flags);
-
-	/*
-	 * Writing HW register with 0 equals to disable
-	 * the timer, that is not really what we want
-	 */
-	tl -= MSECS(4+16+7);
-
-	/*
-	 * If the interval in which we are requested to sleep is too
-	 * short then give up and remain awake
-	 */
-	if (((tl >= rb) && (tl-rb) <= MSECS(MIN_SLEEP_TIME))
-		|| ((rb > tl) && (rb-tl) < MSECS(MIN_SLEEP_TIME))) {
-		spin_unlock_irqrestore(&priv->ps_lock, flags);
-		netdev_warn(dev, "too short to sleep\n");
-		return;
-	}
-
-	{
-		u32 tmp = (tl > rb) ? (tl-rb) : (rb-tl);
-
-		priv->DozePeriodInPast2Sec += jiffies_to_msecs(tmp);
-		/* as tl may be less than rb */
-		queue_delayed_work(priv->ieee80211->wq,
-			&priv->ieee80211->hw_wakeup_wq, tmp);
-	}
-	/*
-	 * If we suspect the TimerInt is gone beyond tl
-	 * while setting it, then give up
-	 */
-
-	if (((tl > rb) && ((tl-rb) > MSECS(MAX_SLEEP_TIME))) ||
-		((tl < rb) && ((rb-tl) > MSECS(MAX_SLEEP_TIME)))) {
-		spin_unlock_irqrestore(&priv->ps_lock, flags);
-		return;
-	}
-
-	queue_work(priv->ieee80211->wq, (void *)&priv->ieee80211->hw_sleep_wq);
-	spin_unlock_irqrestore(&priv->ps_lock, flags);
-}
-
-static void rtl8180_wmm_single_param_update(struct net_device *dev,
-	u8 mode, AC_CODING eACI, PAC_PARAM param)
-{
-	u8 u1bAIFS;
-	u32 u4bAcParam;
-
-	/* Retrieve parameters to update. */
-	/* Mode G/A: slotTimeTimer = 9; Mode B: 20 */
-	u1bAIFS = param->f.AciAifsn.f.AIFSN * ((mode & IEEE_G) == IEEE_G ?
-		9 : 20) + aSifsTime;
-	u4bAcParam = (((u32)param->f.TXOPLimit << AC_PARAM_TXOP_LIMIT_OFFSET) |
-		((u32)param->f.Ecw.f.ECWmax << AC_PARAM_ECW_MAX_OFFSET) |
-		((u32)param->f.Ecw.f.ECWmin << AC_PARAM_ECW_MIN_OFFSET) |
-		((u32)u1bAIFS << AC_PARAM_AIFS_OFFSET));
-
-	switch (eACI) {
-	case AC1_BK:
-		write_nic_dword(dev, AC_BK_PARAM, u4bAcParam);
-		return;
-	case AC0_BE:
-		write_nic_dword(dev, AC_BE_PARAM, u4bAcParam);
-		return;
-	case AC2_VI:
-		write_nic_dword(dev, AC_VI_PARAM, u4bAcParam);
-		return;
-	case AC3_VO:
-		write_nic_dword(dev, AC_VO_PARAM, u4bAcParam);
-		return;
-	default:
-		pr_warn("SetHwReg8185(): invalid ACI: %d!\n", eACI);
-		return;
-	}
-}
-
-static void rtl8180_wmm_param_update(struct work_struct *work)
-{
-	struct ieee80211_device *ieee = container_of(work,
-		struct ieee80211_device, wmm_param_update_wq);
-	struct net_device *dev = ieee->dev;
-	u8 *ac_param = (u8 *)(ieee->current_network.wmm_param);
-	u8 mode = ieee->current_network.mode;
-	AC_CODING eACI;
-	AC_PARAM AcParam;
-
-	if (!ieee->current_network.QoS_Enable) {
-		/* legacy ac_xx_param update */
-		AcParam.longData = 0;
-		AcParam.f.AciAifsn.f.AIFSN = 2; /* Follow 802.11 DIFS. */
-		AcParam.f.AciAifsn.f.ACM = 0;
-		AcParam.f.Ecw.f.ECWmin = 3; /* Follow 802.11 CWmin. */
-		AcParam.f.Ecw.f.ECWmax = 7; /* Follow 802.11 CWmax. */
-		AcParam.f.TXOPLimit = 0;
-
-		for (eACI = 0; eACI < AC_MAX; eACI++) {
-			AcParam.f.AciAifsn.f.ACI = (u8)eACI;
-
-			rtl8180_wmm_single_param_update(dev, mode, eACI,
-				(PAC_PARAM)&AcParam);
-		}
-		return;
-	}
-
-	for (eACI = 0; eACI < AC_MAX; eACI++) {
-		rtl8180_wmm_single_param_update(dev, mode,
-			((PAC_PARAM)ac_param)->f.AciAifsn.f.ACI,
-			(PAC_PARAM)ac_param);
-
-		ac_param += sizeof(AC_PARAM);
-	}
-}
-
-void rtl8180_restart_wq(struct work_struct *work);
-void rtl8180_watch_dog_wq(struct work_struct *work);
-void rtl8180_hw_wakeup_wq(struct work_struct *work);
-void rtl8180_hw_sleep_wq(struct work_struct *work);
-void rtl8180_sw_antenna_wq(struct work_struct *work);
-void rtl8180_watch_dog(struct net_device *dev);
-
-static void watch_dog_adaptive(unsigned long data)
-{
-	struct r8180_priv *priv = ieee80211_priv((struct net_device *)data);
-
-	if (!priv->up) {
-		DMESG("<----watch_dog_adaptive():driver is not up!\n");
-		return;
-	}
-
-	/* Tx High Power Mechanism. */
-	if (CheckHighPower((struct net_device *)data))
-		queue_work(priv->ieee80211->wq,
-			(void *)&priv->ieee80211->tx_pw_wq);
-
-	/* Tx Power Tracking on 87SE. */
-	if (CheckTxPwrTracking((struct net_device *)data))
-		TxPwrTracking87SE((struct net_device *)data);
-
-	/* Perform DIG immediately. */
-	if (CheckDig((struct net_device *)data))
-		queue_work(priv->ieee80211->wq,
-			(void *)&priv->ieee80211->hw_dig_wq);
-
-	rtl8180_watch_dog((struct net_device *)data);
-
-	queue_work(priv->ieee80211->wq,
-		(void *)&priv->ieee80211->GPIOChangeRFWorkItem);
-
-	priv->watch_dog_timer.expires = jiffies +
-		MSECS(IEEE80211_WATCH_DOG_TIME);
-
-	add_timer(&priv->watch_dog_timer);
-}
-
-static struct rtl8187se_channel_list channel_plan_list[] = {
-	/* FCC */
-	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 36, 40,
-		44, 48, 52, 56, 60, 64}, 19},
-
-	/* IC */
-	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11}, 11},
-
-	/* ETSI */
-	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 36, 40,
-		44, 48, 52, 56, 60, 64}, 21},
-
-	/* Spain. Change to ETSI. */
-	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 36, 40,
-		44, 48, 52, 56, 60, 64}, 21},
-
-	/* France. Change to ETSI. */
-	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 36, 40,
-		44, 48, 52, 56, 60, 64}, 21},
-
-	/* MKK */
-	{{14, 36, 40, 44, 48, 52, 56, 60, 64}, 9},
-
-	/* MKK1 */
-	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 36,
-		40, 44, 48, 52, 56, 60, 64}, 22},
-
-	/* Israel. */
-	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 36, 40,
-		44, 48, 52, 56, 60, 64}, 21},
-
-	/* For 11a , TELEC */
-	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 34, 38, 42, 46}, 17},
-
-	/* For Global Domain. 1-11 active, 12-14 passive. */
-	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14}, 14},
-
-	/* world wide 13: ch1~ch11 active, ch12~13 passive */
-	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}, 13}
-};
-
-static void rtl8180_set_channel_map(u8 channel_plan,
-				    struct ieee80211_device *ieee)
-{
-	int i;
-
-	ieee->MinPassiveChnlNum = MAX_CHANNEL_NUMBER+1;
-	ieee->IbssStartChnl = 0;
-
-	switch (channel_plan) {
-	case COUNTRY_CODE_FCC:
-	case COUNTRY_CODE_IC:
-	case COUNTRY_CODE_ETSI:
-	case COUNTRY_CODE_SPAIN:
-	case COUNTRY_CODE_FRANCE:
-	case COUNTRY_CODE_MKK:
-	case COUNTRY_CODE_MKK1:
-	case COUNTRY_CODE_ISRAEL:
-	case COUNTRY_CODE_TELEC:
-		{
-			Dot11d_Init(ieee);
-			ieee->bGlobalDomain = false;
-			if (channel_plan_list[channel_plan].len != 0) {
-				/* Clear old channel map */
-				memset(GET_DOT11D_INFO(ieee)->channel_map, 0, sizeof(GET_DOT11D_INFO(ieee)->channel_map));
-				/* Set new channel map */
-				for (i = 0; i < channel_plan_list[channel_plan].len; i++) {
-					if (channel_plan_list[channel_plan].channel[i] <= 14)
-						GET_DOT11D_INFO(ieee)->channel_map[channel_plan_list[channel_plan].channel[i]] = 1;
-				}
-			}
-			break;
-		}
-	case COUNTRY_CODE_GLOBAL_DOMAIN:
-		{
-			GET_DOT11D_INFO(ieee)->bEnabled = false;
-			Dot11d_Reset(ieee);
-			ieee->bGlobalDomain = true;
-			break;
-		}
-	case COUNTRY_CODE_WORLD_WIDE_13_INDEX:
-		{
-			ieee->MinPassiveChnlNum = 12;
-			ieee->IbssStartChnl = 10;
-			break;
-		}
-	default:
-		{
-			Dot11d_Init(ieee);
-			ieee->bGlobalDomain = false;
-			memset(GET_DOT11D_INFO(ieee)->channel_map, 0, sizeof(GET_DOT11D_INFO(ieee)->channel_map));
-			for (i = 1; i <= 14; i++)
-				GET_DOT11D_INFO(ieee)->channel_map[i] = 1;
-			break;
-		}
-	}
-}
-
-void GPIOChangeRFWorkItemCallBack(struct work_struct *work);
-
-static void rtl8180_statistics_init(struct stats *pstats)
-{
-	memset(pstats, 0, sizeof(struct stats));
-}
-
-static void rtl8180_link_detect_init(struct link_detect_t *plink_detect)
-{
-	memset(plink_detect, 0, sizeof(struct link_detect_t));
-	plink_detect->slot_num = DEFAULT_SLOT_NUM;
-}
-
-static void rtl8187se_eeprom_register_read(struct eeprom_93cx6 *eeprom)
-{
-	struct net_device *dev = eeprom->data;
-	u8 reg = read_nic_byte(dev, EPROM_CMD);
-
-	eeprom->reg_data_in = reg & RTL818X_EEPROM_CMD_WRITE;
-	eeprom->reg_data_out = reg & RTL818X_EEPROM_CMD_READ;
-	eeprom->reg_data_clock = reg & RTL818X_EEPROM_CMD_CK;
-	eeprom->reg_chip_select = reg & RTL818X_EEPROM_CMD_CS;
-}
-
-static void rtl8187se_eeprom_register_write(struct eeprom_93cx6 *eeprom)
-{
-	struct net_device *dev = eeprom->data;
-	u8 reg = 2 << 6;
-
-	if (eeprom->reg_data_in)
-		reg |= RTL818X_EEPROM_CMD_WRITE;
-	if (eeprom->reg_data_out)
-		reg |= RTL818X_EEPROM_CMD_READ;
-	if (eeprom->reg_data_clock)
-		reg |= RTL818X_EEPROM_CMD_CK;
-	if (eeprom->reg_chip_select)
-		reg |= RTL818X_EEPROM_CMD_CS;
-
-	write_nic_byte(dev, EPROM_CMD, reg);
-	read_nic_byte(dev, EPROM_CMD);
-	udelay(10);
-}
-
-static short rtl8180_init(struct net_device *dev)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	u16 word;
-	u16 usValue;
-	u16 tmpu16;
-	int i, j;
-	struct eeprom_93cx6 eeprom;
-	u16 eeprom_val;
-
-	eeprom.data = dev;
-	eeprom.register_read = rtl8187se_eeprom_register_read;
-	eeprom.register_write = rtl8187se_eeprom_register_write;
-	eeprom.width = PCI_EEPROM_WIDTH_93C46;
-
-	eeprom_93cx6_read(&eeprom, EEPROM_COUNTRY_CODE>>1, &eeprom_val);
-	priv->channel_plan = eeprom_val & 0xFF;
-	if (priv->channel_plan > COUNTRY_CODE_GLOBAL_DOMAIN) {
-		netdev_err(dev, "rtl8180_init: Invalid channel plan! Set to default.\n");
-		priv->channel_plan = 0;
-	}
-
-	DMESG("Channel plan is %d\n", priv->channel_plan);
-	rtl8180_set_channel_map(priv->channel_plan, priv->ieee80211);
-
-	/* FIXME: these constants are placed in a bad pleace. */
-	priv->txbuffsize = 2048;	/* 1024; */
-	priv->txringcount = 32;		/* 32; */
-	priv->rxbuffersize = 2048;	/* 1024; */
-	priv->rxringcount = 64;		/* 32; */
-	priv->txbeaconcount = 2;
-	priv->rx_skb_complete = 1;
-
-	priv->RFChangeInProgress = false;
-	priv->SetRFPowerStateInProgress = false;
-	priv->RFProgType = 0;
-
-	priv->irq_enabled = 0;
-
-	rtl8180_statistics_init(&priv->stats);
-	rtl8180_link_detect_init(&priv->link_detect);
-
-	priv->ack_tx_to_ieee = 0;
-	priv->ieee80211->current_network.beacon_interval =
-		DEFAULT_BEACONINTERVAL;
-	priv->ieee80211->iw_mode = IW_MODE_INFRA;
-	priv->ieee80211->softmac_features  = IEEE_SOFTMAC_SCAN |
-		IEEE_SOFTMAC_ASSOCIATE | IEEE_SOFTMAC_PROBERQ |
-		IEEE_SOFTMAC_PROBERS | IEEE_SOFTMAC_TX_QUEUE;
-	priv->ieee80211->active_scan = 1;
-	priv->ieee80211->rate = 110; /* 11 mbps */
-	priv->ieee80211->modulation = IEEE80211_CCK_MODULATION;
-	priv->ieee80211->host_encrypt = 1;
-	priv->ieee80211->host_decrypt = 1;
-	priv->ieee80211->sta_wake_up = rtl8180_hw_wakeup;
-	priv->ieee80211->ps_request_tx_ack = rtl8180_rq_tx_ack;
-	priv->ieee80211->enter_sleep_state = rtl8180_hw_sleep;
-	priv->ieee80211->ps_is_queue_empty = rtl8180_is_tx_queue_empty;
-
-	priv->hw_wep = hwwep;
-	priv->dev = dev;
-	priv->retry_rts = DEFAULT_RETRY_RTS;
-	priv->retry_data = DEFAULT_RETRY_DATA;
-	priv->RFChangeInProgress = false;
-	priv->SetRFPowerStateInProgress = false;
-	priv->RFProgType = 0;
-	priv->bInactivePs = true; /* false; */
-	priv->ieee80211->bInactivePs = priv->bInactivePs;
-	priv->bSwRfProcessing = false;
-	priv->eRFPowerState = RF_OFF;
-	priv->RfOffReason = 0;
-	priv->led_strategy = SW_LED_MODE0;
-	priv->TxPollingTimes = 0;
-	priv->bLeisurePs = true;
-	priv->dot11PowerSaveMode = ACTIVE;
-	priv->AdMinCheckPeriod = 5;
-	priv->AdMaxCheckPeriod = 10;
-	priv->AdMaxRxSsThreshold = 30;	/* 60->30 */
-	priv->AdRxSsThreshold = 20;	/* 50->20 */
-	priv->AdCheckPeriod = priv->AdMinCheckPeriod;
-	priv->AdTickCount = 0;
-	priv->AdRxSignalStrength = -1;
-	priv->RegSwAntennaDiversityMechanism = 0;
-	priv->RegDefaultAntenna = 0;
-	priv->SignalStrength = 0;
-	priv->AdRxOkCnt = 0;
-	priv->CurrAntennaIndex = 0;
-	priv->AdRxSsBeforeSwitched = 0;
-	init_timer(&priv->SwAntennaDiversityTimer);
-	priv->SwAntennaDiversityTimer.data = (unsigned long)dev;
-	priv->SwAntennaDiversityTimer.function =
-		(void *)SwAntennaDiversityTimerCallback;
-	priv->bDigMechanism = true;
-	priv->InitialGain = 6;
-	priv->bXtalCalibration = false;
-	priv->XtalCal_Xin = 0;
-	priv->XtalCal_Xout = 0;
-	priv->bTxPowerTrack = false;
-	priv->ThermalMeter = 0;
-	priv->FalseAlarmRegValue = 0;
-	priv->RegDigOfdmFaUpTh = 0xc; /* Upper threshold of OFDM false alarm,
-					which is used in DIG. */
-	priv->DIG_NumberFallbackVote = 0;
-	priv->DIG_NumberUpgradeVote = 0;
-	priv->LastSignalStrengthInPercent = 0;
-	priv->Stats_SignalStrength = 0;
-	priv->LastRxPktAntenna = 0;
-	priv->SignalQuality = 0; /* in 0-100 index. */
-	priv->Stats_SignalQuality = 0;
-	priv->RecvSignalPower = 0; /* in dBm. */
-	priv->Stats_RecvSignalPower = 0;
-	priv->AdMainAntennaRxOkCnt = 0;
-	priv->AdAuxAntennaRxOkCnt = 0;
-	priv->bHWAdSwitched = false;
-	priv->bRegHighPowerMechanism = true;
-	priv->RegHiPwrUpperTh = 77;
-	priv->RegHiPwrLowerTh = 75;
-	priv->RegRSSIHiPwrUpperTh = 70;
-	priv->RegRSSIHiPwrLowerTh = 20;
-	priv->bCurCCKPkt = false;
-	priv->UndecoratedSmoothedSS = -1;
-	priv->bToUpdateTxPwr = false;
-	priv->CurCCKRSSI = 0;
-	priv->RxPower = 0;
-	priv->RSSI = 0;
-	priv->NumTxOkTotal = 0;
-	priv->NumTxUnicast = 0;
-	priv->keepAliveLevel = DEFAULT_KEEP_ALIVE_LEVEL;
-	priv->CurrRetryCnt = 0;
-	priv->LastRetryCnt = 0;
-	priv->LastTxokCnt = 0;
-	priv->LastRxokCnt = 0;
-	priv->LastRetryRate = 0;
-	priv->bTryuping = 0;
-	priv->CurrTxRate = 0;
-	priv->CurrRetryRate = 0;
-	priv->TryupingCount = 0;
-	priv->TryupingCountNoData = 0;
-	priv->TryDownCountLowData = 0;
-	priv->LastTxOKBytes = 0;
-	priv->LastFailTxRate = 0;
-	priv->LastFailTxRateSS = 0;
-	priv->FailTxRateCount = 0;
-	priv->LastTxThroughput = 0;
-	priv->NumTxOkBytesTotal = 0;
-	priv->ForcedDataRate = 0;
-	priv->RegBModeGainStage = 1;
-
-	priv->promisc = (dev->flags & IFF_PROMISC) ? 1 : 0;
-	spin_lock_init(&priv->irq_th_lock);
-	spin_lock_init(&priv->tx_lock);
-	spin_lock_init(&priv->ps_lock);
-	spin_lock_init(&priv->rf_ps_lock);
-	sema_init(&priv->wx_sem, 1);
-	INIT_WORK(&priv->reset_wq, (void *)rtl8180_restart_wq);
-	INIT_DELAYED_WORK(&priv->ieee80211->hw_wakeup_wq,
-			  (void *)rtl8180_hw_wakeup_wq);
-	INIT_DELAYED_WORK(&priv->ieee80211->hw_sleep_wq,
-			  (void *)rtl8180_hw_sleep_wq);
-	INIT_WORK(&priv->ieee80211->wmm_param_update_wq,
-		  (void *)rtl8180_wmm_param_update);
-	INIT_DELAYED_WORK(&priv->ieee80211->rate_adapter_wq,
-			  (void *)rtl8180_rate_adapter);
-	INIT_DELAYED_WORK(&priv->ieee80211->hw_dig_wq,
-			 (void *)rtl8180_hw_dig_wq);
-	INIT_DELAYED_WORK(&priv->ieee80211->tx_pw_wq,
-			 (void *)rtl8180_tx_pw_wq);
-	INIT_DELAYED_WORK(&priv->ieee80211->GPIOChangeRFWorkItem,
-			 (void *) GPIOChangeRFWorkItemCallBack);
-	tasklet_init(&priv->irq_rx_tasklet,
-		     (void(*)(unsigned long)) rtl8180_irq_rx_tasklet,
-		     (unsigned long)priv);
-
-	init_timer(&priv->watch_dog_timer);
-	priv->watch_dog_timer.data = (unsigned long)dev;
-	priv->watch_dog_timer.function = watch_dog_adaptive;
-
-	init_timer(&priv->rateadapter_timer);
-	priv->rateadapter_timer.data = (unsigned long)dev;
-	priv->rateadapter_timer.function = timer_rate_adaptive;
-	priv->RateAdaptivePeriod = RATE_ADAPTIVE_TIMER_PERIOD;
-	priv->bEnhanceTxPwr = false;
-
-	priv->ieee80211->softmac_hard_start_xmit = rtl8180_hard_start_xmit;
-	priv->ieee80211->set_chan = rtl8180_set_chan;
-	priv->ieee80211->link_change = rtl8180_link_change;
-	priv->ieee80211->softmac_data_hard_start_xmit = rtl8180_hard_data_xmit;
-	priv->ieee80211->data_hard_stop = rtl8180_data_hard_stop;
-	priv->ieee80211->data_hard_resume = rtl8180_data_hard_resume;
-
-	priv->ieee80211->init_wmmparam_flag = 0;
-
-	priv->ieee80211->start_send_beacons = rtl8180_start_tx_beacon;
-	priv->ieee80211->stop_send_beacons = rtl8180_beacon_tx_disable;
-	priv->ieee80211->fts = DEFAULT_FRAG_THRESHOLD;
-
-	priv->ShortRetryLimit = 7;
-	priv->LongRetryLimit = 7;
-	priv->EarlyRxThreshold = 7;
-
-	priv->TransmitConfig =	(1<<TCR_DurProcMode_OFFSET) |
-				(7<<TCR_MXDMA_OFFSET) |
-				(priv->ShortRetryLimit<<TCR_SRL_OFFSET) |
-				(priv->LongRetryLimit<<TCR_LRL_OFFSET);
-
-	priv->ReceiveConfig =	RCR_AMF | RCR_ADF | RCR_ACF |
-				RCR_AB | RCR_AM | RCR_APM |
-				(7<<RCR_MXDMA_OFFSET) |
-				(priv->EarlyRxThreshold<<RCR_FIFO_OFFSET) |
-				(priv->EarlyRxThreshold == 7 ?
-					 RCR_ONLYERLPKT : 0);
-
-	priv->IntrMask		= IMR_TMGDOK | IMR_TBDER |
-				  IMR_THPDER | IMR_THPDOK |
-				  IMR_TVODER | IMR_TVODOK |
-				  IMR_TVIDER | IMR_TVIDOK |
-				  IMR_TBEDER | IMR_TBEDOK |
-				  IMR_TBKDER | IMR_TBKDOK |
-				  IMR_RDU |
-				  IMR_RER | IMR_ROK |
-				  IMR_RQoSOK;
-
-	priv->InitialGain = 6;
-
-	DMESG("MAC controller is a RTL8187SE b/g");
-
-	priv->ieee80211->modulation |= IEEE80211_OFDM_MODULATION;
-	priv->ieee80211->short_slot = 1;
-
-	eeprom_93cx6_read(&eeprom, EEPROM_SW_REVD_OFFSET, &usValue);
-	DMESG("usValue is %#hx\n", usValue);
-	/* 3Read AntennaDiversity */
-
-	/* SW Antenna Diversity. */
-	priv->EEPROMSwAntennaDiversity = (usValue & EEPROM_SW_AD_MASK) ==
-		EEPROM_SW_AD_ENABLE;
-
-	/* Default Antenna to use. */
-	priv->EEPROMDefaultAntenna1 = (usValue & EEPROM_DEF_ANT_MASK) ==
-		EEPROM_DEF_ANT_1;
-
-	if (priv->RegSwAntennaDiversityMechanism == 0) /* Auto */
-		/* 0: default from EEPROM. */
-		priv->bSwAntennaDiverity = priv->EEPROMSwAntennaDiversity;
-	else
-		/* 1:disable antenna diversity, 2: enable antenna diversity. */
-		priv->bSwAntennaDiverity =
-			priv->RegSwAntennaDiversityMechanism == 2;
-
-	if (priv->RegDefaultAntenna == 0)
-		/* 0: default from EEPROM. */
-		priv->bDefaultAntenna1 = priv->EEPROMDefaultAntenna1;
-	else
-		/* 1: main, 2: aux. */
-		priv->bDefaultAntenna1 = priv->RegDefaultAntenna == 2;
-
-	priv->plcp_preamble_mode = 2;
-	/* the eeprom type is stored in RCR register bit #6 */
-	if (RCR_9356SEL & read_nic_dword(dev, RCR))
-		priv->epromtype = EPROM_93c56;
-	else
-		priv->epromtype = EPROM_93c46;
-
-	eeprom_93cx6_multiread(&eeprom, 0x7, (__le16 *)
-			       dev->dev_addr, 3);
-
-	for (i = 1, j = 0; i < 14; i += 2, j++) {
-		eeprom_93cx6_read(&eeprom, EPROM_TXPW_CH1_2 + j, &word);
-		priv->chtxpwr[i] = word & 0xff;
-		priv->chtxpwr[i+1] = (word & 0xff00)>>8;
-	}
-	for (i = 1, j = 0; i < 14; i += 2, j++) {
-		eeprom_93cx6_read(&eeprom, EPROM_TXPW_OFDM_CH1_2 + j, &word);
-		priv->chtxpwr_ofdm[i] = word & 0xff;
-		priv->chtxpwr_ofdm[i+1] = (word & 0xff00) >> 8;
-	}
-
-	/* 3Read crystal calibration and thermal meter indication on 87SE. */
-	eeprom_93cx6_read(&eeprom, EEPROM_RSV>>1, &tmpu16);
-
-	/* Crystal calibration for Xin and Xout resp. */
-	priv->XtalCal_Xout = tmpu16 & EEPROM_XTAL_CAL_XOUT_MASK;
-	priv->XtalCal_Xin = (tmpu16 & EEPROM_XTAL_CAL_XIN_MASK) >> 4;
-	if ((tmpu16 & EEPROM_XTAL_CAL_ENABLE) >> 12)
-		priv->bXtalCalibration = true;
-
-	/* Thermal meter reference indication. */
-	priv->ThermalMeter =  (u8)((tmpu16 & EEPROM_THERMAL_METER_MASK) >> 8);
-	if ((tmpu16 & EEPROM_THERMAL_METER_ENABLE) >> 13)
-		priv->bTxPowerTrack = true;
-
-	priv->rf_sleep = rtl8225z4_rf_sleep;
-	priv->rf_wakeup = rtl8225z4_rf_wakeup;
-	DMESGW("**PLEASE** REPORT SUCCESSFUL/UNSUCCESSFUL TO Realtek!");
-
-	priv->rf_close = rtl8225z2_rf_close;
-	priv->rf_init = rtl8225z2_rf_init;
-	priv->rf_set_chan = rtl8225z2_rf_set_chan;
-	priv->rf_set_sens = NULL;
-
-	if (0 != alloc_rx_desc_ring(dev, priv->rxbuffersize, priv->rxringcount))
-		return -ENOMEM;
-
-	if (0 != alloc_tx_desc_ring(dev, priv->txbuffsize, priv->txringcount,
-				  TX_MANAGEPRIORITY_RING_ADDR))
-		return -ENOMEM;
-
-	if (0 != alloc_tx_desc_ring(dev, priv->txbuffsize, priv->txringcount,
-				 TX_BKPRIORITY_RING_ADDR))
-		return -ENOMEM;
-
-	if (0 != alloc_tx_desc_ring(dev, priv->txbuffsize, priv->txringcount,
-				 TX_BEPRIORITY_RING_ADDR))
-		return -ENOMEM;
-
-	if (0 != alloc_tx_desc_ring(dev, priv->txbuffsize, priv->txringcount,
-				  TX_VIPRIORITY_RING_ADDR))
-		return -ENOMEM;
-
-	if (0 != alloc_tx_desc_ring(dev, priv->txbuffsize, priv->txringcount,
-				  TX_VOPRIORITY_RING_ADDR))
-		return -ENOMEM;
-
-	if (0 != alloc_tx_desc_ring(dev, priv->txbuffsize, priv->txringcount,
-				  TX_HIGHPRIORITY_RING_ADDR))
-		return -ENOMEM;
-
-	if (0 != alloc_tx_desc_ring(dev, priv->txbuffsize, priv->txbeaconcount,
-				  TX_BEACON_RING_ADDR))
-		return -ENOMEM;
-
-	if (request_irq(dev->irq, rtl8180_interrupt,
-		IRQF_SHARED, dev->name, dev)) {
-		DMESGE("Error allocating IRQ %d", dev->irq);
-		return -1;
-	} else {
-		priv->irq = dev->irq;
-		DMESG("IRQ %d", dev->irq);
-	}
-
-	return 0;
-}
-
-void rtl8180_no_hw_wep(struct net_device *dev)
-{
-}
-
-void rtl8180_set_hw_wep(struct net_device *dev)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	u8 pgreg;
-	u8 security;
-	u32 key0_word4;
-
-	pgreg = read_nic_byte(dev, PGSELECT);
-	write_nic_byte(dev, PGSELECT, pgreg & ~(1<<PGSELECT_PG_SHIFT));
-
-	key0_word4 = read_nic_dword(dev, KEY0+4+4+4);
-	key0_word4 &= ~0xff;
-	key0_word4 |= priv->key0[3] & 0xff;
-	write_nic_dword(dev, KEY0, (priv->key0[0]));
-	write_nic_dword(dev, KEY0+4, (priv->key0[1]));
-	write_nic_dword(dev, KEY0+4+4, (priv->key0[2]));
-	write_nic_dword(dev, KEY0+4+4+4, (key0_word4));
-
-	security  = read_nic_byte(dev, SECURITY);
-	security |= (1<<SECURITY_WEP_TX_ENABLE_SHIFT);
-	security |= (1<<SECURITY_WEP_RX_ENABLE_SHIFT);
-	security &= ~SECURITY_ENCRYP_MASK;
-	security |= (SECURITY_ENCRYP_104<<SECURITY_ENCRYP_SHIFT);
-
-	write_nic_byte(dev, SECURITY, security);
-
-	DMESG("key %x %x %x %x", read_nic_dword(dev, KEY0+4+4+4),
-	      read_nic_dword(dev, KEY0+4+4), read_nic_dword(dev, KEY0+4),
-	      read_nic_dword(dev, KEY0));
-}
-
-
-void rtl8185_rf_pins_enable(struct net_device *dev)
-{
-	write_nic_word(dev, RFPinsEnable, 0x1fff); /* | tmp); */
-}
-
-void rtl8185_set_anaparam2(struct net_device *dev, u32 a)
-{
-	u8 conf3;
-
-	rtl8180_set_mode(dev, EPROM_CMD_CONFIG);
-
-	conf3 = read_nic_byte(dev, CONFIG3);
-	write_nic_byte(dev, CONFIG3, conf3 | (1<<CONFIG3_ANAPARAM_W_SHIFT));
-	write_nic_dword(dev, ANAPARAM2, a);
-
-	conf3 = read_nic_byte(dev, CONFIG3);
-	write_nic_byte(dev, CONFIG3, conf3 & ~(1<<CONFIG3_ANAPARAM_W_SHIFT));
-	rtl8180_set_mode(dev, EPROM_CMD_NORMAL);
-}
-
-void rtl8180_set_anaparam(struct net_device *dev, u32 a)
-{
-	u8 conf3;
-
-	rtl8180_set_mode(dev, EPROM_CMD_CONFIG);
-
-	conf3 = read_nic_byte(dev, CONFIG3);
-	write_nic_byte(dev, CONFIG3, conf3 | (1<<CONFIG3_ANAPARAM_W_SHIFT));
-	write_nic_dword(dev, ANAPARAM, a);
-
-	conf3 = read_nic_byte(dev, CONFIG3);
-	write_nic_byte(dev, CONFIG3, conf3 & ~(1<<CONFIG3_ANAPARAM_W_SHIFT));
-	rtl8180_set_mode(dev, EPROM_CMD_NORMAL);
-}
-
-void rtl8185_tx_antenna(struct net_device *dev, u8 ant)
-{
-	write_nic_byte(dev, TX_ANTENNA, ant);
-	force_pci_posting(dev);
-	mdelay(1);
-}
-
-static void rtl8185_write_phy(struct net_device *dev, u8 adr, u32 data)
-{
-	u32 phyw;
-
-	adr |= 0x80;
-
-	phyw = ((data<<8) | adr);
-
-	/* Note: we must write 0xff7c after 0x7d-0x7f to write BB register. */
-	write_nic_byte(dev, 0x7f, ((phyw & 0xff000000) >> 24));
-	write_nic_byte(dev, 0x7e, ((phyw & 0x00ff0000) >> 16));
-	write_nic_byte(dev, 0x7d, ((phyw & 0x0000ff00) >> 8));
-	write_nic_byte(dev, 0x7c, ((phyw & 0x000000ff)));
-}
-
-inline void write_phy_ofdm(struct net_device *dev, u8 adr, u32 data)
-{
-	data = data & 0xff;
-	rtl8185_write_phy(dev, adr, data);
-}
-
-void write_phy_cck(struct net_device *dev, u8 adr, u32 data)
-{
-	data = data & 0xff;
-	rtl8185_write_phy(dev, adr, data | 0x10000);
-}
-
-/*
- * This configures registers for beacon tx and enables it via
- * rtl8180_beacon_tx_enable(). rtl8180_beacon_tx_disable() might
- * be used to stop beacon transmission
- */
-void rtl8180_start_tx_beacon(struct net_device *dev)
-{
-	u16 word;
-
-	DMESG("Enabling beacon TX");
-	rtl8180_prepare_beacon(dev);
-	rtl8180_irq_disable(dev);
-	rtl8180_beacon_tx_enable(dev);
-
-	word = read_nic_word(dev, AtimWnd) & ~AtimWnd_AtimWnd;
-	write_nic_word(dev, AtimWnd, word); /* word |= */
-
-	word  = read_nic_word(dev, BintrItv);
-	word &= ~BintrItv_BintrItv;
-	word |= 1000; /* priv->ieee80211->current_network.beacon_interval *
-		       * ((priv->txbeaconcount > 1)?(priv->txbeaconcount-1):1);
-		       * FIXME: check if correct ^^ worked with 0x3e8;
-		       */
-	write_nic_word(dev, BintrItv, word);
-
-	rtl8180_set_mode(dev, EPROM_CMD_NORMAL);
-
-	rtl8185b_irq_enable(dev);
-}
-
-static struct net_device_stats *rtl8180_stats(struct net_device *dev)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-	return &priv->ieee80211->stats;
-}
-
-/*
- * Change current and default preamble mode.
- */
-static bool MgntActSet_802_11_PowerSaveMode(struct r8180_priv *priv,
-				     enum rt_ps_mode rtPsMode)
-{
-	/* Currently, we do not change power save mode on IBSS mode. */
-	if (priv->ieee80211->iw_mode == IW_MODE_ADHOC)
-		return false;
-
-	priv->ieee80211->ps = rtPsMode;
-
-	return true;
-}
-
-static void LeisurePSEnter(struct r8180_priv *priv)
-{
-	if (priv->bLeisurePs)
-		if (priv->ieee80211->ps == IEEE80211_PS_DISABLED)
-			/* IEEE80211_PS_ENABLE */
-			MgntActSet_802_11_PowerSaveMode(priv,
-				IEEE80211_PS_MBCAST | IEEE80211_PS_UNICAST);
-}
-
-static void LeisurePSLeave(struct r8180_priv *priv)
-{
-	if (priv->bLeisurePs)
-		if (priv->ieee80211->ps != IEEE80211_PS_DISABLED)
-			MgntActSet_802_11_PowerSaveMode(
-				priv, IEEE80211_PS_DISABLED);
-}
-
-void rtl8180_hw_wakeup_wq(struct work_struct *work)
-{
-	struct delayed_work *dwork = to_delayed_work(work);
-	struct ieee80211_device *ieee = container_of(
-		dwork, struct ieee80211_device, hw_wakeup_wq);
-	struct net_device *dev = ieee->dev;
-
-	rtl8180_hw_wakeup(dev);
-}
-
-void rtl8180_hw_sleep_wq(struct work_struct *work)
-{
-	struct delayed_work *dwork = to_delayed_work(work);
-	struct ieee80211_device *ieee = container_of(
-		dwork, struct ieee80211_device, hw_sleep_wq);
-	struct net_device *dev = ieee->dev;
-
-	rtl8180_hw_sleep_down(dev);
-}
-
-static void MgntLinkKeepAlive(struct r8180_priv *priv)
-{
-	if (priv->keepAliveLevel == 0)
-		return;
-
-	if (priv->ieee80211->state == IEEE80211_LINKED) {
-		/*
-		 * Keep-Alive.
-		 */
-
-		if ((priv->keepAliveLevel == 2) ||
-			(priv->link_detect.last_num_tx_unicast ==
-				priv->NumTxUnicast &&
-			priv->link_detect.last_num_rx_unicast ==
-				priv->ieee80211->NumRxUnicast)
-			) {
-			priv->link_detect.idle_count++;
-
-			/*
-			 * Send a Keep-Alive packet packet to AP if we had
-			 * been idle for a while.
-			 */
-			if (priv->link_detect.idle_count >=
-				KEEP_ALIVE_INTERVAL /
-				CHECK_FOR_HANG_PERIOD - 1) {
-				priv->link_detect.idle_count = 0;
-				ieee80211_sta_ps_send_null_frame(
-					priv->ieee80211, false);
-			}
-		} else {
-			priv->link_detect.idle_count = 0;
-		}
-		priv->link_detect.last_num_tx_unicast = priv->NumTxUnicast;
-		priv->link_detect.last_num_rx_unicast =
-			priv->ieee80211->NumRxUnicast;
-	}
-}
-
-void rtl8180_watch_dog(struct net_device *dev)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	bool bEnterPS = false;
-	bool bBusyTraffic = false;
-	u32 TotalRxNum = 0;
-	u16 SlotIndex = 0;
-	u16 i = 0;
-	if (priv->ieee80211->actscanning == false) {
-		if ((priv->ieee80211->iw_mode != IW_MODE_ADHOC) &&
-		    (priv->ieee80211->state == IEEE80211_NOLINK) &&
-		    (priv->ieee80211->beinretry == false) &&
-		    (priv->eRFPowerState == RF_ON))
-			IPSEnter(dev);
-	}
-	if ((priv->ieee80211->state == IEEE80211_LINKED) &&
-		(priv->ieee80211->iw_mode == IW_MODE_INFRA)) {
-		SlotIndex = (priv->link_detect.slot_index++) %
-			priv->link_detect.slot_num;
-
-		priv->link_detect.rx_frame_num[SlotIndex] =
-			priv->ieee80211->NumRxDataInPeriod +
-			priv->ieee80211->NumRxBcnInPeriod;
-
-		for (i = 0; i < priv->link_detect.slot_num; i++)
-			TotalRxNum += priv->link_detect.rx_frame_num[i];
-
-		if (TotalRxNum == 0) {
-			priv->ieee80211->state = IEEE80211_ASSOCIATING;
-			queue_work(priv->ieee80211->wq,
-				&priv->ieee80211->associate_procedure_wq);
-		}
-	}
-
-	MgntLinkKeepAlive(priv);
-
-	LeisurePSLeave(priv);
-
-	if (priv->ieee80211->state == IEEE80211_LINKED) {
-		priv->link_detect.num_rx_ok_in_period =
-			priv->ieee80211->NumRxDataInPeriod;
-		if (priv->link_detect.num_rx_ok_in_period > 666 ||
-			priv->link_detect.num_tx_ok_in_period > 666) {
-			bBusyTraffic = true;
-		}
-		if ((priv->link_detect.num_rx_ok_in_period +
-			priv->link_detect.num_tx_ok_in_period > 8)
-			|| (priv->link_detect.num_rx_ok_in_period > 2)) {
-			bEnterPS = false;
-		} else
-			bEnterPS = true;
-
-		if (bEnterPS)
-			LeisurePSEnter(priv);
-		else
-			LeisurePSLeave(priv);
-	} else
-		LeisurePSLeave(priv);
-	priv->link_detect.b_busy_traffic = bBusyTraffic;
-	priv->link_detect.num_rx_ok_in_period = 0;
-	priv->link_detect.num_tx_ok_in_period = 0;
-	priv->ieee80211->NumRxDataInPeriod = 0;
-	priv->ieee80211->NumRxBcnInPeriod = 0;
-}
-
-static int _rtl8180_up(struct net_device *dev)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-	priv->up = 1;
-
-	DMESG("Bringing up iface");
-	rtl8185b_adapter_start(dev);
-	rtl8185b_rx_enable(dev);
-	rtl8185b_tx_enable(dev);
-	if (priv->bInactivePs) {
-		if (priv->ieee80211->iw_mode == IW_MODE_ADHOC)
-			IPSLeave(dev);
-	}
-	timer_rate_adaptive((unsigned long)dev);
-	watch_dog_adaptive((unsigned long)dev);
-	if (priv->bSwAntennaDiverity)
-			SwAntennaDiversityTimerCallback(dev);
-	ieee80211_softmac_start_protocol(priv->ieee80211);
-	return 0;
-}
-
-static int rtl8180_open(struct net_device *dev)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	int ret;
-
-	down(&priv->wx_sem);
-	ret = rtl8180_up(dev);
-	up(&priv->wx_sem);
-	return ret;
-}
-
-int rtl8180_up(struct net_device *dev)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-	if (priv->up == 1)
-		return -1;
-
-	return _rtl8180_up(dev);
-}
-
-static int rtl8180_close(struct net_device *dev)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	int ret;
-
-	down(&priv->wx_sem);
-	ret = rtl8180_down(dev);
-	up(&priv->wx_sem);
-
-	return ret;
-}
-
-int rtl8180_down(struct net_device *dev)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-	if (priv->up == 0)
-		return -1;
-
-	priv->up = 0;
-
-	ieee80211_softmac_stop_protocol(priv->ieee80211);
-	/* FIXME */
-	if (!netif_queue_stopped(dev))
-		netif_stop_queue(dev);
-	rtl8180_rtx_disable(dev);
-	rtl8180_irq_disable(dev);
-	del_timer_sync(&priv->watch_dog_timer);
-	del_timer_sync(&priv->rateadapter_timer);
-	cancel_delayed_work(&priv->ieee80211->rate_adapter_wq);
-	cancel_delayed_work(&priv->ieee80211->hw_wakeup_wq);
-	cancel_delayed_work(&priv->ieee80211->hw_sleep_wq);
-	cancel_delayed_work(&priv->ieee80211->hw_dig_wq);
-	cancel_delayed_work(&priv->ieee80211->tx_pw_wq);
-	del_timer_sync(&priv->SwAntennaDiversityTimer);
-	SetZebraRFPowerState8185(dev, RF_OFF);
-	memset(&priv->ieee80211->current_network,
-		0, sizeof(struct ieee80211_network));
-	priv->ieee80211->state = IEEE80211_NOLINK;
-	return 0;
-}
-
-void rtl8180_restart_wq(struct work_struct *work)
-{
-	struct r8180_priv *priv = container_of(
-		work, struct r8180_priv, reset_wq);
-	struct net_device *dev = priv->dev;
-
-	down(&priv->wx_sem);
-
-	rtl8180_commit(dev);
-
-	up(&priv->wx_sem);
-}
-
-static void rtl8180_restart(struct net_device *dev)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-	schedule_work(&priv->reset_wq);
-}
-
-void rtl8180_commit(struct net_device *dev)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-	if (priv->up == 0)
-		return;
-
-	del_timer_sync(&priv->watch_dog_timer);
-	del_timer_sync(&priv->rateadapter_timer);
-	cancel_delayed_work(&priv->ieee80211->rate_adapter_wq);
-	cancel_delayed_work(&priv->ieee80211->hw_wakeup_wq);
-	cancel_delayed_work(&priv->ieee80211->hw_sleep_wq);
-	cancel_delayed_work(&priv->ieee80211->hw_dig_wq);
-	cancel_delayed_work(&priv->ieee80211->tx_pw_wq);
-	del_timer_sync(&priv->SwAntennaDiversityTimer);
-	ieee80211_softmac_stop_protocol(priv->ieee80211);
-	rtl8180_irq_disable(dev);
-	rtl8180_rtx_disable(dev);
-	_rtl8180_up(dev);
-}
-
-static void r8180_set_multicast(struct net_device *dev)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	short promisc;
-
-	promisc = (dev->flags & IFF_PROMISC) ? 1 : 0;
-
-	if (promisc != priv->promisc)
-		rtl8180_restart(dev);
-
-	priv->promisc = promisc;
-}
-
-static int r8180_set_mac_adr(struct net_device *dev, void *mac)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	struct sockaddr *addr = mac;
-
-	down(&priv->wx_sem);
-
-	memcpy(dev->dev_addr, addr->sa_data, ETH_ALEN);
-
-	if (priv->ieee80211->iw_mode == IW_MODE_MASTER)
-		memcpy(priv->ieee80211->current_network.bssid,
-			dev->dev_addr, ETH_ALEN);
-
-	if (priv->up) {
-		rtl8180_down(dev);
-		rtl8180_up(dev);
-	}
-
-	up(&priv->wx_sem);
-
-	return 0;
-}
-
-/* based on ipw2200 driver */
-static int rtl8180_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	struct iwreq *wrq = (struct iwreq *) rq;
-	int ret = -1;
-
-	switch (cmd) {
-	case RTL_IOCTL_WPA_SUPPLICANT:
-		ret = ieee80211_wpa_supplicant_ioctl(
-			priv->ieee80211, &wrq->u.data);
-		return ret;
-	default:
-		return -EOPNOTSUPP;
-	}
-
-	return -EOPNOTSUPP;
-}
-
-static const struct net_device_ops rtl8180_netdev_ops = {
-	.ndo_open		= rtl8180_open,
-	.ndo_stop		= rtl8180_close,
-	.ndo_get_stats		= rtl8180_stats,
-	.ndo_tx_timeout		= rtl8180_restart,
-	.ndo_do_ioctl		= rtl8180_ioctl,
-	.ndo_set_rx_mode	= r8180_set_multicast,
-	.ndo_set_mac_address	= r8180_set_mac_adr,
-	.ndo_validate_addr	= eth_validate_addr,
-	.ndo_change_mtu		= eth_change_mtu,
-	.ndo_start_xmit		= ieee80211_rtl_xmit,
-};
-
-static int rtl8180_pci_probe(struct pci_dev *pdev,
-			     const struct pci_device_id *id)
-{
-	unsigned long ioaddr = 0;
-	struct net_device *dev = NULL;
-	struct r8180_priv *priv = NULL;
-	u8 unit = 0;
-	int ret = -ENODEV;
-
-	unsigned long pmem_start, pmem_len, pmem_flags;
-
-	DMESG("Configuring chip resources");
-
-	if (pci_enable_device(pdev)) {
-		DMESG("Failed to enable PCI device");
-		return -EIO;
-	}
-
-	pci_set_master(pdev);
-	pci_set_dma_mask(pdev, 0xffffff00ULL);
-	pci_set_consistent_dma_mask(pdev, 0xffffff00ULL);
-	dev = alloc_ieee80211(sizeof(struct r8180_priv));
-	if (!dev) {
-		ret = -ENOMEM;
-		goto fail_free;
-	}
-	priv = ieee80211_priv(dev);
-	priv->ieee80211 = netdev_priv(dev);
-
-	pci_set_drvdata(pdev, dev);
-	SET_NETDEV_DEV(dev, &pdev->dev);
-
-	priv = ieee80211_priv(dev);
-	priv->pdev = pdev;
-
-	pmem_start = pci_resource_start(pdev, 1);
-	pmem_len = pci_resource_len(pdev, 1);
-	pmem_flags = pci_resource_flags(pdev, 1);
-
-	if (!(pmem_flags & IORESOURCE_MEM)) {
-		DMESG("region #1 not a MMIO resource, aborting");
-		goto fail;
-	}
-
-	if (!request_mem_region(pmem_start, pmem_len, RTL8180_MODULE_NAME)) {
-		DMESG("request_mem_region failed!");
-		goto fail;
-	}
-
-	ioaddr = (unsigned long)ioremap_nocache(pmem_start, pmem_len);
-	if (ioaddr == (unsigned long)NULL) {
-		DMESG("ioremap failed!");
-		goto fail1;
-	}
-
-	dev->mem_start = ioaddr; /* shared mem start */
-	dev->mem_end = ioaddr + pci_resource_len(pdev, 0); /* shared mem end */
-
-	pci_read_config_byte(pdev, 0x05, &unit);
-	pci_write_config_byte(pdev, 0x05, unit & (~0x04));
-
-	dev->irq = pdev->irq;
-	priv->irq = 0;
-
-	dev->netdev_ops = &rtl8180_netdev_ops;
-	dev->wireless_handlers = &r8180_wx_handlers_def;
-
-	dev->type = ARPHRD_ETHER;
-	dev->watchdog_timeo = HZ*3;
-
-	if (dev_alloc_name(dev, ifname) < 0) {
-		DMESG("Oops: devname already taken! Trying wlan%%d...\n");
-		strcpy(ifname, "wlan%d");
-		dev_alloc_name(dev, ifname);
-	}
-
-	if (rtl8180_init(dev) != 0) {
-		DMESG("Initialization failed");
-		goto fail1;
-	}
-
-	netif_carrier_off(dev);
-
-	if (register_netdev(dev))
-		goto fail1;
-
-	rtl8180_proc_init_one(dev);
-
-	DMESG("Driver probe completed\n");
-	return 0;
-fail1:
-	if (dev->mem_start != (unsigned long)NULL) {
-		iounmap((void __iomem *)dev->mem_start);
-		release_mem_region(pci_resource_start(pdev, 1),
-				   pci_resource_len(pdev, 1));
-	}
-fail:
-	if (dev) {
-		if (priv->irq) {
-			free_irq(dev->irq, dev);
-			dev->irq = 0;
-		}
-		free_ieee80211(dev);
-	}
-
-fail_free:
-	pci_disable_device(pdev);
-
-	DMESG("wlan driver load failed\n");
-	return ret;
-}
-
-static void rtl8180_pci_remove(struct pci_dev *pdev)
-{
-	struct r8180_priv *priv;
-	struct net_device *dev = pci_get_drvdata(pdev);
-
-	if (dev) {
-		unregister_netdev(dev);
-
-		priv = ieee80211_priv(dev);
-
-		rtl8180_proc_remove_one(dev);
-		rtl8180_down(dev);
-		priv->rf_close(dev);
-		rtl8180_reset(dev);
-		mdelay(10);
-
-		if (priv->irq) {
-			DMESG("Freeing irq %d", dev->irq);
-			free_irq(dev->irq, dev);
-			priv->irq = 0;
-		}
-
-		free_rx_desc_ring(dev);
-		free_tx_desc_rings(dev);
-
-		if (dev->mem_start != (unsigned long)NULL) {
-			iounmap((void __iomem *)dev->mem_start);
-			release_mem_region(pci_resource_start(pdev, 1),
-					   pci_resource_len(pdev, 1));
-		}
-
-		free_ieee80211(dev);
-	}
-	pci_disable_device(pdev);
-
-	DMESG("wlan driver removed\n");
-}
-
-static int __init rtl8180_pci_module_init(void)
-{
-	int ret;
-
-	ret = ieee80211_crypto_init();
-	if (ret) {
-		pr_err("ieee80211_crypto_init() failed %d\n", ret);
-		return ret;
-	}
-	ret = ieee80211_crypto_tkip_init();
-	if (ret) {
-		pr_err("ieee80211_crypto_tkip_init() failed %d\n", ret);
-		return ret;
-	}
-	ret = ieee80211_crypto_ccmp_init();
-	if (ret) {
-		pr_err("ieee80211_crypto_ccmp_init() failed %d\n", ret);
-		return ret;
-	}
-	ret = ieee80211_crypto_wep_init();
-	if (ret) {
-		pr_err("ieee80211_crypto_wep_init() failed %d\n", ret);
-		return ret;
-	}
-
-	pr_info("\nLinux kernel driver for RTL8180 / RTL8185 based WLAN cards\n");
-	pr_info("Copyright (c) 2004-2005, Andrea Merello\n");
-	DMESG("Initializing module");
-	DMESG("Wireless extensions version %d", WIRELESS_EXT);
-	rtl8180_proc_module_init();
-
-	if (pci_register_driver(&rtl8180_pci_driver)) {
-		DMESG("No device found");
-		return -ENODEV;
-	}
-	return 0;
-}
-
-static void __exit rtl8180_pci_module_exit(void)
-{
-	pci_unregister_driver(&rtl8180_pci_driver);
-	rtl8180_proc_module_remove();
-	ieee80211_crypto_tkip_exit();
-	ieee80211_crypto_ccmp_exit();
-	ieee80211_crypto_wep_exit();
-	ieee80211_crypto_deinit();
-	DMESG("Exiting");
-}
-
-static void rtl8180_try_wake_queue(struct net_device *dev, int pri)
-{
-	unsigned long flags;
-	short enough_desc;
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-
-	spin_lock_irqsave(&priv->tx_lock, flags);
-	enough_desc = check_nic_enought_desc(dev, pri);
-	spin_unlock_irqrestore(&priv->tx_lock, flags);
-
-	if (enough_desc)
-		ieee80211_rtl_wake_queue(priv->ieee80211);
-}
-
-static void rtl8180_tx_isr(struct net_device *dev, int pri, short error)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	u32 *tail; /* tail virtual addr */
-	u32 *head; /* head virtual addr */
-	u32 *begin; /* start of ring virtual addr */
-	u32 *nicv; /* nic pointer virtual addr */
-	u32 nic; /* nic pointer physical addr */
-	u32 nicbegin; /* start of ring physical addr */
-	unsigned long flag;
-	/* physical addr are ok on 32 bits since we set DMA mask */
-	int offs;
-	int j, i;
-	int hd;
-	if (error)
-		priv->stats.txretry++;
-	spin_lock_irqsave(&priv->tx_lock, flag);
-	switch (pri) {
-	case MANAGE_PRIORITY:
-		tail = priv->txmapringtail;
-		begin = priv->txmapring;
-		head = priv->txmapringhead;
-		nic = read_nic_dword(dev, TX_MANAGEPRIORITY_RING_ADDR);
-		nicbegin = priv->txmapringdma;
-		break;
-	case BK_PRIORITY:
-		tail = priv->txbkpringtail;
-		begin = priv->txbkpring;
-		head = priv->txbkpringhead;
-		nic = read_nic_dword(dev, TX_BKPRIORITY_RING_ADDR);
-		nicbegin = priv->txbkpringdma;
-		break;
-	case BE_PRIORITY:
-		tail = priv->txbepringtail;
-		begin = priv->txbepring;
-		head = priv->txbepringhead;
-		nic = read_nic_dword(dev, TX_BEPRIORITY_RING_ADDR);
-		nicbegin = priv->txbepringdma;
-		break;
-	case VI_PRIORITY:
-		tail = priv->txvipringtail;
-		begin = priv->txvipring;
-		head = priv->txvipringhead;
-		nic = read_nic_dword(dev, TX_VIPRIORITY_RING_ADDR);
-		nicbegin = priv->txvipringdma;
-		break;
-	case VO_PRIORITY:
-		tail = priv->txvopringtail;
-		begin = priv->txvopring;
-		head = priv->txvopringhead;
-		nic = read_nic_dword(dev, TX_VOPRIORITY_RING_ADDR);
-		nicbegin = priv->txvopringdma;
-		break;
-	case HI_PRIORITY:
-		tail = priv->txhpringtail;
-		begin = priv->txhpring;
-		head = priv->txhpringhead;
-		nic = read_nic_dword(dev, TX_HIGHPRIORITY_RING_ADDR);
-		nicbegin = priv->txhpringdma;
-		break;
-
-	default:
-		spin_unlock_irqrestore(&priv->tx_lock, flag);
-		return;
-	}
-
-	nicv = (u32 *)((nic - nicbegin) + (u8 *)begin);
-	if ((head <= tail && (nicv > tail || nicv < head)) ||
-		(head > tail && (nicv > tail && nicv < head))) {
-			DMESGW("nic has lost pointer");
-			spin_unlock_irqrestore(&priv->tx_lock, flag);
-			rtl8180_restart(dev);
-			return;
-		}
-
-	/*
-	 * We check all the descriptors between the head and the nic,
-	 * but not the currently pointed by the nic (the next to be txed)
-	 * and the previous of the pointed (might be in process ??)
-	 */
-	offs = (nic - nicbegin);
-	offs = offs / 8 / 4;
-	hd = (head - begin) / 8;
-
-	if (offs >= hd)
-		j = offs - hd;
-	else
-		j = offs + (priv->txringcount-1-hd);
-
-	j -= 2;
-	if (j < 0)
-		j = 0;
-
-	for (i = 0; i < j; i++) {
-		if ((*head) & (1<<31))
-			break;
-		if (((*head)&(0x10000000)) != 0) {
-			priv->CurrRetryCnt += (u16)((*head) & (0x000000ff));
-			if (!error)
-				priv->NumTxOkTotal++;
-		}
-
-		if (!error)
-			priv->NumTxOkBytesTotal += (*(head+3)) & (0x00000fff);
-
-		*head = *head & ~(1<<31);
-
-		if ((head - begin)/8 == priv->txringcount-1)
-			head = begin;
-		else
-			head += 8;
-	}
-
-	/*
-	 * The head has been moved to the last certainly TXed
-	 * (or at least processed by the nic) packet.
-	 * The driver take forcefully owning of all these packets
-	 * If the packet previous of the nic pointer has been
-	 * processed this doesn't matter: it will be checked
-	 * here at the next round. Anyway if no more packet are
-	 * TXed no memory leak occur at all.
-	 */
-
-	switch (pri) {
-	case MANAGE_PRIORITY:
-		priv->txmapringhead = head;
-
-		if (priv->ack_tx_to_ieee) {
-			if (rtl8180_is_tx_queue_empty(dev)) {
-				priv->ack_tx_to_ieee = 0;
-				ieee80211_ps_tx_ack(priv->ieee80211, !error);
-			}
-		}
-		break;
-	case BK_PRIORITY:
-		priv->txbkpringhead = head;
-		break;
-	case BE_PRIORITY:
-		priv->txbepringhead = head;
-		break;
-	case VI_PRIORITY:
-		priv->txvipringhead = head;
-		break;
-	case VO_PRIORITY:
-		priv->txvopringhead = head;
-		break;
-	case HI_PRIORITY:
-		priv->txhpringhead = head;
-		break;
-	}
-
-	spin_unlock_irqrestore(&priv->tx_lock, flag);
-}
-
-static irqreturn_t rtl8180_interrupt(int irq, void *netdev)
-{
-	struct net_device *dev = (struct net_device *) netdev;
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	unsigned long flags;
-	u32 inta;
-
-	/* We should return IRQ_NONE, but for now let me keep this */
-	if (priv->irq_enabled == 0)
-		return IRQ_HANDLED;
-
-	spin_lock_irqsave(&priv->irq_th_lock, flags);
-
-	/* ISR: 4bytes */
-	inta = read_nic_dword(dev, ISR);
-	write_nic_dword(dev, ISR, inta); /* reset int situation */
-
-	priv->stats.shints++;
-
-	if (!inta) {
-		spin_unlock_irqrestore(&priv->irq_th_lock, flags);
-		return IRQ_HANDLED;
-	/*
-	 * most probably we can safely return IRQ_NONE,
-	 * but for now is better to avoid problems
-	 */
-	}
-
-	if (inta == 0xffff) {
-		/* HW disappeared */
-		spin_unlock_irqrestore(&priv->irq_th_lock, flags);
-		return IRQ_HANDLED;
-	}
-
-	priv->stats.ints++;
-
-	if (!netif_running(dev)) {
-		spin_unlock_irqrestore(&priv->irq_th_lock, flags);
-		return IRQ_HANDLED;
-	}
-
-	if (inta & ISR_TimeOut)
-		write_nic_dword(dev, TimerInt, 0);
-
-	if (inta & ISR_TBDOK)
-		priv->stats.txbeacon++;
-
-	if (inta & ISR_TBDER)
-		priv->stats.txbeaconerr++;
-
-	if (inta & IMR_TMGDOK)
-		rtl8180_tx_isr(dev, MANAGE_PRIORITY, 0);
-
-	if (inta & ISR_THPDER) {
-		priv->stats.txhperr++;
-		rtl8180_tx_isr(dev, HI_PRIORITY, 1);
-		priv->ieee80211->stats.tx_errors++;
-	}
-
-	if (inta & ISR_THPDOK) { /* High priority tx ok */
-		priv->link_detect.num_tx_ok_in_period++;
-		priv->stats.txhpokint++;
-		rtl8180_tx_isr(dev, HI_PRIORITY, 0);
-	}
-
-	if (inta & ISR_RER)
-		priv->stats.rxerr++;
-
-	if (inta & ISR_TBKDER) { /* corresponding to BK_PRIORITY */
-		priv->stats.txbkperr++;
-		priv->ieee80211->stats.tx_errors++;
-		rtl8180_tx_isr(dev, BK_PRIORITY, 1);
-		rtl8180_try_wake_queue(dev, BK_PRIORITY);
-	}
-
-	if (inta & ISR_TBEDER) { /* corresponding to BE_PRIORITY */
-		priv->stats.txbeperr++;
-		priv->ieee80211->stats.tx_errors++;
-		rtl8180_tx_isr(dev, BE_PRIORITY, 1);
-		rtl8180_try_wake_queue(dev, BE_PRIORITY);
-	}
-	if (inta & ISR_TNPDER) { /* corresponding to VO_PRIORITY */
-		priv->stats.txnperr++;
-		priv->ieee80211->stats.tx_errors++;
-		rtl8180_tx_isr(dev, NORM_PRIORITY, 1);
-		rtl8180_try_wake_queue(dev, NORM_PRIORITY);
-	}
-
-	if (inta & ISR_TLPDER) { /* corresponding to VI_PRIORITY */
-		priv->stats.txlperr++;
-		priv->ieee80211->stats.tx_errors++;
-		rtl8180_tx_isr(dev, LOW_PRIORITY, 1);
-		rtl8180_try_wake_queue(dev, LOW_PRIORITY);
-	}
-
-	if (inta & ISR_ROK) {
-		priv->stats.rxint++;
-		tasklet_schedule(&priv->irq_rx_tasklet);
-	}
-
-	if (inta & ISR_RQoSOK) {
-		priv->stats.rxint++;
-		tasklet_schedule(&priv->irq_rx_tasklet);
-	}
-
-	if (inta & ISR_BcnInt)
-		rtl8180_prepare_beacon(dev);
-
-	if (inta & ISR_RDU) {
-		DMESGW("No RX descriptor available");
-		priv->stats.rxrdu++;
-		tasklet_schedule(&priv->irq_rx_tasklet);
-	}
-
-	if (inta & ISR_RXFOVW) {
-		priv->stats.rxoverflow++;
-		tasklet_schedule(&priv->irq_rx_tasklet);
-	}
-
-	if (inta & ISR_TXFOVW)
-		priv->stats.txoverflow++;
-
-	if (inta & ISR_TNPDOK) { /* Normal priority tx ok */
-		priv->link_detect.num_tx_ok_in_period++;
-		priv->stats.txnpokint++;
-		rtl8180_tx_isr(dev, NORM_PRIORITY, 0);
-		rtl8180_try_wake_queue(dev, NORM_PRIORITY);
-	}
-
-	if (inta & ISR_TLPDOK) { /* Low priority tx ok */
-		priv->link_detect.num_tx_ok_in_period++;
-		priv->stats.txlpokint++;
-		rtl8180_tx_isr(dev, LOW_PRIORITY, 0);
-		rtl8180_try_wake_queue(dev, LOW_PRIORITY);
-	}
-
-	if (inta & ISR_TBKDOK) { /* corresponding to BK_PRIORITY */
-		priv->stats.txbkpokint++;
-		priv->link_detect.num_tx_ok_in_period++;
-		rtl8180_tx_isr(dev, BK_PRIORITY, 0);
-		rtl8180_try_wake_queue(dev, BE_PRIORITY);
-	}
-
-	if (inta & ISR_TBEDOK) { /* corresponding to BE_PRIORITY */
-		priv->stats.txbeperr++;
-		priv->link_detect.num_tx_ok_in_period++;
-		rtl8180_tx_isr(dev, BE_PRIORITY, 0);
-		rtl8180_try_wake_queue(dev, BE_PRIORITY);
-	}
-	force_pci_posting(dev);
-	spin_unlock_irqrestore(&priv->irq_th_lock, flags);
-
-	return IRQ_HANDLED;
-}
-
-void rtl8180_irq_rx_tasklet(struct r8180_priv *priv)
-{
-	rtl8180_rx(priv->dev);
-}
-
-void GPIOChangeRFWorkItemCallBack(struct work_struct *work)
-{
-	struct ieee80211_device *ieee = container_of(
-		work, struct ieee80211_device, GPIOChangeRFWorkItem.work);
-	struct net_device *dev = ieee->dev;
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	u8 btPSR;
-	u8 btConfig0;
-	enum rt_rf_power_state eRfPowerStateToSet;
-	bool bActuallySet = false;
-
-	char *argv[3];
-	static char *RadioPowerPath = "/etc/acpi/events/RadioPower.sh";
-	static char *envp[] = {"HOME=/", "TERM=linux",
-		"PATH=/usr/bin:/bin", NULL};
-	static int readf_count;
-
-	readf_count = (readf_count+1)%0xffff;
-	/* We should turn off LED before polling FF51[4]. */
-
-	/* Turn off LED. */
-	btPSR = read_nic_byte(dev, PSR);
-	write_nic_byte(dev, PSR, (btPSR & ~BIT3));
-
-	/* It need to delay 4us suggested */
-	udelay(4);
-
-	/* HW radio On/Off according to the value of FF51[4](config0) */
-	btConfig0 = btPSR = read_nic_byte(dev, CONFIG0);
-
-	eRfPowerStateToSet = (btConfig0 & BIT4) ?  RF_ON : RF_OFF;
-
-	/* Turn LED back on when radio enabled */
-	if (eRfPowerStateToSet == RF_ON)
-		write_nic_byte(dev, PSR, btPSR | BIT3);
-
-	if ((priv->ieee80211->bHwRadioOff == true) &&
-	   (eRfPowerStateToSet == RF_ON)) {
-		priv->ieee80211->bHwRadioOff = false;
-		bActuallySet = true;
-	} else if ((priv->ieee80211->bHwRadioOff == false) &&
-		  (eRfPowerStateToSet == RF_OFF)) {
-		priv->ieee80211->bHwRadioOff = true;
-		bActuallySet = true;
-	}
-
-	if (bActuallySet) {
-		MgntActSet_RF_State(dev, eRfPowerStateToSet, RF_CHANGE_BY_HW);
-
-		/* To update the UI status for Power status changed */
-		if (priv->ieee80211->bHwRadioOff == true)
-			argv[1] = "RFOFF";
-		else
-			argv[1] = "RFON";
-		argv[0] = RadioPowerPath;
-		argv[2] = NULL;
-
-		call_usermodehelper(RadioPowerPath, argv, envp, UMH_WAIT_PROC);
-	}
-}
-
-module_init(rtl8180_pci_module_init);
-module_exit(rtl8180_pci_module_exit);
diff --git a/drivers/staging/rtl8187se/r8180_dm.c b/drivers/staging/rtl8187se/r8180_dm.c
deleted file mode 100644
index 8c020e06486..00000000000
--- a/drivers/staging/rtl8187se/r8180_dm.c
+++ /dev/null
@@ -1,1139 +0,0 @@
-#include "r8180_dm.h"
-#include "r8180_hw.h"
-#include "r8180_93cx6.h"
-
- /*	Return TRUE if we shall perform High Power Mechanism, FALSE otherwise. */
-#define RATE_ADAPTIVE_TIMER_PERIOD      300
-
-bool CheckHighPower(struct net_device *dev)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	struct ieee80211_device *ieee = priv->ieee80211;
-
-	if (!priv->bRegHighPowerMechanism)
-		return false;
-
-	if (ieee->state == IEEE80211_LINKED_SCANNING)
-		return false;
-
-	return true;
-}
-
-/*
- *	Description:
- *		Update Tx power level if necessary.
- *		See also DoRxHighPower() and SetTxPowerLevel8185() for reference.
- *
- *	Note:
- *		The reason why we udpate Tx power level here instead of DoRxHighPower()
- *		is the number of IO to change Tx power is much more than channel TR switch
- *		and they are related to OFDM and MAC registers.
- *		So, we don't want to update it so frequently in per-Rx packet base.
- */
-static void DoTxHighPower(struct net_device *dev)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	u16			HiPwrUpperTh = 0;
-	u16			HiPwrLowerTh = 0;
-	u8			RSSIHiPwrUpperTh;
-	u8			RSSIHiPwrLowerTh;
-	u8			u1bTmp;
-	char			OfdmTxPwrIdx, CckTxPwrIdx;
-
-	HiPwrUpperTh = priv->RegHiPwrUpperTh;
-	HiPwrLowerTh = priv->RegHiPwrLowerTh;
-
-	HiPwrUpperTh = HiPwrUpperTh * 10;
-	HiPwrLowerTh = HiPwrLowerTh * 10;
-	RSSIHiPwrUpperTh = priv->RegRSSIHiPwrUpperTh;
-	RSSIHiPwrLowerTh = priv->RegRSSIHiPwrLowerTh;
-
-	/* lzm add 080826 */
-	OfdmTxPwrIdx  = priv->chtxpwr_ofdm[priv->ieee80211->current_network.channel];
-	CckTxPwrIdx  = priv->chtxpwr[priv->ieee80211->current_network.channel];
-
-	if ((priv->UndecoratedSmoothedSS > HiPwrUpperTh) ||
-		(priv->bCurCCKPkt && (priv->CurCCKRSSI > RSSIHiPwrUpperTh))) {
-		/* Stevenl suggested that degrade 8dbm in high power sate. 2007-12-04 Isaiah */
-
-		priv->bToUpdateTxPwr = true;
-		u1bTmp = read_nic_byte(dev, CCK_TXAGC);
-
-		/* If it never enter High Power. */
-		if (CckTxPwrIdx == u1bTmp) {
-			u1bTmp = (u1bTmp > 16) ? (u1bTmp - 16) : 0;  /* 8dbm */
-			write_nic_byte(dev, CCK_TXAGC, u1bTmp);
-
-			u1bTmp = read_nic_byte(dev, OFDM_TXAGC);
-			u1bTmp = (u1bTmp > 16) ? (u1bTmp - 16) : 0;  /* 8dbm */
-			write_nic_byte(dev, OFDM_TXAGC, u1bTmp);
-		}
-
-	} else if ((priv->UndecoratedSmoothedSS < HiPwrLowerTh) &&
-		(!priv->bCurCCKPkt || priv->CurCCKRSSI < RSSIHiPwrLowerTh)) {
-		if (priv->bToUpdateTxPwr) {
-			priv->bToUpdateTxPwr = false;
-			/* SD3 required. */
-			u1bTmp = read_nic_byte(dev, CCK_TXAGC);
-			if (u1bTmp < CckTxPwrIdx) {
-				write_nic_byte(dev, CCK_TXAGC, CckTxPwrIdx);
-			}
-
-			u1bTmp = read_nic_byte(dev, OFDM_TXAGC);
-			if (u1bTmp < OfdmTxPwrIdx) {
-				write_nic_byte(dev, OFDM_TXAGC, OfdmTxPwrIdx);
-			}
-		}
-	}
-}
-
-
-/*
- *	Description:
- *		Callback function of UpdateTxPowerWorkItem.
- *		Because of some event happened, e.g. CCX TPC, High Power Mechanism,
- *		We update Tx power of current channel again.
- */
-void rtl8180_tx_pw_wq(struct work_struct *work)
-{
-	struct delayed_work *dwork = to_delayed_work(work);
-	struct ieee80211_device *ieee = container_of(dwork, struct ieee80211_device, tx_pw_wq);
-	struct net_device *dev = ieee->dev;
-
-	DoTxHighPower(dev);
-}
-
-
-/*
- *	Return TRUE if we shall perform DIG Mechanism, FALSE otherwise.
- */
-bool CheckDig(struct net_device *dev)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	struct ieee80211_device *ieee = priv->ieee80211;
-
-	if (!priv->bDigMechanism)
-		return false;
-
-	if (ieee->state != IEEE80211_LINKED)
-		return false;
-
-	if ((priv->ieee80211->rate / 5) < 36) /* Schedule Dig under all OFDM rates. By Bruce, 2007-06-01. */
-		return false;
-	return true;
-}
-/*
- *	Implementation of DIG for Zebra and Zebra2.
- */
-static void DIG_Zebra(struct net_device *dev)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	u16			CCKFalseAlarm, OFDMFalseAlarm;
-	u16			OfdmFA1, OfdmFA2;
-	int			InitialGainStep = 7; /* The number of initial gain stages. */
-	int			LowestGainStage = 4; /* The capable lowest stage of performing dig workitem. */
-	u32			AwakePeriodIn2Sec = 0;
-
-	CCKFalseAlarm = (u16)(priv->FalseAlarmRegValue & 0x0000ffff);
-	OFDMFalseAlarm = (u16)((priv->FalseAlarmRegValue >> 16) & 0x0000ffff);
-	OfdmFA1 =  0x15;
-	OfdmFA2 = ((u16)(priv->RegDigOfdmFaUpTh)) << 8;
-
-	/* The number of initial gain steps is different, by Bruce, 2007-04-13. */
-	if (priv->InitialGain == 0) { /* autoDIG */
-		/* Advised from SD3 DZ */
-		priv->InitialGain = 4; /* In 87B, m74dBm means State 4 (m82dBm) */
-	}
-	/* Advised from SD3 DZ */
-	OfdmFA1 = 0x20;
-
-#if 1 /* lzm reserved 080826 */
-	AwakePeriodIn2Sec = (2000 - priv->DozePeriodInPast2Sec);
-	priv->DozePeriodInPast2Sec = 0;
-
-	if (AwakePeriodIn2Sec) {
-		OfdmFA1 = (u16)((OfdmFA1 * AwakePeriodIn2Sec) / 2000);
-		OfdmFA2 = (u16)((OfdmFA2 * AwakePeriodIn2Sec) / 2000);
-	} else {
-		;
-	}
-#endif
-
-	InitialGainStep = 8;
-	LowestGainStage = priv->RegBModeGainStage; /* Lowest gain stage. */
-
-	if (OFDMFalseAlarm > OfdmFA1) {
-		if (OFDMFalseAlarm > OfdmFA2) {
-			priv->DIG_NumberFallbackVote++;
-			if (priv->DIG_NumberFallbackVote > 1) {
-				/* serious OFDM  False Alarm, need fallback */
-				if (priv->InitialGain < InitialGainStep) {
-					priv->InitialGainBackUp = priv->InitialGain;
-
-					priv->InitialGain = (priv->InitialGain + 1);
-					UpdateInitialGain(dev);
-				}
-				priv->DIG_NumberFallbackVote = 0;
-				priv->DIG_NumberUpgradeVote = 0;
-			}
-		} else {
-			if (priv->DIG_NumberFallbackVote)
-				priv->DIG_NumberFallbackVote--;
-		}
-		priv->DIG_NumberUpgradeVote = 0;
-	} else {
-		if (priv->DIG_NumberFallbackVote)
-			priv->DIG_NumberFallbackVote--;
-		priv->DIG_NumberUpgradeVote++;
-
-		if (priv->DIG_NumberUpgradeVote > 9) {
-			if (priv->InitialGain > LowestGainStage) { /* In 87B, m78dBm means State 4 (m864dBm) */
-				priv->InitialGainBackUp = priv->InitialGain;
-
-				priv->InitialGain = (priv->InitialGain - 1);
-				UpdateInitialGain(dev);
-			}
-			priv->DIG_NumberFallbackVote = 0;
-			priv->DIG_NumberUpgradeVote = 0;
-		}
-	}
-}
-
-/*
- *	Dispatch DIG implementation according to RF.
- */
-static void DynamicInitGain(struct net_device *dev)
-{
-	DIG_Zebra(dev);
-}
-
-void rtl8180_hw_dig_wq(struct work_struct *work)
-{
-	struct delayed_work *dwork = to_delayed_work(work);
-	struct ieee80211_device *ieee = container_of(dwork, struct ieee80211_device, hw_dig_wq);
-	struct net_device *dev = ieee->dev;
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-	/* Read CCK and OFDM False Alarm. */
-	priv->FalseAlarmRegValue = read_nic_dword(dev, CCK_FALSE_ALARM);
-
-
-	/* Adjust Initial Gain dynamically. */
-	DynamicInitGain(dev);
-
-}
-
-static int IncludedInSupportedRates(struct r8180_priv *priv, u8 TxRate)
-{
-	u8 rate_len;
-	u8 rate_ex_len;
-	u8                      RateMask = 0x7F;
-	u8                      idx;
-	unsigned short          Found = 0;
-	u8                      NaiveTxRate = TxRate&RateMask;
-
-	rate_len = priv->ieee80211->current_network.rates_len;
-	rate_ex_len = priv->ieee80211->current_network.rates_ex_len;
-	for (idx = 0; idx < rate_len; idx++) {
-		if ((priv->ieee80211->current_network.rates[idx] & RateMask) == NaiveTxRate) {
-			Found = 1;
-			goto found_rate;
-		}
-	}
-	for (idx = 0; idx < rate_ex_len; idx++) {
-		if ((priv->ieee80211->current_network.rates_ex[idx] & RateMask) == NaiveTxRate) {
-			Found = 1;
-			goto found_rate;
-		}
-	}
-	return Found;
-found_rate:
-	return Found;
-}
-
-/*
- *	Get the Tx rate one degree up form the input rate in the supported rates.
- *	Return the upgrade rate if it is successed, otherwise return the input rate.
- */
-static u8 GetUpgradeTxRate(struct net_device *dev, u8 rate)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	u8                      UpRate;
-
-	/* Upgrade 1 degree. */
-	switch (rate) {
-	case 108: /* Up to 54Mbps. */
-		UpRate = 108;
-		break;
-
-	case 96: /* Up to 54Mbps. */
-		UpRate = 108;
-		break;
-
-	case 72: /* Up to 48Mbps. */
-		UpRate = 96;
-		break;
-
-	case 48: /* Up to 36Mbps. */
-		UpRate = 72;
-		break;
-
-	case 36: /* Up to 24Mbps. */
-		UpRate = 48;
-		break;
-
-	case 22: /* Up to 18Mbps. */
-		UpRate = 36;
-		break;
-
-	case 11: /* Up to 11Mbps. */
-		UpRate = 22;
-		break;
-
-	case 4: /* Up to 5.5Mbps. */
-		UpRate = 11;
-		break;
-
-	case 2: /* Up to 2Mbps. */
-		UpRate = 4;
-		break;
-
-	default:
-		printk("GetUpgradeTxRate(): Input Tx Rate(%d) is undefined!\n", rate);
-		return rate;
-	}
-	/* Check if the rate is valid. */
-	if (IncludedInSupportedRates(priv, UpRate)) {
-		return UpRate;
-	} else {
-		return rate;
-	}
-	return rate;
-}
-/*
- *	Get the Tx rate one degree down form the input rate in the supported rates.
- *	Return the degrade rate if it is successed, otherwise return the input rate.
- */
-
-static u8 GetDegradeTxRate(struct net_device *dev, u8 rate)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	u8                      DownRate;
-
-	/* Upgrade 1 degree. */
-	switch (rate) {
-	case 108: /* Down to 48Mbps. */
-		DownRate = 96;
-		break;
-
-	case 96: /* Down to 36Mbps. */
-		DownRate = 72;
-		break;
-
-	case 72: /* Down to 24Mbps. */
-		DownRate = 48;
-		break;
-
-	case 48: /* Down to 18Mbps. */
-		DownRate = 36;
-		break;
-
-	case 36: /* Down to 11Mbps. */
-		DownRate = 22;
-		break;
-
-	case 22: /* Down to 5.5Mbps. */
-		DownRate = 11;
-		break;
-
-	case 11: /* Down to 2Mbps. */
-		DownRate = 4;
-		break;
-
-	case 4: /* Down to 1Mbps. */
-		DownRate = 2;
-		break;
-
-	case 2: /* Down to 1Mbps. */
-		DownRate = 2;
-		break;
-
-	default:
-		printk("GetDegradeTxRate(): Input Tx Rate(%d) is undefined!\n", rate);
-		return rate;
-	}
-	/* Check if the rate is valid. */
-	if (IncludedInSupportedRates(priv, DownRate)) {
-		return DownRate;
-	} else {
-		return rate;
-	}
-	return rate;
-}
-/*
- *      Helper function to determine if specified data rate is
- *      CCK rate.
- */
-
-static bool MgntIsCckRate(u16 rate)
-{
-	bool bReturn = false;
-
-	if ((rate <= 22) && (rate != 12) && (rate != 18)) {
-		bReturn = true;
-	}
-
-	return bReturn;
-}
-/*
- *	Description:
- *		Tx Power tracking mechanism routine on 87SE.
- */
-void TxPwrTracking87SE(struct net_device *dev)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	u8	tmpu1Byte, CurrentThermal, Idx;
-	char	CckTxPwrIdx, OfdmTxPwrIdx;
-
-	tmpu1Byte = read_nic_byte(dev, EN_LPF_CAL);
-	CurrentThermal = (tmpu1Byte & 0xf0) >> 4; /*[ 7:4]: thermal meter indication. */
-	CurrentThermal = (CurrentThermal > 0x0c) ? 0x0c : CurrentThermal;/* lzm add 080826 */
-
-	if (CurrentThermal != priv->ThermalMeter) {
-		/* Update Tx Power level on each channel. */
-		for (Idx = 1; Idx < 15; Idx++) {
-			CckTxPwrIdx = priv->chtxpwr[Idx];
-			OfdmTxPwrIdx = priv->chtxpwr_ofdm[Idx];
-
-			if (CurrentThermal > priv->ThermalMeter) {
-				/* higher thermal meter. */
-				CckTxPwrIdx += (CurrentThermal - priv->ThermalMeter) * 2;
-				OfdmTxPwrIdx += (CurrentThermal - priv->ThermalMeter) * 2;
-
-				if (CckTxPwrIdx > 35)
-					CckTxPwrIdx = 35; /* Force TxPower to maximal index. */
-				if (OfdmTxPwrIdx > 35)
-					OfdmTxPwrIdx = 35;
-			} else {
-				/* lower thermal meter. */
-				CckTxPwrIdx -= (priv->ThermalMeter - CurrentThermal) * 2;
-				OfdmTxPwrIdx -= (priv->ThermalMeter - CurrentThermal) * 2;
-
-				if (CckTxPwrIdx < 0)
-					CckTxPwrIdx = 0;
-				if (OfdmTxPwrIdx < 0)
-					OfdmTxPwrIdx = 0;
-			}
-
-			/* Update TxPower level on CCK and OFDM resp. */
-			priv->chtxpwr[Idx] = CckTxPwrIdx;
-			priv->chtxpwr_ofdm[Idx] = OfdmTxPwrIdx;
-		}
-
-		/* Update TxPower level immediately. */
-		rtl8225z2_SetTXPowerLevel(dev, priv->ieee80211->current_network.channel);
-	}
-	priv->ThermalMeter = CurrentThermal;
-}
-static void StaRateAdaptive87SE(struct net_device *dev)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	unsigned long	CurrTxokCnt;
-	u16		CurrRetryCnt;
-	u16		CurrRetryRate;
-	unsigned long	CurrRxokCnt;
-	bool		bTryUp = false;
-	bool		bTryDown = false;
-	u8		TryUpTh = 1;
-	u8		TryDownTh = 2;
-	u32		TxThroughput;
-	long		CurrSignalStrength;
-	bool		bUpdateInitialGain = false;
-	u8		u1bOfdm = 0, u1bCck = 0;
-	char		OfdmTxPwrIdx, CckTxPwrIdx;
-
-	priv->RateAdaptivePeriod = RATE_ADAPTIVE_TIMER_PERIOD;
-
-
-	CurrRetryCnt	= priv->CurrRetryCnt;
-	CurrTxokCnt	= priv->NumTxOkTotal - priv->LastTxokCnt;
-	CurrRxokCnt	= priv->ieee80211->NumRxOkTotal - priv->LastRxokCnt;
-	CurrSignalStrength = priv->Stats_RecvSignalPower;
-	TxThroughput = (u32)(priv->NumTxOkBytesTotal - priv->LastTxOKBytes);
-	priv->LastTxOKBytes = priv->NumTxOkBytesTotal;
-	priv->CurrentOperaRate = priv->ieee80211->rate / 5;
-	/* 2 Compute retry ratio. */
-	if (CurrTxokCnt > 0) {
-		CurrRetryRate = (u16)(CurrRetryCnt * 100 / CurrTxokCnt);
-	} else {
-	/* It may be serious retry. To distinguish serious retry or no packets modified by Bruce */
-		CurrRetryRate = (u16)(CurrRetryCnt * 100 / 1);
-	}
-
-	priv->LastRetryCnt = priv->CurrRetryCnt;
-	priv->LastTxokCnt = priv->NumTxOkTotal;
-	priv->LastRxokCnt = priv->ieee80211->NumRxOkTotal;
-	priv->CurrRetryCnt = 0;
-
-	/* 2No Tx packets, return to init_rate or not? */
-	if (CurrRetryRate == 0 && CurrTxokCnt == 0) {
-		/*
-		 * After 9 (30*300ms) seconds in this condition, we try to raise rate.
-		 */
-		priv->TryupingCountNoData++;
-
-		/* [TRC Dell Lab] Extend raised period from 4.5sec to 9sec, Isaiah 2008-02-15 18:00 */
-		if (priv->TryupingCountNoData > 30) {
-			priv->TryupingCountNoData = 0;
-			priv->CurrentOperaRate = GetUpgradeTxRate(dev, priv->CurrentOperaRate);
-			/* Reset Fail Record */
-			priv->LastFailTxRate = 0;
-			priv->LastFailTxRateSS = -200;
-			priv->FailTxRateCount = 0;
-		}
-		goto SetInitialGain;
-	} else {
-		priv->TryupingCountNoData = 0; /*Reset trying up times. */
-	}
-
-
-	/*
-	 * For Netgear case, I comment out the following signal strength estimation,
-	 * which can results in lower rate to transmit when sample is NOT enough (e.g. PING request).
-	 *
-	 * Restructure rate adaptive as the following main stages:
-	 * (1) Add retry threshold in 54M upgrading condition with signal strength.
-	 * (2) Add the mechanism to degrade to CCK rate according to signal strength
-	 *		and retry rate.
-	 * (3) Remove all Initial Gain Updates over OFDM rate. To avoid the complicated
-	 *		situation, Initial Gain Update is upon on DIG mechanism except CCK rate.
-	 * (4) Add the mechanism of trying to upgrade tx rate.
-	 * (5) Record the information of upping tx rate to avoid trying upping tx rate constantly.
-	 *
-	 */
-
-	/*
-	 * 11Mbps or 36Mbps
-	 * Check more times in these rate(key rates).
-	 */
-	if (priv->CurrentOperaRate == 22 || priv->CurrentOperaRate == 72)
-		TryUpTh += 9;
-	/*
-	 * Let these rates down more difficult.
-	 */
-	if (MgntIsCckRate(priv->CurrentOperaRate) || priv->CurrentOperaRate == 36)
-		TryDownTh += 1;
-
-	/* 1 Adjust Rate. */
-	if (priv->bTryuping == true) {
-		/* 2 For Test Upgrading mechanism
-		 * Note:
-		 *	Sometimes the throughput is upon on the capability between the AP and NIC,
-		 *	thus the low data rate does not improve the performance.
-		 *	We randomly upgrade the data rate and check if the retry rate is improved.
-		 */
-
-		/* Upgrading rate did not improve the retry rate, fallback to the original rate. */
-		if ((CurrRetryRate > 25) && TxThroughput < priv->LastTxThroughput) {
-			/*Not necessary raising rate, fall back rate. */
-			bTryDown = true;
-		} else {
-			priv->bTryuping = false;
-		}
-	} else if (CurrSignalStrength > -47 && (CurrRetryRate < 50)) {
-		/*
-		 * 2For High Power
-		 *
-		 * Return to highest data rate, if signal strength is good enough.
-		 * SignalStrength threshold(-50dbm) is for RTL8186.
-		 * Revise SignalStrength threshold to -51dbm.
-		 */
-		/* Also need to check retry rate for safety, by Bruce, 2007-06-05. */
-		if (priv->CurrentOperaRate != priv->ieee80211->current_network.HighestOperaRate) {
-			bTryUp = true;
-			/* Upgrade Tx Rate directly. */
-			priv->TryupingCount += TryUpTh;
-		}
-
-	} else if (CurrTxokCnt > 9 && CurrTxokCnt < 100 && CurrRetryRate >= 600) {
-		/*
-		 *2 For Serious Retry
-		 *
-		 * Traffic is not busy but our Tx retry is serious.
-		 */
-		bTryDown = true;
-		/* Let Rate Mechanism to degrade tx rate directly. */
-		priv->TryDownCountLowData += TryDownTh;
-	} else if (priv->CurrentOperaRate == 108) {
-		/* 2For 54Mbps */
-		/* Air Link */
-		if ((CurrRetryRate > 26) && (priv->LastRetryRate > 25)) {
-			bTryDown = true;
-		}
-		/* Cable Link */
-		else if ((CurrRetryRate > 17) && (priv->LastRetryRate > 16) && (CurrSignalStrength > -72)) {
-			bTryDown = true;
-		}
-
-		if (bTryDown && (CurrSignalStrength < -75)) /* cable link */
-			priv->TryDownCountLowData += TryDownTh;
-	} else if (priv->CurrentOperaRate == 96) {
-		/* 2For 48Mbps */
-		/* Air Link */
-		if (((CurrRetryRate > 48) && (priv->LastRetryRate > 47))) {
-			bTryDown = true;
-		} else if (((CurrRetryRate > 21) && (priv->LastRetryRate > 20)) && (CurrSignalStrength > -74)) { /* Cable Link */
-			/* Down to rate 36Mbps. */
-			bTryDown = true;
-		} else if ((CurrRetryRate > (priv->LastRetryRate + 50)) && (priv->FailTxRateCount > 2)) {
-			bTryDown = true;
-			priv->TryDownCountLowData += TryDownTh;
-		} else if ((CurrRetryRate < 8) && (priv->LastRetryRate < 8)) { /* TO DO: need to consider (RSSI) */
-			bTryUp = true;
-		}
-
-		if (bTryDown && (CurrSignalStrength < -75)) {
-			priv->TryDownCountLowData += TryDownTh;
-		}
-	} else if (priv->CurrentOperaRate == 72) {
-		/* 2For 36Mbps */
-		if ((CurrRetryRate > 43) && (priv->LastRetryRate > 41)) {
-			/* Down to rate 24Mbps. */
-			bTryDown = true;
-		} else if ((CurrRetryRate > (priv->LastRetryRate + 50)) && (priv->FailTxRateCount > 2)) {
-			bTryDown = true;
-			priv->TryDownCountLowData += TryDownTh;
-		} else if ((CurrRetryRate < 15) &&  (priv->LastRetryRate < 16)) { /* TO DO: need to consider (RSSI) */
-			bTryUp = true;
-		}
-
-		if (bTryDown && (CurrSignalStrength < -80))
-			priv->TryDownCountLowData += TryDownTh;
-
-	} else if (priv->CurrentOperaRate == 48) {
-		/* 2For 24Mbps */
-		/* Air Link */
-		if (((CurrRetryRate > 63) && (priv->LastRetryRate > 62))) {
-			bTryDown = true;
-		} else if (((CurrRetryRate > 33) && (priv->LastRetryRate > 32)) && (CurrSignalStrength > -82)) { /* Cable Link */
-			bTryDown = true;
-		} else if ((CurrRetryRate > (priv->LastRetryRate + 50)) && (priv->FailTxRateCount > 2)) {
-			bTryDown = true;
-			priv->TryDownCountLowData += TryDownTh;
-		} else if ((CurrRetryRate < 20) && (priv->LastRetryRate < 21)) { /* TO DO: need to consider (RSSI) */
-			bTryUp = true;
-		}
-
-		if (bTryDown && (CurrSignalStrength < -82))
-			priv->TryDownCountLowData += TryDownTh;
-
-	} else if (priv->CurrentOperaRate == 36) {
-		if (((CurrRetryRate > 85) && (priv->LastRetryRate > 86))) {
-			bTryDown = true;
-		} else if ((CurrRetryRate > (priv->LastRetryRate + 50)) && (priv->FailTxRateCount > 2)) {
-			bTryDown = true;
-			priv->TryDownCountLowData += TryDownTh;
-		} else if ((CurrRetryRate < 22) && (priv->LastRetryRate < 23)) { /* TO DO: need to consider (RSSI) */
-			bTryUp = true;
-		}
-	} else if (priv->CurrentOperaRate == 22) {
-		/* 2For 11Mbps */
-		if (CurrRetryRate > 95) {
-			bTryDown = true;
-		} else if ((CurrRetryRate < 29) && (priv->LastRetryRate < 30)) { /*TO DO: need to consider (RSSI) */
-			bTryUp = true;
-		}
-	} else if (priv->CurrentOperaRate == 11) {
-		/* 2For 5.5Mbps */
-		if (CurrRetryRate > 149) {
-			bTryDown = true;
-		} else if ((CurrRetryRate < 60) && (priv->LastRetryRate < 65)) {
-			bTryUp = true;
-		}
-	} else if (priv->CurrentOperaRate == 4) {
-		/* 2For 2 Mbps */
-		if ((CurrRetryRate > 99) && (priv->LastRetryRate > 99)) {
-			bTryDown = true;
-		} else if ((CurrRetryRate < 65) && (priv->LastRetryRate < 70)) {
-			bTryUp = true;
-		}
-	} else if (priv->CurrentOperaRate == 2) {
-		/* 2For 1 Mbps */
-		if ((CurrRetryRate < 70) && (priv->LastRetryRate < 75)) {
-			bTryUp = true;
-		}
-	}
-
-	if (bTryUp && bTryDown)
-		printk("StaRateAdaptive87B(): Tx Rate tried upping and downing simultaneously!\n");
-
-	/* 1 Test Upgrading Tx Rate
-	 * Sometimes the cause of the low throughput (high retry rate) is the compatibility between the AP and NIC.
-	 * To test if the upper rate may cause lower retry rate, this mechanism randomly occurs to test upgrading tx rate.
-	 */
-	if (!bTryUp && !bTryDown && (priv->TryupingCount == 0) && (priv->TryDownCountLowData == 0)
-		&& priv->CurrentOperaRate != priv->ieee80211->current_network.HighestOperaRate && priv->FailTxRateCount < 2) {
-		if (jiffies % (CurrRetryRate + 101) == 0) {
-			bTryUp = true;
-			priv->bTryuping = true;
-		}
-	}
-
-	/* 1 Rate Mechanism */
-	if (bTryUp) {
-		priv->TryupingCount++;
-		priv->TryDownCountLowData = 0;
-
-		/*
-		 * Check more times if we need to upgrade indeed.
-		 * Because the largest value of pHalData->TryupingCount is 0xFFFF and
-		 * the largest value of pHalData->FailTxRateCount is 0x14,
-		 * this condition will be satisfied at most every 2 min.
-		 */
-
-		if ((priv->TryupingCount > (TryUpTh + priv->FailTxRateCount * priv->FailTxRateCount)) ||
-			(CurrSignalStrength > priv->LastFailTxRateSS) || priv->bTryuping) {
-			priv->TryupingCount = 0;
-			/*
-			 * When transferring from CCK to OFDM, DIG is an important issue.
-			 */
-			if (priv->CurrentOperaRate == 22)
-				bUpdateInitialGain = true;
-
-			/*
-			 * The difference in throughput between 48Mbps and 36Mbps is 8M.
-			 * So, we must be careful in this rate scale. Isaiah 2008-02-15.
-			 */
-			if (((priv->CurrentOperaRate == 72) || (priv->CurrentOperaRate == 48) || (priv->CurrentOperaRate == 36)) &&
-				(priv->FailTxRateCount > 2))
-				priv->RateAdaptivePeriod = (RATE_ADAPTIVE_TIMER_PERIOD / 2);
-
-			/* (1)To avoid upgrade frequently to the fail tx rate, add the FailTxRateCount into the threshold. */
-			/* (2)If the signal strength is increased, it may be able to upgrade. */
-
-			priv->CurrentOperaRate = GetUpgradeTxRate(dev, priv->CurrentOperaRate);
-
-			if (priv->CurrentOperaRate == 36) {
-				priv->bUpdateARFR = true;
-				write_nic_word(dev, ARFR, 0x0F8F); /* bypass 12/9/6 */
-			} else if (priv->bUpdateARFR) {
-				priv->bUpdateARFR = false;
-				write_nic_word(dev, ARFR, 0x0FFF); /* set 1M ~ 54Mbps. */
-			}
-
-			/* Update Fail Tx rate and count. */
-			if (priv->LastFailTxRate != priv->CurrentOperaRate) {
-				priv->LastFailTxRate = priv->CurrentOperaRate;
-				priv->FailTxRateCount = 0;
-				priv->LastFailTxRateSS = -200; /* Set lowest power. */
-			}
-		}
-	} else {
-		if (priv->TryupingCount > 0)
-			priv->TryupingCount--;
-	}
-
-	if (bTryDown) {
-		priv->TryDownCountLowData++;
-		priv->TryupingCount = 0;
-
-		/* Check if Tx rate can be degraded or Test trying upgrading should fallback. */
-		if (priv->TryDownCountLowData > TryDownTh || priv->bTryuping) {
-			priv->TryDownCountLowData = 0;
-			priv->bTryuping = false;
-			/* Update fail information. */
-			if (priv->LastFailTxRate == priv->CurrentOperaRate) {
-				priv->FailTxRateCount++;
-				/* Record the Tx fail rate signal strength. */
-				if (CurrSignalStrength > priv->LastFailTxRateSS)
-					priv->LastFailTxRateSS = CurrSignalStrength;
-			} else {
-				priv->LastFailTxRate = priv->CurrentOperaRate;
-				priv->FailTxRateCount = 1;
-				priv->LastFailTxRateSS = CurrSignalStrength;
-			}
-			priv->CurrentOperaRate = GetDegradeTxRate(dev, priv->CurrentOperaRate);
-
-			/* Reduce chariot training time at weak signal strength situation. SD3 ED demand. */
-			if ((CurrSignalStrength < -80) && (priv->CurrentOperaRate > 72)) {
-				priv->CurrentOperaRate = 72;
-			}
-
-			if (priv->CurrentOperaRate == 36) {
-				priv->bUpdateARFR = true;
-				write_nic_word(dev, ARFR, 0x0F8F); /* bypass 12/9/6 */
-			} else if (priv->bUpdateARFR) {
-				priv->bUpdateARFR = false;
-				write_nic_word(dev, ARFR, 0x0FFF); /* set 1M ~ 54Mbps. */
-			}
-
-			/*
-			 * When it is CCK rate, it may need to update initial gain to receive lower power packets.
-			 */
-			if (MgntIsCckRate(priv->CurrentOperaRate)) {
-				bUpdateInitialGain = true;
-			}
-		}
-	} else {
-		if (priv->TryDownCountLowData > 0)
-			priv->TryDownCountLowData--;
-	}
-
-	/*
-	 * Keep the Tx fail rate count to equal to 0x15 at most.
-	 * Reduce the fail count at least to 10 sec if tx rate is tending stable.
-	 */
-	if (priv->FailTxRateCount >= 0x15 ||
-		(!bTryUp && !bTryDown && priv->TryDownCountLowData == 0 && priv->TryupingCount && priv->FailTxRateCount > 0x6)) {
-		priv->FailTxRateCount--;
-	}
-
-
-	OfdmTxPwrIdx  = priv->chtxpwr_ofdm[priv->ieee80211->current_network.channel];
-	CckTxPwrIdx  = priv->chtxpwr[priv->ieee80211->current_network.channel];
-
-	/* Mac0x9e increase 2 level in 36M~18M situation */
-	if ((priv->CurrentOperaRate < 96) && (priv->CurrentOperaRate > 22)) {
-		u1bCck = read_nic_byte(dev, CCK_TXAGC);
-		u1bOfdm = read_nic_byte(dev, OFDM_TXAGC);
-
-		/* case 1: Never enter High power */
-		if (u1bCck == CckTxPwrIdx) {
-			if (u1bOfdm != (OfdmTxPwrIdx + 2)) {
-			priv->bEnhanceTxPwr = true;
-			u1bOfdm = ((u1bOfdm + 2) > 35) ? 35 : (u1bOfdm + 2);
-			write_nic_byte(dev, OFDM_TXAGC, u1bOfdm);
-			}
-		} else if (u1bCck < CckTxPwrIdx) {
-		/* case 2: enter high power */
-			if (!priv->bEnhanceTxPwr) {
-				priv->bEnhanceTxPwr = true;
-				u1bOfdm = ((u1bOfdm + 2) > 35) ? 35 : (u1bOfdm + 2);
-				write_nic_byte(dev, OFDM_TXAGC, u1bOfdm);
-			}
-		}
-	} else if (priv->bEnhanceTxPwr) {  /* 54/48/11/5.5/2/1 */
-		u1bCck = read_nic_byte(dev, CCK_TXAGC);
-		u1bOfdm = read_nic_byte(dev, OFDM_TXAGC);
-
-		/* case 1: Never enter High power */
-		if (u1bCck == CckTxPwrIdx) {
-			priv->bEnhanceTxPwr = false;
-			write_nic_byte(dev, OFDM_TXAGC, OfdmTxPwrIdx);
-		}
-		/* case 2: enter high power */
-		else if (u1bCck < CckTxPwrIdx) {
-			priv->bEnhanceTxPwr = false;
-			u1bOfdm = ((u1bOfdm - 2) > 0) ? (u1bOfdm - 2) : 0;
-			write_nic_byte(dev, OFDM_TXAGC, u1bOfdm);
-		}
-	}
-
-	/*
-	 * We need update initial gain when we set tx rate "from OFDM to CCK" or
-	 * "from CCK to OFDM".
-	 */
-SetInitialGain:
-	if (bUpdateInitialGain) {
-		if (MgntIsCckRate(priv->CurrentOperaRate)) { /* CCK */
-			if (priv->InitialGain > priv->RegBModeGainStage) {
-				priv->InitialGainBackUp = priv->InitialGain;
-
-				if (CurrSignalStrength < -85) /* Low power, OFDM [0x17] = 26. */
-					/* SD3 SYs suggest that CurrSignalStrength < -65, ofdm 0x17=26. */
-					priv->InitialGain = priv->RegBModeGainStage;
-
-				else if (priv->InitialGain > priv->RegBModeGainStage + 1)
-					priv->InitialGain -= 2;
-
-				else
-					priv->InitialGain--;
-
-				printk("StaRateAdaptive87SE(): update init_gain to index %d for date rate %d\n", priv->InitialGain, priv->CurrentOperaRate);
-				UpdateInitialGain(dev);
-			}
-		} else { /* OFDM */
-			if (priv->InitialGain < 4) {
-				priv->InitialGainBackUp = priv->InitialGain;
-
-				priv->InitialGain++;
-				printk("StaRateAdaptive87SE(): update init_gain to index %d for date rate %d\n", priv->InitialGain, priv->CurrentOperaRate);
-				UpdateInitialGain(dev);
-			}
-		}
-	}
-
-	/* Record the related info */
-	priv->LastRetryRate = CurrRetryRate;
-	priv->LastTxThroughput = TxThroughput;
-	priv->ieee80211->rate = priv->CurrentOperaRate * 5;
-}
-
-void rtl8180_rate_adapter(struct work_struct *work)
-{
-	struct delayed_work *dwork = to_delayed_work(work);
-	struct ieee80211_device *ieee = container_of(dwork, struct ieee80211_device, rate_adapter_wq);
-	struct net_device *dev = ieee->dev;
-	StaRateAdaptive87SE(dev);
-}
-void timer_rate_adaptive(unsigned long data)
-{
-	struct r8180_priv *priv = ieee80211_priv((struct net_device *)data);
-	if (!priv->up) {
-		return;
-	}
-	if ((priv->ieee80211->iw_mode != IW_MODE_MASTER)
-			&& (priv->ieee80211->state == IEEE80211_LINKED) &&
-			(priv->ForcedDataRate == 0)) {
-		queue_work(priv->ieee80211->wq, (void *)&priv->ieee80211->rate_adapter_wq);
-	}
-	priv->rateadapter_timer.expires = jiffies + MSECS(priv->RateAdaptivePeriod);
-	add_timer(&priv->rateadapter_timer);
-}
-
-void SwAntennaDiversityRxOk8185(struct net_device *dev, u8 SignalStrength)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-
-	priv->AdRxOkCnt++;
-
-	if (priv->AdRxSignalStrength != -1) {
-		priv->AdRxSignalStrength = ((priv->AdRxSignalStrength * 7) + (SignalStrength * 3)) / 10;
-	} else { /* Initialization case. */
-		priv->AdRxSignalStrength = SignalStrength;
-	}
-
-	if (priv->LastRxPktAntenna) /* Main antenna. */
-		priv->AdMainAntennaRxOkCnt++;
-	else	 /* Aux antenna. */
-		priv->AdAuxAntennaRxOkCnt++;
-}
- /*	Change Antenna Switch. */
-bool SetAntenna8185(struct net_device *dev, u8 u1bAntennaIndex)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	bool bAntennaSwitched = false;
-
-	switch (u1bAntennaIndex) {
-	case 0:
-		/* Mac register, main antenna */
-		write_nic_byte(dev, ANTSEL, 0x03);
-		/* base band */
-		write_phy_cck(dev, 0x11, 0x9b); /* Config CCK RX antenna. */
-		write_phy_ofdm(dev, 0x0d, 0x5c); /* Config OFDM RX antenna. */
-
-		bAntennaSwitched = true;
-		break;
-
-	case 1:
-		/* Mac register, aux antenna */
-		write_nic_byte(dev, ANTSEL, 0x00);
-		/* base band */
-		write_phy_cck(dev, 0x11, 0xbb); /* Config CCK RX antenna. */
-		write_phy_ofdm(dev, 0x0d, 0x54); /* Config OFDM RX antenna. */
-
-		bAntennaSwitched = true;
-
-		break;
-
-	default:
-		printk("SetAntenna8185: unknown u1bAntennaIndex(%d)\n", u1bAntennaIndex);
-		break;
-	}
-
-	if (bAntennaSwitched)
-		priv->CurrAntennaIndex = u1bAntennaIndex;
-
-	return bAntennaSwitched;
-}
- /*	Toggle Antenna switch. */
-bool SwitchAntenna(struct net_device *dev)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-
-	bool		bResult;
-
-	if (priv->CurrAntennaIndex == 0) {
-		bResult = SetAntenna8185(dev, 1);
-	} else {
-		bResult = SetAntenna8185(dev, 0);
-	}
-
-	return bResult;
-}
-/*
- * Engine of SW Antenna Diversity mechanism.
- * Since 8187 has no Tx part information,
- * this implementation is only dependend on Rx part information.
- */
-void SwAntennaDiversity(struct net_device *dev)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	bool   bSwCheckSS = false;
-	if (bSwCheckSS) {
-		priv->AdTickCount++;
-
-		printk("(1) AdTickCount: %d, AdCheckPeriod: %d\n",
-			priv->AdTickCount, priv->AdCheckPeriod);
-		printk("(2) AdRxSignalStrength: %ld, AdRxSsThreshold: %ld\n",
-			priv->AdRxSignalStrength, priv->AdRxSsThreshold);
-	}
-
-	/* Case 1. No Link. */
-	if (priv->ieee80211->state != IEEE80211_LINKED) {
-		priv->bAdSwitchedChecking = false;
-		/* I switch antenna here to prevent any one of antenna is broken before link established, 2006.04.18, by rcnjko.. */
-		SwitchAntenna(dev);
-
-	  /* Case 2. Linked but no packet receive.d */
-	} else if (priv->AdRxOkCnt == 0) {
-		priv->bAdSwitchedChecking = false;
-		SwitchAntenna(dev);
-
-	  /* Case 3. Evaluate last antenna switch action and undo it if necessary. */
-	} else if (priv->bAdSwitchedChecking == true) {
-		priv->bAdSwitchedChecking = false;
-
-		/* Adjust Rx signal strength threshold. */
-		priv->AdRxSsThreshold = (priv->AdRxSignalStrength + priv->AdRxSsBeforeSwitched) / 2;
-
-		priv->AdRxSsThreshold = (priv->AdRxSsThreshold > priv->AdMaxRxSsThreshold) ?
-					priv->AdMaxRxSsThreshold : priv->AdRxSsThreshold;
-		if (priv->AdRxSignalStrength < priv->AdRxSsBeforeSwitched) {
-		/* Rx signal strength is not improved after we swtiched antenna. => Swich back. */
-			/* Increase Antenna Diversity checking period due to bad decision. */
-			priv->AdCheckPeriod *= 2;
-			/* Increase Antenna Diversity checking period. */
-			if (priv->AdCheckPeriod > priv->AdMaxCheckPeriod)
-				priv->AdCheckPeriod = priv->AdMaxCheckPeriod;
-
-			/* Wrong decision => switch back. */
-			SwitchAntenna(dev);
-		} else {
-		/* Rx Signal Strength is improved. */
-
-			/* Reset Antenna Diversity checking period to its min value. */
-			priv->AdCheckPeriod = priv->AdMinCheckPeriod;
-		}
-
-	}
-	/* Case 4. Evaluate if we shall switch antenna now. */
-	/* Cause Table Speed is very fast in TRC Dell Lab, we check it every time. */
-	else {
-		priv->AdTickCount = 0;
-
-		/*
-		 * <Roger_Notes> We evaluate RxOk counts for each antenna first and than
-		 * evaluate signal strength.
-		 * The following operation can overcome the disability of CCA on both two antennas
-		 * When signal strength was extremely low or high.
-		 * 2008.01.30.
-		 */
-
-		/*
-		 * Evaluate RxOk count from each antenna if we shall switch default antenna now.
-		 */
-		if ((priv->AdMainAntennaRxOkCnt < priv->AdAuxAntennaRxOkCnt)
-			&& (priv->CurrAntennaIndex == 0)) {
-		/* We set Main antenna as default but RxOk count was less than Aux ones. */
-
-			/* Switch to Aux antenna. */
-			SwitchAntenna(dev);
-			priv->bHWAdSwitched = true;
-		} else if ((priv->AdAuxAntennaRxOkCnt < priv->AdMainAntennaRxOkCnt)
-			&& (priv->CurrAntennaIndex == 1)) {
-		/* We set Aux antenna as default but RxOk count was less than Main ones. */
-
-			/* Switch to Main antenna. */
-			SwitchAntenna(dev);
-			priv->bHWAdSwitched = true;
-		} else {
-		/* Default antenna is better. */
-
-			/* Still need to check current signal strength. */
-			priv->bHWAdSwitched = false;
-		}
-		/*
-		 * <Roger_Notes> We evaluate Rx signal strength ONLY when default antenna
-		 * didn't change by HW evaluation.
-		 * 2008.02.27.
-		 *
-		 * [TRC Dell Lab] SignalStrength is inaccuracy. Isaiah 2008-03-05
-		 * For example, Throughput of aux is better than main antenna(about 10M v.s 2M),
-		 * but AdRxSignalStrength is less than main.
-		 * Our guess is that main antenna have lower throughput and get many change
-		 * to receive more CCK packets(ex.Beacon) which have stronger SignalStrength.
-		 */
-		if ((!priv->bHWAdSwitched) && (bSwCheckSS)) {
-			/* Evaluate Rx signal strength if we shall switch antenna now. */
-			if (priv->AdRxSignalStrength < priv->AdRxSsThreshold) {
-			/* Rx signal strength is weak => Switch Antenna. */
-				priv->AdRxSsBeforeSwitched = priv->AdRxSignalStrength;
-				priv->bAdSwitchedChecking = true;
-
-				SwitchAntenna(dev);
-			} else {
-			/* Rx signal strength is OK. */
-				priv->bAdSwitchedChecking = false;
-				/* Increase Rx signal strength threshold if necessary. */
-				if ((priv->AdRxSignalStrength > (priv->AdRxSsThreshold + 10)) && /* Signal is much stronger than current threshold */
-					priv->AdRxSsThreshold <= priv->AdMaxRxSsThreshold) { /* Current threhold is not yet reach upper limit. */
-
-					priv->AdRxSsThreshold = (priv->AdRxSsThreshold + priv->AdRxSignalStrength) / 2;
-					priv->AdRxSsThreshold = (priv->AdRxSsThreshold > priv->AdMaxRxSsThreshold) ?
-								priv->AdMaxRxSsThreshold : priv->AdRxSsThreshold;/* +by amy 080312 */
-				}
-
-				/* Reduce Antenna Diversity checking period if possible. */
-				if (priv->AdCheckPeriod > priv->AdMinCheckPeriod)
-					priv->AdCheckPeriod /= 2;
-			}
-		}
-	}
-	/* Reset antenna diversity Rx related statistics. */
-	priv->AdRxOkCnt = 0;
-	priv->AdMainAntennaRxOkCnt = 0;
-	priv->AdAuxAntennaRxOkCnt = 0;
-}
-
- /*	Return TRUE if we shall perform Tx Power Tracking Mechanism, FALSE otherwise. */
-bool CheckTxPwrTracking(struct net_device *dev)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-
-	if (!priv->bTxPowerTrack)
-		return false;
-
-	/* if 87SE is in High Power , don't do Tx Power Tracking. asked by SD3 ED. 2008-08-08 Isaiah */
-	if (priv->bToUpdateTxPwr)
-		return false;
-
-	return true;
-}
-
-
- /*	Timer callback function of SW Antenna Diversity. */
-void SwAntennaDiversityTimerCallback(struct net_device *dev)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	enum rt_rf_power_state rtState;
-
-	 /* We do NOT need to switch antenna while RF is off. */
-	rtState = priv->eRFPowerState;
-	do {
-		if (rtState == RF_OFF) {
-			break;
-		} else if (rtState == RF_SLEEP) {
-			/* Don't access BB/RF under Disable PLL situation. */
-			break;
-		}
-		SwAntennaDiversity(dev);
-
-	} while (false);
-
-	if (priv->up) {
-		priv->SwAntennaDiversityTimer.expires = jiffies + MSECS(ANTENNA_DIVERSITY_TIMER_PERIOD);
-		add_timer(&priv->SwAntennaDiversityTimer);
-	}
-}
-
diff --git a/drivers/staging/rtl8187se/r8180_dm.h b/drivers/staging/rtl8187se/r8180_dm.h
deleted file mode 100644
index cb4046f346e..00000000000
--- a/drivers/staging/rtl8187se/r8180_dm.h
+++ /dev/null
@@ -1,23 +0,0 @@
-#ifndef R8180_DM_H
-#define R8180_DM_H
-
-#include "r8180.h"
-/* #include "r8180_hw.h"	*/
-/* #include "r8180_93cx6.h"	*/
-void SwAntennaDiversityRxOk8185(struct net_device *dev, u8 SignalStrength);
-bool SetAntenna8185(struct net_device *dev, u8 u1bAntennaIndex);
-bool SwitchAntenna(struct net_device *dev);
-void SwAntennaDiversity(struct net_device *dev);
-void SwAntennaDiversityTimerCallback(struct net_device *dev);
-bool CheckDig(struct net_device *dev);
-bool CheckHighPower(struct net_device *dev);
-void rtl8180_hw_dig_wq(struct work_struct *work);
-void rtl8180_tx_pw_wq(struct work_struct *work);
-void rtl8180_rate_adapter(struct work_struct *work);
-void TxPwrTracking87SE(struct net_device *dev);
-bool CheckTxPwrTracking(struct net_device *dev);
-void rtl8180_rate_adapter(struct work_struct *work);
-void timer_rate_adaptive(unsigned long data);
-
-
-#endif
diff --git a/drivers/staging/rtl8187se/r8180_hw.h b/drivers/staging/rtl8187se/r8180_hw.h
deleted file mode 100644
index e59d74f8ecf..00000000000
--- a/drivers/staging/rtl8187se/r8180_hw.h
+++ /dev/null
@@ -1,588 +0,0 @@
-/*
-	This is part of rtl8180 OpenSource driver.
-	Copyright (C) Andrea Merello 2004-2005  <andrea.merello@gmail.com>
-	Released under the terms of GPL (General Public Licence)
-
-	Parts of this driver are based on the GPL part of the
-	official Realtek driver.
-	Parts of this driver are based on the rtl8180 driver skeleton
-	from Patric Schenke & Andres Salomon.
-	Parts of this driver are based on the Intel Pro Wireless
-	2100 GPL driver.
-
-	We want to tanks the Authors of those projects
-	and the Ndiswrapper project Authors.
-*/
-
-/* Mariusz Matuszek added full registers definition with Realtek's name */
-
-/* this file contains register definitions for the rtl8180 MAC controller */
-#ifndef R8180_HW
-#define R8180_HW
-
-
-#define BIT0	0x00000001
-#define BIT1	0x00000002
-#define BIT2	0x00000004
-#define BIT3	0x00000008
-#define BIT4	0x00000010
-#define BIT5	0x00000020
-#define BIT6	0x00000040
-#define BIT7	0x00000080
-#define BIT9	0x00000200
-#define BIT11	0x00000800
-#define BIT13	0x00002000
-#define BIT15	0x00008000
-#define BIT20	0x00100000
-#define BIT21	0x00200000
-#define BIT22	0x00400000
-#define BIT23	0x00800000
-#define BIT24	0x01000000
-#define BIT25	0x02000000
-#define BIT26	0x04000000
-#define BIT27	0x08000000
-#define BIT28	0x10000000
-#define BIT29	0x20000000
-#define BIT30	0x40000000
-#define BIT31	0x80000000
-
-#define MAX_SLEEP_TIME (10000)
-#define MIN_SLEEP_TIME (50)
-
-#define BB_HOST_BANG_EN (1<<2)
-#define BB_HOST_BANG_CLK (1<<1)
-
-#define MAC0 0
-#define MAC4 4
-
-#define CMD 0x37
-#define CMD_RST_SHIFT 4
-#define CMD_RX_ENABLE_SHIFT 3
-#define CMD_TX_ENABLE_SHIFT 2
-
-#define EPROM_CMD 0x50
-#define EPROM_CMD_RESERVED_MASK ((1<<5)|(1<<4))
-#define EPROM_CMD_OPERATING_MODE_SHIFT 6
-#define EPROM_CMD_OPERATING_MODE_MASK ((1<<7)|(1<<6))
-#define EPROM_CMD_CONFIG 0x3
-#define EPROM_CMD_NORMAL 0
-#define EPROM_CMD_LOAD 1
-#define EPROM_CMD_PROGRAM 2
-#define EPROM_CS_SHIFT 3
-#define EPROM_CK_SHIFT 2
-#define EPROM_W_SHIFT 1
-#define EPROM_R_SHIFT 0
-#define CONFIG2_DMA_POLLING_MODE_SHIFT 3
-
-#define INTA_TXOVERFLOW (1<<15)
-#define INTA_TIMEOUT (1<<14)
-#define INTA_HIPRIORITYDESCERR (1<<9)
-#define INTA_HIPRIORITYDESCOK (1<<8)
-#define INTA_NORMPRIORITYDESCERR (1<<7)
-#define INTA_NORMPRIORITYDESCOK (1<<6)
-#define INTA_RXOVERFLOW (1<<5)
-#define INTA_RXDESCERR (1<<4)
-#define INTA_LOWPRIORITYDESCERR (1<<3)
-#define INTA_LOWPRIORITYDESCOK (1<<2)
-#define INTA_RXOK (1)
-#define INTA_MASK 0x3c
-
-#define RXRING_ADDR 0xe4 /* page 0 */
-#define PGSELECT 0x5e
-#define PGSELECT_PG_SHIFT 0
-#define RX_CONF 0x44
-#define MAC_FILTER_MASK ((1<<0) | (1<<1) | (1<<2) | (1<<3) | (1<<5) | \
-(1<<12) | (1<<18) | (1<<19) | (1<<20) | (1<<21) | (1<<22) | (1<<23))
-#define RX_CHECK_BSSID_SHIFT 23
-#define ACCEPT_PWR_FRAME_SHIFT 22
-#define ACCEPT_MNG_FRAME_SHIFT 20
-#define ACCEPT_CTL_FRAME_SHIFT 19
-#define ACCEPT_DATA_FRAME_SHIFT 18
-#define ACCEPT_ICVERR_FRAME_SHIFT 12
-#define ACCEPT_CRCERR_FRAME_SHIFT 5
-#define ACCEPT_BCAST_FRAME_SHIFT 3
-#define ACCEPT_MCAST_FRAME_SHIFT 2
-#define ACCEPT_ALLMAC_FRAME_SHIFT 0
-#define ACCEPT_NICMAC_FRAME_SHIFT 1
-
-#define RX_FIFO_THRESHOLD_MASK ((1<<13) | (1<<14) | (1<<15))
-#define RX_FIFO_THRESHOLD_SHIFT 13
-#define RX_FIFO_THRESHOLD_NONE 7
-#define RX_AUTORESETPHY_SHIFT 28
-
-#define TX_CONF 0x40
-#define TX_CONF_HEADER_AUTOICREMENT_SHIFT 30
-#define TX_LOOPBACK_SHIFT 17
-#define TX_LOOPBACK_NONE 0
-#define TX_LOOPBACK_CONTINUE 3
-#define TX_LOOPBACK_MASK ((1<<17)|(1<<18))
-#define TX_DPRETRY_SHIFT 0
-#define R8180_MAX_RETRY 255
-#define TX_RTSRETRY_SHIFT 8
-#define TX_NOICV_SHIFT 19
-#define TX_NOCRC_SHIFT 16
-#define TX_DMA_POLLING 0xd9
-#define TX_DMA_POLLING_BEACON_SHIFT 7
-#define TX_DMA_POLLING_HIPRIORITY_SHIFT 6
-#define TX_DMA_POLLING_NORMPRIORITY_SHIFT 5
-#define TX_DMA_POLLING_LOWPRIORITY_SHIFT 4
-#define TX_MANAGEPRIORITY_RING_ADDR 0x0C
-#define TX_BKPRIORITY_RING_ADDR 0x10
-#define TX_BEPRIORITY_RING_ADDR 0x14
-#define TX_VIPRIORITY_RING_ADDR 0x20
-#define TX_VOPRIORITY_RING_ADDR 0x24
-#define TX_HIGHPRIORITY_RING_ADDR 0x28
-#define MAX_RX_DMA_MASK ((1<<8) | (1<<9) | (1<<10))
-#define MAX_RX_DMA_2048 7
-#define MAX_RX_DMA_1024	6
-#define MAX_RX_DMA_SHIFT 10
-#define INT_TIMEOUT 0x48
-#define CONFIG3_CLKRUN_SHIFT 2
-#define CONFIG3_ANAPARAM_W_SHIFT 6
-#define ANAPARAM 0x54
-#define BEACON_INTERVAL 0x70
-#define BEACON_INTERVAL_MASK ((1<<0)|(1<<1)|(1<<2)|(1<<3)|(1<<4)|(1<<5)| \
-(1<<6)|(1<<7)|(1<<8)|(1<<9))
-#define ATIM_MASK ((1<<0)|(1<<1)|(1<<2)|(1<<3)|(1<<4)|(1<<5)|(1<<6)|(1<<7)| \
-(1<<8)|(1<<9))
-#define ATIM 0x72
-#define EPROM_CS_SHIFT 3
-#define EPROM_CK_SHIFT 2
-#define PHY_ADR 0x7c
-#define SECURITY 0x5f /* 1209 this is sth wrong */
-#define SECURITY_WEP_TX_ENABLE_SHIFT 1
-#define SECURITY_WEP_RX_ENABLE_SHIFT 0
-#define SECURITY_ENCRYP_104 1
-#define SECURITY_ENCRYP_SHIFT 4
-#define SECURITY_ENCRYP_MASK ((1<<4)|(1<<5))
-#define KEY0 0x90  /* 1209 this is sth wrong */
-#define CONFIG2_ANTENNA_SHIFT 6
-#define TX_BEACON_RING_ADDR 0x4c
-#define CONFIG0_WEP40_SHIFT 7
-#define CONFIG0_WEP104_SHIFT 6
-#define AGCRESET_SHIFT 5
-
-
-
-/*
- * Operational registers offsets in PCI (I/O) space.
- * RealTek names are used.
- */
-
-#define TSFTR 0x0018
-
-#define TLPDA 0x0020
-
-#define BSSID 0x002E
-
-#define CR 0x0037
-
-#define RF_SW_CONFIG	        0x8			/* store data which is transmitted to RF for driver	*/
-#define RF_SW_CFG_SI		BIT1
-#define EIFS			0x2D			/* Extended InterFrame Space Timer, in unit of 4 us.	*/
-
-#define BRSR			0x34			/* Basic rate set										*/
-
-#define IMR 0x006C
-#define ISR 0x003C
-
-#define TCR 0x0040
-
-#define RCR 0x0044
-
-#define TimerInt 0x0048
-
-#define CR9346 0x0050
-
-#define CONFIG0 0x0051
-#define CONFIG2 0x0053
-
-#define MSR 0x0058
-
-#define CONFIG3 0x0059
-#define CONFIG4 0x005A
-	/* SD3 szuyitasi: Mac0x57= CC -> B0 Mac0x60= D1 -> C6	*/
-	/* Mac0x60 = 0x000004C6 power save parameters			*/
-	#define ANAPARM_ASIC_ON    0xB0054D00
-	#define ANAPARM2_ASIC_ON  0x000004C6
-
-	#define ANAPARM_ON ANAPARM_ASIC_ON
-	#define ANAPARM2_ON ANAPARM2_ASIC_ON
-
-#define TESTR 0x005B
-
-#define PSR 0x005E
-
-#define BcnItv 0x0070
-
-#define AtimWnd 0x0072
-
-#define BintrItv 0x0074
-
-#define PhyAddr 0x007C
-#define PhyDataR 0x007E
-
-/* following are for rtl8185 */
-#define RFPinsOutput 0x80
-#define RFPinsEnable 0x82
-#define RF_TIMING 0x8c
-#define RFPinsSelect 0x84
-#define ANAPARAM2 0x60
-#define RF_PARA 0x88
-#define RFPinsInput 0x86
-#define GP_ENABLE 0x90
-#define GPIO 0x91
-#define SW_CONTROL_GPIO 0x400
-#define TX_ANTENNA 0x9f
-#define TX_GAIN_OFDM 0x9e
-#define TX_GAIN_CCK 0x9d
-#define WPA_CONFIG 0xb0
-#define TX_AGC_CTL 0x9c
-#define TX_AGC_CTL_PERPACKET_GAIN_SHIFT 0
-#define TX_AGC_CTL_PERPACKET_ANTSEL_SHIFT 1
-#define TX_AGC_CTL_FEEDBACK_ANT 2
-#define RESP_RATE 0x34
-#define SIFS 0xb4
-#define DIFS 0xb5
-
-#define SLOT 0xb6
-#define CW_CONF 0xbc
-#define CW_CONF_PERPACKET_RETRY_SHIFT 1
-#define CW_CONF_PERPACKET_CW_SHIFT 0
-#define CW_VAL 0xbd
-#define MAX_RESP_RATE_SHIFT 4
-#define MIN_RESP_RATE_SHIFT 0
-#define RATE_FALLBACK 0xbe
-
-#define CONFIG5 0x00D8
-
-#define PHYPR			0xDA			/* 0xDA - 0x0B PHY Parameter Register.	*/
-
-#define FEMR			0x1D4	/* Function Event Mask register	*/
-
-#define FFER 0x00FC
-#define FFER_END 0x00FF
-
-
-
-/*
- * Bitmasks for specific register functions.
- * Names are derived from the register name and function name.
- *
- * <REGISTER>_<FUNCTION>[<bit>]
- *
- * this leads to some awkward names...
- */
-
-#define BRSR_BPLCP  ((1 << 8))
-#define BRSR_MBR    ((1 << 1)|(1 << 0))
-#define BRSR_MBR_8185 ((1 << 11)|(1 << 10)|(1 << 9)|(1 << 8)|(1 << 7)|(1 << 6)|(1 << 5)|(1 << 4)|(1 << 3)|(1 << 2)|(1 << 1)|(1 << 0))
-#define BRSR_MBR0   ((1 << 0))
-#define BRSR_MBR1   ((1 << 1))
-
-#define CR_RST      ((1 << 4))
-#define CR_RE       ((1 << 3))
-#define CR_TE       ((1 << 2))
-#define CR_MulRW    ((1 << 0))
-
-#define IMR_Dot11hInt	((1 << 25))			/*802.11h Measurement Interrupt					*/
-#define IMR_BcnDmaInt	((1 << 24))			/*Beacon DMA Interrupt */ /*What differenct between BcnDmaInt and BcnInt???	*/
-#define IMR_WakeInt		((1 << 23))			/*Wake Up Interrupt								*/
-#define IMR_TXFOVW		((1 << 22))			/*Tx FIFO Overflow Interrupt					*/
-#define IMR_TimeOut1	((1 << 21))			/*Time Out Interrupt 1							*/
-#define IMR_BcnInt		((1 << 20))			/*Beacon Time out Interrupt						*/
-#define IMR_ATIMInt		((1 << 19))			/*ATIM Time Out Interrupt						*/
-#define IMR_TBDER		((1 << 18))			/*Tx Beacon Descriptor Error Interrupt			*/
-#define IMR_TBDOK		((1 << 17))			/*Tx Beacon Descriptor OK Interrupt				*/
-#define IMR_THPDER		((1 << 16))			/*Tx High Priority Descriptor Error Interrupt	*/
-#define IMR_THPDOK		((1 << 15))			/*Tx High Priority Descriptor OK Interrupt		*/
-#define IMR_TVODER		((1 << 14))			/*Tx AC_VO Descriptor Error Interrupt			*/
-#define IMR_TVODOK		((1 << 13))			/*Tx AC_VO Descriptor OK Interrupt				*/
-#define IMR_FOVW		((1 << 12))			/*Rx FIFO Overflow Interrupt					*/
-#define IMR_RDU			((1 << 11))			/*Rx Descriptor Unavailable Interrupt			*/
-#define IMR_TVIDER		((1 << 10))			/*Tx AC_VI Descriptor Error Interrupt			*/
-#define IMR_TVIDOK		((1 << 9))			/*Tx AC_VI Descriptor OK Interrupt				*/
-#define IMR_RER			((1 << 8))			/*Rx Error Interrupt							*/
-#define IMR_ROK			((1 << 7))			/*Receive OK Interrupt							*/
-#define IMR_TBEDER		((1 << 6))			/*Tx AC_BE Descriptor Error Interrupt			*/
-#define IMR_TBEDOK		((1 << 5))			/*Tx AC_BE Descriptor OK Interrupt				*/
-#define IMR_TBKDER		((1 << 4))			/*Tx AC_BK Descriptor Error Interrupt			*/
-#define IMR_TBKDOK		((1 << 3))			/*Tx AC_BK Descriptor OK Interrupt				*/
-#define IMR_RQoSOK		((1 << 2))			/*Rx QoS OK Interrupt							*/
-#define IMR_TimeOut2	((1 << 1))			/*Time Out Interrupt 2							*/
-#define IMR_TimeOut3	((1 << 0))			/*Time Out Interrupt 3							*/
-#define IMR_TMGDOK      ((1 << 30))
-#define ISR_Dot11hInt	((1 << 25))			/*802.11h Measurement Interrupt					*/
-#define ISR_BcnDmaInt	((1 << 24))			/*Beacon DMA Interrupt	*/ /*What differenct between BcnDmaInt and BcnInt???	*/
-#define ISR_WakeInt		((1 << 23))			/*Wake Up Interrupt								*/
-#define ISR_TXFOVW		((1 << 22))			/*Tx FIFO Overflow Interrupt					*/
-#define ISR_TimeOut1	((1 << 21))			/*Time Out Interrupt 1							*/
-#define ISR_BcnInt		((1 << 20))			/*Beacon Time out Interrupt						*/
-#define ISR_ATIMInt		((1 << 19))			/*ATIM Time Out Interrupt						*/
-#define ISR_TBDER		((1 << 18))			/*Tx Beacon Descriptor Error Interrupt			*/
-#define ISR_TBDOK		((1 << 17))			/*Tx Beacon Descriptor OK Interrupt				*/
-#define ISR_THPDER		((1 << 16))			/*Tx High Priority Descriptor Error Interrupt	*/
-#define ISR_THPDOK		((1 << 15))			/*Tx High Priority Descriptor OK Interrupt		*/
-#define ISR_TVODER		((1 << 14))			/*Tx AC_VO Descriptor Error Interrupt			*/
-#define ISR_TVODOK		((1 << 13))			/*Tx AC_VO Descriptor OK Interrupt				*/
-#define ISR_FOVW		((1 << 12))			/*Rx FIFO Overflow Interrupt					*/
-#define ISR_RDU			((1 << 11))			/*Rx Descriptor Unavailable Interrupt			*/
-#define ISR_TVIDER		((1 << 10))			/*Tx AC_VI Descriptor Error Interrupt			*/
-#define ISR_TVIDOK		((1 << 9))			/*Tx AC_VI Descriptor OK Interrupt				*/
-#define ISR_RER			((1 << 8))			/*Rx Error Interrupt							*/
-#define ISR_ROK			((1 << 7))			/*Receive OK Interrupt							*/
-#define ISR_TBEDER		((1 << 6))			/*Tx AC_BE Descriptor Error Interrupt			*/
-#define ISR_TBEDOK		((1 << 5))			/*Tx AC_BE Descriptor OK Interrupt				*/
-#define ISR_TBKDER		((1 << 4))			/*Tx AC_BK Descriptor Error Interrupt			*/
-#define ISR_TBKDOK		((1 << 3))			/*Tx AC_BK Descriptor OK Interrupt				*/
-#define ISR_RQoSOK		((1 << 2))			/*Rx QoS OK Interrupt							*/
-#define ISR_TimeOut2	((1 << 1))			/*Time Out Interrupt 2							*/
-#define ISR_TimeOut3	((1 << 0))			/*Time Out Interrupt 3							*/
-
-/* these definition is used for Tx/Rx test temporarily */
-#define ISR_TLPDER  ISR_TVIDER
-#define ISR_TLPDOK  ISR_TVIDOK
-#define ISR_TNPDER  ISR_TVODER
-#define ISR_TNPDOK  ISR_TVODOK
-#define ISR_TimeOut ISR_TimeOut1
-#define ISR_RXFOVW ISR_FOVW
-
-
-#define HW_VERID_R8180_F 3
-#define HW_VERID_R8180_ABCD 2
-#define HW_VERID_R8185_ABC 4
-#define HW_VERID_R8185_D 5
-#define HW_VERID_R8185B_B 6
-
-#define TCR_CWMIN   ((1 << 31))
-#define TCR_SWSEQ   ((1 << 30))
-#define TCR_HWVERID_MASK ((1 << 27)|(1 << 26)|(1 << 25))
-#define TCR_HWVERID_SHIFT 25
-#define TCR_SAT     ((1 << 24))
-#define TCR_PLCP_LEN TCR_SAT /* rtl8180 */
-#define TCR_MXDMA_MASK   ((1 << 23)|(1 << 22)|(1 << 21))
-#define TCR_MXDMA_1024 6
-#define TCR_MXDMA_2048 7
-#define TCR_MXDMA_SHIFT  21
-#define TCR_DISCW   ((1 << 20))
-#define TCR_ICV     ((1 << 19))
-#define TCR_LBK     ((1 << 18)|(1 << 17))
-#define TCR_LBK1    ((1 << 18))
-#define TCR_LBK0    ((1 << 17))
-#define TCR_CRC     ((1 << 16))
-#define TCR_DPRETRY_MASK   ((1 << 15)|(1 << 14)|(1 << 13)|(1 << 12)|(1 << 11)|(1 << 10)|(1 << 9)|(1 << 8))
-#define TCR_RTSRETRY_MASK   ((1 << 0)|(1 << 1)|(1 << 2)|(1 << 3)|(1 << 4)|(1 << 5)|(1 << 6)|(1 << 7))
-#define TCR_PROBE_NOTIMESTAMP_SHIFT 29 /* rtl8185 */
-
-#define RCR_ONLYERLPKT ((1 << 31))
-#define RCR_CS_SHIFT   29
-#define RCR_CS_MASK    ((1 << 30) | (1 << 29))
-#define RCR_ENMARP     ((1 << 28))
-#define RCR_CBSSID     ((1 << 23))
-#define RCR_APWRMGT    ((1 << 22))
-#define RCR_ADD3       ((1 << 21))
-#define RCR_AMF        ((1 << 20))
-#define RCR_ACF        ((1 << 19))
-#define RCR_ADF        ((1 << 18))
-#define RCR_RXFTH      ((1 << 15)|(1 << 14)|(1 << 13))
-#define RCR_RXFTH2     ((1 << 15))
-#define RCR_RXFTH1     ((1 << 14))
-#define RCR_RXFTH0     ((1 << 13))
-#define RCR_AICV       ((1 << 12))
-#define RCR_MXDMA      ((1 << 10)|(1 << 9)|(1 << 8))
-#define RCR_MXDMA2     ((1 << 10))
-#define RCR_MXDMA1     ((1 << 9))
-#define RCR_MXDMA0     ((1 << 8))
-#define RCR_9356SEL    ((1 << 6))
-#define RCR_ACRC32     ((1 << 5))
-#define RCR_AB         ((1 << 3))
-#define RCR_AM         ((1 << 2))
-#define RCR_APM        ((1 << 1))
-#define RCR_AAP        ((1 << 0))
-
-#define CR9346_EEM     ((1 << 7)|(1 << 6))
-#define CR9346_EEM1    ((1 << 7))
-#define CR9346_EEM0    ((1 << 6))
-#define CR9346_EECS    ((1 << 3))
-#define CR9346_EESK    ((1 << 2))
-#define CR9346_EED1    ((1 << 1))
-#define CR9346_EED0    ((1 << 0))
-
-#define CONFIG3_PARM_En    ((1 << 6))
-#define CONFIG3_FuncRegEn  ((1 << 1))
-
-#define CONFIG4_PWRMGT     ((1 << 5))
-
-#define MSR_LINK_MASK      ((1 << 2)|(1 << 3))
-#define MSR_LINK_MANAGED   2
-#define MSR_LINK_NONE      0
-#define MSR_LINK_SHIFT     2
-#define MSR_LINK_ADHOC     1
-#define MSR_LINK_MASTER    3
-
-#define BcnItv_BcnItv      (0x01FF)
-
-#define AtimWnd_AtimWnd    (0x01FF)
-
-#define BintrItv_BintrItv  (0x01FF)
-
-#define FEMR_INTR    ((1 << 15))
-#define FEMR_WKUP    ((1 << 14))
-#define FEMR_GWAKE   ((1 << 4))
-
-#define FFER_INTR    ((1 << 15))
-#define FFER_GWAKE   ((1 << 4))
-
-/* Three wire mode.					*/
-#define SW_THREE_WIRE			0
-#define HW_THREE_WIRE			2
-/* RTL8187S by amy					*/
-#define HW_THREE_WIRE_PI		5
-#define HW_THREE_WIRE_SI		6
-/* by amy							*/
-#define TCR_LRL_OFFSET		0
-#define TCR_SRL_OFFSET		8
-#define TCR_MXDMA_OFFSET	21
-#define TCR_DISReqQsize_OFFSET		28
-#define TCR_DurProcMode_OFFSET		30
-
-#define RCR_MXDMA_OFFSET				8
-#define RCR_FIFO_OFFSET					13
-
-#define AckTimeOutReg	0x79		/* ACK timeout register, in unit of 4 us. */
-
-#define RFTiming			0x8C
-
-#define TPPollStop		0x93
-
-#define TXAGC_CTL		0x9C			/*< RJ_TODO_8185B> TX_AGC_CONTROL (0x9C seems be removed at 8185B, see p37). */
-#define CCK_TXAGC		0x9D
-#define OFDM_TXAGC		0x9E
-#define ANTSEL			0x9F
-
-#define ACM_CONTROL             0x00BF      /* ACM Control Registe */
-
-#define	IntMig			0xE2			/* Interrupt Migration (0xE2 ~ 0xE3)	*/
-
-#define TID_AC_MAP		0xE8			/* TID to AC Mapping Register			*/
-
-#define ANAPARAM3		0xEE			/* <RJ_TODO_8185B> How to use it?		*/
-
-#define AC_VO_PARAM		0xF0			/* AC_VO Parameters Record				*/
-#define AC_VI_PARAM		0xF4			/* AC_VI Parameters Record				*/
-#define AC_BE_PARAM		0xF8			/* AC_BE Parameters Record				*/
-#define AC_BK_PARAM		0xFC			/* AC_BK Parameters Record				*/
-
-#define GPIOCtrl			0x16B			/*GPIO Control Register.			*/
-#define ARFR			0x1E0	/* Auto Rate Fallback Register (0x1e0 ~ 0x1e2)	*/
-
-#define RFSW_CTRL			0x272	/* 0x272-0x273.								*/
-#define SW_3W_DB0			0x274	/* Software 3-wire data buffer bit 31~0.		*/
-#define SW_3W_DB1			0x278	/* Software 3-wire data buffer bit 63~32.	*/
-#define SW_3W_CMD0			0x27C	/* Software 3-wire Control/Status Register.	*/
-#define SW_3W_CMD1			0x27D	/* Software 3-wire Control/Status Register.	*/
-
-#define PI_DATA_READ		0X360	/* 0x360 - 0x361  Parallel Interface Data Register.	*/
-#define SI_DATA_READ		0x362	/* 0x362 - 0x363  Serial Interface Data Register.	*/
-
-/*
-----------------------------------------------------------------------------
-		8185B TPPollStop bits					(offset 0x93, 1 byte)
-----------------------------------------------------------------------------
-*/
-#define TPPOLLSTOP_BQ			(0x01 << 7)
-#define TPPOLLSTOP_AC_VIQ		(0x01 << 4)
-
-#define MSR_LINK_ENEDCA	   (1<<4)
-
-/*
-----------------------------------------------------------------------------
-		8187B AC_XX_PARAM bits
-----------------------------------------------------------------------------
-*/
-#define AC_PARAM_TXOP_LIMIT_OFFSET		16
-#define AC_PARAM_ECW_MAX_OFFSET			12
-#define AC_PARAM_ECW_MIN_OFFSET			8
-#define AC_PARAM_AIFS_OFFSET			0
-
-/*
-----------------------------------------------------------------------------
-		8187B ACM_CONTROL bits					(Offset 0xBF, 1 Byte)
-----------------------------------------------------------------------------
-*/
-#define VOQ_ACM_EN				(0x01 << 7)	/*BIT7	*/
-#define VIQ_ACM_EN				(0x01 << 6)	/*BIT6	*/
-#define BEQ_ACM_EN				(0x01 << 5)	/*BIT5	*/
-#define ACM_HW_EN				(0x01 << 4)	/*BIT4	*/
-#define VOQ_ACM_CTL				(0x01 << 2)	/*BIT2	*/	/* Set to 1 when AC_VO used time reaches or exceeds the admitted time	*/
-#define VIQ_ACM_CTL				(0x01 << 1)	/*BIT1	*/	/* Set to 1 when AC_VI used time reaches or exceeds the admitted time	*/
-#define BEQ_ACM_CTL				(0x01 << 0)	/*BIT0	*/	/* Set to 1 when AC_BE used time reaches or exceeds the admitted time	*/
-
-
-/*
-----------------------------------------------------------------------------
-		8185B SW_3W_CMD bits					(Offset 0x27C-0x27D, 16bit)
-----------------------------------------------------------------------------
-*/
-#define SW_3W_CMD0_HOLD		((1 << 7))
-#define SW_3W_CMD1_RE		((1 << 0)) /* BIT8		*/
-#define SW_3W_CMD1_WE		((1 << 1)) /* BIT9		*/
-#define SW_3W_CMD1_DONE		((1 << 2)) /* BIT10		*/
-
-#define BB_HOST_BANG_RW		(1 << 3)
-
-/*
-----------------------------------------------------------------------------
-		8185B RATE_FALLBACK_CTL bits			(Offset 0xBE, 8bit)
-----------------------------------------------------------------------------
-*/
-#define RATE_FALLBACK_CTL_ENABLE				((1 << 7))
-#define RATE_FALLBACK_CTL_ENABLE_RTSCTS		((1 << 6))
-/* Auto rate fallback per 2^n retry. */
-#define RATE_FALLBACK_CTL_AUTO_STEP0	0x00
-#define RATE_FALLBACK_CTL_AUTO_STEP1	0x01
-#define RATE_FALLBACK_CTL_AUTO_STEP2	0x02
-#define RATE_FALLBACK_CTL_AUTO_STEP3	0x03
-
-
-#define RTL8225z2_ANAPARAM_OFF	0x55480658
-#define RTL8225z2_ANAPARAM2_OFF	0x72003f70
-/* by amy for power save		*/
-#define RF_CHANGE_BY_HW BIT30
-#define RF_CHANGE_BY_PS BIT29
-#define RF_CHANGE_BY_IPS BIT28
-/* by amy for power save		*/
-/* by amy for antenna			*/
-#define EEPROM_SW_REVD_OFFSET 0x3f
-
-/*  BIT[8-9] is for SW Antenna Diversity.
- *  Only the value EEPROM_SW_AD_ENABLE means enable, other values are disable.
- */
-#define EEPROM_SW_AD_MASK			0x0300
-#define EEPROM_SW_AD_ENABLE			0x0100
-
-/* BIT[10-11] determine if Antenna 1 is the Default Antenna.
- * Only the value EEPROM_DEF_ANT_1 means TRUE, other values are FALSE.
- */
-#define EEPROM_DEF_ANT_MASK			0x0C00
-#define EEPROM_DEF_ANT_1			0x0400
-/*by amy for antenna																				*/
-/* {by amy 080312																					*/
-/* 0x7C, 0x7D Crystal calibration and Tx Power tracking mechanism. Added by Roger. 2007.12.10.		*/
-#define EEPROM_RSV						0x7C
-#define EEPROM_XTAL_CAL_XOUT_MASK	0x0F	/* 0x7C[3:0], Crystal calibration for Xout.				*/
-#define EEPROM_XTAL_CAL_XIN_MASK		0xF0	/* 0x7C[7:4], Crystal calibration for Xin.			*/
-#define EEPROM_THERMAL_METER_MASK	0x0F00	/* 0x7D[3:0], Thermal meter reference level.			*/
-#define EEPROM_XTAL_CAL_ENABLE		0x1000	/* 0x7D[4], Crystal calibration enabled/disabled BIT.	*/
-#define EEPROM_THERMAL_METER_ENABLE	0x2000	/* 0x7D[5], Thermal meter enabled/disabled BIT.			*/
-#define EN_LPF_CAL			0x238	/* Enable LPF Calibration.										*/
-#define PWR_METER_EN		BIT1
-/* <RJ_TODO_8185B> where are false alarm counters in 8185B? */
-#define CCK_FALSE_ALARM		0xD0
-/* by amy 080312} */
-
-/* YJ,add for Country IE, 080630 */
-#define EEPROM_COUNTRY_CODE  0x2E
-/* YJ,add,080630,end */
-
-#endif
diff --git a/drivers/staging/rtl8187se/r8180_rtl8225.h b/drivers/staging/rtl8187se/r8180_rtl8225.h
deleted file mode 100644
index 7df73927b3c..00000000000
--- a/drivers/staging/rtl8187se/r8180_rtl8225.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * This is part of the rtl8180-sa2400 driver released under the GPL (See file
- * COPYING for details).
- *
- * Copyright (c) 2005 Andrea Merello <andrea.merello@gmail.com>
- *
- * This files contains programming code for the rtl8225 radio frontend.
- *
- * *Many* thanks to Realtek Corp. for their great support!
- */
-
-#include "r8180.h"
-
-#define RTL8225_ANAPARAM_ON  0xa0000b59
-#define RTL8225_ANAPARAM_OFF 0xa00beb59
-#define RTL8225_ANAPARAM2_OFF 0x840dec11
-#define RTL8225_ANAPARAM2_ON  0x860dec11
-#define RTL8225_ANAPARAM_SLEEP 0xa00bab59
-#define RTL8225_ANAPARAM2_SLEEP 0x840dec11
-
-void rtl8225z2_rf_init(struct net_device *dev);
-void rtl8225z2_rf_set_chan(struct net_device *dev, short ch);
-void rtl8225z2_rf_close(struct net_device *dev);
-
-void RF_WriteReg(struct net_device *dev, u8 offset, u16 data);
-u16 RF_ReadReg(struct net_device *dev, u8 offset);
-
-void rtl8180_set_mode(struct net_device *dev, int mode);
-void rtl8180_set_mode(struct net_device *dev, int mode);
-bool SetZebraRFPowerState8185(struct net_device *dev,
-			      enum rt_rf_power_state eRFPowerState);
-void rtl8225z4_rf_sleep(struct net_device *dev);
-void rtl8225z4_rf_wakeup(struct net_device *dev);
-
diff --git a/drivers/staging/rtl8187se/r8180_rtl8225z2.c b/drivers/staging/rtl8187se/r8180_rtl8225z2.c
deleted file mode 100644
index 47104fa05c5..00000000000
--- a/drivers/staging/rtl8187se/r8180_rtl8225z2.c
+++ /dev/null
@@ -1,811 +0,0 @@
-/*
- * This is part of the rtl8180-sa2400 driver
- * released under the GPL (See file COPYING for details).
- * Copyright (c) 2005 Andrea Merello <andrea.merello@gmail.com>
- *
- * This files contains programming code for the rtl8225
- * radio frontend.
- *
- * *Many* thanks to Realtek Corp. for their great support!
- */
-
-#include "r8180_hw.h"
-#include "r8180_rtl8225.h"
-#include "r8180_93cx6.h"
-
-#include "ieee80211/dot11d.h"
-
-static void write_rtl8225(struct net_device *dev, u8 adr, u16 data)
-{
-	int i;
-	u16 out, select;
-	u8 bit;
-	u32 bangdata = (data << 4) | (adr & 0xf);
-
-	out = read_nic_word(dev, RFPinsOutput) & 0xfff3;
-
-	write_nic_word(dev, RFPinsEnable,
-		(read_nic_word(dev, RFPinsEnable) | 0x7));
-
-	select = read_nic_word(dev, RFPinsSelect);
-
-	write_nic_word(dev, RFPinsSelect, select | 0x7 |
-		       SW_CONTROL_GPIO);
-
-	force_pci_posting(dev);
-	udelay(10);
-
-	write_nic_word(dev, RFPinsOutput, out | BB_HOST_BANG_EN);
-
-	force_pci_posting(dev);
-	udelay(2);
-
-	write_nic_word(dev, RFPinsOutput, out);
-
-	force_pci_posting(dev);
-	udelay(10);
-
-	for (i = 15; i >= 0; i--) {
-		bit = (bangdata & (1 << i)) >> i;
-
-		write_nic_word(dev, RFPinsOutput, bit | out);
-
-		write_nic_word(dev, RFPinsOutput, bit | out | BB_HOST_BANG_CLK);
-		write_nic_word(dev, RFPinsOutput, bit | out | BB_HOST_BANG_CLK);
-
-		i--;
-		bit = (bangdata & (1 << i)) >> i;
-
-		write_nic_word(dev, RFPinsOutput, bit | out | BB_HOST_BANG_CLK);
-		write_nic_word(dev, RFPinsOutput, bit | out | BB_HOST_BANG_CLK);
-
-		write_nic_word(dev, RFPinsOutput, bit | out);
-
-	}
-
-	write_nic_word(dev, RFPinsOutput, out | BB_HOST_BANG_EN);
-
-	force_pci_posting(dev);
-	udelay(10);
-
-	write_nic_word(dev, RFPinsOutput, out | BB_HOST_BANG_EN);
-
-	write_nic_word(dev, RFPinsSelect, select | SW_CONTROL_GPIO);
-
-	rtl8185_rf_pins_enable(dev);
-}
-
-static const u8 rtl8225_agc[] = {
-	0x9e, 0x9e, 0x9e, 0x9e, 0x9e, 0x9e, 0x9e, 0x9e,
-	0x9d, 0x9c, 0x9b, 0x9a, 0x99, 0x98, 0x97, 0x96,
-	0x95, 0x94, 0x93, 0x92, 0x91, 0x90, 0x8f, 0x8e,
-	0x8d, 0x8c, 0x8b, 0x8a, 0x89, 0x88, 0x87, 0x86,
-	0x85, 0x84, 0x83, 0x82, 0x81, 0x80, 0x3f, 0x3e,
-	0x3d, 0x3c, 0x3b, 0x3a, 0x39, 0x38, 0x37, 0x36,
-	0x35, 0x34, 0x33, 0x32, 0x31, 0x30, 0x2f, 0x2e,
-	0x2d, 0x2c, 0x2b, 0x2a, 0x29, 0x28, 0x27, 0x26,
-	0x25, 0x24, 0x23, 0x22, 0x21, 0x20, 0x1f, 0x1e,
-	0x1d, 0x1c, 0x1b, 0x1a, 0x19, 0x18, 0x17, 0x16,
-	0x15, 0x14, 0x13, 0x12, 0x11, 0x10, 0x0f, 0x0e,
-	0x0d, 0x0c, 0x0b, 0x0a, 0x09, 0x08, 0x07, 0x06,
-	0x05, 0x04, 0x03, 0x02, 0x01, 0x01, 0x01, 0x01,
-	0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
-	0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
-	0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
-};
-
-static const u32 rtl8225_chan[] = {
-	0,
-	0x0080, 0x0100, 0x0180, 0x0200, 0x0280, 0x0300, 0x0380,
-	0x0400, 0x0480, 0x0500, 0x0580, 0x0600, 0x0680, 0x074A,
-};
-
-static const u8 rtl8225z2_gain_bg[] = {
-	0x23, 0x15, 0xa5, /* -82-1dBm */
-	0x23, 0x15, 0xb5, /* -82-2dBm */
-	0x23, 0x15, 0xc5, /* -82-3dBm */
-	0x33, 0x15, 0xc5, /* -78dBm */
-	0x43, 0x15, 0xc5, /* -74dBm */
-	0x53, 0x15, 0xc5, /* -70dBm */
-	0x63, 0x15, 0xc5, /* -66dBm */
-};
-
-static const u8 rtl8225z2_gain_a[] = {
-	0x13, 0x27, 0x5a, /* -82dBm */
-	0x23, 0x23, 0x58, /* -82dBm */
-	0x33, 0x1f, 0x56, /* -82dBm */
-	0x43, 0x1b, 0x54, /* -78dBm */
-	0x53, 0x17, 0x51, /* -74dBm */
-	0x63, 0x24, 0x4f, /* -70dBm */
-	0x73, 0x0f, 0x4c, /* -66dBm */
-};
-
-static const u16 rtl8225z2_rxgain[] = {
-	0x0400, 0x0401, 0x0402, 0x0403, 0x0404, 0x0405, 0x0408, 0x0409,
-	0x040a, 0x040b, 0x0502, 0x0503, 0x0504, 0x0505, 0x0540, 0x0541,
-	0x0542, 0x0543, 0x0544, 0x0545, 0x0580, 0x0581, 0x0582, 0x0583,
-	0x0584, 0x0585, 0x0588, 0x0589, 0x058a, 0x058b, 0x0643, 0x0644,
-	0x0645, 0x0680, 0x0681, 0x0682, 0x0683, 0x0684, 0x0685, 0x0688,
-	0x0689, 0x068a, 0x068b, 0x068c, 0x0742, 0x0743, 0x0744, 0x0745,
-	0x0780, 0x0781, 0x0782, 0x0783, 0x0784, 0x0785, 0x0788, 0x0789,
-	0x078a, 0x078b, 0x078c, 0x078d, 0x0790, 0x0791, 0x0792, 0x0793,
-	0x0794, 0x0795, 0x0798, 0x0799, 0x079a, 0x079b, 0x079c, 0x079d,
-	0x07a0, 0x07a1, 0x07a2, 0x07a3, 0x07a4, 0x07a5, 0x07a8, 0x07a9,
-	0x03aa, 0x03ab, 0x03ac, 0x03ad, 0x03b0, 0x03b1, 0x03b2, 0x03b3,
-	0x03b4, 0x03b5, 0x03b8, 0x03b9, 0x03ba, 0x03bb
-
-};
-
-static void rtl8225z2_set_gain(struct net_device *dev, short gain)
-{
-	const u8 *rtl8225_gain;
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	u8 mode = priv->ieee80211->mode;
-
-	if (mode == IEEE_B || mode == IEEE_G)
-		rtl8225_gain = rtl8225z2_gain_bg;
-	else
-		rtl8225_gain = rtl8225z2_gain_a;
-
-	write_phy_ofdm(dev, 0x0b, rtl8225_gain[gain * 3]);
-	write_phy_ofdm(dev, 0x1b, rtl8225_gain[gain * 3 + 1]);
-	write_phy_ofdm(dev, 0x1d, rtl8225_gain[gain * 3 + 2]);
-	write_phy_ofdm(dev, 0x21, 0x37);
-}
-
-static u32 read_rtl8225(struct net_device *dev, u8 adr)
-{
-	u32 data2Write = ((u32)(adr & 0x1f)) << 27;
-	u32 dataRead;
-	u32 mask;
-	u16 oval, oval2, oval3, tmp;
-	int i;
-	short bit, rw;
-	u8 wLength = 6;
-	u8 rLength = 12;
-	u8 low2high = 0;
-
-	oval = read_nic_word(dev, RFPinsOutput);
-	oval2 = read_nic_word(dev, RFPinsEnable);
-	oval3 = read_nic_word(dev, RFPinsSelect);
-
-	write_nic_word(dev, RFPinsEnable, (oval2|0xf));
-	write_nic_word(dev, RFPinsSelect, (oval3|0xf));
-
-	dataRead = 0;
-
-	oval &= ~0xf;
-
-	write_nic_word(dev, RFPinsOutput, oval | BB_HOST_BANG_EN);
-	udelay(4);
-
-	write_nic_word(dev, RFPinsOutput, oval);
-	udelay(5);
-
-	rw = 0;
-
-	mask = (low2high) ? 0x01 : (((u32)0x01)<<(32-1));
-
-	for (i = 0; i < wLength/2; i++) {
-		bit = ((data2Write&mask) != 0) ? 1 : 0;
-		write_nic_word(dev, RFPinsOutput, bit | oval | rw);
-		udelay(1);
-
-		write_nic_word(dev, RFPinsOutput,
-				bit | oval | BB_HOST_BANG_CLK | rw);
-		udelay(2);
-		write_nic_word(dev, RFPinsOutput,
-				bit | oval | BB_HOST_BANG_CLK | rw);
-		udelay(2);
-
-		mask = (low2high) ? (mask<<1) : (mask>>1);
-
-		if (i == 2) {
-			rw = BB_HOST_BANG_RW;
-			write_nic_word(dev, RFPinsOutput,
-					bit | oval | BB_HOST_BANG_CLK | rw);
-			udelay(2);
-			write_nic_word(dev, RFPinsOutput, bit | oval | rw);
-			udelay(2);
-			break;
-		}
-
-		bit = ((data2Write&mask) != 0) ? 1 : 0;
-
-		write_nic_word(dev, RFPinsOutput,
-				oval | bit | rw | BB_HOST_BANG_CLK);
-		udelay(2);
-		write_nic_word(dev, RFPinsOutput,
-				oval | bit | rw | BB_HOST_BANG_CLK);
-		udelay(2);
-
-		write_nic_word(dev, RFPinsOutput, oval | bit | rw);
-		udelay(1);
-
-		mask = (low2high) ? (mask<<1) : (mask>>1);
-	}
-
-	write_nic_word(dev, RFPinsOutput, rw|oval);
-	udelay(2);
-	mask = (low2high) ? 0x01 : (((u32)0x01) << (12-1));
-
-	/*
-	 * We must set data pin to HW controlled, otherwise RF can't driver it
-	 * and value RF register won't be able to read back properly.
-	 */
-	write_nic_word(dev, RFPinsEnable, (oval2 & (~0x01)));
-
-	for (i = 0; i < rLength; i++) {
-		write_nic_word(dev, RFPinsOutput, rw|oval); udelay(1);
-
-		write_nic_word(dev, RFPinsOutput, rw|oval|BB_HOST_BANG_CLK);
-		udelay(2);
-		write_nic_word(dev, RFPinsOutput, rw|oval|BB_HOST_BANG_CLK);
-		udelay(2);
-		write_nic_word(dev, RFPinsOutput, rw|oval|BB_HOST_BANG_CLK);
-		udelay(2);
-		tmp = read_nic_word(dev, RFPinsInput);
-
-		dataRead |= (tmp & BB_HOST_BANG_CLK ? mask : 0);
-
-		write_nic_word(dev, RFPinsOutput, (rw|oval)); udelay(2);
-
-		mask = (low2high) ? (mask<<1) : (mask>>1);
-	}
-
-	write_nic_word(dev, RFPinsOutput,
-			BB_HOST_BANG_EN | BB_HOST_BANG_RW | oval);
-	udelay(2);
-
-	write_nic_word(dev, RFPinsEnable, oval2);
-	write_nic_word(dev, RFPinsSelect, oval3); /* Set To SW Switch */
-	write_nic_word(dev, RFPinsOutput, 0x3a0);
-
-	return dataRead;
-}
-
-void rtl8225z2_rf_close(struct net_device *dev)
-{
-	RF_WriteReg(dev, 0x4, 0x1f);
-
-	force_pci_posting(dev);
-	mdelay(1);
-
-	rtl8180_set_anaparam(dev, RTL8225z2_ANAPARAM_OFF);
-	rtl8185_set_anaparam2(dev, RTL8225z2_ANAPARAM2_OFF);
-}
-
-/*
- * Map dBm into Tx power index according to current HW model, for example,
- * RF and PA, and current wireless mode.
- */
-static s8 DbmToTxPwrIdx(struct r8180_priv *priv,
-			enum wireless_mode mode, s32 PowerInDbm)
-{
-	bool bUseDefault = true;
-	s8 TxPwrIdx = 0;
-
-	/*
-	 * OFDM Power in dBm = Index * 0.5 + 0
-	 * CCK Power in dBm = Index * 0.25 + 13
-	 */
-	s32 tmp = 0;
-
-	if (mode == WIRELESS_MODE_G) {
-		bUseDefault = false;
-		tmp = (2 * PowerInDbm);
-
-		if (tmp < 0)
-			TxPwrIdx = 0;
-		else if (tmp > 40) /* 40 means 20 dBm. */
-			TxPwrIdx = 40;
-		else
-			TxPwrIdx = (s8)tmp;
-	} else if (mode == WIRELESS_MODE_B) {
-		bUseDefault = false;
-		tmp = (4 * PowerInDbm) - 52;
-
-		if (tmp < 0)
-			TxPwrIdx = 0;
-		else if (tmp > 28) /* 28 means 20 dBm. */
-			TxPwrIdx = 28;
-		else
-			TxPwrIdx = (s8)tmp;
-	}
-
-	/*
-	 * TRUE if we want to use a default implementation.
-	 * We shall set it to FALSE when we have exact translation formula
-	 * for target IC. 070622, by rcnjko.
-	 */
-	if (bUseDefault) {
-		if (PowerInDbm < 0)
-			TxPwrIdx = 0;
-		else if (PowerInDbm > 35)
-			TxPwrIdx = 35;
-		else
-			TxPwrIdx = (u8)PowerInDbm;
-	}
-
-	return TxPwrIdx;
-}
-
-void rtl8225z2_SetTXPowerLevel(struct net_device *dev, short ch)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	u8 max_cck_power_level;
-	u8 max_ofdm_power_level;
-	u8 min_ofdm_power_level;
-	char cck_power_level = (char)(0xff & priv->chtxpwr[ch]);
-	char ofdm_power_level = (char)(0xff & priv->chtxpwr_ofdm[ch]);
-
-	if (IS_DOT11D_ENABLE(priv->ieee80211) &&
-	    IS_DOT11D_STATE_DONE(priv->ieee80211)) {
-		u8 MaxTxPwrInDbm = DOT11D_GetMaxTxPwrInDbm(priv->ieee80211, ch);
-		u8 CckMaxPwrIdx = DbmToTxPwrIdx(priv, WIRELESS_MODE_B,
-							MaxTxPwrInDbm);
-		u8 OfdmMaxPwrIdx = DbmToTxPwrIdx(priv, WIRELESS_MODE_G,
-							MaxTxPwrInDbm);
-
-		if (cck_power_level > CckMaxPwrIdx)
-			cck_power_level = CckMaxPwrIdx;
-		if (ofdm_power_level > OfdmMaxPwrIdx)
-			ofdm_power_level = OfdmMaxPwrIdx;
-	}
-
-	max_cck_power_level = 15;
-	max_ofdm_power_level = 25;
-	min_ofdm_power_level = 10;
-
-	if (cck_power_level > 35)
-		cck_power_level = 35;
-
-	write_nic_byte(dev, CCK_TXAGC, cck_power_level);
-	force_pci_posting(dev);
-	mdelay(1);
-
-	if (ofdm_power_level > 35)
-		ofdm_power_level = 35;
-
-	if (priv->up == 0) {
-		write_phy_ofdm(dev, 2, 0x42);
-		write_phy_ofdm(dev, 5, 0x00);
-		write_phy_ofdm(dev, 6, 0x40);
-		write_phy_ofdm(dev, 7, 0x00);
-		write_phy_ofdm(dev, 8, 0x40);
-	}
-
-	write_nic_byte(dev, OFDM_TXAGC, ofdm_power_level);
-
-	if (ofdm_power_level <= 11) {
-		write_phy_ofdm(dev, 0x07, 0x5c);
-		write_phy_ofdm(dev, 0x09, 0x5c);
-	}
-
-	if (ofdm_power_level <= 17) {
-		write_phy_ofdm(dev, 0x07, 0x54);
-		write_phy_ofdm(dev, 0x09, 0x54);
-	} else {
-		write_phy_ofdm(dev, 0x07, 0x50);
-		write_phy_ofdm(dev, 0x09, 0x50);
-	}
-
-	force_pci_posting(dev);
-	mdelay(1);
-}
-
-void rtl8225z2_rf_set_chan(struct net_device *dev, short ch)
-{
-	rtl8225z2_SetTXPowerLevel(dev, ch);
-
-	RF_WriteReg(dev, 0x7, rtl8225_chan[ch]);
-
-	if ((RF_ReadReg(dev, 0x7) & 0x0F80) != rtl8225_chan[ch])
-		RF_WriteReg(dev, 0x7, rtl8225_chan[ch]);
-
-	mdelay(1);
-
-	force_pci_posting(dev);
-	mdelay(10);
-}
-
-static void rtl8225_host_pci_init(struct net_device *dev)
-{
-	write_nic_word(dev, RFPinsOutput, 0x480);
-
-	rtl8185_rf_pins_enable(dev);
-
-	write_nic_word(dev, RFPinsSelect, 0x88 | SW_CONTROL_GPIO);
-
-	write_nic_byte(dev, GP_ENABLE, 0);
-
-	force_pci_posting(dev);
-	mdelay(200);
-
-	/* bit 6 is for RF on/off detection */
-	write_nic_word(dev, GP_ENABLE, 0xff & (~(1 << 6)));
-}
-
-void rtl8225z2_rf_init(struct net_device *dev)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	int i;
-	short channel = 1;
-	u16 brsr;
-	u32 data;
-
-	priv->chan = channel;
-
-	rtl8225_host_pci_init(dev);
-
-	write_nic_dword(dev, RF_TIMING, 0x000a8008);
-
-	brsr = read_nic_word(dev, BRSR);
-
-	write_nic_word(dev, BRSR, 0xffff);
-
-	write_nic_dword(dev, RF_PARA, 0x100044);
-
-	rtl8180_set_mode(dev, EPROM_CMD_CONFIG);
-	write_nic_byte(dev, CONFIG3, 0x44);
-	rtl8180_set_mode(dev, EPROM_CMD_NORMAL);
-
-	rtl8185_rf_pins_enable(dev);
-
-	write_rtl8225(dev, 0x0, 0x2bf); mdelay(1);
-	write_rtl8225(dev, 0x1, 0xee0); mdelay(1);
-	write_rtl8225(dev, 0x2, 0x44d); mdelay(1);
-	write_rtl8225(dev, 0x3, 0x441); mdelay(1);
-	write_rtl8225(dev, 0x4, 0x8c3); mdelay(1);
-	write_rtl8225(dev, 0x5, 0xc72); mdelay(1);
-	write_rtl8225(dev, 0x6, 0xe6);  mdelay(1);
-	write_rtl8225(dev, 0x7, rtl8225_chan[channel]);  mdelay(1);
-	write_rtl8225(dev, 0x8, 0x3f);  mdelay(1);
-	write_rtl8225(dev, 0x9, 0x335); mdelay(1);
-	write_rtl8225(dev, 0xa, 0x9d4); mdelay(1);
-	write_rtl8225(dev, 0xb, 0x7bb); mdelay(1);
-	write_rtl8225(dev, 0xc, 0x850); mdelay(1);
-	write_rtl8225(dev, 0xd, 0xcdf); mdelay(1);
-	write_rtl8225(dev, 0xe, 0x2b);  mdelay(1);
-	write_rtl8225(dev, 0xf, 0x114);
-
-	mdelay(100);
-
-	write_rtl8225(dev, 0x0, 0x1b7);
-
-	for (i = 0; i < ARRAY_SIZE(rtl8225z2_rxgain); i++) {
-		write_rtl8225(dev, 0x1, i + 1);
-		write_rtl8225(dev, 0x2, rtl8225z2_rxgain[i]);
-	}
-
-	write_rtl8225(dev, 0x3, 0x80);
-	write_rtl8225(dev, 0x5, 0x4);
-
-	write_rtl8225(dev, 0x0, 0xb7);
-
-	write_rtl8225(dev, 0x2, 0xc4d);
-
-	/* FIXME!! rtl8187 we have to check if calibrarion
-	 * is successful and eventually cal. again (repeat
-	 * the two write on reg 2)
-	 */
-	data = read_rtl8225(dev, 6);
-	if (!(data & 0x00000080)) {
-		write_rtl8225(dev, 0x02, 0x0c4d);
-		force_pci_posting(dev); mdelay(200);
-		write_rtl8225(dev, 0x02, 0x044d);
-		force_pci_posting(dev); mdelay(100);
-		data = read_rtl8225(dev, 6);
-		if (!(data & 0x00000080))
-			DMESGW("RF Calibration Failed!!!!\n");
-	}
-
-	mdelay(200);
-
-	write_rtl8225(dev, 0x0, 0x2bf);
-
-	for (i = 0; i < ARRAY_SIZE(rtl8225_agc); i++) {
-		write_phy_ofdm(dev, 0xb, rtl8225_agc[i]);
-		mdelay(1);
-
-		/* enable writing AGC table */
-		write_phy_ofdm(dev, 0xa, i + 0x80);
-		mdelay(1);
-	}
-
-	force_pci_posting(dev);
-	mdelay(1);
-
-	write_phy_ofdm(dev, 0x00, 0x01); mdelay(1);
-	write_phy_ofdm(dev, 0x01, 0x02); mdelay(1);
-	write_phy_ofdm(dev, 0x02, 0x62); mdelay(1);
-	write_phy_ofdm(dev, 0x03, 0x00); mdelay(1);
-	write_phy_ofdm(dev, 0x04, 0x00); mdelay(1);
-	write_phy_ofdm(dev, 0x05, 0x00); mdelay(1);
-	write_phy_ofdm(dev, 0x06, 0x40); mdelay(1);
-	write_phy_ofdm(dev, 0x07, 0x00); mdelay(1);
-	write_phy_ofdm(dev, 0x08, 0x40); mdelay(1);
-	write_phy_ofdm(dev, 0x09, 0xfe); mdelay(1);
-	write_phy_ofdm(dev, 0x0a, 0x08); mdelay(1);
-	write_phy_ofdm(dev, 0x0b, 0x80); mdelay(1);
-	write_phy_ofdm(dev, 0x0c, 0x01); mdelay(1);
-	write_phy_ofdm(dev, 0x0d, 0x43);
-	write_phy_ofdm(dev, 0x0e, 0xd3); mdelay(1);
-	write_phy_ofdm(dev, 0x0f, 0x38); mdelay(1);
-	write_phy_ofdm(dev, 0x10, 0x84); mdelay(1);
-	write_phy_ofdm(dev, 0x11, 0x07); mdelay(1);
-	write_phy_ofdm(dev, 0x12, 0x20); mdelay(1);
-	write_phy_ofdm(dev, 0x13, 0x20); mdelay(1);
-	write_phy_ofdm(dev, 0x14, 0x00); mdelay(1);
-	write_phy_ofdm(dev, 0x15, 0x40); mdelay(1);
-	write_phy_ofdm(dev, 0x16, 0x00); mdelay(1);
-	write_phy_ofdm(dev, 0x17, 0x40); mdelay(1);
-	write_phy_ofdm(dev, 0x18, 0xef); mdelay(1);
-	write_phy_ofdm(dev, 0x19, 0x19); mdelay(1);
-	write_phy_ofdm(dev, 0x1a, 0x20); mdelay(1);
-	write_phy_ofdm(dev, 0x1b, 0x15); mdelay(1);
-	write_phy_ofdm(dev, 0x1c, 0x04); mdelay(1);
-	write_phy_ofdm(dev, 0x1d, 0xc5); mdelay(1);
-	write_phy_ofdm(dev, 0x1e, 0x95); mdelay(1);
-	write_phy_ofdm(dev, 0x1f, 0x75); mdelay(1);
-	write_phy_ofdm(dev, 0x20, 0x1f); mdelay(1);
-	write_phy_ofdm(dev, 0x21, 0x17); mdelay(1);
-	write_phy_ofdm(dev, 0x22, 0x16); mdelay(1);
-	write_phy_ofdm(dev, 0x23, 0x80); mdelay(1); /* FIXME maybe not needed */
-	write_phy_ofdm(dev, 0x24, 0x46); mdelay(1);
-	write_phy_ofdm(dev, 0x25, 0x00); mdelay(1);
-	write_phy_ofdm(dev, 0x26, 0x90); mdelay(1);
-	write_phy_ofdm(dev, 0x27, 0x88); mdelay(1);
-
-	rtl8225z2_set_gain(dev, 4);
-
-	write_phy_cck(dev, 0x0, 0x98); mdelay(1);
-	write_phy_cck(dev, 0x3, 0x20); mdelay(1);
-	write_phy_cck(dev, 0x4, 0x7e); mdelay(1);
-	write_phy_cck(dev, 0x5, 0x12); mdelay(1);
-	write_phy_cck(dev, 0x6, 0xfc); mdelay(1);
-	write_phy_cck(dev, 0x7, 0x78); mdelay(1);
-	write_phy_cck(dev, 0x8, 0x2e); mdelay(1);
-	write_phy_cck(dev, 0x10, 0x93); mdelay(1);
-	write_phy_cck(dev, 0x11, 0x88); mdelay(1);
-	write_phy_cck(dev, 0x12, 0x47); mdelay(1);
-	write_phy_cck(dev, 0x13, 0xd0);
-	write_phy_cck(dev, 0x19, 0x00);
-	write_phy_cck(dev, 0x1a, 0xa0);
-	write_phy_cck(dev, 0x1b, 0x08);
-	write_phy_cck(dev, 0x40, 0x86); /* CCK Carrier Sense Threshold */
-	write_phy_cck(dev, 0x41, 0x8d); mdelay(1);
-	write_phy_cck(dev, 0x42, 0x15); mdelay(1);
-	write_phy_cck(dev, 0x43, 0x18); mdelay(1);
-	write_phy_cck(dev, 0x44, 0x36); mdelay(1);
-	write_phy_cck(dev, 0x45, 0x35); mdelay(1);
-	write_phy_cck(dev, 0x46, 0x2e); mdelay(1);
-	write_phy_cck(dev, 0x47, 0x25); mdelay(1);
-	write_phy_cck(dev, 0x48, 0x1c); mdelay(1);
-	write_phy_cck(dev, 0x49, 0x12); mdelay(1);
-	write_phy_cck(dev, 0x4a, 0x09); mdelay(1);
-	write_phy_cck(dev, 0x4b, 0x04); mdelay(1);
-	write_phy_cck(dev, 0x4c, 0x05); mdelay(1);
-
-	write_nic_byte(dev, 0x5b, 0x0d); mdelay(1);
-
-	rtl8225z2_SetTXPowerLevel(dev, channel);
-
-	/* RX antenna default to A */
-	write_phy_cck(dev, 0x11, 0x9b); mdelay(1);		/* B: 0xDB */
-	write_phy_ofdm(dev, 0x26, 0x90); mdelay(1);		/* B: 0x10 */
-
-	rtl8185_tx_antenna(dev, 0x03);				/* B: 0x00 */
-
-	/* switch to high-speed 3-wire
-	 * last digit. 2 for both cck and ofdm
-	 */
-	write_nic_dword(dev, 0x94, 0x15c00002);
-	rtl8185_rf_pins_enable(dev);
-
-	rtl8225z2_rf_set_chan(dev, priv->chan);
-}
-
-#define MAX_DOZE_WAITING_TIMES_85B		20
-#define MAX_POLLING_24F_TIMES_87SE		10
-#define LPS_MAX_SLEEP_WAITING_TIMES_87SE	5
-
-bool SetZebraRFPowerState8185(struct net_device *dev,
-			      enum rt_rf_power_state eRFPowerState)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	u8			btCR9346, btConfig3;
-	bool bActionAllowed = true, bTurnOffBB = true;
-	u8			u1bTmp;
-	int			i;
-	bool		bResult = true;
-	u8			QueueID;
-
-	if (priv->SetRFPowerStateInProgress == true)
-		return false;
-
-	priv->SetRFPowerStateInProgress = true;
-
-	btCR9346 = read_nic_byte(dev, CR9346);
-	write_nic_byte(dev, CR9346, (btCR9346 | 0xC0));
-
-	btConfig3 = read_nic_byte(dev, CONFIG3);
-	write_nic_byte(dev, CONFIG3, (btConfig3 | CONFIG3_PARM_En));
-
-	switch (eRFPowerState) {
-	case RF_ON:
-		write_nic_word(dev, 0x37C, 0x00EC);
-
-		/* turn on AFE */
-		write_nic_byte(dev, 0x54, 0x00);
-		write_nic_byte(dev, 0x62, 0x00);
-
-		/* turn on RF */
-		RF_WriteReg(dev, 0x0, 0x009f); udelay(500);
-		RF_WriteReg(dev, 0x4, 0x0972); udelay(500);
-
-		/* turn on RF again */
-		RF_WriteReg(dev, 0x0, 0x009f); udelay(500);
-		RF_WriteReg(dev, 0x4, 0x0972); udelay(500);
-
-		/* turn on BB */
-		write_phy_ofdm(dev, 0x10, 0x40);
-		write_phy_ofdm(dev, 0x12, 0x40);
-
-		/* Avoid power down at init time. */
-		write_nic_byte(dev, CONFIG4, priv->RFProgType);
-
-		u1bTmp = read_nic_byte(dev, 0x24E);
-		write_nic_byte(dev, 0x24E, (u1bTmp & (~(BIT5 | BIT6))));
-		break;
-	case RF_SLEEP:
-		for (QueueID = 0, i = 0; QueueID < 6;) {
-			if (get_curr_tx_free_desc(dev, QueueID) ==
-							priv->txringcount) {
-				QueueID++;
-				continue;
-			} else {
-				priv->TxPollingTimes++;
-				if (priv->TxPollingTimes >=
-					LPS_MAX_SLEEP_WAITING_TIMES_87SE) {
-					bActionAllowed = false;
-					break;
-				} else
-					udelay(10);
-			}
-		}
-
-		if (bActionAllowed) {
-			/* turn off BB RXIQ matrix to cut off rx signal */
-			write_phy_ofdm(dev, 0x10, 0x00);
-			write_phy_ofdm(dev, 0x12, 0x00);
-
-			/* turn off RF */
-			RF_WriteReg(dev, 0x4, 0x0000);
-			RF_WriteReg(dev, 0x0, 0x0000);
-
-			/* turn off AFE except PLL */
-			write_nic_byte(dev, 0x62, 0xff);
-			write_nic_byte(dev, 0x54, 0xec);
-
-			mdelay(1);
-
-			{
-				int i = 0;
-				while (true) {
-					u8 tmp24F = read_nic_byte(dev, 0x24f);
-
-					if ((tmp24F == 0x01) ||
-							(tmp24F == 0x09)) {
-						bTurnOffBB = true;
-						break;
-					} else {
-						udelay(10);
-						i++;
-						priv->TxPollingTimes++;
-
-						if (priv->TxPollingTimes >= LPS_MAX_SLEEP_WAITING_TIMES_87SE) {
-							bTurnOffBB = false;
-							break;
-						} else
-							udelay(10);
-					}
-				}
-			}
-
-			if (bTurnOffBB) {
-				/* turn off BB */
-				u1bTmp = read_nic_byte(dev, 0x24E);
-				write_nic_byte(dev, 0x24E,
-						(u1bTmp | BIT5 | BIT6));
-
-				/* turn off AFE PLL */
-				write_nic_byte(dev, 0x54, 0xFC);
-				write_nic_word(dev, 0x37C, 0x00FC);
-			}
-		}
-		break;
-	case RF_OFF:
-		for (QueueID = 0, i = 0; QueueID < 6;) {
-			if (get_curr_tx_free_desc(dev, QueueID) ==
-					priv->txringcount) {
-				QueueID++;
-				continue;
-			} else {
-				udelay(10);
-				i++;
-			}
-
-			if (i >= MAX_DOZE_WAITING_TIMES_85B)
-				break;
-		}
-
-		/* turn off BB RXIQ matrix to cut off rx signal */
-		write_phy_ofdm(dev, 0x10, 0x00);
-		write_phy_ofdm(dev, 0x12, 0x00);
-
-		/* turn off RF */
-		RF_WriteReg(dev, 0x4, 0x0000);
-		RF_WriteReg(dev, 0x0, 0x0000);
-
-		/* turn off AFE except PLL */
-		write_nic_byte(dev, 0x62, 0xff);
-		write_nic_byte(dev, 0x54, 0xec);
-
-		mdelay(1);
-
-		{
-			int i = 0;
-
-			while (true) {
-				u8 tmp24F = read_nic_byte(dev, 0x24f);
-
-				if ((tmp24F == 0x01) || (tmp24F == 0x09)) {
-					bTurnOffBB = true;
-					break;
-				} else {
-					bTurnOffBB = false;
-					udelay(10);
-					i++;
-				}
-
-				if (i > MAX_POLLING_24F_TIMES_87SE)
-					break;
-			}
-		}
-
-		if (bTurnOffBB) {
-			/* turn off BB */
-			u1bTmp = read_nic_byte(dev, 0x24E);
-			write_nic_byte(dev, 0x24E, (u1bTmp | BIT5 | BIT6));
-
-			/* turn off AFE PLL (80M) */
-			write_nic_byte(dev, 0x54, 0xFC);
-			write_nic_word(dev, 0x37C, 0x00FC);
-		}
-		break;
-	}
-
-	btConfig3 &= ~(CONFIG3_PARM_En);
-	write_nic_byte(dev, CONFIG3, btConfig3);
-
-	btCR9346 &= ~(0xC0);
-	write_nic_byte(dev, CR9346, btCR9346);
-
-	if (bResult && bActionAllowed)
-		priv->eRFPowerState = eRFPowerState;
-
-	priv->SetRFPowerStateInProgress = false;
-
-	return bResult && bActionAllowed;
-}
-
-void rtl8225z4_rf_sleep(struct net_device *dev)
-{
-	MgntActSet_RF_State(dev, RF_SLEEP, RF_CHANGE_BY_PS);
-}
-
-void rtl8225z4_rf_wakeup(struct net_device *dev)
-{
-	MgntActSet_RF_State(dev, RF_ON, RF_CHANGE_BY_PS);
-}
diff --git a/drivers/staging/rtl8187se/r8180_wx.c b/drivers/staging/rtl8187se/r8180_wx.c
deleted file mode 100644
index b55249170f1..00000000000
--- a/drivers/staging/rtl8187se/r8180_wx.c
+++ /dev/null
@@ -1,1409 +0,0 @@
-/*
-	This file contains wireless extension handlers.
-
-	This is part of rtl8180 OpenSource driver.
-	Copyright (C) Andrea Merello 2004-2005  <andrea.merello@gmail.com>
-	Released under the terms of GPL (General Public Licence)
-
-	Parts of this driver are based on the GPL part
-	of the official realtek driver.
-
-	Parts of this driver are based on the rtl8180 driver skeleton
-	from Patric Schenke & Andres Salomon.
-
-	Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver.
-
-	We want to thanks the Authors of those projects and the Ndiswrapper
-	project Authors.
-*/
-
-
-#include "r8180.h"
-#include "r8180_hw.h"
-
-#include <net/iw_handler.h>
-#include "ieee80211/dot11d.h"
-
-static u32 rtl8180_rates[] = {1000000, 2000000, 5500000, 11000000,
-	6000000, 9000000, 12000000, 18000000, 24000000, 36000000, 48000000, 54000000};
-
-#define RATE_COUNT ARRAY_SIZE(rtl8180_rates)
-
-static struct rtl8187se_channel_list default_channel_plan[] = {
-	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 36, 40, 44, 48, 52, 56, 60, 64}, 19},		/* FCC */
-	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11}, 11},						/* IC */
-	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 36, 40, 44, 48, 52, 56, 60, 64}, 21},	/* ETSI	*/
-	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 36, 40, 44, 48, 52, 56, 60, 64}, 21},	/* Spain. Change to ETSI. */
-	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 36, 40, 44, 48, 52, 56, 60, 64}, 21},	/* France. Change to ETSI. */
-	{{14, 36, 40, 44, 48, 52, 56, 60, 64}, 9},						/* MKK */
-	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 36, 40, 44, 48, 52, 56, 60, 64}, 22},	/* MKK1	*/
-	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 36, 40, 44, 48, 52, 56, 60, 64}, 21},	/* Israel */
-	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 34, 38, 42, 46}, 17},			/* For 11a , TELEC */
-	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14}, 14}					/* For Global Domain. 1-11:active scan, 12-14 passive scan.*/	/* +YJ, 080626 */
-};
-static int r8180_wx_get_freq(struct net_device *dev,
-			     struct iw_request_info *a,
-			     union iwreq_data *wrqu, char *b)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-	return ieee80211_wx_get_freq(priv->ieee80211, a, wrqu, b);
-}
-
-
-static int r8180_wx_set_key(struct net_device *dev,
-			    struct iw_request_info *info,
-			    union iwreq_data *wrqu, char *key)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	struct iw_point *erq = &(wrqu->encoding);
-
-	if (priv->ieee80211->bHwRadioOff)
-		return 0;
-
-	if (erq->length > 0) {
-		u32 *tkey = (u32 *) key;
-		priv->key0[0] = tkey[0];
-		priv->key0[1] = tkey[1];
-		priv->key0[2] = tkey[2];
-		priv->key0[3] = tkey[3] & 0xff;
-		DMESG("Setting wep key to %x %x %x %x",
-		      tkey[0], tkey[1], tkey[2], tkey[3]);
-		rtl8180_set_hw_wep(dev);
-	}
-	return 0;
-}
-
-
-static int r8180_wx_set_beaconinterval(struct net_device *dev,
-				       struct iw_request_info *aa,
-				       union iwreq_data *wrqu, char *b)
-{
-	int *parms = (int *)b;
-	int bi = parms[0];
-
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-	if (priv->ieee80211->bHwRadioOff)
-		return 0;
-
-	down(&priv->wx_sem);
-	DMESG("setting beacon interval to %x", bi);
-
-	priv->ieee80211->current_network.beacon_interval = bi;
-	rtl8180_commit(dev);
-	up(&priv->wx_sem);
-
-	return 0;
-}
-
-
-
-static int r8180_wx_get_mode(struct net_device *dev, struct iw_request_info *a,
-			     union iwreq_data *wrqu, char *b)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	return ieee80211_wx_get_mode(priv->ieee80211, a, wrqu, b);
-}
-
-
-
-static int r8180_wx_get_rate(struct net_device *dev,
-			     struct iw_request_info *info,
-			     union iwreq_data *wrqu, char *extra)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	return ieee80211_wx_get_rate(priv->ieee80211, info, wrqu, extra);
-}
-
-
-
-static int r8180_wx_set_rate(struct net_device *dev,
-			     struct iw_request_info *info,
-			     union iwreq_data *wrqu, char *extra)
-{
-	int ret;
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-
-	if (priv->ieee80211->bHwRadioOff)
-		return 0;
-
-	down(&priv->wx_sem);
-
-	ret = ieee80211_wx_set_rate(priv->ieee80211, info, wrqu, extra);
-
-	up(&priv->wx_sem);
-
-	return ret;
-}
-
-
-static int r8180_wx_set_crcmon(struct net_device *dev,
-			       struct iw_request_info *info,
-			       union iwreq_data *wrqu, char *extra)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	int *parms = (int *)extra;
-	int enable = (parms[0] > 0);
-	short prev = priv->crcmon;
-
-
-	if (priv->ieee80211->bHwRadioOff)
-		return 0;
-
-	down(&priv->wx_sem);
-
-	if (enable)
-		priv->crcmon = 1;
-	else
-		priv->crcmon = 0;
-
-	DMESG("bad CRC in monitor mode are %s",
-	      priv->crcmon ? "accepted" : "rejected");
-
-	if (prev != priv->crcmon && priv->up)	{
-		rtl8180_down(dev);
-		rtl8180_up(dev);
-	}
-
-	up(&priv->wx_sem);
-
-	return 0;
-}
-
-
-static int r8180_wx_set_mode(struct net_device *dev, struct iw_request_info *a,
-			     union iwreq_data *wrqu, char *b)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	int ret;
-
-
-	if (priv->ieee80211->bHwRadioOff)
-		return 0;
-
-	down(&priv->wx_sem);
-	if (priv->bInactivePs)	{
-		if (wrqu->mode == IW_MODE_ADHOC)
-			IPSLeave(dev);
-	}
-	ret = ieee80211_wx_set_mode(priv->ieee80211, a, wrqu, b);
-
-	up(&priv->wx_sem);
-	return ret;
-}
-
-/* YJ,add,080819,for hidden ap */
-struct  iw_range_with_scan_capa	{
-		/* Informative stuff (to choose between different interface) */
-
-		__u32		throughput; /* To give an idea... */
-
-		/* In theory this value should be the maximum benchmarked
-		 * TCP/IP throughput, because with most of these devices the
-		 * bit rate is meaningless (overhead an co) to estimate how
-		 * fast the connection will go and pick the fastest one.
-		 * I suggest people to play with Netperf or any benchmark...
-		 */
-
-		/* NWID (or domain id)	*/
-		__u32           min_nwid; /* Minimal NWID we are able to set */
-		__u32			max_nwid; /* Maximal NWID we are able to set */
-
-		/* Old Frequency (backward compat - moved lower ) */
-		__u16			old_num_channels;
-		__u8			old_num_frequency;
-
-		/* Scan capabilities */
-		__u8			scan_capa;
-};
-/* YJ,add,080819,for hidden ap */
-
-
-static int rtl8180_wx_get_range(struct net_device *dev,
-				struct iw_request_info *info,
-				union iwreq_data *wrqu, char *extra)
-{
-	struct iw_range *range = (struct iw_range *)extra;
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	u16 val;
-	int i;
-
-	wrqu->data.length = sizeof(*range);
-	memset(range, 0, sizeof(*range));
-
-	/* Let's try to keep this struct in the same order as in
-	 * linux/include/wireless.h
-	 */
-
-	/* TODO: See what values we can set, and remove the ones we can't
-	 * set, or fill them with some default data.
-	 */
-
-	/* ~5 Mb/s real (802.11b) */
-	range->throughput = 5 * 1000 * 1000;
-
-	/* TODO: Not used in 802.11b?	*/
-/*	range->min_nwid; */	/* Minimal NWID we are able to set */
-	/* TODO: Not used in 802.11b?	*/
-/*	range->max_nwid; */	/* Maximal NWID we are able to set */
-
-		/* Old Frequency (backward compat - moved lower ) */
-/*	range->old_num_channels; */
-/*	range->old_num_frequency; */
-/*	range->old_freq[6]; */ /* Filler to keep "version" at the same offset */
-	if (priv->rf_set_sens != NULL)
-		range->sensitivity = priv->max_sens;	/* signal level threshold range */
-
-	range->max_qual.qual = 100;
-	/* TODO: Find real max RSSI and stick here */
-	range->max_qual.level = 0;
-	range->max_qual.noise = -98;
-	range->max_qual.updated = 7; /* Updated all three */
-
-	range->avg_qual.qual = 92; /* > 8% missed beacons is 'bad' */
-	/* TODO: Find real 'good' to 'bad' threshold value for RSSI */
-	range->avg_qual.level = 20 + -98;
-	range->avg_qual.noise = 0;
-	range->avg_qual.updated = 7; /* Updated all three */
-
-	range->num_bitrates = RATE_COUNT;
-
-	for (i = 0; i < RATE_COUNT && i < IW_MAX_BITRATES; i++)
-		range->bitrate[i] = rtl8180_rates[i];
-
-	range->min_frag = MIN_FRAG_THRESHOLD;
-	range->max_frag = MAX_FRAG_THRESHOLD;
-
-	range->pm_capa = 0;
-
-	range->we_version_compiled = WIRELESS_EXT;
-	range->we_version_source = 16;
-
-		range->num_channels = 14;
-
-	for (i = 0, val = 0; i < 14; i++) {
-
-		/* Include only legal frequencies for some countries */
-		if ((GET_DOT11D_INFO(priv->ieee80211)->channel_map)[i+1]) {
-				range->freq[val].i = i + 1;
-			range->freq[val].m = ieee80211_wlan_frequencies[i] * 100000;
-			range->freq[val].e = 1;
-			val++;
-		} else {
-			/* FIXME: do we need to set anything for channels */
-			/* we don't use ? */
-		}
-
-		if (val == IW_MAX_FREQUENCIES)
-			break;
-	}
-
-	range->num_frequency = val;
-	range->enc_capa = IW_ENC_CAPA_WPA | IW_ENC_CAPA_WPA2 |
-						IW_ENC_CAPA_CIPHER_TKIP | IW_ENC_CAPA_CIPHER_CCMP;
-
-	return 0;
-}
-
-
-static int r8180_wx_set_scan(struct net_device *dev, struct iw_request_info *a,
-			     union iwreq_data *wrqu, char *b)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	int ret;
-	struct ieee80211_device *ieee = priv->ieee80211;
-
-
-	if (priv->ieee80211->bHwRadioOff)
-		return 0;
-
-	if (wrqu->data.flags & IW_SCAN_THIS_ESSID)	{
-		struct iw_scan_req *req = (struct iw_scan_req *)b;
-		if (req->essid_len)		{
-			ieee->current_network.ssid_len = req->essid_len;
-			memcpy(ieee->current_network.ssid, req->essid, req->essid_len);
-		}
-	}
-
-	down(&priv->wx_sem);
-	if (priv->up)	{
-		priv->ieee80211->actscanning = true;
-		if (priv->bInactivePs && (priv->ieee80211->state != IEEE80211_LINKED))	{
-			IPSLeave(dev);
-		ieee80211_softmac_ips_scan_syncro(priv->ieee80211);
-			ret = 0;
-		}	else	{
-			/* prevent scan in BusyTraffic */
-			/* FIXME: Need to consider last scan time */
-			if ((priv->link_detect.b_busy_traffic) && (true)) {
-				ret = 0;
-				printk("Now traffic is busy, please try later!\n");
-			}	else
-				/* prevent scan in BusyTraffic,end */
-				ret = ieee80211_wx_set_scan(priv->ieee80211, a, wrqu, b);
-		}
-	}	else
-			ret = -1;
-
-	up(&priv->wx_sem);
-
-	return ret;
-}
-
-
-static int r8180_wx_get_scan(struct net_device *dev, struct iw_request_info *a,
-			     union iwreq_data *wrqu, char *b)
-{
-
-	int ret;
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-	down(&priv->wx_sem);
-	if (priv->up)
-		ret = ieee80211_wx_get_scan(priv->ieee80211, a, wrqu, b);
-	else
-		ret = -1;
-
-	up(&priv->wx_sem);
-	return ret;
-}
-
-
-static int r8180_wx_set_essid(struct net_device *dev,
-			      struct iw_request_info *a,
-			      union iwreq_data *wrqu, char *b)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-	int ret;
-
-	if (priv->ieee80211->bHwRadioOff)
-		return 0;
-
-	down(&priv->wx_sem);
-	if (priv->bInactivePs)
-		IPSLeave(dev);
-
-	ret = ieee80211_wx_set_essid(priv->ieee80211, a, wrqu, b);
-
-	up(&priv->wx_sem);
-	return ret;
-}
-
-
-static int r8180_wx_get_essid(struct net_device *dev,
-			      struct iw_request_info *a,
-			      union iwreq_data *wrqu, char *b)
-{
-	int ret;
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-	down(&priv->wx_sem);
-
-	ret = ieee80211_wx_get_essid(priv->ieee80211, a, wrqu, b);
-
-	up(&priv->wx_sem);
-
-	return ret;
-}
-
-
-static int r8180_wx_set_freq(struct net_device *dev, struct iw_request_info *a,
-			     union iwreq_data *wrqu, char *b)
-{
-	int ret;
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-
-	if (priv->ieee80211->bHwRadioOff)
-		return 0;
-
-	down(&priv->wx_sem);
-
-	ret = ieee80211_wx_set_freq(priv->ieee80211, a, wrqu, b);
-
-	up(&priv->wx_sem);
-	return ret;
-}
-
-
-static int r8180_wx_get_name(struct net_device *dev,
-			     struct iw_request_info *info,
-			     union iwreq_data *wrqu, char *extra)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	return ieee80211_wx_get_name(priv->ieee80211, info, wrqu, extra);
-}
-
-static int r8180_wx_set_frag(struct net_device *dev,
-			     struct iw_request_info *info,
-			     union iwreq_data *wrqu, char *extra)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-	if (priv->ieee80211->bHwRadioOff)
-		return 0;
-
-	if (wrqu->frag.disabled)
-		priv->ieee80211->fts = DEFAULT_FRAG_THRESHOLD;
-	else {
-		if (wrqu->frag.value < MIN_FRAG_THRESHOLD ||
-		    wrqu->frag.value > MAX_FRAG_THRESHOLD)
-			return -EINVAL;
-
-		priv->ieee80211->fts = wrqu->frag.value & ~0x1;
-	}
-
-	return 0;
-}
-
-
-static int r8180_wx_get_frag(struct net_device *dev,
-			     struct iw_request_info *info,
-			     union iwreq_data *wrqu, char *extra)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-	wrqu->frag.value = priv->ieee80211->fts;
-	wrqu->frag.fixed = 0;	/* no auto select */
-	wrqu->frag.disabled = (wrqu->frag.value == DEFAULT_FRAG_THRESHOLD);
-
-	return 0;
-}
-
-
-static int r8180_wx_set_wap(struct net_device *dev,
-			    struct iw_request_info *info,
-			    union iwreq_data *awrq, char *extra)
-{
-	int ret;
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-	if (priv->ieee80211->bHwRadioOff)
-		return 0;
-
-	down(&priv->wx_sem);
-
-	ret = ieee80211_wx_set_wap(priv->ieee80211, info, awrq, extra);
-
-	up(&priv->wx_sem);
-	return ret;
-
-}
-
-
-static int r8180_wx_get_wap(struct net_device *dev,
-			    struct iw_request_info *info,
-			    union iwreq_data *wrqu, char *extra)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-	return ieee80211_wx_get_wap(priv->ieee80211, info, wrqu, extra);
-}
-
-
-static int r8180_wx_set_enc(struct net_device *dev,
-			    struct iw_request_info *info,
-			    union iwreq_data *wrqu, char *key)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	int ret;
-
-	if (priv->ieee80211->bHwRadioOff)
-		return 0;
-
-
-	down(&priv->wx_sem);
-
-	if (priv->hw_wep)
-		ret = r8180_wx_set_key(dev, info, wrqu, key);
-	else	{
-		DMESG("Setting SW wep key");
-		ret = ieee80211_wx_set_encode(priv->ieee80211, info, wrqu, key);
-	}
-
-	up(&priv->wx_sem);
-	return ret;
-}
-
-
-static int r8180_wx_get_enc(struct net_device *dev,
-			    struct iw_request_info *info,
-			    union iwreq_data *wrqu, char *key)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-	return ieee80211_wx_get_encode(priv->ieee80211, info, wrqu, key);
-}
-
-
-static int r8180_wx_set_scan_type(struct net_device *dev,
-				  struct iw_request_info *aa,
-				  union	iwreq_data *wrqu, char *p)
-{
-
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	int *parms = (int *)p;
-	int mode = parms[0];
-
-	if (priv->ieee80211->bHwRadioOff)
-		return 0;
-
-	priv->ieee80211->active_scan = mode;
-
-	return 1;
-}
-
-static int r8180_wx_set_retry(struct net_device *dev,
-			      struct iw_request_info *info,
-			      union iwreq_data *wrqu, char *extra)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	int err = 0;
-
-	if (priv->ieee80211->bHwRadioOff)
-		return 0;
-
-	down(&priv->wx_sem);
-
-	if (wrqu->retry.flags & IW_RETRY_LIFETIME ||
-	    wrqu->retry.disabled)	{
-		err = -EINVAL;
-		goto exit;
-	}
-	if (!(wrqu->retry.flags & IW_RETRY_LIMIT))	{
-		err = -EINVAL;
-		goto exit;
-	}
-
-	if (wrqu->retry.value > R8180_MAX_RETRY)	{
-		err = -EINVAL;
-		goto exit;
-	}
-	if (wrqu->retry.flags & IW_RETRY_MAX) {
-		priv->retry_rts = wrqu->retry.value;
-		DMESG("Setting retry for RTS/CTS data to %d", wrqu->retry.value);
-
-	}	else {
-		priv->retry_data = wrqu->retry.value;
-		DMESG("Setting retry for non RTS/CTS data to %d", wrqu->retry.value);
-	}
-
-	/* FIXME !
-	 * We might try to write directly the TX config register
-	 * or to restart just the (R)TX process.
-	 * I'm unsure if whole reset is really needed
-	 */
-
-	rtl8180_commit(dev);
-exit:
-	up(&priv->wx_sem);
-
-	return err;
-}
-
-static int r8180_wx_get_retry(struct net_device *dev,
-			      struct iw_request_info *info,
-			      union iwreq_data *wrqu, char *extra)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-
-	wrqu->retry.disabled = 0; /* can't be disabled */
-
-	if ((wrqu->retry.flags & IW_RETRY_TYPE) ==
-	    IW_RETRY_LIFETIME)
-		return -EINVAL;
-
-	if (wrqu->retry.flags & IW_RETRY_MAX) {
-		wrqu->retry.flags = IW_RETRY_LIMIT | IW_RETRY_MAX;
-		wrqu->retry.value = priv->retry_rts;
-	} else {
-		wrqu->retry.flags = IW_RETRY_LIMIT | IW_RETRY_MIN;
-		wrqu->retry.value = priv->retry_data;
-	}
-
-	return 0;
-}
-
-static int r8180_wx_get_sens(struct net_device *dev,
-			     struct iw_request_info *info,
-			     union iwreq_data *wrqu, char *extra)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	if (priv->rf_set_sens == NULL)
-		return -1; /* we have not this support for this radio */
-	wrqu->sens.value = priv->sens;
-	return 0;
-}
-
-
-static int r8180_wx_set_sens(struct net_device *dev,
-			     struct iw_request_info *info,
-			     union iwreq_data *wrqu, char *extra)
-{
-
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-	short err = 0;
-
-	if (priv->ieee80211->bHwRadioOff)
-		return 0;
-
-	down(&priv->wx_sem);
-	if (priv->rf_set_sens == NULL) {
-		err = -1; /* we have not this support for this radio */
-		goto exit;
-	}
-	if (priv->rf_set_sens(dev, wrqu->sens.value) == 0)
-		priv->sens = wrqu->sens.value;
-	else
-		err = -EINVAL;
-
-exit:
-	up(&priv->wx_sem);
-
-	return err;
-}
-
-
-static int r8180_wx_set_rawtx(struct net_device *dev,
-			      struct iw_request_info *info,
-			      union iwreq_data *wrqu, char *extra)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	int ret;
-
-	if (priv->ieee80211->bHwRadioOff)
-		return 0;
-
-	down(&priv->wx_sem);
-
-	ret = ieee80211_wx_set_rawtx(priv->ieee80211, info, wrqu, extra);
-
-	up(&priv->wx_sem);
-
-	return ret;
-
-}
-
-static int r8180_wx_get_power(struct net_device *dev,
-			      struct iw_request_info *info,
-			      union iwreq_data *wrqu, char *extra)
-{
-	int ret;
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-	down(&priv->wx_sem);
-
-	ret = ieee80211_wx_get_power(priv->ieee80211, info, wrqu, extra);
-
-	up(&priv->wx_sem);
-
-	return ret;
-}
-
-static int r8180_wx_set_power(struct net_device *dev,
-			      struct iw_request_info *info,
-			      union iwreq_data *wrqu, char *extra)
-{
-	int ret;
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-
-	if (priv->ieee80211->bHwRadioOff)
-		return 0;
-
-	down(&priv->wx_sem);
-	printk("=>>>>>>>>>>=============================>set power:%d, %d!\n", wrqu->power.disabled, wrqu->power.flags);
-	if (wrqu->power.disabled == 0) {
-		wrqu->power.flags |= IW_POWER_ALL_R;
-		wrqu->power.flags |= IW_POWER_TIMEOUT;
-		wrqu->power.value = 1000;
-	}
-
-	ret = ieee80211_wx_set_power(priv->ieee80211, info, wrqu, extra);
-
-	up(&priv->wx_sem);
-
-	return ret;
-}
-
-static int r8180_wx_set_rts(struct net_device *dev,
-			    struct iw_request_info *info,
-			    union iwreq_data *wrqu, char *extra)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-
-	if (priv->ieee80211->bHwRadioOff)
-		return 0;
-
-	if (wrqu->rts.disabled)
-		priv->rts = DEFAULT_RTS_THRESHOLD;
-	else {
-		if (wrqu->rts.value < MIN_RTS_THRESHOLD ||
-		    wrqu->rts.value > MAX_RTS_THRESHOLD)
-			return -EINVAL;
-
-		priv->rts = wrqu->rts.value;
-	}
-
-	return 0;
-}
-static int r8180_wx_get_rts(struct net_device *dev,
-			    struct iw_request_info *info,
-			    union iwreq_data *wrqu, char *extra)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-
-
-	wrqu->rts.value = priv->rts;
-	wrqu->rts.fixed = 0;	/* no auto select */
-	wrqu->rts.disabled = (wrqu->rts.value == 0);
-
-	return 0;
-}
-static int dummy(struct net_device *dev, struct iw_request_info *a,
-		 union iwreq_data *wrqu, char *b)
-{
-	return -1;
-}
-
-static int r8180_wx_get_iwmode(struct net_device *dev,
-			       struct iw_request_info *info,
-			       union iwreq_data *wrqu, char *extra)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	struct ieee80211_device *ieee;
-	int ret = 0;
-
-
-
-	down(&priv->wx_sem);
-
-	ieee = priv->ieee80211;
-
-	strcpy(extra, "802.11");
-	if (ieee->modulation & IEEE80211_CCK_MODULATION) {
-		strcat(extra, "b");
-		if (ieee->modulation & IEEE80211_OFDM_MODULATION)
-			strcat(extra, "/g");
-	} else if (ieee->modulation & IEEE80211_OFDM_MODULATION)
-		strcat(extra, "g");
-
-	up(&priv->wx_sem);
-
-	return ret;
-}
-static int r8180_wx_set_iwmode(struct net_device *dev,
-			       struct iw_request_info *info,
-			       union iwreq_data *wrqu, char *extra)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	struct ieee80211_device *ieee = priv->ieee80211;
-	int *param = (int *)extra;
-	int ret = 0;
-	int modulation = 0, mode = 0;
-
-
-	if (priv->ieee80211->bHwRadioOff)
-		return 0;
-
-	down(&priv->wx_sem);
-
-	if (*param == 1) {
-		modulation |= IEEE80211_CCK_MODULATION;
-		mode = IEEE_B;
-	printk(KERN_INFO "B mode!\n");
-	} else if (*param == 2) {
-		modulation |= IEEE80211_OFDM_MODULATION;
-		mode = IEEE_G;
-	printk(KERN_INFO "G mode!\n");
-	} else if (*param == 3) {
-		modulation |= IEEE80211_CCK_MODULATION;
-		modulation |= IEEE80211_OFDM_MODULATION;
-		mode = IEEE_B|IEEE_G;
-	printk(KERN_INFO "B/G mode!\n");
-	}
-
-	if (ieee->proto_started) {
-		ieee80211_stop_protocol(ieee);
-		ieee->mode = mode;
-		ieee->modulation = modulation;
-		ieee80211_start_protocol(ieee);
-	} else {
-		ieee->mode = mode;
-		ieee->modulation = modulation;
-	}
-
-	up(&priv->wx_sem);
-
-	return ret;
-}
-static int r8180_wx_get_preamble(struct net_device *dev,
-				 struct iw_request_info *info,
-				 union iwreq_data *wrqu, char *extra)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-
-
-	down(&priv->wx_sem);
-
-
-
-	*extra = (char) priv->plcp_preamble_mode;	/* 0:auto 1:short 2:long */
-	up(&priv->wx_sem);
-
-	return 0;
-}
-static int r8180_wx_set_preamble(struct net_device *dev,
-				 struct iw_request_info *info,
-				 union iwreq_data *wrqu, char *extra)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	int ret = 0;
-
-
-	if (priv->ieee80211->bHwRadioOff)
-		return 0;
-
-	down(&priv->wx_sem);
-	if (*extra < 0 || *extra > 2)
-		ret = -1;
-	else
-		priv->plcp_preamble_mode = *((short *)extra);
-
-
-
-	up(&priv->wx_sem);
-
-	return ret;
-}
-static int r8180_wx_get_siglevel(struct net_device *dev,
-				 struct iw_request_info *info,
-				 union iwreq_data *wrqu, char *extra)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	int ret = 0;
-
-
-
-	down(&priv->wx_sem);
-	/* Modify by hikaru 6.5 */
-	*((int *)extra) = priv->wstats.qual.level;/*for interface test ,it should be the priv->wstats.qual.level; */
-
-
-
-	up(&priv->wx_sem);
-
-	return ret;
-}
-static int r8180_wx_get_sigqual(struct net_device *dev,
-				struct iw_request_info *info,
-				union iwreq_data *wrqu, char *extra)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	int ret = 0;
-
-
-
-	down(&priv->wx_sem);
-	/* Modify by hikaru 6.5	*/
-	*((int *)extra) = priv->wstats.qual.qual;/* for interface test ,it should be the priv->wstats.qual.qual; */
-
-
-
-	up(&priv->wx_sem);
-
-	return ret;
-}
-static int r8180_wx_reset_stats(struct net_device *dev,
-				struct iw_request_info *info,
-				union iwreq_data *wrqu, char *extra)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	down(&priv->wx_sem);
-
-	priv->stats.txrdu = 0;
-	priv->stats.rxrdu = 0;
-	priv->stats.rxnolast = 0;
-	priv->stats.rxnodata = 0;
-	priv->stats.rxnopointer = 0;
-	priv->stats.txnperr = 0;
-	priv->stats.txresumed = 0;
-	priv->stats.rxerr = 0;
-	priv->stats.rxoverflow = 0;
-	priv->stats.rxint = 0;
-
-	priv->stats.txnpokint = 0;
-	priv->stats.txhpokint = 0;
-	priv->stats.txhperr = 0;
-	priv->stats.ints = 0;
-	priv->stats.shints = 0;
-	priv->stats.txoverflow = 0;
-	priv->stats.rxdmafail = 0;
-	priv->stats.txbeacon = 0;
-	priv->stats.txbeaconerr = 0;
-	priv->stats.txlpokint = 0;
-	priv->stats.txlperr = 0;
-	priv->stats.txretry = 0;/* 20060601 */
-	priv->stats.rxcrcerrmin = 0 ;
-	priv->stats.rxcrcerrmid = 0;
-	priv->stats.rxcrcerrmax = 0;
-	priv->stats.rxicverr = 0;
-
-	up(&priv->wx_sem);
-
-	return 0;
-
-}
-static int r8180_wx_radio_on(struct net_device *dev,
-			     struct iw_request_info *info,
-			     union iwreq_data *wrqu, char *extra)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-	if (priv->ieee80211->bHwRadioOff)
-		return 0;
-
-
-	down(&priv->wx_sem);
-	priv->rf_wakeup(dev);
-
-	up(&priv->wx_sem);
-
-	return 0;
-
-}
-
-static int r8180_wx_radio_off(struct net_device *dev,
-			      struct iw_request_info *info,
-			      union iwreq_data *wrqu, char *extra)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-	if (priv->ieee80211->bHwRadioOff)
-		return 0;
-
-
-	down(&priv->wx_sem);
-	priv->rf_sleep(dev);
-
-	up(&priv->wx_sem);
-
-	return 0;
-
-}
-static int r8180_wx_get_channelplan(struct net_device *dev,
-				    struct iw_request_info *info,
-				    union iwreq_data *wrqu, char *extra)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-
-
-	down(&priv->wx_sem);
-	*extra = priv->channel_plan;
-
-
-
-	up(&priv->wx_sem);
-
-	return 0;
-}
-static int r8180_wx_set_channelplan(struct net_device *dev,
-				    struct iw_request_info *info,
-				    union iwreq_data *wrqu, char *extra)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	int *val = (int *)extra;
-	int i;
-	printk("-----in fun %s\n", __func__);
-
-	if (priv->ieee80211->bHwRadioOff)
-		return 0;
-
-	/* unsigned long flags; */
-	down(&priv->wx_sem);
-	if (default_channel_plan[*val].len != 0) {
-		priv->channel_plan = *val;
-		/* Clear old channel map 8 */
-		for (i = 1; i <= MAX_CHANNEL_NUMBER; i++)
-			GET_DOT11D_INFO(priv->ieee80211)->channel_map[i] = 0;
-
-		/* Set new channel map */
-		for (i = 1; i <= default_channel_plan[*val].len; i++)
-			GET_DOT11D_INFO(priv->ieee80211)->channel_map[default_channel_plan[*val].channel[i-1]] = 1;
-
-	}
-	up(&priv->wx_sem);
-
-	return 0;
-}
-
-static int r8180_wx_get_version(struct net_device *dev,
-				struct iw_request_info *info,
-				union iwreq_data *wrqu, char *extra)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	/* struct ieee80211_device *ieee; */
-
-	down(&priv->wx_sem);
-	strcpy(extra, "1020.0808");
-	up(&priv->wx_sem);
-
-	return 0;
-}
-
-/* added by amy 080818 */
-/*receive datarate from user typing valid rate is from 2 to 108 (1 - 54M), if input 0, return to normal rate adaptive. */
-static int r8180_wx_set_forcerate(struct net_device *dev,
-				  struct iw_request_info *info,
-				  union iwreq_data *wrqu, char *extra)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	u8 forcerate = *extra;
-
-	down(&priv->wx_sem);
-
-	printk("==============>%s(): forcerate is %d\n", __func__, forcerate);
-	if ((forcerate == 2) || (forcerate == 4) || (forcerate == 11) || (forcerate == 22) || (forcerate == 12) ||
-		(forcerate == 18) || (forcerate == 24) || (forcerate == 36) || (forcerate == 48) || (forcerate == 72) ||
-		(forcerate == 96) || (forcerate == 108)) {
-		priv->ForcedDataRate = 1;
-		priv->ieee80211->rate = forcerate * 5;
-	}	else if (forcerate == 0)	{
-		priv->ForcedDataRate = 0;
-		printk("OK! return rate adaptive\n");
-	}	else
-			printk("ERR: wrong rate\n");
-	up(&priv->wx_sem);
-	return 0;
-}
-
-static int r8180_wx_set_enc_ext(struct net_device *dev,
-				struct iw_request_info *info,
-				union iwreq_data *wrqu, char *extra)
-{
-
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-	int ret = 0;
-
-	if (priv->ieee80211->bHwRadioOff)
-		return 0;
-
-	down(&priv->wx_sem);
-	ret = ieee80211_wx_set_encode_ext(priv->ieee80211, info, wrqu, extra);
-	up(&priv->wx_sem);
-	return ret;
-
-}
-static int r8180_wx_set_auth(struct net_device *dev,
-			     struct iw_request_info *info,
-			     union iwreq_data *wrqu, char *extra)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	int ret = 0;
-
-	if (priv->ieee80211->bHwRadioOff)
-		return 0;
-
-	down(&priv->wx_sem);
-	ret = ieee80211_wx_set_auth(priv->ieee80211, info, &wrqu->param, extra);
-	up(&priv->wx_sem);
-	return ret;
-}
-
-static int r8180_wx_set_mlme(struct net_device *dev,
-			     struct iw_request_info *info,
-			     union iwreq_data *wrqu, char *extra)
-{
-	int ret = 0;
-	struct r8180_priv *priv = ieee80211_priv(dev);
-
-
-	if (priv->ieee80211->bHwRadioOff)
-		return 0;
-
-
-	down(&priv->wx_sem);
-#if 1
-	ret = ieee80211_wx_set_mlme(priv->ieee80211, info, wrqu, extra);
-#endif
-	up(&priv->wx_sem);
-	return ret;
-}
-static int r8180_wx_set_gen_ie(struct net_device *dev,
-			       struct iw_request_info *info,
-			       union iwreq_data *wrqu, char *extra)
-{
-	int ret = 0;
-		struct r8180_priv *priv = ieee80211_priv(dev);
-
-
-	if (priv->ieee80211->bHwRadioOff)
-		return 0;
-
-	down(&priv->wx_sem);
-#if 1
-	ret = ieee80211_wx_set_gen_ie(priv->ieee80211, extra, wrqu->data.length);
-#endif
-	up(&priv->wx_sem);
-	return ret;
-
-
-}
-
-static const iw_handler r8180_wx_handlers[] =	{
-	IW_HANDLER(SIOCGIWNAME,		r8180_wx_get_name),
-	IW_HANDLER(SIOCSIWNWID,		dummy),
-	IW_HANDLER(SIOCGIWNWID,		dummy),
-	IW_HANDLER(SIOCSIWFREQ,		r8180_wx_set_freq),
-	IW_HANDLER(SIOCGIWFREQ,		r8180_wx_get_freq),
-	IW_HANDLER(SIOCSIWMODE, 	r8180_wx_set_mode),
-	IW_HANDLER(SIOCGIWMODE,		r8180_wx_get_mode),
-	IW_HANDLER(SIOCSIWSENS,		r8180_wx_set_sens),
-	IW_HANDLER(SIOCGIWSENS,		r8180_wx_get_sens),
-	IW_HANDLER(SIOCGIWRANGE,	rtl8180_wx_get_range),
-	IW_HANDLER(SIOCSIWSPY,		dummy),
-	IW_HANDLER(SIOCGIWSPY,		dummy),
-	IW_HANDLER(SIOCSIWAP,		r8180_wx_set_wap),
-	IW_HANDLER(SIOCGIWAP,		r8180_wx_get_wap),
-	IW_HANDLER(SIOCSIWMLME,		r8180_wx_set_mlme),
-	IW_HANDLER(SIOCGIWAPLIST,	dummy),		/* deprecated */
-	IW_HANDLER(SIOCSIWSCAN,		r8180_wx_set_scan),
-	IW_HANDLER(SIOCGIWSCAN,		r8180_wx_get_scan),
-	IW_HANDLER(SIOCSIWESSID,	r8180_wx_set_essid),
-	IW_HANDLER(SIOCGIWESSID,	r8180_wx_get_essid),
-	IW_HANDLER(SIOCSIWNICKN,	dummy),
-	IW_HANDLER(SIOCGIWNICKN,	dummy),
-	IW_HANDLER(SIOCSIWRATE,		r8180_wx_set_rate),
-	IW_HANDLER(SIOCGIWRATE,		r8180_wx_get_rate),
-	IW_HANDLER(SIOCSIWRTS,		r8180_wx_set_rts),
-	IW_HANDLER(SIOCGIWRTS,		r8180_wx_get_rts),
-	IW_HANDLER(SIOCSIWFRAG,		r8180_wx_set_frag),
-	IW_HANDLER(SIOCGIWFRAG,		r8180_wx_get_frag),
-	IW_HANDLER(SIOCSIWTXPOW,	dummy),
-	IW_HANDLER(SIOCGIWTXPOW,	dummy),
-	IW_HANDLER(SIOCSIWRETRY,	r8180_wx_set_retry),
-	IW_HANDLER(SIOCGIWRETRY,	r8180_wx_get_retry),
-	IW_HANDLER(SIOCSIWENCODE,	r8180_wx_set_enc),
-	IW_HANDLER(SIOCGIWENCODE,	r8180_wx_get_enc),
-	IW_HANDLER(SIOCSIWPOWER,	r8180_wx_set_power),
-	IW_HANDLER(SIOCGIWPOWER,	r8180_wx_get_power),
-	IW_HANDLER(SIOCSIWGENIE,	r8180_wx_set_gen_ie),
-	IW_HANDLER(SIOCSIWAUTH,		r8180_wx_set_auth),
-	IW_HANDLER(SIOCSIWENCODEEXT,	r8180_wx_set_enc_ext),
-};
-
-static const struct iw_priv_args r8180_private_args[] = {
-	{
-		SIOCIWFIRSTPRIV + 0x0,
-		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "badcrc"
-	},
-	{	SIOCIWFIRSTPRIV + 0x1,
-		0, 0, "dummy"
-
-	},
-	{
-		SIOCIWFIRSTPRIV + 0x2,
-		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "beaconint"
-	},
-	{	SIOCIWFIRSTPRIV + 0x3,
-		0, 0, "dummy"
-
-	},
-	{
-		SIOCIWFIRSTPRIV + 0x4,
-		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "activescan"
-
-	},
-	{	SIOCIWFIRSTPRIV + 0x5,
-		0, 0, "dummy"
-
-	},
-	{
-		SIOCIWFIRSTPRIV + 0x6,
-		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "rawtx"
-
-	},
-	{	SIOCIWFIRSTPRIV + 0x7,
-		0, 0, "dummy"
-
-	},
-	{
-		SIOCIWFIRSTPRIV + 0x8,
-		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "setiwmode"
-	},
-	{
-		SIOCIWFIRSTPRIV + 0x9,
-		0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | 32, "getiwmode"
-	},
-	{
-		SIOCIWFIRSTPRIV + 0xA,
-		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "setpreamble"
-	},
-	{
-		SIOCIWFIRSTPRIV + 0xB,
-		0, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, "getpreamble"
-	},
-	{	SIOCIWFIRSTPRIV + 0xC,
-		0, 0, "dummy"
-	},
-	{
-		SIOCIWFIRSTPRIV + 0xD,
-		0, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, "getrssi"
-	},
-	{	SIOCIWFIRSTPRIV + 0xE,
-		0, 0, "dummy"
-	},
-	{
-		SIOCIWFIRSTPRIV + 0xF,
-		0, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, "getlinkqual"
-	},
-	{
-		SIOCIWFIRSTPRIV + 0x10,
-		0, 0, "resetstats"
-	},
-	{
-		SIOCIWFIRSTPRIV + 0x11,
-		0, 0, "dummy"
-	},
-	{
-		SIOCIWFIRSTPRIV + 0x12,
-		0, 0, "radioon"
-	},
-	{
-		SIOCIWFIRSTPRIV + 0x13,
-		0, 0, "radiooff"
-	},
-	{
-		SIOCIWFIRSTPRIV + 0x14,
-		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "setchannel"
-	},
-	{
-		SIOCIWFIRSTPRIV + 0x15,
-		0, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, "getchannel"
-	},
-	{
-		SIOCIWFIRSTPRIV + 0x16,
-		0, 0, "dummy"
-	},
-	{
-		SIOCIWFIRSTPRIV + 0x17,
-		0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | 32, "getversion"
-	},
-	{
-		SIOCIWFIRSTPRIV + 0x18,
-		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "setrate"
-	},
-};
-
-
-static iw_handler r8180_private_handler[] = {
-	r8180_wx_set_crcmon, /*SIOCIWSECONDPRIV*/
-	dummy,
-	r8180_wx_set_beaconinterval,
-	dummy,
-	/* r8180_wx_set_monitor_type, */
-	r8180_wx_set_scan_type,
-	dummy,
-	r8180_wx_set_rawtx,
-	dummy,
-	r8180_wx_set_iwmode,
-	r8180_wx_get_iwmode,
-	r8180_wx_set_preamble,
-	r8180_wx_get_preamble,
-	dummy,
-	r8180_wx_get_siglevel,
-	dummy,
-	r8180_wx_get_sigqual,
-	r8180_wx_reset_stats,
-	dummy,/* r8180_wx_get_stats */
-	r8180_wx_radio_on,
-	r8180_wx_radio_off,
-	r8180_wx_set_channelplan,
-	r8180_wx_get_channelplan,
-	dummy,
-	r8180_wx_get_version,
-	r8180_wx_set_forcerate,
-};
-
-static inline int is_same_network(struct ieee80211_network *src,
-				  struct ieee80211_network *dst,
-				  struct ieee80211_device *ieee)
-{
-		/* A network is only a duplicate if the channel, BSSID, ESSID
-		 * and the capability field (in particular IBSS and BSS) all match.
-		 * We treat all <hidden> with the same BSSID and channel
-		 * as one network
-		 */
-		if (src->channel != dst->channel)
-			return 0;
-
-		if (memcmp(src->bssid, dst->bssid, ETH_ALEN) != 0)
-			return 0;
-
-		if (ieee->iw_mode != IW_MODE_INFRA) {
-			if (src->ssid_len != dst->ssid_len)
-				return 0;
-			if (memcmp(src->ssid, dst->ssid, src->ssid_len) != 0)
-				return 0;
-		}
-
-		if ((src->capability & WLAN_CAPABILITY_IBSS) !=
-		    (dst->capability & WLAN_CAPABILITY_IBSS))
-			return 0;
-		if ((src->capability & WLAN_CAPABILITY_BSS) !=
-		    (dst->capability & WLAN_CAPABILITY_BSS))
-			return 0;
-
-		return 1;
-}
-
-/* WB modified to show signal to GUI on 18-01-2008 */
-static struct iw_statistics *r8180_get_wireless_stats(struct net_device *dev)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	struct ieee80211_device *ieee = priv->ieee80211;
-	struct iw_statistics *wstats = &priv->wstats;
-	int tmp_level = 0;
-	int tmp_qual = 0;
-	int tmp_noise = 0;
-
-	if (ieee->state < IEEE80211_LINKED)	{
-		wstats->qual.qual = 0;
-		wstats->qual.level = 0;
-		wstats->qual.noise = 0;
-		wstats->qual.updated = IW_QUAL_ALL_UPDATED | IW_QUAL_DBM;
-		return wstats;
-	}
-
-	tmp_level = (&ieee->current_network)->stats.signal;
-	tmp_qual = (&ieee->current_network)->stats.signalstrength;
-	tmp_noise = (&ieee->current_network)->stats.noise;
-
-	wstats->qual.level = tmp_level;
-	wstats->qual.qual = tmp_qual;
-	wstats->qual.noise = tmp_noise;
-	wstats->qual.updated = IW_QUAL_ALL_UPDATED | IW_QUAL_DBM;
-	return wstats;
-}
-
-struct iw_handler_def  r8180_wx_handlers_def = {
-	.standard = r8180_wx_handlers,
-	.num_standard = ARRAY_SIZE(r8180_wx_handlers),
-	.private = r8180_private_handler,
-	.num_private = ARRAY_SIZE(r8180_private_handler),
-	.num_private_args = sizeof(r8180_private_args) / sizeof(struct iw_priv_args),
-	.get_wireless_stats = r8180_get_wireless_stats,
-	.private_args = (struct iw_priv_args *)r8180_private_args,
-};
-
-
diff --git a/drivers/staging/rtl8187se/r8180_wx.h b/drivers/staging/rtl8187se/r8180_wx.h
deleted file mode 100644
index d471520ac77..00000000000
--- a/drivers/staging/rtl8187se/r8180_wx.h
+++ /dev/null
@@ -1,21 +0,0 @@
-/*
-	This is part of rtl8180 OpenSource driver - v 0.3
-	Copyright (C) Andrea Merello 2004  <andrea.merello@gmail.com>
-	Released under the terms of GPL (General Public Licence)
-
-	Parts of this driver are based on the GPL part of the official realtek driver
-	Parts of this driver are based on the rtl8180 driver skeleton from Patric Schenke & Andres Salomon
-	Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver
-
-	We want to thanks the Authors of such projects and the Ndiswrapper project Authors.
-*/
-
-/* this file (will) contains wireless extension handlers*/
-
-#ifndef R8180_WX_H
-#define R8180_WX_H
-#include <linux/wireless.h>
-#include "ieee80211/ieee80211.h"
-extern struct iw_handler_def r8180_wx_handlers_def;
-
-#endif
diff --git a/drivers/staging/rtl8187se/r8185b_init.c b/drivers/staging/rtl8187se/r8185b_init.c
deleted file mode 100644
index cc6f100814f..00000000000
--- a/drivers/staging/rtl8187se/r8185b_init.c
+++ /dev/null
@@ -1,1464 +0,0 @@
-/*
- * Copyright (c) Realtek Semiconductor Corp. All rights reserved.
- *
- * Module Name:
- *	r8185b_init.c
- *
- * Abstract:
- *	Hardware Initialization and Hardware IO for RTL8185B
- *
- * Major Change History:
- *	When		Who				What
- *	----------	---------------		-------------------------------
- *	2006-11-15	Xiong			Created
- *
- * Notes:
- *	This file is ported from RTL8185B Windows driver.
- *
- *
- */
-
-/*--------------------------Include File------------------------------------*/
-#include <linux/spinlock.h>
-#include "r8180_hw.h"
-#include "r8180.h"
-#include "r8180_rtl8225.h" /* RTL8225 Radio frontend */
-#include "r8180_93cx6.h"   /* Card EEPROM */
-#include "r8180_wx.h"
-#include "ieee80211/dot11d.h"
-/* #define CONFIG_RTL8180_IO_MAP */
-#define TC_3W_POLL_MAX_TRY_CNT 5
-
-static u8 MAC_REG_TABLE[][2] =	{
-	/*
-	 * PAGE 0:
-	 * 0x34(BRSR), 0xBE(RATE_FALLBACK_CTL), 0x1E0(ARFR) would set in
-	 * HwConfigureRTL8185()
-	 * 0x272(RFSW_CTRL), 0x1CE(AESMSK_QC) set in InitializeAdapter8185().
-	 * 0x1F0~0x1F8  set in MacConfig_85BASIC()
-	 */
-	{0x08, 0xae}, {0x0a, 0x72}, {0x5b, 0x42},
-	{0x84, 0x88}, {0x85, 0x24}, {0x88, 0x54}, {0x8b, 0xb8}, {0x8c, 0x03},
-	{0x8d, 0x40}, {0x8e, 0x00}, {0x8f, 0x00}, {0x5b, 0x18}, {0x91, 0x03},
-	{0x94, 0x0F}, {0x95, 0x32},
-	{0x96, 0x00}, {0x97, 0x07}, {0xb4, 0x22}, {0xdb, 0x00},
-	{0xf0, 0x32}, {0xf1, 0x32}, {0xf2, 0x00}, {0xf3, 0x00}, {0xf4, 0x32},
-	{0xf5, 0x43}, {0xf6, 0x00}, {0xf7, 0x00}, {0xf8, 0x46}, {0xf9, 0xa4},
-	{0xfa, 0x00}, {0xfb, 0x00}, {0xfc, 0x96}, {0xfd, 0xa4}, {0xfe, 0x00},
-	{0xff, 0x00},
-
-	/*
-	 * PAGE 1:
-	 * For Flextronics system Logo PCIHCT failure:
-	 * 0x1C4~0x1CD set no-zero value to avoid PCI configuration
-	 * space 0x45[7]=1
-	 */
-	{0x5e, 0x01},
-	{0x58, 0x00}, {0x59, 0x00}, {0x5a, 0x04}, {0x5b, 0x00}, {0x60, 0x24},
-	{0x61, 0x97}, {0x62, 0xF0}, {0x63, 0x09}, {0x80, 0x0F}, {0x81, 0xFF},
-	{0x82, 0xFF}, {0x83, 0x03},
-	/* lzm add 080826 */
-	{0xC4, 0x22}, {0xC5, 0x22}, {0xC6, 0x22}, {0xC7, 0x22}, {0xC8, 0x22},
-	/* lzm add 080826 */
-	{0xC9, 0x22}, {0xCA, 0x22}, {0xCB, 0x22}, {0xCC, 0x22}, {0xCD, 0x22},
-	{0xe2, 0x00},
-
-
-	/* PAGE 2: */
-	{0x5e, 0x02},
-	{0x0c, 0x04}, {0x4c, 0x30}, {0x4d, 0x08}, {0x50, 0x05}, {0x51, 0xf5},
-	{0x52, 0x04}, {0x53, 0xa0}, {0x54, 0xff}, {0x55, 0xff}, {0x56, 0xff},
-	{0x57, 0xff}, {0x58, 0x08}, {0x59, 0x08}, {0x5a, 0x08}, {0x5b, 0x08},
-	{0x60, 0x08}, {0x61, 0x08}, {0x62, 0x08}, {0x63, 0x08}, {0x64, 0x2f},
-	{0x8c, 0x3f}, {0x8d, 0x3f}, {0x8e, 0x3f},
-	{0x8f, 0x3f}, {0xc4, 0xff}, {0xc5, 0xff}, {0xc6, 0xff}, {0xc7, 0xff},
-	{0xc8, 0x00}, {0xc9, 0x00}, {0xca, 0x80}, {0xcb, 0x00},
-
-	/* PAGE 0: */
-	{0x5e, 0x00}, {0x9f, 0x03}
-	};
-
-
-static u8  ZEBRA_AGC[] = {
-	0,
-	0x7E, 0x7E, 0x7E, 0x7E, 0x7D, 0x7C, 0x7B, 0x7A, 0x79, 0x78, 0x77, 0x76,
-	0x75, 0x74, 0x73, 0x72, 0x71, 0x70, 0x6F, 0x6E, 0x6D, 0x6C, 0x6B, 0x6A,
-	0x69, 0x68, 0x67, 0x66, 0x65, 0x64, 0x63, 0x62, 0x48, 0x47, 0x46, 0x45,
-	0x44, 0x29, 0x28, 0x27, 0x26, 0x25, 0x24, 0x23, 0x22, 0x21, 0x08, 0x07,
-	0x06, 0x05, 0x04, 0x03, 0x02, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f,
-	0x0f, 0x0f, 0x10, 0x11, 0x12, 0x13, 0x15, 0x16, 0x17, 0x17, 0x18, 0x18,
-	0x19, 0x1a, 0x1a, 0x1b, 0x1b, 0x1c, 0x1c, 0x1d, 0x1d, 0x1d, 0x1e, 0x1e,
-	0x1f, 0x1f, 0x1f, 0x20, 0x20, 0x20, 0x20, 0x21, 0x21, 0x21, 0x22, 0x22,
-	0x22, 0x23, 0x23, 0x24, 0x24, 0x25, 0x25, 0x25, 0x26, 0x26, 0x27, 0x27,
-	0x2F, 0x2F, 0x2F, 0x2F, 0x2F, 0x2F, 0x2F, 0x2F
-	};
-
-static u32 ZEBRA_RF_RX_GAIN_TABLE[] = {
-	0x0096, 0x0076, 0x0056, 0x0036, 0x0016, 0x01f6, 0x01d6, 0x01b6,
-	0x0196, 0x0176, 0x00F7, 0x00D7, 0x00B7, 0x0097, 0x0077, 0x0057,
-	0x0037, 0x00FB, 0x00DB, 0x00BB, 0x00FF, 0x00E3, 0x00C3, 0x00A3,
-	0x0083, 0x0063, 0x0043, 0x0023, 0x0003, 0x01E3, 0x01C3, 0x01A3,
-	0x0183, 0x0163, 0x0143, 0x0123, 0x0103
-	};
-
-static u8 OFDM_CONFIG[]	= {
-	/* OFDM reg0x06[7:0]=0xFF: Enable power saving mode in RX */
-	/* OFDM reg0x3C[4]=1'b1: Enable RX power saving mode */
-	/* ofdm 0x3a = 0x7b ,(original : 0xfb) For ECS shielding room TP test */
-	/* 0x00	*/
-	0x10, 0x0F, 0x0A, 0x0C, 0x14, 0xFA, 0xFF, 0x50,
-	0x00, 0x50, 0x00, 0x00, 0x00, 0x5C, 0x00, 0x00,
-	/* 0x10	*/
-	0x40, 0x00, 0x40, 0x00, 0x00, 0x00, 0xA8, 0x26,
-	0x32, 0x33, 0x06, 0xA5, 0x6F, 0x55, 0xC8, 0xBB,
-	/* 0x20	*/
-	0x0A, 0xE1, 0x2C, 0x4A, 0x86, 0x83, 0x34, 0x00,
-	0x4F, 0x24, 0x6F, 0xC2, 0x03, 0x40, 0x80, 0x00,
-	/* 0x30	*/
-	0xC0, 0xC1, 0x58, 0xF1, 0x00, 0xC4, 0x90, 0x3e,
-	0xD8, 0x3C, 0x7B, 0x10, 0x10
-	};
-
-	/*---------------------------------------------------------------
-	 *	Hardware IO
-	 *	the code is ported from Windows source code
-	 *---------------------------------------------------------------
-	 */
-
-static u8 PlatformIORead1Byte(struct net_device *dev, u32 offset)
-{
-	return read_nic_byte(dev, offset);
-}
-
-static void PlatformIOWrite1Byte(struct net_device *dev, u32 offset, u8 data)
-{
-	write_nic_byte(dev, offset, data);
-	/*
-	 * To make sure write operation is completed,
-	 * 2005.11.09, by rcnjko.
-	 */
-	read_nic_byte(dev, offset);
-}
-
-static void PlatformIOWrite2Byte(struct net_device *dev, u32 offset, u16 data)
-{
-	write_nic_word(dev, offset, data);
-	/*
-	 * To make sure write operation is completed,
-	 *  2005.11.09, by rcnjko.
-	 */
-	read_nic_word(dev, offset);
-}
-
-static void PlatformIOWrite4Byte(struct net_device *dev, u32 offset, u32 data)
-{
-	if (offset == PhyAddr) {
-		/* For Base Band configuration. */
-		unsigned char	cmdByte;
-		unsigned long	dataBytes;
-		unsigned char	idx;
-		u8		u1bTmp;
-
-		cmdByte = (u8)(data & 0x000000ff);
-		dataBytes = data>>8;
-
-		/*
-		 *	071010, rcnjko:
-		 *	The critical section is only BB read/write race
-		 *	condition. Assumption:
-		 *	1. We assume NO one will access BB at DIRQL, otherwise,
-		 *	system will crash for
-		 *	acquiring the spinlock in such context.
-		 *	2. PlatformIOWrite4Byte() MUST NOT be recursive.
-		 */
-		/* NdisAcquireSpinLock( &(pDevice->IoSpinLock) ); */
-
-		for (idx = 0; idx < 30; idx++) {
-			/* Make sure command bit is clear before access it. */
-			u1bTmp = PlatformIORead1Byte(dev, PhyAddr);
-			if ((u1bTmp & BIT7) == 0)
-				break;
-			else
-				mdelay(10);
-		}
-
-		for (idx = 0; idx < 3; idx++)
-			PlatformIOWrite1Byte(dev, offset+1+idx,
-					     ((u8 *)&dataBytes)[idx]);
-
-		write_nic_byte(dev, offset, cmdByte);
-
-		/* NdisReleaseSpinLock( &(pDevice->IoSpinLock) ); */
-	} else {
-		write_nic_dword(dev, offset, data);
-		/*
-		 * To make sure write operation is completed, 2005.11.09,
-		 *  by rcnjko.
-		 */
-		read_nic_dword(dev, offset);
-	}
-}
-
-static void SetOutputEnableOfRfPins(struct net_device *dev)
-{
-	write_nic_word(dev, RFPinsEnable, 0x1bff);
-}
-
-static bool HwHSSIThreeWire(struct net_device *dev,
-			    u8 *pDataBuf,
-			    bool write)
-{
-	u8	TryCnt;
-	u8	u1bTmp;
-
-	/* Check if WE and RE are cleared. */
-	for (TryCnt = 0; TryCnt < TC_3W_POLL_MAX_TRY_CNT; TryCnt++) {
-		u1bTmp = read_nic_byte(dev, SW_3W_CMD1);
-		if ((u1bTmp & (SW_3W_CMD1_RE|SW_3W_CMD1_WE)) == 0)
-			break;
-
-		udelay(10);
-	}
-	if (TryCnt == TC_3W_POLL_MAX_TRY_CNT) {
-		netdev_err(dev,
-			   "HwThreeWire(): CmdReg: %#X RE|WE bits are not clear!!\n",
-			   u1bTmp);
-	return false;
-	}
-
-	/* RTL8187S HSSI Read/Write Function */
-	u1bTmp = read_nic_byte(dev, RF_SW_CONFIG);
-	u1bTmp |= RF_SW_CFG_SI; /* reg08[1]=1 Serial Interface(SI) */
-	write_nic_byte(dev, RF_SW_CONFIG, u1bTmp);
-
-	/* jong: HW SI read must set reg84[3]=0. */
-	u1bTmp = read_nic_byte(dev, RFPinsSelect);
-	u1bTmp &= ~BIT3;
-	write_nic_byte(dev, RFPinsSelect, u1bTmp);
-	/*  Fill up data buffer for write operation. */
-
-	/* SI - reg274[3:0] : RF register's Address */
-	if (write)
-		write_nic_word(dev, SW_3W_DB0, *((u16 *)pDataBuf));
-	else
-		write_nic_word(dev, SW_3W_DB0, *((u16 *)pDataBuf));
-
-	/* Set up command: WE or RE. */
-	if (write)
-		write_nic_byte(dev, SW_3W_CMD1, SW_3W_CMD1_WE);
-	else
-		write_nic_byte(dev, SW_3W_CMD1, SW_3W_CMD1_RE);
-
-
-	/* Check if DONE is set. */
-	for (TryCnt = 0; TryCnt < TC_3W_POLL_MAX_TRY_CNT; TryCnt++) {
-		u1bTmp = read_nic_byte(dev, SW_3W_CMD1);
-		if (u1bTmp & SW_3W_CMD1_DONE)
-			break;
-
-		udelay(10);
-	}
-
-	write_nic_byte(dev, SW_3W_CMD1, 0);
-
-	/* Read back data for read operation. */
-	if (!write) {
-		/* Serial Interface : reg363_362[11:0] */
-		*((u16 *)pDataBuf) = read_nic_word(dev, SI_DATA_READ);
-		*((u16 *)pDataBuf) &= 0x0FFF;
-	}
-
-	return true;
-}
-
-void RF_WriteReg(struct net_device *dev, u8 offset, u16 data)
-{
-	u16 reg = (data << 4) | (offset & 0x0f);
-	HwHSSIThreeWire(dev, (u8 *)&reg, true);
-}
-
-u16 RF_ReadReg(struct net_device *dev, u8 offset)
-{
-	u16 reg = offset & 0x0f;
-	HwHSSIThreeWire(dev, (u8 *)&reg, false);
-	return reg;
-}
-
-static u8 ReadBBPortUchar(struct net_device *dev, u32 addr)
-{
-	PlatformIOWrite4Byte(dev, PhyAddr, addr & 0xffffff7f);
-	return PlatformIORead1Byte(dev, PhyDataR);
-}
-
-/* by Owen on 04/07/14 for writing BB register successfully */
-static void WriteBBPortUchar(struct net_device *dev, u32 Data)
-{
-	PlatformIOWrite4Byte(dev, PhyAddr, Data);
-	ReadBBPortUchar(dev, Data);
-}
-
-/*
- *	Description:
- *	Perform Antenna settings with antenna diversity on 87SE.
- *		Created by Roger, 2008.01.25.
- */
-bool SetAntennaConfig87SE(struct net_device *dev,
-			  u8   DefaultAnt, /* 0: Main, 1: Aux. */
-			  bool bAntDiversity) /* 1:Enable, 0: Disable. */
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	bool   bAntennaSwitched = true;
-	/* 0x00 = disabled, 0x80 = enabled */
-	u8	ant_diversity_offset = 0x00;
-
-	/*
-	 * printk("SetAntennaConfig87SE(): DefaultAnt(%d), bAntDiversity(%d)\n",
-	 * DefaultAnt, bAntDiversity);
-	 */
-
-	/* Threshold for antenna diversity. */
-	write_phy_cck(dev, 0x0c, 0x09); /* Reg0c : 09 */
-
-	if (bAntDiversity)	/*	Enable Antenna Diversity. */
-		ant_diversity_offset = 0x80;
-
-	if (DefaultAnt == 1) { /* aux Antenna */
-		/* Mac register, aux antenna */
-		write_nic_byte(dev, ANTSEL, 0x00);
-
-		/* Config CCK RX antenna. */
-		write_phy_cck(dev, 0x11, 0xbb); /* Reg11 : bb */
-
-		/* Reg01 : 47 | ant_diversity_offset */
-		write_phy_cck(dev, 0x01, 0x47|ant_diversity_offset);
-
-		/* Config OFDM RX antenna. */
-		write_phy_ofdm(dev, 0x0D, 0x54);	/* Reg0d : 54 */
-		/* Reg18 : 32 */
-		write_phy_ofdm(dev, 0x18, 0x32|ant_diversity_offset);
-	} else { /* main Antenna */
-		/* Mac register, main antenna */
-		write_nic_byte(dev, ANTSEL, 0x03);
-
-		/* Config CCK RX antenna.	*/
-		write_phy_cck(dev, 0x11, 0x9b); /* Reg11 : 9b */
-		/* Reg01 : 47 */
-		write_phy_cck(dev, 0x01, 0x47|ant_diversity_offset);
-
-		/* Config OFDM RX antenna. */
-		write_phy_ofdm(dev, 0x0D, 0x5c); /* Reg0d : 5c */
-		/*Reg18 : 32 */
-		write_phy_ofdm(dev, 0x18, 0x32|ant_diversity_offset);
-	}
-	priv->CurrAntennaIndex = DefaultAnt; /* Update default settings. */
-	return	bAntennaSwitched;
-}
-/*
- *--------------------------------------------------------------
- *		Hardware Initialization.
- *		the code is ported from Windows source code
- *--------------------------------------------------------------
- */
-
-static void ZEBRA_Config_85BASIC_HardCode(struct net_device *dev)
-{
-
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	u32			i;
-	u32	addr, data;
-	u32 u4bRegOffset, u4bRegValue;
-	u16 u4bRF23, u4bRF24;
-	u8			u1b24E;
-	int d_cut = 0;
-
-
-/*
- *===========================================================================
- *	87S_PCIE :: RADIOCFG.TXT
- *===========================================================================
- */
-
-
-	/* Page1 : reg16-reg30 */
-	RF_WriteReg(dev, 0x00, 0x013f);		mdelay(1); /* switch to page1 */
-	u4bRF23 = RF_ReadReg(dev, 0x08);	mdelay(1);
-	u4bRF24 = RF_ReadReg(dev, 0x09);	mdelay(1);
-
-	if (u4bRF23 == 0x818 && u4bRF24 == 0x70C) {
-		d_cut = 1;
-		netdev_info(dev, "card type changed from C- to D-cut\n");
-	}
-
-	/* Page0 : reg0-reg15 */
-
-	RF_WriteReg(dev, 0x00, 0x009f);		mdelay(1);/* 1	*/
-	RF_WriteReg(dev, 0x01, 0x06e0);		mdelay(1);
-	RF_WriteReg(dev, 0x02, 0x004d);		mdelay(1);/* 2	*/
-	RF_WriteReg(dev, 0x03, 0x07f1);		mdelay(1);/* 3	*/
-	RF_WriteReg(dev, 0x04, 0x0975);		mdelay(1);
-	RF_WriteReg(dev, 0x05, 0x0c72);		mdelay(1);
-	RF_WriteReg(dev, 0x06, 0x0ae6);		mdelay(1);
-	RF_WriteReg(dev, 0x07, 0x00ca);		mdelay(1);
-	RF_WriteReg(dev, 0x08, 0x0e1c);		mdelay(1);
-	RF_WriteReg(dev, 0x09, 0x02f0);		mdelay(1);
-	RF_WriteReg(dev, 0x0a, 0x09d0);		mdelay(1);
-	RF_WriteReg(dev, 0x0b, 0x01ba);		mdelay(1);
-	RF_WriteReg(dev, 0x0c, 0x0640);		mdelay(1);
-	RF_WriteReg(dev, 0x0d, 0x08df);		mdelay(1);
-	RF_WriteReg(dev, 0x0e, 0x0020);		mdelay(1);
-	RF_WriteReg(dev, 0x0f, 0x0990);		mdelay(1);
-
-	/*  Page1 : reg16-reg30 */
-	RF_WriteReg(dev, 0x00, 0x013f);		mdelay(1);
-	RF_WriteReg(dev, 0x03, 0x0806);		mdelay(1);
-	RF_WriteReg(dev, 0x04, 0x03a7);		mdelay(1);
-	RF_WriteReg(dev, 0x05, 0x059b);		mdelay(1);
-	RF_WriteReg(dev, 0x06, 0x0081);		mdelay(1);
-	RF_WriteReg(dev, 0x07, 0x01A0);		mdelay(1);
-/*
- * Don't write RF23/RF24 to make a difference between 87S C cut and D cut.
- * asked by SD3 stevenl.
- */
-	RF_WriteReg(dev, 0x0a, 0x0001);		mdelay(1);
-	RF_WriteReg(dev, 0x0b, 0x0418);		mdelay(1);
-
-	if (d_cut) {
-		RF_WriteReg(dev, 0x0c, 0x0fbe);		mdelay(1);
-		RF_WriteReg(dev, 0x0d, 0x0008);		mdelay(1);
-		/* RX LO buffer */
-		RF_WriteReg(dev, 0x0e, 0x0807);		mdelay(1);
-	} else {
-		RF_WriteReg(dev, 0x0c, 0x0fbe);		mdelay(1);
-		RF_WriteReg(dev, 0x0d, 0x0008);		mdelay(1);
-		/* RX LO buffer */
-		RF_WriteReg(dev, 0x0e, 0x0806);		mdelay(1);
-	}
-
-	RF_WriteReg(dev, 0x0f, 0x0acc);		mdelay(1);
-	RF_WriteReg(dev, 0x00, 0x01d7);		mdelay(1); /* 6 */
-	RF_WriteReg(dev, 0x03, 0x0e00);		mdelay(1);
-	RF_WriteReg(dev, 0x04, 0x0e50);		mdelay(1);
-
-	for (i = 0; i <= 36; i++) {
-		RF_WriteReg(dev, 0x01, i);		mdelay(1);
-		RF_WriteReg(dev, 0x02, ZEBRA_RF_RX_GAIN_TABLE[i]); mdelay(1);
-	}
-
-	RF_WriteReg(dev, 0x05, 0x0203);		mdelay(1); /* 203, 343 */
-	RF_WriteReg(dev, 0x06, 0x0200);		mdelay(1); /* 400 */
-	/* switch to reg16-reg30, and HSSI disable 137 */
-	RF_WriteReg(dev, 0x00, 0x0137);		mdelay(1);
-	mdelay(10); /* Deay 10 ms. */		/* 0xfd */
-
-	/* Z4 synthesizer loop filter setting, 392 */
-	RF_WriteReg(dev, 0x0d, 0x0008);		mdelay(1);
-	mdelay(10); /* Deay 10 ms. */		/* 0xfd */
-
-	/* switch to reg0-reg15, and HSSI disable */
-	RF_WriteReg(dev, 0x00, 0x0037);		mdelay(1);
-	mdelay(10); /* Deay 10 ms. */		/* 0xfd	*/
-
-	/* CBC on, Tx Rx disable, High gain */
-	RF_WriteReg(dev, 0x04, 0x0160);		mdelay(1);
-	mdelay(10); /* Deay 10 ms. */		/* 0xfd	*/
-
-	/* Z4 setted channel 1 */
-	RF_WriteReg(dev, 0x07, 0x0080);		mdelay(1);
-	mdelay(10); /* Deay 10 ms. */		/* 0xfd	*/
-
-	RF_WriteReg(dev, 0x02, 0x088D);		mdelay(1); /* LC calibration */
-	mdelay(200); /* Deay 200 ms. */		/* 0xfd	*/
-	mdelay(10);  /* Deay 10 ms. */		/* 0xfd	*/
-	mdelay(10);  /* Deay 10 ms. */		/* 0xfd	*/
-
-	/* switch to reg16-reg30 137, and HSSI disable 137 */
-	RF_WriteReg(dev, 0x00, 0x0137);		mdelay(1);
-	mdelay(10); /* Deay 10 ms. */		/* 0xfd	*/
-
-	RF_WriteReg(dev, 0x07, 0x0000);		mdelay(1);
-	RF_WriteReg(dev, 0x07, 0x0180);		mdelay(1);
-	RF_WriteReg(dev, 0x07, 0x0220);		mdelay(1);
-	RF_WriteReg(dev, 0x07, 0x03E0);		mdelay(1);
-
-	/* DAC calibration off 20070702	*/
-	RF_WriteReg(dev, 0x06, 0x00c1);		mdelay(1);
-	RF_WriteReg(dev, 0x0a, 0x0001);		mdelay(1);
-	/* For crystal calibration, added by Roger, 2007.12.11. */
-	if (priv->bXtalCalibration) { /* reg 30.	*/
-	 /*
-	  *	enable crystal calibration.
-	  *	RF Reg[30], (1)Xin:[12:9], Xout:[8:5],  addr[4:0].
-	  *	(2)PA Pwr delay timer[15:14], default: 2.4us,
-	  *	set BIT15=0
-	  *	(3)RF signal on/off when calibration[13], default: on,
-	  *	set BIT13=0.
-	  *	So we should minus 4 BITs offset.
-	  */
-		RF_WriteReg(dev, 0x0f, (priv->XtalCal_Xin<<5) |
-			    (priv->XtalCal_Xout<<1) | BIT11 | BIT9); mdelay(1);
-		netdev_info(dev, "ZEBRA_Config_85BASIC_HardCode(): (%02x)\n",
-		      (priv->XtalCal_Xin<<5) | (priv->XtalCal_Xout<<1) |
-		       BIT11 | BIT9);
-	} else {
-		/* using default value. Xin=6, Xout=6.	*/
-		RF_WriteReg(dev, 0x0f, 0x0acc);		mdelay(1);
-	}
-	/* switch to reg0-reg15, and HSSI enable */
-	RF_WriteReg(dev, 0x00, 0x00bf);		mdelay(1);
-	/* Rx BB start calibration, 00c//+edward */
-	RF_WriteReg(dev, 0x0d, 0x08df);		mdelay(1);
-	/* temperature meter off */
-	RF_WriteReg(dev, 0x02, 0x004d);		mdelay(1);
-	RF_WriteReg(dev, 0x04, 0x0975);		mdelay(1); /* Rx mode */
-	mdelay(10);	/* Deay 10 ms.*/	/* 0xfe	*/
-	mdelay(10);	/* Deay 10 ms.*/	/* 0xfe	*/
-	mdelay(10);	/* Deay 10 ms.*/	/* 0xfe	*/
-	/* Rx mode*/	/*+edward */
-	RF_WriteReg(dev, 0x00, 0x0197);		mdelay(1);
-	/* Rx mode*/	/*+edward */
-	RF_WriteReg(dev, 0x05, 0x05ab);		mdelay(1);
-	/* Rx mode*/	/*+edward */
-	RF_WriteReg(dev, 0x00, 0x009f);		mdelay(1);
-	/* Rx mode*/	/*+edward */
-	RF_WriteReg(dev, 0x01, 0x0000);		mdelay(1);
-	/* Rx mode*/	/*+edward */
-	RF_WriteReg(dev, 0x02, 0x0000);		mdelay(1);
-	/* power save parameters. */
-	u1b24E = read_nic_byte(dev, 0x24E);
-	write_nic_byte(dev, 0x24E, (u1b24E & (~(BIT5|BIT6))));
-
-	/*======================================================================
-	 *
-	 *======================================================================
-	 * CCKCONF.TXT
-	 *======================================================================
-	 *
-	 *	[POWER SAVE] Power Saving Parameters by jong. 2007-11-27
-	 *	CCK reg0x00[7]=1'b1 :power saving for TX (default)
-	 *	CCK reg0x00[6]=1'b1: power saving for RX (default)
-	 *	CCK reg0x06[4]=1'b1: turn off channel estimation related
-	 *	circuits if not doing channel estimation.
-	 *	CCK reg0x06[3]=1'b1: turn off unused circuits before cca = 1
-	 *	CCK reg0x06[2]=1'b1: turn off cck's circuit if macrst =0
-	 */
-
-	write_phy_cck(dev, 0x00, 0xc8);
-	write_phy_cck(dev, 0x06, 0x1c);
-	write_phy_cck(dev, 0x10, 0x78);
-	write_phy_cck(dev, 0x2e, 0xd0);
-	write_phy_cck(dev, 0x2f, 0x06);
-	write_phy_cck(dev, 0x01, 0x46);
-
-	/* power control */
-	write_nic_byte(dev, CCK_TXAGC, 0x10);
-	write_nic_byte(dev, OFDM_TXAGC, 0x1B);
-	write_nic_byte(dev, ANTSEL, 0x03);
-
-
-
-	/*
-	 *======================================================================
-	 *	AGC.txt
-	 *======================================================================
-	 */
-
-	write_phy_ofdm(dev, 0x00, 0x12);
-
-	for (i = 0; i < 128; i++) {
-
-		data = ZEBRA_AGC[i+1];
-		data = data << 8;
-		data = data | 0x0000008F;
-
-		addr = i + 0x80; /* enable writing AGC table */
-		addr = addr << 8;
-		addr = addr | 0x0000008E;
-
-		WriteBBPortUchar(dev, data);
-		WriteBBPortUchar(dev, addr);
-		WriteBBPortUchar(dev, 0x0000008E);
-	}
-
-	PlatformIOWrite4Byte(dev, PhyAddr, 0x00001080);	/* Annie, 2006-05-05 */
-
-	/*
-	 *======================================================================
-	 *
-	 *======================================================================
-	 * OFDMCONF.TXT
-	 *======================================================================
-	 */
-
-	for (i = 0; i < 60; i++) {
-		u4bRegOffset = i;
-		u4bRegValue = OFDM_CONFIG[i];
-
-		WriteBBPortUchar(dev,
-				(0x00000080 |
-				(u4bRegOffset & 0x7f) |
-				((u4bRegValue & 0xff) << 8)));
-	}
-
-	/*
-	 *======================================================================
-	 * by amy for antenna
-	 *======================================================================
-	 */
-	/*
-	 * Config Sw/Hw  Combinational Antenna Diversity. Added by Roger,
-	 * 2008.02.26.
-	 */
-	SetAntennaConfig87SE(dev, priv->bDefaultAntenna1,
-			     priv->bSwAntennaDiverity);
-}
-
-
-void UpdateInitialGain(struct net_device *dev)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-
-	/* lzm add 080826 */
-	if (priv->eRFPowerState != RF_ON) {
-		/*	Don't access BB/RF under disable PLL situation.
-		 *	RT_TRACE(COMP_DIG, DBG_LOUD, ("UpdateInitialGain -
-		 *	pHalData->eRFPowerState!=RF_ON\n"));
-		 *	Back to the original state
-		 */
-		priv->InitialGain = priv->InitialGainBackUp;
-		return;
-	}
-
-	switch (priv->InitialGain) {
-	case 1: /* m861dBm */
-		write_phy_ofdm(dev, 0x17, 0x26);	mdelay(1);
-		write_phy_ofdm(dev, 0x24, 0x86);	mdelay(1);
-		write_phy_ofdm(dev, 0x05, 0xfa);	mdelay(1);
-		break;
-
-	case 2: /* m862dBm */
-		write_phy_ofdm(dev, 0x17, 0x36);	mdelay(1);
-		write_phy_ofdm(dev, 0x24, 0x86);	mdelay(1);
-		write_phy_ofdm(dev, 0x05, 0xfa);	mdelay(1);
-		break;
-
-	case 3: /* m863dBm */
-		write_phy_ofdm(dev, 0x17, 0x36);	mdelay(1);
-		write_phy_ofdm(dev, 0x24, 0x86);	mdelay(1);
-		write_phy_ofdm(dev, 0x05, 0xfb);	mdelay(1);
-		break;
-
-	case 4: /* m864dBm */
-		write_phy_ofdm(dev, 0x17, 0x46);	mdelay(1);
-		write_phy_ofdm(dev, 0x24, 0x86);	mdelay(1);
-		write_phy_ofdm(dev, 0x05, 0xfb);	mdelay(1);
-		break;
-
-	case 5: /* m82dBm */
-		write_phy_ofdm(dev, 0x17, 0x46);	mdelay(1);
-		write_phy_ofdm(dev, 0x24, 0x96);	mdelay(1);
-		write_phy_ofdm(dev, 0x05, 0xfb);	mdelay(1);
-		break;
-
-	case 6: /* m78dBm */
-		write_phy_ofdm(dev, 0x17, 0x56);	mdelay(1);
-		write_phy_ofdm(dev, 0x24, 0x96);	mdelay(1);
-		write_phy_ofdm(dev, 0x05, 0xfc);	mdelay(1);
-		break;
-
-	case 7: /* m74dBm */
-		write_phy_ofdm(dev, 0x17, 0x56);	mdelay(1);
-		write_phy_ofdm(dev, 0x24, 0xa6);	mdelay(1);
-		write_phy_ofdm(dev, 0x05, 0xfc);	mdelay(1);
-		break;
-
-	case 8:
-		write_phy_ofdm(dev, 0x17, 0x66);	mdelay(1);
-		write_phy_ofdm(dev, 0x24, 0xb6);	mdelay(1);
-		write_phy_ofdm(dev, 0x05, 0xfc);	mdelay(1);
-		break;
-
-	default: /* MP */
-		write_phy_ofdm(dev, 0x17, 0x26);	mdelay(1);
-		write_phy_ofdm(dev, 0x24, 0x86);	mdelay(1);
-		write_phy_ofdm(dev, 0x05, 0xfa);	mdelay(1);
-		break;
-	}
-}
-/*
- *	Description:
- *		Tx Power tracking mechanism routine on 87SE.
- *	Created by Roger, 2007.12.11.
- */
-static void InitTxPwrTracking87SE(struct net_device *dev)
-{
-	u32	u4bRfReg;
-
-	u4bRfReg = RF_ReadReg(dev, 0x02);
-
-	/* Enable Thermal meter indication.	*/
-	RF_WriteReg(dev, 0x02, u4bRfReg|PWR_METER_EN);		mdelay(1);
-}
-
-static void PhyConfig8185(struct net_device *dev)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-		write_nic_dword(dev, RCR, priv->ReceiveConfig);
-	   priv->RFProgType = read_nic_byte(dev, CONFIG4) & 0x03;
-	/*  RF config */
-	ZEBRA_Config_85BASIC_HardCode(dev);
-	/* Set default initial gain state to 4, approved by SD3 DZ, by Bruce,
-	 * 2007-06-06.
-	 */
-	if (priv->bDigMechanism) {
-		if (priv->InitialGain == 0)
-			priv->InitialGain = 4;
-	}
-
-	/*
-	 *	Enable thermal meter indication to implement TxPower tracking
-	 *	on 87SE. We initialize thermal meter here to avoid unsuccessful
-	 *	configuration. Added by Roger, 2007.12.11.
-	 */
-	if (priv->bTxPowerTrack)
-		InitTxPwrTracking87SE(dev);
-
-	priv->InitialGainBackUp = priv->InitialGain;
-	UpdateInitialGain(dev);
-
-	return;
-}
-
-static void HwConfigureRTL8185(struct net_device *dev)
-{
-	/*
-	 * RTL8185_TODO: Determine Retrylimit, TxAGC,
-	 * AutoRateFallback control.
-	 */
-	u8 bUNIVERSAL_CONTROL_RL = 0;
-	u8 bUNIVERSAL_CONTROL_AGC = 1;
-	u8 bUNIVERSAL_CONTROL_ANT = 1;
-	u8 bAUTO_RATE_FALLBACK_CTL = 1;
-	u8 val8;
-	write_nic_word(dev, BRSR, 0x0fff);
-	/* Retry limit */
-	val8 = read_nic_byte(dev, CW_CONF);
-
-	if (bUNIVERSAL_CONTROL_RL)
-		val8 = val8 & 0xfd;
-	else
-		val8 = val8 | 0x02;
-
-	write_nic_byte(dev, CW_CONF, val8);
-
-	/* Tx AGC */
-	val8 = read_nic_byte(dev, TXAGC_CTL);
-	if (bUNIVERSAL_CONTROL_AGC) {
-		write_nic_byte(dev, CCK_TXAGC, 128);
-		write_nic_byte(dev, OFDM_TXAGC, 128);
-		val8 = val8 & 0xfe;
-	} else {
-		val8 = val8 | 0x01;
-	}
-
-
-	write_nic_byte(dev, TXAGC_CTL, val8);
-
-	/* Tx Antenna including Feedback control */
-	val8 = read_nic_byte(dev, TXAGC_CTL);
-
-	if (bUNIVERSAL_CONTROL_ANT) {
-		write_nic_byte(dev, ANTSEL, 0x00);
-		val8 = val8 & 0xfd;
-	} else {
-		val8 = val8 & (val8|0x02); /* xiong-2006-11-15 */
-	}
-
-	write_nic_byte(dev, TXAGC_CTL, val8);
-
-	/* Auto Rate fallback control	*/
-	val8 = read_nic_byte(dev, RATE_FALLBACK);
-	val8 &= 0x7c;
-	if (bAUTO_RATE_FALLBACK_CTL) {
-		val8 |= RATE_FALLBACK_CTL_ENABLE | RATE_FALLBACK_CTL_AUTO_STEP1;
-
-		/* <RJ_TODO_8185B> We shall set up the ARFR according
-		 * to user's setting.
-		 */
-		PlatformIOWrite2Byte(dev, ARFR, 0x0fff); /* set 1M ~ 54Mbps. */
-	}
-	write_nic_byte(dev, RATE_FALLBACK, val8);
-}
-
-static void MacConfig_85BASIC_HardCode(struct net_device *dev)
-{
-	/*
-	 *======================================================================
-	 * MACREG.TXT
-	 *======================================================================
-	 */
-	int nLinesRead = 0;
-	u32 u4bRegOffset, u4bRegValue, u4bPageIndex = 0;
-	int i;
-
-	nLinesRead = sizeof(MAC_REG_TABLE)/2;
-
-	for (i = 0; i < nLinesRead; i++) { /* nLinesRead=101 */
-		u4bRegOffset = MAC_REG_TABLE[i][0];
-		u4bRegValue = MAC_REG_TABLE[i][1];
-
-				if (u4bRegOffset == 0x5e)
-					u4bPageIndex = u4bRegValue;
-				else
-					u4bRegOffset |= (u4bPageIndex << 8);
-
-		write_nic_byte(dev, u4bRegOffset, (u8)u4bRegValue);
-	}
-	/* ================================================================= */
-}
-
-static void MacConfig_85BASIC(struct net_device *dev)
-{
-
-	u8			u1DA;
-	MacConfig_85BASIC_HardCode(dev);
-
-	/* ================================================================= */
-
-	/* Follow TID_AC_MAP of WMac. */
-	write_nic_word(dev, TID_AC_MAP, 0xfa50);
-
-	/* Interrupt Migration, Jong suggested we use set 0x0000 first,
-	 * 2005.12.14, by rcnjko.
-	 */
-	write_nic_word(dev, IntMig, 0x0000);
-
-	/* Prevent TPC to cause CRC error. Added by Annie, 2006-06-10. */
-	PlatformIOWrite4Byte(dev, 0x1F0, 0x00000000);
-	PlatformIOWrite4Byte(dev, 0x1F4, 0x00000000);
-	PlatformIOWrite1Byte(dev, 0x1F8, 0x00);
-
-	/* Asked for by SD3 CM Lin, 2006.06.27, by rcnjko. */
-
-	/*
-	 *  power save parameter based on
-	 * "87SE power save parameters 20071127.doc", as follow.
-	 */
-
-	/* Enable DA10 TX power saving */
-	u1DA = read_nic_byte(dev, PHYPR);
-	write_nic_byte(dev, PHYPR, (u1DA | BIT2));
-
-	/* POWER: */
-	write_nic_word(dev, 0x360, 0x1000);
-	write_nic_word(dev, 0x362, 0x1000);
-
-	/* AFE. */
-	write_nic_word(dev, 0x370, 0x0560);
-	write_nic_word(dev, 0x372, 0x0560);
-	write_nic_word(dev, 0x374, 0x0DA4);
-	write_nic_word(dev, 0x376, 0x0DA4);
-	write_nic_word(dev, 0x378, 0x0560);
-	write_nic_word(dev, 0x37A, 0x0560);
-	write_nic_word(dev, 0x37C, 0x00EC);
-	write_nic_word(dev, 0x37E, 0x00EC); /* +edward */
-	write_nic_byte(dev, 0x24E, 0x01);
-}
-
-static u8 GetSupportedWirelessMode8185(struct net_device *dev)
-{
-	return WIRELESS_MODE_B | WIRELESS_MODE_G;
-}
-
-static void
-ActUpdateChannelAccessSetting(struct net_device *dev,
-			      enum wireless_mode mode,
-			      struct chnl_access_setting *chnl_access_setting)
-{
-	AC_CODING	eACI;
-
-	/*
-	 *	<RJ_TODO_8185B>
-	 *	TODO: We still don't know how to set up these registers,
-	 *	just follow WMAC to verify 8185B FPAG.
-	 *
-	 *	<RJ_TODO_8185B>
-	 *	Jong said CWmin/CWmax register are not functional in 8185B,
-	 *	so we shall fill channel access realted register into AC
-	 *	parameter registers,
-	 *	even in nQBss.
-	 */
-
-	/* Suggested by Jong, 2005.12.08. */
-	chnl_access_setting->sifs_timer = 0x22;
-	chnl_access_setting->difs_timer = 0x1C; /* 2006.06.02, by rcnjko. */
-	chnl_access_setting->slot_time_timer = 9; /* 2006.06.02, by rcnjko. */
-	/*
-	 * Suggested by wcchu, it is the default value of EIFS register,
-	 * 2005.12.08.
-	 */
-	chnl_access_setting->eifs_timer = 0x5B;
-	chnl_access_setting->cwmin_index = 3; /* 2006.06.02, by rcnjko. */
-	chnl_access_setting->cwmax_index = 7; /* 2006.06.02, by rcnjko. */
-
-	write_nic_byte(dev, SIFS, chnl_access_setting->sifs_timer);
-	/*
-	 * Rewrited from directly use PlatformEFIOWrite1Byte(),
-	 * by Annie, 2006-03-29.
-	 */
-	write_nic_byte(dev, SLOT, chnl_access_setting->slot_time_timer);
-
-	write_nic_byte(dev, EIFS, chnl_access_setting->eifs_timer);
-
-	/*
-	 * <RJ_EXPR_QOS> Suggested by wcchu, it is the default value of EIFS
-	 * register, 2005.12.08.
-	 */
-	write_nic_byte(dev, AckTimeOutReg, 0x5B);
-
-	for (eACI = 0; eACI < AC_MAX; eACI++)
-		write_nic_byte(dev, ACM_CONTROL, 0);
-}
-
-static void ActSetWirelessMode8185(struct net_device *dev, u8 btWirelessMode)
-{
-	struct  r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	struct  ieee80211_device *ieee = priv->ieee80211;
-	u8	btSupportedWirelessMode = GetSupportedWirelessMode8185(dev);
-
-	if ((btWirelessMode & btSupportedWirelessMode) == 0)	{
-		/*
-		 * Don't switch to unsupported wireless mode, 2006.02.15,
-		 * by rcnjko.
-		 */
-		DMESGW("ActSetWirelessMode8185(): WirelessMode(%d) is not supported (%d)!\n",
-			btWirelessMode, btSupportedWirelessMode);
-		return;
-	}
-
-	/* 1. Assign wireless mode to switch if necessary. */
-	if (btWirelessMode == WIRELESS_MODE_AUTO) {
-		if ((btSupportedWirelessMode & WIRELESS_MODE_A)) {
-			btWirelessMode = WIRELESS_MODE_A;
-		} else if (btSupportedWirelessMode & WIRELESS_MODE_G) {
-				btWirelessMode = WIRELESS_MODE_G;
-
-		} else if ((btSupportedWirelessMode & WIRELESS_MODE_B))	{
-				btWirelessMode = WIRELESS_MODE_B;
-		} else {
-			DMESGW("ActSetWirelessMode8185(): No valid wireless mode supported, btSupportedWirelessMode(%x)!!!\n",
-			       btSupportedWirelessMode);
-			btWirelessMode = WIRELESS_MODE_B;
-		}
-	}
-
-	/*
-	 * 2. Swtich band: RF or BB specific actions,
-	 * for example, refresh tables in omc8255, or change initial gain if
-	 * necessary. Nothing to do for Zebra to switch band. Update current
-	 * wireless mode if we switch to specified band successfully.
-	 */
-
-	ieee->mode = (enum wireless_mode)btWirelessMode;
-
-	/* 3. Change related setting. */
-	if (ieee->mode == WIRELESS_MODE_A)
-		DMESG("WIRELESS_MODE_A\n");
-	else if (ieee->mode == WIRELESS_MODE_B)
-		DMESG("WIRELESS_MODE_B\n");
-	else if (ieee->mode == WIRELESS_MODE_G)
-		DMESG("WIRELESS_MODE_G\n");
-
-	ActUpdateChannelAccessSetting(dev, ieee->mode,
-				      &priv->ChannelAccessSetting);
-}
-
-void rtl8185b_irq_enable(struct net_device *dev)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-
-	priv->irq_enabled = 1;
-	write_nic_dword(dev, IMR, priv->IntrMask);
-}
-
-static void MgntDisconnectIBSS(struct net_device *dev)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	u8 i;
-
-	for (i = 0; i < 6; i++)
-		priv->ieee80211->current_network.bssid[i] = 0x55;
-
-
-
-	priv->ieee80211->state = IEEE80211_NOLINK;
-	/*
-	 *	Stop Beacon.
-	 *
-	 *	Vista add a Adhoc profile, HW radio off until
-	 *	OID_DOT11_RESET_REQUEST Driver would set MSR=NO_LINK,
-	 *	then HW Radio ON, MgntQueue Stuck. Because Bcn DMA isn't
-	 *	complete, mgnt queue would stuck until Bcn packet send.
-	 *
-	 *	Disable Beacon Queue Own bit, suggested by jong
-	 */
-	ieee80211_stop_send_beacons(priv->ieee80211);
-
-	priv->ieee80211->link_change(dev);
-	notify_wx_assoc_event(priv->ieee80211);
-}
-
-static void MlmeDisassociateRequest(struct net_device *dev, u8 *asSta, u8 asRsn)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	u8 i;
-
-	SendDisassociation(priv->ieee80211, asSta, asRsn);
-
-	if (memcmp(priv->ieee80211->current_network.bssid, asSta, 6) == 0) {
-		/* ShuChen TODO: change media status. */
-
-		for (i = 0; i < 6; i++)
-			priv->ieee80211->current_network.bssid[i] = 0x22;
-
-		ieee80211_disassociate(priv->ieee80211);
-	}
-}
-
-static void MgntDisconnectAP(struct net_device *dev, u8 asRsn)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-
-	/*
-	 * Commented out by rcnjko, 2005.01.27:
-	 * I move SecClearAllKeys() to MgntActSet_802_11_DISASSOCIATE().
-	 *
-	 *	2004/09/15, kcwu, the key should be cleared, or the new
-	 *	handshaking will not success
-	 *
-	 *	In WPA WPA2 need to Clear all key ... because new key will set
-	 *	after new handshaking. 2004.10.11, by rcnjko.
-	 */
-	MlmeDisassociateRequest(dev, priv->ieee80211->current_network.bssid,
-				asRsn);
-
-	priv->ieee80211->state = IEEE80211_NOLINK;
-}
-
-static bool MgntDisconnect(struct net_device *dev, u8 asRsn)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	/*
-	 *	Schedule an workitem to wake up for ps mode, 070109, by rcnjko.
-	 */
-
-	if (IS_DOT11D_ENABLE(priv->ieee80211))
-		Dot11d_Reset(priv->ieee80211);
-	/* In adhoc mode, update beacon frame. */
-	if (priv->ieee80211->state == IEEE80211_LINKED)	{
-		if (priv->ieee80211->iw_mode == IW_MODE_ADHOC)
-			MgntDisconnectIBSS(dev);
-
-		if (priv->ieee80211->iw_mode == IW_MODE_INFRA) {
-			/*
-			 *	We clear key here instead of MgntDisconnectAP()
-			 *	because that MgntActSet_802_11_DISASSOCIATE()
-			 *	is an interface called by OS, e.g.
-			 *	OID_802_11_DISASSOCIATE in Windows while as
-			 *	MgntDisconnectAP() is used to handle
-			 *	disassociation related things to AP, e.g. send
-			 *	Disassoc frame to AP.  2005.01.27, by rcnjko.
-			 */
-			MgntDisconnectAP(dev, asRsn);
-		}
-		/* Indicate Disconnect, 2005.02.23, by rcnjko.	*/
-	}
-	return true;
-}
-/*
- *	Description:
- *		Chang RF Power State.
- *		Note that, only MgntActSet_RF_State() is allowed to set
- *		HW_VAR_RF_STATE.
- *
- *	Assumption:
- *		PASSIVE LEVEL.
- */
-static bool SetRFPowerState(struct net_device *dev,
-			    enum rt_rf_power_state eRFPowerState)
-{
-	struct	r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	bool	bResult = false;
-
-	if (eRFPowerState == priv->eRFPowerState)
-		return bResult;
-
-	bResult = SetZebraRFPowerState8185(dev, eRFPowerState);
-
-	return bResult;
-}
-
-bool MgntActSet_RF_State(struct net_device *dev, enum rt_rf_power_state StateToSet,
-			 u32 ChangeSource)
-{
-	struct	r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	bool	bActionAllowed = false;
-	bool	bConnectBySSID = false;
-	enum rt_rf_power_state rtState;
-	u16	RFWaitCounter = 0;
-	unsigned long flag;
-	/*
-	 *	Prevent the race condition of RF state change. By Bruce,
-	 *	2007-11-28. Only one thread can change the RF state at one time,
-	 *	and others should wait to be executed.
-	 */
-	while (true) {
-		spin_lock_irqsave(&priv->rf_ps_lock, flag);
-		if (priv->RFChangeInProgress) {
-			spin_unlock_irqrestore(&priv->rf_ps_lock, flag);
-			/*  Set RF after the previous action is done.	*/
-			while (priv->RFChangeInProgress) {
-				RFWaitCounter++;
-				udelay(1000); /* 1 ms	*/
-
-				/*
-				 *	Wait too long, return FALSE to avoid
-				 *	to be stuck here.
-				 */
-				if (RFWaitCounter > 1000) { /* 1sec */
-					netdev_info(dev, "MgntActSet_RF_State(): Wait too long to set RF\n");
-					/* TODO: Reset RF state? */
-					return false;
-				}
-			}
-		} else {
-			priv->RFChangeInProgress = true;
-			spin_unlock_irqrestore(&priv->rf_ps_lock, flag);
-			break;
-		}
-	}
-	rtState = priv->eRFPowerState;
-
-	switch (StateToSet) {
-	case RF_ON:
-		/*
-		 *	Turn On RF no matter the IPS setting because we need to
-		 *	update the RF state to Ndis under Vista, or the Windows
-		 *	does not allow the driver to perform site survey any
-		 *	more. By Bruce, 2007-10-02.
-		 */
-		priv->RfOffReason &= (~ChangeSource);
-
-		if (!priv->RfOffReason)	{
-			priv->RfOffReason = 0;
-			bActionAllowed = true;
-
-			if (rtState == RF_OFF &&
-			    ChangeSource >= RF_CHANGE_BY_HW)
-				bConnectBySSID = true;
-		}
-		break;
-
-	case RF_OFF:
-		 /* 070125, rcnjko: we always keep connected in AP mode. */
-
-		if (priv->RfOffReason > RF_CHANGE_BY_IPS) {
-			/*
-			 *	060808, Annie:
-			 *	Disconnect to current BSS when radio off.
-			 *	Asked by QuanTa.
-			 *
-			 *	Calling MgntDisconnect() instead of
-			 *	MgntActSet_802_11_DISASSOCIATE(), because
-			 *	we do NOT need to set ssid to dummy ones.
-			 */
-			MgntDisconnect(dev, disas_lv_ss);
-			/*
-			 *	Clear content of bssDesc[] and bssDesc4Query[]
-			 *	to avoid reporting old bss to UI.
-			 */
-		}
-
-		priv->RfOffReason |= ChangeSource;
-		bActionAllowed = true;
-		break;
-	case RF_SLEEP:
-		priv->RfOffReason |= ChangeSource;
-		bActionAllowed = true;
-		break;
-	default:
-		break;
-	}
-
-	if (bActionAllowed) {
-		/* Config HW to the specified mode. */
-		SetRFPowerState(dev, StateToSet);
-	}
-
-	/* Release RF spinlock	*/
-	spin_lock_irqsave(&priv->rf_ps_lock, flag);
-	priv->RFChangeInProgress = false;
-	spin_unlock_irqrestore(&priv->rf_ps_lock, flag);
-	return bActionAllowed;
-}
-
-static void InactivePowerSave(struct net_device *dev)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	/*
-	 *	This flag "bSwRfProcessing", indicates the status of IPS
-	 *	procedure, should be set if the IPS workitem is really
-	 *	scheduled. The old code, sets this flag before scheduling the
-	 *	IPS workitem and however, at the same time the previous IPS
-	 *	workitem did not end yet, fails to schedule the current
-	 *	workitem. Thus, bSwRfProcessing blocks the IPS procedure of
-	 *	switching RF.
-	 */
-	priv->bSwRfProcessing = true;
-
-	MgntActSet_RF_State(dev, priv->eInactivePowerState, RF_CHANGE_BY_IPS);
-
-	/*
-	 *	To solve CAM values miss in RF OFF, rewrite CAM values after
-	 *	RF ON. By Bruce, 2007-09-20.
-	 */
-
-	priv->bSwRfProcessing = false;
-}
-
-/*
- *	Description:
- *		Enter the inactive power save mode. RF will be off
- */
-void IPSEnter(struct net_device *dev)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	enum rt_rf_power_state rtState;
-	if (priv->bInactivePs) {
-		rtState = priv->eRFPowerState;
-
-		/*
-		 *	Do not enter IPS in the following conditions:
-		 *	(1) RF is already OFF or
-		 *	Sleep (2) bSwRfProcessing (indicates the IPS is still
-		 *	under going) (3) Connected (only disconnected can
-		 *	trigger IPS)(4) IBSS (send Beacon)
-		 *	(5) AP mode (send Beacon)
-		 */
-		if (rtState == RF_ON && !priv->bSwRfProcessing
-			&& (priv->ieee80211->state != IEEE80211_LINKED)) {
-			priv->eInactivePowerState = RF_OFF;
-			InactivePowerSave(dev);
-		}
-	}
-}
-void IPSLeave(struct net_device *dev)
-{
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-	enum rt_rf_power_state rtState;
-	if (priv->bInactivePs) {
-		rtState = priv->eRFPowerState;
-		if ((rtState == RF_OFF || rtState == RF_SLEEP) &&
-		    !priv->bSwRfProcessing
-		    && priv->RfOffReason <= RF_CHANGE_BY_IPS) {
-			priv->eInactivePowerState = RF_ON;
-			InactivePowerSave(dev);
-		}
-	}
-}
-
-void rtl8185b_adapter_start(struct net_device *dev)
-{
-	struct r8180_priv *priv = ieee80211_priv(dev);
-	struct ieee80211_device *ieee = priv->ieee80211;
-
-	u8 SupportedWirelessMode;
-	u8 InitWirelessMode;
-	u8 bInvalidWirelessMode = 0;
-	u8 tmpu8;
-	u8 btCR9346;
-	u8 TmpU1b;
-	u8 btPSR;
-
-	write_nic_byte(dev, 0x24e, (BIT5|BIT6|BIT0));
-	rtl8180_reset(dev);
-
-	priv->dma_poll_mask = 0;
-	priv->dma_poll_stop_mask = 0;
-
-	HwConfigureRTL8185(dev);
-	write_nic_dword(dev, MAC0, ((u32 *)dev->dev_addr)[0]);
-	write_nic_word(dev, MAC4, ((u32 *)dev->dev_addr)[1] & 0xffff);
-	/* default network type to 'No Link' */
-	write_nic_byte(dev, MSR, read_nic_byte(dev, MSR) & 0xf3);
-	write_nic_word(dev, BcnItv, 100);
-	write_nic_word(dev, AtimWnd, 2);
-	PlatformIOWrite2Byte(dev, FEMR, 0xFFFF);
-	write_nic_byte(dev, WPA_CONFIG, 0);
-	MacConfig_85BASIC(dev);
-	/* Override the RFSW_CTRL (MAC offset 0x272-0x273), 2006.06.07,
-	 * by rcnjko.
-	 */
-	/* BT_DEMO_BOARD type */
-	PlatformIOWrite2Byte(dev, RFSW_CTRL, 0x569a);
-
-	/*
-	 *---------------------------------------------------------------------
-	 *	Set up PHY related.
-	 *---------------------------------------------------------------------
-	 */
-	/* Enable Config3.PARAM_En to revise AnaaParm. */
-	write_nic_byte(dev, CR9346, 0xc0); /* enable config register write */
-	tmpu8 = read_nic_byte(dev, CONFIG3);
-	write_nic_byte(dev, CONFIG3, (tmpu8 | CONFIG3_PARM_En));
-	/* Turn on Analog power. */
-	/* Asked for by William, otherwise, MAC 3-wire can't work,
-	 * 2006.06.27, by rcnjko.
-	 */
-	write_nic_dword(dev, ANAPARAM2, ANAPARM2_ASIC_ON);
-	write_nic_dword(dev, ANAPARAM, ANAPARM_ASIC_ON);
-	write_nic_word(dev, ANAPARAM3, 0x0010);
-
-	write_nic_byte(dev, CONFIG3, tmpu8);
-	write_nic_byte(dev, CR9346, 0x00);
-	/* enable EEM0 and EEM1 in 9346CR */
-	btCR9346 = read_nic_byte(dev, CR9346);
-	write_nic_byte(dev, CR9346, (btCR9346 | 0xC0));
-
-	/* B cut use LED1 to control HW RF on/off */
-	TmpU1b = read_nic_byte(dev, CONFIG5);
-	TmpU1b = TmpU1b & ~BIT3;
-	write_nic_byte(dev, CONFIG5, TmpU1b);
-
-	/* disable EEM0 and EEM1 in 9346CR */
-	btCR9346 &= ~(0xC0);
-	write_nic_byte(dev, CR9346, btCR9346);
-
-	/* Enable Led (suggested by Jong) */
-	/* B-cut RF Radio on/off  5e[3]=0 */
-	btPSR = read_nic_byte(dev, PSR);
-	write_nic_byte(dev, PSR, (btPSR | BIT3));
-	/* setup initial timing for RFE. */
-	write_nic_word(dev, RFPinsOutput, 0x0480);
-	SetOutputEnableOfRfPins(dev);
-	write_nic_word(dev, RFPinsSelect, 0x2488);
-
-	/* PHY config. */
-	PhyConfig8185(dev);
-
-	/*
-	 *	We assume RegWirelessMode has already been initialized before,
-	 *	however, we has to validate the wireless mode here and provide a
-	 *	reasonable initialized value if necessary. 2005.01.13,
-	 *	by rcnjko.
-	 */
-	SupportedWirelessMode = GetSupportedWirelessMode8185(dev);
-	if ((ieee->mode != WIRELESS_MODE_B) &&
-		(ieee->mode != WIRELESS_MODE_G) &&
-		(ieee->mode != WIRELESS_MODE_A) &&
-		(ieee->mode != WIRELESS_MODE_AUTO)) {
-		/* It should be one of B, G, A, or AUTO. */
-		bInvalidWirelessMode = 1;
-	} else {
-	/* One of B, G, A, or AUTO. */
-		/* Check if the wireless mode is supported by RF. */
-		if	((ieee->mode != WIRELESS_MODE_AUTO) &&
-			(ieee->mode & SupportedWirelessMode) == 0) {
-			bInvalidWirelessMode = 1;
-		}
-	}
-
-	if (bInvalidWirelessMode || ieee->mode == WIRELESS_MODE_AUTO) {
-		/* Auto or other invalid value. */
-		/* Assigne a wireless mode to initialize. */
-		if ((SupportedWirelessMode & WIRELESS_MODE_A)) {
-			InitWirelessMode = WIRELESS_MODE_A;
-		} else if ((SupportedWirelessMode & WIRELESS_MODE_G)) {
-			InitWirelessMode = WIRELESS_MODE_G;
-		} else if ((SupportedWirelessMode & WIRELESS_MODE_B)) {
-			InitWirelessMode = WIRELESS_MODE_B;
-		} else {
-			DMESGW("InitializeAdapter8185(): No valid wireless mode supported, SupportedWirelessMode(%x)!!!\n",
-				 SupportedWirelessMode);
-			InitWirelessMode = WIRELESS_MODE_B;
-		}
-
-		/* Initialize RegWirelessMode if it is not a valid one.	*/
-		if (bInvalidWirelessMode)
-			ieee->mode = (enum wireless_mode)InitWirelessMode;
-
-	} else {
-	/* One of B, G, A. */
-		InitWirelessMode = ieee->mode;
-	}
-	priv->eRFPowerState = RF_OFF;
-	priv->RfOffReason = 0;
-	{
-		MgntActSet_RF_State(dev, RF_ON, 0);
-	}
-		/*
-		 * If inactive power mode is enabled, disable rf while in
-		 * disconnected state.
-		 */
-	if (priv->bInactivePs)
-		MgntActSet_RF_State(dev , RF_OFF, RF_CHANGE_BY_IPS);
-
-	ActSetWirelessMode8185(dev, (u8)(InitWirelessMode));
-
-	/* ----------------------------------------------------------------- */
-
-	rtl8185b_irq_enable(dev);
-
-	netif_start_queue(dev);
-}
-
-void rtl8185b_rx_enable(struct net_device *dev)
-{
-	u8 cmd;
-	/* for now we accept data, management & ctl frame*/
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-
-
-	if (dev->flags & IFF_PROMISC)
-		DMESG("NIC in promisc mode");
-
-	if (priv->ieee80211->iw_mode == IW_MODE_MONITOR || dev->flags &
-	    IFF_PROMISC) {
-		priv->ReceiveConfig = priv->ReceiveConfig & (~RCR_APM);
-		priv->ReceiveConfig = priv->ReceiveConfig | RCR_AAP;
-	}
-
-	if (priv->ieee80211->iw_mode == IW_MODE_MONITOR)
-		priv->ReceiveConfig = priv->ReceiveConfig | RCR_ACF |
-				      RCR_APWRMGT | RCR_AICV;
-
-
-	if (priv->crcmon == 1 && priv->ieee80211->iw_mode == IW_MODE_MONITOR)
-		priv->ReceiveConfig = priv->ReceiveConfig | RCR_ACRC32;
-
-	write_nic_dword(dev, RCR, priv->ReceiveConfig);
-
-	fix_rx_fifo(dev);
-
-	cmd = read_nic_byte(dev, CMD);
-	write_nic_byte(dev, CMD, cmd | (1<<CMD_RX_ENABLE_SHIFT));
-
-}
-
-void rtl8185b_tx_enable(struct net_device *dev)
-{
-	u8 cmd;
-	u8 byte;
-	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
-
-	write_nic_dword(dev, TCR, priv->TransmitConfig);
-	byte = read_nic_byte(dev, MSR);
-	byte |= MSR_LINK_ENEDCA;
-	write_nic_byte(dev, MSR, byte);
-
-	fix_tx_fifo(dev);
-
-	cmd = read_nic_byte(dev, CMD);
-	write_nic_byte(dev, CMD, cmd | (1<<CMD_TX_ENABLE_SHIFT));
-}
-
diff --git a/drivers/staging/rtl8188eu/core/rtw_ap.c b/drivers/staging/rtl8188eu/core/rtw_ap.c
index ff74d0d7efa..85fda6128db 100644
--- a/drivers/staging/rtl8188eu/core/rtw_ap.c
+++ b/drivers/staging/rtl8188eu/core/rtw_ap.c
@@ -1306,6 +1306,10 @@ static void update_bcn_wps_ie(struct adapter *padapter)
 
 	DBG_88E("%s\n", __func__);
 
+	pwps_ie_src = pmlmepriv->wps_beacon_ie;
+	if (pwps_ie_src == NULL)
+		return;
+
 	pwps_ie = rtw_get_wps_ie(ie+_FIXED_IE_LENGTH_, ielen-_FIXED_IE_LENGTH_, NULL, &wps_ielen);
 
 	if (pwps_ie == NULL || wps_ielen == 0)
@@ -1323,10 +1327,6 @@ static void update_bcn_wps_ie(struct adapter *padapter)
 			memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen);
 	}
 
-	pwps_ie_src = pmlmepriv->wps_beacon_ie;
-	if (pwps_ie_src == NULL)
-		return;
-
 	wps_ielen = (uint)pwps_ie_src[1];/* to get ie data len */
 	if ((wps_offset+wps_ielen+2+remainder_ielen) <= MAX_IE_SZ) {
 		memcpy(pwps_ie, pwps_ie_src, wps_ielen+2);
diff --git a/drivers/staging/rtl8188eu/core/rtw_br_ext.c b/drivers/staging/rtl8188eu/core/rtw_br_ext.c
index e843c6bd452..f97f05f4165 100644
--- a/drivers/staging/rtl8188eu/core/rtw_br_ext.c
+++ b/drivers/staging/rtl8188eu/core/rtw_br_ext.c
@@ -442,6 +442,7 @@ static void __nat25_db_print(struct adapter *priv)
 void nat25_db_cleanup(struct adapter *priv)
 {
 	int i;
+
 	spin_lock_bh(&priv->br_ext_lock);
 
 	for (i = 0; i < NAT25_HASH_SIZE; i++) {
diff --git a/drivers/staging/rtl8188eu/core/rtw_cmd.c b/drivers/staging/rtl8188eu/core/rtw_cmd.c
index c2fb050337d..1e0b8b49fc1 100644
--- a/drivers/staging/rtl8188eu/core/rtw_cmd.c
+++ b/drivers/staging/rtl8188eu/core/rtw_cmd.c
@@ -1431,41 +1431,6 @@ exit:
 	return res;
 }
 
-u8 rtw_led_blink_cmd(struct adapter *padapter, struct LED_871x *pLed)
-{
-	struct	cmd_obj *pcmdobj;
-	struct	LedBlink_param *ledBlink_param;
-	struct	cmd_priv   *pcmdpriv = &padapter->cmdpriv;
-
-	u8	res = _SUCCESS;
-
-
-	RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_led_blink_cmd\n"));
-
-	pcmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(struct	cmd_obj));
-	if (pcmdobj == NULL) {
-		res = _FAIL;
-		goto exit;
-	}
-
-	ledBlink_param = (struct	LedBlink_param *)rtw_zmalloc(sizeof(struct	LedBlink_param));
-	if (ledBlink_param == NULL) {
-		kfree(pcmdobj);
-		res = _FAIL;
-		goto exit;
-	}
-
-	ledBlink_param->pLed = pLed;
-
-	init_h2fwcmd_w_parm_no_rsp(pcmdobj, ledBlink_param, GEN_CMD_CODE(_LedBlink));
-	res = rtw_enqueue_cmd(pcmdpriv, pcmdobj);
-
-exit:
-
-
-	return res;
-}
-
 u8 rtw_set_csa_cmd(struct adapter *padapter, u8 new_ch_no)
 {
 	struct	cmd_obj *pcmdobj;
diff --git a/drivers/staging/rtl8188eu/core/rtw_led.c b/drivers/staging/rtl8188eu/core/rtw_led.c
index 42b41ab1bce..87d6f063476 100644
--- a/drivers/staging/rtl8188eu/core/rtw_led.c
+++ b/drivers/staging/rtl8188eu/core/rtw_led.c
@@ -40,7 +40,6 @@ void BlinkTimerCallback(void *data)
 /*  */
 /*	Description: */
 /*		Callback function of LED BlinkWorkItem. */
-/*		We dispatch acture LED blink action according to LedStrategy. */
 /*  */
 void BlinkWorkItemCallback(struct work_struct *work)
 {
@@ -71,10 +70,9 @@ void ResetLedStatus(struct LED_871x *pLed)
 
 /*Description: */
 /*		Initialize an LED_871x object. */
-void InitLed871x(struct adapter *padapter, struct LED_871x *pLed, enum LED_PIN_871x LedPin)
+void InitLed871x(struct adapter *padapter, struct LED_871x *pLed)
 {
 	pLed->padapter = padapter;
-	pLed->LedPin = LedPin;
 
 	ResetLedStatus(pLed);
 
@@ -101,87 +99,6 @@ void DeInitLed871x(struct LED_871x *pLed)
 /*		It toggle off LED and schedule corresponding timer if necessary. */
 /*  */
 
-static void SwLedBlink(struct LED_871x *pLed)
-{
-	struct adapter *padapter = pLed->padapter;
-	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
-	u8 bStopBlinking = false;
-
-	/*  Change LED according to BlinkingLedState specified. */
-	if (pLed->BlinkingLedState == RTW_LED_ON) {
-		SwLedOn(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn on\n", pLed->BlinkTimes));
-	} else {
-		SwLedOff(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn off\n", pLed->BlinkTimes));
-	}
-
-	/*  Determine if we shall change LED state again. */
-	pLed->BlinkTimes--;
-	switch (pLed->CurrLedState) {
-	case LED_BLINK_NORMAL:
-		if (pLed->BlinkTimes == 0)
-			bStopBlinking = true;
-		break;
-	case LED_BLINK_StartToBlink:
-		if (check_fwstate(pmlmepriv, _FW_LINKED) && check_fwstate(pmlmepriv, WIFI_STATION_STATE))
-			bStopBlinking = true;
-		if (check_fwstate(pmlmepriv, _FW_LINKED) &&
-		    (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) ||
-		    check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)))
-			bStopBlinking = true;
-		else if (pLed->BlinkTimes == 0)
-			bStopBlinking = true;
-		break;
-	case LED_BLINK_WPS:
-		if (pLed->BlinkTimes == 0)
-			bStopBlinking = true;
-		break;
-	default:
-		bStopBlinking = true;
-		break;
-	}
-
-	if (bStopBlinking) {
-		/* if (padapter->pwrctrlpriv.cpwm >= PS_STATE_S2) */
-		if (0) {
-			SwLedOff(padapter, pLed);
-		} else if ((check_fwstate(pmlmepriv, _FW_LINKED)) && (!pLed->bLedOn)) {
-			SwLedOn(padapter, pLed);
-		} else if ((check_fwstate(pmlmepriv, _FW_LINKED)) &&  pLed->bLedOn) {
-			SwLedOff(padapter, pLed);
-		}
-		pLed->BlinkTimes = 0;
-		pLed->bLedBlinkInProgress = false;
-	} else {
-		/*  Assign LED state to toggle. */
-		if (pLed->BlinkingLedState == RTW_LED_ON)
-			pLed->BlinkingLedState = RTW_LED_OFF;
-		else
-			pLed->BlinkingLedState = RTW_LED_ON;
-
-		/*  Schedule a timer to toggle LED state. */
-		switch (pLed->CurrLedState) {
-		case LED_BLINK_NORMAL:
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
-			break;
-		case LED_BLINK_SLOWLY:
-		case LED_BLINK_StartToBlink:
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_SLOWLY_INTERVAL);
-			break;
-		case LED_BLINK_WPS:
-			if (pLed->BlinkingLedState == RTW_LED_ON)
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_LONG_INTERVAL);
-			else
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_LONG_INTERVAL);
-			break;
-		default:
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_SLOWLY_INTERVAL);
-			break;
-		}
-	}
-}
-
 static void SwLedBlink1(struct LED_871x *pLed)
 {
 	struct adapter *padapter = pLed->padapter;
@@ -319,449 +236,6 @@ static void SwLedBlink1(struct LED_871x *pLed)
 	}
 }
 
-static void SwLedBlink2(struct LED_871x *pLed)
-{
-	struct adapter *padapter = pLed->padapter;
-	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
-	u8 bStopBlinking = false;
-
-	/*  Change LED according to BlinkingLedState specified. */
-	if (pLed->BlinkingLedState == RTW_LED_ON) {
-		SwLedOn(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn on\n", pLed->BlinkTimes));
-	} else {
-		SwLedOff(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn off\n", pLed->BlinkTimes));
-	}
-
-	switch (pLed->CurrLedState) {
-	case LED_BLINK_SCAN:
-		pLed->BlinkTimes--;
-		if (pLed->BlinkTimes == 0)
-			bStopBlinking = true;
-		if (bStopBlinking) {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
-				SwLedOff(padapter, pLed);
-			} else if (check_fwstate(pmlmepriv, _FW_LINKED)) {
-				pLed->CurrLedState = RTW_LED_ON;
-				pLed->BlinkingLedState = RTW_LED_ON;
-				SwLedOn(padapter, pLed);
-				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("stop scan blink CurrLedState %d\n", pLed->CurrLedState));
-
-			} else if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
-				pLed->CurrLedState = RTW_LED_OFF;
-				pLed->BlinkingLedState = RTW_LED_OFF;
-				SwLedOff(padapter, pLed);
-				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("stop scan blink CurrLedState %d\n", pLed->CurrLedState));
-			}
-			pLed->bLedScanBlinkInProgress = false;
-		} else {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
-				SwLedOff(padapter, pLed);
-			} else {
-				 if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
-			}
-		}
-		break;
-	case LED_BLINK_TXRX:
-		pLed->BlinkTimes--;
-		if (pLed->BlinkTimes == 0)
-			bStopBlinking = true;
-		if (bStopBlinking) {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
-				SwLedOff(padapter, pLed);
-			} else if (check_fwstate(pmlmepriv, _FW_LINKED)) {
-				pLed->CurrLedState = RTW_LED_ON;
-				pLed->BlinkingLedState = RTW_LED_ON;
-				SwLedOn(padapter, pLed);
-				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("stop CurrLedState %d\n", pLed->CurrLedState));
-			} else if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
-				pLed->CurrLedState = RTW_LED_OFF;
-				pLed->BlinkingLedState = RTW_LED_OFF;
-				SwLedOff(padapter, pLed);
-				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("stop CurrLedState %d\n", pLed->CurrLedState));
-			}
-			pLed->bLedBlinkInProgress = false;
-		} else {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
-				SwLedOff(padapter, pLed);
-			} else {
-				 if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
-			}
-		}
-		break;
-	default:
-		break;
-	}
-}
-
-static void SwLedBlink3(struct LED_871x *pLed)
-{
-	struct adapter *padapter = pLed->padapter;
-	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
-	u8 bStopBlinking = false;
-
-	/*  Change LED according to BlinkingLedState specified. */
-	if (pLed->BlinkingLedState == RTW_LED_ON) {
-		SwLedOn(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn on\n", pLed->BlinkTimes));
-	} else {
-		if (pLed->CurrLedState != LED_BLINK_WPS_STOP)
-			SwLedOff(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn off\n", pLed->BlinkTimes));
-	}
-
-	switch (pLed->CurrLedState) {
-	case LED_BLINK_SCAN:
-		pLed->BlinkTimes--;
-		if (pLed->BlinkTimes == 0)
-			bStopBlinking = true;
-		if (bStopBlinking) {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
-				SwLedOff(padapter, pLed);
-			} else if (check_fwstate(pmlmepriv, _FW_LINKED)) {
-				pLed->CurrLedState = RTW_LED_ON;
-				pLed->BlinkingLedState = RTW_LED_ON;
-				if (!pLed->bLedOn)
-					SwLedOn(padapter, pLed);
-				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-			} else if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
-				pLed->CurrLedState = RTW_LED_OFF;
-				pLed->BlinkingLedState = RTW_LED_OFF;
-				if (pLed->bLedOn)
-					SwLedOff(padapter, pLed);
-				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-			}
-			pLed->bLedScanBlinkInProgress = false;
-		} else {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
-				SwLedOff(padapter, pLed);
-			} else {
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
-			}
-		}
-		break;
-	case LED_BLINK_TXRX:
-		pLed->BlinkTimes--;
-		if (pLed->BlinkTimes == 0)
-			bStopBlinking = true;
-		if (bStopBlinking) {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
-				SwLedOff(padapter, pLed);
-			} else if (check_fwstate(pmlmepriv, _FW_LINKED)) {
-				pLed->CurrLedState = RTW_LED_ON;
-				pLed->BlinkingLedState = RTW_LED_ON;
-				if (!pLed->bLedOn)
-					SwLedOn(padapter, pLed);
-				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-			} else if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
-				pLed->CurrLedState = RTW_LED_OFF;
-				pLed->BlinkingLedState = RTW_LED_OFF;
-
-				if (pLed->bLedOn)
-					SwLedOff(padapter, pLed);
-				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-			}
-			pLed->bLedBlinkInProgress = false;
-		} else {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
-				SwLedOff(padapter, pLed);
-			} else {
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
-			}
-		}
-		break;
-	case LED_BLINK_WPS:
-		if (pLed->bLedOn)
-			pLed->BlinkingLedState = RTW_LED_OFF;
-		else
-			pLed->BlinkingLedState = RTW_LED_ON;
-		_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
-		break;
-	case LED_BLINK_WPS_STOP:	/* WPS success */
-		if (pLed->BlinkingLedState == RTW_LED_ON) {
-			pLed->BlinkingLedState = RTW_LED_OFF;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA);
-			bStopBlinking = false;
-		} else {
-			bStopBlinking = true;
-		}
-		if (bStopBlinking) {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
-				SwLedOff(padapter, pLed);
-			} else {
-				pLed->CurrLedState = RTW_LED_ON;
-				pLed->BlinkingLedState = RTW_LED_ON;
-				SwLedOn(padapter, pLed);
-				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-			}
-			pLed->bLedWPSBlinkInProgress = false;
-		}
-		break;
-	default:
-		break;
-	}
-}
-
-static void SwLedBlink4(struct LED_871x *pLed)
-{
-	struct adapter *padapter = pLed->padapter;
-	struct led_priv *ledpriv = &(padapter->ledpriv);
-	struct LED_871x *pLed1 = &(ledpriv->SwLed1);
-	u8 bStopBlinking = false;
-
-	/*  Change LED according to BlinkingLedState specified. */
-	if (pLed->BlinkingLedState == RTW_LED_ON) {
-		SwLedOn(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn on\n", pLed->BlinkTimes));
-	} else {
-		SwLedOff(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn off\n", pLed->BlinkTimes));
-	}
-
-	if (!pLed1->bLedWPSBlinkInProgress && pLed1->BlinkingLedState == LED_UNKNOWN) {
-		pLed1->BlinkingLedState = RTW_LED_OFF;
-		pLed1->CurrLedState = RTW_LED_OFF;
-		SwLedOff(padapter, pLed1);
-	}
-
-	switch (pLed->CurrLedState) {
-	case LED_BLINK_SLOWLY:
-		if (pLed->bLedOn)
-			pLed->BlinkingLedState = RTW_LED_OFF;
-		else
-			pLed->BlinkingLedState = RTW_LED_ON;
-		_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
-		break;
-	case LED_BLINK_StartToBlink:
-		if (pLed->bLedOn) {
-			pLed->BlinkingLedState = RTW_LED_OFF;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_SLOWLY_INTERVAL);
-		} else {
-			pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
-		}
-		break;
-	case LED_BLINK_SCAN:
-		pLed->BlinkTimes--;
-		if (pLed->BlinkTimes == 0)
-			bStopBlinking = false;
-		if (bStopBlinking) {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
-				SwLedOff(padapter, pLed);
-			} else {
-				pLed->bLedNoLinkBlinkInProgress = false;
-				pLed->CurrLedState = LED_BLINK_SLOWLY;
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
-			}
-			pLed->bLedScanBlinkInProgress = false;
-		} else {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
-				SwLedOff(padapter, pLed);
-			} else {
-				 if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
-			}
-		}
-		break;
-	case LED_BLINK_TXRX:
-		pLed->BlinkTimes--;
-		if (pLed->BlinkTimes == 0)
-			bStopBlinking = true;
-		if (bStopBlinking) {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
-				SwLedOff(padapter, pLed);
-			} else {
-				pLed->bLedNoLinkBlinkInProgress = true;
-				pLed->CurrLedState = LED_BLINK_SLOWLY;
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
-			}
-			pLed->bLedBlinkInProgress = false;
-		} else {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
-				SwLedOff(padapter, pLed);
-			} else {
-				 if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
-			}
-		}
-		break;
-	case LED_BLINK_WPS:
-		if (pLed->bLedOn) {
-			pLed->BlinkingLedState = RTW_LED_OFF;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_SLOWLY_INTERVAL);
-		} else {
-			pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
-		}
-		break;
-	case LED_BLINK_WPS_STOP:	/* WPS authentication fail */
-		if (pLed->bLedOn)
-			pLed->BlinkingLedState = RTW_LED_OFF;
-		else
-			pLed->BlinkingLedState = RTW_LED_ON;
-
-		_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
-		break;
-	case LED_BLINK_WPS_STOP_OVERLAP:	/* WPS session overlap */
-		pLed->BlinkTimes--;
-		if (pLed->BlinkTimes == 0) {
-			if (pLed->bLedOn)
-				pLed->BlinkTimes = 1;
-			else
-				bStopBlinking = true;
-		}
-
-		if (bStopBlinking) {
-			pLed->BlinkTimes = 10;
-			pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_LINK_INTERVAL_ALPHA);
-		} else {
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
-		}
-		break;
-	default:
-		break;
-	}
-	RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("SwLedBlink4 CurrLedState %d\n", pLed->CurrLedState));
-}
-
-static void SwLedBlink5(struct LED_871x *pLed)
-{
-	struct adapter *padapter = pLed->padapter;
-	u8 bStopBlinking = false;
-
-	/*  Change LED according to BlinkingLedState specified. */
-	if (pLed->BlinkingLedState == RTW_LED_ON) {
-		SwLedOn(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn on\n", pLed->BlinkTimes));
-	} else {
-		SwLedOff(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn off\n", pLed->BlinkTimes));
-	}
-
-	switch (pLed->CurrLedState) {
-	case LED_BLINK_SCAN:
-		pLed->BlinkTimes--;
-		if (pLed->BlinkTimes == 0)
-			bStopBlinking = true;
-
-		if (bStopBlinking) {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
-				pLed->CurrLedState = RTW_LED_OFF;
-				pLed->BlinkingLedState = RTW_LED_OFF;
-				if (pLed->bLedOn)
-					SwLedOff(padapter, pLed);
-			} else {
-					pLed->CurrLedState = RTW_LED_ON;
-					pLed->BlinkingLedState = RTW_LED_ON;
-					if (!pLed->bLedOn)
-						_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
-			}
-
-			pLed->bLedScanBlinkInProgress = false;
-		} else {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
-				SwLedOff(padapter, pLed);
-			} else {
-				if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
-			}
-		}
-		break;
-	case LED_BLINK_TXRX:
-		pLed->BlinkTimes--;
-		if (pLed->BlinkTimes == 0)
-			bStopBlinking = true;
-
-		if (bStopBlinking) {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
-				pLed->CurrLedState = RTW_LED_OFF;
-				pLed->BlinkingLedState = RTW_LED_OFF;
-				if (pLed->bLedOn)
-					SwLedOff(padapter, pLed);
-			} else {
-				pLed->CurrLedState = RTW_LED_ON;
-				pLed->BlinkingLedState = RTW_LED_ON;
-				if (!pLed->bLedOn)
-					_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
-			}
-
-			pLed->bLedBlinkInProgress = false;
-		} else {
-			if (padapter->pwrctrlpriv.rf_pwrstate != rf_on && padapter->pwrctrlpriv.rfoff_reason > RF_CHANGE_BY_PS) {
-				SwLedOff(padapter, pLed);
-			} else {
-				 if (pLed->bLedOn)
-					pLed->BlinkingLedState = RTW_LED_OFF;
-				else
-					pLed->BlinkingLedState = RTW_LED_ON;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
-			}
-		}
-		break;
-
-	default:
-		break;
-	}
-
-	RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("SwLedBlink5 CurrLedState %d\n", pLed->CurrLedState));
-}
-
-static void SwLedBlink6(struct LED_871x *pLed)
-{
-	struct adapter *padapter = pLed->padapter;
-
-	/*  Change LED according to BlinkingLedState specified. */
-	if (pLed->BlinkingLedState == RTW_LED_ON) {
-		SwLedOn(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn on\n", pLed->BlinkTimes));
-	} else {
-		SwLedOff(padapter, pLed);
-		RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Blinktimes (%d): turn off\n", pLed->BlinkTimes));
-	}
-
-	RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("<==== blink6\n"));
-}
-
  /* ALPHA, added by chiyoko, 20090106 */
 static void SwLedControlMode1(struct adapter *padapter, enum LED_CTL_MODE LedAction)
 {
@@ -969,676 +443,18 @@ static void SwLedControlMode1(struct adapter *padapter, enum LED_CTL_MODE LedAct
 	RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Led %d\n", pLed->CurrLedState));
 }
 
- /* Arcadyan/Sitecom , added by chiyoko, 20090216 */
-static void SwLedControlMode2(struct adapter *padapter, enum LED_CTL_MODE LedAction)
-{
-	struct led_priv *ledpriv = &(padapter->ledpriv);
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct LED_871x *pLed = &(ledpriv->SwLed0);
-
-	switch (LedAction) {
-	case LED_CTL_SITE_SURVEY:
-		if (pmlmepriv->LinkDetectInfo.bBusyTraffic) {
-		} else if (!pLed->bLedScanBlinkInProgress) {
-			if (IS_LED_WPS_BLINKING(pLed))
-				return;
-
-			if (pLed->bLedBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedBlinkInProgress = false;
-			}
-			pLed->bLedScanBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_SCAN;
-			pLed->BlinkTimes = 24;
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
-		 }
-		break;
-	case LED_CTL_TX:
-	case LED_CTL_RX:
-		if ((!pLed->bLedBlinkInProgress) && (check_fwstate(pmlmepriv, _FW_LINKED))) {
-			if (pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))
-				return;
-			pLed->bLedBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_TXRX;
-			pLed->BlinkTimes = 2;
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
-		}
-		break;
-	case LED_CTL_LINK:
-		pLed->CurrLedState = RTW_LED_ON;
-		pLed->BlinkingLedState = RTW_LED_ON;
-		if (pLed->bLedBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedBlinkInProgress = false;
-		}
-		if (pLed->bLedScanBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedScanBlinkInProgress = false;
-		}
-		_set_timer(&(pLed->BlinkTimer), 0);
-		break;
-	case LED_CTL_START_WPS: /* wait until xinpin finish */
-	case LED_CTL_START_WPS_BOTTON:
-		if (!pLed->bLedWPSBlinkInProgress) {
-			if (pLed->bLedBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedBlinkInProgress = false;
-			}
-			if (pLed->bLedScanBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedScanBlinkInProgress = false;
-			}
-			pLed->bLedWPSBlinkInProgress = true;
-			pLed->CurrLedState = RTW_LED_ON;
-			pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), 0);
-		 }
-		break;
-	case LED_CTL_STOP_WPS:
-		pLed->bLedWPSBlinkInProgress = false;
-		if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
-			SwLedOff(padapter, pLed);
-		} else {
-			pLed->CurrLedState = RTW_LED_ON;
-			pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), 0);
-			RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-		}
-		break;
-	case LED_CTL_STOP_WPS_FAIL:
-		pLed->bLedWPSBlinkInProgress = false;
-		if (padapter->pwrctrlpriv.rf_pwrstate != rf_on) {
-			SwLedOff(padapter, pLed);
-		} else {
-			pLed->CurrLedState = RTW_LED_OFF;
-			pLed->BlinkingLedState = RTW_LED_OFF;
-			_set_timer(&(pLed->BlinkTimer), 0);
-			RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-		}
-		break;
-	case LED_CTL_START_TO_LINK:
-	case LED_CTL_NO_LINK:
-		if (!IS_LED_BLINKING(pLed)) {
-			pLed->CurrLedState = RTW_LED_OFF;
-			pLed->BlinkingLedState = RTW_LED_OFF;
-			_set_timer(&(pLed->BlinkTimer), 0);
-		}
-		break;
-	case LED_CTL_POWER_OFF:
-		pLed->CurrLedState = RTW_LED_OFF;
-		pLed->BlinkingLedState = RTW_LED_OFF;
-		if (pLed->bLedBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedBlinkInProgress = false;
-		}
-		if (pLed->bLedScanBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedScanBlinkInProgress = false;
-		}
-		if (pLed->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedWPSBlinkInProgress = false;
-		}
-
-		_set_timer(&(pLed->BlinkTimer), 0);
-		break;
-	default:
-		break;
-	}
-
-	RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-}
-
-  /* COREGA, added by chiyoko, 20090316 */
- static void SwLedControlMode3(struct adapter *padapter, enum LED_CTL_MODE LedAction)
-{
-	struct led_priv *ledpriv = &(padapter->ledpriv);
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct LED_871x *pLed = &(ledpriv->SwLed0);
-
-	switch (LedAction) {
-	case LED_CTL_SITE_SURVEY:
-		if (pmlmepriv->LinkDetectInfo.bBusyTraffic) {
-		} else if (!pLed->bLedScanBlinkInProgress) {
-			if (IS_LED_WPS_BLINKING(pLed))
-				return;
-
-			if (pLed->bLedBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedBlinkInProgress = false;
-			}
-			pLed->bLedScanBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_SCAN;
-			pLed->BlinkTimes = 24;
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
-		}
-		break;
-	case LED_CTL_TX:
-	case LED_CTL_RX:
-		if ((!pLed->bLedBlinkInProgress) && (check_fwstate(pmlmepriv, _FW_LINKED))) {
-			if (pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))
-				return;
-			pLed->bLedBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_TXRX;
-			pLed->BlinkTimes = 2;
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
-		}
-		break;
-	case LED_CTL_LINK:
-		if (IS_LED_WPS_BLINKING(pLed))
-			return;
-		pLed->CurrLedState = RTW_LED_ON;
-		pLed->BlinkingLedState = RTW_LED_ON;
-		if (pLed->bLedBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedBlinkInProgress = false;
-		}
-		if (pLed->bLedScanBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedScanBlinkInProgress = false;
-		}
-
-		_set_timer(&(pLed->BlinkTimer), 0);
-		break;
-	case LED_CTL_START_WPS: /* wait until xinpin finish */
-	case LED_CTL_START_WPS_BOTTON:
-		if (!pLed->bLedWPSBlinkInProgress) {
-			if (pLed->bLedBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedBlinkInProgress = false;
-			}
-			if (pLed->bLedScanBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedScanBlinkInProgress = false;
-			}
-			pLed->bLedWPSBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_WPS;
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
-		}
-		break;
-	case LED_CTL_STOP_WPS:
-		if (pLed->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedWPSBlinkInProgress = false;
-		} else {
-			pLed->bLedWPSBlinkInProgress = true;
-		}
-
-		pLed->CurrLedState = LED_BLINK_WPS_STOP;
-		if (pLed->bLedOn) {
-			pLed->BlinkingLedState = RTW_LED_OFF;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA);
-		} else {
-			pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), 0);
-		}
-		break;
-	case LED_CTL_STOP_WPS_FAIL:
-		if (pLed->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedWPSBlinkInProgress = false;
-		}
-		pLed->CurrLedState = RTW_LED_OFF;
-		pLed->BlinkingLedState = RTW_LED_OFF;
-		_set_timer(&(pLed->BlinkTimer), 0);
-		break;
-	case LED_CTL_START_TO_LINK:
-	case LED_CTL_NO_LINK:
-		if (!IS_LED_BLINKING(pLed)) {
-			pLed->CurrLedState = RTW_LED_OFF;
-			pLed->BlinkingLedState = RTW_LED_OFF;
-			_set_timer(&(pLed->BlinkTimer), 0);
-		}
-		break;
-	case LED_CTL_POWER_OFF:
-		pLed->CurrLedState = RTW_LED_OFF;
-		pLed->BlinkingLedState = RTW_LED_OFF;
-		if (pLed->bLedBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedBlinkInProgress = false;
-		}
-		if (pLed->bLedScanBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedScanBlinkInProgress = false;
-		}
-		if (pLed->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedWPSBlinkInProgress = false;
-		}
-
-		_set_timer(&(pLed->BlinkTimer), 0);
-		break;
-	default:
-		break;
-	}
-	RT_TRACE(_module_rtl8712_led_c_, _drv_info_,
-		 ("CurrLedState %d\n", pLed->CurrLedState));
-}
-
- /* Edimax-Belkin, added by chiyoko, 20090413 */
-static void SwLedControlMode4(struct adapter *padapter, enum LED_CTL_MODE LedAction)
-{
-	struct led_priv *ledpriv = &(padapter->ledpriv);
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct LED_871x *pLed = &(ledpriv->SwLed0);
-	struct LED_871x *pLed1 = &(ledpriv->SwLed1);
-
-	switch (LedAction) {
-	case LED_CTL_START_TO_LINK:
-		if (pLed1->bLedWPSBlinkInProgress) {
-			pLed1->bLedWPSBlinkInProgress = false;
-			_cancel_timer_ex(&(pLed1->BlinkTimer));
-
-			pLed1->BlinkingLedState = RTW_LED_OFF;
-			pLed1->CurrLedState = RTW_LED_OFF;
-
-			if (pLed1->bLedOn)
-				_set_timer(&(pLed->BlinkTimer), 0);
-		}
-
-		if (!pLed->bLedStartToLinkBlinkInProgress) {
-			if (pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))
-				return;
-			if (pLed->bLedBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedBlinkInProgress = false;
-			}
-			if (pLed->bLedNoLinkBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedNoLinkBlinkInProgress = false;
-			}
-
-			pLed->bLedStartToLinkBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_StartToBlink;
-			if (pLed->bLedOn) {
-				pLed->BlinkingLedState = RTW_LED_OFF;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_SLOWLY_INTERVAL);
-			} else {
-				pLed->BlinkingLedState = RTW_LED_ON;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
-			}
-		}
-		break;
-	case LED_CTL_LINK:
-	case LED_CTL_NO_LINK:
-		/* LED1 settings */
-		if (LedAction == LED_CTL_LINK) {
-			if (pLed1->bLedWPSBlinkInProgress) {
-				pLed1->bLedWPSBlinkInProgress = false;
-				_cancel_timer_ex(&(pLed1->BlinkTimer));
-
-				pLed1->BlinkingLedState = RTW_LED_OFF;
-				pLed1->CurrLedState = RTW_LED_OFF;
-
-				if (pLed1->bLedOn)
-					_set_timer(&(pLed->BlinkTimer), 0);
-			}
-		}
-
-		if (!pLed->bLedNoLinkBlinkInProgress) {
-			if (pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))
-				return;
-			if (pLed->bLedBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedBlinkInProgress = false;
-			}
-
-			pLed->bLedNoLinkBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_SLOWLY;
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
-		}
-		break;
-	case LED_CTL_SITE_SURVEY:
-		if ((pmlmepriv->LinkDetectInfo.bBusyTraffic) && (check_fwstate(pmlmepriv, _FW_LINKED))) {
-		} else if (!pLed->bLedScanBlinkInProgress) {
-			if (IS_LED_WPS_BLINKING(pLed))
-				return;
-
-			if (pLed->bLedNoLinkBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedNoLinkBlinkInProgress = false;
-			}
-			if (pLed->bLedBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedBlinkInProgress = false;
-			}
-			pLed->bLedScanBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_SCAN;
-			pLed->BlinkTimes = 24;
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
-		}
-		break;
-	case LED_CTL_TX:
-	case LED_CTL_RX:
-		if (!pLed->bLedBlinkInProgress) {
-			if (pLed->CurrLedState == LED_BLINK_SCAN || IS_LED_WPS_BLINKING(pLed))
-				return;
-			if (pLed->bLedNoLinkBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedNoLinkBlinkInProgress = false;
-			}
-			pLed->bLedBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_TXRX;
-			pLed->BlinkTimes = 2;
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
-		}
-		break;
-	case LED_CTL_START_WPS: /* wait until xinpin finish */
-	case LED_CTL_START_WPS_BOTTON:
-		if (pLed1->bLedWPSBlinkInProgress) {
-			pLed1->bLedWPSBlinkInProgress = false;
-			_cancel_timer_ex(&(pLed1->BlinkTimer));
-
-			pLed1->BlinkingLedState = RTW_LED_OFF;
-			pLed1->CurrLedState = RTW_LED_OFF;
-
-			if (pLed1->bLedOn)
-				_set_timer(&(pLed->BlinkTimer), 0);
-		}
-
-		if (!pLed->bLedWPSBlinkInProgress) {
-			if (pLed->bLedNoLinkBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedNoLinkBlinkInProgress = false;
-			}
-			if (pLed->bLedBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedBlinkInProgress = false;
-			}
-			if (pLed->bLedScanBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedScanBlinkInProgress = false;
-			}
-			pLed->bLedWPSBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_WPS;
-			if (pLed->bLedOn) {
-				pLed->BlinkingLedState = RTW_LED_OFF;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_SLOWLY_INTERVAL);
-			} else {
-				pLed->BlinkingLedState = RTW_LED_ON;
-				_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
-			}
-		}
-		break;
-	case LED_CTL_STOP_WPS:	/* WPS connect success */
-		if (pLed->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedWPSBlinkInProgress = false;
-		}
-
-		pLed->bLedNoLinkBlinkInProgress = true;
-		pLed->CurrLedState = LED_BLINK_SLOWLY;
-		if (pLed->bLedOn)
-			pLed->BlinkingLedState = RTW_LED_OFF;
-		else
-			pLed->BlinkingLedState = RTW_LED_ON;
-		_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
-
-		break;
-	case LED_CTL_STOP_WPS_FAIL:		/* WPS authentication fail */
-		if (pLed->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedWPSBlinkInProgress = false;
-		}
-		pLed->bLedNoLinkBlinkInProgress = true;
-		pLed->CurrLedState = LED_BLINK_SLOWLY;
-		if (pLed->bLedOn)
-			pLed->BlinkingLedState = RTW_LED_OFF;
-		else
-			pLed->BlinkingLedState = RTW_LED_ON;
-		_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
-
-		/* LED1 settings */
-		if (pLed1->bLedWPSBlinkInProgress)
-			_cancel_timer_ex(&(pLed1->BlinkTimer));
-		else
-			pLed1->bLedWPSBlinkInProgress = true;
-		pLed1->CurrLedState = LED_BLINK_WPS_STOP;
-		if (pLed1->bLedOn)
-			pLed1->BlinkingLedState = RTW_LED_OFF;
-		else
-			pLed1->BlinkingLedState = RTW_LED_ON;
-		_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
-		break;
-	case LED_CTL_STOP_WPS_FAIL_OVERLAP:	/* WPS session overlap */
-		if (pLed->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedWPSBlinkInProgress = false;
-		}
-		pLed->bLedNoLinkBlinkInProgress = true;
-		pLed->CurrLedState = LED_BLINK_SLOWLY;
-		if (pLed->bLedOn)
-			pLed->BlinkingLedState = RTW_LED_OFF;
-		else
-			pLed->BlinkingLedState = RTW_LED_ON;
-		_set_timer(&(pLed->BlinkTimer), LED_BLINK_NO_LINK_INTERVAL_ALPHA);
-
-		/* LED1 settings */
-		if (pLed1->bLedWPSBlinkInProgress)
-			_cancel_timer_ex(&(pLed1->BlinkTimer));
-		else
-			pLed1->bLedWPSBlinkInProgress = true;
-		pLed1->CurrLedState = LED_BLINK_WPS_STOP_OVERLAP;
-		pLed1->BlinkTimes = 10;
-		if (pLed1->bLedOn)
-			pLed1->BlinkingLedState = RTW_LED_OFF;
-		else
-			pLed1->BlinkingLedState = RTW_LED_ON;
-		_set_timer(&(pLed->BlinkTimer), LED_BLINK_NORMAL_INTERVAL);
-		break;
-	case LED_CTL_POWER_OFF:
-		pLed->CurrLedState = RTW_LED_OFF;
-		pLed->BlinkingLedState = RTW_LED_OFF;
-
-		if (pLed->bLedNoLinkBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedNoLinkBlinkInProgress = false;
-		}
-		if (pLed->bLedLinkBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedLinkBlinkInProgress = false;
-		}
-		if (pLed->bLedBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedBlinkInProgress = false;
-		}
-		if (pLed->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedWPSBlinkInProgress = false;
-		}
-		if (pLed->bLedScanBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedScanBlinkInProgress = false;
-		}
-		if (pLed->bLedStartToLinkBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedStartToLinkBlinkInProgress = false;
-		}
-		if (pLed1->bLedWPSBlinkInProgress) {
-			_cancel_timer_ex(&(pLed1->BlinkTimer));
-			pLed1->bLedWPSBlinkInProgress = false;
-		}
-		pLed1->BlinkingLedState = LED_UNKNOWN;
-		SwLedOff(padapter, pLed);
-		SwLedOff(padapter, pLed1);
-		break;
-	default:
-		break;
-	}
-
-	RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Led %d\n", pLed->CurrLedState));
-}
-
-
-
- /* Sercomm-Belkin, added by chiyoko, 20090415 */
-static void
-SwLedControlMode5(
-	struct adapter *padapter,
-	enum LED_CTL_MODE LedAction
-)
-{
-	struct led_priv *ledpriv = &(padapter->ledpriv);
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct LED_871x *pLed = &(ledpriv->SwLed0);
-
-	switch (LedAction) {
-	case LED_CTL_POWER_ON:
-	case LED_CTL_NO_LINK:
-	case LED_CTL_LINK:	/* solid blue */
-		pLed->CurrLedState = RTW_LED_ON;
-		pLed->BlinkingLedState = RTW_LED_ON;
-
-		_set_timer(&(pLed->BlinkTimer), 0);
-		break;
-	case LED_CTL_SITE_SURVEY:
-		if ((pmlmepriv->LinkDetectInfo.bBusyTraffic) && (check_fwstate(pmlmepriv, _FW_LINKED))) {
-		} else if (!pLed->bLedScanBlinkInProgress) {
-			if (pLed->bLedBlinkInProgress) {
-				_cancel_timer_ex(&(pLed->BlinkTimer));
-				pLed->bLedBlinkInProgress = false;
-			}
-			pLed->bLedScanBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_SCAN;
-			pLed->BlinkTimes = 24;
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_SCAN_INTERVAL_ALPHA);
-		}
-		break;
-	case LED_CTL_TX:
-	case LED_CTL_RX:
-		if (!pLed->bLedBlinkInProgress) {
-			if (pLed->CurrLedState == LED_BLINK_SCAN)
-				return;
-			pLed->bLedBlinkInProgress = true;
-			pLed->CurrLedState = LED_BLINK_TXRX;
-			pLed->BlinkTimes = 2;
-			if (pLed->bLedOn)
-				pLed->BlinkingLedState = RTW_LED_OFF;
-			else
-				pLed->BlinkingLedState = RTW_LED_ON;
-			_set_timer(&(pLed->BlinkTimer), LED_BLINK_FASTER_INTERVAL_ALPHA);
-		}
-		break;
-	case LED_CTL_POWER_OFF:
-		pLed->CurrLedState = RTW_LED_OFF;
-		pLed->BlinkingLedState = RTW_LED_OFF;
-
-		if (pLed->bLedBlinkInProgress) {
-			_cancel_timer_ex(&(pLed->BlinkTimer));
-			pLed->bLedBlinkInProgress = false;
-		}
-		SwLedOff(padapter, pLed);
-		break;
-	default:
-		break;
-	}
-
-	RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("Led %d\n", pLed->CurrLedState));
-}
-
- /* WNC-Corega, added by chiyoko, 20090902 */
-static void
-SwLedControlMode6(
-	struct adapter *padapter,
-	enum LED_CTL_MODE LedAction
-)
-{
-	struct led_priv *ledpriv = &(padapter->ledpriv);
-	struct LED_871x *pLed0 = &(ledpriv->SwLed0);
-
-	switch (LedAction) {
-	case LED_CTL_POWER_ON:
-	case LED_CTL_LINK:
-	case LED_CTL_NO_LINK:
-		_cancel_timer_ex(&(pLed0->BlinkTimer));
-		pLed0->CurrLedState = RTW_LED_ON;
-		pLed0->BlinkingLedState = RTW_LED_ON;
-		_set_timer(&(pLed0->BlinkTimer), 0);
-		break;
-	case LED_CTL_POWER_OFF:
-		SwLedOff(padapter, pLed0);
-		break;
-	default:
-		break;
-	}
-
-	RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("ledcontrol 6 Led %d\n", pLed0->CurrLedState));
-}
-
 /*  */
 /*	Description: */
 /*		Handler function of LED Blinking. */
-/*		We dispatch acture LED blink action according to LedStrategy. */
 /*  */
 void BlinkHandler(struct LED_871x *pLed)
 {
 	struct adapter *padapter = pLed->padapter;
-	struct led_priv *ledpriv = &(padapter->ledpriv);
 
 	if ((padapter->bSurpriseRemoved) || (padapter->bDriverStopped))
 		return;
 
-	switch (ledpriv->LedStrategy) {
-	case SW_LED_MODE0:
-		SwLedBlink(pLed);
-		break;
-	case SW_LED_MODE1:
-		SwLedBlink1(pLed);
-		break;
-	case SW_LED_MODE2:
-		SwLedBlink2(pLed);
-		break;
-	case SW_LED_MODE3:
-		SwLedBlink3(pLed);
-		break;
-	case SW_LED_MODE4:
-		SwLedBlink4(pLed);
-		break;
-	case SW_LED_MODE5:
-		SwLedBlink5(pLed);
-		break;
-	case SW_LED_MODE6:
-		SwLedBlink6(pLed);
-		break;
-	default:
-		break;
-	}
+	SwLedBlink1(pLed);
 }
 
 void LedControl8188eu(struct adapter *padapter, enum LED_CTL_MODE LedAction)
@@ -1661,32 +477,5 @@ void LedControl8188eu(struct adapter *padapter, enum LED_CTL_MODE LedAction)
 	     LedAction == LED_CTL_POWER_ON))
 		return;
 
-	switch (ledpriv->LedStrategy) {
-	case SW_LED_MODE0:
-		break;
-	case SW_LED_MODE1:
-		SwLedControlMode1(padapter, LedAction);
-		break;
-	case SW_LED_MODE2:
-		SwLedControlMode2(padapter, LedAction);
-		break;
-	case SW_LED_MODE3:
-		SwLedControlMode3(padapter, LedAction);
-		break;
-	case SW_LED_MODE4:
-		SwLedControlMode4(padapter, LedAction);
-		break;
-	case SW_LED_MODE5:
-		SwLedControlMode5(padapter, LedAction);
-		break;
-	case SW_LED_MODE6:
-		SwLedControlMode6(padapter, LedAction);
-		break;
-	default:
-		break;
-	}
-
-	RT_TRACE(_module_rtl8712_led_c_, _drv_info_,
-		 ("LedStrategy:%d, LedAction %d\n",
-		 ledpriv->LedStrategy, LedAction));
+	SwLedControlMode1(padapter, LedAction);
 }
diff --git a/drivers/staging/rtl8188eu/core/rtw_mlme.c b/drivers/staging/rtl8188eu/core/rtw_mlme.c
index 769d4ddc675..155282ef78f 100644
--- a/drivers/staging/rtl8188eu/core/rtw_mlme.c
+++ b/drivers/staging/rtl8188eu/core/rtw_mlme.c
@@ -1727,15 +1727,13 @@ int rtw_set_key(struct adapter *adapter, struct security_priv *psecuritypriv, in
 	int	res = _SUCCESS;
 
 	pcmd = (struct	cmd_obj *)rtw_zmalloc(sizeof(struct	cmd_obj));
-	if (pcmd == NULL) {
-		res = _FAIL;  /* try again */
-		goto exit;
-	}
+	if (pcmd == NULL)
+		return _FAIL;  /* try again */
+
 	psetkeyparm = (struct setkey_parm *)rtw_zmalloc(sizeof(struct setkey_parm));
 	if (psetkeyparm == NULL) {
-		kfree(pcmd);
 		res = _FAIL;
-		goto exit;
+		goto err_free_cmd;
 	}
 
 	_rtw_memset(psetkeyparm, 0, sizeof(struct setkey_parm));
@@ -1784,7 +1782,7 @@ int rtw_set_key(struct adapter *adapter, struct security_priv *psecuritypriv, in
 			 ("\n rtw_set_key:psecuritypriv->dot11PrivacyAlgrthm=%x (must be 1 or 2 or 4 or 5)\n",
 			 psecuritypriv->dot11PrivacyAlgrthm));
 		res = _FAIL;
-		goto exit;
+		goto err_free_parm;
 	}
 	pcmd->cmdcode = _SetKey_CMD_;
 	pcmd->parmbuf = (u8 *)psetkeyparm;
@@ -1793,7 +1791,12 @@ int rtw_set_key(struct adapter *adapter, struct security_priv *psecuritypriv, in
 	pcmd->rspsz = 0;
 	_rtw_init_listhead(&pcmd->list);
 	res = rtw_enqueue_cmd(pcmdpriv, pcmd);
-exit:
+	return res;
+
+err_free_parm:
+	kfree(psetkeyparm);
+err_free_cmd:
+	kfree(pcmd);
 	return res;
 }
 
diff --git a/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c b/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c
index 3ed5941bedc..f5b49f351a6 100644
--- a/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c
+++ b/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c
@@ -502,8 +502,6 @@ void mgt_dispatcher(struct adapter *padapter, struct recv_frame *precv_frame)
 		break;
 	default:
 		_mgt_dispatcher(padapter, ptable, precv_frame);
-		if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
-			rtw_hostapd_mlme_rx(padapter, precv_frame);
 		break;
 	}
 #else
@@ -6553,7 +6551,6 @@ u8 collect_bss_info(struct adapter *padapter, struct recv_frame *precv_frame, st
 	struct registry_priv	*pregistrypriv = &padapter->registrypriv;
 	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
-	__le32 le32_tmp;
 
 	len = packet_len - sizeof(struct rtw_ieee80211_hdr_3addr);
 
@@ -6600,13 +6597,13 @@ u8 collect_bss_info(struct adapter *padapter, struct recv_frame *precv_frame, st
 		return _FAIL;
 	}
 
-	if (*(p + 1)) {
+	if (len) {
 		if (len > NDIS_802_11_LENGTH_SSID) {
 			DBG_88E("%s()-%d: IE too long (%d) for survey event\n", __func__, __LINE__, len);
 			return _FAIL;
 		}
-		memcpy(bssid->Ssid.Ssid, (p + 2), *(p + 1));
-		bssid->Ssid.SsidLength = *(p + 1);
+		memcpy(bssid->Ssid.Ssid, (p + 2), len);
+		bssid->Ssid.SsidLength = len;
 	} else {
 		bssid->Ssid.SsidLength = 0;
 	}
@@ -6667,8 +6664,8 @@ u8 collect_bss_info(struct adapter *padapter, struct recv_frame *precv_frame, st
 		return _SUCCESS;
 	}
 
-	memcpy(&le32_tmp, rtw_get_beacon_interval_from_ie(bssid->IEs), 2);
-	bssid->Configuration.BeaconPeriod = le32_to_cpu(le32_tmp);
+	bssid->Configuration.BeaconPeriod =
+		get_unaligned_le16(rtw_get_beacon_interval_from_ie(bssid->IEs));
 
 	val16 = rtw_get_capability((struct wlan_bssid_ex *)bssid);
 
diff --git a/drivers/staging/rtl8188eu/core/rtw_mp.c b/drivers/staging/rtl8188eu/core/rtw_mp.c
index 705f666bca6..17427a68b66 100644
--- a/drivers/staging/rtl8188eu/core/rtw_mp.c
+++ b/drivers/staging/rtl8188eu/core/rtw_mp.c
@@ -21,7 +21,6 @@
 
 #include <drv_types.h>
 
-#include "odm_precomp.h"
 #include "rtl8188e_hal.h"
 #include <linux/vmalloc.h>
 
diff --git a/drivers/staging/rtl8188eu/core/rtw_p2p.c b/drivers/staging/rtl8188eu/core/rtw_p2p.c
index 9425c4991cc..0a15f8cf0d3 100644
--- a/drivers/staging/rtl8188eu/core/rtw_p2p.c
+++ b/drivers/staging/rtl8188eu/core/rtw_p2p.c
@@ -1505,9 +1505,9 @@ static void find_phase_handler(struct adapter *padapter)
 
 	rtw_p2p_set_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH);
 
-        spin_lock_bh(&pmlmepriv->lock);
-        rtw_sitesurvey_cmd(padapter, &ssid, 1, NULL, 0);
-        spin_unlock_bh(&pmlmepriv->lock);
+	spin_lock_bh(&pmlmepriv->lock);
+	rtw_sitesurvey_cmd(padapter, &ssid, 1, NULL, 0);
+	spin_unlock_bh(&pmlmepriv->lock);
 }
 
 void p2p_concurrent_handler(struct adapter *padapter);
diff --git a/drivers/staging/rtl8188eu/core/rtw_pwrctrl.c b/drivers/staging/rtl8188eu/core/rtw_pwrctrl.c
index f6583734aef..739e2504169 100644
--- a/drivers/staging/rtl8188eu/core/rtw_pwrctrl.c
+++ b/drivers/staging/rtl8188eu/core/rtw_pwrctrl.c
@@ -122,7 +122,7 @@ static bool rtw_pwr_unassociated_idle(struct adapter *adapter)
 
 	bool ret = false;
 
-	if (adapter->pwrctrlpriv.ips_deny_time >= jiffies)
+	if (time_after_eq(adapter->pwrctrlpriv.ips_deny_time, jiffies))
 		goto exit;
 
 	if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE|WIFI_SITE_MONITOR) ||
@@ -497,15 +497,6 @@ void rtw_init_pwrctrl_priv(struct adapter *padapter)
 	_init_timer(&(pwrctrlpriv->pwr_state_check_timer), padapter->pnetdev, pwr_state_check_handler, (u8 *)padapter);
 }
 
-u8 rtw_interface_ps_func(struct adapter *padapter, enum hal_intf_ps_func efunc_id, u8 *val)
-{
-	u8 bResult = true;
-	rtw_hal_intf_ps_func(padapter, efunc_id, val);
-
-	return bResult;
-}
-
-
 inline void rtw_set_ips_deny(struct adapter *padapter, u32 ms)
 {
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
@@ -523,9 +514,11 @@ int _rtw_pwr_wakeup(struct adapter *padapter, u32 ips_deffer_ms, const char *cal
 {
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	unsigned long expires;
 	int ret = _SUCCESS;
 
-	if (pwrpriv->ips_deny_time < jiffies + rtw_ms_to_systime(ips_deffer_ms))
+	expires = jiffies + rtw_ms_to_systime(ips_deffer_ms);
+	if (time_before(pwrpriv->ips_deny_time, expires))
 		pwrpriv->ips_deny_time = jiffies + rtw_ms_to_systime(ips_deffer_ms);
 
 {
@@ -580,7 +573,8 @@ int _rtw_pwr_wakeup(struct adapter *padapter, u32 ips_deffer_ms, const char *cal
 	}
 
 exit:
-	if (pwrpriv->ips_deny_time < jiffies + rtw_ms_to_systime(ips_deffer_ms))
+	expires = jiffies + rtw_ms_to_systime(ips_deffer_ms);
+	if (time_before(pwrpriv->ips_deny_time, expires))
 		pwrpriv->ips_deny_time = jiffies + rtw_ms_to_systime(ips_deffer_ms);
 	return ret;
 }
diff --git a/drivers/staging/rtl8188eu/core/rtw_recv.c b/drivers/staging/rtl8188eu/core/rtw_recv.c
index 636ec553ae8..0e73df5975b 100644
--- a/drivers/staging/rtl8188eu/core/rtw_recv.c
+++ b/drivers/staging/rtl8188eu/core/rtw_recv.c
@@ -63,8 +63,6 @@ int _rtw_init_recv_priv(struct recv_priv *precvpriv, struct adapter *padapter)
 
 	int	res = _SUCCESS;
 
-	spin_lock_init(&precvpriv->lock);
-
 	_rtw_init_queue(&precvpriv->free_recv_queue);
 	_rtw_init_queue(&precvpriv->recv_pending_queue);
 	_rtw_init_queue(&precvpriv->uc_swdec_pending_queue);
@@ -73,8 +71,6 @@ int _rtw_init_recv_priv(struct recv_priv *precvpriv, struct adapter *padapter)
 
 	precvpriv->free_recvframe_cnt = NR_RECVFRAME;
 
-	rtw_os_recv_resource_init(precvpriv, padapter);
-
 	precvpriv->pallocated_frame_buf = vzalloc(NR_RECVFRAME * sizeof(struct recv_frame) + RXFRAME_ALIGN_SZ);
 
 	if (precvpriv->pallocated_frame_buf == NULL) {
@@ -101,8 +97,6 @@ int _rtw_init_recv_priv(struct recv_priv *precvpriv, struct adapter *padapter)
 	}
 	precvpriv->rx_pending_cnt = 1;
 
-	sema_init(&precvpriv->allrxreturnevt, 0);
-
 	res = rtw_hal_init_recv_priv(padapter);
 
 	_init_timer(&precvpriv->signal_stat_timer, padapter->pnetdev, RTW_TIMER_HDL_NAME(signal_stat), padapter);
@@ -123,8 +117,6 @@ void _rtw_free_recv_priv (struct recv_priv *precvpriv)
 
 	rtw_free_uc_swdec_pending_queue(padapter);
 
-	rtw_os_recv_resource_free(precvpriv);
-
 	if (precvpriv->pallocated_frame_buf) {
 		vfree(precvpriv->pallocated_frame_buf);
 	}
@@ -289,55 +281,6 @@ u32 rtw_free_uc_swdec_pending_queue(struct adapter *adapter)
 	return cnt;
 }
 
-int rtw_enqueue_recvbuf_to_head(struct recv_buf *precvbuf, struct __queue *queue)
-{
-	spin_lock_bh(&queue->lock);
-
-	rtw_list_delete(&precvbuf->list);
-	rtw_list_insert_head(&precvbuf->list, get_list_head(queue));
-
-	spin_unlock_bh(&queue->lock);
-
-	return _SUCCESS;
-}
-
-int rtw_enqueue_recvbuf(struct recv_buf *precvbuf, struct __queue *queue)
-{
-	unsigned long irqL;
-	spin_lock_irqsave(&queue->lock, irqL);
-
-	rtw_list_delete(&precvbuf->list);
-
-	rtw_list_insert_tail(&precvbuf->list, get_list_head(queue));
-	spin_unlock_irqrestore(&queue->lock, irqL);
-	return _SUCCESS;
-}
-
-struct recv_buf *rtw_dequeue_recvbuf (struct __queue *queue)
-{
-	unsigned long irqL;
-	struct recv_buf *precvbuf;
-	struct list_head *plist, *phead;
-
-	spin_lock_irqsave(&queue->lock, irqL);
-
-	if (_rtw_queue_empty(queue)) {
-		precvbuf = NULL;
-	} else {
-		phead = get_list_head(queue);
-
-		plist = phead->next;
-
-		precvbuf = container_of(plist, struct recv_buf, list);
-
-		rtw_list_delete(&precvbuf->list);
-	}
-
-	spin_unlock_irqrestore(&queue->lock, irqL);
-
-	return precvbuf;
-}
-
 static int recvframe_chkmic(struct adapter *adapter,
 			    struct recv_frame *precvframe)
 {
@@ -545,20 +488,19 @@ static struct recv_frame *decryptor(struct adapter *padapter,
 static struct recv_frame *portctrl(struct adapter *adapter,
 				   struct recv_frame *precv_frame)
 {
-	u8   *psta_addr = NULL, *ptr;
+	u8   *psta_addr, *ptr;
 	uint  auth_alg;
 	struct recv_frame *pfhdr;
 	struct sta_info *psta;
 	struct sta_priv *pstapriv;
 	struct recv_frame *prtnframe;
-	u16	ether_type = 0;
+	u16	ether_type;
 	u16  eapol_type = 0x888e;/* for Funia BD's WPA issue */
 	struct rx_pkt_attrib *pattrib;
 	__be16 be_tmp;
 
 
 	pstapriv = &adapter->stapriv;
-	psta = rtw_get_stainfo(pstapriv, psta_addr);
 
 	auth_alg = adapter->securitypriv.dot11AuthAlgrthm;
 
@@ -566,24 +508,23 @@ static struct recv_frame *portctrl(struct adapter *adapter,
 	pfhdr = precv_frame;
 	pattrib = &pfhdr->attrib;
 	psta_addr = pattrib->ta;
+	psta = rtw_get_stainfo(pstapriv, psta_addr);
 
 	prtnframe = NULL;
 
 	RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("########portctrl:adapter->securitypriv.dot11AuthAlgrthm=%d\n", adapter->securitypriv.dot11AuthAlgrthm));
 
 	if (auth_alg == 2) {
+		/* get ether_type */
+		ptr = ptr + pfhdr->attrib.hdrlen + LLC_HEADER_SIZE;
+		memcpy(&be_tmp, ptr, 2);
+		ether_type = ntohs(be_tmp);
+
 		if ((psta != NULL) && (psta->ieee8021x_blocked)) {
 			/* blocked */
 			/* only accept EAPOL frame */
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("########portctrl:psta->ieee8021x_blocked==1\n"));
 
-			prtnframe = precv_frame;
-
-			/* get ether_type */
-			ptr = ptr+pfhdr->attrib.hdrlen+pfhdr->attrib.iv_len+LLC_HEADER_SIZE;
-			memcpy(&be_tmp, ptr, 2);
-			ether_type = ntohs(be_tmp);
-
 			if (ether_type == eapol_type) {
 				prtnframe = precv_frame;
 			} else {
@@ -650,8 +591,8 @@ static int recv_decache(struct recv_frame *precv_frame, u8 bretry,
 	return _SUCCESS;
 }
 
-void process_pwrbit_data(struct adapter *padapter,
-			 struct recv_frame *precv_frame)
+static void process_pwrbit_data(struct adapter *padapter,
+				struct recv_frame *precv_frame)
 {
 #ifdef CONFIG_88EU_AP_MODE
 	unsigned char pwrbit;
@@ -1825,8 +1766,8 @@ static int check_indicate_seq(struct recv_reorder_ctrl *preorder_ctrl, u16 seq_n
 	return true;
 }
 
-int enqueue_reorder_recvframe(struct recv_reorder_ctrl *preorder_ctrl,
-			      struct recv_frame *prframe)
+static int enqueue_reorder_recvframe(struct recv_reorder_ctrl *preorder_ctrl,
+				     struct recv_frame *prframe)
 {
 	struct rx_pkt_attrib *pattrib = &prframe->attrib;
 	struct __queue *ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
@@ -1874,7 +1815,7 @@ static int recv_indicatepkts_in_order(struct adapter *padapter, struct recv_reor
 			return true;
 
 		prhdr = container_of(plist, struct recv_frame, list);
-	        pattrib = &prhdr->attrib;
+		pattrib = &prhdr->attrib;
 		preorder_ctrl->indicate_seq = pattrib->seq_num;
 	}
 
diff --git a/drivers/staging/rtl8188eu/core/rtw_security.c b/drivers/staging/rtl8188eu/core/rtw_security.c
index c4b16ea6348..05335959b54 100644
--- a/drivers/staging/rtl8188eu/core/rtw_security.c
+++ b/drivers/staging/rtl8188eu/core/rtw_security.c
@@ -1153,7 +1153,7 @@ static int aes_cipher(u8 *key, uint hdrlen, u8 *pframe, uint plen)
 
 	/* Insert MIC into payload */
 	for (j = 0; j < 8; j++)
-	pframe[payload_index+j] = mic[j];	/* message[payload_index+j] = mic[j]; */
+		pframe[payload_index+j] = mic[j];
 
 	payload_index = hdrlen + 8;
 	for (i = 0; i < num_blocks; i++) {
diff --git a/drivers/staging/rtl8188eu/core/rtw_wlan_util.c b/drivers/staging/rtl8188eu/core/rtw_wlan_util.c
index 3dd90599fd4..6fb8caa94ab 100644
--- a/drivers/staging/rtl8188eu/core/rtw_wlan_util.c
+++ b/drivers/staging/rtl8188eu/core/rtw_wlan_util.c
@@ -1599,13 +1599,18 @@ int update_sta_support_rate(struct adapter *padapter, u8 *pvar_ie, uint var_ie_l
 	pIE = (struct ndis_802_11_var_ie *)rtw_get_ie(pvar_ie, _SUPPORTEDRATES_IE_, &ie_len, var_ie_len);
 	if (pIE == NULL)
 		return _FAIL;
+	if (ie_len > NDIS_802_11_LENGTH_RATES_EX)
+		return _FAIL;
 
 	memcpy(pmlmeinfo->FW_sta_info[cam_idx].SupportedRates, pIE->data, ie_len);
 	supportRateNum = ie_len;
 
 	pIE = (struct ndis_802_11_var_ie *)rtw_get_ie(pvar_ie, _EXT_SUPPORTEDRATES_IE_, &ie_len, var_ie_len);
-	if (pIE)
+	if (pIE) {
+		if (supportRateNum + ie_len > NDIS_802_11_LENGTH_RATES_EX)
+			return _FAIL;
 		memcpy((pmlmeinfo->FW_sta_info[cam_idx].SupportedRates + supportRateNum), pIE->data, ie_len);
+	}
 
 	return _SUCCESS;
 }
@@ -1656,26 +1661,3 @@ void beacon_timing_control(struct adapter *padapter)
 {
 	rtw_hal_bcn_related_reg_setting(padapter);
 }
-
-static struct adapter *pbuddy_padapter;
-
-int rtw_handle_dualmac(struct adapter *adapter, bool init)
-{
-	int status = _SUCCESS;
-
-	if (init) {
-		if (pbuddy_padapter == NULL) {
-			pbuddy_padapter = adapter;
-			DBG_88E("%s(): pbuddy_padapter == NULL, Set pbuddy_padapter\n", __func__);
-		} else {
-			adapter->pbuddy_adapter = pbuddy_padapter;
-			pbuddy_padapter->pbuddy_adapter = adapter;
-			/*  clear global value */
-			pbuddy_padapter = NULL;
-			DBG_88E("%s(): pbuddy_padapter exist, Exchange Information\n", __func__);
-		}
-	} else {
-		pbuddy_padapter = NULL;
-	}
-	return status;
-}
diff --git a/drivers/staging/rtl8188eu/core/rtw_xmit.c b/drivers/staging/rtl8188eu/core/rtw_xmit.c
index 8d4265fb486..1413ec8ad3b 100644
--- a/drivers/staging/rtl8188eu/core/rtw_xmit.c
+++ b/drivers/staging/rtl8188eu/core/rtw_xmit.c
@@ -621,8 +621,6 @@ static s32 update_attrib(struct adapter *padapter, struct sk_buff *pkt, struct p
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("update_attrib: bswenc = false\n"));
 	}
 
-	rtw_set_tx_chksum_offload(pkt, pattrib);
-
 	update_attrib_phy_info(pattrib, psta);
 
 exit:
diff --git a/drivers/staging/rtl8188eu/hal/HalPhyRf_8188e.c b/drivers/staging/rtl8188eu/hal/HalPhyRf_8188e.c
index 056052ddd82..7c22658ed0f 100644
--- a/drivers/staging/rtl8188eu/hal/HalPhyRf_8188e.c
+++ b/drivers/staging/rtl8188eu/hal/HalPhyRf_8188e.c
@@ -804,24 +804,12 @@ static void pathb_fill_iqk(struct adapter *adapt, bool iqkok, s32 result[][8], u
 	}
 }
 
-/*  */
-/*  2011/07/26 MH Add an API for testing IQK fail case. */
-/*  */
-/*  MP Already declare in odm.c */
-static bool ODM_CheckPowerStatus(struct adapter *Adapter)
-{
-	return	true;
-}
-
 void _PHY_SaveADDARegisters(struct adapter *adapt, u32 *ADDAReg, u32 *ADDABackup, u32 RegisterNum)
 {
 	u32 i;
 	struct hal_data_8188e	*pHalData = GET_HAL_DATA(adapt);
 	struct odm_dm_struct *dm_odm = &pHalData->odmpriv;
 
-	if (!ODM_CheckPowerStatus(adapt))
-		return;
-
 	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Save ADDA parameters.\n"));
 	for (i = 0; i < RegisterNum; i++) {
 		ADDABackup[i] = PHY_QueryBBReg(adapt, ADDAReg[i], bMaskDWord);
@@ -1294,8 +1282,6 @@ void PHY_IQCalibrate_8188E(struct adapter *adapt, bool recovery)
 	bool is2t;
 
 	is2t = (dm_odm->RFType == ODM_2T2R) ? true : false;
-	if (!ODM_CheckPowerStatus(adapt))
-		return;
 
 	if (!(dm_odm->SupportAbility & ODM_RF_CALIBRATION))
 		return;
diff --git a/drivers/staging/rtl8188eu/hal/hal_intf.c b/drivers/staging/rtl8188eu/hal/hal_intf.c
index d75ca7a27c8..d0ac4a18333 100644
--- a/drivers/staging/rtl8188eu/hal/hal_intf.c
+++ b/drivers/staging/rtl8188eu/hal/hal_intf.c
@@ -60,13 +60,6 @@ void rtw_hal_dm_init(struct adapter *adapt)
 		adapt->HalFunc.dm_init(adapt);
 }
 
-void rtw_hal_dm_deinit(struct adapter *adapt)
-{
-	/*  cancel dm  timer */
-	if (adapt->HalFunc.dm_deinit)
-		adapt->HalFunc.dm_deinit(adapt);
-}
-
 void rtw_hal_sw_led_init(struct adapter *adapt)
 {
 	if (adapt->HalFunc.InitSwLeds)
@@ -207,15 +200,6 @@ u32 rtw_hal_inirp_deinit(struct adapter *adapt)
 	return _FAIL;
 }
 
-u8 rtw_hal_intf_ps_func(struct adapter *adapt,
-			enum hal_intf_ps_func efunc_id, u8 *val)
-{
-	if (adapt->HalFunc.interface_ps_func)
-		return adapt->HalFunc.interface_ps_func(adapt, efunc_id,
-							   val);
-	return _FAIL;
-}
-
 s32 rtw_hal_xmit(struct adapter *adapt, struct xmit_frame *pxmitframe)
 {
 	if (adapt->HalFunc.hal_xmit)
diff --git a/drivers/staging/rtl8188eu/hal/odm.c b/drivers/staging/rtl8188eu/hal/odm.c
index 89a26e3e217..2a0ac4ab23d 100644
--- a/drivers/staging/rtl8188eu/hal/odm.c
+++ b/drivers/staging/rtl8188eu/hal/odm.c
@@ -183,7 +183,6 @@ void ODM_DMInit(struct odm_dm_struct *pDM_Odm)
 	odm_RateAdaptiveMaskInit(pDM_Odm);
 
 	odm_PrimaryCCA_Init(pDM_Odm);    /*  Gary */
-	odm_DynamicBBPowerSavingInit(pDM_Odm);
 	odm_DynamicTxPowerInit(pDM_Odm);
 	odm_TXPowerTrackingInit(pDM_Odm);
 	ODM_EdcaTurboInit(pDM_Odm);
@@ -215,8 +214,6 @@ void ODM_DMWatchdog(struct odm_dm_struct *pDM_Odm)
 
 	odm_RefreshRateAdaptiveMask(pDM_Odm);
 
-	odm_DynamicBBPowerSaving(pDM_Odm);
-	odm_DynamicPrimaryCCA(pDM_Odm);
 	if ((pDM_Odm->AntDivType ==  CG_TRX_HW_ANTDIV)	||
 	    (pDM_Odm->AntDivType == CGCS_RX_HW_ANTDIV)	||
 	    (pDM_Odm->AntDivType == CG_TRX_SMART_ANTDIV))
@@ -514,64 +511,10 @@ void ODM_Write_DIG(struct odm_dm_struct *pDM_Odm, u8 CurrentIGI)
 	struct rtw_dig *pDM_DigTable = &pDM_Odm->DM_DigTable;
 	struct adapter *adapter = pDM_Odm->Adapter;
 
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD,
-		     ("ODM_REG(IGI_A,pDM_Odm)=0x%x, ODM_BIT(IGI,pDM_Odm)=0x%x\n",
-		     ODM_REG(IGI_A, pDM_Odm), ODM_BIT(IGI, pDM_Odm)));
-
 	if (pDM_DigTable->CurIGValue != CurrentIGI) {
-		PHY_SetBBReg(adapter, ODM_REG(IGI_A, pDM_Odm), ODM_BIT(IGI, pDM_Odm), CurrentIGI);
-		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("CurrentIGI(0x%02x).\n", CurrentIGI));
-		/* pDM_DigTable->PreIGValue = pDM_DigTable->CurIGValue; */
+		PHY_SetBBReg(adapter, ODM_REG_IGI_A_11N, ODM_BIT_IGI_11N, CurrentIGI);
 		pDM_DigTable->CurIGValue = CurrentIGI;
 	}
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("ODM_Write_DIG():CurrentIGI=0x%x\n", CurrentIGI));
-
-/*  Add by Neil Chen to enable edcca to MP Platform */
-}
-
-/* Need LPS mode for CE platform --2012--08--24--- */
-/* 8723AS/8189ES */
-void odm_DIGbyRSSI_LPS(struct odm_dm_struct *pDM_Odm)
-{
-	struct adapter *pAdapter = pDM_Odm->Adapter;
-	struct false_alarm_stats *pFalseAlmCnt = &pDM_Odm->FalseAlmCnt;
-
-	u8 RSSI_Lower = DM_DIG_MIN_NIC;   /* 0x1E or 0x1C */
-	u8 bFwCurrentInPSMode = false;
-	u8 CurrentIGI = pDM_Odm->RSSI_Min;
-
-	CurrentIGI = CurrentIGI + RSSI_OFFSET_DIG;
-	bFwCurrentInPSMode = pAdapter->pwrctrlpriv.bFwCurrentInPSMode;
-
-	/*  Using FW PS mode to make IGI */
-	if (bFwCurrentInPSMode) {
-		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("---Neil---odm_DIG is in LPS mode\n"));
-		/* Adjust by  FA in LPS MODE */
-		if (pFalseAlmCnt->Cnt_all > DM_DIG_FA_TH2_LPS)
-			CurrentIGI = CurrentIGI+2;
-		else if (pFalseAlmCnt->Cnt_all > DM_DIG_FA_TH1_LPS)
-			CurrentIGI = CurrentIGI+1;
-		else if (pFalseAlmCnt->Cnt_all < DM_DIG_FA_TH0_LPS)
-			CurrentIGI = CurrentIGI-1;
-	} else {
-		CurrentIGI = RSSI_Lower;
-	}
-
-	/* Lower bound checking */
-
-	/* RSSI Lower bound check */
-	if ((pDM_Odm->RSSI_Min-10) > DM_DIG_MIN_NIC)
-		RSSI_Lower = (pDM_Odm->RSSI_Min-10);
-	else
-		RSSI_Lower = DM_DIG_MIN_NIC;
-
-	/* Upper and Lower Bound checking */
-	 if (CurrentIGI > DM_DIG_MAX_NIC)
-		CurrentIGI = DM_DIG_MAX_NIC;
-	 else if (CurrentIGI < RSSI_Lower)
-		CurrentIGI = RSSI_Lower;
-
-	ODM_Write_DIG(pDM_Odm, CurrentIGI);/* ODM_Write_DIG(pDM_Odm, pDM_DigTable->CurIGValue); */
 }
 
 void odm_DIGInit(struct odm_dm_struct *pDM_Odm)
@@ -579,7 +522,7 @@ void odm_DIGInit(struct odm_dm_struct *pDM_Odm)
 	struct adapter *adapter = pDM_Odm->Adapter;
 	struct rtw_dig *pDM_DigTable = &pDM_Odm->DM_DigTable;
 
-	pDM_DigTable->CurIGValue = (u8) PHY_QueryBBReg(adapter, ODM_REG(IGI_A, pDM_Odm), ODM_BIT(IGI, pDM_Odm));
+	pDM_DigTable->CurIGValue = (u8) PHY_QueryBBReg(adapter, ODM_REG_IGI_A_11N, ODM_BIT_IGI_11N);
 	pDM_DigTable->RssiLowThresh	= DM_DIG_THRESH_LOW;
 	pDM_DigTable->RssiHighThresh	= DM_DIG_THRESH_HIGH;
 	pDM_DigTable->FALowThresh	= DM_false_ALARM_THRESH_LOW;
@@ -890,64 +833,11 @@ void ODM_Write_CCK_CCA_Thres(struct odm_dm_struct *pDM_Odm, u8 CurCCK_CCAThres)
 	struct adapter *adapt = pDM_Odm->Adapter;
 
 	if (pDM_DigTable->CurCCK_CCAThres != CurCCK_CCAThres)		/* modify by Guo.Mingzhi 2012-01-03 */
-		rtw_write8(adapt, ODM_REG(CCK_CCA, pDM_Odm), CurCCK_CCAThres);
+		rtw_write8(adapt, ODM_REG_CCK_CCA_11N, CurCCK_CCAThres);
 	pDM_DigTable->PreCCK_CCAThres = pDM_DigTable->CurCCK_CCAThres;
 	pDM_DigTable->CurCCK_CCAThres = CurCCK_CCAThres;
 }
 
-/* 3============================================================ */
-/* 3 BB Power Save */
-/* 3============================================================ */
-void odm_DynamicBBPowerSavingInit(struct odm_dm_struct *pDM_Odm)
-{
-	struct rtl_ps *pDM_PSTable = &pDM_Odm->DM_PSTable;
-
-	pDM_PSTable->PreCCAState = CCA_MAX;
-	pDM_PSTable->CurCCAState = CCA_MAX;
-	pDM_PSTable->PreRFState = RF_MAX;
-	pDM_PSTable->CurRFState = RF_MAX;
-	pDM_PSTable->Rssi_val_min = 0;
-	pDM_PSTable->initialize = 0;
-}
-
-void odm_DynamicBBPowerSaving(struct odm_dm_struct *pDM_Odm)
-{
-}
-
-void odm_1R_CCA(struct odm_dm_struct *pDM_Odm)
-{
-	struct adapter *adapter = pDM_Odm->Adapter;
-	struct rtl_ps *pDM_PSTable = &pDM_Odm->DM_PSTable;
-
-	if (pDM_Odm->RSSI_Min != 0xFF) {
-		if (pDM_PSTable->PreCCAState == CCA_2R) {
-			if (pDM_Odm->RSSI_Min >= 35)
-				pDM_PSTable->CurCCAState = CCA_1R;
-			else
-				pDM_PSTable->CurCCAState = CCA_2R;
-		} else {
-			if (pDM_Odm->RSSI_Min <= 30)
-				pDM_PSTable->CurCCAState = CCA_2R;
-			else
-				pDM_PSTable->CurCCAState = CCA_1R;
-		}
-	} else {
-		pDM_PSTable->CurCCAState = CCA_MAX;
-	}
-
-	if (pDM_PSTable->PreCCAState != pDM_PSTable->CurCCAState) {
-		if (pDM_PSTable->CurCCAState == CCA_1R) {
-			if (pDM_Odm->RFType == ODM_2T2R)
-				PHY_SetBBReg(adapter, 0xc04, bMaskByte0, 0x13);
-			else
-				PHY_SetBBReg(adapter, 0xc04, bMaskByte0, 0x23);
-		} else {
-			PHY_SetBBReg(adapter, 0xc04, bMaskByte0, 0x33);
-		}
-		pDM_PSTable->PreCCAState = pDM_PSTable->CurCCAState;
-	}
-}
-
 void ODM_RF_Saving(struct odm_dm_struct *pDM_Odm, u8 bForceInNormal)
 {
 	struct adapter *adapter = pDM_Odm->Adapter;
@@ -1133,10 +1023,6 @@ void odm_RefreshRateAdaptiveMask(struct odm_dm_struct *pDM_Odm)
 	odm_RefreshRateAdaptiveMaskCE(pDM_Odm);
 }
 
-void odm_RefreshRateAdaptiveMaskMP(struct odm_dm_struct *pDM_Odm)
-{
-}
-
 void odm_RefreshRateAdaptiveMaskCE(struct odm_dm_struct *pDM_Odm)
 {
 	u8 i;
@@ -1165,10 +1051,6 @@ void odm_RefreshRateAdaptiveMaskCE(struct odm_dm_struct *pDM_Odm)
 	}
 }
 
-void odm_RefreshRateAdaptiveMaskAPADSL(struct odm_dm_struct *pDM_Odm)
-{
-}
-
 /*  Return Value: bool */
 /*  - true: RATRState is changed. */
 bool ODM_RAStateCheck(struct odm_dm_struct *pDM_Odm, s32 RSSI, bool bForceUpdate, u8 *pRATRState)
@@ -1239,15 +1121,6 @@ void odm_DynamicTxPower(struct odm_dm_struct *pDM_Odm)
 	/*  2012/01/12 MH According to Luke's suggestion, only high power will support the feature. */
 	if (!pDM_Odm->ExtPA)
 		return;
-
-	/*  2011/09/29 MH In HW integration first stage, we provide 4 different handle to operate */
-	/*  at the same time. In the stage2/3, we need to prive universal interface and merge all */
-	/*  HW dynamic mechanism. */
-	odm_DynamicTxPowerNIC(pDM_Odm);
-}
-
-void odm_DynamicTxPowerNIC(struct odm_dm_struct *pDM_Odm)
-{
 }
 
 /* 3============================================================ */
@@ -1386,22 +1259,6 @@ void odm_TXPowerTrackingCheckCE(struct odm_dm_struct *pDM_Odm)
 	}
 }
 
-/* antenna mapping info */
-/*  1: right-side antenna */
-/*  2/0: left-side antenna */
-/* PDM_SWAT_Table->CCK_Ant1_Cnt /OFDM_Ant1_Cnt:  for right-side antenna:   Ant:1    RxDefaultAnt1 */
-/* PDM_SWAT_Table->CCK_Ant2_Cnt /OFDM_Ant2_Cnt:  for left-side antenna:     Ant:0    RxDefaultAnt2 */
-/*  We select left antenna as default antenna in initial process, modify it as needed */
-/*  */
-
-/* 3============================================================ */
-/* 3 SW Antenna Diversity */
-/* 3============================================================ */
-
-void odm_SwAntDivChkAntSwitchCallback(void *FunctionContext)
-{
-}
-
 /* 3============================================================ */
 /* 3 SW Antenna Diversity */
 /* 3============================================================ */
@@ -1527,28 +1384,3 @@ dm_CheckEdcaTurbo_EXIT:
 	pxmitpriv->last_tx_bytes = pxmitpriv->tx_bytes;
 	precvpriv->last_rx_bytes = precvpriv->rx_bytes;
 }
-
-u32 ConvertTo_dB(u32 Value)
-{
-	u8 i;
-	u8 j;
-	u32 dB;
-
-	Value = Value & 0xFFFF;
-	for (i = 0; i < 8; i++) {
-		if (Value <= dB_Invert_Table[i][11])
-			break;
-	}
-
-	if (i >= 8)
-		return 96;	/*  maximum 96 dB */
-
-	for (j = 0; j < 12; j++) {
-		if (Value <= dB_Invert_Table[i][j])
-			break;
-	}
-
-	dB = i*12 + j + 1;
-
-	return dB;
-}
diff --git a/drivers/staging/rtl8188eu/hal/odm_HWConfig.c b/drivers/staging/rtl8188eu/hal/odm_HWConfig.c
index a755df35ec7..f2e1d02b8ae 100644
--- a/drivers/staging/rtl8188eu/hal/odm_HWConfig.c
+++ b/drivers/staging/rtl8188eu/hal/odm_HWConfig.c
@@ -275,10 +275,6 @@ static void odm_RxPhyStatus92CSeries_Parsing(struct odm_dm_struct *dm_odm,
 	dm_odm->DM_FatTable.antsel_rx_keep_2 = pPhyStaRpt->antsel_rx_keep_2;
 }
 
-void odm_Init_RSSIForDM(struct odm_dm_struct *dm_odm)
-{
-}
-
 static void odm_Process_RSSIForDM(struct odm_dm_struct *dm_odm,
 				  struct odm_phy_status_info *pPhyInfo,
 				  struct odm_per_pkt_info *pPktinfo)
@@ -436,14 +432,6 @@ void ODM_PhyStatusQuery(struct odm_dm_struct *dm_odm,
 	ODM_PhyStatusQuery_92CSeries(dm_odm, pPhyInfo, pPhyStatus, pPktinfo);
 }
 
-/*  For future use. */
-void ODM_MacStatusQuery(struct odm_dm_struct *dm_odm, u8 *mac_stat,
-			u8 macid, bool pkt_match_bssid,
-			bool pkttoself, bool pkt_beacon)
-{
-	/*  2011/10/19 Driver team will handle in the future. */
-}
-
 enum HAL_STATUS ODM_ConfigRFWithHeaderFile(struct odm_dm_struct *dm_odm,
 					   enum rf_radio_path content,
 					   enum rf_radio_path rfpath)
diff --git a/drivers/staging/rtl8188eu/hal/odm_RTL8188E.c b/drivers/staging/rtl8188eu/hal/odm_RTL8188E.c
index 323eb93be41..a24d954e273 100644
--- a/drivers/staging/rtl8188eu/hal/odm_RTL8188E.c
+++ b/drivers/staging/rtl8188eu/hal/odm_RTL8188E.c
@@ -20,18 +20,6 @@
 
 #include "odm_precomp.h"
 
-void ODM_DIG_LowerBound_88E(struct odm_dm_struct *dm_odm)
-{
-	struct rtw_dig *pDM_DigTable = &dm_odm->DM_DigTable;
-
-	if (dm_odm->AntDivType == CG_TRX_HW_ANTDIV) {
-		pDM_DigTable->rx_gain_range_min = (u8) pDM_DigTable->AntDiv_RSSI_max;
-		ODM_RT_TRACE(dm_odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD,
-			     ("ODM_DIG_LowerBound_88E(): pDM_DigTable->AntDiv_RSSI_max=%d\n", pDM_DigTable->AntDiv_RSSI_max));
-	}
-	/* If only one Entry connected */
-}
-
 static void odm_RX_HWAntDivInit(struct odm_dm_struct *dm_odm)
 {
 	struct adapter *adapter = dm_odm->Adapter;
@@ -388,15 +376,3 @@ void odm_PrimaryCCA_Init(struct odm_dm_struct *dm_odm)
 	PrimaryCCA->Monitor_flag = 0;
 	PrimaryCCA->PriCCA_flag = 0;
 }
-
-bool ODM_DynamicPrimaryCCA_DupRTS(struct odm_dm_struct *dm_odm)
-{
-	struct dyn_primary_cca *PrimaryCCA = &(dm_odm->DM_PriCCA);
-
-	return	PrimaryCCA->DupRTS_flag;
-}
-
-void odm_DynamicPrimaryCCA(struct odm_dm_struct *dm_odm)
-{
-	return;
-}
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188e_dm.c b/drivers/staging/rtl8188eu/hal/rtl8188e_dm.c
index cf88bf247c8..d5cd30bcb3c 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188e_dm.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188e_dm.c
@@ -31,10 +31,6 @@
 
 #include <rtl8188e_hal.h>
 
-static void dm_CheckStatistics(struct adapter *Adapter)
-{
-}
-
 /*  Initialize GPIO setting registers */
 static void dm_InitGPIOSetting(struct adapter *Adapter)
 {
@@ -173,12 +169,6 @@ void rtl8188e_HalDmWatchDog(struct adapter *Adapter)
 	if (Adapter->wdinfo.p2p_ps_mode)
 		fw_ps_awake = false;
 
-	if (hw_init_completed && ((!fw_cur_in_ps) && fw_ps_awake)) {
-		/*  Calculate Tx/Rx statistics. */
-		dm_CheckStatistics(Adapter);
-
-	}
-
 	/* ODM */
 	if (hw_init_completed) {
 		struct mlme_priv *pmlmepriv = &Adapter->mlmepriv;
@@ -213,10 +203,6 @@ void rtl8188e_init_dm_priv(struct adapter *Adapter)
 	ODM_InitDebugSetting(podmpriv);
 }
 
-void rtl8188e_deinit_dm_priv(struct adapter *Adapter)
-{
-}
-
 /*  Add new function to reset the state of antenna diversity before link. */
 /*  Compare RSSI for deciding antenna */
 void AntDivCompare8188E(struct adapter *Adapter, struct wlan_bssid_ex *dst, struct wlan_bssid_ex *src)
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188e_hal_init.c b/drivers/staging/rtl8188eu/hal/rtl8188e_hal_init.c
index f9d5558431e..5a22c6df4d0 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188e_hal_init.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188e_hal_init.c
@@ -1843,7 +1843,6 @@ void rtl8188e_set_hal_ops(struct hal_ops *pHalFunc)
 	pHalFunc->free_hal_data = &rtl8188e_free_hal_data;
 
 	pHalFunc->dm_init = &rtl8188e_init_dm_priv;
-	pHalFunc->dm_deinit = &rtl8188e_deinit_dm_priv;
 
 	pHalFunc->read_chip_version = &rtl8188e_read_chip_version;
 
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188e_phycfg.c b/drivers/staging/rtl8188eu/hal/rtl8188e_phycfg.c
index 8079fc67861..941ff7467c3 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188e_phycfg.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188e_phycfg.c
@@ -631,169 +631,6 @@ int PHY_RFConfig8188E(struct adapter *Adapter)
 	return rtStatus;
 }
 
-
-/*-----------------------------------------------------------------------------
- * Function:    PHY_ConfigRFWithParaFile()
- *
- * Overview:    This function read RF parameters from general file format, and do RF 3-wire
- *
- * Input:	struct adapter *Adapter
- *			ps8					pFileName
- *			enum rf_radio_path eRFPath
- *
- * Output:      NONE
- *
- * Return:      RT_STATUS_SUCCESS: configuration file exist
- *
- * Note:		Delay may be required for RF configuration
- *---------------------------------------------------------------------------*/
-int rtl8188e_PHY_ConfigRFWithParaFile(struct adapter *Adapter, u8 *pFileName, enum rf_radio_path eRFPath)
-{
-	return _SUCCESS;
-}
-
-void
-rtl8192c_PHY_GetHWRegOriginalValue(
-		struct adapter *Adapter
-	)
-{
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(Adapter);
-
-	/*  read rx initial gain */
-	pHalData->DefaultInitialGain[0] = (u8)PHY_QueryBBReg(Adapter, rOFDM0_XAAGCCore1, bMaskByte0);
-	pHalData->DefaultInitialGain[1] = (u8)PHY_QueryBBReg(Adapter, rOFDM0_XBAGCCore1, bMaskByte0);
-	pHalData->DefaultInitialGain[2] = (u8)PHY_QueryBBReg(Adapter, rOFDM0_XCAGCCore1, bMaskByte0);
-	pHalData->DefaultInitialGain[3] = (u8)PHY_QueryBBReg(Adapter, rOFDM0_XDAGCCore1, bMaskByte0);
-
-	/*  read framesync */
-	pHalData->framesync = (u8)PHY_QueryBBReg(Adapter, rOFDM0_RxDetector3, bMaskByte0);
-	pHalData->framesyncC34 = PHY_QueryBBReg(Adapter, rOFDM0_RxDetector2, bMaskDWord);
-}
-
-/*  */
-/*	Description: */
-/*		Map dBm into Tx power index according to */
-/*		current HW model, for example, RF and PA, and */
-/*		current wireless mode. */
-/*	By Bruce, 2008-01-29. */
-/*  */
-static	u8 phy_DbmToTxPwrIdx(struct adapter *Adapter, enum wireless_mode WirelessMode, int PowerInDbm)
-{
-	u8 TxPwrIdx = 0;
-	int				Offset = 0;
-
-
-	/*  */
-	/*  Tested by MP, we found that CCK Index 0 equals to 8dbm, OFDM legacy equals to */
-	/*  3dbm, and OFDM HT equals to 0dbm respectively. */
-	/*  Note: */
-	/*	The mapping may be different by different NICs. Do not use this formula for what needs accurate result. */
-	/*  By Bruce, 2008-01-29. */
-	/*  */
-	switch (WirelessMode) {
-	case WIRELESS_MODE_B:
-		Offset = -7;
-		break;
-
-	case WIRELESS_MODE_G:
-	case WIRELESS_MODE_N_24G:
-	default:
-		Offset = -8;
-		break;
-	}
-
-	if ((PowerInDbm - Offset) > 0)
-		TxPwrIdx = (u8)((PowerInDbm - Offset) * 2);
-	else
-		TxPwrIdx = 0;
-
-	/*  Tx Power Index is too large. */
-	if (TxPwrIdx > MAX_TXPWR_IDX_NMODE_92S)
-		TxPwrIdx = MAX_TXPWR_IDX_NMODE_92S;
-
-	return TxPwrIdx;
-}
-
-/*  */
-/*	Description: */
-/*		Map Tx power index into dBm according to */
-/*		current HW model, for example, RF and PA, and */
-/*		current wireless mode. */
-/*	By Bruce, 2008-01-29. */
-/*  */
-static int phy_TxPwrIdxToDbm(struct adapter *Adapter, enum wireless_mode WirelessMode, u8 TxPwrIdx)
-{
-	int				Offset = 0;
-	int				PwrOutDbm = 0;
-
-	/*  */
-	/*  Tested by MP, we found that CCK Index 0 equals to -7dbm, OFDM legacy equals to -8dbm. */
-	/*  Note: */
-	/*	The mapping may be different by different NICs. Do not use this formula for what needs accurate result. */
-	/*  By Bruce, 2008-01-29. */
-	/*  */
-	switch (WirelessMode) {
-	case WIRELESS_MODE_B:
-		Offset = -7;
-		break;
-	case WIRELESS_MODE_G:
-	case WIRELESS_MODE_N_24G:
-	default:
-		Offset = -8;
-		break;
-	}
-
-	PwrOutDbm = TxPwrIdx / 2 + Offset; /*  Discard the decimal part. */
-
-	return PwrOutDbm;
-}
-
-
-/*-----------------------------------------------------------------------------
- * Function:    GetTxPowerLevel8190()
- *
- * Overview:    This function is export to "common" moudule
- *
- * Input:       struct adapter *Adapter
- *			psByte			Power Level
- *
- * Output:      NONE
- *
- * Return:      NONE
- *
- *---------------------------------------------------------------------------*/
-void PHY_GetTxPowerLevel8188E(struct adapter *Adapter, u32 *powerlevel)
-{
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(Adapter);
-	u8 TxPwrLevel = 0;
-	int			TxPwrDbm;
-
-	/*  */
-	/*  Because the Tx power indexes are different, we report the maximum of them to */
-	/*  meet the CCX TPC request. By Bruce, 2008-01-31. */
-	/*  */
-
-	/*  CCK */
-	TxPwrLevel = pHalData->CurrentCckTxPwrIdx;
-	TxPwrDbm = phy_TxPwrIdxToDbm(Adapter, WIRELESS_MODE_B, TxPwrLevel);
-
-	/*  Legacy OFDM */
-	TxPwrLevel = pHalData->CurrentOfdm24GTxPwrIdx + pHalData->LegacyHTTxPowerDiff;
-
-	/*  Compare with Legacy OFDM Tx power. */
-	if (phy_TxPwrIdxToDbm(Adapter, WIRELESS_MODE_G, TxPwrLevel) > TxPwrDbm)
-		TxPwrDbm = phy_TxPwrIdxToDbm(Adapter, WIRELESS_MODE_G, TxPwrLevel);
-
-	/*  HT OFDM */
-	TxPwrLevel = pHalData->CurrentOfdm24GTxPwrIdx;
-
-	/*  Compare with HT OFDM Tx power. */
-	if (phy_TxPwrIdxToDbm(Adapter, WIRELESS_MODE_N_24G, TxPwrLevel) > TxPwrDbm)
-		TxPwrDbm = phy_TxPwrIdxToDbm(Adapter, WIRELESS_MODE_N_24G, TxPwrLevel);
-
-	*powerlevel = TxPwrDbm;
-}
-
 static void getTxPowerIndex88E(struct adapter *Adapter, u8 channel, u8 *cckPowerLevel,
 			       u8 *ofdmPowerLevel, u8 *BW20PowerLevel,
 			       u8 *BW40PowerLevel)
@@ -917,51 +754,6 @@ PHY_SetTxPowerLevel8188E(
 	rtl8188e_PHY_RF6052SetOFDMTxPower(Adapter, &ofdmPowerLevel[0], &BW20PowerLevel[0], &BW40PowerLevel[0], channel);
 }
 
-/*  */
-/*	Description: */
-/*		Update transmit power level of all channel supported. */
-/*  */
-/*	TODO: */
-/*		A mode. */
-/*	By Bruce, 2008-02-04. */
-/*  */
-bool
-PHY_UpdateTxPowerDbm8188E(
-		struct adapter *Adapter,
-		int		powerInDbm
-	)
-{
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(Adapter);
-	u8 idx;
-	u8 rf_path;
-
-	/*  TODO: A mode Tx power. */
-	u8 CckTxPwrIdx = phy_DbmToTxPwrIdx(Adapter, WIRELESS_MODE_B, powerInDbm);
-	u8 OfdmTxPwrIdx = phy_DbmToTxPwrIdx(Adapter, WIRELESS_MODE_N_24G, powerInDbm);
-
-	if (OfdmTxPwrIdx - pHalData->LegacyHTTxPowerDiff > 0)
-		OfdmTxPwrIdx -= pHalData->LegacyHTTxPowerDiff;
-	else
-		OfdmTxPwrIdx = 0;
-
-	for (idx = 0; idx < 14; idx++) {
-		for (rf_path = 0; rf_path < 2; rf_path++) {
-			pHalData->TxPwrLevelCck[rf_path][idx] = CckTxPwrIdx;
-			pHalData->TxPwrLevelHT40_1S[rf_path][idx] =
-			pHalData->TxPwrLevelHT40_2S[rf_path][idx] = OfdmTxPwrIdx;
-		}
-	}
-	return true;
-}
-
-void
-PHY_ScanOperationBackup8188E(
-		struct adapter *Adapter,
-		u8 Operation
-	)
-{
-}
-
 /*-----------------------------------------------------------------------------
  * Function:    PHY_SetBWModeCallback8192C()
  *
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188e_rf6052.c b/drivers/staging/rtl8188eu/hal/rtl8188e_rf6052.c
index b1cb5c4a6fd..52103da3e26 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188e_rf6052.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188e_rf6052.c
@@ -46,51 +46,6 @@
 
 #include <rtl8188e_hal.h>
 
-/*---------------------------Define Local Constant---------------------------*/
-/*  Define local structure for debug!!!!! */
-struct rf_shadow {
-	/*  Shadow register value */
-	u32 Value;
-	/*  Compare or not flag */
-	u8 Compare;
-	/*  Record If it had ever modified unpredicted */
-	u8 ErrorOrNot;
-	/*  Recorver Flag */
-	u8 Recorver;
-	/*  */
-	u8 Driver_Write;
-};
-
-/*---------------------------Define Local Constant---------------------------*/
-
-
-/*------------------------Define global variable-----------------------------*/
-
-/*------------------------Define local variable------------------------------*/
-
-/*-----------------------------------------------------------------------------
- * Function:	RF_ChangeTxPath
- *
- * Overview:	For RL6052, we must change some RF settign for 1T or 2T.
- *
- * Input:		u16 DataRate		0x80-8f, 0x90-9f
- *
- * Output:      NONE
- *
- * Return:      NONE
- *
- * Revised History:
- * When			Who		Remark
- * 09/25/2008	MHC		Create Version 0.
- *						Firmwaer support the utility later.
- *
- *---------------------------------------------------------------------------*/
-void rtl8188e_RF_ChangeTxPath(struct adapter *Adapter, u16 DataRate)
-{
-/*  We do not support gain table change inACUT now !!!! Delete later !!! */
-}	/* RF_ChangeTxPath */
-
-
 /*-----------------------------------------------------------------------------
  * Function:    PHY_RF6052SetBandwidth()
  *
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188eu_led.c b/drivers/staging/rtl8188eu/hal/rtl8188eu_led.c
index 08dfd94163e..77dce584f0d 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188eu_led.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188eu_led.c
@@ -35,16 +35,7 @@ void SwLedOn(struct adapter *padapter, struct LED_871x *pLed)
 	if (padapter->bSurpriseRemoved || padapter->bDriverStopped)
 		return;
 	LedCfg = rtw_read8(padapter, REG_LEDCFG2);
-	switch (pLed->LedPin) {
-	case LED_PIN_LED0:
-		rtw_write8(padapter, REG_LEDCFG2, (LedCfg&0xf0)|BIT5|BIT6); /*  SW control led0 on. */
-		break;
-	case LED_PIN_LED1:
-		rtw_write8(padapter, REG_LEDCFG2, (LedCfg&0x0f)|BIT5); /*  SW control led1 on. */
-		break;
-	default:
-		break;
-	}
+	rtw_write8(padapter, REG_LEDCFG2, (LedCfg&0xf0)|BIT5|BIT6); /*  SW control led0 on. */
 	pLed->bLedOn = true;
 }
 
@@ -60,25 +51,15 @@ void SwLedOff(struct adapter *padapter, struct LED_871x *pLed)
 
 	LedCfg = rtw_read8(padapter, REG_LEDCFG2);/* 0x4E */
 
-	switch (pLed->LedPin) {
-	case LED_PIN_LED0:
-		if (pHalData->bLedOpenDrain) {
+	if (pHalData->bLedOpenDrain) {
 			/*  Open-drain arrangement for controlling the LED) */
-			LedCfg &= 0x90; /*  Set to software control. */
-			rtw_write8(padapter, REG_LEDCFG2, (LedCfg|BIT3));
-			LedCfg = rtw_read8(padapter, REG_MAC_PINMUX_CFG);
-			LedCfg &= 0xFE;
-			rtw_write8(padapter, REG_MAC_PINMUX_CFG, LedCfg);
-		} else {
-			rtw_write8(padapter, REG_LEDCFG2, (LedCfg|BIT3|BIT5|BIT6));
-		}
-		break;
-	case LED_PIN_LED1:
-		LedCfg &= 0x0f; /*  Set to software control. */
+		LedCfg &= 0x90; /*  Set to software control. */
 		rtw_write8(padapter, REG_LEDCFG2, (LedCfg|BIT3));
-		break;
-	default:
-		break;
+		LedCfg = rtw_read8(padapter, REG_MAC_PINMUX_CFG);
+		LedCfg &= 0xFE;
+		rtw_write8(padapter, REG_MAC_PINMUX_CFG, LedCfg);
+	} else {
+		rtw_write8(padapter, REG_LEDCFG2, (LedCfg|BIT3|BIT5|BIT6));
 	}
 exit:
 	pLed->bLedOn = false;
@@ -92,12 +73,13 @@ exit:
 void rtl8188eu_InitSwLeds(struct adapter *padapter)
 {
 	struct led_priv *pledpriv = &(padapter->ledpriv);
+	struct hal_data_8188e   *haldata = GET_HAL_DATA(padapter);
 
+	pledpriv->bRegUseLed = true;
 	pledpriv->LedControlHandler = LedControl8188eu;
+	haldata->bLedOpenDrain = true;
 
-	InitLed871x(padapter, &(pledpriv->SwLed0), LED_PIN_LED0);
-
-	InitLed871x(padapter, &(pledpriv->SwLed1), LED_PIN_LED1);
+	InitLed871x(padapter, &(pledpriv->SwLed0));
 }
 
 /*	Description: */
@@ -107,5 +89,4 @@ void rtl8188eu_DeInitSwLeds(struct adapter *padapter)
 	struct led_priv	*ledpriv = &(padapter->ledpriv);
 
 	DeInitLed871x(&(ledpriv->SwLed0));
-	DeInitLed871x(&(ledpriv->SwLed1));
 }
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188eu_recv.c b/drivers/staging/rtl8188eu/hal/rtl8188eu_recv.c
index b1b1584af1c..0f6222dab4a 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188eu_recv.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188eu_recv.c
@@ -28,22 +28,6 @@
 
 #include <rtl8188e_hal.h>
 
-void rtl8188eu_init_recvbuf(struct adapter *padapter, struct recv_buf *precvbuf)
-{
-	precvbuf->transfer_len = 0;
-
-	precvbuf->len = 0;
-
-	precvbuf->ref_cnt = 0;
-
-	if (precvbuf->pbuf) {
-		precvbuf->pdata = precvbuf->pbuf;
-		precvbuf->phead = precvbuf->pbuf;
-		precvbuf->ptail = precvbuf->pbuf;
-		precvbuf->pend = precvbuf->pdata + MAX_RECVBUF_SZ;
-	}
-}
-
 int	rtl8188eu_init_recv_priv(struct adapter *padapter)
 {
 	struct recv_priv	*precvpriv = &padapter->recvpriv;
@@ -71,13 +55,9 @@ int	rtl8188eu_init_recv_priv(struct adapter *padapter)
 	precvbuf = (struct recv_buf *)precvpriv->precv_buf;
 
 	for (i = 0; i < NR_RECVBUFF; i++) {
-		_rtw_init_listhead(&precvbuf->list);
-		spin_lock_init(&precvbuf->recvbuf_lock);
-		precvbuf->alloc_sz = MAX_RECVBUF_SZ;
 		res = rtw_os_recvbuf_resource_alloc(padapter, precvbuf);
 		if (res == _FAIL)
 			break;
-		precvbuf->ref_cnt = 0;
 		precvbuf->adapter = padapter;
 		precvbuf++;
 	}
@@ -117,7 +97,7 @@ void rtl8188eu_free_recv_priv(struct adapter *padapter)
 	precvbuf = (struct recv_buf *)precvpriv->precv_buf;
 
 	for (i = 0; i < NR_RECVBUFF; i++) {
-		rtw_os_recvbuf_resource_free(padapter, precvbuf);
+		usb_free_urb(precvbuf->purb);
 		precvbuf++;
 	}
 
diff --git a/drivers/staging/rtl8188eu/hal/usb_halinit.c b/drivers/staging/rtl8188eu/hal/usb_halinit.c
index c92067f0ef1..141f85ae561 100644
--- a/drivers/staging/rtl8188eu/hal/usb_halinit.c
+++ b/drivers/staging/rtl8188eu/hal/usb_halinit.c
@@ -420,22 +420,6 @@ static void _InitEDCA(struct adapter *Adapter)
 	rtw_write32(Adapter, REG_EDCA_VO_PARAM, 0x002FA226);
 }
 
-static void _InitBeaconMaxError(struct adapter *Adapter, bool		InfraMode)
-{
-}
-
-static void _InitHWLed(struct adapter *Adapter)
-{
-	struct led_priv *pledpriv = &(Adapter->ledpriv);
-
-	if (pledpriv->LedStrategy != HW_LED)
-		return;
-
-/*  HW led control */
-/*  to do .... */
-/* must consider cases of antenna diversity/ commbo card/solo card/mini card */
-}
-
 static void _InitRDGSetting(struct adapter *Adapter)
 {
 	rtw_write8(Adapter, REG_RD_CTRL, 0xFF);
@@ -599,10 +583,6 @@ static void InitUsbAggregationSetting(struct adapter *Adapter)
 	haldata->UsbRxHighSpeedMode = false;
 }
 
-static void _InitOperationMode(struct adapter *Adapter)
-{
-}
-
 static void _InitBeaconParameters(struct adapter *Adapter)
 {
 	struct hal_data_8188e	*haldata = GET_HAL_DATA(Adapter);
@@ -713,7 +693,6 @@ static u32 rtl8188eu_hal_init(struct adapter *Adapter)
 	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_BEGIN);
 
 	if (Adapter->pwrctrlpriv.bkeepfwalive) {
-		_ps_open_RF(Adapter);
 
 		if (haldata->odmpriv.RFCalibrateInfo.bIQKInitialized) {
 			PHY_IQCalibrate_8188E(Adapter, true);
@@ -840,14 +819,9 @@ static u32 rtl8188eu_hal_init(struct adapter *Adapter)
 	_InitEDCA(Adapter);
 	_InitRetryFunction(Adapter);
 	InitUsbAggregationSetting(Adapter);
-	_InitOperationMode(Adapter);/* todo */
 	_InitBeaconParameters(Adapter);
-	_InitBeaconMaxError(Adapter, true);
-
-	/*  */
 	/*  Init CR MACTXEN, MACRXEN after setting RxFF boundary REG_TRXFF_BNDY to patch */
 	/*  Hw bug which Hw initials RxFF boundary size to a value which is larger than the real Rx buffer size in 88E. */
-	/*  */
 	/*  Enable MACTXEN/MACRXEN block */
 	value16 = rtw_read16(Adapter, REG_CR);
 	value16 |= (MACTXEN | MACRXEN);
@@ -870,8 +844,6 @@ static u32 rtl8188eu_hal_init(struct adapter *Adapter)
 	rtw_write16(Adapter, REG_PKT_VO_VI_LIFE_TIME, 0x0400);	/*  unit: 256us. 256ms */
 	rtw_write16(Adapter, REG_PKT_BE_BK_LIFE_TIME, 0x0400);	/*  unit: 256us. 256ms */
 
-	_InitHWLed(Adapter);
-
 	/* Keep RfRegChnlVal for later use. */
 	haldata->RfRegChnlVal[0] = PHY_QueryRFReg(Adapter, (enum rf_radio_path)0, RF_CHNLBW, bRFRegOffsetMask);
 	haldata->RfRegChnlVal[1] = PHY_QueryRFReg(Adapter, (enum rf_radio_path)1, RF_CHNLBW, bRFRegOffsetMask);
@@ -970,18 +942,6 @@ HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_END);
 	return status;
 }
 
-void _ps_open_RF(struct adapter *adapt)
-{
-	/* here call with bRegSSPwrLvl 1, bRegSSPwrLvl 2 needs to be verified */
-	/* phy_SsPwrSwitch92CU(adapt, rf_on, 1); */
-}
-
-static void _ps_close_RF(struct adapter *adapt)
-{
-	/* here call with bRegSSPwrLvl 1, bRegSSPwrLvl 2 needs to be verified */
-	/* phy_SsPwrSwitch92CU(adapt, rf_off, 1); */
-}
-
 static void CardDisableRTL8188EU(struct adapter *Adapter)
 {
 	u8 val8;
@@ -1059,7 +1019,6 @@ static u32 rtl8188eu_hal_deinit(struct adapter *Adapter)
 
 	DBG_88E("bkeepfwalive(%x)\n", Adapter->pwrctrlpriv.bkeepfwalive);
 	if (Adapter->pwrctrlpriv.bkeepfwalive) {
-		_ps_close_RF(Adapter);
 		if ((Adapter->pwrctrlpriv.bHWPwrPindetect) && (Adapter->pwrctrlpriv.bHWPowerdown))
 			rtl8192cu_hw_power_down(Adapter);
 	} else {
@@ -1129,16 +1088,6 @@ static unsigned int rtl8188eu_inirp_deinit(struct adapter *Adapter)
 /*	EEPROM/EFUSE Content Parsing */
 /*  */
 /*  */
-static void _ReadLEDSetting(struct adapter *Adapter, u8 *PROMContent, bool AutoloadFail)
-{
-	struct led_priv *pledpriv = &(Adapter->ledpriv);
-	struct hal_data_8188e	*haldata = GET_HAL_DATA(Adapter);
-
-	pledpriv->bRegUseLed = true;
-	pledpriv->LedStrategy = SW_LED_MODE1;
-	haldata->bLedOpenDrain = true;/*  Support Open-drain arrangement for controlling the LED. */
-}
-
 static void Hal_EfuseParsePIDVID_8188EU(struct adapter *adapt, u8 *hwinfo, bool AutoLoadFail)
 {
 	struct hal_data_8188e	*haldata = GET_HAL_DATA(adapt);
@@ -1184,10 +1133,6 @@ static void Hal_EfuseParseMACAddr_8188EU(struct adapter *adapt, u8 *hwinfo, bool
 		 eeprom->mac_addr[4], eeprom->mac_addr[5]));
 }
 
-static void Hal_CustomizeByCustomerID_8188EU(struct adapter *adapt)
-{
-}
-
 static void
 readAdapterInfo_8188EU(
 		struct adapter *adapt
@@ -1214,9 +1159,6 @@ readAdapterInfo_8188EU(
 	/*  The following part initialize some vars by PG info. */
 	/*  */
 	Hal_InitChannelPlan(adapt);
-	Hal_CustomizeByCustomerID_8188EU(adapt);
-
-	_ReadLEDSetting(adapt, eeprom->efuse_eeprom_data, eeprom->bautoload_fail_flag);
 }
 
 static void _ReadPROMContent(
@@ -2284,12 +2226,6 @@ static void rtl8188eu_init_default_value(struct adapter *adapt)
 		haldata->odmpriv.RFCalibrateInfo.ThermalValue_HP[i] = 0;
 }
 
-static u8 rtl8188eu_ps_func(struct adapter *Adapter, enum hal_intf_ps_func efunc_id, u8 *val)
-{
-	u8 bResult = true;
-	return bResult;
-}
-
 void rtl8188eu_set_hal_ops(struct adapter *adapt)
 {
 	struct hal_ops	*halfunc = &adapt->HalFunc;
@@ -2330,7 +2266,5 @@ void rtl8188eu_set_hal_ops(struct adapter *adapt)
 	halfunc->hal_xmit = &rtl8188eu_hal_xmit;
 	halfunc->mgnt_xmit = &rtl8188eu_mgnt_xmit;
 
-	halfunc->interface_ps_func = &rtl8188eu_ps_func;
-
 	rtl8188e_set_hal_ops(halfunc);
 }
diff --git a/drivers/staging/rtl8188eu/hal/usb_ops_linux.c b/drivers/staging/rtl8188eu/hal/usb_ops_linux.c
index 1fa5370f1da..3aadf560de0 100644
--- a/drivers/staging/rtl8188eu/hal/usb_ops_linux.c
+++ b/drivers/staging/rtl8188eu/hal/usb_ops_linux.c
@@ -333,7 +333,6 @@ static int recvbuf2recvframe(struct adapter *adapt, struct sk_buff *pskb)
 		}
 
 		_rtw_init_listhead(&precvframe->list);
-		precvframe->precvbuf = NULL;	/* can't access the precvbuf for new arch. */
 		precvframe->len = 0;
 
 		update_recvframe_attrib_88e(precvframe, prxstat);
@@ -518,7 +517,6 @@ static void usb_read_port_complete(struct urb *purb, struct pt_regs *regs)
 		} else {
 			rtw_reset_continual_urb_error(adapter_to_dvobj(adapt));
 
-			precvbuf->transfer_len = purb->actual_length;
 			skb_put(precvbuf->pskb, purb->actual_length);
 			skb_queue_tail(&precvpriv->rx_skb_queue, precvbuf->pskb);
 
@@ -559,7 +557,7 @@ static void usb_read_port_complete(struct urb *purb, struct pt_regs *regs)
 			rtw_read_port(adapt, precvpriv->ff_hwaddr, 0, (unsigned char *)precvbuf);
 			break;
 		case -EINPROGRESS:
-			DBG_88E("ERROR: URB IS IN PROGRESS!/n");
+			DBG_88E("ERROR: URB IS IN PROGRESS!\n");
 			break;
 		default:
 			break;
@@ -601,8 +599,6 @@ static u32 usb_read_port(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *rmem)
 			precvbuf->reuse = true;
 	}
 
-	rtl8188eu_init_recvbuf(adapter, precvbuf);
-
 	/* re-assign for linux based on skb */
 	if ((!precvbuf->reuse) || (precvbuf->pskb == NULL)) {
 		precvbuf->pskb = netdev_alloc_skb(adapter->pnetdev, MAX_RECVBUF_SZ + RECVBUFF_ALIGN_SZ);
@@ -615,19 +611,7 @@ static u32 usb_read_port(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *rmem)
 		tmpaddr = (size_t)precvbuf->pskb->data;
 		alignment = tmpaddr & (RECVBUFF_ALIGN_SZ-1);
 		skb_reserve(precvbuf->pskb, (RECVBUFF_ALIGN_SZ - alignment));
-
-		precvbuf->phead = precvbuf->pskb->head;
-		precvbuf->pdata = precvbuf->pskb->data;
-		precvbuf->ptail = skb_tail_pointer(precvbuf->pskb);
-		precvbuf->pend = skb_end_pointer(precvbuf->pskb);
-		precvbuf->pbuf = precvbuf->pskb->data;
 	} else { /* reuse skb */
-		precvbuf->phead = precvbuf->pskb->head;
-		precvbuf->pdata = precvbuf->pskb->data;
-		precvbuf->ptail = skb_tail_pointer(precvbuf->pskb);
-		precvbuf->pend = skb_end_pointer(precvbuf->pskb);
-		precvbuf->pbuf = precvbuf->pskb->data;
-
 		precvbuf->reuse = false;
 	}
 
@@ -639,7 +623,7 @@ static u32 usb_read_port(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *rmem)
 	pipe = ffaddr2pipehdl(pdvobj, addr);
 
 	usb_fill_bulk_urb(purb, pusbd, pipe,
-			  precvbuf->pbuf,
+			  precvbuf->pskb->data,
 			  MAX_RECVBUF_SZ,
 			  usb_read_port_complete,
 			  precvbuf);/* context is precvbuf */
@@ -698,9 +682,3 @@ void rtl8188eu_set_intf_ops(struct _io_ops	*pops)
 	pops->_read_port_cancel = &usb_read_port_cancel;
 	pops->_write_port_cancel = &usb_write_port_cancel;
 }
-
-void rtl8188eu_set_hw_type(struct adapter *adapt)
-{
-	adapt->chip_type = RTL8188E;
-	DBG_88E("CHIP TYPE: RTL8188E\n");
-}
diff --git a/drivers/staging/rtl8188eu/include/Hal8188EPhyCfg.h b/drivers/staging/rtl8188eu/include/Hal8188EPhyCfg.h
index 25cae8147e7..260ea6bf9e8 100644
--- a/drivers/staging/rtl8188eu/include/Hal8188EPhyCfg.h
+++ b/drivers/staging/rtl8188eu/include/Hal8188EPhyCfg.h
@@ -224,7 +224,6 @@ void rtl8192c_PHY_GetHWRegOriginalValue(struct adapter *adapter);
 /*  BB TX Power R/W */
 void PHY_GetTxPowerLevel8188E(struct adapter *adapter, u32 *powerlevel);
 void PHY_SetTxPowerLevel8188E(struct adapter *adapter, u8 channel);
-bool PHY_UpdateTxPowerDbm8188E(struct adapter *adapter, int power);
 
 void PHY_ScanOperationBackup8188E(struct adapter *Adapter, u8 Operation);
 
diff --git a/drivers/staging/rtl8188eu/include/drv_types.h b/drivers/staging/rtl8188eu/include/drv_types.h
index 936c196699a..10cc1a1bcab 100644
--- a/drivers/staging/rtl8188eu/include/drv_types.h
+++ b/drivers/staging/rtl8188eu/include/drv_types.h
@@ -31,7 +31,6 @@
 
 #include <osdep_service.h>
 #include <wlan_bssdef.h>
-#include <drv_types_linux.h>
 #include <rtw_ht.h>
 #include <rtw_cmd.h>
 #include <rtw_xmit.h>
diff --git a/drivers/staging/rtl8188eu/include/hal_intf.h b/drivers/staging/rtl8188eu/include/hal_intf.h
index c274b349f61..c59fccde39c 100644
--- a/drivers/staging/rtl8188eu/include/hal_intf.h
+++ b/drivers/staging/rtl8188eu/include/hal_intf.h
@@ -166,7 +166,6 @@ struct hal_ops {
 	void	(*DeInitSwLeds)(struct adapter *padapter);
 
 	void	(*dm_init)(struct adapter *padapter);
-	void	(*dm_deinit)(struct adapter *padapter);
 	void	(*read_chip_version)(struct adapter *padapter);
 
 	void	(*init_default_value)(struct adapter *padapter);
@@ -218,9 +217,6 @@ struct hal_ops {
 	void	(*AntDivCompareHandler)(struct adapter *adapter,
 					struct wlan_bssid_ex *dst,
 					struct wlan_bssid_ex *src);
-	u8	(*interface_ps_func)(struct adapter *padapter,
-				     enum hal_intf_ps_func efunc_id, u8 *val);
-
 	s32	(*hal_xmit)(struct adapter *padapter,
 			    struct xmit_frame *pxmitframe);
 	s32 (*mgnt_xmit)(struct adapter *padapter,
@@ -299,7 +295,6 @@ void rtw_hal_def_value_init(struct adapter *padapter);
 void	rtw_hal_free_data(struct adapter *padapter);
 
 void rtw_hal_dm_init(struct adapter *padapter);
-void rtw_hal_dm_deinit(struct adapter *padapter);
 void rtw_hal_sw_led_init(struct adapter *padapter);
 void rtw_hal_sw_led_deinit(struct adapter *padapter);
 
@@ -332,9 +327,6 @@ void rtw_hal_disable_interrupt(struct adapter *padapter);
 u32	rtw_hal_inirp_init(struct adapter *padapter);
 u32	rtw_hal_inirp_deinit(struct adapter *padapter);
 
-u8	rtw_hal_intf_ps_func(struct adapter *padapter,
-			     enum hal_intf_ps_func efunc_id, u8 *val);
-
 s32	rtw_hal_xmit(struct adapter *padapter, struct xmit_frame *pxmitframe);
 s32	rtw_hal_mgnt_xmit(struct adapter *padapter,
 			  struct xmit_frame *pmgntframe);
diff --git a/drivers/staging/rtl8188eu/include/odm.h b/drivers/staging/rtl8188eu/include/odm.h
index 9d1a79c21a2..e83812f35ad 100644
--- a/drivers/staging/rtl8188eu/include/odm.h
+++ b/drivers/staging/rtl8188eu/include/odm.h
@@ -90,14 +90,6 @@
 #define ANTTESTA		0x01	/* Ant A will be Testing */
 #define ANTTESTB		0x02	/* Ant B will be testing */
 
-/*  structure and define */
-
-/*  Add for AP/ADSLpseudo DM structuer requirement. */
-/*  We need to remove to other position??? */
-struct rtl8192cd_priv {
-	u8		temp;
-};
-
 struct rtw_dig {
 	u8		Dig_Enable_Flag;
 	u8		Dig_Ext_Port_Stage;
diff --git a/drivers/staging/rtl8188eu/include/odm_RTL8188E.h b/drivers/staging/rtl8188eu/include/odm_RTL8188E.h
index f96ad5af4bd..02ac78d326d 100644
--- a/drivers/staging/rtl8188eu/include/odm_RTL8188E.h
+++ b/drivers/staging/rtl8188eu/include/odm_RTL8188E.h
@@ -51,6 +51,4 @@ void odm_PrimaryCCA_Init(struct odm_dm_struct *pDM_Odm);
 
 bool ODM_DynamicPrimaryCCA_DupRTS(struct odm_dm_struct *pDM_Odm);
 
-void odm_DynamicPrimaryCCA(struct odm_dm_struct *pDM_Odm);
-
 #endif
diff --git a/drivers/staging/rtl8188eu/include/odm_debug.h b/drivers/staging/rtl8188eu/include/odm_debug.h
index e8c4cab2c35..db7b44e16c4 100644
--- a/drivers/staging/rtl8188eu/include/odm_debug.h
+++ b/drivers/staging/rtl8188eu/include/odm_debug.h
@@ -85,7 +85,7 @@
 /*------------------------Export Marco Definition---------------------------*/
 #define DbgPrint	pr_info
 #define RT_PRINTK(fmt, args...)				\
-	DbgPrint( "%s(): " fmt, __func__, ## args);
+	DbgPrint("%s(): " fmt, __func__, ## args);
 
 #ifndef ASSERT
 	#define ASSERT(expr)
@@ -106,8 +106,8 @@
 
 #define ODM_RT_ASSERT(pDM_Odm, expr, fmt)				\
 	if (!(expr)) {							\
-		DbgPrint( "Assertion failed! %s at ......\n", #expr);	\
-		DbgPrint( "      ......%s,%s,line=%d\n", __FILE__,	\
+		DbgPrint("Assertion failed! %s at ......\n", #expr);	\
+		DbgPrint("      ......%s,%s,line=%d\n", __FILE__,	\
 			__func__, __LINE__);				\
 		RT_PRINTK fmt;						\
 		ASSERT(false);						\
diff --git a/drivers/staging/rtl8188eu/include/odm_interface.h b/drivers/staging/rtl8188eu/include/odm_interface.h
deleted file mode 100644
index 548a309db19..00000000000
--- a/drivers/staging/rtl8188eu/include/odm_interface.h
+++ /dev/null
@@ -1,91 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
- *
- * 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.
- *
- * 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, USA
- *
- *
- ******************************************************************************/
-
-#ifndef	__ODM_INTERFACE_H__
-#define __ODM_INTERFACE_H__
-
-/*  */
-/*  =========== Constant/Structure/Enum/... Define */
-/*  */
-
-/*  */
-/*  =========== Macro Define */
-/*  */
-
-#define _reg_all(_name)			ODM_##_name
-#define _reg_ic(_name, _ic)		ODM_##_name##_ic
-#define _bit_all(_name)			BIT_##_name
-#define _bit_ic(_name, _ic)		BIT_##_name##_ic
-
-/*  _cat: implemented by Token-Pasting Operator. */
-
-/*===================================
-
-#define ODM_REG_DIG_11N		0xC50
-#define ODM_REG_DIG_11AC	0xDDD
-
-ODM_REG(DIG,_pDM_Odm)
-=====================================*/
-
-#define _reg_11N(_name)			ODM_REG_##_name##_11N
-#define _reg_11AC(_name)		ODM_REG_##_name##_11AC
-#define _bit_11N(_name)			ODM_BIT_##_name##_11N
-#define _bit_11AC(_name)		ODM_BIT_##_name##_11AC
-
-#define _cat(_name, _ic_type, _func)					\
-	(								\
-		(_ic_type) ? _func##_11N(_name) :			\
-		_func##_11AC(_name)					\
-	)
-
-/*  _name: name of register or bit. */
-/*  Example: "ODM_REG(R_A_AGC_CORE1, pDM_Odm)" */
-/*         gets "ODM_R_A_AGC_CORE1" or "ODM_R_A_AGC_CORE1_8192C",
- *	   depends on SupportICType. */
-#define ODM_REG(_name, _pDM_Odm) _cat(_name, _pDM_Odm->SupportICType, _reg)
-#define ODM_BIT(_name, _pDM_Odm) _cat(_name, _pDM_Odm->SupportICType, _bit)
-
-enum odm_h2c_cmd {
-	ODM_H2C_RSSI_REPORT = 0,
-	ODM_H2C_PSD_RESULT = 1,
-	ODM_H2C_PathDiv = 2,
-	ODM_MAX_H2CCMD
-};
-
-/*  2012/02/17 MH For non-MP compile pass only. Linux does not support workitem. */
-/*  Suggest HW team to use thread instead of workitem. Windows also support the feature. */
-typedef void (*RT_WORKITEM_CALL_BACK)(void *pContext);
-
-/*  =========== Extern Variable ??? It should be forbidden. */
-
-/*  =========== EXtern Function Prototype */
-
-/*  Memory Relative Function. */
-
-/*  ODM Timer relative API. */
-
-void ODM_CancelTimer(struct odm_dm_struct *pDM_Odm, struct timer_list *pTimer);
-
-/*  ODM FW relative API. */
-u32 ODM_FillH2CCmd(u8 *pH2CBuffer, u32 H2CBufferLen, u32 CmdNum,
-		   u32 *pElementID, u32 *pCmdLen, u8 **pCmbBuffer,
-		   u8 *CmdStartSeq);
-
-#endif	/*  __ODM_INTERFACE_H__ */
diff --git a/drivers/staging/rtl8188eu/include/odm_precomp.h b/drivers/staging/rtl8188eu/include/odm_precomp.h
index 6e6a656b415..2eb769b3f35 100644
--- a/drivers/staging/rtl8188eu/include/odm_precomp.h
+++ b/drivers/staging/rtl8188eu/include/odm_precomp.h
@@ -44,7 +44,6 @@
 #include "Hal8188ERateAdaptive.h"/* for  RA,Power training */
 #include "rtl8188e_hal.h"
 
-#include "odm_interface.h"
 #include "odm_reg.h"
 
 #include "HalHWImg8188E_MAC.h"
diff --git a/drivers/staging/rtl8188eu/include/odm_types.h b/drivers/staging/rtl8188eu/include/odm_types.h
index 78ee2bac0d8..c1355b959c5 100644
--- a/drivers/staging/rtl8188eu/include/odm_types.h
+++ b/drivers/staging/rtl8188eu/include/odm_types.h
@@ -20,31 +20,13 @@
 #ifndef __ODM_TYPES_H__
 #define __ODM_TYPES_H__
 
-/*  */
-/*  Define Different SW team support */
-/*  */
-#define	ODM_AP			0x01	 /* BIT0 */
-#define	ODM_ADSL		0x02	/* BIT1 */
 #define	ODM_CE			0x04	/* BIT2 */
-#define	ODM_MP			0x08	/* BIT3 */
-
-#define		RT_PCI_INTERFACE				1
-#define		RT_USB_INTERFACE				2
-#define		RT_SDIO_INTERFACE				3
 
 enum HAL_STATUS {
 	HAL_STATUS_SUCCESS,
 	HAL_STATUS_FAILURE,
 };
 
-enum RT_SPINLOCK_TYPE {
-	RT_TEMP = 1,
-};
-
-#include <basic_types.h>
-
-#define DEV_BUS_TYPE	RT_USB_INTERFACE
-
 #define SET_TX_DESC_ANTSEL_A_88E(__pTxDesc, __Value)			\
 	SET_BITS_TO_LE_4BYTE(__pTxDesc+8, 24, 1, __Value)
 #define SET_TX_DESC_ANTSEL_B_88E(__pTxDesc, __Value)			\
@@ -52,11 +34,4 @@ enum RT_SPINLOCK_TYPE {
 #define SET_TX_DESC_ANTSEL_C_88E(__pTxDesc, __Value)			\
 	SET_BITS_TO_LE_4BYTE(__pTxDesc+28, 29, 1, __Value)
 
-/* define useless flag to avoid compile warning */
-#define	USE_WORKITEM			0
-#define		FOR_BRAZIL_PRETEST	0
-#define	BT_30_SUPPORT			0
-#define   FPGA_TWO_MAC_VERIFICATION	0
-
-
 #endif /*  __ODM_TYPES_H__ */
diff --git a/drivers/staging/rtl8188eu/include/osdep_service.h b/drivers/staging/rtl8188eu/include/osdep_service.h
index 5889f58ed7d..3859acd358a 100644
--- a/drivers/staging/rtl8188eu/include/osdep_service.h
+++ b/drivers/staging/rtl8188eu/include/osdep_service.h
@@ -119,21 +119,6 @@ static inline int rtw_netif_queue_stopped(struct net_device *pnetdev)
 		netif_tx_queue_stopped(netdev_get_tx_queue(pnetdev, 3));
 }
 
-static inline void rtw_netif_wake_queue(struct net_device *pnetdev)
-{
-	netif_tx_wake_all_queues(pnetdev);
-}
-
-static inline void rtw_netif_start_queue(struct net_device *pnetdev)
-{
-	netif_tx_start_all_queues(pnetdev);
-}
-
-static inline void rtw_netif_stop_queue(struct net_device *pnetdev)
-{
-	netif_tx_stop_all_queues(pnetdev);
-}
-
 #ifndef BIT
 	#define BIT(x)	(1 << (x))
 #endif
@@ -254,11 +239,6 @@ static inline void flush_signals_thread(void)
 		flush_signals(current);
 }
 
-static inline int res_to_status(int res)
-{
-	return res;
-}
-
 #define _RND(sz, r) ((((sz)+((r)-1))/(r))*(r))
 #define RND4(x)	(((x >> 2) + (((x & 3) == 0) ?  0 : 1)) << 2)
 
@@ -359,15 +339,7 @@ u64 rtw_division64(u64 x, u64 y);
 
 #define RTW_GET_BE24(a) ((((u32) (a)[0]) << 16) | (((u32) (a)[1]) << 8) | \
 			 ((u32) (a)[2]))
-#define RTW_PUT_BE24(a, val)					\
-	do {							\
-		(a)[0] = (u8) ((((u32) (val)) >> 16) & 0xff);	\
-		(a)[1] = (u8) ((((u32) (val)) >> 8) & 0xff);	\
-		(a)[2] = (u8) (((u32) (val)) & 0xff);		\
-	} while (0)
 
-#define RTW_GET_BE32(a) ((((u32) (a)[0]) << 24) | (((u32) (a)[1]) << 16) | \
-			 (((u32) (a)[2]) << 8) | ((u32) (a)[3]))
 #define RTW_PUT_BE32(a, val)					\
 	do {							\
 		(a)[0] = (u8) ((((u32) (val)) >> 24) & 0xff);	\
@@ -376,37 +348,6 @@ u64 rtw_division64(u64 x, u64 y);
 		(a)[3] = (u8) (((u32) (val)) & 0xff);		\
 	} while (0)
 
-#define RTW_GET_LE32(a) ((((u32) (a)[3]) << 24) | (((u32) (a)[2]) << 16) | \
-			 (((u32) (a)[1]) << 8) | ((u32) (a)[0]))
-#define RTW_PUT_LE32(a, val)					\
-	do {							\
-		(a)[3] = (u8) ((((u32) (val)) >> 24) & 0xff);	\
-		(a)[2] = (u8) ((((u32) (val)) >> 16) & 0xff);	\
-		(a)[1] = (u8) ((((u32) (val)) >> 8) & 0xff);	\
-		(a)[0] = (u8) (((u32) (val)) & 0xff);		\
-	} while (0)
-
-#define RTW_GET_BE64(a) ((((u64) (a)[0]) << 56) | (((u64) (a)[1]) << 48) | \
-			 (((u64) (a)[2]) << 40) | (((u64) (a)[3]) << 32) | \
-			 (((u64) (a)[4]) << 24) | (((u64) (a)[5]) << 16) | \
-			 (((u64) (a)[6]) << 8) | ((u64) (a)[7]))
-#define RTW_PUT_BE64(a, val)				\
-	do {						\
-		(a)[0] = (u8) (((u64) (val)) >> 56);	\
-		(a)[1] = (u8) (((u64) (val)) >> 48);	\
-		(a)[2] = (u8) (((u64) (val)) >> 40);	\
-		(a)[3] = (u8) (((u64) (val)) >> 32);	\
-		(a)[4] = (u8) (((u64) (val)) >> 24);	\
-		(a)[5] = (u8) (((u64) (val)) >> 16);	\
-		(a)[6] = (u8) (((u64) (val)) >> 8);	\
-		(a)[7] = (u8) (((u64) (val)) & 0xff);	\
-	} while (0)
-
-#define RTW_GET_LE64(a) ((((u64) (a)[7]) << 56) | (((u64) (a)[6]) << 48) | \
-			 (((u64) (a)[5]) << 40) | (((u64) (a)[4]) << 32) | \
-			 (((u64) (a)[3]) << 24) | (((u64) (a)[2]) << 16) | \
-			 (((u64) (a)[1]) << 8) | ((u64) (a)[0]))
-
 void rtw_buf_free(u8 **buf, u32 *buf_len);
 void rtw_buf_update(u8 **buf, u32 *buf_len, u8 *src, u32 src_len);
 
diff --git a/drivers/staging/rtl8188eu/include/recv_osdep.h b/drivers/staging/rtl8188eu/include/recv_osdep.h
index d76cc2db502..a4fd95798f0 100644
--- a/drivers/staging/rtl8188eu/include/recv_osdep.h
+++ b/drivers/staging/rtl8188eu/include/recv_osdep.h
@@ -33,19 +33,15 @@ int rtw_recv_indicatepkt(struct adapter *adapter,
 			 struct recv_frame *recv_frame);
 void rtw_recv_returnpacket(struct  net_device *cnxt, struct sk_buff *retpkt);
 
-void rtw_hostapd_mlme_rx(struct adapter *padapter, struct recv_frame *recv_fr);
 void rtw_handle_tkip_mic_err(struct adapter *padapter, u8 bgroup);
 
 int rtw_init_recv_priv(struct recv_priv *precvpriv, struct adapter *padapter);
 void rtw_free_recv_priv(struct recv_priv *precvpriv);
 
-int rtw_os_recv_resource_init(struct recv_priv *recvpr, struct adapter *adapt);
 int rtw_os_recv_resource_alloc(struct adapter *adapt,
 			       struct recv_frame *recvfr);
-void rtw_os_recv_resource_free(struct recv_priv *precvpriv);
 
 int rtw_os_recvbuf_resource_alloc(struct adapter *adapt, struct recv_buf *buf);
-int rtw_os_recvbuf_resource_free(struct adapter *adapt, struct recv_buf *buf);
 
 void rtw_os_read_port(struct adapter *padapter, struct recv_buf *precvbuf);
 
diff --git a/drivers/staging/rtl8188eu/include/rtl8188e_dm.h b/drivers/staging/rtl8188eu/include/rtl8188e_dm.h
index 97a3175250e..5e0ac31ef46 100644
--- a/drivers/staging/rtl8188eu/include/rtl8188e_dm.h
+++ b/drivers/staging/rtl8188eu/include/rtl8188e_dm.h
@@ -51,7 +51,6 @@ struct	dm_priv {
 };
 
 void rtl8188e_init_dm_priv(struct adapter *adapt);
-void rtl8188e_deinit_dm_priv(struct adapter *adapt);
 void rtl8188e_InitHalDm(struct adapter *adapt);
 void rtl8188e_HalDmWatchDog(struct adapter *adapt);
 
diff --git a/drivers/staging/rtl8188eu/include/rtl8188e_hal.h b/drivers/staging/rtl8188eu/include/rtl8188e_hal.h
index 75e41c4aeb2..fe3b4545cdb 100644
--- a/drivers/staging/rtl8188eu/include/rtl8188e_hal.h
+++ b/drivers/staging/rtl8188eu/include/rtl8188e_hal.h
@@ -471,6 +471,5 @@ void rtl8188e_stop_thread(struct adapter *padapter);
 void rtw_IOL_cmd_tx_pkt_buf_dump(struct adapter  *Adapter, int len);
 s32 rtl8188e_iol_efuse_patch(struct adapter *padapter);
 void rtw_cancel_all_timer(struct adapter *padapter);
-void _ps_open_RF(struct adapter *adapt);
 
 #endif /* __RTL8188E_HAL_H__ */
diff --git a/drivers/staging/rtl8188eu/include/rtl8188e_recv.h b/drivers/staging/rtl8188eu/include/rtl8188e_recv.h
index 07e5f522733..5fed30d389a 100644
--- a/drivers/staging/rtl8188eu/include/rtl8188e_recv.h
+++ b/drivers/staging/rtl8188eu/include/rtl8188e_recv.h
@@ -56,7 +56,6 @@ enum rx_packet_type {
 };
 
 #define INTERRUPT_MSG_FORMAT_LEN 60
-void rtl8188eu_init_recvbuf(struct adapter *padapter, struct recv_buf *buf);
 s32 rtl8188eu_init_recv_priv(struct adapter *padapter);
 void rtl8188eu_free_recv_priv(struct adapter *padapter);
 void rtl8188eu_recv_hdl(struct adapter *padapter, struct recv_buf *precvbuf);
diff --git a/drivers/staging/rtl8188eu/include/rtw_cmd.h b/drivers/staging/rtl8188eu/include/rtw_cmd.h
index 3d347029ff7..66467f711a5 100644
--- a/drivers/staging/rtl8188eu/include/rtw_cmd.h
+++ b/drivers/staging/rtl8188eu/include/rtw_cmd.h
@@ -807,7 +807,6 @@ u8 rtw_chk_hi_queue_cmd(struct adapter *padapter);
 u8 rtw_set_ch_cmd(struct adapter *padapter, u8 ch, u8 bw, u8 ch_offset,
 		  u8 enqueue);
 u8 rtw_set_chplan_cmd(struct adapter *padapter, u8 chplan, u8 enqueue);
-u8 rtw_led_blink_cmd(struct adapter *padapter, struct LED_871x *pLed);
 u8 rtw_set_csa_cmd(struct adapter *padapter, u8 new_ch_no);
 u8 rtw_tdls_cmd(struct adapter *padapter, u8 *addr, u8 option);
 
diff --git a/drivers/staging/rtl8188eu/include/rtw_led.h b/drivers/staging/rtl8188eu/include/rtw_led.h
index 0da4e27a70f..c5194b620da 100644
--- a/drivers/staging/rtl8188eu/include/rtw_led.h
+++ b/drivers/staging/rtl8188eu/include/rtw_led.h
@@ -23,92 +23,43 @@
 #include <osdep_service.h>
 #include <drv_types.h>
 
-#define MSECS(t)        (HZ * ((t) / 1000) + (HZ * ((t) % 1000)) / 1000)
-
-#define LED_BLINK_NORMAL_INTERVAL		100
-#define LED_BLINK_SLOWLY_INTERVAL		200
-#define LED_BLINK_LONG_INTERVAL			400
-
 #define LED_BLINK_NO_LINK_INTERVAL_ALPHA	1000
 #define LED_BLINK_LINK_INTERVAL_ALPHA		500	/* 500 */
 #define LED_BLINK_SCAN_INTERVAL_ALPHA		180	/* 150 */
 #define LED_BLINK_FASTER_INTERVAL_ALPHA		50
 #define LED_BLINK_WPS_SUCESS_INTERVAL_ALPHA	5000
 
-#define LED_BLINK_NORMAL_INTERVAL_NETTRONIX	100
-#define LED_BLINK_SLOWLY_INTERVAL_NETTRONIX	2000
-
-#define LED_BLINK_SLOWLY_INTERVAL_PORNET	1000
-#define LED_BLINK_NORMAL_INTERVAL_PORNET	100
-
-#define LED_BLINK_FAST_INTERVAL_BITLAND		30
-
-/*  060403, rcnjko: Customized for AzWave. */
-#define LED_CM2_BLINK_ON_INTERVAL		250
-#define LED_CM2_BLINK_OFF_INTERVAL		4750
-
-#define LED_CM8_BLINK_INTERVAL			500	/* for QMI */
-#define LED_CM8_BLINK_OFF_INTERVAL		3750	/* for QMI */
-
-/*  080124, lanhsin: Customized for RunTop */
-#define LED_RunTop_BLINK_INTERVAL		300
-
-/*  060421, rcnjko: Customized for Sercomm Printer Server case. */
-#define LED_CM3_BLINK_INTERVAL			1500
-
 enum LED_CTL_MODE {
-	LED_CTL_POWER_ON = 1,
-	LED_CTL_LINK = 2,
-	LED_CTL_NO_LINK = 3,
-	LED_CTL_TX = 4,
-	LED_CTL_RX = 5,
-	LED_CTL_SITE_SURVEY = 6,
-	LED_CTL_POWER_OFF = 7,
-	LED_CTL_START_TO_LINK = 8,
-	LED_CTL_START_WPS = 9,
-	LED_CTL_STOP_WPS = 10,
-	LED_CTL_START_WPS_BOTTON = 11, /* added for runtop */
-	LED_CTL_STOP_WPS_FAIL = 12, /* added for ALPHA */
-	LED_CTL_STOP_WPS_FAIL_OVERLAP = 13, /* added for BELKIN */
-	LED_CTL_CONNECTION_NO_TRANSFER = 14,
+	LED_CTL_POWER_ON,
+	LED_CTL_LINK,
+	LED_CTL_NO_LINK,
+	LED_CTL_TX,
+	LED_CTL_RX ,
+	LED_CTL_SITE_SURVEY,
+	LED_CTL_POWER_OFF,
+	LED_CTL_START_TO_LINK,
+	LED_CTL_START_WPS,
+	LED_CTL_STOP_WPS,
+	LED_CTL_START_WPS_BOTTON,
+	LED_CTL_STOP_WPS_FAIL
 };
 
 enum LED_STATE_871x {
-	LED_UNKNOWN = 0,
-	RTW_LED_ON = 1,
-	RTW_LED_OFF = 2,
-	LED_BLINK_NORMAL = 3,
-	LED_BLINK_SLOWLY = 4,
-	LED_BLINK_POWER_ON = 5,
-	LED_BLINK_SCAN = 6, /*  LED is blinking during scanning period,
-			     * the # of times to blink is depend on time
-			     * for scanning. */
-	LED_BLINK_NO_LINK = 7, /*  LED is blinking during no link state. */
-	LED_BLINK_StartToBlink = 8,/*  Customzied for Sercomm Printer
-				    * Server case */
-	LED_BLINK_TXRX = 9,
-	LED_BLINK_WPS = 10,	/*  LED is blinkg during WPS communication */
-	LED_BLINK_WPS_STOP = 11,	/* for ALPHA */
-	LED_BLINK_WPS_STOP_OVERLAP = 12,	/* for BELKIN */
-	LED_BLINK_RUNTOP = 13, /*  Customized for RunTop */
-	LED_BLINK_CAMEO = 14,
-	LED_BLINK_XAVI = 15,
-	LED_BLINK_ALWAYS_ON = 16,
-};
-
-enum LED_PIN_871x {
-	LED_PIN_NULL = 0,
-	LED_PIN_LED0 = 1,
-	LED_PIN_LED1 = 2,
-	LED_PIN_LED2 = 3,
-	LED_PIN_GPIO0 = 4,
+	LED_UNKNOWN,
+	RTW_LED_ON,
+	RTW_LED_OFF,
+	LED_BLINK_NORMAL,
+	LED_BLINK_SLOWLY,
+	LED_BLINK_POWER_ON,
+	LED_BLINK_SCAN,
+	LED_BLINK_TXRX,
+	LED_BLINK_WPS,
+	LED_BLINK_WPS_STOP
 };
 
 struct LED_871x {
 	struct adapter *padapter;
 
-	enum LED_PIN_871x	LedPin;	/* Identify how to implement this
-					 * SW led. */
 	enum LED_STATE_871x	CurrLedState; /*  Current LED state. */
 	enum LED_STATE_871x	BlinkingLedState; /*  Next state for blinking,
 				   * either RTW_LED_ON or RTW_LED_OFF are. */
@@ -139,34 +90,11 @@ struct LED_871x {
 	((struct LED_871x *)_LED_871x)->CurrLedState == LED_BLINK_WPS_STOP || \
 	((struct LED_871x *)_LED_871x)->bLedWPSBlinkInProgress)
 
-#define IS_LED_BLINKING(_LED_871x)					\
-	(((struct LED_871x *)_LED_871x)->bLedWPSBlinkInProgress	||	\
-	((struct LED_871x *)_LED_871x)->bLedScanBlinkInProgress)
-
-/*  LED customization. */
-
-enum LED_STRATEGY_871x {
-	SW_LED_MODE0 = 0, /* SW control 1 LED via GPIO0. It is default option.*/
-	SW_LED_MODE1 = 1, /*  2 LEDs, through LED0 and LED1. For ALPHA. */
-	SW_LED_MODE2 = 2, /*  SW control 1 LED via GPIO0, customized for AzWave
-			   * 8187 minicard. */
-	SW_LED_MODE3 = 3, /*  SW control 1 LED via GPIO0, customized for Sercomm
-			   * Printer Server case. */
-	SW_LED_MODE4 = 4, /* for Edimax / Belkin */
-	SW_LED_MODE5 = 5, /* for Sercomm / Belkin */
-	SW_LED_MODE6 = 6, /* for 88CU minicard, porting from ce SW_LED_MODE7 */
-	HW_LED = 50, /*  HW control 2 LEDs, LED0 and LED1 (there are 4
-		      * different control modes, see MAC.CONFIG1 for details.)*/
-	LED_ST_NONE = 99,
-};
-
 void LedControl8188eu(struct adapter *padapter, enum LED_CTL_MODE	LedAction);
 
 struct led_priv{
 	/* add for led control */
 	struct LED_871x			SwLed0;
-	struct LED_871x			SwLed1;
-	enum LED_STRATEGY_871x	LedStrategy;
 	u8	bRegUseLed;
 	void (*LedControlHandler)(struct adapter *padapter,
 				  enum LED_CTL_MODE LedAction);
@@ -184,8 +112,7 @@ void BlinkWorkItemCallback(struct work_struct *work);
 
 void ResetLedStatus(struct LED_871x *pLed);
 
-void InitLed871x(struct adapter *padapter, struct LED_871x *pLed,
-		 enum LED_PIN_871x LedPin);
+void InitLed871x(struct adapter *padapter, struct LED_871x *pLed);
 
 void DeInitLed871x(struct LED_871x *pLed);
 
diff --git a/drivers/staging/rtl8188eu/include/rtw_pwrctrl.h b/drivers/staging/rtl8188eu/include/rtw_pwrctrl.h
index 9a42859d612..54dfbf07564 100644
--- a/drivers/staging/rtl8188eu/include/rtw_pwrctrl.h
+++ b/drivers/staging/rtl8188eu/include/rtw_pwrctrl.h
@@ -192,7 +192,7 @@ struct pwrctrl_priv {
 	u8	ips_mode_req;	/*  used to accept the mode setting request,
 				 *  will update to ipsmode later */
 	uint bips_processing;
-	u32 ips_deny_time; /* will deny IPS when system time less than this */
+	unsigned long ips_deny_time; /* will deny IPS when system time less than this */
 	u8 ps_processing; /* temp used to mark whether in rtw_ps_processor */
 
 	u8	bLeisurePs;
@@ -258,8 +258,6 @@ s32 LPS_RF_ON_check(struct adapter *adapter, u32 delay_ms);
 void LPS_Enter(struct adapter *adapter);
 void LPS_Leave(struct adapter *adapter);
 
-u8 rtw_interface_ps_func(struct adapter *adapter,
-			 enum hal_intf_ps_func efunc_id, u8 *val);
 void rtw_set_ips_deny(struct adapter *adapter, u32 ms);
 int _rtw_pwr_wakeup(struct adapter *adapter, u32 ips_defer_ms,
 		    const char *caller);
diff --git a/drivers/staging/rtl8188eu/include/rtw_recv.h b/drivers/staging/rtl8188eu/include/rtw_recv.h
index bcbce46cec8..f0c26ef8f66 100644
--- a/drivers/staging/rtl8188eu/include/rtw_recv.h
+++ b/drivers/staging/rtl8188eu/include/rtw_recv.h
@@ -175,7 +175,6 @@ recv_thread(passive) ; returnpkt(dispatch)
 using enter_critical section to protect
 */
 struct recv_priv {
-	spinlock_t lock;
 	struct __queue free_recv_queue;
 	struct __queue recv_pending_queue;
 	struct __queue uc_swdec_pending_queue;
@@ -189,11 +188,6 @@ struct recv_priv {
 	u64	rx_drop;
 	u64	last_rx_bytes;
 
-	uint  rx_icv_err;
-	uint  rx_largepacket_crcerr;
-	uint  rx_smallpacket_crcerr;
-	uint  rx_middlepacket_crcerr;
-	struct semaphore allrxreturnevt;
 	uint	ff_hwaddr;
 	u8	rx_pending_cnt;
 
@@ -213,9 +207,7 @@ struct recv_priv {
 	u8 signal_strength;
 	u8 signal_qual;
 	u8 noise;
-	int RxSNRdB[2];
 	s8 RxRssi[2];
-	int FalseAlmCnt_all;
 
 	struct timer_list signal_stat_timer;
 	u32 signal_stat_sampling_interval;
@@ -235,22 +227,8 @@ struct sta_recv_priv {
 };
 
 struct recv_buf {
-	struct list_head list;
-	spinlock_t recvbuf_lock;
-	u32	ref_cnt;
 	struct adapter *adapter;
-	u8	*pbuf;
-	u8	*pallocated_buf;
-	u32	len;
-	u8	*phead;
-	u8	*pdata;
-	u8	*ptail;
-	u8	*pend;
 	struct urb *purb;
-	dma_addr_t dma_transfer_addr;	/* (in) dma addr for transfer_buffer */
-	u32 alloc_sz;
-	u8  irp_pending;
-	int  transfer_len;
 	struct sk_buff *pskb;
 	u8	reuse;
 };
@@ -275,15 +253,12 @@ struct recv_frame {
 	struct sk_buff	 *pkt;
 	struct sk_buff	 *pkt_newalloc;
 	struct adapter  *adapter;
-	u8 fragcnt;
-	int frame_tag;
 	struct rx_pkt_attrib attrib;
 	uint  len;
 	u8 *rx_head;
 	u8 *rx_data;
 	u8 *rx_tail;
 	u8 *rx_end;
-	void *precvbuf;
 	struct sta_info *psta;
 	/* for A-MPDU Rx reordering buffer control */
 	struct recv_reorder_ctrl *preorder_ctrl;
@@ -302,9 +277,6 @@ int rtw_enqueue_recvframe(struct recv_frame *precvframe, struct __queue *queue);
 void rtw_free_recvframe_queue(struct __queue *pframequeue,
 			      struct __queue *pfree_recv_queue);
 u32 rtw_free_uc_swdec_pending_queue(struct adapter *adapter);
-int rtw_enqueue_recvbuf_to_head(struct recv_buf *buf, struct __queue *queue);
-int rtw_enqueue_recvbuf(struct recv_buf *precvbuf, struct __queue *queue);
-struct recv_buf *rtw_dequeue_recvbuf(struct __queue *queue);
 
 void rtw_reordering_ctrl_timeout_handler(void *pcontext);
 
@@ -316,31 +288,6 @@ static inline u8 *get_rxmem(struct recv_frame *precvframe)
 	return precvframe->rx_head;
 }
 
-static inline u8 *get_rx_status(struct recv_frame *precvframe)
-{
-	return get_rxmem(precvframe);
-}
-
-static inline u8 *recvframe_push(struct recv_frame *precvframe, int 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->rx_data -= sz;
-	if (precvframe->rx_data < precvframe->rx_head) {
-		precvframe->rx_data += sz;
-		return NULL;
-	}
-	precvframe->len += sz;
-	return precvframe->rx_data;
-}
-
 static inline u8 *recvframe_pull(struct recv_frame *precvframe, int sz)
 {
 	/*  rx_data += sz; move rx_data sz bytes  hereafter */
diff --git a/drivers/staging/rtl8188eu/include/usb_ops.h b/drivers/staging/rtl8188eu/include/usb_ops.h
index 7d33477d551..a290e0ff306 100644
--- a/drivers/staging/rtl8188eu/include/usb_ops.h
+++ b/drivers/staging/rtl8188eu/include/usb_ops.h
@@ -63,7 +63,6 @@ enum{
 #include <usb_ops_linux.h>
 
 void rtl8188eu_set_hw_type(struct adapter *padapter);
-#define hal_set_hw_type rtl8188eu_set_hw_type
 void rtl8188eu_set_intf_ops(struct _io_ops *pops);
 #define usb_set_intf_ops rtl8188eu_set_intf_ops
 
diff --git a/drivers/staging/rtl8188eu/include/xmit_osdep.h b/drivers/staging/rtl8188eu/include/xmit_osdep.h
index 2ff622ba24f..13965f2489d 100644
--- a/drivers/staging/rtl8188eu/include/xmit_osdep.h
+++ b/drivers/staging/rtl8188eu/include/xmit_osdep.h
@@ -32,10 +32,6 @@ struct pkt_file {
 	size_t buf_len;
 };
 
-extern int rtw_ht_enable;
-extern int rtw_cbw40_enable;
-extern int rtw_ampdu_enable;/* for enable tx_ampdu */
-
 #define NR_XMITFRAME	256
 
 struct xmit_priv;
@@ -53,8 +49,6 @@ int rtw_os_xmit_resource_alloc(struct adapter *padapter,
 void rtw_os_xmit_resource_free(struct adapter *padapter,
 			       struct xmit_buf *pxmitbuf, u32 free_sz);
 
-void rtw_set_tx_chksum_offload(struct sk_buff *pkt, struct pkt_attrib *pattrib);
-
 uint rtw_remainder_len(struct pkt_file *pfile);
 void _rtw_open_pktfile(struct sk_buff *pkt, struct pkt_file *pfile);
 uint _rtw_pktfile_read(struct pkt_file *pfile, u8 *rmem, uint rlen);
diff --git a/drivers/staging/rtl8188eu/os_dep/ioctl_linux.c b/drivers/staging/rtl8188eu/os_dep/ioctl_linux.c
index 2636e7f3dbb..f04aaa375f0 100644
--- a/drivers/staging/rtl8188eu/os_dep/ioctl_linux.c
+++ b/drivers/staging/rtl8188eu/os_dep/ioctl_linux.c
@@ -359,7 +359,7 @@ static char *translate_scan(struct adapter *padapter,
 		if (wpa_len > 0) {
 			p = buf;
 			_rtw_memset(buf, 0, MAX_WPA_IE_LEN);
-			p += sprintf(p, "wpa_ie =");
+			p += sprintf(p, "wpa_ie=");
 			for (i = 0; i < wpa_len; i++)
 				p += sprintf(p, "%02x", wpa_ie[i]);
 
@@ -376,7 +376,7 @@ static char *translate_scan(struct adapter *padapter,
 		if (rsn_len > 0) {
 			p = buf;
 			_rtw_memset(buf, 0, MAX_WPA_IE_LEN);
-			p += sprintf(p, "rsn_ie =");
+			p += sprintf(p, "rsn_ie=");
 			for (i = 0; i < rsn_len; i++)
 				p += sprintf(p, "%02x", rsn_ie[i]);
 			_rtw_memset(&iwe, 0, sizeof(iwe));
@@ -2097,7 +2097,8 @@ static int rtw_wx_set_enc_ext(struct net_device *dev,
 		alg_name = "CCMP";
 		break;
 	default:
-		return -1;
+		ret = -1;
+		goto exit;
 	}
 
 	strncpy((char *)param->u.crypt.alg, alg_name, IEEE_CRYPT_ALG_NAME_LEN);
@@ -2124,6 +2125,7 @@ static int rtw_wx_set_enc_ext(struct net_device *dev,
 
 	ret =  wpa_set_encryption(dev, param, param_len);
 
+exit:
 	kfree(param);
 	return ret;
 }
@@ -2154,6 +2156,7 @@ static int rtw_wx_read32(struct net_device *dev,
 	u32 bytes;
 	u8 *ptmp;
 	int rv;
+	int ret = 0;
 
 	padapter = (struct adapter *)rtw_netdev_priv(dev);
 	p = &wrqu->data;
@@ -2163,16 +2166,16 @@ static int rtw_wx_read32(struct net_device *dev,
 		return -ENOMEM;
 
 	if (copy_from_user(ptmp, p->pointer, len)) {
-		kfree(ptmp);
-		return -EFAULT;
+		ret = -EFAULT;
+		goto exit;
 	}
 
 	bytes = 0;
 	addr = 0;
 	rv = sscanf(ptmp, "%d,%x", &bytes, &addr);
 	if (rv != 2) {
-		kfree(ptmp);
-		return -EINVAL;
+		ret = -EINVAL;
+		goto exit;
 	}
 
 	switch (bytes) {
@@ -2190,12 +2193,14 @@ static int rtw_wx_read32(struct net_device *dev,
 		break;
 	default:
 		DBG_88E(KERN_INFO "%s: usage> read [bytes],[address(hex)]\n", __func__);
-		return -EINVAL;
+		ret = -EINVAL;
+		goto exit;
 	}
 	DBG_88E(KERN_INFO "%s: addr = 0x%08X data =%s\n", __func__, addr, extra);
 
+exit:
 	kfree(ptmp);
-	return 0;
+	return ret;
 }
 
 static int rtw_wx_write32(struct net_device *dev,
@@ -2899,7 +2904,7 @@ static int rtw_p2p_get_status(struct net_device *dev,
 	/*	Commented by Albert 2010/10/12 */
 	/*	Because of the output size limitation, I had removed the "Role" information. */
 	/*	About the "Role" information, we will use the new private IOCTL to get the "Role" information. */
-	sprintf(extra, "\n\nStatus =%.2d\n", rtw_p2p_state(pwdinfo));
+	sprintf(extra, "\n\nStatus=%.2d\n", rtw_p2p_state(pwdinfo));
 	wrqu->data.length = strlen(extra);
 
 	return ret;
@@ -2918,7 +2923,7 @@ static int rtw_p2p_get_req_cm(struct net_device *dev,
 	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
 	struct wifidirect_info *pwdinfo = &(padapter->wdinfo);
 
-	sprintf(extra, "\n\nCM =%s\n", pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req);
+	sprintf(extra, "\n\nCM=%s\n", pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req);
 	wrqu->data.length = strlen(extra);
 	return ret;
 }
@@ -2935,7 +2940,7 @@ static int rtw_p2p_get_role(struct net_device *dev,
 			pwdinfo->p2p_peer_interface_addr[0], pwdinfo->p2p_peer_interface_addr[1], pwdinfo->p2p_peer_interface_addr[2],
 			pwdinfo->p2p_peer_interface_addr[3], pwdinfo->p2p_peer_interface_addr[4], pwdinfo->p2p_peer_interface_addr[5]);
 
-	sprintf(extra, "\n\nRole =%.2d\n", rtw_p2p_role(pwdinfo));
+	sprintf(extra, "\n\nRole=%.2d\n", rtw_p2p_role(pwdinfo));
 	wrqu->data.length = strlen(extra);
 	return ret;
 }
@@ -3022,7 +3027,7 @@ static int rtw_p2p_get_op_ch(struct net_device *dev,
 
 	DBG_88E("[%s] Op_ch = %02x\n", __func__, pwdinfo->operating_channel);
 
-	sprintf(extra, "\n\nOp_ch =%.2d\n", pwdinfo->operating_channel);
+	sprintf(extra, "\n\nOp_ch=%.2d\n", pwdinfo->operating_channel);
 	wrqu->data.length = strlen(extra);
 	return ret;
 }
@@ -3043,7 +3048,7 @@ static int rtw_p2p_get_wps_configmethod(struct net_device *dev,
 	u8 blnMatch = 0;
 	u16	attr_content = 0;
 	uint attr_contentlen = 0;
-	/* 6 is the string "wpsCM =", 17 is the MAC addr, we have to clear it at wrqu->data.pointer */
+	/* 6 is the string "wpsCM=", 17 is the MAC addr, we have to clear it at wrqu->data.pointer */
 	u8 attr_content_str[6 + 17] = {0x00};
 
 	/*	Commented by Albert 20110727 */
@@ -3079,7 +3084,7 @@ static int rtw_p2p_get_wps_configmethod(struct net_device *dev,
 				rtw_get_wps_attr_content(wpsie, wpsie_len, WPS_ATTR_CONF_METHOD, (u8 *) &be_tmp, &attr_contentlen);
 				if (attr_contentlen) {
 					attr_content = be16_to_cpu(be_tmp);
-					sprintf(attr_content_str, "\n\nM =%.4d", attr_content);
+					sprintf(attr_content_str, "\n\nM=%.4d", attr_content);
 					blnMatch = 1;
 				}
 			}
@@ -3091,7 +3096,7 @@ static int rtw_p2p_get_wps_configmethod(struct net_device *dev,
 	spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
 
 	if (!blnMatch)
-		sprintf(attr_content_str, "\n\nM = 0000");
+		sprintf(attr_content_str, "\n\nM=0000");
 
 	if (copy_to_user(wrqu->data.pointer, attr_content_str, 6 + 17))
 		return -EFAULT;
@@ -3172,9 +3177,9 @@ static int rtw_p2p_get_go_device_address(struct net_device *dev,
 	spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
 
 	if (!blnMatch)
-		snprintf(go_devadd_str, sizeof(go_devadd_str), "\n\ndev_add = NULL");
+		snprintf(go_devadd_str, sizeof(go_devadd_str), "\n\ndev_add=NULL");
 	else
-		snprintf(go_devadd_str, sizeof(go_devadd_str), "\n\ndev_add =%.2X:%.2X:%.2X:%.2X:%.2X:%.2X",
+		snprintf(go_devadd_str, sizeof(go_devadd_str), "\n\ndev_add=%.2X:%.2X:%.2X:%.2X:%.2X:%.2X",
 			attr_content[0], attr_content[1], attr_content[2], attr_content[3], attr_content[4], attr_content[5]);
 
 	if (copy_to_user(wrqu->data.pointer, go_devadd_str, sizeof(go_devadd_str)))
@@ -3198,7 +3203,7 @@ static int rtw_p2p_get_device_type(struct net_device *dev,
 	u8 blnMatch = 0;
 	u8 dev_type[8] = {0x00};
 	uint dev_type_len = 0;
-	u8 dev_type_str[17 + 9] = {0x00};	/*  +9 is for the str "dev_type =", we have to clear it at wrqu->data.pointer */
+	u8 dev_type_str[17 + 9] = {0x00};	/*  +9 is for the str "dev_type=", we have to clear it at wrqu->data.pointer */
 
 	/*	Commented by Albert 20121209 */
 	/*	The input data is the MAC address which the application wants to know its device type. */
@@ -3239,7 +3244,7 @@ static int rtw_p2p_get_device_type(struct net_device *dev,
 
 					memcpy(&be_tmp, dev_type, 2);
 					type = be16_to_cpu(be_tmp);
-					sprintf(dev_type_str, "\n\nN =%.2d", type);
+					sprintf(dev_type_str, "\n\nN=%.2d", type);
 					blnMatch = 1;
 				}
 			}
@@ -3252,7 +3257,7 @@ static int rtw_p2p_get_device_type(struct net_device *dev,
 	spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
 
 	if (!blnMatch)
-		sprintf(dev_type_str, "\n\nN = 00");
+		sprintf(dev_type_str, "\n\nN=00");
 
 	if (copy_to_user(wrqu->data.pointer, dev_type_str, 9 + 17)) {
 		return -EFAULT;
@@ -3277,7 +3282,7 @@ static int rtw_p2p_get_device_name(struct net_device *dev,
 	u8 blnMatch = 0;
 	u8 dev_name[WPS_MAX_DEVICE_NAME_LEN] = {0x00};
 	uint dev_len = 0;
-	u8 dev_name_str[WPS_MAX_DEVICE_NAME_LEN + 5] = {0x00};	/*  +5 is for the str "devN =", we have to clear it at wrqu->data.pointer */
+	u8 dev_name_str[WPS_MAX_DEVICE_NAME_LEN + 5] = {0x00};	/*  +5 is for the str "devN=", we have to clear it at wrqu->data.pointer */
 
 	/*	Commented by Albert 20121225 */
 	/*	The input data is the MAC address which the application wants to know its device name. */
@@ -3310,7 +3315,7 @@ static int rtw_p2p_get_device_name(struct net_device *dev,
 			if (wpsie) {
 				rtw_get_wps_attr_content(wpsie, wpsie_len, WPS_ATTR_DEVICE_NAME, dev_name, &dev_len);
 				if (dev_len) {
-					sprintf(dev_name_str, "\n\nN =%s", dev_name);
+					sprintf(dev_name_str, "\n\nN=%s", dev_name);
 					blnMatch = 1;
 				}
 			}
@@ -3323,7 +3328,7 @@ static int rtw_p2p_get_device_name(struct net_device *dev,
 	spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
 
 	if (!blnMatch)
-		sprintf(dev_name_str, "\n\nN = 0000");
+		sprintf(dev_name_str, "\n\nN=0000");
 
 	if (copy_to_user(wrqu->data.pointer, dev_name_str, 5 + ((dev_len > 17) ? dev_len : 17)))
 		return -EFAULT;
@@ -3349,7 +3354,7 @@ static int rtw_p2p_get_invitation_procedure(struct net_device *dev,
 	u8 attr_content[2] = {0x00};
 
 	u8 inv_proc_str[17 + 8] = {0x00};
-	/*  +8 is for the str "InvProc =", we have to clear it at wrqu->data.pointer */
+	/*  +8 is for the str "InvProc=", we have to clear it at wrqu->data.pointer */
 
 	/*	Commented by Ouden 20121226 */
 	/*	The application wants to know P2P initiation procedure is supported or not. */
@@ -3397,12 +3402,12 @@ static int rtw_p2p_get_invitation_procedure(struct net_device *dev,
 	spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
 
 	if (!blnMatch) {
-		sprintf(inv_proc_str, "\nIP =-1");
+		sprintf(inv_proc_str, "\nIP=-1");
 	} else {
 		if (attr_content[0] & 0x20)
-			sprintf(inv_proc_str, "\nIP = 1");
+			sprintf(inv_proc_str, "\nIP=1");
 		else
-			sprintf(inv_proc_str, "\nIP = 0");
+			sprintf(inv_proc_str, "\nIP=0");
 	}
 	if (copy_to_user(wrqu->data.pointer, inv_proc_str, 8 + 17))
 		return -EFAULT;
@@ -3512,7 +3517,7 @@ static int rtw_p2p_invite_req(struct net_device *dev,
 	/*	The input data contains two informations. */
 	/*	1. First information is the P2P device address which you want to send to. */
 	/*	2. Second information is the group id which combines with GO's mac address, space and GO's ssid. */
-	/*	Command line sample: iwpriv wlan0 p2p_set invite ="00:11:22:33:44:55 00:E0:4C:00:00:05 DIRECT-xy" */
+	/*	Command line sample: iwpriv wlan0 p2p_set invite="00:11:22:33:44:55 00:E0:4C:00:00:05 DIRECT-xy" */
 	/*	Format: 00:11:22:33:44:55 00:E0:4C:00:00:05 DIRECT-xy */
 
 	DBG_88E("[%s] data = %s\n", __func__, extra);
@@ -3805,48 +3810,48 @@ static int rtw_p2p_set(struct net_device *dev,
 
 #ifdef CONFIG_88EU_P2P
 	DBG_88E("[%s] extra = %s\n", __func__, extra);
-	if (!memcmp(extra, "enable =", 7)) {
+	if (!memcmp(extra, "enable=", 7)) {
 		rtw_wext_p2p_enable(dev, info, wrqu, &extra[7]);
-	} else if (!memcmp(extra, "setDN =", 6)) {
+	} else if (!memcmp(extra, "setDN=", 6)) {
 		wrqu->data.length -= 6;
 		rtw_p2p_setDN(dev, info, wrqu, &extra[6]);
-	} else if (!memcmp(extra, "profilefound =", 13)) {
+	} else if (!memcmp(extra, "profilefound=", 13)) {
 		wrqu->data.length -= 13;
 		rtw_p2p_profilefound(dev, info, wrqu, &extra[13]);
-	} else if (!memcmp(extra, "prov_disc =", 10)) {
+	} else if (!memcmp(extra, "prov_disc=", 10)) {
 		wrqu->data.length -= 10;
 		rtw_p2p_prov_disc(dev, info, wrqu, &extra[10]);
-	} else if (!memcmp(extra, "nego =", 5)) {
+	} else if (!memcmp(extra, "nego=", 5)) {
 		wrqu->data.length -= 5;
 		rtw_p2p_connect(dev, info, wrqu, &extra[5]);
-	} else if (!memcmp(extra, "intent =", 7)) {
+	} else if (!memcmp(extra, "intent=", 7)) {
 		/*	Commented by Albert 2011/03/23 */
 		/*	The wrqu->data.length will include the null character */
 		/*	So, we will decrease 7 + 1 */
 		wrqu->data.length -= 8;
 		rtw_p2p_set_intent(dev, info, wrqu, &extra[7]);
-	} else if (!memcmp(extra, "ssid =", 5)) {
+	} else if (!memcmp(extra, "ssid=", 5)) {
 		wrqu->data.length -= 5;
 		rtw_p2p_set_go_nego_ssid(dev, info, wrqu, &extra[5]);
-	} else if (!memcmp(extra, "got_wpsinfo =", 12)) {
+	} else if (!memcmp(extra, "got_wpsinfo=", 12)) {
 		wrqu->data.length -= 12;
 		rtw_p2p_got_wpsinfo(dev, info, wrqu, &extra[12]);
-	} else if (!memcmp(extra, "listen_ch =", 10)) {
+	} else if (!memcmp(extra, "listen_ch=", 10)) {
 		/*	Commented by Albert 2011/05/24 */
 		/*	The wrqu->data.length will include the null character */
 		/*	So, we will decrease (10 + 1) */
 		wrqu->data.length -= 11;
 		rtw_p2p_set_listen_ch(dev, info, wrqu, &extra[10]);
-	} else if (!memcmp(extra, "op_ch =", 6)) {
+	} else if (!memcmp(extra, "op_ch=", 6)) {
 		/*	Commented by Albert 2011/05/24 */
 		/*	The wrqu->data.length will include the null character */
 		/*	So, we will decrease (6 + 1) */
 		wrqu->data.length -= 7;
 		rtw_p2p_set_op_ch(dev, info, wrqu, &extra[6]);
-	} else if (!memcmp(extra, "invite =", 7)) {
+	} else if (!memcmp(extra, "invite=", 7)) {
 		wrqu->data.length -= 8;
 		rtw_p2p_invite_req(dev, info, wrqu, &extra[7]);
-	} else if (!memcmp(extra, "persistent =", 11)) {
+	} else if (!memcmp(extra, "persistent=", 11)) {
 		wrqu->data.length -= 11;
 		rtw_p2p_set_persistent(dev, info, wrqu, &extra[11]);
 	}
@@ -3887,7 +3892,7 @@ static int rtw_p2p_get(struct net_device *dev,
 			"group_id", 8)) {
 		rtw_p2p_get_groupid(dev, info, wrqu, extra);
 	} else if (!memcmp((__force const char *)wrqu->data.pointer,
-			"peer_deva_inv", 9)) {
+			"peer_deva_inv", 13)) {
 		/*	Get the P2P device address when receiving the P2P Invitation request frame. */
 		rtw_p2p_get_peer_devaddr_by_invitation(dev, info, wrqu, extra);
 	} else if (!memcmp((__force const char *)wrqu->data.pointer,
@@ -6920,7 +6925,7 @@ static int rtw_mp_ctx(struct net_device *dev,
 
 	DBG_88E("%s: in =%s\n", __func__, extra);
 
-	countPkTx = strncmp(extra, "count =", 5); /*  strncmp true is 0 */
+	countPkTx = strncmp(extra, "count=", 6); /*  strncmp true is 0 */
 	cotuTx = strncmp(extra, "background", 20);
 	CarrSprTx = strncmp(extra, "background, cs", 20);
 	scTx = strncmp(extra, "background, sc", 20);
@@ -7044,7 +7049,7 @@ static int rtw_mp_arx(struct net_device *dev,
 	DBG_88E("%s: %s\n", __func__, input);
 
 	bStartRx = (strncmp(input, "start", 5) == 0) ? 1 : 0; /*  strncmp true is 0 */
-	bStopRx = (strncmp(input, "stop", 5) == 0) ? 1 : 0; /*  strncmp true is 0 */
+	bStopRx = (strncmp(input, "stop", 4) == 0) ? 1 : 0; /*  strncmp true is 0 */
 	bQueryPhy = (strncmp(input, "phy", 3) == 0) ? 1 : 0; /*  strncmp true is 0 */
 
 	if (bStartRx) {
@@ -7114,15 +7119,15 @@ static int rtw_mp_pwrtrk(struct net_device *dev,
 {
 	u8 enable;
 	u32 thermal;
-	s32 ret;
 	struct adapter *padapter = rtw_netdev_priv(dev);
 	char	*input = kmalloc(wrqu->length, GFP_KERNEL);
+	int ret = 0;
 
 	if (!input)
 		return -ENOMEM;
 	if (copy_from_user(input, wrqu->pointer, wrqu->length)) {
-		kfree(input);
-		return -EFAULT;
+		ret = -EFAULT;
+		goto exit;
 	}
 	_rtw_memset(extra, 0, wrqu->length);
 
@@ -7133,22 +7138,28 @@ static int rtw_mp_pwrtrk(struct net_device *dev,
 			sprintf(extra, "mp tx power tracking stop");
 		} else if (sscanf(input, "ther =%d", &thermal)) {
 				ret = Hal_SetThermalMeter(padapter, (u8)thermal);
-				if (ret == _FAIL)
-					return -EPERM;
+				if (ret == _FAIL) {
+					ret = -EPERM;
+					goto exit;
+				}
 				sprintf(extra, "mp tx power tracking start, target value =%d ok ", thermal);
 		} else {
-			kfree(input);
-			return -EINVAL;
+			ret = -EINVAL;
+			goto exit;
 		}
 	}
 
-	kfree(input);
 	ret = Hal_SetPowerTracking(padapter, enable);
-	if (ret == _FAIL)
-		return -EPERM;
+	if (ret == _FAIL) {
+		ret = -EPERM;
+		goto exit;
+	}
 
 	wrqu->length = strlen(extra);
-	return 0;
+
+exit:
+	kfree(input);
+	return ret;
 }
 
 static int rtw_mp_psd(struct net_device *dev,
@@ -7316,11 +7327,14 @@ static int rtw_mp_SetRFPath(struct net_device *dev,
 	struct adapter *padapter = rtw_netdev_priv(dev);
 	char	*input = kmalloc(wrqu->data.length, GFP_KERNEL);
 	u8 bMain = 1, bTurnoff = 1;
+	int ret = 0;
 
 	if (!input)
 		return -ENOMEM;
-	if (copy_from_user(input, wrqu->data.pointer, wrqu->data.length))
-			return -EFAULT;
+	if (copy_from_user(input, wrqu->data.pointer, wrqu->data.length)) {
+		ret = -EFAULT;
+		goto exit;
+	}
 	DBG_88E("%s:iwpriv in =%s\n", __func__, input);
 
 	bMain = strncmp(input, "1", 2); /*  strncmp true is 0 */
@@ -7333,8 +7347,10 @@ static int rtw_mp_SetRFPath(struct net_device *dev,
 		MP_PHY_SetRFPathSwitch(padapter, false);
 		DBG_88E("%s:PHY_SetRFPathSwitch = false\n", __func__);
 	}
+
+exit:
 	kfree(input);
-	return 0;
+	return ret;
 }
 
 static int rtw_mp_QueryDrv(struct net_device *dev,
@@ -7345,12 +7361,15 @@ static int rtw_mp_QueryDrv(struct net_device *dev,
 	char	*input = kmalloc(wrqu->data.length, GFP_KERNEL);
 	u8 qAutoLoad = 1;
 	struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(padapter);
+	int ret = 0;
 
 	if (!input)
 		return -ENOMEM;
 
-	if (copy_from_user(input, wrqu->data.pointer, wrqu->data.length))
-			return -EFAULT;
+	if (copy_from_user(input, wrqu->data.pointer, wrqu->data.length)) {
+		ret = -EFAULT;
+		goto exit;
+	}
 	DBG_88E("%s:iwpriv in =%s\n", __func__, input);
 
 	qAutoLoad = strncmp(input, "autoload", 8); /*  strncmp true is 0 */
@@ -7364,8 +7383,10 @@ static int rtw_mp_QueryDrv(struct net_device *dev,
 		sprintf(extra, "ok");
 	}
 	wrqu->data.length = strlen(extra) + 1;
+
+exit:
 	kfree(input);
-	return 0;
+	return ret;
 }
 
 static int rtw_mp_set(struct net_device *dev,
diff --git a/drivers/staging/rtl8188eu/os_dep/os_intfs.c b/drivers/staging/rtl8188eu/os_dep/os_intfs.c
index b225d1c0721..0e0c32d4643 100644
--- a/drivers/staging/rtl8188eu/os_dep/os_intfs.c
+++ b/drivers/staging/rtl8188eu/os_dep/os_intfs.c
@@ -84,9 +84,9 @@ static int rtw_uapsd_acbe_en;
 static int rtw_uapsd_acvi_en;
 static int rtw_uapsd_acvo_en;
 
-int rtw_ht_enable = 1;
-int rtw_cbw40_enable = 3; /*  0 :disable, bit(0): enable 2.4g, bit(1): enable 5g */
-int rtw_ampdu_enable = 1;/* for enable tx_ampdu */
+static int rtw_ht_enable = 1;
+static int rtw_cbw40_enable = 3; /*  0 :disable, bit(0): enable 2.4g, bit(1): enable 5g */
+static int rtw_ampdu_enable = 1;/* for enable tx_ampdu */
 static int rtw_rx_stbc = 1;/*  0: disable, bit(0):enable 2.4g, bit(1):enable 5g, default is set to enable 2.4GHZ for IOT issue with bufflao's AP at 5GHZ */
 static int rtw_ampdu_amsdu;/*  0: disabled, 1:enabled, 2:auto */
 
@@ -951,8 +951,6 @@ void rtw_cancel_all_timer(struct adapter *padapter)
 	_cancel_timer_ex(&padapter->pwrctrlpriv.pwr_state_check_timer);
 
 	_cancel_timer_ex(&padapter->recvpriv.signal_stat_timer);
-	/* cancel dm timer */
-	rtw_hal_dm_deinit(padapter);
 }
 
 u8 rtw_free_drv_sw(struct adapter *padapter)
@@ -1084,9 +1082,9 @@ int _netdev_open(struct net_device *pnetdev)
 	rtw_set_pwr_state_check_timer(&padapter->pwrctrlpriv);
 
 	if (!rtw_netif_queue_stopped(pnetdev))
-		rtw_netif_start_queue(pnetdev);
+		netif_tx_start_all_queues(pnetdev);
 	else
-		rtw_netif_wake_queue(pnetdev);
+		netif_tx_wake_all_queues(pnetdev);
 
 	netdev_br_init(pnetdev);
 
@@ -1098,7 +1096,7 @@ netdev_open_normal_process:
 netdev_open_error:
 	padapter->bup = false;
 	netif_carrier_off(pnetdev);
-	rtw_netif_stop_queue(pnetdev);
+	netif_tx_stop_all_queues(pnetdev);
 	RT_TRACE(_module_os_intfs_c_, _drv_err_, ("-88eu_drv - dev_open, fail!\n"));
 	DBG_88E("-88eu_drv - drv_open fail, bup =%d\n", padapter->bup);
 	return -1;
@@ -1221,7 +1219,7 @@ int netdev_close(struct net_device *pnetdev)
 		/* s1. */
 		if (pnetdev) {
 			if (!rtw_netif_queue_stopped(pnetdev))
-				rtw_netif_stop_queue(pnetdev);
+				netif_tx_stop_all_queues(pnetdev);
 		}
 
 		/* s2. */
diff --git a/drivers/staging/rtl8188eu/os_dep/recv_linux.c b/drivers/staging/rtl8188eu/os_dep/recv_linux.c
index da397e4c677..c0fa8fdb9d9 100644
--- a/drivers/staging/rtl8188eu/os_dep/recv_linux.c
+++ b/drivers/staging/rtl8188eu/os_dep/recv_linux.c
@@ -28,13 +28,6 @@
 #include <osdep_intf.h>
 #include <usb_ops.h>
 
-/* init os related resource in struct recv_priv */
-int rtw_os_recv_resource_init(struct recv_priv *precvpriv,
-			      struct adapter *padapter)
-{
-	return _SUCCESS;
-}
-
 /* alloc os related resource in struct recv_frame */
 int rtw_os_recv_resource_alloc(struct adapter *padapter,
 			       struct recv_frame *precvframe)
@@ -44,42 +37,20 @@ int rtw_os_recv_resource_alloc(struct adapter *padapter,
 	return _SUCCESS;
 }
 
-/* free os related resource in struct recv_frame */
-void rtw_os_recv_resource_free(struct recv_priv *precvpriv)
-{
-}
-
 /* alloc os related resource in struct recv_buf */
 int rtw_os_recvbuf_resource_alloc(struct adapter *padapter,
 				  struct recv_buf *precvbuf)
 {
 	int res = _SUCCESS;
 
-	precvbuf->irp_pending = false;
 	precvbuf->purb = usb_alloc_urb(0, GFP_KERNEL);
 	if (precvbuf->purb == NULL)
 		res = _FAIL;
 	precvbuf->pskb = NULL;
 	precvbuf->reuse = false;
-	precvbuf->pallocated_buf = NULL;
-	precvbuf->pbuf = NULL;
-	precvbuf->pdata = NULL;
-	precvbuf->phead = NULL;
-	precvbuf->ptail = NULL;
-	precvbuf->pend = NULL;
-	precvbuf->transfer_len = 0;
-	precvbuf->len = 0;
 	return res;
 }
 
-/* free os related resource in struct recv_buf */
-int rtw_os_recvbuf_resource_free(struct adapter *padapter,
-				 struct recv_buf *precvbuf)
-{
-	usb_free_urb(precvbuf->purb);
-	return _SUCCESS;
-}
-
 void rtw_handle_tkip_mic_err(struct adapter *padapter, u8 bgroup)
 {
 	union iwreq_data wrqu;
@@ -116,11 +87,6 @@ void rtw_handle_tkip_mic_err(struct adapter *padapter, u8 bgroup)
 			    &wrqu, (char *)&ev);
 }
 
-void rtw_hostapd_mlme_rx(struct adapter *padapter,
-			 struct recv_frame *precv_frame)
-{
-}
-
 int rtw_recv_indicatepkt(struct adapter *padapter,
 			 struct recv_frame *precv_frame)
 {
@@ -229,14 +195,12 @@ void rtw_os_read_port(struct adapter *padapter, struct recv_buf *precvbuf)
 {
 	struct recv_priv *precvpriv = &padapter->recvpriv;
 
-	precvbuf->ref_cnt--;
 	/* free skb in recv_buf */
 	dev_kfree_skb_any(precvbuf->pskb);
 	precvbuf->pskb = NULL;
 	precvbuf->reuse = false;
-	if (!precvbuf->irp_pending)
-		rtw_read_port(padapter, precvpriv->ff_hwaddr, 0,
-			      (unsigned char *)precvbuf);
+	rtw_read_port(padapter, precvpriv->ff_hwaddr, 0,
+			(unsigned char *)precvbuf);
 }
 
 static void _rtw_reordering_ctrl_timeout_handler(void *func_context)
diff --git a/drivers/staging/rtl8188eu/os_dep/usb_intf.c b/drivers/staging/rtl8188eu/os_dep/usb_intf.c
index 2e49cd58321..7526b989dcb 100644
--- a/drivers/staging/rtl8188eu/os_dep/usb_intf.c
+++ b/drivers/staging/rtl8188eu/os_dep/usb_intf.c
@@ -62,10 +62,6 @@ static struct usb_device_id rtw_usb_id_tbl[] = {
 
 MODULE_DEVICE_TABLE(usb, rtw_usb_id_tbl);
 
-static struct specific_device_id specific_device_id_tbl[] = {
-	{}		/* empty table for now */
-};
-
 struct rtw_usb_drv {
 	struct usb_driver usbdrv;
 	int drv_registered;
@@ -84,46 +80,6 @@ static struct rtw_usb_drv rtl8188e_usb_drv = {
 
 static struct rtw_usb_drv *usb_drv = &rtl8188e_usb_drv;
 
-static inline int RT_usb_endpoint_dir_in(const struct usb_endpoint_descriptor *epd)
-{
-	return (epd->bEndpointAddress & USB_ENDPOINT_DIR_MASK) == USB_DIR_IN;
-}
-
-static inline int RT_usb_endpoint_dir_out(const struct usb_endpoint_descriptor *epd)
-{
-	return (epd->bEndpointAddress & USB_ENDPOINT_DIR_MASK) == USB_DIR_OUT;
-}
-
-static inline int RT_usb_endpoint_xfer_int(const struct usb_endpoint_descriptor *epd)
-{
-	return (epd->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK) == USB_ENDPOINT_XFER_INT;
-}
-
-static inline int RT_usb_endpoint_xfer_bulk(const struct usb_endpoint_descriptor *epd)
-{
-	return (epd->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK) == USB_ENDPOINT_XFER_BULK;
-}
-
-static inline int RT_usb_endpoint_is_bulk_in(const struct usb_endpoint_descriptor *epd)
-{
-	return RT_usb_endpoint_xfer_bulk(epd) && RT_usb_endpoint_dir_in(epd);
-}
-
-static inline int RT_usb_endpoint_is_bulk_out(const struct usb_endpoint_descriptor *epd)
-{
-	return RT_usb_endpoint_xfer_bulk(epd) && RT_usb_endpoint_dir_out(epd);
-}
-
-static inline int usb_endpoint_is_int(const struct usb_endpoint_descriptor *epd)
-{
-	return RT_usb_endpoint_xfer_int(epd) && RT_usb_endpoint_dir_in(epd);
-}
-
-static inline int RT_usb_endpoint_num(const struct usb_endpoint_descriptor *epd)
-{
-	return epd->bEndpointAddress & USB_ENDPOINT_NUMBER_MASK;
-}
-
 static u8 rtw_init_intf_priv(struct dvobj_priv *dvobj)
 {
 	u8 rst = _SUCCESS;
@@ -132,7 +88,7 @@ static u8 rtw_init_intf_priv(struct dvobj_priv *dvobj)
 
 	dvobj->usb_alloc_vendor_req_buf = rtw_zmalloc(MAX_USB_IO_CTL_SIZE);
 	if (dvobj->usb_alloc_vendor_req_buf == NULL) {
-		DBG_88E("alloc usb_vendor_req_buf failed... /n");
+		DBG_88E("alloc usb_vendor_req_buf failed...\n");
 		rst = _FAIL;
 		goto exit;
 	}
@@ -187,60 +143,35 @@ static struct dvobj_priv *usb_dvobj_init(struct usb_interface *usb_intf)
 	pdvobjpriv->nr_endpoint = piface_desc->bNumEndpoints;
 
 	for (i = 0; i < pdvobjpriv->nr_endpoint; i++) {
+		int ep_num;
 		phost_endp = phost_iface->endpoint + i;
+
 		if (phost_endp) {
 			pendp_desc = &phost_endp->desc;
+			ep_num = usb_endpoint_num(pendp_desc);
 
-			DBG_88E("\nusb_endpoint_descriptor(%d):\n", i);
-			DBG_88E("bLength=%x\n", pendp_desc->bLength);
-			DBG_88E("bDescriptorType=%x\n",
-				pendp_desc->bDescriptorType);
-			DBG_88E("bEndpointAddress=%x\n",
-				pendp_desc->bEndpointAddress);
-			DBG_88E("wMaxPacketSize=%d\n",
-				le16_to_cpu(pendp_desc->wMaxPacketSize));
-			DBG_88E("bInterval=%x\n", pendp_desc->bInterval);
-
-			if (RT_usb_endpoint_is_bulk_in(pendp_desc)) {
-				DBG_88E("RT_usb_endpoint_is_bulk_in = %x\n",
-					RT_usb_endpoint_num(pendp_desc));
-				pdvobjpriv->RtInPipe[pdvobjpriv->RtNumInPipes] = RT_usb_endpoint_num(pendp_desc);
+			if (usb_endpoint_is_bulk_in(pendp_desc)) {
+				pdvobjpriv->RtInPipe[pdvobjpriv->RtNumInPipes] = ep_num;
 				pdvobjpriv->RtNumInPipes++;
-			} else if (usb_endpoint_is_int(pendp_desc)) {
-				DBG_88E("usb_endpoint_is_int = %x, Interval = %x\n",
-					RT_usb_endpoint_num(pendp_desc),
-					pendp_desc->bInterval);
-				pdvobjpriv->RtInPipe[pdvobjpriv->RtNumInPipes] = RT_usb_endpoint_num(pendp_desc);
+			} else if (usb_endpoint_is_int_in(pendp_desc)) {
+				pdvobjpriv->RtInPipe[pdvobjpriv->RtNumInPipes] = ep_num;
 				pdvobjpriv->RtNumInPipes++;
-			} else if (RT_usb_endpoint_is_bulk_out(pendp_desc)) {
-				DBG_88E("RT_usb_endpoint_is_bulk_out = %x\n",
-					RT_usb_endpoint_num(pendp_desc));
-				pdvobjpriv->RtOutPipe[pdvobjpriv->RtNumOutPipes] = RT_usb_endpoint_num(pendp_desc);
+			} else if (usb_endpoint_is_bulk_out(pendp_desc)) {
+				pdvobjpriv->RtOutPipe[pdvobjpriv->RtNumOutPipes] = ep_num;
 				pdvobjpriv->RtNumOutPipes++;
 			}
-			pdvobjpriv->ep_num[i] = RT_usb_endpoint_num(pendp_desc);
+			pdvobjpriv->ep_num[i] = ep_num;
 		}
 	}
 
-	DBG_88E("nr_endpoint=%d, in_num=%d, out_num=%d\n\n",
-		pdvobjpriv->nr_endpoint, pdvobjpriv->RtNumInPipes,
-		pdvobjpriv->RtNumOutPipes);
-
-	if (pusbd->speed == USB_SPEED_HIGH) {
+	if (pusbd->speed == USB_SPEED_HIGH)
 		pdvobjpriv->ishighspeed = true;
-		DBG_88E("USB_SPEED_HIGH\n");
-	} else {
+	else
 		pdvobjpriv->ishighspeed = false;
-		DBG_88E("NON USB_SPEED_HIGH\n");
-	}
 
-	if (rtw_init_intf_priv(pdvobjpriv) == _FAIL) {
-		RT_TRACE(_module_os_intfs_c_, _drv_err_,
-			 ("\n Can't INIT rtw_init_intf_priv\n"));
+	if (rtw_init_intf_priv(pdvobjpriv) == _FAIL)
 		goto free_dvobj;
-	}
 
-	/* 3 misc */
 	sema_init(&(pdvobjpriv->usb_suspend_sema), 0);
 	rtw_reset_continual_urb_error(pdvobjpriv);
 
@@ -288,13 +219,6 @@ static void usb_dvobj_deinit(struct usb_interface *usb_intf)
 
 }
 
-static void chip_by_usb_id(struct adapter *padapter,
-			   const struct usb_device_id *pdid)
-{
-	padapter->chip_type = NULL_CHIP_TYPE;
-	hal_set_hw_type(padapter);
-}
-
 static void usb_intf_start(struct adapter *padapter)
 {
 	RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("+usb_intf_start\n"));
@@ -360,28 +284,6 @@ static void rtw_dev_unload(struct adapter *padapter)
 	RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("-rtw_dev_unload\n"));
 }
 
-static void process_spec_devid(const struct usb_device_id *pdid)
-{
-	u16 vid, pid;
-	u32 flags;
-	int i;
-	int num = sizeof(specific_device_id_tbl) /
-		  sizeof(struct specific_device_id);
-
-	for (i = 0; i < num; i++) {
-		vid = specific_device_id_tbl[i].idVendor;
-		pid = specific_device_id_tbl[i].idProduct;
-		flags = specific_device_id_tbl[i].flags;
-
-		if ((pdid->idVendor == vid) && (pdid->idProduct == pid) &&
-		    (flags&SPEC_DEV_ID_DISABLE_HT)) {
-			rtw_ht_enable = 0;
-			rtw_cbw40_enable = 0;
-			rtw_ampdu_enable = 0;
-		}
-	}
-}
-
 int rtw_hw_suspend(struct adapter *padapter)
 {
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
@@ -396,49 +298,47 @@ int rtw_hw_suspend(struct adapter *padapter)
 		goto error_exit;
 	}
 
-	if (padapter) { /* system suspend */
-		LeaveAllPowerSaveMode(padapter);
+	/* system suspend */
+	LeaveAllPowerSaveMode(padapter);
 
-		DBG_88E("==> rtw_hw_suspend\n");
-		_enter_pwrlock(&pwrpriv->lock);
-		pwrpriv->bips_processing = true;
-		/* s1. */
-		if (pnetdev) {
-			netif_carrier_off(pnetdev);
-			rtw_netif_stop_queue(pnetdev);
-		}
+	DBG_88E("==> rtw_hw_suspend\n");
+	_enter_pwrlock(&pwrpriv->lock);
+	pwrpriv->bips_processing = true;
+	/* s1. */
+	if (pnetdev) {
+		netif_carrier_off(pnetdev);
+		netif_tx_stop_all_queues(pnetdev);
+	}
 
-		/* s2. */
-		rtw_disassoc_cmd(padapter, 500, false);
+	/* s2. */
+	rtw_disassoc_cmd(padapter, 500, false);
 
-		/* s2-2.  indicate disconnect to os */
-		{
-			struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	/* s2-2.  indicate disconnect to os */
+	{
+		struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
-			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
-				_clr_fwstate_(pmlmepriv, _FW_LINKED);
+		if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+			_clr_fwstate_(pmlmepriv, _FW_LINKED);
 
-				rtw_led_control(padapter, LED_CTL_NO_LINK);
+			rtw_led_control(padapter, LED_CTL_NO_LINK);
 
-				rtw_os_indicate_disconnect(padapter);
+			rtw_os_indicate_disconnect(padapter);
 
-				/* donnot enqueue cmd */
-				rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_DISCONNECT, 0);
-			}
+			/* donnot enqueue cmd */
+			rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_DISCONNECT, 0);
 		}
-		/* s2-3. */
-		rtw_free_assoc_resources(padapter, 1);
+	}
+	/* s2-3. */
+	rtw_free_assoc_resources(padapter, 1);
 
-		/* s2-4. */
-		rtw_free_network_queue(padapter, true);
-		rtw_ips_dev_unload(padapter);
-		pwrpriv->rf_pwrstate = rf_off;
-		pwrpriv->bips_processing = false;
+	/* s2-4. */
+	rtw_free_network_queue(padapter, true);
+	rtw_ips_dev_unload(padapter);
+	pwrpriv->rf_pwrstate = rf_off;
+	pwrpriv->bips_processing = false;
+
+	_exit_pwrlock(&pwrpriv->lock);
 
-		_exit_pwrlock(&pwrpriv->lock);
-	} else {
-		goto error_exit;
-	}
 	return 0;
 
 error_exit:
@@ -452,35 +352,32 @@ int rtw_hw_resume(struct adapter *padapter)
 	struct net_device *pnetdev = padapter->pnetdev;
 
 
-	if (padapter) { /* system resume */
-		DBG_88E("==> rtw_hw_resume\n");
-		_enter_pwrlock(&pwrpriv->lock);
-		pwrpriv->bips_processing = true;
-		rtw_reset_drv_sw(padapter);
+	/* system resume */
+	DBG_88E("==> rtw_hw_resume\n");
+	_enter_pwrlock(&pwrpriv->lock);
+	pwrpriv->bips_processing = true;
+	rtw_reset_drv_sw(padapter);
 
-		if (pm_netdev_open(pnetdev, false) != 0) {
-			_exit_pwrlock(&pwrpriv->lock);
-			goto error_exit;
-		}
+	if (pm_netdev_open(pnetdev, false) != 0) {
+		_exit_pwrlock(&pwrpriv->lock);
+		goto error_exit;
+	}
 
-		netif_device_attach(pnetdev);
-		netif_carrier_on(pnetdev);
+	netif_device_attach(pnetdev);
+	netif_carrier_on(pnetdev);
 
-		if (!netif_queue_stopped(pnetdev))
-			netif_start_queue(pnetdev);
-		else
-			netif_wake_queue(pnetdev);
+	if (!netif_queue_stopped(pnetdev))
+		netif_start_queue(pnetdev);
+	else
+		netif_wake_queue(pnetdev);
 
-		pwrpriv->bkeepfwalive = false;
-		pwrpriv->brfoffbyhw = false;
+	pwrpriv->bkeepfwalive = false;
+	pwrpriv->brfoffbyhw = false;
 
-		pwrpriv->rf_pwrstate = rf_on;
-		pwrpriv->bips_processing = false;
+	pwrpriv->rf_pwrstate = rf_on;
+	pwrpriv->bips_processing = false;
 
-		_exit_pwrlock(&pwrpriv->lock);
-	} else {
-		goto error_exit;
-	}
+	_exit_pwrlock(&pwrpriv->lock);
 
 
 	return 0;
@@ -519,7 +416,7 @@ static int rtw_suspend(struct usb_interface *pusb_intf, pm_message_t message)
 	/* s1. */
 	if (pnetdev) {
 		netif_carrier_off(pnetdev);
-		rtw_netif_stop_queue(pnetdev);
+		netif_tx_stop_all_queues(pnetdev);
 	}
 
 	/* s2. */
@@ -591,8 +488,7 @@ int rtw_resume_process(struct adapter *padapter)
 
 	_enter_pwrlock(&pwrpriv->lock);
 	rtw_reset_drv_sw(padapter);
-	if (pwrpriv)
-		pwrpriv->bkeepfwalive = false;
+	pwrpriv->bkeepfwalive = false;
 
 	DBG_88E("bkeepfwalive(%x)\n", pwrpriv->bkeepfwalive);
 	if (pm_netdev_open(pnetdev, true) != 0)
@@ -643,18 +539,12 @@ static struct adapter *rtw_usb_if1_init(struct dvobj_priv *dvobj,
 	dvobj->if1 = padapter;
 
 	padapter->bDriverStopped = true;
-
 	padapter->hw_init_mutex = &usb_drv->hw_init_mutex;
-
-	/* step 1-1., decide the chip_type via vid/pid */
-	chip_by_usb_id(padapter, pdid);
-
-	if (rtw_handle_dualmac(padapter, 1) != _SUCCESS)
-		goto free_adapter;
+	padapter->chip_type = RTL8188E;
 
 	pnetdev = rtw_init_netdev(padapter);
 	if (pnetdev == NULL)
-		goto handle_dualmac;
+		goto free_adapter;
 	SET_NETDEV_DEV(pnetdev, dvobj_to_dev(dvobj));
 	padapter = rtw_netdev_priv(pnetdev);
 
@@ -728,9 +618,6 @@ static struct adapter *rtw_usb_if1_init(struct dvobj_priv *dvobj,
 free_hal_data:
 	if (status != _SUCCESS)
 		kfree(padapter->HalData);
-handle_dualmac:
-	if (status != _SUCCESS)
-		rtw_handle_dualmac(padapter, 0);
 free_adapter:
 	if (status != _SUCCESS) {
 		if (pnetdev)
@@ -767,7 +654,6 @@ static void rtw_usb_if1_deinit(struct adapter *if1)
 	rtw_dev_unload(if1);
 	DBG_88E("+r871xu_dev_remove, hw_init_completed=%d\n",
 		if1->hw_init_completed);
-	rtw_handle_dualmac(if1, 0);
 	rtw_free_drv_sw(if1);
 	if (pnetdev)
 		rtw_free_netdev(pnetdev);
@@ -776,14 +662,11 @@ static void rtw_usb_if1_deinit(struct adapter *if1)
 static int rtw_drv_init(struct usb_interface *pusb_intf, const struct usb_device_id *pdid)
 {
 	struct adapter *if1 = NULL;
-	int status;
+	int status = _FAIL;
 	struct dvobj_priv *dvobj;
 
 	RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("+rtw_drv_init\n"));
 
-	/* step 0. */
-	process_spec_devid(pdid);
-
 	/* Initialize dvobj_priv */
 	dvobj = usb_dvobj_init(pusb_intf);
 	if (dvobj == NULL) {
@@ -807,8 +690,6 @@ static int rtw_drv_init(struct usb_interface *pusb_intf, const struct usb_device
 
 	status = _SUCCESS;
 
-	if (status != _SUCCESS && if1)
-		rtw_usb_if1_deinit(if1);
 free_dvobj:
 	if (status != _SUCCESS)
 		usb_dvobj_deinit(pusb_intf);
diff --git a/drivers/staging/rtl8188eu/os_dep/usb_ops_linux.c b/drivers/staging/rtl8188eu/os_dep/usb_ops_linux.c
index fb0bba85373..ba2a8ab80d7 100644
--- a/drivers/staging/rtl8188eu/os_dep/usb_ops_linux.c
+++ b/drivers/staging/rtl8188eu/os_dep/usb_ops_linux.c
@@ -39,13 +39,6 @@ unsigned int ffaddr2pipehdl(struct dvobj_priv *pdvobj, u32 addr)
 	return pipe;
 }
 
-struct zero_bulkout_context {
-	void *pbuf;
-	void *purb;
-	void *pirp;
-	void *padapter;
-};
-
 void usb_read_mem(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *rmem)
 {
 }
diff --git a/drivers/staging/rtl8188eu/os_dep/xmit_linux.c b/drivers/staging/rtl8188eu/os_dep/xmit_linux.c
index 2c8e3f72dec..400356831c5 100644
--- a/drivers/staging/rtl8188eu/os_dep/xmit_linux.c
+++ b/drivers/staging/rtl8188eu/os_dep/xmit_linux.c
@@ -19,7 +19,6 @@
  ******************************************************************************/
 #define _XMIT_OSDEP_C_
 
-#include <linux/version.h>
 #include <osdep_service.h>
 #include <drv_types.h>
 
@@ -77,10 +76,6 @@ int rtw_endofpktfile(struct pkt_file *pfile)
 	return false;
 }
 
-void rtw_set_tx_chksum_offload(struct sk_buff *pkt, struct pkt_attrib *pattrib)
-{
-}
-
 int rtw_os_xmit_resource_alloc(struct adapter *padapter, struct xmit_buf *pxmitbuf, u32 alloc_sz)
 {
 	int i;
@@ -117,7 +112,6 @@ void rtw_os_xmit_resource_free(struct adapter *padapter,
 
 void rtw_os_pkt_complete(struct adapter *padapter, struct sk_buff *pkt)
 {
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 35))
 	u16	queue;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 
@@ -130,10 +124,6 @@ void rtw_os_pkt_complete(struct adapter *padapter, struct sk_buff *pkt)
 		if (__netif_subqueue_stopped(padapter->pnetdev, queue))
 			netif_wake_subqueue(padapter->pnetdev, queue);
 	}
-#else
-	if (netif_queue_stopped(padapter->pnetdev))
-		netif_wake_queue(padapter->pnetdev);
-#endif
 
 	dev_kfree_skb_any(pkt);
 }
diff --git a/drivers/staging/rtl8192e/rtl8192e/Kconfig b/drivers/staging/rtl8192e/rtl8192e/Kconfig
index ad82bc348a7..282e293da18 100644
--- a/drivers/staging/rtl8192e/rtl8192e/Kconfig
+++ b/drivers/staging/rtl8192e/rtl8192e/Kconfig
@@ -5,4 +5,5 @@ config RTL8192E
 	select WIRELESS_EXT
 	select WEXT_PRIV
 	select CRYPTO
+	select FW_LOADER
 	---help---
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c
index 5f10e4075d3..79d86b9e800 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c
@@ -373,7 +373,7 @@ u32 cmpk_message_handle_rx(struct net_device *dev,
 		default:
 
 			RT_TRACE(COMP_CMDPKT, "---->cmpk_message_handle_rx():"
-				 "unknow CMD Element\n");
+				 "unknown CMD Element\n");
 			return 1;
 		}
 
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c
index abcd22f8fdd..1a95d1f87f9 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c
@@ -228,13 +228,6 @@ bool init_firmware(struct net_device *dev)
 	struct r8192_priv *priv = rtllib_priv(dev);
 	bool			rt_status = true;
 
-	u8	*firmware_img_buf[3] = { &Rtl8192PciEFwBootArray[0],
-					 &Rtl8192PciEFwMainArray[0],
-					 &Rtl8192PciEFwDataArray[0]};
-
-	u32	firmware_img_len[3] = { sizeof(Rtl8192PciEFwBootArray),
-					sizeof(Rtl8192PciEFwMainArray),
-					sizeof(Rtl8192PciEFwDataArray)};
 	u32	file_length = 0;
 	u8	*mapped_file = NULL;
 	u8	init_step = 0;
@@ -257,77 +250,55 @@ bool init_firmware(struct net_device *dev)
 			 " firmware state\n");
 	}
 
-	priv->firmware_source = FW_SOURCE_IMG_FILE;
 	for (init_step = starting_state; init_step <= FW_INIT_STEP2_DATA;
 	     init_step++) {
 		if (rst_opt == OPT_SYSTEM_RESET) {
-			switch (priv->firmware_source) {
-			case FW_SOURCE_IMG_FILE:
-			{
-				if (pfirmware->firmware_buf_size[init_step] == 0) {
-					const char *fw_name[3] = {
-							RTL8192E_BOOT_IMG_FW,
-							RTL8192E_MAIN_IMG_FW,
-							RTL8192E_DATA_IMG_FW
-					};
-					const struct firmware	*fw_entry;
-					int rc;
-					rc = request_firmware(&fw_entry,
-					 fw_name[init_step], &priv->pdev->dev);
-					if (rc < 0) {
-						RT_TRACE(COMP_FIRMWARE, "request firm"
-						 "ware fail!\n");
-						goto download_firmware_fail;
-					}
-					if (fw_entry->size >
+			if (pfirmware->firmware_buf_size[init_step] == 0) {
+				const char *fw_name[3] = {
+					RTL8192E_BOOT_IMG_FW,
+					RTL8192E_MAIN_IMG_FW,
+					RTL8192E_DATA_IMG_FW
+				};
+				const struct firmware *fw_entry;
+				int rc;
+				rc = request_firmware(&fw_entry,
+						      fw_name[init_step],
+						      &priv->pdev->dev);
+				if (rc < 0) {
+					RT_TRACE(COMP_FIRMWARE, "request firmware fail!\n");
+					goto download_firmware_fail;
+				}
+				if (fw_entry->size >
 				    sizeof(pfirmware->firmware_buf[init_step])) {
-						RT_TRACE(COMP_FIRMWARE, "img file size "
+					RT_TRACE(COMP_FIRMWARE, "img file size "
 						 "exceed the container struct "
 						 "buffer fail!\n");
-						goto download_firmware_fail;
-					}
+					goto download_firmware_fail;
+				}
 
-					if (init_step != FW_INIT_STEP1_MAIN) {
-						memcpy(pfirmware->firmware_buf[init_step],
+				if (init_step != FW_INIT_STEP1_MAIN) {
+					memcpy(pfirmware->firmware_buf[init_step],
 					       fw_entry->data, fw_entry->size);
-						pfirmware->firmware_buf_size[init_step] =
-					       fw_entry->size;
+					pfirmware->firmware_buf_size[init_step] =
+						fw_entry->size;
 
-					} else {
-						memset(pfirmware->firmware_buf[init_step],
+				} else {
+					memset(pfirmware->firmware_buf[init_step],
 					       0, 128);
-						memcpy(&pfirmware->firmware_buf[init_step][128],
+					memcpy(&pfirmware->firmware_buf[init_step][128],
 					       fw_entry->data, fw_entry->size);
-						pfirmware->firmware_buf_size[init_step] =
-							 fw_entry->size + 128;
-					}
-
-					if (rst_opt == OPT_SYSTEM_RESET)
-						release_firmware(fw_entry);
+					pfirmware->firmware_buf_size[init_step] =
+						fw_entry->size + 128;
 				}
-				mapped_file = pfirmware->firmware_buf[init_step];
-				file_length = pfirmware->firmware_buf_size[init_step];
-				break;
-			}
-			case FW_SOURCE_HEADER_FILE:
-				mapped_file =  firmware_img_buf[init_step];
-				file_length  = firmware_img_len[init_step];
-				if (init_step == FW_INIT_STEP2_DATA) {
-					memcpy(pfirmware->firmware_buf[init_step], mapped_file, file_length);
-					pfirmware->firmware_buf_size[init_step] = file_length;
-				}
-				break;
 
-			default:
-				break;
+				if (rst_opt == OPT_SYSTEM_RESET)
+					release_firmware(fw_entry);
 			}
-
-
-		} else if (rst_opt == OPT_FIRMWARE_RESET) {
-			mapped_file = pfirmware->firmware_buf[init_step];
-			file_length = pfirmware->firmware_buf_size[init_step];
 		}
 
+		mapped_file = pfirmware->firmware_buf[init_step];
+		file_length = pfirmware->firmware_buf_size[init_step];
+
 		rt_status = fw_download_code(dev, mapped_file, file_length);
 		if (!rt_status) {
 			goto download_firmware_fail;
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.h
index 06d6abc8345..94fa16b4993 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.h
@@ -43,11 +43,6 @@ enum desc_packet_type {
 	DESC_PACKET_TYPE_NORMAL = 1,
 };
 
-enum firmware_source {
-	FW_SOURCE_IMG_FILE = 0,
-	FW_SOURCE_HEADER_FILE = 1,
-};
-
 enum firmware_status {
 	FW_STATUS_0_INIT = 0,
 	FW_STATUS_1_MOVE_BOOT_CODE = 1,
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.c
index 08e7dbb6694..6767b5965c1 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.c
@@ -20,2777 +20,6 @@
 
 #include "r8192E_hwimg.h"
 
-u8 Rtl8192PciEFwBootArray[BootArrayLengthPciE] = {
-0x10,0x00,0x00,0x01,0x00,0x00,0x00,0x00,0x3c,0x08,0xbf,0xc0,0x25,0x08,0x00,0x08,
-0x3c,0x09,0xb0,0x03,0xad,0x28,0x00,0x20,0x40,0x80,0x68,0x00,0x00,0x00,0x00,0x00,
-0x3c,0x0a,0xd0,0x00,0x40,0x8a,0x60,0x00,0x00,0x00,0x00,0x00,0x3c,0x08,0x80,0x01,
-0x25,0x08,0xa8,0x04,0x24,0x09,0x00,0x01,0x3c,0x01,0x7f,0xff,0x34,0x21,0xff,0xff,
-0x01,0x01,0x50,0x24,0x00,0x09,0x48,0x40,0x35,0x29,0x00,0x01,0x01,0x2a,0x10,0x2b,
-0x14,0x40,0xff,0xfc,0x00,0x00,0x00,0x00,0x3c,0x0a,0x00,0x00,0x25,0x4a,0x00,0x00,
-0x4c,0x8a,0x00,0x00,0x4c,0x89,0x08,0x00,0x00,0x00,0x00,0x00,0x3c,0x08,0x80,0x01,
-0x25,0x08,0xa8,0x04,0x3c,0x01,0x80,0x00,0x01,0x21,0x48,0x25,0x3c,0x0a,0xbf,0xc0,
-0x25,0x4a,0x00,0x7c,0x3c,0x0b,0xb0,0x03,0xad,0x6a,0x00,0x20,0xad,0x00,0x00,0x00,
-0x21,0x08,0x00,0x04,0x01,0x09,0x10,0x2b,0x14,0x40,0xff,0xf8,0x00,0x00,0x00,0x00,
-0x3c,0x08,0x80,0x01,0x25,0x08,0x7f,0xff,0x24,0x09,0x00,0x01,0x3c,0x01,0x7f,0xff,
-0x34,0x21,0xff,0xff,0x01,0x01,0x50,0x24,0x00,0x09,0x48,0x40,0x35,0x29,0x00,0x01,
-0x01,0x2a,0x10,0x2b,0x14,0x40,0xff,0xfc,0x00,0x00,0x00,0x00,0x3c,0x0a,0x80,0x01,
-0x25,0x4a,0x00,0x00,0x3c,0x01,0x7f,0xff,0x34,0x21,0xff,0xff,0x01,0x41,0x50,0x24,
-0x3c,0x09,0x00,0x01,0x35,0x29,0x7f,0xff,0x4c,0x8a,0x20,0x00,0x4c,0x89,0x28,0x00,
-0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x24,0x08,0x04,0x10,
-0x00,0x00,0x00,0x00,0x40,0x88,0xa0,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
-0x3c,0x08,0xbf,0xc0,0x00,0x00,0x00,0x00,0x8d,0x09,0x00,0x00,0x00,0x00,0x00,0x00,
-0x3c,0x0a,0xbf,0xc0,0x25,0x4a,0x01,0x20,0x3c,0x0b,0xb0,0x03,0xad,0x6a,0x00,0x20,
-0x3c,0x08,0xb0,0x03,0x8d,0x09,0x00,0x00,0x00,0x00,0x00,0x00,0x35,0x29,0x00,0x10,
-0xad,0x09,0x00,0x00,0x00,0x00,0x00,0x00,0x3c,0x08,0x80,0x00,0x25,0x08,0x4b,0x94,
-0x01,0x00,0x00,0x08,0x00,0x00,0x00,0x00,};
-
-u8 Rtl8192PciEFwMainArray[MainArrayLengthPciE] = {
-0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
-0x40,0x04,0x68,0x00,0x40,0x05,0x70,0x00,0x40,0x06,0x40,0x00,0x0c,0x00,0x12,0x98,
-0x00,0x00,0x00,0x00,0x40,0x1a,0x68,0x00,0x33,0x5b,0x00,0x3c,0x17,0x60,0x00,0x09,
-0x00,0x00,0x00,0x00,0x40,0x1b,0x60,0x00,0x00,0x00,0x00,0x00,0x03,0x5b,0xd0,0x24,
-0x40,0x1a,0x70,0x00,0x03,0x40,0x00,0x08,0x42,0x00,0x00,0x10,0x00,0x00,0x00,0x00,
-0x00,0x00,0x00,0x00,0x3c,0x02,0xff,0xff,0x34,0x42,0xff,0xff,0x8c,0x43,0x00,0x00,
-0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,0x34,0x63,0x00,0x20,0x24,0x42,0x00,0xd0,
-0xac,0x62,0x00,0x00,0x00,0x00,0x20,0x21,0x27,0x85,0x8b,0x70,0x00,0x85,0x18,0x21,
-0x24,0x84,0x00,0x01,0x28,0x82,0x00,0x0a,0x14,0x40,0xff,0xfc,0xa0,0x60,0x00,0x00,
-0x27,0x82,0x8b,0x7a,0x24,0x04,0x00,0x06,0x24,0x84,0xff,0xff,0xa4,0x40,0x00,0x00,
-0x04,0x81,0xff,0xfd,0x24,0x42,0x00,0x02,0x24,0x02,0x00,0x03,0xa3,0x82,0x8b,0x70,
-0x24,0x02,0x00,0x0a,0x24,0x03,0x09,0xc4,0xa3,0x82,0x8b,0x72,0x24,0x02,0x00,0x04,
-0x24,0x04,0x00,0x01,0x24,0x05,0x00,0x02,0xa7,0x83,0x8b,0x86,0xa3,0x82,0x8b,0x78,
-0x24,0x03,0x04,0x00,0x24,0x02,0x02,0x00,0xaf,0x83,0x8b,0x8c,0xa3,0x85,0x8b,0x79,
-0xa7,0x82,0x8b,0x7a,0xa7,0x84,0x8b,0x7c,0xaf,0x84,0x8b,0x88,0xa3,0x84,0x8b,0x71,
-0xa3,0x80,0x8b,0x73,0xa3,0x80,0x8b,0x74,0xa3,0x80,0x8b,0x75,0xa3,0x84,0x8b,0x76,
-0xa3,0x85,0x8b,0x77,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x3c,0x03,0xb0,0x03,
-0x3c,0x02,0x80,0x00,0x24,0x42,0x01,0x7c,0x34,0x63,0x00,0x20,0xac,0x62,0x00,0x00,
-0x27,0x84,0x8b,0x98,0x00,0x00,0x10,0x21,0x24,0x42,0x00,0x01,0x00,0x02,0x16,0x00,
-0x00,0x02,0x16,0x03,0x28,0x43,0x00,0x03,0xac,0x80,0xff,0xfc,0xa0,0x80,0x00,0x00,
-0x14,0x60,0xff,0xf9,0x24,0x84,0x00,0x0c,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,
-0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,0x34,0x63,0x00,0x20,0x24,0x42,0x01,0xc0,
-0x3c,0x08,0xb0,0x03,0xac,0x62,0x00,0x00,0x35,0x08,0x00,0x70,0x8d,0x02,0x00,0x00,
-0x00,0xa0,0x48,0x21,0x00,0x04,0x26,0x00,0x00,0x02,0x2a,0x43,0x00,0x06,0x36,0x00,
-0x00,0x07,0x3e,0x00,0x00,0x02,0x12,0x03,0x29,0x23,0x00,0x03,0x00,0x04,0x56,0x03,
-0x00,0x06,0x36,0x03,0x00,0x07,0x3e,0x03,0x30,0x48,0x00,0x01,0x10,0x60,0x00,0x11,
-0x30,0xa5,0x00,0x07,0x24,0x02,0x00,0x02,0x00,0x49,0x10,0x23,0x00,0x45,0x10,0x07,
-0x30,0x42,0x00,0x01,0x10,0x40,0x00,0x66,0x00,0x00,0x00,0x00,0x8f,0xa2,0x00,0x10,
-0x00,0x00,0x00,0x00,0x00,0x02,0x21,0x43,0x11,0x00,0x00,0x10,0x00,0x07,0x20,0x0b,
-0x15,0x20,0x00,0x06,0x24,0x02,0x00,0x01,0x3c,0x02,0xb0,0x05,0x34,0x42,0x01,0x20,
-0xa4,0x44,0x00,0x00,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x11,0x22,0x00,0x04,
-0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x05,0x08,0x00,0x00,0x94,0x34,0x42,0x01,0x24,
-0x3c,0x02,0xb0,0x05,0x08,0x00,0x00,0x94,0x34,0x42,0x01,0x22,0x15,0x20,0x00,0x54,
-0x24,0x02,0x00,0x01,0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0x74,0x90,0x43,0x00,0x00,
-0x00,0x00,0x00,0x00,0xaf,0x83,0x8b,0x94,0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0x70,
-0x90,0x43,0x00,0x00,0x00,0x00,0x00,0x00,0x30,0x6b,0x00,0x08,0x11,0x60,0x00,0x18,
-0x00,0x09,0x28,0x40,0x00,0x00,0x40,0x21,0x27,0x85,0x8b,0x90,0x8c,0xa3,0x00,0x00,
-0x8c,0xa2,0x00,0x04,0x00,0x00,0x00,0x00,0x00,0x62,0x38,0x23,0x00,0x43,0x10,0x2a,
-0x10,0x40,0x00,0x3d,0x00,0x00,0x00,0x00,0xac,0xa7,0x00,0x00,0x25,0x02,0x00,0x01,
-0x00,0x02,0x16,0x00,0x00,0x02,0x46,0x03,0x29,0x03,0x00,0x03,0x14,0x60,0xff,0xf3,
-0x24,0xa5,0x00,0x0c,0x3c,0x03,0xb0,0x03,0x34,0x63,0x00,0x70,0x90,0x62,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x4b,0x10,0x23,0xa0,0x62,0x00,0x00,0x00,0x09,0x28,0x40,
-0x00,0xa9,0x10,0x21,0x00,0x02,0x10,0x80,0x27,0x83,0x8b,0x98,0x00,0x0a,0x20,0x0b,
-0x00,0x43,0x18,0x21,0x10,0xc0,0x00,0x05,0x00,0x00,0x38,0x21,0x80,0x62,0x00,0x01,
-0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x05,0x00,0x00,0x00,0x00,0x80,0x62,0x00,0x00,
-0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x03,0x00,0xa9,0x10,0x21,0x24,0x07,0x00,0x01,
-0x00,0xa9,0x10,0x21,0x00,0x02,0x30,0x80,0x27,0x82,0x8b,0x98,0xa0,0x67,0x00,0x01,
-0x00,0xc2,0x38,0x21,0x80,0xe3,0x00,0x01,0x00,0x00,0x00,0x00,0x10,0x60,0x00,0x07,
-0x00,0x00,0x00,0x00,0x27,0x83,0x8b,0x90,0x00,0xc3,0x18,0x21,0x8c,0x62,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x44,0x10,0x21,0xac,0x62,0x00,0x00,0x27,0x85,0x8b,0x94,
-0x27,0x82,0x8b,0x90,0x00,0xc5,0x28,0x21,0x00,0xc2,0x10,0x21,0x8c,0x43,0x00,0x00,
-0x8c,0xa4,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x64,0x18,0x2a,0x14,0x60,0x00,0x03,
-0x24,0x02,0x00,0x01,0x03,0xe0,0x00,0x08,0xa0,0xe2,0x00,0x00,0xa0,0xe0,0x00,0x00,
-0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x08,0x00,0x00,0xb7,0xac,0xa0,0x00,0x00,
-0x11,0x22,0x00,0x08,0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0x7c,
-0x90,0x43,0x00,0x00,0x00,0x00,0x00,0x00,0xaf,0x83,0x8b,0xac,0x08,0x00,0x00,0xa7,
-0x3c,0x02,0xb0,0x03,0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0x78,0x90,0x43,0x00,0x00,
-0x00,0x00,0x00,0x00,0xaf,0x83,0x8b,0xa0,0x08,0x00,0x00,0xa7,0x3c,0x02,0xb0,0x03,
-0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,0x34,0x63,0x00,0x20,0x24,0x42,0x04,0x10,
-0x3c,0x05,0xb0,0x03,0xac,0x62,0x00,0x00,0x34,0xa5,0x00,0x70,0x8c,0xa2,0x00,0x00,
-0x90,0x84,0x00,0x08,0x3c,0x06,0xb0,0x03,0x00,0x02,0x16,0x00,0x2c,0x83,0x00,0x03,
-0x34,0xc6,0x00,0x72,0x24,0x07,0x00,0x01,0x10,0x60,0x00,0x11,0x00,0x02,0x2f,0xc2,
-0x90,0xc2,0x00,0x00,0x00,0x00,0x18,0x21,0x00,0x02,0x16,0x00,0x10,0xa7,0x00,0x09,
-0x00,0x02,0x16,0x03,0x14,0x80,0x00,0x0c,0x30,0x43,0x00,0x03,0x83,0x82,0x8b,0x98,
-0x00,0x00,0x00,0x00,0x00,0x02,0x10,0x80,0x00,0x43,0x10,0x21,0x00,0x02,0x16,0x00,
-0x00,0x02,0x1e,0x03,0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0x72,0xa0,0x43,0x00,0x00,
-0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x30,0x45,0x00,0x05,0x10,0x87,0x00,0x04,
-0x30,0x43,0x00,0x06,0x93,0x82,0x8b,0xb0,0x08,0x00,0x01,0x1f,0x00,0x43,0x10,0x21,
-0x83,0x82,0x8b,0xa4,0x00,0x00,0x00,0x00,0x00,0x02,0x10,0x40,0x08,0x00,0x01,0x1f,
-0x00,0x45,0x10,0x21,0x10,0x80,0x00,0x05,0x00,0x00,0x18,0x21,0x24,0x63,0x00,0x01,
-0x00,0x64,0x10,0x2b,0x14,0x40,0xff,0xfd,0x00,0x00,0x00,0x00,0x03,0xe0,0x00,0x08,
-0x00,0x00,0x00,0x00,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,0x24,0x42,0x04,0xe4,
-0x3c,0x04,0xb0,0x02,0x34,0x63,0x00,0x20,0xac,0x62,0x00,0x00,0x34,0x84,0x00,0x08,
-0x24,0x02,0x00,0x01,0xaf,0x84,0x8b,0xc0,0xa3,0x82,0x8b,0xd0,0xa7,0x80,0x8b,0xc4,
-0xa7,0x80,0x8b,0xc6,0xaf,0x80,0x8b,0xc8,0xaf,0x80,0x8b,0xcc,0x03,0xe0,0x00,0x08,
-0x00,0x00,0x00,0x00,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,0x34,0x63,0x00,0x20,
-0x24,0x42,0x05,0x24,0x3c,0x04,0xb0,0x03,0xac,0x62,0x00,0x00,0x34,0x84,0x00,0xac,
-0x80,0xa2,0x00,0x15,0x8c,0x83,0x00,0x00,0x27,0xbd,0xff,0xf0,0x00,0x43,0x10,0x21,
-0xac,0x82,0x00,0x00,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x10,0x3c,0x02,0xb0,0x03,
-0x3c,0x03,0x80,0x00,0x34,0x42,0x00,0x20,0x24,0x63,0x05,0x5c,0x27,0xbd,0xff,0xe0,
-0xac,0x43,0x00,0x00,0xaf,0xb1,0x00,0x14,0xaf,0xb0,0x00,0x10,0xaf,0xbf,0x00,0x18,
-0x8f,0x90,0x8b,0xc0,0x0c,0x00,0x02,0x98,0x00,0x80,0x88,0x21,0x14,0x40,0x00,0x2a,
-0x3c,0x02,0x00,0x80,0x16,0x20,0x00,0x02,0x34,0x42,0x02,0x01,0x24,0x02,0x02,0x01,
-0xae,0x02,0x00,0x00,0x97,0x84,0x8b,0xc4,0x97,0x82,0x8b,0xc6,0x3c,0x03,0xb0,0x02,
-0x00,0x83,0x20,0x21,0x24,0x42,0x00,0x04,0xa7,0x82,0x8b,0xc6,0xa4,0x82,0x00,0x00,
-0x8f,0x84,0x8b,0xc8,0x8f,0x82,0x8b,0xc0,0x93,0x85,0x8b,0x72,0x24,0x84,0x00,0x01,
-0x24,0x42,0x00,0x04,0x24,0x03,0x8f,0xff,0x3c,0x07,0xb0,0x06,0x3c,0x06,0xb0,0x03,
-0x00,0x43,0x10,0x24,0x00,0x85,0x28,0x2a,0x34,0xe7,0x80,0x18,0xaf,0x82,0x8b,0xc0,
-0xaf,0x84,0x8b,0xc8,0x10,0xa0,0x00,0x08,0x34,0xc6,0x01,0x08,0x8f,0x83,0x8b,0xcc,
-0x8f,0x84,0x8b,0x8c,0x8c,0xc2,0x00,0x00,0x00,0x64,0x18,0x21,0x00,0x43,0x10,0x2b,
-0x14,0x40,0x00,0x09,0x00,0x00,0x00,0x00,0x8c,0xe2,0x00,0x00,0x3c,0x03,0x0f,0x00,
-0x3c,0x04,0x04,0x00,0x00,0x43,0x10,0x24,0x10,0x44,0x00,0x03,0x00,0x00,0x00,0x00,
-0x0c,0x00,0x04,0x96,0x00,0x00,0x00,0x00,0x8f,0xbf,0x00,0x18,0x7b,0xb0,0x00,0xbc,
-0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x20,0x27,0xbd,0xff,0xd8,0x3c,0x02,0xb0,0x03,
-0x3c,0x03,0x80,0x00,0x24,0x63,0x06,0x48,0xaf,0xb0,0x00,0x10,0x34,0x42,0x00,0x20,
-0x8f,0x90,0x8b,0xc0,0xac,0x43,0x00,0x00,0xaf,0xb3,0x00,0x1c,0xaf,0xb2,0x00,0x18,
-0xaf,0xb1,0x00,0x14,0xaf,0xbf,0x00,0x20,0x00,0x80,0x88,0x21,0x00,0xa0,0x90,0x21,
-0x0c,0x00,0x02,0x98,0x00,0xc0,0x98,0x21,0x24,0x07,0x8f,0xff,0x14,0x40,0x00,0x19,
-0x26,0x03,0x00,0x04,0x24,0x02,0x0e,0x03,0xae,0x02,0x00,0x00,0x00,0x67,0x80,0x24,
-0x26,0x02,0x00,0x04,0xae,0x11,0x00,0x00,0x00,0x47,0x80,0x24,0x97,0x86,0x8b,0xc4,
-0x26,0x03,0x00,0x04,0xae,0x12,0x00,0x00,0x00,0x67,0x80,0x24,0xae,0x13,0x00,0x00,
-0x8f,0x84,0x8b,0xc0,0x3c,0x02,0xb0,0x02,0x97,0x85,0x8b,0xc6,0x00,0xc2,0x30,0x21,
-0x8f,0x82,0x8b,0xc8,0x24,0x84,0x00,0x10,0x24,0xa5,0x00,0x10,0x00,0x87,0x20,0x24,
-0x24,0x42,0x00,0x01,0xa7,0x85,0x8b,0xc6,0xaf,0x84,0x8b,0xc0,0xaf,0x82,0x8b,0xc8,
-0xa4,0xc5,0x00,0x00,0x8f,0xbf,0x00,0x20,0x7b,0xb2,0x00,0xfc,0x7b,0xb0,0x00,0xbc,
-0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x28,0x27,0xbd,0xff,0xe8,0xaf,0xbf,0x00,0x10,
-0x94,0x82,0x00,0x04,0x00,0x00,0x00,0x00,0x30,0x42,0xe0,0x00,0x14,0x40,0x00,0x14,
-0x00,0x00,0x00,0x00,0x90,0x82,0x00,0x02,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0xfc,
-0x00,0x82,0x28,0x21,0x8c,0xa4,0x00,0x00,0x3c,0x02,0x00,0x70,0x8c,0xa6,0x00,0x08,
-0x00,0x82,0x10,0x21,0x2c,0x43,0x00,0x06,0x10,0x60,0x00,0x09,0x3c,0x03,0x80,0x01,
-0x00,0x02,0x10,0x80,0x24,0x63,0x01,0xe8,0x00,0x43,0x10,0x21,0x8c,0x44,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x80,0x00,0x08,0x00,0x00,0x00,0x00,0xaf,0x86,0x80,0x14,
-0x8f,0xbf,0x00,0x10,0x00,0x00,0x00,0x00,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x18,
-0x8c,0xa4,0x00,0x00,0x0c,0x00,0x17,0x84,0x00,0x00,0x00,0x00,0x08,0x00,0x01,0xdc,
-0x00,0x00,0x00,0x00,0x0c,0x00,0x24,0x49,0x00,0xc0,0x20,0x21,0x08,0x00,0x01,0xdc,
-0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x03,0x34,0x42,0x01,0x08,0x8c,0x44,0x00,0x00,
-0x8f,0x82,0x80,0x18,0x3c,0x03,0x00,0x0f,0x34,0x63,0x42,0x40,0x00,0x43,0x10,0x21,
-0x00,0x82,0x20,0x2b,0x10,0x80,0x00,0x09,0x24,0x03,0x00,0x05,0x8f,0x82,0x83,0x60,
-0x00,0x00,0x00,0x00,0x24,0x42,0x00,0x01,0xaf,0x82,0x83,0x60,0x10,0x43,0x00,0x03,
-0x00,0x00,0x00,0x00,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x3c,0x03,0xb0,0x03,
-0x8c,0x63,0x01,0x08,0x24,0x02,0x00,0x01,0xa3,0x82,0x80,0x11,0xaf,0x80,0x83,0x60,
-0xaf,0x83,0x80,0x18,0x08,0x00,0x01,0xf9,0x00,0x00,0x00,0x00,0x30,0x84,0x00,0xff,
-0x14,0x80,0x00,0x2f,0x00,0x00,0x00,0x00,0x8f,0x82,0x80,0x14,0xa3,0x85,0x83,0x93,
-0x10,0x40,0x00,0x2b,0x2c,0xa2,0x00,0x04,0x14,0x40,0x00,0x06,0x00,0x05,0x10,0x40,
-0x24,0xa2,0xff,0xfc,0x2c,0x42,0x00,0x08,0x10,0x40,0x00,0x09,0x24,0xa2,0xff,0xf0,
-0x00,0x05,0x10,0x40,0x27,0x84,0x83,0x9c,0x00,0x44,0x10,0x21,0x94,0x43,0x00,0x00,
-0x00,0x00,0x00,0x00,0x24,0x63,0x00,0x01,0x03,0xe0,0x00,0x08,0xa4,0x43,0x00,0x00,
-0x2c,0x42,0x00,0x10,0x14,0x40,0x00,0x0a,0x00,0x05,0x10,0x40,0x24,0xa2,0xff,0xe0,
-0x2c,0x42,0x00,0x10,0x14,0x40,0x00,0x06,0x00,0x05,0x10,0x40,0x24,0xa2,0xff,0xd0,
-0x2c,0x42,0x00,0x10,0x10,0x40,0x00,0x09,0x24,0xa2,0xff,0xc0,0x00,0x05,0x10,0x40,
-0x27,0x84,0x83,0x9c,0x00,0x44,0x10,0x21,0x94,0x43,0xff,0xf8,0x00,0x00,0x00,0x00,
-0x24,0x63,0x00,0x01,0x03,0xe0,0x00,0x08,0xa4,0x43,0xff,0xf8,0x2c,0x42,0x00,0x10,
-0x10,0x40,0x00,0x07,0x00,0x05,0x10,0x40,0x27,0x84,0x83,0x9c,0x00,0x44,0x10,0x21,
-0x94,0x43,0xff,0xf8,0x00,0x00,0x00,0x00,0x24,0x63,0x00,0x01,0xa4,0x43,0xff,0xf8,
-0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x8f,0x86,0x8b,0xc0,0x8f,0x82,0x80,0x14,
-0x27,0xbd,0xff,0xe8,0xaf,0xbf,0x00,0x10,0x10,0x40,0x00,0x2a,0x00,0xc0,0x38,0x21,
-0x24,0x02,0x00,0x07,0x24,0x03,0xff,0x9c,0xa3,0x82,0x83,0x9b,0xa3,0x83,0x83,0x9a,
-0x27,0x8a,0x83,0x98,0x00,0x00,0x20,0x21,0x24,0x09,0x8f,0xff,0x00,0x04,0x10,0x80,
-0x00,0x4a,0x28,0x21,0x8c,0xa2,0x00,0x00,0x24,0xe3,0x00,0x04,0x24,0x88,0x00,0x01,
-0xac,0xe2,0x00,0x00,0x10,0x80,0x00,0x02,0x00,0x69,0x38,0x24,0xac,0xa0,0x00,0x00,
-0x31,0x04,0x00,0xff,0x2c,0x82,0x00,0x27,0x14,0x40,0xff,0xf5,0x00,0x04,0x10,0x80,
-0x97,0x83,0x8b,0xc6,0x97,0x85,0x8b,0xc4,0x3c,0x02,0xb0,0x02,0x24,0x63,0x00,0x9c,
-0x00,0xa2,0x28,0x21,0x3c,0x04,0xb0,0x06,0xa7,0x83,0x8b,0xc6,0x34,0x84,0x80,0x18,
-0xa4,0xa3,0x00,0x00,0x8c,0x85,0x00,0x00,0x24,0x02,0x8f,0xff,0x24,0xc6,0x00,0x9c,
-0x3c,0x03,0x0f,0x00,0x00,0xc2,0x30,0x24,0x00,0xa3,0x28,0x24,0x3c,0x02,0x04,0x00,
-0xaf,0x86,0x8b,0xc0,0x10,0xa2,0x00,0x03,0x00,0x00,0x00,0x00,0x0c,0x00,0x04,0x96,
-0x00,0x00,0x00,0x00,0x8f,0xbf,0x00,0x10,0x00,0x00,0x00,0x00,0x03,0xe0,0x00,0x08,
-0x27,0xbd,0x00,0x18,0x8f,0x86,0x8b,0xc0,0x27,0xbd,0xff,0xc8,0x24,0x02,0x00,0x08,
-0x24,0x03,0x00,0x20,0xaf,0xbf,0x00,0x30,0xa3,0xa2,0x00,0x13,0xa3,0xa3,0x00,0x12,
-0xa7,0xa4,0x00,0x10,0x00,0xc0,0x28,0x21,0x27,0xa9,0x00,0x10,0x00,0x00,0x38,0x21,
-0x24,0x08,0x8f,0xff,0x00,0x07,0x10,0x80,0x00,0x49,0x10,0x21,0x8c,0x44,0x00,0x00,
-0x24,0xe3,0x00,0x01,0x30,0x67,0x00,0xff,0x24,0xa2,0x00,0x04,0x2c,0xe3,0x00,0x08,
-0xac,0xa4,0x00,0x00,0x14,0x60,0xff,0xf7,0x00,0x48,0x28,0x24,0x97,0x83,0x8b,0xc6,
-0x97,0x85,0x8b,0xc4,0x3c,0x02,0xb0,0x02,0x24,0x63,0x00,0x20,0x00,0xa2,0x28,0x21,
-0x3c,0x04,0xb0,0x06,0xa7,0x83,0x8b,0xc6,0x34,0x84,0x80,0x18,0xa4,0xa3,0x00,0x00,
-0x8c,0x85,0x00,0x00,0x24,0x02,0x8f,0xff,0x24,0xc6,0x00,0x20,0x3c,0x03,0x0f,0x00,
-0x00,0xc2,0x30,0x24,0x00,0xa3,0x28,0x24,0x3c,0x02,0x04,0x00,0xaf,0x86,0x8b,0xc0,
-0x10,0xa2,0x00,0x03,0x00,0x00,0x00,0x00,0x0c,0x00,0x04,0x96,0x00,0x00,0x00,0x00,
-0x8f,0xbf,0x00,0x30,0x00,0x00,0x00,0x00,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x38,
-0x93,0x82,0x8b,0xd0,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x11,0x24,0x06,0x00,0x01,
-0x8f,0x82,0x8b,0xc8,0x3c,0x05,0xb0,0x06,0x3c,0x04,0xb0,0x03,0x34,0xa5,0x80,0x18,
-0x34,0x84,0x01,0x08,0x14,0x40,0x00,0x09,0x00,0x00,0x30,0x21,0x97,0x82,0x8b,0xc4,
-0x8c,0x84,0x00,0x00,0x3c,0x03,0xb0,0x02,0x00,0x43,0x10,0x21,0xaf,0x84,0x8b,0xcc,
-0xa7,0x80,0x8b,0xc6,0xac,0x40,0x00,0x00,0xac,0x40,0x00,0x04,0x8c,0xa2,0x00,0x00,
-0x03,0xe0,0x00,0x08,0x00,0xc0,0x10,0x21,0x8f,0x86,0x8b,0xc0,0x8f,0x82,0x8b,0xc8,
-0x27,0xbd,0xff,0xe8,0xaf,0xbf,0x00,0x10,0x00,0xc0,0x40,0x21,0x14,0x40,0x00,0x0a,
-0x00,0x40,0x50,0x21,0x00,0x00,0x38,0x21,0x27,0x89,0x83,0x68,0x24,0xe2,0x00,0x01,
-0x00,0x07,0x18,0x80,0x30,0x47,0x00,0xff,0x00,0x69,0x18,0x21,0x2c,0xe2,0x00,0x0a,
-0x14,0x40,0xff,0xfa,0xac,0x60,0x00,0x00,0x3c,0x02,0x00,0x80,0x10,0x82,0x00,0x6f,
-0x00,0x00,0x00,0x00,0x97,0x82,0x83,0x6e,0x00,0x00,0x00,0x00,0x24,0x42,0x00,0x01,
-0xa7,0x82,0x83,0x6e,0x90,0xa3,0x00,0x15,0x97,0x82,0x83,0x70,0x00,0x03,0x1e,0x00,
-0x00,0x03,0x1e,0x03,0x00,0x43,0x10,0x21,0xa7,0x82,0x83,0x70,0x8c,0xa4,0x00,0x20,
-0x3c,0x02,0x00,0x60,0x3c,0x03,0x00,0x20,0x00,0x82,0x20,0x24,0x10,0x83,0x00,0x54,
-0x00,0x00,0x00,0x00,0x14,0x80,0x00,0x47,0x00,0x00,0x00,0x00,0x97,0x82,0x83,0x74,
-0x00,0x00,0x00,0x00,0x24,0x42,0x00,0x01,0xa7,0x82,0x83,0x74,0x84,0xa3,0x00,0x06,
-0x8f,0x82,0x83,0x84,0x00,0x00,0x00,0x00,0x00,0x43,0x10,0x21,0xaf,0x82,0x83,0x84,
-0x25,0x42,0x00,0x01,0x28,0x43,0x27,0x10,0xaf,0x82,0x8b,0xc8,0x10,0x60,0x00,0x09,
-0x24,0x02,0x00,0x04,0x93,0x83,0x80,0x11,0x24,0x02,0x00,0x01,0x10,0x62,0x00,0x05,
-0x24,0x02,0x00,0x04,0x8f,0xbf,0x00,0x10,0x00,0x00,0x00,0x00,0x03,0xe0,0x00,0x08,
-0x27,0xbd,0x00,0x18,0x24,0x03,0x00,0x28,0xa3,0x83,0x83,0x6a,0xa3,0x82,0x83,0x6b,
-0x90,0xa2,0x00,0x18,0x93,0x83,0x83,0x93,0x00,0x00,0x38,0x21,0x00,0x02,0x16,0x00,
-0x00,0x02,0x16,0x03,0xa7,0x82,0x83,0x7e,0xa3,0x83,0x83,0x8c,0x27,0x89,0x83,0x68,
-0x24,0x05,0x8f,0xff,0x00,0x07,0x10,0x80,0x00,0x49,0x10,0x21,0x8c,0x44,0x00,0x00,
-0x24,0xe3,0x00,0x01,0x30,0x67,0x00,0xff,0x25,0x02,0x00,0x04,0x2c,0xe3,0x00,0x0a,
-0xad,0x04,0x00,0x00,0x14,0x60,0xff,0xf7,0x00,0x45,0x40,0x24,0x97,0x83,0x8b,0xc6,
-0x97,0x85,0x8b,0xc4,0x3c,0x02,0xb0,0x02,0x24,0x63,0x00,0x28,0x00,0xa2,0x28,0x21,
-0x3c,0x04,0xb0,0x06,0xa7,0x83,0x8b,0xc6,0x34,0x84,0x80,0x18,0xa4,0xa3,0x00,0x00,
-0x8c,0x85,0x00,0x00,0x24,0x02,0x8f,0xff,0x24,0xc6,0x00,0x28,0x3c,0x03,0x0f,0x00,
-0x00,0xc2,0x30,0x24,0x00,0xa3,0x28,0x24,0x3c,0x02,0x04,0x00,0xaf,0x86,0x8b,0xc0,
-0x10,0xa2,0x00,0x03,0x00,0x00,0x00,0x00,0x0c,0x00,0x04,0x96,0x00,0x00,0x00,0x00,
-0x0c,0x00,0x02,0x36,0x00,0x00,0x00,0x00,0xa3,0x80,0x80,0x11,0x08,0x00,0x02,0xe5,
-0x00,0x00,0x00,0x00,0x97,0x82,0x83,0x76,0x00,0x00,0x00,0x00,0x24,0x42,0x00,0x01,
-0xa7,0x82,0x83,0x76,0x84,0xa3,0x00,0x06,0x8f,0x82,0x83,0x88,0x00,0x00,0x00,0x00,
-0x00,0x43,0x10,0x21,0xaf,0x82,0x83,0x88,0x08,0x00,0x02,0xdd,0x25,0x42,0x00,0x01,
-0x97,0x82,0x83,0x72,0x00,0x00,0x00,0x00,0x24,0x42,0x00,0x01,0xa7,0x82,0x83,0x72,
-0x84,0xa3,0x00,0x06,0x8f,0x82,0x83,0x80,0x00,0x00,0x00,0x00,0x00,0x43,0x10,0x21,
-0xaf,0x82,0x83,0x80,0x08,0x00,0x02,0xdd,0x25,0x42,0x00,0x01,0x97,0x82,0x83,0x6c,
-0x00,0x00,0x00,0x00,0x24,0x42,0x00,0x01,0xa7,0x82,0x83,0x6c,0x08,0x00,0x02,0xc5,
-0x00,0x00,0x00,0x00,0x27,0xbd,0xff,0xd0,0xaf,0xbf,0x00,0x28,0x8c,0xa3,0x00,0x20,
-0x8f,0x8a,0x8b,0xc0,0x3c,0x02,0x00,0x10,0x00,0x62,0x10,0x24,0x00,0xa0,0x38,0x21,
-0x01,0x40,0x48,0x21,0x10,0x40,0x00,0x3d,0x00,0x80,0x28,0x21,0x8c,0xe4,0x00,0x1c,
-0x34,0xa5,0x12,0x06,0xaf,0xa5,0x00,0x10,0x8c,0x82,0x00,0x08,0x00,0x03,0x1c,0x42,
-0x30,0x63,0x00,0x30,0x00,0x02,0x13,0x02,0x30,0x42,0x00,0x40,0x00,0x43,0x10,0x25,
-0x90,0xe6,0x00,0x10,0x90,0xe4,0x00,0x13,0x94,0xe8,0x00,0x0c,0x94,0xe3,0x00,0x1a,
-0x00,0x02,0x16,0x00,0x90,0xe7,0x00,0x12,0x00,0xa2,0x28,0x25,0x24,0x02,0x12,0x34,
-0xa7,0xa2,0x00,0x1c,0x24,0x02,0x56,0x78,0xaf,0xa5,0x00,0x10,0xa3,0xa6,0x00,0x18,
-0xa3,0xa7,0x00,0x1f,0xa7,0xa3,0x00,0x1a,0xa3,0xa4,0x00,0x19,0xa7,0xa8,0x00,0x20,
-0xa7,0xa2,0x00,0x22,0x00,0x00,0x28,0x21,0x27,0xa7,0x00,0x10,0x24,0x06,0x8f,0xff,
-0x00,0x05,0x10,0x80,0x00,0x47,0x10,0x21,0x8c,0x44,0x00,0x00,0x24,0xa3,0x00,0x01,
-0x30,0x65,0x00,0xff,0x25,0x22,0x00,0x04,0x2c,0xa3,0x00,0x05,0xad,0x24,0x00,0x00,
-0x14,0x60,0xff,0xf7,0x00,0x46,0x48,0x24,0x97,0x83,0x8b,0xc6,0x97,0x85,0x8b,0xc4,
-0x3c,0x02,0xb0,0x02,0x24,0x63,0x00,0x14,0x00,0xa2,0x28,0x21,0x3c,0x04,0xb0,0x06,
-0xa7,0x83,0x8b,0xc6,0x34,0x84,0x80,0x18,0xa4,0xa3,0x00,0x00,0x8c,0x85,0x00,0x00,
-0x24,0x02,0x8f,0xff,0x25,0x46,0x00,0x14,0x3c,0x03,0x0f,0x00,0x00,0xc2,0x50,0x24,
-0x00,0xa3,0x28,0x24,0x3c,0x02,0x04,0x00,0xaf,0x8a,0x8b,0xc0,0x10,0xa2,0x00,0x03,
-0x00,0x00,0x00,0x00,0x0c,0x00,0x04,0x96,0x00,0x00,0x00,0x00,0x8f,0xbf,0x00,0x28,
-0x00,0x00,0x00,0x00,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x30,0x3c,0x05,0xb0,0x03,
-0x3c,0x02,0x80,0x00,0x27,0xbd,0xff,0xc8,0x00,0x04,0x22,0x00,0x34,0xa5,0x00,0x20,
-0x24,0x42,0x0d,0xfc,0x3c,0x03,0xb0,0x00,0xaf,0xb5,0x00,0x24,0xaf,0xb4,0x00,0x20,
-0xaf,0xb2,0x00,0x18,0xaf,0xb0,0x00,0x10,0xaf,0xbf,0x00,0x30,0x00,0x83,0x80,0x21,
-0xaf,0xb7,0x00,0x2c,0xaf,0xb6,0x00,0x28,0xaf,0xb3,0x00,0x1c,0xaf,0xb1,0x00,0x14,
-0xac,0xa2,0x00,0x00,0x8e,0x09,0x00,0x00,0x00,0x00,0x90,0x21,0x26,0x10,0x00,0x08,
-0x00,0x09,0xa6,0x02,0x12,0x80,0x00,0x13,0x00,0x00,0xa8,0x21,0x24,0x13,0x00,0x02,
-0x3c,0x16,0x00,0xff,0x3c,0x17,0xff,0x00,0x8e,0x09,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x09,0x12,0x02,0x24,0x42,0x00,0x02,0x31,0x25,0x00,0xff,0x10,0xb3,0x00,0x76,
-0x30,0x51,0x00,0xff,0x24,0x02,0x00,0x03,0x10,0xa2,0x00,0x18,0x00,0x00,0x00,0x00,
-0x02,0x51,0x10,0x21,0x30,0x52,0xff,0xff,0x02,0x54,0x18,0x2b,0x14,0x60,0xff,0xf2,
-0x02,0x11,0x80,0x21,0x12,0xa0,0x00,0x0a,0x3c,0x02,0xb0,0x06,0x34,0x42,0x80,0x18,
-0x8c,0x43,0x00,0x00,0x3c,0x04,0x0f,0x00,0x3c,0x02,0x04,0x00,0x00,0x64,0x18,0x24,
-0x10,0x62,0x00,0x03,0x00,0x00,0x00,0x00,0x0c,0x00,0x04,0x96,0x00,0x00,0x00,0x00,
-0x8f,0xbf,0x00,0x30,0x7b,0xb6,0x01,0x7c,0x7b,0xb4,0x01,0x3c,0x7b,0xb2,0x00,0xfc,
-0x7b,0xb0,0x00,0xbc,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x38,0x8e,0x09,0x00,0x04,
-0x24,0x15,0x00,0x01,0x8e,0x06,0x00,0x0c,0x00,0x09,0x11,0x42,0x00,0x09,0x18,0xc2,
-0x30,0x48,0x00,0x03,0x00,0x09,0x14,0x02,0x30,0x6c,0x00,0x03,0x00,0x09,0x26,0x02,
-0x11,0x15,0x00,0x45,0x30,0x43,0x00,0x0f,0x29,0x02,0x00,0x02,0x14,0x40,0x00,0x26,
-0x00,0x00,0x00,0x00,0x11,0x13,0x00,0x0f,0x00,0x00,0x38,0x21,0x00,0x07,0x22,0x02,
-0x30,0x84,0xff,0x00,0x3c,0x03,0x00,0xff,0x00,0x07,0x2e,0x02,0x00,0x07,0x12,0x00,
-0x00,0x43,0x10,0x24,0x00,0xa4,0x28,0x25,0x00,0xa2,0x28,0x25,0x00,0x07,0x1e,0x00,
-0x00,0xa3,0x28,0x25,0x0c,0x00,0x01,0x92,0x01,0x20,0x20,0x21,0x08,0x00,0x03,0xa5,
-0x02,0x51,0x10,0x21,0x11,0x95,0x00,0x0f,0x00,0x00,0x00,0x00,0x11,0x88,0x00,0x07,
-0x00,0x00,0x00,0x00,0x00,0x04,0x10,0x80,0x27,0x83,0x8b,0x70,0x00,0x43,0x10,0x21,
-0x8c,0x47,0x00,0x18,0x08,0x00,0x03,0xcc,0x00,0x07,0x22,0x02,0x00,0x04,0x10,0x40,
-0x27,0x83,0x8b,0x78,0x00,0x43,0x10,0x21,0x94,0x47,0x00,0x02,0x08,0x00,0x03,0xcc,
-0x00,0x07,0x22,0x02,0x27,0x82,0x8b,0x70,0x00,0x82,0x10,0x21,0x90,0x47,0x00,0x00,
-0x08,0x00,0x03,0xcc,0x00,0x07,0x22,0x02,0x15,0x00,0xff,0xdc,0x00,0x00,0x38,0x21,
-0x10,0x75,0x00,0x05,0x00,0x80,0x38,0x21,0x00,0x65,0x18,0x26,0x24,0x82,0x01,0x00,
-0x00,0x00,0x38,0x21,0x00,0x43,0x38,0x0a,0x24,0x02,0x00,0x01,0x11,0x82,0x00,0x0e,
-0x3c,0x02,0xb0,0x03,0x24,0x02,0x00,0x02,0x11,0x82,0x00,0x06,0x00,0x00,0x00,0x00,
-0x3c,0x02,0xb0,0x03,0x00,0xe2,0x10,0x21,0x8c,0x47,0x00,0x00,0x08,0x00,0x03,0xcc,
-0x00,0x07,0x22,0x02,0x3c,0x02,0xb0,0x03,0x00,0xe2,0x10,0x21,0x94,0x43,0x00,0x00,
-0x08,0x00,0x03,0xcb,0x30,0x67,0xff,0xff,0x00,0xe2,0x10,0x21,0x90,0x43,0x00,0x00,
-0x08,0x00,0x03,0xcb,0x30,0x67,0x00,0xff,0x30,0x62,0x00,0x03,0x00,0x02,0x12,0x00,
-0x11,0x95,0x00,0x07,0x00,0x44,0x38,0x21,0x11,0x93,0x00,0x03,0x00,0x00,0x00,0x00,
-0x08,0x00,0x03,0xfd,0x3c,0x02,0xb0,0x0a,0x08,0x00,0x04,0x02,0x3c,0x02,0xb0,0x0a,
-0x08,0x00,0x04,0x06,0x3c,0x02,0xb0,0x0a,0x8e,0x09,0x00,0x04,0x8e,0x02,0x00,0x08,
-0x8e,0x03,0x00,0x0c,0x00,0x09,0x41,0x42,0x00,0x02,0x22,0x02,0x00,0x03,0x3a,0x02,
-0x30,0x84,0xff,0x00,0x30,0xe7,0xff,0x00,0x00,0x02,0x5e,0x02,0x00,0x02,0x32,0x00,
-0x00,0x03,0x56,0x02,0x00,0x03,0x2a,0x00,0x01,0x64,0x58,0x25,0x00,0xd6,0x30,0x24,
-0x01,0x47,0x50,0x25,0x00,0x02,0x16,0x00,0x00,0xb6,0x28,0x24,0x00,0x03,0x1e,0x00,
-0x01,0x66,0x58,0x25,0x01,0x45,0x50,0x25,0x00,0x57,0x10,0x24,0x00,0x77,0x18,0x24,
-0x01,0x62,0x38,0x25,0x01,0x43,0x30,0x25,0x00,0x09,0x10,0xc2,0x00,0x09,0x1c,0x02,
-0x31,0x08,0x00,0x03,0x30,0x4c,0x00,0x03,0x30,0x63,0x00,0x0f,0x00,0x09,0x26,0x02,
-0x00,0xe0,0x58,0x21,0x15,0x00,0x00,0x28,0x00,0xc0,0x50,0x21,0x24,0x02,0x00,0x01,
-0x10,0x62,0x00,0x06,0x00,0x80,0x28,0x21,0x24,0x02,0x00,0x03,0x14,0x62,0xff,0x69,
-0x02,0x51,0x10,0x21,0x24,0x85,0x01,0x00,0x24,0x02,0x00,0x01,0x11,0x82,0x00,0x15,
-0x24,0x02,0x00,0x02,0x11,0x82,0x00,0x0a,0x3c,0x03,0xb0,0x03,0x00,0xa3,0x18,0x21,
-0x8c,0x62,0x00,0x00,0x00,0x0a,0x20,0x27,0x01,0x6a,0x28,0x24,0x00,0x44,0x10,0x24,
-0x00,0x45,0x10,0x25,0xac,0x62,0x00,0x00,0x08,0x00,0x03,0xa5,0x02,0x51,0x10,0x21,
-0x00,0xa3,0x18,0x21,0x94,0x62,0x00,0x00,0x00,0x0a,0x20,0x27,0x01,0x6a,0x28,0x24,
-0x00,0x44,0x10,0x24,0x00,0x45,0x10,0x25,0xa4,0x62,0x00,0x00,0x08,0x00,0x03,0xa5,
-0x02,0x51,0x10,0x21,0x3c,0x03,0xb0,0x03,0x00,0xa3,0x18,0x21,0x90,0x62,0x00,0x00,
-0x00,0x0a,0x20,0x27,0x01,0x6a,0x28,0x24,0x00,0x44,0x10,0x24,0x00,0x45,0x10,0x25,
-0x08,0x00,0x03,0xa4,0xa0,0x62,0x00,0x00,0x24,0x02,0x00,0x01,0x11,0x02,0x00,0x21,
-0x00,0x00,0x00,0x00,0x15,0x13,0xff,0x42,0x00,0x00,0x00,0x00,0x11,0x82,0x00,0x17,
-0x00,0x00,0x00,0x00,0x11,0x88,0x00,0x0b,0x00,0x00,0x00,0x00,0x27,0x83,0x8b,0x70,
-0x00,0x04,0x20,0x80,0x00,0x83,0x20,0x21,0x8c,0x82,0x00,0x18,0x00,0x06,0x18,0x27,
-0x00,0xe6,0x28,0x24,0x00,0x43,0x10,0x24,0x00,0x45,0x10,0x25,0x08,0x00,0x03,0xa4,
-0xac,0x82,0x00,0x18,0x27,0x83,0x8b,0x78,0x00,0x04,0x20,0x40,0x00,0x83,0x20,0x21,
-0x94,0x82,0x00,0x02,0x00,0x06,0x18,0x27,0x00,0xe6,0x28,0x24,0x00,0x43,0x10,0x24,
-0x00,0x45,0x10,0x25,0x08,0x00,0x03,0xa4,0xa4,0x82,0x00,0x02,0x27,0x83,0x8b,0x70,
-0x00,0x83,0x18,0x21,0x90,0x62,0x00,0x00,0x00,0x06,0x20,0x27,0x08,0x00,0x04,0x5a,
-0x00,0xe6,0x28,0x24,0x30,0x62,0x00,0x07,0x00,0x02,0x12,0x00,0x11,0x88,0x00,0x0f,
-0x00,0x44,0x10,0x21,0x11,0x93,0x00,0x07,0x00,0x00,0x00,0x00,0x3c,0x03,0xb0,0x0a,
-0x00,0x43,0x18,0x21,0x8c,0x62,0x00,0x00,0x00,0x06,0x20,0x27,0x08,0x00,0x04,0x47,
-0x00,0xe6,0x28,0x24,0x3c,0x03,0xb0,0x0a,0x00,0x43,0x18,0x21,0x94,0x62,0x00,0x00,
-0x00,0x06,0x20,0x27,0x08,0x00,0x04,0x50,0x00,0xe6,0x28,0x24,0x3c,0x03,0xb0,0x0a,
-0x08,0x00,0x04,0x7d,0x00,0x43,0x18,0x21,0x97,0x85,0x8b,0xc4,0x3c,0x07,0xb0,0x02,
-0x3c,0x04,0xb0,0x03,0x3c,0x02,0x80,0x00,0x00,0xa7,0x28,0x21,0x34,0x84,0x00,0x20,
-0x24,0x42,0x12,0x58,0x24,0x03,0xff,0x80,0xac,0x82,0x00,0x00,0xa0,0xa3,0x00,0x07,
-0x97,0x82,0x8b,0xc6,0x97,0x85,0x8b,0xc4,0x3c,0x06,0xb0,0x06,0x30,0x42,0xff,0xf8,
-0x24,0x42,0x00,0x10,0x00,0xa2,0x10,0x21,0x30,0x42,0x0f,0xff,0x24,0x44,0x00,0x08,
-0x30,0x84,0x0f,0xff,0x00,0x05,0x28,0xc2,0x3c,0x03,0x00,0x40,0x00,0xa3,0x28,0x25,
-0x00,0x87,0x20,0x21,0x34,0xc6,0x80,0x18,0xac,0xc5,0x00,0x00,0xaf,0x84,0x8b,0xc0,
-0xa7,0x82,0x8b,0xc4,0xa7,0x80,0x8b,0xc6,0xaf,0x80,0x8b,0xc8,0x03,0xe0,0x00,0x08,
-0x00,0x00,0x00,0x00,0x30,0xa5,0x00,0xff,0x30,0x84,0x00,0xff,0x24,0x02,0x00,0x01,
-0x00,0xe0,0x48,0x21,0x30,0xc6,0x00,0xff,0x8f,0xa7,0x00,0x10,0x10,0x82,0x00,0x07,
-0x00,0xa0,0x40,0x21,0x24,0x02,0x00,0x03,0x10,0x82,0x00,0x03,0x00,0x00,0x00,0x00,
-0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x24,0xa8,0x01,0x00,0x3c,0x03,0xb0,0x03,
-0x24,0x02,0x00,0x01,0x00,0x07,0x20,0x27,0x01,0x27,0x28,0x24,0x10,0xc2,0x00,0x14,
-0x01,0x03,0x18,0x21,0x24,0x02,0x00,0x02,0x10,0xc2,0x00,0x09,0x00,0x07,0x50,0x27,
-0x3c,0x03,0xb0,0x03,0x01,0x03,0x18,0x21,0x8c,0x62,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x4a,0x10,0x24,0x00,0x45,0x10,0x25,0x08,0x00,0x04,0xe1,0xac,0x62,0x00,0x00,
-0x3c,0x03,0xb0,0x03,0x01,0x03,0x18,0x21,0x94,0x62,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x4a,0x10,0x24,0x00,0x45,0x10,0x25,0x03,0xe0,0x00,0x08,0xa4,0x62,0x00,0x00,
-0x90,0x62,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x44,0x10,0x24,0x00,0x45,0x10,0x25,
-0xa0,0x62,0x00,0x00,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x30,0x84,0x00,0x07,
-0x00,0x04,0x22,0x00,0x30,0xa5,0x00,0xff,0x00,0x85,0x28,0x21,0x3c,0x02,0xb0,0x0a,
-0x00,0xa2,0x40,0x21,0x30,0xc6,0x00,0xff,0x24,0x02,0x00,0x01,0x8f,0xa4,0x00,0x10,
-0x10,0xc2,0x00,0x14,0x24,0x02,0x00,0x02,0x00,0x04,0x50,0x27,0x10,0xc2,0x00,0x09,
-0x00,0xe4,0x48,0x24,0x3c,0x03,0xb0,0x0a,0x00,0xa3,0x18,0x21,0x8c,0x62,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x4a,0x10,0x24,0x00,0x49,0x10,0x25,0x03,0xe0,0x00,0x08,
-0xac,0x62,0x00,0x00,0x3c,0x03,0xb0,0x0a,0x00,0xa3,0x18,0x21,0x94,0x62,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x4a,0x10,0x24,0x00,0x49,0x10,0x25,0x03,0xe0,0x00,0x08,
-0xa4,0x62,0x00,0x00,0x91,0x02,0x00,0x00,0x00,0x04,0x18,0x27,0x00,0xe4,0x20,0x24,
-0x00,0x43,0x10,0x24,0x00,0x44,0x10,0x25,0x03,0xe0,0x00,0x08,0xa1,0x02,0x00,0x00,
-0x30,0xa9,0x00,0xff,0x27,0x83,0x8b,0x70,0x30,0x85,0x00,0xff,0x24,0x02,0x00,0x01,
-0x00,0x07,0x50,0x27,0x00,0xc7,0x40,0x24,0x11,0x22,0x00,0x17,0x00,0xa3,0x18,0x21,
-0x00,0x05,0x20,0x40,0x27,0x82,0x8b,0x70,0x00,0x05,0x28,0x80,0x27,0x83,0x8b,0x78,
-0x00,0x83,0x50,0x21,0x00,0xa2,0x20,0x21,0x24,0x02,0x00,0x02,0x00,0x07,0x40,0x27,
-0x11,0x22,0x00,0x07,0x00,0xc7,0x28,0x24,0x8c,0x82,0x00,0x18,0x00,0x00,0x00,0x00,
-0x00,0x48,0x10,0x24,0x00,0x45,0x10,0x25,0x03,0xe0,0x00,0x08,0xac,0x82,0x00,0x18,
-0x95,0x42,0x00,0x02,0x00,0x00,0x00,0x00,0x00,0x48,0x10,0x24,0x00,0x45,0x10,0x25,
-0x03,0xe0,0x00,0x08,0xa5,0x42,0x00,0x02,0x90,0x62,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x4a,0x10,0x24,0x00,0x48,0x10,0x25,0x03,0xe0,0x00,0x08,0xa0,0x62,0x00,0x00,
-0x00,0x04,0x32,0x02,0x30,0xc6,0xff,0x00,0x00,0x04,0x16,0x02,0x00,0x04,0x1a,0x00,
-0x3c,0x05,0x00,0xff,0x00,0x65,0x18,0x24,0x00,0x46,0x10,0x25,0x00,0x43,0x10,0x25,
-0x00,0x04,0x26,0x00,0x03,0xe0,0x00,0x08,0x00,0x44,0x10,0x25,0x3c,0x03,0xb0,0x03,
-0x3c,0x02,0x80,0x00,0x27,0xbd,0xff,0xe8,0x34,0x63,0x00,0x20,0x24,0x42,0x14,0xdc,
-0x3c,0x04,0xb0,0x03,0xaf,0xbf,0x00,0x14,0xac,0x62,0x00,0x00,0xaf,0xb0,0x00,0x10,
-0x34,0x84,0x00,0x2c,0x8c,0x83,0x00,0x00,0xa7,0x80,0xbc,0x00,0x00,0x03,0x12,0x02,
-0x00,0x03,0x2d,0x02,0x30,0x42,0x0f,0xff,0xa3,0x83,0xbc,0x08,0xa7,0x85,0xbc,0x0c,
-0xa7,0x82,0xbc,0x0a,0xa7,0x80,0xbc,0x02,0xa7,0x80,0xbc,0x04,0xa7,0x80,0xbc,0x06,
-0x0c,0x00,0x06,0xd1,0x24,0x04,0x05,0x00,0x3c,0x05,0x08,0x00,0x00,0x45,0x28,0x25,
-0x24,0x04,0x05,0x00,0x0c,0x00,0x06,0xbf,0x00,0x40,0x80,0x21,0x3c,0x02,0xf7,0xff,
-0x34,0x42,0xff,0xff,0x02,0x02,0x80,0x24,0x02,0x00,0x28,0x21,0x0c,0x00,0x06,0xbf,
-0x24,0x04,0x05,0x00,0x3c,0x02,0xb0,0x03,0x3c,0x03,0xb0,0x03,0x34,0x42,0x01,0x08,
-0x34,0x63,0x01,0x18,0x8c,0x45,0x00,0x00,0x8c,0x64,0x00,0x00,0x3c,0x02,0x00,0x0f,
-0x3c,0x03,0x00,0x4c,0x30,0x84,0x02,0x00,0x34,0x63,0x4b,0x40,0xaf,0x85,0xbc,0x10,
-0x10,0x80,0x00,0x06,0x34,0x42,0x42,0x40,0xaf,0x83,0xbc,0x14,0x8f,0xbf,0x00,0x14,
-0x8f,0xb0,0x00,0x10,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x18,0xaf,0x82,0xbc,0x14,
-0x08,0x00,0x05,0x67,0x00,0x00,0x00,0x00,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,
-0x27,0xbd,0xff,0xc8,0x34,0x63,0x00,0x20,0x24,0x42,0x15,0xb8,0x30,0x84,0x00,0xff,
-0xaf,0xbf,0x00,0x30,0xaf,0xb7,0x00,0x2c,0xaf,0xb6,0x00,0x28,0xaf,0xb5,0x00,0x24,
-0xaf,0xb4,0x00,0x20,0xaf,0xb3,0x00,0x1c,0xaf,0xb2,0x00,0x18,0xaf,0xb1,0x00,0x14,
-0xaf,0xb0,0x00,0x10,0xac,0x62,0x00,0x00,0x10,0x80,0x00,0x1c,0x24,0x02,0x00,0x02,
-0x10,0x82,0x00,0x08,0x00,0x00,0x00,0x00,0x8f,0xbf,0x00,0x30,0x7b,0xb6,0x01,0x7c,
-0x7b,0xb4,0x01,0x3c,0x7b,0xb2,0x00,0xfc,0x7b,0xb0,0x00,0xbc,0x03,0xe0,0x00,0x08,
-0x27,0xbd,0x00,0x38,0xa7,0x80,0xbc,0x00,0xa7,0x80,0xbc,0x02,0xa7,0x80,0xbc,0x04,
-0xa7,0x80,0xbc,0x06,0x0c,0x00,0x06,0xd1,0x24,0x04,0x05,0x00,0x3c,0x05,0x08,0x00,
-0x00,0x45,0x28,0x25,0x24,0x04,0x05,0x00,0x0c,0x00,0x06,0xbf,0x00,0x40,0x80,0x21,
-0x3c,0x05,0xf7,0xff,0x34,0xa5,0xff,0xff,0x02,0x05,0x28,0x24,0x0c,0x00,0x06,0xbf,
-0x24,0x04,0x05,0x00,0x08,0x00,0x05,0x82,0x00,0x00,0x00,0x00,0x0c,0x00,0x06,0xd1,
-0x24,0x04,0x05,0xa0,0x24,0x04,0x05,0xa4,0x0c,0x00,0x06,0xd1,0x00,0x02,0xbc,0x02,
-0x24,0x04,0x05,0xa8,0x00,0x02,0xb4,0x02,0x0c,0x00,0x06,0xd1,0x30,0x55,0xff,0xff,
-0x00,0x40,0x80,0x21,0x97,0x84,0xbc,0x00,0x97,0x82,0xbc,0x02,0x97,0x83,0xbc,0x06,
-0x02,0xe4,0x20,0x23,0x02,0xa2,0x10,0x23,0x00,0x82,0x20,0x21,0x97,0x82,0xbc,0x04,
-0x32,0x14,0xff,0xff,0x02,0x83,0x18,0x23,0x02,0xc2,0x10,0x23,0x00,0x82,0x20,0x21,
-0x93,0x82,0xbc,0x08,0x00,0x83,0x20,0x21,0x30,0x84,0xff,0xff,0x00,0x82,0x10,0x2b,
-0x14,0x40,0x00,0xaa,0x00,0x00,0x00,0x00,0x97,0x82,0xbc,0x0c,0x00,0x00,0x00,0x00,
-0x00,0x44,0x10,0x2b,0x14,0x40,0x00,0x7f,0x00,0x00,0x00,0x00,0x97,0x82,0xbc,0x0a,
-0x00,0x00,0x00,0x00,0x00,0x44,0x10,0x2b,0x10,0x40,0x00,0x3a,0x00,0x00,0x00,0x00,
-0x0c,0x00,0x06,0xd1,0x24,0x04,0x04,0x50,0x30,0x51,0x00,0x7f,0x00,0x40,0x80,0x21,
-0x2e,0x22,0x00,0x32,0x10,0x40,0x00,0x13,0x24,0x02,0x00,0x20,0x12,0x22,0x00,0x17,
-0x24,0x02,0xff,0x80,0x02,0x02,0x10,0x24,0x26,0x31,0x00,0x01,0x00,0x51,0x80,0x25,
-0x02,0x00,0x28,0x21,0x0c,0x00,0x06,0xbf,0x24,0x04,0x04,0x50,0x02,0x00,0x28,0x21,
-0x0c,0x00,0x06,0xbf,0x24,0x04,0x04,0x58,0x02,0x00,0x28,0x21,0x0c,0x00,0x06,0xbf,
-0x24,0x04,0x04,0x60,0x02,0x00,0x28,0x21,0x24,0x04,0x04,0x68,0x0c,0x00,0x06,0xbf,
-0x00,0x00,0x00,0x00,0xa7,0x97,0xbc,0x00,0xa7,0x95,0xbc,0x02,0xa7,0x96,0xbc,0x04,
-0xa7,0x94,0xbc,0x06,0x08,0x00,0x05,0x82,0x00,0x00,0x00,0x00,0x0c,0x00,0x06,0xd1,
-0x24,0x04,0x02,0x08,0x3c,0x04,0x00,0xc0,0x00,0x40,0x28,0x21,0x00,0x44,0x10,0x24,
-0x00,0x02,0x15,0x82,0x24,0x03,0x00,0x03,0x10,0x43,0x00,0x07,0x00,0x00,0x00,0x00,
-0x3c,0x02,0xff,0x3f,0x34,0x42,0xff,0xff,0x00,0xa2,0x10,0x24,0x00,0x44,0x28,0x25,
-0x0c,0x00,0x06,0xbf,0x24,0x04,0x02,0x08,0x0c,0x00,0x06,0xd1,0x24,0x04,0x02,0x2c,
-0x00,0x40,0x90,0x21,0x3c,0x02,0xff,0xff,0x34,0x42,0x3f,0xff,0x02,0x42,0x90,0x24,
-0x02,0x40,0x28,0x21,0x0c,0x00,0x06,0xbf,0x24,0x04,0x02,0x2c,0x08,0x00,0x05,0xc9,
-0x24,0x02,0xff,0x80,0x0c,0x00,0x06,0xd1,0x24,0x04,0x04,0x50,0x30,0x51,0x00,0x7f,
-0x24,0x02,0x00,0x20,0x16,0x22,0xff,0xdb,0x00,0x00,0x00,0x00,0x0c,0x00,0x06,0xd1,
-0x24,0x04,0x02,0x2c,0x34,0x52,0x40,0x00,0x02,0x40,0x28,0x21,0x0c,0x00,0x06,0xbf,
-0x24,0x04,0x02,0x2c,0x0c,0x00,0x06,0xd1,0x24,0x04,0x02,0x58,0x24,0x04,0x02,0x5c,
-0x0c,0x00,0x06,0xd1,0x00,0x02,0x9e,0x02,0x30,0x43,0x00,0xff,0x00,0x13,0x12,0x00,
-0x00,0x43,0x10,0x25,0x2c,0x43,0x00,0x04,0x14,0x60,0x00,0x1d,0x2c,0x42,0x00,0x11,
-0x10,0x40,0x00,0x0b,0x00,0x00,0x00,0x00,0x3c,0x02,0xff,0xff,0x34,0x42,0x3f,0xff,
-0x02,0x42,0x90,0x24,0x02,0x40,0x28,0x21,0x24,0x04,0x02,0x2c,0x0c,0x00,0x06,0xbf,
-0x36,0x52,0x80,0x00,0x02,0x40,0x28,0x21,0x08,0x00,0x05,0xd7,0x24,0x04,0x02,0x2c,
-0x0c,0x00,0x06,0xd1,0x24,0x04,0x02,0x08,0x3c,0x04,0x00,0xc0,0x00,0x40,0x28,0x21,
-0x00,0x44,0x10,0x24,0x00,0x02,0x15,0x82,0x24,0x03,0x00,0x02,0x14,0x43,0xff,0xee,
-0x3c,0x02,0xff,0x3f,0x34,0x42,0xff,0xff,0x00,0xa2,0x10,0x24,0x00,0x44,0x28,0x25,
-0x0c,0x00,0x06,0xbf,0x24,0x04,0x02,0x08,0x08,0x00,0x06,0x13,0x3c,0x02,0xff,0xff,
-0x0c,0x00,0x06,0xd1,0x24,0x04,0x02,0x08,0x00,0x40,0x28,0x21,0x00,0x02,0x15,0x82,
-0x30,0x42,0x00,0x03,0x24,0x03,0x00,0x03,0x14,0x43,0xff,0xdf,0x3c,0x02,0xff,0x3f,
-0x34,0x42,0xff,0xff,0x00,0xa2,0x10,0x24,0x3c,0x03,0x00,0x80,0x08,0x00,0x06,0x28,
-0x00,0x43,0x28,0x25,0x0c,0x00,0x06,0xd1,0x24,0x04,0x04,0x50,0x30,0x51,0x00,0x7f,
-0x00,0x40,0x80,0x21,0x2e,0x22,0x00,0x32,0x10,0x40,0xff,0x9a,0x24,0x02,0x00,0x20,
-0x12,0x22,0x00,0x04,0x24,0x02,0xff,0x80,0x02,0x02,0x10,0x24,0x08,0x00,0x05,0xcb,
-0x26,0x31,0x00,0x02,0x0c,0x00,0x06,0xd1,0x24,0x04,0x02,0x08,0x3c,0x04,0x00,0xc0,
-0x00,0x40,0x28,0x21,0x00,0x44,0x10,0x24,0x00,0x02,0x15,0x82,0x24,0x03,0x00,0x03,
-0x10,0x43,0x00,0x07,0x00,0x00,0x00,0x00,0x3c,0x02,0xff,0x3f,0x34,0x42,0xff,0xff,
-0x00,0xa2,0x10,0x24,0x00,0x44,0x28,0x25,0x0c,0x00,0x06,0xbf,0x24,0x04,0x02,0x08,
-0x0c,0x00,0x06,0xd1,0x24,0x04,0x02,0x2c,0x00,0x40,0x90,0x21,0x3c,0x02,0xff,0xff,
-0x34,0x42,0x3f,0xff,0x02,0x42,0x90,0x24,0x02,0x40,0x28,0x21,0x0c,0x00,0x06,0xbf,
-0x24,0x04,0x02,0x2c,0x08,0x00,0x06,0x42,0x24,0x02,0xff,0x80,0x0c,0x00,0x06,0xd1,
-0x24,0x04,0x04,0x50,0x00,0x40,0x80,0x21,0x30,0x51,0x00,0x7f,0x24,0x02,0x00,0x20,
-0x12,0x22,0x00,0x1d,0x2e,0x22,0x00,0x21,0x14,0x40,0xff,0x72,0x24,0x02,0xff,0x80,
-0x02,0x02,0x10,0x24,0x26,0x31,0xff,0xff,0x00,0x51,0x80,0x25,0x24,0x04,0x04,0x50,
-0x0c,0x00,0x06,0xbf,0x02,0x00,0x28,0x21,0x24,0x04,0x04,0x58,0x0c,0x00,0x06,0xbf,
-0x02,0x00,0x28,0x21,0x24,0x04,0x04,0x60,0x0c,0x00,0x06,0xbf,0x02,0x00,0x28,0x21,
-0x02,0x00,0x28,0x21,0x0c,0x00,0x06,0xbf,0x24,0x04,0x04,0x68,0x24,0x02,0x00,0x20,
-0x16,0x22,0xff,0x60,0x00,0x00,0x00,0x00,0x0c,0x00,0x06,0xd1,0x24,0x04,0x02,0x2c,
-0x00,0x40,0x90,0x21,0x3c,0x02,0xff,0xff,0x34,0x42,0x3f,0xff,0x02,0x42,0x10,0x24,
-0x08,0x00,0x06,0x19,0x34,0x52,0x80,0x00,0x0c,0x00,0x06,0xd1,0x24,0x04,0x02,0x2c,
-0x34,0x52,0x40,0x00,0x02,0x40,0x28,0x21,0x0c,0x00,0x06,0xbf,0x24,0x04,0x02,0x2c,
-0x0c,0x00,0x06,0xd1,0x24,0x04,0x02,0x58,0x24,0x04,0x02,0x5c,0x0c,0x00,0x06,0xd1,
-0x00,0x02,0x9e,0x02,0x30,0x43,0x00,0xff,0x00,0x13,0x12,0x00,0x00,0x43,0x10,0x25,
-0x2c,0x43,0x00,0x04,0x14,0x60,0x00,0x20,0x2c,0x42,0x00,0x11,0x10,0x40,0x00,0x0d,
-0x00,0x00,0x00,0x00,0x3c,0x02,0xff,0xff,0x34,0x42,0x3f,0xff,0x02,0x42,0x90,0x24,
-0x02,0x40,0x28,0x21,0x24,0x04,0x02,0x2c,0x0c,0x00,0x06,0xbf,0x36,0x52,0x80,0x00,
-0x02,0x40,0x28,0x21,0x0c,0x00,0x06,0xbf,0x24,0x04,0x02,0x2c,0x08,0x00,0x06,0x66,
-0x2e,0x22,0x00,0x21,0x0c,0x00,0x06,0xd1,0x24,0x04,0x02,0x08,0x3c,0x04,0x00,0xc0,
-0x00,0x40,0x28,0x21,0x00,0x44,0x10,0x24,0x00,0x02,0x15,0x82,0x24,0x03,0x00,0x02,
-0x14,0x43,0xff,0xec,0x00,0x00,0x00,0x00,0x3c,0x02,0xff,0x3f,0x34,0x42,0xff,0xff,
-0x00,0xa2,0x10,0x24,0x00,0x44,0x28,0x25,0x0c,0x00,0x06,0xbf,0x24,0x04,0x02,0x08,
-0x08,0x00,0x06,0x96,0x3c,0x02,0xff,0xff,0x0c,0x00,0x06,0xd1,0x24,0x04,0x02,0x08,
-0x00,0x40,0x28,0x21,0x00,0x02,0x15,0x82,0x30,0x42,0x00,0x03,0x24,0x03,0x00,0x03,
-0x14,0x43,0xff,0xdc,0x3c,0x03,0x00,0x80,0x3c,0x02,0xff,0x3f,0x34,0x42,0xff,0xff,
-0x00,0xa2,0x10,0x24,0x08,0x00,0x06,0xae,0x00,0x43,0x28,0x25,0x30,0x83,0x00,0x03,
-0x00,0x04,0x20,0x40,0x00,0x83,0x20,0x23,0x3c,0x02,0xb0,0x0a,0x00,0x82,0x20,0x21,
-0x3c,0x06,0x00,0x01,0xac,0x85,0x00,0x00,0x24,0x07,0x00,0x01,0x00,0x00,0x28,0x21,
-0x34,0xc6,0x86,0x9f,0x8c,0x82,0x10,0x00,0x24,0xa5,0x00,0x01,0x10,0x47,0x00,0x03,
-0x00,0xc5,0x18,0x2b,0x10,0x60,0xff,0xfb,0x00,0x00,0x00,0x00,0x03,0xe0,0x00,0x08,
-0x00,0x00,0x00,0x00,0x30,0x83,0x00,0x03,0x00,0x04,0x20,0x40,0x3c,0x02,0xb0,0x0a,
-0x00,0x83,0x20,0x23,0x00,0x82,0x20,0x21,0x3c,0x06,0x00,0x01,0x24,0x02,0xff,0xff,
-0xac,0x82,0x10,0x00,0x00,0x00,0x28,0x21,0x24,0x07,0x00,0x01,0x34,0xc6,0x86,0x9f,
-0x8c,0x82,0x10,0x00,0x24,0xa5,0x00,0x01,0x10,0x47,0x00,0x03,0x00,0xc5,0x18,0x2b,
-0x10,0x60,0xff,0xfb,0x00,0x00,0x00,0x00,0x8c,0x82,0x00,0x00,0x03,0xe0,0x00,0x08,
-0x00,0x00,0x00,0x00,0x3c,0x05,0xb0,0x03,0x3c,0x02,0x80,0x00,0x24,0x42,0x1b,0x94,
-0x24,0x03,0x00,0x01,0x34,0xa5,0x00,0x20,0x3c,0x06,0xb0,0x03,0xac,0xa2,0x00,0x00,
-0x34,0xc6,0x01,0x04,0xa0,0x83,0x00,0x48,0xa0,0x80,0x00,0x04,0xa0,0x80,0x00,0x05,
-0xa0,0x80,0x00,0x06,0xa0,0x80,0x00,0x07,0xa0,0x80,0x00,0x08,0xa0,0x80,0x00,0x09,
-0xa0,0x80,0x00,0x0a,0xa0,0x80,0x00,0x11,0xa0,0x80,0x00,0x13,0xa0,0x80,0x00,0x49,
-0x94,0xc2,0x00,0x00,0xac,0x80,0x00,0x00,0xa0,0x80,0x00,0x4e,0x00,0x02,0x14,0x00,
-0x00,0x02,0x14,0x03,0x30,0x43,0x00,0xff,0x30,0x42,0xff,0x00,0xa4,0x82,0x00,0x44,
-0xa4,0x83,0x00,0x46,0xac,0x80,0x00,0x24,0xac,0x80,0x00,0x28,0xac,0x80,0x00,0x2c,
-0xac,0x80,0x00,0x30,0xac,0x80,0x00,0x34,0xac,0x80,0x00,0x38,0xac,0x80,0x00,0x3c,
-0x03,0xe0,0x00,0x08,0xac,0x80,0x00,0x40,0x84,0x83,0x00,0x0c,0x3c,0x07,0xb0,0x03,
-0x34,0xe7,0x00,0x20,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x80,
-0x27,0x83,0x90,0x04,0x00,0x43,0x10,0x21,0x8c,0x48,0x00,0x18,0x3c,0x02,0x80,0x00,
-0x24,0x42,0x1c,0x28,0xac,0xe2,0x00,0x00,0x8d,0x03,0x00,0x08,0x80,0x82,0x00,0x13,
-0x00,0x05,0x2c,0x00,0x00,0x03,0x1e,0x02,0x00,0x02,0x12,0x00,0x30,0x63,0x00,0x7e,
-0x00,0x62,0x18,0x21,0x00,0x65,0x18,0x21,0x3c,0x02,0xc0,0x00,0x3c,0x05,0xb0,0x05,
-0x34,0x42,0x04,0x00,0x24,0x63,0x00,0x01,0x3c,0x07,0xb0,0x05,0x3c,0x08,0xb0,0x05,
-0x34,0xa5,0x04,0x20,0xac,0xa3,0x00,0x00,0x00,0xc2,0x30,0x21,0x34,0xe7,0x04,0x24,
-0x35,0x08,0x02,0x28,0x24,0x02,0x00,0x01,0x24,0x03,0x00,0x20,0xac,0xe6,0x00,0x00,
-0xac,0x82,0x00,0x3c,0x03,0xe0,0x00,0x08,0xa1,0x03,0x00,0x00,0x27,0xbd,0xff,0xa8,
-0x00,0x07,0x60,0x80,0x27,0x82,0xb4,0x00,0xaf,0xbe,0x00,0x50,0xaf,0xb7,0x00,0x4c,
-0xaf,0xb5,0x00,0x44,0xaf,0xb4,0x00,0x40,0xaf,0xbf,0x00,0x54,0xaf,0xb6,0x00,0x48,
-0xaf,0xb3,0x00,0x3c,0xaf,0xb2,0x00,0x38,0xaf,0xb1,0x00,0x34,0xaf,0xb0,0x00,0x30,
-0x01,0x82,0x10,0x21,0x8c,0x43,0x00,0x00,0x00,0xe0,0x70,0x21,0x3c,0x02,0x80,0x00,
-0x94,0x73,0x00,0x14,0x3c,0x07,0xb0,0x03,0x34,0xe7,0x00,0x20,0x24,0x42,0x1c,0xbc,
-0x3c,0x03,0xb0,0x05,0xac,0xe2,0x00,0x00,0x34,0x63,0x01,0x28,0x90,0x67,0x00,0x00,
-0x00,0x13,0xa8,0xc0,0x02,0xb3,0x18,0x21,0x27,0x82,0x90,0x04,0x00,0x03,0x18,0x80,
-0x00,0x62,0x18,0x21,0x00,0x05,0x2c,0x00,0x00,0x07,0x3e,0x00,0x28,0xc2,0x00,0x03,
-0x00,0xc0,0xa0,0x21,0x00,0x80,0x78,0x21,0x00,0x05,0xbc,0x03,0x8c,0x68,0x00,0x18,
-0x02,0xa0,0x58,0x21,0x10,0x40,0x01,0x81,0x00,0x07,0xf6,0x03,0x00,0xde,0x10,0x07,
-0x30,0x5e,0x00,0x01,0x01,0x73,0x10,0x21,0x27,0x83,0x90,0x08,0x00,0x02,0x10,0x80,
-0x00,0x43,0x10,0x21,0x80,0x4d,0x00,0x06,0x8d,0x03,0x00,0x00,0x8d,0x02,0x00,0x04,
-0x8d,0x0a,0x00,0x08,0x8d,0x03,0x00,0x0c,0xaf,0xa2,0x00,0x20,0x11,0xa0,0x01,0x71,
-0xaf,0xa3,0x00,0x18,0x27,0x82,0xb4,0x00,0x01,0x82,0x10,0x21,0x8c,0x44,0x00,0x00,
-0x00,0x00,0x00,0x00,0x90,0x83,0x00,0x16,0x00,0x00,0x00,0x00,0x30,0x63,0x00,0x04,
-0x14,0x60,0x00,0x12,0x00,0x00,0xb0,0x21,0x3c,0x02,0xb0,0x09,0x34,0x42,0x01,0x46,
-0x90,0x43,0x00,0x00,0x2a,0x84,0x00,0x04,0x10,0x80,0x01,0x56,0x30,0x65,0x00,0x01,
-0x91,0xe2,0x00,0x09,0x00,0x00,0x00,0x00,0x12,0x82,0x00,0x02,0x00,0x00,0x00,0x00,
-0x00,0x00,0x28,0x21,0x14,0xa0,0x00,0x03,0x00,0x00,0x38,0x21,0x13,0xc0,0x00,0x03,
-0x38,0xf6,0x00,0x01,0x24,0x07,0x00,0x01,0x38,0xf6,0x00,0x01,0x01,0x73,0x10,0x21,
-0x00,0x02,0x30,0x80,0x27,0x83,0x90,0x10,0x00,0xc3,0x48,0x21,0x91,0x25,0x00,0x00,
-0x8f,0xa4,0x00,0x20,0x2c,0xa3,0x00,0x04,0x00,0x04,0x11,0xc3,0x30,0x42,0x00,0x01,
-0x00,0x03,0xb0,0x0b,0x12,0xc0,0x00,0xd8,0xaf,0xa2,0x00,0x24,0x93,0x90,0xbb,0xea,
-0x00,0x0a,0x16,0x42,0x30,0x52,0x00,0x3f,0x2e,0x06,0x00,0x0c,0x10,0xc0,0x00,0xc0,
-0x00,0xa0,0x20,0x21,0x2c,0xa2,0x00,0x10,0x14,0x40,0x00,0x04,0x00,0x90,0x10,0x2b,
-0x30,0xa2,0x00,0x07,0x24,0x44,0x00,0x04,0x00,0x90,0x10,0x2b,0x10,0x40,0x00,0x0b,
-0x01,0x73,0x10,0x21,0x27,0x85,0xbb,0x1c,0x00,0x10,0x10,0x40,0x00,0x50,0x10,0x21,
-0x00,0x45,0x10,0x21,0x90,0x50,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x90,0x18,0x2b,
-0x14,0x60,0xff,0xfa,0x00,0x10,0x10,0x40,0x01,0x73,0x10,0x21,0x00,0x02,0x10,0x80,
-0x27,0x83,0x90,0x08,0x00,0x43,0x10,0x21,0x31,0xa4,0x00,0x01,0x10,0x80,0x00,0xa5,
-0xa0,0x50,0x00,0x07,0x3c,0x04,0xb0,0x05,0x34,0x84,0x00,0x08,0x24,0x02,0x00,0x01,
-0x3c,0x03,0x80,0x00,0xa1,0xe2,0x00,0x4e,0xac,0x83,0x00,0x00,0x8c,0x85,0x00,0x00,
-0x3c,0x02,0x00,0xf0,0x3c,0x03,0x40,0xf0,0x34,0x42,0xf0,0x00,0x34,0x63,0xf0,0x00,
-0x24,0x17,0x00,0x0e,0x24,0x13,0x01,0x06,0xac,0x82,0x00,0x00,0xac,0x83,0x00,0x00,
-0x27,0x82,0xb4,0x00,0x01,0x82,0x10,0x21,0x8c,0x43,0x00,0x00,0x24,0x05,0x00,0x01,
-0xaf,0xa5,0x00,0x1c,0x90,0x62,0x00,0x16,0x00,0x13,0xa8,0xc0,0x32,0x51,0x00,0x02,
-0x34,0x42,0x00,0x04,0xa0,0x62,0x00,0x16,0x8f,0xa3,0x00,0x20,0x8f,0xa4,0x00,0x18,
-0x00,0x03,0x13,0x43,0x00,0x04,0x1a,0x02,0x30,0x47,0x00,0x01,0x12,0x20,0x00,0x04,
-0x30,0x64,0x07,0xff,0x2e,0x03,0x00,0x04,0x32,0x42,0x00,0x33,0x00,0x43,0x90,0x0b,
-0x8f,0xa5,0x00,0x24,0x8f,0xa6,0x00,0x1c,0x00,0x12,0x10,0x40,0x00,0x05,0x19,0xc0,
-0x00,0x47,0x10,0x21,0x00,0x06,0x2a,0x80,0x00,0x43,0x10,0x21,0x00,0x10,0x32,0x00,
-0x00,0x04,0x24,0x80,0x02,0x65,0x28,0x21,0x00,0xa4,0x28,0x21,0x00,0x46,0x10,0x21,
-0x00,0x17,0x1c,0x00,0x3c,0x04,0xc0,0x00,0x00,0x43,0x30,0x21,0x16,0x80,0x00,0x29,
-0x00,0xa4,0x28,0x21,0x3c,0x02,0xb0,0x05,0x34,0x42,0x04,0x00,0x3c,0x03,0xb0,0x05,
-0x3c,0x04,0xb0,0x05,0xac,0x46,0x00,0x00,0x34,0x63,0x04,0x04,0x34,0x84,0x02,0x28,
-0x24,0x02,0x00,0x01,0xac,0x65,0x00,0x00,0xa0,0x82,0x00,0x00,0x3c,0x02,0xb0,0x09,
-0x34,0x42,0x01,0x46,0x90,0x44,0x00,0x00,0x91,0xe3,0x00,0x09,0x30,0x86,0x00,0x01,
-0x02,0x83,0x18,0x26,0x00,0x03,0x30,0x0b,0x14,0xc0,0x00,0x03,0x00,0x00,0x28,0x21,
-0x13,0xc0,0x00,0x03,0x02,0xb3,0x10,0x21,0x24,0x05,0x00,0x01,0x02,0xb3,0x10,0x21,
-0x27,0x83,0x90,0x08,0x00,0x02,0x10,0x80,0x00,0x43,0x10,0x21,0x84,0x48,0x00,0x04,
-0x00,0xa0,0x30,0x21,0x00,0xe0,0x20,0x21,0x02,0x80,0x28,0x21,0x02,0xc0,0x38,0x21,
-0x0c,0x00,0x00,0x70,0xaf,0xa8,0x00,0x10,0x7b,0xbe,0x02,0xbc,0x7b,0xb6,0x02,0x7c,
-0x7b,0xb4,0x02,0x3c,0x7b,0xb2,0x01,0xfc,0x7b,0xb0,0x01,0xbc,0x03,0xe0,0x00,0x08,
-0x27,0xbd,0x00,0x58,0x24,0x02,0x00,0x01,0x12,0x82,0x00,0x3d,0x3c,0x02,0xb0,0x05,
-0x24,0x02,0x00,0x02,0x12,0x82,0x00,0x31,0x3c,0x02,0xb0,0x05,0x24,0x02,0x00,0x03,
-0x12,0x82,0x00,0x25,0x3c,0x02,0xb0,0x05,0x24,0x02,0x00,0x10,0x12,0x82,0x00,0x19,
-0x3c,0x02,0xb0,0x05,0x24,0x02,0x00,0x11,0x12,0x82,0x00,0x0d,0x3c,0x02,0xb0,0x05,
-0x24,0x02,0x00,0x12,0x16,0x82,0xff,0xd1,0x3c,0x02,0xb0,0x05,0x3c,0x03,0xb0,0x05,
-0x34,0x42,0x04,0x20,0x3c,0x04,0xb0,0x05,0x34,0x63,0x04,0x24,0xac,0x46,0x00,0x00,
-0x34,0x84,0x02,0x28,0xac,0x65,0x00,0x00,0x08,0x00,0x07,0xe6,0x24,0x02,0x00,0x20,
-0x34,0x42,0x04,0x40,0x3c,0x03,0xb0,0x05,0x3c,0x04,0xb0,0x05,0xac,0x46,0x00,0x00,
-0x34,0x63,0x04,0x44,0x34,0x84,0x02,0x28,0x24,0x02,0x00,0x40,0x08,0x00,0x07,0xe6,
-0xac,0x65,0x00,0x00,0x34,0x42,0x04,0x28,0x3c,0x03,0xb0,0x05,0x3c,0x04,0xb0,0x05,
-0xac,0x46,0x00,0x00,0x34,0x63,0x04,0x2c,0x34,0x84,0x02,0x28,0x24,0x02,0xff,0x80,
-0x08,0x00,0x07,0xe6,0xac,0x65,0x00,0x00,0x34,0x42,0x04,0x18,0x3c,0x03,0xb0,0x05,
-0x3c,0x04,0xb0,0x05,0xac,0x46,0x00,0x00,0x34,0x63,0x04,0x1c,0x34,0x84,0x02,0x28,
-0x24,0x02,0x00,0x08,0x08,0x00,0x07,0xe6,0xac,0x65,0x00,0x00,0x34,0x42,0x04,0x10,
-0x3c,0x03,0xb0,0x05,0x3c,0x04,0xb0,0x05,0xac,0x46,0x00,0x00,0x34,0x63,0x04,0x14,
-0x34,0x84,0x02,0x28,0x24,0x02,0x00,0x04,0x08,0x00,0x07,0xe6,0xac,0x65,0x00,0x00,
-0x34,0x42,0x04,0x08,0x3c,0x03,0xb0,0x05,0x3c,0x04,0xb0,0x05,0xac,0x46,0x00,0x00,
-0x34,0x63,0x04,0x0c,0x34,0x84,0x02,0x28,0x24,0x02,0x00,0x02,0x08,0x00,0x07,0xe6,
-0xac,0x65,0x00,0x00,0x24,0x17,0x00,0x14,0x08,0x00,0x07,0xb8,0x24,0x13,0x01,0x02,
-0x30,0xa2,0x00,0x07,0x24,0x44,0x00,0x0c,0x00,0x90,0x18,0x2b,0x10,0x60,0x00,0x0c,
-0x26,0x02,0x00,0x04,0x27,0x85,0xbb,0x1c,0x00,0x10,0x10,0x40,0x00,0x50,0x10,0x21,
-0x00,0x45,0x10,0x21,0x90,0x50,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x90,0x18,0x2b,
-0x14,0x60,0xff,0xfa,0x00,0x10,0x10,0x40,0x2e,0x06,0x00,0x0c,0x26,0x02,0x00,0x04,
-0x08,0x00,0x07,0xa2,0x00,0x46,0x80,0x0a,0x27,0x82,0xb4,0x00,0x01,0x82,0x20,0x21,
-0x8c,0x87,0x00,0x00,0x00,0x00,0x00,0x00,0x90,0xe2,0x00,0x19,0x00,0x00,0x00,0x00,
-0x14,0x40,0x00,0x07,0x00,0x00,0x00,0x00,0x27,0x82,0x90,0x20,0x00,0xc2,0x10,0x21,
-0x90,0x43,0x00,0x00,0x00,0x00,0x00,0x00,0x10,0x60,0x00,0x14,0x00,0x00,0x00,0x00,
-0x90,0xe3,0x00,0x16,0x27,0x82,0x90,0x08,0x00,0xc2,0x10,0x21,0x34,0x63,0x00,0x20,
-0x90,0x50,0x00,0x07,0xa0,0xe3,0x00,0x16,0x8c,0x84,0x00,0x00,0x00,0x0a,0x1e,0x42,
-0x24,0x06,0x00,0x01,0x90,0x82,0x00,0x16,0x30,0x71,0x00,0x02,0x30,0x72,0x00,0x3f,
-0x30,0x42,0x00,0xfb,0x24,0x17,0x00,0x18,0x24,0x13,0x01,0x03,0x24,0x15,0x08,0x18,
-0xaf,0xa6,0x00,0x1c,0x08,0x00,0x07,0xc2,0xa0,0x82,0x00,0x16,0x8d,0x02,0x00,0x04,
-0x00,0x0a,0x1c,0x42,0x30,0x42,0x00,0x10,0x14,0x40,0x00,0x15,0x30,0x72,0x00,0x3f,
-0x81,0x22,0x00,0x05,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x11,0x30,0x72,0x00,0x3e,
-0x27,0x83,0x90,0x18,0x00,0xc3,0x18,0x21,0x80,0x64,0x00,0x00,0x27,0x83,0xb5,0x78,
-0x00,0x04,0x11,0x00,0x00,0x44,0x10,0x23,0x00,0x02,0x10,0x80,0x00,0x44,0x10,0x23,
-0x00,0x02,0x10,0x80,0x00,0x43,0x10,0x21,0x90,0x44,0x00,0x05,0x90,0x43,0x00,0x04,
-0x00,0x00,0x00,0x00,0x00,0x64,0x18,0x24,0x30,0x63,0x00,0x01,0x02,0x43,0x90,0x25,
-0x27,0x85,0xb4,0x00,0x01,0x85,0x28,0x21,0x8c,0xa6,0x00,0x00,0x01,0x73,0x10,0x21,
-0x27,0x83,0x90,0x10,0x90,0xc4,0x00,0x16,0x00,0x02,0x10,0x80,0x00,0x43,0x10,0x21,
-0x30,0x84,0x00,0xdf,0x90,0x50,0x00,0x00,0xa0,0xc4,0x00,0x16,0x80,0xc6,0x00,0x12,
-0x8c,0xa3,0x00,0x00,0x2d,0xc4,0x00,0x02,0xaf,0xa6,0x00,0x1c,0x90,0x62,0x00,0x16,
-0x00,0x00,0x00,0x00,0x30,0x42,0x00,0xfb,0x14,0x80,0x00,0x06,0xa0,0x62,0x00,0x16,
-0x24,0x02,0x00,0x06,0x11,0xc2,0x00,0x03,0x24,0x02,0x00,0x04,0x15,0xc2,0xff,0x0e,
-0x32,0x51,0x00,0x02,0x32,0x51,0x00,0x02,0x2e,0x02,0x00,0x0c,0x14,0x40,0x00,0x0f,
-0x00,0x11,0x18,0x2b,0x32,0x02,0x00,0x0f,0x34,0x42,0x00,0x10,0x00,0x03,0x19,0x00,
-0x00,0x43,0x18,0x21,0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0xe0,0xa0,0x43,0x00,0x00,
-0x00,0x00,0x20,0x21,0x02,0x00,0x28,0x21,0x0c,0x00,0x02,0x03,0xaf,0xaf,0x00,0x28,
-0x8f,0xaf,0x00,0x28,0x08,0x00,0x07,0xc2,0x00,0x00,0x00,0x00,0x08,0x00,0x08,0xbd,
-0x32,0x03,0x00,0xff,0x3c,0x03,0xb0,0x05,0x34,0x63,0x02,0x42,0x90,0x62,0x00,0x00,
-0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x0f,0x14,0x40,0xfe,0xaa,0x00,0x00,0x00,0x00,
-0x91,0xe2,0x00,0x09,0x00,0x00,0x00,0x00,0x02,0x82,0x10,0x26,0x08,0x00,0x07,0x79,
-0x00,0x02,0x28,0x0b,0x08,0x00,0x07,0x7f,0x00,0x00,0xb0,0x21,0x24,0x02,0x00,0x10,
-0x10,0xc2,0x00,0x08,0x24,0x02,0x00,0x11,0x10,0xc2,0xfe,0x7d,0x00,0x07,0x17,0x83,
-0x24,0x02,0x00,0x12,0x14,0xc2,0xfe,0x7b,0x00,0x07,0x17,0x43,0x08,0x00,0x07,0x59,
-0x30,0x5e,0x00,0x01,0x08,0x00,0x07,0x59,0x00,0x07,0xf7,0xc2,0x00,0x04,0x10,0x40,
-0x27,0x83,0x80,0x1c,0x00,0x43,0x10,0x21,0x00,0x80,0x40,0x21,0x94,0x44,0x00,0x00,
-0x2d,0x07,0x00,0x04,0x24,0xc2,0x00,0x03,0x00,0x47,0x30,0x0a,0x00,0x86,0x00,0x18,
-0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,0x34,0x63,0x00,0x20,0x24,0x42,0x23,0x8c,
-0xac,0x62,0x00,0x00,0x2d,0x06,0x00,0x10,0x00,0x00,0x20,0x12,0x00,0x04,0x22,0x42,
-0x24,0x84,0x00,0x01,0x24,0x83,0x00,0xc0,0x10,0xe0,0x00,0x0b,0x24,0x82,0x00,0x60,
-0x00,0x40,0x20,0x21,0x00,0x65,0x20,0x0a,0x3c,0x03,0xb0,0x03,0x34,0x63,0x01,0x00,
-0x90,0x62,0x00,0x00,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x01,0x00,0x44,0x20,0x04,
-0x03,0xe0,0x00,0x08,0x00,0x80,0x10,0x21,0x24,0x85,0x00,0x28,0x24,0x83,0x00,0x24,
-0x31,0x02,0x00,0x08,0x14,0xc0,0xff,0xf4,0x24,0x84,0x00,0x14,0x00,0x60,0x20,0x21,
-0x08,0x00,0x08,0xfa,0x00,0xa2,0x20,0x0b,0x27,0xbd,0xff,0xe0,0x3c,0x03,0xb0,0x03,
-0x3c,0x02,0x80,0x00,0xaf,0xb0,0x00,0x10,0x24,0x42,0x24,0x28,0x00,0x80,0x80,0x21,
-0x34,0x63,0x00,0x20,0x3c,0x04,0xb0,0x03,0xaf,0xb2,0x00,0x18,0xaf,0xb1,0x00,0x14,
-0xaf,0xbf,0x00,0x1c,0x83,0xb1,0x00,0x33,0x83,0xa8,0x00,0x37,0x34,0x84,0x01,0x10,
-0xac,0x62,0x00,0x00,0x2e,0x02,0x00,0x10,0x00,0xe0,0x90,0x21,0x8c,0x87,0x00,0x00,
-0x14,0x40,0x00,0x0c,0x2e,0x02,0x00,0x0c,0x3c,0x02,0x00,0x0f,0x34,0x42,0xf0,0x00,
-0x00,0xe2,0x10,0x24,0x14,0x40,0x00,0x37,0x32,0x02,0x00,0x08,0x32,0x02,0x00,0x07,
-0x27,0x83,0x80,0xcc,0x00,0x43,0x10,0x21,0x90,0x50,0x00,0x00,0x00,0x00,0x00,0x00,
-0x2e,0x02,0x00,0x0c,0x14,0x40,0x00,0x03,0x02,0x00,0x20,0x21,0x32,0x02,0x00,0x0f,
-0x24,0x44,0x00,0x0c,0x00,0x87,0x10,0x06,0x30,0x42,0x00,0x01,0x14,0x40,0x00,0x07,
-0x2c,0x82,0x00,0x0c,0x00,0x04,0x10,0x80,0x27,0x83,0xb4,0x50,0x00,0x43,0x10,0x21,
-0x8c,0x44,0x00,0x00,0x00,0x00,0x00,0x00,0x2c,0x82,0x00,0x0c,0x14,0x40,0x00,0x05,
-0x00,0x05,0x10,0x40,0x00,0x46,0x10,0x21,0x00,0x02,0x11,0x00,0x00,0x82,0x10,0x21,
-0x24,0x44,0x00,0x04,0x15,0x00,0x00,0x02,0x24,0x06,0x00,0x20,0x24,0x06,0x00,0x0e,
-0x0c,0x00,0x08,0xe3,0x00,0x00,0x00,0x00,0x00,0x40,0x30,0x21,0x3c,0x02,0xb0,0x03,
-0x34,0x42,0x01,0x00,0x90,0x43,0x00,0x00,0x2e,0x04,0x00,0x04,0x24,0x02,0x00,0x10,
-0x24,0x05,0x00,0x0a,0x00,0x44,0x28,0x0a,0x30,0x63,0x00,0x01,0x14,0x60,0x00,0x02,
-0x00,0x05,0x10,0x40,0x00,0xa0,0x10,0x21,0x30,0x45,0x00,0xff,0x00,0xc5,0x10,0x21,
-0x24,0x46,0x00,0x46,0x02,0x26,0x18,0x04,0xa6,0x43,0x00,0x00,0x8f,0xbf,0x00,0x1c,
-0x8f,0xb2,0x00,0x18,0x7b,0xb0,0x00,0xbc,0x00,0xc0,0x10,0x21,0x03,0xe0,0x00,0x08,
-0x27,0xbd,0x00,0x20,0x10,0x40,0xff,0xcf,0x2e,0x02,0x00,0x0c,0x32,0x02,0x00,0x07,
-0x27,0x83,0x80,0xc4,0x00,0x43,0x10,0x21,0x90,0x44,0x00,0x00,0x08,0x00,0x09,0x28,
-0x02,0x04,0x80,0x23,0x27,0xbd,0xff,0xb8,0x00,0x05,0x38,0x80,0x27,0x82,0xb4,0x00,
-0xaf,0xbe,0x00,0x40,0xaf,0xb6,0x00,0x38,0xaf,0xb3,0x00,0x2c,0xaf,0xbf,0x00,0x44,
-0xaf,0xb7,0x00,0x3c,0xaf,0xb5,0x00,0x34,0xaf,0xb4,0x00,0x30,0xaf,0xb2,0x00,0x28,
-0xaf,0xb1,0x00,0x24,0xaf,0xb0,0x00,0x20,0x00,0xe2,0x38,0x21,0x8c,0xe6,0x00,0x00,
-0xaf,0xa5,0x00,0x4c,0x3c,0x02,0x80,0x00,0x3c,0x05,0xb0,0x03,0x34,0xa5,0x00,0x20,
-0x24,0x42,0x25,0x84,0x24,0x03,0x00,0x01,0xac,0xa2,0x00,0x00,0xa0,0xc3,0x00,0x12,
-0x8c,0xe5,0x00,0x00,0x94,0xc3,0x00,0x06,0x90,0xa2,0x00,0x16,0xa4,0xc3,0x00,0x14,
-0x27,0x83,0x90,0x00,0x34,0x42,0x00,0x08,0xa0,0xa2,0x00,0x16,0x8c,0xe8,0x00,0x00,
-0xaf,0xa4,0x00,0x48,0x27,0x82,0x90,0x04,0x95,0x11,0x00,0x14,0x00,0x00,0x00,0x00,
-0x00,0x11,0x98,0xc0,0x02,0x71,0x20,0x21,0x00,0x04,0x20,0x80,0x00,0x82,0x10,0x21,
-0x8c,0x52,0x00,0x18,0x00,0x83,0x18,0x21,0x84,0x75,0x00,0x06,0x8e,0x45,0x00,0x08,
-0x8e,0x46,0x00,0x04,0x8e,0x47,0x00,0x04,0x00,0x05,0x1c,0x82,0x00,0x06,0x31,0x42,
-0x27,0x82,0x90,0x10,0x30,0x63,0x00,0x01,0x30,0xc6,0x00,0x01,0x00,0x82,0x20,0x21,
-0xa5,0x15,0x00,0x1a,0x00,0x05,0x14,0x42,0xaf,0xa3,0x00,0x18,0xaf,0xa6,0x00,0x1c,
-0x30,0xe7,0x00,0x10,0x30,0x56,0x00,0x01,0x80,0x97,0x00,0x06,0x14,0xe0,0x00,0x47,
-0x00,0x05,0xf7,0xc2,0x80,0x82,0x00,0x05,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x44,
-0x02,0x71,0x10,0x21,0x93,0x90,0xbb,0xe9,0x00,0x00,0x00,0x00,0x2e,0x02,0x00,0x0c,
-0x14,0x40,0x00,0x06,0x02,0x00,0x20,0x21,0x00,0x16,0x10,0x40,0x00,0x43,0x10,0x21,
-0x00,0x02,0x11,0x00,0x02,0x02,0x10,0x21,0x24,0x44,0x00,0x04,0x02,0x71,0x10,0x21,
-0x00,0x02,0x10,0x80,0x27,0x83,0x90,0x10,0x00,0x43,0x10,0x21,0x00,0x80,0x80,0x21,
-0xa0,0x44,0x00,0x03,0xa0,0x44,0x00,0x00,0x02,0x00,0x20,0x21,0x02,0xc0,0x28,0x21,
-0x0c,0x00,0x08,0xe3,0x02,0xa0,0x30,0x21,0x02,0x71,0x18,0x21,0x00,0x03,0x88,0x80,
-0x00,0x40,0xa0,0x21,0x27,0x82,0x90,0x20,0x02,0x22,0x10,0x21,0x8c,0x44,0x00,0x00,
-0x26,0xe3,0x00,0x02,0x00,0x03,0x17,0xc2,0x00,0x62,0x18,0x21,0x00,0x04,0x25,0xc2,
-0x00,0x03,0x18,0x43,0x30,0x84,0x00,0x01,0x00,0x03,0x18,0x40,0x03,0xc4,0x20,0x24,
-0x14,0x80,0x00,0x15,0x02,0x43,0x38,0x21,0x3c,0x08,0xb0,0x03,0x35,0x08,0x00,0x28,
-0x8d,0x03,0x00,0x00,0x8f,0xa6,0x00,0x4c,0x8f,0xa4,0x00,0x48,0x27,0x82,0x90,0x08,
-0x02,0x22,0x10,0x21,0x24,0x63,0x00,0x01,0x02,0xa0,0x28,0x21,0xa4,0x54,0x00,0x04,
-0x00,0xc0,0x38,0x21,0x0c,0x00,0x07,0x2f,0xad,0x03,0x00,0x00,0x7b,0xbe,0x02,0x3c,
-0x7b,0xb6,0x01,0xfc,0x7b,0xb4,0x01,0xbc,0x7b,0xb2,0x01,0x7c,0x7b,0xb0,0x01,0x3c,
-0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x48,0x8f,0xa2,0x00,0x1c,0x8f,0xa6,0x00,0x18,
-0x02,0x00,0x20,0x21,0x02,0xc0,0x28,0x21,0xaf,0xa2,0x00,0x10,0x0c,0x00,0x09,0x0a,
-0xaf,0xa0,0x00,0x14,0x08,0x00,0x09,0xc6,0x02,0x82,0xa0,0x21,0x02,0x71,0x10,0x21,
-0x00,0x02,0x10,0x80,0x27,0x83,0x90,0x10,0x00,0x43,0x10,0x21,0x90,0x50,0x00,0x00,
-0x08,0x00,0x09,0xb2,0xa0,0x50,0x00,0x03,0x27,0xbd,0xff,0xb8,0xaf,0xb1,0x00,0x24,
-0x8f,0xb1,0x00,0x5c,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,0x34,0x63,0x00,0x20,
-0x24,0x42,0x27,0xa8,0xaf,0xbe,0x00,0x40,0xaf,0xb7,0x00,0x3c,0xaf,0xb6,0x00,0x38,
-0xaf,0xb5,0x00,0x34,0xaf,0xb4,0x00,0x30,0xaf,0xa5,0x00,0x4c,0x8f,0xb5,0x00,0x58,
-0xaf,0xbf,0x00,0x44,0xaf,0xb3,0x00,0x2c,0xaf,0xb2,0x00,0x28,0xaf,0xb0,0x00,0x20,
-0x00,0xe0,0xb0,0x21,0xac,0x62,0x00,0x00,0x00,0x80,0xf0,0x21,0x00,0x00,0xb8,0x21,
-0x16,0x20,0x00,0x2b,0x00,0x00,0xa0,0x21,0x27,0x85,0xb4,0x00,0x00,0x07,0x10,0x80,
-0x00,0x45,0x10,0x21,0x8c,0x53,0x00,0x00,0x00,0x15,0x18,0x80,0x00,0x65,0x18,0x21,
-0x92,0x62,0x00,0x16,0x8c,0x72,0x00,0x00,0x30,0x42,0x00,0x03,0x14,0x40,0x00,0x2d,
-0x00,0x00,0x00,0x00,0x92,0x42,0x00,0x16,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x03,
-0x14,0x40,0x00,0x28,0x00,0x00,0x00,0x00,0x8c,0x82,0x00,0x34,0x00,0x00,0x00,0x00,
-0x14,0x40,0x00,0x18,0x02,0x20,0x10,0x21,0x8c,0x82,0x00,0x38,0x00,0x00,0x00,0x00,
-0x14,0x40,0x00,0x14,0x02,0x20,0x10,0x21,0x8c,0x82,0x00,0x3c,0x00,0x00,0x00,0x00,
-0x14,0x40,0x00,0x0f,0x3c,0x03,0xb0,0x09,0x3c,0x05,0xb0,0x05,0x34,0x63,0x01,0x44,
-0x34,0xa5,0x02,0x52,0x94,0x66,0x00,0x00,0x90,0xa2,0x00,0x00,0x8f,0xa3,0x00,0x4c,
-0x00,0x00,0x00,0x00,0x00,0x62,0x10,0x06,0x30,0x42,0x00,0x01,0x10,0x40,0x00,0x04,
-0x30,0xc6,0xff,0xff,0x2c,0xc2,0x00,0x41,0x10,0x40,0x00,0x09,0x24,0x05,0x00,0x14,
-0x02,0x20,0x10,0x21,0x7b,0xbe,0x02,0x3c,0x7b,0xb6,0x01,0xfc,0x7b,0xb4,0x01,0xbc,
-0x7b,0xb2,0x01,0x7c,0x7b,0xb0,0x01,0x3c,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x48,
-0x0c,0x00,0x07,0x0a,0x24,0x06,0x01,0x07,0x24,0x02,0x00,0x01,0x08,0x00,0x0a,0x2c,
-0xa3,0xc2,0x00,0x11,0x10,0xc0,0x00,0x1c,0x24,0x02,0x00,0x01,0x10,0xc2,0x00,0x17,
-0x00,0xc0,0x88,0x21,0x96,0x54,0x00,0x1a,0x02,0xa0,0xb8,0x21,0x12,0x20,0xff,0xed,
-0x02,0x20,0x10,0x21,0x27,0x83,0xb4,0x00,0x00,0x17,0x10,0x80,0x00,0x43,0x10,0x21,
-0x8c,0x44,0x00,0x00,0x3c,0x03,0xb0,0x03,0x34,0x63,0x00,0x28,0x80,0x86,0x00,0x12,
-0x8c,0x62,0x00,0x00,0x00,0x14,0x2c,0x00,0x00,0x05,0x2c,0x03,0x00,0x46,0x10,0x21,
-0x8f,0xa6,0x00,0x4c,0x02,0xe0,0x38,0x21,0x03,0xc0,0x20,0x21,0x0c,0x00,0x07,0x2f,
-0xac,0x62,0x00,0x00,0x08,0x00,0x0a,0x2c,0xaf,0xd1,0x00,0x40,0x96,0x74,0x00,0x1a,
-0x08,0x00,0x0a,0x3f,0x02,0xc0,0xb8,0x21,0x3c,0x02,0xb0,0x03,0x34,0x42,0x01,0x08,
-0x8c,0x50,0x00,0x00,0x02,0x60,0x20,0x21,0x0c,0x00,0x1e,0xf3,0x02,0x00,0x28,0x21,
-0x30,0x42,0x00,0xff,0x02,0x00,0x28,0x21,0x02,0x40,0x20,0x21,0x0c,0x00,0x1e,0xf3,
-0xaf,0xa2,0x00,0x18,0x8f,0xa4,0x00,0x18,0x00,0x00,0x00,0x00,0x10,0x80,0x00,0xed,
-0x30,0x50,0x00,0xff,0x12,0x00,0x00,0x18,0x24,0x11,0x00,0x01,0x96,0x63,0x00,0x14,
-0x96,0x44,0x00,0x14,0x27,0x85,0x90,0x00,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,
-0x00,0x02,0x10,0x80,0x00,0x45,0x10,0x21,0x00,0x04,0x18,0xc0,0x8c,0x46,0x00,0x08,
-0x00,0x64,0x18,0x21,0x00,0x03,0x18,0x80,0x00,0x65,0x18,0x21,0x00,0x06,0x17,0x02,
-0x24,0x04,0x00,0xff,0x8c,0x63,0x00,0x08,0x10,0x44,0x00,0xd6,0x00,0x03,0x17,0x02,
-0x10,0x44,0x00,0xd5,0x3c,0x02,0x80,0x00,0x00,0x66,0x18,0x2b,0x24,0x11,0x00,0x02,
-0x24,0x02,0x00,0x01,0x00,0x43,0x88,0x0a,0x24,0x02,0x00,0x01,0x12,0x22,0x00,0x5a,
-0x24,0x02,0x00,0x02,0x16,0x22,0xff,0xbd,0x00,0x00,0x00,0x00,0x96,0x49,0x00,0x14,
-0x27,0x82,0x90,0x04,0x02,0xa0,0xb8,0x21,0x00,0x09,0x50,0xc0,0x01,0x49,0x18,0x21,
-0x00,0x03,0x40,0x80,0x01,0x02,0x10,0x21,0x8c,0x43,0x00,0x18,0x00,0x00,0x00,0x00,
-0x8c,0x65,0x00,0x08,0x8c,0x62,0x00,0x0c,0x8c,0x62,0x00,0x04,0x00,0x05,0x24,0x42,
-0x00,0x05,0x1c,0x82,0x30,0x42,0x00,0x10,0x30,0x66,0x00,0x01,0x14,0x40,0x00,0x41,
-0x30,0x87,0x00,0x01,0x27,0x82,0x90,0x18,0x01,0x02,0x10,0x21,0x80,0x44,0x00,0x00,
-0x27,0x82,0xb5,0x78,0x00,0x04,0x19,0x00,0x00,0x64,0x18,0x23,0x00,0x03,0x18,0x80,
-0x00,0x64,0x18,0x23,0x00,0x03,0x18,0x80,0x00,0x62,0x10,0x21,0x90,0x45,0x00,0x05,
-0x27,0x84,0xb4,0xa0,0x00,0x64,0x18,0x21,0x90,0x63,0x00,0x00,0x10,0xa0,0x00,0x2b,
-0x2c,0x64,0x00,0x0c,0x14,0x80,0x00,0x04,0x00,0x60,0x10,0x21,0x00,0x06,0x11,0x00,
-0x00,0x62,0x10,0x21,0x24,0x42,0x00,0x24,0x3c,0x01,0xb0,0x03,0xa0,0x22,0x00,0xe1,
-0x14,0x80,0x00,0x06,0x00,0x60,0x28,0x21,0x00,0x07,0x10,0x40,0x00,0x46,0x10,0x21,
-0x00,0x02,0x11,0x00,0x00,0x62,0x10,0x21,0x24,0x45,0x00,0x04,0x01,0x49,0x10,0x21,
-0x27,0x83,0x90,0x10,0x00,0x02,0x10,0x80,0x00,0x43,0x10,0x21,0x00,0xa0,0x18,0x21,
-0xa0,0x45,0x00,0x03,0xa0,0x45,0x00,0x00,0x24,0x02,0x00,0x08,0x12,0x02,0x00,0x0b,
-0x24,0x02,0x00,0x01,0x00,0x60,0x28,0x21,0x02,0x40,0x20,0x21,0x0c,0x00,0x1f,0x6f,
-0xaf,0xa2,0x00,0x10,0x30,0x54,0xff,0xff,0x92,0x42,0x00,0x16,0x00,0x00,0x00,0x00,
-0x02,0x02,0x10,0x25,0x08,0x00,0x0a,0x3f,0xa2,0x42,0x00,0x16,0x00,0x60,0x28,0x21,
-0x02,0x40,0x20,0x21,0x0c,0x00,0x1f,0x20,0xaf,0xa0,0x00,0x10,0x08,0x00,0x0a,0xc2,
-0x30,0x54,0xff,0xff,0x08,0x00,0x0a,0xaa,0x00,0x60,0x10,0x21,0x14,0x80,0xff,0xfd,
-0x00,0x00,0x00,0x00,0x00,0x06,0x11,0x00,0x00,0x62,0x10,0x21,0x08,0x00,0x0a,0xaa,
-0x24,0x42,0x00,0x04,0x27,0x82,0x90,0x10,0x01,0x02,0x10,0x21,0x90,0x43,0x00,0x00,
-0x08,0x00,0x0a,0xba,0xa0,0x43,0x00,0x03,0x96,0x69,0x00,0x14,0x02,0xc0,0xb8,0x21,
-0x24,0x0b,0x00,0x01,0x00,0x09,0x10,0xc0,0x00,0x49,0x18,0x21,0x00,0x03,0x40,0x80,
-0x00,0x40,0x50,0x21,0x27,0x82,0x90,0x04,0x01,0x02,0x10,0x21,0x8c,0x43,0x00,0x18,
-0x00,0x00,0x00,0x00,0x8c,0x65,0x00,0x08,0x8c,0x62,0x00,0x0c,0x8c,0x62,0x00,0x04,
-0x00,0x05,0x24,0x42,0x00,0x05,0x1c,0x82,0x30,0x42,0x00,0x10,0x30,0x66,0x00,0x01,
-0x10,0x40,0x00,0x0d,0x30,0x87,0x00,0x01,0x27,0x82,0x90,0x18,0x01,0x02,0x10,0x21,
-0x80,0x43,0x00,0x00,0x00,0x00,0x58,0x21,0x00,0x03,0x11,0x00,0x00,0x43,0x10,0x23,
-0x00,0x02,0x10,0x80,0x00,0x43,0x10,0x23,0x00,0x02,0x10,0x80,0x27,0x83,0xb5,0x70,
-0x00,0x43,0x10,0x21,0xa0,0x40,0x00,0x04,0x11,0x60,0x00,0x4f,0x00,0x00,0x00,0x00,
-0x01,0x49,0x10,0x21,0x00,0x02,0x20,0x80,0x27,0x85,0x90,0x10,0x00,0x85,0x10,0x21,
-0x80,0x43,0x00,0x05,0x00,0x00,0x00,0x00,0x14,0x60,0x00,0x42,0x01,0x49,0x10,0x21,
-0x27,0x82,0x90,0x18,0x00,0x82,0x10,0x21,0x80,0x44,0x00,0x00,0x27,0x82,0xb5,0x78,
-0x00,0x04,0x19,0x00,0x00,0x64,0x18,0x23,0x00,0x03,0x18,0x80,0x00,0x64,0x18,0x23,
-0x00,0x03,0x18,0x80,0x00,0x62,0x10,0x21,0x90,0x45,0x00,0x05,0x27,0x84,0xb4,0xa0,
-0x00,0x64,0x18,0x21,0x90,0x63,0x00,0x00,0x10,0xa0,0x00,0x2c,0x2c,0x64,0x00,0x0c,
-0x14,0x80,0x00,0x04,0x00,0x60,0x10,0x21,0x00,0x06,0x11,0x00,0x00,0x62,0x10,0x21,
-0x24,0x42,0x00,0x24,0x3c,0x01,0xb0,0x03,0xa0,0x22,0x00,0xe1,0x14,0x80,0x00,0x06,
-0x00,0x60,0x28,0x21,0x00,0x07,0x10,0x40,0x00,0x46,0x10,0x21,0x00,0x02,0x11,0x00,
-0x00,0x62,0x10,0x21,0x24,0x45,0x00,0x04,0x01,0x49,0x10,0x21,0x27,0x83,0x90,0x10,
-0x00,0x02,0x10,0x80,0x00,0x43,0x10,0x21,0x00,0xa0,0x18,0x21,0xa0,0x45,0x00,0x03,
-0xa0,0x45,0x00,0x00,0x8f,0xa4,0x00,0x18,0x24,0x02,0x00,0x08,0x10,0x82,0x00,0x0c,
-0x00,0x60,0x28,0x21,0x24,0x02,0x00,0x01,0x02,0x60,0x20,0x21,0x0c,0x00,0x1f,0x6f,
-0xaf,0xa2,0x00,0x10,0x8f,0xa3,0x00,0x18,0x30,0x54,0xff,0xff,0x92,0x62,0x00,0x16,
-0x00,0x00,0x00,0x00,0x00,0x62,0x10,0x25,0x08,0x00,0x0a,0x3f,0xa2,0x62,0x00,0x16,
-0x02,0x60,0x20,0x21,0x0c,0x00,0x1f,0x20,0xaf,0xa0,0x00,0x10,0x08,0x00,0x0b,0x31,
-0x00,0x00,0x00,0x00,0x08,0x00,0x0b,0x19,0x00,0x60,0x10,0x21,0x14,0x80,0xff,0xfd,
-0x00,0x00,0x00,0x00,0x00,0x06,0x11,0x00,0x00,0x62,0x10,0x21,0x08,0x00,0x0b,0x19,
-0x24,0x42,0x00,0x04,0x00,0x02,0x10,0x80,0x00,0x45,0x10,0x21,0x90,0x43,0x00,0x00,
-0x08,0x00,0x0b,0x29,0xa0,0x43,0x00,0x03,0x27,0x85,0x90,0x10,0x08,0x00,0x0b,0x45,
-0x01,0x49,0x10,0x21,0x3c,0x02,0x80,0x00,0x00,0x62,0x18,0x26,0x08,0x00,0x0a,0x7a,
-0x00,0xc2,0x30,0x26,0x12,0x00,0xff,0x2d,0x24,0x02,0x00,0x01,0x08,0x00,0x0a,0x7f,
-0x24,0x11,0x00,0x02,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,0x27,0xbd,0xff,0xd0,
-0x24,0x42,0x2d,0x54,0x34,0x63,0x00,0x20,0x3c,0x05,0xb0,0x05,0xaf,0xb3,0x00,0x24,
-0xaf,0xb2,0x00,0x20,0xaf,0xb1,0x00,0x1c,0xaf,0xbf,0x00,0x28,0xaf,0xb0,0x00,0x18,
-0xac,0x62,0x00,0x00,0x34,0xa5,0x02,0x42,0x90,0xa2,0x00,0x00,0x00,0x80,0x90,0x21,
-0x24,0x11,0x00,0x10,0x30,0x53,0x00,0xff,0x24,0x02,0x00,0x10,0x12,0x22,0x00,0xcf,
-0x00,0x00,0x18,0x21,0x24,0x02,0x00,0x11,0x12,0x22,0x00,0xc1,0x24,0x02,0x00,0x12,
-0x12,0x22,0x00,0xb4,0x00,0x00,0x00,0x00,0x14,0x60,0x00,0xad,0xae,0x43,0x00,0x40,
-0x3c,0x02,0xb0,0x05,0x34,0x42,0x02,0x2c,0x8c,0x44,0x00,0x00,0x3c,0x03,0x00,0x02,
-0x34,0x63,0x00,0xff,0x00,0x83,0x80,0x24,0x00,0x10,0x14,0x43,0x10,0x40,0x00,0x05,
-0x00,0x00,0x00,0x00,0x8e,0x42,0x00,0x34,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x92,
-0x00,0x00,0x00,0x00,0x93,0x83,0x8b,0x71,0x00,0x00,0x00,0x00,0x30,0x62,0x00,0x02,
-0x10,0x40,0x00,0x04,0x32,0x10,0x00,0xff,0x00,0x10,0x11,0xc3,0x14,0x40,0x00,0x86,
-0x00,0x00,0x00,0x00,0x16,0x00,0x00,0x15,0x02,0x00,0x10,0x21,0x26,0x22,0x00,0x01,
-0x30,0x51,0x00,0xff,0x2e,0x23,0x00,0x13,0x14,0x60,0xff,0xdb,0x24,0x03,0x00,0x02,
-0x12,0x63,0x00,0x73,0x24,0x02,0x00,0x05,0x2a,0x62,0x00,0x03,0x10,0x40,0x00,0x58,
-0x24,0x02,0x00,0x04,0x24,0x02,0x00,0x01,0x12,0x62,0x00,0x4b,0x02,0x40,0x20,0x21,
-0x3c,0x02,0xb0,0x05,0x34,0x42,0x02,0x2c,0x8c,0x43,0x00,0x00,0x00,0x00,0x00,0x00,
-0x30,0x70,0x00,0xff,0x12,0x00,0x00,0x06,0x02,0x00,0x10,0x21,0x8f,0xbf,0x00,0x28,
-0x7b,0xb2,0x01,0x3c,0x7b,0xb0,0x00,0xfc,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x30,
-0x92,0x46,0x00,0x04,0x8e,0x43,0x00,0x24,0x24,0x02,0x00,0x07,0x02,0x40,0x20,0x21,
-0x00,0x00,0x28,0x21,0x24,0x07,0x00,0x06,0xaf,0xa2,0x00,0x10,0x0c,0x00,0x09,0xea,
-0xaf,0xa3,0x00,0x14,0xae,0x42,0x00,0x24,0x3c,0x02,0xb0,0x05,0x8c,0x42,0x02,0x2c,
-0x00,0x00,0x00,0x00,0x30,0x50,0x00,0xff,0x16,0x00,0xff,0xec,0x02,0x00,0x10,0x21,
-0x92,0x46,0x00,0x05,0x8e,0x43,0x00,0x28,0x24,0x02,0x00,0x05,0x02,0x40,0x20,0x21,
-0x24,0x05,0x00,0x01,0x24,0x07,0x00,0x04,0xaf,0xa2,0x00,0x10,0x0c,0x00,0x09,0xea,
-0xaf,0xa3,0x00,0x14,0xae,0x42,0x00,0x28,0x3c,0x02,0xb0,0x05,0x8c,0x42,0x02,0x2c,
-0x00,0x00,0x00,0x00,0x30,0x50,0x00,0xff,0x16,0x00,0xff,0xdc,0x02,0x00,0x10,0x21,
-0x92,0x46,0x00,0x06,0x8e,0x43,0x00,0x2c,0x24,0x02,0x00,0x03,0x02,0x40,0x20,0x21,
-0x24,0x05,0x00,0x02,0x00,0x00,0x38,0x21,0xaf,0xa2,0x00,0x10,0x0c,0x00,0x09,0xea,
-0xaf,0xa3,0x00,0x14,0xae,0x42,0x00,0x2c,0x3c,0x02,0xb0,0x05,0x8c,0x42,0x02,0x2c,
-0x00,0x00,0x00,0x00,0x30,0x50,0x00,0xff,0x16,0x00,0xff,0xcc,0x02,0x00,0x10,0x21,
-0x92,0x46,0x00,0x07,0x8e,0x43,0x00,0x30,0x24,0x02,0x00,0x02,0x02,0x40,0x20,0x21,
-0x24,0x05,0x00,0x03,0x24,0x07,0x00,0x01,0xaf,0xa2,0x00,0x10,0x0c,0x00,0x09,0xea,
-0xaf,0xa3,0x00,0x14,0xae,0x42,0x00,0x30,0x3c,0x02,0xb0,0x05,0x8c,0x42,0x02,0x2c,
-0x08,0x00,0x0b,0x9b,0x30,0x42,0x00,0xff,0x92,0x46,0x00,0x04,0x8e,0x43,0x00,0x24,
-0x24,0x02,0x00,0x07,0x00,0x00,0x28,0x21,0x24,0x07,0x00,0x06,0xaf,0xa2,0x00,0x10,
-0x0c,0x00,0x09,0xea,0xaf,0xa3,0x00,0x14,0x08,0x00,0x0b,0x94,0xae,0x42,0x00,0x24,
-0x12,0x62,0x00,0x0d,0x24,0x02,0x00,0x03,0x24,0x02,0x00,0x08,0x16,0x62,0xff,0xa8,
-0x02,0x40,0x20,0x21,0x92,0x46,0x00,0x07,0x8e,0x42,0x00,0x30,0x24,0x05,0x00,0x03,
-0x24,0x07,0x00,0x01,0xaf,0xa3,0x00,0x10,0x0c,0x00,0x09,0xea,0xaf,0xa2,0x00,0x14,
-0x08,0x00,0x0b,0x94,0xae,0x42,0x00,0x30,0x92,0x46,0x00,0x06,0x8e,0x43,0x00,0x2c,
-0x02,0x40,0x20,0x21,0x24,0x05,0x00,0x02,0x00,0x00,0x38,0x21,0xaf,0xa2,0x00,0x10,
-0x0c,0x00,0x09,0xea,0xaf,0xa3,0x00,0x14,0x08,0x00,0x0b,0x94,0xae,0x42,0x00,0x2c,
-0x92,0x46,0x00,0x05,0x8e,0x43,0x00,0x28,0x02,0x40,0x20,0x21,0x24,0x05,0x00,0x01,
-0x24,0x07,0x00,0x04,0xaf,0xa2,0x00,0x10,0x0c,0x00,0x09,0xea,0xaf,0xa3,0x00,0x14,
-0x08,0x00,0x0b,0x94,0xae,0x42,0x00,0x28,0x0c,0x00,0x01,0x57,0x24,0x04,0x00,0x01,
-0x08,0x00,0x0b,0x85,0x00,0x00,0x00,0x00,0x8f,0x84,0xb4,0x40,0xae,0x40,0x00,0x34,
-0x94,0x85,0x00,0x14,0x0c,0x00,0x1b,0x66,0x00,0x00,0x00,0x00,0x93,0x83,0x8b,0x71,
-0x00,0x00,0x00,0x00,0x30,0x62,0x00,0x02,0x10,0x40,0xff,0x69,0x00,0x00,0x00,0x00,
-0x0c,0x00,0x01,0x57,0x00,0x00,0x20,0x21,0x08,0x00,0x0b,0x7d,0x00,0x00,0x00,0x00,
-0x02,0x40,0x20,0x21,0x0c,0x00,0x09,0x61,0x02,0x20,0x28,0x21,0x08,0x00,0x0b,0x71,
-0x3c,0x02,0xb0,0x05,0x8e,0x42,0x00,0x3c,0x00,0x00,0x00,0x00,0x14,0x40,0xff,0x4a,
-0x00,0x00,0x00,0x00,0x8f,0x82,0xb4,0x48,0x00,0x00,0x00,0x00,0x90,0x42,0x00,0x0a,
-0x00,0x00,0x00,0x00,0x00,0x02,0x18,0x2b,0x08,0x00,0x0b,0x6e,0xae,0x43,0x00,0x3c,
-0x8e,0x42,0x00,0x38,0x00,0x00,0x00,0x00,0x14,0x40,0xff,0x3d,0x24,0x02,0x00,0x12,
-0x8f,0x82,0xb4,0x44,0x00,0x00,0x00,0x00,0x90,0x42,0x00,0x0a,0x00,0x00,0x00,0x00,
-0x00,0x02,0x18,0x2b,0x08,0x00,0x0b,0x6e,0xae,0x43,0x00,0x38,0x8e,0x42,0x00,0x34,
-0x00,0x00,0x00,0x00,0x14,0x40,0xff,0x30,0x24,0x02,0x00,0x11,0x8f,0x82,0xb4,0x40,
-0x00,0x00,0x00,0x00,0x90,0x42,0x00,0x0a,0x00,0x00,0x00,0x00,0x00,0x02,0x18,0x2b,
-0x08,0x00,0x0b,0x6e,0xae,0x43,0x00,0x34,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,
-0x27,0xbd,0xff,0xe0,0x34,0x63,0x00,0x20,0x24,0x42,0x31,0x08,0x3c,0x08,0xb0,0x03,
-0xaf,0xb1,0x00,0x14,0xac,0x62,0x00,0x00,0x35,0x08,0x01,0x00,0xaf,0xbf,0x00,0x18,
-0xaf,0xb0,0x00,0x10,0x91,0x03,0x00,0x00,0x00,0xa0,0x48,0x21,0x24,0x11,0x00,0x0a,
-0x2c,0xa5,0x00,0x04,0x24,0x02,0x00,0x10,0x00,0x45,0x88,0x0a,0x30,0x63,0x00,0x01,
-0x00,0xc0,0x28,0x21,0x14,0x60,0x00,0x02,0x00,0x11,0x40,0x40,0x02,0x20,0x40,0x21,
-0x84,0x83,0x00,0x0c,0x31,0x11,0x00,0xff,0x01,0x20,0x20,0x21,0x00,0x03,0x10,0xc0,
-0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x80,0x27,0x83,0x90,0x08,0x00,0x43,0x10,0x21,
-0x84,0x43,0x00,0x04,0x24,0x06,0x00,0x0e,0x10,0xe0,0x00,0x06,0x02,0x23,0x80,0x21,
-0x02,0x00,0x10,0x21,0x8f,0xbf,0x00,0x18,0x7b,0xb0,0x00,0xbc,0x03,0xe0,0x00,0x08,
-0x27,0xbd,0x00,0x20,0x0c,0x00,0x08,0xe3,0x00,0x00,0x00,0x00,0x02,0x11,0x18,0x21,
-0x08,0x00,0x0c,0x64,0x00,0x62,0x80,0x21,0x27,0xbd,0xff,0xd0,0xaf,0xbf,0x00,0x28,
-0xaf,0xb4,0x00,0x20,0xaf,0xb3,0x00,0x1c,0xaf,0xb2,0x00,0x18,0xaf,0xb5,0x00,0x24,
-0xaf,0xb1,0x00,0x14,0xaf,0xb0,0x00,0x10,0x84,0x82,0x00,0x0c,0x3c,0x06,0xb0,0x03,
-0x34,0xc6,0x00,0x20,0x00,0x02,0x18,0xc0,0x00,0x62,0x18,0x21,0x00,0x03,0x18,0x80,
-0x27,0x82,0x90,0x04,0x00,0x62,0x10,0x21,0x8c,0x55,0x00,0x18,0x3c,0x02,0x80,0x00,
-0x24,0x42,0x31,0xb8,0xac,0xc2,0x00,0x00,0x8e,0xb0,0x00,0x08,0x27,0x82,0x90,0x08,
-0x00,0x62,0x18,0x21,0x90,0x71,0x00,0x07,0x00,0x10,0x86,0x43,0x32,0x10,0x00,0x01,
-0x00,0xa0,0x38,0x21,0x02,0x00,0x30,0x21,0x00,0xa0,0x98,0x21,0x02,0x20,0x28,0x21,
-0x0c,0x00,0x0c,0x42,0x00,0x80,0x90,0x21,0x02,0x20,0x20,0x21,0x02,0x00,0x28,0x21,
-0x24,0x06,0x00,0x14,0x0c,0x00,0x08,0xe3,0x00,0x40,0xa0,0x21,0x86,0x43,0x00,0x0c,
-0x3c,0x09,0xb0,0x09,0x3c,0x08,0xb0,0x09,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,
-0x00,0x02,0x10,0x80,0x27,0x83,0x90,0x10,0x00,0x43,0x10,0x21,0x80,0x43,0x00,0x06,
-0x3c,0x07,0xb0,0x09,0x3c,0x05,0xb0,0x09,0x28,0x62,0x00,0x00,0x24,0x64,0x00,0x03,
-0x00,0x82,0x18,0x0b,0x00,0x03,0x18,0x83,0x3c,0x02,0xb0,0x09,0x00,0x03,0x18,0x80,
-0x34,0x42,0x01,0x02,0x35,0x29,0x01,0x10,0x35,0x08,0x01,0x14,0x34,0xe7,0x01,0x20,
-0x34,0xa5,0x01,0x24,0xa4,0x54,0x00,0x00,0x12,0x60,0x00,0x11,0x02,0xa3,0xa8,0x21,
-0x8e,0xa2,0x00,0x0c,0x8e,0xa3,0x00,0x08,0x00,0x02,0x14,0x00,0x00,0x03,0x1c,0x02,
-0x00,0x43,0x10,0x21,0xad,0x22,0x00,0x00,0x8e,0xa3,0x00,0x0c,0x00,0x00,0x00,0x00,
-0x00,0x03,0x1c,0x02,0xa5,0x03,0x00,0x00,0x8f,0xbf,0x00,0x28,0x7b,0xb4,0x01,0x3c,
-0x7b,0xb2,0x00,0xfc,0x7b,0xb0,0x00,0xbc,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x30,
-0x8e,0xa2,0x00,0x04,0x00,0x00,0x00,0x00,0xad,0x22,0x00,0x00,0x8e,0xa4,0x00,0x08,
-0x00,0x00,0x00,0x00,0xa5,0x04,0x00,0x00,0x7a,0xa2,0x00,0x7c,0x00,0x00,0x00,0x00,
-0x00,0x03,0x1c,0x00,0x00,0x02,0x14,0x02,0x00,0x62,0x18,0x21,0xac,0xe3,0x00,0x00,
-0x8e,0xa2,0x00,0x0c,0x00,0x00,0x00,0x00,0x00,0x02,0x14,0x02,0x08,0x00,0x0c,0xb6,
-0xa4,0xa2,0x00,0x00,0x27,0xbd,0xff,0xe0,0xaf,0xb2,0x00,0x18,0xaf,0xb0,0x00,0x10,
-0xaf,0xbf,0x00,0x1c,0xaf,0xb1,0x00,0x14,0x84,0x82,0x00,0x0c,0x00,0x80,0x90,0x21,
-0x3c,0x05,0xb0,0x03,0x00,0x02,0x20,0xc0,0x00,0x82,0x20,0x21,0x00,0x04,0x20,0x80,
-0x27,0x82,0x90,0x04,0x00,0x82,0x10,0x21,0x8c,0x51,0x00,0x18,0x3c,0x02,0x80,0x00,
-0x34,0xa5,0x00,0x20,0x24,0x42,0x33,0x34,0x27,0x83,0x90,0x08,0xac,0xa2,0x00,0x00,
-0x00,0x83,0x20,0x21,0x3c,0x02,0xb0,0x03,0x90,0x86,0x00,0x07,0x34,0x42,0x01,0x00,
-0x8e,0x23,0x00,0x08,0x90,0x44,0x00,0x00,0x2c,0xc5,0x00,0x04,0x24,0x02,0x00,0x10,
-0x24,0x10,0x00,0x0a,0x00,0x45,0x80,0x0a,0x00,0x03,0x1e,0x43,0x30,0x84,0x00,0x01,
-0x30,0x65,0x00,0x01,0x14,0x80,0x00,0x02,0x00,0x10,0x10,0x40,0x02,0x00,0x10,0x21,
-0x00,0xc0,0x20,0x21,0x24,0x06,0x00,0x20,0x0c,0x00,0x08,0xe3,0x30,0x50,0x00,0xff,
-0x86,0x44,0x00,0x0c,0x27,0x85,0x90,0x10,0x3c,0x06,0xb0,0x09,0x00,0x04,0x18,0xc0,
-0x00,0x64,0x18,0x21,0x00,0x03,0x18,0x80,0x00,0x65,0x18,0x21,0x80,0x64,0x00,0x06,
-0x00,0x50,0x10,0x21,0x34,0xc6,0x01,0x02,0x24,0x85,0x00,0x03,0x28,0x83,0x00,0x00,
-0x00,0xa3,0x20,0x0b,0x00,0x04,0x20,0x83,0x00,0x04,0x20,0x80,0xa4,0xc2,0x00,0x00,
-0x02,0x24,0x20,0x21,0x8c,0x83,0x00,0x04,0x3c,0x02,0xb0,0x09,0x34,0x42,0x01,0x10,
-0xac,0x43,0x00,0x00,0x8c,0x86,0x00,0x08,0x3c,0x02,0xb0,0x09,0x34,0x42,0x01,0x14,
-0xa4,0x46,0x00,0x00,0x8c,0x85,0x00,0x0c,0x8c,0x82,0x00,0x08,0x3c,0x06,0xb0,0x09,
-0x00,0x05,0x2c,0x00,0x00,0x02,0x14,0x02,0x00,0xa2,0x28,0x21,0x34,0xc6,0x01,0x20,
-0xac,0xc5,0x00,0x00,0x8c,0x83,0x00,0x0c,0x3c,0x05,0xb0,0x09,0x34,0xa5,0x01,0x24,
-0x00,0x03,0x1c,0x02,0xa4,0xa3,0x00,0x00,0x92,0x42,0x00,0x0a,0x3c,0x03,0xb0,0x09,
-0x34,0x63,0x01,0x30,0x00,0x02,0x13,0x00,0x24,0x42,0x00,0x04,0x30,0x42,0xff,0xff,
-0xa4,0x62,0x00,0x00,0x86,0x44,0x00,0x0c,0x27,0x83,0x90,0x18,0x8f,0xbf,0x00,0x1c,
-0x00,0x04,0x10,0xc0,0x00,0x44,0x10,0x21,0x00,0x02,0x10,0x80,0x00,0x43,0x10,0x21,
-0x94,0x44,0x00,0x02,0x8f,0xb2,0x00,0x18,0x7b,0xb0,0x00,0xbc,0x3c,0x05,0xb0,0x09,
-0x34,0xa5,0x01,0x32,0xa4,0xa4,0x00,0x00,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x20,
-0x27,0xbd,0xff,0xe0,0x3c,0x02,0xb0,0x03,0x3c,0x03,0x80,0x00,0xaf,0xb0,0x00,0x10,
-0x34,0x42,0x00,0x20,0x00,0xa0,0x80,0x21,0x24,0x63,0x34,0xc0,0x00,0x05,0x2c,0x43,
-0xaf,0xb1,0x00,0x14,0xaf,0xbf,0x00,0x18,0xac,0x43,0x00,0x00,0x10,0xa0,0x00,0x05,
-0x00,0x80,0x88,0x21,0x8c,0x82,0x00,0x34,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0xb6,
-0x00,0x00,0x00,0x00,0x32,0x10,0x00,0xff,0x12,0x00,0x00,0x4c,0x00,0x00,0x10,0x21,
-0x24,0x02,0x00,0x08,0x12,0x02,0x00,0xa3,0x2a,0x02,0x00,0x09,0x10,0x40,0x00,0x89,
-0x24,0x02,0x00,0x40,0x24,0x04,0x00,0x02,0x12,0x04,0x00,0x79,0x2a,0x02,0x00,0x03,
-0x10,0x40,0x00,0x69,0x24,0x02,0x00,0x04,0x24,0x02,0x00,0x01,0x12,0x02,0x00,0x5a,
-0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x05,0x34,0x42,0x00,0x08,0x3c,0x03,0x80,0x00,
-0xa2,0x20,0x00,0x4e,0xac,0x43,0x00,0x00,0x82,0x24,0x00,0x11,0x92,0x27,0x00,0x11,
-0x10,0x80,0x00,0x4e,0x00,0x00,0x00,0x00,0x92,0x26,0x00,0x0a,0x24,0x02,0x00,0x12,
-0x10,0x46,0x00,0x09,0x30,0xc2,0x00,0xff,0x27,0x83,0xb4,0x00,0x00,0x02,0x10,0x80,
-0x00,0x43,0x10,0x21,0x8c,0x44,0x00,0x00,0x00,0x00,0x00,0x00,0x94,0x83,0x00,0x14,
-0x00,0x00,0x00,0x00,0xa6,0x23,0x00,0x0c,0x3c,0x02,0xb0,0x09,0x34,0x42,0x00,0x40,
-0x90,0x43,0x00,0x00,0x00,0x00,0x00,0x00,0x30,0x63,0x00,0x03,0xa2,0x23,0x00,0x10,
-0x14,0x60,0x00,0x2b,0x30,0x65,0x00,0x01,0x30,0xc2,0x00,0xff,0x27,0x83,0xb4,0x00,
-0x00,0x02,0x10,0x80,0x00,0x43,0x10,0x21,0x8c,0x44,0x00,0x00,0x82,0x23,0x00,0x12,
-0x90,0x82,0x00,0x16,0x00,0x00,0x00,0x00,0x00,0x02,0x11,0x42,0x30,0x42,0x00,0x01,
-0x00,0x62,0x18,0x21,0x00,0x03,0x26,0x00,0x14,0x80,0x00,0x18,0xa2,0x23,0x00,0x12,
-0x00,0x07,0x16,0x00,0x14,0x40,0x00,0x11,0x24,0x02,0x00,0x01,0x96,0x23,0x00,0x0c,
-0x27,0x84,0x90,0x10,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x80,
-0x00,0x44,0x10,0x21,0x80,0x45,0x00,0x06,0x00,0x03,0x1a,0x00,0x3c,0x02,0xb0,0x00,
-0x00,0x65,0x18,0x21,0x00,0x62,0x18,0x21,0x90,0x64,0x00,0x00,0x90,0x62,0x00,0x04,
-0xa2,0x20,0x00,0x15,0xa3,0x80,0x8b,0xd4,0x24,0x02,0x00,0x01,0x8f,0xbf,0x00,0x18,
-0x7b,0xb0,0x00,0xbc,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x20,0x0c,0x00,0x0c,0xcd,
-0x02,0x20,0x20,0x21,0x92,0x27,0x00,0x11,0x08,0x00,0x0d,0x7d,0x00,0x07,0x16,0x00,
-0x0c,0x00,0x0c,0x6e,0x02,0x20,0x20,0x21,0x86,0x23,0x00,0x0c,0x27,0x84,0x90,0x08,
-0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x80,0x00,0x44,0x20,0x21,
-0x90,0x85,0x00,0x07,0x27,0x83,0x90,0x10,0x00,0x43,0x10,0x21,0xa2,0x25,0x00,0x13,
-0x90,0x83,0x00,0x07,0x08,0x00,0x0d,0x95,0xa0,0x43,0x00,0x02,0x92,0x26,0x00,0x0a,
-0x08,0x00,0x0d,0x5e,0x30,0xc2,0x00,0xff,0x8e,0x22,0x00,0x24,0x00,0x00,0x00,0x00,
-0x10,0x50,0x00,0x07,0xa2,0x20,0x00,0x08,0x24,0x02,0x00,0x07,0xa2,0x22,0x00,0x0a,
-0x92,0x22,0x00,0x27,0xae,0x20,0x00,0x24,0x08,0x00,0x0d,0x51,0xa2,0x22,0x00,0x04,
-0x08,0x00,0x0d,0xaf,0x24,0x02,0x00,0x06,0x16,0x02,0xff,0x9b,0x3c,0x02,0xb0,0x05,
-0x8e,0x23,0x00,0x2c,0x24,0x02,0x00,0x01,0x10,0x62,0x00,0x07,0xa2,0x24,0x00,0x08,
-0x24,0x02,0x00,0x03,0xa2,0x22,0x00,0x0a,0x92,0x22,0x00,0x2f,0xae,0x20,0x00,0x2c,
-0x08,0x00,0x0d,0x51,0xa2,0x22,0x00,0x06,0x08,0x00,0x0d,0xbe,0xa2,0x20,0x00,0x0a,
-0x8e,0x22,0x00,0x28,0x24,0x03,0x00,0x01,0x24,0x04,0x00,0x01,0x10,0x44,0x00,0x07,
-0xa2,0x23,0x00,0x08,0x24,0x02,0x00,0x05,0xa2,0x22,0x00,0x0a,0x92,0x22,0x00,0x2b,
-0xae,0x20,0x00,0x28,0x08,0x00,0x0d,0x51,0xa2,0x22,0x00,0x05,0x08,0x00,0x0d,0xca,
-0x24,0x02,0x00,0x04,0x12,0x02,0x00,0x12,0x2a,0x02,0x00,0x41,0x10,0x40,0x00,0x09,
-0x24,0x02,0x00,0x80,0x24,0x02,0x00,0x20,0x16,0x02,0xff,0x7b,0x3c,0x02,0xb0,0x05,
-0x24,0x02,0x00,0x12,0xa2,0x22,0x00,0x0a,0xa2,0x22,0x00,0x08,0x08,0x00,0x0d,0x51,
-0xae,0x20,0x00,0x3c,0x16,0x02,0xff,0x74,0x3c,0x02,0xb0,0x05,0x24,0x02,0x00,0x10,
-0xa2,0x22,0x00,0x0a,0xa2,0x22,0x00,0x08,0x08,0x00,0x0d,0x51,0xae,0x20,0x00,0x34,
-0x24,0x02,0x00,0x11,0xa2,0x22,0x00,0x0a,0xa2,0x22,0x00,0x08,0x08,0x00,0x0d,0x51,
-0xae,0x20,0x00,0x38,0x8e,0x24,0x00,0x30,0x24,0x02,0x00,0x03,0x24,0x03,0x00,0x01,
-0x10,0x83,0x00,0x07,0xa2,0x22,0x00,0x08,0x24,0x02,0x00,0x02,0xa2,0x22,0x00,0x0a,
-0x92,0x22,0x00,0x33,0xae,0x20,0x00,0x30,0x08,0x00,0x0d,0x51,0xa2,0x22,0x00,0x07,
-0x08,0x00,0x0d,0xf0,0xa2,0x24,0x00,0x0a,0x8f,0x84,0xb4,0x40,0xae,0x20,0x00,0x34,
-0x94,0x85,0x00,0x14,0x0c,0x00,0x1b,0x66,0x32,0x10,0x00,0xff,0x08,0x00,0x0d,0x42,
-0x00,0x00,0x00,0x00,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,0x24,0x42,0x37,0xf4,
-0x34,0x63,0x00,0x20,0xac,0x62,0x00,0x00,0x80,0xa2,0x00,0x15,0x3c,0x06,0xb0,0x05,
-0x10,0x40,0x00,0x0a,0x34,0xc6,0x02,0x54,0x83,0x83,0x8b,0xd4,0x00,0x00,0x00,0x00,
-0xac,0x83,0x00,0x24,0x8c,0xc2,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x02,0x17,0x42,
-0x30,0x42,0x00,0x01,0x03,0xe0,0x00,0x08,0xac,0x82,0x00,0x28,0x8c,0x82,0x00,0x2c,
-0x3c,0x06,0xb0,0x05,0x34,0xc6,0x04,0x50,0x00,0x02,0x18,0x43,0x30,0x63,0x00,0x01,
-0x10,0x40,0x00,0x04,0x30,0x45,0x00,0x01,0xac,0x83,0x00,0x28,0x03,0xe0,0x00,0x08,
-0xac,0x85,0x00,0x24,0x90,0xc2,0x00,0x00,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0xff,
-0x30,0x43,0x00,0x02,0x30,0x42,0x00,0x01,0xac,0x83,0x00,0x28,0x03,0xe0,0x00,0x08,
-0xac,0x82,0x00,0x24,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,0x27,0xbd,0xff,0xd8,
-0x34,0x63,0x00,0x20,0x24,0x42,0x38,0x84,0xac,0x62,0x00,0x00,0xaf,0xb1,0x00,0x1c,
-0xaf,0xbf,0x00,0x20,0xaf,0xb0,0x00,0x18,0x90,0xa6,0x00,0x0a,0x27,0x83,0xb4,0x00,
-0x00,0xa0,0x88,0x21,0x00,0x06,0x10,0x80,0x00,0x43,0x10,0x21,0x8c,0x50,0x00,0x00,
-0x80,0xa5,0x00,0x11,0x92,0x03,0x00,0x12,0x10,0xa0,0x00,0x04,0xa2,0x20,0x00,0x15,
-0x24,0x02,0x00,0x12,0x10,0xc2,0x00,0xda,0x00,0x00,0x00,0x00,0x82,0x22,0x00,0x12,
-0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x67,0x00,0x00,0x00,0x00,0xa2,0x20,0x00,0x12,
-0xa2,0x00,0x00,0x19,0x86,0x23,0x00,0x0c,0x00,0x00,0x00,0x00,0x00,0x03,0x10,0xc0,
-0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x80,0x27,0x83,0x90,0x20,0x00,0x43,0x10,0x21,
-0xa0,0x40,0x00,0x00,0x92,0x03,0x00,0x16,0x00,0x00,0x00,0x00,0x30,0x63,0x00,0xdf,
-0xa2,0x03,0x00,0x16,0x82,0x02,0x00,0x12,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x20,
-0x00,0x00,0x00,0x00,0x92,0x23,0x00,0x08,0x00,0x00,0x00,0x00,0x14,0x60,0x00,0x45,
-0x24,0x02,0x00,0x01,0xa2,0x20,0x00,0x04,0x92,0x08,0x00,0x04,0x00,0x00,0x00,0x00,
-0x15,0x00,0x00,0x1e,0x24,0x02,0x00,0x01,0x92,0x07,0x00,0x0a,0xa2,0x02,0x00,0x17,
-0x92,0x02,0x00,0x16,0x30,0xe3,0x00,0xff,0x30,0x42,0x00,0xe4,0x10,0x60,0x00,0x03,
-0xa2,0x02,0x00,0x16,0x34,0x42,0x00,0x01,0xa2,0x02,0x00,0x16,0x11,0x00,0x00,0x05,
-0x00,0x00,0x00,0x00,0x92,0x02,0x00,0x16,0x00,0x00,0x00,0x00,0x34,0x42,0x00,0x02,
-0xa2,0x02,0x00,0x16,0x92,0x02,0x00,0x17,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x08,
-0x00,0x00,0x00,0x00,0x96,0x02,0x00,0x06,0x00,0x00,0x00,0x00,0xa6,0x02,0x00,0x14,
-0x8f,0xbf,0x00,0x20,0x7b,0xb0,0x00,0xfc,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x28,
-0x96,0x02,0x00,0x00,0x08,0x00,0x0e,0x6c,0xa6,0x02,0x00,0x14,0x92,0x07,0x00,0x0a,
-0x00,0x00,0x00,0x00,0x14,0xe0,0x00,0x03,0x00,0x00,0x00,0x00,0x08,0x00,0x0e,0x58,
-0xa2,0x00,0x00,0x17,0x96,0x04,0x00,0x00,0x96,0x05,0x00,0x06,0x27,0x86,0x90,0x00,
-0x00,0x04,0x18,0xc0,0x00,0x64,0x18,0x21,0x00,0x05,0x10,0xc0,0x00,0x45,0x10,0x21,
-0x00,0x03,0x18,0x80,0x00,0x66,0x18,0x21,0x00,0x02,0x10,0x80,0x00,0x46,0x10,0x21,
-0x8c,0x66,0x00,0x08,0x8c,0x45,0x00,0x08,0x3c,0x03,0x80,0x00,0x00,0xc3,0x20,0x24,
-0x10,0x80,0x00,0x08,0x00,0xa3,0x10,0x24,0x10,0x40,0x00,0x04,0x00,0x00,0x18,0x21,
-0x10,0x80,0x00,0x02,0x24,0x03,0x00,0x01,0x00,0xa6,0x18,0x2b,0x08,0x00,0x0e,0x58,
-0xa2,0x03,0x00,0x17,0x10,0x40,0xff,0xfd,0x00,0xa6,0x18,0x2b,0x08,0x00,0x0e,0x8c,
-0x00,0x00,0x00,0x00,0x10,0x62,0x00,0x09,0x24,0x02,0x00,0x02,0x10,0x62,0x00,0x05,
-0x24,0x02,0x00,0x03,0x14,0x62,0xff,0xb8,0x00,0x00,0x00,0x00,0x08,0x00,0x0e,0x52,
-0xa2,0x20,0x00,0x07,0x08,0x00,0x0e,0x52,0xa2,0x20,0x00,0x06,0x08,0x00,0x0e,0x52,
-0xa2,0x20,0x00,0x05,0x82,0x22,0x00,0x10,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x69,
-0x2c,0x62,0x00,0x02,0x10,0x40,0x00,0x49,0x3c,0x02,0xb0,0x09,0x92,0x25,0x00,0x08,
-0x00,0x00,0x00,0x00,0x30,0xa6,0x00,0xff,0x2c,0xc2,0x00,0x04,0x10,0x40,0x00,0x3b,
-0x2c,0xc2,0x00,0x10,0x3c,0x04,0xb0,0x05,0x34,0x84,0x02,0x29,0x90,0x83,0x00,0x00,
-0x24,0x02,0x00,0x01,0x00,0xc2,0x10,0x04,0x00,0x02,0x10,0x27,0x00,0x62,0x18,0x24,
-0xa0,0x83,0x00,0x00,0x86,0x23,0x00,0x0c,0x96,0x26,0x00,0x0c,0x00,0x03,0x10,0xc0,
-0x00,0x43,0x10,0x21,0x00,0x02,0x28,0x80,0x27,0x83,0x90,0x04,0x00,0xa3,0x18,0x21,
-0x8c,0x64,0x00,0x18,0x00,0x00,0x00,0x00,0x8c,0x82,0x00,0x04,0x00,0x00,0x00,0x00,
-0x30,0x42,0x00,0x10,0x10,0x40,0x00,0x18,0x24,0x07,0x00,0x01,0x93,0x82,0x8b,0x71,
-0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x01,0x14,0x40,0x00,0x0a,0x24,0x05,0x00,0x24,
-0x00,0x06,0x2c,0x00,0x00,0x05,0x2c,0x03,0x0c,0x00,0x1b,0x66,0x02,0x00,0x20,0x21,
-0x92,0x02,0x00,0x16,0xa2,0x00,0x00,0x12,0x30,0x42,0x00,0xe7,0x08,0x00,0x0e,0x49,
-0xa2,0x02,0x00,0x16,0xf0,0xc5,0x00,0x06,0x00,0x00,0x28,0x12,0x27,0x82,0x90,0x00,
-0x00,0xa2,0x28,0x21,0x0c,0x00,0x01,0x49,0x3c,0x04,0x00,0x80,0x96,0x26,0x00,0x0c,
-0x08,0x00,0x0e,0xc9,0x00,0x06,0x2c,0x00,0x27,0x83,0x90,0x10,0x27,0x82,0x90,0x18,
-0x00,0xa2,0x10,0x21,0x00,0xa3,0x18,0x21,0x90,0x44,0x00,0x00,0x90,0x65,0x00,0x05,
-0x93,0x82,0x80,0x10,0x00,0x00,0x30,0x21,0x0c,0x00,0x21,0x9a,0xaf,0xa2,0x00,0x10,
-0x96,0x26,0x00,0x0c,0x08,0x00,0x0e,0xc3,0x00,0x00,0x00,0x00,0x14,0x40,0xff,0xcd,
-0x3c,0x04,0xb0,0x05,0x34,0x84,0x02,0x29,0x90,0x83,0x00,0x00,0x30,0xa5,0x00,0x0f,
-0x24,0x02,0x00,0x80,0x08,0x00,0x0e,0xb2,0x00,0xa2,0x10,0x07,0x86,0x26,0x00,0x0c,
-0x3c,0x03,0xb0,0x09,0x34,0x42,0x01,0x72,0x34,0x63,0x01,0x78,0x94,0x47,0x00,0x00,
-0x8c,0x65,0x00,0x00,0x00,0x06,0x10,0xc0,0x00,0x46,0x10,0x21,0x3c,0x04,0xb0,0x09,
-0xae,0x25,0x00,0x1c,0x34,0x84,0x01,0x7c,0x27,0x83,0x90,0x04,0x00,0x02,0x10,0x80,
-0x8c,0x85,0x00,0x00,0x00,0x43,0x10,0x21,0x8c,0x43,0x00,0x18,0xae,0x25,0x00,0x20,
-0xa6,0x27,0x00,0x18,0x8c,0x66,0x00,0x08,0x02,0x20,0x20,0x21,0x0c,0x00,0x0f,0x19,
-0x00,0x00,0x28,0x21,0x86,0x25,0x00,0x18,0x8e,0x26,0x00,0x1c,0x8e,0x27,0x00,0x20,
-0x02,0x20,0x20,0x21,0x0c,0x00,0x1c,0x68,0xaf,0xa2,0x00,0x10,0x08,0x00,0x0e,0x49,
-0xa2,0x02,0x00,0x12,0x92,0x22,0x00,0x08,0x08,0x00,0x0e,0x49,0xa2,0x22,0x00,0x09,
-0xa2,0x20,0x00,0x11,0x80,0x82,0x00,0x50,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x03,
-0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0xd0,0xac,0x40,0x00,0x00,0x08,0x00,0x0e,0x49,
-0xa0,0x80,0x00,0x50,0x94,0x8a,0x00,0x0c,0x24,0x03,0x00,0x24,0x00,0x80,0x70,0x21,
-0x3c,0x02,0x80,0x00,0x3c,0x04,0xb0,0x03,0x24,0x42,0x3c,0x64,0xf1,0x43,0x00,0x06,
-0x34,0x84,0x00,0x20,0x00,0x00,0x18,0x12,0x00,0xa0,0x68,0x21,0xac,0x82,0x00,0x00,
-0x27,0x85,0x90,0x10,0x27,0x82,0x90,0x0f,0x27,0xbd,0xff,0xf8,0x00,0x62,0x60,0x21,
-0x00,0x65,0x58,0x21,0x00,0x00,0xc0,0x21,0x11,0xa0,0x00,0xcc,0x00,0x00,0x78,0x21,
-0x00,0x0a,0x1c,0x00,0x00,0x03,0x1c,0x03,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,
-0x00,0x02,0x10,0x80,0x00,0x45,0x10,0x21,0x91,0x87,0x00,0x00,0x80,0x48,0x00,0x04,
-0x03,0xa0,0x60,0x21,0x00,0x0a,0x1c,0x00,0x00,0x03,0x1c,0x03,0x00,0x03,0x10,0xc0,
-0x00,0x43,0x10,0x21,0x00,0x02,0x48,0x80,0x27,0x83,0x90,0x04,0xa3,0xa7,0x00,0x00,
-0x01,0x23,0x18,0x21,0x8c,0x64,0x00,0x18,0x25,0x02,0xff,0xff,0x00,0x48,0x40,0x0b,
-0x8c,0x83,0x00,0x04,0x2d,0x05,0x00,0x07,0x24,0x02,0x00,0x06,0x30,0x63,0x00,0x08,
-0x14,0x60,0x00,0x35,0x00,0x45,0x40,0x0a,0x93,0xa7,0x00,0x00,0x27,0x82,0x90,0x18,
-0x01,0x22,0x10,0x21,0x30,0xe3,0x00,0xf0,0x38,0x63,0x00,0x50,0x30,0xe5,0x00,0xff,
-0x00,0x05,0x20,0x2b,0x00,0x03,0x18,0x2b,0x00,0x64,0x18,0x24,0x90,0x49,0x00,0x00,
-0x10,0x60,0x00,0x16,0x30,0xe4,0x00,0x0f,0x24,0x02,0x00,0x04,0x10,0xa2,0x00,0x9d,
-0x00,0x00,0x00,0x00,0x11,0xa0,0x00,0x3a,0x2c,0xa2,0x00,0x0c,0x10,0x40,0x00,0x02,
-0x24,0x84,0x00,0x0c,0x00,0xe0,0x20,0x21,0x30,0x84,0x00,0xff,0x00,0x04,0x10,0x40,
-0x27,0x83,0xbb,0x1c,0x00,0x44,0x10,0x21,0x00,0x43,0x10,0x21,0x90,0x47,0x00,0x00,
-0x00,0x00,0x00,0x00,0x2c,0xe3,0x00,0x0c,0xa3,0xa7,0x00,0x00,0x10,0x60,0x00,0x02,
-0x24,0xe2,0x00,0x04,0x00,0xe0,0x10,0x21,0xa3,0xa2,0x00,0x00,0x91,0x65,0x00,0x00,
-0x91,0x82,0x00,0x00,0x30,0xa3,0x00,0xff,0x00,0x62,0x10,0x2b,0x10,0x40,0x00,0x0e,
-0x2c,0x62,0x00,0x0c,0x14,0x40,0x00,0x03,0x00,0x60,0x20,0x21,0x30,0xa2,0x00,0x0f,
-0x24,0x44,0x00,0x0c,0x00,0x04,0x10,0x40,0x00,0x44,0x20,0x21,0x27,0x83,0xbb,0x1c,
-0x00,0x83,0x18,0x21,0x90,0x62,0x00,0x02,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x05,
-0x00,0x09,0x11,0x00,0xa1,0x85,0x00,0x00,0x93,0xa2,0x00,0x00,0x03,0xe0,0x00,0x08,
-0x27,0xbd,0x00,0x08,0x00,0x49,0x10,0x23,0x00,0x02,0x10,0x80,0x00,0x49,0x10,0x23,
-0x00,0x02,0x10,0x80,0x00,0x44,0x10,0x21,0x27,0x83,0xb4,0xa8,0x00,0x43,0x10,0x21,
-0x90,0x44,0x00,0x00,0x00,0x00,0x00,0x00,0x2c,0x83,0x00,0x0c,0x14,0x60,0x00,0x06,
-0x00,0x80,0x10,0x21,0x00,0x18,0x10,0x40,0x00,0x4f,0x10,0x21,0x00,0x02,0x11,0x00,
-0x00,0x82,0x10,0x21,0x24,0x42,0x00,0x04,0x08,0x00,0x0f,0x7a,0xa1,0x82,0x00,0x00,
-0x8f,0x8d,0x81,0x5c,0x00,0x00,0x00,0x00,0x01,0xa8,0x10,0x21,0x90,0x43,0x00,0x00,
-0x00,0x00,0x00,0x00,0x10,0x60,0xff,0xd1,0x00,0x00,0x28,0x21,0x00,0x06,0x74,0x82,
-0x30,0xe2,0x00,0xff,0x2c,0x42,0x00,0x0c,0x14,0x40,0x00,0x03,0x00,0xe0,0x10,0x21,
-0x30,0xe2,0x00,0x0f,0x24,0x42,0x00,0x0c,0x30,0x44,0x00,0xff,0xa3,0xa2,0x00,0x00,
-0x24,0x02,0x00,0x0c,0x10,0x82,0x00,0x0d,0x00,0x09,0x11,0x00,0x00,0x49,0x10,0x23,
-0x00,0x02,0x10,0x80,0x00,0x04,0x18,0x40,0x00,0x49,0x10,0x23,0x00,0x64,0x18,0x21,
-0x00,0x02,0x10,0x80,0x00,0x43,0x10,0x21,0x27,0x84,0xb4,0xa8,0x00,0x44,0x10,0x21,
-0x90,0x47,0x00,0x00,0x00,0x00,0x00,0x00,0xa3,0xa7,0x00,0x00,0x00,0x0a,0x1c,0x00,
-0x00,0x03,0x1c,0x03,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x80,
-0x27,0x83,0x90,0x04,0x00,0x43,0x10,0x21,0x8c,0x44,0x00,0x18,0x00,0x00,0x00,0x00,
-0x8c,0x83,0x00,0x04,0x00,0x00,0x00,0x00,0x30,0x63,0x00,0x10,0x14,0x60,0x00,0x33,
-0x00,0x06,0x14,0x42,0x00,0x09,0x11,0x00,0x00,0x49,0x10,0x23,0x00,0x02,0x10,0x80,
-0x00,0x49,0x10,0x23,0x27,0x83,0xb5,0x78,0x00,0x02,0x10,0x80,0x00,0x43,0x10,0x21,
-0x90,0x44,0x00,0x04,0x90,0x43,0x00,0x05,0x00,0x00,0x00,0x00,0x00,0x64,0xc0,0x24,
-0x93,0xa7,0x00,0x00,0x00,0x00,0x00,0x00,0x2c,0xe2,0x00,0x0f,0x10,0x40,0x00,0x0f,
-0x31,0xcf,0x00,0x01,0x00,0x0a,0x1c,0x00,0x00,0x03,0x1c,0x03,0x00,0x03,0x10,0xc0,
-0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x80,0x27,0x84,0x90,0x00,0x00,0x44,0x10,0x21,
-0x84,0x43,0x00,0x06,0x00,0x00,0x00,0x00,0x28,0x63,0x06,0x41,0x14,0x60,0x00,0x04,
-0x30,0xe2,0x00,0xff,0x24,0x07,0x00,0x0f,0xa3,0xa7,0x00,0x00,0x30,0xe2,0x00,0xff,
-0x2c,0x42,0x00,0x0c,0x14,0x40,0x00,0x06,0x00,0xe0,0x10,0x21,0x00,0x18,0x10,0x40,
-0x00,0x4f,0x10,0x21,0x00,0x02,0x11,0x00,0x00,0x47,0x10,0x21,0x24,0x42,0x00,0x04,
-0xa3,0xa2,0x00,0x00,0x00,0x40,0x38,0x21,0x01,0xa8,0x10,0x21,0x90,0x43,0x00,0x00,
-0x24,0xa4,0x00,0x01,0x30,0x85,0xff,0xff,0x00,0xa3,0x18,0x2b,0x14,0x60,0xff,0xad,
-0x30,0xe2,0x00,0xff,0x08,0x00,0x0f,0x67,0x00,0x00,0x00,0x00,0x08,0x00,0x0f,0xc8,
-0x30,0x58,0x00,0x01,0x81,0xc2,0x00,0x48,0x00,0x00,0x00,0x00,0x10,0x40,0xff,0x73,
-0x00,0x00,0x00,0x00,0x08,0x00,0x0f,0x55,0x00,0x00,0x00,0x00,0x00,0x0a,0x1c,0x00,
-0x00,0x03,0x1c,0x03,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x80,
-0x00,0x45,0x10,0x21,0x80,0x48,0x00,0x05,0x91,0x67,0x00,0x00,0x08,0x00,0x0f,0x35,
-0x03,0xa0,0x58,0x21,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,0x34,0x63,0x00,0x20,
-0x24,0x42,0x40,0x04,0x03,0xe0,0x00,0x08,0xac,0x62,0x00,0x00,0x27,0xbd,0xff,0xc0,
-0xaf,0xb7,0x00,0x34,0xaf,0xb6,0x00,0x30,0xaf,0xb5,0x00,0x2c,0xaf,0xb4,0x00,0x28,
-0xaf,0xb3,0x00,0x24,0xaf,0xb2,0x00,0x20,0xaf,0xbf,0x00,0x3c,0xaf,0xbe,0x00,0x38,
-0xaf,0xb1,0x00,0x1c,0xaf,0xb0,0x00,0x18,0x84,0x82,0x00,0x0c,0x27,0x93,0x90,0x04,
-0x3c,0x05,0xb0,0x03,0x00,0x02,0x18,0xc0,0x00,0x62,0x18,0x21,0x00,0x03,0x18,0x80,
-0x00,0x73,0x10,0x21,0x8c,0x5e,0x00,0x18,0x3c,0x02,0x80,0x00,0x34,0xa5,0x00,0x20,
-0x24,0x42,0x40,0x1c,0xac,0xa2,0x00,0x00,0x8f,0xd0,0x00,0x08,0x27,0x95,0x90,0x10,
-0x00,0x75,0x18,0x21,0x00,0x00,0x28,0x21,0x02,0x00,0x30,0x21,0x90,0x71,0x00,0x00,
-0x0c,0x00,0x0f,0x19,0x00,0x80,0xb0,0x21,0x00,0x40,0x90,0x21,0x00,0x10,0x14,0x42,
-0x30,0x54,0x00,0x01,0x02,0x40,0x20,0x21,0x00,0x10,0x14,0x82,0x02,0x80,0x28,0x21,
-0x12,0x51,0x00,0x23,0x00,0x10,0xbf,0xc2,0x86,0xc3,0x00,0x0c,0x30,0x50,0x00,0x01,
-0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x80,0x00,0x55,0x10,0x21,
-0xa0,0x52,0x00,0x00,0x86,0xc3,0x00,0x0c,0x00,0x00,0x00,0x00,0x00,0x03,0x10,0xc0,
-0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x80,0x00,0x53,0x30,0x21,0x8c,0xc7,0x00,0x18,
-0x27,0x83,0x90,0x00,0x00,0x43,0x10,0x21,0x8c,0xe3,0x00,0x04,0x84,0x46,0x00,0x06,
-0x00,0x03,0x19,0x42,0x0c,0x00,0x08,0xe3,0x30,0x73,0x00,0x01,0x00,0x40,0x88,0x21,
-0x02,0x40,0x20,0x21,0x02,0x80,0x28,0x21,0x16,0xe0,0x00,0x10,0x02,0x00,0x30,0x21,
-0x86,0xc2,0x00,0x0c,0x00,0x00,0x00,0x00,0x00,0x02,0x18,0xc0,0x00,0x62,0x18,0x21,
-0x00,0x03,0x18,0x80,0x27,0x82,0x90,0x08,0x00,0x62,0x18,0x21,0xa4,0x71,0x00,0x04,
-0x7b,0xbe,0x01,0xfc,0x7b,0xb6,0x01,0xbc,0x7b,0xb4,0x01,0x7c,0x7b,0xb2,0x01,0x3c,
-0x7b,0xb0,0x00,0xfc,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x40,0x86,0xc3,0x00,0x0c,
-0xaf,0xb3,0x00,0x10,0xaf,0xa0,0x00,0x14,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,
-0x00,0x02,0x10,0x80,0x00,0x55,0x10,0x21,0x80,0x47,0x00,0x06,0x00,0x00,0x00,0x00,
-0x24,0xe7,0x00,0x02,0x00,0x07,0x17,0xc2,0x00,0xe2,0x38,0x21,0x00,0x07,0x38,0x43,
-0x00,0x07,0x38,0x40,0x0c,0x00,0x09,0x0a,0x03,0xc7,0x38,0x21,0x08,0x00,0x10,0x48,
-0x02,0x22,0x88,0x21,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,0x27,0xbd,0xff,0xd0,
-0x34,0x63,0x00,0x20,0x24,0x42,0x41,0xa4,0xaf,0xb2,0x00,0x20,0xac,0x62,0x00,0x00,
-0xaf,0xbf,0x00,0x28,0xaf,0xb3,0x00,0x24,0xaf,0xb1,0x00,0x1c,0xaf,0xb0,0x00,0x18,
-0x3c,0x02,0xb0,0x03,0x90,0x83,0x00,0x0a,0x34,0x42,0x01,0x04,0x94,0x45,0x00,0x00,
-0x00,0x03,0x18,0x80,0x27,0x82,0xb4,0x00,0x00,0x62,0x18,0x21,0x30,0xa6,0xff,0xff,
-0x8c,0x71,0x00,0x00,0x80,0x85,0x00,0x12,0x30,0xc9,0x00,0xff,0x00,0x06,0x32,0x02,
-0xa4,0x86,0x00,0x44,0xa4,0x89,0x00,0x46,0x82,0x22,0x00,0x12,0x00,0x80,0x90,0x21,
-0x10,0xa0,0x00,0x1b,0xa0,0x80,0x00,0x15,0x00,0xc5,0x10,0x2a,0x10,0x40,0x00,0x14,
-0x00,0x00,0x00,0x00,0xa2,0x20,0x00,0x19,0x84,0x83,0x00,0x0c,0x00,0x00,0x00,0x00,
-0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x80,0x27,0x83,0x90,0x20,
-0x00,0x43,0x10,0x21,0xa0,0x40,0x00,0x00,0xa0,0x80,0x00,0x12,0x92,0x22,0x00,0x16,
-0x00,0x00,0x00,0x00,0x30,0x42,0x00,0xdf,0xa2,0x22,0x00,0x16,0x8f,0xbf,0x00,0x28,
-0x7b,0xb2,0x01,0x3c,0x7b,0xb0,0x00,0xfc,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x30,
-0x0c,0x00,0x10,0x01,0x00,0x00,0x00,0x00,0x08,0x00,0x10,0x97,0x00,0x00,0x00,0x00,
-0x28,0x42,0x00,0x02,0x10,0x40,0x01,0x76,0x00,0x00,0x28,0x21,0x94,0x87,0x00,0x0c,
-0x00,0x00,0x00,0x00,0x00,0xe0,0x10,0x21,0x00,0x02,0x14,0x00,0x00,0x02,0x14,0x03,
-0x00,0x07,0x24,0x00,0x00,0x04,0x24,0x03,0x00,0x02,0x18,0xc0,0x00,0x62,0x18,0x21,
-0x00,0x04,0x28,0xc0,0x00,0xa4,0x28,0x21,0x27,0x82,0x90,0x20,0x00,0x03,0x18,0x80,
-0x00,0x62,0x18,0x21,0x00,0x05,0x28,0x80,0x27,0x82,0x90,0x08,0x00,0xa2,0x10,0x21,
-0x8c,0x68,0x00,0x00,0x80,0x44,0x00,0x06,0x27,0x82,0x90,0x10,0x00,0x08,0x1d,0x02,
-0x00,0xa2,0x28,0x21,0x38,0x84,0x00,0x00,0x30,0x63,0x00,0x01,0x01,0x24,0x30,0x0b,
-0x80,0xaa,0x00,0x04,0x80,0xa9,0x00,0x05,0x10,0x60,0x00,0x02,0x00,0x08,0x14,0x02,
-0x30,0x46,0x00,0x0f,0x15,0x20,0x00,0x28,0x01,0x49,0x10,0x21,0x15,0x40,0x00,0x11,
-0x30,0xe3,0xff,0xff,0x92,0x45,0x00,0x08,0x00,0x00,0x00,0x00,0x30,0xa8,0x00,0xff,
-0x2d,0x02,0x00,0x04,0x10,0x40,0x01,0x46,0x2d,0x02,0x00,0x10,0x3c,0x04,0xb0,0x05,
-0x34,0x84,0x02,0x29,0x90,0x83,0x00,0x00,0x24,0x02,0x00,0x01,0x01,0x02,0x10,0x04,
-0x00,0x62,0x18,0x25,0xa0,0x83,0x00,0x00,0x96,0x47,0x00,0x0c,0x00,0x00,0x00,0x00,
-0x30,0xe3,0xff,0xff,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,0x27,0x84,0x90,0x10,
-0x00,0x02,0x10,0x80,0x00,0x44,0x10,0x21,0x80,0x45,0x00,0x06,0x00,0x03,0x1a,0x00,
-0x3c,0x04,0xb0,0x00,0x00,0x65,0x18,0x21,0x00,0x64,0x20,0x21,0x94,0x82,0x00,0x00,
-0x82,0x43,0x00,0x10,0x00,0x02,0x14,0x00,0x14,0x60,0x00,0x06,0x00,0x02,0x3c,0x03,
-0x30,0xe2,0x00,0x04,0x14,0x40,0x00,0x04,0x01,0x49,0x10,0x21,0x34,0xe2,0x08,0x00,
-0xa4,0x82,0x00,0x00,0x01,0x49,0x10,0x21,0x00,0x02,0x16,0x00,0x00,0x02,0x16,0x03,
-0x00,0x46,0x10,0x2a,0x10,0x40,0x00,0x7c,0x00,0x00,0x00,0x00,0x82,0x42,0x00,0x10,
-0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x0e,0x00,0x00,0x00,0x00,0x86,0x43,0x00,0x0c,
-0x25,0x44,0x00,0x01,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x80,
-0x27,0x83,0x90,0x10,0x00,0x43,0x10,0x21,0xa0,0x44,0x00,0x04,0x92,0x23,0x00,0x16,
-0x02,0x40,0x20,0x21,0x30,0x63,0x00,0xfb,0x08,0x00,0x10,0x9c,0xa2,0x23,0x00,0x16,
-0x86,0x43,0x00,0x0c,0x25,0x24,0x00,0x01,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,
-0x00,0x02,0x10,0x80,0x27,0x83,0x90,0x10,0x00,0x43,0x10,0x21,0xa0,0x44,0x00,0x05,
-0x86,0x45,0x00,0x0c,0x0c,0x00,0x1e,0xea,0x02,0x20,0x20,0x21,0x10,0x40,0x00,0x5a,
-0x00,0x00,0x00,0x00,0x92,0x45,0x00,0x08,0x00,0x00,0x00,0x00,0x30,0xa6,0x00,0xff,
-0x2c,0xc2,0x00,0x04,0x10,0x40,0x00,0x4c,0x2c,0xc2,0x00,0x10,0x3c,0x04,0xb0,0x05,
-0x34,0x84,0x02,0x29,0x90,0x83,0x00,0x00,0x24,0x02,0x00,0x01,0x00,0xc2,0x10,0x04,
-0x00,0x02,0x10,0x27,0x00,0x62,0x18,0x24,0xa0,0x83,0x00,0x00,0x92,0x45,0x00,0x08,
-0x00,0x00,0x00,0x00,0x30,0xa5,0x00,0xff,0x14,0xa0,0x00,0x33,0x24,0x02,0x00,0x01,
-0xa2,0x40,0x00,0x04,0x92,0x22,0x00,0x04,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x0c,
-0x24,0x02,0x00,0x01,0xa2,0x22,0x00,0x17,0x92,0x22,0x00,0x17,0x00,0x00,0x00,0x00,
-0x10,0x40,0x00,0x04,0x00,0x00,0x00,0x00,0x96,0x22,0x00,0x06,0x08,0x00,0x10,0x97,
-0xa6,0x22,0x00,0x14,0x96,0x22,0x00,0x00,0x08,0x00,0x10,0x97,0xa6,0x22,0x00,0x14,
-0x92,0x22,0x00,0x0a,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x03,0x00,0x00,0x00,0x00,
-0x08,0x00,0x11,0x26,0xa2,0x20,0x00,0x17,0x96,0x24,0x00,0x00,0x96,0x25,0x00,0x06,
-0x27,0x86,0x90,0x00,0x00,0x04,0x18,0xc0,0x00,0x64,0x18,0x21,0x00,0x05,0x10,0xc0,
-0x00,0x45,0x10,0x21,0x00,0x03,0x18,0x80,0x00,0x66,0x18,0x21,0x00,0x02,0x10,0x80,
-0x00,0x46,0x10,0x21,0x8c,0x65,0x00,0x08,0x8c,0x44,0x00,0x08,0x3c,0x03,0x80,0x00,
-0x00,0xa3,0x30,0x24,0x10,0xc0,0x00,0x08,0x00,0x83,0x10,0x24,0x10,0x40,0x00,0x04,
-0x00,0x00,0x18,0x21,0x10,0xc0,0x00,0x02,0x24,0x03,0x00,0x01,0x00,0x85,0x18,0x2b,
-0x08,0x00,0x11,0x26,0xa2,0x23,0x00,0x17,0x10,0x40,0xff,0xfd,0x00,0x85,0x18,0x2b,
-0x08,0x00,0x11,0x49,0x00,0x00,0x00,0x00,0x10,0xa2,0x00,0x09,0x24,0x02,0x00,0x02,
-0x10,0xa2,0x00,0x05,0x24,0x02,0x00,0x03,0x14,0xa2,0xff,0xca,0x00,0x00,0x00,0x00,
-0x08,0x00,0x11,0x21,0xa2,0x40,0x00,0x07,0x08,0x00,0x11,0x21,0xa2,0x40,0x00,0x06,
-0x08,0x00,0x11,0x21,0xa2,0x40,0x00,0x05,0x14,0x40,0xff,0xbe,0x3c,0x04,0xb0,0x05,
-0x34,0x84,0x02,0x29,0x90,0x83,0x00,0x00,0x30,0xa5,0x00,0x0f,0x24,0x02,0x00,0x80,
-0x08,0x00,0x11,0x18,0x00,0xa2,0x10,0x07,0x0c,0x00,0x10,0x07,0x02,0x40,0x20,0x21,
-0x08,0x00,0x10,0x97,0x00,0x00,0x00,0x00,0x92,0x45,0x00,0x08,0x00,0x00,0x00,0x00,
-0x30,0xa6,0x00,0xff,0x2c,0xc2,0x00,0x04,0x10,0x40,0x00,0x99,0x2c,0xc2,0x00,0x10,
-0x3c,0x04,0xb0,0x05,0x34,0x84,0x02,0x29,0x90,0x83,0x00,0x00,0x24,0x02,0x00,0x01,
-0x00,0xc2,0x10,0x04,0x00,0x02,0x10,0x27,0x00,0x62,0x18,0x24,0xa0,0x83,0x00,0x00,
-0x92,0x45,0x00,0x08,0x00,0x00,0x00,0x00,0x30,0xa5,0x00,0xff,0x14,0xa0,0x00,0x80,
-0x24,0x02,0x00,0x01,0xa2,0x40,0x00,0x04,0x86,0x43,0x00,0x0c,0x27,0x93,0x90,0x04,
-0x96,0x47,0x00,0x0c,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,0x00,0x02,0x28,0x80,
-0x00,0xb3,0x18,0x21,0x8c,0x64,0x00,0x18,0x00,0x00,0x00,0x00,0x8c,0x82,0x00,0x04,
-0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x10,0x10,0x40,0x00,0x64,0x00,0x00,0x30,0x21,
-0x00,0x07,0x1c,0x00,0x00,0x03,0x1c,0x03,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,
-0x00,0x02,0x10,0x80,0x00,0x53,0x10,0x21,0x8c,0x43,0x00,0x18,0x93,0x82,0x8b,0x71,
-0x8c,0x64,0x00,0x04,0x30,0x42,0x00,0x01,0x00,0x04,0x21,0x42,0x14,0x40,0x00,0x4d,
-0x30,0x90,0x00,0x01,0x00,0x07,0x2c,0x00,0x00,0x05,0x2c,0x03,0x0c,0x00,0x1b,0x66,
-0x02,0x20,0x20,0x21,0x96,0x26,0x00,0x06,0x12,0x00,0x00,0x14,0x30,0xc5,0xff,0xff,
-0x02,0x60,0x90,0x21,0x00,0x05,0x10,0xc0,0x00,0x45,0x10,0x21,0x00,0x02,0x10,0x80,
-0x00,0x52,0x18,0x21,0x92,0x22,0x00,0x0a,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x0b,
-0x02,0x20,0x20,0x21,0x8c,0x63,0x00,0x18,0x00,0x00,0x00,0x00,0x8c,0x62,0x00,0x04,
-0x00,0x00,0x00,0x00,0x00,0x02,0x11,0x42,0x0c,0x00,0x1b,0x66,0x30,0x50,0x00,0x01,
-0x96,0x26,0x00,0x06,0x16,0x00,0xff,0xef,0x30,0xc5,0xff,0xff,0x92,0x22,0x00,0x04,
-0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x0d,0x24,0x02,0x00,0x01,0xa2,0x22,0x00,0x17,
-0x92,0x22,0x00,0x17,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x05,0x00,0x00,0x00,0x00,
-0xa6,0x26,0x00,0x14,0x92,0x22,0x00,0x16,0x08,0x00,0x10,0x96,0x30,0x42,0x00,0xc3,
-0x96,0x22,0x00,0x00,0x08,0x00,0x11,0xbd,0xa6,0x22,0x00,0x14,0x92,0x22,0x00,0x0a,
-0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x03,0x00,0x00,0x00,0x00,0x08,0x00,0x11,0xb8,
-0xa2,0x20,0x00,0x17,0x96,0x24,0x00,0x00,0x30,0xc5,0xff,0xff,0x00,0x05,0x18,0xc0,
-0x00,0x04,0x10,0xc0,0x00,0x44,0x10,0x21,0x00,0x65,0x18,0x21,0x27,0x84,0x90,0x00,
-0x00,0x02,0x10,0x80,0x00,0x44,0x10,0x21,0x00,0x03,0x18,0x80,0x8c,0x45,0x00,0x08,
-0x00,0x64,0x18,0x21,0x8c,0x64,0x00,0x08,0x3c,0x02,0x80,0x00,0x00,0xa2,0x38,0x24,
-0x10,0xe0,0x00,0x08,0x00,0x82,0x10,0x24,0x10,0x40,0x00,0x04,0x00,0x00,0x18,0x21,
-0x10,0xe0,0x00,0x02,0x24,0x03,0x00,0x01,0x00,0x85,0x18,0x2b,0x08,0x00,0x11,0xb8,
-0xa2,0x23,0x00,0x17,0x10,0x40,0xff,0xfd,0x00,0x85,0x18,0x2b,0x08,0x00,0x11,0xdc,
-0x00,0x00,0x00,0x00,0x24,0x05,0x00,0x24,0xf0,0xe5,0x00,0x06,0x00,0x00,0x28,0x12,
-0x27,0x82,0x90,0x00,0x00,0xa2,0x28,0x21,0x0c,0x00,0x01,0x49,0x00,0x00,0x20,0x21,
-0x96,0x47,0x00,0x0c,0x08,0x00,0x11,0x9a,0x00,0x07,0x2c,0x00,0x27,0x83,0x90,0x10,
-0x27,0x82,0x90,0x18,0x00,0xa2,0x10,0x21,0x00,0xa3,0x18,0x21,0x90,0x44,0x00,0x00,
-0x90,0x65,0x00,0x05,0x93,0x82,0x80,0x10,0x24,0x07,0x00,0x01,0x0c,0x00,0x21,0x9a,
-0xaf,0xa2,0x00,0x10,0x96,0x47,0x00,0x0c,0x08,0x00,0x11,0x8d,0x00,0x07,0x1c,0x00,
-0x10,0xa2,0x00,0x09,0x24,0x02,0x00,0x02,0x10,0xa2,0x00,0x05,0x24,0x02,0x00,0x03,
-0x14,0xa2,0xff,0x7d,0x00,0x00,0x00,0x00,0x08,0x00,0x11,0x7e,0xa2,0x40,0x00,0x07,
-0x08,0x00,0x11,0x7e,0xa2,0x40,0x00,0x06,0x08,0x00,0x11,0x7e,0xa2,0x40,0x00,0x05,
-0x14,0x40,0xff,0x71,0x3c,0x04,0xb0,0x05,0x34,0x84,0x02,0x29,0x90,0x83,0x00,0x00,
-0x30,0xa5,0x00,0x0f,0x24,0x02,0x00,0x80,0x08,0x00,0x11,0x75,0x00,0xa2,0x10,0x07,
-0x14,0x40,0xfe,0xc3,0x3c,0x04,0xb0,0x05,0x34,0x84,0x02,0x29,0x90,0x83,0x00,0x00,
-0x30,0xa5,0x00,0x0f,0x24,0x02,0x00,0x80,0x08,0x00,0x10,0xd0,0x00,0xa2,0x10,0x07,
-0x84,0x83,0x00,0x0c,0x00,0x00,0x00,0x00,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,
-0x00,0x02,0x10,0x80,0x27,0x83,0x90,0x04,0x00,0x43,0x10,0x21,0x8c,0x47,0x00,0x18,
-0x00,0x00,0x00,0x00,0x8c,0xe6,0x00,0x08,0x0c,0x00,0x0f,0x19,0x00,0x00,0x00,0x00,
-0x02,0x40,0x20,0x21,0x00,0x00,0x28,0x21,0x00,0x00,0x30,0x21,0x00,0x00,0x38,0x21,
-0x0c,0x00,0x1c,0x68,0xaf,0xa2,0x00,0x10,0x00,0x02,0x1e,0x00,0x14,0x60,0xfe,0x6b,
-0xa2,0x22,0x00,0x12,0x92,0x43,0x00,0x08,0x00,0x00,0x00,0x00,0x14,0x60,0x00,0x40,
-0x24,0x02,0x00,0x01,0xa2,0x40,0x00,0x04,0x92,0x28,0x00,0x04,0x00,0x00,0x00,0x00,
-0x15,0x00,0x00,0x19,0x24,0x02,0x00,0x01,0x92,0x27,0x00,0x0a,0xa2,0x22,0x00,0x17,
-0x92,0x22,0x00,0x17,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x10,0x00,0x00,0x00,0x00,
-0x96,0x22,0x00,0x06,0x00,0x00,0x00,0x00,0xa6,0x22,0x00,0x14,0x92,0x22,0x00,0x16,
-0x30,0xe3,0x00,0xff,0x30,0x42,0x00,0xc0,0x10,0x60,0x00,0x03,0xa2,0x22,0x00,0x16,
-0x34,0x42,0x00,0x01,0xa2,0x22,0x00,0x16,0x11,0x00,0xfe,0x50,0x00,0x00,0x00,0x00,
-0x92,0x22,0x00,0x16,0x08,0x00,0x10,0x96,0x34,0x42,0x00,0x02,0x96,0x22,0x00,0x00,
-0x08,0x00,0x12,0x3f,0xa6,0x22,0x00,0x14,0x92,0x27,0x00,0x0a,0x00,0x00,0x00,0x00,
-0x14,0xe0,0x00,0x03,0x00,0x00,0x00,0x00,0x08,0x00,0x12,0x38,0xa2,0x20,0x00,0x17,
-0x96,0x24,0x00,0x00,0x96,0x25,0x00,0x06,0x27,0x86,0x90,0x00,0x00,0x04,0x18,0xc0,
-0x00,0x64,0x18,0x21,0x00,0x05,0x10,0xc0,0x00,0x45,0x10,0x21,0x00,0x03,0x18,0x80,
-0x00,0x66,0x18,0x21,0x00,0x02,0x10,0x80,0x00,0x46,0x10,0x21,0x8c,0x65,0x00,0x08,
-0x8c,0x44,0x00,0x08,0x3c,0x03,0x80,0x00,0x00,0xa3,0x30,0x24,0x10,0xc0,0x00,0x08,
-0x00,0x83,0x10,0x24,0x10,0x40,0x00,0x04,0x00,0x00,0x18,0x21,0x10,0xc0,0x00,0x02,
-0x24,0x03,0x00,0x01,0x00,0x85,0x18,0x2b,0x08,0x00,0x12,0x38,0xa2,0x23,0x00,0x17,
-0x10,0x40,0xff,0xfd,0x00,0x85,0x18,0x2b,0x08,0x00,0x12,0x67,0x00,0x00,0x00,0x00,
-0x10,0x62,0x00,0x09,0x24,0x02,0x00,0x02,0x10,0x62,0x00,0x05,0x24,0x02,0x00,0x03,
-0x14,0x62,0xff,0xbd,0x00,0x00,0x00,0x00,0x08,0x00,0x12,0x32,0xa2,0x40,0x00,0x07,
-0x08,0x00,0x12,0x32,0xa2,0x40,0x00,0x06,0x08,0x00,0x12,0x32,0xa2,0x40,0x00,0x05,
-0x3c,0x02,0x80,0x00,0x00,0x82,0x30,0x24,0x10,0xc0,0x00,0x08,0x00,0xa2,0x18,0x24,
-0x10,0x60,0x00,0x04,0x00,0x00,0x10,0x21,0x10,0xc0,0x00,0x02,0x24,0x02,0x00,0x01,
-0x00,0xa4,0x10,0x2b,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x10,0x60,0xff,0xfd,
-0x00,0xa4,0x10,0x2b,0x08,0x00,0x12,0x82,0x00,0x00,0x00,0x00,0x30,0x82,0xff,0xff,
-0x00,0x02,0x18,0xc0,0x00,0x62,0x18,0x21,0x27,0x84,0x90,0x10,0x00,0x03,0x18,0x80,
-0x00,0x64,0x18,0x21,0x80,0x66,0x00,0x06,0x00,0x02,0x12,0x00,0x3c,0x03,0xb0,0x00,
-0x00,0x46,0x10,0x21,0x00,0x45,0x10,0x21,0x03,0xe0,0x00,0x08,0x00,0x43,0x10,0x21,
-0x27,0xbd,0xff,0xe0,0x30,0x82,0x00,0x7c,0x30,0x84,0xff,0x00,0xaf,0xbf,0x00,0x1c,
-0xaf,0xb2,0x00,0x18,0xaf,0xb1,0x00,0x14,0xaf,0xb0,0x00,0x10,0x14,0x40,0x00,0x41,
-0x00,0x04,0x22,0x03,0x24,0x02,0x00,0x04,0x3c,0x10,0xb0,0x03,0x8e,0x10,0x00,0x00,
-0x10,0x82,0x00,0x32,0x24,0x02,0x00,0x08,0x10,0x82,0x00,0x03,0x32,0x02,0x00,0x20,
-0x08,0x00,0x12,0xa8,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x17,0x3c,0x02,0xb0,0x06,
-0x34,0x42,0x80,0x24,0x8c,0x43,0x00,0x00,0x00,0x00,0x00,0x00,0x30,0x67,0x00,0xff,
-0x10,0xe0,0x00,0x23,0x00,0x00,0x88,0x21,0x8f,0x85,0x8f,0xe0,0x00,0x40,0x30,0x21,
-0x94,0xa2,0x00,0x08,0x8c,0xc3,0x00,0x00,0x26,0x31,0x00,0x01,0x24,0x42,0x00,0x02,
-0x30,0x42,0x01,0xff,0x34,0x63,0x01,0x00,0x02,0x27,0x20,0x2a,0xa4,0xa2,0x00,0x08,
-0x14,0x80,0xff,0xf7,0xac,0xc3,0x00,0x00,0x84,0xa3,0x00,0x08,0x3c,0x02,0xb0,0x03,
-0x34,0x42,0x00,0x30,0xac,0x43,0x00,0x00,0x27,0x92,0xb4,0x00,0x24,0x11,0x00,0x12,
-0x8e,0x44,0x00,0x00,0x26,0x31,0xff,0xff,0x90,0x82,0x00,0x10,0x00,0x00,0x00,0x00,
-0x10,0x40,0x00,0x03,0x26,0x52,0x00,0x04,0x0c,0x00,0x18,0xd0,0x00,0x00,0x00,0x00,
-0x06,0x21,0xff,0xf7,0x24,0x02,0xff,0xdf,0x02,0x02,0x80,0x24,0x3c,0x01,0xb0,0x03,
-0x0c,0x00,0x13,0x1c,0xac,0x30,0x00,0x00,0x08,0x00,0x12,0xa8,0x00,0x00,0x00,0x00,
-0x8f,0x85,0x8f,0xe0,0x08,0x00,0x12,0xbe,0x00,0x00,0x00,0x00,0x24,0x02,0xff,0x95,
-0x3c,0x03,0xb0,0x03,0x02,0x02,0x80,0x24,0x34,0x63,0x00,0x30,0x3c,0x01,0xb0,0x03,
-0xac,0x30,0x00,0x00,0x0c,0x00,0x12,0xe5,0xac,0x60,0x00,0x00,0x08,0x00,0x12,0xa8,
-0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0x50,0x08,0x00,0x12,0xa8,
-0xac,0x46,0x00,0x00,0x3c,0x0a,0x80,0x00,0x25,0x4a,0x4b,0x94,0x3c,0x0b,0xb0,0x03,
-0xad,0x6a,0x00,0x20,0x3c,0x08,0x80,0x01,0x25,0x08,0x00,0x00,0x3c,0x09,0x80,0x01,
-0x25,0x29,0x03,0x50,0x11,0x09,0x00,0x10,0x00,0x00,0x00,0x00,0x3c,0x0a,0x80,0x00,
-0x25,0x4a,0x4b,0xbc,0x3c,0x0b,0xb0,0x03,0xad,0x6a,0x00,0x20,0x3c,0x08,0xb0,0x06,
-0x35,0x08,0x80,0x10,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x8d,0x09,0x00,0x00,
-0x00,0x00,0x00,0x00,0x31,0x29,0x00,0x01,0x00,0x00,0x00,0x00,0x24,0x01,0x00,0x01,
-0x15,0x21,0xff,0xf2,0x00,0x00,0x00,0x00,0x3c,0x0a,0x80,0x00,0x25,0x4a,0x4b,0xf8,
-0x3c,0x0b,0xb0,0x03,0xad,0x6a,0x00,0x20,0x3c,0x02,0xb0,0x03,0x8c,0x43,0x00,0x00,
-0x00,0x00,0x00,0x00,0x34,0x63,0x00,0x40,0x00,0x00,0x00,0x00,0xac,0x43,0x00,0x00,
-0x00,0x00,0x00,0x00,0x3c,0x0a,0x80,0x00,0x25,0x4a,0x4c,0x24,0x3c,0x0b,0xb0,0x03,
-0xad,0x6a,0x00,0x20,0x3c,0x02,0x80,0x01,0x24,0x42,0x00,0x00,0x3c,0x03,0x80,0x01,
-0x24,0x63,0x03,0x50,0x3c,0x04,0xb0,0x00,0x8c,0x85,0x00,0x00,0x00,0x00,0x00,0x00,
-0xac,0x45,0x00,0x00,0x24,0x42,0x00,0x04,0x24,0x84,0x00,0x04,0x00,0x43,0x08,0x2a,
-0x14,0x20,0xff,0xf9,0x00,0x00,0x00,0x00,0x0c,0x00,0x13,0x1c,0x00,0x00,0x00,0x00,
-0x3c,0x0a,0x80,0x00,0x25,0x4a,0x4c,0x70,0x3c,0x0b,0xb0,0x03,0xad,0x6a,0x00,0x20,
-0x3c,0x02,0x80,0x01,0x24,0x42,0x03,0x50,0x3c,0x03,0x80,0x01,0x24,0x63,0x3f,0x24,
-0xac,0x40,0x00,0x00,0xac,0x40,0x00,0x04,0xac,0x40,0x00,0x08,0xac,0x40,0x00,0x0c,
-0x24,0x42,0x00,0x10,0x00,0x43,0x08,0x2a,0x14,0x20,0xff,0xf9,0x00,0x00,0x00,0x00,
-0x3c,0x0a,0x80,0x00,0x25,0x4a,0x4c,0xb0,0x3c,0x0b,0xb0,0x03,0xad,0x6a,0x00,0x20,
-0x3c,0x1c,0x80,0x01,0x27,0x9c,0x7f,0xf0,0x27,0x9d,0x8b,0xe0,0x00,0x00,0x00,0x00,
-0x27,0x9d,0x8f,0xc8,0x3c,0x0a,0x80,0x00,0x25,0x4a,0x4c,0xd4,0x3c,0x0b,0xb0,0x03,
-0xad,0x6a,0x00,0x20,0x40,0x80,0x68,0x00,0x40,0x08,0x60,0x00,0x00,0x00,0x00,0x00,
-0x35,0x08,0xff,0x01,0x40,0x88,0x60,0x00,0x00,0x00,0x00,0x00,0x0c,0x00,0x15,0x62,
-0x00,0x00,0x00,0x00,0x24,0x84,0xf8,0x00,0x30,0x87,0x00,0x03,0x00,0x04,0x30,0x40,
-0x00,0xc7,0x20,0x23,0x3c,0x02,0xb0,0x0a,0x27,0xbd,0xff,0xe0,0x24,0x03,0xff,0xff,
-0x00,0x82,0x20,0x21,0xaf,0xb1,0x00,0x14,0xac,0x83,0x10,0x00,0xaf,0xbf,0x00,0x18,
-0xaf,0xb0,0x00,0x10,0x00,0xa0,0x88,0x21,0x24,0x03,0x00,0x01,0x8c,0x82,0x10,0x00,
-0x00,0x00,0x00,0x00,0x14,0x43,0xff,0xfd,0x00,0xc7,0x10,0x23,0x3c,0x03,0xb0,0x0a,
-0x00,0x43,0x10,0x21,0x8c,0x50,0x00,0x00,0x0c,0x00,0x13,0x99,0x02,0x20,0x20,0x21,
-0x02,0x11,0x80,0x24,0x00,0x50,0x80,0x06,0x02,0x00,0x10,0x21,0x8f,0xbf,0x00,0x18,
-0x7b,0xb0,0x00,0xbc,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x20,0x27,0xbd,0xff,0xd8,
-0xaf,0xb2,0x00,0x18,0x00,0xa0,0x90,0x21,0x24,0x05,0xff,0xff,0xaf,0xb3,0x00,0x1c,
-0xaf,0xbf,0x00,0x20,0xaf,0xb1,0x00,0x14,0xaf,0xb0,0x00,0x10,0x00,0xc0,0x98,0x21,
-0x12,0x45,0x00,0x23,0x24,0x84,0xf8,0x00,0x30,0x83,0x00,0x03,0x00,0x04,0x10,0x40,
-0x00,0x40,0x88,0x21,0x00,0x60,0x20,0x21,0x00,0x43,0x10,0x23,0x3c,0x03,0xb0,0x0a,
-0x00,0x43,0x10,0x21,0xac,0x45,0x10,0x00,0x00,0x40,0x18,0x21,0x24,0x05,0x00,0x01,
-0x8c,0x62,0x10,0x00,0x00,0x00,0x00,0x00,0x14,0x45,0xff,0xfd,0x3c,0x02,0xb0,0x0a,
-0x02,0x24,0x88,0x23,0x02,0x22,0x88,0x21,0x8e,0x30,0x00,0x00,0x0c,0x00,0x13,0x99,
-0x02,0x40,0x20,0x21,0x00,0x12,0x18,0x27,0x02,0x03,0x80,0x24,0x00,0x53,0x10,0x04,
-0x02,0x02,0x80,0x25,0xae,0x30,0x00,0x00,0x24,0x03,0x00,0x01,0x8e,0x22,0x10,0x00,
-0x00,0x00,0x00,0x00,0x14,0x43,0xff,0xfd,0x00,0x00,0x00,0x00,0x8f,0xbf,0x00,0x20,
-0x7b,0xb2,0x00,0xfc,0x7b,0xb0,0x00,0xbc,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x28,
-0x30,0x82,0x00,0x03,0x00,0x04,0x18,0x40,0x00,0x62,0x18,0x23,0x3c,0x04,0xb0,0x0a,
-0x00,0x64,0x18,0x21,0xac,0x66,0x00,0x00,0x24,0x04,0x00,0x01,0x8c,0x62,0x10,0x00,
-0x00,0x00,0x00,0x00,0x14,0x44,0xff,0xfd,0x00,0x00,0x00,0x00,0x08,0x00,0x13,0x87,
-0x00,0x00,0x00,0x00,0x00,0x00,0x18,0x21,0x00,0x64,0x10,0x06,0x30,0x42,0x00,0x01,
-0x14,0x40,0x00,0x05,0x00,0x00,0x00,0x00,0x24,0x63,0x00,0x01,0x2c,0x62,0x00,0x20,
-0x14,0x40,0xff,0xf9,0x00,0x00,0x00,0x00,0x03,0xe0,0x00,0x08,0x00,0x60,0x10,0x21,
-0x27,0xbd,0xff,0xe0,0x3c,0x03,0xb0,0x05,0xaf,0xb2,0x00,0x18,0xaf,0xb1,0x00,0x14,
-0xaf,0xb0,0x00,0x10,0xaf,0xbf,0x00,0x1c,0x00,0x80,0x90,0x21,0x00,0xa0,0x80,0x21,
-0x00,0xc0,0x88,0x21,0x34,0x63,0x02,0x2e,0x90,0x62,0x00,0x00,0x00,0x00,0x00,0x00,
-0x30,0x42,0x00,0x01,0x14,0x40,0xff,0xfc,0x24,0x04,0x08,0x24,0x3c,0x05,0x00,0xc0,
-0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x03,0x24,0x04,0x08,0x34,0x3c,0x05,0x00,0xc0,
-0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x03,0x3c,0x02,0xc0,0x00,0x00,0x10,0x1c,0x00,
-0x34,0x42,0x04,0x00,0x3c,0x04,0xb0,0x05,0x3c,0x05,0xb0,0x05,0x24,0x63,0x16,0x09,
-0x02,0x22,0x10,0x21,0x34,0x84,0x04,0x20,0x34,0xa5,0x04,0x24,0x3c,0x06,0xb0,0x05,
-0xac,0x83,0x00,0x00,0x24,0x07,0x00,0x01,0xac,0xa2,0x00,0x00,0x34,0xc6,0x02,0x28,
-0x24,0x02,0x00,0x20,0xae,0x47,0x00,0x3c,0x24,0x04,0x08,0x24,0xa0,0xc2,0x00,0x00,
-0x3c,0x05,0x00,0xc0,0xa2,0x47,0x00,0x11,0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x01,
-0x24,0x04,0x08,0x34,0x3c,0x05,0x00,0xc0,0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x01,
-0x8f,0xbf,0x00,0x1c,0x8f,0xb2,0x00,0x18,0x7b,0xb0,0x00,0xbc,0x03,0xe0,0x00,0x08,
-0x27,0xbd,0x00,0x20,0x24,0x02,0x00,0x06,0xac,0x82,0x00,0x0c,0xa0,0x80,0x00,0x50,
-0xac,0x80,0x00,0x00,0xac,0x80,0x00,0x04,0xac,0x80,0x00,0x08,0xac,0x80,0x00,0x14,
-0xac,0x80,0x00,0x18,0xac,0x80,0x00,0x1c,0xa4,0x80,0x00,0x20,0xac,0x80,0x00,0x24,
-0xac,0x80,0x00,0x28,0xac,0x80,0x00,0x2c,0xa0,0x80,0x00,0x30,0xa0,0x80,0x00,0x31,
-0xac,0x80,0x00,0x34,0xac,0x80,0x00,0x38,0xa0,0x80,0x00,0x3c,0xac,0x82,0x00,0x10,
-0xa0,0x80,0x00,0x44,0xac,0x80,0x00,0x48,0x03,0xe0,0x00,0x08,0xac,0x80,0x00,0x4c,
-0x3c,0x04,0xb0,0x06,0x34,0x84,0x80,0x00,0x8c,0x83,0x00,0x00,0x3c,0x02,0x12,0x00,
-0x3c,0x05,0xb0,0x03,0x00,0x62,0x18,0x25,0x34,0xa5,0x00,0x8b,0x24,0x02,0xff,0x80,
-0xac,0x83,0x00,0x00,0x03,0xe0,0x00,0x08,0xa0,0xa2,0x00,0x00,0x3c,0x04,0xb0,0x03,
-0x34,0x84,0x00,0x0b,0x24,0x02,0x00,0x22,0x3c,0x05,0xb0,0x01,0x3c,0x06,0x45,0x67,
-0x3c,0x0a,0xb0,0x09,0xa0,0x82,0x00,0x00,0x34,0xa5,0x00,0x04,0x34,0xc6,0x89,0xaa,
-0x35,0x4a,0x00,0x04,0x24,0x02,0x01,0x23,0x3c,0x0b,0xb0,0x09,0x3c,0x07,0x01,0x23,
-0x3c,0x0c,0xb0,0x09,0x3c,0x01,0xb0,0x01,0xac,0x20,0x00,0x00,0x27,0xbd,0xff,0xe0,
-0xac,0xa0,0x00,0x00,0x35,0x6b,0x00,0x08,0x3c,0x01,0xb0,0x09,0xac,0x26,0x00,0x00,
-0x34,0xe7,0x45,0x66,0xa5,0x42,0x00,0x00,0x35,0x8c,0x00,0x0c,0x24,0x02,0xcd,0xef,
-0x3c,0x0d,0xb0,0x09,0x3c,0x08,0xcd,0xef,0x3c,0x0e,0xb0,0x09,0xad,0x67,0x00,0x00,
-0xaf,0xb7,0x00,0x1c,0xa5,0x82,0x00,0x00,0xaf,0xb6,0x00,0x18,0xaf,0xb5,0x00,0x14,
-0xaf,0xb4,0x00,0x10,0xaf,0xb3,0x00,0x0c,0xaf,0xb2,0x00,0x08,0xaf,0xb1,0x00,0x04,
-0xaf,0xb0,0x00,0x00,0x35,0xad,0x00,0x10,0x35,0x08,0x01,0x22,0x35,0xce,0x00,0x14,
-0x24,0x02,0x89,0xab,0x3c,0x0f,0xb0,0x09,0x3c,0x09,0x89,0xab,0x3c,0x10,0xb0,0x09,
-0x3c,0x11,0xb0,0x09,0x3c,0x12,0xb0,0x09,0x3c,0x13,0xb0,0x09,0x3c,0x14,0xb0,0x09,
-0x3c,0x15,0xb0,0x09,0x3c,0x16,0xb0,0x09,0x3c,0x17,0xb0,0x09,0xad,0xa8,0x00,0x00,
-0x24,0x03,0xff,0xff,0xa5,0xc2,0x00,0x00,0x35,0xef,0x00,0x18,0x35,0x29,0xcd,0xee,
-0x36,0x10,0x00,0x1c,0x36,0x31,0x00,0x20,0x36,0x52,0x00,0x24,0x36,0x73,0x00,0x28,
-0x36,0x94,0x00,0x2c,0x36,0xb5,0x00,0x30,0x36,0xd6,0x00,0x34,0x36,0xf7,0x00,0x38,
-0x24,0x02,0x45,0x67,0xad,0xe9,0x00,0x00,0xa6,0x02,0x00,0x00,0xae,0x23,0x00,0x00,
-0x8f,0xb0,0x00,0x00,0xa6,0x43,0x00,0x00,0x8f,0xb1,0x00,0x04,0xae,0x63,0x00,0x00,
-0x8f,0xb2,0x00,0x08,0xa6,0x83,0x00,0x00,0x8f,0xb3,0x00,0x0c,0xae,0xa3,0x00,0x00,
-0x8f,0xb4,0x00,0x10,0xa6,0xc3,0x00,0x00,0x8f,0xb5,0x00,0x14,0xae,0xe3,0x00,0x00,
-0x7b,0xb6,0x00,0xfc,0x3c,0x18,0xb0,0x09,0x37,0x18,0x00,0x3c,0xa7,0x03,0x00,0x00,
-0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x20,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,
-0x34,0x63,0x00,0x20,0x24,0x42,0x51,0x48,0xac,0x62,0x00,0x00,0x8c,0x83,0x00,0x34,
-0x34,0x02,0xff,0xff,0x00,0x43,0x10,0x2a,0x14,0x40,0x01,0x04,0x00,0x80,0x28,0x21,
-0x8c,0x86,0x00,0x08,0x24,0x02,0x00,0x03,0x10,0xc2,0x00,0xf7,0x00,0x00,0x00,0x00,
-0x8c,0xa2,0x00,0x2c,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x4f,0x24,0x02,0x00,0x06,
-0x3c,0x03,0xb0,0x05,0x34,0x63,0x04,0x50,0x90,0x62,0x00,0x00,0x00,0x00,0x00,0x00,
-0x30,0x42,0x00,0xff,0x14,0x40,0x00,0xdd,0xac,0xa2,0x00,0x2c,0x24,0x02,0x00,0x01,
-0x10,0xc2,0x00,0xdc,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0xc2,0x00,0xca,
-0x00,0x00,0x00,0x00,0x8c,0xa7,0x00,0x04,0x24,0x02,0x00,0x02,0x10,0xe2,0x00,0xc0,
-0x00,0x00,0x00,0x00,0x8c,0xa2,0x00,0x14,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x09,
-0x24,0x02,0x00,0x01,0x3c,0x03,0xb0,0x09,0x34,0x63,0x01,0x60,0x90,0x62,0x00,0x00,
-0x00,0x00,0x00,0x00,0x30,0x42,0x00,0xff,0x10,0x40,0x00,0x05,0xac,0xa2,0x00,0x14,
-0x24,0x02,0x00,0x01,0xac,0xa2,0x00,0x00,0x03,0xe0,0x00,0x08,0xac,0xa0,0x00,0x14,
-0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0xd0,0x8c,0x43,0x00,0x00,0x00,0x00,0x00,0x00,
-0x04,0x61,0x00,0x19,0x3c,0x02,0xb0,0x03,0x3c,0x03,0xb0,0x05,0x34,0x63,0x02,0x2e,
-0x90,0x62,0x00,0x00,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x01,0x14,0x40,0x00,0x12,
-0x3c,0x02,0xb0,0x03,0x3c,0x02,0xb0,0x05,0x34,0x42,0x02,0x42,0x90,0x43,0x00,0x00,
-0x00,0x00,0x00,0x00,0x14,0x60,0x00,0x0c,0x3c,0x02,0xb0,0x03,0x80,0xa2,0x00,0x50,
-0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x08,0x3c,0x02,0xb0,0x03,0x14,0xc0,0x00,0x07,
-0x34,0x42,0x00,0x3f,0x24,0x02,0x00,0x0e,0x24,0x03,0x00,0x01,0xac,0xa2,0x00,0x00,
-0x03,0xe0,0x00,0x08,0xa0,0xa3,0x00,0x50,0x34,0x42,0x00,0x3f,0x90,0x44,0x00,0x00,
-0x24,0x03,0x00,0x01,0x10,0x64,0x00,0x7f,0x3c,0x03,0xb0,0x05,0x80,0xa2,0x00,0x31,
-0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x0a,0x3c,0x02,0xb0,0x06,0x34,0x42,0x80,0x18,
-0x8c,0x43,0x00,0x00,0x3c,0x04,0xf0,0x00,0x3c,0x02,0x80,0x00,0x00,0x64,0x18,0x24,
-0x10,0x62,0x00,0x03,0x24,0x02,0x00,0x09,0x03,0xe0,0x00,0x08,0xac,0xa2,0x00,0x00,
-0x8c,0xa2,0x00,0x40,0x00,0x00,0x00,0x00,0x8c,0x43,0x00,0x00,0x00,0x00,0x00,0x00,
-0x10,0x60,0x00,0x09,0x3c,0x03,0xb0,0x03,0x3c,0x02,0xb0,0x05,0x34,0x42,0x02,0x2c,
-0x8c,0x43,0x00,0x00,0x3c,0x04,0x00,0x02,0x00,0x64,0x18,0x24,0x14,0x60,0xff,0xf2,
-0x24,0x02,0x00,0x10,0x3c,0x03,0xb0,0x03,0x34,0x63,0x02,0x01,0x90,0x62,0x00,0x00,
-0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x80,0x10,0x40,0x00,0x0e,0x00,0x00,0x00,0x00,
-0x8c,0xa3,0x00,0x0c,0x00,0x00,0x00,0x00,0xac,0xa3,0x00,0x10,0x3c,0x02,0xb0,0x03,
-0x90,0x42,0x02,0x01,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x0f,0xac,0xa2,0x00,0x0c,
-0x90,0xa3,0x00,0x0f,0x24,0x02,0x00,0x0d,0x3c,0x01,0xb0,0x03,0x08,0x00,0x14,0xb2,
-0xa0,0x23,0x02,0x01,0x3c,0x02,0xb0,0x09,0x34,0x42,0x01,0x80,0x90,0x44,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x04,0x1e,0x00,0x00,0x03,0x1e,0x03,0x10,0x60,0x00,0x15,
-0xa0,0xa4,0x00,0x44,0x24,0x02,0x00,0x01,0x10,0x62,0x00,0x0b,0x24,0x02,0x00,0x02,
-0x10,0x62,0x00,0x03,0x24,0x03,0x00,0x0d,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,
-0x8c,0xa2,0x00,0x0c,0xac,0xa3,0x00,0x00,0x24,0x03,0x00,0x04,0xac,0xa2,0x00,0x10,
-0x03,0xe0,0x00,0x08,0xac,0xa3,0x00,0x0c,0x24,0x02,0x00,0x0d,0xac,0xa2,0x00,0x00,
-0x24,0x03,0x00,0x04,0x24,0x02,0x00,0x06,0xac,0xa3,0x00,0x10,0x03,0xe0,0x00,0x08,
-0xac,0xa2,0x00,0x0c,0x8c,0xa3,0x00,0x38,0x24,0x04,0x00,0x01,0x10,0x64,0x00,0x2d,
-0x24,0x02,0x00,0x02,0x10,0x60,0x00,0x19,0x00,0x00,0x00,0x00,0x10,0x62,0x00,0x10,
-0x24,0x02,0x00,0x04,0x10,0x62,0x00,0x04,0x00,0x00,0x00,0x00,0xac,0xa0,0x00,0x38,
-0x03,0xe0,0x00,0x08,0xac,0xa0,0x00,0x00,0x10,0xe4,0x00,0x07,0x24,0x02,0x00,0x03,
-0x80,0xa2,0x00,0x30,0x00,0x00,0x00,0x00,0x00,0x02,0x18,0x0b,0xac,0xa3,0x00,0x00,
-0x03,0xe0,0x00,0x08,0xac,0xa0,0x00,0x38,0x08,0x00,0x15,0x04,0xac,0xa2,0x00,0x00,
-0x10,0xe4,0x00,0x02,0x24,0x02,0x00,0x03,0x24,0x02,0x00,0x0c,0xac,0xa2,0x00,0x00,
-0x24,0x02,0x00,0x04,0x03,0xe0,0x00,0x08,0xac,0xa2,0x00,0x38,0x10,0xe4,0x00,0x0e,
-0x3c,0x03,0xb0,0x06,0x34,0x63,0x80,0x24,0x8c,0x62,0x00,0x00,0x00,0x00,0x00,0x00,
-0x30,0x42,0x00,0xff,0x10,0x40,0x00,0x06,0xac,0xa2,0x00,0x18,0x24,0x02,0x00,0x02,
-0xac,0xa2,0x00,0x00,0xac,0xa0,0x00,0x18,0x08,0x00,0x15,0x0d,0x24,0x02,0x00,0x01,
-0x08,0x00,0x15,0x1a,0xac,0xa0,0x00,0x00,0x24,0x02,0x00,0x03,0x08,0x00,0x15,0x1a,
-0xac,0xa2,0x00,0x00,0x24,0x03,0x00,0x0b,0xac,0xa2,0x00,0x38,0x03,0xe0,0x00,0x08,
-0xac,0xa3,0x00,0x00,0x34,0x63,0x02,0x2e,0x90,0x62,0x00,0x00,0x00,0x00,0x00,0x00,
-0x30,0x42,0x00,0x01,0x14,0x40,0xff,0x7d,0x3c,0x02,0xb0,0x05,0x34,0x42,0x02,0x42,
-0x90,0x43,0x00,0x00,0x00,0x00,0x00,0x00,0x14,0x60,0xff,0x78,0x00,0x00,0x00,0x00,
-0x10,0xc0,0xff,0x81,0x24,0x02,0x00,0x0e,0x08,0x00,0x14,0xa7,0x00,0x00,0x00,0x00,
-0x80,0xa2,0x00,0x30,0x00,0x00,0x00,0x00,0x14,0x40,0xff,0x3e,0x24,0x02,0x00,0x04,
-0x08,0x00,0x14,0xb2,0x00,0x00,0x00,0x00,0x84,0xa2,0x00,0x20,0x00,0x00,0x00,0x00,
-0x10,0x40,0xff,0x75,0x24,0x02,0x00,0x06,0x3c,0x02,0xb0,0x05,0x34,0x42,0x02,0x2e,
-0x90,0x43,0x00,0x00,0x00,0x00,0x00,0x00,0x30,0x63,0x00,0x01,0x30,0x63,0x00,0xff,
-0x00,0x60,0x10,0x21,0x14,0x40,0xff,0x2b,0xa4,0xa3,0x00,0x20,0x08,0x00,0x14,0xb2,
-0x24,0x02,0x00,0x06,0x8c,0xa2,0x00,0x1c,0x00,0x00,0x00,0x00,0x14,0x40,0xff,0x66,
-0x24,0x02,0x00,0x05,0x3c,0x03,0xb0,0x05,0x34,0x63,0x02,0x2c,0x8c,0x62,0x00,0x00,
-0x00,0x00,0x00,0x00,0x30,0x42,0x00,0xff,0x10,0x40,0xff,0x1b,0xac,0xa2,0x00,0x1c,
-0x08,0x00,0x14,0xb2,0x24,0x02,0x00,0x05,0x3c,0x02,0xb0,0x05,0x8c,0x42,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x02,0x17,0x42,0x30,0x42,0x00,0x01,0x14,0x40,0xff,0x56,
-0x24,0x02,0x00,0x06,0x08,0x00,0x14,0x60,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x0a,
-0x03,0xe0,0x00,0x08,0xac,0x82,0x00,0x00,0x27,0xbd,0xff,0xd8,0xaf,0xb0,0x00,0x10,
-0x27,0x90,0x86,0x58,0xaf,0xbf,0x00,0x20,0xaf,0xb3,0x00,0x1c,0xaf,0xb2,0x00,0x18,
-0x0c,0x00,0x29,0xd5,0xaf,0xb1,0x00,0x14,0xaf,0x90,0x8f,0xe0,0x48,0x02,0x00,0x00,
-0x0c,0x00,0x13,0xf0,0x00,0x00,0x00,0x00,0x0c,0x00,0x18,0x1f,0x02,0x00,0x20,0x21,
-0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0x3a,0x94,0x43,0x00,0x00,0x00,0x00,0x00,0x00,
-0xa3,0x83,0x8f,0xe4,0x0c,0x00,0x00,0x34,0x00,0x00,0x00,0x00,0x0c,0x00,0x13,0xfb,
-0x00,0x00,0x00,0x00,0x27,0x84,0x84,0x98,0x0c,0x00,0x27,0x59,0x00,0x00,0x00,0x00,
-0x93,0x84,0x80,0x10,0x0c,0x00,0x21,0x3f,0x00,0x00,0x00,0x00,0x27,0x84,0x89,0x18,
-0x0c,0x00,0x06,0xe5,0x00,0x00,0x00,0x00,0x0c,0x00,0x01,0x39,0x00,0x00,0x00,0x00,
-0x27,0x84,0x84,0x40,0x0c,0x00,0x13,0xd9,0x00,0x00,0x00,0x00,0x27,0x82,0x89,0x4c,
-0xaf,0x82,0x84,0x80,0x0c,0x00,0x00,0x5f,0x00,0x00,0x00,0x00,0x3c,0x03,0xb0,0x03,
-0x34,0x63,0x01,0x08,0x3c,0x04,0xb0,0x09,0x3c,0x05,0xb0,0x09,0x8c,0x66,0x00,0x00,
-0x34,0x84,0x01,0x68,0x34,0xa5,0x01,0x40,0x24,0x02,0xc8,0x80,0x24,0x03,0x00,0x0a,
-0xa4,0x82,0x00,0x00,0xa4,0xa3,0x00,0x00,0x3c,0x04,0xb0,0x03,0x8c,0x82,0x00,0x00,
-0x8f,0x85,0x84,0x40,0xaf,0x86,0x84,0x38,0x34,0x42,0x00,0x20,0xac,0x82,0x00,0x00,
-0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0x58,0x8c,0x43,0x00,0x00,0x2c,0xa4,0x00,0x11,
-0x34,0x63,0x01,0x00,0xac,0x43,0x00,0x00,0x10,0x80,0xff,0xfa,0x3c,0x02,0xb0,0x03,
-0x3c,0x03,0x80,0x01,0x00,0x05,0x10,0x80,0x24,0x63,0x02,0x00,0x00,0x43,0x10,0x21,
-0x8c,0x44,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x00,0x08,0x00,0x00,0x00,0x00,
-0x27,0x84,0x84,0x98,0x0c,0x00,0x26,0x8e,0x00,0x00,0x00,0x00,0x27,0x84,0x84,0x40,
-0x0c,0x00,0x14,0x52,0x00,0x00,0x00,0x00,0x93,0x83,0x81,0xf1,0x24,0x02,0x00,0x01,
-0x10,0x62,0x00,0x08,0x00,0x00,0x00,0x00,0x8f,0x85,0x84,0x40,0x8f,0x82,0x84,0x74,
-0x00,0x00,0x00,0x00,0x24,0x42,0x00,0x01,0xaf,0x82,0x84,0x74,0x08,0x00,0x15,0x9d,
-0x3c,0x02,0xb0,0x03,0x27,0x84,0x84,0x98,0x0c,0x00,0x27,0x0d,0x00,0x00,0x00,0x00,
-0x08,0x00,0x15,0xb6,0x00,0x00,0x00,0x00,0x27,0x84,0x84,0x98,0x0c,0x00,0x28,0xdd,
-0x00,0x00,0x00,0x00,0xa3,0x82,0x84,0x71,0x8f,0x82,0x84,0x74,0xaf,0x80,0x84,0x40,
-0x24,0x42,0x00,0x01,0xaf,0x82,0x84,0x74,0x08,0x00,0x15,0x9c,0x00,0x00,0x28,0x21,
-0x27,0x84,0x86,0x58,0x0c,0x00,0x19,0x5b,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0xff,
-0x14,0x40,0x00,0x05,0x3c,0x03,0xb0,0x05,0xaf,0x80,0x84,0x40,0xaf,0x80,0x84,0x44,
-0x08,0x00,0x15,0xb6,0x00,0x00,0x00,0x00,0x34,0x63,0x04,0x50,0x90,0x62,0x00,0x00,
-0x00,0x00,0x00,0x00,0x30,0x42,0x00,0xff,0xaf,0x82,0x84,0x6c,0x14,0x40,0x00,0x20,
-0x24,0x02,0x00,0x01,0x8f,0x84,0x84,0x48,0x00,0x00,0x00,0x00,0x10,0x82,0x00,0x20,
-0x3c,0x03,0xb0,0x09,0x34,0x63,0x01,0x60,0x90,0x62,0x00,0x00,0x00,0x00,0x00,0x00,
-0x30,0x42,0x00,0xff,0xaf,0x82,0x84,0x54,0x14,0x40,0x00,0x15,0x24,0x02,0x00,0x01,
-0x24,0x02,0x00,0x02,0x10,0x82,0x00,0x07,0x00,0x00,0x00,0x00,0x24,0x05,0x00,0x03,
-0x24,0x02,0x00,0x01,0xaf,0x82,0x84,0x44,0xaf,0x85,0x84,0x40,0x08,0x00,0x15,0xb6,
-0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x05,0x34,0x42,0x02,0x2e,0x90,0x43,0x00,0x00,
-0x00,0x00,0x00,0x00,0x30,0x63,0x00,0x01,0x30,0x63,0x00,0xff,0x00,0x60,0x10,0x21,
-0xa7,0x83,0x84,0x60,0x14,0x40,0xff,0xf1,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,
-0xaf,0x82,0x84,0x44,0xaf,0x80,0x84,0x40,0x08,0x00,0x15,0xb6,0x00,0x00,0x00,0x00,
-0x3c,0x03,0xb0,0x05,0x34,0x63,0x02,0x2c,0x8c,0x62,0x00,0x00,0x00,0x00,0x00,0x00,
-0x30,0x42,0x00,0xff,0xaf,0x82,0x84,0x5c,0x14,0x40,0xff,0xf5,0x24,0x02,0x00,0x01,
-0x08,0x00,0x15,0xe1,0x3c,0x03,0xb0,0x09,0x27,0x84,0x86,0x58,0x0c,0x00,0x1a,0xd1,
-0x00,0x00,0x00,0x00,0x83,0x82,0x84,0x70,0x00,0x00,0x00,0x00,0x14,0x40,0xff,0xec,
-0x24,0x02,0x00,0x02,0x3c,0x03,0xb0,0x05,0x34,0x63,0x04,0x50,0x90,0x62,0x00,0x00,
-0x00,0x00,0x00,0x00,0x30,0x42,0x00,0xff,0xaf,0x82,0x84,0x6c,0x14,0x40,0xff,0xe4,
-0x24,0x02,0x00,0x02,0x8f,0x84,0x84,0x48,0x24,0x02,0x00,0x01,0x10,0x82,0x00,0x12,
-0x24,0x02,0x00,0x02,0x10,0x82,0x00,0x04,0x00,0x00,0x00,0x00,0x24,0x05,0x00,0x04,
-0x08,0x00,0x15,0xed,0x24,0x02,0x00,0x02,0x3c,0x02,0xb0,0x05,0x34,0x42,0x02,0x2e,
-0x90,0x43,0x00,0x00,0x00,0x00,0x00,0x00,0x30,0x63,0x00,0x01,0x30,0x63,0x00,0xff,
-0x00,0x60,0x10,0x21,0xa7,0x83,0x84,0x60,0x14,0x40,0xff,0xf4,0x00,0x00,0x00,0x00,
-0x08,0x00,0x15,0xfc,0x24,0x02,0x00,0x02,0x3c,0x03,0xb0,0x05,0x34,0x63,0x02,0x2c,
-0x8c,0x62,0x00,0x00,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0xff,0xaf,0x82,0x84,0x5c,
-0x14,0x40,0xff,0xf7,0x00,0x00,0x00,0x00,0x08,0x00,0x16,0x1d,0x24,0x02,0x00,0x02,
-0x27,0x84,0x89,0x18,0x0c,0x00,0x0b,0x55,0x00,0x00,0x00,0x00,0x8f,0x83,0x84,0x44,
-0xaf,0x82,0x84,0x5c,0x38,0x64,0x00,0x02,0x00,0x04,0x18,0x0a,0xaf,0x83,0x84,0x44,
-0x14,0x40,0xff,0xad,0x24,0x05,0x00,0x05,0x8f,0x82,0x89,0x58,0xaf,0x80,0x84,0x40,
-0x10,0x40,0x00,0x02,0x24,0x04,0x00,0x01,0xaf,0x84,0x84,0x48,0x93,0x82,0x89,0x66,
-0x00,0x00,0x00,0x00,0x10,0x40,0xff,0x6c,0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x05,
-0x34,0x42,0x00,0x08,0x8c,0x43,0x00,0x00,0x3c,0x04,0x20,0x00,0x00,0x64,0x18,0x24,
-0x10,0x60,0xff,0x65,0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0xa0,
-0x8c,0x43,0x00,0x00,0x3c,0x04,0x80,0x00,0xaf,0x80,0x89,0x40,0x24,0x63,0x00,0x01,
-0xac,0x43,0x00,0x00,0x3c,0x01,0xb0,0x05,0xac,0x24,0x00,0x08,0xaf,0x80,0x89,0x3c,
-0xaf,0x80,0x89,0x44,0xaf,0x80,0x89,0x48,0xaf,0x80,0x89,0x54,0xaf,0x80,0x89,0x4c,
-0x08,0x00,0x15,0xb6,0x00,0x00,0x00,0x00,0x83,0x82,0x84,0x90,0x00,0x00,0x00,0x00,
-0x10,0x40,0x00,0x02,0x24,0x02,0x00,0x20,0xaf,0x82,0x84,0x5c,0x8f,0x85,0x84,0x5c,
-0x27,0x84,0x89,0x18,0x0c,0x00,0x0d,0x30,0x00,0x00,0x00,0x00,0x00,0x02,0x1e,0x00,
-0xa3,0x82,0x84,0x70,0xaf,0x80,0x84,0x5c,0x10,0x60,0xff,0x8e,0x00,0x00,0x00,0x00,
-0x3c,0x02,0xb0,0x05,0x34,0x42,0x02,0x2e,0x90,0x43,0x00,0x00,0x00,0x00,0x00,0x00,
-0x30,0x63,0x00,0x01,0x30,0x63,0x00,0xff,0x00,0x60,0x10,0x21,0xa7,0x83,0x84,0x60,
-0x10,0x40,0x00,0x04,0x24,0x04,0x00,0x02,0xaf,0x84,0x84,0x48,0x08,0x00,0x15,0xfd,
-0x00,0x00,0x00,0x00,0x08,0x00,0x15,0xee,0x24,0x05,0x00,0x06,0x27,0x84,0x84,0x40,
-0x27,0x85,0x89,0x18,0x0c,0x00,0x0d,0xfd,0x00,0x00,0x00,0x00,0x8f,0x82,0x84,0x64,
-0xaf,0x80,0x84,0x6c,0x14,0x40,0x00,0x19,0x00,0x40,0x18,0x21,0x8f,0x82,0x84,0x68,
-0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x15,0x24,0x02,0x00,0x02,0x8f,0x83,0x84,0x48,
-0x00,0x00,0x00,0x00,0x10,0x62,0x00,0x0b,0x3c,0x02,0x40,0x00,0x8f,0x83,0x84,0x44,
-0x24,0x02,0x00,0x01,0x10,0x62,0x00,0x02,0x24,0x05,0x00,0x03,0x24,0x05,0x00,0x06,
-0xaf,0x85,0x84,0x40,0x24,0x04,0x00,0x03,0xaf,0x84,0x84,0x48,0x08,0x00,0x15,0xb6,
-0x00,0x00,0x00,0x00,0x34,0x42,0x00,0x14,0x3c,0x01,0xb0,0x05,0xac,0x22,0x00,0x00,
-0xaf,0x80,0x84,0x40,0x08,0x00,0x16,0x96,0x24,0x04,0x00,0x03,0x10,0x60,0x00,0x10,
-0x00,0x00,0x00,0x00,0x27,0x85,0x89,0x18,0x27,0x84,0x84,0x40,0x0c,0x00,0x0e,0x21,
-0x00,0x00,0x00,0x00,0x8f,0x83,0x84,0x44,0x24,0x02,0x00,0x01,0xa3,0x80,0x84,0x70,
-0xaf,0x80,0x84,0x48,0x10,0x62,0x00,0x02,0x24,0x05,0x00,0x03,0x24,0x05,0x00,0x04,
-0xaf,0x85,0x84,0x40,0xaf,0x80,0x84,0x64,0x08,0x00,0x15,0xb6,0x00,0x00,0x00,0x00,
-0x83,0x82,0x84,0x90,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x04,0x00,0x00,0x00,0x00,
-0x27,0x84,0x89,0x18,0x0c,0x00,0x10,0x69,0x00,0x00,0x00,0x00,0x8f,0x82,0x84,0x44,
-0xa3,0x80,0x84,0x70,0xaf,0x80,0x84,0x40,0xaf,0x80,0x84,0x48,0x14,0x40,0x00,0x03,
-0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0xaf,0x82,0x84,0x44,0xaf,0x80,0x84,0x68,
-0x08,0x00,0x15,0xb6,0x00,0x00,0x00,0x00,0x27,0x84,0x84,0x40,0x27,0x85,0x89,0x18,
-0x0c,0x00,0x0e,0x21,0x00,0x00,0x00,0x00,0x8f,0x82,0x84,0x44,0xa3,0x80,0x84,0x70,
-0xaf,0x80,0x84,0x40,0xaf,0x80,0x84,0x48,0x14,0x40,0xfe,0xeb,0x00,0x00,0x00,0x00,
-0x24,0x02,0x00,0x02,0xaf,0x82,0x84,0x44,0x08,0x00,0x15,0xb6,0x00,0x00,0x00,0x00,
-0x27,0x84,0x89,0x18,0x0c,0x00,0x10,0x69,0x00,0x00,0x00,0x00,0x08,0x00,0x16,0xc6,
-0x00,0x00,0x00,0x00,0x27,0x84,0x84,0x98,0x0c,0x00,0x29,0x73,0x00,0x00,0x00,0x00,
-0x08,0x00,0x15,0xc5,0x00,0x00,0x00,0x00,0x0c,0x00,0x24,0x05,0x00,0x00,0x00,0x00,
-0x0c,0x00,0x26,0xff,0x00,0x00,0x00,0x00,0x0c,0x00,0x18,0x11,0x00,0x00,0x00,0x00,
-0x93,0x83,0xbc,0x18,0x00,0x00,0x00,0x00,0x14,0x60,0x00,0x2b,0x3c,0x02,0xb0,0x03,
-0x34,0x42,0x01,0x08,0x8c,0x44,0x00,0x00,0x8f,0x83,0xbc,0x10,0x8f,0x82,0xbc,0x14,
-0x00,0x83,0x18,0x23,0x00,0x43,0x10,0x2b,0x10,0x40,0x00,0x23,0x3c,0x02,0xb0,0x03,
-0x24,0x04,0x05,0xa0,0x34,0x42,0x01,0x18,0x8c,0x42,0x00,0x00,0x0c,0x00,0x06,0xd1,
-0x00,0x00,0x00,0x00,0x24,0x04,0x05,0xa4,0x0c,0x00,0x06,0xd1,0x00,0x02,0x84,0x02,
-0x30,0x51,0xff,0xff,0x24,0x04,0x05,0xa8,0x00,0x02,0x94,0x02,0x0c,0x00,0x06,0xd1,
-0x3a,0x10,0xff,0xff,0x3a,0x31,0xff,0xff,0x30,0x42,0xff,0xff,0x2e,0x10,0x00,0x01,
-0x2e,0x31,0x00,0x01,0x3a,0x52,0xff,0xff,0x02,0x11,0x80,0x25,0x2e,0x52,0x00,0x01,
-0x38,0x42,0xff,0xff,0x02,0x12,0x80,0x25,0x2c,0x42,0x00,0x01,0x02,0x02,0x80,0x25,
-0x16,0x00,0x00,0x02,0x24,0x04,0x00,0x02,0x00,0x00,0x20,0x21,0x0c,0x00,0x05,0x6e,
-0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x03,0x34,0x42,0x01,0x08,0x8c,0x43,0x00,0x00,
-0x00,0x00,0x00,0x00,0xaf,0x83,0xbc,0x10,0x0c,0x00,0x01,0xe9,0x00,0x00,0x00,0x00,
-0xaf,0x80,0x84,0x40,0xaf,0x80,0x84,0x74,0x08,0x00,0x15,0x9c,0x00,0x00,0x28,0x21,
-0x27,0x90,0xb4,0x00,0x24,0x11,0x00,0x12,0x8e,0x04,0x00,0x00,0x00,0x00,0x00,0x00,
-0x90,0x82,0x00,0x10,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x03,0x00,0x00,0x00,0x00,
-0x0c,0x00,0x18,0xd0,0x00,0x00,0x00,0x00,0x26,0x31,0xff,0xff,0x06,0x21,0xff,0xf6,
-0x26,0x10,0x00,0x04,0xaf,0x80,0x84,0x40,0x08,0x00,0x15,0xb7,0x00,0x00,0x28,0x21,
-0x3c,0x02,0xb0,0x03,0x34,0x42,0x01,0x08,0x8c,0x44,0x00,0x00,0x8f,0x82,0x84,0x38,
-0x00,0x04,0x19,0xc2,0x00,0x02,0x11,0xc2,0x10,0x62,0xff,0xf6,0x00,0x00,0x00,0x00,
-0x3c,0x02,0xb0,0x03,0x34,0x42,0x01,0x02,0x90,0x43,0x00,0x00,0x3c,0x12,0xb0,0x05,
-0xaf,0x84,0x84,0x38,0x30,0x63,0x00,0xff,0x00,0x03,0x11,0x40,0x00,0x43,0x10,0x23,
-0x00,0x02,0x10,0x80,0x00,0x43,0x10,0x21,0x00,0x02,0x99,0x00,0x00,0x00,0x88,0x21,
-0x36,0x52,0x02,0x2c,0x27,0x90,0xb4,0x00,0x8e,0x04,0x00,0x00,0x00,0x00,0x00,0x00,
-0x90,0x83,0x00,0x16,0x00,0x00,0x00,0x00,0x30,0x62,0x00,0x03,0x10,0x40,0x00,0x06,
-0x30,0x62,0x00,0x1c,0x14,0x40,0x00,0x04,0x00,0x00,0x00,0x00,0x8f,0x85,0x84,0x38,
-0x0c,0x00,0x1e,0x94,0x02,0x60,0x30,0x21,0x8e,0x42,0x00,0x00,0x00,0x00,0x00,0x00,
-0x30,0x42,0x00,0xff,0x14,0x40,0xff,0xd7,0x00,0x00,0x00,0x00,0x26,0x31,0x00,0x01,
-0x2a,0x22,0x00,0x13,0x14,0x40,0xff,0xec,0x26,0x10,0x00,0x04,0x08,0x00,0x17,0x21,
-0x00,0x00,0x00,0x00,0x8f,0x84,0x84,0x4c,0x27,0x85,0x89,0x18,0x0c,0x00,0x17,0xa4,
-0x00,0x00,0x00,0x00,0x8f,0x83,0x84,0x4c,0x24,0x02,0x00,0x04,0x14,0x62,0xfe,0xa5,
-0x00,0x00,0x00,0x00,0x08,0x00,0x15,0xee,0x24,0x05,0x00,0x05,0x3c,0x02,0xb0,0x03,
-0x34,0x42,0x00,0x3f,0x90,0x44,0x00,0x00,0x24,0x03,0x00,0x01,0x10,0x64,0x00,0x08,
-0x00,0x00,0x00,0x00,0x27,0x84,0x89,0x18,0x0c,0x00,0x24,0x2c,0x00,0x00,0x00,0x00,
-0x24,0x05,0x00,0x05,0xaf,0x85,0x84,0x40,0x08,0x00,0x15,0xb7,0x00,0x00,0x00,0x00,
-0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0x14,0x8c,0x44,0x00,0x00,0x0c,0x00,0x24,0x49,
-0x00,0x00,0x00,0x00,0x08,0x00,0x17,0x65,0x24,0x05,0x00,0x05,0x8f,0x82,0x89,0x4c,
-0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x0d,0x00,0x00,0x00,0x00,0x8f,0x84,0xb4,0x40,
-0xaf,0x80,0x89,0x4c,0x94,0x85,0x00,0x14,0x0c,0x00,0x1b,0x66,0x00,0x00,0x00,0x00,
-0x93,0x82,0x8b,0x71,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x02,0x10,0x40,0x00,0x03,
-0x00,0x00,0x00,0x00,0x0c,0x00,0x01,0x57,0x00,0x00,0x20,0x21,0x8f,0x84,0xb4,0x40,
-0x0c,0x00,0x18,0xd0,0x00,0x00,0x00,0x00,0x08,0x00,0x17,0x21,0x00,0x00,0x00,0x00,
-0x3c,0x02,0xff,0x90,0x27,0xbd,0xff,0xe8,0x00,0x80,0x18,0x21,0x34,0x42,0x00,0x01,
-0x27,0x84,0x89,0x18,0x10,0x62,0x00,0x05,0xaf,0xbf,0x00,0x10,0x8f,0xbf,0x00,0x10,
-0x00,0x00,0x00,0x00,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x18,0x0c,0x00,0x06,0xe5,
-0x00,0x00,0x00,0x00,0x27,0x84,0x86,0x58,0x0c,0x00,0x18,0x1f,0x00,0x00,0x00,0x00,
-0x27,0x84,0x84,0x40,0x0c,0x00,0x13,0xd9,0x00,0x00,0x00,0x00,0x08,0x00,0x17,0x8b,
-0x00,0x00,0x00,0x00,0x8f,0x82,0x89,0x58,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x05,
-0x00,0x00,0x18,0x21,0x8f,0x82,0x84,0x48,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x02,
-0x00,0x00,0x00,0x00,0x24,0x03,0x00,0x01,0x03,0xe0,0x00,0x08,0x00,0x60,0x10,0x21,
-0x27,0xbd,0xff,0xe0,0x3c,0x06,0xb0,0x03,0xaf,0xb1,0x00,0x14,0xaf,0xb0,0x00,0x10,
-0x34,0xc6,0x00,0x5f,0xaf,0xbf,0x00,0x18,0x90,0xc3,0x00,0x00,0x3c,0x07,0xb0,0x03,
-0x34,0xe7,0x00,0x5d,0x34,0x63,0x00,0x01,0x3c,0x09,0xb0,0x03,0x24,0x02,0x00,0x01,
-0xa0,0xc3,0x00,0x00,0x00,0x80,0x80,0x21,0xa0,0xe2,0x00,0x00,0x00,0xa0,0x88,0x21,
-0x35,0x29,0x00,0x5e,0x00,0xe0,0x40,0x21,0x24,0x04,0x00,0x01,0x91,0x22,0x00,0x00,
-0x91,0x03,0x00,0x00,0x30,0x42,0x00,0x01,0x14,0x83,0x00,0x03,0x30,0x42,0x00,0x01,
-0x14,0x40,0xff,0xfa,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x04,0x12,0x02,0x00,0x2c,
-0x24,0x05,0x0f,0x00,0x24,0x02,0x00,0x06,0x12,0x02,0x00,0x08,0x24,0x05,0x00,0x0f,
-0x3c,0x02,0xb0,0x03,0x34,0x42,0x02,0x00,0xa0,0x50,0x00,0x00,0x8f,0xbf,0x00,0x18,
-0x7b,0xb0,0x00,0xbc,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x20,0x24,0x04,0x0c,0x04,
-0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x0f,0x24,0x04,0x0d,0x04,0x24,0x05,0x00,0x0f,
-0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x0f,0x24,0x04,0x08,0x80,0x24,0x05,0x1e,0x00,
-0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x0f,0x24,0x04,0x08,0x8c,0x24,0x05,0x0f,0x00,
-0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x0f,0x24,0x04,0x08,0x24,0x3c,0x05,0x00,0x30,
-0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x02,0x24,0x04,0x08,0x2c,0x3c,0x05,0x00,0x30,
-0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x02,0x24,0x04,0x08,0x34,0x3c,0x05,0x00,0x30,
-0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x02,0x24,0x04,0x08,0x3c,0x3c,0x05,0x00,0x30,
-0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x02,0x08,0x00,0x17,0xc5,0x3c,0x02,0xb0,0x03,
-0x24,0x04,0x08,0x8c,0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x04,0x24,0x04,0x08,0x80,
-0x24,0x05,0x1e,0x00,0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x04,0x24,0x04,0x0c,0x04,
-0x24,0x05,0x00,0x0f,0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x04,0x24,0x04,0x0d,0x04,
-0x24,0x05,0x00,0x0f,0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x04,0x24,0x04,0x08,0x24,
-0x3c,0x05,0x00,0x30,0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x03,0x24,0x04,0x08,0x2c,
-0x3c,0x05,0x00,0x30,0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x03,0x24,0x04,0x08,0x34,
-0x3c,0x05,0x00,0x30,0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x02,0x3c,0x05,0x00,0x30,
-0x24,0x06,0x00,0x03,0x0c,0x00,0x13,0x5f,0x24,0x04,0x08,0x3c,0x02,0x20,0x20,0x21,
-0x24,0x05,0x00,0x14,0x0c,0x00,0x13,0xa4,0x24,0x06,0x01,0x07,0x08,0x00,0x17,0xc5,
-0x3c,0x02,0xb0,0x03,0x3c,0x03,0xb0,0x03,0x34,0x63,0x00,0x73,0x90,0x62,0x00,0x00,
-0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x02,0x14,0x40,0x00,0x04,0x00,0x00,0x00,0x00,
-0xa3,0x80,0x81,0x58,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,
-0xa3,0x82,0x81,0x58,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x3c,0x03,0xb0,0x03,
-0x3c,0x02,0x80,0x00,0x00,0x80,0x70,0x21,0x34,0x63,0x00,0x20,0x24,0x42,0x60,0x7c,
-0x3c,0x04,0xb0,0x03,0xac,0x62,0x00,0x00,0x34,0x84,0x00,0x30,0xad,0xc0,0x02,0xb8,
-0x8c,0x83,0x00,0x00,0x24,0x02,0x00,0xff,0xa5,0xc0,0x00,0x0a,0x00,0x00,0x30,0x21,
-0xa7,0x82,0x8f,0xf0,0x27,0x88,0x90,0x00,0xa5,0xc3,0x00,0x08,0x3c,0x07,0xb0,0x08,
-0x30,0xc2,0xff,0xff,0x00,0x02,0x20,0xc0,0x24,0xc3,0x00,0x01,0x00,0x82,0x10,0x21,
-0x00,0x60,0x30,0x21,0x00,0x02,0x10,0x80,0x30,0x63,0xff,0xff,0x00,0x48,0x10,0x21,
-0x00,0x87,0x20,0x21,0x28,0xc5,0x00,0xff,0xac,0x83,0x00,0x00,0x14,0xa0,0xff,0xf4,
-0xa4,0x43,0x00,0x00,0x3c,0x02,0xb0,0x08,0x34,0x03,0xff,0xff,0x25,0xc4,0x00,0x0c,
-0x24,0x0a,0x00,0x02,0x34,0x42,0x07,0xf8,0x3c,0x06,0xb0,0x03,0xa7,0x83,0xb3,0xdc,
-0xac,0x43,0x00,0x00,0xaf,0x84,0xb4,0x00,0x34,0xc6,0x00,0x64,0xa0,0x8a,0x00,0x18,
-0x94,0xc5,0x00,0x00,0x8f,0x82,0xb4,0x00,0x25,0xc4,0x00,0x30,0x24,0x08,0x00,0x03,
-0x3c,0x03,0xb0,0x03,0xa0,0x45,0x00,0x21,0x34,0x63,0x00,0x66,0xaf,0x84,0xb4,0x04,
-0xa0,0x88,0x00,0x18,0x94,0x65,0x00,0x00,0x8f,0x82,0xb4,0x04,0x25,0xc4,0x00,0x54,
-0x25,0xc7,0x00,0x78,0xa0,0x45,0x00,0x21,0xaf,0x84,0xb4,0x08,0xa0,0x88,0x00,0x18,
-0x94,0x65,0x00,0x00,0x8f,0x82,0xb4,0x08,0x25,0xc8,0x00,0x9c,0x24,0x09,0x00,0x01,
-0xa0,0x45,0x00,0x21,0xaf,0x87,0xb4,0x0c,0xa0,0xea,0x00,0x18,0x94,0xc4,0x00,0x00,
-0x8f,0x82,0xb4,0x0c,0x3c,0x03,0xb0,0x03,0x34,0x63,0x00,0x62,0xa0,0x44,0x00,0x21,
-0xaf,0x88,0xb4,0x10,0xa1,0x09,0x00,0x18,0x94,0x65,0x00,0x00,0x8f,0x82,0xb4,0x10,
-0x25,0xc4,0x00,0xc0,0x3c,0x06,0xb0,0x03,0xa0,0x45,0x00,0x21,0xaf,0x84,0xb4,0x14,
-0xa0,0x89,0x00,0x18,0x94,0x65,0x00,0x00,0x8f,0x82,0xb4,0x14,0x25,0xc4,0x00,0xe4,
-0x34,0xc6,0x00,0x60,0xa0,0x45,0x00,0x21,0xaf,0x84,0xb4,0x18,0xa0,0x80,0x00,0x18,
-0x94,0xc5,0x00,0x00,0x8f,0x82,0xb4,0x18,0x25,0xc3,0x01,0x08,0x25,0xc7,0x01,0x2c,
-0xa0,0x45,0x00,0x21,0xaf,0x83,0xb4,0x1c,0xa0,0x60,0x00,0x18,0x94,0xc8,0x00,0x00,
-0x8f,0x82,0xb4,0x1c,0x25,0xc4,0x01,0x50,0x25,0xc5,0x01,0x74,0xa0,0x48,0x00,0x21,
-0x25,0xc6,0x01,0x98,0x25,0xc9,0x01,0xbc,0x25,0xca,0x01,0xe0,0x25,0xcb,0x02,0x04,
-0x25,0xcc,0x02,0x28,0x25,0xcd,0x02,0x4c,0x24,0x02,0x00,0x10,0x3c,0x03,0xb0,0x03,
-0xaf,0x87,0xb4,0x20,0x34,0x63,0x00,0x38,0xa0,0xe0,0x00,0x18,0xaf,0x84,0xb4,0x24,
-0xa0,0x80,0x00,0x18,0xaf,0x85,0xb4,0x28,0xa0,0xa0,0x00,0x18,0xaf,0x86,0xb4,0x2c,
-0xa0,0xc0,0x00,0x18,0xaf,0x89,0xb4,0x30,0xa1,0x20,0x00,0x18,0xaf,0x8a,0xb4,0x34,
-0xa1,0x40,0x00,0x18,0xaf,0x8b,0xb4,0x38,0xa1,0x60,0x00,0x18,0xaf,0x8c,0xb4,0x3c,
-0xa1,0x80,0x00,0x18,0xaf,0x8d,0xb4,0x40,0xa1,0xa2,0x00,0x18,0x94,0x64,0x00,0x00,
-0x8f,0x82,0xb4,0x40,0x25,0xc5,0x02,0x70,0x3c,0x03,0xb0,0x03,0xa0,0x44,0x00,0x21,
-0x24,0x02,0x00,0x11,0xaf,0x85,0xb4,0x44,0x34,0x63,0x00,0x6e,0xa0,0xa2,0x00,0x18,
-0x94,0x64,0x00,0x00,0x8f,0x82,0xb4,0x44,0x25,0xc5,0x02,0x94,0x3c,0x03,0xb0,0x03,
-0xa0,0x44,0x00,0x21,0x24,0x02,0x00,0x12,0xaf,0x85,0xb4,0x48,0x34,0x63,0x00,0x6c,
-0xa0,0xa2,0x00,0x18,0x94,0x64,0x00,0x00,0x8f,0x82,0xb4,0x48,0x24,0x05,0xff,0xff,
-0x24,0x07,0x00,0x01,0xa0,0x44,0x00,0x21,0x24,0x06,0x00,0x12,0x27,0x84,0xb4,0x00,
-0x8c,0x82,0x00,0x00,0x24,0xc6,0xff,0xff,0xa0,0x40,0x00,0x04,0x8c,0x83,0x00,0x00,
-0xa4,0x45,0x00,0x00,0xa4,0x45,0x00,0x02,0xa0,0x60,0x00,0x0a,0x8c,0x82,0x00,0x00,
-0xa4,0x65,0x00,0x06,0xa4,0x65,0x00,0x08,0xa0,0x40,0x00,0x10,0x8c,0x83,0x00,0x00,
-0xa4,0x45,0x00,0x0c,0xa4,0x45,0x00,0x0e,0xa0,0x60,0x00,0x12,0x8c,0x82,0x00,0x00,
-0x00,0x00,0x00,0x00,0xa0,0x40,0x00,0x16,0x8c,0x83,0x00,0x00,0xa4,0x45,0x00,0x14,
-0xa0,0x67,0x00,0x17,0x8c,0x82,0x00,0x00,0x24,0x84,0x00,0x04,0xa0,0x40,0x00,0x20,
-0x04,0xc1,0xff,0xe7,0xac,0x40,0x00,0x1c,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,
-0x3c,0x02,0xb0,0x03,0x3c,0x03,0x80,0x00,0x34,0x42,0x00,0x20,0x24,0x63,0x63,0x40,
-0xac,0x43,0x00,0x00,0x90,0x82,0x00,0x10,0x00,0x80,0x60,0x21,0x10,0x40,0x00,0x56,
-0x00,0x00,0x70,0x21,0x97,0x82,0x8f,0xf0,0x94,0x8a,0x00,0x0c,0x27,0x87,0x90,0x00,
-0x00,0x02,0x40,0xc0,0x01,0x02,0x10,0x21,0x00,0x02,0x10,0x80,0x00,0x47,0x10,0x21,
-0x90,0x8b,0x00,0x18,0xa4,0x4a,0x00,0x00,0x94,0x83,0x00,0x0e,0x39,0x64,0x00,0x10,
-0x2c,0x84,0x00,0x01,0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,0x34,0x85,0x00,0x02,
-0x39,0x63,0x00,0x11,0x00,0x83,0x28,0x0b,0x34,0xa3,0x00,0x08,0x39,0x64,0x00,0x12,
-0x00,0x02,0x10,0x80,0x00,0xa4,0x18,0x0b,0x00,0x47,0x10,0x21,0x94,0x49,0x00,0x04,
-0x34,0x64,0x00,0x20,0x00,0x6b,0x20,0x0b,0x34,0x83,0x00,0x40,0x39,0x62,0x00,0x01,
-0x00,0x82,0x18,0x0b,0x00,0x09,0x30,0xc0,0x34,0x64,0x00,0x80,0x00,0xc9,0x28,0x21,
-0x39,0x62,0x00,0x02,0x00,0x60,0x68,0x21,0x00,0x82,0x68,0x0a,0x00,0x05,0x28,0x80,
-0x3c,0x02,0xb0,0x08,0x00,0xa7,0x28,0x21,0x00,0xc2,0x30,0x21,0x01,0x02,0x40,0x21,
-0x34,0x03,0xff,0xff,0x35,0xa4,0x01,0x00,0x39,0x62,0x00,0x03,0x2d,0x67,0x00,0x13,
-0xad,0x0a,0x00,0x00,0xa4,0xa3,0x00,0x00,0xac,0xc3,0x00,0x00,0xa7,0x89,0x8f,0xf0,
-0x10,0xe0,0x00,0x0f,0x00,0x82,0x68,0x0a,0x3c,0x03,0x80,0x01,0x00,0x0b,0x10,0x80,
-0x24,0x63,0x02,0x44,0x00,0x43,0x10,0x21,0x8c,0x44,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x80,0x00,0x08,0x00,0x00,0x00,0x00,0x3c,0x03,0xb0,0x03,0x34,0x63,0x00,0x60,
-0x94,0x62,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x02,0x14,0x00,0x00,0x02,0x74,0x03,
-0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0x3a,0x94,0x44,0x00,0x00,0x93,0x83,0x8f,0xe4,
-0x91,0x82,0x00,0x21,0x01,0xc4,0x20,0x21,0x91,0x85,0x00,0x10,0x00,0x04,0x24,0x00,
-0x00,0x62,0x18,0x21,0x00,0x04,0x74,0x03,0x00,0x6e,0x18,0x23,0x00,0x65,0x10,0x2a,
-0x00,0xa2,0x18,0x0a,0x00,0x0d,0x24,0x00,0x3c,0x02,0xb0,0x06,0x24,0x05,0xff,0xff,
-0x00,0x64,0x18,0x25,0x34,0x42,0x80,0x20,0xac,0x43,0x00,0x00,0xa5,0x85,0x00,0x0e,
-0xa1,0x80,0x00,0x10,0xa5,0x85,0x00,0x0c,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,
-0x3c,0x03,0xb0,0x03,0x08,0x00,0x19,0x14,0x34,0x63,0x00,0x62,0x3c,0x03,0xb0,0x03,
-0x08,0x00,0x19,0x14,0x34,0x63,0x00,0x64,0x3c,0x03,0xb0,0x03,0x08,0x00,0x19,0x14,
-0x34,0x63,0x00,0x66,0x3c,0x03,0xb0,0x03,0x08,0x00,0x19,0x14,0x34,0x63,0x00,0x38,
-0x3c,0x03,0xb0,0x03,0x08,0x00,0x19,0x14,0x34,0x63,0x00,0x6e,0x3c,0x03,0xb0,0x03,
-0x08,0x00,0x19,0x14,0x34,0x63,0x00,0x6c,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,
-0x34,0x63,0x00,0x20,0x24,0x42,0x65,0x08,0x00,0x05,0x28,0x40,0xac,0x62,0x00,0x00,
-0x00,0xa6,0x28,0x21,0x2c,0xe2,0x00,0x10,0x14,0x80,0x00,0x06,0x00,0x00,0x18,0x21,
-0x10,0x40,0x00,0x02,0x00,0x00,0x00,0x00,0x00,0xe0,0x18,0x21,0x03,0xe0,0x00,0x08,
-0x00,0x60,0x10,0x21,0x24,0x02,0x00,0x20,0x10,0xe2,0x00,0x06,0x2c,0xe4,0x00,0x10,
-0x24,0xa2,0x00,0x01,0x10,0x80,0xff,0xf9,0x00,0x02,0x11,0x00,0x08,0x00,0x19,0x4f,
-0x00,0x47,0x18,0x21,0x08,0x00,0x19,0x4f,0x24,0xa3,0x00,0x50,0x27,0xbd,0xff,0xc8,
-0xaf,0xb3,0x00,0x1c,0xaf,0xb2,0x00,0x18,0xaf,0xb1,0x00,0x14,0xaf,0xbf,0x00,0x30,
-0xaf,0xb7,0x00,0x2c,0xaf,0xb6,0x00,0x28,0xaf,0xb5,0x00,0x24,0xaf,0xb4,0x00,0x20,
-0xaf,0xb0,0x00,0x10,0x00,0x80,0x88,0x21,0x84,0x84,0x00,0x08,0x3c,0x05,0xb0,0x03,
-0x3c,0x02,0x80,0x00,0x34,0xa5,0x00,0x20,0x24,0x42,0x65,0x6c,0x3c,0x03,0xb0,0x06,
-0x00,0x04,0x20,0x80,0xac,0xa2,0x00,0x00,0x00,0x83,0x20,0x21,0x3c,0x06,0xb0,0x06,
-0x8c,0x82,0x00,0x00,0x34,0xc6,0x80,0x24,0x8c,0x88,0x00,0x00,0x8c,0xc4,0x00,0x00,
-0x96,0x25,0x00,0x08,0x30,0x52,0xff,0xff,0x00,0x08,0x44,0x02,0x34,0x84,0x01,0x00,
-0x3c,0x02,0xb0,0x00,0x00,0x08,0x18,0xc0,0x00,0x12,0x3a,0x00,0xac,0xc4,0x00,0x00,
-0x00,0xe2,0x38,0x21,0xae,0x32,0x02,0xb8,0x00,0x68,0x18,0x21,0x24,0xa5,0x00,0x02,
-0x8c,0xf6,0x00,0x00,0x30,0xa5,0x01,0xff,0x8c,0xf4,0x00,0x04,0x27,0x86,0x90,0x00,
-0x00,0x03,0x18,0x80,0x00,0x12,0x98,0xc0,0xa6,0x25,0x00,0x08,0x00,0x66,0x18,0x21,
-0x02,0x72,0x10,0x21,0x94,0x65,0x00,0x00,0x00,0x02,0x48,0x80,0x01,0x26,0x30,0x21,
-0x24,0x02,0xff,0xff,0x00,0x14,0x1a,0x02,0x27,0x84,0x90,0x10,0xa4,0xc2,0x00,0x02,
-0x30,0x63,0x00,0x1f,0x24,0x02,0x00,0x10,0x01,0x24,0x20,0x21,0xa4,0xc8,0x00,0x04,
-0x8c,0xf0,0x00,0x08,0xa6,0x23,0x00,0x06,0xa6,0x25,0x00,0x0a,0xa0,0x82,0x00,0x06,
-0x86,0x25,0x00,0x06,0x27,0x82,0x90,0x04,0x01,0x22,0x10,0x21,0x24,0x03,0x00,0x13,
-0x10,0xa3,0x00,0xee,0xac,0x47,0x00,0x18,0x3c,0x03,0xb0,0x03,0x34,0x63,0x01,0x00,
-0xa6,0x20,0x00,0x02,0x3c,0x02,0xb0,0x03,0x90,0x64,0x00,0x00,0x34,0x42,0x01,0x08,
-0x8c,0x45,0x00,0x00,0x00,0x10,0x1b,0xc2,0x00,0x04,0x20,0x82,0x30,0x63,0x00,0x01,
-0xac,0xc5,0x00,0x08,0x10,0x60,0x00,0xc7,0x30,0x97,0x00,0x01,0x00,0x10,0x16,0x82,
-0x30,0x46,0x00,0x01,0x00,0x10,0x12,0x02,0x00,0x10,0x19,0xc2,0x00,0x10,0x26,0x02,
-0x00,0x10,0x2e,0x42,0x30,0x48,0x00,0x7f,0x24,0x02,0x00,0x01,0x30,0x75,0x00,0x01,
-0x30,0x84,0x00,0x01,0x10,0xc2,0x00,0xb3,0x30,0xa3,0x00,0x01,0x00,0x60,0x28,0x21,
-0x0c,0x00,0x19,0x42,0x01,0x00,0x38,0x21,0x02,0x72,0x18,0x21,0x00,0x03,0x18,0x80,
-0x2c,0x46,0x00,0x54,0x27,0x85,0x90,0x10,0x27,0x84,0x90,0x08,0x00,0x06,0x10,0x0a,
-0x00,0x65,0x28,0x21,0x26,0xa6,0x00,0x02,0x00,0x64,0x18,0x21,0xa0,0xa2,0x00,0x02,
-0xa0,0x66,0x00,0x06,0xa0,0x62,0x00,0x07,0xa0,0xa2,0x00,0x01,0x02,0x72,0x28,0x21,
-0x00,0x05,0x28,0x80,0x27,0x82,0x90,0x04,0x00,0xa2,0x58,0x21,0x8d,0x64,0x00,0x18,
-0x00,0x10,0x15,0xc2,0x30,0x42,0x00,0x01,0x8c,0x83,0x00,0x0c,0x27,0x84,0x90,0x20,
-0x00,0xa4,0x48,0x21,0xa6,0x22,0x00,0x00,0xa6,0x36,0x00,0x04,0x8d,0x26,0x00,0x00,
-0x00,0x03,0x19,0x42,0x3c,0x02,0xff,0xef,0x34,0x42,0xff,0xff,0x30,0x63,0x00,0x01,
-0x00,0xc2,0x40,0x24,0x00,0x03,0x1d,0x00,0x01,0x03,0x40,0x25,0x00,0x08,0x15,0x02,
-0x00,0x14,0x19,0x82,0x00,0x14,0x25,0x82,0x00,0x10,0x34,0x42,0x00,0x10,0x3c,0x82,
-0x00,0x10,0x2c,0x02,0x30,0x42,0x00,0x01,0x30,0xcd,0x00,0x01,0x30,0x6c,0x00,0x01,
-0x30,0xe6,0x00,0x01,0x30,0x8a,0x00,0x03,0x32,0x94,0x00,0x07,0x30,0xa5,0x00,0x01,
-0xad,0x28,0x00,0x00,0x10,0x40,0x00,0x0b,0x32,0x07,0x00,0x7f,0x8d,0x64,0x00,0x18,
-0x3c,0x03,0xff,0xf0,0x34,0x63,0xff,0xff,0x8c,0x82,0x00,0x0c,0x01,0x03,0x18,0x24,
-0x00,0x02,0x13,0x82,0x30,0x42,0x00,0x0f,0x00,0x02,0x14,0x00,0x00,0x62,0x18,0x25,
-0xad,0x23,0x00,0x00,0x24,0x02,0x00,0x01,0x10,0xc2,0x00,0x6a,0x00,0x00,0x00,0x00,
-0x15,0x80,0x00,0x03,0x00,0x00,0x00,0x00,0x15,0x40,0x00,0x5b,0x24,0x02,0x00,0x01,
-0x96,0x22,0x00,0x04,0x00,0x00,0x00,0x00,0x24,0x42,0x00,0x04,0xa6,0x22,0x00,0x04,
-0x00,0xa0,0x20,0x21,0x0c,0x00,0x19,0x42,0x01,0xa0,0x28,0x21,0x02,0x72,0x18,0x21,
-0x00,0x03,0x40,0x80,0x2c,0x45,0x00,0x54,0x27,0x84,0x90,0x10,0x01,0x04,0x20,0x21,
-0x00,0x05,0x10,0x0a,0xa0,0x82,0x00,0x00,0xa0,0x80,0x00,0x04,0xa0,0x80,0x00,0x05,
-0x96,0x23,0x00,0x04,0x27,0x82,0x90,0x00,0x01,0x02,0x10,0x21,0xa4,0x43,0x00,0x06,
-0x27,0x82,0x90,0x04,0x92,0x26,0x00,0x01,0x01,0x02,0x10,0x21,0x8c,0x45,0x00,0x18,
-0x27,0x83,0x90,0x20,0x01,0x03,0x18,0x21,0xa0,0x60,0x00,0x00,0xa0,0x86,0x00,0x07,
-0x94,0xa2,0x00,0x10,0x24,0x03,0x00,0x04,0x30,0x42,0x00,0x0f,0x10,0x43,0x00,0x36,
-0x24,0xa5,0x00,0x10,0x94,0xa3,0x00,0x16,0x27,0x87,0x90,0x18,0x01,0x07,0x10,0x21,
-0xa4,0x43,0x00,0x02,0x94,0xa2,0x00,0x04,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x01,
-0x14,0x40,0x00,0x24,0x02,0x72,0x20,0x21,0x94,0xa2,0x00,0x00,0x24,0x03,0x00,0xa4,
-0x30,0x42,0x00,0xff,0x10,0x43,0x00,0x1f,0x00,0x00,0x00,0x00,0x94,0xa2,0x00,0x00,
-0x24,0x03,0x00,0x88,0x30,0x42,0x00,0x88,0x10,0x43,0x00,0x14,0x02,0x72,0x18,0x21,
-0x27,0x84,0x90,0x20,0x00,0x03,0x18,0x80,0x00,0x64,0x18,0x21,0x8c,0x62,0x00,0x00,
-0x3c,0x04,0x00,0x80,0x00,0x44,0x10,0x25,0xac,0x62,0x00,0x00,0x02,0x72,0x10,0x21,
-0x00,0x02,0x10,0x80,0x00,0x47,0x10,0x21,0xa0,0x54,0x00,0x00,0x8f,0xbf,0x00,0x30,
-0x7b,0xb6,0x01,0x7c,0x7b,0xb4,0x01,0x3c,0x7b,0xb2,0x00,0xfc,0x7b,0xb0,0x00,0xbc,
-0x24,0x02,0x00,0x01,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x38,0x94,0xa2,0x00,0x18,
-0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x60,0x10,0x40,0xff,0xe9,0x02,0x72,0x18,0x21,
-0x02,0x72,0x20,0x21,0x27,0x82,0x90,0x20,0x00,0x04,0x20,0x80,0x00,0x82,0x20,0x21,
-0x8c,0x83,0x00,0x00,0x3c,0x02,0xff,0x7f,0x34,0x42,0xff,0xff,0x00,0x62,0x18,0x24,
-0x08,0x00,0x1a,0x37,0xac,0x83,0x00,0x00,0x27,0x87,0x90,0x18,0x01,0x07,0x10,0x21,
-0x08,0x00,0x1a,0x21,0xa4,0x40,0x00,0x02,0x11,0x42,0x00,0x07,0x00,0x00,0x00,0x00,
-0x2d,0x42,0x00,0x02,0x14,0x40,0xff,0xa7,0x00,0xa0,0x20,0x21,0x96,0x22,0x00,0x04,
-0x08,0x00,0x19,0xff,0x24,0x42,0x00,0x0c,0x96,0x22,0x00,0x04,0x08,0x00,0x19,0xff,
-0x24,0x42,0x00,0x08,0x16,0xe6,0xff,0x96,0x3c,0x02,0xff,0xfb,0x8d,0x63,0x00,0x18,
-0x34,0x42,0xff,0xff,0x02,0x02,0x10,0x24,0xac,0x62,0x00,0x08,0x08,0x00,0x19,0xf8,
-0x00,0x00,0x30,0x21,0x16,0xe6,0xff,0x4e,0x00,0x60,0x28,0x21,0x3c,0x02,0xfb,0xff,
-0x34,0x42,0xff,0xff,0x02,0x02,0x10,0x24,0xac,0xe2,0x00,0x08,0x08,0x00,0x19,0xb7,
-0x00,0x00,0x30,0x21,0x93,0x87,0xbb,0x14,0x00,0x10,0x1e,0x42,0x00,0x10,0x26,0x82,
-0x27,0x82,0x90,0x08,0x2c,0xe5,0x00,0x0c,0x01,0x22,0x48,0x21,0x30,0x63,0x00,0x01,
-0x30,0x86,0x00,0x01,0x14,0xa0,0x00,0x06,0x00,0xe0,0x40,0x21,0x00,0x03,0x10,0x40,
-0x00,0x46,0x10,0x21,0x00,0x02,0x11,0x00,0x00,0xe2,0x10,0x21,0x24,0x48,0x00,0x04,
-0x02,0x72,0x10,0x21,0x00,0x02,0x10,0x80,0x27,0x84,0x90,0x10,0x27,0x83,0x90,0x08,
-0x00,0x44,0x20,0x21,0x00,0x43,0x10,0x21,0xa1,0x28,0x00,0x07,0xa0,0x40,0x00,0x06,
-0xa0,0x80,0x00,0x02,0x08,0x00,0x19,0xc7,0xa0,0x80,0x00,0x01,0x24,0x02,0x00,0x01,
-0xa6,0x22,0x00,0x02,0x0c,0x00,0x01,0xc2,0x00,0xe0,0x20,0x21,0x08,0x00,0x1a,0x3b,
-0x00,0x00,0x00,0x00,0x30,0xa7,0xff,0xff,0x00,0x07,0x18,0xc0,0x00,0x67,0x18,0x21,
-0x3c,0x06,0xb0,0x03,0x3c,0x02,0x80,0x00,0x24,0x42,0x6a,0x44,0x27,0x85,0x90,0x10,
-0x00,0x03,0x18,0x80,0x34,0xc6,0x00,0x20,0x00,0x65,0x18,0x21,0xac,0xc2,0x00,0x00,
-0x80,0x62,0x00,0x07,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x29,0x00,0x80,0x28,0x21,
-0x90,0x82,0x00,0x16,0x00,0x00,0x00,0x00,0x34,0x42,0x00,0x02,0x30,0x43,0x00,0x01,
-0x14,0x60,0x00,0x02,0xa0,0x82,0x00,0x16,0xa0,0x80,0x00,0x17,0x90,0xa2,0x00,0x04,
-0x3c,0x03,0xb0,0x03,0x27,0x86,0x90,0x00,0x14,0x40,0x00,0x06,0x34,0x63,0x00,0x20,
-0x24,0x02,0x00,0x01,0xa0,0xa2,0x00,0x04,0xa4,0xa7,0x00,0x02,0x03,0xe0,0x00,0x08,
-0xa4,0xa7,0x00,0x00,0x94,0xa4,0x00,0x02,0x3c,0x02,0x80,0x01,0x24,0x42,0x82,0x6c,
-0xac,0x62,0x00,0x00,0x00,0x04,0x18,0xc0,0x00,0x64,0x18,0x21,0x00,0x03,0x18,0x80,
-0x00,0x66,0x18,0x21,0x94,0x62,0x00,0x04,0xa4,0x67,0x00,0x02,0x3c,0x03,0xb0,0x08,
-0x00,0x02,0x20,0xc0,0x00,0x82,0x10,0x21,0x00,0x02,0x10,0x80,0x00,0x46,0x10,0x21,
-0x00,0x83,0x20,0x21,0xa4,0x47,0x00,0x00,0xac,0x87,0x00,0x00,0x90,0xa2,0x00,0x04,
-0xa4,0xa7,0x00,0x02,0x24,0x42,0x00,0x01,0x03,0xe0,0x00,0x08,0xa0,0xa2,0x00,0x04,
-0x90,0x82,0x00,0x16,0x24,0x85,0x00,0x06,0x34,0x42,0x00,0x01,0x30,0x43,0x00,0x02,
-0x14,0x60,0xff,0xda,0xa0,0x82,0x00,0x16,0x24,0x02,0x00,0x01,0x08,0x00,0x1a,0xa7,
-0xa0,0x82,0x00,0x17,0x27,0xbd,0xff,0xe8,0xaf,0xbf,0x00,0x10,0x00,0x80,0x38,0x21,
-0x84,0x84,0x00,0x02,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,0x3c,0x0a,0xb0,0x06,
-0x34,0x63,0x00,0x20,0x24,0x42,0x6b,0x44,0x3c,0x0b,0xb0,0x08,0x27,0x89,0x90,0x00,
-0x34,0x0c,0xff,0xff,0x35,0x4a,0x80,0x20,0x10,0x80,0x00,0x30,0xac,0x62,0x00,0x00,
-0x97,0x82,0x8f,0xf0,0x94,0xe6,0x02,0xba,0x00,0x02,0x18,0xc0,0x00,0x6b,0x28,0x21,
-0xac,0xa6,0x00,0x00,0x8c,0xe4,0x02,0xb8,0x00,0x62,0x18,0x21,0x00,0x03,0x18,0x80,
-0x00,0x04,0x10,0xc0,0x00,0x44,0x10,0x21,0x00,0x02,0x10,0x80,0x00,0x49,0x10,0x21,
-0x94,0x48,0x00,0x04,0x00,0x69,0x18,0x21,0xa4,0x66,0x00,0x00,0x00,0x08,0x28,0xc0,
-0x00,0xab,0x10,0x21,0xac,0x4c,0x00,0x00,0x8c,0xe4,0x02,0xb8,0x27,0x82,0x90,0x04,
-0x00,0xa8,0x28,0x21,0x00,0x04,0x18,0xc0,0x00,0x64,0x18,0x21,0x00,0x03,0x18,0x80,
-0x00,0x62,0x10,0x21,0x8c,0x46,0x00,0x18,0x27,0x84,0x90,0x10,0x00,0x64,0x18,0x21,
-0x8c,0xc2,0x00,0x00,0x80,0x67,0x00,0x06,0x00,0x05,0x28,0x80,0x30,0x42,0xff,0xff,
-0x00,0x47,0x10,0x21,0x30,0x43,0x00,0xff,0x00,0x03,0x18,0x2b,0x00,0x02,0x12,0x02,
-0x00,0x43,0x10,0x21,0x3c,0x04,0x00,0x04,0x00,0xa9,0x28,0x21,0x00,0x44,0x10,0x25,
-0xa4,0xac,0x00,0x00,0xad,0x42,0x00,0x00,0xa7,0x88,0x8f,0xf0,0x8f,0xbf,0x00,0x10,
-0x00,0x00,0x00,0x00,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x18,0x84,0xe3,0x00,0x06,
-0x27,0x82,0xb4,0x00,0x94,0xe5,0x02,0xba,0x00,0x03,0x18,0x80,0x00,0x62,0x18,0x21,
-0x8c,0x64,0x00,0x00,0x0c,0x00,0x1a,0x91,0x00,0x00,0x00,0x00,0x08,0x00,0x1b,0x0b,
-0x00,0x00,0x00,0x00,0x94,0x88,0x00,0x00,0x00,0x80,0x58,0x21,0x27,0x8a,0x90,0x00,
-0x00,0x08,0x18,0xc0,0x00,0x68,0x18,0x21,0x3c,0x04,0xb0,0x03,0x00,0x03,0x18,0x80,
-0x3c,0x02,0x80,0x00,0x00,0x6a,0x18,0x21,0x34,0x84,0x00,0x20,0x24,0x42,0x6c,0x64,
-0x30,0xa5,0xff,0xff,0xac,0x82,0x00,0x00,0x94,0x67,0x00,0x02,0x11,0x05,0x00,0x35,
-0x24,0x04,0x00,0x01,0x91,0x66,0x00,0x04,0x00,0x00,0x00,0x00,0x00,0x86,0x10,0x2a,
-0x10,0x40,0x00,0x10,0x00,0xc0,0x48,0x21,0x3c,0x0d,0xb0,0x03,0x01,0x40,0x60,0x21,
-0x35,0xad,0x00,0x20,0x10,0xe5,0x00,0x0d,0x24,0x84,0x00,0x01,0x00,0x07,0x10,0xc0,
-0x00,0x47,0x10,0x21,0x00,0x02,0x10,0x80,0x01,0x20,0x30,0x21,0x00,0x4a,0x10,0x21,
-0x00,0x86,0x18,0x2a,0x00,0xe0,0x40,0x21,0x94,0x47,0x00,0x02,0x14,0x60,0xff,0xf5,
-0x00,0x00,0x00,0x00,0x03,0xe0,0x00,0x08,0x00,0x00,0x10,0x21,0x00,0x08,0x20,0xc0,
-0x00,0x88,0x20,0x21,0x24,0xc2,0xff,0xff,0x00,0x04,0x20,0x80,0xa1,0x62,0x00,0x04,
-0x00,0x8c,0x20,0x21,0x94,0x83,0x00,0x04,0x00,0x07,0x10,0xc0,0x00,0x47,0x10,0x21,
-0x00,0x02,0x10,0x80,0x00,0x4c,0x10,0x21,0x00,0x03,0x28,0xc0,0x94,0x46,0x00,0x02,
-0x00,0xa3,0x18,0x21,0x00,0x03,0x18,0x80,0x00,0x6c,0x18,0x21,0xa4,0x66,0x00,0x00,
-0xa4,0x86,0x00,0x02,0x95,0x64,0x00,0x02,0x3c,0x03,0xb0,0x08,0x3c,0x02,0x80,0x01,
-0x00,0xa3,0x28,0x21,0x24,0x42,0x82,0x6c,0xad,0xa2,0x00,0x00,0x10,0x87,0x00,0x03,
-0xac,0xa6,0x00,0x00,0x03,0xe0,0x00,0x08,0x24,0x02,0x00,0x01,0x08,0x00,0x1b,0x59,
-0xa5,0x68,0x00,0x02,0x91,0x62,0x00,0x04,0xa5,0x67,0x00,0x00,0x24,0x42,0xff,0xff,
-0x30,0x43,0x00,0xff,0x14,0x60,0xff,0xf7,0xa1,0x62,0x00,0x04,0x24,0x02,0xff,0xff,
-0x08,0x00,0x1b,0x59,0xa5,0x62,0x00,0x02,0x00,0x05,0x40,0xc0,0x01,0x05,0x30,0x21,
-0x27,0xbd,0xff,0xd8,0x00,0x06,0x30,0x80,0x27,0x82,0x90,0x04,0xaf,0xb2,0x00,0x18,
-0xaf,0xb1,0x00,0x14,0xaf,0xbf,0x00,0x20,0xaf,0xb3,0x00,0x1c,0xaf,0xb0,0x00,0x10,
-0x00,0xc2,0x10,0x21,0x8c,0x47,0x00,0x18,0x00,0xa0,0x90,0x21,0x3c,0x02,0x80,0x00,
-0x3c,0x05,0xb0,0x03,0x34,0xa5,0x00,0x20,0x24,0x42,0x6d,0x98,0xac,0xa2,0x00,0x00,
-0x27,0x83,0x90,0x10,0x00,0xc3,0x30,0x21,0x8c,0xe2,0x00,0x00,0x80,0xc5,0x00,0x06,
-0x00,0x80,0x88,0x21,0x30,0x42,0xff,0xff,0x00,0x45,0x10,0x21,0x30,0x43,0x00,0xff,
-0x10,0x60,0x00,0x02,0x00,0x02,0x12,0x02,0x24,0x42,0x00,0x01,0x30,0x53,0x00,0xff,
-0x01,0x12,0x10,0x21,0x00,0x02,0x10,0x80,0x27,0x83,0x90,0x10,0x00,0x43,0x10,0x21,
-0x80,0x44,0x00,0x07,0x00,0x00,0x00,0x00,0x10,0x80,0x00,0x4b,0x26,0x24,0x00,0x06,
-0x32,0x50,0xff,0xff,0x02,0x20,0x20,0x21,0x0c,0x00,0x1b,0x19,0x02,0x00,0x28,0x21,
-0x92,0x22,0x00,0x10,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x2e,0x3c,0x03,0xb0,0x08,
-0x3c,0x09,0x80,0x01,0x27,0x88,0x90,0x00,0xa6,0x32,0x00,0x0c,0x00,0x10,0x20,0xc0,
-0x00,0x90,0x20,0x21,0x00,0x04,0x20,0x80,0x00,0x88,0x20,0x21,0x94,0x82,0x00,0x04,
-0x3c,0x03,0xb0,0x08,0x3c,0x07,0xb0,0x03,0x00,0x02,0x28,0xc0,0x00,0xa2,0x10,0x21,
-0x00,0x02,0x10,0x80,0x00,0x48,0x10,0x21,0x00,0xa3,0x28,0x21,0x25,0x26,0x82,0x6c,
-0x34,0x03,0xff,0xff,0x34,0xe7,0x00,0x20,0xac,0xe6,0x00,0x00,0xa4,0x83,0x00,0x02,
-0xa4,0x43,0x00,0x00,0xac,0xa3,0x00,0x00,0x92,0x22,0x00,0x10,0x92,0x23,0x00,0x0a,
-0xa6,0x32,0x00,0x0e,0x02,0x62,0x10,0x21,0x14,0x60,0x00,0x05,0xa2,0x22,0x00,0x10,
-0x92,0x22,0x00,0x16,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0xfe,0xa2,0x22,0x00,0x16,
-0x92,0x22,0x00,0x04,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x05,0x00,0x00,0x00,0x00,
-0x92,0x22,0x00,0x16,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0xfd,0xa2,0x22,0x00,0x16,
-0x8f,0xbf,0x00,0x20,0x7b,0xb2,0x00,0xfc,0x7b,0xb0,0x00,0xbc,0x03,0xe0,0x00,0x08,
-0x27,0xbd,0x00,0x28,0x96,0x22,0x00,0x0e,0x27,0x88,0x90,0x00,0x00,0x02,0x20,0xc0,
-0x00,0x82,0x20,0x21,0x00,0x04,0x20,0x80,0x00,0x88,0x20,0x21,0x94,0x82,0x00,0x04,
-0x3c,0x06,0xb0,0x03,0x3c,0x09,0x80,0x01,0x00,0x02,0x28,0xc0,0x00,0xa2,0x10,0x21,
-0x00,0x02,0x10,0x80,0x00,0xa3,0x28,0x21,0x00,0x48,0x10,0x21,0x34,0xc6,0x00,0x20,
-0x25,0x23,0x82,0x6c,0xac,0xc3,0x00,0x00,0xa4,0x50,0x00,0x00,0xac,0xb0,0x00,0x00,
-0x08,0x00,0x1b,0x97,0xa4,0x90,0x00,0x02,0x08,0x00,0x1b,0x8e,0x32,0x50,0xff,0xff,
-0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,0x24,0x42,0x6f,0x60,0x34,0x63,0x00,0x20,
-0xac,0x62,0x00,0x00,0x90,0x82,0x00,0x04,0x97,0xaa,0x00,0x12,0x00,0x80,0x60,0x21,
-0x30,0xa8,0xff,0xff,0x00,0x4a,0x20,0x23,0x34,0x09,0xff,0xff,0x30,0xcf,0xff,0xff,
-0x30,0xee,0xff,0xff,0x11,0x09,0x00,0x73,0xa1,0x84,0x00,0x04,0x00,0x0e,0xc0,0xc0,
-0x00,0x08,0x10,0xc0,0x00,0x48,0x10,0x21,0x03,0x0e,0x20,0x21,0x27,0x8d,0x90,0x00,
-0x00,0x04,0x20,0x80,0x00,0x02,0x10,0x80,0x00,0x4d,0x10,0x21,0x00,0x8d,0x20,0x21,
-0x94,0x86,0x00,0x02,0x94,0x43,0x00,0x04,0x3c,0x19,0x80,0x01,0xa4,0x46,0x00,0x02,
-0x00,0x03,0x28,0xc0,0x00,0xa3,0x18,0x21,0x94,0x87,0x00,0x02,0x3c,0x02,0xb0,0x08,
-0x00,0x03,0x18,0x80,0x00,0xa2,0x28,0x21,0x00,0x6d,0x18,0x21,0x27,0x22,0x82,0x6c,
-0x3c,0x01,0xb0,0x03,0xac,0x22,0x00,0x20,0xa4,0x66,0x00,0x00,0x10,0xe9,0x00,0x57,
-0xac,0xa6,0x00,0x00,0x01,0xe0,0x30,0x21,0x11,0x40,0x00,0x1d,0x00,0x00,0x48,0x21,
-0x01,0x40,0x38,0x21,0x27,0x8b,0x90,0x04,0x27,0x8a,0x90,0x10,0x00,0x06,0x40,0xc0,
-0x01,0x06,0x18,0x21,0x00,0x03,0x18,0x80,0x00,0x6b,0x10,0x21,0x8c,0x44,0x00,0x18,
-0x00,0x6a,0x18,0x21,0x80,0x65,0x00,0x06,0x8c,0x82,0x00,0x00,0x00,0x00,0x00,0x00,
-0x30,0x42,0xff,0xff,0x00,0x45,0x10,0x21,0x30,0x44,0x00,0xff,0x00,0x02,0x12,0x02,
-0x01,0x22,0x18,0x21,0x24,0x62,0x00,0x01,0x14,0x80,0x00,0x02,0x30,0x49,0x00,0xff,
-0x30,0x69,0x00,0xff,0x01,0x06,0x10,0x21,0x00,0x02,0x10,0x80,0x00,0x4d,0x10,0x21,
-0x24,0xe7,0xff,0xff,0x94,0x46,0x00,0x02,0x14,0xe0,0xff,0xe9,0x00,0x06,0x40,0xc0,
-0x91,0x82,0x00,0x10,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x20,0x3c,0x06,0xb0,0x03,
-0xa5,0x8f,0x00,0x0c,0x03,0x0e,0x20,0x21,0x00,0x04,0x20,0x80,0x00,0x8d,0x20,0x21,
-0x94,0x82,0x00,0x04,0x3c,0x03,0xb0,0x08,0x3c,0x07,0xb0,0x03,0x00,0x02,0x28,0xc0,
-0x00,0xa2,0x10,0x21,0x00,0x02,0x10,0x80,0x00,0x4d,0x10,0x21,0x00,0xa3,0x28,0x21,
-0x27,0x26,0x82,0x6c,0x34,0x03,0xff,0xff,0x34,0xe7,0x00,0x20,0xac,0xe6,0x00,0x00,
-0xa4,0x83,0x00,0x02,0xa4,0x43,0x00,0x00,0xac,0xa3,0x00,0x00,0x91,0x82,0x00,0x10,
-0x91,0x83,0x00,0x04,0xa5,0x8e,0x00,0x0e,0x01,0x22,0x10,0x21,0x14,0x60,0x00,0x05,
-0xa1,0x82,0x00,0x10,0x91,0x82,0x00,0x16,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0xfd,
-0xa1,0x82,0x00,0x16,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x95,0x82,0x00,0x0e,
-0x3c,0x03,0xb0,0x08,0x00,0x02,0x20,0xc0,0x00,0x82,0x20,0x21,0x00,0x04,0x20,0x80,
-0x00,0x8d,0x20,0x21,0x94,0x82,0x00,0x04,0x34,0xc6,0x00,0x20,0x27,0x27,0x82,0x6c,
-0x00,0x02,0x28,0xc0,0x00,0xa2,0x10,0x21,0x00,0x02,0x10,0x80,0x00,0xa3,0x28,0x21,
-0x00,0x4d,0x10,0x21,0xac,0xc7,0x00,0x00,0xa4,0x8f,0x00,0x02,0xa4,0x4f,0x00,0x00,
-0xac,0xaf,0x00,0x00,0x08,0x00,0x1c,0x26,0x03,0x0e,0x20,0x21,0x08,0x00,0x1c,0x01,
-0xa5,0x88,0x00,0x02,0x00,0x0e,0xc0,0xc0,0x03,0x0e,0x10,0x21,0x00,0x02,0x10,0x80,
-0x27,0x8d,0x90,0x00,0x00,0x4d,0x10,0x21,0x94,0x43,0x00,0x02,0x30,0x84,0x00,0xff,
-0x14,0x80,0x00,0x05,0xa5,0x83,0x00,0x00,0x24,0x02,0xff,0xff,0x3c,0x19,0x80,0x01,
-0x08,0x00,0x1c,0x01,0xa5,0x82,0x00,0x02,0x08,0x00,0x1c,0x01,0x3c,0x19,0x80,0x01,
-0x3c,0x08,0xb0,0x03,0x3c,0x02,0x80,0x00,0x27,0xbd,0xff,0x78,0x35,0x08,0x00,0x20,
-0x24,0x42,0x71,0xa0,0xaf,0xb2,0x00,0x68,0xaf,0xb1,0x00,0x64,0xaf,0xb0,0x00,0x60,
-0xad,0x02,0x00,0x00,0xaf,0xbf,0x00,0x84,0xaf,0xbe,0x00,0x80,0xaf,0xb7,0x00,0x7c,
-0xaf,0xb6,0x00,0x78,0xaf,0xb5,0x00,0x74,0xaf,0xb4,0x00,0x70,0xaf,0xb3,0x00,0x6c,
-0xaf,0xa4,0x00,0x88,0x90,0x83,0x00,0x0a,0x27,0x82,0xb4,0x00,0xaf,0xa6,0x00,0x90,
-0x00,0x03,0x18,0x80,0x00,0x62,0x18,0x21,0x8c,0x63,0x00,0x00,0xaf,0xa7,0x00,0x94,
-0x27,0x86,0x90,0x04,0xaf,0xa3,0x00,0x1c,0x94,0x63,0x00,0x14,0x30,0xb1,0xff,0xff,
-0x24,0x08,0x00,0x01,0x00,0x03,0x20,0xc0,0xaf,0xa3,0x00,0x18,0x00,0x83,0x18,0x21,
-0xaf,0xa4,0x00,0x54,0x00,0x03,0x18,0x80,0x27,0x84,0x90,0x10,0x00,0x64,0x20,0x21,
-0x80,0x82,0x00,0x06,0x00,0x66,0x18,0x21,0x8c,0x66,0x00,0x18,0x24,0x42,0x00,0x02,
-0x00,0x02,0x1f,0xc2,0x8c,0xc4,0x00,0x08,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x43,
-0x00,0x02,0x10,0x40,0x00,0x04,0x2f,0xc2,0x00,0x04,0x1c,0x82,0x00,0xc2,0x38,0x21,
-0x00,0x04,0x24,0x42,0x8f,0xa2,0x00,0x1c,0x30,0x63,0x00,0x01,0x30,0x84,0x00,0x01,
-0xaf,0xa5,0x00,0x3c,0xaf,0xa3,0x00,0x34,0xaf,0xa4,0x00,0x38,0xaf,0xa0,0x00,0x40,
-0xaf,0xa0,0x00,0x44,0xaf,0xa0,0x00,0x50,0xaf,0xa8,0x00,0x20,0x80,0x42,0x00,0x12,
-0x8f,0xb2,0x00,0x18,0xaf,0xa2,0x00,0x28,0x8c,0xd0,0x00,0x0c,0x14,0xa0,0x01,0xe4,
-0x00,0x60,0x30,0x21,0x00,0x10,0x10,0x82,0x30,0x45,0x00,0x07,0x10,0xa0,0x00,0x11,
-0xaf,0xa0,0x00,0x30,0x8f,0xa4,0x00,0x98,0x27,0x82,0x80,0x1c,0x00,0x04,0x18,0x40,
-0x00,0x62,0x18,0x21,0x24,0xa2,0x00,0x06,0x8f,0xa5,0x00,0x20,0x94,0x64,0x00,0x00,
-0x00,0x45,0x10,0x04,0x00,0x44,0x00,0x1a,0x14,0x80,0x00,0x02,0x00,0x00,0x00,0x00,
-0x00,0x07,0x00,0x0d,0x00,0x00,0x10,0x12,0x24,0x42,0x00,0x20,0x30,0x42,0xff,0xfc,
-0xaf,0xa2,0x00,0x30,0x8f,0xa3,0x00,0x18,0x8f,0xa4,0x00,0x28,0x34,0x02,0xff,0xff,
-0xaf,0xa0,0x00,0x2c,0xaf,0xa2,0x00,0x48,0xaf,0xa3,0x00,0x4c,0x00,0x60,0xf0,0x21,
-0x00,0x00,0xb8,0x21,0x18,0x80,0x00,0x48,0xaf,0xa0,0x00,0x24,0x00,0x11,0x89,0x02,
-0xaf,0xb1,0x00,0x58,0x00,0x80,0xa8,0x21,0x00,0x12,0x10,0xc0,0x00,0x52,0x18,0x21,
-0x00,0x03,0x80,0x80,0x27,0x85,0x90,0x00,0x02,0x40,0x20,0x21,0x00,0x40,0xa0,0x21,
-0x02,0x05,0x10,0x21,0x94,0x56,0x00,0x02,0x0c,0x00,0x12,0x8b,0x00,0x00,0x28,0x21,
-0x90,0x42,0x00,0x00,0x24,0x03,0x00,0x08,0x30,0x42,0x00,0x0c,0x10,0x43,0x01,0x9e,
-0x24,0x04,0x00,0x01,0x24,0x02,0x00,0x01,0x10,0x82,0x01,0x7c,0x3c,0x02,0xb0,0x03,
-0x8f,0xa6,0x00,0x88,0x34,0x42,0x01,0x04,0x84,0xc5,0x00,0x0c,0x02,0x92,0x18,0x21,
-0x94,0x46,0x00,0x00,0x00,0x05,0x20,0xc0,0x00,0x85,0x20,0x21,0x00,0x03,0x18,0x80,
-0x27,0x82,0x90,0x10,0x27,0x85,0x90,0x08,0x00,0x65,0x28,0x21,0x00,0x62,0x18,0x21,
-0x80,0x71,0x00,0x05,0x80,0x73,0x00,0x04,0x8f,0xa3,0x00,0x88,0x30,0xd0,0xff,0xff,
-0x00,0x10,0x3a,0x03,0x32,0x08,0x00,0xff,0x27,0x82,0x90,0x20,0x00,0x04,0x20,0x80,
-0x80,0xa6,0x00,0x06,0x00,0x82,0x20,0x21,0xa4,0x67,0x00,0x44,0xa4,0x68,0x00,0x46,
-0x8c,0x84,0x00,0x00,0x38,0xc6,0x00,0x00,0x01,0x00,0x80,0x21,0x00,0x04,0x15,0x02,
-0x30,0x42,0x00,0x01,0x10,0x40,0x00,0x03,0x00,0xe6,0x80,0x0a,0x00,0x04,0x14,0x02,
-0x30,0x50,0x00,0x0f,0x12,0x20,0x01,0x50,0x02,0x40,0x20,0x21,0x02,0x71,0x10,0x21,
-0x00,0x50,0x10,0x2a,0x14,0x40,0x00,0xed,0x02,0x92,0x10,0x21,0x93,0x82,0x8b,0x71,
-0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x01,0x14,0x40,0x00,0xe0,0x02,0x92,0x28,0x21,
-0x26,0xe2,0x00,0x01,0x30,0x57,0xff,0xff,0x02,0x40,0xf0,0x21,0x26,0xb5,0xff,0xff,
-0x16,0xa0,0xff,0xbd,0x02,0xc0,0x90,0x21,0x16,0xe0,0x00,0xd0,0x00,0x00,0x00,0x00,
-0x8f,0xa3,0x00,0x98,0x00,0x00,0x00,0x00,0x2c,0x62,0x00,0x10,0x10,0x40,0x00,0x2e,
-0x00,0x00,0x00,0x00,0x8f,0xa4,0x00,0x24,0x00,0x00,0x00,0x00,0x18,0x80,0x00,0x2a,
-0x24,0x03,0x00,0x01,0x8f,0xa5,0x00,0x1c,0x27,0x84,0x90,0x04,0x94,0xb2,0x00,0x14,
-0xa0,0xa3,0x00,0x12,0x8f,0xa6,0x00,0x3c,0x00,0x12,0x10,0xc0,0x00,0x52,0x10,0x21,
-0x00,0x02,0x80,0x80,0x27,0x82,0x90,0x10,0x02,0x02,0x10,0x21,0x80,0x43,0x00,0x06,
-0x02,0x04,0x20,0x21,0x8c,0x85,0x00,0x18,0x24,0x63,0x00,0x02,0x00,0x03,0x17,0xc2,
-0x00,0x62,0x18,0x21,0x00,0x03,0x18,0x43,0x00,0x03,0x18,0x40,0x14,0xc0,0x00,0x0e,
-0x00,0xa3,0x38,0x21,0x27,0x82,0x90,0x00,0x02,0x02,0x10,0x21,0x94,0x43,0x00,0x06,
-0x8f,0xa8,0x00,0x1c,0x24,0x02,0x00,0x01,0xa5,0x03,0x00,0x1a,0x7b,0xbe,0x04,0x3c,
-0x7b,0xb6,0x03,0xfc,0x7b,0xb4,0x03,0xbc,0x7b,0xb2,0x03,0x7c,0x7b,0xb0,0x03,0x3c,
-0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x88,0x8f,0xa4,0x00,0x98,0x8f,0xa5,0x00,0x38,
-0x8f,0xa6,0x00,0x34,0xaf,0xa0,0x00,0x10,0x0c,0x00,0x09,0x0a,0xaf,0xa0,0x00,0x14,
-0x08,0x00,0x1d,0x2d,0x00,0x00,0x00,0x00,0x8f,0xa3,0x00,0x44,0x93,0x82,0x81,0x58,
-0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x61,0x30,0x69,0x00,0x03,0x8f,0xa4,0x00,0x24,
-0x8f,0xa5,0x00,0x28,0x00,0x00,0x00,0x00,0x00,0x85,0x10,0x2a,0x10,0x40,0x00,0x8f,
-0x00,0x00,0x00,0x00,0x8f,0xa6,0x00,0x1c,0x00,0x00,0x00,0x00,0x90,0xc4,0x00,0x04,
-0x00,0x00,0x00,0x00,0x30,0x83,0x00,0xff,0x00,0xa3,0x10,0x2a,0x10,0x40,0x00,0x87,
-0x00,0x00,0x00,0x00,0x8f,0xa8,0x00,0x24,0x00,0x00,0x00,0x00,0x11,0x00,0x00,0x83,
-0x00,0x65,0x10,0x23,0x00,0xa8,0x18,0x23,0x00,0x62,0x10,0x2a,0x14,0x40,0x00,0x7d,
-0x30,0x63,0x00,0xff,0x00,0x85,0x10,0x23,0x30,0x42,0x00,0xff,0xaf,0xa2,0x00,0x50,
-0x8f,0xa2,0x00,0x50,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x73,0x00,0x00,0xa8,0x21,
-0x27,0x8c,0x90,0x00,0x3c,0x0b,0x80,0xff,0x24,0x10,0x00,0x04,0x27,0x91,0x90,0x04,
-0x35,0x6b,0xff,0xff,0x3c,0x0d,0x7f,0x00,0x27,0x8e,0x90,0x10,0x01,0x80,0x78,0x21,
-0x00,0x12,0x30,0xc0,0x00,0xd2,0x10,0x21,0x00,0x02,0x10,0x80,0x00,0x4c,0x10,0x21,
-0x94,0x42,0x00,0x06,0x8f,0xa3,0x00,0x2c,0x8f,0xa4,0x00,0x30,0xaf,0xa2,0x00,0x44,
-0x8f,0xa5,0x00,0x44,0x30,0x49,0x00,0x03,0x02,0x09,0x10,0x23,0x30,0x42,0x00,0x03,
-0x00,0xa2,0x10,0x21,0x8f,0xa8,0x00,0x30,0x24,0x42,0x00,0x04,0x30,0x42,0xff,0xff,
-0x00,0x64,0x38,0x21,0x01,0x02,0x28,0x23,0x00,0x62,0x18,0x21,0x00,0x48,0x10,0x2b,
-0x10,0x40,0x00,0x52,0x00,0x00,0x20,0x21,0x30,0xe7,0xff,0xff,0x30,0xa4,0xff,0xff,
-0xaf,0xa7,0x00,0x2c,0x00,0xd2,0x10,0x21,0x00,0x02,0x10,0x80,0x00,0x51,0x18,0x21,
-0x8c,0x65,0x00,0x18,0x00,0x04,0x25,0x40,0x00,0x8d,0x20,0x24,0x8c,0xa8,0x00,0x04,
-0x00,0x4e,0x18,0x21,0x00,0x4f,0x50,0x21,0x01,0x0b,0x40,0x24,0x01,0x04,0x40,0x25,
-0xac,0xa8,0x00,0x04,0x8f,0xa4,0x00,0x98,0x8f,0xa2,0x00,0x50,0x26,0xb5,0x00,0x01,
-0xa0,0x64,0x00,0x00,0x8c,0xa4,0x00,0x08,0x00,0x00,0x00,0x00,0x04,0x81,0x00,0x0c,
-0x02,0xa2,0x30,0x2a,0x80,0x62,0x00,0x06,0x00,0x00,0x00,0x00,0x24,0x42,0x00,0x02,
-0x00,0x02,0x1f,0xc2,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x43,0x00,0x02,0x10,0x40,
-0x00,0xa2,0x38,0x21,0x8f,0xa5,0x00,0x40,0x00,0x00,0x00,0x00,0xa4,0xe5,0x00,0x00,
-0x95,0x52,0x00,0x02,0x14,0xc0,0xff,0xc7,0x00,0x12,0x30,0xc0,0x8f,0xa4,0x00,0x24,
-0x8f,0xa5,0x00,0x50,0x8f,0xa6,0x00,0x1c,0x8f,0xa3,0x00,0x2c,0x00,0x85,0x80,0x21,
-0xa0,0xd0,0x00,0x12,0x00,0x09,0x10,0x23,0x30,0x42,0x00,0x03,0x8f,0xa8,0x00,0x88,
-0x00,0x62,0x10,0x23,0xa4,0xc2,0x00,0x1a,0x85,0x03,0x00,0x0c,0x00,0x00,0x00,0x00,
-0x00,0x03,0x10,0xc0,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x80,0x27,0x83,0x90,0x04,
-0x00,0x43,0x10,0x21,0x8c,0x44,0x00,0x18,0x00,0x00,0x00,0x00,0x8c,0x83,0x00,0x04,
-0x00,0x00,0x00,0x00,0x30,0x63,0x00,0x10,0x14,0x60,0xff,0x74,0x02,0x00,0x10,0x21,
-0x8f,0xa3,0x00,0x54,0x8f,0xa4,0x00,0x18,0x8f,0xa5,0x00,0x24,0x00,0x64,0x10,0x21,
-0x00,0x02,0x10,0x80,0x27,0x83,0x90,0x18,0x00,0x43,0x10,0x21,0x90,0x44,0x00,0x00,
-0x10,0xa0,0x00,0x03,0x00,0x00,0x30,0x21,0x08,0x00,0x1d,0x33,0x02,0x00,0x10,0x21,
-0x93,0x82,0x80,0x10,0x00,0x00,0x28,0x21,0x00,0x00,0x38,0x21,0x0c,0x00,0x21,0x9a,
-0xaf,0xa2,0x00,0x10,0x08,0x00,0x1d,0x33,0x02,0x00,0x10,0x21,0x30,0x63,0xff,0xff,
-0x08,0x00,0x1d,0x85,0xaf,0xa3,0x00,0x2c,0x8f,0xa8,0x00,0x44,0x08,0x00,0x1d,0xa7,
-0x31,0x09,0x00,0x03,0x08,0x00,0x1d,0x60,0xaf,0xa3,0x00,0x50,0x8f,0xa6,0x00,0x44,
-0xaf,0xa0,0x00,0x50,0x08,0x00,0x1d,0xa7,0x30,0xc9,0x00,0x03,0x8f,0xa5,0x00,0x48,
-0x8f,0xa6,0x00,0x4c,0x8f,0xa4,0x00,0x1c,0x03,0xc0,0x38,0x21,0x0c,0x00,0x1b,0xd8,
-0xaf,0xb7,0x00,0x10,0x08,0x00,0x1d,0x10,0x00,0x00,0x00,0x00,0x00,0x05,0x28,0x80,
-0x27,0x82,0x90,0x00,0x00,0xa2,0x28,0x21,0x00,0x00,0x20,0x21,0x0c,0x00,0x01,0x49,
-0x00,0x00,0x00,0x00,0x08,0x00,0x1d,0x09,0x26,0xe2,0x00,0x01,0x00,0x02,0x80,0x80,
-0x27,0x83,0x90,0x10,0x8f,0xa4,0x00,0x1c,0x02,0x03,0x18,0x21,0x26,0x31,0x00,0x01,
-0x02,0x40,0x28,0x21,0x0c,0x00,0x1e,0xea,0xa0,0x71,0x00,0x05,0x14,0x40,0xff,0x13,
-0x00,0x00,0x00,0x00,0x16,0xe0,0x00,0x4d,0x03,0xc0,0x38,0x21,0x8f,0xa4,0x00,0x24,
-0x8f,0xa5,0x00,0x20,0x24,0x02,0x00,0x01,0x24,0x84,0x00,0x01,0xaf,0xb2,0x00,0x48,
-0xaf,0xb6,0x00,0x4c,0x02,0xc0,0xf0,0x21,0x10,0xa2,0x00,0x41,0xaf,0xa4,0x00,0x24,
-0x27,0x82,0x90,0x00,0x02,0x02,0x10,0x21,0x94,0x42,0x00,0x06,0x8f,0xa4,0x00,0x30,
-0xaf,0xa0,0x00,0x20,0xaf,0xa2,0x00,0x44,0x30,0x49,0x00,0x03,0x8f,0xa8,0x00,0x44,
-0x00,0x09,0x10,0x23,0x30,0x42,0x00,0x03,0x01,0x02,0x10,0x21,0x24,0x42,0x00,0x04,
-0x30,0x42,0xff,0xff,0x00,0x44,0x18,0x2b,0x10,0x60,0x00,0x2b,0x00,0x00,0x00,0x00,
-0x8f,0xa5,0x00,0x2c,0x00,0x82,0x10,0x23,0x00,0xa4,0x18,0x21,0x30,0x63,0xff,0xff,
-0x30,0x44,0xff,0xff,0xaf,0xa3,0x00,0x2c,0x02,0x92,0x28,0x21,0x00,0x05,0x28,0x80,
-0x27,0x82,0x90,0x04,0x00,0xa2,0x10,0x21,0x8c,0x46,0x00,0x18,0x3c,0x03,0x80,0xff,
-0x3c,0x02,0x7f,0x00,0x8c,0xc8,0x00,0x04,0x00,0x04,0x25,0x40,0x34,0x63,0xff,0xff,
-0x00,0x82,0x20,0x24,0x01,0x03,0x40,0x24,0x01,0x04,0x40,0x25,0xac,0xc8,0x00,0x04,
-0x8f,0xa8,0x00,0x98,0x27,0x82,0x90,0x10,0x00,0xa2,0x10,0x21,0xa0,0x48,0x00,0x00,
-0x8c,0xc4,0x00,0x08,0x00,0x00,0x00,0x00,0x00,0x04,0x27,0xc2,0x10,0x80,0xfe,0xdb,
-0xaf,0xa4,0x00,0x3c,0x80,0x42,0x00,0x06,0x00,0x00,0x00,0x00,0x24,0x42,0x00,0x02,
-0x00,0x02,0x1f,0xc2,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x43,0x00,0x02,0x10,0x40,
-0x00,0xc2,0x38,0x21,0x8f,0xa2,0x00,0x40,0x00,0x00,0x00,0x00,0xa4,0xe2,0x00,0x00,
-0x08,0x00,0x1d,0x0c,0x26,0xb5,0xff,0xff,0x8f,0xa6,0x00,0x2c,0x00,0x00,0x20,0x21,
-0x00,0xc2,0x10,0x21,0x30,0x42,0xff,0xff,0x08,0x00,0x1e,0x1a,0xaf,0xa2,0x00,0x2c,
-0x8f,0xa6,0x00,0x1c,0x08,0x00,0x1e,0x04,0xa4,0xd2,0x00,0x14,0x8f,0xa5,0x00,0x48,
-0x8f,0xa6,0x00,0x4c,0x8f,0xa4,0x00,0x1c,0x0c,0x00,0x1b,0xd8,0xaf,0xb7,0x00,0x10,
-0x08,0x00,0x1d,0xfb,0x00,0x00,0xb8,0x21,0x0c,0x00,0x12,0x8b,0x00,0x00,0x28,0x21,
-0x00,0x40,0x18,0x21,0x94,0x42,0x00,0x00,0x00,0x00,0x00,0x00,0x34,0x42,0x08,0x00,
-0xa4,0x62,0x00,0x00,0x08,0x00,0x1d,0x00,0x02,0x71,0x10,0x21,0x02,0x92,0x18,0x21,
-0x00,0x03,0x80,0x80,0x27,0x82,0x90,0x04,0x02,0x02,0x10,0x21,0x8c,0x44,0x00,0x18,
-0x00,0x00,0x00,0x00,0x8c,0x83,0x00,0x04,0x00,0x00,0x00,0x00,0x30,0x63,0x00,0x10,
-0x10,0x60,0x00,0x09,0x24,0x06,0x00,0x01,0x93,0x82,0x8b,0x71,0x00,0x00,0x00,0x00,
-0x30,0x42,0x00,0x01,0x10,0x40,0xfe,0xa2,0x3c,0x04,0x00,0x80,0x27,0x85,0x90,0x00,
-0x08,0x00,0x1d,0xeb,0x02,0x05,0x28,0x21,0x27,0x83,0x90,0x18,0x27,0x82,0x90,0x10,
-0x02,0x03,0x18,0x21,0x02,0x02,0x10,0x21,0x90,0x64,0x00,0x00,0x90,0x45,0x00,0x05,
-0x93,0x83,0x80,0x10,0x00,0x00,0x38,0x21,0x0c,0x00,0x21,0x9a,0xaf,0xa3,0x00,0x10,
-0x08,0x00,0x1e,0x62,0x00,0x00,0x00,0x00,0x27,0x82,0x90,0x18,0x02,0x02,0x10,0x21,
-0x94,0x43,0x00,0x02,0x8f,0xa6,0x00,0x58,0x00,0x03,0x19,0x02,0x00,0x66,0x18,0x23,
-0x30,0x63,0x0f,0xff,0x28,0x62,0x00,0x20,0x10,0x40,0x00,0x06,0x28,0x62,0x00,0x40,
-0x8f,0xa8,0x00,0x90,0x00,0x00,0x00,0x00,0x00,0x68,0x10,0x06,0x08,0x00,0x1c,0xd9,
-0x30,0x44,0x00,0x01,0x10,0x40,0x00,0x04,0x00,0x00,0x00,0x00,0x8f,0xa4,0x00,0x94,
-0x08,0x00,0x1e,0x83,0x00,0x64,0x10,0x06,0x08,0x00,0x1c,0xd9,0x00,0x00,0x20,0x21,
-0x8f,0xa4,0x00,0x98,0x8f,0xa5,0x00,0x38,0xaf,0xa0,0x00,0x10,0x0c,0x00,0x09,0x0a,
-0xaf,0xa8,0x00,0x14,0x30,0x42,0xff,0xff,0x08,0x00,0x1c,0xa9,0xaf,0xa2,0x00,0x40,
-0x3c,0x02,0xb0,0x03,0x3c,0x03,0x80,0x00,0x27,0xbd,0xff,0xe0,0x34,0x42,0x00,0x20,
-0x24,0x63,0x7a,0x50,0xaf,0xb1,0x00,0x14,0xaf,0xb0,0x00,0x10,0xaf,0xbf,0x00,0x18,
-0xac,0x43,0x00,0x00,0x90,0x82,0x00,0x0a,0x00,0x80,0x80,0x21,0x14,0x40,0x00,0x45,
-0x00,0x00,0x88,0x21,0x92,0x02,0x00,0x04,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x3c,
-0x00,0x00,0x00,0x00,0x12,0x20,0x00,0x18,0x00,0x00,0x00,0x00,0x92,0x02,0x00,0x16,
-0x92,0x05,0x00,0x0a,0x30,0x42,0x00,0xfc,0x10,0xa0,0x00,0x03,0xa2,0x02,0x00,0x16,
-0x34,0x42,0x00,0x01,0xa2,0x02,0x00,0x16,0x92,0x04,0x00,0x04,0x00,0x00,0x00,0x00,
-0x30,0x83,0x00,0xff,0x10,0x60,0x00,0x05,0x00,0x00,0x00,0x00,0x92,0x02,0x00,0x16,
-0x00,0x00,0x00,0x00,0x34,0x42,0x00,0x02,0xa2,0x02,0x00,0x16,0x10,0x60,0x00,0x0a,
-0x00,0x00,0x00,0x00,0x14,0xa0,0x00,0x08,0x00,0x00,0x00,0x00,0x96,0x02,0x00,0x00,
-0xa2,0x00,0x00,0x17,0xa6,0x02,0x00,0x14,0x8f,0xbf,0x00,0x18,0x7b,0xb0,0x00,0xbc,
-0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x20,0x14,0x80,0x00,0x05,0x24,0x02,0x00,0x01,
-0x96,0x03,0x00,0x06,0xa2,0x02,0x00,0x17,0x08,0x00,0x1e,0xbe,0xa6,0x03,0x00,0x14,
-0x96,0x04,0x00,0x00,0x96,0x05,0x00,0x06,0x27,0x86,0x90,0x00,0x00,0x04,0x10,0xc0,
-0x00,0x05,0x18,0xc0,0x00,0x44,0x10,0x21,0x00,0x65,0x18,0x21,0x00,0x02,0x10,0x80,
-0x00,0x03,0x18,0x80,0x00,0x66,0x18,0x21,0x00,0x46,0x10,0x21,0x8c,0x65,0x00,0x08,
-0x8c,0x44,0x00,0x08,0x0c,0x00,0x12,0x7c,0x00,0x00,0x00,0x00,0x30,0x43,0x00,0xff,
-0x10,0x60,0x00,0x04,0xa2,0x02,0x00,0x17,0x96,0x02,0x00,0x06,0x08,0x00,0x1e,0xbe,
-0xa6,0x02,0x00,0x14,0x96,0x02,0x00,0x00,0x08,0x00,0x1e,0xbe,0xa6,0x02,0x00,0x14,
-0x96,0x05,0x00,0x00,0x0c,0x00,0x1e,0xea,0x02,0x00,0x20,0x21,0x08,0x00,0x1e,0xa5,
-0x02,0x22,0x88,0x21,0x94,0x85,0x00,0x06,0x0c,0x00,0x1e,0xea,0x00,0x00,0x00,0x00,
-0x08,0x00,0x1e,0xa1,0x00,0x40,0x88,0x21,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x00,
-0x34,0x63,0x00,0x20,0x24,0x42,0x7b,0xa8,0x27,0xbd,0xff,0xf0,0xac,0x62,0x00,0x00,
-0x00,0x00,0x10,0x21,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x10,0x3c,0x03,0xb0,0x03,
-0x3c,0x02,0x80,0x00,0x34,0x63,0x00,0x20,0x24,0x42,0x7b,0xcc,0xac,0x62,0x00,0x00,
-0x90,0x89,0x00,0x0a,0x00,0x80,0x30,0x21,0x11,0x20,0x00,0x05,0x00,0xa0,0x50,0x21,
-0x90,0x82,0x00,0x17,0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x1b,0x00,0x00,0x00,0x00,
-0x90,0xc7,0x00,0x04,0x00,0x00,0x00,0x00,0x10,0xe0,0x00,0x1b,0x00,0x00,0x00,0x00,
-0x94,0xc8,0x00,0x00,0x27,0x83,0x90,0x00,0x93,0x85,0x8b,0x70,0x00,0x08,0x10,0xc0,
-0x00,0x48,0x10,0x21,0x00,0x02,0x10,0x80,0x00,0x43,0x10,0x21,0x8c,0x44,0x00,0x08,
-0x00,0xe5,0x28,0x2b,0x10,0xa0,0x00,0x06,0x01,0x44,0x18,0x23,0x8f,0x82,0x8b,0x88,
-0x00,0x00,0x00,0x00,0x00,0x43,0x10,0x2b,0x10,0x40,0x00,0x05,0x00,0x00,0x00,0x00,
-0x24,0x03,0x00,0x10,0xa4,0xc8,0x00,0x14,0x03,0xe0,0x00,0x08,0x00,0x60,0x10,0x21,
-0x11,0x20,0x00,0x05,0x00,0x00,0x00,0x00,0x94,0xc2,0x00,0x06,0x24,0x03,0x00,0x08,
-0x08,0x00,0x1f,0x16,0xa4,0xc2,0x00,0x14,0x08,0x00,0x1f,0x16,0x00,0x00,0x18,0x21,
-0x27,0xbd,0xff,0xc8,0xaf,0xb5,0x00,0x2c,0xaf,0xb4,0x00,0x28,0xaf,0xb3,0x00,0x24,
-0xaf,0xb0,0x00,0x18,0xaf,0xbf,0x00,0x30,0xaf,0xb2,0x00,0x20,0xaf,0xb1,0x00,0x1c,
-0x94,0x91,0x00,0x06,0x00,0x80,0xa0,0x21,0x3c,0x02,0x80,0x00,0x3c,0x04,0xb0,0x03,
-0x00,0x11,0xa8,0xc0,0x34,0x84,0x00,0x20,0x24,0x42,0x7c,0x80,0x02,0xb1,0x48,0x21,
-0xac,0x82,0x00,0x00,0x00,0x09,0x48,0x80,0x24,0x03,0x00,0x01,0x27,0x82,0x90,0x10,
-0xa2,0x83,0x00,0x12,0x01,0x22,0x10,0x21,0x27,0x84,0x90,0x04,0x01,0x24,0x20,0x21,
-0x80,0x48,0x00,0x06,0x8c,0x8a,0x00,0x18,0x27,0x83,0x90,0x20,0x01,0x23,0x48,0x21,
-0x8d,0x24,0x00,0x00,0x25,0x08,0x00,0x02,0x8d,0x42,0x00,0x00,0x8d,0x49,0x00,0x04,
-0x00,0x08,0x17,0xc2,0x8d,0x43,0x00,0x08,0x01,0x02,0x40,0x21,0x00,0x04,0x25,0xc2,
-0x00,0x08,0x40,0x43,0x30,0x84,0x00,0x01,0x00,0x03,0x1f,0xc2,0x00,0x08,0x40,0x40,
-0x00,0xe0,0x80,0x21,0x00,0x64,0x18,0x24,0x00,0x09,0x49,0x42,0x01,0x48,0x10,0x21,
-0x00,0xa0,0x98,0x21,0x00,0xa0,0x20,0x21,0x00,0x40,0x38,0x21,0x02,0x00,0x28,0x21,
-0x14,0x60,0x00,0x19,0x31,0x29,0x00,0x01,0x94,0x42,0x00,0x00,0x02,0xb1,0x88,0x21,
-0x02,0x00,0x28,0x21,0x00,0x11,0x88,0x80,0x27,0x90,0x90,0x00,0x02,0x30,0x80,0x21,
-0x96,0x03,0x00,0x06,0x30,0x52,0xff,0xff,0x02,0x60,0x20,0x21,0x00,0x60,0x30,0x21,
-0xa6,0x83,0x00,0x1a,0x27,0x82,0x90,0x08,0x0c,0x00,0x08,0xe3,0x02,0x22,0x88,0x21,
-0x00,0x52,0x10,0x21,0x96,0x03,0x00,0x06,0xa6,0x22,0x00,0x04,0x8f,0xbf,0x00,0x30,
-0x7b,0xb4,0x01,0x7c,0x7b,0xb2,0x01,0x3c,0x7b,0xb0,0x00,0xfc,0x00,0x60,0x10,0x21,
-0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x38,0xaf,0xa9,0x00,0x10,0x0c,0x00,0x09,0x0a,
-0xaf,0xa0,0x00,0x14,0x08,0x00,0x1f,0x54,0x02,0xb1,0x88,0x21,0x27,0xbd,0xff,0xc0,
-0xaf,0xbe,0x00,0x38,0xaf,0xb7,0x00,0x34,0xaf,0xb6,0x00,0x30,0xaf,0xb5,0x00,0x2c,
-0xaf,0xb3,0x00,0x24,0xaf,0xb1,0x00,0x1c,0xaf,0xbf,0x00,0x3c,0xaf,0xb4,0x00,0x28,
-0xaf,0xb2,0x00,0x20,0xaf,0xb0,0x00,0x18,0x94,0x90,0x00,0x00,0x3c,0x08,0xb0,0x03,
-0x35,0x08,0x00,0x20,0x00,0x10,0x10,0xc0,0x00,0x50,0x18,0x21,0x00,0x40,0x88,0x21,
-0x3c,0x02,0x80,0x00,0x00,0x03,0x48,0x80,0x24,0x42,0x7d,0xbc,0x00,0x80,0x98,0x21,
-0x27,0x84,0x90,0x10,0x01,0x24,0x20,0x21,0x93,0xb7,0x00,0x53,0xad,0x02,0x00,0x00,
-0x80,0x83,0x00,0x06,0x27,0x82,0x90,0x04,0x01,0x22,0x10,0x21,0x8c,0x44,0x00,0x18,
-0x24,0x63,0x00,0x02,0x00,0x03,0x17,0xc2,0x8c,0x88,0x00,0x08,0x00,0x62,0x18,0x21,
-0x00,0x03,0x18,0x43,0x00,0x03,0x18,0x40,0xaf,0xa7,0x00,0x4c,0x2c,0xa2,0x00,0x10,
-0x00,0xa0,0xa8,0x21,0x00,0x83,0x50,0x21,0x00,0x08,0x47,0xc2,0x00,0xc0,0x58,0x21,
-0x00,0x00,0xb0,0x21,0x8c,0x92,0x00,0x0c,0x14,0x40,0x00,0x13,0x00,0x00,0xf0,0x21,
-0x92,0x67,0x00,0x04,0x24,0x14,0x00,0x01,0x12,0x87,0x00,0x10,0x02,0x30,0x10,0x21,
-0x27,0x83,0x90,0x18,0x01,0x23,0x18,0x21,0x80,0x64,0x00,0x00,0x27,0x83,0xb5,0x70,
-0x00,0x04,0x11,0x00,0x00,0x44,0x10,0x23,0x00,0x02,0x10,0x80,0x00,0x44,0x10,0x23,
-0x00,0x02,0x10,0x80,0x00,0x43,0x10,0x21,0x90,0x44,0x00,0x04,0x00,0x00,0x00,0x00,
-0x10,0x80,0x00,0x23,0x00,0x00,0x00,0x00,0x02,0x30,0x10,0x21,0x00,0x02,0x80,0x80,
-0x24,0x04,0x00,0x01,0x27,0x83,0x90,0x20,0xa2,0x64,0x00,0x12,0x02,0x03,0x18,0x21,
-0x8c,0x62,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x02,0x15,0xc2,0x30,0x42,0x00,0x01,
-0x01,0x02,0x10,0x24,0x14,0x40,0x00,0x0e,0x02,0xa0,0x20,0x21,0x27,0x82,0x90,0x00,
-0x02,0x02,0x10,0x21,0x94,0x43,0x00,0x06,0x00,0x00,0x00,0x00,0xa6,0x63,0x00,0x1a,
-0x94,0x42,0x00,0x06,0x7b,0xbe,0x01,0xfc,0x7b,0xb6,0x01,0xbc,0x7b,0xb4,0x01,0x7c,
-0x7b,0xb2,0x01,0x3c,0x7b,0xb0,0x00,0xfc,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x40,
-0x8f,0xa5,0x00,0x4c,0x01,0x60,0x30,0x21,0x01,0x40,0x38,0x21,0xaf,0xa0,0x00,0x10,
-0x0c,0x00,0x09,0x0a,0xaf,0xa0,0x00,0x14,0x08,0x00,0x1f,0xbb,0x00,0x00,0x00,0x00,
-0x27,0x83,0x90,0x20,0x01,0x23,0x18,0x21,0x8c,0x62,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x02,0x15,0xc2,0x30,0x42,0x00,0x01,0x01,0x02,0x10,0x24,0x14,0x40,0x00,0xaf,
-0x00,0xa0,0x20,0x21,0x32,0x4f,0x00,0x03,0x00,0x12,0x10,0x82,0x25,0xe3,0x00,0x0d,
-0x30,0x45,0x00,0x07,0x00,0x74,0x78,0x04,0x10,0xa0,0x00,0x0e,0x00,0x00,0x90,0x21,
-0x27,0x82,0x80,0x1c,0x00,0x15,0x18,0x40,0x00,0x62,0x18,0x21,0x94,0x64,0x00,0x00,
-0x24,0xa2,0x00,0x06,0x00,0x54,0x10,0x04,0x00,0x44,0x00,0x1a,0x14,0x80,0x00,0x02,
-0x00,0x00,0x00,0x00,0x00,0x07,0x00,0x0d,0x00,0x00,0x10,0x12,0x24,0x42,0x00,0x20,
-0x30,0x52,0xff,0xfc,0x02,0x30,0x10,0x21,0x27,0x83,0x90,0x10,0x00,0x02,0x10,0x80,
-0x00,0x43,0x10,0x21,0x90,0x44,0x00,0x03,0x00,0x00,0x00,0x00,0x30,0x83,0x00,0xff,
-0x2c,0x62,0x00,0x0c,0x14,0x40,0x00,0x04,0x2c,0x62,0x00,0x19,0x30,0x82,0x00,0x0f,
-0x24,0x43,0x00,0x0c,0x2c,0x62,0x00,0x19,0x10,0x40,0x00,0x19,0x24,0x0e,0x00,0x20,
-0x24,0x62,0xff,0xe9,0x2c,0x42,0x00,0x02,0x14,0x40,0x00,0x15,0x24,0x0e,0x00,0x10,
-0x24,0x62,0xff,0xeb,0x2c,0x42,0x00,0x02,0x14,0x40,0x00,0x11,0x24,0x0e,0x00,0x08,
-0x24,0x02,0x00,0x14,0x10,0x62,0x00,0x0e,0x24,0x0e,0x00,0x02,0x24,0x62,0xff,0xef,
-0x2c,0x42,0x00,0x03,0x14,0x40,0x00,0x0a,0x24,0x0e,0x00,0x10,0x24,0x62,0xff,0xf1,
-0x2c,0x42,0x00,0x02,0x14,0x40,0x00,0x06,0x24,0x0e,0x00,0x08,0x24,0x62,0xff,0xf3,
-0x2c,0x42,0x00,0x02,0x24,0x0e,0x00,0x04,0x24,0x03,0x00,0x02,0x00,0x62,0x70,0x0a,
-0x30,0xe2,0x00,0xff,0x00,0x00,0x48,0x21,0x00,0x00,0x68,0x21,0x10,0x40,0x00,0x6d,
-0x00,0x00,0x58,0x21,0x3c,0x14,0x80,0xff,0x27,0x99,0x90,0x00,0x01,0xf2,0xc0,0x23,
-0x36,0x94,0xff,0xff,0x01,0xc9,0x10,0x2a,0x14,0x40,0x00,0x64,0x24,0x03,0x00,0x04,
-0x00,0x10,0x28,0xc0,0x00,0xb0,0x10,0x21,0x00,0x02,0x10,0x80,0x00,0x59,0x10,0x21,
-0x94,0x56,0x00,0x06,0x00,0x00,0x00,0x00,0x32,0xcc,0x00,0x03,0x00,0x6c,0x10,0x23,
-0x30,0x42,0x00,0x03,0x02,0xc2,0x10,0x21,0x24,0x42,0x00,0x04,0x30,0x51,0xff,0xff,
-0x02,0x32,0x18,0x2b,0x10,0x60,0x00,0x4d,0x01,0xf1,0x10,0x23,0x02,0x51,0x10,0x23,
-0x01,0x78,0x18,0x2b,0x10,0x60,0x00,0x34,0x30,0x44,0xff,0xff,0x29,0x22,0x00,0x40,
-0x10,0x40,0x00,0x31,0x01,0x72,0x18,0x21,0x25,0x22,0x00,0x01,0x00,0x02,0x16,0x00,
-0x00,0x02,0x4e,0x03,0x00,0xb0,0x10,0x21,0x00,0x02,0x30,0x80,0x27,0x82,0x90,0x04,
-0x30,0x6b,0xff,0xff,0x00,0xc2,0x18,0x21,0x8c,0x67,0x00,0x18,0x00,0x04,0x25,0x40,
-0x3c,0x03,0x7f,0x00,0x8c,0xe2,0x00,0x04,0x00,0x83,0x20,0x24,0x27,0x83,0x90,0x10,
-0x00,0x54,0x10,0x24,0x00,0xc3,0x28,0x21,0x00,0x44,0x10,0x25,0xac,0xe2,0x00,0x04,
-0x16,0xe0,0x00,0x02,0xa0,0xb5,0x00,0x00,0xa0,0xb5,0x00,0x03,0x27,0x84,0x90,0x20,
-0x00,0xc4,0x18,0x21,0x8c,0x62,0x00,0x00,0x8c,0xe8,0x00,0x08,0x00,0x02,0x15,0xc2,
-0x00,0x08,0x47,0xc2,0x30,0x42,0x00,0x01,0x01,0x02,0x10,0x24,0x10,0x40,0x00,0x0a,
-0x00,0x00,0x00,0x00,0x80,0xa2,0x00,0x06,0x00,0x00,0x00,0x00,0x24,0x42,0x00,0x02,
-0x00,0x02,0x1f,0xc2,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x43,0x00,0x02,0x10,0x40,
-0x00,0xe2,0x50,0x21,0xa5,0x5e,0x00,0x00,0x92,0x62,0x00,0x04,0x25,0xad,0x00,0x01,
-0x27,0x84,0x90,0x00,0x00,0xc4,0x18,0x21,0x01,0xa2,0x10,0x2a,0x94,0x70,0x00,0x02,
-0x14,0x40,0xff,0xb8,0x00,0x00,0x00,0x00,0x96,0x63,0x00,0x14,0x00,0x0c,0x10,0x23,
-0xa2,0x69,0x00,0x12,0x30,0x42,0x00,0x03,0x01,0x62,0x10,0x23,0x00,0x03,0x80,0xc0,
-0x8f,0xa5,0x00,0x4c,0x30,0x4b,0xff,0xff,0x02,0x03,0x80,0x21,0x27,0x82,0x90,0x08,
-0x00,0x10,0x80,0x80,0xa6,0x6b,0x00,0x1a,0x02,0xa0,0x20,0x21,0x01,0x60,0x30,0x21,
-0x01,0x60,0x88,0x21,0x0c,0x00,0x08,0xe3,0x02,0x02,0x80,0x21,0x00,0x5e,0x10,0x21,
-0xa6,0x02,0x00,0x04,0x08,0x00,0x1f,0xc1,0x02,0x20,0x10,0x21,0x01,0x62,0x10,0x2b,
-0x10,0x40,0xff,0xe9,0x00,0x00,0x20,0x21,0x29,0x22,0x00,0x40,0x10,0x40,0xff,0xe6,
-0x01,0x71,0x18,0x21,0x08,0x00,0x20,0x37,0x25,0x22,0x00,0x01,0x08,0x00,0x20,0x66,
-0x32,0xcc,0x00,0x03,0x08,0x00,0x20,0x66,0x00,0x00,0x60,0x21,0x8f,0xa5,0x00,0x4c,
-0x01,0x40,0x38,0x21,0xaf,0xa0,0x00,0x10,0x0c,0x00,0x09,0x0a,0xaf,0xb4,0x00,0x14,
-0x92,0x67,0x00,0x04,0x08,0x00,0x1f,0xd9,0x30,0x5e,0xff,0xff,0x30,0x84,0xff,0xff,
-0x00,0x04,0x30,0xc0,0x00,0xc4,0x20,0x21,0x00,0x04,0x20,0x80,0x27,0x82,0x90,0x00,
-0x3c,0x03,0xb0,0x08,0x30,0xa5,0xff,0xff,0x00,0x82,0x20,0x21,0x00,0xc3,0x30,0x21,
-0xac,0xc5,0x00,0x00,0x03,0xe0,0x00,0x08,0xa4,0x85,0x00,0x00,0x30,0x84,0xff,0xff,
-0x00,0x04,0x30,0xc0,0x00,0xc4,0x30,0x21,0x27,0x88,0x90,0x00,0x00,0x06,0x30,0x80,
-0x00,0xc8,0x30,0x21,0x94,0xc3,0x00,0x04,0x3c,0x02,0xb0,0x08,0x3c,0x07,0xb0,0x03,
-0x00,0x03,0x20,0xc0,0x00,0x83,0x18,0x21,0x00,0x03,0x18,0x80,0x00,0x82,0x20,0x21,
-0x3c,0x02,0x80,0x01,0x30,0xa5,0xff,0xff,0x00,0x68,0x18,0x21,0x34,0xe7,0x00,0x20,
-0x24,0x42,0x82,0x6c,0xac,0xe2,0x00,0x00,0xa4,0xc5,0x00,0x02,0xa4,0x65,0x00,0x00,
-0x03,0xe0,0x00,0x08,0xac,0x85,0x00,0x00,0x30,0x84,0xff,0xff,0x00,0x04,0x10,0xc0,
-0x00,0x44,0x10,0x21,0x27,0x89,0x90,0x00,0x00,0x02,0x10,0x80,0x00,0x49,0x10,0x21,
-0x97,0x83,0x8f,0xf0,0x94,0x4a,0x00,0x04,0x3c,0x02,0xb0,0x08,0x00,0x03,0x38,0xc0,
-0x00,0x0a,0x40,0xc0,0x00,0xe3,0x18,0x21,0x01,0x0a,0x28,0x21,0x00,0xe2,0x38,0x21,
-0x01,0x02,0x40,0x21,0x00,0x03,0x18,0x80,0x00,0x05,0x28,0x80,0x3c,0x06,0xb0,0x03,
-0x3c,0x02,0x80,0x01,0x00,0xa9,0x28,0x21,0x00,0x69,0x18,0x21,0x34,0xc6,0x00,0x20,
-0x34,0x09,0xff,0xff,0x24,0x42,0x82,0xc8,0xac,0xc2,0x00,0x00,0xa4,0x64,0x00,0x00,
-0xac,0xe4,0x00,0x00,0xa4,0xa9,0x00,0x00,0xad,0x09,0x00,0x00,0xa7,0x8a,0x8f,0xf0,
-0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x01,
-0x34,0x63,0x00,0x20,0x24,0x42,0x83,0x48,0x3c,0x04,0xb0,0x03,0xac,0x62,0x00,0x00,
-0x34,0x84,0x01,0x10,0x8c,0x82,0x00,0x00,0x97,0x83,0x81,0x60,0x30,0x42,0xff,0xff,
-0x10,0x62,0x00,0x16,0x24,0x0a,0x00,0x01,0xa7,0x82,0x81,0x60,0xaf,0x80,0xb4,0x50,
-0x00,0x40,0x28,0x21,0x24,0x06,0x00,0x01,0x27,0x84,0xb4,0x54,0x25,0x43,0xff,0xff,
-0x00,0x66,0x10,0x04,0x00,0xa2,0x10,0x24,0x14,0x40,0x00,0x07,0x00,0x00,0x00,0x00,
-0x8c,0x83,0xff,0xfc,0x00,0x00,0x00,0x00,0x00,0x66,0x10,0x04,0x00,0xa2,0x10,0x24,
-0x38,0x42,0x00,0x00,0x01,0x42,0x18,0x0a,0x25,0x4a,0x00,0x01,0x2d,0x42,0x00,0x14,
-0xac,0x83,0x00,0x00,0x14,0x40,0xff,0xf1,0x24,0x84,0x00,0x04,0x3c,0x0b,0xb0,0x03,
-0x00,0x00,0x50,0x21,0x3c,0x0c,0x80,0x00,0x27,0x89,0xb4,0xa0,0x35,0x6b,0x01,0x20,
-0x8d,0x68,0x00,0x00,0x8d,0x23,0x00,0x04,0x01,0x0c,0x10,0x24,0x00,0x02,0x17,0xc2,
-0x11,0x03,0x00,0x37,0xa1,0x22,0x00,0xdc,0xa1,0x20,0x00,0xd5,0xa1,0x20,0x00,0xd6,
-0x01,0x20,0x30,0x21,0x00,0x00,0x38,0x21,0x00,0x00,0x28,0x21,0x01,0x20,0x20,0x21,
-0x00,0xa8,0x10,0x06,0x30,0x42,0x00,0x01,0x10,0xe0,0x00,0x10,0xa0,0x82,0x00,0x0a,
-0x90,0x82,0x00,0x07,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x31,0x24,0xa2,0xff,0xff,
-0xa0,0x82,0x00,0x08,0x90,0x82,0x00,0x0a,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x09,
-0x00,0x00,0x00,0x00,0x90,0x83,0x00,0x08,0x00,0x00,0x00,0x00,0x00,0x03,0x10,0x40,
-0x00,0x43,0x10,0x21,0x00,0x46,0x10,0x21,0xa0,0x45,0x00,0x09,0x90,0x82,0x00,0x0a,
-0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x07,0x00,0x00,0x00,0x00,0x14,0xe0,0x00,0x04,
-0x00,0x00,0x00,0x00,0xa0,0xc5,0x00,0xd5,0x24,0x07,0x00,0x01,0xa0,0x85,0x00,0x08,
-0xa0,0xc5,0x00,0xd6,0x24,0xa5,0x00,0x01,0x2c,0xa2,0x00,0x1c,0x14,0x40,0xff,0xe0,
-0x24,0x84,0x00,0x03,0x90,0xc4,0x00,0xd5,0x00,0x00,0x28,0x21,0x00,0xa4,0x10,0x2b,
-0x10,0x40,0x00,0x0b,0x00,0x00,0x00,0x00,0x00,0xc0,0x18,0x21,0xa0,0x64,0x00,0x08,
-0x90,0xc2,0x00,0xd5,0x24,0xa5,0x00,0x01,0xa0,0x62,0x00,0x09,0x90,0xc4,0x00,0xd5,
-0x00,0x00,0x00,0x00,0x00,0xa4,0x10,0x2b,0x14,0x40,0xff,0xf8,0x24,0x63,0x00,0x03,
-0x25,0x4a,0x00,0x01,0x2d,0x42,0x00,0x08,0xad,0x28,0x00,0x04,0x25,0x6b,0x00,0x04,
-0x14,0x40,0xff,0xbf,0x25,0x29,0x00,0xec,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,
-0x90,0x82,0x00,0x05,0x08,0x00,0x21,0x0d,0xa0,0x82,0x00,0x08,0x97,0x85,0x8b,0x7a,
-0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x01,0x27,0xbd,0xff,0xe8,0x34,0x63,0x00,0x20,
-0x24,0x42,0x84,0xfc,0xaf,0xb0,0x00,0x10,0xaf,0xbf,0x00,0x14,0xac,0x62,0x00,0x00,
-0x30,0x90,0x00,0xff,0x00,0x05,0x28,0x42,0x00,0x00,0x48,0x21,0x27,0x8f,0xb4,0xa4,
-0x00,0x00,0x50,0x21,0x00,0x00,0x58,0x21,0x27,0x98,0xb5,0x84,0x27,0x99,0xb5,0x80,
-0x27,0x8e,0xb5,0x7e,0x27,0x8c,0xb4,0xa8,0x27,0x8d,0xb5,0x00,0x27,0x88,0xb5,0x78,
-0x00,0x0a,0x18,0x80,0x01,0x6f,0x10,0x21,0xac,0x40,0x00,0x00,0xac,0x45,0x00,0x58,
-0x00,0x6e,0x20,0x21,0x00,0x78,0x10,0x21,0xa1,0x00,0xff,0xfc,0xad,0x00,0x00,0x00,
-0xa1,0x00,0x00,0x04,0xa1,0x00,0x00,0x05,0xad,0x00,0xff,0xf8,0x00,0x79,0x18,0x21,
-0x24,0x06,0x00,0x01,0x24,0xc6,0xff,0xff,0xa0,0x80,0x00,0x00,0xa4,0x60,0x00,0x00,
-0xac,0x40,0x00,0x00,0x24,0x63,0x00,0x02,0x24,0x42,0x00,0x04,0x04,0xc1,0xff,0xf9,
-0x24,0x84,0x00,0x01,0x00,0x0a,0x10,0x80,0x00,0x4d,0x20,0x21,0x00,0x00,0x30,0x21,
-0x00,0x4c,0x18,0x21,0x27,0x87,0x81,0x64,0x8c,0xe2,0x00,0x00,0x24,0xe7,0x00,0x04,
-0xac,0x82,0x00,0x00,0xa0,0x66,0x00,0x00,0xa0,0x66,0x00,0x01,0x24,0xc6,0x00,0x01,
-0x28,0xc2,0x00,0x1c,0xa0,0x60,0x00,0x02,0x24,0x84,0x00,0x04,0x14,0x40,0xff,0xf6,
-0x24,0x63,0x00,0x03,0x25,0x29,0x00,0x01,0x29,0x22,0x00,0x08,0x25,0x4a,0x00,0x3b,
-0x25,0x08,0x00,0xec,0x14,0x40,0xff,0xd6,0x25,0x6b,0x00,0xec,0xa7,0x80,0x81,0x60,
-0x00,0x00,0x48,0x21,0x27,0x83,0xb4,0x50,0xac,0x69,0x00,0x00,0x25,0x29,0x00,0x01,
-0x29,0x22,0x00,0x0c,0x14,0x40,0xff,0xfc,0x24,0x63,0x00,0x04,0x0c,0x00,0x20,0xd2,
-0x00,0x00,0x00,0x00,0x2e,0x04,0x00,0x14,0x27,0x83,0xb4,0xa0,0x24,0x09,0x00,0x07,
-0x10,0x80,0x00,0x0a,0x00,0x00,0x00,0x00,0x90,0x62,0x00,0xd5,0x25,0x29,0xff,0xff,
-0xa0,0x62,0x00,0x00,0x05,0x21,0xff,0xfa,0x24,0x63,0x00,0xec,0x8f,0xbf,0x00,0x14,
-0x8f,0xb0,0x00,0x10,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x18,0x90,0x62,0x00,0xd6,
-0x08,0x00,0x21,0x90,0x25,0x29,0xff,0xff,0x30,0x84,0x00,0xff,0x00,0x04,0x11,0x00,
-0x00,0x44,0x10,0x23,0x00,0x02,0x10,0x80,0x00,0x44,0x10,0x23,0x00,0x02,0x10,0x80,
-0x27,0x83,0xb4,0xa0,0x00,0x43,0x60,0x21,0x3c,0x04,0xb0,0x03,0x3c,0x02,0x80,0x01,
-0x34,0x84,0x00,0x20,0x24,0x42,0x86,0x68,0x30,0xc6,0x00,0xff,0x93,0xaa,0x00,0x13,
-0x30,0xa5,0x00,0xff,0x30,0xe7,0x00,0xff,0xac,0x82,0x00,0x00,0x10,0xc0,0x00,0xe8,
-0x25,0x8f,0x00,0xd0,0x91,0x82,0x00,0x00,0x00,0x00,0x00,0x00,0x24,0x42,0xff,0xfc,
-0x2c,0x43,0x00,0x18,0x10,0x60,0x00,0xc7,0x3c,0x03,0x80,0x01,0x00,0x02,0x10,0x80,
-0x24,0x63,0x02,0x90,0x00,0x43,0x10,0x21,0x8c,0x44,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x80,0x00,0x08,0x00,0x00,0x00,0x00,0x2d,0x42,0x00,0x30,0x14,0x40,0x00,0x1c,
-0x00,0x00,0x00,0x00,0x10,0xa0,0x00,0x17,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,
-0x10,0xa2,0x00,0x11,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0xa2,0x00,0x0c,
-0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x03,0x10,0xa2,0x00,0x06,0x00,0x00,0x00,0x00,
-0x8d,0x82,0x00,0xd0,0x00,0x00,0x00,0x00,0x24,0x42,0xff,0xe0,0x03,0xe0,0x00,0x08,
-0xad,0x82,0x00,0xd0,0x8d,0x82,0x00,0xd0,0x08,0x00,0x21,0xcb,0x24,0x42,0xff,0xe8,
-0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x8d,0x82,0x00,0xd0,0x08,0x00,0x21,0xcb,
-0x24,0x42,0x00,0x01,0x8d,0x82,0x00,0xd0,0x08,0x00,0x21,0xcb,0x24,0x42,0x00,0x02,
-0x10,0xa0,0xff,0xf9,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,0x10,0xa2,0x00,0x0a,
-0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0xa2,0xff,0xe9,0x00,0x00,0x00,0x00,
-0x24,0x02,0x00,0x03,0x10,0xa2,0xff,0xe6,0x00,0x00,0x00,0x00,0x8d,0x82,0x00,0xd0,
-0x08,0x00,0x21,0xcb,0x24,0x42,0xff,0xd0,0x8d,0x82,0x00,0xd0,0x08,0x00,0x21,0xcb,
-0x24,0x42,0xff,0xfc,0x10,0xa0,0xff,0xeb,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,
-0x10,0xa2,0xff,0xe5,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0xa2,0xff,0xe0,
-0x24,0x02,0x00,0x03,0x14,0xa2,0xff,0xdb,0x00,0x00,0x00,0x00,0x8d,0x82,0x00,0xd0,
-0x08,0x00,0x21,0xcb,0x24,0x42,0xff,0xf8,0x2d,0x42,0x00,0x19,0x14,0x40,0xff,0xc5,
-0x00,0x00,0x00,0x00,0x10,0xa0,0xff,0xdb,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,
-0x10,0xa2,0xff,0xd5,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0xa2,0xff,0xd0,
-0x24,0x02,0x00,0x03,0x10,0xa2,0xff,0xf1,0x00,0x00,0x00,0x00,0x8d,0x82,0x00,0xd0,
-0x08,0x00,0x21,0xcb,0x24,0x42,0xff,0xf0,0x2d,0x42,0x00,0x1b,0x10,0x40,0xff,0xf1,
-0x00,0x00,0x00,0x00,0x10,0xa0,0xff,0xcb,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,
-0x10,0xa2,0xff,0xc5,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x14,0xa2,0xff,0xb5,
-0x00,0x00,0x00,0x00,0x8d,0x82,0x00,0xd0,0x08,0x00,0x21,0xcb,0x24,0x42,0xff,0xf4,
-0x2d,0x42,0x00,0x1e,0x10,0x40,0xff,0xe3,0x00,0x00,0x00,0x00,0x10,0xa0,0xff,0xbd,
-0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,0x10,0xa2,0xff,0xb5,0x24,0x02,0x00,0x02,
-0x10,0xa2,0xff,0xd6,0x00,0x00,0x00,0x00,0x08,0x00,0x21,0xc6,0x24,0x02,0x00,0x03,
-0x2d,0x42,0x00,0x23,0x10,0x40,0xff,0xd7,0x00,0x00,0x00,0x00,0x10,0xa0,0xff,0xae,
-0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,0x10,0xa2,0xff,0xa9,0x24,0x02,0x00,0x02,
-0x14,0xa2,0xff,0xb7,0x00,0x00,0x00,0x00,0x08,0x00,0x22,0x03,0x00,0x00,0x00,0x00,
-0x2d,0x42,0x00,0x25,0x10,0x40,0xff,0xcb,0x00,0x00,0x00,0x00,0x08,0x00,0x21,0xd8,
-0x00,0x00,0x00,0x00,0x2d,0x42,0x00,0x16,0x14,0x40,0x00,0x0e,0x00,0x00,0x00,0x00,
-0x10,0xa0,0xff,0xa0,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,0x10,0xa2,0xff,0x9a,
-0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0xa2,0xff,0x95,0x24,0x02,0x00,0x03,
-0x14,0xa2,0xff,0xb6,0x00,0x00,0x00,0x00,0x8d,0x82,0x00,0xd0,0x08,0x00,0x21,0xcb,
-0x24,0x42,0xff,0xfa,0x10,0xa0,0xff,0x93,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,
-0x10,0xa2,0xff,0x8d,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0xa2,0xff,0x88,
-0x00,0x00,0x00,0x00,0x08,0x00,0x21,0xf3,0x00,0x00,0x00,0x00,0x2d,0x42,0x00,0x17,
-0x14,0x40,0xff,0xac,0x00,0x00,0x00,0x00,0x08,0x00,0x22,0x34,0x00,0x00,0x00,0x00,
-0x2d,0x42,0x00,0x19,0x10,0x40,0xff,0xe2,0x00,0x00,0x00,0x00,0x10,0xa0,0xff,0x81,
-0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,0x10,0xa2,0xff,0x7b,0x00,0x00,0x00,0x00,
-0x24,0x02,0x00,0x02,0x10,0xa2,0xff,0x76,0x24,0x02,0x00,0x03,0x10,0xa2,0xff,0x97,
-0x00,0x00,0x00,0x00,0x08,0x00,0x21,0xc8,0x00,0x00,0x00,0x00,0x08,0x00,0x22,0x51,
-0x2d,0x42,0x00,0x1b,0x2d,0x42,0x00,0x1e,0x10,0x40,0xff,0xde,0x00,0x00,0x00,0x00,
-0x10,0xa0,0xff,0x70,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,0x10,0xa2,0xff,0x6a,
-0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0xa2,0xff,0x65,0x24,0x02,0x00,0x03,
-0x10,0xa2,0xff,0x96,0x00,0x00,0x00,0x00,0x08,0x00,0x21,0xc8,0x00,0x00,0x00,0x00,
-0x2d,0x42,0x00,0x23,0x14,0x40,0xff,0xf2,0x00,0x00,0x00,0x00,0x08,0x00,0x21,0xf9,
-0x00,0x00,0x00,0x00,0x08,0x00,0x21,0xf7,0x2d,0x42,0x00,0x25,0x08,0x00,0x22,0x2d,
-0x2d,0x42,0x00,0x27,0x10,0xa0,0xff,0x5b,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,
-0x10,0xa2,0xff,0x55,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0xa2,0xff,0x50,
-0x24,0x02,0x00,0x03,0x14,0xa2,0xff,0x71,0x00,0x00,0x00,0x00,0x08,0x00,0x21,0xe6,
-0x00,0x00,0x00,0x00,0x2d,0x42,0x00,0x27,0x14,0x40,0xff,0xad,0x00,0x00,0x00,0x00,
-0x08,0x00,0x22,0x79,0x00,0x00,0x00,0x00,0x2d,0x42,0x00,0x2a,0x14,0x40,0xff,0xd8,
-0x00,0x00,0x00,0x00,0x08,0x00,0x21,0xe9,0x00,0x00,0x00,0x00,0x2d,0x42,0x00,0x2c,
-0x14,0x40,0xff,0x78,0x00,0x00,0x00,0x00,0x08,0x00,0x21,0xbd,0x00,0x00,0x00,0x00,
-0x91,0x86,0x00,0x00,0x91,0x83,0x00,0xd4,0x25,0x8d,0x00,0x5c,0x30,0xc4,0x00,0xff,
-0x00,0x04,0x10,0x40,0x00,0x44,0x10,0x21,0x00,0x04,0x48,0x80,0x01,0x82,0x58,0x21,
-0x01,0x89,0x40,0x21,0x25,0x78,0x00,0x08,0x10,0x60,0x00,0x37,0x25,0x0e,0x00,0x60,
-0x2c,0xa2,0x00,0x03,0x14,0x40,0x00,0x25,0x00,0x00,0x00,0x00,0x91,0x82,0x00,0xdd,
-0x00,0x00,0x00,0x00,0x14,0x40,0x00,0x1e,0x00,0x00,0x00,0x00,0x27,0x87,0x81,0x64,
-0x01,0x27,0x10,0x21,0x8c,0x43,0x00,0x00,0x00,0x00,0x00,0x00,0xad,0x03,0x00,0x60,
-0x91,0x62,0x00,0x08,0x00,0x00,0x00,0x00,0x00,0x40,0x30,0x21,0xa1,0x82,0x00,0x00,
-0x30,0xc2,0x00,0xff,0x00,0x02,0x10,0x80,0x00,0x47,0x10,0x21,0x8c,0x43,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x03,0x18,0x42,0xad,0xa3,0x00,0x00,0x91,0x84,0x00,0x00,
-0x8d,0xc5,0x00,0x00,0x00,0x04,0x20,0x80,0x00,0x87,0x10,0x21,0x8c,0x43,0x00,0x00,
-0x00,0x05,0x28,0x40,0x00,0x8c,0x20,0x21,0x00,0x03,0x18,0x80,0x00,0xa3,0x10,0x2b,
-0x00,0x62,0x28,0x0a,0xac,0x85,0x00,0x60,0x03,0xe0,0x00,0x08,0xa1,0x80,0x00,0xd4,
-0x27,0x87,0x81,0x64,0x08,0x00,0x22,0xb0,0xa1,0x80,0x00,0xdd,0x27,0x82,0x81,0xd4,
-0x8d,0x83,0x00,0xd8,0x00,0x82,0x10,0x21,0x90,0x44,0x00,0x00,0x24,0x63,0x00,0x01,
-0x00,0x64,0x20,0x2b,0x14,0x80,0xff,0x02,0xad,0x83,0x00,0xd8,0x8d,0x02,0x00,0x60,
-0xa1,0x80,0x00,0xd4,0x00,0x02,0x1f,0xc2,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x43,
-0x03,0xe0,0x00,0x08,0xad,0x82,0x00,0x5c,0x10,0xe0,0x00,0x1d,0x24,0x83,0xff,0xfc,
-0x2c,0x62,0x00,0x18,0x10,0x40,0x01,0x10,0x00,0x03,0x10,0x80,0x3c,0x03,0x80,0x01,
-0x24,0x63,0x02,0xf0,0x00,0x43,0x10,0x21,0x8c,0x44,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x80,0x00,0x08,0x00,0x00,0x00,0x00,0x2d,0x42,0x00,0x30,0x14,0x40,0x00,0x65,
-0x00,0x00,0x00,0x00,0x10,0xa0,0x00,0x60,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,
-0x10,0xa2,0x00,0x5a,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0xa2,0x00,0x08,
-0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x03,0x10,0xa2,0x00,0x51,0x00,0x00,0x00,0x00,
-0x8d,0x82,0x00,0xd0,0x00,0x00,0x00,0x00,0x24,0x42,0xff,0xe0,0xad,0x82,0x00,0xd0,
-0x8d,0xe3,0x00,0x00,0x8d,0xa2,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x43,0x10,0x21,
-0xad,0xa2,0x00,0x00,0xad,0xe0,0x00,0x00,0x8d,0xa3,0x00,0x00,0x8d,0xc4,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x83,0x10,0x2a,0x10,0x40,0x00,0x22,0x00,0x00,0x00,0x00,
-0x93,0x05,0x00,0x01,0x91,0x82,0x00,0x00,0x00,0x00,0x00,0x00,0x10,0x45,0x00,0x05,
-0x24,0x02,0x00,0x01,0xa1,0x85,0x00,0x00,0xa1,0x82,0x00,0xd4,0x03,0xe0,0x00,0x08,
-0xad,0x80,0x00,0xd8,0x91,0x82,0x00,0xdd,0x24,0x03,0x00,0x01,0x10,0x43,0x00,0x05,
-0x00,0x00,0x00,0x00,0xa1,0x83,0x00,0xd4,0xad,0x80,0x00,0xd8,0x03,0xe0,0x00,0x08,
-0xa1,0x83,0x00,0xdd,0x00,0x04,0x17,0xc2,0x00,0x82,0x10,0x21,0x00,0x02,0x10,0x43,
-0xad,0xa2,0x00,0x00,0x91,0x83,0x00,0x00,0x27,0x82,0x81,0x64,0x8d,0xc5,0x00,0x00,
-0x00,0x03,0x18,0x80,0x00,0x62,0x18,0x21,0x8c,0x64,0x00,0x00,0x00,0x05,0x28,0x40,
-0x00,0x04,0x18,0x80,0x00,0xa3,0x10,0x2b,0x00,0x62,0x28,0x0a,0x08,0x00,0x22,0xc2,
-0xad,0xc5,0x00,0x00,0x97,0x82,0x8b,0x7c,0x00,0x00,0x00,0x00,0x00,0x62,0x10,0x2a,
-0x10,0x40,0xfe,0xab,0x00,0x00,0x00,0x00,0x91,0x82,0x00,0xdd,0x00,0x00,0x00,0x00,
-0x14,0x40,0x00,0x15,0x00,0x00,0x00,0x00,0x91,0x83,0x00,0x00,0x27,0x82,0x81,0x64,
-0x00,0x03,0x18,0x80,0x00,0x62,0x10,0x21,0x8c,0x44,0x00,0x00,0x00,0x6c,0x18,0x21,
-0xac,0x64,0x00,0x60,0x93,0x05,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x05,0x10,0x80,
-0x01,0x82,0x10,0x21,0x24,0x4e,0x00,0x60,0xa1,0x85,0x00,0x00,0x8d,0xc2,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x02,0x1f,0xc2,0x00,0x43,0x10,0x21,0x00,0x02,0x10,0x43,
-0x03,0xe0,0x00,0x08,0xad,0xa2,0x00,0x00,0x08,0x00,0x23,0x37,0xa1,0x80,0x00,0xdd,
-0x8d,0x82,0x00,0xd0,0x08,0x00,0x22,0xf3,0x24,0x42,0xff,0xe8,0x8d,0x82,0x00,0xd0,
-0x08,0x00,0x22,0xf3,0x24,0x42,0x00,0x01,0x8d,0x82,0x00,0xd0,0x08,0x00,0x22,0xf3,
-0x24,0x42,0x00,0x02,0x10,0xa0,0xff,0xf9,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,
-0x10,0xa2,0x00,0x0a,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0xa2,0xff,0xa0,
-0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x03,0x10,0xa2,0xff,0x9d,0x00,0x00,0x00,0x00,
-0x8d,0x82,0x00,0xd0,0x08,0x00,0x22,0xf3,0x24,0x42,0xff,0xd0,0x8d,0x82,0x00,0xd0,
-0x08,0x00,0x22,0xf3,0x24,0x42,0xff,0xfc,0x10,0xa0,0xff,0xeb,0x00,0x00,0x00,0x00,
-0x24,0x02,0x00,0x01,0x10,0xa2,0xff,0xe5,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,
-0x10,0xa2,0xff,0x93,0x24,0x02,0x00,0x03,0x14,0xa2,0xff,0xdd,0x00,0x00,0x00,0x00,
-0x8d,0x82,0x00,0xd0,0x08,0x00,0x22,0xf3,0x24,0x42,0xff,0xf8,0x2d,0x42,0x00,0x19,
-0x14,0x40,0xff,0x7c,0x00,0x00,0x00,0x00,0x10,0xa0,0xff,0xdb,0x00,0x00,0x00,0x00,
-0x24,0x02,0x00,0x01,0x10,0xa2,0xff,0xd5,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,
-0x10,0xa2,0xff,0x83,0x24,0x02,0x00,0x03,0x10,0xa2,0xff,0xf1,0x00,0x00,0x00,0x00,
-0x8d,0x82,0x00,0xd0,0x08,0x00,0x22,0xf3,0x24,0x42,0xff,0xf0,0x2d,0x42,0x00,0x1b,
-0x10,0x40,0xff,0xf1,0x00,0x00,0x00,0x00,0x10,0xa0,0xff,0xcb,0x00,0x00,0x00,0x00,
-0x24,0x02,0x00,0x01,0x10,0xa2,0xff,0xc5,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,
-0x14,0xa2,0xff,0x6c,0x00,0x00,0x00,0x00,0x8d,0x82,0x00,0xd0,0x08,0x00,0x22,0xf3,
-0x24,0x42,0xff,0xf4,0x2d,0x42,0x00,0x1e,0x10,0x40,0xff,0xe3,0x00,0x00,0x00,0x00,
-0x10,0xa0,0xff,0xbd,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,0x10,0xa2,0xff,0x68,
-0x24,0x02,0x00,0x02,0x10,0xa2,0xff,0xd6,0x00,0x00,0x00,0x00,0x08,0x00,0x22,0xee,
-0x24,0x02,0x00,0x03,0x2d,0x42,0x00,0x23,0x10,0x40,0xff,0xd7,0x00,0x00,0x00,0x00,
-0x10,0xa0,0xff,0xae,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,0x10,0xa2,0xff,0x5c,
-0x24,0x02,0x00,0x02,0x14,0xa2,0xff,0xb7,0x00,0x00,0x00,0x00,0x08,0x00,0x23,0x74,
-0x00,0x00,0x00,0x00,0x2d,0x42,0x00,0x25,0x10,0x40,0xff,0xcb,0x00,0x00,0x00,0x00,
-0x08,0x00,0x23,0x49,0x00,0x00,0x00,0x00,0x2d,0x42,0x00,0x16,0x14,0x40,0x00,0x0e,
-0x00,0x00,0x00,0x00,0x10,0xa0,0xff,0xa0,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,
-0x10,0xa2,0xff,0x9a,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0xa2,0xff,0x48,
-0x24,0x02,0x00,0x03,0x14,0xa2,0xff,0xb6,0x00,0x00,0x00,0x00,0x8d,0x82,0x00,0xd0,
-0x08,0x00,0x22,0xf3,0x24,0x42,0xff,0xfa,0x10,0xa0,0xff,0x93,0x00,0x00,0x00,0x00,
-0x24,0x02,0x00,0x01,0x10,0xa2,0xff,0x8d,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,
-0x10,0xa2,0xff,0x3b,0x00,0x00,0x00,0x00,0x08,0x00,0x23,0x64,0x00,0x00,0x00,0x00,
-0x2d,0x42,0x00,0x17,0x14,0x40,0xff,0xac,0x00,0x00,0x00,0x00,0x08,0x00,0x23,0xa5,
-0x00,0x00,0x00,0x00,0x2d,0x42,0x00,0x19,0x10,0x40,0xff,0xe2,0x00,0x00,0x00,0x00,
-0x10,0xa0,0xff,0x81,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,0x10,0xa2,0xff,0x7b,
-0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0xa2,0xff,0x29,0x24,0x02,0x00,0x03,
-0x10,0xa2,0xff,0x97,0x00,0x00,0x00,0x00,0x08,0x00,0x22,0xf0,0x00,0x00,0x00,0x00,
-0x08,0x00,0x23,0xc2,0x2d,0x42,0x00,0x1b,0x2d,0x42,0x00,0x1e,0x10,0x40,0xff,0xde,
-0x00,0x00,0x00,0x00,0x10,0xa0,0xff,0x70,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,
-0x10,0xa2,0xff,0x6a,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0xa2,0xff,0x18,
-0x24,0x02,0x00,0x03,0x10,0xa2,0xff,0x96,0x00,0x00,0x00,0x00,0x08,0x00,0x22,0xf0,
-0x00,0x00,0x00,0x00,0x2d,0x42,0x00,0x23,0x14,0x40,0xff,0xf2,0x00,0x00,0x00,0x00,
-0x08,0x00,0x23,0x6a,0x00,0x00,0x00,0x00,0x08,0x00,0x23,0x68,0x2d,0x42,0x00,0x25,
-0x08,0x00,0x23,0x9e,0x2d,0x42,0x00,0x27,0x10,0xa0,0xff,0x5b,0x00,0x00,0x00,0x00,
-0x24,0x02,0x00,0x01,0x10,0xa2,0xff,0x55,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,
-0x10,0xa2,0xff,0x03,0x24,0x02,0x00,0x03,0x14,0xa2,0xff,0x71,0x00,0x00,0x00,0x00,
-0x08,0x00,0x23,0x57,0x00,0x00,0x00,0x00,0x2d,0x42,0x00,0x27,0x14,0x40,0xff,0xad,
-0x00,0x00,0x00,0x00,0x08,0x00,0x23,0xea,0x00,0x00,0x00,0x00,0x2d,0x42,0x00,0x2a,
-0x14,0x40,0xff,0xd8,0x00,0x00,0x00,0x00,0x08,0x00,0x23,0x5a,0x00,0x00,0x00,0x00,
-0x2d,0x42,0x00,0x2c,0x14,0x40,0xff,0x78,0x00,0x00,0x00,0x00,0x08,0x00,0x22,0xe5,
-0x00,0x00,0x00,0x00,0x27,0xbd,0xff,0xe8,0x3c,0x02,0xb0,0x03,0xaf,0xbf,0x00,0x14,
-0xaf,0xb0,0x00,0x10,0x34,0x42,0x01,0x18,0x3c,0x03,0xb0,0x03,0x8c,0x50,0x00,0x00,
-0x34,0x63,0x01,0x2c,0x90,0x62,0x00,0x00,0x32,0x05,0x00,0x01,0xa3,0x82,0x80,0x10,
-0x14,0xa0,0x00,0x14,0x30,0x44,0x00,0xff,0x32,0x02,0x01,0x00,0x14,0x40,0x00,0x09,
-0x00,0x00,0x00,0x00,0x32,0x02,0x08,0x00,0x10,0x40,0x00,0x02,0x24,0x02,0x00,0x01,
-0xa3,0x82,0xbc,0x18,0x8f,0xbf,0x00,0x14,0x8f,0xb0,0x00,0x10,0x03,0xe0,0x00,0x08,
-0x27,0xbd,0x00,0x18,0x0c,0x00,0x05,0x37,0x00,0x00,0x00,0x00,0x26,0x02,0xff,0x00,
-0xa3,0x80,0xbc,0x18,0x3c,0x01,0xb0,0x03,0xac,0x22,0x01,0x18,0x08,0x00,0x24,0x16,
-0x32,0x02,0x08,0x00,0x0c,0x00,0x21,0x3f,0x00,0x00,0x00,0x00,0x26,0x02,0xff,0xff,
-0x3c,0x01,0xb0,0x03,0xac,0x22,0x01,0x18,0x08,0x00,0x24,0x13,0x32,0x02,0x01,0x00,
-0x27,0xbd,0xff,0xe0,0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0xd0,0xaf,0xbf,0x00,0x18,
-0x8c,0x43,0x00,0x00,0x3c,0x02,0x00,0x40,0x24,0x07,0x0f,0xff,0x00,0x03,0x33,0x02,
-0x00,0x03,0x2d,0x02,0x00,0x03,0x43,0x02,0x30,0x69,0x0f,0xff,0x00,0x62,0x18,0x24,
-0x30,0xa5,0x00,0x03,0x30,0xc6,0x00,0xff,0x10,0x60,0x00,0x08,0x31,0x08,0x00,0xff,
-0x01,0x00,0x30,0x21,0x0c,0x00,0x24,0xdf,0xaf,0xa9,0x00,0x10,0x8f,0xbf,0x00,0x18,
-0x00,0x00,0x00,0x00,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x20,0x0c,0x00,0x25,0x31,
-0x00,0x00,0x00,0x00,0x3c,0x03,0xb0,0x03,0x34,0x63,0x00,0xd4,0x08,0x00,0x24,0x3f,
-0xac,0x62,0x00,0x00,0x27,0xbd,0xff,0xc0,0x3c,0x02,0xb0,0x03,0xaf,0xbe,0x00,0x38,
-0xaf,0xb5,0x00,0x2c,0xaf,0xb1,0x00,0x1c,0xaf,0xb0,0x00,0x18,0xaf,0xbf,0x00,0x3c,
-0xaf,0xb7,0x00,0x34,0xaf,0xb6,0x00,0x30,0xaf,0xb4,0x00,0x28,0xaf,0xb3,0x00,0x24,
-0xaf,0xb2,0x00,0x20,0x34,0x42,0x00,0x3f,0x90,0x43,0x00,0x00,0x00,0x80,0x80,0x21,
-0x00,0x00,0xf0,0x21,0x00,0x00,0x88,0x21,0x10,0x60,0x00,0x76,0x00,0x00,0xa8,0x21,
-0x3c,0x01,0xb0,0x03,0xa0,0x20,0x00,0x3f,0x00,0x10,0x12,0x02,0x24,0x04,0x06,0x14,
-0x0c,0x00,0x06,0xd1,0x30,0x54,0x00,0x0f,0x24,0x04,0x06,0x14,0x0c,0x00,0x06,0xd1,
-0xaf,0xa2,0x00,0x10,0x3c,0x03,0x00,0xff,0x34,0x63,0xff,0xff,0x32,0x10,0x00,0x7f,
-0x00,0x43,0x10,0x24,0x00,0x10,0x86,0x00,0x02,0x02,0x80,0x25,0x02,0x00,0x28,0x21,
-0x24,0x04,0x06,0x14,0x3c,0x13,0xbf,0xff,0x0c,0x00,0x06,0xbf,0x3c,0x16,0xb0,0x03,
-0x00,0x00,0x90,0x21,0x3c,0x17,0x40,0x00,0x36,0x73,0xff,0xff,0x36,0xd6,0x00,0x3e,
-0x0c,0x00,0x06,0xd1,0x24,0x04,0x04,0x00,0x00,0x57,0x10,0x25,0x00,0x40,0x28,0x21,
-0x0c,0x00,0x06,0xbf,0x24,0x04,0x04,0x00,0x00,0x00,0x80,0x21,0x0c,0x00,0x25,0xf9,
-0x00,0x00,0x00,0x00,0x26,0x03,0x00,0x01,0x10,0x40,0x00,0x46,0x30,0x70,0x00,0xff,
-0x12,0x00,0xff,0xfa,0x00,0x00,0x00,0x00,0x0c,0x00,0x06,0xd1,0x24,0x04,0x04,0x00,
-0x00,0x53,0x10,0x24,0x00,0x40,0x28,0x21,0x0c,0x00,0x06,0xbf,0x24,0x04,0x04,0x00,
-0x24,0x02,0x00,0x01,0x12,0x82,0x00,0x37,0x00,0x00,0x00,0x00,0x12,0x80,0x00,0x35,
-0x00,0x00,0x00,0x00,0x32,0x31,0x00,0x7f,0x12,0x20,0x00,0x04,0x24,0x03,0x00,0x04,
-0x27,0xc2,0x00,0x01,0x30,0x5e,0x00,0xff,0x02,0xb1,0xa8,0x21,0x12,0x43,0x00,0x2a,
-0x3c,0x03,0xb0,0x03,0x02,0x43,0x10,0x21,0xa0,0x51,0x00,0x34,0x26,0x42,0x00,0x01,
-0x30,0x52,0x00,0xff,0x2e,0x43,0x00,0x05,0x14,0x60,0xff,0xd9,0x00,0x00,0x00,0x00,
-0x8f,0xa5,0x00,0x10,0x0c,0x00,0x06,0xbf,0x24,0x04,0x06,0x14,0x12,0xa0,0x00,0x0e,
-0x3c,0x02,0xb0,0x03,0x13,0xc0,0x00,0x0d,0x34,0x42,0x00,0x3c,0x00,0x15,0x10,0x40,
-0x00,0x55,0x10,0x21,0x00,0x02,0x10,0xc0,0x00,0x55,0x10,0x21,0x00,0x02,0xa8,0x80,
-0x02,0xbe,0x00,0x1b,0x17,0xc0,0x00,0x02,0x00,0x00,0x00,0x00,0x00,0x07,0x00,0x0d,
-0x00,0x00,0xa8,0x12,0x3c,0x02,0xb0,0x03,0x34,0x42,0x00,0x3c,0x3c,0x03,0xb0,0x03,
-0x3c,0x04,0xb0,0x03,0xa4,0x55,0x00,0x00,0x34,0x63,0x00,0x1c,0x34,0x84,0x00,0x1d,
-0x24,0x02,0x00,0x01,0xa0,0x60,0x00,0x00,0xa0,0x82,0x00,0x00,0x7b,0xbe,0x01,0xfc,
-0x7b,0xb6,0x01,0xbc,0x7b,0xb4,0x01,0x7c,0x7b,0xb2,0x01,0x3c,0x7b,0xb0,0x00,0xfc,
-0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x40,0xa2,0xd1,0x00,0x00,0x08,0x00,0x24,0x98,
-0x26,0x42,0x00,0x01,0x0c,0x00,0x06,0xd1,0x24,0x04,0x04,0xfc,0x08,0x00,0x24,0x8d,
-0x00,0x40,0x88,0x21,0x3c,0x03,0xb0,0x03,0x34,0x63,0x00,0x3c,0x3c,0x04,0xb0,0x03,
-0x3c,0x05,0xb0,0x03,0xa4,0x60,0x00,0x00,0x34,0x84,0x00,0x1c,0x34,0xa5,0x00,0x1d,
-0x24,0x02,0x00,0x02,0x24,0x03,0x00,0x01,0xa0,0x82,0x00,0x00,0x08,0x00,0x24,0xb7,
-0xa0,0xa3,0x00,0x00,0x0c,0x00,0x17,0x99,0x00,0x00,0x00,0x00,0x10,0x40,0xff,0x8b,
-0x00,0x10,0x12,0x02,0x3c,0x02,0xb0,0x03,0x3c,0x04,0xb0,0x03,0x34,0x42,0x00,0x3c,
-0x34,0x84,0x00,0x14,0x24,0x03,0x00,0x01,0xa4,0x40,0x00,0x00,0x3c,0x01,0xb0,0x03,
-0xa0,0x23,0x00,0x3f,0x08,0x00,0x24,0xb7,0xac,0x90,0x00,0x00,0x27,0xbd,0xff,0xd8,
-0xaf,0xb0,0x00,0x10,0x30,0xd0,0x00,0xff,0x2e,0x02,0x00,0x2e,0xaf,0xb2,0x00,0x18,
-0xaf,0xb1,0x00,0x14,0xaf,0xbf,0x00,0x20,0xaf,0xb3,0x00,0x1c,0x30,0xb1,0x00,0xff,
-0x14,0x40,0x00,0x06,0x00,0x80,0x90,0x21,0x8f,0xbf,0x00,0x20,0x7b,0xb2,0x00,0xfc,
-0x7b,0xb0,0x00,0xbc,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x28,0x2e,0x13,0x00,0x10,
-0x24,0x05,0x00,0x14,0x0c,0x00,0x13,0xa4,0x24,0x06,0x01,0x07,0x12,0x60,0x00,0x38,
-0x02,0x00,0x30,0x21,0x8f,0xa2,0x00,0x38,0x30,0xc3,0x00,0x3f,0x3c,0x04,0xb0,0x09,
-0x00,0x02,0x14,0x00,0x00,0x43,0x30,0x25,0x34,0x84,0x01,0x60,0x90,0x82,0x00,0x00,
-0x00,0x00,0x00,0x00,0x14,0x40,0xff,0xfd,0x24,0x02,0x00,0x01,0x12,0x22,0x00,0x2a,
-0x2a,0x22,0x00,0x02,0x14,0x40,0x00,0x24,0x24,0x02,0x00,0x02,0x12,0x22,0x00,0x20,
-0x24,0x02,0x00,0x03,0x12,0x22,0x00,0x19,0x00,0x00,0x00,0x00,0x16,0x60,0xff,0xe2,
-0x24,0x02,0x00,0x01,0x12,0x22,0x00,0x13,0x2a,0x22,0x00,0x02,0x14,0x40,0x00,0x0d,
-0x24,0x02,0x00,0x02,0x12,0x22,0x00,0x09,0x24,0x02,0x00,0x03,0x16,0x22,0xff,0xda,
-0x00,0x00,0x00,0x00,0x24,0x04,0x08,0x4c,0x24,0x05,0xff,0xff,0x0c,0x00,0x13,0x5f,
-0x3c,0x06,0x0c,0xb8,0x08,0x00,0x24,0xea,0x00,0x00,0x00,0x00,0x08,0x00,0x25,0x12,
-0x24,0x04,0x08,0x48,0x16,0x20,0xff,0xd0,0x00,0x00,0x00,0x00,0x08,0x00,0x25,0x12,
-0x24,0x04,0x08,0x40,0x08,0x00,0x25,0x12,0x24,0x04,0x08,0x44,0x24,0x04,0x08,0x4c,
-0x0c,0x00,0x13,0x5f,0x24,0x05,0xff,0xff,0x08,0x00,0x25,0x07,0x00,0x00,0x00,0x00,
-0x08,0x00,0x25,0x20,0x24,0x04,0x08,0x48,0x16,0x20,0xff,0xe0,0x00,0x00,0x00,0x00,
-0x08,0x00,0x25,0x20,0x24,0x04,0x08,0x40,0x08,0x00,0x25,0x20,0x24,0x04,0x08,0x44,
-0x02,0x40,0x20,0x21,0x0c,0x00,0x25,0x71,0x02,0x20,0x28,0x21,0x08,0x00,0x24,0xf5,
-0x00,0x40,0x30,0x21,0x27,0xbd,0xff,0xd8,0x2c,0xc2,0x00,0x2e,0xaf,0xb2,0x00,0x18,
-0xaf,0xb1,0x00,0x14,0xaf,0xb0,0x00,0x10,0xaf,0xbf,0x00,0x20,0xaf,0xb3,0x00,0x1c,
-0x00,0xc0,0x80,0x21,0x30,0xb1,0x00,0xff,0x00,0x80,0x90,0x21,0x14,0x40,0x00,0x07,
-0x00,0x00,0x18,0x21,0x8f,0xbf,0x00,0x20,0x7b,0xb2,0x00,0xfc,0x7b,0xb0,0x00,0xbc,
-0x00,0x60,0x10,0x21,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x28,0x2e,0x13,0x00,0x10,
-0x24,0x05,0x00,0x14,0x0c,0x00,0x13,0xa4,0x24,0x06,0x01,0x07,0x12,0x60,0x00,0x24,
-0x02,0x00,0x30,0x21,0x3c,0x03,0xb0,0x09,0x34,0x63,0x01,0x60,0x90,0x62,0x00,0x00,
-0x00,0x00,0x00,0x00,0x14,0x40,0xff,0xfd,0x30,0xc5,0x00,0x3f,0x0c,0x00,0x25,0xae,
-0x02,0x20,0x20,0x21,0x16,0x60,0x00,0x0a,0x00,0x40,0x80,0x21,0x24,0x02,0x00,0x01,
-0x12,0x22,0x00,0x15,0x2a,0x22,0x00,0x02,0x14,0x40,0x00,0x0f,0x24,0x02,0x00,0x02,
-0x12,0x22,0x00,0x0b,0x24,0x02,0x00,0x03,0x12,0x22,0x00,0x03,0x00,0x00,0x00,0x00,
-0x08,0x00,0x25,0x3d,0x02,0x00,0x18,0x21,0x24,0x04,0x08,0x4c,0x24,0x05,0xff,0xff,
-0x0c,0x00,0x13,0x5f,0x3c,0x06,0x0c,0xb8,0x08,0x00,0x25,0x3d,0x02,0x00,0x18,0x21,
-0x08,0x00,0x25,0x5f,0x24,0x04,0x08,0x48,0x16,0x20,0xff,0xf5,0x00,0x00,0x00,0x00,
-0x08,0x00,0x25,0x5f,0x24,0x04,0x08,0x40,0x08,0x00,0x25,0x5f,0x24,0x04,0x08,0x44,
-0x02,0x40,0x20,0x21,0x0c,0x00,0x25,0x71,0x02,0x20,0x28,0x21,0x08,0x00,0x25,0x49,
-0x00,0x40,0x30,0x21,0x27,0xbd,0xff,0xe8,0x2c,0xc2,0x00,0x1f,0xaf,0xb0,0x00,0x10,
-0xaf,0xbf,0x00,0x14,0x00,0xc0,0x80,0x21,0x14,0x40,0x00,0x1d,0x30,0xa5,0x00,0xff,
-0x24,0x02,0x00,0x01,0x10,0xa2,0x00,0x18,0x28,0xa2,0x00,0x02,0x14,0x40,0x00,0x12,
-0x24,0x02,0x00,0x02,0x10,0xa2,0x00,0x0e,0x24,0x02,0x00,0x03,0x10,0xa2,0x00,0x07,
-0x24,0x04,0x08,0x4c,0x26,0x10,0xff,0xe2,0x02,0x00,0x10,0x21,0x8f,0xbf,0x00,0x14,
-0x8f,0xb0,0x00,0x10,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x18,0x24,0x05,0xff,0xff,
-0x0c,0x00,0x13,0x5f,0x3c,0x06,0x0d,0xf8,0x08,0x00,0x25,0x82,0x26,0x10,0xff,0xe2,
-0x08,0x00,0x25,0x87,0x24,0x04,0x08,0x48,0x14,0xa0,0xff,0xf2,0x24,0x04,0x08,0x40,
-0x08,0x00,0x25,0x88,0x24,0x05,0xff,0xff,0x08,0x00,0x25,0x87,0x24,0x04,0x08,0x44,
-0x2c,0xc2,0x00,0x10,0x14,0x40,0xff,0xec,0x24,0x02,0x00,0x01,0x10,0xa2,0x00,0x14,
-0x28,0xa2,0x00,0x02,0x14,0x40,0x00,0x0e,0x24,0x02,0x00,0x02,0x10,0xa2,0x00,0x0a,
-0x24,0x02,0x00,0x03,0x10,0xa2,0x00,0x03,0x24,0x04,0x08,0x4c,0x08,0x00,0x25,0x82,
-0x26,0x10,0xff,0xf1,0x24,0x05,0xff,0xff,0x0c,0x00,0x13,0x5f,0x3c,0x06,0x0d,0xb8,
-0x08,0x00,0x25,0x82,0x26,0x10,0xff,0xf1,0x08,0x00,0x25,0xa1,0x24,0x04,0x08,0x48,
-0x14,0xa0,0xff,0xf6,0x24,0x04,0x08,0x40,0x08,0x00,0x25,0xa2,0x24,0x05,0xff,0xff,
-0x08,0x00,0x25,0xa1,0x24,0x04,0x08,0x44,0x27,0xbd,0xff,0xe8,0x30,0x84,0x00,0xff,
-0x24,0x02,0x00,0x01,0x10,0x82,0x00,0x39,0xaf,0xbf,0x00,0x10,0x28,0x82,0x00,0x02,
-0x14,0x40,0x00,0x27,0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x02,0x10,0x82,0x00,0x17,
-0x00,0xa0,0x30,0x21,0x24,0x02,0x00,0x03,0x10,0x82,0x00,0x05,0x24,0x04,0x08,0x3c,
-0x8f,0xbf,0x00,0x10,0x00,0x00,0x00,0x00,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x18,
-0x0c,0x00,0x13,0x5f,0x3c,0x05,0x3f,0x00,0x24,0x04,0x08,0x3c,0x3c,0x05,0x80,0x00,
-0x0c,0x00,0x13,0x5f,0x00,0x00,0x30,0x21,0x24,0x04,0x08,0x3c,0x3c,0x05,0x80,0x00,
-0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x01,0x24,0x04,0x08,0xac,0x0c,0x00,0x13,0x41,
-0x24,0x05,0x0f,0xff,0x08,0x00,0x25,0xbc,0x00,0x00,0x00,0x00,0x24,0x04,0x08,0x34,
-0x0c,0x00,0x13,0x5f,0x3c,0x05,0x3f,0x00,0x24,0x04,0x08,0x34,0x3c,0x05,0x80,0x00,
-0x0c,0x00,0x13,0x5f,0x00,0x00,0x30,0x21,0x24,0x04,0x08,0x34,0x3c,0x05,0x80,0x00,
-0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x01,0x08,0x00,0x25,0xcb,0x24,0x04,0x08,0xa8,
-0x14,0x80,0xff,0xdf,0x00,0xa0,0x30,0x21,0x24,0x04,0x08,0x24,0x0c,0x00,0x13,0x5f,
-0x3c,0x05,0x3f,0x00,0x24,0x04,0x08,0x24,0x3c,0x05,0x80,0x00,0x0c,0x00,0x13,0x5f,
-0x00,0x00,0x30,0x21,0x24,0x04,0x08,0x24,0x3c,0x05,0x80,0x00,0x0c,0x00,0x13,0x5f,
-0x24,0x06,0x00,0x01,0x08,0x00,0x25,0xcb,0x24,0x04,0x08,0xa0,0x00,0xa0,0x30,0x21,
-0x24,0x04,0x08,0x2c,0x0c,0x00,0x13,0x5f,0x3c,0x05,0x3f,0x00,0x24,0x04,0x08,0x2c,
-0x3c,0x05,0x80,0x00,0x0c,0x00,0x13,0x5f,0x00,0x00,0x30,0x21,0x24,0x04,0x08,0x2c,
-0x3c,0x05,0x80,0x00,0x0c,0x00,0x13,0x5f,0x24,0x06,0x00,0x01,0x08,0x00,0x25,0xcb,
-0x24,0x04,0x08,0xa4,0x3c,0x05,0x00,0x14,0x3c,0x02,0xb0,0x05,0x34,0x42,0x04,0x20,
-0x3c,0x06,0xc0,0x00,0x3c,0x03,0xb0,0x05,0x3c,0x04,0xb0,0x05,0x34,0xa5,0x17,0x09,
-0xac,0x45,0x00,0x00,0x34,0xc6,0x05,0x07,0x34,0x63,0x04,0x24,0x34,0x84,0x02,0x28,
-0x3c,0x07,0xb0,0x05,0x24,0x02,0x00,0x20,0xac,0x66,0x00,0x00,0x34,0xe7,0x04,0x50,
-0xa0,0x82,0x00,0x00,0x90,0xe2,0x00,0x00,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x03,
-0x10,0x40,0xff,0xfc,0x24,0x02,0x00,0x01,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,
-0x93,0x85,0x81,0xf1,0x24,0x02,0x00,0x01,0x14,0xa2,0x00,0x53,0x00,0x80,0x40,0x21,
-0x8c,0x89,0x00,0x04,0x3c,0x02,0xb0,0x01,0x01,0x22,0x30,0x21,0x8c,0xc3,0x00,0x04,
-0x3c,0x02,0x01,0x00,0x00,0x62,0x10,0x24,0x10,0x40,0x00,0x4b,0x30,0x62,0x00,0x08,
-0x10,0x45,0x00,0x59,0x00,0x00,0x00,0x00,0x94,0xc2,0x00,0x38,0x24,0x03,0x00,0xb4,
-0x30,0x44,0x00,0xff,0x10,0x83,0x00,0x61,0x24,0x02,0x00,0xc4,0x10,0x82,0x00,0x54,
-0x24,0x02,0x00,0x94,0x10,0x82,0x00,0x45,0x00,0x00,0x00,0x00,0x94,0xc2,0x00,0x38,
-0x00,0x00,0x00,0x00,0x30,0x47,0xff,0xff,0x30,0xe3,0x40,0xff,0x24,0x02,0x40,0x88,
-0x14,0x62,0x00,0x39,0x30,0xe3,0x03,0x00,0x24,0x02,0x03,0x00,0x10,0x62,0x00,0x38,
-0x00,0x00,0x00,0x00,0x94,0xc2,0x00,0x56,0x00,0x00,0x00,0x00,0x30,0x47,0xff,0xff,
-0x30,0xe2,0x00,0x80,0x14,0x40,0x00,0x30,0x3c,0x02,0xb0,0x01,0x01,0x22,0x30,0x21,
-0x94,0xc3,0x00,0x60,0x24,0x02,0x00,0x08,0x14,0x43,0x00,0x3b,0x00,0x00,0x00,0x00,
-0x90,0xc2,0x00,0x62,0x24,0x03,0x00,0x04,0x00,0x02,0x39,0x02,0x10,0xe3,0x00,0x15,
-0x24,0x02,0x00,0x06,0x14,0xe2,0x00,0x34,0x00,0x00,0x00,0x00,0x8d,0x05,0x01,0xac,
-0x94,0xc4,0x00,0x66,0x27,0x82,0x89,0x68,0x00,0x05,0x28,0x80,0x30,0x87,0xff,0xff,
-0x00,0xa2,0x28,0x21,0x00,0x07,0x1a,0x00,0x8c,0xa4,0x00,0x00,0x00,0x07,0x12,0x02,
-0x00,0x43,0x10,0x25,0x24,0x42,0x00,0x5e,0x24,0x03,0xc0,0x00,0x30,0x47,0xff,0xff,
-0x00,0x83,0x20,0x24,0x00,0x87,0x20,0x25,0xac,0xa4,0x00,0x00,0x08,0x00,0x26,0x76,
-0xad,0x07,0x00,0x10,0x8d,0x05,0x01,0xac,0x94,0xc4,0x00,0x64,0x27,0x82,0x89,0x68,
-0x00,0x05,0x28,0x80,0x30,0x87,0xff,0xff,0x00,0xa2,0x28,0x21,0x00,0x07,0x1a,0x00,
-0x8c,0xa4,0x00,0x00,0x00,0x07,0x12,0x02,0x00,0x43,0x10,0x25,0x24,0x42,0x00,0x36,
-0x3c,0x03,0xff,0xff,0x30,0x47,0xff,0xff,0x00,0x83,0x20,0x24,0x00,0x87,0x20,0x25,
-0xac,0xa4,0x00,0x00,0xad,0x07,0x00,0x10,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,
-0x94,0xc2,0x00,0x50,0x08,0x00,0x26,0x34,0x30,0x47,0xff,0xff,0x8d,0x04,0x01,0xac,
-0x27,0x83,0x89,0x68,0x00,0x04,0x20,0x80,0x00,0x83,0x20,0x21,0x8c,0x82,0x00,0x00,
-0x3c,0x03,0xff,0xff,0x00,0x43,0x10,0x24,0x34,0x42,0x00,0x2e,0xac,0x82,0x00,0x00,
-0x24,0x03,0x00,0x2e,0xad,0x03,0x00,0x10,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,
-0x8d,0x04,0x01,0xac,0x27,0x83,0x89,0x68,0x00,0x04,0x20,0x80,0x00,0x83,0x20,0x21,
-0x8c,0x82,0x00,0x00,0x3c,0x03,0xff,0xff,0x00,0x43,0x10,0x24,0x34,0x42,0x00,0x0e,
-0x24,0x03,0x00,0x0e,0x08,0x00,0x26,0x75,0xac,0x82,0x00,0x00,0x8d,0x04,0x01,0xac,
-0x27,0x83,0x89,0x68,0x00,0x04,0x20,0x80,0x00,0x83,0x20,0x21,0x8c,0x82,0x00,0x00,
-0x3c,0x03,0xff,0xff,0x00,0x43,0x10,0x24,0x34,0x42,0x00,0x14,0x24,0x03,0x00,0x14,
-0x08,0x00,0x26,0x75,0xac,0x82,0x00,0x00,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,
-0x30,0xc6,0x00,0xff,0x00,0x06,0x48,0x40,0x01,0x26,0x10,0x21,0x00,0x02,0x10,0x80,
-0x27,0x8b,0xbc,0x30,0x27,0x83,0xbc,0x36,0x00,0x4b,0x40,0x21,0x00,0x43,0x10,0x21,
-0x94,0x47,0x00,0x00,0x30,0xa2,0x3f,0xff,0x10,0xe2,0x00,0x29,0x30,0x8a,0xff,0xff,
-0x95,0x02,0x00,0x02,0x24,0x03,0x00,0x01,0x00,0x02,0x11,0x82,0x30,0x42,0x00,0x01,
-0x10,0x43,0x00,0x18,0x00,0x00,0x00,0x00,0x01,0x26,0x10,0x21,0x00,0x02,0x10,0x80,
-0x00,0x4b,0x30,0x21,0x94,0xc4,0x00,0x02,0x27,0x83,0xbc,0x36,0x27,0x85,0xbc,0x34,
-0x00,0x45,0x28,0x21,0x30,0x84,0xff,0xdf,0x00,0x43,0x10,0x21,0xa4,0xc4,0x00,0x02,
-0xa4,0x40,0x00,0x00,0xa4,0xa0,0x00,0x00,0x94,0xc3,0x00,0x02,0x3c,0x04,0xb0,0x01,
-0x01,0x44,0x20,0x21,0x30,0x63,0xff,0xbf,0xa4,0xc3,0x00,0x02,0xa0,0xc0,0x00,0x00,
-0x8c,0x82,0x00,0x04,0x24,0x03,0xf0,0xff,0x00,0x43,0x10,0x24,0x03,0xe0,0x00,0x08,
-0xac,0x82,0x00,0x04,0x24,0x02,0xc0,0x00,0x91,0x04,0x00,0x01,0x00,0xa2,0x10,0x24,
-0x00,0x47,0x28,0x25,0x3c,0x03,0xb0,0x01,0x24,0x02,0x00,0x02,0x14,0x82,0xff,0xe2,
-0x01,0x43,0x18,0x21,0xac,0x65,0x00,0x00,0x08,0x00,0x26,0xa3,0x01,0x26,0x10,0x21,
-0x08,0x00,0x26,0xa3,0x01,0x26,0x10,0x21,0x93,0x83,0x81,0xf1,0x24,0x02,0x00,0x01,
-0x14,0x62,0x00,0x0d,0x3c,0x02,0xb0,0x01,0x8c,0x84,0x00,0x04,0x3c,0x06,0xb0,0x09,
-0x00,0x82,0x20,0x21,0x8c,0x85,0x00,0x08,0x8c,0x83,0x00,0x04,0x3c,0x02,0x01,0x00,
-0x34,0xc6,0x01,0x00,0x00,0x62,0x18,0x24,0x14,0x60,0x00,0x05,0x30,0xa5,0x20,0x00,
-0x24,0x02,0x00,0x06,0xa0,0xc2,0x00,0x00,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,
-0x3c,0x03,0xb0,0x09,0x10,0xa0,0xff,0xfc,0x34,0x63,0x01,0x00,0x24,0x02,0x00,0x0e,
-0x08,0x00,0x26,0xd6,0xa0,0x62,0x00,0x00,0x3c,0x02,0xb0,0x01,0x30,0xa5,0xff,0xff,
-0x00,0xa2,0x28,0x21,0x8c,0xa3,0x00,0x00,0x3c,0x02,0x10,0x00,0x00,0x80,0x30,0x21,
-0x00,0x62,0x18,0x24,0x8c,0xa2,0x00,0x04,0x10,0x60,0x00,0x04,0x00,0x00,0x00,0x00,
-0x30,0x42,0x80,0x00,0x10,0x40,0x00,0x13,0x00,0x00,0x00,0x00,0x8c,0xc2,0x01,0xa8,
-0x00,0x00,0x00,0x00,0x24,0x44,0x00,0x01,0x28,0x83,0x00,0x00,0x24,0x42,0x00,0x40,
-0x00,0x83,0x10,0x0a,0x93,0x83,0x81,0xf0,0x00,0x02,0x11,0x83,0x00,0x02,0x11,0x80,
-0x00,0x82,0x20,0x23,0x24,0x63,0xff,0xff,0xac,0xc4,0x01,0xa8,0xa3,0x83,0x81,0xf0,
-0x8c,0xc4,0x01,0xac,0x8c,0xc2,0x01,0xa8,0x00,0x00,0x00,0x00,0x00,0x44,0x10,0x26,
-0x00,0x02,0x10,0x2b,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x3c,0x03,0xb0,0x03,
-0x34,0x63,0x00,0x73,0x90,0x62,0x00,0x00,0x00,0x00,0x00,0x00,0x30,0x42,0x00,0x01,
-0x14,0x40,0x00,0x04,0x00,0x00,0x00,0x00,0xa3,0x80,0x81,0xf1,0x03,0xe0,0x00,0x08,
-0x00,0x00,0x00,0x00,0x24,0x02,0x00,0x01,0xa3,0x82,0x81,0xf1,0x03,0xe0,0x00,0x08,
-0x00,0x00,0x00,0x00,0x8c,0x82,0x00,0x04,0x3c,0x05,0xb0,0x01,0x00,0x80,0x50,0x21,
-0x00,0x45,0x10,0x21,0x8c,0x43,0x00,0x04,0x24,0x02,0x00,0x05,0x00,0x03,0x1a,0x02,
-0x30,0x69,0x00,0x0f,0x11,0x22,0x00,0x0b,0x24,0x02,0x00,0x07,0x11,0x22,0x00,0x09,
-0x24,0x02,0x00,0x0a,0x11,0x22,0x00,0x07,0x24,0x02,0x00,0x0b,0x11,0x22,0x00,0x05,
-0x24,0x02,0x00,0x01,0x93,0x83,0x81,0xf0,0x3c,0x04,0xb0,0x06,0x10,0x62,0x00,0x03,
-0x34,0x84,0x80,0x18,0x03,0xe0,0x00,0x08,0x00,0x00,0x00,0x00,0x8c,0x82,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x02,0x17,0x02,0x14,0x40,0xff,0xfa,0x00,0x00,0x00,0x00,
-0x8d,0x43,0x01,0xa8,0x27,0x82,0x89,0x68,0x00,0x03,0x18,0x80,0x00,0x6a,0x20,0x21,
-0x8c,0x87,0x00,0xa8,0x00,0x62,0x18,0x21,0x8c,0x68,0x00,0x00,0x00,0xe5,0x28,0x21,
-0x8c,0xa9,0x00,0x00,0x3c,0x02,0xff,0xff,0x27,0x83,0x8a,0x68,0x01,0x22,0x10,0x24,
-0x00,0x48,0x10,0x25,0xac,0xa2,0x00,0x00,0x8d,0x44,0x01,0xa8,0x00,0x07,0x30,0xc2,
-0x3c,0x02,0x00,0x80,0x00,0x04,0x20,0x80,0x00,0x83,0x20,0x21,0x00,0x06,0x32,0x00,
-0x8c,0xa9,0x00,0x04,0x00,0xc2,0x30,0x25,0x8c,0x82,0x00,0x00,0x3c,0x03,0x80,0x00,
-0x01,0x22,0x10,0x25,0x00,0x43,0x10,0x25,0xac,0xa2,0x00,0x04,0xaf,0x87,0xbc,0x20,
-0x8c,0xa2,0x00,0x00,0x00,0x00,0x00,0x00,0xaf,0x82,0xbc,0x28,0x8c,0xa3,0x00,0x04,
-0x3c,0x01,0xb0,0x07,0xac,0x26,0x80,0x18,0x8d,0x42,0x01,0xa8,0xaf,0x83,0xbc,0x24,
-0x93,0x85,0x81,0xf0,0x24,0x44,0x00,0x01,0x28,0x83,0x00,0x00,0x24,0x42,0x00,0x40,
-0x00,0x83,0x10,0x0a,0x00,0x02,0x11,0x83,0x00,0x02,0x11,0x80,0x24,0xa5,0xff,0xff,
-0x00,0x82,0x20,0x23,0xad,0x44,0x01,0xa8,0xa3,0x85,0x81,0xf0,0x08,0x00,0x27,0x21,
-0x00,0x00,0x00,0x00,0x3c,0x05,0xb0,0x03,0x3c,0x02,0x80,0x01,0x34,0xa5,0x00,0x20,
-0x24,0x42,0x9d,0x64,0xac,0xa2,0x00,0x00,0x24,0x02,0x00,0x02,0x24,0x03,0x00,0x20,
-0xac,0x82,0x00,0x64,0x3c,0x02,0x80,0x01,0xac,0x83,0x00,0x60,0xac,0x80,0x00,0x00,
-0xac,0x80,0x00,0x04,0xac,0x80,0x00,0x08,0xac,0x80,0x00,0x4c,0xac,0x80,0x00,0x50,
-0xac,0x80,0x00,0x54,0xac,0x80,0x00,0x0c,0xac,0x80,0x00,0x58,0xa0,0x80,0x00,0x5c,
-0x24,0x42,0x9e,0x28,0x24,0x83,0x00,0x68,0x24,0x05,0x00,0x0f,0x24,0xa5,0xff,0xff,
-0xac,0x62,0x00,0x00,0x04,0xa1,0xff,0xfd,0x24,0x63,0x00,0x04,0x3c,0x02,0x80,0x01,
-0x24,0x42,0x9f,0x10,0xac,0x82,0x00,0x78,0x3c,0x03,0x80,0x01,0x3c,0x02,0x80,0x01,
-0x24,0x63,0xa0,0x9c,0x24,0x42,0xa0,0x08,0xac,0x83,0x00,0x88,0xac,0x82,0x00,0x98,
-0x3c,0x03,0x80,0x01,0x3c,0x02,0x80,0x01,0x24,0x63,0xa1,0x44,0x24,0x42,0xa2,0x5c,
-0xac,0x83,0x00,0xa0,0xac,0x82,0x00,0xa4,0xa0,0x80,0x01,0xba,0xac,0x80,0x01,0xa8,
-0xac,0x80,0x01,0xac,0xac,0x80,0x01,0xb0,0xac,0x80,0x01,0xb4,0xa0,0x80,0x01,0xb8,
-0x03,0xe0,0x00,0x08,0xa0,0x80,0x01,0xb9,0x3c,0x03,0xb0,0x03,0x3c,0x02,0x80,0x01,
-0x34,0x63,0x00,0x20,0x24,0x42,0x9e,0x28,0x03,0xe0,0x00,0x08,0xac,0x62,0x00,0x00,
-0x3c,0x02,0xb0,0x03,0x3c,0x03,0x80,0x01,0x34,0x42,0x00,0x20,0x24,0x63,0x9e,0x40,
-0xac,0x43,0x00,0x00,0x8c,0x82,0x00,0x10,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x11,
-0x00,0x80,0x28,0x21,0x8c,0x82,0x00,0x14,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x0d,
-0x00,0x00,0x00,0x00,0x8c,0x84,0x00,0x10,0x8c,0xa3,0x00,0x14,0x8c,0xa2,0x00,0x04,
-0x00,0x83,0x20,0x21,0x00,0x44,0x10,0x21,0x30,0x43,0x00,0xff,0x00,0x03,0x18,0x2b,
-0x00,0x02,0x12,0x02,0x00,0x43,0x10,0x21,0x00,0x02,0x12,0x00,0x30,0x42,0x3f,0xff,
-0xac,0xa2,0x00,0x04,0xac,0xa0,0x00,0x00,0xac,0xa0,0x00,0x4c,0xac,0xa0,0x00,0x50,
-0xac,0xa0,0x00,0x54,0x03,0xe0,0x00,0x08,0xac,0xa0,0x00,0x0c,0x3c,0x03,0xb0,0x03,
-0x3c,0x02,0x80,0x01,0x34,0x63,0x00,0x20,0x24,0x42,0x9e,0xbc,0xac,0x62,0x00,0x00,
-0x8c,0x86,0x00,0x04,0x3c,0x02,0xb0,0x01,0x24,0x03,0x00,0x01,0x00,0xc2,0x10,0x21,
-0x8c,0x45,0x00,0x00,0xac,0x83,0x00,0x4c,0x00,0x05,0x14,0x02,0x30,0xa3,0x3f,0xff,
-0x30,0x42,0x00,0xff,0xac,0x83,0x00,0x10,0xac,0x82,0x00,0x14,0x8c,0x83,0x00,0x14,
-0xac,0x85,0x00,0x40,0x00,0xc3,0x30,0x21,0x03,0xe0,0x00,0x08,0xac,0x86,0x00,0x08,
-0x3c,0x02,0xb0,0x03,0x3c,0x03,0x80,0x01,0x27,0xbd,0xff,0xe8,0x34,0x42,0x00,0x20,
-0x24,0x63,0x9f,0x10,0xaf,0xb0,0x00,0x10,0xaf,0xbf,0x00,0x14,0xac,0x43,0x00,0x00,
-0x8c,0x82,0x00,0x4c,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x0a,0x00,0x80,0x80,0x21,
-0xae,0x00,0x00,0x00,0xae,0x00,0x00,0x4c,0xae,0x00,0x00,0x50,0xae,0x00,0x00,0x54,
-0xae,0x00,0x00,0x0c,0x8f,0xbf,0x00,0x14,0x8f,0xb0,0x00,0x10,0x03,0xe0,0x00,0x08,
-0x27,0xbd,0x00,0x18,0x0c,0x00,0x27,0xaf,0x00,0x00,0x00,0x00,0x08,0x00,0x27,0xd1,
-0xae,0x00,0x00,0x00,0x3c,0x02,0xb0,0x03,0x3c,0x03,0x80,0x01,0x27,0xbd,0xff,0xe8,
-0x34,0x42,0x00,0x20,0x24,0x63,0x9f,0x74,0xaf,0xb0,0x00,0x10,0xaf,0xbf,0x00,0x14,
-0xac,0x43,0x00,0x00,0x8c,0x82,0x00,0x4c,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x16,
-0x00,0x80,0x80,0x21,0x8e,0x03,0x00,0x08,0x3c,0x02,0xb0,0x01,0x8e,0x04,0x00,0x44,
-0x00,0x62,0x18,0x21,0x90,0x65,0x00,0x00,0x24,0x02,0x00,0x01,0xae,0x02,0x00,0x50,
-0x30,0xa3,0x00,0xff,0x00,0x03,0x10,0x82,0x00,0x04,0x23,0x02,0x30,0x84,0x00,0x0f,
-0x30,0x42,0x00,0x03,0x00,0x03,0x19,0x02,0xae,0x04,0x00,0x34,0xae,0x02,0x00,0x2c,
-0xae,0x03,0x00,0x30,0xa2,0x05,0x00,0x48,0x8f,0xbf,0x00,0x14,0x8f,0xb0,0x00,0x10,
-0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x18,0x0c,0x00,0x27,0xaf,0x00,0x00,0x00,0x00,
-0x08,0x00,0x27,0xe9,0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x03,0x3c,0x03,0x80,0x01,
-0x27,0xbd,0xff,0xe8,0x34,0x42,0x00,0x20,0x24,0x63,0xa0,0x08,0xaf,0xb0,0x00,0x10,
-0xaf,0xbf,0x00,0x14,0xac,0x43,0x00,0x00,0x8c,0x82,0x00,0x50,0x00,0x00,0x00,0x00,
-0x10,0x40,0x00,0x16,0x00,0x80,0x80,0x21,0x92,0x03,0x00,0x44,0x8e,0x02,0x00,0x40,
-0x83,0x85,0x8b,0xd4,0x92,0x04,0x00,0x41,0x30,0x63,0x00,0x01,0x00,0x02,0x16,0x02,
-0xae,0x04,0x00,0x14,0x00,0x00,0x30,0x21,0xae,0x02,0x00,0x18,0x10,0xa0,0x00,0x04,
-0xae,0x03,0x00,0x3c,0x10,0x60,0x00,0x03,0x24,0x02,0x00,0x01,0x24,0x06,0x00,0x01,
-0x24,0x02,0x00,0x01,0xa3,0x86,0x8b,0xd4,0x8f,0xbf,0x00,0x14,0xae,0x02,0x00,0x54,
-0x8f,0xb0,0x00,0x10,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x18,0x0c,0x00,0x27,0xdd,
-0x00,0x00,0x00,0x00,0x08,0x00,0x28,0x0e,0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x03,
-0x3c,0x03,0x80,0x01,0x27,0xbd,0xff,0xe8,0x34,0x42,0x00,0x20,0x24,0x63,0xa0,0x9c,
-0xaf,0xb0,0x00,0x10,0xaf,0xbf,0x00,0x14,0xac,0x43,0x00,0x00,0x8c,0x82,0x00,0x50,
-0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x1b,0x00,0x80,0x80,0x21,0x3c,0x02,0xb0,0x03,
-0x8c,0x42,0x00,0x00,0x92,0x04,0x00,0x44,0x8e,0x03,0x00,0x40,0x83,0x86,0x8b,0xd4,
-0x92,0x05,0x00,0x41,0x30,0x42,0x08,0x00,0x30,0x84,0x00,0x01,0x00,0x02,0x12,0xc2,
-0x00,0x03,0x1e,0x02,0x00,0x82,0x20,0x25,0xae,0x05,0x00,0x14,0x00,0x00,0x38,0x21,
-0xae,0x03,0x00,0x18,0x10,0xc0,0x00,0x04,0xae,0x04,0x00,0x3c,0x10,0x80,0x00,0x03,
-0x24,0x02,0x00,0x01,0x24,0x07,0x00,0x01,0x24,0x02,0x00,0x01,0xa3,0x87,0x8b,0xd4,
-0x8f,0xbf,0x00,0x14,0xae,0x02,0x00,0x54,0x8f,0xb0,0x00,0x10,0x03,0xe0,0x00,0x08,
-0x27,0xbd,0x00,0x18,0x0c,0x00,0x27,0xdd,0x00,0x00,0x00,0x00,0x08,0x00,0x28,0x33,
-0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x03,0x3c,0x03,0x80,0x01,0x27,0xbd,0xff,0xe8,
-0x34,0x42,0x00,0x20,0x24,0x63,0xa1,0x44,0xaf,0xb0,0x00,0x10,0xaf,0xbf,0x00,0x14,
-0xac,0x43,0x00,0x00,0x8c,0x82,0x00,0x54,0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x37,
-0x00,0x80,0x80,0x21,0x8e,0x04,0x00,0x04,0x8e,0x03,0x00,0x44,0x3c,0x02,0x80,0x00,
-0x3c,0x05,0xb0,0x01,0x34,0x42,0x00,0x10,0x00,0x85,0x20,0x21,0x00,0x62,0x18,0x25,
-0xac,0x83,0x00,0x04,0x8e,0x02,0x00,0x04,0x8e,0x03,0x01,0xac,0x02,0x00,0x20,0x21,
-0x00,0x45,0x10,0x21,0x8c,0x46,0x00,0x00,0x00,0x03,0x18,0x80,0x27,0x82,0x89,0x68,
-0x00,0x62,0x18,0x21,0xac,0x66,0x00,0x00,0x8e,0x02,0x00,0x04,0x8e,0x03,0x01,0xac,
-0x00,0x45,0x10,0x21,0x8c,0x46,0x00,0x04,0x00,0x03,0x18,0x80,0x27,0x82,0x8a,0x68,
-0x00,0x62,0x18,0x21,0x0c,0x00,0x26,0x10,0xac,0x66,0x00,0x00,0x8e,0x03,0x01,0xac,
-0x8e,0x07,0x00,0x04,0x3c,0x06,0xb0,0x03,0x24,0x65,0x00,0x01,0x28,0xa4,0x00,0x00,
-0x24,0x62,0x00,0x40,0x00,0xa4,0x10,0x0a,0x00,0x02,0x11,0x83,0x00,0x02,0x11,0x80,
-0x00,0x03,0x18,0x80,0x00,0xa2,0x28,0x23,0x00,0x70,0x18,0x21,0xae,0x05,0x01,0xac,
-0xac,0x67,0x00,0xa8,0x34,0xc6,0x00,0x30,0x8c,0xc3,0x00,0x00,0x93,0x82,0x81,0xf0,
-0x02,0x00,0x20,0x21,0x24,0x63,0x00,0x01,0x24,0x42,0x00,0x01,0xac,0xc3,0x00,0x00,
-0xa3,0x82,0x81,0xf0,0x0c,0x00,0x27,0x90,0x00,0x00,0x00,0x00,0x8f,0xbf,0x00,0x14,
-0x8f,0xb0,0x00,0x10,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x18,0x0c,0x00,0x28,0x27,
-0x00,0x00,0x00,0x00,0x08,0x00,0x28,0x5d,0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x03,
-0x3c,0x03,0x80,0x01,0x27,0xbd,0xff,0xe8,0x34,0x42,0x00,0x20,0x24,0x63,0xa2,0x5c,
-0xaf,0xb0,0x00,0x10,0xaf,0xbf,0x00,0x14,0xac,0x43,0x00,0x00,0x8c,0x82,0x00,0x54,
-0x00,0x00,0x00,0x00,0x10,0x40,0x00,0x37,0x00,0x80,0x80,0x21,0x8e,0x04,0x00,0x04,
-0x8e,0x03,0x00,0x44,0x3c,0x02,0x80,0x00,0x3c,0x05,0xb0,0x01,0x34,0x42,0x00,0x10,
-0x00,0x85,0x20,0x21,0x00,0x62,0x18,0x25,0xac,0x83,0x00,0x04,0x8e,0x02,0x00,0x04,
-0x8e,0x03,0x01,0xac,0x02,0x00,0x20,0x21,0x00,0x45,0x10,0x21,0x8c,0x46,0x00,0x00,
-0x00,0x03,0x18,0x80,0x27,0x82,0x89,0x68,0x00,0x62,0x18,0x21,0xac,0x66,0x00,0x00,
-0x8e,0x02,0x00,0x04,0x8e,0x03,0x01,0xac,0x00,0x45,0x10,0x21,0x8c,0x46,0x00,0x04,
-0x00,0x03,0x18,0x80,0x27,0x82,0x8a,0x68,0x00,0x62,0x18,0x21,0x0c,0x00,0x26,0x10,
-0xac,0x66,0x00,0x00,0x8e,0x03,0x01,0xac,0x8e,0x07,0x00,0x04,0x3c,0x06,0xb0,0x03,
-0x24,0x65,0x00,0x01,0x28,0xa4,0x00,0x00,0x24,0x62,0x00,0x40,0x00,0xa4,0x10,0x0a,
-0x00,0x02,0x11,0x83,0x00,0x02,0x11,0x80,0x00,0x03,0x18,0x80,0x00,0xa2,0x28,0x23,
-0x00,0x70,0x18,0x21,0xae,0x05,0x01,0xac,0xac,0x67,0x00,0xa8,0x34,0xc6,0x00,0x30,
-0x8c,0xc3,0x00,0x00,0x93,0x82,0x81,0xf0,0x02,0x00,0x20,0x21,0x24,0x63,0x00,0x01,
-0x24,0x42,0x00,0x01,0xac,0xc3,0x00,0x00,0xa3,0x82,0x81,0xf0,0x0c,0x00,0x27,0x90,
-0x00,0x00,0x00,0x00,0x8f,0xbf,0x00,0x14,0x8f,0xb0,0x00,0x10,0x03,0xe0,0x00,0x08,
-0x27,0xbd,0x00,0x18,0x0c,0x00,0x28,0x27,0x00,0x00,0x00,0x00,0x08,0x00,0x28,0xa3,
-0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x03,0x3c,0x03,0x80,0x01,0x34,0x42,0x00,0x20,
-0x24,0x63,0xa3,0x74,0x27,0xbd,0xff,0xe0,0xac,0x43,0x00,0x00,0x3c,0x02,0x80,0x01,
-0xaf,0xb2,0x00,0x18,0xaf,0xb1,0x00,0x14,0xaf,0xb0,0x00,0x10,0xaf,0xbf,0x00,0x1c,
-0x00,0x80,0x80,0x21,0x24,0x52,0x9e,0x28,0x00,0x00,0x88,0x21,0x3c,0x03,0xb0,0x09,
-0x34,0x63,0x00,0x06,0x8e,0x06,0x00,0x04,0x90,0x62,0x00,0x00,0x00,0x06,0x22,0x02,
-0x00,0x44,0x10,0x23,0x24,0x44,0x00,0x40,0x28,0x83,0x00,0x00,0x24,0x42,0x00,0x7f,
-0x00,0x83,0x10,0x0a,0x00,0x02,0x11,0x83,0x00,0x02,0x11,0x80,0x24,0x84,0xff,0xff,
-0x10,0x44,0x00,0x68,0x00,0x00,0x28,0x21,0x3c,0x02,0xb0,0x01,0x00,0xc2,0x10,0x21,
-0x8c,0x44,0x00,0x04,0x3c,0x03,0x7c,0x00,0x34,0x63,0x00,0xf0,0x00,0x83,0x18,0x24,
-0xae,0x04,0x00,0x44,0x8c,0x44,0x00,0x00,0x10,0x60,0x00,0x69,0x00,0x00,0x38,0x21,
-0x3c,0x09,0xb0,0x03,0x3c,0x06,0x7c,0x00,0x35,0x29,0x00,0x99,0x3c,0x0a,0xb0,0x01,
-0x24,0x08,0x00,0x40,0x34,0xc6,0x00,0xf0,0x3c,0x0b,0xff,0xff,0x3c,0x0c,0x28,0x38,
-0x16,0x20,0x00,0x06,0x24,0xa5,0x00,0x01,0x93,0x82,0x81,0xf6,0x24,0x11,0x00,0x01,
-0x24,0x42,0x00,0x01,0xa1,0x22,0x00,0x00,0xa3,0x82,0x81,0xf6,0x8e,0x02,0x00,0x04,
-0x24,0x07,0x00,0x01,0x24,0x42,0x01,0x00,0x30,0x42,0x3f,0xff,0xae,0x02,0x00,0x04,
-0x00,0x4a,0x10,0x21,0x8c,0x43,0x00,0x04,0x00,0x00,0x00,0x00,0xae,0x03,0x00,0x44,
-0x8c,0x44,0x00,0x00,0x10,0xa8,0x00,0x2d,0x00,0x66,0x18,0x24,0x14,0x60,0xff,0xec,
-0x00,0x8b,0x10,0x24,0x14,0x4c,0xff,0xea,0x24,0x02,0x00,0x01,0x10,0xe2,0x00,0x2f,
-0x3c,0x03,0xb0,0x09,0x8e,0x02,0x00,0x44,0x8e,0x04,0x00,0x60,0x00,0x02,0x1e,0x42,
-0x00,0x02,0x12,0x02,0x30,0x42,0x00,0x0f,0x30,0x63,0x00,0x01,0xae,0x02,0x00,0x00,
-0x10,0x44,0x00,0x1a,0xae,0x03,0x00,0x58,0x8e,0x02,0x00,0x64,0x8e,0x04,0x00,0x58,
-0x00,0x00,0x00,0x00,0x10,0x82,0x00,0x05,0x00,0x00,0x00,0x00,0xae,0x00,0x00,0x4c,
-0xae,0x00,0x00,0x50,0xae,0x00,0x00,0x54,0xae,0x00,0x00,0x0c,0x8e,0x03,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x03,0x10,0x80,0x00,0x50,0x10,0x21,0x8c,0x42,0x00,0x68,
-0x00,0x00,0x00,0x00,0x10,0x52,0x00,0x06,0x00,0x00,0x00,0x00,0x00,0x40,0xf8,0x09,
-0x02,0x00,0x20,0x21,0x8e,0x04,0x00,0x58,0x8e,0x03,0x00,0x00,0x00,0x00,0x00,0x00,
-0xae,0x03,0x00,0x60,0x08,0x00,0x28,0xeb,0xae,0x04,0x00,0x64,0x8e,0x02,0x00,0x64,
-0x00,0x00,0x00,0x00,0x14,0x62,0xff,0xe5,0x00,0x00,0x00,0x00,0x7a,0x02,0x0d,0x7c,
-0x8f,0xbf,0x00,0x1c,0x8f,0xb2,0x00,0x18,0x7b,0xb0,0x00,0xbc,0x00,0x43,0x10,0x26,
-0x00,0x02,0x10,0x2b,0x03,0xe0,0x00,0x08,0x27,0xbd,0x00,0x20,0x34,0x63,0x00,0x06,
-0x8e,0x04,0x00,0x04,0x90,0x62,0x00,0x00,0x00,0x04,0x22,0x02,0x00,0x44,0x10,0x23,
-0x24,0x44,0x00,0x40,0x28,0x83,0x00,0x00,0x24,0x42,0x00,0x7f,0x00,0x83,0x10,0x0a,
-0x00,0x02,0x11,0x83,0x00,0x02,0x11,0x80,0x00,0x82,0x20,0x23,0x14,0x87,0xff,0xc5,
-0x00,0x00,0x00,0x00,0x8e,0x03,0x00,0x00,0x00,0x00,0x00,0x00,0x2c,0x62,0x00,0x03,
-0x14,0x40,0x00,0x05,0x24,0x02,0x00,0x0d,0x10,0x62,0x00,0x03,0x24,0x02,0x00,0x01,
-0x08,0x00,0x29,0x4b,0xa2,0x02,0x00,0x5c,0x08,0x00,0x29,0x4b,0xa2,0x00,0x00,0x5c,
-0x3c,0x02,0xff,0xff,0x00,0x82,0x10,0x24,0x3c,0x03,0x28,0x38,0x14,0x43,0xff,0x94,
-0x24,0x02,0x00,0x01,0x08,0x00,0x29,0x23,0x00,0x00,0x00,0x00,0x3c,0x02,0xb0,0x03,
-0x3c,0x03,0x80,0x01,0x34,0x42,0x00,0x20,0x24,0x63,0xa5,0xcc,0xac,0x43,0x00,0x00,
-0x8c,0x83,0x01,0xa8,0x8c,0x82,0x01,0xac,0x00,0x80,0x40,0x21,0x10,0x62,0x00,0x20,
-0x00,0x00,0x20,0x21,0x93,0x82,0x81,0xf1,0x00,0x03,0x28,0x80,0x3c,0x07,0xb0,0x06,
-0x00,0xa8,0x18,0x21,0x24,0x04,0x00,0x01,0x8c,0x66,0x00,0xa8,0x10,0x44,0x00,0x1c,
-0x34,0xe7,0x80,0x18,0x3c,0x05,0xb0,0x01,0xaf,0x86,0xbc,0x20,0x00,0xc5,0x28,0x21,
-0x8c,0xa3,0x00,0x00,0x00,0x06,0x20,0xc2,0x3c,0x02,0x00,0x80,0x00,0x04,0x22,0x00,
-0x00,0x82,0x20,0x25,0xaf,0x83,0xbc,0x28,0x8c,0xa2,0x00,0x04,0xac,0xe4,0x00,0x00,
-0x8d,0x03,0x01,0xa8,0xaf,0x82,0xbc,0x24,0x24,0x64,0x00,0x01,0x04,0x80,0x00,0x0a,
-0x00,0x80,0x10,0x21,0x00,0x02,0x11,0x83,0x8d,0x03,0x01,0xac,0x00,0x02,0x11,0x80,
-0x00,0x82,0x10,0x23,0x00,0x43,0x18,0x26,0xad,0x02,0x01,0xa8,0x00,0x03,0x20,0x2b,
-0x03,0xe0,0x00,0x08,0x00,0x80,0x10,0x21,0x08,0x00,0x29,0x95,0x24,0x62,0x00,0x40,
-0x27,0x82,0x89,0x68,0x00,0x06,0x20,0xc2,0x00,0x04,0x22,0x00,0x00,0xa2,0x48,0x21,
-0x3c,0x02,0x00,0x80,0x00,0x82,0x58,0x25,0x93,0x82,0x81,0xf0,0x3c,0x0a,0xb0,0x06,
-0x3c,0x03,0xb0,0x01,0x2c,0x42,0x00,0x02,0x00,0xc3,0x38,0x21,0x35,0x4a,0x80,0x18,
-0x14,0x40,0xff,0xef,0x00,0x00,0x20,0x21,0x8c,0xe5,0x00,0x00,0x8d,0x23,0x00,0x00,
-0x24,0x02,0xc0,0x00,0x00,0xa2,0x10,0x24,0x00,0x43,0x10,0x25,0xac,0xe2,0x00,0x00,
-0x8d,0x04,0x01,0xa8,0x27,0x83,0x8a,0x68,0x8c,0xe5,0x00,0x04,0x00,0x04,0x20,0x80,
-0x00,0x83,0x20,0x21,0x8c,0x82,0x00,0x00,0x3c,0x03,0x80,0x00,0x00,0xa2,0x10,0x25,
-0x00,0x43,0x10,0x25,0xac,0xe2,0x00,0x04,0xaf,0x86,0xbc,0x20,0x8c,0xe2,0x00,0x00,
-0x93,0x85,0x81,0xf0,0xaf,0x82,0xbc,0x28,0x8c,0xe3,0x00,0x04,0xad,0x4b,0x00,0x00,
-0x8d,0x02,0x01,0xa8,0xaf,0x83,0xbc,0x24,0x24,0xa5,0xff,0xff,0x24,0x44,0x00,0x01,
-0x28,0x83,0x00,0x00,0x24,0x42,0x00,0x40,0x00,0x83,0x10,0x0a,0x00,0x02,0x11,0x83,
-0x00,0x02,0x11,0x80,0x00,0x82,0x20,0x23,0xad,0x04,0x01,0xa8,0xa3,0x85,0x81,0xf0,
-0x79,0x02,0x0d,0x7c,0x00,0x00,0x00,0x00,0x00,0x43,0x10,0x26,0x08,0x00,0x29,0x9c,
-0x00,0x02,0x20,0x2b,0x3c,0x04,0xb0,0x03,0x3c,0x06,0xb0,0x07,0x3c,0x02,0x80,0x01,
-0x34,0xc6,0x00,0x18,0x34,0x84,0x00,0x20,0x24,0x42,0xa7,0x54,0x24,0x03,0xff,0x83,
-0xac,0x82,0x00,0x00,0xa0,0xc3,0x00,0x00,0x90,0xc4,0x00,0x00,0x27,0xbd,0xff,0xf8,
-0x3c,0x03,0xb0,0x07,0x24,0x02,0xff,0x82,0xa3,0xa4,0x00,0x00,0xa0,0x62,0x00,0x00,
-0x90,0x64,0x00,0x00,0x3c,0x02,0xb0,0x07,0x34,0x42,0x00,0x08,0xa3,0xa4,0x00,0x01,
-0xa0,0x40,0x00,0x00,0x90,0x43,0x00,0x00,0x24,0x02,0x00,0x03,0x3c,0x05,0xb0,0x07,
-0xa3,0xa3,0x00,0x00,0xa0,0xc2,0x00,0x00,0x90,0xc4,0x00,0x00,0x34,0xa5,0x00,0x10,
-0x24,0x02,0x00,0x06,0x3c,0x03,0xb0,0x07,0xa3,0xa4,0x00,0x00,0x34,0x63,0x00,0x38,
-0xa0,0xa2,0x00,0x00,0x90,0x64,0x00,0x00,0x3c,0x02,0xb0,0x07,0x34,0x42,0x00,0x20,
-0xa3,0xa4,0x00,0x00,0xa0,0xa0,0x00,0x00,0x90,0xa3,0x00,0x00,0xaf,0x82,0xbf,0x30,
-0xa3,0xa3,0x00,0x00,0xa0,0x40,0x00,0x00,0x90,0x43,0x00,0x00,0x03,0xe0,0x00,0x08,
-0x27,0xbd,0x00,0x08,};
-
-u8 Rtl8192PciEFwDataArray[DataArrayLengthPciE] = {
-0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x00,0x00,0x00,0x00,0x10,0x00,0x08,0x00,
-0x02,0xe9,0x01,0x74,0x02,0xab,0x01,0xc7,0x01,0x55,0x00,0xe4,0x00,0xab,0x00,0x72,
-0x00,0x55,0x00,0x4c,0x00,0x4c,0x00,0x4c,0x00,0x4c,0x00,0x4c,0x02,0x76,0x01,0x3b,
-0x00,0xd2,0x00,0x9e,0x00,0x69,0x00,0x4f,0x00,0x46,0x00,0x3f,0x01,0x3b,0x00,0x9e,
-0x00,0x69,0x00,0x4f,0x00,0x35,0x00,0x27,0x00,0x23,0x00,0x20,0x01,0x2f,0x00,0x98,
-0x00,0x65,0x00,0x4c,0x00,0x33,0x00,0x26,0x00,0x22,0x00,0x1e,0x00,0x98,0x00,0x4c,
-0x00,0x33,0x00,0x26,0x00,0x19,0x00,0x13,0x00,0x11,0x00,0x0f,0x02,0x39,0x01,0x1c,
-0x00,0xbd,0x00,0x8e,0x00,0x5f,0x00,0x47,0x00,0x3f,0x00,0x39,0x01,0x1c,0x00,0x8e,
-0x00,0x5f,0x00,0x47,0x00,0x2f,0x00,0x23,0x00,0x20,0x00,0x1c,0x01,0x11,0x00,0x89,
-0x00,0x5b,0x00,0x44,0x00,0x2e,0x00,0x22,0x00,0x1e,0x00,0x1b,0x00,0x89,0x00,0x44,
-0x00,0x2e,0x00,0x22,0x00,0x17,0x00,0x11,0x00,0x0f,0x00,0x0e,0x02,0xab,0x02,0xab,
-0x02,0x66,0x02,0x66,0x07,0x06,0x06,0x06,0x05,0x06,0x07,0x08,0x04,0x06,0x07,0x08,
-0x09,0x0a,0x0b,0x0b,0x49,0x6e,0x74,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x54,0x4c,
-0x42,0x4d,0x4f,0x44,0x00,0x00,0x00,0x00,0x54,0x4c,0x42,0x4c,0x5f,0x64,0x61,0x74,
-0x61,0x00,0x54,0x4c,0x42,0x53,0x00,0x00,0x00,0x00,0x00,0x00,0x41,0x64,0x45,0x4c,
-0x5f,0x64,0x61,0x74,0x61,0x00,0x41,0x64,0x45,0x53,0x00,0x00,0x00,0x00,0x00,0x00,
-0x45,0x78,0x63,0x43,0x6f,0x64,0x65,0x36,0x00,0x00,0x45,0x78,0x63,0x43,0x6f,0x64,
-0x65,0x37,0x00,0x00,0x53,0x79,0x73,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x42,0x70,
-0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x52,0x49,0x00,0x00,0x00,0x00,0x00,0x00,
-0x00,0x00,0x43,0x70,0x55,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x4f,0x76,0x00,0x00,
-0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x01,0x0b,0x63,
-0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x08,0x00,0x00,0x00,0x10,0x00,0x00,0x00,0x2c,
-0x00,0x00,0x00,0x58,0x00,0x00,0x00,0x30,0x00,0x00,0x00,0x48,0x00,0x00,0x00,0x60,
-0x00,0x00,0x00,0x90,0x00,0x00,0x00,0xc0,0x00,0x00,0x01,0x20,0x00,0x00,0x01,0x80,
-0x00,0x00,0x01,0xb0,0x00,0x00,0x00,0x34,0x00,0x00,0x00,0x68,0x00,0x00,0x00,0x9c,
-0x00,0x00,0x00,0xd0,0x00,0x00,0x01,0x38,0x00,0x00,0x01,0xa0,0x00,0x00,0x01,0xd4,
-0x00,0x00,0x02,0x08,0x00,0x00,0x00,0x68,0x00,0x00,0x00,0xd0,0x00,0x00,0x01,0x38,
-0x00,0x00,0x01,0xa0,0x00,0x00,0x02,0x6f,0x00,0x00,0x03,0x40,0x00,0x00,0x03,0xa8,
-0x00,0x00,0x04,0x10,0x01,0x01,0x01,0x02,0x01,0x01,0x02,0x02,0x03,0x03,0x04,0x04,
-0x01,0x01,0x02,0x02,0x03,0x03,0x04,0x04,0x02,0x03,0x03,0x04,0x05,0x06,0x07,0x08,
-0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x00,0x07,0x6c,0x80,0x00,0x07,0x80,
-0x80,0x00,0x07,0x80,0x80,0x00,0x07,0x70,0x80,0x00,0x07,0x70,0x80,0x00,0x07,0x94,
-0x80,0x00,0x56,0xb0,0x80,0x00,0x57,0x08,0x80,0x00,0x57,0x30,0x80,0x00,0x58,0x28,
-0x80,0x00,0x58,0xe0,0x80,0x00,0x59,0x88,0x80,0x00,0x59,0xfc,0x80,0x00,0x5b,0x08,
-0x80,0x00,0x5b,0x40,0x80,0x00,0x5b,0x54,0x80,0x00,0x5b,0x68,0x80,0x00,0x5c,0x50,
-0x80,0x00,0x5c,0x90,0x80,0x00,0x5d,0x44,0x80,0x00,0x5d,0x6c,0x80,0x00,0x56,0x70,
-0x80,0x00,0x5d,0xbc,0x80,0x00,0x64,0x48,0x80,0x00,0x64,0xc0,0x80,0x00,0x64,0xcc,
-0x80,0x00,0x64,0xd8,0x80,0x00,0x64,0x60,0x80,0x00,0x64,0x60,0x80,0x00,0x64,0x60,
-0x80,0x00,0x64,0x60,0x80,0x00,0x64,0x60,0x80,0x00,0x64,0x60,0x80,0x00,0x64,0x60,
-0x80,0x00,0x64,0x60,0x80,0x00,0x64,0x60,0x80,0x00,0x64,0x60,0x80,0x00,0x64,0x60,
-0x80,0x00,0x64,0x60,0x80,0x00,0x64,0xe4,0x80,0x00,0x64,0xf0,0x80,0x00,0x64,0xfc,
-0x80,0x00,0x87,0xa4,0x80,0x00,0x87,0xa4,0x80,0x00,0x87,0xa4,0x80,0x00,0x87,0xd8,
-0x80,0x00,0x88,0x18,0x80,0x00,0x88,0x50,0x80,0x00,0x88,0x80,0x80,0x00,0x88,0xb0,
-0x80,0x00,0x88,0xc4,0x80,0x00,0x89,0x2c,0x80,0x00,0x89,0x40,0x80,0x00,0x89,0x7c,
-0x80,0x00,0x89,0x84,0x80,0x00,0x89,0xc0,0x80,0x00,0x89,0xd4,0x80,0x00,0x89,0xdc,
-0x80,0x00,0x89,0xe4,0x80,0x00,0x89,0xe4,0x80,0x00,0x89,0xe4,0x80,0x00,0x89,0xe4,
-0x80,0x00,0x8a,0x14,0x80,0x00,0x8a,0x28,0x80,0x00,0x8a,0x3c,0x80,0x00,0x86,0xe8,
-0x80,0x00,0x8d,0x68,0x80,0x00,0x8d,0x68,0x80,0x00,0x8d,0x68,0x80,0x00,0x8d,0x9c,
-0x80,0x00,0x8d,0xdc,0x80,0x00,0x8e,0x14,0x80,0x00,0x8e,0x44,0x80,0x00,0x8e,0x74,
-0x80,0x00,0x8e,0x88,0x80,0x00,0x8e,0xf0,0x80,0x00,0x8f,0x04,0x80,0x00,0x8f,0x40,
-0x80,0x00,0x8f,0x48,0x80,0x00,0x8f,0x84,0x80,0x00,0x8f,0x98,0x80,0x00,0x8f,0xa0,
-0x80,0x00,0x8f,0xa8,0x80,0x00,0x8f,0xa8,0x80,0x00,0x8f,0xa8,0x80,0x00,0x8f,0xa8,
-0x80,0x00,0x8f,0xd8,0x80,0x00,0x8f,0xec,0x80,0x00,0x90,0x00,0x80,0x00,0x8b,0x88,
-};
-
 u32 Rtl8192PciEPHY_REGArray[PHY_REGArrayLengthPciE] = {0x0,};
 
 u32 Rtl8192PciEPHY_REG_1T2RArray[PHY_REG_1T2RArrayLengthPciE] = {
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c
index 5d6d304c9c0..88489215dc2 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c
@@ -729,7 +729,6 @@ u8 rtl8192_phy_ConfigRFWithHeaderFile(struct net_device *dev,
 {
 
 	int i;
-	u8 ret = 0;
 
 	switch (eRFPath) {
 	case RF90_PATH_A:
@@ -787,7 +786,7 @@ u8 rtl8192_phy_ConfigRFWithHeaderFile(struct net_device *dev,
 		break;
 	}
 
-	return ret;
+	return 0;
 
 }
 static void rtl8192_SetTxPowerLevel(struct net_device *dev, u8 channel)
@@ -1555,7 +1554,7 @@ static bool SetRFPowerState8190(struct net_device *dev,
 
 		default:
 			bResult = false;
-			RT_TRACE(COMP_ERR, "SetRFPowerState8190(): unknow state"
+			RT_TRACE(COMP_ERR, "SetRFPowerState8190(): unknown state"
 				 " to set: 0x%X!!!\n", eRFPowerState);
 			break;
 		}
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_core.c b/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
index c01abc23213..2920e406030 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
@@ -1915,8 +1915,7 @@ int rtl8192_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	memcpy((unsigned char *)(skb->cb), &dev, sizeof(dev));
 	if (queue_index == TXCMD_QUEUE) {
 		rtl8192_tx_cmd(dev, skb);
-		ret = 0;
-		return ret;
+		return 0;
 	} else {
 		tcb_desc->RATRIndex = 7;
 		tcb_desc->bTxDisableRateFallBack = 1;
@@ -2926,8 +2925,7 @@ static int rtl8192_pci_probe(struct pci_dev *pdev,
 
 	dev->netdev_ops = &rtl8192_netdev_ops;
 
-	dev->wireless_handlers = (struct iw_handler_def *)
-				 &r8192_wx_handlers_def;
+	dev->wireless_handlers = &r8192_wx_handlers_def;
 	dev->ethtool_ops = &rtl819x_ethtool_ops;
 
 	dev->type = ARPHRD_ETHER;
@@ -3030,7 +3028,7 @@ bool NicIFEnableNIC(struct net_device *dev)
 		RT_TRACE(COMP_ERR, "ERR!!! %s(): Driver is already down!\n",
 			 __func__);
 		priv->bdisable_nic = false;
-		return RT_STATUS_FAILURE;
+		return false;
 	}
 
 	RT_TRACE(COMP_PS, "===========>%s()\n", __func__);
@@ -3040,7 +3038,7 @@ bool NicIFEnableNIC(struct net_device *dev)
 		RT_TRACE(COMP_ERR, "ERR!!! %s(): initialization is failed!\n",
 			 __func__);
 		priv->bdisable_nic = false;
-		return -1;
+		return false;
 	}
 	RT_TRACE(COMP_INIT, "start adapter finished\n");
 	RT_CLEAR_PS_LEVEL(pPSC, RT_RF_OFF_LEVL_HALT_NIC);
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_core.h b/drivers/staging/rtl8192e/rtl8192e/rtl_core.h
index 35fc1164eff..d1438c2f898 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_core.h
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_core.h
@@ -582,7 +582,6 @@ struct r8192_priv {
 
 	struct rt_firmware			*pFirmware;
 	enum rtl819x_loopback LoopbackMode;
-	enum firmware_source firmware_source;
 
 	struct timer_list			watch_dog_timer;
 	struct timer_list			fsync_timer;
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c
index 498995d833e..52870041fcb 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c
@@ -19,6 +19,7 @@
 
 #include <linux/string.h>
 #include "rtl_core.h"
+#include "rtl_wx.h"
 
 #define RATE_COUNT 12
 static u32 rtl8192_rates[] = {
@@ -1130,11 +1131,18 @@ static int r8192_wx_set_PromiscuousMode(struct net_device *dev,
 	struct r8192_priv *priv = rtllib_priv(dev);
 	struct rtllib_device *ieee = priv->rtllib;
 
-	u32 *info_buf = (u32 *)(wrqu->data.pointer);
+	u32 info_buf[3];
 
-	u32 oid = info_buf[0];
-	u32 bPromiscuousOn = info_buf[1];
-	u32 bFilterSourceStationFrame = info_buf[2];
+	u32 oid;
+	u32 bPromiscuousOn;
+	u32 bFilterSourceStationFrame;
+
+	if (copy_from_user(info_buf, wrqu->data.pointer, sizeof(info_buf)))
+		return -EFAULT;
+
+	oid = info_buf[0];
+	bPromiscuousOn = info_buf[1];
+	bFilterSourceStationFrame = info_buf[2];
 
 	if (OID_RT_INTEL_PROMISCUOUS_MODE == oid) {
 		ieee->IntelPromiscuousModeInfo.bPromiscuousOn =
@@ -1213,7 +1221,7 @@ static iw_handler r8192_wx_handlers[] = {
 };
 
 /*
- * the following rule need to be follwing,
+ * the following rule need to be following,
  * Odd : get (world access),
  * even : set (root access)
  * */
@@ -1320,7 +1328,7 @@ static struct iw_statistics *r8192_get_wireless_stats(struct net_device *dev)
 	return wstats;
 }
 
-struct iw_handler_def  r8192_wx_handlers_def = {
+const struct iw_handler_def r8192_wx_handlers_def = {
 	.standard = r8192_wx_handlers,
 	.num_standard = ARRAY_SIZE(r8192_wx_handlers),
 	.private = r8192_private_handler,
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_wx.h b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.h
index 6a51a25ec87..58398517f5b 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_wx.h
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.h
@@ -24,8 +24,7 @@ struct net_device;
 struct iw_handler_def;
 struct iw_statistics;
 
-extern struct iw_handler_def r8192_wx_handlers_def;
-struct iw_statistics *r8192_get_wireless_stats(struct net_device *dev);
+extern const struct iw_handler_def r8192_wx_handlers_def;
 u16 rtl8192_11n_user_show_rates(struct net_device *dev);
 
 #endif
diff --git a/drivers/staging/rtl8192e/rtl819x_BAProc.c b/drivers/staging/rtl8192e/rtl819x_BAProc.c
index adc6cc7ca3d..48a142b82d9 100644
--- a/drivers/staging/rtl8192e/rtl819x_BAProc.c
+++ b/drivers/staging/rtl8192e/rtl819x_BAProc.c
@@ -109,9 +109,9 @@ static struct sk_buff *rtllib_ADDBA(struct rtllib_device *ieee, u8 *Dst,
 	BAReq->frame_ctl = cpu_to_le16(RTLLIB_STYPE_MANAGE_ACT);
 
 	tag = (u8 *)skb_put(skb, 9);
-	*tag ++= ACT_CAT_BA;
-	*tag ++= type;
-	*tag ++= pBA->DialogToken;
+	*tag++ = ACT_CAT_BA;
+	*tag++ = type;
+	*tag++ = pBA->DialogToken;
 
 	if (ACT_ADDBARSP == type) {
 		RT_TRACE(COMP_DBG, "====>to send ADDBARSP\n");
@@ -175,8 +175,8 @@ static struct sk_buff *rtllib_DELBA(struct rtllib_device *ieee, u8 *dst,
 
 	tag = (u8 *)skb_put(skb, 6);
 
-	*tag ++= ACT_CAT_BA;
-	*tag ++= ACT_DELBA;
+	*tag++ = ACT_CAT_BA;
+	*tag++ = ACT_DELBA;
 
 	tmp = DelbaParamSet.shortData;
 	memcpy(tag, (u8 *)&tmp, 2);
@@ -230,8 +230,8 @@ static void rtllib_send_DELBA(struct rtllib_device *ieee, u8 *dst,
 	if (skb)
 		softmac_mgmt_xmit(skb, ieee);
 	else
-		RTLLIB_DEBUG(RTLLIB_DL_ERR, "alloc skb error in func"
-			     "tion %s()\n", __func__);
+		RTLLIB_DEBUG(RTLLIB_DL_ERR, "alloc skb error in function"
+			     " %s()\n", __func__);
 	return ;
 }
 
diff --git a/drivers/staging/rtl8192e/rtl819x_TSProc.c b/drivers/staging/rtl8192e/rtl819x_TSProc.c
index 29608e5488a..52b297724fb 100644
--- a/drivers/staging/rtl8192e/rtl819x_TSProc.c
+++ b/drivers/staging/rtl8192e/rtl819x_TSProc.c
@@ -79,7 +79,7 @@ static void RxPktPendingTimeout(unsigned long data)
 
 		if (index > REORDER_WIN_SIZE) {
 			RTLLIB_DEBUG(RTLLIB_DL_ERR, "RxReorderIndicatePacket():"
-				     " Rx Reorer struct buffer full!!\n");
+				     " Rx Reorder struct buffer full!!\n");
 			spin_unlock_irqrestore(&(ieee->reorder_spinlock),
 					       flags);
 			return;
diff --git a/drivers/staging/rtl8192e/rtllib_rx.c b/drivers/staging/rtl8192e/rtllib_rx.c
index 6c8a8e12b2b..60de54cc601 100644
--- a/drivers/staging/rtl8192e/rtllib_rx.c
+++ b/drivers/staging/rtl8192e/rtllib_rx.c
@@ -712,7 +712,7 @@ static void RxReorderIndicatePacket(struct rtllib_device *ieee,
 
 		if (index > REORDER_WIN_SIZE) {
 			RTLLIB_DEBUG(RTLLIB_DL_ERR, "RxReorderIndicatePacket():"
-				     " Rx Reorer struct buffer full!!\n");
+				     " Rx Reorder struct buffer full!!\n");
 			spin_unlock_irqrestore(&(ieee->reorder_spinlock),
 					       flags);
 			return;
@@ -1589,7 +1589,6 @@ static int rtllib_qos_convert_ac_to_parameters(struct rtllib_qos_parameter_info
 {
 	struct rtllib_qos_ac_parameter *ac_params;
 	struct rtllib_qos_parameters *qos_param = &(qos_data->parameters);
-	int rc = 0;
 	int i;
 	u8 aci;
 	u8 acm;
@@ -1640,7 +1639,7 @@ static int rtllib_qos_convert_ac_to_parameters(struct rtllib_qos_parameter_info
 		    (ac_params->aci_aifsn & 0x10) ? 0x01 : 0x00;
 		qos_param->tx_op_limit[aci] = ac_params->tx_op_limit;
 	}
-	return rc;
+	return 0;
 }
 
 /*
diff --git a/drivers/staging/rtl8192e/rtllib_softmac.c b/drivers/staging/rtl8192e/rtllib_softmac.c
index 4bf72bc1ba7..684ceed1723 100644
--- a/drivers/staging/rtl8192e/rtllib_softmac.c
+++ b/drivers/staging/rtl8192e/rtllib_softmac.c
@@ -543,7 +543,7 @@ void rtllib_softmac_scan_syncro(struct rtllib_device *ieee, u8 is_mesh)
 				goto out; /* scan completed */
 		} while (!ieee->active_channel_map[ch]);
 
-		/* this fuction can be called in two situations
+		/* this function can be called in two situations
 		 * 1- We have switched to ad-hoc mode and we are
 		 *    performing a complete syncro scan before conclude
 		 *    there are no interesting cell and to create a
@@ -3239,7 +3239,6 @@ static int rtllib_wpa_set_auth_algs(struct rtllib_device *ieee, int value)
 	struct rtllib_security sec = {
 		.flags = SEC_AUTH_MODE,
 	};
-	int ret = 0;
 
 	if (value & AUTH_ALG_SHARED_KEY) {
 		sec.auth_mode = WLAN_AUTH_SHARED_KEY;
@@ -3259,7 +3258,7 @@ static int rtllib_wpa_set_auth_algs(struct rtllib_device *ieee, int value)
 	if (ieee->set_security)
 		ieee->set_security(ieee->dev, &sec);
 
-	return ret;
+	return 0;
 }
 
 static int rtllib_wpa_set_param(struct rtllib_device *ieee, u8 name, u32 value)
diff --git a/drivers/staging/rtl8192e/rtllib_softmac_wx.c b/drivers/staging/rtl8192e/rtllib_softmac_wx.c
index e6af8cfab12..65b650cc3d0 100644
--- a/drivers/staging/rtl8192e/rtllib_softmac_wx.c
+++ b/drivers/staging/rtl8192e/rtllib_softmac_wx.c
@@ -628,8 +628,6 @@ int rtllib_wx_get_power(struct rtllib_device *ieee,
 				 struct iw_request_info *info,
 				 union iwreq_data *wrqu, char *extra)
 {
-	int ret = 0;
-
 	down(&ieee->wx_sem);
 
 	if (ieee->ps == RTLLIB_PS_DISABLED) {
@@ -657,7 +655,7 @@ int rtllib_wx_get_power(struct rtllib_device *ieee,
 
 exit:
 	up(&ieee->wx_sem);
-	return ret;
+	return 0;
 
 }
 EXPORT_SYMBOL(rtllib_wx_get_power);
diff --git a/drivers/staging/rtl8192ee/Kconfig b/drivers/staging/rtl8192ee/Kconfig
new file mode 100644
index 00000000000..beb07ac24e8
--- /dev/null
+++ b/drivers/staging/rtl8192ee/Kconfig
@@ -0,0 +1,15 @@
+config R8192EE
+	tristate "Realtek RTL8192EE Wireless Network Adapter"
+	depends on PCI && WLAN && MAC80211
+	depends on m
+	select WIRELESS_EXT
+	select WEXT_PRIV
+	select EEPROM_93CX6
+	select CRYPTO
+	select FW_LOADER
+	default N
+	---help---
+	This is the driver for Realtek RTL8192EE 802.11 PCIe
+	wireless network adapters.
+
+	If you choose to build it as a module, it will be called r8192ee
diff --git a/drivers/staging/rtl8192ee/Makefile b/drivers/staging/rtl8192ee/Makefile
new file mode 100644
index 00000000000..31708620d7e
--- /dev/null
+++ b/drivers/staging/rtl8192ee/Makefile
@@ -0,0 +1,40 @@
+
+PCI_MAIN_OBJS	:= base.o	\
+		cam.o	\
+		core.o	\
+		debug.o	\
+		efuse.o	\
+		pci.o	\
+		ps.o	\
+		rc.o	\
+		regd.o	\
+		stats.o
+
+PCI_8192EE_HAL_OBJS:=		\
+	rtl8192ee/dm.o		\
+	rtl8192ee/fw.o		\
+	rtl8192ee/hw.o		\
+	rtl8192ee/led.o		\
+	rtl8192ee/phy.o		\
+	rtl8192ee/pwrseq.o	\
+	rtl8192ee/pwrseqcmd.o	\
+	rtl8192ee/rf.o		\
+	rtl8192ee/sw.o		\
+	rtl8192ee/table.o	\
+	rtl8192ee/trx.o
+
+
+
+BT_COEXIST_OBJS:=	btcoexist/halbtc8192e2ant.o\
+			btcoexist/halbtc8723b1ant.o\
+			btcoexist/halbtc8723b2ant.o\
+			btcoexist/halbtc8821a1ant.o\
+			btcoexist/halbtc8821a2ant.o\
+			btcoexist/halbtcoutsrc.o\
+			btcoexist/rtl_btc.o
+
+r8192ee-objs += $(PCI_MAIN_OBJS) $(PCI_8192EE_HAL_OBJS) $(BT_COEXIST_OBJS)
+
+obj-$(CONFIG_R8192EE) += r8192ee.o
+
+ccflags-y += -D__CHECK_ENDIAN__
diff --git a/drivers/staging/rtl8192ee/TODO b/drivers/staging/rtl8192ee/TODO
new file mode 100644
index 00000000000..162092a0316
--- /dev/null
+++ b/drivers/staging/rtl8192ee/TODO
@@ -0,0 +1,12 @@
+TODO:
+- convert any remaining unusual variable types
+- find codes that can use %pM and %Nph formatting
+- checkpatch.pl fixes - most of the remaining ones are lines too long. Many
+  of them will require refactoring
+- merge Realtek's bugfixes and new features into the driver
+- Convert the versions of rtlwifi and btcoexist in drivers/net/wireless/rtlwifi/...
+  to work with the RTL8192EE
+- move this driver to drivers/net/wireless/rtlwifi
+
+Please send any patches to Greg Kroah-Hartman <gregkh@linux.com>,
+and Larry Finger <Larry.Finger@lwfinger.net>.
diff --git a/drivers/staging/rtl8192ee/base.c b/drivers/staging/rtl8192ee/base.c
new file mode 100644
index 00000000000..64ade216a15
--- /dev/null
+++ b/drivers/staging/rtl8192ee/base.c
@@ -0,0 +1,1852 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include <linux/ip.h>
+#include <linux/module.h>
+#include "wifi.h"
+#include "rc.h"
+#include "base.h"
+#include "efuse.h"
+#include "cam.h"
+#include "ps.h"
+#include "regd.h"
+#include "pci.h"
+
+/*
+ *NOTICE!!!: This file will be very big, we hsould
+ *keep it clear under follwing roles:
+ *
+ *This file include follwing part, so, if you add new
+ *functions into this file, please check which part it
+ *should includes. or check if you should add new part
+ *for this file:
+ *
+ *1) mac80211 init functions
+ *2) tx information functions
+ *3) functions called by core.c
+ *4) wq & timer callback functions
+ *5) frame process functions
+ *6) IOT functions
+ *7) sysfs functions
+ *8) vif functions
+ *9) ...
+ */
+
+/*********************************************************
+ *
+ * mac80211 init functions
+ *
+ *********************************************************/
+static struct ieee80211_channel rtl_channeltable_2g[] = {
+	{.center_freq = 2412, .hw_value = 1,},
+	{.center_freq = 2417, .hw_value = 2,},
+	{.center_freq = 2422, .hw_value = 3,},
+	{.center_freq = 2427, .hw_value = 4,},
+	{.center_freq = 2432, .hw_value = 5,},
+	{.center_freq = 2437, .hw_value = 6,},
+	{.center_freq = 2442, .hw_value = 7,},
+	{.center_freq = 2447, .hw_value = 8,},
+	{.center_freq = 2452, .hw_value = 9,},
+	{.center_freq = 2457, .hw_value = 10,},
+	{.center_freq = 2462, .hw_value = 11,},
+	{.center_freq = 2467, .hw_value = 12,},
+	{.center_freq = 2472, .hw_value = 13,},
+	{.center_freq = 2484, .hw_value = 14,},
+};
+
+static struct ieee80211_channel rtl_channeltable_5g[] = {
+	{.center_freq = 5180, .hw_value = 36,},
+	{.center_freq = 5200, .hw_value = 40,},
+	{.center_freq = 5220, .hw_value = 44,},
+	{.center_freq = 5240, .hw_value = 48,},
+	{.center_freq = 5260, .hw_value = 52,},
+	{.center_freq = 5280, .hw_value = 56,},
+	{.center_freq = 5300, .hw_value = 60,},
+	{.center_freq = 5320, .hw_value = 64,},
+	{.center_freq = 5500, .hw_value = 100,},
+	{.center_freq = 5520, .hw_value = 104,},
+	{.center_freq = 5540, .hw_value = 108,},
+	{.center_freq = 5560, .hw_value = 112,},
+	{.center_freq = 5580, .hw_value = 116,},
+	{.center_freq = 5600, .hw_value = 120,},
+	{.center_freq = 5620, .hw_value = 124,},
+	{.center_freq = 5640, .hw_value = 128,},
+	{.center_freq = 5660, .hw_value = 132,},
+	{.center_freq = 5680, .hw_value = 136,},
+	{.center_freq = 5700, .hw_value = 140,},
+	{.center_freq = 5745, .hw_value = 149,},
+	{.center_freq = 5765, .hw_value = 153,},
+	{.center_freq = 5785, .hw_value = 157,},
+	{.center_freq = 5805, .hw_value = 161,},
+	{.center_freq = 5825, .hw_value = 165,},
+};
+
+static struct ieee80211_rate rtl_ratetable_2g[] = {
+	{.bitrate = 10, .hw_value = 0x00,},
+	{.bitrate = 20, .hw_value = 0x01,},
+	{.bitrate = 55, .hw_value = 0x02,},
+	{.bitrate = 110, .hw_value = 0x03,},
+	{.bitrate = 60, .hw_value = 0x04,},
+	{.bitrate = 90, .hw_value = 0x05,},
+	{.bitrate = 120, .hw_value = 0x06,},
+	{.bitrate = 180, .hw_value = 0x07,},
+	{.bitrate = 240, .hw_value = 0x08,},
+	{.bitrate = 360, .hw_value = 0x09,},
+	{.bitrate = 480, .hw_value = 0x0a,},
+	{.bitrate = 540, .hw_value = 0x0b,},
+};
+
+static struct ieee80211_rate rtl_ratetable_5g[] = {
+	{.bitrate = 60, .hw_value = 0x04,},
+	{.bitrate = 90, .hw_value = 0x05,},
+	{.bitrate = 120, .hw_value = 0x06,},
+	{.bitrate = 180, .hw_value = 0x07,},
+	{.bitrate = 240, .hw_value = 0x08,},
+	{.bitrate = 360, .hw_value = 0x09,},
+	{.bitrate = 480, .hw_value = 0x0a,},
+	{.bitrate = 540, .hw_value = 0x0b,},
+};
+
+static const struct ieee80211_supported_band rtl_band_2ghz = {
+	.band = IEEE80211_BAND_2GHZ,
+
+	.channels = rtl_channeltable_2g,
+	.n_channels = ARRAY_SIZE(rtl_channeltable_2g),
+
+	.bitrates = rtl_ratetable_2g,
+	.n_bitrates = ARRAY_SIZE(rtl_ratetable_2g),
+
+	.ht_cap = {0},
+};
+
+static struct ieee80211_supported_band rtl_band_5ghz = {
+	.band = IEEE80211_BAND_5GHZ,
+
+	.channels = rtl_channeltable_5g,
+	.n_channels = ARRAY_SIZE(rtl_channeltable_5g),
+
+	.bitrates = rtl_ratetable_5g,
+	.n_bitrates = ARRAY_SIZE(rtl_ratetable_5g),
+
+	.ht_cap = {0},
+};
+
+static const u8 tid_to_ac[] = {
+	2, /* IEEE80211_AC_BE */
+	3, /* IEEE80211_AC_BK */
+	3, /* IEEE80211_AC_BK */
+	2, /* IEEE80211_AC_BE */
+	1, /* IEEE80211_AC_VI */
+	1, /* IEEE80211_AC_VI */
+	0, /* IEEE80211_AC_VO */
+	0, /* IEEE80211_AC_VO */
+};
+
+u8 rtl92e_tid_to_ac(struct ieee80211_hw *hw, u8 tid)
+{
+	return tid_to_ac[tid];
+}
+
+static void _rtl_init_hw_ht_capab(struct ieee80211_hw *hw,
+				  struct ieee80211_sta_ht_cap *ht_cap)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	ht_cap->ht_supported = true;
+	ht_cap->cap = IEEE80211_HT_CAP_SUP_WIDTH_20_40 |
+	    IEEE80211_HT_CAP_SGI_40 |
+	    IEEE80211_HT_CAP_SGI_20 |
+	    IEEE80211_HT_CAP_DSSSCCK40 | IEEE80211_HT_CAP_MAX_AMSDU;
+
+	if (rtlpriv->rtlhal.disable_amsdu_8k)
+		ht_cap->cap &= ~IEEE80211_HT_CAP_MAX_AMSDU;
+
+	/*
+	 *Maximum length of AMPDU that the STA can receive.
+	 *Length = 2 ^ (13 + max_ampdu_length_exp) - 1 (octets)
+	 */
+	ht_cap->ampdu_factor = IEEE80211_HT_MAX_AMPDU_64K;
+
+	/*Minimum MPDU start spacing , */
+	ht_cap->ampdu_density = IEEE80211_HT_MPDU_DENSITY_16;
+
+	ht_cap->mcs.tx_params = IEEE80211_HT_MCS_TX_DEFINED;
+
+	/*
+	 *hw->wiphy->bands[IEEE80211_BAND_2GHZ]
+	 *base on ant_num
+	 *rx_mask: RX mask
+	 *if rx_ant =1 rx_mask[0]=0xff;==>MCS0-MCS7
+	 *if rx_ant =2 rx_mask[1]=0xff;==>MCS8-MCS15
+	 *if rx_ant >=3 rx_mask[2]=0xff;
+	 *if BW_40 rx_mask[4]=0x01;
+	 *highest supported RX rate
+	 */
+	if (rtlpriv->dm.supp_phymode_switch) {
+		RT_TRACE(COMP_INIT, DBG_EMERG, ("Support phy mode switch\n"));
+
+		ht_cap->mcs.rx_mask[0] = 0xFF;
+		ht_cap->mcs.rx_mask[1] = 0xFF;
+		ht_cap->mcs.rx_mask[4] = 0x01;
+
+		ht_cap->mcs.rx_highest = cpu_to_le16(MAX_BIT_RATE_40MHZ_MCS15);
+	} else {
+		if (get_rf_type(rtlphy) == RF_1T2R ||
+		    get_rf_type(rtlphy) == RF_2T2R) {
+			RT_TRACE(COMP_INIT, DBG_DMESG, ("1T2R or 2T2R\n"));
+
+			ht_cap->mcs.rx_mask[0] = 0xFF;
+			ht_cap->mcs.rx_mask[1] = 0xFF;
+			ht_cap->mcs.rx_mask[4] = 0x01;
+
+			ht_cap->mcs.rx_highest = cpu_to_le16(MAX_BIT_RATE_40MHZ_MCS15);
+		} else if (get_rf_type(rtlphy) == RF_1T1R) {
+			RT_TRACE(COMP_INIT, DBG_DMESG, ("1T1R\n"));
+
+			ht_cap->mcs.rx_mask[0] = 0xFF;
+			ht_cap->mcs.rx_mask[1] = 0x00;
+			ht_cap->mcs.rx_mask[4] = 0x01;
+
+			ht_cap->mcs.rx_highest = cpu_to_le16(MAX_BIT_RATE_40MHZ_MCS7);
+		}
+	}
+}
+
+static void _rtl_init_hw_vht_capab(struct ieee80211_hw *hw,
+				   struct ieee80211_sta_vht_cap *vht_cap)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+		u16 mcs_map;
+		vht_cap->vht_supported = true;
+		vht_cap->cap =
+			IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_3895 |
+			IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_7991 |
+			IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_11454 |
+			IEEE80211_VHT_CAP_SHORT_GI_80 |
+			IEEE80211_VHT_CAP_TXSTBC |
+			IEEE80211_VHT_CAP_RXSTBC_1 |
+			IEEE80211_VHT_CAP_SU_BEAMFORMER_CAPABLE |
+			IEEE80211_VHT_CAP_SU_BEAMFORMEE_CAPABLE |
+			IEEE80211_VHT_CAP_HTC_VHT |
+			IEEE80211_VHT_CAP_MAX_A_MPDU_LENGTH_EXPONENT_MASK |
+			IEEE80211_VHT_CAP_RX_ANTENNA_PATTERN |
+			IEEE80211_VHT_CAP_TX_ANTENNA_PATTERN |
+			0;
+
+		mcs_map = IEEE80211_VHT_MCS_SUPPORT_0_9 << 0 |
+			IEEE80211_VHT_MCS_SUPPORT_0_9 << 2 |
+			IEEE80211_VHT_MCS_NOT_SUPPORTED << 4 |
+			IEEE80211_VHT_MCS_NOT_SUPPORTED << 6 |
+			IEEE80211_VHT_MCS_NOT_SUPPORTED << 8 |
+			IEEE80211_VHT_MCS_NOT_SUPPORTED << 10 |
+			IEEE80211_VHT_MCS_NOT_SUPPORTED << 12 |
+			IEEE80211_VHT_MCS_NOT_SUPPORTED << 14;
+
+		vht_cap->vht_mcs.rx_mcs_map = cpu_to_le16(mcs_map);
+		vht_cap->vht_mcs.rx_highest =
+			cpu_to_le16(MAX_BIT_RATE_SHORT_GI_2NSS_80MHZ_MCS9);
+		vht_cap->vht_mcs.tx_mcs_map = cpu_to_le16(mcs_map);
+		vht_cap->vht_mcs.tx_highest =
+			cpu_to_le16(MAX_BIT_RATE_SHORT_GI_2NSS_80MHZ_MCS9);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+		u16 mcs_map;
+
+		vht_cap->vht_supported = true;
+		vht_cap->cap =
+			IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_3895 |
+			IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_7991 |
+			IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_11454 |
+			IEEE80211_VHT_CAP_SHORT_GI_80 |
+			IEEE80211_VHT_CAP_TXSTBC |
+			IEEE80211_VHT_CAP_RXSTBC_1 |
+			IEEE80211_VHT_CAP_SU_BEAMFORMER_CAPABLE |
+			IEEE80211_VHT_CAP_SU_BEAMFORMEE_CAPABLE |
+			IEEE80211_VHT_CAP_HTC_VHT |
+			IEEE80211_VHT_CAP_MAX_A_MPDU_LENGTH_EXPONENT_MASK |
+			IEEE80211_VHT_CAP_RX_ANTENNA_PATTERN |
+			IEEE80211_VHT_CAP_TX_ANTENNA_PATTERN |
+			0;
+
+		mcs_map = IEEE80211_VHT_MCS_SUPPORT_0_9 << 0 |
+			IEEE80211_VHT_MCS_NOT_SUPPORTED << 2 |
+			IEEE80211_VHT_MCS_NOT_SUPPORTED << 4 |
+			IEEE80211_VHT_MCS_NOT_SUPPORTED << 6 |
+			IEEE80211_VHT_MCS_NOT_SUPPORTED << 8 |
+			IEEE80211_VHT_MCS_NOT_SUPPORTED << 10 |
+			IEEE80211_VHT_MCS_NOT_SUPPORTED << 12 |
+			IEEE80211_VHT_MCS_NOT_SUPPORTED << 14;
+
+		vht_cap->vht_mcs.rx_mcs_map = cpu_to_le16(mcs_map);
+		vht_cap->vht_mcs.rx_highest =
+			cpu_to_le16(MAX_BIT_RATE_SHORT_GI_1NSS_80MHZ_MCS9);
+		vht_cap->vht_mcs.tx_mcs_map = cpu_to_le16(mcs_map);
+		vht_cap->vht_mcs.tx_highest =
+			cpu_to_le16(MAX_BIT_RATE_SHORT_GI_1NSS_80MHZ_MCS9);
+	}
+}
+
+static void _rtl_init_mac80211(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	struct rtl_mac *rtlmac = rtl_mac(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct ieee80211_supported_band *sband;
+
+
+	if (rtlhal->macphymode == SINGLEMAC_SINGLEPHY &&
+	    rtlhal->bandset == BAND_ON_BOTH) {
+		/* 1: 2.4 G bands */
+		/* <1> use  mac->bands as mem for hw->wiphy->bands */
+		sband = &(rtlmac->bands[IEEE80211_BAND_2GHZ]);
+
+		/* <2> set hw->wiphy->bands[IEEE80211_BAND_2GHZ]
+		 * to default value(1T1R) */
+		memcpy(&(rtlmac->bands[IEEE80211_BAND_2GHZ]), &rtl_band_2ghz,
+		       sizeof(struct ieee80211_supported_band));
+
+		/* <3> init ht cap base on ant_num */
+		_rtl_init_hw_ht_capab(hw, &sband->ht_cap);
+
+		/* <4> set mac->sband to wiphy->sband */
+		hw->wiphy->bands[IEEE80211_BAND_2GHZ] = sband;
+
+		/* 2: 5 G bands */
+		/* <1> use  mac->bands as mem for hw->wiphy->bands */
+		sband = &(rtlmac->bands[IEEE80211_BAND_5GHZ]);
+
+		/* <2> set hw->wiphy->bands[IEEE80211_BAND_5GHZ]
+		 * to default value(1T1R) */
+		memcpy(&(rtlmac->bands[IEEE80211_BAND_5GHZ]), &rtl_band_5ghz,
+		       sizeof(struct ieee80211_supported_band));
+
+		/* <3> init ht cap base on ant_num */
+		_rtl_init_hw_ht_capab(hw, &sband->ht_cap);
+
+		_rtl_init_hw_vht_capab(hw, &sband->vht_cap);
+
+		/* <4> set mac->sband to wiphy->sband */
+		hw->wiphy->bands[IEEE80211_BAND_5GHZ] = sband;
+	} else {
+		if (rtlhal->current_bandtype == BAND_ON_2_4G) {
+			/* <1> use  mac->bands as mem for hw->wiphy->bands */
+			sband = &(rtlmac->bands[IEEE80211_BAND_2GHZ]);
+
+			/* <2> set hw->wiphy->bands[IEEE80211_BAND_2GHZ]
+			 * to default value(1T1R) */
+			memcpy(&(rtlmac->bands[IEEE80211_BAND_2GHZ]),
+			       &rtl_band_2ghz,
+			       sizeof(struct ieee80211_supported_band));
+
+			/* <3> init ht cap base on ant_num */
+			_rtl_init_hw_ht_capab(hw, &sband->ht_cap);
+
+			/* <4> set mac->sband to wiphy->sband */
+			hw->wiphy->bands[IEEE80211_BAND_2GHZ] = sband;
+		} else if (rtlhal->current_bandtype == BAND_ON_5G) {
+			/* <1> use  mac->bands as mem for hw->wiphy->bands */
+			sband = &(rtlmac->bands[IEEE80211_BAND_5GHZ]);
+
+			/* <2> set hw->wiphy->bands[IEEE80211_BAND_5GHZ]
+			 * to default value(1T1R) */
+			memcpy(&(rtlmac->bands[IEEE80211_BAND_5GHZ]),
+			       &rtl_band_5ghz,
+			       sizeof(struct ieee80211_supported_band));
+
+			/* <3> init ht cap base on ant_num */
+			_rtl_init_hw_ht_capab(hw, &sband->ht_cap);
+
+			_rtl_init_hw_vht_capab(hw, &sband->vht_cap);
+
+			/* <4> set mac->sband to wiphy->sband */
+			hw->wiphy->bands[IEEE80211_BAND_5GHZ] = sband;
+		} else {
+			RT_TRACE(COMP_INIT, DBG_EMERG,
+				 ("Err BAND %d\n", rtlhal->current_bandtype));
+		}
+	}
+	/* <5> set hw caps */
+	hw->flags = IEEE80211_HW_SIGNAL_DBM |
+	    IEEE80211_HW_RX_INCLUDES_FCS |
+	    IEEE80211_HW_AMPDU_AGGREGATION |
+	    IEEE80211_HW_REPORTS_TX_ACK_STATUS |
+	    IEEE80211_HW_CONNECTION_MONITOR |
+	    /* IEEE80211_HW_SUPPORTS_CQM_RSSI | */
+	    IEEE80211_HW_MFP_CAPABLE | 0;
+
+	/* swlps or hwlps has been set in diff chip in init_sw_vars */
+	if (rtlpriv->psc.b_swctrl_lps)
+		hw->flags |= IEEE80211_HW_SUPPORTS_PS |
+			IEEE80211_HW_PS_NULLFUNC_STACK |
+			/* IEEE80211_HW_SUPPORTS_DYNAMIC_PS | */
+			0;
+/*<delete in kernel start>*/
+	hw->wiphy->interface_modes =
+		BIT(NL80211_IFTYPE_AP) |
+		BIT(NL80211_IFTYPE_STATION) |
+		BIT(NL80211_IFTYPE_ADHOC) |
+		BIT(NL80211_IFTYPE_MESH_POINT) |
+		BIT(NL80211_IFTYPE_P2P_CLIENT) |
+		BIT(NL80211_IFTYPE_P2P_GO);
+	hw->wiphy->flags |= WIPHY_FLAG_IBSS_RSN;
+
+	hw->wiphy->flags |= WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL;
+
+	hw->wiphy->rts_threshold = 2347;
+
+	hw->queues = AC_MAX;
+	hw->extra_tx_headroom = RTL_TX_HEADER_SIZE;
+
+	/* TODO: Correct this value for our hw */
+	/* TODO: define these hard code value */
+	/* hw->channel_change_time = 100; kernel does not use it*/
+	hw->max_listen_interval = 10;
+	hw->max_rate_tries = 4;
+	/* hw->max_rates = 1; */
+	hw->sta_data_size = sizeof(struct rtl_sta_info);
+
+/* wowlan is not supported by kernel if CONFIG_PM is not defined */
+#ifdef CONFIG_PM
+	if (rtlpriv->psc.wo_wlan_mode) {
+		if (rtlpriv->psc.wo_wlan_mode & WAKE_ON_MAGIC_PACKET)
+			rtlpriv->wowlan.flags = WIPHY_WOWLAN_MAGIC_PKT;
+		if (rtlpriv->psc.wo_wlan_mode & WAKE_ON_PATTERN_MATCH) {
+			rtlpriv->wowlan.n_patterns =
+				MAX_SUPPORT_WOL_PATTERN_NUM;
+			rtlpriv->wowlan.pattern_min_len = MIN_WOL_PATTERN_SIZE;
+			rtlpriv->wowlan.pattern_max_len = MAX_WOL_PATTERN_SIZE;
+		}
+		hw->wiphy->wowlan = &(rtlpriv->wowlan);
+	}
+#endif
+
+	/* <6> mac address */
+	if (is_valid_ether_addr(rtlefuse->dev_addr)) {
+		SET_IEEE80211_PERM_ADDR(hw, rtlefuse->dev_addr);
+	} else {
+		u8 rtlmac[] = { 0x00, 0xe0, 0x4c, 0x81, 0x92, 0x00 };
+		get_random_bytes((rtlmac + (ETH_ALEN - 1)), 1);
+		SET_IEEE80211_PERM_ADDR(hw, rtlmac);
+	}
+}
+
+static void _rtl_init_deferred_work(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	/* <1> timer */
+	init_timer(&rtlpriv->works.watchdog_timer);
+	setup_timer(&rtlpriv->works.watchdog_timer,
+		    rtl92e_watch_dog_timer_callback, (unsigned long)hw);
+	init_timer(&rtlpriv->works.dualmac_easyconcurrent_retrytimer);
+	setup_timer(&rtlpriv->works.dualmac_easyconcurrent_retrytimer,
+		    rtl92e_easy_concurrent_retrytimer_callback, (unsigned long)hw);
+	/* <2> work queue */
+	rtlpriv->works.hw = hw;
+	rtlpriv->works.rtl_wq = alloc_workqueue(rtlpriv->cfg->name, 0, 0);
+	INIT_DELAYED_WORK(&rtlpriv->works.watchdog_wq,
+			  (void *)rtl92e_watchdog_wq_callback);
+	INIT_DELAYED_WORK(&rtlpriv->works.ips_nic_off_wq,
+			  (void *)rtl92e_ips_nic_off_wq_callback);
+	INIT_DELAYED_WORK(&rtlpriv->works.ps_work,
+			  (void *)rtl92e_swlps_wq_callback);
+	INIT_DELAYED_WORK(&rtlpriv->works.ps_rfon_wq,
+			  (void *)rtl92e_swlps_rfon_wq_callback);
+	INIT_DELAYED_WORK(&rtlpriv->works.fwevt_wq,
+			  (void *)rtl92e_fwevt_wq_callback);
+}
+
+void rtl92e_deinit_deferred_work(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	del_timer_sync(&rtlpriv->works.watchdog_timer);
+
+	cancel_delayed_work(&rtlpriv->works.watchdog_wq);
+	cancel_delayed_work(&rtlpriv->works.ips_nic_off_wq);
+	cancel_delayed_work(&rtlpriv->works.ps_work);
+	cancel_delayed_work(&rtlpriv->works.ps_rfon_wq);
+	cancel_delayed_work(&rtlpriv->works.fwevt_wq);
+}
+
+void rtl92e_init_rfkill(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	bool radio_state;
+	bool blocked;
+	u8 valid = 0;
+
+	/*set init state to on */
+	rtlpriv->rfkill.rfkill_state = 1;
+	wiphy_rfkill_set_hw_state(hw->wiphy, 0);
+
+	radio_state = rtlpriv->cfg->ops->radio_onoff_checking(hw, &valid);
+
+	if (valid) {
+		pr_info("rtlwifi: wireless switch is %s\n",
+			rtlpriv->rfkill.rfkill_state ? "on" : "off");
+
+		rtlpriv->rfkill.rfkill_state = radio_state;
+
+		blocked = (rtlpriv->rfkill.rfkill_state == 1) ? 0 : 1;
+		wiphy_rfkill_set_hw_state(hw->wiphy, blocked);
+	}
+
+	wiphy_rfkill_start_polling(hw->wiphy);
+}
+
+void rtl92e_deinit_rfkill(struct ieee80211_hw *hw)
+{
+	wiphy_rfkill_stop_polling(hw->wiphy);
+}
+
+#ifdef VIF_TODO
+static void rtl_init_vif(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	INIT_LIST_HEAD(&rtlpriv->vif_priv.vif_list);
+
+	rtlpriv->vif_priv.vifs = 0;
+}
+#endif
+
+int rtl92e_init_core(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *rtlmac = rtl_mac(rtl_priv(hw));
+
+	/* <1> init mac80211 */
+	_rtl_init_mac80211(hw);
+	rtlmac->hw = hw;
+	rtlmac->link_state = MAC80211_NOLINK;
+
+	/* <2> rate control register */
+	hw->rate_control_algorithm = "rtl_rc";
+
+	/*
+	 * <3> init CRDA must come after init
+	 * mac80211 hw  in _rtl_init_mac80211.
+	 */
+	if (rtl92e_regd_init(hw, rtl92e_reg_notifier)) {
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("REGD init failed\n"));
+		return 1;
+	}
+
+	/* <4> locks */
+	mutex_init(&rtlpriv->locks.conf_mutex);
+	spin_lock_init(&rtlpriv->locks.ips_lock);
+	spin_lock_init(&rtlpriv->locks.irq_th_lock);
+	spin_lock_init(&rtlpriv->locks.h2c_lock);
+	spin_lock_init(&rtlpriv->locks.rf_ps_lock);
+	spin_lock_init(&rtlpriv->locks.rf_lock);
+	spin_lock_init(&rtlpriv->locks.lps_lock);
+	spin_lock_init(&rtlpriv->locks.waitq_lock);
+	spin_lock_init(&rtlpriv->locks.entry_list_lock);
+	spin_lock_init(&rtlpriv->locks.cck_and_rw_pagea_lock);
+	spin_lock_init(&rtlpriv->locks.check_sendpkt_lock);
+	spin_lock_init(&rtlpriv->locks.fw_ps_lock);
+	spin_lock_init(&rtlpriv->locks.iqk_lock);
+	/* <5> init list */
+	INIT_LIST_HEAD(&rtlpriv->entry_list);
+
+	/* <6> init deferred work */
+	_rtl_init_deferred_work(hw);
+
+	/* <7> */
+#ifdef VIF_TODO
+	rtl_init_vif(hw);
+#endif
+
+	return 0;
+}
+
+void rtl92e_deinit_core(struct ieee80211_hw *hw)
+{
+}
+
+void rtl92e_init_rx_config(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_RCR, (u8 *)(&mac->rx_conf));
+}
+
+/*********************************************************
+ *
+ * tx information functions
+ *
+ *********************************************************/
+static void _rtl_qurey_shortpreamble_mode(struct ieee80211_hw *hw,
+					  struct rtl_tcb_desc *tcb_desc,
+					  struct ieee80211_tx_info *info)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 rate_flag = info->control.rates[0].flags;
+
+	tcb_desc->use_shortpreamble = false;
+
+	/* 1M can only use Long Preamble. 11B spec */
+	if (tcb_desc->hw_rate == rtlpriv->cfg->maps[RTL_RC_CCK_RATE1M])
+		return;
+	else if (rate_flag & IEEE80211_TX_RC_USE_SHORT_PREAMBLE)
+		tcb_desc->use_shortpreamble = true;
+
+	return;
+}
+
+static void _rtl_query_shortgi(struct ieee80211_hw *hw,
+			       struct ieee80211_sta *sta,
+			       struct rtl_tcb_desc *tcb_desc,
+			       struct ieee80211_tx_info *info)
+{
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u8 rate_flag = info->control.rates[0].flags;
+	u8 sgi_40 = 0, sgi_20 = 0, bw_40 = 0;
+	u8 sgi_80 = 0, bw_80 = 0;
+	tcb_desc->use_shortgi = false;
+
+	if (sta == NULL)
+		return;
+
+	sgi_40 = sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40;
+	sgi_20 = sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20;
+	sgi_80 = sta->vht_cap.cap & IEEE80211_VHT_CAP_SHORT_GI_80;
+
+	if (!(sta->ht_cap.ht_supported) && !(sta->vht_cap.vht_supported))
+		return;
+
+	if (!sgi_40 && !sgi_20)
+		return;
+
+	if (mac->opmode == NL80211_IFTYPE_STATION) {
+		bw_40 = mac->bw_40;
+		bw_80 = mac->bw_80;
+	} else if (mac->opmode == NL80211_IFTYPE_AP ||
+		   mac->opmode == NL80211_IFTYPE_ADHOC ||
+		   mac->opmode == NL80211_IFTYPE_MESH_POINT) {
+		bw_40 = sta->ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40;
+		bw_80 = sta->vht_cap.vht_supported;
+	}
+
+	if (bw_80) {
+		if (sgi_80)
+			tcb_desc->use_shortgi = true;
+		else
+			tcb_desc->use_shortgi = false;
+	} else {
+		if (bw_40 && sgi_40)
+			tcb_desc->use_shortgi = true;
+		else if (!bw_40 && sgi_20)
+			tcb_desc->use_shortgi = true;
+	}
+
+	if (!(rate_flag & IEEE80211_TX_RC_SHORT_GI))
+		tcb_desc->use_shortgi = false;
+}
+
+static void _rtl_query_protection_mode(struct ieee80211_hw *hw,
+				       struct rtl_tcb_desc *tcb_desc,
+				       struct ieee80211_tx_info *info)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 rate_flag = info->control.rates[0].flags;
+
+	/* Common Settings */
+	tcb_desc->b_rts_stbc = false;
+	tcb_desc->b_cts_enable = false;
+	tcb_desc->rts_sc = 0;
+	tcb_desc->b_rts_bw = false;
+	tcb_desc->b_rts_use_shortpreamble = false;
+	tcb_desc->b_rts_use_shortgi = false;
+
+	if (rate_flag & IEEE80211_TX_RC_USE_CTS_PROTECT) {
+		/* Use CTS-to-SELF in protection mode. */
+		tcb_desc->b_rts_enable = true;
+		tcb_desc->b_cts_enable = true;
+		tcb_desc->rts_rate = rtlpriv->cfg->maps[RTL_RC_OFDM_RATE24M];
+	} else if (rate_flag & IEEE80211_TX_RC_USE_RTS_CTS) {
+		/* Use RTS-CTS in protection mode. */
+		tcb_desc->b_rts_enable = true;
+		tcb_desc->rts_rate = rtlpriv->cfg->maps[RTL_RC_OFDM_RATE24M];
+	}
+}
+
+static void _rtl_txrate_selectmode(struct ieee80211_hw *hw,
+				   struct ieee80211_sta *sta,
+				   struct rtl_tcb_desc *tcb_desc)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_sta_info *sta_entry = NULL;
+	u8 ratr_index = 7;
+
+	if (sta) {
+		sta_entry = (struct rtl_sta_info *)sta->drv_priv;
+		ratr_index = sta_entry->ratr_index;
+	}
+	if (!tcb_desc->disable_ratefallback || !tcb_desc->use_driver_rate) {
+		if (mac->opmode == NL80211_IFTYPE_STATION) {
+			tcb_desc->ratr_index = 0;
+		} else if (mac->opmode == NL80211_IFTYPE_ADHOC ||
+				mac->opmode == NL80211_IFTYPE_MESH_POINT) {
+			if (tcb_desc->b_multicast || tcb_desc->b_broadcast) {
+				tcb_desc->hw_rate =
+				    rtlpriv->cfg->maps[RTL_RC_CCK_RATE2M];
+				tcb_desc->use_driver_rate = 1;
+				tcb_desc->ratr_index = RATR_INX_WIRELESS_MC;
+			} else {
+				tcb_desc->ratr_index = ratr_index;
+			}
+		} else if (mac->opmode == NL80211_IFTYPE_AP) {
+			tcb_desc->ratr_index = ratr_index;
+		}
+	}
+
+	if (rtlpriv->dm.b_useramask) {
+		tcb_desc->ratr_index = ratr_index;
+		/* TODO we will differentiate adhoc and station futrue  */
+		if (mac->opmode == NL80211_IFTYPE_STATION ||
+		    mac->opmode == NL80211_IFTYPE_MESH_POINT) {
+			tcb_desc->mac_id = 0;
+
+			if (mac->mode == WIRELESS_MODE_AC_5G)
+				tcb_desc->ratr_index =
+					RATR_INX_WIRELESS_AC_5N;
+			else if (mac->mode == WIRELESS_MODE_AC_24G)
+				tcb_desc->ratr_index =
+					RATR_INX_WIRELESS_AC_24N;
+			else if (mac->mode == WIRELESS_MODE_N_24G)
+				tcb_desc->ratr_index = RATR_INX_WIRELESS_NGB;
+			else if (mac->mode == WIRELESS_MODE_N_5G)
+				tcb_desc->ratr_index = RATR_INX_WIRELESS_NG;
+			else if (mac->mode & WIRELESS_MODE_G)
+				tcb_desc->ratr_index = RATR_INX_WIRELESS_GB;
+			else if (mac->mode & WIRELESS_MODE_B)
+				tcb_desc->ratr_index = RATR_INX_WIRELESS_B;
+			else if (mac->mode & WIRELESS_MODE_A)
+				tcb_desc->ratr_index = RATR_INX_WIRELESS_G;
+
+		} else if (mac->opmode == NL80211_IFTYPE_AP ||
+			   mac->opmode == NL80211_IFTYPE_ADHOC) {
+			if (sta) {
+				if (sta->aid > 0)
+					tcb_desc->mac_id = sta->aid + 1;
+				else
+					tcb_desc->mac_id = 1;
+			} else {
+				tcb_desc->mac_id = 0;
+			}
+		}
+	}
+}
+
+static void _rtl_query_bandwidth_mode(struct ieee80211_hw *hw,
+				      struct ieee80211_sta *sta,
+				      struct rtl_tcb_desc *tcb_desc)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+
+	tcb_desc->packet_bw = 0;
+	if (!sta)
+		return;
+	if (mac->opmode == NL80211_IFTYPE_AP ||
+	    mac->opmode == NL80211_IFTYPE_ADHOC ||
+	    mac->opmode == NL80211_IFTYPE_MESH_POINT) {
+		if (!(sta->ht_cap.ht_supported) ||
+		    !(sta->ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40))
+			return;
+	} else if (mac->opmode == NL80211_IFTYPE_STATION) {
+		if (!mac->bw_40 || !(sta->ht_cap.ht_supported))
+			return;
+	}
+	if (tcb_desc->b_multicast || tcb_desc->b_broadcast)
+		return;
+
+	/*use legency rate, shall use 20MHz */
+	if (tcb_desc->hw_rate <= rtlpriv->cfg->maps[RTL_RC_OFDM_RATE54M])
+		return;
+
+	tcb_desc->packet_bw = HT_CHANNEL_WIDTH_20_40;
+
+	if (rtlpriv->rtlhal.hw_type == HARDWARE_TYPE_RTL8812AE ||
+	    rtlpriv->rtlhal.hw_type == HARDWARE_TYPE_RTL8821AE) {
+		if (mac->opmode == NL80211_IFTYPE_AP ||
+		    mac->opmode == NL80211_IFTYPE_ADHOC ||
+		    mac->opmode == NL80211_IFTYPE_MESH_POINT) {
+			if (!(sta->vht_cap.vht_supported)) {
+				return;
+			} else if (mac->opmode == NL80211_IFTYPE_STATION) {
+				if (!mac->bw_80 ||
+				    !(sta->vht_cap.vht_supported))
+					return;
+			}
+		}
+		if (tcb_desc->hw_rate <=
+			rtlpriv->cfg->maps[RTL_RC_HT_RATEMCS15])
+			return;
+		tcb_desc->packet_bw = HT_CHANNEL_WIDTH_80;
+	}
+}
+
+static u8 _rtl_get_vht_highest_n_rate(struct ieee80211_hw *hw,
+				      struct ieee80211_sta *sta)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 hw_rate;
+	u16 map = le16_to_cpu(sta->vht_cap.vht_mcs.tx_mcs_map);
+
+	if ((get_rf_type(rtlphy) == RF_2T2R) &&
+	    (map & 0x000c) != 0x000c0) {
+		if ((map & 0x000c) >> 2 == IEEE80211_VHT_MCS_SUPPORT_0_7)
+			hw_rate =
+			rtlpriv->cfg->maps[RTL_RC_VHT_RATE_2SS_MCS7];
+		else if ((map  & 0x000c) >> 2 == IEEE80211_VHT_MCS_SUPPORT_0_8)
+			hw_rate = rtlpriv->cfg->maps[RTL_RC_VHT_RATE_2SS_MCS9];
+		else
+			hw_rate = rtlpriv->cfg->maps[RTL_RC_VHT_RATE_2SS_MCS9];
+	} else {
+		if ((map  & 0x0003) == IEEE80211_VHT_MCS_SUPPORT_0_7)
+			hw_rate = rtlpriv->cfg->maps[RTL_RC_VHT_RATE_1SS_MCS7];
+		else if ((map  & 0x0003) == IEEE80211_VHT_MCS_SUPPORT_0_8)
+			hw_rate = rtlpriv->cfg->maps[RTL_RC_VHT_RATE_1SS_MCS9];
+		else
+			hw_rate = rtlpriv->cfg->maps[RTL_RC_VHT_RATE_1SS_MCS9];
+	}
+
+	return hw_rate;
+}
+
+static u8 _rtl_get_highest_n_rate(struct ieee80211_hw *hw,
+				  struct ieee80211_sta *sta)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 hw_rate;
+
+	if ((get_rf_type(rtlphy) == RF_2T2R) &&
+	    (sta->ht_cap.mcs.rx_mask[1] != 0))
+		hw_rate = rtlpriv->cfg->maps[RTL_RC_HT_RATEMCS15];
+	else
+		hw_rate = rtlpriv->cfg->maps[RTL_RC_HT_RATEMCS7];
+
+	return hw_rate;
+}
+
+void stg_rtl_get_tcb_desc(struct ieee80211_hw *hw,
+			  struct ieee80211_tx_info *info,
+			  struct ieee80211_sta *sta,
+			  struct sk_buff *skb, struct rtl_tcb_desc *tcb_desc)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *rtlmac = rtl_mac(rtl_priv(hw));
+	struct ieee80211_hdr *hdr = rtl_get_hdr(skb);
+	struct ieee80211_rate *txrate;
+	__le16 fc = rtl_get_fc(skb);
+
+	txrate = ieee80211_get_tx_rate(hw, info);
+	if (txrate != NULL)
+		tcb_desc->hw_rate = txrate->hw_value;
+
+	if (ieee80211_is_data(fc)) {
+		/*
+		 *we set data rate INX 0
+		 *in rtl_rc.c   if skb is special data or
+		 *mgt which need low data rate.
+		 */
+
+		/*
+		 *So tcb_desc->hw_rate is just used for
+		 *special data and mgt frames
+		 */
+		if (info->control.rates[0].idx == 0 ||
+		    ieee80211_is_nullfunc(fc)) {
+			tcb_desc->use_driver_rate = true;
+			tcb_desc->ratr_index = RATR_INX_WIRELESS_MC;
+
+			tcb_desc->disable_ratefallback = 1;
+		} else {
+			/*
+			 *because hw will nerver use hw_rate
+			 *when tcb_desc->use_driver_rate = false
+			 *so we never set highest N rate here,
+			 *and N rate will all be controled by FW
+			 *when tcb_desc->use_driver_rate = false
+			 */
+			if (sta && sta->vht_cap.vht_supported) {
+				tcb_desc->hw_rate =
+				_rtl_get_vht_highest_n_rate(hw, sta);
+			} else if (sta && (sta->ht_cap.ht_supported)) {
+				tcb_desc->hw_rate =
+					_rtl_get_highest_n_rate(hw, sta);
+			} else {
+				if (rtlmac->mode == WIRELESS_MODE_B) {
+					tcb_desc->hw_rate =
+					    rtlpriv->cfg->maps[RTL_RC_CCK_RATE11M];
+				} else {
+					tcb_desc->hw_rate =
+					    rtlpriv->cfg->maps[RTL_RC_OFDM_RATE54M];
+				}
+			}
+		}
+
+		if (is_multicast_ether_addr(ieee80211_get_DA(hdr)))
+			tcb_desc->b_multicast = 1;
+		else if (is_broadcast_ether_addr(ieee80211_get_DA(hdr)))
+			tcb_desc->b_broadcast = 1;
+
+		_rtl_txrate_selectmode(hw, sta, tcb_desc);
+		_rtl_query_bandwidth_mode(hw, sta, tcb_desc);
+		_rtl_qurey_shortpreamble_mode(hw, tcb_desc, info);
+		_rtl_query_shortgi(hw, sta, tcb_desc, info);
+		_rtl_query_protection_mode(hw, tcb_desc, info);
+	} else {
+		tcb_desc->use_driver_rate = true;
+		tcb_desc->ratr_index = RATR_INX_WIRELESS_MC;
+		tcb_desc->disable_ratefallback = 1;
+		tcb_desc->mac_id = 0;
+		tcb_desc->packet_bw = 0;
+	}
+}
+EXPORT_SYMBOL(stg_rtl_get_tcb_desc);
+
+bool rtl92e_tx_mgmt_proc(struct ieee80211_hw *hw, struct sk_buff *skb)
+{
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	__le16 fc = rtl_get_fc(skb);
+
+	if (rtlpriv->dm.supp_phymode_switch &&
+	    mac->link_state < MAC80211_LINKED &&
+	    (ieee80211_is_auth(fc) || ieee80211_is_probe_req(fc))) {
+		if (rtlpriv->cfg->ops->check_switch_to_dmdp)
+			rtlpriv->cfg->ops->check_switch_to_dmdp(hw);
+	}
+	if (ieee80211_is_auth(fc)) {
+		RT_TRACE(COMP_SEND, DBG_DMESG, ("MAC80211_LINKING\n"));
+		rtl92e_ips_nic_on(hw);
+
+		mac->link_state = MAC80211_LINKING;
+		/* Dul mac */
+		rtlpriv->phy.b_need_iqk = true;
+	}
+	return true;
+}
+
+struct sk_buff *rtl92e_make_del_ba(struct ieee80211_hw *hw, u8 *sa,
+				   u8 *bssid, u16 tid);
+
+bool rtl92e_action_proc(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx)
+{
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct ieee80211_hdr *hdr = rtl_get_hdr(skb);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	__le16 fc = rtl_get_fc(skb);
+	u8 *act = (u8 *)(((u8 *)skb->data + MAC80211_3ADDR_LEN));
+	u8 category;
+
+	if (!ieee80211_is_action(fc))
+		return true;
+
+	category = *act;
+	act++;
+	switch (category) {
+	case ACT_CAT_BA:
+		switch (*act) {
+		case ACT_ADDBAREQ:
+			if (mac->act_scanning)
+				return false;
+
+			RT_TRACE((COMP_SEND | COMP_RECV), DBG_DMESG,
+				 ("%s ACT_ADDBAREQ From:%pM\n",
+				is_tx ? "Tx" : "Rx", hdr->addr2));
+			RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_DMESG, "req\n",
+				      skb->data, skb->len);
+			if (!is_tx) {
+				struct ieee80211_sta *sta = NULL;
+				struct rtl_sta_info *sta_entry = NULL;
+				struct ieee80211_mgmt *mgmt = (void *)skb->data;
+				u16 capab = 0, tid = 0;
+				struct rtl_tid_data *tid_data;
+				struct sk_buff *skb_delba = NULL;
+				struct ieee80211_rx_status rx_status = { 0 };
+
+				rcu_read_lock();
+				sta = rtl_find_sta(hw, hdr->addr3);
+				if (sta == NULL) {
+					RT_TRACE((COMP_SEND | COMP_RECV),
+						 DBG_TRACE, ("sta is NULL\n"));
+					rcu_read_unlock();
+					return true;
+				}
+
+				sta_entry =
+					(struct rtl_sta_info *)sta->drv_priv;
+				if (!sta_entry) {
+					rcu_read_unlock();
+					return true;
+				}
+				capab = le16_to_cpu(mgmt->u.action.u.addba_req.capab);
+				tid = (capab &
+					IEEE80211_ADDBA_PARAM_TID_MASK) >> 2;
+				tid_data = &sta_entry->tids[tid];
+				if (tid_data->agg.rx_agg_state ==
+				    RTL_RX_AGG_START) {
+					skb_delba = rtl92e_make_del_ba(hw,
+								    hdr->addr2,
+								    hdr->addr3,
+								    tid);
+					if (skb_delba) {
+						rx_status.freq =
+							hw->conf.chandef.chan->center_freq;
+						rx_status.band =
+							hw->conf.chandef.chan->band;
+						rx_status.flag |= RX_FLAG_DECRYPTED;
+						rx_status.flag |= RX_FLAG_MACTIME_MPDU;
+						rx_status.rate_idx = 0;
+						rx_status.signal = 50 + 10;
+						memcpy(IEEE80211_SKB_RXCB(skb_delba),
+						       &rx_status, sizeof(rx_status));
+						RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_DMESG,
+							      "fake del\n",
+							      skb_delba->data, skb_delba->len);
+						ieee80211_rx_irqsafe(hw, skb_delba);
+					}
+				}
+				rcu_read_unlock();
+			}
+			break;
+		case ACT_ADDBARSP:
+			RT_TRACE((COMP_SEND | COMP_RECV), DBG_DMESG,
+				 ("%s ACT_ADDBARSP From :%pM\n",
+				  is_tx ? "Tx" : "Rx", hdr->addr2));
+			break;
+		case ACT_DELBA:
+			RT_TRACE((COMP_SEND | COMP_RECV), DBG_DMESG,
+				 ("ACT_ADDBADEL From :%pM\n", hdr->addr2));
+			break;
+		}
+		break;
+	default:
+		break;
+	}
+
+	return true;
+}
+
+/*should call before software enc*/
+u8 rtl92e_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb,
+			  u8 is_tx)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	__le16 fc = rtl_get_fc(skb);
+	u16 ether_type;
+	u8 mac_hdr_len = ieee80211_get_hdrlen_from_skb(skb);
+	const struct iphdr *ip;
+
+	if (!ieee80211_is_data(fc))
+		goto end;
+
+	ip = (struct iphdr *)((u8 *)skb->data + mac_hdr_len +
+			      SNAP_SIZE + PROTOC_TYPE_SIZE);
+	ether_type = be16_to_cpup((__be16 *)
+				  (skb->data + mac_hdr_len + SNAP_SIZE));
+
+	if (ETH_P_IP == ether_type) {
+		if (IPPROTO_UDP == ip->protocol) {
+			struct udphdr *udp = (struct udphdr *)((u8 *)ip +
+							       (ip->ihl << 2));
+			if (((((u8 *)udp)[1] == 68) &&
+			     (((u8 *)udp)[3] == 67)) ||
+			    ((((u8 *)udp)[1] == 67) &&
+			     (((u8 *)udp)[3] == 68))) {
+				/*
+				 * 68 : UDP BOOTP client
+				 * 67 : UDP BOOTP server
+				 */
+				RT_TRACE((COMP_SEND | COMP_RECV),
+					 DBG_DMESG, ("dhcp %s !!\n",
+						     (is_tx) ? "Tx" : "Rx"));
+
+				if (is_tx) {
+					rtlpriv->ra.is_special_data = true;
+					if (rtlpriv->cfg->ops->get_btc_status())
+						rtlpriv->btcoexist.btc_ops->btc_special_packet_notify(
+									rtlpriv, 1);
+					rtl92e_lps_leave(hw);
+					ppsc->last_delaylps_stamp_jiffies =
+									jiffies;
+				}
+
+				return true;
+			}
+		}
+	} else if (ETH_P_ARP == ether_type) {
+		if (is_tx) {
+			rtlpriv->ra.is_special_data = true;
+			if (rtlpriv->cfg->ops->get_btc_status())
+				rtlpriv->btcoexist.btc_ops->btc_special_packet_notify(
+							rtlpriv, 1);
+			rtl92e_lps_leave(hw);
+			ppsc->last_delaylps_stamp_jiffies = jiffies;
+		}
+
+		return true;
+	} else if (ETH_P_PAE == ether_type) {
+		RT_TRACE((COMP_SEND | COMP_RECV), DBG_DMESG,
+			 ("802.1X %s EAPOL pkt!!\n", (is_tx) ? "Tx" : "Rx"));
+
+		if (is_tx) {
+			rtlpriv->ra.is_special_data = true;
+			rtl92e_lps_leave(hw);
+			ppsc->last_delaylps_stamp_jiffies = jiffies;
+		}
+
+		return true;
+	} else if (0x86DD == ether_type) {
+		return true;
+	}
+
+end:
+	rtlpriv->ra.is_special_data = false;
+	return false;
+}
+
+/*********************************************************
+ *
+ * functions called by core.c
+ *
+ *********************************************************/
+int rtl92e_tx_agg_start(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+			struct ieee80211_sta *sta, u16 tid, u16 *ssn)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_tid_data *tid_data;
+	struct rtl_sta_info *sta_entry = NULL;
+
+	if (sta == NULL)
+		return -EINVAL;
+
+	if (unlikely(tid >= MAX_TID_COUNT))
+		return -EINVAL;
+
+	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
+	if (!sta_entry)
+		return -ENXIO;
+	tid_data = &sta_entry->tids[tid];
+
+	RT_TRACE(COMP_SEND, DBG_DMESG,
+		 ("on ra = %pM tid = %d seq:%d\n", sta->addr, tid,
+		  tid_data->seq_number));
+
+	*ssn = tid_data->seq_number;
+	tid_data->agg.agg_state = RTL_AGG_START;
+
+	ieee80211_start_tx_ba_cb_irqsafe(vif, sta->addr, tid);
+	return 0;
+}
+
+int rtl92e_tx_agg_stop(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		       struct ieee80211_sta *sta, u16 tid)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_tid_data *tid_data;
+	struct rtl_sta_info *sta_entry = NULL;
+
+	if (sta == NULL)
+		return -EINVAL;
+
+	/* Comparing an array to null is not useful */
+	/*if (!sta->addr) {
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("ra = NULL\n"));
+		return -EINVAL;
+	}*/
+
+	RT_TRACE(COMP_SEND, DBG_DMESG,
+		 ("on ra = %pM tid = %d\n", sta->addr, tid));
+
+	if (unlikely(tid >= MAX_TID_COUNT))
+		return -EINVAL;
+
+	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
+	tid_data = &sta_entry->tids[tid];
+	sta_entry->tids[tid].agg.agg_state = RTL_AGG_STOP;
+
+	ieee80211_stop_tx_ba_cb_irqsafe(vif, sta->addr, tid);
+	return 0;
+}
+
+int rtl92e_rx_agg_start(struct ieee80211_hw *hw,
+			struct ieee80211_sta *sta, u16 tid)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_tid_data *tid_data;
+	struct rtl_sta_info *sta_entry = NULL;
+
+	if (sta == NULL)
+		return -EINVAL;
+
+	if (unlikely(tid >= MAX_TID_COUNT))
+		return -EINVAL;
+
+	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
+	if (!sta_entry)
+		return -ENXIO;
+	tid_data = &sta_entry->tids[tid];
+
+	RT_TRACE(COMP_RECV, DBG_DMESG,
+		 ("on ra = %pM tid = %d seq:%d\n", sta->addr, tid,
+		 tid_data->seq_number));
+
+	tid_data->agg.rx_agg_state = RTL_RX_AGG_START;
+	return 0;
+}
+
+int rtl92e_rx_agg_stop(struct ieee80211_hw *hw,
+		       struct ieee80211_sta *sta, u16 tid)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_tid_data *tid_data;
+	struct rtl_sta_info *sta_entry = NULL;
+
+	if (sta == NULL)
+		return -EINVAL;
+
+	/* Comparing an array to null is not useful */
+	/*if (!sta->addr) {
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("ra = NULL\n"));
+		return -EINVAL;
+	}*/
+
+	RT_TRACE(COMP_SEND, DBG_DMESG,
+		 ("on ra = %pM tid = %d\n", sta->addr, tid));
+
+	if (unlikely(tid >= MAX_TID_COUNT))
+		return -EINVAL;
+
+	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
+	tid_data = &sta_entry->tids[tid];
+	sta_entry->tids[tid].agg.rx_agg_state = RTL_RX_AGG_STOP;
+
+	return 0;
+}
+
+int rtl92e_tx_agg_oper(struct ieee80211_hw *hw,
+		       struct ieee80211_sta *sta, u16 tid)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_tid_data *tid_data;
+	struct rtl_sta_info *sta_entry = NULL;
+
+	if (sta == NULL)
+		return -EINVAL;
+
+	/* Comparing an array to null is not useful */
+	/*if (!sta->addr) {
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("ra = NULL\n"));
+		return -EINVAL;
+	}*/
+
+	RT_TRACE(COMP_SEND, DBG_DMESG,
+		 ("on ra = %pM tid = %d\n", sta->addr, tid));
+
+	if (unlikely(tid >= MAX_TID_COUNT))
+		return -EINVAL;
+
+	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
+	tid_data = &sta_entry->tids[tid];
+	sta_entry->tids[tid].agg.agg_state = RTL_AGG_OPERATIONAL;
+
+	return 0;
+}
+
+/*********************************************************
+ *
+ * wq & timer callback functions
+ *
+ *********************************************************/
+/* this function is used for roaming */
+void rtl92e_beacon_statistic(struct ieee80211_hw *hw, struct sk_buff *skb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+
+	if (rtlpriv->mac80211.opmode != NL80211_IFTYPE_STATION)
+		return;
+
+	if (rtlpriv->mac80211.link_state < MAC80211_LINKED)
+		return;
+
+	/* check if this really is a beacon */
+	if (!ieee80211_is_beacon(hdr->frame_control) &&
+	    !ieee80211_is_probe_resp(hdr->frame_control))
+		return;
+
+	/* min. beacon length + FCS_LEN */
+	if (skb->len <= 40 + FCS_LEN)
+		return;
+
+	/* and only beacons from the associated BSSID, please */
+	if (!ether_addr_equal(hdr->addr3, rtlpriv->mac80211.bssid))
+		return;
+
+	rtlpriv->link_info.bcn_rx_inperiod++;
+}
+
+void rtl92e_watchdog_wq_callback(void *data)
+{
+	struct rtl_works *rtlworks = container_of_dwork_rtl(data,
+							    struct rtl_works,
+							    watchdog_wq);
+	struct ieee80211_hw *hw = rtlworks->hw;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	bool b_busytraffic = false;
+	bool b_tx_busy_traffic = false;
+	bool b_rx_busy_traffic = false;
+	bool b_higher_busytraffic = false;
+	bool b_higher_busyrxtraffic = false;
+	u8 idx, tid;
+	u32 rx_cnt_inp4eriod = 0;
+	u32 tx_cnt_inp4eriod = 0;
+	u32 aver_rx_cnt_inperiod = 0;
+	u32 aver_tx_cnt_inperiod = 0;
+	u32 aver_tidtx_inperiod[MAX_TID_COUNT] = {0};
+	u32 tidtx_inp4eriod[MAX_TID_COUNT] = {0};
+	bool benter_ps = false;
+
+	if (is_hal_stop(rtlhal))
+		return;
+
+	/* <1> Determine if action frame is allowed */
+	if (mac->link_state > MAC80211_NOLINK) {
+		if (mac->cnt_after_linked < 20)
+			mac->cnt_after_linked++;
+	} else {
+		mac->cnt_after_linked = 0;
+	}
+
+	/* <2> to check if traffic busy, if
+	 * busytraffic we don't change channel */
+	if (mac->link_state >= MAC80211_LINKED) {
+		/* (1) get aver_rx_cnt_inperiod & aver_tx_cnt_inperiod */
+		for (idx = 0; idx <= 2; idx++) {
+			rtlpriv->link_info.num_rx_in4period[idx] =
+			    rtlpriv->link_info.num_rx_in4period[idx + 1];
+			rtlpriv->link_info.num_tx_in4period[idx] =
+			    rtlpriv->link_info.num_tx_in4period[idx + 1];
+		}
+		rtlpriv->link_info.num_rx_in4period[3] =
+		    rtlpriv->link_info.num_rx_inperiod;
+		rtlpriv->link_info.num_tx_in4period[3] =
+		    rtlpriv->link_info.num_tx_inperiod;
+		for (idx = 0; idx <= 3; idx++) {
+			rx_cnt_inp4eriod +=
+			    rtlpriv->link_info.num_rx_in4period[idx];
+			tx_cnt_inp4eriod +=
+			    rtlpriv->link_info.num_tx_in4period[idx];
+		}
+		aver_rx_cnt_inperiod = rx_cnt_inp4eriod / 4;
+		aver_tx_cnt_inperiod = tx_cnt_inp4eriod / 4;
+
+		/* (2) check traffic busy */
+		if (aver_rx_cnt_inperiod > 100 || aver_tx_cnt_inperiod > 100) {
+			b_busytraffic = true;
+			if (aver_rx_cnt_inperiod > aver_tx_cnt_inperiod)
+				b_rx_busy_traffic = true;
+			else
+				b_tx_busy_traffic = false;
+		}
+
+		/* Higher Tx/Rx data. */
+		if (aver_rx_cnt_inperiod > 4000 ||
+		    aver_tx_cnt_inperiod > 4000) {
+			b_higher_busytraffic = true;
+
+			/* Extremely high Rx data. */
+			if (aver_rx_cnt_inperiod > 5000)
+				b_higher_busyrxtraffic = true;
+		}
+
+		/* check every tid's tx traffic */
+		for (tid = 0; tid <= 7; tid++) {
+			for (idx = 0; idx <= 2; idx++)
+				rtlpriv->link_info.tidtx_in4period[tid][idx] =
+					rtlpriv->link_info.tidtx_in4period[tid]
+					[idx + 1];
+			rtlpriv->link_info.tidtx_in4period[tid][3] =
+				rtlpriv->link_info.tidtx_inperiod[tid];
+
+			for (idx = 0; idx <= 3; idx++)
+				tidtx_inp4eriod[tid] +=
+				   rtlpriv->link_info.tidtx_in4period[tid][idx];
+			aver_tidtx_inperiod[tid] = tidtx_inp4eriod[tid] / 4;
+			if (aver_tidtx_inperiod[tid] > 5000)
+				rtlpriv->link_info.higher_busytxtraffic[tid] =
+									true;
+			else
+				rtlpriv->link_info.higher_busytxtraffic[tid] =
+									false;
+		}
+
+		if (((rtlpriv->link_info.num_rx_inperiod +
+		      rtlpriv->link_info.num_tx_inperiod) > 8) ||
+		    (rtlpriv->link_info.num_rx_inperiod > 2))
+			benter_ps = false;
+		else
+			benter_ps = true;
+
+		/* LeisurePS only work in infra mode. */
+		if (benter_ps)
+			rtl92e_lps_enter(hw);
+		else
+			rtl92e_lps_leave(hw);
+	}
+
+	rtlpriv->link_info.num_rx_inperiod = 0;
+	rtlpriv->link_info.num_tx_inperiod = 0;
+	for (tid = 0; tid <= 7; tid++)
+		rtlpriv->link_info.tidtx_inperiod[tid] = 0;
+
+	rtlpriv->link_info.b_busytraffic = b_busytraffic;
+	rtlpriv->link_info.b_rx_busy_traffic = b_rx_busy_traffic;
+	rtlpriv->link_info.b_tx_busy_traffic = b_tx_busy_traffic;
+	rtlpriv->link_info.b_higher_busytraffic = b_higher_busytraffic;
+	rtlpriv->link_info.b_higher_busyrxtraffic = b_higher_busyrxtraffic;
+
+	/* <3> DM */
+	rtlpriv->cfg->ops->dm_watchdog(hw);
+
+	/* <4> roaming */
+	if (mac->link_state == MAC80211_LINKED &&
+	    mac->opmode == NL80211_IFTYPE_STATION) {
+		if ((rtlpriv->link_info.bcn_rx_inperiod +
+			rtlpriv->link_info.num_rx_inperiod) == 0) {
+			rtlpriv->link_info.roam_times++;
+			RT_TRACE(COMP_ERR, DBG_DMESG,
+				 ("AP off for %d s\n",
+				  (rtlpriv->link_info.roam_times * 2)));
+
+			/* if we can't recv beacon for 10s,
+			* we should reconnect this AP */
+			if (rtlpriv->link_info.roam_times >= 5) {
+				RT_TRACE(COMP_ERR, DBG_EMERG,
+					 ("AP off, try to reconnect now\n"));
+				rtlpriv->link_info.roam_times = 0;
+				ieee80211_connection_loss(
+					rtlpriv->mac80211.vif);
+			}
+		} else {
+			rtlpriv->link_info.roam_times = 0;
+		}
+	}
+
+	if (rtlpriv->cfg->ops->get_btc_status())
+		rtlpriv->btcoexist.btc_ops->btc_periodical(rtlpriv);
+
+	rtlpriv->link_info.bcn_rx_inperiod = 0;
+}
+
+void rtl92e_watch_dog_timer_callback(unsigned long data)
+{
+	struct ieee80211_hw *hw = (struct ieee80211_hw *)data;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	queue_delayed_work(rtlpriv->works.rtl_wq,
+			   &rtlpriv->works.watchdog_wq, 0);
+
+	mod_timer(&rtlpriv->works.watchdog_timer,
+		  jiffies + MSECS(RTL_WATCH_DOG_TIME));
+}
+void rtl92e_fwevt_wq_callback(void *data)
+{
+	struct rtl_works *rtlworks =
+		container_of_dwork_rtl(data, struct rtl_works, fwevt_wq);
+	struct ieee80211_hw *hw = rtlworks->hw;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->cfg->ops->c2h_command_handle(hw);
+}
+void rtl92e_easy_concurrent_retrytimer_callback(unsigned long data)
+{
+	struct ieee80211_hw *hw = (struct ieee80211_hw *)data;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_priv *buddy_priv = rtlpriv->buddy_priv;
+
+	if (buddy_priv == NULL)
+		return;
+
+	rtlpriv->cfg->ops->dualmac_easy_concurrent(hw);
+}
+/*********************************************************
+ *
+ * frame process functions
+ *
+ *********************************************************/
+u8 *rtl92e_find_ie(u8 *data, unsigned int len, u8 ie)
+{
+	struct ieee80211_mgmt *mgmt = (void *)data;
+	u8 *pos, *end;
+
+	pos = (u8 *)mgmt->u.beacon.variable;
+	end = data + len;
+	while (pos < end) {
+		if (pos + 2 + pos[1] > end)
+			return NULL;
+
+		if (pos[0] == ie)
+			return pos;
+
+		pos += 2 + pos[1];
+	}
+	return NULL;
+}
+
+/* when we use 2 rx ants we send IEEE80211_SMPS_OFF */
+/* when we use 1 rx ant we send IEEE80211_SMPS_STATIC */
+static struct sk_buff *rtl_make_smps_action(struct ieee80211_hw *hw,
+					    enum ieee80211_smps_mode smps,
+					    u8 *da, u8 *bssid)
+{
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct sk_buff *skb;
+	struct ieee80211_mgmt_compat *action_frame;
+
+	/* 27 = header + category + action + smps mode */
+	skb = dev_alloc_skb(27 + hw->extra_tx_headroom);
+	if (!skb)
+		return NULL;
+
+	skb_reserve(skb, hw->extra_tx_headroom);
+	action_frame = (void *)skb_put(skb, 27);
+	memset(action_frame, 0, 27);
+	ether_addr_copy(action_frame->da, da);
+	ether_addr_copy(action_frame->sa, rtlefuse->dev_addr);
+	ether_addr_copy(action_frame->bssid, bssid);
+	action_frame->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+						  IEEE80211_STYPE_ACTION);
+	action_frame->u.action.category = WLAN_CATEGORY_HT;
+	action_frame->u.action.u.ht_smps.action = WLAN_HT_ACTION_SMPS;
+	switch (smps) {
+	case IEEE80211_SMPS_AUTOMATIC:/* 0 */
+	case IEEE80211_SMPS_NUM_MODES:/* 4 */
+		WARN_ON(1);
+	/* Here will get a 'MISSING_BREAK' in Coverity Test, just ignore it.
+	 * According to Kernel Code, here is right.
+	 */
+	case IEEE80211_SMPS_OFF:/* 1 */ /*MIMO_PS_NOLIMIT*/
+		action_frame->u.action.u.ht_smps.smps_control =
+				WLAN_HT_SMPS_CONTROL_DISABLED;/* 0 */
+		break;
+	case IEEE80211_SMPS_STATIC:/* 2 */ /*MIMO_PS_STATIC*/
+		action_frame->u.action.u.ht_smps.smps_control =
+				WLAN_HT_SMPS_CONTROL_STATIC;/* 1 */
+		break;
+	case IEEE80211_SMPS_DYNAMIC:/* 3 */ /*MIMO_PS_DYNAMIC*/
+		action_frame->u.action.u.ht_smps.smps_control =
+				WLAN_HT_SMPS_CONTROL_DYNAMIC;/* 3 */
+		break;
+	}
+
+	return skb;
+}
+
+int stg_rtl_send_smps_action(struct ieee80211_hw *hw,
+			     struct ieee80211_sta *sta,
+			     enum ieee80211_smps_mode smps)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct sk_buff *skb = NULL;
+	struct rtl_tcb_desc tcb_desc;
+	u8 bssid[ETH_ALEN] = {0};
+
+	memset(&tcb_desc, 0, sizeof(struct rtl_tcb_desc));
+
+	if (rtlpriv->mac80211.act_scanning)
+		goto err_free;
+
+	if (!sta)
+		goto err_free;
+
+	if (unlikely(is_hal_stop(rtlhal) || ppsc->rfpwr_state != ERFON))
+		goto err_free;
+
+	if (!test_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status))
+		goto err_free;
+
+	if (rtlpriv->mac80211.opmode == NL80211_IFTYPE_AP)
+		ether_addr_copy(bssid, rtlpriv->efuse.dev_addr);
+	else
+		ether_addr_copy(bssid, rtlpriv->mac80211.bssid);
+
+	skb = rtl_make_smps_action(hw, smps, sta->addr, bssid);
+	/* this is a type = mgmt * stype = action frame */
+	if (skb) {
+		struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+		struct rtl_sta_info *sta_entry =
+			(struct rtl_sta_info *)sta->drv_priv;
+		sta_entry->mimo_ps = smps;
+		/* rtlpriv->cfg->ops->update_rate_tbl(hw, sta, 0); */
+
+		info->control.rates[0].idx = 0;
+		info->band = hw->conf.chandef.chan->band;
+		rtlpriv->intf_ops->adapter_tx(hw, sta, skb, &tcb_desc);
+	}
+	return 1;
+
+err_free:
+	return 0;
+}
+EXPORT_SYMBOL(stg_rtl_send_smps_action);
+
+/* because mac80211 have issues when can receive del ba
+ * so here we just make a fake del_ba if we receive a ba_req
+ * but rx_agg was opened to let mac80211 release some ba
+ * related resources, so please this del_ba for tx */
+struct sk_buff *rtl92e_make_del_ba(struct ieee80211_hw *hw,
+				   u8 *sa, u8 *bssid, u16 tid)
+{
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct sk_buff *skb;
+	struct ieee80211_mgmt *action_frame;
+	u16 params;
+
+	/* 27 = header + category + action + smps mode */
+	skb = dev_alloc_skb(34 + hw->extra_tx_headroom);
+	if (!skb)
+		return NULL;
+
+	skb_reserve(skb, hw->extra_tx_headroom);
+	action_frame = (void *)skb_put(skb, 34);
+	memset(action_frame, 0, 34);
+	ether_addr_copy(action_frame->sa, sa);
+	ether_addr_copy(action_frame->da, rtlefuse->dev_addr);
+	ether_addr_copy(action_frame->bssid, bssid);
+	action_frame->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+						  IEEE80211_STYPE_ACTION);
+	action_frame->u.action.category = WLAN_CATEGORY_BACK;
+	action_frame->u.action.u.delba.action_code = WLAN_ACTION_DELBA;
+	params = (u16)(1 << 11);	/* bit 11 initiator */
+	params |= (u16)(tid << 12);	/* bit 15:12 TID number */
+
+	action_frame->u.action.u.delba.params = cpu_to_le16(params);
+	action_frame->u.action.u.delba.reason_code =
+		cpu_to_le16(WLAN_REASON_QSTA_TIMEOUT);
+
+	return skb;
+}
+
+/*********************************************************
+ *
+ * IOT functions
+ *
+ *********************************************************/
+static bool rtl_chk_vendor_ouisub(struct ieee80211_hw *hw,
+				  struct octet_string vendor_ie)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	bool matched = false;
+	static u8 athcap_1[] = { 0x00, 0x03, 0x7F };
+	static u8 athcap_2[] = { 0x00, 0x13, 0x74 };
+	static u8 broadcap_1[] = { 0x00, 0x10, 0x18 };
+	static u8 broadcap_2[] = { 0x00, 0x0a, 0xf7 };
+	static u8 broadcap_3[] = { 0x00, 0x05, 0xb5 };
+	static u8 racap[] = { 0x00, 0x0c, 0x43 };
+	static u8 ciscocap[] = { 0x00, 0x40, 0x96 };
+	static u8 marvcap[] = { 0x00, 0x50, 0x43 };
+
+	if (memcmp(vendor_ie.octet, athcap_1, 3) == 0 ||
+	    memcmp(vendor_ie.octet, athcap_2, 3) == 0) {
+		rtlpriv->mac80211.vendor = PEER_ATH;
+		matched = true;
+	} else if (memcmp(vendor_ie.octet, broadcap_1, 3) == 0 ||
+		   memcmp(vendor_ie.octet, broadcap_2, 3) == 0 ||
+		   memcmp(vendor_ie.octet, broadcap_3, 3) == 0) {
+		rtlpriv->mac80211.vendor = PEER_BROAD;
+		matched = true;
+	} else if (memcmp(vendor_ie.octet, racap, 3) == 0) {
+		rtlpriv->mac80211.vendor = PEER_RAL;
+		matched = true;
+	} else if (memcmp(vendor_ie.octet, ciscocap, 3) == 0) {
+		rtlpriv->mac80211.vendor = PEER_CISCO;
+		matched = true;
+	} else if (memcmp(vendor_ie.octet, marvcap, 3) == 0) {
+		rtlpriv->mac80211.vendor = PEER_MARV;
+		matched = true;
+	}
+
+	return matched;
+}
+
+static bool rtl_find_221_ie(struct ieee80211_hw *hw, u8 *data, unsigned int len)
+{
+	struct ieee80211_mgmt *mgmt = (void *)data;
+	struct octet_string vendor_ie;
+	u8 *pos, *end;
+
+	pos = (u8 *)mgmt->u.beacon.variable;
+	end = data + len;
+	while (pos < end) {
+		if (pos[0] == 221) {
+			vendor_ie.length = pos[1];
+			vendor_ie.octet = &pos[2];
+			if (rtl_chk_vendor_ouisub(hw, vendor_ie))
+				return true;
+		}
+
+		if (pos + 2 + pos[1] > end)
+			return false;
+
+		pos += 2 + pos[1];
+	}
+	return false;
+}
+
+void rtl92e_recognize_peer(struct ieee80211_hw *hw, u8 *data, unsigned int len)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct ieee80211_hdr *hdr = (void *)data;
+	u32 vendor = PEER_UNKNOWN;
+
+	static u8 ap3_1[3] = { 0x00, 0x14, 0xbf };
+	static u8 ap3_2[3] = { 0x00, 0x1a, 0x70 };
+	static u8 ap3_3[3] = { 0x00, 0x1d, 0x7e };
+	static u8 ap4_1[3] = { 0x00, 0x90, 0xcc };
+	static u8 ap4_2[3] = { 0x00, 0x0e, 0x2e };
+	static u8 ap4_3[3] = { 0x00, 0x18, 0x02 };
+	static u8 ap4_4[3] = { 0x00, 0x17, 0x3f };
+	static u8 ap4_5[3] = { 0x00, 0x1c, 0xdf };
+	static u8 ap5_1[3] = { 0x00, 0x1c, 0xf0 };
+	static u8 ap5_2[3] = { 0x00, 0x21, 0x91 };
+	static u8 ap5_3[3] = { 0x00, 0x24, 0x01 };
+	static u8 ap5_4[3] = { 0x00, 0x15, 0xe9 };
+	static u8 ap5_5[3] = { 0x00, 0x17, 0x9A };
+	static u8 ap5_6[3] = { 0x00, 0x18, 0xE7 };
+	static u8 ap6_1[3] = { 0x00, 0x17, 0x94 };
+	static u8 ap7_1[3] = { 0x00, 0x14, 0xa4 };
+
+	if (mac->opmode != NL80211_IFTYPE_STATION)
+		return;
+
+	if (mac->link_state == MAC80211_NOLINK) {
+		mac->vendor = PEER_UNKNOWN;
+		return;
+	}
+
+	if (mac->cnt_after_linked > 2)
+		return;
+
+	/* check if this really is a beacon */
+	if (!ieee80211_is_beacon(hdr->frame_control))
+		return;
+
+	/* min. beacon length + FCS_LEN */
+	if (len <= 40 + FCS_LEN)
+		return;
+
+	/* and only beacons from the associated BSSID, please */
+	if (!ether_addr_equal(hdr->addr3, rtlpriv->mac80211.bssid))
+		return;
+
+	if (rtl_find_221_ie(hw, data, len))
+		vendor = mac->vendor;
+
+	if ((memcmp(mac->bssid, ap5_1, 3) == 0) ||
+	    (memcmp(mac->bssid, ap5_2, 3) == 0) ||
+	    (memcmp(mac->bssid, ap5_3, 3) == 0) ||
+	    (memcmp(mac->bssid, ap5_4, 3) == 0) ||
+	    (memcmp(mac->bssid, ap5_5, 3) == 0) ||
+	    (memcmp(mac->bssid, ap5_6, 3) == 0) ||
+		vendor == PEER_ATH) {
+		vendor = PEER_ATH;
+		RT_TRACE(COMP_MAC80211, DBG_LOUD, ("=>ath find\n"));
+	} else if ((memcmp(mac->bssid, ap4_4, 3) == 0) ||
+		   (memcmp(mac->bssid, ap4_5, 3) == 0) ||
+		   (memcmp(mac->bssid, ap4_1, 3) == 0) ||
+		   (memcmp(mac->bssid, ap4_2, 3) == 0) ||
+		   (memcmp(mac->bssid, ap4_3, 3) == 0) ||
+		vendor == PEER_RAL) {
+		RT_TRACE(COMP_MAC80211, DBG_LOUD, ("=>ral findn\n"));
+		vendor = PEER_RAL;
+	} else if (memcmp(mac->bssid, ap6_1, 3) == 0 ||
+		vendor == PEER_CISCO) {
+		vendor = PEER_CISCO;
+		RT_TRACE(COMP_MAC80211, DBG_LOUD, ("=>cisco find\n"));
+	} else if ((memcmp(mac->bssid, ap3_1, 3) == 0) ||
+		(memcmp(mac->bssid, ap3_2, 3) == 0) ||
+		(memcmp(mac->bssid, ap3_3, 3) == 0) ||
+		vendor == PEER_BROAD) {
+		RT_TRACE(COMP_MAC80211, DBG_LOUD, ("=>broad find\n"));
+		vendor = PEER_BROAD;
+	} else if (memcmp(mac->bssid, ap7_1, 3) == 0 ||
+		vendor == PEER_MARV) {
+		vendor = PEER_MARV;
+		RT_TRACE(COMP_MAC80211, DBG_LOUD, ("=>marv find\n"));
+	}
+
+	mac->vendor = vendor;
+}
+
+/*********************************************************
+ *
+ * sysfs functions
+ *
+ *********************************************************/
+struct rtl_global_var global_var = {};
+
+int  rtl_core_module_init(void)
+{
+	static int here_once;
+
+	if (here_once++)
+		return 0;
+
+	if (rtl92e_rate_control_register())
+		pr_debug("rtl: Unable to register rtl_rc, use default RC !!\n");
+
+	/* init some global vars */
+	INIT_LIST_HEAD(&global_var.glb_priv_list);
+	spin_lock_init(&global_var.glb_list_lock);
+
+	return 0;
+}
+void  rtl_core_module_exit(void)
+{
+	/*RC*/
+	rtl92e_rate_control_unregister();
+}
diff --git a/drivers/staging/rtl8192ee/base.h b/drivers/staging/rtl8192ee/base.h
new file mode 100644
index 00000000000..c7929a7b02f
--- /dev/null
+++ b/drivers/staging/rtl8192ee/base.h
@@ -0,0 +1,163 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL_BASE_H__
+#define __RTL_BASE_H__
+
+#include "compat.h"
+
+enum ap_peer {
+	PEER_UNKNOWN = 0,
+	PEER_RTL = 1,
+	PEER_RTL_92SE = 2,
+	PEER_BROAD = 3,
+	PEER_RAL = 4,
+	PEER_ATH = 5,
+	PEER_CISCO = 6,
+	PEER_MARV = 7,
+	PEER_AIRGO = 9,
+	PEER_MAX = 10,
+};
+
+#define RTL_DUMMY_OFFSET	0
+#define RTL_DUMMY_UNIT		8
+#define RTL_TX_DUMMY_SIZE	(RTL_DUMMY_OFFSET * RTL_DUMMY_UNIT)
+#define RTL_TX_DESC_SIZE	32
+#define RTL_TX_HEADER_SIZE	(RTL_TX_DESC_SIZE + RTL_TX_DUMMY_SIZE)
+
+#define HT_AMSDU_SIZE_4K	3839
+#define HT_AMSDU_SIZE_8K	7935
+
+#define MAX_BIT_RATE_40MHZ_MCS15	300	/* Mbps */
+#define MAX_BIT_RATE_40MHZ_MCS7		150	/* Mbps */
+
+#define MAX_BIT_RATE_SHORT_GI_2NSS_80MHZ_MCS9	867	/* Mbps */
+#define MAX_BIT_RATE_SHORT_GI_2NSS_80MHZ_MCS7	650	/* Mbps */
+#define MAX_BIT_RATE_LONG_GI_2NSS_80MHZ_MCS9	780	/* Mbps */
+#define MAX_BIT_RATE_LONG_GI_2NSS_80MHZ_MCS7	585	/* Mbps */
+
+#define MAX_BIT_RATE_SHORT_GI_1NSS_80MHZ_MCS9	434	/* Mbps */
+#define MAX_BIT_RATE_SHORT_GI_1NSS_80MHZ_MCS7	325	/* Mbps */
+#define MAX_BIT_RATE_LONG_GI_1NSS_80MHZ_MCS9	390	/* Mbps */
+#define MAX_BIT_RATE_LONG_GI_1NSS_80MHZ_MCS7	293	/* Mbps */
+
+
+#define RTL_RATE_COUNT_LEGACY		12
+#define RTL_CHANNEL_COUNT		14
+
+#define FRAME_OFFSET_FRAME_CONTROL	0
+#define FRAME_OFFSET_DURATION		2
+#define FRAME_OFFSET_ADDRESS1		4
+#define FRAME_OFFSET_ADDRESS2		10
+#define FRAME_OFFSET_ADDRESS3		16
+#define FRAME_OFFSET_SEQUENCE		22
+#define FRAME_OFFSET_ADDRESS4		24
+
+#define SET_80211_HDR_FRAME_CONTROL(_hdr, _val)		\
+	WRITEEF2BYTE(_hdr, _val)
+#define SET_80211_HDR_TYPE_AND_SUBTYPE(_hdr, _val)	\
+	WRITEEF1BYTE(_hdr, _val)
+#define SET_80211_HDR_PWR_MGNT(_hdr, _val)		\
+	SET_BITS_TO_LE_2BYTE(_hdr, 12, 1, _val)
+#define SET_80211_HDR_TO_DS(_hdr, _val)			\
+	SET_BITS_TO_LE_2BYTE(_hdr, 8, 1, _val)
+
+#define SET_80211_PS_POLL_AID(_hdr, _val)		\
+	(*(u16 *)((u8 *)(_hdr) + 2) = _val)
+#define SET_80211_PS_POLL_BSSID(_hdr, _val)		\
+	memcpy(((u8 *)(_hdr)) + 4, (u8 *)(_val), ETH_ALEN)
+#define SET_80211_PS_POLL_TA(_hdr, _val)		\
+	memcpy(((u8 *)(_hdr)) + 10, (u8 *)(_val), ETH_ALEN)
+
+#define SET_80211_HDR_DURATION(_hdr, _val)	\
+	WRITEEF2BYTE((u8 *)(_hdr)+FRAME_OFFSET_DURATION, _val)
+#define SET_80211_HDR_ADDRESS1(_hdr, _val)	\
+	CP_MACADDR((u8 *)(_hdr)+FRAME_OFFSET_ADDRESS1, (u8 *)(_val))
+#define SET_80211_HDR_ADDRESS2(_hdr, _val)	\
+	CP_MACADDR((u8 *)(_hdr)+FRAME_OFFSET_ADDRESS2, (u8 *)(_val))
+#define SET_80211_HDR_ADDRESS3(_hdr, _val)	\
+	CP_MACADDR((u8 *)(_hdr)+FRAME_OFFSET_ADDRESS3, (u8 *)(_val))
+#define SET_80211_HDR_FRAGMENT_SEQUENCE(_hdr, _val)  \
+	WRITEEF2BYTE((u8 *)(_hdr)+FRAME_OFFSET_SEQUENCE, _val)
+
+#define SET_BEACON_PROBE_RSP_TIME_STAMP_LOW(__phdr, __val)	\
+	WRITEEF4BYTE(((u8 *)(__phdr)) + 24, __val)
+#define SET_BEACON_PROBE_RSP_TIME_STAMP_HIGH(__phdr, __val) \
+	WRITEEF4BYTE(((u8 *)(__phdr)) + 28, __val)
+#define SET_BEACON_PROBE_RSP_BEACON_INTERVAL(__phdr, __val) \
+	WRITEEF2BYTE(((u8 *)(__phdr)) + 32, __val)
+#define GET_BEACON_PROBE_RSP_CAPABILITY_INFO(__phdr)		\
+	READEF2BYTE(((u8 *)(__phdr)) + 34)
+#define SET_BEACON_PROBE_RSP_CAPABILITY_INFO(__phdr, __val) \
+	WRITEEF2BYTE(((u8 *)(__phdr)) + 34, __val)
+#define MASK_BEACON_PROBE_RSP_CAPABILITY_INFO(__phdr, __val) \
+	SET_BEACON_PROBE_RSP_CAPABILITY_INFO(__phdr, \
+	(GET_BEACON_PROBE_RSP_CAPABILITY_INFO(__phdr) & (~(__val))))
+
+int rtl92e_init_core(struct ieee80211_hw *hw);
+void rtl92e_deinit_core(struct ieee80211_hw *hw);
+void rtl92e_init_rx_config(struct ieee80211_hw *hw);
+void rtl92e_init_rfkill(struct ieee80211_hw *hw);
+void rtl92e_deinit_rfkill(struct ieee80211_hw *hw);
+
+void rtl92e_watch_dog_timer_callback(unsigned long data);
+void rtl92e_deinit_deferred_work(struct ieee80211_hw *hw);
+
+bool rtl92e_action_proc(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx);
+bool rtl92e_tx_mgmt_proc(struct ieee80211_hw *hw, struct sk_buff *skb);
+u8 rtl92e_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb,
+			  u8 is_tx);
+void rtl92e_beacon_statistic(struct ieee80211_hw *hw, struct sk_buff *skb);
+void rtl92e_watch_dog_timer_callback(unsigned long data);
+int rtl92e_tx_agg_start(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		        struct ieee80211_sta *sta, u16 tid, u16 *ssn);
+int rtl92e_tx_agg_stop(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		       struct ieee80211_sta *sta, u16 tid);
+int rtl92e_tx_agg_oper(struct ieee80211_hw *hw,
+		       struct ieee80211_sta *sta, u16 tid);
+int rtl92e_rx_agg_start(struct ieee80211_hw *hw,
+		        struct ieee80211_sta *sta, u16 tid);
+int rtl92e_rx_agg_stop(struct ieee80211_hw *hw,
+		       struct ieee80211_sta *sta, u16 tid);
+void rtl92e_watchdog_wq_callback(void *data);
+void rtl92e_fwevt_wq_callback(void *data);
+
+void stg_rtl_get_tcb_desc(struct ieee80211_hw *hw,
+			  struct ieee80211_tx_info *info,
+			  struct ieee80211_sta *sta,
+			  struct sk_buff *skb, struct rtl_tcb_desc *tcb_desc);
+
+int stg_rtl_send_smps_action(struct ieee80211_hw *hw,
+			     struct ieee80211_sta *sta,
+			     enum ieee80211_smps_mode smps);
+u8 *rtl92e_find_ie(u8 *data, unsigned int len, u8 ie);
+void rtl92e_recognize_peer(struct ieee80211_hw *hw, u8 *data, unsigned int len);
+u8 rtl92e_tid_to_ac(struct ieee80211_hw *hw, u8 tid);
+void rtl92e_easy_concurrent_retrytimer_callback(unsigned long data);
+extern struct rtl_global_var global_var;
+int  rtl_core_module_init(void);
+void  rtl_core_module_exit(void);
+
+#endif
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbt_precomp.h b/drivers/staging/rtl8192ee/btcoexist/halbt_precomp.h
new file mode 100644
index 00000000000..56e6c465189
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/halbt_precomp.h
@@ -0,0 +1,50 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * 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.
+ *
+ * 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	__HALBT_PRECOMP_H__
+#define __HALBT_PRECOMP_H__
+/*************************************************************
+ * include files
+ *************************************************************/
+#include "../wifi.h"
+#include "../efuse.h"
+#include "../base.h"
+#include "../regd.h"
+#include "../cam.h"
+#include "../ps.h"
+#include "../pci.h"
+
+#include "halbtcoutsrc.h"
+
+
+#include "halbtc8192e2ant.h"
+#include "halbtc8723b1ant.h"
+#include "halbtc8723b2ant.h"
+#include "halbtc8821a2ant.h"
+#include "halbtc8821a1ant.h"
+
+#define	MASKBYTE0			0xff
+#define	MASKBYTE1			0xff00
+#define	MASKBYTE2			0xff0000
+#define	MASKBYTE3			0xff000000
+#define	MASKHWORD			0xffff0000
+#define	MASKLWORD			0x0000ffff
+#define	MASKDWORD			0xffffffff
+#define	MASK12BITS			0xfff
+#define	MASKH4BITS			0xf0000000
+#define MASKOFDM_D			0xffc00000
+#define	MASKCCK				0x3f3f3f3f
+
+#endif	/* __HALBT_PRECOMP_H__ */
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbtc8192e2ant.c b/drivers/staging/rtl8192ee/btcoexist/halbtc8192e2ant.c
new file mode 100644
index 00000000000..ab2cc2fadd8
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/halbtc8192e2ant.c
@@ -0,0 +1,4110 @@
+/**************************************************************
+ * Description:
+ *
+ * This file is for RTL8192E Co-exist mechanism
+ *
+ * History
+ * 2012/11/15 Cosa first check in.
+ *
+ **************************************************************/
+
+/**************************************************************
+ *   include files
+ **************************************************************/
+#include "halbt_precomp.h"
+/**************************************************************
+ *   Global variables, these are static variables
+ **************************************************************/
+static struct coex_dm_8192e_2ant glcoex_dm_8192e_2ant;
+static struct coex_dm_8192e_2ant *coex_dm = &glcoex_dm_8192e_2ant;
+static struct coex_sta_8192e_2ant glcoex_sta_8192e_2ant;
+static struct coex_sta_8192e_2ant *coex_sta = &glcoex_sta_8192e_2ant;
+
+static const char *const glbt_infosrc8192e2ant[] = {
+	"BT Info[wifi fw]",
+	"BT Info[bt rsp]",
+	"BT Info[bt auto report]",
+};
+
+static u32 glcoex_ver_date_8192e_2ant = 20130902;
+static u32 glcoex_ver_8192e_2ant = 0x34;
+
+/**************************************************************
+ *   local function proto type if needed
+ **************************************************************/
+/**************************************************************
+ *   local function start with halbtc8192e2ant_
+ **************************************************************/
+static u8 halbtc8192e2ant_btrssi_state(u8 level_num, u8 rssi_thresh,
+				       u8 rssi_thresh1)
+{
+	int btrssi = 0;
+	u8 btrssi_state = coex_sta->pre_bt_rssi_state;
+
+	btrssi = coex_sta->bt_rssi;
+
+	if (level_num == 2) {
+		if ((coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_LOW) ||
+		    (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+				  "BT Rssi pre state = LOW\n");
+			if (btrssi >= (rssi_thresh +
+				       BTC_RSSI_COEX_THRESH_TOL_8192E_2ANT)) {
+				btrssi_state = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "BT Rssi state switch to High\n");
+			} else {
+				btrssi_state = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "BT Rssi state stay at Low\n");
+			}
+		} else {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+				  "BT Rssi pre state = HIGH\n");
+			if (btrssi < rssi_thresh) {
+				btrssi_state = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "BT Rssi state switch to Low\n");
+			} else {
+				btrssi_state = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "BT Rssi state stay at High\n");
+			}
+		}
+	} else if (level_num == 3) {
+		if (rssi_thresh > rssi_thresh1) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+				  "BT Rssi thresh error!!\n");
+			return coex_sta->pre_bt_rssi_state;
+		}
+
+		if ((coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_LOW) ||
+		    (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+				  "BT Rssi pre state = LOW\n");
+			if (btrssi >= (rssi_thresh +
+				      BTC_RSSI_COEX_THRESH_TOL_8192E_2ANT)) {
+				btrssi_state = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "BT Rssi state switch to Medium\n");
+			} else {
+				btrssi_state = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "BT Rssi state stay at Low\n");
+			}
+		} else if ((coex_sta->pre_bt_rssi_state ==
+			    BTC_RSSI_STATE_MEDIUM) ||
+			   (coex_sta->pre_bt_rssi_state ==
+			    BTC_RSSI_STATE_STAY_MEDIUM)) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+				  "[BTCoex], BT Rssi pre state = MEDIUM\n");
+			if (btrssi >= (rssi_thresh1 +
+				       BTC_RSSI_COEX_THRESH_TOL_8192E_2ANT)) {
+				btrssi_state = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "BT Rssi state switch to High\n");
+			} else if (btrssi < rssi_thresh) {
+				btrssi_state = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "BT Rssi state switch to Low\n");
+			} else {
+				btrssi_state = BTC_RSSI_STATE_STAY_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "BT Rssi state stay at Medium\n");
+			}
+		} else {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+				  "BT Rssi pre state = HIGH\n");
+			if (btrssi < rssi_thresh1) {
+				btrssi_state = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "BT Rssi state switch to Medium\n");
+			} else {
+				btrssi_state = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "BT Rssi state stay at High\n");
+			}
+		}
+	}
+
+	coex_sta->pre_bt_rssi_state = btrssi_state;
+
+	return btrssi_state;
+}
+
+static u8 halbtc8192e2ant_wifirssi_state(struct btc_coexist *btcoexist,
+					 u8 index, u8 level_num, u8 rssi_thresh,
+					 u8 rssi_thresh1)
+{
+	int wifirssi = 0;
+	u8 wifirssi_state = coex_sta->pre_wifi_rssi_state[index];
+
+	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifirssi);
+
+	if (level_num == 2) {
+		if ((coex_sta->pre_wifi_rssi_state[index] ==
+		     BTC_RSSI_STATE_LOW) ||
+		    (coex_sta->pre_wifi_rssi_state[index] ==
+		     BTC_RSSI_STATE_STAY_LOW)) {
+			if (wifirssi >= (rssi_thresh +
+					 BTC_RSSI_COEX_THRESH_TOL_8192E_2ANT)) {
+				wifirssi_state = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "wifi RSSI state switch to High\n");
+			} else {
+				wifirssi_state = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "wifi RSSI state stay at Low\n");
+			}
+		} else {
+			if (wifirssi < rssi_thresh) {
+				wifirssi_state = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "wifi RSSI state switch to Low\n");
+			} else {
+				wifirssi_state = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "wifi RSSI state stay at High\n");
+			}
+		}
+	} else if (level_num == 3) {
+		if (rssi_thresh > rssi_thresh1) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE,
+				  "wifi RSSI thresh error!!\n");
+			return coex_sta->pre_wifi_rssi_state[index];
+		}
+
+		if ((coex_sta->pre_wifi_rssi_state[index] ==
+		     BTC_RSSI_STATE_LOW) ||
+		    (coex_sta->pre_wifi_rssi_state[index] ==
+		     BTC_RSSI_STATE_STAY_LOW)) {
+			if (wifirssi >= (rssi_thresh +
+					 BTC_RSSI_COEX_THRESH_TOL_8192E_2ANT)) {
+				wifirssi_state = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "wifi RSSI state switch to Medium\n");
+			} else {
+				wifirssi_state = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "wifi RSSI state stay at Low\n");
+			}
+		} else if ((coex_sta->pre_wifi_rssi_state[index] ==
+			    BTC_RSSI_STATE_MEDIUM) ||
+			   (coex_sta->pre_wifi_rssi_state[index] ==
+			    BTC_RSSI_STATE_STAY_MEDIUM)) {
+			if (wifirssi >= (rssi_thresh1 +
+					 BTC_RSSI_COEX_THRESH_TOL_8192E_2ANT)) {
+				wifirssi_state = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "wifi RSSI state switch to High\n");
+			} else if (wifirssi < rssi_thresh) {
+				wifirssi_state = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "wifi RSSI state switch to Low\n");
+			} else {
+				wifirssi_state = BTC_RSSI_STATE_STAY_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "wifi RSSI state stay at Medium\n");
+			}
+		} else {
+			if (wifirssi < rssi_thresh1) {
+				wifirssi_state = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "wifi RSSI state switch to Medium\n");
+			} else {
+				wifirssi_state = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "wifi RSSI state stay at High\n");
+			}
+		}
+	}
+
+	coex_sta->pre_wifi_rssi_state[index] = wifirssi_state;
+
+	return wifirssi_state;
+}
+
+static void halbtc_monitor_bt_enable_disable(struct btc_coexist *btcoexist)
+{
+	static bool pre_bt_disabled;
+	static u32 bt_disable_cnt;
+	bool bt_active = true, bt_disabled = false;
+
+	/* This function check if bt is disabled */
+
+	if (coex_sta->high_priority_tx == 0 &&
+	    coex_sta->high_priority_rx == 0 &&
+	    coex_sta->low_priority_tx == 0 &&
+	    coex_sta->low_priority_rx == 0)
+		bt_active = false;
+
+	if (coex_sta->high_priority_tx == 0xffff &&
+	    coex_sta->high_priority_rx == 0xffff &&
+	    coex_sta->low_priority_tx == 0xffff &&
+	    coex_sta->low_priority_rx == 0xffff)
+		bt_active = false;
+
+	if (bt_active) {
+		bt_disable_cnt = 0;
+		bt_disabled = false;
+		btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_DISABLE,
+				   &bt_disabled);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+			  "[BTCoex], BT is enabled !!\n");
+	} else {
+		bt_disable_cnt++;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+			  "[BTCoex], bt all counters = 0, %d times!!\n",
+			  bt_disable_cnt);
+		if (bt_disable_cnt >= 2) {
+			bt_disabled = true;
+			btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_DISABLE,
+					   &bt_disabled);
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+				  "[BTCoex], BT is disabled !!\n");
+		}
+	}
+	if (pre_bt_disabled != bt_disabled) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+			  "[BTCoex], BT is from %s to %s!!\n",
+			  (pre_bt_disabled ? "disabled" : "enabled"),
+			  (bt_disabled ? "disabled" : "enabled"));
+		pre_bt_disabled = bt_disabled;
+	}
+}
+
+static u32 halbtc8192e2ant_decidera_mask(struct btc_coexist *btcoexist,
+					 u8 sstype, u32 ra_masktype)
+{
+	u32 disra_mask = 0x0;
+
+	switch (ra_masktype) {
+	case 0: /* normal mode */
+		if (sstype == 2)
+			disra_mask = 0x0;	/* enable 2ss */
+		else
+			disra_mask = 0xfff00000;/* disable 2ss */
+		break;
+	case 1: /* disable cck 1/2 */
+		if (sstype == 2)
+			disra_mask = 0x00000003;/* enable 2ss */
+		else
+			disra_mask = 0xfff00003;/* disable 2ss */
+		break;
+	case 2: /* disable cck 1/2/5.5, ofdm 6/9/12/18/24, mcs 0/1/2/3/4 */
+		if (sstype == 2)
+			disra_mask = 0x0001f1f7;/* enable 2ss */
+		else
+			disra_mask = 0xfff1f1f7;/* disable 2ss */
+		break;
+	default:
+		break;
+	}
+
+	return disra_mask;
+}
+
+static void halbtc8192e2ant_updatera_mask(struct btc_coexist *btcoexist,
+					  bool force_exec, u32 dis_ratemask)
+{
+	coex_dm->curra_mask = dis_ratemask;
+
+	if (force_exec || (coex_dm->prera_mask != coex_dm->curra_mask))
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_UPDATE_ra_mask,
+				   &coex_dm->curra_mask);
+	coex_dm->prera_mask = coex_dm->curra_mask;
+}
+
+static void autorate_fallback_retry(struct btc_coexist *btcoexist,
+				    bool force_exec, u8 type)
+{
+	bool wifi_under_bmode = false;
+
+	coex_dm->cur_arfrtype = type;
+
+	if (force_exec || (coex_dm->pre_arfrtype != coex_dm->cur_arfrtype)) {
+		switch (coex_dm->cur_arfrtype) {
+		case 0:	/* normal mode */
+			btcoexist->btc_write_4byte(btcoexist, 0x430,
+						   coex_dm->backup_arfr_cnt1);
+			btcoexist->btc_write_4byte(btcoexist, 0x434,
+						   coex_dm->backup_arfr_cnt2);
+			break;
+		case 1:
+			btcoexist->btc_get(btcoexist,
+					   BTC_GET_BL_WIFI_UNDER_B_MODE,
+					   &wifi_under_bmode);
+			if (wifi_under_bmode) {
+				btcoexist->btc_write_4byte(btcoexist, 0x430,
+							   0x0);
+				btcoexist->btc_write_4byte(btcoexist, 0x434,
+							   0x01010101);
+			} else {
+				btcoexist->btc_write_4byte(btcoexist, 0x430,
+							   0x0);
+				btcoexist->btc_write_4byte(btcoexist, 0x434,
+							   0x04030201);
+			}
+			break;
+		default:
+			break;
+		}
+	}
+
+	coex_dm->pre_arfrtype = coex_dm->cur_arfrtype;
+}
+
+static void halbtc8192e2ant_retrylimit(struct btc_coexist *btcoexist,
+				       bool force_exec, u8 type)
+{
+	coex_dm->cur_retrylimit_type = type;
+
+	if (force_exec || (coex_dm->pre_retrylimit_type !=
+			   coex_dm->cur_retrylimit_type)) {
+		switch (coex_dm->cur_retrylimit_type) {
+		case 0:	/* normal mode */
+			btcoexist->btc_write_2byte(btcoexist, 0x42a,
+					    coex_dm->backup_retrylimit);
+			break;
+		case 1:	/* retry limit = 8 */
+			btcoexist->btc_write_2byte(btcoexist, 0x42a,
+						   0x0808);
+			break;
+		default:
+			break;
+		}
+	}
+
+	coex_dm->pre_retrylimit_type = coex_dm->cur_retrylimit_type;
+}
+
+static void halbtc8192e2ant_ampdu_maxtime(struct btc_coexist *btcoexist,
+					  bool force_exec, u8 type)
+{
+	coex_dm->cur_ampdutime_type = type;
+
+	if (force_exec || (coex_dm->pre_ampdutime_type !=
+			   coex_dm->cur_ampdutime_type)) {
+		switch (coex_dm->cur_ampdutime_type) {
+		case 0:	/* normal mode */
+			btcoexist->btc_write_1byte(btcoexist, 0x456,
+						coex_dm->backup_ampdu_maxtime);
+			break;
+		case 1:	/* AMPDU timw = 0x38 * 32us */
+			btcoexist->btc_write_1byte(btcoexist, 0x456, 0x38);
+			break;
+		default:
+			break;
+		}
+	}
+
+	coex_dm->pre_ampdutime_type = coex_dm->cur_ampdutime_type;
+}
+
+static void halbtc8192e2ant_limited_tx(struct btc_coexist *btcoexist,
+				       bool force_exec, u8 ra_masktype,
+				       u8 arfr_type, u8 retrylimit_type,
+				       u8 ampdutime_type)
+{
+	u32 disra_mask = 0x0;
+
+	coex_dm->curra_masktype = ra_masktype;
+	disra_mask = halbtc8192e2ant_decidera_mask(btcoexist,
+						   coex_dm->cur_sstype,
+						   ra_masktype);
+	halbtc8192e2ant_updatera_mask(btcoexist, force_exec, disra_mask);
+
+	autorate_fallback_retry(btcoexist, force_exec, arfr_type);
+	halbtc8192e2ant_retrylimit(btcoexist, force_exec, retrylimit_type);
+	halbtc8192e2ant_ampdu_maxtime(btcoexist, force_exec, ampdutime_type);
+}
+
+static void halbtc8192e2ant_limited_rx(struct btc_coexist *btcoexist,
+				       bool force_exec, bool rej_ap_agg_pkt,
+				       bool b_bt_ctrl_agg_buf_size,
+				       u8 agg_buf_size)
+{
+	bool reject_rx_agg = rej_ap_agg_pkt;
+	bool bt_ctrl_rx_agg_size = b_bt_ctrl_agg_buf_size;
+	u8 rx_agg_size = agg_buf_size;
+
+	/*********************************************
+	 *	Rx Aggregation related setting
+	 *********************************************/
+	btcoexist->btc_set(btcoexist, BTC_SET_BL_TO_REJ_AP_AGG_PKT,
+			   &reject_rx_agg);
+	/* decide BT control aggregation buf size or not */
+	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_CTRL_AGG_SIZE,
+			   &bt_ctrl_rx_agg_size);
+	/* aggregation buf size, only work
+	 * when BT control Rx aggregation size. */
+	btcoexist->btc_set(btcoexist, BTC_SET_U1_AGG_BUF_SIZE, &rx_agg_size);
+	/* real update aggregation setting */
+	btcoexist->btc_set(btcoexist, BTC_SET_ACT_AGGREGATE_CTRL, NULL);
+}
+
+static void halbtc8192e2ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
+{
+	u32 reg_hp_txrx, reg_lp_txrx, u32tmp;
+	u32 reg_hp_tx = 0, reg_hp_rx = 0, reg_lp_tx = 0, reg_lp_rx = 0;
+
+	reg_hp_txrx = 0x770;
+	reg_lp_txrx = 0x774;
+
+	u32tmp = btcoexist->btc_read_4byte(btcoexist, reg_hp_txrx);
+	reg_hp_tx = u32tmp & MASKLWORD;
+	reg_hp_rx = (u32tmp & MASKHWORD)>>16;
+
+	u32tmp = btcoexist->btc_read_4byte(btcoexist, reg_lp_txrx);
+	reg_lp_tx = u32tmp & MASKLWORD;
+	reg_lp_rx = (u32tmp & MASKHWORD)>>16;
+
+	coex_sta->high_priority_tx = reg_hp_tx;
+	coex_sta->high_priority_rx = reg_hp_rx;
+	coex_sta->low_priority_tx = reg_lp_tx;
+	coex_sta->low_priority_rx = reg_lp_rx;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+		  "[BTCoex] High Priority Tx/Rx (reg 0x%x) = 0x%x(%d)/0x%x(%d)\n",
+		  reg_hp_txrx, reg_hp_tx, reg_hp_tx, reg_hp_rx, reg_hp_rx);
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+		  "[BTCoex] Low Priority Tx/Rx (reg 0x%x) = 0x%x(%d)/0x%x(%d)\n",
+		  reg_lp_txrx, reg_lp_tx, reg_lp_tx, reg_lp_rx, reg_lp_rx);
+
+	/* reset counter */
+	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
+}
+
+static void halbtc8192e2ant_querybt_info(struct btc_coexist *btcoexist)
+{
+	u8 h2c_parameter[1] = {0};
+
+	coex_sta->c2h_bt_info_req_sent = true;
+
+	h2c_parameter[0] |= BIT(0);	/* trigger */
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], Query Bt Info, FW write 0x61 = 0x%x\n",
+		  h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x61, 1, h2c_parameter);
+}
+
+static void halbtc8192e2ant_update_btlink_info(struct btc_coexist *btcoexist)
+{
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	bool bt_hson = false;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hson);
+
+	bt_link_info->bt_link_exist = coex_sta->bt_link_exist;
+	bt_link_info->sco_exist = coex_sta->sco_exist;
+	bt_link_info->a2dp_exist = coex_sta->a2dp_exist;
+	bt_link_info->pan_exist = coex_sta->pan_exist;
+	bt_link_info->hid_exist = coex_sta->hid_exist;
+
+	/* work around for HS mode. */
+	if (bt_hson) {
+		bt_link_info->pan_exist = true;
+		bt_link_info->bt_link_exist = true;
+	}
+
+	/* check if Sco only */
+	if (bt_link_info->sco_exist &&
+	    !bt_link_info->a2dp_exist &&
+	    !bt_link_info->pan_exist &&
+	    !bt_link_info->hid_exist)
+		bt_link_info->sco_only = true;
+	else
+		bt_link_info->sco_only = false;
+
+	/* check if A2dp only */
+	if (!bt_link_info->sco_exist &&
+	    bt_link_info->a2dp_exist &&
+	    !bt_link_info->pan_exist &&
+	    !bt_link_info->hid_exist)
+		bt_link_info->a2dp_only = true;
+	else
+		bt_link_info->a2dp_only = false;
+
+	/* check if Pan only */
+	if (!bt_link_info->sco_exist &&
+	    !bt_link_info->a2dp_exist &&
+	    bt_link_info->pan_exist &&
+	    !bt_link_info->hid_exist)
+		bt_link_info->pan_only = true;
+	else
+		bt_link_info->pan_only = false;
+
+	/* check if Hid only */
+	if (!bt_link_info->sco_exist &&
+	    !bt_link_info->a2dp_exist &&
+	    !bt_link_info->pan_exist &&
+	    bt_link_info->hid_exist)
+		bt_link_info->hid_only = true;
+	else
+		bt_link_info->hid_only = false;
+}
+
+static u8 halbtc8192e2ant_action_algorithm(struct btc_coexist *btcoexist)
+{
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	struct btc_stack_info *stack_info = &btcoexist->stack_info;
+	bool bt_hson = false;
+	u8 algorithm = BT_8192E_2ANT_COEX_ALGO_UNDEFINED;
+	u8 num_diffprofile = 0;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hson);
+
+	if (!bt_link_info->bt_link_exist) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "No BT link exists!!!\n");
+		return algorithm;
+	}
+
+	if (bt_link_info->sco_exist)
+		num_diffprofile++;
+	if (bt_link_info->hid_exist)
+		num_diffprofile++;
+	if (bt_link_info->pan_exist)
+		num_diffprofile++;
+	if (bt_link_info->a2dp_exist)
+		num_diffprofile++;
+
+	if (num_diffprofile == 1) {
+		if (bt_link_info->sco_exist) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "SCO only\n");
+			algorithm = BT_8192E_2ANT_COEX_ALGO_SCO;
+		} else {
+			if (bt_link_info->hid_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "HID only\n");
+				algorithm = BT_8192E_2ANT_COEX_ALGO_HID;
+			} else if (bt_link_info->a2dp_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "A2DP only\n");
+				algorithm = BT_8192E_2ANT_COEX_ALGO_A2DP;
+			} else if (bt_link_info->pan_exist) {
+				if (bt_hson) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "PAN(HS) only\n");
+					algorithm =
+						BT_8192E_2ANT_COEX_ALGO_PANHS;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "PAN(EDR) only\n");
+					algorithm =
+						BT_8192E_2ANT_COEX_ALGO_PANEDR;
+				}
+			}
+		}
+	} else if (num_diffprofile == 2) {
+		if (bt_link_info->sco_exist) {
+			if (bt_link_info->hid_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "SCO + HID\n");
+				algorithm = BT_8192E_2ANT_COEX_ALGO_SCO;
+			} else if (bt_link_info->a2dp_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "SCO + A2DP ==> SCO\n");
+				algorithm = BT_8192E_2ANT_COEX_ALGO_PANEDR_HID;
+			} else if (bt_link_info->pan_exist) {
+				if (bt_hson) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "SCO + PAN(HS)\n");
+					algorithm = BT_8192E_2ANT_COEX_ALGO_SCO;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "SCO + PAN(EDR)\n");
+					algorithm =
+						BT_8192E_2ANT_COEX_ALGO_SCO_PAN;
+				}
+			}
+		} else {
+			if (bt_link_info->hid_exist &&
+			    bt_link_info->a2dp_exist) {
+				if (stack_info->num_of_hid >= 2) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "HID*2 + A2DP\n");
+					algorithm =
+					BT_8192E_2ANT_COEX_ALGO_HID_A2DP_PANEDR;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "HID + A2DP\n");
+					algorithm =
+					    BT_8192E_2ANT_COEX_ALGO_HID_A2DP;
+				}
+			} else if (bt_link_info->hid_exist &&
+				   bt_link_info->pan_exist) {
+				if (bt_hson) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "HID + PAN(HS)\n");
+					algorithm = BT_8192E_2ANT_COEX_ALGO_HID;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "HID + PAN(EDR)\n");
+					algorithm =
+					    BT_8192E_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			} else if (bt_link_info->pan_exist &&
+				   bt_link_info->a2dp_exist) {
+				if (bt_hson) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "A2DP + PAN(HS)\n");
+					algorithm =
+					    BT_8192E_2ANT_COEX_ALGO_A2DP_PANHS;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "A2DP + PAN(EDR)\n");
+					algorithm =
+					    BT_8192E_2ANT_COEX_ALGO_PANEDR_A2DP;
+				}
+			}
+		}
+	} else if (num_diffprofile == 3) {
+		if (bt_link_info->sco_exist) {
+			if (bt_link_info->hid_exist &&
+			    bt_link_info->a2dp_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "SCO + HID + A2DP ==> HID\n");
+				algorithm = BT_8192E_2ANT_COEX_ALGO_PANEDR_HID;
+			} else if (bt_link_info->hid_exist &&
+				   bt_link_info->pan_exist) {
+				if (bt_hson) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "SCO + HID + PAN(HS)\n");
+					algorithm = BT_8192E_2ANT_COEX_ALGO_SCO;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "SCO + HID + PAN(EDR)\n");
+					algorithm =
+						BT_8192E_2ANT_COEX_ALGO_SCO_PAN;
+				}
+			} else if (bt_link_info->pan_exist &&
+				   bt_link_info->a2dp_exist) {
+				if (bt_hson) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "SCO + A2DP + PAN(HS)\n");
+					algorithm = BT_8192E_2ANT_COEX_ALGO_SCO;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "SCO + A2DP + PAN(EDR)\n");
+					algorithm =
+					    BT_8192E_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		} else {
+			if (bt_link_info->hid_exist &&
+			    bt_link_info->pan_exist &&
+			    bt_link_info->a2dp_exist) {
+				if (bt_hson) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "HID + A2DP + PAN(HS)\n");
+					algorithm =
+					    BT_8192E_2ANT_COEX_ALGO_HID_A2DP;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "HID + A2DP + PAN(EDR)\n");
+					algorithm =
+					BT_8192E_2ANT_COEX_ALGO_HID_A2DP_PANEDR;
+				}
+			}
+		}
+	} else if (num_diffprofile >= 3) {
+		if (bt_link_info->sco_exist) {
+			if (bt_link_info->hid_exist &&
+			    bt_link_info->pan_exist &&
+			    bt_link_info->a2dp_exist) {
+				if (bt_hson) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "ErrorSCO+HID+A2DP+PAN(HS)\n");
+
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "SCO+HID+A2DP+PAN(EDR)\n");
+					algorithm =
+					    BT_8192E_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		}
+	}
+
+	return algorithm;
+}
+
+static void halbtc8192e2ant_setfw_dac_swinglevel(struct btc_coexist *btcoexist,
+						 u8 dac_swinglvl)
+{
+	u8 h2c_parameter[1] = {0};
+
+	/* There are several type of dacswing
+	 * 0x18/ 0x10/ 0xc/ 0x8/ 0x4/ 0x6 */
+	h2c_parameter[0] = dac_swinglvl;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], Set Dac Swing Level = 0x%x\n", dac_swinglvl);
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], FW write 0x64 = 0x%x\n", h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x64, 1, h2c_parameter);
+}
+
+static void halbtc8192e2ant_set_fwdec_btpwr(struct btc_coexist *btcoexist,
+					    u8 dec_btpwr_lvl)
+{
+	u8 h2c_parameter[1] = {0};
+
+	h2c_parameter[0] = dec_btpwr_lvl;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex] decrease Bt Power level = %d, FW write 0x62 = 0x%x\n",
+		  dec_btpwr_lvl, h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x62, 1, h2c_parameter);
+}
+
+static void halbtc8192e2ant_dec_btpwr(struct btc_coexist *btcoexist,
+				      bool force_exec, u8 dec_btpwr_lvl)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], %s Dec BT power level = %d\n",
+		  (force_exec ? "force to" : ""), dec_btpwr_lvl);
+	coex_dm->cur_dec_bt_pwr = dec_btpwr_lvl;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], preBtDecPwrLvl =%d, curBtDecPwrLvl =%d\n",
+			  coex_dm->pre_dec_bt_pwr, coex_dm->cur_dec_bt_pwr);
+	}
+	halbtc8192e2ant_set_fwdec_btpwr(btcoexist, coex_dm->cur_dec_bt_pwr);
+
+	coex_dm->pre_dec_bt_pwr = coex_dm->cur_dec_bt_pwr;
+}
+
+static void halbtc8192e2ant_set_bt_autoreport(struct btc_coexist *btcoexist,
+					      bool enable_autoreport)
+{
+	u8 h2c_parameter[1] = {0};
+
+	h2c_parameter[0] = 0;
+
+	if (enable_autoreport)
+		h2c_parameter[0] |= BIT(0);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], BT FW auto report : %s, FW write 0x68 = 0x%x\n",
+		  (enable_autoreport ? "Enabled!!" : "Disabled!!"),
+		  h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x68, 1, h2c_parameter);
+}
+
+static void halbtc8192e2ant_bt_autoreport(struct btc_coexist *btcoexist,
+					  bool force_exec,
+					  bool enable_autoreport)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], %s BT Auto report = %s\n",
+		  (force_exec ? "force to" : ""),
+		  ((enable_autoreport) ? "Enabled" : "Disabled"));
+	coex_dm->cur_bt_auto_report = enable_autoreport;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex] bPreBtAutoReport =%d, bCurBtAutoReport =%d\n",
+			  coex_dm->pre_bt_auto_report,
+			  coex_dm->cur_bt_auto_report);
+
+		if (coex_dm->pre_bt_auto_report == coex_dm->cur_bt_auto_report)
+			return;
+	}
+	halbtc8192e2ant_set_bt_autoreport(btcoexist,
+					  coex_dm->cur_bt_auto_report);
+
+	coex_dm->pre_bt_auto_report = coex_dm->cur_bt_auto_report;
+}
+
+static void halbtc8192e2ant_fw_dac_swinglvl(struct btc_coexist *btcoexist,
+					    bool force_exec, u8 fw_dac_swinglvl)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], %s set FW Dac Swing level = %d\n",
+		  (force_exec ? "force to" : ""), fw_dac_swinglvl);
+	coex_dm->cur_fw_dac_swing_lvl = fw_dac_swinglvl;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex] preFwDacSwingLvl =%d, curFwDacSwingLvl =%d\n",
+			  coex_dm->pre_fw_dac_swing_lvl,
+			  coex_dm->cur_fw_dac_swing_lvl);
+
+		if (coex_dm->pre_fw_dac_swing_lvl ==
+		    coex_dm->cur_fw_dac_swing_lvl)
+			return;
+	}
+
+	halbtc8192e2ant_setfw_dac_swinglevel(btcoexist,
+					     coex_dm->cur_fw_dac_swing_lvl);
+
+	coex_dm->pre_fw_dac_swing_lvl = coex_dm->cur_fw_dac_swing_lvl;
+}
+
+static void set_sw_rf_rx_lpf_corner(struct btc_coexist *btcoexist,
+				    bool rx_rf_shrink_on)
+{
+	if (rx_rf_shrink_on) {
+		/* Shrink RF Rx LPF corner */
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+			  "[BTCoex], Shrink RF Rx LPF corner!!\n");
+		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1e,
+					  0xfffff, 0xffffc);
+	} else {
+		/* Resume RF Rx LPF corner
+		 * After initialized, we can use coex_dm->btRf0x1eBackup */
+		if (btcoexist->initilized) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+				  "[BTCoex], Resume RF Rx LPF corner!!\n");
+			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1e,
+						  0xfffff,
+						  coex_dm->bt_rf0x1e_backup);
+		}
+	}
+}
+
+static void halbtc8192e2ant_rf_shrink(struct btc_coexist *btcoexist,
+				      bool force_exec, bool rx_rf_shrink_on)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
+		  "[BTCoex], %s turn Rx RF Shrink = %s\n",
+		  (force_exec ? "force to" : ""),
+		  ((rx_rf_shrink_on) ? "ON" : "OFF"));
+	coex_dm->cur_rf_rx_lpf_shrink = rx_rf_shrink_on;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "[BTCoex]bPreRfRxLpfShrink =%d, bCurRfRxLpfShrink =%d\n",
+			  coex_dm->pre_rf_rx_lpf_shrink,
+			  coex_dm->cur_rf_rx_lpf_shrink);
+
+		if (coex_dm->pre_rf_rx_lpf_shrink ==
+		    coex_dm->cur_rf_rx_lpf_shrink)
+			return;
+	}
+	set_sw_rf_rx_lpf_corner(btcoexist, coex_dm->cur_rf_rx_lpf_shrink);
+
+	coex_dm->pre_rf_rx_lpf_shrink = coex_dm->cur_rf_rx_lpf_shrink;
+}
+
+static void halbtc8192e2ant_set_dac_swingreg(struct btc_coexist *btcoexist,
+					     u32 level)
+{
+	u8 val = (u8)level;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+		  "[BTCoex], Write SwDacSwing = 0x%x\n", level);
+	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x883, 0x3e, val);
+}
+
+static void setsw_fulltime_dacswing(struct btc_coexist *btcoexist,
+				    bool sw_dac_swingon,
+				    u32 sw_dac_swinglvl)
+{
+	if (sw_dac_swingon)
+		halbtc8192e2ant_set_dac_swingreg(btcoexist, sw_dac_swinglvl);
+	else
+		halbtc8192e2ant_set_dac_swingreg(btcoexist, 0x18);
+}
+
+static void halbtc8192e2ant_dacswing(struct btc_coexist *btcoexist,
+				     bool force_exec, bool dac_swingon,
+				     u32 dac_swinglvl)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
+		  "[BTCoex], %s turn DacSwing =%s, dac_swinglvl = 0x%x\n",
+		  (force_exec ? "force to" : ""),
+		  ((dac_swingon) ? "ON" : "OFF"), dac_swinglvl);
+	coex_dm->cur_dac_swing_on = dac_swingon;
+	coex_dm->cur_dac_swing_lvl = dac_swinglvl;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "[BTCoex], bPreDacSwingOn =%d, preDacSwingLvl = 0x%x, ",
+			  coex_dm->pre_dac_swing_on,
+			  coex_dm->pre_dac_swing_lvl);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "bCurDacSwingOn =%d, curDacSwingLvl = 0x%x\n",
+			  coex_dm->cur_dac_swing_on,
+			  coex_dm->cur_dac_swing_lvl);
+
+		if ((coex_dm->pre_dac_swing_on == coex_dm->cur_dac_swing_on) &&
+		    (coex_dm->pre_dac_swing_lvl == coex_dm->cur_dac_swing_lvl))
+			return;
+	}
+	mdelay(30);
+	setsw_fulltime_dacswing(btcoexist, dac_swingon, dac_swinglvl);
+
+	coex_dm->pre_dac_swing_on = coex_dm->cur_dac_swing_on;
+	coex_dm->pre_dac_swing_lvl = coex_dm->cur_dac_swing_lvl;
+}
+
+static void halbtc8192e2ant_set_agc_table(struct btc_coexist *btcoexist,
+					  bool agc_table_en)
+{
+	/* BB AGC Gain Table */
+	if (agc_table_en) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+			  "[BTCoex], BB Agc Table On!\n");
+		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x0a1A0001);
+		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x091B0001);
+		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x081C0001);
+		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x071D0001);
+		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x061E0001);
+		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x051F0001);
+	} else {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+			  "[BTCoex], BB Agc Table Off!\n");
+		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xaa1A0001);
+		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa91B0001);
+		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa81C0001);
+		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa71D0001);
+		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa61E0001);
+		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa51F0001);
+	}
+}
+
+static void halbtc8192e2ant_agctable(struct btc_coexist *btcoexist,
+				     bool force_exec, bool agc_table_en)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
+		  "[BTCoex], %s %s Agc Table\n",
+		  (force_exec ? "force to" : ""),
+		  ((agc_table_en) ? "Enable" : "Disable"));
+	coex_dm->cur_agc_table_en = agc_table_en;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "[BTCoex], bPreAgcTableEn =%d, bCurAgcTableEn =%d\n",
+			  coex_dm->pre_agc_table_en, coex_dm->cur_agc_table_en);
+
+		if (coex_dm->pre_agc_table_en == coex_dm->cur_agc_table_en)
+			return;
+	}
+	halbtc8192e2ant_set_agc_table(btcoexist, agc_table_en);
+
+	coex_dm->pre_agc_table_en = coex_dm->cur_agc_table_en;
+}
+
+static void halbtc8192e2ant_set_coex_table(struct btc_coexist *btcoexist,
+					   u32 val0x6c0, u32 val0x6c4,
+					   u32 val0x6c8, u8 val0x6cc)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+		  "[BTCoex], set coex table, set 0x6c0 = 0x%x\n", val0x6c0);
+	btcoexist->btc_write_4byte(btcoexist, 0x6c0, val0x6c0);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+		  "[BTCoex], set coex table, set 0x6c4 = 0x%x\n", val0x6c4);
+	btcoexist->btc_write_4byte(btcoexist, 0x6c4, val0x6c4);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+		  "[BTCoex], set coex table, set 0x6c8 = 0x%x\n", val0x6c8);
+	btcoexist->btc_write_4byte(btcoexist, 0x6c8, val0x6c8);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+		  "[BTCoex], set coex table, set 0x6cc = 0x%x\n", val0x6cc);
+	btcoexist->btc_write_1byte(btcoexist, 0x6cc, val0x6cc);
+}
+
+static void halbtc_coex_table(struct btc_coexist *btcoexist, bool force_exec,
+			      u32 val0x6c0, u32 val0x6c4,
+			      u32 val0x6c8, u8 val0x6cc)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
+		  "[BTCoex], %s write Coex Table 0x6c0 = 0x%x, ",
+		  (force_exec ? "force to" : ""), val0x6c0);
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
+		  "0x6c4 = 0x%x, 0x6c8 = 0x%x, 0x6cc = 0x%x\n",
+		  val0x6c4, val0x6c8, val0x6cc);
+	coex_dm->cur_val0x6c0 = val0x6c0;
+	coex_dm->cur_val0x6c4 = val0x6c4;
+	coex_dm->cur_val0x6c8 = val0x6c8;
+	coex_dm->cur_val0x6cc = val0x6cc;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "[BTCoex], preVal0x6c0 = 0x%x, preVal0x6c4 = 0x%x, ",
+			  coex_dm->pre_val0x6c0, coex_dm->pre_val0x6c4);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "preVal0x6c8 = 0x%x, preVal0x6cc = 0x%x!!\n",
+			  coex_dm->pre_val0x6c8, coex_dm->pre_val0x6cc);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "[BTCoex], curVal0x6c0 = 0x%x, curVal0x6c4 = 0x%x\n",
+			  coex_dm->cur_val0x6c0, coex_dm->cur_val0x6c4);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "curVal0x6c8 = 0x%x, curVal0x6cc = 0x%x !!\n",
+			  coex_dm->cur_val0x6c8, coex_dm->cur_val0x6cc);
+
+		if ((coex_dm->pre_val0x6c0 == coex_dm->cur_val0x6c0) &&
+		    (coex_dm->pre_val0x6c4 == coex_dm->cur_val0x6c4) &&
+		    (coex_dm->pre_val0x6c8 == coex_dm->cur_val0x6c8) &&
+		    (coex_dm->pre_val0x6cc == coex_dm->cur_val0x6cc))
+			return;
+	}
+	halbtc8192e2ant_set_coex_table(btcoexist, val0x6c0, val0x6c4,
+				       val0x6c8, val0x6cc);
+
+	coex_dm->pre_val0x6c0 = coex_dm->cur_val0x6c0;
+	coex_dm->pre_val0x6c4 = coex_dm->cur_val0x6c4;
+	coex_dm->pre_val0x6c8 = coex_dm->cur_val0x6c8;
+	coex_dm->pre_val0x6cc = coex_dm->cur_val0x6cc;
+}
+
+static void halbtc_coex_table_with_type(struct btc_coexist *btcoexist,
+					bool force_exec, u8 type)
+{
+	switch (type) {
+	case 0:
+		halbtc_coex_table(btcoexist, force_exec, 0x55555555,
+				  0x5a5a5a5a, 0xffffff, 0x3);
+		break;
+	case 1:
+		halbtc_coex_table(btcoexist, force_exec, 0x5a5a5a5a,
+				  0x5a5a5a5a, 0xffffff, 0x3);
+		break;
+	case 2:
+		halbtc_coex_table(btcoexist, force_exec, 0x55555555,
+				  0x5ffb5ffb, 0xffffff, 0x3);
+		break;
+	case 3:
+		halbtc_coex_table(btcoexist, force_exec, 0xdfffdfff,
+				  0x5fdb5fdb, 0xffffff, 0x3);
+		break;
+	case 4:
+		halbtc_coex_table(btcoexist, force_exec, 0xdfffdfff,
+				  0x5ffb5ffb, 0xffffff, 0x3);
+		break;
+	default:
+		break;
+	}
+}
+
+static void halbtc8192e2ant_set_fw_ignore_wlanact(struct btc_coexist *btcoexist,
+						  bool enable)
+{
+	u8 h2c_parameter[1] = {0};
+
+	if (enable)
+		h2c_parameter[0] |= BIT(0); /* function enable */
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex]set FW for BT Ignore Wlan_Act, FW write 0x63 = 0x%x\n",
+		  h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x63, 1, h2c_parameter);
+}
+
+static void halbtc8192e2ant_ignorewlanact(struct btc_coexist *btcoexist,
+					  bool force_exec, bool enable)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], %s turn Ignore WlanAct %s\n",
+		  (force_exec ? "force to" : ""), (enable ? "ON" : "OFF"));
+	coex_dm->cur_ignore_wlan_act = enable;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], bPreIgnoreWlanAct = %d ",
+			  coex_dm->pre_ignore_wlan_act);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "bCurIgnoreWlanAct = %d!!\n",
+			  coex_dm->cur_ignore_wlan_act);
+
+		if (coex_dm->pre_ignore_wlan_act ==
+		    coex_dm->cur_ignore_wlan_act)
+			return;
+	}
+	halbtc8192e2ant_set_fw_ignore_wlanact(btcoexist, enable);
+
+	coex_dm->pre_ignore_wlan_act = coex_dm->cur_ignore_wlan_act;
+}
+
+static void halbtc8192e2ant_setfwpstdma(struct btc_coexist *btcoexist, u8 byte1,
+					u8 byte2, u8 byte3, u8 byte4, u8 byte5)
+{
+	u8 h2c_parameter[5];
+
+	h2c_parameter[0] = byte1;
+	h2c_parameter[1] = byte2;
+	h2c_parameter[2] = byte3;
+	h2c_parameter[3] = byte4;
+	h2c_parameter[4] = byte5;
+
+	coex_dm->ps_tdma_para[0] = byte1;
+	coex_dm->ps_tdma_para[1] = byte2;
+	coex_dm->ps_tdma_para[2] = byte3;
+	coex_dm->ps_tdma_para[3] = byte4;
+	coex_dm->ps_tdma_para[4] = byte5;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], FW write 0x60(5bytes) = 0x%x%08x\n",
+		  h2c_parameter[0],
+		  h2c_parameter[1] << 24 | h2c_parameter[2] << 16 |
+		  h2c_parameter[3] << 8 | h2c_parameter[4]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x60, 5, h2c_parameter);
+}
+
+static void halbtc_sw_mechanism1(struct btc_coexist *btcoexist,
+				 bool shrink_rx_lpf, bool low_penalty_ra,
+				 bool limited_dig, bool btlan_constrain)
+{
+	halbtc8192e2ant_rf_shrink(btcoexist, NORMAL_EXEC, shrink_rx_lpf);
+}
+
+static void halbtc_sw_mechanism2(struct btc_coexist *btcoexist,
+				 bool agc_table_shift, bool adc_backoff,
+				 bool sw_dac_swing, u32 dac_swinglvl)
+{
+	halbtc8192e2ant_agctable(btcoexist, NORMAL_EXEC, agc_table_shift);
+	halbtc8192e2ant_dacswing(btcoexist, NORMAL_EXEC, sw_dac_swing,
+				 dac_swinglvl);
+}
+
+static void halbtc8192e2ant_ps_tdma(struct btc_coexist *btcoexist,
+				    bool force_exec, bool turn_on, u8 type)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], %s turn %s PS TDMA, type =%d\n",
+		  (force_exec ? "force to" : ""),
+		  (turn_on ? "ON" : "OFF"), type);
+	coex_dm->cur_ps_tdma_on = turn_on;
+	coex_dm->cur_ps_tdma = type;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], bPrePsTdmaOn = %d, bCurPsTdmaOn = %d!!\n",
+			  coex_dm->pre_ps_tdma_on, coex_dm->cur_ps_tdma_on);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], prePsTdma = %d, curPsTdma = %d!!\n",
+			  coex_dm->pre_ps_tdma, coex_dm->cur_ps_tdma);
+
+		if ((coex_dm->pre_ps_tdma_on == coex_dm->cur_ps_tdma_on) &&
+		    (coex_dm->pre_ps_tdma == coex_dm->cur_ps_tdma))
+			return;
+	}
+	if (turn_on) {
+		switch (type) {
+		case 1:
+		default:
+			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x1a,
+						    0x1a, 0xe1, 0x90);
+			break;
+		case 2:
+			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x12,
+						    0x12, 0xe1, 0x90);
+			break;
+		case 3:
+			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x1c,
+						    0x3, 0xf1, 0x90);
+			break;
+		case 4:
+			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x10,
+						    0x3, 0xf1, 0x90);
+			break;
+		case 5:
+			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x1a,
+						    0x1a, 0x60, 0x90);
+			break;
+		case 6:
+			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x12,
+						    0x12, 0x60, 0x90);
+			break;
+		case 7:
+			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x1c,
+						    0x3, 0x70, 0x90);
+			break;
+		case 8:
+			halbtc8192e2ant_setfwpstdma(btcoexist, 0xa3, 0x10,
+						    0x3, 0x70, 0x90);
+			break;
+		case 9:
+			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x1a,
+						    0x1a, 0xe1, 0x10);
+			break;
+		case 10:
+			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x12,
+						    0x12, 0xe1, 0x10);
+			break;
+		case 11:
+			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x1c,
+						    0x3, 0xf1, 0x10);
+			break;
+		case 12:
+			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x10,
+						    0x3, 0xf1, 0x10);
+			break;
+		case 13:
+			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x1a,
+						    0x1a, 0xe0, 0x10);
+			break;
+		case 14:
+			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x12,
+						    0x12, 0xe0, 0x10);
+			break;
+		case 15:
+			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x1c,
+						    0x3, 0xf0, 0x10);
+			break;
+		case 16:
+			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x12,
+						    0x3, 0xf0, 0x10);
+			break;
+		case 17:
+			halbtc8192e2ant_setfwpstdma(btcoexist, 0x61, 0x20,
+						    0x03, 0x10, 0x10);
+			break;
+		case 18:
+			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x5,
+						    0x5, 0xe1, 0x90);
+			break;
+		case 19:
+			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x25,
+						    0x25, 0xe1, 0x90);
+			break;
+		case 20:
+			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x25,
+						    0x25, 0x60, 0x90);
+			break;
+		case 21:
+			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x15,
+						    0x03, 0x70, 0x90);
+			break;
+		case 71:
+			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x1a,
+						    0x1a, 0xe1, 0x90);
+			break;
+		}
+	} else {
+		/* disable PS tdma */
+		switch (type) {
+		default:
+		case 0:
+			halbtc8192e2ant_setfwpstdma(btcoexist, 0x8, 0x0, 0x0,
+						    0x0, 0x0);
+			btcoexist->btc_write_1byte(btcoexist, 0x92c, 0x4);
+			break;
+		case 1:
+			halbtc8192e2ant_setfwpstdma(btcoexist, 0x0, 0x0, 0x0,
+						    0x8, 0x0);
+			mdelay(5);
+			btcoexist->btc_write_1byte(btcoexist, 0x92c, 0x20);
+			break;
+		}
+	}
+
+	/* update pre state */
+	coex_dm->pre_ps_tdma_on = coex_dm->cur_ps_tdma_on;
+	coex_dm->pre_ps_tdma = coex_dm->cur_ps_tdma;
+}
+
+static void set_switch_sstype(struct btc_coexist *btcoexist, u8 sstype)
+{
+	u8 mimops = BTC_MIMO_PS_DYNAMIC;
+	u32 disra_mask = 0x0;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+		  "[BTCoex], REAL set SS Type = %d\n", sstype);
+
+	disra_mask = halbtc8192e2ant_decidera_mask(btcoexist, sstype,
+						   coex_dm->curra_masktype);
+	halbtc8192e2ant_updatera_mask(btcoexist, FORCE_EXEC, disra_mask);
+
+	if (sstype == 1) {
+		halbtc8192e2ant_ps_tdma(btcoexist, FORCE_EXEC, false, 1);
+		/* switch ofdm path */
+		btcoexist->btc_write_1byte(btcoexist, 0xc04, 0x11);
+		btcoexist->btc_write_1byte(btcoexist, 0xd04, 0x1);
+		btcoexist->btc_write_4byte(btcoexist, 0x90c, 0x81111111);
+		/* switch cck patch */
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0xe77, 0x4, 0x1);
+		btcoexist->btc_write_1byte(btcoexist, 0xa07, 0x81);
+		mimops = BTC_MIMO_PS_STATIC;
+	} else if (sstype == 2) {
+		halbtc8192e2ant_ps_tdma(btcoexist, FORCE_EXEC, false, 0);
+		btcoexist->btc_write_1byte(btcoexist, 0xc04, 0x33);
+		btcoexist->btc_write_1byte(btcoexist, 0xd04, 0x3);
+		btcoexist->btc_write_4byte(btcoexist, 0x90c, 0x81121313);
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0xe77, 0x4, 0x0);
+		btcoexist->btc_write_1byte(btcoexist, 0xa07, 0x41);
+		mimops = BTC_MIMO_PS_DYNAMIC;
+	}
+	/* set rx 1ss or 2ss */
+	btcoexist->btc_set(btcoexist, BTC_SET_ACT_SEND_MIMO_PS, &mimops);
+}
+
+static void halbtc8192e2ant_switch_sstype(struct btc_coexist *btcoexist,
+					  bool force_exec, u8 new_sstype)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+		  "[BTCoex], %s Switch SS Type = %d\n",
+		  (force_exec ? "force to" : ""), new_sstype);
+	coex_dm->cur_sstype = new_sstype;
+
+	if (!force_exec) {
+		if (coex_dm->pre_sstype == coex_dm->cur_sstype)
+			return;
+	}
+	set_switch_sstype(btcoexist, coex_dm->cur_sstype);
+
+	coex_dm->pre_sstype = coex_dm->cur_sstype;
+}
+
+static void halbtc8192e2ant_coex_alloff(struct btc_coexist *btcoexist)
+{
+	/* fw all off */
+	halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
+	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
+	halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+
+	/* sw all off */
+	halbtc_sw_mechanism1(btcoexist, false, false, false, false);
+	halbtc_sw_mechanism2(btcoexist, false, false, false, 0x18);
+
+	/* hw all off */
+	halbtc_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
+}
+
+static void halbtc8192e2ant_init_coex_dm(struct btc_coexist *btcoexist)
+{
+	/* force to reset coex mechanism */
+
+	halbtc8192e2ant_ps_tdma(btcoexist, FORCE_EXEC, false, 1);
+	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, FORCE_EXEC, 6);
+	halbtc8192e2ant_dec_btpwr(btcoexist, FORCE_EXEC, 0);
+
+	halbtc_coex_table_with_type(btcoexist, FORCE_EXEC, 0);
+	halbtc8192e2ant_switch_sstype(btcoexist, FORCE_EXEC, 2);
+
+	halbtc_sw_mechanism1(btcoexist, false, false, false, false);
+	halbtc_sw_mechanism2(btcoexist, false, false, false, 0x18);
+}
+
+static void halbtc8192e2ant_action_bt_inquiry(struct btc_coexist *btcoexist)
+{
+	bool low_pwr_disable = true;
+
+	btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
+			   &low_pwr_disable);
+
+	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
+
+	halbtc_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+	halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 3);
+	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
+	halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+
+	halbtc_sw_mechanism1(btcoexist, false, false, false, false);
+	halbtc_sw_mechanism2(btcoexist, false, false, false, 0x18);
+}
+
+static bool halbtc8192e2ant_is_common_action(struct btc_coexist *btcoexist)
+{
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	bool common = false, wifi_connected = false, wifi_busy = false;
+	bool bt_hson = false, low_pwr_disable = false;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hson);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+			   &wifi_connected);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+
+	if (bt_link_info->sco_exist || bt_link_info->hid_exist)
+		halbtc8192e2ant_limited_tx(btcoexist, NORMAL_EXEC, 1, 0, 0, 0);
+	else
+		halbtc8192e2ant_limited_tx(btcoexist, NORMAL_EXEC, 0, 0, 0, 0);
+
+	if (!wifi_connected) {
+		low_pwr_disable = false;
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
+				   &low_pwr_disable);
+
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Wifi non-connected idle!!\n");
+
+		if ((BT_8192E_2ANT_BT_STATUS_NON_CONNECTED_IDLE ==
+		     coex_dm->bt_status) ||
+		    (BT_8192E_2ANT_BT_STATUS_CONNECTED_IDLE ==
+		     coex_dm->bt_status)) {
+			halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC,
+						      2);
+			halbtc_coex_table_with_type(btcoexist,
+						    NORMAL_EXEC, 1);
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, false,
+						0);
+		} else {
+			halbtc8192e2ant_switch_sstype(btcoexist,
+						      NORMAL_EXEC, 1);
+			halbtc_coex_table_with_type(btcoexist,
+						    NORMAL_EXEC, 0);
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						false, 1);
+		}
+
+		halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+
+		halbtc_sw_mechanism1(btcoexist, false, false, false,
+				     false);
+		halbtc_sw_mechanism2(btcoexist, false, false, false,
+				     0x18);
+
+		common = true;
+	} else {
+		if (BT_8192E_2ANT_BT_STATUS_NON_CONNECTED_IDLE ==
+		    coex_dm->bt_status) {
+			low_pwr_disable = false;
+			btcoexist->btc_set(btcoexist,
+					   BTC_SET_ACT_DISABLE_LOW_POWER,
+					   &low_pwr_disable);
+
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "Wifi connected + BT non connected-idle!!\n");
+
+			halbtc8192e2ant_switch_sstype(btcoexist,
+						      NORMAL_EXEC, 2);
+			halbtc_coex_table_with_type(btcoexist,
+						    NORMAL_EXEC, 1);
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						false, 0);
+			halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC,
+							6);
+			halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+
+			halbtc_sw_mechanism1(btcoexist, false, false,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, false, false,
+					     false, 0x18);
+
+			common = true;
+		} else if (BT_8192E_2ANT_BT_STATUS_CONNECTED_IDLE ==
+			   coex_dm->bt_status) {
+			low_pwr_disable = true;
+			btcoexist->btc_set(btcoexist,
+					   BTC_SET_ACT_DISABLE_LOW_POWER,
+					   &low_pwr_disable);
+
+			if (bt_hson)
+				return false;
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "Wifi connected + BT connected-idle!!\n");
+
+			halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC,
+						      2);
+			halbtc_coex_table_with_type(btcoexist,
+						    NORMAL_EXEC, 1);
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						false, 0);
+			halbtc8192e2ant_fw_dac_swinglvl(btcoexist,
+							NORMAL_EXEC, 6);
+			halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+
+			halbtc_sw_mechanism1(btcoexist, true, false,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, false, false,
+					     false, 0x18);
+
+			common = true;
+		} else {
+			low_pwr_disable = true;
+			btcoexist->btc_set(btcoexist,
+					   BTC_SET_ACT_DISABLE_LOW_POWER,
+					   &low_pwr_disable);
+
+			if (wifi_busy) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "Wifi Connected-Busy + BT Busy!!\n");
+				common = false;
+			} else {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "Wifi Connected-Idle + BT Busy!!\n");
+
+				halbtc8192e2ant_switch_sstype(btcoexist,
+							      NORMAL_EXEC, 1);
+				halbtc_coex_table_with_type(btcoexist,
+							    NORMAL_EXEC, 2);
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 21);
+				halbtc8192e2ant_fw_dac_swinglvl(btcoexist,
+								NORMAL_EXEC, 6);
+				halbtc8192e2ant_dec_btpwr(btcoexist,
+							  NORMAL_EXEC, 0);
+				halbtc_sw_mechanism1(btcoexist, false,
+						     false, false,
+						     false);
+				halbtc_sw_mechanism2(btcoexist, false,
+						     false, false,
+						     0x18);
+				common = true;
+			}
+		}
+	}
+	return common;
+}
+
+static void halbtc8192e2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
+						 bool sco_hid, bool tx_pause,
+						 u8 max_interval)
+{
+	static int up, dn, m, n, wait_cnt;
+	/* 0: no change, +1: increase WiFi duration,
+	 * -1: decrease WiFi duration */
+	int result;
+	u8 retry_cnt = 0;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], TdmaDurationAdjust()\n");
+
+	if (!coex_dm->auto_tdma_adjust) {
+		coex_dm->auto_tdma_adjust = true;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], first run TdmaDurationAdjust()!!\n");
+		if (sco_hid) {
+			if (tx_pause) {
+				if (max_interval == 1) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 13);
+					coex_dm->ps_tdma_du_adj_type = 13;
+				} else if (max_interval == 2) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 14);
+					coex_dm->ps_tdma_du_adj_type = 14;
+				} else if (max_interval == 3) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 15);
+					coex_dm->ps_tdma_du_adj_type = 15;
+				} else {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 15);
+					coex_dm->ps_tdma_du_adj_type = 15;
+				}
+			} else {
+				if (max_interval == 1) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 9);
+					coex_dm->ps_tdma_du_adj_type = 9;
+				} else if (max_interval == 2) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 10);
+					coex_dm->ps_tdma_du_adj_type = 10;
+				} else if (max_interval == 3) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 11);
+					coex_dm->ps_tdma_du_adj_type = 11;
+				} else {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 11);
+					coex_dm->ps_tdma_du_adj_type = 11;
+				}
+			}
+		} else {
+			if (tx_pause) {
+				if (max_interval == 1) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 5);
+					coex_dm->ps_tdma_du_adj_type = 5;
+				} else if (max_interval == 2) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 6);
+					coex_dm->ps_tdma_du_adj_type = 6;
+				} else if (max_interval == 3) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 7);
+					coex_dm->ps_tdma_du_adj_type = 7;
+				} else {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 7);
+					coex_dm->ps_tdma_du_adj_type = 7;
+				}
+			} else {
+				if (max_interval == 1) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 1);
+					coex_dm->ps_tdma_du_adj_type = 1;
+				} else if (max_interval == 2) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 2);
+					coex_dm->ps_tdma_du_adj_type = 2;
+				} else if (max_interval == 3) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 3);
+					coex_dm->ps_tdma_du_adj_type = 3;
+				} else {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 3);
+					coex_dm->ps_tdma_du_adj_type = 3;
+				}
+			}
+		}
+
+		up = 0;
+		dn = 0;
+		m = 1;
+		n = 3;
+		result = 0;
+		wait_cnt = 0;
+	} else {
+		/* accquire the BT TRx retry count from BT_Info byte2 */
+		retry_cnt = coex_sta->bt_retry_cnt;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], retry_cnt = %d\n", retry_cnt);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], up =%d, dn =%d, m =%d, n =%d, wait_cnt =%d\n",
+			  up, dn, m, n, wait_cnt);
+		result = 0;
+		wait_cnt++;
+		/* no retry in the last 2-second duration */
+		if (retry_cnt == 0) {
+			up++;
+			dn--;
+
+			if (dn <= 0)
+				dn = 0;
+
+			if (up >= n) {
+				wait_cnt = 0;
+				n = 3;
+				up = 0;
+				dn = 0;
+				result = 1;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_TRACE_FW_DETAIL,
+					  "[BTCoex]Increase wifi duration!!\n");
+			}
+		} else if (retry_cnt <= 3) {
+			up--;
+			dn++;
+
+			if (up <= 0)
+				up = 0;
+
+			if (dn == 2) {
+				if (wait_cnt <= 2)
+					m++;
+				else
+					m = 1;
+
+				if (m >= 20)
+					m = 20;
+
+				n = 3 * m;
+				up = 0;
+				dn = 0;
+				wait_cnt = 0;
+				result = -1;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_TRACE_FW_DETAIL,
+					  "Reduce wifi duration for retry<3\n");
+			}
+		} else {
+			if (wait_cnt == 1)
+				m++;
+			else
+				m = 1;
+
+			if (m >= 20)
+				m = 20;
+
+			n = 3*m;
+			up = 0;
+			dn = 0;
+			wait_cnt = 0;
+			result = -1;
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+				  "Decrease wifi duration for retryCounter>3!!\n");
+		}
+
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], max Interval = %d\n", max_interval);
+		if (max_interval == 1) {
+			if (tx_pause) {
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_TRACE_FW_DETAIL,
+					  "[BTCoex], TxPause = 1\n");
+
+				if (coex_dm->cur_ps_tdma == 71) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 5);
+					coex_dm->ps_tdma_du_adj_type = 5;
+				} else if (coex_dm->cur_ps_tdma == 1) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 5);
+					coex_dm->ps_tdma_du_adj_type = 5;
+				} else if (coex_dm->cur_ps_tdma == 2) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 6);
+					coex_dm->ps_tdma_du_adj_type = 6;
+				} else if (coex_dm->cur_ps_tdma == 3) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 7);
+					coex_dm->ps_tdma_du_adj_type = 7;
+				} else if (coex_dm->cur_ps_tdma == 4) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 8);
+					coex_dm->ps_tdma_du_adj_type = 8;
+				}
+				if (coex_dm->cur_ps_tdma == 9) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 13);
+					coex_dm->ps_tdma_du_adj_type = 13;
+				} else if (coex_dm->cur_ps_tdma == 10) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 14);
+					coex_dm->ps_tdma_du_adj_type = 14;
+				} else if (coex_dm->cur_ps_tdma == 11) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 15);
+					coex_dm->ps_tdma_du_adj_type = 15;
+				} else if (coex_dm->cur_ps_tdma == 12) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 16);
+					coex_dm->ps_tdma_du_adj_type = 16;
+				}
+
+				if (result == -1) {
+					if (coex_dm->cur_ps_tdma == 5) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 6);
+						coex_dm->ps_tdma_du_adj_type =
+									     6;
+					} else if (coex_dm->cur_ps_tdma == 6) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 7);
+						coex_dm->ps_tdma_du_adj_type =
+									     7;
+					} else if (coex_dm->cur_ps_tdma == 7) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 8);
+						coex_dm->ps_tdma_du_adj_type =
+									     8;
+					} else if (coex_dm->cur_ps_tdma == 13) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 14);
+						coex_dm->ps_tdma_du_adj_type =
+									     14;
+					} else if (coex_dm->cur_ps_tdma == 14) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 15);
+						coex_dm->ps_tdma_du_adj_type =
+									     15;
+					} else if (coex_dm->cur_ps_tdma == 15) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 16);
+						coex_dm->ps_tdma_du_adj_type =
+									     16;
+					}
+				} else if (result == 1) {
+					if (coex_dm->cur_ps_tdma == 8) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 7);
+						coex_dm->ps_tdma_du_adj_type =
+									     7;
+					} else if (coex_dm->cur_ps_tdma == 7) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 6);
+						coex_dm->ps_tdma_du_adj_type =
+									     6;
+					} else if (coex_dm->cur_ps_tdma == 6) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 5);
+						coex_dm->ps_tdma_du_adj_type =
+									     5;
+					} else if (coex_dm->cur_ps_tdma == 16) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 15);
+						coex_dm->ps_tdma_du_adj_type =
+									     15;
+					} else if (coex_dm->cur_ps_tdma == 15) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 14);
+						coex_dm->ps_tdma_du_adj_type =
+									     14;
+					} else if (coex_dm->cur_ps_tdma == 14) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 13);
+						coex_dm->ps_tdma_du_adj_type =
+									     13;
+					}
+				}
+			} else {
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_TRACE_FW_DETAIL,
+					  "[BTCoex], TxPause = 0\n");
+				if (coex_dm->cur_ps_tdma == 5) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 71);
+					coex_dm->ps_tdma_du_adj_type = 71;
+				} else if (coex_dm->cur_ps_tdma == 6) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 2);
+					coex_dm->ps_tdma_du_adj_type = 2;
+				} else if (coex_dm->cur_ps_tdma == 7) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 3);
+					coex_dm->ps_tdma_du_adj_type = 3;
+				} else if (coex_dm->cur_ps_tdma == 8) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 4);
+					coex_dm->ps_tdma_du_adj_type = 4;
+				}
+				if (coex_dm->cur_ps_tdma == 13) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 9);
+					coex_dm->ps_tdma_du_adj_type = 9;
+				} else if (coex_dm->cur_ps_tdma == 14) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 10);
+					coex_dm->ps_tdma_du_adj_type = 10;
+				} else if (coex_dm->cur_ps_tdma == 15) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 11);
+					coex_dm->ps_tdma_du_adj_type = 11;
+				} else if (coex_dm->cur_ps_tdma == 16) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 12);
+					coex_dm->ps_tdma_du_adj_type = 12;
+				}
+
+				if (result == -1) {
+					if (coex_dm->cur_ps_tdma == 71) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 1);
+						coex_dm->ps_tdma_du_adj_type =
+									     1;
+					} else if (coex_dm->cur_ps_tdma == 1) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 2);
+						coex_dm->ps_tdma_du_adj_type =
+									     2;
+					} else if (coex_dm->cur_ps_tdma == 2) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 3);
+						coex_dm->ps_tdma_du_adj_type =
+									     3;
+					} else if (coex_dm->cur_ps_tdma == 3) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 4);
+						coex_dm->ps_tdma_du_adj_type =
+									     4;
+					} else if (coex_dm->cur_ps_tdma == 9) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 10);
+						coex_dm->ps_tdma_du_adj_type =
+									     10;
+					} else if (coex_dm->cur_ps_tdma == 10) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 11);
+						coex_dm->ps_tdma_du_adj_type =
+									     11;
+					} else if (coex_dm->cur_ps_tdma == 11) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 12);
+						coex_dm->ps_tdma_du_adj_type =
+									     12;
+					}
+				} else if (result == 1) {
+					if (coex_dm->cur_ps_tdma == 4) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 3);
+						coex_dm->ps_tdma_du_adj_type =
+									     3;
+					} else if (coex_dm->cur_ps_tdma == 3) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 2);
+						coex_dm->ps_tdma_du_adj_type =
+									     2;
+					} else if (coex_dm->cur_ps_tdma == 2) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 1);
+						coex_dm->ps_tdma_du_adj_type =
+									     1;
+					} else if (coex_dm->cur_ps_tdma == 1) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 71);
+						coex_dm->ps_tdma_du_adj_type =
+									     71;
+					} else if (coex_dm->cur_ps_tdma == 12) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 11);
+						coex_dm->ps_tdma_du_adj_type =
+									     11;
+					} else if (coex_dm->cur_ps_tdma == 11) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 10);
+						coex_dm->ps_tdma_du_adj_type =
+									     10;
+					} else if (coex_dm->cur_ps_tdma == 10) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 9);
+						coex_dm->ps_tdma_du_adj_type =
+									     9;
+					}
+				}
+			}
+		} else if (max_interval == 2) {
+			if (tx_pause) {
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_TRACE_FW_DETAIL,
+					  "[BTCoex], TxPause = 1\n");
+				if (coex_dm->cur_ps_tdma == 1) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 6);
+					coex_dm->ps_tdma_du_adj_type = 6;
+				} else if (coex_dm->cur_ps_tdma == 2) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 6);
+					coex_dm->ps_tdma_du_adj_type = 6;
+				} else if (coex_dm->cur_ps_tdma == 3) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 7);
+					coex_dm->ps_tdma_du_adj_type = 7;
+				} else if (coex_dm->cur_ps_tdma == 4) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 8);
+					coex_dm->ps_tdma_du_adj_type = 8;
+				}
+				if (coex_dm->cur_ps_tdma == 9) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 14);
+					coex_dm->ps_tdma_du_adj_type = 14;
+				} else if (coex_dm->cur_ps_tdma == 10) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 14);
+					coex_dm->ps_tdma_du_adj_type = 14;
+				} else if (coex_dm->cur_ps_tdma == 11) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 15);
+					coex_dm->ps_tdma_du_adj_type = 15;
+				} else if (coex_dm->cur_ps_tdma == 12) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 16);
+					coex_dm->ps_tdma_du_adj_type = 16;
+				}
+				if (result == -1) {
+					if (coex_dm->cur_ps_tdma == 5) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 6);
+						coex_dm->ps_tdma_du_adj_type =
+									     6;
+					} else if (coex_dm->cur_ps_tdma == 6) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 7);
+						coex_dm->ps_tdma_du_adj_type =
+									     7;
+					} else if (coex_dm->cur_ps_tdma == 7) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 8);
+						coex_dm->ps_tdma_du_adj_type =
+									     8;
+					} else if (coex_dm->cur_ps_tdma == 13) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 14);
+						coex_dm->ps_tdma_du_adj_type =
+									     14;
+					} else if (coex_dm->cur_ps_tdma == 14) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 15);
+						coex_dm->ps_tdma_du_adj_type =
+									     15;
+					} else if (coex_dm->cur_ps_tdma == 15) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 16);
+						coex_dm->ps_tdma_du_adj_type =
+									     16;
+					}
+				} else if (result == 1) {
+					if (coex_dm->cur_ps_tdma == 8) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 7);
+						coex_dm->ps_tdma_du_adj_type =
+									     7;
+					} else if (coex_dm->cur_ps_tdma == 7) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 6);
+						coex_dm->ps_tdma_du_adj_type =
+									     6;
+					} else if (coex_dm->cur_ps_tdma == 6) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 6);
+						coex_dm->ps_tdma_du_adj_type =
+									     6;
+					} else if (coex_dm->cur_ps_tdma == 16) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 15);
+						coex_dm->ps_tdma_du_adj_type =
+									     15;
+					} else if (coex_dm->cur_ps_tdma == 15) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 14);
+						coex_dm->ps_tdma_du_adj_type =
+									     14;
+					} else if (coex_dm->cur_ps_tdma == 14) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 14);
+						coex_dm->ps_tdma_du_adj_type =
+									     14;
+					}
+				}
+			} else {
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_TRACE_FW_DETAIL,
+					  "[BTCoex], TxPause = 0\n");
+				if (coex_dm->cur_ps_tdma == 5) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 2);
+					coex_dm->ps_tdma_du_adj_type = 2;
+				} else if (coex_dm->cur_ps_tdma == 6) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 2);
+					coex_dm->ps_tdma_du_adj_type = 2;
+				} else if (coex_dm->cur_ps_tdma == 7) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 3);
+					coex_dm->ps_tdma_du_adj_type = 3;
+				} else if (coex_dm->cur_ps_tdma == 8) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 4);
+					coex_dm->ps_tdma_du_adj_type = 4;
+				}
+				if (coex_dm->cur_ps_tdma == 13) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 10);
+					coex_dm->ps_tdma_du_adj_type = 10;
+				} else if (coex_dm->cur_ps_tdma == 14) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 10);
+					coex_dm->ps_tdma_du_adj_type = 10;
+				} else if (coex_dm->cur_ps_tdma == 15) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 11);
+					coex_dm->ps_tdma_du_adj_type = 11;
+				} else if (coex_dm->cur_ps_tdma == 16) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 12);
+					coex_dm->ps_tdma_du_adj_type = 12;
+				}
+				if (result == -1) {
+					if (coex_dm->cur_ps_tdma == 1) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 2);
+						coex_dm->ps_tdma_du_adj_type =
+									     2;
+					} else if (coex_dm->cur_ps_tdma == 2) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 3);
+						coex_dm->ps_tdma_du_adj_type =
+									     3;
+					} else if (coex_dm->cur_ps_tdma == 3) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 4);
+						coex_dm->ps_tdma_du_adj_type =
+									     4;
+					} else if (coex_dm->cur_ps_tdma == 9) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 10);
+						coex_dm->ps_tdma_du_adj_type =
+									     10;
+					} else if (coex_dm->cur_ps_tdma == 10) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 11);
+						coex_dm->ps_tdma_du_adj_type =
+									     11;
+					} else if (coex_dm->cur_ps_tdma == 11) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 12);
+						coex_dm->ps_tdma_du_adj_type =
+									     12;
+					}
+				} else if (result == 1) {
+					if (coex_dm->cur_ps_tdma == 4) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 3);
+						coex_dm->ps_tdma_du_adj_type =
+									     3;
+					} else if (coex_dm->cur_ps_tdma == 3) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 2);
+						coex_dm->ps_tdma_du_adj_type =
+									     2;
+					} else if (coex_dm->cur_ps_tdma == 2) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 2);
+						coex_dm->ps_tdma_du_adj_type =
+									     2;
+					} else if (coex_dm->cur_ps_tdma == 12) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 11);
+						coex_dm->ps_tdma_du_adj_type =
+									     11;
+					} else if (coex_dm->cur_ps_tdma == 11) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 10);
+						coex_dm->ps_tdma_du_adj_type =
+									     10;
+					} else if (coex_dm->cur_ps_tdma == 10) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 10);
+						coex_dm->ps_tdma_du_adj_type =
+									     10;
+					}
+				}
+			}
+		} else if (max_interval == 3) {
+			if (tx_pause) {
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_TRACE_FW_DETAIL,
+					  "[BTCoex], TxPause = 1\n");
+				if (coex_dm->cur_ps_tdma == 1) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 7);
+					coex_dm->ps_tdma_du_adj_type = 7;
+				} else if (coex_dm->cur_ps_tdma == 2) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 7);
+					coex_dm->ps_tdma_du_adj_type = 7;
+				} else if (coex_dm->cur_ps_tdma == 3) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 7);
+					coex_dm->ps_tdma_du_adj_type = 7;
+				} else if (coex_dm->cur_ps_tdma == 4) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 8);
+					coex_dm->ps_tdma_du_adj_type = 8;
+				}
+				if (coex_dm->cur_ps_tdma == 9) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 15);
+					coex_dm->ps_tdma_du_adj_type = 15;
+				} else if (coex_dm->cur_ps_tdma == 10) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 15);
+					coex_dm->ps_tdma_du_adj_type = 15;
+				} else if (coex_dm->cur_ps_tdma == 11) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 15);
+					coex_dm->ps_tdma_du_adj_type = 15;
+				} else if (coex_dm->cur_ps_tdma == 12) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 16);
+					coex_dm->ps_tdma_du_adj_type = 16;
+				}
+				if (result == -1) {
+					if (coex_dm->cur_ps_tdma == 5) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 7);
+						coex_dm->ps_tdma_du_adj_type =
+									     7;
+					} else if (coex_dm->cur_ps_tdma == 6) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 7);
+						coex_dm->ps_tdma_du_adj_type =
+									     7;
+					} else if (coex_dm->cur_ps_tdma == 7) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 8);
+						coex_dm->ps_tdma_du_adj_type =
+									     8;
+					} else if (coex_dm->cur_ps_tdma == 13) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 15);
+						coex_dm->ps_tdma_du_adj_type =
+									     15;
+					} else if (coex_dm->cur_ps_tdma == 14) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 15);
+						coex_dm->ps_tdma_du_adj_type =
+									     15;
+					} else if (coex_dm->cur_ps_tdma == 15) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 16);
+						coex_dm->ps_tdma_du_adj_type =
+									     16;
+					}
+				} else if (result == 1) {
+					if (coex_dm->cur_ps_tdma == 8) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 7);
+						coex_dm->ps_tdma_du_adj_type =
+									     7;
+					} else if (coex_dm->cur_ps_tdma == 7) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 7);
+						coex_dm->ps_tdma_du_adj_type =
+									     7;
+					} else if (coex_dm->cur_ps_tdma == 6) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 7);
+						coex_dm->ps_tdma_du_adj_type =
+									     7;
+					} else if (coex_dm->cur_ps_tdma == 16) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 15);
+						coex_dm->ps_tdma_du_adj_type =
+									     15;
+					} else if (coex_dm->cur_ps_tdma == 15) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 15);
+						coex_dm->ps_tdma_du_adj_type =
+									     15;
+					} else if (coex_dm->cur_ps_tdma == 14) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 15);
+						coex_dm->ps_tdma_du_adj_type =
+									     15;
+					}
+				}
+			} else {
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_TRACE_FW_DETAIL,
+					  "[BTCoex], TxPause = 0\n");
+				if (coex_dm->cur_ps_tdma == 5) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 3);
+					coex_dm->ps_tdma_du_adj_type = 3;
+				} else if (coex_dm->cur_ps_tdma == 6) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 3);
+					coex_dm->ps_tdma_du_adj_type = 3;
+				} else if (coex_dm->cur_ps_tdma == 7) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 3);
+					coex_dm->ps_tdma_du_adj_type = 3;
+				} else if (coex_dm->cur_ps_tdma == 8) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 4);
+					coex_dm->ps_tdma_du_adj_type = 4;
+				}
+				if (coex_dm->cur_ps_tdma == 13) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 11);
+					coex_dm->ps_tdma_du_adj_type = 11;
+				} else if (coex_dm->cur_ps_tdma == 14) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 11);
+					coex_dm->ps_tdma_du_adj_type = 11;
+				} else if (coex_dm->cur_ps_tdma == 15) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 11);
+					coex_dm->ps_tdma_du_adj_type = 11;
+				} else if (coex_dm->cur_ps_tdma == 16) {
+					halbtc8192e2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 12);
+					coex_dm->ps_tdma_du_adj_type = 12;
+				}
+				if (result == -1) {
+					if (coex_dm->cur_ps_tdma == 1) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 3);
+						coex_dm->ps_tdma_du_adj_type =
+									     3;
+					} else if (coex_dm->cur_ps_tdma == 2) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 3);
+						coex_dm->ps_tdma_du_adj_type =
+									     3;
+					} else if (coex_dm->cur_ps_tdma == 3) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 4);
+						coex_dm->ps_tdma_du_adj_type =
+									     4;
+					} else if (coex_dm->cur_ps_tdma == 9) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 11);
+						coex_dm->ps_tdma_du_adj_type =
+									     11;
+					} else if (coex_dm->cur_ps_tdma == 10) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 11);
+						coex_dm->ps_tdma_du_adj_type =
+									     11;
+					} else if (coex_dm->cur_ps_tdma == 11) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 12);
+						coex_dm->ps_tdma_du_adj_type =
+									     12;
+					}
+				} else if (result == 1) {
+					if (coex_dm->cur_ps_tdma == 4) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 3);
+						coex_dm->ps_tdma_du_adj_type =
+									     3;
+					} else if (coex_dm->cur_ps_tdma == 3) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 3);
+						coex_dm->ps_tdma_du_adj_type =
+									     3;
+					} else if (coex_dm->cur_ps_tdma == 2) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 3);
+						coex_dm->ps_tdma_du_adj_type =
+									     3;
+					} else if (coex_dm->cur_ps_tdma == 12) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 11);
+						coex_dm->ps_tdma_du_adj_type =
+									     11;
+					} else if (coex_dm->cur_ps_tdma == 11) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 11);
+						coex_dm->ps_tdma_du_adj_type =
+									     11;
+					} else if (coex_dm->cur_ps_tdma == 10) {
+						halbtc8192e2ant_ps_tdma(
+								    btcoexist,
+								    NORMAL_EXEC,
+								    true, 11);
+						coex_dm->ps_tdma_du_adj_type =
+									     11;
+					}
+				}
+			}
+		}
+	}
+
+	/* if current PsTdma not match with
+	 * the recorded one (when scan, dhcp...),
+	 * then we have to adjust it back to the previous record one. */
+	if (coex_dm->cur_ps_tdma != coex_dm->ps_tdma_du_adj_type) {
+		bool scan = false, link = false, roam = false;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], PsTdma type dismatch!!!, ");
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "curPsTdma =%d, recordPsTdma =%d\n",
+			  coex_dm->cur_ps_tdma, coex_dm->ps_tdma_du_adj_type);
+
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
+
+		if (!scan && !link && !roam)
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true,
+						coex_dm->ps_tdma_du_adj_type);
+		else
+			BTC_PRINT(BTC_MSG_ALGORITHM,
+				  ALGO_TRACE_FW_DETAIL,
+				  "[BTCoex], roaming/link/scan is under progress, will adjust next time!!!\n");
+	}
+}
+
+/* SCO only or SCO+PAN(HS) */
+static void halbtc8192e2ant_action_sco(struct btc_coexist *btcoexist)
+{
+	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_STAY_LOW;
+	u32 wifi_bw;
+
+	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
+
+	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
+	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
+
+	halbtc_coex_table_with_type(btcoexist, NORMAL_EXEC, 4);
+
+	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
+
+	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
+	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 13);
+	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
+	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
+	}
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	/* sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc_sw_mechanism1(btcoexist, true, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x6);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, true, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, false, false,
+					     false, 0x6);
+		}
+	} else {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc_sw_mechanism1(btcoexist, false, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x6);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, false, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, false, false,
+					     false, 0x6);
+		}
+	}
+}
+
+static void halbtc8192e2ant_action_sco_pan(struct btc_coexist *btcoexist)
+{
+	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_STAY_LOW;
+	u32 wifi_bw;
+
+	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
+
+	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
+	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
+
+	halbtc_coex_table_with_type(btcoexist, NORMAL_EXEC, 4);
+
+	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
+
+	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
+	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 14);
+	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 10);
+	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 10);
+	}
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	/* sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc_sw_mechanism1(btcoexist, true, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x6);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, true, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, false, false,
+					     false, 0x6);
+		}
+	} else {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc_sw_mechanism1(btcoexist, false, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x6);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, false, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, false, false,
+					     false, 0x6);
+		}
+	}
+}
+
+static void halbtc8192e2ant_action_hid(struct btc_coexist *btcoexist)
+{
+	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
+	u32 wifi_bw;
+
+	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
+	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
+
+	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
+	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	halbtc_coex_table_with_type(btcoexist, NORMAL_EXEC, 3);
+
+	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
+	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 13);
+	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
+	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
+	}
+
+	/* sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc_sw_mechanism1(btcoexist, true, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, true, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, false, false,
+					     false, 0x18);
+		}
+	} else {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc_sw_mechanism1(btcoexist, false, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, false, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, false, false,
+					     false, 0x18);
+		}
+	}
+}
+
+/* A2DP only / PAN(EDR) only/ A2DP+PAN(HS) */
+static void halbtc8192e2ant_action_a2dp(struct btc_coexist *btcoexist)
+{
+	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
+	u32 wifi_bw;
+	bool long_dist = false;
+
+	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
+	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
+
+	if ((btrssi_state == BTC_RSSI_STATE_LOW ||
+	     btrssi_state == BTC_RSSI_STATE_STAY_LOW) &&
+	    (wifirssi_state == BTC_RSSI_STATE_LOW ||
+	     wifirssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], A2dp, wifi/bt rssi both LOW!!\n");
+		long_dist = true;
+	}
+	if (long_dist) {
+		halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 2);
+		halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, true,
+					   0x4);
+	} else {
+		halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
+		halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false,
+					   0x8);
+	}
+
+	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (long_dist)
+		halbtc_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
+	else
+		halbtc_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+
+	if (long_dist) {
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 17);
+		coex_dm->auto_tdma_adjust = false;
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+	} else {
+		if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
+		    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+			halbtc8192e2ant_tdma_duration_adjust(btcoexist, false,
+							     true, 1);
+			halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+		} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
+			   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
+			halbtc8192e2ant_tdma_duration_adjust(btcoexist, false,
+							     false, 1);
+			halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
+		} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
+			   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8192e2ant_tdma_duration_adjust(btcoexist, false,
+							     false, 1);
+			halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
+		}
+	}
+
+	/* sw mechanism */
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc_sw_mechanism1(btcoexist, true, false,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, true, false,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, false, false,
+					     false, 0x18);
+		}
+	} else {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc_sw_mechanism1(btcoexist, false, false,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, false, false,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, false, false,
+					     false, 0x18);
+		}
+	}
+}
+
+static void halbtc8192e2ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
+{
+	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
+	u32 wifi_bw;
+
+	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
+	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
+
+	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
+	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
+	halbtc_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+
+	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
+	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+		halbtc8192e2ant_tdma_duration_adjust(btcoexist, false, true, 2);
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
+		halbtc8192e2ant_tdma_duration_adjust(btcoexist, false, false,
+						     2);
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
+	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+		halbtc8192e2ant_tdma_duration_adjust(btcoexist, false, false,
+						     2);
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
+	}
+
+	/* sw mechanism */
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc_sw_mechanism1(btcoexist, true, false,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     true, 0x6);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, true, false,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, false, false,
+					     true, 0x6);
+		}
+	} else {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc_sw_mechanism1(btcoexist, false, false,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     true, 0x6);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, false, false,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, false, false,
+					     true, 0x6);
+		}
+	}
+}
+
+static void halbtc8192e2ant_action_pan_edr(struct btc_coexist *btcoexist)
+{
+	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
+	u32 wifi_bw;
+
+	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
+	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
+
+	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
+	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
+
+	halbtc_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+
+	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
+	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
+	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 1);
+	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 1);
+	}
+
+	/* sw mechanism */
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc_sw_mechanism1(btcoexist, true, false,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, true, false,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, false, false,
+					     false, 0x18);
+		}
+	} else {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc_sw_mechanism1(btcoexist, false, false,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, false, false,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, false, false,
+					     false, 0x18);
+		}
+	}
+}
+
+/* PAN(HS) only */
+static void halbtc8192e2ant_action_pan_hs(struct btc_coexist *btcoexist)
+{
+	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
+	u32 wifi_bw;
+
+	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
+	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
+
+	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
+	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
+
+	halbtc_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+
+	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
+	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
+	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
+	}
+	halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc_sw_mechanism1(btcoexist, true, false,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, true, false,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, false, false,
+					     false, 0x18);
+		}
+	} else {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc_sw_mechanism1(btcoexist, false, false,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, false, false,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, false, false,
+					     false, 0x18);
+		}
+	}
+}
+
+/* PAN(EDR)+A2DP */
+static void halbtc8192e2ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
+{
+	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
+	u32 wifi_bw;
+
+	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
+	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
+
+	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
+	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
+
+	halbtc_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
+	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+		halbtc8192e2ant_tdma_duration_adjust(btcoexist, false, true, 3);
+	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
+		halbtc8192e2ant_tdma_duration_adjust(btcoexist, false, false,
+						     3);
+	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
+		halbtc8192e2ant_tdma_duration_adjust(btcoexist, false, false,
+						     3);
+	}
+
+	/* sw mechanism	*/
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc_sw_mechanism1(btcoexist, true, false,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, true, false,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, false, false,
+					     false, 0x18);
+		}
+	} else {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc_sw_mechanism1(btcoexist, false, false,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, false, false,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, false, false,
+					     false, 0x18);
+		}
+	}
+}
+
+static void halbtc8192e2ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
+{
+	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
+	u32 wifi_bw;
+
+	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
+	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
+	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
+
+	halbtc_coex_table_with_type(btcoexist, NORMAL_EXEC, 3);
+
+	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
+	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 14);
+	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 10);
+	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 10);
+	}
+
+	/* sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc_sw_mechanism1(btcoexist, true, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, true, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, false, false,
+					     false, 0x18);
+		}
+	} else {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc_sw_mechanism1(btcoexist, false, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, false, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, false, false,
+					     false, 0x18);
+		}
+	}
+}
+
+/* HID+A2DP+PAN(EDR) */
+static void action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
+{
+	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
+	u32 wifi_bw;
+
+	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
+	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
+
+	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
+	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
+
+	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	halbtc_coex_table_with_type(btcoexist, NORMAL_EXEC, 3);
+
+	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
+	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+		halbtc8192e2ant_tdma_duration_adjust(btcoexist, true, true, 3);
+	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
+		halbtc8192e2ant_tdma_duration_adjust(btcoexist, true, false, 3);
+	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
+		halbtc8192e2ant_tdma_duration_adjust(btcoexist, true, false, 3);
+	}
+
+	/* sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc_sw_mechanism1(btcoexist, true, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, true, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, false, false,
+					     false, 0x18);
+		}
+	} else {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc_sw_mechanism1(btcoexist, false, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, false, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, false, false,
+					     false, 0x18);
+		}
+	}
+}
+
+static void halbtc8192e2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
+{
+	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
+	u32 wifi_bw;
+
+	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
+	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
+
+	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
+	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	halbtc_coex_table_with_type(btcoexist, NORMAL_EXEC, 3);
+
+	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
+	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
+		halbtc8192e2ant_tdma_duration_adjust(btcoexist, true, true, 2);
+	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM))	{
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
+		halbtc8192e2ant_tdma_duration_adjust(btcoexist, true, false, 2);
+	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
+		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
+		halbtc8192e2ant_tdma_duration_adjust(btcoexist, true, false, 2);
+	}
+
+	/* sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc_sw_mechanism1(btcoexist, true, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, true, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, false, false,
+					     false, 0x18);
+		}
+	} else {
+		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc_sw_mechanism1(btcoexist, false, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, true, false,
+					     false, 0x18);
+		} else {
+			halbtc_sw_mechanism1(btcoexist, false, true,
+					     false, false);
+			halbtc_sw_mechanism2(btcoexist, false, false,
+					     false, 0x18);
+		}
+	}
+}
+
+static void halbtc8192e2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
+{
+	u8 algorithm = 0;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+		  "[BTCoex], RunCoexistMechanism() ===>\n");
+
+	if (btcoexist->manual_control) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], return for Manual CTRL <===\n");
+		return;
+	}
+
+	if (coex_sta->under_ips) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], wifi is under IPS !!!\n");
+		return;
+	}
+
+	algorithm = halbtc8192e2ant_action_algorithm(btcoexist);
+	if (coex_sta->c2h_bt_inquiry_page &&
+	    (BT_8192E_2ANT_COEX_ALGO_PANHS != algorithm)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BT is under inquiry/page scan !!\n");
+		halbtc8192e2ant_action_bt_inquiry(btcoexist);
+		return;
+	}
+
+	coex_dm->cur_algorithm = algorithm;
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+		  "[BTCoex], Algorithm = %d\n", coex_dm->cur_algorithm);
+
+	if (halbtc8192e2ant_is_common_action(btcoexist)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Action 2-Ant common.\n");
+		coex_dm->auto_tdma_adjust = false;
+	} else {
+		if (coex_dm->cur_algorithm != coex_dm->pre_algorithm) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex] preAlgorithm =%d, curAlgorithm =%d\n",
+				  coex_dm->pre_algorithm,
+				  coex_dm->cur_algorithm);
+			coex_dm->auto_tdma_adjust = false;
+		}
+		switch (coex_dm->cur_algorithm) {
+		case BT_8192E_2ANT_COEX_ALGO_SCO:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "Action 2-Ant, algorithm = SCO.\n");
+			halbtc8192e2ant_action_sco(btcoexist);
+			break;
+		case BT_8192E_2ANT_COEX_ALGO_SCO_PAN:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "Action 2-Ant, algorithm = SCO+PAN(EDR).\n");
+			halbtc8192e2ant_action_sco_pan(btcoexist);
+			break;
+		case BT_8192E_2ANT_COEX_ALGO_HID:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "Action 2-Ant, algorithm = HID.\n");
+			halbtc8192e2ant_action_hid(btcoexist);
+			break;
+		case BT_8192E_2ANT_COEX_ALGO_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "Action 2-Ant, algorithm = A2DP.\n");
+			halbtc8192e2ant_action_a2dp(btcoexist);
+			break;
+		case BT_8192E_2ANT_COEX_ALGO_A2DP_PANHS:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "Action 2-Ant, algorithm = A2DP+PAN(HS).\n");
+			halbtc8192e2ant_action_a2dp_pan_hs(btcoexist);
+			break;
+		case BT_8192E_2ANT_COEX_ALGO_PANEDR:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "Action 2-Ant, algorithm = PAN(EDR).\n");
+			halbtc8192e2ant_action_pan_edr(btcoexist);
+			break;
+		case BT_8192E_2ANT_COEX_ALGO_PANHS:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "Action 2-Ant, algorithm = HS mode.\n");
+			halbtc8192e2ant_action_pan_hs(btcoexist);
+			break;
+		case BT_8192E_2ANT_COEX_ALGO_PANEDR_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "Action 2-Ant, algorithm = PAN+A2DP.\n");
+			halbtc8192e2ant_action_pan_edr_a2dp(btcoexist);
+			break;
+		case BT_8192E_2ANT_COEX_ALGO_PANEDR_HID:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "Action 2-Ant, algorithm = PAN(EDR)+HID.\n");
+			halbtc8192e2ant_action_pan_edr_hid(btcoexist);
+			break;
+		case BT_8192E_2ANT_COEX_ALGO_HID_A2DP_PANEDR:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "Action 2-Ant, algorithm = HID+A2DP+PAN.\n");
+			action_hid_a2dp_pan_edr(btcoexist);
+			break;
+		case BT_8192E_2ANT_COEX_ALGO_HID_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "Action 2-Ant, algorithm = HID+A2DP.\n");
+			halbtc8192e2ant_action_hid_a2dp(btcoexist);
+			break;
+		default:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "Action 2-Ant, algorithm = unknown!!\n");
+			/* halbtc8192e2ant_coex_alloff(btcoexist); */
+			break;
+		}
+		coex_dm->pre_algorithm = coex_dm->cur_algorithm;
+	}
+}
+
+static void halbtc8192e2ant_init_hwconfig(struct btc_coexist *btcoexist,
+					  bool backup)
+{
+	u16 u16tmp = 0;
+	u8 u8tmp = 0;
+
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+		  "[BTCoex], 2Ant Init HW Config!!\n");
+
+	if (backup) {
+		/* backup rf 0x1e value */
+		coex_dm->bt_rf0x1e_backup =
+			btcoexist->btc_get_rf_reg(btcoexist, BTC_RF_A,
+						  0x1e, 0xfffff);
+
+		coex_dm->backup_arfr_cnt1 = btcoexist->btc_read_4byte(btcoexist,
+								      0x430);
+		coex_dm->backup_arfr_cnt2 = btcoexist->btc_read_4byte(btcoexist,
+								     0x434);
+		coex_dm->backup_retrylimit = btcoexist->btc_read_2byte(
+								    btcoexist,
+								    0x42a);
+		coex_dm->backup_ampdu_maxtime = btcoexist->btc_read_1byte(
+								    btcoexist,
+								    0x456);
+	}
+
+	/* antenna sw ctrl to bt */
+	btcoexist->btc_write_1byte(btcoexist, 0x4f, 0x6);
+	btcoexist->btc_write_1byte(btcoexist, 0x944, 0x24);
+	btcoexist->btc_write_4byte(btcoexist, 0x930, 0x700700);
+	btcoexist->btc_write_1byte(btcoexist, 0x92c, 0x20);
+	if (btcoexist->chip_interface == BTC_INTF_USB)
+		btcoexist->btc_write_4byte(btcoexist, 0x64, 0x30430004);
+	else
+		btcoexist->btc_write_4byte(btcoexist, 0x64, 0x30030004);
+
+	halbtc_coex_table_with_type(btcoexist, FORCE_EXEC, 0);
+
+	/* antenna switch control parameter */
+	btcoexist->btc_write_4byte(btcoexist, 0x858, 0x55555555);
+
+	/* coex parameters */
+	btcoexist->btc_write_1byte(btcoexist, 0x778, 0x3);
+	/* 0x790[5:0] = 0x5 */
+	u8tmp = btcoexist->btc_read_1byte(btcoexist, 0x790);
+	u8tmp &= 0xc0;
+	u8tmp |= 0x5;
+	btcoexist->btc_write_1byte(btcoexist, 0x790, u8tmp);
+
+	/* enable counter statistics */
+	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0x4);
+
+	/* enable PTA */
+	btcoexist->btc_write_1byte(btcoexist, 0x40, 0x20);
+	/* enable mailbox interface */
+	u16tmp = btcoexist->btc_read_2byte(btcoexist, 0x40);
+	u16tmp |= BIT(9);
+	btcoexist->btc_write_2byte(btcoexist, 0x40, u16tmp);
+
+	/* enable PTA I2C mailbox  */
+	u8tmp = btcoexist->btc_read_1byte(btcoexist, 0x101);
+	u8tmp |= BIT(4);
+	btcoexist->btc_write_1byte(btcoexist, 0x101, u8tmp);
+
+	/* enable bt clock when wifi is disabled. */
+	u8tmp = btcoexist->btc_read_1byte(btcoexist, 0x93);
+	u8tmp |= BIT(0);
+	btcoexist->btc_write_1byte(btcoexist, 0x93, u8tmp);
+	/* enable bt clock when suspend. */
+	u8tmp = btcoexist->btc_read_1byte(btcoexist, 0x7);
+	u8tmp |= BIT(0);
+	btcoexist->btc_write_1byte(btcoexist, 0x7, u8tmp);
+}
+
+/*************************************************************
+ *   work around function start with wa_halbtc8192e2ant_
+ *************************************************************/
+
+/************************************************************
+ *   extern function start with EXhalbtc8192e2ant_
+ ************************************************************/
+
+void ex_halbtc8192e2ant_init_hwconfig(struct btc_coexist *btcoexist)
+{
+	halbtc8192e2ant_init_hwconfig(btcoexist, true);
+}
+
+void ex_halbtc8192e2ant_init_coex_dm(struct btc_coexist *btcoexist)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+		  "[BTCoex], Coex Mechanism Init!!\n");
+	halbtc8192e2ant_init_coex_dm(btcoexist);
+}
+
+void ex_halbtc8192e2ant_display_coex_info(struct btc_coexist *btcoexist)
+{
+	struct btc_board_info *board_info = &btcoexist->board_info;
+	struct btc_stack_info *stack_info = &btcoexist->stack_info;
+	u8 *cli_buf = btcoexist->cli_buf;
+	u8 u8tmp[4], i, bt_info_ext, ps_tdma_case = 0;
+	u16 u16tmp[4];
+	u32 u32tmp[4];
+	bool roam = false, scan = false, link = false, wifi_under_5g = false;
+	bool bt_hson = false, wifi_busy = false;
+	int wifirssi = 0, bt_hs_rssi = 0;
+	u32 wifi_bw, wifi_traffic_dir;
+	u8 wifi_dot11_chnl, wifi_hs_chnl;
+	u32 fw_ver = 0, bt_patch_ver = 0;
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n ============[BT Coexist info] ============");
+	CL_PRINTF(cli_buf);
+
+	if (btcoexist->manual_control) {
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\r\n ===========[Under Manual Control] ===========");
+		CL_PRINTF(cli_buf);
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\r\n ==========================================");
+		CL_PRINTF(cli_buf);
+	}
+
+	if (!board_info->bt_exist) {
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n BT not exists !!!");
+		CL_PRINTF(cli_buf);
+		return;
+	}
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n %-35s = %d/ %d ", "Ant PG number/ Ant mechanism: ",
+		   board_info->pg_ant_num, board_info->btdm_ant_num);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %d",
+		   "BT stack/ hci ext ver",
+		   ((stack_info->profile_notified) ? "Yes" : "No"),
+		   stack_info->hci_version);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER, &bt_patch_ver);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n %-35s = %d_%d/ 0x%x/ 0x%x(%d)",
+		   "CoexVer/ FwVer/ PatchVer",
+		   glcoex_ver_date_8192e_2ant, glcoex_ver_8192e_2ant,
+		   fw_ver, bt_patch_ver, bt_patch_ver);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hson);
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_DOT11_CHNL,
+			   &wifi_dot11_chnl);
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifi_hs_chnl);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d(%d)",
+		   "Dot11 channel / HsMode(HsChnl)",
+		   wifi_dot11_chnl, bt_hson, wifi_hs_chnl);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x ",
+		   "H2C Wifi inform bt chnl Info", coex_dm->wifi_chnl_info[0],
+		   coex_dm->wifi_chnl_info[1], coex_dm->wifi_chnl_info[2]);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifirssi);
+	btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
+		   "Wifi rssi/ HS rssi", wifirssi, bt_hs_rssi);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ",
+		   "Wifi link/ roam/ scan", link, roam, scan);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under_5g);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION,
+			   &wifi_traffic_dir);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %s/ %s ",
+		   "Wifi status", (wifi_under_5g ? "5G" : "2.4G"),
+		   ((BTC_WIFI_BW_LEGACY == wifi_bw) ? "Legacy" :
+			(((BTC_WIFI_BW_HT40 == wifi_bw) ? "HT40" : "HT20"))),
+		   ((!wifi_busy) ? "idle" :
+			((BTC_WIFI_TRAFFIC_TX == wifi_traffic_dir) ?
+				"uplink" : "downlink")));
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = [%s/ %d/ %d] ",
+		   "BT [status/ rssi/ retryCnt]",
+		   ((btcoexist->bt_info.bt_disabled) ? ("disabled") :
+		    ((coex_sta->c2h_bt_inquiry_page) ?
+		     ("inquiry/page scan") :
+		      ((BT_8192E_2ANT_BT_STATUS_NON_CONNECTED_IDLE ==
+			coex_dm->bt_status) ? "non-connected idle" :
+			 ((BT_8192E_2ANT_BT_STATUS_CONNECTED_IDLE ==
+			   coex_dm->bt_status) ? "connected-idle" : "busy")))),
+		   coex_sta->bt_rssi, coex_sta->bt_retry_cnt);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d / %d / %d",
+		   "SCO/HID/PAN/A2DP", stack_info->sco_exist,
+		   stack_info->hid_exist, stack_info->pan_exist,
+		   stack_info->a2dp_exist);
+	CL_PRINTF(cli_buf);
+	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_BT_LINK_INFO);
+
+	bt_info_ext = coex_sta->bt_info_ext;
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s",
+		   "BT Info A2DP rate",
+		   (bt_info_ext & BIT(0)) ? "Basic rate" : "EDR rate");
+	CL_PRINTF(cli_buf);
+
+	for (i = 0; i < BT_INFO_SRC_8192E_2ANT_MAX; i++) {
+		if (coex_sta->bt_info_c2h_cnt[i]) {
+			CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+				   "\r\n %-35s = %02x %02x %02x %02x ",
+				   glbt_infosrc8192e2ant[i],
+				   coex_sta->bt_info_c2h[i][0],
+				   coex_sta->bt_info_c2h[i][1],
+				   coex_sta->bt_info_c2h[i][2],
+				   coex_sta->bt_info_c2h[i][3]);
+			CL_PRINTF(cli_buf);
+			CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+				   "%02x %02x %02x(%d)",
+				   coex_sta->bt_info_c2h[i][4],
+				   coex_sta->bt_info_c2h[i][5],
+				   coex_sta->bt_info_c2h[i][6],
+				   coex_sta->bt_info_c2h_cnt[i]);
+			CL_PRINTF(cli_buf);
+		}
+	}
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s/%s",
+		   "PS state, IPS/LPS",
+		   ((coex_sta->under_ips ? "IPS ON" : "IPS OFF")),
+		   ((coex_sta->under_lps ? "LPS ON" : "LPS OFF")));
+	CL_PRINTF(cli_buf);
+	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_FW_PWR_MODE_CMD);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x ", "SS Type",
+		   coex_dm->cur_sstype);
+	CL_PRINTF(cli_buf);
+
+	/* Sw mechanism	*/
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
+		   "============[Sw mechanism] ============");
+	CL_PRINTF(cli_buf);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ",
+		   "SM1[ShRf/ LpRA/ LimDig]", coex_dm->cur_rf_rx_lpf_shrink,
+		   coex_dm->cur_low_penalty_ra, coex_dm->limited_dig);
+	CL_PRINTF(cli_buf);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d(0x%x) ",
+		   "SM2[AgcT/ AdcB/ SwDacSwing(lvl)]",
+		   coex_dm->cur_agc_table_en, coex_dm->cur_adc_back_off,
+		   coex_dm->cur_dac_swing_on, coex_dm->cur_dac_swing_lvl);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x ", "Rate Mask",
+		   btcoexist->bt_info.ra_mask);
+	CL_PRINTF(cli_buf);
+
+	/* Fw mechanism	*/
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
+		   "============[Fw mechanism] ============");
+	CL_PRINTF(cli_buf);
+
+	ps_tdma_case = coex_dm->cur_ps_tdma;
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n %-35s = %02x %02x %02x %02x %02x case-%d (auto:%d)",
+		   "PS TDMA", coex_dm->ps_tdma_para[0],
+		   coex_dm->ps_tdma_para[1], coex_dm->ps_tdma_para[2],
+		   coex_dm->ps_tdma_para[3], coex_dm->ps_tdma_para[4],
+		   ps_tdma_case, coex_dm->auto_tdma_adjust);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ",
+		   "DecBtPwr/ IgnWlanAct",
+		   coex_dm->cur_dec_bt_pwr, coex_dm->cur_ignore_wlan_act);
+	CL_PRINTF(cli_buf);
+
+	/* Hw setting */
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
+		   "============[Hw setting] ============");
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x",
+		   "RF-A, 0x1e initVal", coex_dm->bt_rf0x1e_backup);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/0x%x/0x%x/0x%x",
+		   "backup ARFR1/ARFR2/RL/AMaxTime", coex_dm->backup_arfr_cnt1,
+		   coex_dm->backup_arfr_cnt2, coex_dm->backup_retrylimit,
+		   coex_dm->backup_ampdu_maxtime);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x430);
+	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x434);
+	u16tmp[0] = btcoexist->btc_read_2byte(btcoexist, 0x42a);
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x456);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/0x%x/0x%x/0x%x",
+		   "0x430/0x434/0x42a/0x456",
+		   u32tmp[0], u32tmp[1], u16tmp[0], u8tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xc04);
+	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0xd04);
+	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x90c);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
+		   "0xc04/ 0xd04/ 0x90c", u32tmp[0], u32tmp[1], u32tmp[2]);
+	CL_PRINTF(cli_buf);
+
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x778);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x778",
+		   u8tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x92c);
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x930);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
+		   "0x92c/ 0x930", (u8tmp[0]), u32tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x40);
+	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x4f);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
+		   "0x40/ 0x4f", u8tmp[0], u8tmp[1]);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x550);
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x522);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
+		   "0x550(bcn ctrl)/0x522", u32tmp[0], u8tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xc50);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0xc50(dig)",
+		   u32tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x6c0);
+	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x6c4);
+	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x6c8);
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x6cc);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x",
+		   "0x6c0/0x6c4/0x6c8/0x6cc(coexTable)",
+		   u32tmp[0], u32tmp[1], u32tmp[2], u8tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
+		   "0x770(hp rx[31:16]/tx[15:0])",
+		   coex_sta->high_priority_rx, coex_sta->high_priority_tx);
+	CL_PRINTF(cli_buf);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
+		   "0x774(lp rx[31:16]/tx[15:0])",
+		   coex_sta->low_priority_rx, coex_sta->low_priority_tx);
+	CL_PRINTF(cli_buf);
+#if (BT_AUTO_REPORT_ONLY_8192E_2ANT == 1)
+	halbtc8192e2ant_monitor_bt_ctr(btcoexist);
+#endif
+	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_COEX_STATISTICS);
+}
+
+void ex_halbtc8192e2ant_ips_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	if (BTC_IPS_ENTER == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], IPS ENTER notify\n");
+		coex_sta->under_ips = true;
+		halbtc8192e2ant_coex_alloff(btcoexist);
+	} else if (BTC_IPS_LEAVE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], IPS LEAVE notify\n");
+		coex_sta->under_ips = false;
+	}
+}
+
+void ex_halbtc8192e2ant_lps_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	if (BTC_LPS_ENABLE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], LPS ENABLE notify\n");
+		coex_sta->under_lps = true;
+	} else if (BTC_LPS_DISABLE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], LPS DISABLE notify\n");
+		coex_sta->under_lps = false;
+	}
+}
+
+void ex_halbtc8192e2ant_scan_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	if (BTC_SCAN_START == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], SCAN START notify\n");
+	else if (BTC_SCAN_FINISH == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], SCAN FINISH notify\n");
+}
+
+void ex_halbtc8192e2ant_connect_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	if (BTC_ASSOCIATE_START == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], CONNECT START notify\n");
+	else if (BTC_ASSOCIATE_FINISH == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], CONNECT FINISH notify\n");
+}
+
+void ex_halbtc8192e2ant_media_status_notify(struct btc_coexist *btcoexist,
+					    u8 type)
+{
+	u8 h2c_parameter[3] = {0};
+	u32 wifi_bw;
+	u8 wifi_center_chnl;
+
+	if (btcoexist->manual_control ||
+	    btcoexist->stop_coex_dm ||
+	    btcoexist->bt_info.bt_disabled)
+		return;
+
+	if (BTC_MEDIA_CONNECT == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], MEDIA connect notify\n");
+	else
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], MEDIA disconnect notify\n");
+
+	/* only 2.4G we need to inform bt the chnl mask */
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_CENTRAL_CHNL,
+			   &wifi_center_chnl);
+	if ((BTC_MEDIA_CONNECT == type) &&
+	    (wifi_center_chnl <= 14)) {
+		h2c_parameter[0] = 0x1;
+		h2c_parameter[1] = wifi_center_chnl;
+		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+		if (BTC_WIFI_BW_HT40 == wifi_bw)
+			h2c_parameter[2] = 0x30;
+		else
+			h2c_parameter[2] = 0x20;
+	}
+
+	coex_dm->wifi_chnl_info[0] = h2c_parameter[0];
+	coex_dm->wifi_chnl_info[1] = h2c_parameter[1];
+	coex_dm->wifi_chnl_info[2] = h2c_parameter[2];
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], FW write 0x66 = 0x%x\n",
+		  h2c_parameter[0] << 16 | h2c_parameter[1] << 8 |
+		  h2c_parameter[2]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x66, 3, h2c_parameter);
+}
+
+void ex_halbtc8192e2ant_special_packet_notify(struct btc_coexist *btcoexist,
+					      u8 type)
+{
+	if (type == BTC_PACKET_DHCP)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], DHCP Packet notify\n");
+}
+
+void ex_halbtc8192e2ant_bt_info_notify(struct btc_coexist *btcoexist,
+				       u8 *tmp_buf, u8 length)
+{
+	u8 bt_info = 0;
+	u8 i, rspsource = 0;
+	bool bt_busy = false, limited_dig = false;
+	bool wifi_connected = false;
+
+	coex_sta->c2h_bt_info_req_sent = false;
+
+	rspsource = tmp_buf[0] & 0xf;
+	if (rspsource >= BT_INFO_SRC_8192E_2ANT_MAX)
+		rspsource = BT_INFO_SRC_8192E_2ANT_WIFI_FW;
+	coex_sta->bt_info_c2h_cnt[rspsource]++;
+
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+		  "[BTCoex], Bt info[%d], length =%d, hex data =[",
+		  rspsource, length);
+	for (i = 0; i < length; i++) {
+		coex_sta->bt_info_c2h[rspsource][i] = tmp_buf[i];
+		if (i == 1)
+			bt_info = tmp_buf[i];
+		if (i == length-1)
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+				  "0x%02x]\n", tmp_buf[i]);
+		else
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+				  "0x%02x, ", tmp_buf[i]);
+	}
+
+	if (BT_INFO_SRC_8192E_2ANT_WIFI_FW != rspsource) {
+		coex_sta->bt_retry_cnt =	/* [3:0] */
+			coex_sta->bt_info_c2h[rspsource][2] & 0xf;
+
+		coex_sta->bt_rssi =
+			coex_sta->bt_info_c2h[rspsource][3] * 2 + 10;
+
+		coex_sta->bt_info_ext =
+			coex_sta->bt_info_c2h[rspsource][4];
+
+		/* Here we need to resend some wifi info to BT
+		 * because bt is reset and loss of the info. */
+		if ((coex_sta->bt_info_ext & BIT(1))) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "bit1, send wifi BW&Chnl to BT!!\n");
+			btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+					   &wifi_connected);
+			if (wifi_connected)
+				ex_halbtc8192e2ant_media_status_notify(
+							btcoexist,
+							BTC_MEDIA_CONNECT);
+			else
+				ex_halbtc8192e2ant_media_status_notify(
+							btcoexist,
+							BTC_MEDIA_DISCONNECT);
+		}
+
+		if ((coex_sta->bt_info_ext & BIT(3))) {
+			if (!btcoexist->manual_control &&
+			    !btcoexist->stop_coex_dm) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "bit3, BT NOT ignore Wlan active!\n");
+				halbtc8192e2ant_ignorewlanact(btcoexist,
+							      FORCE_EXEC,
+							      false);
+			}
+		} else {
+			/* BT already NOT ignore Wlan active,
+			 * do nothing here. */
+		}
+
+#if (BT_AUTO_REPORT_ONLY_8192E_2ANT == 0)
+		if ((coex_sta->bt_info_ext & BIT(4))) {
+			/* BT auto report already enabled, do nothing */
+		} else {
+			halbtc8192e2ant_bt_autoreport(btcoexist, FORCE_EXEC,
+						      true);
+		}
+#endif
+	}
+
+	/* check BIT(2) first ==> check if bt is under inquiry or page scan */
+	if (bt_info & BT_INFO_8192E_2ANT_B_INQ_PAGE)
+		coex_sta->c2h_bt_inquiry_page = true;
+	else
+		coex_sta->c2h_bt_inquiry_page = false;
+
+	/* set link exist status */
+	if (!(bt_info&BT_INFO_8192E_2ANT_B_CONNECTION)) {
+		coex_sta->bt_link_exist = false;
+		coex_sta->pan_exist = false;
+		coex_sta->a2dp_exist = false;
+		coex_sta->hid_exist = false;
+		coex_sta->sco_exist = false;
+	} else {/* connection exists */
+		coex_sta->bt_link_exist = true;
+		if (bt_info & BT_INFO_8192E_2ANT_B_FTP)
+			coex_sta->pan_exist = true;
+		else
+			coex_sta->pan_exist = false;
+		if (bt_info & BT_INFO_8192E_2ANT_B_A2DP)
+			coex_sta->a2dp_exist = true;
+		else
+			coex_sta->a2dp_exist = false;
+		if (bt_info & BT_INFO_8192E_2ANT_B_HID)
+			coex_sta->hid_exist = true;
+		else
+			coex_sta->hid_exist = false;
+		if (bt_info & BT_INFO_8192E_2ANT_B_SCO_ESCO)
+			coex_sta->sco_exist = true;
+		else
+			coex_sta->sco_exist = false;
+	}
+
+	halbtc8192e2ant_update_btlink_info(btcoexist);
+
+	if (!(bt_info&BT_INFO_8192E_2ANT_B_CONNECTION)) {
+		coex_dm->bt_status = BT_8192E_2ANT_BT_STATUS_NON_CONNECTED_IDLE;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BT Non-Connected idle!!!\n");
+	} else if (bt_info == BT_INFO_8192E_2ANT_B_CONNECTION) {
+		coex_dm->bt_status = BT_8192E_2ANT_BT_STATUS_CONNECTED_IDLE;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], bt_infoNotify(), BT Connected-idle!!!\n");
+	} else if ((bt_info&BT_INFO_8192E_2ANT_B_SCO_ESCO) ||
+		   (bt_info&BT_INFO_8192E_2ANT_B_SCO_BUSY)) {
+		coex_dm->bt_status = BT_8192E_2ANT_BT_STATUS_SCO_BUSY;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], bt_infoNotify(), BT SCO busy!!!\n");
+	} else if (bt_info&BT_INFO_8192E_2ANT_B_ACL_BUSY) {
+		coex_dm->bt_status = BT_8192E_2ANT_BT_STATUS_ACL_BUSY;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], bt_infoNotify(), BT ACL busy!!!\n");
+	} else {
+		coex_dm->bt_status = BT_8192E_2ANT_BT_STATUS_MAX;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex]bt_infoNotify(), BT Non-Defined state!!!\n");
+	}
+
+	if ((BT_8192E_2ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) ||
+	    (BT_8192E_2ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
+	    (BT_8192E_2ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status)) {
+		bt_busy = true;
+		limited_dig = true;
+	} else {
+		bt_busy = false;
+		limited_dig = false;
+	}
+
+	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bt_busy);
+
+	coex_dm->limited_dig = limited_dig;
+	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_LIMITED_DIG, &limited_dig);
+
+	halbtc8192e2ant_run_coexist_mechanism(btcoexist);
+}
+
+void ex_halbtc8192e2ant_stack_operation_notify(struct btc_coexist *btcoexist,
+					       u8 type)
+{
+}
+
+void ex_halbtc8192e2ant_halt_notify(struct btc_coexist *btcoexist)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, "[BTCoex], Halt notify\n");
+
+	halbtc8192e2ant_ignorewlanact(btcoexist, FORCE_EXEC, true);
+	ex_halbtc8192e2ant_media_status_notify(btcoexist, BTC_MEDIA_DISCONNECT);
+}
+
+void ex_halbtc8192e2ant_periodical(struct btc_coexist *btcoexist)
+{
+	static u8 dis_ver_info_cnt;
+	u32 fw_ver = 0, bt_patch_ver = 0;
+	struct btc_board_info *board_info = &btcoexist->board_info;
+	struct btc_stack_info *stack_info = &btcoexist->stack_info;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+		  "======================= Periodical =======================\n");
+	if (dis_ver_info_cnt <= 5) {
+		dis_ver_info_cnt += 1;
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "************************************************\n");
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "Ant PG Num/ Ant Mech/ Ant Pos = %d/ %d/ %d\n",
+			  board_info->pg_ant_num, board_info->btdm_ant_num,
+			  board_info->btdm_ant_pos);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "BT stack/ hci ext ver = %s / %d\n",
+			  ((stack_info->profile_notified) ? "Yes" : "No"),
+			  stack_info->hci_version);
+		btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER,
+				   &bt_patch_ver);
+		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "CoexVer/ FwVer/ PatchVer = %d_%x/ 0x%x/ 0x%x(%d)\n",
+			  glcoex_ver_date_8192e_2ant, glcoex_ver_8192e_2ant,
+			  fw_ver, bt_patch_ver, bt_patch_ver);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "************************************************\n");
+	}
+
+#if (BT_AUTO_REPORT_ONLY_8192E_2ANT == 0)
+	halbtc8192e2ant_querybt_info(btcoexist);
+	halbtc8192e2ant_monitor_bt_ctr(btcoexist);
+	halbtc_monitor_bt_enable_disable(btcoexist);
+#else
+	if (halbtc8192e2ant_iswifi_status_changed(btcoexist) ||
+	    coex_dm->auto_tdma_adjust)
+		halbtc8192e2ant_run_coexist_mechanism(btcoexist);
+#endif
+}
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbtc8192e2ant.h b/drivers/staging/rtl8192ee/btcoexist/halbtc8192e2ant.h
new file mode 100644
index 00000000000..ece3e10e6df
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/halbtc8192e2ant.h
@@ -0,0 +1,161 @@
+/*****************************************************************
+ *   The following is for 8192E 2Ant BT Co-exist definition
+ *****************************************************************/
+#define	BT_AUTO_REPORT_ONLY_8192E_2ANT			0
+
+#define	BT_INFO_8192E_2ANT_B_FTP			BIT(7)
+#define	BT_INFO_8192E_2ANT_B_A2DP			BIT(6)
+#define	BT_INFO_8192E_2ANT_B_HID			BIT(5)
+#define	BT_INFO_8192E_2ANT_B_SCO_BUSY			BIT(4)
+#define	BT_INFO_8192E_2ANT_B_ACL_BUSY			BIT(3)
+#define	BT_INFO_8192E_2ANT_B_INQ_PAGE			BIT(2)
+#define	BT_INFO_8192E_2ANT_B_SCO_ESCO			BIT(1)
+#define	BT_INFO_8192E_2ANT_B_CONNECTION			BIT(0)
+
+#define BTC_RSSI_COEX_THRESH_TOL_8192E_2ANT		2
+
+enum bt_info_src_8192e_2ant {
+	BT_INFO_SRC_8192E_2ANT_WIFI_FW			= 0x0,
+	BT_INFO_SRC_8192E_2ANT_BT_RSP			= 0x1,
+	BT_INFO_SRC_8192E_2ANT_BT_ACTIVE_SEND		= 0x2,
+	BT_INFO_SRC_8192E_2ANT_MAX
+};
+
+enum bt_8192e_2ant_bt_status {
+	BT_8192E_2ANT_BT_STATUS_NON_CONNECTED_IDLE	= 0x0,
+	BT_8192E_2ANT_BT_STATUS_CONNECTED_IDLE		= 0x1,
+	BT_8192E_2ANT_BT_STATUS_INQ_PAGE		= 0x2,
+	BT_8192E_2ANT_BT_STATUS_ACL_BUSY		= 0x3,
+	BT_8192E_2ANT_BT_STATUS_SCO_BUSY		= 0x4,
+	BT_8192E_2ANT_BT_STATUS_ACL_SCO_BUSY		= 0x5,
+	BT_8192E_2ANT_BT_STATUS_MAX
+};
+
+enum bt_8192e_2ant_coex_algo {
+	BT_8192E_2ANT_COEX_ALGO_UNDEFINED		= 0x0,
+	BT_8192E_2ANT_COEX_ALGO_SCO			= 0x1,
+	BT_8192E_2ANT_COEX_ALGO_SCO_PAN			= 0x2,
+	BT_8192E_2ANT_COEX_ALGO_HID			= 0x3,
+	BT_8192E_2ANT_COEX_ALGO_A2DP			= 0x4,
+	BT_8192E_2ANT_COEX_ALGO_A2DP_PANHS		= 0x5,
+	BT_8192E_2ANT_COEX_ALGO_PANEDR			= 0x6,
+	BT_8192E_2ANT_COEX_ALGO_PANHS			= 0x7,
+	BT_8192E_2ANT_COEX_ALGO_PANEDR_A2DP		= 0x8,
+	BT_8192E_2ANT_COEX_ALGO_PANEDR_HID		= 0x9,
+	BT_8192E_2ANT_COEX_ALGO_HID_A2DP_PANEDR		= 0xa,
+	BT_8192E_2ANT_COEX_ALGO_HID_A2DP		= 0xb,
+	BT_8192E_2ANT_COEX_ALGO_MAX			= 0xc
+};
+
+struct coex_dm_8192e_2ant {
+	/* fw mechanism */
+	u8 pre_dec_bt_pwr;
+	u8 cur_dec_bt_pwr;
+	u8 pre_fw_dac_swing_lvl;
+	u8 cur_fw_dac_swing_lvl;
+	bool cur_ignore_wlan_act;
+	bool pre_ignore_wlan_act;
+	u8 pre_ps_tdma;
+	u8 cur_ps_tdma;
+	u8 ps_tdma_para[5];
+	u8 ps_tdma_du_adj_type;
+	bool reset_tdma_adjust;
+	bool auto_tdma_adjust;
+	bool pre_ps_tdma_on;
+	bool cur_ps_tdma_on;
+	bool pre_bt_auto_report;
+	bool cur_bt_auto_report;
+
+	/* sw mechanism */
+	bool pre_rf_rx_lpf_shrink;
+	bool cur_rf_rx_lpf_shrink;
+	u32 bt_rf0x1e_backup;
+	bool pre_low_penalty_ra;
+	bool cur_low_penalty_ra;
+	bool pre_dac_swing_on;
+	u32 pre_dac_swing_lvl;
+	bool cur_dac_swing_on;
+	u32 cur_dac_swing_lvl;
+	bool pre_adc_back_off;
+	bool cur_adc_back_off;
+	bool pre_agc_table_en;
+	bool cur_agc_table_en;
+	u32 pre_val0x6c0;
+	u32 cur_val0x6c0;
+	u32 pre_val0x6c4;
+	u32 cur_val0x6c4;
+	u32 pre_val0x6c8;
+	u32 cur_val0x6c8;
+	u8 pre_val0x6cc;
+	u8 cur_val0x6cc;
+	bool limited_dig;
+
+	u32 backup_arfr_cnt1;	/* Auto Rate Fallback Retry cnt */
+	u32 backup_arfr_cnt2;	/* Auto Rate Fallback Retry cnt */
+	u16 backup_retrylimit;
+	u8 backup_ampdu_maxtime;
+
+	/* algorithm related */
+	u8 pre_algorithm;
+	u8 cur_algorithm;
+	u8 bt_status;
+	u8 wifi_chnl_info[3];
+
+	u8 pre_sstype;
+	u8 cur_sstype;
+
+	u32 prera_mask;
+	u32 curra_mask;
+	u8 curra_masktype;
+	u8 pre_arfrtype;
+	u8 cur_arfrtype;
+	u8 pre_retrylimit_type;
+	u8 cur_retrylimit_type;
+	u8 pre_ampdutime_type;
+	u8 cur_ampdutime_type;
+};
+
+struct coex_sta_8192e_2ant {
+	bool bt_link_exist;
+	bool sco_exist;
+	bool a2dp_exist;
+	bool hid_exist;
+	bool pan_exist;
+
+	bool under_lps;
+	bool under_ips;
+	u32 high_priority_tx;
+	u32 high_priority_rx;
+	u32 low_priority_tx;
+	u32 low_priority_rx;
+	u8 bt_rssi;
+	u8 pre_bt_rssi_state;
+	u8 pre_wifi_rssi_state[4];
+	bool c2h_bt_info_req_sent;
+	u8 bt_info_c2h[BT_INFO_SRC_8192E_2ANT_MAX][10];
+	u32 bt_info_c2h_cnt[BT_INFO_SRC_8192E_2ANT_MAX];
+	bool c2h_bt_inquiry_page;
+	u8 bt_retry_cnt;
+	u8 bt_info_ext;
+};
+
+/****************************************************************
+ *    The following is interface which will notify coex module.
+ ****************************************************************/
+void ex_halbtc8192e2ant_init_hwconfig(struct btc_coexist *btcoexist);
+void ex_halbtc8192e2ant_init_coex_dm(struct btc_coexist *btcoexist);
+void ex_halbtc8192e2ant_ips_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_halbtc8192e2ant_lps_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_halbtc8192e2ant_scan_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_halbtc8192e2ant_connect_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_halbtc8192e2ant_media_status_notify(struct btc_coexist *btcoexist,
+					    u8 type);
+void ex_halbtc8192e2ant_special_packet_notify(struct btc_coexist *btcoexist,
+					      u8 type);
+void ex_halbtc8192e2ant_bt_info_notify(struct btc_coexist *btcoexist,
+				       u8 *tmpbuf, u8 length);
+void ex_halbtc8192e2ant_stack_operation_notify(struct btc_coexist *btcoexist,
+					       u8 type);
+void ex_halbtc8192e2ant_halt_notify(struct btc_coexist *btcoexist);
+void ex_halbtc8192e2ant_periodical(struct btc_coexist *btcoexist);
+void ex_halbtc8192e2ant_display_coex_info(struct btc_coexist *btcoexist);
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbtc8723b1ant.c b/drivers/staging/rtl8192ee/btcoexist/halbtc8723b1ant.c
new file mode 100644
index 00000000000..153048f6b4d
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/halbtc8723b1ant.c
@@ -0,0 +1,3146 @@
+/***************************************************************
+ * Description:
+ *
+ * This file is for RTL8723B Co-exist mechanism
+ *
+ * History
+ * 2012/11/15 Cosa first check in.
+ *
+ ***************************************************************/
+
+/***************************************************************
+ * include files
+ ***************************************************************/
+#include "halbt_precomp.h"
+/***************************************************************
+ * Global variables, these are static variables
+ ***************************************************************/
+static struct coex_dm_8723b_1ant glcoex_dm_8723b_1ant;
+static struct coex_dm_8723b_1ant *coex_dm = &glcoex_dm_8723b_1ant;
+static struct coex_sta_8723b_1ant glcoex_sta_8723b_1ant;
+static struct coex_sta_8723b_1ant *coex_sta = &glcoex_sta_8723b_1ant;
+
+static const char *const glb_infosrc8723b1ant[] = {
+	"BT Info[wifi fw]",
+	"BT Info[bt rsp]",
+	"BT Info[bt auto report]",
+};
+
+static u32 glcoex_ver_date_8723b_1ant = 20130918;
+static u32 glcoex_ver_8723b_1ant = 0x47;
+
+/***************************************************************
+ * local function proto type if needed
+ ***************************************************************/
+/***************************************************************
+ * local function start with halbtc8723b1ant_
+ ***************************************************************/
+static u8 halbtc8723b1ant_bt_rssi_state(u8 level_num, u8 rssi_thresh,
+					u8 rssi_thresh1)
+{
+	s32 bt_rssi = 0;
+	u8 rssi_state = coex_sta->pre_bt_rssi_state;
+
+	bt_rssi = coex_sta->bt_rssi;
+
+	if (level_num == 2) {
+		if ((coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_LOW) ||
+		    (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+			if (bt_rssi >= rssi_thresh +
+					BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT) {
+				rssi_state = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state switch to High\n");
+			} else {
+				rssi_state = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state stay at Low\n");
+			}
+		} else {
+			if (bt_rssi < rssi_thresh) {
+				rssi_state = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state switch to Low\n");
+			} else {
+				rssi_state = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state stay at High\n");
+			}
+		}
+	} else if (level_num == 3) {
+		if (rssi_thresh > rssi_thresh1) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+				  "[BTCoex], BT Rssi thresh error!!\n");
+			return coex_sta->pre_bt_rssi_state;
+		}
+
+		if ((coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_LOW) ||
+		    (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+			if (bt_rssi >= rssi_thresh +
+					BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT) {
+				rssi_state = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state switch to Medium\n");
+			} else {
+				rssi_state = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state stay at Low\n");
+			}
+		} else if ((coex_sta->pre_bt_rssi_state ==
+					BTC_RSSI_STATE_MEDIUM) ||
+			  (coex_sta->pre_bt_rssi_state ==
+					BTC_RSSI_STATE_STAY_MEDIUM)) {
+			if (bt_rssi >= rssi_thresh1 +
+					BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT) {
+				rssi_state = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state switch to High\n");
+			} else if (bt_rssi < rssi_thresh) {
+				rssi_state = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state switch to Low\n");
+			} else {
+				rssi_state = BTC_RSSI_STATE_STAY_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state stay at Medium\n");
+			}
+		} else {
+			if (bt_rssi < rssi_thresh1) {
+				rssi_state = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state switch to Medium\n");
+			} else {
+				rssi_state = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state stay at High\n");
+			}
+		}
+	}
+
+	coex_sta->pre_bt_rssi_state = rssi_state;
+
+	return rssi_state;
+}
+
+static u8 halbtc8723b1ant_wifi_rssi_state(struct btc_coexist *btcoexist,
+					  u8 index, u8 level_num,
+					  u8 rssi_thresh, u8 rssi_thresh1)
+{
+	s32 wifi_rssi = 0;
+	u8 wifi_rssi_state = coex_sta->pre_wifi_rssi_state[index];
+
+	btcoexist->btc_get(btcoexist,
+		BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
+
+	if (level_num == 2) {
+		if ((coex_sta->pre_wifi_rssi_state[index] ==
+					BTC_RSSI_STATE_LOW) ||
+		    (coex_sta->pre_wifi_rssi_state[index] ==
+					BTC_RSSI_STATE_STAY_LOW)) {
+			if (wifi_rssi >= rssi_thresh +
+					BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT) {
+				wifi_rssi_state = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state switch to High\n");
+			} else {
+				wifi_rssi_state = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state stay at Low\n");
+			}
+		} else {
+			if (wifi_rssi < rssi_thresh) {
+				wifi_rssi_state = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state switch to Low\n");
+			} else {
+				wifi_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state stay at High\n");
+			}
+		}
+	} else if (level_num == 3) {
+		if (rssi_thresh > rssi_thresh1) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE,
+				  "[BTCoex], wifi RSSI thresh error!!\n");
+			return coex_sta->pre_wifi_rssi_state[index];
+		}
+
+		if ((coex_sta->pre_wifi_rssi_state[index] ==
+						BTC_RSSI_STATE_LOW) ||
+		    (coex_sta->pre_wifi_rssi_state[index] ==
+						BTC_RSSI_STATE_STAY_LOW)) {
+			if (wifi_rssi >= rssi_thresh +
+					 BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT) {
+				wifi_rssi_state = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state switch to Medium\n");
+			} else {
+				wifi_rssi_state = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state stay at Low\n");
+			}
+		} else if ((coex_sta->pre_wifi_rssi_state[index] ==
+						BTC_RSSI_STATE_MEDIUM) ||
+			   (coex_sta->pre_wifi_rssi_state[index] ==
+						BTC_RSSI_STATE_STAY_MEDIUM)) {
+			if (wifi_rssi >= rssi_thresh1 +
+					 BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT) {
+				wifi_rssi_state = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state switch to High\n");
+			} else if (wifi_rssi < rssi_thresh) {
+				wifi_rssi_state = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state switch to Low\n");
+			} else {
+				wifi_rssi_state = BTC_RSSI_STATE_STAY_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state stay at Medium\n");
+			}
+		} else {
+			if (wifi_rssi < rssi_thresh1) {
+				wifi_rssi_state = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state switch to Medium\n");
+			} else {
+				wifi_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state stay at High\n");
+			}
+		}
+	}
+
+	coex_sta->pre_wifi_rssi_state[index] = wifi_rssi_state;
+
+	return wifi_rssi_state;
+}
+
+static void halbtc8723b1ant_updatera_mask(struct btc_coexist *btcoexist,
+					  bool force_exec, u32 dis_rate_mask)
+{
+	coex_dm->curra_mask = dis_rate_mask;
+
+	if (force_exec || (coex_dm->prera_mask != coex_dm->curra_mask))
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_UPDATE_ra_mask,
+				   &coex_dm->curra_mask);
+
+	coex_dm->prera_mask = coex_dm->curra_mask;
+}
+
+static void auto_rate_fallback_retry(struct btc_coexist *btcoexist,
+				     bool force_exec, u8 type)
+{
+	bool wifi_under_bmode = false;
+
+	coex_dm->cur_arfr_type = type;
+
+	if (force_exec || (coex_dm->pre_arfr_type != coex_dm->cur_arfr_type)) {
+		switch (coex_dm->cur_arfr_type) {
+		case 0:	/* normal mode */
+			btcoexist->btc_write_4byte(btcoexist, 0x430,
+						   coex_dm->backup_arfr_cnt1);
+			btcoexist->btc_write_4byte(btcoexist, 0x434,
+						   coex_dm->backup_arfr_cnt2);
+			break;
+		case 1:
+			btcoexist->btc_get(btcoexist,
+					   BTC_GET_BL_WIFI_UNDER_B_MODE,
+					   &wifi_under_bmode);
+			if (wifi_under_bmode) {
+				btcoexist->btc_write_4byte(btcoexist,
+							   0x430, 0x0);
+				btcoexist->btc_write_4byte(btcoexist,
+							   0x434, 0x01010101);
+			} else {
+				btcoexist->btc_write_4byte(btcoexist,
+							   0x430, 0x0);
+				btcoexist->btc_write_4byte(btcoexist,
+							   0x434, 0x04030201);
+			}
+			break;
+		default:
+			break;
+		}
+	}
+
+	coex_dm->pre_arfr_type = coex_dm->cur_arfr_type;
+}
+
+static void halbtc8723b1ant_retry_limit(struct btc_coexist *btcoexist,
+					bool force_exec, u8 type)
+{
+	coex_dm->cur_retry_limit_type = type;
+
+	if (force_exec || (coex_dm->pre_retry_limit_type !=
+			   coex_dm->cur_retry_limit_type)) {
+		switch (coex_dm->cur_retry_limit_type) {
+		case 0:	/* normal mode */
+			btcoexist->btc_write_2byte(btcoexist, 0x42a,
+						   coex_dm->backup_retry_limit);
+			break;
+		case 1:	/* retry limit = 8 */
+			btcoexist->btc_write_2byte(btcoexist, 0x42a, 0x0808);
+			break;
+		default:
+			break;
+		}
+	}
+
+	coex_dm->pre_retry_limit_type = coex_dm->cur_retry_limit_type;
+}
+
+static void halbtc8723b1ant_ampdu_maxtime(struct btc_coexist *btcoexist,
+					  bool force_exec, u8 type)
+{
+	coex_dm->cur_ampdu_time_type = type;
+
+	if (force_exec || (coex_dm->pre_ampdu_time_type !=
+		coex_dm->cur_ampdu_time_type)) {
+		switch (coex_dm->cur_ampdu_time_type) {
+		case 0:	/* normal mode */
+			btcoexist->btc_write_1byte(btcoexist, 0x456,
+					coex_dm->backup_ampdu_max_time);
+			break;
+		case 1:	/* AMPDU timw = 0x38 * 32us */
+			btcoexist->btc_write_1byte(btcoexist,
+						   0x456, 0x38);
+			break;
+		default:
+			break;
+		}
+	}
+
+	coex_dm->pre_ampdu_time_type = coex_dm->cur_ampdu_time_type;
+}
+
+static void halbtc8723b1ant_limited_tx(struct btc_coexist *btcoexist,
+				       bool force_exec, u8 ra_masktype,
+				       u8 arfr_type, u8 retry_limit_type,
+				       u8 ampdu_time_type)
+{
+	switch (ra_masktype) {
+	case 0:	/* normal mode */
+		halbtc8723b1ant_updatera_mask(btcoexist, force_exec, 0x0);
+		break;
+	case 1:	/* disable cck 1/2 */
+		halbtc8723b1ant_updatera_mask(btcoexist, force_exec,
+					      0x00000003);
+		break;
+	/* disable cck 1/2/5.5, ofdm 6/9/12/18/24, mcs 0/1/2/3/4*/
+	case 2:
+		halbtc8723b1ant_updatera_mask(btcoexist, force_exec,
+					      0x0001f1f7);
+		break;
+	default:
+		break;
+	}
+
+	auto_rate_fallback_retry(btcoexist, force_exec, arfr_type);
+	halbtc8723b1ant_retry_limit(btcoexist, force_exec, retry_limit_type);
+	halbtc8723b1ant_ampdu_maxtime(btcoexist, force_exec, ampdu_time_type);
+}
+
+static void halbtc8723b1ant_limited_rx(struct btc_coexist *btcoexist,
+				       bool force_exec, bool rej_ap_agg_pkt,
+				bool b_bt_ctrl_agg_buf_size, u8 agg_buf_size)
+{
+	bool reject_rx_agg = rej_ap_agg_pkt;
+	bool bt_ctrl_rx_agg_size = b_bt_ctrl_agg_buf_size;
+	u8 rxaggsize = agg_buf_size;
+
+	/**********************************************
+	 *	Rx Aggregation related setting
+	 **********************************************/
+	btcoexist->btc_set(btcoexist, BTC_SET_BL_TO_REJ_AP_AGG_PKT,
+			   &reject_rx_agg);
+	/* decide BT control aggregation buf size or not  */
+	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_CTRL_AGG_SIZE,
+			   &bt_ctrl_rx_agg_size);
+	/* aggregation buf size, only work
+	 *when BT control Rx aggregation size.  */
+	btcoexist->btc_set(btcoexist, BTC_SET_U1_AGG_BUF_SIZE, &rxaggsize);
+	/* real update aggregation setting  */
+	btcoexist->btc_set(btcoexist, BTC_SET_ACT_AGGREGATE_CTRL, NULL);
+}
+
+static void halbtc8723b1ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
+{
+	u32 reg_hp_txrx, reg_lp_txrx, u32tmp;
+	u32 reg_hp_tx = 0, reg_hp_rx = 0;
+	u32 reg_lp_tx = 0, reg_lp_rx = 0;
+
+	reg_hp_txrx = 0x770;
+	reg_lp_txrx = 0x774;
+
+	u32tmp = btcoexist->btc_read_4byte(btcoexist, reg_hp_txrx);
+	reg_hp_tx = u32tmp & MASKLWORD;
+	reg_hp_rx = (u32tmp & MASKHWORD) >> 16;
+
+	u32tmp = btcoexist->btc_read_4byte(btcoexist, reg_lp_txrx);
+	reg_lp_tx = u32tmp & MASKLWORD;
+	reg_lp_rx = (u32tmp & MASKHWORD) >> 16;
+
+	coex_sta->high_priority_tx = reg_hp_tx;
+	coex_sta->high_priority_rx = reg_hp_rx;
+	coex_sta->low_priority_tx = reg_lp_tx;
+	coex_sta->low_priority_rx = reg_lp_rx;
+
+	/* reset counter */
+	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
+}
+
+static void halbtc8723b1ant_query_bt_info(struct btc_coexist *btcoexist)
+{
+	u8 h2c_parameter[1] = {0};
+
+	coex_sta->c2h_bt_info_req_sent = true;
+
+	h2c_parameter[0] |= BIT(0);	/* trigger*/
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], Query Bt Info, FW write 0x61 = 0x%x\n",
+		  h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x61, 1, h2c_parameter);
+}
+
+static bool is_wifi_status_changed(struct btc_coexist *btcoexist)
+{
+	static bool pre_wifi_busy;
+	static bool pre_under_4way, pre_bt_hs_on;
+	bool wifi_busy = false, under_4way = false, bt_hs_on = false;
+	bool wifi_connected = false;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+			   &wifi_connected);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS,
+			   &under_4way);
+
+	if (wifi_connected) {
+		if (wifi_busy != pre_wifi_busy) {
+			pre_wifi_busy = wifi_busy;
+			return true;
+		}
+		if (under_4way != pre_under_4way) {
+			pre_under_4way = under_4way;
+			return true;
+		}
+		if (bt_hs_on != pre_bt_hs_on) {
+			pre_bt_hs_on = bt_hs_on;
+			return true;
+		}
+	}
+
+	return false;
+}
+
+static void halbtc8723b1ant_update_bt_link_info(struct btc_coexist *btcoexist)
+{
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	bool bt_hs_on = false;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+
+	bt_link_info->bt_link_exist = coex_sta->bt_link_exist;
+	bt_link_info->sco_exist = coex_sta->sco_exist;
+	bt_link_info->a2dp_exist = coex_sta->a2dp_exist;
+	bt_link_info->pan_exist = coex_sta->pan_exist;
+	bt_link_info->hid_exist = coex_sta->hid_exist;
+
+	/* work around for HS mode. */
+	if (bt_hs_on) {
+		bt_link_info->pan_exist = true;
+		bt_link_info->bt_link_exist = true;
+	}
+
+	/* check if Sco only */
+	if (bt_link_info->sco_exist && !bt_link_info->a2dp_exist &&
+	    !bt_link_info->pan_exist && !bt_link_info->hid_exist)
+		bt_link_info->sco_only = true;
+	else
+		bt_link_info->sco_only = false;
+
+	/* check if A2dp only */
+	if (!bt_link_info->sco_exist && bt_link_info->a2dp_exist &&
+	    !bt_link_info->pan_exist && !bt_link_info->hid_exist)
+		bt_link_info->a2dp_only = true;
+	else
+		bt_link_info->a2dp_only = false;
+
+	/* check if Pan only */
+	if (!bt_link_info->sco_exist && !bt_link_info->a2dp_exist &&
+	    bt_link_info->pan_exist && !bt_link_info->hid_exist)
+		bt_link_info->pan_only = true;
+	else
+		bt_link_info->pan_only = false;
+
+	/* check if Hid only */
+	if (!bt_link_info->sco_exist && !bt_link_info->a2dp_exist &&
+	    !bt_link_info->pan_exist && bt_link_info->hid_exist)
+		bt_link_info->hid_only = true;
+	else
+		bt_link_info->hid_only = false;
+}
+
+static u8 halbtc8723b1ant_action_algorithm(struct btc_coexist *btcoexist)
+{
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	bool bt_hs_on = false;
+	u8 algorithm = BT_8723B_1ANT_COEX_ALGO_UNDEFINED;
+	u8 numofdiffprofile = 0;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+
+	if (!bt_link_info->bt_link_exist) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], No BT link exists!!!\n");
+		return algorithm;
+	}
+
+	if (bt_link_info->sco_exist)
+		numofdiffprofile++;
+	if (bt_link_info->hid_exist)
+		numofdiffprofile++;
+	if (bt_link_info->pan_exist)
+		numofdiffprofile++;
+	if (bt_link_info->a2dp_exist)
+		numofdiffprofile++;
+
+	if (numofdiffprofile == 1) {
+		if (bt_link_info->sco_exist) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], BT Profile = SCO only\n");
+			algorithm = BT_8723B_1ANT_COEX_ALGO_SCO;
+		} else {
+			if (bt_link_info->hid_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], BT Profile = HID only\n");
+				algorithm = BT_8723B_1ANT_COEX_ALGO_HID;
+			} else if (bt_link_info->a2dp_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], BT Profile = A2DP only\n");
+				algorithm = BT_8723B_1ANT_COEX_ALGO_A2DP;
+			} else if (bt_link_info->pan_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = PAN(HS) only\n");
+					algorithm =
+						BT_8723B_1ANT_COEX_ALGO_PANHS;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = PAN(EDR) only\n");
+					algorithm =
+						BT_8723B_1ANT_COEX_ALGO_PANEDR;
+				}
+			}
+		}
+	} else if (numofdiffprofile == 2) {
+		if (bt_link_info->sco_exist) {
+			if (bt_link_info->hid_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], BT Profile = SCO + HID\n");
+				algorithm = BT_8723B_1ANT_COEX_ALGO_HID;
+			} else if (bt_link_info->a2dp_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], BT Profile = SCO + A2DP ==> SCO\n");
+				algorithm = BT_8723B_1ANT_COEX_ALGO_SCO;
+			} else if (bt_link_info->pan_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = SCO + PAN(HS)\n");
+					algorithm = BT_8723B_1ANT_COEX_ALGO_SCO;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = SCO + PAN(EDR)\n");
+					algorithm =
+					    BT_8723B_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		} else {
+			if (bt_link_info->hid_exist &&
+			    bt_link_info->a2dp_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], BT Profile = HID + A2DP\n");
+				algorithm = BT_8723B_1ANT_COEX_ALGO_HID_A2DP;
+			} else if (bt_link_info->hid_exist &&
+				   bt_link_info->pan_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = HID + PAN(HS)\n");
+					algorithm =
+					    BT_8723B_1ANT_COEX_ALGO_HID_A2DP;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = HID + PAN(EDR)\n");
+					algorithm =
+					    BT_8723B_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			} else if (bt_link_info->pan_exist &&
+				   bt_link_info->a2dp_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = A2DP + PAN(HS)\n");
+					algorithm =
+					    BT_8723B_1ANT_COEX_ALGO_A2DP_PANHS;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = A2DP + PAN(EDR)\n");
+					algorithm =
+					    BT_8723B_1ANT_COEX_ALGO_PANEDR_A2DP;
+				}
+			}
+		}
+	} else if (numofdiffprofile == 3) {
+		if (bt_link_info->sco_exist) {
+			if (bt_link_info->hid_exist &&
+			    bt_link_info->a2dp_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], BT Profile = SCO + HID + A2DP ==> HID\n");
+				algorithm = BT_8723B_1ANT_COEX_ALGO_HID;
+			} else if (bt_link_info->hid_exist &&
+				   bt_link_info->pan_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = SCO + HID + PAN(HS)\n");
+					algorithm =
+					    BT_8723B_1ANT_COEX_ALGO_HID_A2DP;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = SCO + HID + PAN(EDR)\n");
+					algorithm =
+					    BT_8723B_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			} else if (bt_link_info->pan_exist &&
+				   bt_link_info->a2dp_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = SCO + A2DP + PAN(HS)\n");
+					algorithm = BT_8723B_1ANT_COEX_ALGO_SCO;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = SCO + A2DP + PAN(EDR) ==> HID\n");
+					algorithm =
+					    BT_8723B_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		} else {
+			if (bt_link_info->hid_exist &&
+			    bt_link_info->pan_exist &&
+			    bt_link_info->a2dp_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = HID + A2DP + PAN(HS)\n");
+					algorithm =
+					    BT_8723B_1ANT_COEX_ALGO_HID_A2DP;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = HID + A2DP + PAN(EDR)\n");
+					algorithm =
+					    BT_8723B_1ANT_COEX_ALGO_HID_A2DP_PANEDR;
+				}
+			}
+		}
+	} else if (numofdiffprofile >= 3) {
+		if (bt_link_info->sco_exist) {
+			if (bt_link_info->hid_exist &&
+			    bt_link_info->pan_exist &&
+			    bt_link_info->a2dp_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], Error!!! BT Profile = SCO + HID + A2DP + PAN(HS)\n");
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = SCO + HID + A2DP + PAN(EDR) ==>PAN(EDR)+HID\n");
+					algorithm =
+					    BT_8723B_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		}
+	}
+
+	return algorithm;
+}
+
+static void set_sw_penalty_tx_rate_adapt(struct btc_coexist *btcoexist,
+					 bool low_penalty_ra)
+{
+	u8 h2c_parameter[6] = {0};
+
+	h2c_parameter[0] = 0x6;	/* opCode, 0x6 = Retry_Penalty */
+
+	if (low_penalty_ra) {
+		h2c_parameter[1] |= BIT(0);
+		/*normal rate except MCS7/6/5, OFDM54/48/36 */
+		h2c_parameter[2] = 0x00;
+		h2c_parameter[3] = 0xf7;  /*MCS7 or OFDM54 */
+		h2c_parameter[4] = 0xf8;  /*MCS6 or OFDM48 */
+		h2c_parameter[5] = 0xf9;  /*MCS5 or OFDM36 */
+	}
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], set WiFi Low-Penalty Retry: %s",
+		  (low_penalty_ra ? "ON!!" : "OFF!!"));
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x69, 6, h2c_parameter);
+}
+
+static void halbtc8723b1ant_low_penalty_ra(struct btc_coexist *btcoexist,
+					   bool force_exec, bool low_penalty_ra)
+{
+	coex_dm->cur_low_penalty_ra = low_penalty_ra;
+
+	if (!force_exec) {
+		if (coex_dm->pre_low_penalty_ra == coex_dm->cur_low_penalty_ra)
+			return;
+	}
+	set_sw_penalty_tx_rate_adapt(btcoexist, coex_dm->cur_low_penalty_ra);
+
+	coex_dm->pre_low_penalty_ra = coex_dm->cur_low_penalty_ra;
+}
+
+static void halbtc8723b1ant_set_coex_table(struct btc_coexist *btcoexist,
+					   u32 val0x6c0, u32 val0x6c4,
+					   u32 val0x6c8, u8 val0x6cc)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+		  "[BTCoex], set coex table, set 0x6c0 = 0x%x\n", val0x6c0);
+	btcoexist->btc_write_4byte(btcoexist, 0x6c0, val0x6c0);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+		  "[BTCoex], set coex table, set 0x6c4 = 0x%x\n", val0x6c4);
+	btcoexist->btc_write_4byte(btcoexist, 0x6c4, val0x6c4);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+		  "[BTCoex], set coex table, set 0x6c8 = 0x%x\n", val0x6c8);
+	btcoexist->btc_write_4byte(btcoexist, 0x6c8, val0x6c8);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+		  "[BTCoex], set coex table, set 0x6cc = 0x%x\n", val0x6cc);
+	btcoexist->btc_write_1byte(btcoexist, 0x6cc, val0x6cc);
+}
+
+static void halbtc8723b1ant_coex_table(struct btc_coexist *btcoexist,
+				       bool force_exec, u32 val0x6c0,
+				       u32 val0x6c4, u32 val0x6c8,
+				       u8 val0x6cc)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
+		  "[BTCoex], %s write Coex Table 0x6c0 = 0x%x, 0x6c4 = 0x%x, 0x6cc = 0x%x\n",
+		  (force_exec ? "force to" : ""),
+		  val0x6c0, val0x6c4, val0x6cc);
+	coex_dm->cur_val0x6c0 = val0x6c0;
+	coex_dm->cur_val0x6c4 = val0x6c4;
+	coex_dm->cur_val0x6c8 = val0x6c8;
+	coex_dm->cur_val0x6cc = val0x6cc;
+
+	if (!force_exec) {
+		if ((coex_dm->pre_val0x6c0 == coex_dm->cur_val0x6c0) &&
+		    (coex_dm->pre_val0x6c4 == coex_dm->cur_val0x6c4) &&
+		    (coex_dm->pre_val0x6c8 == coex_dm->cur_val0x6c8) &&
+		    (coex_dm->pre_val0x6cc == coex_dm->cur_val0x6cc))
+			return;
+	}
+	halbtc8723b1ant_set_coex_table(btcoexist, val0x6c0, val0x6c4,
+				       val0x6c8, val0x6cc);
+
+	coex_dm->pre_val0x6c0 = coex_dm->cur_val0x6c0;
+	coex_dm->pre_val0x6c4 = coex_dm->cur_val0x6c4;
+	coex_dm->pre_val0x6c8 = coex_dm->cur_val0x6c8;
+	coex_dm->pre_val0x6cc = coex_dm->cur_val0x6cc;
+}
+
+static void coex_table_with_type(struct btc_coexist *btcoexist,
+				 bool force_exec, u8 type)
+{
+	switch (type) {
+	case 0:
+		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x55555555,
+					   0x55555555, 0xffffff, 0x3);
+		break;
+	case 1:
+		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x55555555,
+					   0x5a5a5a5a, 0xffffff, 0x3);
+		break;
+	case 2:
+		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x5a5a5a5a,
+					   0x5a5a5a5a, 0xffffff, 0x3);
+		break;
+	case 3:
+		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x55555555,
+					   0xaaaaaaaa, 0xffffff, 0x3);
+		break;
+	case 4:
+		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x55555555,
+					   0x5aaa5aaa, 0xffffff, 0x3);
+		break;
+	case 5:
+		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x5a5a5a5a,
+					   0xaaaa5a5a, 0xffffff, 0x3);
+		break;
+	case 6:
+		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x55555555,
+					   0xaaaa5a5a, 0xffffff, 0x3);
+		break;
+	case 7:
+		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0xaaaaaaaa,
+					   0xaaaaaaaa, 0xffffff, 0x3);
+		break;
+	default:
+		break;
+	}
+}
+
+static void set_fw_ignore_wlan_act(struct btc_coexist *btcoexist,
+				   bool enable)
+{
+	u8 h2c_parameter[1] = {0};
+
+	if (enable)
+		h2c_parameter[0] |= BIT(0);	/* function enable */
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], set FW for BT Ignore Wlan_Act, FW write 0x63 = 0x%x\n",
+		  h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x63, 1, h2c_parameter);
+}
+
+static void halbtc8723b1ant_ignore_wlan_act(struct btc_coexist *btcoexist,
+					    bool force_exec, bool enable)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], %s turn Ignore WlanAct %s\n",
+		  (force_exec ? "force to" : ""), (enable ? "ON" : "OFF"));
+	coex_dm->cur_ignore_wlan_act = enable;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], bPreIgnoreWlanAct = %d, bCurIgnoreWlanAct = %d!!\n",
+			  coex_dm->pre_ignore_wlan_act,
+			  coex_dm->cur_ignore_wlan_act);
+
+		if (coex_dm->pre_ignore_wlan_act ==
+		    coex_dm->cur_ignore_wlan_act)
+			return;
+	}
+	set_fw_ignore_wlan_act(btcoexist, enable);
+
+	coex_dm->pre_ignore_wlan_act = coex_dm->cur_ignore_wlan_act;
+}
+
+static void halbtc8723b1ant_set_fw_ps_tdma(struct btc_coexist *btcoexist,
+					   u8 byte1, u8 byte2, u8 byte3,
+					   u8 byte4, u8 byte5)
+{
+	u8 h2c_parameter[5] = {0};
+	u8 real_byte1 = byte1, real_byte5 = byte5;
+	bool ap_enable = false;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_AP_MODE_ENABLE,
+			   &ap_enable);
+
+	if (ap_enable) {
+		if ((byte1 & BIT(4)) && !(byte1 & BIT(5))) {
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+				  "[BTCoex], FW for 1Ant AP mode\n");
+			real_byte1 &= ~BIT(4);
+			real_byte1 |= BIT(5);
+
+			real_byte5 |= BIT(5);
+			real_byte5 &= ~BIT(6);
+		}
+	}
+
+	h2c_parameter[0] = real_byte1;
+	h2c_parameter[1] = byte2;
+	h2c_parameter[2] = byte3;
+	h2c_parameter[3] = byte4;
+	h2c_parameter[4] = real_byte5;
+
+	coex_dm->ps_tdma_para[0] = real_byte1;
+	coex_dm->ps_tdma_para[1] = byte2;
+	coex_dm->ps_tdma_para[2] = byte3;
+	coex_dm->ps_tdma_para[3] = byte4;
+	coex_dm->ps_tdma_para[4] = real_byte5;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], PS-TDMA H2C cmd = 0x%x%08x\n",
+		  h2c_parameter[0],
+		  h2c_parameter[1] << 24 |
+		  h2c_parameter[2] << 16 |
+		  h2c_parameter[3] << 8 |
+		  h2c_parameter[4]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x60, 5, h2c_parameter);
+}
+
+static void halbtc8723b1ant_set_lps_rpwm(struct btc_coexist *btcoexist,
+					 u8 lps_val, u8 rpwm_val)
+{
+	u8 lps = lps_val;
+	u8 rpwm = rpwm_val;
+
+	btcoexist->btc_set(btcoexist, BTC_SET_U1_LPS_VAL, &lps);
+	btcoexist->btc_set(btcoexist, BTC_SET_U1_RPWM_VAL, &rpwm);
+}
+
+static void halbtc8723b1ant_lpsrpwm(struct btc_coexist *btcoexist,
+				    bool force_exec,
+				    u8 lps_val, u8 rpwm_val)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], %s set lps/rpwm = 0x%x/0x%x\n",
+		  (force_exec ? "force to" : ""), lps_val, rpwm_val);
+	coex_dm->cur_lps = lps_val;
+	coex_dm->cur_rpwm = rpwm_val;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], LPS-RxBeaconMode = 0x%x, LPS-RPWM = 0x%x!!\n",
+			  coex_dm->cur_lps, coex_dm->cur_rpwm);
+
+		if ((coex_dm->pre_lps == coex_dm->cur_lps) &&
+		    (coex_dm->pre_rpwm == coex_dm->cur_rpwm)) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+				  "[BTCoex], LPS-RPWM_Last = 0x%x, LPS-RPWM_Now = 0x%x!!\n",
+				  coex_dm->pre_rpwm, coex_dm->cur_rpwm);
+
+			return;
+		}
+	}
+	halbtc8723b1ant_set_lps_rpwm(btcoexist, lps_val, rpwm_val);
+
+	coex_dm->pre_lps = coex_dm->cur_lps;
+	coex_dm->pre_rpwm = coex_dm->cur_rpwm;
+}
+
+static void halbtc8723b1ant_sw_mechanism(struct btc_coexist *btcoexist,
+					 bool low_penalty_ra)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+		  "[BTCoex], SM[LpRA] = %d\n", low_penalty_ra);
+
+	halbtc8723b1ant_low_penalty_ra(btcoexist, NORMAL_EXEC, low_penalty_ra);
+}
+
+static void halbtc8723b1ant_setantpath(struct btc_coexist *btcoexist,
+				       u8 ant_pos_type, bool init_hw_cfg,
+				bool wifi_off)
+{
+	struct btc_board_info *brd_info = &btcoexist->board_info;
+	u32 fw_ver = 0, u32tmp = 0;
+	bool pg_ext_switch = false;
+	bool use_ext_switch = false;
+	u8 h2c_parameter[2] = {0};
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_EXT_SWITCH, &pg_ext_switch);
+	/* [31:16] = fw ver, [15:0] = fw sub ver */
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
+
+	if ((fw_ver < 0xc0000) || pg_ext_switch)
+		use_ext_switch = true;
+
+	if (init_hw_cfg) {
+		/*BT select s0/s1 is controlled by WiFi */
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x67, 0x20, 0x1);
+
+		/*Force GNT_BT to Normal */
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x765, 0x18, 0x0);
+	} else if (wifi_off) {
+		/*Force GNT_BT to High */
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x765, 0x18, 0x3);
+		/*BT select s0/s1 is controlled by BT */
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x67, 0x20, 0x0);
+
+		/* 0x4c[24:23] = 00, Set Antenna control by BT_RFE_CTRL
+		 * BT Vendor 0xac = 0xf002 */
+		u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
+		u32tmp &= ~BIT(23);
+		u32tmp &= ~BIT(24);
+		btcoexist->btc_write_4byte(btcoexist, 0x4c, u32tmp);
+	}
+
+	if (use_ext_switch) {
+		if (init_hw_cfg) {
+			/* 0x4c[23] = 0, 0x4c[24] = 1  Antenna ctrl by WL/BT */
+			u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
+			u32tmp &= ~BIT(23);
+			u32tmp |= BIT(24);
+			btcoexist->btc_write_4byte(btcoexist, 0x4c, u32tmp);
+
+			if (brd_info->btdm_ant_pos ==
+			    BTC_ANTENNA_AT_MAIN_PORT) {
+				/* Main Ant to  BT for IPS case 0x4c[23] = 1 */
+				btcoexist->btc_write_1byte_bitmask(btcoexist,
+								   0x64, 0x1,
+								   0x1);
+
+				/*tell firmware "no antenna inverse"*/
+				h2c_parameter[0] = 0;
+				h2c_parameter[1] = 1;  /*ext switch type*/
+				btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
+							h2c_parameter);
+			} else {
+				/*Aux Ant to  BT for IPS case 0x4c[23] = 1 */
+				btcoexist->btc_write_1byte_bitmask(btcoexist,
+								   0x64, 0x1,
+								   0x0);
+
+				/*tell firmware "antenna inverse"*/
+				h2c_parameter[0] = 1;
+				h2c_parameter[1] = 1;  /*ext switch type*/
+				btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
+							h2c_parameter);
+			}
+		}
+
+		/* fixed internal switch first*/
+		/* fixed internal switch S1->WiFi, S0->BT*/
+		if (brd_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
+			btcoexist->btc_write_2byte(btcoexist, 0x948, 0x0);
+		else/* fixed internal switch S0->WiFi, S1->BT*/
+			btcoexist->btc_write_2byte(btcoexist, 0x948, 0x280);
+
+		/* ext switch setting */
+		switch (ant_pos_type) {
+		case BTC_ANT_PATH_WIFI:
+			if (brd_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
+				btcoexist->btc_write_1byte_bitmask(btcoexist,
+								   0x92c, 0x3,
+								   0x1);
+			else
+				btcoexist->btc_write_1byte_bitmask(btcoexist,
+								   0x92c, 0x3,
+								   0x2);
+			break;
+		case BTC_ANT_PATH_BT:
+			if (brd_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
+				btcoexist->btc_write_1byte_bitmask(btcoexist,
+								   0x92c, 0x3,
+								   0x2);
+			else
+				btcoexist->btc_write_1byte_bitmask(btcoexist,
+								   0x92c, 0x3,
+								   0x1);
+			break;
+		default:
+		case BTC_ANT_PATH_PTA:
+			if (brd_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
+				btcoexist->btc_write_1byte_bitmask(btcoexist,
+								   0x92c, 0x3,
+								   0x1);
+			else
+				btcoexist->btc_write_1byte_bitmask(btcoexist,
+								   0x92c, 0x3,
+								   0x2);
+			break;
+		}
+
+	} else {
+		if (init_hw_cfg) {
+			/* 0x4c[23] = 1, 0x4c[24] = 0  Antenna control by 0x64*/
+			u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
+			u32tmp |= BIT(23);
+			u32tmp &= ~BIT(24);
+			btcoexist->btc_write_4byte(btcoexist, 0x4c, u32tmp);
+
+			if (brd_info->btdm_ant_pos ==
+			    BTC_ANTENNA_AT_MAIN_PORT) {
+				/*Main Ant to  WiFi for IPS case 0x4c[23] = 1*/
+				btcoexist->btc_write_1byte_bitmask(btcoexist,
+								   0x64, 0x1,
+								   0x0);
+
+				/*tell firmware "no antenna inverse"*/
+				h2c_parameter[0] = 0;
+				h2c_parameter[1] = 0;  /*internal switch type*/
+				btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
+							h2c_parameter);
+			} else {
+				/*Aux Ant to  BT for IPS case 0x4c[23] = 1*/
+				btcoexist->btc_write_1byte_bitmask(btcoexist,
+								   0x64, 0x1,
+								   0x1);
+
+				/*tell firmware "antenna inverse"*/
+				h2c_parameter[0] = 1;
+				h2c_parameter[1] = 0;  /*internal switch type*/
+				btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
+							h2c_parameter);
+			}
+		}
+
+		/* fixed external switch first*/
+		/*Main->WiFi, Aux->BT*/
+		if (brd_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x92c,
+							   0x3, 0x1);
+		else/*Main->BT, Aux->WiFi */
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x92c,
+							   0x3, 0x2);
+
+		/* internal switch setting*/
+		switch (ant_pos_type) {
+		case BTC_ANT_PATH_WIFI:
+			if (brd_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
+				btcoexist->btc_write_2byte(btcoexist, 0x948,
+							   0x0);
+			else
+				btcoexist->btc_write_2byte(btcoexist, 0x948,
+							   0x280);
+			break;
+		case BTC_ANT_PATH_BT:
+			if (brd_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
+				btcoexist->btc_write_2byte(btcoexist, 0x948,
+							   0x280);
+			else
+				btcoexist->btc_write_2byte(btcoexist, 0x948,
+							   0x0);
+			break;
+		default:
+		case BTC_ANT_PATH_PTA:
+			if (brd_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
+				btcoexist->btc_write_2byte(btcoexist, 0x948,
+							   0x200);
+			else
+				btcoexist->btc_write_2byte(btcoexist, 0x948,
+							   0x80);
+			break;
+		}
+	}
+}
+
+static void halbtc8723b1ant_ps_tdma(struct btc_coexist *btcoexist,
+				    bool force_exec, bool turn_on, u8 type)
+{
+	bool wifi_busy = false;
+	u8 rssi_adjust_val = 0;
+
+	coex_dm->cur_ps_tdma_on = turn_on;
+	coex_dm->cur_ps_tdma = type;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+
+	if (!force_exec) {
+		if (coex_dm->cur_ps_tdma_on)
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+				  "[BTCoex], ******** TDMA(on, %d) *********\n",
+				  coex_dm->cur_ps_tdma);
+		else
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+				  "[BTCoex], ******** TDMA(off, %d) ********\n",
+				  coex_dm->cur_ps_tdma);
+
+		if ((coex_dm->pre_ps_tdma_on == coex_dm->cur_ps_tdma_on) &&
+		    (coex_dm->pre_ps_tdma == coex_dm->cur_ps_tdma))
+			return;
+	}
+	if (turn_on) {
+		switch (type) {
+		default:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x1a,
+						       0x1a, 0x0, 0x50);
+			break;
+		case 1:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x3a,
+						       0x03, 0x10, 0x50);
+
+			rssi_adjust_val = 11;
+			break;
+		case 2:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x2b,
+						       0x03, 0x10, 0x50);
+			rssi_adjust_val = 14;
+			break;
+		case 3:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x1d,
+						       0x1d, 0x0, 0x52);
+			break;
+		case 4:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x93, 0x15,
+						       0x3, 0x14, 0x0);
+			rssi_adjust_val = 17;
+			break;
+		case 5:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x61, 0x15,
+						       0x3, 0x11, 0x10);
+			break;
+		case 6:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x61, 0x20,
+						       0x3, 0x11, 0x13);
+			break;
+		case 7:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x13, 0xc,
+						       0x5, 0x0, 0x0);
+			break;
+		case 8:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x93, 0x25,
+						       0x3, 0x10, 0x0);
+			break;
+		case 9:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51,  0x21,
+						       0x3, 0x10, 0x50);
+			rssi_adjust_val = 18;
+			break;
+		case 10:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x13, 0xa,
+						       0xa, 0x0, 0x40);
+			break;
+		case 11:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x15,
+						       0x03, 0x10, 0x50);
+			rssi_adjust_val = 20;
+			break;
+		case 12:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x0a,
+						       0x0a, 0x0, 0x50);
+			break;
+		case 13:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x15,
+						       0x15, 0x0, 0x50);
+			break;
+		case 14:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x21,
+						       0x3, 0x10, 0x52);
+			break;
+		case 15:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x13, 0xa,
+						       0x3, 0x8, 0x0);
+			break;
+		case 16:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x93, 0x15,
+						       0x3, 0x10, 0x0);
+			rssi_adjust_val = 18;
+			break;
+		case 18:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x93, 0x25,
+						       0x3, 0x10, 0x0);
+			rssi_adjust_val = 14;
+			break;
+		case 20:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x61, 0x35,
+						       0x03, 0x11, 0x10);
+			break;
+		case 21:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x61, 0x25,
+						       0x03, 0x11, 0x11);
+			break;
+		case 22:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x61, 0x25,
+						       0x03, 0x11, 0x10);
+			break;
+		case 23:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x25,
+						       0x3, 0x31, 0x18);
+			rssi_adjust_val = 22;
+			break;
+		case 24:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x15,
+						       0x3, 0x31, 0x18);
+			rssi_adjust_val = 22;
+			break;
+		case 25:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xe3, 0xa,
+						       0x3, 0x31, 0x18);
+			rssi_adjust_val = 22;
+			break;
+		case 26:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xe3, 0xa,
+						       0x3, 0x31, 0x18);
+			rssi_adjust_val = 22;
+			break;
+		case 27:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x25,
+						       0x3, 0x31, 0x98);
+			rssi_adjust_val = 22;
+			break;
+		case 28:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x69, 0x25,
+						       0x3, 0x31, 0x0);
+			break;
+		case 29:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xab, 0x1a,
+						       0x1a, 0x1, 0x10);
+			break;
+		case 30:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x14,
+						       0x3, 0x10, 0x50);
+			break;
+		case 31:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xd3, 0x1a,
+						       0x1a, 0, 0x58);
+			break;
+		case 32:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x61, 0xa,
+						       0x3, 0x10, 0x0);
+			break;
+		case 33:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xa3, 0x25,
+						       0x3, 0x30, 0x90);
+			break;
+		case 34:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x53, 0x1a,
+						       0x1a, 0x0, 0x10);
+			break;
+		case 35:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x63, 0x1a,
+						       0x1a, 0x0, 0x10);
+			break;
+		case 36:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xd3, 0x12,
+						       0x3, 0x14, 0x50);
+			break;
+		/* SoftAP only with no sta associated, BT disable ,
+		 * TDMA mode for power saving
+		 * here softap mode screen off will cost 70-80mA for phone */
+		case 40:
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x23, 0x18,
+						       0x00, 0x10, 0x24);
+			break;
+		}
+	} else {
+		switch (type) {
+		case 8: /*PTA Control */
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x8, 0x0,
+						       0x0, 0x0, 0x0);
+			halbtc8723b1ant_setantpath(btcoexist, BTC_ANT_PATH_PTA,
+						   false, false);
+			break;
+		case 0:
+		default:  /*Software control, Antenna at BT side */
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x0, 0x0,
+						       0x0, 0x0, 0x0);
+			halbtc8723b1ant_setantpath(btcoexist, BTC_ANT_PATH_BT,
+						   false, false);
+			break;
+		case 9:   /*Software control, Antenna at WiFi side */
+			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x0, 0x0,
+						       0x0, 0x0, 0x0);
+			halbtc8723b1ant_setantpath(btcoexist, BTC_ANT_PATH_WIFI,
+						   false, false);
+			break;
+		}
+	}
+	rssi_adjust_val = 0;
+	btcoexist->btc_set(btcoexist,
+			   BTC_SET_U1_RSSI_ADJ_VAL_FOR_1ANT_COEX_TYPE,
+			   &rssi_adjust_val);
+
+	/* update pre state */
+	coex_dm->pre_ps_tdma_on = coex_dm->cur_ps_tdma_on;
+	coex_dm->pre_ps_tdma = coex_dm->cur_ps_tdma;
+}
+
+static bool halbtc8723b1ant_is_common_action(struct btc_coexist *btcoexist)
+{
+	bool commom = false, wifi_connected = false;
+	bool wifi_busy = false;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+			   &wifi_connected);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+
+	if (!wifi_connected &&
+	    BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE == coex_dm->bt_status) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Wifi non connected-idle + BT non connected-idle!!\n");
+		halbtc8723b1ant_sw_mechanism(btcoexist, false);
+		commom = true;
+	} else if (wifi_connected &&
+		   (BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE ==
+		    coex_dm->bt_status)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Wifi connected + BT non connected-idle!!\n");
+		halbtc8723b1ant_sw_mechanism(btcoexist, false);
+		commom = true;
+	} else if (!wifi_connected &&
+		   (BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE ==
+		    coex_dm->bt_status)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Wifi non connected-idle + BT connected-idle!!\n");
+		halbtc8723b1ant_sw_mechanism(btcoexist, false);
+		commom = true;
+	} else if (wifi_connected &&
+		   (BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE ==
+		    coex_dm->bt_status)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Wifi connected + BT connected-idle!!\n");
+		halbtc8723b1ant_sw_mechanism(btcoexist, false);
+		commom = true;
+	} else if (!wifi_connected &&
+		   (BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE !=
+		    coex_dm->bt_status)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Wifi non connected-idle + BT Busy!!\n");
+		halbtc8723b1ant_sw_mechanism(btcoexist, false);
+		commom = true;
+	} else {
+		if (wifi_busy)
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Wifi Connected-Busy + BT Busy!!\n");
+		else
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Wifi Connected-Idle + BT Busy!!\n");
+
+		commom = false;
+	}
+
+	return commom;
+}
+
+static void tdma_duration_adjust_for_acl(struct btc_coexist *btcoexist,
+					 u8 wifi_status)
+{
+	static s32 up, dn, m, n, wait_count;
+	/* 0: no change, +1: increase WiFi duration,
+	 * -1: decrease WiFi duration */
+	s32 result;
+	u8 retry_count = 0, bt_info_ext;
+	bool wifi_busy = false;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], TdmaDurationAdjustForAcl()\n");
+
+	if (BT_8723B_1ANT_WIFI_STATUS_CONNECTED_BUSY == wifi_status)
+		wifi_busy = true;
+	else
+		wifi_busy = false;
+
+	if ((BT_8723B_1ANT_WIFI_STATUS_NON_CONNECTED_ASSO_AUTH_SCAN ==
+							 wifi_status) ||
+	    (BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SCAN == wifi_status) ||
+	    (BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SPECIAL_PKT == wifi_status)) {
+		if (coex_dm->cur_ps_tdma != 1 && coex_dm->cur_ps_tdma != 2 &&
+		    coex_dm->cur_ps_tdma != 3 && coex_dm->cur_ps_tdma != 9) {
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 9);
+			coex_dm->ps_tdma_du_adj_type = 9;
+
+			up = 0;
+			dn = 0;
+			m = 1;
+			n = 3;
+			result = 0;
+			wait_count = 0;
+		}
+		return;
+	}
+
+	if (!coex_dm->auto_tdma_adjust) {
+		coex_dm->auto_tdma_adjust = true;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], first run TdmaDurationAdjust()!!\n");
+
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 2);
+		coex_dm->ps_tdma_du_adj_type = 2;
+
+		up = 0;
+		dn = 0;
+		m = 1;
+		n = 3;
+		result = 0;
+		wait_count = 0;
+	} else {
+		/*accquire the BT TRx retry count from BT_Info byte2 */
+		retry_count = coex_sta->bt_retry_cnt;
+		bt_info_ext = coex_sta->bt_info_ext;
+		result = 0;
+		wait_count++;
+		/* no retry in the last 2-second duration */
+		if (retry_count == 0) {
+			up++;
+			dn--;
+
+			if (dn <= 0)
+				dn = 0;
+
+			if (up >= n) {
+				wait_count = 0;
+				n = 3;
+				up = 0;
+				dn = 0;
+				result = 1;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_TRACE_FW_DETAIL,
+					  "[BTCoex], Increase wifi duration!!\n");
+			}
+		} else if (retry_count <= 3) {
+			up--;
+			dn++;
+
+			if (up <= 0)
+				up = 0;
+
+			if (dn == 2) {
+				if (wait_count <= 2)
+					m++;
+				else
+					m = 1;
+
+				if (m >= 20)
+					m = 20;
+
+				n = 3 * m;
+				up = 0;
+				dn = 0;
+				wait_count = 0;
+				result = -1;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_TRACE_FW_DETAIL,
+					  "[BTCoex], Decrease wifi duration for retryCounter<3!!\n");
+			}
+		} else {
+			if (wait_count == 1)
+				m++;
+			else
+				m = 1;
+
+			if (m >= 20)
+				m = 20;
+
+			n = 3 * m;
+			up = 0;
+			dn = 0;
+			wait_count = 0;
+			result = -1;
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+				  "[BTCoex], Decrease wifi duration for retryCounter>3!!\n");
+		}
+
+		if (result == -1) {
+			if ((BT_INFO_8723B_1ANT_A2DP_BASIC_RATE(bt_info_ext)) &&
+			    ((coex_dm->cur_ps_tdma == 1) ||
+			     (coex_dm->cur_ps_tdma == 2))) {
+				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 9);
+				coex_dm->ps_tdma_du_adj_type = 9;
+			} else if (coex_dm->cur_ps_tdma == 1) {
+				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 2);
+				coex_dm->ps_tdma_du_adj_type = 2;
+			} else if (coex_dm->cur_ps_tdma == 2) {
+				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 9);
+				coex_dm->ps_tdma_du_adj_type = 9;
+			} else if (coex_dm->cur_ps_tdma == 9) {
+				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 11);
+				coex_dm->ps_tdma_du_adj_type = 11;
+			}
+		} else if (result == 1) {
+			if ((BT_INFO_8723B_1ANT_A2DP_BASIC_RATE(bt_info_ext)) &&
+			    ((coex_dm->cur_ps_tdma == 1) ||
+			     (coex_dm->cur_ps_tdma == 2))) {
+				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 9);
+				coex_dm->ps_tdma_du_adj_type = 9;
+			} else if (coex_dm->cur_ps_tdma == 11) {
+				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 9);
+				coex_dm->ps_tdma_du_adj_type = 9;
+			} else if (coex_dm->cur_ps_tdma == 9) {
+				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 2);
+				coex_dm->ps_tdma_du_adj_type = 2;
+			} else if (coex_dm->cur_ps_tdma == 2) {
+				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 1);
+				coex_dm->ps_tdma_du_adj_type = 1;
+			}
+		} else {	  /*no change */
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+				  "[BTCoex],********* TDMA(on, %d) ********\n",
+				  coex_dm->cur_ps_tdma);
+		}
+
+		if (coex_dm->cur_ps_tdma != 1 && coex_dm->cur_ps_tdma != 2 &&
+		    coex_dm->cur_ps_tdma != 9 && coex_dm->cur_ps_tdma != 11) {
+			/* recover to previous adjust type */
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true,
+						coex_dm->ps_tdma_du_adj_type);
+		}
+	}
+}
+
+static void pstdmacheckforpowersavestate(struct btc_coexist *btcoexist,
+					 bool new_ps_state)
+{
+	u8 lps_mode = 0x0;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_LPS_MODE, &lps_mode);
+
+	if (lps_mode) {	/* already under LPS state */
+		if (new_ps_state) {
+			/* keep state under LPS, do nothing. */
+		} else {
+			/* will leave LPS state, turn off psTdma first */
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						false, 0);
+		}
+	} else {	/* NO PS state */
+		if (new_ps_state) {
+			/* will enter LPS state, turn off psTdma first */
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						false, 0);
+		} else {
+			/* keep state under NO PS state, do nothing. */
+		}
+	}
+}
+
+static void halbtc8723b1ant_power_save_state(struct btc_coexist *btcoexist,
+					     u8 ps_type, u8 lps_val,
+					     u8 rpwm_val)
+{
+	bool low_pwr_disable = false;
+
+	switch (ps_type) {
+	case BTC_PS_WIFI_NATIVE:
+		/* recover to original 32k low power setting */
+		low_pwr_disable = false;
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
+				   &low_pwr_disable);
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_NORMAL_LPS, NULL);
+		break;
+	case BTC_PS_LPS_ON:
+		pstdmacheckforpowersavestate(btcoexist, true);
+		halbtc8723b1ant_lpsrpwm(btcoexist, NORMAL_EXEC, lps_val,
+					rpwm_val);
+		/* when coex force to enter LPS, do not enter 32k low power. */
+		low_pwr_disable = true;
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
+				   &low_pwr_disable);
+		/* power save must executed before psTdma.	 */
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_ENTER_LPS, NULL);
+		break;
+	case BTC_PS_LPS_OFF:
+		pstdmacheckforpowersavestate(btcoexist, false);
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
+		break;
+	default:
+		break;
+	}
+}
+
+/***************************************************
+ *
+ *	Software Coex Mechanism start
+ *
+ ***************************************************/
+/* SCO only or SCO+PAN(HS) */
+static void halbtc8723b1ant_action_sco(struct btc_coexist *btcoexist)
+{
+	halbtc8723b1ant_sw_mechanism(btcoexist, true);
+}
+
+static void halbtc8723b1ant_action_hid(struct btc_coexist *btcoexist)
+{
+	halbtc8723b1ant_sw_mechanism(btcoexist, true);
+}
+
+/*A2DP only / PAN(EDR) only/ A2DP+PAN(HS) */
+static void halbtc8723b1ant_action_a2dp(struct btc_coexist *btcoexist)
+{
+	halbtc8723b1ant_sw_mechanism(btcoexist, false);
+}
+
+static void halbtc8723b1ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
+{
+	halbtc8723b1ant_sw_mechanism(btcoexist, false);
+}
+
+static void halbtc8723b1ant_action_pan_edr(struct btc_coexist *btcoexist)
+{
+	halbtc8723b1ant_sw_mechanism(btcoexist, false);
+}
+
+/* PAN(HS) only */
+static void halbtc8723b1ant_action_pan_hs(struct btc_coexist *btcoexist)
+{
+	halbtc8723b1ant_sw_mechanism(btcoexist, false);
+}
+
+/*PAN(EDR)+A2DP */
+static void halbtc8723b1ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
+{
+	halbtc8723b1ant_sw_mechanism(btcoexist, false);
+}
+
+static void halbtc8723b1ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
+{
+	halbtc8723b1ant_sw_mechanism(btcoexist, true);
+}
+
+/* HID+A2DP+PAN(EDR) */
+static void action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
+{
+	halbtc8723b1ant_sw_mechanism(btcoexist, true);
+}
+
+static void halbtc8723b1ant_action_hid_a2dp(struct btc_coexist *btcoexist)
+{
+	halbtc8723b1ant_sw_mechanism(btcoexist, true);
+}
+
+/*****************************************************
+ *
+ *	Non-Software Coex Mechanism start
+ *
+ *****************************************************/
+static void halbtc8723b1ant_action_wifi_multiport(struct btc_coexist *btcoexist)
+{
+	halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+					 0x0, 0x0);
+
+	halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
+	coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+}
+
+static void halbtc8723b1ant_action_hs(struct btc_coexist *btcoexist)
+{
+	halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
+	coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+}
+
+static void halbtc8723b1ant_action_bt_inquiry(struct btc_coexist *btcoexist)
+{
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	bool wifi_connected = false, ap_enable = false;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_AP_MODE_ENABLE,
+			   &ap_enable);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+			   &wifi_connected);
+
+	if (!wifi_connected) {
+		halbtc8723b1ant_power_save_state(btcoexist,
+						 BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+	} else if (bt_link_info->sco_exist || bt_link_info->hid_only) {
+		/* SCO/HID-only busy */
+		halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+						 0x0, 0x0);
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 32);
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+	} else {
+		if (ap_enable)
+			halbtc8723b1ant_power_save_state(btcoexist,
+							 BTC_PS_WIFI_NATIVE,
+							 0x0, 0x0);
+		else
+			halbtc8723b1ant_power_save_state(btcoexist,
+							 BTC_PS_LPS_ON,
+							 0x50, 0x4);
+
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 30);
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+	}
+}
+
+static void action_bt_sco_hid_only_busy(struct btc_coexist *btcoexist,
+					u8 wifi_status)
+{
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	bool wifi_connected = false;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+			   &wifi_connected);
+
+	/* tdma and coex table */
+
+	if (bt_link_info->sco_exist) {
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+	} else { /* HID */
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 6);
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 5);
+	}
+}
+
+static void halbtc8723b1ant_action_wifi_connected_bt_acl_busy(
+					struct btc_coexist *btcoexist,
+					u8 wifi_status)
+{
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	u8 rssi_state;
+
+	rssi_state = halbtc8723b1ant_bt_rssi_state(2, 28, 0);
+
+	if (bt_link_info->hid_only) {  /*HID */
+		action_bt_sco_hid_only_busy(btcoexist, wifi_status);
+		coex_dm->auto_tdma_adjust = false;
+		return;
+	} else if (bt_link_info->a2dp_only) { /*A2DP */
+		if (BT_8723B_1ANT_WIFI_STATUS_CONNECTED_IDLE == wifi_status) {
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false,
+						8);
+			coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+			coex_dm->auto_tdma_adjust = false;
+		} else if ((rssi_state == BTC_RSSI_STATE_HIGH) ||
+			   (rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			tdma_duration_adjust_for_acl(btcoexist, wifi_status);
+			coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		} else { /*for low BT RSSI */
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 11);
+			coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+			coex_dm->auto_tdma_adjust = false;
+		}
+	} else if (bt_link_info->hid_exist &&
+		   bt_link_info->a2dp_exist) { /*HID+A2DP */
+		if ((rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 14);
+			coex_dm->auto_tdma_adjust = false;
+		} else { /*for low BT RSSI*/
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 14);
+			coex_dm->auto_tdma_adjust = false;
+		}
+
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 6);
+	 /*PAN(OPP, FTP), HID+PAN(OPP, FTP) */
+	} else if (bt_link_info->pan_only ||
+		   (bt_link_info->hid_exist && bt_link_info->pan_exist)) {
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 3);
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 6);
+		coex_dm->auto_tdma_adjust = false;
+	 /*A2DP+PAN(OPP, FTP), HID+A2DP+PAN(OPP, FTP)*/
+	} else if ((bt_link_info->a2dp_exist && bt_link_info->pan_exist) ||
+		   (bt_link_info->hid_exist && bt_link_info->a2dp_exist &&
+		    bt_link_info->pan_exist)) {
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 13);
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		coex_dm->auto_tdma_adjust = false;
+	} else {
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 11);
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		coex_dm->auto_tdma_adjust = false;
+	}
+}
+
+static void action_wifi_not_connected(struct btc_coexist *btcoexist)
+{
+	/* power save state */
+	halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+					 0x0, 0x0);
+
+	/* tdma and coex table */
+	halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
+	coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
+}
+
+static void action_wifi_not_connected_scan(struct btc_coexist *btcoexist)
+{
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+					 0x0, 0x0);
+
+	/* tdma and coex table */
+	if (BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
+		if (bt_link_info->a2dp_exist && bt_link_info->pan_exist) {
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 22);
+			coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		} else if (bt_link_info->pan_only) {
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 20);
+			coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+		} else {
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 20);
+			coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		}
+	} else if ((BT_8723B_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
+		   (BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY ==
+		    coex_dm->bt_status)) {
+		action_bt_sco_hid_only_busy(btcoexist,
+				BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SCAN);
+	} else {
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+	}
+}
+
+static void action_wifi_not_connected_asso_auth(struct btc_coexist *btcoexist)
+{
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+
+	halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+					 0x0, 0x0);
+
+	if ((BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE == coex_dm->bt_status) ||
+	    (bt_link_info->sco_exist) || (bt_link_info->hid_only) ||
+	    (bt_link_info->a2dp_only) || (bt_link_info->pan_only)) {
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
+	} else {
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 20);
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+	}
+}
+
+static void action_wifi_connected_scan(struct btc_coexist *btcoexist)
+{
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+
+	halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+					 0x0, 0x0);
+
+	/* tdma and coex table */
+	if (BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
+		if (bt_link_info->a2dp_exist && bt_link_info->pan_exist) {
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 22);
+			coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		} else if (bt_link_info->pan_only) {
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 20);
+			coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+		} else {
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 20);
+			coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		}
+	} else if ((BT_8723B_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
+		   (BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY ==
+		    coex_dm->bt_status)) {
+		action_bt_sco_hid_only_busy(btcoexist,
+				BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SCAN);
+	} else {
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+	}
+}
+
+static void action_wifi_connected_special_packet(struct btc_coexist *btcoexist)
+{
+	bool hs_connecting = false;
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_CONNECTING, &hs_connecting);
+
+	halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+					 0x0, 0x0);
+
+	/* tdma and coex table */
+	if ((BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE == coex_dm->bt_status) ||
+	    (bt_link_info->sco_exist) || (bt_link_info->hid_only) ||
+	    (bt_link_info->a2dp_only) || (bt_link_info->pan_only)) {
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
+	} else {
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 20);
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+	}
+}
+
+static void halbtc8723b1ant_action_wifi_connected(struct btc_coexist *btcoexist)
+{
+	bool wifi_busy = false;
+	bool scan = false, link = false, roam = false;
+	bool under_4way = false, ap_enable = false;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+		  "[BTCoex], CoexForWifiConnect() ===>\n");
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS,
+			   &under_4way);
+	if (under_4way) {
+		action_wifi_connected_special_packet(btcoexist);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], CoexForWifiConnect(), return for wifi is under 4way<===\n");
+		return;
+	}
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
+
+	if (scan || link || roam) {
+		if (scan)
+			action_wifi_connected_scan(btcoexist);
+		else
+			action_wifi_connected_special_packet(btcoexist);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], CoexForWifiConnect(), return for wifi is under scan<===\n");
+		return;
+	}
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_AP_MODE_ENABLE,
+			   &ap_enable);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+	/* power save state */
+	if (!ap_enable &&
+	    BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status &&
+	    !btcoexist->bt_link_info.hid_only) {
+		if (!wifi_busy && btcoexist->bt_link_info.a2dp_only)
+			halbtc8723b1ant_power_save_state(btcoexist,
+							 BTC_PS_WIFI_NATIVE,
+							 0x0, 0x0);
+		else
+			halbtc8723b1ant_power_save_state(btcoexist,
+							 BTC_PS_LPS_ON,
+							 0x50, 0x4);
+	} else {
+		halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+						 0x0, 0x0);
+	}
+	/* tdma and coex table */
+	if (!wifi_busy) {
+		if (BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
+			halbtc8723b1ant_action_wifi_connected_bt_acl_busy(btcoexist,
+				      BT_8723B_1ANT_WIFI_STATUS_CONNECTED_IDLE);
+		} else if ((BT_8723B_1ANT_BT_STATUS_SCO_BUSY ==
+						coex_dm->bt_status) ||
+			   (BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY ==
+						coex_dm->bt_status)) {
+			action_bt_sco_hid_only_busy(btcoexist,
+				     BT_8723B_1ANT_WIFI_STATUS_CONNECTED_IDLE);
+		} else {
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						false, 8);
+			coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+		}
+	} else {
+		if (BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
+			halbtc8723b1ant_action_wifi_connected_bt_acl_busy(btcoexist,
+				    BT_8723B_1ANT_WIFI_STATUS_CONNECTED_BUSY);
+		} else if ((BT_8723B_1ANT_BT_STATUS_SCO_BUSY ==
+			    coex_dm->bt_status) ||
+			   (BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY ==
+			    coex_dm->bt_status)) {
+			action_bt_sco_hid_only_busy(btcoexist,
+				    BT_8723B_1ANT_WIFI_STATUS_CONNECTED_BUSY);
+		} else {
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
+			coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+		}
+	}
+}
+
+static void run_sw_coexist_mechanism(struct btc_coexist *btcoexist)
+{
+	u8 algorithm = 0;
+
+	algorithm = halbtc8723b1ant_action_algorithm(btcoexist);
+	coex_dm->cur_algorithm = algorithm;
+
+	if (!halbtc8723b1ant_is_common_action(btcoexist)) {
+		switch (coex_dm->cur_algorithm) {
+		case BT_8723B_1ANT_COEX_ALGO_SCO:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = SCO.\n");
+			halbtc8723b1ant_action_sco(btcoexist);
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_HID:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = HID.\n");
+			halbtc8723b1ant_action_hid(btcoexist);
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = A2DP.\n");
+			halbtc8723b1ant_action_a2dp(btcoexist);
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_A2DP_PANHS:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = A2DP+PAN(HS).\n");
+			halbtc8723b1ant_action_a2dp_pan_hs(btcoexist);
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_PANEDR:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = PAN(EDR).\n");
+			halbtc8723b1ant_action_pan_edr(btcoexist);
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_PANHS:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = HS mode.\n");
+			halbtc8723b1ant_action_pan_hs(btcoexist);
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_PANEDR_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = PAN+A2DP.\n");
+			halbtc8723b1ant_action_pan_edr_a2dp(btcoexist);
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_PANEDR_HID:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = PAN(EDR)+HID.\n");
+			halbtc8723b1ant_action_pan_edr_hid(btcoexist);
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_HID_A2DP_PANEDR:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = HID+A2DP+PAN.\n");
+			action_hid_a2dp_pan_edr(btcoexist);
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_HID_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = HID+A2DP.\n");
+			halbtc8723b1ant_action_hid_a2dp(btcoexist);
+			break;
+		default:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = coexist All Off!!\n");
+			break;
+		}
+		coex_dm->pre_algorithm = coex_dm->cur_algorithm;
+	}
+}
+
+static void halbtc8723b1ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
+{
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	bool wifi_connected = false, bt_hs_on = false;
+	bool increase_scan_dev_num = false;
+	bool b_bt_ctrl_agg_buf_size = false;
+	u8 agg_buf_size = 5;
+	u8 wifi_rssi_state = BTC_RSSI_STATE_HIGH;
+	u32 wifi_link_status = 0;
+	u32 num_of_wifi_link = 0;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+		  "[BTCoex], RunCoexistMechanism() ===>\n");
+
+	if (btcoexist->manual_control) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], RunCoexistMechanism(), return for Manual CTRL <===\n");
+		return;
+	}
+
+	if (btcoexist->stop_coex_dm) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], RunCoexistMechanism(), return for Stop Coex DM <===\n");
+		return;
+	}
+
+	if (coex_sta->under_ips) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], wifi is under IPS !!!\n");
+		return;
+	}
+
+	if ((BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) ||
+	    (BT_8723B_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
+	    (BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status))
+		increase_scan_dev_num = true;
+
+	btcoexist->btc_set(btcoexist, BTC_SET_BL_INC_SCAN_DEV_NUM,
+			   &increase_scan_dev_num);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+			   &wifi_connected);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_LINK_STATUS,
+			   &wifi_link_status);
+	num_of_wifi_link = wifi_link_status >> 16;
+	if (num_of_wifi_link >= 2) {
+		halbtc8723b1ant_limited_tx(btcoexist, NORMAL_EXEC, 0, 0, 0, 0);
+		halbtc8723b1ant_limited_rx(btcoexist, NORMAL_EXEC, false,
+					   b_bt_ctrl_agg_buf_size,
+					   agg_buf_size);
+		halbtc8723b1ant_action_wifi_multiport(btcoexist);
+		return;
+	}
+
+	if (!bt_link_info->sco_exist && !bt_link_info->hid_exist) {
+		halbtc8723b1ant_limited_tx(btcoexist, NORMAL_EXEC, 0, 0, 0, 0);
+	} else {
+		if (wifi_connected) {
+			wifi_rssi_state =
+				halbtc8723b1ant_wifi_rssi_state(btcoexist,
+								1, 2, 30, 0);
+			if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+				halbtc8723b1ant_limited_tx(btcoexist,
+							   NORMAL_EXEC,
+							   1, 1, 1, 1);
+			} else {
+				halbtc8723b1ant_limited_tx(btcoexist,
+							   NORMAL_EXEC,
+							   1, 1, 1, 1);
+			}
+		} else {
+			halbtc8723b1ant_limited_tx(btcoexist, NORMAL_EXEC,
+						   0, 0, 0, 0);
+		}
+	}
+
+	if (bt_link_info->sco_exist) {
+		b_bt_ctrl_agg_buf_size = true;
+		agg_buf_size = 0x3;
+	} else if (bt_link_info->hid_exist) {
+		b_bt_ctrl_agg_buf_size = true;
+		agg_buf_size = 0x5;
+	} else if (bt_link_info->a2dp_exist || bt_link_info->pan_exist) {
+		b_bt_ctrl_agg_buf_size = true;
+		agg_buf_size = 0x8;
+	}
+	halbtc8723b1ant_limited_rx(btcoexist, NORMAL_EXEC, false,
+				   b_bt_ctrl_agg_buf_size, agg_buf_size);
+
+	run_sw_coexist_mechanism(btcoexist);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+
+	if (coex_sta->c2h_bt_inquiry_page) {
+		halbtc8723b1ant_action_bt_inquiry(btcoexist);
+		return;
+	} else if (bt_hs_on) {
+		halbtc8723b1ant_action_hs(btcoexist);
+		return;
+	}
+
+	if (!wifi_connected) {
+		bool scan = false, link = false, roam = false;
+
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], wifi is non connected-idle !!!\n");
+
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
+
+		if (scan || link || roam) {
+			if (scan)
+				action_wifi_not_connected_scan(btcoexist);
+			else
+				action_wifi_not_connected_asso_auth(btcoexist);
+		} else {
+			action_wifi_not_connected(btcoexist);
+		}
+	} else { /* wifi LPS/Busy */
+		halbtc8723b1ant_action_wifi_connected(btcoexist);
+	}
+}
+
+static void halbtc8723b1ant_init_coex_dm(struct btc_coexist *btcoexist)
+{
+	/* sw all off */
+	halbtc8723b1ant_sw_mechanism(btcoexist, false);
+
+	halbtc8723b1ant_ps_tdma(btcoexist, FORCE_EXEC, false, 8);
+	coex_table_with_type(btcoexist, FORCE_EXEC, 0);
+}
+
+static void init_hw_config(struct btc_coexist *btcoexist, bool backup)
+{
+	u32 u32tmp = 0;
+	u8 u8tmp = 0;
+	u32 cnt_bt_cal_chk = 0;
+
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+		  "[BTCoex], 1Ant Init HW Config!!\n");
+
+	if (backup) {/* backup rf 0x1e value */
+		coex_dm->backup_arfr_cnt1 =
+			btcoexist->btc_read_4byte(btcoexist, 0x430);
+		coex_dm->backup_arfr_cnt2 =
+			btcoexist->btc_read_4byte(btcoexist, 0x434);
+		coex_dm->backup_retry_limit =
+			btcoexist->btc_read_2byte(btcoexist, 0x42a);
+		coex_dm->backup_ampdu_max_time =
+			btcoexist->btc_read_1byte(btcoexist, 0x456);
+	}
+
+	/* WiFi goto standby while GNT_BT 0-->1 */
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x780);
+	/* BT goto standby while GNT_BT 1-->0 */
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x2, 0xfffff, 0x500);
+
+	btcoexist->btc_write_1byte(btcoexist, 0x974, 0xff);
+	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x944, 0x3, 0x3);
+	btcoexist->btc_write_1byte(btcoexist, 0x930, 0x77);
+
+	/* BT calibration check */
+	while (cnt_bt_cal_chk <= 20) {
+		u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x49d);
+		cnt_bt_cal_chk++;
+		if (u32tmp & BIT(0)) {
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+				  "[BTCoex], ########### BT calibration(cnt =%d) ###########\n",
+				  cnt_bt_cal_chk);
+			mdelay(50);
+		} else {
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+				  "[BTCoex], ********** BT NOT calibration (cnt =%d)**********\n",
+				  cnt_bt_cal_chk);
+			break;
+		}
+	}
+
+	/* 0x790[5:0] = 0x5 */
+	u8tmp = btcoexist->btc_read_1byte(btcoexist, 0x790);
+	u8tmp &= 0xc0;
+	u8tmp |= 0x5;
+	btcoexist->btc_write_1byte(btcoexist, 0x790, u8tmp);
+
+	/* Enable counter statistics */
+	/*0x76e[3] = 1, WLAN_Act control by PTA */
+	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
+	btcoexist->btc_write_1byte(btcoexist, 0x778, 0x1);
+	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x40, 0x20, 0x1);
+
+	/*Antenna config */
+	halbtc8723b1ant_setantpath(btcoexist, BTC_ANT_PATH_PTA, true, false);
+	/* PTA parameter */
+	coex_table_with_type(btcoexist, FORCE_EXEC, 0);
+}
+
+static void halbtc8723b1ant_wifi_off_hw_cfg(struct btc_coexist *btcoexist)
+{
+	/* set wlan_act to low */
+	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0x4);
+}
+
+/**************************************************************
+ * work around function start with wa_halbtc8723b1ant_
+ **************************************************************/
+/**************************************************************
+ * extern function start with EXhalbtc8723b1ant_
+ **************************************************************/
+
+void ex_halbtc8723b1ant_init_hwconfig(struct btc_coexist *btcoexist)
+{
+	init_hw_config(btcoexist, true);
+}
+
+void ex_halbtc8723b1ant_init_coex_dm(struct btc_coexist *btcoexist)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+		  "[BTCoex], Coex Mechanism Init!!\n");
+
+	btcoexist->stop_coex_dm = false;
+
+	halbtc8723b1ant_init_coex_dm(btcoexist);
+
+	halbtc8723b1ant_query_bt_info(btcoexist);
+}
+
+void ex_halbtc8723b1ant_display_coex_info(struct btc_coexist *btcoexist)
+{
+	struct btc_board_info *brd_info = &btcoexist->board_info;
+	struct btc_stack_info *stack_info = &btcoexist->stack_info;
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	u8 *cli_buf = btcoexist->cli_buf;
+	u8 u8tmp[4], i, bt_info_ext, stdmacase = 0;
+	u16 u16tmp[4];
+	u32 u32tmp[4];
+	bool roam = false, scan = false;
+	bool link = false, wifi_under_5g = false;
+	bool bt_hs_on = false, wifi_busy = false;
+	s32 wifi_rssi = 0, bt_hs_rssi = 0;
+	u32 wifi_bw, wifi_traffic_dir, fa_ofdm, fa_cck, wifi_link_status;
+	u8 wifi_dot11_chnl, wifi_hs_chnl;
+	u32 fw_ver = 0, bt_patch_ver = 0;
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n ============[BT Coexist info] ============");
+	CL_PRINTF(cli_buf);
+
+	if (btcoexist->manual_control) {
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\r\n ============[Under Manual Control] ==========");
+		CL_PRINTF(cli_buf);
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\r\n ==========================================");
+		CL_PRINTF(cli_buf);
+	}
+	if (btcoexist->stop_coex_dm) {
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\r\n ============[Coex is STOPPED] ============");
+		CL_PRINTF(cli_buf);
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\r\n ==========================================");
+		CL_PRINTF(cli_buf);
+	}
+
+	if (!brd_info->bt_exist) {
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n BT not exists !!!");
+		CL_PRINTF(cli_buf);
+		return;
+	}
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d",
+		   "Ant PG Num/ Ant Mech/ Ant Pos: ",
+		   brd_info->pg_ant_num, brd_info->btdm_ant_num,
+		   brd_info->btdm_ant_pos);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %d",
+		   "BT stack/ hci ext ver",
+		   ((stack_info->profile_notified) ? "Yes" : "No"),
+		   stack_info->hci_version);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER, &bt_patch_ver);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n %-35s = %d_%x/ 0x%x/ 0x%x(%d)",
+		   "CoexVer/ FwVer/ PatchVer",
+		   glcoex_ver_date_8723b_1ant, glcoex_ver_8723b_1ant,
+		   fw_ver, bt_patch_ver, bt_patch_ver);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_DOT11_CHNL,
+			   &wifi_dot11_chnl);
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifi_hs_chnl);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d(%d)",
+		   "Dot11 channel / HsChnl(HsMode)",
+		   wifi_dot11_chnl, wifi_hs_chnl, bt_hs_on);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x ",
+		   "H2C Wifi inform bt chnl Info",
+		   coex_dm->wifi_chnl_info[0], coex_dm->wifi_chnl_info[1],
+		   coex_dm->wifi_chnl_info[2]);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
+	btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
+		   "Wifi rssi/ HS rssi", wifi_rssi, bt_hs_rssi);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ",
+		   "Wifi link/ roam/ scan", link, roam, scan);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G,
+			   &wifi_under_5g);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION,
+			   &wifi_traffic_dir);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %s/ %s ",
+		   "Wifi status", (wifi_under_5g ? "5G" : "2.4G"),
+		   ((BTC_WIFI_BW_LEGACY == wifi_bw) ? "Legacy" :
+			(((BTC_WIFI_BW_HT40 == wifi_bw) ? "HT40" : "HT20"))),
+		   ((!wifi_busy) ? "idle" :
+			((BTC_WIFI_TRAFFIC_TX == wifi_traffic_dir) ?
+				"uplink" : "downlink")));
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_LINK_STATUS,
+			   &wifi_link_status);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d/ %d/ %d",
+		   "sta/vwifi/hs/p2pGo/p2pGc",
+		   ((wifi_link_status & WIFI_STA_CONNECTED) ? 1 : 0),
+		   ((wifi_link_status & WIFI_AP_CONNECTED) ? 1 : 0),
+		   ((wifi_link_status & WIFI_HS_CONNECTED) ? 1 : 0),
+		   ((wifi_link_status & WIFI_P2P_GO_CONNECTED) ? 1 : 0),
+		   ((wifi_link_status & WIFI_P2P_GC_CONNECTED) ? 1 : 0));
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = [%s/ %d/ %d] ",
+		   "BT [status/ rssi/ retryCnt]",
+		   ((btcoexist->bt_info.bt_disabled) ? ("disabled") :
+		    ((coex_sta->c2h_bt_inquiry_page) ? ("inquiry/page scan") :
+		     ((BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE == coex_dm->bt_status) ?
+		      "non-connected idle" :
+		      ((BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE == coex_dm->bt_status) ?
+		       "connected-idle" : "busy")))),
+		       coex_sta->bt_rssi, coex_sta->bt_retry_cnt);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d / %d / %d",
+		"SCO/HID/PAN/A2DP", bt_link_info->sco_exist,
+		bt_link_info->hid_exist, bt_link_info->pan_exist,
+		bt_link_info->a2dp_exist);
+	CL_PRINTF(cli_buf);
+	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_BT_LINK_INFO);
+
+	bt_info_ext = coex_sta->bt_info_ext;
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s",
+		   "BT Info A2DP rate",
+		   (bt_info_ext & BIT(0)) ? "Basic rate" : "EDR rate");
+	CL_PRINTF(cli_buf);
+
+	for (i = 0; i < BT_INFO_SRC_8723B_1ANT_MAX; i++) {
+		if (coex_sta->bt_info_c2h_cnt[i]) {
+			CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+				   "\r\n %-35s = %02x %02x %02x %02x %02x %02x %02x(%d)",
+				   glb_infosrc8723b1ant[i],
+				   coex_sta->bt_info_c2h[i][0],
+				   coex_sta->bt_info_c2h[i][1],
+				   coex_sta->bt_info_c2h[i][2],
+				   coex_sta->bt_info_c2h[i][3],
+				   coex_sta->bt_info_c2h[i][4],
+				   coex_sta->bt_info_c2h[i][5],
+				   coex_sta->bt_info_c2h[i][6],
+				   coex_sta->bt_info_c2h_cnt[i]);
+			CL_PRINTF(cli_buf);
+		}
+	}
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n %-35s = %s/%s, (0x%x/0x%x)",
+		   "PS state, IPS/LPS, (lps/rpwm)",
+		   ((coex_sta->under_ips ? "IPS ON" : "IPS OFF")),
+		   ((coex_sta->under_lps ? "LPS ON" : "LPS OFF")),
+		   btcoexist->bt_info.lps_val,
+		   btcoexist->bt_info.rpwm_val);
+	CL_PRINTF(cli_buf);
+	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_FW_PWR_MODE_CMD);
+
+	if (!btcoexist->manual_control) {
+		/* Sw mechanism	*/
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
+			   "============[Sw mechanism] ============");
+		CL_PRINTF(cli_buf);
+
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/",
+			   "SM[LowPenaltyRA]", coex_dm->cur_low_penalty_ra);
+		CL_PRINTF(cli_buf);
+
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s/ %s/ %d ",
+			   "DelBA/ BtCtrlAgg/ AggSize",
+			   (btcoexist->bt_info.reject_agg_pkt ? "Yes" : "No"),
+			   (btcoexist->bt_info.b_bt_ctrl_buf_size ? "Yes" : "No"),
+			   btcoexist->bt_info.agg_buf_size);
+		CL_PRINTF(cli_buf);
+
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x ",
+			   "Rate Mask", btcoexist->bt_info.ra_mask);
+		CL_PRINTF(cli_buf);
+
+		/* Fw mechanism	*/
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
+			   "============[Fw mechanism] ============");
+		CL_PRINTF(cli_buf);
+
+		stdmacase = coex_dm->cur_ps_tdma;
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\r\n %-35s = %02x %02x %02x %02x %02x case-%d (auto:%d)",
+			   "PS TDMA", coex_dm->ps_tdma_para[0],
+			   coex_dm->ps_tdma_para[1], coex_dm->ps_tdma_para[2],
+			   coex_dm->ps_tdma_para[3], coex_dm->ps_tdma_para[4],
+			   stdmacase, coex_dm->auto_tdma_adjust);
+		CL_PRINTF(cli_buf);
+
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d ",
+			   "IgnWlanAct", coex_dm->cur_ignore_wlan_act);
+		CL_PRINTF(cli_buf);
+
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x ",
+			   "Latest error condition(should be 0)",
+			   coex_dm->error_condition);
+		CL_PRINTF(cli_buf);
+	}
+
+	/* Hw setting */
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
+		   "============[Hw setting] ============");
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/0x%x/0x%x/0x%x",
+		   "backup ARFR1/ARFR2/RL/AMaxTime", coex_dm->backup_arfr_cnt1,
+		   coex_dm->backup_arfr_cnt2, coex_dm->backup_retry_limit,
+		   coex_dm->backup_ampdu_max_time);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x430);
+	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x434);
+	u16tmp[0] = btcoexist->btc_read_2byte(btcoexist, 0x42a);
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x456);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/0x%x/0x%x/0x%x",
+		   "0x430/0x434/0x42a/0x456",
+		   u32tmp[0], u32tmp[1], u16tmp[0], u8tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x778);
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x6cc);
+	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x880);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
+		   "0x778/0x6cc/0x880[29:25]", u8tmp[0], u32tmp[0],
+		   (u32tmp[1] & 0x3e000000) >> 25);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x948);
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x67);
+	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x765);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x/ 0x%x/ 0x%x",
+		   "0x948/ 0x67[5] / 0x765",
+		   u32tmp[0], ((u8tmp[0] & 0x20) >> 5), u8tmp[1]);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x92c);
+	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x930);
+	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x944);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
+		   "0x92c[1:0]/ 0x930[7:0]/0x944[1:0]",
+		   u32tmp[0] & 0x3, u32tmp[1] & 0xff, u32tmp[2] & 0x3);
+	CL_PRINTF(cli_buf);
+
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x39);
+	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x40);
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x4c);
+	u8tmp[2] = btcoexist->btc_read_1byte(btcoexist, 0x64);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x",
+		   "0x38[11]/0x40/0x4c[24:23]/0x64[0]",
+		   ((u8tmp[0] & 0x8)>>3), u8tmp[1],
+		   ((u32tmp[0] & 0x01800000) >> 23), u8tmp[2] & 0x1);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x550);
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x522);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
+		   "0x550(bcn ctrl)/0x522", u32tmp[0], u8tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xc50);
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x49c);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
+		   "0xc50(dig)/0x49c(null-drop)", u32tmp[0] & 0xff, u8tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xda0);
+	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0xda4);
+	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0xda8);
+	u32tmp[3] = btcoexist->btc_read_4byte(btcoexist, 0xcf0);
+
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0xa5b);
+	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0xa5c);
+
+	fa_ofdm = ((u32tmp[0] & 0xffff0000) >> 16) +
+		  ((u32tmp[1] & 0xffff0000) >> 16) +
+		   (u32tmp[1] & 0xffff) +
+		   (u32tmp[2] & 0xffff) +
+		  ((u32tmp[3] & 0xffff0000) >> 16) +
+		   (u32tmp[3] & 0xffff);
+	fa_cck = (u8tmp[0] << 8) + u8tmp[1];
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
+		   "OFDM-CCA/OFDM-FA/CCK-FA",
+		   u32tmp[0] & 0xffff, fa_ofdm, fa_cck);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x6c0);
+	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x6c4);
+	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x6c8);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
+		   "0x6c0/0x6c4/0x6c8(coexTable)",
+		   u32tmp[0], u32tmp[1], u32tmp[2]);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
+		   "0x770(high-pri rx/tx)", coex_sta->high_priority_rx,
+		   coex_sta->high_priority_tx);
+	CL_PRINTF(cli_buf);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
+		   "0x774(low-pri rx/tx)", coex_sta->low_priority_rx,
+		   coex_sta->low_priority_tx);
+	CL_PRINTF(cli_buf);
+#if (BT_AUTO_REPORT_ONLY_8723B_1ANT == 1)
+	halbtc8723b1ant_monitor_bt_ctr(btcoexist);
+#endif
+	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_COEX_STATISTICS);
+}
+
+void ex_halbtc8723b1ant_ips_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	if (btcoexist->manual_control || btcoexist->stop_coex_dm)
+		return;
+
+	if (BTC_IPS_ENTER == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], IPS ENTER notify\n");
+		coex_sta->under_ips = true;
+
+		halbtc8723b1ant_setantpath(btcoexist, BTC_ANT_PATH_BT,
+					   false, true);
+		/* set PTA control */
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 0);
+		coex_table_with_type(btcoexist,
+						     NORMAL_EXEC, 0);
+		halbtc8723b1ant_wifi_off_hw_cfg(btcoexist);
+	} else if (BTC_IPS_LEAVE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], IPS LEAVE notify\n");
+		coex_sta->under_ips = false;
+
+		init_hw_config(btcoexist, false);
+		halbtc8723b1ant_init_coex_dm(btcoexist);
+		halbtc8723b1ant_query_bt_info(btcoexist);
+	}
+}
+
+void ex_halbtc8723b1ant_lps_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	if (btcoexist->manual_control || btcoexist->stop_coex_dm)
+		return;
+
+	if (BTC_LPS_ENABLE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], LPS ENABLE notify\n");
+		coex_sta->under_lps = true;
+	} else if (BTC_LPS_DISABLE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], LPS DISABLE notify\n");
+		coex_sta->under_lps = false;
+	}
+}
+
+void ex_halbtc8723b1ant_scan_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	bool wifi_connected = false, bt_hs_on = false;
+	u32 wifi_link_status = 0;
+	u32 num_of_wifi_link = 0;
+	bool bt_ctrl_agg_buf_size = false;
+	u8 agg_buf_size = 5;
+
+	if (btcoexist->manual_control || btcoexist->stop_coex_dm ||
+	    btcoexist->bt_info.bt_disabled)
+		return;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+			   &wifi_connected);
+
+	halbtc8723b1ant_query_bt_info(btcoexist);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_LINK_STATUS,
+			   &wifi_link_status);
+	num_of_wifi_link = wifi_link_status >> 16;
+	if (num_of_wifi_link >= 2) {
+		halbtc8723b1ant_limited_tx(btcoexist, NORMAL_EXEC, 0, 0, 0, 0);
+		halbtc8723b1ant_limited_rx(btcoexist, NORMAL_EXEC, false,
+					   bt_ctrl_agg_buf_size, agg_buf_size);
+		halbtc8723b1ant_action_wifi_multiport(btcoexist);
+		return;
+	}
+
+	if (coex_sta->c2h_bt_inquiry_page) {
+		halbtc8723b1ant_action_bt_inquiry(btcoexist);
+		return;
+	} else if (bt_hs_on) {
+		halbtc8723b1ant_action_hs(btcoexist);
+		return;
+	}
+
+	if (BTC_SCAN_START == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], SCAN START notify\n");
+		if (!wifi_connected)	/* non-connected scan */
+			action_wifi_not_connected_scan(btcoexist);
+		else	/* wifi is connected */
+			action_wifi_connected_scan(btcoexist);
+	} else if (BTC_SCAN_FINISH == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], SCAN FINISH notify\n");
+		if (!wifi_connected)	/* non-connected scan */
+			action_wifi_not_connected(btcoexist);
+		else
+			halbtc8723b1ant_action_wifi_connected(btcoexist);
+	}
+}
+
+void ex_halbtc8723b1ant_connect_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	bool wifi_connected = false, bt_hs_on = false;
+	u32 wifi_link_status = 0;
+	u32 num_of_wifi_link = 0;
+	bool bt_ctrl_agg_buf_size = false;
+	u8 agg_buf_size = 5;
+
+	if (btcoexist->manual_control || btcoexist->stop_coex_dm ||
+	    btcoexist->bt_info.bt_disabled)
+		return;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_LINK_STATUS,
+			   &wifi_link_status);
+	num_of_wifi_link = wifi_link_status>>16;
+	if (num_of_wifi_link >= 2) {
+		halbtc8723b1ant_limited_tx(btcoexist, NORMAL_EXEC, 0, 0, 0, 0);
+		halbtc8723b1ant_limited_rx(btcoexist, NORMAL_EXEC, false,
+					   bt_ctrl_agg_buf_size, agg_buf_size);
+		halbtc8723b1ant_action_wifi_multiport(btcoexist);
+		return;
+	}
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	if (coex_sta->c2h_bt_inquiry_page) {
+		halbtc8723b1ant_action_bt_inquiry(btcoexist);
+		return;
+	} else if (bt_hs_on) {
+		halbtc8723b1ant_action_hs(btcoexist);
+		return;
+	}
+
+	if (BTC_ASSOCIATE_START == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], CONNECT START notify\n");
+		action_wifi_not_connected_asso_auth(btcoexist);
+	} else if (BTC_ASSOCIATE_FINISH == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], CONNECT FINISH notify\n");
+
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+				   &wifi_connected);
+		if (!wifi_connected) /* non-connected scan */
+			action_wifi_not_connected(btcoexist);
+		else
+			halbtc8723b1ant_action_wifi_connected(btcoexist);
+	}
+}
+
+void ex_halbtc8723b1ant_media_status_notify(struct btc_coexist *btcoexist,
+					    u8 type)
+{
+	u8 h2c_parameter[3] = {0};
+	u32 wifi_bw;
+	u8 wificentralchnl;
+
+	if (btcoexist->manual_control || btcoexist->stop_coex_dm ||
+	    btcoexist->bt_info.bt_disabled)
+		return;
+
+	if (BTC_MEDIA_CONNECT == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], MEDIA connect notify\n");
+	else
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], MEDIA disconnect notify\n");
+
+	/* only 2.4G we need to inform bt the chnl mask */
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_CENTRAL_CHNL,
+			   &wificentralchnl);
+
+	if ((BTC_MEDIA_CONNECT == type) &&
+	    (wificentralchnl <= 14)) {
+		h2c_parameter[0] = 0x0;
+		h2c_parameter[1] = wificentralchnl;
+		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+		if (BTC_WIFI_BW_HT40 == wifi_bw)
+			h2c_parameter[2] = 0x30;
+		else
+			h2c_parameter[2] = 0x20;
+	}
+
+	coex_dm->wifi_chnl_info[0] = h2c_parameter[0];
+	coex_dm->wifi_chnl_info[1] = h2c_parameter[1];
+	coex_dm->wifi_chnl_info[2] = h2c_parameter[2];
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], FW write 0x66 = 0x%x\n",
+		  h2c_parameter[0] << 16 | h2c_parameter[1] << 8 |
+		  h2c_parameter[2]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x66, 3, h2c_parameter);
+}
+
+void ex_halbtc8723b1ant_special_packet_notify(struct btc_coexist *btcoexist,
+					      u8 type)
+{
+	bool bt_hs_on = false;
+	u32 wifi_link_status = 0;
+	u32 num_of_wifi_link = 0;
+	bool bt_ctrl_agg_buf_size = false;
+	u8 agg_buf_size = 5;
+
+	if (btcoexist->manual_control || btcoexist->stop_coex_dm ||
+	    btcoexist->bt_info.bt_disabled)
+		return;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_LINK_STATUS,
+			   &wifi_link_status);
+	num_of_wifi_link = wifi_link_status >> 16;
+	if (num_of_wifi_link >= 2) {
+		halbtc8723b1ant_limited_tx(btcoexist, NORMAL_EXEC, 0, 0, 0, 0);
+		halbtc8723b1ant_limited_rx(btcoexist, NORMAL_EXEC, false,
+					   bt_ctrl_agg_buf_size, agg_buf_size);
+		halbtc8723b1ant_action_wifi_multiport(btcoexist);
+		return;
+	}
+
+	coex_sta->special_pkt_period_cnt = 0;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	if (coex_sta->c2h_bt_inquiry_page) {
+		halbtc8723b1ant_action_bt_inquiry(btcoexist);
+		return;
+	} else if (bt_hs_on) {
+		halbtc8723b1ant_action_hs(btcoexist);
+		return;
+	}
+
+	if (BTC_PACKET_DHCP == type ||
+	    BTC_PACKET_EAPOL == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], special Packet(%d) notify\n", type);
+		action_wifi_connected_special_packet(btcoexist);
+	}
+}
+
+void ex_halbtc8723b1ant_bt_info_notify(struct btc_coexist *btcoexist,
+				       u8 *tmp_buf, u8 length)
+{
+	u8 bt_info = 0;
+	u8 i, rsp_source = 0;
+	bool wifi_connected = false;
+	bool bt_busy = false;
+
+	coex_sta->c2h_bt_info_req_sent = false;
+
+	rsp_source = tmp_buf[0] & 0xf;
+	if (rsp_source >= BT_INFO_SRC_8723B_1ANT_MAX)
+		rsp_source = BT_INFO_SRC_8723B_1ANT_WIFI_FW;
+	coex_sta->bt_info_c2h_cnt[rsp_source]++;
+
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+		  "[BTCoex], Bt info[%d], length =%d, hex data =[",
+		  rsp_source, length);
+	for (i = 0; i < length; i++) {
+		coex_sta->bt_info_c2h[rsp_source][i] = tmp_buf[i];
+		if (i == 1)
+			bt_info = tmp_buf[i];
+		if (i == length - 1)
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+				  "0x%02x]\n", tmp_buf[i]);
+		else
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+				  "0x%02x, ", tmp_buf[i]);
+	}
+
+	if (BT_INFO_SRC_8723B_1ANT_WIFI_FW != rsp_source) {
+		coex_sta->bt_retry_cnt =	/* [3:0] */
+			coex_sta->bt_info_c2h[rsp_source][2] & 0xf;
+
+		coex_sta->bt_rssi =
+			coex_sta->bt_info_c2h[rsp_source][3] * 2 + 10;
+
+		coex_sta->bt_info_ext =
+			coex_sta->bt_info_c2h[rsp_source][4];
+
+		/* Here we need to resend some wifi info to BT
+		 * because bt is reset and loss of the info.*/
+		if (coex_sta->bt_info_ext & BIT(1)) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], BT ext info bit1 check, send wifi BW&Chnl to BT!!\n");
+			btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+					   &wifi_connected);
+			if (wifi_connected)
+				ex_halbtc8723b1ant_media_status_notify(btcoexist,
+							     BTC_MEDIA_CONNECT);
+			else
+				ex_halbtc8723b1ant_media_status_notify(btcoexist,
+							  BTC_MEDIA_DISCONNECT);
+		}
+
+		if (coex_sta->bt_info_ext & BIT(3)) {
+			if (!btcoexist->manual_control &&
+			    !btcoexist->stop_coex_dm) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], BT ext info bit3 check, set BT NOT ignore Wlan active!!\n");
+				halbtc8723b1ant_ignore_wlan_act(btcoexist,
+								FORCE_EXEC,
+								false);
+			}
+		} else {
+			/* BT already NOT ignore Wlan active, do nothing here.*/
+		}
+#if (BT_AUTO_REPORT_ONLY_8723B_1ANT == 0)
+		if (coex_sta->bt_info_ext & BIT(4)) {
+			/* BT auto report already enabled, do nothing */
+		} else {
+			halbtc8723b1ant_bt_auto_report(btcoexist, FORCE_EXEC,
+						       true);
+		}
+#endif
+	}
+
+	/* check BIT(2) first ==> check if bt is under inquiry or page scan */
+	if (bt_info & BT_INFO_8723B_1ANT_B_INQ_PAGE)
+		coex_sta->c2h_bt_inquiry_page = true;
+	else
+		coex_sta->c2h_bt_inquiry_page = false;
+
+	/* set link exist status */
+	if (!(bt_info & BT_INFO_8723B_1ANT_B_CONNECTION)) {
+		coex_sta->bt_link_exist = false;
+		coex_sta->pan_exist = false;
+		coex_sta->a2dp_exist = false;
+		coex_sta->hid_exist = false;
+		coex_sta->sco_exist = false;
+	} else { /* connection exists */
+		coex_sta->bt_link_exist = true;
+		if (bt_info & BT_INFO_8723B_1ANT_B_FTP)
+			coex_sta->pan_exist = true;
+		else
+			coex_sta->pan_exist = false;
+		if (bt_info & BT_INFO_8723B_1ANT_B_A2DP)
+			coex_sta->a2dp_exist = true;
+		else
+			coex_sta->a2dp_exist = false;
+		if (bt_info & BT_INFO_8723B_1ANT_B_HID)
+			coex_sta->hid_exist = true;
+		else
+			coex_sta->hid_exist = false;
+		if (bt_info & BT_INFO_8723B_1ANT_B_SCO_ESCO)
+			coex_sta->sco_exist = true;
+		else
+			coex_sta->sco_exist = false;
+	}
+
+	halbtc8723b1ant_update_bt_link_info(btcoexist);
+
+	if (!(bt_info&BT_INFO_8723B_1ANT_B_CONNECTION)) {
+		coex_dm->bt_status = BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), BT Non-Connected idle!\n");
+	/* connection exists but no busy */
+	} else if (bt_info == BT_INFO_8723B_1ANT_B_CONNECTION) {
+		coex_dm->bt_status = BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), BT Connected-idle!!!\n");
+	} else if ((bt_info & BT_INFO_8723B_1ANT_B_SCO_ESCO) ||
+		(bt_info & BT_INFO_8723B_1ANT_B_SCO_BUSY)) {
+		coex_dm->bt_status = BT_8723B_1ANT_BT_STATUS_SCO_BUSY;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), BT SCO busy!!!\n");
+	} else if (bt_info & BT_INFO_8723B_1ANT_B_ACL_BUSY) {
+		if (BT_8723B_1ANT_BT_STATUS_ACL_BUSY != coex_dm->bt_status)
+			coex_dm->auto_tdma_adjust = false;
+
+		coex_dm->bt_status = BT_8723B_1ANT_BT_STATUS_ACL_BUSY;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), BT ACL busy!!!\n");
+	} else {
+		coex_dm->bt_status =
+			BT_8723B_1ANT_BT_STATUS_MAX;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), BT Non-Defined state!!\n");
+	}
+
+	if ((BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) ||
+	    (BT_8723B_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
+	    (BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status))
+		bt_busy = true;
+	else
+		bt_busy = false;
+	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bt_busy);
+
+	halbtc8723b1ant_run_coexist_mechanism(btcoexist);
+}
+
+void ex_halbtc8723b1ant_halt_notify(struct btc_coexist *btcoexist)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, "[BTCoex], Halt notify\n");
+
+	btcoexist->stop_coex_dm = true;
+
+	halbtc8723b1ant_setantpath(btcoexist, BTC_ANT_PATH_BT, false, true);
+
+	halbtc8723b1ant_wifi_off_hw_cfg(btcoexist);
+	halbtc8723b1ant_ignore_wlan_act(btcoexist, FORCE_EXEC, true);
+
+	halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+					 0x0, 0x0);
+	halbtc8723b1ant_ps_tdma(btcoexist, FORCE_EXEC, false, 0);
+
+	ex_halbtc8723b1ant_media_status_notify(btcoexist, BTC_MEDIA_DISCONNECT);
+}
+
+void ex_halbtc8723b1ant_pnp_notify(struct btc_coexist *btcoexist, u8 pnp_state)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, "[BTCoex], Pnp notify\n");
+
+	if (BTC_WIFI_PNP_SLEEP == pnp_state) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], Pnp notify to SLEEP\n");
+		btcoexist->stop_coex_dm = true;
+		halbtc8723b1ant_setantpath(btcoexist, BTC_ANT_PATH_BT, false,
+					   true);
+		halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+						 0x0, 0x0);
+		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 0);
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+		halbtc8723b1ant_wifi_off_hw_cfg(btcoexist);
+	} else if (BTC_WIFI_PNP_WAKE_UP == pnp_state) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], Pnp notify to WAKE UP\n");
+		btcoexist->stop_coex_dm = false;
+		init_hw_config(btcoexist, false);
+		halbtc8723b1ant_init_coex_dm(btcoexist);
+		halbtc8723b1ant_query_bt_info(btcoexist);
+	}
+}
+
+void ex_halbtc8723b1ant_coex_dm_reset(struct btc_coexist *btcoexist)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+		  "[BTCoex], *****************Coex DM Reset****************\n");
+
+	init_hw_config(btcoexist, false);
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x2, 0xfffff, 0x0);
+	halbtc8723b1ant_init_coex_dm(btcoexist);
+}
+
+void ex_halbtc8723b1ant_periodical(struct btc_coexist *btcoexist)
+{
+	struct btc_board_info *brd_info = &btcoexist->board_info;
+	struct btc_stack_info *stack_info = &btcoexist->stack_info;
+	static u8 dis_ver_info_cnt;
+	u32 fw_ver = 0, bt_patch_ver = 0;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+		  "[BTCoex], ========================== Periodical ===========================\n");
+
+	if (dis_ver_info_cnt <= 5) {
+		dis_ver_info_cnt += 1;
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], ****************************************************************\n");
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], Ant PG Num/ Ant Mech/ Ant Pos = %d/ %d/ %d\n",
+			  brd_info->pg_ant_num, brd_info->btdm_ant_num,
+			  brd_info->btdm_ant_pos);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], BT stack/ hci ext ver = %s / %d\n",
+			  ((stack_info->profile_notified) ? "Yes" : "No"),
+			  stack_info->hci_version);
+		btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER,
+				   &bt_patch_ver);
+		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], CoexVer/ FwVer/ PatchVer = %d_%x/ 0x%x/ 0x%x(%d)\n",
+			  glcoex_ver_date_8723b_1ant,
+			  glcoex_ver_8723b_1ant, fw_ver,
+			  bt_patch_ver, bt_patch_ver);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], ****************************************************************\n");
+	}
+
+#if (BT_AUTO_REPORT_ONLY_8723B_1ANT == 0)
+	halbtc8723b1ant_query_bt_info(btcoexist);
+	halbtc8723b1ant_monitor_bt_ctr(btcoexist);
+	halbtc8723b1ant_monitor_bt_enable_disable(btcoexist);
+#else
+	if (is_wifi_status_changed(btcoexist) ||
+	    coex_dm->auto_tdma_adjust) {
+		halbtc8723b1ant_run_coexist_mechanism(btcoexist);
+	}
+
+	coex_sta->special_pkt_period_cnt++;
+#endif
+}
+
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbtc8723b1ant.h b/drivers/staging/rtl8192ee/btcoexist/halbtc8723b1ant.h
new file mode 100644
index 00000000000..bded3738f24
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/halbtc8723b1ant.h
@@ -0,0 +1,160 @@
+/**********************************************************************
+ * The following is for 8723B 1ANT BT Co-exist definition
+ **********************************************************************/
+#define	BT_AUTO_REPORT_ONLY_8723B_1ANT			1
+
+#define	BT_INFO_8723B_1ANT_B_FTP			BIT(7)
+#define	BT_INFO_8723B_1ANT_B_A2DP			BIT(6)
+#define	BT_INFO_8723B_1ANT_B_HID			BIT(5)
+#define	BT_INFO_8723B_1ANT_B_SCO_BUSY			BIT(4)
+#define	BT_INFO_8723B_1ANT_B_ACL_BUSY			BIT(3)
+#define	BT_INFO_8723B_1ANT_B_INQ_PAGE			BIT(2)
+#define	BT_INFO_8723B_1ANT_B_SCO_ESCO			BIT(1)
+#define	BT_INFO_8723B_1ANT_B_CONNECTION			BIT(0)
+
+#define	BT_INFO_8723B_1ANT_A2DP_BASIC_RATE(_BT_INFO_EXT_)	\
+		(((_BT_INFO_EXT_&BIT(0))) ? true : false)
+
+#define	BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT		2
+
+enum BT_INFO_SRC_8723B_1ANT {
+	BT_INFO_SRC_8723B_1ANT_WIFI_FW			= 0x0,
+	BT_INFO_SRC_8723B_1ANT_BT_RSP			= 0x1,
+	BT_INFO_SRC_8723B_1ANT_BT_ACTIVE_SEND		= 0x2,
+	BT_INFO_SRC_8723B_1ANT_MAX
+};
+
+enum BT_8723B_1ANT_BT_STATUS {
+	BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE	= 0x0,
+	BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE		= 0x1,
+	BT_8723B_1ANT_BT_STATUS_INQ_PAGE		= 0x2,
+	BT_8723B_1ANT_BT_STATUS_ACL_BUSY		= 0x3,
+	BT_8723B_1ANT_BT_STATUS_SCO_BUSY		= 0x4,
+	BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY		= 0x5,
+	BT_8723B_1ANT_BT_STATUS_MAX
+};
+
+enum BT_8723B_1ANT_WIFI_STATUS {
+	BT_8723B_1ANT_WIFI_STATUS_NON_CONNECTED_IDLE			= 0x0,
+	BT_8723B_1ANT_WIFI_STATUS_NON_CONNECTED_ASSO_AUTH_SCAN		= 0x1,
+	BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SCAN			= 0x2,
+	BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SPECIAL_PKT			= 0x3,
+	BT_8723B_1ANT_WIFI_STATUS_CONNECTED_IDLE			= 0x4,
+	BT_8723B_1ANT_WIFI_STATUS_CONNECTED_BUSY			= 0x5,
+	BT_8723B_1ANT_WIFI_STATUS_MAX
+};
+
+enum BT_8723B_1ANT_COEX_ALGO {
+	BT_8723B_1ANT_COEX_ALGO_UNDEFINED		= 0x0,
+	BT_8723B_1ANT_COEX_ALGO_SCO			= 0x1,
+	BT_8723B_1ANT_COEX_ALGO_HID			= 0x2,
+	BT_8723B_1ANT_COEX_ALGO_A2DP			= 0x3,
+	BT_8723B_1ANT_COEX_ALGO_A2DP_PANHS		= 0x4,
+	BT_8723B_1ANT_COEX_ALGO_PANEDR			= 0x5,
+	BT_8723B_1ANT_COEX_ALGO_PANHS			= 0x6,
+	BT_8723B_1ANT_COEX_ALGO_PANEDR_A2DP		= 0x7,
+	BT_8723B_1ANT_COEX_ALGO_PANEDR_HID		= 0x8,
+	BT_8723B_1ANT_COEX_ALGO_HID_A2DP_PANEDR		= 0x9,
+	BT_8723B_1ANT_COEX_ALGO_HID_A2DP		= 0xa,
+	BT_8723B_1ANT_COEX_ALGO_MAX			= 0xb,
+};
+
+struct coex_dm_8723b_1ant {
+	/* fw mechanism */
+	bool cur_ignore_wlan_act;
+	bool pre_ignore_wlan_act;
+	u8 pre_ps_tdma;
+	u8 cur_ps_tdma;
+	u8 ps_tdma_para[5];
+	u8 ps_tdma_du_adj_type;
+	bool auto_tdma_adjust;
+	bool pre_ps_tdma_on;
+	bool cur_ps_tdma_on;
+	bool pre_bt_auto_report;
+	bool cur_bt_auto_report;
+	u8 pre_lps;
+	u8 cur_lps;
+	u8 pre_rpwm;
+	u8 cur_rpwm;
+
+	/* sw mechanism */
+	bool pre_low_penalty_ra;
+	bool cur_low_penalty_ra;
+	u32 pre_val0x6c0;
+	u32 cur_val0x6c0;
+	u32 pre_val0x6c4;
+	u32 cur_val0x6c4;
+	u32 pre_val0x6c8;
+	u32 cur_val0x6c8;
+	u8 pre_val0x6cc;
+	u8 cur_val0x6cc;
+	bool limited_dig;
+
+	u32 backup_arfr_cnt1;	/* Auto Rate Fallback Retry cnt */
+	u32 backup_arfr_cnt2;	/* Auto Rate Fallback Retry cnt */
+	u16 backup_retry_limit;
+	u8 backup_ampdu_max_time;
+
+	/* algorithm related */
+	u8 pre_algorithm;
+	u8 cur_algorithm;
+	u8 bt_status;
+	u8 wifi_chnl_info[3];
+
+	u32 prera_mask;
+	u32 curra_mask;
+	u8 pre_arfr_type;
+	u8 cur_arfr_type;
+	u8 pre_retry_limit_type;
+	u8 cur_retry_limit_type;
+	u8 pre_ampdu_time_type;
+	u8 cur_ampdu_time_type;
+
+	u8 error_condition;
+};
+
+struct coex_sta_8723b_1ant {
+	bool bt_link_exist;
+	bool sco_exist;
+	bool a2dp_exist;
+	bool hid_exist;
+	bool pan_exist;
+
+	bool under_lps;
+	bool under_ips;
+	u32 special_pkt_period_cnt;
+	u32 high_priority_tx;
+	u32 high_priority_rx;
+	u32 low_priority_tx;
+	u32 low_priority_rx;
+	u8 bt_rssi;
+	u8 pre_bt_rssi_state;
+	u8 pre_wifi_rssi_state[4];
+	bool c2h_bt_info_req_sent;
+	u8 bt_info_c2h[BT_INFO_SRC_8723B_1ANT_MAX][10];
+	u32 bt_info_c2h_cnt[BT_INFO_SRC_8723B_1ANT_MAX];
+	bool c2h_bt_inquiry_page;
+	u8 bt_retry_cnt;
+	u8 bt_info_ext;
+};
+
+/*************************************************************************
+ * The following is interface which will notify coex module.
+ *************************************************************************/
+void ex_halbtc8723b1ant_init_hwconfig(struct btc_coexist *btcoexist);
+void ex_halbtc8723b1ant_init_coex_dm(struct btc_coexist *btcoexist);
+void ex_halbtc8723b1ant_ips_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_halbtc8723b1ant_lps_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_halbtc8723b1ant_scan_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_halbtc8723b1ant_connect_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_halbtc8723b1ant_media_status_notify(struct btc_coexist *btcoexist,
+					    u8 type);
+void ex_halbtc8723b1ant_special_packet_notify(struct btc_coexist *btcoexist,
+					      u8 type);
+void ex_halbtc8723b1ant_bt_info_notify(struct btc_coexist *btcoexist,
+				       u8 *tmpbuf, u8 length);
+void ex_halbtc8723b1ant_halt_notify(struct btc_coexist *btcoexist);
+void ex_halbtc8723b1ant_pnp_notify(struct btc_coexist *btcoexist, u8 state);
+void ex_halbtc8723b1ant_coex_dm_reset(struct btc_coexist *btcoexist);
+void ex_halbtc8723b1ant_periodical(struct btc_coexist *btcoexist);
+void ex_halbtc8723b1ant_display_coex_info(struct btc_coexist *btcoexist);
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbtc8723b2ant.c b/drivers/staging/rtl8192ee/btcoexist/halbtc8723b2ant.c
new file mode 100644
index 00000000000..836206357e6
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/halbtc8723b2ant.c
@@ -0,0 +1,3929 @@
+/***************************************************************
+ * Description:
+ *
+ * This file is for RTL8723B Co-exist mechanism
+ *
+ * History
+ * 2012/11/15 Cosa first check in.
+ *
+ **************************************************************/
+/**************************************************************
+ * include files
+ **************************************************************/
+#include "halbt_precomp.h"
+/**************************************************************
+ * Global variables, these are static variables
+ **************************************************************/
+static struct coex_dm_8723b_2ant glcoex_dm_8723b_2ant;
+static struct coex_dm_8723b_2ant *coex_dm = &glcoex_dm_8723b_2ant;
+static struct coex_sta_8723b_2ant glcoex_sta_8723b_2ant;
+static struct coex_sta_8723b_2ant *coex_sta = &glcoex_sta_8723b_2ant;
+
+static const char *const glbt_info_src_8723b_2ant[] = {
+	"BT Info[wifi fw]",
+	"BT Info[bt rsp]",
+	"BT Info[bt auto report]",
+};
+
+static u32 glcoex_ver_date_8723b_2ant = 20131113;
+static u32 glcoex_ver_8723b_2ant = 0x3f;
+
+/**************************************************************
+ * local function proto type if needed
+ **************************************************************/
+/**************************************************************
+ * local function start with halbtc8723b2ant_
+ **************************************************************/
+static u8 halbtc8723b2ant_bt_rssi_state(u8 level_num, u8 rssi_thresh,
+					u8 rssi_thresh1)
+{
+	s32 bt_rssi = 0;
+	u8 bt_rssi_state = coex_sta->pre_bt_rssi_state;
+
+	bt_rssi = coex_sta->bt_rssi;
+
+	if (level_num == 2) {
+		if ((coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_LOW) ||
+		    (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+			if (bt_rssi >= rssi_thresh +
+				       BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT) {
+				bt_rssi_state = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state switch to High\n");
+			} else {
+				bt_rssi_state = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state stay at Low\n");
+			}
+		} else {
+			if (bt_rssi < rssi_thresh) {
+				bt_rssi_state = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state switch to Low\n");
+			} else {
+				bt_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state stay at High\n");
+			}
+		}
+	} else if (level_num == 3) {
+		if (rssi_thresh > rssi_thresh1) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+				  "[BTCoex], BT Rssi thresh error!!\n");
+			return coex_sta->pre_bt_rssi_state;
+		}
+
+		if ((coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_LOW) ||
+		    (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+			if (bt_rssi >= rssi_thresh +
+				       BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT) {
+				bt_rssi_state = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state switch to Medium\n");
+			} else {
+				bt_rssi_state = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state stay at Low\n");
+			}
+		} else if ((coex_sta->pre_bt_rssi_state ==
+						BTC_RSSI_STATE_MEDIUM) ||
+			   (coex_sta->pre_bt_rssi_state ==
+						BTC_RSSI_STATE_STAY_MEDIUM)) {
+			if (bt_rssi >= rssi_thresh1 +
+				       BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT) {
+				bt_rssi_state = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state switch to High\n");
+			} else if (bt_rssi < rssi_thresh) {
+				bt_rssi_state = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state switch to Low\n");
+			} else {
+				bt_rssi_state = BTC_RSSI_STATE_STAY_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state stay at Medium\n");
+			}
+		} else {
+			if (bt_rssi < rssi_thresh1) {
+				bt_rssi_state = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state switch to Medium\n");
+			} else {
+				bt_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state stay at High\n");
+			}
+		}
+	}
+
+	coex_sta->pre_bt_rssi_state = bt_rssi_state;
+
+	return bt_rssi_state;
+}
+
+static u8 halbtc8723b2ant_wifi_rssi_state(struct btc_coexist *btcoexist,
+					  u8 index, u8 level_num,
+					  u8 rssi_thresh, u8 rssi_thresh1)
+{
+	s32 wifi_rssi = 0;
+	u8 wifi_rssi_state = coex_sta->pre_wifi_rssi_state[index];
+
+	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
+
+	if (level_num == 2) {
+		if ((coex_sta->pre_wifi_rssi_state[index] ==
+						BTC_RSSI_STATE_LOW) ||
+		    (coex_sta->pre_wifi_rssi_state[index] ==
+						BTC_RSSI_STATE_STAY_LOW)) {
+			if (wifi_rssi >= rssi_thresh +
+					 BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT) {
+				wifi_rssi_state = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state switch to High\n");
+			} else {
+				wifi_rssi_state = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state stay at Low\n");
+			}
+		} else {
+			if (wifi_rssi < rssi_thresh) {
+				wifi_rssi_state = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state switch to Low\n");
+			} else {
+				wifi_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state stay at High\n");
+			}
+		}
+	} else if (level_num == 3) {
+		if (rssi_thresh > rssi_thresh1) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE,
+				  "[BTCoex], wifi RSSI thresh error!!\n");
+			return coex_sta->pre_wifi_rssi_state[index];
+		}
+
+		if ((coex_sta->pre_wifi_rssi_state[index] ==
+						BTC_RSSI_STATE_LOW) ||
+		    (coex_sta->pre_wifi_rssi_state[index] ==
+						BTC_RSSI_STATE_STAY_LOW)) {
+			if (wifi_rssi >= rssi_thresh +
+					BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT) {
+				wifi_rssi_state = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state switch to Medium\n");
+			} else {
+				wifi_rssi_state = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state stay at Low\n");
+			}
+		} else if ((coex_sta->pre_wifi_rssi_state[index] ==
+						BTC_RSSI_STATE_MEDIUM) ||
+			   (coex_sta->pre_wifi_rssi_state[index] ==
+						BTC_RSSI_STATE_STAY_MEDIUM)) {
+			if (wifi_rssi >= rssi_thresh1 +
+					 BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT) {
+				wifi_rssi_state = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state switch to High\n");
+			} else if (wifi_rssi < rssi_thresh) {
+				wifi_rssi_state = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state switch to Low\n");
+			} else {
+				wifi_rssi_state = BTC_RSSI_STATE_STAY_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state stay at Medium\n");
+			}
+		} else {
+			if (wifi_rssi < rssi_thresh1) {
+				wifi_rssi_state = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state switch to Medium\n");
+			} else {
+				wifi_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state stay at High\n");
+			}
+		}
+	}
+
+	coex_sta->pre_wifi_rssi_state[index] = wifi_rssi_state;
+
+	return wifi_rssi_state;
+}
+
+static void halbtc8723b2ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
+{
+	u32 reg_hp_txrx, reg_lp_txrx, u32tmp;
+	u32 reg_hp_tx = 0, reg_hp_rx = 0;
+	u32 reg_lp_tx = 0, reg_lp_rx = 0;
+
+	reg_hp_txrx = 0x770;
+	reg_lp_txrx = 0x774;
+
+	u32tmp = btcoexist->btc_read_4byte(btcoexist, reg_hp_txrx);
+	reg_hp_tx = u32tmp & MASKLWORD;
+	reg_hp_rx = (u32tmp & MASKHWORD) >> 16;
+
+	u32tmp = btcoexist->btc_read_4byte(btcoexist, reg_lp_txrx);
+	reg_lp_tx = u32tmp & MASKLWORD;
+	reg_lp_rx = (u32tmp & MASKHWORD) >> 16;
+
+	coex_sta->high_priority_tx = reg_hp_tx;
+	coex_sta->high_priority_rx = reg_hp_rx;
+	coex_sta->low_priority_tx = reg_lp_tx;
+	coex_sta->low_priority_rx = reg_lp_rx;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+		  "[BTCoex], High Priority Tx/Rx(reg 0x%x) = 0x%x(%d)/0x%x(%d)\n",
+		  reg_hp_txrx, reg_hp_tx, reg_hp_tx, reg_hp_rx, reg_hp_rx);
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+		  "[BTCoex], Low Priority Tx/Rx(reg 0x%x) = 0x%x(%d)/0x%x(%d)\n",
+		  reg_lp_txrx, reg_lp_tx, reg_lp_tx, reg_lp_rx, reg_lp_rx);
+
+	/* reset counter */
+	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
+}
+
+static void halbtc8723b2ant_query_bt_info(struct btc_coexist *btcoexist)
+{
+	u8 h2c_parameter[1] = {0};
+
+	coex_sta->c2h_bt_info_req_sent = true;
+
+	h2c_parameter[0] |= BIT(0);	/* trigger */
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], Query Bt Info, FW write 0x61 = 0x%x\n",
+		  h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x61, 1, h2c_parameter);
+}
+
+static bool is_wifi_status_changed(struct btc_coexist *btcoexist)
+{
+	static bool pre_wifi_busy;
+	static bool pre_under_4way;
+	static bool pre_bt_hs_on;
+	bool wifi_busy = false, under_4way = false, bt_hs_on = false;
+	bool wifi_connected = false;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+			   &wifi_connected);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS,
+			   &under_4way);
+
+	if (wifi_connected) {
+		if (wifi_busy != pre_wifi_busy) {
+			pre_wifi_busy = wifi_busy;
+			return true;
+		}
+
+		if (under_4way != pre_under_4way) {
+			pre_under_4way = under_4way;
+			return true;
+		}
+
+		if (bt_hs_on != pre_bt_hs_on) {
+			pre_bt_hs_on = bt_hs_on;
+			return true;
+		}
+	}
+
+	return false;
+}
+
+static void halbtc8723b2ant_update_bt_link_info(struct btc_coexist *btcoexist)
+{
+	/*struct btc_stack_info *stack_info = &btcoexist->stack_info;*/
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	bool bt_hs_on = false;
+
+#if (BT_AUTO_REPORT_ONLY_8723B_2ANT == 1) /* profile from bt patch */
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+
+	bt_link_info->bt_link_exist = coex_sta->bt_link_exist;
+	bt_link_info->sco_exist = coex_sta->sco_exist;
+	bt_link_info->a2dp_exist = coex_sta->a2dp_exist;
+	bt_link_info->pan_exist = coex_sta->pan_exist;
+	bt_link_info->hid_exist = coex_sta->hid_exist;
+
+	/* work around for HS mode. */
+	if (bt_hs_on) {
+		bt_link_info->pan_exist = true;
+		bt_link_info->bt_link_exist = true;
+	}
+#else	/* profile from bt stack */
+	bt_link_info->bt_link_exist = stack_info->bt_link_exist;
+	bt_link_info->sco_exist = stack_info->sco_exist;
+	bt_link_info->a2dp_exist = stack_info->a2dp_exist;
+	bt_link_info->pan_exist = stack_info->pan_exist;
+	bt_link_info->hid_exist = stack_info->hid_exist;
+
+	/*for win-8 stack HID report error*/
+	if (!stack_info->hid_exist)
+		stack_info->hid_exist = coex_sta->hid_exist;
+	/*sync  BTInfo with BT firmware and stack*/
+	/* when stack HID report error, here we use the info from bt fw.*/
+	if (!stack_info->bt_link_exist)
+		stack_info->bt_link_exist = coex_sta->bt_link_exist;
+#endif
+	/* check if Sco only */
+	if (bt_link_info->sco_exist && !bt_link_info->a2dp_exist &&
+	    !bt_link_info->pan_exist && !bt_link_info->hid_exist)
+		bt_link_info->sco_only = true;
+	else
+		bt_link_info->sco_only = false;
+
+	/* check if A2dp only */
+	if (!bt_link_info->sco_exist && bt_link_info->a2dp_exist &&
+	    !bt_link_info->pan_exist && !bt_link_info->hid_exist)
+		bt_link_info->a2dp_only = true;
+	else
+		bt_link_info->a2dp_only = false;
+
+	/* check if Pan only */
+	if (!bt_link_info->sco_exist && !bt_link_info->a2dp_exist &&
+	    bt_link_info->pan_exist && !bt_link_info->hid_exist)
+		bt_link_info->pan_only = true;
+	else
+		bt_link_info->pan_only = false;
+
+	/* check if Hid only */
+	if (!bt_link_info->sco_exist && !bt_link_info->a2dp_exist &&
+	    !bt_link_info->pan_exist && bt_link_info->hid_exist)
+		bt_link_info->hid_only = true;
+	else
+		bt_link_info->hid_only = false;
+}
+
+static u8 halbtc8723b2ant_action_algorithm(struct btc_coexist *btcoexist)
+{
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	bool bt_hs_on = false;
+	u8 algorithm = BT_8723B_2ANT_COEX_ALGO_UNDEFINED;
+	u8 num_of_diff_profile = 0;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+
+	if (!bt_link_info->bt_link_exist) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], No BT link exists!!!\n");
+		return algorithm;
+	}
+
+	if (bt_link_info->sco_exist)
+		num_of_diff_profile++;
+	if (bt_link_info->hid_exist)
+		num_of_diff_profile++;
+	if (bt_link_info->pan_exist)
+		num_of_diff_profile++;
+	if (bt_link_info->a2dp_exist)
+		num_of_diff_profile++;
+
+	if (num_of_diff_profile == 1) {
+		if (bt_link_info->sco_exist) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], SCO only\n");
+			algorithm = BT_8723B_2ANT_COEX_ALGO_SCO;
+		} else {
+			if (bt_link_info->hid_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], HID only\n");
+				algorithm = BT_8723B_2ANT_COEX_ALGO_HID;
+			} else if (bt_link_info->a2dp_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], A2DP only\n");
+				algorithm = BT_8723B_2ANT_COEX_ALGO_A2DP;
+			} else if (bt_link_info->pan_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], PAN(HS) only\n");
+					algorithm =
+						BT_8723B_2ANT_COEX_ALGO_PANHS;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], PAN(EDR) only\n");
+					algorithm =
+						BT_8723B_2ANT_COEX_ALGO_PANEDR;
+				}
+			}
+		}
+	} else if (num_of_diff_profile == 2) {
+		if (bt_link_info->sco_exist) {
+			if (bt_link_info->hid_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], SCO + HID\n");
+				algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+			} else if (bt_link_info->a2dp_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], SCO + A2DP ==> SCO\n");
+				algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+			} else if (bt_link_info->pan_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], SCO + PAN(HS)\n");
+					algorithm = BT_8723B_2ANT_COEX_ALGO_SCO;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], SCO + PAN(EDR)\n");
+					algorithm =
+					    BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		} else {
+			if (bt_link_info->hid_exist &&
+			    bt_link_info->a2dp_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], HID + A2DP\n");
+				algorithm = BT_8723B_2ANT_COEX_ALGO_HID_A2DP;
+			} else if (bt_link_info->hid_exist &&
+				   bt_link_info->pan_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], HID + PAN(HS)\n");
+					algorithm = BT_8723B_2ANT_COEX_ALGO_HID;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], HID + PAN(EDR)\n");
+					algorithm =
+					    BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			} else if (bt_link_info->pan_exist &&
+				   bt_link_info->a2dp_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], A2DP + PAN(HS)\n");
+					algorithm =
+					    BT_8723B_2ANT_COEX_ALGO_A2DP_PANHS;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], A2DP + PAN(EDR)\n");
+					algorithm =
+					    BT_8723B_2ANT_COEX_ALGO_PANEDR_A2DP;
+				}
+			}
+		}
+	} else if (num_of_diff_profile == 3) {
+		if (bt_link_info->sco_exist) {
+			if (bt_link_info->hid_exist &&
+			    bt_link_info->a2dp_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], SCO + HID + A2DP ==> HID\n");
+				algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+			} else if (bt_link_info->hid_exist &&
+				   bt_link_info->pan_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], SCO + HID + PAN(HS)\n");
+					algorithm =
+					    BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], SCO + HID + PAN(EDR)\n");
+					algorithm =
+					    BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			} else if (bt_link_info->pan_exist &&
+				   bt_link_info->a2dp_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], SCO + A2DP + PAN(HS)\n");
+					algorithm =
+					    BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], SCO + A2DP + PAN(EDR) ==> HID\n");
+					algorithm =
+					    BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		} else {
+			if (bt_link_info->hid_exist &&
+			    bt_link_info->pan_exist &&
+			    bt_link_info->a2dp_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], HID + A2DP + PAN(HS)\n");
+					algorithm =
+					    BT_8723B_2ANT_COEX_ALGO_HID_A2DP;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], HID + A2DP + PAN(EDR)\n");
+					algorithm =
+					BT_8723B_2ANT_COEX_ALGO_HID_A2DP_PANEDR;
+				}
+			}
+		}
+	} else if (num_of_diff_profile >= 3) {
+		if (bt_link_info->sco_exist) {
+			if (bt_link_info->hid_exist &&
+			    bt_link_info->pan_exist &&
+			    bt_link_info->a2dp_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], Error!!! SCO + HID + A2DP + PAN(HS)\n");
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], SCO + HID + A2DP + PAN(EDR) ==>PAN(EDR)+HID\n");
+					algorithm =
+					    BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		}
+	}
+
+	return algorithm;
+}
+
+static bool halbtc8723b2ant_need_to_dec_bt_pwr(struct btc_coexist *btcoexist)
+{
+	bool ret = false;
+	bool bt_hs_on = false, wifi_connected = false;
+	s32 bt_hs_rssi = 0;
+	u8 bt_rssi_state;
+
+	if (!btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION,
+				&bt_hs_on))
+		return false;
+	if (!btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+				&wifi_connected))
+		return false;
+	if (!btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI,
+				&bt_hs_rssi))
+		return false;
+
+	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
+
+	if (wifi_connected) {
+		if (bt_hs_on) {
+			if (bt_hs_rssi > 37) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+					  "[BTCoex], Need to decrease bt power for HS mode!!\n");
+				ret = true;
+			}
+		} else {
+			if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+					  "[BTCoex], Need to decrease bt power for Wifi is connected!!\n");
+				ret = true;
+			}
+		}
+	}
+
+	return ret;
+}
+
+static void set_fw_dac_swing_level(struct btc_coexist *btcoexist,
+				   u8 dac_swing_lvl)
+{
+	u8 h2c_parameter[1] = {0};
+
+	/* There are several type of dacswing
+	 * 0x18/ 0x10/ 0xc/ 0x8/ 0x4/ 0x6 */
+	h2c_parameter[0] = dac_swing_lvl;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], Set Dac Swing Level = 0x%x\n", dac_swing_lvl);
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], FW write 0x64 = 0x%x\n", h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x64, 1, h2c_parameter);
+}
+
+static void halbtc8723b2ant_set_fw_dec_bt_pwr(struct btc_coexist *btcoexist,
+					      bool dec_bt_pwr)
+{
+	u8 h2c_parameter[1] = {0};
+
+	h2c_parameter[0] = 0;
+
+	if (dec_bt_pwr)
+		h2c_parameter[0] |= BIT(1);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], decrease Bt Power : %s, FW write 0x62 = 0x%x\n",
+		  (dec_bt_pwr ? "Yes!!" : "No!!"), h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x62, 1, h2c_parameter);
+}
+
+static void halbtc8723b2ant_dec_bt_pwr(struct btc_coexist *btcoexist,
+				       bool force_exec, bool dec_bt_pwr)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], %s Dec BT power = %s\n",
+		  (force_exec ? "force to" : ""), (dec_bt_pwr ? "ON" : "OFF"));
+	coex_dm->cur_dec_bt_pwr = dec_bt_pwr;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], bPreDecBtPwr =%d, bCurDecBtPwr =%d\n",
+			  coex_dm->pre_dec_bt_pwr, coex_dm->cur_dec_bt_pwr);
+
+		if (coex_dm->pre_dec_bt_pwr == coex_dm->cur_dec_bt_pwr)
+			return;
+	}
+	halbtc8723b2ant_set_fw_dec_bt_pwr(btcoexist, coex_dm->cur_dec_bt_pwr);
+
+	coex_dm->pre_dec_bt_pwr = coex_dm->cur_dec_bt_pwr;
+}
+
+static void halbtc8723b2ant_fw_dac_swing_lvl(struct btc_coexist *btcoexist,
+					     bool force_exec,
+					     u8 fw_dac_swing_lvl)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], %s set FW Dac Swing level = %d\n",
+		  (force_exec ? "force to" : ""), fw_dac_swing_lvl);
+	coex_dm->cur_fw_dac_swing_lvl = fw_dac_swing_lvl;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], preFwDacSwingLvl =%d, curFwDacSwingLvl =%d\n",
+			  coex_dm->pre_fw_dac_swing_lvl,
+			  coex_dm->cur_fw_dac_swing_lvl);
+
+		if (coex_dm->pre_fw_dac_swing_lvl ==
+		   coex_dm->cur_fw_dac_swing_lvl)
+			return;
+	}
+
+	set_fw_dac_swing_level(btcoexist, coex_dm->cur_fw_dac_swing_lvl);
+	coex_dm->pre_fw_dac_swing_lvl = coex_dm->cur_fw_dac_swing_lvl;
+}
+
+static void set_sw_rf_rx_lpf_corner(struct btc_coexist *btcoexist,
+				    bool rx_rf_shrink_on)
+{
+	if (rx_rf_shrink_on) {
+		/* Shrink RF Rx LPF corner */
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+			  "[BTCoex], Shrink RF Rx LPF corner!!\n");
+		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1e,
+					  0xfffff, 0xffffc);
+	} else {
+		/* Resume RF Rx LPF corner */
+		/* After initialized, we can use coex_dm->btRf0x1eBackup */
+		if (btcoexist->initilized) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+				  "[BTCoex], Resume RF Rx LPF corner!!\n");
+			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1e,
+						  0xfffff,
+						  coex_dm->bt_rf0x1e_backup);
+		}
+	}
+}
+
+static void halbtc8723b2ant_rf_shrink(struct btc_coexist *btcoexist,
+				      bool force_exec, bool rx_rf_shrink_on)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
+		  "[BTCoex], %s turn Rx RF Shrink = %s\n",
+		  (force_exec ? "force to" : ""),
+		  (rx_rf_shrink_on ? "ON" : "OFF"));
+	coex_dm->cur_rf_rx_lpf_shrink = rx_rf_shrink_on;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "[BTCoex], bPreRfRxLpfShrink =%d, bCurRfRxLpfShrink =%d\n",
+			  coex_dm->pre_rf_rx_lpf_shrink,
+			  coex_dm->cur_rf_rx_lpf_shrink);
+
+		if (coex_dm->pre_rf_rx_lpf_shrink ==
+		    coex_dm->cur_rf_rx_lpf_shrink)
+			return;
+	}
+	set_sw_rf_rx_lpf_corner(btcoexist, coex_dm->cur_rf_rx_lpf_shrink);
+
+	coex_dm->pre_rf_rx_lpf_shrink = coex_dm->cur_rf_rx_lpf_shrink;
+}
+
+static void set_sw_penalty_txrate_adaptive(
+						struct btc_coexist *btcoexist,
+						bool low_penalty_ra)
+{
+	u8 h2c_parameter[6] = {0};
+
+	h2c_parameter[0] = 0x6;	/* opCode, 0x6 = Retry_Penalty*/
+
+	if (low_penalty_ra) {
+		h2c_parameter[1] |= BIT(0);
+		/*normal rate except MCS7/6/5, OFDM54/48/36*/
+		h2c_parameter[2] = 0x00;
+		h2c_parameter[3] = 0xf7;  /*MCS7 or OFDM54*/
+		h2c_parameter[4] = 0xf8;  /*MCS6 or OFDM48*/
+		h2c_parameter[5] = 0xf9;  /*MCS5 or OFDM36*/
+	}
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], set WiFi Low-Penalty Retry: %s",
+		  (low_penalty_ra ? "ON!!" : "OFF!!"));
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x69, 6, h2c_parameter);
+}
+
+static void halbtc8723b2ant_low_penalty_ra(struct btc_coexist *btcoexist,
+					   bool force_exec, bool low_penalty_ra)
+{
+	/*return; */
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
+		  "[BTCoex], %s turn LowPenaltyRA = %s\n",
+		  (force_exec ? "force to" : ""),
+		  (low_penalty_ra ? "ON" : "OFF"));
+	coex_dm->cur_low_penalty_ra = low_penalty_ra;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "[BTCoex], bPreLowPenaltyRa =%d, bCurLowPenaltyRa =%d\n",
+			  coex_dm->pre_low_penalty_ra,
+			  coex_dm->cur_low_penalty_ra);
+
+		if (coex_dm->pre_low_penalty_ra == coex_dm->cur_low_penalty_ra)
+			return;
+	}
+	set_sw_penalty_txrate_adaptive(btcoexist, coex_dm->cur_low_penalty_ra);
+
+	coex_dm->pre_low_penalty_ra = coex_dm->cur_low_penalty_ra;
+}
+
+static void halbtc8723b2ant_set_dac_swing_reg(struct btc_coexist *btcoexist,
+					      u32 level)
+{
+	u8 val = (u8) level;
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+		  "[BTCoex], Write SwDacSwing = 0x%x\n", level);
+	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x883, 0x3e, val);
+}
+
+static void set_sw_fulltime_dac_swing(struct btc_coexist *btcoexist,
+				      bool sw_dac_swing_on,
+				      u32 sw_dac_swing_lvl)
+{
+	if (sw_dac_swing_on)
+		halbtc8723b2ant_set_dac_swing_reg(btcoexist, sw_dac_swing_lvl);
+	else
+		halbtc8723b2ant_set_dac_swing_reg(btcoexist, 0x18);
+}
+
+static void halbtc8723b2ant_dac_swing(struct btc_coexist *btcoexist,
+				      bool force_exec, bool dac_swing_on,
+			       u32 dac_swing_lvl)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
+		  "[BTCoex], %s turn DacSwing =%s, dac_swing_lvl = 0x%x\n",
+		  (force_exec ? "force to" : ""),
+		  (dac_swing_on ? "ON" : "OFF"), dac_swing_lvl);
+	coex_dm->cur_dac_swing_on = dac_swing_on;
+	coex_dm->cur_dac_swing_lvl = dac_swing_lvl;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "[BTCoex], bPreDacSwingOn =%d, preDacSwingLvl = 0x%x, bCurDacSwingOn =%d, curDacSwingLvl = 0x%x\n",
+			  coex_dm->pre_dac_swing_on, coex_dm->pre_dac_swing_lvl,
+			  coex_dm->cur_dac_swing_on,
+			  coex_dm->cur_dac_swing_lvl);
+
+		if ((coex_dm->pre_dac_swing_on == coex_dm->cur_dac_swing_on) &&
+		    (coex_dm->pre_dac_swing_lvl == coex_dm->cur_dac_swing_lvl))
+			return;
+	}
+	mdelay(30);
+	set_sw_fulltime_dac_swing(btcoexist, dac_swing_on,
+				  dac_swing_lvl);
+
+	coex_dm->pre_dac_swing_on = coex_dm->cur_dac_swing_on;
+	coex_dm->pre_dac_swing_lvl = coex_dm->cur_dac_swing_lvl;
+}
+
+static void halbtc8723b2ant_set_agc_table(struct btc_coexist *btcoexist,
+					  bool agc_table_en)
+{
+	u8 rssi_adjust_val = 0;
+
+	/*  BB AGC Gain Table */
+	if (agc_table_en) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+			  "[BTCoex], BB Agc Table On!\n");
+		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x6e1A0001);
+		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x6d1B0001);
+		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x6c1C0001);
+		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x6b1D0001);
+		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x6a1E0001);
+		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x691F0001);
+		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x68200001);
+	} else {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+			  "[BTCoex], BB Agc Table Off!\n");
+		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xaa1A0001);
+		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa91B0001);
+		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa81C0001);
+		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa71D0001);
+		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa61E0001);
+		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa51F0001);
+		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa4200001);
+	}
+
+	/* RF Gain */
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0xef, 0xfffff, 0x02000);
+	if (agc_table_en) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+			  "[BTCoex], Agc Table On!\n");
+		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b,
+					  0xfffff, 0x38fff);
+		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b,
+					  0xfffff, 0x38ffe);
+	} else {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+			  "[BTCoex], Agc Table Off!\n");
+		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b,
+					  0xfffff, 0x380c3);
+		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b,
+					  0xfffff, 0x28ce6);
+	}
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0xef, 0xfffff, 0x0);
+
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0xed, 0xfffff, 0x1);
+
+	if (agc_table_en) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+			  "[BTCoex], Agc Table On!\n");
+		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x40,
+					  0xfffff, 0x38fff);
+		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x40,
+					  0xfffff, 0x38ffe);
+	} else {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+			  "[BTCoex], Agc Table Off!\n");
+		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x40,
+					  0xfffff, 0x380c3);
+		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x40,
+					  0xfffff, 0x28ce6);
+	}
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0xed, 0xfffff, 0x0);
+
+	/* set rssiAdjustVal for wifi module. */
+	if (agc_table_en)
+		rssi_adjust_val = 8;
+	btcoexist->btc_set(btcoexist, BTC_SET_U1_RSSI_ADJ_VAL_FOR_AGC_TABLE_ON,
+			   &rssi_adjust_val);
+}
+
+static void halbtc8723b2ant_agc_table(struct btc_coexist *btcoexist,
+				      bool force_exec, bool agc_table_en)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
+		  "[BTCoex], %s %s Agc Table\n",
+		  (force_exec ? "force to" : ""),
+		  (agc_table_en ? "Enable" : "Disable"));
+	coex_dm->cur_agc_table_en = agc_table_en;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "[BTCoex], bPreAgcTableEn =%d, bCurAgcTableEn =%d\n",
+			  coex_dm->pre_agc_table_en, coex_dm->cur_agc_table_en);
+
+		if (coex_dm->pre_agc_table_en == coex_dm->cur_agc_table_en)
+			return;
+	}
+	halbtc8723b2ant_set_agc_table(btcoexist, agc_table_en);
+
+	coex_dm->pre_agc_table_en = coex_dm->cur_agc_table_en;
+}
+
+static void halbtc8723b2ant_set_coex_table(struct btc_coexist *btcoexist,
+					   u32 val0x6c0, u32 val0x6c4,
+					   u32 val0x6c8, u8 val0x6cc)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+		  "[BTCoex], set coex table, set 0x6c0 = 0x%x\n", val0x6c0);
+	btcoexist->btc_write_4byte(btcoexist, 0x6c0, val0x6c0);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+		  "[BTCoex], set coex table, set 0x6c4 = 0x%x\n", val0x6c4);
+	btcoexist->btc_write_4byte(btcoexist, 0x6c4, val0x6c4);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+		  "[BTCoex], set coex table, set 0x6c8 = 0x%x\n", val0x6c8);
+	btcoexist->btc_write_4byte(btcoexist, 0x6c8, val0x6c8);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+		  "[BTCoex], set coex table, set 0x6cc = 0x%x\n", val0x6cc);
+	btcoexist->btc_write_1byte(btcoexist, 0x6cc, val0x6cc);
+}
+
+static void halbtc8723b2ant_coex_table(struct btc_coexist *btcoexist,
+				       bool force_exec, u32 val0x6c0,
+				       u32 val0x6c4, u32 val0x6c8,
+				       u8 val0x6cc)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
+		  "[BTCoex], %s write Coex Table 0x6c0 = 0x%x, 0x6c4 = 0x%x, 0x6c8 = 0x%x, 0x6cc = 0x%x\n",
+		  (force_exec ? "force to" : ""), val0x6c0,
+		  val0x6c4, val0x6c8, val0x6cc);
+	coex_dm->cur_val0x6c0 = val0x6c0;
+	coex_dm->cur_val0x6c4 = val0x6c4;
+	coex_dm->cur_val0x6c8 = val0x6c8;
+	coex_dm->cur_val0x6cc = val0x6cc;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "[BTCoex], preVal0x6c0 = 0x%x, preVal0x6c4 = 0x%x, preVal0x6c8 = 0x%x, preVal0x6cc = 0x%x !!\n",
+			  coex_dm->pre_val0x6c0, coex_dm->pre_val0x6c4,
+			  coex_dm->pre_val0x6c8, coex_dm->pre_val0x6cc);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "[BTCoex], curVal0x6c0 = 0x%x, curVal0x6c4 = 0x%x, curVal0x6c8 = 0x%x, curVal0x6cc = 0x%x !!\n",
+			  coex_dm->cur_val0x6c0, coex_dm->cur_val0x6c4,
+			  coex_dm->cur_val0x6c8, coex_dm->cur_val0x6cc);
+
+		if ((coex_dm->pre_val0x6c0 == coex_dm->cur_val0x6c0) &&
+		    (coex_dm->pre_val0x6c4 == coex_dm->cur_val0x6c4) &&
+		    (coex_dm->pre_val0x6c8 == coex_dm->cur_val0x6c8) &&
+		    (coex_dm->pre_val0x6cc == coex_dm->cur_val0x6cc))
+			return;
+	}
+	halbtc8723b2ant_set_coex_table(btcoexist, val0x6c0, val0x6c4,
+				       val0x6c8, val0x6cc);
+
+	coex_dm->pre_val0x6c0 = coex_dm->cur_val0x6c0;
+	coex_dm->pre_val0x6c4 = coex_dm->cur_val0x6c4;
+	coex_dm->pre_val0x6c8 = coex_dm->cur_val0x6c8;
+	coex_dm->pre_val0x6cc = coex_dm->cur_val0x6cc;
+}
+
+static void coex_table_with_type(struct btc_coexist *btcoexist,
+						 bool force_exec, u8 type)
+{
+	switch (type) {
+	case 0:
+		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55555555,
+					   0x55555555, 0xffff, 0x3);
+		break;
+	case 1:
+		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55555555,
+					   0x5afa5afa, 0xffff, 0x3);
+		break;
+	case 2:
+		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x5a5a5a5a,
+					   0x5a5a5a5a, 0xffff, 0x3);
+		break;
+	case 3:
+		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0xaaaaaaaa,
+					   0xaaaaaaaa, 0xffff, 0x3);
+		break;
+	case 4:
+		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0xffffffff,
+					   0xffffffff, 0xffff, 0x3);
+		break;
+	case 5:
+		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x5fff5fff,
+					   0x5fff5fff, 0xffff, 0x3);
+		break;
+	case 6:
+		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
+					   0x5a5a5a5a, 0xffff, 0x3);
+		break;
+	case 7:
+		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
+					   0x5afa5afa, 0xffff, 0x3);
+		break;
+	case 8:
+		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x5aea5aea,
+					   0x5aea5aea, 0xffff, 0x3);
+		break;
+	case 9:
+		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
+					   0x5aea5aea, 0xffff, 0x3);
+		break;
+	case 10:
+		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
+					   0x5aff5aff, 0xffff, 0x3);
+		break;
+	case 11:
+		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
+					   0x5a5f5a5f, 0xffff, 0x3);
+		break;
+	case 12:
+		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
+					   0x5f5f5f5f, 0xffff, 0x3);
+		break;
+	default:
+		break;
+	}
+}
+
+static void set_fw_ignore_wlan_act(struct btc_coexist *btcoexist,
+				   bool enable)
+{
+	u8 h2c_parameter[1] = {0};
+
+	if (enable)
+		h2c_parameter[0] |= BIT(0);/* function enable*/
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], set FW for BT Ignore Wlan_Act, FW write 0x63 = 0x%x\n",
+		  h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x63, 1, h2c_parameter);
+}
+
+static void halbtc8723b2ant_ignore_wlan_act(struct btc_coexist *btcoexist,
+					    bool force_exec, bool enable)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], %s turn Ignore WlanAct %s\n",
+		  (force_exec ? "force to" : ""), (enable ? "ON" : "OFF"));
+	coex_dm->cur_ignore_wlan_act = enable;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], bPreIgnoreWlanAct = %d, bCurIgnoreWlanAct = %d!!\n",
+			  coex_dm->pre_ignore_wlan_act,
+			  coex_dm->cur_ignore_wlan_act);
+
+		if (coex_dm->pre_ignore_wlan_act ==
+		    coex_dm->cur_ignore_wlan_act)
+			return;
+	}
+	set_fw_ignore_wlan_act(btcoexist, enable);
+
+	coex_dm->pre_ignore_wlan_act = coex_dm->cur_ignore_wlan_act;
+}
+
+static void set_fw_ps_tdma(struct btc_coexist *btcoexist, u8 byte1,
+			   u8 byte2, u8 byte3, u8 byte4, u8 byte5)
+{
+	u8 h2c_parameter[5] = {0};
+
+	h2c_parameter[0] = byte1;
+	h2c_parameter[1] = byte2;
+	h2c_parameter[2] = byte3;
+	h2c_parameter[3] = byte4;
+	h2c_parameter[4] = byte5;
+
+	coex_dm->ps_tdma_para[0] = byte1;
+	coex_dm->ps_tdma_para[1] = byte2;
+	coex_dm->ps_tdma_para[2] = byte3;
+	coex_dm->ps_tdma_para[3] = byte4;
+	coex_dm->ps_tdma_para[4] = byte5;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], FW write 0x60(5bytes) = 0x%x%08x\n",
+		  h2c_parameter[0],
+		  h2c_parameter[1] << 24 | h2c_parameter[2] << 16 |
+		  h2c_parameter[3] << 8 | h2c_parameter[4]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x60, 5, h2c_parameter);
+}
+
+static void halbtc8723b2ant_sw_mechanism1(struct btc_coexist *btcoexist,
+					  bool shrink_rx_lpf,
+					  bool low_penalty_ra,
+					  bool limited_dig,
+					  bool bt_lna_constrain)
+{
+	halbtc8723b2ant_rf_shrink(btcoexist, NORMAL_EXEC, shrink_rx_lpf);
+	halbtc8723b2ant_low_penalty_ra(btcoexist, NORMAL_EXEC, low_penalty_ra);
+}
+
+static void halbtc8723b2ant_sw_mechanism2(struct btc_coexist *btcoexist,
+					  bool agc_table_shift,
+					  bool adc_backoff,
+					  bool sw_dac_swing, u32 dac_swing_lvl)
+{
+	halbtc8723b2ant_agc_table(btcoexist, NORMAL_EXEC, agc_table_shift);
+	halbtc8723b2ant_dac_swing(btcoexist, NORMAL_EXEC, sw_dac_swing,
+				  dac_swing_lvl);
+}
+
+static void halbtc8723b2ant_set_ant_path(struct btc_coexist *btcoexist,
+					 u8 antpos_type, bool init_hwcfg,
+					 bool wifi_off)
+{
+	struct btc_board_info *board_info = &btcoexist->board_info;
+	u32 fw_ver = 0, u32tmp = 0;
+	bool pg_ext_switch = false;
+	bool use_ext_switch = false;
+	u8 h2c_parameter[2] = {0};
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_EXT_SWITCH, &pg_ext_switch);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
+
+	if ((fw_ver < 0xc0000) || pg_ext_switch)
+		use_ext_switch = true;
+
+	if (init_hwcfg) {
+		/* 0x4c[23] = 0, 0x4c[24] = 1  Antenna control by WL/BT */
+		u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
+		u32tmp &= ~BIT(23);
+		u32tmp |= BIT(24);
+		btcoexist->btc_write_4byte(btcoexist, 0x4c, u32tmp);
+
+		btcoexist->btc_write_1byte(btcoexist, 0x974, 0xff);
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x944, 0x3, 0x3);
+		btcoexist->btc_write_1byte(btcoexist, 0x930, 0x77);
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x67, 0x20, 0x1);
+
+		/* Force GNT_BT to low */
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x765, 0x18, 0x0);
+		btcoexist->btc_write_2byte(btcoexist, 0x948, 0x0);
+
+		if (board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT) {
+			/* tell firmware "no antenna inverse" */
+			h2c_parameter[0] = 0;
+			h2c_parameter[1] = 1;  /* ext switch type */
+			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
+						h2c_parameter);
+		} else {
+			/* tell firmware "antenna inverse" */
+			h2c_parameter[0] = 1;
+			h2c_parameter[1] = 1;  /* ext switch type */
+			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
+						h2c_parameter);
+		}
+	}
+
+	/* ext switch setting */
+	if (use_ext_switch) {
+		/* fixed internal switch S1->WiFi, S0->BT */
+		btcoexist->btc_write_2byte(btcoexist, 0x948, 0x0);
+		switch (antpos_type) {
+		case BTC_ANT_WIFI_AT_MAIN:
+			/* ext switch main at wifi */
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x92c,
+							   0x3, 0x1);
+			break;
+		case BTC_ANT_WIFI_AT_AUX:
+			/* ext switch aux at wifi */
+			btcoexist->btc_write_1byte_bitmask(btcoexist,
+							   0x92c, 0x3, 0x2);
+			break;
+		}
+	} else {	/* internal switch */
+		/* fixed ext switch */
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x92c, 0x3, 0x1);
+		switch (antpos_type) {
+		case BTC_ANT_WIFI_AT_MAIN:
+			/* fixed internal switch S1->WiFi, S0->BT */
+			btcoexist->btc_write_2byte(btcoexist, 0x948, 0x0);
+			break;
+		case BTC_ANT_WIFI_AT_AUX:
+			/* fixed internal switch S0->WiFi, S1->BT */
+			btcoexist->btc_write_2byte(btcoexist, 0x948, 0x280);
+			break;
+		}
+	}
+}
+
+static void halbtc8723b2ant_ps_tdma(struct btc_coexist *btcoexist,
+				    bool force_exec, bool turn_on, u8 type)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], %s turn %s PS TDMA, type =%d\n",
+		  (force_exec ? "force to" : ""),
+		  (turn_on ? "ON" : "OFF"), type);
+	coex_dm->cur_ps_tdma_on = turn_on;
+	coex_dm->cur_ps_tdma = type;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], bPrePsTdmaOn = %d, bCurPsTdmaOn = %d!!\n",
+			  coex_dm->pre_ps_tdma_on, coex_dm->cur_ps_tdma_on);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], prePsTdma = %d, curPsTdma = %d!!\n",
+			  coex_dm->pre_ps_tdma, coex_dm->cur_ps_tdma);
+
+		if ((coex_dm->pre_ps_tdma_on == coex_dm->cur_ps_tdma_on) &&
+		    (coex_dm->pre_ps_tdma == coex_dm->cur_ps_tdma))
+			return;
+	}
+	if (turn_on) {
+		switch (type) {
+		case 1:
+		default:
+			set_fw_ps_tdma(btcoexist, 0xe3, 0x1a, 0x1a, 0xe1, 0x90);
+			break;
+		case 2:
+			set_fw_ps_tdma(btcoexist, 0xe3, 0x12, 0x12, 0xe1, 0x90);
+			break;
+		case 3:
+			set_fw_ps_tdma(btcoexist, 0xe3, 0x1c, 0x3, 0xf1, 0x90);
+			break;
+		case 4:
+			set_fw_ps_tdma(btcoexist, 0xe3, 0x10, 0x03, 0xf1, 0x90);
+			break;
+		case 5:
+			set_fw_ps_tdma(btcoexist, 0xe3, 0x1a, 0x1a, 0x60, 0x90);
+			break;
+		case 6:
+			set_fw_ps_tdma(btcoexist, 0xe3, 0x12, 0x12, 0x60, 0x90);
+			break;
+		case 7:
+			set_fw_ps_tdma(btcoexist, 0xe3, 0x1c, 0x3, 0x70, 0x90);
+			break;
+		case 8:
+			set_fw_ps_tdma(btcoexist, 0xa3, 0x10, 0x3, 0x70, 0x90);
+			break;
+		case 9:
+			set_fw_ps_tdma(btcoexist, 0xe3, 0x1a, 0x1a, 0xe1, 0x90);
+			break;
+		case 10:
+			set_fw_ps_tdma(btcoexist, 0xe3, 0x12, 0x12, 0xe1, 0x90);
+			break;
+		case 11:
+			set_fw_ps_tdma(btcoexist, 0xe3, 0xa, 0xa, 0xe1, 0x90);
+			break;
+		case 12:
+			set_fw_ps_tdma(btcoexist, 0xe3, 0x5, 0x5, 0xe1, 0x90);
+			break;
+		case 13:
+			set_fw_ps_tdma(btcoexist, 0xe3, 0x1a, 0x1a, 0x60, 0x90);
+			break;
+		case 14:
+			set_fw_ps_tdma(btcoexist, 0xe3, 0x12, 0x12, 0x60, 0x90);
+			break;
+		case 15:
+			set_fw_ps_tdma(btcoexist, 0xe3, 0xa, 0xa, 0x60, 0x90);
+			break;
+		case 16:
+			set_fw_ps_tdma(btcoexist, 0xe3, 0x5, 0x5, 0x60, 0x90);
+			break;
+		case 17:
+			set_fw_ps_tdma(btcoexist, 0xa3, 0x2f, 0x2f, 0x60, 0x90);
+			break;
+		case 18:
+			set_fw_ps_tdma(btcoexist, 0xe3, 0x5, 0x5, 0xe1, 0x90);
+			break;
+		case 19:
+			set_fw_ps_tdma(btcoexist, 0xe3, 0x25, 0x25, 0xe1, 0x90);
+			break;
+		case 20:
+			set_fw_ps_tdma(btcoexist, 0xe3, 0x25, 0x25, 0x60, 0x90);
+			break;
+		case 21:
+			set_fw_ps_tdma(btcoexist, 0xe3, 0x15, 0x03, 0x70, 0x90);
+			break;
+		case 71:
+			set_fw_ps_tdma(btcoexist, 0xe3, 0x1a, 0x1a, 0xe1, 0x90);
+			break;
+		}
+	} else {
+		/* disable PS tdma */
+		switch (type) {
+		case 0:
+			set_fw_ps_tdma(btcoexist, 0x0, 0x0, 0x0, 0x40, 0x0);
+			break;
+		case 1:
+			set_fw_ps_tdma(btcoexist, 0x0, 0x0, 0x0, 0x48, 0x0);
+			break;
+		default:
+			set_fw_ps_tdma(btcoexist, 0x0, 0x0, 0x0, 0x40, 0x0);
+			break;
+		}
+	}
+
+	/* update pre state */
+	coex_dm->pre_ps_tdma_on = coex_dm->cur_ps_tdma_on;
+	coex_dm->pre_ps_tdma = coex_dm->cur_ps_tdma;
+}
+
+static void halbtc8723b2ant_coex_alloff(struct btc_coexist *btcoexist)
+{
+	/* fw all off */
+	halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
+	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+	halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	/* sw all off */
+	halbtc8723b2ant_sw_mechanism1(btcoexist, false, false, false, false);
+	halbtc8723b2ant_sw_mechanism2(btcoexist, false, false, false, 0x18);
+
+	/* hw all off */
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+	coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
+}
+
+static void halbtc8723b2ant_init_coex_dm(struct btc_coexist *btcoexist)
+{
+	/* force to reset coex mechanism*/
+
+	halbtc8723b2ant_ps_tdma(btcoexist, FORCE_EXEC, false, 1);
+	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, FORCE_EXEC, 6);
+	halbtc8723b2ant_dec_bt_pwr(btcoexist, FORCE_EXEC, false);
+
+	halbtc8723b2ant_sw_mechanism1(btcoexist, false, false, false, false);
+	halbtc8723b2ant_sw_mechanism2(btcoexist, false, false, false, 0x18);
+}
+
+static void halbtc8723b2ant_action_bt_inquiry(struct btc_coexist *btcoexist)
+{
+	bool wifi_connected = false;
+	bool low_pwr_disable = true;
+
+	btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
+			   &low_pwr_disable);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+			   &wifi_connected);
+
+	if (wifi_connected) {
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
+		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 3);
+	} else {
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
+		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
+	}
+	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, FORCE_EXEC, 6);
+	halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	halbtc8723b2ant_sw_mechanism1(btcoexist, false, false, false, false);
+	halbtc8723b2ant_sw_mechanism2(btcoexist, false, false, false, 0x18);
+
+	coex_dm->need_recover_0x948 = true;
+	coex_dm->backup_0x948 = btcoexist->btc_read_2byte(btcoexist, 0x948);
+
+	halbtc8723b2ant_set_ant_path(btcoexist, BTC_ANT_WIFI_AT_AUX,
+				     false, false);
+}
+
+static bool halbtc8723b2ant_is_common_action(struct btc_coexist *btcoexist)
+{
+	bool common = false, wifi_connected = false;
+	bool wifi_busy = false;
+	bool bt_hs_on = false, low_pwr_disable = false;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+			   &wifi_connected);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+
+	if (!wifi_connected) {
+		low_pwr_disable = false;
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
+				   &low_pwr_disable);
+
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Wifi non-connected idle!!\n");
+
+		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff,
+					  0x0);
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
+		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
+		halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+		halbtc8723b2ant_sw_mechanism1(btcoexist, false, false, false,
+					      false);
+		halbtc8723b2ant_sw_mechanism2(btcoexist, false, false, false,
+					      0x18);
+
+		common = true;
+	} else {
+		if (BT_8723B_2ANT_BT_STATUS_NON_CONNECTED_IDLE ==
+		    coex_dm->bt_status) {
+			low_pwr_disable = false;
+			btcoexist->btc_set(btcoexist,
+					   BTC_SET_ACT_DISABLE_LOW_POWER,
+					   &low_pwr_disable);
+
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Wifi connected + BT non connected-idle!!\n");
+
+			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1,
+						  0xfffff, 0x0);
+			coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 0);
+			halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false,
+						1);
+			halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC,
+							 0xb);
+			halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC,
+						   false);
+
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+
+			common = true;
+		} else if (BT_8723B_2ANT_BT_STATUS_CONNECTED_IDLE ==
+			   coex_dm->bt_status) {
+			low_pwr_disable = true;
+			btcoexist->btc_set(btcoexist,
+					   BTC_SET_ACT_DISABLE_LOW_POWER,
+					   &low_pwr_disable);
+
+			if (bt_hs_on)
+				return false;
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Wifi connected + BT connected-idle!!\n");
+
+			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1,
+						  0xfffff, 0x0);
+			coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 0);
+			halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false,
+						1);
+			halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC,
+							 0xb);
+			halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC,
+						   false);
+
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+
+			common = true;
+		} else {
+			low_pwr_disable = true;
+			btcoexist->btc_set(btcoexist,
+					   BTC_SET_ACT_DISABLE_LOW_POWER,
+					   &low_pwr_disable);
+
+			if (wifi_busy) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], Wifi Connected-Busy + BT Busy!!\n");
+				common = false;
+			} else {
+				if (bt_hs_on)
+					return false;
+
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], Wifi Connected-Idle + BT Busy!!\n");
+
+				btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A,
+							  0x1, 0xfffff, 0x0);
+				coex_table_with_type(btcoexist,
+					    NORMAL_EXEC, 7);
+				halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 21);
+				halbtc8723b2ant_fw_dac_swing_lvl(btcoexist,
+								 NORMAL_EXEC,
+								 0xb);
+				if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
+					halbtc8723b2ant_dec_bt_pwr(btcoexist,
+								   NORMAL_EXEC,
+								   true);
+				else
+					halbtc8723b2ant_dec_bt_pwr(btcoexist,
+								   NORMAL_EXEC,
+								   false);
+				halbtc8723b2ant_sw_mechanism1(btcoexist, false,
+							      false, false,
+							      false);
+				halbtc8723b2ant_sw_mechanism2(btcoexist, false,
+							      false, false,
+							      0x18);
+				common = true;
+			}
+		}
+	}
+
+	return common;
+}
+static void halbtc8723b2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
+						 bool sco_hid, bool tx_pause,
+						 u8 max_interval)
+{
+	static s32 up, dn, m, n, wait_count;
+	/*0: no change, +1: increase WiFi duration, -1: decrease WiFi duration*/
+	s32 result;
+	u8 retrycount = 0;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], TdmaDurationAdjust()\n");
+
+	if (!coex_dm->auto_tdma_adjust) {
+		coex_dm->auto_tdma_adjust = true;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], first run TdmaDurationAdjust()!!\n");
+		if (sco_hid) {
+			if (tx_pause) {
+				if (max_interval == 1) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 13);
+					coex_dm->ps_tdma_du_adj_type = 13;
+				} else if (max_interval == 2) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 14);
+					coex_dm->ps_tdma_du_adj_type = 14;
+				} else if (max_interval == 3) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 15);
+					coex_dm->ps_tdma_du_adj_type = 15;
+				} else {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 15);
+					coex_dm->ps_tdma_du_adj_type = 15;
+				}
+			} else {
+				if (max_interval == 1) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 9);
+					coex_dm->ps_tdma_du_adj_type = 9;
+				} else if (max_interval == 2) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 10);
+					coex_dm->ps_tdma_du_adj_type = 10;
+				} else if (max_interval == 3) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 11);
+					coex_dm->ps_tdma_du_adj_type = 11;
+				} else {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 11);
+					coex_dm->ps_tdma_du_adj_type = 11;
+				}
+			}
+		} else {
+			if (tx_pause) {
+				if (max_interval == 1) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 5);
+					coex_dm->ps_tdma_du_adj_type = 5;
+				} else if (max_interval == 2) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 6);
+					coex_dm->ps_tdma_du_adj_type = 6;
+				} else if (max_interval == 3) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 7);
+					coex_dm->ps_tdma_du_adj_type = 7;
+				} else {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 7);
+					coex_dm->ps_tdma_du_adj_type = 7;
+				}
+			} else {
+				if (max_interval == 1) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 1);
+					coex_dm->ps_tdma_du_adj_type = 1;
+				} else if (max_interval == 2) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 2);
+					coex_dm->ps_tdma_du_adj_type = 2;
+				} else if (max_interval == 3) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 3);
+					coex_dm->ps_tdma_du_adj_type = 3;
+				} else {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 3);
+					coex_dm->ps_tdma_du_adj_type = 3;
+				}
+			}
+		}
+
+		up = 0;
+		dn = 0;
+		m = 1;
+		n = 3;
+		result = 0;
+		wait_count = 0;
+	} else {
+		/*accquire the BT TRx retry count from BT_Info byte2*/
+		retrycount = coex_sta->bt_retry_cnt;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], retrycount = %d\n", retrycount);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], up =%d, dn =%d, m =%d, n =%d, wait_count =%d\n",
+			  up, dn, m, n, wait_count);
+		result = 0;
+		wait_count++;
+		 /* no retry in the last 2-second duration*/
+		if (retrycount == 0) {
+			up++;
+			dn--;
+
+			if (dn <= 0)
+				dn = 0;
+
+			if (up >= n) {
+				wait_count = 0;
+				n = 3;
+				up = 0;
+				dn = 0;
+				result = 1;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_TRACE_FW_DETAIL,
+					  "[BTCoex], Increase wifi duration!!\n");
+			} /* <= 3 retry in the last 2-second duration*/
+		} else if (retrycount <= 3) {
+			up--;
+			dn++;
+
+			if (up <= 0)
+				up = 0;
+
+			if (dn == 2) {
+				if (wait_count <= 2)
+					m++;
+				else
+					m = 1;
+
+				if (m >= 20)
+					m = 20;
+
+				n = 3 * m;
+				up = 0;
+				dn = 0;
+				wait_count = 0;
+				result = -1;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_TRACE_FW_DETAIL,
+					  "[BTCoex], Decrease wifi duration for retrycounter<3!!\n");
+			}
+		} else {
+			if (wait_count == 1)
+				m++;
+			else
+				m = 1;
+
+			if (m >= 20)
+				m = 20;
+
+			n = 3 * m;
+			up = 0;
+			dn = 0;
+			wait_count = 0;
+			result = -1;
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+				  "[BTCoex], Decrease wifi duration for retrycounter>3!!\n");
+		}
+
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], max Interval = %d\n", max_interval);
+		if (max_interval == 1) {
+			if (tx_pause) {
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_TRACE_FW_DETAIL,
+					  "[BTCoex], TxPause = 1\n");
+
+				if (coex_dm->cur_ps_tdma == 71) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 5);
+					coex_dm->ps_tdma_du_adj_type = 5;
+				} else if (coex_dm->cur_ps_tdma == 1) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 5);
+					coex_dm->ps_tdma_du_adj_type = 5;
+				} else if (coex_dm->cur_ps_tdma == 2) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 6);
+					coex_dm->ps_tdma_du_adj_type = 6;
+				} else if (coex_dm->cur_ps_tdma == 3) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 7);
+					coex_dm->ps_tdma_du_adj_type = 7;
+				} else if (coex_dm->cur_ps_tdma == 4) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 8);
+					coex_dm->ps_tdma_du_adj_type = 8;
+				}
+
+				if (coex_dm->cur_ps_tdma == 9) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 13);
+					coex_dm->ps_tdma_du_adj_type = 13;
+				} else if (coex_dm->cur_ps_tdma == 10) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 14);
+					coex_dm->ps_tdma_du_adj_type = 14;
+				} else if (coex_dm->cur_ps_tdma == 11) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 15);
+					coex_dm->ps_tdma_du_adj_type = 15;
+				} else if (coex_dm->cur_ps_tdma == 12) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 16);
+					coex_dm->ps_tdma_du_adj_type = 16;
+				}
+
+				if (result == -1) {
+					if (coex_dm->cur_ps_tdma == 5) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 6);
+						coex_dm->ps_tdma_du_adj_type =
+									      6;
+					} else if (coex_dm->cur_ps_tdma == 6) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 7);
+						coex_dm->ps_tdma_du_adj_type =
+									      7;
+					} else if (coex_dm->cur_ps_tdma == 7) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 8);
+						coex_dm->ps_tdma_du_adj_type =
+									      8;
+					} else if (coex_dm->cur_ps_tdma == 13) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 14);
+						coex_dm->ps_tdma_du_adj_type =
+									     14;
+					} else if (coex_dm->cur_ps_tdma == 14) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 15);
+						coex_dm->ps_tdma_du_adj_type =
+									     15;
+					} else if (coex_dm->cur_ps_tdma == 15) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 16);
+						coex_dm->ps_tdma_du_adj_type =
+									     16;
+					}
+				}  else if (result == 1) {
+					if (coex_dm->cur_ps_tdma == 8) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 7);
+						coex_dm->ps_tdma_du_adj_type =
+									      7;
+					} else if (coex_dm->cur_ps_tdma == 7) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 6);
+						coex_dm->ps_tdma_du_adj_type =
+									      6;
+					} else if (coex_dm->cur_ps_tdma == 6) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 5);
+						coex_dm->ps_tdma_du_adj_type =
+									      5;
+					} else if (coex_dm->cur_ps_tdma == 16) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 15);
+						coex_dm->ps_tdma_du_adj_type =
+									     15;
+					} else if (coex_dm->cur_ps_tdma == 15) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 14);
+						coex_dm->ps_tdma_du_adj_type =
+									     14;
+					} else if (coex_dm->cur_ps_tdma == 14) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 13);
+						coex_dm->ps_tdma_du_adj_type =
+									     13;
+					}
+				}
+			} else {
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_TRACE_FW_DETAIL,
+					  "[BTCoex], TxPause = 0\n");
+				if (coex_dm->cur_ps_tdma == 5) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 71);
+					coex_dm->ps_tdma_du_adj_type = 71;
+				} else if (coex_dm->cur_ps_tdma == 6) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 2);
+					coex_dm->ps_tdma_du_adj_type = 2;
+				} else if (coex_dm->cur_ps_tdma == 7) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 3);
+					coex_dm->ps_tdma_du_adj_type = 3;
+				} else if (coex_dm->cur_ps_tdma == 8) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 4);
+					coex_dm->ps_tdma_du_adj_type = 4;
+				}
+
+				if (coex_dm->cur_ps_tdma == 13) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 9);
+					coex_dm->ps_tdma_du_adj_type = 9;
+				} else if (coex_dm->cur_ps_tdma == 14) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 10);
+					coex_dm->ps_tdma_du_adj_type = 10;
+				} else if (coex_dm->cur_ps_tdma == 15) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 11);
+					coex_dm->ps_tdma_du_adj_type = 11;
+				} else if (coex_dm->cur_ps_tdma == 16) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 12);
+					coex_dm->ps_tdma_du_adj_type = 12;
+				}
+
+				if (result == -1) {
+					if (coex_dm->cur_ps_tdma == 71) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 1);
+						coex_dm->ps_tdma_du_adj_type =
+									      1;
+					} else if (coex_dm->cur_ps_tdma == 1) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 2);
+						coex_dm->ps_tdma_du_adj_type =
+									      2;
+					} else if (coex_dm->cur_ps_tdma == 2) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 3);
+						coex_dm->ps_tdma_du_adj_type =
+									      3;
+					} else if (coex_dm->cur_ps_tdma == 3) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 4);
+						coex_dm->ps_tdma_du_adj_type =
+									      4;
+					} else if (coex_dm->cur_ps_tdma == 9) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 10);
+						coex_dm->ps_tdma_du_adj_type =
+									     10;
+					} else if (coex_dm->cur_ps_tdma == 10) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 11);
+						coex_dm->ps_tdma_du_adj_type =
+									     11;
+					} else if (coex_dm->cur_ps_tdma == 11) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 12);
+						coex_dm->ps_tdma_du_adj_type =
+									     12;
+					}
+				}  else if (result == 1) {
+					if (coex_dm->cur_ps_tdma == 4) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 3);
+						coex_dm->ps_tdma_du_adj_type =
+									      3;
+					} else if (coex_dm->cur_ps_tdma == 3) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 2);
+						coex_dm->ps_tdma_du_adj_type =
+									      2;
+					} else if (coex_dm->cur_ps_tdma == 2) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 1);
+						coex_dm->ps_tdma_du_adj_type =
+									      1;
+					} else if (coex_dm->cur_ps_tdma == 1) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 71);
+						coex_dm->ps_tdma_du_adj_type =
+									     71;
+					} else if (coex_dm->cur_ps_tdma == 12) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 11);
+						coex_dm->ps_tdma_du_adj_type =
+									     11;
+					} else if (coex_dm->cur_ps_tdma == 11) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 10);
+						coex_dm->ps_tdma_du_adj_type =
+									     10;
+					} else if (coex_dm->cur_ps_tdma == 10) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 9);
+						coex_dm->ps_tdma_du_adj_type =
+									      9;
+					}
+				}
+			}
+		} else if (max_interval == 2) {
+			if (tx_pause) {
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_TRACE_FW_DETAIL,
+					  "[BTCoex], TxPause = 1\n");
+				if (coex_dm->cur_ps_tdma == 1) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 6);
+					coex_dm->ps_tdma_du_adj_type = 6;
+				} else if (coex_dm->cur_ps_tdma == 2) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 6);
+					coex_dm->ps_tdma_du_adj_type = 6;
+				} else if (coex_dm->cur_ps_tdma == 3) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 7);
+					coex_dm->ps_tdma_du_adj_type = 7;
+				} else if (coex_dm->cur_ps_tdma == 4) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 8);
+					coex_dm->ps_tdma_du_adj_type = 8;
+				}
+				if (coex_dm->cur_ps_tdma == 9) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 14);
+					coex_dm->ps_tdma_du_adj_type = 14;
+				} else if (coex_dm->cur_ps_tdma == 10) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 14);
+					coex_dm->ps_tdma_du_adj_type = 14;
+				} else if (coex_dm->cur_ps_tdma == 11) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 15);
+					coex_dm->ps_tdma_du_adj_type = 15;
+				} else if (coex_dm->cur_ps_tdma == 12) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 16);
+					coex_dm->ps_tdma_du_adj_type = 16;
+				}
+				if (result == -1) {
+					if (coex_dm->cur_ps_tdma == 5) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 6);
+						coex_dm->ps_tdma_du_adj_type =
+									      6;
+					} else if (coex_dm->cur_ps_tdma == 6) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 7);
+						coex_dm->ps_tdma_du_adj_type =
+									      7;
+					} else if (coex_dm->cur_ps_tdma == 7) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 8);
+						coex_dm->ps_tdma_du_adj_type =
+									      8;
+					} else if (coex_dm->cur_ps_tdma == 13) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 14);
+						coex_dm->ps_tdma_du_adj_type =
+									     14;
+					} else if (coex_dm->cur_ps_tdma == 14) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 15);
+						coex_dm->ps_tdma_du_adj_type =
+									     15;
+					} else if (coex_dm->cur_ps_tdma == 15) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 16);
+						coex_dm->ps_tdma_du_adj_type =
+									     16;
+					}
+				}  else if (result == 1) {
+					if (coex_dm->cur_ps_tdma == 8) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 7);
+						coex_dm->ps_tdma_du_adj_type =
+									      7;
+					} else if (coex_dm->cur_ps_tdma == 7) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 6);
+						coex_dm->ps_tdma_du_adj_type =
+									      6;
+					} else if (coex_dm->cur_ps_tdma == 6) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 6);
+						coex_dm->ps_tdma_du_adj_type =
+									      6;
+					} else if (coex_dm->cur_ps_tdma == 16) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 15);
+						coex_dm->ps_tdma_du_adj_type =
+									     15;
+					} else if (coex_dm->cur_ps_tdma == 15) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 14);
+						coex_dm->ps_tdma_du_adj_type =
+									     14;
+					} else if (coex_dm->cur_ps_tdma == 14) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 14);
+						coex_dm->ps_tdma_du_adj_type =
+									     14;
+					}
+				}
+			} else {
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_TRACE_FW_DETAIL,
+					  "[BTCoex], TxPause = 0\n");
+				if (coex_dm->cur_ps_tdma == 5) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 2);
+					coex_dm->ps_tdma_du_adj_type = 2;
+				} else if (coex_dm->cur_ps_tdma == 6) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 2);
+					coex_dm->ps_tdma_du_adj_type = 2;
+				} else if (coex_dm->cur_ps_tdma == 7) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 3);
+					coex_dm->ps_tdma_du_adj_type = 3;
+				} else if (coex_dm->cur_ps_tdma == 8) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 4);
+					coex_dm->ps_tdma_du_adj_type = 4;
+				}
+				if (coex_dm->cur_ps_tdma == 13) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 10);
+					coex_dm->ps_tdma_du_adj_type = 10;
+				} else if (coex_dm->cur_ps_tdma == 14) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 10);
+					coex_dm->ps_tdma_du_adj_type = 10;
+				} else if (coex_dm->cur_ps_tdma == 15) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 11);
+					coex_dm->ps_tdma_du_adj_type = 11;
+				} else if (coex_dm->cur_ps_tdma == 16) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 12);
+					coex_dm->ps_tdma_du_adj_type = 12;
+				}
+				if (result == -1) {
+					if (coex_dm->cur_ps_tdma == 1) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 2);
+						coex_dm->ps_tdma_du_adj_type =
+									      2;
+					} else if (coex_dm->cur_ps_tdma == 2) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 3);
+						coex_dm->ps_tdma_du_adj_type =
+									      3;
+					} else if (coex_dm->cur_ps_tdma == 3) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 4);
+						coex_dm->ps_tdma_du_adj_type =
+									      4;
+					} else if (coex_dm->cur_ps_tdma == 9) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 10);
+						coex_dm->ps_tdma_du_adj_type =
+									     10;
+					} else if (coex_dm->cur_ps_tdma == 10) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 11);
+						coex_dm->ps_tdma_du_adj_type =
+									     11;
+					} else if (coex_dm->cur_ps_tdma == 11) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 12);
+						coex_dm->ps_tdma_du_adj_type =
+									     12;
+					}
+				} else if (result == 1) {
+					if (coex_dm->cur_ps_tdma == 4) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 3);
+						coex_dm->ps_tdma_du_adj_type =
+									      3;
+					} else if (coex_dm->cur_ps_tdma == 3) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 2);
+						coex_dm->ps_tdma_du_adj_type =
+									      2;
+					} else if (coex_dm->cur_ps_tdma == 2) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 2);
+						coex_dm->ps_tdma_du_adj_type =
+									      2;
+					} else if (coex_dm->cur_ps_tdma == 12) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 11);
+						coex_dm->ps_tdma_du_adj_type =
+									     11;
+					} else if (coex_dm->cur_ps_tdma == 11) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 10);
+						coex_dm->ps_tdma_du_adj_type =
+									     10;
+					} else if (coex_dm->cur_ps_tdma == 10) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 10);
+						coex_dm->ps_tdma_du_adj_type =
+									     10;
+					}
+				}
+			}
+		} else if (max_interval == 3) {
+			if (tx_pause) {
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_TRACE_FW_DETAIL,
+					  "[BTCoex], TxPause = 1\n");
+				if (coex_dm->cur_ps_tdma == 1) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 7);
+					coex_dm->ps_tdma_du_adj_type = 7;
+				} else if (coex_dm->cur_ps_tdma == 2) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 7);
+					coex_dm->ps_tdma_du_adj_type = 7;
+				} else if (coex_dm->cur_ps_tdma == 3) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 7);
+					coex_dm->ps_tdma_du_adj_type = 7;
+				} else if (coex_dm->cur_ps_tdma == 4) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 8);
+					coex_dm->ps_tdma_du_adj_type = 8;
+				}
+				if (coex_dm->cur_ps_tdma == 9) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 15);
+					coex_dm->ps_tdma_du_adj_type = 15;
+				} else if (coex_dm->cur_ps_tdma == 10) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 15);
+					coex_dm->ps_tdma_du_adj_type = 15;
+				} else if (coex_dm->cur_ps_tdma == 11) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 15);
+					coex_dm->ps_tdma_du_adj_type = 15;
+				} else if (coex_dm->cur_ps_tdma == 12) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 16);
+					coex_dm->ps_tdma_du_adj_type = 16;
+				}
+				if (result == -1) {
+					if (coex_dm->cur_ps_tdma == 5) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 7);
+						coex_dm->ps_tdma_du_adj_type =
+									      7;
+					} else if (coex_dm->cur_ps_tdma == 6) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 7);
+						coex_dm->ps_tdma_du_adj_type =
+									      7;
+					} else if (coex_dm->cur_ps_tdma == 7) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 8);
+						coex_dm->ps_tdma_du_adj_type =
+									      8;
+					} else if (coex_dm->cur_ps_tdma == 13) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 15);
+						coex_dm->ps_tdma_du_adj_type =
+									     15;
+					} else if (coex_dm->cur_ps_tdma == 14) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 15);
+						coex_dm->ps_tdma_du_adj_type =
+									     15;
+					} else if (coex_dm->cur_ps_tdma == 15) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 16);
+						coex_dm->ps_tdma_du_adj_type =
+									     16;
+					}
+				}  else if (result == 1) {
+					if (coex_dm->cur_ps_tdma == 8) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 7);
+						coex_dm->ps_tdma_du_adj_type =
+									      7;
+					} else if (coex_dm->cur_ps_tdma == 7) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 7);
+						coex_dm->ps_tdma_du_adj_type =
+									      7;
+					} else if (coex_dm->cur_ps_tdma == 6) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 7);
+						coex_dm->ps_tdma_du_adj_type =
+									      7;
+					} else if (coex_dm->cur_ps_tdma == 16) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 15);
+						coex_dm->ps_tdma_du_adj_type =
+									     15;
+					} else if (coex_dm->cur_ps_tdma == 15) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 15);
+						coex_dm->ps_tdma_du_adj_type =
+									     15;
+					} else if (coex_dm->cur_ps_tdma == 14) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 15);
+						coex_dm->ps_tdma_du_adj_type =
+									     15;
+					}
+				}
+			} else {
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_TRACE_FW_DETAIL,
+					  "[BTCoex], TxPause = 0\n");
+				if (coex_dm->cur_ps_tdma == 5) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 3);
+					coex_dm->ps_tdma_du_adj_type = 3;
+				} else if (coex_dm->cur_ps_tdma == 6) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 3);
+					coex_dm->ps_tdma_du_adj_type = 3;
+				} else if (coex_dm->cur_ps_tdma == 7) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 3);
+					coex_dm->ps_tdma_du_adj_type = 3;
+				} else if (coex_dm->cur_ps_tdma == 8) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 4);
+					coex_dm->ps_tdma_du_adj_type = 4;
+				}
+				if (coex_dm->cur_ps_tdma == 13) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 11);
+					coex_dm->ps_tdma_du_adj_type = 11;
+				} else if (coex_dm->cur_ps_tdma == 14) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 11);
+					coex_dm->ps_tdma_du_adj_type = 11;
+				} else if (coex_dm->cur_ps_tdma == 15) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 11);
+					coex_dm->ps_tdma_du_adj_type = 11;
+				} else if (coex_dm->cur_ps_tdma == 16) {
+					halbtc8723b2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 12);
+					coex_dm->ps_tdma_du_adj_type = 12;
+				}
+				if (result == -1) {
+					if (coex_dm->cur_ps_tdma == 1) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 3);
+						coex_dm->ps_tdma_du_adj_type =
+									      3;
+					} else if (coex_dm->cur_ps_tdma == 2) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 3);
+						coex_dm->ps_tdma_du_adj_type =
+									      3;
+					} else if (coex_dm->cur_ps_tdma == 3) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 4);
+						coex_dm->ps_tdma_du_adj_type =
+									      4;
+					} else if (coex_dm->cur_ps_tdma == 9) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 11);
+						coex_dm->ps_tdma_du_adj_type =
+									     11;
+					} else if (coex_dm->cur_ps_tdma == 10) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 11);
+						coex_dm->ps_tdma_du_adj_type =
+									     11;
+					} else if (coex_dm->cur_ps_tdma == 11) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 12);
+						coex_dm->ps_tdma_du_adj_type =
+									     12;
+					}
+				} else if (result == 1) {
+					if (coex_dm->cur_ps_tdma == 4) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 3);
+						coex_dm->ps_tdma_du_adj_type =
+									      3;
+					} else if (coex_dm->cur_ps_tdma == 3) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 3);
+						coex_dm->ps_tdma_du_adj_type =
+									      3;
+					} else if (coex_dm->cur_ps_tdma == 2) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 3);
+						coex_dm->ps_tdma_du_adj_type =
+									      3;
+					} else if (coex_dm->cur_ps_tdma == 12) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 11);
+						coex_dm->ps_tdma_du_adj_type =
+									     11;
+					} else if (coex_dm->cur_ps_tdma == 11) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 11);
+						coex_dm->ps_tdma_du_adj_type =
+									     11;
+					} else if (coex_dm->cur_ps_tdma == 10) {
+						halbtc8723b2ant_ps_tdma(
+								btcoexist,
+								NORMAL_EXEC,
+								true, 11);
+						coex_dm->ps_tdma_du_adj_type =
+									     11;
+					}
+				}
+			}
+		}
+	}
+
+	/*if current PsTdma not match with the recorded one (when scan, dhcp..),
+	 *then we have to adjust it back to the previous record one.*/
+	if (coex_dm->cur_ps_tdma != coex_dm->ps_tdma_du_adj_type) {
+		bool scan = false, link = false, roam = false;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], PsTdma type dismatch!!!, curPsTdma =%d, recordPsTdma =%d\n",
+			  coex_dm->cur_ps_tdma, coex_dm->ps_tdma_du_adj_type);
+
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
+
+		if (!scan && !link && !roam)
+			halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true,
+						coex_dm->ps_tdma_du_adj_type);
+		else
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+				  "[BTCoex], roaming/link/scan is under progress, will adjust next time!!!\n");
+	}
+}
+
+/* SCO only or SCO+PAN(HS) */
+static void halbtc8723b2ant_action_sco(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
+							  0, 2, 15, 0);
+
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 4);
+
+	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	/*for SCO quality at 11b/g mode*/
+	if (BTC_WIFI_BW_LEGACY == wifi_bw)
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+	else  /*for SCO quality & wifi performance balance at 11n mode*/
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 8);
+
+	/*for voice quality */
+	halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 0);
+
+	/* sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      true, 0x4);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      true, 0x4);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      true, 0x4);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      true, 0x4);
+		}
+	}
+}
+
+static void halbtc8723b2ant_action_hid(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
+							  0, 2, 15, 0);
+	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
+
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if (BTC_WIFI_BW_LEGACY == wifi_bw) /*/for HID at 11b/g mode*/
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
+	else  /*for HID quality & wifi performance balance at 11n mode*/
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 9);
+
+	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
+	else
+		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 13);
+
+	/* sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+/*A2DP only / PAN(EDR) only/ A2DP+PAN(HS)*/
+static void halbtc8723b2ant_action_a2dp(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, wifi_rssi_state1, bt_rssi_state;
+	u32 wifi_bw;
+	u8 ap_num = 0;
+
+	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
+							  0, 2, 15, 0);
+	wifi_rssi_state1 = halbtc8723b2ant_wifi_rssi_state(btcoexist,
+							   1, 2, 40, 0);
+	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_AP_NUM, &ap_num);
+
+	/* define the office environment */
+	/* driver don't know AP num in Linux, so we will never enter this if */
+	if (ap_num >= 10 && BTC_RSSI_HIGH(wifi_rssi_state1)) {
+		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff,
+					  0x0);
+		halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
+		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
+
+		/* sw mechanism */
+		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+		if (BTC_WIFI_BW_HT40 == wifi_bw) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      true, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      true, 0x18);
+		}
+		return;
+	}
+
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
+
+	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+		halbtc8723b2ant_tdma_duration_adjust(btcoexist, false, false, 1);
+	else
+		halbtc8723b2ant_tdma_duration_adjust(btcoexist, false, true, 1);
+
+	/* sw mechanism */
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+static void halbtc8723b2ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
+							  0, 2, 15, 0);
+
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
+
+	halbtc8723b2ant_tdma_duration_adjust(btcoexist, false, true, 2);
+
+	/* sw mechanism */
+	btcoexist->btc_get(btcoexist,
+		BTC_GET_U4_WIFI_BW, &wifi_bw);
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+static void halbtc8723b2ant_action_pan_edr(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
+							  0, 2, 15, 0);
+	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
+
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	coex_table_with_type(btcoexist, NORMAL_EXEC, 10);
+
+	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 1);
+	else
+		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
+
+	/* sw mechanism */
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+/*PAN(HS) only*/
+static void halbtc8723b2ant_action_pan_hs(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
+							  0, 2, 15, 0);
+
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+	    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
+
+	halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+/*PAN(EDR)+A2DP*/
+static void halbtc8723b2ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
+							  0, 2, 15, 0);
+	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
+
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 12);
+		if (BTC_WIFI_BW_HT40 == wifi_bw)
+			halbtc8723b2ant_tdma_duration_adjust(btcoexist, false,
+							     true, 3);
+		else
+			halbtc8723b2ant_tdma_duration_adjust(btcoexist, false,
+							     false, 3);
+	} else {
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
+		halbtc8723b2ant_tdma_duration_adjust(btcoexist, false, true, 3);
+	}
+
+	/* sw mechanism	*/
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+static void halbtc8723b2ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
+							  0, 2, 15, 0);
+	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+		if (BTC_WIFI_BW_HT40 == wifi_bw) {
+			halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC,
+							 3);
+			coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 11);
+			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1,
+						  0xfffff, 0x780);
+		} else {
+			halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC,
+							 6);
+			coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 7);
+			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1,
+						  0xfffff, 0x0);
+		}
+		halbtc8723b2ant_tdma_duration_adjust(btcoexist, true, false, 2);
+	} else {
+		halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+		coex_table_with_type(btcoexist, NORMAL_EXEC, 11);
+		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff,
+					  0x0);
+		halbtc8723b2ant_tdma_duration_adjust(btcoexist, true, true, 2);
+	}
+
+	/* sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+/* HID+A2DP+PAN(EDR) */
+static void halbtc8723b2ant_action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
+							  0, 2, 15, 0);
+	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
+
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
+
+	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+		if (BTC_WIFI_BW_HT40 == wifi_bw)
+			halbtc8723b2ant_tdma_duration_adjust(btcoexist, true,
+							     true, 2);
+		else
+			halbtc8723b2ant_tdma_duration_adjust(btcoexist, true,
+							     false, 3);
+	} else {
+		halbtc8723b2ant_tdma_duration_adjust(btcoexist, true, true, 3);
+	}
+
+	/* sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+static void halbtc8723b2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
+							  0, 2, 15, 0);
+	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
+
+	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
+
+	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
+
+	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+		halbtc8723b2ant_tdma_duration_adjust(btcoexist, true, false, 2);
+	else
+		halbtc8723b2ant_tdma_duration_adjust(btcoexist, true, true, 2);
+
+	/* sw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	} else {
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						      false, 0x18);
+		} else {
+			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
+						      false, false);
+			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
+						      false, 0x18);
+		}
+	}
+}
+
+static void halbtc8723b2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
+{
+	u8 algorithm = 0;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+		  "[BTCoex], RunCoexistMechanism() ===>\n");
+
+	if (btcoexist->manual_control) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], RunCoexistMechanism(), return for Manual CTRL <===\n");
+		return;
+	}
+
+	if (coex_sta->under_ips) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], wifi is under IPS !!!\n");
+		return;
+	}
+
+	algorithm = halbtc8723b2ant_action_algorithm(btcoexist);
+	if (coex_sta->c2h_bt_inquiry_page &&
+	    (BT_8723B_2ANT_COEX_ALGO_PANHS != algorithm)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BT is under inquiry/page scan !!\n");
+		halbtc8723b2ant_action_bt_inquiry(btcoexist);
+		return;
+	} else {
+		if (coex_dm->need_recover_0x948) {
+			coex_dm->need_recover_0x948 = false;
+			btcoexist->btc_write_2byte(btcoexist, 0x948,
+						   coex_dm->backup_0x948);
+		}
+	}
+
+	coex_dm->cur_algorithm = algorithm;
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], Algorithm = %d \n",
+		  coex_dm->cur_algorithm);
+
+	if (halbtc8723b2ant_is_common_action(btcoexist)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Action 2-Ant common.\n");
+		coex_dm->auto_tdma_adjust = false;
+	} else {
+		if (coex_dm->cur_algorithm != coex_dm->pre_algorithm) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], preAlgorithm =%d, curAlgorithm =%d\n", coex_dm->pre_algorithm,
+				  coex_dm->cur_algorithm);
+			coex_dm->auto_tdma_adjust = false;
+		}
+		switch (coex_dm->cur_algorithm) {
+		case BT_8723B_2ANT_COEX_ALGO_SCO:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = SCO.\n");
+			halbtc8723b2ant_action_sco(btcoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_HID:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = HID.\n");
+			halbtc8723b2ant_action_hid(btcoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = A2DP.\n");
+			halbtc8723b2ant_action_a2dp(btcoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_A2DP_PANHS:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = A2DP+PAN(HS).\n");
+			halbtc8723b2ant_action_a2dp_pan_hs(btcoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_PANEDR:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = PAN(EDR).\n");
+			halbtc8723b2ant_action_pan_edr(btcoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_PANHS:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = HS mode.\n");
+			halbtc8723b2ant_action_pan_hs(btcoexist);
+				break;
+		case BT_8723B_2ANT_COEX_ALGO_PANEDR_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = PAN+A2DP.\n");
+			halbtc8723b2ant_action_pan_edr_a2dp(btcoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_PANEDR_HID:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = PAN(EDR)+HID.\n");
+			halbtc8723b2ant_action_pan_edr_hid(btcoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_HID_A2DP_PANEDR:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = HID+A2DP+PAN.\n");
+			halbtc8723b2ant_action_hid_a2dp_pan_edr(btcoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_HID_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = HID+A2DP.\n");
+			halbtc8723b2ant_action_hid_a2dp(btcoexist);
+			break;
+		default:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = coexist All Off!!\n");
+			halbtc8723b2ant_coex_alloff(btcoexist);
+			break;
+		}
+		coex_dm->pre_algorithm = coex_dm->cur_algorithm;
+	}
+}
+
+static void halbtc8723b2ant_wifioff_hwcfg(struct btc_coexist *btcoexist)
+{
+	/* set wlan_act to low */
+	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0x4);
+	/* Force GNT_BT to High */
+	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x765, 0x18, 0x3);
+	/* BT select s0/s1 is controlled by BT */
+	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x67, 0x20, 0x0);
+}
+
+/*********************************************************************
+ *  work around function start with wa_halbtc8723b2ant_
+ *********************************************************************/
+/*********************************************************************
+ *  extern function start with EXhalbtc8723b2ant_
+ *********************************************************************/
+void ex92e_halbtc8723b2ant_init_hwconfig(struct btc_coexist *btcoexist)
+{
+	u8 u8tmp = 0;
+
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+		  "[BTCoex], 2Ant Init HW Config!!\n");
+	coex_dm->bt_rf0x1e_backup =
+		btcoexist->btc_get_rf_reg(btcoexist, BTC_RF_A, 0x1e, 0xfffff);
+
+	/* 0x790[5:0] = 0x5 */
+	u8tmp = btcoexist->btc_read_1byte(btcoexist, 0x790);
+	u8tmp &= 0xc0;
+	u8tmp |= 0x5;
+	btcoexist->btc_write_1byte(btcoexist, 0x790, u8tmp);
+
+	/*Antenna config	*/
+	halbtc8723b2ant_set_ant_path(btcoexist, BTC_ANT_WIFI_AT_MAIN,
+				     true, false);
+
+
+
+	/* PTA parameter */
+	coex_table_with_type(btcoexist, FORCE_EXEC, 0);
+
+	/* Enable counter statistics */
+	/*0x76e[3] = 1, WLAN_Act control by PTA*/
+	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
+	btcoexist->btc_write_1byte(btcoexist, 0x778, 0x3);
+	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x40, 0x20, 0x1);
+}
+
+void ex92e_halbtc8723b2ant_init_coex_dm(struct btc_coexist *btcoexist)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+		  "[BTCoex], Coex Mechanism Init!!\n");
+	halbtc8723b2ant_init_coex_dm(btcoexist);
+}
+
+void ex_halbtc8723b2ant92e_display_coex_info(struct btc_coexist *btcoexist)
+{
+	struct btc_board_info *board_info = &btcoexist->board_info;
+	struct btc_stack_info *stack_info = &btcoexist->stack_info;
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	u8 *cli_buf = btcoexist->cli_buf;
+	u8 u8tmp[4], i, bt_info_ext, ps_tdma_case = 0;
+	u32 u32tmp[4];
+	bool roam = false, scan = false;
+	bool link = false, wifi_under_5g = false;
+	bool bt_hs_on = false, wifi_busy = false;
+	s32 wifi_rssi = 0, bt_hs_rssi = 0;
+	u32 wifi_bw, wifi_traffic_dir, fa_ofdm, fa_cck;
+	u8 wifi_dot11_chnl, wifi_hs_chnl;
+	u32 fw_ver = 0, bt_patch_ver = 0;
+	u8 ap_num = 0;
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n ============[BT Coexist info] ============");
+	CL_PRINTF(cli_buf);
+
+	if (btcoexist->manual_control) {
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\r\n ==========[Under Manual Control] ============");
+		CL_PRINTF(cli_buf);
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\r\n ==========================================");
+		CL_PRINTF(cli_buf);
+	}
+
+	if (!board_info->bt_exist) {
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n BT not exists !!!");
+		CL_PRINTF(cli_buf);
+		return;
+	}
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ",
+		   "Ant PG number/ Ant mechanism: ",
+		   board_info->pg_ant_num, board_info->btdm_ant_num);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %d",
+		   "BT stack/ hci ext ver",
+		   ((stack_info->profile_notified) ? "Yes" : "No"),
+		   stack_info->hci_version);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER, &bt_patch_ver);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n %-35s = %d_%x/ 0x%x/ 0x%x(%d)",
+		   "CoexVer/ fw_ver/ PatchVer",
+		   glcoex_ver_date_8723b_2ant, glcoex_ver_8723b_2ant,
+		   fw_ver, bt_patch_ver, bt_patch_ver);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_DOT11_CHNL,
+			   &wifi_dot11_chnl);
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifi_hs_chnl);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d(%d)",
+		   "Dot11 channel / HsChnl(HsMode)",
+		   wifi_dot11_chnl, wifi_hs_chnl, bt_hs_on);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x ",
+		   "H2C Wifi inform bt chnl Info", coex_dm->wifi_chnl_info[0],
+		   coex_dm->wifi_chnl_info[1], coex_dm->wifi_chnl_info[2]);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
+	btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi);
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_AP_NUM, &ap_num);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d",
+		   "Wifi rssi/ HS rssi/ AP#", wifi_rssi, bt_hs_rssi, ap_num);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ",
+		   "Wifi link/ roam/ scan", link, roam, scan);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under_5g);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION,
+			   &wifi_traffic_dir);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %s/ %s ",
+		   "Wifi status", (wifi_under_5g ? "5G" : "2.4G"),
+		   ((BTC_WIFI_BW_LEGACY == wifi_bw) ? "Legacy" :
+		   (((BTC_WIFI_BW_HT40 == wifi_bw) ? "HT40" : "HT20"))),
+		   ((!wifi_busy) ? "idle" :
+		   ((BTC_WIFI_TRAFFIC_TX == wifi_traffic_dir) ?
+		   "uplink" : "downlink")));
+	CL_PRINTF(cli_buf);
+
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d / %d / %d",
+		   "SCO/HID/PAN/A2DP",
+		   bt_link_info->sco_exist, bt_link_info->hid_exist,
+		   bt_link_info->pan_exist, bt_link_info->a2dp_exist);
+	CL_PRINTF(cli_buf);
+	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_BT_LINK_INFO);
+
+	bt_info_ext = coex_sta->bt_info_ext;
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s",
+		   "BT Info A2DP rate",
+		   (bt_info_ext & BIT(0)) ? "Basic rate" : "EDR rate");
+	CL_PRINTF(cli_buf);
+
+	for (i = 0; i < BT_INFO_SRC_8723B_2ANT_MAX; i++) {
+		if (coex_sta->bt_info_c2h_cnt[i]) {
+			CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+				   "\r\n %-35s = %02x %02x %02x %02x %02x %02x %02x(%d)",
+				   glbt_info_src_8723b_2ant[i],
+				   coex_sta->bt_info_c2h[i][0],
+				   coex_sta->bt_info_c2h[i][1],
+				   coex_sta->bt_info_c2h[i][2],
+				   coex_sta->bt_info_c2h[i][3],
+				   coex_sta->bt_info_c2h[i][4],
+				   coex_sta->bt_info_c2h[i][5],
+				   coex_sta->bt_info_c2h[i][6],
+				   coex_sta->bt_info_c2h_cnt[i]);
+			CL_PRINTF(cli_buf);
+		}
+	}
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s/%s",
+		   "PS state, IPS/LPS",
+		   ((coex_sta->under_ips ? "IPS ON" : "IPS OFF")),
+		   ((coex_sta->under_lps ? "LPS ON" : "LPS OFF")));
+	CL_PRINTF(cli_buf);
+	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_FW_PWR_MODE_CMD);
+
+	/* Sw mechanism	*/
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n %-35s", "============[Sw mechanism] ============");
+	CL_PRINTF(cli_buf);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ",
+		   "SM1[ShRf/ LpRA/ LimDig]", coex_dm->cur_rf_rx_lpf_shrink,
+		   coex_dm->cur_low_penalty_ra, coex_dm->limited_dig);
+	CL_PRINTF(cli_buf);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d(0x%x) ",
+		   "SM2[AgcT/ AdcB/ SwDacSwing(lvl)]",
+		   coex_dm->cur_agc_table_en, coex_dm->cur_adc_back_off,
+		   coex_dm->cur_dac_swing_on, coex_dm->cur_dac_swing_lvl);
+	CL_PRINTF(cli_buf);
+
+	/* Fw mechanism	*/
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
+		   "============[Fw mechanism] ============");
+	CL_PRINTF(cli_buf);
+
+	ps_tdma_case = coex_dm->cur_ps_tdma;
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n %-35s = %02x %02x %02x %02x %02x case-%d (auto:%d)",
+		   "PS TDMA", coex_dm->ps_tdma_para[0],
+		   coex_dm->ps_tdma_para[1], coex_dm->ps_tdma_para[2],
+		   coex_dm->ps_tdma_para[3], coex_dm->ps_tdma_para[4],
+		   ps_tdma_case, coex_dm->auto_tdma_adjust);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ",
+		   "DecBtPwr/ IgnWlanAct", coex_dm->cur_dec_bt_pwr,
+		   coex_dm->cur_ignore_wlan_act);
+	CL_PRINTF(cli_buf);
+
+	/* Hw setting */
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
+		   "============[Hw setting] ============");
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x",
+		   "RF-A, 0x1e initVal", coex_dm->bt_rf0x1e_backup);
+	CL_PRINTF(cli_buf);
+
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x778);
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x880);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
+		   "0x778/0x880[29:25]", u8tmp[0],
+		   (u32tmp[0]&0x3e000000) >> 25);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x948);
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x67);
+	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x765);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
+		   "0x948/ 0x67[5] / 0x765",
+		   u32tmp[0], ((u8tmp[0]&0x20) >> 5), u8tmp[1]);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x92c);
+	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x930);
+	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x944);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
+		   "0x92c[1:0]/ 0x930[7:0]/0x944[1:0]",
+		   u32tmp[0]&0x3, u32tmp[1]&0xff, u32tmp[2]&0x3);
+	CL_PRINTF(cli_buf);
+
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x39);
+	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x40);
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x4c);
+	u8tmp[2] = btcoexist->btc_read_1byte(btcoexist, 0x64);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x",
+		   "0x38[11]/0x40/0x4c[24:23]/0x64[0]",
+		   ((u8tmp[0] & 0x8)>>3), u8tmp[1],
+		   ((u32tmp[0]&0x01800000)>>23), u8tmp[2]&0x1);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x550);
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x522);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
+		   "0x550(bcn ctrl)/0x522", u32tmp[0], u8tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xc50);
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x49c);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
+		   "0xc50(dig)/0x49c(null-drop)", u32tmp[0]&0xff, u8tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xda0);
+	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0xda4);
+	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0xda8);
+	u32tmp[3] = btcoexist->btc_read_4byte(btcoexist, 0xcf0);
+
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0xa5b);
+	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0xa5c);
+
+	fa_ofdm = ((u32tmp[0]&0xffff0000) >> 16) +
+		  ((u32tmp[1]&0xffff0000) >> 16) +
+		   (u32tmp[1] & 0xffff) +
+		   (u32tmp[2] & 0xffff) +
+		  ((u32tmp[3]&0xffff0000) >> 16) +
+		   (u32tmp[3] & 0xffff);
+	fa_cck = (u8tmp[0] << 8) + u8tmp[1];
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
+		   "OFDM-CCA/OFDM-FA/CCK-FA",
+		   u32tmp[0]&0xffff, fa_ofdm, fa_cck);
+	CL_PRINTF(cli_buf);
+
+	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x6c0);
+	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x6c4);
+	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x6c8);
+	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x6cc);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x",
+		   "0x6c0/0x6c4/0x6c8/0x6cc(coexTable)", \
+		   u32tmp[0], u32tmp[1], u32tmp[2], u8tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
+		   "0x770(high-pri rx/tx)",
+		   coex_sta->high_priority_rx, coex_sta->high_priority_tx);
+	CL_PRINTF(cli_buf);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
+		   "0x774(low-pri rx/tx)", coex_sta->low_priority_rx,
+		   coex_sta->low_priority_tx);
+	CL_PRINTF(cli_buf);
+#if (BT_AUTO_REPORT_ONLY_8723B_2ANT == 1)
+	halbtc8723b2ant_monitor_bt_ctr(btcoexist);
+#endif
+	btcoexist->btc_disp_dbg_msg(btcoexist,
+	BTC_DBG_DISP_COEX_STATISTICS);
+}
+
+void ex92e_halbtc8723b2ant_ips_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	if (BTC_IPS_ENTER == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], IPS ENTER notify\n");
+		coex_sta->under_ips = true;
+		halbtc8723b2ant_wifioff_hwcfg(btcoexist);
+		halbtc8723b2ant_ignore_wlan_act(btcoexist, FORCE_EXEC, true);
+		halbtc8723b2ant_coex_alloff(btcoexist);
+	} else if (BTC_IPS_LEAVE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], IPS LEAVE notify\n");
+		coex_sta->under_ips = false;
+		ex92e_halbtc8723b2ant_init_hwconfig(btcoexist);
+		halbtc8723b2ant_init_coex_dm(btcoexist);
+		halbtc8723b2ant_query_bt_info(btcoexist);
+	}
+}
+
+void ex92e_halbtc8723b2ant_lps_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	if (BTC_LPS_ENABLE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], LPS ENABLE notify\n");
+		coex_sta->under_lps = true;
+	} else if (BTC_LPS_DISABLE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], LPS DISABLE notify\n");
+		coex_sta->under_lps = false;
+	}
+}
+
+void ex92e_halbtc8723b2ant_scan_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	if (BTC_SCAN_START == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], SCAN START notify\n");
+	else if (BTC_SCAN_FINISH == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], SCAN FINISH notify\n");
+}
+
+void ex92e_halbtc8723b2ant_connect_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	if (BTC_ASSOCIATE_START == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], CONNECT START notify\n");
+	else if (BTC_ASSOCIATE_FINISH == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], CONNECT FINISH notify\n");
+}
+
+void ex92e_halbtc8723b2ant_media_status_notify(struct btc_coexist *btcoexist,
+					    u8 type)
+{
+	u8 h2c_parameter[3] = {0};
+	u32 wifi_bw;
+	u8 wifi_central_chnl;
+
+	if (BTC_MEDIA_CONNECT == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], MEDIA connect notify\n");
+	else
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], MEDIA disconnect notify\n");
+
+	/* only 2.4G we need to inform bt the chnl mask */
+	btcoexist->btc_get(btcoexist,
+		BTC_GET_U1_WIFI_CENTRAL_CHNL, &wifi_central_chnl);
+	if ((BTC_MEDIA_CONNECT == type) &&
+	    (wifi_central_chnl <= 14)) {
+		h2c_parameter[0] = 0x1;
+		h2c_parameter[1] = wifi_central_chnl;
+		btcoexist->btc_get(btcoexist,
+			BTC_GET_U4_WIFI_BW, &wifi_bw);
+		if (BTC_WIFI_BW_HT40 == wifi_bw)
+			h2c_parameter[2] = 0x30;
+		else
+			h2c_parameter[2] = 0x20;
+	}
+
+	coex_dm->wifi_chnl_info[0] = h2c_parameter[0];
+	coex_dm->wifi_chnl_info[1] = h2c_parameter[1];
+	coex_dm->wifi_chnl_info[2] = h2c_parameter[2];
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], FW write 0x66 = 0x%x\n",
+		  h2c_parameter[0] << 16 | h2c_parameter[1] << 8 |
+		  h2c_parameter[2]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x66, 3, h2c_parameter);
+}
+
+void ex92e_halbtc8723b2ant_special_packet_notify(struct btc_coexist *btcoexist,
+					      u8 type)
+{
+	if (type == BTC_PACKET_DHCP)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], DHCP Packet notify\n");
+}
+
+void ex92e_halbtc8723b2ant_bt_info_notify(struct btc_coexist *btcoexist,
+				       u8 *tmpbuf, u8 length)
+{
+	u8 btInfo = 0;
+	u8 i, rsp_source = 0;
+	bool bt_busy = false, limited_dig = false;
+	bool wifi_connected = false;
+
+	coex_sta->c2h_bt_info_req_sent = false;
+
+	rsp_source = tmpbuf[0]&0xf;
+	if (rsp_source >= BT_INFO_SRC_8723B_2ANT_MAX)
+		rsp_source = BT_INFO_SRC_8723B_2ANT_WIFI_FW;
+	coex_sta->bt_info_c2h_cnt[rsp_source]++;
+
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+		  "[BTCoex], Bt info[%d], length =%d, hex data =[",
+		  rsp_source, length);
+	for (i = 0; i < length; i++) {
+		coex_sta->bt_info_c2h[rsp_source][i] = tmpbuf[i];
+		if (i == 1)
+			btInfo = tmpbuf[i];
+		if (i == length-1)
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+				  "0x%02x]\n", tmpbuf[i]);
+		else
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+				  "0x%02x, ", tmpbuf[i]);
+	}
+
+	if (btcoexist->manual_control) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), return for Manual CTRL<===\n");
+		return;
+	}
+
+	if (BT_INFO_SRC_8723B_2ANT_WIFI_FW != rsp_source) {
+		coex_sta->bt_retry_cnt =	/* [3:0]*/
+			coex_sta->bt_info_c2h[rsp_source][2] & 0xf;
+
+		coex_sta->bt_rssi =
+			coex_sta->bt_info_c2h[rsp_source][3] * 2 + 10;
+
+		coex_sta->bt_info_ext =
+			coex_sta->bt_info_c2h[rsp_source][4];
+
+		/* Here we need to resend some wifi info to BT
+		     because bt is reset and loss of the info.*/
+		if ((coex_sta->bt_info_ext & BIT(1))) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], BT ext info bit1 check, send wifi BW&Chnl to BT!!\n");
+			btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+					   &wifi_connected);
+			if (wifi_connected)
+				ex92e_halbtc8723b2ant_media_status_notify(
+							btcoexist,
+							BTC_MEDIA_CONNECT);
+			else
+				ex92e_halbtc8723b2ant_media_status_notify(
+							btcoexist,
+							BTC_MEDIA_DISCONNECT);
+		}
+
+		if ((coex_sta->bt_info_ext & BIT(3))) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], BT ext info bit3 check, set BT NOT to ignore Wlan active!!\n");
+			halbtc8723b2ant_ignore_wlan_act(btcoexist, FORCE_EXEC,
+							false);
+		} else {
+			/* BT already NOT ignore Wlan active, do nothing here.*/
+		}
+#if (BT_AUTO_REPORT_ONLY_8723B_2ANT == 0)
+		if ((coex_sta->bt_info_ext & BIT(4))) {
+			/* BT auto report already enabled, do nothing*/
+		} else {
+			halbtc8723b2ant_bt_auto_report(btcoexist, FORCE_EXEC,
+						       true);
+		}
+#endif
+	}
+
+	/* check BIT(2) first ==> check if bt is under inquiry or page scan*/
+	if (btInfo & BT_INFO_8723B_2ANT_B_INQ_PAGE)
+		coex_sta->c2h_bt_inquiry_page = true;
+	else
+		coex_sta->c2h_bt_inquiry_page = false;
+
+	/* set link exist status*/
+	if (!(btInfo & BT_INFO_8723B_2ANT_B_CONNECTION)) {
+		coex_sta->bt_link_exist = false;
+		coex_sta->pan_exist = false;
+		coex_sta->a2dp_exist = false;
+		coex_sta->hid_exist = false;
+		coex_sta->sco_exist = false;
+	} else {/*  connection exists */
+		coex_sta->bt_link_exist = true;
+		if (btInfo & BT_INFO_8723B_2ANT_B_FTP)
+			coex_sta->pan_exist = true;
+		else
+			coex_sta->pan_exist = false;
+		if (btInfo & BT_INFO_8723B_2ANT_B_A2DP)
+			coex_sta->a2dp_exist = true;
+		else
+			coex_sta->a2dp_exist = false;
+		if (btInfo & BT_INFO_8723B_2ANT_B_HID)
+			coex_sta->hid_exist = true;
+		else
+			coex_sta->hid_exist = false;
+		if (btInfo & BT_INFO_8723B_2ANT_B_SCO_ESCO)
+			coex_sta->sco_exist = true;
+		else
+			coex_sta->sco_exist = false;
+	}
+
+	halbtc8723b2ant_update_bt_link_info(btcoexist);
+
+	if (!(btInfo & BT_INFO_8723B_2ANT_B_CONNECTION)) {
+		coex_dm->bt_status = BT_8723B_2ANT_BT_STATUS_NON_CONNECTED_IDLE;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), BT Non-Connected idle!!!\n");
+	/* connection exists but no busy */
+	} else if (btInfo == BT_INFO_8723B_2ANT_B_CONNECTION) {
+		coex_dm->bt_status = BT_8723B_2ANT_BT_STATUS_CONNECTED_IDLE;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), BT Connected-idle!!!\n");
+	} else if ((btInfo & BT_INFO_8723B_2ANT_B_SCO_ESCO) ||
+		   (btInfo & BT_INFO_8723B_2ANT_B_SCO_BUSY)) {
+		coex_dm->bt_status =
+			BT_8723B_2ANT_BT_STATUS_SCO_BUSY;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), BT SCO busy!!!\n");
+	} else if (btInfo&BT_INFO_8723B_2ANT_B_ACL_BUSY) {
+		coex_dm->bt_status =
+			BT_8723B_2ANT_BT_STATUS_ACL_BUSY;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), BT ACL busy!!!\n");
+	} else {
+		coex_dm->bt_status = BT_8723B_2ANT_BT_STATUS_MAX;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), BT Non-Defined state!!!\n");
+	}
+
+	if ((BT_8723B_2ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) ||
+	    (BT_8723B_2ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
+	    (BT_8723B_2ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status)) {
+		bt_busy = true;
+		limited_dig = true;
+	} else {
+		bt_busy = false;
+		limited_dig = false;
+	}
+
+	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bt_busy);
+
+	coex_dm->limited_dig = limited_dig;
+	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_LIMITED_DIG, &limited_dig);
+
+	halbtc8723b2ant_run_coexist_mechanism(btcoexist);
+}
+
+void ex92e_halbtc8723b2ant_halt_notify(struct btc_coexist *btcoexist)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, "[BTCoex], Halt notify\n");
+
+	halbtc8723b2ant_wifioff_hwcfg(btcoexist);
+	halbtc8723b2ant_ignore_wlan_act(btcoexist, FORCE_EXEC, true);
+	ex92e_halbtc8723b2ant_media_status_notify(btcoexist, BTC_MEDIA_DISCONNECT);
+}
+
+void ex92e_halbtc8723b2ant_periodical(struct btc_coexist *btcoexist)
+{
+	struct btc_board_info *board_info = &btcoexist->board_info;
+	struct btc_stack_info *stack_info = &btcoexist->stack_info;
+	static u8 dis_ver_info_cnt;
+	u32 fw_ver = 0, bt_patch_ver = 0;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+		  "[BTCoex], ========================== Periodical ===========================\n");
+
+	if (dis_ver_info_cnt <= 5) {
+		dis_ver_info_cnt += 1;
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], ****************************************************************\n");
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], Ant PG Num/ Ant Mech/ Ant Pos = %d/%d/%d\n", board_info->pg_ant_num,
+			  board_info->btdm_ant_num, board_info->btdm_ant_pos);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], BT stack/hci ext ver = %s/%d\n",
+			  ((stack_info->profile_notified) ? "Yes" : "No"),
+			  stack_info->hci_version);
+		btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER,
+				   &bt_patch_ver);
+		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], CoexVer/fw_ver/PatchVer = %d_%x/0x%x/0x%x(%d)\n",
+			  glcoex_ver_date_8723b_2ant, glcoex_ver_8723b_2ant,
+			  fw_ver, bt_patch_ver, bt_patch_ver);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], ****************************************************************\n");
+	}
+
+#if (BT_AUTO_REPORT_ONLY_8723B_2ANT == 0)
+	halbtc8723b2ant_query_bt_info(btcoexist);
+	halbtc8723b2ant_monitor_bt_ctr(btcoexist);
+	halbtc8723b2ant_monitor_bt_enable_disable(btcoexist);
+#else
+	if (is_wifi_status_changed(btcoexist) ||
+	    coex_dm->auto_tdma_adjust)
+		halbtc8723b2ant_run_coexist_mechanism(btcoexist);
+#endif
+}
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbtc8723b2ant.h b/drivers/staging/rtl8192ee/btcoexist/halbtc8723b2ant.h
new file mode 100644
index 00000000000..8437e1c3ecd
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/halbtc8723b2ant.h
@@ -0,0 +1,145 @@
+/************************************************************************
+ * The following is for 8723B 2Ant BT Co-exist definition
+ ************************************************************************/
+#define	BT_AUTO_REPORT_ONLY_8723B_2ANT			1
+
+
+#define	BT_INFO_8723B_2ANT_B_FTP			BIT(7)
+#define	BT_INFO_8723B_2ANT_B_A2DP			BIT(6)
+#define	BT_INFO_8723B_2ANT_B_HID			BIT(5)
+#define	BT_INFO_8723B_2ANT_B_SCO_BUSY			BIT(4)
+#define	BT_INFO_8723B_2ANT_B_ACL_BUSY			BIT(3)
+#define	BT_INFO_8723B_2ANT_B_INQ_PAGE			BIT(2)
+#define	BT_INFO_8723B_2ANT_B_SCO_ESCO			BIT(1)
+#define	BT_INFO_8723B_2ANT_B_CONNECTION			BIT(0)
+
+#define BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT		2
+
+enum BT_INFO_SRC_8723B_2ANT {
+	BT_INFO_SRC_8723B_2ANT_WIFI_FW			= 0x0,
+	BT_INFO_SRC_8723B_2ANT_BT_RSP			= 0x1,
+	BT_INFO_SRC_8723B_2ANT_BT_ACTIVE_SEND		= 0x2,
+	BT_INFO_SRC_8723B_2ANT_MAX
+};
+
+enum BT_8723B_2ANT_BT_STATUS {
+	BT_8723B_2ANT_BT_STATUS_NON_CONNECTED_IDLE	= 0x0,
+	BT_8723B_2ANT_BT_STATUS_CONNECTED_IDLE		= 0x1,
+	BT_8723B_2ANT_BT_STATUS_INQ_PAGE		= 0x2,
+	BT_8723B_2ANT_BT_STATUS_ACL_BUSY		= 0x3,
+	BT_8723B_2ANT_BT_STATUS_SCO_BUSY		= 0x4,
+	BT_8723B_2ANT_BT_STATUS_ACL_SCO_BUSY		= 0x5,
+	BT_8723B_2ANT_BT_STATUS_MAX
+};
+
+enum BT_8723B_2ANT_COEX_ALGO {
+	BT_8723B_2ANT_COEX_ALGO_UNDEFINED		= 0x0,
+	BT_8723B_2ANT_COEX_ALGO_SCO			= 0x1,
+	BT_8723B_2ANT_COEX_ALGO_HID			= 0x2,
+	BT_8723B_2ANT_COEX_ALGO_A2DP			= 0x3,
+	BT_8723B_2ANT_COEX_ALGO_A2DP_PANHS		= 0x4,
+	BT_8723B_2ANT_COEX_ALGO_PANEDR			= 0x5,
+	BT_8723B_2ANT_COEX_ALGO_PANHS			= 0x6,
+	BT_8723B_2ANT_COEX_ALGO_PANEDR_A2DP		= 0x7,
+	BT_8723B_2ANT_COEX_ALGO_PANEDR_HID		= 0x8,
+	BT_8723B_2ANT_COEX_ALGO_HID_A2DP_PANEDR		= 0x9,
+	BT_8723B_2ANT_COEX_ALGO_HID_A2DP		= 0xa,
+	BT_8723B_2ANT_COEX_ALGO_MAX			= 0xb,
+};
+
+struct coex_dm_8723b_2ant {
+	/* fw mechanism */
+	bool pre_dec_bt_pwr;
+	bool cur_dec_bt_pwr;
+	u8 pre_fw_dac_swing_lvl;
+	u8 cur_fw_dac_swing_lvl;
+	bool cur_ignore_wlan_act;
+	bool pre_ignore_wlan_act;
+	u8 pre_ps_tdma;
+	u8 cur_ps_tdma;
+	u8 ps_tdma_para[5];
+	u8 ps_tdma_du_adj_type;
+	bool reset_tdma_adjust;
+	bool auto_tdma_adjust;
+	bool pre_ps_tdma_on;
+	bool cur_ps_tdma_on;
+	bool pre_bt_auto_report;
+	bool cur_bt_auto_report;
+
+	/* sw mechanism */
+	bool pre_rf_rx_lpf_shrink;
+	bool cur_rf_rx_lpf_shrink;
+	u32 bt_rf0x1e_backup;
+	bool pre_low_penalty_ra;
+	bool cur_low_penalty_ra;
+	bool pre_dac_swing_on;
+	u32 pre_dac_swing_lvl;
+	bool cur_dac_swing_on;
+	u32 cur_dac_swing_lvl;
+	bool pre_adc_back_off;
+	bool cur_adc_back_off;
+	bool pre_agc_table_en;
+	bool cur_agc_table_en;
+	u32 pre_val0x6c0;
+	u32 cur_val0x6c0;
+	u32 pre_val0x6c4;
+	u32 cur_val0x6c4;
+	u32 pre_val0x6c8;
+	u32 cur_val0x6c8;
+	u8 pre_val0x6cc;
+	u8 cur_val0x6cc;
+	bool limited_dig;
+
+	/* algorithm related */
+	u8 pre_algorithm;
+	u8 cur_algorithm;
+	u8 bt_status;
+	u8 wifi_chnl_info[3];
+
+	bool need_recover_0x948;
+	u16 backup_0x948;
+};
+
+struct coex_sta_8723b_2ant {
+	bool bt_link_exist;
+	bool sco_exist;
+	bool a2dp_exist;
+	bool hid_exist;
+	bool pan_exist;
+
+	bool under_lps;
+	bool under_ips;
+	u32 high_priority_tx;
+	u32 high_priority_rx;
+	u32 low_priority_tx;
+	u32 low_priority_rx;
+	u8 bt_rssi;
+	u8 pre_bt_rssi_state;
+	u8 pre_wifi_rssi_state[4];
+	bool c2h_bt_info_req_sent;
+	u8 bt_info_c2h[BT_INFO_SRC_8723B_2ANT_MAX][10];
+	u32 bt_info_c2h_cnt[BT_INFO_SRC_8723B_2ANT_MAX];
+	bool c2h_bt_inquiry_page;
+	u8 bt_retry_cnt;
+	u8 bt_info_ext;
+};
+
+/*********************************************************************
+ * The following is interface which will notify coex module.
+ *********************************************************************/
+void ex92e_halbtc8723b2ant_init_hwconfig(struct btc_coexist *btcoexist);
+void ex92e_halbtc8723b2ant_init_coex_dm(struct btc_coexist *btcoexist);
+void ex92e_halbtc8723b2ant_ips_notify(struct btc_coexist *btcoexist, u8 type);
+void ex92e_halbtc8723b2ant_lps_notify(struct btc_coexist *btcoexist, u8 type);
+void ex92e_halbtc8723b2ant_scan_notify(struct btc_coexist *btcoexist, u8 type);
+void ex92e_halbtc8723b2ant_connect_notify(struct btc_coexist *btcoexist,
+					  u8 type);
+void ex92e_halbtc8723b2ant_media_status_notify(struct btc_coexist *btcoexist,
+					       u8 type);
+void ex92e_halbtc8723b2ant_special_packet_notify(struct btc_coexist *btcoexist,
+						 u8 type);
+void ex92e_halbtc8723b2ant_bt_info_notify(struct btc_coexist *btcoexist,
+					  u8 *tmpbuf, u8 length);
+void ex92e_halbtc8723b2ant_halt_notify(struct btc_coexist *btcoexist);
+void ex92e_halbtc8723b2ant_periodical(struct btc_coexist *btcoexist);
+void ex_halbtc8723b2ant92e_display_coex_info(struct btc_coexist *btcoexist);
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbtc8821a1ant.c b/drivers/staging/rtl8192ee/btcoexist/halbtc8821a1ant.c
new file mode 100644
index 00000000000..fb5286387d8
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/halbtc8821a1ant.c
@@ -0,0 +1,2780 @@
+/*  */
+/*  Description: */
+/*  */
+/*  This file is for RTL8821A Co-exist mechanism */
+/*  */
+/*  History */
+/*  2012/11/15 Cosa first check in. */
+/*  */
+/*  */
+
+/*  */
+/*  include files */
+/*  */
+#include "halbt_precomp.h"
+/*  */
+/*  Global variables, these are static variables */
+/*  */
+static struct coex_dm_8821a_1ant glcoex_dm_8821a_1ant;
+static struct coex_dm_8821a_1ant *coex_dm = &glcoex_dm_8821a_1ant;
+static struct coex_sta_8821a_1ant glcoex_sta_8821a_1ant;
+static struct coex_sta_8821a_1ant *coex_sta = &glcoex_sta_8821a_1ant;
+
+static const char *const glbt_info_src_8821a_1ant[] = {
+	"BT Info[wifi fw]",
+	"BT Info[bt rsp]",
+	"BT Info[bt auto report]",
+};
+
+static u32 glcoex_ver_date_8821a_1ant = 20130816;
+static u32 glcoex_ver_8821a_1ant = 0x41;
+
+/*  local function proto type if needed */
+/*  local function start with halbtc8821a1ant_ */
+static u8 halbtc8821a1ant_bt_rssi_state(u8 level_num, u8 rssi_thresh,
+					u8 rssi_thresh1)
+{
+	long			bt_rssi = 0;
+	u8 bt_rssi_state = coex_sta->pre_bt_rssi_state;
+
+	bt_rssi = coex_sta->bt_rssi;
+
+	if (level_num == 2) {
+		if ((coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_LOW) ||
+		    (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+			if (bt_rssi >= (rssi_thresh+BTC_RSSI_COEX_THRESH_TOL_8821A_1ANT)) {
+				bt_rssi_state = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state switch to High\n");
+			} else {
+				bt_rssi_state = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state stay at Low\n");
+			}
+		} else {
+			if (bt_rssi < rssi_thresh) {
+				bt_rssi_state = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state switch to Low\n");
+			} else {
+				bt_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state stay at High\n");
+			}
+		}
+	} else if (level_num == 3) {
+		if (rssi_thresh > rssi_thresh1) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+				  "[BTCoex], BT Rssi thresh error!!\n");
+			return coex_sta->pre_bt_rssi_state;
+		}
+
+		if ((coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_LOW) ||
+		    (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+			if (bt_rssi >= (rssi_thresh+BTC_RSSI_COEX_THRESH_TOL_8821A_1ANT)) {
+				bt_rssi_state = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state switch to Medium\n");
+			} else {
+				bt_rssi_state = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state stay at Low\n");
+			}
+		} else if ((coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_MEDIUM) ||
+			   (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
+			if (bt_rssi >= (rssi_thresh1+BTC_RSSI_COEX_THRESH_TOL_8821A_1ANT)) {
+				bt_rssi_state = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state switch to High\n");
+			} else if (bt_rssi < rssi_thresh) {
+				bt_rssi_state = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state switch to Low\n");
+			} else {
+				bt_rssi_state = BTC_RSSI_STATE_STAY_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state stay at Medium\n");
+			}
+		} else {
+			if (bt_rssi < rssi_thresh1) {
+				bt_rssi_state = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state switch to Medium\n");
+			} else {
+				bt_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state stay at High\n");
+			}
+		}
+	}
+
+	coex_sta->pre_bt_rssi_state = bt_rssi_state;
+
+	return bt_rssi_state;
+}
+
+static u8 Wifi_rssi_state(struct btc_coexist *btcoexist, u8 index,
+			  u8 level_num, u8 rssi_thresh,
+			  u8 rssi_thresh1)
+{
+	long	wifi_rssi = 0;
+	u8 wifi_rssi_state = coex_sta->pre_wifi_rssi_state[index];
+
+	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
+
+	if (level_num == 2) {
+		if ((coex_sta->pre_wifi_rssi_state[index] == BTC_RSSI_STATE_LOW) ||
+		    (coex_sta->pre_wifi_rssi_state[index] == BTC_RSSI_STATE_STAY_LOW)) {
+			if (wifi_rssi >= (rssi_thresh+BTC_RSSI_COEX_THRESH_TOL_8821A_1ANT)) {
+				wifi_rssi_state = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state switch to High\n");
+			} else {
+				wifi_rssi_state = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state stay at Low\n");
+			}
+		} else {
+			if (wifi_rssi < rssi_thresh) {
+				wifi_rssi_state = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state switch to Low\n");
+			} else {
+				wifi_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state stay at High\n");
+			}
+		}
+	} else if (level_num == 3) {
+		if (rssi_thresh > rssi_thresh1) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE,
+				  "[BTCoex], wifi RSSI thresh error!!\n");
+			return coex_sta->pre_wifi_rssi_state[index];
+		}
+
+		if ((coex_sta->pre_wifi_rssi_state[index] ==
+		     BTC_RSSI_STATE_LOW) ||
+		    (coex_sta->pre_wifi_rssi_state[index] ==
+		     BTC_RSSI_STATE_STAY_LOW)) {
+			if (wifi_rssi >= (rssi_thresh +
+			    BTC_RSSI_COEX_THRESH_TOL_8821A_1ANT)) {
+				wifi_rssi_state = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state switch to Medium\n");
+			} else {
+				wifi_rssi_state = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state stay at Low\n");
+			}
+		} else if ((coex_sta->pre_wifi_rssi_state[index] ==
+			    BTC_RSSI_STATE_MEDIUM) ||
+			   (coex_sta->pre_wifi_rssi_state[index] ==
+			    BTC_RSSI_STATE_STAY_MEDIUM)) {
+			if (wifi_rssi >= (rssi_thresh1 +
+			    BTC_RSSI_COEX_THRESH_TOL_8821A_1ANT)) {
+				wifi_rssi_state = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state switch to High\n");
+			} else if (wifi_rssi < rssi_thresh) {
+				wifi_rssi_state = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state switch to Low\n");
+			} else {
+				wifi_rssi_state = BTC_RSSI_STATE_STAY_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state stay at Medium\n");
+			}
+		} else {
+			if (wifi_rssi < rssi_thresh1) {
+				wifi_rssi_state = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state switch to Medium\n");
+			} else {
+				wifi_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state stay at High\n");
+			}
+		}
+	}
+
+	coex_sta->pre_wifi_rssi_state[index] = wifi_rssi_state;
+
+	return wifi_rssi_state;
+}
+
+static void update_ra_mask(struct btc_coexist *btcoexist,
+			   bool force_exec, u32 dis_rate_mask)
+{
+	coex_dm->cur_ra_mask = dis_rate_mask;
+
+	if (force_exec || (coex_dm->pre_ra_mask != coex_dm->cur_ra_mask)) {
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_UPDATE_ra_mask,
+				   &coex_dm->cur_ra_mask);
+	}
+	coex_dm->pre_ra_mask = coex_dm->cur_ra_mask;
+}
+
+static void auto_rate_fallback_retry(struct btc_coexist *btcoexist,
+				     bool force_exec, u8 type)
+{
+	bool wifi_under_b_mode = false;
+
+	coex_dm->cur_arfr_type = type;
+
+	if (force_exec || (coex_dm->pre_arfr_type != coex_dm->cur_arfr_type)) {
+		switch (coex_dm->cur_arfr_type) {
+		case 0:	/*  normal mode */
+			btcoexist->btc_write_4byte(btcoexist, 0x430,
+						   coex_dm->backup_arfr_cnt1);
+			btcoexist->btc_write_4byte(btcoexist, 0x434,
+						   coex_dm->backup_arfr_cnt2);
+			break;
+		case 1:
+			btcoexist->btc_get(btcoexist,
+					   BTC_GET_BL_WIFI_UNDER_B_MODE,
+					   &wifi_under_b_mode);
+			if (wifi_under_b_mode) {
+				btcoexist->btc_write_4byte(btcoexist,
+							   0x430, 0x0);
+				btcoexist->btc_write_4byte(btcoexist,
+							   0x434,
+							   0x01010101);
+			} else {
+				btcoexist->btc_write_4byte(btcoexist,
+							   0x430, 0x0);
+				btcoexist->btc_write_4byte(btcoexist,
+							   0x434,
+							   0x04030201);
+			}
+			break;
+		default:
+			break;
+		}
+	}
+
+	coex_dm->pre_arfr_type = coex_dm->cur_arfr_type;
+}
+
+static void halbtc8821a1ant_retry_limit(struct btc_coexist *btcoexist,
+					bool force_exec, u8 type)
+{
+	coex_dm->cur_retry_limit_type = type;
+
+	if (force_exec || (coex_dm->pre_retry_limit_type !=
+	    coex_dm->cur_retry_limit_type)) {
+		switch (coex_dm->cur_retry_limit_type) {
+		case 0:	/*  normal mode */
+			btcoexist->btc_write_2byte(btcoexist, 0x42a, coex_dm->backup_retry_limit);
+			break;
+		case 1:	/*  retry limit = 8 */
+			btcoexist->btc_write_2byte(btcoexist, 0x42a, 0x0808);
+			break;
+		default:
+			break;
+		}
+	}
+
+	coex_dm->pre_retry_limit_type = coex_dm->cur_retry_limit_type;
+}
+
+static void halbtc8821a1ant_ampdu_max_time(struct btc_coexist *btcoexist,
+					   bool force_exec, u8 type)
+{
+	coex_dm->cur_ampdu_time_type = type;
+
+	if (force_exec ||
+	    (coex_dm->pre_ampdu_time_type != coex_dm->cur_ampdu_time_type)) {
+		switch (coex_dm->cur_ampdu_time_type) {
+		case 0:	/*  normal mode */
+			btcoexist->btc_write_1byte(btcoexist, 0x456, coex_dm->backup_ampdu_max_time);
+			break;
+		case 1:	/*  AMPDU timw = 0x38 * 32us */
+			btcoexist->btc_write_1byte(btcoexist, 0x456, 0x38);
+			break;
+		default:
+			break;
+		}
+	}
+
+	coex_dm->pre_ampdu_time_type = coex_dm->cur_ampdu_time_type;
+}
+
+static void halbtc8821a1ant_limited_tx(struct btc_coexist *btcoexist,
+				       bool force_exec, u8 ra_mask_type,
+				       u8 arfr_type, u8 retry_limit_type,
+				       u8 ampdu_time_type)
+{
+	switch (ra_mask_type) {
+	case 0:	/*  normal mode */
+		update_ra_mask(btcoexist, force_exec, 0x0);
+		break;
+	case 1:	/*  disable cck 1/2 */
+		update_ra_mask(btcoexist, force_exec, 0x00000003);
+		break;
+	case 2:	/*  disable cck 1/2/5.5, ofdm 6/9/12/18/24, mcs 0/1/2/3/4 */
+		update_ra_mask(btcoexist, force_exec, 0x0001f1f7);
+		break;
+	default:
+		break;
+	}
+
+	auto_rate_fallback_retry(btcoexist, force_exec, arfr_type);
+	halbtc8821a1ant_retry_limit(btcoexist, force_exec, retry_limit_type);
+	halbtc8821a1ant_ampdu_max_time(btcoexist, force_exec, ampdu_time_type);
+}
+
+static void halbtc8821a1ant_limited_rx(struct btc_coexist *btcoexist,
+				       bool force_exec, bool rej_ap_agg_pkt,
+				       bool bt_ctrl_agg_buf_size,
+				       u8 agg_buf_size)
+{
+	bool reject_rx_agg = rej_ap_agg_pkt;
+	bool bt_ctrl_rx_agg_size = bt_ctrl_agg_buf_size;
+	u8 rx_agg_size = agg_buf_size;
+
+	/*  */
+	/*	Rx Aggregation related setting */
+	/*  */
+	btcoexist->btc_set(btcoexist, BTC_SET_BL_TO_REJ_AP_AGG_PKT, &reject_rx_agg);
+	/*  decide BT control aggregation buf size or not */
+	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_CTRL_AGG_SIZE, &bt_ctrl_rx_agg_size);
+	/*  aggregation buf size, only work when BT control Rx aggregation size. */
+	btcoexist->btc_set(btcoexist, BTC_SET_U1_AGG_BUF_SIZE, &rx_agg_size);
+	/*  real update aggregation setting */
+	btcoexist->btc_set(btcoexist, BTC_SET_ACT_AGGREGATE_CTRL, NULL);
+
+}
+
+static void halbtc8821a1ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
+{
+	u32 reg_hp_tx_rx, reg_lp_tx_rx, u4_tmp;
+	u32 reg_hp_tx = 0, reg_hp_rx = 0, reg_lp_tx = 0, reg_lp_rx = 0;
+
+	reg_hp_tx_rx = 0x770;
+	reg_lp_tx_rx = 0x774;
+
+	u4_tmp = btcoexist->btc_read_4byte(btcoexist, reg_hp_tx_rx);
+	reg_hp_tx = u4_tmp & MASKLWORD;
+	reg_hp_rx = (u4_tmp & MASKHWORD)>>16;
+
+	u4_tmp = btcoexist->btc_read_4byte(btcoexist, reg_lp_tx_rx);
+	reg_lp_tx = u4_tmp & MASKLWORD;
+	reg_lp_rx = (u4_tmp & MASKHWORD)>>16;
+
+	coex_sta->high_priority_tx = reg_hp_tx;
+	coex_sta->high_priority_rx = reg_hp_rx;
+	coex_sta->low_priority_tx = reg_lp_tx;
+	coex_sta->low_priority_rx = reg_lp_rx;
+
+	/*  reset counter */
+	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
+}
+
+static void halbtc8821a1ant_query_bt_info(struct btc_coexist *btcoexist)
+{
+	u8 h2c_parameter[1] = {0};
+
+	coex_sta->c2h_bt_info_req_sent = true;
+
+	h2c_parameter[0] |= BIT(0);	/*  trigger */
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], Query Bt Info, FW write 0x61 = 0x%x\n",
+		h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x61, 1, h2c_parameter);
+}
+
+static void halbtc8821a1ant_update_bt_link_info(struct btc_coexist *btcoexist)
+{
+	struct btc_bt_link_info	*bt_link_info = &btcoexist->bt_link_info;
+	bool bt_hs_on = false;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+
+	bt_link_info->bt_link_exist = coex_sta->bt_link_exist;
+	bt_link_info->sco_exist = coex_sta->sco_exist;
+	bt_link_info->a2dp_exist = coex_sta->a2dp_exist;
+	bt_link_info->pan_exist = coex_sta->pan_exist;
+	bt_link_info->hid_exist = coex_sta->hid_exist;
+
+	/*  work around for HS mode. */
+	if (bt_hs_on) {
+		bt_link_info->pan_exist = true;
+		bt_link_info->bt_link_exist = true;
+	}
+
+	/*  check if Sco only */
+	if (bt_link_info->sco_exist &&
+		!bt_link_info->a2dp_exist &&
+		!bt_link_info->pan_exist &&
+		!bt_link_info->hid_exist)
+		bt_link_info->sco_only = true;
+	else
+		bt_link_info->sco_only = false;
+
+	/*  check if A2dp only */
+	if (!bt_link_info->sco_exist &&
+		bt_link_info->a2dp_exist &&
+		!bt_link_info->pan_exist &&
+		!bt_link_info->hid_exist)
+		bt_link_info->a2dp_only = true;
+	else
+		bt_link_info->a2dp_only = false;
+
+	/*  check if Pan only */
+	if (!bt_link_info->sco_exist &&
+		!bt_link_info->a2dp_exist &&
+		bt_link_info->pan_exist &&
+		!bt_link_info->hid_exist)
+		bt_link_info->pan_only = true;
+	else
+		bt_link_info->pan_only = false;
+
+	/*  check if Hid only */
+	if (!bt_link_info->sco_exist &&
+		!bt_link_info->a2dp_exist &&
+		!bt_link_info->pan_exist &&
+		bt_link_info->hid_exist)
+		bt_link_info->hid_only = true;
+	else
+		bt_link_info->hid_only = false;
+}
+
+static u8 halbtc8821a1ant_action_algorithm(struct btc_coexist *btcoexist)
+{
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	bool bt_hs_on = false;
+	u8 algorithm = BT_8821A_1ANT_COEX_ALGO_UNDEFINED;
+	u8 num_of_diff_profile = 0;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+
+	if (!bt_link_info->bt_link_exist) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], No BT link exists!!!\n");
+		return algorithm;
+	}
+
+	if (bt_link_info->sco_exist)
+		num_of_diff_profile++;
+	if (bt_link_info->hid_exist)
+		num_of_diff_profile++;
+	if (bt_link_info->pan_exist)
+		num_of_diff_profile++;
+	if (bt_link_info->a2dp_exist)
+		num_of_diff_profile++;
+
+	if (num_of_diff_profile == 1) {
+		if (bt_link_info->sco_exist) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = SCO only\n");
+			algorithm = BT_8821A_1ANT_COEX_ALGO_SCO;
+		} else {
+			if (bt_link_info->hid_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = HID only\n");
+				algorithm = BT_8821A_1ANT_COEX_ALGO_HID;
+			} else if (bt_link_info->a2dp_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = A2DP only\n");
+				algorithm = BT_8821A_1ANT_COEX_ALGO_A2DP;
+			} else if (bt_link_info->pan_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = PAN(HS) only\n");
+					algorithm = BT_8821A_1ANT_COEX_ALGO_PANHS;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = PAN(EDR) only\n");
+					algorithm = BT_8821A_1ANT_COEX_ALGO_PANEDR;
+				}
+			}
+		}
+	} else if (num_of_diff_profile == 2) {
+		if (bt_link_info->sco_exist) {
+			if (bt_link_info->hid_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = SCO + HID\n");
+				algorithm = BT_8821A_1ANT_COEX_ALGO_HID;
+			} else if (bt_link_info->a2dp_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = SCO + A2DP ==> SCO\n");
+				algorithm = BT_8821A_1ANT_COEX_ALGO_SCO;
+			} else if (bt_link_info->pan_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = SCO + PAN(HS)\n");
+					algorithm = BT_8821A_1ANT_COEX_ALGO_SCO;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = SCO + PAN(EDR)\n");
+					algorithm = BT_8821A_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		} else {
+			if (bt_link_info->hid_exist &&
+				bt_link_info->a2dp_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = HID + A2DP\n");
+				algorithm = BT_8821A_1ANT_COEX_ALGO_HID_A2DP;
+			} else if (bt_link_info->hid_exist &&
+				bt_link_info->pan_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = HID + PAN(HS)\n");
+					algorithm = BT_8821A_1ANT_COEX_ALGO_HID_A2DP;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = HID + PAN(EDR)\n");
+					algorithm = BT_8821A_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			} else if (bt_link_info->pan_exist &&
+				bt_link_info->a2dp_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = A2DP + PAN(HS)\n");
+					algorithm = BT_8821A_1ANT_COEX_ALGO_A2DP_PANHS;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = A2DP + PAN(EDR)\n");
+					algorithm = BT_8821A_1ANT_COEX_ALGO_PANEDR_A2DP;
+				}
+			}
+		}
+	} else if (num_of_diff_profile == 3) {
+		if (bt_link_info->sco_exist) {
+			if (bt_link_info->hid_exist &&
+				bt_link_info->a2dp_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = SCO + HID + A2DP ==> HID\n");
+				algorithm = BT_8821A_1ANT_COEX_ALGO_HID;
+			} else if (bt_link_info->hid_exist &&
+				bt_link_info->pan_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = SCO + HID + PAN(HS)\n");
+					algorithm = BT_8821A_1ANT_COEX_ALGO_HID_A2DP;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = SCO + HID + PAN(EDR)\n");
+					algorithm = BT_8821A_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			} else if (bt_link_info->pan_exist &&
+				bt_link_info->a2dp_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = SCO + A2DP + PAN(HS)\n");
+					algorithm = BT_8821A_1ANT_COEX_ALGO_SCO;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = SCO + A2DP + PAN(EDR) ==> HID\n");
+					algorithm = BT_8821A_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		} else {
+			if (bt_link_info->hid_exist &&
+				bt_link_info->pan_exist &&
+				bt_link_info->a2dp_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = HID + A2DP + PAN(HS)\n");
+					algorithm = BT_8821A_1ANT_COEX_ALGO_HID_A2DP;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = HID + A2DP + PAN(EDR)\n");
+					algorithm = BT_8821A_1ANT_COEX_ALGO_HID_A2DP_PANEDR;
+				}
+			}
+		}
+	} else if (num_of_diff_profile >= 3) {
+		if (bt_link_info->sco_exist) {
+			if (bt_link_info->hid_exist &&
+				bt_link_info->pan_exist &&
+				bt_link_info->a2dp_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], Error!!! BT Profile = SCO + HID + A2DP + PAN(HS)\n");
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = SCO + HID + A2DP + PAN(EDR) ==>PAN(EDR)+HID\n");
+					algorithm = BT_8821A_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		}
+	}
+
+	return algorithm;
+}
+
+static void halbtc8821a1ant_set_bt_auto_report(struct btc_coexist *btcoexist,
+					       bool enable_auto_report)
+{
+	u8 h2c_parameter[1] = {0};
+
+	h2c_parameter[0] = 0;
+
+	if (enable_auto_report)
+		h2c_parameter[0] |= BIT(0);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], BT FW auto report : %s, FW write 0x68 = 0x%x\n",
+		  (enable_auto_report ? "Enabled!!" : "Disabled!!"),
+		  h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x68, 1, h2c_parameter);
+}
+
+static void halbtc8821a1ant_bt_auto_report(struct btc_coexist *btcoexist,
+					   bool force_exec,
+					   bool enable_auto_report)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, "[BTCoex], %s BT Auto report = %s\n",
+		(force_exec ? "force to" : ""), ((enable_auto_report) ? "Enabled" : "Disabled"));
+	coex_dm->cur_bt_auto_report = enable_auto_report;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, "[BTCoex], pre_bt_auto_report =%d, cur_bt_auto_report =%d\n",
+			coex_dm->pre_bt_auto_report, coex_dm->cur_bt_auto_report);
+
+		if (coex_dm->pre_bt_auto_report == coex_dm->cur_bt_auto_report)
+			return;
+	}
+	halbtc8821a1ant_set_bt_auto_report(btcoexist, coex_dm->cur_bt_auto_report);
+
+	coex_dm->pre_bt_auto_report = coex_dm->cur_bt_auto_report;
+}
+
+static void set_sw_penalty_tx_rate_adaptive(struct btc_coexist *btcoexist,
+					    bool low_penalty_ra)
+{
+	u8 h2c_parameter[6] = {0};
+
+	h2c_parameter[0] = 0x6;	/*  opCode, 0x6 = Retry_Penalty */
+
+	if (low_penalty_ra) {
+		h2c_parameter[1] |= BIT(0);
+		h2c_parameter[2] = 0x00;  /* normal rate except MCS7/6/5,
+					   * OFDM54/48/36 */
+		h2c_parameter[3] = 0xf7;  /* MCS7 or OFDM54 */
+		h2c_parameter[4] = 0xf8;  /* MCS6 or OFDM48 */
+		h2c_parameter[5] = 0xf9;  /* MCS5 or OFDM36  */
+	}
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], set WiFi Low-Penalty Retry: %s",
+		  (low_penalty_ra ? "ON!!" : "OFF!!"));
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x69, 6, h2c_parameter);
+}
+
+static void halbtc8821a1ant_low_penalty_ra(struct btc_coexist *btcoexist,
+					   bool force_exec, bool low_penalty_ra)
+{
+	coex_dm->cur_low_penalty_ra = low_penalty_ra;
+
+	if (!force_exec) {
+		if (coex_dm->pre_low_penalty_ra == coex_dm->cur_low_penalty_ra)
+			return;
+	}
+	set_sw_penalty_tx_rate_adaptive(btcoexist, coex_dm->cur_low_penalty_ra);
+
+	coex_dm->pre_low_penalty_ra = coex_dm->cur_low_penalty_ra;
+}
+
+static void halbtc8821a1ant_set_coex_table(struct btc_coexist *btcoexist,
+					   u32 val0x6c0, u32 val0x6c4,
+					   u32 val0x6c8, u8 val0x6cc)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+		  "[BTCoex], set coex table, set 0x6c0 = 0x%x\n", val0x6c0);
+	btcoexist->btc_write_4byte(btcoexist, 0x6c0, val0x6c0);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+		  "[BTCoex], set coex table, set 0x6c4 = 0x%x\n", val0x6c4);
+	btcoexist->btc_write_4byte(btcoexist, 0x6c4, val0x6c4);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+		  "[BTCoex], set coex table, set 0x6c8 = 0x%x\n", val0x6c8);
+	btcoexist->btc_write_4byte(btcoexist, 0x6c8, val0x6c8);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+		  "[BTCoex], set coex table, set 0x6cc = 0x%x\n", val0x6cc);
+	btcoexist->btc_write_1byte(btcoexist, 0x6cc, val0x6cc);
+}
+
+static void halbtc8821a1ant_coex_table(struct btc_coexist *btcoexist,
+				       bool force_exec, u32 val0x6c0,
+				       u32 val0x6c4, u32 val0x6c8, u8 val0x6cc)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
+		  "[BTCoex], %s write Coex Table 0x6c0 = 0x%x, 0x6c4 = 0x%x, 0x6c8 = 0x%x, 0x6cc = 0x%x\n",
+		  (force_exec ? "force to" : ""), val0x6c0, val0x6c4,
+		  val0x6c8, val0x6cc);
+	coex_dm->cur_val_0x6c0 = val0x6c0;
+	coex_dm->cur_val_0x6c4 = val0x6c4;
+	coex_dm->cur_val_0x6c8 = val0x6c8;
+	coex_dm->cur_val_0x6cc = val0x6cc;
+
+	if (!force_exec) {
+		if ((coex_dm->pre_val_0x6c0 == coex_dm->cur_val_0x6c0) &&
+		    (coex_dm->pre_val_0x6c4 == coex_dm->cur_val_0x6c4) &&
+		    (coex_dm->pre_val_0x6c8 == coex_dm->cur_val_0x6c8) &&
+		    (coex_dm->pre_val_0x6cc == coex_dm->cur_val_0x6cc))
+			return;
+	}
+	halbtc8821a1ant_set_coex_table(btcoexist, val0x6c0, val0x6c4,
+				       val0x6c8, val0x6cc);
+
+	coex_dm->pre_val_0x6c0 = coex_dm->cur_val_0x6c0;
+	coex_dm->pre_val_0x6c4 = coex_dm->cur_val_0x6c4;
+	coex_dm->pre_val_0x6c8 = coex_dm->cur_val_0x6c8;
+	coex_dm->pre_val_0x6cc = coex_dm->cur_val_0x6cc;
+}
+
+static void halbtc8821a1ant_coex_table_with_type(struct btc_coexist *btcoexist,
+						 bool force_exec, u8 type)
+{
+	switch (type) {
+	case 0:
+		halbtc8821a1ant_coex_table(btcoexist, force_exec,
+					   0x55555555, 0x55555555,
+					   0xffffff, 0x3);
+		break;
+	case 1:
+		halbtc8821a1ant_coex_table(btcoexist, force_exec,
+					   0x55555555, 0x5a5a5a5a,
+					   0xffffff, 0x3);
+		break;
+	case 2:
+		halbtc8821a1ant_coex_table(btcoexist, force_exec,
+					   0x5a5a5a5a, 0x5a5a5a5a,
+					   0xffffff, 0x3);
+		break;
+	case 3:
+		halbtc8821a1ant_coex_table(btcoexist, force_exec,
+					   0x55555555, 0xaaaaaaaa,
+					   0xffffff, 0x3);
+		break;
+	case 4:
+		halbtc8821a1ant_coex_table(btcoexist, force_exec,
+					   0xffffffff, 0xffffffff,
+					   0xffffff, 0x3);
+		break;
+	case 5:
+		halbtc8821a1ant_coex_table(btcoexist, force_exec,
+					   0x5fff5fff, 0x5fff5fff,
+					   0xffffff, 0x3);
+		break;
+	case 6:
+		halbtc8821a1ant_coex_table(btcoexist, force_exec,
+					   0x55ff55ff, 0x5a5a5a5a,
+					   0xffffff, 0x3);
+		break;
+	case 7:
+		halbtc8821a1ant_coex_table(btcoexist, force_exec,
+					   0x5afa5afa, 0x5afa5afa,
+					   0xffffff, 0x3);
+		break;
+	default:
+		break;
+	}
+}
+
+static void halbtc8821a1ant_set_fw_ignore_wlan_act(struct btc_coexist *btcoexist,
+						   bool enable)
+{
+	u8 h2c_parameter[1] = {0};
+
+	if (enable)
+		h2c_parameter[0] |= BIT(0);	/*  function enable */
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], set FW for BT Ignore Wlan_Act, FW write 0x63 = 0x%x\n",
+		  h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x63, 1, h2c_parameter);
+}
+
+static void halbtc8821a1ant_ignore_wlan_act(struct btc_coexist *btcoexist,
+					    bool force_exec, bool enable)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], %s turn Ignore WlanAct %s\n",
+		  (force_exec ? "force to" : ""), (enable ? "ON" : "OFF"));
+	coex_dm->cur_ignore_wlan_act = enable;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], pre_ignore_wlan_act = %d, cur_ignore_wlan_act = %d!!\n",
+			  coex_dm->pre_ignore_wlan_act,
+			  coex_dm->cur_ignore_wlan_act);
+
+		if (coex_dm->pre_ignore_wlan_act ==
+		    coex_dm->cur_ignore_wlan_act)
+			return;
+	}
+	halbtc8821a1ant_set_fw_ignore_wlan_act(btcoexist, enable);
+
+	coex_dm->pre_ignore_wlan_act = coex_dm->cur_ignore_wlan_act;
+}
+
+static void
+halbtc8821a1ant_set_fw_pstdma(
+	struct btc_coexist *btcoexist,
+	u8 byte1,
+	u8 byte2,
+	u8 byte3,
+	u8 byte4,
+	u8 byte5
+	)
+{
+	u8 h2c_parameter[5] = {0};
+
+	h2c_parameter[0] = byte1;
+	h2c_parameter[1] = byte2;
+	h2c_parameter[2] = byte3;
+	h2c_parameter[3] = byte4;
+	h2c_parameter[4] = byte5;
+
+	coex_dm->ps_tdma_para[0] = byte1;
+	coex_dm->ps_tdma_para[1] = byte2;
+	coex_dm->ps_tdma_para[2] = byte3;
+	coex_dm->ps_tdma_para[3] = byte4;
+	coex_dm->ps_tdma_para[4] = byte5;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, "[BTCoex], PS-TDMA H2C cmd = 0x%x%08x\n",
+		h2c_parameter[0],
+		h2c_parameter[1]<<24|h2c_parameter[2]<<16|h2c_parameter[3]<<8|h2c_parameter[4]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x60, 5, h2c_parameter);
+}
+
+static void
+halbtc8821a1ant_set_lps_rpwm(
+	struct btc_coexist *btcoexist,
+	u8 lps_val,
+	u8 rpwm_val
+	)
+{
+	u8 lps = lps_val;
+	u8 rpwm = rpwm_val;
+
+	btcoexist->btc_set(btcoexist, BTC_SET_U1_LPS_VAL, &lps);
+	btcoexist->btc_set(btcoexist, BTC_SET_U1_RPWM_VAL, &rpwm);
+}
+
+static void
+halbtc8821a1ant_lps_rpwm(
+	struct btc_coexist *btcoexist,
+	bool force_exec,
+	u8 lps_val,
+	u8 rpwm_val
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, "[BTCoex], %s set lps/rpwm = 0x%x/0x%x\n",
+		(force_exec ? "force to" : ""), lps_val, rpwm_val);
+	coex_dm->cur_lps = lps_val;
+	coex_dm->cur_rpwm = rpwm_val;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, "[BTCoex], LPS-RxBeaconMode = 0x%x , LPS-RPWM = 0x%x!!\n",
+			 coex_dm->cur_lps, coex_dm->cur_rpwm);
+
+		if ((coex_dm->pre_lps == coex_dm->cur_lps) &&
+			(coex_dm->pre_rpwm == coex_dm->cur_rpwm)) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, "[BTCoex], LPS-RPWM_Last = 0x%x , LPS-RPWM_Now = 0x%x!!\n",
+				 coex_dm->pre_rpwm, coex_dm->cur_rpwm);
+
+			return;
+		}
+	}
+	halbtc8821a1ant_set_lps_rpwm(btcoexist, lps_val, rpwm_val);
+
+	coex_dm->pre_lps = coex_dm->cur_lps;
+	coex_dm->pre_rpwm = coex_dm->cur_rpwm;
+}
+
+static void
+halbtc8821a1ant_sw_mechanism(
+	struct btc_coexist *btcoexist,
+	bool low_penalty_ra
+	)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, "[BTCoex], SM[LpRA] = %d\n", low_penalty_ra);
+
+	halbtc8821a1ant_low_penalty_ra(btcoexist, NORMAL_EXEC, low_penalty_ra);
+}
+
+static void
+halbtc8821a1ant_set_ant_path(
+	struct btc_coexist *btcoexist,
+	u8 ant_pos_type,
+	bool init_hw_cfg,
+	bool wifi_off
+	)
+{
+	struct btc_board_info *board_info = &btcoexist->board_info;
+	u32 u4_tmp = 0;
+	u8 h2c_parameter[2] = {0};
+
+	if (init_hw_cfg) {
+		/*  0x4c[23] = 0, 0x4c[24] = 1  Antenna control by WL/BT */
+		u4_tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
+		u4_tmp &= ~BIT(23);
+		u4_tmp |= BIT(24);
+		btcoexist->btc_write_4byte(btcoexist, 0x4c, u4_tmp);
+
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x975, 0x3, 0x3);
+		btcoexist->btc_write_1byte(btcoexist, 0xcb4, 0x77);
+
+		if (board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT) {
+			/* tell firmware "antenna inverse"  ==> WRONG firmware antenna control code.==>need fw to fix */
+			h2c_parameter[0] = 1;
+			h2c_parameter[1] = 1;
+			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2, h2c_parameter);
+
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x64, 0x1, 0x1); /* Main Ant to  BT for IPS case 0x4c[23] = 1 */
+		} else {
+			/* tell firmware "no antenna inverse" ==> WRONG firmware antenna control code.==>need fw to fix */
+			h2c_parameter[0] = 0;
+			h2c_parameter[1] = 1;
+			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2, h2c_parameter);
+
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x64, 0x1, 0x0); /* Aux Ant to  BT for IPS case 0x4c[23] = 1 */
+		}
+	} else if (wifi_off) {
+		/*  0x4c[24:23] = 00, Set Antenna control by BT_RFE_CTRL	BT Vendor 0xac = 0xf002 */
+		u4_tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
+		u4_tmp &= ~BIT(23);
+		u4_tmp &= ~BIT(24);
+		btcoexist->btc_write_4byte(btcoexist, 0x4c, u4_tmp);
+	}
+
+	/*  ext switch setting */
+	switch (ant_pos_type) {
+	case BTC_ANT_PATH_WIFI:
+		if (board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0xcb7, 0x30, 0x1);
+		else
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0xcb7, 0x30, 0x2);
+		break;
+	case BTC_ANT_PATH_BT:
+		if (board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0xcb7, 0x30, 0x2);
+		else
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0xcb7, 0x30, 0x1);
+		break;
+	case BTC_ANT_PATH_PTA:
+	default:
+		if (board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0xcb7, 0x30, 0x1);
+		else
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0xcb7, 0x30, 0x2);
+		break;
+	}
+}
+
+static void
+halbtc8821a1ant_ps_tdma(
+	struct btc_coexist *btcoexist,
+	bool force_exec,
+	bool turn_on,
+	u8 type
+	)
+{
+	u8 rssi_adjust_val = 0;
+
+	coex_dm->cur_ps_tdma_on = turn_on;
+	coex_dm->cur_ps_tdma = type;
+
+	if (!force_exec) {
+		if (coex_dm->cur_ps_tdma_on) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, "[BTCoex], ********** TDMA(on, %d) **********\n",
+				coex_dm->cur_ps_tdma);
+		} else {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, "[BTCoex], ********** TDMA(off, %d) **********\n",
+				coex_dm->cur_ps_tdma);
+		}
+
+		if ((coex_dm->pre_ps_tdma_on == coex_dm->cur_ps_tdma_on) &&
+			(coex_dm->pre_ps_tdma == coex_dm->cur_ps_tdma))
+			return;
+	}
+	if (turn_on) {
+		switch (type) {
+		default:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x1a, 0x1a, 0x0, 0x50);
+			break;
+		case 1:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x3a, 0x03, 0x10, 0x50);
+			rssi_adjust_val = 11;
+			break;
+		case 2:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x2b, 0x03, 0x10, 0x50);
+			rssi_adjust_val = 14;
+			break;
+		case 3:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x1d, 0x1d, 0x0, 0x10);
+			break;
+		case 4:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x93, 0x15, 0x3, 0x14, 0x0);
+			rssi_adjust_val = 17;
+			break;
+		case 5:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x61, 0x15, 0x3, 0x11, 0x10);
+			break;
+		case 6:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x13, 0xa, 0x3, 0x0, 0x0);
+			break;
+		case 7:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x13, 0xc, 0x5, 0x0, 0x0);
+			break;
+		case 8:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x93, 0x25, 0x3, 0x10, 0x0);
+			break;
+		case 9:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x21, 0x3, 0x10, 0x50);
+			rssi_adjust_val = 18;
+			break;
+		case 10:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x13, 0xa, 0xa, 0x0, 0x40);
+			break;
+		case 11:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x14, 0x03, 0x10, 0x10);
+			rssi_adjust_val = 20;
+			break;
+		case 12:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x0a, 0x0a, 0x0, 0x50);
+			break;
+		case 13:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x18, 0x18, 0x0, 0x10);
+			break;
+		case 14:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x21, 0x3, 0x10, 0x10);
+			break;
+		case 15:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x13, 0xa, 0x3, 0x8, 0x0);
+			break;
+		case 16:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x93, 0x15, 0x3, 0x10, 0x0);
+			rssi_adjust_val = 18;
+			break;
+		case 18:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x93, 0x25, 0x3, 0x10, 0x0);
+			rssi_adjust_val = 14;
+			break;
+		case 20:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x61, 0x35, 0x03, 0x11, 0x10);
+			break;
+		case 21:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x61, 0x15, 0x03, 0x11, 0x10);
+			break;
+		case 22:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x61, 0x25, 0x03, 0x11, 0x10);
+			break;
+		case 23:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xe3, 0x25, 0x3, 0x31, 0x18);
+			rssi_adjust_val = 22;
+			break;
+		case 24:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xe3, 0x15, 0x3, 0x31, 0x18);
+			rssi_adjust_val = 22;
+			break;
+		case 25:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xe3, 0xa, 0x3, 0x31, 0x18);
+			rssi_adjust_val = 22;
+			break;
+		case 26:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xe3, 0xa, 0x3, 0x31, 0x18);
+			rssi_adjust_val = 22;
+			break;
+		case 27:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xe3, 0x25, 0x3, 0x31, 0x98);
+			rssi_adjust_val = 22;
+			break;
+		case 28:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x69, 0x25, 0x3, 0x31, 0x0);
+			break;
+		case 29:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xab, 0x1a, 0x1a, 0x1, 0x10);
+			break;
+		case 30:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x14, 0x3, 0x10, 0x50);
+			break;
+		case 31:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xd3, 0x1a, 0x1a, 0, 0x58);
+			break;
+		case 32:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x61, 0xa, 0x3, 0x10, 0x0);
+			break;
+		case 33:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xa3, 0x25, 0x3, 0x30, 0x90);
+			break;
+		case 34:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x53, 0x1a, 0x1a, 0x0, 0x10);
+			break;
+		case 35:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x63, 0x1a, 0x1a, 0x0, 0x10);
+			break;
+		case 36:
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xd3, 0x12, 0x3, 0x14, 0x50);
+				break;
+		}
+	} else {
+		/*  disable PS tdma */
+		switch (type) {
+		case 8: /* PTA Control */
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x8, 0x0, 0x0, 0x0, 0x0);
+			halbtc8821a1ant_set_ant_path(btcoexist, BTC_ANT_PATH_PTA, false, false);
+			break;
+		case 0:
+		default:  /* Software control, Antenna at BT side */
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x0, 0x0, 0x0, 0x0, 0x0);
+			halbtc8821a1ant_set_ant_path(btcoexist, BTC_ANT_PATH_BT, false, false);
+			break;
+		case 9:   /* Software control, Antenna at WiFi side */
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x0, 0x0, 0x0, 0x0, 0x0);
+			halbtc8821a1ant_set_ant_path(btcoexist, BTC_ANT_PATH_WIFI, false, false);
+			break;
+		case 10:	/*  under 5G */
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x0, 0x0, 0x0, 0x8, 0x0);
+			halbtc8821a1ant_set_ant_path(btcoexist, BTC_ANT_PATH_BT, false, false);
+			break;
+		}
+	}
+	rssi_adjust_val = 0;
+	btcoexist->btc_set(btcoexist, BTC_SET_U1_RSSI_ADJ_VAL_FOR_1ANT_COEX_TYPE, &rssi_adjust_val);
+
+	/*  update pre state */
+	coex_dm->pre_ps_tdma_on = coex_dm->cur_ps_tdma_on;
+	coex_dm->pre_ps_tdma = coex_dm->cur_ps_tdma;
+}
+
+static bool
+halbtc8821a1ant_is_common_action(
+	struct btc_coexist *btcoexist
+	)
+{
+	bool bCommon = false, wifi_connected = false, wifi_busy = false;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED, &wifi_connected);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+
+	if (!wifi_connected &&
+		BT_8821A_1ANT_BT_STATUS_NON_CONNECTED_IDLE == coex_dm->bt_status) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], Wifi non connected-idle + BT non connected-idle!!\n");
+		halbtc8821a1ant_sw_mechanism(btcoexist, false);
+
+		bCommon = true;
+	} else if (wifi_connected &&
+			(BT_8821A_1ANT_BT_STATUS_NON_CONNECTED_IDLE == coex_dm->bt_status)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], Wifi connected + BT non connected-idle!!\n");
+		halbtc8821a1ant_sw_mechanism(btcoexist, false);
+
+		bCommon = true;
+	} else if (!wifi_connected &&
+		(BT_8821A_1ANT_BT_STATUS_CONNECTED_IDLE == coex_dm->bt_status)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], Wifi non connected-idle + BT connected-idle!!\n");
+		halbtc8821a1ant_sw_mechanism(btcoexist, false);
+
+		bCommon = true;
+	} else if (wifi_connected &&
+		(BT_8821A_1ANT_BT_STATUS_CONNECTED_IDLE == coex_dm->bt_status)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], Wifi connected + BT connected-idle!!\n");
+		halbtc8821a1ant_sw_mechanism(btcoexist, false);
+
+		bCommon = true;
+	} else if (!wifi_connected &&
+		(BT_8821A_1ANT_BT_STATUS_CONNECTED_IDLE != coex_dm->bt_status)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], Wifi non connected-idle + BT Busy!!\n");
+		halbtc8821a1ant_sw_mechanism(btcoexist, false);
+
+		bCommon = true;
+	} else {
+		if (wifi_busy) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], Wifi Connected-Busy + BT Busy!!\n");
+		} else {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], Wifi Connected-Idle + BT Busy!!\n");
+		}
+
+		bCommon = false;
+	}
+
+	return bCommon;
+}
+
+static void
+halbtc8821a1ant_tdma_duration_adjust_for_acl(
+	struct btc_coexist *btcoexist,
+	u8 wifi_status
+	)
+{
+	static long		up, dn, m, n, wait_count;
+	long			result;   /* 0: no change, +1: increase WiFi duration, -1: decrease WiFi duration */
+	u8 retry_count = 0, bt_info_ext;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, "[BTCoex], TdmaDurationAdjustForAcl()\n");
+
+	if ((BT_8821A_1ANT_WIFI_STATUS_NON_CONNECTED_ASSO_AUTH_SCAN == wifi_status) ||
+		(BT_8821A_1ANT_WIFI_STATUS_CONNECTED_SCAN == wifi_status) ||
+		(BT_8821A_1ANT_WIFI_STATUS_CONNECTED_SPECIAL_PKT == wifi_status)) {
+		if (coex_dm->cur_ps_tdma != 1 &&
+			coex_dm->cur_ps_tdma != 2 &&
+			coex_dm->cur_ps_tdma != 3 &&
+			coex_dm->cur_ps_tdma != 9) {
+			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
+			coex_dm->ps_tdma_du_adj_type = 9;
+
+			up = 0;
+			dn = 0;
+			m = 1;
+			n = 3;
+			result = 0;
+			wait_count = 0;
+		}
+		return;
+	}
+
+	if (!coex_dm->auto_tdma_adjust) {
+		coex_dm->auto_tdma_adjust = true;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, "[BTCoex], first run TdmaDurationAdjust()!!\n");
+
+		halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 2);
+		coex_dm->ps_tdma_du_adj_type = 2;
+		/*  */
+		up = 0;
+		dn = 0;
+		m = 1;
+		n = 3;
+		result = 0;
+		wait_count = 0;
+	} else {
+		/* accquire the BT TRx retry count from BT_Info byte2 */
+		retry_count = coex_sta->bt_retry_cnt;
+		bt_info_ext = coex_sta->bt_info_ext;
+		/* BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, "[BTCoex], retry_count = %d\n", retry_count)); */
+		/* BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, "[BTCoex], up =%d, dn =%d, m =%d, n =%d, wait_count =%d\n",  */
+		/*	up, dn, m, n, wait_count)); */
+		result = 0;
+		wait_count++;
+
+		if (retry_count == 0) {
+			/*  no retry in the last 2-second duration */
+			up++;
+			dn--;
+
+			if (dn <= 0)
+				dn = 0;
+
+			if (up >= n) {
+				wait_count = 0;
+				n = 3;
+				up = 0;
+				dn = 0;
+				result = 1;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, "[BTCoex], Increase wifi duration!!\n");
+			}
+		} else if (retry_count <= 3) {
+			up--;
+			dn++;
+
+			if (up <= 0)
+				up = 0;
+
+			if (dn == 2) {
+				if (wait_count <= 2)
+					m++;
+				else
+					m = 1;
+				if (m >= 20)
+					m = 20;
+
+				n = 3*m;
+				up = 0;
+				dn = 0;
+				wait_count = 0;
+				result = -1;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, "[BTCoex], Decrease wifi duration for retryCounter<3!!\n");
+			}
+		} else {
+			if (wait_count == 1)
+				m++;
+			else
+				m = 1;
+			if (m >= 20)
+				m = 20;
+
+			n = 3*m;
+			up = 0;
+			dn = 0;
+			wait_count = 0;
+			result = -1;
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, "[BTCoex], Decrease wifi duration for retryCounter>3!!\n");
+		}
+
+		if (result == -1) {
+			if ((BT_INFO_8821A_1ANT_A2DP_BASIC_RATE(bt_info_ext)) &&
+				((coex_dm->cur_ps_tdma == 1) || (coex_dm->cur_ps_tdma == 2))) {
+				halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
+				coex_dm->ps_tdma_du_adj_type = 9;
+			} else if (coex_dm->cur_ps_tdma == 1) {
+				halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 2);
+				coex_dm->ps_tdma_du_adj_type = 2;
+			} else if (coex_dm->cur_ps_tdma == 2) {
+				halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
+				coex_dm->ps_tdma_du_adj_type = 9;
+			} else if (coex_dm->cur_ps_tdma == 9) {
+				halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 11);
+				coex_dm->ps_tdma_du_adj_type = 11;
+			}
+		} else if (result == 1) {
+			if ((BT_INFO_8821A_1ANT_A2DP_BASIC_RATE(bt_info_ext)) &&
+				((coex_dm->cur_ps_tdma == 1) || (coex_dm->cur_ps_tdma == 2))) {
+				halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
+				coex_dm->ps_tdma_du_adj_type = 9;
+			} else if (coex_dm->cur_ps_tdma == 11) {
+				halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
+				coex_dm->ps_tdma_du_adj_type = 9;
+			} else if (coex_dm->cur_ps_tdma == 9) {
+				halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 2);
+				coex_dm->ps_tdma_du_adj_type = 2;
+			} else if (coex_dm->cur_ps_tdma == 2) {
+				halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 1);
+				coex_dm->ps_tdma_du_adj_type = 1;
+			}
+		} else {
+			/* no change */
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, "[BTCoex], ********** TDMA(on, %d) **********\n",
+				coex_dm->cur_ps_tdma);
+		}
+
+		if (coex_dm->cur_ps_tdma != 1 &&
+			coex_dm->cur_ps_tdma != 2 &&
+			coex_dm->cur_ps_tdma != 9 &&
+			coex_dm->cur_ps_tdma != 11) {
+			/*  recover to previous adjust type */
+			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, coex_dm->ps_tdma_du_adj_type);
+		}
+	}
+}
+
+static void
+halbtc8821a1ant_ps_tdma_check_for_power_save_state(
+	struct btc_coexist *btcoexist,
+	bool new_ps_state
+	)
+{
+	u8 lps_mode = 0x0;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_LPS_MODE, &lps_mode);
+
+	if (lps_mode) {
+		/*  already under LPS state */
+		if (!new_ps_state) {
+			/*  will leave LPS state, turn off psTdma first */
+			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 0);
+		}
+	} else {
+		/*  NO PS state */
+		if (new_ps_state) {
+			/*  will enter LPS state, turn off psTdma first */
+			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 0);
+		} else {
+			/*  keep state under NO PS state, do nothing. */
+		}
+	}
+}
+
+static void
+halbtc8821a1ant_power_save_state(
+	struct btc_coexist *btcoexist,
+	u8 ps_type,
+	u8 lps_val,
+	u8 rpwm_val
+	)
+{
+	bool low_pwr_disable = false;
+
+	switch (ps_type) {
+	case BTC_PS_WIFI_NATIVE:
+		/*  recover to original 32k low power setting */
+		low_pwr_disable = false;
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER, &low_pwr_disable);
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_NORMAL_LPS, NULL);
+		break;
+	case BTC_PS_LPS_ON:
+		halbtc8821a1ant_ps_tdma_check_for_power_save_state(btcoexist, true);
+		halbtc8821a1ant_lps_rpwm(btcoexist, NORMAL_EXEC, lps_val, rpwm_val);
+		/*  when coex force to enter LPS, do not enter 32k low power. */
+		low_pwr_disable = true;
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER, &low_pwr_disable);
+		/*  power save must executed before psTdma. */
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_ENTER_LPS, NULL);
+		break;
+	case BTC_PS_LPS_OFF:
+		halbtc8821a1ant_ps_tdma_check_for_power_save_state(btcoexist, false);
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
+		break;
+	default:
+		break;
+	}
+}
+
+static void
+halbtc8821a1ant_coex_under_5g(
+	struct btc_coexist *btcoexist
+	)
+{
+	halbtc8821a1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+
+	halbtc8821a1ant_ignore_wlan_act(btcoexist, NORMAL_EXEC, true);
+
+	halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 10);
+
+	halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
+
+	halbtc8821a1ant_limited_tx(btcoexist, NORMAL_EXEC, 0, 0, 0, 0);
+
+	halbtc8821a1ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 5);
+}
+
+static void
+halbtc8821a1ant_action_wifi_only(
+	struct btc_coexist *btcoexist
+	)
+{
+	halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
+	halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 9);
+}
+
+static void
+halbtc8821a1ant_monitor_bt_enable_disable(
+	struct btc_coexist *btcoexist
+	)
+{
+	static bool pre_bt_disabled;
+	static u32 bt_disable_cnt;
+	bool bt_active = true, bt_disabled = false;
+
+	/*  This function check if bt is disabled */
+
+	if (coex_sta->high_priority_tx == 0 &&
+		coex_sta->high_priority_rx == 0 &&
+		coex_sta->low_priority_tx == 0 &&
+		coex_sta->low_priority_rx == 0) {
+		bt_active = false;
+	}
+	if (coex_sta->high_priority_tx == 0xffff &&
+	    coex_sta->high_priority_rx == 0xffff &&
+	    coex_sta->low_priority_tx == 0xffff &&
+	    coex_sta->low_priority_rx == 0xffff) {
+		bt_active = false;
+	}
+	if (bt_active) {
+		bt_disable_cnt = 0;
+		bt_disabled = false;
+		btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_DISABLE,
+				   &bt_disabled);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+			  "[BTCoex], BT is enabled !!\n");
+	} else {
+		bt_disable_cnt++;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+			  "[BTCoex], bt all counters = 0, %d times!!\n",
+			  bt_disable_cnt);
+		if (bt_disable_cnt >= 2) {
+			bt_disabled = true;
+			btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_DISABLE,
+					   &bt_disabled);
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+				  "[BTCoex], BT is disabled !!\n");
+			halbtc8821a1ant_action_wifi_only(btcoexist);
+		}
+	}
+	if (pre_bt_disabled != bt_disabled) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+			  "[BTCoex], BT is from %s to %s!!\n",
+			  (pre_bt_disabled ? "disabled" : "enabled"),
+			  (bt_disabled ? "disabled" : "enabled"));
+		pre_bt_disabled = bt_disabled;
+		if (bt_disabled) {
+			btcoexist->btc_set(btcoexist,
+					   BTC_SET_ACT_LEAVE_LPS, NULL);
+			btcoexist->btc_set(btcoexist,
+					   BTC_SET_ACT_NORMAL_LPS, NULL);
+		}
+	}
+}
+
+/*	Software Coex Mechanism start */
+static void halbtc8821a1ant_action_sco(struct btc_coexist *btcoexist)
+{
+	halbtc8821a1ant_sw_mechanism(btcoexist, true);
+}
+
+static void halbtc8821a1ant_action_hid(struct btc_coexist *btcoexist)
+{
+	halbtc8821a1ant_sw_mechanism(btcoexist, true);
+}
+
+/* A2DP only / PAN(EDR) only/ A2DP+PAN(HS) */
+static void halbtc8821a1ant_action_a2dp(struct btc_coexist *btcoexist)
+{
+	halbtc8821a1ant_sw_mechanism(btcoexist, false);
+}
+
+static void halbtc8821a1ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
+{
+	halbtc8821a1ant_sw_mechanism(btcoexist, false);
+}
+
+static void halbtc8821a1ant_action_pan_edr(struct btc_coexist *btcoexist)
+{
+	halbtc8821a1ant_sw_mechanism(btcoexist, false);
+}
+
+/* PAN(HS) only */
+static void halbtc8821a1ant_action_pan_hs(struct btc_coexist *btcoexist)
+{
+	halbtc8821a1ant_sw_mechanism(btcoexist, false);
+}
+
+/* PAN(EDR)+A2DP */
+static void halbtc8821a1ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
+{
+	halbtc8821a1ant_sw_mechanism(btcoexist, false);
+}
+
+static void halbtc8821a1ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
+{
+	halbtc8821a1ant_sw_mechanism(btcoexist, true);
+}
+
+/*  HID+A2DP+PAN(EDR) */
+static void halbtc8821a1ant_action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
+{
+	halbtc8821a1ant_sw_mechanism(btcoexist, true);
+}
+
+static void halbtc8821a1ant_action_hid_a2dp(struct btc_coexist *btcoexist)
+{
+	halbtc8821a1ant_sw_mechanism(btcoexist, true);
+}
+
+/*	Non-Software Coex Mechanism start */
+static void halbtc8821a1ant_action_hs(struct btc_coexist *btcoexist)
+{
+	halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
+	halbtc8821a1ant_coex_table_with_type(btcoexist, FORCE_EXEC, 2);
+}
+
+static void halbtc8821a1ant_action_bt_inquiry(struct btc_coexist *btcoexist)
+{
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	bool wifi_connected = false;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED, &wifi_connected);
+
+	if (!wifi_connected) {
+		halbtc8821a1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+		halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
+		halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+	} else if ((bt_link_info->sco_exist) ||
+			(bt_link_info->hid_only)) {
+		/*  SCO/HID-only busy */
+		halbtc8821a1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+		halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 32);
+		halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+	} else {
+		halbtc8821a1ant_power_save_state(btcoexist, BTC_PS_LPS_ON, 0x50, 0x4);
+		halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 30);
+		halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+	}
+}
+
+static void
+halbtc8821a1ant_action_bt_sco_hid_only_busy(
+	struct btc_coexist *btcoexist,
+	u8 wifi_status
+	)
+{
+	/*  tdma and coex table */
+	halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
+
+	if (BT_8821A_1ANT_WIFI_STATUS_NON_CONNECTED_ASSO_AUTH_SCAN == wifi_status)
+		halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+	else
+		halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+}
+
+static void action_wifi_connected_bt_acl_busy(struct btc_coexist *btcoexist, u8 wifi_status)
+{
+	u8 bt_rssi_state;
+
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	bt_rssi_state = halbtc8821a1ant_bt_rssi_state(2, 28, 0);
+
+	if (bt_link_info->hid_only)  {
+		/* HID */
+		halbtc8821a1ant_action_bt_sco_hid_only_busy(btcoexist, wifi_status);
+		coex_dm->auto_tdma_adjust = false;
+		return;
+	} else if (bt_link_info->a2dp_only) { /* A2DP */
+		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			(bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			 halbtc8821a1ant_tdma_duration_adjust_for_acl(btcoexist, wifi_status);
+		} else { /* for low BT RSSI */
+			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 11);
+			coex_dm->auto_tdma_adjust = false;
+		}
+
+		halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+	} else if (bt_link_info->hid_exist && bt_link_info->a2dp_exist) {
+		/* HID+A2DP */
+		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 14);
+			coex_dm->auto_tdma_adjust = false;
+		} else /* for low BT RSSI */ {
+			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 11);
+			coex_dm->auto_tdma_adjust = false;
+		}
+
+		halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+	} else if ((bt_link_info->pan_only) || (bt_link_info->hid_exist && bt_link_info->pan_exist)) {
+		/* PAN(OPP, FTP), HID+PAN(OPP, FTP) */
+		halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 3);
+		halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		coex_dm->auto_tdma_adjust = false;
+	} else if (((bt_link_info->a2dp_exist) && (bt_link_info->pan_exist)) ||
+		   (bt_link_info->hid_exist && bt_link_info->a2dp_exist &&
+		    bt_link_info->pan_exist)) {
+		/* A2DP+PAN(OPP, FTP), HID+A2DP+PAN(OPP, FTP) */
+		halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 13);
+		halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		coex_dm->auto_tdma_adjust = false;
+	} else {
+		halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 11);
+		halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		coex_dm->auto_tdma_adjust = false;
+	}
+}
+
+static void
+halbtc8821a1ant_action_wifi_not_connected(
+	struct btc_coexist *btcoexist
+	)
+{
+	/*  power save state */
+	halbtc8821a1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+
+	/*  tdma and coex table	 */
+	halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
+	halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
+}
+
+static void wifi_not_connected_asso_auth_scan(struct btc_coexist *btcoexist)
+{
+	halbtc8821a1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+
+	halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 22);
+	halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+}
+
+static void
+halbtc8821a1ant_action_wifi_connected_scan(
+	struct btc_coexist *btcoexist
+	)
+{
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+
+	/*  power save state */
+	halbtc8821a1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+
+	/*  tdma and coex table */
+	if (BT_8821A_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
+		if (bt_link_info->a2dp_exist && bt_link_info->pan_exist) {
+			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 22);
+			halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		} else {
+		halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 20);
+		halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+	}
+	} else if ((BT_8821A_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
+			(BT_8821A_1ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status)) {
+		halbtc8821a1ant_action_bt_sco_hid_only_busy(btcoexist,
+			BT_8821A_1ANT_WIFI_STATUS_CONNECTED_SCAN);
+	} else {
+		halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 20);
+		halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+	}
+}
+
+static void action_wifi_connected_special_packet(struct btc_coexist *btcoexist)
+{
+	bool hs_connecting = false;
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_CONNECTING, &hs_connecting);
+
+	halbtc8821a1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+
+	/*  tdma and coex table */
+	if (BT_8821A_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
+		if (bt_link_info->a2dp_exist && bt_link_info->pan_exist) {
+			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 22);
+			halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		} else {
+			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 20);
+			halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		}
+	} else {
+		halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 20);
+		halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+	}
+}
+
+static void halbtc8821a1ant_action_wifi_connected(struct btc_coexist *btcoexist)
+{
+	bool wifi_busy = false;
+	bool scan = false, link = false, roam = false;
+	bool under_4way = false;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], CoexForWifiConnect() ===>\n");
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS, &under_4way);
+	if (under_4way) {
+		action_wifi_connected_special_packet(btcoexist);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], CoexForWifiConnect(), return for wifi is under 4way<===\n");
+		return;
+	}
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
+	if (scan || link || roam) {
+		halbtc8821a1ant_action_wifi_connected_scan(btcoexist);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], CoexForWifiConnect(), return for wifi is under scan<===\n");
+		return;
+	}
+
+	/*  power save state */
+	if (BT_8821A_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status && !btcoexist->bt_link_info.hid_only)
+		halbtc8821a1ant_power_save_state(btcoexist, BTC_PS_LPS_ON, 0x50, 0x4);
+	else
+		halbtc8821a1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+
+	/*  tdma and coex table */
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+	if (!wifi_busy) {
+		if (BT_8821A_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
+			action_wifi_connected_bt_acl_busy(btcoexist,
+				BT_8821A_1ANT_WIFI_STATUS_CONNECTED_IDLE);
+		} else if ((BT_8821A_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
+			(BT_8821A_1ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status)) {
+			halbtc8821a1ant_action_bt_sco_hid_only_busy(btcoexist,
+				BT_8821A_1ANT_WIFI_STATUS_CONNECTED_IDLE);
+		} else {
+			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
+			halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+		}
+	} else {
+		if (BT_8821A_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
+			action_wifi_connected_bt_acl_busy(btcoexist,
+				BT_8821A_1ANT_WIFI_STATUS_CONNECTED_BUSY);
+		} else if ((BT_8821A_1ANT_BT_STATUS_SCO_BUSY ==
+			    coex_dm->bt_status) ||
+			   (BT_8821A_1ANT_BT_STATUS_ACL_SCO_BUSY ==
+			    coex_dm->bt_status)) {
+			halbtc8821a1ant_action_bt_sco_hid_only_busy(btcoexist,
+				BT_8821A_1ANT_WIFI_STATUS_CONNECTED_BUSY);
+		} else {
+			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 5);
+			halbtc8821a1ant_coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 2);
+		}
+	}
+}
+
+static void run_sw_coexist_mechanism(struct btc_coexist *btcoexist)
+{
+	u8 algorithm = 0;
+
+	algorithm = halbtc8821a1ant_action_algorithm(btcoexist);
+	coex_dm->cur_algorithm = algorithm;
+
+	if (!halbtc8821a1ant_is_common_action(btcoexist)) {
+		switch (coex_dm->cur_algorithm) {
+		case BT_8821A_1ANT_COEX_ALGO_SCO:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = SCO.\n");
+			halbtc8821a1ant_action_sco(btcoexist);
+			break;
+		case BT_8821A_1ANT_COEX_ALGO_HID:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = HID.\n");
+			halbtc8821a1ant_action_hid(btcoexist);
+			break;
+		case BT_8821A_1ANT_COEX_ALGO_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = A2DP.\n");
+			halbtc8821a1ant_action_a2dp(btcoexist);
+			break;
+		case BT_8821A_1ANT_COEX_ALGO_A2DP_PANHS:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = A2DP+PAN(HS).\n");
+			halbtc8821a1ant_action_a2dp_pan_hs(btcoexist);
+			break;
+		case BT_8821A_1ANT_COEX_ALGO_PANEDR:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = PAN(EDR).\n");
+			halbtc8821a1ant_action_pan_edr(btcoexist);
+			break;
+		case BT_8821A_1ANT_COEX_ALGO_PANHS:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = HS mode.\n");
+			halbtc8821a1ant_action_pan_hs(btcoexist);
+			break;
+		case BT_8821A_1ANT_COEX_ALGO_PANEDR_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = PAN+A2DP.\n");
+			halbtc8821a1ant_action_pan_edr_a2dp(btcoexist);
+			break;
+		case BT_8821A_1ANT_COEX_ALGO_PANEDR_HID:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = PAN(EDR)+HID.\n");
+			halbtc8821a1ant_action_pan_edr_hid(btcoexist);
+			break;
+		case BT_8821A_1ANT_COEX_ALGO_HID_A2DP_PANEDR:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = HID+A2DP+PAN.\n");
+			halbtc8821a1ant_action_hid_a2dp_pan_edr(btcoexist);
+			break;
+		case BT_8821A_1ANT_COEX_ALGO_HID_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = HID+A2DP.\n");
+			halbtc8821a1ant_action_hid_a2dp(btcoexist);
+			break;
+		default:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action algorithm = coexist All Off!!\n");
+			break;
+		}
+		coex_dm->pre_algorithm = coex_dm->cur_algorithm;
+	}
+}
+
+static void halbtc8821a1ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
+{
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	bool wifi_connected = false, bt_hs_on = false;
+	bool increase_scan_dev_num = false;
+	bool bt_ctrl_agg_buf_size = false;
+	u8 agg_buf_size = 5;
+	u8 wifi_rssi_state = BTC_RSSI_STATE_HIGH;
+	bool wifi_under_5g = false;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+		  "[BTCoex], RunCoexistMechanism() ===>\n");
+
+	if (btcoexist->manual_control) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], RunCoexistMechanism(), return for Manual CTRL <===\n");
+		return;
+	}
+
+	if (btcoexist->stop_coex_dm) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], RunCoexistMechanism(), return for Stop Coex DM <===\n");
+		return;
+	}
+
+	if (coex_sta->under_ips) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], wifi is under IPS !!!\n");
+		return;
+	}
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G,
+			   &wifi_under_5g);
+	if (wifi_under_5g) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], RunCoexistMechanism(), return for 5G <===\n");
+		halbtc8821a1ant_coex_under_5g(btcoexist);
+		return;
+	}
+
+	if ((BT_8821A_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) ||
+	    (BT_8821A_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
+	    (BT_8821A_1ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status)) {
+		increase_scan_dev_num = true;
+	}
+
+	btcoexist->btc_set(btcoexist, BTC_SET_BL_INC_SCAN_DEV_NUM,
+			   &increase_scan_dev_num);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+			   &wifi_connected);
+
+	if (!bt_link_info->sco_exist && !bt_link_info->hid_exist) {
+		halbtc8821a1ant_limited_tx(btcoexist, NORMAL_EXEC, 0, 0, 0, 0);
+	} else {
+		if (wifi_connected) {
+			wifi_rssi_state = Wifi_rssi_state(btcoexist, 1, 2,
+							  30, 0);
+			if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+				halbtc8821a1ant_limited_tx(btcoexist,
+							   NORMAL_EXEC, 1, 1,
+							   1, 1);
+			else
+				halbtc8821a1ant_limited_tx(btcoexist,
+							   NORMAL_EXEC, 1, 1,
+							   1, 1);
+		} else {
+			halbtc8821a1ant_limited_tx(btcoexist, NORMAL_EXEC,
+						   0, 0, 0, 0);
+		}
+	}
+
+	if (bt_link_info->sco_exist) {
+		bt_ctrl_agg_buf_size = true;
+		agg_buf_size = 0x3;
+	} else if (bt_link_info->hid_exist) {
+		bt_ctrl_agg_buf_size = true;
+		agg_buf_size = 0x5;
+	} else if (bt_link_info->a2dp_exist || bt_link_info->pan_exist) {
+		bt_ctrl_agg_buf_size = true;
+		agg_buf_size = 0x8;
+	}
+	halbtc8821a1ant_limited_rx(btcoexist, NORMAL_EXEC, false,
+				   bt_ctrl_agg_buf_size, agg_buf_size);
+
+	run_sw_coexist_mechanism(btcoexist);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	if (coex_sta->c2h_bt_inquiry_page) {
+		halbtc8821a1ant_action_bt_inquiry(btcoexist);
+		return;
+	} else if (bt_hs_on) {
+		halbtc8821a1ant_action_hs(btcoexist);
+		return;
+	}
+
+	if (!wifi_connected) {
+		bool scan = false, link = false, roam = false;
+
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], wifi is non connected-idle !!!\n");
+
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
+
+		if (scan || link || roam)
+			wifi_not_connected_asso_auth_scan(btcoexist);
+		else
+			halbtc8821a1ant_action_wifi_not_connected(btcoexist);
+	} else {
+		/*  wifi LPS/Busy */
+		halbtc8821a1ant_action_wifi_connected(btcoexist);
+	}
+}
+
+static void halbtc8821a1ant_init_coex_dm(struct btc_coexist *btcoexist)
+{
+	/*  force to reset coex mechanism */
+	/*  sw all off */
+	halbtc8821a1ant_sw_mechanism(btcoexist, false);
+
+	halbtc8821a1ant_ps_tdma(btcoexist, FORCE_EXEC, false, 8);
+	halbtc8821a1ant_coex_table_with_type(btcoexist, FORCE_EXEC, 0);
+}
+
+static void halbtc8821a1ant_init_hw_config(struct btc_coexist *btcoexist,
+					   bool back_up)
+{
+	u8 u1_tmp = 0;
+	bool wifi_under_5g = false;
+
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+		  "[BTCoex], 1Ant Init HW Config!!\n");
+
+	if (back_up) {
+		coex_dm->backup_arfr_cnt1 =
+			btcoexist->btc_read_4byte(btcoexist, 0x430);
+		coex_dm->backup_arfr_cnt2 =
+			btcoexist->btc_read_4byte(btcoexist, 0x434);
+		coex_dm->backup_retry_limit =
+			btcoexist->btc_read_2byte(btcoexist, 0x42a);
+		coex_dm->backup_ampdu_max_time =
+			btcoexist->btc_read_1byte(btcoexist, 0x456);
+	}
+
+	/*  0x790[5:0] = 0x5 */
+	u1_tmp = btcoexist->btc_read_1byte(btcoexist, 0x790);
+	u1_tmp &= 0xc0;
+	u1_tmp |= 0x5;
+	btcoexist->btc_write_1byte(btcoexist, 0x790, u1_tmp);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under_5g);
+
+	/* Antenna config */
+	if (wifi_under_5g)
+		halbtc8821a1ant_set_ant_path(btcoexist, BTC_ANT_PATH_BT,
+					     true, false);
+	else
+		halbtc8821a1ant_set_ant_path(btcoexist, BTC_ANT_PATH_PTA,
+					     true, false);
+	/*  PTA parameter */
+	halbtc8821a1ant_coex_table_with_type(btcoexist, FORCE_EXEC, 0);
+
+	/*  Enable counter statistics */
+	/* 0x76e[3] = 1, WLAN_Act control by PTA */
+	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
+	btcoexist->btc_write_1byte(btcoexist, 0x778, 0x3);
+	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x40, 0x20, 0x1);
+}
+
+/*  */
+/*  work around function start with wa_halbtc8821a1ant_ */
+/*  */
+/*  */
+/*  extern function start with EXhalbtc8821a1ant_ */
+/*  */
+void
+ex_halbtc8821a1ant_init_hwconfig(
+	struct btc_coexist *btcoexist
+	)
+{
+	halbtc8821a1ant_init_hw_config(btcoexist, true);
+}
+
+void
+ex_halbtc8821a1ant_init_coex_dm(
+	struct btc_coexist *btcoexist
+	)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, "[BTCoex], Coex Mechanism Init!!\n");
+
+	btcoexist->stop_coex_dm = false;
+
+	halbtc8821a1ant_init_coex_dm(btcoexist);
+
+	halbtc8821a1ant_query_bt_info(btcoexist);
+}
+
+void ex_halbtc8821a1ant_display_coex_info(struct btc_coexist *btcoexist)
+{
+	struct btc_board_info *board_info = &btcoexist->board_info;
+	struct btc_stack_info *stack_info = &btcoexist->stack_info;
+	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	u8 *cli_buf = btcoexist->cli_buf;
+	u8 u1_tmp[4], i, bt_info_ext, ps_tdma_case = 0;
+	u16 u2_tmp[4];
+	u32 u4_tmp[4];
+	bool roam = false, scan = false, link = false, wifi_under_5g = false;
+	bool bt_hs_on = false, wifi_busy = false;
+	long wifi_rssi = 0, bt_hs_rssi = 0;
+	u32 wifi_bw, wifi_traffic_dir;
+	u8 wifi_dot11_chnl, wifi_hs_chnl;
+	u32 fw_ver = 0, bt_patch_ver = 0;
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\n ============[BT Coexist info] ============");
+	CL_PRINTF(cli_buf);
+
+	if (btcoexist->manual_control) {
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\n ============[Under Manual Control] ============");
+		CL_PRINTF(cli_buf);
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\n ==========================================");
+		CL_PRINTF(cli_buf);
+	}
+	if (btcoexist->stop_coex_dm) {
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\n ============[Coex is STOPPED] ============");
+		CL_PRINTF(cli_buf);
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\n ==========================================");
+		CL_PRINTF(cli_buf);
+	}
+
+	if (!board_info->bt_exist) {
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n BT not exists !!!");
+		CL_PRINTF(cli_buf);
+		return;
+	}
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d/ %d/ %d",
+		   "Ant PG Num/ Ant Mech/ Ant Pos: ",
+		   board_info->pg_ant_num, board_info->btdm_ant_num,
+		   board_info->btdm_ant_pos);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %s / %d",
+		   "BT stack/ hci ext ver",
+		   ((stack_info->profile_notified) ? "Yes" : "No"),
+		   stack_info->hci_version);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER, &bt_patch_ver);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d_%x/ 0x%x/ 0x%x(%d)",
+		   "CoexVer/ FwVer/ PatchVer",
+		   glcoex_ver_date_8821a_1ant, glcoex_ver_8821a_1ant, fw_ver,
+		   bt_patch_ver, bt_patch_ver);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_DOT11_CHNL,
+			   &wifi_dot11_chnl);
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifi_hs_chnl);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d / %d(%d)",
+		   "Dot11 channel / HsChnl(HsMode)",
+		   wifi_dot11_chnl, wifi_hs_chnl, bt_hs_on);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %02x %02x %02x ",
+		   "H2C Wifi inform bt chnl Info",
+		   coex_dm->wifi_chnl_info[0], coex_dm->wifi_chnl_info[1],
+		   coex_dm->wifi_chnl_info[2]);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
+	btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d/ %d",
+		   "Wifi rssi/ HS rssi",
+		   (int)wifi_rssi, (int)bt_hs_rssi);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d/ %d/ %d ",
+		   "Wifi link/ roam/ scan",
+		   link, roam, scan);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under_5g);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION,
+			   &wifi_traffic_dir);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %s / %s/ %s ",
+		   "Wifi status",
+		   (wifi_under_5g ? "5G" : "2.4G"),
+		   ((BTC_WIFI_BW_LEGACY == wifi_bw) ? "Legacy" :
+		    (((BTC_WIFI_BW_HT40 == wifi_bw) ? "HT40" : "HT20"))),
+		   ((!wifi_busy) ? "idle" :
+		    ((BTC_WIFI_TRAFFIC_TX == wifi_traffic_dir) ?
+		     "uplink" : "downlink")));
+	CL_PRINTF(cli_buf);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\n %-35s = [%s/ %d/ %d] ", "BT [status/ rssi/ retryCnt]",
+		   ((btcoexist->bt_info.bt_disabled) ? ("disabled") :
+		    ((coex_sta->c2h_bt_inquiry_page) ? ("inquiry/page scan") :
+		     ((BT_8821A_1ANT_BT_STATUS_NON_CONNECTED_IDLE ==
+		       coex_dm->bt_status) ? "non-connected idle" :
+		      ((BT_8821A_1ANT_BT_STATUS_CONNECTED_IDLE ==
+		       coex_dm->bt_status) ? "connected-idle" : "busy")))),
+		   coex_sta->bt_rssi, coex_sta->bt_retry_cnt);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d / %d / %d / %d",
+		   "SCO/HID/PAN/A2DP",
+		   bt_link_info->sco_exist, bt_link_info->hid_exist,
+		   bt_link_info->pan_exist, bt_link_info->a2dp_exist);
+	CL_PRINTF(cli_buf);
+	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_BT_LINK_INFO);
+
+	bt_info_ext = coex_sta->bt_info_ext;
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %s",
+		   "BT Info A2DP rate",
+		   (bt_info_ext & BIT(0)) ? "Basic rate" : "EDR rate");
+	CL_PRINTF(cli_buf);
+
+	for (i = 0; i < BT_INFO_SRC_8821A_1ANT_MAX; i++) {
+		if (coex_sta->bt_info_c2h_cnt[i]) {
+			CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+				   "\n %-35s = %02x %02x %02x %02x %02x %02x %02x(%d)",
+				   glbt_info_src_8821a_1ant[i],
+				   coex_sta->bt_info_c2h[i][0],
+				   coex_sta->bt_info_c2h[i][1],
+				   coex_sta->bt_info_c2h[i][2],
+				   coex_sta->bt_info_c2h[i][3],
+				   coex_sta->bt_info_c2h[i][4],
+				   coex_sta->bt_info_c2h[i][5],
+				   coex_sta->bt_info_c2h[i][6],
+				   coex_sta->bt_info_c2h_cnt[i]);
+			CL_PRINTF(cli_buf);
+		}
+	}
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %s/%s, (0x%x/0x%x)",
+		   "PS state, IPS/LPS, (lps/rpwm)",
+		   ((coex_sta->under_ips ? "IPS ON" : "IPS OFF")),
+		   ((coex_sta->under_lps ? "LPS ON" : "LPS OFF")),
+		   btcoexist->bt_info.lps_val,
+		   btcoexist->bt_info.rpwm_val);
+	CL_PRINTF(cli_buf);
+	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_FW_PWR_MODE_CMD);
+
+	if (!btcoexist->manual_control) {
+		/*  Sw mechanism	 */
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\n %-35s", "============[Sw mechanism] ============");
+		CL_PRINTF(cli_buf);
+
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\n %-35s = %d", "SM[LowPenaltyRA]",
+			   coex_dm->cur_low_penalty_ra);
+		CL_PRINTF(cli_buf);
+
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\n %-35s = %s/ %s/ %d ", "DelBA/ BtCtrlAgg/ AggSize",
+			   (btcoexist->bt_info.reject_agg_pkt ? "Yes" : "No"),
+			   (btcoexist->bt_info.b_bt_ctrl_buf_size ?
+			    "Yes" : "No"),
+			   btcoexist->bt_info.agg_buf_size);
+		CL_PRINTF(cli_buf);
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\n %-35s = 0x%x ", "Rate Mask",
+			   btcoexist->bt_info.ra_mask);
+		CL_PRINTF(cli_buf);
+
+		/*  Fw mechanism */
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s",
+			   "============[Fw mechanism] ============");
+		CL_PRINTF(cli_buf);
+
+		ps_tdma_case = coex_dm->cur_ps_tdma;
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\n %-35s = %02x %02x %02x %02x %02x case-%d (auto:%d)",
+			   "PS TDMA",
+			   coex_dm->ps_tdma_para[0], coex_dm->ps_tdma_para[1],
+			   coex_dm->ps_tdma_para[2], coex_dm->ps_tdma_para[3],
+			   coex_dm->ps_tdma_para[4], ps_tdma_case,
+			   coex_dm->auto_tdma_adjust);
+		CL_PRINTF(cli_buf);
+
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x ",
+			   "Latest error condition(should be 0)",
+			   coex_dm->error_condition);
+		CL_PRINTF(cli_buf);
+
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d ",
+			   "IgnWlanAct",
+			   coex_dm->cur_ignore_wlan_act);
+		CL_PRINTF(cli_buf);
+	}
+
+	/*  Hw setting		 */
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s",
+		   "============[Hw setting] ============");
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x/0x%x/0x%x/0x%x",
+		   "backup ARFR1/ARFR2/RL/AMaxTime",
+		   coex_dm->backup_arfr_cnt1, coex_dm->backup_arfr_cnt2,
+		   coex_dm->backup_retry_limit, coex_dm->backup_ampdu_max_time);
+	CL_PRINTF(cli_buf);
+
+	u4_tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x430);
+	u4_tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x434);
+	u2_tmp[0] = btcoexist->btc_read_2byte(btcoexist, 0x42a);
+	u1_tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x456);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x/0x%x/0x%x/0x%x",
+		   "0x430/0x434/0x42a/0x456",
+		   u4_tmp[0], u4_tmp[1], u2_tmp[0], u1_tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	u1_tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x778);
+	u4_tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xc58);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\n %-35s = 0x%x/ 0x%x", "0x778/ 0xc58[29:25]",
+		   u1_tmp[0], (u4_tmp[0]&0x3e000000) >> 25);
+	CL_PRINTF(cli_buf);
+
+	u1_tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x8db);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x", "0x8db[6:5]",
+		   ((u1_tmp[0]&0x60)>>5));
+	CL_PRINTF(cli_buf);
+
+	u1_tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x975);
+	u4_tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xcb4);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x/ 0x%x/ 0x%x",
+		   "0xcb4[29:28]/0xcb4[7:0]/0x974[9:8]",
+		   (u4_tmp[0]&0x30000000)>>28, u4_tmp[0]&0xff, u1_tmp[0] & 0x3);
+	CL_PRINTF(cli_buf);
+
+	u1_tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x40);
+	u4_tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x4c);
+	u1_tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x64);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x/ 0x%x/ 0x%x",
+		   "0x40/0x4c[24:23]/0x64[0]",
+		   u1_tmp[0], ((u4_tmp[0]&0x01800000)>>23), u1_tmp[1]&0x1);
+	CL_PRINTF(cli_buf);
+
+	u4_tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x550);
+	u1_tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x522);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x/ 0x%x",
+		   "0x550(bcn ctrl)/0x522",
+		   u4_tmp[0], u1_tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	u4_tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xc50);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x", "0xc50(dig)",
+		   u4_tmp[0]&0xff);
+	CL_PRINTF(cli_buf);
+
+	u4_tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xf48);
+	u1_tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0xa5d);
+	u1_tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0xa5c);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x/ 0x%x",
+		   "OFDM-FA/ CCK-FA",
+		   u4_tmp[0], (u1_tmp[0]<<8) + u1_tmp[1]);
+	CL_PRINTF(cli_buf);
+
+	u4_tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x6c0);
+	u4_tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x6c4);
+	u4_tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x6c8);
+	u1_tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x6cc);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x",
+		   "0x6c0/0x6c4/0x6c8/0x6cc(coexTable)",
+		   u4_tmp[0], u4_tmp[1], u4_tmp[2], u1_tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d/ %d",
+		   "0x770(high-pri rx/tx)",
+		   coex_sta->high_priority_rx, coex_sta->high_priority_tx);
+	CL_PRINTF(cli_buf);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d/ %d",
+		   "0x774(low-pri rx/tx)",
+		   coex_sta->low_priority_rx, coex_sta->low_priority_tx);
+	CL_PRINTF(cli_buf);
+#if (BT_AUTO_REPORT_ONLY_8821A_1ANT == 1)
+	halbtc8821a1ant_monitor_bt_ctr(btcoexist);
+#endif
+	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_COEX_STATISTICS);
+}
+
+void ex_halbtc8821a1ant_ips_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	if (btcoexist->manual_control || btcoexist->stop_coex_dm)
+		return;
+
+	if (BTC_IPS_ENTER == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], IPS ENTER notify\n");
+		coex_sta->under_ips = true;
+		halbtc8821a1ant_set_ant_path(btcoexist, BTC_ANT_PATH_BT,
+					     false, true);
+		/* set PTA control */
+		halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
+		halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
+	} else if (BTC_IPS_LEAVE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], IPS LEAVE notify\n");
+		coex_sta->under_ips = false;
+
+		halbtc8821a1ant_run_coexist_mechanism(btcoexist);
+	}
+}
+
+void ex_halbtc8821a1ant_lps_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	if (btcoexist->manual_control || btcoexist->stop_coex_dm)
+		return;
+
+	if (BTC_LPS_ENABLE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], LPS ENABLE notify\n");
+		coex_sta->under_lps = true;
+	} else if (BTC_LPS_DISABLE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], LPS DISABLE notify\n");
+		coex_sta->under_lps = false;
+	}
+}
+
+void ex_halbtc8821a1ant_scan_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	bool wifi_connected = false, bt_hs_on = false;
+
+	if (btcoexist->manual_control ||
+	    btcoexist->stop_coex_dm ||
+	    btcoexist->bt_info.bt_disabled)
+		return;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+			   &wifi_connected);
+
+	halbtc8821a1ant_query_bt_info(btcoexist);
+
+	if (coex_sta->c2h_bt_inquiry_page) {
+		halbtc8821a1ant_action_bt_inquiry(btcoexist);
+		return;
+	} else if (bt_hs_on) {
+		halbtc8821a1ant_action_hs(btcoexist);
+		return;
+	}
+
+	if (BTC_SCAN_START == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], SCAN START notify\n");
+		if (!wifi_connected) {
+			/*  non-connected scan */
+			wifi_not_connected_asso_auth_scan(btcoexist);
+		} else {
+			/*  wifi is connected */
+			halbtc8821a1ant_action_wifi_connected_scan(btcoexist);
+		}
+	} else if (BTC_SCAN_FINISH == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], SCAN FINISH notify\n");
+		if (!wifi_connected)	/*  non-connected scan */
+			halbtc8821a1ant_action_wifi_not_connected(btcoexist);
+		else
+			halbtc8821a1ant_action_wifi_connected(btcoexist);
+	}
+}
+
+void ex_halbtc8821a1ant_connect_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	bool wifi_connected = false, bt_hs_on = false;
+
+	if (btcoexist->manual_control ||
+	    btcoexist->stop_coex_dm ||
+	    btcoexist->bt_info.bt_disabled)
+		return;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	if (coex_sta->c2h_bt_inquiry_page) {
+		halbtc8821a1ant_action_bt_inquiry(btcoexist);
+		return;
+	} else if (bt_hs_on) {
+		halbtc8821a1ant_action_hs(btcoexist);
+		return;
+	}
+
+	if (BTC_ASSOCIATE_START == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], CONNECT START notify\n");
+		wifi_not_connected_asso_auth_scan(btcoexist);
+	} else if (BTC_ASSOCIATE_FINISH == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], CONNECT FINISH notify\n");
+
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+				   &wifi_connected);
+		if (!wifi_connected) /*  non-connected scan */
+			halbtc8821a1ant_action_wifi_not_connected(btcoexist);
+		else
+			halbtc8821a1ant_action_wifi_connected(btcoexist);
+	}
+}
+
+void ex_halbtc8821a1ant_media_status_notify(struct btc_coexist *btcoexist,
+					    u8 type)
+{
+	u8 h2c_parameter[3] = {0};
+	u32 wifi_bw;
+	u8 wifi_central_chnl;
+
+	if (btcoexist->manual_control ||
+	    btcoexist->stop_coex_dm ||
+	    btcoexist->bt_info.bt_disabled)
+		return;
+
+	if (BTC_MEDIA_CONNECT == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], MEDIA connect notify\n");
+	else
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], MEDIA disconnect notify\n");
+
+	/*  only 2.4G we need to inform bt the chnl mask */
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_CENTRAL_CHNL,
+			   &wifi_central_chnl);
+	if ((BTC_MEDIA_CONNECT == type) &&
+	    (wifi_central_chnl <= 14)) {
+		/* h2c_parameter[0] = 0x1; */
+		h2c_parameter[0] = 0x0;
+		h2c_parameter[1] = wifi_central_chnl;
+		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+		if (BTC_WIFI_BW_HT40 == wifi_bw)
+			h2c_parameter[2] = 0x30;
+		else
+			h2c_parameter[2] = 0x20;
+	}
+
+	coex_dm->wifi_chnl_info[0] = h2c_parameter[0];
+	coex_dm->wifi_chnl_info[1] = h2c_parameter[1];
+	coex_dm->wifi_chnl_info[2] = h2c_parameter[2];
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], FW write 0x66 = 0x%x\n",
+		  h2c_parameter[0]<<16|h2c_parameter[1]<<8|h2c_parameter[2]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x66, 3, h2c_parameter);
+}
+
+void ex_halbtc8821a1ant_special_packet_notify(struct btc_coexist *btcoexist,
+					      u8 type)
+{
+	bool bt_hs_on = false;
+
+	if (btcoexist->manual_control ||
+	    btcoexist->stop_coex_dm ||
+	    btcoexist->bt_info.bt_disabled)
+		return;
+
+	coex_sta->special_pkt_period_cnt = 0;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	if (coex_sta->c2h_bt_inquiry_page) {
+		halbtc8821a1ant_action_bt_inquiry(btcoexist);
+		return;
+	} else if (bt_hs_on) {
+		halbtc8821a1ant_action_hs(btcoexist);
+		return;
+	}
+
+	if (BTC_PACKET_DHCP == type ||
+	    BTC_PACKET_EAPOL == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], special Packet(%d) notify\n", type);
+		action_wifi_connected_special_packet(btcoexist);
+	}
+}
+
+void ex_halbtc8821a1ant_bt_info_notify(struct btc_coexist *btcoexist,
+				       u8 *tmp_buf, u8 length)
+{
+	u8 bt_info = 0;
+	u8 i, rsp_source = 0;
+	bool wifi_connected = false;
+	bool bt_busy = false;
+	bool wifi_under_5g = false;
+
+	coex_sta->c2h_bt_info_req_sent = false;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under_5g);
+
+	rsp_source = tmp_buf[0]&0xf;
+	if (rsp_source >= BT_INFO_SRC_8821A_1ANT_MAX)
+		rsp_source = BT_INFO_SRC_8821A_1ANT_WIFI_FW;
+	coex_sta->bt_info_c2h_cnt[rsp_source]++;
+
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+		  "[BTCoex], Bt info[%d], length =%d, hex data =[",
+		  rsp_source, length);
+	for (i = 0; i < length; i++) {
+		coex_sta->bt_info_c2h[rsp_source][i] = tmp_buf[i];
+		if (i == 1)
+			bt_info = tmp_buf[i];
+		if (i == length-1)
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+				  "0x%02x]\n", tmp_buf[i]);
+		else
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+				  "0x%02x, ", tmp_buf[i]);
+	}
+
+	if (BT_INFO_SRC_8821A_1ANT_WIFI_FW != rsp_source) {
+		coex_sta->bt_retry_cnt =	/*  [3:0] */
+			coex_sta->bt_info_c2h[rsp_source][2]&0xf;
+
+		coex_sta->bt_rssi =
+			coex_sta->bt_info_c2h[rsp_source][3]*2+10;
+
+		coex_sta->bt_info_ext =
+			coex_sta->bt_info_c2h[rsp_source][4];
+
+		/*  Here we need to resend some wifi info to BT */
+		/*  because bt is reset and loss of the info. */
+		if (coex_sta->bt_info_ext & BIT(1)) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], BT ext info bit1 check, send wifi BW&Chnl to BT!!\n");
+			btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+					   &wifi_connected);
+			if (wifi_connected)
+				ex_halbtc8821a1ant_media_status_notify(btcoexist,
+								       BTC_MEDIA_CONNECT);
+			else
+				ex_halbtc8821a1ant_media_status_notify(btcoexist,
+								       BTC_MEDIA_DISCONNECT);
+		}
+
+		if ((coex_sta->bt_info_ext & BIT(3)) && !wifi_under_5g) {
+			if (!btcoexist->manual_control &&
+			    !btcoexist->stop_coex_dm) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], BT ext info bit3 check, set BT NOT to ignore Wlan active!!\n");
+				halbtc8821a1ant_ignore_wlan_act(btcoexist,
+								FORCE_EXEC, false);
+			}
+		} else {
+			/*  BT already NOT ignore Wlan active, do nothing here. */
+		}
+#if (BT_AUTO_REPORT_ONLY_8821A_1ANT == 0)
+		if ((coex_sta->bt_info_ext & BIT(4))) {
+			/*  BT auto report already enabled, do nothing */
+		} else {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], BT ext info bit4 check, set BT to enable Auto Report!!\n");
+			halbtc8821a1ant_bt_auto_report(btcoexist,
+						       FORCE_EXEC, true);
+		}
+#endif
+	}
+
+	/*  check BIT(2) first ==> check if bt is under inquiry or page scan */
+	if (bt_info & BT_INFO_8821A_1ANT_B_INQ_PAGE)
+		coex_sta->c2h_bt_inquiry_page = true;
+	else
+		coex_sta->c2h_bt_inquiry_page = false;
+
+	/*  set link exist status */
+	if (!(bt_info&BT_INFO_8821A_1ANT_B_CONNECTION)) {
+		coex_sta->bt_link_exist = false;
+		coex_sta->pan_exist = false;
+		coex_sta->a2dp_exist = false;
+		coex_sta->hid_exist = false;
+		coex_sta->sco_exist = false;
+	} else {
+		/*  connection exists */
+		coex_sta->bt_link_exist = true;
+		if (bt_info & BT_INFO_8821A_1ANT_B_FTP)
+			coex_sta->pan_exist = true;
+		else
+			coex_sta->pan_exist = false;
+		if (bt_info & BT_INFO_8821A_1ANT_B_A2DP)
+			coex_sta->a2dp_exist = true;
+		else
+			coex_sta->a2dp_exist = false;
+		if (bt_info & BT_INFO_8821A_1ANT_B_HID)
+			coex_sta->hid_exist = true;
+		else
+			coex_sta->hid_exist = false;
+		if (bt_info & BT_INFO_8821A_1ANT_B_SCO_ESCO)
+			coex_sta->sco_exist = true;
+		else
+			coex_sta->sco_exist = false;
+	}
+
+	halbtc8821a1ant_update_bt_link_info(btcoexist);
+
+	if (!(bt_info&BT_INFO_8821A_1ANT_B_CONNECTION)) {
+		coex_dm->bt_status = BT_8821A_1ANT_BT_STATUS_NON_CONNECTED_IDLE;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), BT Non-Connected idle!!!\n");
+	} else if (bt_info == BT_INFO_8821A_1ANT_B_CONNECTION) {
+		/*  connection exists but not busy */
+		coex_dm->bt_status = BT_8821A_1ANT_BT_STATUS_CONNECTED_IDLE;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), BT Connected-idle!!!\n");
+	} else if ((bt_info&BT_INFO_8821A_1ANT_B_SCO_ESCO) ||
+		   (bt_info&BT_INFO_8821A_1ANT_B_SCO_BUSY)) {
+		coex_dm->bt_status = BT_8821A_1ANT_BT_STATUS_SCO_BUSY;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), BT SCO busy!!!\n");
+	} else if (bt_info&BT_INFO_8821A_1ANT_B_ACL_BUSY) {
+		if (BT_8821A_1ANT_BT_STATUS_ACL_BUSY != coex_dm->bt_status)
+			coex_dm->auto_tdma_adjust = false;
+		coex_dm->bt_status = BT_8821A_1ANT_BT_STATUS_ACL_BUSY;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), BT ACL busy!!!\n");
+	} else {
+		coex_dm->bt_status = BT_8821A_1ANT_BT_STATUS_MAX;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BtInfoNotify(), BT Non-Defined state!!!\n");
+	}
+
+	if ((BT_8821A_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) ||
+	    (BT_8821A_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
+	    (BT_8821A_1ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status))
+		bt_busy = true;
+	else
+		bt_busy = false;
+	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bt_busy);
+
+	halbtc8821a1ant_run_coexist_mechanism(btcoexist);
+}
+
+void ex_halbtc8821a1ant_halt_notify(struct btc_coexist *btcoexist)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, "[BTCoex], Halt notify\n");
+
+	btcoexist->stop_coex_dm = true;
+
+	halbtc8821a1ant_set_ant_path(btcoexist, BTC_ANT_PATH_BT, false, true);
+	halbtc8821a1ant_ignore_wlan_act(btcoexist, FORCE_EXEC, true);
+
+	halbtc8821a1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+					 0x0, 0x0);
+	halbtc8821a1ant_ps_tdma(btcoexist, FORCE_EXEC, false, 0);
+
+	ex_halbtc8821a1ant_media_status_notify(btcoexist, BTC_MEDIA_DISCONNECT);
+}
+
+void ex_halbtc8821a1ant_pnp_notify(struct btc_coexist *btcoexist, u8 pnp_state)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, "[BTCoex], Pnp notify\n");
+
+	if (BTC_WIFI_PNP_SLEEP == pnp_state) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], Pnp notify to SLEEP\n");
+		btcoexist->stop_coex_dm = true;
+		halbtc8821a1ant_ignore_wlan_act(btcoexist, FORCE_EXEC, true);
+		halbtc8821a1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+						 0x0, 0x0);
+		halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 9);
+	} else if (BTC_WIFI_PNP_WAKE_UP == pnp_state) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], Pnp notify to WAKE UP\n");
+		btcoexist->stop_coex_dm = false;
+		halbtc8821a1ant_init_hw_config(btcoexist, false);
+		halbtc8821a1ant_init_coex_dm(btcoexist);
+		halbtc8821a1ant_query_bt_info(btcoexist);
+	}
+}
+
+void ex_halbtc8821a1ant_periodical(struct btc_coexist *btcoexist)
+{
+	static u8 dis_ver_info_cnt;
+	u32 fw_ver = 0, bt_patch_ver = 0;
+	struct btc_board_info *board_info = &btcoexist->board_info;
+	struct btc_stack_info *stack_info = &btcoexist->stack_info;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+		  "[BTCoex], ========================== Periodical ===========================\n");
+
+	if (dis_ver_info_cnt <= 5) {
+		dis_ver_info_cnt += 1;
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], ****************************************************************\n");
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], Ant PG Num/ Ant Mech/ Ant Pos = %d/ %d/ %d\n",
+			  board_info->pg_ant_num, board_info->btdm_ant_num,
+			  board_info->btdm_ant_pos);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], BT stack/ hci ext ver = %s / %d\n",
+			  ((stack_info->profile_notified) ? "Yes" : "No"),
+			  stack_info->hci_version);
+		btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER,
+				   &bt_patch_ver);
+		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], CoexVer/ FwVer/ PatchVer = %d_%x/ 0x%x/ 0x%x(%d)\n",
+			  glcoex_ver_date_8821a_1ant, glcoex_ver_8821a_1ant,
+			  fw_ver, bt_patch_ver, bt_patch_ver);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], ****************************************************************\n");
+	}
+
+#if (BT_AUTO_REPORT_ONLY_8821A_1ANT == 0)
+	halbtc8821a1ant_query_bt_info(btcoexist);
+	halbtc8821a1ant_monitor_bt_ctr(btcoexist);
+	halbtc8821a1ant_monitor_bt_enable_disable(btcoexist);
+#else
+	if (halbtc8821a1ant_Is_wifi_status_changed(btcoexist) ||
+	    coex_dm->auto_tdma_adjust) {
+		if (coex_sta->special_pkt_period_cnt > 2)
+			halbtc8821a1ant_run_coexist_mechanism(btcoexist);
+	}
+
+	coex_sta->special_pkt_period_cnt++;
+#endif
+}
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbtc8821a1ant.h b/drivers/staging/rtl8192ee/btcoexist/halbtc8821a1ant.h
new file mode 100644
index 00000000000..9b991d021a8
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/halbtc8821a1ant.h
@@ -0,0 +1,158 @@
+/*  */
+/*  The following is for 8821A 1ANT BT Co-exist definition */
+/*  */
+#define	BT_AUTO_REPORT_ONLY_8821A_1ANT				0
+
+#define	BT_INFO_8821A_1ANT_B_FTP				BIT(7)
+#define	BT_INFO_8821A_1ANT_B_A2DP				BIT(6)
+#define	BT_INFO_8821A_1ANT_B_HID				BIT(5)
+#define	BT_INFO_8821A_1ANT_B_SCO_BUSY				BIT(4)
+#define	BT_INFO_8821A_1ANT_B_ACL_BUSY				BIT(3)
+#define	BT_INFO_8821A_1ANT_B_INQ_PAGE				BIT(2)
+#define	BT_INFO_8821A_1ANT_B_SCO_ESCO				BIT(1)
+#define	BT_INFO_8821A_1ANT_B_CONNECTION				BIT(0)
+
+#define	BT_INFO_8821A_1ANT_A2DP_BASIC_RATE(_BT_INFO_EXT_)	\
+		(((_BT_INFO_EXT_&BIT(0))) ? true : false)
+
+#define	BTC_RSSI_COEX_THRESH_TOL_8821A_1ANT		2
+
+enum BT_INFO_SRC_8821A_1ANT {
+	BT_INFO_SRC_8821A_1ANT_WIFI_FW			= 0x0,
+	BT_INFO_SRC_8821A_1ANT_BT_RSP			= 0x1,
+	BT_INFO_SRC_8821A_1ANT_BT_ACTIVE_SEND		= 0x2,
+	BT_INFO_SRC_8821A_1ANT_MAX
+};
+
+enum BT_8821A_1ANT_BT_STATUS {
+	BT_8821A_1ANT_BT_STATUS_NON_CONNECTED_IDLE	= 0x0,
+	BT_8821A_1ANT_BT_STATUS_CONNECTED_IDLE		= 0x1,
+	BT_8821A_1ANT_BT_STATUS_INQ_PAGE		= 0x2,
+	BT_8821A_1ANT_BT_STATUS_ACL_BUSY		= 0x3,
+	BT_8821A_1ANT_BT_STATUS_SCO_BUSY		= 0x4,
+	BT_8821A_1ANT_BT_STATUS_ACL_SCO_BUSY		= 0x5,
+	BT_8821A_1ANT_BT_STATUS_MAX
+};
+
+enum BT_8821A_1ANT_WIFI_STATUS {
+	BT_8821A_1ANT_WIFI_STATUS_NON_CONNECTED_IDLE			= 0x0,
+	BT_8821A_1ANT_WIFI_STATUS_NON_CONNECTED_ASSO_AUTH_SCAN		= 0x1,
+	BT_8821A_1ANT_WIFI_STATUS_CONNECTED_SCAN			= 0x2,
+	BT_8821A_1ANT_WIFI_STATUS_CONNECTED_SPECIAL_PKT			= 0x3,
+	BT_8821A_1ANT_WIFI_STATUS_CONNECTED_IDLE			= 0x4,
+	BT_8821A_1ANT_WIFI_STATUS_CONNECTED_BUSY			= 0x5,
+	BT_8821A_1ANT_WIFI_STATUS_MAX
+};
+
+enum BT_8821A_1ANT_COEX_ALGO {
+	BT_8821A_1ANT_COEX_ALGO_UNDEFINED		= 0x0,
+	BT_8821A_1ANT_COEX_ALGO_SCO			= 0x1,
+	BT_8821A_1ANT_COEX_ALGO_HID			= 0x2,
+	BT_8821A_1ANT_COEX_ALGO_A2DP			= 0x3,
+	BT_8821A_1ANT_COEX_ALGO_A2DP_PANHS		= 0x4,
+	BT_8821A_1ANT_COEX_ALGO_PANEDR			= 0x5,
+	BT_8821A_1ANT_COEX_ALGO_PANHS			= 0x6,
+	BT_8821A_1ANT_COEX_ALGO_PANEDR_A2DP		= 0x7,
+	BT_8821A_1ANT_COEX_ALGO_PANEDR_HID		= 0x8,
+	BT_8821A_1ANT_COEX_ALGO_HID_A2DP_PANEDR		= 0x9,
+	BT_8821A_1ANT_COEX_ALGO_HID_A2DP		= 0xa,
+	BT_8821A_1ANT_COEX_ALGO_MAX			= 0xb,
+};
+
+struct coex_dm_8821a_1ant {
+	/*  fw mechanism */
+	bool		cur_ignore_wlan_act;
+	bool		pre_ignore_wlan_act;
+	u8		pre_ps_tdma;
+	u8		cur_ps_tdma;
+	u8		ps_tdma_para[5];
+	u8		ps_tdma_du_adj_type;
+	bool		auto_tdma_adjust;
+	bool		pre_ps_tdma_on;
+	bool		cur_ps_tdma_on;
+	bool		pre_bt_auto_report;
+	bool		cur_bt_auto_report;
+	u8		pre_lps;
+	u8		cur_lps;
+	u8		pre_rpwm;
+	u8		cur_rpwm;
+
+	/*  sw mechanism */
+	bool	pre_low_penalty_ra;
+	bool		cur_low_penalty_ra;
+	u32		pre_val_0x6c0;
+	u32		cur_val_0x6c0;
+	u32		pre_val_0x6c4;
+	u32		cur_val_0x6c4;
+	u32		pre_val_0x6c8;
+	u32		cur_val_0x6c8;
+	u8		pre_val_0x6cc;
+	u8		cur_val_0x6cc;
+
+	u32		backup_arfr_cnt1; /*  Auto Rate Fallback Retry cnt */
+	u32		backup_arfr_cnt2; /*  Auto Rate Fallback Retry cnt */
+	u16		backup_retry_limit;
+	u8		backup_ampdu_max_time;
+
+	/*  algorithm related */
+	u8		pre_algorithm;
+	u8		cur_algorithm;
+	u8		bt_status;
+	u8		wifi_chnl_info[3];
+
+	u32		pre_ra_mask;
+	u32		cur_ra_mask;
+	u8		pre_arfr_type;
+	u8		cur_arfr_type;
+	u8		pre_retry_limit_type;
+	u8		cur_retry_limit_type;
+	u8		pre_ampdu_time_type;
+	u8		cur_ampdu_time_type;
+
+	u8		error_condition;
+};
+
+struct coex_sta_8821a_1ant {
+	bool		bt_link_exist;
+	bool		sco_exist;
+	bool		a2dp_exist;
+	bool		hid_exist;
+	bool		pan_exist;
+
+	bool		under_lps;
+	bool		under_ips;
+	u32		special_pkt_period_cnt;
+	u32		high_priority_tx;
+	u32		high_priority_rx;
+	u32		low_priority_tx;
+	u32		low_priority_rx;
+	u8		bt_rssi;
+	u8		pre_bt_rssi_state;
+	u8		pre_wifi_rssi_state[4];
+	bool		c2h_bt_info_req_sent;
+	u8		bt_info_c2h[BT_INFO_SRC_8821A_1ANT_MAX][10];
+	u32		bt_info_c2h_cnt[BT_INFO_SRC_8821A_1ANT_MAX];
+	bool		c2h_bt_inquiry_page;
+	u8		bt_retry_cnt;
+	u8		bt_info_ext;
+};
+
+/*  The following is interface which will notify coex module. */
+void ex_halbtc8821a1ant_init_hwconfig(struct btc_coexist *btcoexist);
+void ex_halbtc8821a1ant_init_coex_dm(struct btc_coexist *btcoexist);
+void ex_halbtc8821a1ant_ips_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_halbtc8821a1ant_lps_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_halbtc8821a1ant_scan_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_halbtc8821a1ant_connect_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_halbtc8821a1ant_media_status_notify(struct btc_coexist *btcoexist,
+					    u8 type);
+void ex_halbtc8821a1ant_special_packet_notify(struct btc_coexist *btcoexist,
+					      u8 type);
+void ex_halbtc8821a1ant_bt_info_notify(struct btc_coexist *btcoexist,
+				       u8 *tmpbuf, u8 length);
+void ex_halbtc8821a1ant_halt_notify(struct btc_coexist *btcoexist);
+void ex_halbtc8821a1ant_pnp_notify(struct btc_coexist *btcoexist, u8 pnpstate);
+void ex_halbtc8821a1ant_periodical(struct btc_coexist *btcoexist);
+void ex_halbtc8821a1ant_display_coex_info(struct btc_coexist *btcoexist);
+void ex_halbtc8821a1ant_dbg_control(struct btc_coexist *btcoexist, u8 op_code,
+				    u8 op_len, u8 *data);
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbtc8821a2ant.c b/drivers/staging/rtl8192ee/btcoexist/halbtc8821a2ant.c
new file mode 100644
index 00000000000..7fb59073901
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/halbtc8821a2ant.c
@@ -0,0 +1,3438 @@
+/*  Description: */
+/*  This file is for RTL8821A Co-exist mechanism */
+/*  History */
+/*  2012/08/22 Cosa first check in. */
+/*  2012/11/14 Cosa Revise for 8821A 2Ant out sourcing. */
+
+/*  include files */
+#include "halbt_precomp.h"
+/*  Global variables, these are static variables */
+static struct coex_dm_8821a_2ant	glcoex_dm_8821a_2ant;
+static struct coex_dm_8821a_2ant	*coex_dm = &glcoex_dm_8821a_2ant;
+static struct coex_sta_8821a_2ant	glcoex_sta_8821a_2ant;
+static struct coex_sta_8821a_2ant	*coex_sta = &glcoex_sta_8821a_2ant;
+
+static const char *const glbt_info_src_8821a_2ant[] = {
+	"BT Info[wifi fw]",
+	"BT Info[bt rsp]",
+	"BT Info[bt auto report]",
+};
+
+static u32 glcoex_ver_date_8821a_2ant = 20130618;
+static u32 glcoex_ver_8821a_2ant = 0x5050;
+
+/*  local function proto type if needed */
+/*  local function start with halbtc8821a2ant_ */
+static u8 halbtc8821a2ant_bt_rssi_state(u8 level_num, u8 rssi_thresh,
+					u8 rssi_thresh1)
+{
+	long bt_rssi = 0;
+	u8 bt_rssi_state = coex_sta->pre_bt_rssi_state;
+
+	bt_rssi = coex_sta->bt_rssi;
+
+	if (level_num == 2) {
+		if ((coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_LOW) ||
+		    (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+			if (bt_rssi >= (rssi_thresh +
+			    BTC_RSSI_COEX_THRESH_TOL_8821A_2ANT)) {
+				bt_rssi_state = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state switch to High\n");
+			} else {
+				bt_rssi_state = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state stay at Low\n");
+			}
+		} else {
+			if (bt_rssi < rssi_thresh) {
+				bt_rssi_state = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state switch to Low\n");
+			} else {
+				bt_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state stay at High\n");
+			}
+		}
+	} else if (level_num == 3) {
+		if (rssi_thresh > rssi_thresh1) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+				  "[BTCoex], BT Rssi thresh error!!\n");
+			return coex_sta->pre_bt_rssi_state;
+		}
+
+		if ((coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_LOW) ||
+		    (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW)) {
+			if (bt_rssi >= (rssi_thresh +
+			    BTC_RSSI_COEX_THRESH_TOL_8821A_2ANT)) {
+				bt_rssi_state = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state switch to Medium\n");
+			} else {
+				bt_rssi_state = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state stay at Low\n");
+			}
+		} else if ((coex_sta->pre_bt_rssi_state ==
+			    BTC_RSSI_STATE_MEDIUM) ||
+			   (coex_sta->pre_bt_rssi_state ==
+			    BTC_RSSI_STATE_STAY_MEDIUM)) {
+			if (bt_rssi >= (rssi_thresh1 +
+			    BTC_RSSI_COEX_THRESH_TOL_8821A_2ANT)) {
+				bt_rssi_state = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state switch to High\n");
+			} else if (bt_rssi < rssi_thresh) {
+				bt_rssi_state = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state switch to Low\n");
+			} else {
+				bt_rssi_state = BTC_RSSI_STATE_STAY_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state stay at Medium\n");
+			}
+		} else {
+			if (bt_rssi < rssi_thresh1) {
+				bt_rssi_state = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state switch to Medium\n");
+			} else {
+				bt_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_BT_RSSI_STATE,
+					  "[BTCoex], BT Rssi state stay at High\n");
+			}
+		}
+	}
+
+	coex_sta->pre_bt_rssi_state = bt_rssi_state;
+
+	return bt_rssi_state;
+}
+
+static u8 wifi21a_rssi_state(struct btc_coexist *btcoexist,
+			     u8 index, u8 level_num,
+			     u8 rssi_thresh, u8 rssi_thresh1)
+{
+	long	wifi_rssi = 0;
+	u8 wifi_rssi_state = coex_sta->pre_wifi_rssi_state[index];
+
+	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
+
+	if (level_num == 2) {
+		if ((coex_sta->pre_wifi_rssi_state[index] ==
+		     BTC_RSSI_STATE_LOW) ||
+		    (coex_sta->pre_wifi_rssi_state[index] ==
+		     BTC_RSSI_STATE_STAY_LOW)) {
+			if (wifi_rssi >= (rssi_thresh +
+			    BTC_RSSI_COEX_THRESH_TOL_8821A_2ANT)) {
+				wifi_rssi_state = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state switch to High\n");
+			} else {
+				wifi_rssi_state = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state stay at Low\n");
+			}
+		} else {
+			if (wifi_rssi < rssi_thresh) {
+				wifi_rssi_state = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state switch to Low\n");
+			} else {
+				wifi_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state stay at High\n");
+			}
+		}
+	} else if (level_num == 3) {
+		if (rssi_thresh > rssi_thresh1) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE,
+				  "[BTCoex], wifi RSSI thresh error!!\n");
+			return coex_sta->pre_wifi_rssi_state[index];
+		}
+
+		if ((coex_sta->pre_wifi_rssi_state[index] ==
+		    BTC_RSSI_STATE_LOW) ||
+		    (coex_sta->pre_wifi_rssi_state[index] ==
+		     BTC_RSSI_STATE_STAY_LOW)) {
+			if (wifi_rssi >= (rssi_thresh +
+			    BTC_RSSI_COEX_THRESH_TOL_8821A_2ANT)) {
+				wifi_rssi_state = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state switch to Medium\n");
+			} else {
+				wifi_rssi_state = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state stay at Low\n");
+			}
+		} else if ((coex_sta->pre_wifi_rssi_state[index] ==
+			    BTC_RSSI_STATE_MEDIUM) ||
+			   (coex_sta->pre_wifi_rssi_state[index] ==
+			    BTC_RSSI_STATE_STAY_MEDIUM)) {
+			if (wifi_rssi >= (rssi_thresh1+BTC_RSSI_COEX_THRESH_TOL_8821A_2ANT)) {
+				wifi_rssi_state = BTC_RSSI_STATE_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state switch to High\n");
+			} else if (wifi_rssi < rssi_thresh) {
+				wifi_rssi_state = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state switch to Low\n");
+			} else {
+				wifi_rssi_state = BTC_RSSI_STATE_STAY_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state stay at Medium\n");
+			}
+		} else {
+			if (wifi_rssi < rssi_thresh1) {
+				wifi_rssi_state = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state switch to Medium\n");
+			} else {
+				wifi_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE,
+					  "[BTCoex], wifi RSSI state stay at High\n");
+			}
+		}
+	}
+
+	coex_sta->pre_wifi_rssi_state[index] = wifi_rssi_state;
+
+	return wifi_rssi_state;
+}
+
+static void monitor_bt_enable_disable(struct btc_coexist *btcoexist)
+{
+	static bool pre_bt_disabled;
+	static u32 bt_disable_cnt;
+	bool bt_active = true, bt_disabled = false;
+
+	/*  This function check if bt is disabled */
+
+	if (coex_sta->high_priority_tx == 0 &&
+	    coex_sta->high_priority_rx == 0 &&
+	    coex_sta->low_priority_tx == 0 &&
+	    coex_sta->low_priority_rx == 0)
+		bt_active = false;
+	if (coex_sta->high_priority_tx == 0xffff &&
+	    coex_sta->high_priority_rx == 0xffff &&
+	    coex_sta->low_priority_tx == 0xffff &&
+	    coex_sta->low_priority_rx == 0xffff)
+		bt_active = false;
+	if (bt_active) {
+		bt_disable_cnt = 0;
+		bt_disabled = false;
+		btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_DISABLE,
+				   &bt_disabled);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+			  "[BTCoex], BT is enabled !!\n");
+	} else {
+		bt_disable_cnt++;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+			  "[BTCoex], bt all counters = 0, %d times!!\n",
+			  bt_disable_cnt);
+		if (bt_disable_cnt >= 2) {
+			bt_disabled = true;
+			btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_DISABLE,
+					   &bt_disabled);
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+				  "[BTCoex], BT is disabled !!\n");
+		}
+	}
+	if (pre_bt_disabled != bt_disabled) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+			  "[BTCoex], BT is from %s to %s!!\n",
+			  (pre_bt_disabled ? "disabled" : "enabled"),
+			  (bt_disabled ? "disabled" : "enabled"));
+		pre_bt_disabled = bt_disabled;
+	}
+}
+
+static void halbtc8821a2ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
+{
+	u32 reg_hp_txrx, reg_lp_txrx, u4tmp;
+	u32 reg_hp_tx = 0, reg_hp_rx = 0, reg_lp_tx = 0, reg_lp_rx = 0;
+
+	reg_hp_txrx = 0x770;
+	reg_lp_txrx = 0x774;
+
+	u4tmp = btcoexist->btc_read_4byte(btcoexist, reg_hp_txrx);
+	reg_hp_tx = u4tmp & MASKLWORD;
+	reg_hp_rx = (u4tmp & MASKHWORD)>>16;
+
+	u4tmp = btcoexist->btc_read_4byte(btcoexist, reg_lp_txrx);
+	reg_lp_tx = u4tmp & MASKLWORD;
+	reg_lp_rx = (u4tmp & MASKHWORD)>>16;
+
+	coex_sta->high_priority_tx = reg_hp_tx;
+	coex_sta->high_priority_rx = reg_hp_rx;
+	coex_sta->low_priority_tx = reg_lp_tx;
+	coex_sta->low_priority_rx = reg_lp_rx;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+		  "[BTCoex], High Priority Tx/Rx (reg 0x%x) = 0x%x(%d)/0x%x(%d)\n",
+		  reg_hp_txrx, reg_hp_tx, reg_hp_tx, reg_hp_rx, reg_hp_rx);
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+		  "[BTCoex], Low Priority Tx/Rx (reg 0x%x) = 0x%x(%d)/0x%x(%d)\n",
+		  reg_lp_txrx, reg_lp_tx, reg_lp_tx, reg_lp_rx, reg_lp_rx);
+
+	/*  reset counter */
+	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
+}
+
+static void halbtc8821a2ant_query_bt_info(struct btc_coexist *btcoexist)
+{
+	u8 h2c_parameter[1] = {0};
+
+	coex_sta->c2h_bt_info_req_sent = true;
+
+	h2c_parameter[0] |= BIT(0);	/*  trigger */
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], Query Bt Info, FW write 0x61 = 0x%x\n",
+		  h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x61, 1, h2c_parameter);
+}
+
+static u8 halbtc8821a2ant_action_algorithm(struct btc_coexist *btcoexist)
+{
+	struct btc_stack_info *stack_info = &btcoexist->stack_info;
+	bool bt_hs_on = false;
+	u8 algorithm = BT_8821A_2ANT_COEX_ALGO_UNDEFINED;
+	u8 num_of_diff_profile = 0;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+
+	/* for win-8 stack HID report error */
+	if (!stack_info->hid_exist) {
+		/* sync  BTInfo with BT firmware and stack */
+		stack_info->hid_exist = coex_sta->hid_exist;
+	}
+	/*  when stack HID report error, here we use the info from bt fw. */
+	if (!stack_info->bt_link_exist)
+		stack_info->bt_link_exist = coex_sta->bt_link_exist;
+
+	if (!coex_sta->bt_link_exist) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], No profile exists!!!\n");
+		return algorithm;
+	}
+
+	if (coex_sta->sco_exist)
+		num_of_diff_profile++;
+	if (coex_sta->hid_exist)
+		num_of_diff_profile++;
+	if (coex_sta->pan_exist)
+		num_of_diff_profile++;
+	if (coex_sta->a2dp_exist)
+		num_of_diff_profile++;
+
+	if (num_of_diff_profile == 1) {
+		if (coex_sta->sco_exist) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], SCO only\n");
+			algorithm = BT_8821A_2ANT_COEX_ALGO_SCO;
+		} else {
+			if (coex_sta->hid_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], HID only\n");
+				algorithm = BT_8821A_2ANT_COEX_ALGO_HID;
+			} else if (coex_sta->a2dp_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], A2DP only\n");
+				algorithm = BT_8821A_2ANT_COEX_ALGO_A2DP;
+			} else if (coex_sta->pan_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], PAN(HS) only\n");
+					algorithm = BT_8821A_2ANT_COEX_ALGO_PANHS;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], PAN(EDR) only\n");
+					algorithm = BT_8821A_2ANT_COEX_ALGO_PANEDR;
+				}
+			}
+		}
+	} else if (num_of_diff_profile == 2) {
+		if (coex_sta->sco_exist) {
+			if (coex_sta->hid_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], SCO + HID\n");
+				algorithm = BT_8821A_2ANT_COEX_ALGO_PANEDR_HID;
+			} else if (coex_sta->a2dp_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], SCO + A2DP ==> SCO\n");
+				algorithm = BT_8821A_2ANT_COEX_ALGO_PANEDR_HID;
+			} else if (coex_sta->pan_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], SCO + PAN(HS)\n");
+					algorithm = BT_8821A_2ANT_COEX_ALGO_SCO;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], SCO + PAN(EDR)\n");
+					algorithm = BT_8821A_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		} else {
+			if (coex_sta->hid_exist &&
+			    coex_sta->a2dp_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], HID + A2DP\n");
+				algorithm = BT_8821A_2ANT_COEX_ALGO_HID_A2DP;
+			} else if (coex_sta->hid_exist &&
+				   coex_sta->pan_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], HID + PAN(HS)\n");
+					algorithm =  BT_8821A_2ANT_COEX_ALGO_HID;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], HID + PAN(EDR)\n");
+					algorithm = BT_8821A_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			} else if (coex_sta->pan_exist &&
+				   coex_sta->a2dp_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], A2DP + PAN(HS)\n");
+					algorithm = BT_8821A_2ANT_COEX_ALGO_A2DP_PANHS;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], A2DP + PAN(EDR)\n");
+					algorithm = BT_8821A_2ANT_COEX_ALGO_PANEDR_A2DP;
+				}
+			}
+		}
+	} else if (num_of_diff_profile == 3) {
+		if (coex_sta->sco_exist) {
+			if (coex_sta->hid_exist &&
+			    coex_sta->a2dp_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], SCO + HID + A2DP ==> HID\n");
+				algorithm = BT_8821A_2ANT_COEX_ALGO_PANEDR_HID;
+			} else if (coex_sta->hid_exist &&
+				   coex_sta->pan_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], SCO + HID + PAN(HS)\n");
+					algorithm = BT_8821A_2ANT_COEX_ALGO_PANEDR_HID;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], SCO + HID + PAN(EDR)\n");
+					algorithm = BT_8821A_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			} else if (coex_sta->pan_exist &&
+				   coex_sta->a2dp_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], SCO + A2DP + PAN(HS)\n");
+					algorithm = BT_8821A_2ANT_COEX_ALGO_PANEDR_HID;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], SCO + A2DP + PAN(EDR) ==> HID\n");
+					algorithm = BT_8821A_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		} else {
+			if (coex_sta->hid_exist &&
+			    coex_sta->pan_exist &&
+			    coex_sta->a2dp_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], HID + A2DP + PAN(HS)\n");
+					algorithm = BT_8821A_2ANT_COEX_ALGO_HID_A2DP;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], HID + A2DP + PAN(EDR)\n");
+					algorithm = BT_8821A_2ANT_COEX_ALGO_HID_A2DP_PANEDR;
+				}
+			}
+		}
+	} else if (num_of_diff_profile >= 3) {
+		if (coex_sta->sco_exist) {
+			if (coex_sta->hid_exist &&
+			    coex_sta->pan_exist &&
+			    coex_sta->a2dp_exist) {
+				if (bt_hs_on) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], Error!!! SCO + HID + A2DP + PAN(HS)\n");
+
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], SCO + HID + A2DP + PAN(EDR) ==>PAN(EDR)+HID\n");
+					algorithm = BT_8821A_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		}
+	}
+	return algorithm;
+}
+
+static bool halbtc8821a2ant_need_to_dec_bt_pwr(struct btc_coexist *btcoexist)
+{
+	bool ret = false;
+	bool bt_hs_on = false, wifi_connected = false;
+	long bt_hs_rssi = 0;
+	u8 bt_rssi_state;
+
+	if (!btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on))
+		return false;
+	if (!btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+				&wifi_connected))
+		return false;
+	if (!btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi))
+		return false;
+
+	bt_rssi_state = halbtc8821a2ant_bt_rssi_state(2, 35, 0);
+
+	if (wifi_connected) {
+		if (bt_hs_on) {
+			if (bt_hs_rssi > 37) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+					  "[BTCoex], Need to decrease bt power for HS mode!!\n");
+				ret = true;
+			}
+		} else {
+			if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+					  "[BTCoex], Need to decrease bt power for Wifi is connected!!\n");
+				ret = true;
+			}
+		}
+	}
+	return ret;
+}
+
+static void set_fw_dac_swing_level(struct btc_coexist *btcoexist,
+				   u8 dac_swing_lvl)
+{
+	u8 h2c_parameter[1] = {0};
+
+	/*  There are several type of dacswing */
+	/*  0x18/ 0x10/ 0xc/ 0x8/ 0x4/ 0x6 */
+	h2c_parameter[0] = dac_swing_lvl;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], Set Dac Swing Level = 0x%x\n", dac_swing_lvl);
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], FW write 0x64 = 0x%x\n", h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x64, 1, h2c_parameter);
+}
+
+static void halbtc8821a2ant_set_fw_dec_bt_pwr(struct btc_coexist *btcoexist,
+					      bool dec_bt_pwr)
+{
+	u8 h2c_parameter[1] = {0};
+
+	h2c_parameter[0] = 0;
+
+	if (dec_bt_pwr)
+		h2c_parameter[0] |= BIT(1);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], decrease Bt Power : %s, FW write 0x62 = 0x%x\n",
+		  (dec_bt_pwr ? "Yes!!" : "No!!"), h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x62, 1, h2c_parameter);
+}
+
+static void halbtc8821a2ant_dec_bt_pwr(struct btc_coexist *btcoexist,
+				       bool force_exec, bool dec_bt_pwr)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], %s Dec BT power = %s\n",
+		  (force_exec ? "force to" : ""),
+		  ((dec_bt_pwr) ? "ON" : "OFF"));
+	coex_dm->cur_dec_bt_pwr = dec_bt_pwr;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], pre_dec_bt_pwr =%d, cur_dec_bt_pwr =%d\n",
+			  coex_dm->pre_dec_bt_pwr, coex_dm->cur_dec_bt_pwr);
+
+		if (coex_dm->pre_dec_bt_pwr == coex_dm->cur_dec_bt_pwr)
+			return;
+	}
+	halbtc8821a2ant_set_fw_dec_bt_pwr(btcoexist, coex_dm->cur_dec_bt_pwr);
+
+	coex_dm->pre_dec_bt_pwr = coex_dm->cur_dec_bt_pwr;
+}
+
+static void set_fw_bt_lna_constrain(struct btc_coexist *btcoexist,
+				    bool bt_lna_cons_on)
+{
+	u8 h2c_parameter[2] = {0};
+
+	h2c_parameter[0] = 0x3;	/*  opCode, 0x3 = BT_SET_LNA_CONSTRAIN */
+
+	if (bt_lna_cons_on)
+		h2c_parameter[1] |= BIT(0);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], set BT LNA Constrain: %s, FW write 0x69 = 0x%x\n",
+		  (bt_lna_cons_on ? "ON!!" : "OFF!!"),
+		  h2c_parameter[0]<<8|h2c_parameter[1]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x69, 2, h2c_parameter);
+}
+
+static void set_bt_lna_constrain(struct btc_coexist *btcoexist, bool force_exec,
+				 bool bt_lna_cons_on)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], %s BT Constrain = %s\n",
+		  (force_exec ? "force" : ""),
+		  ((bt_lna_cons_on) ? "ON" : "OFF"));
+	coex_dm->cur_bt_lna_constrain = bt_lna_cons_on;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], pre_bt_lna_constrain =%d, cur_bt_lna_constrain =%d\n",
+			  coex_dm->pre_bt_lna_constrain,
+			  coex_dm->cur_bt_lna_constrain);
+
+		if (coex_dm->pre_bt_lna_constrain ==
+		    coex_dm->cur_bt_lna_constrain)
+			return;
+	}
+	set_fw_bt_lna_constrain(btcoexist, coex_dm->cur_bt_lna_constrain);
+
+	coex_dm->pre_bt_lna_constrain = coex_dm->cur_bt_lna_constrain;
+}
+
+static void halbtc8821a2ant_set_fw_bt_psd_mode(struct btc_coexist *btcoexist,
+					       u8 bt_psd_mode)
+{
+	u8 h2c_parameter[2] = {0};
+
+	h2c_parameter[0] = 0x2;	/*  opCode, 0x2 = BT_SET_PSD_MODE */
+
+	h2c_parameter[1] = bt_psd_mode;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], set BT PSD mode = 0x%x, FW write 0x69 = 0x%x\n",
+		  h2c_parameter[1],
+		  h2c_parameter[0] << 8 | h2c_parameter[1]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x69, 2, h2c_parameter);
+}
+
+static void halbtc8821a2ant_set_bt_psd_mode(struct btc_coexist *btcoexist,
+					    bool force_exec, u8 bt_psd_mode)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], %s BT PSD mode = 0x%x\n",
+		  (force_exec ? "force" : ""), bt_psd_mode);
+	coex_dm->cur_bt_psd_mode = bt_psd_mode;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], pre_bt_psd_mode = 0x%x, cur_bt_psd_mode = 0x%x\n",
+			  coex_dm->pre_bt_psd_mode, coex_dm->cur_bt_psd_mode);
+
+		if (coex_dm->pre_bt_psd_mode == coex_dm->cur_bt_psd_mode)
+			return;
+	}
+	halbtc8821a2ant_set_fw_bt_psd_mode(btcoexist, coex_dm->cur_bt_psd_mode);
+
+	coex_dm->pre_bt_psd_mode = coex_dm->cur_bt_psd_mode;
+}
+
+static void halbtc8821a2ant_set_bt_auto_report(struct btc_coexist *btcoexist,
+					       bool enable_auto_report)
+{
+	u8 h2c_parameter[1] = {0};
+
+	h2c_parameter[0] = 0;
+
+	if (enable_auto_report)
+		h2c_parameter[0] |= BIT(0);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], BT FW auto report : %s, FW write 0x68 = 0x%x\n",
+		  (enable_auto_report ? "Enabled!!" : "Disabled!!"),
+		  h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x68, 1, h2c_parameter);
+}
+
+static void halbtc8821a2ant_bt_auto_report(struct btc_coexist *btcoexist,
+					   bool force_exec,
+					   bool enable_auto_report)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], %s BT Auto report = %s\n",
+		  (force_exec ? "force to" : ""),
+		  ((enable_auto_report) ? "Enabled" : "Disabled"));
+	coex_dm->cur_bt_auto_report = enable_auto_report;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], pre_bt_auto_report =%d, cur_bt_auto_report =%d\n",
+			  coex_dm->pre_bt_auto_report,
+			  coex_dm->cur_bt_auto_report);
+
+		if (coex_dm->pre_bt_auto_report == coex_dm->cur_bt_auto_report)
+			return;
+	}
+	halbtc8821a2ant_set_bt_auto_report(btcoexist,
+					   coex_dm->cur_bt_auto_report);
+
+	coex_dm->pre_bt_auto_report = coex_dm->cur_bt_auto_report;
+}
+
+static void halbtc8821a2ant_fw_dac_swing_lvl(struct btc_coexist *btcoexist,
+					     bool force_exec,
+					     u8 fw_dac_swing_lvl)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], %s set FW Dac Swing level = %d\n",
+		  (force_exec ? "force to" : ""), fw_dac_swing_lvl);
+	coex_dm->cur_fw_dac_swing_lvl = fw_dac_swing_lvl;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], pre_fw_dac_swing_lvl =%d, cur_fw_dac_swing_lvl =%d\n",
+			  coex_dm->pre_fw_dac_swing_lvl,
+			  coex_dm->cur_fw_dac_swing_lvl);
+
+		if (coex_dm->pre_fw_dac_swing_lvl ==
+		    coex_dm->cur_fw_dac_swing_lvl)
+			return;
+	}
+
+	set_fw_dac_swing_level(btcoexist, coex_dm->cur_fw_dac_swing_lvl);
+
+	coex_dm->pre_fw_dac_swing_lvl = coex_dm->cur_fw_dac_swing_lvl;
+}
+
+static void set_sw_rf_rx_lpf_corner(struct btc_coexist *btcoexist,
+				    bool rx_rf_shrink_on)
+{
+	if (rx_rf_shrink_on) {
+		/* Shrink RF Rx LPF corner */
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+			  "[BTCoex], Shrink RF Rx LPF corner!!\n");
+		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1e,
+					  0xfffff, 0xffffc);
+	} else {
+		/* Resume RF Rx LPF corner */
+		/*  After initialized, we can use coex_dm->bt_rf0x1e_backup */
+		if (btcoexist->initilized) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+				  "[BTCoex], Resume RF Rx LPF corner!!\n");
+			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1e,
+						  0xfffff,
+						  coex_dm->bt_rf0x1e_backup);
+		}
+	}
+}
+
+static void halbtc8821a2ant_RfShrink(struct btc_coexist *btcoexist,
+				     bool force_exec, bool rx_rf_shrink_on)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
+		  "[BTCoex], %s turn Rx RF Shrink = %s\n",
+		  (force_exec ? "force to" : ""),
+		  ((rx_rf_shrink_on) ? "ON" : "OFF"));
+	coex_dm->cur_rf_rx_lpf_shrink = rx_rf_shrink_on;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "[BTCoex], pre_rf_rx_lpf_shrink =%d, cur_rf_rx_lpf_shrink =%d\n",
+			  coex_dm->pre_rf_rx_lpf_shrink,
+			  coex_dm->cur_rf_rx_lpf_shrink);
+
+		if (coex_dm->pre_rf_rx_lpf_shrink ==
+		    coex_dm->cur_rf_rx_lpf_shrink)
+			return;
+	}
+	set_sw_rf_rx_lpf_corner(btcoexist, coex_dm->cur_rf_rx_lpf_shrink);
+
+	coex_dm->pre_rf_rx_lpf_shrink = coex_dm->cur_rf_rx_lpf_shrink;
+}
+
+static void set_sw_penalty_tx_rate_adap(struct btc_coexist *btcoexist,
+					bool low_penalty_ra)
+{
+	u8 h2c_parameter[6] = {0};
+
+	h2c_parameter[0] = 0x6;	/*  opCode, 0x6 = Retry_Penalty */
+
+	if (low_penalty_ra) {
+		h2c_parameter[1] |= BIT(0);
+		/* normal rate except MCS7/6/5, OFDM54/48/36 */
+		h2c_parameter[2] = 0x00;
+		h2c_parameter[3] = 0xf7;  /* MCS7 or OFDM54 */
+		h2c_parameter[4] = 0xf8;  /* MCS6 or OFDM48 */
+		h2c_parameter[5] = 0xf9;  /* MCS5 or OFDM36  */
+	}
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], set WiFi Low-Penalty Retry: %s",
+		  (low_penalty_ra ? "ON!!" : "OFF!!"));
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x69, 6, h2c_parameter);
+}
+
+static void halbtc8821a2ant_low_penalty_ra(struct btc_coexist *btcoexist,
+					   bool force_exec, bool low_penalty_ra)
+{
+	/* return; */
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
+		  "[BTCoex], %s turn LowPenaltyRA = %s\n",
+		  (force_exec ? "force to" : ""),
+		  ((low_penalty_ra) ? "ON" : "OFF"));
+	coex_dm->cur_low_penalty_ra = low_penalty_ra;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "[BTCoex], pre_low_penalty_ra =%d, cur_low_penalty_ra =%d\n",
+			  coex_dm->pre_low_penalty_ra,
+			  coex_dm->cur_low_penalty_ra);
+
+		if (coex_dm->pre_low_penalty_ra == coex_dm->cur_low_penalty_ra)
+			return;
+	}
+	set_sw_penalty_tx_rate_adap(btcoexist, coex_dm->cur_low_penalty_ra);
+
+	coex_dm->pre_low_penalty_ra = coex_dm->cur_low_penalty_ra;
+}
+
+static void halbtc8821a2ant_set_dac_swing_reg(struct btc_coexist *btcoexist,
+					      u32 level)
+{
+	u8 val = (u8)level;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+		  "[BTCoex], Write SwDacSwing = 0x%x\n", level);
+	btcoexist->btc_write_1byte_bitmask(btcoexist, 0xc5b, 0x3e, val);
+}
+
+static void set_sw_fulltime_dac_swing(struct btc_coexist *btcoexist,
+				      bool sw_dac_swing_on,
+				      u32 sw_dac_swing_lvl)
+{
+	if (sw_dac_swing_on)
+		halbtc8821a2ant_set_dac_swing_reg(btcoexist, sw_dac_swing_lvl);
+	else
+		halbtc8821a2ant_set_dac_swing_reg(btcoexist, 0x18);
+}
+
+static void halbtc8821a2ant_dac_swing(struct btc_coexist *btcoexist,
+				      bool force_exec, bool dac_swing_on,
+				      u32 dac_swing_lvl)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
+		  "[BTCoex], %s turn DacSwing =%s, dac_swing_lvl = 0x%x\n",
+		  (force_exec ? "force to" : ""),
+		  ((dac_swing_on) ? "ON" : "OFF"), dac_swing_lvl);
+	coex_dm->cur_dac_swing_on = dac_swing_on;
+	coex_dm->cur_dac_swing_lvl = dac_swing_lvl;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "[BTCoex], pre_dac_swing_on =%d, pre_dac_swing_lvl = 0x%x, cur_dac_swing_on =%d, cur_dac_swing_lvl = 0x%x\n",
+			  coex_dm->pre_dac_swing_on,
+			  coex_dm->pre_dac_swing_lvl,
+			  coex_dm->cur_dac_swing_on,
+			  coex_dm->cur_dac_swing_lvl);
+
+		if ((coex_dm->pre_dac_swing_on == coex_dm->cur_dac_swing_on) &&
+		    (coex_dm->pre_dac_swing_lvl == coex_dm->cur_dac_swing_lvl))
+			return;
+	}
+	mdelay(30);
+	set_sw_fulltime_dac_swing(btcoexist, dac_swing_on, dac_swing_lvl);
+
+	coex_dm->pre_dac_swing_on = coex_dm->cur_dac_swing_on;
+	coex_dm->pre_dac_swing_lvl = coex_dm->cur_dac_swing_lvl;
+}
+
+static void halbtc8821a2ant_set_adc_back_off(struct btc_coexist *btcoexist,
+					     bool adc_back_off)
+{
+	if (adc_back_off) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+			  "[BTCoex], BB BackOff Level On!\n");
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x8db, 0x60, 0x3);
+	} else {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+			  "[BTCoex], BB BackOff Level Off!\n");
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x8db, 0x60, 0x1);
+	}
+}
+
+static void halbtc8821a2ant_adc_back_off(struct btc_coexist *btcoexist,
+					 bool force_exec, bool adc_back_off)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
+		  "[BTCoex], %s turn AdcBackOff = %s\n",
+		  (force_exec ? "force to" : ""),
+		  ((adc_back_off) ? "ON" : "OFF"));
+	coex_dm->cur_adc_back_off = adc_back_off;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "[BTCoex], pre_adc_back_off =%d, cur_adc_back_off =%d\n",
+			coex_dm->pre_adc_back_off, coex_dm->cur_adc_back_off);
+
+		if (coex_dm->pre_adc_back_off == coex_dm->cur_adc_back_off)
+			return;
+	}
+	halbtc8821a2ant_set_adc_back_off(btcoexist, coex_dm->cur_adc_back_off);
+
+	coex_dm->pre_adc_back_off = coex_dm->cur_adc_back_off;
+}
+
+static void halbtc8821a2ant_set_coex_table(struct btc_coexist *btcoexist,
+					   u32 val0x6c0, u32 val0x6c4,
+					   u32 val0x6c8, u8 val0x6cc)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+		  "[BTCoex], set coex table, set 0x6c0 = 0x%x\n", val0x6c0);
+	btcoexist->btc_write_4byte(btcoexist, 0x6c0, val0x6c0);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+		  "[BTCoex], set coex table, set 0x6c4 = 0x%x\n", val0x6c4);
+	btcoexist->btc_write_4byte(btcoexist, 0x6c4, val0x6c4);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+		  "[BTCoex], set coex table, set 0x6c8 = 0x%x\n", val0x6c8);
+	btcoexist->btc_write_4byte(btcoexist, 0x6c8, val0x6c8);
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
+		  "[BTCoex], set coex table, set 0x6cc = 0x%x\n", val0x6cc);
+	btcoexist->btc_write_1byte(btcoexist, 0x6cc, val0x6cc);
+}
+
+static void halbtc8821a2ant_coex_table(struct btc_coexist *btcoexist,
+				       bool force_exec, u32 val0x6c0,
+				       u32 val0x6c4, u32 val0x6c8, u8 val0x6cc)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
+		  "[BTCoex], %s write Coex Table 0x6c0 = 0x%x, 0x6c4 = 0x%x, 0x6c8 = 0x%x, 0x6cc = 0x%x\n",
+		  (force_exec ? "force to" : ""),
+		  val0x6c0, val0x6c4, val0x6c8, val0x6cc);
+	coex_dm->cur_val0x6c0 = val0x6c0;
+	coex_dm->cur_val0x6c4 = val0x6c4;
+	coex_dm->cur_val0x6c8 = val0x6c8;
+	coex_dm->cur_val0x6cc = val0x6cc;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "[BTCoex], pre_val0x6c0 = 0x%x, pre_val0x6c4 = 0x%x, pre_val0x6c8 = 0x%x, pre_val0x6cc = 0x%x !!\n",
+			  coex_dm->pre_val0x6c0, coex_dm->pre_val0x6c4,
+			  coex_dm->pre_val0x6c8, coex_dm->pre_val0x6cc);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
+			  "[BTCoex], cur_val0x6c0 = 0x%x, cur_val0x6c4 = 0x%x, cur_val0x6c8 = 0x%x, cur_val0x6cc = 0x%x !!\n",
+			  coex_dm->cur_val0x6c0, coex_dm->cur_val0x6c4,
+			  coex_dm->cur_val0x6c8, coex_dm->cur_val0x6cc);
+
+		if ((coex_dm->pre_val0x6c0 == coex_dm->cur_val0x6c0) &&
+			(coex_dm->pre_val0x6c4 == coex_dm->cur_val0x6c4) &&
+			(coex_dm->pre_val0x6c8 == coex_dm->cur_val0x6c8) &&
+			(coex_dm->pre_val0x6cc == coex_dm->cur_val0x6cc))
+			return;
+	}
+	halbtc8821a2ant_set_coex_table(btcoexist, val0x6c0, val0x6c4,
+				       val0x6c8, val0x6cc);
+
+	coex_dm->pre_val0x6c0 = coex_dm->cur_val0x6c0;
+	coex_dm->pre_val0x6c4 = coex_dm->cur_val0x6c4;
+	coex_dm->pre_val0x6c8 = coex_dm->cur_val0x6c8;
+	coex_dm->pre_val0x6cc = coex_dm->cur_val0x6cc;
+}
+
+static void set_fw_ignore_wlan_act(struct btc_coexist *btcoexist, bool enable)
+{
+	u8 h2c_parameter[1] = {0};
+
+	if (enable)
+		h2c_parameter[0] |= BIT(0);		/*  function enable */
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], set FW for BT Ignore Wlan_Act, FW write 0x63 = 0x%x\n",
+		  h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x63, 1, h2c_parameter);
+}
+
+static void halbtc8821a2ant_ignore_wlan_act(struct btc_coexist *btcoexist,
+					    bool force_exec, bool enable)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], %s turn Ignore WlanAct %s\n",
+		  (force_exec ? "force to" : ""), (enable ? "ON" : "OFF"));
+	coex_dm->cur_ignore_wlan_act = enable;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], pre_ignore_wlan_act = %d, cur_ignore_wlan_act = %d!!\n",
+			  coex_dm->pre_ignore_wlan_act,
+			  coex_dm->cur_ignore_wlan_act);
+		if (coex_dm->pre_ignore_wlan_act ==
+		    coex_dm->cur_ignore_wlan_act)
+			return;
+	}
+	set_fw_ignore_wlan_act(btcoexist, enable);
+
+	coex_dm->pre_ignore_wlan_act = coex_dm->cur_ignore_wlan_act;
+}
+
+static void halbtc8821a2ant_set_fw_pstdma(struct btc_coexist *btcoexist,
+					  u8 byte1, u8 byte2, u8 byte3,
+					  u8 byte4, u8 byte5)
+{
+	u8 h2c_parameter[5] = {0};
+
+	h2c_parameter[0] = byte1;
+	h2c_parameter[1] = byte2;
+	h2c_parameter[2] = byte3;
+	h2c_parameter[3] = byte4;
+	h2c_parameter[4] = byte5;
+
+	coex_dm->ps_tdma_para[0] = byte1;
+	coex_dm->ps_tdma_para[1] = byte2;
+	coex_dm->ps_tdma_para[2] = byte3;
+	coex_dm->ps_tdma_para[3] = byte4;
+	coex_dm->ps_tdma_para[4] = byte5;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], FW write 0x60(5bytes) = 0x%x%08x\n",
+		  h2c_parameter[0],
+		  h2c_parameter[1] << 24 | h2c_parameter[2] << 16 |
+		  h2c_parameter[3]<<8|h2c_parameter[4]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x60, 5, h2c_parameter);
+}
+
+static void sw_mechanism1(struct btc_coexist *btcoexist, bool shrink_rx_lpf,
+			  bool low_penalty_ra, bool limited_dig,
+			  bool bt_lna_constrain)
+{
+	u32 wifi_bw;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if (BTC_WIFI_BW_HT40 != wifi_bw) {  /* only shrink RF Rx LPF for HT40 */
+		if (shrink_rx_lpf)
+			shrink_rx_lpf = false;
+	}
+
+	 halbtc8821a2ant_RfShrink(btcoexist, NORMAL_EXEC, shrink_rx_lpf);
+	halbtc8821a2ant_low_penalty_ra(btcoexist, NORMAL_EXEC, low_penalty_ra);
+
+	/* no limited DIG */
+	/* set_bt_lna_constrain(btcoexist, NORMAL_EXEC, bBTLNAConstrain); */
+}
+
+static void sw_mechanism2(struct btc_coexist *btcoexist, bool agc_table_shift,
+			  bool adc_back_off, bool sw_dac_swing,
+			  u32 dac_swing_lvl)
+{
+	/* halbtc8821a2ant_AgcTable(btcoexist, NORMAL_EXEC, bAGCTableShift); */
+	halbtc8821a2ant_adc_back_off(btcoexist, NORMAL_EXEC, adc_back_off);
+	halbtc8821a2ant_dac_swing(btcoexist, NORMAL_EXEC, sw_dac_swing,
+				  sw_dac_swing);
+}
+
+static void halbtc8821a2ant_set_ant_path(struct btc_coexist *btcoexist,
+					 u8 ant_pos_type, bool init_hw_cfg,
+					 bool wifi_off)
+{
+	struct btc_board_info *board_info = &btcoexist->board_info;
+	u32 u4tmp = 0;
+	u8 h2c_parameter[2] = {0};
+
+	if (init_hw_cfg) {
+		/*  0x4c[23] = 0, 0x4c[24] = 1  Antenna control by WL/BT */
+		u4tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
+		u4tmp &= ~BIT(23);
+		u4tmp |= BIT(24);
+		btcoexist->btc_write_4byte(btcoexist, 0x4c, u4tmp);
+
+		btcoexist->btc_write_4byte(btcoexist, 0x974, 0x3ff);
+		btcoexist->btc_write_1byte(btcoexist, 0xcb4, 0x77);
+
+		if (board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT) {
+			/* tell firmware "antenna inverse"  ==> WRONG firmware
+			 * antenna control code.==>need fw to fix */
+			h2c_parameter[0] = 1;
+			h2c_parameter[1] = 1;
+			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2, h2c_parameter);
+		} else {
+			/* tell firmware "no antenna inverse" ==> WRONG firmware
+			 * antenna control code.==>need fw to fix */
+			h2c_parameter[0] = 0;
+			h2c_parameter[1] = 1;
+			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
+						h2c_parameter);
+		}
+	}
+
+	/*  ext switch setting */
+	switch (ant_pos_type) {
+	case BTC_ANT_WIFI_AT_MAIN:
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0xcb7, 0x30, 0x1);
+		break;
+	case BTC_ANT_WIFI_AT_AUX:
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0xcb7, 0x30, 0x2);
+		break;
+	}
+}
+
+static void ps21a_tdma(struct btc_coexist *btcoexist, bool force_exec,
+		       bool turn_on, u8 type)
+{
+	/* bool turn_on_by_cnt = false; */
+	/* u8 ps_tdma_type_by_cnt = 0; */
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], %s turn %s PS TDMA, type =%d\n",
+		  (force_exec ? "force to" : ""),
+		  (turn_on ? "ON" : "OFF"), type);
+	coex_dm->cur_ps_tdma_on = turn_on;
+	coex_dm->cur_ps_tdma = type;
+
+	if (!force_exec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], pre_ps_tdma_on = %d, cur_ps_tdma_on = %d!!\n",
+			  coex_dm->pre_ps_tdma_on, coex_dm->cur_ps_tdma_on);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], pre_ps_tdma = %d, cur_ps_tdma = %d!!\n",
+			  coex_dm->pre_ps_tdma, coex_dm->cur_ps_tdma);
+
+		if ((coex_dm->pre_ps_tdma_on == coex_dm->cur_ps_tdma_on) &&
+		    (coex_dm->pre_ps_tdma == coex_dm->cur_ps_tdma))
+			return;
+	}
+	if (turn_on) {
+		switch (type) {
+		case 1:
+		default:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x1a,
+						      0x1a, 0xe1, 0x90);
+			break;
+		case 2:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x12,
+						      0x12, 0xe1, 0x90);
+			break;
+		case 3:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x1c,
+						      0x3, 0xf1, 0x90);
+			break;
+		case 4:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x10,
+						      0x03, 0xf1, 0x90);
+			break;
+		case 5:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x1a,
+						      0x1a, 0x60, 0x90);
+			break;
+		case 6:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x12,
+						      0x12, 0x60, 0x90);
+			break;
+		case 7:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x1c,
+						      0x3, 0x70, 0x90);
+			break;
+		case 8:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xa3, 0x10,
+						      0x3, 0x70, 0x90);
+			break;
+		case 9:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x1a,
+						      0x1a, 0xe1, 0x90);
+			break;
+		case 10:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x12,
+						      0x12, 0xe1, 0x90);
+			break;
+		case 11:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0xa,
+						      0xa, 0xe1, 0x90);
+			break;
+		case 12:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x5,
+						      0x5, 0xe1, 0x90);
+			break;
+		case 13:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x1a,
+						      0x1a, 0x60, 0x90);
+			break;
+		case 14:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x12,
+						      0x12, 0x60, 0x90);
+			break;
+		case 15:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0xa,
+						      0xa, 0x60, 0x90);
+			break;
+		case 16:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x5,
+						      0x5, 0x60, 0x90);
+			break;
+		case 17:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xa3, 0x2f,
+						      0x2f, 0x60, 0x90);
+			break;
+		case 18:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x5,
+						      0x5, 0xe1, 0x90);
+			break;
+		case 19:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x25,
+						      0x25, 0xe1, 0x90);
+			break;
+		case 20:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x25,
+						      0x25, 0x60, 0x90);
+			break;
+		case 21:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x15,
+						      0x03, 0x70, 0x90);
+			break;
+		case 71:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x1a,
+						      0x1a, 0xe1, 0x90);
+			break;
+		}
+	} else {
+		/*  disable PS tdma */
+		switch (type) {
+		case 0:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0x0, 0x0, 0x0,
+						      0x40, 0x0);
+			break;
+		case 1:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0x0, 0x0, 0x0,
+						      0x48, 0x0);
+			break;
+		default:
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0x0, 0x0, 0x0,
+						      0x40, 0x0);
+			break;
+		}
+	}
+
+	/*  update pre state */
+	coex_dm->pre_ps_tdma_on = coex_dm->cur_ps_tdma_on;
+	coex_dm->pre_ps_tdma = coex_dm->cur_ps_tdma;
+}
+
+static void halbtc8821a2ant_coex_all_off(struct btc_coexist *btcoexist)
+{
+	/*  fw all off */
+	ps21a_tdma(btcoexist, NORMAL_EXEC, false, 1);
+	halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+	halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	/*  sw all off */
+	sw_mechanism1(btcoexist, false, false, false, false);
+	sw_mechanism2(btcoexist, false, false, false, 0x18);
+
+	/*  hw all off */
+	halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55555555,
+				   0x55555555, 0xffff, 0x3);
+}
+
+static void halbtc8821a2ant_coex_under_5g(struct btc_coexist *btcoexist)
+{
+	halbtc8821a2ant_coex_all_off(btcoexist);
+}
+
+static void halbtc8821a2ant_init_coex_dm(struct btc_coexist *btcoexist)
+{
+	/*  force to reset coex mechanism */
+	halbtc8821a2ant_coex_table(btcoexist, FORCE_EXEC, 0x55555555,
+				   0x55555555, 0xffff, 0x3);
+
+	ps21a_tdma(btcoexist, FORCE_EXEC, false, 1);
+	halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, FORCE_EXEC, 6);
+	halbtc8821a2ant_dec_bt_pwr(btcoexist, FORCE_EXEC, false);
+
+	sw_mechanism1(btcoexist, false, false, false, false);
+	sw_mechanism2(btcoexist, false, false, false, 0x18);
+}
+
+static void halbtc8821a2ant_bt_inquiry_page(struct btc_coexist *btcoexist)
+{
+	bool low_pwr_disable = true;
+
+	btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
+			   &low_pwr_disable);
+
+	halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
+				   0x5afa5afa, 0xffff, 0x3);
+	ps21a_tdma(btcoexist, NORMAL_EXEC, true, 3);
+}
+
+static bool halbtc8821a2ant_is_common_action(struct btc_coexist *btcoexist)
+{
+	bool common = false, wifi_connected = false, wifi_busy = false;
+	bool low_pwr_disable = false;
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+			   &wifi_connected);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+
+	halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
+				   0x5afa5afa, 0xffff, 0x3);
+
+	if (!wifi_connected &&
+	    BT_8821A_2ANT_BT_STATUS_IDLE == coex_dm->bt_status) {
+		low_pwr_disable = false;
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
+				   &low_pwr_disable);
+
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Wifi IPS + BT IPS!!\n");
+
+		ps21a_tdma(btcoexist, NORMAL_EXEC, false, 1);
+		halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+		sw_mechanism1(btcoexist, false, false, false, false);
+		sw_mechanism2(btcoexist, false, false, false, 0x18);
+
+		common = true;
+	} else if (wifi_connected &&
+		   (BT_8821A_2ANT_BT_STATUS_IDLE == coex_dm->bt_status)) {
+		low_pwr_disable = false;
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
+				   &low_pwr_disable);
+
+		if (wifi_busy) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Wifi Busy + BT IPS!!\n");
+			ps21a_tdma(btcoexist, NORMAL_EXEC, false, 1);
+		} else {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Wifi LPS + BT IPS!!\n");
+			ps21a_tdma(btcoexist, NORMAL_EXEC, false, 1);
+		}
+
+		halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+		sw_mechanism1(btcoexist, false, false, false, false);
+		sw_mechanism2(btcoexist, false, false, false, 0x18);
+
+		common = true;
+	} else if (!wifi_connected &&
+		   (BT_8821A_2ANT_BT_STATUS_CONNECTED_IDLE ==
+		    coex_dm->bt_status)) {
+		low_pwr_disable = true;
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
+				   &low_pwr_disable);
+
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Wifi IPS + BT LPS!!\n");
+
+		ps21a_tdma(btcoexist, NORMAL_EXEC, false, 1);
+		halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+		sw_mechanism1(btcoexist, false, false, false, false);
+		sw_mechanism2(btcoexist, false, false, false, 0x18);
+
+		common = true;
+	} else if (wifi_connected &&
+		   (BT_8821A_2ANT_BT_STATUS_CONNECTED_IDLE ==
+		    coex_dm->bt_status)) {
+		low_pwr_disable = true;
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
+				   &low_pwr_disable);
+
+		if (wifi_busy) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Wifi Busy + BT LPS!!\n");
+			ps21a_tdma(btcoexist, NORMAL_EXEC, false, 1);
+		} else {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Wifi LPS + BT LPS!!\n");
+			ps21a_tdma(btcoexist, NORMAL_EXEC, false, 1);
+		}
+
+		halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+		sw_mechanism1(btcoexist, true, true, true, true);
+		sw_mechanism2(btcoexist, false, false, false, 0x18);
+
+		common = true;
+	} else if (!wifi_connected &&
+		   (BT_8821A_2ANT_BT_STATUS_NON_IDLE == coex_dm->bt_status)) {
+		low_pwr_disable = false;
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
+				   &low_pwr_disable);
+
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Wifi IPS + BT Busy!!\n");
+
+		ps21a_tdma(btcoexist, NORMAL_EXEC, false, 1);
+		halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+		sw_mechanism1(btcoexist, false, false, false, false);
+		sw_mechanism2(btcoexist, false, false, false, 0x18);
+
+		common = true;
+	} else {
+		low_pwr_disable = true;
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
+				   &low_pwr_disable);
+
+		if (wifi_busy) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Wifi Busy + BT Busy!!\n");
+			common = false;
+		} else {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Wifi LPS + BT Busy!!\n");
+			ps21a_tdma(btcoexist, NORMAL_EXEC, true, 21);
+
+			if (halbtc8821a2ant_need_to_dec_bt_pwr(btcoexist))
+				halbtc8821a2ant_dec_bt_pwr(btcoexist,
+							   NORMAL_EXEC, true);
+			else
+				halbtc8821a2ant_dec_bt_pwr(btcoexist,
+							   NORMAL_EXEC, false);
+
+			common = true;
+		}
+		sw_mechanism1(btcoexist, true, true, true, true);
+	}
+	return common;
+}
+
+static void tdma_duration_adjust(struct btc_coexist *btcoexist,
+				 bool sco_hid, bool tx_pause, u8 max_interval)
+{
+	static long up, dn, m, n, wait_count;
+	long result;
+	/* 0: no change, +1: incr WiFi duration, -1: decr WiFi duration */
+	u8 retry_count = 0;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], TdmaDurationAdjust()\n");
+
+	if (coex_dm->reset_tdma_adjust) {
+		coex_dm->reset_tdma_adjust = false;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], first run TdmaDurationAdjust()!!\n");
+		if (sco_hid) {
+			if (tx_pause) {
+				if (max_interval == 1) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 13);
+					coex_dm->ps_tdma_du_adj_type = 13;
+				} else if (max_interval == 2) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 14);
+					coex_dm->ps_tdma_du_adj_type = 14;
+				} else if (max_interval == 3) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
+					coex_dm->ps_tdma_du_adj_type = 15;
+				} else {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
+					coex_dm->ps_tdma_du_adj_type = 15;
+				}
+			} else {
+				if (max_interval == 1) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 9);
+					coex_dm->ps_tdma_du_adj_type = 9;
+				} else if (max_interval == 2) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 10);
+					coex_dm->ps_tdma_du_adj_type = 10;
+				} else if (max_interval == 3) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 11);
+					coex_dm->ps_tdma_du_adj_type = 11;
+				} else {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 11);
+					coex_dm->ps_tdma_du_adj_type = 11;
+				}
+			}
+		} else {
+			if (tx_pause) {
+				if (max_interval == 1) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 5);
+					coex_dm->ps_tdma_du_adj_type = 5;
+				} else if (max_interval == 2) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 6);
+					coex_dm->ps_tdma_du_adj_type = 6;
+				} else if (max_interval == 3) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
+					coex_dm->ps_tdma_du_adj_type = 7;
+				} else {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
+					coex_dm->ps_tdma_du_adj_type = 7;
+				}
+			} else {
+				if (max_interval == 1) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 1);
+					coex_dm->ps_tdma_du_adj_type = 1;
+				} else if (max_interval == 2) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 2);
+					coex_dm->ps_tdma_du_adj_type = 2;
+				} else if (max_interval == 3) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 3);
+					coex_dm->ps_tdma_du_adj_type = 3;
+				} else {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 3);
+					coex_dm->ps_tdma_du_adj_type = 3;
+				}
+			}
+		}
+		up = 0;
+		dn = 0;
+		m = 1;
+		n = 3;
+		result = 0;
+		wait_count = 0;
+	} else {
+		/* accquire the BT TRx retry count from BT_Info byte2 */
+		retry_count = coex_sta->bt_retry_cnt;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], retry_count = %d\n", retry_count);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], up =%d, dn =%d, m =%d, n =%d, wait_count =%d\n",
+			  (int)up, (int)dn, (int)m, (int)n, (int)wait_count);
+		result = 0;
+		wait_count++;
+
+		if (retry_count == 0) {
+			/*  no retry in the last 2-second duration */
+			up++;
+			dn--;
+
+			if (dn <= 0)
+				dn = 0;
+
+			if (up >= n) {
+				wait_count = 0;
+				n = 3;
+				up = 0;
+				dn = 0;
+				result = 1;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_TRACE_FW_DETAIL,
+					  "[BTCoex], Increase wifi duration!!\n");
+			}
+		} else if (retry_count <= 3) {
+			/*  <= 3 retry in the last 2-second duration */
+			up--;
+			dn++;
+
+			if (up <= 0)
+				up = 0;
+
+			if (dn == 2) {
+				if (wait_count <= 2)
+					m++;
+				else
+					m = 1;
+
+				if (m >= 20)
+					m = 20;
+
+				n = 3*m;
+				up = 0;
+				dn = 0;
+				wait_count = 0;
+				result = -1;
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_TRACE_FW_DETAIL,
+					  "[BTCoex], Decrease wifi duration for retryCounter<3!!\n");
+			}
+		} else {
+			if (wait_count == 1)
+				m++;
+			else
+				m = 1;
+
+			if (m >= 20)
+				m = 20;
+
+			n = 3*m;
+			up = 0;
+			dn = 0;
+			wait_count = 0;
+			result = -1;
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+				  "[BTCoex], Decrease wifi duration for retryCounter>3!!\n");
+		}
+
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], max Interval = %d\n", max_interval);
+		if (max_interval == 1) {
+			if (tx_pause) {
+				/* TODO: refactor here */
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_TRACE_FW_DETAIL,
+					  "[BTCoex], TxPause = 1\n");
+				if (coex_dm->cur_ps_tdma == 71) {
+					ps21a_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 5);
+					coex_dm->ps_tdma_du_adj_type = 5;
+				} else if (coex_dm->cur_ps_tdma == 1) {
+					ps21a_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 5);
+					coex_dm->ps_tdma_du_adj_type = 5;
+				} else if (coex_dm->cur_ps_tdma == 2) {
+					ps21a_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 6);
+					coex_dm->ps_tdma_du_adj_type = 6;
+				} else if (coex_dm->cur_ps_tdma == 3) {
+					ps21a_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 7);
+					coex_dm->ps_tdma_du_adj_type = 7;
+				} else if (coex_dm->cur_ps_tdma == 4) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 8);
+					coex_dm->ps_tdma_du_adj_type = 8;
+				}
+				if (coex_dm->cur_ps_tdma == 9) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 13);
+					coex_dm->ps_tdma_du_adj_type = 13;
+				} else if (coex_dm->cur_ps_tdma == 10) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 14);
+					coex_dm->ps_tdma_du_adj_type = 14;
+				} else if (coex_dm->cur_ps_tdma == 11) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
+					coex_dm->ps_tdma_du_adj_type = 15;
+				} else if (coex_dm->cur_ps_tdma == 12) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 16);
+					coex_dm->ps_tdma_du_adj_type = 16;
+				}
+
+				if (result == -1) {
+					if (coex_dm->cur_ps_tdma == 5) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 6);
+						coex_dm->ps_tdma_du_adj_type = 6;
+					} else if (coex_dm->cur_ps_tdma == 6) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
+						coex_dm->ps_tdma_du_adj_type = 7;
+					} else if (coex_dm->cur_ps_tdma == 7) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 8);
+						coex_dm->ps_tdma_du_adj_type = 8;
+					} else if (coex_dm->cur_ps_tdma == 13) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 14);
+						coex_dm->ps_tdma_du_adj_type = 14;
+					} else if (coex_dm->cur_ps_tdma == 14) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
+						coex_dm->ps_tdma_du_adj_type = 15;
+					} else if (coex_dm->cur_ps_tdma == 15) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 16);
+						coex_dm->ps_tdma_du_adj_type = 16;
+					}
+				} else if (result == 1) {
+					if (coex_dm->cur_ps_tdma == 8) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
+						coex_dm->ps_tdma_du_adj_type = 7;
+					} else if (coex_dm->cur_ps_tdma == 7) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 6);
+						coex_dm->ps_tdma_du_adj_type = 6;
+					} else if (coex_dm->cur_ps_tdma == 6) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 5);
+						coex_dm->ps_tdma_du_adj_type = 5;
+					} else if (coex_dm->cur_ps_tdma == 16) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
+						coex_dm->ps_tdma_du_adj_type = 15;
+					} else if (coex_dm->cur_ps_tdma == 15) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 14);
+						coex_dm->ps_tdma_du_adj_type = 14;
+					} else if (coex_dm->cur_ps_tdma == 14) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 13);
+						coex_dm->ps_tdma_du_adj_type = 13;
+					}
+				}
+			} else {
+				/* TODO: refactor here */
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, "[BTCoex], TxPause = 0\n");
+				if (coex_dm->cur_ps_tdma == 5) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 71);
+					coex_dm->ps_tdma_du_adj_type = 71;
+				} else if (coex_dm->cur_ps_tdma == 6) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 2);
+					coex_dm->ps_tdma_du_adj_type = 2;
+				} else if (coex_dm->cur_ps_tdma == 7) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 3);
+					coex_dm->ps_tdma_du_adj_type = 3;
+				} else if (coex_dm->cur_ps_tdma == 8) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 4);
+					coex_dm->ps_tdma_du_adj_type = 4;
+				}
+				if (coex_dm->cur_ps_tdma == 13) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 9);
+					coex_dm->ps_tdma_du_adj_type = 9;
+				} else if (coex_dm->cur_ps_tdma == 14) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 10);
+					coex_dm->ps_tdma_du_adj_type = 10;
+				} else if (coex_dm->cur_ps_tdma == 15) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 11);
+					coex_dm->ps_tdma_du_adj_type = 11;
+				} else if (coex_dm->cur_ps_tdma == 16) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 12);
+					coex_dm->ps_tdma_du_adj_type = 12;
+				}
+
+				if (result == -1) {
+					if (coex_dm->cur_ps_tdma == 71) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 1);
+						coex_dm->ps_tdma_du_adj_type = 1;
+					} else if (coex_dm->cur_ps_tdma == 1) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 2);
+						coex_dm->ps_tdma_du_adj_type = 2;
+					} else if (coex_dm->cur_ps_tdma == 2) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 3);
+						coex_dm->ps_tdma_du_adj_type = 3;
+					} else if (coex_dm->cur_ps_tdma == 3) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 4);
+						coex_dm->ps_tdma_du_adj_type = 4;
+					} else if (coex_dm->cur_ps_tdma == 9) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 10);
+						coex_dm->ps_tdma_du_adj_type = 10;
+					} else if (coex_dm->cur_ps_tdma == 10) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 11);
+						coex_dm->ps_tdma_du_adj_type = 11;
+					} else if (coex_dm->cur_ps_tdma == 11) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 12);
+						coex_dm->ps_tdma_du_adj_type = 12;
+					}
+				} else if (result == 1) {
+					if (coex_dm->cur_ps_tdma == 4) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 3);
+						coex_dm->ps_tdma_du_adj_type = 3;
+					} else if (coex_dm->cur_ps_tdma == 3) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 2);
+						coex_dm->ps_tdma_du_adj_type = 2;
+					} else if (coex_dm->cur_ps_tdma == 2) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 1);
+						coex_dm->ps_tdma_du_adj_type = 1;
+					} else if (coex_dm->cur_ps_tdma == 1) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 71);
+						coex_dm->ps_tdma_du_adj_type = 71;
+					} else if (coex_dm->cur_ps_tdma == 12) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 11);
+						coex_dm->ps_tdma_du_adj_type = 11;
+					} else if (coex_dm->cur_ps_tdma == 11) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 10);
+						coex_dm->ps_tdma_du_adj_type = 10;
+					} else if (coex_dm->cur_ps_tdma == 10) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 9);
+						coex_dm->ps_tdma_du_adj_type = 9;
+					}
+				}
+			}
+		} else if (max_interval == 2) {
+			if (tx_pause) {
+				/* TODO: refactor here */
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, "[BTCoex], TxPause = 1\n");
+				if (coex_dm->cur_ps_tdma == 1) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 6);
+					coex_dm->ps_tdma_du_adj_type = 6;
+				} else if (coex_dm->cur_ps_tdma == 2) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 6);
+					coex_dm->ps_tdma_du_adj_type = 6;
+				} else if (coex_dm->cur_ps_tdma == 3) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
+					coex_dm->ps_tdma_du_adj_type = 7;
+				} else if (coex_dm->cur_ps_tdma == 4) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 8);
+					coex_dm->ps_tdma_du_adj_type = 8;
+				}
+				if (coex_dm->cur_ps_tdma == 9) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 14);
+					coex_dm->ps_tdma_du_adj_type = 14;
+				} else if (coex_dm->cur_ps_tdma == 10) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 14);
+					coex_dm->ps_tdma_du_adj_type = 14;
+				} else if (coex_dm->cur_ps_tdma == 11) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
+					coex_dm->ps_tdma_du_adj_type = 15;
+				} else if (coex_dm->cur_ps_tdma == 12) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 16);
+					coex_dm->ps_tdma_du_adj_type = 16;
+				}
+				if (result == -1) {
+					if (coex_dm->cur_ps_tdma == 5) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 6);
+						coex_dm->ps_tdma_du_adj_type = 6;
+					} else if (coex_dm->cur_ps_tdma == 6) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
+						coex_dm->ps_tdma_du_adj_type = 7;
+					} else if (coex_dm->cur_ps_tdma == 7) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 8);
+						coex_dm->ps_tdma_du_adj_type = 8;
+					} else if (coex_dm->cur_ps_tdma == 13) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 14);
+						coex_dm->ps_tdma_du_adj_type = 14;
+					} else if (coex_dm->cur_ps_tdma == 14) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
+						coex_dm->ps_tdma_du_adj_type = 15;
+					} else if (coex_dm->cur_ps_tdma == 15) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 16);
+						coex_dm->ps_tdma_du_adj_type = 16;
+					}
+				} else if (result == 1) {
+					if (coex_dm->cur_ps_tdma == 8) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
+						coex_dm->ps_tdma_du_adj_type = 7;
+					} else if (coex_dm->cur_ps_tdma == 7) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 6);
+						coex_dm->ps_tdma_du_adj_type = 6;
+					} else if (coex_dm->cur_ps_tdma == 6) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 6);
+						coex_dm->ps_tdma_du_adj_type = 6;
+					} else if (coex_dm->cur_ps_tdma == 16) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
+						coex_dm->ps_tdma_du_adj_type = 15;
+					} else if (coex_dm->cur_ps_tdma == 15) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 14);
+						coex_dm->ps_tdma_du_adj_type = 14;
+					} else if (coex_dm->cur_ps_tdma == 14) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 14);
+						coex_dm->ps_tdma_du_adj_type = 14;
+					}
+				}
+			} else {
+				/* TODO: refactor here */
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+					  "[BTCoex], TxPause = 0\n");
+				if (coex_dm->cur_ps_tdma == 5) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 2);
+					coex_dm->ps_tdma_du_adj_type = 2;
+				} else if (coex_dm->cur_ps_tdma == 6) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 2);
+					coex_dm->ps_tdma_du_adj_type = 2;
+				} else if (coex_dm->cur_ps_tdma == 7) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 3);
+					coex_dm->ps_tdma_du_adj_type = 3;
+				} else if (coex_dm->cur_ps_tdma == 8) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 4);
+					coex_dm->ps_tdma_du_adj_type = 4;
+				}
+				if (coex_dm->cur_ps_tdma == 13) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 10);
+					coex_dm->ps_tdma_du_adj_type = 10;
+				} else if (coex_dm->cur_ps_tdma == 14) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 10);
+					coex_dm->ps_tdma_du_adj_type = 10;
+				} else if (coex_dm->cur_ps_tdma == 15) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 11);
+					coex_dm->ps_tdma_du_adj_type = 11;
+				} else if (coex_dm->cur_ps_tdma == 16) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 12);
+					coex_dm->ps_tdma_du_adj_type = 12;
+				}
+				if (result == -1) {
+					if (coex_dm->cur_ps_tdma == 1) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 2);
+						coex_dm->ps_tdma_du_adj_type = 2;
+					} else if (coex_dm->cur_ps_tdma == 2) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 3);
+						coex_dm->ps_tdma_du_adj_type = 3;
+					} else if (coex_dm->cur_ps_tdma == 3) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 4);
+						coex_dm->ps_tdma_du_adj_type = 4;
+					} else if (coex_dm->cur_ps_tdma == 9) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 10);
+						coex_dm->ps_tdma_du_adj_type = 10;
+					} else if (coex_dm->cur_ps_tdma == 10) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 11);
+						coex_dm->ps_tdma_du_adj_type = 11;
+					} else if (coex_dm->cur_ps_tdma == 11) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 12);
+						coex_dm->ps_tdma_du_adj_type = 12;
+					}
+				} else if (result == 1) {
+					if (coex_dm->cur_ps_tdma == 4) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 3);
+						coex_dm->ps_tdma_du_adj_type = 3;
+					} else if (coex_dm->cur_ps_tdma == 3) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 2);
+						coex_dm->ps_tdma_du_adj_type = 2;
+					} else if (coex_dm->cur_ps_tdma == 2) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 2);
+						coex_dm->ps_tdma_du_adj_type = 2;
+					} else if (coex_dm->cur_ps_tdma == 12) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 11);
+						coex_dm->ps_tdma_du_adj_type = 11;
+					} else if (coex_dm->cur_ps_tdma == 11) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 10);
+						coex_dm->ps_tdma_du_adj_type = 10;
+					} else if (coex_dm->cur_ps_tdma == 10) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 10);
+						coex_dm->ps_tdma_du_adj_type = 10;
+					}
+				}
+			}
+		} else if (max_interval == 3) {
+			if (tx_pause) {
+				/* TODO: refactor here */
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+					  "[BTCoex], TxPause = 1\n");
+				if (coex_dm->cur_ps_tdma == 1) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
+					coex_dm->ps_tdma_du_adj_type = 7;
+				} else if (coex_dm->cur_ps_tdma == 2) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
+					coex_dm->ps_tdma_du_adj_type = 7;
+				} else if (coex_dm->cur_ps_tdma == 3) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
+					coex_dm->ps_tdma_du_adj_type = 7;
+				} else if (coex_dm->cur_ps_tdma == 4) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 8);
+					coex_dm->ps_tdma_du_adj_type = 8;
+				}
+				if (coex_dm->cur_ps_tdma == 9) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
+					coex_dm->ps_tdma_du_adj_type = 15;
+				} else if (coex_dm->cur_ps_tdma == 10) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
+					coex_dm->ps_tdma_du_adj_type = 15;
+				} else if (coex_dm->cur_ps_tdma == 11) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
+					coex_dm->ps_tdma_du_adj_type = 15;
+				} else if (coex_dm->cur_ps_tdma == 12) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 16);
+					coex_dm->ps_tdma_du_adj_type = 16;
+				}
+				if (result == -1) {
+					if (coex_dm->cur_ps_tdma == 5) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
+						coex_dm->ps_tdma_du_adj_type = 7;
+					} else if (coex_dm->cur_ps_tdma == 6) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
+						coex_dm->ps_tdma_du_adj_type = 7;
+					} else if (coex_dm->cur_ps_tdma == 7) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 8);
+						coex_dm->ps_tdma_du_adj_type = 8;
+					} else if (coex_dm->cur_ps_tdma == 13) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
+						coex_dm->ps_tdma_du_adj_type = 15;
+					} else if (coex_dm->cur_ps_tdma == 14) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
+						coex_dm->ps_tdma_du_adj_type = 15;
+					} else if (coex_dm->cur_ps_tdma == 15) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 16);
+						coex_dm->ps_tdma_du_adj_type = 16;
+					}
+				} else if (result == 1) {
+					if (coex_dm->cur_ps_tdma == 8) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
+						coex_dm->ps_tdma_du_adj_type = 7;
+					} else if (coex_dm->cur_ps_tdma == 7) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
+						coex_dm->ps_tdma_du_adj_type = 7;
+					} else if (coex_dm->cur_ps_tdma == 6) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
+						coex_dm->ps_tdma_du_adj_type = 7;
+					} else if (coex_dm->cur_ps_tdma == 16) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
+						coex_dm->ps_tdma_du_adj_type = 15;
+					} else if (coex_dm->cur_ps_tdma == 15) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
+						coex_dm->ps_tdma_du_adj_type = 15;
+					} else if (coex_dm->cur_ps_tdma == 14) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
+						coex_dm->ps_tdma_du_adj_type = 15;
+					}
+				}
+			} else {
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_TRACE_FW_DETAIL,
+					  "[BTCoex], TxPause = 0\n");
+				if (coex_dm->cur_ps_tdma == 5) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC,
+						   true, 3);
+					coex_dm->ps_tdma_du_adj_type = 3;
+				} else if (coex_dm->cur_ps_tdma == 6) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC,
+						   true, 3);
+					coex_dm->ps_tdma_du_adj_type = 3;
+				} else if (coex_dm->cur_ps_tdma == 7) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC,
+						   true, 3);
+					coex_dm->ps_tdma_du_adj_type = 3;
+				} else if (coex_dm->cur_ps_tdma == 8) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC,
+						   true, 4);
+					coex_dm->ps_tdma_du_adj_type = 4;
+				}
+				if (coex_dm->cur_ps_tdma == 13) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC,
+						   true, 11);
+					coex_dm->ps_tdma_du_adj_type = 11;
+				} else if (coex_dm->cur_ps_tdma == 14) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC,
+						   true, 11);
+					coex_dm->ps_tdma_du_adj_type = 11;
+				} else if (coex_dm->cur_ps_tdma == 15) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC,
+						   true, 11);
+					coex_dm->ps_tdma_du_adj_type = 11;
+				} else if (coex_dm->cur_ps_tdma == 16) {
+					ps21a_tdma(btcoexist, NORMAL_EXEC,
+						   true, 12);
+					coex_dm->ps_tdma_du_adj_type = 12;
+				}
+				if (result == -1) {
+					if (coex_dm->cur_ps_tdma == 1) {
+						ps21a_tdma(btcoexist,
+							   NORMAL_EXEC,
+							   true, 3);
+						coex_dm->ps_tdma_du_adj_type = 3;
+					} else if (coex_dm->cur_ps_tdma == 2) {
+						ps21a_tdma(btcoexist,
+							   NORMAL_EXEC,
+							   true, 3);
+						coex_dm->ps_tdma_du_adj_type = 3;
+					} else if (coex_dm->cur_ps_tdma == 3) {
+						ps21a_tdma(btcoexist,
+							   NORMAL_EXEC,
+							   true, 4);
+						coex_dm->ps_tdma_du_adj_type = 4;
+					} else if (coex_dm->cur_ps_tdma == 9) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 11);
+						coex_dm->ps_tdma_du_adj_type = 11;
+					} else if (coex_dm->cur_ps_tdma == 10) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 11);
+						coex_dm->ps_tdma_du_adj_type = 11;
+					} else if (coex_dm->cur_ps_tdma == 11) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 12);
+						coex_dm->ps_tdma_du_adj_type = 12;
+					}
+				} else if (result == 1) {
+					if (coex_dm->cur_ps_tdma == 4) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 3);
+						coex_dm->ps_tdma_du_adj_type = 3;
+					} else if (coex_dm->cur_ps_tdma == 3) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 3);
+						coex_dm->ps_tdma_du_adj_type = 3;
+					} else if (coex_dm->cur_ps_tdma == 2) {
+						ps21a_tdma(btcoexist,
+							   NORMAL_EXEC, true, 3);
+						coex_dm->ps_tdma_du_adj_type = 3;
+					} else if (coex_dm->cur_ps_tdma == 12) {
+						ps21a_tdma(btcoexist, NORMAL_EXEC,
+							   true, 11);
+						coex_dm->ps_tdma_du_adj_type = 11;
+					} else if (coex_dm->cur_ps_tdma == 11) {
+						ps21a_tdma(btcoexist,
+							   NORMAL_EXEC, true, 11);
+						coex_dm->ps_tdma_du_adj_type = 11;
+					} else if (coex_dm->cur_ps_tdma == 10) {
+						ps21a_tdma(btcoexist,
+							   NORMAL_EXEC, true, 11);
+						coex_dm->ps_tdma_du_adj_type = 11;
+					}
+				}
+			}
+		}
+	}
+
+	/*  if current PsTdma not match with the recorded one
+	 * (when scan, dhcp...),
+	 *  then we have to adjust it back to the previous record one. */
+	if (coex_dm->cur_ps_tdma != coex_dm->ps_tdma_du_adj_type) {
+		bool scan = false, link = false, roam = false;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], PsTdma type dismatch!!!, cur_ps_tdma =%d, recordPsTdma =%d\n",
+			  coex_dm->cur_ps_tdma, coex_dm->ps_tdma_du_adj_type);
+
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
+		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
+
+		if (!scan && !link && !roam)
+			ps21a_tdma(btcoexist, NORMAL_EXEC, true,
+				   coex_dm->ps_tdma_du_adj_type);
+		else
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+				  "[BTCoex], roaming/link/scan is under progress, will adjust next time!!!\n");
+	}
+
+	/* when tdma_duration_adjust() is called, fw dac swing is
+	 * included in the function. */
+	halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 0x6);
+}
+
+/*  SCO only or SCO+PAN(HS) */
+static void halbtc8821a2ant_action_sco(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = wifi21a_rssi_state(btcoexist, 0, 2, 15, 0);
+	bt_rssi_state = halbtc8821a2ant_bt_rssi_state(2, 35, 0);
+
+	halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 4);
+
+	if (halbtc8821a2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if (BTC_WIFI_BW_LEGACY == wifi_bw) /* for SCO quality at 11b/g mode */
+		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x5a5a5a5a,
+					   0x5a5a5a5a, 0xffff, 0x3);
+	else  /* for SCO quality & wifi performance balance at 11n mode */
+		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x5aea5aea,
+					   0x5aea5aea, 0xffff, 0x3);
+
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		/*  fw mechanism */
+		/* ps21a_tdma(btcoexist, NORMAL_EXEC, true, 5); */
+
+		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+			ps21a_tdma(btcoexist, NORMAL_EXEC,
+				   false, 0); /* for voice qual */
+		else
+			ps21a_tdma(btcoexist, NORMAL_EXEC,
+				   false, 0); /* for voice qual */
+
+		/*  sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			sw_mechanism1(btcoexist, true, true, false, false);
+			sw_mechanism2(btcoexist, true, false, false, 0x18);
+		} else {
+			sw_mechanism1(btcoexist, true, true, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	} else {
+		/*  fw mechanism */
+
+		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+			ps21a_tdma(btcoexist, NORMAL_EXEC,
+				   false, 0); /* for voice qual */
+		else
+			ps21a_tdma(btcoexist, NORMAL_EXEC,
+				   false, 0); /* for voice qual */
+
+		/*  sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			sw_mechanism1(btcoexist, false, true, false, false);
+			sw_mechanism2(btcoexist, true, false, false, 0x18);
+		} else {
+			sw_mechanism1(btcoexist, false, true, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+static void halbtc8821a2ant_action_hid(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = wifi21a_rssi_state(btcoexist, 0, 2, 15, 0);
+	bt_rssi_state = halbtc8821a2ant_bt_rssi_state(2, 35, 0);
+
+	halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (halbtc8821a2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if (BTC_WIFI_BW_LEGACY == wifi_bw) /* for HID at 11b/g mode */
+		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
+					   0x5a5a5a5a, 0xffff, 0x3);
+	else  /* for HID quality & wifi performance balance at 11n mode */
+		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
+					   0x5aea5aea, 0xffff, 0x3);
+
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		/*  fw mechanism */
+		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+			ps21a_tdma(btcoexist, NORMAL_EXEC, true, 9);
+		else
+			ps21a_tdma(btcoexist, NORMAL_EXEC, true, 13);
+		/*  sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			sw_mechanism1(btcoexist, true, true, false, false);
+			sw_mechanism2(btcoexist, true, false, false, 0x18);
+		} else {
+			sw_mechanism1(btcoexist, true, true, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	} else {
+		/*  fw mechanism */
+		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+			ps21a_tdma(btcoexist, NORMAL_EXEC, true, 9);
+		else
+			ps21a_tdma(btcoexist, NORMAL_EXEC, true, 13);
+
+		/*  sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			sw_mechanism1(btcoexist, false, true, false, false);
+			sw_mechanism2(btcoexist, true, false, false, 0x18);
+		} else {
+			sw_mechanism1(btcoexist, false, true, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+/* A2DP only / PAN(EDR) only/ A2DP+PAN(HS) */
+static void halbtc8821a2ant_action_a2dp(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = wifi21a_rssi_state(btcoexist, 0, 2, 15, 0);
+	bt_rssi_state = halbtc8821a2ant_bt_rssi_state(2, 35, 0);
+
+	/* fw dac swing is called in tdma_duration_adjust() */
+	/* halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6); */
+
+	if (halbtc8821a2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		/*  fw mechanism */
+		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+			tdma_duration_adjust(btcoexist, false, false, 1);
+		else
+			tdma_duration_adjust(btcoexist, false, true, 1);
+
+		/*  sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			sw_mechanism1(btcoexist, true, false, false, false);
+			sw_mechanism2(btcoexist, true, false, false, 0x18);
+		} else {
+			sw_mechanism1(btcoexist, true, false, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	} else {
+		/*  fw mechanism */
+		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+			tdma_duration_adjust(btcoexist, false, false, 1);
+		else
+			tdma_duration_adjust(btcoexist, false, true, 1);
+
+		/*  sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			sw_mechanism1(btcoexist, false, false, false, false);
+			sw_mechanism2(btcoexist, true, false, false, 0x18);
+		} else {
+			sw_mechanism1(btcoexist, false, false, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+static void halbtc8821a2ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state, bt_info_ext;
+	u32 wifi_bw;
+
+	bt_info_ext = coex_sta->bt_info_ext;
+	wifi_rssi_state = wifi21a_rssi_state(btcoexist, 0, 2, 15, 0);
+	bt_rssi_state = halbtc8821a2ant_bt_rssi_state(2, 35, 0);
+
+	/* fw dac swing is called in tdma_duration_adjust() */
+	/* halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6); */
+
+	if (halbtc8821a2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		/*  fw mechanism */
+		if (bt_info_ext & BIT(0))	/* a2dp basic rate */
+			tdma_duration_adjust(btcoexist, false, true, 2);
+		else				/* a2dp edr rate */
+			tdma_duration_adjust(btcoexist, false, true, 1);
+
+		/*  sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			sw_mechanism1(btcoexist, true, false, false, false);
+			sw_mechanism2(btcoexist, true, false, false, 0x18);
+		} else {
+			sw_mechanism1(btcoexist, true, false, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	} else {
+		/*  fw mechanism */
+		if (bt_info_ext & BIT(0))	/* a2dp basic rate */
+			tdma_duration_adjust(btcoexist, false, true, 2);
+		else				/* a2dp edr rate */
+			tdma_duration_adjust(btcoexist, false, true, 1);
+
+		/*  sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			sw_mechanism1(btcoexist, false, false, false, false);
+			sw_mechanism2(btcoexist, true, false, false, 0x18);
+		} else {
+			sw_mechanism1(btcoexist, false, false, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+static void halbtc8821a2ant_action_pan_edr(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = wifi21a_rssi_state(btcoexist, 0, 2, 15, 0);
+	bt_rssi_state = halbtc8821a2ant_bt_rssi_state(2, 35, 0);
+
+	halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (halbtc8821a2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if (BTC_WIFI_BW_LEGACY == wifi_bw) /* for HID at 11b/g mode */
+		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
+					   0x5aff5aff, 0xffff, 0x3);
+	else  /* for HID quality & wifi performance balance at 11n mode */
+		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
+					   0x5aff5aff, 0xffff, 0x3);
+
+		if (BTC_WIFI_BW_HT40 == wifi_bw) {
+			/*  fw mechanism */
+		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+			ps21a_tdma(btcoexist, NORMAL_EXEC, true, 1);
+		else
+			ps21a_tdma(btcoexist, NORMAL_EXEC, true, 5);
+
+		/*  sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			sw_mechanism1(btcoexist, true, false, false, false);
+			sw_mechanism2(btcoexist, true, false, false, 0x18);
+		} else {
+			sw_mechanism1(btcoexist, true, false, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	} else {
+		/*  fw mechanism */
+		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+			ps21a_tdma(btcoexist, NORMAL_EXEC, true, 1);
+		else
+			ps21a_tdma(btcoexist, NORMAL_EXEC, true, 5);
+
+		/*  sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			sw_mechanism1(btcoexist, false, false, false, false);
+			sw_mechanism2(btcoexist, true, false, false, 0x18);
+		} else {
+			sw_mechanism1(btcoexist, false, false, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+/* PAN(HS) only */
+static void halbtc8821a2ant_action_pan_hs(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = wifi21a_rssi_state(btcoexist, 0, 2, 15, 0);
+	bt_rssi_state = halbtc8821a2ant_bt_rssi_state(2, 35, 0);
+
+	halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		/*  fw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+			halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC,
+						   true);
+		else
+			halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC,
+						   false);
+		ps21a_tdma(btcoexist, NORMAL_EXEC, false, 1);
+
+		/*  sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			sw_mechanism1(btcoexist, true, false, false, false);
+			sw_mechanism2(btcoexist, true, false, false, 0x18);
+		} else {
+			sw_mechanism1(btcoexist, true, false, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	} else {
+		/*  fw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+			halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC,
+						   true);
+		else
+			halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC,
+						   false);
+
+		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+			ps21a_tdma(btcoexist, NORMAL_EXEC, false, 1);
+		else
+			ps21a_tdma(btcoexist, NORMAL_EXEC, false, 1);
+
+		/*  sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			sw_mechanism1(btcoexist, false, false, false, false);
+			sw_mechanism2(btcoexist, true, false, false, 0x18);
+		} else {
+			sw_mechanism1(btcoexist, false, false, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+/* PAN(EDR)+A2DP */
+static void halbtc8821a2ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state, bt_info_ext;
+	u32 wifi_bw;
+
+	bt_info_ext = coex_sta->bt_info_ext;
+	wifi_rssi_state = wifi21a_rssi_state(btcoexist, 0, 2, 15, 0);
+	bt_rssi_state = halbtc8821a2ant_bt_rssi_state(2, 35, 0);
+
+	halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (halbtc8821a2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if (BTC_WIFI_BW_LEGACY == wifi_bw) /* for HID at 11b/g mode */
+		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
+					   0x5afa5afa, 0xffff, 0x3);
+	else  /* for HID quality & wifi performance balance at 11n mode */
+		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
+					   0x5afa5afa, 0xffff, 0x3);
+
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		/*  fw mechanism */
+		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			if (bt_info_ext & BIT(0))	/* a2dp basic rate */
+				tdma_duration_adjust(btcoexist, false,
+						     false, 3);
+			else				/* a2dp edr rate */
+				tdma_duration_adjust(btcoexist, false,
+						     false, 3);
+		} else {
+			if (bt_info_ext & BIT(0))	/* a2dp basic rate */
+				tdma_duration_adjust(btcoexist, false, true, 3);
+			else				/* a2dp edr rate */
+				tdma_duration_adjust(btcoexist, false, true, 3);
+		}
+
+		/*  sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			sw_mechanism1(btcoexist, true, false, false, false);
+			sw_mechanism2(btcoexist, true, false, false, 0x18);
+		} else {
+			sw_mechanism1(btcoexist, true, false, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		};
+	} else {
+		/*  fw mechanism */
+		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			if (bt_info_ext & BIT(0))	/* a2dp basic rate */
+				tdma_duration_adjust(btcoexist, false,
+						     false, 3);
+			else				/* a2dp edr rate */
+				tdma_duration_adjust(btcoexist, false,
+						     false, 3);
+		} else {
+			if (bt_info_ext & BIT(0))	/* a2dp basic rate */
+				tdma_duration_adjust(btcoexist, false, true, 3);
+			else				/* a2dp edr rate */
+				tdma_duration_adjust(btcoexist, false, true, 3);
+		}
+
+		/*  sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			sw_mechanism1(btcoexist, false, false, false, false);
+			sw_mechanism2(btcoexist, true, false, false, 0x18);
+		} else {
+			sw_mechanism1(btcoexist, false, false, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+static void halbtc8821a2ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state;
+	u32 wifi_bw;
+
+	wifi_rssi_state = wifi21a_rssi_state(btcoexist, 0, 2, 15, 0);
+	bt_rssi_state = halbtc8821a2ant_bt_rssi_state(2, 35, 0);
+
+	halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (halbtc8821a2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if (BTC_WIFI_BW_LEGACY == wifi_bw) /* for HID at 11b/g mode */
+		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
+					   0x5a5f5a5f, 0xffff, 0x3);
+	else  /* for HID quality & wifi performance balance at 11n mode */
+		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
+					   0x5a5f5a5f, 0xffff, 0x3);
+
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 3);
+		/*  fw mechanism */
+		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+			ps21a_tdma(btcoexist, NORMAL_EXEC, true, 10);
+		else
+			ps21a_tdma(btcoexist, NORMAL_EXEC, true, 14);
+
+		/*  sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			sw_mechanism1(btcoexist, true, true, false, false);
+			sw_mechanism2(btcoexist, true, false, false, 0x18);
+		} else {
+			sw_mechanism1(btcoexist, true, true, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	} else {
+		halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+		/*  fw mechanism */
+		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+			ps21a_tdma(btcoexist, NORMAL_EXEC, true, 10);
+		else
+			ps21a_tdma(btcoexist, NORMAL_EXEC, true, 14);
+
+		/*  sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			sw_mechanism1(btcoexist, false, true, false, false);
+			sw_mechanism2(btcoexist, true, false, false, 0x18);
+		} else {
+			sw_mechanism1(btcoexist, false, true, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+/*  HID+A2DP+PAN(EDR) */
+static void action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state, bt_info_ext;
+	u32 wifi_bw;
+
+	bt_info_ext = coex_sta->bt_info_ext;
+	wifi_rssi_state = wifi21a_rssi_state(btcoexist, 0, 2, 15, 0);
+	bt_rssi_state = halbtc8821a2ant_bt_rssi_state(2, 35, 0);
+
+	halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+
+	if (halbtc8821a2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if (BTC_WIFI_BW_LEGACY == wifi_bw) /* for HID at 11b/g mode */
+		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
+					   0x5a5a5a5a, 0xffff, 0x3);
+	else  /* for HID quality & wifi performance balance at 11n mode */
+		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
+					   0x5a5a5a5a, 0xffff, 0x3);
+
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		/*  fw mechanism */
+		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			if (bt_info_ext & BIT(0)) {	/* a2dp basic rate */
+				tdma_duration_adjust(btcoexist, true, true, 3);
+			} else {
+				/* a2dp edr rate */
+				tdma_duration_adjust(btcoexist, true, true, 3);
+			}
+		} else {
+			if (bt_info_ext & BIT(0))	/* a2dp basic rate */
+				tdma_duration_adjust(btcoexist, true, true, 3);
+			else				/* a2dp edr rate */
+				tdma_duration_adjust(btcoexist, true, true, 3);
+		}
+
+		/*  sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			sw_mechanism1(btcoexist, true, true, false, false);
+			sw_mechanism2(btcoexist, true, false, false, 0x18);
+		} else {
+			sw_mechanism1(btcoexist, true, true, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	} else {
+		/*  fw mechanism */
+		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			if (bt_info_ext & BIT(0))	/* a2dp basic rate */
+				tdma_duration_adjust(btcoexist, true, false, 3);
+			else				/* a2dp edr rate */
+				tdma_duration_adjust(btcoexist, true, false, 3);
+		} else {
+			if (bt_info_ext & BIT(0)) {
+				/* a2dp basic rate */
+				tdma_duration_adjust(btcoexist, true, true, 3);
+			} else				/* a2dp edr rate */ {
+				tdma_duration_adjust(btcoexist, true, true, 3);
+			}
+		}
+
+		/*  sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			sw_mechanism1(btcoexist, false, true, false, false);
+			sw_mechanism2(btcoexist, true, false, false, 0x18);
+		} else {
+			sw_mechanism1(btcoexist, false, true, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+static void halbtc8821a2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
+{
+	u8 wifi_rssi_state, bt_rssi_state, bt_info_ext;
+	u32 wifi_bw;
+
+	bt_info_ext = coex_sta->bt_info_ext;
+	wifi_rssi_state = wifi21a_rssi_state(btcoexist, 0, 2, 15, 0);
+	bt_rssi_state = halbtc8821a2ant_bt_rssi_state(2, 35, 0);
+
+	if (halbtc8821a2ant_need_to_dec_bt_pwr(btcoexist))
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
+	else
+		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+
+	if (BTC_WIFI_BW_LEGACY == wifi_bw) /* for HID at 11b/g mode */
+		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
+					   0x5f5b5f5b, 0xffffff, 0x3);
+	else  /* for HID quality & wifi performance balance at 11n mode */
+		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
+					   0x5f5b5f5b, 0xffffff, 0x3);
+
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		/*  fw mechanism */
+		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			if (bt_info_ext & BIT(0))	/* a2dp basic rate */
+				tdma_duration_adjust(btcoexist, true, true, 2);
+			else				/* a2dp edr rate */
+				tdma_duration_adjust(btcoexist, true, true, 2);
+		} else {
+			if (bt_info_ext & BIT(0))	/* a2dp basic rate */
+				tdma_duration_adjust(btcoexist, true, true, 2);
+			else				/* a2dp edr rate */
+				tdma_duration_adjust(btcoexist, true, true, 2);
+		}
+
+		/*  sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			sw_mechanism1(btcoexist, true, true, false, false);
+			sw_mechanism2(btcoexist, true, false, false, 0x18);
+		} else {
+			sw_mechanism1(btcoexist, true, true, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	} else {
+		/*  fw mechanism */
+		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			if (bt_info_ext & BIT(0))	/* a2dp basic rate */
+				tdma_duration_adjust(btcoexist, true, true, 2);
+			else				/* a2dp edr rate */
+				tdma_duration_adjust(btcoexist, true, true, 2);
+		} else {
+			if (bt_info_ext & BIT(0))	/* a2dp basic rate */
+				tdma_duration_adjust(btcoexist, true, true, 2);
+			else				/* a2dp edr rate */
+				tdma_duration_adjust(btcoexist, true, true, 2);
+		}
+
+		/*  sw mechanism */
+		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			sw_mechanism1(btcoexist, false, true, false, false);
+			sw_mechanism2(btcoexist, true, false, false, 0x18);
+		} else {
+			sw_mechanism1(btcoexist, false, true, false, false);
+			sw_mechanism2(btcoexist, false, false, false, 0x18);
+		}
+	}
+}
+
+static void halbtc8821a2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
+{
+	bool wifi_under_5g = false;
+	u8 algorithm = 0;
+
+	if (btcoexist->manual_control) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Manual control!!!\n");
+		return;
+	}
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under_5g);
+
+	if (wifi_under_5g) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], RunCoexistMechanism(), run 5G coex setting!!<===\n");
+		halbtc8821a2ant_coex_under_5g(btcoexist);
+		return;
+	}
+
+	algorithm = halbtc8821a2ant_action_algorithm(btcoexist);
+	if (coex_sta->c2h_bt_inquiry_page &&
+	    (BT_8821A_2ANT_COEX_ALGO_PANHS != algorithm)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], BT is under inquiry/page scan !!\n");
+		halbtc8821a2ant_bt_inquiry_page(btcoexist);
+		return;
+	}
+
+	coex_dm->cur_algorithm = algorithm;
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+		  "[BTCoex], Algorithm = %d\n", coex_dm->cur_algorithm);
+
+	if (halbtc8821a2ant_is_common_action(btcoexist)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Action 2-Ant common.\n");
+		coex_dm->reset_tdma_adjust = true;
+	} else {
+		if (coex_dm->cur_algorithm != coex_dm->pre_algorithm) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], pre_algorithm =%d, cur_algorithm =%d\n",
+				  coex_dm->pre_algorithm,
+				  coex_dm->cur_algorithm);
+			coex_dm->reset_tdma_adjust = true;
+		}
+		switch (coex_dm->cur_algorithm) {
+		case BT_8821A_2ANT_COEX_ALGO_SCO:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = SCO.\n");
+			halbtc8821a2ant_action_sco(btcoexist);
+			break;
+		case BT_8821A_2ANT_COEX_ALGO_HID:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = HID.\n");
+			halbtc8821a2ant_action_hid(btcoexist);
+			break;
+		case BT_8821A_2ANT_COEX_ALGO_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = A2DP.\n");
+			halbtc8821a2ant_action_a2dp(btcoexist);
+			break;
+		case BT_8821A_2ANT_COEX_ALGO_A2DP_PANHS:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = A2DP+PAN(HS).\n");
+			halbtc8821a2ant_action_a2dp_pan_hs(btcoexist);
+			break;
+		case BT_8821A_2ANT_COEX_ALGO_PANEDR:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = PAN(EDR).\n");
+			halbtc8821a2ant_action_pan_edr(btcoexist);
+			break;
+		case BT_8821A_2ANT_COEX_ALGO_PANHS:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = HS mode.\n");
+			halbtc8821a2ant_action_pan_hs(btcoexist);
+			break;
+		case BT_8821A_2ANT_COEX_ALGO_PANEDR_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = PAN+A2DP.\n");
+			halbtc8821a2ant_action_pan_edr_a2dp(btcoexist);
+			break;
+		case BT_8821A_2ANT_COEX_ALGO_PANEDR_HID:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = PAN(EDR)+HID.\n");
+			halbtc8821a2ant_action_pan_edr_hid(btcoexist);
+			break;
+		case BT_8821A_2ANT_COEX_ALGO_HID_A2DP_PANEDR:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = HID+A2DP+PAN.\n");
+			action_hid_a2dp_pan_edr(btcoexist);
+			break;
+		case BT_8821A_2ANT_COEX_ALGO_HID_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = HID+A2DP.\n");
+			halbtc8821a2ant_action_hid_a2dp(btcoexist);
+			break;
+		default:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Action 2-Ant, algorithm = coexist All Off!!\n");
+			halbtc8821a2ant_coex_all_off(btcoexist);
+			break;
+		}
+		coex_dm->pre_algorithm = coex_dm->cur_algorithm;
+	}
+}
+
+/*  work around function start with wa_halbtc8821a2ant_ */
+/*  extern function start with EXhalbtc8821a2ant_ */
+void ex_halbtc8821a2ant_init_hwconfig(struct btc_coexist *btcoexist)
+{
+	u8 u1tmp = 0;
+
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+		  "[BTCoex], 2Ant Init HW Config!!\n");
+
+	/*  backup rf 0x1e value */
+	coex_dm->bt_rf0x1e_backup =
+		btcoexist->btc_get_rf_reg(btcoexist, BTC_RF_A, 0x1e, 0xfffff);
+
+	/*  0x790[5:0] = 0x5 */
+	u1tmp = btcoexist->btc_read_1byte(btcoexist, 0x790);
+	u1tmp &= 0xc0;
+	u1tmp |= 0x5;
+	btcoexist->btc_write_1byte(btcoexist, 0x790, u1tmp);
+
+	/* Antenna config */
+	halbtc8821a2ant_set_ant_path(btcoexist, BTC_ANT_WIFI_AT_MAIN,
+				     true, false);
+
+	/*  PTA parameter */
+	halbtc8821a2ant_coex_table(btcoexist, FORCE_EXEC,
+				   0x55555555, 0x55555555,
+				   0xffff, 0x3);
+
+	/*  Enable counter statistics */
+	/* 0x76e[3] = 1, WLAN_Act control by PTA */
+	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
+	btcoexist->btc_write_1byte(btcoexist, 0x778, 0x3);
+	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x40, 0x20, 0x1);
+}
+
+void ex_halbtc8821a2ant_init_coex_dm(struct btc_coexist *btcoexist)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+		  "[BTCoex], Coex Mechanism Init!!\n");
+
+	halbtc8821a2ant_init_coex_dm(btcoexist);
+}
+
+void ex_halbtc8821a2ant_display_coex_info(struct btc_coexist *btcoexist)
+{
+	struct btc_board_info *board_info = &btcoexist->board_info;
+	struct btc_stack_info *stack_info = &btcoexist->stack_info;
+	u8 *cli_buf = btcoexist->cli_buf;
+	u8 u1tmp[4], i, bt_info_ext, ps_tdma_case = 0;
+	u32 u4tmp[4];
+	bool roam = false, scan = false, link = false, wifi_under_5g = false;
+	bool bt_hs_on = false, wifi_busy = false;
+	long wifi_rssi = 0, bt_hs_rssi = 0;
+	u32 wifi_bw, wifi_traffic_dir;
+	u8 wifi_dot_11_chnl, wifi_hs_chnl;
+	u32 fw_ver = 0, bt_patch_ver = 0;
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\n ============[BT Coexist info] ============");
+	CL_PRINTF(cli_buf);
+
+	if (!board_info->bt_exist) {
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n BT not exists !!!");
+		CL_PRINTF(cli_buf);
+		return;
+	}
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d/ %d ",
+		   "Ant PG number/ Ant mechanism: ",
+		   board_info->pg_ant_num, board_info->btdm_ant_num);
+	CL_PRINTF(cli_buf);
+
+	if (btcoexist->manual_control) {
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s",
+			   "[Action Manual control]!!");
+		CL_PRINTF(cli_buf);
+	}
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %s / %d",
+		   "BT stack/ hci ext ver",
+		   ((stack_info->profile_notified) ? "Yes" : "No"),
+		   stack_info->hci_version);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER, &bt_patch_ver);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d_%d/ 0x%x/ 0x%x(%d)",
+		   "CoexVer/ FwVer/ PatchVer",
+		   glcoex_ver_date_8821a_2ant, glcoex_ver_8821a_2ant,
+		   fw_ver, bt_patch_ver, bt_patch_ver);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_DOT11_CHNL,
+			   &wifi_dot_11_chnl);
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifi_hs_chnl);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d / %d(%d)",
+		   "Dot11 channel / HsMode(HsChnl)",
+		   wifi_dot_11_chnl, bt_hs_on, wifi_hs_chnl);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\n %-35s = %02x %02x %02x ", "H2C Wifi inform bt chnl Info",
+		   coex_dm->wifi_chnl_info[0], coex_dm->wifi_chnl_info[1],
+		   coex_dm->wifi_chnl_info[2]);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
+	btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\n %-35s = %ld/ %ld", "Wifi rssi/ HS rssi",
+		   wifi_rssi, bt_hs_rssi);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d/ %d/ %d ",
+		   "Wifi link/ roam/ scan",
+		   link, roam, scan);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under_5g);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION,
+			   &wifi_traffic_dir);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %s / %s/ %s ",
+		   "Wifi status",
+		   (wifi_under_5g ? "5G" : "2.4G"),
+		   ((BTC_WIFI_BW_LEGACY == wifi_bw) ? "Legacy" :
+		    (((BTC_WIFI_BW_HT40 == wifi_bw) ? "HT40" : "HT20"))),
+		   ((!wifi_busy) ? "idle" :
+		    ((BTC_WIFI_TRAFFIC_TX == wifi_traffic_dir) ? "uplink" :
+		    "downlink")));
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = [%s/ %d/ %d] ",
+		   "BT [status/ rssi/ retryCnt]",
+		   ((coex_sta->c2h_bt_inquiry_page) ? ("inquiry/page scan") :
+		   ((BT_8821A_2ANT_BT_STATUS_IDLE == coex_dm->bt_status) ?
+		    "idle" : ((BT_8821A_2ANT_BT_STATUS_CONNECTED_IDLE ==
+		    coex_dm->bt_status) ? "connected-idle" : "busy"))),
+		coex_sta->bt_rssi, coex_sta->bt_retry_cnt);
+	CL_PRINTF(cli_buf);
+
+	if (stack_info->profile_notified) {
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\n %-35s = %d / %d / %d / %d", "SCO/HID/PAN/A2DP",
+			   stack_info->sco_exist, stack_info->hid_exist,
+			   stack_info->pan_exist, stack_info->a2dp_exist);
+		CL_PRINTF(cli_buf);
+
+		btcoexist->btc_disp_dbg_msg(btcoexist,
+					    BTC_DBG_DISP_BT_LINK_INFO);
+	}
+
+	bt_info_ext = coex_sta->bt_info_ext;
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %s",
+		   "BT Info A2DP rate",
+		   (bt_info_ext & BIT(0)) ? "Basic rate" : "EDR rate");
+	CL_PRINTF(cli_buf);
+
+	for (i = 0; i < BT_INFO_SRC_8821A_2ANT_MAX; i++) {
+		if (coex_sta->bt_info_c2h_cnt[i]) {
+			CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+				   "\n %-35s = %02x %02x %02x %02x %02x %02x %02x(%d)",
+				   glbt_info_src_8821a_2ant[i],
+				   coex_sta->bt_info_c2h[i][0],
+				   coex_sta->bt_info_c2h[i][1],
+				   coex_sta->bt_info_c2h[i][2],
+				   coex_sta->bt_info_c2h[i][3],
+				   coex_sta->bt_info_c2h[i][4],
+				   coex_sta->bt_info_c2h[i][5],
+				   coex_sta->bt_info_c2h[i][6],
+				   coex_sta->bt_info_c2h_cnt[i]);
+			CL_PRINTF(cli_buf);
+		}
+	}
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %s/%s",
+		   "PS state, IPS/LPS",
+		   ((coex_sta->under_ips ? "IPS ON" : "IPS OFF")),
+		   ((coex_sta->under_lps ? "LPS ON" : "LPS OFF")));
+	CL_PRINTF(cli_buf);
+	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_FW_PWR_MODE_CMD);
+
+	/*  Sw mechanism	 */
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s",
+		   "============[Sw mechanism] ============");
+	CL_PRINTF(cli_buf);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d/ %d/ %d/ %d ",
+		   "SM1[ShRf/ LpRA/ LimDig/ btLna]",
+		   coex_dm->cur_rf_rx_lpf_shrink, coex_dm->cur_low_penalty_ra,
+		   coex_dm->limited_dig, coex_dm->cur_bt_lna_constrain);
+	CL_PRINTF(cli_buf);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d/ %d/ %d(0x%x) ",
+		   "SM2[AgcT/ AdcB/ SwDacSwing(lvl)]",
+		   coex_dm->cur_agc_table_en, coex_dm->cur_adc_back_off,
+		   coex_dm->cur_dac_swing_on, coex_dm->cur_dac_swing_lvl);
+	CL_PRINTF(cli_buf);
+
+	/*  Fw mechanism		 */
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s",
+		   "============[Fw mechanism] ============");
+	CL_PRINTF(cli_buf);
+
+	if (!btcoexist->manual_control) {
+		ps_tdma_case = coex_dm->cur_ps_tdma;
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			   "\n %-35s = %02x %02x %02x %02x %02x case-%d",
+			   "PS TDMA",
+			   coex_dm->ps_tdma_para[0], coex_dm->ps_tdma_para[1],
+			   coex_dm->ps_tdma_para[2], coex_dm->ps_tdma_para[3],
+			   coex_dm->ps_tdma_para[4], ps_tdma_case);
+		CL_PRINTF(cli_buf);
+
+		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d/ %d ",
+			   "DecBtPwr/ IgnWlanAct",
+			   coex_dm->cur_dec_bt_pwr,
+			   coex_dm->cur_ignore_wlan_act);
+		CL_PRINTF(cli_buf);
+	}
+
+	/*  Hw setting		 */
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\n %-35s", "============[Hw setting] ============");
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x",
+		   "RF-A, 0x1e initVal",
+		   coex_dm->bt_rf0x1e_backup);
+	CL_PRINTF(cli_buf);
+
+	u1tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x778);
+	u1tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x6cc);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x/ 0x%x ",
+		   "0x778 (W_Act)/ 0x6cc (CoTab Sel)",
+		   u1tmp[0], u1tmp[1]);
+	CL_PRINTF(cli_buf);
+
+	u1tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x8db);
+	u1tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0xc5b);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x/ 0x%x",
+		   "0x8db(ADC)/0xc5b[29:25](DAC)",
+		   ((u1tmp[0]&0x60)>>5), ((u1tmp[1]&0x3e)>>1));
+	CL_PRINTF(cli_buf);
+
+	u4tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xcb4);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x/ 0x%x",
+		   "0xcb4[7:0](ctrl)/ 0xcb4[29:28](val)",
+		   u4tmp[0]&0xff, ((u4tmp[0]&0x30000000)>>28));
+	CL_PRINTF(cli_buf);
+
+	u1tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x40);
+	u4tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x4c);
+	u4tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x974);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x/ 0x%x/ 0x%x",
+		   "0x40/ 0x4c[24:23]/ 0x974",
+		   u1tmp[0], ((u4tmp[0]&0x01800000)>>23), u4tmp[1]);
+	CL_PRINTF(cli_buf);
+
+	u4tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x550);
+	u1tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x522);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x/ 0x%x",
+		   "0x550(bcn ctrl)/0x522",
+		   u4tmp[0], u1tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	u4tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xc50);
+	u1tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0xa0a);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x/ 0x%x",
+		   "0xc50(DIG)/0xa0a(CCK-TH)",
+		   u4tmp[0], u1tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	u4tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xf48);
+	u1tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0xa5b);
+	u1tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0xa5c);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		   "\n %-35s = 0x%x/ 0x%x", "OFDM-FA/ CCK-FA",
+		   u4tmp[0], (u1tmp[0]<<8) + u1tmp[1]);
+	CL_PRINTF(cli_buf);
+
+	u4tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x6c0);
+	u4tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x6c4);
+	u4tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x6c8);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x/ 0x%x/ 0x%x",
+		   "0x6c0/0x6c4/0x6c8",
+		   u4tmp[0], u4tmp[1], u4tmp[2]);
+	CL_PRINTF(cli_buf);
+
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d/ %d",
+		   "0x770 (hi-pri Rx/Tx)",
+		   coex_sta->high_priority_rx, coex_sta->high_priority_tx);
+	CL_PRINTF(cli_buf);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d/ %d",
+		   "0x774(low-pri Rx/Tx)",
+		   coex_sta->low_priority_rx, coex_sta->low_priority_tx);
+	CL_PRINTF(cli_buf);
+
+	/*  Tx mgnt queue hang or not, 0x41b should = 0xf, ex: 0xd ==>hang */
+	u1tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x41b);
+	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x",
+		   "0x41b (mgntQ hang chk == 0xf)",
+		   u1tmp[0]);
+	CL_PRINTF(cli_buf);
+
+	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_COEX_STATISTICS);
+}
+
+void ex_halbtc8821a2ant_ips_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	if (BTC_IPS_ENTER == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], IPS ENTER notify\n");
+		coex_sta->under_ips = true;
+		halbtc8821a2ant_coex_all_off(btcoexist);
+	} else if (BTC_IPS_LEAVE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], IPS LEAVE notify\n");
+		coex_sta->under_ips = false;
+	}
+}
+
+void ex_halbtc8821a2ant_lps_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	if (BTC_LPS_ENABLE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], LPS ENABLE notify\n");
+		coex_sta->under_lps = true;
+	} else if (BTC_LPS_DISABLE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], LPS DISABLE notify\n");
+		coex_sta->under_lps = false;
+	}
+}
+
+void ex_halbtc8821a2ant_scan_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	if (BTC_SCAN_START == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], SCAN START notify\n");
+	else if (BTC_SCAN_FINISH == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], SCAN FINISH notify\n");
+}
+
+void ex_halbtc8821a2ant_connect_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	if (BTC_ASSOCIATE_START == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], CONNECT START notify\n");
+	else if (BTC_ASSOCIATE_FINISH == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], CONNECT FINISH notify\n");
+}
+
+void ex_halbtc8821a2ant_media_status_notify(struct btc_coexist *btcoexist,
+					    u8 type)
+{
+	u8 h2c_parameter[3] = {0};
+	u32 wifi_bw;
+	u8 wifi_central_chnl;
+
+	if (BTC_MEDIA_CONNECT == type)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], MEDIA connect notify\n");
+	else
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], MEDIA disconnect notify\n");
+
+	/*  only 2.4G we need to inform bt the chnl mask */
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_CENTRAL_CHNL,
+			   &wifi_central_chnl);
+	if ((BTC_MEDIA_CONNECT == type) &&
+	    (wifi_central_chnl <= 14)) {
+		h2c_parameter[0] = 0x1;
+		h2c_parameter[1] = wifi_central_chnl;
+		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+		if (BTC_WIFI_BW_HT40 == wifi_bw)
+			h2c_parameter[2] = 0x30;
+		else
+			h2c_parameter[2] = 0x20;
+	}
+
+	coex_dm->wifi_chnl_info[0] = h2c_parameter[0];
+	coex_dm->wifi_chnl_info[1] = h2c_parameter[1];
+	coex_dm->wifi_chnl_info[2] = h2c_parameter[2];
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], FW write 0x66 = 0x%x\n",
+		  h2c_parameter[0] << 16 |
+		  h2c_parameter[1] << 8 | h2c_parameter[2]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x66, 3, h2c_parameter);
+}
+
+void ex_halbtc8821a2ant_special_packet_notify(struct btc_coexist *btcoexist,
+					      u8 type)
+{
+	if (type == BTC_PACKET_DHCP)
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+			  "[BTCoex], DHCP Packet notify\n");
+}
+
+void ex_halbtc8821a2ant_bt_info_notify(struct btc_coexist *btcoexist,
+				       u8 *tmp_buf, u8 length)
+{
+	u8 bt_info = 0;
+	u8 i, rsp_source = 0;
+	static u32 set_bt_lna_cnt, set_bt_psd_mode;
+	bool bt_busy = false, limited_dig = false;
+	bool wifi_connected = false, bt_hs_on = false;
+
+	coex_sta->c2h_bt_info_req_sent = false;
+	rsp_source = tmp_buf[0]&0xf;
+	if (rsp_source >= BT_INFO_SRC_8821A_2ANT_MAX)
+		rsp_source = BT_INFO_SRC_8821A_2ANT_WIFI_FW;
+	coex_sta->bt_info_c2h_cnt[rsp_source]++;
+
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+		  "[BTCoex], Bt info[%d], length =%d, hex data =[",
+		  rsp_source, length);
+	for (i = 0; i < length; i++) {
+		coex_sta->bt_info_c2h[rsp_source][i] = tmp_buf[i];
+		if (i == 1)
+			bt_info = tmp_buf[i];
+		if (i == length-1)
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, "0x%02x]\n",
+				  tmp_buf[i]);
+		else
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, "0x%02x, ",
+				  tmp_buf[i]);
+	}
+
+	if (BT_INFO_SRC_8821A_2ANT_WIFI_FW != rsp_source) {
+		coex_sta->bt_retry_cnt =	/*  [3:0] */
+			coex_sta->bt_info_c2h[rsp_source][2]&0xf;
+		coex_sta->bt_rssi =
+			coex_sta->bt_info_c2h[rsp_source][3]*2+10;
+		coex_sta->bt_info_ext =
+			coex_sta->bt_info_c2h[rsp_source][4];
+
+		/*  Here we need to resend some wifi info to BT */
+		/*  because bt is reset and loss of the info. */
+		if ((coex_sta->bt_info_ext & BIT(1))) {
+			btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+					   &wifi_connected);
+			if (wifi_connected)
+				ex_halbtc8821a2ant_media_status_notify(btcoexist,
+								       BTC_MEDIA_CONNECT);
+			else
+				ex_halbtc8821a2ant_media_status_notify(btcoexist,
+								       BTC_MEDIA_DISCONNECT);
+
+			set_bt_psd_mode = 0;
+		}
+		if (set_bt_psd_mode <= 3) {
+			/* fix CH-BW mode  */
+			halbtc8821a2ant_set_bt_psd_mode(btcoexist,
+							FORCE_EXEC, 0x0);
+			set_bt_psd_mode++;
+		}
+
+		if (coex_dm->cur_bt_lna_constrain) {
+			if (!(coex_sta->bt_info_ext & BIT(2))) {
+				if (set_bt_lna_cnt <= 3) {
+					set_bt_lna_constrain(btcoexist,
+							     FORCE_EXEC, true);
+					set_bt_lna_cnt++;
+				}
+			}
+		} else {
+			set_bt_lna_cnt = 0;
+		}
+
+		if ((coex_sta->bt_info_ext & BIT(3)))
+			halbtc8821a2ant_ignore_wlan_act(btcoexist,
+							FORCE_EXEC, false);
+		else
+			/* BT already NOT ignore Wlan active, do nothing here */
+
+		if (!(coex_sta->bt_info_ext & BIT(4)))
+			halbtc8821a2ant_bt_auto_report(btcoexist,
+						       FORCE_EXEC, true);
+	}
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	/*  check BIT(2) first ==> check if bt is under inquiry or page scan */
+	if (bt_info & BT_INFO_8821A_2ANT_B_INQ_PAGE) {
+		coex_sta->c2h_bt_inquiry_page = true;
+		coex_dm->bt_status = BT_8821A_2ANT_BT_STATUS_NON_IDLE;
+	} else {
+		coex_sta->c2h_bt_inquiry_page = false;
+		if (bt_info == 0x1) {	/*  connection exists but not busy */
+			coex_sta->bt_link_exist = true;
+			coex_dm->bt_status =
+				BT_8821A_2ANT_BT_STATUS_CONNECTED_IDLE;
+		} else if (bt_info & BT_INFO_8821A_2ANT_B_CONNECTION) {
+			/*  connection exists and some link is busy */
+			coex_sta->bt_link_exist = true;
+			if (bt_info & BT_INFO_8821A_2ANT_B_FTP)
+				coex_sta->pan_exist = true;
+			else
+				coex_sta->pan_exist = false;
+			if (bt_info & BT_INFO_8821A_2ANT_B_A2DP)
+				coex_sta->a2dp_exist = true;
+			else
+				coex_sta->a2dp_exist = false;
+			if (bt_info & BT_INFO_8821A_2ANT_B_HID)
+				coex_sta->hid_exist = true;
+			else
+				coex_sta->hid_exist = false;
+			if (bt_info & BT_INFO_8821A_2ANT_B_SCO_ESCO)
+				coex_sta->sco_exist = true;
+			else
+				coex_sta->sco_exist = false;
+			coex_dm->bt_status = BT_8821A_2ANT_BT_STATUS_NON_IDLE;
+		} else {
+			coex_sta->bt_link_exist = false;
+			coex_sta->pan_exist = false;
+			coex_sta->a2dp_exist = false;
+			coex_sta->hid_exist = false;
+			coex_sta->sco_exist = false;
+			coex_dm->bt_status = BT_8821A_2ANT_BT_STATUS_IDLE;
+		}
+
+		if (bt_hs_on)
+			coex_dm->bt_status = BT_8821A_2ANT_BT_STATUS_NON_IDLE;
+	}
+
+	if (BT_8821A_2ANT_BT_STATUS_NON_IDLE == coex_dm->bt_status)
+		bt_busy = true;
+	else
+		bt_busy = false;
+	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bt_busy);
+
+	if (BT_8821A_2ANT_BT_STATUS_IDLE != coex_dm->bt_status)
+		limited_dig = true;
+	else
+		limited_dig = false;
+	coex_dm->limited_dig = limited_dig;
+	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_LIMITED_DIG, &limited_dig);
+
+	halbtc8821a2ant_run_coexist_mechanism(btcoexist);
+}
+
+void ex_halbtc8821a2ant_halt_notify(struct btc_coexist *btcoexist)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, "[BTCoex], Halt notify\n");
+
+	halbtc8821a2ant_ignore_wlan_act(btcoexist, FORCE_EXEC, true);
+	ex_halbtc8821a2ant_media_status_notify(btcoexist, BTC_MEDIA_DISCONNECT);
+}
+
+void ex_halbtc8821a2ant_periodical(struct btc_coexist *btcoexist)
+{
+	static u8 dis_ver_info_cnt;
+	u32 fw_ver = 0, bt_patch_ver = 0;
+	struct btc_board_info *board_info = &btcoexist->board_info;
+	struct btc_stack_info *stack_info = &btcoexist->stack_info;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+		  "[BTCoex], ========================== Periodical ===========================\n");
+
+	if (dis_ver_info_cnt <= 5) {
+		dis_ver_info_cnt += 1;
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], ****************************************************************\n");
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], Ant PG Num/ Ant Mech/ Ant Pos = %d/ %d/ %d\n",
+			  board_info->pg_ant_num, board_info->btdm_ant_num,
+			  board_info->btdm_ant_pos);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], BT stack/ hci ext ver = %s / %d\n",
+			  ((stack_info->profile_notified) ? "Yes" : "No"),
+			  stack_info->hci_version);
+		btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER,
+				   &bt_patch_ver);
+		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], CoexVer/ FwVer/ PatchVer = %d_%x/ 0x%x/ 0x%x(%d)\n",
+			  glcoex_ver_date_8821a_2ant,
+			  glcoex_ver_8821a_2ant,
+			  fw_ver, bt_patch_ver, bt_patch_ver);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+			  "[BTCoex], ****************************************************************\n");
+	}
+
+	halbtc8821a2ant_query_bt_info(btcoexist);
+	halbtc8821a2ant_monitor_bt_ctr(btcoexist);
+	monitor_bt_enable_disable(btcoexist);
+}
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbtc8821a2ant.h b/drivers/staging/rtl8192ee/btcoexist/halbtc8821a2ant.h
new file mode 100644
index 00000000000..745506b48d6
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/halbtc8821a2ant.h
@@ -0,0 +1,179 @@
+/*  */
+/*  The following is for 8821A 2Ant BT Co-exist definition */
+/*  */
+#define	BT_INFO_8821A_2ANT_B_FTP				BIT(7)
+#define	BT_INFO_8821A_2ANT_B_A2DP				BIT(6)
+#define	BT_INFO_8821A_2ANT_B_HID				BIT(5)
+#define	BT_INFO_8821A_2ANT_B_SCO_BUSY				BIT(4)
+#define	BT_INFO_8821A_2ANT_B_ACL_BUSY				BIT(3)
+#define	BT_INFO_8821A_2ANT_B_INQ_PAGE				BIT(2)
+#define	BT_INFO_8821A_2ANT_B_SCO_ESCO				BIT(1)
+#define	BT_INFO_8821A_2ANT_B_CONNECTION				BIT(0)
+
+#define		BTC_RSSI_COEX_THRESH_TOL_8821A_2ANT		2
+
+enum BT_INFO_SRC_8821A_2ANT {
+	BT_INFO_SRC_8821A_2ANT_WIFI_FW			= 0x0,
+	BT_INFO_SRC_8821A_2ANT_BT_RSP			= 0x1,
+	BT_INFO_SRC_8821A_2ANT_BT_ACTIVE_SEND		= 0x2,
+	BT_INFO_SRC_8821A_2ANT_MAX
+};
+
+enum BT_8821A_2ANT_BT_STATUS {
+	BT_8821A_2ANT_BT_STATUS_IDLE				= 0x0,
+	BT_8821A_2ANT_BT_STATUS_CONNECTED_IDLE	= 0x1,
+	BT_8821A_2ANT_BT_STATUS_NON_IDLE			= 0x2,
+	BT_8821A_2ANT_BT_STATUS_MAX
+};
+
+enum BT_8821A_2ANT_COEX_ALGO {
+	BT_8821A_2ANT_COEX_ALGO_UNDEFINED			= 0x0,
+	BT_8821A_2ANT_COEX_ALGO_SCO				= 0x1,
+	BT_8821A_2ANT_COEX_ALGO_HID				= 0x2,
+	BT_8821A_2ANT_COEX_ALGO_A2DP				= 0x3,
+	BT_8821A_2ANT_COEX_ALGO_A2DP_PANHS		= 0x4,
+	BT_8821A_2ANT_COEX_ALGO_PANEDR			= 0x5,
+	BT_8821A_2ANT_COEX_ALGO_PANHS			= 0x6,
+	BT_8821A_2ANT_COEX_ALGO_PANEDR_A2DP		= 0x7,
+	BT_8821A_2ANT_COEX_ALGO_PANEDR_HID		= 0x8,
+	BT_8821A_2ANT_COEX_ALGO_HID_A2DP_PANEDR	= 0x9,
+	BT_8821A_2ANT_COEX_ALGO_HID_A2DP			= 0xa,
+	BT_8821A_2ANT_COEX_ALGO_MAX				= 0xb,
+};
+
+struct coex_dm_8821a_2ant {
+	/*  fw mechanism */
+	bool		pre_dec_bt_pwr;
+	bool		cur_dec_bt_pwr;
+	bool		pre_bt_lna_constrain;
+	bool		cur_bt_lna_constrain;
+	u8		pre_bt_psd_mode;
+	u8		cur_bt_psd_mode;
+	u8		pre_fw_dac_swing_lvl;
+	u8		cur_fw_dac_swing_lvl;
+	bool		cur_ignore_wlan_act;
+	bool		pre_ignore_wlan_act;
+	u8		pre_ps_tdma;
+	u8		cur_ps_tdma;
+	u8		ps_tdma_para[5];
+	u8		ps_tdma_du_adj_type;
+	bool		reset_tdma_adjust;
+	bool		pre_ps_tdma_on;
+	bool		cur_ps_tdma_on;
+	bool		pre_bt_auto_report;
+	bool		cur_bt_auto_report;
+
+	/*  sw mechanism */
+	bool		pre_rf_rx_lpf_shrink;
+	bool		cur_rf_rx_lpf_shrink;
+	u32		bt_rf0x1e_backup;
+	bool	pre_low_penalty_ra;
+	bool		cur_low_penalty_ra;
+	bool		pre_dac_swing_on;
+	u32		pre_dac_swing_lvl;
+	bool		cur_dac_swing_on;
+	u32		cur_dac_swing_lvl;
+	bool		pre_adc_back_off;
+	bool		cur_adc_back_off;
+	bool	pre_agc_table_en;
+	bool		cur_agc_table_en;
+	u32		pre_val0x6c0;
+	u32		cur_val0x6c0;
+	u32		pre_val0x6c4;
+	u32		cur_val0x6c4;
+	u32		pre_val0x6c8;
+	u32		cur_val0x6c8;
+	u8		pre_val0x6cc;
+	u8		cur_val0x6cc;
+	bool		limited_dig;
+
+	/*  algorithm related */
+	u8		pre_algorithm;
+	u8		cur_algorithm;
+	u8		bt_status;
+	u8		wifi_chnl_info[3];
+};
+
+struct coex_sta_8821a_2ant {
+	bool		bt_link_exist;
+	bool		sco_exist;
+	bool		a2dp_exist;
+	bool		hid_exist;
+	bool		pan_exist;
+
+	bool		under_lps;
+	bool		under_ips;
+	u32		high_priority_tx;
+	u32		high_priority_rx;
+	u32		low_priority_tx;
+	u32		low_priority_rx;
+	u8		bt_rssi;
+	u8		pre_bt_rssi_state;
+	u8		pre_wifi_rssi_state[4];
+	bool		c2h_bt_info_req_sent;
+	u8		bt_info_c2h[BT_INFO_SRC_8821A_2ANT_MAX][10];
+	u32		bt_info_c2h_cnt[BT_INFO_SRC_8821A_2ANT_MAX];
+	bool		c2h_bt_inquiry_page;
+	u8		bt_retry_cnt;
+	u8		bt_info_ext;
+};
+
+/*  */
+/*  The following is interface which will notify coex module. */
+/*  */
+void
+ex_halbtc8821a2ant_init_hwconfig(
+	struct btc_coexist *btcoexist
+	);
+void
+ex_halbtc8821a2ant_init_coex_dm(
+	struct btc_coexist *btcoexist
+	);
+void
+ex_halbtc8821a2ant_ips_notify(
+	struct btc_coexist *btcoexist,
+	u8 type
+	);
+void
+ex_halbtc8821a2ant_lps_notify(
+	struct btc_coexist *btcoexist,
+	u8 type
+	);
+void
+ex_halbtc8821a2ant_scan_notify(
+	struct btc_coexist *btcoexist,
+	u8 type
+	);
+void
+ex_halbtc8821a2ant_connect_notify(
+	struct btc_coexist *btcoexist,
+	u8 type
+	);
+void
+ex_halbtc8821a2ant_media_status_notify(
+	struct btc_coexist *btcoexist,
+	u8 type
+	);
+void
+ex_halbtc8821a2ant_special_packet_notify(
+	struct btc_coexist *btcoexist,
+	u8 type
+	);
+void
+ex_halbtc8821a2ant_bt_info_notify(
+	struct btc_coexist *btcoexist,
+	u8 *tmp_buf,
+	u8 length
+	);
+void
+ex_halbtc8821a2ant_halt_notify(
+	struct btc_coexist *btcoexist
+	);
+void
+ex_halbtc8821a2ant_periodical(
+	struct btc_coexist *btcoexist
+	);
+void
+ex_halbtc8821a2ant_display_coex_info(
+	struct btc_coexist *btcoexist
+	);
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbtcoutsrc.c b/drivers/staging/rtl8192ee/btcoexist/halbtcoutsrc.c
new file mode 100644
index 00000000000..2d9fc24b1e2
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/halbtcoutsrc.c
@@ -0,0 +1,1297 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2013 Realtek Corporation. All rights reserved.
+ *
+ * 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.
+ *
+ * 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 "halbt_precomp.h"
+
+/***********************************************
+ *		Global variables
+ ***********************************************/
+static const char *const bt_profile_string[] = {
+	"NONE",
+	"A2DP",
+	"PAN",
+	"HID",
+	"SCO",
+};
+
+static const char *const bt_spec_string[] = {
+	"1.0b",
+	"1.1",
+	"1.2",
+	"2.0+EDR",
+	"2.1+EDR",
+	"3.0+HS",
+	"4.0",
+};
+
+static const char *const bt_link_role_string[] = {
+	"Master",
+	"Slave",
+};
+
+static const char *const h2c_state_string[] = {
+	"successful",
+	"h2c busy",
+	"rf off",
+	"fw not read",
+};
+
+static const char *const io_state_string[] = {
+	"IO_STATUS_SUCCESS",
+	"IO_STATUS_FAIL_CANNOT_IO",
+	"IO_STATUS_FAIL_RF_OFF",
+	"IO_STATUS_FAIL_FW_READ_CLEAR_TIMEOUT",
+	"IO_STATUS_FAIL_WAIT_IO_EVENT_TIMEOUT",
+	"IO_STATUS_INVALID_LEN",
+	"IO_STATUS_IO_IDLE_QUEUE_EMPTY",
+	"IO_STATUS_IO_INSERT_WAIT_QUEUE_FAIL",
+	"IO_STATUS_UNKNOWN_FAIL",
+	"IO_STATUS_WRONG_LEVEL",
+	"IO_STATUS_H2C_STOPPED",
+};
+
+struct btc_coexist gl92e_bt_coexist;
+
+u32 btc_92edbg_type[BTC_MSG_MAX];
+static u8 btc_dbg_buf[100];
+
+/***************************************************
+ *		Debug related function
+ ***************************************************/
+static bool is_any_client_connect_to_ap(struct btc_coexist *btcoexist)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_mac *mac = rtl_mac(rtlpriv);
+	struct rtl_sta_info *drv_priv;
+	u8 cnt = 0;
+
+	if (mac->opmode == NL80211_IFTYPE_ADHOC ||
+	    mac->opmode == NL80211_IFTYPE_MESH_POINT ||
+	    mac->opmode == NL80211_IFTYPE_AP) {
+		spin_lock_bh(&rtlpriv->locks.entry_list_lock);
+		list_for_each_entry(drv_priv, &rtlpriv->entry_list, list) {
+			cnt++;
+		}
+		spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
+	}
+	if (cnt > 0)
+		return true;
+	else
+		return false;
+}
+
+static bool halbtc_is_bt_coexist_available(struct btc_coexist *btcoexist)
+{
+	if (!btcoexist->binded || NULL == btcoexist->adapter)
+		return false;
+
+	return true;
+}
+
+static bool halbtc_is_wifi_busy(struct rtl_priv *rtlpriv)
+{
+	if (rtlpriv->link_info.b_busytraffic)
+		return true;
+	else
+		return false;
+}
+
+
+static void halbtc_dbg_init(void)
+{
+	u8 i;
+
+	for (i = 0; i < BTC_MSG_MAX; i++)
+		btc_92edbg_type[i] = 0;
+
+	btc_92edbg_type[BTC_MSG_INTERFACE] = 0;
+
+	btc_92edbg_type[BTC_MSG_ALGORITHM] = 0;
+}
+
+static bool halbtc_is_bt40(struct rtl_priv *adapter)
+{
+	struct rtl_priv *rtlpriv = adapter;
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	bool is_ht40 = true;
+	enum ht_channel_width bw = rtlphy->current_chan_bw;
+
+
+	if (bw == HT_CHANNEL_WIDTH_20)
+		is_ht40 = false;
+	else if (bw == HT_CHANNEL_WIDTH_20_40)
+		is_ht40 = true;
+
+	return is_ht40;
+}
+
+static bool halbtc_legacy(struct rtl_priv *adapter)
+{
+	struct rtl_priv *rtlpriv = adapter;
+	struct rtl_mac *mac = rtl_mac(rtlpriv);
+
+	bool is_legacy = false;
+
+	if ((mac->mode == WIRELESS_MODE_B) || (mac->mode == WIRELESS_MODE_G))
+		is_legacy = true;
+
+	return is_legacy;
+}
+
+bool halbtc92e_is_wifi_uplink(struct rtl_priv *adapter)
+{
+	struct rtl_priv *rtlpriv = adapter;
+
+	if (rtlpriv->link_info.b_tx_busy_traffic)
+		return true;
+	else
+		return false;
+}
+
+static u32 halbtc_get_wifi_bw(struct btc_coexist *btcoexist)
+{
+	struct rtl_priv *rtlpriv =
+		(struct rtl_priv *)btcoexist->adapter;
+	u32 wifi_bw = BTC_WIFI_BW_HT20;
+
+	if (halbtc_is_bt40(rtlpriv)) {
+		wifi_bw = BTC_WIFI_BW_HT40;
+	} else {
+		if (halbtc_legacy(rtlpriv))
+			wifi_bw = BTC_WIFI_BW_LEGACY;
+		else
+			wifi_bw = BTC_WIFI_BW_HT20;
+	}
+	return wifi_bw;
+}
+
+static u8 halbtc_get_wifi_central_chnl(struct btc_coexist *btcoexist)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_phy	*rtlphy = &(rtlpriv->phy);
+	u8 chnl = 1;
+
+
+	if (rtlphy->current_channel != 0)
+		chnl = rtlphy->current_channel;
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+		  "halbtc_get_wifi_central_chnl:%d\n", chnl);
+	return chnl;
+}
+
+static void halbtc_leave_lps(struct btc_coexist *btcoexist)
+{
+	struct rtl_priv *rtlpriv;
+	struct rtl_ps_ctl *ppsc;
+	bool ap_enable = false;
+
+	rtlpriv = btcoexist->adapter;
+	ppsc = rtl_psc(rtlpriv);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_AP_MODE_ENABLE,
+			   &ap_enable);
+
+	if (ap_enable) {
+		pr_debug("halbtc_leave_lps()<--dont leave lps under AP mode\n");
+		return;
+	}
+
+	btcoexist->bt_info.bt_ctrl_lps = true;
+	btcoexist->bt_info.bt_lps_on = false;
+	rtl92e_lps_leave(rtlpriv->mac80211.hw);
+}
+
+static void halbtc_enter_lps(struct btc_coexist *btcoexist)
+{
+	struct rtl_priv *rtlpriv;
+	struct rtl_ps_ctl *ppsc;
+	bool ap_enable = false;
+
+	rtlpriv = btcoexist->adapter;
+	ppsc = rtl_psc(rtlpriv);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_AP_MODE_ENABLE,
+			   &ap_enable);
+
+	if (ap_enable) {
+		pr_debug("halbtc_enter_lps()<--dont enter lps under AP mode\n");
+		return;
+	}
+
+	btcoexist->bt_info.bt_ctrl_lps = true;
+	btcoexist->bt_info.bt_lps_on = false;
+	rtl92e_lps_enter(rtlpriv->mac80211.hw);
+}
+
+static void halbtc_normal_lps(struct btc_coexist *btcoexist)
+{
+	if (btcoexist->bt_info.bt_ctrl_lps) {
+		btcoexist->bt_info.bt_lps_on = false;
+		btcoexist->bt_info.bt_ctrl_lps = false;
+	}
+}
+
+static void halbtc_aggregation_check(struct btc_coexist *btcoexist)
+{
+}
+
+static u32 halbtcoutsrc_get_wifi_link_status(struct btc_coexist *btcoexist)
+{
+	/*------------------------------------
+	 * return value:
+	 * [31:16] => connected port number
+	 * [15:0] => port connected bit define
+	 *------------------------------------
+	 */
+
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_mac *mac = rtl_mac(rtlpriv);
+	u32 ret_val = 0;
+	u32 port_connected_status = 0, num_of_connected_port = 0;
+
+	if (mac->opmode == NL80211_IFTYPE_STATION &&
+	    mac->link_state >= MAC80211_LINKED) {
+		port_connected_status |= WIFI_STA_CONNECTED;
+		num_of_connected_port++;
+	}
+	/* AP & ADHOC & MESH */
+	if (is_any_client_connect_to_ap(btcoexist)) {
+		port_connected_status |= WIFI_AP_CONNECTED;
+		num_of_connected_port++;
+	}
+	/*if (BT_HsConnectionEstablished(Adapter))
+	{
+		port_connected_status |= WIFI_HS_CONNECTED;
+		num_of_connected_port++;
+	}*/
+	/* TODO:
+	 * P2P Connected Status	*/
+
+	ret_val = (num_of_connected_port << 16) | port_connected_status;
+
+	return ret_val;
+}
+
+
+static u32 halbtc_get_bt_patch_version(struct btc_coexist *btcoexist)
+{
+	return 0;
+}
+
+static s32 halbtc_get_wifi_rssi(struct rtl_priv *adapter)
+{
+	struct rtl_priv *rtlpriv = adapter;
+	s32	undecorated_smoothed_pwdb = 0;
+
+	if (rtlpriv->mac80211.link_state >= MAC80211_LINKED)
+		undecorated_smoothed_pwdb =
+			rtlpriv->dm.undecorated_smoothed_pwdb;
+	else /* associated entry pwdb */
+		undecorated_smoothed_pwdb =
+			rtlpriv->dm.undecorated_smoothed_pwdb;
+	return undecorated_smoothed_pwdb;
+}
+
+static bool halbtc_get(void *void_btcoexist, u8 get_type, void *out_buf)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)void_btcoexist;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_mac *mac = rtl_mac(rtlpriv);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	bool *bool_tmp = (bool *)out_buf;
+	int *s32_tmp = (int *)out_buf;
+	u32 *u32_tmp = (u32 *)out_buf;
+	u8 *u8_tmp = (u8 *)out_buf;
+	bool tmp = false;
+
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return false;
+
+
+	switch (get_type) {
+	case BTC_GET_BL_HS_OPERATION:
+		*bool_tmp = false;
+		break;
+	case BTC_GET_BL_HS_CONNECTING:
+		*bool_tmp = false;
+		break;
+	case BTC_GET_BL_WIFI_CONNECTED:
+		if (rtlpriv->mac80211.opmode == NL80211_IFTYPE_STATION &&
+		    rtlpriv->mac80211.link_state >= MAC80211_LINKED)
+			tmp = true;
+		if (is_any_client_connect_to_ap(btcoexist))
+			tmp = true;
+		*bool_tmp = tmp;
+		break;
+	case BTC_GET_BL_WIFI_BUSY:
+		if (halbtc_is_wifi_busy(rtlpriv))
+			*bool_tmp = true;
+		else
+			*bool_tmp = false;
+		break;
+	case BTC_GET_BL_WIFI_SCAN:
+		if (mac->act_scanning)
+			*bool_tmp = true;
+		else
+			*bool_tmp = false;
+		break;
+	case BTC_GET_BL_WIFI_LINK:
+		if (mac->link_state == MAC80211_LINKING)
+			*bool_tmp = true;
+		else
+			*bool_tmp = false;
+		break;
+	case BTC_GET_BL_WIFI_ROAM:	/*TODO*/
+		*bool_tmp = false;
+		break;
+	case BTC_GET_BL_WIFI_4_WAY_PROGRESS:	/*TODO*/
+		*bool_tmp = false;
+		break;
+	case BTC_GET_BL_WIFI_UNDER_5G:
+		*bool_tmp = false; /*TODO*/
+
+	case BTC_GET_BL_WIFI_DHCP:	/*TODO*/
+		break;
+	case BTC_GET_BL_WIFI_SOFTAP_IDLE:
+		*bool_tmp = true;
+		break;
+	case BTC_GET_BL_WIFI_SOFTAP_LINKING:
+		*bool_tmp = false;
+		break;
+	case BTC_GET_BL_WIFI_IN_EARLY_SUSPEND:
+		*bool_tmp = false;
+		break;
+	case BTC_GET_BL_WIFI_AP_MODE_ENABLE:
+		*bool_tmp = false;
+		break;
+	case BTC_GET_BL_WIFI_ENABLE_ENCRYPTION:
+		if (NO_ENCRYPTION == rtlpriv->sec.pairwise_enc_algorithm)
+			*bool_tmp = false;
+		else
+			*bool_tmp = true;
+		break;
+	case BTC_GET_BL_WIFI_UNDER_B_MODE:
+		if (WIRELESS_MODE_B == rtlpriv->mac80211.mode)
+			*bool_tmp = true;
+		else
+			*bool_tmp = false;
+		break;
+	case BTC_GET_BL_EXT_SWITCH:
+		*bool_tmp = false;
+		break;
+	case BTC_GET_S4_WIFI_RSSI:
+		*s32_tmp = halbtc_get_wifi_rssi(rtlpriv);
+		break;
+	case BTC_GET_S4_HS_RSSI:	/*TODO*/
+		*s32_tmp = halbtc_get_wifi_rssi(rtlpriv);
+		break;
+	case BTC_GET_U4_WIFI_BW:
+		*u32_tmp = halbtc_get_wifi_bw(btcoexist);
+		break;
+	case BTC_GET_U4_WIFI_TRAFFIC_DIRECTION:
+		if (halbtc92e_is_wifi_uplink(rtlpriv))
+			*u32_tmp = BTC_WIFI_TRAFFIC_TX;
+		else
+			*u32_tmp = BTC_WIFI_TRAFFIC_RX;
+		break;
+	case BTC_GET_U4_WIFI_FW_VER:
+		*u32_tmp = (rtlhal->fw_version << 16) | rtlhal->fw_subversion;
+		break;
+	case BTC_GET_U4_WIFI_LINK_STATUS:
+		*u32_tmp = halbtcoutsrc_get_wifi_link_status(btcoexist);
+		break;
+	case BTC_GET_U4_BT_PATCH_VER:
+		*u32_tmp = halbtc_get_bt_patch_version(btcoexist);
+		break;
+	case BTC_GET_U1_WIFI_DOT11_CHNL:
+		*u8_tmp = rtlphy->current_channel;
+		break;
+	case BTC_GET_U1_WIFI_CENTRAL_CHNL:
+		*u8_tmp = halbtc_get_wifi_central_chnl(btcoexist);
+		break;
+	case BTC_GET_U1_WIFI_HS_CHNL:
+		*u8_tmp = 1;/* BT_OperateChnl(rtlpriv); */
+		break;
+	case BTC_GET_U1_MAC_PHY_MODE:
+		*u8_tmp = BTC_MP_UNKNOWN;
+		break;
+	case BTC_GET_U1_AP_NUM:
+		/* driver don't know AP num in Linux,
+		 * So, the return value here is not right */
+		*u8_tmp = 1;/* pDefMgntInfo->NumBssDesc4Query; */
+		break;
+
+	/************* 1Ant **************/
+	case BTC_GET_U1_LPS_MODE:
+		*u8_tmp = btcoexist->pwr_mode_val[0];
+		break;
+
+	default:
+		break;
+	}
+
+	return true;
+}
+
+static bool halbtc_set(void *void_btcoexist, u8 set_type, void *in_buf)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)void_btcoexist;
+	bool *bool_tmp = (bool *)in_buf;
+	u8 *u8_tmp = (u8 *)in_buf;
+	u32 *u32_tmp = (u32 *)in_buf;
+
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return false;
+
+	switch (set_type) {
+	/* set some bool type variables. */
+	case BTC_SET_BL_BT_DISABLE:
+		btcoexist->bt_info.bt_disabled = *bool_tmp;
+		break;
+	case BTC_SET_BL_BT_TRAFFIC_BUSY:
+		btcoexist->bt_info.bt_busy = *bool_tmp;
+		break;
+	case BTC_SET_BL_BT_LIMITED_DIG:
+		btcoexist->bt_info.limited_dig = *bool_tmp;
+		break;
+	case BTC_SET_BL_FORCE_TO_ROAM:
+		btcoexist->bt_info.force_to_roam = *bool_tmp;
+		break;
+	case BTC_SET_BL_TO_REJ_AP_AGG_PKT:
+		btcoexist->bt_info.reject_agg_pkt = *bool_tmp;
+		break;
+	case BTC_SET_BL_BT_CTRL_AGG_SIZE:
+		btcoexist->bt_info.b_bt_ctrl_buf_size = *bool_tmp;
+		break;
+	case BTC_SET_BL_INC_SCAN_DEV_NUM:
+		btcoexist->bt_info.increase_scan_dev_num = *bool_tmp;
+		break;
+		/* set some u1Byte type variables. */
+	case BTC_SET_U1_RSSI_ADJ_VAL_FOR_AGC_TABLE_ON:
+		btcoexist->bt_info.rssi_adjust_for_agc_table_on = *u8_tmp;
+		break;
+	case BTC_SET_U1_AGG_BUF_SIZE:
+		btcoexist->bt_info.agg_buf_size = *u8_tmp;
+		break;
+		/* the following are some action which will be triggered */
+	case BTC_SET_ACT_GET_BT_RSSI:
+		/*BTHCI_SendGetBtRssiEvent(rtlpriv);*/
+		break;
+	case BTC_SET_ACT_AGGREGATE_CTRL:
+		halbtc_aggregation_check(btcoexist);
+		break;
+
+		/* 1Ant */
+	case BTC_SET_U1_RSSI_ADJ_VAL_FOR_1ANT_COEX_TYPE:
+		btcoexist->bt_info.rssi_adjust_for_1ant_coex_type = *u8_tmp;
+		break;
+	case BTC_SET_U1_LPS_VAL:
+		btcoexist->bt_info.lps_val = *u8_tmp;
+		break;
+	case BTC_SET_U1_RPWM_VAL:
+		btcoexist->bt_info.rpwm_val = *u8_tmp;
+		break;
+	/* the following are some action which will be triggered  */
+	case BTC_SET_ACT_LEAVE_LPS:
+		halbtc_leave_lps(btcoexist);
+		break;
+	case BTC_SET_ACT_ENTER_LPS:
+		halbtc_enter_lps(btcoexist);
+		break;
+	case BTC_SET_ACT_NORMAL_LPS:
+		halbtc_normal_lps(btcoexist);
+		break;
+	case BTC_SET_ACT_DISABLE_LOW_POWER:
+		break;
+	case BTC_SET_ACT_UPDATE_ra_mask:
+		btcoexist->bt_info.ra_mask = *u32_tmp;
+		break;
+	case BTC_SET_ACT_SEND_MIMO_PS:
+		break;
+	case BTC_SET_ACT_INC_FORCE_EXEC_PWR_CMD_CNT:
+		btcoexist->bt_info.force_exec_pwr_cmd_cnt++;
+		break;
+	case BTC_SET_ACT_CTRL_BT_INFO: /*wait for 8812/8821*/
+		break;
+	case BTC_SET_ACT_CTRL_BT_COEX:
+		break;
+	default:
+		break;
+	}
+
+	return true;
+}
+
+static void halbtc_display_coex_statistics(struct btc_coexist *btcoexist)
+{
+}
+
+static void halbtc_display_bt_link_info(struct btc_coexist *btcoexist)
+{
+}
+
+static void halbtc_display_bt_fw_info(struct btc_coexist *btcoexist)
+{
+}
+
+static void halbtc_display_fw_pwr_mode_cmd(struct btc_coexist *btcoexist)
+{
+}
+
+/************************************************************
+ *		IO related function
+ ************************************************************/
+static u8 halbtc_read_1byte(void *bt_context, u32 reg_addr)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+
+	return	rtl_read_byte(rtlpriv, reg_addr);
+}
+
+
+static u16 halbtc_read_2byte(void *bt_context, u32 reg_addr)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+
+	return	rtl_read_word(rtlpriv, reg_addr);
+}
+
+
+static u32 halbtc_read_4byte(void *bt_context, u32 reg_addr)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+
+	return	rtl_read_dword(rtlpriv, reg_addr);
+}
+
+
+static void halbtc_write_1byte(void *bt_context, u32 reg_addr, u8 data)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+
+	rtl_write_byte(rtlpriv, reg_addr, data);
+}
+
+static void halbtc_bitmask_write_1byte(void *bt_context, u32 reg_addr,
+				       u8 bit_mask, u8 data)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	u8 original_value, bit_shift = 0;
+	u8 i;
+
+	if (bit_mask != MASKBYTE0) {/*if not "byte" write*/
+		original_value = rtl_read_byte(rtlpriv, reg_addr);
+		for (i = 0; i <= 7; i++) {
+			if ((bit_mask>>i)&0x1)
+				break;
+		}
+		bit_shift = i;
+		data = (original_value & (~bit_mask)) |
+			((data << bit_shift) & bit_mask);
+	}
+	rtl_write_byte(rtlpriv, reg_addr, data);
+}
+
+
+static void halbtc_write_2byte(void *bt_context, u32 reg_addr, u16 data)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+
+	rtl_write_word(rtlpriv, reg_addr, data);
+}
+
+
+static void halbtc_write_4byte(void *bt_context, u32 reg_addr, u32 data)
+{
+	struct btc_coexist *btcoexist =
+		(struct btc_coexist *)bt_context;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+
+	rtl_write_dword(rtlpriv, reg_addr, data);
+}
+
+
+static void halbtc_set_bbreg(void *bt_context, u32 reg_addr,
+			     u32 bit_mask, u32 data)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+
+	rtl_set_bbreg(rtlpriv->mac80211.hw, reg_addr, bit_mask, data);
+}
+
+
+static u32 halbtc_get_bbreg(void *bt_context, u32 reg_addr, u32 bit_mask)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+
+	return rtl_get_bbreg(rtlpriv->mac80211.hw, reg_addr, bit_mask);
+}
+
+
+static void halbtc_set_rfreg(void *bt_context, u8 rf_path, u32 reg_addr,
+			     u32 bit_mask, u32 data)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+
+	rtl_set_rfreg(rtlpriv->mac80211.hw, rf_path, reg_addr, bit_mask, data);
+}
+
+
+static u32 halbtc_get_rfreg(void *bt_context, u8 rf_path, u32 reg_addr,
+			    u32 bit_mask)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+
+	return rtl_get_rfreg(rtlpriv->mac80211.hw, rf_path, reg_addr, bit_mask);
+}
+
+
+static void halbtc_fill_h2c_cmd(void *bt_context, u8 element_id,
+				u32 cmd_len, u8 *cmd_buf)
+{
+	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+
+	rtlpriv->cfg->ops->fill_h2c_cmd(rtlpriv->mac80211.hw, element_id,
+					cmd_len, cmd_buf);
+}
+
+static void halbtc_display_dbg_msg(void *bt_context, u8 disp_type)
+{
+	struct btc_coexist *btcoexist =	(struct btc_coexist *)bt_context;
+	switch (disp_type) {
+	case BTC_DBG_DISP_COEX_STATISTICS:
+		halbtc_display_coex_statistics(btcoexist);
+		break;
+	case BTC_DBG_DISP_BT_LINK_INFO:
+		halbtc_display_bt_link_info(btcoexist);
+		break;
+	case BTC_DBG_DISP_BT_FW_VER:
+		halbtc_display_bt_fw_info(btcoexist);
+		break;
+	case BTC_DBG_DISP_FW_PWR_MODE_CMD:
+		halbtc_display_fw_pwr_mode_cmd(btcoexist);
+		break;
+	default:
+		break;
+	}
+}
+
+static bool halbtc_under_ips(struct btc_coexist *btcoexist)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtlpriv);
+	enum rf_pwrstate rtstate;
+
+	if (ppsc->b_inactiveps) {
+		rtstate = ppsc->rfpwr_state;
+
+		if (rtstate != ERFON &&
+		    ppsc->rfoff_reason == RF_CHANGE_BY_IPS)
+			return true;
+	}
+
+	return false;
+}
+
+/*****************************************************************
+ *         Extern functions called by other module
+ *****************************************************************/
+bool exhalbtc92e_initlize_variables(struct rtl_priv *adapter)
+{
+	struct btc_coexist *btcoexist = &gl92e_bt_coexist;
+
+	btcoexist->statistics.cnt_bind++;
+
+	halbtc_dbg_init();
+
+	if (btcoexist->binded)
+		return false;
+	else
+		btcoexist->binded = true;
+
+	btcoexist->chip_interface = BTC_INTF_UNKNOWN;
+
+	if (NULL == btcoexist->adapter)
+		btcoexist->adapter = adapter;
+
+	btcoexist->stack_info.profile_notified = false;
+
+	btcoexist->btc_read_1byte = halbtc_read_1byte;
+	btcoexist->btc_write_1byte = halbtc_write_1byte;
+	btcoexist->btc_write_1byte_bitmask = halbtc_bitmask_write_1byte;
+	btcoexist->btc_read_2byte = halbtc_read_2byte;
+	btcoexist->btc_write_2byte = halbtc_write_2byte;
+	btcoexist->btc_read_4byte = halbtc_read_4byte;
+	btcoexist->btc_write_4byte = halbtc_write_4byte;
+
+	btcoexist->btc_set_bb_reg = halbtc_set_bbreg;
+	btcoexist->btc_get_bb_reg = halbtc_get_bbreg;
+
+	btcoexist->btc_set_rf_reg = halbtc_set_rfreg;
+	btcoexist->btc_get_rf_reg = halbtc_get_rfreg;
+
+	btcoexist->btc_fill_h2c = halbtc_fill_h2c_cmd;
+	btcoexist->btc_disp_dbg_msg = halbtc_display_dbg_msg;
+
+	btcoexist->btc_get = halbtc_get;
+	btcoexist->btc_set = halbtc_set;
+
+	btcoexist->cli_buf = &btc_dbg_buf[0];
+
+	btcoexist->bt_info.b_bt_ctrl_buf_size = false;
+	btcoexist->bt_info.agg_buf_size = 5;
+
+	btcoexist->bt_info.increase_scan_dev_num = false;
+	return true;
+}
+
+void exhalbtc92e_init_hw_config(struct btc_coexist *btcoexist)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+
+	btcoexist->statistics.cnt_init_hw_config++;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex92e_halbtc8723b2ant_init_hwconfig(btcoexist);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8723b1ant_init_hwconfig(btcoexist);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
+		ex_halbtc8192e2ant_init_hwconfig(btcoexist);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_halbtc8821a2ant_init_hwconfig(btcoexist);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8821a1ant_init_hwconfig(btcoexist);
+	}
+}
+
+void exhalbtc92e_init_coex_dm(struct btc_coexist *btcoexist)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+
+	btcoexist->statistics.cnt_init_coex_dm++;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex92e_halbtc8723b2ant_init_coex_dm(btcoexist);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8723b1ant_init_coex_dm(btcoexist);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
+		ex_halbtc8192e2ant_init_coex_dm(btcoexist);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_halbtc8821a2ant_init_coex_dm(btcoexist);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8821a1ant_init_coex_dm(btcoexist);
+	}
+
+	btcoexist->initilized = true;
+}
+
+void exhalbtc92e_ips_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	u8 ips_type;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_ips_notify++;
+	if (btcoexist->manual_control)
+		return;
+
+	if (ERFOFF == type)
+		ips_type = BTC_IPS_ENTER;
+	else
+		ips_type = BTC_IPS_LEAVE;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex92e_halbtc8723b2ant_ips_notify(btcoexist, ips_type);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8723b1ant_ips_notify(btcoexist, ips_type);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
+		ex_halbtc8192e2ant_ips_notify(btcoexist, ips_type);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_halbtc8821a2ant_ips_notify(btcoexist, ips_type);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8821a1ant_ips_notify(btcoexist, ips_type);
+	}
+}
+
+void exhalbtc92e_lps_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	u8 lps_type;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_lps_notify++;
+	if (btcoexist->manual_control)
+		return;
+
+	if (EACTIVE == type)
+		lps_type = BTC_LPS_DISABLE;
+	else
+		lps_type = BTC_LPS_ENABLE;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex92e_halbtc8723b2ant_lps_notify(btcoexist, lps_type);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8723b1ant_lps_notify(btcoexist, lps_type);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
+		ex_halbtc8192e2ant_lps_notify(btcoexist, lps_type);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_halbtc8821a2ant_lps_notify(btcoexist, lps_type);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8821a1ant_lps_notify(btcoexist, lps_type);
+	}
+}
+
+void exhalbtc92e_scan_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	u8 scan_type;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_scan_notify++;
+	if (btcoexist->manual_control)
+		return;
+
+	if (type)
+		scan_type = BTC_SCAN_START;
+	else
+		scan_type = BTC_SCAN_FINISH;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex92e_halbtc8723b2ant_scan_notify(btcoexist, scan_type);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8723b1ant_scan_notify(btcoexist, scan_type);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
+		ex_halbtc8192e2ant_scan_notify(btcoexist, scan_type);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_halbtc8821a2ant_scan_notify(btcoexist, scan_type);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8821a1ant_scan_notify(btcoexist, scan_type);
+	}
+}
+
+void exhalbtc92e_connect_notify(struct btc_coexist *btcoexist, u8 action)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	u8 asso_type;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_connect_notify++;
+	if (btcoexist->manual_control)
+		return;
+
+	if (action)
+		asso_type = BTC_ASSOCIATE_START;
+	else
+		asso_type = BTC_ASSOCIATE_FINISH;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex92e_halbtc8723b2ant_connect_notify(btcoexist,
+							     asso_type);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8723b1ant_connect_notify(btcoexist, asso_type);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
+		ex_halbtc8192e2ant_connect_notify(btcoexist, asso_type);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_halbtc8821a2ant_connect_notify(btcoexist,
+							  asso_type);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8821a1ant_connect_notify(btcoexist,
+							  asso_type);
+	}
+}
+
+void exhalbtc92e_mediastatus_notify(struct btc_coexist *btcoexist,
+				    enum rt_media_status media_status)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	u8 status;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_media_status_notify++;
+	if (btcoexist->manual_control)
+		return;
+
+	if (RT_MEDIA_CONNECT == media_status)
+		status = BTC_MEDIA_CONNECT;
+	else
+		status = BTC_MEDIA_DISCONNECT;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex92e_halbtc8723b2ant_media_status_notify(btcoexist,
+								  status);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8723b1ant_media_status_notify(btcoexist,
+							       status);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
+		ex_halbtc8192e2ant_media_status_notify(btcoexist, status);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_halbtc8821a2ant_media_status_notify(btcoexist,
+							       status);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8821a1ant_media_status_notify(btcoexist,
+							       status);
+	}
+}
+
+void exhalbtc92e_special_packet_notify(struct btc_coexist *btcoexist,
+				       u8 pkt_type)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	u8 packet_type;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_special_packet_notify++;
+	if (btcoexist->manual_control)
+		return;
+
+	if (PACKET_DHCP == pkt_type) {
+		packet_type = BTC_PACKET_DHCP;
+	} else if (PACKET_EAPOL == pkt_type) {
+		packet_type = BTC_PACKET_EAPOL;
+	} else if (PACKET_ARP == pkt_type) {
+		packet_type = BTC_PACKET_ARP;
+	} else {
+		packet_type = BTC_PACKET_UNKNOWN;
+		return;
+	}
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex92e_halbtc8723b2ant_special_packet_notify(btcoexist,
+								    packet_type);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8723b1ant_special_packet_notify(btcoexist,
+								 packet_type);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_halbtc8821a2ant_special_packet_notify(btcoexist,
+								 packet_type);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8821a1ant_special_packet_notify(btcoexist,
+								 packet_type);
+	}
+}
+
+void exhalbtc92e_bt_info_notify(struct btc_coexist *btcoexist,
+				u8 *tmp_buf, u8 length)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_bt_info_notify++;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex92e_halbtc8723b2ant_bt_info_notify(btcoexist,
+							     tmp_buf, length);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8723b1ant_bt_info_notify(btcoexist, tmp_buf,
+							  length);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
+		/* ex_halbtc8192e2ant_bt_info_notify(btcoexist,
+						     tmp_buf, length); */
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_halbtc8821a2ant_bt_info_notify(btcoexist,
+							  tmp_buf, length);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8821a1ant_bt_info_notify(btcoexist,
+							  tmp_buf, length);
+	}
+}
+
+void exhalbtc92e_stack_operation_notify(struct btc_coexist *btcoexist, u8 type)
+{
+	u8 stack_op_type;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_stack_operation_notify++;
+	if (btcoexist->manual_control)
+		return;
+
+	if ((HCI_BT_OP_INQUIRY_START == type) ||
+	    (HCI_BT_OP_PAGING_START == type) ||
+	    (HCI_BT_OP_PAIRING_START == type))
+		stack_op_type = BTC_STACK_OP_INQ_PAGE_PAIR_START;
+	else if ((HCI_BT_OP_INQUIRY_FINISH == type) ||
+		 (HCI_BT_OP_PAGING_SUCCESS == type) ||
+		 (HCI_BT_OP_PAGING_UNSUCCESS == type) ||
+		 (HCI_BT_OP_PAIRING_FINISH == type))
+		stack_op_type = BTC_STACK_OP_INQ_PAGE_PAIR_FINISH;
+	else
+		stack_op_type = BTC_STACK_OP_NONE;
+}
+
+void exhalbtc92e_halt_notify(struct btc_coexist *btcoexist)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+
+	btcoexist->binded = false;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex92e_halbtc8723b2ant_halt_notify(btcoexist);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8723b1ant_halt_notify(btcoexist);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
+		ex_halbtc8192e2ant_halt_notify(btcoexist);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_halbtc8821a2ant_halt_notify(btcoexist);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8821a1ant_halt_notify(btcoexist);
+	}
+}
+
+void exhalbtc92e_pnp_notify(struct btc_coexist *btcoexist, u8 pnp_state)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
+		if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8723b1ant_pnp_notify(btcoexist, pnp_state);
+	}
+}
+
+void exhalbtc_coex_dm_switch(struct btc_coexist *btcoexist)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_coex_dm_switch++;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
+		if (btcoexist->board_info.btdm_ant_num == 1) {
+			btcoexist->stop_coex_dm = true;
+			ex_halbtc8723b1ant_coex_dm_reset(btcoexist);
+			exhalbtc92e_set_ant_num(BT_COEX_ANT_TYPE_DETECTED, 2);
+			ex92e_halbtc8723b2ant_init_hwconfig(btcoexist);
+			ex92e_halbtc8723b2ant_init_coex_dm(btcoexist);
+			btcoexist->stop_coex_dm = false;
+		}
+	}
+}
+
+void exhalbtc92e_periodical(struct btc_coexist *btcoexist)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_periodical++;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex92e_halbtc8723b2ant_periodical(btcoexist);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8723b1ant_periodical(btcoexist);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
+		ex_halbtc8192e2ant_periodical(btcoexist);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_halbtc8821a2ant_periodical(btcoexist);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			if (!halbtc_under_ips(btcoexist))
+				ex_halbtc8821a1ant_periodical(btcoexist);
+	}
+}
+
+void exhalbtc92e_dbg_control(struct btc_coexist *btcoexist,
+			     u8 code, u8 len, u8 *data)
+{
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+	btcoexist->statistics.cnt_dbg_ctrl++;
+}
+
+void exhalbtc92e_stack_update_profile_info(void)
+{
+}
+
+void exhalbtc92e_update_min_bt_rssi(char bt_rssi)
+{
+	struct btc_coexist *btcoexist = &gl92e_bt_coexist;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+
+	btcoexist->stack_info.min_bt_rssi = bt_rssi;
+}
+
+
+void exhalbtc92e_set_hci_version(u16 hci_version)
+{
+	struct btc_coexist *btcoexist = &gl92e_bt_coexist;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+
+	btcoexist->stack_info.hci_version = hci_version;
+}
+
+void exhalbtc92e_set_bt_patch_version(u16 bt_hci_version, u16 bt_patch_version)
+{
+	struct btc_coexist *btcoexist = &gl92e_bt_coexist;
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+
+	btcoexist->bt_info.bt_real_fw_ver = bt_patch_version;
+	btcoexist->bt_info.bt_hci_ver = bt_hci_version;
+}
+
+void exhalbtc92e_set_bt_exist(bool bt_exist)
+{
+	gl92e_bt_coexist.board_info.bt_exist = bt_exist;
+}
+
+void exhalbtc92e_set_chip_type(u8 chip_type)
+{
+	switch (chip_type) {
+	default:
+	case BT_2WIRE:
+	case BT_ISSC_3WIRE:
+	case BT_ACCEL:
+	case BT_RTL8756:
+		gl92e_bt_coexist.board_info.bt_chip_type = BTC_CHIP_UNDEF;
+		break;
+	case BT_CSR_BC4:
+		gl92e_bt_coexist.board_info.bt_chip_type = BTC_CHIP_CSR_BC4;
+		break;
+	case BT_CSR_BC8:
+		gl92e_bt_coexist.board_info.bt_chip_type = BTC_CHIP_CSR_BC8;
+		break;
+	case BT_RTL8723A:
+		gl92e_bt_coexist.board_info.bt_chip_type = BTC_CHIP_RTL8723A;
+		break;
+	case BT_RTL8821A:
+		gl92e_bt_coexist.board_info.bt_chip_type = BTC_CHIP_RTL8821;
+		break;
+	case BT_RTL8723B:
+		gl92e_bt_coexist.board_info.bt_chip_type = BTC_CHIP_RTL8723B;
+		break;
+	}
+}
+
+void exhalbtc92e_set_ant_num(u8 type, u8 ant_num)
+{
+	if (BT_COEX_ANT_TYPE_PG == type) {
+		gl92e_bt_coexist.board_info.pg_ant_num = ant_num;
+		gl92e_bt_coexist.board_info.btdm_ant_num = ant_num;
+		/* The antenna position:
+		 * Main (default) or Aux for pgAntNum = 2 && btdmAntNum = 1.
+		 * The antenna position should be determined by
+		 * auto-detect mechanism.
+		 * The following is assumed to main,
+		 * and those must be modified
+		 * if y auto-detect mechanism is ready
+		 */
+		if ((gl92e_bt_coexist.board_info.pg_ant_num == 2) &&
+		    (gl92e_bt_coexist.board_info.btdm_ant_num == 1))
+			gl92e_bt_coexist.board_info.btdm_ant_pos =
+						       BTC_ANTENNA_AT_MAIN_PORT;
+		else
+			gl92e_bt_coexist.board_info.btdm_ant_pos =
+						       BTC_ANTENNA_AT_MAIN_PORT;
+	} else if (BT_COEX_ANT_TYPE_ANTDIV == type) {
+		gl92e_bt_coexist.board_info.btdm_ant_num = ant_num;
+		gl92e_bt_coexist.board_info.btdm_ant_pos =
+						       BTC_ANTENNA_AT_MAIN_PORT;
+	} else if (BT_COEX_ANT_TYPE_DETECTED == type) {
+		gl92e_bt_coexist.board_info.btdm_ant_num = ant_num;
+		gl92e_bt_coexist.board_info.btdm_ant_pos =
+						       BTC_ANTENNA_AT_MAIN_PORT;
+	}
+}
+
+void exhalbtc92e_display_bt_coex_info(struct btc_coexist *btcoexist)
+{
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+
+	if (!halbtc_is_bt_coexist_available(btcoexist))
+		return;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_halbtc8723b2ant92e_display_coex_info(btcoexist);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8723b1ant_display_coex_info(btcoexist);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+		if (btcoexist->board_info.btdm_ant_num == 2)
+			ex_halbtc8821a2ant_display_coex_info(btcoexist);
+		else if (btcoexist->board_info.btdm_ant_num == 1)
+			ex_halbtc8821a1ant_display_coex_info(btcoexist);
+	}
+}
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbtcoutsrc.h b/drivers/staging/rtl8192ee/btcoexist/halbtcoutsrc.h
new file mode 100644
index 00000000000..c0a4286430a
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/halbtcoutsrc.h
@@ -0,0 +1,537 @@
+#ifndef	__HALBTC_OUT_SRC_H__
+#define __HALBTC_OUT_SRC_H__
+
+#include	"../wifi.h"
+
+#define		NORMAL_EXEC				false
+#define		FORCE_EXEC				true
+
+#define		BTC_RF_A				RF90_PATH_A
+#define		BTC_RF_B				RF90_PATH_B
+#define		BTC_RF_C				RF90_PATH_C
+#define		BTC_RF_D				RF90_PATH_D
+
+#define		BTC_SMSP				SINGLEMAC_SINGLEPHY
+#define		BTC_DMDP				DUALMAC_DUALPHY
+#define		BTC_DMSP				DUALMAC_SINGLEPHY
+#define		BTC_MP_UNKNOWN				0xff
+
+#define		IN
+#define		OUT
+
+#define		BT_TMP_BUF_SIZE				100
+
+#define		BT_COEX_ANT_TYPE_PG			0
+#define		BT_COEX_ANT_TYPE_ANTDIV			1
+#define		BT_COEX_ANT_TYPE_DETECTED		2
+
+#define		BTC_MIMO_PS_STATIC			0
+#define		BTC_MIMO_PS_DYNAMIC			1
+
+#define		BTC_RATE_DISABLE			0
+#define		BTC_RATE_ENABLE				1
+
+/* single Antenna definition */
+#define		BTC_ANT_PATH_WIFI			0
+#define		BTC_ANT_PATH_BT				1
+#define		BTC_ANT_PATH_PTA			2
+/* dual Antenna definition */
+#define		BTC_ANT_WIFI_AT_MAIN			0
+#define		BTC_ANT_WIFI_AT_AUX			1
+/* coupler Antenna definition */
+#define		BTC_ANT_WIFI_AT_CPL_MAIN		0
+#define		BTC_ANT_WIFI_AT_CPL_AUX			1
+
+enum btc_chip_interface {
+	BTC_INTF_UNKNOWN	= 0,
+	BTC_INTF_PCI		= 1,
+	BTC_INTF_USB		= 2,
+	BTC_INTF_SDIO		= 3,
+	BTC_INTF_GSPI		= 4,
+	BTC_INTF_MAX
+};
+
+enum btc_chip_type {
+	BTC_CHIP_UNDEF		= 0,
+	BTC_CHIP_CSR_BC4	= 1,
+	BTC_CHIP_CSR_BC8	= 2,
+	BTC_CHIP_RTL8723A	= 3,
+	BTC_CHIP_RTL8821	= 4,
+	BTC_CHIP_RTL8723B	= 5,
+	BTC_CHIP_MAX
+};
+
+enum btc_msg_type {
+	BTC_MSG_INTERFACE	= 0x0,
+	BTC_MSG_ALGORITHM	= 0x1,
+	BTC_MSG_MAX
+};
+
+extern u32 btc_92edbg_type[];
+
+/* following is for BTC_MSG_INTERFACE */
+#define		INTF_INIT				BIT(0)
+#define		INTF_NOTIFY				BIT(2)
+
+/* following is for BTC_ALGORITHM */
+#define		ALGO_BT_RSSI_STATE			BIT(0)
+#define		ALGO_WIFI_RSSI_STATE			BIT(1)
+#define		ALGO_BT_MONITOR				BIT(2)
+#define		ALGO_TRACE				BIT(3)
+#define		ALGO_TRACE_FW				BIT(4)
+#define		ALGO_TRACE_FW_DETAIL			BIT(5)
+#define		ALGO_TRACE_FW_EXEC			BIT(6)
+#define		ALGO_TRACE_SW				BIT(7)
+#define		ALGO_TRACE_SW_DETAIL			BIT(8)
+#define		ALGO_TRACE_SW_EXEC			BIT(9)
+
+/* following is for wifi link status */
+#define		WIFI_STA_CONNECTED			BIT(0)
+#define		WIFI_AP_CONNECTED			BIT(1)
+#define		WIFI_HS_CONNECTED			BIT(2)
+#define		WIFI_P2P_GO_CONNECTED			BIT(3)
+#define		WIFI_P2P_GC_CONNECTED			BIT(4)
+
+
+#define	CL_SPRINTF	snprintf
+#define	CL_PRINTF	printk
+
+#define	BTC_PRINT(dbgtype, dbgflag, printstr, ...)		\
+	do {							\
+		if (unlikely(btc_92edbg_type[dbgtype] & dbgflag)) {\
+			pr_debug(printstr, ##__VA_ARGS__);	\
+		}						\
+	} while (0)
+
+#define	BTC_PRINT_F(dbgtype, dbgflag, printstr, ...)		\
+	do {							\
+		if (unlikely(btc_92edbg_type[dbgtype] & dbgflag)) {\
+			pr_debug("%s: ", __func__);	\
+			pr_cont(printstr, ##__VA_ARGS__);	\
+		}						\
+	} while (0)
+
+#define	BTC_PRINT_ADDR(dbgtype, dbgflag, printstr, _ptr)	\
+	do {							\
+		if (unlikely(btc_92edbg_type[dbgtype] & dbgflag)) {	\
+			int __i;				\
+			u8 *__ptr = (u8 *)_ptr;			\
+			pr_debug printstr;			\
+			for (__i = 0; __i < 6; __i++)		\
+				pr_cont("%02X%s", __ptr[__i],	\
+					(__i == 5) ? "" : "-");	\
+			pr_debug("\n");				\
+		}						\
+	} while (0)
+
+#define BTC_PRINT_DATA(dbgtype, dbgflag, _titlestring, _hexdata, _hexdatalen) \
+	do {								\
+		if (unlikely(btc_92edbg_type[dbgtype] & dbgflag)) {	\
+			int __i;					\
+			u8 *__ptr = (u8 *)_hexdata;			\
+			pr_debug(_titlestring);				\
+			for (__i = 0; __i < (int)_hexdatalen; __i++) {	\
+				pr_cont("%02X%s", __ptr[__i], (((__i + 1) % 4) \
+							== 0) ? "  " : " ");\
+				if (((__i + 1) % 16) == 0)		\
+					pr_cont("\n");			\
+			}						\
+			pr_debug("\n");			\
+		}							\
+	} while (0)
+
+
+#define	BTC_RSSI_HIGH(_rssi_)	\
+	((_rssi_ == BTC_RSSI_STATE_HIGH ||	\
+	  _rssi_ == BTC_RSSI_STATE_STAY_HIGH) ? true : false)
+#define	BTC_RSSI_MEDIUM(_rssi_)	\
+	((_rssi_ == BTC_RSSI_STATE_MEDIUM ||	\
+	  _rssi_ == BTC_RSSI_STATE_STAY_MEDIUM) ? true : false)
+#define	BTC_RSSI_LOW(_rssi_)	\
+	((_rssi_ == BTC_RSSI_STATE_LOW ||	\
+	  _rssi_ == BTC_RSSI_STATE_STAY_LOW) ? true : false)
+
+
+enum btc_power_save_type {
+	BTC_PS_WIFI_NATIVE = 0,
+	BTC_PS_LPS_ON = 1,
+	BTC_PS_LPS_OFF = 2,
+	BTC_PS_LPS_MAX
+};
+
+struct btc_board_info {
+	/* The following is some board information */
+	u8 bt_chip_type;
+	u8 pg_ant_num;	/* pg ant number */
+	u8 btdm_ant_num;	/* ant number for btdm */
+	u8 btdm_ant_pos;
+	bool bt_exist;
+};
+
+enum btc_dbg_opcode {
+	BTC_DBG_SET_COEX_NORMAL = 0x0,
+	BTC_DBG_SET_COEX_WIFI_ONLY = 0x1,
+	BTC_DBG_SET_COEX_BT_ONLY = 0x2,
+	BTC_DBG_MAX
+};
+
+enum btc_rssi_state {
+	BTC_RSSI_STATE_HIGH = 0x0,
+	BTC_RSSI_STATE_MEDIUM = 0x1,
+	BTC_RSSI_STATE_LOW = 0x2,
+	BTC_RSSI_STATE_STAY_HIGH = 0x3,
+	BTC_RSSI_STATE_STAY_MEDIUM = 0x4,
+	BTC_RSSI_STATE_STAY_LOW = 0x5,
+	BTC_RSSI_MAX
+};
+
+enum btc_wifi_role {
+	BTC_ROLE_STATION = 0x0,
+	BTC_ROLE_AP = 0x1,
+	BTC_ROLE_IBSS = 0x2,
+	BTC_ROLE_HS_MODE = 0x3,
+	BTC_ROLE_MAX
+};
+
+enum btc_wifi_bw_mode {
+	BTC_WIFI_BW_LEGACY = 0x0,
+	BTC_WIFI_BW_HT20 = 0x1,
+	BTC_WIFI_BW_HT40 = 0x2,
+	BTC_WIFI_BW_MAX
+};
+
+enum btc_wifi_traffic_dir {
+	BTC_WIFI_TRAFFIC_TX = 0x0,
+	BTC_WIFI_TRAFFIC_RX = 0x1,
+	BTC_WIFI_TRAFFIC_MAX
+};
+
+enum btc_wifi_pnp {
+	BTC_WIFI_PNP_WAKE_UP = 0x0,
+	BTC_WIFI_PNP_SLEEP = 0x1,
+	BTC_WIFI_PNP_MAX
+};
+
+
+enum btc_get_type {
+	/* type bool */
+	BTC_GET_BL_HS_OPERATION,
+	BTC_GET_BL_HS_CONNECTING,
+	BTC_GET_BL_WIFI_CONNECTED,
+	BTC_GET_BL_WIFI_BUSY,
+	BTC_GET_BL_WIFI_SCAN,
+	BTC_GET_BL_WIFI_LINK,
+	BTC_GET_BL_WIFI_DHCP,
+	BTC_GET_BL_WIFI_SOFTAP_IDLE,
+	BTC_GET_BL_WIFI_SOFTAP_LINKING,
+	BTC_GET_BL_WIFI_IN_EARLY_SUSPEND,
+	BTC_GET_BL_WIFI_ROAM,
+	BTC_GET_BL_WIFI_4_WAY_PROGRESS,
+	BTC_GET_BL_WIFI_UNDER_5G,
+	BTC_GET_BL_WIFI_AP_MODE_ENABLE,
+	BTC_GET_BL_WIFI_ENABLE_ENCRYPTION,
+	BTC_GET_BL_WIFI_UNDER_B_MODE,
+	BTC_GET_BL_EXT_SWITCH,
+
+	/* type s4Byte */
+	BTC_GET_S4_WIFI_RSSI,
+	BTC_GET_S4_HS_RSSI,
+
+	/* type u32 */
+	BTC_GET_U4_WIFI_BW,
+	BTC_GET_U4_WIFI_TRAFFIC_DIRECTION,
+	BTC_GET_U4_WIFI_FW_VER,
+	BTC_GET_U4_WIFI_LINK_STATUS,
+	BTC_GET_U4_BT_PATCH_VER,
+
+	/* type u1Byte */
+	BTC_GET_U1_WIFI_DOT11_CHNL,
+	BTC_GET_U1_WIFI_CENTRAL_CHNL,
+	BTC_GET_U1_WIFI_HS_CHNL,
+	BTC_GET_U1_MAC_PHY_MODE,
+	BTC_GET_U1_AP_NUM,
+
+	/* for 1Ant */
+	BTC_GET_U1_LPS_MODE,
+	BTC_GET_BL_BT_SCO_BUSY,
+
+	/* for test mode */
+	BTC_GET_DRIVER_TEST_CFG,
+	BTC_GET_MAX
+};
+
+
+enum btc_set_type {
+	/* type bool */
+	BTC_SET_BL_BT_DISABLE,
+	BTC_SET_BL_BT_TRAFFIC_BUSY,
+	BTC_SET_BL_BT_LIMITED_DIG,
+	BTC_SET_BL_FORCE_TO_ROAM,
+	BTC_SET_BL_TO_REJ_AP_AGG_PKT,
+	BTC_SET_BL_BT_CTRL_AGG_SIZE,
+	BTC_SET_BL_INC_SCAN_DEV_NUM,
+
+	/* type u1Byte */
+	BTC_SET_U1_RSSI_ADJ_VAL_FOR_AGC_TABLE_ON,
+	BTC_SET_U1_AGG_BUF_SIZE,
+
+	/* type trigger some action */
+	BTC_SET_ACT_GET_BT_RSSI,
+	BTC_SET_ACT_AGGREGATE_CTRL,
+
+	/********* for 1Ant **********/
+	/* type bool */
+	BTC_SET_BL_BT_SCO_BUSY,
+	/* type u1Byte */
+	BTC_SET_U1_RSSI_ADJ_VAL_FOR_1ANT_COEX_TYPE,
+	BTC_SET_U1_LPS_VAL,
+	BTC_SET_U1_RPWM_VAL,
+	BTC_SET_U1_1ANT_LPS,
+	BTC_SET_U1_1ANT_RPWM,
+	/* type trigger some action */
+	BTC_SET_ACT_LEAVE_LPS,
+	BTC_SET_ACT_ENTER_LPS,
+	BTC_SET_ACT_NORMAL_LPS,
+	BTC_SET_ACT_INC_FORCE_EXEC_PWR_CMD_CNT,
+	BTC_SET_ACT_DISABLE_LOW_POWER,
+	BTC_SET_ACT_UPDATE_ra_mask,
+	BTC_SET_ACT_SEND_MIMO_PS,
+	/* BT Coex related */
+	BTC_SET_ACT_CTRL_BT_INFO,
+	BTC_SET_ACT_CTRL_BT_COEX,
+	/***************************/
+	BTC_SET_MAX
+};
+
+enum btc_dbg_disp_type {
+	BTC_DBG_DISP_COEX_STATISTICS = 0x0,
+	BTC_DBG_DISP_BT_LINK_INFO = 0x1,
+	BTC_DBG_DISP_BT_FW_VER = 0x2,
+	BTC_DBG_DISP_FW_PWR_MODE_CMD = 0x3,
+	BTC_DBG_DISP_MAX
+};
+
+enum btc_notify_type_ips {
+	BTC_IPS_LEAVE = 0x0,
+	BTC_IPS_ENTER = 0x1,
+	BTC_IPS_MAX
+};
+
+enum btc_notify_type_lps {
+	BTC_LPS_DISABLE = 0x0,
+	BTC_LPS_ENABLE = 0x1,
+	BTC_LPS_MAX
+};
+
+enum btc_notify_type_scan {
+	BTC_SCAN_FINISH = 0x0,
+	BTC_SCAN_START = 0x1,
+	BTC_SCAN_MAX
+};
+
+enum btc_notify_type_associate {
+	BTC_ASSOCIATE_FINISH = 0x0,
+	BTC_ASSOCIATE_START = 0x1,
+	BTC_ASSOCIATE_MAX
+};
+
+enum btc_notify_type_media_status {
+	BTC_MEDIA_DISCONNECT = 0x0,
+	BTC_MEDIA_CONNECT = 0x1,
+	BTC_MEDIA_MAX
+};
+
+enum btc_notify_type_special_packet {
+	BTC_PACKET_UNKNOWN = 0x0,
+	BTC_PACKET_DHCP = 0x1,
+	BTC_PACKET_ARP = 0x2,
+	BTC_PACKET_EAPOL = 0x3,
+	BTC_PACKET_MAX
+};
+
+enum hci_ext_bt_operation {
+	HCI_BT_OP_NONE = 0x0,
+	HCI_BT_OP_INQUIRY_START = 0x1,
+	HCI_BT_OP_INQUIRY_FINISH = 0x2,
+	HCI_BT_OP_PAGING_START = 0x3,
+	HCI_BT_OP_PAGING_SUCCESS = 0x4,
+	HCI_BT_OP_PAGING_UNSUCCESS = 0x5,
+	HCI_BT_OP_PAIRING_START = 0x6,
+	HCI_BT_OP_PAIRING_FINISH = 0x7,
+	HCI_BT_OP_BT_DEV_ENABLE = 0x8,
+	HCI_BT_OP_BT_DEV_DISABLE = 0x9,
+	HCI_BT_OP_MAX
+};
+
+enum btc_notify_type_stack_operation {
+	BTC_STACK_OP_NONE = 0x0,
+	BTC_STACK_OP_INQ_PAGE_PAIR_START = 0x1,
+	BTC_STACK_OP_INQ_PAGE_PAIR_FINISH = 0x2,
+	BTC_STACK_OP_MAX
+};
+
+
+struct btc_bt_info {
+	bool bt_disabled;
+	u8 rssi_adjust_for_agc_table_on;
+	u8 rssi_adjust_for_1ant_coex_type;
+	bool bt_busy;
+	u8 agg_buf_size;
+	bool limited_dig;
+	bool reject_agg_pkt;
+	bool b_bt_ctrl_buf_size;
+	bool increase_scan_dev_num;
+	u16 bt_hci_ver;
+	u16 bt_real_fw_ver;
+	u8 bt_fw_ver;
+
+	bool bt_disable_low_pwr;
+
+	/* the following is for 1Ant solution */
+	bool bt_ctrl_lps;
+	bool bt_pwr_save_mode;
+	bool bt_lps_on;
+	bool force_to_roam;
+	u8 force_exec_pwr_cmd_cnt;
+	u8 lps_val;
+	u8 rpwm_val;
+	u32 ra_mask;
+};
+
+struct btc_stack_info {
+	bool profile_notified;
+	u16 hci_version;	/* stack hci version */
+	u8 num_of_link;
+	bool bt_link_exist;
+	bool sco_exist;
+	bool acl_exist;
+	bool a2dp_exist;
+	bool hid_exist;
+	u8 num_of_hid;
+	bool pan_exist;
+	bool unknown_acl_exist;
+	char min_bt_rssi;
+};
+
+struct btc_statistics {
+	u32 cnt_bind;
+	u32 cnt_init_hw_config;
+	u32 cnt_init_coex_dm;
+	u32 cnt_ips_notify;
+	u32 cnt_lps_notify;
+	u32 cnt_scan_notify;
+	u32 cnt_connect_notify;
+	u32 cnt_media_status_notify;
+	u32 cnt_special_packet_notify;
+	u32 cnt_bt_info_notify;
+	u32 cnt_periodical;
+	u32 cnt_coex_dm_switch;
+	u32 cnt_stack_operation_notify;
+	u32 cnt_dbg_ctrl;
+};
+
+struct btc_bt_link_info {
+	bool bt_link_exist;
+	bool sco_exist;
+	bool sco_only;
+	bool a2dp_exist;
+	bool a2dp_only;
+	bool hid_exist;
+	bool hid_only;
+	bool pan_exist;
+	bool pan_only;
+};
+
+enum btc_antenna_pos {
+	BTC_ANTENNA_AT_MAIN_PORT = 0x1,
+	BTC_ANTENNA_AT_AUX_PORT = 0x2,
+};
+
+struct btc_coexist {
+	/* make sure only one adapter can bind the data context  */
+	bool binded;
+	/* default adapter */
+	void *adapter;
+	struct btc_board_info board_info;
+	/* some bt info referenced by non-bt module */
+	struct btc_bt_info bt_info;
+	struct btc_stack_info stack_info;
+	enum btc_chip_interface	chip_interface;
+	struct btc_bt_link_info bt_link_info;
+
+	bool initilized;
+	bool stop_coex_dm;
+	bool manual_control;
+	u8 *cli_buf;
+	struct btc_statistics statistics;
+	u8 pwr_mode_val[10];
+
+	/* function pointers io related */
+	u8 (*btc_read_1byte)(void *btc_context, u32 reg_addr);
+	void (*btc_write_1byte)(void *btc_context, u32 reg_addr, u8 data);
+	void (*btc_write_1byte_bitmask)(void *btc_context, u32 reg_addr,
+					u8 bit_mask, u8 data1b);
+	u16 (*btc_read_2byte)(void *btc_context, u32 reg_addr);
+	void (*btc_write_2byte)(void *btc_context, u32 reg_addr, u16 data);
+	u32 (*btc_read_4byte)(void *btc_context, u32 reg_addr);
+	void (*btc_write_4byte)(void *btc_context, u32 reg_addr, u32 data);
+
+	void (*btc_set_bb_reg)(void *btc_context, u32 reg_addr,
+			       u32 bit_mask, u32 data);
+	u32 (*btc_get_bb_reg)(void *btc_context, u32 reg_addr,
+			      u32 bit_mask);
+
+	void (*btc_set_rf_reg)(void *btc_context, u8 rf_path, u32 reg_addr,
+			       u32 bit_mask, u32 data);
+	u32 (*btc_get_rf_reg)(void *btc_context, u8 rf_path,
+			      u32 reg_addr, u32 bit_mask);
+
+
+	void (*btc_fill_h2c)(void *btc_context, u8 element_id,
+			     u32 cmd_len, u8 *cmd_buffer);
+
+	void (*btc_disp_dbg_msg)(void *btcoexist, u8 disp_type);
+
+	bool (*btc_get)(void *btcoexist, u8 get_type, void *out_buf);
+	bool (*btc_set)(void *btcoexist, u8 set_type, void *in_buf);
+};
+
+
+bool halbtc92e_is_wifi_uplink(struct rtl_priv *adapter);
+
+
+extern struct btc_coexist gl92e_bt_coexist;
+
+bool exhalbtc92e_initlize_variables(struct rtl_priv *adapter);
+void exhalbtc92e_init_hw_config(struct btc_coexist *btcoexist);
+void exhalbtc92e_init_coex_dm(struct btc_coexist *btcoexist);
+void exhalbtc92e_ips_notify(struct btc_coexist *btcoexist, u8 type);
+void exhalbtc92e_lps_notify(struct btc_coexist *btcoexist, u8 type);
+void exhalbtc92e_scan_notify(struct btc_coexist *btcoexist, u8 type);
+void exhalbtc92e_connect_notify(struct btc_coexist *btcoexist, u8 action);
+void exhalbtc92e_mediastatus_notify(struct btc_coexist *btcoexist,
+				    enum rt_media_status media_status);
+void exhalbtc92e_special_packet_notify(struct btc_coexist *btcoexist,
+				       u8 pkt_type);
+void exhalbtc92e_bt_info_notify(struct btc_coexist *btcoexist, u8 *tmp_buf,
+				u8 length);
+void exhalbtc92e_stack_operation_notify(struct btc_coexist *btcoexist, u8 type);
+void exhalbtc92e_halt_notify(struct btc_coexist *btcoexist);
+void exhalbtc92e_pnp_notify(struct btc_coexist *btcoexist, u8 pnp_state);
+void exhalbtc_coex_dm_switch(struct btc_coexist *btcoexist);
+void exhalbtc92e_periodical(struct btc_coexist *btcoexist);
+void exhalbtc92e_dbg_control(struct btc_coexist *btcoexist, u8 code, u8 len,
+			     u8 *data);
+void exhalbtc92e_stack_update_profile_info(void);
+void exhalbtc92e_set_hci_version(u16 hci_version);
+void exhalbtc92e_set_bt_patch_version(u16 bt_hci_version, u16 bt_patch_version);
+void exhalbtc92e_update_min_bt_rssi(char bt_rssi);
+void exhalbtc92e_set_bt_exist(bool bt_exist);
+void exhalbtc92e_set_chip_type(u8 chip_type);
+void exhalbtc92e_set_ant_num(u8 type, u8 ant_num);
+void exhalbtc92e_display_bt_coex_info(struct btc_coexist *btcoexist);
+void exhalbtc_signal_compensation(struct btc_coexist *btcoexist,
+				  u8 *rssi_wifi, u8 *rssi_bt);
+void exhalbtc_lps_leave(struct btc_coexist *btcoexist);
+void exhalbtc_low_wifi_traffic_notify(struct btc_coexist *btcoexist);
+#endif
diff --git a/drivers/staging/rtl8192ee/btcoexist/rtl_btc.c b/drivers/staging/rtl8192ee/btcoexist/rtl_btc.c
new file mode 100644
index 00000000000..50c012a4c17
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/rtl_btc.c
@@ -0,0 +1,194 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+#include <linux/vmalloc.h>
+#include <linux/module.h>
+
+#include "rtl_btc.h"
+#include "halbt_precomp.h"
+
+static struct rtl_btc_ops rtl_btc_operation = {
+	.btc_init_variables = rtl92e_btc_init_variables,
+	.btc_init_hal_vars = rtl92e_btc_init_hal_vars,
+	.btc_init_hw_config = rtl92e_btc_init_hw_config,
+	.btc_ips_notify = rtl92e_btc_ips_notify,
+	.btc_lps_notify = rtl_btc_lps_notify,
+	.btc_scan_notify = rtl92e_btc_scan_notify,
+	.btc_connect_notify = rtl92e_btc_connect_notify,
+	.btc_mediastatus_notify = rtl92e_btc_mediastatus_notify,
+	.btc_periodical = rtl92e_btc_periodical,
+	.btc_halt_notify = rtl92e_btc_halt_notify,
+	.btc_btinfo_notify = rtl92e_btc_btinfo_notify,
+	.btc_is_limited_dig = rtl92e_btc_is_limited_dig,
+	.btc_is_disable_edca_turbo = rtl92e_btc_is_disable_edca_turbo,
+	.btc_is_bt_disabled = rtl92e_btc_is_bt_disabled,
+	.btc_special_packet_notify = rtl_btc_special_packet_notify,
+};
+
+void rtl92e_btc_init_variables(struct rtl_priv *rtlpriv)
+{
+	exhalbtc92e_initlize_variables(rtlpriv);
+}
+
+void rtl92e_btc_init_hal_vars(struct rtl_priv *rtlpriv)
+{
+	u8 ant_num;
+	u8 bt_exist;
+	u8 bt_type;
+	ant_num = rtl92e_get_hwpg_ant_num(rtlpriv);
+	RT_TRACE(COMP_INIT, DBG_DMESG,
+		 ("%s, antNum is %d\n", __func__, ant_num));
+
+	bt_exist = rtl92e_get_hwpg_bt_exist(rtlpriv);
+	RT_TRACE(COMP_INIT, DBG_DMESG,
+		 ("%s, bt_exist is %d\n", __func__, bt_exist));
+	exhalbtc92e_set_bt_exist(bt_exist);
+
+	bt_type = rtl92e_get_hwpg_bt_type(rtlpriv);
+	RT_TRACE(COMP_INIT, DBG_DMESG,
+		 ("%s, bt_type is %d\n", __func__, bt_type));
+	exhalbtc92e_set_chip_type(bt_type);
+
+	exhalbtc92e_set_ant_num(BT_COEX_ANT_TYPE_PG, ant_num);
+}
+
+void rtl92e_btc_init_hw_config(struct rtl_priv *rtlpriv)
+{
+	exhalbtc92e_init_hw_config(&gl92e_bt_coexist);
+	exhalbtc92e_init_coex_dm(&gl92e_bt_coexist);
+}
+
+void rtl92e_btc_ips_notify(struct rtl_priv *rtlpriv, u8 type)
+{
+	exhalbtc92e_ips_notify(&gl92e_bt_coexist, type);
+}
+
+void rtl_btc_lps_notify(struct rtl_priv *rtlpriv, u8 type)
+{
+	exhalbtc92e_lps_notify(&gl92e_bt_coexist, type);
+}
+
+void rtl92e_btc_scan_notify(struct rtl_priv *rtlpriv, u8 scantype)
+{
+	exhalbtc92e_scan_notify(&gl92e_bt_coexist, scantype);
+}
+
+void rtl92e_btc_connect_notify(struct rtl_priv *rtlpriv, u8 action)
+{
+	exhalbtc92e_connect_notify(&gl92e_bt_coexist, action);
+}
+
+void rtl92e_btc_mediastatus_notify(struct rtl_priv *rtlpriv,
+				   enum rt_media_status mstatus)
+{
+	exhalbtc92e_mediastatus_notify(&gl92e_bt_coexist, mstatus);
+}
+
+void rtl92e_btc_periodical(struct rtl_priv *rtlpriv)
+{
+	exhalbtc92e_periodical(&gl92e_bt_coexist);
+}
+
+void rtl92e_btc_halt_notify(void)
+{
+	exhalbtc92e_halt_notify(&gl92e_bt_coexist);
+}
+
+void rtl92e_btc_btinfo_notify(struct rtl_priv *rtlpriv, u8 *tmp_buf, u8 length)
+{
+	exhalbtc92e_bt_info_notify(&gl92e_bt_coexist, tmp_buf, length);
+}
+
+bool rtl92e_btc_is_limited_dig(struct rtl_priv *rtlpriv)
+{
+	return gl92e_bt_coexist.bt_info.limited_dig;
+}
+
+bool rtl92e_btc_is_disable_edca_turbo(struct rtl_priv *rtlpriv)
+{
+	bool bt_change_edca = false;
+	u32 cur_edca_val;
+	u32 edca_bt_hs_uplink = 0x5ea42b, edca_bt_hs_downlink = 0x5ea42b;
+	u32 edca_hs;
+	u32 edca_addr = 0x504;
+
+	cur_edca_val = rtl_read_dword(rtlpriv, edca_addr);
+	if (halbtc92e_is_wifi_uplink(rtlpriv)) {
+		if (cur_edca_val != edca_bt_hs_uplink) {
+			edca_hs = edca_bt_hs_uplink;
+			bt_change_edca = true;
+		}
+	} else {
+		if (cur_edca_val != edca_bt_hs_downlink) {
+			edca_hs = edca_bt_hs_downlink;
+			bt_change_edca = true;
+		}
+	}
+
+	if (bt_change_edca)
+		rtl_write_dword(rtlpriv, edca_addr, edca_hs);
+
+	return true;
+}
+
+bool rtl92e_btc_is_bt_disabled(struct rtl_priv *rtlpriv)
+{
+	/* It seems 'bt_disabled' is never be initialized or set. */
+	if (gl92e_bt_coexist.bt_info.bt_disabled)
+		return true;
+	else
+		return false;
+}
+
+void rtl_btc_special_packet_notify(struct rtl_priv *rtlpriv, u8 pkt_type)
+{
+	return exhalbtc92e_special_packet_notify(&gl92e_bt_coexist, pkt_type);
+}
+
+struct rtl_btc_ops *stg_rtl_btc_get_ops_pointer(void)
+{
+	return &rtl_btc_operation;
+}
+EXPORT_SYMBOL(stg_rtl_btc_get_ops_pointer);
+
+u8 rtl92e_get_hwpg_ant_num(struct rtl_priv *rtlpriv)
+{
+	u8 num;
+
+	if (rtlpriv->btcoexist.btc_info.ant_num == ANT_X2)
+		num = 2;
+	else
+		num = 1;
+
+	return num;
+}
+u8 rtl92e_get_hwpg_bt_exist(struct rtl_priv *rtlpriv)
+{
+	return rtlpriv->btcoexist.btc_info.btcoexist;
+}
+
+u8 rtl92e_get_hwpg_bt_type(struct rtl_priv *rtlpriv)
+{
+	return rtlpriv->btcoexist.btc_info.bt_type;
+}
diff --git a/drivers/staging/rtl8192ee/btcoexist/rtl_btc.h b/drivers/staging/rtl8192ee/btcoexist/rtl_btc.h
new file mode 100644
index 00000000000..9530eb1a664
--- /dev/null
+++ b/drivers/staging/rtl8192ee/btcoexist/rtl_btc.h
@@ -0,0 +1,62 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL_BTC_H__
+#define __RTL_BTC_H__
+
+#include "halbt_precomp.h"
+
+void rtl92e_btc_init_variables(struct rtl_priv *rtlpriv);
+void rtl92e_btc_init_hal_vars(struct rtl_priv *rtlpriv);
+void rtl92e_btc_init_hw_config(struct rtl_priv *rtlpriv);
+void rtl92e_btc_ips_notify(struct rtl_priv *rtlpriv, u8 type);
+void rtl_btc_lps_notify(struct rtl_priv *rtlpriv, u8 type);
+void rtl92e_btc_scan_notify(struct rtl_priv *rtlpriv, u8 scantype);
+void rtl92e_btc_connect_notify(struct rtl_priv *rtlpriv, u8 action);
+void rtl92e_btc_mediastatus_notify(struct rtl_priv *rtlpriv,
+				   enum rt_media_status mstatus);
+void rtl92e_btc_periodical(struct rtl_priv *rtlpriv);
+void rtl92e_btc_halt_notify(void);
+void rtl92e_btc_btinfo_notify(struct rtl_priv *rtlpriv, u8 *tmpbuf, u8 length);
+bool rtl92e_btc_is_limited_dig(struct rtl_priv *rtlpriv);
+bool rtl92e_btc_is_disable_edca_turbo(struct rtl_priv *rtlpriv);
+bool rtl92e_btc_is_bt_disabled(struct rtl_priv *rtlpriv);
+void rtl_btc_special_packet_notify(struct rtl_priv *rtlpriv, u8 pkt_type);
+
+
+struct rtl_btc_ops *stg_rtl_btc_get_ops_pointer(void);
+
+u8 rtl92e_get_hwpg_ant_num(struct rtl_priv *rtlpriv);
+u8 rtl92e_get_hwpg_bt_exist(struct rtl_priv *rtlpriv);
+u8 rtl92e_get_hwpg_bt_type(struct rtl_priv *rtlpriv);
+enum rt_media_status mgnt_link_status_query(struct ieee80211_hw *hw);
+
+
+
+
+
+
+
+
+#endif
diff --git a/drivers/staging/rtl8192ee/cam.c b/drivers/staging/rtl8192ee/cam.c
new file mode 100644
index 00000000000..e32c329da87
--- /dev/null
+++ b/drivers/staging/rtl8192ee/cam.c
@@ -0,0 +1,337 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+#include "wifi.h"
+#include "cam.h"
+
+void rtl92e_cam_reset_sec_info(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->sec.use_defaultkey = false;
+	rtlpriv->sec.pairwise_enc_algorithm = NO_ENCRYPTION;
+	rtlpriv->sec.group_enc_algorithm = NO_ENCRYPTION;
+	memset(rtlpriv->sec.key_buf, 0, KEY_BUF_SIZE * MAX_KEY_LEN);
+	memset(rtlpriv->sec.key_len, 0, KEY_BUF_SIZE);
+	rtlpriv->sec.pairwise_key = NULL;
+}
+
+static void rtl_cam_program_entry(struct ieee80211_hw *hw, u32 entry_no,
+				  u8 *mac_addr, u8 *key_cont_128, u16 us_config)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	u32 target_command;
+	u32 target_content = 0;
+	u8 entry_i;
+
+	RT_PRINT_DATA(rtlpriv, COMP_SEC, DBG_DMESG, "Key content:",
+		      key_cont_128, 16);
+
+	for (entry_i = 0; entry_i < CAM_CONTENT_COUNT; entry_i++) {
+		target_command = entry_i + CAM_CONTENT_COUNT * entry_no;
+		target_command = target_command | BIT(31) | BIT(16);
+
+		if (entry_i == 0) {
+			target_content = (u32) (*(mac_addr + 0)) << 16 |
+			    (u32) (*(mac_addr + 1)) << 24 | (u32) us_config;
+
+			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[WCAMI],
+					target_content);
+			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM],
+					target_command);
+
+			RT_TRACE(COMP_SEC, DBG_LOUD,
+				 ("WRITE %x: %x\n",
+				  rtlpriv->cfg->maps[WCAMI], target_content));
+			RT_TRACE(COMP_SEC, DBG_LOUD,
+				 ("The Key ID is %d\n", entry_no));
+			RT_TRACE(COMP_SEC, DBG_LOUD,
+				 ("WRITE %x: %x\n",
+				  rtlpriv->cfg->maps[RWCAM], target_command));
+		} else if (entry_i == 1) {
+			target_content = (u32) (*(mac_addr + 5)) << 24 |
+			    (u32) (*(mac_addr + 4)) << 16 |
+			    (u32) (*(mac_addr + 3)) << 8 |
+			    (u32) (*(mac_addr + 2));
+
+			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[WCAMI],
+					target_content);
+			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM],
+					target_command);
+
+			RT_TRACE(COMP_SEC, DBG_LOUD,
+				 ("WRITE A4: %x\n", target_content));
+			RT_TRACE(COMP_SEC, DBG_LOUD,
+				 ("WRITE A0: %x\n", target_command));
+		} else {
+			target_content =
+			    (u32) (*(key_cont_128 + (entry_i * 4 - 8) + 3)) <<
+			    24 | (u32) (*(key_cont_128 + (entry_i * 4 - 8) + 2))
+			    << 16 |
+			    (u32) (*(key_cont_128 + (entry_i * 4 - 8) + 1)) << 8
+			    | (u32) (*(key_cont_128 + (entry_i * 4 - 8) + 0));
+
+			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[WCAMI],
+					target_content);
+			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM],
+					target_command);
+			udelay(100);
+
+			RT_TRACE(COMP_SEC, DBG_LOUD,
+				 ("WRITE A4: %x\n", target_content));
+			RT_TRACE(COMP_SEC, DBG_LOUD,
+				 ("WRITE A0: %x\n", target_command));
+		}
+	}
+
+	RT_TRACE(COMP_SEC, DBG_LOUD,
+		 ("after set key, usconfig:%x\n", us_config));
+}
+
+u8 stg_rtl_cam_add_one_entry(struct ieee80211_hw *hw, u8 *mac_addr,
+			     u32 ul_key_id, u32 ul_entry_idx, u32 ul_enc_alg,
+			     u32 ul_default_key, u8 *key_content)
+{
+	u32 us_config;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	RT_TRACE(COMP_SEC, DBG_DMESG,
+		 ("EntryNo:%x, ulKeyId=%x, ulEncAlg=%x, ulUseDK=%x MacAddr %pM\n",
+		  ul_entry_idx, ul_key_id, ul_enc_alg,
+		  ul_default_key, mac_addr));
+
+	if (ul_key_id == TOTAL_CAM_ENTRY) {
+		RT_TRACE(COMP_ERR, DBG_WARNING,
+			 ("ulKeyId exceed!\n"));
+		return 0;
+	}
+
+	if (ul_default_key == 1)
+		us_config = CFG_VALID | ((u16) (ul_enc_alg) << 2);
+	else
+		us_config = CFG_VALID | ((ul_enc_alg) << 2) | ul_key_id;
+
+	rtl_cam_program_entry(hw, ul_entry_idx, mac_addr,
+			      (u8 *)key_content, us_config);
+
+	RT_TRACE(COMP_SEC, DBG_DMESG, ("end\n"));
+
+	return 1;
+}
+EXPORT_SYMBOL(stg_rtl_cam_add_one_entry);
+
+int stg_rtl_cam_delete_one_entry(struct ieee80211_hw *hw,
+				 u8 *mac_addr, u32 ul_key_id)
+{
+	u32 ul_command;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	RT_TRACE(COMP_SEC, DBG_DMESG, ("key_idx:%d\n", ul_key_id));
+
+	ul_command = ul_key_id * CAM_CONTENT_COUNT;
+	ul_command = ul_command | BIT(31) | BIT(16);
+
+	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[WCAMI], 0);
+	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM], ul_command);
+
+	RT_TRACE(COMP_SEC, DBG_DMESG,
+		 ("stg_rtl_cam_delete_one_entry(): WRITE A4: %x\n", 0));
+	RT_TRACE(COMP_SEC, DBG_DMESG,
+		 ("stg_rtl_cam_delete_one_entry(): WRITE A0: %x\n",
+		  ul_command));
+	return 0;
+}
+EXPORT_SYMBOL(stg_rtl_cam_delete_one_entry);
+
+void stg_rtl_cam_reset_all_entry(struct ieee80211_hw *hw)
+{
+	u32 ul_command;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	ul_command = BIT(31) | BIT(30);
+	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM], ul_command);
+}
+EXPORT_SYMBOL(stg_rtl_cam_reset_all_entry);
+
+void stg_rtl_cam_mark_invalid(struct ieee80211_hw *hw, u8 uc_index)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	u32 ul_command;
+	u32 ul_content;
+	u32 ul_enc_algo = rtlpriv->cfg->maps[SEC_CAM_AES];
+
+	switch (rtlpriv->sec.pairwise_enc_algorithm) {
+	case WEP40_ENCRYPTION:
+		ul_enc_algo = rtlpriv->cfg->maps[SEC_CAM_WEP40];
+		break;
+	case WEP104_ENCRYPTION:
+		ul_enc_algo = rtlpriv->cfg->maps[SEC_CAM_WEP104];
+		break;
+	case TKIP_ENCRYPTION:
+		ul_enc_algo = rtlpriv->cfg->maps[SEC_CAM_TKIP];
+		break;
+	case AESCCMP_ENCRYPTION:
+		ul_enc_algo = rtlpriv->cfg->maps[SEC_CAM_AES];
+		break;
+	default:
+		ul_enc_algo = rtlpriv->cfg->maps[SEC_CAM_AES];
+	}
+
+	ul_content = (uc_index & 3) | ((u16) (ul_enc_algo) << 2);
+
+	ul_content |= BIT(15);
+	ul_command = CAM_CONTENT_COUNT * uc_index;
+	ul_command = ul_command | BIT(31) | BIT(16);
+
+	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[WCAMI], ul_content);
+	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM], ul_command);
+
+	RT_TRACE(COMP_SEC, DBG_DMESG,
+		 ("stg_rtl_cam_mark_invalid(): WRITE A4: %x\n", ul_content));
+	RT_TRACE(COMP_SEC, DBG_DMESG,
+		 ("stg_rtl_cam_mark_invalid(): WRITE A0: %x\n", ul_command));
+}
+EXPORT_SYMBOL(stg_rtl_cam_mark_invalid);
+
+void stg_rtl_cam_empty_entry(struct ieee80211_hw *hw, u8 uc_index)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	u32 ul_command;
+	u32 ul_content;
+	u32 ul_encalgo = rtlpriv->cfg->maps[SEC_CAM_AES];
+	u8 entry_i;
+
+	switch (rtlpriv->sec.pairwise_enc_algorithm) {
+	case WEP40_ENCRYPTION:
+		ul_encalgo = rtlpriv->cfg->maps[SEC_CAM_WEP40];
+		break;
+	case WEP104_ENCRYPTION:
+		ul_encalgo = rtlpriv->cfg->maps[SEC_CAM_WEP104];
+		break;
+	case TKIP_ENCRYPTION:
+		ul_encalgo = rtlpriv->cfg->maps[SEC_CAM_TKIP];
+		break;
+	case AESCCMP_ENCRYPTION:
+		ul_encalgo = rtlpriv->cfg->maps[SEC_CAM_AES];
+		break;
+	default:
+		ul_encalgo = rtlpriv->cfg->maps[SEC_CAM_AES];
+	}
+
+	for (entry_i = 0; entry_i < CAM_CONTENT_COUNT; entry_i++) {
+		if (entry_i == 0) {
+			ul_content =
+			    (uc_index & 0x03) | ((u16) (ul_encalgo) << 2);
+			ul_content |= BIT(15);
+
+		} else {
+			ul_content = 0;
+		}
+
+		ul_command = CAM_CONTENT_COUNT * uc_index + entry_i;
+		ul_command = ul_command | BIT(31) | BIT(16);
+
+		rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[WCAMI], ul_content);
+		rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM], ul_command);
+
+		RT_TRACE(COMP_SEC, DBG_LOUD,
+			 ("stg_rtl_cam_empty_entry(): WRITE A4: %x\n",
+			  ul_content));
+		RT_TRACE(COMP_SEC, DBG_LOUD,
+			 ("stg_rtl_cam_empty_entry(): WRITE A0: %x\n",
+			  ul_command));
+	}
+}
+EXPORT_SYMBOL(stg_rtl_cam_empty_entry);
+
+u8 stg_rtl_cam_get_free_entry(struct ieee80211_hw *hw, u8 *sta_addr)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 bitmap = (rtlpriv->sec.hwsec_cam_bitmap) >> 4;
+	u8 entry_idx = 0;
+	u8 i, *addr;
+
+	if (!sta_addr) {
+		RT_TRACE(COMP_SEC, DBG_EMERG,
+			 ("sta_addr is NULL\n"));
+		return TOTAL_CAM_ENTRY;
+	}
+	/* Does STA already exist? */
+	for (i = 4; i < TOTAL_CAM_ENTRY; i++) {
+		addr = rtlpriv->sec.hwsec_cam_sta_addr[i];
+		if (memcmp(addr, sta_addr, ETH_ALEN) == 0)
+			return i;
+	}
+	/* Get a free CAM entry. */
+	for (entry_idx = 4; entry_idx < TOTAL_CAM_ENTRY; entry_idx++) {
+		if ((bitmap & BIT(0)) == 0) {
+			RT_TRACE(COMP_SEC, DBG_EMERG,
+				 ("-----hwsec_cam_bitmap: 0x%x entry_idx=%d\n",
+					  rtlpriv->sec.hwsec_cam_bitmap, entry_idx));
+			rtlpriv->sec.hwsec_cam_bitmap |= BIT(0) << entry_idx;
+			memcpy(rtlpriv->sec.hwsec_cam_sta_addr[entry_idx],
+			       sta_addr, ETH_ALEN);
+			return entry_idx;
+		}
+		bitmap = bitmap >> 1;
+	}
+	return TOTAL_CAM_ENTRY;
+}
+EXPORT_SYMBOL(stg_rtl_cam_get_free_entry);
+
+void stg_rtl_cam_del_entry(struct ieee80211_hw *hw, u8 *sta_addr)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 bitmap;
+	u8 i, *addr;
+
+	if (NULL == sta_addr) {
+		RT_TRACE(COMP_SEC, DBG_EMERG,
+			 ("sta_addr is NULL.\n"));
+		return;
+	}
+
+	if (is_zero_ether_addr(sta_addr)) {
+		RT_TRACE(COMP_SEC, DBG_EMERG,
+			 ("sta_addr is 00:00:00:00:00:00.\n"));
+		return;
+	}
+	/* Does STA already exist? */
+	for (i = 4; i < TOTAL_CAM_ENTRY; i++) {
+		addr = rtlpriv->sec.hwsec_cam_sta_addr[i];
+		bitmap = (rtlpriv->sec.hwsec_cam_bitmap) >> i;
+		if (((bitmap & BIT(0)) == BIT(0)) &&
+		    (memcmp(addr, sta_addr, ETH_ALEN) == 0)) {
+			/* Remove from HW Security CAM */
+			memset(rtlpriv->sec.hwsec_cam_sta_addr[i], 0, ETH_ALEN);
+			rtlpriv->sec.hwsec_cam_bitmap &= ~(BIT(0) << i);
+			pr_info("&&&&&&&&&del entry %d\n", i);
+		}
+	}
+	return;
+}
+EXPORT_SYMBOL(stg_rtl_cam_del_entry);
diff --git a/drivers/staging/rtl8192ee/cam.h b/drivers/staging/rtl8192ee/cam.h
new file mode 100644
index 00000000000..b3a9464e7ed
--- /dev/null
+++ b/drivers/staging/rtl8192ee/cam.h
@@ -0,0 +1,52 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL_CAM_H_
+#define __RTL_CAM_H_
+
+#define CAM_CONTENT_COUNT				8
+
+#define CFG_DEFAULT_KEY					BIT(5)
+#define CFG_VALID					BIT(15)
+
+#define PAIRWISE_KEYIDX					0
+#define CAM_PAIRWISE_KEY_POSITION			4
+
+#define	CAM_CONFIG_USEDK				1
+#define	CAM_CONFIG_NO_USEDK				0
+
+void stg_rtl_cam_reset_all_entry(struct ieee80211_hw *hw);
+u8 stg_rtl_cam_add_one_entry(struct ieee80211_hw *hw, u8 *mac_addr,
+			     u32 ul_key_id, u32 ul_entry_idx, u32 ul_enc_alg,
+			     u32 ul_default_key, u8 *key_content);
+int stg_rtl_cam_delete_one_entry(struct ieee80211_hw *hw, u8 *mac_addr,
+				 u32 ul_key_id);
+void stg_rtl_cam_mark_invalid(struct ieee80211_hw *hw, u8 uc_index);
+void stg_rtl_cam_empty_entry(struct ieee80211_hw *hw, u8 uc_index);
+void rtl92e_cam_reset_sec_info(struct ieee80211_hw *hw);
+u8 stg_rtl_cam_get_free_entry(struct ieee80211_hw *hw, u8 *sta_addr);
+void stg_rtl_cam_del_entry(struct ieee80211_hw *hw, u8 *sta_addr);
+
+#endif
diff --git a/drivers/staging/rtl8192ee/compat.h b/drivers/staging/rtl8192ee/compat.h
new file mode 100644
index 00000000000..72a3c13afed
--- /dev/null
+++ b/drivers/staging/rtl8192ee/compat.h
@@ -0,0 +1,70 @@
+#ifndef __RTL_COMPAT_H__
+#define __RTL_COMPAT_H__
+
+
+#define RX_FLAG_MACTIME_MPDU RX_FLAG_MACTIME_START
+
+#define IEEE80211_KEY_FLAG_SW_MGMT IEEE80211_KEY_FLAG_SW_MGMT_TX
+
+struct ieee80211_mgmt_compat {
+	__le16 frame_control;
+	__le16 duration;
+	u8 da[6];
+	u8 sa[6];
+	u8 bssid[6];
+	__le16 seq_ctrl;
+	union {
+		struct {
+			u8 category;
+			union {
+				struct {
+					u8 action_code;
+					u8 dialog_token;
+					u8 status_code;
+					u8 variable[0];
+				} __packed wme_action;
+				struct {
+					u8 action_code;
+					u8 dialog_token;
+					__le16 capab;
+					__le16 timeout;
+					__le16 start_seq_num;
+				} __packed addba_req;
+				struct{
+					u8 action_code;
+					u8 dialog_token;
+					__le16 status;
+					__le16 capab;
+					__le16 timeout;
+				} __packed addba_resp;
+				struct {
+					u8 action_code;
+					__le16 params;
+					__le16 reason_code;
+				} __packed delba;
+				struct {
+					u8 action_code;
+					/* capab_info for open and confirm,
+					 * reason for close
+					 */
+					__le16 aux;
+					/* Followed in plink_confirm by status
+					 * code, AID and supported rates,
+					 * and directly by supported rates in
+					 * plink_open and plink_close
+					 */
+					u8 variable[0];
+				} __packed plink_action;
+				struct {
+					u8 action_code;
+					u8 variable[0];
+				} __packed mesh_action;
+				struct {
+					u8 action;
+					u8 smps_control;
+				} __packed ht_smps;
+			} u;
+		} __packed action;
+	} u;
+} __packed;
+#endif
diff --git a/drivers/staging/rtl8192ee/core.c b/drivers/staging/rtl8192ee/core.c
new file mode 100644
index 00000000000..76ea356163b
--- /dev/null
+++ b/drivers/staging/rtl8192ee/core.c
@@ -0,0 +1,1600 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "wifi.h"
+#include "core.h"
+#include "cam.h"
+#include "base.h"
+#include "ps.h"
+
+#include "btcoexist/rtl_btc.h"
+
+/*mutex for start & stop is must here. */
+static int rtl_op_start(struct ieee80211_hw *hw)
+{
+	int err = 0;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	if (!is_hal_stop(rtlhal))
+		return 0;
+	if (!test_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status))
+		return 0;
+	mutex_lock(&rtlpriv->locks.conf_mutex);
+	err = rtlpriv->intf_ops->adapter_start(hw);
+	if (err)
+		goto out;
+	rtl92e_watch_dog_timer_callback((unsigned long)hw);
+
+out:
+	mutex_unlock(&rtlpriv->locks.conf_mutex);
+	return err;
+}
+
+static void rtl_op_stop(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	bool b_support_remote_wakeup = false;
+
+	if (is_hal_stop(rtlhal))
+		return;
+
+	rtlpriv->cfg->ops->get_hw_reg(hw, HAL_DEF_WOWLAN,
+				      (u8 *)(&b_support_remote_wakeup));
+	/* here is must, because adhoc do stop and start,
+	 * but stop with RFOFF may cause something wrong,
+	 * like adhoc TP */
+	if (unlikely(ppsc->rfpwr_state == ERFOFF))
+		rtl92e_ips_nic_on(hw);
+
+	mutex_lock(&rtlpriv->locks.conf_mutex);
+	/* if wowlan supported, DON'T clear connected info */
+	if (!(b_support_remote_wakeup &&
+	      rtlhal->b_enter_pnp_sleep)) {
+		mac->link_state = MAC80211_NOLINK;
+		memset(mac->bssid, 0, 6);
+		mac->vendor = PEER_UNKNOWN;
+
+		/* reset sec info */
+		rtl92e_cam_reset_sec_info(hw);
+
+		rtl92e_deinit_deferred_work(hw);
+	}
+	rtlpriv->intf_ops->adapter_stop(hw);
+
+	mutex_unlock(&rtlpriv->locks.conf_mutex);
+}
+
+static void rtl_op_tx(struct ieee80211_hw *hw,
+		      struct ieee80211_tx_control *control,
+		      struct sk_buff *skb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_tcb_desc tcb_desc;
+	memset(&tcb_desc, 0, sizeof(struct rtl_tcb_desc));
+
+	if (unlikely(is_hal_stop(rtlhal) || ppsc->rfpwr_state != ERFON))
+		goto err_free;
+
+	if (!test_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status))
+		goto err_free;
+
+	if (!rtlpriv->intf_ops->waitq_insert(hw, control->sta, skb))
+		rtlpriv->intf_ops->adapter_tx(hw, control->sta, skb, &tcb_desc);
+	return;
+
+err_free:
+	dev_kfree_skb_any(skb);
+	return;
+}
+
+static int rtl_op_add_interface(struct ieee80211_hw *hw,
+				struct ieee80211_vif *vif)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	int err = 0;
+
+	if (mac->vif) {
+		RT_TRACE(COMP_ERR, DBG_WARNING,
+			 ("vif has been set!! mac->vif = 0x%p\n", mac->vif));
+		return -EOPNOTSUPP;
+	}
+
+	vif->driver_flags |= IEEE80211_VIF_BEACON_FILTER;
+
+	rtl92e_ips_nic_on(hw);
+
+	mutex_lock(&rtlpriv->locks.conf_mutex);
+	switch (ieee80211_vif_type_p2p(vif)) {
+	case NL80211_IFTYPE_P2P_CLIENT:
+		mac->p2p = P2P_ROLE_CLIENT;
+		/*fall through*/
+	case NL80211_IFTYPE_STATION:
+		if (mac->beacon_enabled == 1) {
+			RT_TRACE(COMP_MAC80211, DBG_LOUD,
+				 ("NL80211_IFTYPE_STATION\n"));
+			mac->beacon_enabled = 0;
+			rtlpriv->cfg->ops->update_interrupt_mask(hw, 0,
+					rtlpriv->cfg->maps[RTL_IBSS_INT_MASKS]);
+		}
+		break;
+	case NL80211_IFTYPE_ADHOC:
+		RT_TRACE(COMP_MAC80211, DBG_LOUD,
+			 ("NL80211_IFTYPE_ADHOC\n"));
+		mac->link_state = MAC80211_LINKED;
+		rtlpriv->cfg->ops->set_bcn_reg(hw);
+		if (rtlpriv->rtlhal.current_bandtype == BAND_ON_2_4G)
+			mac->basic_rates = 0xfff;
+		else
+			mac->basic_rates = 0xff0;
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BASIC_RATE,
+				(u8 *)(&mac->basic_rates));
+		break;
+	case NL80211_IFTYPE_P2P_GO:
+		mac->p2p = P2P_ROLE_GO;
+		/*fall through*/
+	case NL80211_IFTYPE_AP:
+		RT_TRACE(COMP_MAC80211, DBG_LOUD,
+			 ("NL80211_IFTYPE_AP\n"));
+
+		mac->link_state = MAC80211_LINKED;
+		rtlpriv->cfg->ops->set_bcn_reg(hw);
+		if (rtlpriv->rtlhal.current_bandtype == BAND_ON_2_4G)
+			mac->basic_rates = 0xfff;
+		else
+			mac->basic_rates = 0xff0;
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BASIC_RATE,
+					      (u8 *)(&mac->basic_rates));
+		break;
+	case NL80211_IFTYPE_MESH_POINT:
+		RT_TRACE(COMP_MAC80211, DBG_LOUD,
+			 ("NL80211_IFTYPE_MESH_POINT\n"));
+
+		mac->link_state = MAC80211_LINKED;
+		rtlpriv->cfg->ops->set_bcn_reg(hw);
+		if (rtlpriv->rtlhal.current_bandtype == BAND_ON_2_4G)
+			mac->basic_rates = 0xfff;
+		else
+			mac->basic_rates = 0xff0;
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BASIC_RATE,
+				(u8 *)(&mac->basic_rates));
+		break;
+	default:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("operation mode %d is not support!\n", vif->type));
+		err = -EOPNOTSUPP;
+		goto out;
+	}
+
+#ifdef VIF_TODO
+	if (!rtl_set_vif_info(hw, vif))
+		goto out;
+#endif
+
+	if (mac->p2p) {
+		RT_TRACE(COMP_MAC80211, DBG_LOUD,
+			 ("p2p role %x\n", vif->type));
+		mac->basic_rates = 0xff0;/*disable cck rate for p2p*/
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BASIC_RATE,
+				(u8 *)(&mac->basic_rates));
+	}
+	mac->vif = vif;
+	mac->opmode = vif->type;
+	rtlpriv->cfg->ops->set_network_type(hw, vif->type);
+	ether_addr_copy(mac->mac_addr, vif->addr);
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ETHER_ADDR, mac->mac_addr);
+
+out:
+	mutex_unlock(&rtlpriv->locks.conf_mutex);
+	return err;
+}
+
+static void rtl_op_remove_interface(struct ieee80211_hw *hw,
+				    struct ieee80211_vif *vif)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+
+	mutex_lock(&rtlpriv->locks.conf_mutex);
+
+	/* Free beacon resources */
+	if ((vif->type == NL80211_IFTYPE_AP) ||
+	    (vif->type == NL80211_IFTYPE_ADHOC) ||
+	    (vif->type == NL80211_IFTYPE_MESH_POINT)) {
+		if (mac->beacon_enabled == 1) {
+			mac->beacon_enabled = 0;
+			rtlpriv->cfg->ops->update_interrupt_mask(hw, 0,
+					rtlpriv->cfg->maps[RTL_IBSS_INT_MASKS]);
+		}
+	}
+
+	/*
+	 *Note: We assume NL80211_IFTYPE_UNSPECIFIED as
+	 *NO LINK for our hardware.
+	 */
+	mac->p2p = 0;
+	mac->vif = NULL;
+	mac->link_state = MAC80211_NOLINK;
+	memset(mac->bssid, 0, 6);
+	mac->vendor = PEER_UNKNOWN;
+	mac->opmode = NL80211_IFTYPE_UNSPECIFIED;
+	rtlpriv->cfg->ops->set_network_type(hw, mac->opmode);
+
+	mutex_unlock(&rtlpriv->locks.conf_mutex);
+}
+/*<delete in kernel start>*/
+static int rtl_op_change_interface(struct ieee80211_hw *hw,
+				   struct ieee80211_vif *vif,
+				   enum nl80211_iftype new_type, bool p2p)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int ret;
+	rtl_op_remove_interface(hw, vif);
+
+	vif->type = new_type;
+	vif->p2p = p2p;
+	ret = rtl_op_add_interface(hw, vif);
+	RT_TRACE(COMP_MAC80211, DBG_LOUD,
+		 (" p2p  %x\n", p2p));
+	return ret;
+}
+
+#ifdef CONFIG_PM
+static u16 crc16_ccitt(u8 data, u16 crc)
+{
+	u8 shift_in, data_bit, crc_bit11, crc_bit4, crc_bit15;
+	u8 i;
+	u16 result;
+
+	for (i = 0; i < 8; i++) {
+		crc_bit15 = ((crc & BIT(15)) ? 1 : 0);
+		data_bit  = (data & (BIT(0) << i) ? 1 : 0);
+		shift_in = crc_bit15 ^ data_bit;
+
+		result = crc << 1;
+		if (shift_in == 0)
+			result &= (~BIT(0));
+		else
+			result |= BIT(0);
+
+		crc_bit11 = ((crc & BIT(11)) ? 1 : 0) ^ shift_in;
+		if (crc_bit11 == 0)
+			result &= (~BIT(12));
+		else
+			result |= BIT(12);
+
+		crc_bit4 = ((crc & BIT(4)) ? 1 : 0) ^ shift_in;
+		if (crc_bit4 == 0)
+			result &= (~BIT(5));
+		else
+			result |= BIT(5);
+
+		crc = result;
+	}
+
+	return crc;
+}
+
+static u16 _calculate_wol_pattern_crc(u8 *pattern, u16 len)
+{
+	u16 crc = 0xffff;
+	u32 i;
+
+	for (i = 0; i < len; i++)
+		crc = crc16_ccitt(pattern[i], crc);
+	crc = ~crc;
+
+	return crc;
+}
+
+static void _rtl_add_wowlan_patterns(struct ieee80211_hw *hw,
+				     struct cfg80211_wowlan *wow)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = &(rtlpriv->mac80211);
+	struct cfg80211_pkt_pattern *patterns = wow->patterns;
+	struct rtl_wow_pattern rtl_pattern;
+	u8 *pattern_os, *mask_os;
+	u8 mask[MAX_WOL_BIT_MASK_SIZE] = {0};
+	u8 content[MAX_WOL_PATTERN_SIZE] = {0};
+	u8 broadcast_addr[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	u8 multicast_addr1[2] = {0x33, 0x33};
+	u8 multicast_addr2[3] = {0x01, 0x00, 0x5e};
+	u8 i, mask_len;
+	u16 j, len;
+
+	for (i = 0; i < wow->n_patterns; i++) {
+		memset(&rtl_pattern, 0, sizeof(struct rtl_wow_pattern));
+		memset(mask, 0, MAX_WOL_BIT_MASK_SIZE);
+		if (patterns[i].pattern_len > MAX_WOL_PATTERN_SIZE) {
+			RT_TRACE(COMP_POWER, DBG_WARNING,
+				 ("Pattern[%d] is too long\n", i));
+			continue;
+		}
+		pattern_os = patterns[i].pattern;
+		mask_len = DIV_ROUND_UP(patterns[i].pattern_len, 8);
+		mask_os = patterns[i].mask;
+		RT_PRINT_DATA(rtlpriv, COMP_POWER, DBG_TRACE,
+			      "pattern content\n", pattern_os,
+			      patterns[i].pattern_len);
+		RT_PRINT_DATA(rtlpriv, COMP_POWER, DBG_TRACE,
+			      "mask content\n", mask_os, mask_len);
+		/* 1. unicast? multicast? or broadcast? */
+		if (memcmp(pattern_os, broadcast_addr, 6) == 0)
+			rtl_pattern.type = BROADCAST_PATTERN;
+		else if (memcmp(pattern_os, multicast_addr1, 2) == 0 ||
+			 memcmp(pattern_os, multicast_addr2, 3) == 0)
+			rtl_pattern.type = MULTICAST_PATTERN;
+		else if  (memcmp(pattern_os, mac->mac_addr, 6) == 0)
+			rtl_pattern.type = UNICAST_PATTERN;
+		else
+			rtl_pattern.type = UNKNOWN_TYPE;
+
+		/* 2. translate mask_from_os to mask_for_hw */
+
+/******************************************************************************
+ * pattern from OS uses 'ethenet frame', like this:
+
+		   |    6   |    6   |   2  |     20    |  Variable  |	4  |
+		   |--------+--------+------+-----------+------------+-----|
+		   |    802.3 Mac Header    | IP Header | TCP Packet | FCS |
+		   |   DA   |   SA   | Type |
+
+ * BUT, packet catched by our HW is in '802.11 frame', begin from LLC,
+
+	|     24 or 30      |    6   |   2  |     20    |  Variable  |  4  |
+	|-------------------+--------+------+-----------+------------+-----|
+	| 802.11 MAC Header |       LLC     | IP Header | TCP Packet | FCS |
+			    | Others | Tpye |
+
+ * Therefore, we need translate mask_from_OS to mask_to_hw.
+ * We should left-shift mask by 6 bits, then set the new bit[0~5] = 0,
+ * because new mask[0~5] means 'SA', but our HW packet begins from LLC,
+ * bit[0~5] corresponds to first 6 Bytes in LLC, they just don't match.
+ ******************************************************************************/
+
+		/* Shift 6 bits */
+		for (j = 0; j < mask_len - 1; j++) {
+			mask[j] = mask_os[j] >> 6;
+			mask[j] |= (mask_os[j + 1] & 0x3F) << 2;
+		}
+		mask[j] = (mask_os[j] >> 6) & 0x3F;
+		/* Set bit 0-5 to zero */
+		mask[0] &= 0xC0;
+
+		RT_PRINT_DATA(rtlpriv, COMP_POWER, DBG_TRACE,
+			      "mask to hw\n", mask, mask_len);
+		for (j = 0; j < (MAX_WOL_BIT_MASK_SIZE + 1) / 4; j++) {
+			rtl_pattern.mask[j] = mask[j * 4];
+			rtl_pattern.mask[j] |= (mask[j * 4 + 1] << 8);
+			rtl_pattern.mask[j] |= (mask[j * 4 + 2] << 16);
+			rtl_pattern.mask[j] |= (mask[j * 4 + 3] << 24);
+		}
+
+		/* To get the wake up pattern from the mask.
+		 * We do not count first 12 bits which means
+		 * DA[6] and SA[6] in the pattern to match HW design. */
+		len = 0;
+		for (j = 12; j < patterns[i].pattern_len; j++) {
+			if ((mask_os[j / 8] >> (j % 8)) & 0x01) {
+				content[len] = pattern_os[j];
+				len++;
+			}
+		}
+
+		RT_PRINT_DATA(rtlpriv, COMP_POWER, DBG_TRACE,
+			      "pattern to hw\n", content, len);
+		/* 3. calculate crc */
+		rtl_pattern.crc = _calculate_wol_pattern_crc(content, len);
+		RT_TRACE(COMP_POWER, DBG_TRACE,
+			 ("CRC_Remainder = 0x%x", rtl_pattern.crc));
+
+		/* 4. write crc & mask_for_hw to hw */
+		rtlpriv->cfg->ops->add_wowlan_pattern(hw, &rtl_pattern, i);
+	}
+	rtl_write_byte(rtlpriv, 0x698, wow->n_patterns);
+}
+
+static int rtl_op_suspend(struct ieee80211_hw *hw,
+			  struct cfg80211_wowlan *wow)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct timeval ts;
+
+	RT_TRACE(COMP_POWER, DBG_DMESG, ("\n"));
+	if (WARN_ON(!wow))
+		return -EINVAL;
+
+	/* to resolve s4 can not wake up*/
+	do_gettimeofday(&ts);
+	rtlhal->last_suspend_sec = ts.tv_sec;
+
+	if ((ppsc->wo_wlan_mode & WAKE_ON_PATTERN_MATCH) && wow->n_patterns)
+		_rtl_add_wowlan_patterns(hw, wow);
+
+	rtlhal->driver_is_goingto_unload = true;
+	rtlhal->b_enter_pnp_sleep = true;
+
+	rtl92e_lps_leave(hw);
+	rtl_op_stop(hw);
+	device_set_wakeup_enable(wiphy_dev(hw->wiphy), true);
+	return 0;
+}
+
+static int rtl_op_resume(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct timeval ts;
+
+	RT_TRACE(COMP_POWER, DBG_DMESG, ("\n"));
+	rtlhal->driver_is_goingto_unload = false;
+	rtlhal->b_enter_pnp_sleep = false;
+	rtlhal->b_wake_from_pnp_sleep = true;
+
+	/* to resovle s4 can not wake up*/
+	do_gettimeofday(&ts);
+	if (ts.tv_sec - rtlhal->last_suspend_sec < 5)
+		return -1;
+
+	rtl_op_start(hw);
+	device_set_wakeup_enable(wiphy_dev(hw->wiphy), false);
+	ieee80211_resume_disconnect(mac->vif);
+	rtlhal->b_wake_from_pnp_sleep = false;
+	return 0;
+}
+#endif
+
+static int rtl_op_config(struct ieee80211_hw *hw, u32 changed)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct ieee80211_conf *conf = &hw->conf;
+
+	if (mac->skip_scan)
+		return 1;
+
+
+	mutex_lock(&rtlpriv->locks.conf_mutex);
+	if (changed & IEEE80211_CONF_CHANGE_LISTEN_INTERVAL) {	/* BIT(2) */
+		RT_TRACE(COMP_MAC80211, DBG_LOUD,
+			 ("IEEE80211_CONF_CHANGE_LISTEN_INTERVAL\n"));
+	}
+
+	/*For IPS */
+	if (changed & IEEE80211_CONF_CHANGE_IDLE) {
+		if (hw->conf.flags & IEEE80211_CONF_IDLE)
+			rtl92e_ips_nic_off(hw);
+		else
+			rtl92e_ips_nic_on(hw);
+	} else {
+		/*
+		 *although rfoff may not cause by ips, but we will
+		 *check the reason in set_rf_power_state function
+		 */
+		if (unlikely(ppsc->rfpwr_state == ERFOFF))
+			rtl92e_ips_nic_on(hw);
+	}
+
+	/*For LPS */
+	if (changed & IEEE80211_CONF_CHANGE_PS) {
+		cancel_delayed_work(&rtlpriv->works.ps_work);
+		cancel_delayed_work(&rtlpriv->works.ps_rfon_wq);
+		if (conf->flags & IEEE80211_CONF_PS) {
+			rtlpriv->psc.sw_ps_enabled = true;
+			/* sleep here is must, or we may recv the beacon and
+			 * cause mac80211 into wrong ps state, this will cause
+			 * power save nullfunc send fail, and further cause
+			 * pkt loss, So sleep must quickly but not immediatly
+			 * because that will cause nullfunc send by mac80211
+			 * fail, and cause pkt loss, we have tested that 5mA
+			 * is worked very well */
+			if (!rtlpriv->psc.multi_buffered)
+				queue_delayed_work(rtlpriv->works.rtl_wq,
+						   &rtlpriv->works.ps_work,
+						   MSECS(5));
+		} else {
+			rtl92e_swlps_rf_awake(hw);
+			rtlpriv->psc.sw_ps_enabled = false;
+		}
+	}
+
+	if (changed & IEEE80211_CONF_CHANGE_RETRY_LIMITS) {
+		RT_TRACE(COMP_MAC80211, DBG_LOUD,
+			 ("IEEE80211_CONF_CHANGE_RETRY_LIMITS %x\n",
+			  hw->conf.long_frame_max_tx_count));
+		mac->retry_long = hw->conf.long_frame_max_tx_count;
+		mac->retry_short = hw->conf.long_frame_max_tx_count;
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_RETRY_LIMIT,
+				(u8 *)(&hw->conf.long_frame_max_tx_count));
+	}
+	if (changed & IEEE80211_CONF_CHANGE_CHANNEL &&
+	    !rtlpriv->proximity.proxim_on) {
+		struct ieee80211_channel *channel = hw->conf.chandef.chan;
+		enum nl80211_chan_width width = hw->conf.chandef.width;
+		u8 wide_chan = (u8) channel->hw_value;
+		enum nl80211_channel_type channel_type = NL80211_CHAN_NO_HT;
+
+		/* channel_type is for 20&40M */
+		if (width < NL80211_CHAN_WIDTH_80)
+			channel_type = cfg80211_get_chandef_type(&(hw->conf.chandef));
+		if (mac->act_scanning)
+			mac->n_channels++;
+
+		if (rtlpriv->dm.supp_phymode_switch &&
+		    mac->link_state < MAC80211_LINKED &&
+		    !mac->act_scanning) {
+			if (rtlpriv->cfg->ops->check_switch_to_dmdp)
+				rtlpriv->cfg->ops->check_switch_to_dmdp(hw);
+		}
+
+		/*
+		 *because we should back channel to
+		 *current_network.chan in in scanning,
+		 *So if set_chan == current_network.chan
+		 *we should set it.
+		 *because mac80211 tell us wrong bw40
+		 *info for cisco1253 bw20, so we modify
+		 *it here based on UPPER & LOWER
+		 */
+
+		if (width >= NL80211_CHAN_WIDTH_80) {
+			if (width == NL80211_CHAN_WIDTH_80) {
+				u32 center_freq = hw->conf.chandef.center_freq1;
+				u32 primary_freq =
+				(u32)hw->conf.chandef.chan->center_freq;
+
+				rtlphy->current_chan_bw =
+					HT_CHANNEL_WIDTH_80;
+				mac->bw_80 = true;
+				mac->bw_40 = true;
+				if (center_freq > primary_freq) {
+					mac->cur_80_prime_sc =
+					PRIME_CHNL_OFFSET_LOWER;
+					if (center_freq - primary_freq == 10) {
+						mac->cur_40_prime_sc =
+						PRIME_CHNL_OFFSET_UPPER;
+
+						wide_chan += 2;
+					} else if (center_freq - primary_freq == 30) {
+						mac->cur_40_prime_sc =
+						PRIME_CHNL_OFFSET_LOWER;
+
+						wide_chan += 6;
+					}
+				} else {
+					mac->cur_80_prime_sc =
+					PRIME_CHNL_OFFSET_UPPER;
+					if (primary_freq - center_freq == 10) {
+						mac->cur_40_prime_sc =
+						PRIME_CHNL_OFFSET_LOWER;
+
+						wide_chan -= 2;
+					} else if (primary_freq - center_freq == 30) {
+						mac->cur_40_prime_sc =
+						PRIME_CHNL_OFFSET_UPPER;
+
+						wide_chan -= 6;
+					}
+				}
+			}
+		} else {
+			switch (channel_type) {
+			case NL80211_CHAN_HT20:
+			case NL80211_CHAN_NO_HT:
+				/* SC */
+				mac->cur_40_prime_sc =
+					PRIME_CHNL_OFFSET_DONT_CARE;
+				rtlphy->current_chan_bw =
+					HT_CHANNEL_WIDTH_20;
+				mac->bw_40 = false;
+				mac->bw_80 = false;
+				break;
+			case NL80211_CHAN_HT40MINUS:
+				/* SC */
+				mac->cur_40_prime_sc =
+					PRIME_CHNL_OFFSET_UPPER;
+				rtlphy->current_chan_bw =
+					HT_CHANNEL_WIDTH_20_40;
+				mac->bw_40 = true;
+				mac->bw_80 = false;
+
+				/*wide channel */
+				wide_chan -= 2;
+				break;
+			case NL80211_CHAN_HT40PLUS:
+				/* SC */
+				mac->cur_40_prime_sc =
+					PRIME_CHNL_OFFSET_LOWER;
+				rtlphy->current_chan_bw =
+					HT_CHANNEL_WIDTH_20_40;
+				mac->bw_40 = true;
+				mac->bw_80 = false;
+				/*wide channel */
+				wide_chan += 2;
+				break;
+			default:
+				mac->bw_40 = false;
+				mac->bw_80 = false;
+				RT_TRACE(COMP_ERR, DBG_EMERG,
+					 ("switch case not processed\n"));
+				break;
+			}
+		}
+
+		if (wide_chan <= 0)
+			wide_chan = 1;
+
+		/* in scanning, when before we offchannel we may send a ps=1
+		 * null to AP, and then we may send a ps = 0 null to AP quickly,
+		 * but first null have cause AP's put lots of packet to hw tx
+		 * buffer, these packet must be tx before off channel so we must
+		 * delay more time to let AP flush these packets before
+		 * offchannel, or dis-association or delete BA will happen by AP
+		 */
+		if (rtlpriv->mac80211.offchan_deley) {
+			rtlpriv->mac80211.offchan_deley = false;
+			mdelay(50);
+		}
+
+		rtlphy->current_channel = wide_chan;
+
+		rtlpriv->cfg->ops->switch_channel(hw);
+		rtlpriv->cfg->ops->set_channel_access(hw);
+		rtlpriv->cfg->ops->set_bw_mode(hw,
+			channel_type);
+	}
+
+	mutex_unlock(&rtlpriv->locks.conf_mutex);
+
+	return 0;
+}
+
+static void rtl_op_configure_filter(struct ieee80211_hw *hw,
+				    unsigned int changed_flags,
+				    unsigned int *new_flags, u64 multicast)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+
+	*new_flags &= RTL_SUPPORTED_FILTERS;
+	if (0 == changed_flags)
+		return;
+
+	/*TODO: we disable broadcase now, so enable here */
+	if (changed_flags & FIF_ALLMULTI) {
+		if (*new_flags & FIF_ALLMULTI) {
+			mac->rx_conf |= rtlpriv->cfg->maps[MAC_RCR_AM] |
+			    rtlpriv->cfg->maps[MAC_RCR_AB];
+			RT_TRACE(COMP_MAC80211, DBG_LOUD,
+				 ("Enable receive multicast frame.\n"));
+		} else {
+			mac->rx_conf &= ~(rtlpriv->cfg->maps[MAC_RCR_AM] |
+					  rtlpriv->cfg->maps[MAC_RCR_AB]);
+			RT_TRACE(COMP_MAC80211, DBG_LOUD,
+				 ("Disable receive multicast frame.\n"));
+		}
+	}
+
+	if (changed_flags & FIF_FCSFAIL) {
+		if (*new_flags & FIF_FCSFAIL) {
+			mac->rx_conf |= rtlpriv->cfg->maps[MAC_RCR_ACRC32];
+			RT_TRACE(COMP_MAC80211, DBG_LOUD,
+				 ("Enable receive FCS error frame.\n"));
+		} else {
+			mac->rx_conf &= ~rtlpriv->cfg->maps[MAC_RCR_ACRC32];
+			RT_TRACE(COMP_MAC80211, DBG_LOUD,
+				 ("Disable receive FCS error frame.\n"));
+		}
+	}
+
+	/* if ssid not set to hw don't check bssid
+	 * here just used for linked scanning, & linked
+	 * and nolink check bssid is set in set network_type */
+	if ((changed_flags & FIF_BCN_PRBRESP_PROMISC) &&
+	    (mac->link_state >= MAC80211_LINKED)) {
+		if (mac->opmode != NL80211_IFTYPE_AP &&
+		    mac->opmode != NL80211_IFTYPE_MESH_POINT) {
+			if (*new_flags & FIF_BCN_PRBRESP_PROMISC)
+				rtlpriv->cfg->ops->set_chk_bssid(hw, false);
+			else
+				rtlpriv->cfg->ops->set_chk_bssid(hw, true);
+		}
+	}
+
+	if (changed_flags & FIF_CONTROL) {
+		if (*new_flags & FIF_CONTROL) {
+			mac->rx_conf |= rtlpriv->cfg->maps[MAC_RCR_ACF];
+
+			RT_TRACE(COMP_MAC80211, DBG_LOUD,
+				 ("Enable receive control frame.\n"));
+		} else {
+			mac->rx_conf &= ~rtlpriv->cfg->maps[MAC_RCR_ACF];
+			RT_TRACE(COMP_MAC80211, DBG_LOUD,
+				 ("Disable receive control frame.\n"));
+		}
+	}
+
+	if (changed_flags & FIF_OTHER_BSS) {
+		if (*new_flags & FIF_OTHER_BSS) {
+			mac->rx_conf |= rtlpriv->cfg->maps[MAC_RCR_AAP];
+			RT_TRACE(COMP_MAC80211, DBG_LOUD,
+				 ("Enable receive other BSS's frame.\n"));
+		} else {
+			mac->rx_conf &= ~rtlpriv->cfg->maps[MAC_RCR_AAP];
+			RT_TRACE(COMP_MAC80211, DBG_LOUD,
+				 ("Disable receive other BSS's frame.\n"));
+		}
+	}
+}
+static int rtl_op_sta_add(struct ieee80211_hw *hw,
+			  struct ieee80211_vif *vif,
+			  struct ieee80211_sta *sta)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_sta_info *sta_entry;
+
+	if (sta) {
+		sta_entry = (struct rtl_sta_info *)sta->drv_priv;
+		spin_lock_bh(&rtlpriv->locks.entry_list_lock);
+		list_add_tail(&sta_entry->list, &rtlpriv->entry_list);
+		spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
+		if (rtlhal->current_bandtype == BAND_ON_2_4G) {
+			sta_entry->wireless_mode = WIRELESS_MODE_G;
+			if (sta->supp_rates[0] <= 0xf)
+				sta_entry->wireless_mode = WIRELESS_MODE_B;
+			if (sta->ht_cap.ht_supported)
+				sta_entry->wireless_mode = WIRELESS_MODE_N_24G;
+
+			if (vif->type == NL80211_IFTYPE_ADHOC)
+				sta_entry->wireless_mode = WIRELESS_MODE_G;
+		} else if (rtlhal->current_bandtype == BAND_ON_5G) {
+			sta_entry->wireless_mode = WIRELESS_MODE_A;
+			if (sta->ht_cap.ht_supported)
+				sta_entry->wireless_mode = WIRELESS_MODE_N_5G;
+			if (sta->vht_cap.vht_supported)
+				sta_entry->wireless_mode = WIRELESS_MODE_AC_5G;
+
+			if (vif->type == NL80211_IFTYPE_ADHOC)
+				sta_entry->wireless_mode = WIRELESS_MODE_A;
+		}
+		/*disable cck rate for p2p*/
+		if (mac->p2p)
+			sta->supp_rates[0] &= 0xfffffff0;
+
+		ether_addr_copy(sta_entry->mac_addr, sta->addr);
+		RT_TRACE(COMP_MAC80211, DBG_DMESG,
+			 ("Add sta addr is %pM\n", sta->addr));
+		rtlpriv->cfg->ops->update_rate_tbl(hw, sta, 0);
+	}
+
+	return 0;
+}
+
+static int rtl_op_sta_remove(struct ieee80211_hw *hw,
+			     struct ieee80211_vif *vif,
+			     struct ieee80211_sta *sta)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_sta_info *sta_entry;
+	if (sta) {
+		RT_TRACE(COMP_MAC80211, DBG_DMESG,
+			 ("Remove sta addr is %pM\n", sta->addr));
+		sta_entry = (struct rtl_sta_info *)sta->drv_priv;
+		sta_entry->wireless_mode = 0;
+		sta_entry->ratr_index = 0;
+		spin_lock_bh(&rtlpriv->locks.entry_list_lock);
+		list_del(&sta_entry->list);
+		spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
+	}
+	return 0;
+}
+static int _rtl_get_hal_qnum(u16 queue)
+{
+	int qnum;
+
+	switch (queue) {
+	case 0:
+		qnum = AC3_VO;
+		break;
+	case 1:
+		qnum = AC2_VI;
+		break;
+	case 2:
+		qnum = AC0_BE;
+		break;
+	case 3:
+		qnum = AC1_BK;
+		break;
+	default:
+		qnum = AC0_BE;
+		break;
+	}
+	return qnum;
+}
+
+/*
+ *for mac80211 VO=0, VI=1, BE=2, BK=3
+ *for rtl819x  BE=0, BK=1, VI=2, VO=3
+ */
+static int rtl_op_conf_tx(struct ieee80211_hw *hw,
+			  struct ieee80211_vif *vif, u16 queue,
+			  const struct ieee80211_tx_queue_params *param)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	int aci;
+
+	if (queue >= AC_MAX) {
+		RT_TRACE(COMP_ERR, DBG_WARNING,
+			 ("queue number %d is incorrect!\n", queue));
+		return -EINVAL;
+	}
+
+	aci = _rtl_get_hal_qnum(queue);
+	mac->ac[aci].aifs = param->aifs;
+	mac->ac[aci].cw_min = cpu_to_le16(param->cw_min);
+	mac->ac[aci].cw_max = cpu_to_le16(param->cw_max);
+	mac->ac[aci].tx_op = cpu_to_le16(param->txop);
+	memcpy(&mac->edca_param[aci], param, sizeof(*param));
+	rtlpriv->cfg->ops->set_qos(hw, aci);
+	return 0;
+}
+
+static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
+				    struct ieee80211_vif *vif,
+				    struct ieee80211_bss_conf *bss_conf,
+				    u32 changed)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+
+	mutex_lock(&rtlpriv->locks.conf_mutex);
+	if ((vif->type == NL80211_IFTYPE_ADHOC) ||
+	    (vif->type == NL80211_IFTYPE_AP) ||
+	    (vif->type == NL80211_IFTYPE_MESH_POINT)) {
+		if ((changed & BSS_CHANGED_BEACON) ||
+		    (changed & BSS_CHANGED_BEACON_ENABLED &&
+		     bss_conf->enable_beacon)) {
+			if (mac->beacon_enabled == 0) {
+				RT_TRACE(COMP_MAC80211, DBG_DMESG,
+					 ("BSS_CHANGED_BEACON_ENABLED\n"));
+
+				/*start hw beacon interrupt. */
+				/*rtlpriv->cfg->ops->set_bcn_reg(hw); */
+				mac->beacon_enabled = 1;
+				rtlpriv->cfg->ops->update_interrupt_mask(hw,
+						rtlpriv->cfg->maps
+						[RTL_IBSS_INT_MASKS], 0);
+
+				if (rtlpriv->cfg->ops->linked_set_reg)
+					rtlpriv->cfg->ops->linked_set_reg(hw);
+			}
+		}
+		if ((changed & BSS_CHANGED_BEACON_ENABLED) &&
+		    !bss_conf->enable_beacon) {
+			if (mac->beacon_enabled == 1) {
+				RT_TRACE(COMP_MAC80211, DBG_DMESG,
+					 ("ADHOC DISABLE BEACON\n"));
+
+				mac->beacon_enabled = 0;
+				rtlpriv->cfg->ops->update_interrupt_mask(hw, 0,
+						rtlpriv->cfg->maps
+						[RTL_IBSS_INT_MASKS]);
+			}
+		}
+		if (changed & BSS_CHANGED_BEACON_INT) {
+			RT_TRACE(COMP_BEACON, DBG_TRACE,
+				 ("BSS_CHANGED_BEACON_INT\n"));
+			mac->beacon_interval = bss_conf->beacon_int;
+			rtlpriv->cfg->ops->set_bcn_intv(hw);
+		}
+	}
+
+	/*TODO: reference to enum ieee80211_bss_change */
+	if (changed & BSS_CHANGED_ASSOC) {
+		u8 mstatus;
+		if (bss_conf->assoc) {
+			struct ieee80211_sta *sta = NULL;
+			u8 keep_alive = 10;
+
+			mstatus = RT_MEDIA_CONNECT;
+			/* we should reset all sec info & cam
+			 * before set cam after linked, we should not
+			 * reset in disassoc, that will cause tkip->wep
+			 * fail because some flag will be wrong */
+			/* reset sec info */
+			rtl92e_cam_reset_sec_info(hw);
+			/* reset cam to fix wep fail issue
+			 * when change from wpa to wep */
+			stg_rtl_cam_reset_all_entry(hw);
+
+			mac->link_state = MAC80211_LINKED;
+			mac->cnt_after_linked = 0;
+			mac->assoc_id = bss_conf->aid;
+			memcpy(mac->bssid, bss_conf->bssid, 6);
+
+			if (rtlpriv->cfg->ops->linked_set_reg)
+				rtlpriv->cfg->ops->linked_set_reg(hw);
+
+			rcu_read_lock();
+			sta = ieee80211_find_sta(vif, (u8 *)bss_conf->bssid);
+			if (!sta) {
+				pr_err("ieee80211_find_sta returned NULL\n");
+				rcu_read_unlock();
+				goto out;
+			}
+
+			if (vif->type == NL80211_IFTYPE_STATION && sta)
+				rtlpriv->cfg->ops->update_rate_tbl(hw, sta, 0);
+			RT_TRACE(COMP_EASY_CONCURRENT, DBG_LOUD,
+				 ("send PS STATIC frame\n"));
+			if (rtlpriv->dm.supp_phymode_switch) {
+				if (sta->ht_cap.ht_supported)
+					stg_rtl_send_smps_action(hw, sta,
+							IEEE80211_SMPS_STATIC);
+			}
+
+			if (rtlhal->current_bandtype == BAND_ON_5G) {
+				mac->mode = WIRELESS_MODE_A;
+			} else {
+				if (sta->supp_rates[0] <= 0xf)
+					mac->mode = WIRELESS_MODE_B;
+				else
+					mac->mode = WIRELESS_MODE_G;
+			}
+
+			if (sta->ht_cap.ht_supported) {
+				if (rtlhal->current_bandtype == BAND_ON_2_4G)
+					mac->mode = WIRELESS_MODE_N_24G;
+				else
+					mac->mode = WIRELESS_MODE_N_5G;
+			}
+
+			if (sta->vht_cap.vht_supported) {
+				if (rtlhal->current_bandtype == BAND_ON_5G)
+					mac->mode = WIRELESS_MODE_AC_5G;
+				else
+					mac->mode = WIRELESS_MODE_AC_24G;
+			}
+
+			rcu_read_unlock();
+
+			/* to avoid AP Disassociation caused by inactivity */
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+						      HW_VAR_KEEP_ALIVE,
+						      (u8 *)(&keep_alive));
+
+			RT_TRACE(COMP_MAC80211, DBG_DMESG,
+				 ("BSS_CHANGED_ASSOC\n"));
+		} else {
+			mstatus = RT_MEDIA_DISCONNECT;
+
+			if (mac->link_state == MAC80211_LINKED)
+				rtl92e_lps_leave(hw);
+			if (ppsc->p2p_ps_info.p2p_ps_mode > P2P_PS_NONE)
+				rtl92e_p2p_ps_cmd(hw, P2P_PS_DISABLE);
+			mac->link_state = MAC80211_NOLINK;
+			memset(mac->bssid, 0, 6);
+			mac->vendor = PEER_UNKNOWN;
+			mac->mode = 0;
+
+			if (rtlpriv->dm.supp_phymode_switch) {
+				if (rtlpriv->cfg->ops->check_switch_to_dmdp)
+					rtlpriv->cfg->ops->check_switch_to_dmdp(hw);
+			}
+			RT_TRACE(COMP_MAC80211, DBG_DMESG,
+				 ("BSS_CHANGED_UN_ASSOC\n"));
+		}
+		rtlpriv->cfg->ops->set_network_type(hw, vif->type);
+		/* For FW LPS:
+		 * To tell firmware we have connected or disconnected*/
+		rtlpriv->cfg->ops->set_hw_reg(hw,
+					      HW_VAR_H2C_FW_JOINBSSRPT,
+					      (u8 *)(&mstatus));
+		ppsc->report_linked = (mstatus == RT_MEDIA_CONNECT) ?
+				      true : false;
+
+		if (rtlpriv->cfg->ops->get_btc_status())
+			rtlpriv->btcoexist.btc_ops->btc_mediastatus_notify(
+							rtlpriv, mstatus);
+	}
+
+	if (changed & BSS_CHANGED_ERP_CTS_PROT) {
+		RT_TRACE(COMP_MAC80211, DBG_TRACE,
+			 ("BSS_CHANGED_ERP_CTS_PROT\n"));
+		mac->use_cts_protect = bss_conf->use_cts_prot;
+	}
+
+	if (changed & BSS_CHANGED_ERP_PREAMBLE) {
+		RT_TRACE(COMP_MAC80211, DBG_LOUD,
+			 ("BSS_CHANGED_ERP_PREAMBLE use short preamble:%x\n",
+			  bss_conf->use_short_preamble));
+
+		mac->short_preamble = bss_conf->use_short_preamble;
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ACK_PREAMBLE,
+					      (u8 *)(&mac->short_preamble));
+	}
+
+	if (changed & BSS_CHANGED_ERP_SLOT) {
+		RT_TRACE(COMP_MAC80211, DBG_TRACE,
+			 ("BSS_CHANGED_ERP_SLOT\n"));
+
+		if (bss_conf->use_short_slot)
+			mac->slot_time = RTL_SLOT_TIME_9;
+		else
+			mac->slot_time = RTL_SLOT_TIME_20;
+
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SLOT_TIME,
+					      (u8 *)(&mac->slot_time));
+	}
+
+	if (changed & BSS_CHANGED_HT) {
+		struct ieee80211_sta *sta = NULL;
+
+		RT_TRACE(COMP_MAC80211, DBG_TRACE,
+			 ("BSS_CHANGED_HT\n"));
+
+		rcu_read_lock();
+		sta = ieee80211_find_sta(vif, (u8 *)bss_conf->bssid);
+		if (sta) {
+			if (sta->ht_cap.ampdu_density >
+			    mac->current_ampdu_density)
+				mac->current_ampdu_density =
+				    sta->ht_cap.ampdu_density;
+			if (sta->ht_cap.ampdu_factor <
+			    mac->current_ampdu_factor)
+				mac->current_ampdu_factor =
+				    sta->ht_cap.ampdu_factor;
+		}
+		rcu_read_unlock();
+
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SHORTGI_DENSITY,
+					      (u8 *)(&mac->max_mss_density));
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AMPDU_FACTOR,
+					      &mac->current_ampdu_factor);
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AMPDU_MIN_SPACE,
+					      &mac->current_ampdu_density);
+	}
+
+	if (changed & BSS_CHANGED_BSSID) {
+		u32 basic_rates;
+		struct ieee80211_sta *sta = NULL;
+
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BSSID,
+					      (u8 *)bss_conf->bssid);
+
+		RT_TRACE(COMP_MAC80211, DBG_DMESG,
+			 ("bssid: %pM\n", bss_conf->bssid));
+
+		mac->vendor = PEER_UNKNOWN;
+		memcpy(mac->bssid, bss_conf->bssid, 6);
+
+		rcu_read_lock();
+		sta = ieee80211_find_sta(vif, (u8 *)bss_conf->bssid);
+		if (!sta) {
+			rcu_read_unlock();
+			goto out;
+		}
+
+		if (rtlhal->current_bandtype == BAND_ON_5G) {
+			mac->mode = WIRELESS_MODE_A;
+		} else {
+			if (sta->supp_rates[0] <= 0xf)
+				mac->mode = WIRELESS_MODE_B;
+			else
+				mac->mode = WIRELESS_MODE_G;
+		}
+
+		if (sta->ht_cap.ht_supported) {
+			if (rtlhal->current_bandtype == BAND_ON_2_4G)
+				mac->mode = WIRELESS_MODE_N_24G;
+			else
+				mac->mode = WIRELESS_MODE_N_5G;
+		}
+
+		if (sta->vht_cap.vht_supported) {
+			if (rtlhal->current_bandtype == BAND_ON_5G)
+				mac->mode = WIRELESS_MODE_AC_5G;
+			else
+				mac->mode = WIRELESS_MODE_AC_24G;
+		}
+
+		/* just station need it, because ibss & ap mode will
+		 * set in sta_add, and will be NULL here */
+		if (vif->type == NL80211_IFTYPE_STATION) {
+			struct rtl_sta_info *sta_entry;
+			sta_entry = (struct rtl_sta_info *)sta->drv_priv;
+			sta_entry->wireless_mode = mac->mode;
+		}
+
+		if (sta->ht_cap.ht_supported) {
+			mac->ht_enable = true;
+
+			/*
+			 * for cisco 1252 bw20 it's wrong
+			 * if (ht_cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40) {
+			 *	mac->bw_40 = true;
+			 * }
+			 * */
+		}
+
+		if (sta->vht_cap.vht_supported)
+			mac->vht_enable = true;
+
+		if (changed & BSS_CHANGED_BASIC_RATES) {
+			/* for 5G must << RATE_6M_INDEX=4,
+			 * because 5G have no cck rate*/
+			if (rtlhal->current_bandtype == BAND_ON_5G)
+				basic_rates = sta->supp_rates[1] << 4;
+			else
+				basic_rates = sta->supp_rates[0];
+
+			mac->basic_rates = basic_rates;
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BASIC_RATE,
+					(u8 *)(&basic_rates));
+		}
+		rcu_read_unlock();
+	}
+out:
+	mutex_unlock(&rtlpriv->locks.conf_mutex);
+}
+
+static u64 rtl_op_get_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u64 tsf;
+
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_CORRECT_TSF, (u8 *)(&tsf));
+	return tsf;
+}
+
+static void rtl_op_set_tsf(struct ieee80211_hw *hw,
+			   struct ieee80211_vif *vif, u64 tsf)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u8 bibss = (mac->opmode == NL80211_IFTYPE_ADHOC) ? 1 : 0;
+
+	mac->tsf = tsf;
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_CORRECT_TSF, (u8 *)(&bibss));
+}
+
+static void rtl_op_reset_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmp = 0;
+
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_DUAL_TSF_RST, (u8 *)(&tmp));
+}
+
+static void rtl_op_sta_notify(struct ieee80211_hw *hw,
+			      struct ieee80211_vif *vif,
+			      enum sta_notify_cmd cmd,
+			      struct ieee80211_sta *sta)
+{
+	switch (cmd) {
+	case STA_NOTIFY_SLEEP:
+		break;
+	case STA_NOTIFY_AWAKE:
+		break;
+	default:
+		break;
+	}
+}
+
+static int rtl_op_ampdu_action(struct ieee80211_hw *hw,
+			       struct ieee80211_vif *vif,
+			       enum ieee80211_ampdu_mlme_action action,
+			       struct ieee80211_sta *sta, u16 tid, u16 *ssn,
+			       u8 buf_size)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	switch (action) {
+	case IEEE80211_AMPDU_TX_START:
+		RT_TRACE(COMP_MAC80211, DBG_TRACE,
+			 ("IEEE80211_AMPDU_TX_START: TID:%d\n", tid));
+		return rtl92e_tx_agg_start(hw, vif, sta, tid, ssn);
+		break;
+	case IEEE80211_AMPDU_TX_STOP_CONT:
+	case IEEE80211_AMPDU_TX_STOP_FLUSH:
+	case IEEE80211_AMPDU_TX_STOP_FLUSH_CONT:
+		RT_TRACE(COMP_MAC80211, DBG_TRACE,
+			 ("IEEE80211_AMPDU_TX_STOP: TID:%d\n", tid));
+		return rtl92e_tx_agg_stop(hw, vif, sta, tid);
+		break;
+	case IEEE80211_AMPDU_TX_OPERATIONAL:
+		RT_TRACE(COMP_MAC80211, DBG_TRACE,
+			 ("IEEE80211_AMPDU_TX_OPERATIONAL:TID:%d\n", tid));
+		rtl92e_tx_agg_oper(hw, sta, tid);
+		break;
+	case IEEE80211_AMPDU_RX_START:
+		RT_TRACE(COMP_MAC80211, DBG_TRACE,
+			 ("IEEE80211_AMPDU_RX_START:TID:%d\n", tid));
+		return rtl92e_rx_agg_start(hw, sta, tid);
+		break;
+	case IEEE80211_AMPDU_RX_STOP:
+		RT_TRACE(COMP_MAC80211, DBG_TRACE,
+			 ("IEEE80211_AMPDU_RX_STOP:TID:%d\n", tid));
+		return rtl92e_rx_agg_stop(hw, sta, tid);
+		break;
+	default:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("IEEE80211_AMPDU_ERR!!!!:\n"));
+		return -EOPNOTSUPP;
+	}
+	return 0;
+}
+
+static void rtl_op_sw_scan_start(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+
+	RT_TRACE(COMP_MAC80211, DBG_LOUD, ("\n"));
+	mac->act_scanning = true;
+	if (rtlpriv->link_info.b_higher_busytraffic) {
+		mac->skip_scan = true;
+		return;
+	}
+
+	if (rtlpriv->cfg->ops->get_btc_status())
+		rtlpriv->btcoexist.btc_ops->btc_scan_notify(rtlpriv, 1);
+
+	if (rtlpriv->dm.supp_phymode_switch) {
+		if (rtlpriv->cfg->ops->check_switch_to_dmdp)
+			rtlpriv->cfg->ops->check_switch_to_dmdp(hw);
+	}
+
+	if (mac->link_state == MAC80211_LINKED) {
+		rtl92e_lps_leave(hw);
+		mac->link_state = MAC80211_LINKED_SCANNING;
+	} else {
+		rtl92e_ips_nic_on(hw);
+	}
+
+	/* Dul mac */
+	rtlpriv->rtlhal.b_load_imrandiqk_setting_for2g = false;
+
+	rtlpriv->cfg->ops->led_control(hw, LED_CTL_SITE_SURVEY);
+	rtlpriv->cfg->ops->scan_operation_backup(hw, SCAN_OPT_BACKUP_BAND0);
+}
+
+static void rtl_op_sw_scan_complete(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+
+	RT_TRACE(COMP_MAC80211, DBG_LOUD, ("\n"));
+	mac->act_scanning = false;
+	mac->skip_scan = false;
+	if (rtlpriv->link_info.b_higher_busytraffic)
+		return;
+
+	/* p2p will use 1/6/11 to scan */
+	if (mac->n_channels == 3)
+		mac->p2p_in_use = true;
+	else
+		mac->p2p_in_use = false;
+	mac->n_channels = 0;
+	/* Dul mac */
+	rtlpriv->rtlhal.b_load_imrandiqk_setting_for2g = false;
+
+	if (mac->link_state == MAC80211_LINKED_SCANNING) {
+		mac->link_state = MAC80211_LINKED;
+		if (mac->opmode == NL80211_IFTYPE_STATION) {
+			/* fix fwlps issue */
+			rtlpriv->cfg->ops->set_network_type(hw, mac->opmode);
+		}
+	}
+
+	rtlpriv->cfg->ops->scan_operation_backup(hw, SCAN_OPT_RESTORE);
+	if (rtlpriv->cfg->ops->get_btc_status())
+		rtlpriv->btcoexist.btc_ops->btc_scan_notify(rtlpriv, 0);
+}
+
+static int rtl_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
+			  struct ieee80211_vif *vif, struct ieee80211_sta *sta,
+			  struct ieee80211_key_conf *key)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 key_type = NO_ENCRYPTION;
+	u8 key_idx;
+	bool group_key = false;
+	bool wep_only = false;
+	int err = 0;
+	u8 mac_addr[ETH_ALEN];
+	u8 bcast_addr[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
+
+	if (rtlpriv->cfg->mod_params->sw_crypto || rtlpriv->sec.use_sw_sec) {
+		RT_TRACE(COMP_ERR, DBG_WARNING,
+			 ("not open hw encryption\n"));
+		return -ENOSPC;	/*User disabled HW-crypto */
+	}
+	/* To support IBSS, use sw-crypto for GTK */
+	if (((vif->type == NL80211_IFTYPE_ADHOC) ||
+	     (vif->type == NL80211_IFTYPE_MESH_POINT)) &&
+	     !(key->flags & IEEE80211_KEY_FLAG_PAIRWISE))
+		return -ENOSPC;
+	RT_TRACE(COMP_SEC, DBG_DMESG,
+		 ("%s hardware based encryption for keyidx: %d, mac: %pM\n",
+		  cmd == SET_KEY ? "Using" : "Disabling", key->keyidx,
+		  sta ? sta->addr : bcast_addr));
+	rtlpriv->sec.being_setkey = true;
+	rtl92e_ips_nic_on(hw);
+	mutex_lock(&rtlpriv->locks.conf_mutex);
+	/* <1> get encryption alg */
+
+	switch (key->cipher) {
+	case WLAN_CIPHER_SUITE_WEP40:
+		key_type = WEP40_ENCRYPTION;
+		RT_TRACE(COMP_SEC, DBG_DMESG, ("alg:WEP40\n"));
+		break;
+	case WLAN_CIPHER_SUITE_WEP104:
+		RT_TRACE(COMP_SEC, DBG_DMESG, ("alg:WEP104\n"));
+		key_type = WEP104_ENCRYPTION;
+		break;
+	case WLAN_CIPHER_SUITE_TKIP:
+		key_type = TKIP_ENCRYPTION;
+		RT_TRACE(COMP_SEC, DBG_DMESG, ("alg:TKIP\n"));
+		break;
+	case WLAN_CIPHER_SUITE_CCMP:
+		key_type = AESCCMP_ENCRYPTION;
+		RT_TRACE(COMP_SEC, DBG_DMESG, ("alg:CCMP\n"));
+		break;
+	case WLAN_CIPHER_SUITE_AES_CMAC:
+		/* HW don't support CMAC encryption,
+		 * use software CMAC encryption */
+		key_type = AESCMAC_ENCRYPTION;
+		RT_TRACE(COMP_SEC, DBG_DMESG, ("alg:CMAC\n"));
+		RT_TRACE(COMP_SEC, DBG_DMESG,
+			 ("HW don't support CMAC encrypiton, use software CMAC encryption\n"));
+		err = -EOPNOTSUPP;
+		goto out_unlock;
+	default:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("alg_err:%x!!!!:\n", key->cipher));
+		goto out_unlock;
+	}
+	if (key_type == WEP40_ENCRYPTION ||
+	    key_type == WEP104_ENCRYPTION ||
+	    vif->type == NL80211_IFTYPE_ADHOC)
+		rtlpriv->sec.use_defaultkey = true;
+
+	/* <2> get key_idx */
+	key_idx = (u8) (key->keyidx);
+	if (key_idx > 3)
+		goto out_unlock;
+	/* <3> if pairwise key enable_hw_sec */
+	group_key = !(key->flags & IEEE80211_KEY_FLAG_PAIRWISE);
+
+	/* wep always be group key, but there are two conditions:
+	 * 1) wep only: is just for wep enc, in this condition
+	 * rtlpriv->sec.pairwise_enc_algorithm == NO_ENCRYPTION
+	 * will be true & enable_hw_sec will be set when wep
+	 * ke setting.
+	 * 2) wep(group) + AES(pairwise): some AP like cisco
+	 * may use it, in this condition enable_hw_sec will not
+	 * be set when wep key setting */
+	/* we must reset sec_info after lingked before set key,
+	 * or some flag will be wrong*/
+	if (vif->type == NL80211_IFTYPE_AP ||
+	    vif->type == NL80211_IFTYPE_MESH_POINT) {
+		if (!group_key || key_type == WEP40_ENCRYPTION ||
+		    key_type == WEP104_ENCRYPTION) {
+			if (group_key)
+				wep_only = true;
+			rtlpriv->cfg->ops->enable_hw_sec(hw);
+		}
+	} else {
+		if ((!group_key) || (vif->type == NL80211_IFTYPE_ADHOC) ||
+		    rtlpriv->sec.pairwise_enc_algorithm == NO_ENCRYPTION) {
+			if (rtlpriv->sec.pairwise_enc_algorithm ==
+			    NO_ENCRYPTION &&
+			   (key_type == WEP40_ENCRYPTION ||
+			    key_type == WEP104_ENCRYPTION))
+				wep_only = true;
+			rtlpriv->sec.pairwise_enc_algorithm = key_type;
+			RT_TRACE(COMP_SEC, DBG_DMESG,
+				 ("set enable_hw_sec, key_type:%x(OPEN:0 WEP40:1 TKIP:2 AES:4 WEP104:5)\n",
+				  key_type));
+			rtlpriv->cfg->ops->enable_hw_sec(hw);
+		}
+	}
+	/* <4> set key based on cmd */
+	switch (cmd) {
+	case SET_KEY:
+		if (wep_only) {
+			RT_TRACE(COMP_SEC, DBG_DMESG,
+				 ("set WEP(group/pairwise) key\n"));
+			/* Pairwise key with an assigned MAC address. */
+			rtlpriv->sec.pairwise_enc_algorithm = key_type;
+			rtlpriv->sec.group_enc_algorithm = key_type;
+			/*set local buf about wep key. */
+			memcpy(rtlpriv->sec.key_buf[key_idx],
+			       key->key, key->keylen);
+			rtlpriv->sec.key_len[key_idx] = key->keylen;
+			eth_zero_addr(mac_addr);
+		} else if (group_key) {	/* group key */
+			RT_TRACE(COMP_SEC, DBG_DMESG,
+				 ("set group key\n"));
+			/* group key */
+			rtlpriv->sec.group_enc_algorithm = key_type;
+			/*set local buf about group key. */
+			memcpy(rtlpriv->sec.key_buf[key_idx],
+			       key->key, key->keylen);
+			rtlpriv->sec.key_len[key_idx] = key->keylen;
+			ether_addr_copy(mac_addr, bcast_addr);
+		} else {	/* pairwise key */
+			RT_TRACE(COMP_SEC, DBG_DMESG,
+				 ("set pairwise key\n"));
+			if (!sta) {
+				RT_ASSERT(false,
+					  ("pairwise key without mac_addr\n"));
+
+				err = -EOPNOTSUPP;
+				goto out_unlock;
+			}
+			/* Pairwise key with an assigned MAC address. */
+			rtlpriv->sec.pairwise_enc_algorithm = key_type;
+			/*set local buf about pairwise key. */
+			memcpy(rtlpriv->sec.key_buf[PAIRWISE_KEYIDX],
+			       key->key, key->keylen);
+			rtlpriv->sec.key_len[PAIRWISE_KEYIDX] = key->keylen;
+			rtlpriv->sec.pairwise_key =
+			    rtlpriv->sec.key_buf[PAIRWISE_KEYIDX];
+			ether_addr_copy(mac_addr, sta->addr);
+		}
+		rtlpriv->cfg->ops->set_key(hw, key_idx, mac_addr,
+					   group_key, key_type, wep_only,
+					   false);
+		/* <5> tell mac80211 do something: */
+		/*must use sw generate IV, or can not work !!!!. */
+		key->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
+		key->hw_key_idx = key_idx;
+		if (key_type == TKIP_ENCRYPTION)
+			key->flags |= IEEE80211_KEY_FLAG_GENERATE_MMIC;
+		/*use software CCMP encryption for management frames (MFP) */
+		if (key_type == AESCCMP_ENCRYPTION)
+			key->flags |= IEEE80211_KEY_FLAG_SW_MGMT;
+		break;
+	case DISABLE_KEY:
+		RT_TRACE(COMP_SEC, DBG_DMESG,
+			 ("disable key delete one entry\n"));
+		/*set local buf about wep key. */
+		if (vif->type == NL80211_IFTYPE_AP ||
+		    vif->type == NL80211_IFTYPE_MESH_POINT) {
+			if (sta)
+				stg_rtl_cam_del_entry(hw, sta->addr);
+		}
+		memset(rtlpriv->sec.key_buf[key_idx], 0, key->keylen);
+		rtlpriv->sec.key_len[key_idx] = 0;
+		eth_zero_addr(mac_addr);
+		/*
+		 *mac80211 will delete entrys one by one,
+		 *so don't use stg_rtl_cam_reset_all_entry
+		 *or clear all entry here.
+		 */
+		stg_rtl_cam_delete_one_entry(hw, mac_addr, key_idx);
+		break;
+	default:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("cmd_err:%x!!!!:\n", cmd));
+	}
+out_unlock:
+	mutex_unlock(&rtlpriv->locks.conf_mutex);
+	rtlpriv->sec.being_setkey = false;
+	return err;
+}
+
+static void rtl_op_rfkill_poll(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	bool radio_state;
+	bool blocked;
+	u8 valid = 0;
+
+	if (!test_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status))
+		return;
+
+	mutex_lock(&rtlpriv->locks.conf_mutex);
+
+	/*if Radio On return true here */
+	radio_state = rtlpriv->cfg->ops->radio_onoff_checking(hw, &valid);
+
+	if (valid) {
+		if (unlikely(radio_state != rtlpriv->rfkill.rfkill_state)) {
+			rtlpriv->rfkill.rfkill_state = radio_state;
+
+			RT_TRACE(COMP_RF, DBG_DMESG,
+				 (KERN_INFO "wireless radio switch turned %s\n",
+				  radio_state ? "on" : "off"));
+
+			blocked = (rtlpriv->rfkill.rfkill_state == 1) ? 0 : 1;
+			wiphy_rfkill_set_hw_state(hw->wiphy, blocked);
+		}
+	}
+
+	mutex_unlock(&rtlpriv->locks.conf_mutex);
+}
+
+/* this function is called by mac80211 to flush tx buffer
+ * before switch channle or power save, or tx buffer packet
+ * maybe send after offchannel or rf sleep, this may cause
+ * dis-association by AP */
+static void rtl_op_flush(struct ieee80211_hw *hw,
+			 u32 queues, bool drop)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (rtlpriv->intf_ops->flush)
+		rtlpriv->intf_ops->flush(hw, queues, drop);
+}
+
+const struct ieee80211_ops rtl92e_ops = {
+	.start = rtl_op_start,
+	.stop = rtl_op_stop,
+	.tx = rtl_op_tx,
+	.add_interface = rtl_op_add_interface,
+	.remove_interface = rtl_op_remove_interface,
+	.change_interface = rtl_op_change_interface,
+#ifdef CONFIG_PM
+	.suspend = rtl_op_suspend,
+	.resume = rtl_op_resume,
+#endif
+	.config = rtl_op_config,
+	.configure_filter = rtl_op_configure_filter,
+	.set_key = rtl_op_set_key,
+	.conf_tx = rtl_op_conf_tx,
+	.bss_info_changed = rtl_op_bss_info_changed,
+	.get_tsf = rtl_op_get_tsf,
+	.set_tsf = rtl_op_set_tsf,
+	.reset_tsf = rtl_op_reset_tsf,
+	.sta_notify = rtl_op_sta_notify,
+	.ampdu_action = rtl_op_ampdu_action,
+	.sw_scan_start = rtl_op_sw_scan_start,
+	.sw_scan_complete = rtl_op_sw_scan_complete,
+	.rfkill_poll = rtl_op_rfkill_poll,
+	.sta_add = rtl_op_sta_add,
+	.sta_remove = rtl_op_sta_remove,
+	.flush = rtl_op_flush,
+};
diff --git a/drivers/staging/rtl8192ee/core.h b/drivers/staging/rtl8192ee/core.h
new file mode 100644
index 00000000000..ef75ad5f029
--- /dev/null
+++ b/drivers/staging/rtl8192ee/core.h
@@ -0,0 +1,39 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * Tmis 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.
+ *
+ * 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.
+ *
+ * Tme full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL_CORE_H__
+#define __RTL_CORE_H__
+
+#define RTL_SUPPORTED_FILTERS		\
+	(FIF_PROMISC_IN_BSS | \
+	FIF_ALLMULTI | FIF_CONTROL | \
+	FIF_OTHER_BSS | \
+	FIF_FCSFAIL | \
+	FIF_BCN_PRBRESP_PROMISC)
+
+#define RTL_SUPPORTED_CTRL_FILTER	0xFF
+
+extern const struct ieee80211_ops rtl92e_ops;
+#endif
diff --git a/drivers/staging/rtl8192ee/debug.c b/drivers/staging/rtl8192ee/debug.c
new file mode 100644
index 00000000000..feec3948585
--- /dev/null
+++ b/drivers/staging/rtl8192ee/debug.c
@@ -0,0 +1,978 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * Tmis 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "wifi.h"
+#include "cam.h"
+
+#define GET_INODE_DATA(__node)		PDE_DATA(__node)
+
+
+void rtl92e_dbgp_flag_init(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 i;
+
+	rtlpriv->dbg.global_debuglevel = DBG_DMESG;
+
+	rtlpriv->dbg.global_debugcomponents =
+		COMP_ERR |
+		COMP_FW |
+		COMP_INIT |
+		COMP_RECV |
+		COMP_SEND |
+		COMP_MLME |
+		COMP_SCAN |
+		COMP_INTR |
+		COMP_LED |
+		COMP_SEC |
+		COMP_BEACON |
+		COMP_RATE |
+		COMP_RXDESC |
+		COMP_DIG |
+		COMP_TXAGC |
+		COMP_POWER |
+		COMP_POWER_TRACKING |
+		COMP_BB_POWERSAVING |
+		COMP_SWAS |
+		COMP_RF |
+		COMP_TURBO |
+		COMP_RATR |
+		COMP_CMD |
+		COMP_EASY_CONCURRENT |
+		COMP_EFUSE |
+		COMP_QOS | COMP_MAC80211 | COMP_REGD |
+		COMP_CHAN |
+		COMP_BT_COEXIST |
+		COMP_IQK |
+		0;
+
+	for (i = 0; i < DBGP_TYPE_MAX; i++)
+		rtlpriv->dbg.dbgp_type[i] = 0;
+
+	/*Init Debug flag enable condition */
+}
+
+static struct proc_dir_entry *proc_topdir;
+
+static int rtl_proc_get_mac_0(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int i, n, page;
+	int max = 0xff;
+	page = 0x000;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_read_dword(rtlpriv, (page | n)));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_mac_0(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_mac_0, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_mac_0 = {
+	.open = dl_proc_open_mac_0,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_mac_1(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int i, n, page;
+	int max = 0xff;
+	page = 0x100;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_read_dword(rtlpriv, (page | n)));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_mac_1(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_mac_1, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_mac_1 = {
+	.open = dl_proc_open_mac_1,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_mac_2(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int i, n, page;
+	int max = 0xff;
+	page = 0x200;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_read_dword(rtlpriv, (page | n)));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_mac_2(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_mac_2, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_mac_2 = {
+	.open = dl_proc_open_mac_2,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_mac_3(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int i, n, page;
+	int max = 0xff;
+	page = 0x300;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_read_dword(rtlpriv, (page | n)));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_mac_3(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_mac_3, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_mac_3 = {
+	.open = dl_proc_open_mac_3,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_mac_4(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int i, n, page;
+	int max = 0xff;
+	page = 0x400;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_read_dword(rtlpriv, (page | n)));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_mac_4(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_mac_4, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_mac_4 = {
+	.open = dl_proc_open_mac_4,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_mac_5(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int i, n, page;
+	int max = 0xff;
+	page = 0x500;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_read_dword(rtlpriv, (page | n)));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_mac_5(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_mac_5, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_mac_5 = {
+	.open = dl_proc_open_mac_5,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_mac_6(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int i, n, page;
+	int max = 0xff;
+	page = 0x600;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_read_dword(rtlpriv, (page | n)));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_mac_6(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_mac_6, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_mac_6 = {
+	.open = dl_proc_open_mac_6,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_mac_7(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int i, n, page;
+	int max = 0xff;
+	page = 0x700;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_read_dword(rtlpriv, (page | n)));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_mac_7(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_mac_7, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_mac_7 = {
+	.open = dl_proc_open_mac_7,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_bb_8(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	int i, n, page;
+	int max = 0xff;
+	page = 0x800;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_bb_8(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_bb_8, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_bb_8 = {
+	.open = dl_proc_open_bb_8,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_bb_9(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	int i, n, page;
+	int max = 0xff;
+	page = 0x900;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_bb_9(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_bb_9, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_bb_9 = {
+	.open = dl_proc_open_bb_9,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_bb_a(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	int i, n, page;
+	int max = 0xff;
+	page = 0xa00;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_bb_a(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_bb_a, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_bb_a = {
+	.open = dl_proc_open_bb_a,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_bb_b(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	int i, n, page;
+	int max = 0xff;
+	page = 0xb00;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_bb_b(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_bb_b, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_bb_b = {
+	.open = dl_proc_open_bb_b,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_bb_c(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	int i, n, page;
+	int max = 0xff;
+	page = 0xc00;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_bb_c(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_bb_c, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_bb_c = {
+	.open = dl_proc_open_bb_c,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_bb_d(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	int i, n, page;
+	int max = 0xff;
+	page = 0xd00;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_bb_d(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_bb_d, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_bb_d = {
+	.open = dl_proc_open_bb_d,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_bb_e(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	int i, n, page;
+	int max = 0xff;
+	page = 0xe00;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_bb_e(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_bb_e, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_bb_e = {
+	.open = dl_proc_open_bb_e,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_bb_f(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	int i, n, page;
+	int max = 0xff;
+	page = 0xf00;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n + page);
+		for (i = 0; i < 4 && n <= max; i++, n += 4)
+			seq_printf(m, "%8.8x    ",
+				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_bb_f(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_bb_f, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_bb_f = {
+	.open = dl_proc_open_bb_f,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_reg_rf_a(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	int i, n;
+	int max = 0x40;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n);
+		for (i = 0; i < 4 && n <= max; n += 1, i++)
+			seq_printf(m, "%8.8x    ",
+				   rtl_get_rfreg(hw, RF90_PATH_A, n, 0xffffffff));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_rf_a(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_reg_rf_a, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_rf_a = {
+	.open = dl_proc_open_rf_a,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_reg_rf_b(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	int i, n;
+	int max = 0x40;
+
+	for (n = 0; n <= max; ) {
+		seq_printf(m, "\n%8.8x  ", n);
+		for (i = 0; i < 4 && n <= max; n += 1, i++)
+			seq_printf(m, "%8.8x    ",
+				   rtl_get_rfreg(hw, RF90_PATH_B, n,
+						 0xffffffff));
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_rf_b(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_reg_rf_b, GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_rf_b = {
+	.open = dl_proc_open_rf_b,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_cam_register_1(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 target_cmd = 0;
+	u32 target_val = 0;
+	u8 entry_i = 0;
+	u32 ulstatus;
+	int i = 100, j = 0;
+
+	/* This dump the current register page */
+	seq_puts(m,
+	    "\n#################### SECURITY CAM (0-10) ##################\n ");
+
+	for (j = 0; j < 11; j++) {
+		seq_printf(m, "\nD:  %2x > ", j);
+		for (entry_i = 0; entry_i < CAM_CONTENT_COUNT; entry_i++) {
+			/* polling bit, and No Write enable, and address  */
+			target_cmd = entry_i + CAM_CONTENT_COUNT * j;
+			target_cmd = target_cmd | BIT(31);
+
+			/* Check polling bit is clear */
+			while ((i--) >= 0) {
+				ulstatus = rtl_read_dword(rtlpriv,
+						rtlpriv->cfg->maps[RWCAM]);
+				if (ulstatus & BIT(31))
+					continue;
+				else
+					break;
+			}
+
+			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM],
+					target_cmd);
+			target_val = rtl_read_dword(rtlpriv,
+						    rtlpriv->cfg->maps[RCAMO]);
+			seq_printf(m, "%8.8x ", target_val);
+		}
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_cam_1(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_cam_register_1,
+			   GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_cam_1 = {
+	.open = dl_proc_open_cam_1,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_cam_register_2(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 target_cmd = 0;
+	u32 target_val = 0;
+	u8 entry_i = 0;
+	u32 ulstatus;
+	int i = 100, j = 0;
+
+	/* This dump the current register page */
+	seq_puts(m,
+	    "\n################### SECURITY CAM (11-21) ##################\n ");
+
+	for (j = 11; j < 22; j++) {
+		seq_printf(m, "\nD:  %2x > ", j);
+		for (entry_i = 0; entry_i < CAM_CONTENT_COUNT; entry_i++) {
+			target_cmd = entry_i + CAM_CONTENT_COUNT * j;
+			target_cmd = target_cmd | BIT(31);
+
+			while ((i--) >= 0) {
+				ulstatus = rtl_read_dword(rtlpriv,
+						rtlpriv->cfg->maps[RWCAM]);
+				if (ulstatus & BIT(31))
+					continue;
+				else
+					break;
+			}
+
+			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM],
+					target_cmd);
+			target_val = rtl_read_dword(rtlpriv,
+						    rtlpriv->cfg->maps[RCAMO]);
+			seq_printf(m, "%8.8x ", target_val);
+		}
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_cam_2(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_cam_register_2,
+			   GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_cam_2 = {
+	.open = dl_proc_open_cam_2,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int rtl_proc_get_cam_register_3(struct seq_file *m, void *v)
+{
+	struct ieee80211_hw *hw = m->private;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 target_cmd = 0;
+	u32 target_val = 0;
+	u8 entry_i = 0;
+	u32 ulstatus;
+	int i = 100, j = 0;
+
+	/* This dump the current register page */
+	seq_puts(m,
+	    "\n################### SECURITY CAM (22-31) ##################\n ");
+
+	for (j = 22; j < TOTAL_CAM_ENTRY; j++) {
+		seq_printf(m, "\nD:  %2x > ", j);
+		for (entry_i = 0; entry_i < CAM_CONTENT_COUNT; entry_i++) {
+			target_cmd = entry_i+CAM_CONTENT_COUNT*j;
+			target_cmd = target_cmd | BIT(31);
+
+			while ((i--) >= 0) {
+				ulstatus = rtl_read_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM]);
+				if (ulstatus & BIT(31))
+					continue;
+				else
+					break;
+			}
+
+			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM],
+					target_cmd);
+			target_val = rtl_read_dword(rtlpriv,
+						    rtlpriv->cfg->maps[RCAMO]);
+			seq_printf(m, "%8.8x ", target_val);
+		}
+	}
+	seq_puts(m, "\n");
+	return 0;
+}
+
+static int dl_proc_open_cam_3(struct inode *inode, struct file *file)
+{
+	return single_open(file, rtl_proc_get_cam_register_3,
+			   GET_INODE_DATA(inode));
+}
+
+static const struct file_operations file_ops_cam_3 = {
+	.open = dl_proc_open_cam_3,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+void rtl_proc_add_one(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct proc_dir_entry *entry;
+
+	snprintf(rtlpriv->dbg.proc_name, 18, "%x-%x-%x-%x-%x-%x",
+		 rtlefuse->dev_addr[0], rtlefuse->dev_addr[1],
+		 rtlefuse->dev_addr[2], rtlefuse->dev_addr[3],
+		 rtlefuse->dev_addr[4], rtlefuse->dev_addr[5]);
+
+	rtlpriv->dbg.proc_dir = proc_mkdir(rtlpriv->dbg.proc_name, proc_topdir);
+	if (!rtlpriv->dbg.proc_dir) {
+		RT_TRACE(COMP_INIT, DBG_EMERG,
+			 ("Unable to init /proc/net/%s/%s\n",
+			  rtlpriv->cfg->name,
+			  rtlpriv->dbg.proc_name));
+		return;
+	}
+
+	entry = proc_create_data("mac-0", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_mac_0, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, DBG_EMERG,
+			 ("Unable to initialize /proc/net/%s/%s/mac-0\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("mac-1", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_mac_1, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/mac-1\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("mac-2", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_mac_2, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/mac-2\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("mac-3", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_mac_3, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/mac-3\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("mac-4", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_mac_4, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/mac-4\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("mac-5", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_mac_5, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/mac-5\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("mac-6", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_mac_6, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/mac-6\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("mac-7", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_mac_7, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/mac-7\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("bb-8", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_bb_8, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/bb-8\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("bb-9", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_bb_9, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/bb-9\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("bb-a", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_bb_a, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/bb-a\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("bb-b", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_bb_b, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/bb-b\n",
+		      rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("bb-c", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_bb_c, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/bb-c\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("bb-d", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_bb_d, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/bb-d\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("bb-e", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_bb_e, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/bb-e\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("bb-f", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_bb_f, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/bb-f\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("rf-a", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_rf_a, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/rf-a\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("rf-b", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_rf_b, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/rf-b\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("cam-1", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_cam_1, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/cam-1\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("cam-2", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_cam_2, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/cam-2\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+
+	entry = proc_create_data("cam-3", S_IFREG | S_IRUGO,
+				 rtlpriv->dbg.proc_dir, &file_ops_cam_3, hw);
+	if (!entry)
+		RT_TRACE(COMP_INIT, COMP_ERR,
+			 ("Unable to initialize /proc/net/%s/%s/cam-3\n",
+			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
+}
+
+void rtl_proc_remove_one(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (rtlpriv->dbg.proc_dir) {
+		remove_proc_entry("mac-0", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("mac-1", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("mac-2", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("mac-3", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("mac-4", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("mac-5", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("mac-6", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("mac-7", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("bb-8", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("bb-9", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("bb-a", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("bb-b", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("bb-c", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("bb-d", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("bb-e", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("bb-f", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("rf-a", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("rf-b", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("cam-1", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("cam-2", rtlpriv->dbg.proc_dir);
+		remove_proc_entry("cam-3", rtlpriv->dbg.proc_dir);
+
+		remove_proc_entry(rtlpriv->dbg.proc_name, proc_topdir);
+
+		rtlpriv->dbg.proc_dir = NULL;
+	}
+}
+
+void rtl_proc_add_topdir(void)
+{
+	proc_topdir = proc_mkdir("rtlwifi", init_net.proc_net);
+}
+
+void rtl_proc_remove_topdir(void)
+{
+	if (proc_topdir)
+		remove_proc_entry("rtlwifi", init_net.proc_net);
+}
diff --git a/drivers/staging/rtl8192ee/debug.h b/drivers/staging/rtl8192ee/debug.h
new file mode 100644
index 00000000000..093128d1f36
--- /dev/null
+++ b/drivers/staging/rtl8192ee/debug.h
@@ -0,0 +1,221 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * Tmis 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.
+ *
+ * Tmis 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.
+ *
+ * Tme full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL_DEBUG_H__
+#define __RTL_DEBUG_H__
+
+/*--------------------------------------------------------------
+			Debug level
+--------------------------------------------------------------*/
+/*
+ *Fatal bug.
+ *For example, Tx/Rx/IO locked up,
+ *memory access violation,
+ *resource allocation failed,
+ *unexpected HW behavior, HW BUG
+ *and so on.
+ */
+#define DBG_EMERG			1
+
+/*
+ *Abnormal, rare, or unexpeted cases.
+ *For example, Packet/IO Ctl canceled,
+ *device suprisely unremoved and so on.
+ */
+#define	DBG_WARNING			2
+
+/*
+ *Normal case driver developer should
+ *open, we can see link status like
+ *assoc/AddBA/DHCP/adapter start and
+ *so on basic and useful infromations.
+ */
+#define DBG_DMESG			3
+
+/*
+ *Normal case with useful information
+ *about current SW or HW state.
+ *For example, Tx/Rx descriptor to fill,
+ *Tx/Rx descriptor completed status,
+ *SW protocol state change, dynamic
+ *mechanism state change and so on.
+ */
+#define DBG_LOUD			4
+
+/*
+ *Normal case with detail execution
+ *flow or information.
+ */
+#define	DBG_TRACE			5
+
+/*--------------------------------------------------------------
+		Define the rt_trace components
+--------------------------------------------------------------*/
+#define COMP_ERR			BIT(0)
+#define COMP_FW				BIT(1)
+#define COMP_INIT			BIT(2)	/*For init/deinit */
+#define COMP_RECV			BIT(3)	/*For Rx. */
+#define COMP_SEND			BIT(4)	/*For Tx. */
+#define COMP_MLME			BIT(5)	/*For MLME. */
+#define COMP_SCAN			BIT(6)	/*For Scan. */
+#define COMP_INTR			BIT(7)	/*For interrupt Related. */
+#define COMP_LED			BIT(8)	/*For LED. */
+#define COMP_SEC			BIT(9)	/*For sec. */
+#define COMP_BEACON			BIT(10)	/*For beacon. */
+#define COMP_RATE			BIT(11)	/*For rate. */
+#define COMP_RXDESC			BIT(12)	/*For rx desc. */
+#define COMP_DIG			BIT(13)	/*For DIG */
+#define COMP_TXAGC			BIT(14)	/*For Tx power */
+#define COMP_HIPWR			BIT(15)	/*For High Power Mechanism */
+#define COMP_POWER			BIT(16)	/*For lps/ips/aspm. */
+#define COMP_POWER_TRACKING		BIT(17)	/*For TX POWER TRACKING */
+#define COMP_BB_POWERSAVING		BIT(18)
+#define COMP_SWAS			BIT(19)	/*For SW Antenna Switch */
+#define COMP_RF				BIT(20)	/*For RF. */
+#define COMP_TURBO			BIT(21)	/*For EDCA TURBO. */
+#define COMP_RATR			BIT(22)
+#define COMP_CMD			BIT(23)
+#define COMP_EFUSE			BIT(24)
+#define COMP_QOS			BIT(25)
+#define COMP_MAC80211			BIT(26)
+#define COMP_REGD			BIT(27)
+#define COMP_CHAN			BIT(28)
+#define COMP_EASY_CONCURRENT		BIT(29)
+#define COMP_BT_COEXIST			BIT(30)
+#define COMP_IQK			BIT(31)
+
+/*--------------------------------------------------------------
+		Define the rt_print components
+--------------------------------------------------------------*/
+/* Define EEPROM and EFUSE  check module bit*/
+#define EEPROM_W			BIT(0)
+#define EFUSE_PG			BIT(1)
+#define EFUSE_READ_ALL			BIT(2)
+
+/* Define init check for module bit*/
+#define	INIT_EEPROM			BIT(0)
+#define	INIT_TxPower			BIT(1)
+#define	INIT_IQK			BIT(2)
+#define	INIT_RF				BIT(3)
+
+/* Define PHY-BB/RF/MAC check module bit */
+#define	PHY_BBR				BIT(0)
+#define	PHY_BBW				BIT(1)
+#define	PHY_RFR				BIT(2)
+#define	PHY_RFW				BIT(3)
+#define	PHY_MACR			BIT(4)
+#define	PHY_MACW			BIT(5)
+#define	PHY_ALLR			BIT(6)
+#define	PHY_ALLW			BIT(7)
+#define	PHY_TXPWR			BIT(8)
+#define	PHY_PWRDIFF			BIT(9)
+
+/* Define Dynamic Mechanism check module bit --> FDM */
+#define WA_IOT				BIT(0)
+#define DM_PWDB				BIT(1)
+#define DM_MONITOR			BIT(2)
+#define DM_DIG				BIT(3)
+#define DM_EDCA_TURBO			BIT(4)
+
+enum dbgp_flag_e {
+	FQOS = 0,
+	FTX = 1,
+	FRX = 2,
+	FSEC = 3,
+	FMGNT = 4,
+	FMLME = 5,
+	FRESOURCE = 6,
+	FBEACON = 7,
+	FISR = 8,
+	FPHY = 9,
+	FMP = 10,
+	FEEPROM = 11,
+	FPWR = 12,
+	FDM = 13,
+	FDBGCtrl = 14,
+	FC2H = 15,
+	FBT = 16,
+	FINIT = 17,
+	FIOCTL = 18,
+	DBGP_TYPE_MAX
+};
+
+#define RT_ASSERT(_exp , fmt)				\
+	do { \
+		if (!(_exp))	{			\
+			pr_debug("%s:%s(): ", KBUILD_MODNAME, \
+			__func__);	\
+			pr_cont fmt;			\
+		} \
+	} while (0)
+
+#define RT_TRACE(comp, level, fmt)\
+	do { \
+		if (unlikely(((comp) & rtlpriv->dbg.global_debugcomponents) && \
+			((level) <= rtlpriv->dbg.global_debuglevel))) {\
+			pr_debug("%s-%d:%s():<%lx> ", \
+			KBUILD_MODNAME, \
+			rtlpriv->rtlhal.interfaceindex, __func__, \
+			in_interrupt());	\
+			pr_cont fmt;			\
+		} \
+	} while (0)
+
+#define RTPRINT(rtlpriv, dbgtype, dbgflag, fmt, ...)			\
+do {									\
+	if (unlikely(rtlpriv->dbg.dbgp_type[dbgtype] & dbgflag)) {	\
+		pr_debug(KBUILD_MODNAME ": " fmt,		\
+		       ##__VA_ARGS__);					\
+	}								\
+} while (0)
+
+#define RT_PRINT_DATA(rtlpriv, _comp, _level, _titlestring, _hexdata, \
+		_hexdatalen) \
+	do {\
+		if (unlikely(((_comp) & rtlpriv->dbg.global_debugcomponents) &&\
+			(_level <= rtlpriv->dbg.global_debuglevel)))	{ \
+			int __i;					\
+			u8 *ptr = (u8 *)_hexdata;			\
+			pr_debug("%s: ", KBUILD_MODNAME);	\
+			pr_cont("In process \"%s\" (pid %i):", \
+					current->comm,	\
+					current->pid); \
+			pr_cont(_titlestring);		\
+			for (__i = 0; __i < (int)_hexdatalen; __i++) {	\
+				pr_cont("%02X%s", ptr[__i], (((__i + 1) % 4) \
+							== 0) ? "  " : " ");\
+				if (((__i + 1) % 16) == 0)	\
+					pr_cont("\n");	\
+			}				\
+			pr_cont("\n");			\
+		} \
+	} while (0)
+
+void rtl92e_dbgp_flag_init(struct ieee80211_hw *hw);
+void rtl_proc_add_one(struct ieee80211_hw *hw);
+void rtl_proc_remove_one(struct ieee80211_hw *hw);
+void rtl_proc_add_topdir(void);
+void rtl_proc_remove_topdir(void);
+#endif
diff --git a/drivers/staging/rtl8192ee/efuse.c b/drivers/staging/rtl8192ee/efuse.c
new file mode 100644
index 00000000000..3fae1836983
--- /dev/null
+++ b/drivers/staging/rtl8192ee/efuse.c
@@ -0,0 +1,1233 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * Tmis 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+#include "wifi.h"
+#include "efuse.h"
+
+static const u8 MAX_PGPKT_SIZE = 9;
+static const u8 PGPKT_DATA_SIZE = 8;
+static const int EFUSE_MAX_SIZE = 512;
+
+static const struct efuse_map RTL8712_SDIO_EFUSE_TABLE[] = {
+	{0, 0, 0, 2},
+	{0, 1, 0, 2},
+	{0, 2, 0, 2},
+	{1, 0, 0, 1},
+	{1, 0, 1, 1},
+	{1, 1, 0, 1},
+	{1, 1, 1, 3},
+	{1, 3, 0, 17},
+	{3, 3, 1, 48},
+	{10, 0, 0, 6},
+	{10, 3, 0, 1},
+	{10, 3, 1, 1},
+	{11, 0, 0, 28}
+};
+
+static void efuse92e_shadow_read_1byte(struct ieee80211_hw *hw, u16 offset,
+				       u8 *value);
+static void efuse92e_shadow_read_2byte(struct ieee80211_hw *hw, u16 offset,
+				       u16 *value);
+static void efuse92e_shadow_read_4byte(struct ieee80211_hw *hw, u16 offset,
+				       u32 *value);
+static void efuse92e_shadow_write_1byte(struct ieee80211_hw *hw, u16 offset,
+					u8 value);
+static void efuse92e_shadow_write_2byte(struct ieee80211_hw *hw, u16 offset,
+					u16 value);
+static void efuse92e_shadow_write_4byte(struct ieee80211_hw *hw, u16 offset,
+					u32 value);
+static int efuse_one_byte_write(struct ieee80211_hw *hw, u16 addr,
+				u8 data);
+static void efuse_read_all_map(struct ieee80211_hw *hw, u8 *efuse);
+static int efuse_pg_packet_read(struct ieee80211_hw *hw, u8 offset,
+				u8 *data);
+static int efuse_pg_packet_write(struct ieee80211_hw *hw, u8 offset,
+				 u8 word_en, u8 *data);
+static void efuse_word_enable_data_read(u8 word_en, u8 *sourdata,
+					u8 *targetdata);
+static u8 efuse_word_enable_data_write(struct ieee80211_hw *hw,
+				       u16 efuse_addr, u8 word_en, u8 *data);
+static void efuse_power_switch(struct ieee80211_hw *hw, u8 bwrite,
+			       u8 pwrstate);
+static u16 efuse_get_current_size(struct ieee80211_hw *hw);
+static u8 efuse_calculate_word_cnts(u8 word_en);
+
+void efuse92e_initialize(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 bytetemp;
+	u8 temp;
+
+	bytetemp = rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[SYS_FUNC_EN] + 1);
+	temp = bytetemp | 0x20;
+	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[SYS_FUNC_EN] + 1, temp);
+
+	bytetemp = rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[SYS_ISO_CTRL] + 1);
+	temp = bytetemp & 0xFE;
+	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[SYS_ISO_CTRL] + 1, temp);
+
+	bytetemp = rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_TEST] + 3);
+	temp = bytetemp | 0x80;
+	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_TEST] + 3, temp);
+
+	rtl_write_byte(rtlpriv, 0x2F8, 0x3);
+
+	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 3, 0x72);
+}
+
+u8 stg_efuse_read_1byte(struct ieee80211_hw *hw, u16 address)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 data;
+	u8 bytetemp;
+	u8 temp;
+	u32 k = 0;
+	const u32 efuse_real_content_len =
+		rtlpriv->cfg->maps[EFUSE_REAL_CONTENT_SIZE];
+
+	if (address < efuse_real_content_len) {
+		temp = address & 0xFF;
+		rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 1,
+			       temp);
+		bytetemp = rtl_read_byte(rtlpriv,
+					 rtlpriv->cfg->maps[EFUSE_CTRL] + 2);
+		temp = ((address >> 8) & 0x03) | (bytetemp & 0xFC);
+		rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 2,
+			       temp);
+
+		bytetemp = rtl_read_byte(rtlpriv,
+					 rtlpriv->cfg->maps[EFUSE_CTRL] + 3);
+		temp = bytetemp & 0x7F;
+		rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 3,
+			       temp);
+
+		bytetemp = rtl_read_byte(rtlpriv,
+					 rtlpriv->cfg->maps[EFUSE_CTRL] + 3);
+		while (!(bytetemp & 0x80)) {
+			bytetemp = rtl_read_byte(rtlpriv,
+						 rtlpriv->cfg->
+						 maps[EFUSE_CTRL] + 3);
+			k++;
+			if (k == 1000) {
+				k = 0;
+				break;
+			}
+		}
+		data = rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL]);
+		return data;
+	} else {
+		return 0xFF;
+	}
+}
+EXPORT_SYMBOL(stg_efuse_read_1byte);
+
+void efuse92e_write_1byte(struct ieee80211_hw *hw, u16 address, u8 value)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 bytetemp;
+	u8 temp;
+	u32 k = 0;
+	const u32 efuse_real_content_len =
+		rtlpriv->cfg->maps[EFUSE_REAL_CONTENT_SIZE];
+
+	RT_TRACE(COMP_EFUSE, DBG_LOUD,
+		 ("Addr=%x Data =%x\n", address, value));
+
+	if (address < efuse_real_content_len) {
+		rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL], value);
+
+		temp = address & 0xFF;
+		rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 1,
+			       temp);
+		bytetemp = rtl_read_byte(rtlpriv,
+					 rtlpriv->cfg->maps[EFUSE_CTRL] + 2);
+
+		temp = ((address >> 8) & 0x03) | (bytetemp & 0xFC);
+		rtl_write_byte(rtlpriv,
+			       rtlpriv->cfg->maps[EFUSE_CTRL] + 2, temp);
+
+		bytetemp = rtl_read_byte(rtlpriv,
+					 rtlpriv->cfg->maps[EFUSE_CTRL] + 3);
+		temp = bytetemp | 0x80;
+		rtl_write_byte(rtlpriv,
+			       rtlpriv->cfg->maps[EFUSE_CTRL] + 3, temp);
+
+		bytetemp = rtl_read_byte(rtlpriv,
+					 rtlpriv->cfg->maps[EFUSE_CTRL] + 3);
+
+		while (bytetemp & 0x80) {
+			bytetemp = rtl_read_byte(rtlpriv,
+						 rtlpriv->cfg->
+						 maps[EFUSE_CTRL] + 3);
+			k++;
+			if (k == 100) {
+				k = 0;
+				break;
+			}
+		}
+	}
+}
+
+void read92e_efuse_byte(struct ieee80211_hw *hw, u16 _offset, u8 *pbuf)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 value32;
+	u8 readbyte;
+	u16 retry;
+
+	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 1,
+		       (_offset & 0xff));
+	readbyte = rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 2);
+	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 2,
+		       ((_offset >> 8) & 0x03) | (readbyte & 0xfc));
+
+	readbyte = rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 3);
+	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 3,
+		       (readbyte & 0x7f));
+
+	retry = 0;
+	value32 = rtl_read_dword(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL]);
+	while (!(((value32 >> 24) & 0xff) & 0x80) && (retry < 10000)) {
+		value32 = rtl_read_dword(rtlpriv,
+					 rtlpriv->cfg->maps[EFUSE_CTRL]);
+		retry++;
+	}
+
+	udelay(50);
+	value32 = rtl_read_dword(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL]);
+
+	*pbuf = (u8) (value32 & 0xff);
+}
+
+void read92e_efuse(struct ieee80211_hw *hw, u16 _offset, u16 _size_byte,
+		   u8 *pbuf)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u8 *efuse_tbl;
+	u8 rtemp8[1];
+	u16 efuse_addr = 0;
+	u8 offset, wren;
+	u8 u1temp = 0;
+	u16 i;
+	u16 j;
+	const u16 efuse_max_section =
+		rtlpriv->cfg->maps[EFUSE_MAX_SECTION_MAP];
+	const u32 efuse_real_content_len =
+		rtlpriv->cfg->maps[EFUSE_REAL_CONTENT_SIZE];
+	u16 **efuse_word;
+	u16 efuse_utilized = 0;
+	u8 efuse_usage;
+
+	if ((_offset + _size_byte) > rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE]) {
+		RT_TRACE(COMP_EFUSE, DBG_LOUD,
+			 ("read92e_efuse(): Invalid offset(%#x) with read bytes(%#x)!!\n",
+			 _offset, _size_byte));
+		return;
+	}
+
+	/* allocate memory for efuse_tbl and efuse_word */
+	efuse_tbl = kmalloc(rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE] *
+			    sizeof(u8), GFP_ATOMIC);
+	if (!efuse_tbl)
+		return;
+	efuse_word = kzalloc(EFUSE_MAX_WORD_UNIT * sizeof(u16 *), GFP_ATOMIC);
+	if (!efuse_word)
+		goto out;
+	for (i = 0; i < EFUSE_MAX_WORD_UNIT; i++) {
+		efuse_word[i] = kmalloc(efuse_max_section * sizeof(u16),
+					GFP_ATOMIC);
+		if (!efuse_word[i])
+			goto done;
+	}
+
+	for (i = 0; i < efuse_max_section; i++)
+		for (j = 0; j < EFUSE_MAX_WORD_UNIT; j++)
+			efuse_word[j][j] = 0xFFFF;
+
+	read92e_efuse_byte(hw, efuse_addr, rtemp8);
+	if (*rtemp8 != 0xFF) {
+		efuse_utilized++;
+		RTPRINT(rtlpriv, FEEPROM, EFUSE_READ_ALL,
+			"Addr=%d\n", efuse_addr);
+		efuse_addr++;
+	}
+
+	while ((*rtemp8 != 0xFF) && (efuse_addr < efuse_real_content_len)) {
+		/*  Check PG header for section num.  */
+		if ((*rtemp8 & 0x1F) == 0x0F) {/* extended header */
+			u1temp = ((*rtemp8 & 0xE0) >> 5);
+			read92e_efuse_byte(hw, efuse_addr, rtemp8);
+
+			if ((*rtemp8 & 0x0F) == 0x0F) {
+				efuse_addr++;
+				read92e_efuse_byte(hw, efuse_addr, rtemp8);
+
+				if (*rtemp8 != 0xFF &&
+				    (efuse_addr < efuse_real_content_len)) {
+					efuse_addr++;
+				}
+				continue;
+			} else {
+				offset = ((*rtemp8 & 0xF0) >> 1) | u1temp;
+				wren = (*rtemp8 & 0x0F);
+				efuse_addr++;
+			}
+		} else {
+			offset = ((*rtemp8 >> 4) & 0x0f);
+			wren = (*rtemp8 & 0x0f);
+		}
+
+		if (offset < efuse_max_section) {
+			RTPRINT(rtlpriv, FEEPROM, EFUSE_READ_ALL,
+				"offset-%d Worden=%x\n", offset, wren);
+
+			for (i = 0; i < EFUSE_MAX_WORD_UNIT; i++) {
+				if (!(wren & 0x01)) {
+					RTPRINT(rtlpriv, FEEPROM,
+						EFUSE_READ_ALL, "Addr=%d\n",
+						efuse_addr);
+
+					read92e_efuse_byte(hw, efuse_addr,
+							   rtemp8);
+					efuse_addr++;
+					efuse_utilized++;
+					efuse_word[i][offset] = (*rtemp8 &
+								 0xff);
+
+					if (efuse_addr >=
+					    efuse_real_content_len)
+						break;
+
+					RTPRINT(rtlpriv, FEEPROM,
+						EFUSE_READ_ALL, "Addr=%d\n",
+						efuse_addr);
+
+					read92e_efuse_byte(hw, efuse_addr,
+							   rtemp8);
+					efuse_addr++;
+					efuse_utilized++;
+					efuse_word[i][offset] |=
+					    (((u16) *rtemp8 << 8) & 0xff00);
+
+					if (efuse_addr >=
+					    efuse_real_content_len)
+						break;
+				}
+
+				wren >>= 1;
+			}
+		}
+
+		RTPRINT(rtlpriv, FEEPROM, EFUSE_READ_ALL,
+			"Addr=%d\n", efuse_addr);
+		read92e_efuse_byte(hw, efuse_addr, rtemp8);
+		if (*rtemp8 != 0xFF && (efuse_addr < efuse_real_content_len)) {
+			efuse_utilized++;
+			efuse_addr++;
+		}
+	}
+
+	for (i = 0; i < efuse_max_section; i++) {
+		for (j = 0; j < EFUSE_MAX_WORD_UNIT; j++) {
+			efuse_tbl[(i * 8) + (j * 2)] =
+			    (efuse_word[j][i] & 0xff);
+			efuse_tbl[(i * 8) + ((j * 2) + 1)] =
+			    ((efuse_word[j][i] >> 8) & 0xff);
+		}
+	}
+
+	for (i = 0; i < _size_byte; i++)
+		pbuf[i] = efuse_tbl[_offset + i];
+
+	rtlefuse->efuse_usedbytes = efuse_utilized;
+	efuse_usage = (u8) ((efuse_utilized * 100) / efuse_real_content_len);
+	rtlefuse->efuse_usedpercentage = efuse_usage;
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_EFUSE_BYTES,
+				      (u8 *)&efuse_utilized);
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_EFUSE_USAGE,
+				      (u8 *)&efuse_usage);
+done:
+	for (i = 0; i < EFUSE_MAX_WORD_UNIT; i++)
+		kfree(efuse_word[i]);
+	kfree(efuse_word);
+out:
+	kfree(efuse_tbl);
+}
+
+bool efuse92e_shadow_update_chk(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u8 section_idx, i, Base;
+	u16 words_need = 0, hdr_num = 0, totalbytes, efuse_used;
+	bool bwordchanged, bresult = true;
+
+	for (section_idx = 0; section_idx < 16; section_idx++) {
+		Base = section_idx * 8;
+		bwordchanged = false;
+
+		for (i = 0; i < 8; i = i + 2) {
+			if ((rtlefuse->efuse_map[EFUSE_INIT_MAP][Base + i] !=
+			     rtlefuse->efuse_map[EFUSE_MODIFY_MAP][Base + i]) ||
+			    (rtlefuse->efuse_map[EFUSE_INIT_MAP][Base + i + 1] !=
+			     rtlefuse->efuse_map[EFUSE_MODIFY_MAP][Base + i +
+								   1])) {
+				words_need++;
+				bwordchanged = true;
+			}
+		}
+
+		if (bwordchanged)
+			hdr_num++;
+	}
+
+	totalbytes = hdr_num + words_need * 2;
+	efuse_used = rtlefuse->efuse_usedbytes;
+
+	if ((totalbytes + efuse_used) >=
+	    (EFUSE_MAX_SIZE - rtlpriv->cfg->maps[EFUSE_OOB_PROTECT_BYTES_LEN]))
+		bresult = false;
+
+	RT_TRACE(COMP_EFUSE, DBG_LOUD,
+		 ("efuse92e_shadow_update_chk(): totalbytes(%#x), hdr_num(%#x), words_need(%#x), efuse_used(%d)\n",
+		  totalbytes, hdr_num, words_need, efuse_used));
+
+	return bresult;
+}
+
+void efuse92e_shadow_read(struct ieee80211_hw *hw, u8 type,
+		       u16 offset, u32 *value)
+{
+	if (type == 1)
+		efuse92e_shadow_read_1byte(hw, offset, (u8 *)value);
+	else if (type == 2)
+		efuse92e_shadow_read_2byte(hw, offset, (u16 *)value);
+	else if (type == 4)
+		efuse92e_shadow_read_4byte(hw, offset, (u32 *)value);
+}
+EXPORT_SYMBOL(efuse92e_shadow_read);
+
+void efuse92e_shadow_write(struct ieee80211_hw *hw, u8 type, u16 offset,
+			   u32 value)
+{
+	if (type == 1)
+		efuse92e_shadow_write_1byte(hw, offset, (u8)value);
+	else if (type == 2)
+		efuse92e_shadow_write_2byte(hw, offset, (u16)value);
+	else if (type == 4)
+		efuse92e_shadow_write_4byte(hw, offset, (u32)value);
+}
+
+bool efuse92e_shadow_update(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u16 i, offset, base;
+	u8 word_en = 0x0F;
+	u8 first_pg = false;
+
+	RT_TRACE(COMP_EFUSE, DBG_LOUD, ("\n"));
+
+	if (!efuse92e_shadow_update_chk(hw)) {
+		efuse_read_all_map(hw, &rtlefuse->efuse_map[EFUSE_INIT_MAP][0]);
+		memcpy(&rtlefuse->efuse_map[EFUSE_MODIFY_MAP][0],
+		       &rtlefuse->efuse_map[EFUSE_INIT_MAP][0],
+		       rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE]);
+
+		RT_TRACE(COMP_EFUSE, DBG_LOUD,
+			 ("efuse out of capacity!!\n"));
+		return false;
+	}
+	efuse_power_switch(hw, true, true);
+
+	for (offset = 0; offset < 16; offset++) {
+		word_en = 0x0F;
+		base = offset * 8;
+
+		for (i = 0; i < 8; i++) {
+			if (first_pg) {
+				word_en &= ~(BIT(i / 2));
+
+				rtlefuse->efuse_map[EFUSE_INIT_MAP][base + i] =
+				    rtlefuse->efuse_map[EFUSE_MODIFY_MAP][base + i];
+			} else {
+				if (rtlefuse->efuse_map[EFUSE_INIT_MAP][base + i] !=
+				    rtlefuse->efuse_map[EFUSE_MODIFY_MAP][base + i]) {
+					word_en &= ~(BIT(i / 2));
+
+					rtlefuse->efuse_map[EFUSE_INIT_MAP][base + i] =
+					    rtlefuse->efuse_map[EFUSE_MODIFY_MAP][base + i];
+				}
+			}
+		}
+
+		if (word_en != 0x0F) {
+			u8 tmpdata[8];
+			memcpy(tmpdata,
+			       (&rtlefuse->efuse_map[EFUSE_MODIFY_MAP][base]),
+			       8);
+			RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_LOUD,
+				      "U-efuse\n", tmpdata, 8);
+
+			if (!efuse_pg_packet_write(hw, (u8) offset, word_en,
+						   tmpdata)) {
+				RT_TRACE(COMP_ERR, DBG_WARNING,
+					 ("PG section(%#x) fail!!\n", offset));
+				break;
+			}
+		}
+	}
+
+	efuse_power_switch(hw, true, false);
+	efuse_read_all_map(hw, &rtlefuse->efuse_map[EFUSE_INIT_MAP][0]);
+
+	memcpy(&rtlefuse->efuse_map[EFUSE_MODIFY_MAP][0],
+	       &rtlefuse->efuse_map[EFUSE_INIT_MAP][0],
+	       rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE]);
+
+	RT_TRACE(COMP_EFUSE, DBG_LOUD, ("\n"));
+	return true;
+}
+
+void stg_rtl_efuse92e_shadow_map_update(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+
+	if (rtlefuse->autoload_failflag) {
+		memset(&rtlefuse->efuse_map[EFUSE_INIT_MAP][0],
+		       0xFF, rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE]);
+	} else {
+		efuse_read_all_map(hw, &rtlefuse->efuse_map[EFUSE_INIT_MAP][0]);
+	}
+
+	memcpy(&rtlefuse->efuse_map[EFUSE_MODIFY_MAP][0],
+	       &rtlefuse->efuse_map[EFUSE_INIT_MAP][0],
+	       rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE]);
+}
+EXPORT_SYMBOL(stg_rtl_efuse92e_shadow_map_update);
+
+void efuse92e_force_write_vendor_Id(struct ieee80211_hw *hw)
+{
+	u8 tmpdata[8] = { 0xFF, 0xFF, 0xEC, 0x10, 0xFF, 0xFF, 0xFF, 0xFF };
+
+	efuse_power_switch(hw, true, true);
+	efuse_pg_packet_write(hw, 1, 0xD, tmpdata);
+	efuse_power_switch(hw, true, false);
+}
+
+void efuse92e_re_pg_section(struct ieee80211_hw *hw, u8 section_idx)
+{
+}
+
+static void efuse92e_shadow_read_1byte(struct ieee80211_hw *hw,
+				       u16 offset, u8 *value)
+{
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	*value = rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset];
+}
+
+static void efuse92e_shadow_read_2byte(struct ieee80211_hw *hw,
+				       u16 offset, u16 *value)
+{
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+
+	*value = rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset];
+	*value |= rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 1] << 8;
+}
+
+static void efuse92e_shadow_read_4byte(struct ieee80211_hw *hw,
+				       u16 offset, u32 *value)
+{
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+
+	*value = rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset];
+	*value |= rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 1] << 8;
+	*value |= rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 2] << 16;
+	*value |= rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 3] << 24;
+}
+
+static void efuse92e_shadow_write_1byte(struct ieee80211_hw *hw,
+					u16 offset, u8 value)
+{
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+
+	rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset] = value;
+}
+
+static void efuse92e_shadow_write_2byte(struct ieee80211_hw *hw,
+					u16 offset, u16 value)
+{
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+
+	rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset] = value & 0x00FF;
+	rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 1] = value >> 8;
+}
+
+static void efuse92e_shadow_write_4byte(struct ieee80211_hw *hw,
+					u16 offset, u32 value)
+{
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+
+	rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset] =
+	    (u8) (value & 0x000000FF);
+	rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 1] =
+	    (u8) ((value >> 8) & 0x0000FF);
+	rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 2] =
+	    (u8) ((value >> 16) & 0x00FF);
+	rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 3] =
+	    (u8) ((value >> 24) & 0xFF);
+}
+
+int stg_efuse_one_byte_read(struct ieee80211_hw *hw, u16 addr, u8 *data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmpidx = 0;
+	int bresult;
+
+	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 1,
+		       (u8) (addr & 0xff));
+	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 2,
+		       ((u8) ((addr >> 8) & 0x03)) |
+		       (rtl_read_byte(rtlpriv,
+				      rtlpriv->cfg->maps[EFUSE_CTRL] + 2) &
+			0xFC));
+
+	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 3, 0x72);
+
+	while (!(0x80 & rtl_read_byte(rtlpriv,
+				      rtlpriv->cfg->maps[EFUSE_CTRL] + 3)) &&
+	       (tmpidx < 100)) {
+		tmpidx++;
+	}
+
+	if (tmpidx < 100) {
+		*data = rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL]);
+		bresult = true;
+	} else {
+		*data = 0xff;
+		bresult = false;
+	}
+	return bresult;
+}
+EXPORT_SYMBOL(stg_efuse_one_byte_read);
+
+static int efuse_one_byte_write(struct ieee80211_hw *hw, u16 addr, u8 data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmpidx = 0;
+	bool bresult;
+
+	RT_TRACE(COMP_EFUSE, DBG_LOUD,
+		 ("Addr = %x Data=%x\n", addr, data));
+
+	rtl_write_byte(rtlpriv,
+		       rtlpriv->cfg->maps[EFUSE_CTRL] + 1, (u8) (addr & 0xff));
+	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 2,
+		       (rtl_read_byte(rtlpriv,
+			 rtlpriv->cfg->maps[EFUSE_CTRL] +
+			 2) & 0xFC) | (u8) ((addr >> 8) & 0x03));
+
+	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL], data);
+	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 3, 0xF2);
+
+	while ((0x80 & rtl_read_byte(rtlpriv,
+				     rtlpriv->cfg->maps[EFUSE_CTRL] + 3)) &&
+	       (tmpidx < 100)) {
+		tmpidx++;
+	}
+
+	if (tmpidx < 100)
+		bresult = true;
+	else
+		bresult = false;
+
+	return bresult;
+}
+
+static void efuse_read_all_map(struct ieee80211_hw *hw, u8 *efuse)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	efuse_power_switch(hw, false, true);
+	read92e_efuse(hw, 0, rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE], efuse);
+	efuse_power_switch(hw, false, false);
+}
+
+static void efuse_read_data_case1(struct ieee80211_hw *hw, u16 *efuse_addr,
+				u8 efuse_data, u8 offset, u8 *tmpdata,
+				u8 *readstate)
+{
+	bool bdataempty = true;
+	u8 hoffset;
+	u8 tmpidx;
+	u8 hworden;
+	u8 word_cnts;
+
+	hoffset = (efuse_data >> 4) & 0x0F;
+	hworden = efuse_data & 0x0F;
+	word_cnts = efuse_calculate_word_cnts(hworden);
+
+	if (hoffset == offset) {
+		for (tmpidx = 0; tmpidx < word_cnts * 2; tmpidx++) {
+			if (stg_efuse_one_byte_read(hw, *efuse_addr + 1 + tmpidx,
+						    &efuse_data)) {
+				tmpdata[tmpidx] = efuse_data;
+				if (efuse_data != 0xff)
+					bdataempty = false;
+			}
+		}
+
+		if (!bdataempty) {
+			*readstate = PG_STATE_DATA;
+		} else {
+			*efuse_addr = *efuse_addr + (word_cnts * 2) + 1;
+			*readstate = PG_STATE_HEADER;
+		}
+
+	} else {
+		*efuse_addr = *efuse_addr + (word_cnts * 2) + 1;
+		*readstate = PG_STATE_HEADER;
+	}
+}
+
+static int efuse_pg_packet_read(struct ieee80211_hw *hw, u8 offset, u8 *data)
+{
+	u8 readstate = PG_STATE_HEADER;
+
+	bool bcontinual = true;
+
+	u8 efuse_data, word_cnts = 0;
+	u16 efuse_addr = 0;
+	u8 hworden = 0;
+	u8 tmpdata[8];
+
+	if (data == NULL)
+		return false;
+	if (offset > 15)
+		return false;
+
+	memset(data, 0xff, PGPKT_DATA_SIZE * sizeof(u8));
+	memset(tmpdata, 0xff, PGPKT_DATA_SIZE * sizeof(u8));
+
+	while (bcontinual && (efuse_addr < EFUSE_MAX_SIZE)) {
+		if (readstate & PG_STATE_HEADER) {
+			if (stg_efuse_one_byte_read(hw, efuse_addr, &efuse_data) &&
+			    (efuse_data != 0xFF))
+				efuse_read_data_case1(hw, &efuse_addr,
+						      efuse_data, offset,
+						      tmpdata, &readstate);
+			else
+				bcontinual = false;
+		} else if (readstate & PG_STATE_DATA) {
+			efuse_word_enable_data_read(hworden, tmpdata, data);
+			efuse_addr = efuse_addr + (word_cnts * 2) + 1;
+			readstate = PG_STATE_HEADER;
+		}
+	}
+
+	if ((data[0] == 0xff) && (data[1] == 0xff) &&
+	    (data[2] == 0xff) && (data[3] == 0xff) &&
+	    (data[4] == 0xff) && (data[5] == 0xff) &&
+	    (data[6] == 0xff) && (data[7] == 0xff))
+		return false;
+	else
+		return true;
+}
+
+static void efuse_write_data_case1(struct ieee80211_hw *hw, u16 *efuse_addr,
+				   u8 efuse_data, u8 offset,
+				   int *bcontinual, u8 *write_state,
+				   struct pgpkt_struct *target_pkt,
+				   int *repeat_times, int *bresult, u8 word_en)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct pgpkt_struct tmp_pkt;
+	int bdataempty = true;
+	u8 originaldata[8 * sizeof(u8)];
+	u8 badworden = 0x0F;
+	u8 match_word_en, tmp_word_en;
+	u8 tmpindex;
+	u8 tmp_header = efuse_data;
+	u8 tmp_word_cnts;
+
+	tmp_pkt.offset = (tmp_header >> 4) & 0x0F;
+	tmp_pkt.word_en = tmp_header & 0x0F;
+	tmp_word_cnts = efuse_calculate_word_cnts(tmp_pkt.word_en);
+
+	if (tmp_pkt.offset != target_pkt->offset) {
+		*efuse_addr = *efuse_addr + (tmp_word_cnts * 2) + 1;
+		*write_state = PG_STATE_HEADER;
+	} else {
+		for (tmpindex = 0; tmpindex < (tmp_word_cnts * 2); tmpindex++) {
+			if (stg_efuse_one_byte_read(hw,
+						(*efuse_addr + 1 + tmpindex),
+						&efuse_data) &&
+			    (efuse_data != 0xFF))
+				bdataempty = false;
+		}
+
+		if (!bdataempty) {
+			*efuse_addr = *efuse_addr + (tmp_word_cnts * 2) + 1;
+			*write_state = PG_STATE_HEADER;
+		} else {
+			match_word_en = 0x0F;
+			if (!((target_pkt->word_en & BIT(0)) |
+			    (tmp_pkt.word_en & BIT(0))))
+				match_word_en &= (~BIT(0));
+
+			if (!((target_pkt->word_en & BIT(1)) |
+			    (tmp_pkt.word_en & BIT(1))))
+				match_word_en &= (~BIT(1));
+
+			if (!((target_pkt->word_en & BIT(2)) |
+			    (tmp_pkt.word_en & BIT(2))))
+				match_word_en &= (~BIT(2));
+
+			if (!((target_pkt->word_en & BIT(3)) |
+			    (tmp_pkt.word_en & BIT(3))))
+				match_word_en &= (~BIT(3));
+
+			if ((match_word_en & 0x0F) != 0x0F) {
+				badworden = efuse_word_enable_data_write(hw,
+							*efuse_addr + 1,
+							tmp_pkt.word_en,
+							target_pkt->data);
+
+				if (0x0F != (badworden & 0x0F))	{
+					u8 reorg_offset = offset;
+					u8 reorg_worden = badworden;
+					efuse_pg_packet_write(hw, reorg_offset,
+							      reorg_worden,
+							      originaldata);
+				}
+
+				tmp_word_en = 0x0F;
+				if ((target_pkt->word_en & BIT(0)) ^
+				    (match_word_en & BIT(0)))
+					tmp_word_en &= (~BIT(0));
+
+				if ((target_pkt->word_en & BIT(1)) ^
+				    (match_word_en & BIT(1)))
+					tmp_word_en &= (~BIT(1));
+
+				if ((target_pkt->word_en & BIT(2)) ^
+				    (match_word_en & BIT(2)))
+					tmp_word_en &= (~BIT(2));
+
+				if ((target_pkt->word_en & BIT(3)) ^
+				    (match_word_en & BIT(3)))
+					tmp_word_en &= (~BIT(3));
+
+				if ((tmp_word_en & 0x0F) != 0x0F) {
+					*efuse_addr = efuse_get_current_size(hw);
+					target_pkt->offset = offset;
+					target_pkt->word_en = tmp_word_en;
+				} else {
+					*bcontinual = false;
+				}
+				*write_state = PG_STATE_HEADER;
+				*repeat_times += 1;
+				if (*repeat_times > EFUSE_REPEAT_THRESHOLD_) {
+					*bcontinual = false;
+					*bresult = false;
+				}
+			} else {
+				*efuse_addr += (2 * tmp_word_cnts) + 1;
+				target_pkt->offset = offset;
+				target_pkt->word_en = word_en;
+				*write_state = PG_STATE_HEADER;
+			}
+		}
+	}
+	RTPRINT(rtlpriv, FEEPROM, EFUSE_PG, "efuse PG_STATE_HEADER-1\n");
+}
+
+static void efuse_write_data_case2(struct ieee80211_hw *hw, u16 *efuse_addr,
+				   int *bcontinual, u8 *write_state,
+				   struct pgpkt_struct target_pkt,
+				   int *repeat_times, int *bresult)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct pgpkt_struct tmp_pkt;
+	u8 pg_header;
+	u8 tmp_header;
+	u8 originaldata[8 * sizeof(u8)];
+	u8 tmp_word_cnts;
+	u8 badworden = 0x0F;
+
+	pg_header = ((target_pkt.offset << 4) & 0xf0) | target_pkt.word_en;
+	efuse_one_byte_write(hw, *efuse_addr, pg_header);
+	stg_efuse_one_byte_read(hw, *efuse_addr, &tmp_header);
+
+	if (tmp_header == pg_header) {
+		*write_state = PG_STATE_DATA;
+	} else if (tmp_header == 0xFF) {
+		*write_state = PG_STATE_HEADER;
+		*repeat_times += 1;
+		if (*repeat_times > EFUSE_REPEAT_THRESHOLD_) {
+			*bcontinual = false;
+			*bresult = false;
+		}
+	} else {
+		tmp_pkt.offset = (tmp_header >> 4) & 0x0F;
+		tmp_pkt.word_en = tmp_header & 0x0F;
+
+		tmp_word_cnts = efuse_calculate_word_cnts(tmp_pkt.word_en);
+
+		memset(originaldata, 0xff,  8 * sizeof(u8));
+
+		if (efuse_pg_packet_read(hw, tmp_pkt.offset, originaldata)) {
+			badworden = efuse_word_enable_data_write(hw,
+								 *efuse_addr + 1,
+								 tmp_pkt.word_en,
+								 originaldata);
+
+			if (0x0F != (badworden & 0x0F)) {
+				u8 reorg_offset = tmp_pkt.offset;
+				u8 reorg_worden = badworden;
+				efuse_pg_packet_write(hw, reorg_offset,
+						      reorg_worden,
+						      originaldata);
+				*efuse_addr = efuse_get_current_size(hw);
+			} else {
+				*efuse_addr = *efuse_addr +
+					      (tmp_word_cnts * 2) + 1;
+			}
+		} else {
+			*efuse_addr = *efuse_addr + (tmp_word_cnts * 2) + 1;
+		}
+
+		*write_state = PG_STATE_HEADER;
+		*repeat_times += 1;
+		if (*repeat_times > EFUSE_REPEAT_THRESHOLD_) {
+			*bcontinual = false;
+			*bresult = false;
+		}
+
+		RTPRINT(rtlpriv, FEEPROM, EFUSE_PG,
+			"efuse PG_STATE_HEADER-2\n");
+	}
+}
+
+static int efuse_pg_packet_write(struct ieee80211_hw *hw,
+				 u8 offset, u8 word_en, u8 *data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct pgpkt_struct target_pkt;
+	u8 write_state = PG_STATE_HEADER;
+	int bcontinual = true, bdataempty = true, bresult = true;
+	u16 efuse_addr = 0;
+	u8 efuse_data;
+	u8 target_word_cnts = 0;
+	u8 badworden = 0x0F;
+	static int repeat_times;
+
+	if (efuse_get_current_size(hw) >= (EFUSE_MAX_SIZE -
+		rtlpriv->cfg->maps[EFUSE_OOB_PROTECT_BYTES_LEN])) {
+		RTPRINT(rtlpriv, FEEPROM, EFUSE_PG,
+			"efuse_pg_packet_write error\n");
+		return false;
+	}
+
+	target_pkt.offset = offset;
+	target_pkt.word_en = word_en;
+
+	memset(target_pkt.data, 0xFF,  8 * sizeof(u8));
+
+	efuse_word_enable_data_read(word_en, data, target_pkt.data);
+	target_word_cnts = efuse_calculate_word_cnts(target_pkt.word_en);
+
+	RTPRINT(rtlpriv, FEEPROM, EFUSE_PG, "efuse Power ON\n");
+
+	while (bcontinual && (efuse_addr < (EFUSE_MAX_SIZE -
+	       rtlpriv->cfg->maps[EFUSE_OOB_PROTECT_BYTES_LEN]))) {
+		if (write_state == PG_STATE_HEADER) {
+			bdataempty = true;
+			badworden = 0x0F;
+			RTPRINT(rtlpriv, FEEPROM, EFUSE_PG,
+				"efuse PG_STATE_HEADER\n");
+
+			if (stg_efuse_one_byte_read(hw, efuse_addr, &efuse_data) &&
+			    (efuse_data != 0xFF))
+				efuse_write_data_case1(hw, &efuse_addr,
+						       efuse_data, offset,
+						       &bcontinual,
+						       &write_state,
+						       &target_pkt,
+						       &repeat_times, &bresult,
+						       word_en);
+			else
+				efuse_write_data_case2(hw, &efuse_addr,
+						       &bcontinual,
+						       &write_state,
+						       target_pkt,
+						       &repeat_times,
+						       &bresult);
+
+		} else if (write_state == PG_STATE_DATA) {
+			RTPRINT(rtlpriv, FEEPROM, EFUSE_PG,
+				"efuse PG_STATE_DATA\n");
+			badworden = 0x0f;
+			badworden =
+			    efuse_word_enable_data_write(hw, efuse_addr + 1,
+							 target_pkt.word_en,
+							 target_pkt.data);
+
+			if ((badworden & 0x0F) == 0x0F) {
+				bcontinual = false;
+			} else {
+				efuse_addr =
+				    efuse_addr + (2 * target_word_cnts) + 1;
+
+				target_pkt.offset = offset;
+				target_pkt.word_en = badworden;
+				target_word_cnts =
+				    efuse_calculate_word_cnts(target_pkt.
+							      word_en);
+				write_state = PG_STATE_HEADER;
+				repeat_times++;
+				if (repeat_times > EFUSE_REPEAT_THRESHOLD_) {
+					bcontinual = false;
+					bresult = false;
+				}
+				RTPRINT(rtlpriv, FEEPROM, EFUSE_PG,
+					"efuse PG_STATE_HEADER-3\n");
+			}
+		}
+	}
+
+	if (efuse_addr >= (EFUSE_MAX_SIZE -
+		rtlpriv->cfg->maps[EFUSE_OOB_PROTECT_BYTES_LEN])) {
+		RT_TRACE(COMP_EFUSE, DBG_LOUD,
+			 ("efuse_addr(%#x) Out of size!!\n", efuse_addr));
+	}
+
+	return true;
+}
+
+static void efuse_word_enable_data_read(u8 word_en, u8 *sourdata,
+					u8 *targetdata)
+{
+	if (!(word_en & BIT(0))) {
+		targetdata[0] = sourdata[0];
+		targetdata[1] = sourdata[1];
+	}
+
+	if (!(word_en & BIT(1))) {
+		targetdata[2] = sourdata[2];
+		targetdata[3] = sourdata[3];
+	}
+
+	if (!(word_en & BIT(2))) {
+		targetdata[4] = sourdata[4];
+		targetdata[5] = sourdata[5];
+	}
+
+	if (!(word_en & BIT(3))) {
+		targetdata[6] = sourdata[6];
+		targetdata[7] = sourdata[7];
+	}
+}
+
+static u8 efuse_word_enable_data_write(struct ieee80211_hw *hw,
+				       u16 efuse_addr, u8 word_en, u8 *data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u16 tmpaddr;
+	u16 start_addr = efuse_addr;
+	u8 badworden = 0x0F;
+	u8 tmpdata[8];
+
+	memset(tmpdata, 0xff, PGPKT_DATA_SIZE);
+	RT_TRACE(COMP_EFUSE, DBG_LOUD,
+		 ("word_en = %x efuse_addr=%x\n", word_en, efuse_addr));
+
+	if (!(word_en & BIT(0))) {
+		tmpaddr = start_addr;
+		efuse_one_byte_write(hw, start_addr++, data[0]);
+		efuse_one_byte_write(hw, start_addr++, data[1]);
+
+		stg_efuse_one_byte_read(hw, tmpaddr, &tmpdata[0]);
+		stg_efuse_one_byte_read(hw, tmpaddr + 1, &tmpdata[1]);
+		if ((data[0] != tmpdata[0]) || (data[1] != tmpdata[1]))
+			badworden &= (~BIT(0));
+	}
+
+	if (!(word_en & BIT(1))) {
+		tmpaddr = start_addr;
+		efuse_one_byte_write(hw, start_addr++, data[2]);
+		efuse_one_byte_write(hw, start_addr++, data[3]);
+
+		stg_efuse_one_byte_read(hw, tmpaddr, &tmpdata[2]);
+		stg_efuse_one_byte_read(hw, tmpaddr + 1, &tmpdata[3]);
+		if ((data[2] != tmpdata[2]) || (data[3] != tmpdata[3]))
+			badworden &= (~BIT(1));
+	}
+
+	if (!(word_en & BIT(2))) {
+		tmpaddr = start_addr;
+		efuse_one_byte_write(hw, start_addr++, data[4]);
+		efuse_one_byte_write(hw, start_addr++, data[5]);
+
+		stg_efuse_one_byte_read(hw, tmpaddr, &tmpdata[4]);
+		stg_efuse_one_byte_read(hw, tmpaddr + 1, &tmpdata[5]);
+		if ((data[4] != tmpdata[4]) || (data[5] != tmpdata[5]))
+			badworden &= (~BIT(2));
+	}
+
+	if (!(word_en & BIT(3))) {
+		tmpaddr = start_addr;
+		efuse_one_byte_write(hw, start_addr++, data[6]);
+		efuse_one_byte_write(hw, start_addr++, data[7]);
+
+		stg_efuse_one_byte_read(hw, tmpaddr, &tmpdata[6]);
+		stg_efuse_one_byte_read(hw, tmpaddr + 1, &tmpdata[7]);
+		if ((data[6] != tmpdata[6]) || (data[7] != tmpdata[7]))
+			badworden &= (~BIT(3));
+	}
+
+	return badworden;
+}
+
+static void efuse_power_switch(struct ieee80211_hw *hw, u8 bwrite, u8 pwrstate)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 tempval;
+	u16 tmpv16;
+
+	if (pwrstate && (rtlhal->hw_type != HARDWARE_TYPE_RTL8192SE)) {
+		if (rtlhal->hw_type != HARDWARE_TYPE_RTL8192CE &&
+		    rtlhal->hw_type != HARDWARE_TYPE_RTL8192DE) {
+			rtl_write_byte(rtlpriv,
+				       rtlpriv->cfg->maps[EFUSE_ACCESS], 0x69);
+		} else {
+			tmpv16 = rtl_read_word(rtlpriv,
+					       rtlpriv->cfg->maps[SYS_ISO_CTRL]);
+			if (!(tmpv16 & rtlpriv->cfg->maps[EFUSE_PWC_EV12V])) {
+				tmpv16 |= rtlpriv->cfg->maps[EFUSE_PWC_EV12V];
+				rtl_write_word(rtlpriv,
+					       rtlpriv->cfg->maps[SYS_ISO_CTRL],
+					       tmpv16);
+			}
+		}
+		tmpv16 = rtl_read_word(rtlpriv,
+				       rtlpriv->cfg->maps[SYS_FUNC_EN]);
+		if (!(tmpv16 & rtlpriv->cfg->maps[EFUSE_FEN_ELDR])) {
+			tmpv16 |= rtlpriv->cfg->maps[EFUSE_FEN_ELDR];
+			rtl_write_word(rtlpriv,
+				       rtlpriv->cfg->maps[SYS_FUNC_EN], tmpv16);
+		}
+
+		tmpv16 = rtl_read_word(rtlpriv, rtlpriv->cfg->maps[SYS_CLK]);
+		if ((!(tmpv16 & rtlpriv->cfg->maps[EFUSE_LOADER_CLK_EN])) ||
+		    (!(tmpv16 & rtlpriv->cfg->maps[EFUSE_ANA8M]))) {
+			tmpv16 |= (rtlpriv->cfg->maps[EFUSE_LOADER_CLK_EN] |
+				   rtlpriv->cfg->maps[EFUSE_ANA8M]);
+			rtl_write_word(rtlpriv,
+				       rtlpriv->cfg->maps[SYS_CLK], tmpv16);
+		}
+	}
+
+	if (pwrstate) {
+		if (bwrite) {
+			tempval = rtl_read_byte(rtlpriv,
+						rtlpriv->cfg->maps[EFUSE_TEST] +
+						3);
+
+			if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+				tempval &= ~(BIT(3) | BIT(4) | BIT(5) | BIT(6));
+				tempval |= (VOLTAGE_V25 << 3);
+			} else if (rtlhal->hw_type != HARDWARE_TYPE_RTL8192SE) {
+				tempval &= 0x0F;
+				tempval |= (VOLTAGE_V25 << 4);
+			}
+
+			rtl_write_byte(rtlpriv,
+				       rtlpriv->cfg->maps[EFUSE_TEST] + 3,
+				       (tempval | 0x80));
+		}
+
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192SE) {
+			rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CLK],
+				       0x03);
+		}
+	} else {
+		if (rtlhal->hw_type != HARDWARE_TYPE_RTL8192CE &&
+		    rtlhal->hw_type != HARDWARE_TYPE_RTL8192DE)
+			rtl_write_byte(rtlpriv,
+				       rtlpriv->cfg->maps[EFUSE_ACCESS], 0);
+		if (bwrite) {
+			tempval = rtl_read_byte(rtlpriv,
+						rtlpriv->cfg->maps[EFUSE_TEST] +
+						3);
+			rtl_write_byte(rtlpriv,
+				       rtlpriv->cfg->maps[EFUSE_TEST] + 3,
+				       (tempval & 0x7F));
+		}
+
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192SE) {
+			rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CLK],
+				       0x02);
+		}
+	}
+}
+
+static u16 efuse_get_current_size(struct ieee80211_hw *hw)
+{
+	int bcontinual = true;
+	u16 efuse_addr = 0;
+	u8 hoffset, hworden;
+	u8 efuse_data, word_cnts;
+
+	while (bcontinual &&
+	       stg_efuse_one_byte_read(hw, efuse_addr, &efuse_data) &&
+	       (efuse_addr < EFUSE_MAX_SIZE)) {
+		if (efuse_data != 0xFF) {
+			hoffset = (efuse_data >> 4) & 0x0F;
+			hworden = efuse_data & 0x0F;
+			word_cnts = efuse_calculate_word_cnts(hworden);
+			efuse_addr = efuse_addr + (word_cnts * 2) + 1;
+		} else {
+			bcontinual = false;
+		}
+	}
+
+	return efuse_addr;
+}
+
+static u8 efuse_calculate_word_cnts(u8 word_en)
+{
+	u8 word_cnts = 0;
+	if (!(word_en & BIT(0)))
+		word_cnts++;
+	if (!(word_en & BIT(1)))
+		word_cnts++;
+	if (!(word_en & BIT(2)))
+		word_cnts++;
+	if (!(word_en & BIT(3)))
+		word_cnts++;
+	return word_cnts;
+}
diff --git a/drivers/staging/rtl8192ee/efuse.h b/drivers/staging/rtl8192ee/efuse.h
new file mode 100644
index 00000000000..cc3e1116b3e
--- /dev/null
+++ b/drivers/staging/rtl8192ee/efuse.h
@@ -0,0 +1,127 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL_EFUSE_H_
+#define __RTL_EFUSE_H_
+
+#define EFUSE_IC_ID_OFFSET		506
+
+/*
+#define EFUSE_REAL_CONTENT_LEN	512
+#define EFUSE_MAP_LEN			128
+#define EFUSE_MAX_SECTION		16
+#define EFUSE_MAX_WORD_UNIT		4
+#define EFUSE_IC_ID_OFFSET		506
+*/
+
+#define EFUSE_MAX_WORD_UNIT		4
+
+#define EFUSE_INIT_MAP			0
+#define EFUSE_MODIFY_MAP		1
+
+#define PG_STATE_HEADER			0x01
+#define PG_STATE_WORD_0			0x02
+#define PG_STATE_WORD_1			0x04
+#define PG_STATE_WORD_2			0x08
+#define PG_STATE_WORD_3			0x10
+#define PG_STATE_DATA			0x20
+
+#define PG_SWBYTE_H			0x01
+#define PG_SWBYTE_L			0x02
+
+#define _POWERON_DELAY_
+#define _PRE_EXECUTE_READ_CMD_
+
+#define EFUSE_REPEAT_THRESHOLD_		3
+#define EFUSE_ERROE_HANDLE		1
+
+struct efuse_map {
+	u8 offset;
+	u8 word_start;
+	u8 byte_start;
+	u8 byte_cnts;
+};
+
+struct pgpkt_struct {
+	u8 offset;
+	u8 word_en;
+	u8 data[8];
+};
+
+enum efuse_data_item {
+	EFUSE_CHIP_ID = 0,
+	EFUSE_LDO_SETTING,
+	EFUSE_CLK_SETTING,
+	EFUSE_SDIO_SETTING,
+	EFUSE_CCCR,
+	EFUSE_SDIO_MODE,
+	EFUSE_OCR,
+	EFUSE_F0CIS,
+	EFUSE_F1CIS,
+	EFUSE_MAC_ADDR,
+	EFUSE_EEPROM_VER,
+	EFUSE_CHAN_PLAN,
+	EFUSE_TXPW_TAB
+};
+
+enum {
+	VOLTAGE_V25 = 0x03,
+	LDOE25_SHIFT = 28,
+};
+
+struct efuse_priv {
+	u8 id[2];
+	u8 ldo_setting[2];
+	u8 clk_setting[2];
+	u8 cccr;
+	u8 sdio_mode;
+	u8 ocr[3];
+	u8 cis0[17];
+	u8 cis1[48];
+	u8 mac_addr[6];
+	u8 eeprom_verno;
+	u8 channel_plan;
+	u8 tx_power_b[14];
+	u8 tx_power_g[14];
+};
+
+void read92e_efuse_byte(struct ieee80211_hw *hw, u16 _offset, u8 *pbuf);
+void efuse92e_initialize(struct ieee80211_hw *hw);
+u8 stg_efuse_read_1byte(struct ieee80211_hw *hw, u16 address);
+int stg_efuse_one_byte_read(struct ieee80211_hw *hw, u16 addr, u8 *data);
+void efuse92e_write_1byte(struct ieee80211_hw *hw, u16 address, u8 value);
+void read92e_efuse(struct ieee80211_hw *hw, u16 _offset,
+		   u16 _size_byte, u8 *pbuf);
+void efuse92e_shadow_read(struct ieee80211_hw *hw, u8 type,
+		          u16 offset, u32 *value);
+void efuse92e_shadow_write(struct ieee80211_hw *hw, u8 type,
+			   u16 offset, u32 value);
+bool efuse92e_shadow_update(struct ieee80211_hw *hw);
+bool efuse92e_shadow_update_chk(struct ieee80211_hw *hw);
+void stg_rtl_efuse92e_shadow_map_update(struct ieee80211_hw *hw);
+void efuse92e_force_write_vendor_Id(struct ieee80211_hw *hw);
+void efuse92e_re_pg_section(struct ieee80211_hw *hw, u8 section_idx);
+
+#endif
diff --git a/drivers/staging/rtl8192ee/pci.c b/drivers/staging/rtl8192ee/pci.c
new file mode 100644
index 00000000000..3fe9b7ba01d
--- /dev/null
+++ b/drivers/staging/rtl8192ee/pci.c
@@ -0,0 +1,2397 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "core.h"
+#include "wifi.h"
+#include "pci.h"
+#include "base.h"
+#include "ps.h"
+#include "efuse.h"
+
+static const u16 pcibridge_vendors[PCI_BRIDGE_VENDOR_MAX] = {
+	INTEL_VENDOR_ID,
+	ATI_VENDOR_ID,
+	AMD_VENDOR_ID,
+	SIS_VENDOR_ID
+};
+
+static const u8 ac_to_hwq[] = {
+	VO_QUEUE,
+	VI_QUEUE,
+	BE_QUEUE,
+	BK_QUEUE
+};
+
+static u8 _rtl_mac_to_hwqueue(struct ieee80211_hw *hw,
+			      struct sk_buff *skb)
+{
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	__le16 fc = rtl_get_fc(skb);
+	u8 queue_index = skb_get_queue_mapping(skb);
+
+	if (unlikely(ieee80211_is_beacon(fc)))
+		return BEACON_QUEUE;
+	if (ieee80211_is_mgmt(fc) || ieee80211_is_ctl(fc))
+		return MGNT_QUEUE;
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192SE)
+		if (ieee80211_is_nullfunc(fc))
+			return HIGH_QUEUE;
+
+	return ac_to_hwq[queue_index];
+}
+
+/* Update PCI dependent default settings*/
+static void _rtl_pci_update_default_setting(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u8 pcibridge_vendor = pcipriv->ndis_adapter.pcibridge_vendor;
+	u8 init_aspm;
+
+	ppsc->reg_rfps_level = 0;
+	ppsc->b_support_aspm = 0;
+
+	/*Update PCI ASPM setting */
+	ppsc->const_amdpci_aspm = rtlpci->const_amdpci_aspm;
+	switch (rtlpci->const_pci_aspm) {
+	case 0:
+		/*No ASPM */
+		break;
+
+	case 1:
+		/*ASPM dynamically enabled/disable. */
+		ppsc->reg_rfps_level |= RT_RF_LPS_LEVEL_ASPM;
+		break;
+
+	case 2:
+		/*ASPM with Clock Req dynamically enabled/disable. */
+		ppsc->reg_rfps_level |= (RT_RF_LPS_LEVEL_ASPM |
+					 RT_RF_OFF_LEVL_CLK_REQ);
+		break;
+
+	case 3:
+		/*
+		 * Always enable ASPM and Clock Req
+		 * from initialization to halt.
+		 * */
+		ppsc->reg_rfps_level &= ~(RT_RF_LPS_LEVEL_ASPM);
+		ppsc->reg_rfps_level |= (RT_RF_PS_LEVEL_ALWAYS_ASPM |
+					 RT_RF_OFF_LEVL_CLK_REQ);
+		break;
+
+	case 4:
+		/*
+		 * Always enable ASPM without Clock Req
+		 * from initialization to halt.
+		 * */
+		ppsc->reg_rfps_level &= ~(RT_RF_LPS_LEVEL_ASPM |
+					  RT_RF_OFF_LEVL_CLK_REQ);
+		ppsc->reg_rfps_level |= RT_RF_PS_LEVEL_ALWAYS_ASPM;
+		break;
+	}
+
+	ppsc->reg_rfps_level |= RT_RF_OFF_LEVL_HALT_NIC;
+
+	/*Update Radio OFF setting */
+	switch (rtlpci->const_hwsw_rfoff_d3) {
+	case 1:
+		if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM)
+			ppsc->reg_rfps_level |= RT_RF_OFF_LEVL_ASPM;
+		break;
+
+	case 2:
+		if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM)
+			ppsc->reg_rfps_level |= RT_RF_OFF_LEVL_ASPM;
+		ppsc->reg_rfps_level |= RT_RF_OFF_LEVL_HALT_NIC;
+		break;
+
+	case 3:
+		ppsc->reg_rfps_level |= RT_RF_OFF_LEVL_PCI_D3;
+		break;
+	}
+
+	/*Set HW definition to determine if it supports ASPM. */
+	switch (rtlpci->const_support_pciaspm) {
+	case 0:{
+			/*Not support ASPM. */
+			bool b_support_aspm = false;
+			ppsc->b_support_aspm = b_support_aspm;
+			break;
+		}
+	case 1:{
+			/*Support ASPM. */
+			bool b_support_aspm = true;
+			bool b_support_backdoor = true;
+			ppsc->b_support_aspm = b_support_aspm;
+
+			/*if (priv->oem_id == RT_CID_TOSHIBA &&
+			   !priv->ndis_adapter.amd_l1_patch)
+			   b_support_backdoor = false; */
+
+			ppsc->b_support_backdoor = b_support_backdoor;
+
+			break;
+		}
+	case 2:
+		/*ASPM value set by chipset. */
+		if (pcibridge_vendor == PCI_BRIDGE_VENDOR_INTEL) {
+			bool b_support_aspm = true;
+			ppsc->b_support_aspm = b_support_aspm;
+		}
+		break;
+	default:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("switch case not process\n"));
+		break;
+	}
+
+	/* toshiba aspm issue, toshiba will set aspm selfly
+	 * so we should not set aspm in driver */
+	pci_read_config_byte(rtlpci->pdev, 0x80, &init_aspm);
+	if (rtlpriv->rtlhal.hw_type == HARDWARE_TYPE_RTL8192SE &&
+	    init_aspm == 0x43)
+		ppsc->b_support_aspm = false;
+}
+
+static bool _rtl_pci_platform_switch_device_pci_aspm(struct ieee80211_hw *hw,
+						     u8 value)
+{
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool bresult = false;
+
+	if (rtlhal->hw_type != HARDWARE_TYPE_RTL8192SE)
+		value |= 0x40;
+
+	pci_write_config_byte(rtlpci->pdev, 0x80, value);
+
+	return bresult;
+}
+
+/*When we set 0x01 to enable clk request. Set 0x0 to disable clk req.*/
+static bool _rtl_pci_switch_clk_req(struct ieee80211_hw *hw, u8 value)
+{
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool bresult = false;
+
+	pci_write_config_byte(rtlpci->pdev, 0x81, value);
+	bresult = true;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192SE)
+		udelay(100);
+
+	return bresult;
+}
+
+/*Disable RTL8192SE ASPM & Disable Pci Bridge ASPM*/
+static void rtl_pci_disable_aspm(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u8 pcibridge_vendor = pcipriv->ndis_adapter.pcibridge_vendor;
+	u32 pcicfg_addrport = pcipriv->ndis_adapter.pcicfg_addrport;
+	u8 num4bytes = pcipriv->ndis_adapter.num4bytes;
+	/*Retrieve original configuration settings. */
+	u8 linkctrl_reg = pcipriv->ndis_adapter.linkctrl_reg;
+	u16 pcibridge_linkctrlreg = pcipriv->ndis_adapter.
+				pcibridge_linkctrlreg;
+	u16 aspmlevel = 0;
+
+	if (!ppsc->b_support_aspm)
+		return;
+
+	if (pcibridge_vendor == PCI_BRIDGE_VENDOR_UNKNOWN) {
+		RT_TRACE(COMP_POWER, DBG_TRACE,
+			 ("PCI(Bridge) UNKNOWN.\n"));
+
+		return;
+	}
+
+	if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_CLK_REQ) {
+		RT_CLEAR_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_CLK_REQ);
+		_rtl_pci_switch_clk_req(hw, 0x0);
+	}
+
+	if (1) {
+		/*for promising device will in L0 state after an I/O. */
+		u8 tmp_u1b;
+		pci_read_config_byte(rtlpci->pdev, 0x80, &tmp_u1b);
+	}
+
+	/*Set corresponding value. */
+	aspmlevel |= BIT(0) | BIT(1);
+	linkctrl_reg &= ~aspmlevel;
+	pcibridge_linkctrlreg &= ~(BIT(0) | BIT(1));
+
+	_rtl_pci_platform_switch_device_pci_aspm(hw, linkctrl_reg);
+	udelay(50);
+
+	/*4 Disable Pci Bridge ASPM */
+	rtl_pci_raw_write_port_ulong(PCI_CONF_ADDRESS,
+				     pcicfg_addrport + (num4bytes << 2));
+	rtl_pci_raw_write_port_uchar(PCI_CONF_DATA, pcibridge_linkctrlreg);
+
+	udelay(50);
+}
+
+/*
+ *Enable RTL8192SE ASPM & Enable Pci Bridge ASPM for
+ *power saving We should follow the sequence to enable
+ *RTL8192SE first then enable Pci Bridge ASPM
+ *or the system will show bluescreen.
+ */
+static void rtl_pci_enable_aspm(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u8 pcibridge_vendor = pcipriv->ndis_adapter.pcibridge_vendor;
+	u32 pcicfg_addrport = pcipriv->ndis_adapter.pcicfg_addrport;
+	u8 num4bytes = pcipriv->ndis_adapter.num4bytes;
+	u16 aspmlevel;
+	u8 u_pcibridge_aspmsetting;
+	u8 u_device_aspmsetting;
+
+	if (!ppsc->b_support_aspm)
+		return;
+
+	if (pcibridge_vendor == PCI_BRIDGE_VENDOR_UNKNOWN) {
+		RT_TRACE(COMP_POWER, DBG_TRACE,
+			 ("PCI(Bridge) UNKNOWN.\n"));
+		return;
+	}
+
+	/*4 Enable Pci Bridge ASPM */
+	rtl_pci_raw_write_port_ulong(PCI_CONF_ADDRESS,
+				     pcicfg_addrport + (num4bytes << 2));
+
+	u_pcibridge_aspmsetting =
+	    pcipriv->ndis_adapter.pcibridge_linkctrlreg |
+	    rtlpci->const_hostpci_aspm_setting;
+
+	if (pcibridge_vendor == PCI_BRIDGE_VENDOR_INTEL)
+		u_pcibridge_aspmsetting &= ~BIT(0);
+
+	rtl_pci_raw_write_port_uchar(PCI_CONF_DATA, u_pcibridge_aspmsetting);
+
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("PlatformEnableASPM(): Write reg[%x] = %x\n",
+		  (pcipriv->ndis_adapter.pcibridge_pciehdr_offset + 0x10),
+		  u_pcibridge_aspmsetting));
+
+	udelay(50);
+
+	/*Get ASPM level (with/without Clock Req) */
+	aspmlevel = rtlpci->const_devicepci_aspm_setting;
+	u_device_aspmsetting = pcipriv->ndis_adapter.linkctrl_reg;
+
+	/*_rtl_pci_platform_switch_device_pci_aspm(dev,*/
+	/*(priv->ndis_adapter.linkctrl_reg | ASPMLevel)); */
+
+	u_device_aspmsetting |= aspmlevel;
+
+	_rtl_pci_platform_switch_device_pci_aspm(hw, u_device_aspmsetting);
+
+	if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_CLK_REQ) {
+		_rtl_pci_switch_clk_req(hw, (ppsc->reg_rfps_level &
+					     RT_RF_OFF_LEVL_CLK_REQ) ? 1 : 0);
+		RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_CLK_REQ);
+	}
+	udelay(100);
+}
+
+static bool rtl_pci_get_amd_l1_patch(struct ieee80211_hw *hw)
+{
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	u32 pcicfg_addrport = pcipriv->ndis_adapter.pcicfg_addrport;
+
+	bool status = false;
+	u8 offset_e0;
+	unsigned offset_e4;
+
+	rtl_pci_raw_write_port_ulong(PCI_CONF_ADDRESS, pcicfg_addrport + 0xE0);
+	rtl_pci_raw_write_port_uchar(PCI_CONF_DATA, 0xA0);
+
+	rtl_pci_raw_write_port_ulong(PCI_CONF_ADDRESS, pcicfg_addrport + 0xE0);
+	rtl_pci_raw_read_port_uchar(PCI_CONF_DATA, &offset_e0);
+
+	if (offset_e0 == 0xA0) {
+		rtl_pci_raw_write_port_ulong(PCI_CONF_ADDRESS,
+					     pcicfg_addrport + 0xE4);
+		rtl_pci_raw_read_port_ulong(PCI_CONF_DATA, &offset_e4);
+		if (offset_e4 & BIT(23))
+			status = true;
+	}
+
+	return status;
+}
+
+static bool rtl_pci_check_buddy_priv(struct ieee80211_hw *hw,
+				     struct rtl_priv **buddy_priv)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	bool find_buddy_priv = false;
+	struct rtl_priv *tpriv = NULL;
+	struct rtl_pci_priv *tpcipriv = NULL;
+
+	if (!list_empty(&rtlpriv->glb_var->glb_priv_list)) {
+		list_for_each_entry(tpriv, &rtlpriv->glb_var->glb_priv_list,
+				    list) {
+			if (tpriv == NULL)
+				break;
+
+			tpcipriv = (struct rtl_pci_priv *)tpriv->priv;
+			RT_TRACE(COMP_INIT, DBG_LOUD,
+				 ("pcipriv->ndis_adapter.funcnumber %x\n",
+				  pcipriv->ndis_adapter.funcnumber));
+			RT_TRACE(COMP_INIT, DBG_LOUD,
+				 ("tpcipriv->ndis_adapter.funcnumber %x\n",
+				  tpcipriv->ndis_adapter.funcnumber));
+
+			if ((pcipriv->ndis_adapter.busnumber ==
+			     tpcipriv->ndis_adapter.busnumber) &&
+			    (pcipriv->ndis_adapter.devnumber ==
+			     tpcipriv->ndis_adapter.devnumber) &&
+			    (pcipriv->ndis_adapter.funcnumber !=
+			     tpcipriv->ndis_adapter.funcnumber)) {
+				find_buddy_priv = true;
+				break;
+			}
+		}
+	}
+
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("find_buddy_priv %d\n", find_buddy_priv));
+
+	if (find_buddy_priv)
+		*buddy_priv = tpriv;
+
+	return find_buddy_priv;
+}
+
+static void rtl_pci_get_linkcontrol_field(struct ieee80211_hw *hw)
+{
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	u8 capabilityoffset = pcipriv->ndis_adapter.pcibridge_pciehdr_offset;
+	u32 pcicfg_addrport = pcipriv->ndis_adapter.pcicfg_addrport;
+	u8 linkctrl_reg;
+	u8 num4bbytes;
+
+	num4bbytes = (capabilityoffset + 0x10) / 4;
+
+	/*Read  Link Control Register */
+	rtl_pci_raw_write_port_ulong(PCI_CONF_ADDRESS,
+				     pcicfg_addrport + (num4bbytes << 2));
+	rtl_pci_raw_read_port_uchar(PCI_CONF_DATA, &linkctrl_reg);
+
+	pcipriv->ndis_adapter.pcibridge_linkctrlreg = linkctrl_reg;
+}
+
+static void rtl_pci_parse_configuration(struct pci_dev *pdev,
+					struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+
+	u8 tmp;
+	int pos;
+	u8 linkctrl_reg;
+
+	/*Link Control Register */
+	pos = pci_find_capability(pdev, PCI_CAP_ID_EXP);
+	pci_read_config_byte(pdev, pos + PCI_EXP_LNKCTL, &linkctrl_reg);
+	pcipriv->ndis_adapter.linkctrl_reg = linkctrl_reg;
+
+	RT_TRACE(COMP_INIT, DBG_TRACE,
+		 ("Link Control Register =%x\n",
+		  pcipriv->ndis_adapter.linkctrl_reg));
+
+	pci_read_config_byte(pdev, 0x98, &tmp);
+	tmp |= BIT(4);
+	pci_write_config_byte(pdev, 0x98, tmp);
+
+	tmp = 0x17;
+	pci_write_config_byte(pdev, 0x70f, tmp);
+}
+
+static void rtl_pci_init_aspm(struct ieee80211_hw *hw)
+{
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+
+	_rtl_pci_update_default_setting(hw);
+
+	if (ppsc->reg_rfps_level & RT_RF_PS_LEVEL_ALWAYS_ASPM) {
+		/*Always enable ASPM & Clock Req. */
+		rtl_pci_enable_aspm(hw);
+		RT_SET_PS_LEVEL(ppsc, RT_RF_PS_LEVEL_ALWAYS_ASPM);
+	}
+}
+
+static void _rtl_pci_io_handler_init(struct device *dev,
+				     struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->io.dev = dev;
+
+	rtlpriv->io.write8_async = pci_write8_async;
+	rtlpriv->io.write16_async = pci_write16_async;
+	rtlpriv->io.write32_async = pci_write32_async;
+
+	rtlpriv->io.read8_sync = pci_read8_sync;
+	rtlpriv->io.read16_sync = pci_read16_sync;
+	rtlpriv->io.read32_sync = pci_read32_sync;
+}
+
+static bool _rtl_pci_update_earlymode_info(struct ieee80211_hw *hw,
+					   struct sk_buff *skb,
+					   struct rtl_tcb_desc *tcb_desc,
+					   u8 tid)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct sk_buff *next_skb;
+	u8 additionlen = FCS_LEN;
+
+	/* here open is 4, wep/tkip is 8, aes is 12*/
+	if (info->control.hw_key)
+		additionlen += info->control.hw_key->icv_len;
+
+	/* The most skb num is 6 */
+	tcb_desc->empkt_num = 0;
+	spin_lock_bh(&rtlpriv->locks.waitq_lock);
+	skb_queue_walk(&rtlpriv->mac80211.skb_waitq[tid], next_skb) {
+		struct ieee80211_tx_info *next_info;
+
+		next_info = IEEE80211_SKB_CB(next_skb);
+		if (next_info->flags & IEEE80211_TX_CTL_AMPDU) {
+			tcb_desc->empkt_len[tcb_desc->empkt_num] =
+				next_skb->len + additionlen;
+			tcb_desc->empkt_num++;
+		} else {
+			break;
+		}
+
+		if (skb_queue_is_last(&rtlpriv->mac80211.skb_waitq[tid],
+				      next_skb))
+			break;
+
+		if (tcb_desc->empkt_num >= rtlhal->max_earlymode_num)
+			break;
+	}
+	spin_unlock_bh(&rtlpriv->locks.waitq_lock);
+	return true;
+}
+
+/* just for early mode now */
+static void _rtl_pci_tx_chk_waitq(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct sk_buff *skb = NULL;
+	struct ieee80211_tx_info *info = NULL;
+	int tid; /* should be int */
+
+	if (!rtlpriv->rtlhal.b_earlymode_enable)
+		return;
+	if (rtlpriv->dm.supp_phymode_switch &&
+	    (rtlpriv->easy_concurrent_ctl.bswitch_in_process ||
+	    (rtlpriv->buddy_priv &&
+	     rtlpriv->buddy_priv->easy_concurrent_ctl.bswitch_in_process)))
+		return;
+	/* we juse use em for BE/BK/VI/VO */
+	for (tid = 7; tid >= 0; tid--) {
+		u8 hw_queue = ac_to_hwq[rtl92e_tid_to_ac(hw, tid)];
+		struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[hw_queue];
+		while (!mac->act_scanning &&
+		       rtlpriv->psc.rfpwr_state == ERFON) {
+			struct rtl_tcb_desc tcb_desc;
+			memset(&tcb_desc, 0, sizeof(struct rtl_tcb_desc));
+
+			spin_lock_bh(&rtlpriv->locks.waitq_lock);
+			if (!skb_queue_empty(&mac->skb_waitq[tid]) &&
+			    (ring->entries - skb_queue_len(&ring->queue) >
+			     rtlhal->max_earlymode_num)) {
+				skb = skb_dequeue(&mac->skb_waitq[tid]);
+			} else {
+				spin_unlock_bh(&rtlpriv->locks.waitq_lock);
+				break;
+			}
+			spin_unlock_bh(&rtlpriv->locks.waitq_lock);
+
+			/* Some macaddr can't do early mode. like
+			 * multicast/broadcast/no_qos data */
+			info = IEEE80211_SKB_CB(skb);
+			if (info->flags & IEEE80211_TX_CTL_AMPDU)
+				_rtl_pci_update_earlymode_info(hw, skb,
+							       &tcb_desc, tid);
+
+			rtlpriv->intf_ops->adapter_tx(hw, NULL, skb, &tcb_desc);
+		}
+	}
+}
+
+static void _rtl_pci_tx_isr(struct ieee80211_hw *hw, int prio)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[prio];
+
+	while (skb_queue_len(&ring->queue)) {
+		struct sk_buff *skb;
+		struct ieee80211_tx_info *info;
+		__le16 fc;
+		u8 tid;
+		u8 *entry;
+
+
+		if (rtlpriv->use_new_trx_flow)
+			entry = (u8 *)(&ring->buffer_desc[ring->idx]);
+		else
+			entry = (u8 *)(&ring->desc[ring->idx]);
+
+		if (rtlpriv->cfg->ops->is_tx_desc_closed &&
+		    !rtlpriv->cfg->ops->is_tx_desc_closed(hw, prio, ring->idx))
+			return;
+
+		ring->idx = (ring->idx + 1) % ring->entries;
+
+		skb = __skb_dequeue(&ring->queue);
+
+		pci_unmap_single(rtlpci->pdev,
+				 rtlpriv->cfg->ops->
+					     get_desc((u8 *)entry, true,
+						      HW_DESC_TXBUFF_ADDR),
+				 skb->len, PCI_DMA_TODEVICE);
+
+		/* remove early mode header */
+		if (rtlpriv->rtlhal.b_earlymode_enable)
+			skb_pull(skb, EM_HDR_LEN);
+
+		RT_TRACE((COMP_INTR | COMP_SEND), DBG_TRACE,
+			 ("new ring->idx:%d, free: skb_queue_len:%d, free: seq:%d\n",
+			  ring->idx,
+			  skb_queue_len(&ring->queue),
+			  *(u16 *)(skb->data + 22)));
+
+		if (prio == TXCMD_QUEUE) {
+			dev_kfree_skb(skb);
+			goto tx_status_ok;
+		}
+
+		/* for sw LPS, just after NULL skb send out, we can
+		 * sure AP knows that we are sleeping, our we should not let
+		 * rf to sleep
+		 */
+		fc = rtl_get_fc(skb);
+		if (ieee80211_is_nullfunc(fc)) {
+			if (ieee80211_has_pm(fc)) {
+				rtlpriv->mac80211.offchan_deley = true;
+				rtlpriv->psc.state_inap = 1;
+			} else {
+				rtlpriv->psc.state_inap = 0;
+			}
+		}
+		if (ieee80211_is_action(fc)) {
+			struct ieee80211_mgmt_compat *action_frame =
+				(struct ieee80211_mgmt_compat *)skb->data;
+			if (action_frame->u.action.u.ht_smps.action ==
+				WLAN_HT_ACTION_SMPS) {
+				dev_kfree_skb(skb);
+				goto tx_status_ok;
+			}
+		}
+
+		/* update tid tx pkt num */
+		tid = rtl_get_tid(skb);
+		if (tid <= 7)
+			rtlpriv->link_info.tidtx_inperiod[tid]++;
+
+		info = IEEE80211_SKB_CB(skb);
+		ieee80211_tx_info_clear_status(info);
+
+		info->flags |= IEEE80211_TX_STAT_ACK;
+		/*info->status.rates[0].count = 1; */
+
+		ieee80211_tx_status_irqsafe(hw, skb);
+
+		if ((ring->entries - skb_queue_len(&ring->queue)) == 2) {
+			RT_TRACE(COMP_ERR, DBG_LOUD,
+				 ("more desc left, wake skb_queue@%d,ring->idx = %d, skb_queue_len = 0x%d\n",
+					 prio, ring->idx,
+					 skb_queue_len(&ring->queue)));
+
+			ieee80211_wake_queue(hw, skb_get_queue_mapping
+					     (skb));
+		}
+tx_status_ok:
+		skb = NULL;
+	}
+
+	if (((rtlpriv->link_info.num_rx_inperiod +
+		rtlpriv->link_info.num_tx_inperiod) > 8) ||
+		(rtlpriv->link_info.num_rx_inperiod > 2)) {
+		rtl92e_lps_leave(hw);
+	}
+}
+
+static int _rtl_pci_init_one_rxdesc(struct ieee80211_hw *hw,
+				    u8 *entry, int rxring_idx, int desc_idx)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct sk_buff *skb;
+	u32 bufferaddress;
+	u8 tmp_one = 1;
+
+	skb = dev_alloc_skb(rtlpci->rxbuffersize);
+	if (!skb)
+		return 0;
+	rtlpci->rx_ring[rxring_idx].rx_buf[desc_idx] = skb;
+
+	/* just set skb->cb to mapping addr
+	 * for pci_unmap_single use
+	 */
+	*((dma_addr_t *)skb->cb) = pci_map_single(rtlpci->pdev,
+				skb_tail_pointer(skb), rtlpci->rxbuffersize,
+				PCI_DMA_FROMDEVICE);
+	bufferaddress = *((dma_addr_t *)skb->cb);
+	if (pci_dma_mapping_error(rtlpci->pdev, bufferaddress))
+		return 0;
+	if (rtlpriv->use_new_trx_flow) {
+		rtlpriv->cfg->ops->set_desc(hw, (u8 *)entry, false,
+					    HW_DESC_RX_PREPARE,
+					    (u8 *)&bufferaddress);
+	} else {
+		rtlpriv->cfg->ops->set_desc(hw, (u8 *)entry, false,
+					    HW_DESC_RXBUFF_ADDR,
+					    (u8 *)&bufferaddress);
+		rtlpriv->cfg->ops->set_desc(hw, (u8 *)entry, false,
+					    HW_DESC_RXPKT_LEN,
+					    (u8 *)&rtlpci->rxbuffersize);
+		rtlpriv->cfg->ops->set_desc(hw, (u8 *)entry, false,
+					    HW_DESC_RXOWN,
+					    (u8 *)&tmp_one);
+	}
+	return 1;
+}
+
+/* inorder to receive 8K AMSDU we have set skb to
+ * 9100bytes in init rx ring, but if this packet is
+ * not a AMSDU, this so big packet will be sent to
+ * TCP/IP directly, this cause big packet ping fail
+ * like: "ping -s 65507", so here we will realloc skb
+ * based on the true size of packet, I think mac80211
+ * do it will be better, but now mac80211 haven't */
+
+/* but some platform will fail when alloc skb sometimes.
+ * in this condition, we will send the old skb to
+ * mac80211 directly, this will not cause any other
+ * issues, but only be losted by TCP/IP */
+static void _rtl_pci_rx_to_mac80211(struct ieee80211_hw *hw,
+				    struct sk_buff *skb,
+				    struct ieee80211_rx_status rx_status)
+{
+	if (unlikely(!rtl92e_action_proc(hw, skb, false))) {
+		dev_kfree_skb_any(skb);
+	} else {
+		struct sk_buff *uskb = NULL;
+		u8 *pdata;
+
+		uskb = dev_alloc_skb(skb->len + 128);
+		if (likely(uskb)) {
+			memcpy(IEEE80211_SKB_RXCB(uskb), &rx_status,
+			       sizeof(rx_status));
+			pdata = (u8 *)skb_put(uskb, skb->len);
+			memcpy(pdata, skb->data, skb->len);
+			dev_kfree_skb_any(skb);
+
+			ieee80211_rx_irqsafe(hw, uskb);
+		} else {
+			ieee80211_rx_irqsafe(hw, skb);
+		}
+	}
+}
+
+/*hsisr interrupt handler*/
+static void _rtl_pci_hs_interrupt(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[MAC_HSISR],
+		       rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[MAC_HSISR]) |
+		       rtlpci->sys_irq_mask);
+}
+
+static void _rtl_receive_one(struct ieee80211_hw *hw, struct sk_buff *skb,
+			     struct ieee80211_rx_status rx_status)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct ieee80211_hdr *hdr = rtl_get_hdr(skb);
+	__le16 fc = rtl_get_fc(skb);
+	bool unicast = false;
+
+	memcpy(IEEE80211_SKB_RXCB(skb), &rx_status, sizeof(rx_status));
+
+	if (is_broadcast_ether_addr(hdr->addr1)) {
+		;/*TODO*/
+	} else if (is_multicast_ether_addr(hdr->addr1)) {
+		;/*TODO*/
+	} else {
+		unicast = true;
+		rtlpriv->stats.rxbytesunicast += skb->len;
+	}
+
+	rtl92e_is_special_data(hw, skb, false);
+	if (ieee80211_is_data(fc)) {
+		rtlpriv->cfg->ops->led_control(hw, LED_CTL_RX);
+
+		if (unicast)
+			rtlpriv->link_info.num_rx_inperiod++;
+	}
+
+	/* static bcn for roaming */
+	rtl92e_beacon_statistic(hw, skb);
+	rtl92e_p2p_info(hw, (void *)skb->data, skb->len);
+
+	/* for sw lps */
+	rtl92e_swlps_beacon(hw, (void *)skb->data, skb->len);
+	rtl92e_recognize_peer(hw, (void *)skb->data, skb->len);
+	if ((rtlpriv->mac80211.opmode == NL80211_IFTYPE_AP) &&
+	    (rtlpriv->rtlhal.current_bandtype == BAND_ON_2_4G) &&
+	    (ieee80211_is_beacon(fc) ||
+	     ieee80211_is_probe_resp(fc)))
+		dev_kfree_skb_any(skb);
+	else
+		_rtl_pci_rx_to_mac80211(hw, skb, rx_status);
+}
+
+static void _rtl_pci_rx_interrupt(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct ieee80211_rx_status rx_status = { 0 };
+	int rxring_idx = RTL_PCI_RX_MPDU_QUEUE;
+	unsigned int count = rtlpci->rxringcount;
+	u8 hw_queue = 0;
+	unsigned int rx_remained_cnt;
+	u8 own;
+	u8 tmp_one;
+	static int err_count;
+	struct rtl_stats stats = {
+		.signal = 0,
+		.rate = 0,
+	};
+
+	/*RX NORMAL PKT */
+	while (count--) {
+		struct ieee80211_hdr *hdr;
+		__le16 fc;
+		u16 len;
+		/*rx buffer descriptor */
+		struct rtl_rx_buffer_desc *buffer_desc = NULL;
+		/*if use new trx flow, it means wifi info */
+		struct rtl_rx_desc *pdesc = NULL;
+		/*rx pkt */
+		struct sk_buff *skb = rtlpci->rx_ring[rxring_idx].rx_buf[
+					rtlpci->rx_ring[rxring_idx].idx];
+
+		if (rtlpriv->use_new_trx_flow) {
+			rx_remained_cnt =
+				rtlpriv->cfg->ops->rx_desc_buff_remained_cnt(hw,
+								      hw_queue);
+			if (rx_remained_cnt < 1)
+				return;
+
+		} else {	/* rx descriptor */
+			pdesc = &rtlpci->rx_ring[rxring_idx].desc[
+				rtlpci->rx_ring[rxring_idx].idx];
+
+			own = (u8) rtlpriv->cfg->ops->get_desc((u8 *)pdesc,
+							       false,
+							       HW_DESC_OWN);
+			if (own) /* wait data to be filled by hardware */
+				return;
+		}
+
+		/* If we get here, the data is filled already
+		 * Attention !!!
+		 * We can NOT access 'skb' before 'pci_unmap_single'
+		 */
+		pci_unmap_single(rtlpci->pdev, *((dma_addr_t *)skb->cb),
+				 rtlpci->rxbuffersize, PCI_DMA_FROMDEVICE);
+
+		if (rtlpriv->use_new_trx_flow) {
+			buffer_desc = &rtlpci->rx_ring[rxring_idx].buffer_desc[
+				rtlpci->rx_ring[rxring_idx].idx];
+			/*means rx wifi info*/
+			pdesc = (struct rtl_rx_desc *)skb->data;
+		}
+		memset(&rx_status , 0 , sizeof(rx_status));
+		rtlpriv->cfg->ops->query_rx_desc(hw, &stats,
+						 &rx_status, (u8 *)pdesc, skb);
+
+		if (rtlpriv->use_new_trx_flow)
+			rtlpriv->cfg->ops->rx_check_dma_ok(hw,
+							   (u8 *)buffer_desc,
+							   hw_queue);
+		len = rtlpriv->cfg->ops->get_desc((u8 *)pdesc, false,
+						  HW_DESC_RXPKT_LEN);
+
+		if (skb->end - skb->tail > len) {
+			skb_put(skb, len);
+			if (rtlpriv->use_new_trx_flow)
+				skb_reserve(skb, stats.rx_drvinfo_size +
+						 stats.rx_bufshift + 24);
+			else
+				skb_reserve(skb, stats.rx_drvinfo_size +
+						 stats.rx_bufshift);
+
+		} else {
+			if (err_count++ < 10) {
+				pr_info("skb->end (%d) - skb->tail (%d) > len (%d)\n",
+					skb->end, skb->tail, len);
+				RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_EMERG,
+					      "RX desc\n",
+					      (u8 *)pdesc, 32);
+			}
+			break;
+		}
+
+		/* handle command packet here */
+		if (rtlpriv->cfg->ops->rx_command_packet(hw, stats, skb)) {
+				dev_kfree_skb_any(skb);
+				goto end;
+		}
+
+		/* NOTICE This can not be use for mac80211,
+		 *this is done in mac80211 code,
+		 *if you done here sec DHCP will fail
+		 *skb_trim(skb, skb->len - 4);
+		 */
+
+		hdr = rtl_get_hdr(skb);
+		fc = rtl_get_fc(skb);
+
+		if (!stats.b_crc && !stats.b_hwerror)
+			_rtl_receive_one(hw, skb, rx_status);
+		else
+			dev_kfree_skb_any(skb);
+		if (rtlpriv->use_new_trx_flow) {
+			rtlpci->rx_ring[hw_queue].next_rx_rp += 1;
+			rtlpci->rx_ring[hw_queue].next_rx_rp %=
+							RTL_PCI_MAX_RX_COUNT;
+
+
+			rx_remained_cnt--;
+			rtl_write_word(rtlpriv, 0x3B4,
+				       rtlpci->rx_ring[hw_queue].next_rx_rp);
+		}
+		if (((rtlpriv->link_info.num_rx_inperiod +
+		      rtlpriv->link_info.num_tx_inperiod) > 8) ||
+		    (rtlpriv->link_info.num_rx_inperiod > 2)) {
+			rtl92e_lps_leave(hw);
+		}
+end:
+		if (rtlpriv->use_new_trx_flow) {
+			_rtl_pci_init_one_rxdesc(hw, (u8 *)buffer_desc,
+						 rxring_idx,
+					       rtlpci->rx_ring[rxring_idx].idx);
+		} else {
+			_rtl_pci_init_one_rxdesc(hw, (u8 *)pdesc, rxring_idx,
+						 rtlpci->rx_ring[rxring_idx].idx);
+
+			if (rtlpci->rx_ring[rxring_idx].idx ==
+			    rtlpci->rxringcount - 1)
+				rtlpriv->cfg->ops->set_desc(hw, (u8 *)pdesc,
+							    false,
+							    HW_DESC_RXERO,
+							    (u8 *)&tmp_one);
+		}
+		rtlpci->rx_ring[rxring_idx].idx =
+				(rtlpci->rx_ring[rxring_idx].idx + 1) %
+				rtlpci->rxringcount;
+	}
+}
+
+static irqreturn_t _rtl_pci_interrupt(int irq, void *dev_id)
+{
+	struct ieee80211_hw *hw = dev_id;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	unsigned long flags;
+	u32 inta = 0;
+	u32 intb = 0;
+
+	if (rtlpci->irq_enabled == 0)
+		return IRQ_HANDLED;
+
+	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock , flags);
+	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[MAC_HIMR], 0x0);
+	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[MAC_HIMRE], 0x0);
+
+	/*read ISR: 4/8bytes */
+	rtlpriv->cfg->ops->interrupt_recognized(hw, &inta, &intb);
+
+	/*Shared IRQ or HW disappared */
+	if (!inta || inta == 0xffff)
+		goto done;
+	/*<1> beacon related */
+	if (inta & rtlpriv->cfg->maps[RTL_IMR_TBDOK])
+		RT_TRACE(COMP_INTR, DBG_TRACE, ("beacon ok interrupt!\n"));
+
+	if (unlikely(inta & rtlpriv->cfg->maps[RTL_IMR_TBDER]))
+		RT_TRACE(COMP_INTR, DBG_TRACE, ("beacon err interrupt!\n"));
+
+	if (inta & rtlpriv->cfg->maps[RTL_IMR_BDOK])
+		RT_TRACE(COMP_INTR, DBG_TRACE, ("beacon interrupt!\n"));
+
+	if (inta & rtlpriv->cfg->maps[RTL_IMR_BcnInt]) {
+		RT_TRACE(COMP_INTR, DBG_TRACE,
+			 ("prepare beacon for interrupt!\n"));
+		tasklet_schedule(&rtlpriv->works.irq_prepare_bcn_tasklet);
+	}
+
+	/*<2> tx related */
+	if (unlikely(intb & rtlpriv->cfg->maps[RTL_IMR_TXFOVW]))
+		RT_TRACE(COMP_ERR, DBG_TRACE, ("IMR_TXFOVW!\n"));
+
+	if (inta & rtlpriv->cfg->maps[RTL_IMR_MGNTDOK]) {
+		RT_TRACE(COMP_INTR, DBG_TRACE, ("Manage ok interrupt!\n"));
+		_rtl_pci_tx_isr(hw, MGNT_QUEUE);
+	}
+
+	if (inta & rtlpriv->cfg->maps[RTL_IMR_HIGHDOK]) {
+		RT_TRACE(COMP_INTR, DBG_TRACE, ("HIGH_QUEUE ok interrupt!\n"));
+		_rtl_pci_tx_isr(hw, HIGH_QUEUE);
+	}
+
+	if (inta & rtlpriv->cfg->maps[RTL_IMR_BKDOK]) {
+		rtlpriv->link_info.num_tx_inperiod++;
+
+		RT_TRACE(COMP_INTR, DBG_TRACE, ("BK Tx OK interrupt!\n"));
+		_rtl_pci_tx_isr(hw, BK_QUEUE);
+	}
+
+	if (inta & rtlpriv->cfg->maps[RTL_IMR_BEDOK]) {
+		rtlpriv->link_info.num_tx_inperiod++;
+
+		RT_TRACE(COMP_INTR, DBG_TRACE, ("BE TX OK interrupt!\n"));
+		_rtl_pci_tx_isr(hw, BE_QUEUE);
+	}
+
+	if (inta & rtlpriv->cfg->maps[RTL_IMR_VIDOK]) {
+		rtlpriv->link_info.num_tx_inperiod++;
+
+		RT_TRACE(COMP_INTR, DBG_TRACE, ("VI TX OK interrupt!\n"));
+		_rtl_pci_tx_isr(hw, VI_QUEUE);
+	}
+
+	if (inta & rtlpriv->cfg->maps[RTL_IMR_VODOK]) {
+		rtlpriv->link_info.num_tx_inperiod++;
+
+		RT_TRACE(COMP_INTR, DBG_TRACE, ("Vo TX OK interrupt!\n"));
+		_rtl_pci_tx_isr(hw, VO_QUEUE);
+	}
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192SE) {
+		if (inta & rtlpriv->cfg->maps[RTL_IMR_COMDOK]) {
+			rtlpriv->link_info.num_tx_inperiod++;
+
+			RT_TRACE(COMP_INTR, DBG_TRACE,
+				 ("CMD TX OK interrupt!\n"));
+			_rtl_pci_tx_isr(hw, TXCMD_QUEUE);
+		}
+	}
+
+	/*<3> rx related */
+	if (inta & rtlpriv->cfg->maps[RTL_IMR_ROK]) {
+		RT_TRACE(COMP_INTR, DBG_TRACE, ("Rx ok interrupt!\n"));
+		_rtl_pci_rx_interrupt(hw);
+	}
+
+	if (unlikely(inta & rtlpriv->cfg->maps[RTL_IMR_RDU])) {
+		RT_TRACE(COMP_ERR, DBG_WARNING,
+			 ("rx descriptor unavailable!\n"));
+		_rtl_pci_rx_interrupt(hw);
+	}
+
+	if (unlikely(intb & rtlpriv->cfg->maps[RTL_IMR_RXFOVW])) {
+		RT_TRACE(COMP_ERR, DBG_WARNING, ("rx overflow !\n"));
+		_rtl_pci_rx_interrupt(hw);
+	}
+
+	/*<4> fw related*/
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723AE) {
+		if (inta & rtlpriv->cfg->maps[RTL_IMR_C2HCMD]) {
+			RT_TRACE(COMP_INTR, DBG_TRACE,
+				 ("firmware interrupt!\n"));
+			queue_delayed_work(rtlpriv->works.rtl_wq,
+					   &rtlpriv->works.fwevt_wq, 0);
+		}
+	}
+
+	/*<5> hsisr related*/
+	/* Only 8188EE & 8723BE Supported.
+	 * If Other ICs Come in, System will corrupt,
+	 * because maps[RTL_IMR_HSISR_IND] & maps[MAC_HSISR]
+	 * are not initialized*/
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8188EE ||
+	    rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
+		if (unlikely(inta & rtlpriv->cfg->maps[RTL_IMR_HSISR_IND])) {
+			RT_TRACE(COMP_INTR, DBG_TRACE,
+				 ("hsisr interrupt!\n"));
+			_rtl_pci_hs_interrupt(hw);
+		}
+	}
+
+
+	if (rtlpriv->rtlhal.b_earlymode_enable)
+		tasklet_schedule(&rtlpriv->works.irq_tasklet);
+
+done:
+	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[MAC_HIMR],
+			rtlpci->irq_mask[0]);
+	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[MAC_HIMRE],
+			rtlpci->irq_mask[1]);
+	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
+	return IRQ_HANDLED;
+}
+
+static void _rtl_pci_irq_tasklet(struct ieee80211_hw *hw)
+{
+	_rtl_pci_tx_chk_waitq(hw);
+}
+
+static void _rtl_pci_prepare_bcn_tasklet(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl8192_tx_ring *ring = NULL;
+	struct ieee80211_hdr *hdr = NULL;
+	struct ieee80211_tx_info *info = NULL;
+	struct sk_buff *pskb = NULL;
+	struct rtl_tx_desc *pdesc = NULL;
+	struct rtl_tcb_desc tcb_desc;
+	/*This is for new trx flow*/
+	struct rtl_tx_buffer_desc *pbuffer_desc = NULL;
+	u8 temp_one = 1;
+
+	memset(&tcb_desc, 0, sizeof(struct rtl_tcb_desc));
+	ring = &rtlpci->tx_ring[BEACON_QUEUE];
+	pskb = __skb_dequeue(&ring->queue);
+	if (pskb)
+		kfree_skb(pskb);
+
+	/*NB: the beacon data buffer must be 32-bit aligned. */
+	pskb = ieee80211_beacon_get(hw, mac->vif);
+	if (pskb == NULL)
+		return;
+	hdr = rtl_get_hdr(pskb);
+	info = IEEE80211_SKB_CB(pskb);
+	pdesc = &ring->desc[0];
+	if (rtlpriv->use_new_trx_flow)
+		pbuffer_desc = &ring->buffer_desc[0];
+
+	rtlpriv->cfg->ops->fill_tx_desc(hw, hdr, (u8 *)pdesc,
+					(u8 *)pbuffer_desc, info, NULL, pskb,
+					BEACON_QUEUE, &tcb_desc);
+
+	__skb_queue_tail(&ring->queue, pskb);
+
+	rtlpriv->cfg->ops->set_desc(hw, (u8 *)pdesc, true, HW_DESC_OWN,
+				    (u8 *)&temp_one);
+
+	return;
+}
+
+static void _rtl_pci_init_trx_var(struct ieee80211_hw *hw)
+{
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	u8 i;
+	u16 desc_num;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE)
+		desc_num = TX_DESC_NUM_92E;
+	else
+		desc_num = RT_TXDESC_NUM;
+
+	for (i = 0; i < RTL_PCI_MAX_TX_QUEUE_COUNT; i++)
+		rtlpci->txringcount[i] = desc_num;
+	/*
+	 *we just alloc 2 desc for beacon queue,
+	 *because we just need first desc in hw beacon.
+	 */
+	rtlpci->txringcount[BEACON_QUEUE] = 2;
+
+	/*
+	 *BE queue need more descriptor for performance
+	 *consideration or, No more tx desc will happen,
+	 *and may cause mac80211 mem leakage.
+	 */
+	if (!rtl_priv(hw)->use_new_trx_flow)
+		rtlpci->txringcount[BE_QUEUE] = RT_TXDESC_NUM_BE_QUEUE;
+
+	rtlpci->rxbuffersize = 9100;	/*2048/1024; */
+	rtlpci->rxringcount = RTL_PCI_MAX_RX_COUNT;	/*64; */
+}
+
+static void _rtl_pci_init_struct(struct ieee80211_hw *hw,
+				 struct pci_dev *pdev)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	rtlpriv->rtlhal.up_first_time = true;
+	rtlpriv->rtlhal.being_init_adapter = false;
+
+	rtlhal->hw = hw;
+	rtlpci->pdev = pdev;
+
+	/*Tx/Rx related var */
+	_rtl_pci_init_trx_var(hw);
+
+	/*IBSS*/ mac->beacon_interval = 100;
+
+	/*AMPDU*/
+	mac->min_space_cfg = 0;
+	mac->max_mss_density = 0;
+	/*set sane AMPDU defaults */
+	mac->current_ampdu_density = 7;
+	mac->current_ampdu_factor = 3;
+
+	/*QOS*/
+	rtlpci->acm_method = eAcmWay2_SW;
+
+	/*task */
+	tasklet_init(&rtlpriv->works.irq_tasklet,
+		     (void (*)(unsigned long))_rtl_pci_irq_tasklet,
+		     (unsigned long)hw);
+	tasklet_init(&rtlpriv->works.irq_prepare_bcn_tasklet,
+		     (void (*)(unsigned long))_rtl_pci_prepare_bcn_tasklet,
+		     (unsigned long)hw);
+}
+
+static int _rtl_pci_init_tx_ring(struct ieee80211_hw *hw,
+				 unsigned int prio, unsigned int entries)
+{
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_tx_buffer_desc *buffer_desc;
+	struct rtl_tx_desc *desc;
+	dma_addr_t buffer_desc_dma, desc_dma;
+	u32 nextdescaddress;
+	int i;
+
+	/* alloc tx buffer desc for new trx flow*/
+	if (rtlpriv->use_new_trx_flow) {
+		buffer_desc = pci_alloc_consistent(rtlpci->pdev,
+					sizeof(*buffer_desc) * entries,
+					&buffer_desc_dma);
+
+		if (!buffer_desc || (unsigned long)buffer_desc & 0xFF) {
+			RT_TRACE(COMP_ERR, DBG_EMERG,
+				 ("Cannot allocate TX ring (prio = %d)\n",
+				 prio));
+			return -ENOMEM;
+		}
+
+		memset(buffer_desc, 0, sizeof(*buffer_desc) * entries);
+		rtlpci->tx_ring[prio].buffer_desc = buffer_desc;
+		rtlpci->tx_ring[prio].buffer_desc_dma = buffer_desc_dma;
+
+		rtlpci->tx_ring[prio].cur_tx_rp = 0;
+		rtlpci->tx_ring[prio].cur_tx_wp = 0;
+		rtlpci->tx_ring[prio].avl_desc = entries;
+	}
+
+	/* alloc dma for this ring */
+	desc = pci_alloc_consistent(rtlpci->pdev,
+				    sizeof(*desc) * entries, &desc_dma);
+
+	if (!desc || (unsigned long)desc & 0xFF) {
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("Cannot allocate TX ring (prio = %d)\n", prio));
+		return -ENOMEM;
+	}
+
+	memset(desc, 0, sizeof(*desc) * entries);
+	rtlpci->tx_ring[prio].desc = desc;
+	rtlpci->tx_ring[prio].dma = desc_dma;
+
+	rtlpci->tx_ring[prio].idx = 0;
+	rtlpci->tx_ring[prio].entries = entries;
+	skb_queue_head_init(&rtlpci->tx_ring[prio].queue);
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("queue:%d, ring_addr:%p\n", prio, desc));
+
+	/* init every desc in this ring */
+	if (!rtlpriv->use_new_trx_flow) {
+		for (i = 0; i < entries; i++) {
+			nextdescaddress = (u32) desc_dma +
+						      ((i +	1) % entries) *
+						      sizeof(*desc);
+
+			rtlpriv->cfg->ops->set_desc(hw, (u8 *)&(desc[i]),
+						    true,
+						    HW_DESC_TX_NEXTDESC_ADDR,
+						    (u8 *)&nextdescaddress);
+		}
+	}
+	return 0;
+}
+
+static int _rtl_pci_init_rx_ring(struct ieee80211_hw *hw, int rxring_idx)
+{
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int i;
+
+	if (rtlpriv->use_new_trx_flow) {
+		struct rtl_rx_buffer_desc *entry = NULL;
+		/* alloc dma for this ring */
+		rtlpci->rx_ring[rxring_idx].buffer_desc =
+		    pci_alloc_consistent(rtlpci->pdev,
+					 sizeof(*rtlpci->rx_ring[rxring_idx].
+						buffer_desc) *
+						rtlpci->rxringcount,
+					 &rtlpci->rx_ring[rxring_idx].dma);
+		if (!rtlpci->rx_ring[rxring_idx].buffer_desc ||
+		    (unsigned long)rtlpci->rx_ring[rxring_idx].buffer_desc & 0xFF) {
+			RT_TRACE(COMP_ERR, DBG_EMERG,
+				 ("Cannot allocate RX ring\n"));
+			return -ENOMEM;
+		}
+
+		memset(rtlpci->rx_ring[rxring_idx].buffer_desc, 0,
+		       sizeof(*rtlpci->rx_ring[rxring_idx].buffer_desc) *
+		       rtlpci->rxringcount);
+
+		/* init every desc in this ring */
+		rtlpci->rx_ring[rxring_idx].idx = 0;
+
+		for (i = 0; i < rtlpci->rxringcount; i++) {
+			entry = &rtlpci->rx_ring[rxring_idx].buffer_desc[i];
+			if (!_rtl_pci_init_one_rxdesc(hw, (u8 *)entry,
+						      rxring_idx, i))
+				return -ENOMEM;
+		}
+	} else {
+		struct rtl_rx_desc *entry = NULL;
+		u8 tmp_one = 1;
+		/* alloc dma for this ring */
+		rtlpci->rx_ring[rxring_idx].desc =
+		    pci_alloc_consistent(rtlpci->pdev,
+					 sizeof(*rtlpci->rx_ring[rxring_idx].
+					desc) * rtlpci->rxringcount,
+					 &rtlpci->rx_ring[rxring_idx].dma);
+		if (!rtlpci->rx_ring[rxring_idx].desc ||
+		    (unsigned long)rtlpci->rx_ring[rxring_idx].desc & 0xFF) {
+			RT_TRACE(COMP_ERR, DBG_EMERG,
+				 ("Cannot allocate RX ring\n"));
+			return -ENOMEM;
+		}
+		memset(rtlpci->rx_ring[rxring_idx].desc, 0,
+		       sizeof(*rtlpci->rx_ring[rxring_idx].desc) *
+		       rtlpci->rxringcount);
+
+		/* init every desc in this ring */
+		rtlpci->rx_ring[rxring_idx].idx = 0;
+		for (i = 0; i < rtlpci->rxringcount; i++) {
+			entry = &rtlpci->rx_ring[rxring_idx].desc[i];
+			if (!_rtl_pci_init_one_rxdesc(hw, (u8 *)entry,
+						      rxring_idx, i))
+				return -ENOMEM;
+		}
+		rtlpriv->cfg->ops->set_desc(hw, (u8 *)entry, false,
+					    HW_DESC_RXERO, (u8 *) &tmp_one);
+	}
+	return 0;
+}
+
+static void _rtl_pci_free_tx_ring(struct ieee80211_hw *hw,
+				  unsigned int prio)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[prio];
+
+	/* free every desc in this ring */
+	while (skb_queue_len(&ring->queue)) {
+		struct sk_buff *skb = __skb_dequeue(&ring->queue);
+		u8 *entry;
+
+		if (rtlpriv->use_new_trx_flow)
+			entry = (u8 *)(&ring->buffer_desc[ring->idx]);
+		else
+			entry = (u8 *)(&ring->desc[ring->idx]);
+
+		pci_unmap_single(rtlpci->pdev,
+				 rtlpriv->cfg->ops->get_desc((u8 *)entry, true,
+				 HW_DESC_TXBUFF_ADDR),
+				 skb->len, PCI_DMA_TODEVICE);
+		kfree_skb(skb);
+		ring->idx = (ring->idx + 1) % ring->entries;
+	}
+
+	/* free dma of this ring */
+	pci_free_consistent(rtlpci->pdev,
+			    sizeof(*ring->desc) * ring->entries,
+			    ring->desc, ring->dma);
+	ring->desc = NULL;
+	if (rtlpriv->use_new_trx_flow) {
+		pci_free_consistent(rtlpci->pdev,
+				    sizeof(*ring->buffer_desc) * ring->entries,
+				    ring->buffer_desc, ring->buffer_desc_dma);
+		ring->buffer_desc = NULL;
+	}
+}
+
+static void _rtl_pci_free_rx_ring(struct ieee80211_hw *hw, int rxring_idx)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	int i;
+
+	/* free every desc in this ring */
+	for (i = 0; i < rtlpci->rxringcount; i++) {
+		struct sk_buff *skb = rtlpci->rx_ring[rxring_idx].rx_buf[i];
+
+		if (!skb)
+			continue;
+
+		pci_unmap_single(rtlpci->pdev, *((dma_addr_t *)skb->cb),
+				 rtlpci->rxbuffersize, PCI_DMA_FROMDEVICE);
+		kfree_skb(skb);
+	}
+
+	/* free dma of this ring */
+	if (rtlpriv->use_new_trx_flow) {
+		pci_free_consistent(rtlpci->pdev,
+				    sizeof(*rtlpci->rx_ring[rxring_idx].
+				    buffer_desc) * rtlpci->rxringcount,
+				    rtlpci->rx_ring[rxring_idx].buffer_desc,
+				    rtlpci->rx_ring[rxring_idx].dma);
+		rtlpci->rx_ring[rxring_idx].buffer_desc = NULL;
+	} else {
+		pci_free_consistent(rtlpci->pdev,
+				    sizeof(*rtlpci->rx_ring[rxring_idx].desc) *
+				    rtlpci->rxringcount,
+				    rtlpci->rx_ring[rxring_idx].desc,
+				    rtlpci->rx_ring[rxring_idx].dma);
+		rtlpci->rx_ring[rxring_idx].desc = NULL;
+	}
+}
+
+static int _rtl_pci_init_trx_ring(struct ieee80211_hw *hw)
+{
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	int ret;
+	int i, rxring_idx;
+
+	/* rxring_idx 0:RX_MPDU_QUEUE
+	 * rxring_idx 1:RX_CMD_QUEUE */
+	for (rxring_idx = 0; rxring_idx < RTL_PCI_MAX_RX_QUEUE; rxring_idx++) {
+		ret = _rtl_pci_init_rx_ring(hw, rxring_idx);
+		if (ret)
+			return ret;
+	}
+
+	for (i = 0; i < RTL_PCI_MAX_TX_QUEUE_COUNT; i++) {
+		ret = _rtl_pci_init_tx_ring(hw, i,
+					    rtlpci->txringcount[i]);
+		if (ret)
+			goto err_free_rings;
+	}
+
+	return 0;
+
+err_free_rings:
+	for (rxring_idx = 0; rxring_idx < RTL_PCI_MAX_RX_QUEUE; rxring_idx++)
+		_rtl_pci_free_rx_ring(hw, rxring_idx);
+
+	for (i = 0; i < RTL_PCI_MAX_TX_QUEUE_COUNT; i++)
+		if (rtlpci->tx_ring[i].desc ||
+		    rtlpci->tx_ring[i].buffer_desc)
+			_rtl_pci_free_tx_ring(hw, i);
+
+	return 1;
+}
+
+static int _rtl_pci_deinit_trx_ring(struct ieee80211_hw *hw)
+{
+	u32 i, rxring_idx;
+
+	/*free rx rings */
+	for (rxring_idx = 0; rxring_idx < RTL_PCI_MAX_RX_QUEUE; rxring_idx++)
+		_rtl_pci_free_rx_ring(hw, rxring_idx);
+
+	/*free tx rings */
+	for (i = 0; i < RTL_PCI_MAX_TX_QUEUE_COUNT; i++)
+		_rtl_pci_free_tx_ring(hw, i);
+
+	return 0;
+}
+
+int rtl92e_pci_reset_trx_ring(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	int i, rxring_idx;
+	unsigned long flags;
+	u8 tmp_one = 1;
+	/* rxring_idx 0:RX_MPDU_QUEUE */
+	/* rxring_idx 1:RX_CMD_QUEUE */
+	for (rxring_idx = 0; rxring_idx < RTL_PCI_MAX_RX_QUEUE; rxring_idx++) {
+		/* force the rx_ring[RX_MPDU_QUEUE]
+		 * RX_CMD_QUEUE].idx to the first one
+		 * If using the new trx flow, do nothing
+		 */
+		if (!rtlpriv->use_new_trx_flow &&
+		    rtlpci->rx_ring[rxring_idx].desc) {
+			struct rtl_rx_desc *entry = NULL;
+
+			for (i = 0; i < rtlpci->rxringcount; i++) {
+				entry = &rtlpci->rx_ring[rxring_idx].desc[i];
+				rtlpriv->cfg->ops->set_desc(hw, (u8 *)entry,
+							    false,
+							    HW_DESC_RXOWN,
+							    &tmp_one);
+			}
+		}
+		rtlpci->rx_ring[rxring_idx].idx = 0;
+	}
+
+	/* after reset, release previous pending packet,
+	 * and force the  tx idx to the first one
+	 */
+	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
+	for (i = 0; i < RTL_PCI_MAX_TX_QUEUE_COUNT; i++) {
+		if (rtlpci->tx_ring[i].desc ||
+		    rtlpci->tx_ring[i].buffer_desc) {
+			struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[i];
+
+			while (skb_queue_len(&ring->queue)) {
+				struct sk_buff *skb =
+					__skb_dequeue(&ring->queue);
+				u8 *entry;
+
+				if (rtlpriv->use_new_trx_flow)
+					entry = (u8 *)(&ring->buffer_desc
+								[ring->idx]);
+				else
+					entry = (u8 *)(&ring->desc[ring->idx]);
+
+				pci_unmap_single(rtlpci->pdev,
+						 rtlpriv->cfg->ops->get_desc(
+						 (u8 *)entry, true,
+						 HW_DESC_TXBUFF_ADDR),
+					skb->len, PCI_DMA_TODEVICE);
+				kfree_skb(skb);
+				ring->idx = (ring->idx + 1) % ring->entries;
+			}
+			ring->idx = 0;
+		}
+	}
+
+	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
+
+	return 0;
+}
+
+static bool rtl_pci_tx_chk_waitq_insert(struct ieee80211_hw *hw,
+					struct ieee80211_sta *sta,
+					struct sk_buff *skb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_sta_info *sta_entry = NULL;
+	u8 tid = rtl_get_tid(skb);
+	__le16 fc = rtl_get_fc(skb);
+
+	if (!sta)
+		return false;
+	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
+
+	if (!rtlpriv->rtlhal.b_earlymode_enable)
+		return false;
+	if (ieee80211_is_nullfunc(fc))
+		return false;
+	if (ieee80211_is_qos_nullfunc(fc))
+		return false;
+	if (ieee80211_is_pspoll(fc))
+		return false;
+
+	if (sta_entry->tids[tid].agg.agg_state != RTL_AGG_OPERATIONAL)
+		return false;
+	if (_rtl_mac_to_hwqueue(hw, skb) > VO_QUEUE)
+		return false;
+	if (tid > 7)
+		return false;
+	/* maybe every tid should be checked */
+	if (!rtlpriv->link_info.higher_busytxtraffic[tid])
+		return false;
+
+	spin_lock_bh(&rtlpriv->locks.waitq_lock);
+	skb_queue_tail(&rtlpriv->mac80211.skb_waitq[tid], skb);
+	spin_unlock_bh(&rtlpriv->locks.waitq_lock);
+
+	return true;
+}
+
+static int rtl_pci_tx(struct ieee80211_hw *hw,
+		      struct ieee80211_sta *sta,
+		      struct sk_buff *skb,
+		      struct rtl_tcb_desc *ptcb_desc)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_sta_info *sta_entry = NULL;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct rtl8192_tx_ring *ring;
+	struct rtl_tx_desc *pdesc;
+	struct rtl_tx_buffer_desc *ptx_bd_desc = NULL;
+	u16 idx;
+	u8 own;
+	u8 temp_one = 1;
+	u8 hw_queue = _rtl_mac_to_hwqueue(hw, skb);
+	unsigned long flags;
+	struct ieee80211_hdr *hdr = rtl_get_hdr(skb);
+	__le16 fc = rtl_get_fc(skb);
+	u8 *pda_addr = hdr->addr1;
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	/*ssn */
+	u8 tid = 0;
+	u16 seq_number = 0;
+
+	if (ieee80211_is_mgmt(fc))
+		rtl92e_tx_mgmt_proc(hw, skb);
+
+	if (rtlpriv->psc.sw_ps_enabled) {
+		if (ieee80211_is_data(fc) && !ieee80211_is_nullfunc(fc) &&
+		    !ieee80211_has_pm(fc))
+			hdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_PM);
+	}
+
+	rtl92e_action_proc(hw, skb, true);
+
+	if (is_multicast_ether_addr(pda_addr))
+		rtlpriv->stats.txbytesmulticast += skb->len;
+	else if (is_broadcast_ether_addr(pda_addr))
+		rtlpriv->stats.txbytesbroadcast += skb->len;
+	else
+		rtlpriv->stats.txbytesunicast += skb->len;
+
+	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
+	ring = &rtlpci->tx_ring[hw_queue];
+	if (hw_queue != BEACON_QUEUE) {
+		if (rtlpriv->use_new_trx_flow)
+			idx = ring->cur_tx_wp;
+		else
+			idx = (ring->idx + skb_queue_len(&ring->queue)) %
+			      ring->entries;
+	} else {
+		idx = 0;
+	}
+
+	pdesc = &ring->desc[idx];
+
+	if (rtlpriv->use_new_trx_flow) {
+		ptx_bd_desc = &ring->buffer_desc[idx];
+	} else {
+		own = (u8) rtlpriv->cfg->ops->get_desc((u8 *)pdesc,
+				true, HW_DESC_OWN);
+
+		if ((own == 1) && (hw_queue != BEACON_QUEUE)) {
+			RT_TRACE(COMP_ERR, DBG_WARNING,
+				 ("No more TX desc@%d, ring->idx = %d, idx = %d, skb_queue_len = 0x%d\n",
+				  hw_queue, ring->idx, idx,
+				  skb_queue_len(&ring->queue)));
+
+			spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock,
+					       flags);
+			return skb->len;
+		}
+	}
+
+	if (ieee80211_is_data_qos(fc)) {
+		tid = rtl_get_tid(skb);
+		if (sta) {
+			sta_entry = (struct rtl_sta_info *)sta->drv_priv;
+			seq_number = (le16_to_cpu(hdr->seq_ctrl) &
+				      IEEE80211_SCTL_SEQ) >> 4;
+			seq_number += 1;
+
+			if (!ieee80211_has_morefrags(hdr->frame_control))
+				sta_entry->tids[tid].seq_number = seq_number;
+		}
+	}
+
+	if (ieee80211_is_data(fc))
+		rtlpriv->cfg->ops->led_control(hw, LED_CTL_TX);
+
+	rtlpriv->cfg->ops->fill_tx_desc(hw, hdr, (u8 *)pdesc,
+					(u8 *)ptx_bd_desc, info, sta, skb,
+					hw_queue, ptcb_desc);
+
+	__skb_queue_tail(&ring->queue, skb);
+	if (rtlpriv->use_new_trx_flow) {
+		rtlpriv->cfg->ops->set_desc(hw, (u8 *)pdesc, true,
+					    HW_DESC_OWN, (u8 *)&hw_queue);
+	} else {
+		rtlpriv->cfg->ops->set_desc(hw, (u8 *)pdesc, true,
+					    HW_DESC_OWN, (u8 *)&temp_one);
+	}
+
+	if ((ring->entries - skb_queue_len(&ring->queue)) < 2 &&
+	    hw_queue != BEACON_QUEUE) {
+		RT_TRACE(COMP_ERR, DBG_LOUD,
+			 ("less desc left, stop skb_queue@%d, ring->idx = %d, idx = %d, skb_queue_len = 0x%d\n",
+			  hw_queue, ring->idx, idx,
+			  skb_queue_len(&ring->queue)));
+
+		ieee80211_stop_queue(hw, skb_get_queue_mapping(skb));
+	}
+
+	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
+
+	if (rtlpriv->cfg->ops->tx_polling)
+		rtlpriv->cfg->ops->tx_polling(hw, hw_queue);
+
+	return 0;
+}
+static void rtl_pci_flush(struct ieee80211_hw *hw, u32 queues, bool drop)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u16 i = 0;
+	int queue_id;
+	struct rtl8192_tx_ring *ring;
+
+	if (mac->skip_scan)
+		return;
+
+	for (queue_id = RTL_PCI_MAX_TX_QUEUE_COUNT - 1; queue_id >= 0;) {
+		u32 queue_len;
+		if (((queues >> queue_id) & 0x1) == 0) {
+			queue_id--;
+			continue;
+		}
+		ring = &pcipriv->dev.tx_ring[queue_id];
+		queue_len = skb_queue_len(&ring->queue);
+		if (queue_len == 0 || queue_id == BEACON_QUEUE ||
+		    queue_id == TXCMD_QUEUE) {
+			queue_id--;
+			continue;
+		} else {
+			msleep(5);
+			i++;
+		}
+
+		/* we just wait 1s for all queues */
+		if (rtlpriv->psc.rfpwr_state == ERFOFF ||
+		    is_hal_stop(rtlhal) || i >= 200)
+			return;
+	}
+}
+
+static void rtl_pci_deinit(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	_rtl_pci_deinit_trx_ring(hw);
+
+	synchronize_irq(rtlpci->pdev->irq);
+	tasklet_kill(&rtlpriv->works.irq_tasklet);
+
+	flush_workqueue(rtlpriv->works.rtl_wq);
+	destroy_workqueue(rtlpriv->works.rtl_wq);
+}
+
+static int rtl_pci_init(struct ieee80211_hw *hw, struct pci_dev *pdev)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int err;
+
+	_rtl_pci_init_struct(hw, pdev);
+
+	err = _rtl_pci_init_trx_ring(hw);
+	if (err) {
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("tx ring initialization failed"));
+		return err;
+	}
+
+	return 1;
+}
+
+static int rtl_pci_start(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	int err = 0;
+
+	RT_TRACE(COMP_INIT, DBG_DMESG, (" rtl_pci_start\n"));
+	rtl92e_pci_reset_trx_ring(hw);
+
+	rtlpriv->rtlhal.driver_is_goingto_unload = false;
+	if (rtlpriv->cfg->ops->get_btc_status()) {
+		rtlpriv->btcoexist.btc_ops->btc_init_variables(rtlpriv);
+		rtlpriv->btcoexist.btc_ops->btc_init_hal_vars(rtlpriv);
+	}
+
+	err = rtlpriv->cfg->ops->hw_init(hw);
+	if (err) {
+		RT_TRACE(COMP_INIT, DBG_DMESG,
+			 ("Failed to config hardware err %x!\n" , err));
+		return err;
+	}
+
+	rtlpriv->cfg->ops->enable_interrupt(hw);
+	RT_TRACE(COMP_INIT, DBG_LOUD, ("enable_interrupt OK\n"));
+
+	rtl92e_init_rx_config(hw);
+
+	/*should after adapter start and interrupt enable. */
+	set_hal_start(rtlhal);
+
+	RT_CLEAR_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
+
+	rtlpriv->rtlhal.up_first_time = false;
+
+	RT_TRACE(COMP_INIT, DBG_DMESG, ("rtl_pci_start OK\n"));
+	return 0;
+}
+
+static void rtl_pci_stop(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 RFInProgressTimeOut = 0;
+
+	if (rtlpriv->cfg->ops->get_btc_status())
+		rtlpriv->btcoexist.btc_ops->btc_halt_notify();
+
+	/*
+	 *should before disable interrrupt&adapter
+	 *and will do it immediately.
+	 */
+	set_hal_stop(rtlhal);
+
+	rtlpriv->cfg->ops->disable_interrupt(hw);
+
+	spin_lock(&rtlpriv->locks.rf_ps_lock);
+	while (ppsc->rfchange_inprogress) {
+		spin_unlock(&rtlpriv->locks.rf_ps_lock);
+		if (RFInProgressTimeOut > 100) {
+			spin_lock(&rtlpriv->locks.rf_ps_lock);
+			break;
+		}
+		mdelay(1);
+		RFInProgressTimeOut++;
+		spin_lock(&rtlpriv->locks.rf_ps_lock);
+	}
+	ppsc->rfchange_inprogress = true;
+	spin_unlock(&rtlpriv->locks.rf_ps_lock);
+
+	rtlpriv->rtlhal.driver_is_goingto_unload = true;
+	rtlpriv->cfg->ops->hw_disable(hw);
+	rtlpriv->cfg->ops->led_control(hw, LED_CTL_POWER_OFF);
+
+	spin_lock(&rtlpriv->locks.rf_ps_lock);
+	ppsc->rfchange_inprogress = false;
+	spin_unlock(&rtlpriv->locks.rf_ps_lock);
+
+	rtl_pci_enable_aspm(hw);
+}
+
+static bool _rtl_pci_find_adapter(struct pci_dev *pdev,
+				  struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct pci_dev *bridge_pdev = pdev->bus->self;
+	u16 venderid;
+	u16 deviceid;
+	u8 revisionid;
+	u16 irqline;
+	u8 tmp;
+
+	venderid = pdev->vendor;
+	deviceid = pdev->device;
+	pci_read_config_byte(pdev, 0x8, &revisionid);
+	pci_read_config_word(pdev, 0x3C, &irqline);
+
+	if (deviceid == RTL_PCI_8192_DID ||
+	    deviceid == RTL_PCI_0044_DID ||
+	    deviceid == RTL_PCI_0047_DID ||
+	    deviceid == RTL_PCI_8192SE_DID ||
+	    deviceid == RTL_PCI_8174_DID ||
+	    deviceid == RTL_PCI_8173_DID ||
+	    deviceid == RTL_PCI_8172_DID ||
+	    deviceid == RTL_PCI_8171_DID) {
+		switch (revisionid) {
+		case RTL_PCI_REVISION_ID_8192PCIE:
+			RT_TRACE(COMP_INIT, DBG_DMESG,
+				 ("8192E is found but not supported now-vid/did=%x/%x\n",
+				  venderid, deviceid));
+			rtlhal->hw_type = HARDWARE_TYPE_RTL8192E;
+			return false;
+			break;
+		case RTL_PCI_REVISION_ID_8192SE:
+			RT_TRACE(COMP_INIT, DBG_DMESG,
+				 ("8192SE is found - vid/did=%x/%x\n",
+				  venderid, deviceid));
+			rtlhal->hw_type = HARDWARE_TYPE_RTL8192SE;
+			break;
+		default:
+			RT_TRACE(COMP_ERR, DBG_WARNING,
+				 ("Err: Unknown device - vid/did=%x/%x\n",
+				  venderid, deviceid));
+			rtlhal->hw_type = HARDWARE_TYPE_RTL8192SE;
+			break;
+		}
+	} else if (deviceid == RTL_PCI_8723AE_DID) {
+		rtlhal->hw_type = HARDWARE_TYPE_RTL8723AE;
+		RT_TRACE(COMP_INIT, DBG_DMESG,
+			 ("8723AE PCI-E is found - vid/did=%x/%x\n",
+			  venderid, deviceid));
+	} else if (deviceid == RTL_PCI_8192CET_DID ||
+		   deviceid == RTL_PCI_8192CE_DID ||
+		   deviceid == RTL_PCI_8191CE_DID ||
+		   deviceid == RTL_PCI_8188CE_DID) {
+		rtlhal->hw_type = HARDWARE_TYPE_RTL8192CE;
+		RT_TRACE(COMP_INIT, DBG_DMESG,
+			 ("8192C PCI-E is found - vid/did=%x/%x\n",
+			  venderid, deviceid));
+	} else if (deviceid == RTL_PCI_8192DE_DID ||
+		   deviceid == RTL_PCI_8192DE_DID2) {
+		rtlhal->hw_type = HARDWARE_TYPE_RTL8192DE;
+		RT_TRACE(COMP_INIT, DBG_DMESG,
+			 ("8192D PCI-E is found - vid/did=%x/%x\n",
+			  venderid, deviceid));
+	} else if (deviceid == RTL_PCI_8188EE_DID) {
+			rtlhal->hw_type = HARDWARE_TYPE_RTL8188EE;
+			RT_TRACE(COMP_INIT , DBG_LOUD,
+				 ("Find adapter, Hardware type is 8188EE\n"));
+	} else if (deviceid == RTL_PCI_8723BE_DID) {
+			rtlhal->hw_type = HARDWARE_TYPE_RTL8723BE;
+			RT_TRACE(COMP_INIT , DBG_LOUD,
+				 ("Find adapter, Hardware type is 8723BE\n"));
+	} else if (deviceid == RTL_PCI_8192EE_DID) {
+			rtlhal->hw_type = HARDWARE_TYPE_RTL8192EE;
+			RT_TRACE(COMP_INIT , DBG_LOUD,
+				 ("Find adapter, Hardware type is 8192EE\n"));
+	} else if (deviceid == RTL_PCI_8821AE_DID) {
+			rtlhal->hw_type = HARDWARE_TYPE_RTL8821AE;
+			RT_TRACE(COMP_INIT , DBG_LOUD,
+				 ("Find adapter, Hardware type is 8821AE\n"));
+	} else if (deviceid == RTL_PCI_8812AE_DID) {
+			rtlhal->hw_type = HARDWARE_TYPE_RTL8812AE;
+			RT_TRACE(COMP_INIT , DBG_LOUD,
+				 ("Find adapter, Hardware type is 8812AE\n"));
+	} else {
+		RT_TRACE(COMP_ERR, DBG_WARNING,
+			 ("Err: Unknown device - vid/did=%x/%x\n",
+			  venderid, deviceid));
+
+		rtlhal->hw_type = RTL_DEFAULT_HARDWARE_TYPE;
+	}
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192DE) {
+		if (revisionid == 0 || revisionid == 1) {
+			if (revisionid == 0) {
+				RT_TRACE(COMP_INIT, DBG_LOUD,
+					 ("Find 92DE MAC0.\n"));
+				rtlhal->interfaceindex = 0;
+			} else if (revisionid == 1) {
+				RT_TRACE(COMP_INIT, DBG_LOUD,
+					 ("Find 92DE MAC1.\n"));
+				rtlhal->interfaceindex = 1;
+			}
+		} else {
+			RT_TRACE(COMP_INIT, DBG_LOUD,
+				 ("Unknown device - VendorID/DeviceID=%x/%x, Revision=%x\n",
+				  venderid, deviceid, revisionid));
+			rtlhal->interfaceindex = 0;
+		}
+	}
+
+	/* 92ee use new trx flow */
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE)
+		rtlpriv->use_new_trx_flow = true;
+	else
+		rtlpriv->use_new_trx_flow = false;
+
+	/*find bus info */
+	pcipriv->ndis_adapter.busnumber = pdev->bus->number;
+	pcipriv->ndis_adapter.devnumber = PCI_SLOT(pdev->devfn);
+	pcipriv->ndis_adapter.funcnumber = PCI_FUNC(pdev->devfn);
+
+	/*find bridge info */
+	pcipriv->ndis_adapter.pcibridge_vendor = PCI_BRIDGE_VENDOR_UNKNOWN;
+	/* some ARM have no bridge_pdev and will crash here
+	 * so we should check if bridge_pdev is NULL */
+	if (bridge_pdev) {
+		pcipriv->ndis_adapter.pcibridge_vendorid = bridge_pdev->vendor;
+		for (tmp = 0; tmp < PCI_BRIDGE_VENDOR_MAX; tmp++) {
+			if (bridge_pdev->vendor == pcibridge_vendors[tmp]) {
+				pcipriv->ndis_adapter.pcibridge_vendor = tmp;
+				RT_TRACE(COMP_INIT, DBG_DMESG,
+					 ("Pci Bridge Vendor is found index: %d\n",
+					  tmp));
+				break;
+			}
+		}
+	}
+
+	if (pcipriv->ndis_adapter.pcibridge_vendor !=
+	    PCI_BRIDGE_VENDOR_UNKNOWN) {
+		pcipriv->ndis_adapter.pcibridge_busnum =
+		    bridge_pdev->bus->number;
+		pcipriv->ndis_adapter.pcibridge_devnum =
+		    PCI_SLOT(bridge_pdev->devfn);
+		pcipriv->ndis_adapter.pcibridge_funcnum =
+		    PCI_FUNC(bridge_pdev->devfn);
+		pcipriv->ndis_adapter.pcicfg_addrport =
+		    (pcipriv->ndis_adapter.pcibridge_busnum << 16) |
+		    (pcipriv->ndis_adapter.pcibridge_devnum << 11) |
+		    (pcipriv->ndis_adapter.pcibridge_funcnum << 8) | (1 << 31);
+		pcipriv->ndis_adapter.pcibridge_pciehdr_offset =
+		    pci_pcie_cap(bridge_pdev);
+		pcipriv->ndis_adapter.num4bytes =
+		    (pcipriv->ndis_adapter.pcibridge_pciehdr_offset + 0x10) / 4;
+
+		rtl_pci_get_linkcontrol_field(hw);
+
+		if (pcipriv->ndis_adapter.pcibridge_vendor ==
+		    PCI_BRIDGE_VENDOR_AMD) {
+			pcipriv->ndis_adapter.amd_l1_patch =
+			    rtl_pci_get_amd_l1_patch(hw);
+		}
+	}
+
+	RT_TRACE(COMP_INIT, DBG_DMESG,
+		 ("pcidev busnumber:devnumber:funcnumber:vendor:link_ctl %d:%d:%d:%x:%x\n",
+		  pcipriv->ndis_adapter.busnumber,
+		  pcipriv->ndis_adapter.devnumber,
+		  pcipriv->ndis_adapter.funcnumber,
+		  pdev->vendor, pcipriv->ndis_adapter.linkctrl_reg));
+
+	RT_TRACE(COMP_INIT, DBG_DMESG,
+		 ("pci_bridge busnumber:devnumber:funcnumber:vendor:pcie_cap:link_ctl_reg:amd %d:%d:%d:%x:%x:%x:%x\n",
+		  pcipriv->ndis_adapter.pcibridge_busnum,
+		  pcipriv->ndis_adapter.pcibridge_devnum,
+		  pcipriv->ndis_adapter.pcibridge_funcnum,
+		  pcibridge_vendors[pcipriv->ndis_adapter.pcibridge_vendor],
+		  pcipriv->ndis_adapter.pcibridge_pciehdr_offset,
+		  pcipriv->ndis_adapter.pcibridge_linkctrlreg,
+		  pcipriv->ndis_adapter.amd_l1_patch));
+
+	rtl_pci_parse_configuration(pdev, hw);
+	list_add_tail(&rtlpriv->list, &rtlpriv->glb_var->glb_priv_list);
+	return true;
+}
+
+static int rtl_pci_intr_mode_msi(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(pcipriv);
+	int ret;
+	ret = pci_enable_msi(rtlpci->pdev);
+	if (ret < 0)
+		return ret;
+
+	ret = request_irq(rtlpci->pdev->irq, &_rtl_pci_interrupt,
+			  IRQF_SHARED, KBUILD_MODNAME, hw);
+	if (ret < 0) {
+		pci_disable_msi(rtlpci->pdev);
+		return ret;
+	}
+
+	rtlpci->using_msi = true;
+
+	RT_TRACE(COMP_INIT|COMP_INTR, DBG_DMESG, ("MSI Interrupt Mode!\n"));
+	return 0;
+}
+
+static int rtl_pci_intr_mode_legacy(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(pcipriv);
+	int ret;
+
+	ret = request_irq(rtlpci->pdev->irq, &_rtl_pci_interrupt,
+			  IRQF_SHARED, KBUILD_MODNAME, hw);
+	if (ret < 0)
+		return ret;
+
+	rtlpci->using_msi = false;
+	RT_TRACE(COMP_INIT|COMP_INTR, DBG_DMESG,
+		 ("Pin-based Interrupt Mode!\n"));
+	return 0;
+}
+
+static int rtl_pci_intr_mode_decide(struct ieee80211_hw *hw)
+{
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(pcipriv);
+	int ret;
+	if (rtlpci->msi_support) {
+		ret = rtl_pci_intr_mode_msi(hw);
+		if (ret < 0)
+			ret = rtl_pci_intr_mode_legacy(hw);
+	} else {
+		ret = rtl_pci_intr_mode_legacy(hw);
+	}
+	return ret;
+}
+
+/* this is used for other modules get
+ * hw pointer in rtl_pci_get_hw_pointer */
+static struct ieee80211_hw *hw_export;
+
+int stg_rtl_pci_probe(struct pci_dev *pdev,
+		      const struct pci_device_id *id)
+{
+	struct ieee80211_hw *hw = NULL;
+	struct rtl_priv *rtlpriv = NULL;
+	struct rtl_pci_priv *pcipriv = NULL;
+	struct rtl_pci *rtlpci;
+	unsigned long pmem_start, pmem_len, pmem_flags;
+	int err;
+
+	err = pci_enable_device(pdev);
+	if (err) {
+		RT_ASSERT(false,
+			  ("%s : Cannot enable new PCI device\n",
+			   pci_name(pdev)));
+		return err;
+	}
+
+	if (!pci_set_dma_mask(pdev, DMA_BIT_MASK(32))) {
+		if (pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32))) {
+			RT_ASSERT(false,
+				  ("Unable to obtain 32bit DMA for consistent allocations\n"));
+			pci_disable_device(pdev);
+			return -ENOMEM;
+		}
+	}
+
+	pci_set_master(pdev);
+
+	hw = ieee80211_alloc_hw(sizeof(struct rtl_pci_priv) +
+				sizeof(struct rtl_priv), &rtl92e_ops);
+	if (!hw) {
+		RT_ASSERT(false,
+			  ("%s : ieee80211 alloc failed\n", pci_name(pdev)));
+		err = -ENOMEM;
+		goto fail1;
+	}
+	hw_export = hw;
+
+	SET_IEEE80211_DEV(hw, &pdev->dev);
+	pci_set_drvdata(pdev, hw);
+
+	rtlpriv = hw->priv;
+	pcipriv = (void *)rtlpriv->priv;
+	pcipriv->dev.pdev = pdev;
+
+	/* init cfg & intf_ops */
+	rtlpriv->rtlhal.interface = INTF_PCI;
+	rtlpriv->cfg = (struct rtl_hal_cfg *)(id->driver_data);
+	rtlpriv->intf_ops = &rtl92e_pci_ops;
+	rtlpriv->glb_var = &global_var;
+
+	/*
+	 *init dbgp flags before all
+	 *other functions, because we will
+	 *use it in other funtions like
+	 *RT_TRACE/RT_PRINT/RTL_PRINT_DATA
+	 *you can not use these macro
+	 *before this
+	 */
+	rtl92e_dbgp_flag_init(hw);
+
+	/* MEM map */
+	err = pci_request_regions(pdev, KBUILD_MODNAME);
+	if (err) {
+		RT_ASSERT(false, ("Can't obtain PCI resources\n"));
+		return err;
+	}
+
+	pmem_start = pci_resource_start(pdev, rtlpriv->cfg->bar_id);
+	pmem_len = pci_resource_len(pdev, rtlpriv->cfg->bar_id);
+	pmem_flags = pci_resource_flags(pdev, rtlpriv->cfg->bar_id);
+
+	/*shared mem start */
+	rtlpriv->io.pci_mem_start =
+			(unsigned long)pci_iomap(pdev,
+			rtlpriv->cfg->bar_id, pmem_len);
+	if (rtlpriv->io.pci_mem_start == 0) {
+		RT_ASSERT(false, ("Can't map PCI mem\n"));
+		goto fail2;
+	}
+
+	RT_TRACE(COMP_INIT, DBG_DMESG,
+		 ("mem mapped space: start: 0x%08lx len:%08lx flags:%08lx, after map:0x%08lx\n",
+		  pmem_start, pmem_len, pmem_flags,
+		  rtlpriv->io.pci_mem_start));
+
+	/* Disable Clk Request */
+	pci_write_config_byte(pdev, 0x81, 0);
+	/* leave D3 mode */
+	pci_write_config_byte(pdev, 0x44, 0);
+	pci_write_config_byte(pdev, 0x04, 0x06);
+	pci_write_config_byte(pdev, 0x04, 0x07);
+
+	/* The next statement is needed when built as single module */
+	rtl_core_module_init();
+
+	/* find adapter */
+	/* if chip not support, will return false */
+	if (!_rtl_pci_find_adapter(pdev, hw))
+		goto fail3;
+
+	/* Init IO handler */
+	_rtl_pci_io_handler_init(&pdev->dev, hw);
+
+	/*like read eeprom and so on */
+	rtlpriv->cfg->ops->read_eeprom_info(hw);
+
+	if (rtlpriv->cfg->ops->init_sw_vars(hw)) {
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("Can't init_sw_vars.\n"));
+		goto fail3;
+	}
+
+	rtlpriv->cfg->ops->init_sw_leds(hw);
+
+	/*aspm */
+	rtl_pci_init_aspm(hw);
+
+	/* Init mac80211 sw */
+	err = rtl92e_init_core(hw);
+	if (err) {
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("Can't allocate sw for mac80211.\n"));
+		goto fail3;
+	}
+
+	/* Init PCI sw */
+	err = !rtl_pci_init(hw, pdev);
+	if (err) {
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("Failed to init PCI.\n"));
+		goto fail3;
+	}
+
+	err = ieee80211_register_hw(hw);
+	if (err) {
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("Can't register mac80211 hw.\n"));
+		goto fail3;
+	} else {
+		rtlpriv->mac80211.mac80211_registered = 1;
+	}
+	/* the wiphy must have been registed to
+	 * cfg80211 prior to regulatory_hint */
+	if (regulatory_hint(hw->wiphy, rtlpriv->regd.alpha2))
+		RT_TRACE(COMP_ERR, DBG_WARNING, ("regulatory_hint fail\n"));
+
+	/* add for prov */
+	rtl_proc_add_one(hw);
+
+	/*init rfkill */
+	rtl92e_init_rfkill(hw);
+
+	rtlpci = rtl_pcidev(pcipriv);
+	err = rtl_pci_intr_mode_decide(hw);
+	if (err) {
+		RT_TRACE(COMP_INIT, DBG_DMESG,
+			 ("%s: failed to register IRQ handler\n",
+			  wiphy_name(hw->wiphy)));
+		goto fail3;
+	} else {
+		rtlpci->irq_alloc = 1;
+	}
+
+	set_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status);
+	return 0;
+
+fail3:
+	pci_set_drvdata(pdev, NULL);
+	rtl92e_deinit_core(hw);
+	ieee80211_free_hw(hw);
+
+	if (rtlpriv->io.pci_mem_start != 0)
+		pci_iounmap(pdev, (void __iomem *)rtlpriv->io.pci_mem_start);
+
+fail2:
+	pci_release_regions(pdev);
+
+fail1:
+
+	pci_disable_device(pdev);
+
+	return -ENODEV;
+}
+EXPORT_SYMBOL(stg_rtl_pci_probe);
+
+struct ieee80211_hw *rtl_pci_get_hw_pointer(void)
+{
+	return hw_export;
+}
+EXPORT_SYMBOL(rtl_pci_get_hw_pointer);
+
+void stg_rtl_pci_disconnect(struct pci_dev *pdev)
+{
+	struct ieee80211_hw *hw = pci_get_drvdata(pdev);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(pcipriv);
+	struct rtl_mac *rtlmac = rtl_mac(rtlpriv);
+
+	clear_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status);
+
+	/* add for prov */
+	rtl_proc_remove_one(hw);
+
+	/*ieee80211_unregister_hw will call ops_stop */
+	if (rtlmac->mac80211_registered == 1) {
+		ieee80211_unregister_hw(hw);
+		rtlmac->mac80211_registered = 0;
+	} else {
+		rtl92e_deinit_deferred_work(hw);
+		rtlpriv->intf_ops->adapter_stop(hw);
+	}
+
+	/*deinit rfkill */
+	rtl92e_deinit_rfkill(hw);
+
+	rtl_pci_deinit(hw);
+	rtl92e_deinit_core(hw);
+	rtlpriv->cfg->ops->deinit_sw_vars(hw);
+
+	if (rtlpci->irq_alloc) {
+		synchronize_irq(rtlpci->pdev->irq);
+		free_irq(rtlpci->pdev->irq, hw);
+		rtlpci->irq_alloc = 0;
+	}
+
+	if (rtlpci->using_msi)
+		pci_disable_msi(rtlpci->pdev);
+
+	list_del(&rtlpriv->list);
+	if (rtlpriv->io.pci_mem_start != 0) {
+		pci_iounmap(pdev, (void __iomem *)rtlpriv->io.pci_mem_start);
+		pci_release_regions(pdev);
+	}
+
+	pci_disable_device(pdev);
+
+	rtl_pci_disable_aspm(hw);
+
+	pci_set_drvdata(pdev, NULL);
+
+	ieee80211_free_hw(hw);
+}
+EXPORT_SYMBOL(stg_rtl_pci_disconnect);
+
+/***************************************
+kernel pci power state define:
+PCI_D0         ((pci_power_t __force) 0)
+PCI_D1         ((pci_power_t __force) 1)
+PCI_D2         ((pci_power_t __force) 2)
+PCI_D3hot      ((pci_power_t __force) 3)
+PCI_D3cold     ((pci_power_t __force) 4)
+PCI_UNKNOWN    ((pci_power_t __force) 5)
+
+This function is called when system
+goes into suspend state mac80211 will
+call rtl_mac_stop() from the mac80211
+suspend function first, So there is
+no need to call hw_disable here.
+****************************************/
+int stg_rtl_pci_suspend(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct ieee80211_hw *hw = pci_get_drvdata(pdev);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->cfg->ops->hw_suspend(hw);
+	rtl92e_deinit_rfkill(hw);
+
+	return 0;
+}
+EXPORT_SYMBOL(stg_rtl_pci_suspend);
+
+int stg_rtl_pci_resume(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct ieee80211_hw *hw = pci_get_drvdata(pdev);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->cfg->ops->hw_resume(hw);
+	rtl92e_init_rfkill(hw);
+
+	return 0;
+}
+EXPORT_SYMBOL(stg_rtl_pci_resume);
+
+struct rtl_intf_ops rtl92e_pci_ops = {
+	.read92e_efuse_byte = read92e_efuse_byte,
+	.adapter_start = rtl_pci_start,
+	.adapter_stop = rtl_pci_stop,
+	.check_buddy_priv = rtl_pci_check_buddy_priv,
+	.adapter_tx = rtl_pci_tx,
+	.flush = rtl_pci_flush,
+	.reset_trx_ring = rtl92e_pci_reset_trx_ring,
+	.waitq_insert = rtl_pci_tx_chk_waitq_insert,
+
+	.disable_aspm = rtl_pci_disable_aspm,
+	.enable_aspm = rtl_pci_enable_aspm,
+};
diff --git a/drivers/staging/rtl8192ee/pci.h b/drivers/staging/rtl8192ee/pci.h
new file mode 100644
index 00000000000..62c23a7f450
--- /dev/null
+++ b/drivers/staging/rtl8192ee/pci.h
@@ -0,0 +1,342 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL_PCI_H__
+#define __RTL_PCI_H__
+
+#include <linux/pci.h>
+/*
+1: MSDU packet queue,
+2: Rx Command Queue
+*/
+#define RTL_PCI_RX_MPDU_QUEUE			0
+#define RTL_PCI_RX_CMD_QUEUE			1
+#define RTL_PCI_MAX_RX_QUEUE			2
+
+#define RTL_PCI_MAX_RX_COUNT			512/*64*/
+#define RTL_PCI_MAX_TX_QUEUE_COUNT		9
+
+#define RT_TXDESC_NUM				128
+#define TX_DESC_NUM_92E				512
+#define RT_TXDESC_NUM_BE_QUEUE			256
+
+#define BK_QUEUE				0
+#define BE_QUEUE				1
+#define VI_QUEUE				2
+#define VO_QUEUE				3
+#define BEACON_QUEUE				4
+#define TXCMD_QUEUE				5
+#define MGNT_QUEUE				6
+#define HIGH_QUEUE				7
+#define HCCA_QUEUE				8
+
+#define RTL_PCI_DEVICE(vend, dev, cfg)  \
+	.vendor = (vend), \
+	.device = (dev), \
+	.subvendor = PCI_ANY_ID, \
+	.subdevice = PCI_ANY_ID,\
+	.driver_data = (kernel_ulong_t)&(cfg)
+
+#define INTEL_VENDOR_ID				0x8086
+#define SIS_VENDOR_ID				0x1039
+#define ATI_VENDOR_ID				0x1002
+#define ATI_DEVICE_ID				0x7914
+#define AMD_VENDOR_ID				0x1022
+
+#define PCI_MAX_BRIDGE_NUMBER			255
+#define PCI_MAX_DEVICES				32
+#define PCI_MAX_FUNCTION			8
+
+#define PCI_CONF_ADDRESS	0x0CF8	/*PCI Configuration Space Address */
+#define PCI_CONF_DATA		0x0CFC	/*PCI Configuration Space Data */
+
+#define PCI_CLASS_BRIDGE_DEV		0x06
+#define PCI_SUBCLASS_BR_PCI_TO_PCI	0x04
+#define PCI_CAPABILITY_ID_PCI_EXPRESS	0x10
+#define PCI_CAP_ID_EXP			0x10
+
+#define U1DONTCARE			0xFF
+#define U2DONTCARE			0xFFFF
+#define U4DONTCARE			0xFFFFFFFF
+
+#define RTL_PCI_8192_DID	0x8192	/*8192 PCI-E */
+#define RTL_PCI_8192SE_DID	0x8192	/*8192 SE */
+#define RTL_PCI_8174_DID	0x8174	/*8192 SE */
+#define RTL_PCI_8173_DID	0x8173	/*8191 SE Crab */
+#define RTL_PCI_8172_DID	0x8172	/*8191 SE RE */
+#define RTL_PCI_8171_DID	0x8171	/*8191 SE Unicron */
+#define RTL_PCI_0045_DID	0x0045	/*8190 PCI for Ceraga */
+#define RTL_PCI_0046_DID	0x0046	/*8190 Cardbus for Ceraga */
+#define RTL_PCI_0044_DID	0x0044	/*8192e PCIE for Ceraga */
+#define RTL_PCI_0047_DID	0x0047	/*8192e Express Card for Ceraga */
+#define RTL_PCI_700F_DID	0x700F
+#define RTL_PCI_701F_DID	0x701F
+#define RTL_PCI_DLINK_DID	0x3304
+#define RTL_PCI_8723AE_DID	0x8723	/*8723e */
+#define RTL_PCI_8192CET_DID	0x8191	/*8192ce */
+#define RTL_PCI_8192CE_DID	0x8178	/*8192ce */
+#define RTL_PCI_8191CE_DID	0x8177	/*8192ce */
+#define RTL_PCI_8188CE_DID	0x8176	/*8192ce */
+#define RTL_PCI_8192CU_DID	0x8191	/*8192ce */
+#define RTL_PCI_8192DE_DID	0x8193	/*8192de */
+#define RTL_PCI_8192DE_DID2	0x002B	/*92DE*/
+#define RTL_PCI_8188EE_DID	0x8179  /*8188ee*/
+#define RTL_PCI_8723BE_DID	0xB723  /*8723be*/
+#define RTL_PCI_8192EE_DID	0x818B	/*8192ee*/
+#define RTL_PCI_8821AE_DID	0x8821	/*8821ae*/
+#define RTL_PCI_8812AE_DID	0x8812	/*8812ae*/
+
+/*8192 support 16 pages of IO registers*/
+#define RTL_MEM_MAPPED_IO_RANGE_8190PCI		0x1000
+#define RTL_MEM_MAPPED_IO_RANGE_8192PCIE	0x4000
+#define RTL_MEM_MAPPED_IO_RANGE_8192SE		0x4000
+#define RTL_MEM_MAPPED_IO_RANGE_8192CE		0x4000
+#define RTL_MEM_MAPPED_IO_RANGE_8192DE		0x4000
+
+#define RTL_PCI_REVISION_ID_8190PCI		0x00
+#define RTL_PCI_REVISION_ID_8192PCIE		0x01
+#define RTL_PCI_REVISION_ID_8192SE		0x10
+#define RTL_PCI_REVISION_ID_8192CE		0x1
+#define RTL_PCI_REVISION_ID_8192DE		0x0
+
+#define RTL_DEFAULT_HARDWARE_TYPE	HARDWARE_TYPE_RTL8192CE
+
+enum pci_bridge_vendor {
+	PCI_BRIDGE_VENDOR_INTEL = 0x0,	/*0b'0000,0001 */
+	PCI_BRIDGE_VENDOR_ATI,		/*0b'0000,0010*/
+	PCI_BRIDGE_VENDOR_AMD,		/*0b'0000,0100*/
+	PCI_BRIDGE_VENDOR_SIS,		/*0b'0000,1000*/
+	PCI_BRIDGE_VENDOR_UNKNOWN,	/*0b'0100,0000*/
+	PCI_BRIDGE_VENDOR_MAX,
+};
+
+struct rtl_pci_capabilities_header {
+	u8 capability_id;
+	u8 next;
+};
+
+/* In new TRX flow, Buffer_desc is new concept
+  * But TX wifi info == TX descriptor in old flow
+  * RX wifi info == RX descriptor in old flow */
+struct rtl_tx_buffer_desc {
+#if (RTL8192EE_SEG_NUM == 2)
+	u32 dword[2*(DMA_IS_64BIT + 1)*8]; /*seg = 8*/
+#elif (RTL8192EE_SEG_NUM == 1)
+	u32 dword[2*(DMA_IS_64BIT + 1)*4]; /*seg = 4*/
+#elif (RTL8192EE_SEG_NUM == 0)
+	u32 dword[2*(DMA_IS_64BIT + 1)*2]; /*seg = 2*/
+#endif
+} __packed;
+
+struct rtl_tx_desc {/*old: tx desc new: tx wifi info*/
+	u32 dword[16];
+} __packed;
+
+struct rtl_rx_buffer_desc { /*rx buffer desc*/
+	u32 dword[2];
+} __packed;
+
+struct rtl_rx_desc { /*old: rx desc new: rx wifi info*/
+	u32 dword[8];
+} __packed;
+
+struct rtl_tx_cmd_desc {
+	u32 dword[16];
+} __packed;
+
+struct rtl8192_tx_ring {
+	struct rtl_tx_desc *desc; /*tx desc / tx wifi info*/
+	dma_addr_t dma; /*tx desc dma memory / tx wifi info dma memory*/
+	unsigned int idx;
+	unsigned int entries;
+	struct sk_buff_head queue;
+	/*add for new trx flow*/
+	struct rtl_tx_buffer_desc *buffer_desc; /*tx buffer descriptor*/
+	dma_addr_t buffer_desc_dma; /*tx bufferd desc dma memory*/
+	u16 avl_desc; /* available_desc_to_write */
+	u16 cur_tx_wp; /* current_tx_write_point */
+	u16 cur_tx_rp; /* current_tx_read_point */
+};
+
+struct rtl8192_rx_ring {
+	struct rtl_rx_desc *desc;/*for old trx flow, not uesd in new trx*/
+	/*dma matches either 'desc' or 'buffer_desc'*/
+	dma_addr_t dma;
+	unsigned int idx;
+	struct sk_buff *rx_buf[RTL_PCI_MAX_RX_COUNT];
+	/*add for new trx flow*/
+	struct rtl_rx_buffer_desc *buffer_desc; /*rx buffer descriptor*/
+	u16 next_rx_rp; /* next_rx_read_point */
+};
+
+struct rtl_pci {
+	struct pci_dev *pdev;
+	bool irq_enabled;
+
+	/*Tx */
+	struct rtl8192_tx_ring tx_ring[RTL_PCI_MAX_TX_QUEUE_COUNT];
+	int txringcount[RTL_PCI_MAX_TX_QUEUE_COUNT];
+	u32 transmit_config;
+
+	/*Rx */
+	struct rtl8192_rx_ring rx_ring[RTL_PCI_MAX_RX_QUEUE];
+	int rxringcount;
+	u16 rxbuffersize;
+	u32 receive_config;
+
+	/*irq */
+	u8 irq_alloc;
+	u32 irq_mask[2];
+	u32 sys_irq_mask;
+
+	/*Bcn control register setting */
+	u32 reg_bcn_ctrl_val;
+
+	 /*ASPM*/ u8 const_pci_aspm;
+	u8 const_amdpci_aspm;
+	u8 const_hwsw_rfoff_d3;
+	u8 const_support_pciaspm;
+	/*pci-e bridge */
+	u8 const_hostpci_aspm_setting;
+	/*pci-e device */
+	u8 const_devicepci_aspm_setting;
+	/*If it supports ASPM, Offset[560h] = 0x40,
+	   otherwise Offset[560h] = 0x00. */
+	bool b_support_aspm;
+	bool b_support_backdoor;
+
+	/*QOS & EDCA */
+	enum acm_method acm_method;
+
+	u16 shortretry_limit;
+	u16 longretry_limit;
+
+	/* MSI support */
+	bool msi_support;
+	bool using_msi;
+};
+
+struct mp_adapter {
+	u8 linkctrl_reg;
+
+	u8 busnumber;
+	u8 devnumber;
+	u8 funcnumber;
+
+	u8 pcibridge_busnum;
+	u8 pcibridge_devnum;
+	u8 pcibridge_funcnum;
+
+	u8 pcibridge_vendor;
+	u16 pcibridge_vendorid;
+	u16 pcibridge_deviceid;
+
+	u32 pcicfg_addrport;
+	u8 num4bytes;
+
+	u8 pcibridge_pciehdr_offset;
+	u8 pcibridge_linkctrlreg;
+
+	bool amd_l1_patch;
+};
+
+struct rtl_pci_priv {
+	struct rtl_pci dev;
+	struct mp_adapter ndis_adapter;
+	struct rtl_led_ctl ledctl;
+	struct bt_coexist_info btcoexist;
+};
+
+#define rtl_pcipriv(hw)		(((struct rtl_pci_priv *)(rtl_priv(hw))->priv))
+#define rtl_pcidev(pcipriv)	(&((pcipriv)->dev))
+
+int rtl92e_pci_reset_trx_ring(struct ieee80211_hw *hw);
+
+extern struct rtl_intf_ops rtl92e_pci_ops;
+
+int stg_rtl_pci_probe(struct pci_dev *pdev,
+		      const struct pci_device_id *id);
+void stg_rtl_pci_disconnect(struct pci_dev *pdev);
+int stg_rtl_pci_suspend(struct device *dev);
+int stg_rtl_pci_resume(struct device *dev);
+
+static inline u8 pci_read8_sync(struct rtl_priv *rtlpriv, u32 addr)
+{
+	return 0xff & readb((u8 __iomem *)rtlpriv->io.pci_mem_start + addr);
+}
+
+static inline u16 pci_read16_sync(struct rtl_priv *rtlpriv, u32 addr)
+{
+	return readw((u8 __iomem *)rtlpriv->io.pci_mem_start + addr);
+}
+
+static inline u32 pci_read32_sync(struct rtl_priv *rtlpriv, u32 addr)
+{
+	return readl((u8 __iomem *)rtlpriv->io.pci_mem_start + addr);
+}
+
+static inline void pci_write8_async(struct rtl_priv *rtlpriv, u32 addr, u8 val)
+{
+	writeb(val, (u8 __iomem *)rtlpriv->io.pci_mem_start + addr);
+}
+
+static inline void pci_write16_async(struct rtl_priv *rtlpriv,
+				     u32 addr, u16 val)
+{
+	writew(val, (u8 __iomem *)rtlpriv->io.pci_mem_start + addr);
+}
+
+static inline void pci_write32_async(struct rtl_priv *rtlpriv,
+				     u32 addr, u32 val)
+{
+	writel(val, (u8 __iomem *)rtlpriv->io.pci_mem_start + addr);
+}
+
+static inline void rtl_pci_raw_write_port_ulong(u32 port, u32 val)
+{
+	outl(val, port);
+}
+
+static inline void rtl_pci_raw_write_port_uchar(u32 port, u8 val)
+{
+	outb(val, port);
+}
+
+static inline void rtl_pci_raw_read_port_uchar(u32 port, u8 *pval)
+{
+	*pval = inb(port);
+}
+
+static inline void rtl_pci_raw_read_port_ushort(u32 port, u16 *pval)
+{
+	*pval = inw(port);
+}
+
+static inline void rtl_pci_raw_read_port_ulong(u32 port, u32 *pval)
+{
+	*pval = inl(port);
+}
+
+#endif
diff --git a/drivers/staging/rtl8192ee/ps.c b/drivers/staging/rtl8192ee/ps.c
new file mode 100644
index 00000000000..90c3fc2e62b
--- /dev/null
+++ b/drivers/staging/rtl8192ee/ps.c
@@ -0,0 +1,983 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "wifi.h"
+#include "base.h"
+#include "ps.h"
+#include "btcoexist/rtl_btc.h"
+
+bool stg_rtl_ps_enable_nic(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool init_status = true;
+
+	/*<1> reset trx ring */
+	if (rtlhal->interface == INTF_PCI)
+		rtlpriv->intf_ops->reset_trx_ring(hw);
+
+	if (is_hal_stop(rtlhal))
+		RT_TRACE(COMP_ERR, DBG_WARNING, ("Driver is already down!\n"));
+
+	/*<2> Enable Adapter */
+	rtlpriv->cfg->ops->hw_init(hw);
+	RT_CLEAR_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
+	/*init_status = false; */
+
+	/*<3> Enable Interrupt */
+	rtlpriv->cfg->ops->enable_interrupt(hw);
+
+	/*<enable timer> */
+	rtl92e_watch_dog_timer_callback((unsigned long)hw);
+
+	return init_status;
+}
+EXPORT_SYMBOL(stg_rtl_ps_enable_nic);
+
+bool stg_rtl_ps_disable_nic(struct ieee80211_hw *hw)
+{
+	bool status = true;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	/*<1> Stop all timer */
+	rtl92e_deinit_deferred_work(hw);
+
+	/*<2> Disable Interrupt */
+	rtlpriv->cfg->ops->disable_interrupt(hw);
+
+	/*<3> Disable Adapter */
+	rtlpriv->cfg->ops->hw_disable(hw);
+
+	return status;
+}
+EXPORT_SYMBOL(stg_rtl_ps_disable_nic);
+
+bool stg_rtl_ps_set_rf_state(struct ieee80211_hw *hw,
+			     enum rf_pwrstate state_toset,
+			     u32 changesource, bool protect_or_not)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	enum rf_pwrstate rtstate;
+	bool b_actionallowed = false;
+	u16 rfwait_cnt = 0;
+
+	/*protect_or_not = true; */
+
+	if (protect_or_not)
+		goto no_protect;
+
+	/*
+	 *Only one thread can change
+	 *the RF state at one time, and others
+	 *should wait to be executed.
+	 */
+	while (true) {
+		spin_lock(&rtlpriv->locks.rf_ps_lock);
+		if (ppsc->rfchange_inprogress) {
+			spin_unlock(&rtlpriv->locks.rf_ps_lock);
+
+			RT_TRACE(COMP_ERR, DBG_WARNING,
+				 ("RF Change in progress! Wait to set..state_toset(%d)\n",
+				  state_toset));
+
+			/* Set RF after the previous action is done.  */
+			while (ppsc->rfchange_inprogress) {
+				rfwait_cnt++;
+				mdelay(1);
+				/*
+				 *Wait too long, return false to avoid
+				 *to be stuck here.
+				 */
+				if (rfwait_cnt > 100)
+					return false;
+			}
+		} else {
+			ppsc->rfchange_inprogress = true;
+			spin_unlock(&rtlpriv->locks.rf_ps_lock);
+			break;
+		}
+	}
+
+no_protect:
+	rtstate = ppsc->rfpwr_state;
+
+	switch (state_toset) {
+	case ERFON:
+		ppsc->rfoff_reason &= (~changesource);
+
+		if ((changesource == RF_CHANGE_BY_HW) &&
+		    (ppsc->b_hwradiooff)) {
+			ppsc->b_hwradiooff = false;
+		}
+		if (!ppsc->rfoff_reason) {
+			ppsc->rfoff_reason = 0;
+			b_actionallowed = true;
+		}
+		break;
+	case ERFOFF:
+		if ((changesource == RF_CHANGE_BY_HW) &&
+		    (!ppsc->b_hwradiooff)) {
+			ppsc->b_hwradiooff = true;
+		}
+		ppsc->rfoff_reason |= changesource;
+		b_actionallowed = true;
+		break;
+	case ERFSLEEP:
+		ppsc->rfoff_reason |= changesource;
+		b_actionallowed = true;
+		break;
+	default:
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("switch case not process\n"));
+		break;
+	}
+
+	if (b_actionallowed)
+		rtlpriv->cfg->ops->set_rf_power_state(hw, state_toset);
+
+	if (!protect_or_not) {
+		spin_lock(&rtlpriv->locks.rf_ps_lock);
+		ppsc->rfchange_inprogress = false;
+		spin_unlock(&rtlpriv->locks.rf_ps_lock);
+	}
+
+	return b_actionallowed;
+}
+EXPORT_SYMBOL(stg_rtl_ps_set_rf_state);
+
+static void _rtl_ps_inactive_ps(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+
+	ppsc->b_swrf_processing = true;
+
+	if (ppsc->inactive_pwrstate == ERFON && rtlhal->interface == INTF_PCI) {
+		if ((ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM) &&
+		    RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM) &&
+		    rtlhal->interface == INTF_PCI) {
+			rtlpriv->intf_ops->disable_aspm(hw);
+			RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
+		}
+	}
+
+	stg_rtl_ps_set_rf_state(hw, ppsc->inactive_pwrstate,
+				RF_CHANGE_BY_IPS, false);
+
+	if (ppsc->inactive_pwrstate == ERFOFF &&
+	    rtlhal->interface == INTF_PCI) {
+		if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM &&
+		    !RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
+			rtlpriv->intf_ops->enable_aspm(hw);
+			RT_SET_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
+		}
+	}
+
+	ppsc->b_swrf_processing = false;
+}
+
+void rtl92e_ips_nic_off_wq_callback(void *data)
+{
+	struct rtl_works *rtlworks =
+	    container_of_dwork_rtl(data, struct rtl_works, ips_nic_off_wq);
+	struct ieee80211_hw *hw = rtlworks->hw;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	enum rf_pwrstate rtstate;
+
+	if (mac->opmode != NL80211_IFTYPE_STATION) {
+		RT_TRACE(COMP_ERR, DBG_WARNING, ("not station return\n"));
+		return;
+	}
+
+	if (mac->p2p_in_use)
+		return;
+
+	if (mac->link_state > MAC80211_NOLINK)
+		return;
+
+	if (is_hal_stop(rtlhal))
+		return;
+
+	if (rtlpriv->sec.being_setkey)
+		return;
+
+	if (rtlpriv->cfg->ops->bt_turn_off_bt_coexist_before_enter_lps)
+		rtlpriv->cfg->ops->bt_turn_off_bt_coexist_before_enter_lps(hw);
+
+	if (ppsc->b_inactiveps) {
+		rtstate = ppsc->rfpwr_state;
+
+		/*
+		 *Do not enter IPS in the following conditions:
+		 *(1) RF is already OFF or Sleep
+		 *(2) b_swrf_processing (indicates the IPS is still under going)
+		 *(3) Connectted (only disconnected can trigger IPS)
+		 *(4) IBSS (send Beacon)
+		 *(5) AP mode (send Beacon)
+		 *(6) monitor mode (rcv packet)
+		 */
+
+		if (rtstate == ERFON &&
+		    !ppsc->b_swrf_processing &&
+		    (mac->link_state == MAC80211_NOLINK) &&
+		    !mac->act_scanning) {
+			RT_TRACE(COMP_RF, DBG_LOUD,
+				 ("IPSEnter(): Turn off RF.\n"));
+
+			ppsc->inactive_pwrstate = ERFOFF;
+			ppsc->b_in_powersavemode = true;
+
+			/* call before RF off */
+			if (rtlpriv->cfg->ops->get_btc_status())
+				rtlpriv->btcoexist.btc_ops->btc_ips_notify(rtlpriv,
+									ppsc->inactive_pwrstate);
+
+			/*rtl92e_pci_reset_trx_ring(hw); */
+			_rtl_ps_inactive_ps(hw);
+		}
+	}
+}
+
+void rtl92e_ips_nic_off(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	/*
+	 *because when link with ap, mac80211 will ask us
+	 *to disable nic quickly after scan before linking,
+	 *this will cause link failed, so we delay 100ms here
+	 */
+	queue_delayed_work(rtlpriv->works.rtl_wq,
+			   &rtlpriv->works.ips_nic_off_wq, MSECS(100));
+}
+
+/* NOTICE: any opmode should exc nic_on, or disable without
+ * nic_on may something wrong, like adhoc TP*/
+void rtl92e_ips_nic_on(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	enum rf_pwrstate rtstate;
+
+	cancel_delayed_work(&rtlpriv->works.ips_nic_off_wq);
+
+	spin_lock(&rtlpriv->locks.ips_lock);
+	if (ppsc->b_inactiveps) {
+		rtstate = ppsc->rfpwr_state;
+
+		if (rtstate != ERFON &&
+		    !ppsc->b_swrf_processing &&
+		    ppsc->rfoff_reason <= RF_CHANGE_BY_IPS) {
+			ppsc->inactive_pwrstate = ERFON;
+			ppsc->b_in_powersavemode = false;
+			_rtl_ps_inactive_ps(hw);
+			/* call after RF on */
+			if (rtlpriv->cfg->ops->get_btc_status())
+				rtlpriv->btcoexist.btc_ops->btc_ips_notify(rtlpriv,
+									ppsc->inactive_pwrstate);
+		}
+	}
+	spin_unlock(&rtlpriv->locks.ips_lock);
+}
+
+/*for FW LPS*/
+
+/*
+ *Determine if we can set Fw into PS mode
+ *in current condition.Return true if it
+ *can enter PS mode.
+ */
+static bool rtl_get_fwlps_doze(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	u32 ps_timediff;
+
+	ps_timediff = jiffies_to_msecs(jiffies -
+				       ppsc->last_delaylps_stamp_jiffies);
+
+	if (ps_timediff < 2000) {
+		RT_TRACE(COMP_POWER, DBG_LOUD,
+			 ("Delay enter Fw LPS for DHCP, ARP, or EAPOL exchanging state\n"));
+		return false;
+	}
+
+	if (mac->link_state != MAC80211_LINKED)
+		return false;
+
+	if (mac->opmode == NL80211_IFTYPE_ADHOC)
+		return false;
+
+	return true;
+}
+
+/* Change current and default preamble mode.*/
+void rtl_lps_set_psmode(struct ieee80211_hw *hw, u8 rt_psmode)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	bool enter_fwlps;
+
+	if (mac->opmode == NL80211_IFTYPE_ADHOC)
+		return;
+
+	if (mac->link_state != MAC80211_LINKED)
+		return;
+
+	if (ppsc->dot11_psmode == rt_psmode)
+		return;
+
+	/* Update power save mode configured. */
+	ppsc->dot11_psmode = rt_psmode;
+
+	/*
+	 *<FW control LPS>
+	 *1. Enter PS mode
+	 *   Set RPWM to Fw to turn RF off and send H2C fw_pwrmode
+	 *   cmd to set Fw into PS mode.
+	 *2. Leave PS mode
+	 *   Send H2C fw_pwrmode cmd to Fw to set Fw into Active
+	 *   mode and set RPWM to turn RF on.
+	 */
+
+	if ((ppsc->b_fwctrl_lps) && ppsc->report_linked) {
+		if (ppsc->dot11_psmode == EACTIVE) {
+			RT_TRACE(COMP_RF, DBG_DMESG,
+				 ("FW LPS leave ps_mode:%x\n",
+				  FW_PS_ACTIVE_MODE));
+			enter_fwlps = false;
+			ppsc->pwr_mode = FW_PS_ACTIVE_MODE;
+			ppsc->smart_ps = 0;
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_LPS_ACTION,
+						      (u8 *)(&enter_fwlps));
+			if (ppsc->p2p_ps_info.opp_ps)
+				rtl92e_p2p_ps_cmd(hw , P2P_PS_ENABLE);
+
+			if (rtlpriv->cfg->ops->get_btc_status())
+				rtlpriv->btcoexist.btc_ops->btc_lps_notify(rtlpriv, rt_psmode);
+		} else {
+			if (rtl_get_fwlps_doze(hw)) {
+				RT_TRACE(COMP_RF, DBG_DMESG,
+					 ("FW LPS enter ps_mode:%x\n",
+					 ppsc->fwctrl_psmode));
+				if (rtlpriv->cfg->ops->get_btc_status())
+					rtlpriv->btcoexist.btc_ops->btc_lps_notify(rtlpriv, rt_psmode);
+				enter_fwlps = true;
+				ppsc->pwr_mode = ppsc->fwctrl_psmode;
+				ppsc->smart_ps = 2;
+				rtlpriv->cfg->ops->set_hw_reg(hw,
+							HW_VAR_FW_LPS_ACTION,
+							(u8 *)(&enter_fwlps));
+
+			} else {
+				/* Reset the power save related parameters. */
+				ppsc->dot11_psmode = EACTIVE;
+			}
+		}
+	}
+}
+
+/*Enter the leisure power save mode.*/
+void rtl92e_lps_enter(struct ieee80211_hw *hw)
+{
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	unsigned long flag;
+
+	if (!ppsc->b_fwctrl_lps)
+		return;
+
+	if (rtlpriv->sec.being_setkey)
+		return;
+
+	if (rtlpriv->link_info.b_busytraffic)
+		return;
+
+	/*sleep after linked 10s, to let DHCP and 4-way handshake ok enough!! */
+	if (mac->cnt_after_linked < 5)
+		return;
+
+	if (mac->opmode == NL80211_IFTYPE_ADHOC)
+		return;
+
+	if (mac->link_state != MAC80211_LINKED)
+		return;
+
+	spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag);
+
+	/* Idle for a while if we connect to AP a while ago. */
+	if (mac->cnt_after_linked >= 2) {
+		if (ppsc->dot11_psmode == EACTIVE) {
+			RT_TRACE(COMP_POWER, DBG_LOUD,
+				 ("Enter 802.11 power save mode...\n"));
+
+			rtl_lps_set_psmode(hw, EAUTOPS);
+		}
+	}
+
+	spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag);
+}
+EXPORT_SYMBOL(rtl92e_lps_enter);
+
+/*Leave the leisure power save mode.*/
+void rtl92e_lps_leave(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	unsigned long flag;
+
+	spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag);
+
+	if (ppsc->b_fwctrl_lps) {
+		if (ppsc->dot11_psmode != EACTIVE) {
+			if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM &&
+			    RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM) &&
+			    rtlhal->interface == INTF_PCI) {
+				rtlpriv->intf_ops->disable_aspm(hw);
+				RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
+			}
+
+			RT_TRACE(COMP_POWER, DBG_LOUD,
+				 ("Busy Traffic,Leave 802.11 power save..\n"));
+
+			rtl_lps_set_psmode(hw, EACTIVE);
+		}
+	}
+	spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag);
+}
+EXPORT_SYMBOL(rtl92e_lps_leave);
+
+/* For sw LPS*/
+void rtl92e_swlps_beacon(struct ieee80211_hw *hw, void *data, unsigned int len)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct ieee80211_hdr *hdr = (void *)data;
+	struct ieee80211_tim_ie *tim_ie;
+	u8 *tim;
+	u8 tim_len;
+	bool u_buffed;
+	bool m_buffed;
+
+	if (mac->opmode != NL80211_IFTYPE_STATION)
+		return;
+
+	if (!rtlpriv->psc.b_swctrl_lps)
+		return;
+
+	if (rtlpriv->mac80211.link_state != MAC80211_LINKED)
+		return;
+
+	if (!rtlpriv->psc.sw_ps_enabled)
+		return;
+
+	if (rtlpriv->psc.b_fwctrl_lps)
+		return;
+
+	if (likely(!(hw->conf.flags & IEEE80211_CONF_PS)))
+		return;
+
+	/* check if this really is a beacon */
+	if (!ieee80211_is_beacon(hdr->frame_control))
+		return;
+
+	/* min. beacon length + FCS_LEN */
+	if (len <= 40 + FCS_LEN)
+		return;
+
+	/* and only beacons from the associated BSSID, please */
+	if (!ether_addr_equal(hdr->addr3, rtlpriv->mac80211.bssid))
+		return;
+
+	rtlpriv->psc.last_beacon = jiffies;
+
+	tim = rtl92e_find_ie(data, len - FCS_LEN, WLAN_EID_TIM);
+	if (!tim)
+		return;
+
+	if (tim[1] < sizeof(*tim_ie))
+		return;
+
+	tim_len = tim[1];
+	tim_ie = (struct ieee80211_tim_ie *)&tim[2];
+
+	if (!WARN_ON_ONCE(!hw->conf.ps_dtim_period))
+		rtlpriv->psc.dtim_counter = tim_ie->dtim_count;
+
+	/* Check whenever the PHY can be turned off again. */
+
+	/* 1. What about buffered unicast traffic for our AID? */
+	u_buffed = ieee80211_check_tim(tim_ie, tim_len,
+				       rtlpriv->mac80211.assoc_id);
+
+	/* 2. Maybe the AP wants to send multicast/broadcast data? */
+	m_buffed = tim_ie->bitmap_ctrl & 0x01;
+	rtlpriv->psc.multi_buffered = m_buffed;
+
+	/* unicast will process by mac80211 through
+	 * set ~IEEE80211_CONF_PS, So we just check
+	 * multicast frames here */
+	if (!m_buffed) {/*&&) { !rtlpriv->psc.tx_doing) { */
+		/* back to low-power land. and delay is
+		 * prevent null power save frame tx fail */
+		queue_delayed_work(rtlpriv->works.rtl_wq,
+				   &rtlpriv->works.ps_work, MSECS(5));
+	} else {
+		RT_TRACE(COMP_POWER, DBG_DMESG,
+			 ("u_bufferd: %x, m_buffered: %x\n",
+			  u_buffed, m_buffed));
+	}
+}
+
+void rtl92e_swlps_rf_awake(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	unsigned long flag;
+
+	if (!rtlpriv->psc.b_swctrl_lps)
+		return;
+	if (mac->link_state != MAC80211_LINKED)
+		return;
+
+	if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM &&
+	    RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
+		rtlpriv->intf_ops->disable_aspm(hw);
+		RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
+	}
+
+	spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag);
+	stg_rtl_ps_set_rf_state(hw, ERFON, RF_CHANGE_BY_PS, false);
+	spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag);
+}
+
+void rtl92e_swlps_rfon_wq_callback(void *data)
+{
+	struct rtl_works *rtlworks =
+	    container_of_dwork_rtl(data, struct rtl_works, ps_rfon_wq);
+	struct ieee80211_hw *hw = rtlworks->hw;
+
+	rtl92e_swlps_rf_awake(hw);
+}
+
+void rtl92e_swlps_rf_sleep(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	unsigned long flag;
+	u8 sleep_intv;
+
+	if (!rtlpriv->psc.sw_ps_enabled)
+		return;
+
+	if ((rtlpriv->sec.being_setkey) ||
+	    (mac->opmode == NL80211_IFTYPE_ADHOC))
+		return;
+
+	/*sleep after linked 10s, to let DHCP and 4-way handshake ok enough!! */
+	if ((mac->link_state != MAC80211_LINKED) || (mac->cnt_after_linked < 5))
+		return;
+
+	if (rtlpriv->link_info.b_busytraffic)
+		return;
+
+	spin_lock(&rtlpriv->locks.rf_ps_lock);
+	if (rtlpriv->psc.rfchange_inprogress) {
+		spin_unlock(&rtlpriv->locks.rf_ps_lock);
+		return;
+	}
+	spin_unlock(&rtlpriv->locks.rf_ps_lock);
+
+	spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag);
+	stg_rtl_ps_set_rf_state(hw, ERFSLEEP, RF_CHANGE_BY_PS , false);
+	spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag);
+
+	if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM &&
+	    !RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
+		rtlpriv->intf_ops->enable_aspm(hw);
+		RT_SET_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
+	}
+
+	/* here is power save alg, when this beacon is DTIM
+	 * we will set sleep time to dtim_period * n;
+	 * when this beacon is not DTIM, we will set sleep
+	 * time to sleep_intv = rtlpriv->psc.dtim_counter or
+	 * MAX_SW_LPS_SLEEP_INTV(default set to 5) */
+
+	if (rtlpriv->psc.dtim_counter == 0) {
+		if (hw->conf.ps_dtim_period == 1)
+			sleep_intv = hw->conf.ps_dtim_period * 2;
+		else
+			sleep_intv = hw->conf.ps_dtim_period;
+	} else {
+		sleep_intv = rtlpriv->psc.dtim_counter;
+	}
+
+	if (sleep_intv > MAX_SW_LPS_SLEEP_INTV)
+		sleep_intv = MAX_SW_LPS_SLEEP_INTV;
+
+	/* this print should always be dtim_conter = 0 &
+	 * sleep  = dtim_period, that meaons, we should
+	 * awake before every dtim */
+	RT_TRACE(COMP_POWER, DBG_DMESG,
+		 ("dtim_counter:%x will sleep :%d beacon_intv\n",
+		  rtlpriv->psc.dtim_counter, sleep_intv));
+
+	/* we tested that 40ms is enough for sw & hw sw delay */
+	queue_delayed_work(rtlpriv->works.rtl_wq, &rtlpriv->works.ps_rfon_wq,
+			   MSECS(sleep_intv*mac->vif->bss_conf.beacon_int-40));
+}
+
+
+void rtl92e_swlps_wq_callback(void *data)
+{
+	struct rtl_works *rtlworks =
+		container_of_dwork_rtl(data, struct rtl_works, ps_work);
+	struct ieee80211_hw *hw = rtlworks->hw;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	bool ps = false;
+
+	ps = (hw->conf.flags & IEEE80211_CONF_PS);
+
+	/* we can sleep after ps null send ok */
+	if (rtlpriv->psc.state_inap) {
+		rtl92e_swlps_rf_sleep(hw);
+
+		if (rtlpriv->psc.state && !ps) {
+			rtlpriv->psc.sleep_ms =
+				jiffies_to_msecs(jiffies -
+						 rtlpriv->psc.last_action);
+		}
+
+		if (ps)
+			rtlpriv->psc.last_slept = jiffies;
+
+		rtlpriv->psc.last_action = jiffies;
+		rtlpriv->psc.state = ps;
+	}
+}
+
+static void rtl_p2p_noa_ie(struct ieee80211_hw *hw, void *data,
+			   unsigned int len)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct ieee80211_mgmt *mgmt = (void *)data;
+	struct rtl_p2p_ps_info *p2pinfo = &(rtlpriv->psc.p2p_ps_info);
+	u8 *pos, *end, *ie;
+	u16 noa_len;
+	static u8 p2p_oui_ie_type[4] = {0x50, 0x6f, 0x9a, 0x09};
+	u8 noa_num, index , i, noa_index = 0;
+	bool find_p2p_ie = false , find_p2p_ps_ie = false;
+	pos = (u8 *)mgmt->u.beacon.variable;
+	end = data + len;
+	ie = NULL;
+
+	while (pos + 1 < end) {
+		if (pos + 2 + pos[1] > end)
+			return;
+
+		if (pos[0] == 221 && pos[1] > 4) {
+			if (memcmp(&pos[2], p2p_oui_ie_type, 4) == 0) {
+				ie = pos + 2+4;
+				break;
+			}
+		}
+		pos += 2 + pos[1];
+	}
+
+	if (ie == NULL)
+		return;
+	find_p2p_ie = true;
+	/*to find noa ie*/
+	while (ie + 1 < end) {
+		noa_len = READEF2BYTE((__le16 *)&ie[1]);
+		if (ie + 3 + ie[1] > end)
+			return;
+
+		if (ie[0] == 12) {
+			find_p2p_ps_ie = true;
+			if ((noa_len - 2) % 13 != 0) {
+				RT_TRACE(COMP_INIT, DBG_LOUD,
+					 ("P2P notice of absence: invalid length%d\n",
+					 noa_len));
+				return;
+			} else {
+				noa_num = (noa_len - 2) / 13;
+			}
+			noa_index = ie[3];
+			if (rtlpriv->psc.p2p_ps_info.p2p_ps_mode ==
+			    P2P_PS_NONE || noa_index != p2pinfo->noa_index) {
+				RT_TRACE(COMP_FW, DBG_LOUD,
+					 ("update NOA ie.\n"));
+				p2pinfo->noa_index = noa_index;
+				p2pinfo->opp_ps = (ie[4] >> 7);
+				p2pinfo->ctwindow = ie[4] & 0x7F;
+				p2pinfo->noa_num = noa_num;
+				index = 5;
+				for (i = 0; i < noa_num; i++) {
+					p2pinfo->noa_count_type[i] =
+							READEF1BYTE(ie+index);
+					index += 1;
+					p2pinfo->noa_duration[i] =
+						 READEF4BYTE((__le32 *)ie+index);
+					index += 4;
+					p2pinfo->noa_interval[i] =
+						 READEF4BYTE((__le32 *)ie+index);
+					index += 4;
+					p2pinfo->noa_start_time[i] =
+						 READEF4BYTE((__le32 *)ie+index);
+					index += 4;
+				}
+
+				if (p2pinfo->opp_ps == 1) {
+					p2pinfo->p2p_ps_mode = P2P_PS_CTWINDOW;
+					/* Driver should wait LPS
+					 * entering CTWindow*/
+					if (rtlpriv->psc.b_fw_current_inpsmode) {
+						rtl92e_p2p_ps_cmd(hw,
+							       P2P_PS_ENABLE);
+					}
+				} else if (p2pinfo->noa_num > 0) {
+					p2pinfo->p2p_ps_mode = P2P_PS_NOA;
+					rtl92e_p2p_ps_cmd(hw, P2P_PS_ENABLE);
+				} else if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
+					rtl92e_p2p_ps_cmd(hw, P2P_PS_DISABLE);
+				}
+			}
+
+		break;
+		}
+		ie += 3 + noa_len;
+	}
+
+	if (find_p2p_ie) {
+		if ((p2pinfo->p2p_ps_mode > P2P_PS_NONE) &&
+		    (!find_p2p_ps_ie))
+			rtl92e_p2p_ps_cmd(hw, P2P_PS_DISABLE);
+	}
+}
+
+static void rtl_p2p_action_ie(struct ieee80211_hw *hw, void *data,
+			      unsigned int len)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct ieee80211_mgmt *mgmt = (void *)data;
+	struct rtl_p2p_ps_info *p2pinfo = &(rtlpriv->psc.p2p_ps_info);
+	bool find_p2p_ie = false, find_p2p_ps_ie = false;
+	u8 noa_num, index, i, noa_index = 0;
+	u8 *pos, *end, *ie;
+	u16 noa_len;
+	static u8 p2p_oui_ie_type[4] = {0x50, 0x6f, 0x9a, 0x09};
+
+	pos = (u8 *)&mgmt->u.action.category;
+	end = data + len;
+	ie = NULL;
+
+	if (pos[0] == 0x7f) {
+		if (memcmp(&pos[1], p2p_oui_ie_type, 4) == 0)
+			ie = pos + 3+4;
+	}
+
+	if (ie == NULL)
+		return;
+	find_p2p_ie = true;
+
+	RT_TRACE(COMP_FW, DBG_LOUD, ("action frame find P2P IE.\n"));
+	/*to find noa ie*/
+	while (ie + 1 < end) {
+		noa_len = READEF2BYTE((__le16 *)&ie[1]);
+		if (ie + 3 + ie[1] > end)
+			return;
+
+		if (ie[0] == 12) {
+			RT_TRACE(COMP_FW, DBG_LOUD, ("find NOA IE\n"));
+			RT_PRINT_DATA(rtlpriv, COMP_FW, DBG_LOUD, "noa ie ",
+				      ie, noa_len);
+			find_p2p_ps_ie = true;
+			if ((noa_len - 2) % 13 != 0) {
+				RT_TRACE(COMP_FW, DBG_LOUD,
+					 ("P2P notice of absence: invalid length%d\n",
+					 noa_len));
+				return;
+			} else {
+				noa_num = (noa_len - 2) / 13;
+			}
+			noa_index = ie[3];
+			if (rtlpriv->psc.p2p_ps_info.p2p_ps_mode ==
+			    P2P_PS_NONE ||
+			    noa_index != p2pinfo->noa_index) {
+				p2pinfo->noa_index = noa_index;
+				p2pinfo->opp_ps = (ie[4] >> 7);
+				p2pinfo->ctwindow = ie[4] & 0x7F;
+				p2pinfo->noa_num = noa_num;
+				index = 5;
+				for (i = 0; i < noa_num; i++) {
+					p2pinfo->noa_count_type[i] =
+							READEF1BYTE(ie+index);
+					index += 1;
+					p2pinfo->noa_duration[i] =
+						 READEF4BYTE((__le32 *)ie+index);
+					index += 4;
+					p2pinfo->noa_interval[i] =
+						 READEF4BYTE((__le32 *)ie+index);
+					index += 4;
+					p2pinfo->noa_start_time[i] =
+						 READEF4BYTE((__le32 *)ie+index);
+					index += 4;
+				}
+
+				if (p2pinfo->opp_ps == 1) {
+					p2pinfo->p2p_ps_mode = P2P_PS_CTWINDOW;
+					/* Driver should wait LPS
+					 * entering CTWindow */
+					if (rtlpriv->psc.b_fw_current_inpsmode) {
+						rtl92e_p2p_ps_cmd(hw,
+							       P2P_PS_ENABLE);
+					}
+				} else if (p2pinfo->noa_num > 0) {
+					p2pinfo->p2p_ps_mode = P2P_PS_NOA;
+					rtl92e_p2p_ps_cmd(hw, P2P_PS_ENABLE);
+				} else if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
+					rtl92e_p2p_ps_cmd(hw, P2P_PS_DISABLE);
+				}
+			}
+
+		break;
+		}
+		ie += 3 + noa_len;
+	}
+}
+
+void rtl92e_p2p_ps_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *rtlps = rtl_psc(rtl_priv(hw));
+	struct rtl_p2p_ps_info  *p2pinfo = &(rtlpriv->psc.p2p_ps_info);
+
+	RT_TRACE(COMP_FW, DBG_LOUD, ("p2p state %x\n", p2p_ps_state));
+	switch (p2p_ps_state) {
+	case P2P_PS_DISABLE:
+		p2pinfo->p2p_ps_state = p2p_ps_state;
+		rtlpriv->cfg->ops->set_hw_reg(hw,
+					   HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
+					   (u8 *)(&p2p_ps_state));
+
+		p2pinfo->noa_index = 0;
+		p2pinfo->ctwindow = 0;
+		p2pinfo->opp_ps = 0;
+		p2pinfo->noa_num = 0;
+		p2pinfo->p2p_ps_mode = P2P_PS_NONE;
+		if (rtlps->b_fw_current_inpsmode) {
+			if (rtlps->smart_ps == 0) {
+				rtlps->smart_ps = 2;
+				rtlpriv->cfg->ops->set_hw_reg(hw,
+					    HW_VAR_H2C_FW_PWRMODE,
+					    (u8 *)(&rtlps->pwr_mode));
+			}
+		}
+		break;
+	case P2P_PS_ENABLE:
+		if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
+			p2pinfo->p2p_ps_state = p2p_ps_state;
+
+			if (p2pinfo->ctwindow > 0) {
+				if (rtlps->smart_ps != 0) {
+					rtlps->smart_ps = 0;
+					rtlpriv->cfg->ops->set_hw_reg(
+					    hw, HW_VAR_H2C_FW_PWRMODE,
+					    (u8 *)(&rtlps->pwr_mode));
+				}
+			}
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+						HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
+						(u8 *)(&p2p_ps_state));
+			}
+			break;
+	case P2P_PS_SCAN:
+	case P2P_PS_SCAN_DONE:
+	case P2P_PS_ALLSTASLEEP:
+		if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
+			p2pinfo->p2p_ps_state = p2p_ps_state;
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+						HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
+						(u8 *)(&p2p_ps_state));
+		}
+		break;
+	default:
+		break;
+	}
+	RT_TRACE(COMP_FW, DBG_LOUD, (" ctwindow %x oppps %x\n",
+				     p2pinfo->ctwindow , p2pinfo->opp_ps));
+	RT_TRACE(COMP_FW, DBG_LOUD,
+		 ("count %x duration %x index %x interval %x start time %x noa num %x\n",
+		 p2pinfo->noa_count_type[0],
+		 p2pinfo->noa_duration[0],
+		 p2pinfo->noa_index,
+		 p2pinfo->noa_interval[0],
+		 p2pinfo->noa_start_time[0],
+		 p2pinfo->noa_num));
+	RT_TRACE(COMP_FW, DBG_LOUD, ("end\n"));
+}
+
+void rtl92e_p2p_info(struct ieee80211_hw *hw, void *data, unsigned int len)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct ieee80211_hdr *hdr = (void *)data;
+
+	if (!mac->p2p)
+		return;
+	if (mac->link_state != MAC80211_LINKED)
+		return;
+	/* min. beacon length + FCS_LEN */
+	if (len <= 40 + FCS_LEN)
+		return;
+
+	/* and only beacons from the associated BSSID, please */
+	if (!ether_addr_equal(hdr->addr3, rtlpriv->mac80211.bssid))
+		return;
+
+	/* check if this really is a beacon */
+	if (!(ieee80211_is_beacon(hdr->frame_control) ||
+	      ieee80211_is_probe_resp(hdr->frame_control) ||
+	      ieee80211_is_action(hdr->frame_control)))
+		return;
+
+	if (ieee80211_is_action(hdr->frame_control))
+		rtl_p2p_action_ie(hw , data , len - FCS_LEN);
+	else
+		rtl_p2p_noa_ie(hw , data , len - FCS_LEN);
+}
diff --git a/drivers/staging/rtl8192ee/ps.h b/drivers/staging/rtl8192ee/ps.h
new file mode 100644
index 00000000000..1533661a2f4
--- /dev/null
+++ b/drivers/staging/rtl8192ee/ps.h
@@ -0,0 +1,52 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __REALTEK_RTL_PCI_PS_H__
+#define __REALTEK_RTL_PCI_PS_H__
+
+#define MAX_SW_LPS_SLEEP_INTV	5
+
+bool stg_rtl_ps_set_rf_state(struct ieee80211_hw *hw,
+			     enum rf_pwrstate state_toset, u32 changesource,
+			     bool protect_or_not);
+bool stg_rtl_ps_enable_nic(struct ieee80211_hw *hw);
+bool stg_rtl_ps_disable_nic(struct ieee80211_hw *hw);
+void rtl92e_ips_nic_off(struct ieee80211_hw *hw);
+void rtl92e_ips_nic_on(struct ieee80211_hw *hw);
+void rtl92e_ips_nic_off_wq_callback(void *data);
+void rtl92e_lps_enter(struct ieee80211_hw *hw);
+void rtl92e_lps_leave(struct ieee80211_hw *hw);
+
+void rtl_lps_set_psmode(struct ieee80211_hw *hw, u8 rt_psmode);
+
+void rtl92e_swlps_beacon(struct ieee80211_hw *hw, void *data, unsigned int len);
+void rtl92e_swlps_wq_callback(void *data);
+void rtl92e_swlps_rfon_wq_callback(void *data);
+void rtl92e_swlps_rf_awake(struct ieee80211_hw *hw);
+void rtl92e_swlps_rf_sleep(struct ieee80211_hw *hw);
+void rtl92e_p2p_ps_cmd(struct ieee80211_hw *hw , u8 p2p_ps_state);
+void rtl92e_p2p_info(struct ieee80211_hw *hw, void *data, unsigned int len);
+
+#endif
diff --git a/drivers/staging/rtl8192ee/rc.c b/drivers/staging/rtl8192ee/rc.c
new file mode 100644
index 00000000000..c4c34ddcf8c
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rc.c
@@ -0,0 +1,288 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "wifi.h"
+#include "base.h"
+#include "rc.h"
+
+/*
+ *Finds the highest rate index we can use
+ *if skb is special data like DHCP/EAPOL, we set should
+ *it to lowest rate CCK_1M, otherwise we set rate to
+ *highest rate based on wireless mode used for iwconfig
+ *show Tx rate.
+ */
+static u8 _rtl_rc_get_highest_rix(struct rtl_priv *rtlpriv,
+				  struct ieee80211_sta *sta,
+				  struct sk_buff *skb, bool not_data)
+{
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_sta_info *sta_entry = NULL;
+	u8 wireless_mode = 0;
+
+	/*
+	 *this rate is no use for true rate, firmware
+	 *will control rate at all it just used for
+	 *1.show in iwconfig in B/G mode
+	 *2.in stg_rtl_get_tcb_desc when we check rate is
+	 *      1M we will not use FW rate but user rate.
+	 */
+
+	if (sta) {
+		sta_entry = (struct rtl_sta_info *)sta->drv_priv;
+		wireless_mode = sta_entry->wireless_mode;
+	}
+
+	if (rtl92e_is_special_data(rtlpriv->mac80211.hw, skb, true) ||
+	    not_data) {
+		return 0;
+	} else {
+		if (rtlhal->current_bandtype == BAND_ON_2_4G) {
+			if (wireless_mode == WIRELESS_MODE_B) {
+				return B_MODE_MAX_RIX;
+			} else if (wireless_mode == WIRELESS_MODE_G) {
+				return G_MODE_MAX_RIX;
+			} else if (wireless_mode == WIRELESS_MODE_N_24G) {
+				if (get_rf_type(rtlphy) != RF_2T2R)
+					return N_MODE_MCS7_RIX;
+				else
+					return N_MODE_MCS15_RIX;
+			} else if (wireless_mode == WIRELESS_MODE_AC_24G) {
+				return AC_MODE_MCS9_RIX;
+			} else {
+				return 0;
+			}
+		} else {
+			if (wireless_mode == WIRELESS_MODE_A) {
+				return A_MODE_MAX_RIX;
+			} else if (wireless_mode == WIRELESS_MODE_N_5G) {
+				if (get_rf_type(rtlphy) != RF_2T2R)
+					return N_MODE_MCS7_RIX;
+				else
+					return N_MODE_MCS15_RIX;
+			} else if (wireless_mode == WIRELESS_MODE_AC_5G) {
+				return AC_MODE_MCS9_RIX;
+			} else {
+				return 0;
+			}
+		}
+	}
+}
+
+static void _rtl_rc_rate_set_series(struct rtl_priv *rtlpriv,
+				    struct ieee80211_sta *sta,
+				    struct ieee80211_tx_rate *rate,
+				    struct ieee80211_tx_rate_control *txrc,
+				    u8 tries, char rix, int rtsctsenable,
+				    bool not_data)
+{
+	struct rtl_mac *mac = rtl_mac(rtlpriv);
+	u8 sgi_20 = 0, sgi_40 = 0, sgi_80 = 0;
+
+	if (sta) {
+		sgi_20 = sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20;
+		sgi_40 = sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40;
+		sgi_80 = sta->vht_cap.cap & IEEE80211_VHT_CAP_SHORT_GI_80;
+	}
+	rate->count = tries;
+	rate->idx = rix >= 0x00 ? rix : 0x00;
+
+	if (!not_data) {
+		if (txrc->short_preamble)
+			rate->flags |= IEEE80211_TX_RC_USE_SHORT_PREAMBLE;
+		if (mac->opmode == NL80211_IFTYPE_AP ||
+		    mac->opmode == NL80211_IFTYPE_ADHOC) {
+			if (sta && (sta->ht_cap.cap &
+				    IEEE80211_HT_CAP_SUP_WIDTH_20_40))
+				rate->flags |= IEEE80211_TX_RC_40_MHZ_WIDTH;
+			if (sta && (sta->vht_cap.vht_supported))
+				rate->flags |= IEEE80211_TX_RC_80_MHZ_WIDTH;
+		} else {
+			if (mac->bw_40)
+				rate->flags |= IEEE80211_TX_RC_40_MHZ_WIDTH;
+			if (mac->bw_80)
+				rate->flags |= IEEE80211_TX_RC_80_MHZ_WIDTH;
+		}
+
+		if (sgi_20 || sgi_40 || sgi_80)
+			rate->flags |= IEEE80211_TX_RC_SHORT_GI;
+		if (sta && sta->ht_cap.ht_supported)
+			rate->flags |= IEEE80211_TX_RC_MCS;
+		if (sta && sta->vht_cap.vht_supported)
+			rate->flags |= IEEE80211_TX_RC_VHT_MCS;
+	}
+}
+
+static void rtl_get_rate(void *ppriv, struct ieee80211_sta *sta,
+			 void *priv_sta,
+			 struct ieee80211_tx_rate_control *txrc)
+{
+	struct rtl_priv *rtlpriv = ppriv;
+	struct sk_buff *skb = txrc->skb;
+	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
+	struct ieee80211_tx_rate *rates = tx_info->control.rates;
+	__le16 fc = rtl_get_fc(skb);
+	u8 try_per_rate, i, rix;
+	bool not_data = !ieee80211_is_data(fc);
+
+	if (rate_control_send_low(sta, priv_sta, txrc))
+		return;
+
+	rix = _rtl_rc_get_highest_rix(rtlpriv, sta, skb, not_data);
+	try_per_rate = 1;
+	_rtl_rc_rate_set_series(rtlpriv, sta, &rates[0], txrc,
+				try_per_rate, rix, 1, not_data);
+
+	if (!not_data) {
+		for (i = 1; i < 4; i++)
+			_rtl_rc_rate_set_series(rtlpriv, sta, &rates[i],
+						txrc, i, (rix - i), 1,
+						not_data);
+	}
+}
+
+static bool _rtl_tx_aggr_check(struct rtl_priv *rtlpriv,
+			       struct rtl_sta_info *sta_entry, u16 tid)
+{
+	struct rtl_mac *mac = rtl_mac(rtlpriv);
+
+	if (mac->act_scanning)
+		return false;
+
+	if (mac->opmode == NL80211_IFTYPE_STATION &&
+	    mac->cnt_after_linked < 3)
+		return false;
+
+	if (sta_entry->tids[tid].agg.agg_state == RTL_AGG_STOP)
+		return true;
+
+	return false;
+}
+
+/*mac80211 Rate Control callbacks*/
+static void rtl_tx_status(void *ppriv,
+			  struct ieee80211_supported_band *sband,
+			  struct ieee80211_sta *sta, void *priv_sta,
+			  struct sk_buff *skb)
+{
+	struct rtl_priv *rtlpriv = ppriv;
+	struct rtl_mac *mac = rtl_mac(rtlpriv);
+	struct ieee80211_hdr *hdr = rtl_get_hdr(skb);
+	__le16 fc = rtl_get_fc(skb);
+	struct rtl_sta_info *sta_entry;
+
+	if (!priv_sta || !ieee80211_is_data(fc))
+		return;
+
+	if (rtl92e_is_special_data(mac->hw, skb, true))
+		return;
+
+	if (is_multicast_ether_addr(ieee80211_get_DA(hdr)) ||
+	    is_broadcast_ether_addr(ieee80211_get_DA(hdr)))
+		return;
+
+	if (sta) {
+		/* Check if aggregation has to be enabled for this tid */
+		sta_entry = (struct rtl_sta_info *)sta->drv_priv;
+		if ((sta->ht_cap.ht_supported) &&
+		    !(skb->protocol == cpu_to_be16(ETH_P_PAE))) {
+			if (ieee80211_is_data_qos(fc)) {
+				u8 tid = rtl_get_tid(skb);
+				if (_rtl_tx_aggr_check(rtlpriv, sta_entry,
+						       tid)) {
+					sta_entry->tids[tid].agg.agg_state =
+						RTL_AGG_PROGRESS;
+					ieee80211_start_tx_ba_session(sta, tid,
+								      5000);
+				}
+			}
+		}
+	}
+}
+
+static void rtl_rate_init(void *ppriv,
+			  struct ieee80211_supported_band *sband,
+			  struct cfg80211_chan_def *chandef,
+			  struct ieee80211_sta *sta, void *priv_sta)
+{
+}
+
+static void *rtl_rate_alloc(struct ieee80211_hw *hw, struct dentry *debugfsdir)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	return rtlpriv;
+}
+
+static void rtl_rate_free(void *rtlpriv)
+{
+	return;
+}
+
+static void *rtl_rate_alloc_sta(void *ppriv,
+				struct ieee80211_sta *sta, gfp_t gfp)
+{
+	struct rtl_priv *rtlpriv = ppriv;
+	struct rtl_rate_priv *rate_priv;
+
+	rate_priv = kzalloc(sizeof(*rate_priv), gfp);
+	if (!rate_priv) {
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("Unable to allocate private rc structure\n"));
+		return NULL;
+	}
+
+	rtlpriv->rate_priv = rate_priv;
+
+	return rate_priv;
+}
+
+static void rtl_rate_free_sta(void *rtlpriv,
+			      struct ieee80211_sta *sta, void *priv_sta)
+{
+	struct rtl_rate_priv *rate_priv = priv_sta;
+	kfree(rate_priv);
+}
+
+static struct rate_control_ops rtl_rate_ops = {
+	.name = "rtl_rc_92e",
+	.alloc = rtl_rate_alloc,
+	.free = rtl_rate_free,
+	.alloc_sta = rtl_rate_alloc_sta,
+	.free_sta = rtl_rate_free_sta,
+	.rate_init = rtl_rate_init,
+	.tx_status = rtl_tx_status,
+	.get_rate = rtl_get_rate,
+};
+
+int rtl92e_rate_control_register(void)
+{
+	return ieee80211_rate_control_register(&rtl_rate_ops);
+}
+
+void rtl92e_rate_control_unregister(void)
+{
+	ieee80211_rate_control_unregister(&rtl_rate_ops);
+}
diff --git a/drivers/staging/rtl8192ee/rc.h b/drivers/staging/rtl8192ee/rc.h
new file mode 100644
index 00000000000..928f570b4b8
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rc.h
@@ -0,0 +1,47 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL_RC_H__
+#define __RTL_RC_H__
+
+#define B_MODE_MAX_RIX 3
+#define G_MODE_MAX_RIX 11
+#define A_MODE_MAX_RIX 7
+
+/* in mac80211 mcs0-mcs15 is idx0-idx15*/
+#define N_MODE_MCS7_RIX 7
+#define N_MODE_MCS15_RIX 15
+
+#define AC_MODE_MCS7_RIX 7
+#define AC_MODE_MCS8_RIX 8
+#define AC_MODE_MCS9_RIX 9
+
+struct rtl_rate_priv {
+	u8 ht_cap;
+};
+
+int rtl92e_rate_control_register(void);
+void rtl92e_rate_control_unregister(void);
+#endif
diff --git a/drivers/staging/rtl8192ee/regd.c b/drivers/staging/rtl8192ee/regd.c
new file mode 100644
index 00000000000..7272fae68ec
--- /dev/null
+++ b/drivers/staging/rtl8192ee/regd.c
@@ -0,0 +1,448 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "wifi.h"
+#include "regd.h"
+
+static struct country_code_to_enum_rd allcountries[] = {
+	{COUNTRY_CODE_FCC, "US"},
+	{COUNTRY_CODE_IC, "US"},
+	{COUNTRY_CODE_ETSI, "EC"},
+	{COUNTRY_CODE_SPAIN, "EC"},
+	{COUNTRY_CODE_FRANCE, "EC"},
+	{COUNTRY_CODE_MKK, "JP"},
+	{COUNTRY_CODE_MKK1, "JP"},
+	{COUNTRY_CODE_ISRAEL, "EC"},
+	{COUNTRY_CODE_TELEC, "JP"},
+	{COUNTRY_CODE_MIC, "JP"},
+	{COUNTRY_CODE_GLOBAL_DOMAIN, "JP"},
+	{COUNTRY_CODE_WORLD_WIDE_13, "EC"},
+	{COUNTRY_CODE_TELEC_NETGEAR, "EC"},
+};
+
+/*
+ *Only these channels all allow active
+ *scan on all world regulatory domains
+ */
+#define RTL819x_2GHZ_CH01_11	\
+	REG_RULE(2412-10, 2462+10, 40, 0, 20, 0)
+
+/*
+ *We enable active scan on these a case
+ *by case basis by regulatory domain
+ */
+#define RTL819x_2GHZ_CH12_13	\
+	REG_RULE(2467-10, 2472+10, 40, 0, 20,\
+	NL80211_RRF_PASSIVE_SCAN)
+
+#define RTL819x_2GHZ_CH14	\
+	REG_RULE(2484-10, 2484+10, 40, 0, 20, \
+	NL80211_RRF_PASSIVE_SCAN | \
+	NL80211_RRF_NO_OFDM)
+
+/* 5G chan 36 - chan 64*/
+#define RTL819x_5GHZ_5150_5350	\
+	REG_RULE(5150-10, 5350+10, 80, 0, 30, \
+	NL80211_RRF_PASSIVE_SCAN | \
+	NL80211_RRF_NO_IBSS)
+
+/* 5G chan 100 - chan 165*/
+#define RTL819x_5GHZ_5470_5850	\
+	REG_RULE(5470-10, 5850+10, 80, 0, 30, \
+	NL80211_RRF_PASSIVE_SCAN | \
+	NL80211_RRF_NO_IBSS)
+
+/* 5G chan 149 - chan 165*/
+#define RTL819x_5GHZ_5725_5850	\
+	REG_RULE(5725-10, 5850+10, 80, 0, 30, \
+	NL80211_RRF_PASSIVE_SCAN | \
+	NL80211_RRF_NO_IBSS)
+
+#define RTL819x_5GHZ_ALL	\
+	(RTL819x_5GHZ_5150_5350, RTL819x_5GHZ_5470_5850)
+
+static const struct ieee80211_regdomain rtl_regdom_11 = {
+	.n_reg_rules = 1,
+	.alpha2 = "99",
+	.reg_rules = {
+		      RTL819x_2GHZ_CH01_11,
+		      }
+};
+
+static const struct ieee80211_regdomain rtl_regdom_12_13 = {
+	.n_reg_rules = 2,
+	.alpha2 = "99",
+	.reg_rules = {
+		      RTL819x_2GHZ_CH01_11,
+			  RTL819x_2GHZ_CH12_13,
+		      }
+};
+
+static const struct ieee80211_regdomain rtl_regdom_no_midband = {
+	.n_reg_rules = 3,
+	.alpha2 = "99",
+	.reg_rules = {
+		      RTL819x_2GHZ_CH01_11,
+			  RTL819x_5GHZ_5150_5350,
+			  RTL819x_5GHZ_5725_5850,
+		      }
+};
+
+static const struct ieee80211_regdomain rtl_regdom_60_64 = {
+	.n_reg_rules = 3,
+	.alpha2 = "99",
+	.reg_rules = {
+		      RTL819x_2GHZ_CH01_11,
+			  RTL819x_2GHZ_CH12_13,
+			  RTL819x_5GHZ_5725_5850,
+		      }
+};
+
+static const struct ieee80211_regdomain rtl_regdom_14_60_64 = {
+	.n_reg_rules = 4,
+	.alpha2 = "99",
+	.reg_rules = {
+		      RTL819x_2GHZ_CH01_11,
+			  RTL819x_2GHZ_CH12_13,
+			  RTL819x_2GHZ_CH14,
+			  RTL819x_5GHZ_5725_5850,
+		      }
+};
+
+static const struct ieee80211_regdomain rtl_regdom_14 = {
+	.n_reg_rules = 3,
+	.alpha2 = "99",
+	.reg_rules = {
+		      RTL819x_2GHZ_CH01_11,
+			  RTL819x_2GHZ_CH12_13,
+			  RTL819x_2GHZ_CH14,
+		      }
+};
+
+static bool _rtl_is_radar_freq(u16 center_freq)
+{
+	return center_freq >= 5260 && center_freq <= 5700;
+}
+
+static void _rtl_reg_apply_beaconing_flags(struct wiphy *wiphy,
+					   enum nl80211_reg_initiator initiator)
+{
+	enum ieee80211_band band;
+	struct ieee80211_supported_band *sband;
+	const struct ieee80211_reg_rule *reg_rule;
+	struct ieee80211_channel *ch;
+	unsigned int i;
+
+	for (band = 0; band < IEEE80211_NUM_BANDS; band++) {
+		if (!wiphy->bands[band])
+			continue;
+
+		sband = wiphy->bands[band];
+
+		for (i = 0; i < sband->n_channels; i++) {
+			ch = &sband->channels[i];
+			if (_rtl_is_radar_freq(ch->center_freq) ||
+			    (ch->flags & IEEE80211_CHAN_RADAR))
+				continue;
+			if (initiator == NL80211_REGDOM_SET_BY_COUNTRY_IE) {
+				reg_rule = freq_reg_info(wiphy,
+							 ch->center_freq);
+				if (IS_ERR(reg_rule))
+					continue;
+
+				/*
+				 *If 11d had a rule for this channel ensure
+				 *we enable adhoc/beaconing if it allows us to
+				 *use it. Note that we would have disabled it
+				 *by applying our static world regdomain by
+				 *default during init, prior to calling our
+				 *regulatory_hint().
+				 */
+
+				if (!(reg_rule->flags & NL80211_RRF_NO_IBSS))
+					ch->flags &= ~IEEE80211_CHAN_NO_IBSS;
+				if (!(reg_rule->flags &
+				      NL80211_RRF_PASSIVE_SCAN))
+					ch->flags &=
+					    ~IEEE80211_CHAN_PASSIVE_SCAN;
+			} else {
+				if (ch->beacon_found)
+					ch->flags &= ~(IEEE80211_CHAN_NO_IBSS |
+						   IEEE80211_CHAN_PASSIVE_SCAN);
+			}
+		}
+	}
+}
+
+/* Allows active scan scan on Ch 12 and 13 */
+static void _rtl_reg_apply_active_scan_flags(struct wiphy *wiphy,
+					     enum nl80211_reg_initiator
+					     initiator)
+{
+	struct ieee80211_supported_band *sband;
+	struct ieee80211_channel *ch;
+	const struct ieee80211_reg_rule *reg_rule;
+
+	if (!wiphy->bands[IEEE80211_BAND_2GHZ])
+		return;
+	sband = wiphy->bands[IEEE80211_BAND_2GHZ];
+
+	/*
+	 *If no country IE has been received always enable active scan
+	 *on these channels. This is only done for specific regulatory SKUs
+	 */
+	if (initiator != NL80211_REGDOM_SET_BY_COUNTRY_IE) {
+		ch = &sband->channels[11];	/* CH 12 */
+		if (ch->flags & IEEE80211_CHAN_PASSIVE_SCAN)
+			ch->flags &= ~IEEE80211_CHAN_PASSIVE_SCAN;
+		ch = &sband->channels[12];	/* CH 13 */
+		if (ch->flags & IEEE80211_CHAN_PASSIVE_SCAN)
+			ch->flags &= ~IEEE80211_CHAN_PASSIVE_SCAN;
+		return;
+	}
+
+	/*
+	 *If a country IE has been recieved check its rule for this
+	 *channel first before enabling active scan. The passive scan
+	 *would have been enforced by the initial processing of our
+	 *custom regulatory domain.
+	 */
+
+	ch = &sband->channels[11];	/* CH 12 */
+	reg_rule = freq_reg_info(wiphy, ch->center_freq);
+	if (!IS_ERR(reg_rule)) {
+		if (!(reg_rule->flags & NL80211_RRF_PASSIVE_SCAN))
+			if (ch->flags & IEEE80211_CHAN_PASSIVE_SCAN)
+				ch->flags &= ~IEEE80211_CHAN_PASSIVE_SCAN;
+	}
+
+	ch = &sband->channels[12];	/* CH 13 */
+	reg_rule = freq_reg_info(wiphy, ch->center_freq);
+	if (!IS_ERR(reg_rule)) {
+		if (!(reg_rule->flags & NL80211_RRF_PASSIVE_SCAN))
+			if (ch->flags & IEEE80211_CHAN_PASSIVE_SCAN)
+				ch->flags &= ~IEEE80211_CHAN_PASSIVE_SCAN;
+	}
+}
+
+/*
+ *Always apply Radar/DFS rules on
+ *freq range 5260 MHz - 5700 MHz
+ */
+static void _rtl_reg_apply_radar_flags(struct wiphy *wiphy)
+{
+	struct ieee80211_supported_band *sband;
+	struct ieee80211_channel *ch;
+	unsigned int i;
+
+	if (!wiphy->bands[IEEE80211_BAND_5GHZ])
+		return;
+
+	sband = wiphy->bands[IEEE80211_BAND_5GHZ];
+
+	for (i = 0; i < sband->n_channels; i++) {
+		ch = &sband->channels[i];
+		if (!_rtl_is_radar_freq(ch->center_freq))
+			continue;
+
+		/*
+		 *We always enable radar detection/DFS on this
+		 *frequency range. Additionally we also apply on
+		 *this frequency range:
+		 *- If STA mode does not yet have DFS supports disable
+		 * active scanning
+		 *- If adhoc mode does not support DFS yet then disable
+		 * adhoc in the frequency.
+		 *- If AP mode does not yet support radar detection/DFS
+		 *do not allow AP mode
+		 */
+		if (!(ch->flags & IEEE80211_CHAN_DISABLED))
+			ch->flags |= IEEE80211_CHAN_RADAR |
+			    IEEE80211_CHAN_NO_IBSS |
+			    IEEE80211_CHAN_PASSIVE_SCAN;
+	}
+}
+
+static void _rtl_reg_apply_world_flags(struct wiphy *wiphy,
+				       enum nl80211_reg_initiator initiator,
+				       struct rtl_regulatory *reg)
+{
+	_rtl_reg_apply_beaconing_flags(wiphy, initiator);
+	_rtl_reg_apply_active_scan_flags(wiphy, initiator);
+	return;
+}
+
+static void _rtl_dump_channel_map(struct wiphy *wiphy)
+{
+	enum ieee80211_band band;
+	struct ieee80211_supported_band *sband;
+	struct ieee80211_channel *ch;
+	unsigned int i;
+
+	for (band = 0; band < IEEE80211_NUM_BANDS; band++) {
+		if (!wiphy->bands[band])
+			continue;
+		sband = wiphy->bands[band];
+		for (i = 0; i < sband->n_channels; i++)
+			ch = &sband->channels[i];
+	}
+}
+
+static int _rtl92e_reg_notifier_apply(struct wiphy *wiphy,
+				      struct regulatory_request *request,
+				      struct rtl_regulatory *reg)
+{
+	/* We always apply this */
+	_rtl_reg_apply_radar_flags(wiphy);
+
+	switch (request->initiator) {
+	case NL80211_REGDOM_SET_BY_DRIVER:
+	case NL80211_REGDOM_SET_BY_CORE:
+	case NL80211_REGDOM_SET_BY_USER:
+		break;
+	case NL80211_REGDOM_SET_BY_COUNTRY_IE:
+		_rtl_reg_apply_world_flags(wiphy, request->initiator, reg);
+		break;
+	}
+
+	_rtl_dump_channel_map(wiphy);
+
+	return 0;
+}
+
+static const struct ieee80211_regdomain *_rtl_regdomain_select(
+						struct rtl_regulatory *reg)
+{
+	switch (reg->country_code) {
+	case COUNTRY_CODE_FCC:
+		return &rtl_regdom_no_midband;
+	case COUNTRY_CODE_IC:
+		return &rtl_regdom_11;
+	case COUNTRY_CODE_ETSI:
+	case COUNTRY_CODE_TELEC_NETGEAR:
+		return &rtl_regdom_60_64;
+	case COUNTRY_CODE_SPAIN:
+	case COUNTRY_CODE_FRANCE:
+	case COUNTRY_CODE_ISRAEL:
+	case COUNTRY_CODE_WORLD_WIDE_13:
+		return &rtl_regdom_12_13;
+	case COUNTRY_CODE_MKK:
+	case COUNTRY_CODE_MKK1:
+	case COUNTRY_CODE_TELEC:
+	case COUNTRY_CODE_MIC:
+		return &rtl_regdom_14_60_64;
+	case COUNTRY_CODE_GLOBAL_DOMAIN:
+		return &rtl_regdom_14;
+	default:
+		return &rtl_regdom_no_midband;
+	}
+}
+
+static int _rtl92e_regd_init_wiphy(struct rtl_regulatory *reg,
+				   struct wiphy *wiphy,
+				   void (*reg_notifier)(struct wiphy *wiphy,
+						        struct regulatory_request *
+						        request))
+{
+	const struct ieee80211_regdomain *regd;
+
+	wiphy->reg_notifier = reg_notifier;
+
+	wiphy->regulatory_flags |= REGULATORY_CUSTOM_REG;
+	wiphy->regulatory_flags &= ~REGULATORY_STRICT_REG;
+	wiphy->regulatory_flags &= ~REGULATORY_DISABLE_BEACON_HINTS;
+
+	regd = _rtl_regdomain_select(reg);
+	wiphy_apply_custom_regulatory(wiphy, regd);
+	_rtl_reg_apply_radar_flags(wiphy);
+	_rtl_reg_apply_world_flags(wiphy, NL80211_REGDOM_SET_BY_DRIVER, reg);
+	return 0;
+}
+
+static struct country_code_to_enum_rd *_rtl_regd_find_country(u16 countrycode)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(allcountries); i++) {
+		if (allcountries[i].countrycode == countrycode)
+			return &allcountries[i];
+	}
+	return NULL;
+}
+
+int rtl92e_regd_init(struct ieee80211_hw *hw,
+		     void (*reg_notifier)(struct wiphy *wiphy,
+					  struct regulatory_request *request))
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct wiphy *wiphy = hw->wiphy;
+	struct country_code_to_enum_rd *country = NULL;
+
+	if (wiphy == NULL || &rtlpriv->regd == NULL)
+		return -EINVAL;
+
+	/* init country_code from efuse channel plan */
+	rtlpriv->regd.country_code = rtlpriv->efuse.channel_plan;
+
+	RT_TRACE(COMP_REGD, DBG_TRACE,
+		 (KERN_DEBUG "rtl: EEPROM regdomain: 0x%0x\n",
+		  rtlpriv->regd.country_code));
+
+	if (rtlpriv->regd.country_code >= COUNTRY_CODE_MAX) {
+		RT_TRACE(COMP_REGD, DBG_DMESG,
+			 ("rtl: EEPROM indicates invalid contry code world wide 13 should be used\n"));
+
+		rtlpriv->regd.country_code = COUNTRY_CODE_WORLD_WIDE_13;
+	}
+
+	country = _rtl_regd_find_country(rtlpriv->regd.country_code);
+
+	if (country) {
+		rtlpriv->regd.alpha2[0] = country->iso_name[0];
+		rtlpriv->regd.alpha2[1] = country->iso_name[1];
+	} else {
+		rtlpriv->regd.alpha2[0] = '0';
+		rtlpriv->regd.alpha2[1] = '0';
+	}
+
+	RT_TRACE(COMP_REGD, DBG_TRACE,
+		 (KERN_DEBUG "rtl: Country alpha2 being used: %c%c\n",
+		  rtlpriv->regd.alpha2[0], rtlpriv->regd.alpha2[1]));
+
+	_rtl92e_regd_init_wiphy(&rtlpriv->regd, wiphy, reg_notifier);
+
+	return 0;
+}
+
+void rtl92e_reg_notifier(struct wiphy *wiphy,
+			 struct regulatory_request *request)
+{
+	struct ieee80211_hw *hw = wiphy_to_ieee80211_hw(wiphy);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	RT_TRACE(COMP_REGD, DBG_LOUD, ("\n"));
+
+	_rtl92e_reg_notifier_apply(wiphy, request, &rtlpriv->regd);
+}
diff --git a/drivers/staging/rtl8192ee/regd.h b/drivers/staging/rtl8192ee/regd.h
new file mode 100644
index 00000000000..1f26f0e5261
--- /dev/null
+++ b/drivers/staging/rtl8192ee/regd.h
@@ -0,0 +1,63 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL_REGD_H__
+#define __RTL_REGD_H__
+
+/* for kernel 3.14 , both value are changed to IEEE80211_CHAN_NO_IR*/
+#define IEEE80211_CHAN_NO_IBSS IEEE80211_CHAN_NO_IR
+#define IEEE80211_CHAN_PASSIVE_SCAN IEEE80211_CHAN_NO_IR
+
+struct country_code_to_enum_rd {
+	u16 countrycode;
+	const char *iso_name;
+};
+
+enum country_code_type_t {
+	COUNTRY_CODE_FCC = 0,
+	COUNTRY_CODE_IC = 1,
+	COUNTRY_CODE_ETSI = 2,
+	COUNTRY_CODE_SPAIN = 3,
+	COUNTRY_CODE_FRANCE = 4,
+	COUNTRY_CODE_MKK = 5,
+	COUNTRY_CODE_MKK1 = 6,
+	COUNTRY_CODE_ISRAEL = 7,
+	COUNTRY_CODE_TELEC = 8,
+	COUNTRY_CODE_MIC = 9,
+	COUNTRY_CODE_GLOBAL_DOMAIN = 10,
+	COUNTRY_CODE_WORLD_WIDE_13 = 11,
+	COUNTRY_CODE_TELEC_NETGEAR = 12,
+
+	/*add new channel plan above this line */
+	COUNTRY_CODE_MAX
+};
+
+int rtl92e_regd_init(struct ieee80211_hw *hw,
+		     void (*reg_notifier)(struct wiphy *wiphy,
+					  struct regulatory_request *request));
+void rtl92e_reg_notifier(struct wiphy *wiphy,
+			 struct regulatory_request *request);
+
+#endif
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/def.h b/drivers/staging/rtl8192ee/rtl8192ee/def.h
new file mode 100644
index 00000000000..7566c1e8bae
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/def.h
@@ -0,0 +1,106 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92E_DEF_H__
+#define __RTL92E_DEF_H__
+
+#define RX_DESC_NUM_92E					512
+
+#define HAL_PRIME_CHNL_OFFSET_DONT_CARE			0
+#define HAL_PRIME_CHNL_OFFSET_LOWER			1
+#define HAL_PRIME_CHNL_OFFSET_UPPER			2
+
+#define RX_MPDU_QUEUE					0
+
+#define IS_HT_RATE(_rate)	\
+	(_rate >= DESC92C_RATEMCS0)
+#define IS_CCK_RATE(_rate)	\
+	(_rate >= DESC92C_RATE1M && _rate <= DESC92C_RATE11M)
+#define IS_OFDM_RATE(_rate)	\
+	(_rate >= DESC92C_RATE6M && _rate <= DESC92C_RATE54M)
+
+
+enum version_8192e {
+	VERSION_TEST_CHIP_2T2R_8192E = 0x0024,
+	VERSION_NORMAL_CHIP_2T2R_8192E = 0x102C,
+	VERSION_UNKNOWN = 0xFF,
+};
+
+enum rx_packet_type {
+	NORMAL_RX,
+	TX_REPORT1,
+	TX_REPORT2,
+	HIS_REPORT,
+	C2H_PACKET,
+};
+
+enum rtl_desc_qsel {
+	QSLT_BK = 0x2,
+	QSLT_BE = 0x0,
+	QSLT_VI = 0x5,
+	QSLT_VO = 0x7,
+	QSLT_BEACON = 0x10,
+	QSLT_HIGH = 0x11,
+	QSLT_MGNT = 0x12,
+	QSLT_CMD = 0x13,
+};
+
+enum rtl_desc92c_rate {
+	DESC92C_RATE1M = 0x00,
+	DESC92C_RATE2M = 0x01,
+	DESC92C_RATE5_5M = 0x02,
+	DESC92C_RATE11M = 0x03,
+
+	DESC92C_RATE6M = 0x04,
+	DESC92C_RATE9M = 0x05,
+	DESC92C_RATE12M = 0x06,
+	DESC92C_RATE18M = 0x07,
+	DESC92C_RATE24M = 0x08,
+	DESC92C_RATE36M = 0x09,
+	DESC92C_RATE48M = 0x0a,
+	DESC92C_RATE54M = 0x0b,
+
+	DESC92C_RATEMCS0 = 0x0c,
+	DESC92C_RATEMCS1 = 0x0d,
+	DESC92C_RATEMCS2 = 0x0e,
+	DESC92C_RATEMCS3 = 0x0f,
+	DESC92C_RATEMCS4 = 0x10,
+	DESC92C_RATEMCS5 = 0x11,
+	DESC92C_RATEMCS6 = 0x12,
+	DESC92C_RATEMCS7 = 0x13,
+	DESC92C_RATEMCS8 = 0x14,
+	DESC92C_RATEMCS9 = 0x15,
+	DESC92C_RATEMCS10 = 0x16,
+	DESC92C_RATEMCS11 = 0x17,
+	DESC92C_RATEMCS12 = 0x18,
+	DESC92C_RATEMCS13 = 0x19,
+	DESC92C_RATEMCS14 = 0x1a,
+	DESC92C_RATEMCS15 = 0x1b,
+};
+#endif
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/dm.c b/drivers/staging/rtl8192ee/rtl8192ee/dm.c
new file mode 100644
index 00000000000..41c2d98e81d
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/dm.c
@@ -0,0 +1,1258 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../base.h"
+#include "../pci.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "dm.h"
+#include "fw.h"
+#include "trx.h"
+
+struct dig_t dm_dig;
+
+static const u32 ofdmswing_table[OFDM_TABLE_SIZE] = {
+	0x7f8001fe,		/* 0, +6.0dB */
+	0x788001e2,		/* 1, +5.5dB */
+	0x71c001c7,		/* 2, +5.0dB */
+	0x6b8001ae,		/* 3, +4.5dB */
+	0x65400195,		/* 4, +4.0dB */
+	0x5fc0017f,		/* 5, +3.5dB */
+	0x5a400169,		/* 6, +3.0dB */
+	0x55400155,		/* 7, +2.5dB */
+	0x50800142,		/* 8, +2.0dB */
+	0x4c000130,		/* 9, +1.5dB */
+	0x47c0011f,		/* 10, +1.0dB */
+	0x43c0010f,		/* 11, +0.5dB */
+	0x40000100,		/* 12, +0dB */
+	0x3c8000f2,		/* 13, -0.5dB */
+	0x390000e4,		/* 14, -1.0dB */
+	0x35c000d7,		/* 15, -1.5dB */
+	0x32c000cb,		/* 16, -2.0dB */
+	0x300000c0,		/* 17, -2.5dB */
+	0x2d4000b5,		/* 18, -3.0dB */
+	0x2ac000ab,		/* 19, -3.5dB */
+	0x288000a2,		/* 20, -4.0dB */
+	0x26000098,		/* 21, -4.5dB */
+	0x24000090,		/* 22, -5.0dB */
+	0x22000088,		/* 23, -5.5dB */
+	0x20000080,		/* 24, -6.0dB */
+	0x1e400079,		/* 25, -6.5dB */
+	0x1c800072,		/* 26, -7.0dB */
+	0x1b00006c,		/* 27. -7.5dB */
+	0x19800066,		/* 28, -8.0dB */
+	0x18000060,		/* 29, -8.5dB */
+	0x16c0005b,		/* 30, -9.0dB */
+	0x15800056,		/* 31, -9.5dB */
+	0x14400051,		/* 32, -10.0dB */
+	0x1300004c,		/* 33, -10.5dB */
+	0x12000048,		/* 34, -11.0dB */
+	0x11000044,		/* 35, -11.5dB */
+	0x10000040,		/* 36, -12.0dB */
+	0x0f00003c,		/* 37, -12.5dB */
+	0x0e400039,		/* 38, -13.0dB */
+	0x0d800036,		/* 39, -13.5dB */
+	0x0cc00033,		/* 40, -14.0dB */
+	0x0c000030,		/* 41, -14.5dB */
+	0x0b40002d,		/* 42, -15.0dB */
+};
+
+static const u8 cckswing_table_ch1ch13[CCK_TABLE_SIZE][8] = {
+	{0x36, 0x35, 0x2e, 0x25, 0x1c, 0x12, 0x09, 0x04}, /* 0, +0dB */
+	{0x33, 0x32, 0x2b, 0x23, 0x1a, 0x11, 0x08, 0x04}, /* 1, -0.5dB */
+	{0x30, 0x2f, 0x29, 0x21, 0x19, 0x10, 0x08, 0x03}, /* 2, -1.0dB */
+	{0x2d, 0x2d, 0x27, 0x1f, 0x18, 0x0f, 0x08, 0x03}, /* 3, -1.5dB */
+	{0x2b, 0x2a, 0x25, 0x1e, 0x16, 0x0e, 0x07, 0x03}, /* 4, -2.0dB */
+	{0x28, 0x28, 0x22, 0x1c, 0x15, 0x0d, 0x07, 0x03}, /* 5, -2.5dB */
+	{0x26, 0x25, 0x21, 0x1b, 0x14, 0x0d, 0x06, 0x03}, /* 6, -3.0dB */
+	{0x24, 0x23, 0x1f, 0x19, 0x13, 0x0c, 0x06, 0x03}, /* 7, -3.5dB */
+	{0x22, 0x21, 0x1d, 0x18, 0x11, 0x0b, 0x06, 0x02}, /* 8, -4.0dB */
+	{0x20, 0x20, 0x1b, 0x16, 0x11, 0x08, 0x05, 0x02}, /* 9, -4.5dB */
+	{0x1f, 0x1e, 0x1a, 0x15, 0x10, 0x0a, 0x05, 0x02}, /* 10, -5.0dB */
+	{0x1d, 0x1c, 0x18, 0x14, 0x0f, 0x0a, 0x05, 0x02}, /* 11, -5.5dB */
+	{0x1b, 0x1a, 0x17, 0x13, 0x0e, 0x09, 0x04, 0x02}, /* 12, -6.0dB */
+	{0x1a, 0x19, 0x16, 0x12, 0x0d, 0x09, 0x04, 0x02}, /* 13, -6.5dB */
+	{0x18, 0x17, 0x15, 0x11, 0x0c, 0x08, 0x04, 0x02}, /* 14, -7.0dB */
+	{0x17, 0x16, 0x13, 0x10, 0x0c, 0x08, 0x04, 0x02}, /* 15, -7.5dB */
+	{0x16, 0x15, 0x12, 0x0f, 0x0b, 0x07, 0x04, 0x01}, /* 16, -8.0dB */
+	{0x14, 0x14, 0x11, 0x0e, 0x0b, 0x07, 0x03, 0x02}, /* 17, -8.5dB */
+	{0x13, 0x13, 0x10, 0x0d, 0x0a, 0x06, 0x03, 0x01}, /* 18, -9.0dB */
+	{0x12, 0x12, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01}, /* 19, -9.5dB */
+	{0x11, 0x11, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01}, /* 20, -10.0dB */
+	{0x10, 0x10, 0x0e, 0x0b, 0x08, 0x05, 0x03, 0x01}, /* 21, -10.5dB */
+	{0x0f, 0x0f, 0x0d, 0x0b, 0x08, 0x05, 0x03, 0x01}, /* 22, -11.0dB */
+	{0x0e, 0x0e, 0x0c, 0x0a, 0x08, 0x05, 0x02, 0x01}, /* 23, -11.5dB */
+	{0x0d, 0x0d, 0x0c, 0x0a, 0x07, 0x05, 0x02, 0x01}, /* 24, -12.0dB */
+	{0x0d, 0x0c, 0x0b, 0x09, 0x07, 0x04, 0x02, 0x01}, /* 25, -12.5dB */
+	{0x0c, 0x0c, 0x0a, 0x09, 0x06, 0x04, 0x02, 0x01}, /* 26, -13.0dB */
+	{0x0b, 0x0b, 0x0a, 0x08, 0x06, 0x04, 0x02, 0x01}, /* 27, -13.5dB */
+	{0x0b, 0x0a, 0x09, 0x08, 0x06, 0x04, 0x02, 0x01}, /* 28, -14.0dB */
+	{0x0a, 0x0a, 0x09, 0x07, 0x05, 0x03, 0x02, 0x01}, /* 29, -14.5dB */
+	{0x0a, 0x09, 0x08, 0x07, 0x05, 0x03, 0x02, 0x01}, /* 30, -15.0dB */
+	{0x09, 0x09, 0x08, 0x06, 0x05, 0x03, 0x01, 0x01}, /* 31, -15.5dB */
+	{0x09, 0x08, 0x07, 0x06, 0x04, 0x03, 0x01, 0x01}  /* 32, -16.0dB */
+};
+
+static const u8 cckswing_table_ch14[CCK_TABLE_SIZE][8] = {
+	{0x36, 0x35, 0x2e, 0x1b, 0x00, 0x00, 0x00, 0x00}, /* 0, +0dB */
+	{0x33, 0x32, 0x2b, 0x19, 0x00, 0x00, 0x00, 0x00}, /* 1, -0.5dB */
+	{0x30, 0x2f, 0x29, 0x18, 0x00, 0x00, 0x00, 0x00}, /* 2, -1.0dB */
+	{0x2d, 0x2d, 0x17, 0x17, 0x00, 0x00, 0x00, 0x00}, /* 3, -1.5dB */
+	{0x2b, 0x2a, 0x25, 0x15, 0x00, 0x00, 0x00, 0x00}, /* 4, -2.0dB */
+	{0x28, 0x28, 0x24, 0x14, 0x00, 0x00, 0x00, 0x00}, /* 5, -2.5dB */
+	{0x26, 0x25, 0x21, 0x13, 0x00, 0x00, 0x00, 0x00}, /* 6, -3.0dB */
+	{0x24, 0x23, 0x1f, 0x12, 0x00, 0x00, 0x00, 0x00}, /* 7, -3.5dB */
+	{0x22, 0x21, 0x1d, 0x11, 0x00, 0x00, 0x00, 0x00}, /* 8, -4.0dB */
+	{0x20, 0x20, 0x1b, 0x10, 0x00, 0x00, 0x00, 0x00}, /* 9, -4.5dB */
+	{0x1f, 0x1e, 0x1a, 0x0f, 0x00, 0x00, 0x00, 0x00}, /* 10, -5.0dB */
+	{0x1d, 0x1c, 0x18, 0x0e, 0x00, 0x00, 0x00, 0x00}, /* 11, -5.5dB */
+	{0x1b, 0x1a, 0x17, 0x0e, 0x00, 0x00, 0x00, 0x00}, /* 12, -6.0dB */
+	{0x1a, 0x19, 0x16, 0x0d, 0x00, 0x00, 0x00, 0x00}, /* 13, -6.5dB */
+	{0x18, 0x17, 0x15, 0x0c, 0x00, 0x00, 0x00, 0x00}, /* 14, -7.0dB */
+	{0x17, 0x16, 0x13, 0x0b, 0x00, 0x00, 0x00, 0x00}, /* 15, -7.5dB */
+	{0x16, 0x15, 0x12, 0x0b, 0x00, 0x00, 0x00, 0x00}, /* 16, -8.0dB */
+	{0x14, 0x14, 0x11, 0x0a, 0x00, 0x00, 0x00, 0x00}, /* 17, -8.5dB */
+	{0x13, 0x13, 0x10, 0x0a, 0x00, 0x00, 0x00, 0x00}, /* 18, -9.0dB */
+	{0x12, 0x12, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00}, /* 19, -9.5dB */
+	{0x11, 0x11, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00}, /* 20, -10.0dB */
+	{0x10, 0x10, 0x0e, 0x08, 0x00, 0x00, 0x00, 0x00}, /* 21, -10.5dB */
+	{0x0f, 0x0f, 0x0d, 0x08, 0x00, 0x00, 0x00, 0x00}, /* 22, -11.0dB */
+	{0x0e, 0x0e, 0x0c, 0x07, 0x00, 0x00, 0x00, 0x00}, /* 23, -11.5dB */
+	{0x0d, 0x0d, 0x0c, 0x07, 0x00, 0x00, 0x00, 0x00}, /* 24, -12.0dB */
+	{0x0d, 0x0c, 0x0b, 0x06, 0x00, 0x00, 0x00, 0x00}, /* 25, -12.5dB */
+	{0x0c, 0x0c, 0x0a, 0x06, 0x00, 0x00, 0x00, 0x00}, /* 26, -13.0dB */
+	{0x0b, 0x0b, 0x0a, 0x06, 0x00, 0x00, 0x00, 0x00}, /* 27, -13.5dB */
+	{0x0b, 0x0a, 0x09, 0x05, 0x00, 0x00, 0x00, 0x00}, /* 28, -14.0dB */
+	{0x0a, 0x0a, 0x09, 0x05, 0x00, 0x00, 0x00, 0x00}, /* 29, -14.5dB */
+	{0x0a, 0x09, 0x08, 0x05, 0x00, 0x00, 0x00, 0x00}, /* 30, -15.0dB */
+	{0x09, 0x09, 0x08, 0x05, 0x00, 0x00, 0x00, 0x00}, /* 31, -15.5dB */
+	{0x09, 0x08, 0x07, 0x04, 0x00, 0x00, 0x00, 0x00}  /* 32, -16.0dB */
+};
+
+static void rtl92ee_dm_diginit(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	dm_dig.cur_igvalue = rtl_get_bbreg(hw, DM_REG_IGI_A_11N,
+						DM_BIT_IGI_11N);
+	dm_dig.rssi_lowthresh = DM_DIG_THRESH_LOW;
+	dm_dig.rssi_highthresh = DM_DIG_THRESH_HIGH;
+	dm_dig.fa_lowthresh = DM_FALSEALARM_THRESH_LOW;
+	dm_dig.fa_highthresh = DM_FALSEALARM_THRESH_HIGH;
+	dm_dig.rx_gain_range_max = DM_DIG_MAX;
+	dm_dig.rx_gain_range_min = DM_DIG_MIN;
+	dm_dig.backoff_val = DM_DIG_BACKOFF_DEFAULT;
+	dm_dig.backoff_val_range_max = DM_DIG_BACKOFF_MAX;
+	dm_dig.backoff_val_range_min = DM_DIG_BACKOFF_MIN;
+	dm_dig.pre_cck_cca_thres = 0xff;
+	dm_dig.cur_cck_cca_thres = 0x83;
+	dm_dig.forbidden_igi = DM_DIG_MIN;
+	dm_dig.large_fa_hit = 0;
+	dm_dig.recover_cnt = 0;
+	dm_dig.dig_dynamic_min_0 = DM_DIG_MIN;
+	dm_dig.dig_dynamic_min_1 = DM_DIG_MIN;
+	dm_dig.b_media_connect_0 = false;
+	dm_dig.b_media_connect_1 = false;
+	rtlpriv->dm.b_dm_initialgain_enable = true;
+	dm_dig.bt30_cur_igi = 0x32;
+}
+
+static void rtl92ee_dm_false_alarm_counter_statistics(struct ieee80211_hw *hw)
+{
+	u32 ret_value;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct false_alarm_statistics *falsealm_cnt = &(rtlpriv->falsealm_cnt);
+
+	rtl_set_bbreg(hw, DM_REG_OFDM_FA_HOLDC_11N, BIT(31), 1);
+	rtl_set_bbreg(hw, DM_REG_OFDM_FA_RSTD_11N, BIT(31), 1);
+
+	ret_value = rtl_get_bbreg(hw, DM_REG_OFDM_FA_TYPE1_11N, MASKDWORD);
+	falsealm_cnt->cnt_fast_fsync_fail = (ret_value & 0xffff);
+	falsealm_cnt->cnt_sb_search_fail = ((ret_value & 0xffff0000) >> 16);
+
+	ret_value = rtl_get_bbreg(hw, DM_REG_OFDM_FA_TYPE2_11N, MASKDWORD);
+	falsealm_cnt->cnt_ofdm_cca = (ret_value & 0xffff);
+	falsealm_cnt->cnt_parity_fail = ((ret_value & 0xffff0000) >> 16);
+
+	ret_value = rtl_get_bbreg(hw, DM_REG_OFDM_FA_TYPE3_11N, MASKDWORD);
+	falsealm_cnt->cnt_rate_illegal = (ret_value & 0xffff);
+	falsealm_cnt->cnt_crc8_fail = ((ret_value & 0xffff0000) >> 16);
+
+	ret_value = rtl_get_bbreg(hw, DM_REG_OFDM_FA_TYPE4_11N, MASKDWORD);
+	falsealm_cnt->cnt_mcs_fail = (ret_value & 0xffff);
+
+	falsealm_cnt->cnt_ofdm_fail = falsealm_cnt->cnt_parity_fail +
+				      falsealm_cnt->cnt_rate_illegal +
+				      falsealm_cnt->cnt_crc8_fail +
+				      falsealm_cnt->cnt_mcs_fail +
+				      falsealm_cnt->cnt_fast_fsync_fail +
+				      falsealm_cnt->cnt_sb_search_fail;
+
+	ret_value = rtl_get_bbreg(hw, DM_REG_SC_CNT_11N, MASKDWORD);
+	falsealm_cnt->cnt_bw_lsc = (ret_value & 0xffff);
+	falsealm_cnt->cnt_bw_usc = ((ret_value & 0xffff0000) >> 16);
+
+	rtl_set_bbreg(hw, DM_REG_CCK_FA_RST_11N, BIT(12), 1);
+	rtl_set_bbreg(hw, DM_REG_CCK_FA_RST_11N, BIT(14), 1);
+
+	ret_value = rtl_get_bbreg(hw, DM_REG_CCK_FA_LSB_11N, MASKBYTE0);
+	falsealm_cnt->cnt_cck_fail = ret_value;
+
+	ret_value = rtl_get_bbreg(hw, DM_REG_CCK_FA_MSB_11N, MASKBYTE3);
+	falsealm_cnt->cnt_cck_fail += (ret_value & 0xff) << 8;
+
+	ret_value = rtl_get_bbreg(hw, DM_REG_CCK_CCA_CNT_11N, MASKDWORD);
+	falsealm_cnt->cnt_cck_cca = ((ret_value & 0xff) << 8) |
+				    ((ret_value & 0xFF00) >> 8);
+
+	falsealm_cnt->cnt_all = falsealm_cnt->cnt_fast_fsync_fail +
+				falsealm_cnt->cnt_sb_search_fail +
+				falsealm_cnt->cnt_parity_fail +
+				falsealm_cnt->cnt_rate_illegal +
+				falsealm_cnt->cnt_crc8_fail +
+				falsealm_cnt->cnt_mcs_fail +
+				falsealm_cnt->cnt_cck_fail;
+
+	falsealm_cnt->cnt_cca_all = falsealm_cnt->cnt_ofdm_cca +
+				    falsealm_cnt->cnt_cck_cca;
+
+	/*reset false alarm counter registers*/
+	rtl_set_bbreg(hw, DM_REG_OFDM_FA_RSTC_11N, BIT(31), 1);
+	rtl_set_bbreg(hw, DM_REG_OFDM_FA_RSTC_11N, BIT(31), 0);
+	rtl_set_bbreg(hw, DM_REG_OFDM_FA_RSTD_11N, BIT(27), 1);
+	rtl_set_bbreg(hw, DM_REG_OFDM_FA_RSTD_11N, BIT(27), 0);
+	/*update ofdm counter*/
+	rtl_set_bbreg(hw, DM_REG_OFDM_FA_HOLDC_11N, BIT(31), 0);
+	rtl_set_bbreg(hw, DM_REG_OFDM_FA_RSTD_11N, BIT(31), 0);
+	/*reset CCK CCA counter*/
+	rtl_set_bbreg(hw, DM_REG_CCK_FA_RST_11N, BIT(13) | BIT(12), 0);
+	rtl_set_bbreg(hw, DM_REG_CCK_FA_RST_11N, BIT(13) | BIT(12), 2);
+	/*reset CCK FA counter*/
+	rtl_set_bbreg(hw, DM_REG_CCK_FA_RST_11N, BIT(15) | BIT(14), 0);
+	rtl_set_bbreg(hw, DM_REG_CCK_FA_RST_11N, BIT(15) | BIT(14), 2);
+
+
+	RT_TRACE(COMP_DIG, DBG_TRACE,
+		 ("cnt_parity_fail = %d, cnt_rate_illegal = %d, "
+		  "cnt_crc8_fail = %d, cnt_mcs_fail = %d\n",
+		  falsealm_cnt->cnt_parity_fail,
+		  falsealm_cnt->cnt_rate_illegal,
+		  falsealm_cnt->cnt_crc8_fail, falsealm_cnt->cnt_mcs_fail));
+
+	RT_TRACE(COMP_DIG, DBG_TRACE,
+		 ("cnt_ofdm_fail = %x, cnt_cck_fail = %x, cnt_all = %x\n",
+		  falsealm_cnt->cnt_ofdm_fail,
+		  falsealm_cnt->cnt_cck_fail, falsealm_cnt->cnt_all));
+}
+
+static void rtl92ee_dm_cck_packet_detection_thresh(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 cur_cck_cca_thresh;
+	if (rtlpriv->mac80211.link_state >= MAC80211_LINKED) {
+		if (dm_dig.rssi_val_min > 25) {
+			cur_cck_cca_thresh = 0xcd;
+		} else if ((dm_dig.rssi_val_min <= 25) &&
+			   (dm_dig.rssi_val_min > 10)) {
+			cur_cck_cca_thresh = 0x83;
+		} else {
+			if (rtlpriv->falsealm_cnt.cnt_cck_fail > 1000)
+				cur_cck_cca_thresh = 0x83;
+			else
+				cur_cck_cca_thresh = 0x40;
+		}
+	} else {
+		if (rtlpriv->falsealm_cnt.cnt_cck_fail > 1000)
+			cur_cck_cca_thresh = 0x83;
+		else
+			cur_cck_cca_thresh = 0x40;
+	}
+	rtl92ee_dm_write_cck_cca_thres(hw, cur_cck_cca_thresh);
+}
+
+static void rtl92ee_dm_dig(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u8 dig_dynamic_min , dig_maxofmin;
+	bool bfirstconnect , bfirstdisconnect;
+	u8 dm_dig_max, dm_dig_min;
+	u8 current_igi = dm_dig.cur_igvalue;
+	u8 offset;
+
+	/* AP, BT */
+	if (mac->act_scanning == true)
+		return;
+
+	dig_dynamic_min = dm_dig.dig_dynamic_min_0;
+	bfirstconnect = (mac->link_state >= MAC80211_LINKED) &&
+			(dm_dig.b_media_connect_0 == false);
+	bfirstdisconnect = (mac->link_state < MAC80211_LINKED) &&
+			   (dm_dig.b_media_connect_0 == true);
+
+	dm_dig_max = 0x5a;
+	dm_dig_min = DM_DIG_MIN;
+	dig_maxofmin = DM_DIG_MAX_AP;
+
+	if (mac->link_state >= MAC80211_LINKED) {
+		if ((dm_dig.rssi_val_min + 10) > dm_dig_max)
+			dm_dig.rx_gain_range_max = dm_dig_max;
+		else if ((dm_dig.rssi_val_min + 10) < dm_dig_min)
+			dm_dig.rx_gain_range_max = dm_dig_min;
+		else
+			dm_dig.rx_gain_range_max = dm_dig.rssi_val_min + 10;
+
+		if (rtlpriv->dm.b_one_entry_only) {
+			offset = 0;
+			if (dm_dig.rssi_val_min - offset < dm_dig_min)
+				dig_dynamic_min = dm_dig_min;
+			else if (dm_dig.rssi_val_min - offset >
+				 dig_maxofmin)
+				dig_dynamic_min = dig_maxofmin;
+			else
+				dig_dynamic_min = dm_dig.rssi_val_min - offset;
+		} else {
+			dig_dynamic_min = dm_dig_min;
+		}
+
+	} else {
+		dm_dig.rx_gain_range_max = dm_dig_max;
+		dig_dynamic_min = dm_dig_min;
+		RT_TRACE(COMP_DIG, DBG_LOUD, ("no link\n"));
+	}
+
+	if (rtlpriv->falsealm_cnt.cnt_all > 10000) {
+		if (dm_dig.large_fa_hit != 3)
+			dm_dig.large_fa_hit++;
+		if (dm_dig.forbidden_igi < current_igi) {
+			dm_dig.forbidden_igi = current_igi;
+			dm_dig.large_fa_hit = 1;
+		}
+
+		if (dm_dig.large_fa_hit >= 3) {
+			if (dm_dig.forbidden_igi + 1 > dm_dig.rx_gain_range_max)
+				dm_dig.rx_gain_range_min =
+						dm_dig.rx_gain_range_max;
+			else
+				dm_dig.rx_gain_range_min =
+						dm_dig.forbidden_igi + 1;
+			dm_dig.recover_cnt = 3600;
+		}
+	} else {
+		if (dm_dig.recover_cnt != 0) {
+			dm_dig.recover_cnt--;
+		} else {
+			if (dm_dig.large_fa_hit < 3) {
+				if ((dm_dig.forbidden_igi - 1) <
+				    dig_dynamic_min) {
+					dm_dig.forbidden_igi = dig_dynamic_min;
+					dm_dig.rx_gain_range_min =
+								dig_dynamic_min;
+				} else {
+					dm_dig.forbidden_igi--;
+					dm_dig.rx_gain_range_min =
+						dm_dig.forbidden_igi + 1;
+				}
+			} else {
+				dm_dig.large_fa_hit = 0;
+			}
+		}
+	}
+
+	if (rtlpriv->dm.dbginfo.num_qry_beacon_pkt < 5)
+		dm_dig.rx_gain_range_min = dm_dig_min;
+
+	if (dm_dig.rx_gain_range_min > dm_dig.rx_gain_range_max)
+		dm_dig.rx_gain_range_min = dm_dig.rx_gain_range_max;
+
+	if (mac->link_state >= MAC80211_LINKED) {
+		if (bfirstconnect) {
+			if (dm_dig.rssi_val_min <= dig_maxofmin)
+				current_igi = dm_dig.rssi_val_min;
+			else
+				current_igi = dig_maxofmin;
+
+			dm_dig.large_fa_hit = 0;
+		} else {
+			if (rtlpriv->falsealm_cnt.cnt_all > DM_DIG_FA_TH2)
+				current_igi += 4;
+			else if (rtlpriv->falsealm_cnt.cnt_all > DM_DIG_FA_TH1)
+				current_igi += 2;
+			else if (rtlpriv->falsealm_cnt.cnt_all < DM_DIG_FA_TH0)
+				current_igi -= 2;
+
+			if (rtlpriv->dm.dbginfo.num_qry_beacon_pkt < 5 &&
+			    rtlpriv->falsealm_cnt.cnt_all < DM_DIG_FA_TH1)
+				current_igi = dm_dig.rx_gain_range_min;
+		}
+	} else {
+		if (bfirstdisconnect) {
+			current_igi = dm_dig.rx_gain_range_min;
+		} else {
+			if (rtlpriv->falsealm_cnt.cnt_all > 10000)
+				current_igi += 4;
+			else if (rtlpriv->falsealm_cnt.cnt_all > 8000)
+				current_igi += 2;
+			else if (rtlpriv->falsealm_cnt.cnt_all < 500)
+				current_igi -= 2;
+		}
+	}
+
+	if (current_igi > dm_dig.rx_gain_range_max)
+		current_igi = dm_dig.rx_gain_range_max;
+	if (current_igi < dm_dig.rx_gain_range_min)
+		current_igi = dm_dig.rx_gain_range_min;
+
+	rtl92ee_dm_write_dig(hw , current_igi);
+	dm_dig.b_media_connect_0 = ((mac->link_state >= MAC80211_LINKED) ?
+				   true : false);
+	dm_dig.dig_dynamic_min_0 = dig_dynamic_min;
+}
+
+void rtl92ee_dm_write_cck_cca_thres(struct ieee80211_hw *hw, u8 cur_thres)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	if (dm_dig.cur_cck_cca_thres != cur_thres)
+		rtl_write_byte(rtlpriv, DM_REG_CCK_CCA_11N, cur_thres);
+
+	dm_dig.pre_cck_cca_thres = dm_dig.cur_cck_cca_thres;
+	dm_dig.cur_cck_cca_thres = cur_thres;
+}
+
+void rtl92ee_dm_write_dig(struct ieee80211_hw *hw, u8 current_igi)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	if (dm_dig.stop_dig)
+		return;
+
+	if (dm_dig.cur_igvalue != current_igi) {
+		rtl_set_bbreg(hw, ROFDM0_XAAGCCORE1, 0x7f, current_igi);
+		if (rtlpriv->phy.rf_type != RF_1T1R)
+			rtl_set_bbreg(hw, ROFDM0_XBAGCCORE1, 0x7f, current_igi);
+	}
+	dm_dig.pre_igvalue = dm_dig.cur_igvalue;
+	dm_dig.cur_igvalue = current_igi;
+}
+
+static void rtl92ee_rssi_dump_to_register(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	rtl_write_byte(rtlpriv, RA_RSSIDUMP,
+		       rtlpriv->stats.rx_rssi_percentage[0]);
+	rtl_write_byte(rtlpriv, RB_RSSIDUMP,
+		       rtlpriv->stats.rx_rssi_percentage[1]);
+	/*It seems the following values is not initialized.
+	  *According to Windows code,
+	  *these value will only be valid when JAGUAR chips*/
+	/* Rx EVM */
+	rtl_write_byte(rtlpriv, RS1_RXEVMDUMP, rtlpriv->stats.rx_evm_dbm[0]);
+	rtl_write_byte(rtlpriv, RS2_RXEVMDUMP, rtlpriv->stats.rx_evm_dbm[1]);
+	/* Rx SNR */
+	rtl_write_byte(rtlpriv, RA_RXSNRDUMP,
+		       (u8)(rtlpriv->stats.rx_snr_db[0]));
+	rtl_write_byte(rtlpriv, RB_RXSNRDUMP,
+		       (u8)(rtlpriv->stats.rx_snr_db[1]));
+	/* Rx Cfo_Short */
+	rtl_write_word(rtlpriv, RA_CFOSHORTDUMP,
+		       rtlpriv->stats.rx_cfo_short[0]);
+	rtl_write_word(rtlpriv, RB_CFOSHORTDUMP,
+		       rtlpriv->stats.rx_cfo_short[1]);
+	/* Rx Cfo_Tail */
+	rtl_write_word(rtlpriv, RA_CFOLONGDUMP, rtlpriv->stats.rx_cfo_tail[0]);
+	rtl_write_word(rtlpriv, RB_CFOLONGDUMP, rtlpriv->stats.rx_cfo_tail[1]);
+}
+
+static void rtl92ee_dm_find_minimum_rssi(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_dig *rtl_dm_dig = &(rtlpriv->dm.dm_digtable);
+	struct rtl_mac *mac = rtl_mac(rtlpriv);
+
+	/* Determine the minimum RSSI  */
+	if ((mac->link_state < MAC80211_LINKED) &&
+	    (rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb == 0)) {
+		rtl_dm_dig->min_undecorated_pwdb_for_dm = 0;
+		RT_TRACE(COMP_BB_POWERSAVING, DBG_LOUD,
+			 ("Not connected to any\n"));
+	}
+	if (mac->link_state >= MAC80211_LINKED) {
+		if (mac->opmode == NL80211_IFTYPE_AP ||
+		    mac->opmode == NL80211_IFTYPE_ADHOC) {
+			rtl_dm_dig->min_undecorated_pwdb_for_dm =
+				rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb;
+			RT_TRACE(COMP_BB_POWERSAVING, DBG_LOUD,
+			      ("AP Client PWDB = 0x%lx\n",
+			       rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb));
+		} else {
+			rtl_dm_dig->min_undecorated_pwdb_for_dm =
+			    rtlpriv->dm.undecorated_smoothed_pwdb;
+			RT_TRACE(COMP_BB_POWERSAVING, DBG_LOUD,
+				("STA Default Port PWDB = 0x%x\n",
+				rtl_dm_dig->min_undecorated_pwdb_for_dm));
+		}
+	} else {
+		rtl_dm_dig->min_undecorated_pwdb_for_dm =
+			rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb;
+		RT_TRACE(COMP_BB_POWERSAVING, DBG_LOUD,
+			("AP Ext Port or disconnet PWDB = 0x%x\n",
+			rtl_dm_dig->min_undecorated_pwdb_for_dm));
+	}
+	RT_TRACE(COMP_DIG, DBG_LOUD, ("MinUndecoratedPWDBForDM =%d\n",
+		rtl_dm_dig->min_undecorated_pwdb_for_dm));
+}
+
+static void rtl92ee_dm_check_rssi_monitor(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtlpriv);
+	struct rtl_dm *dm = rtl_dm(rtlpriv);
+	struct rtl_sta_info *drv_priv;
+	u8 h2c[4] = { 0 };
+	long max = 0, min = 0xff;
+	u8 i = 0;
+
+	if (mac->opmode == NL80211_IFTYPE_AP ||
+	    mac->opmode == NL80211_IFTYPE_ADHOC ||
+	    mac->opmode == NL80211_IFTYPE_MESH_POINT) {
+		/* AP & ADHOC & MESH */
+		spin_lock_bh(&rtlpriv->locks.entry_list_lock);
+		list_for_each_entry(drv_priv, &rtlpriv->entry_list, list) {
+			struct rssi_sta *stat = &(drv_priv->rssi_stat);
+			if (stat->undecorated_smoothed_pwdb < min)
+				min = stat->undecorated_smoothed_pwdb;
+			if (stat->undecorated_smoothed_pwdb > max)
+				max = stat->undecorated_smoothed_pwdb;
+
+			h2c[3] = 0;
+			h2c[2] = (u8) (dm->undecorated_smoothed_pwdb & 0xFF);
+			h2c[1] = 0x20;
+			h2c[0] = ++i;
+			rtl92ee_fill_h2c_cmd(hw, H2C_92E_RSSI_REPORT, 4, h2c);
+		}
+		spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
+
+		/* If associated entry is found */
+		if (max != 0) {
+			dm->entry_max_undecoratedsmoothed_pwdb = max;
+			RTPRINT(rtlpriv, FDM, DM_PWDB,
+				"EntryMaxPWDB = 0x%lx(%ld)\n", max, max);
+		} else {
+			dm->entry_max_undecoratedsmoothed_pwdb = 0;
+		}
+		/* If associated entry is found */
+		if (min != 0xff) {
+			dm->entry_min_undecoratedsmoothed_pwdb = min;
+			RTPRINT(rtlpriv, FDM, DM_PWDB,
+				"EntryMinPWDB = 0x%lx(%ld)\n", min, min);
+		} else {
+			dm->entry_min_undecoratedsmoothed_pwdb = 0;
+		}
+	}
+
+	/* Indicate Rx signal strength to FW. */
+	if (dm->b_useramask) {
+		h2c[3] = 0;
+		h2c[2] = (u8) (dm->undecorated_smoothed_pwdb & 0xFF);
+		h2c[1] = 0x20;
+		h2c[0] = 0;
+		rtl92ee_fill_h2c_cmd(hw, H2C_92E_RSSI_REPORT, 4, h2c);
+	} else {
+		rtl_write_byte(rtlpriv, 0x4fe, dm->undecorated_smoothed_pwdb);
+	}
+	rtl92ee_rssi_dump_to_register(hw);
+	rtl92ee_dm_find_minimum_rssi(hw);
+	dm_dig.rssi_val_min = dm->dm_digtable.min_undecorated_pwdb_for_dm;
+}
+
+static void rtl92ee_dm_init_primary_cca_check(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	struct dynamic_primary_cca *primarycca = &(rtlpriv->primarycca);
+
+	rtlhal->rts_en = 0;
+	primarycca->dup_rts_flag = 0;
+	primarycca->intf_flag = 0;
+	primarycca->intf_type = 0;
+	primarycca->monitor_flag = 0;
+	primarycca->ch_offset = 0;
+	primarycca->mf_state = 0;
+}
+
+static bool rtl92ee_dm_is_edca_turbo_disable(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (rtlpriv->mac80211.mode == WIRELESS_MODE_B)
+		return true;
+
+	return false;
+}
+
+void rtl92ee_dm_init_edca_turbo(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->dm.bcurrent_turbo_edca = false;
+	rtlpriv->dm.bis_cur_rdlstate = false;
+	rtlpriv->dm.bis_any_nonbepkts = false;
+}
+
+static void rtl92ee_dm_check_edca_turbo(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	static u64 last_txok_cnt;
+	static u64 last_rxok_cnt;
+	u64 cur_txok_cnt = 0;
+	u64 cur_rxok_cnt = 0;
+	u32 edca_be_ul = 0x5ea42b;
+	u32 edca_be_dl = 0x5ea42b; /*not sure*/
+	u32 edca_be = 0x5ea42b;
+	bool b_is_cur_rdlstate;
+	bool b_edca_turbo_on = false;
+
+	if (rtlpriv->dm.dbginfo.num_non_be_pkt > 0x100)
+		rtlpriv->dm.bis_any_nonbepkts = true;
+	rtlpriv->dm.dbginfo.num_non_be_pkt = 0;
+
+	cur_txok_cnt = rtlpriv->stats.txbytesunicast - last_txok_cnt;
+	cur_rxok_cnt = rtlpriv->stats.rxbytesunicast - last_rxok_cnt;
+
+	/*b_bias_on_rx = false;*/
+	b_edca_turbo_on = ((!rtlpriv->dm.bis_any_nonbepkts) &&
+			   (!rtlpriv->dm.b_disable_framebursting)) ?
+			  true : false;
+
+	if (rtl92ee_dm_is_edca_turbo_disable(hw))
+		goto dm_CheckEdcaTurbo_EXIT;
+
+	if (b_edca_turbo_on) {
+		b_is_cur_rdlstate = (cur_rxok_cnt > cur_txok_cnt * 4) ?
+				    true : false;
+
+		edca_be = b_is_cur_rdlstate ? edca_be_dl : edca_be_ul;
+		rtl_write_dword(rtlpriv , REG_EDCA_BE_PARAM , edca_be);
+		rtlpriv->dm.bis_cur_rdlstate = b_is_cur_rdlstate;
+		rtlpriv->dm.bcurrent_turbo_edca = true;
+	} else {
+		if (rtlpriv->dm.bcurrent_turbo_edca) {
+			u8 tmp = AC0_BE;
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AC_PARAM,
+						      (u8 *) (&tmp));
+		}
+		rtlpriv->dm.bcurrent_turbo_edca = false;
+	}
+
+dm_CheckEdcaTurbo_EXIT:
+	rtlpriv->dm.bis_any_nonbepkts = false;
+	last_txok_cnt = rtlpriv->stats.txbytesunicast;
+	last_rxok_cnt = rtlpriv->stats.rxbytesunicast;
+}
+
+static void rtl92ee_dm_dynamic_edcca(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 reg_c50 , reg_c58;
+	bool b_fw_current_in_ps_mode = false;
+
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
+				      (u8 *)(&b_fw_current_in_ps_mode));
+	if (b_fw_current_in_ps_mode)
+		return;
+
+	reg_c50 = rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, MASKBYTE0);
+	reg_c58 = rtl_get_bbreg(hw, ROFDM0_XBAGCCORE1, MASKBYTE0);
+
+	if (reg_c50 > 0x28 && reg_c58 > 0x28) {
+		if (!rtlpriv->rtlhal.b_pre_edcca_enable) {
+			rtl_write_byte(rtlpriv, ROFDM0_ECCATHRESHOLD, 0x03);
+			rtl_write_byte(rtlpriv, ROFDM0_ECCATHRESHOLD + 2, 0x00);
+			rtlpriv->rtlhal.b_pre_edcca_enable = true;
+		}
+	} else if (reg_c50 < 0x25 && reg_c58 < 0x25) {
+		if (rtlpriv->rtlhal.b_pre_edcca_enable) {
+			rtl_write_byte(rtlpriv, ROFDM0_ECCATHRESHOLD, 0x7f);
+			rtl_write_byte(rtlpriv, ROFDM0_ECCATHRESHOLD + 2, 0x7f);
+			rtlpriv->rtlhal.b_pre_edcca_enable = false;
+		}
+	}
+}
+
+static void rtl92ee_dm_adaptivity(struct ieee80211_hw *hw)
+{
+	rtl92ee_dm_dynamic_edcca(hw);
+}
+
+static void rtl92ee_dm_write_dynamic_cca(struct ieee80211_hw *hw, u8 cur_mf_state)
+{
+	struct dynamic_primary_cca *primarycca = &(rtl_priv(hw)->primarycca);
+
+	if (primarycca->mf_state != cur_mf_state)
+		rtl_set_bbreg(hw, DM_REG_L1SBD_PD_CH_11N, BIT(8) | BIT(7),
+			      cur_mf_state);
+
+	primarycca->mf_state = cur_mf_state;
+}
+
+static void rtl92ee_dm_dynamic_primary_cca_ckeck(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct false_alarm_statistics *falsealm_cnt = &(rtlpriv->falsealm_cnt);
+	struct dynamic_primary_cca *primarycca = &(rtlpriv->primarycca);
+	bool is40mhz = false;
+	u64 ofdm_cca, ofdm_fa, bw_usc_cnt, bw_lsc_cnt;
+	u8 sec_ch_offset;
+	u8 cur_mf_state;
+	static u8 count_down = MONITOR_TIME;
+
+	ofdm_cca = falsealm_cnt->cnt_ofdm_cca;
+	ofdm_fa = falsealm_cnt->cnt_ofdm_fail;
+	bw_usc_cnt = falsealm_cnt->cnt_bw_usc;
+	bw_lsc_cnt = falsealm_cnt->cnt_bw_lsc;
+	is40mhz = rtlpriv->mac80211.bw_40;
+	sec_ch_offset = rtlpriv->mac80211.cur_40_prime_sc;
+	/* NIC: 2: sec is below,  1: sec is above */
+
+	if (rtlpriv->mac80211.opmode == NL80211_IFTYPE_AP) {
+		cur_mf_state = MF_USC_LSC;
+		rtl92ee_dm_write_dynamic_cca(hw, cur_mf_state);
+		return;
+	}
+
+	if (rtlpriv->mac80211.link_state < MAC80211_LINKED)
+		return;
+
+	if (is40mhz)
+		return;
+
+	if (primarycca->pricca_flag == 0) {
+		/* Primary channel is above
+		 * NOTE: duplicate CTS can remove this condition*/
+		if (sec_ch_offset == 2) {
+			if ((ofdm_cca > OFDMCCA_TH) &&
+			    (bw_lsc_cnt > (bw_usc_cnt + BW_IND_BIAS)) &&
+			    (ofdm_fa > (ofdm_cca >> 1))) {
+				primarycca->intf_type = 1;
+				primarycca->intf_flag = 1;
+				cur_mf_state = MF_USC;
+				rtl92ee_dm_write_dynamic_cca(hw, cur_mf_state);
+				primarycca->pricca_flag = 1;
+			} else if ((ofdm_cca > OFDMCCA_TH) &&
+				   (bw_lsc_cnt > (bw_usc_cnt + BW_IND_BIAS)) &&
+				   (ofdm_fa < (ofdm_cca >> 1))) {
+				primarycca->intf_type = 2;
+				primarycca->intf_flag = 1;
+				cur_mf_state = MF_USC;
+				rtl92ee_dm_write_dynamic_cca(hw, cur_mf_state);
+				primarycca->pricca_flag = 1;
+				primarycca->dup_rts_flag = 1;
+				rtlpriv->rtlhal.rts_en = 1;
+			} else {
+				primarycca->intf_type = 0;
+				primarycca->intf_flag = 0;
+				cur_mf_state = MF_USC_LSC;
+				rtl92ee_dm_write_dynamic_cca(hw, cur_mf_state);
+				rtlpriv->rtlhal.rts_en = 0;
+				primarycca->dup_rts_flag = 0;
+			}
+		} else if (sec_ch_offset == 1) {
+			if ((ofdm_cca > OFDMCCA_TH) &&
+			    (bw_usc_cnt > (bw_lsc_cnt + BW_IND_BIAS)) &&
+			    (ofdm_fa > (ofdm_cca >> 1))) {
+				primarycca->intf_type = 1;
+				primarycca->intf_flag = 1;
+				cur_mf_state = MF_LSC;
+				rtl92ee_dm_write_dynamic_cca(hw, cur_mf_state);
+				primarycca->pricca_flag = 1;
+			} else if ((ofdm_cca > OFDMCCA_TH) &&
+				   (bw_usc_cnt > (bw_lsc_cnt + BW_IND_BIAS)) &&
+				   (ofdm_fa < (ofdm_cca >> 1))) {
+				primarycca->intf_type = 2;
+				primarycca->intf_flag = 1;
+				cur_mf_state = MF_LSC;
+				rtl92ee_dm_write_dynamic_cca(hw, cur_mf_state);
+				primarycca->pricca_flag = 1;
+				primarycca->dup_rts_flag = 1;
+				rtlpriv->rtlhal.rts_en = 1;
+			} else {
+				primarycca->intf_type = 0;
+				primarycca->intf_flag = 0;
+				cur_mf_state = MF_USC_LSC;
+				rtl92ee_dm_write_dynamic_cca(hw, cur_mf_state);
+				rtlpriv->rtlhal.rts_en = 0;
+				primarycca->dup_rts_flag = 0;
+			}
+		}
+	} else {/* PrimaryCCA->PriCCA_flag == 1 */
+		count_down--;
+		if (count_down == 0) {
+			count_down = MONITOR_TIME;
+			primarycca->pricca_flag = 0;
+			cur_mf_state = MF_USC_LSC;
+			/* default */
+			rtl92ee_dm_write_dynamic_cca(hw, cur_mf_state);
+			rtlpriv->rtlhal.rts_en = 0;
+			primarycca->dup_rts_flag = 0;
+			primarycca->intf_type = 0;
+			primarycca->intf_flag = 0;
+		}
+	}
+}
+
+static void rtl92ee_dm_dynamic_atc_switch(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	u8 crystal_cap;
+	u32 packet_count;
+	int cfo_khz_a , cfo_khz_b , cfo_ave = 0, adjust_xtal = 0;
+	int cfo_ave_diff;
+
+	if (rtlpriv->mac80211.link_state < MAC80211_LINKED) {
+		if (rtldm->atc_status == ATC_STATUS_OFF) {
+			rtl_set_bbreg(hw, ROFDM1_CFOTRACKING, BIT(11),
+				      ATC_STATUS_ON);
+			rtldm->atc_status = ATC_STATUS_ON;
+		}
+		/* Disable CFO tracking for BT */
+		if (rtlpriv->cfg->ops->get_btc_status()) {
+			if (!rtlpriv->btcoexist.btc_ops->btc_is_bt_disabled(rtlpriv)) {
+				RT_TRACE(COMP_BT_COEXIST, DBG_LOUD,
+					("odm_DynamicATCSwitch(): "
+					"Disable CFO tracking for BT!!\n"));
+				return;
+			}
+		}
+		/* Reset Crystal Cap */
+		if (rtldm->crystal_cap != rtlpriv->efuse.crystalcap) {
+			rtldm->crystal_cap = rtlpriv->efuse.crystalcap;
+			crystal_cap = rtldm->crystal_cap & 0x3f;
+			rtl_set_bbreg(hw, REG_MAC_PHY_CTRL, 0xFFF000,
+				      (crystal_cap | (crystal_cap << 6)));
+		}
+	} else {
+		cfo_khz_a = (int)(rtldm->cfo_tail[0] * 3125) / 1280;
+		cfo_khz_b = (int)(rtldm->cfo_tail[1] * 3125) / 1280;
+		packet_count = rtldm->packet_count;
+
+		if (packet_count == rtldm->packet_count_pre)
+			return;
+
+		rtldm->packet_count_pre = packet_count;
+
+		if (rtlpriv->phy.rf_type == RF_1T1R)
+			cfo_ave = cfo_khz_a;
+		else
+			cfo_ave = (int)(cfo_khz_a + cfo_khz_b) >> 1;
+
+		cfo_ave_diff = (rtldm->cfo_ave_pre >= cfo_ave) ?
+			       (rtldm->cfo_ave_pre - cfo_ave) :
+			       (cfo_ave - rtldm->cfo_ave_pre);
+
+		if (cfo_ave_diff > 20 && rtldm->large_cfo_hit == 0) {
+			rtldm->large_cfo_hit = 1;
+			return;
+		} else {
+			rtldm->large_cfo_hit = 0;
+		}
+
+		rtldm->cfo_ave_pre = cfo_ave;
+
+		if (cfo_ave >= -rtldm->cfo_threshold &&
+		    cfo_ave <= rtldm->cfo_threshold && rtldm->is_freeze == 0) {
+			if (rtldm->cfo_threshold == CFO_THRESHOLD_XTAL) {
+				rtldm->cfo_threshold = CFO_THRESHOLD_XTAL + 10;
+				rtldm->is_freeze = 1;
+			} else {
+				rtldm->cfo_threshold = CFO_THRESHOLD_XTAL;
+			}
+		}
+
+		if (cfo_ave > rtldm->cfo_threshold && rtldm->crystal_cap < 0x3f)
+			adjust_xtal = ((cfo_ave - CFO_THRESHOLD_XTAL) >> 2) + 1;
+		else if ((cfo_ave < -rtlpriv->dm.cfo_threshold) &&
+			 rtlpriv->dm.crystal_cap > 0)
+			adjust_xtal = ((cfo_ave + CFO_THRESHOLD_XTAL) >> 2) - 1;
+
+		if (adjust_xtal != 0) {
+			rtldm->is_freeze = 0;
+			rtldm->crystal_cap += adjust_xtal;
+
+			if (rtldm->crystal_cap > 0x3f)
+				rtldm->crystal_cap = 0x3f;
+			else if (rtldm->crystal_cap < 0)
+				rtldm->crystal_cap = 0;
+
+			crystal_cap = rtldm->crystal_cap & 0x3f;
+			rtl_set_bbreg(hw, REG_MAC_PHY_CTRL, 0xFFF000,
+				      (crystal_cap | (crystal_cap << 6)));
+		}
+
+		if (cfo_ave < CFO_THRESHOLD_ATC &&
+		    cfo_ave > -CFO_THRESHOLD_ATC) {
+			if (rtldm->atc_status == ATC_STATUS_ON) {
+				rtl_set_bbreg(hw, ROFDM1_CFOTRACKING, BIT(11),
+					      ATC_STATUS_OFF);
+				rtldm->atc_status = ATC_STATUS_OFF;
+			}
+		} else {
+			if (rtldm->atc_status == ATC_STATUS_OFF) {
+				rtl_set_bbreg(hw, ROFDM1_CFOTRACKING, BIT(11),
+					      ATC_STATUS_ON);
+				rtldm->atc_status = ATC_STATUS_ON;
+			}
+		}
+	}
+}
+
+static void rtl92ee_dm_init_txpower_tracking(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_dm *dm = rtl_dm(rtlpriv);
+	u8 path;
+
+	dm->btxpower_tracking = true;
+	dm->default_ofdm_index = 30;
+	dm->default_cck_index = 20;
+
+	dm->bb_swing_idx_cck_base = dm->default_cck_index;
+	dm->cck_index = dm->default_cck_index;
+
+	for (path = RF90_PATH_A; path < MAX_RF_PATH; path++) {
+		dm->bb_swing_idx_ofdm_base[path] = dm->default_ofdm_index;
+		dm->ofdm_index[path] = dm->default_ofdm_index;
+		dm->delta_power_index[path] = 0;
+		dm->delta_power_index_last[path] = 0;
+		dm->power_index_offset[path] = 0;
+	}
+}
+
+void rtl92ee_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rate_adaptive *p_ra = &(rtlpriv->ra);
+
+	p_ra->ratr_state = DM_RATR_STA_INIT;
+	p_ra->pre_ratr_state = DM_RATR_STA_INIT;
+
+	if (rtlpriv->dm.dm_type == DM_TYPE_BYDRIVER)
+		rtlpriv->dm.b_useramask = true;
+	else
+		rtlpriv->dm.b_useramask = false;
+
+	p_ra->ldpc_thres = 35;
+	p_ra->use_ldpc = false;
+	p_ra->high_rssi_thresh_for_ra = 50;
+	p_ra->low_rssi_thresh_for_ra = 20;
+
+}
+
+static bool _rtl92ee_dm_ra_state_check(struct ieee80211_hw *hw,
+				      s32 rssi, u8 *ratr_state)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rate_adaptive *p_ra = &(rtlpriv->ra);
+	const u8 go_up_gap = 5;
+	u32 high_rssithresh_for_ra = p_ra->high_rssi_thresh_for_ra;
+	u32 low_rssithresh_for_ra = p_ra->low_rssi_thresh_for_ra;
+	u8 state;
+
+	/* Threshold Adjustment:
+	 * when RSSI state trends to go up one or two levels,
+	 * make sure RSSI is high enough.
+	 * Here GoUpGap is added to solve
+	 * the boundary's level alternation issue.
+	 */
+	switch (*ratr_state) {
+	case DM_RATR_STA_INIT:
+	case DM_RATR_STA_HIGH:
+			break;
+
+	case DM_RATR_STA_MIDDLE:
+			high_rssithresh_for_ra += go_up_gap;
+			break;
+
+	case DM_RATR_STA_LOW:
+			high_rssithresh_for_ra += go_up_gap;
+			low_rssithresh_for_ra += go_up_gap;
+			break;
+
+	default:
+			RT_TRACE(COMP_RATR, DBG_DMESG,
+				("wrong rssi level setting %d !", *ratr_state));
+			break;
+	}
+
+	/* Decide RATRState by RSSI. */
+	if (rssi > high_rssithresh_for_ra)
+		state = DM_RATR_STA_HIGH;
+	else if (rssi > low_rssithresh_for_ra)
+		state = DM_RATR_STA_MIDDLE;
+	else
+		state = DM_RATR_STA_LOW;
+
+	if (*ratr_state != state) {
+		*ratr_state = state;
+		return true;
+	}
+
+	return false;
+}
+
+static void rtl92ee_dm_refresh_rate_adaptive_mask(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rate_adaptive *p_ra = &(rtlpriv->ra);
+	struct ieee80211_sta *sta = NULL;
+
+	if (is_hal_stop(rtlhal)) {
+		RT_TRACE(COMP_RATE, DBG_LOUD,
+			 ("driver is going to unload\n"));
+		return;
+	}
+
+	if (!rtlpriv->dm.b_useramask) {
+		RT_TRACE(COMP_RATE, DBG_LOUD,
+			 ("driver does not control rate adaptive mask\n"));
+		return;
+	}
+
+	if (mac->link_state == MAC80211_LINKED &&
+		mac->opmode == NL80211_IFTYPE_STATION) {
+
+		if (rtlpriv->dm.undecorated_smoothed_pwdb < p_ra->ldpc_thres) {
+			p_ra->use_ldpc = true;
+			p_ra->lower_rts_rate = true;
+		} else if (rtlpriv->dm.undecorated_smoothed_pwdb >
+			   (p_ra->ldpc_thres - 5)) {
+			p_ra->use_ldpc = false;
+			p_ra->lower_rts_rate = false;
+		}
+		if (_rtl92ee_dm_ra_state_check(hw,
+					rtlpriv->dm.undecorated_smoothed_pwdb,
+					&(p_ra->ratr_state))) {
+
+			rcu_read_lock();
+			sta = rtl_find_sta(hw, mac->bssid);
+			if (sta)
+				rtlpriv->cfg->ops->update_rate_tbl(hw, sta,
+							      p_ra->ratr_state);
+			rcu_read_unlock();
+
+			p_ra->pre_ratr_state = p_ra->ratr_state;
+		}
+	}
+}
+
+static void rtl92ee_dm_init_dynamic_atc_switch(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->dm.crystal_cap = rtlpriv->efuse.crystalcap;
+
+	rtlpriv->dm.atc_status = rtl_get_bbreg(hw, ROFDM1_CFOTRACKING, BIT(11));
+	rtlpriv->dm.cfo_threshold = CFO_THRESHOLD_XTAL;
+}
+
+void rtl92ee_dm_init(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->dm.dm_type = DM_TYPE_BYDRIVER;
+
+	rtl92ee_dm_diginit(hw);
+	rtl92ee_dm_init_rate_adaptive_mask(hw);
+	rtl92ee_dm_init_primary_cca_check(hw);
+	rtl92ee_dm_init_edca_turbo(hw);
+	rtl92ee_dm_init_txpower_tracking(hw);
+	rtl92ee_dm_init_dynamic_atc_switch(hw);
+}
+
+static void rtl92ee_dm_common_info_self_update(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 cnt = 0;
+	struct rtl_sta_info *drv_priv;
+
+	rtlpriv->dm.b_one_entry_only = false;
+
+	if (rtlpriv->mac80211.opmode == NL80211_IFTYPE_STATION &&
+		rtlpriv->mac80211.link_state >= MAC80211_LINKED) {
+		rtlpriv->dm.b_one_entry_only = true;
+		return;
+	}
+
+	if (rtlpriv->mac80211.opmode == NL80211_IFTYPE_AP ||
+	    rtlpriv->mac80211.opmode == NL80211_IFTYPE_ADHOC ||
+	    rtlpriv->mac80211.opmode == NL80211_IFTYPE_MESH_POINT) {
+		spin_lock_bh(&rtlpriv->locks.entry_list_lock);
+		list_for_each_entry(drv_priv, &rtlpriv->entry_list, list) {
+			cnt++;
+		}
+		spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
+
+		if (cnt == 1)
+			rtlpriv->dm.b_one_entry_only = true;
+	}
+}
+
+void rtl92ee_dm_dynamic_arfb_select(struct ieee80211_hw *hw,
+				    u8 rate, bool collision_state)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (rate >= DESC92C_RATEMCS8  && rate <= DESC92C_RATEMCS12) {
+		if (collision_state == 1) {
+			if (rate == DESC92C_RATEMCS12) {
+				rtl_write_dword(rtlpriv, REG_DARFRC, 0x0);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x07060501);
+			} else if (rate == DESC92C_RATEMCS11) {
+				rtl_write_dword(rtlpriv, REG_DARFRC, 0x0);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x07070605);
+			} else if (rate == DESC92C_RATEMCS10) {
+				rtl_write_dword(rtlpriv, REG_DARFRC, 0x0);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x08080706);
+			} else if (rate == DESC92C_RATEMCS9) {
+				rtl_write_dword(rtlpriv, REG_DARFRC, 0x0);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x08080707);
+			} else {
+				rtl_write_dword(rtlpriv, REG_DARFRC, 0x0);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x09090808);
+			}
+		} else {   /* collision_state == 0 */
+			if (rate == DESC92C_RATEMCS12) {
+				rtl_write_dword(rtlpriv, REG_DARFRC,
+						0x05010000);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x09080706);
+			} else if (rate == DESC92C_RATEMCS11) {
+				rtl_write_dword(rtlpriv, REG_DARFRC,
+						0x06050000);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x09080807);
+			} else if (rate == DESC92C_RATEMCS10) {
+				rtl_write_dword(rtlpriv, REG_DARFRC,
+						0x07060000);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x0a090908);
+			} else if (rate == DESC92C_RATEMCS9) {
+				rtl_write_dword(rtlpriv, REG_DARFRC,
+						0x07070000);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x0a090808);
+			} else {
+				rtl_write_dword(rtlpriv, REG_DARFRC,
+						0x08080000);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x0b0a0909);
+			}
+		}
+	} else {  /* MCS13~MCS15,  1SS, G-mode */
+		if (collision_state == 1) {
+			if (rate == DESC92C_RATEMCS15) {
+				rtl_write_dword(rtlpriv, REG_DARFRC,
+						0x00000000);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x05040302);
+			} else if (rate == DESC92C_RATEMCS14) {
+				rtl_write_dword(rtlpriv, REG_DARFRC,
+						0x00000000);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x06050302);
+			} else if (rate == DESC92C_RATEMCS13) {
+				rtl_write_dword(rtlpriv, REG_DARFRC,
+						0x00000000);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x07060502);
+			} else {
+				rtl_write_dword(rtlpriv, REG_DARFRC,
+						0x00000000);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x06050402);
+			}
+		} else{   /* collision_state == 0 */
+			if (rate == DESC92C_RATEMCS15) {
+				rtl_write_dword(rtlpriv, REG_DARFRC,
+						0x03020000);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x07060504);
+			} else if (rate == DESC92C_RATEMCS14) {
+				rtl_write_dword(rtlpriv, REG_DARFRC,
+						0x03020000);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x08070605);
+			} else if (rate == DESC92C_RATEMCS13) {
+				rtl_write_dword(rtlpriv, REG_DARFRC,
+						0x05020000);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x09080706);
+			} else {
+				rtl_write_dword(rtlpriv, REG_DARFRC,
+						0x04020000);
+				rtl_write_dword(rtlpriv, REG_DARFRC + 4,
+						0x08070605);
+			}
+		}
+	}
+}
+
+void rtl92ee_dm_watchdog(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	bool b_fw_current_inpsmode = false;
+	bool b_fw_ps_awake = true;
+
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
+				      (u8 *) (&b_fw_current_inpsmode));
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FWLPS_RF_ON,
+				      (u8 *) (&b_fw_ps_awake));
+	if (ppsc->p2p_ps_info.p2p_ps_mode)
+		b_fw_ps_awake = false;
+
+	if ((ppsc->rfpwr_state == ERFON) &&
+	    ((!b_fw_current_inpsmode) && b_fw_ps_awake) &&
+	    (!ppsc->rfchange_inprogress)) {
+		rtl92ee_dm_common_info_self_update(hw);
+		rtl92ee_dm_false_alarm_counter_statistics(hw);
+		rtl92ee_dm_check_rssi_monitor(hw);
+		rtl92ee_dm_dig(hw);
+		rtl92ee_dm_adaptivity(hw);
+		rtl92ee_dm_cck_packet_detection_thresh(hw);
+		rtl92ee_dm_refresh_rate_adaptive_mask(hw);
+		rtl92ee_dm_check_edca_turbo(hw);
+		rtl92ee_dm_dynamic_atc_switch(hw);
+		rtl92ee_dm_dynamic_primary_cca_ckeck(hw);
+	}
+}
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/dm.h b/drivers/staging/rtl8192ee/rtl8192ee/dm.h
new file mode 100644
index 00000000000..30b8fa6d3f7
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/dm.h
@@ -0,0 +1,343 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef	__RTL92E_DM_H__
+#define __RTL92E_DM_H__
+
+#define	OFDMCCA_TH				500
+#define	BW_IND_BIAS				500
+#define	MF_USC					2
+#define	MF_LSC					1
+#define	MF_USC_LSC				0
+#define	MONITOR_TIME				30
+
+#define	MAIN_ANT				0
+#define	AUX_ANT					1
+#define	MAIN_ANT_CG_TRX				1
+#define	AUX_ANT_CG_TRX				0
+#define	MAIN_ANT_CGCS_RX			0
+#define	AUX_ANT_CGCS_RX				1
+
+/*RF REG LIST*/
+#define	DM_REG_RF_MODE_11N			0x00
+#define	DM_REG_RF_0B_11N			0x0B
+#define	DM_REG_CHNBW_11N			0x18
+#define	DM_REG_T_METER_11N			0x24
+#define	DM_REG_RF_25_11N			0x25
+#define	DM_REG_RF_26_11N			0x26
+#define	DM_REG_RF_27_11N			0x27
+#define	DM_REG_RF_2B_11N			0x2B
+#define	DM_REG_RF_2C_11N			0x2C
+#define	DM_REG_RXRF_A3_11N			0x3C
+#define	DM_REG_T_METER_92D_11N			0x42
+#define	DM_REG_T_METER_92E_11N			0x42
+
+
+
+/*BB REG LIST*/
+/*PAGE 8 */
+#define	DM_REG_BB_CTRL_11N			0x800
+#define	DM_REG_RF_PIN_11N			0x804
+#define	DM_REG_PSD_CTRL_11N			0x808
+#define	DM_REG_TX_ANT_CTRL_11N			0x80C
+#define	DM_REG_BB_PWR_SAV5_11N			0x818
+#define	DM_REG_CCK_RPT_FORMAT_11N		0x824
+#define	DM_REG_RX_DEFUALT_A_11N			0x858
+#define	DM_REG_RX_DEFUALT_B_11N			0x85A
+#define	DM_REG_BB_PWR_SAV3_11N			0x85C
+#define	DM_REG_ANTSEL_CTRL_11N			0x860
+#define	DM_REG_RX_ANT_CTRL_11N			0x864
+#define	DM_REG_PIN_CTRL_11N			0x870
+#define	DM_REG_BB_PWR_SAV1_11N			0x874
+#define	DM_REG_ANTSEL_PATH_11N			0x878
+#define	DM_REG_BB_3WIRE_11N			0x88C
+#define	DM_REG_SC_CNT_11N			0x8C4
+#define	DM_REG_PSD_DATA_11N			0x8B4
+/*PAGE 9*/
+#define	DM_REG_ANT_MAPPING1_11N			0x914
+#define	DM_REG_ANT_MAPPING2_11N			0x918
+/*PAGE A*/
+#define	DM_REG_CCK_ANTDIV_PARA1_11N		0xA00
+#define	DM_REG_CCK_CCA_11N			0xA0A
+#define	DM_REG_CCK_ANTDIV_PARA2_11N		0xA0C
+#define	DM_REG_CCK_ANTDIV_PARA3_11N		0xA10
+#define	DM_REG_CCK_ANTDIV_PARA4_11N		0xA14
+#define	DM_REG_CCK_FILTER_PARA1_11N		0xA22
+#define	DM_REG_CCK_FILTER_PARA2_11N		0xA23
+#define	DM_REG_CCK_FILTER_PARA3_11N		0xA24
+#define	DM_REG_CCK_FILTER_PARA4_11N		0xA25
+#define	DM_REG_CCK_FILTER_PARA5_11N		0xA26
+#define	DM_REG_CCK_FILTER_PARA6_11N		0xA27
+#define	DM_REG_CCK_FILTER_PARA7_11N		0xA28
+#define	DM_REG_CCK_FILTER_PARA8_11N		0xA29
+#define	DM_REG_CCK_FA_RST_11N			0xA2C
+#define	DM_REG_CCK_FA_MSB_11N			0xA58
+#define	DM_REG_CCK_FA_LSB_11N			0xA5C
+#define	DM_REG_CCK_CCA_CNT_11N			0xA60
+#define	DM_REG_BB_PWR_SAV4_11N			0xA74
+/*PAGE B */
+#define	DM_REG_LNA_SWITCH_11N			0xB2C
+#define	DM_REG_PATH_SWITCH_11N			0xB30
+#define	DM_REG_RSSI_CTRL_11N			0xB38
+#define	DM_REG_CONFIG_ANTA_11N			0xB68
+#define	DM_REG_RSSI_BT_11N			0xB9C
+/*PAGE C */
+#define	DM_REG_OFDM_FA_HOLDC_11N		0xC00
+#define	DM_REG_RX_PATH_11N			0xC04
+#define	DM_REG_TRMUX_11N			0xC08
+#define	DM_REG_OFDM_FA_RSTC_11N			0xC0C
+#define	DM_REG_RXIQI_MATRIX_11N			0xC14
+#define	DM_REG_TXIQK_MATRIX_LSB1_11N		0xC4C
+#define	DM_REG_IGI_A_11N			0xC50
+#define	DM_REG_ANTDIV_PARA2_11N			0xC54
+#define	DM_REG_IGI_B_11N			0xC58
+#define	DM_REG_ANTDIV_PARA3_11N			0xC5C
+#define DM_REG_L1SBD_PD_CH_11N			0XC6C
+#define	DM_REG_BB_PWR_SAV2_11N			0xC70
+#define	DM_REG_RX_OFF_11N			0xC7C
+#define	DM_REG_TXIQK_MATRIXA_11N		0xC80
+#define	DM_REG_TXIQK_MATRIXB_11N		0xC88
+#define	DM_REG_TXIQK_MATRIXA_LSB2_11N		0xC94
+#define	DM_REG_TXIQK_MATRIXB_LSB2_11N		0xC9C
+#define	DM_REG_RXIQK_MATRIX_LSB_11N		0xCA0
+#define	DM_REG_ANTDIV_PARA1_11N			0xCA4
+#define	DM_REG_OFDM_FA_TYPE1_11N		0xCF0
+/*PAGE D */
+#define	DM_REG_OFDM_FA_RSTD_11N			0xD00
+#define	DM_REG_OFDM_FA_TYPE2_11N		0xDA0
+#define	DM_REG_OFDM_FA_TYPE3_11N		0xDA4
+#define	DM_REG_OFDM_FA_TYPE4_11N		0xDA8
+/*PAGE E */
+#define	DM_REG_TXAGC_A_6_18_11N			0xE00
+#define	DM_REG_TXAGC_A_24_54_11N		0xE04
+#define	DM_REG_TXAGC_A_1_MCS32_11N		0xE08
+#define	DM_REG_TXAGC_A_MCS0_3_11N		0xE10
+#define	DM_REG_TXAGC_A_MCS4_7_11N		0xE14
+#define	DM_REG_TXAGC_A_MCS8_11_11N		0xE18
+#define	DM_REG_TXAGC_A_MCS12_15_11N		0xE1C
+#define	DM_REG_FPGA0_IQK_11N			0xE28
+#define	DM_REG_TXIQK_TONE_A_11N			0xE30
+#define	DM_REG_RXIQK_TONE_A_11N			0xE34
+#define	DM_REG_TXIQK_PI_A_11N			0xE38
+#define	DM_REG_RXIQK_PI_A_11N			0xE3C
+#define	DM_REG_TXIQK_11N			0xE40
+#define	DM_REG_RXIQK_11N			0xE44
+#define	DM_REG_IQK_AGC_PTS_11N			0xE48
+#define	DM_REG_IQK_AGC_RSP_11N			0xE4C
+#define	DM_REG_BLUETOOTH_11N			0xE6C
+#define	DM_REG_RX_WAIT_CCA_11N			0xE70
+#define	DM_REG_TX_CCK_RFON_11N			0xE74
+#define	DM_REG_TX_CCK_BBON_11N			0xE78
+#define	DM_REG_OFDM_RFON_11N			0xE7C
+#define	DM_REG_OFDM_BBON_11N			0xE80
+#define		DM_REG_TX2RX_11N		0xE84
+#define	DM_REG_TX2TX_11N			0xE88
+#define	DM_REG_RX_CCK_11N			0xE8C
+#define	DM_REG_RX_OFDM_11N			0xED0
+#define	DM_REG_RX_WAIT_RIFS_11N			0xED4
+#define	DM_REG_RX2RX_11N			0xED8
+#define	DM_REG_STANDBY_11N			0xEDC
+#define	DM_REG_SLEEP_11N			0xEE0
+#define	DM_REG_PMPD_ANAEN_11N			0xEEC
+
+
+/*MAC REG LIST*/
+#define	DM_REG_BB_RST_11N			0x02
+#define	DM_REG_ANTSEL_PIN_11N			0x4C
+#define	DM_REG_EARLY_MODE_11N			0x4D0
+#define	DM_REG_RSSI_MONITOR_11N			0x4FE
+#define	DM_REG_EDCA_VO_11N			0x500
+#define	DM_REG_EDCA_VI_11N			0x504
+#define	DM_REG_EDCA_BE_11N			0x508
+#define	DM_REG_EDCA_BK_11N			0x50C
+#define	DM_REG_TXPAUSE_11N			0x522
+#define	DM_REG_RESP_TX_11N			0x6D8
+#define	DM_REG_ANT_TRAIN_PARA1_11N		0x7b0
+#define	DM_REG_ANT_TRAIN_PARA2_11N		0x7b4
+
+
+/*DIG Related*/
+#define	DM_BIT_IGI_11N				0x0000007F
+
+
+
+#define HAL_DM_DIG_DISABLE			BIT(0)
+#define HAL_DM_HIPWR_DISABLE			BIT(1)
+
+#define OFDM_TABLE_LENGTH			43
+#define CCK_TABLE_LENGTH			33
+
+#define OFDM_TABLE_SIZE				43
+#define CCK_TABLE_SIZE				33
+
+#define BW_AUTO_SWITCH_HIGH_LOW			25
+#define BW_AUTO_SWITCH_LOW_HIGH			30
+
+#define DM_DIG_THRESH_HIGH			40
+#define DM_DIG_THRESH_LOW			35
+
+#define DM_FALSEALARM_THRESH_LOW		400
+#define DM_FALSEALARM_THRESH_HIGH		1000
+
+#define DM_DIG_MAX				0x3e
+#define DM_DIG_MIN				0x1e
+
+#define DM_DIG_MAX_AP				0x32
+#define DM_DIG_MIN_AP				0x20
+
+#define DM_DIG_FA_UPPER				0x3e
+#define DM_DIG_FA_LOWER				0x1e
+#define DM_DIG_FA_TH0				0x200
+#define DM_DIG_FA_TH1				0x300
+#define DM_DIG_FA_TH2				0x400
+
+#define DM_DIG_BACKOFF_MAX			12
+#define DM_DIG_BACKOFF_MIN			-4
+#define DM_DIG_BACKOFF_DEFAULT			10
+
+#define RXPATHSELECTION_SS_TH_lOW		30
+#define RXPATHSELECTION_DIFF_TH			18
+
+#define DM_RATR_STA_INIT			0
+#define DM_RATR_STA_HIGH			1
+#define DM_RATR_STA_MIDDLE			2
+#define DM_RATR_STA_LOW				3
+
+#define CTS2SELF_THVAL				30
+#define REGC38_TH				20
+
+#define WAIOTTHVal				25
+
+#define TXHIGHPWRLEVEL_NORMAL			0
+#define TXHIGHPWRLEVEL_LEVEL1			1
+#define TXHIGHPWRLEVEL_LEVEL2			2
+#define TXHIGHPWRLEVEL_BT1			3
+#define TXHIGHPWRLEVEL_BT2			4
+
+#define DM_TYPE_BYFW				0
+#define DM_TYPE_BYDRIVER			1
+
+#define TX_POWER_NEAR_FIELD_THRESH_LVL2		74
+#define TX_POWER_NEAR_FIELD_THRESH_LVL1		67
+#define TXPWRTRACK_MAX_IDX			6
+
+/* Dynamic ATC switch */
+#define ATC_STATUS_OFF				0x0	/* enable */
+#define	ATC_STATUS_ON				0x1	/* disable */
+#define	CFO_THRESHOLD_XTAL			10	/* kHz */
+#define	CFO_THRESHOLD_ATC			80	/* kHz */
+
+/* RSSI Dump Message */
+#define RA_RSSIDUMP				0xcb0
+#define RB_RSSIDUMP				0xcb1
+#define RS1_RXEVMDUMP				0xcb2
+#define RS2_RXEVMDUMP				0xcb3
+#define RA_RXSNRDUMP				0xcb4
+#define RB_RXSNRDUMP				0xcb5
+#define RA_CFOSHORTDUMP				0xcb6
+#define RB_CFOSHORTDUMP				0xcb8
+#define RA_CFOLONGDUMP				0xcba
+#define RB_CFOLONGDUMP				0xcbc
+
+struct ps_t {
+	u8 pre_ccastate;
+	u8 cur_ccasate;
+	u8 pre_rfstate;
+	u8 cur_rfstate;
+	long rssi_val_min;
+
+};
+
+struct dig_t {
+	u8 dig_enable_flag;
+	u8 dig_ext_port_stage;
+	u32 rssi_lowthresh;
+	u32 rssi_highthresh;
+
+	u32 fa_lowthresh;
+	u32 fa_highthresh;
+
+	u8 cursta_connectctate;
+	u8 presta_connectstate;
+	u8 curmultista_connectstate;
+
+	u8 pre_igvalue;
+	u8 cur_igvalue;
+	u8 backup_igvalue;
+	u8 bt30_cur_igi;
+	u8 stop_dig;
+
+	char backoff_val;
+	char backoff_val_range_max;
+	char backoff_val_range_min;
+	u8 rx_gain_range_max;
+	u8 rx_gain_range_min;
+	u8 rssi_val_min;
+
+	u8 pre_cck_cca_thres;
+	u8 cur_cck_cca_thres;
+	u8 pre_cck_pd_state;
+	u8 cur_cck_pd_state;
+
+	u8 large_fa_hit;
+	u8 forbidden_igi;
+	u32 recover_cnt;
+
+	char th_l2h_ini;
+	char th_edcca_hl_diff;
+	char igi_base;
+	u8 igi_target;
+	bool force_edcca;
+	u8 adapen_rssi;
+
+	u8 dig_dynamic_min_0;
+	u8 dig_dynamic_min_1;
+	bool b_media_connect_0;
+	bool b_media_connect_1;
+
+	u32 antdiv_rssi_max;
+	u32 rssi_max;
+};
+
+enum pwr_track_control_method {
+	BBSWING,
+	TXAGC
+};
+
+extern struct dig_t dm_dig;
+void rtl92ee_dm_init(struct ieee80211_hw *hw);
+void rtl92ee_dm_watchdog(struct ieee80211_hw *hw);
+void rtl92ee_dm_write_cck_cca_thres(struct ieee80211_hw *hw,
+				    u8 cur_thres);
+void rtl92ee_dm_write_dig(struct ieee80211_hw *hw, u8 current_igi);
+void rtl92ee_dm_init_edca_turbo(struct ieee80211_hw *hw);
+void rtl92ee_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw);
+void rtl92ee_dm_dynamic_arfb_select(struct ieee80211_hw *hw,
+				    u8 rate, bool collision_state);
+#endif
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/fw.c b/drivers/staging/rtl8192ee/rtl8192ee/fw.c
new file mode 100644
index 00000000000..ea6cafa80f4
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/fw.c
@@ -0,0 +1,945 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../pci.h"
+#include "../base.h"
+#include "reg.h"
+#include "def.h"
+#include "fw.h"
+#include "dm.h"
+
+static void _rtl92ee_enable_fw_download(struct ieee80211_hw *hw, bool enable)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmp;
+
+	if (enable) {
+		rtl_write_byte(rtlpriv, REG_MCUFWDL, 0x05);
+
+		tmp = rtl_read_byte(rtlpriv, REG_MCUFWDL + 2);
+		rtl_write_byte(rtlpriv, REG_MCUFWDL + 2, tmp & 0xf7);
+	} else {
+
+		tmp = rtl_read_byte(rtlpriv, REG_MCUFWDL);
+		rtl_write_byte(rtlpriv, REG_MCUFWDL, tmp & 0xfe);
+	}
+}
+
+static void _rtl92ee_fw_block_write(struct ieee80211_hw *hw,
+				    const u8 *buffer, u32 size)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 blockSize = sizeof(u32);
+	u8 *bufferPtr = (u8 *) buffer;
+	u32 *pu4BytePtr = (u32 *) buffer;
+	u32 i, offset, blockCount, remainSize;
+
+	blockCount = size / blockSize;
+	remainSize = size % blockSize;
+
+	for (i = 0; i < blockCount; i++) {
+		offset = i * blockSize;
+		rtl_write_dword(rtlpriv, (FW_8192C_START_ADDRESS + offset),
+				*(pu4BytePtr + i));
+	}
+
+	if (remainSize) {
+		offset = blockCount * blockSize;
+		bufferPtr += offset;
+		for (i = 0; i < remainSize; i++) {
+			rtl_write_byte(rtlpriv,
+				       (FW_8192C_START_ADDRESS + offset + i),
+				       *(bufferPtr + i));
+		}
+	}
+}
+
+static void _rtl92ee_fw_page_write(struct ieee80211_hw *hw, u32 page,
+				   const u8 *buffer, u32 size)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 value8;
+	u8 u8page = (u8) (page & 0x07);
+
+	value8 = (rtl_read_byte(rtlpriv, REG_MCUFWDL + 2) & 0xF8) | u8page;
+	rtl_write_byte(rtlpriv, (REG_MCUFWDL + 2), value8);
+
+	_rtl92ee_fw_block_write(hw, buffer, size);
+}
+
+static void _rtl92ee_fill_dummy(u8 *pfwbuf, u32 *pfwlen)
+{
+	u32 fwlen = *pfwlen;
+	u8 remain = (u8) (fwlen % 4);
+
+	remain = (remain == 0) ? 0 : (4 - remain);
+
+	while (remain > 0) {
+		pfwbuf[fwlen] = 0;
+		fwlen++;
+		remain--;
+	}
+
+	*pfwlen = fwlen;
+}
+
+static void _rtl92ee_write_fw(struct ieee80211_hw *hw,
+			      enum version_8192e version,
+			      u8 *buffer, u32 size)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 *bufferPtr = (u8 *) buffer;
+	u32 pageNums, remainSize;
+	u32 page, offset;
+
+	RT_TRACE(COMP_FW, DBG_LOUD , ("FW size is %d bytes,\n", size));
+
+	_rtl92ee_fill_dummy(bufferPtr, &size);
+
+	pageNums = size / FW_8192C_PAGE_SIZE;
+	remainSize = size % FW_8192C_PAGE_SIZE;
+
+	if (pageNums > 8) {
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("Page numbers should not greater then 8\n"));
+	}
+
+	for (page = 0; page < pageNums; page++) {
+		offset = page * FW_8192C_PAGE_SIZE;
+		_rtl92ee_fw_page_write(hw, page, (bufferPtr + offset),
+				      FW_8192C_PAGE_SIZE);
+		udelay(2);
+	}
+
+	if (remainSize) {
+		offset = pageNums * FW_8192C_PAGE_SIZE;
+		page = pageNums;
+		_rtl92ee_fw_page_write(hw, page, (bufferPtr + offset),
+				       remainSize);
+	}
+
+}
+
+static int _rtl92ee_fw_free_to_go(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int err = -EIO;
+	u32 counter = 0;
+	u32 value32;
+
+	do {
+		value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
+	} while ((counter++ < FW_8192C_POLLING_TIMEOUT_COUNT) &&
+		 (!(value32 & FWDL_ChkSum_rpt)));
+
+	if (counter >= FW_8192C_POLLING_TIMEOUT_COUNT) {
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("chksum report faill ! REG_MCUFWDL:0x%08x .\n",
+			  value32));
+		goto exit;
+	}
+
+	RT_TRACE(COMP_FW, DBG_TRACE,
+		 ("Checksum report OK ! REG_MCUFWDL:0x%08x .\n", value32));
+
+	value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
+	value32 |= MCUFWDL_RDY;
+	value32 &= ~WINTINI_RDY;
+	rtl_write_dword(rtlpriv, REG_MCUFWDL, value32);
+
+	rtl92ee_firmware_selfreset(hw);
+	counter = 0;
+
+	do {
+		value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
+		if (value32 & WINTINI_RDY) {
+			RT_TRACE(COMP_FW, DBG_LOUD ,
+				("Polling FW ready success!! REG_MCUFWDL:"
+				"0x%08x. count = %d\n", value32, counter));
+			err = 0;
+			goto exit;
+		}
+
+		udelay(FW_8192C_POLLING_DELAY*10);
+
+	} while (counter++ < FW_8192C_POLLING_TIMEOUT_COUNT);
+
+	RT_TRACE(COMP_ERR, DBG_EMERG,
+		 ("Polling FW ready fail!! REG_MCUFWDL:0x%08x. count = %d\n",
+		 value32, counter));
+
+exit:
+	return err;
+}
+
+int rtl92ee_download_fw(struct ieee80211_hw *hw, bool buse_wake_on_wlan_fw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl92c_firmware_header *pfwheader;
+	u8 *pfwdata;
+	u32 fwsize;
+	int err;
+	enum version_8192e version = rtlhal->version;
+
+	if (!rtlhal->pfirmware)
+		return 1;
+
+	pfwheader = (struct rtl92c_firmware_header *)rtlhal->pfirmware;
+	rtlhal->fw_version = pfwheader->version;
+	rtlhal->fw_subversion = pfwheader->subversion;
+	pfwdata = (u8 *) rtlhal->pfirmware;
+	fwsize = rtlhal->fwsize;
+	RT_TRACE(COMP_FW, DBG_DMESG,
+		 ("normal Firmware SIZE %d\n" , fwsize));
+
+	if (IS_FW_HEADER_EXIST(pfwheader)) {
+		RT_TRACE(COMP_FW, DBG_DMESG,
+			 ("Firmware Version(%d), Signature(%#x), Size(%d)\n",
+			  pfwheader->version, pfwheader->signature,
+			  (int)sizeof(struct rtl92c_firmware_header)));
+
+		pfwdata = pfwdata + sizeof(struct rtl92c_firmware_header);
+		fwsize = fwsize - sizeof(struct rtl92c_firmware_header);
+	} else {
+		RT_TRACE(COMP_FW, DBG_DMESG,
+			 ("Firmware no Header, Signature(%#x)\n",
+			  pfwheader->signature));
+	}
+
+	if (rtlhal->b_mac_func_enable) {
+		if (rtl_read_byte(rtlpriv, REG_MCUFWDL) & BIT(7)) {
+			rtl_write_byte(rtlpriv, REG_MCUFWDL, 0);
+			rtl92ee_firmware_selfreset(hw);
+		}
+	}
+	_rtl92ee_enable_fw_download(hw, true);
+	_rtl92ee_write_fw(hw, version, pfwdata, fwsize);
+	_rtl92ee_enable_fw_download(hw, false);
+
+	err = _rtl92ee_fw_free_to_go(hw);
+	if (err) {
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("Firmware is not ready to run!\n"));
+	} else {
+		RT_TRACE(COMP_FW, DBG_LOUD ,
+			 ("Firmware is ready to run!\n"));
+	}
+
+	return 0;
+}
+
+static bool _rtl92ee_check_fw_read_last_h2c(struct ieee80211_hw *hw, u8 boxnum)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 val_hmetfr;
+	bool result = false;
+
+	val_hmetfr = rtl_read_byte(rtlpriv, REG_HMETFR);
+	if (((val_hmetfr >> boxnum) & BIT(0)) == 0)
+		result = true;
+	return result;
+}
+
+static void _rtl92ee_fill_h2c_command(struct ieee80211_hw *hw, u8 element_id,
+				      u32 cmd_len, u8 *p_cmdbuffer)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	u8 boxnum;
+	u16 box_reg = 0, box_extreg = 0;
+	u8 u1b_tmp;
+	bool isfw_read = false;
+	u8 buf_index = 0;
+	bool bwrite_sucess = false;
+	u8 wait_h2c_limmit = 100;
+	u8 boxcontent[4], boxextcontent[4];
+	u32 h2c_waitcounter = 0;
+	unsigned long flag;
+	u8 idx;
+
+	if (ppsc->dot11_psmode != EACTIVE ||
+		ppsc->inactive_pwrstate == ERFOFF) {
+		RT_TRACE(COMP_CMD, DBG_LOUD ,
+			("FillH2CCommand8192E(): "
+			"Return because RF is off!!!\n"));
+		return;
+	}
+
+	RT_TRACE(COMP_CMD, DBG_LOUD , ("come in\n"));
+
+	/* 1. Prevent race condition in setting H2C cmd.
+	 * (copy from MgntActSet_RF_State().)
+	 */
+	while (true) {
+		spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
+		if (rtlhal->b_h2c_setinprogress) {
+			RT_TRACE(COMP_CMD, DBG_LOUD ,
+				 ("H2C set in progress! Wait to set.."
+				  "element_id(%d).\n", element_id));
+
+			while (rtlhal->b_h2c_setinprogress) {
+				spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock,
+						       flag);
+				h2c_waitcounter++;
+				RT_TRACE(COMP_CMD, DBG_LOUD ,
+					 ("Wait 100 us (%d times)...\n",
+					  h2c_waitcounter));
+				udelay(100);
+
+				if (h2c_waitcounter > 1000)
+					return;
+				spin_lock_irqsave(&rtlpriv->locks.h2c_lock,
+						  flag);
+			}
+			spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
+		} else {
+			rtlhal->b_h2c_setinprogress = true;
+			spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
+			break;
+		}
+	}
+
+	while (!bwrite_sucess) {
+		/*	cosa remove this because never reach this. */
+		/*wait_writeh2c_limmit--;
+		if (wait_writeh2c_limmit == 0) {
+			RT_TRACE(COMP_ERR, DBG_EMERG,
+				 ("Write H2C fail because no trigger "
+				  "for FW INT!\n"));
+			break;
+		}
+		*/
+		/* 2. Find the last BOX number which has been writen. */
+		boxnum = rtlhal->last_hmeboxnum;
+		switch (boxnum) {
+		case 0:
+			box_reg = REG_HMEBOX_0;
+			box_extreg = REG_HMEBOX_EXT_0;
+			break;
+		case 1:
+			box_reg = REG_HMEBOX_1;
+			box_extreg = REG_HMEBOX_EXT_1;
+			break;
+		case 2:
+			box_reg = REG_HMEBOX_2;
+			box_extreg = REG_HMEBOX_EXT_2;
+			break;
+		case 3:
+			box_reg = REG_HMEBOX_3;
+			box_extreg = REG_HMEBOX_EXT_3;
+			break;
+		default:
+			RT_TRACE(COMP_ERR, DBG_EMERG,
+				 ("switch case not process\n"));
+			break;
+		}
+
+		/* 3. Check if the box content is empty. */
+		isfw_read = false;
+		u1b_tmp = rtl_read_byte(rtlpriv, REG_CR);
+
+		if (u1b_tmp != 0xea) {
+			isfw_read = true;
+		} else {
+			if (rtl_read_byte(rtlpriv, REG_TXDMA_STATUS) == 0xea ||
+			    rtl_read_byte(rtlpriv, REG_TXPKT_EMPTY) == 0xea)
+				rtl_write_byte(rtlpriv, REG_SYS_CFG1 + 3, 0xff);
+		}
+
+		if (isfw_read == true) {
+			wait_h2c_limmit = 100;
+			isfw_read = _rtl92ee_check_fw_read_last_h2c(hw, boxnum);
+			while (!isfw_read) {
+				wait_h2c_limmit--;
+				if (wait_h2c_limmit == 0) {
+					RT_TRACE(COMP_CMD, DBG_LOUD ,
+						("Wating too long for FW"
+						"read clear HMEBox(%d)!!!\n",
+						boxnum));
+					break;
+				}
+				udelay(10);
+				isfw_read = _rtl92ee_check_fw_read_last_h2c(hw,
+									boxnum);
+				u1b_tmp = rtl_read_byte(rtlpriv, 0x130);
+				RT_TRACE(COMP_CMD, DBG_LOUD ,
+					 ("Wating for FW read clear HMEBox(%d)!!! 0x130 = %2x\n",
+					 boxnum, u1b_tmp));
+			}
+		}
+
+		/* If Fw has not read the last
+		 H2C cmd, break and give up this H2C. */
+		if (!isfw_read) {
+			RT_TRACE(COMP_CMD, DBG_LOUD ,
+				 ("Write H2C reg BOX[%d] fail, Fw don't read.\n",
+				 boxnum));
+			break;
+		}
+		/* 4. Fill the H2C cmd into box */
+		memset(boxcontent, 0, sizeof(boxcontent));
+		memset(boxextcontent, 0, sizeof(boxextcontent));
+		boxcontent[0] = element_id;
+		RT_TRACE(COMP_CMD, DBG_LOUD ,
+			 ("Write element_id box_reg(%4x) = %2x\n",
+			  box_reg, element_id));
+
+		switch (cmd_len) {
+		case 1:
+		case 2:
+		case 3:
+			/*boxcontent[0] &= ~(BIT(7));*/
+			memcpy((u8 *) (boxcontent) + 1,
+			       p_cmdbuffer + buf_index, cmd_len);
+
+			for (idx = 0; idx < 4; idx++) {
+				rtl_write_byte(rtlpriv, box_reg + idx,
+					       boxcontent[idx]);
+			}
+			break;
+		case 4:
+		case 5:
+		case 6:
+		case 7:
+			/*boxcontent[0] |= (BIT(7));*/
+			memcpy((u8 *) (boxextcontent),
+			       p_cmdbuffer + buf_index+3, cmd_len-3);
+			memcpy((u8 *) (boxcontent) + 1,
+			       p_cmdbuffer + buf_index, 3);
+
+			for (idx = 0; idx < 4; idx++) {
+				rtl_write_byte(rtlpriv, box_extreg + idx,
+					       boxextcontent[idx]);
+			}
+
+			for (idx = 0; idx < 4; idx++) {
+				rtl_write_byte(rtlpriv, box_reg + idx,
+					       boxcontent[idx]);
+			}
+			break;
+		default:
+			RT_TRACE(COMP_ERR, DBG_EMERG,
+				 ("switch case not process\n"));
+			break;
+		}
+
+		bwrite_sucess = true;
+
+		rtlhal->last_hmeboxnum = boxnum + 1;
+		if (rtlhal->last_hmeboxnum == 4)
+			rtlhal->last_hmeboxnum = 0;
+
+		RT_TRACE(COMP_CMD, DBG_LOUD ,
+			 ("pHalData->last_hmeboxnum  = %d\n",
+			  rtlhal->last_hmeboxnum));
+	}
+
+	spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
+	rtlhal->b_h2c_setinprogress = false;
+	spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
+
+	RT_TRACE(COMP_CMD, DBG_LOUD , ("go out\n"));
+}
+
+void rtl92ee_fill_h2c_cmd(struct ieee80211_hw *hw,
+			 u8 element_id, u32 cmd_len, u8 *p_cmdbuffer)
+{
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u32 tmp_cmdbuf[2];
+
+	if (rtlhal->bfw_ready == false) {
+		RT_ASSERT(false, ("return H2C cmd because of Fw "
+				  "download fail!!!\n"));
+		return;
+	}
+
+	memset(tmp_cmdbuf, 0, 8);
+	memcpy(tmp_cmdbuf, p_cmdbuffer, cmd_len);
+	_rtl92ee_fill_h2c_command(hw, element_id, cmd_len, (u8 *)&tmp_cmdbuf);
+
+	return;
+}
+
+void rtl92ee_firmware_selfreset(struct ieee80211_hw *hw)
+{
+	u8 u1b_tmp;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL + 1);
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL + 1, (u1b_tmp & (~BIT(0))));
+
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN + 1, (u1b_tmp & (~BIT(2))));
+
+	udelay(50);
+
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL + 1);
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL + 1, (u1b_tmp | BIT(0)));
+
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN + 1, (u1b_tmp | BIT(2)));
+
+	RT_TRACE(COMP_INIT, DBG_LOUD ,
+		 ("  _8051Reset92E(): 8051 reset success .\n"));
+}
+
+void rtl92ee_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 u1_h2c_set_pwrmode[H2C_92E_PWEMODE_LENGTH] = { 0 };
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	u8 rlbm , power_state = 0;
+	RT_TRACE(COMP_POWER, DBG_LOUD , ("FW LPS mode = %d\n", mode));
+
+	SET_H2CCMD_PWRMODE_PARM_MODE(u1_h2c_set_pwrmode, ((mode) ? 1 : 0));
+	rlbm = 0;/*YJ, temp, 120316. FW now not support RLBM = 2.*/
+	SET_H2CCMD_PWRMODE_PARM_RLBM(u1_h2c_set_pwrmode, rlbm);
+	SET_H2CCMD_PWRMODE_PARM_SMART_PS(u1_h2c_set_pwrmode,
+					 (rtlpriv->mac80211.p2p) ?
+					 ppsc->smart_ps : 1);
+	SET_H2CCMD_PWRMODE_PARM_AWAKE_INTERVAL(u1_h2c_set_pwrmode,
+					       ppsc->reg_max_lps_awakeintvl);
+	SET_H2CCMD_PWRMODE_PARM_ALL_QUEUE_UAPSD(u1_h2c_set_pwrmode, 0);
+	if (mode == FW_PS_ACTIVE_MODE)
+		power_state |= FW_PWR_STATE_ACTIVE;
+	else
+		power_state |= FW_PWR_STATE_RF_OFF;
+	SET_H2CCMD_PWRMODE_PARM_PWR_STATE(u1_h2c_set_pwrmode, power_state);
+
+	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
+		      "rtl92c_set_fw_pwrmode(): u1_h2c_set_pwrmode\n",
+		      u1_h2c_set_pwrmode, H2C_92E_PWEMODE_LENGTH);
+	rtl92ee_fill_h2c_cmd(hw, H2C_92E_SETPWRMODE, H2C_92E_PWEMODE_LENGTH,
+			     u1_h2c_set_pwrmode);
+
+}
+
+void rtl92ee_set_fw_media_status_rpt_cmd(struct ieee80211_hw *hw, u8 mstatus)
+{
+	u8 parm[3] = { 0 , 0 , 0 };
+	/* parm[0]: bit0 = 0-->Disconnect, bit0 = 1-->Connect
+	 *          bit1 = 0-->update Media Status to MACID
+	 *          bit1 = 1-->update Media Status from MACID to MACID_End
+	 * parm[1]: MACID, if this is INFRA_STA, MacID = 0
+	 * parm[2]: MACID_End*/
+
+	SET_H2CCMD_MSRRPT_PARM_OPMODE(parm, mstatus);
+	SET_H2CCMD_MSRRPT_PARM_MACID_IND(parm, 0);
+
+	rtl92ee_fill_h2c_cmd(hw, H2C_92E_MSRRPT, 3, parm);
+}
+
+static bool _rtl92ee_cmd_send_packet(struct ieee80211_hw *hw,
+				     struct sk_buff *skb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl8192_tx_ring *ring;
+	struct rtl_tx_desc *pdesc;
+	unsigned long flags;
+	struct sk_buff *pskb = NULL;
+
+	ring = &rtlpci->tx_ring[BEACON_QUEUE];
+
+	pskb = __skb_dequeue(&ring->queue);
+	if (pskb)
+		kfree_skb(pskb);
+
+	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
+	/*this is wrong, fill_tx_cmddesc needs update*/
+	pdesc = &ring->desc[0];
+
+	rtlpriv->cfg->ops->fill_tx_cmddesc(hw, (u8 *) pdesc, 1, 1, skb);
+
+	__skb_queue_tail(&ring->queue, skb);
+
+	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
+
+	return true;
+}
+
+#define BEACON_PG		0 /* ->1 */
+#define PSPOLL_PG		2
+#define NULL_PG			3
+#define PROBERSP_PG		4 /* ->5 */
+
+#define TOTAL_RESERVED_PKT_LEN	768
+
+
+
+static u8 reserved_page_packet[TOTAL_RESERVED_PKT_LEN] = {
+	/* page 0 beacon */
+	0x80, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0x00, 0xE0, 0x4C, 0x02, 0xB1, 0x78,
+	0xEC, 0x1A, 0x59, 0x0B, 0xAD, 0xD4, 0x20, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x64, 0x00, 0x10, 0x04, 0x00, 0x05, 0x54, 0x65,
+	0x73, 0x74, 0x32, 0x01, 0x08, 0x82, 0x84, 0x0B,
+	0x16, 0x24, 0x30, 0x48, 0x6C, 0x03, 0x01, 0x06,
+	0x06, 0x02, 0x00, 0x00, 0x2A, 0x01, 0x02, 0x32,
+	0x04, 0x0C, 0x12, 0x18, 0x60, 0x2D, 0x1A, 0x6C,
+	0x09, 0x03, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x3D, 0x00, 0xDD, 0x07, 0x00, 0xE0, 0x4C,
+	0x02, 0x02, 0x00, 0x00, 0xDD, 0x18, 0x00, 0x50,
+	0xF2, 0x01, 0x01, 0x00, 0x00, 0x50, 0xF2, 0x04,
+	0x01, 0x00, 0x00, 0x50, 0xF2, 0x04, 0x01, 0x00,
+
+	/* page 1 beacon */
+	0x00, 0x50, 0xF2, 0x02, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x10, 0x00, 0x28, 0x8C, 0x00, 0x12, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x81, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	/* page 2  ps-poll */
+	0xA4, 0x10, 0x01, 0xC0, 0xEC, 0x1A, 0x59, 0x0B,
+	0xAD, 0xD4, 0x00, 0xE0, 0x4C, 0x02, 0xB1, 0x78,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x18, 0x00, 0x28, 0x8C, 0x00, 0x12, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	/* page 3  null */
+	0x48, 0x01, 0x00, 0x00, 0xEC, 0x1A, 0x59, 0x0B,
+	0xAD, 0xD4, 0x00, 0xE0, 0x4C, 0x02, 0xB1, 0x78,
+	0xEC, 0x1A, 0x59, 0x0B, 0xAD, 0xD4, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x72, 0x00, 0x28, 0x8C, 0x00, 0x12, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	/* page 4  probe_resp */
+	0x50, 0x00, 0x00, 0x00, 0x00, 0x40, 0x10, 0x10,
+	0x00, 0x03, 0x00, 0xE0, 0x4C, 0x76, 0x00, 0x42,
+	0x00, 0x40, 0x10, 0x10, 0x00, 0x03, 0x00, 0x00,
+	0x9E, 0x46, 0x15, 0x32, 0x27, 0xF2, 0x2D, 0x00,
+	0x64, 0x00, 0x00, 0x04, 0x00, 0x0C, 0x6C, 0x69,
+	0x6E, 0x6B, 0x73, 0x79, 0x73, 0x5F, 0x77, 0x6C,
+	0x61, 0x6E, 0x01, 0x04, 0x82, 0x84, 0x8B, 0x96,
+	0x03, 0x01, 0x01, 0x06, 0x02, 0x00, 0x00, 0x2A,
+	0x01, 0x00, 0x32, 0x08, 0x24, 0x30, 0x48, 0x6C,
+	0x0C, 0x12, 0x18, 0x60, 0x2D, 0x1A, 0x6C, 0x18,
+	0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x3D, 0x00, 0xDD, 0x06, 0x00, 0xE0, 0x4C, 0x02,
+	0x01, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+
+	/* page 5  probe_resp */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+};
+
+
+
+void rtl92ee_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct sk_buff *skb = NULL;
+
+	u32 totalpacketlen;
+	bool rtstatus;
+	u8 u1RsvdPageLoc[5] = { 0 };
+	bool b_dlok = false;
+
+	u8 *beacon;
+	u8 *p_pspoll;
+	u8 *nullfunc;
+	u8 *p_probersp;
+	/*---------------------------------------------------------
+				(1) beacon
+	---------------------------------------------------------*/
+	beacon = &reserved_page_packet[BEACON_PG * 128];
+	SET_80211_HDR_ADDRESS2(beacon, mac->mac_addr);
+	SET_80211_HDR_ADDRESS3(beacon, mac->bssid);
+
+	/*-------------------------------------------------------
+				(2) ps-poll
+	--------------------------------------------------------*/
+	p_pspoll = &reserved_page_packet[PSPOLL_PG * 128];
+	SET_80211_PS_POLL_AID(p_pspoll, (mac->assoc_id | 0xc000));
+	SET_80211_PS_POLL_BSSID(p_pspoll, mac->bssid);
+	SET_80211_PS_POLL_TA(p_pspoll, mac->mac_addr);
+
+	SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(u1RsvdPageLoc, PSPOLL_PG);
+
+	/*--------------------------------------------------------
+				(3) null data
+	---------------------------------------------------------*/
+	nullfunc = &reserved_page_packet[NULL_PG * 128];
+	SET_80211_HDR_ADDRESS1(nullfunc, mac->bssid);
+	SET_80211_HDR_ADDRESS2(nullfunc, mac->mac_addr);
+	SET_80211_HDR_ADDRESS3(nullfunc, mac->bssid);
+
+	SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(u1RsvdPageLoc, NULL_PG);
+
+	/*---------------------------------------------------------
+				(4) probe response
+	----------------------------------------------------------*/
+	p_probersp = &reserved_page_packet[PROBERSP_PG * 128];
+	SET_80211_HDR_ADDRESS1(p_probersp, mac->bssid);
+	SET_80211_HDR_ADDRESS2(p_probersp, mac->mac_addr);
+	SET_80211_HDR_ADDRESS3(p_probersp, mac->bssid);
+
+	SET_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(u1RsvdPageLoc, PROBERSP_PG);
+
+	totalpacketlen = TOTAL_RESERVED_PKT_LEN;
+
+	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD ,
+		      "rtl92ee_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL\n",
+		      &reserved_page_packet[0], totalpacketlen);
+	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD ,
+		      "rtl92ee_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL\n",
+		      u1RsvdPageLoc, 3);
+
+
+	skb = dev_alloc_skb(totalpacketlen);
+	memcpy((u8 *) skb_put(skb, totalpacketlen),
+	       &reserved_page_packet, totalpacketlen);
+
+	rtstatus = _rtl92ee_cmd_send_packet(hw, skb);
+
+	if (rtstatus)
+		b_dlok = true;
+
+	if (b_dlok) {
+		RT_TRACE(COMP_POWER, DBG_LOUD ,
+			 ("Set RSVD page location to Fw.\n"));
+		RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD ,
+			      "H2C_RSVDPAGE:\n", u1RsvdPageLoc, 3);
+		rtl92ee_fill_h2c_cmd(hw, H2C_92E_RSVDPAGE,
+				     sizeof(u1RsvdPageLoc), u1RsvdPageLoc);
+	} else
+		RT_TRACE(COMP_ERR, DBG_WARNING,
+			 ("Set RSVD page location to Fw FAIL!!!!!!.\n"));
+}
+
+/*Shoud check FW support p2p or not.*/
+static void rtl92ee_set_p2p_ctw_period_cmd(struct ieee80211_hw *hw, u8 ctwindow)
+{
+	u8 u1_ctwindow_period[1] = {ctwindow};
+
+	rtl92ee_fill_h2c_cmd(hw, H2C_92E_P2P_PS_CTW_CMD, 1, u1_ctwindow_period);
+
+}
+
+void rtl92ee_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *rtlps = rtl_psc(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_p2p_ps_info *p2pinfo = &(rtlps->p2p_ps_info);
+	struct p2p_ps_offload_t *p2p_ps_offload = &rtlhal->p2p_ps_offload;
+	u8 i;
+	u16 ctwindow;
+	u32 start_time, tsf_low;
+
+	switch (p2p_ps_state) {
+	case P2P_PS_DISABLE:
+		RT_TRACE(COMP_FW, DBG_LOUD , ("P2P_PS_DISABLE\n"));
+		memset(p2p_ps_offload, 0, 1);
+		break;
+	case P2P_PS_ENABLE:
+		RT_TRACE(COMP_FW, DBG_LOUD , ("P2P_PS_ENABLE\n"));
+		/* update CTWindow value. */
+		if (p2pinfo->ctwindow > 0) {
+			p2p_ps_offload->CTWindow_En = 1;
+			ctwindow = p2pinfo->ctwindow;
+			rtl92ee_set_p2p_ctw_period_cmd(hw, ctwindow);
+		}
+		/* hw only support 2 set of NoA */
+		for (i = 0 ; i < p2pinfo->noa_num ; i++) {
+			/* To control the register setting for which NOA*/
+			rtl_write_byte(rtlpriv, 0x5cf, (i << 4));
+			if (i == 0)
+				p2p_ps_offload->NoA0_En = 1;
+			else
+				p2p_ps_offload->NoA1_En = 1;
+			/* config P2P NoA Descriptor Register */
+			rtl_write_dword(rtlpriv, 0x5E0,
+					p2pinfo->noa_duration[i]);
+			rtl_write_dword(rtlpriv, 0x5E4,
+					p2pinfo->noa_interval[i]);
+
+			/*Get Current TSF value */
+			tsf_low = rtl_read_dword(rtlpriv, REG_TSFTR);
+
+			start_time = p2pinfo->noa_start_time[i];
+			if (p2pinfo->noa_count_type[i] != 1) {
+				while (start_time <= (tsf_low + (50 * 1024))) {
+					start_time += p2pinfo->noa_interval[i];
+					if (p2pinfo->noa_count_type[i] != 255)
+						p2pinfo->noa_count_type[i]--;
+				}
+			}
+			rtl_write_dword(rtlpriv, 0x5E8, start_time);
+			rtl_write_dword(rtlpriv, 0x5EC,
+					p2pinfo->noa_count_type[i]);
+		}
+		if ((p2pinfo->opp_ps == 1) || (p2pinfo->noa_num > 0)) {
+			/* rst p2p circuit */
+			rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST, BIT(4));
+			p2p_ps_offload->Offload_En = 1;
+
+			if (P2P_ROLE_GO == rtlpriv->mac80211.p2p) {
+				p2p_ps_offload->role = 1;
+				p2p_ps_offload->AllStaSleep = 0;
+			} else {
+				p2p_ps_offload->role = 0;
+			}
+			p2p_ps_offload->discovery = 0;
+		}
+		break;
+	case P2P_PS_SCAN:
+		RT_TRACE(COMP_FW, DBG_LOUD , ("P2P_PS_SCAN\n"));
+		p2p_ps_offload->discovery = 1;
+		break;
+	case P2P_PS_SCAN_DONE:
+		RT_TRACE(COMP_FW, DBG_LOUD , ("P2P_PS_SCAN_DONE\n"));
+		p2p_ps_offload->discovery = 0;
+		p2pinfo->p2p_ps_state = P2P_PS_ENABLE;
+		break;
+	default:
+		break;
+	}
+
+	rtl92ee_fill_h2c_cmd(hw, H2C_92E_P2P_PS_OFFLOAD, 1,
+			     (u8 *)p2p_ps_offload);
+
+}
+
+static void _rtl92ee_c2h_ra_report_handler(struct ieee80211_hw *hw,
+					   u8 *cmd_buf, u8 cmd_len)
+{
+	u8 rate = cmd_buf[0] & 0x3F;
+	bool collision_state = cmd_buf[3] & BIT(0);
+
+	rtl92ee_dm_dynamic_arfb_select(hw, rate, collision_state);
+}
+
+static void _rtl92ee_c2h_content_parsing(struct ieee80211_hw *hw, u8 c2h_cmd_id,
+					 u8 c2h_cmd_len, u8 *tmp_buf)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	switch (c2h_cmd_id) {
+	case C2H_8192E_DBG:
+		RT_TRACE(COMP_FW, DBG_TRACE , ("[C2H], C2H_8723BE_DBG!!\n"));
+		break;
+	case C2H_8192E_TXBF:
+		RT_TRACE(COMP_FW, DBG_TRACE , ("[C2H], C2H_8192E_TXBF!!\n"));
+		break;
+	case C2H_8192E_TX_REPORT:
+		RT_TRACE(COMP_FW, DBG_TRACE , ("[C2H], C2H_8723BE_TX_REPORT!\n"));
+		break;
+	case C2H_8192E_BT_INFO:
+		RT_TRACE(COMP_FW, DBG_TRACE , ("[C2H], C2H_8723BE_BT_INFO!!\n"));
+		rtlpriv->btcoexist.btc_ops->btc_btinfo_notify(rtlpriv, tmp_buf,
+							      c2h_cmd_len);
+		break;
+	case C2H_8192E_BT_MP:
+		RT_TRACE(COMP_FW, DBG_TRACE, ("[C2H], C2H_8723BE_BT_MP!!\n"));
+		break;
+	case C2H_8192E_RA_RPT:
+		_rtl92ee_c2h_ra_report_handler(hw, tmp_buf, c2h_cmd_len);
+		break;
+	default:
+		RT_TRACE(COMP_FW, DBG_TRACE,
+			 ("[C2H], Unkown packet!! CmdId(%#X)!\n", c2h_cmd_id));
+		break;
+	}
+}
+
+void rtl92ee_c2h_packet_handler(struct ieee80211_hw *hw, u8 *buffer, u8 len)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 c2h_cmd_id = 0, c2h_cmd_seq = 0, c2h_cmd_len = 0;
+	u8 *tmp_buf = NULL;
+
+	c2h_cmd_id = buffer[0];
+	c2h_cmd_seq = buffer[1];
+	c2h_cmd_len = len - 2;
+	tmp_buf = buffer + 2;
+
+	RT_TRACE(COMP_FW, DBG_TRACE,
+		("[C2H packet], c2hCmdId = 0x%x, c2hCmdSeq = 0x%x, c2hCmdLen =%d\n",
+		c2h_cmd_id, c2h_cmd_seq, c2h_cmd_len));
+
+	RT_PRINT_DATA(rtlpriv, COMP_FW, DBG_TRACE,
+		      "[C2H packet], Content Hex:\n", tmp_buf, c2h_cmd_len);
+
+	_rtl92ee_c2h_content_parsing(hw, c2h_cmd_id, c2h_cmd_len, tmp_buf);
+}
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/fw.h b/drivers/staging/rtl8192ee/rtl8192ee/fw.h
new file mode 100644
index 00000000000..143992d5f62
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/fw.h
@@ -0,0 +1,213 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92E__FW__H__
+#define __RTL92E__FW__H__
+
+#define FW_8192C_SIZE				0x8000
+#define FW_8192C_START_ADDRESS			0x1000
+#define FW_8192C_END_ADDRESS			0x5FFF
+#define FW_8192C_PAGE_SIZE			4096
+#define FW_8192C_POLLING_DELAY			5
+#define FW_8192C_POLLING_TIMEOUT_COUNT		3000
+
+#define IS_FW_HEADER_EXIST(_pfwhdr)	\
+	((_pfwhdr->signature&0xFFF0) == 0x92E0)
+#define USE_OLD_WOWLAN_DEBUG_FW 0
+
+#define H2C_92E_RSVDPAGE_LOC_LEN		5
+#define H2C_92E_PWEMODE_LENGTH			5
+#define H2C_92E_JOINBSSRPT_LENGTH		1
+#define H2C_92E_AP_OFFLOAD_LENGTH		3
+#define H2C_92E_WOWLAN_LENGTH			3
+#define H2C_92E_KEEP_ALIVE_CTRL_LENGTH		3
+#if (USE_OLD_WOWLAN_DEBUG_FW == 0)
+#define H2C_92E_REMOTE_WAKE_CTRL_LEN		1
+#else
+#define H2C_92E_REMOTE_WAKE_CTRL_LEN		3
+#endif
+#define H2C_92E_AOAC_GLOBAL_INFO_LEN		2
+#define H2C_92E_AOAC_RSVDPAGE_LOC_LEN		7
+
+
+/* Fw PS state for RPWM.
+*BIT[2:0] = HW state
+*BIT[3] = Protocol PS state,  1: register active state, 0: register sleep state
+*BIT[4] = sub-state
+*/
+#define	FW_PS_RF_ON		BIT(2)
+#define	FW_PS_REGISTER_ACTIVE	BIT(3)
+
+#define	FW_PS_ACK		BIT(6)
+#define	FW_PS_TOGGLE		BIT(7)
+
+ /* 92E RPWM value*/
+ /* BIT[0] = 1: 32k, 0: 40M*/
+#define	FW_PS_CLOCK_OFF		BIT(0)		/* 32k */
+#define	FW_PS_CLOCK_ON		0		/* 40M */
+
+#define	FW_PS_STATE_MASK		(0x0F)
+#define	FW_PS_STATE_HW_MASK		(0x07)
+#define	FW_PS_STATE_INT_MASK		(0x3F)
+
+#define	FW_PS_STATE(x)			(FW_PS_STATE_MASK & (x))
+
+#define	FW_PS_STATE_ALL_ON_92E		(FW_PS_CLOCK_ON)
+#define	FW_PS_STATE_RF_ON_92E		(FW_PS_CLOCK_ON)
+#define	FW_PS_STATE_RF_OFF_92E		(FW_PS_CLOCK_ON)
+#define	FW_PS_STATE_RF_OFF_LOW_PWR	(FW_PS_CLOCK_OFF)
+
+/* For 92E H2C PwrMode Cmd ID 5.*/
+#define	FW_PWR_STATE_ACTIVE	((FW_PS_RF_ON) | (FW_PS_REGISTER_ACTIVE))
+#define	FW_PWR_STATE_RF_OFF	0
+
+#define	FW_PS_IS_ACK(x)		((x) & FW_PS_ACK)
+
+#define	IS_IN_LOW_POWER_STATE_92E(FwPSState)		\
+	(FW_PS_STATE(FwPSState) == FW_PS_CLOCK_OFF)
+
+#define	FW_PWR_STATE_ACTIVE	((FW_PS_RF_ON) | (FW_PS_REGISTER_ACTIVE))
+#define	FW_PWR_STATE_RF_OFF	0
+
+struct rtl92c_firmware_header {
+	u16 signature;
+	u8 category;
+	u8 function;
+	u16 version;
+	u8 subversion;
+	u8 rsvd1;
+	u8 month;
+	u8 date;
+	u8 hour;
+	u8 minute;
+	u16 ramcodeSize;
+	u16 rsvd2;
+	u32 svnindex;
+	u32 rsvd3;
+	u32 rsvd4;
+	u32 rsvd5;
+};
+
+enum rtl8192c_h2c_cmd {
+	H2C_92E_RSVDPAGE = 0,
+	H2C_92E_MSRRPT = 1,
+	H2C_92E_SCAN = 2,
+	H2C_92E_KEEP_ALIVE_CTRL = 3,
+	H2C_92E_DISCONNECT_DECISION = 4,
+#if (USE_OLD_WOWLAN_DEBUG_FW == 1)
+	H2C_92E_WO_WLAN = 5,
+#endif
+	H2C_92E_INIT_OFFLOAD = 6,
+#if (USE_OLD_WOWLAN_DEBUG_FW == 1)
+	H2C_92E_REMOTE_WAKE_CTRL = 7,
+#endif
+	H2C_92E_AP_OFFLOAD = 8,
+	H2C_92E_BCN_RSVDPAGE = 9,
+	H2C_92E_PROBERSP_RSVDPAGE = 10,
+
+	H2C_92E_SETPWRMODE = 0x20,
+	H2C_92E_PS_TUNING_PARA = 0x21,
+	H2C_92E_PS_TUNING_PARA2 = 0x22,
+	H2C_92E_PS_LPS_PARA = 0x23,
+	H2C_92E_P2P_PS_OFFLOAD = 024,
+
+#if (USE_OLD_WOWLAN_DEBUG_FW == 0)
+	H2C_92E_WO_WLAN = 0x80,
+	H2C_92E_REMOTE_WAKE_CTRL = 0x81,
+	H2C_92E_AOAC_GLOBAL_INFO = 0x82,
+	H2C_92E_AOAC_RSVDPAGE = 0x83,
+#endif
+	H2C_92E_RA_MASK = 0x40,
+	H2C_92E_RSSI_REPORT = 0x42,
+	H2C_92E_SELECTIVE_SUSPEND_ROF_CMD,
+	H2C_92E_P2P_PS_MODE,
+	H2C_92E_PSD_RESULT,
+	/*Not defined CTW CMD for P2P yet*/
+	H2C_92E_P2P_PS_CTW_CMD,
+	MAX_92E_H2CCMD
+};
+
+enum rtl8192e_c2h_evt {
+	C2H_8192E_DBG = 0,
+	C2H_8192E_LB = 1,
+	C2H_8192E_TXBF = 2,
+	C2H_8192E_TX_REPORT = 3,
+	C2H_8192E_BT_INFO = 9,
+	C2H_8192E_BT_MP = 11,
+	C2H_8192E_RA_RPT = 12,
+	MAX_8192E_C2HEVENT
+};
+
+#define pagenum_128(_len)	\
+	(u32)(((_len) >> 7) + ((_len) & 0x7F ? 1 : 0))
+
+#define SET_H2CCMD_PWRMODE_PARM_MODE(__ph2ccmd, __val)			\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 8, __val)
+#define SET_H2CCMD_PWRMODE_PARM_RLBM(__pH2CCmd, __val)			\
+	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+1, 0, 4, __val)
+#define SET_H2CCMD_PWRMODE_PARM_SMART_PS(__pH2CCmd, __val)		\
+	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+1, 4, 4, __val)
+#define SET_H2CCMD_PWRMODE_PARM_AWAKE_INTERVAL(__pH2CCmd, __val)	\
+	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+2, 0, 8, __val)
+#define SET_H2CCMD_PWRMODE_PARM_ALL_QUEUE_UAPSD(__pH2CCmd, __val)	\
+	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+3, 0, 8, __val)
+#define SET_H2CCMD_PWRMODE_PARM_PWR_STATE(__pH2CCmd, __val)		\
+	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+4, 0, 8, __val)
+#define GET_92E_H2CCMD_PWRMODE_PARM_MODE(__pH2CCmd)			\
+	LE_BITS_TO_1BYTE(__pH2CCmd, 0, 8)
+
+#define SET_H2CCMD_JOINBSSRPT_PARM_OPMODE(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 8, __val)
+#define SET_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 8, __val)
+#define SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+1, 0, 8, __val)
+#define SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+2, 0, 8, __val)
+
+/* _MEDIA_STATUS_RPT_PARM_CMD1 */
+#define SET_H2CCMD_MSRRPT_PARM_OPMODE(__pH2CCmd, __Value)		\
+	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 0, 1, __Value)
+#define SET_H2CCMD_MSRRPT_PARM_MACID_IND(__pH2CCmd, __Value)		\
+	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 1, 1, __Value)
+#define SET_H2CCMD_MSRRPT_PARM_MACID(__pH2CCmd, __Value)		\
+	SET_BITS_TO_LE_1BYTE(__pH2CCmd+1, 0, 8, __Value)
+#define SET_H2CCMD_MSRRPT_PARM_MACID_END(__pH2CCmd, __Value)		\
+	SET_BITS_TO_LE_1BYTE(__pH2CCmd+2, 0, 8, __Value)
+
+
+int rtl92ee_download_fw(struct ieee80211_hw *hw, bool buse_wake_on_wlan_fw);
+void rtl92ee_fill_h2c_cmd(struct ieee80211_hw *hw, u8 element_id,
+			  u32 cmd_len, u8 *p_cmdbuffer);
+void rtl92ee_firmware_selfreset(struct ieee80211_hw *hw);
+void rtl92ee_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode);
+void rtl92ee_set_fw_media_status_rpt_cmd(struct ieee80211_hw *hw, u8 mstatus);
+void rtl92ee_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished);
+void rtl92ee_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state);
+void rtl92ee_c2h_packet_handler(struct ieee80211_hw *hw, u8 *buffer, u8 len);
+#endif
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/hw.c b/drivers/staging/rtl8192ee/rtl8192ee/hw.c
new file mode 100644
index 00000000000..26af119e2ca
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/hw.c
@@ -0,0 +1,2544 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../efuse.h"
+#include "../base.h"
+#include "../regd.h"
+#include "../cam.h"
+#include "../ps.h"
+#include "../pci.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "dm.h"
+#include "fw.h"
+#include "led.h"
+#include "hw.h"
+#include "pwrseqcmd.h"
+#include "pwrseq.h"
+
+#define LLT_CONFIG	5
+
+static void _rtl92ee_set_bcn_ctrl_reg(struct ieee80211_hw *hw,
+				      u8 set_bits, u8 clear_bits)
+{
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpci->reg_bcn_ctrl_val |= set_bits;
+	rtlpci->reg_bcn_ctrl_val &= ~clear_bits;
+
+	rtl_write_byte(rtlpriv, REG_BCN_CTRL, (u8) rtlpci->reg_bcn_ctrl_val);
+}
+
+static void _rtl92ee_stop_tx_beacon(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmp;
+
+	tmp = rtl_read_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2);
+	rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2, tmp & (~BIT(6)));
+	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 1, 0x64);
+	tmp = rtl_read_byte(rtlpriv, REG_TBTT_PROHIBIT + 2);
+	tmp &= ~(BIT(0));
+	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 2, tmp);
+}
+
+static void _rtl92ee_resume_tx_beacon(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmp;
+
+	tmp = rtl_read_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2);
+	rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2, tmp | BIT(6));
+	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 1, 0xff);
+	tmp = rtl_read_byte(rtlpriv, REG_TBTT_PROHIBIT + 2);
+	tmp |= BIT(0);
+	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 2, tmp);
+}
+
+static void _rtl92ee_enable_bcn_sub_func(struct ieee80211_hw *hw)
+{
+	_rtl92ee_set_bcn_ctrl_reg(hw, 0, BIT(1));
+}
+
+static void _rtl92ee_return_beacon_queue_skb(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[BEACON_QUEUE];
+
+	while (skb_queue_len(&ring->queue)) {
+		struct rtl_tx_buffer_desc *entry =
+						&ring->buffer_desc[ring->idx];
+		struct sk_buff *skb = __skb_dequeue(&ring->queue);
+
+		pci_unmap_single(rtlpci->pdev,
+				 rtlpriv->cfg->ops->get_desc(
+				 (u8 *) entry, true, HW_DESC_TXBUFF_ADDR),
+				 skb->len, PCI_DMA_TODEVICE);
+		kfree_skb(skb);
+		ring->idx = (ring->idx + 1) % ring->entries;
+	}
+
+}
+static void _rtl92ee_disable_bcn_sub_func(struct ieee80211_hw *hw)
+{
+	_rtl92ee_set_bcn_ctrl_reg(hw, BIT(1), 0);
+}
+
+static void _rtl92ee_set_fw_clock_on(struct ieee80211_hw *hw,
+	u8 rpwm_val, bool b_need_turn_off_ckk)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool b_support_remote_wake_up;
+	u32 count = 0 , isr_regaddr , content;
+	bool b_schedule_timer = b_need_turn_off_ckk;
+	rtlpriv->cfg->ops->get_hw_reg(hw, HAL_DEF_WOWLAN,
+				      (u8 *) (&b_support_remote_wake_up));
+
+	if (!rtlhal->bfw_ready)
+		return;
+	if (!rtlpriv->psc.b_fw_current_inpsmode)
+		return;
+
+	while (1) {
+		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+		if (rtlhal->bfw_clk_change_in_progress) {
+			while (rtlhal->bfw_clk_change_in_progress) {
+				spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+				count++;
+				udelay(100);
+				if (count > 1000)
+					return;
+				spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+			}
+			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+		} else {
+			rtlhal->bfw_clk_change_in_progress = false;
+			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+			break;
+		}
+	}
+
+	if (IS_IN_LOW_POWER_STATE_92E(rtlhal->fw_ps_state)) {
+		rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_SET_RPWM,
+					      (u8 *) (&rpwm_val));
+		if (FW_PS_IS_ACK(rpwm_val)) {
+			isr_regaddr = REG_HISR;
+			content = rtl_read_dword(rtlpriv, isr_regaddr);
+			while (!(content & IMR_CPWM) && (count < 500)) {
+				udelay(50);
+				count++;
+				content = rtl_read_dword(rtlpriv, isr_regaddr);
+			}
+
+			if (content & IMR_CPWM) {
+				rtl_write_word(rtlpriv , isr_regaddr, 0x0100);
+				rtlhal->fw_ps_state = FW_PS_STATE_RF_ON_92E;
+				RT_TRACE(COMP_POWER, DBG_LOUD,
+					 ("Receive CPWM INT!!! PSState = %X\n",
+					 rtlhal->fw_ps_state));
+			}
+		}
+
+		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+		rtlhal->bfw_clk_change_in_progress = false;
+		spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+		if (b_schedule_timer) {
+			mod_timer(&rtlpriv->works.fw_clockoff_timer,
+				  jiffies + MSECS(10));
+		}
+
+	} else  {
+		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+		rtlhal->bfw_clk_change_in_progress = false;
+		spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+	}
+
+
+}
+
+static void _rtl92ee_set_fw_clock_off(struct ieee80211_hw *hw, u8 rpwm_val)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl8192_tx_ring *ring;
+	enum rf_pwrstate rtstate;
+	bool b_schedule_timer = false;
+	u8 queue;
+
+	if (!rtlhal->bfw_ready)
+		return;
+	if (!rtlpriv->psc.b_fw_current_inpsmode)
+		return;
+	if (!rtlhal->ballow_sw_to_change_hwclc)
+		return;
+
+	rtlpriv->cfg->ops->get_hw_reg(hw , HW_VAR_RF_STATE , (u8 *)(&rtstate));
+	if (rtstate == ERFOFF || rtlpriv->psc.inactive_pwrstate == ERFOFF)
+		return;
+
+	for (queue = 0; queue < RTL_PCI_MAX_TX_QUEUE_COUNT; queue++) {
+		ring = &rtlpci->tx_ring[queue];
+		if (skb_queue_len(&ring->queue)) {
+			b_schedule_timer = true;
+			break;
+		}
+	}
+
+	if (b_schedule_timer) {
+		mod_timer(&rtlpriv->works.fw_clockoff_timer,
+			  jiffies + MSECS(10));
+		return;
+	}
+
+	if (FW_PS_STATE(rtlhal->fw_ps_state) != FW_PS_STATE_RF_OFF_LOW_PWR) {
+		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+		if (!rtlhal->bfw_clk_change_in_progress) {
+			rtlhal->bfw_clk_change_in_progress = true;
+			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+			rtlhal->fw_ps_state = FW_PS_STATE(rpwm_val);
+			rtl_write_word(rtlpriv, REG_HISR, 0x0100);
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM,
+						      (u8 *) (&rpwm_val));
+			spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+			rtlhal->bfw_clk_change_in_progress = false;
+			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+		} else {
+			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+			mod_timer(&rtlpriv->works.fw_clockoff_timer,
+				  jiffies + MSECS(10));
+		}
+	}
+}
+
+static void _rtl92ee_set_fw_ps_rf_on(struct ieee80211_hw *hw)
+{
+	u8 rpwm_val = 0;
+	rpwm_val |= (FW_PS_STATE_RF_OFF_92E | FW_PS_ACK);
+	_rtl92ee_set_fw_clock_on(hw, rpwm_val, true);
+}
+
+static void _rtl92ee_set_fw_ps_rf_off_low_power(struct ieee80211_hw *hw)
+{
+	u8 rpwm_val = 0;
+	rpwm_val |= FW_PS_STATE_RF_OFF_LOW_PWR;
+	_rtl92ee_set_fw_clock_off(hw, rpwm_val);
+}
+void rtl92ee_fw_clk_off_timer_callback(unsigned long data)
+{
+	struct ieee80211_hw *hw = (struct ieee80211_hw *)data;
+
+	_rtl92ee_set_fw_ps_rf_off_low_power(hw);
+}
+
+static void _rtl92ee_fwlps_leave(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool b_fw_current_inps = false;
+	u8 rpwm_val = 0, fw_pwrmode = FW_PS_ACTIVE_MODE;
+
+	if (ppsc->b_low_power_enable) {
+		rpwm_val = (FW_PS_STATE_ALL_ON_92E | FW_PS_ACK);/* RF on */
+		_rtl92ee_set_fw_clock_on(hw, rpwm_val, false);
+		rtlhal->ballow_sw_to_change_hwclc = false;
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_PWRMODE,
+					      (u8 *) (&fw_pwrmode));
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
+					      (u8 *) (&b_fw_current_inps));
+	} else {
+		rpwm_val = FW_PS_STATE_ALL_ON_92E;	/* RF on */
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM,
+					      (u8 *) (&rpwm_val));
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_PWRMODE,
+					      (u8 *) (&fw_pwrmode));
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
+					      (u8 *) (&b_fw_current_inps));
+	}
+}
+
+static void _rtl92ee_fwlps_enter(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool b_fw_current_inps = true;
+	u8 rpwm_val;
+
+	if (ppsc->b_low_power_enable) {
+		rpwm_val = FW_PS_STATE_RF_OFF_LOW_PWR;	/* RF off */
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
+					      (u8 *) (&b_fw_current_inps));
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_PWRMODE,
+					      (u8 *) (&ppsc->fwctrl_psmode));
+		rtlhal->ballow_sw_to_change_hwclc = true;
+		_rtl92ee_set_fw_clock_off(hw, rpwm_val);
+	} else {
+		rpwm_val = FW_PS_STATE_RF_OFF_92E;	/* RF off */
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
+					      (u8 *) (&b_fw_current_inps));
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_PWRMODE,
+					      (u8 *) (&ppsc->fwctrl_psmode));
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM,
+					      (u8 *) (&rpwm_val));
+	}
+}
+
+void rtl92ee_get_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	switch (variable) {
+	case HW_VAR_RCR:
+		*((u32 *) (val)) = rtlpci->receive_config;
+		break;
+	case HW_VAR_RF_STATE:
+		*((enum rf_pwrstate *)(val)) = ppsc->rfpwr_state;
+		break;
+	case HW_VAR_FWLPS_RF_ON:{
+		enum rf_pwrstate rfState;
+		u32 val_rcr;
+
+		rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_RF_STATE,
+					      (u8 *) (&rfState));
+		if (rfState == ERFOFF) {
+			*((bool *) (val)) = true;
+		} else {
+			val_rcr = rtl_read_dword(rtlpriv, REG_RCR);
+			val_rcr &= 0x00070000;
+			if (val_rcr)
+				*((bool *) (val)) = false;
+			else
+				*((bool *) (val)) = true;
+		}
+		break; }
+	case HW_VAR_FW_PSMODE_STATUS:
+		*((bool *) (val)) = ppsc->b_fw_current_inpsmode;
+		break;
+	case HW_VAR_CORRECT_TSF:{
+		u64 tsf;
+		u32 *ptsf_low = (u32 *) &tsf;
+		u32 *ptsf_high = ((u32 *) &tsf) + 1;
+
+		*ptsf_high = rtl_read_dword(rtlpriv, (REG_TSFTR + 4));
+		*ptsf_low = rtl_read_dword(rtlpriv, REG_TSFTR);
+
+		*((u64 *) (val)) = tsf;
+
+		break; }
+	default:
+		RT_TRACE(COMP_ERR, DBG_LOUD,
+			 ("switch case not process %x\n", variable));
+		break;
+	}
+}
+
+static void _rtl92ee_download_rsvd_page(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmp_regcr, tmp_reg422;
+	u8 bcnvalid_reg, txbc_reg;
+	u8 count = 0, dlbcn_count = 0;
+	bool b_recover = false;
+
+	/*Set REG_CR bit 8. DMA beacon by SW.*/
+	tmp_regcr = rtl_read_byte(rtlpriv, REG_CR + 1);
+	rtl_write_byte(rtlpriv, REG_CR + 1, tmp_regcr | BIT(0));
+
+	/* Disable Hw protection for a time which revserd for Hw sending beacon.
+	 * Fix download reserved page packet fail
+	 * that access collision with the protection time.
+	 * 2010.05.11. Added by tynli. */
+	_rtl92ee_set_bcn_ctrl_reg(hw, 0, BIT(3));
+	_rtl92ee_set_bcn_ctrl_reg(hw, BIT(4), 0);
+
+	/* Set FWHW_TXQ_CTRL 0x422[6]= 0 to
+	 * tell Hw the packet is not a real beacon frame.
+	 */
+	tmp_reg422 = rtl_read_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2);
+	rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2, tmp_reg422 & (~BIT(6)));
+
+	if (tmp_reg422 & BIT(6))
+		b_recover = true;
+
+	do {
+		/* Clear beacon valid check bit */
+		bcnvalid_reg = rtl_read_byte(rtlpriv, REG_DWBCN0_CTRL + 2);
+		rtl_write_byte(rtlpriv, REG_DWBCN0_CTRL + 2,
+			       bcnvalid_reg | BIT(0));
+
+		/* Return Beacon TCB */
+		_rtl92ee_return_beacon_queue_skb(hw);
+
+		/* download rsvd page */
+		rtl92ee_set_fw_rsvdpagepkt(hw, false);
+
+		txbc_reg = rtl_read_byte(rtlpriv, REG_MGQ_TXBD_NUM + 3);
+		count = 0;
+		while ((txbc_reg & BIT(4)) && count < 20) {
+			count++;
+			udelay(10);
+			txbc_reg = rtl_read_byte(rtlpriv, REG_MGQ_TXBD_NUM + 3);
+		}
+		rtl_write_byte(rtlpriv, REG_MGQ_TXBD_NUM + 3,
+			       txbc_reg | BIT(4));
+
+		/* check rsvd page download OK. */
+		bcnvalid_reg = rtl_read_byte(rtlpriv, REG_DWBCN0_CTRL + 2);
+		count = 0;
+		while (!(bcnvalid_reg & BIT(0)) && count < 20) {
+			count++;
+			udelay(50);
+			bcnvalid_reg = rtl_read_byte(rtlpriv,
+						     REG_DWBCN0_CTRL + 2);
+		}
+
+		if (bcnvalid_reg & BIT(0))
+			rtl_write_byte(rtlpriv, REG_DWBCN0_CTRL + 2, BIT(0));
+
+		dlbcn_count++;
+	} while (!(bcnvalid_reg & BIT(0)) && dlbcn_count < 5);
+
+	if (!(bcnvalid_reg & BIT(0)))
+		RT_TRACE(COMP_INIT, DBG_LOUD, ("Download RSVD page failed!\n"));
+
+	/* Enable Bcn */
+	_rtl92ee_set_bcn_ctrl_reg(hw, BIT(3), 0);
+	_rtl92ee_set_bcn_ctrl_reg(hw, 0, BIT(4));
+
+	if (b_recover)
+		rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2, tmp_reg422);
+
+	tmp_regcr = rtl_read_byte(rtlpriv, REG_CR + 1);
+	rtl_write_byte(rtlpriv, REG_CR + 1, tmp_regcr & (~BIT(0)));
+
+}
+
+void rtl92ee_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_efuse *efuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	u8 idx;
+
+	switch (variable) {
+	case HW_VAR_ETHER_ADDR:{
+		for (idx = 0; idx < ETH_ALEN; idx++) {
+			rtl_write_byte(rtlpriv, (REG_MACID + idx), val[idx]);
+		}
+		break; }
+	case HW_VAR_BASIC_RATE:{
+		u16 b_rate_cfg = ((u16 *) val)[0];
+		b_rate_cfg = b_rate_cfg & 0x15f;
+		b_rate_cfg |= 0x01;
+		b_rate_cfg = (b_rate_cfg | 0xd) & (~BIT(1));
+		rtl_write_byte(rtlpriv, REG_RRSR, b_rate_cfg & 0xff);
+		rtl_write_byte(rtlpriv, REG_RRSR + 1, (b_rate_cfg >> 8) & 0xff);
+		break; }
+	case HW_VAR_BSSID:
+		for (idx = 0; idx < ETH_ALEN; idx++) {
+			rtl_write_byte(rtlpriv, (REG_BSSID + idx), val[idx]);
+		}
+		break;
+	case HW_VAR_SIFS:
+		rtl_write_byte(rtlpriv, REG_SIFS_CTX + 1, val[0]);
+		rtl_write_byte(rtlpriv, REG_SIFS_TRX + 1, val[1]);
+
+		rtl_write_byte(rtlpriv, REG_SPEC_SIFS + 1, val[0]);
+		rtl_write_byte(rtlpriv, REG_MAC_SPEC_SIFS + 1, val[0]);
+
+		if (!mac->ht_enable)
+			rtl_write_word(rtlpriv, REG_RESP_SIFS_OFDM, 0x0e0e);
+		else
+			rtl_write_word(rtlpriv, REG_RESP_SIFS_OFDM,
+				       *((u16 *) val));
+		break;
+	case HW_VAR_SLOT_TIME:{
+		u8 e_aci;
+
+		RT_TRACE(COMP_MLME, DBG_TRACE,
+			 ("HW_VAR_SLOT_TIME %x\n", val[0]));
+
+		rtl_write_byte(rtlpriv, REG_SLOT, val[0]);
+
+		for (e_aci = 0; e_aci < AC_MAX; e_aci++) {
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AC_PARAM,
+						      (u8 *) (&e_aci));
+		}
+		break; }
+	case HW_VAR_ACK_PREAMBLE:{
+		u8 reg_tmp;
+		u8 short_preamble = (bool) (*(u8 *) val);
+		reg_tmp = (rtlpriv->mac80211.cur_40_prime_sc) << 5;
+		if (short_preamble)
+			reg_tmp |= 0x80;
+		rtl_write_byte(rtlpriv, REG_RRSR + 2, reg_tmp);
+		rtlpriv->mac80211.short_preamble = short_preamble;
+		break; }
+	case HW_VAR_WPA_CONFIG:
+		rtl_write_byte(rtlpriv, REG_SECCFG, *((u8 *) val));
+		break;
+	case HW_VAR_AMPDU_FACTOR:{
+		u8 regtoset_normal[4] = { 0x41, 0xa8, 0x72, 0xb9 };
+		u8 fac;
+		u8 *reg = NULL;
+		u8 i = 0;
+
+		reg = regtoset_normal;
+
+		fac = *((u8 *) val);
+		if (fac <= 3) {
+			fac = (1 << (fac + 2));
+			if (fac > 0xf)
+				fac = 0xf;
+				for (i = 0; i < 4; i++) {
+				if ((reg[i] & 0xf0) > (fac << 4))
+					reg[i] = (reg[i] & 0x0f) | (fac << 4);
+				if ((reg[i] & 0x0f) > fac)
+					reg[i] = (reg[i] & 0xf0) | fac;
+					rtl_write_byte(rtlpriv,
+						       (REG_AGGLEN_LMT + i),
+						       reg[i]);
+				}
+				RT_TRACE(COMP_MLME, DBG_LOUD,
+					("Set HW_VAR_AMPDU_FACTOR:%#x\n", fac));
+		}
+		break; }
+	case HW_VAR_AC_PARAM:{
+		u8 e_aci = *((u8 *) val);
+
+		if (rtlpci->acm_method != eAcmWay2_SW)
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ACM_CTRL,
+						      (u8 *) (&e_aci));
+		break; }
+	case HW_VAR_ACM_CTRL:{
+		u8 e_aci = *((u8 *) val);
+		union aci_aifsn *aifs = (union aci_aifsn *)(&(mac->ac[0].aifs));
+
+		u8 acm = aifs->f.acm;
+		u8 acm_ctrl = rtl_read_byte(rtlpriv, REG_ACMHWCTRL);
+
+		acm_ctrl = acm_ctrl | ((rtlpci->acm_method == 2) ? 0x0 : 0x1);
+
+		if (acm) {
+			switch (e_aci) {
+			case AC0_BE:
+				acm_ctrl |= AcmHw_BeqEn;
+				break;
+			case AC2_VI:
+				acm_ctrl |= AcmHw_ViqEn;
+				break;
+			case AC3_VO:
+				acm_ctrl |= AcmHw_VoqEn;
+				break;
+			default:
+				RT_TRACE(COMP_ERR, DBG_WARNING,
+					 ("HW_VAR_ACM_CTRL acm set "
+					  "failed: eACI is %d\n", acm));
+				break;
+			}
+		} else {
+			switch (e_aci) {
+			case AC0_BE:
+				acm_ctrl &= (~AcmHw_BeqEn);
+				break;
+			case AC2_VI:
+				acm_ctrl &= (~AcmHw_ViqEn);
+				break;
+			case AC3_VO:
+				acm_ctrl &= (~AcmHw_BeqEn);
+				break;
+			default:
+				RT_TRACE(COMP_ERR, DBG_LOUD,
+					 ("switch case not process \n"));
+				break;
+			}
+		}
+
+		RT_TRACE(COMP_QOS, DBG_TRACE,
+			 ("SetHwReg8190pci(): [HW_VAR_ACM_CTRL] Write 0x%X\n",
+			  acm_ctrl));
+		rtl_write_byte(rtlpriv, REG_ACMHWCTRL, acm_ctrl);
+		break; }
+	case HW_VAR_RCR:{
+		rtl_write_dword(rtlpriv, REG_RCR, ((u32 *) (val))[0]);
+		rtlpci->receive_config = ((u32 *) (val))[0];
+		break; }
+	case HW_VAR_RETRY_LIMIT:{
+		u8 retry_limit = ((u8 *) (val))[0];
+
+		rtl_write_word(rtlpriv, REG_RETRY_LIMIT,
+			       retry_limit << RETRY_LIMIT_SHORT_SHIFT |
+			       retry_limit << RETRY_LIMIT_LONG_SHIFT);
+		break; }
+	case HW_VAR_DUAL_TSF_RST:
+		rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST, (BIT(0) | BIT(1)));
+		break;
+	case HW_VAR_EFUSE_BYTES:
+		efuse->efuse_usedbytes = *((u16 *) val);
+		break;
+	case HW_VAR_EFUSE_USAGE:
+		efuse->efuse_usedpercentage = *((u8 *) val);
+		break;
+	case HW_VAR_IO_CMD:
+		rtl92ee_phy_set_io_cmd(hw, (*(enum io_type *)val));
+		break;
+	case HW_VAR_SET_RPWM:{
+		u8 rpwm_val;
+
+		rpwm_val = rtl_read_byte(rtlpriv, REG_PCIE_HRPWM);
+		udelay(1);
+
+		if (rpwm_val & BIT(7)) {
+			rtl_write_byte(rtlpriv, REG_PCIE_HRPWM, (*(u8 *) val));
+		} else {
+			rtl_write_byte(rtlpriv, REG_PCIE_HRPWM,
+				       ((*(u8 *) val) | BIT(7)));
+		}
+		break; }
+	case HW_VAR_H2C_FW_PWRMODE:
+		rtl92ee_set_fw_pwrmode_cmd(hw, (*(u8 *) val));
+		break;
+	case HW_VAR_FW_PSMODE_STATUS:
+		ppsc->b_fw_current_inpsmode = *((bool *) val);
+		break;
+	case HW_VAR_RESUME_CLK_ON:
+		_rtl92ee_set_fw_ps_rf_on(hw);
+		break;
+	case HW_VAR_FW_LPS_ACTION:{
+		bool b_enter_fwlps = *((bool *) val);
+
+		if (b_enter_fwlps)
+			_rtl92ee_fwlps_enter(hw);
+		else
+			_rtl92ee_fwlps_leave(hw);
+		break; }
+	case HW_VAR_H2C_FW_JOINBSSRPT:{
+		u8 mstatus = (*(u8 *) val);
+
+		if (mstatus == RT_MEDIA_CONNECT) {
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AID, NULL);
+			_rtl92ee_download_rsvd_page(hw);
+		}
+		rtl92ee_set_fw_media_status_rpt_cmd(hw, mstatus);
+		break; }
+	case HW_VAR_H2C_FW_P2P_PS_OFFLOAD:
+		rtl92ee_set_p2p_ps_offload_cmd(hw , (*(u8 *) val));
+		break;
+	case HW_VAR_AID:{
+		u16 u2btmp;
+		u2btmp = rtl_read_word(rtlpriv, REG_BCN_PSR_RPT);
+		u2btmp &= 0xC000;
+		rtl_write_word(rtlpriv, REG_BCN_PSR_RPT,
+			       (u2btmp | mac->assoc_id));
+		break; }
+	case HW_VAR_CORRECT_TSF:{
+		u8 btype_ibss = ((u8 *) (val))[0];
+
+		if (btype_ibss == true)
+			_rtl92ee_stop_tx_beacon(hw);
+
+		_rtl92ee_set_bcn_ctrl_reg(hw, 0, BIT(3));
+
+		rtl_write_dword(rtlpriv, REG_TSFTR,
+				(u32) (mac->tsf & 0xffffffff));
+		rtl_write_dword(rtlpriv, REG_TSFTR + 4,
+				(u32) ((mac->tsf >> 32) & 0xffffffff));
+
+		_rtl92ee_set_bcn_ctrl_reg(hw, BIT(3), 0);
+
+		if (btype_ibss == true)
+			_rtl92ee_resume_tx_beacon(hw);
+		break; }
+	case HW_VAR_KEEP_ALIVE: {
+		u8 array[2];
+		array[0] = 0xff;
+		array[1] = *((u8 *)val);
+		rtl92ee_fill_h2c_cmd(hw, H2C_92E_KEEP_ALIVE_CTRL, 2, array);
+		break; }
+	default:
+		RT_TRACE(COMP_ERR, DBG_LOUD,
+			 ("switch case not process %x\n", variable));
+		break;
+	}
+}
+
+static bool _rtl92ee_llt_table_init(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 txpktbuf_bndy;
+	u8 u8tmp, testcnt = 0;
+
+	txpktbuf_bndy = 0xFA;
+
+	rtl_write_dword(rtlpriv, REG_RQPN, 0x80E90808);
+
+	rtl_write_byte(rtlpriv, REG_TRXFF_BNDY, txpktbuf_bndy);
+	rtl_write_word(rtlpriv, REG_TRXFF_BNDY + 2, 0x3d00 - 1);
+
+	rtl_write_byte(rtlpriv, REG_DWBCN0_CTRL + 1, txpktbuf_bndy);
+	rtl_write_byte(rtlpriv, REG_DWBCN1_CTRL + 1, txpktbuf_bndy);
+
+	rtl_write_byte(rtlpriv, REG_BCNQ_BDNY, txpktbuf_bndy);
+	rtl_write_byte(rtlpriv, REG_BCNQ1_BDNY, txpktbuf_bndy);
+
+	rtl_write_byte(rtlpriv, REG_MGQ_BDNY, txpktbuf_bndy);
+	rtl_write_byte(rtlpriv, 0x45D, txpktbuf_bndy);
+
+	rtl_write_byte(rtlpriv, REG_PBP, 0x31);
+	rtl_write_byte(rtlpriv, REG_RX_DRVINFO_SZ, 0x4);
+
+	u8tmp = rtl_read_byte(rtlpriv, REG_AUTO_LLT + 2);
+	rtl_write_byte(rtlpriv, REG_AUTO_LLT + 2, u8tmp | BIT(0));
+
+	while (u8tmp & BIT(0)) {
+		u8tmp = rtl_read_byte(rtlpriv, REG_AUTO_LLT + 2);
+		udelay(10);
+		testcnt++;
+		if (testcnt > 10)
+			break;
+	}
+
+	return true;
+}
+
+static void _rtl92ee_gen_refresh_led_state(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_led *pLed0 = &(pcipriv->ledctl.sw_led0);
+
+	if (rtlpriv->rtlhal.up_first_time)
+		return;
+
+	if (ppsc->rfoff_reason == RF_CHANGE_BY_IPS)
+		rtl92ee_sw_led_on(hw, pLed0);
+	else if (ppsc->rfoff_reason == RF_CHANGE_BY_INIT)
+		rtl92ee_sw_led_on(hw, pLed0);
+	else
+		rtl92ee_sw_led_off(hw, pLed0);
+}
+
+static bool _rtl92ee_init_mac(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	u8 bytetmp;
+	u16 wordtmp;
+	u32 dwordtmp;
+
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL, 0x0);
+
+	dwordtmp = rtl_read_dword(rtlpriv, REG_SYS_CFG1);
+	if (dwordtmp & BIT(24)) {
+		rtl_write_byte(rtlpriv, 0x7c, 0xc3);
+	} else {
+		bytetmp = rtl_read_byte(rtlpriv, 0x16);
+		rtl_write_byte(rtlpriv, 0x16, bytetmp | BIT(4) | BIT(6));
+		rtl_write_byte(rtlpriv, 0x7c, 0x83);
+	}
+	/* 1. 40Mhz crystal source*/
+	bytetmp = rtl_read_byte(rtlpriv, REG_AFE_CTRL2);
+	bytetmp &= 0xfb;
+	rtl_write_byte(rtlpriv, REG_AFE_CTRL2, bytetmp);
+
+	dwordtmp = rtl_read_dword(rtlpriv, REG_AFE_CTRL4);
+	dwordtmp &= 0xfffffc7f;
+	rtl_write_dword(rtlpriv, REG_AFE_CTRL4, dwordtmp);
+
+	/* 2. 92E AFE parameter
+	  *MP chip then check version */
+	bytetmp = rtl_read_byte(rtlpriv, REG_AFE_CTRL2);
+	bytetmp &= 0xbf;
+	rtl_write_byte(rtlpriv, REG_AFE_CTRL2, bytetmp);
+
+	dwordtmp = rtl_read_dword(rtlpriv, REG_AFE_CTRL4);
+	dwordtmp &= 0xffdfffff;
+	rtl_write_dword(rtlpriv, REG_AFE_CTRL4, dwordtmp);
+
+	/* HW Power on sequence */
+	if (!rtl92e_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
+		PWR_INTF_PCI_MSK, Rtl8192E_NIC_ENABLE_FLOW)) {
+		RT_TRACE(COMP_INIT, DBG_LOUD,
+			 ("init MAC Fail as rtl92e_hal_pwrseqcmdparsing\n"));
+		return false;
+	}
+
+	/* Release MAC IO register reset */
+	bytetmp = rtl_read_byte(rtlpriv, REG_CR);
+	bytetmp = 0xff;
+	rtl_write_byte(rtlpriv, REG_CR, bytetmp);
+	mdelay(2);
+	bytetmp = 0x7f;
+	rtl_write_byte(rtlpriv, REG_HWSEQ_CTRL, bytetmp);
+	mdelay(2);
+
+	/* Add for wakeup online */
+	bytetmp = rtl_read_byte(rtlpriv, REG_SYS_CLKR);
+	rtl_write_byte(rtlpriv, REG_SYS_CLKR, bytetmp | BIT(3));
+	bytetmp = rtl_read_byte(rtlpriv, REG_GPIO_MUXCFG + 1);
+	rtl_write_byte(rtlpriv, REG_GPIO_MUXCFG + 1, bytetmp & (~BIT(4)));
+	/* Release MAC IO register reset */
+	rtl_write_word(rtlpriv, REG_CR, 0x2ff);
+
+	if (!rtlhal->b_mac_func_enable) {
+		if (_rtl92ee_llt_table_init(hw) == false) {
+			RT_TRACE(COMP_INIT, DBG_LOUD,
+				 ("LLT table init fail \n"));
+			return false;
+		}
+	}
+
+	rtl_write_dword(rtlpriv, REG_HISR, 0xffffffff);
+	rtl_write_dword(rtlpriv, REG_HISRE, 0xffffffff);
+
+	wordtmp = rtl_read_word(rtlpriv, REG_TRXDMA_CTRL);
+	wordtmp &= 0xf;
+	wordtmp |= 0xF5B1;
+	rtl_write_word(rtlpriv, REG_TRXDMA_CTRL, wordtmp);
+	/* Reported Tx status from HW for rate adaptive.*/
+	rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 1, 0x1F);
+
+	/* Set RCR register */
+	rtl_write_dword(rtlpriv, REG_RCR, rtlpci->receive_config);
+	rtl_write_word(rtlpriv, REG_RXFLTMAP2, 0xffff);
+
+	/* Set TCR register */
+	rtl_write_dword(rtlpriv, REG_TCR, rtlpci->transmit_config);
+
+	/* Set TX/RX descriptor physical address(from OS API). */
+	rtl_write_dword(rtlpriv, REG_BCNQ_DESA,
+			((u64) rtlpci->tx_ring[BEACON_QUEUE].buffer_desc_dma) &
+			DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_MGQ_DESA,
+			(u64) rtlpci->tx_ring[MGNT_QUEUE].buffer_desc_dma &
+			DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_VOQ_DESA,
+			(u64) rtlpci->tx_ring[VO_QUEUE].buffer_desc_dma &
+			DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_VIQ_DESA,
+			(u64) rtlpci->tx_ring[VI_QUEUE].buffer_desc_dma &
+			DMA_BIT_MASK(32));
+
+	rtl_write_dword(rtlpriv, REG_BEQ_DESA,
+			(u64) rtlpci->tx_ring[BE_QUEUE].buffer_desc_dma &
+			DMA_BIT_MASK(32));
+
+	dwordtmp = rtl_read_dword(rtlpriv, REG_BEQ_DESA);
+
+	rtl_write_dword(rtlpriv, REG_BKQ_DESA,
+			(u64) rtlpci->tx_ring[BK_QUEUE].buffer_desc_dma &
+			DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_HQ0_DESA,
+			(u64) rtlpci->tx_ring[HIGH_QUEUE].buffer_desc_dma &
+			DMA_BIT_MASK(32));
+
+	rtl_write_dword(rtlpriv, REG_RX_DESA,
+			(u64) rtlpci->rx_ring[RX_MPDU_QUEUE].dma &
+			DMA_BIT_MASK(32));
+
+	/* if we want to support 64 bit DMA, we should set it here,
+	 * but now we do not support 64 bit DMA*/
+
+
+	rtl_write_dword(rtlpriv, REG_TSFTIMER_HCI, 0x3fffffff);
+
+	bytetmp = rtl_read_byte(rtlpriv, REG_PCIE_CTRL_REG + 3);
+	rtl_write_byte(rtlpriv, REG_PCIE_CTRL_REG + 3, bytetmp | 0xF7);
+
+	rtl_write_dword(rtlpriv, REG_INT_MIG, 0);
+
+	rtl_write_dword(rtlpriv, REG_MCUTST_1, 0x0);
+
+	rtl_write_word(rtlpriv, REG_MGQ_TXBD_NUM,
+		       TX_DESC_NUM_92E | ((RTL8192EE_SEG_NUM << 12) & 0x3000));
+	rtl_write_word(rtlpriv, REG_VOQ_TXBD_NUM,
+		       TX_DESC_NUM_92E | ((RTL8192EE_SEG_NUM << 12) & 0x3000));
+	rtl_write_word(rtlpriv, REG_VIQ_TXBD_NUM,
+		       TX_DESC_NUM_92E | ((RTL8192EE_SEG_NUM << 12) & 0x3000));
+	rtl_write_word(rtlpriv, REG_BEQ_TXBD_NUM,
+		       TX_DESC_NUM_92E | ((RTL8192EE_SEG_NUM << 12) & 0x3000));
+	rtl_write_word(rtlpriv, REG_VOQ_TXBD_NUM,
+		       TX_DESC_NUM_92E | ((RTL8192EE_SEG_NUM << 12) & 0x3000));
+	rtl_write_word(rtlpriv, REG_BKQ_TXBD_NUM,
+		       TX_DESC_NUM_92E | ((RTL8192EE_SEG_NUM << 12) & 0x3000));
+	rtl_write_word(rtlpriv, REG_HI0Q_TXBD_NUM,
+		       TX_DESC_NUM_92E | ((RTL8192EE_SEG_NUM << 12) & 0x3000));
+	rtl_write_word(rtlpriv, REG_HI1Q_TXBD_NUM,
+		       TX_DESC_NUM_92E | ((RTL8192EE_SEG_NUM << 12) & 0x3000));
+	rtl_write_word(rtlpriv, REG_HI2Q_TXBD_NUM,
+		       TX_DESC_NUM_92E | ((RTL8192EE_SEG_NUM << 12) & 0x3000));
+	rtl_write_word(rtlpriv, REG_HI3Q_TXBD_NUM,
+		       TX_DESC_NUM_92E | ((RTL8192EE_SEG_NUM << 12) & 0x3000));
+	rtl_write_word(rtlpriv, REG_HI4Q_TXBD_NUM,
+		       TX_DESC_NUM_92E | ((RTL8192EE_SEG_NUM << 12) & 0x3000));
+	rtl_write_word(rtlpriv, REG_HI5Q_TXBD_NUM,
+		       TX_DESC_NUM_92E | ((RTL8192EE_SEG_NUM << 12) & 0x3000));
+	rtl_write_word(rtlpriv, REG_HI6Q_TXBD_NUM,
+		       TX_DESC_NUM_92E | ((RTL8192EE_SEG_NUM << 12) & 0x3000));
+	rtl_write_word(rtlpriv, REG_HI7Q_TXBD_NUM,
+		       TX_DESC_NUM_92E | ((RTL8192EE_SEG_NUM << 12) & 0x3000));
+	/*Rx*/
+#if (DMA_IS_64BIT == 1)
+	rtl_write_word(rtlpriv, REG_RX_RXBD_NUM,
+		       RX_DESC_NUM_92E |
+		       ((RTL8192EE_SEG_NUM << 13) & 0x6000) | 0x8000);
+#else
+	rtl_write_word(rtlpriv, REG_RX_RXBD_NUM,
+		       RX_DESC_NUM_92E |
+		       ((RTL8192EE_SEG_NUM << 13) & 0x6000) | 0x0000);
+#endif
+
+	rtl_write_dword(rtlpriv, REG_TSFTIMER_HCI, 0XFFFFFFFF);
+
+	_rtl92ee_gen_refresh_led_state(hw);
+	return true;
+}
+
+static void _rtl92ee_hw_configure(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u32 reg_rrsr;
+
+	reg_rrsr = RATE_ALL_CCK | RATE_ALL_OFDM_AG;
+	/* Init value for RRSR. */
+	rtl_write_dword(rtlpriv, REG_RRSR, reg_rrsr);
+
+	/* ARFB table 9 for 11ac 5G 2SS */
+	rtl_write_dword(rtlpriv, REG_ARFR0, 0x00000010);
+	rtl_write_dword(rtlpriv, REG_ARFR0 + 4, 0x3e0ff000);
+
+	/* ARFB table 10 for 11ac 5G 1SS */
+	rtl_write_dword(rtlpriv, REG_ARFR1, 0x00000010);
+	rtl_write_dword(rtlpriv, REG_ARFR1 + 4, 0x000ff000);
+
+	/* Set SLOT time */
+	rtl_write_byte(rtlpriv, REG_SLOT, 0x09);
+
+	/* CF-End setting. */
+	rtl_write_word(rtlpriv, REG_FWHW_TXQ_CTRL, 0x1F80);
+
+	/* Set retry limit */
+	rtl_write_word(rtlpriv, REG_RETRY_LIMIT, 0x0707);
+
+	/* BAR settings */
+	rtl_write_dword(rtlpriv, REG_BAR_MODE_CTRL, 0x0201ffff);
+
+	/* Set Data / Response auto rate fallack retry count */
+	rtl_write_dword(rtlpriv, REG_DARFRC, 0x01000000);
+	rtl_write_dword(rtlpriv, REG_DARFRC + 4, 0x07060504);
+	rtl_write_dword(rtlpriv, REG_RARFRC, 0x01000000);
+	rtl_write_dword(rtlpriv, REG_RARFRC + 4, 0x07060504);
+
+	/* Beacon related, for rate adaptive */
+	rtl_write_byte(rtlpriv, REG_ATIMWND, 0x2);
+	rtl_write_byte(rtlpriv, REG_BCN_MAX_ERR, 0xff);
+
+	rtlpci->reg_bcn_ctrl_val = 0x1d;
+	rtl_write_byte(rtlpriv, REG_BCN_CTRL, rtlpci->reg_bcn_ctrl_val);
+
+	/* Marked out by Bruce, 2010-09-09.
+	 * This register is configured for the 2nd Beacon (multiple BSSID).
+	 * We shall disable this register if we only support 1 BSSID.
+	 * vivi guess 92d also need this, also 92d now doesnot set this reg
+	 */
+	rtl_write_byte(rtlpriv, REG_BCN_CTRL_1, 0);
+
+	/* TBTT prohibit hold time. Suggested by designer TimChen. */
+	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 1 , 0xff); /* 8 ms */
+
+	rtl_write_byte(rtlpriv, REG_PIFS, 0);
+	rtl_write_byte(rtlpriv, REG_AGGR_BREAK_TIME, 0x16);
+
+	rtl_write_word(rtlpriv, REG_NAV_PROT_LEN, 0x0040);
+	rtl_write_word(rtlpriv, REG_PROT_MODE_CTRL, 0x08ff);
+
+	/* For Rx TP. Suggested by SD1 Richard. Added by tynli. 2010.04.12.*/
+	rtl_write_dword(rtlpriv, REG_FAST_EDCA_CTRL, 0x03086666);
+
+	/* ACKTO for IOT issue. */
+	rtl_write_byte(rtlpriv, REG_ACKTO, 0x40);
+
+	/* Set Spec SIFS (used in NAV) */
+	rtl_write_word(rtlpriv, REG_SPEC_SIFS, 0x100a);
+	rtl_write_word(rtlpriv, REG_MAC_SPEC_SIFS, 0x100a);
+
+	/* Set SIFS for CCK */
+	rtl_write_word(rtlpriv, REG_SIFS_CTX, 0x100a);
+
+	/* Set SIFS for OFDM */
+	rtl_write_word(rtlpriv, REG_SIFS_TRX, 0x100a);
+
+	/* Note Data sheet don't define */
+	rtl_write_word(rtlpriv, 0x4C7, 0x80);
+
+	rtl_write_byte(rtlpriv, REG_RX_PKT_LIMIT, 0x20);
+
+	rtl_write_word(rtlpriv, REG_MAX_AGGR_NUM, 0x1717);
+
+	/* Set Multicast Address. 2009.01.07. by tynli. */
+	rtl_write_dword(rtlpriv, REG_MAR, 0xffffffff);
+	rtl_write_dword(rtlpriv, REG_MAR + 4, 0xffffffff);
+}
+
+static void _rtl92ee_enable_aspm_back_door(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	u32 tmp32 = 0, count = 0;
+	u8 tmp8 = 0;
+
+	rtl_write_word(rtlpriv, REG_BACKDOOR_DBI_DATA, 0x78);
+	rtl_write_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2, 0x2);
+	tmp8 = rtl_read_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2);
+	count = 0;
+	while (tmp8 && count < 20) {
+		udelay(10);
+		tmp8 = rtl_read_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2);
+		count++;
+	}
+
+	if (0 == tmp8) {
+		tmp32 = rtl_read_dword(rtlpriv, REG_BACKDOOR_DBI_RDATA);
+		if ((tmp32 & 0xff00) != 0x2000) {
+			tmp32 &= 0xffff00ff;
+			rtl_write_dword(rtlpriv, REG_BACKDOOR_DBI_WDATA,
+					tmp32 | BIT(13));
+			rtl_write_word(rtlpriv, REG_BACKDOOR_DBI_DATA, 0xf078);
+			rtl_write_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2, 0x1);
+
+			tmp8 = rtl_read_byte(rtlpriv , REG_BACKDOOR_DBI_DATA + 2);
+			count = 0;
+			while (tmp8 && count < 20) {
+				udelay(10);
+				tmp8 = rtl_read_byte(rtlpriv,
+						     REG_BACKDOOR_DBI_DATA + 2);
+				count++;
+			}
+		}
+	}
+
+	rtl_write_word(rtlpriv, REG_BACKDOOR_DBI_DATA, 0x70c);
+	rtl_write_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2, 0x2);
+	tmp8 = rtl_read_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2);
+	count = 0;
+	while (tmp8 && count < 20) {
+		udelay(10);
+		tmp8 = rtl_read_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2);
+		count++;
+	}
+	if (0 == tmp8) {
+		tmp32 = rtl_read_dword(rtlpriv, REG_BACKDOOR_DBI_RDATA);
+		rtl_write_dword(rtlpriv, REG_BACKDOOR_DBI_WDATA,
+				tmp32 | BIT(31));
+		rtl_write_word(rtlpriv, REG_BACKDOOR_DBI_DATA, 0xf70c);
+		rtl_write_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2, 0x1);
+	}
+
+	tmp8 = rtl_read_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2);
+	count = 0;
+	while (tmp8 && count < 20) {
+		udelay(10);
+		tmp8 = rtl_read_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2);
+		count++;
+	}
+
+
+	rtl_write_word(rtlpriv, REG_BACKDOOR_DBI_DATA, 0x718);
+	rtl_write_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2, 0x2);
+	tmp8 = rtl_read_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2);
+	count = 0;
+	while (tmp8 && count < 20) {
+		udelay(10);
+		tmp8 = rtl_read_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2);
+		count++;
+	}
+	if (ppsc->b_support_backdoor || (0 == tmp8)) {
+		tmp32 = rtl_read_dword(rtlpriv, REG_BACKDOOR_DBI_RDATA);
+		rtl_write_dword(rtlpriv, REG_BACKDOOR_DBI_WDATA,
+				tmp32 | BIT(11) | BIT(12));
+		rtl_write_word(rtlpriv, REG_BACKDOOR_DBI_DATA, 0xf718);
+		rtl_write_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2, 0x1);
+	}
+	tmp8 = rtl_read_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2);
+	count = 0;
+	while (tmp8 && count < 20) {
+		udelay(10);
+		tmp8 = rtl_read_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2);
+		count++;
+	}
+}
+
+void rtl92ee_enable_hw_security_config(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 sec_reg_value;
+	u8 tmp;
+
+	RT_TRACE(COMP_INIT, DBG_DMESG,
+		 ("PairwiseEncAlgorithm = %d GroupEncAlgorithm = %d\n",
+		  rtlpriv->sec.pairwise_enc_algorithm,
+		  rtlpriv->sec.group_enc_algorithm));
+
+	if (rtlpriv->cfg->mod_params->sw_crypto || rtlpriv->sec.use_sw_sec) {
+		RT_TRACE(COMP_SEC, DBG_DMESG, ("not open hw encryption\n"));
+		return;
+	}
+
+	sec_reg_value = SCR_TxEncEnable | SCR_RxDecEnable;
+
+	if (rtlpriv->sec.use_defaultkey) {
+		sec_reg_value |= SCR_TxUseDK;
+		sec_reg_value |= SCR_RxUseDK;
+	}
+
+	sec_reg_value |= (SCR_RXBCUSEDK | SCR_TXBCUSEDK);
+
+	tmp = rtl_read_byte(rtlpriv, REG_CR + 1);
+	rtl_write_byte(rtlpriv, REG_CR + 1, tmp | BIT(1));
+
+	RT_TRACE(COMP_SEC, DBG_DMESG,
+		 ("The SECR-value %x \n", sec_reg_value));
+
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_WPA_CONFIG, &sec_reg_value);
+
+}
+
+int rtl92ee_hw_init(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	bool rtstatus = true;
+	int err = 0;
+	u8 tmp_u1b , u1byte;
+	u32 tmp_u4b;
+
+	RT_TRACE(COMP_INIT , DBG_LOUD , (" Rtl8192EE hw init\n"));
+	rtlpriv->rtlhal.being_init_adapter = true;
+	rtlpriv->intf_ops->disable_aspm(hw);
+
+	tmp_u1b = rtl_read_byte(rtlpriv, REG_SYS_CLKR+1);
+	u1byte = rtl_read_byte(rtlpriv, REG_CR);
+	if ((tmp_u1b & BIT(3)) && (u1byte != 0 && u1byte != 0xEA)) {
+		rtlhal->b_mac_func_enable = true;
+	} else {
+		rtlhal->b_mac_func_enable = false;
+		rtlhal->fw_ps_state = FW_PS_STATE_ALL_ON_92E;
+	}
+
+	rtstatus = _rtl92ee_init_mac(hw);
+
+	rtl_write_byte(rtlpriv, 0x577, 0x03);
+
+	/*for Crystal 40 Mhz setting */
+	rtl_write_byte(rtlpriv, REG_AFE_CTRL4, 0x2A);
+	rtl_write_byte(rtlpriv, REG_AFE_CTRL4 + 1, 0x00);
+	rtl_write_byte(rtlpriv, REG_AFE_CTRL2, 0x83);
+
+	/*Forced the antenna b to wifi */
+	if (rtlpriv->btcoexist.btc_info.btcoexist == 1) {
+		rtl_write_byte(rtlpriv, 0x64, 0);
+		rtl_write_byte(rtlpriv, 0x65, 1);
+	}
+	if (rtstatus != true) {
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("Init MAC failed\n"));
+		err = 1;
+		return err;
+	}
+	rtlhal->rx_tag = 0;
+	rtl_write_word(rtlpriv, REG_PCIE_CTRL_REG, 0x8000);
+	err = rtl92ee_download_fw(hw , false);
+	if (err) {
+		RT_TRACE(COMP_ERR, DBG_WARNING,
+			 ("Failed to download FW. Init HW without FW now..\n"));
+		err = 1;
+		rtlhal->bfw_ready = false;
+		return err;
+	} else {
+		rtlhal->bfw_ready = true;
+	}
+	/*fw related variable initialize */
+	ppsc->b_fw_current_inpsmode = false;
+	rtlhal->fw_ps_state = FW_PS_STATE_ALL_ON_92E;
+	rtlhal->bfw_clk_change_in_progress = false;
+	rtlhal->ballow_sw_to_change_hwclc = false;
+	rtlhal->last_hmeboxnum = 0;
+
+
+	rtl92ee_phy_mac_config(hw);
+
+	rtl92ee_phy_bb_config(hw);
+
+	rtl92ee_phy_rf_config(hw);
+
+	rtlphy->rfreg_chnlval[0] = rtl_get_rfreg(hw, RF90_PATH_A,
+						 RF_CHNLBW, RFREG_OFFSET_MASK);
+	rtlphy->rfreg_chnlval[1] = rtl_get_rfreg(hw, RF90_PATH_B,
+						 RF_CHNLBW, RFREG_OFFSET_MASK);
+	rtlphy->backup_rf_0x1a = (u32) rtl_get_rfreg(hw, RF90_PATH_A, RF_RX_G1,
+						     RFREG_OFFSET_MASK);
+	rtlphy->rfreg_chnlval[0] = (rtlphy->rfreg_chnlval[0] & 0xfffff3ff) |
+				   BIT(10) | BIT(11);
+
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_CHNLBW, RFREG_OFFSET_MASK,
+		      rtlphy->rfreg_chnlval[0]);
+	rtl_set_rfreg(hw, RF90_PATH_B, RF_CHNLBW, RFREG_OFFSET_MASK,
+		      rtlphy->rfreg_chnlval[0]);
+
+	/*---- Set CCK and OFDM Block "ON"----*/
+	rtl_set_bbreg(hw, RFPGA0_RFMOD, BCCKEN, 0x1);
+	rtl_set_bbreg(hw, RFPGA0_RFMOD, BOFDMEN, 0x1);
+
+	/* Must set this,
+	 * otherwise the rx sensitivity will be very pool. Maddest
+	 */
+	rtl_set_rfreg(hw, RF90_PATH_A, 0xB1, RFREG_OFFSET_MASK, 0x54418);
+
+	/*Set Hardware(MAC default setting.)*/
+	_rtl92ee_hw_configure(hw);
+
+	rtlhal->b_mac_func_enable = true;
+
+	stg_rtl_cam_reset_all_entry(hw);
+	rtl92ee_enable_hw_security_config(hw);
+
+	ppsc->rfpwr_state = ERFON;
+
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ETHER_ADDR, mac->mac_addr);
+	_rtl92ee_enable_aspm_back_door(hw);
+	rtlpriv->intf_ops->enable_aspm(hw);
+
+	rtl92ee_bt_hw_init(hw);
+
+	rtlpriv->rtlhal.being_init_adapter = false;
+
+	if (ppsc->rfpwr_state == ERFON) {
+		if (rtlphy->iqk_initialized) {
+			rtl92ee_phy_iq_calibrate(hw, true);
+		} else {
+			rtl92ee_phy_iq_calibrate(hw, false);
+			rtlphy->iqk_initialized = true;
+		}
+	}
+
+	rtlphy->rfpath_rx_enable[0] = true;
+	if (rtlphy->rf_type == RF_2T2R)
+		rtlphy->rfpath_rx_enable[1] = true;
+
+	stg_efuse_one_byte_read(hw, 0x1FA, &tmp_u1b);
+	if (!(tmp_u1b & BIT(0))) {
+		rtl_set_rfreg(hw, RF90_PATH_A, 0x15, 0x0F, 0x05);
+		RT_TRACE(COMP_INIT, DBG_LOUD, ("PA BIAS path A\n"));
+	}
+
+	if ((!(tmp_u1b & BIT(1))) && (rtlphy->rf_type == RF_2T2R)) {
+		rtl_set_rfreg(hw, RF90_PATH_B, 0x15, 0x0F, 0x05);
+		RT_TRACE(COMP_INIT, DBG_LOUD, ("PA BIAS path B\n"));
+	}
+
+	rtl_write_byte(rtlpriv, REG_NAV_UPPER, ((30000 + 127) / 128));
+
+	/*Fixed LDPC rx hang issue. */
+	tmp_u4b = rtl_read_dword(rtlpriv, REG_SYS_SWR_CTRL1);
+	rtl_write_byte(rtlpriv, REG_SYS_SWR_CTRL2, 0x75);
+	tmp_u4b =  (tmp_u4b & 0xfff00fff) | (0x7E << 12);
+	rtl_write_dword(rtlpriv, REG_SYS_SWR_CTRL1, tmp_u4b);
+
+	rtl92ee_dm_init(hw);
+
+	rtl_write_dword(rtlpriv, 0x4fc, 0);
+
+	RT_TRACE(COMP_INIT , DBG_LOUD , ("end of Rtl8192EE hw init %x\n" , err));
+	return 0;
+}
+
+static enum version_8192e _rtl92ee_read_chip_version(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	enum version_8192e version = VERSION_UNKNOWN;
+	u32 value32;
+
+	rtlphy->rf_type = RF_2T2R;
+
+	value32 = rtl_read_dword(rtlpriv, REG_SYS_CFG1);
+	if (value32 & TRP_VAUX_EN)
+		version = (enum version_8192e) VERSION_TEST_CHIP_2T2R_8192E;
+	else
+		version = (enum version_8192e) VERSION_NORMAL_CHIP_2T2R_8192E;
+
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("Chip RF Type: %s\n", (rtlphy->rf_type == RF_2T2R) ?
+		  "RF_2T2R" : "RF_1T1R"));
+
+	return version;
+}
+
+static int _rtl92ee_set_media_status(struct ieee80211_hw *hw,
+				     enum nl80211_iftype type)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 bt_msr = rtl_read_byte(rtlpriv, MSR);
+	enum led_ctl_mode ledaction = LED_CTL_NO_LINK;
+	u8 mode = MSR_NOLINK;
+	bt_msr &= 0xfc;
+
+	switch (type) {
+	case NL80211_IFTYPE_UNSPECIFIED:
+		mode = MSR_NOLINK;
+		RT_TRACE(COMP_INIT, DBG_TRACE,
+			 ("Set Network type to NO LINK!\n"));
+		break;
+	case NL80211_IFTYPE_ADHOC:
+	case NL80211_IFTYPE_MESH_POINT:
+		mode = MSR_ADHOC;
+		RT_TRACE(COMP_INIT, DBG_TRACE,
+			 ("Set Network type to Ad Hoc!\n"));
+		break;
+	case NL80211_IFTYPE_STATION:
+		mode = MSR_INFRA;
+		ledaction = LED_CTL_LINK;
+		RT_TRACE(COMP_INIT, DBG_TRACE,
+			 ("Set Network type to STA!\n"));
+		break;
+	case NL80211_IFTYPE_AP:
+		mode = MSR_AP;
+		ledaction = LED_CTL_LINK;
+		RT_TRACE(COMP_INIT, DBG_TRACE,
+			 ("Set Network type to AP!\n"));
+		break;
+	default:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("Network type %d not support!\n", type));
+		return 1;
+		break;
+	}
+
+	/* MSR_INFRA == Link in infrastructure network;
+	 * MSR_ADHOC == Link in ad hoc network;
+	 * Therefore, check link state is necessary.
+	 *
+	 * MSR_AP == AP mode; link state is not cared here.
+	 */
+	if (mode != MSR_AP && rtlpriv->mac80211.link_state < MAC80211_LINKED) {
+		mode = MSR_NOLINK;
+		ledaction = LED_CTL_NO_LINK;
+	}
+
+	if (mode == MSR_NOLINK || mode == MSR_INFRA) {
+		_rtl92ee_stop_tx_beacon(hw);
+		_rtl92ee_enable_bcn_sub_func(hw);
+	} else if (mode == MSR_ADHOC || mode == MSR_AP) {
+		_rtl92ee_resume_tx_beacon(hw);
+		_rtl92ee_disable_bcn_sub_func(hw);
+	} else {
+		RT_TRACE(COMP_ERR, DBG_WARNING,
+			 ("Set HW_VAR_MEDIA_STATUS: "
+			  "No such media status(%x).\n", mode));
+	}
+
+	rtl_write_byte(rtlpriv, (MSR), bt_msr | mode);
+	rtlpriv->cfg->ops->led_control(hw, ledaction);
+	if (mode == MSR_AP)
+		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x00);
+	else
+		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x66);
+	return 0;
+}
+
+void rtl92ee_set_check_bssid(struct ieee80211_hw *hw, bool check_bssid)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u32 reg_rcr = rtlpci->receive_config;
+
+	if (rtlpriv->psc.rfpwr_state != ERFON)
+		return;
+
+	if (check_bssid == true) {
+		reg_rcr |= (RCR_CBSSID_DATA | RCR_CBSSID_BCN);
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_RCR,
+					      (u8 *) (&reg_rcr));
+		_rtl92ee_set_bcn_ctrl_reg(hw, 0, BIT(4));
+	} else if (check_bssid == false) {
+		reg_rcr &= (~(RCR_CBSSID_DATA | RCR_CBSSID_BCN));
+		_rtl92ee_set_bcn_ctrl_reg(hw, BIT(4), 0);
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_RCR,
+					      (u8 *) (&reg_rcr));
+	}
+}
+
+int rtl92ee_set_network_type(struct ieee80211_hw *hw, enum nl80211_iftype type)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (_rtl92ee_set_media_status(hw, type))
+		return -EOPNOTSUPP;
+
+	if (rtlpriv->mac80211.link_state == MAC80211_LINKED) {
+		if (type != NL80211_IFTYPE_AP &&
+		    type != NL80211_IFTYPE_MESH_POINT)
+			rtl92ee_set_check_bssid(hw, true);
+	} else {
+		rtl92ee_set_check_bssid(hw, false);
+	}
+
+	return 0;
+}
+
+/* don't set REG_EDCA_BE_PARAM here because mac80211 will send pkt when scan */
+void rtl92ee_set_qos(struct ieee80211_hw *hw, int aci)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	rtl92ee_dm_init_edca_turbo(hw);
+	switch (aci) {
+	case AC1_BK:
+		rtl_write_dword(rtlpriv, REG_EDCA_BK_PARAM, 0xa44f);
+		break;
+	case AC0_BE:
+		/* rtl_write_dword(rtlpriv, REG_EDCA_BE_PARAM, u4b_ac_param); */
+		break;
+	case AC2_VI:
+		rtl_write_dword(rtlpriv, REG_EDCA_VI_PARAM, 0x5e4322);
+		break;
+	case AC3_VO:
+		rtl_write_dword(rtlpriv, REG_EDCA_VO_PARAM, 0x2f3222);
+		break;
+	default:
+		RT_ASSERT(false, ("invalid aci: %d !\n", aci));
+		break;
+	}
+}
+
+static void rtl92ee_clear_interrupt(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 tmp;
+
+	tmp = rtl_read_dword(rtlpriv, REG_HISR);
+	rtl_write_dword(rtlpriv, REG_HISR, tmp);
+
+	tmp = rtl_read_dword(rtlpriv, REG_HISRE);
+	rtl_write_dword(rtlpriv, REG_HISRE, tmp);
+
+	tmp = rtl_read_dword(rtlpriv, REG_HSISR);
+	rtl_write_dword(rtlpriv, REG_HSISR, tmp);
+}
+
+void rtl92ee_enable_interrupt(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	rtl92ee_clear_interrupt(hw);/*clear it here first*/
+
+	rtl_write_dword(rtlpriv, REG_HIMR, rtlpci->irq_mask[0] & 0xFFFFFFFF);
+	rtl_write_dword(rtlpriv, REG_HIMRE, rtlpci->irq_mask[1] & 0xFFFFFFFF);
+	rtlpci->irq_enabled = true;
+}
+
+void rtl92ee_disable_interrupt(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	rtl_write_dword(rtlpriv, REG_HIMR, IMR_DISABLED);
+	rtl_write_dword(rtlpriv, REG_HIMRE, IMR_DISABLED);
+	rtlpci->irq_enabled = false;
+	/*synchronize_irq(rtlpci->pdev->irq);*/
+}
+
+static void _rtl92ee_poweroff_adapter(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 u1b_tmp;
+	rtlhal->b_mac_func_enable = false;
+
+	RT_TRACE(COMP_INIT , DBG_LOUD , ("POWER OFF adapter \n"));
+
+	/* Run LPS WL RFOFF flow */
+	rtl92e_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
+				 PWR_INTF_PCI_MSK, Rtl8192E_NIC_LPS_ENTER_FLOW);
+	/* turn off RF */
+	rtl_write_byte(rtlpriv, REG_RF_CTRL, 0x00);
+
+	/* ==== Reset digital sequence   ======  */
+	if ((rtl_read_byte(rtlpriv, REG_MCUFWDL) & BIT(7)) && rtlhal->bfw_ready)
+		rtl92ee_firmware_selfreset(hw);
+
+	/* Reset MCU  */
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN + 1, (u1b_tmp & (~BIT(2))));
+
+	/* reset MCU ready status */
+	rtl_write_byte(rtlpriv, REG_MCUFWDL, 0x00);
+
+	/* HW card disable configuration. */
+	rtl92e_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
+				 PWR_INTF_PCI_MSK, Rtl8192E_NIC_DISABLE_FLOW);
+
+	/* Reset MCU IO Wrapper */
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL + 1);
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL + 1, (u1b_tmp & (~BIT(0))));
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL + 1);
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL + 1, (u1b_tmp | BIT(0)));
+
+	/* lock ISO/CLK/Power control register */
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL, 0x0E);
+}
+
+void rtl92ee_card_disable(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	enum nl80211_iftype opmode;
+
+	RT_TRACE(COMP_INIT , DBG_LOUD , ("RTL8192ee card disable\n"));
+
+	RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
+
+	mac->link_state = MAC80211_NOLINK;
+	opmode = NL80211_IFTYPE_UNSPECIFIED;
+
+	_rtl92ee_set_media_status(hw, opmode);
+
+	if (rtlpriv->rtlhal.driver_is_goingto_unload ||
+	    ppsc->rfoff_reason > RF_CHANGE_BY_PS)
+		rtlpriv->cfg->ops->led_control(hw, LED_CTL_POWER_OFF);
+
+	_rtl92ee_poweroff_adapter(hw);
+
+	/* after power off we should do iqk again */
+	rtlpriv->phy.iqk_initialized = false;
+}
+
+void rtl92ee_interrupt_recognized(struct ieee80211_hw *hw,
+				  u32 *p_inta, u32 *p_intb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	*p_inta = rtl_read_dword(rtlpriv, ISR) & rtlpci->irq_mask[0];
+	rtl_write_dword(rtlpriv, ISR, *p_inta);
+
+	*p_intb = rtl_read_dword(rtlpriv, REG_HISRE) & rtlpci->irq_mask[1];
+	rtl_write_dword(rtlpriv, REG_HISRE, *p_intb);
+}
+
+void rtl92ee_set_beacon_related_registers(struct ieee80211_hw *hw)
+{
+
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u16 bcn_interval, atim_window;
+
+	bcn_interval = mac->beacon_interval;
+	atim_window = 2;	/*FIX MERGE */
+	rtl92ee_disable_interrupt(hw);
+	rtl_write_word(rtlpriv, REG_ATIMWND, atim_window);
+	rtl_write_word(rtlpriv, REG_BCN_INTERVAL, bcn_interval);
+	rtl_write_word(rtlpriv, REG_BCNTCFG, 0x660f);
+	rtl_write_byte(rtlpriv, REG_RXTSF_OFFSET_CCK, 0x18);
+	rtl_write_byte(rtlpriv, REG_RXTSF_OFFSET_OFDM, 0x18);
+	rtl_write_byte(rtlpriv, 0x606, 0x30);
+	rtlpci->reg_bcn_ctrl_val |= BIT(3);
+	rtl_write_byte(rtlpriv, REG_BCN_CTRL, (u8) rtlpci->reg_bcn_ctrl_val);
+}
+
+void rtl92ee_set_beacon_interval(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u16 bcn_interval = mac->beacon_interval;
+
+	RT_TRACE(COMP_BEACON, DBG_DMESG,
+		 ("beacon_interval:%d\n", bcn_interval));
+	rtl_write_word(rtlpriv, REG_BCN_INTERVAL, bcn_interval);
+}
+
+void rtl92ee_update_interrupt_mask(struct ieee80211_hw *hw,
+				   u32 add_msr, u32 rm_msr)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	RT_TRACE(COMP_INTR, DBG_LOUD,
+		 ("add_msr:%x, rm_msr:%x\n", add_msr, rm_msr));
+
+	if (add_msr)
+		rtlpci->irq_mask[0] |= add_msr;
+	if (rm_msr)
+		rtlpci->irq_mask[0] &= (~rm_msr);
+	rtl92ee_disable_interrupt(hw);
+	rtl92ee_enable_interrupt(hw);
+}
+
+static u8 _rtl92ee_get_chnl_group(u8 chnl)
+{
+	u8 group = 0;
+
+	if (chnl <= 14) {
+		if (1 <= chnl && chnl <= 2)
+			group = 0;
+		else if (3 <= chnl && chnl <= 5)
+			group = 1;
+		else if (6 <= chnl && chnl <= 8)
+			group = 2;
+		else if (9 <= chnl && chnl <= 11)
+			group = 3;
+		else if (12 <= chnl && chnl <= 14)
+			group = 4;
+	} else {
+		if (36 <= chnl && chnl <= 42)
+			group = 0;
+		else if (44 <= chnl && chnl <= 48)
+			group = 1;
+		else if (50 <= chnl && chnl <= 58)
+			group = 2;
+		else if (60 <= chnl && chnl <= 64)
+			group = 3;
+		else if (100 <= chnl && chnl <= 106)
+			group = 4;
+		else if (108 <= chnl && chnl <= 114)
+			group = 5;
+		else if (116 <= chnl && chnl <= 122)
+			group = 6;
+		else if (124 <= chnl && chnl <= 130)
+			group = 7;
+		else if (132 <= chnl && chnl <= 138)
+			group = 8;
+		else if (140 <= chnl && chnl <= 144)
+			group = 9;
+		else if (149 <= chnl && chnl <= 155)
+			group = 10;
+		else if (157 <= chnl && chnl <= 161)
+			group = 11;
+		else if (165 <= chnl && chnl <= 171)
+			group = 12;
+		else if (173 <= chnl && chnl <= 177)
+			group = 13;
+	}
+	return group;
+}
+
+static void _rtl8192ee_read_power_value_fromprom(struct ieee80211_hw *hw,
+						 struct txpower_info_2g *pwr2g,
+						 struct txpower_info_5g *pwr5g,
+						 bool autoload_fail, u8 *hwinfo)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 rf, addr = EEPROM_TX_PWR_INX, group, i = 0;
+
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("hal_ReadPowerValueFromPROM92E(): PROMContent[0x%x]= 0x%x\n",
+		 (addr + 1), hwinfo[addr + 1]));
+	if (0xFF == hwinfo[addr+1])  /*YJ, add, 120316*/
+		autoload_fail = true;
+
+	if (autoload_fail) {
+		RT_TRACE(COMP_INIT, DBG_LOUD,
+			 ("auto load fail : Use Default value!\n"));
+		for (rf = 0 ; rf < MAX_RF_PATH ; rf++) {
+			/* 2.4G default value */
+			for (group = 0 ; group < MAX_CHNL_GROUP_24G; group++) {
+				pwr2g->index_cck_base[rf][group] = 0x2D;
+				pwr2g->index_bw40_base[rf][group] = 0x2D;
+			}
+			for (i = 0; i < MAX_TX_COUNT; i++) {
+				if (i == 0) {
+					pwr2g->bw20_diff[rf][0] = 0x02;
+					pwr2g->ofdm_diff[rf][0] = 0x04;
+				} else {
+					pwr2g->bw20_diff[rf][i] = 0xFE;
+					pwr2g->bw40_diff[rf][i] = 0xFE;
+					pwr2g->cck_diff[rf][i] = 0xFE;
+					pwr2g->ofdm_diff[rf][i] = 0xFE;
+				}
+			}
+
+			/*5G default value*/
+			for (group = 0 ; group < MAX_CHNL_GROUP_5G; group++)
+				pwr5g->index_bw40_base[rf][group] = 0x2A;
+
+			for (i = 0; i < MAX_TX_COUNT; i++) {
+				if (i == 0) {
+					pwr5g->ofdm_diff[rf][0] = 0x04;
+					pwr5g->bw20_diff[rf][0] = 0x00;
+					pwr5g->bw80_diff[rf][0] = 0xFE;
+					pwr5g->bw160_diff[rf][0] = 0xFE;
+				} else {
+					pwr5g->ofdm_diff[rf][0] = 0xFE;
+					pwr5g->bw20_diff[rf][0] = 0xFE;
+					pwr5g->bw40_diff[rf][0] = 0xFE;
+					pwr5g->bw80_diff[rf][0] = 0xFE;
+					pwr5g->bw160_diff[rf][0] = 0xFE;
+				}
+			}
+		}
+		return;
+	}
+
+	rtl_priv(hw)->efuse.b_txpwr_fromeprom = true;
+
+	for (rf = 0 ; rf < MAX_RF_PATH ; rf++) {
+		/*2.4G default value*/
+		for (group = 0 ; group < MAX_CHNL_GROUP_24G; group++) {
+			pwr2g->index_cck_base[rf][group] = hwinfo[addr++];
+			if (pwr2g->index_cck_base[rf][group] == 0xFF)
+				pwr2g->index_cck_base[rf][group] = 0x2D;
+
+		}
+		for (group = 0 ; group < MAX_CHNL_GROUP_24G - 1; group++) {
+			pwr2g->index_bw40_base[rf][group] = hwinfo[addr++];
+			if (pwr2g->index_bw40_base[rf][group] == 0xFF)
+				pwr2g->index_bw40_base[rf][group] = 0x2D;
+		}
+		for (i = 0; i < MAX_TX_COUNT; i++) {
+			if (i == 0) {
+				pwr2g->bw40_diff[rf][i] = 0;
+				if (hwinfo[addr] == 0xFF) {
+					pwr2g->bw20_diff[rf][i] = 0x02;
+				} else {
+					pwr2g->bw20_diff[rf][i] = (hwinfo[addr]
+								   & 0xf0) >> 4;
+					if (pwr2g->bw20_diff[rf][i] & BIT(3))
+						pwr2g->bw20_diff[rf][i] |= 0xF0;
+				}
+
+				if (hwinfo[addr] == 0xFF) {
+					pwr2g->ofdm_diff[rf][i] = 0x04;
+				} else {
+					pwr2g->ofdm_diff[rf][i] = (hwinfo[addr]
+								   & 0x0f);
+					if (pwr2g->ofdm_diff[rf][i] & BIT(3))
+						pwr2g->ofdm_diff[rf][i] |= 0xF0;
+				}
+				pwr2g->cck_diff[rf][i] = 0;
+				addr++;
+			} else {
+				if (hwinfo[addr] == 0xFF) {
+					pwr2g->bw40_diff[rf][i] = 0xFE;
+				} else {
+					pwr2g->bw40_diff[rf][i] = (hwinfo[addr]
+								   & 0xf0) >> 4;
+					if (pwr2g->bw40_diff[rf][i] & BIT(3))
+						pwr2g->bw40_diff[rf][i] |= 0xF0;
+				}
+
+				if (hwinfo[addr] == 0xFF) {
+					pwr2g->bw20_diff[rf][i] = 0xFE;
+				} else {
+					pwr2g->bw20_diff[rf][i] = (hwinfo[addr]
+								   & 0x0f);
+					if (pwr2g->bw20_diff[rf][i] & BIT(3))
+						pwr2g->bw20_diff[rf][i] |= 0xF0;
+				}
+				addr++;
+
+				if (hwinfo[addr] == 0xFF) {
+					pwr2g->ofdm_diff[rf][i] = 0xFE;
+				} else {
+					pwr2g->ofdm_diff[rf][i] = (hwinfo[addr]
+								   & 0xf0) >> 4;
+					if (pwr2g->ofdm_diff[rf][i] & BIT(3))
+						pwr2g->ofdm_diff[rf][i] |= 0xF0;
+				}
+
+				if (hwinfo[addr] == 0xFF) {
+					pwr2g->cck_diff[rf][i] = 0xFE;
+				} else {
+					pwr2g->cck_diff[rf][i] = (hwinfo[addr]
+								  & 0x0f);
+					if (pwr2g->cck_diff[rf][i] & BIT(3))
+						pwr2g->cck_diff[rf][i] |= 0xF0;
+				}
+				addr++;
+			}
+		}
+
+		/*5G default value*/
+		for (group = 0 ; group < MAX_CHNL_GROUP_5G; group++) {
+			pwr5g->index_bw40_base[rf][group] = hwinfo[addr++];
+			if (pwr5g->index_bw40_base[rf][group] == 0xFF)
+				pwr5g->index_bw40_base[rf][group] = 0xFE;
+		}
+
+		for (i = 0; i < MAX_TX_COUNT; i++) {
+			if (i == 0) {
+				pwr5g->bw40_diff[rf][i] = 0;
+
+				if (hwinfo[addr] == 0xFF) {
+					pwr5g->bw20_diff[rf][i] = 0;
+				} else {
+					pwr5g->bw20_diff[rf][0] = (hwinfo[addr]
+								   & 0xf0) >> 4;
+					if (pwr5g->bw20_diff[rf][i] & BIT(3))
+						pwr5g->bw20_diff[rf][i] |= 0xF0;
+				}
+
+				if (hwinfo[addr] == 0xFF) {
+					pwr5g->ofdm_diff[rf][i] = 0x04;
+				} else {
+					pwr5g->ofdm_diff[rf][0] = (hwinfo[addr]
+								   & 0x0f);
+					if (pwr5g->ofdm_diff[rf][i] & BIT(3))
+						pwr5g->ofdm_diff[rf][i] |= 0xF0;
+				}
+				addr++;
+			} else {
+				if (hwinfo[addr] == 0xFF) {
+					pwr5g->bw40_diff[rf][i] = 0xFE;
+				} else {
+					pwr5g->bw40_diff[rf][i] = (hwinfo[addr]
+								  & 0xf0) >> 4;
+					if (pwr5g->bw40_diff[rf][i] & BIT(3))
+					pwr5g->bw40_diff[rf][i] |= 0xF0;
+				}
+
+				if (hwinfo[addr] == 0xFF) {
+					pwr5g->bw20_diff[rf][i] = 0xFE;
+				} else {
+					pwr5g->bw20_diff[rf][i] = (hwinfo[addr]
+								   & 0x0f);
+					if (pwr5g->bw20_diff[rf][i] & BIT(3))
+					pwr5g->bw20_diff[rf][i] |= 0xF0;
+				}
+				addr++;
+			}
+		}
+
+		if (hwinfo[addr] == 0xFF) {
+			pwr5g->ofdm_diff[rf][1] = 0xFE;
+			pwr5g->ofdm_diff[rf][2] = 0xFE;
+		} else {
+			pwr5g->ofdm_diff[rf][1] = (hwinfo[addr] & 0xf0) >> 4;
+			pwr5g->ofdm_diff[rf][2] = (hwinfo[addr] & 0x0f);
+		}
+		addr++;
+
+		if (hwinfo[addr] == 0xFF)
+			pwr5g->ofdm_diff[rf][3] = 0xFE;
+		else
+			pwr5g->ofdm_diff[rf][3] = (hwinfo[addr] & 0x0f);
+		addr++;
+
+		for (i = 1; i < MAX_TX_COUNT; i++) {
+			if (pwr5g->ofdm_diff[rf][i] == 0xFF)
+				pwr5g->ofdm_diff[rf][i] = 0xFE;
+			else if (pwr5g->ofdm_diff[rf][i] & BIT(3))
+				pwr5g->ofdm_diff[rf][i] |= 0xF0;
+		}
+
+		for (i = 0; i < MAX_TX_COUNT; i++) {
+			if (hwinfo[addr] == 0xFF) {
+				pwr5g->bw80_diff[rf][i] = 0xFE;
+			} else {
+				pwr5g->bw80_diff[rf][i] = (hwinfo[addr] & 0xf0)
+							  >> 4;
+				if (pwr5g->bw80_diff[rf][i] & BIT(3))
+					pwr5g->bw80_diff[rf][i] |= 0xF0;
+			}
+
+			if (hwinfo[addr] == 0xFF) {
+				pwr5g->bw160_diff[rf][i] = 0xFE;
+			} else {
+				pwr5g->bw160_diff[rf][i] = (hwinfo[addr] & 0x0f);
+				if (pwr5g->bw160_diff[rf][i] & BIT(3))
+					pwr5g->bw160_diff[rf][i] |= 0xF0;
+			}
+			addr++;
+		}
+	}
+}
+static void _rtl92ee_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
+						 bool autoload_fail, u8 *hwinfo)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *efu = rtl_efuse(rtl_priv(hw));
+	struct txpower_info_2g pwr2g;
+	struct txpower_info_5g pwr5g;
+	u8 channel5g[CHANNEL_MAX_NUMBER_5G] = {
+		36, 38, 40, 42, 44, 46, 48, 50, 52, 54,
+		56, 58, 60, 62, 64, 100, 102, 104, 106,
+		108, 110, 112, 114, 116, 118, 120, 122,
+		124, 126, 128, 130, 132, 134, 136, 138,
+		140, 142, 144, 149, 151, 153, 155, 157,
+		159, 161, 163, 165, 167, 168, 169, 171,
+		173, 175, 177};
+	u8 channel5g_80m[CHANNEL_MAX_NUMBER_5G_80M] = {
+		42, 58, 106, 122, 138, 155, 171};
+	u8 rf, idx;
+	u8 i;
+
+	_rtl8192ee_read_power_value_fromprom(hw, &pwr2g, &pwr5g,
+					     autoload_fail, hwinfo);
+
+	for (rf = 0; rf < MAX_RF_PATH; rf++) {
+		for (i = 0; i < 14; i++) {
+			idx = _rtl92ee_get_chnl_group(i + 1);
+
+			if (i == CHANNEL_MAX_NUMBER_2G - 1) {
+				efu->txpwrlevel_cck[rf][i] =
+						pwr2g.index_cck_base[rf][5];
+				efu->txpwrlevel_ht40_1s[rf][i] =
+						pwr2g.index_bw40_base[rf][idx];
+			} else {
+				efu->txpwrlevel_cck[rf][i] =
+						pwr2g.index_cck_base[rf][idx];
+				efu->txpwrlevel_ht40_1s[rf][i] =
+						pwr2g.index_bw40_base[rf][idx];
+			}
+		}
+		for (i = 0; i < CHANNEL_MAX_NUMBER_5G; i++) {
+			idx = _rtl92ee_get_chnl_group(channel5g[i]);
+			efu->txpwr_5g_bw40base[rf][i] =
+					pwr5g.index_bw40_base[rf][idx];
+		}
+		for (i = 0; i < CHANNEL_MAX_NUMBER_5G_80M; i++) {
+			u8 upper, lower;
+			idx = _rtl92ee_get_chnl_group(channel5g_80m[i]);
+			upper = pwr5g.index_bw40_base[rf][idx];
+			lower = pwr5g.index_bw40_base[rf][idx + 1];
+
+			efu->txpwr_5g_bw80base[rf][i] = (upper + lower) / 2;
+		}
+		for (i = 0; i < MAX_TX_COUNT; i++) {
+			efu->txpwr_cckdiff[rf][i] = pwr2g.cck_diff[rf][i];
+			efu->txpwr_legacyhtdiff[rf][i] = pwr2g.ofdm_diff[rf][i];
+			efu->txpwr_ht20diff[rf][i] = pwr2g.bw20_diff[rf][i];
+			efu->txpwr_ht40diff[rf][i] = pwr2g.bw40_diff[rf][i];
+
+			efu->txpwr_5g_ofdmdiff[rf][i] = pwr5g.ofdm_diff[rf][i];
+			efu->txpwr_5g_bw20diff[rf][i] = pwr5g.bw20_diff[rf][i];
+			efu->txpwr_5g_bw40diff[rf][i] = pwr5g.bw40_diff[rf][i];
+			efu->txpwr_5g_bw80diff[rf][i] = pwr5g.bw80_diff[rf][i];
+		}
+	}
+
+	if (!autoload_fail)
+		efu->eeprom_thermalmeter = hwinfo[EEPROM_THERMAL_METER_92E];
+	else
+		efu->eeprom_thermalmeter = EEPROM_DEFAULT_THERMALMETER;
+
+	if (efu->eeprom_thermalmeter == 0xff || autoload_fail) {
+		efu->b_apk_thermalmeterignore = true;
+		efu->eeprom_thermalmeter = EEPROM_DEFAULT_THERMALMETER;
+	}
+
+	efu->thermalmeter[0] = efu->eeprom_thermalmeter;
+	RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+		"thermalmeter = 0x%x\n", efu->eeprom_thermalmeter);
+
+	if (!autoload_fail) {
+		efu->eeprom_regulatory = hwinfo[EEPROM_RF_BOARD_OPTION_92E]
+					 & 0x07;
+		if (hwinfo[EEPROM_RF_BOARD_OPTION_92E] == 0xFF)
+			efu->eeprom_regulatory = 0;
+	} else {
+		efu->eeprom_regulatory = 0;
+	}
+	RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+		"eeprom_regulatory = 0x%x\n", efu->eeprom_regulatory);
+}
+
+static void _rtl92ee_read_adapter_info(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u16 i, usvalue;
+	u8 hwinfo[HWSET_MAX_SIZE];
+	u16 eeprom_id;
+
+	if (rtlefuse->epromtype == EEPROM_BOOT_EFUSE) {
+		stg_rtl_efuse92e_shadow_map_update(hw);
+
+		memcpy(hwinfo, &rtlefuse->efuse_map[EFUSE_INIT_MAP][0],
+		       HWSET_MAX_SIZE);
+	} else if (rtlefuse->epromtype == EEPROM_93C46) {
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("RTL819X Not boot from eeprom, check it !!"));
+		return;
+	}  else {
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("boot from neither eeprom nor efuse, check it !!"));
+		return;
+	}
+
+	RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_LOUD, "MAP \n",
+		      hwinfo, HWSET_MAX_SIZE);
+
+	eeprom_id = *((u16 *) &hwinfo[0]);
+	if (eeprom_id != RTL8192E_EEPROM_ID) {
+		RT_TRACE(COMP_ERR, DBG_WARNING,
+			 ("EEPROM ID(%#x) is invalid!!\n", eeprom_id));
+		rtlefuse->autoload_failflag = true;
+	} else {
+		RT_TRACE(COMP_INIT, DBG_LOUD, ("Autoload OK\n"));
+		rtlefuse->autoload_failflag = false;
+	}
+
+	if (rtlefuse->autoload_failflag == true)
+		return;
+	/*VID DID SVID SDID*/
+	rtlefuse->eeprom_vid = *(u16 *) &hwinfo[EEPROM_VID];
+	rtlefuse->eeprom_did = *(u16 *) &hwinfo[EEPROM_DID];
+	rtlefuse->eeprom_svid = *(u16 *) &hwinfo[EEPROM_SVID];
+	rtlefuse->eeprom_smid = *(u16 *) &hwinfo[EEPROM_SMID];
+	RT_TRACE(COMP_INIT, DBG_LOUD, ("EEPROMId = 0x%4x\n", eeprom_id));
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("EEPROM VID = 0x%4x\n", rtlefuse->eeprom_vid));
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("EEPROM DID = 0x%4x\n", rtlefuse->eeprom_did));
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("EEPROM SVID = 0x%4x\n", rtlefuse->eeprom_svid));
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("EEPROM SMID = 0x%4x\n", rtlefuse->eeprom_smid));
+	/*customer ID*/
+	rtlefuse->eeprom_oemid = *(u8 *) &hwinfo[EEPROM_CUSTOMER_ID];
+	if (rtlefuse->eeprom_oemid == 0xFF)
+		rtlefuse->eeprom_oemid = 0;
+
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("EEPROM Customer ID: 0x%2x\n", rtlefuse->eeprom_oemid));
+	/*EEPROM version*/
+	rtlefuse->eeprom_version = *(u8 *) &hwinfo[EEPROM_VERSION];
+	/*mac address*/
+	for (i = 0; i < 6; i += 2) {
+		usvalue = *(u16 *) &hwinfo[EEPROM_MAC_ADDR + i];
+		*((u16 *) (&rtlefuse->dev_addr[i])) = usvalue;
+	}
+
+	RT_TRACE(COMP_INIT, DBG_DMESG,
+		 ("dev_addr: %pM\n", rtlefuse->dev_addr));
+	/*channel plan */
+	rtlefuse->eeprom_channelplan = *(u8 *) &hwinfo[EEPROM_CHANNELPLAN];
+	/* set channel paln to world wide 13 */
+	rtlefuse->channel_plan = COUNTRY_CODE_WORLD_WIDE_13;
+	/*tx power*/
+	_rtl92ee_read_txpower_info_from_hwpg(hw, rtlefuse->autoload_failflag,
+					     hwinfo);
+
+	rtl92ee_read_bt_coexist_info_from_hwpg(hw, rtlefuse->autoload_failflag,
+					       hwinfo);
+
+	/*board type*/
+	rtlefuse->board_type = (((*(u8 *) &hwinfo[EEPROM_RF_BOARD_OPTION_92E])
+				& 0xE0) >> 5);
+	if ((*(u8 *) &hwinfo[EEPROM_RF_BOARD_OPTION_92E]) == 0xFF)
+		rtlefuse->board_type = 0;
+
+	rtlhal->boad_type = rtlefuse->board_type;
+	/*parse xtal*/
+	rtlefuse->crystalcap = hwinfo[EEPROM_XTAL_92E];
+	if (hwinfo[EEPROM_XTAL_92E] == 0xFF)
+		rtlefuse->crystalcap = 0x20;
+
+	/*antenna diversity*/
+	rtlefuse->antenna_div_type = NO_ANTDIV;
+	rtlefuse->antenna_div_cfg = 0;
+
+	if (rtlhal->oem_id == RT_CID_DEFAULT) {
+		switch (rtlefuse->eeprom_oemid) {
+		case EEPROM_CID_DEFAULT:
+			if (rtlefuse->eeprom_did == 0x818B) {
+				if ((rtlefuse->eeprom_svid == 0x10EC) &&
+				    (rtlefuse->eeprom_smid == 0x001B))
+					rtlhal->oem_id = RT_CID_819x_Lenovo;
+			} else {
+				rtlhal->oem_id = RT_CID_DEFAULT;
+			}
+			break;
+		default:
+			rtlhal->oem_id = RT_CID_DEFAULT;
+			break;
+		}
+	}
+}
+
+static void _rtl92ee_hal_customized_behavior(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	pcipriv->ledctl.bled_opendrain = true;
+
+	RT_TRACE(COMP_INIT, DBG_DMESG,
+		 ("RT Customized ID: 0x%02X\n", rtlhal->oem_id));
+}
+
+void rtl92ee_read_eeprom_info(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 tmp_u1b;
+
+	rtlhal->version = _rtl92ee_read_chip_version(hw);
+	if (get_rf_type(rtlphy) == RF_1T1R)
+		rtlpriv->dm.brfpath_rxenable[0] = true;
+	else
+		rtlpriv->dm.brfpath_rxenable[0] =
+		    rtlpriv->dm.brfpath_rxenable[1] = true;
+	RT_TRACE(COMP_INIT, DBG_LOUD, ("VersionID = 0x%4x\n",
+						rtlhal->version));
+	tmp_u1b = rtl_read_byte(rtlpriv, REG_9346CR);
+	if (tmp_u1b & BIT(4)) {
+		RT_TRACE(COMP_INIT, DBG_DMESG, ("Boot from EEPROM\n"));
+		rtlefuse->epromtype = EEPROM_93C46;
+	} else {
+		RT_TRACE(COMP_INIT, DBG_DMESG, ("Boot from EFUSE\n"));
+		rtlefuse->epromtype = EEPROM_BOOT_EFUSE;
+	}
+	if (tmp_u1b & BIT(5)) {
+		RT_TRACE(COMP_INIT, DBG_LOUD, ("Autoload OK\n"));
+		rtlefuse->autoload_failflag = false;
+		_rtl92ee_read_adapter_info(hw);
+	} else {
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("Autoload ERR!!\n"));
+	}
+	_rtl92ee_hal_customized_behavior(hw);
+
+	rtlphy->rfpath_rx_enable[0] = true;
+	if (rtlphy->rf_type == RF_2T2R)
+		rtlphy->rfpath_rx_enable[1] = true;
+}
+
+static u8 _rtl92ee_mrate_idx_to_arfr_id(struct ieee80211_hw *hw, u8 rate_index)
+{
+	u8 ret = 0;
+
+	switch (rate_index) {
+	case RATR_INX_WIRELESS_NGB:
+		ret = 0;
+		break;
+	case RATR_INX_WIRELESS_N:
+	case RATR_INX_WIRELESS_NG:
+		ret = 4;
+		break;
+	case RATR_INX_WIRELESS_NB:
+		ret = 2;
+		break;
+	case RATR_INX_WIRELESS_GB:
+		ret = 6;
+		break;
+	case RATR_INX_WIRELESS_G:
+		ret = 7;
+		break;
+	case RATR_INX_WIRELESS_B:
+		ret = 8;
+		break;
+	default:
+		ret = 0;
+		break;
+	}
+	return ret;
+}
+
+static void rtl92ee_update_hal_rate_mask(struct ieee80211_hw *hw,
+					 struct ieee80211_sta *sta,
+					 u8 rssi_level)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_sta_info *sta_entry = NULL;
+	u32 ratr_bitmap;
+	u8 ratr_index;
+	u8 b_curtxbw_40mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40)
+			     ? 1 : 0;
+	u8 b_curshortgi_40mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40) ?
+				1 : 0;
+	u8 b_curshortgi_20mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20) ?
+				1 : 0;
+	enum wireless_mode wirelessmode = 0;
+	bool b_shortgi = false;
+	u8 rate_mask[7] = {0};
+	u8 macid = 0;
+	/*u8 mimo_ps = IEEE80211_SMPS_OFF;*/
+	sta_entry = (struct rtl_sta_info *) sta->drv_priv;
+	wirelessmode = sta_entry->wireless_mode;
+	if (mac->opmode == NL80211_IFTYPE_STATION ||
+	    mac->opmode == NL80211_IFTYPE_MESH_POINT)
+		b_curtxbw_40mhz = mac->bw_40;
+	else if (mac->opmode == NL80211_IFTYPE_AP ||
+		 mac->opmode == NL80211_IFTYPE_ADHOC)
+		macid = sta->aid + 1;
+
+	ratr_bitmap = sta->supp_rates[0];
+	if (mac->opmode == NL80211_IFTYPE_ADHOC)
+		ratr_bitmap = 0xfff;
+
+	ratr_bitmap |= (sta->ht_cap.mcs.rx_mask[1] << 20 |
+			sta->ht_cap.mcs.rx_mask[0] << 12);
+
+	switch (wirelessmode) {
+	case WIRELESS_MODE_B:
+		ratr_index = RATR_INX_WIRELESS_B;
+		if (ratr_bitmap & 0x0000000c)
+			ratr_bitmap &= 0x0000000d;
+		else
+			ratr_bitmap &= 0x0000000f;
+		break;
+	case WIRELESS_MODE_G:
+		ratr_index = RATR_INX_WIRELESS_GB;
+
+		if (rssi_level == 1)
+			ratr_bitmap &= 0x00000f00;
+		else if (rssi_level == 2)
+			ratr_bitmap &= 0x00000ff0;
+		else
+			ratr_bitmap &= 0x00000ff5;
+		break;
+	case WIRELESS_MODE_N_24G:
+		if (b_curtxbw_40mhz)
+			ratr_index = RATR_INX_WIRELESS_NGB;
+		else
+			ratr_index = RATR_INX_WIRELESS_NB;
+
+		if (rtlphy->rf_type == RF_1T1R) {
+			if (b_curtxbw_40mhz) {
+				if (rssi_level == 1)
+					ratr_bitmap &= 0x000f0000;
+				else if (rssi_level == 2)
+					ratr_bitmap &= 0x000ff000;
+				else
+					ratr_bitmap &= 0x000ff015;
+			} else {
+				if (rssi_level == 1)
+					ratr_bitmap &= 0x000f0000;
+				else if (rssi_level == 2)
+					ratr_bitmap &= 0x000ff000;
+				else
+					ratr_bitmap &= 0x000ff005;
+			}
+		} else {
+			if (b_curtxbw_40mhz) {
+				if (rssi_level == 1)
+					ratr_bitmap &= 0x0f8f0000;
+				else if (rssi_level == 2)
+					ratr_bitmap &= 0x0ffff000;
+				else
+					ratr_bitmap &= 0x0ffff015;
+			} else {
+				if (rssi_level == 1)
+					ratr_bitmap &= 0x0f8f0000;
+				else if (rssi_level == 2)
+					ratr_bitmap &= 0x0ffff000;
+				else
+					ratr_bitmap &= 0x0ffff005;
+			}
+		}
+
+		if ((b_curtxbw_40mhz && b_curshortgi_40mhz) ||
+		    (!b_curtxbw_40mhz && b_curshortgi_20mhz)) {
+
+			if (macid == 0)
+				b_shortgi = true;
+			else if (macid == 1)
+				b_shortgi = false;
+		}
+		break;
+	default:
+		ratr_index = RATR_INX_WIRELESS_NGB;
+
+		if (rtlphy->rf_type == RF_1T1R)
+			ratr_bitmap &= 0x000ff0ff;
+		else
+			ratr_bitmap &= 0x0f8ff0ff;
+		break;
+	}
+	ratr_index = _rtl92ee_mrate_idx_to_arfr_id(hw, ratr_index);
+	sta_entry->ratr_index = ratr_index;
+
+	RT_TRACE(COMP_RATR, DBG_DMESG,
+		 ("ratr_bitmap :%x\n", ratr_bitmap));
+	*(u32 *) &rate_mask = (ratr_bitmap & 0x0fffffff) |
+				       (ratr_index << 28);
+	rate_mask[0] = macid;
+	rate_mask[1] = ratr_index | (b_shortgi ? 0x80 : 0x00);
+	rate_mask[2] = b_curtxbw_40mhz;
+	rate_mask[3] = (u8)(ratr_bitmap & 0x000000ff);
+	rate_mask[4] = (u8)((ratr_bitmap & 0x0000ff00) >> 8);
+	rate_mask[5] = (u8)((ratr_bitmap & 0x00ff0000) >> 16);
+	rate_mask[6] = (u8)((ratr_bitmap & 0xff000000) >> 24);
+	RT_TRACE(COMP_RATR, DBG_DMESG,
+		 ("Rate_index:%x, ratr_val:%x, %x:%x:%x:%x:%x:%x:%x\n",
+		  ratr_index, ratr_bitmap, rate_mask[0], rate_mask[1],
+		  rate_mask[2], rate_mask[3], rate_mask[4],
+		  rate_mask[5], rate_mask[6]));
+	rtl92ee_fill_h2c_cmd(hw, H2C_92E_RA_MASK, 7, rate_mask);
+	_rtl92ee_set_bcn_ctrl_reg(hw, BIT(3), 0);
+}
+
+
+void rtl92ee_update_hal_rate_tbl(struct ieee80211_hw *hw,
+				 struct ieee80211_sta *sta, u8 rssi_level)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (rtlpriv->dm.b_useramask)
+		rtl92ee_update_hal_rate_mask(hw, sta, rssi_level);
+}
+
+void rtl92ee_update_channel_access_setting(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u16 sifs_timer;
+
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SLOT_TIME,
+				      (u8 *) &mac->slot_time);
+	if (!mac->ht_enable)
+		sifs_timer = 0x0a0a;
+	else
+		sifs_timer = 0x0e0e;
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SIFS, (u8 *) &sifs_timer);
+}
+
+bool rtl92ee_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 *valid)
+{
+	*valid = 1;
+	return true;
+}
+
+void rtl92ee_set_key(struct ieee80211_hw *hw, u32 key_index,
+		     u8 *p_macaddr, bool is_group, u8 enc_algo,
+		     bool is_wepkey, bool clear_all)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u8 *macaddr = p_macaddr;
+	u32 entry_id = 0;
+	bool is_pairwise = false;
+
+	static u8 cam_const_addr[4][6] = {
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x01},
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x02},
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x03}
+	};
+	static u8 cam_const_broad[] = {
+		0xff, 0xff, 0xff, 0xff, 0xff, 0xff
+	};
+
+	if (clear_all) {
+		u8 idx = 0;
+		u8 cam_offset = 0;
+		u8 clear_number = 5;
+
+		RT_TRACE(COMP_SEC, DBG_DMESG, ("clear_all\n"));
+
+		for (idx = 0; idx < clear_number; idx++) {
+			stg_rtl_cam_mark_invalid(hw, cam_offset + idx);
+			stg_rtl_cam_empty_entry(hw, cam_offset + idx);
+
+			if (idx < 5) {
+				memset(rtlpriv->sec.key_buf[idx], 0,
+				       MAX_KEY_LEN);
+				rtlpriv->sec.key_len[idx] = 0;
+			}
+		}
+
+	} else {
+		switch (enc_algo) {
+		case WEP40_ENCRYPTION:
+			enc_algo = CAM_WEP40;
+			break;
+		case WEP104_ENCRYPTION:
+			enc_algo = CAM_WEP104;
+			break;
+		case TKIP_ENCRYPTION:
+			enc_algo = CAM_TKIP;
+			break;
+		case AESCCMP_ENCRYPTION:
+			enc_algo = CAM_AES;
+			break;
+		default:
+			RT_TRACE(COMP_ERR, DBG_LOUD,
+				 ("switch case not process \n"));
+			enc_algo = CAM_TKIP;
+			break;
+		}
+
+		if (is_wepkey || rtlpriv->sec.use_defaultkey) {
+			macaddr = cam_const_addr[key_index];
+			entry_id = key_index;
+		} else {
+			if (is_group) {
+				macaddr = cam_const_broad;
+				entry_id = key_index;
+			} else {
+				if (mac->opmode == NL80211_IFTYPE_AP ||
+				    mac->opmode == NL80211_IFTYPE_MESH_POINT) {
+					entry_id = stg_rtl_cam_get_free_entry(hw,
+								     p_macaddr);
+					if (entry_id >=  TOTAL_CAM_ENTRY) {
+						RT_TRACE(COMP_SEC, DBG_EMERG,
+							 ("Can not find free hw security cam entry\n"));
+						return;
+					}
+				} else {
+					entry_id = CAM_PAIRWISE_KEY_POSITION;
+				}
+
+				key_index = PAIRWISE_KEYIDX;
+				is_pairwise = true;
+			}
+		}
+
+		if (rtlpriv->sec.key_len[key_index] == 0) {
+			RT_TRACE(COMP_SEC, DBG_DMESG,
+				 ("delete one entry, entry_id is %d\n",
+				 entry_id));
+			if (mac->opmode == NL80211_IFTYPE_AP ||
+			    mac->opmode == NL80211_IFTYPE_MESH_POINT)
+				stg_rtl_cam_del_entry(hw, p_macaddr);
+			stg_rtl_cam_delete_one_entry(hw, p_macaddr, entry_id);
+		} else {
+			RT_TRACE(COMP_SEC, DBG_DMESG, ("add one entry\n"));
+			if (is_pairwise) {
+				RT_TRACE(COMP_SEC, DBG_DMESG,
+					 ("set Pairwiase key\n"));
+
+				stg_rtl_cam_add_one_entry(hw, macaddr, key_index,
+					       entry_id, enc_algo,
+					       CAM_CONFIG_NO_USEDK,
+					       rtlpriv->sec.key_buf[key_index]);
+			} else {
+				RT_TRACE(COMP_SEC, DBG_DMESG,
+					 ("set group key\n"));
+
+				if (mac->opmode == NL80211_IFTYPE_ADHOC) {
+					stg_rtl_cam_add_one_entry(hw,
+						rtlefuse->dev_addr,
+						PAIRWISE_KEYIDX,
+						CAM_PAIRWISE_KEY_POSITION,
+						enc_algo, CAM_CONFIG_NO_USEDK,
+						rtlpriv->sec.key_buf[entry_id]);
+				}
+
+				stg_rtl_cam_add_one_entry(hw, macaddr, key_index,
+						entry_id, enc_algo,
+						CAM_CONFIG_NO_USEDK,
+						rtlpriv->sec.key_buf[entry_id]);
+			}
+		}
+	}
+}
+
+void rtl92ee_read_bt_coexist_info_from_hwpg(struct ieee80211_hw *hw,
+					    bool auto_load_fail, u8 *hwinfo)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 value;
+
+	if (!auto_load_fail) {
+		value = hwinfo[EEPROM_RF_BOARD_OPTION_92E];
+		if (((value & 0xe0) >> 5) == 0x1)
+			rtlpriv->btcoexist.btc_info.btcoexist = 1;
+		else
+			rtlpriv->btcoexist.btc_info.btcoexist = 0;
+
+		rtlpriv->btcoexist.btc_info.bt_type = BT_RTL8192E;
+		rtlpriv->btcoexist.btc_info.ant_num = ANT_TOTAL_X2;
+	} else {
+		rtlpriv->btcoexist.btc_info.btcoexist = 1;
+		rtlpriv->btcoexist.btc_info.bt_type = BT_RTL8192E;
+		rtlpriv->btcoexist.btc_info.ant_num = ANT_TOTAL_X1;
+	}
+}
+
+void rtl92ee_bt_reg_init(struct ieee80211_hw *hw)
+{
+	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+
+	/* 0:Low, 1:High, 2:From Efuse. */
+	rtlpcipriv->btcoexist.b_reg_bt_iso = 2;
+	/* 0:Idle, 1:None-SCO, 2:SCO, 3:From Counter. */
+	rtlpcipriv->btcoexist.b_reg_bt_sco = 3;
+	/* 0:Disable BT control A-MPDU, 1:Enable BT control A-MPDU. */
+	rtlpcipriv->btcoexist.b_reg_bt_sco = 0;
+}
+
+void rtl92ee_bt_hw_init(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (rtlpriv->cfg->ops->get_btc_status()) {
+		rtlpriv->btcoexist.btc_ops->btc_init_hw_config(rtlpriv);
+	}
+}
+
+void rtl92ee_suspend(struct ieee80211_hw *hw)
+{
+}
+
+void rtl92ee_resume(struct ieee80211_hw *hw)
+{
+}
+
+/* Turn on AAP (RCR:bit 0) for promicuous mode. */
+void rtl92ee_allow_all_destaddr(struct ieee80211_hw *hw,
+				bool allow_all_da, bool write_into_reg)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	if (allow_all_da)	/* Set BIT0 */
+		rtlpci->receive_config |= RCR_AAP;
+	else			/* Clear BIT0 */
+		rtlpci->receive_config &= ~RCR_AAP;
+
+	if (write_into_reg)
+		rtl_write_dword(rtlpriv, REG_RCR, rtlpci->receive_config);
+
+	RT_TRACE(COMP_TURBO | COMP_INIT, DBG_LOUD,
+		 ("receive_config = 0x%08X, write_into_reg =%d\n",
+		  rtlpci->receive_config, write_into_reg));
+}
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/hw.h b/drivers/staging/rtl8192ee/rtl8192ee/hw.h
new file mode 100644
index 00000000000..e99d7bde1c6
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/hw.h
@@ -0,0 +1,67 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92E_HW_H__
+#define __RTL92E_HW_H__
+
+
+void rtl92ee_get_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val);
+void rtl92ee_read_eeprom_info(struct ieee80211_hw *hw);
+void rtl92ee_interrupt_recognized(struct ieee80211_hw *hw,
+				  u32 *p_inta, u32 *p_intb);
+int rtl92ee_hw_init(struct ieee80211_hw *hw);
+void rtl92ee_card_disable(struct ieee80211_hw *hw);
+void rtl92ee_enable_interrupt(struct ieee80211_hw *hw);
+void rtl92ee_disable_interrupt(struct ieee80211_hw *hw);
+int rtl92ee_set_network_type(struct ieee80211_hw *hw, enum nl80211_iftype type);
+void rtl92ee_set_check_bssid(struct ieee80211_hw *hw, bool check_bssid);
+void rtl92ee_set_qos(struct ieee80211_hw *hw, int aci);
+void rtl92ee_set_beacon_related_registers(struct ieee80211_hw *hw);
+void rtl92ee_set_beacon_interval(struct ieee80211_hw *hw);
+void rtl92ee_update_interrupt_mask(struct ieee80211_hw *hw,
+				   u32 add_msr, u32 rm_msr);
+void rtl92ee_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val);
+void rtl92ee_update_hal_rate_tbl(struct ieee80211_hw *hw,
+				 struct ieee80211_sta *sta, u8 rssi_level);
+void rtl92ee_update_channel_access_setting(struct ieee80211_hw *hw);
+bool rtl92ee_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 *valid);
+void rtl92ee_enable_hw_security_config(struct ieee80211_hw *hw);
+void rtl92ee_set_key(struct ieee80211_hw *hw, u32 key_index,
+		     u8 *p_macaddr, bool is_group, u8 enc_algo,
+		     bool is_wepkey, bool clear_all);
+void rtl92ee_read_bt_coexist_info_from_hwpg(struct ieee80211_hw *hw,
+					    bool autoload_fail, u8 *hwinfo);
+void rtl92ee_bt_reg_init(struct ieee80211_hw *hw);
+void rtl92ee_bt_hw_init(struct ieee80211_hw *hw);
+void rtl92ee_suspend(struct ieee80211_hw *hw);
+void rtl92ee_resume(struct ieee80211_hw *hw);
+void rtl92ee_allow_all_destaddr(struct ieee80211_hw *hw, bool allow_all_da,
+				bool write_into_reg);
+void rtl92ee_fw_clk_off_timer_callback(unsigned long data);
+#endif
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/led.c b/drivers/staging/rtl8192ee/rtl8192ee/led.c
new file mode 100644
index 00000000000..3b459c93a84
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/led.c
@@ -0,0 +1,134 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../pci.h"
+#include "led.h"
+#include "reg.h"
+
+static void _rtl92ee_init_led(struct ieee80211_hw *hw,
+			      struct rtl_led *pled, enum rtl_led_pin ledpin)
+{
+	pled->hw = hw;
+	pled->ledpin = ledpin;
+	pled->b_ledon = false;
+}
+
+void rtl92ee_sw_led_on(struct ieee80211_hw *hw, struct rtl_led *pled)
+{
+	u32 ledcfg;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	RT_TRACE(COMP_LED, DBG_LOUD,
+		 ("LedAddr:%X ledpin =%d\n", REG_LEDCFG2, pled->ledpin));
+
+	switch (pled->ledpin) {
+	case LED_PIN_GPIO0:
+		break;
+	case LED_PIN_LED0:
+		ledcfg = rtl_read_dword(rtlpriv, REG_GPIO_PIN_CTRL) | BIT(21);
+		ledcfg &= ~BIT(13) & ~BIT(29);
+		rtl_write_dword(rtlpriv, REG_GPIO_PIN_CTRL, ledcfg);
+		break;
+	case LED_PIN_LED1:
+		break;
+	default:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("switch case not process\n"));
+		break;
+	}
+	pled->b_ledon = true;
+}
+
+void rtl92ee_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 ledcfg;
+
+	RT_TRACE(COMP_LED, DBG_LOUD,
+		 ("LedAddr:%X ledpin =%d\n", REG_LEDCFG2, pled->ledpin));
+
+	switch (pled->ledpin) {
+	case LED_PIN_GPIO0:
+		break;
+	case LED_PIN_LED0:
+		ledcfg = rtl_read_dword(rtlpriv , REG_GPIO_PIN_CTRL) | ~BIT(21);
+		ledcfg &= ~BIT(29);
+		rtl_write_dword(rtlpriv, REG_GPIO_PIN_CTRL, ledcfg);
+		break;
+	case LED_PIN_LED1:
+		break;
+	default:
+		RT_TRACE(COMP_ERR, DBG_LOUD,
+			 ("switch case not process\n"));
+		break;
+	}
+	pled->b_ledon = false;
+}
+
+void rtl92ee_init_sw_leds(struct ieee80211_hw *hw)
+{
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	_rtl92ee_init_led(hw, &(pcipriv->ledctl.sw_led0), LED_PIN_LED0);
+	_rtl92ee_init_led(hw, &(pcipriv->ledctl.sw_led1), LED_PIN_LED1);
+}
+
+static void _rtl92ee_sw_led_control(struct ieee80211_hw *hw,
+				    enum led_ctl_mode ledaction)
+{
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_led *pLed0 = &(pcipriv->ledctl.sw_led0);
+	switch (ledaction) {
+	case LED_CTL_POWER_ON:
+	case LED_CTL_LINK:
+	case LED_CTL_NO_LINK:
+		rtl92ee_sw_led_on(hw, pLed0);
+		break;
+	case LED_CTL_POWER_OFF:
+		rtl92ee_sw_led_off(hw, pLed0);
+		break;
+	default:
+		break;
+	}
+}
+
+void rtl92ee_led_control(struct ieee80211_hw *hw, enum led_ctl_mode ledaction)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+
+	if ((ppsc->rfoff_reason > RF_CHANGE_BY_PS) &&
+	    (ledaction == LED_CTL_TX ||
+	     ledaction == LED_CTL_RX ||
+	     ledaction == LED_CTL_SITE_SURVEY ||
+	     ledaction == LED_CTL_LINK ||
+	     ledaction == LED_CTL_NO_LINK ||
+	     ledaction == LED_CTL_START_TO_LINK ||
+	     ledaction == LED_CTL_POWER_ON)) {
+		return;
+	}
+	RT_TRACE(COMP_LED, DBG_TRACE, ("ledaction %d,\n", ledaction));
+	_rtl92ee_sw_led_control(hw, ledaction);
+}
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/led.h b/drivers/staging/rtl8192ee/rtl8192ee/led.h
new file mode 100644
index 00000000000..7302eda535d
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/led.h
@@ -0,0 +1,37 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92E_LED_H__
+#define __RTL92E_LED_H__
+
+void rtl92ee_init_sw_leds(struct ieee80211_hw *hw);
+void rtl92ee_sw_led_on(struct ieee80211_hw *hw, struct rtl_led *pled);
+void rtl92ee_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled);
+void rtl92ee_led_control(struct ieee80211_hw *hw, enum led_ctl_mode ledaction);
+#endif
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/phy.c b/drivers/staging/rtl8192ee/rtl8192ee/phy.c
new file mode 100644
index 00000000000..beef284615e
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/phy.c
@@ -0,0 +1,3282 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../pci.h"
+#include "../ps.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "rf.h"
+#include "dm.h"
+#include "table.h"
+
+static u32 _rtl92ee_phy_rf_serial_read(struct ieee80211_hw *hw,
+				       enum radio_path rfpath, u32 offset);
+static void _rtl92ee_phy_rf_serial_write(struct ieee80211_hw *hw,
+					 enum radio_path rfpath, u32 offset,
+					 u32 data);
+static u32 _rtl92ee_phy_calculate_bit_shift(u32 bitmask);
+static bool _rtl92ee_phy_bb8192ee_config_parafile(struct ieee80211_hw *hw);
+static bool _rtl92ee_phy_config_mac_with_headerfile(struct ieee80211_hw *hw);
+static bool _rtl92ee_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
+						   u8 configtype);
+static bool _rtl92ee_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
+						     u8 configtype);
+static void _rtl92ee_phy_init_bb_rf_register_definition(struct ieee80211_hw *hw);
+static bool _rtl92ee_phy_set_sw_chnl_cmdarray(struct swchnlcmd *cmdtable,
+					      u32 cmdtableidx, u32 cmdtablesz,
+					      enum swchnlcmd_id cmdid,
+					      u32 para1, u32 para2,
+					      u32 msdelay);
+static bool _rtl92ee_phy_sw_chnl_step_by_step(struct ieee80211_hw *hw,
+					      u8 channel, u8 *stage,
+					      u8 *step, u32 *delay);
+static long _rtl92ee_phy_txpwr_idx_to_dbm(struct ieee80211_hw *hw,
+					  enum wireless_mode wirelessmode,
+					  u8 txpwridx);
+static void rtl92ee_phy_set_rf_on(struct ieee80211_hw *hw);
+static void rtl92ee_phy_set_io(struct ieee80211_hw *hw);
+
+u32 rtl92ee_phy_query_bb_reg(struct ieee80211_hw *hw, u32 regaddr, u32 bitmask)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 returnvalue, originalvalue, bitshift;
+
+	RT_TRACE(COMP_RF, DBG_TRACE,
+		 ("regaddr(%#x), bitmask(%#x)\n", regaddr, bitmask));
+	originalvalue = rtl_read_dword(rtlpriv, regaddr);
+	bitshift = _rtl92ee_phy_calculate_bit_shift(bitmask);
+	returnvalue = (originalvalue & bitmask) >> bitshift;
+
+	RT_TRACE(COMP_RF, DBG_TRACE,
+		 ("BBR MASK = 0x%x Addr[0x%x]= 0x%x\n",
+		  bitmask, regaddr, originalvalue));
+
+	return returnvalue;
+}
+
+void rtl92ee_phy_set_bb_reg(struct ieee80211_hw *hw, u32 regaddr,
+			    u32 bitmask, u32 data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 originalvalue, bitshift;
+
+	RT_TRACE(COMP_RF, DBG_TRACE,
+		 ("regaddr(%#x), bitmask(%#x), data(%#x)\n",
+		  regaddr, bitmask, data));
+
+	if (bitmask != MASKDWORD) {
+		originalvalue = rtl_read_dword(rtlpriv, regaddr);
+		bitshift = _rtl92ee_phy_calculate_bit_shift(bitmask);
+		data = ((originalvalue & (~bitmask)) | (data << bitshift));
+	}
+
+	rtl_write_dword(rtlpriv, regaddr, data);
+
+	RT_TRACE(COMP_RF, DBG_TRACE,
+		 ("regaddr(%#x), bitmask(%#x), data(%#x)\n",
+		  regaddr, bitmask, data));
+}
+
+u32 rtl92ee_phy_query_rf_reg(struct ieee80211_hw *hw,
+			     enum radio_path rfpath, u32 regaddr, u32 bitmask)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 original_value, readback_value, bitshift;
+	unsigned long flags;
+
+	RT_TRACE(COMP_RF, DBG_TRACE,
+		 ("regaddr(%#x), rfpath(%#x), bitmask(%#x)\n",
+		  regaddr, rfpath, bitmask));
+
+	spin_lock_irqsave(&rtlpriv->locks.rf_lock, flags);
+
+
+	original_value = _rtl92ee_phy_rf_serial_read(hw , rfpath, regaddr);
+	bitshift = _rtl92ee_phy_calculate_bit_shift(bitmask);
+	readback_value = (original_value & bitmask) >> bitshift;
+
+	spin_unlock_irqrestore(&rtlpriv->locks.rf_lock, flags);
+
+	RT_TRACE(COMP_RF, DBG_TRACE,
+		 ("regaddr(%#x), rfpath(%#x), bitmask(%#x), original_value(%#x)\n",
+		  regaddr, rfpath, bitmask, original_value));
+
+	return readback_value;
+}
+
+void rtl92ee_phy_set_rf_reg(struct ieee80211_hw *hw,
+			    enum radio_path rfpath,
+			    u32 addr, u32 bitmask, u32 data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 original_value, bitshift;
+	unsigned long flags;
+
+	RT_TRACE(COMP_RF, DBG_TRACE,
+		 ("regaddr(%#x), bitmask(%#x), data(%#x), rfpath(%#x)\n",
+		  addr, bitmask, data, rfpath));
+
+	spin_lock_irqsave(&rtlpriv->locks.rf_lock, flags);
+
+	if (bitmask != RFREG_OFFSET_MASK) {
+		original_value = _rtl92ee_phy_rf_serial_read(hw, rfpath, addr);
+		bitshift = _rtl92ee_phy_calculate_bit_shift(bitmask);
+		data = (original_value & (~bitmask)) | (data << bitshift);
+	}
+
+	_rtl92ee_phy_rf_serial_write(hw, rfpath, addr, data);
+
+	spin_unlock_irqrestore(&rtlpriv->locks.rf_lock, flags);
+
+	RT_TRACE(COMP_RF, DBG_TRACE,
+		 ("regaddr(%#x), bitmask(%#x), data(%#x), rfpath(%#x)\n",
+		  addr, bitmask, data, rfpath));
+}
+
+static u32 _rtl92ee_phy_rf_serial_read(struct ieee80211_hw *hw,
+				       enum radio_path rfpath, u32 offset)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct bb_reg_def *pphyreg = &rtlphy->phyreg_def[rfpath];
+	u32 newoffset;
+	u32 tmplong, tmplong2;
+	u8 rfpi_enable = 0;
+	u32 retvalue;
+
+	offset &= 0xff;
+	newoffset = offset;
+	if (RT_CANNOT_IO(hw)) {
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("return all one\n"));
+		return 0xFFFFFFFF;
+	}
+	tmplong = rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD);
+	if (rfpath == RF90_PATH_A)
+		tmplong2 = tmplong;
+	else
+		tmplong2 = rtl_get_bbreg(hw, pphyreg->rfhssi_para2, MASKDWORD);
+	tmplong2 = (tmplong2 & (~BLSSIREADADDRESS)) |
+		   (newoffset << 23) | BLSSIREADEDGE;
+	rtl_set_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD,
+		      tmplong & (~BLSSIREADEDGE));
+	mdelay(1);
+	rtl_set_bbreg(hw, pphyreg->rfhssi_para2, MASKDWORD, tmplong2);
+	mdelay(1);
+	mdelay(1);
+	if (rfpath == RF90_PATH_A)
+		rfpi_enable = (u8) rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER1,
+						 BIT(8));
+	else if (rfpath == RF90_PATH_B)
+		rfpi_enable = (u8) rtl_get_bbreg(hw, RFPGA0_XB_HSSIPARAMETER1,
+						 BIT(8));
+	if (rfpi_enable)
+		retvalue = rtl_get_bbreg(hw, pphyreg->rflssi_readbackpi,
+					 BLSSIREADBACKDATA);
+	else
+		retvalue = rtl_get_bbreg(hw, pphyreg->rflssi_readback,
+					 BLSSIREADBACKDATA);
+	RT_TRACE(COMP_RF, DBG_TRACE,
+		 ("RFR-%d Addr[0x%x]= 0x%x\n",
+		  rfpath, pphyreg->rflssi_readback, retvalue));
+	return retvalue;
+}
+
+static void _rtl92ee_phy_rf_serial_write(struct ieee80211_hw *hw,
+					 enum radio_path rfpath, u32 offset,
+					 u32 data)
+{
+	u32 data_and_addr;
+	u32 newoffset;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct bb_reg_def *pphyreg = &rtlphy->phyreg_def[rfpath];
+
+	if (RT_CANNOT_IO(hw)) {
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("stop\n"));
+		return;
+	}
+	offset &= 0xff;
+	newoffset = offset;
+	data_and_addr = ((newoffset << 20) | (data & 0x000fffff)) & 0x0fffffff;
+	rtl_set_bbreg(hw, pphyreg->rf3wire_offset, MASKDWORD, data_and_addr);
+	RT_TRACE(COMP_RF, DBG_TRACE,
+		 ("RFW-%d Addr[0x%x]= 0x%x\n", rfpath,
+		  pphyreg->rf3wire_offset, data_and_addr));
+}
+
+static u32 _rtl92ee_phy_calculate_bit_shift(u32 bitmask)
+{
+	u32 i;
+
+	for (i = 0; i <= 31; i++) {
+		if (((bitmask >> i) & 0x1) == 1)
+			break;
+	}
+	return i;
+}
+
+bool rtl92ee_phy_mac_config(struct ieee80211_hw *hw)
+{
+	bool rtstatus = _rtl92ee_phy_config_mac_with_headerfile(hw);
+
+	return rtstatus;
+}
+
+bool rtl92ee_phy_bb_config(struct ieee80211_hw *hw)
+{
+	bool rtstatus = true;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u16 regval;
+	u32 tmp;
+	u8 crystal_cap;
+
+	_rtl92ee_phy_init_bb_rf_register_definition(hw);
+	regval = rtl_read_word(rtlpriv, REG_SYS_FUNC_EN);
+	rtl_write_word(rtlpriv, REG_SYS_FUNC_EN,
+		       regval | BIT(13) | BIT(0) | BIT(1));
+
+	rtl_write_byte(rtlpriv, REG_RF_CTRL, RF_EN | RF_RSTB | RF_SDMRSTB);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN,
+		       FEN_PPLL | FEN_PCIEA | FEN_DIO_PCIE |
+		       FEN_BB_GLB_RSTn | FEN_BBRSTB);
+
+	rtl_write_byte(rtlpriv, REG_AFE_XTAL_CTRL + 1, 0x80);
+
+	tmp = rtl_read_dword(rtlpriv, 0x4c);
+	rtl_write_dword(rtlpriv, 0x4c, tmp | BIT(23));
+
+	rtstatus = _rtl92ee_phy_bb8192ee_config_parafile(hw);
+
+	crystal_cap = rtlpriv->efuse.eeprom_crystalcap & 0x3F;
+	rtl_set_bbreg(hw, REG_MAC_PHY_CTRL, 0xFFF000,
+		      (crystal_cap | (crystal_cap << 6)));
+	return rtstatus;
+}
+
+bool rtl92ee_phy_rf_config(struct ieee80211_hw *hw)
+{
+	return rtl92ee_phy_rf6052_config(hw);
+}
+
+static bool _check_condition(struct ieee80211_hw *hw,
+				     const u32  condition)
+{
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u32 _board = rtlefuse->board_type; /*need efuse define*/
+	u32 _interface = rtlhal->interface;
+	u32 _platform = 0x08;/*SupportPlatform */
+	u32 cond = condition;
+
+	if (condition == 0xCDCDCDCD)
+		return true;
+	cond = condition & 0xFF;
+	if ((_board != cond) && (cond != 0xFF))
+		return false;
+	cond = condition & 0xFF00;
+	cond = cond >> 8;
+	if ((_interface & cond) == 0 && cond != 0x07)
+		return false;
+	cond = condition & 0xFF0000;
+	cond = cond >> 16;
+	if ((_platform & cond) == 0 && cond != 0x0F)
+		return false;
+	return true;
+}
+
+static void _rtl92ee_config_rf_reg(struct ieee80211_hw *hw, u32 addr, u32 data,
+				   enum radio_path rfpath, u32 regaddr)
+{
+	if (addr == 0xfe || addr == 0xffe) {
+		mdelay(50);
+	} else {
+		rtl_set_rfreg(hw, rfpath, regaddr, RFREG_OFFSET_MASK, data);
+		udelay(1);
+
+		if (addr == 0xb6) {
+			u32 getvalue;
+			u8 count = 0;
+			getvalue = rtl_get_rfreg(hw, rfpath, addr, MASKDWORD);
+			udelay(1);
+
+			while ((getvalue >> 8) != (data >> 8)) {
+				count++;
+				rtl_set_rfreg(hw, rfpath, regaddr,
+					      RFREG_OFFSET_MASK, data);
+				udelay(1);
+				getvalue = rtl_get_rfreg(hw, rfpath, addr,
+							 MASKDWORD);
+				if (count > 5)
+					break;
+			}
+		}
+
+		if (addr == 0xb2) {
+			u32 getvalue;
+			u8 count = 0;
+			getvalue = rtl_get_rfreg(hw, rfpath, addr, MASKDWORD);
+
+			udelay(1);
+
+			while (getvalue != data) {
+				count++;
+				rtl_set_rfreg(hw, rfpath, regaddr,
+					      RFREG_OFFSET_MASK, data);
+				udelay(1);
+				rtl_set_rfreg(hw, rfpath, 0x18,
+					      RFREG_OFFSET_MASK, 0x0fc07);
+				udelay(1);
+				getvalue = rtl_get_rfreg(hw, rfpath, addr,
+							 MASKDWORD);
+				if (count > 5)
+					break;
+			}
+		}
+	}
+}
+
+static void _rtl92ee_config_rf_radio_a(struct ieee80211_hw *hw,
+				       u32 addr, u32 data)
+{
+	u32 content = 0x1000; /*RF Content: radio_a_txt*/
+	u32 maskforphyset = (u32)(content & 0xE000);
+
+	_rtl92ee_config_rf_reg(hw, addr, data, RF90_PATH_A,
+			       addr | maskforphyset);
+}
+
+static void _rtl92ee_config_rf_radio_b(struct ieee80211_hw *hw,
+				       u32 addr, u32 data)
+{
+	u32 content = 0x1001; /*RF Content: radio_b_txt*/
+	u32 maskforphyset = (u32)(content & 0xE000);
+
+	_rtl92ee_config_rf_reg(hw, addr, data, RF90_PATH_B,
+			       addr | maskforphyset);
+}
+
+static void _rtl92ee_config_bb_reg(struct ieee80211_hw *hw,
+				   u32 addr, u32 data)
+{
+	if (addr == 0xfe)
+		mdelay(50);
+	else if (addr == 0xfd)
+		mdelay(5);
+	else if (addr == 0xfc)
+		mdelay(1);
+	else if (addr == 0xfb)
+		udelay(50);
+	else if (addr == 0xfa)
+		udelay(5);
+	else if (addr == 0xf9)
+		udelay(1);
+	else
+		rtl_set_bbreg(hw, addr, MASKDWORD , data);
+
+	udelay(1);
+}
+
+static void _rtl92ee_phy_init_tx_power_by_rate(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	u8 band = BAND_ON_2_4G, rf = 0, txnum = 0, sec = 0;
+
+	for (; band <= BAND_ON_5G; ++band)
+		for (; rf < TX_PWR_BY_RATE_NUM_RF; ++rf)
+			for (; txnum < TX_PWR_BY_RATE_NUM_RF; ++txnum)
+				for (; sec < TX_PWR_BY_RATE_NUM_SECTION; ++sec)
+					rtlphy->tx_power_by_rate_offset
+						     [band][rf][txnum][sec] = 0;
+}
+
+static void _rtl92ee_phy_set_txpower_by_rate_base(struct ieee80211_hw *hw,
+						  u8 band, u8 path,
+						  u8 rate_section, u8 txnum, u8 value)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	if (path > RF90_PATH_D) {
+		RT_TRACE(COMP_INIT, DBG_LOUD, ("Invalid Rf Path %d\n", path));
+		return;
+	}
+	if (band == BAND_ON_2_4G) {
+		switch (rate_section) {
+		case CCK:
+			rtlphy->txpwr_by_rate_base_24g[path][txnum][0] = value;
+			break;
+		case OFDM:
+			rtlphy->txpwr_by_rate_base_24g[path][txnum][1] = value;
+			break;
+		case HT_MCS0_MCS7:
+			rtlphy->txpwr_by_rate_base_24g[path][txnum][2] = value;
+			break;
+		case HT_MCS8_MCS15:
+			rtlphy->txpwr_by_rate_base_24g[path][txnum][3] = value;
+			break;
+		default:
+			RT_TRACE(COMP_INIT, DBG_LOUD,
+				 ("Invalid RateSection %d in 2.4G, Rf %d,%dTx\n",
+				  rate_section, path, txnum));
+			break;
+		};
+	} else {
+		RT_TRACE(COMP_INIT, DBG_LOUD, ("Invalid Band %d\n", band));
+	}
+}
+
+static u8 _rtl92ee_phy_get_txpower_by_rate_base(struct ieee80211_hw *hw, u8 band,
+						u8 path, u8 txnum, u8 rate_section)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 value = 0;
+	if (path > RF90_PATH_D) {
+		RT_TRACE(COMP_INIT, DBG_LOUD, ("Invalid Rf Path %d\n", path));
+		return 0;
+	}
+	if (band == BAND_ON_2_4G) {
+		switch (rate_section) {
+		case CCK:
+			value = rtlphy->txpwr_by_rate_base_24g[path][txnum][0];
+			break;
+		case OFDM:
+			value = rtlphy->txpwr_by_rate_base_24g[path][txnum][1];
+			break;
+		case HT_MCS0_MCS7:
+			value = rtlphy->txpwr_by_rate_base_24g[path][txnum][2];
+			break;
+		case HT_MCS8_MCS15:
+			value = rtlphy->txpwr_by_rate_base_24g[path][txnum][3];
+			break;
+		default:
+			RT_TRACE(COMP_INIT, DBG_LOUD,
+				 ("Invalid RateSection %d in 2.4G, Rf %d,%dTx\n",
+				  rate_section, path, txnum));
+			break;
+		};
+	} else {
+		RT_TRACE(COMP_INIT, DBG_LOUD, ("Invalid Band %d()\n", band));
+	}
+	return value;
+}
+
+static void _rtl92ee_phy_store_txpower_by_rate_base(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u16 raw = 0;
+	u8 base = 0, path = 0;
+
+	for (path = RF90_PATH_A; path <= RF90_PATH_B; ++path) {
+		if (path == RF90_PATH_A) {
+			raw = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][path][RF_1TX][3] >> 24) & 0xFF;
+			base = (raw >> 4) * 10 + (raw & 0xF);
+			_rtl92ee_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G,
+							      path, CCK, RF_1TX,
+							      base);
+		} else if (path == RF90_PATH_B) {
+			raw = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][path][RF_1TX][3] >> 0) & 0xFF;
+			base = (raw >> 4) * 10 + (raw & 0xF);
+			_rtl92ee_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G,
+							      path, CCK, RF_1TX,
+							      base);
+		}
+		raw = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][path][RF_1TX][1] >> 24) & 0xFF;
+		base = (raw >> 4) * 10 + (raw & 0xF);
+		_rtl92ee_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G, path,
+						      OFDM, RF_1TX, base);
+
+		raw = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][path][RF_1TX][5] >> 24) & 0xFF;
+		base = (raw >> 4) * 10 + (raw & 0xF);
+		_rtl92ee_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G, path,
+						      HT_MCS0_MCS7, RF_1TX,
+						      base);
+
+		raw = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][path][RF_2TX][7] >> 24) & 0xFF;
+		base = (raw >> 4) * 10 + (raw & 0xF);
+		_rtl92ee_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G, path,
+						      HT_MCS8_MCS15, RF_2TX,
+						      base);
+	}
+}
+
+static void _phy_convert_txpower_dbm_to_relative_value(u32 *data, u8 start,
+						       u8 end, u8 base)
+{
+	char i = 0;
+	u8 tmp = 0;
+	u32 temp_data = 0;
+
+	for (i = 3; i >= 0; --i) {
+		if (i >= start && i <= end) {
+			/* Get the exact value */
+			tmp = (u8) (*data >> (i * 8)) & 0xF;
+			tmp += ((u8) ((*data >> (i * 8 + 4)) & 0xF)) * 10;
+
+			/* Change the value to a relative value */
+			tmp = (tmp > base) ? tmp - base : base - tmp;
+		} else {
+			tmp = (u8) (*data >> (i * 8)) & 0xFF;
+		}
+		temp_data <<= 8;
+		temp_data |= tmp;
+	}
+	*data = temp_data;
+}
+
+static void _rtl92ee_phy_convert_txpower_dbm_to_relative_value(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 base = 0, rf = 0, band = BAND_ON_2_4G;
+
+	for (rf = RF90_PATH_A; rf <= RF90_PATH_B; ++rf) {
+		if (rf == RF90_PATH_A) {
+			base = _rtl92ee_phy_get_txpower_by_rate_base(hw, band,
+								     rf, RF_1TX,
+								     CCK);
+			_phy_convert_txpower_dbm_to_relative_value(
+				&(rtlphy->tx_power_by_rate_offset[band][rf][RF_1TX][2]),
+				1, 1, base);
+			_phy_convert_txpower_dbm_to_relative_value(
+				&(rtlphy->tx_power_by_rate_offset[band][rf][RF_1TX][3]),
+				1, 3, base);
+		} else if (rf == RF90_PATH_B) {
+			base = _rtl92ee_phy_get_txpower_by_rate_base(hw, band,
+								     rf, RF_1TX,
+								     CCK);
+			_phy_convert_txpower_dbm_to_relative_value(
+				&(rtlphy->tx_power_by_rate_offset[band][rf][RF_1TX][3]),
+				0, 0, base);
+			_phy_convert_txpower_dbm_to_relative_value(
+				&(rtlphy->tx_power_by_rate_offset[band][rf][RF_1TX][2]),
+				1, 3, base);
+		}
+		base = _rtl92ee_phy_get_txpower_by_rate_base(hw, band, rf,
+							     RF_1TX, OFDM);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[band][rf][RF_1TX][0]),
+			0, 3, base);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[band][rf][RF_1TX][1]),
+			0, 3, base);
+
+		base = _rtl92ee_phy_get_txpower_by_rate_base(hw, band, rf,
+							     RF_1TX,
+							     HT_MCS0_MCS7);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[band][rf][RF_1TX][4]),
+			0, 3, base);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[band][rf][RF_1TX][5]),
+			0, 3, base);
+		base = _rtl92ee_phy_get_txpower_by_rate_base(hw, band, rf,
+							     RF_2TX,
+							     HT_MCS8_MCS15);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[band][rf][RF_2TX][6]),
+			0, 3, base);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&(rtlphy->tx_power_by_rate_offset[band][rf][RF_2TX][7]),
+			0, 3, base);
+	}
+	RT_TRACE(COMP_POWER, DBG_TRACE,
+		 ("<== _rtl92ee_phy_convert_txpower_dbm_to_relative_value()\n"));
+}
+
+static void _rtl92ee_phy_txpower_by_rate_configuration(struct ieee80211_hw *hw)
+{
+	_rtl92ee_phy_store_txpower_by_rate_base(hw);
+	_rtl92ee_phy_convert_txpower_dbm_to_relative_value(hw);
+}
+
+static bool _rtl92ee_phy_bb8192ee_config_parafile(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	bool rtstatus;
+
+	rtstatus = _rtl92ee_phy_config_bb_with_headerfile(hw,
+						       BASEBAND_CONFIG_PHY_REG);
+	if (!rtstatus) {
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("Write BB Reg Fail!!"));
+		return false;
+	}
+
+	_rtl92ee_phy_init_tx_power_by_rate(hw);
+	if (rtlefuse->autoload_failflag == false) {
+		rtlphy->pwrgroup_cnt = 0;
+		rtstatus = _rtl92ee_phy_config_bb_with_pgheaderfile(hw,
+						       BASEBAND_CONFIG_PHY_REG);
+	}
+	_rtl92ee_phy_txpower_by_rate_configuration(hw);
+	if (!rtstatus) {
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("BB_PG Reg Fail!!"));
+		return false;
+	}
+	rtstatus = _rtl92ee_phy_config_bb_with_headerfile(hw,
+						       BASEBAND_CONFIG_AGC_TAB);
+	if (!rtstatus) {
+		RT_TRACE(COMP_ERR, DBG_EMERG, ("AGC Table Fail\n"));
+		return false;
+	}
+	rtlphy->bcck_high_power = (bool) (rtl_get_bbreg(hw,
+						       RFPGA0_XA_HSSIPARAMETER2,
+						       0x200));
+
+	return true;
+}
+
+static bool _rtl92ee_phy_config_mac_with_headerfile(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i;
+	u32 arraylength;
+	u32 *ptrarray;
+
+	RT_TRACE(COMP_INIT, DBG_TRACE, ("Read Rtl8192EMACPHY_Array\n"));
+	arraylength = RTL8192EE_MAC_ARRAY_LEN;
+	ptrarray = RTL8192EE_MAC_ARRAY;
+	RT_TRACE(COMP_INIT, DBG_LOUD,
+		 ("Img:RTL8192EE_MAC_ARRAY LEN %d\n" , arraylength));
+	for (i = 0; i < arraylength; i = i + 2)
+		rtl_write_byte(rtlpriv, ptrarray[i], (u8) ptrarray[i + 1]);
+	return true;
+}
+
+static bool _rtl92ee_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
+						   u8 configtype)
+{
+	#define READ_NEXT_PAIR(v1, v2, i) \
+		do { \
+			i += 2; \
+			v1 = array[i]; \
+			v2 = array[i+1]; \
+		} while (0)
+
+	int i;
+	u32 *array;
+	u16 len;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 v1 = 0, v2 = 0;
+
+	if (configtype == BASEBAND_CONFIG_PHY_REG) {
+		len = RTL8192EE_PHY_REG_ARRAY_LEN;
+		array = RTL8192EE_PHY_REG_ARRAY;
+
+		for (i = 0; i < len; i = i + 2) {
+			v1 = array[i];
+			v2 = array[i+1];
+			if (v1 < 0xcdcdcdcd) {
+				_rtl92ee_config_bb_reg(hw, v1, v2);
+			} else {/*This line is the start line of branch.*/
+				/* to protect READ_NEXT_PAIR not overrun */
+				if (i >= len - 2)
+					break;
+
+				if (!_check_condition(hw , array[i])) {
+					/*Discard the following pairs*/
+					READ_NEXT_PAIR(v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD && i < len - 2)
+					    READ_NEXT_PAIR(v1, v2, i);
+					i -= 2; /* prevent from for-loop += 2*/
+				} else{/* Configure matched pairs and
+				       * skip to end of if-else. */
+					READ_NEXT_PAIR(v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD && i < len - 2) {
+						_rtl92ee_config_bb_reg(hw, v1,
+								       v2);
+						READ_NEXT_PAIR(v1, v2, i);
+					}
+
+					while (v2 != 0xDEAD && i < len - 2)
+						READ_NEXT_PAIR(v1, v2, i);
+				}
+			}
+		}
+	} else if (configtype == BASEBAND_CONFIG_AGC_TAB) {
+			len = RTL8192EE_AGC_TAB_ARRAY_LEN;
+			array = RTL8192EE_AGC_TAB_ARRAY;
+
+			for (i = 0; i < len; i = i + 2) {
+				v1 = array[i];
+				v2 = array[i+1];
+				if (v1 < 0xCDCDCDCD) {
+					rtl_set_bbreg(hw, array[i], MASKDWORD,
+						      array[i + 1]);
+					udelay(1);
+					continue;
+			    } else{/*This line is the start line of branch.*/
+				  /* to protect READ_NEXT_PAIR not overrun */
+					if (i >= len - 2)
+						break;
+
+					if (!_check_condition(hw , array[i])) {
+						/*Discard the following pairs*/
+						READ_NEXT_PAIR(v1, v2, i);
+						while (v2 != 0xDEAD &&
+						       v2 != 0xCDEF &&
+						       v2 != 0xCDCD &&
+						       i < len - 2)
+						    READ_NEXT_PAIR(v1, v2, i);
+						i -= 2; /* prevent from for-loop += 2*/
+					} else {/* Configure matched pairs and
+					       * skip to end of if-else.*/
+						READ_NEXT_PAIR(v1, v2, i);
+						while (v2 != 0xDEAD &&
+						       v2 != 0xCDEF &&
+						       v2 != 0xCDCD &&
+						       i < len - 2) {
+							rtl_set_bbreg(hw,
+								      array[i],
+								      MASKDWORD,
+								      array[i + 1]);
+							udelay(1);
+							READ_NEXT_PAIR(v1 , v2 , i);
+						}
+
+						while (v2 != 0xDEAD &&
+						       i < len - 2)
+							READ_NEXT_PAIR(v1 , v2 , i);
+					}
+				}
+				RT_TRACE(COMP_INIT, DBG_TRACE,
+					 ("The agctab_array_table[0] is %x Rtl818EEPHY_REGArray[1] is %x\n",
+					  array[i],
+					  array[i + 1]));
+		}
+	}
+	return true;
+}
+
+static u8 _rtl92ee_get_rate_section_index(u32 regaddr)
+{
+	u8 index = 0;
+
+	switch (regaddr) {
+	case RTXAGC_A_RATE18_06:
+		index = 0;
+		break;
+	case RTXAGC_A_RATE54_24:
+		index = 1;
+		break;
+	case RTXAGC_A_CCK1_MCS32:
+		index = 2;
+		break;
+	case RTXAGC_B_CCK11_A_CCK2_11:
+		index = 3;
+		break;
+	case RTXAGC_A_MCS03_MCS00:
+		index = 4;
+		break;
+	case RTXAGC_A_MCS07_MCS04:
+		index = 5;
+		break;
+	case RTXAGC_A_MCS11_MCS08:
+		index = 6;
+		break;
+	case RTXAGC_A_MCS15_MCS12:
+		index = 7;
+		break;
+	case RTXAGC_B_RATE18_06:
+		index = 0;
+		break;
+	case RTXAGC_B_RATE54_24:
+		index = 1;
+		break;
+	case RTXAGC_B_CCK1_55_MCS32:
+		index = 2;
+		break;
+	case RTXAGC_B_MCS03_MCS00:
+		index = 4;
+		break;
+	case RTXAGC_B_MCS07_MCS04:
+		index = 5;
+		break;
+	case RTXAGC_B_MCS11_MCS08:
+		index = 6;
+		break;
+	case RTXAGC_B_MCS15_MCS12:
+		index = 7;
+		break;
+	default:
+		regaddr &= 0xFFF;
+		if (regaddr >= 0xC20 && regaddr <= 0xC4C)
+			index = (u8) ((regaddr - 0xC20) / 4);
+		else if (regaddr >= 0xE20 && regaddr <= 0xE4C)
+			index = (u8) ((regaddr - 0xE20) / 4);
+		break;
+	};
+	return index;
+}
+
+static void _rtl92ee_store_tx_power_by_rate(struct ieee80211_hw *hw,
+					    enum band_type band,
+					    enum radio_path rfpath,
+					    u32 txnum, u32 regaddr,
+					    u32 bitmask, u32 data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 section = _rtl92ee_get_rate_section_index(regaddr);
+
+	if (band != BAND_ON_2_4G && band != BAND_ON_5G) {
+		RT_TRACE(FPHY, PHY_TXPWR, ("Invalid Band %d\n", band));
+		return;
+	}
+
+	if (rfpath > MAX_RF_PATH - 1) {
+		RT_TRACE(FPHY, PHY_TXPWR, ("Invalid RfPath %d\n", rfpath));
+		return;
+	}
+	if (txnum > MAX_RF_PATH - 1) {
+		RT_TRACE(FPHY, PHY_TXPWR, ("Invalid TxNum %d\n", txnum));
+		return;
+	}
+
+	rtlphy->tx_power_by_rate_offset[band][rfpath][txnum][section] = data;
+}
+
+static bool _rtl92ee_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
+						     u8 configtype)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int i;
+	u32 *phy_regarray_table_pg;
+	u16 phy_regarray_pg_len;
+	u32 v1 = 0, v2 = 0, v3 = 0, v4 = 0, v5 = 0, v6 = 0;
+
+	phy_regarray_pg_len = RTL8192EE_PHY_REG_ARRAY_PG_LEN;
+	phy_regarray_table_pg = RTL8192EE_PHY_REG_ARRAY_PG;
+
+	if (configtype == BASEBAND_CONFIG_PHY_REG) {
+		for (i = 0; i < phy_regarray_pg_len; i = i + 6) {
+			v1 = phy_regarray_table_pg[i];
+			v2 = phy_regarray_table_pg[i+1];
+			v3 = phy_regarray_table_pg[i+2];
+			v4 = phy_regarray_table_pg[i+3];
+			v5 = phy_regarray_table_pg[i+4];
+			v6 = phy_regarray_table_pg[i+5];
+
+			if (v1 < 0xcdcdcdcd) {
+				_rtl92ee_store_tx_power_by_rate(hw, v1, v2, v3,
+								v4, v5, v6);
+				continue;
+			}
+		}
+	} else {
+		RT_TRACE(COMP_SEND, DBG_TRACE,
+			 ("configtype != BaseBand_Config_PHY_REG\n"));
+	}
+	return true;
+}
+
+bool rtl92ee_phy_config_rf_with_headerfile(struct ieee80211_hw  *hw,
+					   enum radio_path rfpath)
+{
+	#define READ_NEXT_RF_PAIR(v1, v2, i) \
+		do { \
+			i += 2; \
+			v1 = array[i]; \
+			v2 = array[i+1]; \
+		} while (0)
+
+	int i;
+	u32 *array;
+	u16 len;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 v1 = 0, v2 = 0;
+
+	switch (rfpath) {
+	case RF90_PATH_A:
+		len = RTL8192EE_RADIOA_ARRAY_LEN;
+		array = RTL8192EE_RADIOA_ARRAY;
+		RT_TRACE(COMP_INIT, DBG_LOUD,
+			 ("Radio_A:RTL8192EE_RADIOA_ARRAY %d\n" , len));
+		RT_TRACE(COMP_INIT, DBG_LOUD, ("Radio No %x\n", rfpath));
+		for (i = 0; i < len; i = i + 2) {
+			v1 = array[i];
+			v2 = array[i+1];
+			if (v1 < 0xcdcdcdcd) {
+				_rtl92ee_config_rf_radio_a(hw, v1, v2);
+				continue;
+			} else {/*This line is the start line of branch.*/
+				/* to protect READ_NEXT_PAIR not overrun */
+				if (i >= len - 2)
+					break;
+
+				if (!_check_condition(hw , array[i])) {
+					/*Discard the following pairs*/
+					READ_NEXT_RF_PAIR(v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD && i < len - 2)
+						READ_NEXT_RF_PAIR(v1, v2, i);
+					i -= 2; /* prevent from for-loop += 2*/
+				} else {/* Configure matched pairs and
+					 * skip to end of if-else.*/
+					READ_NEXT_RF_PAIR(v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD && i < len - 2) {
+						_rtl92ee_config_rf_radio_a(hw,
+									   v1,
+									   v2);
+						READ_NEXT_RF_PAIR(v1, v2, i);
+					}
+
+					while (v2 != 0xDEAD && i < len - 2)
+						READ_NEXT_RF_PAIR(v1, v2, i);
+				}
+			}
+		}
+		break;
+
+	case RF90_PATH_B:
+		len = RTL8192EE_RADIOB_ARRAY_LEN;
+		array = RTL8192EE_RADIOB_ARRAY;
+		RT_TRACE(COMP_INIT, DBG_LOUD,
+			 ("Radio_A:RTL8192EE_RADIOB_ARRAY %d\n" , len));
+		RT_TRACE(COMP_INIT, DBG_LOUD, ("Radio No %x\n", rfpath));
+		for (i = 0; i < len; i = i + 2) {
+			v1 = array[i];
+			v2 = array[i+1];
+			if (v1 < 0xcdcdcdcd) {
+				_rtl92ee_config_rf_radio_b(hw, v1, v2);
+				continue;
+			} else {/*This line is the start line of branch.*/
+				/* to protect READ_NEXT_PAIR not overrun */
+				if (i >= len - 2)
+					break;
+
+				if (!_check_condition(hw , array[i])) {
+					/*Discard the following pairs*/
+					READ_NEXT_RF_PAIR(v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD && i < len - 2)
+						READ_NEXT_RF_PAIR(v1, v2, i);
+					i -= 2; /* prevent from for-loop += 2*/
+				} else {/* Configure matched pairs and
+					 * skip to end of if-else.*/
+					READ_NEXT_RF_PAIR(v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD && i < len - 2) {
+						_rtl92ee_config_rf_radio_b(hw,
+									   v1,
+									   v2);
+						READ_NEXT_RF_PAIR(v1, v2, i);
+					}
+
+					while (v2 != 0xDEAD && i < len - 2)
+						READ_NEXT_RF_PAIR(v1, v2, i);
+				}
+			}
+		}
+		break;
+	case RF90_PATH_C:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("switch case not process\n"));
+		break;
+	case RF90_PATH_D:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("switch case not process\n"));
+		break;
+	}
+	return true;
+}
+
+void rtl92ee_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	rtlphy->default_initialgain[0] =
+		(u8) rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, MASKBYTE0);
+	rtlphy->default_initialgain[1] =
+		(u8) rtl_get_bbreg(hw, ROFDM0_XBAGCCORE1, MASKBYTE0);
+	rtlphy->default_initialgain[2] =
+		(u8) rtl_get_bbreg(hw, ROFDM0_XCAGCCORE1, MASKBYTE0);
+	rtlphy->default_initialgain[3] =
+		(u8) rtl_get_bbreg(hw, ROFDM0_XDAGCCORE1, MASKBYTE0);
+
+	RT_TRACE(COMP_INIT, DBG_TRACE,
+		 ("Default initial gain (c50 = 0x%x, c58 = 0x%x, c60 = 0x%x, c68 = 0x%x\n",
+		  rtlphy->default_initialgain[0],
+		  rtlphy->default_initialgain[1],
+		  rtlphy->default_initialgain[2],
+		  rtlphy->default_initialgain[3]));
+
+	rtlphy->framesync = (u8) rtl_get_bbreg(hw,
+					       ROFDM0_RXDETECTOR3, MASKBYTE0);
+	rtlphy->framesync_c34 = rtl_get_bbreg(hw,
+					      ROFDM0_RXDETECTOR2, MASKDWORD);
+
+	RT_TRACE(COMP_INIT, DBG_TRACE,
+		 ("Default framesync (0x%x) = 0x%x\n",
+		  ROFDM0_RXDETECTOR3, rtlphy->framesync));
+}
+
+static void _rtl92ee_phy_init_bb_rf_register_definition(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfintfs = RFPGA0_XAB_RFINTERFACESW;
+	rtlphy->phyreg_def[RF90_PATH_B].rfintfs = RFPGA0_XAB_RFINTERFACESW;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfintfo = RFPGA0_XA_RFINTERFACEOE;
+	rtlphy->phyreg_def[RF90_PATH_B].rfintfo = RFPGA0_XB_RFINTERFACEOE;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfintfe = RFPGA0_XA_RFINTERFACEOE;
+	rtlphy->phyreg_def[RF90_PATH_B].rfintfe = RFPGA0_XB_RFINTERFACEOE;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rf3wire_offset =
+							RFPGA0_XA_LSSIPARAMETER;
+	rtlphy->phyreg_def[RF90_PATH_B].rf3wire_offset =
+							RFPGA0_XB_LSSIPARAMETER;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfhssi_para2 = RFPGA0_XA_HSSIPARAMETER2;
+	rtlphy->phyreg_def[RF90_PATH_B].rfhssi_para2 = RFPGA0_XB_HSSIPARAMETER2;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rflssi_readback =
+							 RFPGA0_XA_LSSIREADBACK;
+	rtlphy->phyreg_def[RF90_PATH_B].rflssi_readback =
+							 RFPGA0_XB_LSSIREADBACK;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rflssi_readbackpi =
+						      TRANSCEIVEA_HSPI_READBACK;
+	rtlphy->phyreg_def[RF90_PATH_B].rflssi_readbackpi =
+						      TRANSCEIVEB_HSPI_READBACK;
+}
+
+void rtl92ee_phy_get_txpower_level(struct ieee80211_hw *hw, long *powerlevel)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 txpwr_level;
+	long txpwr_dbm;
+
+	txpwr_level = rtlphy->cur_cck_txpwridx;
+	txpwr_dbm = _rtl92ee_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_B,
+						  txpwr_level);
+	txpwr_level = rtlphy->cur_ofdm24g_txpwridx;
+	if (_rtl92ee_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_G, txpwr_level) >
+	    txpwr_dbm)
+		txpwr_dbm = _rtl92ee_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_G,
+							  txpwr_level);
+	txpwr_level = rtlphy->cur_ofdm24g_txpwridx;
+	if (_rtl92ee_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_N_24G,
+					  txpwr_level) > txpwr_dbm)
+		txpwr_dbm = _rtl92ee_phy_txpwr_idx_to_dbm(hw,
+							  WIRELESS_MODE_N_24G,
+							  txpwr_level);
+	*powerlevel = txpwr_dbm;
+}
+
+static u8 _rtl92ee_phy_get_ratesection_intxpower_byrate(enum radio_path path,
+							u8 rate)
+{
+	u8 rate_section = 0;
+
+	switch (rate) {
+	case DESC92C_RATE1M:
+		rate_section = 2;
+		break;
+
+	case DESC92C_RATE2M:
+	case DESC92C_RATE5_5M:
+		if (path == RF90_PATH_A)
+			rate_section = 3;
+		else if (path == RF90_PATH_B)
+			rate_section = 2;
+		break;
+
+	case DESC92C_RATE11M:
+		rate_section = 3;
+		break;
+
+	case DESC92C_RATE6M:
+	case DESC92C_RATE9M:
+	case DESC92C_RATE12M:
+	case DESC92C_RATE18M:
+		rate_section = 0;
+		break;
+
+	case DESC92C_RATE24M:
+	case DESC92C_RATE36M:
+	case DESC92C_RATE48M:
+	case DESC92C_RATE54M:
+		rate_section = 1;
+		break;
+
+	case DESC92C_RATEMCS0:
+	case DESC92C_RATEMCS1:
+	case DESC92C_RATEMCS2:
+	case DESC92C_RATEMCS3:
+		rate_section = 4;
+		break;
+
+	case DESC92C_RATEMCS4:
+	case DESC92C_RATEMCS5:
+	case DESC92C_RATEMCS6:
+	case DESC92C_RATEMCS7:
+		rate_section = 5;
+		break;
+
+	case DESC92C_RATEMCS8:
+	case DESC92C_RATEMCS9:
+	case DESC92C_RATEMCS10:
+	case DESC92C_RATEMCS11:
+		rate_section = 6;
+		break;
+
+	case DESC92C_RATEMCS12:
+	case DESC92C_RATEMCS13:
+	case DESC92C_RATEMCS14:
+	case DESC92C_RATEMCS15:
+		rate_section = 7;
+		break;
+
+	default:
+		RT_ASSERT(true, ("Rate_Section is Illegal\n"));
+		break;
+	}
+
+	return rate_section;
+}
+
+static u8 _rtl92ee_get_txpower_by_rate(struct ieee80211_hw *hw,
+				       enum band_type band,
+				       enum radio_path rf, u8 rate)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 shift = 0, sec, tx_num;
+	char diff = 0;
+
+	sec = _rtl92ee_phy_get_ratesection_intxpower_byrate(rf, rate);
+	tx_num = RF_TX_NUM_NONIMPLEMENT;
+
+	if (tx_num == RF_TX_NUM_NONIMPLEMENT) {
+		if ((rate >= DESC92C_RATEMCS8 && rate <= DESC92C_RATEMCS15))
+			tx_num = RF_2TX;
+		else
+			tx_num = RF_1TX;
+	}
+
+	switch (rate) {
+	case DESC92C_RATE1M:
+		shift = 0;
+		break;
+	case DESC92C_RATE2M:
+		shift = 8;
+		break;
+	case DESC92C_RATE5_5M:
+		shift = 16;
+		break;
+	case DESC92C_RATE11M:
+		shift = 24;
+		break;
+
+	case DESC92C_RATE6M:
+		shift = 0;
+		break;
+	case DESC92C_RATE9M:
+		shift = 8;
+		break;
+	case DESC92C_RATE12M:
+		shift = 16;
+		break;
+	case DESC92C_RATE18M:
+		shift = 24;
+		break;
+
+	case DESC92C_RATE24M:
+		shift = 0;
+		break;
+	case DESC92C_RATE36M:
+		shift = 8;
+		break;
+	case DESC92C_RATE48M:
+		shift = 16;
+		break;
+	case DESC92C_RATE54M:
+		shift = 24;
+		break;
+
+	case DESC92C_RATEMCS0:
+		shift = 0;
+		break;
+	case DESC92C_RATEMCS1:
+		shift = 8;
+		break;
+	case DESC92C_RATEMCS2:
+		shift = 16;
+		break;
+	case DESC92C_RATEMCS3:
+		shift = 24;
+		break;
+
+	case DESC92C_RATEMCS4:
+		shift = 0;
+		break;
+	case DESC92C_RATEMCS5:
+		shift = 8;
+		break;
+	case DESC92C_RATEMCS6:
+		shift = 16;
+		break;
+	case DESC92C_RATEMCS7:
+		shift = 24;
+		break;
+
+	case DESC92C_RATEMCS8:
+		shift = 0;
+		break;
+	case DESC92C_RATEMCS9:
+		shift = 8;
+		break;
+	case DESC92C_RATEMCS10:
+		shift = 16;
+		break;
+	case DESC92C_RATEMCS11:
+		shift = 24;
+		break;
+
+	case DESC92C_RATEMCS12:
+		shift = 0;
+		break;
+	case DESC92C_RATEMCS13:
+		shift = 8;
+		break;
+	case DESC92C_RATEMCS14:
+		shift = 16;
+		break;
+	case DESC92C_RATEMCS15:
+		shift = 24;
+		break;
+
+	default:
+		RT_ASSERT(true, ("Rate_Section is Illegal\n"));
+		break;
+	}
+
+	diff = (u8) (rtlphy->tx_power_by_rate_offset[band][rf][tx_num][sec] >>
+		     shift) & 0xff;
+
+	return	diff;
+}
+
+static u8 _rtl92ee_get_txpower_index(struct ieee80211_hw *hw,
+				     enum radio_path rfpath, u8 rate,
+				     u8 bw, u8 channel)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtlpriv);
+	u8 index = (channel - 1);
+	u8 tx_power = 0;
+	u8 diff = 0;
+
+	if (channel < 1 || channel > 14) {
+		index = 0;
+		RT_TRACE(COMP_POWER_TRACKING, DBG_DMESG,
+			 ("Illegal channel!!\n"));
+	}
+
+	if (IS_CCK_RATE(rate))
+		tx_power = rtlefuse->txpwrlevel_cck[rfpath][index];
+	else if (DESC92C_RATE6M <= rate)
+		tx_power = rtlefuse->txpwrlevel_ht40_1s[rfpath][index];
+
+	/* OFDM-1T*/
+	if (DESC92C_RATE6M <= rate && rate <= DESC92C_RATE54M &&
+	    !IS_CCK_RATE(rate))
+		tx_power += rtlefuse->txpwr_legacyhtdiff[rfpath][TX_1S];
+
+	/* BW20-1S, BW20-2S */
+	if (bw == HT_CHANNEL_WIDTH_20) {
+		if (DESC92C_RATEMCS0 <= rate && rate <= DESC92C_RATEMCS15)
+			tx_power += rtlefuse->txpwr_ht20diff[rfpath][TX_1S];
+		if (DESC92C_RATEMCS8 <= rate && rate <= DESC92C_RATEMCS15)
+			tx_power += rtlefuse->txpwr_ht20diff[rfpath][TX_2S];
+	} else if (bw == HT_CHANNEL_WIDTH_20_40) {/* BW40-1S, BW40-2S */
+		if (DESC92C_RATEMCS0 <= rate && rate <= DESC92C_RATEMCS15)
+			tx_power += rtlefuse->txpwr_ht40diff[rfpath][TX_1S];
+		if (DESC92C_RATEMCS8 <= rate && rate <= DESC92C_RATEMCS15)
+			tx_power += rtlefuse->txpwr_ht40diff[rfpath][TX_2S];
+	}
+
+	if (rtlefuse->eeprom_regulatory != 2)
+		diff = _rtl92ee_get_txpower_by_rate(hw, BAND_ON_2_4G,
+						    rfpath, rate);
+
+	tx_power += diff;
+
+	if (tx_power > MAX_POWER_INDEX)
+		tx_power = MAX_POWER_INDEX;
+
+	return tx_power;
+}
+
+static void _rtl92ee_set_txpower_index(struct ieee80211_hw *hw, u8 pwr_idx,
+				       enum radio_path rfpath, u8 rate)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (rfpath == RF90_PATH_A) {
+		switch (rate) {
+		case DESC92C_RATE1M:
+			rtl_set_bbreg(hw, RTXAGC_A_CCK1_MCS32,
+				      MASKBYTE1, pwr_idx);
+			break;
+		case DESC92C_RATE2M:
+			rtl_set_bbreg(hw, RTXAGC_B_CCK11_A_CCK2_11,
+				      MASKBYTE1, pwr_idx);
+			break;
+		case DESC92C_RATE5_5M:
+			rtl_set_bbreg(hw, RTXAGC_B_CCK11_A_CCK2_11,
+				      MASKBYTE2, pwr_idx);
+			break;
+		case DESC92C_RATE11M:
+			rtl_set_bbreg(hw, RTXAGC_B_CCK11_A_CCK2_11,
+				      MASKBYTE3, pwr_idx);
+			break;
+		case DESC92C_RATE6M:
+			rtl_set_bbreg(hw, RTXAGC_A_RATE18_06,
+				      MASKBYTE0, pwr_idx);
+			break;
+		case DESC92C_RATE9M:
+			rtl_set_bbreg(hw, RTXAGC_A_RATE18_06,
+				      MASKBYTE1, pwr_idx);
+			break;
+		case DESC92C_RATE12M:
+			rtl_set_bbreg(hw, RTXAGC_A_RATE18_06,
+				      MASKBYTE2, pwr_idx);
+			break;
+		case DESC92C_RATE18M:
+			rtl_set_bbreg(hw, RTXAGC_A_RATE18_06,
+				      MASKBYTE3, pwr_idx);
+			break;
+		case DESC92C_RATE24M:
+			rtl_set_bbreg(hw, RTXAGC_A_RATE54_24,
+				      MASKBYTE0, pwr_idx);
+			break;
+		case DESC92C_RATE36M:
+			rtl_set_bbreg(hw, RTXAGC_A_RATE54_24,
+				      MASKBYTE1, pwr_idx);
+			break;
+		case DESC92C_RATE48M:
+			rtl_set_bbreg(hw, RTXAGC_A_RATE54_24,
+				      MASKBYTE2, pwr_idx);
+			break;
+		case DESC92C_RATE54M:
+			rtl_set_bbreg(hw, RTXAGC_A_RATE54_24,
+				      MASKBYTE3, pwr_idx);
+			break;
+		case DESC92C_RATEMCS0:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS03_MCS00,
+				      MASKBYTE0, pwr_idx);
+			break;
+		case DESC92C_RATEMCS1:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS03_MCS00,
+				      MASKBYTE1, pwr_idx);
+			break;
+		case DESC92C_RATEMCS2:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS03_MCS00,
+				      MASKBYTE2, pwr_idx);
+			break;
+		case DESC92C_RATEMCS3:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS03_MCS00,
+				      MASKBYTE3, pwr_idx);
+			break;
+		case DESC92C_RATEMCS4:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS07_MCS04,
+				      MASKBYTE0, pwr_idx);
+			break;
+		case DESC92C_RATEMCS5:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS07_MCS04,
+				      MASKBYTE1, pwr_idx);
+			break;
+		case DESC92C_RATEMCS6:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS07_MCS04,
+				      MASKBYTE2, pwr_idx);
+			break;
+		case DESC92C_RATEMCS7:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS07_MCS04,
+				      MASKBYTE3, pwr_idx);
+			break;
+		case DESC92C_RATEMCS8:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS11_MCS08,
+				      MASKBYTE0, pwr_idx);
+			break;
+		case DESC92C_RATEMCS9:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS11_MCS08,
+				      MASKBYTE1, pwr_idx);
+			break;
+		case DESC92C_RATEMCS10:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS11_MCS08,
+				      MASKBYTE2, pwr_idx);
+			break;
+		case DESC92C_RATEMCS11:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS11_MCS08,
+				      MASKBYTE3, pwr_idx);
+			break;
+		case DESC92C_RATEMCS12:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS15_MCS12,
+				      MASKBYTE0, pwr_idx);
+			break;
+		case DESC92C_RATEMCS13:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS15_MCS12,
+				      MASKBYTE1, pwr_idx);
+			break;
+		case DESC92C_RATEMCS14:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS15_MCS12,
+				      MASKBYTE2, pwr_idx);
+			break;
+		case DESC92C_RATEMCS15:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS15_MCS12,
+				      MASKBYTE3, pwr_idx);
+			break;
+		default:
+			RT_TRACE(COMP_POWER, DBG_LOUD, ("Invalid Rate!!\n"));
+			break;
+		}
+	} else if (rfpath == RF90_PATH_B) {
+		switch (rate) {
+		case DESC92C_RATE1M:
+			rtl_set_bbreg(hw, RTXAGC_B_CCK1_55_MCS32,
+				      MASKBYTE1, pwr_idx);
+			break;
+		case DESC92C_RATE2M:
+			rtl_set_bbreg(hw, RTXAGC_B_CCK1_55_MCS32,
+				      MASKBYTE2, pwr_idx);
+			break;
+		case DESC92C_RATE5_5M:
+			rtl_set_bbreg(hw, RTXAGC_B_CCK1_55_MCS32,
+				      MASKBYTE3, pwr_idx);
+			break;
+		case DESC92C_RATE11M:
+			rtl_set_bbreg(hw, RTXAGC_B_CCK11_A_CCK2_11,
+				      MASKBYTE0, pwr_idx);
+			break;
+		case DESC92C_RATE6M:
+			rtl_set_bbreg(hw, RTXAGC_B_RATE18_06,
+				      MASKBYTE0, pwr_idx);
+			break;
+		case DESC92C_RATE9M:
+			rtl_set_bbreg(hw, RTXAGC_B_RATE18_06,
+				      MASKBYTE1, pwr_idx);
+			break;
+		case DESC92C_RATE12M:
+			rtl_set_bbreg(hw, RTXAGC_B_RATE18_06,
+				      MASKBYTE2, pwr_idx);
+			break;
+		case DESC92C_RATE18M:
+			rtl_set_bbreg(hw, RTXAGC_B_RATE18_06,
+				      MASKBYTE3, pwr_idx);
+			break;
+		case DESC92C_RATE24M:
+			rtl_set_bbreg(hw, RTXAGC_B_RATE54_24,
+				      MASKBYTE0, pwr_idx);
+			break;
+		case DESC92C_RATE36M:
+			rtl_set_bbreg(hw, RTXAGC_B_RATE54_24,
+				      MASKBYTE1, pwr_idx);
+			break;
+		case DESC92C_RATE48M:
+			rtl_set_bbreg(hw, RTXAGC_B_RATE54_24,
+				      MASKBYTE2, pwr_idx);
+			break;
+		case DESC92C_RATE54M:
+			rtl_set_bbreg(hw, RTXAGC_B_RATE54_24,
+				      MASKBYTE3, pwr_idx);
+			break;
+		case DESC92C_RATEMCS0:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS03_MCS00,
+				      MASKBYTE0, pwr_idx);
+			break;
+		case DESC92C_RATEMCS1:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS03_MCS00,
+				      MASKBYTE1, pwr_idx);
+			break;
+		case DESC92C_RATEMCS2:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS03_MCS00,
+				      MASKBYTE2, pwr_idx);
+			break;
+		case DESC92C_RATEMCS3:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS03_MCS00,
+				      MASKBYTE3, pwr_idx);
+			break;
+		case DESC92C_RATEMCS4:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS07_MCS04,
+				      MASKBYTE0, pwr_idx);
+			break;
+		case DESC92C_RATEMCS5:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS07_MCS04,
+				      MASKBYTE1, pwr_idx);
+			break;
+		case DESC92C_RATEMCS6:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS07_MCS04,
+				      MASKBYTE2, pwr_idx);
+			break;
+		case DESC92C_RATEMCS7:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS07_MCS04,
+				      MASKBYTE3, pwr_idx);
+			break;
+		case DESC92C_RATEMCS8:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS11_MCS08,
+				      MASKBYTE0, pwr_idx);
+			break;
+		case DESC92C_RATEMCS9:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS11_MCS08,
+				      MASKBYTE1, pwr_idx);
+			break;
+		case DESC92C_RATEMCS10:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS11_MCS08,
+				      MASKBYTE2, pwr_idx);
+			break;
+		case DESC92C_RATEMCS11:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS11_MCS08,
+				      MASKBYTE3, pwr_idx);
+			break;
+		case DESC92C_RATEMCS12:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS15_MCS12,
+				      MASKBYTE0, pwr_idx);
+			break;
+		case DESC92C_RATEMCS13:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS15_MCS12,
+				      MASKBYTE1, pwr_idx);
+			break;
+		case DESC92C_RATEMCS14:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS15_MCS12,
+				      MASKBYTE2, pwr_idx);
+			break;
+		case DESC92C_RATEMCS15:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS15_MCS12,
+				      MASKBYTE3, pwr_idx);
+			break;
+		default:
+			RT_TRACE(COMP_POWER, DBG_LOUD, ("Invalid Rate!!\n"));
+			break;
+		}
+	} else {
+		RT_TRACE(COMP_POWER, DBG_LOUD, ("Invalid RFPath!!\n"));
+	}
+}
+
+static void rtl92ee_phy_set_txpower_index_by_rate_array(struct ieee80211_hw *hw,
+							enum radio_path rfpath, u8 bw,
+							u8 channel, u8 *rates, u8 size)
+{
+	u8 i;
+	u8 power_index;
+	for (i = 0; i < size; i++) {
+		power_index = _rtl92ee_get_txpower_index(hw, rfpath, rates[i],
+							 bw, channel);
+		_rtl92ee_set_txpower_index(hw, power_index, rfpath, rates[i]);
+	}
+}
+
+static void rtl92ee_phy_set_txpower_index_by_rate_section(struct ieee80211_hw *hw,
+							  enum radio_path rfpath,
+							  u8 channel,
+							  enum rate_section section)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	if (section == CCK) {
+		u8 cck_rates[] = {DESC92C_RATE1M, DESC92C_RATE2M,
+				  DESC92C_RATE5_5M, DESC92C_RATE11M};
+		if (rtlhal->current_bandtype == BAND_ON_2_4G)
+			rtl92ee_phy_set_txpower_index_by_rate_array(hw, rfpath,
+							rtlphy->current_chan_bw,
+							channel, cck_rates, 4);
+	} else if (section == OFDM) {
+		u8 ofdm_rates[] = {DESC92C_RATE6M, DESC92C_RATE9M,
+				   DESC92C_RATE12M, DESC92C_RATE18M,
+				   DESC92C_RATE24M, DESC92C_RATE36M,
+				   DESC92C_RATE48M, DESC92C_RATE54M};
+		rtl92ee_phy_set_txpower_index_by_rate_array(hw, rfpath,
+							rtlphy->current_chan_bw,
+							channel, ofdm_rates, 8);
+	} else if (section == HT_MCS0_MCS7) {
+		u8 ht_rates1t[]  = {DESC92C_RATEMCS0, DESC92C_RATEMCS1,
+				    DESC92C_RATEMCS2, DESC92C_RATEMCS3,
+				    DESC92C_RATEMCS4, DESC92C_RATEMCS5,
+				    DESC92C_RATEMCS6, DESC92C_RATEMCS7};
+		rtl92ee_phy_set_txpower_index_by_rate_array(hw, rfpath,
+							rtlphy->current_chan_bw,
+							channel, ht_rates1t, 8);
+	} else if (section == HT_MCS8_MCS15) {
+		u8 ht_rates2t[]  = {DESC92C_RATEMCS8, DESC92C_RATEMCS9,
+				    DESC92C_RATEMCS10, DESC92C_RATEMCS11,
+				    DESC92C_RATEMCS12, DESC92C_RATEMCS13,
+				    DESC92C_RATEMCS14, DESC92C_RATEMCS15};
+		rtl92ee_phy_set_txpower_index_by_rate_array(hw, rfpath,
+							rtlphy->current_chan_bw,
+							channel, ht_rates2t, 8);
+	} else
+		RT_TRACE(FPHY, PHY_TXPWR,
+			 ("Invalid RateSection %d\n", section));
+}
+
+void rtl92ee_phy_set_txpower_level(struct ieee80211_hw *hw, u8 channel)
+{
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &(rtl_priv(hw)->phy);
+	enum radio_path rfpath;
+
+	if (rtlefuse->b_txpwr_fromeprom == false)
+		return;
+	for (rfpath = RF90_PATH_A; rfpath < rtlphy->num_total_rfpath;
+	     rfpath++) {
+		rtl92ee_phy_set_txpower_index_by_rate_section(hw, rfpath,
+							      channel, CCK);
+		rtl92ee_phy_set_txpower_index_by_rate_section(hw, rfpath,
+							      channel, OFDM);
+		rtl92ee_phy_set_txpower_index_by_rate_section(hw, rfpath,
+							      channel,
+							      HT_MCS0_MCS7);
+		if (rtlphy->num_total_rfpath >= 2)
+			rtl92ee_phy_set_txpower_index_by_rate_section(hw,
+								rfpath, channel,
+								HT_MCS8_MCS15);
+	}
+}
+
+static long _rtl92ee_phy_txpwr_idx_to_dbm(struct ieee80211_hw *hw,
+					  enum wireless_mode wirelessmode,
+					  u8 txpwridx)
+{
+	long offset;
+	long pwrout_dbm;
+
+	switch (wirelessmode) {
+	case WIRELESS_MODE_B:
+		offset = -7;
+		break;
+	case WIRELESS_MODE_G:
+	case WIRELESS_MODE_N_24G:
+		offset = -8;
+		break;
+	default:
+		offset = -8;
+		break;
+	}
+	pwrout_dbm = txpwridx / 2 + offset;
+	return pwrout_dbm;
+}
+
+void rtl92ee_phy_scan_operation_backup(struct ieee80211_hw *hw, u8 operation)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	enum io_type iotype;
+
+	if (!is_hal_stop(rtlhal)) {
+		switch (operation) {
+		case SCAN_OPT_BACKUP_BAND0:
+			iotype = IO_CMD_PAUSE_BAND0_DM_BY_SCAN;
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_IO_CMD,
+						      (u8 *)&iotype);
+
+			break;
+		case SCAN_OPT_RESTORE:
+			iotype = IO_CMD_RESUME_DM_BY_SCAN;
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_IO_CMD,
+						      (u8 *)&iotype);
+			break;
+		default:
+			RT_TRACE(COMP_ERR, DBG_EMERG,
+				 ("Unknown Scan Backup operation.\n"));
+			break;
+		}
+	}
+}
+
+void rtl92ee_phy_set_bw_mode_callback(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u8 reg_bw_opmode;
+	u8 reg_prsr_rsc;
+
+	RT_TRACE(COMP_SCAN, DBG_TRACE,
+		 ("Switch to %s bandwidth\n",
+		  rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20 ?
+		  "20MHz" : "40MHz"));
+
+	if (is_hal_stop(rtlhal)) {
+		rtlphy->set_bwmode_inprogress = false;
+		return;
+	}
+
+	reg_bw_opmode = rtl_read_byte(rtlpriv, REG_BWOPMODE);
+	reg_prsr_rsc = rtl_read_byte(rtlpriv, REG_RRSR + 2);
+
+	switch (rtlphy->current_chan_bw) {
+	case HT_CHANNEL_WIDTH_20:
+		reg_bw_opmode |= BW_OPMODE_20MHZ;
+		rtl_write_byte(rtlpriv, REG_BWOPMODE, reg_bw_opmode);
+		break;
+	case HT_CHANNEL_WIDTH_20_40:
+		reg_bw_opmode &= ~BW_OPMODE_20MHZ;
+		rtl_write_byte(rtlpriv, REG_BWOPMODE, reg_bw_opmode);
+		reg_prsr_rsc = (reg_prsr_rsc & 0x90) |
+			       (mac->cur_40_prime_sc << 5);
+		rtl_write_byte(rtlpriv, REG_RRSR + 2, reg_prsr_rsc);
+		break;
+	default:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("unknown bandwidth: %#X\n", rtlphy->current_chan_bw));
+		break;
+	}
+
+	switch (rtlphy->current_chan_bw) {
+	case HT_CHANNEL_WIDTH_20:
+		rtl_set_bbreg(hw, RFPGA0_RFMOD, BRFMOD, 0x0);
+		rtl_set_bbreg(hw, RFPGA1_RFMOD, BRFMOD, 0x0);
+		rtl_set_bbreg(hw, ROFDM0_TXPSEUDONOISEWGT,
+			      (BIT(31) | BIT(30)), 0);
+		break;
+	case HT_CHANNEL_WIDTH_20_40:
+		rtl_set_bbreg(hw, RFPGA0_RFMOD, BRFMOD, 0x1);
+		rtl_set_bbreg(hw, RFPGA1_RFMOD, BRFMOD, 0x1);
+		rtl_set_bbreg(hw, RCCK0_SYSTEM, BCCK_SIDEBAND,
+			      (mac->cur_40_prime_sc >> 1));
+		rtl_set_bbreg(hw, ROFDM1_LSTF, 0xC00,
+			      mac->cur_40_prime_sc);
+
+		rtl_set_bbreg(hw, 0x818, (BIT(26) | BIT(27)),
+			      (mac->cur_40_prime_sc ==
+			       HAL_PRIME_CHNL_OFFSET_LOWER) ? 2 : 1);
+		break;
+	default:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("unknown bandwidth: %#X\n", rtlphy->current_chan_bw));
+		break;
+	}
+	rtl92ee_phy_rf6052_set_bandwidth(hw, rtlphy->current_chan_bw);
+	rtlphy->set_bwmode_inprogress = false;
+	RT_TRACE(COMP_SCAN, DBG_LOUD, ("\n"));
+}
+
+void rtl92ee_phy_set_bw_mode(struct ieee80211_hw *hw,
+			     enum nl80211_channel_type ch_type)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 tmp_bw = rtlphy->current_chan_bw;
+
+	if (rtlphy->set_bwmode_inprogress)
+		return;
+	rtlphy->set_bwmode_inprogress = true;
+	if ((!is_hal_stop(rtlhal)) && !(RT_CANNOT_IO(hw))) {
+		rtl92ee_phy_set_bw_mode_callback(hw);
+	} else {
+		RT_TRACE(COMP_ERR, DBG_WARNING,
+			 ("false driver sleep or unload\n"));
+		rtlphy->set_bwmode_inprogress = false;
+		rtlphy->current_chan_bw = tmp_bw;
+	}
+}
+
+void rtl92ee_phy_sw_chnl_callback(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u32 delay;
+
+	RT_TRACE(COMP_SCAN, DBG_TRACE,
+		 ("switch to channel%d\n", rtlphy->current_channel));
+	if (is_hal_stop(rtlhal))
+		return;
+	do {
+		if (!rtlphy->sw_chnl_inprogress)
+			break;
+		if (!_rtl92ee_phy_sw_chnl_step_by_step
+		    (hw, rtlphy->current_channel, &rtlphy->sw_chnl_stage,
+		     &rtlphy->sw_chnl_step, &delay)) {
+			if (delay > 0)
+				mdelay(delay);
+			else
+				continue;
+		} else {
+			rtlphy->sw_chnl_inprogress = false;
+		}
+		break;
+	} while (true);
+	RT_TRACE(COMP_SCAN, DBG_TRACE, ("\n"));
+}
+
+u8 rtl92ee_phy_sw_chnl(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	if (rtlphy->sw_chnl_inprogress)
+		return 0;
+	if (rtlphy->set_bwmode_inprogress)
+		return 0;
+	RT_ASSERT((rtlphy->current_channel <= 14),
+		  ("WIRELESS_MODE_G but channel>14"));
+	rtlphy->sw_chnl_inprogress = true;
+	rtlphy->sw_chnl_stage = 0;
+	rtlphy->sw_chnl_step = 0;
+	if (!(is_hal_stop(rtlhal)) && !(RT_CANNOT_IO(hw))) {
+		rtl92ee_phy_sw_chnl_callback(hw);
+		RT_TRACE(COMP_CHAN, DBG_LOUD,
+			 ("sw_chnl_inprogress false schdule workitem current channel %d\n",
+			 rtlphy->current_channel));
+		rtlphy->sw_chnl_inprogress = false;
+	} else {
+		RT_TRACE(COMP_CHAN, DBG_LOUD,
+			 ("sw_chnl_inprogress false driver sleep or unload\n"));
+		rtlphy->sw_chnl_inprogress = false;
+	}
+	return 1;
+}
+
+static bool _rtl92ee_phy_sw_chnl_step_by_step(struct ieee80211_hw *hw,
+					      u8 channel, u8 *stage, u8 *step,
+					      u32 *delay)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct swchnlcmd precommoncmd[MAX_PRECMD_CNT];
+	u32 precommoncmdcnt;
+	struct swchnlcmd postcommoncmd[MAX_POSTCMD_CNT];
+	u32 postcommoncmdcnt;
+	struct swchnlcmd rfdependcmd[MAX_RFDEPENDCMD_CNT];
+	u32 rfdependcmdcnt;
+	struct swchnlcmd *currentcmd = NULL;
+	u8 rfpath;
+	u8 num_total_rfpath = rtlphy->num_total_rfpath;
+
+	precommoncmdcnt = 0;
+	_rtl92ee_phy_set_sw_chnl_cmdarray(precommoncmd, precommoncmdcnt++,
+					  MAX_PRECMD_CNT,
+					  CMDID_SET_TXPOWEROWER_LEVEL, 0, 0, 0);
+	_rtl92ee_phy_set_sw_chnl_cmdarray(precommoncmd, precommoncmdcnt++,
+					  MAX_PRECMD_CNT, CMDID_END, 0, 0, 0);
+
+	postcommoncmdcnt = 0;
+
+	_rtl92ee_phy_set_sw_chnl_cmdarray(postcommoncmd, postcommoncmdcnt++,
+					  MAX_POSTCMD_CNT, CMDID_END, 0, 0, 0);
+
+	rfdependcmdcnt = 0;
+
+	RT_ASSERT((channel >= 1 && channel <= 14),
+		  ("illegal channel for Zebra: %d\n", channel));
+
+	_rtl92ee_phy_set_sw_chnl_cmdarray(rfdependcmd, rfdependcmdcnt++,
+					  MAX_RFDEPENDCMD_CNT,
+					  CMDID_RF_WRITEREG,
+					  RF_CHNLBW, channel, 10);
+
+	_rtl92ee_phy_set_sw_chnl_cmdarray(rfdependcmd, rfdependcmdcnt++,
+					  MAX_RFDEPENDCMD_CNT, CMDID_END,
+					  0, 0, 0);
+
+	do {
+		switch (*stage) {
+		case 0:
+			currentcmd = &precommoncmd[*step];
+			break;
+		case 1:
+			currentcmd = &rfdependcmd[*step];
+			break;
+		case 2:
+			currentcmd = &postcommoncmd[*step];
+			break;
+		default:
+			RT_TRACE(COMP_ERR, DBG_EMERG,
+				 ("Invalid 'stage' = %d, Check it!\n" ,
+				  *stage));
+			return true;
+			break;
+		}
+
+		if (currentcmd->cmdid == CMDID_END) {
+			if ((*stage) == 2) {
+				return true;
+			} else {
+				(*stage)++;
+				(*step) = 0;
+				continue;
+			}
+		}
+
+		switch (currentcmd->cmdid) {
+		case CMDID_SET_TXPOWEROWER_LEVEL:
+			rtl92ee_phy_set_txpower_level(hw, channel);
+			break;
+		case CMDID_WRITEPORT_ULONG:
+			rtl_write_dword(rtlpriv, currentcmd->para1,
+					currentcmd->para2);
+			break;
+		case CMDID_WRITEPORT_USHORT:
+			rtl_write_word(rtlpriv, currentcmd->para1,
+				       (u16) currentcmd->para2);
+			break;
+		case CMDID_WRITEPORT_UCHAR:
+			rtl_write_byte(rtlpriv, currentcmd->para1,
+				       (u8) currentcmd->para2);
+			break;
+		case CMDID_RF_WRITEREG:
+			for (rfpath = 0; rfpath < num_total_rfpath; rfpath++) {
+				rtlphy->rfreg_chnlval[rfpath] =
+					((rtlphy->rfreg_chnlval[rfpath] &
+					  0xfffff00) | currentcmd->para2);
+
+				rtl_set_rfreg(hw, (enum radio_path)rfpath,
+					      currentcmd->para1,
+					      0x3ff,
+					      rtlphy->rfreg_chnlval[rfpath]);
+			}
+			break;
+		default:
+			RT_TRACE(COMP_ERR, DBG_EMERG,
+				 ("switch case not process\n"));
+			break;
+		}
+
+		break;
+	} while (true);
+
+	(*delay) = currentcmd->msdelay;
+	(*step)++;
+	return false;
+}
+
+static bool _rtl92ee_phy_set_sw_chnl_cmdarray(struct swchnlcmd *cmdtable,
+					      u32 cmdtableidx, u32 cmdtablesz,
+					      enum swchnlcmd_id cmdid,
+					      u32 para1, u32 para2, u32 msdelay)
+{
+	struct swchnlcmd *pcmd;
+
+	if (cmdtable == NULL) {
+		RT_ASSERT(false, ("cmdtable cannot be NULL.\n"));
+		return false;
+	}
+
+	if (cmdtableidx >= cmdtablesz)
+		return false;
+
+	pcmd = cmdtable + cmdtableidx;
+	pcmd->cmdid = cmdid;
+	pcmd->para1 = para1;
+	pcmd->para2 = para2;
+	pcmd->msdelay = msdelay;
+	return true;
+}
+
+static u8 _rtl92ee_phy_path_a_iqk(struct ieee80211_hw *hw, bool config_pathb)
+{
+	u32 reg_eac, reg_e94, reg_e9c;
+	u8 result = 0x00;
+	/* path-A IQK setting */
+	/* PA/PAD controlled by 0x0 */
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+	rtl_set_rfreg(hw, RF90_PATH_A, 0xdf, RFREG_OFFSET_MASK, 0x180);
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
+
+	rtl_set_bbreg(hw, RTx_IQK_Tone_A, MASKDWORD, 0x18008c1c);
+	rtl_set_bbreg(hw, RRx_IQK_Tone_A, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RTx_IQK_Tone_B, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RRx_IQK_Tone_B, MASKDWORD, 0x38008c1c);
+
+	rtl_set_bbreg(hw, RTx_IQK_PI_A, MASKDWORD, 0x82140303);
+	rtl_set_bbreg(hw, RRx_IQK_PI_A, MASKDWORD, 0x68160000);
+
+	/*LO calibration setting*/
+	rtl_set_bbreg(hw, RIQK_AGC_Rsp, MASKDWORD, 0x00462911);
+
+	/*One shot, path A LOK & IQK*/
+	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xf9000000);
+	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xf8000000);
+
+	mdelay(IQK_DELAY_TIME);
+
+	reg_eac = rtl_get_bbreg(hw, 0xeac, MASKDWORD);
+	reg_e94 = rtl_get_bbreg(hw, 0xe94, MASKDWORD);
+	reg_e9c = rtl_get_bbreg(hw, 0xe9c, MASKDWORD);
+
+	if (!(reg_eac & BIT(28)) &&
+	    (((reg_e94 & 0x03FF0000) >> 16) != 0x142) &&
+	    (((reg_e9c & 0x03FF0000) >> 16) != 0x42))
+		result |= 0x01;
+	else
+		return result;
+
+	return result;
+}
+
+static u8 _rtl92ee_phy_path_b_iqk(struct ieee80211_hw *hw)
+{
+	u32 reg_eac, reg_eb4, reg_ebc;
+	u8 result = 0x00;
+
+	/* PA/PAD controlled by 0x0 */
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+	rtl_set_rfreg(hw, RF90_PATH_B, 0xdf, RFREG_OFFSET_MASK, 0x180);
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
+
+	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x00000000);
+	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x80800000);
+
+	rtl_set_bbreg(hw, RTx_IQK_Tone_A, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RRx_IQK_Tone_A, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RTx_IQK_Tone_B, MASKDWORD, 0x18008c1c);
+	rtl_set_bbreg(hw, RRx_IQK_Tone_B, MASKDWORD, 0x38008c1c);
+
+	rtl_set_bbreg(hw, RTx_IQK_PI_B, MASKDWORD, 0x821403e2);
+	rtl_set_bbreg(hw, RRx_IQK_PI_B, MASKDWORD, 0x68160000);
+
+	/* LO calibration setting */
+	rtl_set_bbreg(hw, RIQK_AGC_Rsp, MASKDWORD, 0x00462911);
+
+	/*One shot, path B LOK & IQK*/
+	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xfa000000);
+	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xf8000000);
+
+	mdelay(IQK_DELAY_TIME);
+
+	reg_eac = rtl_get_bbreg(hw, 0xeac, MASKDWORD);
+	reg_eb4 = rtl_get_bbreg(hw, 0xeb4, MASKDWORD);
+	reg_ebc = rtl_get_bbreg(hw, 0xebc, MASKDWORD);
+
+	if (!(reg_eac & BIT(31)) &&
+	    (((reg_eb4 & 0x03FF0000) >> 16) != 0x142) &&
+	    (((reg_ebc & 0x03FF0000) >> 16) != 0x42))
+		result |= 0x01;
+	else
+		return result;
+
+	return result;
+}
+
+static u8 _rtl92ee_phy_path_a_rx_iqk(struct ieee80211_hw *hw, bool config_pathb)
+{
+	u32 reg_eac, reg_e94, reg_e9c, reg_ea4 , u32temp;
+	u8 result = 0x00;
+
+	/*Get TXIMR Setting*/
+	/*Modify RX IQK mode table*/
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_WE_LUT, RFREG_OFFSET_MASK, 0x800a0);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_RCK_OS, RFREG_OFFSET_MASK, 0x30000);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0000f);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xf117b);
+
+	/*PA/PAD control by 0x56, and set = 0x0*/
+	rtl_set_rfreg(hw, RF90_PATH_A, 0xdf, RFREG_OFFSET_MASK, 0x980);
+	rtl_set_rfreg(hw, RF90_PATH_A, 0x56, RFREG_OFFSET_MASK, 0x51000);
+
+	/*enter IQK mode*/
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
+
+	/*IQK Setting*/
+	rtl_set_bbreg(hw, RTx_IQK, MASKDWORD, 0x01007c00);
+	rtl_set_bbreg(hw, RRx_IQK, MASKDWORD, 0x01004800);
+
+	/*path a IQK setting*/
+	rtl_set_bbreg(hw, RTx_IQK_Tone_A, MASKDWORD, 0x18008c1c);
+	rtl_set_bbreg(hw, RRx_IQK_Tone_A, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RTx_IQK_Tone_B, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RRx_IQK_Tone_B, MASKDWORD, 0x38008c1c);
+
+	rtl_set_bbreg(hw, RTx_IQK_PI_A, MASKDWORD, 0x82160c1f);
+	rtl_set_bbreg(hw, RRx_IQK_PI_A, MASKDWORD, 0x68160c1f);
+
+	/*LO calibration Setting*/
+	rtl_set_bbreg(hw, RIQK_AGC_Rsp, MASKDWORD, 0x0046a911);
+
+	/*one shot, path A LOK & iqk*/
+	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xfa000000);
+	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xf8000000);
+
+	mdelay(IQK_DELAY_TIME);
+
+	/* Check failed */
+	reg_eac = rtl_get_bbreg(hw, RRx_Power_After_IQK_A_2, MASKDWORD);
+	reg_e94 = rtl_get_bbreg(hw, RTx_Power_Before_IQK_A, MASKDWORD);
+	reg_e9c = rtl_get_bbreg(hw, RTx_Power_After_IQK_A, MASKDWORD);
+
+	if (!(reg_eac & BIT(28)) &&
+	    (((reg_e94 & 0x03FF0000) >> 16) != 0x142) &&
+	    (((reg_e9c & 0x03FF0000) >> 16) != 0x42)) {
+		result |= 0x01;
+	} else {
+		/*	PA/PAD controlled by 0x0 */
+		rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+		rtl_set_rfreg(hw, RF90_PATH_A, 0xdf, RFREG_OFFSET_MASK, 0x180);
+		return result;
+	}
+
+	u32temp = 0x80007C00 | (reg_e94 & 0x3FF0000)  |
+		  ((reg_e9c & 0x3FF0000) >> 16);
+	rtl_set_bbreg(hw, RTx_IQK, MASKDWORD, u32temp);
+	/*RX IQK*/
+	/*Modify RX IQK mode table*/
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_WE_LUT, RFREG_OFFSET_MASK, 0x800a0);
+
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_RCK_OS, RFREG_OFFSET_MASK, 0x30000);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0000f);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xf7ffa);
+
+	/*PA/PAD control by 0x56, and set = 0x0*/
+	rtl_set_rfreg(hw, RF90_PATH_A, 0xdf, RFREG_OFFSET_MASK, 0x980);
+	rtl_set_rfreg(hw, RF90_PATH_A, 0x56, RFREG_OFFSET_MASK, 0x51000);
+
+	/*enter IQK mode*/
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
+
+	/*IQK Setting*/
+	rtl_set_bbreg(hw, RRx_IQK, MASKDWORD, 0x01004800);
+
+	/*path a IQK setting*/
+	rtl_set_bbreg(hw, RTx_IQK_Tone_A, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RRx_IQK_Tone_A, MASKDWORD, 0x18008c1c);
+	rtl_set_bbreg(hw, RTx_IQK_Tone_B, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RRx_IQK_Tone_B, MASKDWORD, 0x38008c1c);
+
+	rtl_set_bbreg(hw, RTx_IQK_PI_A, MASKDWORD, 0x82160c1f);
+	rtl_set_bbreg(hw, RRx_IQK_PI_A, MASKDWORD, 0x28160c1f);
+
+	/*LO calibration Setting*/
+	rtl_set_bbreg(hw, RIQK_AGC_Rsp, MASKDWORD, 0x0046a891);
+	/*one shot, path A LOK & iqk*/
+	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xfa000000);
+	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xf8000000);
+
+	mdelay(IQK_DELAY_TIME);
+	/*Check failed*/
+	reg_eac = rtl_get_bbreg(hw, RRx_Power_After_IQK_A_2, MASKDWORD);
+	reg_ea4 = rtl_get_bbreg(hw, RRx_Power_Before_IQK_A_2, MASKDWORD);
+
+	/*PA/PAD controlled by 0x0*/
+	/*leave IQK mode*/
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+	rtl_set_rfreg(hw, RF90_PATH_A, 0xdf, RFREG_OFFSET_MASK, 0x180);
+	/*if Tx is OK, check whether Rx is OK*/
+	if (!(reg_eac & BIT(27)) &&
+	    (((reg_ea4 & 0x03FF0000) >> 16) != 0x132) &&
+	    (((reg_eac & 0x03FF0000) >> 16) != 0x36))
+		result |= 0x02;
+
+	return result;
+}
+
+static u8 _rtl92ee_phy_path_b_rx_iqk(struct ieee80211_hw *hw, bool config_pathb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 reg_eac, reg_eb4, reg_ebc, reg_ecc, reg_ec4, u32temp;
+	u8 result = 0x00;
+
+	/*Get TXIMR Setting*/
+	/*Modify RX IQK mode table*/
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+
+	rtl_set_rfreg(hw, RF90_PATH_B, RF_WE_LUT, RFREG_OFFSET_MASK, 0x800a0);
+	rtl_set_rfreg(hw, RF90_PATH_B, RF_RCK_OS, RFREG_OFFSET_MASK, 0x30000);
+	rtl_set_rfreg(hw, RF90_PATH_B, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0000f);
+	rtl_set_rfreg(hw, RF90_PATH_B, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xf117b);
+
+	/*PA/PAD all off*/
+	rtl_set_rfreg(hw, RF90_PATH_B, 0xdf, RFREG_OFFSET_MASK, 0x980);
+	rtl_set_rfreg(hw, RF90_PATH_B, 0x56, RFREG_OFFSET_MASK, 0x51000);
+
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
+
+	/*IQK Setting*/
+	rtl_set_bbreg(hw, RTx_IQK, MASKDWORD, 0x01007c00);
+	rtl_set_bbreg(hw, RRx_IQK, MASKDWORD, 0x01004800);
+
+	/*path a IQK setting*/
+	rtl_set_bbreg(hw, RTx_IQK_Tone_A, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RRx_IQK_Tone_A, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RTx_IQK_Tone_B, MASKDWORD, 0x18008c1c);
+	rtl_set_bbreg(hw, RRx_IQK_Tone_B, MASKDWORD, 0x38008c1c);
+
+	rtl_set_bbreg(hw, RTx_IQK_PI_B, MASKDWORD, 0x82160c1f);
+	rtl_set_bbreg(hw, RRx_IQK_PI_B, MASKDWORD, 0x68160c1f);
+
+	/*LO calibration Setting*/
+	rtl_set_bbreg(hw, RIQK_AGC_Rsp, MASKDWORD, 0x0046a911);
+
+	/*one shot, path A LOK & iqk*/
+	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xfa000000);
+	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xf8000000);
+
+	mdelay(IQK_DELAY_TIME);
+
+	/* Check failed */
+	reg_eac = rtl_get_bbreg(hw, RRx_Power_After_IQK_A_2, MASKDWORD);
+	reg_eb4 = rtl_get_bbreg(hw, RTx_Power_Before_IQK_B, MASKDWORD);
+	reg_ebc = rtl_get_bbreg(hw, RTx_Power_After_IQK_B, MASKDWORD);
+
+	if (!(reg_eac & BIT(31)) &&
+	    (((reg_eb4 & 0x03FF0000) >> 16) != 0x142) &&
+	    (((reg_ebc & 0x03FF0000) >> 16) != 0x42)) {
+		result |= 0x01;
+	} else {
+		/*	PA/PAD controlled by 0x0 */
+		rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+		rtl_set_rfreg(hw, RF90_PATH_B, 0xdf, RFREG_OFFSET_MASK, 0x180);
+		return result;
+	}
+
+	u32temp = 0x80007C00 | (reg_eb4 & 0x3FF0000) |
+		  ((reg_ebc & 0x3FF0000) >> 16);
+	rtl_set_bbreg(hw, RTx_IQK, MASKDWORD, u32temp);
+	/*RX IQK*/
+	/*Modify RX IQK mode table*/
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+	rtl_set_rfreg(hw, RF90_PATH_B, RF_WE_LUT, RFREG_OFFSET_MASK, 0x800a0);
+
+	rtl_set_rfreg(hw, RF90_PATH_B, RF_RCK_OS, RFREG_OFFSET_MASK, 0x30000);
+	rtl_set_rfreg(hw, RF90_PATH_B, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0000f);
+	rtl_set_rfreg(hw, RF90_PATH_B, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xf7ffa);
+
+	/*PA/PAD all off*/
+	rtl_set_rfreg(hw, RF90_PATH_B, 0xdf, RFREG_OFFSET_MASK, 0x980);
+	rtl_set_rfreg(hw, RF90_PATH_B, 0x56, RFREG_OFFSET_MASK, 0x51000);
+
+	/*enter IQK mode*/
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
+
+	/*IQK Setting*/
+	rtl_set_bbreg(hw, RRx_IQK, MASKDWORD, 0x01004800);
+
+	/*path b IQK setting*/
+	rtl_set_bbreg(hw, RTx_IQK_Tone_A, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RRx_IQK_Tone_A, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RTx_IQK_Tone_B, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RRx_IQK_Tone_B, MASKDWORD, 0x18008c1c);
+
+	rtl_set_bbreg(hw, RTx_IQK_PI_B, MASKDWORD, 0x82160c1f);
+	rtl_set_bbreg(hw, RRx_IQK_PI_B, MASKDWORD, 0x28160c1f);
+
+	/*LO calibration Setting*/
+	rtl_set_bbreg(hw, RIQK_AGC_Rsp, MASKDWORD, 0x0046a891);
+	/*one shot, path A LOK & iqk*/
+	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xfa000000);
+	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xf8000000);
+
+	mdelay(IQK_DELAY_TIME);
+	/*Check failed*/
+	reg_eac = rtl_get_bbreg(hw, RRx_Power_After_IQK_A_2, MASKDWORD);
+	reg_ec4 = rtl_get_bbreg(hw, RRx_Power_Before_IQK_B_2, MASKDWORD);
+	reg_ecc = rtl_get_bbreg(hw, RRx_Power_After_IQK_B_2, MASKDWORD);
+	/*PA/PAD controlled by 0x0*/
+	/*leave IQK mode*/
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+	rtl_set_rfreg(hw, RF90_PATH_B, 0xdf, RFREG_OFFSET_MASK, 0x180);
+	/*if Tx is OK, check whether Rx is OK*/
+	if (!(reg_eac & BIT(30)) &&
+	    (((reg_ec4 & 0x03FF0000) >> 16) != 0x132) &&
+	    (((reg_ecc & 0x03FF0000) >> 16) != 0x36))
+		result |= 0x02;
+	else
+		RT_TRACE(COMP_RF, DBG_LOUD, ("Path B Rx IQK fail!!\n"));
+
+	return result;
+}
+
+static void _rtl92ee_phy_path_a_fill_iqk_matrix(struct ieee80211_hw *hw,
+						bool b_iqk_ok, long result[][8],
+						u8 final_candidate,
+						bool btxonly)
+{
+	u32 oldval_0, x, tx0_a, reg;
+	long y, tx0_c;
+
+	if (final_candidate == 0xFF) {
+		return;
+	} else if (b_iqk_ok) {
+		oldval_0 = (rtl_get_bbreg(hw, ROFDM0_XATXIQIMBALANCE,
+					  MASKDWORD) >> 22) & 0x3FF;
+		x = result[final_candidate][0];
+		if ((x & 0x00000200) != 0)
+			x = x | 0xFFFFFC00;
+		tx0_a = (x * oldval_0) >> 8;
+		rtl_set_bbreg(hw, ROFDM0_XATXIQIMBALANCE, 0x3FF, tx0_a);
+		rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD, BIT(31),
+			      ((x * oldval_0 >> 7) & 0x1));
+		y = result[final_candidate][1];
+		if ((y & 0x00000200) != 0)
+			y = y | 0xFFFFFC00;
+		tx0_c = (y * oldval_0) >> 8;
+		rtl_set_bbreg(hw, ROFDM0_XCTXAFE, 0xF0000000,
+			      ((tx0_c & 0x3C0) >> 6));
+		rtl_set_bbreg(hw, ROFDM0_XATXIQIMBALANCE, 0x003F0000,
+			      (tx0_c & 0x3F));
+		rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD, BIT(29),
+			      ((y * oldval_0 >> 7) & 0x1));
+
+		if (btxonly)
+			return;
+
+		reg = result[final_candidate][2];
+		rtl_set_bbreg(hw, ROFDM0_XARXIQIMBALANCE, 0x3FF, reg);
+
+		reg = result[final_candidate][3] & 0x3F;
+		rtl_set_bbreg(hw, ROFDM0_XARXIQIMBALANCE, 0xFC00, reg);
+
+		reg = (result[final_candidate][3] >> 6) & 0xF;
+		rtl_set_bbreg(hw, ROFDM0_RXIQEXTANTA, 0xF0000000, reg);
+	}
+}
+
+static void _rtl92ee_phy_path_b_fill_iqk_matrix(struct ieee80211_hw *hw,
+						bool b_iqk_ok, long result[][8],
+						u8 final_candidate,
+						bool btxonly)
+{
+	u32 oldval_1, x, tx1_a, reg;
+	long y, tx1_c;
+
+	if (final_candidate == 0xFF) {
+		return;
+	} else if (b_iqk_ok) {
+		oldval_1 = (rtl_get_bbreg(hw, ROFDM0_XATXIQIMBALANCE,
+					  MASKDWORD) >> 22) & 0x3FF;
+		x = result[final_candidate][4];
+		if ((x & 0x00000200) != 0)
+			x = x | 0xFFFFFC00;
+		tx1_a = (x * oldval_1) >> 8;
+		rtl_set_bbreg(hw, ROFDM0_XATXIQIMBALANCE, 0x3FF, tx1_a);
+		rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD, BIT(27),
+			      ((x * oldval_1 >> 7) & 0x1));
+		y = result[final_candidate][5];
+		if ((y & 0x00000200) != 0)
+			y = y | 0xFFFFFC00;
+		tx1_c = (y * oldval_1) >> 8;
+		rtl_set_bbreg(hw, ROFDM0_XDTXAFE, 0xF0000000,
+			      ((tx1_c & 0x3C0) >> 6));
+		rtl_set_bbreg(hw, ROFDM0_XBTXIQIMBALANCE, 0x003F0000,
+			      (tx1_c & 0x3F));
+		rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD, BIT(25),
+			      ((y * oldval_1 >> 7) & 0x1));
+
+		if (btxonly)
+			return;
+
+		reg = result[final_candidate][6];
+		rtl_set_bbreg(hw, ROFDM0_XBRXIQIMBALANCE, 0x3FF, reg);
+
+		reg = result[final_candidate][7] & 0x3F;
+		rtl_set_bbreg(hw, ROFDM0_XBRXIQIMBALANCE, 0xFC00, reg);
+
+		reg = (result[final_candidate][7] >> 6) & 0xF;
+		rtl_set_bbreg(hw, ROFDM0_AGCRSSITABLE, 0xF0000000, reg);
+	}
+}
+
+static void _rtl92ee_phy_save_adda_registers(struct ieee80211_hw *hw,
+					     u32 *addareg, u32 *addabackup,
+					     u32 registernum)
+{
+	u32 i;
+
+	for (i = 0; i < registernum; i++)
+		addabackup[i] = rtl_get_bbreg(hw, addareg[i], MASKDWORD);
+}
+
+static void _rtl92ee_phy_save_mac_registers(struct ieee80211_hw *hw,
+					    u32 *macreg, u32 *macbackup)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i;
+
+	for (i = 0; i < (IQK_MAC_REG_NUM - 1); i++)
+		macbackup[i] = rtl_read_byte(rtlpriv, macreg[i]);
+
+	macbackup[i] = rtl_read_dword(rtlpriv, macreg[i]);
+}
+
+static void _rtl92ee_phy_reload_adda_registers(struct ieee80211_hw *hw,
+					       u32 *addareg, u32 *addabackup,
+					       u32 regiesternum)
+{
+	u32 i;
+
+	for (i = 0; i < regiesternum; i++)
+		rtl_set_bbreg(hw, addareg[i], MASKDWORD, addabackup[i]);
+}
+
+static void _rtl92ee_phy_reload_mac_registers(struct ieee80211_hw *hw,
+					      u32 *macreg, u32 *macbackup)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i;
+
+	for (i = 0; i < (IQK_MAC_REG_NUM - 1); i++)
+		rtl_write_byte(rtlpriv, macreg[i], (u8) macbackup[i]);
+	rtl_write_dword(rtlpriv, macreg[i], macbackup[i]);
+}
+
+static void _rtl92ee_phy_path_adda_on(struct ieee80211_hw *hw, u32 *addareg,
+				      bool is_patha_on, bool is2t)
+{
+	u32 pathon;
+	u32 i;
+
+	pathon = is_patha_on ? 0x0fc01616 : 0x0fc01616;
+	if (!is2t) {
+		pathon = 0x0fc01616;
+		rtl_set_bbreg(hw, addareg[0], MASKDWORD, 0x0fc01616);
+	} else {
+		rtl_set_bbreg(hw, addareg[0], MASKDWORD, pathon);
+	}
+
+	for (i = 1; i < IQK_ADDA_REG_NUM; i++)
+		rtl_set_bbreg(hw, addareg[i], MASKDWORD, pathon);
+}
+
+static void _rtl92ee_phy_mac_setting_calibration(struct ieee80211_hw *hw,
+						 u32 *macreg, u32 *macbackup)
+{
+	rtl_set_bbreg(hw, 0x520, 0x00ff0000, 0xff);
+}
+
+static void _rtl92ee_phy_path_a_standby(struct ieee80211_hw *hw)
+{
+	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x0);
+	rtl_set_rfreg(hw, RF90_PATH_A, 0, RFREG_OFFSET_MASK, 0x10000);
+	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x80800000);
+}
+
+static bool _rtl92ee_phy_simularity_compare(struct ieee80211_hw *hw,
+					    long result[][8], u8 c1, u8 c2)
+{
+	u32 i, j, diff, simularity_bitmap, bound;
+
+	u8 final_candidate[2] = { 0xFF, 0xFF };
+	bool bresult = true/*, is2t = true*/;
+	s32 tmp1, tmp2;
+
+	bound = 8;
+
+	simularity_bitmap = 0;
+
+	for (i = 0; i < bound; i++) {
+		if ((i == 1) || (i == 3) || (i == 5) || (i == 7)) {
+			if ((result[c1][i] & 0x00000200) != 0)
+				tmp1 = result[c1][i] | 0xFFFFFC00;
+			else
+				tmp1 = result[c1][i];
+
+			if ((result[c2][i] & 0x00000200) != 0)
+				tmp2 = result[c2][i] | 0xFFFFFC00;
+			else
+				tmp2 = result[c2][i];
+		} else {
+			tmp1 = result[c1][i];
+			tmp2 = result[c2][i];
+		}
+
+		diff = (tmp1 > tmp2) ? (tmp1 - tmp2) : (tmp2 - tmp1);
+
+		if (diff > MAX_TOLERANCE) {
+			if ((i == 2 || i == 6) && !simularity_bitmap) {
+				if (result[c1][i] + result[c1][i + 1] == 0)
+					final_candidate[(i / 4)] = c2;
+				else if (result[c2][i] + result[c2][i + 1] == 0)
+					final_candidate[(i / 4)] = c1;
+				else
+					simularity_bitmap |= (1 << i);
+			} else {
+				simularity_bitmap |= (1 << i);
+			}
+		}
+	}
+
+	if (simularity_bitmap == 0) {
+		for (i = 0; i < (bound / 4); i++) {
+			if (final_candidate[i] != 0xFF) {
+				for (j = i * 4; j < (i + 1) * 4 - 2; j++)
+					result[3][j] =
+						result[final_candidate[i]][j];
+				bresult = false;
+			}
+		}
+		return bresult;
+	} else {
+		if (!(simularity_bitmap & 0x03)) {/*path A TX OK*/
+			for (i = 0; i < 2; i++)
+				result[3][i] = result[c1][i];
+		}
+		if (!(simularity_bitmap & 0x0c)) {/*path A RX OK*/
+			for (i = 2; i < 4; i++)
+				result[3][i] = result[c1][i];
+		}
+		if (!(simularity_bitmap & 0x30)) {/*path B TX OK*/
+			for (i = 4; i < 6; i++)
+				result[3][i] = result[c1][i];
+		}
+		if (!(simularity_bitmap & 0xc0)) {/*path B RX OK*/
+			for (i = 6; i < 8; i++)
+				result[3][i] = result[c1][i];
+		}
+		return false;
+	}
+}
+
+static void _rtl92ee_phy_iq_calibrate(struct ieee80211_hw *hw,
+				      long result[][8], u8 t, bool is2t)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u32 i;
+	u8 patha_ok, pathb_ok;
+	u8 tmp_0xc50 = (u8) rtl_get_bbreg(hw, 0xc50, MASKBYTE0);
+	u8 tmp_0xc58 = (u8) rtl_get_bbreg(hw, 0xc58, MASKBYTE0);
+	u32 adda_reg[IQK_ADDA_REG_NUM] = {
+		0x85c, 0xe6c, 0xe70, 0xe74,
+		0xe78, 0xe7c, 0xe80, 0xe84,
+		0xe88, 0xe8c, 0xed0, 0xed4,
+		0xed8, 0xedc, 0xee0, 0xeec
+	};
+
+	u32 iqk_mac_reg[IQK_MAC_REG_NUM] = {
+		0x522, 0x550, 0x551, 0x040
+	};
+
+	u32 iqk_bb_reg[IQK_BB_REG_NUM] = {
+		ROFDM0_TRXPATHENABLE, ROFDM0_TRMUXPAR,
+		RFPGA0_XCD_RFINTERFACESW, 0xb68, 0xb6c,
+		0x870, 0x860,
+		0x864, 0x800
+	};
+
+	const u32 retrycount = 2;
+
+
+	if (t == 0) {
+		_rtl92ee_phy_save_adda_registers(hw, adda_reg,
+						 rtlphy->adda_backup,
+						 IQK_ADDA_REG_NUM);
+		_rtl92ee_phy_save_mac_registers(hw, iqk_mac_reg,
+						rtlphy->iqk_mac_backup);
+		_rtl92ee_phy_save_adda_registers(hw, iqk_bb_reg,
+						 rtlphy->iqk_bb_backup,
+						 IQK_BB_REG_NUM);
+	}
+
+	_rtl92ee_phy_path_adda_on(hw, adda_reg, true, is2t);
+
+	/*BB setting*/
+	rtl_set_bbreg(hw, RFPGA0_RFMOD, BIT(24), 0x00);
+	rtl_set_bbreg(hw, ROFDM0_TRXPATHENABLE, MASKDWORD, 0x03a05600);
+	rtl_set_bbreg(hw, ROFDM0_TRMUXPAR, MASKDWORD, 0x000800e4);
+	rtl_set_bbreg(hw, RFPGA0_XCD_RFINTERFACESW, MASKDWORD, 0x22208200);
+
+	rtl_set_bbreg(hw, RFPGA0_XAB_RFINTERFACESW, BIT(10), 0x01);
+	rtl_set_bbreg(hw, RFPGA0_XAB_RFINTERFACESW, BIT(26), 0x01);
+	rtl_set_bbreg(hw, RFPGA0_XA_RFINTERFACEOE, BIT(10), 0x01);
+	rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE, BIT(10), 0x01);
+
+	_rtl92ee_phy_mac_setting_calibration(hw, iqk_mac_reg,
+					     rtlphy->iqk_mac_backup);
+	/* Page B init*/
+	/* IQ calibration setting*/
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
+	rtl_set_bbreg(hw, RTx_IQK, MASKDWORD, 0x01007c00);
+	rtl_set_bbreg(hw, RRx_IQK, MASKDWORD, 0x01004800);
+
+	for (i = 0; i < retrycount; i++) {
+		patha_ok = _rtl92ee_phy_path_a_iqk(hw, is2t);
+
+		if (patha_ok == 0x01) {
+			RT_TRACE(COMP_RF, DBG_LOUD,
+				 ("Path A Tx IQK Success!!\n"));
+			result[t][0] = (rtl_get_bbreg(hw,
+						      RTx_Power_Before_IQK_A,
+						      MASKDWORD) & 0x3FF0000)
+						      >> 16;
+			result[t][1] = (rtl_get_bbreg(hw, RTx_Power_After_IQK_A,
+						      MASKDWORD) & 0x3FF0000)
+						      >> 16;
+			break;
+		} else {
+			RT_TRACE(COMP_RF, DBG_LOUD,
+				 ("Path A Tx IQK Fail!!, ret = 0x%x\n",
+				  patha_ok));
+		}
+	}
+
+	for (i = 0 ; i < retrycount ; i++) {
+		patha_ok = _rtl92ee_phy_path_a_rx_iqk(hw, is2t);
+
+		if (patha_ok == 0x03) {
+			RT_TRACE(COMP_RF, DBG_LOUD,
+				 ("Path A Rx IQK Success!!\n"));
+			result[t][2] = (rtl_get_bbreg(hw,
+						      RRx_Power_Before_IQK_A_2,
+						      MASKDWORD) & 0x3FF0000)
+						      >> 16;
+			result[t][3] = (rtl_get_bbreg(hw,
+						      RRx_Power_After_IQK_A_2,
+						      MASKDWORD) & 0x3FF0000)
+						      >> 16;
+			break;
+		} else {
+			RT_TRACE(COMP_RF, DBG_LOUD,
+				 ("Path A Rx IQK Fail!!, ret = 0x%x\n",
+				  patha_ok));
+		}
+	}
+
+	if (0x00 == patha_ok)
+		RT_TRACE(COMP_RF, DBG_LOUD, ("Path A IQK failed!!, ret = 0\n"));
+
+	if (is2t) {
+		_rtl92ee_phy_path_a_standby(hw);
+		/* Turn Path B ADDA on */
+		_rtl92ee_phy_path_adda_on(hw, adda_reg, false, is2t);
+
+		/* IQ calibration setting */
+		rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
+		rtl_set_bbreg(hw, RTx_IQK, MASKDWORD, 0x01007c00);
+		rtl_set_bbreg(hw, RRx_IQK, MASKDWORD, 0x01004800);
+
+		for (i = 0 ; i < retrycount ; i++) {
+			pathb_ok = _rtl92ee_phy_path_b_iqk(hw);
+			if (pathb_ok == 0x01) {
+				RT_TRACE(COMP_RF, DBG_LOUD,
+					 ("Path B Tx IQK Success!!\n"));
+				result[t][4] = (rtl_get_bbreg(hw,
+							RTx_Power_Before_IQK_B,
+							MASKDWORD) & 0x3FF0000)
+							>> 16;
+				result[t][5] = (rtl_get_bbreg(hw,
+							RTx_Power_After_IQK_B,
+							MASKDWORD) & 0x3FF0000)
+							>> 16;
+				break;
+			} else {
+				RT_TRACE(COMP_RF, DBG_LOUD,
+					 ("Path B Tx IQK Fail!!, ret = 0x%x\n",
+					   pathb_ok));
+			}
+		}
+
+		for (i = 0 ; i < retrycount ; i++) {
+			pathb_ok = _rtl92ee_phy_path_b_rx_iqk(hw, is2t);
+			if (pathb_ok == 0x03) {
+				RT_TRACE(COMP_RF, DBG_LOUD,
+					 ("Path B Rx IQK Success!!\n"));
+				result[t][6] = (rtl_get_bbreg(hw,
+						       RRx_Power_Before_IQK_B_2,
+						       MASKDWORD) & 0x3FF0000)
+						       >> 16;
+				result[t][7] = (rtl_get_bbreg(hw,
+						       RRx_Power_After_IQK_B_2,
+						       MASKDWORD) & 0x3FF0000)
+						       >> 16;
+				break;
+			} else {
+				RT_TRACE(COMP_RF, DBG_LOUD,
+					 ("Path B Rx IQK Fail!!, ret = 0x%x\n",
+					 pathb_ok));
+			}
+		}
+
+		if (0x00 == pathb_ok)
+			RT_TRACE(COMP_RF, DBG_LOUD,
+				 ("Path B IQK failed!!, ret = 0\n"));
+	}
+	/* Back to BB mode, load original value */
+	RT_TRACE(COMP_RF, DBG_LOUD,
+		 ("IQK:Back to BB mode, load original value!\n"));
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0);
+
+	if (t != 0) {
+		/* Reload ADDA power saving parameters */
+		_rtl92ee_phy_reload_adda_registers(hw, adda_reg,
+						   rtlphy->adda_backup,
+						   IQK_ADDA_REG_NUM);
+
+		/* Reload MAC parameters */
+		_rtl92ee_phy_reload_mac_registers(hw, iqk_mac_reg,
+						  rtlphy->iqk_mac_backup);
+
+		_rtl92ee_phy_reload_adda_registers(hw, iqk_bb_reg,
+						   rtlphy->iqk_bb_backup,
+						   IQK_BB_REG_NUM);
+
+		/* Restore RX initial gain */
+		rtl_set_bbreg(hw, 0xc50, MASKBYTE0, 0x50);
+		rtl_set_bbreg(hw, 0xc50, MASKBYTE0, tmp_0xc50);
+		if (is2t) {
+			rtl_set_bbreg(hw, 0xc50, MASKBYTE0, 0x50);
+			rtl_set_bbreg(hw, 0xc58, MASKBYTE0, tmp_0xc58);
+		}
+
+		/* load 0xe30 IQC default value */
+		rtl_set_bbreg(hw, RTx_IQK_Tone_A, MASKDWORD, 0x01008c00);
+		rtl_set_bbreg(hw, RRx_IQK_Tone_A, MASKDWORD, 0x01008c00);
+	}
+	RT_TRACE(COMP_RF, DBG_LOUD, ("_rtl92ee_phy_iq_calibrate() <==\n"));
+}
+
+static void _rtl92ee_phy_lc_calibrate(struct ieee80211_hw *hw, bool is2t)
+{
+	u8 tmpreg;
+	u32 rf_a_mode = 0, rf_b_mode = 0, lc_cal;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	tmpreg = rtl_read_byte(rtlpriv, 0xd03);
+
+	if ((tmpreg & 0x70) != 0)
+		rtl_write_byte(rtlpriv, 0xd03, tmpreg & 0x8F);
+	else
+		rtl_write_byte(rtlpriv, REG_TXPAUSE, 0xFF);
+
+	if ((tmpreg & 0x70) != 0) {
+		rf_a_mode = rtl_get_rfreg(hw, RF90_PATH_A, 0x00, MASK12BITS);
+
+		if (is2t)
+			rf_b_mode = rtl_get_rfreg(hw, RF90_PATH_B, 0x00,
+						  MASK12BITS);
+
+		rtl_set_rfreg(hw, RF90_PATH_A, 0x00, MASK12BITS,
+			      (rf_a_mode & 0x8FFFF) | 0x10000);
+
+		if (is2t)
+			rtl_set_rfreg(hw, RF90_PATH_B, 0x00, MASK12BITS,
+				      (rf_b_mode & 0x8FFFF) | 0x10000);
+	}
+	lc_cal = rtl_get_rfreg(hw, RF90_PATH_A, 0x18, MASK12BITS);
+
+	rtl_set_rfreg(hw, RF90_PATH_A, 0x18, MASK12BITS, lc_cal | 0x08000);
+
+	mdelay(100);
+
+	if ((tmpreg & 0x70) != 0) {
+		rtl_write_byte(rtlpriv, 0xd03, tmpreg);
+		rtl_set_rfreg(hw, RF90_PATH_A, 0x00, MASK12BITS, rf_a_mode);
+
+		if (is2t)
+			rtl_set_rfreg(hw, RF90_PATH_B, 0x00, MASK12BITS,
+				      rf_b_mode);
+	} else {
+		rtl_write_byte(rtlpriv, REG_TXPAUSE, 0x00);
+	}
+	RT_TRACE(COMP_INIT , DBG_LOUD , ("\n"));
+}
+
+static void _rtl92ee_phy_set_rfpath_switch(struct ieee80211_hw *hw,
+					   bool bmain, bool is2t)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	RT_TRACE(COMP_INIT , DBG_LOUD , ("\n"));
+
+	if (is_hal_stop(rtlhal)) {
+		u8 u1btmp;
+		u1btmp = rtl_read_byte(rtlpriv, REG_LEDCFG0);
+		rtl_write_byte(rtlpriv, REG_LEDCFG0, u1btmp | BIT(7));
+		rtl_set_bbreg(hw, rFPGA0_XAB_RFPARAMETER, BIT(13), 0x01);
+	}
+	if (is2t) {
+		if (bmain)
+			rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE,
+				      BIT(5) | BIT(6), 0x1);
+		else
+			rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE,
+				      BIT(5) | BIT(6), 0x2);
+	} else {
+		rtl_set_bbreg(hw, RFPGA0_XAB_RFINTERFACESW, BIT(8) | BIT(9), 0);
+		rtl_set_bbreg(hw, 0x914, MASKLWORD, 0x0201);
+
+		/* We use the RF definition of MAIN and AUX,
+		 * left antenna and right antenna repectively.
+		 * Default output at AUX.*/
+		if (bmain) {
+			rtl_set_bbreg(hw, RFPGA0_XA_RFINTERFACEOE,
+				      BIT(14) | BIT(13) | BIT(12), 0);
+			rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE,
+				      BIT(5) | BIT(4) | BIT(3), 0);
+			if (rtlefuse->antenna_div_type == CGCS_RX_HW_ANTDIV)
+				rtl_set_bbreg(hw, rConfig_ram64x16, BIT(31), 0);
+		} else {
+			rtl_set_bbreg(hw, RFPGA0_XA_RFINTERFACEOE,
+				      BIT(14) | BIT(13) | BIT(12), 1);
+			rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE,
+				      BIT(5) | BIT(4) | BIT(3), 1);
+			if (rtlefuse->antenna_div_type == CGCS_RX_HW_ANTDIV)
+				rtl_set_bbreg(hw, rConfig_ram64x16, BIT(31), 1);
+		}
+	}
+}
+
+#undef IQK_ADDA_REG_NUM
+#undef IQK_DELAY_TIME
+
+static u8 rtl92ee_get_rightchnlplace_for_iqk(u8 chnl)
+{
+	u8 channel_all[59] = {
+		1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
+		36, 38, 40, 42, 44, 46, 48, 50, 52, 54, 56, 58,
+		60, 62, 64, 100, 102, 104, 106, 108, 110, 112,
+		114, 116, 118, 120, 122, 124, 126, 128,	130,
+		132, 134, 136, 138, 140, 149, 151, 153, 155,
+		157, 159, 161, 163, 165
+	};
+	u8 place = chnl;
+
+	if (chnl > 14) {
+		for (place = 14; place < sizeof(channel_all); place++) {
+			if (channel_all[place] == chnl)
+				return place - 13;
+		}
+	}
+
+	return 0;
+}
+
+void rtl92ee_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	long result[4][8];
+	u8 i, final_candidate;
+	bool b_patha_ok, b_pathb_ok;
+	long reg_e94, reg_e9c, reg_ea4, reg_eac;
+	long reg_eb4, reg_ebc, reg_ec4, reg_ecc;
+	bool is12simular, is13simular, is23simular;
+	u8 idx;
+	u32 iqk_bb_reg[IQK_BB_REG_NUM] = {
+		ROFDM0_XARXIQIMBALANCE,
+		ROFDM0_XBRXIQIMBALANCE,
+		ROFDM0_ECCATHRESHOLD,
+		ROFDM0_AGCRSSITABLE,
+		ROFDM0_XATXIQIMBALANCE,
+		ROFDM0_XBTXIQIMBALANCE,
+		ROFDM0_XCTXAFE,
+		ROFDM0_XDTXAFE,
+		ROFDM0_RXIQEXTANTA
+	};
+
+	if (b_recovery) {
+		_rtl92ee_phy_reload_adda_registers(hw, iqk_bb_reg,
+						   rtlphy->iqk_bb_backup, 9);
+		return;
+	}
+
+	for (i = 0; i < 8; i++) {
+		result[0][i] = 0;
+		result[1][i] = 0;
+		result[2][i] = 0;
+
+		if ((i == 0) || (i == 2) || (i == 4)  || (i == 6))
+			result[3][i] = 0x100;
+		else
+			result[3][i] = 0;
+	}
+	final_candidate = 0xff;
+	b_patha_ok = false;
+	b_pathb_ok = false;
+	is12simular = false;
+	is23simular = false;
+	is13simular = false;
+	for (i = 0; i < 3; i++) {
+		_rtl92ee_phy_iq_calibrate(hw, result, i, true);
+		if (i == 1) {
+			is12simular = _rtl92ee_phy_simularity_compare(hw,
+								      result,
+								      0, 1);
+			if (is12simular) {
+				final_candidate = 0;
+				break;
+			}
+		}
+
+		if (i == 2) {
+			is13simular = _rtl92ee_phy_simularity_compare(hw,
+								      result,
+								      0, 2);
+			if (is13simular) {
+				final_candidate = 0;
+				break;
+			}
+			is23simular = _rtl92ee_phy_simularity_compare(hw,
+								      result,
+								      1, 2);
+			if (is23simular)
+				final_candidate = 1;
+			else
+				final_candidate = 3;
+		}
+	}
+	for (i = 0; i < 4; i++) {
+		reg_e94 = result[i][0];
+		reg_e9c = result[i][1];
+		reg_ea4 = result[i][2];
+		reg_eac = result[i][3];
+		reg_eb4 = result[i][4];
+		reg_ebc = result[i][5];
+		reg_ec4 = result[i][6];
+		reg_ecc = result[i][7];
+	}
+	if (final_candidate != 0xff) {
+		reg_e94 = result[final_candidate][0];
+		rtlphy->reg_e94 = reg_e94;
+		reg_e9c = result[final_candidate][1];
+		rtlphy->reg_e9c = reg_e9c;
+		reg_ea4 = result[final_candidate][2];
+		reg_eac = result[final_candidate][3];
+		reg_eb4 = result[final_candidate][4];
+		rtlphy->reg_eb4 = reg_eb4;
+		reg_ebc = result[final_candidate][5];
+		rtlphy->reg_ebc = reg_ebc;
+		reg_ec4 = result[final_candidate][6];
+		reg_ecc = result[final_candidate][7];
+		b_patha_ok = true;
+		b_pathb_ok = true;
+	} else {
+		rtlphy->reg_e94 = 0x100;
+		rtlphy->reg_eb4 = 0x100;
+		rtlphy->reg_e9c = 0x0;
+		rtlphy->reg_ebc = 0x0;
+	}
+
+	if (reg_e94 != 0) /*&&(reg_ea4 != 0) */
+		_rtl92ee_phy_path_a_fill_iqk_matrix(hw, b_patha_ok, result,
+						    final_candidate,
+						    (reg_ea4 == 0));
+
+	_rtl92ee_phy_path_b_fill_iqk_matrix(hw, b_pathb_ok, result,
+					    final_candidate,
+					    (reg_ec4 == 0));
+
+	idx = rtl92ee_get_rightchnlplace_for_iqk(rtlphy->current_channel);
+
+	/* To Fix BSOD when final_candidate is 0xff
+	 * by sherry 20120321 */
+	if (final_candidate < 4) {
+		for (i = 0; i < IQK_MATRIX_REG_NUM; i++)
+			rtlphy->iqk_matrix_regsetting[idx].value[0][i] =
+				result[final_candidate][i];
+
+		rtlphy->iqk_matrix_regsetting[idx].b_iqk_done = true;
+	}
+	_rtl92ee_phy_save_adda_registers(hw, iqk_bb_reg,
+					 rtlphy->iqk_bb_backup, 9);
+}
+
+void rtl92ee_phy_lc_calibrate(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_hal *rtlhal = &(rtlpriv->rtlhal);
+	u32 timeout = 2000, timecount = 0;
+
+	while (rtlpriv->mac80211.act_scanning && timecount < timeout) {
+		udelay(50);
+		timecount += 50;
+	}
+
+	rtlphy->lck_inprogress = true;
+	RTPRINT(rtlpriv, FINIT, INIT_IQK,
+		"LCK:Start!!! currentband %x delay %d ms\n",
+		 rtlhal->current_bandtype, timecount);
+
+	_rtl92ee_phy_lc_calibrate(hw, false);
+
+	rtlphy->lck_inprogress = false;
+}
+
+void rtl92ee_phy_ap_calibrate(struct ieee80211_hw *hw, char delta)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	if (rtlphy->b_apk_done)
+		return;
+
+	return;
+}
+
+void rtl92ee_phy_set_rfpath_switch(struct ieee80211_hw *hw, bool bmain)
+{
+	_rtl92ee_phy_set_rfpath_switch(hw, bmain, false);
+}
+
+bool rtl92ee_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	bool b_postprocessing = false;
+
+	RT_TRACE(COMP_CMD, DBG_TRACE,
+		 ("-->IO Cmd(%#x), set_io_inprogress(%d)\n",
+		  iotype, rtlphy->set_io_inprogress));
+	do {
+		switch (iotype) {
+		case IO_CMD_RESUME_DM_BY_SCAN:
+			RT_TRACE(COMP_CMD, DBG_TRACE,
+				 ("[IO CMD] Resume DM after scan.\n"));
+			b_postprocessing = true;
+			break;
+		case IO_CMD_PAUSE_BAND0_DM_BY_SCAN:
+			RT_TRACE(COMP_CMD, DBG_TRACE,
+				 ("[IO CMD] Pause DM before scan.\n"));
+			b_postprocessing = true;
+			break;
+		default:
+			RT_TRACE(COMP_ERR, DBG_EMERG,
+				 ("switch case not process\n"));
+			break;
+		}
+	} while (false);
+	if (b_postprocessing && !rtlphy->set_io_inprogress) {
+		rtlphy->set_io_inprogress = true;
+		rtlphy->current_io_type = iotype;
+	} else {
+		return false;
+	}
+	rtl92ee_phy_set_io(hw);
+	RT_TRACE(COMP_CMD, DBG_TRACE, ("IO Type(%#x)\n", iotype));
+	return true;
+}
+
+static void rtl92ee_phy_set_io(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	RT_TRACE(COMP_CMD, DBG_TRACE,
+		 ("--->Cmd(%#x), set_io_inprogress(%d)\n",
+		  rtlphy->current_io_type, rtlphy->set_io_inprogress));
+	switch (rtlphy->current_io_type) {
+	case IO_CMD_RESUME_DM_BY_SCAN:
+		rtl92ee_dm_write_dig(hw , rtlphy->initgain_backup.xaagccore1);
+		rtl92ee_dm_write_cck_cca_thres(hw, rtlphy->initgain_backup.cca);
+		RT_TRACE(COMP_CMD, DBG_TRACE , ("no set txpower\n"));
+		rtl92ee_phy_set_txpower_level(hw, rtlphy->current_channel);
+		break;
+	case IO_CMD_PAUSE_BAND0_DM_BY_SCAN:
+		/* 8192eebt */
+		rtlphy->initgain_backup.xaagccore1 = dm_dig.cur_igvalue;
+		rtl92ee_dm_write_dig(hw, 0x17);
+		rtlphy->initgain_backup.cca = dm_dig.cur_cck_cca_thres;
+		rtl92ee_dm_write_cck_cca_thres(hw, 0x40);
+		break;
+	default:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("switch case not process\n"));
+		break;
+	}
+	rtlphy->set_io_inprogress = false;
+	RT_TRACE(COMP_CMD, DBG_TRACE,
+		 ("(%#x)\n", rtlphy->current_io_type));
+}
+
+static void rtl92ee_phy_set_rf_on(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtl_write_byte(rtlpriv, REG_SPS0_CTRL, 0x2b);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE3);
+	/*rtl_write_byte(rtlpriv, REG_APSD_CTRL, 0x00);*/
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE2);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE3);
+	rtl_write_byte(rtlpriv, REG_TXPAUSE, 0x00);
+}
+
+static void _rtl92ee_phy_set_rf_sleep(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtl_write_byte(rtlpriv, REG_TXPAUSE, 0xFF);
+	rtl_set_rfreg(hw, RF90_PATH_A, 0x00, RFREG_OFFSET_MASK, 0x00);
+
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE2);
+	rtl_write_byte(rtlpriv, REG_SPS0_CTRL, 0x22);
+}
+
+static bool _rtl92ee_phy_set_rf_power_state(struct ieee80211_hw *hw,
+					    enum rf_pwrstate rfpwr_state)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	bool bresult = true;
+	u8 i, queue_id;
+	struct rtl8192_tx_ring *ring = NULL;
+
+	switch (rfpwr_state) {
+	case ERFON:
+		if ((ppsc->rfpwr_state == ERFOFF) &&
+		    RT_IN_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC)) {
+			bool rtstatus;
+			u32 init_count = 0;
+			do {
+				init_count++;
+				RT_TRACE(COMP_RF, DBG_DMESG,
+					 ("IPS Set eRf nic enable\n"));
+				rtstatus = stg_rtl_ps_enable_nic(hw);
+			} while (!rtstatus && (init_count < 10));
+			RT_CLEAR_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
+		} else {
+			RT_TRACE(COMP_RF, DBG_DMESG,
+				 ("Set ERFON sleeped:%d ms\n",
+				  jiffies_to_msecs(jiffies -
+						   ppsc->last_sleep_jiffies)));
+			ppsc->last_awake_jiffies = jiffies;
+			rtl92ee_phy_set_rf_on(hw);
+		}
+		if (mac->link_state == MAC80211_LINKED)
+			rtlpriv->cfg->ops->led_control(hw, LED_CTL_LINK);
+		else
+			rtlpriv->cfg->ops->led_control(hw, LED_CTL_NO_LINK);
+		break;
+	case ERFOFF:
+		for (queue_id = 0, i = 0;
+		     queue_id < RTL_PCI_MAX_TX_QUEUE_COUNT;) {
+			ring = &pcipriv->dev.tx_ring[queue_id];
+			if (skb_queue_len(&ring->queue) == 0) {
+				queue_id++;
+				continue;
+			} else {
+				RT_TRACE(COMP_ERR, DBG_WARNING,
+					 ("eRf Off/Sleep: %d times TcbBusyQueue[%d] =%d before "
+					  "doze!\n", (i + 1), queue_id,
+					  skb_queue_len(&ring->queue)));
+
+				udelay(10);
+				i++;
+			}
+			if (i >= MAX_DOZE_WAITING_TIMES_9x) {
+				RT_TRACE(COMP_ERR, DBG_WARNING,
+					 ("\n ERFSLEEP: %d times TcbBusyQueue[%d] = %d !\n",
+					  MAX_DOZE_WAITING_TIMES_9x,
+					  queue_id,
+					  skb_queue_len(&ring->queue)));
+				break;
+			}
+		}
+
+		if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_HALT_NIC) {
+			RT_TRACE(COMP_RF, DBG_DMESG,
+				 ("IPS Set eRf nic disable\n"));
+			stg_rtl_ps_disable_nic(hw);
+			RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
+		} else {
+			if (ppsc->rfoff_reason == RF_CHANGE_BY_IPS) {
+				rtlpriv->cfg->ops->led_control(hw,
+							       LED_CTL_NO_LINK);
+			} else {
+				rtlpriv->cfg->ops->led_control(hw,
+							     LED_CTL_POWER_OFF);
+			}
+		}
+		break;
+	case ERFSLEEP:
+		if (ppsc->rfpwr_state == ERFOFF)
+			break;
+		for (queue_id = 0, i = 0;
+		     queue_id < RTL_PCI_MAX_TX_QUEUE_COUNT;) {
+			ring = &pcipriv->dev.tx_ring[queue_id];
+			if (skb_queue_len(&ring->queue) == 0) {
+				queue_id++;
+				continue;
+			} else {
+				RT_TRACE(COMP_ERR, DBG_WARNING,
+					 ("eRf Off/Sleep: %d times TcbBusyQueue[%d] =%d before doze!\n",
+					  (i + 1), queue_id,
+					  skb_queue_len(&ring->queue)));
+				udelay(10);
+				i++;
+			}
+			if (i >= MAX_DOZE_WAITING_TIMES_9x) {
+				RT_TRACE(COMP_ERR, DBG_WARNING,
+					 ("\n ERFSLEEP: %d times TcbBusyQueue[%d] = %d !\n",
+					  MAX_DOZE_WAITING_TIMES_9x,
+					  queue_id,
+					  skb_queue_len(&ring->queue)));
+				break;
+			}
+		}
+		RT_TRACE(COMP_RF, DBG_DMESG,
+			 ("Set ERFSLEEP awaked:%d ms\n",
+			  jiffies_to_msecs(jiffies -
+					   ppsc->last_awake_jiffies)));
+		ppsc->last_sleep_jiffies = jiffies;
+		_rtl92ee_phy_set_rf_sleep(hw);
+		break;
+	default:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("switch case not process\n"));
+		bresult = false;
+		break;
+	}
+	if (bresult)
+		ppsc->rfpwr_state = rfpwr_state;
+	return bresult;
+}
+
+bool rtl92ee_phy_set_rf_power_state(struct ieee80211_hw *hw,
+				    enum rf_pwrstate rfpwr_state)
+{
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+
+	bool bresult = false;
+
+	if (rfpwr_state == ppsc->rfpwr_state)
+		return bresult;
+	bresult = _rtl92ee_phy_set_rf_power_state(hw, rfpwr_state);
+	return bresult;
+}
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/phy.h b/drivers/staging/rtl8192ee/rtl8192ee/phy.h
new file mode 100644
index 00000000000..5be6c4866e1
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/phy.h
@@ -0,0 +1,154 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92E_PHY_H__
+#define __RTL92E_PHY_H__
+
+/*It must always set to 4, otherwise read efuse table secquence will be wrong.*/
+#define MAX_TX_COUNT				4
+#define TX_1S					0
+#define TX_2S					1
+#define TX_3S					2
+#define TX_4S					3
+
+#define MAX_POWER_INDEX				0x3f
+
+#define MAX_PRECMD_CNT				16
+#define MAX_RFDEPENDCMD_CNT			16
+#define MAX_POSTCMD_CNT				16
+
+#define MAX_DOZE_WAITING_TIMES_9x		64
+
+#define RT_CANNOT_IO(hw)			false
+#define HIGHPOWER_RADIOA_ARRAYLEN		22
+
+#define IQK_ADDA_REG_NUM			16
+#define IQK_MAC_REG_NUM				4
+#define IQK_BB_REG_NUM				9
+#define MAX_TOLERANCE				5
+#define	IQK_DELAY_TIME				10
+#define	index_mapping_NUM			15
+
+#define	APK_BB_REG_NUM				5
+#define	APK_AFE_REG_NUM				16
+#define	APK_CURVE_REG_NUM			4
+#define	PATH_NUM				2
+
+#define LOOP_LIMIT				5
+#define MAX_STALL_TIME				50
+#define AntennaDiversityValue			0x80
+#define MAX_TXPWR_IDX_NMODE_92S			63
+#define Reset_Cnt_Limit				3
+
+#define RF6052_MAX_PATH				2
+
+#define CT_OFFSET_MAC_ADDR			0X16
+
+#define CT_OFFSET_CCK_TX_PWR_IDX		0x5A
+#define CT_OFFSET_HT401S_TX_PWR_IDX		0x60
+#define CT_OFFSET_HT402S_TX_PWR_IDX_DIFF	0x66
+#define CT_OFFSET_HT20_TX_PWR_IDX_DIFF		0x69
+#define CT_OFFSET_OFDM_TX_PWR_IDX_DIFF		0x6C
+
+#define CT_OFFSET_HT40_MAX_PWR_OFFSET		0x6F
+#define CT_OFFSET_HT20_MAX_PWR_OFFSET		0x72
+
+#define CT_OFFSET_CHANNEL_PLAH			0x75
+#define CT_OFFSET_THERMAL_METER			0x78
+#define CT_OFFSET_RF_OPTION			0x79
+#define CT_OFFSET_VERSION			0x7E
+#define CT_OFFSET_CUSTOMER_ID			0x7F
+
+#define RTL92C_MAX_PATH_NUM			2
+
+enum swchnlcmd_id {
+	CMDID_END,
+	CMDID_SET_TXPOWEROWER_LEVEL,
+	CMDID_BBREGWRITE10,
+	CMDID_WRITEPORT_ULONG,
+	CMDID_WRITEPORT_USHORT,
+	CMDID_WRITEPORT_UCHAR,
+	CMDID_RF_WRITEREG,
+};
+
+struct swchnlcmd {
+	enum swchnlcmd_id cmdid;
+	u32 para1;
+	u32 para2;
+	u32 msdelay;
+};
+
+enum baseband_config_type {
+	BASEBAND_CONFIG_PHY_REG = 0,
+	BASEBAND_CONFIG_AGC_TAB = 1,
+};
+
+enum ant_div_type {
+	NO_ANTDIV = 0xFF,
+	CG_TRX_HW_ANTDIV = 0x01,
+	CGCS_RX_HW_ANTDIV = 0x02,
+	FIXED_HW_ANTDIV = 0x03,
+	CG_TRX_SMART_ANTDIV = 0x04,
+	CGCS_RX_SW_ANTDIV = 0x05,
+
+};
+extern u32 rtl92ee_phy_query_bb_reg(struct ieee80211_hw *hw,
+				    u32 regaddr, u32 bitmask);
+extern void rtl92ee_phy_set_bb_reg(struct ieee80211_hw *hw,
+				   u32 regaddr, u32 bitmask, u32 data);
+extern u32 rtl92ee_phy_query_rf_reg(struct ieee80211_hw *hw,
+				    enum radio_path rfpath, u32 regaddr,
+				    u32 bitmask);
+extern void rtl92ee_phy_set_rf_reg(struct ieee80211_hw *hw,
+				   enum radio_path rfpath, u32 regaddr,
+				   u32 bitmask, u32 data);
+extern bool rtl92ee_phy_mac_config(struct ieee80211_hw *hw);
+extern bool rtl92ee_phy_bb_config(struct ieee80211_hw *hw);
+extern bool rtl92ee_phy_rf_config(struct ieee80211_hw *hw);
+extern void rtl92ee_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw);
+extern void rtl92ee_phy_get_txpower_level(struct ieee80211_hw *hw,
+					  long *powerlevel);
+extern void rtl92ee_phy_set_txpower_level(struct ieee80211_hw *hw, u8 channel);
+extern void rtl92ee_phy_scan_operation_backup(struct ieee80211_hw *hw,
+					      u8 operation);
+extern void rtl92ee_phy_set_bw_mode_callback(struct ieee80211_hw *hw);
+extern void rtl92ee_phy_set_bw_mode(struct ieee80211_hw *hw,
+				    enum nl80211_channel_type ch_type);
+extern void rtl92ee_phy_sw_chnl_callback(struct ieee80211_hw *hw);
+extern u8 rtl92ee_phy_sw_chnl(struct ieee80211_hw *hw);
+extern void rtl92ee_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery);
+void rtl92ee_phy_ap_calibrate(struct ieee80211_hw *hw, char delta);
+void rtl92ee_phy_lc_calibrate(struct ieee80211_hw *hw);
+void rtl92ee_phy_set_rfpath_switch(struct ieee80211_hw *hw, bool bmain);
+bool rtl92ee_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
+					   enum radio_path rfpath);
+bool rtl92ee_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype);
+extern bool rtl92ee_phy_set_rf_power_state(struct ieee80211_hw *hw,
+					   enum rf_pwrstate rfpwr_state);
+#endif
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/pwrseq.c b/drivers/staging/rtl8192ee/rtl8192ee/pwrseq.c
new file mode 100644
index 00000000000..08a2df8dfd2
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/pwrseq.c
@@ -0,0 +1,108 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "pwrseqcmd.h"
+#include "pwrseq.h"
+
+
+/*
+    drivers should parse below arrays and do the corresponding actions
+*/
+/*3 Power on  Array*/
+struct wlan_pwr_cfg rtl8192E_power_on_flow[RTL8192E_TRANS_CARDEMU_TO_ACT_STEPS +
+					   RTL8192E_TRANS_END_STEPS] = {
+	RTL8192E_TRANS_CARDEMU_TO_ACT
+	RTL8192E_TRANS_END
+};
+
+/*3Radio off GPIO Array */
+struct wlan_pwr_cfg rtl8192E_radio_off_flow[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS
+					    + RTL8192E_TRANS_END_STEPS] = {
+	RTL8192E_TRANS_ACT_TO_CARDEMU
+	RTL8192E_TRANS_END
+};
+
+/*3Card Disable Array*/
+struct wlan_pwr_cfg rtl8192E_card_disable_flow
+					[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS +
+					 RTL8192E_TRANS_CARDEMU_TO_PDN_STEPS +
+					 RTL8192E_TRANS_END_STEPS] = {
+	RTL8192E_TRANS_ACT_TO_CARDEMU
+	RTL8192E_TRANS_CARDEMU_TO_CARDDIS
+	RTL8192E_TRANS_END
+};
+
+/*3 Card Enable Array*/
+struct wlan_pwr_cfg rtl8192E_card_enable_flow
+					[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS +
+					 RTL8192E_TRANS_CARDEMU_TO_PDN_STEPS +
+					 RTL8192E_TRANS_END_STEPS] = {
+	RTL8192E_TRANS_CARDDIS_TO_CARDEMU
+	RTL8192E_TRANS_CARDEMU_TO_ACT
+	RTL8192E_TRANS_END
+};
+
+/*3Suspend Array*/
+struct wlan_pwr_cfg rtl8192E_suspend_flow[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS +
+					  RTL8192E_TRANS_CARDEMU_TO_SUS_STEPS +
+					  RTL8192E_TRANS_END_STEPS] = {
+	RTL8192E_TRANS_ACT_TO_CARDEMU
+	RTL8192E_TRANS_CARDEMU_TO_SUS
+	RTL8192E_TRANS_END
+};
+
+/*3 Resume Array*/
+struct wlan_pwr_cfg rtl8192E_resume_flow[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS +
+					 RTL8192E_TRANS_CARDEMU_TO_SUS_STEPS +
+					 RTL8192E_TRANS_END_STEPS] = {
+	RTL8192E_TRANS_SUS_TO_CARDEMU
+	RTL8192E_TRANS_CARDEMU_TO_ACT
+	RTL8192E_TRANS_END
+};
+
+/*3HWPDN Array*/
+struct wlan_pwr_cfg rtl8192E_hwpdn_flow[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS +
+					RTL8192E_TRANS_CARDEMU_TO_PDN_STEPS +
+					RTL8192E_TRANS_END_STEPS] = {
+	RTL8192E_TRANS_ACT_TO_CARDEMU
+	RTL8192E_TRANS_CARDEMU_TO_PDN
+	RTL8192E_TRANS_END
+};
+
+/*3 Enter LPS */
+struct wlan_pwr_cfg rtl8192E_enter_lps_flow[RTL8192E_TRANS_ACT_TO_LPS_STEPS +
+					    RTL8192E_TRANS_END_STEPS] = {
+	/*FW behavior*/
+	RTL8192E_TRANS_ACT_TO_LPS
+	RTL8192E_TRANS_END
+};
+
+/*3 Leave LPS */
+struct wlan_pwr_cfg rtl8192E_leave_lps_flow[RTL8192E_TRANS_LPS_TO_ACT_STEPS +
+					    RTL8192E_TRANS_END_STEPS] = {
+	/*FW behavior*/
+	RTL8192E_TRANS_LPS_TO_ACT
+	RTL8192E_TRANS_END
+};
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/pwrseq.h b/drivers/staging/rtl8192ee/rtl8192ee/pwrseq.h
new file mode 100644
index 00000000000..52889721322
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/pwrseq.h
@@ -0,0 +1,355 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92E_PWRSEQ_H__
+#define __RTL92E_PWRSEQ_H__
+
+#include "pwrseqcmd.h"
+/*
+	Check document WM-20110607-Paul-RTL8192E_Power_Architecture-R02.vsd
+	There are 6 HW Power States:
+	0: POFF--Power Off
+	1: PDN--Power Down
+	2: CARDEMU--Card Emulation
+	3: ACT--Active Mode
+	4: LPS--Low Power State
+	5: SUS--Suspend
+
+	The transision from different states are defined below
+	TRANS_CARDEMU_TO_ACT
+	TRANS_ACT_TO_CARDEMU
+	TRANS_CARDEMU_TO_SUS
+	TRANS_SUS_TO_CARDEMU
+	TRANS_CARDEMU_TO_PDN
+	TRANS_ACT_TO_LPS
+	TRANS_LPS_TO_ACT
+
+	TRANS_END
+	PWR SEQ Version: rtl8192E_PwrSeq_V09.h
+*/
+
+#define	RTL8192E_TRANS_CARDEMU_TO_ACT_STEPS	18
+#define	RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS	18
+#define	RTL8192E_TRANS_CARDEMU_TO_SUS_STEPS	18
+#define	RTL8192E_TRANS_SUS_TO_CARDEMU_STEPS	18
+#define	RTL8192E_TRANS_CARDEMU_TO_PDN_STEPS	18
+#define	RTL8192E_TRANS_PDN_TO_CARDEMU_STEPS	18
+#define	RTL8192E_TRANS_ACT_TO_LPS_STEPS		23
+#define	RTL8192E_TRANS_LPS_TO_ACT_STEPS		23
+#define	RTL8192E_TRANS_END_STEPS		1
+
+
+#define RTL8192E_TRANS_CARDEMU_TO_ACT					\
+	/* format */							\
+	/* comments here */						\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },*/\
+	/* disable HWPDN 0x04[15]=0*/					\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(7), 0},			\
+	/* disable SW LPS 0x04[10]=0*/					\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(2), 0},			\
+	/* disable WL suspend*/						\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, (BIT(4)|BIT(3)), 0},		\
+	/* wait till 0x04[17] = 1    power ready*/			\
+	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_POLLING, BIT(1), BIT(1)},		\
+	/* release WLON reset  0x04[16]=1*/				\
+	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(0), BIT(0)},		\
+	/* polling until return 0*/					\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(0), BIT(0)},		\
+	/**/								\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_POLLING, BIT(0), 0},
+
+
+#define RTL8192E_TRANS_ACT_TO_CARDEMU					\
+	/* format */							\
+	/* comments here */						\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },*/\
+	/*0x1F[7:0] = 0 turn off RF*/					\
+	{0x001F, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, 0xFF, 0},			\
+	/*0x4C[23]=0x4E[7]=0, switch DPDT_SEL_P output from register 0x65[2] */\
+	{0x004E, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(7), 0},			\
+	/*0x04[9] = 1 turn off MAC by HW state machine*/		\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(1), BIT(1)},		\
+	/*wait till 0x04[9] = 0 polling until return 0 to disable*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_POLLING, BIT(1), 0},
+
+
+#define RTL8192E_TRANS_CARDEMU_TO_SUS					\
+	/* format */							\
+	/* comments here */						\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },*/\
+	/*0x04[12:11] = 2b'11 enable WL suspend for PCIe*/		\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(4) | BIT(3), (BIT(4) | BIT(3))},\
+	/*0x04[12:11] = 2b'01 enable WL suspend*/			\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
+	 PWR_INTF_USB_MSK | PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC,	\
+	 PWR_CMD_WRITE, BIT(3)|BIT(4), BIT(3)},				\
+	/*0x04[12:11] = 2b'11 enable WL suspend for PCIe*/		\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(3) | BIT(4), BIT(3) | BIT(4)},\
+	/*Set SDIO suspend local register*/				\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_SDIO , PWR_CMD_WRITE, BIT(0), BIT(0)},		\
+	 /*wait power state to suspend*/				\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_SDIO , PWR_CMD_POLLING, BIT(1), 0},
+
+
+#define RTL8192E_TRANS_SUS_TO_CARDEMU					\
+	/* format */							\
+	/* comments here */						\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },*/\
+	/*Set SDIO suspend local register*/				\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_SDIO , PWR_CMD_WRITE, BIT(0), 0},			\
+	/*wait power state to suspend*/					\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_SDIO , PWR_CMD_POLLING, BIT(1), BIT(1)},		\
+	/*0x04[12:11] = 2b'01enable WL suspend*/			\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(3) | BIT(4), 0},
+
+
+#define RTL8192E_TRANS_CARDEMU_TO_CARDDIS				\
+	/* format */							\
+	/* comments here */						\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },*/\
+	/*0x07=0x20 , SOP option to disable BG/MB*/			\
+	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, 0xFF, 0x20},			\
+	/*Unlock small LDO Register*/					\
+	{0x00CC, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(2), BIT(2)},		\
+	/*Disable small LDO*/						\
+	{0x0011, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(0), 0},			\
+	/*0x04[12:11] = 2b'01 enable WL suspend*/			\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
+	 PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,		\
+	 PWR_CMD_WRITE, BIT(3)|BIT(4), BIT(3)},				\
+	/*0x04[10] = 1, enable SW LPS*/					\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(2), BIT(2)},		\
+	/*Set SDIO suspend local register*/				\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_SDIO , PWR_CMD_WRITE, BIT(0), BIT(0)},		\
+	/*wait power state to suspend*/					\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_SDIO , PWR_CMD_POLLING, BIT(1), 0},
+
+
+#define RTL8192E_TRANS_CARDDIS_TO_CARDEMU				\
+	/* format */							\
+	/* comments here */						\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },*/\
+	/*Set SDIO suspend local register*/				\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_SDIO , PWR_CMD_WRITE, BIT(0), 0},			\
+	/*wait power state to suspend*/					\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_SDIO , PWR_CMD_POLLING, BIT(1), BIT(1)},		\
+	/*Enable small LDO*/						\
+	{0x0011, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(0), BIT(0)},		\
+	/*Lock small LDO Register*/					\
+	{0x00CC, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(2), 0},			\
+	/*0x04[12:11] = 2b'01enable WL suspend*/			\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(3) | BIT(4), 0},
+
+
+#define RTL8192E_TRANS_CARDEMU_TO_PDN					\
+	/* format */							\
+	/* comments here */						\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },*/\
+	/* 0x04[16] = 0*/						\
+	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(0), 0},			\
+	/* 0x04[15] = 1*/						\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(7), BIT(7)},
+
+
+#define RTL8192E_TRANS_PDN_TO_CARDEMU					\
+	/* format */							\
+	/* comments here */						\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },*/\
+	/* 0x04[15] = 0*/						\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(7), 0},
+
+
+#define RTL8192E_TRANS_ACT_TO_LPS					\
+	/* format */							\
+	/* comments here */						\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },*/\
+	/*PCIe DMA stop*/						\
+	{0x0301, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, 0xFF, 0xFF},			\
+	/*Tx Pause*/							\
+	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, 0xFF, 0xFF},			\
+	/*Should be zero if no packet is transmitting*/			\
+	{0x05F8, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_POLLING, 0xFF, 0},			\
+	/*Should be zero if no packet is transmitting*/			\
+	{0x05F9, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_POLLING, 0xFF, 0},			\
+	/*Should be zero if no packet is transmitting*/			\
+	{0x05FA, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_POLLING, 0xFF, 0},			\
+	/*Should be zero if no packet is transmitting*/			\
+	{0x05FB, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_POLLING, 0xFF, 0},			\
+	/*CCK and OFDM are disabled,and clock are gated*/		\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(0), 0},			\
+	/*Delay 1us*/							\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_DELAY, 0, PWRSEQ_DELAY_US},		\
+	/*Whole BB is reset*/						\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(1), 0},			\
+	/*Reset MAC TRX*/						\
+	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, 0xFF, 0x03},			\
+	/*check if removed later*/					\
+	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(1), 0},			\
+	/*When driver enter Sus/ Disable, enable LOP for BT*/		\
+	{0x0093, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, 0xFF, 0x00},			\
+	/*Respond TxOK to scheduler*/					\
+	{0x0553, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(5), BIT(5)},
+
+
+#define RTL8192E_TRANS_LPS_TO_ACT					\
+	/* format */							\
+	/* comments here */						\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },*/\
+	/*SDIO RPWM, For Repeatly In and out, Taggle bit should be changed*/\
+	{0x0080, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	 PWR_BASEADDR_SDIO , PWR_CMD_WRITE, 0xFF, 0x84},			\
+	/*USB RPWM*/							\
+	{0xFE58, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, 0xFF, 0x84},			\
+	/*PCIe RPWM*/							\
+	{0x0361, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, 0xFF, 0x84},			\
+	/*Delay*/							\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_DELAY, 0, PWRSEQ_DELAY_MS},		\
+	/*0x08[4] = 0 switch TSF to 40M*/				\
+	{0x0008, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(4), 0},			\
+	/*Polling 0x109[7]=0  TSF in 40M*/				\
+	{0x0109, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_POLLING, BIT(7), 0},			\
+	/*0x101[1] = 1*/						\
+	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(1), BIT(1)},		\
+	/*0x100[7:0] = 0xFF  enable WMAC TRX*/				\
+	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, 0xFF, 0xFF},			\
+	/* 0x02[1:0] = 2b'11 enable BB macro*/				\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(1) | BIT(0), BIT(1) | BIT(0)},\
+	/*0x522 = 0*/							\
+	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, 0xFF, 0},			\
+	/*Clear ISR*/							\
+	{0x013D, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, 0xFF, 0xFF},
+
+
+#define RTL8192E_TRANS_END						\
+	/* format */							\
+	/* comments here */						\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },*/\
+	{0xFFFF, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	 0, PWR_CMD_END, 0, 0},
+
+extern struct wlan_pwr_cfg rtl8192E_power_on_flow
+					[RTL8192E_TRANS_CARDEMU_TO_ACT_STEPS +
+					 RTL8192E_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8192E_radio_off_flow
+					[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS +
+					 RTL8192E_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8192E_card_disable_flow
+					[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS +
+					 RTL8192E_TRANS_CARDEMU_TO_PDN_STEPS +
+					 RTL8192E_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8192E_card_enable_flow
+					[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS +
+					 RTL8192E_TRANS_CARDEMU_TO_PDN_STEPS +
+					 RTL8192E_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8192E_suspend_flow
+					[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS +
+					 RTL8192E_TRANS_CARDEMU_TO_SUS_STEPS +
+					 RTL8192E_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8192E_resume_flow
+					[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS +
+					 RTL8192E_TRANS_CARDEMU_TO_SUS_STEPS +
+					 RTL8192E_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8192E_hwpdn_flow
+					[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS +
+					 RTL8192E_TRANS_CARDEMU_TO_PDN_STEPS +
+					 RTL8192E_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8192E_enter_lps_flow
+					[RTL8192E_TRANS_ACT_TO_LPS_STEPS +
+					 RTL8192E_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8192E_leave_lps_flow
+					[RTL8192E_TRANS_LPS_TO_ACT_STEPS +
+					 RTL8192E_TRANS_END_STEPS];
+
+
+/* RTL8192EE Power Configuration CMDs for PCIe interface */
+#define Rtl8192E_NIC_PWR_ON_FLOW	rtl8192E_power_on_flow
+#define Rtl8192E_NIC_RF_OFF_FLOW	rtl8192E_radio_off_flow
+#define Rtl8192E_NIC_DISABLE_FLOW	rtl8192E_card_disable_flow
+#define Rtl8192E_NIC_ENABLE_FLOW	rtl8192E_card_enable_flow
+#define Rtl8192E_NIC_SUSPEND_FLOW	rtl8192E_suspend_flow
+#define Rtl8192E_NIC_RESUME_FLOW	rtl8192E_resume_flow
+#define Rtl8192E_NIC_PDN_FLOW		rtl8192E_hwpdn_flow
+#define Rtl8192E_NIC_LPS_ENTER_FLOW	rtl8192E_enter_lps_flow
+#define Rtl8192E_NIC_LPS_LEAVE_FLOW	rtl8192E_leave_lps_flow
+#endif
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/pwrseqcmd.c b/drivers/staging/rtl8192ee/rtl8192ee/pwrseqcmd.c
new file mode 100644
index 00000000000..efb00f62236
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/pwrseqcmd.c
@@ -0,0 +1,139 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "pwrseq.h"
+
+
+/*
+*	Description:
+*		This routine deal with the Power Configuration CMDs
+*		 parsing for RTL8723/RTL8188E Series IC.
+*	Assumption:
+*		We should follow specific format which was released from HW SD.
+*
+*	2011.07.07, added by Roger.
+*/
+bool rtl92e_hal_pwrseqcmdparsing(struct rtl_priv *rtlpriv, u8 cut_version,
+			       u8 fab_version, u8 interface_type,
+			       struct wlan_pwr_cfg pwrcfgcmd[])
+
+{
+	struct wlan_pwr_cfg pwr_cfg_cmd = {0};
+	bool b_polling_bit = false;
+	u32 ary_idx = 0;
+	u8 value = 0;
+	u32 offset = 0;
+	u32 polling_count = 0;
+	u32 max_polling_cnt = 5000;
+
+	do {
+		pwr_cfg_cmd = pwrcfgcmd[ary_idx];
+		RT_TRACE(COMP_INIT, DBG_TRACE,
+			 ("offset(%#x), cut_msk(%#x), fab_msk(%#x), interface_msk(%#x), base(%#x), cmd(%#x), msk(%#x), value(%#x)\n",
+			  GET_PWR_CFG_OFFSET(pwr_cfg_cmd),
+			  GET_PWR_CFG_CUT_MASK(pwr_cfg_cmd),
+			  GET_PWR_CFG_FAB_MASK(pwr_cfg_cmd),
+			  GET_PWR_CFG_INTF_MASK(pwr_cfg_cmd),
+			  GET_PWR_CFG_BASE(pwr_cfg_cmd),
+			  GET_PWR_CFG_CMD(pwr_cfg_cmd),
+			  GET_PWR_CFG_MASK(pwr_cfg_cmd),
+			  GET_PWR_CFG_VALUE(pwr_cfg_cmd)));
+
+		if ((GET_PWR_CFG_FAB_MASK(pwr_cfg_cmd)&fab_version) &&
+		    (GET_PWR_CFG_CUT_MASK(pwr_cfg_cmd)&cut_version) &&
+		    (GET_PWR_CFG_INTF_MASK(pwr_cfg_cmd)&interface_type)) {
+			switch (GET_PWR_CFG_CMD(pwr_cfg_cmd)) {
+			case PWR_CMD_READ:
+				RT_TRACE(COMP_INIT, DBG_TRACE,
+					 ("PWR_CMD_READ\n"));
+				break;
+
+			case PWR_CMD_WRITE:
+				RT_TRACE(COMP_INIT, DBG_TRACE,
+					 ("PWR_CMD_WRITE\n"));
+				offset = GET_PWR_CFG_OFFSET(pwr_cfg_cmd);
+
+				/*Read the value from system register*/
+				value = rtl_read_byte(rtlpriv, offset);
+				value &= (~(GET_PWR_CFG_MASK(pwr_cfg_cmd)));
+				value |= (GET_PWR_CFG_VALUE(pwr_cfg_cmd) &
+					  GET_PWR_CFG_MASK(pwr_cfg_cmd));
+
+				/*Write value back to sytem register*/
+				rtl_write_byte(rtlpriv, offset, value);
+				break;
+
+			case PWR_CMD_POLLING:
+				RT_TRACE(COMP_INIT, DBG_TRACE,
+					 ("PWR_CMD_POLLING\n"));
+				b_polling_bit = false;
+				offset = GET_PWR_CFG_OFFSET(pwr_cfg_cmd);
+
+				do {
+					value = rtl_read_byte(rtlpriv, offset);
+
+					value &= GET_PWR_CFG_MASK(pwr_cfg_cmd);
+					if (value ==
+					    (GET_PWR_CFG_VALUE(pwr_cfg_cmd) &
+					     GET_PWR_CFG_MASK(pwr_cfg_cmd)))
+						b_polling_bit = true;
+					else
+						udelay(10);
+
+					if (polling_count++ > max_polling_cnt) {
+						RT_TRACE(COMP_INIT, DBG_LOUD,
+							 ("polling fail\n"));
+						return false;
+					}
+				} while (!b_polling_bit);
+
+				break;
+
+			case PWR_CMD_DELAY:
+				RT_TRACE(COMP_INIT, DBG_TRACE,
+					 ("PWR_CMD_DELAY\n"));
+				if (GET_PWR_CFG_VALUE(pwr_cfg_cmd) ==
+				    PWRSEQ_DELAY_US)
+					udelay(GET_PWR_CFG_OFFSET(pwr_cfg_cmd));
+				else
+					mdelay(GET_PWR_CFG_OFFSET(pwr_cfg_cmd));
+				break;
+
+			case PWR_CMD_END:
+				RT_TRACE(COMP_INIT, DBG_TRACE,
+					 ("PWR_CMD_END\n"));
+				return true;
+				break;
+
+			default:
+				RT_ASSERT(false, ("Unknown CMD!!\n"));
+				break;
+			}
+		}
+
+		ary_idx++;
+	} while (1);
+	return true;
+}
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/pwrseqcmd.h b/drivers/staging/rtl8192ee/rtl8192ee/pwrseqcmd.h
new file mode 100644
index 00000000000..ec40ea8b8c2
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/pwrseqcmd.h
@@ -0,0 +1,69 @@
+#ifndef __RTL92E_PWRSEQCMD_H__
+#define __RTL92E_PWRSEQCMD_H__
+
+#include "../wifi.h"
+/*---------------------------------------------*/
+/* The value of cmd: 4 bits */
+/*---------------------------------------------*/
+#define	PWR_CMD_READ		0x00
+#define PWR_CMD_WRITE		0x01
+#define PWR_CMD_POLLING		0x02
+#define PWR_CMD_DELAY		0x03
+#define PWR_CMD_END		0x04
+
+/* define the base address of each block */
+#define PWR_BASEADDR_MAC	0x00
+#define PWR_BASEADDR_USB	0x01
+#define PWR_BASEADDR_PCIE	0x02
+#define PWR_BASEADDR_SDIO	0x03
+
+#define	PWR_INTF_SDIO_MSK	BIT(0)
+#define	PWR_INTF_USB_MSK	BIT(1)
+#define	PWR_INTF_PCI_MSK	BIT(2)
+#define	PWR_INTF_ALL_MSK	(BIT(0) | BIT(1) | BIT(2) | BIT(3))
+
+#define	PWR_FAB_TSMC_MSK	BIT(0)
+#define	PWR_FAB_UMC_MSK		BIT(1)
+#define	PWR_FAB_ALL_MSK		(BIT(0) | BIT(1) | BIT(2) | BIT(3))
+
+#define	PWR_CUT_TESTCHIP_MSK	BIT(0)
+#define	PWR_CUT_A_MSK		BIT(1)
+#define	PWR_CUT_B_MSK		BIT(2)
+#define	PWR_CUT_C_MSK		BIT(3)
+#define	PWR_CUT_D_MSK		BIT(4)
+#define	PWR_CUT_E_MSK		BIT(5)
+#define	PWR_CUT_F_MSK		BIT(6)
+#define	PWR_CUT_G_MSK		BIT(7)
+#define	PWR_CUT_ALL_MSK		0xFF
+
+
+enum pwrseq_delay_unit {
+	PWRSEQ_DELAY_US,
+	PWRSEQ_DELAY_MS,
+};
+
+struct wlan_pwr_cfg {
+	u16 offset;
+	u8 cut_msk;
+	u8 fab_msk:4;
+	u8 interface_msk:4;
+	u8 base:4;
+	u8 cmd:4;
+	u8 msk;
+	u8 value;
+};
+
+#define	GET_PWR_CFG_OFFSET(__PWR_CMD)		__PWR_CMD.offset
+#define	GET_PWR_CFG_CUT_MASK(__PWR_CMD)		__PWR_CMD.cut_msk
+#define	GET_PWR_CFG_FAB_MASK(__PWR_CMD)		__PWR_CMD.fab_msk
+#define	GET_PWR_CFG_INTF_MASK(__PWR_CMD)	__PWR_CMD.interface_msk
+#define	GET_PWR_CFG_BASE(__PWR_CMD)		__PWR_CMD.base
+#define	GET_PWR_CFG_CMD(__PWR_CMD)		__PWR_CMD.cmd
+#define	GET_PWR_CFG_MASK(__PWR_CMD)		__PWR_CMD.msk
+#define	GET_PWR_CFG_VALUE(__PWR_CMD)		__PWR_CMD.value
+
+bool rtl92e_hal_pwrseqcmdparsing(struct rtl_priv *rtlpriv, u8 cut_version,
+			      u8 fab_version, u8 interface_type,
+			      struct wlan_pwr_cfg pwrcfgcmd[]);
+
+#endif
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/reg.h b/drivers/staging/rtl8192ee/rtl8192ee/reg.h
new file mode 100644
index 00000000000..08c07577239
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/reg.h
@@ -0,0 +1,2240 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92E_REG_H__
+#define __RTL92E_REG_H__
+
+#define TXPKT_BUF_SELECT			0x69
+#define RXPKT_BUF_SELECT			0xA5
+#define DISABLE_TRXPKT_BUF_ACCESS		0x0
+
+#define REG_SYS_ISO_CTRL			0x0000
+#define REG_SYS_FUNC_EN				0x0002
+#define REG_APS_FSMCO				0x0004
+#define REG_SYS_CLKR				0x0008
+#define REG_9346CR				0x000A
+#define REG_EE_VPD				0x000C
+#define REG_SYS_SWR_CTRL1			0x0010
+#define REG_SPS0_CTRL				0x0011
+#define REG_SYS_SWR_CTRL2			0x0014
+#define REG_SYS_SWR_CTRL3			0x0018
+#define REG_RSV_CTRL				0x001C
+#define REG_RF_CTRL				0x001F
+#define REG_LPLDO_CTRL				0x0023
+#define REG_AFE_CTRL1				0x0024
+#define REG_AFE_XTAL_CTRL			0x0024
+#define REG_AFE_CTRL2				0x0028
+#define REG_MAC_PHY_CTRL			0x002c
+#define REG_AFE_CTRL3				0x002c
+#define REG_EFUSE_CTRL				0x0030
+#define REG_EFUSE_TEST				0x0034
+#define REG_PWR_DATA				0x0038
+#define REG_CAL_TIMER				0x003C
+#define REG_ACLK_MON				0x003E
+#define REG_GPIO_MUXCFG				0x0040
+#define REG_GPIO_IO_SEL				0x0042
+#define REG_MAC_PINMUX_CFG			0x0043
+#define REG_GPIO_PIN_CTRL			0x0044
+#define REG_GPIO_INTM				0x0048
+#define REG_LEDCFG0				0x004C
+#define REG_LEDCFG1				0x004D
+#define REG_LEDCFG2				0x004E
+#define REG_LEDCFG3				0x004F
+#define REG_FSIMR				0x0050
+#define REG_FSISR				0x0054
+#define REG_HSIMR				0x0058
+#define REG_HSISR				0x005c
+#define REG_SDIO_CTRL				0x0070
+#define REG_OPT_CTRL				0x0074
+#define REG_GPIO_OUTPUT				0x006c
+#define REG_AFE_CTRL4				0x0078
+#define REG_MCUFWDL				0x0080
+
+#define REG_HIMR				0x00B0
+#define REG_HISR				0x00B4
+#define REG_HIMRE				0x00B8
+#define REG_HISRE				0x00BC
+
+#define REG_EFUSE_ACCESS			0x00CF
+#define REG_HPON_FSM				0x00EC
+#define REG_SYS_CFG1				0x00F0
+#define REG_SYS_CFG2				0x00FC
+
+
+#define REG_CR					0x0100
+#define REG_PBP					0x0104
+#define REG_PKT_BUFF_ACCESS_CTRL		0x0106
+#define REG_TRXDMA_CTRL				0x010C
+#define REG_TRXFF_BNDY				0x0114
+#define REG_TRXFF_STATUS			0x0118
+#define REG_RXFF_PTR				0x011C
+
+#define REG_CPWM				0x012F
+#define REG_FWIMR				0x0130
+#define REG_FWISR				0x0134
+#define REG_PKTBUF_DBG_CTRL			0x0140
+#define REG_RXPKTBUF_CTRL			0x0142
+#define REG_PKTBUF_DBG_DATA_L			0x0144
+#define REG_PKTBUF_DBG_DATA_H			0x0148
+
+#define REG_TC0_CTRL				0x0150
+#define REG_TC1_CTRL				0x0154
+#define REG_TC2_CTRL				0x0158
+#define REG_TC3_CTRL				0x015C
+#define REG_TC4_CTRL				0x0160
+#define REG_TCUNIT_BASE				0x0164
+#define REG_RSVD3				0x0168
+#define REG_C2HEVT_MSG_NORMAL			0x01A0
+#define REG_C2HEVT_CLEAR			0x01AF
+#define REG_MCUTST_1				0x01c0
+#define REG_MCUTST_WOWLAN			0x01C7
+#define REG_FMETHR				0x01C8
+#define REG_HMETFR				0x01CC
+#define REG_HMEBOX_0				0x01D0
+#define REG_HMEBOX_1				0x01D4
+#define REG_HMEBOX_2				0x01D8
+#define REG_HMEBOX_3				0x01DC
+
+#define REG_LLT_INIT				0x01E0
+
+#define REG_HMEBOX_EXT_0			0x01F0
+#define REG_HMEBOX_EXT_1			0x01F4
+#define REG_HMEBOX_EXT_2			0x01F8
+#define REG_HMEBOX_EXT_3			0x01FC
+
+/*-----------------------------------------------------
+ *
+ *	0x0200h ~ 0x027Fh	TXDMA Configuration
+ *
+ *-----------------------------------------------------*/
+#define REG_RQPN				0x0200
+#define REG_FIFOPAGE				0x0204
+#define REG_DWBCN0_CTRL				0x0208
+#define REG_TXDMA_OFFSET_CHK			0x020C
+#define REG_TXDMA_STATUS			0x0210
+#define REG_RQPN_NPQ				0x0214
+#define REG_AUTO_LLT				0x0224
+#define REG_DWBCN1_CTRL				0x0228
+
+/*-----------------------------------------------------
+ *
+ *	0x0280h ~ 0x02FFh	RXDMA Configuration
+ *
+ *-----------------------------------------------------*/
+#define REG_RXDMA_AGG_PG_TH			0x0280
+#define REG_FW_UPD_RDPTR			0x0284
+#define REG_RXDMA_CONTROL			0x0286
+#define REG_RXPKT_NUM				0x0287
+#define REG_RXDMA_STATUS			0x0288
+#define REG_RXDMA_PRO				0x0290
+#define REG_EARLY_MODE_CONTROL			0x02BC
+#define REG_RSVD5				0x02F0
+#define REG_RSVD6				0x02F4
+
+/*-----------------------------------------------------
+ *
+ *	0x0300h ~ 0x03FFh	PCIe
+ *
+ *-----------------------------------------------------*/
+#define	REG_PCIE_CTRL_REG			0x0300
+#define	REG_INT_MIG				0x0304
+#define	REG_BCNQ_DESA				0x0308
+#define	REG_MGQ_DESA				0x0310
+#define	REG_VOQ_DESA				0x0318
+#define	REG_VIQ_DESA				0x0320
+#define	REG_BEQ_DESA				0x0328
+#define	REG_BKQ_DESA				0x0330
+#define	REG_RX_DESA				0x0338
+#define	REG_HQ0_DESA				0x0340
+#define	REG_HQ1_DESA				0x0348
+#define	REG_HQ2_DESA				0x0350
+#define	REG_HQ3_DESA				0x0358
+#define	REG_HQ4_DESA				0x0360
+#define	REG_HQ5_DESA				0x0368
+#define	REG_HQ6_DESA				0x0370
+#define	REG_HQ7_DESA				0x0378
+#define	REG_MGQ_TXBD_NUM			0x0380
+#define	REG_RX_RXBD_NUM				0x0382
+#define	REG_VOQ_TXBD_NUM			0x0384
+#define	REG_VIQ_TXBD_NUM			0x0386
+#define	REG_BEQ_TXBD_NUM			0x0388
+#define	REG_BKQ_TXBD_NUM			0x038A
+#define	REG_HI0Q_TXBD_NUM			0x038C
+#define	REG_HI1Q_TXBD_NUM			0x038E
+#define	REG_HI2Q_TXBD_NUM			0x0390
+#define	REG_HI3Q_TXBD_NUM			0x0392
+#define	REG_HI4Q_TXBD_NUM			0x0394
+#define	REG_HI5Q_TXBD_NUM			0x0396
+#define	REG_HI6Q_TXBD_NUM			0x0398
+#define	REG_HI7Q_TXBD_NUM			0x039A
+#define	REG_TSFTIMER_HCI			0x039C
+/*Read Write Point*/
+#define	REG_VOQ_TXBD_IDX			0x03A0
+#define	REG_VIQ_TXBD_IDX			0x03A4
+#define	REG_BEQ_TXBD_IDX			0x03A8
+#define	REG_BKQ_TXBD_IDX			0x03AC
+#define	REG_MGQ_TXBD_IDX			0x03B0
+#define	REG_RXQ_TXBD_IDX			0x03B4
+
+#define	REG_HI0Q_TXBD_IDX			0x03B8
+#define	REG_HI1Q_TXBD_IDX			0x03BC
+#define	REG_HI2Q_TXBD_IDX			0x03C0
+#define	REG_HI3Q_TXBD_IDX			0x03C4
+
+#define	REG_HI4Q_TXBD_IDX			0x03C8
+#define	REG_HI5Q_TXBD_IDX			0x03CC
+#define	REG_HI6Q_TXBD_IDX			0x03D0
+#define	REG_HI7Q_TXBD_IDX			0x03D4
+#define	REG_PCIE_HCPWM				0x03D8
+#define	REG_PCIE_CTRL2				0x03DB
+#define	REG_PCIE_HRPWM				0x03DC
+#define	REG_H2C_MSG_DRV2FW_INFO			0x03E0
+#define	REG_PCIE_C2H_MSG_REQUEST		0x03E4
+#define	REG_BACKDOOR_DBI_WDATA			0x03E8
+#define	REG_BACKDOOR_DBI_RDATA			0x03EC
+#define	REG_BACKDOOR_DBI_DATA			0x03F0
+#define	REG_MDIO				0x03F4
+#define	REG_MDIO_DATA				0x03F8
+
+#define	REG_HDAQ_DESA_NODEF			0x0000
+#define	REG_CMDQ_DESA_NODEF			0x0000
+/* spec version 11
+ *-----------------------------------------------------
+ *
+ *	0x0400h ~ 0x047Fh	Protocol Configuration
+ *
+ *-----------------------------------------------------*/
+#define REG_VOQ_INFORMATION			0x0400
+#define REG_VIQ_INFORMATION			0x0404
+#define REG_BEQ_INFORMATION			0x0408
+#define REG_BKQ_INFORMATION			0x040C
+#define REG_MGQ_INFORMATION			0x0410
+#define REG_HGQ_INFORMATION			0x0414
+#define REG_BCNQ_INFORMATION			0x0418
+#define REG_TXPKT_EMPTY				0x041A
+
+
+#define REG_FWHW_TXQ_CTRL			0x0420
+#define REG_HWSEQ_CTRL				0x0423
+#define REG_BCNQ_BDNY				0x0424
+#define REG_MGQ_BDNY				0x0425
+#define REG_LIFECTRL_CTRL			0x0426
+#define REG_MULTI_BCNQ_OFFSET			0x0427
+#define REG_SPEC_SIFS				0x0428
+#define REG_RETRY_LIMIT				0x042A
+#define REG_TXBF_CTRL				0x042C
+#define REG_DARFRC				0x0430
+#define REG_RARFRC				0x0438
+#define REG_RRSR				0x0440
+#define REG_ARFR0				0x0444
+#define REG_ARFR1				0x044C
+#define REG_AMPDU_MAX_TIME			0x0456
+#define REG_BCNQ1_BDNY				0x0457
+#define REG_AGGLEN_LMT				0x0458
+#define REG_AMPDU_MIN_SPACE			0x045C
+#define REG_TXPKTBUF_WMAC_LBK_BF_HD		0x045D
+#define REG_NDPA_OPT_CTRL			0x045F
+#define REG_FAST_EDCA_CTRL			0x0460
+#define REG_RD_RESP_PKT_TH			0x0463
+#define REG_POWER_STAGE1			0x04B4
+#define REG_POWER_STAGE2			0x04B8
+#define REG_AMPDU_BURST_MODE			0x04BC
+#define REG_PKT_VO_VI_LIFE_TIME			0x04C0
+#define REG_PKT_BE_BK_LIFE_TIME			0x04C2
+#define REG_STBC_SETTING			0x04C4
+#define REG_PROT_MODE_CTRL			0x04C8
+#define REG_MAX_AGGR_NUM			0x04CA
+#define REG_RTS_MAX_AGGR_NUM			0x04CB
+#define REG_BAR_MODE_CTRL			0x04CC
+#define REG_RA_TRY_RATE_AGG_LMT			0x04CF
+#define REG_MACID_PKT_DROP0			0x04D0
+
+/*-----------------------------------------------------
+ *
+ *	0x0500h ~ 0x05FFh	EDCA Configuration
+ *
+ *-----------------------------------------------------*/
+#define REG_EDCA_VO_PARAM			0x0500
+#define REG_EDCA_VI_PARAM			0x0504
+#define REG_EDCA_BE_PARAM			0x0508
+#define REG_EDCA_BK_PARAM			0x050C
+#define REG_BCNTCFG				0x0510
+#define REG_PIFS				0x0512
+#define REG_RDG_PIFS				0x0513
+#define REG_SIFS_CTX				0x0514
+#define REG_SIFS_TRX				0x0516
+#define REG_AGGR_BREAK_TIME			0x051A
+#define REG_SLOT				0x051B
+#define REG_TX_PTCL_CTRL			0x0520
+#define REG_TXPAUSE				0x0522
+#define REG_DIS_TXREQ_CLR			0x0523
+#define REG_RD_CTRL				0x0524
+
+#define REG_TBTT_PROHIBIT			0x0540
+#define REG_RD_NAV_NXT				0x0544
+#define REG_NAV_PROT_LEN			0x0546
+#define REG_BCN_CTRL				0x0550
+#define REG_BCN_CTRL_1				0x0551
+#define REG_MBID_NUM				0x0552
+#define REG_DUAL_TSF_RST			0x0553
+#define REG_BCN_INTERVAL			0x0554
+#define REG_DRVERLYINT				0x0558
+#define REG_BCNDMATIM				0x0559
+#define REG_ATIMWND				0x055A
+#define REG_BCN_MAX_ERR				0x055D
+#define REG_RXTSF_OFFSET_CCK			0x055E
+#define REG_RXTSF_OFFSET_OFDM			0x055F
+#define REG_TSFTR				0x0560
+#define REG_CTWND				0x0572
+#define REG_PSTIMER				0x0580
+#define REG_TIMER0				0x0584
+#define REG_TIMER1				0x0588
+#define REG_BCN_PREDL_ITV			0x058F
+#define REG_ACMHWCTRL				0x05C0
+
+/*-----------------------------------------------------
+ *
+ *	0x0600h ~ 0x07FFh	WMAC Configuration
+ *
+ *-----------------------------------------------------*/
+#define REG_MAC_CR				0x0600
+#define REG_BWOPMODE				0x0603
+#define REG_TCR					0x0604
+#define REG_RCR					0x0608
+#define REG_RX_PKT_LIMIT			0x060C
+#define REG_RX_DLK_TIME				0x060D
+#define REG_RX_DRVINFO_SZ			0x060F
+
+#define REG_MACID				0x0610
+#define REG_BSSID				0x0618
+#define REG_MAR					0x0620
+#define REG_MBIDCAMCFG				0x0628
+
+#define REG_USTIME_EDCA				0x0638
+#define REG_MAC_SPEC_SIFS			0x063A
+#define REG_RESP_SIFS_CCK			0x063C
+#define REG_RESP_SIFS_OFDM			0x063E
+#define REG_ACKTO				0x0640
+#define REG_CTS2TO				0x0641
+#define REG_EIFS				0x0642
+
+#define	REG_NAV_UPPER				0x0652
+
+/* Security*/
+#define REG_CAMCMD				0x0670
+#define REG_CAMWRITE				0x0674
+#define REG_CAMREAD				0x0678
+#define REG_CAMDBG				0x067C
+#define REG_SECCFG				0x0680
+
+/* Power*/
+#define REG_WOW_CTRL				0x0690
+#define REG_PS_RX_INFO				0x0692
+#define REG_UAPSD_TID				0x0693
+#define REG_WKFMCAM_NUM				0x0698
+#define REG_WKFMCAM_RWD				0x069C
+#define REG_RXFLTMAP0				0x06A0
+#define REG_RXFLTMAP1				0x06A2
+#define REG_RXFLTMAP2				0x06A4
+#define REG_BCN_PSR_RPT				0x06A8
+#define REG_BT_COEX_TABLE			0x06C0
+#define REG_BFMER0_INFO				0x06E4
+#define REG_BFMER1_INFO				0x06EC
+#define REG_CSI_RPT_PARAM_BW20			0x06F4
+#define REG_CSI_RPT_PARAM_BW40			0x06F8
+#define REG_CSI_RPT_PARAM_BW80			0x06FC
+/* Hardware Port 2*/
+#define REG_MACID1				0x0700
+#define REG_BSSID1				0x0708
+#define REG_BFMEE_SEL				0x0714
+#define REG_SND_PTCL_CTRL			0x0718
+
+
+#define	CR9346					REG_9346CR
+#define	MSR					(REG_CR + 2)
+#define	ISR					REG_HISR
+#define	TSFR					REG_TSFTR
+
+#define	MACIDR0					REG_MACID
+#define	MACIDR4					(REG_MACID + 4)
+
+#define PBP					REG_PBP
+
+#define	IDR0					MACIDR0
+#define	IDR4					MACIDR4
+
+#define	UNUSED_REGISTER				0x1BF
+#define	DCAM					UNUSED_REGISTER
+#define	PSR					UNUSED_REGISTER
+#define BBADDR					UNUSED_REGISTER
+#define	PHYDATAR				UNUSED_REGISTER
+
+#define	INVALID_BBRF_VALUE			0x12345678
+
+#define	MAX_MSS_DENSITY_2T			0x13
+#define	MAX_MSS_DENSITY_1T			0x0A
+
+#define	CMDEEPROM_EN				BIT(5)
+#define	CMDEEPROM_SEL				BIT(4)
+#define	CMD9346CR_9356SEL			BIT(4)
+#define	AUTOLOAD_EEPROM				(CMDEEPROM_EN | CMDEEPROM_SEL)
+#define	AUTOLOAD_EFUSE				CMDEEPROM_EN
+
+#define	GPIOSEL_GPIO				0
+#define	GPIOSEL_ENBT				BIT(5)
+
+#define	GPIO_IN					REG_GPIO_PIN_CTRL
+#define	GPIO_OUT				(REG_GPIO_PIN_CTRL + 1)
+#define	GPIO_IO_SEL				(REG_GPIO_PIN_CTRL + 2)
+#define	GPIO_MOD				(REG_GPIO_PIN_CTRL + 3)
+
+#define	MSR_NOLINK				0x00
+#define	MSR_ADHOC				0x01
+#define	MSR_INFRA				0x02
+#define	MSR_AP					0x03
+
+#define	RRSR_RSC_OFFSET				21
+#define	RRSR_SHORT_OFFSET			23
+#define	RRSR_RSC_BW_40M				0x600000
+#define	RRSR_RSC_UPSUBCHNL			0x400000
+#define	RRSR_RSC_LOWSUBCHNL			0x200000
+#define	RRSR_SHORT				0x800000
+#define	RRSR_1M					BIT(0)
+#define	RRSR_2M					BIT(1)
+#define	RRSR_5_5M				BIT(2)
+#define	RRSR_11M				BIT(3)
+#define	RRSR_6M					BIT(4)
+#define	RRSR_9M					BIT(5)
+#define	RRSR_12M				BIT(6)
+#define	RRSR_18M				BIT(7)
+#define	RRSR_24M				BIT(8)
+#define	RRSR_36M				BIT(9)
+#define	RRSR_48M				BIT(10)
+#define	RRSR_54M				BIT(11)
+#define	RRSR_MCS0				BIT(12)
+#define	RRSR_MCS1				BIT(13)
+#define	RRSR_MCS2				BIT(14)
+#define	RRSR_MCS3				BIT(15)
+#define	RRSR_MCS4				BIT(16)
+#define	RRSR_MCS5				BIT(17)
+#define	RRSR_MCS6				BIT(18)
+#define	RRSR_MCS7				BIT(19)
+#define	BRSR_ACKSHORTPMB			BIT(23)
+
+#define	RATR_1M					0x00000001
+#define	RATR_2M					0x00000002
+#define	RATR_55M				0x00000004
+#define	RATR_11M				0x00000008
+#define	RATR_6M					0x00000010
+#define	RATR_9M					0x00000020
+#define	RATR_12M				0x00000040
+#define	RATR_18M				0x00000080
+#define	RATR_24M				0x00000100
+#define	RATR_36M				0x00000200
+#define	RATR_48M				0x00000400
+#define	RATR_54M				0x00000800
+#define	RATR_MCS0				0x00001000
+#define	RATR_MCS1				0x00002000
+#define	RATR_MCS2				0x00004000
+#define	RATR_MCS3				0x00008000
+#define	RATR_MCS4				0x00010000
+#define	RATR_MCS5				0x00020000
+#define	RATR_MCS6				0x00040000
+#define	RATR_MCS7				0x00080000
+#define	RATR_MCS8				0x00100000
+#define	RATR_MCS9				0x00200000
+#define	RATR_MCS10				0x00400000
+#define	RATR_MCS11				0x00800000
+#define	RATR_MCS12				0x01000000
+#define	RATR_MCS13				0x02000000
+#define	RATR_MCS14				0x04000000
+#define	RATR_MCS15				0x08000000
+
+#define RATE_1M					BIT(0)
+#define RATE_2M					BIT(1)
+#define RATE_5_5M				BIT(2)
+#define RATE_11M				BIT(3)
+#define RATE_6M					BIT(4)
+#define RATE_9M					BIT(5)
+#define RATE_12M				BIT(6)
+#define RATE_18M				BIT(7)
+#define RATE_24M				BIT(8)
+#define RATE_36M				BIT(9)
+#define RATE_48M				BIT(10)
+#define RATE_54M				BIT(11)
+#define RATE_MCS0				BIT(12)
+#define RATE_MCS1				BIT(13)
+#define RATE_MCS2				BIT(14)
+#define RATE_MCS3				BIT(15)
+#define RATE_MCS4				BIT(16)
+#define RATE_MCS5				BIT(17)
+#define RATE_MCS6				BIT(18)
+#define RATE_MCS7				BIT(19)
+#define RATE_MCS8				BIT(20)
+#define RATE_MCS9				BIT(21)
+#define RATE_MCS10				BIT(22)
+#define RATE_MCS11				BIT(23)
+#define RATE_MCS12				BIT(24)
+#define RATE_MCS13				BIT(25)
+#define RATE_MCS14				BIT(26)
+#define RATE_MCS15				BIT(27)
+
+#define	RATE_ALL_CCK		(RATR_1M | RATR_2M | RATR_55M | RATR_11M)
+#define	RATE_ALL_OFDM_AG	(RATR_6M | RATR_9M | RATR_12M | RATR_18M |\
+				 RATR_24M | RATR_36M | RATR_48M | RATR_54M)
+#define	RATE_ALL_OFDM_1SS	(RATR_MCS0 | RATR_MCS1 | RATR_MCS2 |\
+				 RATR_MCS3 | RATR_MCS4 | RATR_MCS5 |\
+				 RATR_MCS6 | RATR_MCS7)
+#define	RATE_ALL_OFDM_2SS	(RATR_MCS8 | RATR_MCS9 | RATR_MCS10 |\
+				 RATR_MCS11 | RATR_MCS12 | RATR_MCS13 |\
+				 RATR_MCS14 | RATR_MCS15)
+
+#define	BW_OPMODE_20MHZ				BIT(2)
+#define	BW_OPMODE_5G				BIT(1)
+#define	CAM_VALID				BIT(15)
+#define	CAM_NOTVALID				0x0000
+#define	CAM_USEDK				BIT(5)
+
+#define	CAM_NONE				0x0
+#define	CAM_WEP40				0x01
+#define	CAM_TKIP				0x02
+#define	CAM_AES					0x04
+#define	CAM_WEP104				0x05
+
+#define	TOTAL_CAM_ENTRY				32
+#define	HALF_CAM_ENTRY				16
+
+#define	CAM_WRITE				BIT(16)
+#define	CAM_READ				0x00000000
+#define	CAM_POLLINIG				BIT(31)
+
+#define	SCR_USEDK				0x01
+#define	SCR_TXSEC_ENABLE			0x02
+#define	SCR_RXSEC_ENABLE			0x04
+
+
+/*********************************************
+*       8192EE IMR/ISR bits
+**********************************************/
+#define	IMR_DISABLED				0x0
+/* IMR DW0(0x0060-0063) Bit 0-31 */
+#define	IMR_TIMER2				BIT(31)
+#define	IMR_TIMER1				BIT(30)
+#define	IMR_PSTIMEOUT				BIT(29)
+#define	IMR_GTINT4				BIT(28)
+#define	IMR_GTINT3				BIT(27)
+#define	IMR_TBDER				BIT(26)
+#define	IMR_TBDOK				BIT(25)
+#define	IMR_TSF_BIT32_TOGGLE			BIT(24)
+#define	IMR_BCNDMAINT0				BIT(20)
+#define	IMR_BCNDOK0				BIT(16)
+#define	IMR_BCNDMAINT_E				BIT(14)
+#define	IMR_ATIMEND				BIT(12)
+#define	IMR_HISR1_IND_INT			BIT(11)
+#define	IMR_C2HCMD				BIT(10)
+#define	IMR_CPWM2				BIT(9)
+#define	IMR_CPWM				BIT(8)
+#define	IMR_HIGHDOK				BIT(7)
+#define	IMR_MGNTDOK				BIT(6)
+#define	IMR_BKDOK				BIT(5)
+#define	IMR_BEDOK				BIT(4)
+#define	IMR_VIDOK				BIT(3)
+#define	IMR_VODOK				BIT(2)
+#define	IMR_RDU					BIT(1)
+#define	IMR_ROK					BIT(0)
+
+/* IMR DW1(0x00B4-00B7) Bit 0-31 */
+#define	IMR_MCUERR				BIT(28)
+#define	IMR_BCNDMAINT7				BIT(27)
+#define	IMR_BCNDMAINT6				BIT(26)
+#define	IMR_BCNDMAINT5				BIT(25)
+#define	IMR_BCNDMAINT4				BIT(24)
+#define	IMR_BCNDMAINT3				BIT(23)
+#define	IMR_BCNDMAINT2				BIT(22)
+#define	IMR_BCNDMAINT1				BIT(21)
+#define	IMR_BCNDOK7				BIT(20)
+#define	IMR_BCNDOK6				BIT(19)
+#define	IMR_BCNDOK5				BIT(18)
+#define	IMR_BCNDOK4				BIT(17)
+#define	IMR_BCNDOK3				BIT(16)
+#define	IMR_BCNDOK2				BIT(15)
+#define	IMR_BCNDOK1				BIT(14)
+#define	IMR_ATIMEND_E				BIT(13)
+#define	IMR_TXERR				BIT(11)
+#define	IMR_RXERR				BIT(10)
+#define	IMR_TXFOVW				BIT(9)
+#define	IMR_RXFOVW				BIT(8)
+
+
+#define	HWSET_MAX_SIZE				512
+#define EFUSE_MAX_SECTION			64
+#define EFUSE_REAL_CONTENT_LEN			256
+#define EFUSE_OOB_PROTECT_BYTES			18
+
+
+#define	EEPROM_DEFAULT_TSSI			0x0
+#define EEPROM_DEFAULT_TXPOWERDIFF		0x0
+#define EEPROM_DEFAULT_CRYSTALCAP		0x5
+#define EEPROM_DEFAULT_BOARDTYPE		0x02
+#define EEPROM_DEFAULT_TXPOWER			0x1010
+#define	EEPROM_DEFAULT_HT2T_TXPWR		0x10
+
+#define	EEPROM_DEFAULT_LEGACYHTTXPOWERDIFF	0x3
+#define	EEPROM_DEFAULT_THERMALMETER		0x1A
+#define	EEPROM_DEFAULT_ANTTXPOWERDIFF		0x0
+#define	EEPROM_DEFAULT_TXPWDIFF_CRYSTALCAP	0x5
+#define	EEPROM_DEFAULT_TXPOWERLEVEL		0x22
+#define	EEPROM_DEFAULT_HT40_2SDIFF		0x0
+#define EEPROM_DEFAULT_HT20_DIFF		2
+#define	EEPROM_DEFAULT_LEGACYHTTXPOWERDIFF	0x3
+#define EEPROM_DEFAULT_HT40_PWRMAXOFFSET	0
+#define EEPROM_DEFAULT_HT20_PWRMAXOFFSET	0
+
+#define RF_OPTION1				0x79
+#define RF_OPTION2				0x7A
+#define RF_OPTION3				0x7B
+#define RF_OPTION4				0x7C
+
+#define EEPROM_DEFAULT_PID			0x1234
+#define EEPROM_DEFAULT_VID			0x5678
+#define EEPROM_DEFAULT_CUSTOMERID		0xAB
+#define EEPROM_DEFAULT_SUBCUSTOMERID		0xCD
+#define EEPROM_DEFAULT_VERSION			0
+
+#define	EEPROM_CHANNEL_PLAN_FCC			0x0
+#define	EEPROM_CHANNEL_PLAN_IC			0x1
+#define	EEPROM_CHANNEL_PLAN_ETSI		0x2
+#define	EEPROM_CHANNEL_PLAN_SPAIN		0x3
+#define	EEPROM_CHANNEL_PLAN_FRANCE		0x4
+#define	EEPROM_CHANNEL_PLAN_MKK			0x5
+#define	EEPROM_CHANNEL_PLAN_MKK1		0x6
+#define	EEPROM_CHANNEL_PLAN_ISRAEL		0x7
+#define	EEPROM_CHANNEL_PLAN_TELEC		0x8
+#define	EEPROM_CHANNEL_PLAN_GLOBAL_DOMAIN	0x9
+#define	EEPROM_CHANNEL_PLAN_WORLD_WIDE_13	0xA
+#define	EEPROM_CHANNEL_PLAN_NCC			0xB
+#define	EEPROM_CHANNEL_PLAN_BY_HW_MASK		0x80
+
+#define EEPROM_CID_DEFAULT			0x0
+#define EEPROM_CID_TOSHIBA			0x4
+#define	EEPROM_CID_CCX				0x10
+#define	EEPROM_CID_QMI				0x0D
+#define EEPROM_CID_WHQL				0xFE
+
+#define	RTL8192E_EEPROM_ID			0x8129
+
+#define EEPROM_HPON				0x02
+#define EEPROM_CLK				0x06
+#define EEPROM_TESTR				0x08
+
+
+#define EEPROM_TXPOWERCCK			0x10
+#define	EEPROM_TXPOWERHT40_1S			0x16
+#define EEPROM_TXPOWERHT20DIFF			0x1B
+#define EEPROM_TXPOWER_OFDMDIFF			0x1B
+
+
+
+#define	EEPROM_TX_PWR_INX			0x10
+
+#define	EEPROM_CHANNELPLAN			0xB8
+#define	EEPROM_XTAL_92E				0xB9
+#define	EEPROM_THERMAL_METER_92E		0xBA
+#define	EEPROM_IQK_LCK_92E			0xBB
+
+#define	EEPROM_RF_BOARD_OPTION_92E		0xC1
+#define	EEPROM_RF_FEATURE_OPTION_92E		0xC2
+#define	EEPROM_RF_BT_SETTING_92E		0xC3
+#define	EEPROM_VERSION				0xC4
+#define	EEPROM_CUSTOMER_ID			0xC5
+#define	EEPROM_RF_ANTENNA_OPT_92E		0xC9
+
+#define	EEPROM_MAC_ADDR				0xD0
+#define EEPROM_VID				0xD6
+#define EEPROM_DID				0xD8
+#define EEPROM_SVID				0xDA
+#define EEPROM_SMID				0xDC
+
+#define	STOPBECON				BIT(6)
+#define	STOPHIGHT				BIT(5)
+#define	STOPMGT					BIT(4)
+#define	STOPVO					BIT(3)
+#define	STOPVI					BIT(2)
+#define	STOPBE					BIT(1)
+#define	STOPBK					BIT(0)
+
+#define	RCR_APPFCS				BIT(31)
+#define	RCR_APP_MIC				BIT(30)
+#define	RCR_APP_ICV				BIT(29)
+#define	RCR_APP_PHYST_RXFF			BIT(28)
+#define	RCR_APP_BA_SSN				BIT(27)
+#define	RCR_ENMBID				BIT(24)
+#define	RCR_LSIGEN				BIT(23)
+#define	RCR_MFBEN				BIT(22)
+#define	RCR_HTC_LOC_CTRL			BIT(14)
+#define	RCR_AMF					BIT(13)
+#define	RCR_ACF					BIT(12)
+#define	RCR_ADF					BIT(11)
+#define	RCR_AICV				BIT(9)
+#define	RCR_ACRC32				BIT(8)
+#define	RCR_CBSSID_BCN				BIT(7)
+#define	RCR_CBSSID_DATA				BIT(6)
+#define	RCR_CBSSID				RCR_CBSSID_DATA
+#define	RCR_APWRMGT				BIT(5)
+#define	RCR_ADD3				BIT(4)
+#define	RCR_AB					BIT(3)
+#define	RCR_AM					BIT(2)
+#define	RCR_APM					BIT(1)
+#define	RCR_AAP					BIT(0)
+#define	RCR_MXDMA_OFFSET			8
+#define	RCR_FIFO_OFFSET				13
+
+#define RSV_CTRL				0x001C
+#define RD_CTRL					0x0524
+
+#define REG_USB_INFO				0xFE17
+#define REG_USB_SPECIAL_OPTION			0xFE55
+#define REG_USB_DMA_AGG_TO			0xFE5B
+#define REG_USB_AGG_TO				0xFE5C
+#define REG_USB_AGG_TH				0xFE5D
+
+#define REG_USB_VID				0xFE60
+#define REG_USB_PID				0xFE62
+#define REG_USB_OPTIONAL			0xFE64
+#define REG_USB_CHIRP_K				0xFE65
+#define REG_USB_PHY				0xFE66
+#define REG_USB_MAC_ADDR			0xFE70
+#define REG_USB_HRPWM				0xFE58
+#define REG_USB_HCPWM				0xFE57
+
+#define SW18_FPWM				BIT(3)
+
+#define ISO_MD2PP				BIT(0)
+#define ISO_UA2USB				BIT(1)
+#define ISO_UD2CORE				BIT(2)
+#define ISO_PA2PCIE				BIT(3)
+#define ISO_PD2CORE				BIT(4)
+#define ISO_IP2MAC				BIT(5)
+#define ISO_DIOP				BIT(6)
+#define ISO_DIOE				BIT(7)
+#define ISO_EB2CORE				BIT(8)
+#define ISO_DIOR				BIT(9)
+
+#define PWC_EV25V				BIT(14)
+#define PWC_EV12V				BIT(15)
+
+#define FEN_BBRSTB				BIT(0)
+#define FEN_BB_GLB_RSTn				BIT(1)
+#define FEN_USBA				BIT(2)
+#define FEN_UPLL				BIT(3)
+#define FEN_USBD				BIT(4)
+#define FEN_DIO_PCIE				BIT(5)
+#define FEN_PCIEA				BIT(6)
+#define FEN_PPLL				BIT(7)
+#define FEN_PCIED				BIT(8)
+#define FEN_DIOE				BIT(9)
+#define FEN_CPUEN				BIT(10)
+#define FEN_DCORE				BIT(11)
+#define FEN_ELDR				BIT(12)
+#define FEN_DIO_RF				BIT(13)
+#define FEN_HWPDN				BIT(14)
+#define FEN_MREGEN				BIT(15)
+
+#define PFM_LDALL				BIT(0)
+#define PFM_ALDN				BIT(1)
+#define PFM_LDKP				BIT(2)
+#define PFM_WOWL				BIT(3)
+#define EnPDN					BIT(4)
+#define PDN_PL					BIT(5)
+#define APFM_ONMAC				BIT(8)
+#define APFM_OFF				BIT(9)
+#define APFM_RSM				BIT(10)
+#define AFSM_HSUS				BIT(11)
+#define AFSM_PCIE				BIT(12)
+#define APDM_MAC				BIT(13)
+#define APDM_HOST				BIT(14)
+#define APDM_HPDN				BIT(15)
+#define RDY_MACON				BIT(16)
+#define SUS_HOST				BIT(17)
+#define ROP_ALD					BIT(20)
+#define ROP_PWR					BIT(21)
+#define ROP_SPS					BIT(22)
+#define SOP_MRST				BIT(25)
+#define SOP_FUSE				BIT(26)
+#define SOP_ABG					BIT(27)
+#define SOP_AMB					BIT(28)
+#define SOP_RCK					BIT(29)
+#define SOP_A8M					BIT(30)
+#define XOP_BTCK				BIT(31)
+
+#define ANAD16V_EN				BIT(0)
+#define ANA8M					BIT(1)
+#define MACSLP					BIT(4)
+#define LOADER_CLK_EN				BIT(5)
+#define _80M_SSC_DIS				BIT(7)
+#define _80M_SSC_EN_HO				BIT(8)
+#define PHY_SSC_RSTB				BIT(9)
+#define SEC_CLK_EN				BIT(10)
+#define MAC_CLK_EN				BIT(11)
+#define SYS_CLK_EN				BIT(12)
+#define RING_CLK_EN				BIT(13)
+
+#define	BOOT_FROM_EEPROM			BIT(4)
+#define	EEPROM_EN				BIT(5)
+
+#define AFE_BGEN				BIT(0)
+#define AFE_MBEN				BIT(1)
+#define MAC_ID_EN				BIT(7)
+
+#define WLOCK_ALL				BIT(0)
+#define WLOCK_00				BIT(1)
+#define WLOCK_04				BIT(2)
+#define WLOCK_08				BIT(3)
+#define WLOCK_40				BIT(4)
+#define R_DIS_PRST_0				BIT(5)
+#define R_DIS_PRST_1				BIT(6)
+#define LOCK_ALL_EN				BIT(7)
+
+#define RF_EN					BIT(0)
+#define RF_RSTB					BIT(1)
+#define RF_SDMRSTB				BIT(2)
+
+#define LDA15_EN				BIT(0)
+#define LDA15_STBY				BIT(1)
+#define LDA15_OBUF				BIT(2)
+#define LDA15_REG_VOS				BIT(3)
+#define _LDA15_VOADJ(x)				(((x) & 0x7) << 4)
+
+#define LDV12_EN				BIT(0)
+#define LDV12_SDBY				BIT(1)
+#define LPLDO_HSM				BIT(2)
+#define LPLDO_LSM_DIS				BIT(3)
+#define _LDV12_VADJ(x)				(((x) & 0xF) << 4)
+
+#define XTAL_EN					BIT(0)
+#define XTAL_BSEL				BIT(1)
+#define _XTAL_BOSC(x)				(((x) & 0x3) << 2)
+#define _XTAL_CADJ(x)				(((x) & 0xF) << 4)
+#define XTAL_GATE_USB				BIT(8)
+#define _XTAL_USB_DRV(x)			(((x) & 0x3) << 9)
+#define XTAL_GATE_AFE				BIT(11)
+#define _XTAL_AFE_DRV(x)			(((x) & 0x3) << 12)
+#define XTAL_RF_GATE				BIT(14)
+#define _XTAL_RF_DRV(x)				(((x) & 0x3) << 15)
+#define XTAL_GATE_DIG				BIT(17)
+#define _XTAL_DIG_DRV(x)			(((x) & 0x3) << 18)
+#define XTAL_BT_GATE				BIT(20)
+#define _XTAL_BT_DRV(x)				(((x) & 0x3) << 21)
+#define _XTAL_GPIO(x)				(((x) & 0x7) << 23)
+
+#define CKDLY_AFE				BIT(26)
+#define CKDLY_USB				BIT(27)
+#define CKDLY_DIG				BIT(28)
+#define CKDLY_BT				BIT(29)
+
+#define APLL_EN					BIT(0)
+#define APLL_320_EN				BIT(1)
+#define APLL_FREF_SEL				BIT(2)
+#define APLL_EDGE_SEL				BIT(3)
+#define APLL_WDOGB				BIT(4)
+#define APLL_LPFEN				BIT(5)
+
+#define APLL_REF_CLK_13MHZ			0x1
+#define APLL_REF_CLK_19_2MHZ			0x2
+#define APLL_REF_CLK_20MHZ			0x3
+#define APLL_REF_CLK_25MHZ			0x4
+#define APLL_REF_CLK_26MHZ			0x5
+#define APLL_REF_CLK_38_4MHZ			0x6
+#define APLL_REF_CLK_40MHZ			0x7
+
+#define APLL_320EN				BIT(14)
+#define APLL_80EN				BIT(15)
+#define APLL_1MEN				BIT(24)
+
+#define ALD_EN					BIT(18)
+#define EF_PD					BIT(19)
+#define EF_FLAG					BIT(31)
+
+#define EF_TRPT					BIT(7)
+#define LDOE25_EN				BIT(31)
+
+#define RSM_EN					BIT(0)
+#define Timer_EN				BIT(4)
+
+#define TRSW0EN					BIT(2)
+#define TRSW1EN					BIT(3)
+#define EROM_EN					BIT(4)
+#define EnBT					BIT(5)
+#define EnUart					BIT(8)
+#define Uart_910				BIT(9)
+#define EnPMAC					BIT(10)
+#define SIC_SWRST				BIT(11)
+#define EnSIC					BIT(12)
+#define SIC_23					BIT(13)
+#define EnHDP					BIT(14)
+#define SIC_LBK					BIT(15)
+
+#define LED0PL					BIT(4)
+#define LED1PL					BIT(12)
+#define LED0DIS					BIT(7)
+
+#define MCUFWDL_EN				BIT(0)
+#define MCUFWDL_RDY				BIT(1)
+#define FWDL_ChkSum_rpt				BIT(2)
+#define MACINI_RDY				BIT(3)
+#define BBINI_RDY				BIT(4)
+#define RFINI_RDY				BIT(5)
+#define WINTINI_RDY				BIT(6)
+#define CPRST					BIT(23)
+
+#define XCLK_VLD				BIT(0)
+#define ACLK_VLD				BIT(1)
+#define UCLK_VLD				BIT(2)
+#define PCLK_VLD				BIT(3)
+#define PCIRSTB					BIT(4)
+#define V15_VLD					BIT(5)
+#define TRP_B15V_EN				BIT(7)
+#define SIC_IDLE				BIT(8)
+#define BD_MAC2					BIT(9)
+#define BD_MAC1					BIT(10)
+#define IC_MACPHY_MODE				BIT(11)
+#define VENDOR_ID				BIT(19)
+#define PAD_HWPD_IDN				BIT(22)
+#define TRP_VAUX_EN				BIT(23)
+#define TRP_BT_EN				BIT(24)
+#define BD_PKG_SEL				BIT(25)
+#define BD_HCI_SEL				BIT(26)
+#define TYPE_ID					BIT(27)
+
+#define CHIP_VER_RTL_MASK			0xF000
+#define CHIP_VER_RTL_SHIFT			12
+
+#define REG_LBMODE				(REG_CR + 3)
+
+#define HCI_TXDMA_EN				BIT(0)
+#define HCI_RXDMA_EN				BIT(1)
+#define TXDMA_EN				BIT(2)
+#define RXDMA_EN				BIT(3)
+#define PROTOCOL_EN				BIT(4)
+#define SCHEDULE_EN				BIT(5)
+#define MACTXEN					BIT(6)
+#define MACRXEN					BIT(7)
+#define ENSWBCN					BIT(8)
+#define ENSEC					BIT(9)
+
+#define _NETTYPE(x)				(((x) & 0x3) << 16)
+#define MASK_NETTYPE				0x30000
+#define NT_NO_LINK				0x0
+#define NT_LINK_AD_HOC				0x1
+#define NT_LINK_AP				0x2
+#define NT_AS_AP				0x3
+
+#define _LBMODE(x)				(((x) & 0xF) << 24)
+#define MASK_LBMODE				0xF000000
+#define LOOPBACK_NORMAL				0x0
+#define LOOPBACK_IMMEDIATELY			0xB
+#define LOOPBACK_MAC_DELAY			0x3
+#define LOOPBACK_PHY				0x1
+#define LOOPBACK_DMA				0x7
+
+#define GET_RX_PAGE_SIZE(value)			((value) & 0xF)
+#define GET_TX_PAGE_SIZE(value)			(((value) & 0xF0) >> 4)
+#define _PSRX_MASK				0xF
+#define _PSTX_MASK				0xF0
+#define _PSRX(x)				(x)
+#define _PSTX(x)				((x) << 4)
+
+#define PBP_64					0x0
+#define PBP_128					0x1
+#define PBP_256					0x2
+#define PBP_512					0x3
+#define PBP_1024				0x4
+
+#define RXDMA_ARBBW_EN				BIT(0)
+#define RXSHFT_EN				BIT(1)
+#define RXDMA_AGG_EN				BIT(2)
+#define QS_VO_QUEUE				BIT(8)
+#define QS_VI_QUEUE				BIT(9)
+#define QS_BE_QUEUE				BIT(10)
+#define QS_BK_QUEUE				BIT(11)
+#define QS_MANAGER_QUEUE			BIT(12)
+#define QS_HIGH_QUEUE				BIT(13)
+
+#define HQSEL_VOQ				BIT(0)
+#define HQSEL_VIQ				BIT(1)
+#define HQSEL_BEQ				BIT(2)
+#define HQSEL_BKQ				BIT(3)
+#define HQSEL_MGTQ				BIT(4)
+#define HQSEL_HIQ				BIT(5)
+
+#define _TXDMA_HIQ_MAP(x)			(((x)&0x3) << 14)
+#define _TXDMA_MGQ_MAP(x)			(((x)&0x3) << 12)
+#define _TXDMA_BKQ_MAP(x)			(((x)&0x3) << 10)
+#define _TXDMA_BEQ_MAP(x)			(((x)&0x3) << 8)
+#define _TXDMA_VIQ_MAP(x)			(((x)&0x3) << 6)
+#define _TXDMA_VOQ_MAP(x)			(((x)&0x3) << 4)
+
+#define QUEUE_LOW				1
+#define QUEUE_NORMAL				2
+#define QUEUE_HIGH				3
+
+#define _LLT_NO_ACTIVE				0x0
+#define _LLT_WRITE_ACCESS			0x1
+#define _LLT_READ_ACCESS			0x2
+
+#define _LLT_INIT_DATA(x)			((x) & 0xFF)
+#define _LLT_INIT_ADDR(x)			(((x) & 0xFF) << 8)
+#define _LLT_OP(x)					(((x) & 0x3) << 30)
+#define _LLT_OP_VALUE(x)			(((x) >> 30) & 0x3)
+
+#define BB_WRITE_READ_MASK			(BIT(31) | BIT(30))
+#define BB_WRITE_EN				BIT(30)
+#define BB_READ_EN				BIT(31)
+
+#define _HPQ(x)					((x) & 0xFF)
+#define _LPQ(x)					(((x) & 0xFF) << 8)
+#define _PUBQ(x)				(((x) & 0xFF) << 16)
+#define _NPQ(x)					((x) & 0xFF)
+
+#define HPQ_PUBLIC_DIS				BIT(24)
+#define LPQ_PUBLIC_DIS				BIT(25)
+#define LD_RQPN					BIT(31)
+
+#define BCN_VALID				BIT(16)
+#define BCN_HEAD(x)				(((x) & 0xFF) << 8)
+#define	BCN_HEAD_MASK				0xFF00
+
+#define BLK_DESC_NUM_SHIFT			4
+#define BLK_DESC_NUM_MASK			0xF
+
+#define DROP_DATA_EN				BIT(9)
+
+#define EN_AMPDU_RTY_NEW			BIT(7)
+
+#define _INIRTSMCS_SEL(x)			((x) & 0x3F)
+
+#define _SPEC_SIFS_CCK(x)			((x) & 0xFF)
+#define _SPEC_SIFS_OFDM(x)			(((x) & 0xFF) << 8)
+
+#define RATE_REG_BITMAP_ALL			0xFFFFF
+
+#define _RRSC_BITMAP(x)				((x) & 0xFFFFF)
+
+#define _RRSR_RSC(x)				(((x) & 0x3) << 21)
+#define RRSR_RSC_RESERVED			0x0
+#define RRSR_RSC_UPPER_SUBCHANNEL		0x1
+#define RRSR_RSC_LOWER_SUBCHANNEL		0x2
+#define RRSR_RSC_DUPLICATE_MODE			0x3
+
+#define USE_SHORT_G1				BIT(20)
+
+#define _AGGLMT_MCS0(x)				((x) & 0xF)
+#define _AGGLMT_MCS1(x)				(((x) & 0xF) << 4)
+#define _AGGLMT_MCS2(x)				(((x) & 0xF) << 8)
+#define _AGGLMT_MCS3(x)				(((x) & 0xF) << 12)
+#define _AGGLMT_MCS4(x)				(((x) & 0xF) << 16)
+#define _AGGLMT_MCS5(x)				(((x) & 0xF) << 20)
+#define _AGGLMT_MCS6(x)				(((x) & 0xF) << 24)
+#define _AGGLMT_MCS7(x)				(((x) & 0xF) << 28)
+
+#define	RETRY_LIMIT_SHORT_SHIFT			8
+#define	RETRY_LIMIT_LONG_SHIFT			0
+
+#define _DARF_RC1(x)				((x) & 0x1F)
+#define _DARF_RC2(x)				(((x) & 0x1F) << 8)
+#define _DARF_RC3(x)				(((x) & 0x1F) << 16)
+#define _DARF_RC4(x)				(((x) & 0x1F) << 24)
+#define _DARF_RC5(x)				((x) & 0x1F)
+#define _DARF_RC6(x)				(((x) & 0x1F) << 8)
+#define _DARF_RC7(x)				(((x) & 0x1F) << 16)
+#define _DARF_RC8(x)				(((x) & 0x1F) << 24)
+
+#define _RARF_RC1(x)				((x) & 0x1F)
+#define _RARF_RC2(x)				(((x) & 0x1F) << 8)
+#define _RARF_RC3(x)				(((x) & 0x1F) << 16)
+#define _RARF_RC4(x)				(((x) & 0x1F) << 24)
+#define _RARF_RC5(x)				((x) & 0x1F)
+#define _RARF_RC6(x)				(((x) & 0x1F) << 8)
+#define _RARF_RC7(x)				(((x) & 0x1F) << 16)
+#define _RARF_RC8(x)				(((x) & 0x1F) << 24)
+
+#define AC_PARAM_TXOP_LIMIT_OFFSET		16
+#define AC_PARAM_ECW_MAX_OFFSET			12
+#define AC_PARAM_ECW_MIN_OFFSET			8
+#define AC_PARAM_AIFS_OFFSET			0
+
+#define _AIFS(x)				(x)
+#define _ECW_MAX_MIN(x)				((x) << 8)
+#define _TXOP_LIMIT(x)				((x) << 16)
+
+#define _BCNIFS(x)				((x) & 0xFF)
+#define _BCNECW(x)				((((x) & 0xF)) << 8)
+
+#define _LRL(x)					((x) & 0x3F)
+#define _SRL(x)					(((x) & 0x3F) << 8)
+
+#define _SIFS_CCK_CTX(x)			((x) & 0xFF)
+#define _SIFS_CCK_TRX(x)			(((x) & 0xFF) << 8);
+
+#define _SIFS_OFDM_CTX(x)			((x) & 0xFF)
+#define _SIFS_OFDM_TRX(x)			(((x) & 0xFF) << 8);
+
+#define _TBTT_PROHIBIT_HOLD(x)			(((x) & 0xFF) << 8)
+
+#define DIS_EDCA_CNT_DWN			BIT(11)
+
+#define EN_MBSSID				BIT(1)
+#define EN_TXBCN_RPT				BIT(2)
+#define	EN_BCN_FUNCTION				BIT(3)
+
+#define TSFTR_RST				BIT(0)
+#define TSFTR1_RST				BIT(1)
+
+#define STOP_BCNQ				BIT(6)
+
+#define	DIS_TSF_UDT0_NORMAL_CHIP		BIT(4)
+#define	DIS_TSF_UDT0_TEST_CHIP			BIT(5)
+
+#define	AcmHw_HwEn				BIT(0)
+#define	AcmHw_BeqEn				BIT(1)
+#define	AcmHw_ViqEn				BIT(2)
+#define	AcmHw_VoqEn				BIT(3)
+#define	AcmHw_BeqStatus				BIT(4)
+#define	AcmHw_ViqStatus				BIT(5)
+#define	AcmHw_VoqStatus				BIT(6)
+
+#define APSDOFF					BIT(6)
+#define APSDOFF_STATUS				BIT(7)
+
+#define BW_20MHZ				BIT(2)
+
+#define RATE_BITMAP_ALL				0xFFFFF
+
+#define RATE_RRSR_CCK_ONLY_1M			0xFFFF1
+
+#define TSFRST					BIT(0)
+#define DIS_GCLK				BIT(1)
+#define PAD_SEL					BIT(2)
+#define PWR_ST					BIT(6)
+#define PWRBIT_OW_EN				BIT(7)
+#define ACRC					BIT(8)
+#define CFENDFORM				BIT(9)
+#define ICV					BIT(10)
+
+#define AAP					BIT(0)
+#define APM					BIT(1)
+#define AM					BIT(2)
+#define AB					BIT(3)
+#define ADD3					BIT(4)
+#define APWRMGT					BIT(5)
+#define CBSSID					BIT(6)
+#define CBSSID_DATA				BIT(6)
+#define CBSSID_BCN				BIT(7)
+#define ACRC32					BIT(8)
+#define AICV					BIT(9)
+#define ADF					BIT(11)
+#define ACF					BIT(12)
+#define AMF					BIT(13)
+#define HTC_LOC_CTRL				BIT(14)
+#define UC_DATA_EN				BIT(16)
+#define BM_DATA_EN				BIT(17)
+#define MFBEN					BIT(22)
+#define LSIGEN					BIT(23)
+#define EnMBID					BIT(24)
+#define APP_BASSN				BIT(27)
+#define APP_PHYSTS				BIT(28)
+#define APP_ICV					BIT(29)
+#define APP_MIC					BIT(30)
+#define APP_FCS					BIT(31)
+
+#define _MIN_SPACE(x)				((x) & 0x7)
+#define _SHORT_GI_PADDING(x)			(((x) & 0x1F) << 3)
+
+#define RXERR_TYPE_OFDM_PPDU			0
+#define RXERR_TYPE_OFDM_FALSE_ALARM		1
+#define	RXERR_TYPE_OFDM_MPDU_OK			2
+#define RXERR_TYPE_OFDM_MPDU_FAIL		3
+#define RXERR_TYPE_CCK_PPDU			4
+#define RXERR_TYPE_CCK_FALSE_ALARM		5
+#define RXERR_TYPE_CCK_MPDU_OK			6
+#define RXERR_TYPE_CCK_MPDU_FAIL		7
+#define RXERR_TYPE_HT_PPDU			8
+#define RXERR_TYPE_HT_FALSE_ALARM		9
+#define RXERR_TYPE_HT_MPDU_TOTAL		10
+#define RXERR_TYPE_HT_MPDU_OK			11
+#define RXERR_TYPE_HT_MPDU_FAIL			12
+#define RXERR_TYPE_RX_FULL_DROP			15
+
+#define RXERR_COUNTER_MASK			0xFFFFF
+#define RXERR_RPT_RST				BIT(27)
+#define _RXERR_RPT_SEL(type)			((type) << 28)
+
+#define	SCR_TxUseDK				BIT(0)
+#define	SCR_RxUseDK				BIT(1)
+#define	SCR_TxEncEnable				BIT(2)
+#define	SCR_RxDecEnable				BIT(3)
+#define	SCR_SKByA2				BIT(4)
+#define	SCR_NoSKMC				BIT(5)
+#define SCR_TXBCUSEDK				BIT(6)
+#define SCR_RXBCUSEDK				BIT(7)
+
+#define USB_IS_HIGH_SPEED			0
+#define USB_IS_FULL_SPEED			1
+#define USB_SPEED_MASK				BIT(5)
+
+#define USB_NORMAL_SIE_EP_MASK			0xF
+#define USB_NORMAL_SIE_EP_SHIFT			4
+
+#define USB_TEST_EP_MASK			0x30
+#define USB_TEST_EP_SHIFT			4
+
+#define USB_AGG_EN				BIT(3)
+
+#define MAC_ADDR_LEN				6
+#define LAST_ENTRY_OF_TX_PKT_BUFFER		175
+
+#define POLLING_LLT_THRESHOLD			20
+#define POLLING_READY_TIMEOUT_COUNT		3000
+
+#define	MAX_MSS_DENSITY_2T			0x13
+#define	MAX_MSS_DENSITY_1T			0x0A
+
+#define EPROM_CMD_OPERATING_MODE_MASK		((1 << 7) | (1 << 6))
+#define EPROM_CMD_CONFIG			0x3
+#define EPROM_CMD_LOAD				1
+
+#define	HWSET_MAX_SIZE_92S			HWSET_MAX_SIZE
+
+#define	HAL_8192C_HW_GPIO_WPS_BIT		BIT(2)
+
+#define	RPMAC_RESET				0x100
+#define	RPMAC_TXSTART				0x104
+#define	RPMAC_TXLEGACYSIG			0x108
+#define	RPMAC_TXHTSIG1				0x10c
+#define	RPMAC_TXHTSIG2				0x110
+#define	RPMAC_PHYDEBUG				0x114
+#define	RPMAC_TXPACKETNUM			0x118
+#define	RPMAC_TXIDLE				0x11c
+#define	RPMAC_TXMACHEADER0			0x120
+#define	RPMAC_TXMACHEADER1			0x124
+#define	RPMAC_TXMACHEADER2			0x128
+#define	RPMAC_TXMACHEADER3			0x12c
+#define	RPMAC_TXMACHEADER4			0x130
+#define	RPMAC_TXMACHEADER5			0x134
+#define	RPMAC_TXDADATYPE			0x138
+#define	RPMAC_TXRANDOMSEED			0x13c
+#define	RPMAC_CCKPLCPPREAMBLE			0x140
+#define	RPMAC_CCKPLCPHEADER			0x144
+#define	RPMAC_CCKCRC16				0x148
+#define	RPMAC_OFDMRXCRC32OK			0x170
+#define	RPMAC_OFDMRXCRC32Er			0x174
+#define	RPMAC_OFDMRXPARITYER			0x178
+#define	RPMAC_OFDMRXCRC8ER			0x17c
+#define	RPMAC_CCKCRXRC16ER			0x180
+#define	RPMAC_CCKCRXRC32ER			0x184
+#define	RPMAC_CCKCRXRC32OK			0x188
+#define	RPMAC_TXSTATUS				0x18c
+
+#define	RFPGA0_RFMOD				0x800
+
+#define	RFPGA0_TXINFO				0x804
+#define	RFPGA0_PSDFUNCTION			0x808
+
+#define	RFPGA0_TXGAINSTAGE			0x80c
+
+#define	RFPGA0_RFTIMING1			0x810
+#define	RFPGA0_RFTIMING2			0x814
+
+#define	RFPGA0_XA_HSSIPARAMETER1		0x820
+#define	RFPGA0_XA_HSSIPARAMETER2		0x824
+#define	RFPGA0_XB_HSSIPARAMETER1		0x828
+#define	RFPGA0_XB_HSSIPARAMETER2		0x82c
+
+#define	RFPGA0_XA_LSSIPARAMETER			0x840
+#define	RFPGA0_XB_LSSIPARAMETER			0x844
+
+#define	RFPGA0_RFWAKEUPPARAMETER		0x850
+#define	RFPGA0_RFSLEEPUPPARAMETER		0x854
+
+#define	RFPGA0_XAB_SWITCHCONTROL		0x858
+#define	RFPGA0_XCD_SWITCHCONTROL		0x85c
+
+#define	RFPGA0_XA_RFINTERFACEOE			0x860
+#define	RFPGA0_XB_RFINTERFACEOE			0x864
+
+#define	RFPGA0_XAB_RFINTERFACESW		0x870
+#define	RFPGA0_XCD_RFINTERFACESW		0x874
+
+#define	rFPGA0_XAB_RFPARAMETER			0x878
+#define	rFPGA0_XCD_RFPARAMETER			0x87c
+
+#define	RFPGA0_ANALOGPARAMETER1			0x880
+#define	RFPGA0_ANALOGPARAMETER2			0x884
+#define	RFPGA0_ANALOGPARAMETER3			0x888
+#define	RFPGA0_ANALOGPARAMETER4			0x88c
+
+#define	RFPGA0_XA_LSSIREADBACK			0x8a0
+#define	RFPGA0_XB_LSSIREADBACK			0x8a4
+#define	RFPGA0_XC_LSSIREADBACK			0x8a8
+#define	RFPGA0_XD_LSSIREADBACK			0x8ac
+
+#define	RFPGA0_PSDREPORT			0x8b4
+#define	TRANSCEIVEA_HSPI_READBACK		0x8b8
+#define	TRANSCEIVEB_HSPI_READBACK		0x8bc
+#define	REG_SC_CNT				0x8c4
+#define	RFPGA0_XAB_RFINTERFACERB		0x8e0
+#define	RFPGA0_XCD_RFINTERFACERB		0x8e4
+
+#define	RFPGA1_RFMOD				0x900
+
+#define	RFPGA1_TXBLOCK				0x904
+#define	RFPGA1_DEBUGSELECT			0x908
+#define	RFPGA1_TXINFO				0x90c
+
+#define	RCCK0_SYSTEM				0xa00
+
+#define	RCCK0_AFESETTING			0xa04
+#define	RCCK0_CCA				0xa08
+
+#define	RCCK0_RXAGC1				0xa0c
+#define	RCCK0_RXAGC2				0xa10
+
+#define	RCCK0_RXHP				0xa14
+
+#define	RCCK0_DSPPARAMETER1			0xa18
+#define	RCCK0_DSPPARAMETER2			0xa1c
+
+#define	RCCK0_TXFILTER1				0xa20
+#define	RCCK0_TXFILTER2				0xa24
+#define	RCCK0_DEBUGPORT				0xa28
+#define	RCCK0_FALSEALARMREPORT			0xa2c
+#define	RCCK0_TRSSIREPORT			0xa50
+#define	RCCK0_RXREPORT				0xa54
+#define	RCCK0_FACOUNTERLOWER			0xa5c
+#define	RCCK0_FACOUNTERUPPER			0xa58
+#define	RCCK0_CCA_CNT				0xa60
+
+
+/* PageB(0xB00) */
+#define	rPdp_AntA				0xb00
+#define	rPdp_AntA_4				0xb04
+#define	rPdp_AntA_8				0xb08
+#define	rPdp_AntA_C				0xb0c
+#define	rPdp_AntA_10				0xb10
+#define	rPdp_AntA_14				0xb14
+#define	rPdp_AntA_18				0xb18
+#define	rPdp_AntA_1C				0xb1c
+#define	rPdp_AntA_20				0xb20
+#define	rPdp_AntA_24				0xb24
+
+#define	rConfig_Pmpd_AntA			0xb28
+#define	rConfig_ram64x16			0xb2c
+
+#define	rBndA					0xb30
+#define	rHssiPar				0xb34
+
+#define	rConfig_AntA				0xb68
+#define	rConfig_AntB				0xb6c
+
+#define	rPdp_AntB				0xb70
+#define	rPdp_AntB_4				0xb74
+#define	rPdp_AntB_8				0xb78
+#define	rPdp_AntB_C				0xb7c
+#define	rPdp_AntB_10				0xb80
+#define	rPdp_AntB_14				0xb84
+#define	rPdp_AntB_18				0xb88
+#define	rPdp_AntB_1C				0xb8c
+#define	rPdp_AntB_20				0xb90
+#define	rPdp_AntB_24				0xb94
+
+#define	rConfig_Pmpd_AntB			0xb98
+
+#define	rBndB					0xba0
+
+#define	rAPK					0xbd8
+#define	rPm_Rx0_AntA				0xbdc
+#define	rPm_Rx1_AntA				0xbe0
+#define	rPm_Rx2_AntA				0xbe4
+#define	rPm_Rx3_AntA				0xbe8
+#define	rPm_Rx0_AntB				0xbec
+#define	rPm_Rx1_AntB				0xbf0
+#define	rPm_Rx2_AntB				0xbf4
+#define	rPm_Rx3_AntB				0xbf8
+
+/*Page C*/
+#define	ROFDM0_LSTF				0xc00
+
+#define	ROFDM0_TRXPATHENABLE			0xc04
+#define	ROFDM0_TRMUXPAR				0xc08
+#define	ROFDM0_TRSWISOLATION			0xc0c
+
+#define	ROFDM0_XARXAFE				0xc10
+#define	ROFDM0_XARXIQIMBALANCE			0xc14
+#define	ROFDM0_XBRXAFE				0xc18
+#define	ROFDM0_XBRXIQIMBALANCE			0xc1c
+#define	ROFDM0_XCRXAFE				0xc20
+#define	ROFDM0_XCRXIQIMBANLANCE			0xc24
+#define	ROFDM0_XDRXAFE				0xc28
+#define	ROFDM0_XDRXIQIMBALANCE			0xc2c
+
+#define	ROFDM0_RXDETECTOR1			0xc30
+#define	ROFDM0_RXDETECTOR2			0xc34
+#define	ROFDM0_RXDETECTOR3			0xc38
+#define	ROFDM0_RXDETECTOR4			0xc3c
+
+#define	ROFDM0_RXDSP				0xc40
+#define	ROFDM0_CFOANDDAGC			0xc44
+#define	ROFDM0_CCADROPTHRESHOLD			0xc48
+#define	ROFDM0_ECCATHRESHOLD			0xc4c
+
+#define	ROFDM0_XAAGCCORE1			0xc50
+#define	ROFDM0_XAAGCCORE2			0xc54
+#define	ROFDM0_XBAGCCORE1			0xc58
+#define	ROFDM0_XBAGCCORE2			0xc5c
+#define	ROFDM0_XCAGCCORE1			0xc60
+#define	ROFDM0_XCAGCCORE2			0xc64
+#define	ROFDM0_XDAGCCORE1			0xc68
+#define	ROFDM0_XDAGCCORE2			0xc6c
+
+#define	ROFDM0_AGCPARAMETER1			0xc70
+#define	ROFDM0_AGCPARAMETER2			0xc74
+#define	ROFDM0_AGCRSSITABLE			0xc78
+#define	ROFDM0_HTSTFAGC				0xc7c
+
+#define	ROFDM0_XATXIQIMBALANCE			0xc80
+#define	ROFDM0_XATXAFE				0xc84
+#define	ROFDM0_XBTXIQIMBALANCE			0xc88
+#define	ROFDM0_XBTXAFE				0xc8c
+#define	ROFDM0_XCTXIQIMBALANCE			0xc90
+#define	ROFDM0_XCTXAFE				0xc94
+#define	ROFDM0_XDTXIQIMBALANCE			0xc98
+#define	ROFDM0_XDTXAFE				0xc9c
+
+#define ROFDM0_RXIQEXTANTA			0xca0
+#define	ROFDM0_TXCOEFF1				0xca4
+#define	ROFDM0_TXCOEFF2				0xca8
+#define	ROFDM0_TXCOEFF3				0xcac
+#define	ROFDM0_TXCOEFF4				0xcb0
+#define	ROFDM0_TXCOEFF5				0xcb4
+#define	ROFDM0_TXCOEFF6				0xcb8
+
+#define	ROFDM0_RXHPPARAMETER			0xce0
+#define	ROFDM0_TXPSEUDONOISEWGT			0xce4
+#define	ROFDM0_FRAMESYNC			0xcf0
+#define	ROFDM0_DFSREPORT			0xcf4
+
+
+#define	ROFDM1_LSTF				0xd00
+#define	ROFDM1_TRXPATHENABLE			0xd04
+
+#define	ROFDM1_CF0				0xd08
+#define	ROFDM1_CSI1				0xd10
+#define	ROFDM1_SBD				0xd14
+#define	ROFDM1_CSI2				0xd18
+#define	ROFDM1_CFOTRACKING			0xd2c
+#define	ROFDM1_TRXMESAURE1			0xd34
+#define	ROFDM1_INTFDET				0xd3c
+#define	ROFDM1_PSEUDONOISESTATEAB		0xd50
+#define	ROFDM1_PSEUDONOISESTATECD		0xd54
+#define	ROFDM1_RXPSEUDONOISEWGT			0xd58
+
+#define	ROFDM_PHYCOUNTER1			0xda0
+#define	ROFDM_PHYCOUNTER2			0xda4
+#define	ROFDM_PHYCOUNTER3			0xda8
+
+#define	ROFDM_SHORTCFOAB			0xdac
+#define	ROFDM_SHORTCFOCD			0xdb0
+#define	ROFDM_LONGCFOAB				0xdb4
+#define	ROFDM_LONGCFOCD				0xdb8
+#define	ROFDM_TAILCF0AB				0xdbc
+#define	ROFDM_TAILCF0CD				0xdc0
+#define	ROFDM_PWMEASURE1			0xdc4
+#define	ROFDM_PWMEASURE2			0xdc8
+#define	ROFDM_BWREPORT				0xdcc
+#define	ROFDM_AGCREPORT				0xdd0
+#define	ROFDM_RXSNR				0xdd4
+#define	ROFDM_RXEVMCSI				0xdd8
+#define	ROFDM_SIGREPORT				0xddc
+
+#define	RTXAGC_A_RATE18_06			0xe00
+#define	RTXAGC_A_RATE54_24			0xe04
+#define	RTXAGC_A_CCK1_MCS32			0xe08
+#define	RTXAGC_A_MCS03_MCS00			0xe10
+#define	RTXAGC_A_MCS07_MCS04			0xe14
+#define	RTXAGC_A_MCS11_MCS08			0xe18
+#define	RTXAGC_A_MCS15_MCS12			0xe1c
+
+#define	RTXAGC_B_RATE18_06			0x830
+#define	RTXAGC_B_RATE54_24			0x834
+#define	RTXAGC_B_CCK1_55_MCS32			0x838
+#define	RTXAGC_B_MCS03_MCS00			0x83c
+#define	RTXAGC_B_MCS07_MCS04			0x848
+#define	RTXAGC_B_MCS11_MCS08			0x84c
+#define	RTXAGC_B_MCS15_MCS12			0x868
+#define	RTXAGC_B_CCK11_A_CCK2_11		0x86c
+
+#define	RFPGA0_IQK				0xe28
+#define	RTx_IQK_Tone_A				0xe30
+#define	RRx_IQK_Tone_A				0xe34
+#define	RTx_IQK_PI_A				0xe38
+#define	RRx_IQK_PI_A				0xe3c
+
+#define	RTx_IQK					0xe40
+#define	RRx_IQK					0xe44
+#define	RIQK_AGC_Pts				0xe48
+#define	RIQK_AGC_Rsp				0xe4c
+#define	RTx_IQK_Tone_B				0xe50
+#define	RRx_IQK_Tone_B				0xe54
+#define	RTx_IQK_PI_B				0xe58
+#define	RRx_IQK_PI_B				0xe5c
+#define	RIQK_AGC_Cont				0xe60
+
+#define	RBlue_Tooth				0xe6c
+#define	RRx_Wait_CCA				0xe70
+#define	RTx_CCK_RFON				0xe74
+#define	RTx_CCK_BBON				0xe78
+#define	RTx_OFDM_RFON				0xe7c
+#define	RTx_OFDM_BBON				0xe80
+#define	RTx_To_Rx				0xe84
+#define	RTx_To_Tx				0xe88
+#define	RRx_CCK					0xe8c
+
+#define	RTx_Power_Before_IQK_A			0xe94
+#define	RTx_Power_After_IQK_A			0xe9c
+
+#define	RRx_Power_Before_IQK_A			0xea0
+#define	RRx_Power_Before_IQK_A_2		0xea4
+#define	RRx_Power_After_IQK_A			0xea8
+#define	RRx_Power_After_IQK_A_2			0xeac
+
+#define	RTx_Power_Before_IQK_B			0xeb4
+#define	RTx_Power_After_IQK_B			0xebc
+
+#define	RRx_Power_Before_IQK_B			0xec0
+#define	RRx_Power_Before_IQK_B_2		0xec4
+#define	RRx_Power_After_IQK_B			0xec8
+#define	RRx_Power_After_IQK_B_2			0xecc
+
+#define	RRx_OFDM				0xed0
+#define	RRx_Wait_RIFS				0xed4
+#define	RRx_TO_Rx				0xed8
+#define	RStandby				0xedc
+#define	RSleep					0xee0
+#define	RPMPD_ANAEN				0xeec
+
+#define	RZEBRA1_HSSIENABLE			0x0
+#define	RZEBRA1_TRXENABLE1			0x1
+#define	RZEBRA1_TRXENABLE2			0x2
+#define	RZEBRA1_AGC				0x4
+#define	RZEBRA1_CHARGEPUMP			0x5
+#define	RZEBRA1_CHANNEL				0x7
+
+#define	RZEBRA1_TXGAIN				0x8
+#define	RZEBRA1_TXLPF				0x9
+#define	RZEBRA1_RXLPF				0xb
+#define	RZEBRA1_RXHPFCORNER			0xc
+
+#define	RGLOBALCTRL				0
+#define	RRTL8256_TXLPF				19
+#define	RRTL8256_RXLPF				11
+#define	RRTL8258_TXLPF				0x11
+#define	RRTL8258_RXLPF				0x13
+#define	RRTL8258_RSSILPF			0xa
+
+#define	RF_AC					0x00
+
+#define	RF_IQADJ_G1				0x01
+#define	RF_IQADJ_G2				0x02
+#define	RF_POW_TRSW				0x05
+
+#define	RF_GAIN_RX				0x06
+#define	RF_GAIN_TX				0x07
+
+#define	RF_TXM_IDAC				0x08
+#define	RF_BS_IQGEN				0x0F
+
+#define	RF_MODE1				0x10
+#define	RF_MODE2				0x11
+
+#define	RF_RX_AGC_HP				0x12
+#define	RF_TX_AGC				0x13
+#define	RF_BIAS					0x14
+#define	RF_IPA					0x15
+#define	RF_POW_ABILITY				0x17
+#define	RF_MODE_AG				0x18
+#define	RRFCHANNEL				0x18
+#define	RF_CHNLBW				0x18
+#define	RF_TOP					0x19
+
+#define	RF_RX_G1				0x1A
+#define	RF_RX_G2				0x1B
+
+#define	RF_RX_BB2				0x1C
+#define	RF_RX_BB1				0x1D
+
+#define	RF_RCK1					0x1E
+#define	RF_RCK2					0x1F
+
+#define	RF_TX_G1				0x20
+#define	RF_TX_G2				0x21
+#define	RF_TX_G3				0x22
+
+#define	RF_TX_BB1				0x23
+#define	RF_T_METER				0x42
+
+#define	RF_SYN_G1				0x25
+#define	RF_SYN_G2				0x26
+#define	RF_SYN_G3				0x27
+#define	RF_SYN_G4				0x28
+#define	RF_SYN_G5				0x29
+#define	RF_SYN_G6				0x2A
+#define	RF_SYN_G7				0x2B
+#define	RF_SYN_G8				0x2C
+
+#define	RF_RCK_OS				0x30
+#define	RF_TXPA_G1				0x31
+#define	RF_TXPA_G2				0x32
+#define	RF_TXPA_G3				0x33
+
+#define	RF_TX_BIAS_A				0x35
+#define	RF_TX_BIAS_D				0x36
+#define	RF_LOBF_9				0x38
+#define	RF_RXRF_A3				0x3C
+#define	RF_TRSW					0x3F
+
+#define	RF_TXRF_A2				0x41
+#define	RF_TXPA_G4				0x46
+#define	RF_TXPA_A4				0x4B
+
+#define	RF_WE_LUT				0xEF
+
+#define	BBBRESETB				0x100
+#define	BGLOBALRESETB				0x200
+#define	BOFDMTXSTART				0x4
+#define	BCCKTXSTART				0x8
+#define	BCRC32DEBUG				0x100
+#define	BPMACLOOPBACK				0x10
+#define	BTXLSIG					0xffffff
+#define	BOFDMTXRATE				0xf
+#define	BOFDMTXRESERVED				0x10
+#define	BOFDMTXLENGTH				0x1ffe0
+#define	BOFDMTXPARITY				0x20000
+#define	BTXHTSIG1				0xffffff
+#define	BTXHTMCSRATE				0x7f
+#define	BTXHTBW					0x80
+#define	BTXHTLENGTH				0xffff00
+#define	BTXHTSIG2				0xffffff
+#define	BTXHTSMOOTHING				0x1
+#define	BTXHTSOUNDING				0x2
+#define	BTXHTRESERVED				0x4
+#define	BTXHTAGGREATION				0x8
+#define	BTXHTSTBC				0x30
+#define	BTXHTADVANCECODING			0x40
+#define	BTXHTSHORTGI				0x80
+#define	BTXHTNUMBERHT_LTF			0x300
+#define	BTXHTCRC8				0x3fc00
+#define	BCOUNTERRESET				0x10000
+#define	BNUMOFOFDMTX				0xffff
+#define	BNUMOFCCKTX				0xffff0000
+#define	BTXIDLEINTERVAL				0xffff
+#define	BOFDMSERVICE				0xffff0000
+#define	BTXMACHEADER				0xffffffff
+#define	BTXDATAINIT				0xff
+#define	BTXHTMODE				0x100
+#define	BTXDATATYPE				0x30000
+#define	BTXRANDOMSEED				0xffffffff
+#define	BCCKTXPREAMBLE				0x1
+#define	BCCKTXSFD				0xffff0000
+#define	BCCKTXSIG				0xff
+#define	BCCKTXSERVICE				0xff00
+#define	BCCKLENGTHEXT				0x8000
+#define	BCCKTXLENGHT				0xffff0000
+#define	BCCKTXCRC16				0xffff
+#define	BCCKTXSTATUS				0x1
+#define	BOFDMTXSTATUS				0x2
+#define IS_BB_REG_OFFSET_92S(_Offset)	\
+	((_Offset >= 0x800) && (_Offset <= 0xfff))
+
+#define	BRFMOD					0x1
+#define	BJAPANMODE				0x2
+#define	BCCKTXSC				0x30
+#define	BCCKEN					0x1000000
+#define	BOFDMEN					0x2000000
+
+#define	BOFDMRXADCPHASE				0x10000
+#define	BOFDMTXDACPHASE				0x40000
+#define	BXATXAGC				0x3f
+
+#define	BXBTXAGC				0xf00
+#define	BXCTXAGC				0xf000
+#define	BXDTXAGC				0xf0000
+
+#define	BPASTART				0xf0000000
+#define	BTRSTART				0x00f00000
+#define	BRFSTART				0x0000f000
+#define	BBBSTART				0x000000f0
+#define	BBBCCKSTART				0x0000000f
+#define	BPAEND					0xf
+#define	BTREND					0x0f000000
+#define	BRFEND					0x000f0000
+#define	BCCAMASK				0x000000f0
+#define	BR2RCCAMASK				0x00000f00
+#define	BHSSI_R2TDELAY				0xf8000000
+#define	BHSSI_T2RDELAY				0xf80000
+#define	BCONTXHSSI				0x400
+#define	BIGFROMCCK				0x200
+#define	BAGCADDRESS				0x3f
+#define	BRXHPTX					0x7000
+#define	BRXHP2RX				0x38000
+#define	BRXHPCCKINI				0xc0000
+#define	BAGCTXCODE				0xc00000
+#define	BAGCRXCODE				0x300000
+
+#define	B3WIREDATALENGTH			0x800
+#define	B3WIREADDREAALENGTH			0x400
+
+#define	B3WIRERFPOWERDOWN			0x1
+#define	B5GPAPEPOLARITY				0x40000000
+#define	B2GPAPEPOLARITY				0x80000000
+#define	BRFSW_TXDEFAULTANT			0x3
+#define	BRFSW_TXOPTIONANT			0x30
+#define	BRFSW_RXDEFAULTANT			0x300
+#define	BRFSW_RXOPTIONANT			0x3000
+#define	BRFSI_3WIREDATA				0x1
+#define	BRFSI_3WIRECLOCK			0x2
+#define	BRFSI_3WIRELOAD				0x4
+#define	BRFSI_3WIRERW				0x8
+#define	BRFSI_3WIRE				0xf
+
+#define	BRFSI_RFENV				0x10
+
+#define	BRFSI_TRSW				0x20
+#define	BRFSI_TRSWB				0x40
+#define	BRFSI_ANTSW				0x100
+#define	BRFSI_ANTSWB				0x200
+#define	BRFSI_PAPE				0x400
+#define	BRFSI_PAPE5G				0x800
+#define	BBANDSELECT				0x1
+#define	BHTSIG2_GI				0x80
+#define	BHTSIG2_SMOOTHING			0x01
+#define	BHTSIG2_SOUNDING			0x02
+#define	BHTSIG2_AGGREATON			0x08
+#define	BHTSIG2_STBC				0x30
+#define	BHTSIG2_ADVCODING			0x40
+#define	BHTSIG2_NUMOFHTLTF			0x300
+#define	BHTSIG2_CRC8				0x3fc
+#define	BHTSIG1_MCS				0x7f
+#define	BHTSIG1_BANDWIDTH			0x80
+#define	BHTSIG1_HTLENGTH			0xffff
+#define	BLSIG_RATE				0xf
+#define	BLSIG_RESERVED				0x10
+#define	BLSIG_LENGTH				0x1fffe
+#define	BLSIG_PARITY				0x20
+#define	BCCKRXPHASE				0x4
+
+#define	BLSSIREADADDRESS			0x7f800000
+#define	BLSSIREADEDGE				0x80000000
+
+#define	BLSSIREADBACKDATA			0xfffff
+
+#define	BLSSIREADOKFLAG				0x1000
+#define	BCCKSAMPLERATE				0x8
+#define	BREGULATOR0STANDBY			0x1
+#define	BREGULATORPLLSTANDBY			0x2
+#define	BREGULATOR1STANDBY			0x4
+#define	BPLLPOWERUP				0x8
+#define	BDPLLPOWERUP				0x10
+#define	BDA10POWERUP				0x20
+#define	BAD7POWERUP				0x200
+#define	BDA6POWERUP				0x2000
+#define	BXTALPOWERUP				0x4000
+#define	B40MDCLKPOWERUP				0x8000
+#define	BDA6DEBUGMODE				0x20000
+#define	BDA6SWING				0x380000
+
+#define	BADCLKPHASE				0x4000000
+#define	B80MCLKDELAY				0x18000000
+#define	BAFEWATCHDOGENABLE			0x20000000
+
+#define	BXTALCAP01				0xc0000000
+#define	BXTALCAP23				0x3
+#define	BXTALCAP92X				0x0f000000
+#define BXTALCAP				0x0f000000
+
+#define	BINTDIFCLKENABLE			0x400
+#define	BEXTSIGCLKENABLE			0x800
+#define	BBANDGAP_MBIAS_POWERUP			0x10000
+#define	BAD11SH_GAIN				0xc0000
+#define	BAD11NPUT_RANGE				0x700000
+#define	BAD110P_CURRENT				0x3800000
+#define	BLPATH_LOOPBACK				0x4000000
+#define	BQPATH_LOOPBACK				0x8000000
+#define	BAFE_LOOPBACK				0x10000000
+#define	BDA10_SWING				0x7e0
+#define	BDA10_REVERSE				0x800
+#define	BDA_CLK_SOURCE				0x1000
+#define	BDA7INPUT_RANGE				0x6000
+#define	BDA7_GAIN				0x38000
+#define	BDA7OUTPUT_CM_MODE			0x40000
+#define	BDA7INPUT_CM_MODE			0x380000
+#define	BDA7CURRENT				0xc00000
+#define	BREGULATOR_ADJUST			0x7000000
+#define	BAD11POWERUP_ATTX			0x1
+#define	BDA10PS_ATTX				0x10
+#define	BAD11POWERUP_ATRX			0x100
+#define	BDA10PS_ATRX				0x1000
+#define	BCCKRX_AGC_FORMAT			0x200
+#define	BPSDFFT_SAMPLE_POINT			0xc000
+#define	BPSD_AVERAGE_NUM			0x3000
+#define	BIQPATH_CONTROL				0xc00
+#define	BPSD_FREQ				0x3ff
+#define	BPSD_ANTENNA_PATH			0x30
+#define	BPSD_IQ_SWITCH				0x40
+#define	BPSD_RX_TRIGGER				0x400000
+#define	BPSD_TX_TRIGGER				0x80000000
+#define	BPSD_SINE_TONE_SCALE			0x7f000000
+#define	BPSD_REPORT				0xffff
+
+#define	BOFDM_TXSC				0x30000000
+#define	BCCK_TXON				0x1
+#define	BOFDM_TXON				0x2
+#define	BDEBUG_PAGE				0xfff
+#define	BDEBUG_ITEM				0xff
+#define	BANTL					0x10
+#define	BANT_NONHT				0x100
+#define	BANT_HT1				0x1000
+#define	BANT_HT2				0x10000
+#define	BANT_HT1S1				0x100000
+#define	BANT_NONHTS1				0x1000000
+
+#define	BCCK_BBMODE				0x3
+#define	BCCK_TXPOWERSAVING			0x80
+#define	BCCK_RXPOWERSAVING			0x40
+
+#define	BCCK_SIDEBAND				0x10
+
+#define	BCCK_SCRAMBLE				0x8
+#define	BCCK_ANTDIVERSITY			0x8000
+#define	BCCK_CARRIER_RECOVERY			0x4000
+#define	BCCK_TXRATE				0x3000
+#define	BCCK_DCCANCEL				0x0800
+#define	BCCK_ISICANCEL				0x0400
+#define	BCCK_MATCH_FILTER			0x0200
+#define	BCCK_EQUALIZER				0x0100
+#define	BCCK_PREAMBLE_DETECT			0x800000
+#define	BCCK_FAST_FALSECCA			0x400000
+#define	BCCK_CH_ESTSTART			0x300000
+#define	BCCK_CCA_COUNT				0x080000
+#define	BCCK_CS_LIM				0x070000
+#define	BCCK_BIST_MODE				0x80000000
+#define	BCCK_CCAMASK				0x40000000
+#define	BCCK_TX_DAC_PHASE			0x4
+#define	BCCK_RX_ADC_PHASE			0x20000000
+#define	BCCKR_CP_MODE				0x0100
+#define	BCCK_TXDC_OFFSET			0xf0
+#define	BCCK_RXDC_OFFSET			0xf
+#define	BCCK_CCA_MODE				0xc000
+#define	BCCK_FALSECS_LIM			0x3f00
+#define	BCCK_CS_RATIO				0xc00000
+#define	BCCK_CORGBIT_SEL			0x300000
+#define	BCCK_PD_LIM				0x0f0000
+#define	BCCK_NEWCCA				0x80000000
+#define	BCCK_RXHP_OF_IG				0x8000
+#define	BCCK_RXIG				0x7f00
+#define	BCCK_LNA_POLARITY			0x800000
+#define	BCCK_RX1ST_BAIN				0x7f0000
+#define	BCCK_RF_EXTEND				0x20000000
+#define	BCCK_RXAGC_SATLEVEL			0x1f000000
+#define	BCCK_RXAGC_SATCOUNT			0xe0
+#define	bCCKRxRFSettle				0x1f
+#define	BCCK_FIXED_RXAGC			0x8000
+#define	BCCK_ANTENNA_POLARITY			0x2000
+#define	BCCK_TXFILTER_TYPE			0x0c00
+#define	BCCK_RXAGC_REPORTTYPE			0x0300
+#define	BCCK_RXDAGC_EN				0x80000000
+#define	BCCK_RXDAGC_PERIOD			0x20000000
+#define	BCCK_RXDAGC_SATLEVEL			0x1f000000
+#define	BCCK_TIMING_RECOVERY			0x800000
+#define	BCCK_TXC0				0x3f0000
+#define	BCCK_TXC1				0x3f000000
+#define	BCCK_TXC2				0x3f
+#define	BCCK_TXC3				0x3f00
+#define	BCCK_TXC4				0x3f0000
+#define	BCCK_TXC5				0x3f000000
+#define	BCCK_TXC6				0x3f
+#define	BCCK_TXC7				0x3f00
+#define	BCCK_DEBUGPORT				0xff0000
+#define	BCCK_DAC_DEBUG				0x0f000000
+#define	BCCK_FALSEALARM_ENABLE			0x8000
+#define	BCCK_FALSEALARM_READ			0x4000
+#define	BCCK_TRSSI				0x7f
+#define	BCCK_RXAGC_REPORT			0xfe
+#define	BCCK_RXREPORT_ANTSEL			0x80000000
+#define	BCCK_RXREPORT_MFOFF			0x40000000
+#define	BCCK_RXREPORT_SQLOSS			0x20000000
+#define	BCCK_RXREPORT_PKTLOSS			0x10000000
+#define	BCCK_RXREPORT_LOCKEDBIT			0x08000000
+#define	BCCK_RXREPORT_RATEERROR			0x04000000
+#define	BCCK_RXREPORT_RXRATE			0x03000000
+#define	BCCK_RXFA_COUNTER_LOWER			0xff
+#define	BCCK_RXFA_COUNTER_UPPER			0xff000000
+#define	BCCK_RXHPAGC_START			0xe000
+#define	BCCK_RXHPAGC_FINAL			0x1c00
+#define	BCCK_RXFALSEALARM_ENABLE		0x8000
+#define	BCCK_FACOUNTER_FREEZE			0x4000
+#define	BCCK_TXPATH_SEL				0x10000000
+#define	BCCK_DEFAULT_RXPATH			0xc000000
+#define	BCCK_OPTION_RXPATH			0x3000000
+
+#define	BNUM_OFSTF				0x3
+#define	BSHIFT_L				0xc0
+#define	BGI_TH					0xc
+#define	BRXPATH_A				0x1
+#define	BRXPATH_B				0x2
+#define	BRXPATH_C				0x4
+#define	BRXPATH_D				0x8
+#define	BTXPATH_A				0x1
+#define	BTXPATH_B				0x2
+#define	BTXPATH_C				0x4
+#define	BTXPATH_D				0x8
+#define	BTRSSI_FREQ				0x200
+#define	BADC_BACKOFF				0x3000
+#define	BDFIR_BACKOFF				0xc000
+#define	BTRSSI_LATCH_PHASE			0x10000
+#define	BRX_LDC_OFFSET				0xff
+#define	BRX_QDC_OFFSET				0xff00
+#define	BRX_DFIR_MODE				0x1800000
+#define	BRX_DCNF_TYPE				0xe000000
+#define	BRXIQIMB_A				0x3ff
+#define	BRXIQIMB_B				0xfc00
+#define	BRXIQIMB_C				0x3f0000
+#define	BRXIQIMB_D				0xffc00000
+#define	BDC_DC_NOTCH				0x60000
+#define	BRXNB_NOTCH				0x1f000000
+#define	BPD_TH					0xf
+#define	BPD_TH_OPT2				0xc000
+#define	BPWED_TH				0x700
+#define	BIFMF_WIN_L				0x800
+#define	BPD_OPTION				0x1000
+#define	BMF_WIN_L				0xe000
+#define	BBW_SEARCH_L				0x30000
+#define	BWIN_ENH_L				0xc0000
+#define	BBW_TH					0x700000
+#define	BED_TH2					0x3800000
+#define	BBW_OPTION				0x4000000
+#define	BRADIO_TH				0x18000000
+#define	BWINDOW_L				0xe0000000
+#define	BSBD_OPTION				0x1
+#define	BFRAME_TH				0x1c
+#define	BFS_OPTION				0x60
+#define	BDC_SLOPE_CHECK				0x80
+#define	BFGUARD_COUNTER_DC_L			0xe00
+#define	BFRAME_WEIGHT_SHORT			0x7000
+#define	BSUB_TUNE				0xe00000
+#define	BFRAME_DC_LENGTH			0xe000000
+#define	BSBD_START_OFFSET			0x30000000
+#define	BFRAME_TH_2				0x7
+#define	BFRAME_GI2_TH				0x38
+#define	BGI2_SYNC_EN				0x40
+#define	BSARCH_SHORT_EARLY			0x300
+#define	BSARCH_SHORT_LATE			0xc00
+#define	BSARCH_GI2_LATE				0x70000
+#define	BCFOANTSUM				0x1
+#define	BCFOACC					0x2
+#define	BCFOSTARTOFFSET				0xc
+#define	BCFOLOOPBACK				0x70
+#define	BCFOSUMWEIGHT				0x80
+#define	BDAGCENABLE				0x10000
+#define	BTXIQIMB_A				0x3ff
+#define	BTXIQIMB_b				0xfc00
+#define	BTXIQIMB_C				0x3f0000
+#define	BTXIQIMB_D				0xffc00000
+#define	BTXIDCOFFSET				0xff
+#define	BTXIQDCOFFSET				0xff00
+#define	BTXDFIRMODE				0x10000
+#define	BTXPESUDO_NOISEON			0x4000000
+#define	BTXPESUDO_NOISE_A			0xff
+#define	BTXPESUDO_NOISE_B			0xff00
+#define	BTXPESUDO_NOISE_C			0xff0000
+#define	BTXPESUDO_NOISE_D			0xff000000
+#define	BCCA_DROPOPTION				0x20000
+#define	BCCA_DROPTHRES				0xfff00000
+#define	BEDCCA_H				0xf
+#define	BEDCCA_L				0xf0
+#define	BLAMBDA_ED				0x300
+#define	BRX_INITIALGAIN				0x7f
+#define	BRX_ANTDIV_EN				0x80
+#define	BRX_AGC_ADDRESS_FOR_LNA			0x7f00
+#define	BRX_HIGHPOWER_FLOW			0x8000
+#define	BRX_AGC_FREEZE_THRES			0xc0000
+#define	BRX_FREEZESTEP_AGC1			0x300000
+#define	BRX_FREEZESTEP_AGC2			0xc00000
+#define	BRX_FREEZESTEP_AGC3			0x3000000
+#define	BRX_FREEZESTEP_AGC0			0xc000000
+#define	BRXRSSI_CMP_EN				0x10000000
+#define	BRXQUICK_AGCEN				0x20000000
+#define	BRXAGC_FREEZE_THRES_MODE		0x40000000
+#define	BRX_OVERFLOW_CHECKTYPE			0x80000000
+#define	BRX_AGCSHIFT				0x7f
+#define	BTRSW_TRI_ONLY				0x80
+#define	BPOWER_THRES				0x300
+#define	BRXAGC_EN				0x1
+#define	BRXAGC_TOGETHER_EN			0x2
+#define	BRXAGC_MIN				0x4
+#define	BRXHP_INI				0x7
+#define	BRXHP_TRLNA				0x70
+#define	BRXHP_RSSI				0x700
+#define	BRXHP_BBP1				0x7000
+#define	BRXHP_BBP2				0x70000
+#define	BRXHP_BBP3				0x700000
+#define	BRSSI_H					0x7f0000
+#define	BRSSI_GEN				0x7f000000
+#define	BRXSETTLE_TRSW				0x7
+#define	BRXSETTLE_LNA				0x38
+#define	BRXSETTLE_RSSI				0x1c0
+#define	BRXSETTLE_BBP				0xe00
+#define	BRXSETTLE_RXHP				0x7000
+#define	BRXSETTLE_ANTSW_RSSI			0x38000
+#define	BRXSETTLE_ANTSW				0xc0000
+#define	BRXPROCESS_TIME_DAGC			0x300000
+#define	BRXSETTLE_HSSI				0x400000
+#define	BRXPROCESS_TIME_BBPPW			0x800000
+#define	BRXANTENNA_POWER_SHIFT			0x3000000
+#define	BRSSI_TABLE_SELECT			0xc000000
+#define	BRXHP_FINAL				0x7000000
+#define	BRXHPSETTLE_BBP				0x7
+#define	BRXHTSETTLE_HSSI			0x8
+#define	BRXHTSETTLE_RXHP			0x70
+#define	BRXHTSETTLE_BBPPW			0x80
+#define	BRXHTSETTLE_IDLE			0x300
+#define	BRXHTSETTLE_RESERVED			0x1c00
+#define	BRXHT_RXHP_EN				0x8000
+#define	BRXAGC_FREEZE_THRES			0x30000
+#define	BRXAGC_TOGETHEREN			0x40000
+#define	BRXHTAGC_MIN				0x80000
+#define	BRXHTAGC_EN				0x100000
+#define	BRXHTDAGC_EN				0x200000
+#define	BRXHT_RXHP_BBP				0x1c00000
+#define	BRXHT_RXHP_FINAL			0xe0000000
+#define	BRXPW_RADIO_TH				0x3
+#define	BRXPW_RADIO_EN				0x4
+#define	BRXMF_HOLD				0x3800
+#define	BRXPD_DELAY_TH1				0x38
+#define	BRXPD_DELAY_TH2				0x1c0
+#define	BRXPD_DC_COUNT_MAX			0x600
+#define	BRXPD_DELAY_TH				0x8000
+#define	BRXPROCESS_DELAY			0xf0000
+#define	BRXSEARCHRANGE_GI2_EARLY		0x700000
+#define	BRXFRAME_FUARD_COUNTER_L		0x3800000
+#define	BRXSGI_GUARD_L				0xc000000
+#define	BRXSGI_SEARCH_L				0x30000000
+#define	BRXSGI_TH				0xc0000000
+#define	BDFSCNT0				0xff
+#define	BDFSCNT1				0xff00
+#define	BDFSFLAG				0xf0000
+#define	BMF_WEIGHT_SUM				0x300000
+#define	BMINIDX_TH				0x7f000000
+#define	BDAFORMAT				0x40000
+#define	BTXCH_EMU_ENABLE			0x01000000
+#define	BTRSW_ISOLATION_A			0x7f
+#define	BTRSW_ISOLATION_B			0x7f00
+#define	BTRSW_ISOLATION_C			0x7f0000
+#define	BTRSW_ISOLATION_D			0x7f000000
+#define	BEXT_LNA_GAIN				0x7c00
+
+#define	BSTBC_EN				0x4
+#define	BANTENNA_MAPPING			0x10
+#define	BNSS					0x20
+#define	BCFO_ANTSUM_ID				0x200
+#define	BPHY_COUNTER_RESET			0x8000000
+#define	BCFO_REPORT_GET				0x4000000
+#define	BOFDM_CONTINUE_TX			0x10000000
+#define	BOFDM_SINGLE_CARRIER			0x20000000
+#define	BOFDM_SINGLE_TONE			0x40000000
+#define	BHT_DETECT				0x100
+#define	BCFOEN					0x10000
+#define	BCFOVALUE				0xfff00000
+#define	BSIGTONE_RE				0x3f
+#define	BSIGTONE_IM				0x7f00
+#define	BCOUNTER_CCA				0xffff
+#define	BCOUNTER_PARITYFAIL			0xffff0000
+#define	BCOUNTER_RATEILLEGAL			0xffff
+#define	BCOUNTER_CRC8FAIL			0xffff0000
+#define	BCOUNTER_MCSNOSUPPORT			0xffff
+#define	BCOUNTER_FASTSYNC			0xffff
+#define	BSHORTCFO				0xfff
+#define	BSHORTCFOT_LENGTH			12
+#define	BSHORTCFOF_LENGTH			11
+#define	BLONGCFO				0x7ff
+#define	BLONGCFOT_LENGTH			11
+#define	BLONGCFOF_LENGTH			11
+#define	BTAILCFO				0x1fff
+#define	BTAILCFOT_LENGTH			13
+#define	BTAILCFOF_LENGTH			12
+#define	BNOISE_EN_PWDB				0xffff
+#define	BCC_POWER_DB				0xffff0000
+#define	BMOISE_PWDB				0xffff
+#define	BPOWERMEAST_LENGTH			10
+#define	BPOWERMEASF_LENGTH			3
+#define	BRX_HT_BW				0x1
+#define	BRXSC					0x6
+#define	BRX_HT					0x8
+#define	BNB_INTF_DET_ON				0x1
+#define	BINTF_WIN_LEN_CFG			0x30
+#define	BNB_INTF_TH_CFG				0x1c0
+#define	BRFGAIN					0x3f
+#define	BTABLESEL				0x40
+#define	BTRSW					0x80
+#define	BRXSNR_A				0xff
+#define	BRXSNR_B				0xff00
+#define	BRXSNR_C				0xff0000
+#define	BRXSNR_D				0xff000000
+#define	BSNR_EVMT_LENGTH			8
+#define	BSNR_EVMF_LENGTH			1
+#define	BCSI1ST					0xff
+#define	BCSI2ND					0xff00
+#define	BRXEVM1ST				0xff0000
+#define	BRXEVM2ND				0xff000000
+#define	BSIGEVM					0xff
+#define	BPWDB					0xff00
+#define	BSGIEN					0x10000
+
+#define	BSFACTOR_QMA1				0xf
+#define	BSFACTOR_QMA2				0xf0
+#define	BSFACTOR_QMA3				0xf00
+#define	BSFACTOR_QMA4				0xf000
+#define	BSFACTOR_QMA5				0xf0000
+#define	BSFACTOR_QMA6				0xf0000
+#define	BSFACTOR_QMA7				0xf00000
+#define	BSFACTOR_QMA8				0xf000000
+#define	BSFACTOR_QMA9				0xf0000000
+#define	BCSI_SCHEME				0x100000
+
+#define	BNOISE_LVL_TOP_SET			0x3
+#define	BCHSMOOTH				0x4
+#define	BCHSMOOTH_CFG1				0x38
+#define	BCHSMOOTH_CFG2				0x1c0
+#define	BCHSMOOTH_CFG3				0xe00
+#define	BCHSMOOTH_CFG4				0x7000
+#define	BMRCMODE				0x800000
+#define	BTHEVMCFG				0x7000000
+
+#define	BLOOP_FIT_TYPE				0x1
+#define	BUPD_CFO				0x40
+#define	BUPD_CFO_OFFDATA			0x80
+#define	BADV_UPD_CFO				0x100
+#define	BADV_TIME_CTRL				0x800
+#define	BUPD_CLKO				0x1000
+#define	BFC					0x6000
+#define	BTRACKING_MODE				0x8000
+#define	BPHCMP_ENABLE				0x10000
+#define	BUPD_CLKO_LTF				0x20000
+#define	BCOM_CH_CFO				0x40000
+#define	BCSI_ESTI_MODE				0x80000
+#define	BADV_UPD_EQZ				0x100000
+#define	BUCHCFG					0x7000000
+#define	BUPDEQZ					0x8000000
+
+#define	BRX_PESUDO_NOISE_ON			0x20000000
+#define	BRX_PESUDO_NOISE_A			0xff
+#define	BRX_PESUDO_NOISE_B			0xff00
+#define	BRX_PESUDO_NOISE_C			0xff0000
+#define	BRX_PESUDO_NOISE_D			0xff000000
+#define	BRX_PESUDO_NOISESTATE_A			0xffff
+#define	BRX_PESUDO_NOISESTATE_B			0xffff0000
+#define	BRX_PESUDO_NOISESTATE_C			0xffff
+#define	BRX_PESUDO_NOISESTATE_D			0xffff0000
+
+#define	BZEBRA1_HSSIENABLE			0x8
+#define	BZEBRA1_TRXCONTROL			0xc00
+#define	BZEBRA1_TRXGAINSETTING			0x07f
+#define	BZEBRA1_RXCOUNTER			0xc00
+#define	BZEBRA1_TXCHANGEPUMP			0x38
+#define	BZEBRA1_RXCHANGEPUMP			0x7
+#define	BZEBRA1_CHANNEL_NUM			0xf80
+#define	BZEBRA1_TXLPFBW				0x400
+#define	BZEBRA1_RXLPFBW				0x600
+
+#define	BRTL8256REG_MODE_CTRL1			0x100
+#define	BRTL8256REG_MODE_CTRL0			0x40
+#define	BRTL8256REG_TXLPFBW			0x18
+#define	BRTL8256REG_RXLPFBW			0x600
+
+#define	BRTL8258_TXLPFBW			0xc
+#define	BRTL8258_RXLPFBW			0xc00
+#define	BRTL8258_RSSILPFBW			0xc0
+
+#define	BBYTE0					0x1
+#define	BBYTE1					0x2
+#define	BBYTE2					0x4
+#define	BBYTE3					0x8
+#define	BWORD0					0x3
+#define	BWORD1					0xc
+#define	BWORD					0xf
+
+#define	MASKBYTE0				0xff
+#define	MASKBYTE1				0xff00
+#define	MASKBYTE2				0xff0000
+#define	MASKBYTE3				0xff000000
+#define	MASKHWORD				0xffff0000
+#define	MASKLWORD				0x0000ffff
+#define	MASKDWORD				0xffffffff
+#define	MASK12BITS				0xfff
+#define	MASKH4BITS				0xf0000000
+#define MASKOFDM_D				0xffc00000
+#define	MASKCCK					0x3f3f3f3f
+
+#define	MASK4BITS				0x0f
+#define	MASK20BITS				0xfffff
+#define RFREG_OFFSET_MASK			0xfffff
+
+#define	BENABLE					0x1
+#define	BDISABLE				0x0
+
+#define	LEFT_ANTENNA				0x0
+#define	RIGHT_ANTENNA				0x1
+
+#define	TCHECK_TXSTATUS				500
+#define	TUPDATE_RXCOUNTER			100
+
+#define	REG_UN_used_register			0x01bf
+
+/* WOL bit information */
+#define	HAL92C_WOL_PTK_UPDATE_EVENT		BIT(0)
+#define	HAL92C_WOL_GTK_UPDATE_EVENT		BIT(1)
+#define	HAL92C_WOL_DISASSOC_EVENT		BIT(2)
+#define	HAL92C_WOL_DEAUTH_EVENT			BIT(3)
+#define	HAL92C_WOL_FW_DISCONNECT_EVENT		BIT(4)
+
+#define WOL_REASON_PTK_UPDATE			BIT(0)
+#define WOL_REASON_GTK_UPDATE			BIT(1)
+#define WOL_REASON_DISASSOC			BIT(2)
+#define WOL_REASON_DEAUTH			BIT(3)
+#define WOL_REASON_FW_DISCONNECT		BIT(4)
+#endif
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/rf.c b/drivers/staging/rtl8192ee/rtl8192ee/rf.c
new file mode 100644
index 00000000000..4f5a49ebca1
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/rf.c
@@ -0,0 +1,150 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "rf.h"
+#include "dm.h"
+
+static bool _rtl92ee_phy_rf6052_config_parafile(struct ieee80211_hw *hw);
+
+void rtl92ee_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw, u8 bandwidth)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	switch (bandwidth) {
+	case HT_CHANNEL_WIDTH_20:
+		rtlphy->rfreg_chnlval[0] = ((rtlphy->rfreg_chnlval[0] &
+					     0xfffff3ff) | BIT(10) | BIT(11));
+		rtl_set_rfreg(hw, RF90_PATH_A, RF_CHNLBW, RFREG_OFFSET_MASK,
+			      rtlphy->rfreg_chnlval[0]);
+		rtl_set_rfreg(hw, RF90_PATH_B, RF_CHNLBW, RFREG_OFFSET_MASK,
+			      rtlphy->rfreg_chnlval[0]);
+		break;
+	case HT_CHANNEL_WIDTH_20_40:
+		rtlphy->rfreg_chnlval[0] = ((rtlphy->rfreg_chnlval[0] &
+					     0xfffff3ff) | BIT(10));
+		rtl_set_rfreg(hw, RF90_PATH_A, RF_CHNLBW, RFREG_OFFSET_MASK,
+			      rtlphy->rfreg_chnlval[0]);
+		rtl_set_rfreg(hw, RF90_PATH_B, RF_CHNLBW, RFREG_OFFSET_MASK,
+			      rtlphy->rfreg_chnlval[0]);
+		break;
+	default:
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("unknown bandwidth: %#X\n", bandwidth));
+		break;
+	}
+}
+
+bool rtl92ee_phy_rf6052_config(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+
+	if (rtlphy->rf_type == RF_1T1R)
+		rtlphy->num_total_rfpath = 1;
+	else
+		rtlphy->num_total_rfpath = 2;
+	return _rtl92ee_phy_rf6052_config_parafile(hw);
+}
+
+static bool _rtl92ee_phy_rf6052_config_parafile(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u32 u4_regvalue = 0;
+	u8 rfpath;
+	bool rtstatus = true;
+	struct bb_reg_def *pphyreg;
+
+	for (rfpath = 0; rfpath < rtlphy->num_total_rfpath; rfpath++) {
+		pphyreg = &rtlphy->phyreg_def[rfpath];
+
+		switch (rfpath) {
+		case RF90_PATH_A:
+		case RF90_PATH_C:
+			u4_regvalue = rtl_get_bbreg(hw, pphyreg->rfintfs,
+						    BRFSI_RFENV);
+			break;
+		case RF90_PATH_B:
+		case RF90_PATH_D:
+			u4_regvalue = rtl_get_bbreg(hw, pphyreg->rfintfs,
+						    BRFSI_RFENV << 16);
+			break;
+		}
+
+		rtl_set_bbreg(hw, pphyreg->rfintfe, BRFSI_RFENV << 16, 0x1);
+		udelay(1);
+
+		rtl_set_bbreg(hw, pphyreg->rfintfo, BRFSI_RFENV, 0x1);
+		udelay(1);
+
+		rtl_set_bbreg(hw, pphyreg->rfhssi_para2,
+			      B3WIREADDREAALENGTH, 0x0);
+		udelay(1);
+
+		rtl_set_bbreg(hw, pphyreg->rfhssi_para2, B3WIREDATALENGTH, 0x0);
+		udelay(1);
+
+		switch (rfpath) {
+		case RF90_PATH_A:
+			rtstatus = rtl92ee_phy_config_rf_with_headerfile(hw,
+						       (enum radio_path)rfpath);
+			break;
+		case RF90_PATH_B:
+			rtstatus = rtl92ee_phy_config_rf_with_headerfile(hw,
+						       (enum radio_path)rfpath);
+			break;
+		case RF90_PATH_C:
+			break;
+		case RF90_PATH_D:
+			break;
+		}
+
+		switch (rfpath) {
+		case RF90_PATH_A:
+		case RF90_PATH_C:
+			rtl_set_bbreg(hw, pphyreg->rfintfs,
+				      BRFSI_RFENV, u4_regvalue);
+			break;
+		case RF90_PATH_B:
+		case RF90_PATH_D:
+			rtl_set_bbreg(hw, pphyreg->rfintfs,
+				      BRFSI_RFENV << 16, u4_regvalue);
+			break;
+		}
+		if (!rtstatus) {
+			RT_TRACE(COMP_INIT, DBG_TRACE,
+				 ("Radio[%d] Fail!!", rfpath));
+			return false;
+		}
+	}
+
+	RT_TRACE(COMP_INIT, DBG_TRACE, ("\n"));
+	return rtstatus;
+}
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/rf.h b/drivers/staging/rtl8192ee/rtl8192ee/rf.h
new file mode 100644
index 00000000000..5bc394af476
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/rf.h
@@ -0,0 +1,39 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92E_RF_H__
+#define __RTL92E_RF_H__
+
+#define RF6052_MAX_TX_PWR		0x3F
+#define RF6052_MAX_REG			0x3F
+
+extern void rtl92ee_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw,
+					     u8 bandwidth);
+extern bool rtl92ee_phy_rf6052_config(struct ieee80211_hw *hw);
+#endif
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/sw.c b/drivers/staging/rtl8192ee/rtl8192ee/sw.c
new file mode 100644
index 00000000000..f9c5729e2cd
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/sw.c
@@ -0,0 +1,428 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include <linux/vmalloc.h>
+#include <linux/module.h>
+
+#include "../wifi.h"
+#include "../core.h"
+#include "../pci.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "dm.h"
+#include "hw.h"
+#include "sw.h"
+#include "fw.h"
+#include "trx.h"
+#include "led.h"
+#include "table.h"
+
+#include "../btcoexist/rtl_btc.h"
+
+
+static void rtl92ee_init_aspm_vars(struct ieee80211_hw *hw)
+{
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	/*close ASPM for AMD defaultly */
+	rtlpci->const_amdpci_aspm = 0;
+
+	/*
+	 * ASPM PS mode.
+	 * 0 - Disable ASPM,
+	 * 1 - Enable ASPM without Clock Req,
+	 * 2 - Enable ASPM with Clock Req,
+	 * 3 - Alwyas Enable ASPM with Clock Req,
+	 * 4 - Always Enable ASPM without Clock Req.
+	 * set defult to RTL8192CE:3 RTL8192E:2
+	 * */
+	rtlpci->const_pci_aspm = 3;
+
+	/*Setting for PCI-E device */
+	rtlpci->const_devicepci_aspm_setting = 0x03;
+
+	/*Setting for PCI-E bridge */
+	rtlpci->const_hostpci_aspm_setting = 0x02;
+
+	/*
+	 * In Hw/Sw Radio Off situation.
+	 * 0 - Default,
+	 * 1 - From ASPM setting without low Mac Pwr,
+	 * 2 - From ASPM setting with low Mac Pwr,
+	 * 3 - Bus D3
+	 * set default to RTL8192CE:0 RTL8192SE:2
+	 */
+	rtlpci->const_hwsw_rfoff_d3 = 0;
+
+	/*
+	 * This setting works for those device with
+	 * backdoor ASPM setting such as EPHY setting.
+	 * 0 - Not support ASPM,
+	 * 1 - Support ASPM,
+	 * 2 - According to chipset.
+	 */
+	rtlpci->const_support_pciaspm = 1;
+}
+
+int rtl92ee_init_sw_vars(struct ieee80211_hw *hw)
+{
+	int err = 0;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	const struct firmware *firmware;
+	char *fw_name = NULL;
+
+	rtl92ee_bt_reg_init(hw);
+
+	rtlpci->msi_support = true;
+	rtlpriv->btcoexist.btc_ops = stg_rtl_btc_get_ops_pointer();
+
+	rtlpriv->dm.b_dm_initialgain_enable = 1;
+	rtlpriv->dm.dm_flag = 0;
+	rtlpriv->dm.b_disable_framebursting = 0;
+	/*rtlpriv->dm.thermalvalue = 0;*/
+	rtlpci->transmit_config = CFENDFORM | BIT(15);
+
+	/*just 2.4G band*/
+	rtlpriv->rtlhal.current_bandtype = BAND_ON_2_4G;
+	rtlpriv->rtlhal.bandset = BAND_ON_2_4G;
+	rtlpriv->rtlhal.macphymode = SINGLEMAC_SINGLEPHY;
+
+	rtlpci->receive_config = (RCR_APPFCS			|
+				  RCR_APP_MIC			|
+				  RCR_APP_ICV			|
+				  RCR_APP_PHYST_RXFF		|
+				  RCR_HTC_LOC_CTRL		|
+				  RCR_AMF			|
+				  RCR_ACF			|
+				  RCR_ADF			|
+				  RCR_AICV			|
+				  RCR_ACRC32			|
+				  RCR_AB			|
+				  RCR_AM			|
+				  RCR_APM			|
+				  0);
+
+	rtlpci->irq_mask[0] = (u32) (IMR_PSTIMEOUT		|
+				/*   IMR_TBDER			|
+				     IMR_TBDOK			|
+				     IMR_BCNDMAINT0		|*/
+				     IMR_C2HCMD			|
+				     IMR_HIGHDOK		|
+				     IMR_MGNTDOK		|
+				     IMR_BKDOK			|
+				     IMR_BEDOK			|
+				     IMR_VIDOK			|
+				     IMR_VODOK			|
+				     IMR_RDU			|
+				     IMR_ROK			|
+				     0);
+	rtlpci->irq_mask[1] = (u32) (IMR_RXFOVW | 0);
+
+	/* for debug level */
+	rtlpriv->dbg.global_debuglevel = rtlpriv->cfg->mod_params->debug;
+	/* for LPS & IPS */
+	rtlpriv->psc.b_inactiveps = rtlpriv->cfg->mod_params->b_inactiveps;
+	rtlpriv->psc.b_swctrl_lps = rtlpriv->cfg->mod_params->b_swctrl_lps;
+	rtlpriv->psc.b_fwctrl_lps = rtlpriv->cfg->mod_params->b_fwctrl_lps;
+	rtlpriv->psc.b_reg_fwctrl_lps = 3;
+	rtlpriv->psc.reg_max_lps_awakeintvl = 5;
+	/* for ASPM, you can close aspm through
+	 * set const_support_pciaspm = 0 */
+	rtl92ee_init_aspm_vars(hw);
+
+	if (rtlpriv->psc.b_reg_fwctrl_lps == 1)
+		rtlpriv->psc.fwctrl_psmode = FW_PS_MIN_MODE;
+	else if (rtlpriv->psc.b_reg_fwctrl_lps == 2)
+		rtlpriv->psc.fwctrl_psmode = FW_PS_MAX_MODE;
+	else if (rtlpriv->psc.b_reg_fwctrl_lps == 3)
+		rtlpriv->psc.fwctrl_psmode = FW_PS_DTIM_MODE;
+
+	/* for early mode */
+	rtlpriv->rtlhal.b_earlymode_enable = false;
+
+	/*low power */
+	rtlpriv->psc.b_low_power_enable = false;
+
+
+	/* for firmware buf */
+	rtlpriv->rtlhal.pfirmware = vmalloc(0x8000);
+	if (!rtlpriv->rtlhal.pfirmware) {
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("Can't alloc buffer for fw.\n"));
+		return 1;
+	}
+
+	fw_name = "rtlwifi/rtl8192eefw.bin";
+	err = request_firmware(&firmware, fw_name, rtlpriv->io.dev);
+
+	if (err) {
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("Failed to request firmware!\n"));
+		return 1;
+	}
+	if (firmware->size > 0x8000) {
+		RT_TRACE(COMP_ERR, DBG_EMERG,
+			 ("Firmware is too big!\n"));
+		release_firmware(firmware);
+		return 1;
+	}
+	memcpy(rtlpriv->rtlhal.pfirmware, firmware->data, firmware->size);
+	rtlpriv->rtlhal.fwsize = firmware->size;
+	release_firmware(firmware);
+
+	return err;
+}
+
+void rtl92ee_deinit_sw_vars(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (rtlpriv->rtlhal.pfirmware) {
+		vfree(rtlpriv->rtlhal.pfirmware);
+		rtlpriv->rtlhal.pfirmware = NULL;
+	}
+}
+
+/* get bt coexist status */
+bool rtl92ee_get_btc_status(void)
+{
+	return true;
+}
+
+
+static struct rtl_hal_ops rtl8192ee_hal_ops = {
+	.init_sw_vars = rtl92ee_init_sw_vars,
+	.deinit_sw_vars = rtl92ee_deinit_sw_vars,
+	.read_eeprom_info = rtl92ee_read_eeprom_info,
+	.interrupt_recognized = rtl92ee_interrupt_recognized,/*need check*/
+	.hw_init = rtl92ee_hw_init,
+	.hw_disable = rtl92ee_card_disable,
+	.hw_suspend = rtl92ee_suspend,
+	.hw_resume = rtl92ee_resume,
+	.enable_interrupt = rtl92ee_enable_interrupt,
+	.disable_interrupt = rtl92ee_disable_interrupt,
+	.set_network_type = rtl92ee_set_network_type,
+	.set_chk_bssid = rtl92ee_set_check_bssid,
+	.set_qos = rtl92ee_set_qos,
+	.set_bcn_reg = rtl92ee_set_beacon_related_registers,
+	.set_bcn_intv = rtl92ee_set_beacon_interval,
+	.update_interrupt_mask = rtl92ee_update_interrupt_mask,
+	.get_hw_reg = rtl92ee_get_hw_reg,
+	.set_hw_reg = rtl92ee_set_hw_reg,
+	.update_rate_tbl = rtl92ee_update_hal_rate_tbl,
+	.pre_fill_tx_bd_desc = rtl92ee_pre_fill_tx_bd_desc,
+	.rx_desc_buff_remained_cnt = rtl92ee_rx_desc_buff_remained_cnt,
+	.rx_check_dma_ok = rtl92ee_rx_check_dma_ok,
+	.fill_tx_desc = rtl92ee_tx_fill_desc,
+	.fill_tx_cmddesc = rtl92ee_tx_fill_cmddesc,
+	.query_rx_desc = rtl92ee_rx_query_desc,
+	.set_channel_access = rtl92ee_update_channel_access_setting,
+	.radio_onoff_checking = rtl92ee_gpio_radio_on_off_checking,
+	.set_bw_mode = rtl92ee_phy_set_bw_mode,
+	.switch_channel = rtl92ee_phy_sw_chnl,
+	.dm_watchdog = rtl92ee_dm_watchdog,
+	.scan_operation_backup = rtl92ee_phy_scan_operation_backup,
+	.set_rf_power_state = rtl92ee_phy_set_rf_power_state,
+	.led_control = rtl92ee_led_control,
+	.set_desc = rtl92ee_set_desc,
+	.get_desc = rtl92ee_get_desc,
+	.is_tx_desc_closed = rtl92ee_is_tx_desc_closed,
+	.enable_hw_sec = rtl92ee_enable_hw_security_config,
+	.set_key = rtl92ee_set_key,
+	.init_sw_leds = rtl92ee_init_sw_leds,
+	.allow_all_destaddr = rtl92ee_allow_all_destaddr,
+	.get_bbreg = rtl92ee_phy_query_bb_reg,
+	.set_bbreg = rtl92ee_phy_set_bb_reg,
+	.get_rfreg = rtl92ee_phy_query_rf_reg,
+	.set_rfreg = rtl92ee_phy_set_rf_reg,
+	.fill_h2c_cmd = rtl92ee_fill_h2c_cmd,
+	.get_btc_status = rtl92ee_get_btc_status,
+	.rx_command_packet = rtl92ee_rx_command_packet,
+};
+
+static struct rtl_mod_params rtl92ee_mod_params = {
+	.sw_crypto = false,
+	.b_inactiveps = true,
+	.b_swctrl_lps = false,
+	.b_fwctrl_lps = true,
+	.debug = DBG_EMERG,
+};
+
+static struct rtl_hal_cfg rtl92ee_hal_cfg = {
+	.bar_id = 2,
+	.write_readback = true,
+	.name = "rtl92ee_pci",
+	.fw_name = "rtlwifi/rtl8192eefw.bin",
+	.ops = &rtl8192ee_hal_ops,
+	.mod_params = &rtl92ee_mod_params,
+
+	.maps[SYS_ISO_CTRL] = REG_SYS_ISO_CTRL,
+	.maps[SYS_FUNC_EN] = REG_SYS_FUNC_EN,
+	.maps[SYS_CLK] = REG_SYS_CLKR,
+	.maps[MAC_RCR_AM] = AM,
+	.maps[MAC_RCR_AB] = AB,
+	.maps[MAC_RCR_ACRC32] = ACRC32,
+	.maps[MAC_RCR_ACF] = ACF,
+	.maps[MAC_RCR_AAP] = AAP,
+	.maps[MAC_HIMR] = REG_HIMR,
+	.maps[MAC_HIMRE] = REG_HIMRE,
+
+	.maps[EFUSE_ACCESS] = REG_EFUSE_ACCESS,
+
+	.maps[EFUSE_TEST] = REG_EFUSE_TEST,
+	.maps[EFUSE_CTRL] = REG_EFUSE_CTRL,
+	.maps[EFUSE_CLK] = 0,
+	.maps[EFUSE_CLK_CTRL] = REG_EFUSE_CTRL,
+	.maps[EFUSE_PWC_EV12V] = PWC_EV12V,
+	.maps[EFUSE_FEN_ELDR] = FEN_ELDR,
+	.maps[EFUSE_LOADER_CLK_EN] = LOADER_CLK_EN,
+	.maps[EFUSE_ANA8M] = ANA8M,
+	.maps[EFUSE_HWSET_MAX_SIZE] = HWSET_MAX_SIZE,
+	.maps[EFUSE_MAX_SECTION_MAP] = EFUSE_MAX_SECTION,
+	.maps[EFUSE_REAL_CONTENT_SIZE] = EFUSE_REAL_CONTENT_LEN,
+	.maps[EFUSE_OOB_PROTECT_BYTES_LEN] = EFUSE_OOB_PROTECT_BYTES,
+
+	.maps[RWCAM] = REG_CAMCMD,
+	.maps[WCAMI] = REG_CAMWRITE,
+	.maps[RCAMO] = REG_CAMREAD,
+	.maps[CAMDBG] = REG_CAMDBG,
+	.maps[SECR] = REG_SECCFG,
+	.maps[SEC_CAM_NONE] = CAM_NONE,
+	.maps[SEC_CAM_WEP40] = CAM_WEP40,
+	.maps[SEC_CAM_TKIP] = CAM_TKIP,
+	.maps[SEC_CAM_AES] = CAM_AES,
+	.maps[SEC_CAM_WEP104] = CAM_WEP104,
+
+	.maps[RTL_IMR_BCNDMAINT6] = IMR_BCNDMAINT6,
+	.maps[RTL_IMR_BCNDMAINT5] = IMR_BCNDMAINT5,
+	.maps[RTL_IMR_BCNDMAINT4] = IMR_BCNDMAINT4,
+	.maps[RTL_IMR_BCNDMAINT3] = IMR_BCNDMAINT3,
+	.maps[RTL_IMR_BCNDMAINT2] = IMR_BCNDMAINT2,
+	.maps[RTL_IMR_BCNDMAINT1] = IMR_BCNDMAINT1,
+/*	.maps[RTL_IMR_BCNDOK8] = IMR_BCNDOK8,     */   /*need check*/
+	.maps[RTL_IMR_BCNDOK7] = IMR_BCNDOK7,
+	.maps[RTL_IMR_BCNDOK6] = IMR_BCNDOK6,
+	.maps[RTL_IMR_BCNDOK5] = IMR_BCNDOK5,
+	.maps[RTL_IMR_BCNDOK4] = IMR_BCNDOK4,
+	.maps[RTL_IMR_BCNDOK3] = IMR_BCNDOK3,
+	.maps[RTL_IMR_BCNDOK2] = IMR_BCNDOK2,
+	.maps[RTL_IMR_BCNDOK1] = IMR_BCNDOK1,
+/*	.maps[RTL_IMR_TIMEOUT2] = IMR_TIMEOUT2,*/
+/*	.maps[RTL_IMR_TIMEOUT1] = IMR_TIMEOUT1,*/
+
+	.maps[RTL_IMR_TXFOVW] = IMR_TXFOVW,
+	.maps[RTL_IMR_PSTIMEOUT] = IMR_PSTIMEOUT,
+	.maps[RTL_IMR_BcnInt] = IMR_BCNDMAINT0,
+	.maps[RTL_IMR_RXFOVW] = IMR_RXFOVW,
+	.maps[RTL_IMR_RDU] = IMR_RDU,
+	.maps[RTL_IMR_ATIMEND] = IMR_ATIMEND,
+	.maps[RTL_IMR_BDOK] = IMR_BCNDOK0,
+	.maps[RTL_IMR_MGNTDOK] = IMR_MGNTDOK,
+	.maps[RTL_IMR_TBDER] = IMR_TBDER,
+	.maps[RTL_IMR_HIGHDOK] = IMR_HIGHDOK,
+	.maps[RTL_IMR_TBDOK] = IMR_TBDOK,
+	.maps[RTL_IMR_BKDOK] = IMR_BKDOK,
+	.maps[RTL_IMR_BEDOK] = IMR_BEDOK,
+	.maps[RTL_IMR_VIDOK] = IMR_VIDOK,
+	.maps[RTL_IMR_VODOK] = IMR_VODOK,
+	.maps[RTL_IMR_ROK] = IMR_ROK,
+	.maps[RTL_IBSS_INT_MASKS] = (IMR_BCNDMAINT0 | IMR_TBDOK | IMR_TBDER),
+
+	.maps[RTL_RC_CCK_RATE1M] = DESC92C_RATE1M,
+	.maps[RTL_RC_CCK_RATE2M] = DESC92C_RATE2M,
+	.maps[RTL_RC_CCK_RATE5_5M] = DESC92C_RATE5_5M,
+	.maps[RTL_RC_CCK_RATE11M] = DESC92C_RATE11M,
+	.maps[RTL_RC_OFDM_RATE6M] = DESC92C_RATE6M,
+	.maps[RTL_RC_OFDM_RATE9M] = DESC92C_RATE9M,
+	.maps[RTL_RC_OFDM_RATE12M] = DESC92C_RATE12M,
+	.maps[RTL_RC_OFDM_RATE18M] = DESC92C_RATE18M,
+	.maps[RTL_RC_OFDM_RATE24M] = DESC92C_RATE24M,
+	.maps[RTL_RC_OFDM_RATE36M] = DESC92C_RATE36M,
+	.maps[RTL_RC_OFDM_RATE48M] = DESC92C_RATE48M,
+	.maps[RTL_RC_OFDM_RATE54M] = DESC92C_RATE54M,
+
+	.maps[RTL_RC_HT_RATEMCS7] = DESC92C_RATEMCS7,
+	.maps[RTL_RC_HT_RATEMCS15] = DESC92C_RATEMCS15,
+};
+
+static struct pci_device_id rtl92ee_pci_ids[] = {
+	{RTL_PCI_DEVICE(PCI_VENDOR_ID_REALTEK, 0x818B, rtl92ee_hal_cfg)},
+	{},
+};
+
+MODULE_DEVICE_TABLE(pci, rtl92ee_pci_ids);
+
+MODULE_AUTHOR("Realtek WlanFAE	<wlanfae@realtek.com>");
+MODULE_AUTHOR("Larry Finger	<Larry.Finger@lwfinger.net>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Realtek 8192E 802.11n PCI wireless");
+MODULE_FIRMWARE("rtlwifi/rtl8192eefw.bin");
+
+module_param_named(swenc, rtl92ee_mod_params.sw_crypto, bool, 0444);
+module_param_named(debug, rtl92ee_mod_params.debug, int, 0444);
+module_param_named(ips, rtl92ee_mod_params.b_inactiveps, bool, 0444);
+module_param_named(swlps, rtl92ee_mod_params.b_swctrl_lps, bool, 0444);
+module_param_named(fwlps, rtl92ee_mod_params.b_fwctrl_lps, bool, 0444);
+MODULE_PARM_DESC(swenc, "using hardware crypto (default 0 [hardware])\n");
+MODULE_PARM_DESC(ips, "using no link power save (default 1 is open)\n");
+MODULE_PARM_DESC(fwlps, "using linked fw control power save (default 1 is open)\n");
+MODULE_PARM_DESC(debug, "Set debug level (0-5) (default 0)");
+
+static SIMPLE_DEV_PM_OPS(rtlwifi_pm_ops, stg_rtl_pci_suspend,
+			 stg_rtl_pci_resume);
+
+static struct pci_driver rtl92ee_driver = {
+	.name = KBUILD_MODNAME,
+	.id_table = rtl92ee_pci_ids,
+	.probe = stg_rtl_pci_probe,
+	.remove = stg_rtl_pci_disconnect,
+
+	.driver.pm = &rtlwifi_pm_ops,
+};
+
+static int __init rtl92ee_module_init(void)
+{
+	int ret;
+	ret = rtl_core_module_init();
+	if (ret)
+		return ret;
+
+	ret = pci_register_driver(&rtl92ee_driver);
+	if (ret)
+		RT_ASSERT(false, (": No device found\n"));
+
+	return ret;
+}
+
+static void __exit rtl92ee_module_exit(void)
+{
+	pci_unregister_driver(&rtl92ee_driver);
+	rtl_core_module_exit();
+}
+
+module_init(rtl92ee_module_init);
+module_exit(rtl92ee_module_exit);
diff --git a/drivers/staging/rtl8188eu/include/drv_types_linux.h b/drivers/staging/rtl8192ee/rtl8192ee/sw.h
index 812b7440d4b..0170257a20c 100644
--- a/drivers/staging/rtl8188eu/include/drv_types_linux.h
+++ b/drivers/staging/rtl8192ee/rtl8192ee/sw.h
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ * Copyright(c) 2009-2010  Realtek Corporation.
  *
  * 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
@@ -8,17 +8,32 @@
  *
  * 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
+ * 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, USA
  *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
  *
- ******************************************************************************/
-#ifndef __DRV_TYPES_LINUX_H__
-#define __DRV_TYPES_LINUX_H__
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92E_SW_H__
+#define __RTL92E_SW_H__
 
+int rtl92ee_init_sw_vars(struct ieee80211_hw *hw);
+void rtl92ee_deinit_sw_vars(struct ieee80211_hw *hw);
+bool rtl92ee_get_btc_status(void);
+int rtl_core_module_init(void);
+void rtl_core_module_exit(void);
 
 #endif
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/table.c b/drivers/staging/rtl8192ee/rtl8192ee/table.c
new file mode 100644
index 00000000000..c7eb9be53cc
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/table.c
@@ -0,0 +1,882 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Created on  2010/ 5/18,  1:41
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "table.h"
+u32 RTL8192EE_PHY_REG_ARRAY[] = {
+		0x800, 0x80040000,
+		0x804, 0x00000003,
+		0x808, 0x0000FC00,
+		0x80C, 0x0000000A,
+		0x810, 0x10001331,
+		0x814, 0x020C3D10,
+		0x818, 0x02220385,
+		0x81C, 0x00000000,
+		0x820, 0x01000100,
+		0x824, 0x00390204,
+		0x828, 0x01000100,
+		0x82C, 0x00390204,
+		0x830, 0x32323232,
+		0x834, 0x30303030,
+		0x838, 0x30303030,
+		0x83C, 0x30303030,
+		0x840, 0x00010000,
+		0x844, 0x00010000,
+		0x848, 0x28282828,
+		0x84C, 0x28282828,
+		0x850, 0x00000000,
+		0x854, 0x00000000,
+		0x858, 0x009A009A,
+		0x85C, 0x01000014,
+		0x860, 0x66F60000,
+		0x864, 0x061F0000,
+		0x868, 0x30303030,
+		0x86C, 0x30303030,
+		0x870, 0x00000000,
+		0x874, 0x55004200,
+		0x878, 0x08080808,
+		0x87C, 0x00000000,
+		0x880, 0xB0000C1C,
+		0x884, 0x00000001,
+		0x888, 0x00000000,
+		0x88C, 0xCC0000C0,
+		0x890, 0x00000800,
+		0x894, 0xFFFFFFFE,
+		0x898, 0x40302010,
+		0x900, 0x00000000,
+		0x904, 0x00000023,
+		0x908, 0x00000000,
+		0x90C, 0x81121313,
+		0x910, 0x806C0001,
+		0x914, 0x00000001,
+		0x918, 0x00000000,
+		0x91C, 0x00010000,
+		0x924, 0x00000001,
+		0x928, 0x00000000,
+		0x92C, 0x00000000,
+		0x930, 0x00000000,
+		0x934, 0x00000000,
+		0x938, 0x00000000,
+		0x93C, 0x00000000,
+		0x940, 0x00000000,
+		0x944, 0x00000000,
+		0x94C, 0x00000008,
+		0xA00, 0x00D0C7C8,
+		0xA04, 0x81FF000C,
+		0xA08, 0x8C838300,
+		0xA0C, 0x2E68120F,
+		0xA10, 0x95009B78,
+		0xA14, 0x1114D028,
+		0xA18, 0x00881117,
+		0xA1C, 0x89140F00,
+		0xA20, 0x1A1B0000,
+		0xA24, 0x090E1317,
+		0xA28, 0x00000204,
+		0xA2C, 0x00D30000,
+		0xA70, 0x101FBF00,
+		0xA74, 0x00000007,
+		0xA78, 0x00000900,
+		0xA7C, 0x225B0606,
+		0xA80, 0x218075B1,
+		0xB38, 0x00000000,
+		0xC00, 0x48071D40,
+		0xC04, 0x03A05633,
+		0xC08, 0x000000E4,
+		0xC0C, 0x6C6C6C6C,
+		0xC10, 0x08800000,
+		0xC14, 0x40000100,
+		0xC18, 0x08800000,
+		0xC1C, 0x40000100,
+		0xC20, 0x00000000,
+		0xC24, 0x00000000,
+		0xC28, 0x00000000,
+		0xC2C, 0x00000000,
+		0xC30, 0x69E9AC47,
+		0xC34, 0x469652AF,
+		0xC38, 0x49795994,
+		0xC3C, 0x0A97971C,
+		0xC40, 0x1F7C403F,
+		0xC44, 0x000100B7,
+		0xC48, 0xEC020107,
+		0xC4C, 0x007F037F,
+	0xFF010718, 0xABCD,
+		0xC50, 0x00340220,
+	0xCDCDCDCD, 0xCDCD,
+		0xC50, 0x00340020,
+	0xFF010718, 0xDEAD,
+		0xC54, 0x0080801F,
+	0xFF010718, 0xABCD,
+		0xC58, 0x00000220,
+	0xCDCDCDCD, 0xCDCD,
+		0xC58, 0x00000020,
+	0xFF010718, 0xDEAD,
+		0xC5C, 0x00248492,
+		0xC60, 0x00000000,
+		0xC64, 0x7112848B,
+		0xC68, 0x47C00BFF,
+		0xC6C, 0x00000036,
+		0xC70, 0x00000600,
+		0xC74, 0x02013169,
+		0xC78, 0x0000001F,
+		0xC7C, 0x00B91612,
+	0xFF010718, 0xABCD,
+		0xC80, 0x2D4000B5,
+	0xCDCDCDCD, 0xCDCD,
+		0xC80, 0x40000100,
+	0xFF010718, 0xDEAD,
+		0xC84, 0x21F60000,
+	0xFF010718, 0xABCD,
+		0xC88, 0x2D4000B5,
+	0xCDCDCDCD, 0xCDCD,
+		0xC88, 0x40000100,
+	0xFF010718, 0xDEAD,
+		0xC8C, 0xA0E40000,
+		0xC90, 0x00121820,
+		0xC94, 0x00000000,
+		0xC98, 0x00121820,
+		0xC9C, 0x00007F7F,
+		0xCA0, 0x00000000,
+		0xCA4, 0x000300A0,
+		0xCA8, 0x00000000,
+		0xCAC, 0x00000000,
+		0xCB0, 0x00000000,
+		0xCB4, 0x00000000,
+		0xCB8, 0x00000000,
+		0xCBC, 0x28000000,
+		0xCC0, 0x00000000,
+		0xCC4, 0x00000000,
+		0xCC8, 0x00000000,
+		0xCCC, 0x00000000,
+		0xCD0, 0x00000000,
+		0xCD4, 0x00000000,
+		0xCD8, 0x64B22427,
+		0xCDC, 0x00766932,
+		0xCE0, 0x00222222,
+		0xCE4, 0x00040000,
+		0xCE8, 0x77644302,
+		0xCEC, 0x2F97D40C,
+		0xD00, 0x00080740,
+		0xD04, 0x00020403,
+		0xD08, 0x0000907F,
+		0xD0C, 0x20010201,
+		0xD10, 0xA0633333,
+		0xD14, 0x3333BC43,
+		0xD18, 0x7A8F5B6B,
+		0xD1C, 0x0000007F,
+		0xD2C, 0xCC979975,
+		0xD30, 0x00000000,
+		0xD34, 0x80608000,
+		0xD38, 0x00000000,
+		0xD3C, 0x00127353,
+		0xD40, 0x00000000,
+		0xD44, 0x00000000,
+		0xD48, 0x00000000,
+		0xD4C, 0x00000000,
+		0xD50, 0x6437140A,
+		0xD54, 0x00000000,
+		0xD58, 0x00000282,
+		0xD5C, 0x30032064,
+		0xD60, 0x4653DE68,
+		0xD64, 0x04518A3C,
+		0xD68, 0x00002101,
+		0xD6C, 0x2A201C16,
+		0xD70, 0x1812362E,
+		0xD74, 0x322C2220,
+		0xD78, 0x000E3C24,
+		0xD80, 0x01081008,
+		0xD84, 0x00000800,
+		0xD88, 0xF0B50000,
+		0xE00, 0x30303030,
+		0xE04, 0x30303030,
+		0xE08, 0x03903030,
+		0xE10, 0x30303030,
+		0xE14, 0x30303030,
+		0xE18, 0x30303030,
+		0xE1C, 0x30303030,
+		0xE28, 0x00000000,
+		0xE30, 0x1000DC1F,
+		0xE34, 0x10008C1F,
+		0xE38, 0x02140102,
+		0xE3C, 0x681604C2,
+		0xE40, 0x01007C00,
+		0xE44, 0x01004800,
+		0xE48, 0xFB000000,
+		0xE4C, 0x000028D1,
+		0xE50, 0x1000DC1F,
+		0xE54, 0x10008C1F,
+		0xE58, 0x02140102,
+		0xE5C, 0x28160D05,
+		0xE60, 0x00000008,
+		0xE68, 0x0FC05656,
+		0xE6C, 0x03C09696,
+		0xE70, 0x03C09696,
+		0xE74, 0x0C005656,
+		0xE78, 0x0C005656,
+		0xE7C, 0x0C005656,
+		0xE80, 0x0C005656,
+		0xE84, 0x03C09696,
+		0xE88, 0x0C005656,
+		0xE8C, 0x03C09696,
+		0xED0, 0x03C09696,
+		0xED4, 0x03C09696,
+		0xED8, 0x03C09696,
+		0xEDC, 0x0000D6D6,
+		0xEE0, 0x0000D6D6,
+		0xEEC, 0x0FC01616,
+		0xEE4, 0xB0000C1C,
+		0xEE8, 0x00000001,
+		0xF14, 0x00000003,
+		0xF4C, 0x00000000,
+		0xF00, 0x00000300,
+};
+
+u32 RTL8192EE_PHY_REG_ARRAY_PG[] = {
+	0, 0, 0, 0x00000e08, 0x0000ff00, 0x00003200,
+	0, 0, 1, 0x00000e08, 0x0000ff00, 0x00003200,
+	0, 0, 0, 0x0000086c, 0xffffff00, 0x32323200,
+	0, 0, 1, 0x0000086c, 0xffffff00, 0x32323200,
+	0, 0, 0, 0x00000e00, 0xffffffff, 0x34343636,
+	0, 0, 1, 0x00000e00, 0xffffffff, 0x34343636,
+	0, 0, 0, 0x00000e04, 0xffffffff, 0x28283032,
+	0, 0, 1, 0x00000e04, 0xffffffff, 0x28283032,
+	0, 0, 0, 0x00000e10, 0xffffffff, 0x34363840,
+	0, 0, 1, 0x00000e10, 0xffffffff, 0x34363840,
+	0, 0, 0, 0x00000e14, 0xffffffff, 0x26283032,
+	0, 0, 1, 0x00000e14, 0xffffffff, 0x26283032,
+	0, 0, 1, 0x00000e18, 0xffffffff, 0x36384040,
+	0, 0, 1, 0x00000e1c, 0xffffffff, 0x24262832,
+	0, 1, 0, 0x00000838, 0xffffff00, 0x32323200,
+	0, 1, 1, 0x00000838, 0xffffff00, 0x32323200,
+	0, 1, 0, 0x0000086c, 0x000000ff, 0x00000032,
+	0, 1, 1, 0x0000086c, 0x000000ff, 0x00000032,
+	0, 1, 0, 0x00000830, 0xffffffff, 0x34343636,
+	0, 1, 1, 0x00000830, 0xffffffff, 0x34343636,
+	0, 1, 0, 0x00000834, 0xffffffff, 0x28283032,
+	0, 1, 1, 0x00000834, 0xffffffff, 0x28283032,
+	0, 1, 0, 0x0000083c, 0xffffffff, 0x34363840,
+	0, 1, 1, 0x0000083c, 0xffffffff, 0x34363840,
+	0, 1, 0, 0x00000848, 0xffffffff, 0x26283032,
+	0, 1, 1, 0x00000848, 0xffffffff, 0x26283032,
+	0, 1, 1, 0x0000084c, 0xffffffff, 0x36384040,
+	0, 1, 1, 0x00000868, 0xffffffff, 0x24262832
+};
+
+u32 RTL8192EE_RADIOA_ARRAY[] = {
+		0x07F, 0x00000082,
+		0x081, 0x0003FC00,
+		0x000, 0x00030000,
+		0x008, 0x00008400,
+		0x018, 0x00000407,
+		0x019, 0x00000012,
+		0x01B, 0x00000064,
+		0x01E, 0x00080009,
+		0x01F, 0x00000880,
+		0x02F, 0x0001A060,
+		0x03F, 0x00000000,
+		0x042, 0x000060C0,
+		0x057, 0x000D0000,
+		0x058, 0x000BE180,
+		0x067, 0x00001552,
+		0x083, 0x00000000,
+		0x0B0, 0x000FF9F1,
+		0x0B1, 0x00055418,
+		0x0B2, 0x0008CC00,
+		0x0B4, 0x00043083,
+		0x0B5, 0x00008166,
+		0x0B6, 0x0000803E,
+		0x0B7, 0x0001C69F,
+		0x0B8, 0x0000407F,
+		0x0B9, 0x00080001,
+		0x0BA, 0x00040001,
+		0x0BB, 0x00000400,
+		0x0BF, 0x000C0000,
+		0x0C2, 0x00002400,
+		0x0C3, 0x00000009,
+		0x0C4, 0x00040C91,
+		0x0C5, 0x00099999,
+		0x0C6, 0x000000A3,
+		0x0C7, 0x00088820,
+		0x0C8, 0x00076C06,
+		0x0C9, 0x00000000,
+		0x0CA, 0x00080000,
+		0x0DF, 0x00000180,
+		0x0EF, 0x000001A0,
+		0x051, 0x00069545,
+		0x052, 0x0007E45E,
+		0x053, 0x00000071,
+		0x056, 0x00051FF3,
+		0x035, 0x000000A8,
+		0x035, 0x000001E2,
+		0x035, 0x000002A8,
+		0x036, 0x00001C24,
+		0x036, 0x00009C24,
+		0x036, 0x00011C24,
+		0x036, 0x00019C24,
+		0x018, 0x00000C07,
+		0x05A, 0x00048000,
+		0x019, 0x000739D0,
+	0xFF010718, 0xABCD,
+		0x034, 0x0000A093,
+		0x034, 0x0000908F,
+		0x034, 0x0000808C,
+		0x034, 0x0000704D,
+		0x034, 0x0000604A,
+		0x034, 0x00005047,
+		0x034, 0x0000400A,
+		0x034, 0x00003007,
+		0x034, 0x00002004,
+		0x034, 0x00001001,
+		0x034, 0x00000000,
+	0xCDCDCDCD, 0xCDCD,
+		0x034, 0x0000ADD7,
+		0x034, 0x00009DD4,
+		0x034, 0x00008DD1,
+		0x034, 0x00007DCE,
+		0x034, 0x00006DCB,
+		0x034, 0x00005DC8,
+		0x034, 0x00004DC5,
+		0x034, 0x000034CC,
+		0x034, 0x0000244F,
+		0x034, 0x0000144C,
+		0x034, 0x00000014,
+	0xFF010718, 0xDEAD,
+		0x000, 0x00030159,
+		0x084, 0x00068180,
+		0x086, 0x0000014E,
+		0x087, 0x00048E00,
+		0x08E, 0x00065540,
+		0x08F, 0x00088000,
+		0x0EF, 0x000020A0,
+	0xFF010718, 0xABCD,
+		0x03B, 0x000F07B0,
+	0xCDCDCDCD, 0xCDCD,
+		0x03B, 0x000F02B0,
+	0xFF010718, 0xDEAD,
+		0x03B, 0x000EF7B0,
+		0x03B, 0x000D4FB0,
+		0x03B, 0x000CF060,
+		0x03B, 0x000B0090,
+		0x03B, 0x000A0080,
+		0x03B, 0x00090080,
+		0x03B, 0x0008F780,
+	0xFF010718, 0xABCD,
+		0x03B, 0x000787B0,
+	0xCDCDCDCD, 0xCDCD,
+		0x03B, 0x00078730,
+	0xFF010718, 0xDEAD,
+		0x03B, 0x00060FB0,
+		0x03B, 0x0005FFA0,
+		0x03B, 0x00040620,
+		0x03B, 0x00037090,
+		0x03B, 0x00020080,
+		0x03B, 0x0001F060,
+		0x03B, 0x0000FFB0,
+		0x0EF, 0x000000A0,
+		0x0FE, 0x00000000,
+		0x018, 0x0000FC07,
+		0x0FE, 0x00000000,
+		0x0FE, 0x00000000,
+		0x0FE, 0x00000000,
+		0x0FE, 0x00000000,
+		0x01E, 0x00000001,
+		0x01F, 0x00080000,
+		0x000, 0x00033E70,
+};
+
+u32 RTL8192EE_RADIOB_ARRAY[] = {
+		0x07F, 0x00000082,
+		0x081, 0x0003FC00,
+		0x000, 0x00030000,
+		0x008, 0x00008400,
+		0x018, 0x00000407,
+		0x019, 0x00000012,
+		0x01B, 0x00000064,
+		0x01E, 0x00080009,
+		0x01F, 0x00000880,
+		0x02F, 0x0001A060,
+		0x03F, 0x00000000,
+		0x042, 0x000060C0,
+		0x057, 0x000D0000,
+		0x058, 0x000BE180,
+		0x067, 0x00001552,
+		0x07F, 0x00000082,
+		0x081, 0x0003F000,
+		0x083, 0x00000000,
+		0x0DF, 0x00000180,
+		0x0EF, 0x000001A0,
+		0x051, 0x00069545,
+		0x052, 0x0007E42E,
+		0x053, 0x00000071,
+		0x056, 0x00051FF3,
+		0x035, 0x000000A8,
+		0x035, 0x000001E0,
+		0x035, 0x000002A8,
+		0x036, 0x00001CA8,
+		0x036, 0x00009C24,
+		0x036, 0x00011C24,
+		0x036, 0x00019C24,
+		0x018, 0x00000C07,
+		0x05A, 0x00048000,
+		0x019, 0x000739D0,
+	0xFF010718, 0xABCD,
+		0x034, 0x0000A093,
+		0x034, 0x0000908F,
+		0x034, 0x0000808C,
+		0x034, 0x0000704D,
+		0x034, 0x0000604A,
+		0x034, 0x00005047,
+		0x034, 0x0000400A,
+		0x034, 0x00003007,
+		0x034, 0x00002004,
+		0x034, 0x00001001,
+		0x034, 0x00000000,
+	0xCDCDCDCD, 0xCDCD,
+		0x034, 0x0000ADD7,
+		0x034, 0x00009DD4,
+		0x034, 0x00008DD1,
+		0x034, 0x00007DCE,
+		0x034, 0x00006DCB,
+		0x034, 0x00005DC8,
+		0x034, 0x00004DC5,
+		0x034, 0x000034CC,
+		0x034, 0x0000244F,
+		0x034, 0x0000144C,
+		0x034, 0x00000014,
+	0xFF010718, 0xDEAD,
+		0x000, 0x00030159,
+		0x084, 0x00068180,
+		0x086, 0x000000CE,
+		0x087, 0x00048A00,
+		0x08E, 0x00065540,
+		0x08F, 0x00088000,
+		0x0EF, 0x000020A0,
+	0xFF010718, 0xABCD,
+		0x03B, 0x000F07B0,
+	0xCDCDCDCD, 0xCDCD,
+		0x03B, 0x000F02B0,
+	0xFF010718, 0xDEAD,
+		0x03B, 0x000EF7B0,
+		0x03B, 0x000D4FB0,
+		0x03B, 0x000CF060,
+		0x03B, 0x000B0090,
+		0x03B, 0x000A0080,
+		0x03B, 0x00090080,
+		0x03B, 0x0008F780,
+	0xFF010718, 0xABCD,
+		0x03B, 0x000787B0,
+	0xCDCDCDCD, 0xCDCD,
+		0x03B, 0x00078730,
+	0xFF010718, 0xDEAD,
+		0x03B, 0x00060FB0,
+		0x03B, 0x0005FFA0,
+		0x03B, 0x00040620,
+		0x03B, 0x00037090,
+		0x03B, 0x00020080,
+		0x03B, 0x0001F060,
+		0x03B, 0x0000FFB0,
+		0x0EF, 0x000000A0,
+		0x000, 0x00010159,
+		0x0FE, 0x00000000,
+		0x0FE, 0x00000000,
+		0x0FE, 0x00000000,
+		0x0FE, 0x00000000,
+		0x01E, 0x00000001,
+		0x01F, 0x00080000,
+		0x000, 0x00033E70,
+};
+
+u32 RTL8192EE_MAC_ARRAY[] = {
+		0x011, 0x000000EB,
+		0x012, 0x00000007,
+		0x014, 0x00000075,
+		0x303, 0x000000A7,
+		0x428, 0x0000000A,
+		0x429, 0x00000010,
+		0x430, 0x00000000,
+		0x431, 0x00000000,
+		0x432, 0x00000000,
+		0x433, 0x00000001,
+		0x434, 0x00000004,
+		0x435, 0x00000005,
+		0x436, 0x00000007,
+		0x437, 0x00000008,
+		0x43C, 0x00000004,
+		0x43D, 0x00000005,
+		0x43E, 0x00000007,
+		0x43F, 0x00000008,
+		0x440, 0x0000005D,
+		0x441, 0x00000001,
+		0x442, 0x00000000,
+		0x444, 0x00000010,
+		0x445, 0x00000000,
+		0x446, 0x00000000,
+		0x447, 0x00000000,
+		0x448, 0x00000000,
+		0x449, 0x000000F0,
+		0x44A, 0x0000000F,
+		0x44B, 0x0000003E,
+		0x44C, 0x00000010,
+		0x44D, 0x00000000,
+		0x44E, 0x00000000,
+		0x44F, 0x00000000,
+		0x450, 0x00000000,
+		0x451, 0x000000F0,
+		0x452, 0x0000000F,
+		0x453, 0x00000000,
+		0x456, 0x0000005E,
+		0x460, 0x00000066,
+		0x461, 0x00000066,
+		0x4C8, 0x000000FF,
+		0x4C9, 0x00000008,
+		0x4CC, 0x000000FF,
+		0x4CD, 0x000000FF,
+		0x4CE, 0x00000001,
+		0x500, 0x00000026,
+		0x501, 0x000000A2,
+		0x502, 0x0000002F,
+		0x503, 0x00000000,
+		0x504, 0x00000028,
+		0x505, 0x000000A3,
+		0x506, 0x0000005E,
+		0x507, 0x00000000,
+		0x508, 0x0000002B,
+		0x509, 0x000000A4,
+		0x50A, 0x0000005E,
+		0x50B, 0x00000000,
+		0x50C, 0x0000004F,
+		0x50D, 0x000000A4,
+		0x50E, 0x00000000,
+		0x50F, 0x00000000,
+		0x512, 0x0000001C,
+		0x514, 0x0000000A,
+		0x516, 0x0000000A,
+		0x525, 0x0000004F,
+		0x540, 0x00000012,
+		0x541, 0x00000064,
+		0x550, 0x00000010,
+		0x551, 0x00000010,
+		0x559, 0x00000002,
+		0x55C, 0x00000050,
+		0x55D, 0x000000FF,
+		0x605, 0x00000030,
+		0x608, 0x0000000E,
+		0x609, 0x0000002A,
+		0x620, 0x000000FF,
+		0x621, 0x000000FF,
+		0x622, 0x000000FF,
+		0x623, 0x000000FF,
+		0x624, 0x000000FF,
+		0x625, 0x000000FF,
+		0x626, 0x000000FF,
+		0x627, 0x000000FF,
+		0x638, 0x00000050,
+		0x63C, 0x0000000A,
+		0x63D, 0x0000000A,
+		0x63E, 0x0000000E,
+		0x63F, 0x0000000E,
+		0x640, 0x00000040,
+		0x642, 0x00000040,
+		0x643, 0x00000000,
+		0x652, 0x000000C8,
+		0x66E, 0x00000005,
+		0x700, 0x00000021,
+		0x701, 0x00000043,
+		0x702, 0x00000065,
+		0x703, 0x00000087,
+		0x708, 0x00000021,
+		0x709, 0x00000043,
+		0x70A, 0x00000065,
+		0x70B, 0x00000087,
+};
+
+u32 RTL8192EE_AGC_TAB_ARRAY[] = {
+	0xFF010718, 0xABCD,
+		0xC78, 0xFA000001,
+		0xC78, 0xF9010001,
+		0xC78, 0xF8020001,
+		0xC78, 0xF7030001,
+		0xC78, 0xF6040001,
+		0xC78, 0xF5050001,
+		0xC78, 0xF4060001,
+		0xC78, 0xF3070001,
+		0xC78, 0xF2080001,
+		0xC78, 0xF1090001,
+		0xC78, 0xF00A0001,
+		0xC78, 0xEF0B0001,
+		0xC78, 0xEE0C0001,
+		0xC78, 0xED0D0001,
+		0xC78, 0xEC0E0001,
+		0xC78, 0xEB0F0001,
+		0xC78, 0xEA100001,
+		0xC78, 0xE9110001,
+		0xC78, 0xE8120001,
+		0xC78, 0xE7130001,
+		0xC78, 0xE6140001,
+		0xC78, 0xE5150001,
+		0xC78, 0xE4160001,
+		0xC78, 0xE3170001,
+		0xC78, 0xE2180001,
+		0xC78, 0xE1190001,
+		0xC78, 0x8A1A0001,
+		0xC78, 0x891B0001,
+		0xC78, 0x881C0001,
+		0xC78, 0x871D0001,
+		0xC78, 0x861E0001,
+		0xC78, 0x851F0001,
+		0xC78, 0x84200001,
+		0xC78, 0x83210001,
+		0xC78, 0x82220001,
+		0xC78, 0x6A230001,
+		0xC78, 0x69240001,
+		0xC78, 0x68250001,
+		0xC78, 0x67260001,
+		0xC78, 0x66270001,
+		0xC78, 0x65280001,
+		0xC78, 0x64290001,
+		0xC78, 0x632A0001,
+		0xC78, 0x622B0001,
+		0xC78, 0x612C0001,
+		0xC78, 0x602D0001,
+		0xC78, 0x472E0001,
+		0xC78, 0x462F0001,
+		0xC78, 0x45300001,
+		0xC78, 0x44310001,
+		0xC78, 0x43320001,
+		0xC78, 0x42330001,
+		0xC78, 0x41340001,
+		0xC78, 0x40350001,
+		0xC78, 0x40360001,
+		0xC78, 0x40370001,
+		0xC78, 0x40380001,
+		0xC78, 0x40390001,
+		0xC78, 0x403A0001,
+		0xC78, 0x403B0001,
+		0xC78, 0x403C0001,
+		0xC78, 0x403D0001,
+		0xC78, 0x403E0001,
+		0xC78, 0x403F0001,
+	0xCDCDCDCD, 0xCDCD,
+		0xC78, 0xFB000001,
+		0xC78, 0xFB010001,
+		0xC78, 0xFB020001,
+		0xC78, 0xFB030001,
+		0xC78, 0xFB040001,
+		0xC78, 0xFB050001,
+		0xC78, 0xFA060001,
+		0xC78, 0xF9070001,
+		0xC78, 0xF8080001,
+		0xC78, 0xF7090001,
+		0xC78, 0xF60A0001,
+		0xC78, 0xF50B0001,
+		0xC78, 0xF40C0001,
+		0xC78, 0xF30D0001,
+		0xC78, 0xF20E0001,
+		0xC78, 0xF10F0001,
+		0xC78, 0xF0100001,
+		0xC78, 0xEF110001,
+		0xC78, 0xEE120001,
+		0xC78, 0xED130001,
+		0xC78, 0xEC140001,
+		0xC78, 0xEB150001,
+		0xC78, 0xEA160001,
+		0xC78, 0xE9170001,
+		0xC78, 0xE8180001,
+		0xC78, 0xE7190001,
+		0xC78, 0xC81A0001,
+		0xC78, 0xC71B0001,
+		0xC78, 0xC61C0001,
+		0xC78, 0x071D0001,
+		0xC78, 0x061E0001,
+		0xC78, 0x051F0001,
+		0xC78, 0x04200001,
+		0xC78, 0x03210001,
+		0xC78, 0xAA220001,
+		0xC78, 0xA9230001,
+		0xC78, 0xA8240001,
+		0xC78, 0xA7250001,
+		0xC78, 0xA6260001,
+		0xC78, 0x85270001,
+		0xC78, 0x84280001,
+		0xC78, 0x83290001,
+		0xC78, 0x252A0001,
+		0xC78, 0x242B0001,
+		0xC78, 0x232C0001,
+		0xC78, 0x222D0001,
+		0xC78, 0x672E0001,
+		0xC78, 0x662F0001,
+		0xC78, 0x65300001,
+		0xC78, 0x64310001,
+		0xC78, 0x63320001,
+		0xC78, 0x62330001,
+		0xC78, 0x61340001,
+		0xC78, 0x45350001,
+		0xC78, 0x44360001,
+		0xC78, 0x43370001,
+		0xC78, 0x42380001,
+		0xC78, 0x41390001,
+		0xC78, 0x403A0001,
+		0xC78, 0x403B0001,
+		0xC78, 0x403C0001,
+		0xC78, 0x403D0001,
+		0xC78, 0x403E0001,
+		0xC78, 0x403F0001,
+	0xFF010718, 0xDEAD,
+	0xFF010718, 0xABCD,
+		0xC78, 0xFA400001,
+		0xC78, 0xF9410001,
+		0xC78, 0xF8420001,
+		0xC78, 0xF7430001,
+		0xC78, 0xF6440001,
+		0xC78, 0xF5450001,
+		0xC78, 0xF4460001,
+		0xC78, 0xF3470001,
+		0xC78, 0xF2480001,
+		0xC78, 0xF1490001,
+		0xC78, 0xF04A0001,
+		0xC78, 0xEF4B0001,
+		0xC78, 0xEE4C0001,
+		0xC78, 0xED4D0001,
+		0xC78, 0xEC4E0001,
+		0xC78, 0xEB4F0001,
+		0xC78, 0xEA500001,
+		0xC78, 0xE9510001,
+		0xC78, 0xE8520001,
+		0xC78, 0xE7530001,
+		0xC78, 0xE6540001,
+		0xC78, 0xE5550001,
+		0xC78, 0xE4560001,
+		0xC78, 0xE3570001,
+		0xC78, 0xE2580001,
+		0xC78, 0xE1590001,
+		0xC78, 0x8A5A0001,
+		0xC78, 0x895B0001,
+		0xC78, 0x885C0001,
+		0xC78, 0x875D0001,
+		0xC78, 0x865E0001,
+		0xC78, 0x855F0001,
+		0xC78, 0x84600001,
+		0xC78, 0x83610001,
+		0xC78, 0x82620001,
+		0xC78, 0x6A630001,
+		0xC78, 0x69640001,
+		0xC78, 0x68650001,
+		0xC78, 0x67660001,
+		0xC78, 0x66670001,
+		0xC78, 0x65680001,
+		0xC78, 0x64690001,
+		0xC78, 0x636A0001,
+		0xC78, 0x626B0001,
+		0xC78, 0x616C0001,
+		0xC78, 0x606D0001,
+		0xC78, 0x476E0001,
+		0xC78, 0x466F0001,
+		0xC78, 0x45700001,
+		0xC78, 0x44710001,
+		0xC78, 0x43720001,
+		0xC78, 0x42730001,
+		0xC78, 0x41740001,
+		0xC78, 0x40750001,
+		0xC78, 0x40760001,
+		0xC78, 0x40770001,
+		0xC78, 0x40780001,
+		0xC78, 0x40790001,
+		0xC78, 0x407A0001,
+		0xC78, 0x407B0001,
+		0xC78, 0x407C0001,
+		0xC78, 0x407D0001,
+		0xC78, 0x407E0001,
+		0xC78, 0x407F0001,
+		0xC50, 0x00040222,
+		0xC50, 0x00040220,
+	0xCDCDCDCD, 0xCDCD,
+		0xC78, 0xFB400001,
+		0xC78, 0xFB410001,
+		0xC78, 0xFB420001,
+		0xC78, 0xFB430001,
+		0xC78, 0xFB440001,
+		0xC78, 0xFB450001,
+		0xC78, 0xFA460001,
+		0xC78, 0xF9470001,
+		0xC78, 0xF8480001,
+		0xC78, 0xF7490001,
+		0xC78, 0xF64A0001,
+		0xC78, 0xF54B0001,
+		0xC78, 0xF44C0001,
+		0xC78, 0xF34D0001,
+		0xC78, 0xF24E0001,
+		0xC78, 0xF14F0001,
+		0xC78, 0xF0500001,
+		0xC78, 0xEF510001,
+		0xC78, 0xEE520001,
+		0xC78, 0xED530001,
+		0xC78, 0xEC540001,
+		0xC78, 0xEB550001,
+		0xC78, 0xEA560001,
+		0xC78, 0xE9570001,
+		0xC78, 0xE8580001,
+		0xC78, 0xE7590001,
+		0xC78, 0xE65A0001,
+		0xC78, 0xE55B0001,
+		0xC78, 0xE45C0001,
+		0xC78, 0xE35D0001,
+		0xC78, 0xE25E0001,
+		0xC78, 0xE15F0001,
+		0xC78, 0x8A600001,
+		0xC78, 0x89610001,
+		0xC78, 0x88620001,
+		0xC78, 0x87630001,
+		0xC78, 0x86640001,
+		0xC78, 0x85650001,
+		0xC78, 0x84660001,
+		0xC78, 0x83670001,
+		0xC78, 0x82680001,
+		0xC78, 0x6B690001,
+		0xC78, 0x6A6A0001,
+		0xC78, 0x696B0001,
+		0xC78, 0x686C0001,
+		0xC78, 0x676D0001,
+		0xC78, 0x666E0001,
+		0xC78, 0x656F0001,
+		0xC78, 0x64700001,
+		0xC78, 0x63710001,
+		0xC78, 0x62720001,
+		0xC78, 0x61730001,
+		0xC78, 0x49740001,
+		0xC78, 0x48750001,
+		0xC78, 0x47760001,
+		0xC78, 0x46770001,
+		0xC78, 0x45780001,
+		0xC78, 0x44790001,
+		0xC78, 0x437A0001,
+		0xC78, 0x427B0001,
+		0xC78, 0x417C0001,
+		0xC78, 0x407D0001,
+		0xC78, 0x407E0001,
+		0xC78, 0x407F0001,
+		0xC50, 0x00040022,
+		0xC50, 0x00040020,
+	0xFF010718, 0xDEAD,
+};
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/table.h b/drivers/staging/rtl8192ee/rtl8192ee/table.h
new file mode 100644
index 00000000000..e0f419823b3
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/table.h
@@ -0,0 +1,48 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Created on  2010/ 5/18,  1:41
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92E_TABLE__H_
+#define __RTL92E_TABLE__H_
+
+#include <linux/types.h>
+#define RTL8192EE_PHY_REG_ARRAY_LEN	448
+extern u32 RTL8192EE_PHY_REG_ARRAY[];
+#define RTL8192EE_PHY_REG_ARRAY_PG_LEN	168
+extern u32 RTL8192EE_PHY_REG_ARRAY_PG[];
+#define	RTL8192EE_RADIOA_ARRAY_LEN	238
+extern u32 RTL8192EE_RADIOA_ARRAY[];
+#define	RTL8192EE_RADIOB_ARRAY_LEN	198
+extern u32 RTL8192EE_RADIOB_ARRAY[];
+#define RTL8192EE_MAC_ARRAY_LEN		202
+extern u32 RTL8192EE_MAC_ARRAY[];
+#define RTL8192EE_AGC_TAB_ARRAY_LEN	532
+extern u32 RTL8192EE_AGC_TAB_ARRAY[];
+#endif
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/trx.c b/drivers/staging/rtl8192ee/rtl8192ee/trx.c
new file mode 100644
index 00000000000..c930f52ec8c
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/trx.c
@@ -0,0 +1,1286 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../pci.h"
+#include "../base.h"
+#include "../stats.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "trx.h"
+#include "led.h"
+#include "dm.h"
+#include "fw.h"
+
+static u8 _rtl92ee_map_hwqueue_to_fwqueue(struct sk_buff *skb, u8 hw_queue)
+{
+	__le16 fc = rtl_get_fc(skb);
+
+	if (unlikely(ieee80211_is_beacon(fc)))
+		return QSLT_BEACON;
+	if (ieee80211_is_mgmt(fc) || ieee80211_is_ctl(fc))
+		return QSLT_MGNT;
+
+	return skb->priority;
+}
+
+/* mac80211's rate_idx is like this:
+ *
+ * 2.4G band:rx_status->band == IEEE80211_BAND_2GHZ
+ *
+ * B/G rate:
+ * (rx_status->flag & RX_FLAG_HT) = 0,
+ * DESC92C_RATE1M-->DESC92C_RATE54M ==> idx is 0-->11,
+ *
+ * N rate:
+ * (rx_status->flag & RX_FLAG_HT) = 1,
+ * DESC92C_RATEMCS0-->DESC92C_RATEMCS15 ==> idx is 0-->15
+ *
+ * 5G band:rx_status->band == IEEE80211_BAND_5GHZ
+ * A rate:
+ * (rx_status->flag & RX_FLAG_HT) = 0,
+ * DESC92C_RATE6M-->DESC92C_RATE54M ==> idx is 0-->7,
+ *
+ * N rate:
+ * (rx_status->flag & RX_FLAG_HT) = 1,
+ * DESC92C_RATEMCS0-->DESC92C_RATEMCS15 ==> idx is 0-->15
+ */
+static int _rtl92ee_rate_mapping(struct ieee80211_hw *hw,
+				 bool isht, u8 desc_rate)
+{
+	int rate_idx;
+
+	if (!false) {
+		if (IEEE80211_BAND_2GHZ == hw->conf.chandef.chan->band) {
+			switch (desc_rate) {
+			case DESC92C_RATE1M:
+				rate_idx = 0;
+				break;
+			case DESC92C_RATE2M:
+				rate_idx = 1;
+				break;
+			case DESC92C_RATE5_5M:
+				rate_idx = 2;
+				break;
+			case DESC92C_RATE11M:
+				rate_idx = 3;
+				break;
+			case DESC92C_RATE6M:
+				rate_idx = 4;
+				break;
+			case DESC92C_RATE9M:
+				rate_idx = 5;
+				break;
+			case DESC92C_RATE12M:
+				rate_idx = 6;
+				break;
+			case DESC92C_RATE18M:
+				rate_idx = 7;
+				break;
+			case DESC92C_RATE24M:
+				rate_idx = 8;
+				break;
+			case DESC92C_RATE36M:
+				rate_idx = 9;
+				break;
+			case DESC92C_RATE48M:
+				rate_idx = 10;
+				break;
+			case DESC92C_RATE54M:
+				rate_idx = 11;
+				break;
+			default:
+				rate_idx = 0;
+				break;
+			}
+		} else {
+			switch (desc_rate) {
+			case DESC92C_RATE6M:
+				rate_idx = 0;
+				break;
+			case DESC92C_RATE9M:
+				rate_idx = 1;
+				break;
+			case DESC92C_RATE12M:
+				rate_idx = 2;
+				break;
+			case DESC92C_RATE18M:
+				rate_idx = 3;
+				break;
+			case DESC92C_RATE24M:
+				rate_idx = 4;
+				break;
+			case DESC92C_RATE36M:
+				rate_idx = 5;
+				break;
+			case DESC92C_RATE48M:
+				rate_idx = 6;
+				break;
+			case DESC92C_RATE54M:
+				rate_idx = 7;
+				break;
+			default:
+				rate_idx = 0;
+				break;
+			}
+		}
+	} else {
+		switch (desc_rate) {
+		case DESC92C_RATEMCS0:
+			rate_idx = 0;
+			break;
+		case DESC92C_RATEMCS1:
+			rate_idx = 1;
+			break;
+		case DESC92C_RATEMCS2:
+			rate_idx = 2;
+			break;
+		case DESC92C_RATEMCS3:
+			rate_idx = 3;
+			break;
+		case DESC92C_RATEMCS4:
+			rate_idx = 4;
+			break;
+		case DESC92C_RATEMCS5:
+			rate_idx = 5;
+			break;
+		case DESC92C_RATEMCS6:
+			rate_idx = 6;
+			break;
+		case DESC92C_RATEMCS7:
+			rate_idx = 7;
+			break;
+		case DESC92C_RATEMCS8:
+			rate_idx = 8;
+			break;
+		case DESC92C_RATEMCS9:
+			rate_idx = 9;
+			break;
+		case DESC92C_RATEMCS10:
+			rate_idx = 10;
+			break;
+		case DESC92C_RATEMCS11:
+			rate_idx = 11;
+			break;
+		case DESC92C_RATEMCS12:
+			rate_idx = 12;
+			break;
+		case DESC92C_RATEMCS13:
+			rate_idx = 13;
+			break;
+		case DESC92C_RATEMCS14:
+			rate_idx = 14;
+			break;
+		case DESC92C_RATEMCS15:
+			rate_idx = 15;
+			break;
+		default:
+			rate_idx = 0;
+			break;
+		}
+	}
+	return rate_idx;
+}
+
+static void _rtl92ee_query_rxphystatus(struct ieee80211_hw *hw,
+				       struct rtl_stats *pstatus, u8 *pdesc,
+				       struct rx_fwinfo *p_drvinfo,
+				       bool bpacket_match_bssid,
+				       bool bpacket_toself,
+				       bool b_packet_beacon)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct phy_status_rpt *p_phystrpt = (struct phy_status_rpt *)p_drvinfo;
+	char rx_pwr_all = 0, rx_pwr[4];
+	u8 rf_rx_num = 0, evm, pwdb_all;
+	u8 i, max_spatial_stream;
+	u32 rssi, total_rssi = 0;
+	bool b_is_cck = pstatus->b_is_cck;
+	u8 lan_idx , vga_idx;
+
+	/* Record it for next packet processing */
+	pstatus->b_packet_matchbssid = bpacket_match_bssid;
+	pstatus->b_packet_toself = bpacket_toself;
+	pstatus->b_packet_beacon = b_packet_beacon;
+	pstatus->rx_mimo_signalquality[0] = -1;
+	pstatus->rx_mimo_signalquality[1] = -1;
+
+	if (b_is_cck) {
+		u8 cck_highpwr;
+		u8 cck_agc_rpt;
+		/* CCK Driver info Structure is not the same as OFDM packet. */
+		cck_agc_rpt = p_phystrpt->cck_agc_rpt_ofdm_cfosho_a;
+
+		/* (1)Hardware does not provide RSSI for CCK */
+		/* (2)PWDB, Average PWDB cacluated by
+		 * hardware (for rate adaptive) */
+		cck_highpwr = (u8) rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2,
+						 BIT(9));
+
+		lan_idx = ((cck_agc_rpt & 0xE0) >> 5);
+		vga_idx = (cck_agc_rpt & 0x1f);
+		switch (lan_idx) {
+		case 7: /*VGA_idx = 27~2*/
+				if (vga_idx <= 27)
+					rx_pwr_all = -100 + 2 * (27 - vga_idx);
+				else
+					rx_pwr_all = -100;
+				break;
+		case 6: /*VGA_idx = 2~0*/
+				rx_pwr_all = -48 + 2 * (2 - vga_idx);
+				break;
+		case 5: /*VGA_idx = 7~5*/
+				rx_pwr_all = -42 + 2 * (7 - vga_idx);
+				break;
+		case 4: /*VGA_idx = 7~4*/
+				rx_pwr_all = -36 + 2 * (7 - vga_idx);
+				break;
+		case 3: /*VGA_idx = 7~0*/
+				rx_pwr_all = -24 + 2 * (7 - vga_idx);
+				break;
+		case 2: /*VGA_idx = 5~0*/
+				if (cck_highpwr)
+					rx_pwr_all = -12 + 2 * (5 - vga_idx);
+				else
+					rx_pwr_all = -6 + 2 * (5 - vga_idx);
+				break;
+		case 1:
+				rx_pwr_all = 8 - 2 * vga_idx;
+				break;
+		case 0:
+				rx_pwr_all = 14 - 2 * vga_idx;
+				break;
+		default:
+				break;
+		}
+		rx_pwr_all += 16;
+		pwdb_all = stg_rtl_query_rxpwrpercentage(rx_pwr_all);
+
+		if (!cck_highpwr) {
+			if (pwdb_all >= 80)
+				pwdb_all = ((pwdb_all - 80) << 1) +
+					   ((pwdb_all - 80) >> 1) + 80;
+			else if ((pwdb_all <= 78) && (pwdb_all >= 20))
+				pwdb_all += 3;
+			if (pwdb_all > 100)
+				pwdb_all = 100;
+		}
+
+		pstatus->rx_pwdb_all = pwdb_all;
+		pstatus->bt_rx_rssi_percentage = pwdb_all;
+		pstatus->recvsignalpower = rx_pwr_all;
+
+		/* (3) Get Signal Quality (EVM) */
+		if (bpacket_match_bssid) {
+			u8 sq, sq_rpt;
+
+			if (pstatus->rx_pwdb_all > 40) {
+				sq = 100;
+			} else {
+				sq_rpt = p_phystrpt->cck_sig_qual_ofdm_pwdb_all;
+				if (sq_rpt > 64)
+					sq = 0;
+				else if (sq_rpt < 20)
+					sq = 100;
+				else
+					sq = ((64 - sq_rpt) * 100) / 44;
+			}
+
+			pstatus->signalquality = sq;
+			pstatus->rx_mimo_signalquality[0] = sq;
+			pstatus->rx_mimo_signalquality[1] = -1;
+		}
+	} else {
+		/* (1)Get RSSI for HT rate */
+		for (i = RF90_PATH_A; i < RF6052_MAX_PATH; i++) {
+			/* we will judge RF RX path now. */
+			if (rtlpriv->dm.brfpath_rxenable[i])
+				rf_rx_num++;
+
+			rx_pwr[i] = ((p_phystrpt->path_agc[i].gain & 0x3f) * 2)
+				    - 110;
+
+			pstatus->rx_pwr[i] = rx_pwr[i];
+			/* Translate DBM to percentage. */
+			rssi = stg_rtl_query_rxpwrpercentage(rx_pwr[i]);
+			total_rssi += rssi;
+
+			pstatus->rx_mimo_signalstrength[i] = (u8)rssi;
+		}
+
+		/* (2)PWDB, Average PWDB cacluated by
+		 * hardware (for rate adaptive) */
+		rx_pwr_all = ((p_phystrpt->cck_sig_qual_ofdm_pwdb_all >> 1)
+			      & 0x7f) - 110;
+
+		pwdb_all = stg_rtl_query_rxpwrpercentage(rx_pwr_all);
+		pstatus->rx_pwdb_all = pwdb_all;
+		pstatus->bt_rx_rssi_percentage = pwdb_all;
+		pstatus->rxpower = rx_pwr_all;
+		pstatus->recvsignalpower = rx_pwr_all;
+
+		/* (3)EVM of HT rate */
+		if (pstatus->rate >= DESC92C_RATEMCS8 &&
+		    pstatus->rate <= DESC92C_RATEMCS15)
+			max_spatial_stream = 2;
+		else
+			max_spatial_stream = 1;
+
+		for (i = 0; i < max_spatial_stream; i++) {
+			evm = stg_rtl_evm_db_to_percentage(
+						p_phystrpt->stream_rxevm[i]);
+
+			if (bpacket_match_bssid) {
+				/* Fill value in RFD, Get the first
+				 * spatial stream only */
+				if (i == 0)
+					pstatus->signalquality = (u8)(evm &
+								      0xff);
+				pstatus->rx_mimo_signalquality[i] = (u8)(evm &
+									 0xff);
+			}
+		}
+
+		if (bpacket_match_bssid) {
+			for (i = RF90_PATH_A; i <= RF90_PATH_B; i++)
+				rtl_priv(hw)->dm.cfo_tail[i] =
+					(int)p_phystrpt->path_cfotail[i];
+
+			if (rtl_priv(hw)->dm.packet_count == 0xffffffff)
+				rtl_priv(hw)->dm.packet_count = 0;
+			else
+				rtl_priv(hw)->dm.packet_count++;
+		}
+	}
+
+	/* UI BSS List signal strength(in percentage),
+	 * make it good looking, from 0~100. */
+	if (b_is_cck)
+		pstatus->signalstrength = (u8)(stg_rtl_signal_scale_mapping(hw,
+								     pwdb_all));
+	else if (rf_rx_num != 0)
+		pstatus->signalstrength = (u8)(stg_rtl_signal_scale_mapping(hw,
+						      total_rssi /= rf_rx_num));
+}
+
+static void _rtl92ee_translate_rx_signal_stuff(struct ieee80211_hw *hw,
+					       struct sk_buff *skb,
+					       struct rtl_stats *pstatus,
+					       u8 *pdesc,
+					       struct rx_fwinfo *p_drvinfo)
+{
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct ieee80211_hdr *hdr;
+	u8 *tmp_buf;
+	u8 *praddr;
+	u8 *psaddr;
+	__le16 fc;
+	u16 cpu_fc, type;
+	bool b_packet_matchbssid, b_packet_toself, b_packet_beacon;
+
+	tmp_buf = skb->data + pstatus->rx_drvinfo_size +
+		  pstatus->rx_bufshift + 24;
+
+	hdr = (struct ieee80211_hdr *)tmp_buf;
+	fc = hdr->frame_control;
+	cpu_fc = le16_to_cpu(fc);
+	type = WLAN_FC_GET_TYPE(fc);
+	praddr = hdr->addr1;
+	psaddr = ieee80211_get_SA(hdr);
+	ether_addr_copy(pstatus->psaddr, psaddr);
+
+	b_packet_matchbssid = ((IEEE80211_FTYPE_CTL != type) &&
+			       (ether_addr_equal(mac->bssid,
+						(cpu_fc & IEEE80211_FCTL_TODS) ?
+						hdr->addr1 :
+						(cpu_fc & IEEE80211_FCTL_FROMDS) ?
+						hdr->addr2 : hdr->addr3)) &&
+				(!pstatus->b_hwerror) && (!pstatus->b_crc) &&
+				(!pstatus->b_icv));
+
+	b_packet_toself = b_packet_matchbssid &&
+			 (ether_addr_equal(praddr, rtlefuse->dev_addr));
+
+	if (ieee80211_is_beacon(fc))
+		b_packet_beacon = true;
+	else
+		b_packet_beacon = false;
+
+	if (b_packet_beacon && b_packet_matchbssid)
+		rtl_priv(hw)->dm.dbginfo.num_qry_beacon_pkt++;
+
+	if (b_packet_matchbssid && ieee80211_is_data_qos(fc) &&
+	    !is_multicast_ether_addr(ieee80211_get_DA(hdr))) {
+		struct ieee80211_qos_hdr *hdr_qos =
+					    (struct ieee80211_qos_hdr *)tmp_buf;
+		u16 tid = le16_to_cpu(hdr_qos->qos_ctrl) & 0xf;
+		if (tid != 0 && tid != 3)
+			rtl_priv(hw)->dm.dbginfo.num_non_be_pkt++;
+	}
+	_rtl92ee_query_rxphystatus(hw, pstatus, pdesc, p_drvinfo,
+				   b_packet_matchbssid, b_packet_toself,
+				   b_packet_beacon);
+	stg_rtl_process_phyinfo(hw, tmp_buf, pstatus);
+}
+
+static void _rtl92ee_insert_emcontent(struct rtl_tcb_desc *ptcb_desc,
+				      u8 *virtualaddress)
+{
+	u32 dwtmp = 0;
+	memset(virtualaddress, 0, 8);
+
+	SET_EARLYMODE_PKTNUM(virtualaddress, ptcb_desc->empkt_num);
+	if (ptcb_desc->empkt_num == 1) {
+		dwtmp = ptcb_desc->empkt_len[0];
+	} else {
+		dwtmp = ptcb_desc->empkt_len[0];
+		dwtmp += ((dwtmp % 4) ? (4 - dwtmp % 4) : 0) + 4;
+		dwtmp += ptcb_desc->empkt_len[1];
+	}
+	SET_EARLYMODE_LEN0(virtualaddress, dwtmp);
+
+	if (ptcb_desc->empkt_num <= 3) {
+		dwtmp = ptcb_desc->empkt_len[2];
+	} else {
+		dwtmp = ptcb_desc->empkt_len[2];
+		dwtmp += ((dwtmp % 4) ? (4 - dwtmp % 4) : 0) + 4;
+		dwtmp += ptcb_desc->empkt_len[3];
+	}
+	SET_EARLYMODE_LEN1(virtualaddress, dwtmp);
+	if (ptcb_desc->empkt_num <= 5) {
+		dwtmp = ptcb_desc->empkt_len[4];
+	} else {
+		dwtmp = ptcb_desc->empkt_len[4];
+		dwtmp += ((dwtmp % 4) ? (4 - dwtmp % 4) : 0) + 4;
+		dwtmp += ptcb_desc->empkt_len[5];
+	}
+	SET_EARLYMODE_LEN2_1(virtualaddress, dwtmp & 0xF);
+	SET_EARLYMODE_LEN2_2(virtualaddress, dwtmp >> 4);
+	if (ptcb_desc->empkt_num <= 7) {
+		dwtmp = ptcb_desc->empkt_len[6];
+	} else {
+		dwtmp = ptcb_desc->empkt_len[6];
+		dwtmp += ((dwtmp % 4) ? (4 - dwtmp % 4) : 0) + 4;
+		dwtmp += ptcb_desc->empkt_len[7];
+	}
+	SET_EARLYMODE_LEN3(virtualaddress, dwtmp);
+	if (ptcb_desc->empkt_num <= 9) {
+		dwtmp = ptcb_desc->empkt_len[8];
+	} else {
+		dwtmp = ptcb_desc->empkt_len[8];
+		dwtmp += ((dwtmp % 4) ? (4 - dwtmp % 4) : 0) + 4;
+		dwtmp += ptcb_desc->empkt_len[9];
+	}
+	SET_EARLYMODE_LEN4(virtualaddress, dwtmp);
+}
+
+bool rtl92ee_rx_query_desc(struct ieee80211_hw *hw,
+			   struct rtl_stats *status,
+			   struct ieee80211_rx_status *rx_status,
+			   u8 *pdesc, struct sk_buff *skb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rx_fwinfo *p_drvinfo;
+	struct ieee80211_hdr *hdr;
+
+	u32 phystatus = GET_RX_DESC_PHYST(pdesc);
+	status->length = (u16) GET_RX_DESC_PKT_LEN(pdesc);
+	status->rx_drvinfo_size = (u8) GET_RX_DESC_DRV_INFO_SIZE(pdesc) *
+				  RX_DRV_INFO_SIZE_UNIT;
+	status->rx_bufshift = (u8)(GET_RX_DESC_SHIFT(pdesc) & 0x03);
+	status->b_icv = (u16) GET_RX_DESC_ICV(pdesc);
+	status->b_crc = (u16) GET_RX_DESC_CRC32(pdesc);
+	status->b_hwerror = (status->b_crc | status->b_icv);
+	status->decrypted = !GET_RX_DESC_SWDEC(pdesc);
+	status->rate = (u8) GET_RX_DESC_RXMCS(pdesc);
+	status->b_isampdu = (bool)(GET_RX_DESC_PAGGR(pdesc) == 1);
+		status->timestamp_low = GET_RX_DESC_TSFL(pdesc);
+	status->b_is_cck = RX_HAL_IS_CCK_RATE(status->rate);
+
+	status->macid = GET_RX_DESC_MACID(pdesc);
+	if (GET_RX_STATUS_DESC_MAGIC_MATCH(pdesc))
+		status->wake_match = BIT(2);
+	else if (GET_RX_STATUS_DESC_MAGIC_MATCH(pdesc))
+		status->wake_match = BIT(1);
+	else if (GET_RX_STATUS_DESC_UNICAST_MATCH(pdesc))
+		status->wake_match = BIT(0);
+	else
+		status->wake_match = 0;
+	if (status->wake_match)
+		RT_TRACE(COMP_RXDESC , DBG_LOUD,
+			 ("GGGGGGGGGGGGGet Wakeup Packet!! WakeMatch =%d\n",
+			 status->wake_match));
+	rx_status->freq = hw->conf.chandef.chan->center_freq;
+	rx_status->band = hw->conf.chandef.chan->band;
+
+	hdr = (struct ieee80211_hdr *)(skb->data + status->rx_drvinfo_size +
+				       status->rx_bufshift + 24);
+
+	if (status->b_crc)
+		rx_status->flag |= RX_FLAG_FAILED_FCS_CRC;
+
+	if (status->rx_is40Mhzpacket)
+		rx_status->flag |= RX_FLAG_40MHZ;
+
+	if (status->b_is_ht)
+		rx_status->flag |= RX_FLAG_HT;
+
+	rx_status->flag |= RX_FLAG_MACTIME_MPDU;
+
+	/* hw will set status->decrypted true, if it finds the
+	 * frame is open data frame or mgmt frame. */
+	/* So hw will not decryption robust managment frame
+	 * for IEEE80211w but still set status->decrypted
+	 * true, so here we should set it back to undecrypted
+	 * for IEEE80211w frame, and mac80211 sw will help
+	 * to decrypt it */
+	if (status->decrypted) {
+		if (!hdr) {
+			WARN_ON_ONCE(true);
+			pr_err("decrypted is true but hdr NULL, from skb %p\n",
+			       rtl_get_hdr(skb));
+			return false;
+		}
+
+		if ((!_ieee80211_is_robust_mgmt_frame(hdr)) &&
+		    (ieee80211_has_protected(hdr->frame_control)))
+			rx_status->flag |= RX_FLAG_DECRYPTED;
+		else
+			rx_status->flag &= ~RX_FLAG_DECRYPTED;
+	}
+
+	/* rate_idx: index of data rate into band's
+	 * supported rates or MCS index if HT rates
+	 * are use (RX_FLAG_HT)*/
+	/* Notice: this is diff with windows define */
+	rx_status->rate_idx = _rtl92ee_rate_mapping(hw,
+						    status->b_is_ht,
+						    status->rate);
+
+	rx_status->mactime = status->timestamp_low;
+	if (phystatus) {
+		p_drvinfo = (struct rx_fwinfo *)(skb->data +
+						 status->rx_bufshift + 24);
+
+		_rtl92ee_translate_rx_signal_stuff(hw, skb, status, pdesc,
+						   p_drvinfo);
+	}
+
+	/*rx_status->qual = status->signal; */
+	rx_status->signal = status->recvsignalpower + 10;
+	/*rx_status->noise = -status->noise; */
+	if (status->packet_report_type == TX_REPORT2) {
+		status->macid_valid_entry[0] =
+			GET_RX_RPT2_DESC_MACID_VALID_1(pdesc);
+		status->macid_valid_entry[1] =
+			GET_RX_RPT2_DESC_MACID_VALID_2(pdesc);
+	}
+	return true;
+}
+
+/*in Windows, this == Rx_92EE_Interrupt*/
+void rtl92ee_rx_check_dma_ok(struct ieee80211_hw *hw, u8 *header_desc,
+			     u8 queue_index)
+{
+	u8 first_seg;
+	u8 last_seg;
+	u16 total_len;
+	u16 read_cnt = 0;
+	if (header_desc == NULL)
+		return;
+
+	total_len = (u16)GET_RX_BUFFER_DESC_TOTAL_LENGTH(header_desc);
+
+	first_seg = (u8)GET_RX_BUFFER_DESC_FS(header_desc);
+
+	last_seg = (u8)GET_RX_BUFFER_DESC_LS(header_desc);
+
+	while (total_len == 0 && first_seg == 0 && last_seg == 0) {
+		read_cnt++;
+		total_len = (u16)GET_RX_BUFFER_DESC_TOTAL_LENGTH(header_desc);
+		first_seg = (u8)GET_RX_BUFFER_DESC_FS(header_desc);
+		last_seg = (u8)GET_RX_BUFFER_DESC_LS(header_desc);
+
+		if (read_cnt > 20)
+			break;
+	}
+}
+
+u16 rtl92ee_rx_desc_buff_remained_cnt(struct ieee80211_hw *hw , u8 queue_index)
+{
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u16 read_point = 0 , write_point = 0 , remind_cnt = 0;
+	u32 tmp_4byte = 0;
+	static u16 last_read_point;
+	static bool start_rx;
+
+	tmp_4byte = rtl_read_dword(rtlpriv, REG_RXQ_TXBD_IDX);
+	read_point = (u16)((tmp_4byte>>16) & 0x7ff);
+	write_point = (u16)(tmp_4byte & 0x7ff);
+
+	if (write_point != rtlpci->rx_ring[queue_index].next_rx_rp) {
+		RT_TRACE(COMP_RXDESC, DBG_DMESG,
+			 ("!!!write point is 0x%x, reg 0x3B4 value is 0x%x\n",
+			  write_point, tmp_4byte));
+		tmp_4byte = rtl_read_dword(rtlpriv, REG_RXQ_TXBD_IDX);
+		read_point = (u16)((tmp_4byte>>16) & 0x7ff);
+		write_point = (u16)(tmp_4byte & 0x7ff);
+	}
+
+	if (read_point > 0)
+		start_rx = true;
+	if (!start_rx)
+		return 0;
+
+	if ((last_read_point > (RX_DESC_NUM_92E / 2)) &&
+	    (read_point <= (RX_DESC_NUM_92E / 2))) {
+		remind_cnt = RX_DESC_NUM_92E - write_point;
+	} else {
+		remind_cnt = (read_point >= write_point) ?
+			     (read_point - write_point) :
+			     (RX_DESC_NUM_92E - write_point + read_point);
+	}
+
+	if (remind_cnt == 0)
+		return 0;
+
+	rtlpci->rx_ring[queue_index].next_rx_rp = write_point;
+
+	last_read_point = read_point;
+	return remind_cnt;
+}
+
+static u16 get_desc_addr_fr_q(u16 queue_index)
+{
+	u16 desc_address = REG_BEQ_TXBD_IDX;
+
+	switch (queue_index) {
+	case BK_QUEUE:
+			desc_address = REG_BKQ_TXBD_IDX;
+			break;
+	case BE_QUEUE:
+			desc_address = REG_BEQ_TXBD_IDX;
+			break;
+	case VI_QUEUE:
+			desc_address = REG_VIQ_TXBD_IDX;
+			break;
+	case VO_QUEUE:
+			desc_address = REG_VOQ_TXBD_IDX;
+			break;
+	case BEACON_QUEUE:
+			desc_address = REG_BEQ_TXBD_IDX;
+			break;
+	case TXCMD_QUEUE:
+			desc_address = REG_BEQ_TXBD_IDX;
+			break;
+	case MGNT_QUEUE:
+			desc_address = REG_MGQ_TXBD_IDX;
+			break;
+	case HIGH_QUEUE:
+			desc_address = REG_HI0Q_TXBD_IDX;
+			break;
+	case HCCA_QUEUE:
+			desc_address = REG_BEQ_TXBD_IDX;
+			break;
+	default:
+			break;
+	}
+	return desc_address;
+}
+
+void rtl92ee_get_available_desc(struct ieee80211_hw *hw , u8 q_idx)
+{
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u16 point_diff = 0;
+	u16 current_tx_read_point = 0, current_tx_write_point = 0;
+	u32 tmp_4byte;
+	tmp_4byte = rtl_read_dword(rtlpriv,
+				   get_desc_addr_fr_q(q_idx));
+	current_tx_read_point = (u16)((tmp_4byte >> 16) & 0x0fff);
+	current_tx_write_point = (u16)((tmp_4byte) & 0x0fff);
+
+	point_diff = ((current_tx_read_point > current_tx_write_point) ?
+		      (current_tx_read_point - current_tx_write_point) :
+		      (TX_DESC_NUM_92E - current_tx_write_point +
+		       current_tx_read_point));
+
+	rtlpci->tx_ring[q_idx].avl_desc = point_diff;
+}
+
+void rtl92ee_pre_fill_tx_bd_desc(struct ieee80211_hw *hw,
+				 u8 *tx_bd_desc, u8 *desc, u8 queue_index,
+				 struct sk_buff *skb, dma_addr_t addr)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u32 pkt_len = skb->len;
+	u16 desc_size = 40; /*tx desc size*/
+	u32 psblen = 0;
+	u16 tx_page_size = 0;
+	u32 total_packet_size = 0;
+	u16 current_bd_desc;
+	u8 i = 0;
+	u16 real_desc_size = 0x28;
+	u16	append_early_mode_size = 0;
+#if (RTL8192EE_SEG_NUM == 0)
+	u8 segmentnum = 2;
+#elif (RTL8192EE_SEG_NUM == 1)
+	u8 segmentnum = 4;
+#elif (RTL8192EE_SEG_NUM == 2)
+	u8 segmentnum = 8;
+#endif
+
+	tx_page_size = 2;
+	current_bd_desc = rtlpci->tx_ring[queue_index].cur_tx_wp;
+
+
+	total_packet_size = desc_size+pkt_len;
+
+	if (rtlpriv->rtlhal.b_earlymode_enable)	{
+		if (queue_index < BEACON_QUEUE) {
+			append_early_mode_size = 8;
+			total_packet_size += append_early_mode_size;
+		}
+	}
+
+	if (tx_page_size > 0) {
+		psblen = (pkt_len + real_desc_size + append_early_mode_size) /
+			 (tx_page_size * 128);
+
+		if (psblen * (tx_page_size * 128) < total_packet_size)
+			psblen += 1;
+	}
+
+	/* Reset */
+	SET_TX_BUFF_DESC_LEN_0(tx_bd_desc , 0);
+	SET_TX_BUFF_DESC_PSB(tx_bd_desc , 0);
+	SET_TX_BUFF_DESC_OWN(tx_bd_desc , 0);
+
+	for (i = 1; i < segmentnum; i++) {
+		SET_TXBUFFER_DESC_LEN_WITH_OFFSET(tx_bd_desc, i, 0);
+		SET_TXBUFFER_DESC_AMSDU_WITH_OFFSET(tx_bd_desc, i, 0);
+		SET_TXBUFFER_DESC_ADD_LOW_WITH_OFFSET(tx_bd_desc, i, 0);
+#if (DMA_IS_64BIT == 1)
+		SET_TXBUFFER_DESC_ADD_HIGT_WITH_OFFSET(tx_bd_desc, i, 0);
+#endif
+	}
+	SET_TX_BUFF_DESC_LEN_1(tx_bd_desc, 0);
+	SET_TX_BUFF_DESC_AMSDU_1(tx_bd_desc, 0);
+
+	SET_TX_BUFF_DESC_LEN_2(tx_bd_desc, 0);
+	SET_TX_BUFF_DESC_AMSDU_2(tx_bd_desc, 0);
+	SET_TX_BUFF_DESC_LEN_3(tx_bd_desc, 0);
+	SET_TX_BUFF_DESC_AMSDU_3(tx_bd_desc, 0);
+	/* Clear all status */
+	CLEAR_PCI_TX_DESC_CONTENT(desc, TX_DESC_SIZE);
+
+	if (rtlpriv->rtlhal.b_earlymode_enable) {
+		if (queue_index < BEACON_QUEUE) {
+			/* These macros need braces */
+			SET_TX_BUFF_DESC_LEN_0(tx_bd_desc , desc_size + 8);
+		} else {
+			SET_TX_BUFF_DESC_LEN_0(tx_bd_desc , desc_size);
+		}
+	} else {
+		SET_TX_BUFF_DESC_LEN_0(tx_bd_desc , desc_size);
+	}
+	SET_TX_BUFF_DESC_PSB(tx_bd_desc , psblen);
+	SET_TX_BUFF_DESC_ADDR_LOW_0(tx_bd_desc,
+				    rtlpci->tx_ring[queue_index].dma +
+				    (current_bd_desc * TX_DESC_SIZE));
+
+	SET_TXBUFFER_DESC_LEN_WITH_OFFSET(tx_bd_desc, 1, pkt_len);
+	/* don't using extendsion mode. */
+	SET_TXBUFFER_DESC_AMSDU_WITH_OFFSET(tx_bd_desc, 1, 0);
+	SET_TXBUFFER_DESC_ADD_LOW_WITH_OFFSET(tx_bd_desc, 1, addr);
+
+	SET_TX_DESC_PKT_SIZE(desc, (u16)(pkt_len));
+	SET_TX_DESC_TX_BUFFER_SIZE(desc, (u16)(pkt_len));
+}
+
+void rtl92ee_tx_fill_desc(struct ieee80211_hw *hw,
+			  struct ieee80211_hdr *hdr, u8 *pdesc_tx,
+			  u8 *pbd_desc_tx,
+			  struct ieee80211_tx_info *info,
+			  struct ieee80211_sta *sta,
+			  struct sk_buff *skb,
+			  u8 hw_queue, struct rtl_tcb_desc *ptcb_desc)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	u8 *pdesc = (u8 *)pdesc_tx;
+	u16 seq_number;
+	__le16 fc = hdr->frame_control;
+	unsigned int buf_len = 0;
+	u8 fw_qsel = _rtl92ee_map_hwqueue_to_fwqueue(skb, hw_queue);
+	bool b_firstseg = ((hdr->seq_ctrl &
+			    cpu_to_le16(IEEE80211_SCTL_FRAG)) == 0);
+	bool b_lastseg = ((hdr->frame_control &
+			   cpu_to_le16(IEEE80211_FCTL_MOREFRAGS)) == 0);
+	dma_addr_t mapping;
+	u8 bw_40 = 0;
+	u8 short_gi = 0;
+
+	if (mac->opmode == NL80211_IFTYPE_STATION) {
+		bw_40 = mac->bw_40;
+	} else if (mac->opmode == NL80211_IFTYPE_AP ||
+		   mac->opmode == NL80211_IFTYPE_ADHOC) {
+		if (sta)
+			bw_40 = sta->ht_cap.cap &
+				IEEE80211_HT_CAP_SUP_WIDTH_20_40;
+	}
+	seq_number = (le16_to_cpu(hdr->seq_ctrl) & IEEE80211_SCTL_SEQ) >> 4;
+	stg_rtl_get_tcb_desc(hw, info, sta, skb, ptcb_desc);
+	/* reserve 8 byte for AMPDU early mode */
+	if (rtlhal->b_earlymode_enable) {
+		skb_push(skb, EM_HDR_LEN);
+		memset(skb->data, 0, EM_HDR_LEN);
+	}
+	buf_len = skb->len;
+	mapping = pci_map_single(rtlpci->pdev, skb->data, skb->len,
+				 PCI_DMA_TODEVICE);
+	if (pci_dma_mapping_error(rtlpci->pdev, mapping)) {
+		RT_TRACE(COMP_SEND, DBG_TRACE,
+			 ("DMA mapping error"));
+		return;
+	}
+	if (pbd_desc_tx != NULL)
+		rtl92ee_pre_fill_tx_bd_desc(hw, pbd_desc_tx, pdesc, hw_queue,
+					    skb, mapping);
+
+	if (ieee80211_is_nullfunc(fc) || ieee80211_is_ctl(fc)) {
+		b_firstseg = true;
+		b_lastseg = true;
+	}
+	if (b_firstseg) {
+		if (rtlhal->b_earlymode_enable) {
+			SET_TX_DESC_PKT_OFFSET(pdesc, 1);
+			SET_TX_DESC_OFFSET(pdesc,
+					   USB_HWDESC_HEADER_LEN + EM_HDR_LEN);
+			if (ptcb_desc->empkt_num) {
+				RT_TRACE(COMP_SEND, DBG_TRACE,
+					 ("Insert 8 byte.pTcb->EMPktNum:%d\n",
+					  ptcb_desc->empkt_num));
+				_rtl92ee_insert_emcontent(ptcb_desc,
+							  (u8 *)(skb->data));
+			}
+		} else {
+			SET_TX_DESC_OFFSET(pdesc, USB_HWDESC_HEADER_LEN);
+		}
+
+		SET_TX_DESC_TX_RATE(pdesc, ptcb_desc->hw_rate);
+
+		if (ieee80211_is_mgmt(fc)) {
+			ptcb_desc->use_driver_rate = true;
+		} else {
+			if (rtlpriv->ra.is_special_data) {
+				ptcb_desc->use_driver_rate = true;
+				SET_TX_DESC_TX_RATE(pdesc, DESC92C_RATE11M);
+			} else {
+				ptcb_desc->use_driver_rate = false;
+			}
+		}
+
+		if (ptcb_desc->hw_rate > DESC92C_RATEMCS0)
+			short_gi = (ptcb_desc->use_shortgi) ? 1 : 0;
+		else
+			short_gi = (ptcb_desc->use_shortpreamble) ? 1 : 0;
+
+		if (info->flags & IEEE80211_TX_CTL_AMPDU) {
+			SET_TX_DESC_AGG_ENABLE(pdesc, 1);
+			SET_TX_DESC_MAX_AGG_NUM(pdesc, 0x14);
+		}
+		SET_TX_DESC_SEQ(pdesc, seq_number);
+		SET_TX_DESC_RTS_ENABLE(pdesc,
+				       ((ptcb_desc->b_rts_enable &&
+					 !ptcb_desc->b_cts_enable) ? 1 : 0));
+		SET_TX_DESC_HW_RTS_ENABLE(pdesc , 0);
+		SET_TX_DESC_CTS2SELF(pdesc,
+				     ((ptcb_desc->b_cts_enable) ? 1 : 0));
+
+		SET_TX_DESC_RTS_RATE(pdesc, ptcb_desc->rts_rate);
+		SET_TX_DESC_RTS_SC(pdesc, ptcb_desc->rts_sc);
+		SET_TX_DESC_RTS_SHORT(pdesc,
+				((ptcb_desc->rts_rate <= DESC92C_RATE54M) ?
+				 (ptcb_desc->b_rts_use_shortpreamble ? 1 : 0) :
+				 (ptcb_desc->b_rts_use_shortgi ? 1 : 0)));
+
+		if (ptcb_desc->btx_enable_sw_calc_duration)
+			SET_TX_DESC_NAV_USE_HDR(pdesc, 1);
+
+		if (bw_40) {
+			if (ptcb_desc->packet_bw == HT_CHANNEL_WIDTH_20_40) {
+				SET_TX_DESC_DATA_BW(pdesc, 1);
+				SET_TX_DESC_TX_SUB_CARRIER(pdesc, 3);
+			} else {
+				SET_TX_DESC_DATA_BW(pdesc, 0);
+				SET_TX_DESC_TX_SUB_CARRIER(pdesc,
+							   mac->cur_40_prime_sc);
+			}
+		} else {
+			SET_TX_DESC_DATA_BW(pdesc, 0);
+			SET_TX_DESC_TX_SUB_CARRIER(pdesc, 0);
+		}
+
+		SET_TX_DESC_LINIP(pdesc, 0);
+		if (sta) {
+			u8 ampdu_density = sta->ht_cap.ampdu_density;
+			SET_TX_DESC_AMPDU_DENSITY(pdesc, ampdu_density);
+		}
+		if (info->control.hw_key) {
+			struct ieee80211_key_conf *key = info->control.hw_key;
+			switch (key->cipher) {
+			case WLAN_CIPHER_SUITE_WEP40:
+			case WLAN_CIPHER_SUITE_WEP104:
+			case WLAN_CIPHER_SUITE_TKIP:
+				SET_TX_DESC_SEC_TYPE(pdesc, 0x1);
+				break;
+			case WLAN_CIPHER_SUITE_CCMP:
+				SET_TX_DESC_SEC_TYPE(pdesc, 0x3);
+				break;
+			default:
+				SET_TX_DESC_SEC_TYPE(pdesc, 0x0);
+				break;
+			}
+		}
+
+		SET_TX_DESC_QUEUE_SEL(pdesc, fw_qsel);
+		SET_TX_DESC_DATA_RATE_FB_LIMIT(pdesc, 0x1F);
+		SET_TX_DESC_RTS_RATE_FB_LIMIT(pdesc, 0xF);
+		SET_TX_DESC_DISABLE_FB(pdesc,
+				       ptcb_desc->disable_ratefallback ? 1 : 0);
+		SET_TX_DESC_USE_RATE(pdesc, ptcb_desc->use_driver_rate ? 1 : 0);
+
+		/*SET_TX_DESC_PWR_STATUS(pdesc, pwr_status);*/
+		/* Set TxRate and RTSRate in TxDesc  */
+		/* This prevent Tx initial rate of new-coming packets */
+		/* from being overwritten by retried  packet rate.*/
+		if (!ptcb_desc->use_driver_rate) {
+			/*SET_TX_DESC_RTS_RATE(pdesc, 0x08); */
+			/* SET_TX_DESC_TX_RATE(pdesc, 0x0b); */
+		}
+		if (ieee80211_is_data_qos(fc)) {
+			if (mac->rdg_en) {
+				RT_TRACE(COMP_SEND, DBG_TRACE,
+					 ("Enable RDG function.\n"));
+				SET_TX_DESC_RDG_ENABLE(pdesc, 1);
+				SET_TX_DESC_HTC(pdesc, 1);
+			}
+		}
+	}
+
+	SET_TX_DESC_FIRST_SEG(pdesc, (b_firstseg ? 1 : 0));
+	SET_TX_DESC_LAST_SEG(pdesc, (b_lastseg ? 1 : 0));
+	SET_TX_DESC_TX_BUFFER_ADDRESS(pdesc, mapping);
+	if (rtlpriv->dm.b_useramask) {
+		SET_TX_DESC_RATE_ID(pdesc, ptcb_desc->ratr_index);
+		SET_TX_DESC_MACID(pdesc, ptcb_desc->mac_id);
+	} else {
+		SET_TX_DESC_RATE_ID(pdesc, 0xC + ptcb_desc->ratr_index);
+		SET_TX_DESC_MACID(pdesc, ptcb_desc->ratr_index);
+	}
+
+	SET_TX_DESC_MORE_FRAG(pdesc, (b_lastseg ? 0 : 1));
+	if (is_multicast_ether_addr(ieee80211_get_DA(hdr)) ||
+	    is_broadcast_ether_addr(ieee80211_get_DA(hdr))) {
+		SET_TX_DESC_BMC(pdesc, 1);
+	}
+	RT_TRACE(COMP_SEND, DBG_TRACE, ("\n"));
+}
+
+void rtl92ee_tx_fill_cmddesc(struct ieee80211_hw *hw,
+			     u8 *pdesc, bool b_firstseg,
+			     bool b_lastseg, struct sk_buff *skb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u8 fw_queue = QSLT_BEACON;
+
+	dma_addr_t mapping = pci_map_single(rtlpci->pdev,
+					    skb->data, skb->len,
+					    PCI_DMA_TODEVICE);
+
+
+	u8 txdesc_len = 40;
+
+	if (pci_dma_mapping_error(rtlpci->pdev, mapping)) {
+		RT_TRACE(COMP_SEND, DBG_TRACE,
+			 ("DMA mapping error"));
+		return;
+	}
+	CLEAR_PCI_TX_DESC_CONTENT(pdesc, txdesc_len);
+
+	if (b_firstseg)
+		SET_TX_DESC_OFFSET(pdesc, txdesc_len);
+
+	SET_TX_DESC_TX_RATE(pdesc, DESC92C_RATE1M);
+
+	SET_TX_DESC_SEQ(pdesc, 0);
+
+	SET_TX_DESC_LINIP(pdesc, 0);
+
+	SET_TX_DESC_QUEUE_SEL(pdesc, fw_queue);
+
+	SET_TX_DESC_FIRST_SEG(pdesc, 1);
+	SET_TX_DESC_LAST_SEG(pdesc, 1);
+
+	SET_TX_DESC_TX_BUFFER_SIZE(pdesc, (u16)(skb->len));
+
+	SET_TX_DESC_TX_BUFFER_ADDRESS(pdesc, mapping);
+
+	SET_TX_DESC_RATE_ID(pdesc, 7);
+	SET_TX_DESC_MACID(pdesc, 0);
+
+	SET_TX_DESC_OWN(pdesc, 1);
+
+	SET_TX_DESC_PKT_SIZE((u8 *)pdesc, (u16)(skb->len));
+
+	SET_TX_DESC_FIRST_SEG(pdesc, 1);
+	SET_TX_DESC_LAST_SEG(pdesc, 1);
+
+	SET_TX_DESC_OFFSET(pdesc, 40);
+
+	SET_TX_DESC_USE_RATE(pdesc, 1);
+
+	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD,
+		      "H2C Tx Cmd Content\n", pdesc, txdesc_len);
+}
+
+
+void rtl92ee_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
+		      u8 desc_name, u8 *val)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u16 cur_tx_rp = 0;
+	u16 cur_tx_wp = 0;
+	static u16 last_txw_point;
+	static bool over_run;
+	u32 tmp = 0;
+	u8 q_idx = *val;
+
+	if (istx) {
+		switch (desc_name) {
+		case HW_DESC_TX_NEXTDESC_ADDR:
+			SET_TX_DESC_NEXT_DESC_ADDRESS(pdesc, *(u32 *)val);
+			break;
+		case HW_DESC_OWN:{
+			struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+			struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[q_idx];
+			u16 max_tx_desc = ring->entries;
+			if (q_idx == BEACON_QUEUE) {
+				ring->cur_tx_wp = 0;
+				ring->cur_tx_rp = 0;
+				SET_TX_BUFF_DESC_OWN(pdesc, 1);
+				return;
+			}
+			ring->cur_tx_wp = ((ring->cur_tx_wp + 1) % max_tx_desc);
+
+			if (over_run) {
+				ring->cur_tx_wp = 0;
+				over_run = false;
+			}
+			if (ring->avl_desc > 1) {
+				ring->avl_desc--;
+
+				rtl_write_word(rtlpriv,
+					       get_desc_addr_fr_q(q_idx),
+				       ring->cur_tx_wp);
+
+				if (q_idx == 1)
+					last_txw_point = cur_tx_wp;
+			}
+
+			if (ring->avl_desc < (max_tx_desc - 15)) {
+				u16 point_diff = 0;
+				tmp = rtl_read_dword(rtlpriv,
+						     get_desc_addr_fr_q(q_idx));
+				cur_tx_rp = (u16)((tmp >> 16) & 0x0fff);
+				cur_tx_wp = (u16)(tmp & 0x0fff);
+
+				ring->cur_tx_wp = cur_tx_wp;
+				ring->cur_tx_rp = cur_tx_rp;
+				point_diff = ((cur_tx_rp > cur_tx_wp) ?
+					      (cur_tx_rp - cur_tx_wp) :
+					      (TX_DESC_NUM_92E - 1 -
+					       cur_tx_wp + cur_tx_rp));
+
+				ring->avl_desc = point_diff;
+			}
+		}
+		break;
+		}
+	} else {
+		switch (desc_name) {
+		case HW_DESC_RX_PREPARE:
+			SET_RX_BUFFER_DESC_LS(pdesc , 0);
+			SET_RX_BUFFER_DESC_FS(pdesc , 0);
+			SET_RX_BUFFER_DESC_TOTAL_LENGTH(pdesc , 0);
+
+			SET_RX_BUFFER_DESC_DATA_LENGTH(pdesc,
+						       MAX_RECEIVE_BUFFER_SIZE +
+						       RX_DESC_SIZE);
+
+			SET_RX_BUFFER_PHYSICAL_LOW(pdesc, *(u32 *)val);
+			break;
+		case HW_DESC_RXERO:
+			SET_RX_DESC_EOR(pdesc, 1);
+			break;
+		default:
+			RT_ASSERT(false,
+				  ("ERR rxdesc :%d not process\n", desc_name));
+			break;
+		}
+	}
+}
+
+u32 rtl92ee_get_desc(u8 *pdesc, bool istx, u8 desc_name)
+{
+	u32 ret = 0;
+
+	if (istx) {
+		switch (desc_name) {
+		case HW_DESC_OWN:
+			ret = GET_TX_DESC_OWN(pdesc);
+			break;
+		case HW_DESC_TXBUFF_ADDR:
+			ret = GET_TXBUFFER_DESC_ADDR_LOW(pdesc, 1);
+			break;
+		default:
+			RT_ASSERT(false,
+				  ("ERR txdesc :%d not process\n", desc_name));
+			break;
+		}
+	} else {
+		switch (desc_name) {
+		case HW_DESC_OWN:
+			ret = GET_RX_DESC_OWN(pdesc);
+			break;
+		case HW_DESC_RXPKT_LEN:
+			ret = GET_RX_DESC_PKT_LEN(pdesc);
+			break;
+		case HW_DESC_RXBUFF_ADDR:
+			ret = GET_RX_DESC_BUFF_ADDR(pdesc);
+			break;
+		default:
+			RT_ASSERT(false,
+				  ("ERR rxdesc :%d not process\n", desc_name));
+			break;
+		}
+	}
+	return ret;
+}
+
+bool rtl92ee_is_tx_desc_closed(struct ieee80211_hw *hw, u8 hw_queue, u16 index)
+{
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u16 read_point, write_point, available_desc_num;
+	bool ret = false;
+	static u8 stop_report_cnt;
+	struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[hw_queue];
+
+	/*checking Read/Write Point each interrupt wastes CPU utilization.*/
+	if (stop_report_cnt > 15 || !rtlpriv->link_info.b_busytraffic) {
+		u16 point_diff = 0;
+		u16 cur_tx_rp, cur_tx_wp;
+		u32 tmpu32;
+
+		tmpu32 = rtl_read_dword(rtlpriv, get_desc_addr_fr_q(hw_queue));
+		cur_tx_rp = (u16)((tmpu32 >> 16) & 0x0fff);
+		cur_tx_wp = (u16)(tmpu32 & 0x0fff);
+
+		ring->cur_tx_wp = cur_tx_wp;
+		ring->cur_tx_rp = cur_tx_rp;
+		point_diff = ((cur_tx_rp > cur_tx_wp) ?
+			      (cur_tx_rp - cur_tx_wp) :
+			      (TX_DESC_NUM_92E - cur_tx_wp + cur_tx_rp));
+
+		ring->avl_desc = point_diff;
+	}
+
+	read_point = ring->cur_tx_rp;
+	write_point = ring->cur_tx_wp;
+	available_desc_num = ring->avl_desc;
+
+	if (write_point > read_point) {
+		if (index < write_point && index >= read_point)
+			ret = false;
+		else
+			ret = true;
+	} else if (write_point < read_point) {
+		if (index > write_point && index < read_point)
+			ret = true;
+		else
+			ret = false;
+	} else {
+		if (index != read_point)
+			ret = true;
+	}
+
+	if (hw_queue == BEACON_QUEUE)
+		ret = true;
+
+	if (rtlpriv->rtlhal.driver_is_goingto_unload ||
+	    rtlpriv->psc.rfoff_reason > RF_CHANGE_BY_PS)
+		ret = true;
+
+	if (hw_queue < BEACON_QUEUE) {
+		if (!ret)
+			stop_report_cnt++;
+		else
+			stop_report_cnt = 0;
+	}
+
+	return ret;
+}
+
+u32 rtl92ee_rx_command_packet(struct ieee80211_hw *hw,
+			      struct rtl_stats status,
+			      struct sk_buff *skb)
+{
+	u32 result = 0;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	switch (status.packet_report_type) {
+	case NORMAL_RX:
+		result = 0;
+		break;
+	case C2H_PACKET:
+		rtl92ee_c2h_packet_handler(hw, skb->data, (u8) skb->len);
+		result = 1;
+		break;
+	default:
+		RT_TRACE(COMP_RECV, DBG_TRACE, ("No this packet type!!\n"));
+		break;
+	}
+
+	return result;
+}
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/trx.h b/drivers/staging/rtl8192ee/rtl8192ee/trx.h
new file mode 100644
index 00000000000..c2cd5813a2b
--- /dev/null
+++ b/drivers/staging/rtl8192ee/rtl8192ee/trx.h
@@ -0,0 +1,877 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL92E_TRX_H__
+#define __RTL92E_TRX_H__
+
+
+
+#if (DMA_IS_64BIT == 1)
+#if (RTL8192EE_SEG_NUM == 2)
+#define TX_BD_DESC_SIZE					128
+#elif (RTL8192EE_SEG_NUM == 1)
+#define TX_BD_DESC_SIZE					64
+#elif (RTL8192EE_SEG_NUM == 0)
+#define TX_BD_DESC_SIZE					32
+#endif
+#else
+#if (RTL8192EE_SEG_NUM == 2)
+#define TX_BD_DESC_SIZE					64
+#elif (RTL8192EE_SEG_NUM == 1)
+#define TX_BD_DESC_SIZE					32
+#elif (RTL8192EE_SEG_NUM == 0)
+#define TX_BD_DESC_SIZE					16
+#endif
+#endif
+
+#define TX_DESC_SIZE					64
+
+#define RX_DRV_INFO_SIZE_UNIT				8
+
+#define	TX_DESC_NEXT_DESC_OFFSET			40
+#define USB_HWDESC_HEADER_LEN				40
+
+#define RX_DESC_SIZE					24
+#define MAX_RECEIVE_BUFFER_SIZE				8192
+
+#define SET_TX_DESC_PKT_SIZE(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 0, 16, __val)
+#define SET_TX_DESC_OFFSET(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 16, 8, __val)
+#define SET_TX_DESC_BMC(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 24, 1, __val)
+#define SET_TX_DESC_HTC(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 25, 1, __val)
+#define SET_TX_DESC_LAST_SEG(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 26, 1, __val)
+#define SET_TX_DESC_FIRST_SEG(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 27, 1, __val)
+#define SET_TX_DESC_LINIP(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 28, 1, __val)
+#define SET_TX_DESC_NO_ACM(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 29, 1, __val)
+#define SET_TX_DESC_GF(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 30, 1, __val)
+#define SET_TX_DESC_OWN(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 31, 1, __val)
+
+#define GET_TX_DESC_PKT_SIZE(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 0, 16)
+#define GET_TX_DESC_OFFSET(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 16, 8)
+#define GET_TX_DESC_BMC(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 24, 1)
+#define GET_TX_DESC_HTC(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 25, 1)
+#define GET_TX_DESC_LAST_SEG(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 26, 1)
+#define GET_TX_DESC_FIRST_SEG(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 27, 1)
+#define GET_TX_DESC_LINIP(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 28, 1)
+#define GET_TX_DESC_NO_ACM(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 29, 1)
+#define GET_TX_DESC_GF(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc, 30, 1)
+#define GET_TX_DESC_OWN(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 31, 1)
+
+#define SET_TX_DESC_MACID(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+4, 0, 7, __val)
+#define SET_TX_DESC_QUEUE_SEL(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+4, 8, 5, __val)
+#define SET_TX_DESC_RDG_NAV_EXT(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+4, 13, 1, __val)
+#define SET_TX_DESC_LSIG_TXOP_EN(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+4, 14, 1, __val)
+#define SET_TX_DESC_PIFS(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+4, 15, 1, __val)
+#define SET_TX_DESC_RATE_ID(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+4, 16, 5, __val)
+#define SET_TX_DESC_EN_DESC_ID(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+4, 21, 1, __val)
+#define SET_TX_DESC_SEC_TYPE(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+4, 22, 2, __val)
+#define SET_TX_DESC_PKT_OFFSET(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+4, 24, 5, __val)
+#define SET_TX_DESC_MORE_DATA(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+4, 29, 1, __val)
+#define SET_TX_DESC_TXOP_PS_CAP(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+4, 30, 1, __val)
+#define SET_TX_DESC_TXOP_PS_MODE(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+4, 31, 1, __val)
+
+
+#define GET_TX_DESC_MACID(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 0, 5)
+#define GET_TX_DESC_AGG_ENABLE(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 5, 1)
+#define GET_TX_DESC_AGG_BREAK(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 6, 1)
+#define GET_TX_DESC_RDG_ENABLE(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 7, 1)
+#define GET_TX_DESC_QUEUE_SEL(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 8, 5)
+#define GET_TX_DESC_RDG_NAV_EXT(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+4, 13, 1)
+#define GET_TX_DESC_LSIG_TXOP_EN(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+4, 14, 1)
+#define GET_TX_DESC_PIFS(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 15, 1)
+#define GET_TX_DESC_RATE_ID(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 16, 4)
+#define GET_TX_DESC_NAV_USE_HDR(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+4, 20, 1)
+#define GET_TX_DESC_EN_DESC_ID(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 21, 1)
+#define GET_TX_DESC_SEC_TYPE(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 22, 2)
+#define GET_TX_DESC_PKT_OFFSET(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 24, 5)
+
+#define SET_TX_DESC_PAID(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 0, 9, __val)
+#define SET_TX_DESC_CCA_RTS(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 10, 2, __val)
+#define SET_TX_DESC_AGG_ENABLE(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 12, 1, __val)
+#define SET_TX_DESC_RDG_ENABLE(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 13, 1, __val)
+#define SET_TX_DESC_NULL_0(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 14, 1, __val)
+#define SET_TX_DESC_NULL_1(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 15, 1, __val)
+#define SET_TX_DESC_BK(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 16, 1, __val)
+#define SET_TX_DESC_MORE_FRAG(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 17, 1, __val)
+#define SET_TX_DESC_RAW(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 18, 1, __val)
+#define SET_TX_DESC_SPE_RPT(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 19, 1, __val)
+#define SET_TX_DESC_AMPDU_DENSITY(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 20, 3, __val)
+#define SET_TX_DESC_BT_NULL(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 23, 1, __val)
+#define SET_TX_DESC_GID(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 24, 6, __val)
+
+#define SET_TX_DESC_WHEADER_LEN(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 0, 4, __val)
+#define SET_TX_DESC_CHK_EN(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 4, 1, __val)
+#define SET_TX_DESC_EARLY_RATE(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 5, 1, __val)
+#define SET_TX_DESC_HWSEQ_SEL(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 6, 2, __val)
+#define SET_TX_DESC_USE_RATE(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 8, 1, __val)
+#define SET_TX_DESC_DISABLE_RTS_FB(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 9, 1, __val)
+#define SET_TX_DESC_DISABLE_FB(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 10, 1, __val)
+#define SET_TX_DESC_CTS2SELF(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 11, 1, __val)
+#define SET_TX_DESC_RTS_ENABLE(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 12, 1, __val)
+#define SET_TX_DESC_HW_RTS_ENABLE(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 13, 1, __val)
+#define SET_TX_DESC_HW_PORT_ID(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 14, 1, __val)
+#define SET_TX_DESC_NAV_USE_HDR(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 15, 1, __val)
+#define SET_TX_DESC_USE_MAX_LEN(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 16, 1, __val)
+#define SET_TX_DESC_MAX_AGG_NUM(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 17, 5, __val)
+#define SET_TX_DESC_NDPA(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 22, 2, __val)
+#define SET_TX_DESC_AMPDU_MAX_TIME(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 24, 8, __val)
+
+
+/* Dword 4 */
+#define SET_TX_DESC_TX_RATE(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 0, 7, __val)
+#define SET_TX_DESC_TRY_RATE(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 7, 1, __val)
+#define SET_TX_DESC_DATA_RATE_FB_LIMIT(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 8, 5, __val)
+#define SET_TX_DESC_RTS_RATE_FB_LIMIT(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 13, 4, __val)
+#define SET_TX_DESC_RETRY_LIMIT_ENABLE(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 17, 1, __val)
+#define SET_TX_DESC_DATA_RETRY_LIMIT(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 18, 6, __val)
+#define SET_TX_DESC_RTS_RATE(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 24, 5, __val)
+#define SET_TX_DESC_PCTS_ENABLE(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 29, 1, __val)
+#define SET_TX_DESC_PCTS_MASK_IDX(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 30, 2, __val)
+
+
+/* Dword 5 */
+#define SET_TX_DESC_TX_SUB_CARRIER(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 0, 4, __val)
+#define SET_TX_DESC_DATA_SHORT(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 4, 1, __val)
+#define SET_TX_DESC_DATA_BW(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 5, 2, __val)
+#define SET_TX_DESC_DATA_LDPC(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 7, 1, __val)
+#define SET_TX_DESC_DATA_STBC(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 8, 2, __val)
+#define SET_TX_DESC_VCS_STBC(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 10, 2, __val)
+#define SET_TX_DESC_RTS_SHORT(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 12, 1, __val)
+#define SET_TX_DESC_RTS_SC(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 13, 4, __val)
+#define SET_TX_DESC_TX_ANT(__pdesc , __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 24, 4, __val)
+#define SET_TX_DESC_TX_POWER_0_PSET(__pdesc , __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 28, 3, __val)
+
+/* Dword 6 */
+#define SET_TX_DESC_SW_DEFINE(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+24, 0, 12, __val)
+#define SET_TX_DESC_ANTSEL_A(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+24, 16, 3, __val)
+#define SET_TX_DESC_ANTSEL_B(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+24, 19, 3, __val)
+#define SET_TX_DESC_ANTSEL_C(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+24, 22, 3, __val)
+#define SET_TX_DESC_ANTSEL_D(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+24, 25, 3, __val)
+
+/* Dword 7 */
+#define SET_TX_DESC_TX_BUFFER_SIZE(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+28, 0, 16, __val)
+#define SET_TX_DESC_USB_TXAGG_NUM(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+28, 24, 8, __val)
+
+/* Dword 8 */
+#define SET_TX_DESC_RTS_RC(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+32 , 0 , 6 , __val)
+#define SET_TX_DESC_BAR_RTY_TH(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+32 , 6 , 2 , __val)
+#define SET_TX_DESC_DATA_RC(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+32 , 8 , 6 , __val)
+#define SET_TX_DESC_ENABLE_HW_SELECT(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+32 , 15 , 1 , __val)
+#define SET_TX_DESC_NEXT_HEAD_PAGE(__pdesc , __val)(__pdesc, __val) \
+	SET_BITS_TO_LE_4BYTE(__pdesc+32 , 16 , 8 , __val)
+#define SET_TX_DESC_TAIL_PAGE(__pdesc , __val)(__pdesc, __val)\
+	SET_BITS_TO_LE_4BYTE(__pdesc+32 , 24 , 8 , __val)
+
+/* Dword 9 */
+#define SET_TX_DESC_PADDING_LENGTH(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+36 , 0 , 11 , __val)
+#define SET_TX_DESC_TXBF_PATH(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+36 , 11 , 1 , __val)
+#define SET_TX_DESC_SEQ(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc+36 , 12 , 12 , __val)
+#define SET_TX_DESC_FINAL_DATA_RATE(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+36 , 24 , 8 , __val)
+
+/* Dword 10 */
+#define SET_TX_DESC_TX_BUFFER_ADDRESS(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+40 , 0 , 32 , __val)
+
+
+/* Dword 11*/
+#define SET_TX_DESC_NEXT_DESC_ADDRESS(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+48 , 0 , 32 , __val)
+
+
+#define SET_EARLYMODE_PKTNUM(__paddr , __val)		\
+	SET_BITS_TO_LE_4BYTE(__paddr , 0 , 4 , __val)
+#define SET_EARLYMODE_LEN0(__paddr , __val)		\
+	SET_BITS_TO_LE_4BYTE(__paddr , 4 , 15 , __val)
+#define SET_EARLYMODE_LEN1(__paddr , __val)		\
+	SET_BITS_TO_LE_4BYTE(__paddr , 16 , 2 , __val)
+#define SET_EARLYMODE_LEN1_1(__paddr , __val)		\
+	SET_BITS_TO_LE_4BYTE(__paddr , 19 , 13 , __val)
+#define SET_EARLYMODE_LEN1_2(__paddr , __val)		\
+	SET_BITS_TO_LE_4BYTE(__paddr+4 , 0 , 2 , __val)
+#define SET_EARLYMODE_LEN2(__paddr , __val)		\
+	SET_BITS_TO_LE_4BYTE(__paddr+4 , 2 , 15 ,  __val)
+#define SET_EARLYMODE_LEN2_1(__paddr , __val)		\
+	SET_BITS_TO_LE_4BYTE(__paddr , 2 , 4 ,  __val)
+#define SET_EARLYMODE_LEN2_2(__paddr , __val)		\
+	SET_BITS_TO_LE_4BYTE(__paddr+4 , 0 , 8 ,  __val)
+#define SET_EARLYMODE_LEN3(__paddr , __val)		\
+	SET_BITS_TO_LE_4BYTE(__paddr+4 , 17 , 15, __val)
+#define SET_EARLYMODE_LEN4(__paddr , __val)		\
+	SET_BITS_TO_LE_4BYTE(__paddr+4 , 20 , 12 , __val)
+
+
+/* TX/RX buffer descriptor */
+
+#define SET_TX_EXTBUFF_DESC_LEN(__pdesc, __val, __set)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+(__set*16) , 0 , 16 , __val)
+#define SET_TX_EXTBUFF_DESC_ADDR_LOW(__pdesc, __val, __set)\
+	SET_BITS_TO_LE_4BYTE(__pdesc+(__set*16)+4 , 0 , 32 , __val)
+#define SET_TX_EXTBUFF_DESC_ADDR_HIGH(__pdesc, __val , __set)\
+	SET_BITS_TO_LE_4BYTE(__pdesc+(__set*16)+8 , 0 , 32 , __val)
+
+
+
+/* for Txfilldescroptor92ee, fill the desc content. */
+#if (DMA_IS_64BIT == 1)
+#define SET_TXBUFFER_DESC_LEN_WITH_OFFSET(__pdesc, __offset, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+(__offset*16), 0, 16, __val)
+#define SET_TXBUFFER_DESC_AMSDU_WITH_OFFSET(__pdesc, __offset, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+(__offset*16), 31, 1, __val)
+#define SET_TXBUFFER_DESC_ADD_LOW_WITH_OFFSET(__pdesc, __offset, __val) \
+	SET_BITS_TO_LE_4BYTE(__pdesc+(__offset*16)+4, 0, 32, __val)
+#define SET_TXBUFFER_DESC_ADD_HIGT_WITH_OFFSET(__pdesc, __offset, __val)\
+	SET_BITS_TO_LE_4BYTE(__pdesc+(__offset*16)+8, 0, 32, __val)
+#define GET_TXBUFFER_DESC_ADDR_LOW(__pdesc, __offset)			\
+	LE_BITS_TO_4BYTE(__pdesc+(__offset*16)+4, 0, 32)
+#else
+#define SET_TXBUFFER_DESC_LEN_WITH_OFFSET(__pdesc, __offset, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+(__offset*8), 0, 16, __val)
+#define SET_TXBUFFER_DESC_AMSDU_WITH_OFFSET(__pdesc, __offset, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+(__offset*8), 31, 1, __val)
+#define SET_TXBUFFER_DESC_ADD_LOW_WITH_OFFSET(__pdesc, __offset, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+(__offset*8)+4, 0, 32, __val)
+#define SET_TXBUFFER_DESC_ADD_HIGT_WITH_OFFSET(__pdesc, __offset, __val)
+#define GET_TXBUFFER_DESC_ADDR_LOW(__pdesc, __offset)			\
+	LE_BITS_TO_4BYTE(__pdesc+(__offset*8)+4, 0, 32)
+#endif
+
+/* Dword 0 */
+#define SET_TX_BUFF_DESC_LEN_0(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 0, 14, __val)
+#define SET_TX_BUFF_DESC_PSB(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 16, 15, __val)
+#define SET_TX_BUFF_DESC_OWN(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 31, 1, __val)
+
+/* Dword 1 */
+#define SET_TX_BUFF_DESC_ADDR_LOW_0(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+4, 0, 32, __val)
+#if (DMA_IS_64BIT == 1)
+/* Dword 2 */
+#define SET_TX_BUFF_DESC_ADDR_HIGH_0(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 0, 32, __val)
+/* Dword 3 / RESERVED 0 */
+/* Dword 4 */
+#define SET_TX_BUFF_DESC_LEN_1(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 0, 16, __val)
+#define SET_TX_BUFF_DESC_AMSDU_1(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 31, 1, __val)
+/* Dword 5 */
+#define SET_TX_BUFF_DESC_ADDR_LOW_1(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 0, 32, __val)
+/* Dword 6 */
+#define SET_TX_BUFF_DESC_ADDR_HIGH_1(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+24, 0, 32, __val)
+/* Dword 7 / RESERVED 0 */
+/* Dword 8 */
+#define SET_TX_BUFF_DESC_LEN_2(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+32, 0, 16, __val)
+#define SET_TX_BUFF_DESC_AMSDU_2(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+32, 31, 1, __val)
+/* Dword 9 */
+#define SET_TX_BUFF_DESC_ADDR_LOW_2(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+36, 0, 32, __val)
+/* Dword 10 */
+#define SET_TX_BUFF_DESC_ADDR_HIGH_2(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+40, 0, 32, __val)
+/* Dword 11 / RESERVED 0 */
+/* Dword 12 */
+#define SET_TX_BUFF_DESC_LEN_3(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+48, 0, 16, __val)
+#define SET_TX_BUFF_DESC_AMSDU_3(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+48, 31, 1, __val)
+/* Dword 13 */
+#define SET_TX_BUFF_DESC_ADDR_LOW_3(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+52, 0, 32, __val)
+/* Dword 14 */
+#define SET_TX_BUFF_DESC_ADDR_HIGH_3(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+56, 0, 32, __val)
+/* Dword 15 / RESERVED 0 */
+#else
+#define SET_TX_BUFF_DESC_ADDR_HIGH_0(__pdesc, __val)
+/* Dword 2 */
+#define SET_TX_BUFF_DESC_LEN_1(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 0, 16, __val)
+#define SET_TX_BUFF_DESC_AMSDU_1(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+8, 31, 1, __val)
+/* Dword 3 */
+#define SET_TX_BUFF_DESC_ADDR_LOW_1(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+12, 0, 32, __val)
+#define SET_TX_BUFF_DESC_ADDR_HIGH_1(__pdesc, __val)
+/* Dword 4 */
+#define SET_TX_BUFF_DESC_LEN_2(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 0, 16, __val)
+#define SET_TX_BUFF_DESC_AMSDU_2(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+16, 31, 1, __val)
+/* Dword 5 */
+#define SET_TX_BUFF_DESC_ADDR_LOW_2(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+20, 0, 32, __val)
+#define SET_TX_BUFF_DESC_ADDR_HIGH_2(__pdesc, __val)
+/* Dword 6 */
+#define SET_TX_BUFF_DESC_LEN_3(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+24, 0, 16, __val)
+#define SET_TX_BUFF_DESC_AMSDU_3(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+24, 31, 1, __val)
+/* Dword 7 */
+#define SET_TX_BUFF_DESC_ADDR_LOW_3(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+28, 0, 32, __val)
+#define SET_TX_BUFF_DESC_ADDR_HIGH_3(__pdesc, __val)
+#endif
+
+/* RX buffer  */
+
+/* DWORD 0 */
+#define SET_RX_BUFFER_DESC_DATA_LENGTH(__pRxStatusDesc , __val)	\
+	SET_BITS_TO_LE_4BYTE(__pRxStatusDesc , 0, 14, __val)
+#define SET_RX_BUFFER_DESC_LS(__pRxStatusDesc , __val)		\
+	SET_BITS_TO_LE_4BYTE(__pRxStatusDesc , 15, 1, __val)
+#define SET_RX_BUFFER_DESC_FS(__pRxStatusDesc , __val)		\
+	SET_BITS_TO_LE_4BYTE(__pRxStatusDesc , 16, 1, __val)
+#define SET_RX_BUFFER_DESC_TOTAL_LENGTH(__pRxStatusDesc , __val)	\
+	SET_BITS_TO_LE_4BYTE(__pRxStatusDesc , 16, 15, __val)
+
+#define GET_RX_BUFFER_DESC_OWN(__pRxStatusDesc)			\
+	LE_BITS_TO_4BYTE(__pRxStatusDesc , 31, 1)
+#define GET_RX_BUFFER_DESC_LS(__pRxStatusDesc)			\
+	LE_BITS_TO_4BYTE(__pRxStatusDesc , 15, 1)
+#define GET_RX_BUFFER_DESC_FS(__pRxStatusDesc)			\
+	LE_BITS_TO_4BYTE(__pRxStatusDesc , 16, 1)
+#define GET_RX_BUFFER_DESC_TOTAL_LENGTH(__pRxStatusDesc)	\
+	LE_BITS_TO_4BYTE(__pRxStatusDesc , 16, 15)
+
+
+/* DWORD 1 */
+#define SET_RX_BUFFER_PHYSICAL_LOW(__pRxStatusDesc , __val)	\
+	SET_BITS_TO_LE_4BYTE(__pRxStatusDesc+4, 0, 32, __val)
+
+/* DWORD 2 */
+#define SET_RX_BUFFER_PHYSICAL_HIGH(__pRxStatusDesc , __val)	\
+	SET_BITS_TO_LE_4BYTE(__pRxStatusDesc+8, 0, 32, __val)
+
+#define GET_RX_DESC_PKT_LEN(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 0, 14)
+#define GET_RX_DESC_CRC32(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 14, 1)
+#define GET_RX_DESC_ICV(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 15, 1)
+#define GET_RX_DESC_DRV_INFO_SIZE(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc, 16, 4)
+#define GET_RX_DESC_SECURITY(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 20, 3)
+#define GET_RX_DESC_QOS(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 23, 1)
+#define GET_RX_DESC_SHIFT(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 24, 2)
+#define GET_RX_DESC_PHYST(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 26, 1)
+#define GET_RX_DESC_SWDEC(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 27, 1)
+#define GET_RX_DESC_LS(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc, 28, 1)
+#define GET_RX_DESC_FS(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc, 29, 1)
+#define GET_RX_DESC_EOR(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 30, 1)
+#define GET_RX_DESC_OWN(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc, 31, 1)
+
+#define SET_RX_DESC_PKT_LEN(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 0, 14, __val)
+#define SET_RX_DESC_EOR(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 30, 1, __val)
+#define SET_RX_DESC_OWN(__pdesc, __val)			\
+	SET_BITS_TO_LE_4BYTE(__pdesc, 31, 1, __val)
+
+#define GET_RX_DESC_MACID(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 0, 7)
+#define GET_RX_DESC_TID(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 8, 4)
+#define GET_RX_DESC_MACID_VLD(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 12, 1)
+#define GET_RX_DESC_AMSDU(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 13, 1)
+#define GET_RX_DESC_RXID_MATCH(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 14, 1)
+#define GET_RX_DESC_PAGGR(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 15, 1)
+#define GET_RX_DESC_A1_FIT(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 16, 4)
+#define GET_RX_DESC_TCPOFFLOAD_CHKERR(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+4, 20, 1)
+#define GET_RX_DESC_TCPOFFLOAD_IPVER(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+4, 21, 1)
+#define GET_RX_DESC_TCPOFFLOAD_IS_TCPUDP(__pdesc)	\
+	LE_BITS_TO_4BYTE(__pdesc+4, 22, 1)
+#define GET_RX_DESC_TCPOFFLOAD_CHK_VLD(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+4, 23, 1)
+#define GET_RX_DESC_PAM(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 24, 1)
+#define GET_RX_DESC_PWR(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 25, 1)
+#define GET_RX_DESC_MD(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+4, 26, 1)
+#define GET_RX_DESC_MF(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+4, 27, 1)
+#define GET_RX_DESC_TYPE(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+4, 28, 2)
+#define GET_RX_DESC_MC(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+4, 30, 1)
+#define GET_RX_DESC_BC(__pdesc)				\
+	LE_BITS_TO_4BYTE(__pdesc+4, 31, 1)
+#define GET_RX_DESC_SEQ(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+8, 0, 12)
+#define GET_RX_DESC_FRAG(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+8, 12, 4)
+#define GET_RX_DESC_RX_IS_QOS(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+8, 16, 1)
+
+#define GET_RX_DESC_RXMCS(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+12, 0, 7)
+#define GET_RX_DESC_HTC(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+12, 10, 1)
+#define GET_RX_STATUS_DESC_EOSP(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+12, 11, 1)
+#define GET_RX_STATUS_DESC_BSSID_FIT(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+12, 12, 2)
+#define GET_RX_STATUS_DESC_DMA_AGG_NUM(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+12, 16, 8)
+#define GET_RX_STATUS_DESC_PATTERN_MATCH(__pdesc)	\
+	LE_BITS_TO_4BYTE(__pdesc+12, 29, 1)
+#define GET_RX_STATUS_DESC_UNICAST_MATCH(__pdesc)	\
+	LE_BITS_TO_4BYTE(__pdesc+12, 30, 1)
+#define GET_RX_STATUS_DESC_MAGIC_MATCH(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+12, 31, 1)
+
+
+#define GET_RX_DESC_TSFL(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+20, 0, 32)
+
+#define GET_RX_DESC_BUFF_ADDR(__pdesc)			\
+	LE_BITS_TO_4BYTE(__pdesc+24, 0, 32)
+#define GET_RX_DESC_BUFF_ADDR64(__pdesc)		\
+	LE_BITS_TO_4BYTE(__pdesc+28, 0, 32)
+
+#define SET_RX_DESC_BUFF_ADDR(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+24, 0, 32, __val)
+#define SET_RX_DESC_BUFF_ADDR64(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+28, 0, 32, __val)
+
+
+/* TX report 2 format in Rx desc*/
+
+#define GET_RX_RPT2_DESC_PKT_LEN(__pRxStatusDesc)	\
+	LE_BITS_TO_4BYTE(__pRxStatusDesc , 0, 9)
+#define GET_RX_RPT2_DESC_MACID_VALID_1(__pRxStatusDesc)	\
+	LE_BITS_TO_4BYTE(__pRxStatusDesc+16, 0, 32)
+#define GET_RX_RPT2_DESC_MACID_VALID_2(__pRxStatusDesc)	\
+	LE_BITS_TO_4BYTE(__pRxStatusDesc+20, 0, 32)
+
+
+#define CLEAR_PCI_TX_DESC_CONTENT(__pdesc, _size)		\
+do {								\
+	if (_size > TX_DESC_NEXT_DESC_OFFSET)			\
+		memset(__pdesc, 0, TX_DESC_NEXT_DESC_OFFSET);	\
+	else							\
+		memset(__pdesc, 0, _size);			\
+} while (0)
+
+#define RX_HAL_IS_CCK_RATE(rxmcs)\
+	(rxmcs == DESC92C_RATE1M ||\
+	 rxmcs == DESC92C_RATE2M ||\
+	 rxmcs == DESC92C_RATE5_5M ||\
+	 rxmcs == DESC92C_RATE11M)
+
+#define IS_LITTLE_ENDIAN	1
+
+struct phy_rx_agc_info_t {
+	#if IS_LITTLE_ENDIAN
+		u8 gain:7 , trsw:1;
+	#else
+		u8 trsw:1 , gain:7;
+	#endif
+};
+struct phy_status_rpt {
+	struct phy_rx_agc_info_t path_agc[2];
+	u8 ch_corr[2];
+	u8 cck_sig_qual_ofdm_pwdb_all;
+	u8 cck_agc_rpt_ofdm_cfosho_a;
+	u8 cck_rpt_b_ofdm_cfosho_b;
+	u8 rsvd_1;
+	u8 noise_power_db_msb;
+	u8 path_cfotail[2];
+	u8 pcts_mask[2];
+	u8 stream_rxevm[2];
+	u8 path_rxsnr[2];
+	u8 noise_power_db_lsb;
+	u8 rsvd_2[3];
+	u8 stream_csi[2];
+	u8 stream_target_csi[2];
+	u8 sig_evm;
+	u8 rsvd_3;
+#if IS_LITTLE_ENDIAN
+	u8 antsel_rx_keep_2:1;	/*ex_intf_flg:1;*/
+	u8 sgi_en:1;
+	u8 rxsc:2;
+	u8 idle_long:1;
+	u8 r_ant_train_en:1;
+	u8 ant_sel_b:1;
+	u8 ant_sel:1;
+#else	/* _BIG_ENDIAN_	*/
+	u8 ant_sel:1;
+	u8 ant_sel_b:1;
+	u8 r_ant_train_en:1;
+	u8 idle_long:1;
+	u8 rxsc:2;
+	u8 sgi_en:1;
+	u8 antsel_rx_keep_2:1;	/*ex_intf_flg:1;*/
+#endif
+} __packed;
+
+struct rx_fwinfo {
+	u8 gain_trsw[4];
+	u8 pwdb_all;
+	u8 cfosho[4];
+	u8 cfotail[4];
+	char rxevm[2];
+	char rxsnr[4];
+	u8 pdsnr[2];
+	u8 csi_current[2];
+	u8 csi_target[2];
+	u8 sigevm;
+	u8 max_ex_pwr;
+	u8 ex_intf_flag:1;
+	u8 sgi_en:1;
+	u8 rxsc:2;
+	u8 reserve:4;
+} __packed;
+
+struct tx_desc {
+	u32 pktsize:16;
+	u32 offset:8;
+	u32 bmc:1;
+	u32 htc:1;
+	u32 lastseg:1;
+	u32 firstseg:1;
+	u32 linip:1;
+	u32 noacm:1;
+	u32 gf:1;
+	u32 own:1;
+
+	u32 macid:6;
+	u32 rsvd0:2;
+	u32 queuesel:5;
+	u32 rd_nav_ext:1;
+	u32 lsig_txop_en:1;
+	u32 pifs:1;
+	u32 rateid:4;
+	u32 nav_usehdr:1;
+	u32 en_descid:1;
+	u32 sectype:2;
+	u32 pktoffset:8;
+
+	u32 rts_rc:6;
+	u32 data_rc:6;
+	u32 agg_en:1;
+	u32 rdg_en:1;
+	u32 bar_retryht:2;
+	u32 agg_break:1;
+	u32 morefrag:1;
+	u32 raw:1;
+	u32 ccx:1;
+	u32 ampdudensity:3;
+	u32 bt_int:1;
+	u32 ant_sela:1;
+	u32 ant_selb:1;
+	u32 txant_cck:2;
+	u32 txant_l:2;
+	u32 txant_ht:2;
+
+	u32 nextheadpage:8;
+	u32 tailpage:8;
+	u32 seq:12;
+	u32 cpu_handle:1;
+	u32 tag1:1;
+	u32 trigger_int:1;
+	u32 hwseq_en:1;
+
+	u32 rtsrate:5;
+	u32 apdcfe:1;
+	u32 qos:1;
+	u32 hwseq_ssn:1;
+	u32 userrate:1;
+	u32 dis_rtsfb:1;
+	u32 dis_datafb:1;
+	u32 cts2self:1;
+	u32 rts_en:1;
+	u32 hwrts_en:1;
+	u32 portid:1;
+	u32 pwr_status:3;
+	u32 waitdcts:1;
+	u32 cts2ap_en:1;
+	u32 txsc:2;
+	u32 stbc:2;
+	u32 txshort:1;
+	u32 txbw:1;
+	u32 rtsshort:1;
+	u32 rtsbw:1;
+	u32 rtssc:2;
+	u32 rtsstbc:2;
+
+	u32 txrate:6;
+	u32 shortgi:1;
+	u32 ccxt:1;
+	u32 txrate_fb_lmt:5;
+	u32 rtsrate_fb_lmt:4;
+	u32 retrylmt_en:1;
+	u32 txretrylmt:6;
+	u32 usb_txaggnum:8;
+
+	u32 txagca:5;
+	u32 txagcb:5;
+	u32 usemaxlen:1;
+	u32 maxaggnum:5;
+	u32 mcsg1maxlen:4;
+	u32 mcsg2maxlen:4;
+	u32 mcsg3maxlen:4;
+	u32 mcs7sgimaxlen:4;
+
+	u32 txbuffersize:16;
+	u32 sw_offset30:8;
+	u32 sw_offset31:4;
+	u32 rsvd1:1;
+	u32 antsel_c:1;
+	u32 null_0:1;
+	u32 null_1:1;
+
+	u32 txbuffaddr;
+	u32 txbufferaddr64;
+	u32 nextdescaddress;
+	u32 nextdescaddress64;
+
+	u32 reserve_pass_pcie_mm_limit[4];
+} __packed;
+
+struct rx_desc {
+	u32 length:14;
+	u32 crc32:1;
+	u32 icverror:1;
+	u32 drv_infosize:4;
+	u32 security:3;
+	u32 qos:1;
+	u32 shift:2;
+	u32 phystatus:1;
+	u32 swdec:1;
+	u32 lastseg:1;
+	u32 firstseg:1;
+	u32 eor:1;
+	u32 own:1;
+
+	u32 macid:6;
+	u32 tid:4;
+	u32 hwrsvd:5;
+	u32 paggr:1;
+	u32 faggr:1;
+	u32 a1_fit:4;
+	u32 a2_fit:4;
+	u32 pam:1;
+	u32 pwr:1;
+	u32 moredata:1;
+	u32 morefrag:1;
+	u32 type:2;
+	u32 mc:1;
+	u32 bc:1;
+
+	u32 seq:12;
+	u32 frag:4;
+	u32 nextpktlen:14;
+	u32 nextind:1;
+	u32 rsvd:1;
+
+	u32 rxmcs:6;
+	u32 rxht:1;
+	u32 amsdu:1;
+	u32 splcp:1;
+	u32 bandwidth:1;
+	u32 htc:1;
+	u32 tcpchk_rpt:1;
+	u32 ipcchk_rpt:1;
+	u32 tcpchk_valid:1;
+	u32 hwpcerr:1;
+	u32 hwpcind:1;
+	u32 iv0:16;
+
+	u32 iv1;
+
+	u32 tsfl;
+
+	u32 bufferaddress;
+	u32 bufferaddress64;
+
+} __packed;
+
+void rtl92ee_rx_check_dma_ok(struct ieee80211_hw *hw, u8 *header_desc,
+			     u8 queue_index);
+u16	rtl92ee_rx_desc_buff_remained_cnt(struct ieee80211_hw *hw,
+					  u8 queue_index);
+void rtl92ee_get_available_desc(struct ieee80211_hw *hw , u8 queue_index);
+void rtl92ee_pre_fill_tx_bd_desc(struct ieee80211_hw *hw,
+				 u8 *tx_bd_desc, u8 *desc, u8 queue_index,
+				 struct sk_buff *skb, dma_addr_t addr);
+
+
+void rtl92ee_tx_fill_desc(struct ieee80211_hw *hw,
+			  struct ieee80211_hdr *hdr, u8 *pdesc_tx,
+			  u8 *pbd_desc_tx,
+			  struct ieee80211_tx_info *info,
+			  struct ieee80211_sta *sta,
+			  struct sk_buff *skb,
+			  u8 hw_queue, struct rtl_tcb_desc *ptcb_desc);
+bool rtl92ee_rx_query_desc(struct ieee80211_hw *hw,
+			   struct rtl_stats *status,
+			   struct ieee80211_rx_status *rx_status,
+			   u8 *pdesc, struct sk_buff *skb);
+void rtl92ee_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
+		      u8 desc_name, u8 *val);
+
+u32 rtl92ee_get_desc(u8 *pdesc, bool istx, u8 desc_name);
+bool rtl92ee_is_tx_desc_closed(struct ieee80211_hw *hw, u8 hw_queue, u16 index);
+void rtl92ee_tx_fill_cmddesc(struct ieee80211_hw *hw, u8 *pdesc,
+			     bool b_firstseg, bool b_lastseg,
+			     struct sk_buff *skb);
+u32 rtl92ee_rx_command_packet(struct ieee80211_hw *hw,
+			      struct rtl_stats status,
+			      struct sk_buff *skb);
+#endif
diff --git a/drivers/staging/rtl8192ee/stats.c b/drivers/staging/rtl8192ee/stats.c
new file mode 100644
index 00000000000..7ac302b1a71
--- /dev/null
+++ b/drivers/staging/rtl8192ee/stats.c
@@ -0,0 +1,290 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+#include "wifi.h"
+#include "stats.h"
+
+u8 stg_rtl_query_rxpwrpercentage(char antpower)
+{
+	if ((antpower <= -100) || (antpower >= 20))
+		return 0;
+	else if (antpower >= 0)
+		return 100;
+	else
+		return 100 + antpower;
+}
+EXPORT_SYMBOL(stg_rtl_query_rxpwrpercentage);
+
+u8 stg_rtl_evm_db_to_percentage(char value)
+{
+	char ret_val;
+	ret_val = value;
+
+	if (ret_val >= 0)
+		ret_val = 0;
+	if (ret_val <= -33)
+		ret_val = -33;
+	ret_val = 0 - ret_val;
+	ret_val *= 3;
+	if (ret_val == 99)
+		ret_val = 100;
+
+	return ret_val;
+}
+EXPORT_SYMBOL(stg_rtl_evm_db_to_percentage);
+
+u8 rtl_evm_dbm_jaguar(char value)
+{
+	char ret_val;
+	ret_val = value;
+
+	/* -33dB~0dB to 33dB ~ 0dB*/
+	if (ret_val == -128)
+		ret_val = 127;
+	else if (ret_val < 0)
+		ret_val = 0 - ret_val;
+
+	ret_val  = ret_val >> 1;
+	return ret_val;
+}
+EXPORT_SYMBOL(rtl_evm_dbm_jaguar);
+
+static long rtl_translate_todbm(struct ieee80211_hw *hw,
+				u8 signal_strength_index)
+{
+	long signal_power;
+
+	signal_power = (long)((signal_strength_index + 1) >> 1);
+	signal_power -= 95;
+	return signal_power;
+}
+
+long stg_rtl_signal_scale_mapping(struct ieee80211_hw *hw, long currsig)
+{
+	long retsig;
+
+	if (currsig >= 61 && currsig <= 100)
+		retsig = 90 + ((currsig - 60) / 4);
+	else if (currsig >= 41 && currsig <= 60)
+		retsig = 78 + ((currsig - 40) / 2);
+	else if (currsig >= 31 && currsig <= 40)
+		retsig = 66 + (currsig - 30);
+	else if (currsig >= 21 && currsig <= 30)
+		retsig = 54 + (currsig - 20);
+	else if (currsig >= 5 && currsig <= 20)
+		retsig = 42 + (((currsig - 5) * 2) / 3);
+	else if (currsig == 4)
+		retsig = 36;
+	else if (currsig == 3)
+		retsig = 27;
+	else if (currsig == 2)
+		retsig = 18;
+	else if (currsig == 1)
+		retsig = 9;
+	else
+		retsig = currsig;
+
+	return retsig;
+}
+EXPORT_SYMBOL(stg_rtl_signal_scale_mapping);
+
+static void rtl_process_ui_rssi(struct ieee80211_hw *hw, struct rtl_stats *pstatus)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u8 rfpath;
+	u32 last_rssi, tmpval;
+
+	if (!pstatus->b_packet_toself && !pstatus->b_packet_beacon)
+		return;
+
+	rtlpriv->stats.pwdb_all_cnt += pstatus->rx_pwdb_all;
+	rtlpriv->stats.rssi_calculate_cnt++;
+
+	if (rtlpriv->stats.ui_rssi.total_num++ >= PHY_RSSI_SLID_WIN_MAX) {
+		rtlpriv->stats.ui_rssi.total_num = PHY_RSSI_SLID_WIN_MAX;
+		last_rssi = rtlpriv->stats.ui_rssi.elements[
+			rtlpriv->stats.ui_rssi.index];
+		rtlpriv->stats.ui_rssi.total_val -= last_rssi;
+	}
+	rtlpriv->stats.ui_rssi.total_val += pstatus->signalstrength;
+	rtlpriv->stats.ui_rssi.elements[rtlpriv->stats.ui_rssi.index++] =
+	    pstatus->signalstrength;
+	if (rtlpriv->stats.ui_rssi.index >= PHY_RSSI_SLID_WIN_MAX)
+		rtlpriv->stats.ui_rssi.index = 0;
+	tmpval = rtlpriv->stats.ui_rssi.total_val /
+		rtlpriv->stats.ui_rssi.total_num;
+	rtlpriv->stats.signal_strength = rtl_translate_todbm(hw,
+		(u8) tmpval);
+	pstatus->rssi = rtlpriv->stats.signal_strength;
+
+	if (pstatus->b_is_cck)
+		return;
+
+	for (rfpath = RF90_PATH_A; rfpath < rtlphy->num_total_rfpath;
+	     rfpath++) {
+		if (rtlpriv->stats.rx_rssi_percentage[rfpath] == 0) {
+			rtlpriv->stats.rx_rssi_percentage[rfpath] =
+			    pstatus->rx_mimo_signalstrength[rfpath];
+		}
+		if (pstatus->rx_mimo_signalstrength[rfpath] >
+		    rtlpriv->stats.rx_rssi_percentage[rfpath]) {
+			rtlpriv->stats.rx_rssi_percentage[rfpath] =
+			    ((rtlpriv->stats.rx_rssi_percentage[rfpath] *
+			      (RX_SMOOTH_FACTOR - 1)) +
+			     (pstatus->rx_mimo_signalstrength[rfpath])) /
+			    (RX_SMOOTH_FACTOR);
+			rtlpriv->stats.rx_rssi_percentage[rfpath] =
+			    rtlpriv->stats.rx_rssi_percentage[rfpath] + 1;
+		} else {
+			rtlpriv->stats.rx_rssi_percentage[rfpath] =
+			    ((rtlpriv->stats.rx_rssi_percentage[rfpath] *
+			      (RX_SMOOTH_FACTOR - 1)) +
+			     (pstatus->rx_mimo_signalstrength[rfpath])) /
+			    (RX_SMOOTH_FACTOR);
+		}
+		rtlpriv->stats.rx_snr_db[rfpath] = pstatus->rx_snr[rfpath];
+		rtlpriv->stats.rx_evm_dbm[rfpath] =
+					pstatus->rx_mimo_evm_dbm[rfpath];
+		rtlpriv->stats.rx_cfo_short[rfpath] =
+					pstatus->cfo_short[rfpath];
+		rtlpriv->stats.rx_cfo_tail[rfpath] = pstatus->cfo_tail[rfpath];
+	}
+}
+
+static void rtl_update_rxsignalstatistics(struct ieee80211_hw *hw,
+					  struct rtl_stats *pstatus)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int weighting = 0;
+
+	if (rtlpriv->stats.recv_signal_power == 0)
+		rtlpriv->stats.recv_signal_power = pstatus->recvsignalpower;
+	if (pstatus->recvsignalpower > rtlpriv->stats.recv_signal_power)
+		weighting = 5;
+	else if (pstatus->recvsignalpower < rtlpriv->stats.recv_signal_power)
+		weighting = (-5);
+	rtlpriv->stats.recv_signal_power = (rtlpriv->stats.recv_signal_power *
+		5 + pstatus->recvsignalpower + weighting) / 6;
+}
+
+static void rtl_process_pwdb(struct ieee80211_hw *hw, struct rtl_stats *pstatus)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_sta_info *drv_priv = NULL;
+	struct ieee80211_sta *sta = NULL;
+	long undecorated_smoothed_pwdb;
+
+	rcu_read_lock();
+	if (rtlpriv->mac80211.opmode != NL80211_IFTYPE_STATION)
+		sta = rtl_find_sta(hw, pstatus->psaddr);
+
+	/* adhoc or ap mode */
+	if (sta) {
+		drv_priv = (struct rtl_sta_info *)sta->drv_priv;
+		undecorated_smoothed_pwdb =
+			drv_priv->rssi_stat.undecorated_smoothed_pwdb;
+	} else {
+		undecorated_smoothed_pwdb =
+			rtlpriv->dm.undecorated_smoothed_pwdb;
+	}
+
+	if (undecorated_smoothed_pwdb < 0)
+		undecorated_smoothed_pwdb = pstatus->rx_pwdb_all;
+	if (pstatus->rx_pwdb_all > (u32) undecorated_smoothed_pwdb) {
+		undecorated_smoothed_pwdb = (((undecorated_smoothed_pwdb) *
+		      (RX_SMOOTH_FACTOR - 1)) +
+		     (pstatus->rx_pwdb_all)) / (RX_SMOOTH_FACTOR);
+		undecorated_smoothed_pwdb = undecorated_smoothed_pwdb + 1;
+	} else {
+		undecorated_smoothed_pwdb = (((undecorated_smoothed_pwdb) *
+		      (RX_SMOOTH_FACTOR - 1)) +
+		     (pstatus->rx_pwdb_all)) / (RX_SMOOTH_FACTOR);
+	}
+
+	if (sta) {
+		drv_priv->rssi_stat.undecorated_smoothed_pwdb =
+			undecorated_smoothed_pwdb;
+	} else {
+		rtlpriv->dm.undecorated_smoothed_pwdb = undecorated_smoothed_pwdb;
+	}
+	rcu_read_unlock();
+
+	rtl_update_rxsignalstatistics(hw, pstatus);
+}
+
+static void rtl_process_ui_link_quality(struct ieee80211_hw *hw,
+					struct rtl_stats *pstatus)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 last_evm, n_stream, tmpval;
+
+	if (pstatus->signalquality == 0)
+		return;
+
+	if (rtlpriv->stats.ui_link_quality.total_num++ >=
+	    PHY_LINKQUALITY_SLID_WIN_MAX) {
+		rtlpriv->stats.ui_link_quality.total_num =
+		    PHY_LINKQUALITY_SLID_WIN_MAX;
+		last_evm = rtlpriv->stats.ui_link_quality.elements[
+			rtlpriv->stats.ui_link_quality.index];
+		rtlpriv->stats.ui_link_quality.total_val -= last_evm;
+	}
+	rtlpriv->stats.ui_link_quality.total_val += pstatus->signalquality;
+	rtlpriv->stats.ui_link_quality.elements[
+		rtlpriv->stats.ui_link_quality.index++] =
+							pstatus->signalquality;
+	if (rtlpriv->stats.ui_link_quality.index >=
+	    PHY_LINKQUALITY_SLID_WIN_MAX)
+		rtlpriv->stats.ui_link_quality.index = 0;
+	tmpval = rtlpriv->stats.ui_link_quality.total_val /
+	    rtlpriv->stats.ui_link_quality.total_num;
+	rtlpriv->stats.signal_quality = tmpval;
+	rtlpriv->stats.last_sigstrength_inpercent = tmpval;
+	for (n_stream = 0; n_stream < 2; n_stream++) {
+		if (pstatus->rx_mimo_signalquality[n_stream] != -1) {
+			if (rtlpriv->stats.rx_evm_percentage[n_stream] == 0) {
+				rtlpriv->stats.rx_evm_percentage[n_stream] =
+				    pstatus->rx_mimo_signalquality[n_stream];
+			}
+			rtlpriv->stats.rx_evm_percentage[n_stream] =
+			    ((rtlpriv->stats.rx_evm_percentage[n_stream]
+			      * (RX_SMOOTH_FACTOR - 1)) +
+			     (pstatus->rx_mimo_signalquality[n_stream] * 1)) /
+			    (RX_SMOOTH_FACTOR);
+		}
+	}
+}
+
+void stg_rtl_process_phyinfo(struct ieee80211_hw *hw, u8 *buffer,
+			     struct rtl_stats *pstatus)
+{
+	if (!pstatus->b_packet_matchbssid)
+		return;
+
+	rtl_process_ui_rssi(hw, pstatus);
+	rtl_process_pwdb(hw, pstatus);
+	rtl_process_ui_link_quality(hw, pstatus);
+}
+EXPORT_SYMBOL(stg_rtl_process_phyinfo);
diff --git a/drivers/staging/rtl8192ee/stats.h b/drivers/staging/rtl8192ee/stats.h
new file mode 100644
index 00000000000..07284278530
--- /dev/null
+++ b/drivers/staging/rtl8192ee/stats.h
@@ -0,0 +1,43 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL_STATS_H__
+#define __RTL_STATS_H__
+
+#define	PHY_RSSI_SLID_WIN_MAX			100
+#define	PHY_LINKQUALITY_SLID_WIN_MAX		20
+#define	PHY_BEACON_RSSI_SLID_WIN_MAX		10
+
+/* Rx smooth factor */
+#define	RX_SMOOTH_FACTOR			20
+
+u8 stg_rtl_query_rxpwrpercentage(char antpower);
+u8 stg_rtl_evm_db_to_percentage(char value);
+u8 rtl_evm_dbm_jaguar(char value);
+long stg_rtl_signal_scale_mapping(struct ieee80211_hw *hw, long currsig);
+void stg_rtl_process_phyinfo(struct ieee80211_hw *hw, u8 *buffer,
+			     struct rtl_stats *pstatus);
+
+#endif
diff --git a/drivers/staging/rtl8192ee/wifi.h b/drivers/staging/rtl8192ee/wifi.h
new file mode 100644
index 00000000000..96fa261a70a
--- /dev/null
+++ b/drivers/staging/rtl8192ee/wifi.h
@@ -0,0 +1,2644 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL_WIFI_H__
+#define __RTL_WIFI_H__
+
+#include <linux/interrupt.h>
+#include <linux/sched.h>
+#include <linux/firmware.h>
+#include <linux/etherdevice.h>
+#include <net/mac80211.h>
+#include "debug.h"
+
+
+#define RF_CHANGE_BY_INIT		0
+#define RF_CHANGE_BY_IPS		BIT(28)
+#define RF_CHANGE_BY_PS			BIT(29)
+#define RF_CHANGE_BY_HW			BIT(30)
+#define RF_CHANGE_BY_SW			BIT(31)
+
+#define IQK_ADDA_REG_NUM		16
+#define IQK_MAC_REG_NUM			4
+#define IQK_THRESHOLD			8
+
+#define MAX_KEY_LEN			61
+#define KEY_BUF_SIZE			5
+
+/* QoS related. */
+/*aci: 0x00	Best Effort*/
+/*aci: 0x01	Background*/
+/*aci: 0x10	Video*/
+/*aci: 0x11	Voice*/
+/*Max: define total number.*/
+#define AC0_BE				0
+#define AC1_BK				1
+#define AC2_VI				2
+#define AC3_VO				3
+#define AC_MAX				4
+#define QOS_QUEUE_NUM			4
+#define RTL_MAC80211_NUM_QUEUE		5
+
+#define QBSS_LOAD_SIZE			5
+#define MAX_WMMELE_LENGTH		64
+
+#define TOTAL_CAM_ENTRY			32
+
+/*slot time for 11g. */
+#define RTL_SLOT_TIME_9			9
+#define RTL_SLOT_TIME_20		20
+
+/*related with tcp/ip. */
+/*if_ehther.h*/
+#define ETH_P_PAE			0x888E	/*Port Access Entity
+						 *(IEEE 802.1X) */
+#define ETH_P_IP			0x0800	/*Internet Protocol packet */
+#define ETH_P_ARP			0x0806	/*Address Resolution packet */
+#define SNAP_SIZE			6
+#define PROTOC_TYPE_SIZE		2
+
+/*related with 802.11 frame*/
+#define MAC80211_3ADDR_LEN		24
+#define MAC80211_4ADDR_LEN		30
+
+#define CHANNEL_MAX_NUMBER		(14 + 24 + 21)	/* 14 is the max
+							 * channel number */
+#define CHANNEL_MAX_NUMBER_2G		14
+#define CHANNEL_MAX_NUMBER_5G		54 /* Please refer to
+					    *"phy_GetChnlGroup8812A" and
+					    * "Hal_ReadTxPowerInfo8812A"*/
+
+#define MAX_REGULATION_NUM			4
+#define MAX_RF_PATH_NUM	2
+#define MAX_RATE_SECTION_NUM		6
+#define MAX_2_4G_BANDWITH_NUM		2
+#define MAX_5G_BANDWITH_NUM		4
+
+
+
+
+#define CHANNEL_MAX_NUMBER_5G_80M	7
+#define CHANNEL_GROUP_MAX		(3 + 9)	/* ch1~3, ch4~9, ch10~14
+						 * total three groups */
+#define MAX_PG_GROUP			13
+#define	CHANNEL_GROUP_MAX_2G		3
+#define	CHANNEL_GROUP_IDX_5GL		3
+#define	CHANNEL_GROUP_IDX_5GM		6
+#define	CHANNEL_GROUP_IDX_5GH		9
+#define	CHANNEL_GROUP_MAX_5G		9
+#define CHANNEL_MAX_NUMBER_2G		14
+#define AVG_THERMAL_NUM			8
+#define AVG_THERMAL_NUM_92E		4
+#define AVG_THERMAL_NUM_88E		4
+#define AVG_THERMAL_NUM_8723BE		4
+#define MAX_TID_COUNT			9
+#define MAX_NUM_RATES			264
+
+/*for 88E use*/
+/*It must always set to 4, otherwise read efuse table secquence will be wrong.*/
+#define MAX_TX_COUNT			4
+#define	MAX_RF_PATH			4
+#define	MAX_CHNL_GROUP_24G		6
+#define	MAX_CHNL_GROUP_5G		14
+
+/* BK, BE, VI, VO, HCCA, MANAGEMENT, COMMAND, HIGH, BEACON. */
+#define MAX_TX_QUEUE			9
+
+#define TX_PWR_BY_RATE_NUM_BAND		2
+#define TX_PWR_BY_RATE_NUM_RF		4
+#define TX_PWR_BY_RATE_NUM_SECTION	12
+#define MAX_BASE_NUM_IN_PHY_REG_PG_24G  6
+#define MAX_BASE_NUM_IN_PHY_REG_PG_5G	5
+
+#define DELTA_SWINGIDX_SIZE	30
+#define BAND_NUM				3
+/*Now, it's just for 8192ee
+ *not OK yet, keep it 0*/
+#define DMA_IS_64BIT 0
+#define RTL8192EE_SEG_NUM		1 /* 0:2 seg, 1: 4 seg, 2: 8 seg */
+
+
+#define PACKET_NORMAL			0
+#define PACKET_DHCP			1
+#define PACKET_ARP			2
+#define PACKET_EAPOL			3
+
+#define	MAX_SUPPORT_WOL_PATTERN_NUM	16
+#define	RSVD_WOL_PATTERN_NUM		1
+#define	WKFMCAM_ADDR_NUM		6
+#define	WKFMCAM_SIZE			24
+
+#define	MAX_WOL_BIT_MASK_SIZE		16
+/* MIN LEN keeps 13 here */
+#define	MIN_WOL_PATTERN_SIZE		13
+#define	MAX_WOL_PATTERN_SIZE		128
+
+#define	WAKE_ON_MAGIC_PACKET		BIT(0)
+#define	WAKE_ON_PATTERN_MATCH		BIT(1)
+
+#define	WOL_REASON_PTK_UPDATE		BIT(0)
+#define	WOL_REASON_GTK_UPDATE		BIT(1)
+#define	WOL_REASON_DISASSOC		BIT(2)
+#define	WOL_REASON_DEAUTH		BIT(3)
+#define	WOL_REASON_AP_LOST		BIT(4)
+#define	WOL_REASON_MAGIC_PKT		BIT(5)
+#define	WOL_REASON_UNICAST_PKT		BIT(6)
+#define	WOL_REASON_PATTERN_PKT		BIT(7)
+#define	WOL_REASON_RTD3_SSID_MATCH	BIT(8)
+#define	WOL_REASON_REALWOW_V2_WAKEUPPKT	BIT(9)
+#define	WOL_REASON_REALWOW_V2_ACKLOST	BIT(10)
+
+struct txpower_info_2g {
+	u8 index_cck_base[MAX_RF_PATH][MAX_CHNL_GROUP_24G];
+	u8 index_bw40_base[MAX_RF_PATH][MAX_CHNL_GROUP_24G];
+	/*If only one tx, only BW20 and OFDM are used.*/
+	u8 cck_diff[MAX_RF_PATH][MAX_TX_COUNT];
+	u8 ofdm_diff[MAX_RF_PATH][MAX_TX_COUNT];
+	u8 bw20_diff[MAX_RF_PATH][MAX_TX_COUNT];
+	u8 bw40_diff[MAX_RF_PATH][MAX_TX_COUNT];
+};
+
+struct txpower_info_5g {
+	u8 index_bw40_base[MAX_RF_PATH][MAX_CHNL_GROUP_5G];
+	/*If only one tx, only BW20, OFDM, BW80 and BW160 are used.*/
+	u8 ofdm_diff[MAX_RF_PATH][MAX_TX_COUNT];
+	u8 bw20_diff[MAX_RF_PATH][MAX_TX_COUNT];
+	u8 bw40_diff[MAX_RF_PATH][MAX_TX_COUNT];
+	u8 bw80_diff[MAX_RF_PATH][MAX_TX_COUNT];
+	u8 bw160_diff[MAX_RF_PATH][MAX_TX_COUNT];
+};
+
+
+/* for early mode */
+#define EM_HDR_LEN			8
+#define FCS_LEN				4
+
+#define MAX_VIRTUAL_MAC			1
+
+enum rf_tx_num {
+	RF_1TX = 0,
+	RF_2TX,
+	RF_MAX_TX_NUM,
+	RF_TX_NUM_NONIMPLEMENT,
+};
+
+enum rate_section {
+	CCK = 0,
+	OFDM,
+	HT_MCS0_MCS7,
+	HT_MCS8_MCS15,
+	VHT_1SSMCS0_1SSMCS9,
+	VHT_2SSMCS0_2SSMCS9,
+};
+
+enum intf_type {
+	INTF_PCI = 0,
+	INTF_USB = 1,
+};
+
+enum radio_path {
+	RF90_PATH_A = 0,
+	RF90_PATH_B = 1,
+	RF90_PATH_C = 2,
+	RF90_PATH_D = 3,
+};
+
+enum regulation_txpwr_lmt {
+	TXPWR_LMT_FCC = 0,
+	TXPWR_LMT_MKK = 1,
+	TXPWR_LMT_ETSI = 2,
+	TXPWR_LMT_WW = 3,
+
+	TXPWR_LMT_MAX_REGULATION_NUM = 4
+};
+
+
+enum rt_eeprom_type {
+	EEPROM_93C46,
+	EEPROM_93C56,
+	EEPROM_BOOT_EFUSE,
+};
+
+enum rtl_status {
+	RTL_STATUS_INTERFACE_START = 0,
+};
+
+enum hardware_type {
+	HARDWARE_TYPE_RTL8192E,
+	HARDWARE_TYPE_RTL8192U,
+	HARDWARE_TYPE_RTL8192SE,
+	HARDWARE_TYPE_RTL8192SU,
+	HARDWARE_TYPE_RTL8192CE,
+	HARDWARE_TYPE_RTL8192CU,
+	HARDWARE_TYPE_RTL8192DE,
+	HARDWARE_TYPE_RTL8192DU,
+	HARDWARE_TYPE_RTL8723AE,
+	HARDWARE_TYPE_RTL8188EE,
+	HARDWARE_TYPE_RTL8723BE,
+	HARDWARE_TYPE_RTL8192EE,
+	HARDWARE_TYPE_RTL8821AE,
+	HARDWARE_TYPE_RTL8812AE,
+	/* keep it last */
+	HARDWARE_TYPE_NUM
+};
+
+enum scan_operation_backup_opt {
+	SCAN_OPT_BACKUP_BAND0 = 0,
+	SCAN_OPT_BACKUP_BAND1,
+	SCAN_OPT_RESTORE,
+	SCAN_OPT_MAX
+};
+
+/*RF state.*/
+enum rf_pwrstate {
+	ERFON,
+	ERFSLEEP,
+	ERFOFF
+};
+
+struct bb_reg_def {
+	u32 rfintfs;
+	u32 rfintfi;
+	u32 rfintfo;
+	u32 rfintfe;
+	u32 rf3wire_offset;
+	u32 rflssi_select;
+	u32 rftxgain_stage;
+	u32 rfhssi_para1;
+	u32 rfhssi_para2;
+	u32 rfswitch_control;
+	u32 rfagc_control1;
+	u32 rfagc_control2;
+	u32 rfrxiq_imbalance;
+	u32 rfrx_afe;
+	u32 rftxiq_imbalance;
+	u32 rftx_afe;
+	u32 rflssi_readback;
+	u32 rflssi_readbackpi;
+};
+
+enum io_type {
+	IO_CMD_PAUSE_BAND0_DM_BY_SCAN = 0,
+	IO_CMD_PAUSE_BAND1_DM_BY_SCAN = 1,
+	IO_CMD_RESUME_DM_BY_SCAN = 2,
+};
+
+enum hw_variables {
+	HW_VAR_ETHER_ADDR,
+	HW_VAR_MULTICAST_REG,
+	HW_VAR_BASIC_RATE,
+	HW_VAR_BSSID,
+	HW_VAR_MEDIA_STATUS,
+	HW_VAR_SECURITY_CONF,
+	HW_VAR_BEACON_INTERVAL,
+	HW_VAR_ATIM_WINDOW,
+	HW_VAR_LISTEN_INTERVAL,
+	HW_VAR_CS_COUNTER,
+	HW_VAR_DEFAULTKEY0,
+	HW_VAR_DEFAULTKEY1,
+	HW_VAR_DEFAULTKEY2,
+	HW_VAR_DEFAULTKEY3,
+	HW_VAR_SIFS,
+	HW_VAR_R2T_SIFS,
+	HW_VAR_DIFS,
+	HW_VAR_EIFS,
+	HW_VAR_SLOT_TIME,
+	HW_VAR_ACK_PREAMBLE,
+	HW_VAR_CW_CONFIG,
+	HW_VAR_CW_VALUES,
+	HW_VAR_RATE_FALLBACK_CONTROL,
+	HW_VAR_CONTENTION_WINDOW,
+	HW_VAR_RETRY_COUNT,
+	HW_VAR_TR_SWITCH,
+	HW_VAR_COMMAND,
+	HW_VAR_WPA_CONFIG,
+	HW_VAR_AMPDU_MIN_SPACE,
+	HW_VAR_SHORTGI_DENSITY,
+	HW_VAR_AMPDU_FACTOR,
+	HW_VAR_MCS_RATE_AVAILABLE,
+	HW_VAR_AC_PARAM,
+	HW_VAR_ACM_CTRL,
+	HW_VAR_DIS_Req_Qsize,
+	HW_VAR_CCX_CHNL_LOAD,
+	HW_VAR_CCX_NOISE_HISTOGRAM,
+	HW_VAR_CCX_CLM_NHM,
+	HW_VAR_TxOPLimit,
+	HW_VAR_TURBO_MODE,
+	HW_VAR_RF_STATE,
+	HW_VAR_RF_OFF_BY_HW,
+	HW_VAR_BUS_SPEED,
+	HW_VAR_SET_DEV_POWER,
+
+	HW_VAR_RCR,
+	HW_VAR_RATR_0,
+	HW_VAR_RRSR,
+	HW_VAR_CPU_RST,
+	HW_VAR_CECHK_BSSID,
+	HW_VAR_LBK_MODE,
+	HW_VAR_AES_11N_FIX,
+	HW_VAR_USB_RX_AGGR,
+	HW_VAR_USER_CONTROL_TURBO_MODE,
+	HW_VAR_RETRY_LIMIT,
+	HW_VAR_INIT_TX_RATE,
+	HW_VAR_TX_RATE_REG,
+	HW_VAR_EFUSE_USAGE,
+	HW_VAR_EFUSE_BYTES,
+	HW_VAR_AUTOLOAD_STATUS,
+	HW_VAR_RF_2R_DISABLE,
+	HW_VAR_SET_RPWM,
+	HW_VAR_H2C_FW_PWRMODE,
+	HW_VAR_H2C_FW_JOINBSSRPT,
+	HW_VAR_H2C_FW_MEDIASTATUSRPT,
+	HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
+	HW_VAR_FW_PSMODE_STATUS,
+	HW_VAR_INIT_RTS_RATE,
+	HW_VAR_RESUME_CLK_ON,
+	HW_VAR_FW_LPS_ACTION,
+	HW_VAR_1X1_RECV_COMBINE,
+	HW_VAR_STOP_SEND_BEACON,
+	HW_VAR_TSF_TIMER,
+	HW_VAR_IO_CMD,
+
+	HW_VAR_RF_RECOVERY,
+	HW_VAR_H2C_FW_UPDATE_GTK,
+	HW_VAR_WF_MASK,
+	HW_VAR_WF_CRC,
+	HW_VAR_WF_IS_MAC_ADDR,
+	HW_VAR_H2C_FW_OFFLOAD,
+	HW_VAR_RESET_WFCRC,
+
+	HW_VAR_HANDLE_FW_C2H,
+	HW_VAR_DL_FW_RSVD_PAGE,
+	HW_VAR_AID,
+	HW_VAR_HW_SEQ_ENABLE,
+	HW_VAR_CORRECT_TSF,
+	HW_VAR_BCN_VALID,
+	HW_VAR_FWLPS_RF_ON,
+	HW_VAR_DUAL_TSF_RST,
+	HW_VAR_SWITCH_EPHY_WoWLAN,
+	HW_VAR_INT_MIGRATION,
+	HW_VAR_INT_AC,
+	HW_VAR_RF_TIMING,
+
+	HAL_DEF_WOWLAN,
+	HW_VAR_MRC,
+	HW_VAR_KEEP_ALIVE,
+	HW_VAR_NAV_UPPER,
+};
+
+enum rt_media_status {
+	RT_MEDIA_DISCONNECT = 0,
+	RT_MEDIA_CONNECT = 1
+};
+
+enum rt_oem_id {
+	RT_CID_DEFAULT = 0,
+	RT_CID_8187_ALPHA0 = 1,
+	RT_CID_8187_SERCOMM_PS = 2,
+	RT_CID_8187_HW_LED = 3,
+	RT_CID_8187_NETGEAR = 4,
+	RT_CID_WHQL = 5,
+	RT_CID_819x_CAMEO = 6,
+	RT_CID_819x_RUNTOP = 7,
+	RT_CID_819x_Senao = 8,
+	RT_CID_TOSHIBA = 9,
+	RT_CID_819x_Netcore = 10,
+	RT_CID_Nettronix = 11,
+	RT_CID_DLINK = 12,
+	RT_CID_PRONET = 13,
+	RT_CID_COREGA = 14,
+	RT_CID_819x_ALPHA = 15,
+	RT_CID_819x_Sitecom = 16,
+	RT_CID_CCX = 17,
+	RT_CID_819x_Lenovo = 18,
+	RT_CID_819x_QMI = 19,
+	RT_CID_819x_Edimax_Belkin = 20,
+	RT_CID_819x_Sercomm_Belkin = 21,
+	RT_CID_819x_CAMEO1 = 22,
+	RT_CID_819x_MSI = 23,
+	RT_CID_819x_Acer = 24,
+	RT_CID_819x_HP = 27,
+	RT_CID_819x_CLEVO = 28,
+	RT_CID_819x_Arcadyan_Belkin = 29,
+	RT_CID_819x_SAMSUNG = 30,
+	RT_CID_819x_WNC_COREGA = 31,
+	RT_CID_819x_Foxcoon = 32,
+	RT_CID_819x_DELL = 33,
+	RT_CID_819x_PRONETS = 34,
+	RT_CID_819x_Edimax_ASUS = 35,
+	RT_CID_NETGEAR = 36,
+	RT_CID_PLANEX = 37,
+	RT_CID_CC_C = 38,
+};
+
+enum hw_descs {
+	HW_DESC_OWN,
+	HW_DESC_RXOWN,
+	HW_DESC_TX_NEXTDESC_ADDR,
+	HW_DESC_TXBUFF_ADDR,
+	HW_DESC_RXBUFF_ADDR,
+	HW_DESC_RXPKT_LEN,
+	HW_DESC_RXERO,
+	HW_DESC_RX_PREPARE,
+};
+
+enum prime_sc {
+	PRIME_CHNL_OFFSET_DONT_CARE = 0,
+	PRIME_CHNL_OFFSET_LOWER = 1,
+	PRIME_CHNL_OFFSET_UPPER = 2,
+};
+
+enum rf_type {
+	RF_1T1R = 0,
+	RF_1T2R = 1,
+	RF_2T2R = 2,
+	RF_2T2R_GREEN = 3,
+};
+
+enum ht_channel_width {
+	HT_CHANNEL_WIDTH_20 = 0,
+	HT_CHANNEL_WIDTH_20_40 = 1,
+	HT_CHANNEL_WIDTH_80 = 2,
+};
+
+/* Ref: 802.11i sepc D10.0 7.3.2.25.1
+Cipher Suites Encryption Algorithms */
+enum rt_enc_alg {
+	NO_ENCRYPTION = 0,
+	WEP40_ENCRYPTION = 1,
+	TKIP_ENCRYPTION = 2,
+	RSERVED_ENCRYPTION = 3,
+	AESCCMP_ENCRYPTION = 4,
+	WEP104_ENCRYPTION = 5,
+	AESCMAC_ENCRYPTION = 6,	/*IEEE802.11w */
+};
+
+enum rtl_hal_state {
+	_HAL_STATE_STOP = 0,
+	_HAL_STATE_START = 1,
+};
+
+enum rtl_var_map {
+	/*reg map */
+	SYS_ISO_CTRL = 0,
+	SYS_FUNC_EN,
+	SYS_CLK,
+	MAC_RCR_AM,
+	MAC_RCR_AB,
+	MAC_RCR_ACRC32,
+	MAC_RCR_ACF,
+	MAC_RCR_AAP,
+	MAC_HIMR,
+	MAC_HIMRE,
+	MAC_HSISR,
+
+	/*efuse map */
+	EFUSE_TEST,
+	EFUSE_CTRL,
+	EFUSE_CLK,
+	EFUSE_CLK_CTRL,
+	EFUSE_PWC_EV12V,
+	EFUSE_FEN_ELDR,
+	EFUSE_LOADER_CLK_EN,
+	EFUSE_ANA8M,
+	EFUSE_HWSET_MAX_SIZE,
+	EFUSE_MAX_SECTION_MAP,
+	EFUSE_REAL_CONTENT_SIZE,
+	EFUSE_OOB_PROTECT_BYTES_LEN,
+	EFUSE_ACCESS,
+	/*CAM map */
+	RWCAM,
+	WCAMI,
+	RCAMO,
+	CAMDBG,
+	SECR,
+	SEC_CAM_NONE,
+	SEC_CAM_WEP40,
+	SEC_CAM_TKIP,
+	SEC_CAM_AES,
+	SEC_CAM_WEP104,
+
+	/*IMR map */
+	RTL_IMR_BCNDMAINT6,	/*Beacon DMA Interrupt 6 */
+	RTL_IMR_BCNDMAINT5,	/*Beacon DMA Interrupt 5 */
+	RTL_IMR_BCNDMAINT4,	/*Beacon DMA Interrupt 4 */
+	RTL_IMR_BCNDMAINT3,	/*Beacon DMA Interrupt 3 */
+	RTL_IMR_BCNDMAINT2,	/*Beacon DMA Interrupt 2 */
+	RTL_IMR_BCNDMAINT1,	/*Beacon DMA Interrupt 1 */
+	RTL_IMR_BCNDOK8,	/*Beacon Queue DMA OK Interrup 8 */
+	RTL_IMR_BCNDOK7,	/*Beacon Queue DMA OK Interrup 7 */
+	RTL_IMR_BCNDOK6,	/*Beacon Queue DMA OK Interrup 6 */
+	RTL_IMR_BCNDOK5,	/*Beacon Queue DMA OK Interrup 5 */
+	RTL_IMR_BCNDOK4,	/*Beacon Queue DMA OK Interrup 4 */
+	RTL_IMR_BCNDOK3,	/*Beacon Queue DMA OK Interrup 3 */
+	RTL_IMR_BCNDOK2,	/*Beacon Queue DMA OK Interrup 2 */
+	RTL_IMR_BCNDOK1,	/*Beacon Queue DMA OK Interrup 1 */
+	RTL_IMR_TIMEOUT2,	/*Timeout interrupt 2 */
+	RTL_IMR_TIMEOUT1,	/*Timeout interrupt 1 */
+	RTL_IMR_TXFOVW,		/*Transmit FIFO Overflow */
+	RTL_IMR_PSTIMEOUT,	/*Power save time out interrupt */
+	RTL_IMR_BcnInt,		/*Beacon DMA Interrupt 0 */
+	RTL_IMR_RXFOVW,		/*Receive FIFO Overflow */
+	RTL_IMR_RDU,		/*Receive Descriptor Unavailable */
+	RTL_IMR_ATIMEND,	/*For 92C,ATIM Window End Interrupt */
+	RTL_IMR_BDOK,		/*Beacon Queue DMA OK Interrup */
+	RTL_IMR_HIGHDOK,	/*High Queue DMA OK Interrupt */
+	RTL_IMR_COMDOK,		/*Command Queue DMA OK Interrupt*/
+	RTL_IMR_TBDOK,		/*Transmit Beacon OK interrup */
+	RTL_IMR_MGNTDOK,	/*Management Queue DMA OK Interrupt */
+	RTL_IMR_TBDER,		/*For 92C,Transmit Beacon Error Interrupt */
+	RTL_IMR_BKDOK,		/*AC_BK DMA OK Interrupt */
+	RTL_IMR_BEDOK,		/*AC_BE DMA OK Interrupt */
+	RTL_IMR_VIDOK,		/*AC_VI DMA OK Interrupt */
+	RTL_IMR_VODOK,		/*AC_VO DMA Interrupt */
+	RTL_IMR_ROK,		/*Receive DMA OK Interrupt */
+	RTL_IMR_HSISR_IND,  /*HSISR Interrupt*/
+	RTL_IBSS_INT_MASKS,	/*(RTL_IMR_BcnInt | RTL_IMR_TBDOK |
+				 * RTL_IMR_TBDER) */
+	RTL_IMR_C2HCMD,		/*fw interrupt*/
+
+	/*CCK Rates, TxHT = 0 */
+	RTL_RC_CCK_RATE1M,
+	RTL_RC_CCK_RATE2M,
+	RTL_RC_CCK_RATE5_5M,
+	RTL_RC_CCK_RATE11M,
+
+	/*OFDM Rates, TxHT = 0 */
+	RTL_RC_OFDM_RATE6M,
+	RTL_RC_OFDM_RATE9M,
+	RTL_RC_OFDM_RATE12M,
+	RTL_RC_OFDM_RATE18M,
+	RTL_RC_OFDM_RATE24M,
+	RTL_RC_OFDM_RATE36M,
+	RTL_RC_OFDM_RATE48M,
+	RTL_RC_OFDM_RATE54M,
+
+	RTL_RC_HT_RATEMCS7,
+	RTL_RC_HT_RATEMCS15,
+
+	RTL_RC_VHT_RATE_1SS_MCS7,
+	RTL_RC_VHT_RATE_1SS_MCS8,
+	RTL_RC_VHT_RATE_1SS_MCS9,
+	RTL_RC_VHT_RATE_2SS_MCS7,
+	RTL_RC_VHT_RATE_2SS_MCS8,
+	RTL_RC_VHT_RATE_2SS_MCS9,
+
+	/*keep it last */
+	RTL_VAR_MAP_MAX,
+};
+
+/*Firmware PS mode for control LPS.*/
+enum _fw_ps_mode {
+	FW_PS_ACTIVE_MODE = 0,
+	FW_PS_MIN_MODE = 1,
+	FW_PS_MAX_MODE = 2,
+	FW_PS_DTIM_MODE = 3,
+	FW_PS_VOIP_MODE = 4,
+	FW_PS_UAPSD_WMM_MODE = 5,
+	FW_PS_UAPSD_MODE = 6,
+	FW_PS_IBSS_MODE = 7,
+	FW_PS_WWLAN_MODE = 8,
+	FW_PS_PM_Radio_Off = 9,
+	FW_PS_PM_Card_Disable = 10,
+};
+
+enum rt_psmode {
+	EACTIVE,		/*Active/Continuous access. */
+	EMAXPS,			/*Max power save mode. */
+	EFASTPS,		/*Fast power save mode. */
+	EAUTOPS,		/*Auto power save mode. */
+};
+
+/*LED related.*/
+enum led_ctl_mode {
+	LED_CTL_POWER_ON = 1,
+	LED_CTL_LINK = 2,
+	LED_CTL_NO_LINK = 3,
+	LED_CTL_TX = 4,
+	LED_CTL_RX = 5,
+	LED_CTL_SITE_SURVEY = 6,
+	LED_CTL_POWER_OFF = 7,
+	LED_CTL_START_TO_LINK = 8,
+	LED_CTL_START_WPS = 9,
+	LED_CTL_STOP_WPS = 10,
+};
+
+enum rtl_led_pin {
+	LED_PIN_GPIO0,
+	LED_PIN_LED0,
+	LED_PIN_LED1,
+	LED_PIN_LED2
+};
+
+/*QoS related.*/
+/*acm implementation method.*/
+enum acm_method {
+	eAcmWay0_SwAndHw = 0,
+	eAcmWay1_HW = 1,
+	eAcmWay2_SW = 2,
+};
+
+enum macphy_mode {
+	SINGLEMAC_SINGLEPHY = 0,
+	DUALMAC_DUALPHY,
+	DUALMAC_SINGLEPHY,
+};
+
+enum band_type {
+	BAND_ON_2_4G = 0,
+	BAND_ON_5G,
+	BAND_ON_BOTH,
+	BANDMAX
+};
+
+/*aci/aifsn Field.
+Ref: WMM spec 2.2.2: WME Parameter Element, p.12.*/
+union aci_aifsn {
+	u8 char_data;
+
+	struct {
+		u8 aifsn:4;
+		u8 acm:1;
+		u8 aci:2;
+		u8 reserved:1;
+	} f;			/* Field */
+};
+
+/*mlme related.*/
+enum wireless_mode {
+	WIRELESS_MODE_UNKNOWN = 0x00,
+	WIRELESS_MODE_A = 0x01,
+	WIRELESS_MODE_B = 0x02,
+	WIRELESS_MODE_G = 0x04,
+	WIRELESS_MODE_AUTO = 0x08,
+	WIRELESS_MODE_N_24G = 0x10,
+	WIRELESS_MODE_N_5G = 0x20,
+	WIRELESS_MODE_AC_5G = 0x40,
+	WIRELESS_MODE_AC_24G = 0x80,
+	WIRELESS_MODE_AC_ONLY = 0x100,
+	WIRELESS_MODE_MAX = 0x800
+};
+
+enum ratr_table_mode {
+	RATR_INX_WIRELESS_NGB = 0,
+	RATR_INX_WIRELESS_NG = 1,
+	RATR_INX_WIRELESS_NB = 2,
+	RATR_INX_WIRELESS_N = 3,
+	RATR_INX_WIRELESS_GB = 4,
+	RATR_INX_WIRELESS_G = 5,
+	RATR_INX_WIRELESS_B = 6,
+	RATR_INX_WIRELESS_MC = 7,
+	RATR_INX_WIRELESS_AC_5N = 8,
+	RATR_INX_WIRELESS_AC_24N = 9,
+};
+
+enum rtl_link_state {
+	MAC80211_NOLINK = 0,
+	MAC80211_LINKING = 1,
+	MAC80211_LINKED = 2,
+	MAC80211_LINKED_SCANNING = 3,
+};
+
+enum act_category {
+	ACT_CAT_QOS = 1,
+	ACT_CAT_DLS = 2,
+	ACT_CAT_BA = 3,
+	ACT_CAT_HT = 7,
+	ACT_CAT_WMM = 17,
+};
+
+enum ba_action {
+	ACT_ADDBAREQ = 0,
+	ACT_ADDBARSP = 1,
+	ACT_DELBA = 2,
+};
+
+enum rt_polarity_ctl {
+	RT_POLARITY_LOW_ACT = 0,
+	RT_POLARITY_HIGH_ACT = 1,
+};
+
+/* After 8188E, we use V2 reason define. 88C/8723A use V1 reason. */
+enum fw_wow_reason_v2 {
+	FW_WOW_V2_PTK_UPDATE_EVENT = 0x01,
+	FW_WOW_V2_GTK_UPDATE_EVENT = 0x02,
+	FW_WOW_V2_DISASSOC_EVENT = 0x04,
+	FW_WOW_V2_DEAUTH_EVENT = 0x08,
+	FW_WOW_V2_FW_DISCONNECT_EVENT = 0x10,
+	FW_WOW_V2_MAGIC_PKT_EVENT = 0x21,
+	FW_WOW_V2_UNICAST_PKT_EVENT = 0x22,
+	FW_WOW_V2_PATTERN_PKT_EVENT = 0x23,
+	FW_WOW_V2_RTD3_SSID_MATCH_EVENT = 0x24,
+	FW_WOW_V2_REALWOW_V2_WAKEUPPKT = 0x30,
+	FW_WOW_V2_REALWOW_V2_ACKLOST = 0x31,
+	FW_WOW_V2_REASON_MAX = 0xff,
+};
+
+enum wolpattern_type {
+	UNICAST_PATTERN = 0,
+	MULTICAST_PATTERN = 1,
+	BROADCAST_PATTERN = 2,
+	DONT_CARE_DA = 3,
+	UNKNOWN_TYPE = 4,
+};
+
+struct octet_string {
+	u8 *octet;
+	u16 length;
+};
+
+struct rtl_hdr_3addr {
+	__le16 frame_ctl;
+	__le16 duration_id;
+	u8 addr1[ETH_ALEN];
+	u8 addr2[ETH_ALEN];
+	u8 addr3[ETH_ALEN];
+	__le16 seq_ctl;
+	u8 payload[0];
+} __packed;
+
+struct rtl_info_element {
+	u8 id;
+	u8 len;
+	u8 data[0];
+} __packed;
+
+struct rtl_probe_rsp {
+	struct rtl_hdr_3addr header;
+	u32 time_stamp[2];
+	__le16 beacon_interval;
+	__le16 capability;
+	/*SSID, supported rates, FH params, DS params,
+	   CF params, IBSS params, TIM (if beacon), RSN */
+	struct rtl_info_element info_element[0];
+} __packed;
+
+/*LED related.*/
+/*ledpin Identify how to implement this SW led.*/
+struct rtl_led {
+	void *hw;
+	enum rtl_led_pin ledpin;
+	bool b_ledon;
+};
+
+struct rtl_led_ctl {
+	bool bled_opendrain;
+	struct rtl_led sw_led0;
+	struct rtl_led sw_led1;
+};
+
+struct rtl_qos_parameters {
+	__le16 cw_min;
+	__le16 cw_max;
+	u8 aifs;
+	u8 flag;
+	__le16 tx_op;
+} __packed;
+
+struct rt_smooth_data {
+	u32 elements[100];	/*array to store values */
+	u32 index;		/*index to current array to store */
+	u32 total_num;		/*num of valid elements */
+	u32 total_val;		/*sum of valid elements */
+};
+
+struct rtl_ht_agg {
+	u16 txq_id;
+	u16 wait_for_ba;
+	u16 start_idx;
+	u64 bitmap;
+	u32 rate_n_flags;
+	u8 agg_state;
+	u8 rx_agg_state;
+};
+
+struct rtl_tid_data {
+	u16 seq_number;
+	struct rtl_ht_agg agg;
+};
+
+struct rssi_sta {
+	long undecorated_smoothed_pwdb;
+};
+
+struct rtl_sta_info {
+	struct list_head list;
+	u8 ratr_index;
+	u8 wireless_mode;
+	u8 mimo_ps;
+	u8 mac_addr[6];
+	struct rtl_tid_data tids[MAX_TID_COUNT];
+
+	/* just used for ap adhoc or mesh*/
+	struct rssi_sta rssi_stat;
+} __packed;
+
+struct false_alarm_statistics {
+	u32 cnt_parity_fail;
+	u32 cnt_rate_illegal;
+	u32 cnt_crc8_fail;
+	u32 cnt_mcs_fail;
+	u32 cnt_fast_fsync_fail;
+	u32 cnt_sb_search_fail;
+	u32 cnt_ofdm_fail;
+	u32 cnt_cck_fail;
+	u32 cnt_all;
+	u32 cnt_ofdm_cca;
+	u32 cnt_cck_cca;
+	u32 cnt_cca_all;
+	u32 cnt_bw_usc;
+	u32 cnt_bw_lsc;
+};
+
+struct init_gain {
+	u8 xaagccore1;
+	u8 xbagccore1;
+	u8 xcagccore1;
+	u8 xdagccore1;
+	u8 cca;
+
+};
+
+struct wireless_stats {
+	unsigned long txbytesunicast;
+	unsigned long txbytesmulticast;
+	unsigned long txbytesbroadcast;
+	unsigned long rxbytesunicast;
+
+	long rx_snr_db[4];
+	/*Correct smoothed ss in Dbm, only used
+	   in driver to report real power now. */
+	long recv_signal_power;
+	long signal_quality;
+	long last_sigstrength_inpercent;
+
+	u32 rssi_calculate_cnt;
+	u32 pwdb_all_cnt;
+
+	/*Transformed, in dbm. Beautified signal
+	   strength for UI, not correct. */
+	long signal_strength;
+
+	u8 rx_rssi_percentage[4];
+	u8 rx_evm_dbm[4];
+	u8 rx_evm_percentage[2];
+
+	u16 rx_cfo_short[4];
+	u16 rx_cfo_tail[4];
+
+	struct rt_smooth_data ui_rssi;
+	struct rt_smooth_data ui_link_quality;
+};
+
+struct rate_adaptive {
+	u8 rate_adaptive_disabled;
+	u8 ratr_state;
+	u16 reserve;
+
+	u32 high_rssi_thresh_for_ra;
+	u32 high2low_rssi_thresh_for_ra;
+	u8 low2high_rssi_thresh_for_ra;
+	u32 low_rssi_thresh_for_ra;
+	u32 upper_rssi_threshold_ratr;
+	u32 middleupper_rssi_threshold_ratr;
+	u32 middle_rssi_threshold_ratr;
+	u32 middlelow_rssi_threshold_ratr;
+	u32 low_rssi_threshold_ratr;
+	u32 ultralow_rssi_threshold_ratr;
+	u32 low_rssi_threshold_ratr_40m;
+	u32 low_rssi_threshold_ratr_20m;
+	u8 ping_rssi_enable;
+	u32 ping_rssi_ratr;
+	u32 ping_rssi_thresh_for_ra;
+	u32 last_ratr;
+	u8 pre_ratr_state;
+	u8 ldpc_thres;
+	bool use_ldpc;
+	bool lower_rts_rate;
+	bool is_special_data;
+};
+
+struct regd_pair_mapping {
+	u16 reg_dmnenum;
+	u16 reg_5ghz_ctl;
+	u16 reg_2ghz_ctl;
+};
+
+struct dynamic_primary_cca {
+	u8 pricca_flag;
+	u8 intf_flag;
+	u8 intf_type;
+	u8 dup_rts_flag;
+	u8 monitor_flag;
+	u8 ch_offset;
+	u8 mf_state;
+};
+
+struct rtl_regulatory {
+	char alpha2[2];
+	u16 country_code;
+	u16 max_power_level;
+	u32 tp_scale;
+	u16 current_rd;
+	u16 current_rd_ext;
+	int16_t power_limit;
+	struct regd_pair_mapping *regpair;
+};
+
+struct rtl_rfkill {
+	bool rfkill_state;	/*0 is off, 1 is on */
+};
+
+/*for P2P PS**/
+#define	P2P_MAX_NOA_NUM		2
+
+enum p2p_role {
+	P2P_ROLE_DISABLE = 0,
+	P2P_ROLE_DEVICE = 1,
+	P2P_ROLE_CLIENT = 2,
+	P2P_ROLE_GO = 3
+};
+
+enum p2p_ps_state {
+	P2P_PS_DISABLE = 0,
+	P2P_PS_ENABLE = 1,
+	P2P_PS_SCAN = 2,
+	P2P_PS_SCAN_DONE = 3,
+	P2P_PS_ALLSTASLEEP = 4, /* for P2P GO */
+};
+
+enum p2p_ps_mode {
+	P2P_PS_NONE = 0,
+	P2P_PS_CTWINDOW = 1,
+	P2P_PS_NOA = 2,
+	P2P_PS_MIX = 3, /* CTWindow and NoA*/
+};
+
+struct rtl_p2p_ps_info {
+	enum p2p_ps_mode p2p_ps_mode; /* indicate p2p ps mode */
+	enum p2p_ps_state p2p_ps_state; /* indicate p2p ps state */
+	u8 noa_index; /* Identifies and instance of Notice of Absence timing. */
+	/* Client traffic window. A period of time in TU after TBTT. */
+	u8 ctwindow;
+	u8 opp_ps; /* opportunistic power save. */
+	u8 noa_num; /* number of NoA descriptor in P2P IE. */
+	/* Count for owner, Type of client. */
+	u8 noa_count_type[P2P_MAX_NOA_NUM];
+	/* Max duration for owner, preferred or
+	 * min acceptable duration for client. */
+	u32 noa_duration[P2P_MAX_NOA_NUM];
+	/* Length of interval for owner, preferred or
+	 * max acceptable interval of client. */
+	u32 noa_interval[P2P_MAX_NOA_NUM];
+	/* schedule expressed in terms of the lower 4 bytes of the TSF timer. */
+	u32 noa_start_time[P2P_MAX_NOA_NUM];
+};
+
+struct p2p_ps_offload_t {
+	u8 Offload_En:1;
+	u8 role:1; /* 1: Owner, 0: Client */
+	u8 CTWindow_En:1;
+	u8 NoA0_En:1;
+	u8 NoA1_En:1;
+	u8 AllStaSleep:1;
+	u8 discovery:1;
+	u8 reserved:1;
+};
+
+#define IQK_MATRIX_REG_NUM	8
+/* Channels_2_4G_NUM + Channels_5G_20M_NUM + Channels_5G */
+#define IQK_MATRIX_SETTINGS_NUM	(14 + 24 + 21)
+struct iqk_matrix_regs {
+	bool b_iqk_done;
+	long value[1][IQK_MATRIX_REG_NUM];
+};
+
+struct rtl_phy {
+	struct bb_reg_def phyreg_def[4];	/*Radio A/B/C/D */
+	struct init_gain initgain_backup;
+	enum io_type current_io_type;
+
+	u8 rf_mode;
+	u8 rf_type;
+	u8 current_chan_bw;
+	u8 set_bwmode_inprogress;
+	u8 sw_chnl_inprogress;
+	u8 sw_chnl_stage;
+	u8 sw_chnl_step;
+	u8 current_channel;
+	u8 h2c_box_num;
+	u8 set_io_inprogress;
+	u8 lck_inprogress;
+	bool iqk_inprogress;
+
+	/* record for power tracking */
+	s32 reg_e94;
+	s32 reg_e9c;
+	s32 reg_ea4;
+	s32 reg_eac;
+	s32 reg_eb4;
+	s32 reg_ebc;
+	s32 reg_ec4;
+	s32 reg_ecc;
+	u8 rfpienable;
+	u8 reserve_0;
+	u16 reserve_1;
+	u32 reg_c04, reg_c08, reg_874;
+	u32 adda_backup[16];
+	u32 iqk_mac_backup[IQK_MAC_REG_NUM];
+	u32 iqk_bb_backup[10];
+	bool iqk_initialized;
+
+	bool rfpath_rx_enable[MAX_RF_PATH];
+	/*Jaguar*/
+	u8 reg_837;
+	/* Dul mac */
+	bool b_need_iqk;
+	struct iqk_matrix_regs iqk_matrix_regsetting[IQK_MATRIX_SETTINGS_NUM];
+
+	bool b_rfpi_enable;
+
+	u8 pwrgroup_cnt;
+	u8 bcck_high_power;
+	/* this is for 88E & 8723A */
+	u32 mcs_txpwrlevel_origoffset[MAX_PG_GROUP][16];
+	/* this is for 92EE */
+	u32 tx_power_by_rate_offset[TX_PWR_BY_RATE_NUM_BAND]
+				   [TX_PWR_BY_RATE_NUM_RF]
+				   [TX_PWR_BY_RATE_NUM_RF]
+				   [TX_PWR_BY_RATE_NUM_SECTION];
+	u8 txpwr_by_rate_base_24g[TX_PWR_BY_RATE_NUM_RF]
+				 [TX_PWR_BY_RATE_NUM_RF]
+				 [MAX_BASE_NUM_IN_PHY_REG_PG_24G];
+
+	u8 txpwr_by_rate_base_5g[TX_PWR_BY_RATE_NUM_RF]
+				[TX_PWR_BY_RATE_NUM_RF]
+				[MAX_BASE_NUM_IN_PHY_REG_PG_5G];
+	u8 default_initialgain[4];
+
+	/* the current Tx power level */
+	u8 cur_cck_txpwridx;
+	u8 cur_ofdm24g_txpwridx;
+	u8 cur_bw20_txpwridx;
+	u8 cur_bw40_txpwridx;
+
+	char txpwr_limit_2_4g[MAX_REGULATION_NUM]
+				[MAX_2_4G_BANDWITH_NUM]
+				[MAX_RATE_SECTION_NUM]
+				[CHANNEL_MAX_NUMBER_2G]
+				[MAX_RF_PATH_NUM];
+	char txpwr_limit_5g[MAX_REGULATION_NUM]
+			   [MAX_5G_BANDWITH_NUM]
+			   [MAX_RATE_SECTION_NUM]
+			   [CHANNEL_MAX_NUMBER_5G]
+			   [MAX_RF_PATH_NUM];
+
+	u32 rfreg_chnlval[2];
+	bool b_apk_done;
+	u32 reg_rf3c[2];	/* pathA / pathB  */
+
+	u32 backup_rf_0x1a;/*92ee*/
+	/* bfsync */
+	u8 framesync;
+	u32 framesync_c34;
+
+	u8 num_total_rfpath;
+	u16 rf_pathmap;
+
+	enum rt_polarity_ctl polarity_ctl;
+};
+
+#define RTL_AGG_STOP						0
+#define RTL_AGG_PROGRESS					1
+#define RTL_AGG_START						2
+#define RTL_AGG_OPERATIONAL					3
+#define RTL_RX_AGG_START					1
+#define RTL_RX_AGG_STOP						0
+
+struct rtl_priv;
+struct rtl_io {
+	struct device *dev;
+
+	/*PCI MEM map */
+	unsigned long pci_mem_end;	/*shared mem end        */
+	unsigned long pci_mem_start;	/*shared mem start */
+
+	/*PCI IO map */
+	unsigned long pci_base_addr;	/*device I/O address */
+
+	void (*write8_async)(struct rtl_priv *rtlpriv, u32 addr, u8 val);
+	void (*write16_async)(struct rtl_priv *rtlpriv, u32 addr, u16 val);
+	void (*write32_async)(struct rtl_priv *rtlpriv, u32 addr, u32 val);
+
+	u8 (*read8_sync)(struct rtl_priv *rtlpriv, u32 addr);
+	u16 (*read16_sync)(struct rtl_priv *rtlpriv, u32 addr);
+	u32 (*read32_sync)(struct rtl_priv *rtlpriv, u32 addr);
+
+};
+
+struct rtl_mac {
+	u8 mac_addr[ETH_ALEN];
+	u8 mac80211_registered;
+	u8 beacon_enabled;
+
+	u32 tx_ss_num;
+	u32 rx_ss_num;
+
+	struct ieee80211_supported_band bands[IEEE80211_NUM_BANDS];
+	struct ieee80211_hw *hw;
+	struct ieee80211_vif *vif;
+	enum nl80211_iftype opmode;
+
+	/*Probe Beacon management */
+	enum rtl_link_state link_state;
+
+	int n_channels;
+	int n_bitrates;
+
+	bool offchan_deley;
+	u8 p2p;	/*using p2p role*/
+	bool p2p_in_use;
+
+	/*filters */
+	u32 rx_conf;
+
+	bool act_scanning;
+	u8 cnt_after_linked;
+	bool skip_scan;
+
+	/* early mode */
+	/* skb wait queue */
+	struct sk_buff_head skb_waitq[MAX_TID_COUNT];
+
+	/*RDG*/
+	bool rdg_en;
+
+	u8 ht_stbc_cap;
+	u8 ht_cur_stbc;
+
+	/*vht support*/
+	u8 vht_enable;
+	u8 bw_80;
+	u8 vht_cur_ldpc;
+	u8 vht_cur_stbc;
+	u8 vht_stbc_cap;
+	u8 vht_ldpc_cap;
+
+	/*AP*/
+	u8 bssid[6];
+	u32 vendor;
+	u32 basic_rates; /* b/g rates */
+	u8 ht_enable;
+	u8 bw_40;
+	u8 mode;		/* wireless mode */
+	u8 slot_time;
+	u8 short_preamble;
+	u8 use_cts_protect;
+	u8 cur_40_prime_sc;
+	u8 cur_40_prime_sc_bk;
+	u8 cur_80_prime_sc;
+	u64 tsf;
+	u8 retry_short;
+	u8 retry_long;
+	u16 assoc_id;
+	bool bhiddenssid;
+
+	/*IBSS*/
+	int beacon_interval;
+
+	/*AMPDU*/
+	u8 min_space_cfg;	/*For Min spacing configurations */
+	u8 max_mss_density;
+	u8 current_ampdu_factor;
+	u8 current_ampdu_density;
+
+	/*QOS & EDCA */
+	struct ieee80211_tx_queue_params edca_param[RTL_MAC80211_NUM_QUEUE];
+	struct rtl_qos_parameters ac[AC_MAX];
+};
+
+struct rtl_hal {
+	struct ieee80211_hw *hw;
+
+	bool driver_is_goingto_unload;
+	bool up_first_time;
+	bool bfirst_init;
+	bool being_init_adapter;
+	bool b_bbrf_ready;
+	bool b_mac_func_enable;
+	bool b_pre_edcca_enable;
+
+	enum intf_type interface;
+	u16 hw_type;		/*92c or 92d or 92s and so on */
+	u8 ic_class;
+	u8 oem_id;
+	u32 version;		/*version of chip */
+	u8 state;		/*stop 0, start 1 */
+	u8 boad_type;
+
+	u8 pa_mode;
+	u8 pa_type_2g;
+	u8 pa_type_5g;
+	u8 lna_type_2g;
+	u8 lna_type_5g;
+	u8 external_pa_2g;
+	u8 external_lna_2g;
+	u8 external_pa_5g;
+	u8 external_lna_5g;
+	u8 rfe_type;
+
+	/*firmware */
+	u32 fwsize;
+	u8 *pfirmware;
+	u16 fw_version;
+	u16 fw_subversion;
+	bool b_h2c_setinprogress;
+	u8 last_hmeboxnum;
+	bool bfw_ready;
+
+	/*Reserve page start offset except beacon in TxQ. */
+	u8 fw_rsvdpage_startoffset;
+	u8 h2c_txcmd_seq;
+	u8 current_ra_rate;
+
+	/* FW Cmd IO related */
+	u16 fwcmd_iomap;
+	u32 fwcmd_ioparam;
+	bool set_fwcmd_inprogress;
+	u8 current_fwcmd_io;
+
+	bool bfw_clk_change_in_progress;
+	bool ballow_sw_to_change_hwclc;
+	u8 fw_ps_state;
+	 struct p2p_ps_offload_t p2p_ps_offload;
+	/**/
+	bool driver_going2unload;
+
+	/*AMPDU init min space*/
+	u8 minspace_cfg;	/*For Min spacing configurations */
+
+	/* Dul mac */
+	enum macphy_mode macphymode;
+	enum band_type current_bandtype;	/* 0:2.4G, 1:5G */
+	enum band_type current_bandtypebackup;
+	enum band_type bandset;
+	/* dual MAC 0--Mac0 1--Mac1 */
+	u32 interfaceindex;
+	/* just for DulMac S3S4 */
+	u8 macphyctl_reg;
+	bool b_earlymode_enable;
+	u8 max_earlymode_num;
+	/* Dul mac*/
+	bool during_mac0init_radiob;
+	bool during_mac1init_radioa;
+	bool reloadtxpowerindex;
+	/* True if IMR or IQK  have done
+	for 2.4G in scan progress */
+	bool b_load_imrandiqk_setting_for2g;
+
+	bool disable_amsdu_8k;
+	bool bmaster_of_dmsp;
+	bool bslave_of_dmsp;
+
+	u16 rx_tag;/*for 92ee*/
+	u8 rts_en;
+
+	/*for wowlan*/
+	bool wow_enable;
+	bool b_enter_pnp_sleep;
+	bool b_wake_from_pnp_sleep;
+	bool wow_enabled;
+	__kernel_time_t last_suspend_sec;
+	u32 wowlan_fwsize;
+	u8 *p_wowlan_firmware;
+
+	u8 hw_rof_enable; /*Enable GPIO[9] as WL RF HW PDn source*/
+
+	bool real_wow_v2_enable;
+	bool re_init_llt_table;
+};
+
+struct rtl_security {
+	/*default 0 */
+	bool use_sw_sec;
+
+	bool being_setkey;
+	bool use_defaultkey;
+	/*Encryption Algorithm for Unicast Packet */
+	enum rt_enc_alg pairwise_enc_algorithm;
+	/*Encryption Algorithm for Brocast/Multicast */
+	enum rt_enc_alg group_enc_algorithm;
+	/*Cam Entry Bitmap */
+	u32 hwsec_cam_bitmap;
+	u8 hwsec_cam_sta_addr[TOTAL_CAM_ENTRY][ETH_ALEN];
+	/*local Key buffer, indx 0 is for
+	   pairwise key 1-4 is for agoup key. */
+	u8 key_buf[KEY_BUF_SIZE][MAX_KEY_LEN];
+	u8 key_len[KEY_BUF_SIZE];
+
+	/*The pointer of Pairwise Key,
+	   it always points to KeyBuf[4] */
+	u8 *pairwise_key;
+};
+
+struct rtl_dig {
+	u8 dig_enable_flag;
+	u8 dig_ext_port_stage;
+
+	u32 rssi_lowthresh;
+	u32 rssi_highthresh;
+
+	u32 fa_lowthresh;
+	u32 fa_highthresh;
+
+	u8 cursta_connectstate;
+	u8 presta_connectstate;
+	u8 curmultista_connectstate;
+
+	u8 pre_igvalue;
+	u8 cur_igvalue;
+
+	char backoff_val;
+	char backoff_val_range_max;
+	char backoff_val_range_min;
+	u8 rx_gain_range_max;
+	u8 rx_gain_range_min;
+	u8 rssi_val_min;
+	u8 min_undecorated_pwdb_for_dm;
+	long last_min_undecorated_pwdb_for_dm;
+
+	u8 pre_cck_pd_state;
+	u8 cur_cck_pd_state;
+
+	u8 large_fa_hit;
+	u8 forbidden_igi;
+	u32 recover_cnt;
+
+};
+
+struct rtl_pstbl {
+	u8 pre_ccastate;
+	u8 cur_ccasate;
+
+	u8 pre_rfstate;
+	u8 cur_rfstate;
+
+	long rssi_val_min;
+
+};
+
+#define ASSOCIATE_ENTRY_NUM	(32 + 1)
+
+struct fast_ant_trainning {
+	u8 bssid[6];
+	u8 antsel_rx_keep_0;
+	u8 antsel_rx_keep_1;
+	u8 antsel_rx_keep_2;
+	u32 ant_sum_rssi[7];
+	u32 ant_rssi_cnt[7];
+	u32 ant_ave_rssi[7];
+	u8 fat_state;
+	u32 train_idx;
+	u8 antsel_a[ASSOCIATE_ENTRY_NUM];
+	u8 antsel_b[ASSOCIATE_ENTRY_NUM];
+	u8 antsel_c[ASSOCIATE_ENTRY_NUM];
+	u32 main_ant_sum[ASSOCIATE_ENTRY_NUM];
+	u32 aux_ant_sum[ASSOCIATE_ENTRY_NUM];
+	u32 main_ant_cnt[ASSOCIATE_ENTRY_NUM];
+	u32 aux_ant_cnt[ASSOCIATE_ENTRY_NUM];
+	u8 rx_idle_ant;
+	bool b_becomelinked;
+};
+
+struct dm_phy_dbg_info {
+	char rx_snrdb[4];
+	u64 num_qry_phy_status;
+	u64 num_qry_phy_status_cck;
+	u64 num_qry_phy_status_ofdm;
+	u16 num_qry_beacon_pkt;
+	u16 num_non_be_pkt;
+	s32 rx_evm[4];
+};
+
+struct rtl_dm {
+	/*PHY status for DM */
+	long entry_min_undecoratedsmoothed_pwdb;
+	long undecorated_smoothed_pwdb;	/*out dm */
+	long entry_max_undecoratedsmoothed_pwdb;
+	bool b_dm_initialgain_enable;
+	bool bdynamic_txpower_enable;
+	bool bcurrent_turbo_edca;
+	bool bis_any_nonbepkts;	/*out dm */
+	bool bis_cur_rdlstate;
+	bool btxpower_trackinginit;
+	bool b_disable_framebursting;
+	bool b_cck_inch14;
+	bool btxpower_tracking;
+	bool b_useramask;
+	bool brfpath_rxenable[4];
+	bool binform_fw_driverctrldm;
+	bool bcurrent_mrc_switch;
+	u8 txpowercount;
+
+	u8 thermalvalue_rxgain;
+	u8 thermalvalue_iqk;
+	u8 thermalvalue_lck;
+	u8 thermalvalue;
+	u8 thermalvalue_avg[AVG_THERMAL_NUM];
+	u8 thermalvalue_avg_index;
+	bool bdone_txpower;
+	u8 last_dtp_lvl;
+	u8 dynamic_txhighpower_lvl;	/*Tx high power level */
+	u8 dm_flag;	/*Indicate if each dynamic mechanism's status. */
+	u8 dm_type;
+	u8 txpower_track_control;
+	bool binterrupt_migration;
+	bool bdisable_tx_int;
+	char ofdm_index[MAX_RF_PATH];
+	char cck_index;
+	u8 default_ofdm_index;
+	u8 default_cck_index;
+	char delta_power_index[MAX_RF_PATH];
+	char delta_power_index_last[MAX_RF_PATH];
+	char power_index_offset[MAX_RF_PATH];
+	char aboslute_ofdm_swing_idx[MAX_RF_PATH];
+	char remnant_ofdm_swing_idx[MAX_RF_PATH];
+	char remnant_cck_idx;
+	bool modify_txagc_flag_path_a;
+	bool modify_txagc_flag_path_b;
+
+	bool b_one_entry_only;
+	struct dm_phy_dbg_info dbginfo;
+	/* Dynamic ATC switch */
+
+	bool atc_status;
+	bool large_cfo_hit;
+	bool is_freeze;
+	int cfo_tail[2];
+	int cfo_ave_pre;
+	int crystal_cap;
+	u8 cfo_threshold;
+	u32 packet_count;
+	u32 packet_count_pre;
+	u8 tx_rate;
+
+
+	/*88e tx power tracking*/
+	u8 bb_swing_idx_ofdm[2];
+	u8 bb_swing_idx_ofdm_current;
+	u8 bb_swing_idx_ofdm_base[MAX_RF_PATH];
+	bool bb_swing_flag_Ofdm;
+	u8 bb_swing_idx_cck;
+	u8 bb_swing_idx_cck_current;
+	u8 bb_swing_idx_cck_base;
+	bool bb_swing_flag_cck;
+
+	char bb_swing_diff_2g;
+	char bb_swing_diff_5g;
+
+	/* DMSP */
+	bool supp_phymode_switch;
+
+	/* DulMac */
+	struct rtl_dig dm_digtable;
+	struct rtl_pstbl dm_pstable;
+	struct fast_ant_trainning fat_table;
+
+	u8 linked_interval;
+
+	u64 last_tx_ok_cnt;
+	u64 last_rx_ok_cnt;
+
+	bool cck_high_power;
+};
+
+#define	EFUSE_MAX_LOGICAL_SIZE		256
+
+struct rtl_efuse {
+	bool bautoLoad_ok;
+	bool bootfromefuse;
+	u16 max_physical_size;
+
+	u8 efuse_map[2][EFUSE_MAX_LOGICAL_SIZE];
+	u16 efuse_usedbytes;
+	u8 efuse_usedpercentage;
+	u8 autoload_failflag;
+	u8 autoload_status;
+
+	short epromtype;
+	u16 eeprom_vid;
+	u16 eeprom_did;
+	u16 eeprom_svid;
+	u16 eeprom_smid;
+	u8 eeprom_oemid;
+	u16 eeprom_channelplan;
+	u8 eeprom_version;
+
+	u8 dev_addr[6];
+	u8 board_type;
+	u8 wowlan_enable;
+	u8 antenna_div_cfg;
+	u8 antenna_div_type;
+
+	bool b_txpwr_fromeprom;
+	u8 eeprom_crystalcap;
+	u8 eeprom_tssi[2];
+	u8 eeprom_tssi_5g[3][2]; /* for 5GL/5GM/5GH band. */
+	u8 eeprom_pwrlimit_ht20[CHANNEL_GROUP_MAX];
+	u8 eeprom_pwrlimit_ht40[CHANNEL_GROUP_MAX];
+	u8 eeprom_chnlarea_txpwr_cck[2][CHANNEL_GROUP_MAX_2G];
+	u8 eeprom_chnlarea_txpwr_ht40_1s[2][CHANNEL_GROUP_MAX];
+	u8 eeprom_chnlarea_txpwr_ht40_2sdiif[2][CHANNEL_GROUP_MAX];
+
+
+	u8 internal_pa_5g[2];	/* pathA / pathB */
+	u8 eeprom_c9;
+	u8 eeprom_cc;
+
+	/*For power group */
+	u8 eeprom_pwrgroup[2][3];
+	u8 pwrgroup_ht20[2][CHANNEL_MAX_NUMBER];
+	u8 pwrgroup_ht40[2][CHANNEL_MAX_NUMBER];
+
+	u8 txpwrlevel_cck[MAX_RF_PATH][CHANNEL_MAX_NUMBER_2G];
+	/*For HT 40MHZ pwr */
+	u8 txpwrlevel_ht40_1s[MAX_RF_PATH][CHANNEL_MAX_NUMBER];
+	/*For HT 40MHZ pwr */
+	u8 txpwrlevel_ht40_2s[MAX_RF_PATH][CHANNEL_MAX_NUMBER];
+
+	/*--------------------------------------------------------*
+	 * 8192CE\8192SE\8192DE\8723AE use the following 4 arrays,
+	 * other ICs (8188EE\8723BE\8192EE\8812AE...)
+	 * define new arrays in Windows code.
+	 * BUT, in linux code, we use the same array for all ICs.
+	 *
+	 * The Correspondance relation between two arrays is:
+	 * txpwr_cckdiff[][] == CCK_24G_Diff[][]
+	 * txpwr_ht20diff[][] == BW20_24G_Diff[][]
+	 * txpwr_ht40diff[][] == BW40_24G_Diff[][]
+	 * txpwr_legacyhtdiff[][] == OFDM_24G_Diff[][]
+	 *
+	 * Sizes of these arrays are decided by the larger ones.
+	 */
+	char txpwr_cckdiff[MAX_RF_PATH][CHANNEL_MAX_NUMBER];
+	char txpwr_ht20diff[MAX_RF_PATH][CHANNEL_MAX_NUMBER];
+	char txpwr_ht40diff[MAX_RF_PATH][CHANNEL_MAX_NUMBER];
+	char txpwr_legacyhtdiff[MAX_RF_PATH][CHANNEL_MAX_NUMBER];
+	/*--------------------------------------------------------*/
+
+	u8 txpwr_5g_bw40base[MAX_RF_PATH][CHANNEL_MAX_NUMBER];
+	u8 txpwr_5g_bw80base[MAX_RF_PATH][CHANNEL_MAX_NUMBER_5G_80M];
+	char txpwr_5g_ofdmdiff[MAX_RF_PATH][MAX_TX_COUNT];
+	char txpwr_5g_bw20diff[MAX_RF_PATH][MAX_TX_COUNT];
+	char txpwr_5g_bw40diff[MAX_RF_PATH][MAX_TX_COUNT];
+	char txpwr_5g_bw80diff[MAX_RF_PATH][MAX_TX_COUNT];
+
+	u8 txpwr_safetyflag;		/* Band edge enable flag */
+	u16 eeprom_txpowerdiff;
+	u8 legacy_httxpowerdiff;	/* Legacy to HT rate power diff */
+	u8 antenna_txpwdiff[3];
+
+	u8 eeprom_regulatory;
+	u8 eeprom_thermalmeter;
+	u8 thermalmeter[2];/*ThermalMeter, index 0 for RFIC0, and 1 for RFIC1 */
+	u16 tssi_13dbm;
+	u8 crystalcap;		/* CrystalCap. */
+	u8 delta_iqk;
+	u8 delta_lck;
+
+	u8 legacy_ht_txpowerdiff;	/*Legacy to HT rate power diff */
+	bool b_apk_thermalmeterignore;
+
+	bool b1x1_recvcombine;
+	bool b1ss_support;
+
+	/*channel plan */
+	u8 channel_plan;
+};
+
+struct rtl_ps_ctl {
+	bool pwrdomain_protect;
+	bool b_in_powersavemode;
+	bool rfchange_inprogress;
+	bool b_swrf_processing;
+	bool b_hwradiooff;
+	/*
+	 * just for PCIE ASPM
+	 * If it supports ASPM, Offset[560h] = 0x40,
+	 * otherwise Offset[560h] = 0x00.
+	 * */
+	bool b_support_aspm;
+	bool b_support_backdoor;
+
+	/*for LPS */
+	enum rt_psmode dot11_psmode;	/*Power save mode configured. */
+	bool b_swctrl_lps;
+	bool b_fwctrl_lps;
+	u8 fwctrl_psmode;
+	/*For Fw control LPS mode */
+	u8 b_reg_fwctrl_lps;
+	/*Record Fw PS mode status. */
+	bool b_fw_current_inpsmode;
+	u8 reg_max_lps_awakeintvl;
+	bool report_linked;
+	bool b_low_power_enable;/*for 32k*/
+
+	/*for IPS */
+	bool b_inactiveps;
+
+	u32 rfoff_reason;
+
+	/*RF OFF Level */
+	u32 cur_ps_level;
+	u32 reg_rfps_level;
+
+	/*just for PCIE ASPM */
+	u8 const_amdpci_aspm;
+
+	enum rf_pwrstate inactive_pwrstate;
+	enum rf_pwrstate rfpwr_state;	/*cur power state */
+
+	/* for SW LPS*/
+	bool sw_ps_enabled;
+	bool state;
+	bool state_inap;
+	bool multi_buffered;
+	u16 nullfunc_seq;
+	unsigned int dtim_counter;
+	unsigned int sleep_ms;
+	unsigned long last_sleep_jiffies;
+	unsigned long last_awake_jiffies;
+	unsigned long last_delaylps_stamp_jiffies;
+	unsigned long last_dtim;
+	unsigned long last_beacon;
+	unsigned long last_action;
+	unsigned long last_slept;
+
+	/*For P2P PS */
+	struct rtl_p2p_ps_info p2p_ps_info;
+	u8 pwr_mode;
+	u8 smart_ps;
+
+	/* wake up on line */
+	u8 wo_wlan_mode;
+	u8 arp_offload_enable;
+	u8 gtk_offload_enable;
+	/* Used for WOL, indicates the reason for waking event.*/
+	u32 wakeup_reason;
+	/* Record the last waking time for comparison with setting key. */
+	u64 last_wakeup_time;
+};
+
+struct rtl_stats {
+	u8 psaddr[ETH_ALEN];
+	u32 mac_time[2];
+	s8 rssi;
+	u8 signal;
+	u8 noise;
+	u8 rate;		/* hw desc rate */
+	u8 rawdata;
+	u8 received_channel;
+	u8 control;
+	u8 mask;
+	u8 freq;
+	u16 len;
+	u64 tsf;
+	u32 beacon_time;
+	u8 nic_type;
+	u16 length;
+	u8 signalquality;	/*in 0-100 index. */
+	/*
+	 * Real power in dBm for this packet,
+	 * no beautification and aggregation.
+	 * */
+	s32 recvsignalpower;
+	s8 rxpower;		/*in dBm Translate from PWdB */
+	u8 signalstrength;	/*in 0-100 index. */
+	u16 b_hwerror:1;
+	u16 b_crc:1;
+	u16 b_icv:1;
+	u16 b_shortpreamble:1;
+	u16 antenna:1;
+	u16 decrypted:1;
+	u16 wakeup:1;
+	u32 timestamp_low;
+	u32 timestamp_high;
+	bool b_shift;
+
+	u8 rx_drvinfo_size;
+	u8 rx_bufshift;
+	bool b_isampdu;
+	bool b_isfirst_ampdu;
+	bool rx_is40Mhzpacket;
+	u8 rx_packet_bw;
+	u32 rx_pwdb_all;
+	u8 rx_mimo_signalstrength[4];	/*in 0~100 index */
+	s8 rx_mimo_signalquality[4];
+	u8 rx_mimo_evm_dbm[4];
+	u16 cfo_short[4];		/* per-path's Cfo_short */
+	u16 cfo_tail[4];
+
+	u8 rx_pwr[4]; /* per-path's pwdb */
+	u8 rx_snr[4]; /* per-path's SNR */
+	u8 bandwidth;
+	u8 bt_coex_pwr_adjust;
+	bool b_packet_matchbssid;
+	bool b_is_cck;
+	bool b_is_ht;
+	bool b_packet_toself;
+	bool b_packet_beacon;	/*for rssi */
+	char cck_adc_pwdb[4];	/*for rx path selection */
+
+	bool b_is_vht;
+	bool b_is_short_gi;
+	u8 vht_nss;
+
+	u8 packet_report_type;
+
+	u32 macid;
+	u8 wake_match;
+	u32 bt_rx_rssi_percentage;
+	u32 macid_valid_entry[2];
+};
+
+struct rt_link_detect {
+	/* count for raoming */
+	u32 bcn_rx_inperiod;
+	u32 roam_times;
+
+	u32 num_tx_in4period[4];
+	u32 num_rx_in4period[4];
+
+	u32 num_tx_inperiod;
+	u32 num_rx_inperiod;
+
+	bool b_busytraffic;
+	bool b_tx_busy_traffic;
+	bool b_rx_busy_traffic;
+	bool b_higher_busytraffic;
+	bool b_higher_busyrxtraffic;
+
+	u32 tidtx_in4period[MAX_TID_COUNT][4];
+	u32 tidtx_inperiod[MAX_TID_COUNT];
+	bool higher_busytxtraffic[MAX_TID_COUNT];
+};
+
+struct rtl_tcb_desc {
+	u8 packet_bw:2;
+	u8 b_multicast:1;
+	u8 b_broadcast:1;
+
+	u8 b_rts_stbc:1;
+	u8 b_rts_enable:1;
+	u8 b_cts_enable:1;
+	u8 b_rts_use_shortpreamble:1;
+	u8 b_rts_use_shortgi:1;
+	u8 rts_sc:1;
+	u8 b_rts_bw:1;
+	u8 rts_rate;
+
+	u8 use_shortgi:1;
+	u8 use_shortpreamble:1;
+	u8 use_driver_rate:1;
+	u8 disable_ratefallback:1;
+
+	u8 ratr_index;
+	u8 mac_id;
+	u8 hw_rate;
+
+	u8 b_last_inipkt:1;
+	u8 b_cmd_or_init:1;
+	u8 queue_index;
+
+	/* early mode */
+	u8 empkt_num;
+	/* The max value by HW */
+	u32 empkt_len[10];
+	bool btx_enable_sw_calc_duration;
+	/* used for hal construct pkt,
+	 * we may set desc when tx */
+	u8 self_desc;
+};
+
+struct rtl_wow_pattern {
+	u8 type;
+	u16 crc;
+	u32 mask[4];
+};
+
+struct proxim {
+	bool proxim_on;
+
+	void *proximity_priv;
+	int (*proxim_rx)(struct ieee80211_hw *hw, struct rtl_stats *status,
+			 struct sk_buff *skb);
+	u8  (*proxim_get_var)(struct ieee80211_hw *hw, u8 type);
+};
+
+struct rtl_hal_ops {
+	int (*init_sw_vars)(struct ieee80211_hw *hw);
+	void (*deinit_sw_vars)(struct ieee80211_hw *hw);
+	void (*read_eeprom_info)(struct ieee80211_hw *hw);
+	void (*interrupt_recognized)(struct ieee80211_hw *hw,
+				     u32 *p_inta, u32 *p_intb);
+	int (*hw_init)(struct ieee80211_hw *hw);
+	void (*hw_disable)(struct ieee80211_hw *hw);
+	void (*hw_suspend)(struct ieee80211_hw *hw);
+	void (*hw_resume)(struct ieee80211_hw *hw);
+	void (*enable_interrupt)(struct ieee80211_hw *hw);
+	void (*disable_interrupt)(struct ieee80211_hw *hw);
+	int (*set_network_type)(struct ieee80211_hw *hw,
+				enum nl80211_iftype type);
+	void (*set_chk_bssid)(struct ieee80211_hw *hw,
+			      bool check_bssid);
+	void (*set_bw_mode)(struct ieee80211_hw *hw,
+			    enum nl80211_channel_type ch_type);
+	u8 (*switch_channel)(struct ieee80211_hw *hw);
+	void (*set_qos)(struct ieee80211_hw *hw, int aci);
+	void (*set_bcn_reg)(struct ieee80211_hw *hw);
+	void (*set_bcn_intv)(struct ieee80211_hw *hw);
+	void (*update_interrupt_mask)(struct ieee80211_hw *hw,
+				      u32 add_msr, u32 rm_msr);
+	void (*get_hw_reg)(struct ieee80211_hw *hw, u8 variable, u8 *val);
+	void (*set_hw_reg)(struct ieee80211_hw *hw, u8 variable, u8 *val);
+	void (*update_rate_tbl)(struct ieee80211_hw *hw,
+				struct ieee80211_sta *sta, u8 rssi_level);
+	void (*pre_fill_tx_bd_desc)(struct ieee80211_hw *hw, u8 *tx_bd_desc,
+				    u8 *desc, u8 queue_index,
+				    struct sk_buff *skb, dma_addr_t addr);
+	u16 (*rx_desc_buff_remained_cnt)(struct ieee80211_hw *hw,
+					 u8 queue_index);
+	void (*rx_check_dma_ok)(struct ieee80211_hw *hw, u8 *header_desc,
+				u8 queue_index);
+	void (*fill_tx_desc)(struct ieee80211_hw *hw,
+			     struct ieee80211_hdr *hdr,
+			     u8 *pdesc_tx, u8 *pbd_desc,
+			     struct ieee80211_tx_info *info,
+			     struct ieee80211_sta *sta,
+			     struct sk_buff *skb, u8 hw_queue,
+			     struct rtl_tcb_desc *ptcb_desc);
+	void (*fill_tx_cmddesc)(struct ieee80211_hw *hw, u8 *pdesc,
+				bool b_firstseg, bool b_lastseg,
+				struct sk_buff *skb);
+	bool (*query_rx_desc)(struct ieee80211_hw *hw,
+			      struct rtl_stats *status,
+			      struct ieee80211_rx_status *rx_status,
+			      u8 *pdesc, struct sk_buff *skb);
+	void (*set_channel_access)(struct ieee80211_hw *hw);
+	bool (*radio_onoff_checking)(struct ieee80211_hw *hw, u8 *valid);
+	void (*dm_watchdog)(struct ieee80211_hw *hw);
+	void (*scan_operation_backup)(struct ieee80211_hw *hw, u8 operation);
+	bool (*set_rf_power_state)(struct ieee80211_hw *hw,
+				   enum rf_pwrstate rfpwr_state);
+	void (*led_control)(struct ieee80211_hw *hw,
+			    enum led_ctl_mode ledaction);
+	void (*set_desc)(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
+			 u8 desc_name, u8 *val);
+	u32 (*get_desc)(u8 *pdesc, bool istx, u8 desc_name);
+	bool (*is_tx_desc_closed)(struct ieee80211_hw *hw,
+				  u8 hw_queue, u16 index);
+	void (*tx_polling)(struct ieee80211_hw *hw, u8 hw_queue);
+	void (*enable_hw_sec)(struct ieee80211_hw *hw);
+	void (*set_key)(struct ieee80211_hw *hw, u32 key_index,
+			u8 *p_macaddr, bool is_group, u8 enc_algo,
+			bool is_wepkey, bool clear_all);
+	void (*init_sw_leds)(struct ieee80211_hw *hw);
+	u32 (*get_bbreg)(struct ieee80211_hw *hw, u32 regaddr, u32 bitmask);
+	void (*set_bbreg)(struct ieee80211_hw *hw, u32 regaddr, u32 bitmask,
+			  u32 data);
+	u32 (*get_rfreg)(struct ieee80211_hw *hw, enum radio_path rfpath,
+			 u32 regaddr, u32 bitmask);
+	void (*set_rfreg)(struct ieee80211_hw *hw, enum radio_path rfpath,
+			  u32 regaddr, u32 bitmask, u32 data);
+	void (*allow_all_destaddr)(struct ieee80211_hw *hw,
+				   bool allow_all_da, bool write_into_reg);
+	void (*linked_set_reg)(struct ieee80211_hw *hw);
+	void (*check_switch_to_dmdp)(struct ieee80211_hw *hw);
+	void (*dualmac_easy_concurrent)(struct ieee80211_hw *hw);
+	void (*dualmac_switch_to_dmdp)(struct ieee80211_hw *hw);
+	void (*c2h_command_handle)(struct ieee80211_hw *hw);
+	void (*bt_wifi_media_status_notify)(struct ieee80211_hw *hw,
+					    bool mstate);
+	void (*bt_turn_off_bt_coexist_before_enter_lps)(struct ieee80211_hw *w);
+	void (*fill_h2c_cmd)(struct ieee80211_hw *hw, u8 element_id,
+			     u32 cmd_len, u8 *p_cmdbuffer);
+	bool (*get_btc_status)(void);
+	u32 (*rx_command_packet)(struct ieee80211_hw *hw,
+				 struct rtl_stats status, struct sk_buff *skb);
+	void (*add_wowlan_pattern)(struct ieee80211_hw *hw,
+				   struct rtl_wow_pattern *rtl_pattern,
+				   u8 index);
+};
+
+struct rtl_intf_ops {
+	/*com */
+	void (*read92e_efuse_byte)(struct ieee80211_hw *hw, u16 _offset,
+				   u8 *pbuf);
+	int (*adapter_start)(struct ieee80211_hw *hw);
+	void (*adapter_stop)(struct ieee80211_hw *hw);
+	bool (*check_buddy_priv)(struct ieee80211_hw *hw,
+				 struct rtl_priv **buddy_priv);
+
+	int (*adapter_tx)(struct ieee80211_hw *hw,
+			  struct ieee80211_sta *sta,
+			  struct sk_buff *skb,
+			  struct rtl_tcb_desc *ptcb_desc);
+	void (*flush)(struct ieee80211_hw *hw, u32 queues, bool drop);
+	int (*reset_trx_ring)(struct ieee80211_hw *hw);
+	bool (*waitq_insert)(struct ieee80211_hw *hw,
+			     struct ieee80211_sta *sta,
+			     struct sk_buff *skb);
+
+	/*pci */
+	void (*disable_aspm)(struct ieee80211_hw *hw);
+	void (*enable_aspm)(struct ieee80211_hw *hw);
+
+	/*usb */
+};
+
+struct rtl_mod_params {
+	/* default: 0 = using hardware encryption */
+	bool sw_crypto;
+
+	/* default: 0 = DBG_EMERG (0)*/
+	int debug;
+
+	/* default: 1 = using no linked power save */
+	bool b_inactiveps;
+
+	/* default: 1 = using linked sw power save */
+	bool b_swctrl_lps;
+
+	/* default: 1 = using linked fw power save */
+	bool b_fwctrl_lps;
+};
+
+struct rtl_hal_cfg {
+	u8 bar_id;
+	bool write_readback;
+	char *name;
+	char *fw_name;
+	struct rtl_hal_ops *ops;
+	struct rtl_mod_params *mod_params;
+
+	/*this map used for some registers or vars
+	   defined int HAL but used in MAIN */
+	u32 maps[RTL_VAR_MAP_MAX];
+
+};
+
+struct rtl_locks {
+	/* mutex */
+	struct mutex conf_mutex;
+
+	/*spin lock */
+	spinlock_t ips_lock;
+	spinlock_t irq_th_lock;
+	spinlock_t h2c_lock;
+	spinlock_t rf_ps_lock;
+	spinlock_t rf_lock;
+	spinlock_t lps_lock;
+	spinlock_t waitq_lock;
+	spinlock_t entry_list_lock;
+
+	/*FW clock change */
+	spinlock_t fw_ps_lock;
+
+	/*Dul mac*/
+	spinlock_t cck_and_rw_pagea_lock;
+
+	/*Easy concurrent*/
+	spinlock_t check_sendpkt_lock;
+
+	spinlock_t iqk_lock;
+};
+
+struct rtl_works {
+	struct ieee80211_hw *hw;
+
+	/*timer */
+	struct timer_list watchdog_timer;
+	struct timer_list dualmac_easyconcurrent_retrytimer;
+	struct timer_list fw_clockoff_timer;
+	struct timer_list fast_antenna_trainning_timer;
+	/*task */
+	struct tasklet_struct irq_tasklet;
+	struct tasklet_struct irq_prepare_bcn_tasklet;
+
+	/*work queue */
+	struct workqueue_struct *rtl_wq;
+	struct delayed_work watchdog_wq;
+	struct delayed_work ips_nic_off_wq;
+
+	/* For SW LPS */
+	struct delayed_work ps_work;
+	struct delayed_work ps_rfon_wq;
+	struct delayed_work fwevt_wq;
+};
+
+struct rtl_debug {
+	u32 dbgp_type[DBGP_TYPE_MAX];
+	u32 global_debuglevel;
+	u64 global_debugcomponents;
+
+	/* add for proc debug */
+	struct proc_dir_entry *proc_dir;
+	char proc_name[20];
+};
+
+#define MIMO_PS_STATIC			0
+#define MIMO_PS_DYNAMIC			1
+#define MIMO_PS_NOLIMIT			3
+
+struct rtl_dualmac_easy_concurrent_ctl {
+	enum band_type currentbandtype_backfordmdp;
+	bool bclose_bbandrf_for_dmsp;
+	bool bchange_to_dmdp;
+	bool bchange_to_dmsp;
+	bool bswitch_in_process;
+};
+
+struct rtl_dmsp_ctl {
+	bool bactivescan_for_slaveofdmsp;
+	bool bscan_for_anothermac_fordmsp;
+	bool bscan_for_itself_fordmsp;
+	bool bwritedig_for_anothermacofdmsp;
+	u32 curdigvalue_for_anothermacofdmsp;
+	bool bchangecckpdstate_for_anothermacofdmsp;
+	u8 curcckpdstate_for_anothermacofdmsp;
+	bool bchangetxhighpowerlvl_for_anothermacofdmsp;
+	u8 curtxhighlvl_for_anothermacofdmsp;
+	long rssivalmin_for_anothermacofdmsp;
+};
+
+struct rtl_global_var {
+	/* from this list we can get
+	 * other adapter's rtl_priv */
+	struct list_head glb_priv_list;
+	spinlock_t glb_list_lock;
+};
+
+struct rtl_btc_info {
+	u8 bt_type;
+	u8 btcoexist;
+	u8 ant_num;
+};
+
+struct rtl_btc_ops {
+	void (*btc_init_variables)(struct rtl_priv *rtlpriv);
+	void (*btc_init_hal_vars)(struct rtl_priv *rtlpriv);
+	void (*btc_init_hw_config)(struct rtl_priv *rtlpriv);
+	void (*btc_ips_notify)(struct rtl_priv *rtlpriv, u8 type);
+	void (*btc_lps_notify)(struct rtl_priv *rtlpriv, u8 type);
+	void (*btc_scan_notify)(struct rtl_priv *rtlpriv, u8 scantype);
+	void (*btc_connect_notify)(struct rtl_priv *rtlpriv, u8 action);
+	void (*btc_mediastatus_notify)(struct rtl_priv *rtlpriv,
+				       enum rt_media_status mstatus);
+	void (*btc_periodical)(struct rtl_priv *rtlpriv);
+	void (*btc_halt_notify)(void);
+	void (*btc_btinfo_notify)(struct rtl_priv *rtlpriv,
+				  u8 *tmp_buf, u8 length);
+	bool (*btc_is_limited_dig)(struct rtl_priv *rtlpriv);
+	bool (*btc_is_disable_edca_turbo)(struct rtl_priv *rtlpriv);
+	bool (*btc_is_bt_disabled)(struct rtl_priv *rtlpriv);
+	void (*btc_special_packet_notify)(struct rtl_priv *rtlpriv,
+					  u8 pkt_type);
+};
+
+struct rtl_bt_coexist {
+	struct rtl_btc_ops *btc_ops;
+	struct rtl_btc_info btc_info;
+};
+
+
+struct rtl_priv {
+	struct list_head list;
+	struct rtl_priv *buddy_priv;
+	struct rtl_global_var *glb_var;
+	struct rtl_dualmac_easy_concurrent_ctl easy_concurrent_ctl;
+	struct rtl_dmsp_ctl dmsp_ctl;
+	struct rtl_locks locks;
+	struct rtl_works works;
+	struct rtl_mac mac80211;
+	struct rtl_hal rtlhal;
+	struct rtl_regulatory regd;
+	struct rtl_rfkill rfkill;
+	struct rtl_io io;
+	struct rtl_phy phy;
+	struct rtl_dm dm;
+	struct rtl_security sec;
+	struct rtl_efuse efuse;
+
+	struct rtl_ps_ctl psc;
+	struct rate_adaptive ra;
+	struct dynamic_primary_cca primarycca;
+	struct wireless_stats stats;
+	struct rt_link_detect link_info;
+	struct false_alarm_statistics falsealm_cnt;
+
+	struct rtl_rate_priv *rate_priv;
+
+	struct rtl_debug dbg;
+
+	/* sta entry list for ap adhoc or mesh */
+	struct list_head entry_list;
+
+	/*
+	 *hal_cfg : for diff cards
+	 *intf_ops : for diff interrface usb/pcie
+	 */
+	struct rtl_hal_cfg *cfg;
+	struct rtl_intf_ops *intf_ops;
+
+	/*this var will be set by set_bit,
+	   and was used to indicate status of
+	   interface or hardware */
+	unsigned long status;
+
+	/* intel Proximity, should be alloc mem
+	 * in intel Proximity module and can only
+	 * be used in intel Proximity mode */
+	struct proxim proximity;
+
+	/*for bt coexist use*/
+	struct rtl_bt_coexist btcoexist;
+
+	/* seperate 92ee from other ICs,
+	  * 92ee use new trx flow. */
+	bool use_new_trx_flow;
+
+#ifdef CONFIG_PM
+	struct wiphy_wowlan_support wowlan;
+#endif
+	/*This must be the last item so
+	   that it points to the data allocated
+	   beyond  this structure like:
+	   rtl_pci_priv or rtl_usb_priv */
+	u8 priv[0];
+};
+
+#define rtl_priv(hw)		(((struct rtl_priv *)(hw)->priv))
+#define rtl_mac(rtlpriv)	(&((rtlpriv)->mac80211))
+#define rtl_hal(rtlpriv)	(&((rtlpriv)->rtlhal))
+#define rtl_efuse(rtlpriv)	(&((rtlpriv)->efuse))
+#define rtl_psc(rtlpriv)	(&((rtlpriv)->psc))
+#define rtl_sec(rtlpriv)	(&((rtlpriv)->sec))
+#define rtl_dm(rtlpriv)	(&((rtlpriv)->dm))
+/***************************************
+    Bluetooth Co-existance Related
+****************************************/
+
+enum bt_ant_num {
+	ANT_X2 = 0,
+	ANT_X1 = 1,
+};
+
+enum bt_co_type {
+	BT_2WIRE = 0,
+	BT_ISSC_3WIRE = 1,
+	BT_ACCEL = 2,
+	BT_CSR_BC4 = 3,
+	BT_CSR_BC8 = 4,
+	BT_RTL8756 = 5,
+	BT_RTL8723A = 6,
+	BT_RTL8821A = 7,
+	BT_RTL8723B = 8,
+	BT_RTL8192E = 9,
+	BT_RTL8812A = 11,
+};
+
+enum bt_total_ant_num {
+	ANT_TOTAL_X2 = 0,
+	ANT_TOTAL_X1 = 1
+};
+
+enum bt_cur_state {
+	BT_OFF = 0,
+	BT_ON = 1,
+};
+
+enum bt_service_type {
+	BT_SCO = 0,
+	BT_A2DP = 1,
+	BT_HID = 2,
+	BT_HID_IDLE = 3,
+	BT_SCAN = 4,
+	BT_IDLE = 5,
+	BT_OTHER_ACTION = 6,
+	BT_BUSY = 7,
+	BT_OTHERBUSY = 8,
+	BT_PAN = 9,
+};
+
+enum bt_radio_shared {
+	BT_RADIO_SHARED = 0,
+	BT_RADIO_INDIVIDUAL = 1,
+};
+
+struct bt_coexist_info {
+	/* EEPROM BT info. */
+	u8 eeprom_bt_coexist;
+	u8 eeprom_bt_type;
+	u8 eeprom_bt_ant_num;
+	u8 eeprom_bt_ant_isolation;
+	u8 eeprom_bt_radio_shared;
+
+	u8 bt_coexistence;
+	u8 bt_ant_num;
+	u8 bt_coexist_type;
+	u8 bt_state;
+	u8 bt_cur_state;	/* 0:on, 1:off */
+	u8 bt_ant_isolation;	/* 0:good, 1:bad */
+	u8 bt_pape_ctrl;	/* 0:SW, 1:SW/HW dynamic */
+	u8 bt_service;
+	u8 bt_radio_shared_type;
+	u8 bt_rfreg_origin_1e;
+	u8 bt_rfreg_origin_1f;
+	u8 bt_rssi_state;
+	u32 ratio_tx;
+	u32 ratio_pri;
+	u32 bt_edca_ul;
+	u32 bt_edca_dl;
+
+	bool b_init_set;
+	bool b_bt_busy_traffic;
+	bool b_bt_traffic_mode_set;
+	bool b_bt_non_traffic_mode_set;
+
+	bool b_fw_coexist_all_off;
+	bool b_sw_coexist_all_off;
+	bool b_hw_coexist_all_off;
+	u32 current_state;
+	u32 previous_state;
+	u32 current_state_h;
+	u32 previous_state_h;
+
+	u8 bt_pre_rssi_state;
+	u8 bt_pre_rssi_state1;
+
+	u8 b_reg_bt_iso;
+	u8 b_reg_bt_sco;
+	bool b_balance_on;
+	u8 bt_active_zero_cnt;
+	bool b_cur_bt_disabled;
+	bool b_pre_bt_disabled;
+
+	u8 bt_profile_case;
+	u8 bt_profile_action;
+	bool b_bt_busy;
+	bool b_hold_for_bt_operation;
+	u8 lps_counter;
+};
+
+/****************************************
+	mem access macro define start
+	Call endian free function when
+	1. Read/write packet content.
+	2. Before write integer to IO.
+	3. After read integer from IO.
+****************************************/
+/* Convert little data endian to host ordering */
+#define EF1BYTE(_val)		\
+	((u8)(_val))
+#define EF2BYTE(_val)		\
+	(le16_to_cpu(_val))
+#define EF4BYTE(_val)		\
+	(le32_to_cpu(_val))
+
+/* Read data from memory */
+#define READEF1BYTE(_ptr)	\
+	EF1BYTE(*((u8 *)(_ptr)))
+/* Read le16 data from memory and convert to host ordering */
+#define READEF2BYTE(_ptr)	\
+	EF2BYTE(*(_ptr))
+#define READEF4BYTE(_ptr)	\
+	EF4BYTE(*(_ptr))
+
+/* Write data to memory */
+#define WRITEEF1BYTE(_ptr, _val)	\
+	(*((u8 *)(_ptr))) = EF1BYTE(_val)
+/* Write le16 data to memory in host ordering */
+#define WRITEEF2BYTE(_ptr, _val)	\
+	(*((u16 *)(_ptr))) = EF2BYTE(_val)
+#define WRITEEF4BYTE(_ptr, _val)	\
+	(*((u32 *)(_ptr))) = EF2BYTE(_val)
+
+/* Create a bit mask
+ * Examples:
+ * BIT_LEN_MASK_32(0) => 0x00000000
+ * BIT_LEN_MASK_32(1) => 0x00000001
+ * BIT_LEN_MASK_32(2) => 0x00000003
+ * BIT_LEN_MASK_32(32) => 0xFFFFFFFF
+ */
+#define BIT_LEN_MASK_32(__bitlen)	 \
+	(0xFFFFFFFF >> (32 - (__bitlen)))
+#define BIT_LEN_MASK_16(__bitlen)	 \
+	(0xFFFF >> (16 - (__bitlen)))
+#define BIT_LEN_MASK_8(__bitlen) \
+	(0xFF >> (8 - (__bitlen)))
+
+/* Create an offset bit mask
+ * Examples:
+ * BIT_OFFSET_LEN_MASK_32(0, 2) => 0x00000003
+ * BIT_OFFSET_LEN_MASK_32(16, 2) => 0x00030000
+ */
+#define BIT_OFFSET_LEN_MASK_32(__bitoffset, __bitlen) \
+	(BIT_LEN_MASK_32(__bitlen) << (__bitoffset))
+#define BIT_OFFSET_LEN_MASK_16(__bitoffset, __bitlen) \
+	(BIT_LEN_MASK_16(__bitlen) << (__bitoffset))
+#define BIT_OFFSET_LEN_MASK_8(__bitoffset, __bitlen) \
+	(BIT_LEN_MASK_8(__bitlen) << (__bitoffset))
+
+/*Description:
+ * Return 4-byte value in host byte ordering from
+ * 4-byte pointer in little-endian system.
+ */
+#define LE_P4BYTE_TO_HOST_4BYTE(__pstart) \
+	(EF4BYTE(*((__le32 *)(__pstart))))
+#define LE_P2BYTE_TO_HOST_2BYTE(__pstart) \
+	(EF2BYTE(*((__le16 *)(__pstart))))
+#define LE_P1BYTE_TO_HOST_1BYTE(__pstart) \
+	(EF1BYTE(*((u8 *)(__pstart))))
+
+/*Description:
+Translate subfield (continuous bits in little-endian) of 4-byte
+value to host byte ordering.*/
+#define LE_BITS_TO_4BYTE(__pstart, __bitoffset, __bitlen) \
+	( \
+		(LE_P4BYTE_TO_HOST_4BYTE(__pstart) >> (__bitoffset))  & \
+		BIT_LEN_MASK_32(__bitlen) \
+	)
+#define LE_BITS_TO_2BYTE(__pstart, __bitoffset, __bitlen) \
+	( \
+		(LE_P2BYTE_TO_HOST_2BYTE(__pstart) >> (__bitoffset)) & \
+		BIT_LEN_MASK_16(__bitlen) \
+	)
+#define LE_BITS_TO_1BYTE(__pstart, __bitoffset, __bitlen) \
+	( \
+		(LE_P1BYTE_TO_HOST_1BYTE(__pstart) >> (__bitoffset)) & \
+		BIT_LEN_MASK_8(__bitlen) \
+	)
+
+/* Description:
+ * Mask subfield (continuous bits in little-endian) of 4-byte value
+ * and return the result in 4-byte value in host byte ordering.
+ */
+#define LE_BITS_CLEARED_TO_4BYTE(__pstart, __bitoffset, __bitlen) \
+	( \
+		LE_P4BYTE_TO_HOST_4BYTE(__pstart)  & \
+		(~BIT_OFFSET_LEN_MASK_32(__bitoffset, __bitlen)) \
+	)
+#define LE_BITS_CLEARED_TO_2BYTE(__pstart, __bitoffset, __bitlen) \
+	( \
+		LE_P2BYTE_TO_HOST_2BYTE(__pstart) & \
+		(~BIT_OFFSET_LEN_MASK_16(__bitoffset, __bitlen)) \
+	)
+#define LE_BITS_CLEARED_TO_1BYTE(__pstart, __bitoffset, __bitlen) \
+	( \
+		LE_P1BYTE_TO_HOST_1BYTE(__pstart) & \
+		(~BIT_OFFSET_LEN_MASK_8(__bitoffset, __bitlen)) \
+	)
+
+/* Description:
+ * Set subfield of little-endian 4-byte value to specified value.
+ */
+#define SET_BITS_TO_LE_4BYTE(__pstart, __bitoffset, __bitlen, __val) \
+	*((u32 *)(__pstart)) = \
+	( \
+		LE_BITS_CLEARED_TO_4BYTE(__pstart, __bitoffset, __bitlen) | \
+		((((u32)__val) & BIT_LEN_MASK_32(__bitlen)) << (__bitoffset)) \
+	);
+#define SET_BITS_TO_LE_2BYTE(__pstart, __bitoffset, __bitlen, __val) \
+	*((u16 *)(__pstart)) = \
+	( \
+		LE_BITS_CLEARED_TO_2BYTE(__pstart, __bitoffset, __bitlen) | \
+		((((u16)__val) & BIT_LEN_MASK_16(__bitlen)) << (__bitoffset)) \
+	);
+#define SET_BITS_TO_LE_1BYTE(__pstart, __bitoffset, __bitlen, __val) \
+	*((u8 *)(__pstart)) = EF1BYTE \
+	( \
+		LE_BITS_CLEARED_TO_1BYTE(__pstart, __bitoffset, __bitlen) | \
+		((((u8)__val) & BIT_LEN_MASK_8(__bitlen)) << (__bitoffset)) \
+	);
+
+#define	N_BYTE_ALIGMENT(__value, __aligment) ((__aligment == 1) ? \
+	(__value) : (((__value + __aligment - 1) / __aligment) * __aligment))
+
+/****************************************
+	mem access macro define end
+****************************************/
+
+#define byte(x , n) ((x >> (8 * n)) & 0xff)
+
+#define packet_get_type(_packet) (EF1BYTE((_packet).octet[0]) & 0xFC)
+#define RTL_WATCH_DOG_TIME	2000
+#define MSECS(t)		msecs_to_jiffies(t)
+#define WLAN_FC_GET_VERS(fc)	(le16_to_cpu(fc) & IEEE80211_FCTL_VERS)
+#define WLAN_FC_GET_TYPE(fc)	(le16_to_cpu(fc) & IEEE80211_FCTL_FTYPE)
+#define WLAN_FC_GET_STYPE(fc)	(le16_to_cou(fc) & IEEE80211_FCTL_STYPE)
+#define WLAN_FC_MORE_DATA(fc)	(le16_to_cpu(fc) & IEEE80211_FCTL_MOREDATA)
+#define SEQ_TO_SN(seq)		(((seq) & IEEE80211_SCTL_SEQ) >> 4)
+#define SN_TO_SEQ(ssn)		(((ssn) << 4) & IEEE80211_SCTL_SEQ)
+#define MAX_SN			((IEEE80211_SCTL_SEQ) >> 4)
+
+#define	RT_RF_OFF_LEVL_ASPM		BIT(0)	/*PCI ASPM */
+#define	RT_RF_OFF_LEVL_CLK_REQ		BIT(1)	/*PCI clock request */
+#define	RT_RF_OFF_LEVL_PCI_D3		BIT(2)	/*PCI D3 mode */
+/*NIC halt, re-initialize hw parameters*/
+#define	RT_RF_OFF_LEVL_HALT_NIC		BIT(3)
+#define	RT_RF_OFF_LEVL_FREE_FW		BIT(4)	/*FW free, re-download the FW */
+#define	RT_RF_OFF_LEVL_FW_32K		BIT(5)	/*FW in 32k */
+/*Always enable ASPM and Clock Req in initialization.*/
+#define	RT_RF_PS_LEVEL_ALWAYS_ASPM	BIT(6)
+/* no matter RFOFF or SLEEP we set PS_ASPM_LEVL*/
+#define	RT_PS_LEVEL_ASPM		BIT(7)
+/*When LPS is on, disable 2R if no packet is received or transmittd.*/
+#define	RT_RF_LPS_DISALBE_2R		BIT(30)
+#define	RT_RF_LPS_LEVEL_ASPM		BIT(31)	/*LPS with ASPM */
+#define	RT_IN_PS_LEVEL(ppsc, _ps_flg)		\
+	((ppsc->cur_ps_level & _ps_flg) ? true : false)
+#define	RT_CLEAR_PS_LEVEL(ppsc, _ps_flg)	\
+	(ppsc->cur_ps_level &= (~(_ps_flg)))
+#define	RT_SET_PS_LEVEL(ppsc, _ps_flg)		\
+	(ppsc->cur_ps_level |= _ps_flg)
+
+#define container_of_dwork_rtl(x , y , z) \
+	container_of(container_of(x, struct delayed_work, work), y, z)
+
+#define FILL_OCTET_STRING(_os , _octet , _len)	\
+		(_os).octet = (u8 *)(_octet);		\
+		(_os).length = (_len);
+
+#define CP_MACADDR(des , src)	\
+	((des)[0] = (src)[0] , (des)[1] = (src)[1],\
+	(des)[2] = (src)[2] , (des)[3] = (src)[3],\
+	(des)[4] = (src)[4] , (des)[5] = (src)[5])
+
+#define	LDPC_HT_ENABLE_RX			BIT(0)
+#define	LDPC_HT_ENABLE_TX			BIT(1)
+#define	LDPC_HT_TEST_TX_ENABLE			BIT(2)
+#define	LDPC_HT_CAP_TX				BIT(3)
+
+#define	STBC_HT_ENABLE_RX			BIT(0)
+#define	STBC_HT_ENABLE_TX			BIT(1)
+#define	STBC_HT_TEST_TX_ENABLE			BIT(2)
+#define	STBC_HT_CAP_TX				BIT(3)
+
+
+#define	LDPC_VHT_ENABLE_RX			BIT(0)
+#define	LDPC_VHT_ENABLE_TX			BIT(1)
+#define	LDPC_VHT_TEST_TX_ENABLE			BIT(2)
+#define	LDPC_VHT_CAP_TX				BIT(3)
+
+#define	STBC_VHT_ENABLE_RX			BIT(0)
+#define	STBC_VHT_ENABLE_TX			BIT(1)
+#define	STBC_VHT_TEST_TX_ENABLE			BIT(2)
+#define	STBC_VHT_CAP_TX				BIT(3)
+
+
+static inline u8 rtl_read_byte(struct rtl_priv *rtlpriv, u32 addr)
+{
+	return rtlpriv->io.read8_sync(rtlpriv, addr);
+}
+
+static inline u16 rtl_read_word(struct rtl_priv *rtlpriv, u32 addr)
+{
+	return rtlpriv->io.read16_sync(rtlpriv, addr);
+}
+
+static inline u32 rtl_read_dword(struct rtl_priv *rtlpriv, u32 addr)
+{
+	return rtlpriv->io.read32_sync(rtlpriv, addr);
+}
+
+static inline void rtl_write_byte(struct rtl_priv *rtlpriv, u32 addr, u8 val8)
+{
+	rtlpriv->io.write8_async(rtlpriv, addr, val8);
+
+	if (rtlpriv->cfg->write_readback)
+		rtlpriv->io.read8_sync(rtlpriv, addr);
+}
+
+static inline void rtl_write_word(struct rtl_priv *rtlpriv, u32 addr, u16 val16)
+{
+	rtlpriv->io.write16_async(rtlpriv, addr, val16);
+
+	if (rtlpriv->cfg->write_readback)
+		rtlpriv->io.read16_sync(rtlpriv, addr);
+}
+
+static inline void rtl_write_dword(struct rtl_priv *rtlpriv,
+				   u32 addr, u32 val32)
+{
+	rtlpriv->io.write32_async(rtlpriv, addr, val32);
+
+	if (rtlpriv->cfg->write_readback)
+		rtlpriv->io.read32_sync(rtlpriv, addr);
+}
+
+static inline u32 rtl_get_bbreg(struct ieee80211_hw *hw,
+				u32 regaddr, u32 bitmask)
+{
+	return ((struct rtl_priv *)(hw)->priv)->cfg->ops->get_bbreg(hw,
+								    regaddr,
+								    bitmask);
+}
+
+static inline void rtl_set_bbreg(struct ieee80211_hw *hw, u32 regaddr,
+				 u32 bitmask, u32 data)
+{
+	struct rtl_priv *rtlpriv = hw->priv;
+
+	rtlpriv->cfg->ops->set_bbreg(hw, regaddr, bitmask, data);
+}
+
+static inline u32 rtl_get_rfreg(struct ieee80211_hw *hw,
+				enum radio_path rfpath, u32 regaddr,
+				u32 bitmask)
+{
+	return ((struct rtl_priv *)(hw)->priv)->cfg->ops->get_rfreg(hw,
+								    rfpath,
+								    regaddr,
+								    bitmask);
+}
+
+static inline void rtl_set_rfreg(struct ieee80211_hw *hw,
+				 enum radio_path rfpath, u32 regaddr,
+				 u32 bitmask, u32 data)
+{
+	((struct rtl_priv *)(hw)->priv)->cfg->ops->set_rfreg(hw,
+							     rfpath, regaddr,
+							     bitmask, data);
+}
+
+static inline bool is_hal_stop(struct rtl_hal *rtlhal)
+{
+	return _HAL_STATE_STOP == rtlhal->state;
+}
+
+static inline void set_hal_start(struct rtl_hal *rtlhal)
+{
+	rtlhal->state = _HAL_STATE_START;
+}
+
+static inline void set_hal_stop(struct rtl_hal *rtlhal)
+{
+	rtlhal->state = _HAL_STATE_STOP;
+}
+
+static inline u8 get_rf_type(struct rtl_phy *rtlphy)
+{
+	return rtlphy->rf_type;
+}
+
+static inline struct ieee80211_hdr *rtl_get_hdr(struct sk_buff *skb)
+{
+	return (struct ieee80211_hdr *)(skb->data);
+}
+
+static inline __le16 rtl_get_fc(struct sk_buff *skb)
+{
+	return rtl_get_hdr(skb)->frame_control;
+}
+
+static inline u16 rtl_get_tid_h(struct ieee80211_hdr *hdr)
+{
+	return (ieee80211_get_qos_ctl(hdr))[0] & IEEE80211_QOS_CTL_TID_MASK;
+}
+
+static inline u16 rtl_get_tid(struct sk_buff *skb)
+{
+	return rtl_get_tid_h(rtl_get_hdr(skb));
+}
+
+static inline struct ieee80211_sta *rtl_find_sta(struct ieee80211_hw *hw,
+						 u8 *mac_addr)
+{
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	return ieee80211_find_sta(mac->vif, mac_addr);
+}
+
+struct ieee80211_hw *rtl_pci_get_hw_pointer(void);
+#endif
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211.h b/drivers/staging/rtl8192u/ieee80211/ieee80211.h
index cac2056b7f8..e0aa069fe9b 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211.h
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211.h
@@ -354,12 +354,10 @@ enum	_ReasonCode{
 #define ieee80211_wx_get_scan		ieee80211_wx_get_scan_rsl
 #define ieee80211_wx_set_encode		ieee80211_wx_set_encode_rsl
 #define ieee80211_wx_get_encode		ieee80211_wx_get_encode_rsl
-#if WIRELESS_EXT >= 18
 #define ieee80211_wx_set_mlme		ieee80211_wx_set_mlme_rsl
 #define ieee80211_wx_set_auth		ieee80211_wx_set_auth_rsl
 #define ieee80211_wx_set_encode_ext	ieee80211_wx_set_encode_ext_rsl
 #define ieee80211_wx_get_encode_ext	ieee80211_wx_get_encode_ext_rsl
-#endif
 
 
 typedef struct ieee_param {
@@ -392,16 +390,6 @@ typedef struct ieee_param {
 }ieee_param;
 
 
-#if WIRELESS_EXT < 17
-#define IW_QUAL_QUAL_INVALID   0x10
-#define IW_QUAL_LEVEL_INVALID  0x20
-#define IW_QUAL_NOISE_INVALID  0x40
-#define IW_QUAL_QUAL_UPDATED   0x1
-#define IW_QUAL_LEVEL_UPDATED  0x2
-#define IW_QUAL_NOISE_UPDATED  0x4
-#endif
-
-
 // linux under 2.6.9 release may not support it, so modify it for common use
 #define MSECS(t) msecs_to_jiffies(t)
 #define msleep_interruptible_rsl  msleep_interruptible
@@ -539,7 +527,7 @@ do { if (ieee80211_debug_level & (level)) \
 		{	\
 			int i;					\
 			u8 *pdata = (u8 *) data;			\
-			printk(KERN_DEBUG "ieee80211: %s()\n", __FUNCTION__);	\
+			printk(KERN_DEBUG "ieee80211: %s()\n", __func__);	\
 			for(i=0; i<(int)(datalen); i++)			\
 			{						\
 				printk("%2x ", pdata[i]);		\
@@ -2398,7 +2386,6 @@ extern int ieee80211_wx_set_encode(struct ieee80211_device *ieee,
 extern int ieee80211_wx_get_encode(struct ieee80211_device *ieee,
 				   struct iw_request_info *info,
 				   union iwreq_data *wrqu, char *key);
-#if WIRELESS_EXT >= 18
 extern int ieee80211_wx_get_encode_ext(struct ieee80211_device *ieee,
 			    struct iw_request_info *info,
 			    union iwreq_data *wrqu, char *extra);
@@ -2411,7 +2398,6 @@ extern int ieee80211_wx_set_auth(struct ieee80211_device *ieee,
 extern int ieee80211_wx_set_mlme(struct ieee80211_device *ieee,
 			       struct iw_request_info *info,
 			       union iwreq_data *wrqu, char *extra);
-#endif
 extern int ieee80211_wx_set_gen_ie(struct ieee80211_device *ieee, u8 *ie, size_t len);
 
 /* ieee80211_softmac.c */
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_ccmp.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_ccmp.c
index f2b16775a63..af2128b956b 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_ccmp.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_ccmp.c
@@ -444,7 +444,7 @@ static char *ieee80211_ccmp_print_stats(char *p, void *priv)
 
 void ieee80211_ccmp_null(void)
 {
-//    printk("============>%s()\n", __FUNCTION__);
+//    printk("============>%s()\n", __func__);
 	return;
 }
 
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_tkip.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_tkip.c
index 93121b42f16..fcc90a5700d 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_tkip.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_tkip.c
@@ -773,6 +773,6 @@ void __exit ieee80211_crypto_tkip_exit(void)
 
 void ieee80211_tkip_null(void)
 {
-//    printk("============>%s()\n", __FUNCTION__);
+//    printk("============>%s()\n", __func__);
 	return;
 }
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_wep.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_wep.c
index f2022369589..8c1bf1f5688 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_wep.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_wep.c
@@ -290,6 +290,6 @@ void __exit ieee80211_crypto_wep_exit(void)
 
 void ieee80211_wep_null(void)
 {
-//	printk("============>%s()\n", __FUNCTION__);
+//	printk("============>%s()\n", __func__);
 	return;
 }
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
index a98414a72bf..d9a8299c48e 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
@@ -549,7 +549,7 @@ void ieee80211_indicate_packets(struct ieee80211_device *ieee, struct ieee80211_
 	u8 i = 0 , j=0;
 	u16 ethertype;
 //	if(index > 1)
-//		IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): hahahahhhh, We indicate packet from reorder list, index is %u\n",__FUNCTION__,index);
+//		IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): hahahahhhh, We indicate packet from reorder list, index is %u\n",__func__,index);
 	for(j = 0; j<index; j++)
 	{
 //added by amy for reorder
@@ -609,7 +609,7 @@ static void RxReorderIndicatePacket(struct ieee80211_device *ieee,
 	u16			WinEnd = (pTS->RxIndicateSeq + WinSize -1)%4096;
 	u8			index = 0;
 	bool			bMatchWinStart = false, bPktInBuf = false;
-	IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): Seq is %d,pTS->RxIndicateSeq is %d, WinSize is %d\n",__FUNCTION__,SeqNum,pTS->RxIndicateSeq,WinSize);
+	IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): Seq is %d,pTS->RxIndicateSeq is %d, WinSize is %d\n",__func__,SeqNum,pTS->RxIndicateSeq,WinSize);
 	/* Rx Reorder initialize condition.*/
 	if(pTS->RxIndicateSeq == 0xffff) {
 		pTS->RxIndicateSeq = SeqNum;
@@ -662,11 +662,11 @@ static void RxReorderIndicatePacket(struct ieee80211_device *ieee,
 		IEEE80211_DEBUG(IEEE80211_DL_REORDER, "Packets indication!! IndicateSeq: %d, NewSeq: %d\n",\
 				pTS->RxIndicateSeq, SeqNum);
 		prxbIndicateArray[0] = prxb;
-//		printk("========================>%s(): SeqNum is %d\n",__FUNCTION__,SeqNum);
+//		printk("========================>%s(): SeqNum is %d\n",__func__,SeqNum);
 		index = 1;
 	} else {
 		/* Current packet is going to be inserted into pending list.*/
-		//IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): We RX no ordered packed, insert to ordered list\n",__FUNCTION__);
+		//IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): We RX no ordered packed, insert to ordered list\n",__func__);
 		if(!list_empty(&ieee->RxReorder_Unused_List)) {
 			pReorderEntry = (PRX_REORDER_ENTRY)list_entry(ieee->RxReorder_Unused_List.next,RX_REORDER_ENTRY,List);
 			list_del_init(&pReorderEntry->List);
@@ -674,11 +674,11 @@ static void RxReorderIndicatePacket(struct ieee80211_device *ieee,
 			/* Make a reorder entry and insert into a the packet list.*/
 			pReorderEntry->SeqNum = SeqNum;
 			pReorderEntry->prxb = prxb;
-	//		IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): pREorderEntry->SeqNum is %d\n",__FUNCTION__,pReorderEntry->SeqNum);
+	//		IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): pREorderEntry->SeqNum is %d\n",__func__,pReorderEntry->SeqNum);
 
 			if(!AddReorderEntry(pTS, pReorderEntry)) {
 				IEEE80211_DEBUG(IEEE80211_DL_REORDER, "%s(): Duplicate packet is dropped!! IndicateSeq: %d, NewSeq: %d\n",
-					__FUNCTION__, pTS->RxIndicateSeq, SeqNum);
+					__func__, pTS->RxIndicateSeq, SeqNum);
 				list_add_tail(&pReorderEntry->List,&ieee->RxReorder_Unused_List);
 				{
 					int i;
@@ -713,7 +713,7 @@ static void RxReorderIndicatePacket(struct ieee80211_device *ieee,
 
 	/* Check if there is any packet need indicate.*/
 	while(!list_empty(&pTS->RxPendingPktList)) {
-		IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): start RREORDER indicate\n",__FUNCTION__);
+		IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): start RREORDER indicate\n",__func__);
 		pReorderEntry = (PRX_REORDER_ENTRY)list_entry(pTS->RxPendingPktList.prev,RX_REORDER_ENTRY,List);
 		if( SN_LESS(pReorderEntry->SeqNum, pTS->RxIndicateSeq) ||
 				SN_EQUAL(pReorderEntry->SeqNum, pTS->RxIndicateSeq))
@@ -732,7 +732,7 @@ static void RxReorderIndicatePacket(struct ieee80211_device *ieee,
 
 			IEEE80211_DEBUG(IEEE80211_DL_REORDER,"Packets indication!! IndicateSeq: %d, NewSeq: %d\n",pTS->RxIndicateSeq, SeqNum);
 			prxbIndicateArray[index] = pReorderEntry->prxb;
-		//	printk("========================>%s(): pReorderEntry->SeqNum is %d\n",__FUNCTION__,pReorderEntry->SeqNum);
+		//	printk("========================>%s(): pReorderEntry->SeqNum is %d\n",__func__,pReorderEntry->SeqNum);
 			index++;
 
 			list_add_tail(&pReorderEntry->List,&ieee->RxReorder_Unused_List);
@@ -758,7 +758,7 @@ static void RxReorderIndicatePacket(struct ieee80211_device *ieee,
 
 	if(bPktInBuf && pTS->RxTimeoutIndicateSeq==0xffff) {
 		// Set new pending timer.
-		IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): SET rx timeout timer\n", __FUNCTION__);
+		IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): SET rx timeout timer\n", __func__);
 		pTS->RxTimeoutIndicateSeq = pTS->RxIndicateSeq;
 		if(timer_pending(&pTS->RxPktPendingTimer))
 			del_timer_sync(&pTS->RxPktPendingTimer);
@@ -832,8 +832,8 @@ static u8 parse_subframe(struct sk_buff *skb,
 
 			if(skb->len<(ETHERNET_HEADER_SIZE + nSubframe_Length)) {
 				printk("%s: A-MSDU parse error!! pRfd->nTotalSubframe : %d\n",\
-						__FUNCTION__,rxb->nr_subframes);
-				printk("%s: A-MSDU parse error!! Subframe Length: %d\n",__FUNCTION__, nSubframe_Length);
+						__func__,rxb->nr_subframes);
+				printk("%s: A-MSDU parse error!! Subframe Length: %d\n",__func__, nSubframe_Length);
 				printk("nRemain_Length is %d and nSubframe_Length is : %d\n",skb->len,nSubframe_Length);
 				printk("The Packet SeqNum is %d\n",SeqNum);
 				return 0;
@@ -950,7 +950,6 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 
 	//IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA, skb->data, skb->len);
 #ifdef NOT_YET
-#if WIRELESS_EXT > 15
 	/* Put this code here so that we avoid duplicating it in all
 	 * Rx paths. - Jean II */
 #ifdef IW_WIRELESS_SPY		/* defined in iw_handler.h */
@@ -964,18 +963,16 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 		wireless_spy_update(dev, hdr->addr2, &wstats);
 	}
 #endif /* IW_WIRELESS_SPY */
-#endif /* WIRELESS_EXT > 15 */
 	hostap_update_rx_stats(local->ap, hdr, rx_stats);
 #endif
 
-#if WIRELESS_EXT > 15
 	if (ieee->iw_mode == IW_MODE_MONITOR) {
 		ieee80211_monitor_rx(ieee, skb, rx_stats);
 		stats->rx_packets++;
 		stats->rx_bytes += skb->len;
 		return 1;
 	}
-#endif
+
 	if (ieee->host_decrypt) {
 		int idx = 0;
 		if (skb->len >= hdrlen + 3)
@@ -1027,7 +1024,7 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 	else
 	{
 		PRX_TS_RECORD pRxTS = NULL;
-			//IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): QOS ENABLE AND RECEIVE QOS DATA , we will get Ts, tid:%d\n",__FUNCTION__, tid);
+			//IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): QOS ENABLE AND RECEIVE QOS DATA , we will get Ts, tid:%d\n",__func__, tid);
 		if(GetTs(
 				ieee,
 				(PTS_COMMON_INFO *) &pRxTS,
@@ -1037,7 +1034,7 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 				true))
 		{
 
-		//	IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): pRxTS->RxLastFragNum is %d,frag is %d,pRxTS->RxLastSeqNum is %d,seq is %d\n",__FUNCTION__,pRxTS->RxLastFragNum,frag,pRxTS->RxLastSeqNum,WLAN_GET_SEQ_SEQ(sc));
+		//	IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): pRxTS->RxLastFragNum is %d,frag is %d,pRxTS->RxLastSeqNum is %d,seq is %d\n",__func__,pRxTS->RxLastFragNum,frag,pRxTS->RxLastSeqNum,WLAN_GET_SEQ_SEQ(sc));
 			if(	(fc & (1<<11))  &&
 					(frag == pRxTS->RxLastFragNum) &&
 					(WLAN_GET_SEQ_SEQ(sc) == pRxTS->RxLastSeqNum)	)
@@ -1052,7 +1049,7 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 		}
 		else
 		{
-			IEEE80211_DEBUG(IEEE80211_DL_ERR, "%s(): No TS!! Skip the check!!\n",__FUNCTION__);
+			IEEE80211_DEBUG(IEEE80211_DL_ERR, "%s(): No TS!! Skip the check!!\n",__func__);
 			goto rx_dropped;
 		}
 	}
@@ -1297,7 +1294,7 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 	rxb = kmalloc(sizeof(struct ieee80211_rxb), GFP_ATOMIC);
 	if(rxb == NULL)
 	{
-		IEEE80211_DEBUG(IEEE80211_DL_ERR,"%s(): kmalloc rxb error\n",__FUNCTION__);
+		IEEE80211_DEBUG(IEEE80211_DL_ERR,"%s(): kmalloc rxb error\n",__func__);
 		goto rx_dropped;
 	}
 	/* to parse amsdu packets */
@@ -1363,7 +1360,7 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 	}
 	else
 	{
-		IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): REORDER ENABLE AND PTS not NULL, and we will enter RxReorderIndicatePacket()\n",__FUNCTION__);
+		IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): REORDER ENABLE AND PTS not NULL, and we will enter RxReorderIndicatePacket()\n",__func__);
 		RxReorderIndicatePacket(ieee, rxb, pTS, SeqNum);
 	}
 #ifndef JOHN_NOCPY
@@ -1387,6 +1384,7 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 	 * hardware as a DMA target */
 	return 0;
 }
+EXPORT_SYMBOL(ieee80211_rx);
 
 #define MGMT_FRAME_FIXED_PART_LENGTH            0x24
 
@@ -1479,7 +1477,6 @@ static int ieee80211_qos_convert_ac_to_parameters(struct
 						  ieee80211_qos_parameters
 						  *qos_param)
 {
-	int rc = 0;
 	int i;
 	struct ieee80211_qos_ac_parameter *ac_params;
 	u8 aci;
@@ -1506,7 +1503,7 @@ static int ieee80211_qos_convert_ac_to_parameters(struct
 		    (ac_params->aci_aifsn & 0x10) ? 0x01 : 0x00;
 		qos_param->tx_op_limit[aci] = le16_to_cpu(ac_params->tx_op_limit);
 	}
-	return rc;
+	return 0;
 }
 
 /*
@@ -1929,7 +1926,7 @@ int ieee80211_parse_info_param(struct ieee80211_device *ieee,
 				info_element->data[1] == 0x13 &&
 				info_element->data[2] == 0x74))
 			{
-				printk("========>%s(): athros AP is exist\n",__FUNCTION__);
+				printk("========>%s(): athros AP is exist\n",__func__);
 				network->atheros_cap_exist = true;
 			}
 			else
@@ -2643,6 +2640,4 @@ void ieee80211_rx_mgt(struct ieee80211_device *ieee,
 
 	}
 }
-
 EXPORT_SYMBOL(ieee80211_rx_mgt);
-EXPORT_SYMBOL(ieee80211_rx);
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
index 2131912113d..c5a0a5da97d 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
@@ -28,11 +28,13 @@ short ieee80211_is_54g(const struct ieee80211_network *net)
 {
 	return (net->rates_ex_len > 0) || (net->rates_len > 4);
 }
+EXPORT_SYMBOL(ieee80211_is_54g);
 
 short ieee80211_is_shortslot(const struct ieee80211_network *net)
 {
 	return net->capability & WLAN_CAPABILITY_SHORT_SLOT;
 }
+EXPORT_SYMBOL(ieee80211_is_shortslot);
 
 /* returns the total length needed for pleacing the RATE MFIE
  * tag and the EXTENDED RATE MFIE tag if needed.
@@ -271,7 +273,7 @@ inline void softmac_mgmt_xmit(struct sk_buff *skb, struct ieee80211_device *ieee
 			/* as for the completion function, it does not need
 			 * to check it any more.
 			 * */
-			printk("%s():insert to waitqueue!\n",__FUNCTION__);
+			printk("%s():insert to waitqueue!\n",__func__);
 			skb_queue_tail(&ieee->skb_waitQ[tcb_desc->queue_index], skb);
 		} else {
 			//printk("TX packet!\n");
@@ -415,6 +417,7 @@ void ieee80211_send_probe_requests(struct ieee80211_device *ieee)
 		ieee80211_send_probe(ieee);
 	}
 }
+EXPORT_SYMBOL(ieee80211_send_probe_requests);
 
 /* this performs syncro scan blocking the caller until all channels
  * in the allowed channel map has been checked.
@@ -485,7 +488,7 @@ out:
 	up(&ieee->scan_sem);
 }
 }
-
+EXPORT_SYMBOL(ieee80211_softmac_scan_syncro);
 
 static void ieee80211_softmac_scan_wq(struct work_struct *work)
 {
@@ -563,7 +566,7 @@ void ieee80211_stop_send_beacons(struct ieee80211_device *ieee)
 	if (ieee->softmac_features & IEEE_SOFTMAC_BEACONS)
 		ieee80211_beacons_stop(ieee);
 }
-
+EXPORT_SYMBOL(ieee80211_stop_send_beacons);
 
 void ieee80211_start_send_beacons(struct ieee80211_device *ieee)
 {
@@ -572,7 +575,7 @@ void ieee80211_start_send_beacons(struct ieee80211_device *ieee)
 	if(ieee->softmac_features & IEEE_SOFTMAC_BEACONS)
 		ieee80211_beacons_start(ieee);
 }
-
+EXPORT_SYMBOL(ieee80211_start_send_beacons);
 
 static void ieee80211_softmac_stop_scan(struct ieee80211_device *ieee)
 {
@@ -600,6 +603,7 @@ void ieee80211_stop_scan(struct ieee80211_device *ieee)
 	else
 		ieee->stop_scan(ieee->dev);
 }
+EXPORT_SYMBOL(ieee80211_stop_scan);
 
 /* called with ieee->lock held */
 static void ieee80211_start_scan(struct ieee80211_device *ieee)
@@ -638,6 +642,7 @@ void ieee80211_start_scan_syncro(struct ieee80211_device *ieee)
 		ieee->scan_syncro(ieee->dev);
 
 }
+EXPORT_SYMBOL(ieee80211_start_scan_syncro);
 
 inline struct sk_buff *ieee80211_authentication_req(struct ieee80211_network *beacon,
 	struct ieee80211_device *ieee, int challengelen)
@@ -670,7 +675,7 @@ inline struct sk_buff *ieee80211_authentication_req(struct ieee80211_network *be
 		auth->algorithm = WLAN_AUTH_SHARED_KEY;
 	else if(ieee->auth_mode == 2)
 		auth->algorithm = WLAN_AUTH_OPEN;//0x80;
-	printk("=================>%s():auth->algorithm is %d\n",__FUNCTION__,auth->algorithm);
+	printk("=================>%s():auth->algorithm is %d\n",__func__,auth->algorithm);
 	auth->transaction = cpu_to_le16(ieee->associate_seq);
 	ieee->associate_seq++;
 
@@ -1212,7 +1217,7 @@ inline struct sk_buff *ieee80211_association_req(struct ieee80211_network *beaco
 			memcpy(tag, realtek_ie_buf,realtek_ie_len -2 );
 		}
 	}
-//	printk("<=====%s(), %p, %p\n", __FUNCTION__, ieee->dev, ieee->dev->dev_addr);
+//	printk("<=====%s(), %p, %p\n", __func__, ieee->dev, ieee->dev->dev_addr);
 //	IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA, skb->data, skb->len);
 	return skb;
 }
@@ -1398,7 +1403,7 @@ static void ieee80211_associate_procedure_wq(struct work_struct *work)
 		ieee->data_hard_stop(ieee->dev);
 
 	ieee80211_stop_scan(ieee);
-	printk("===>%s(), chan:%d\n", __FUNCTION__, ieee->current_network.channel);
+	printk("===>%s(), chan:%d\n", __func__, ieee->current_network.channel);
 	//ieee->set_chan(ieee->dev, ieee->current_network.channel);
 	HTSetConnectBwMode(ieee, HT_CHANNEL_WIDTH_20, HT_EXTCHNL_OFFSET_NO_EXT);
 
@@ -1709,8 +1714,6 @@ ieee80211_rx_assoc_rq(struct ieee80211_device *ieee, struct sk_buff *skb)
 	//FIXME
 }
 
-
-
 static void ieee80211_sta_ps_send_null_frame(struct ieee80211_device *ieee,
 					     short pwr)
 {
@@ -1721,7 +1724,7 @@ static void ieee80211_sta_ps_send_null_frame(struct ieee80211_device *ieee,
 		softmac_ps_mgmt_xmit(buf, ieee);
 
 }
-
+/* EXPORT_SYMBOL(ieee80211_sta_ps_send_null_frame); */
 
 static short ieee80211_sta_ps_sleep(struct ieee80211_device *ieee, u32 *time_h,
 				    u32 *time_l)
@@ -1889,6 +1892,8 @@ void ieee80211_ps_tx_ack(struct ieee80211_device *ieee, short success)
 	}
 	spin_unlock_irqrestore(&ieee->lock, flags);
 }
+EXPORT_SYMBOL(ieee80211_ps_tx_ack);
+
 static void ieee80211_process_action(struct ieee80211_device *ieee,
 				     struct sk_buff *skb)
 {
@@ -1903,20 +1908,17 @@ static void ieee80211_process_action(struct ieee80211_device *ieee,
 	}
 	tmp = *act;
 	act ++;
-	switch (tmp)
-	{
-		case ACT_CAT_BA:
-			if (*act == ACT_ADDBAREQ)
+	switch (tmp) {
+	case ACT_CAT_BA:
+		if (*act == ACT_ADDBAREQ)
 			ieee80211_rx_ADDBAReq(ieee, skb);
-			else if (*act == ACT_ADDBARSP)
+		else if (*act == ACT_ADDBARSP)
 			ieee80211_rx_ADDBARsp(ieee, skb);
-			else if (*act == ACT_DELBA)
+		else if (*act == ACT_DELBA)
 			ieee80211_rx_DELBA(ieee, skb);
-			break;
-		default:
-//			if (net_ratelimit())
-//			IEEE80211_DEBUG(IEEE80211_DL_BA, "unknown action frame(%d)\n", tmp);
-			break;
+		break;
+	default:
+		break;
 	}
 	return;
 
@@ -2191,6 +2193,7 @@ void ieee80211_softmac_xmit(struct ieee80211_txb *txb, struct ieee80211_device *
 	spin_unlock_irqrestore(&ieee->lock,flags);
 
 }
+EXPORT_SYMBOL(ieee80211_softmac_xmit);
 
 /* called with ieee->lock acquired */
 static void ieee80211_resume_tx(struct ieee80211_device *ieee)
@@ -2232,6 +2235,7 @@ void ieee80211_reset_queue(struct ieee80211_device *ieee)
 	spin_unlock_irqrestore(&ieee->lock,flags);
 
 }
+EXPORT_SYMBOL(ieee80211_reset_queue);
 
 void ieee80211_wake_queue(struct ieee80211_device *ieee)
 {
@@ -2272,7 +2276,7 @@ void ieee80211_wake_queue(struct ieee80211_device *ieee)
 exit :
 	spin_unlock_irqrestore(&ieee->lock,flags);
 }
-
+EXPORT_SYMBOL(ieee80211_wake_queue);
 
 void ieee80211_stop_queue(struct ieee80211_device *ieee)
 {
@@ -2287,7 +2291,7 @@ void ieee80211_stop_queue(struct ieee80211_device *ieee)
 	//spin_unlock_irqrestore(&ieee->lock,flags);
 
 }
-
+EXPORT_SYMBOL(ieee80211_stop_queue);
 
 inline void ieee80211_randomize_cell(struct ieee80211_device *ieee)
 {
@@ -2505,6 +2509,8 @@ void ieee80211_disassociate(struct ieee80211_device *ieee)
 	notify_wx_assoc_event(ieee);
 
 }
+EXPORT_SYMBOL(ieee80211_disassociate);
+
 static void ieee80211_associate_retry_wq(struct work_struct *work)
 {
 	struct delayed_work *dwork = container_of(work, struct delayed_work, work);
@@ -2584,6 +2590,7 @@ struct sk_buff *ieee80211_get_beacon(struct ieee80211_device *ieee)
 
 	return skb;
 }
+EXPORT_SYMBOL(ieee80211_get_beacon);
 
 void ieee80211_softmac_stop_protocol(struct ieee80211_device *ieee)
 {
@@ -2592,7 +2599,7 @@ void ieee80211_softmac_stop_protocol(struct ieee80211_device *ieee)
 	ieee80211_stop_protocol(ieee);
 	up(&ieee->wx_sem);
 }
-
+EXPORT_SYMBOL(ieee80211_softmac_stop_protocol);
 
 void ieee80211_stop_protocol(struct ieee80211_device *ieee)
 {
@@ -2618,6 +2625,7 @@ void ieee80211_softmac_start_protocol(struct ieee80211_device *ieee)
 	ieee80211_start_protocol(ieee);
 	up(&ieee->wx_sem);
 }
+EXPORT_SYMBOL(ieee80211_softmac_start_protocol);
 
 void ieee80211_start_protocol(struct ieee80211_device *ieee)
 {
@@ -2639,7 +2647,7 @@ void ieee80211_start_protocol(struct ieee80211_device *ieee)
 
 	if (ieee->current_network.beacon_interval == 0)
 		ieee->current_network.beacon_interval = 100;
-//	printk("===>%s(), chan:%d\n", __FUNCTION__, ieee->current_network.channel);
+//	printk("===>%s(), chan:%d\n", __func__, ieee->current_network.channel);
 //	ieee->set_chan(ieee->dev,ieee->current_network.channel);
 
 	for(i = 0; i < 17; i++) {
@@ -2846,7 +2854,6 @@ static int ieee80211_wpa_set_auth_algs(struct ieee80211_device *ieee, int value)
 	struct ieee80211_security sec = {
 		.flags = SEC_AUTH_MODE,
 	};
-	int ret = 0;
 
 	if (value & AUTH_ALG_SHARED_KEY) {
 		sec.auth_mode = WLAN_AUTH_SHARED_KEY;
@@ -2869,7 +2876,7 @@ static int ieee80211_wpa_set_auth_algs(struct ieee80211_device *ieee, int value)
 	//else
 	//	ret = -EOPNOTSUPP;
 
-	return ret;
+	return 0;
 }
 
 static int ieee80211_wpa_set_param(struct ieee80211_device *ieee, u8 name, u32 value)
@@ -3141,6 +3148,7 @@ SendDisassociation(
 				//dev_kfree_skb_any(skb);//edit by thomas
 		}
 }
+EXPORT_SYMBOL(SendDisassociation);
 
 int ieee80211_wpa_supplicant_ioctl(struct ieee80211_device *ieee, struct iw_point *p)
 {
@@ -3196,6 +3204,7 @@ out:
 
 	return ret;
 }
+EXPORT_SYMBOL(ieee80211_wpa_supplicant_ioctl);
 
 void notify_wx_assoc_event(struct ieee80211_device *ieee)
 {
@@ -3207,25 +3216,4 @@ void notify_wx_assoc_event(struct ieee80211_device *ieee)
 		memset(wrqu.ap_addr.sa_data, 0, ETH_ALEN);
 	wireless_send_event(ieee->dev, SIOCGIWAP, &wrqu, NULL);
 }
-
-EXPORT_SYMBOL(ieee80211_get_beacon);
-EXPORT_SYMBOL(ieee80211_wake_queue);
-EXPORT_SYMBOL(ieee80211_stop_queue);
-EXPORT_SYMBOL(ieee80211_reset_queue);
-EXPORT_SYMBOL(ieee80211_softmac_stop_protocol);
-EXPORT_SYMBOL(ieee80211_softmac_start_protocol);
-EXPORT_SYMBOL(ieee80211_is_shortslot);
-EXPORT_SYMBOL(ieee80211_is_54g);
-EXPORT_SYMBOL(ieee80211_wpa_supplicant_ioctl);
-EXPORT_SYMBOL(ieee80211_ps_tx_ack);
-EXPORT_SYMBOL(ieee80211_softmac_xmit);
-EXPORT_SYMBOL(ieee80211_stop_send_beacons);
 EXPORT_SYMBOL(notify_wx_assoc_event);
-EXPORT_SYMBOL(SendDisassociation);
-EXPORT_SYMBOL(ieee80211_disassociate);
-EXPORT_SYMBOL(ieee80211_start_send_beacons);
-EXPORT_SYMBOL(ieee80211_stop_scan);
-EXPORT_SYMBOL(ieee80211_send_probe_requests);
-EXPORT_SYMBOL(ieee80211_softmac_scan_syncro);
-EXPORT_SYMBOL(ieee80211_start_scan_syncro);
-//EXPORT_SYMBOL(ieee80211_sta_ps_send_null_frame);
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac_wx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac_wx.c
index 7b7d929f153..935a8f4adcf 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac_wx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac_wx.c
@@ -26,7 +26,7 @@ const long ieee80211_wlan_frequencies[] = {
 	2452, 2457, 2462, 2467,
 	2472, 2484
 };
-
+EXPORT_SYMBOL(ieee80211_wlan_frequencies);
 
 int ieee80211_wx_set_freq(struct ieee80211_device *ieee, struct iw_request_info *a,
 			     union iwreq_data *wrqu, char *b)
@@ -83,7 +83,7 @@ out:
 	up(&ieee->wx_sem);
 	return ret;
 }
-
+EXPORT_SYMBOL(ieee80211_wx_set_freq);
 
 int ieee80211_wx_get_freq(struct ieee80211_device *ieee,
 			     struct iw_request_info *a,
@@ -101,6 +101,7 @@ int ieee80211_wx_get_freq(struct ieee80211_device *ieee,
 
 	return 0;
 }
+EXPORT_SYMBOL(ieee80211_wx_get_freq);
 
 int ieee80211_wx_get_wap(struct ieee80211_device *ieee,
 			    struct iw_request_info *info,
@@ -129,7 +130,7 @@ int ieee80211_wx_get_wap(struct ieee80211_device *ieee,
 
 	return 0;
 }
-
+EXPORT_SYMBOL(ieee80211_wx_get_wap);
 
 int ieee80211_wx_set_wap(struct ieee80211_device *ieee,
 			 struct iw_request_info *info,
@@ -176,6 +177,7 @@ out:
 	up(&ieee->wx_sem);
 	return ret;
 }
+EXPORT_SYMBOL(ieee80211_wx_set_wap);
 
  int ieee80211_wx_get_essid(struct ieee80211_device *ieee, struct iw_request_info *a,union iwreq_data *wrqu,char *b)
 {
@@ -211,6 +213,7 @@ out:
 	return ret;
 
 }
+EXPORT_SYMBOL(ieee80211_wx_get_essid);
 
 int ieee80211_wx_set_rate(struct ieee80211_device *ieee,
 			     struct iw_request_info *info,
@@ -223,8 +226,7 @@ int ieee80211_wx_set_rate(struct ieee80211_device *ieee,
 	//FIXME: we might want to limit rate also in management protocols.
 	return 0;
 }
-
-
+EXPORT_SYMBOL(ieee80211_wx_set_rate);
 
 int ieee80211_wx_get_rate(struct ieee80211_device *ieee,
 			     struct iw_request_info *info,
@@ -237,7 +239,7 @@ int ieee80211_wx_get_rate(struct ieee80211_device *ieee,
 
 	return 0;
 }
-
+EXPORT_SYMBOL(ieee80211_wx_get_rate);
 
 int ieee80211_wx_set_rts(struct ieee80211_device *ieee,
 			     struct iw_request_info *info,
@@ -254,6 +256,7 @@ int ieee80211_wx_set_rts(struct ieee80211_device *ieee,
 	}
 	return 0;
 }
+EXPORT_SYMBOL(ieee80211_wx_set_rts);
 
 int ieee80211_wx_get_rts(struct ieee80211_device *ieee,
 			     struct iw_request_info *info,
@@ -264,6 +267,8 @@ int ieee80211_wx_get_rts(struct ieee80211_device *ieee,
 	wrqu->rts.disabled = (wrqu->rts.value == DEFAULT_RTS_THRESHOLD);
 	return 0;
 }
+EXPORT_SYMBOL(ieee80211_wx_get_rts);
+
 int ieee80211_wx_set_mode(struct ieee80211_device *ieee, struct iw_request_info *a,
 			     union iwreq_data *wrqu, char *b)
 {
@@ -294,6 +299,7 @@ out:
 	up(&ieee->wx_sem);
 	return 0;
 }
+EXPORT_SYMBOL(ieee80211_wx_set_mode);
 
 void ieee80211_wx_sync_scan_wq(struct work_struct *work)
 {
@@ -378,6 +384,7 @@ out:
 	up(&ieee->wx_sem);
 	return ret;
 }
+EXPORT_SYMBOL(ieee80211_wx_set_scan);
 
 int ieee80211_wx_set_essid(struct ieee80211_device *ieee,
 			      struct iw_request_info *a,
@@ -432,6 +439,7 @@ out:
 	up(&ieee->wx_sem);
 	return ret;
 }
+EXPORT_SYMBOL(ieee80211_wx_set_essid);
 
  int ieee80211_wx_get_mode(struct ieee80211_device *ieee, struct iw_request_info *a,
 			     union iwreq_data *wrqu, char *b)
@@ -440,6 +448,7 @@ out:
 	wrqu->mode = ieee->iw_mode;
 	return 0;
 }
+EXPORT_SYMBOL(ieee80211_wx_get_mode);
 
  int ieee80211_wx_set_rawtx(struct ieee80211_device *ieee,
 			       struct iw_request_info *info,
@@ -477,6 +486,7 @@ out:
 
 	return 0;
 }
+EXPORT_SYMBOL(ieee80211_wx_set_rawtx);
 
 int ieee80211_wx_get_name(struct ieee80211_device *ieee,
 			     struct iw_request_info *info,
@@ -502,7 +512,7 @@ int ieee80211_wx_get_name(struct ieee80211_device *ieee,
 
 	return 0;
 }
-
+EXPORT_SYMBOL(ieee80211_wx_get_name);
 
 /* this is mostly stolen from hostap */
 int ieee80211_wx_set_power(struct ieee80211_device *ieee,
@@ -553,14 +563,13 @@ exit:
 	return ret;
 
 }
+EXPORT_SYMBOL(ieee80211_wx_set_power);
 
 /* this is stolen from hostap */
 int ieee80211_wx_get_power(struct ieee80211_device *ieee,
 				 struct iw_request_info *info,
 				 union iwreq_data *wrqu, char *extra)
 {
-	int ret =0;
-
 	down(&ieee->wx_sem);
 
 	if(ieee->ps == IEEE80211_PS_DISABLED){
@@ -590,24 +599,7 @@ int ieee80211_wx_get_power(struct ieee80211_device *ieee,
 
 exit:
 	up(&ieee->wx_sem);
-	return ret;
+	return 0;
 
 }
-EXPORT_SYMBOL(ieee80211_wx_get_essid);
-EXPORT_SYMBOL(ieee80211_wx_set_essid);
-EXPORT_SYMBOL(ieee80211_wx_set_rate);
-EXPORT_SYMBOL(ieee80211_wx_get_rate);
-EXPORT_SYMBOL(ieee80211_wx_set_wap);
-EXPORT_SYMBOL(ieee80211_wx_get_wap);
-EXPORT_SYMBOL(ieee80211_wx_set_mode);
-EXPORT_SYMBOL(ieee80211_wx_get_mode);
-EXPORT_SYMBOL(ieee80211_wx_set_scan);
-EXPORT_SYMBOL(ieee80211_wx_get_freq);
-EXPORT_SYMBOL(ieee80211_wx_set_freq);
-EXPORT_SYMBOL(ieee80211_wx_set_rawtx);
-EXPORT_SYMBOL(ieee80211_wx_get_name);
-EXPORT_SYMBOL(ieee80211_wx_set_power);
 EXPORT_SYMBOL(ieee80211_wx_get_power);
-EXPORT_SYMBOL(ieee80211_wlan_frequencies);
-EXPORT_SYMBOL(ieee80211_wx_set_rts);
-EXPORT_SYMBOL(ieee80211_wx_get_rts);
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
index 6779bdd561d..029a9765179 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
@@ -188,7 +188,7 @@ int ieee80211_encrypt_fragment(
 
 	if (!(crypt && crypt->ops))
 	{
-		printk("=========>%s(), crypt is null\n", __FUNCTION__);
+		printk("=========>%s(), crypt is null\n", __func__);
 		return -1;
 	}
 #ifdef CONFIG_IEEE80211_CRYPT_TKIP
@@ -236,6 +236,7 @@ void ieee80211_txb_free(struct ieee80211_txb *txb) {
 		return;
 	kfree(txb);
 }
+EXPORT_SYMBOL(ieee80211_txb_free);
 
 static struct ieee80211_txb *ieee80211_alloc_txb(int nr_frags, int txb_size,
 						 gfp_t gfp_mask)
@@ -913,5 +914,3 @@ int ieee80211_xmit(struct sk_buff *skb, struct net_device *dev)
 	return 1;
 
 }
-
-EXPORT_SYMBOL(ieee80211_txb_free);
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_wx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_wx.c
index bdf67ec5df2..698507e00f3 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_wx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_wx.c
@@ -194,7 +194,7 @@ static inline char *rtl819x_translate_scan(struct ieee80211_device *ieee,
 	iwe.u.data.length = p - custom;
 	if (iwe.u.data.length)
 	    start = iwe_stream_add_point(info, start, stop, &iwe, custom);
-#if (WIRELESS_EXT < 18)
+
 	if (ieee->wpa_enabled && network->wpa_ie_len){
 		char buf[MAX_WPA_IE_LEN * 2 + 30];
 	//	printk("WPA IE\n");
@@ -224,26 +224,6 @@ static inline char *rtl819x_translate_scan(struct ieee80211_device *ieee,
 		iwe.u.data.length = strlen(buf);
 		start = iwe_stream_add_point(info, start, stop, &iwe, buf);
 	}
-#else
-	memset(&iwe, 0, sizeof(iwe));
-	if (network->wpa_ie_len)
-	{
-		char buf[MAX_WPA_IE_LEN];
-		memcpy(buf, network->wpa_ie, network->wpa_ie_len);
-		iwe.cmd = IWEVGENIE;
-		iwe.u.data.length = network->wpa_ie_len;
-		start = iwe_stream_add_point(info, start, stop, &iwe, buf);
-	}
-	memset(&iwe, 0, sizeof(iwe));
-	if (network->rsn_ie_len)
-	{
-		char buf[MAX_WPA_IE_LEN];
-		memcpy(buf, network->rsn_ie, network->rsn_ie_len);
-		iwe.cmd = IWEVGENIE;
-		iwe.u.data.length = network->rsn_ie_len;
-		start = iwe_stream_add_point(info, start, stop, &iwe, buf);
-	}
-#endif
 
 
 	/* Add EXTRA: Age to display seconds since last beacon/probe response
@@ -305,6 +285,7 @@ int ieee80211_wx_get_scan(struct ieee80211_device *ieee,
 
 	return err;
 }
+EXPORT_SYMBOL(ieee80211_wx_get_scan);
 
 int ieee80211_wx_set_encode(struct ieee80211_device *ieee,
 			    struct iw_request_info *info,
@@ -479,6 +460,7 @@ int ieee80211_wx_set_encode(struct ieee80211_device *ieee,
 	}
 	return 0;
 }
+EXPORT_SYMBOL(ieee80211_wx_set_encode);
 
 int ieee80211_wx_get_encode(struct ieee80211_device *ieee,
 			    struct iw_request_info *info,
@@ -521,7 +503,8 @@ int ieee80211_wx_get_encode(struct ieee80211_device *ieee,
 
 	return 0;
 }
-#if (WIRELESS_EXT >= 18)
+EXPORT_SYMBOL(ieee80211_wx_get_encode);
+
 int ieee80211_wx_set_encode_ext(struct ieee80211_device *ieee,
 			       struct iw_request_info *info,
 			       union iwreq_data *wrqu, char *extra)
@@ -695,6 +678,7 @@ done:
 	}
 	return ret;
 }
+EXPORT_SYMBOL(ieee80211_wx_set_encode_ext);
 
 int ieee80211_wx_get_encode_ext(struct ieee80211_device *ieee,
 			       struct iw_request_info *info,
@@ -750,6 +734,7 @@ int ieee80211_wx_get_encode_ext(struct ieee80211_device *ieee,
 
 	return 0;
 }
+EXPORT_SYMBOL(ieee80211_wx_get_encode_ext);
 
 int ieee80211_wx_set_mlme(struct ieee80211_device *ieee,
 			       struct iw_request_info *info,
@@ -766,6 +751,7 @@ int ieee80211_wx_set_mlme(struct ieee80211_device *ieee,
 	}
 	return 0;
 }
+EXPORT_SYMBOL(ieee80211_wx_set_mlme);
 
 int ieee80211_wx_set_auth(struct ieee80211_device *ieee,
 			       struct iw_request_info *info,
@@ -792,7 +778,7 @@ int ieee80211_wx_set_auth(struct ieee80211_device *ieee,
 		break;
 
 	case IW_AUTH_80211_AUTH_ALG:
-		//printk("======>%s():data->value is %d\n",__FUNCTION__,data->value);
+		//printk("======>%s():data->value is %d\n",__func__,data->value);
 	//	ieee->open_wep = (data->value&IW_AUTH_ALG_OPEN_SYSTEM)?1:0;
 		if(data->value & IW_AUTH_ALG_SHARED_KEY){
 			ieee->open_wep = 0;
@@ -828,7 +814,8 @@ int ieee80211_wx_set_auth(struct ieee80211_device *ieee,
 	}
 	return 0;
 }
-#endif
+EXPORT_SYMBOL(ieee80211_wx_set_auth);
+
 int ieee80211_wx_set_gen_ie(struct ieee80211_device *ieee, u8 *ie, size_t len)
 {
 	u8 *buf;
@@ -862,14 +849,4 @@ int ieee80211_wx_set_gen_ie(struct ieee80211_device *ieee, u8 *ie, size_t len)
 	return 0;
 
 }
-
 EXPORT_SYMBOL(ieee80211_wx_set_gen_ie);
-#if (WIRELESS_EXT >= 18)
-EXPORT_SYMBOL(ieee80211_wx_set_mlme);
-EXPORT_SYMBOL(ieee80211_wx_set_auth);
-EXPORT_SYMBOL(ieee80211_wx_set_encode_ext);
-EXPORT_SYMBOL(ieee80211_wx_get_encode_ext);
-#endif
-EXPORT_SYMBOL(ieee80211_wx_get_scan);
-EXPORT_SYMBOL(ieee80211_wx_set_encode);
-EXPORT_SYMBOL(ieee80211_wx_get_encode);
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c b/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c
index 7bf55e39355..b8cc6a537c3 100644
--- a/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c
@@ -113,7 +113,7 @@ static struct sk_buff *ieee80211_ADDBA(struct ieee80211_device *ieee, u8 *Dst, P
 	u16 tmp = 0;
 	u16 len = ieee->tx_headroom + 9;
 	//category(1) + action field(1) + Dialog Token(1) + BA Parameter Set(2) +  BA Timeout Value(2) +  BA Start SeqCtrl(2)(or StatusCode(2))
-	IEEE80211_DEBUG(IEEE80211_DL_TRACE | IEEE80211_DL_BA, "========>%s(), frame(%d) sentd to:%pM, ieee->dev:%p\n", __FUNCTION__, type, Dst, ieee->dev);
+	IEEE80211_DEBUG(IEEE80211_DL_TRACE | IEEE80211_DL_BA, "========>%s(), frame(%d) sentd to:%pM, ieee->dev:%p\n", __func__, type, Dst, ieee->dev);
 	if (pBA == NULL||ieee == NULL)
 	{
 		IEEE80211_DEBUG(IEEE80211_DL_ERR, "pBA(%p) is NULL or ieee(%p) is NULL\n", pBA, ieee);
@@ -201,7 +201,7 @@ static struct sk_buff *ieee80211_DELBA(
 	u16 len = 6 + ieee->tx_headroom;
 
 	if (net_ratelimit())
-	IEEE80211_DEBUG(IEEE80211_DL_TRACE | IEEE80211_DL_BA, "========>%s(), ReasonCode(%d) sentd to:%pM\n", __FUNCTION__, ReasonCode, dst);
+	IEEE80211_DEBUG(IEEE80211_DL_TRACE | IEEE80211_DL_BA, "========>%s(), ReasonCode(%d) sentd to:%pM\n", __func__, ReasonCode, dst);
 
 	memset(&DelbaParamSet, 0, 2);
 
@@ -240,7 +240,7 @@ static struct sk_buff *ieee80211_DELBA(
 
 	IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA|IEEE80211_DL_BA, skb->data, skb->len);
 	if (net_ratelimit())
-	IEEE80211_DEBUG(IEEE80211_DL_TRACE | IEEE80211_DL_BA, "<=====%s()\n", __FUNCTION__);
+	IEEE80211_DEBUG(IEEE80211_DL_TRACE | IEEE80211_DL_BA, "<=====%s()\n", __func__);
 	return skb;
 }
 
@@ -266,7 +266,7 @@ static void ieee80211_send_ADDBAReq(struct ieee80211_device *ieee,
 	}
 	else
 	{
-		IEEE80211_DEBUG(IEEE80211_DL_ERR, "alloc skb error in function %s()\n", __FUNCTION__);
+		IEEE80211_DEBUG(IEEE80211_DL_ERR, "alloc skb error in function %s()\n", __func__);
 	}
 	return;
 }
@@ -291,7 +291,7 @@ static void ieee80211_send_ADDBARsp(struct ieee80211_device *ieee, u8 *dst,
 	}
 	else
 	{
-		IEEE80211_DEBUG(IEEE80211_DL_ERR, "alloc skb error in function %s()\n", __FUNCTION__);
+		IEEE80211_DEBUG(IEEE80211_DL_ERR, "alloc skb error in function %s()\n", __func__);
 	}
 
 	return;
@@ -320,7 +320,7 @@ static void ieee80211_send_DELBA(struct ieee80211_device *ieee, u8 *dst,
 	}
 	else
 	{
-		IEEE80211_DEBUG(IEEE80211_DL_ERR, "alloc skb error in function %s()\n", __FUNCTION__);
+		IEEE80211_DEBUG(IEEE80211_DL_ERR, "alloc skb error in function %s()\n", __func__);
 	}
 	return ;
 }
@@ -380,7 +380,7 @@ int ieee80211_rx_ADDBAReq( struct ieee80211_device *ieee, struct sk_buff *skb)
 			true)	)
 	{
 		rc = ADDBA_STATUS_REFUSED;
-		IEEE80211_DEBUG(IEEE80211_DL_ERR, "can't get TS in %s()\n", __FUNCTION__);
+		IEEE80211_DEBUG(IEEE80211_DL_ERR, "can't get TS in %s()\n", __func__);
 		goto OnADDBAReq_Fail;
 	}
 	pBA = &pTS->RxAdmittedBARecord;
@@ -391,7 +391,7 @@ int ieee80211_rx_ADDBAReq( struct ieee80211_device *ieee, struct sk_buff *skb)
 	if(pBaParamSet->field.BAPolicy == BA_POLICY_DELAYED)
 	{
 		rc = ADDBA_STATUS_INVALID_PARAM;
-		IEEE80211_DEBUG(IEEE80211_DL_ERR, "BA Policy is not correct in %s()\n", __FUNCTION__);
+		IEEE80211_DEBUG(IEEE80211_DL_ERR, "BA Policy is not correct in %s()\n", __func__);
 		goto OnADDBAReq_Fail;
 	}
 		// Admit the ADDBA Request
@@ -479,7 +479,7 @@ int ieee80211_rx_ADDBARsp( struct ieee80211_device *ieee, struct sk_buff *skb)
 			TX_DIR,
 			false)	)
 	{
-		IEEE80211_DEBUG(IEEE80211_DL_ERR, "can't get TS in %s()\n", __FUNCTION__);
+		IEEE80211_DEBUG(IEEE80211_DL_ERR, "can't get TS in %s()\n", __func__);
 		ReasonCode = DELBA_REASON_UNKNOWN_BA;
 		goto OnADDBARsp_Reject;
 	}
@@ -603,7 +603,7 @@ int ieee80211_rx_DELBA(struct ieee80211_device *ieee,struct sk_buff *skb)
 				RX_DIR,
 				false)	)
 		{
-			IEEE80211_DEBUG(IEEE80211_DL_ERR,  "can't get TS for RXTS in %s()\n", __FUNCTION__);
+			IEEE80211_DEBUG(IEEE80211_DL_ERR,  "can't get TS for RXTS in %s()\n", __func__);
 			return -1;
 		}
 
@@ -621,7 +621,7 @@ int ieee80211_rx_DELBA(struct ieee80211_device *ieee,struct sk_buff *skb)
 			TX_DIR,
 			false)	)
 		{
-			IEEE80211_DEBUG(IEEE80211_DL_ERR,  "can't get TS for TXTS in %s()\n", __FUNCTION__);
+			IEEE80211_DEBUG(IEEE80211_DL_ERR,  "can't get TS for TXTS in %s()\n", __func__);
 			return -1;
 		}
 
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c b/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c
index 53ec2d435ff..2694a085e3a 100644
--- a/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c
@@ -130,7 +130,7 @@ void HTDebugHTCapability(u8 *CapIE, u8 *TitleString )
 	if(!memcmp(CapIE, EWC11NHTCap, sizeof(EWC11NHTCap)))
 	{
 		//EWC IE
-		IEEE80211_DEBUG(IEEE80211_DL_HT, "EWC IE in %s()\n", __FUNCTION__);
+		IEEE80211_DEBUG(IEEE80211_DL_HT, "EWC IE in %s()\n", __func__);
 		pCapELE = (PHT_CAPABILITY_ELE)(&CapIE[4]);
 	}else
 		pCapELE = (PHT_CAPABILITY_ELE)(&CapIE[0]);
@@ -167,7 +167,7 @@ void HTDebugHTInfo(u8 *InfoIE, u8 *TitleString)
 	if(!memcmp(InfoIE, EWC11NHTInfo, sizeof(EWC11NHTInfo)))
 	{
 		// Not EWC IE
-		IEEE80211_DEBUG(IEEE80211_DL_HT, "EWC IE in %s()\n", __FUNCTION__);
+		IEEE80211_DEBUG(IEEE80211_DL_HT, "EWC IE in %s()\n", __func__);
 		pHTInfoEle = (PHT_INFORMATION_ELE)(&InfoIE[4]);
 	}else
 		pHTInfoEle = (PHT_INFORMATION_ELE)(&InfoIE[0]);
@@ -415,8 +415,7 @@ static void HTIOTPeerDetermine(struct ieee80211_device *ieee)
   * *****************************************************************************************************************/
 static u8 HTIOTActIsDisableMCS14(struct ieee80211_device *ieee, u8 *PeerMacAddr)
 {
-	u8 ret = 0;
-	return ret;
+	return 0;
  }
 
 
@@ -1134,7 +1133,7 @@ void HTInitializeHTInfo(struct ieee80211_device *ieee)
 	//
 	// These parameters will be reset when receiving deauthentication packet
 	//
-	IEEE80211_DEBUG(IEEE80211_DL_HT, "===========>%s()\n", __FUNCTION__);
+	IEEE80211_DEBUG(IEEE80211_DL_HT, "===========>%s()\n", __func__);
 	pHTInfo->bCurrentHTSupport = false;
 
 	// 40MHz channel support
@@ -1229,7 +1228,7 @@ void HTResetSelfAndSavePeerSetting(struct ieee80211_device *ieee,	struct ieee802
 	//
 	//  Save Peer Setting before Association
 	//
-	IEEE80211_DEBUG(IEEE80211_DL_HT, "==============>%s()\n", __FUNCTION__);
+	IEEE80211_DEBUG(IEEE80211_DL_HT, "==============>%s()\n", __func__);
 	/*unmark bEnableHT flag here is the same reason why unmarked in function ieee80211_softmac_new_net. WB 2008.09.10*/
 //	if( pHTInfo->bEnableHT &&  pNetwork->bssht.bdSupportHT)
 	if (pNetwork->bssht.bdSupportHT)
@@ -1319,6 +1318,7 @@ void HTUpdateSelfAndPeerSetting(struct ieee80211_device *ieee,	struct ieee80211_
 		//
 	}
 }
+EXPORT_SYMBOL(HTUpdateSelfAndPeerSetting);
 
 /********************************************************************************************************************
  *function:  check whether HT control field exists
@@ -1397,7 +1397,7 @@ void HTSetConnectBwModeCallback(struct ieee80211_device *ieee)
 {
 	PRT_HIGH_THROUGHPUT pHTInfo = ieee->pHTInfo;
 
-	IEEE80211_DEBUG(IEEE80211_DL_HT, "======>%s()\n", __FUNCTION__);
+	IEEE80211_DEBUG(IEEE80211_DL_HT, "======>%s()\n", __func__);
 
 	if(pHTInfo->bCurBW40MHz)
 	{
@@ -1416,5 +1416,3 @@ void HTSetConnectBwModeCallback(struct ieee80211_device *ieee)
 
 	pHTInfo->bSwBwInProgress = false;
 }
-
-EXPORT_SYMBOL(HTUpdateSelfAndPeerSetting);
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_TSProc.c b/drivers/staging/rtl8192u/ieee80211/rtl819x_TSProc.c
index 426f223e6a2..c4514109d0e 100644
--- a/drivers/staging/rtl8192u/ieee80211/rtl819x_TSProc.c
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_TSProc.c
@@ -38,7 +38,7 @@ static void RxPktPendingTimeout(unsigned long data)
 
 	spin_lock_irqsave(&(ieee->reorder_spinlock), flags);
 	//PlatformAcquireSpinLock(Adapter, RT_RX_SPINLOCK);
-	IEEE80211_DEBUG(IEEE80211_DL_REORDER,"==================>%s()\n",__FUNCTION__);
+	IEEE80211_DEBUG(IEEE80211_DL_REORDER,"==================>%s()\n",__func__);
 	if(pRxTs->RxTimeoutIndicateSeq != 0xffff)
 	{
 		// Indicate the pending packets sequentially according to SeqNum until meet the gap.
@@ -144,7 +144,7 @@ void TSInitialize(struct ieee80211_device *ieee)
 	PRX_TS_RECORD		pRxTS  = ieee->RxTsRecord;
 	PRX_REORDER_ENTRY	pRxReorderEntry = ieee->RxReorderEntry;
 	u8				count = 0;
-	IEEE80211_DEBUG(IEEE80211_DL_TS, "==========>%s()\n", __FUNCTION__);
+	IEEE80211_DEBUG(IEEE80211_DL_TS, "==========>%s()\n", __func__);
 	// Initialize Tx TS related info.
 	INIT_LIST_HEAD(&ieee->Tx_TS_Admit_List);
 	INIT_LIST_HEAD(&ieee->Tx_TS_Pending_List);
@@ -361,7 +361,7 @@ bool GetTs(
 		// In WMM case: we use 4 TID only
 		if (!IsACValid(TID))
 		{
-			IEEE80211_DEBUG(IEEE80211_DL_ERR, " in %s(), TID(%d) is not valid\n", __FUNCTION__, TID);
+			IEEE80211_DEBUG(IEEE80211_DL_ERR, " in %s(), TID(%d) is not valid\n", __func__, TID);
 			return false;
 		}
 
@@ -463,7 +463,7 @@ bool GetTs(
 			}
 			else
 			{
-				IEEE80211_DEBUG(IEEE80211_DL_ERR, "in function %s() There is not enough TS record to be used!!", __FUNCTION__);
+				IEEE80211_DEBUG(IEEE80211_DL_ERR, "in function %s() There is not enough TS record to be used!!", __func__);
 				return false;
 			}
 		}
@@ -619,5 +619,5 @@ void TsStartAddBaProcess(struct ieee80211_device *ieee, PTX_TS_RECORD	pTxTS)
 		}
 	}
 	else
-		IEEE80211_DEBUG(IEEE80211_DL_ERR, "%s()==>BA timer is already added\n", __FUNCTION__);
+		IEEE80211_DEBUG(IEEE80211_DL_ERR, "%s()==>BA timer is already added\n", __func__);
 }
diff --git a/drivers/staging/rtl8192u/r8180_93cx6.c b/drivers/staging/rtl8192u/r8180_93cx6.c
index cd06054fbed..fb8a7a8ac64 100644
--- a/drivers/staging/rtl8192u/r8180_93cx6.c
+++ b/drivers/staging/rtl8192u/r8180_93cx6.c
@@ -53,7 +53,7 @@ static void eprom_ck_cycle(struct net_device *dev)
 }
 
 
-static void eprom_w(struct net_device *dev,short bit)
+static void eprom_w(struct net_device *dev, short bit)
 {
 	u8 cmdreg;
 
@@ -86,7 +86,7 @@ static void eprom_send_bits_string(struct net_device *dev, short b[], int len)
 {
 	int i;
 
-	for(i=0; i<len; i++){
+	for (i = 0; i < len; i++) {
 		eprom_w(dev, b[i]);
 		eprom_ck_cycle(dev);
 	}
@@ -96,37 +96,37 @@ static void eprom_send_bits_string(struct net_device *dev, short b[], int len)
 u32 eprom_read(struct net_device *dev, u32 addr)
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
-	short read_cmd[]={1,1,0};
+	short read_cmd[] = {1, 1, 0};
 	short addr_str[8];
 	int i;
 	int addr_len;
 	u32 ret;
 
-	ret=0;
+	ret = 0;
 	//enable EPROM programming
 	write_nic_byte_E(dev, EPROM_CMD,
 		       (EPROM_CMD_PROGRAM<<EPROM_CMD_OPERATING_MODE_SHIFT));
 	force_pci_posting(dev);
 	udelay(EPROM_DELAY);
 
-	if (priv->epromtype==EPROM_93c56){
-		addr_str[7]=addr & 1;
-		addr_str[6]=addr & (1<<1);
-		addr_str[5]=addr & (1<<2);
-		addr_str[4]=addr & (1<<3);
-		addr_str[3]=addr & (1<<4);
-		addr_str[2]=addr & (1<<5);
-		addr_str[1]=addr & (1<<6);
-		addr_str[0]=addr & (1<<7);
-		addr_len=8;
-	}else{
-		addr_str[5]=addr & 1;
-		addr_str[4]=addr & (1<<1);
-		addr_str[3]=addr & (1<<2);
-		addr_str[2]=addr & (1<<3);
-		addr_str[1]=addr & (1<<4);
-		addr_str[0]=addr & (1<<5);
-		addr_len=6;
+	if (priv->epromtype == EPROM_93c56) {
+		addr_str[7] = addr & 1;
+		addr_str[6] = addr & (1<<1);
+		addr_str[5] = addr & (1<<2);
+		addr_str[4] = addr & (1<<3);
+		addr_str[3] = addr & (1<<4);
+		addr_str[2] = addr & (1<<5);
+		addr_str[1] = addr & (1<<6);
+		addr_str[0] = addr & (1<<7);
+		addr_len = 8;
+	} else {
+		addr_str[5] = addr & 1;
+		addr_str[4] = addr & (1<<1);
+		addr_str[3] = addr & (1<<2);
+		addr_str[2] = addr & (1<<3);
+		addr_str[1] = addr & (1<<4);
+		addr_str[0] = addr & (1<<5);
+		addr_len = 6;
 	}
 	eprom_cs(dev, 1);
 	eprom_ck_cycle(dev);
@@ -137,7 +137,7 @@ u32 eprom_read(struct net_device *dev, u32 addr)
 	//I'm unsure if it is necessary, but anyway shouldn't hurt
 	eprom_w(dev, 0);
 
-	for(i=0;i<16;i++){
+	for (i = 0; i < 16; i++) {
 		//eeprom needs a clk cycle between writing opcode&adr
 		//and reading data. (eeprom outs a dummy 0)
 		eprom_ck_cycle(dev);
diff --git a/drivers/staging/rtl8192u/r8190_rtl8256.c b/drivers/staging/rtl8192u/r8190_rtl8256.c
index 40b14a2d1cd..08e1bc90555 100644
--- a/drivers/staging/rtl8192u/r8190_rtl8256.c
+++ b/drivers/staging/rtl8192u/r8190_rtl8256.c
@@ -32,7 +32,7 @@ void PHY_SetRF8256Bandwidth(struct net_device *dev , HT_CHANNEL_WIDTH Bandwidth)
 	for(eRFPath = 0; eRFPath <RF90_PATH_MAX; eRFPath++)
 	{
 		if (!rtl8192_phy_CheckIsLegalRFPath(dev, eRFPath))
-				continue;
+			continue;
 
 		switch (Bandwidth)
 		{
@@ -125,7 +125,7 @@ void phy_RF8256_Config_ParaFile(struct net_device *dev)
 	for(eRFPath = (RF90_RADIO_PATH_E)RF90_PATH_A; eRFPath <priv->NumTotalRFPath; eRFPath++)
 	{
 		if (!rtl8192_phy_CheckIsLegalRFPath(dev, eRFPath))
-				continue;
+			continue;
 
 		pPhyReg = &priv->PHYRegDef[eRFPath];
 
@@ -249,7 +249,7 @@ void PHY_SetRF8256CCKTxPower(struct net_device *dev, u8 powerlevel)
 		if(priv->CustomerID == RT_CID_819x_Netcore)
 			TxAGC = 0x22;
 		else
-		TxAGC += priv->CckPwEnl;
+			TxAGC += priv->CckPwEnl;
 	}
 
 	if(TxAGC > 0x24)
diff --git a/drivers/staging/rtl8192u/r8192U_core.c b/drivers/staging/rtl8192u/r8192U_core.c
index 1bb6143cdf0..24272c51bc9 100644
--- a/drivers/staging/rtl8192u/r8192U_core.c
+++ b/drivers/staging/rtl8192u/r8192U_core.c
@@ -97,10 +97,6 @@ double __extendsfdf2(float a)
 #include <linux/seq_file.h>
 // FIXME: check if 2.6.7 is ok
 
-#ifdef CONFIG_RTL8192_PM
-#include "r8192_pm.h"
-#endif
-
 #include "dot11d.h"
 //set here to open your trace code. //WB
 u32 rt_global_debug_component = COMP_DOWN	|
@@ -159,13 +155,8 @@ static struct usb_driver rtl8192_usb_driver = {
 	.id_table	= rtl8192_usb_id_tbl,		  /* PCI_ID table  */
 	.probe		= rtl8192_usb_probe,		  /* probe fn      */
 	.disconnect	= rtl8192_usb_disconnect,	  /* remove fn     */
-#ifdef CONFIG_RTL8192_PM
-	.suspend	= rtl8192_suspend,		  /* PM suspend fn */
-	.resume		= rtl8192_resume,                 /* PM resume fn  */
-#else
 	.suspend	= NULL,				  /* PM suspend fn */
 	.resume		= NULL,				  /* PM resume fn  */
-#endif
 };
 
 
@@ -2045,9 +2036,6 @@ static void rtl8192_qos_activate(struct work_struct *work)
 	u32 u4bAcParam;
 	int i;
 
-	if (priv == NULL)
-		return;
-
 	mutex_lock(&priv->mutex);
 	if (priv->ieee80211->state != IEEE80211_LINKED)
 		goto success;
@@ -4981,12 +4969,8 @@ static int rtl8192_usb_probe(struct usb_interface *intf,
 
 	dev->netdev_ops = &rtl8192_netdev_ops;
 
-#if WIRELESS_EXT >= 12
-#if WIRELESS_EXT < 17
-	dev->get_wireless_stats = r8192_get_wireless_stats;
-#endif
 	dev->wireless_handlers = (struct iw_handler_def *) &r8192_wx_handlers_def;
-#endif
+
 	dev->type = ARPHRD_ETHER;
 
 	dev->watchdog_timeo = HZ*3;	//modified by john, 0805
diff --git a/drivers/staging/rtl8192u/r8192U_dm.c b/drivers/staging/rtl8192u/r8192U_dm.c
index d97ad7b909b..b4264e1353c 100644
--- a/drivers/staging/rtl8192u/r8192U_dm.c
+++ b/drivers/staging/rtl8192u/r8192U_dm.c
@@ -183,9 +183,8 @@ void dm_CheckRxAggregation(struct net_device *dev) {
 	curTxOkCnt = priv->stats.txbytesunicast - lastTxOkCnt;
 	curRxOkCnt = priv->stats.rxbytesunicast - lastRxOkCnt;
 
-	if((curTxOkCnt + curRxOkCnt) < 15000000) {
+	if ((curTxOkCnt + curRxOkCnt) < 15000000)
 		return;
-	}
 
 	if(curTxOkCnt > 4*curRxOkCnt) {
 		if (priv->bCurrentRxAggrEnable) {
@@ -1646,9 +1645,8 @@ void dm_restore_dynamic_mechanism_state(struct net_device *dev)
 			write_nic_byte(dev, UFWP, 1);
 	}
 	//Restore TX Power Tracking Index
-	if(priv->btxpower_trackingInit && priv->btxpower_tracking){
+	if (priv->btxpower_trackingInit && priv->btxpower_tracking)
 		dm_txpower_reset_recovery(dev);
-	}
 
 	//
 	//Restore BB Initial Gain
@@ -2525,7 +2523,7 @@ static void dm_check_edca_turbo(
 	if(priv->ieee80211->pHTInfo->IOTAction & HT_IOT_ACT_DISABLE_EDCA_TURBO)
 		goto dm_CheckEdcaTurbo_EXIT;
 
-//	printk("========>%s():bis_any_nonbepkts is %d\n",__FUNCTION__,priv->bis_any_nonbepkts);
+//	printk("========>%s():bis_any_nonbepkts is %d\n",__func__,priv->bis_any_nonbepkts);
 	// Check the status for current condition.
 	if(!priv->ieee80211->bis_any_nonbepkts)
 	{
@@ -3185,7 +3183,7 @@ void dm_fsync_timer_callback(unsigned long data)
 
 static void dm_StartHWFsync(struct net_device *dev)
 {
-	RT_TRACE(COMP_HALDM, "%s\n", __FUNCTION__);
+	RT_TRACE(COMP_HALDM, "%s\n", __func__);
 	write_nic_dword(dev, rOFDM0_RxDetector2, 0x465c12cf);
 	write_nic_byte(dev, 0xc3b, 0x41);
 }
@@ -3194,7 +3192,7 @@ static void dm_EndSWFsync(struct net_device *dev)
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
 
-	RT_TRACE(COMP_HALDM, "%s\n", __FUNCTION__);
+	RT_TRACE(COMP_HALDM, "%s\n", __func__);
 	del_timer_sync(&(priv->fsync_timer));
 
 	// Let Register return to default value;
@@ -3218,7 +3216,7 @@ static void dm_StartSWFsync(struct net_device *dev)
 	u32			rateIndex;
 	u32			rateBitmap;
 
-	RT_TRACE(COMP_HALDM,"%s\n", __FUNCTION__);
+	RT_TRACE(COMP_HALDM,"%s\n", __func__);
 	// Initial rate record to zero, start to record.
 	priv->rate_record = 0;
 	// Initialize continue diff count to zero, start to record.
@@ -3253,7 +3251,7 @@ static void dm_StartSWFsync(struct net_device *dev)
 
 static void dm_EndHWFsync(struct net_device *dev)
 {
-	RT_TRACE(COMP_HALDM,"%s\n", __FUNCTION__);
+	RT_TRACE(COMP_HALDM,"%s\n", __func__);
 	write_nic_dword(dev, rOFDM0_RxDetector2, 0x465c52cd);
 	write_nic_byte(dev, 0xc3b, 0x49);
 
@@ -3485,7 +3483,7 @@ static void dm_dynamic_txpower(struct net_device *dev)
 		txlowpower_threshold = TX_POWER_NEAR_FIELD_THRESH_LOW;
 	}
 
-//	printk("=======>%s(): txhipower_threshhold is %d,txlowpower_threshold is %d\n",__FUNCTION__,txhipower_threshhold,txlowpower_threshold);
+//	printk("=======>%s(): txhipower_threshhold is %d,txlowpower_threshold is %d\n",__func__,txhipower_threshhold,txlowpower_threshold);
 	RT_TRACE(COMP_TXAGC,"priv->undecorated_smoothed_pwdb = %ld \n" , priv->undecorated_smoothed_pwdb);
 
 	if(priv->ieee80211->state == IEEE80211_LINKED)
diff --git a/drivers/staging/rtl8192u/r8192U_wx.c b/drivers/staging/rtl8192u/r8192U_wx.c
index c70af014a31..675a12d6aa8 100644
--- a/drivers/staging/rtl8192u/r8192U_wx.c
+++ b/drivers/staging/rtl8192u/r8192U_wx.c
@@ -24,8 +24,8 @@
 #include "dot11d.h"
 
 #define RATE_COUNT 12
-u32 rtl8180_rates[] = {1000000,2000000,5500000,11000000,
-	6000000,9000000,12000000,18000000,24000000,36000000,48000000,54000000};
+u32 rtl8180_rates[] = {1000000, 2000000, 5500000, 11000000,
+	6000000, 9000000, 12000000, 18000000, 24000000, 36000000, 48000000, 54000000};
 
 
 #ifndef ENETDOWN
@@ -38,16 +38,16 @@ static int r8192_wx_get_freq(struct net_device *dev,
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
 
-	return ieee80211_wx_get_freq(priv->ieee80211,a,wrqu,b);
+	return ieee80211_wx_get_freq(priv->ieee80211, a, wrqu, b);
 }
 
 
 static int r8192_wx_get_mode(struct net_device *dev, struct iw_request_info *a,
 			     union iwreq_data *wrqu, char *b)
 {
-	struct r8192_priv *priv=ieee80211_priv(dev);
+	struct r8192_priv *priv = ieee80211_priv(dev);
 
-	return ieee80211_wx_get_mode(priv->ieee80211,a,wrqu,b);
+	return ieee80211_wx_get_mode(priv->ieee80211, a, wrqu, b);
 }
 
 
@@ -57,7 +57,7 @@ static int r8192_wx_get_rate(struct net_device *dev,
 			     union iwreq_data *wrqu, char *extra)
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
-	return ieee80211_wx_get_rate(priv->ieee80211,info,wrqu,extra);
+	return ieee80211_wx_get_rate(priv->ieee80211, info, wrqu, extra);
 }
 
 
@@ -71,7 +71,7 @@ static int r8192_wx_set_rate(struct net_device *dev,
 
 	down(&priv->wx_sem);
 
-	ret = ieee80211_wx_set_rate(priv->ieee80211,info,wrqu,extra);
+	ret = ieee80211_wx_set_rate(priv->ieee80211, info, wrqu, extra);
 
 	up(&priv->wx_sem);
 
@@ -88,7 +88,7 @@ static int r8192_wx_set_rts(struct net_device *dev,
 
 	down(&priv->wx_sem);
 
-	ret = ieee80211_wx_set_rts(priv->ieee80211,info,wrqu,extra);
+	ret = ieee80211_wx_set_rts(priv->ieee80211, info, wrqu, extra);
 
 	up(&priv->wx_sem);
 
@@ -100,7 +100,7 @@ static int r8192_wx_get_rts(struct net_device *dev,
 			     union iwreq_data *wrqu, char *extra)
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
-	return ieee80211_wx_get_rts(priv->ieee80211,info,wrqu,extra);
+	return ieee80211_wx_get_rts(priv->ieee80211, info, wrqu, extra);
 }
 
 static int r8192_wx_set_power(struct net_device *dev,
@@ -112,7 +112,7 @@ static int r8192_wx_set_power(struct net_device *dev,
 
 	down(&priv->wx_sem);
 
-	ret = ieee80211_wx_set_power(priv->ieee80211,info,wrqu,extra);
+	ret = ieee80211_wx_set_power(priv->ieee80211, info, wrqu, extra);
 
 	up(&priv->wx_sem);
 
@@ -124,7 +124,7 @@ static int r8192_wx_get_power(struct net_device *dev,
 			     union iwreq_data *wrqu, char *extra)
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
-	return ieee80211_wx_get_power(priv->ieee80211,info,wrqu,extra);
+	return ieee80211_wx_get_power(priv->ieee80211, info, wrqu, extra);
 }
 
 static int r8192_wx_force_reset(struct net_device *dev,
@@ -135,7 +135,7 @@ static int r8192_wx_force_reset(struct net_device *dev,
 
 	down(&priv->wx_sem);
 
-	printk("%s(): force reset ! extra is %d\n",__FUNCTION__, *extra);
+	printk("%s(): force reset ! extra is %d\n", __func__, *extra);
 	priv->force_reset = *extra;
 	up(&priv->wx_sem);
 	return 0;
@@ -171,15 +171,15 @@ static int r8192_wx_set_crcmon(struct net_device *dev,
 
 	down(&priv->wx_sem);
 
-	if(enable)
-		priv->crcmon=1;
+	if (enable)
+		priv->crcmon = 1;
 	else
-		priv->crcmon=0;
+		priv->crcmon = 0;
 
 	DMESG("bad CRC in monitor mode are %s",
 	      priv->crcmon ? "accepted" : "rejected");
 
-	if(prev != priv->crcmon && priv->up){
+	if (prev != priv->crcmon && priv->up) {
 		//rtl8180_down(dev);
 		//rtl8180_up(dev);
 	}
@@ -196,7 +196,7 @@ static int r8192_wx_set_mode(struct net_device *dev, struct iw_request_info *a,
 	int ret;
 	down(&priv->wx_sem);
 
-	ret = ieee80211_wx_set_mode(priv->ieee80211,a,wrqu,b);
+	ret = ieee80211_wx_set_mode(priv->ieee80211, a, wrqu, b);
 
 	rtl8192_set_rxconf(dev);
 
@@ -258,7 +258,7 @@ static int rtl8180_wx_get_range(struct net_device *dev,
 //	range->old_num_channels;
 //	range->old_num_frequency;
 //	range->old_freq[6]; /* Filler to keep "version" at the same offset */
-	if(priv->rf_set_sens != NULL)
+	if (priv->rf_set_sens != NULL)
 		range->sensitivity = priv->max_sens;	/* signal level threshold range */
 
 	range->max_qual.qual = 100;
@@ -275,14 +275,13 @@ static int rtl8180_wx_get_range(struct net_device *dev,
 
 	range->num_bitrates = RATE_COUNT;
 
-	for (i = 0; i < RATE_COUNT && i < IW_MAX_BITRATES; i++) {
+	for (i = 0; i < RATE_COUNT && i < IW_MAX_BITRATES; i++)
 		range->bitrate[i] = rtl8180_rates[i];
-	}
 
 	range->min_frag = MIN_FRAG_THRESHOLD;
 	range->max_frag = MAX_FRAG_THRESHOLD;
 
-	range->min_pmp=0;
+	range->min_pmp = 0;
 	range->max_pmp = 5000000;
 	range->min_pmt = 0;
 	range->max_pmt = 65535*1000;
@@ -316,14 +315,12 @@ static int rtl8180_wx_get_range(struct net_device *dev,
 		}
 
 		if (val == IW_MAX_FREQUENCIES)
-		break;
+			break;
 	}
 	range->num_frequency = val;
 	range->num_channels = val;
-#if WIRELESS_EXT > 17
 	range->enc_capa = IW_ENC_CAPA_WPA|IW_ENC_CAPA_WPA2|
 			  IW_ENC_CAPA_CIPHER_TKIP|IW_ENC_CAPA_CIPHER_CCMP;
-#endif
 	tmp->scan_capa = 0x01;
 	return 0;
 }
@@ -336,15 +333,14 @@ static int r8192_wx_set_scan(struct net_device *dev, struct iw_request_info *a,
 	struct ieee80211_device *ieee = priv->ieee80211;
 	int ret = 0;
 
-	if(!priv->up) return -ENETDOWN;
+	if (!priv->up)
+		return -ENETDOWN;
 
 	if (priv->ieee80211->LinkDetectInfo.bBusyTraffic == true)
 		return -EAGAIN;
-	if (wrqu->data.flags & IW_SCAN_THIS_ESSID)
-	{
+	if (wrqu->data.flags & IW_SCAN_THIS_ESSID) {
 		struct iw_scan_req *req = (struct iw_scan_req *)b;
-		if (req->essid_len)
-		{
+		if (req->essid_len) {
 			//printk("==**&*&*&**===>scan set ssid:%s\n", req->essid);
 			ieee->current_network.ssid_len = req->essid_len;
 			memcpy(ieee->current_network.ssid, req->essid, req->essid_len);
@@ -353,13 +349,13 @@ static int r8192_wx_set_scan(struct net_device *dev, struct iw_request_info *a,
 	}
 
 	down(&priv->wx_sem);
-	if(priv->ieee80211->state != IEEE80211_LINKED){
+	if (priv->ieee80211->state != IEEE80211_LINKED) {
 		priv->ieee80211->scanning = 0;
 		ieee80211_softmac_scan_syncro(priv->ieee80211);
 		ret = 0;
+	} else {
+		ret = ieee80211_wx_set_scan(priv->ieee80211, a, wrqu, b);
 	}
-	else
-	ret = ieee80211_wx_set_scan(priv->ieee80211,a,wrqu,b);
 	up(&priv->wx_sem);
 	return ret;
 }
@@ -372,11 +368,12 @@ static int r8192_wx_get_scan(struct net_device *dev, struct iw_request_info *a,
 	int ret;
 	struct r8192_priv *priv = ieee80211_priv(dev);
 
-	if(!priv->up) return -ENETDOWN;
+	if (!priv->up)
+		return -ENETDOWN;
 
 	down(&priv->wx_sem);
 
-	ret = ieee80211_wx_get_scan(priv->ieee80211,a,wrqu,b);
+	ret = ieee80211_wx_get_scan(priv->ieee80211, a, wrqu, b);
 
 	up(&priv->wx_sem);
 
@@ -391,7 +388,7 @@ static int r8192_wx_set_essid(struct net_device *dev,
 	int ret;
 	down(&priv->wx_sem);
 
-	ret = ieee80211_wx_set_essid(priv->ieee80211,a,wrqu,b);
+	ret = ieee80211_wx_set_essid(priv->ieee80211, a, wrqu, b);
 
 	up(&priv->wx_sem);
 
@@ -486,7 +483,7 @@ static int r8192_wx_set_wap(struct net_device *dev,
 //        struct sockaddr *temp = (struct sockaddr *)awrq;
 	down(&priv->wx_sem);
 
-	ret = ieee80211_wx_set_wap(priv->ieee80211,info,awrq,extra);
+	ret = ieee80211_wx_set_wap(priv->ieee80211, info, awrq, extra);
 
 	up(&priv->wx_sem);
 
@@ -501,7 +498,7 @@ static int r8192_wx_get_wap(struct net_device *dev,
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
 
-	return ieee80211_wx_get_wap(priv->ieee80211,info,wrqu,extra);
+	return ieee80211_wx_get_wap(priv->ieee80211, info, wrqu, extra);
 }
 
 
@@ -523,34 +520,37 @@ static int r8192_wx_set_enc(struct net_device *dev,
 	int ret;
 
 	//u32 TargetContent;
-	u32 hwkey[4]={0,0,0,0};
-	u8 mask=0xff;
-	u32 key_idx=0;
+	u32 hwkey[4] = {0, 0, 0, 0};
+	u8 mask = 0xff;
+	u32 key_idx = 0;
 	//u8 broadcast_addr[6] ={	0xff,0xff,0xff,0xff,0xff,0xff};
-	u8 zero_addr[4][6] ={	{0x00,0x00,0x00,0x00,0x00,0x00},
-				{0x00,0x00,0x00,0x00,0x00,0x01},
-				{0x00,0x00,0x00,0x00,0x00,0x02},
-				{0x00,0x00,0x00,0x00,0x00,0x03} };
+	u8 zero_addr[4][6] = {	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
+				{0x00, 0x00, 0x00, 0x00, 0x00, 0x01},
+				{0x00, 0x00, 0x00, 0x00, 0x00, 0x02},
+				{0x00, 0x00, 0x00, 0x00, 0x00, 0x03} };
 	int i;
 
-       if(!priv->up) return -ENETDOWN;
+	if (!priv->up)
+		return -ENETDOWN;
 
 	down(&priv->wx_sem);
 
 	RT_TRACE(COMP_SEC, "Setting SW wep key");
-	ret = ieee80211_wx_set_encode(priv->ieee80211,info,wrqu,key);
+	ret = ieee80211_wx_set_encode(priv->ieee80211, info, wrqu, key);
 
 	up(&priv->wx_sem);
 
 
 
 	//sometimes, the length is zero while we do not type key value
-	if(wrqu->encoding.length!=0){
+	if (wrqu->encoding.length != 0) {
 
-		for(i=0 ; i<4 ; i++){
+		for (i = 0; i < 4; i++) {
 			hwkey[i] |=  key[4*i+0]&mask;
-			if(i==1&&(4*i+1)==wrqu->encoding.length) mask=0x00;
-			if(i==3&&(4*i+1)==wrqu->encoding.length) mask=0x00;
+			if (i == 1 && (4*i+1) == wrqu->encoding.length)
+				mask = 0x00;
+			if (i == 3 && (4*i+1) == wrqu->encoding.length)
+				mask = 0x00;
 			hwkey[i] |= (key[4*i+1]&mask)<<8;
 			hwkey[i] |= (key[4*i+2]&mask)<<16;
 			hwkey[i] |= (key[4*i+3]&mask)<<24;
@@ -559,20 +559,31 @@ static int r8192_wx_set_enc(struct net_device *dev,
 		#define CONF_WEP40  0x4
 		#define CONF_WEP104 0x14
 
-		switch (wrqu->encoding.flags & IW_ENCODE_INDEX){
-		case 0: key_idx = ieee->tx_keyidx; break;
-		case 1:	key_idx = 0; break;
-		case 2:	key_idx = 1; break;
-		case 3:	key_idx = 2; break;
-		case 4:	key_idx	= 3; break;
-		default: break;
+		switch (wrqu->encoding.flags & IW_ENCODE_INDEX) {
+		case 0:
+			key_idx = ieee->tx_keyidx;
+			break;
+		case 1:
+			key_idx = 0;
+			break;
+		case 2:
+			key_idx = 1;
+			break;
+		case 3:
+			key_idx = 2;
+			break;
+		case 4:
+			key_idx	= 3;
+			break;
+		default:
+			break;
 		}
 
-		if(wrqu->encoding.length==0x5){
+		if (wrqu->encoding.length == 0x5) {
 				ieee->pairwise_key_type = KEY_TYPE_WEP40;
 			EnableHWSecurityConfig8192(dev);
 
-			setKey( dev,
+			setKey(dev,
 				key_idx,                //EntryNo
 				key_idx,                //KeyIndex
 				KEY_TYPE_WEP40,         //KeyType
@@ -582,11 +593,11 @@ static int r8192_wx_set_enc(struct net_device *dev,
 
 		}
 
-		else if(wrqu->encoding.length==0xd){
+		else if (wrqu->encoding.length == 0xd) {
 				ieee->pairwise_key_type = KEY_TYPE_WEP104;
 				EnableHWSecurityConfig8192(dev);
 
-			setKey( dev,
+			setKey(dev,
 				key_idx,                //EntryNo
 				key_idx,                //KeyIndex
 				KEY_TYPE_WEP104,        //KeyType
@@ -594,8 +605,9 @@ static int r8192_wx_set_enc(struct net_device *dev,
 				0,                      //DefaultKey
 				hwkey);                 //KeyContent
 
+		} else {
+			printk("wrong type in WEP, not WEP40 and WEP104\n");
 		}
-		else printk("wrong type in WEP, not WEP40 and WEP104\n");
 
 	}
 
@@ -603,12 +615,13 @@ static int r8192_wx_set_enc(struct net_device *dev,
 }
 
 
-static int r8192_wx_set_scan_type(struct net_device *dev, struct iw_request_info *aa, union
- iwreq_data *wrqu, char *p){
+static int r8192_wx_set_scan_type(struct net_device *dev, struct iw_request_info *aa,
+					union iwreq_data *wrqu, char *p)
+{
 
 	struct r8192_priv *priv = ieee80211_priv(dev);
-	int *parms=(int *)p;
-	int mode=parms[0];
+	int *parms = (int *)p;
+	int mode = parms[0];
 
 	priv->ieee80211->active_scan = mode;
 
@@ -631,20 +644,20 @@ static int r8192_wx_set_retry(struct net_device *dev,
 		err = -EINVAL;
 		goto exit;
 	}
-	if (!(wrqu->retry.flags & IW_RETRY_LIMIT)){
+	if (!(wrqu->retry.flags & IW_RETRY_LIMIT)) {
 		err = -EINVAL;
 		goto exit;
 	}
 
-	if(wrqu->retry.value > R8180_MAX_RETRY){
-		err= -EINVAL;
+	if (wrqu->retry.value > R8180_MAX_RETRY) {
+		err = -EINVAL;
 		goto exit;
 	}
 	if (wrqu->retry.flags & IW_RETRY_MAX) {
 		priv->retry_rts = wrqu->retry.value;
 		DMESG("Setting retry for RTS/CTS data to %d", wrqu->retry.value);
 
-	}else {
+	} else {
 		priv->retry_data = wrqu->retry.value;
 		DMESG("Setting retry for non RTS/CTS data to %d", wrqu->retry.value);
 	}
@@ -701,7 +714,7 @@ static int r8192_wx_get_sens(struct net_device *dev,
 				union iwreq_data *wrqu, char *extra)
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
-	if(priv->rf_set_sens == NULL)
+	if (priv->rf_set_sens == NULL)
 		return -1; /* we have not this support for this radio */
 	wrqu->sens.value = priv->sens;
 	return 0;
@@ -718,14 +731,14 @@ static int r8192_wx_set_sens(struct net_device *dev,
 	short err = 0;
 	down(&priv->wx_sem);
 	//DMESG("attempt to set sensivity to %ddb",wrqu->sens.value);
-	if(priv->rf_set_sens == NULL) {
-		err= -1; /* we have not this support for this radio */
+	if (priv->rf_set_sens == NULL) {
+		err = -1; /* we have not this support for this radio */
 		goto exit;
 	}
-	if(priv->rf_set_sens(dev, wrqu->sens.value) == 0)
+	if (priv->rf_set_sens(dev, wrqu->sens.value) == 0)
 		priv->sens = wrqu->sens.value;
 	else
-		err= -EINVAL;
+		err = -EINVAL;
 
 exit:
 	up(&priv->wx_sem);
@@ -733,72 +746,66 @@ exit:
 	return err;
 }
 
-#if (WIRELESS_EXT >= 18)
 //hw security need to reorganized.
 static int r8192_wx_set_enc_ext(struct net_device *dev,
 					struct iw_request_info *info,
 					union iwreq_data *wrqu, char *extra)
 {
-	int ret=0;
+	int ret = 0;
 	struct r8192_priv *priv = ieee80211_priv(dev);
 	struct ieee80211_device *ieee = priv->ieee80211;
-	//printk("===>%s()\n", __FUNCTION__);
+	//printk("===>%s()\n", __func__);
 
 
 	down(&priv->wx_sem);
 	ret = ieee80211_wx_set_encode_ext(priv->ieee80211, info, wrqu, extra);
 
 	{
-		u8 broadcast_addr[6] = {0xff,0xff,0xff,0xff,0xff,0xff};
+		u8 broadcast_addr[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 		u8 zero[6] = {0};
 		u32 key[4] = {0};
 		struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
 		struct iw_point *encoding = &wrqu->encoding;
 		u8 idx = 0, alg = 0, group = 0;
-		if ((encoding->flags & IW_ENCODE_DISABLED) ||
-		ext->alg == IW_ENCODE_ALG_NONE) //none is not allowed to use hwsec WB 2008.07.01
+		if ((encoding->flags & IW_ENCODE_DISABLED) || ext->alg == IW_ENCODE_ALG_NONE)
+			//none is not allowed to use hwsec WB 2008.07.01
 			goto end_hw_sec;
 
-		alg =  (ext->alg == IW_ENCODE_ALG_CCMP)?KEY_TYPE_CCMP:ext->alg; // as IW_ENCODE_ALG_CCMP is defined to be 3 and KEY_TYPE_CCMP is defined to 4;
+		// as IW_ENCODE_ALG_CCMP is defined to be 3 and KEY_TYPE_CCMP is defined to 4;
+		alg =  (ext->alg == IW_ENCODE_ALG_CCMP)?KEY_TYPE_CCMP:ext->alg;
 		idx = encoding->flags & IW_ENCODE_INDEX;
 		if (idx)
-			idx --;
+			idx--;
 		group = ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY;
 
-		if ((!group) || (IW_MODE_ADHOC == ieee->iw_mode) || (alg ==  KEY_TYPE_WEP40))
-		{
-			if ((ext->key_len == 13) && (alg == KEY_TYPE_WEP40) )
+		if ((!group) || (IW_MODE_ADHOC == ieee->iw_mode) || (alg ==  KEY_TYPE_WEP40)) {
+			if ((ext->key_len == 13) && (alg == KEY_TYPE_WEP40))
 				alg = KEY_TYPE_WEP104;
 			ieee->pairwise_key_type = alg;
 			EnableHWSecurityConfig8192(dev);
 		}
 		memcpy((u8 *)key, ext->key, 16); //we only get 16 bytes key.why? WB 2008.7.1
 
-		if ((alg & KEY_TYPE_WEP40) && (ieee->auth_mode !=2) )
-		{
+		if ((alg & KEY_TYPE_WEP40) && (ieee->auth_mode != 2)) {
 
-			setKey( dev,
+			setKey(dev,
 					idx,//EntryNo
 					idx, //KeyIndex
 					alg,  //KeyType
 					zero, //MacAddr
 					0,              //DefaultKey
 					key);           //KeyContent
-		}
-		else if (group)
-		{
+		} else if (group) {
 			ieee->group_key_type = alg;
-			setKey( dev,
+			setKey(dev,
 					idx,//EntryNo
 					idx, //KeyIndex
 					alg,  //KeyType
 					broadcast_addr, //MacAddr
 					0,              //DefaultKey
 					key);           //KeyContent
-		}
-		else //pairwise key
-		{
-			setKey( dev,
+		} else {//pairwise key
+			setKey(dev,
 					4,//EntryNo
 					idx, //KeyIndex
 					alg,  //KeyType
@@ -820,8 +827,8 @@ static int r8192_wx_set_auth(struct net_device *dev,
 					struct iw_request_info *info,
 					union iwreq_data *data, char *extra)
 {
-	int ret=0;
-	//printk("====>%s()\n", __FUNCTION__);
+	int ret = 0;
+	//printk("====>%s()\n", __func__);
 	struct r8192_priv *priv = ieee80211_priv(dev);
 	down(&priv->wx_sem);
 	ret = ieee80211_wx_set_auth(priv->ieee80211, info, &(data->param), extra);
@@ -833,9 +840,9 @@ static int r8192_wx_set_mlme(struct net_device *dev,
 					struct iw_request_info *info,
 					union iwreq_data *wrqu, char *extra)
 {
-	//printk("====>%s()\n", __FUNCTION__);
+	//printk("====>%s()\n", __func__);
 
-	int ret=0;
+	int ret = 0;
 	struct r8192_priv *priv = ieee80211_priv(dev);
 	down(&priv->wx_sem);
 	ret = ieee80211_wx_set_mlme(priv->ieee80211, info, wrqu, extra);
@@ -843,32 +850,31 @@ static int r8192_wx_set_mlme(struct net_device *dev,
 	up(&priv->wx_sem);
 	return ret;
 }
-#endif
+
 static int r8192_wx_set_gen_ie(struct net_device *dev,
 					struct iw_request_info *info,
 					union iwreq_data *data, char *extra)
 {
-	   //printk("====>%s(), len:%d\n", __FUNCTION__, data->length);
-	int ret=0;
+	   //printk("====>%s(), len:%d\n", __func__, data->length);
+	int ret = 0;
 	struct r8192_priv *priv = ieee80211_priv(dev);
 	down(&priv->wx_sem);
 	ret = ieee80211_wx_set_gen_ie(priv->ieee80211, extra, data->data.length);
 	up(&priv->wx_sem);
-	//printk("<======%s(), ret:%d\n", __FUNCTION__, ret);
+	//printk("<======%s(), ret:%d\n", __func__, ret);
 	return ret;
 
 
 }
 
 static int dummy(struct net_device *dev, struct iw_request_info *a,
-		 union iwreq_data *wrqu,char *b)
+		 union iwreq_data *wrqu, char *b)
 {
 	return -1;
 }
 
 
-static iw_handler r8192_wx_handlers[] =
-{
+static iw_handler r8192_wx_handlers[] = {
 	NULL,                     /* SIOCSIWCOMMIT */
 	r8192_wx_get_name,	  /* SIOCGIWNAME */
 	dummy,                    /* SIOCSIWNWID */
@@ -891,11 +897,7 @@ static iw_handler r8192_wx_handlers[] =
 	NULL,                     /* SIOCWIWTHRSPY */
 	r8192_wx_set_wap,	  /* SIOCSIWAP */
 	r8192_wx_get_wap,         /* SIOCGIWAP */
-#if (WIRELESS_EXT >= 18)
 	r8192_wx_set_mlme,                     /* MLME-- */
-#else
-	 NULL,
-#endif
 	dummy,                     /* SIOCGIWAPLIST -- deprecated */
 	r8192_wx_set_scan,        /* SIOCSIWSCAN */
 	r8192_wx_get_scan,        /* SIOCGIWSCAN */
@@ -924,17 +926,10 @@ static iw_handler r8192_wx_handlers[] =
 	r8192_wx_set_gen_ie,//NULL,			/* SIOCSIWGENIE */
 	NULL,			/* SIOCSIWGENIE */
 
-#if (WIRELESS_EXT >= 18)
 	r8192_wx_set_auth,//NULL,			/* SIOCSIWAUTH */
 	NULL,//r8192_wx_get_auth,//NULL,			/* SIOCSIWAUTH */
 	r8192_wx_set_enc_ext,			/* SIOCSIWENCODEEXT */
 	NULL,//r8192_wx_get_enc_ext,//NULL,			/* SIOCSIWENCODEEXT */
-#else
-	NULL,
-	NULL,
-	NULL,
-	NULL,
-#endif
 	NULL,			/* SIOCSIWPMKSA */
 	NULL,			 /*---hole---*/
 
@@ -978,17 +973,15 @@ static iw_handler r8192_private_handler[] = {
 	r8192_wx_force_reset,
 };
 
-//#if WIRELESS_EXT >= 17
 struct iw_statistics *r8192_get_wireless_stats(struct net_device *dev)
 {
-       struct r8192_priv *priv = ieee80211_priv(dev);
+	struct r8192_priv *priv = ieee80211_priv(dev);
 	struct ieee80211_device *ieee = priv->ieee80211;
 	struct iw_statistics *wstats = &priv->wstats;
 	int tmp_level = 0;
 	int tmp_qual = 0;
 	int tmp_noise = 0;
-	if(ieee->state < IEEE80211_LINKED)
-	{
+	if (ieee->state < IEEE80211_LINKED) {
 		wstats->qual.qual = 0;
 		wstats->qual.level = 0;
 		wstats->qual.noise = 0;
@@ -996,7 +989,7 @@ struct iw_statistics *r8192_get_wireless_stats(struct net_device *dev)
 		return wstats;
 	}
 
-       tmp_level = (&ieee->current_network)->stats.rssi;
+	tmp_level = (&ieee->current_network)->stats.rssi;
 	tmp_qual = (&ieee->current_network)->stats.signal;
 	tmp_noise = (&ieee->current_network)->stats.noise;
 	//printk("level:%d, qual:%d, noise:%d\n", tmp_level, tmp_qual, tmp_noise);
@@ -1007,17 +1000,14 @@ struct iw_statistics *r8192_get_wireless_stats(struct net_device *dev)
 	wstats->qual.updated = IW_QUAL_ALL_UPDATED| IW_QUAL_DBM;
 	return wstats;
 }
-//#endif
 
 
-struct iw_handler_def  r8192_wx_handlers_def={
+struct iw_handler_def  r8192_wx_handlers_def = {
 	.standard = r8192_wx_handlers,
 	.num_standard = ARRAY_SIZE(r8192_wx_handlers),
 	.private = r8192_private_handler,
 	.num_private = ARRAY_SIZE(r8192_private_handler),
 	.num_private_args = sizeof(r8192_private_args) / sizeof(struct iw_priv_args),
-#if WIRELESS_EXT >= 17
 	.get_wireless_stats = r8192_get_wireless_stats,
-#endif
 	.private_args = (struct iw_priv_args *)r8192_private_args,
 };
diff --git a/drivers/staging/rtl8192u/r819xU_cmdpkt.c b/drivers/staging/rtl8192u/r819xU_cmdpkt.c
index 723c8630e9e..545f49ec9c0 100644
--- a/drivers/staging/rtl8192u/r819xU_cmdpkt.c
+++ b/drivers/staging/rtl8192u/r819xU_cmdpkt.c
@@ -28,7 +28,6 @@
 
 rt_status SendTxCommandPacket(struct net_device *dev, void *pData, u32 DataLen)
 {
-	rt_status	rtStatus = RT_STATUS_SUCCESS;
 	struct r8192_priv   *priv = ieee80211_priv(dev);
 	struct sk_buff	    *skb;
 	cb_desc		    *tcb_desc;
@@ -58,7 +57,7 @@ rt_status SendTxCommandPacket(struct net_device *dev, void *pData, u32 DataLen)
 		priv->ieee80211->softmac_hard_start_xmit(skb, dev);
 	}
 
-	return rtStatus;
+	return RT_STATUS_SUCCESS;
 }
 
 /*-----------------------------------------------------------------------------
diff --git a/drivers/staging/rtl8192u/r819xU_firmware.c b/drivers/staging/rtl8192u/r819xU_firmware.c
index ecfb66538eb..04b8f5ec67e 100644
--- a/drivers/staging/rtl8192u/r819xU_firmware.c
+++ b/drivers/staging/rtl8192u/r819xU_firmware.c
@@ -162,7 +162,7 @@ static bool CPUcheck_maincodeok_turnonCPU(struct net_device *dev)
 	return rt_status;
 
 CPUCheckMainCodeOKAndTurnOnCPU_Fail:
-	RT_TRACE(COMP_ERR, "ERR in %s()\n", __FUNCTION__);
+	RT_TRACE(COMP_ERR, "ERR in %s()\n", __func__);
 	rt_status = FALSE;
 	return rt_status;
 }
@@ -191,7 +191,7 @@ static bool CPUcheck_firmware_ready(struct net_device *dev)
 	return rt_status;
 
 CPUCheckFirmwareReady_Fail:
-	RT_TRACE(COMP_ERR, "ERR in %s()\n", __FUNCTION__);
+	RT_TRACE(COMP_ERR, "ERR in %s()\n", __func__);
 	rt_status = false;
 	return rt_status;
 
@@ -277,13 +277,11 @@ bool init_firmware(struct net_device *dev)
 		 *   and Tx descriptor info
 		 * */
 		rt_status = fw_download_code(dev,mapped_file,file_length);
-		if (rst_opt == OPT_SYSTEM_RESET) {
+		if (rst_opt == OPT_SYSTEM_RESET)
 			release_firmware(fw_entry);
-		}
 
-		if (rt_status != TRUE) {
+		if (rt_status != TRUE)
 			goto download_firmware_fail;
-		}
 
 		switch (init_step) {
 		case FW_INIT_STEP0_BOOT:
@@ -336,7 +334,7 @@ bool init_firmware(struct net_device *dev)
 	return rt_status;
 
 download_firmware_fail:
-	RT_TRACE(COMP_ERR, "ERR in %s()\n", __FUNCTION__);
+	RT_TRACE(COMP_ERR, "ERR in %s()\n", __func__);
 	rt_status = FALSE;
 	return rt_status;
 
diff --git a/drivers/staging/rtl8192u/r819xU_firmware_img.c b/drivers/staging/rtl8192u/r819xU_firmware_img.c
index 0785de72a5e..4eb43cfe569 100644
--- a/drivers/staging/rtl8192u/r819xU_firmware_img.c
+++ b/drivers/staging/rtl8192u/r819xU_firmware_img.c
@@ -3,546 +3,546 @@
 #include "r819xU_firmware_img.h"
 
 u32 Rtl8192UsbPHY_REGArray[] = {
-0x0, };
+	0x0, };
 
 u32 Rtl8192UsbPHY_REG_1T2RArray[] = {
-0x800,0x00000000,
-0x804,0x00000001,
-0x808,0x0000fc00,
-0x80c,0x0000001c,
-0x810,0x801010aa,
-0x814,0x008514d0,
-0x818,0x00000040,
-0x81c,0x00000000,
-0x820,0x00000004,
-0x824,0x00690000,
-0x828,0x00000004,
-0x82c,0x00e90000,
-0x830,0x00000004,
-0x834,0x00690000,
-0x838,0x00000004,
-0x83c,0x00e90000,
-0x840,0x00000000,
-0x844,0x00000000,
-0x848,0x00000000,
-0x84c,0x00000000,
-0x850,0x00000000,
-0x854,0x00000000,
-0x858,0x65a965a9,
-0x85c,0x65a965a9,
-0x860,0x001f0010,
-0x864,0x007f0010,
-0x868,0x001f0010,
-0x86c,0x007f0010,
-0x870,0x0f100f70,
-0x874,0x0f100f70,
-0x878,0x00000000,
-0x87c,0x00000000,
-0x880,0x6870e36c,
-0x884,0xe3573600,
-0x888,0x4260c340,
-0x88c,0x0000ff00,
-0x890,0x00000000,
-0x894,0xfffffffe,
-0x898,0x4c42382f,
-0x89c,0x00656056,
-0x8b0,0x00000000,
-0x8e0,0x00000000,
-0x8e4,0x00000000,
-0x900,0x00000000,
-0x904,0x00000023,
-0x908,0x00000000,
-0x90c,0x31121311,
-0xa00,0x00d0c7d8,
-0xa04,0x811f0008,
-0xa08,0x80cd8300,
-0xa0c,0x2e62740f,
-0xa10,0x95009b78,
-0xa14,0x11145008,
-0xa18,0x00881117,
-0xa1c,0x89140fa0,
-0xa20,0x1a1b0000,
-0xa24,0x090e1317,
-0xa28,0x00000204,
-0xa2c,0x00000000,
-0xc00,0x00000040,
-0xc04,0x00005433,
-0xc08,0x000000e4,
-0xc0c,0x6c6c6c6c,
-0xc10,0x08800000,
-0xc14,0x40000100,
-0xc18,0x08000000,
-0xc1c,0x40000100,
-0xc20,0x08000000,
-0xc24,0x40000100,
-0xc28,0x08000000,
-0xc2c,0x40000100,
-0xc30,0x6de9ac44,
-0xc34,0x465c52cd,
-0xc38,0x497f5994,
-0xc3c,0x0a969764,
-0xc40,0x1f7c403f,
-0xc44,0x000100b7,
-0xc48,0xec020000,
-0xc4c,0x00000300,
-0xc50,0x69543420,
-0xc54,0x433c0094,
-0xc58,0x69543420,
-0xc5c,0x433c0094,
-0xc60,0x69543420,
-0xc64,0x433c0094,
-0xc68,0x69543420,
-0xc6c,0x433c0094,
-0xc70,0x2c7f000d,
-0xc74,0x0186175b,
-0xc78,0x0000001f,
-0xc7c,0x00b91612,
-0xc80,0x40000100,
-0xc84,0x20000000,
-0xc88,0x40000100,
-0xc8c,0x20200000,
-0xc90,0x40000100,
-0xc94,0x00000000,
-0xc98,0x40000100,
-0xc9c,0x00000000,
-0xca0,0x00492492,
-0xca4,0x00000000,
-0xca8,0x00000000,
-0xcac,0x00000000,
-0xcb0,0x00000000,
-0xcb4,0x00000000,
-0xcb8,0x00000000,
-0xcbc,0x00492492,
-0xcc0,0x00000000,
-0xcc4,0x00000000,
-0xcc8,0x00000000,
-0xccc,0x00000000,
-0xcd0,0x00000000,
-0xcd4,0x00000000,
-0xcd8,0x64b22427,
-0xcdc,0x00766932,
-0xce0,0x00222222,
-0xd00,0x00000750,
-0xd04,0x00000403,
-0xd08,0x0000907f,
-0xd0c,0x00000001,
-0xd10,0xa0633333,
-0xd14,0x33333c63,
-0xd18,0x6a8f5b6b,
-0xd1c,0x00000000,
-0xd20,0x00000000,
-0xd24,0x00000000,
-0xd28,0x00000000,
-0xd2c,0xcc979975,
-0xd30,0x00000000,
-0xd34,0x00000000,
-0xd38,0x00000000,
-0xd3c,0x00027293,
-0xd40,0x00000000,
-0xd44,0x00000000,
-0xd48,0x00000000,
-0xd4c,0x00000000,
-0xd50,0x6437140a,
-0xd54,0x024dbd02,
-0xd58,0x00000000,
-0xd5c,0x04032064,
-0xe00,0x161a1a1a,
-0xe04,0x12121416,
-0xe08,0x00001800,
-0xe0c,0x00000000,
-0xe10,0x161a1a1a,
-0xe14,0x12121416,
-0xe18,0x161a1a1a,
-0xe1c,0x12121416,
+	0x800, 0x00000000,
+	0x804, 0x00000001,
+	0x808, 0x0000fc00,
+	0x80c, 0x0000001c,
+	0x810, 0x801010aa,
+	0x814, 0x008514d0,
+	0x818, 0x00000040,
+	0x81c, 0x00000000,
+	0x820, 0x00000004,
+	0x824, 0x00690000,
+	0x828, 0x00000004,
+	0x82c, 0x00e90000,
+	0x830, 0x00000004,
+	0x834, 0x00690000,
+	0x838, 0x00000004,
+	0x83c, 0x00e90000,
+	0x840, 0x00000000,
+	0x844, 0x00000000,
+	0x848, 0x00000000,
+	0x84c, 0x00000000,
+	0x850, 0x00000000,
+	0x854, 0x00000000,
+	0x858, 0x65a965a9,
+	0x85c, 0x65a965a9,
+	0x860, 0x001f0010,
+	0x864, 0x007f0010,
+	0x868, 0x001f0010,
+	0x86c, 0x007f0010,
+	0x870, 0x0f100f70,
+	0x874, 0x0f100f70,
+	0x878, 0x00000000,
+	0x87c, 0x00000000,
+	0x880, 0x6870e36c,
+	0x884, 0xe3573600,
+	0x888, 0x4260c340,
+	0x88c, 0x0000ff00,
+	0x890, 0x00000000,
+	0x894, 0xfffffffe,
+	0x898, 0x4c42382f,
+	0x89c, 0x00656056,
+	0x8b0, 0x00000000,
+	0x8e0, 0x00000000,
+	0x8e4, 0x00000000,
+	0x900, 0x00000000,
+	0x904, 0x00000023,
+	0x908, 0x00000000,
+	0x90c, 0x31121311,
+	0xa00, 0x00d0c7d8,
+	0xa04, 0x811f0008,
+	0xa08, 0x80cd8300,
+	0xa0c, 0x2e62740f,
+	0xa10, 0x95009b78,
+	0xa14, 0x11145008,
+	0xa18, 0x00881117,
+	0xa1c, 0x89140fa0,
+	0xa20, 0x1a1b0000,
+	0xa24, 0x090e1317,
+	0xa28, 0x00000204,
+	0xa2c, 0x00000000,
+	0xc00, 0x00000040,
+	0xc04, 0x00005433,
+	0xc08, 0x000000e4,
+	0xc0c, 0x6c6c6c6c,
+	0xc10, 0x08800000,
+	0xc14, 0x40000100,
+	0xc18, 0x08000000,
+	0xc1c, 0x40000100,
+	0xc20, 0x08000000,
+	0xc24, 0x40000100,
+	0xc28, 0x08000000,
+	0xc2c, 0x40000100,
+	0xc30, 0x6de9ac44,
+	0xc34, 0x465c52cd,
+	0xc38, 0x497f5994,
+	0xc3c, 0x0a969764,
+	0xc40, 0x1f7c403f,
+	0xc44, 0x000100b7,
+	0xc48, 0xec020000,
+	0xc4c, 0x00000300,
+	0xc50, 0x69543420,
+	0xc54, 0x433c0094,
+	0xc58, 0x69543420,
+	0xc5c, 0x433c0094,
+	0xc60, 0x69543420,
+	0xc64, 0x433c0094,
+	0xc68, 0x69543420,
+	0xc6c, 0x433c0094,
+	0xc70, 0x2c7f000d,
+	0xc74, 0x0186175b,
+	0xc78, 0x0000001f,
+	0xc7c, 0x00b91612,
+	0xc80, 0x40000100,
+	0xc84, 0x20000000,
+	0xc88, 0x40000100,
+	0xc8c, 0x20200000,
+	0xc90, 0x40000100,
+	0xc94, 0x00000000,
+	0xc98, 0x40000100,
+	0xc9c, 0x00000000,
+	0xca0, 0x00492492,
+	0xca4, 0x00000000,
+	0xca8, 0x00000000,
+	0xcac, 0x00000000,
+	0xcb0, 0x00000000,
+	0xcb4, 0x00000000,
+	0xcb8, 0x00000000,
+	0xcbc, 0x00492492,
+	0xcc0, 0x00000000,
+	0xcc4, 0x00000000,
+	0xcc8, 0x00000000,
+	0xccc, 0x00000000,
+	0xcd0, 0x00000000,
+	0xcd4, 0x00000000,
+	0xcd8, 0x64b22427,
+	0xcdc, 0x00766932,
+	0xce0, 0x00222222,
+	0xd00, 0x00000750,
+	0xd04, 0x00000403,
+	0xd08, 0x0000907f,
+	0xd0c, 0x00000001,
+	0xd10, 0xa0633333,
+	0xd14, 0x33333c63,
+	0xd18, 0x6a8f5b6b,
+	0xd1c, 0x00000000,
+	0xd20, 0x00000000,
+	0xd24, 0x00000000,
+	0xd28, 0x00000000,
+	0xd2c, 0xcc979975,
+	0xd30, 0x00000000,
+	0xd34, 0x00000000,
+	0xd38, 0x00000000,
+	0xd3c, 0x00027293,
+	0xd40, 0x00000000,
+	0xd44, 0x00000000,
+	0xd48, 0x00000000,
+	0xd4c, 0x00000000,
+	0xd50, 0x6437140a,
+	0xd54, 0x024dbd02,
+	0xd58, 0x00000000,
+	0xd5c, 0x04032064,
+	0xe00, 0x161a1a1a,
+	0xe04, 0x12121416,
+	0xe08, 0x00001800,
+	0xe0c, 0x00000000,
+	0xe10, 0x161a1a1a,
+	0xe14, 0x12121416,
+	0xe18, 0x161a1a1a,
+	0xe1c, 0x12121416,
 };
 
 u32 Rtl8192UsbRadioA_Array[] = {
-0x019,0x00000003,
-0x000,0x000000bf,
-0x001,0x00000ee0,
-0x002,0x0000004c,
-0x003,0x000007f1,
-0x004,0x00000975,
-0x005,0x00000c58,
-0x006,0x00000ae6,
-0x007,0x000000ca,
-0x008,0x00000e1c,
-0x009,0x000007f0,
-0x00a,0x000009d0,
-0x00b,0x000001ba,
-0x00c,0x00000240,
-0x00e,0x00000020,
-0x00f,0x00000990,
-0x012,0x00000806,
-0x014,0x000005ab,
-0x015,0x00000f80,
-0x016,0x00000020,
-0x017,0x00000597,
-0x018,0x0000050a,
-0x01a,0x00000f80,
-0x01b,0x00000f5e,
-0x01c,0x00000008,
-0x01d,0x00000607,
-0x01e,0x000006cc,
-0x01f,0x00000000,
-0x020,0x000001a5,
-0x01f,0x00000001,
-0x020,0x00000165,
-0x01f,0x00000002,
-0x020,0x000000c6,
-0x01f,0x00000003,
-0x020,0x00000086,
-0x01f,0x00000004,
-0x020,0x00000046,
-0x01f,0x00000005,
-0x020,0x000001e6,
-0x01f,0x00000006,
-0x020,0x000001a6,
-0x01f,0x00000007,
-0x020,0x00000166,
-0x01f,0x00000008,
-0x020,0x000000c7,
-0x01f,0x00000009,
-0x020,0x00000087,
-0x01f,0x0000000a,
-0x020,0x000000f7,
-0x01f,0x0000000b,
-0x020,0x000000d7,
-0x01f,0x0000000c,
-0x020,0x000000b7,
-0x01f,0x0000000d,
-0x020,0x00000097,
-0x01f,0x0000000e,
-0x020,0x00000077,
-0x01f,0x0000000f,
-0x020,0x00000057,
-0x01f,0x00000010,
-0x020,0x00000037,
-0x01f,0x00000011,
-0x020,0x000000fb,
-0x01f,0x00000012,
-0x020,0x000000db,
-0x01f,0x00000013,
-0x020,0x000000bb,
-0x01f,0x00000014,
-0x020,0x000000ff,
-0x01f,0x00000015,
-0x020,0x000000e3,
-0x01f,0x00000016,
-0x020,0x000000c3,
-0x01f,0x00000017,
-0x020,0x000000a3,
-0x01f,0x00000018,
-0x020,0x00000083,
-0x01f,0x00000019,
-0x020,0x00000063,
-0x01f,0x0000001a,
-0x020,0x00000043,
-0x01f,0x0000001b,
-0x020,0x00000023,
-0x01f,0x0000001c,
-0x020,0x00000003,
-0x01f,0x0000001d,
-0x020,0x000001e3,
-0x01f,0x0000001e,
-0x020,0x000001c3,
-0x01f,0x0000001f,
-0x020,0x000001a3,
-0x01f,0x00000020,
-0x020,0x00000183,
-0x01f,0x00000021,
-0x020,0x00000163,
-0x01f,0x00000022,
-0x020,0x00000143,
-0x01f,0x00000023,
-0x020,0x00000123,
-0x01f,0x00000024,
-0x020,0x00000103,
-0x023,0x00000203,
-0x024,0x00000200,
-0x00b,0x000001ba,
-0x02c,0x000003d7,
-0x02d,0x00000ff0,
-0x000,0x00000037,
-0x004,0x00000160,
-0x007,0x00000080,
-0x002,0x0000088d,
-0x0fe,0x00000000,
-0x0fe,0x00000000,
-0x016,0x00000200,
-0x016,0x00000380,
-0x016,0x00000020,
-0x016,0x000001a0,
-0x000,0x000000bf,
-0x00d,0x0000001f,
-0x00d,0x00000c9f,
-0x002,0x0000004d,
-0x000,0x00000cbf,
-0x004,0x00000975,
-0x007,0x00000700,
+	0x019, 0x00000003,
+	0x000, 0x000000bf,
+	0x001, 0x00000ee0,
+	0x002, 0x0000004c,
+	0x003, 0x000007f1,
+	0x004, 0x00000975,
+	0x005, 0x00000c58,
+	0x006, 0x00000ae6,
+	0x007, 0x000000ca,
+	0x008, 0x00000e1c,
+	0x009, 0x000007f0,
+	0x00a, 0x000009d0,
+	0x00b, 0x000001ba,
+	0x00c, 0x00000240,
+	0x00e, 0x00000020,
+	0x00f, 0x00000990,
+	0x012, 0x00000806,
+	0x014, 0x000005ab,
+	0x015, 0x00000f80,
+	0x016, 0x00000020,
+	0x017, 0x00000597,
+	0x018, 0x0000050a,
+	0x01a, 0x00000f80,
+	0x01b, 0x00000f5e,
+	0x01c, 0x00000008,
+	0x01d, 0x00000607,
+	0x01e, 0x000006cc,
+	0x01f, 0x00000000,
+	0x020, 0x000001a5,
+	0x01f, 0x00000001,
+	0x020, 0x00000165,
+	0x01f, 0x00000002,
+	0x020, 0x000000c6,
+	0x01f, 0x00000003,
+	0x020, 0x00000086,
+	0x01f, 0x00000004,
+	0x020, 0x00000046,
+	0x01f, 0x00000005,
+	0x020, 0x000001e6,
+	0x01f, 0x00000006,
+	0x020, 0x000001a6,
+	0x01f, 0x00000007,
+	0x020, 0x00000166,
+	0x01f, 0x00000008,
+	0x020, 0x000000c7,
+	0x01f, 0x00000009,
+	0x020, 0x00000087,
+	0x01f, 0x0000000a,
+	0x020, 0x000000f7,
+	0x01f, 0x0000000b,
+	0x020, 0x000000d7,
+	0x01f, 0x0000000c,
+	0x020, 0x000000b7,
+	0x01f, 0x0000000d,
+	0x020, 0x00000097,
+	0x01f, 0x0000000e,
+	0x020, 0x00000077,
+	0x01f, 0x0000000f,
+	0x020, 0x00000057,
+	0x01f, 0x00000010,
+	0x020, 0x00000037,
+	0x01f, 0x00000011,
+	0x020, 0x000000fb,
+	0x01f, 0x00000012,
+	0x020, 0x000000db,
+	0x01f, 0x00000013,
+	0x020, 0x000000bb,
+	0x01f, 0x00000014,
+	0x020, 0x000000ff,
+	0x01f, 0x00000015,
+	0x020, 0x000000e3,
+	0x01f, 0x00000016,
+	0x020, 0x000000c3,
+	0x01f, 0x00000017,
+	0x020, 0x000000a3,
+	0x01f, 0x00000018,
+	0x020, 0x00000083,
+	0x01f, 0x00000019,
+	0x020, 0x00000063,
+	0x01f, 0x0000001a,
+	0x020, 0x00000043,
+	0x01f, 0x0000001b,
+	0x020, 0x00000023,
+	0x01f, 0x0000001c,
+	0x020, 0x00000003,
+	0x01f, 0x0000001d,
+	0x020, 0x000001e3,
+	0x01f, 0x0000001e,
+	0x020, 0x000001c3,
+	0x01f, 0x0000001f,
+	0x020, 0x000001a3,
+	0x01f, 0x00000020,
+	0x020, 0x00000183,
+	0x01f, 0x00000021,
+	0x020, 0x00000163,
+	0x01f, 0x00000022,
+	0x020, 0x00000143,
+	0x01f, 0x00000023,
+	0x020, 0x00000123,
+	0x01f, 0x00000024,
+	0x020, 0x00000103,
+	0x023, 0x00000203,
+	0x024, 0x00000200,
+	0x00b, 0x000001ba,
+	0x02c, 0x000003d7,
+	0x02d, 0x00000ff0,
+	0x000, 0x00000037,
+	0x004, 0x00000160,
+	0x007, 0x00000080,
+	0x002, 0x0000088d,
+	0x0fe, 0x00000000,
+	0x0fe, 0x00000000,
+	0x016, 0x00000200,
+	0x016, 0x00000380,
+	0x016, 0x00000020,
+	0x016, 0x000001a0,
+	0x000, 0x000000bf,
+	0x00d, 0x0000001f,
+	0x00d, 0x00000c9f,
+	0x002, 0x0000004d,
+	0x000, 0x00000cbf,
+	0x004, 0x00000975,
+	0x007, 0x00000700,
 };
 
 u32 Rtl8192UsbRadioB_Array[] = {
-0x019,0x00000003,
-0x000,0x000000bf,
-0x001,0x000006e0,
-0x002,0x0000004c,
-0x003,0x000007f1,
-0x004,0x00000975,
-0x005,0x00000c58,
-0x006,0x00000ae6,
-0x007,0x000000ca,
-0x008,0x00000e1c,
-0x000,0x000000b7,
-0x00a,0x00000850,
-0x000,0x000000bf,
-0x00b,0x000001ba,
-0x00c,0x00000240,
-0x00e,0x00000020,
-0x015,0x00000f80,
-0x016,0x00000020,
-0x017,0x00000597,
-0x018,0x0000050a,
-0x01a,0x00000e00,
-0x01b,0x00000f5e,
-0x01d,0x00000607,
-0x01e,0x000006cc,
-0x00b,0x000001ba,
-0x023,0x00000203,
-0x024,0x00000200,
-0x000,0x00000037,
-0x004,0x00000160,
-0x016,0x00000200,
-0x016,0x00000380,
-0x016,0x00000020,
-0x016,0x000001a0,
-0x00d,0x00000ccc,
-0x000,0x000000bf,
-0x002,0x0000004d,
-0x000,0x00000cbf,
-0x004,0x00000975,
-0x007,0x00000700,
+	0x019, 0x00000003,
+	0x000, 0x000000bf,
+	0x001, 0x000006e0,
+	0x002, 0x0000004c,
+	0x003, 0x000007f1,
+	0x004, 0x00000975,
+	0x005, 0x00000c58,
+	0x006, 0x00000ae6,
+	0x007, 0x000000ca,
+	0x008, 0x00000e1c,
+	0x000, 0x000000b7,
+	0x00a, 0x00000850,
+	0x000, 0x000000bf,
+	0x00b, 0x000001ba,
+	0x00c, 0x00000240,
+	0x00e, 0x00000020,
+	0x015, 0x00000f80,
+	0x016, 0x00000020,
+	0x017, 0x00000597,
+	0x018, 0x0000050a,
+	0x01a, 0x00000e00,
+	0x01b, 0x00000f5e,
+	0x01d, 0x00000607,
+	0x01e, 0x000006cc,
+	0x00b, 0x000001ba,
+	0x023, 0x00000203,
+	0x024, 0x00000200,
+	0x000, 0x00000037,
+	0x004, 0x00000160,
+	0x016, 0x00000200,
+	0x016, 0x00000380,
+	0x016, 0x00000020,
+	0x016, 0x000001a0,
+	0x00d, 0x00000ccc,
+	0x000, 0x000000bf,
+	0x002, 0x0000004d,
+	0x000, 0x00000cbf,
+	0x004, 0x00000975,
+	0x007, 0x00000700,
 };
 
 u32 Rtl8192UsbRadioC_Array[] = {
-0x0, };
+	0x0, };
 
 u32 Rtl8192UsbRadioD_Array[] = {
-0x0, };
+	0x0, };
 
 u32 Rtl8192UsbMACPHY_Array[] = {
-0x03c,0xffff0000,0x00000f0f,
-0x340,0xffffffff,0x161a1a1a,
-0x344,0xffffffff,0x12121416,
-0x348,0x0000ffff,0x00001818,
-0x12c,0xffffffff,0x04000802,
-0x318,0x00000fff,0x00000100,
+	0x03c, 0xffff0000, 0x00000f0f,
+	0x340, 0xffffffff, 0x161a1a1a,
+	0x344, 0xffffffff, 0x12121416,
+	0x348, 0x0000ffff, 0x00001818,
+	0x12c, 0xffffffff, 0x04000802,
+	0x318, 0x00000fff, 0x00000100,
 };
 
 u32 Rtl8192UsbMACPHY_Array_PG[] = {
-0x03c,0xffff0000,0x00000f0f,
-0xe00,0xffffffff,0x06090909,
-0xe04,0xffffffff,0x00030306,
-0xe08,0x0000ff00,0x00000000,
-0xe10,0xffffffff,0x0a0c0d0f,
-0xe14,0xffffffff,0x06070809,
-0xe18,0xffffffff,0x0a0c0d0f,
-0xe1c,0xffffffff,0x06070809,
-0x12c,0xffffffff,0x04000802,
-0x318,0x00000fff,0x00000800,
+	0x03c, 0xffff0000, 0x00000f0f,
+	0xe00, 0xffffffff, 0x06090909,
+	0xe04, 0xffffffff, 0x00030306,
+	0xe08, 0x0000ff00, 0x00000000,
+	0xe10, 0xffffffff, 0x0a0c0d0f,
+	0xe14, 0xffffffff, 0x06070809,
+	0xe18, 0xffffffff, 0x0a0c0d0f,
+	0xe1c, 0xffffffff, 0x06070809,
+	0x12c, 0xffffffff, 0x04000802,
+	0x318, 0x00000fff, 0x00000800,
 };
 
 u32 Rtl8192UsbAGCTAB_Array[] = {
-0xc78,0x7d000001,
-0xc78,0x7d010001,
-0xc78,0x7d020001,
-0xc78,0x7d030001,
-0xc78,0x7d040001,
-0xc78,0x7d050001,
-0xc78,0x7c060001,
-0xc78,0x7b070001,
-0xc78,0x7a080001,
-0xc78,0x79090001,
-0xc78,0x780a0001,
-0xc78,0x770b0001,
-0xc78,0x760c0001,
-0xc78,0x750d0001,
-0xc78,0x740e0001,
-0xc78,0x730f0001,
-0xc78,0x72100001,
-0xc78,0x71110001,
-0xc78,0x70120001,
-0xc78,0x6f130001,
-0xc78,0x6e140001,
-0xc78,0x6d150001,
-0xc78,0x6c160001,
-0xc78,0x6b170001,
-0xc78,0x6a180001,
-0xc78,0x69190001,
-0xc78,0x681a0001,
-0xc78,0x671b0001,
-0xc78,0x661c0001,
-0xc78,0x651d0001,
-0xc78,0x641e0001,
-0xc78,0x491f0001,
-0xc78,0x48200001,
-0xc78,0x47210001,
-0xc78,0x46220001,
-0xc78,0x45230001,
-0xc78,0x44240001,
-0xc78,0x43250001,
-0xc78,0x28260001,
-0xc78,0x27270001,
-0xc78,0x26280001,
-0xc78,0x25290001,
-0xc78,0x242a0001,
-0xc78,0x232b0001,
-0xc78,0x222c0001,
-0xc78,0x212d0001,
-0xc78,0x202e0001,
-0xc78,0x0a2f0001,
-0xc78,0x08300001,
-0xc78,0x06310001,
-0xc78,0x05320001,
-0xc78,0x04330001,
-0xc78,0x03340001,
-0xc78,0x02350001,
-0xc78,0x01360001,
-0xc78,0x00370001,
-0xc78,0x00380001,
-0xc78,0x00390001,
-0xc78,0x003a0001,
-0xc78,0x003b0001,
-0xc78,0x003c0001,
-0xc78,0x003d0001,
-0xc78,0x003e0001,
-0xc78,0x003f0001,
-0xc78,0x7d400001,
-0xc78,0x7d410001,
-0xc78,0x7d420001,
-0xc78,0x7d430001,
-0xc78,0x7d440001,
-0xc78,0x7d450001,
-0xc78,0x7c460001,
-0xc78,0x7b470001,
-0xc78,0x7a480001,
-0xc78,0x79490001,
-0xc78,0x784a0001,
-0xc78,0x774b0001,
-0xc78,0x764c0001,
-0xc78,0x754d0001,
-0xc78,0x744e0001,
-0xc78,0x734f0001,
-0xc78,0x72500001,
-0xc78,0x71510001,
-0xc78,0x70520001,
-0xc78,0x6f530001,
-0xc78,0x6e540001,
-0xc78,0x6d550001,
-0xc78,0x6c560001,
-0xc78,0x6b570001,
-0xc78,0x6a580001,
-0xc78,0x69590001,
-0xc78,0x685a0001,
-0xc78,0x675b0001,
-0xc78,0x665c0001,
-0xc78,0x655d0001,
-0xc78,0x645e0001,
-0xc78,0x495f0001,
-0xc78,0x48600001,
-0xc78,0x47610001,
-0xc78,0x46620001,
-0xc78,0x45630001,
-0xc78,0x44640001,
-0xc78,0x43650001,
-0xc78,0x28660001,
-0xc78,0x27670001,
-0xc78,0x26680001,
-0xc78,0x25690001,
-0xc78,0x246a0001,
-0xc78,0x236b0001,
-0xc78,0x226c0001,
-0xc78,0x216d0001,
-0xc78,0x206e0001,
-0xc78,0x0a6f0001,
-0xc78,0x08700001,
-0xc78,0x06710001,
-0xc78,0x05720001,
-0xc78,0x04730001,
-0xc78,0x03740001,
-0xc78,0x02750001,
-0xc78,0x01760001,
-0xc78,0x00770001,
-0xc78,0x00780001,
-0xc78,0x00790001,
-0xc78,0x007a0001,
-0xc78,0x007b0001,
-0xc78,0x007c0001,
-0xc78,0x007d0001,
-0xc78,0x007e0001,
-0xc78,0x007f0001,
-0xc78,0x2e00001e,
-0xc78,0x2e01001e,
-0xc78,0x2e02001e,
-0xc78,0x2e03001e,
-0xc78,0x2e04001e,
-0xc78,0x2e05001e,
-0xc78,0x3006001e,
-0xc78,0x3407001e,
-0xc78,0x3908001e,
-0xc78,0x3c09001e,
-0xc78,0x3f0a001e,
-0xc78,0x420b001e,
-0xc78,0x440c001e,
-0xc78,0x450d001e,
-0xc78,0x460e001e,
-0xc78,0x460f001e,
-0xc78,0x4710001e,
-0xc78,0x4811001e,
-0xc78,0x4912001e,
-0xc78,0x4a13001e,
-0xc78,0x4b14001e,
-0xc78,0x4b15001e,
-0xc78,0x4c16001e,
-0xc78,0x4d17001e,
-0xc78,0x4e18001e,
-0xc78,0x4f19001e,
-0xc78,0x4f1a001e,
-0xc78,0x501b001e,
-0xc78,0x511c001e,
-0xc78,0x521d001e,
-0xc78,0x521e001e,
-0xc78,0x531f001e,
-0xc78,0x5320001e,
-0xc78,0x5421001e,
-0xc78,0x5522001e,
-0xc78,0x5523001e,
-0xc78,0x5624001e,
-0xc78,0x5725001e,
-0xc78,0x5726001e,
-0xc78,0x5827001e,
-0xc78,0x5828001e,
-0xc78,0x5929001e,
-0xc78,0x592a001e,
-0xc78,0x5a2b001e,
-0xc78,0x5b2c001e,
-0xc78,0x5c2d001e,
-0xc78,0x5c2e001e,
-0xc78,0x5d2f001e,
-0xc78,0x5e30001e,
-0xc78,0x5f31001e,
-0xc78,0x6032001e,
-0xc78,0x6033001e,
-0xc78,0x6134001e,
-0xc78,0x6235001e,
-0xc78,0x6336001e,
-0xc78,0x6437001e,
-0xc78,0x6438001e,
-0xc78,0x6539001e,
-0xc78,0x663a001e,
-0xc78,0x673b001e,
-0xc78,0x673c001e,
-0xc78,0x683d001e,
-0xc78,0x693e001e,
-0xc78,0x6a3f001e,
+	0xc78, 0x7d000001,
+	0xc78, 0x7d010001,
+	0xc78, 0x7d020001,
+	0xc78, 0x7d030001,
+	0xc78, 0x7d040001,
+	0xc78, 0x7d050001,
+	0xc78, 0x7c060001,
+	0xc78, 0x7b070001,
+	0xc78, 0x7a080001,
+	0xc78, 0x79090001,
+	0xc78, 0x780a0001,
+	0xc78, 0x770b0001,
+	0xc78, 0x760c0001,
+	0xc78, 0x750d0001,
+	0xc78, 0x740e0001,
+	0xc78, 0x730f0001,
+	0xc78, 0x72100001,
+	0xc78, 0x71110001,
+	0xc78, 0x70120001,
+	0xc78, 0x6f130001,
+	0xc78, 0x6e140001,
+	0xc78, 0x6d150001,
+	0xc78, 0x6c160001,
+	0xc78, 0x6b170001,
+	0xc78, 0x6a180001,
+	0xc78, 0x69190001,
+	0xc78, 0x681a0001,
+	0xc78, 0x671b0001,
+	0xc78, 0x661c0001,
+	0xc78, 0x651d0001,
+	0xc78, 0x641e0001,
+	0xc78, 0x491f0001,
+	0xc78, 0x48200001,
+	0xc78, 0x47210001,
+	0xc78, 0x46220001,
+	0xc78, 0x45230001,
+	0xc78, 0x44240001,
+	0xc78, 0x43250001,
+	0xc78, 0x28260001,
+	0xc78, 0x27270001,
+	0xc78, 0x26280001,
+	0xc78, 0x25290001,
+	0xc78, 0x242a0001,
+	0xc78, 0x232b0001,
+	0xc78, 0x222c0001,
+	0xc78, 0x212d0001,
+	0xc78, 0x202e0001,
+	0xc78, 0x0a2f0001,
+	0xc78, 0x08300001,
+	0xc78, 0x06310001,
+	0xc78, 0x05320001,
+	0xc78, 0x04330001,
+	0xc78, 0x03340001,
+	0xc78, 0x02350001,
+	0xc78, 0x01360001,
+	0xc78, 0x00370001,
+	0xc78, 0x00380001,
+	0xc78, 0x00390001,
+	0xc78, 0x003a0001,
+	0xc78, 0x003b0001,
+	0xc78, 0x003c0001,
+	0xc78, 0x003d0001,
+	0xc78, 0x003e0001,
+	0xc78, 0x003f0001,
+	0xc78, 0x7d400001,
+	0xc78, 0x7d410001,
+	0xc78, 0x7d420001,
+	0xc78, 0x7d430001,
+	0xc78, 0x7d440001,
+	0xc78, 0x7d450001,
+	0xc78, 0x7c460001,
+	0xc78, 0x7b470001,
+	0xc78, 0x7a480001,
+	0xc78, 0x79490001,
+	0xc78, 0x784a0001,
+	0xc78, 0x774b0001,
+	0xc78, 0x764c0001,
+	0xc78, 0x754d0001,
+	0xc78, 0x744e0001,
+	0xc78, 0x734f0001,
+	0xc78, 0x72500001,
+	0xc78, 0x71510001,
+	0xc78, 0x70520001,
+	0xc78, 0x6f530001,
+	0xc78, 0x6e540001,
+	0xc78, 0x6d550001,
+	0xc78, 0x6c560001,
+	0xc78, 0x6b570001,
+	0xc78, 0x6a580001,
+	0xc78, 0x69590001,
+	0xc78, 0x685a0001,
+	0xc78, 0x675b0001,
+	0xc78, 0x665c0001,
+	0xc78, 0x655d0001,
+	0xc78, 0x645e0001,
+	0xc78, 0x495f0001,
+	0xc78, 0x48600001,
+	0xc78, 0x47610001,
+	0xc78, 0x46620001,
+	0xc78, 0x45630001,
+	0xc78, 0x44640001,
+	0xc78, 0x43650001,
+	0xc78, 0x28660001,
+	0xc78, 0x27670001,
+	0xc78, 0x26680001,
+	0xc78, 0x25690001,
+	0xc78, 0x246a0001,
+	0xc78, 0x236b0001,
+	0xc78, 0x226c0001,
+	0xc78, 0x216d0001,
+	0xc78, 0x206e0001,
+	0xc78, 0x0a6f0001,
+	0xc78, 0x08700001,
+	0xc78, 0x06710001,
+	0xc78, 0x05720001,
+	0xc78, 0x04730001,
+	0xc78, 0x03740001,
+	0xc78, 0x02750001,
+	0xc78, 0x01760001,
+	0xc78, 0x00770001,
+	0xc78, 0x00780001,
+	0xc78, 0x00790001,
+	0xc78, 0x007a0001,
+	0xc78, 0x007b0001,
+	0xc78, 0x007c0001,
+	0xc78, 0x007d0001,
+	0xc78, 0x007e0001,
+	0xc78, 0x007f0001,
+	0xc78, 0x2e00001e,
+	0xc78, 0x2e01001e,
+	0xc78, 0x2e02001e,
+	0xc78, 0x2e03001e,
+	0xc78, 0x2e04001e,
+	0xc78, 0x2e05001e,
+	0xc78, 0x3006001e,
+	0xc78, 0x3407001e,
+	0xc78, 0x3908001e,
+	0xc78, 0x3c09001e,
+	0xc78, 0x3f0a001e,
+	0xc78, 0x420b001e,
+	0xc78, 0x440c001e,
+	0xc78, 0x450d001e,
+	0xc78, 0x460e001e,
+	0xc78, 0x460f001e,
+	0xc78, 0x4710001e,
+	0xc78, 0x4811001e,
+	0xc78, 0x4912001e,
+	0xc78, 0x4a13001e,
+	0xc78, 0x4b14001e,
+	0xc78, 0x4b15001e,
+	0xc78, 0x4c16001e,
+	0xc78, 0x4d17001e,
+	0xc78, 0x4e18001e,
+	0xc78, 0x4f19001e,
+	0xc78, 0x4f1a001e,
+	0xc78, 0x501b001e,
+	0xc78, 0x511c001e,
+	0xc78, 0x521d001e,
+	0xc78, 0x521e001e,
+	0xc78, 0x531f001e,
+	0xc78, 0x5320001e,
+	0xc78, 0x5421001e,
+	0xc78, 0x5522001e,
+	0xc78, 0x5523001e,
+	0xc78, 0x5624001e,
+	0xc78, 0x5725001e,
+	0xc78, 0x5726001e,
+	0xc78, 0x5827001e,
+	0xc78, 0x5828001e,
+	0xc78, 0x5929001e,
+	0xc78, 0x592a001e,
+	0xc78, 0x5a2b001e,
+	0xc78, 0x5b2c001e,
+	0xc78, 0x5c2d001e,
+	0xc78, 0x5c2e001e,
+	0xc78, 0x5d2f001e,
+	0xc78, 0x5e30001e,
+	0xc78, 0x5f31001e,
+	0xc78, 0x6032001e,
+	0xc78, 0x6033001e,
+	0xc78, 0x6134001e,
+	0xc78, 0x6235001e,
+	0xc78, 0x6336001e,
+	0xc78, 0x6437001e,
+	0xc78, 0x6438001e,
+	0xc78, 0x6539001e,
+	0xc78, 0x663a001e,
+	0xc78, 0x673b001e,
+	0xc78, 0x673c001e,
+	0xc78, 0x683d001e,
+	0xc78, 0x693e001e,
+	0xc78, 0x6a3f001e,
 };
diff --git a/drivers/staging/rtl8192u/r819xU_phy.c b/drivers/staging/rtl8192u/r819xU_phy.c
index b9f35313c7a..02554c98120 100644
--- a/drivers/staging/rtl8192u/r819xU_phy.c
+++ b/drivers/staging/rtl8192u/r819xU_phy.c
@@ -504,9 +504,8 @@ void rtl8192_phy_configmac(struct net_device *dev)
 		pdwArray = rtl819XMACPHY_Array;
 	}
 	for (i = 0; i < dwArrayLen; i = i+3) {
-		if (pdwArray[i] == 0x318) {
+		if (pdwArray[i] == 0x318)
 			pdwArray[i+2] = 0x00000800;
-		}
 
 		RT_TRACE(COMP_DBG,
 			 "Rtl8190MACPHY_Array[0]=%x Rtl8190MACPHY_Array[1]=%x Rtl8190MACPHY_Array[2]=%x\n",
@@ -992,7 +991,6 @@ u8 rtl8192_phy_ConfigRFWithHeaderFile(struct net_device *dev,
 {
 
 	int i;
-	u8 ret = 0;
 
 	switch (eRFPath) {
 	case RF90_PATH_A:
@@ -1059,7 +1057,7 @@ u8 rtl8192_phy_ConfigRFWithHeaderFile(struct net_device *dev,
 		break;
 	}
 
-	return ret;
+	return 0;
 
 }
 
diff --git a/drivers/staging/rtl8712/hal_init.c b/drivers/staging/rtl8712/hal_init.c
index d801c5af006..36de7e478f3 100644
--- a/drivers/staging/rtl8712/hal_init.c
+++ b/drivers/staging/rtl8712/hal_init.c
@@ -183,7 +183,7 @@ static u8 rtl8712_dl_fw(struct _adapter *padapter)
 		maxlen = (fwhdr.img_IMEM_size > fwhdr.img_SRAM_size) ?
 			  fwhdr.img_IMEM_size : fwhdr.img_SRAM_size;
 		maxlen += txdscp_sz;
-		ptmpchar = _malloc(maxlen + FWBUFF_ALIGN_SZ);
+		ptmpchar = kmalloc(maxlen + FWBUFF_ALIGN_SZ, GFP_ATOMIC);
 		if (ptmpchar == NULL)
 			return ret;
 
diff --git a/drivers/staging/rtl8712/ieee80211.c b/drivers/staging/rtl8712/ieee80211.c
index 57fef70ad98..fe9459e483c 100644
--- a/drivers/staging/rtl8712/ieee80211.c
+++ b/drivers/staging/rtl8712/ieee80211.c
@@ -289,7 +289,7 @@ static int r8712_get_wpa2_cipher_suite(u8 *s)
 int r8712_parse_wpa_ie(u8 *wpa_ie, int wpa_ie_len, int *group_cipher,
 		 int *pairwise_cipher)
 {
-	int i, ret = _SUCCESS;
+	int i;
 	int left, count;
 	u8 *pos;
 
@@ -324,13 +324,13 @@ int r8712_parse_wpa_ie(u8 *wpa_ie, int wpa_ie_len, int *group_cipher,
 		}
 	} else if (left == 1)
 		return _FAIL;
-	return ret;
+	return _SUCCESS;
 }
 
 int r8712_parse_wpa2_ie(u8 *rsn_ie, int rsn_ie_len, int *group_cipher,
 		  int *pairwise_cipher)
 {
-	int i, ret = _SUCCESS;
+	int i;
 	int left, count;
 	u8 *pos;
 
@@ -364,7 +364,7 @@ int r8712_parse_wpa2_ie(u8 *rsn_ie, int rsn_ie_len, int *group_cipher,
 		}
 	} else if (left == 1)
 		return _FAIL;
-	return ret;
+	return _SUCCESS;
 }
 
 int r8712_get_sec_ie(u8 *in_ie, uint in_len, u8 *rsn_ie, u16 *rsn_len,
diff --git a/drivers/staging/rtl8712/mlme_linux.c b/drivers/staging/rtl8712/mlme_linux.c
index f569a7081be..377efb88676 100644
--- a/drivers/staging/rtl8712/mlme_linux.c
+++ b/drivers/staging/rtl8712/mlme_linux.c
@@ -147,10 +147,9 @@ void r8712_report_sec_ie(struct _adapter *adapter, u8 authmode, u8 *sec_ie)
 
 	buff = NULL;
 	if (authmode == _WPA_IE_ID_) {
-		buff = _malloc(IW_CUSTOM_MAX);
+		buff = kzalloc(IW_CUSTOM_MAX, GFP_ATOMIC);
 		if (buff == NULL)
 			return;
-		memset(buff, 0, IW_CUSTOM_MAX);
 		p = buff;
 		p += sprintf(p, "ASSOCINFO(ReqIEs=");
 		len = sec_ie[1] + 2;
diff --git a/drivers/staging/rtl8712/osdep_service.h b/drivers/staging/rtl8712/osdep_service.h
index 566235a14a8..09e156199e7 100644
--- a/drivers/staging/rtl8712/osdep_service.h
+++ b/drivers/staging/rtl8712/osdep_service.h
@@ -168,11 +168,6 @@ static inline void sleep_schedulable(int ms)
 		return;
 }
 
-static inline u8 *_malloc(u32 sz)
-{
-	return	kmalloc(sz, GFP_ATOMIC);
-}
-
 static inline unsigned char _cancel_timer_ex(struct timer_list *ptimer)
 {
 	return del_timer(ptimer);
diff --git a/drivers/staging/rtl8712/rtl8712_cmd.c b/drivers/staging/rtl8712/rtl8712_cmd.c
index 1a4b7a651f9..8ca7d7e68dc 100644
--- a/drivers/staging/rtl8712/rtl8712_cmd.c
+++ b/drivers/staging/rtl8712/rtl8712_cmd.c
@@ -290,8 +290,7 @@ static struct cmd_obj *cmd_hdl_filter(struct _adapter *padapter,
 
 static u8 check_cmd_fifo(struct _adapter *padapter, uint sz)
 {
-	u8 res = _SUCCESS;
-	return res;
+	return _SUCCESS;
 }
 
 u8 r8712_fw_cmd(struct _adapter *pAdapter, u32 cmd)
diff --git a/drivers/staging/rtl8712/rtl8712_recv.c b/drivers/staging/rtl8712/rtl8712_recv.c
index 0723b2f73aa..1f700171af1 100644
--- a/drivers/staging/rtl8712/rtl8712_recv.c
+++ b/drivers/staging/rtl8712/rtl8712_recv.c
@@ -58,12 +58,10 @@ int r8712_init_recv_priv(struct recv_priv *precvpriv, struct _adapter *padapter)
 
 	/*init recv_buf*/
 	_init_queue(&precvpriv->free_recv_buf_queue);
-	precvpriv->pallocated_recv_buf = _malloc(NR_RECVBUFF *
-					 sizeof(struct recv_buf) + 4);
+	precvpriv->pallocated_recv_buf = kzalloc(NR_RECVBUFF * sizeof(struct recv_buf) + 4,
+						 GFP_ATOMIC);
 	if (precvpriv->pallocated_recv_buf == NULL)
 		return _FAIL;
-	memset(precvpriv->pallocated_recv_buf, 0, NR_RECVBUFF *
-		sizeof(struct recv_buf) + 4);
 	precvpriv->precv_buf = precvpriv->pallocated_recv_buf + 4 -
 			      ((addr_t) (precvpriv->pallocated_recv_buf) & 3);
 	precvbuf = (struct recv_buf *)precvpriv->precv_buf;
@@ -123,8 +121,6 @@ void r8712_free_recv_priv(struct recv_priv *precvpriv)
 
 int r8712_init_recvbuf(struct _adapter *padapter, struct recv_buf *precvbuf)
 {
-	int res = _SUCCESS;
-
 	precvbuf->transfer_len = 0;
 	precvbuf->len = 0;
 	precvbuf->ref_cnt = 0;
@@ -134,7 +130,7 @@ int r8712_init_recvbuf(struct _adapter *padapter, struct recv_buf *precvbuf)
 		precvbuf->ptail = precvbuf->pbuf;
 		precvbuf->pend = precvbuf->pdata + MAX_RECVBUF_SZ;
 	}
-	return res;
+	return _SUCCESS;
 }
 
 int r8712_free_recvframe(union recv_frame *precvframe,
@@ -347,7 +343,6 @@ static int amsdu_to_msdu(struct _adapter *padapter, union recv_frame *prframe)
 	_pkt *sub_skb, *subframes[MAX_SUBFRAME_COUNT];
 	struct recv_priv *precvpriv = &padapter->recvpriv;
 	struct  __queue *pfree_recv_queue = &(precvpriv->free_recv_queue);
-	int	ret = _SUCCESS;
 
 	nr_subframes = 0;
 	pattrib = &prframe->u.hdr.attrib;
@@ -435,7 +430,7 @@ static int amsdu_to_msdu(struct _adapter *padapter, union recv_frame *prframe)
 exit:
 	prframe->u.hdr.len = 0;
 	r8712_free_recvframe(prframe, pfree_recv_queue);
-	return ret;
+	return _SUCCESS;
 }
 
 void r8712_rxcmd_event_hdl(struct _adapter *padapter, void *prxcmdbuf)
diff --git a/drivers/staging/rtl8712/rtl871x_cmd.c b/drivers/staging/rtl8712/rtl871x_cmd.c
index a67185db392..7a252200e90 100644
--- a/drivers/staging/rtl8712/rtl871x_cmd.c
+++ b/drivers/staging/rtl8712/rtl871x_cmd.c
@@ -64,13 +64,14 @@ static sint _init_cmd_priv(struct cmd_priv *pcmdpriv)
 
 	/* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */
 	pcmdpriv->cmd_seq = 1;
-	pcmdpriv->cmd_allocated_buf = _malloc(MAX_CMDSZ + CMDBUFF_ALIGN_SZ);
+	pcmdpriv->cmd_allocated_buf = kmalloc(MAX_CMDSZ + CMDBUFF_ALIGN_SZ,
+					      GFP_ATOMIC);
 	if (pcmdpriv->cmd_allocated_buf == NULL)
 		return _FAIL;
 	pcmdpriv->cmd_buf = pcmdpriv->cmd_allocated_buf  +  CMDBUFF_ALIGN_SZ -
 			    ((addr_t)(pcmdpriv->cmd_allocated_buf) &
 			    (CMDBUFF_ALIGN_SZ-1));
-	pcmdpriv->rsp_allocated_buf = _malloc(MAX_RSPSZ + 4);
+	pcmdpriv->rsp_allocated_buf = kmalloc(MAX_RSPSZ + 4, GFP_ATOMIC);
 	if (pcmdpriv->rsp_allocated_buf == NULL)
 		return _FAIL;
 	pcmdpriv->rsp_buf = pcmdpriv->rsp_allocated_buf  +  4 -
@@ -85,7 +86,7 @@ static sint _init_evt_priv(struct evt_priv *pevtpriv)
 {
 	/* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */
 	pevtpriv->event_seq = 0;
-	pevtpriv->evt_allocated_buf = _malloc(MAX_EVTSZ + 4);
+	pevtpriv->evt_allocated_buf = kmalloc(MAX_EVTSZ + 4, GFP_ATOMIC);
 
 	if (pevtpriv->evt_allocated_buf == NULL)
 		return _FAIL;
@@ -226,11 +227,10 @@ u8 r8712_sitesurvey_cmd(struct _adapter *padapter,
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	psurveyPara = (struct sitesurvey_parm *)_malloc(
-		       sizeof(struct sitesurvey_parm));
+	psurveyPara = kmalloc(sizeof(struct sitesurvey_parm), GFP_ATOMIC);
 	if (psurveyPara == NULL) {
 		kfree((unsigned char *) ph2c);
 		return _FAIL;
@@ -259,11 +259,11 @@ u8 r8712_setdatarate_cmd(struct _adapter *padapter, u8 *rateset)
 	struct setdatarate_parm	*pbsetdataratepara;
 	struct cmd_priv		*pcmdpriv = &padapter->cmdpriv;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	pbsetdataratepara = (struct setdatarate_parm *)_malloc(
-			     sizeof(struct setdatarate_parm));
+	pbsetdataratepara = kmalloc(sizeof(struct setdatarate_parm),
+				    GFP_ATOMIC);
 	if (pbsetdataratepara == NULL) {
 		kfree((u8 *) ph2c);
 		return _FAIL;
@@ -282,11 +282,11 @@ u8 r8712_set_chplan_cmd(struct _adapter *padapter, int chplan)
 	struct SetChannelPlan_param *psetchplanpara;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	psetchplanpara = (struct SetChannelPlan_param *)
-		_malloc(sizeof(struct SetChannelPlan_param));
+	psetchplanpara = kmalloc(sizeof(struct SetChannelPlan_param),
+				 GFP_ATOMIC);
 	if (psetchplanpara == NULL) {
 		kfree((u8 *) ph2c);
 		return _FAIL;
@@ -304,11 +304,11 @@ u8 r8712_setbasicrate_cmd(struct _adapter *padapter, u8 *rateset)
 	struct setbasicrate_parm *pssetbasicratepara;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	pssetbasicratepara = (struct setbasicrate_parm *)_malloc(
-			      sizeof(struct setbasicrate_parm));
+	pssetbasicratepara = kmalloc(sizeof(struct setbasicrate_parm),
+				     GFP_ATOMIC);
 	if (pssetbasicratepara == NULL) {
 		kfree((u8 *) ph2c);
 		return _FAIL;
@@ -327,11 +327,10 @@ u8 r8712_setptm_cmd(struct _adapter *padapter, u8 type)
 	struct writePTM_parm	*pwriteptmparm;
 	struct cmd_priv		*pcmdpriv = &padapter->cmdpriv;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	pwriteptmparm = (struct writePTM_parm *)
-		_malloc(sizeof(struct writePTM_parm));
+	pwriteptmparm = kmalloc(sizeof(struct writePTM_parm), GFP_ATOMIC);
 	if (pwriteptmparm == NULL) {
 		kfree((u8 *) ph2c);
 		return _FAIL;
@@ -348,11 +347,10 @@ u8 r8712_setfwdig_cmd(struct _adapter *padapter, u8 type)
 	struct writePTM_parm *pwriteptmparm;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	pwriteptmparm = (struct writePTM_parm *)
-		_malloc(sizeof(struct setdig_parm));
+	pwriteptmparm = kmalloc(sizeof(struct writePTM_parm), GFP_ATOMIC);
 	if (pwriteptmparm == NULL) {
 		kfree((u8 *) ph2c);
 		return _FAIL;
@@ -369,11 +367,10 @@ u8 r8712_setfwra_cmd(struct _adapter *padapter, u8 type)
 	struct writePTM_parm *pwriteptmparm;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	pwriteptmparm = (struct writePTM_parm *)
-		_malloc(sizeof(struct setra_parm));
+	pwriteptmparm = kmalloc(sizeof(struct writePTM_parm), GFP_ATOMIC);
 	if (pwriteptmparm == NULL) {
 		kfree((u8 *) ph2c);
 		return _FAIL;
@@ -390,11 +387,10 @@ u8 r8712_setrfreg_cmd(struct _adapter  *padapter, u8 offset, u32 val)
 	struct writeRF_parm *pwriterfparm;
 	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	pwriterfparm = (struct writeRF_parm *)_malloc(
-			sizeof(struct writeRF_parm));
+	pwriterfparm = kmalloc(sizeof(struct writeRF_parm), GFP_ATOMIC);
 	if (pwriterfparm == NULL) {
 		kfree((u8 *) ph2c);
 		return _FAIL;
@@ -412,10 +408,10 @@ u8 r8712_getrfreg_cmd(struct _adapter *padapter, u8 offset, u8 *pval)
 	struct readRF_parm *prdrfparm;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	prdrfparm = (struct readRF_parm *)_malloc(sizeof(struct readRF_parm));
+	prdrfparm = kmalloc(sizeof(struct readRF_parm), GFP_ATOMIC);
 	if (prdrfparm == NULL) {
 		kfree((u8 *) ph2c);
 		return _FAIL;
@@ -456,7 +452,7 @@ u8 r8712_createbss_cmd(struct _adapter *padapter)
 				 &padapter->registrypriv.dev_network;
 
 	padapter->ledpriv.LedControlHandler(padapter, LED_CTL_START_TO_LINK);
-	pcmd = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	pcmd = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (pcmd == NULL)
 		return _FAIL;
 	_init_listhead(&pcmd->list);
@@ -490,7 +486,7 @@ u8 r8712_joinbss_cmd(struct _adapter  *padapter, struct wlan_network *pnetwork)
 						network.InfrastructureMode;
 
 	padapter->ledpriv.LedControlHandler(padapter, LED_CTL_START_TO_LINK);
-	pcmd = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	pcmd = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (pcmd == NULL)
 		return _FAIL;
 	t_len = sizeof(u32) + 6 * sizeof(unsigned char) + 2 +
@@ -628,11 +624,10 @@ u8 r8712_disassoc_cmd(struct _adapter *padapter) /* for sta_mode */
 	struct disconnect_parm *pdisconnect;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 
-	pdisconnect_cmd = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	pdisconnect_cmd = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (pdisconnect_cmd == NULL)
 		return _FAIL;
-	pdisconnect = (struct disconnect_parm *)_malloc(
-		      sizeof(struct disconnect_parm));
+	pdisconnect = kmalloc(sizeof(struct disconnect_parm), GFP_ATOMIC);
 	if (pdisconnect == NULL) {
 		kfree((u8 *)pdisconnect_cmd);
 		return _FAIL;
@@ -651,11 +646,10 @@ u8 r8712_setopmode_cmd(struct _adapter *padapter,
 
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	psetop = (struct setopmode_parm *)_malloc(
-		  sizeof(struct setopmode_parm));
+	psetop = kmalloc(sizeof(struct setopmode_parm), GFP_ATOMIC);
 	if (psetop == NULL) {
 		kfree((u8 *) ph2c);
 		return _FAIL;
@@ -676,17 +670,15 @@ u8 r8712_setstakey_cmd(struct _adapter *padapter, u8 *psta, u8 unicast_key)
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 	struct sta_info *sta = (struct sta_info *)psta;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	psetstakey_para = (struct set_stakey_parm *)_malloc(
-			  sizeof(struct set_stakey_parm));
+	psetstakey_para = kmalloc(sizeof(struct set_stakey_parm), GFP_ATOMIC);
 	if (psetstakey_para == NULL) {
 		kfree((u8 *) ph2c);
 		return _FAIL;
 	}
-	psetstakey_rsp = (struct set_stakey_rsp *)_malloc(
-			  sizeof(struct set_stakey_rsp));
+	psetstakey_rsp = kmalloc(sizeof(struct set_stakey_rsp), GFP_ATOMIC);
 	if (psetstakey_rsp == NULL) {
 		kfree((u8 *) ph2c);
 		kfree((u8 *) psetstakey_para);
@@ -718,11 +710,10 @@ u8 r8712_setrfintfs_cmd(struct _adapter *padapter, u8 mode)
 	struct setrfintfs_parm *psetrfintfsparm;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	psetrfintfsparm = (struct setrfintfs_parm *)_malloc(
-			   sizeof(struct setrfintfs_parm));
+	psetrfintfsparm = kmalloc(sizeof(struct setrfintfs_parm), GFP_ATOMIC);
 	if (psetrfintfsparm == NULL) {
 		kfree((unsigned char *) ph2c);
 		return _FAIL;
@@ -741,11 +732,10 @@ u8 r8712_setrttbl_cmd(struct _adapter *padapter,
 	struct setratable_parm *psetrttblparm;
 	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	psetrttblparm = (struct setratable_parm *)_malloc(
-			sizeof(struct setratable_parm));
+	psetrttblparm = kmalloc(sizeof(struct setratable_parm), GFP_ATOMIC);
 	if (psetrttblparm == NULL) {
 		kfree((unsigned char *)ph2c);
 		return _FAIL;
@@ -763,11 +753,10 @@ u8 r8712_gettssi_cmd(struct _adapter *padapter, u8 offset, u8 *pval)
 	struct cmd_obj *ph2c;
 	struct readTSSI_parm *prdtssiparm;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	prdtssiparm = (struct readTSSI_parm *)
-		_malloc(sizeof(struct readTSSI_parm));
+	prdtssiparm = kmalloc(sizeof(struct readTSSI_parm), GFP_ATOMIC);
 	if (prdtssiparm == NULL) {
 		kfree((unsigned char *) ph2c);
 		return _FAIL;
@@ -790,11 +779,11 @@ u8 r8712_setMacAddr_cmd(struct _adapter *padapter, u8 *mac_addr)
 	struct cmd_obj *ph2c;
 	struct SetMacAddr_param	*psetMacAddr_para;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	psetMacAddr_para = (struct SetMacAddr_param *)_malloc(
-			   sizeof(struct SetMacAddr_param));
+	psetMacAddr_para = kmalloc(sizeof(struct SetMacAddr_param),
+				   GFP_ATOMIC);
 	if (psetMacAddr_para == NULL) {
 		kfree((u8 *) ph2c);
 		return _FAIL;
@@ -813,17 +802,17 @@ u8 r8712_setassocsta_cmd(struct _adapter *padapter, u8 *mac_addr)
 	struct set_assocsta_parm	*psetassocsta_para;
 	struct set_assocsta_rsp		*psetassocsta_rsp = NULL;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	psetassocsta_para = (struct set_assocsta_parm *)
-			    _malloc(sizeof(struct set_assocsta_parm));
+	psetassocsta_para = kmalloc(sizeof(struct set_assocsta_parm),
+				    GFP_ATOMIC);
 	if (psetassocsta_para == NULL) {
 		kfree((u8 *) ph2c);
 		return _FAIL;
 	}
-	psetassocsta_rsp = (struct set_assocsta_rsp *)_malloc(
-			    sizeof(struct set_assocsta_rsp));
+	psetassocsta_rsp = kmalloc(sizeof(struct set_assocsta_rsp),
+				   GFP_ATOMIC);
 	if (psetassocsta_rsp == NULL) {
 		kfree((u8 *)ph2c);
 		kfree((u8 *)psetassocsta_para);
@@ -843,11 +832,10 @@ u8 r8712_addbareq_cmd(struct _adapter *padapter, u8 tid)
 	struct cmd_obj		*ph2c;
 	struct addBaReq_parm	*paddbareq_parm;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	paddbareq_parm = (struct addBaReq_parm *)_malloc(
-			  sizeof(struct addBaReq_parm));
+	paddbareq_parm = kmalloc(sizeof(struct addBaReq_parm), GFP_ATOMIC);
 	if (paddbareq_parm == NULL) {
 		kfree((unsigned char *)ph2c);
 		return _FAIL;
@@ -865,11 +853,10 @@ u8 r8712_wdg_wk_cmd(struct _adapter *padapter)
 	struct drvint_cmd_parm  *pdrvintcmd_param;
 	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	pdrvintcmd_param = (struct drvint_cmd_parm *)_malloc(
-			   sizeof(struct drvint_cmd_parm));
+	pdrvintcmd_param = kmalloc(sizeof(struct drvint_cmd_parm), GFP_ATOMIC);
 	if (pdrvintcmd_param == NULL) {
 		kfree((unsigned char *)ph2c);
 		return _FAIL;
@@ -1040,16 +1027,14 @@ u8 r8712_disconnectCtrlEx_cmd(struct _adapter *adapter, u32 enableDrvCtrl,
 	struct DisconnectCtrlEx_param *param;
 	struct cmd_priv *pcmdpriv = &adapter->cmdpriv;
 
-	ph2c = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	ph2c = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (ph2c == NULL)
 		return _FAIL;
-	param = (struct DisconnectCtrlEx_param *)
-		_malloc(sizeof(struct DisconnectCtrlEx_param));
+	param = kzalloc(sizeof(struct DisconnectCtrlEx_param), GFP_ATOMIC);
 	if (param == NULL) {
 		kfree((unsigned char *) ph2c);
 		return _FAIL;
 	}
-	memset(param, 0, sizeof(struct DisconnectCtrlEx_param));
 
 	param->EnableDrvCtrl = (unsigned char)enableDrvCtrl;
 	param->TryPktCnt = (unsigned char)tryPktCnt;
diff --git a/drivers/staging/rtl8712/rtl871x_io.c b/drivers/staging/rtl8712/rtl871x_io.c
index abc1c97378f..37a841a1488 100644
--- a/drivers/staging/rtl8712/rtl871x_io.c
+++ b/drivers/staging/rtl8712/rtl871x_io.c
@@ -60,8 +60,8 @@ static uint _init_intf_hdl(struct _adapter *padapter,
 	set_intf_funs = &(r8712_usb_set_intf_funs);
 	set_intf_ops = &r8712_usb_set_intf_ops;
 	init_intf_priv = &r8712_usb_init_intf_priv;
-	pintf_priv = pintf_hdl->pintfpriv = (struct intf_priv *)
-		     _malloc(sizeof(struct intf_priv));
+	pintf_priv = pintf_hdl->pintfpriv = kmalloc(sizeof(struct intf_priv),
+						    GFP_ATOMIC);
 	if (pintf_priv == NULL)
 		goto _init_intf_hdl_fail;
 	pintf_hdl->adapter = (u8 *)padapter;
@@ -112,15 +112,16 @@ uint r8712_alloc_io_queue(struct _adapter *adapter)
 	struct io_queue *pio_queue;
 	struct io_req *pio_req;
 
-	pio_queue = (struct io_queue *)_malloc(sizeof(struct io_queue));
+	pio_queue = kmalloc(sizeof(struct io_queue), GFP_ATOMIC);
 	if (pio_queue == NULL)
 		goto alloc_io_queue_fail;
 	_init_listhead(&pio_queue->free_ioreqs);
 	_init_listhead(&pio_queue->processing);
 	_init_listhead(&pio_queue->pending);
 	spin_lock_init(&pio_queue->lock);
-	pio_queue->pallocated_free_ioreqs_buf = (u8 *)_malloc(NUM_IOREQ *
-						(sizeof(struct io_req)) + 4);
+	pio_queue->pallocated_free_ioreqs_buf = kmalloc(NUM_IOREQ *
+							(sizeof(struct io_req)) + 4,
+							GFP_ATOMIC);
 	if ((pio_queue->pallocated_free_ioreqs_buf) == NULL)
 		goto alloc_io_queue_fail;
 	memset(pio_queue->pallocated_free_ioreqs_buf, 0,
diff --git a/drivers/staging/rtl8712/rtl871x_ioctl_linux.c b/drivers/staging/rtl8712/rtl871x_ioctl_linux.c
index 23d539dba7e..e147c4bfe12 100644
--- a/drivers/staging/rtl8712/rtl871x_ioctl_linux.c
+++ b/drivers/staging/rtl8712/rtl871x_ioctl_linux.c
@@ -424,10 +424,9 @@ static int wpa_set_encryption(struct net_device *dev, struct ieee_param *param,
 			wep_key_idx = 0;
 		if (wep_key_len > 0) {
 			wep_key_len = wep_key_len <= 5 ? 5 : 13;
-			pwep = (struct NDIS_802_11_WEP *)_malloc((u32)
-			       (wep_key_len +
-			       FIELD_OFFSET(struct NDIS_802_11_WEP,
-			       KeyMaterial)));
+			pwep = kmalloc((u32)(wep_key_len +
+				       FIELD_OFFSET(struct NDIS_802_11_WEP, KeyMaterial)),
+				       GFP_ATOMIC);
 			if (pwep == NULL)
 				return -ENOMEM;
 			memset(pwep, 0, sizeof(struct NDIS_802_11_WEP));
@@ -518,10 +517,9 @@ static int r871x_set_wpa_ie(struct _adapter *padapter, char *pie,
 	if ((ielen > MAX_WPA_IE_LEN) || (pie == NULL))
 		return -EINVAL;
 	if (ielen) {
-		buf = _malloc(ielen);
+		buf = kmemdup(pie, ielen, GFP_ATOMIC);
 		if (buf == NULL)
 			return -ENOMEM;
-		memcpy(buf, pie , ielen);
 		pos = buf;
 		if (ielen < RSN_HEADER_LEN) {
 			ret  = -EINVAL;
@@ -959,13 +957,9 @@ static int r871x_wx_set_priv(struct net_device *dev,
 	struct iw_point *dwrq = (struct iw_point *)awrq;
 
 	len = dwrq->length;
-	ext = _malloc(len);
-	if (!ext)
-		return -ENOMEM;
-	if (copy_from_user(ext, dwrq->pointer, len)) {
-		kfree(ext);
-		return -EFAULT;
-	}
+	ext = memdup_user(dwrq->pointer, len);
+	if (IS_ERR(ext))
+		return PTR_ERR(ext);
 
 	if (0 == strcasecmp(ext, "RSSI")) {
 		/*Return received signal strength indicator in -db for */
@@ -1801,13 +1795,6 @@ static int r871x_wx_set_enc_ext(struct net_device *dev,
 	u32 param_len;
 	int ret = 0;
 
-	param_len = sizeof(struct ieee_param) + pext->key_len;
-	param = (struct ieee_param *)_malloc(param_len);
-	if (param == NULL)
-		return -ENOMEM;
-	memset(param, 0, param_len);
-	param->cmd = IEEE_CMD_SET_ENCRYPTION;
-	memset(param->sta_addr, 0xff, ETH_ALEN);
 	switch (pext->alg) {
 	case IW_ENCODE_ALG_NONE:
 		alg_name = "none";
@@ -1824,6 +1811,14 @@ static int r871x_wx_set_enc_ext(struct net_device *dev,
 	default:
 		return -EINVAL;
 	}
+
+	param_len = sizeof(struct ieee_param) + pext->key_len;
+	param = kzalloc(param_len, GFP_ATOMIC);
+	if (param == NULL)
+		return -ENOMEM;
+	param->cmd = IEEE_CMD_SET_ENCRYPTION;
+	memset(param->sta_addr, 0xff, ETH_ALEN);
+
 	strncpy((char *)param->u.crypt.alg, alg_name, IEEE_CRYPT_ALG_NAME_LEN);
 	if (pext->ext_flags & IW_ENCODE_EXT_GROUP_KEY)
 		param->u.crypt.set_tx = 0;
@@ -1920,7 +1915,7 @@ static int r871x_mp_ioctl_hdl(struct net_device *dev,
 	bset = (u8)(p->flags & 0xFFFF);
 	len = p->length;
 	pparmbuf = NULL;
-	pparmbuf = (u8 *)_malloc(len);
+	pparmbuf = kmalloc(len, GFP_ATOMIC);
 	if (pparmbuf == NULL) {
 		ret = -ENOMEM;
 		goto _r871x_mp_ioctl_hdl_exit;
@@ -2193,13 +2188,9 @@ static int wpa_supplicant_ioctl(struct net_device *dev, struct iw_point *p)
 
 	if (p->length < sizeof(struct ieee_param) || !p->pointer)
 		return -EINVAL;
-	param = (struct ieee_param *)_malloc(p->length);
-	if (param == NULL)
-		return -ENOMEM;
-	if (copy_from_user(param, p->pointer, p->length)) {
-		kfree((u8 *)param);
-		return -EFAULT;
-	}
+	param = memdup_user(p->pointer, p->length);
+	if (IS_ERR(param))
+		return PTR_ERR(param);
 	switch (param->cmd) {
 	case IEEE_CMD_SET_WPA_PARAM:
 		ret = wpa_set_param(dev, param->u.wpa_param.name,
diff --git a/drivers/staging/rtl8712/rtl871x_mlme.c b/drivers/staging/rtl8712/rtl871x_mlme.c
index 3ea99aea438..02339e10001 100644
--- a/drivers/staging/rtl8712/rtl871x_mlme.c
+++ b/drivers/staging/rtl8712/rtl871x_mlme.c
@@ -62,7 +62,8 @@ static sint _init_mlme_priv(struct _adapter *padapter)
 	_init_queue(&(pmlmepriv->scanned_queue));
 	set_scanned_network_val(pmlmepriv, 0);
 	memset(&pmlmepriv->assoc_ssid, 0, sizeof(struct ndis_802_11_ssid));
-	pbuf = _malloc(MAX_BSS_CNT * (sizeof(struct wlan_network)));
+	pbuf = kmalloc(MAX_BSS_CNT * (sizeof(struct wlan_network)),
+		       GFP_ATOMIC);
 	if (pbuf == NULL)
 		return _FAIL;
 	pmlmepriv->free_bss_buf = pbuf;
@@ -725,8 +726,7 @@ void r8712_joinbss_event_callback(struct _adapter *adapter, u8 *pbuf)
 	struct wlan_network *pnetwork;
 
 	if (sizeof(struct list_head) == 4 * sizeof(u32)) {
-		pnetwork = (struct wlan_network *)
-			_malloc(sizeof(struct wlan_network));
+		pnetwork = kmalloc(sizeof(struct wlan_network), GFP_ATOMIC);
 		memcpy((u8 *)pnetwork+16, (u8 *)pbuf + 8,
 			sizeof(struct wlan_network) - 16);
 	} else
@@ -1211,19 +1211,16 @@ sint r8712_set_auth(struct _adapter *adapter,
 	struct cmd_priv	*pcmdpriv = &adapter->cmdpriv;
 	struct cmd_obj *pcmd;
 	struct setauth_parm *psetauthparm;
-	sint ret = _SUCCESS;
 
-	pcmd = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	pcmd = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (pcmd == NULL)
 		return _FAIL;
 
-	psetauthparm = (struct setauth_parm *)_malloc(
-			sizeof(struct setauth_parm));
+	psetauthparm = kzalloc(sizeof(struct setauth_parm), GFP_ATOMIC);
 	if (psetauthparm == NULL) {
 		kfree((unsigned char *)pcmd);
 		return _FAIL;
 	}
-	memset(psetauthparm, 0, sizeof(struct setauth_parm));
 	psetauthparm->mode = (u8)psecuritypriv->AuthAlgrthm;
 	pcmd->cmdcode = _SetAuth_CMD_;
 	pcmd->parmbuf = (unsigned char *)psetauthparm;
@@ -1232,7 +1229,7 @@ sint r8712_set_auth(struct _adapter *adapter,
 	pcmd->rspsz = 0;
 	_init_listhead(&pcmd->list);
 	r8712_enqueue_cmd(pcmdpriv, pcmd);
-	return ret;
+	return _SUCCESS;
 }
 
 sint r8712_set_key(struct _adapter *adapter,
@@ -1243,16 +1240,16 @@ sint r8712_set_key(struct _adapter *adapter,
 	struct cmd_obj *pcmd;
 	struct setkey_parm *psetkeyparm;
 	u8 keylen;
+	sint ret = _SUCCESS;
 
-	pcmd = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	pcmd = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (pcmd == NULL)
 		return _FAIL;
-	psetkeyparm = (struct setkey_parm *)_malloc(sizeof(struct setkey_parm));
+	psetkeyparm = kzalloc(sizeof(struct setkey_parm), GFP_ATOMIC);
 	if (psetkeyparm == NULL) {
-		kfree((unsigned char *)pcmd);
-		return _FAIL;
+		ret = _FAIL;
+		goto err_free_cmd;
 	}
-	memset(psetkeyparm, 0, sizeof(struct setkey_parm));
 	if (psecuritypriv->AuthAlgrthm == 2) { /* 802.1X */
 		psetkeyparm->algorithm =
 			 (u8)psecuritypriv->XGrpPrivacy;
@@ -1274,23 +1271,28 @@ sint r8712_set_key(struct _adapter *adapter,
 			psecuritypriv->DefKey[keyid].skey, keylen);
 		break;
 	case _TKIP_:
-		if (keyid < 1 || keyid > 2)
-			return _FAIL;
+		if (keyid < 1 || keyid > 2) {
+			ret = _FAIL;
+			goto err_free_parm;
+		}
 		keylen = 16;
 		memcpy(psetkeyparm->key,
 			&psecuritypriv->XGrpKey[keyid - 1], keylen);
 		psetkeyparm->grpkey = 1;
 		break;
 	case _AES_:
-		if (keyid < 1 || keyid > 2)
-			return _FAIL;
+		if (keyid < 1 || keyid > 2) {
+			ret = _FAIL;
+			goto err_free_parm;
+		}
 		keylen = 16;
 		memcpy(psetkeyparm->key,
 			&psecuritypriv->XGrpKey[keyid - 1], keylen);
 		psetkeyparm->grpkey = 1;
 		break;
 	default:
-		return _FAIL;
+		ret = _FAIL;
+		goto err_free_parm;
 	}
 	pcmd->cmdcode = _SetKey_CMD_;
 	pcmd->parmbuf = (u8 *)psetkeyparm;
@@ -1299,7 +1301,13 @@ sint r8712_set_key(struct _adapter *adapter,
 	pcmd->rspsz = 0;
 	_init_listhead(&pcmd->list);
 	r8712_enqueue_cmd(pcmdpriv, pcmd);
-	return _SUCCESS;
+	return ret;
+
+err_free_parm:
+	kfree(psetkeyparm);
+err_free_cmd:
+	kfree(pcmd);
+	return ret;
 }
 
 /* adjust IEs for r8712_joinbss_cmd in WMM */
diff --git a/drivers/staging/rtl8712/rtl871x_mp.c b/drivers/staging/rtl8712/rtl871x_mp.c
index 0563318a19f..389062fe8ea 100644
--- a/drivers/staging/rtl8712/rtl871x_mp.c
+++ b/drivers/staging/rtl8712/rtl871x_mp.c
@@ -53,8 +53,9 @@ static int init_mp_priv(struct mp_priv *pmp_priv)
 	_init_mp_priv_(pmp_priv);
 	_init_queue(&pmp_priv->free_mp_xmitqueue);
 	pmp_priv->pallocated_mp_xmitframe_buf = NULL;
-	pmp_priv->pallocated_mp_xmitframe_buf = _malloc(NR_MP_XMITFRAME *
-					 sizeof(struct mp_xmit_frame) + 4);
+	pmp_priv->pallocated_mp_xmitframe_buf = kmalloc(NR_MP_XMITFRAME *
+							sizeof(struct mp_xmit_frame) + 4,
+							GFP_ATOMIC);
 	if (pmp_priv->pallocated_mp_xmitframe_buf == NULL) {
 		res = _FAIL;
 		goto _exit_init_mp_priv;
@@ -80,9 +81,8 @@ _exit_init_mp_priv:
 
 static int free_mp_priv(struct mp_priv *pmp_priv)
 {
-	int res = 0;
 	kfree(pmp_priv->pallocated_mp_xmitframe_buf);
-	return res;
+	return 0;
 }
 
 void mp871xinit(struct _adapter *padapter)
@@ -281,11 +281,10 @@ void r8712_SetChannel(struct _adapter *pAdapter)
 	struct SetChannel_parm *pparm = NULL;
 	u16 code = GEN_CMD_CODE(_SetChannel);
 
-	pcmd = (struct cmd_obj *)_malloc(sizeof(struct cmd_obj));
+	pcmd = kmalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (pcmd == NULL)
 		return;
-	pparm = (struct SetChannel_parm *)_malloc(sizeof(struct
-					 SetChannel_parm));
+	pparm = kmalloc(sizeof(struct SetChannel_parm), GFP_ATOMIC);
 	if (pparm == NULL) {
 		kfree(pcmd);
 		return;
diff --git a/drivers/staging/rtl8712/rtl871x_mp_ioctl.c b/drivers/staging/rtl8712/rtl871x_mp_ioctl.c
index 5bd42966fd5..9827ff8143b 100644
--- a/drivers/staging/rtl8712/rtl871x_mp_ioctl.c
+++ b/drivers/staging/rtl8712/rtl871x_mp_ioctl.c
@@ -66,7 +66,6 @@ uint oid_rt_wireless_mode_hdl(struct oid_par_priv *poid_par_priv)
 
 uint oid_rt_pro_write_bb_reg_hdl(struct oid_par_priv *poid_par_priv)
 {
-	uint status = RNDIS_STATUS_SUCCESS;
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
 	struct bb_reg_param *pbbreg;
@@ -83,12 +82,11 @@ uint oid_rt_pro_write_bb_reg_hdl(struct oid_par_priv *poid_par_priv)
 		offset |= BB_REG_BASE_ADDR;
 	value = pbbreg->value;
 	r8712_bb_reg_write(Adapter, offset, value);
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 uint oid_rt_pro_read_bb_reg_hdl(struct oid_par_priv *poid_par_priv)
 {
-	uint status = RNDIS_STATUS_SUCCESS;
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
 	struct bb_reg_param *pbbreg;
@@ -106,12 +104,11 @@ uint oid_rt_pro_read_bb_reg_hdl(struct oid_par_priv *poid_par_priv)
 	value = r8712_bb_reg_read(Adapter, offset);
 	pbbreg->value = value;
 	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 uint oid_rt_pro_write_rf_reg_hdl(struct oid_par_priv *poid_par_priv)
 {
-	uint status = RNDIS_STATUS_SUCCESS;
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
 	struct rf_reg_param *pbbreg;
@@ -130,14 +127,13 @@ uint oid_rt_pro_write_rf_reg_hdl(struct oid_par_priv *poid_par_priv)
 	offset = (u8)pbbreg->offset;
 	value = pbbreg->value;
 	r8712_rf_reg_write(Adapter, path, offset, value);
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 uint oid_rt_pro_read_rf_reg_hdl(struct oid_par_priv *poid_par_priv)
 {
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
-	uint status = RNDIS_STATUS_SUCCESS;
 	struct rf_reg_param *pbbreg;
 	u8 path;
 	u8 offset;
@@ -155,7 +151,7 @@ uint oid_rt_pro_read_rf_reg_hdl(struct oid_par_priv *poid_par_priv)
 	value = r8712_rf_reg_read(Adapter, path, offset);
 	pbbreg->value = value;
 	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 /*This function initializes the DUT to the MP test mode*/
@@ -266,7 +262,6 @@ uint oid_rt_pro_set_data_rate_hdl(struct oid_par_priv
 {
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
-	uint status = RNDIS_STATUS_SUCCESS;
 	u32 ratevalue;
 
 	if (poid_par_priv->type_of_oid != SET_OID)
@@ -278,7 +273,7 @@ uint oid_rt_pro_set_data_rate_hdl(struct oid_par_priv
 		return RNDIS_STATUS_INVALID_DATA;
 	Adapter->mppriv.curr_rateidx = ratevalue;
 	r8712_SetDataRate(Adapter);
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 uint oid_rt_pro_start_test_hdl(struct oid_par_priv *poid_par_priv)
@@ -328,7 +323,6 @@ uint oid_rt_pro_set_channel_direct_call_hdl(struct oid_par_priv
 {
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
-	uint status = RNDIS_STATUS_SUCCESS;
 	u32		Channel;
 
 	if (poid_par_priv->type_of_oid != SET_OID)
@@ -340,14 +334,13 @@ uint oid_rt_pro_set_channel_direct_call_hdl(struct oid_par_priv
 		return RNDIS_STATUS_NOT_ACCEPTED;
 	Adapter->mppriv.curr_ch = Channel;
 	r8712_SetChannel(Adapter);
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 uint oid_rt_pro_set_antenna_bb_hdl(struct oid_par_priv *poid_par_priv)
 {
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
-	uint status = RNDIS_STATUS_SUCCESS;
 	u32 antenna;
 
 	if (poid_par_priv->type_of_oid != SET_OID)
@@ -358,7 +351,7 @@ uint oid_rt_pro_set_antenna_bb_hdl(struct oid_par_priv *poid_par_priv)
 	Adapter->mppriv.antenna_tx = (u16)((antenna & 0xFFFF0000) >> 16);
 	Adapter->mppriv.antenna_rx = (u16)(antenna & 0x0000FFFF);
 	r8712_SwitchAntenna(Adapter);
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 uint oid_rt_pro_set_tx_power_control_hdl(
@@ -366,7 +359,6 @@ uint oid_rt_pro_set_tx_power_control_hdl(
 {
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
-	uint status = RNDIS_STATUS_SUCCESS;
 	u32 tx_pwr_idx;
 
 	if (poid_par_priv->type_of_oid != SET_OID)
@@ -378,7 +370,7 @@ uint oid_rt_pro_set_tx_power_control_hdl(
 		return RNDIS_STATUS_NOT_ACCEPTED;
 	Adapter->mppriv.curr_txpoweridx = (u8)tx_pwr_idx;
 	r8712_SetTxPower(Adapter);
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 uint oid_rt_pro_query_tx_packet_sent_hdl(
@@ -839,7 +831,6 @@ uint oid_rt_poll_rx_status_hdl(struct oid_par_priv *poid_par_priv)
 {
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
-	uint status = RNDIS_STATUS_SUCCESS;
 
 	if (poid_par_priv->type_of_oid != QUERY_OID)
 		return RNDIS_STATUS_NOT_ACCEPTED;
@@ -847,7 +838,7 @@ uint oid_rt_poll_rx_status_hdl(struct oid_par_priv *poid_par_priv)
 		(unsigned char *)&Adapter->mppriv.rxstat,
 		sizeof(struct recv_stat));
 	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 uint oid_rt_pro_cfg_debug_message_hdl(struct oid_par_priv
@@ -875,7 +866,6 @@ uint oid_rt_get_thermal_meter_hdl(struct oid_par_priv *poid_par_priv)
 {
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
-	uint status = RNDIS_STATUS_SUCCESS;
 
 	if (poid_par_priv->type_of_oid != QUERY_OID)
 		return RNDIS_STATUS_NOT_ACCEPTED;
@@ -897,7 +887,7 @@ uint oid_rt_get_thermal_meter_hdl(struct oid_par_priv *poid_par_priv)
 	*(u32 *)poid_par_priv->information_buf =
 				 Adapter->mppriv.workparam.io_value;
 	*poid_par_priv->bytes_rw = sizeof(u32);
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 uint oid_rt_pro_set_power_tracking_hdl(struct oid_par_priv
@@ -1091,7 +1081,6 @@ uint oid_rt_pro_dele_sta_info_hdl(struct oid_par_priv *poid_par_priv)
 				   (poid_par_priv->adapter_context);
 
 	unsigned long			irqL;
-	uint status = RNDIS_STATUS_SUCCESS;
 
 	struct sta_info		*psta = NULL;
 	u8			*macaddr;
@@ -1113,7 +1102,7 @@ uint oid_rt_pro_dele_sta_info_hdl(struct oid_par_priv *poid_par_priv)
 		spin_unlock_irqrestore(&(Adapter->stapriv.sta_hash_lock), irqL);
 	}
 
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 /*--------------------------------------------------------------------------*/
 static u32 mp_query_drv_var(struct _adapter *padapter, u8 offset, u32 var)
@@ -1126,8 +1115,6 @@ uint oid_rt_pro_query_dr_variable_hdl(struct oid_par_priv *poid_par_priv)
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
 
-	uint status = RNDIS_STATUS_SUCCESS;
-
 	struct DR_VARIABLE_STRUCT *pdrv_var;
 
 	if (poid_par_priv->type_of_oid != QUERY_OID)
@@ -1139,7 +1126,7 @@ uint oid_rt_pro_query_dr_variable_hdl(struct oid_par_priv *poid_par_priv)
 	pdrv_var->variable = mp_query_drv_var(Adapter, pdrv_var->offset,
 					      pdrv_var->variable);
 	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 /*--------------------------------------------------------------------------*/
@@ -1243,7 +1230,6 @@ uint oid_rt_get_efuse_current_size_hdl(struct oid_par_priv
 {
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
-	uint status = RNDIS_STATUS_SUCCESS;
 
 	if (poid_par_priv->type_of_oid != QUERY_OID)
 		return RNDIS_STATUS_NOT_ACCEPTED;
@@ -1254,14 +1240,13 @@ uint oid_rt_get_efuse_current_size_hdl(struct oid_par_priv
 				 r8712_efuse_get_current_size(Adapter);
 	r8712_efuse_reg_uninit(Adapter);
 	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 uint oid_rt_get_efuse_max_size_hdl(struct oid_par_priv *poid_par_priv)
 {
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
-	uint status = RNDIS_STATUS_SUCCESS;
 
 	if (poid_par_priv->type_of_oid != QUERY_OID)
 		return RNDIS_STATUS_NOT_ACCEPTED;
@@ -1270,7 +1255,7 @@ uint oid_rt_get_efuse_max_size_hdl(struct oid_par_priv *poid_par_priv)
 	*(int *)poid_par_priv->information_buf =
 					 r8712_efuse_get_max_size(Adapter);
 	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 uint oid_rt_pro_efuse_hdl(struct oid_par_priv *poid_par_priv)
@@ -1320,7 +1305,6 @@ uint oid_rt_set_bandwidth_hdl(struct oid_par_priv *poid_par_priv)
 {
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
-	uint status = RNDIS_STATUS_SUCCESS;
 	u32		bandwidth;
 
 	if (poid_par_priv->type_of_oid != SET_OID)
@@ -1332,14 +1316,13 @@ uint oid_rt_set_bandwidth_hdl(struct oid_par_priv *poid_par_priv)
 		bandwidth = HT_CHANNEL_WIDTH_40;
 	Adapter->mppriv.curr_bandwidth = (u8)bandwidth;
 	r8712_SwitchBandwidth(Adapter);
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 uint oid_rt_set_crystal_cap_hdl(struct oid_par_priv *poid_par_priv)
 {
 	struct _adapter *Adapter = (struct _adapter *)
 				   (poid_par_priv->adapter_context);
-	uint status = RNDIS_STATUS_SUCCESS;
 	u32		crystal_cap = 0;
 
 	if (poid_par_priv->type_of_oid != SET_OID)
@@ -1351,7 +1334,7 @@ uint oid_rt_set_crystal_cap_hdl(struct oid_par_priv *poid_par_priv)
 		return RNDIS_STATUS_NOT_ACCEPTED;
 	Adapter->mppriv.curr_crystalcap = crystal_cap;
 	r8712_SetCrystalCap(Adapter);
-	return status;
+	return RNDIS_STATUS_SUCCESS;
 }
 
 uint oid_rt_set_rx_packet_type_hdl(struct oid_par_priv
diff --git a/drivers/staging/rtl8712/rtl871x_recv.c b/drivers/staging/rtl8712/rtl871x_recv.c
index 23ec684b60e..eb775872c93 100644
--- a/drivers/staging/rtl8712/rtl871x_recv.c
+++ b/drivers/staging/rtl8712/rtl871x_recv.c
@@ -72,9 +72,9 @@ sint _r8712_init_recv_priv(struct recv_priv *precvpriv,
 	_init_queue(&precvpriv->recv_pending_queue);
 	precvpriv->adapter = padapter;
 	precvpriv->free_recvframe_cnt = NR_RECVFRAME;
-	precvpriv->pallocated_frame_buf = _malloc(NR_RECVFRAME *
-					   sizeof(union recv_frame) +
-					   RXFRAME_ALIGN_SZ);
+	precvpriv->pallocated_frame_buf = kmalloc(NR_RECVFRAME *
+						  sizeof(union recv_frame) + RXFRAME_ALIGN_SZ,
+						  GFP_ATOMIC);
 	if (precvpriv->pallocated_frame_buf == NULL)
 		return _FAIL;
 	kmemleak_not_leak(precvpriv->pallocated_frame_buf);
@@ -254,7 +254,7 @@ union recv_frame *r8712_portctrl(struct _adapter *adapter,
 	struct sta_info *psta;
 	struct	sta_priv *pstapriv;
 	union recv_frame *prtnframe;
-	u16 ether_type = 0;
+	u16 ether_type;
 
 	pstapriv = &adapter->stapriv;
 	ptr = get_recvframe_data(precv_frame);
@@ -263,15 +263,14 @@ union recv_frame *r8712_portctrl(struct _adapter *adapter,
 	psta = r8712_get_stainfo(pstapriv, psta_addr);
 	auth_alg = adapter->securitypriv.AuthAlgrthm;
 	if (auth_alg == 2) {
+		/* get ether_type */
+		ptr = ptr + pfhdr->attrib.hdrlen + LLC_HEADER_SIZE;
+		memcpy(&ether_type, ptr, 2);
+		ether_type = ntohs((unsigned short)ether_type);
+
 		if ((psta != NULL) && (psta->ieee8021x_blocked)) {
 			/* blocked
 			 * only accept EAPOL frame */
-			prtnframe = precv_frame;
-			/*get ether_type */
-			ptr = ptr + pfhdr->attrib.hdrlen +
-			      pfhdr->attrib.iv_len + LLC_HEADER_SIZE;
-			memcpy(&ether_type, ptr, 2);
-			ether_type = ntohs((unsigned short)ether_type);
 			if (ether_type == 0x888e)
 				prtnframe = precv_frame;
 			else {
@@ -606,8 +605,6 @@ sint r8712_wlanhdr_to_ethhdr(union recv_frame *precvframe)
 	u8	bsnaphdr;
 	u8	*psnap_type;
 	struct ieee80211_snap_hdr *psnap;
-
-	sint ret = _SUCCESS;
 	struct _adapter	*adapter = precvframe->u.hdr.adapter;
 	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
 
@@ -654,7 +651,7 @@ sint r8712_wlanhdr_to_ethhdr(union recv_frame *precvframe)
 		len = htons(len);
 		memcpy(ptr + 12, &len, 2);
 	}
-	return ret;
+	return _SUCCESS;
 }
 
 s32 r8712_recv_entry(union recv_frame *precvframe)
diff --git a/drivers/staging/rtl8712/rtl871x_security.c b/drivers/staging/rtl8712/rtl871x_security.c
index 5ffc489e950..0912f52def9 100644
--- a/drivers/staging/rtl8712/rtl871x_security.c
+++ b/drivers/staging/rtl8712/rtl871x_security.c
@@ -272,6 +272,7 @@ static void secmicputuint32(u8 *p, u32 val)
 /* Convert from Us4Byte32 to Byte[] in a portable way */
 {
 	long i;
+
 	for (i = 0; i < 4; i++) {
 		*p++ = (u8) (val & 0xff);
 		val >>= 8;
@@ -765,6 +766,7 @@ static void xor_128(u8 *a, u8 *b, u8 *out)
 static void xor_32(u8 *a, u8 *b, u8 *out)
 {
 	sint i;
+
 	for (i = 0; i < 4; i++)
 		out[i] = a[i] ^ b[i];
 }
@@ -798,6 +800,7 @@ static void next_key(u8 *key, sint round)
 static void byte_sub(u8 *in, u8 *out)
 {
 	sint i;
+
 	for (i = 0; i < 16; i++)
 		out[i] = sbox(in[i]);
 }
@@ -1164,7 +1167,7 @@ u32 r8712_aes_encrypt(struct _adapter *padapter, u8 *pxmitframe)
 		return _FAIL;
 	pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + TXDESC_OFFSET;
 	/* 4 start to encrypt each fragment */
-	if ((pattrib->encrypt == _AES_)) {
+	if (pattrib->encrypt == _AES_) {
 		if (pattrib->psta)
 			stainfo = pattrib->psta;
 		else
@@ -1175,7 +1178,7 @@ u32 r8712_aes_encrypt(struct _adapter *padapter, u8 *pxmitframe)
 			prwskeylen = 16;
 			for (curfragnum = 0; curfragnum < pattrib->nr_frags;
 			     curfragnum++) {
-				if ((curfragnum + 1) == pattrib->nr_frags) {\
+				if ((curfragnum + 1) == pattrib->nr_frags) {
 					length = pattrib->last_txcmdsz -
 						 pattrib->hdrlen -
 						 pattrib->iv_len -
@@ -1371,7 +1374,7 @@ u32 r8712_aes_decrypt(struct _adapter *padapter, u8 *precvframe)
 	pframe = (unsigned char *)((union recv_frame *)precvframe)->
 		 u.hdr.rx_data;
 	/* 4 start to encrypt each fragment */
-	if ((prxattrib->encrypt == _AES_)) {
+	if (prxattrib->encrypt == _AES_) {
 		stainfo = r8712_get_stainfo(&padapter->stapriv,
 					    &prxattrib->ta[0]);
 		if (stainfo != NULL) {
diff --git a/drivers/staging/rtl8712/rtl871x_sta_mgt.c b/drivers/staging/rtl8712/rtl871x_sta_mgt.c
index 8db6849d4b2..6c649842abd 100644
--- a/drivers/staging/rtl8712/rtl871x_sta_mgt.c
+++ b/drivers/staging/rtl8712/rtl871x_sta_mgt.c
@@ -51,8 +51,8 @@ u32 _r8712_init_sta_priv(struct	sta_priv *pstapriv)
 	struct sta_info *psta;
 	s32 i;
 
-	pstapriv->pallocated_stainfo_buf = _malloc(sizeof(struct sta_info) *
-						   NUM_STA + 4);
+	pstapriv->pallocated_stainfo_buf = kmalloc(sizeof(struct sta_info) *
+						   NUM_STA + 4, GFP_ATOMIC);
 	if (pstapriv->pallocated_stainfo_buf == NULL)
 		return _FAIL;
 	pstapriv->pstainfo_buf = pstapriv->pallocated_stainfo_buf + 4 -
diff --git a/drivers/staging/rtl8712/rtl871x_xmit.c b/drivers/staging/rtl8712/rtl871x_xmit.c
index 78f570b571a..230681a8042 100644
--- a/drivers/staging/rtl8712/rtl871x_xmit.c
+++ b/drivers/staging/rtl8712/rtl871x_xmit.c
@@ -87,8 +87,8 @@ sint _r8712_init_xmit_priv(struct xmit_priv *pxmitpriv,
 	and initialize free_xmit_frame below.
 	Please also apply  free_txobj to link_up all the xmit_frames...
 	*/
-	pxmitpriv->pallocated_frame_buf = _malloc(NR_XMITFRAME *
-					  sizeof(struct xmit_frame) + 4);
+	pxmitpriv->pallocated_frame_buf = kmalloc(NR_XMITFRAME * sizeof(struct xmit_frame) + 4,
+						  GFP_ATOMIC);
 	if (pxmitpriv->pallocated_frame_buf == NULL) {
 		pxmitpriv->pxmit_frame_buf = NULL;
 		return _FAIL;
@@ -126,8 +126,8 @@ sint _r8712_init_xmit_priv(struct xmit_priv *pxmitpriv,
 	/*init xmit_buf*/
 	_init_queue(&pxmitpriv->free_xmitbuf_queue);
 	_init_queue(&pxmitpriv->pending_xmitbuf_queue);
-	pxmitpriv->pallocated_xmitbuf = _malloc(NR_XMITBUFF *
-					sizeof(struct xmit_buf) + 4);
+	pxmitpriv->pallocated_xmitbuf = kmalloc(NR_XMITBUFF * sizeof(struct xmit_buf) + 4,
+						GFP_ATOMIC);
 	if (pxmitpriv->pallocated_xmitbuf  == NULL)
 		return _FAIL;
 	pxmitpriv->pxmitbuf = pxmitpriv->pallocated_xmitbuf + 4 -
@@ -135,8 +135,8 @@ sint _r8712_init_xmit_priv(struct xmit_priv *pxmitpriv,
 	pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmitbuf;
 	for (i = 0; i < NR_XMITBUFF; i++) {
 		_init_listhead(&pxmitbuf->list);
-		pxmitbuf->pallocated_buf = _malloc(MAX_XMITBUF_SZ +
-					   XMITBUF_ALIGN_SZ);
+		pxmitbuf->pallocated_buf = kmalloc(MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ,
+						   GFP_ATOMIC);
 		if (pxmitbuf->pallocated_buf == NULL)
 			return _FAIL;
 		pxmitbuf->pbuf = pxmitbuf->pallocated_buf + XMITBUF_ALIGN_SZ -
@@ -955,8 +955,8 @@ static void alloc_hwxmits(struct _adapter *padapter)
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 
 	pxmitpriv->hwxmit_entry = HWXMIT_ENTRY;
-	pxmitpriv->hwxmits = (struct hw_xmit *)_malloc(sizeof(struct hw_xmit) *
-			     pxmitpriv->hwxmit_entry);
+	pxmitpriv->hwxmits = kmalloc(sizeof(struct hw_xmit) * pxmitpriv->hwxmit_entry,
+				     GFP_ATOMIC);
 	if (pxmitpriv->hwxmits == NULL)
 		return;
 	hwxmits = pxmitpriv->hwxmits;
diff --git a/drivers/staging/rtl8712/usb_intf.c b/drivers/staging/rtl8712/usb_intf.c
index bbd5888e316..ba743542bab 100644
--- a/drivers/staging/rtl8712/usb_intf.c
+++ b/drivers/staging/rtl8712/usb_intf.c
@@ -595,7 +595,7 @@ static int r871xu_drv_init(struct usb_interface *pusb_intf,
 error:
 	usb_put_dev(udev);
 	usb_set_intfdata(pusb_intf, NULL);
-	if (padapter->dvobj_deinit != NULL)
+	if (padapter && padapter->dvobj_deinit != NULL)
 		padapter->dvobj_deinit(padapter);
 	if (pnetdev)
 		free_netdev(pnetdev);
diff --git a/drivers/staging/rtl8712/usb_ops_linux.c b/drivers/staging/rtl8712/usb_ops_linux.c
index dca398a0656..a6c2aab0a94 100644
--- a/drivers/staging/rtl8712/usb_ops_linux.c
+++ b/drivers/staging/rtl8712/usb_ops_linux.c
@@ -495,11 +495,11 @@ int r8712_usbctrl_vendorreq(struct intf_priv *pintfpriv, u8 request, u16 value,
 	 */
 	u8 *palloc_buf, *pIo_buf;
 
-	palloc_buf = _malloc((u32) len + 16);
+	palloc_buf = kmalloc((u32)len + 16, GFP_ATOMIC);
 	if (palloc_buf == NULL) {
 		dev_err(&udev->dev, "%s: Can't alloc memory for vendor request\n",
 			__func__);
-		return -1;
+		return -ENOMEM;
 	}
 	pIo_buf = palloc_buf + 16 - ((addr_t)(palloc_buf) & 0x0f);
 	if (requesttype == 0x01) {
diff --git a/drivers/staging/rtl8723au/Kconfig b/drivers/staging/rtl8723au/Kconfig
index 07fb5e4e50f..435f3594dab 100644
--- a/drivers/staging/rtl8723au/Kconfig
+++ b/drivers/staging/rtl8723au/Kconfig
@@ -19,14 +19,6 @@ config 8723AU_AP_MODE
 	will never be used as an AP, or the target system has limited memory,
 	"Y" should be selected.
 
-config 8723AU_P2P
-	bool "Realtek RTL8723AU Peer-to-peer mode"
-	default y
-	---help---
-	This option enables peer-to-peer mode for the r8723au driver. Unless you
-	know that peer-to-peer (P2P) mode will never be used, or the target system has
-	limited memory, "Y" should be selected.
-
 config 8723AU_BT_COEXIST
 	bool "Realtek RTL8723AU BlueTooth Coexistence"
 	default y
diff --git a/drivers/staging/rtl8723au/Makefile b/drivers/staging/rtl8723au/Makefile
index 11c6dd48646..a6316af94b7 100644
--- a/drivers/staging/rtl8723au/Makefile
+++ b/drivers/staging/rtl8723au/Makefile
@@ -1,14 +1,11 @@
 r8723au-y :=				\
-		core/rtw_ap.o		\
 		core/rtw_cmd.o		\
 		core/rtw_efuse.o	\
-		core/rtw_io.o		\
 		core/rtw_ioctl_set.o	\
 		core/rtw_ieee80211.o	\
 		core/rtw_led.o		\
 		core/rtw_mlme.o		\
 		core/rtw_mlme_ext.o	\
-		core/rtw_p2p.o		\
 		core/rtw_pwrctrl.o	\
 		core/rtw_recv.o		\
 		core/rtw_security.o	\
@@ -26,7 +23,6 @@ r8723au-y :=				\
 		hal/HalHWImg8723A_RF.o	\
 		hal/HalPwrSeqCmd.o	\
 		hal/odm_RegConfig8723A.o \
-		hal/rtl8723a_bt-coexist.o \
 		hal/odm_debug.o		\
 		hal/odm_interface.o	\
 		hal/odm_HWConfig.o	\
@@ -46,13 +42,15 @@ r8723au-y :=				\
 		hal/usb_ops_linux.o	\
 		os_dep/ioctl_cfg80211.o	\
 		os_dep/mlme_linux.o	\
-		os_dep/osdep_service.o	\
 		os_dep/os_intfs.o	\
 		os_dep/recv_linux.o	\
 		os_dep/usb_intf.o	\
 		os_dep/usb_ops_linux.o	\
 		os_dep/xmit_linux.o
 
+r8723au-$(CONFIG_8723AU_BT_COEXIST) += hal/rtl8723a_bt-coexist.o
+r8723au-$(CONFIG_8723AU_AP_MODE) += core/rtw_ap.o
+
 obj-$(CONFIG_R8723AU)	:= r8723au.o
 
-ccflags-y += -D__CHECK_ENDIAN__ -I$(src)/include
+ccflags-y += -Wtype-limits -D__CHECK_ENDIAN__ -I$(src)/include
diff --git a/drivers/staging/rtl8723au/core/rtw_ap.c b/drivers/staging/rtl8723au/core/rtw_ap.c
index a357e98cb83..c8700b38386 100644
--- a/drivers/staging/rtl8723au/core/rtw_ap.c
+++ b/drivers/staging/rtl8723au/core/rtw_ap.c
@@ -18,10 +18,9 @@
 #include <drv_types.h>
 #include <linux/ieee80211.h>
 #include <wifi.h>
+#include <rtl8723a_cmd.h>
+#include <rtl8723a_hal.h>
 
-#ifdef CONFIG_8723AU_AP_MODE
-
-extern unsigned char RTW_WPA_OUI23A[];
 extern unsigned char WMM_OUI23A[];
 extern unsigned char WPS_OUI23A[];
 extern unsigned char P2P_OUI23A[];
@@ -74,12 +73,13 @@ static void update_BCNTIM(struct rtw_adapter *padapter)
 	struct wlan_bssid_ex *pnetwork_mlmeext = &pmlmeinfo->network;
 	unsigned char *pie = pnetwork_mlmeext->IEs;
 	u8 *p, *dst_ie, *premainder_ie = NULL, *pbackup_remainder_ie = NULL;
-	u16 tim_bitmap_le;
+	__le16 tim_bitmap_le;
 	uint offset, tmp_len, tim_ielen, tim_ie_offset, remainder_ielen;
 
 	tim_bitmap_le = cpu_to_le16(pstapriv->tim_bitmap);
 
-	p = rtw_get_ie23a(pie + _FIXED_IE_LENGTH_, _TIM_IE_, &tim_ielen, pnetwork_mlmeext->IELength - _FIXED_IE_LENGTH_);
+	p = rtw_get_ie23a(pie + _FIXED_IE_LENGTH_, WLAN_EID_TIM, &tim_ielen,
+			  pnetwork_mlmeext->IELength - _FIXED_IE_LENGTH_);
 	if (p != NULL && tim_ielen>0) {
 		tim_ielen += 2;
 
@@ -98,12 +98,16 @@ static void update_BCNTIM(struct rtw_adapter *padapter)
 		offset = _FIXED_IE_LENGTH_;
 
 		/* get ssid_ie len */
-		p = rtw_get_ie23a(pie + _BEACON_IE_OFFSET_, _SSID_IE_, &tmp_len, (pnetwork_mlmeext->IELength - _BEACON_IE_OFFSET_));
+		p = rtw_get_ie23a(pie + _BEACON_IE_OFFSET_, WLAN_EID_SSID,
+				  &tmp_len, (pnetwork_mlmeext->IELength -
+					     _BEACON_IE_OFFSET_));
 		if (p != NULL)
 			offset += tmp_len+2;
 
 		/*  get supported rates len */
-		p = rtw_get_ie23a(pie + _BEACON_IE_OFFSET_, _SUPPORTEDRATES_IE_, &tmp_len, (pnetwork_mlmeext->IELength - _BEACON_IE_OFFSET_));
+		p = rtw_get_ie23a(pie + _BEACON_IE_OFFSET_, WLAN_EID_SUPP_RATES,
+				  &tmp_len, (pnetwork_mlmeext->IELength -
+					     _BEACON_IE_OFFSET_));
 		if (p !=  NULL)
 			offset += tmp_len+2;
 
@@ -124,7 +128,7 @@ static void update_BCNTIM(struct rtw_adapter *padapter)
 			memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen);
 	}
 
-	*dst_ie++= _TIM_IE_;
+	*dst_ie++= WLAN_EID_TIM;
 
 	if ((pstapriv->tim_bitmap&0xff00) && (pstapriv->tim_bitmap&0x00fc))
 		tim_ielen = 5;
@@ -182,7 +186,7 @@ void	expire_timeout_chk23a(struct rtw_adapter *padapter)
 	struct sta_info *psta;
 	struct sta_priv *pstapriv = &padapter->stapriv;
 	u8 chk_alive_num = 0;
-	char chk_alive_list[NUM_STA];
+	struct sta_info *chk_alive_list[NUM_STA];
 	int i;
 
 	spin_lock_bh(&pstapriv->auth_list_lock);
@@ -248,7 +252,7 @@ void	expire_timeout_chk23a(struct rtw_adapter *padapter)
 
 					/* to update bcn with tim_bitmap for this station */
 					pstapriv->tim_bitmap |= CHKBIT(psta->aid);
-					update_beacon23a(padapter, _TIM_IE_, NULL, false);
+					update_beacon23a(padapter, WLAN_EID_TIM, NULL, false);
 
 					if (!pmlmeext->active_keep_alive_check)
 						continue;
@@ -256,13 +260,7 @@ void	expire_timeout_chk23a(struct rtw_adapter *padapter)
 			}
 
 			if (pmlmeext->active_keep_alive_check) {
-				int stainfo_offset;
-
-				stainfo_offset = rtw_stainfo_offset23a(pstapriv, psta);
-				if (stainfo_offset_valid(stainfo_offset)) {
-					chk_alive_list[chk_alive_num++] = stainfo_offset;
-				}
-
+				chk_alive_list[chk_alive_num++] = psta;
 				continue;
 			}
 
@@ -296,14 +294,14 @@ void	expire_timeout_chk23a(struct rtw_adapter *padapter)
 		if (rtw_get_oper_ch23a(padapter) != pmlmeext->cur_channel) {
 			backup_oper_channel = rtw_get_oper_ch23a(padapter);
 			SelectChannel23a(padapter, pmlmeext->cur_channel);
-	}
+		}
 
 	/* issue null data to check sta alive*/
 	for (i = 0; i < chk_alive_num; i++) {
 
 		int ret = _FAIL;
 
-		psta = rtw_get_stainfo23a_by_offset23a(pstapriv, chk_alive_list[i]);
+		psta = chk_alive_list[i];
 		if (!(psta->state &_FW_LINKED))
 			continue;
 
@@ -357,7 +355,7 @@ void add_RATid23a(struct rtw_adapter *padapter, struct sta_info *psta, u8 rssi_l
 	unsigned int tx_ra_bitmap = 0;
 	struct ht_priv *psta_ht = NULL;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct wlan_bssid_ex *pcur_network = (struct wlan_bssid_ex *)&pmlmepriv->cur_network.network;
+	struct wlan_bssid_ex *pcur_network = &pmlmepriv->cur_network.network;
 
 	if (psta)
 		psta_ht = &psta->htpriv;
@@ -374,24 +372,24 @@ void add_RATid23a(struct rtw_adapter *padapter, struct sta_info *psta, u8 rssi_l
 			tx_ra_bitmap |= rtw_get_bit_value_from_ieee_value23a(psta->bssrateset[i]&0x7f);
 	}
 	/* n mode ra_bitmap */
-	if (psta_ht->ht_option)
-	{
-		rtw23a_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+	if (psta_ht->ht_option) {
+		rf_type = rtl8723a_get_rf_type(padapter);
+
 		if (rf_type == RF_2T2R)
 			limit = 16;/*  2R */
 		else
 			limit = 8;/*   1R */
 
-		for (i = 0; i<limit; i++) {
-			if (psta_ht->ht_cap.mcs.rx_mask[i/8] & BIT(i%8))
-				tx_ra_bitmap |= CHKBIT(i+12);
+		for (i = 0; i < limit; i++) {
+			if (psta_ht->ht_cap.mcs.rx_mask[i / 8] & BIT(i % 8))
+				tx_ra_bitmap |= BIT(i + 12);
 		}
 
 		/* max short GI rate */
 		shortGIrate = psta_ht->sgi;
 	}
 
-	if (pcur_network->Configuration.DSConfig > 14) {
+	if (pcur_network->DSConfig > 14) {
 		/*  5G band */
 		if (tx_ra_bitmap & 0xffff000)
 			sta_band |= WIRELESS_11_5N | WIRELESS_11A;
@@ -432,7 +430,7 @@ void add_RATid23a(struct rtw_adapter *padapter, struct sta_info *psta, u8 rssi_l
 		/* bitmap[28:31]= Rate Adaptive id */
 		/* arg[0:4] = macid */
 		/* arg[5] = Short GI */
-		rtw_hal_add_ra_tid23a(padapter, tx_ra_bitmap, arg, rssi_level);
+		rtl8723a_add_rateatid(padapter, tx_ra_bitmap, arg, rssi_level);
 
 		if (shortGIrate == true)
 			init_rate |= BIT(6);
@@ -455,7 +453,7 @@ static void update_bmc_sta(struct rtw_adapter *padapter)
 	int i, supportRateNum = 0;
 	unsigned int tx_ra_bitmap = 0;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct wlan_bssid_ex *pcur_network = (struct wlan_bssid_ex *)&pmlmepriv->cur_network.network;
+	struct wlan_bssid_ex *pcur_network = &pmlmepriv->cur_network.network;
 	struct sta_info *psta = rtw_get_bcmc_stainfo23a(padapter);
 
 	if (psta)
@@ -484,7 +482,7 @@ static void update_bmc_sta(struct rtw_adapter *padapter)
 				tx_ra_bitmap |= rtw_get_bit_value_from_ieee_value23a(psta->bssrateset[i]&0x7f);
 		}
 
-		if (pcur_network->Configuration.DSConfig > 14) {
+		if (pcur_network->DSConfig > 14) {
 			/* force to A mode. 5G doesn't support CCK rates */
 			network_type = WIRELESS_11A;
 			tx_ra_bitmap = 0x150; /*  6, 12, 24 Mbps */
@@ -498,7 +496,7 @@ static void update_bmc_sta(struct rtw_adapter *padapter)
 		init_rate = get_highest_rate_idx23a(tx_ra_bitmap&0x0fffffff)&0x3f;
 
 		/* ap mode */
-		rtw_hal_set_odm_var23a(padapter, HAL_ODM_STA_INFO, psta, true);
+		rtl8723a_SetHalODMVar(padapter, HAL_ODM_STA_INFO, psta, true);
 
 		{
 			u8 arg = 0;
@@ -515,16 +513,13 @@ static void update_bmc_sta(struct rtw_adapter *padapter)
 			/* bitmap[28:31]= Rate Adaptive id */
 			/* arg[0:4] = macid */
 			/* arg[5] = Short GI */
-			rtw_hal_add_ra_tid23a(padapter, tx_ra_bitmap, arg, 0);
-
+			rtl8723a_add_rateatid(padapter, tx_ra_bitmap, arg, 0);
 		}
 
 		/* set ra_id, init_rate */
 		psta->raid = raid;
 		psta->init_rate = init_rate;
 
-		rtw_stassoc_hw_rpt23a(padapter, psta);
-
 		spin_lock_bh(&psta->lock);
 		psta->state = _FW_LINKED;
 		spin_unlock_bh(&psta->lock);
@@ -556,7 +551,7 @@ void update_sta_info23a_apmode23a(struct rtw_adapter *padapter, struct sta_info
 	DBG_8723A("%s\n", __func__);
 
 	/* ap mode */
-	rtw_hal_set_odm_var23a(padapter, HAL_ODM_STA_INFO, psta, true);
+	rtl8723a_SetHalODMVar(padapter, HAL_ODM_STA_INFO, psta, true);
 
 	if (psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X)
 		psta->ieee8021x_blocked = true;
@@ -632,9 +627,8 @@ static void update_hw_ht_param(struct rtw_adapter *padapter)
 
 	min_MPDU_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) >> 2;
 
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_AMPDU_MIN_SPACE, (u8 *)(&min_MPDU_spacing));
-
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_AMPDU_FACTOR, (u8 *)(&max_AMPDU_len));
+	rtl8723a_set_ampdu_min_space(padapter, min_MPDU_spacing);
+	rtl8723a_set_ampdu_factor(padapter, max_AMPDU_len);
 
 	/*  Config SM Power Save setting */
 	pmlmeinfo->SM_PS = (pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info & 0x0C) >> 2;
@@ -644,25 +638,22 @@ static void update_hw_ht_param(struct rtw_adapter *padapter)
 
 static void start_bss_network(struct rtw_adapter *padapter, u8 *pbuf)
 {
-	u8 *p;
+	const u8 *p;
 	u8 val8, cur_channel, cur_bwmode, cur_ch_offset;
 	u16 bcn_interval;
 	u32 acparm;
-	int ie_len;
 	struct registry_priv *pregpriv = &padapter->registrypriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct security_priv* psecuritypriv = &padapter->securitypriv;
-	struct wlan_bssid_ex *pnetwork = (struct wlan_bssid_ex *)&pmlmepriv->cur_network.network;
+	struct wlan_bssid_ex *pnetwork = &pmlmepriv->cur_network.network;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *pnetwork_mlmeext = &pmlmeinfo->network;
 	struct HT_info_element *pht_info = NULL;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-#endif /* CONFIG_8723AU_P2P */
+	int bcn_fixed_size;
 
-	bcn_interval = (u16)pnetwork->Configuration.BeaconPeriod;
-	cur_channel = pnetwork->Configuration.DSConfig;
+	bcn_interval = (u16)pnetwork->BeaconPeriod;
+	cur_channel = pnetwork->DSConfig;
 	cur_bwmode = HT_CHANNEL_WIDTH_20;;
 	cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
 
@@ -675,7 +666,7 @@ static void start_bss_network(struct rtw_adapter *padapter, u8 *pbuf)
 	/* todo: update wmm, ht cap */
 	/* pmlmeinfo->WMM_enable; */
 	/* pmlmeinfo->HT_enable; */
-	if (pmlmepriv->qospriv.qos_option)
+	if (pmlmepriv->qos_option)
 		pmlmeinfo->WMM_enable = true;
 	if (pmlmepriv->htpriv.ht_option) {
 		pmlmeinfo->WMM_enable = true;
@@ -687,7 +678,10 @@ static void start_bss_network(struct rtw_adapter *padapter, u8 *pbuf)
 	if (pmlmepriv->cur_network.join_res != true) {
 		/* setting only at  first time */
 		/* WEP Key will be set before this function, do not clear CAM. */
-		if ((psecuritypriv->dot11PrivacyAlgrthm != _WEP40_) && (psecuritypriv->dot11PrivacyAlgrthm != _WEP104_))
+		if (psecuritypriv->dot11PrivacyAlgrthm !=
+		    WLAN_CIPHER_SUITE_WEP40 &&
+		    psecuritypriv->dot11PrivacyAlgrthm !=
+		    WLAN_CIPHER_SUITE_WEP104)
 			flush_all_cam_entry23a(padapter);	/* clear CAM */
 	}
 
@@ -695,27 +689,28 @@ static void start_bss_network(struct rtw_adapter *padapter, u8 *pbuf)
 	Set_MSR23a(padapter, _HW_STATE_AP_);
 
 	/* Set BSSID REG */
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_BSSID, pnetwork->MacAddress);
+	hw_var_set_bssid(padapter, pnetwork->MacAddress);
 
 	/* Set EDCA param reg */
 	acparm = 0x002F3217; /*  VO */
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acparm));
+	rtl8723a_set_ac_param_vo(padapter, acparm);
 	acparm = 0x005E4317; /*  VI */
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acparm));
+	rtl8723a_set_ac_param_vi(padapter, acparm);
 	acparm = 0x005ea42b;
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acparm));
+	rtl8723a_set_ac_param_be(padapter, acparm);
 	acparm = 0x0000A444; /*  BK */
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acparm));
+	rtl8723a_set_ac_param_bk(padapter, acparm);
 
 	/* Set Security */
-	val8 = (psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X)? 0xcc: 0xcf;
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
+	val8 = (psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) ?
+		0xcc: 0xcf;
+	rtl8723a_set_sec_cfg(padapter, val8);
 
 	/* Beacon Control related register */
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_BEACON_INTERVAL, (u8 *)(&bcn_interval));
+	rtl8723a_set_beacon_interval(padapter, bcn_interval);
 
 	UpdateBrateTbl23a(padapter, pnetwork->SupportedRates);
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_BASIC_RATE, pnetwork->SupportedRates);
+	HalSetBrateCfg23a(padapter, pnetwork->SupportedRates);
 
 	if (!pmlmepriv->cur_network.join_res) {
 		/* setting only at  first time */
@@ -723,21 +718,26 @@ static void start_bss_network(struct rtw_adapter *padapter, u8 *pbuf)
 		/* disable dynamic functions, such as high power, DIG */
 
 		/* turn on all dynamic functions */
-		Switch_DM_Func23a(padapter, DYNAMIC_ALL_FUNC_ENABLE, true);
+		rtl8723a_odm_support_ability_set(padapter,
+						 DYNAMIC_ALL_FUNC_ENABLE);
 	}
 	/* set channel, bwmode */
-	p = rtw_get_ie23a((pnetwork->IEs + sizeof(struct ndis_802_11_fixed_ies)),
-			  _HT_ADD_INFO_IE_, &ie_len, (pnetwork->IELength -
-			  sizeof(struct ndis_802_11_fixed_ies)));
-	if (p && ie_len) {
-		pht_info = (struct HT_info_element *)(p+2);
+	bcn_fixed_size = offsetof(struct ieee80211_mgmt, u.beacon.variable) -
+		offsetof(struct ieee80211_mgmt, u.beacon);
+
+	p = cfg80211_find_ie(WLAN_EID_HT_OPERATION,
+			     pnetwork->IEs + bcn_fixed_size,
+			     pnetwork->IELength - bcn_fixed_size);
+	if (p && p[1]) {
+		pht_info = (struct HT_info_element *)(p + 2);
 
-		if ((pregpriv->cbw40_enable) &&	(pht_info->infos[0] & BIT(2))) {
+		if (pregpriv->cbw40_enable && pht_info->infos[0] & BIT(2)) {
 			/* switch to the 40M Hz mode */
 			cur_bwmode = HT_CHANNEL_WIDTH_40;
 			switch (pht_info->infos[0] & 0x3) {
 			case 1:
-				/* pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER; */
+				/* pmlmeext->cur_ch_offset =
+				   HAL_PRIME_CHNL_OFFSET_LOWER; */
 				cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
 				break;
 			case 3:
@@ -764,19 +764,13 @@ static void start_bss_network(struct rtw_adapter *padapter, u8 *pbuf)
 	update_wireless_mode23a(padapter);
 
 	/* udpate capability after cur_wireless_mode updated */
-	update_capinfo23a(padapter, rtw_get_capability23a((struct wlan_bssid_ex *)pnetwork));
+	update_capinfo23a(padapter, rtw_get_capability23a(pnetwork));
 
 	/* let pnetwork_mlmeext == pnetwork_mlme. */
 	memcpy(pnetwork_mlmeext, pnetwork, pnetwork->Length);
 
-#ifdef CONFIG_8723AU_P2P
-	memcpy(pwdinfo->p2p_group_ssid, pnetwork->Ssid.ssid,
-	       pnetwork->Ssid.ssid_len);
-	pwdinfo->p2p_group_ssid_len = pnetwork->Ssid.ssid_len;
-#endif /* CONFIG_8723AU_P2P */
-
 	if (pmlmeext->bstart_bss) {
-		update_beacon23a(padapter, _TIM_IE_, NULL, false);
+		update_beacon23a(padapter, WLAN_EID_TIM, NULL, false);
 
 		/* issue beacon frame */
 		if (send_beacon23a(padapter) == _FAIL)
@@ -787,19 +781,20 @@ static void start_bss_network(struct rtw_adapter *padapter, u8 *pbuf)
 	update_bmc_sta(padapter);
 }
 
-int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
+int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,
+			     unsigned int len)
 {
 	int ret = _SUCCESS;
 	u8 *p;
 	u8 *pHT_caps_ie = NULL;
 	u8 *pHT_info_ie = NULL;
 	struct sta_info *psta = NULL;
+	__le16 *pbeacon;
 	u16 cap, ht_cap = false;
 	uint ie_len = 0;
 	int group_cipher, pairwise_cipher;
 	u8 channel, network_type, supportRate[NDIS_802_11_LENGTH_RATES_EX];
 	int supportRateNum = 0;
-	u8 OUI1[] = {0x00, 0x50, 0xf2, 0x01};
 	u8 WMM_PARA_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
 	struct registry_priv *pregistrypriv = &padapter->registrypriv;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
@@ -823,7 +818,7 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
 	if (!check_fwstate(pmlmepriv, WIFI_AP_STATE))
 		return _FAIL;
 
-	if (len>MAX_IE_SZ)
+	if (len > MAX_IE_SZ)
 		return _FAIL;
 
 	pbss_network->IELength = len;
@@ -832,7 +827,8 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
 
 	memcpy(ie, pbuf, pbss_network->IELength);
 
-	if (pbss_network->InfrastructureMode!= Ndis802_11APMode)
+	if (pbss_network->ifmode != NL80211_IFTYPE_AP &&
+	    pbss_network->ifmode != NL80211_IFTYPE_P2P_GO)
 		return _FAIL;
 
 	pbss_network->Rssi = 0;
@@ -841,14 +837,14 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
 
 	/* beacon interval */
 	/* ie + 8;  8: TimeStamp, 2: Beacon Interval 2:Capability */
-	p = rtw_get_beacon_interval23a_from_ie(ie);
-	pbss_network->Configuration.BeaconPeriod = get_unaligned_le16(p);
+	pbeacon = rtw_get_beacon_interval23a_from_ie(ie);
+	pbss_network->BeaconPeriod = get_unaligned_le16(pbeacon);
 
 	/* capability */
 	cap = get_unaligned_le16(ie);
 
 	/* SSID */
-	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, _SSID_IE_, &ie_len,
+	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, WLAN_EID_SSID, &ie_len,
 			  (pbss_network->IELength -_BEACON_IE_OFFSET_));
 	if (p && ie_len > 0) {
 		memset(&pbss_network->Ssid, 0, sizeof(struct cfg80211_ssid));
@@ -858,17 +854,16 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
 
 	/* chnnel */
 	channel = 0;
-	pbss_network->Configuration.Length = 0;
-	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, _DSSET_IE_, &ie_len,
+	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, WLAN_EID_DS_PARAMS, &ie_len,
 			  (pbss_network->IELength - _BEACON_IE_OFFSET_));
 	if (p && ie_len > 0)
 		channel = *(p + 2);
 
-	pbss_network->Configuration.DSConfig = channel;
+	pbss_network->DSConfig = channel;
 
 	memset(supportRate, 0, NDIS_802_11_LENGTH_RATES_EX);
 	/*  get supported rates */
-	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, _SUPPORTEDRATES_IE_, &ie_len,
+	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, WLAN_EID_SUPP_RATES, &ie_len,
 			  (pbss_network->IELength - _BEACON_IE_OFFSET_));
 	if (p) {
 		memcpy(supportRate, p+2, ie_len);
@@ -876,7 +871,7 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
 	}
 
 	/* get ext_supported rates */
-	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, _EXT_SUPPORTEDRATES_IE_,
+	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, WLAN_EID_EXT_SUPP_RATES,
 			  &ie_len, pbss_network->IELength - _BEACON_IE_OFFSET_);
 	if (p) {
 		memcpy(supportRate+supportRateNum, p+2, ie_len);
@@ -889,10 +884,10 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
 	rtw_set_supported_rate23a(pbss_network->SupportedRates, network_type);
 
 	/* parsing ERP_IE */
-	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, _ERPINFO_IE_, &ie_len,
+	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, WLAN_EID_ERP_INFO, &ie_len,
 			  (pbss_network->IELength - _BEACON_IE_OFFSET_));
 	if (p && ie_len > 0)
-		ERP_IE_handler23a(padapter, (struct ndis_802_11_var_ies *)p);
+		ERP_IE_handler23a(padapter, p);
 
 	/* update privacy/security */
 	if (cap & BIT(4))
@@ -904,9 +899,9 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
 
 	/* wpa2 */
 	group_cipher = 0; pairwise_cipher = 0;
-	psecuritypriv->wpa2_group_cipher = _NO_PRIVACY_;
-	psecuritypriv->wpa2_pairwise_cipher = _NO_PRIVACY_;
-	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, _RSN_IE_2_, &ie_len,
+	psecuritypriv->wpa2_group_cipher = 0;
+	psecuritypriv->wpa2_pairwise_cipher = 0;
+	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, WLAN_EID_RSN, &ie_len,
 			  (pbss_network->IELength - _BEACON_IE_OFFSET_));
 	if (p && ie_len > 0) {
 		if (rtw_parse_wpa2_ie23a(p, ie_len+2, &group_cipher,
@@ -925,13 +920,13 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
 	ie_len = 0;
 	group_cipher = 0;
 	pairwise_cipher = 0;
-	psecuritypriv->wpa_group_cipher = _NO_PRIVACY_;
-	psecuritypriv->wpa_pairwise_cipher = _NO_PRIVACY_;
+	psecuritypriv->wpa_group_cipher = 0;
+	psecuritypriv->wpa_pairwise_cipher = 0;
 	for (p = ie + _BEACON_IE_OFFSET_; ;p += (ie_len + 2)) {
-		p = rtw_get_ie23a(p, _SSN_IE_1_, &ie_len,
+		p = rtw_get_ie23a(p, WLAN_EID_VENDOR_SPECIFIC, &ie_len,
 				  (pbss_network->IELength - _BEACON_IE_OFFSET_ -
 				  (ie_len + 2)));
-		if ((p) && (!memcmp(p+2, OUI1, 4))) {
+		if ((p) && (!memcmp(p+2, RTW_WPA_OUI23A_TYPE, 4))) {
 			if (rtw_parse_wpa_ie23a(p, ie_len+2, &group_cipher,
 						&pairwise_cipher, NULL) == _SUCCESS) {
 				psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_8021X;
@@ -953,14 +948,14 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
 
 	/* wmm */
 	ie_len = 0;
-	pmlmepriv->qospriv.qos_option = 0;
+	pmlmepriv->qos_option = 0;
 	if (pregistrypriv->wmm_enable) {
 		for (p = ie + _BEACON_IE_OFFSET_; ;p += (ie_len + 2)) {
-			p = rtw_get_ie23a(p, _VENDOR_SPECIFIC_IE_, &ie_len,
+			p = rtw_get_ie23a(p, WLAN_EID_VENDOR_SPECIFIC, &ie_len,
 					  (pbss_network->IELength -
 					  _BEACON_IE_OFFSET_ - (ie_len + 2)));
 			if ((p) && !memcmp(p+2, WMM_PARA_IE, 6)) {
-				pmlmepriv->qospriv.qos_option = 1;
+				pmlmepriv->qos_option = 1;
 
 				*(p+8) |= BIT(7);/* QoS Info, support U-APSD */
 
@@ -978,7 +973,7 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
 		}
 	}
 	/* parsing HT_CAP_IE */
-	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, _HT_CAPABILITY_IE_, &ie_len,
+	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, WLAN_EID_HT_CAPABILITY, &ie_len,
 			  (pbss_network->IELength - _BEACON_IE_OFFSET_));
 	if (p && ie_len > 0) {
 		u8 rf_type;
@@ -990,7 +985,7 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
 		ht_cap = true;
 		network_type |= WIRELESS_11_24N;
 
-		rtw23a_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+		rf_type = rtl8723a_get_rf_type(padapter);
 
 		if ((psecuritypriv->wpa_pairwise_cipher & WPA_CIPHER_CCMP) ||
 		    (psecuritypriv->wpa2_pairwise_cipher & WPA_CIPHER_CCMP))
@@ -1010,29 +1005,11 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
 	}
 
 	/* parsing HT_INFO_IE */
-	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, _HT_ADD_INFO_IE_, &ie_len,
+	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, WLAN_EID_HT_OPERATION, &ie_len,
 			  (pbss_network->IELength - _BEACON_IE_OFFSET_));
 	if (p && ie_len > 0)
 		pHT_info_ie = p;
 
-	switch (network_type) {
-	case WIRELESS_11B:
-		pbss_network->NetworkTypeInUse = Ndis802_11DS;
-		break;
-	case WIRELESS_11G:
-	case WIRELESS_11BG:
-            case WIRELESS_11G_24N:
-	case WIRELESS_11BG_24N:
-		pbss_network->NetworkTypeInUse = Ndis802_11OFDM24;
-		break;
-	case WIRELESS_11A:
-		pbss_network->NetworkTypeInUse = Ndis802_11OFDM5;
-		break;
-	default :
-		pbss_network->NetworkTypeInUse = Ndis802_11OFDM24;
-		break;
-	}
-
 	pmlmepriv->cur_network.network_type = network_type;
 
 	pmlmepriv->htpriv.ht_option = false;
@@ -1040,17 +1017,17 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
 	/* ht_cap */
 	if (pregistrypriv->ht_enable && ht_cap) {
 		pmlmepriv->htpriv.ht_option = true;
-		pmlmepriv->qospriv.qos_option = 1;
+		pmlmepriv->qos_option = 1;
 
 		if (pregistrypriv->ampdu_enable == 1)
 			pmlmepriv->htpriv.ampdu_enable = true;
 
-		HT_caps_handler23a(padapter, (struct ndis_802_11_var_ies *)pHT_caps_ie);
+		HT_caps_handler23a(padapter, pHT_caps_ie);
 
-		HT_info_handler23a(padapter, (struct ndis_802_11_var_ies *)pHT_info_ie);
+		HT_info_handler23a(padapter, pHT_info_ie);
 	}
 
-	pbss_network->Length = get_wlan_bssid_ex_sz((struct wlan_bssid_ex  *)pbss_network);
+	pbss_network->Length = get_wlan_bssid_ex_sz(pbss_network);
 
 	/* issue beacon to start bss network */
 	start_bss_network(padapter, (u8*)pbss_network);
@@ -1058,7 +1035,9 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len)
 	/* alloc sta_info for ap itself */
 	psta = rtw_get_stainfo23a(&padapter->stapriv, pbss_network->MacAddress);
 	if (!psta) {
-		psta = rtw_alloc_stainfo23a(&padapter->stapriv, pbss_network->MacAddress);
+		psta = rtw_alloc_stainfo23a(&padapter->stapriv,
+					    pbss_network->MacAddress,
+					    GFP_KERNEL);
 		if (!psta)
 			return _FAIL;
 	}
@@ -1200,24 +1179,21 @@ static void update_bcn_erpinfo_ie(struct rtw_adapter *padapter)
 		return;
 
 	/* parsing ERP_IE */
-	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, _ERPINFO_IE_, &len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
-	if (p && len>0)
-	{
-		struct ndis_802_11_var_ies * pIE = (struct ndis_802_11_var_ies *)p;
-
+	p = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, WLAN_EID_ERP_INFO, &len, (pnetwork->IELength - _BEACON_IE_OFFSET_));
+	if (p && len > 0) {
 		if (pmlmepriv->num_sta_non_erp == 1)
-			pIE->data[0] |= WLAN_ERP_NON_ERP_PRESENT |
+			p[2] |= WLAN_ERP_NON_ERP_PRESENT |
 				WLAN_ERP_USE_PROTECTION;
 		else
-			pIE->data[0] &= ~(WLAN_ERP_NON_ERP_PRESENT |
-					  WLAN_ERP_USE_PROTECTION);
+			p[2] &= ~(WLAN_ERP_NON_ERP_PRESENT |
+				  WLAN_ERP_USE_PROTECTION);
 
 		if (pmlmepriv->num_sta_no_short_preamble > 0)
-			pIE->data[0] |= WLAN_ERP_BARKER_PREAMBLE;
+			p[2] |= WLAN_ERP_BARKER_PREAMBLE;
 		else
-			pIE->data[0] &= ~(WLAN_ERP_BARKER_PREAMBLE);
+			p[2] &= ~(WLAN_ERP_BARKER_PREAMBLE);
 
-		ERP_IE_handler23a(padapter, pIE);
+		ERP_IE_handler23a(padapter, p);
 	}
 }
 
@@ -1259,6 +1235,10 @@ static void update_bcn_wps_ie(struct rtw_adapter *padapter)
 
 	DBG_8723A("%s\n", __func__);
 
+	pwps_ie_src = pmlmepriv->wps_beacon_ie;
+	if (pwps_ie_src == NULL)
+		return;
+
 	pwps_ie = rtw_get_wps_ie23a(ie+_FIXED_IE_LENGTH_, ielen-_FIXED_IE_LENGTH_, NULL, &wps_ielen);
 
 	if (pwps_ie == NULL || wps_ielen == 0)
@@ -1277,10 +1257,6 @@ static void update_bcn_wps_ie(struct rtw_adapter *padapter)
 			       remainder_ielen);
 	}
 
-	pwps_ie_src = pmlmepriv->wps_beacon_ie;
-	if (pwps_ie_src == NULL)
-		return;
-
 	wps_ielen = (uint)pwps_ie_src[1];/* to get ie data len */
 	if ((wps_offset+wps_ielen+2+remainder_ielen)<= MAX_IE_SZ)
 	{
@@ -1306,26 +1282,16 @@ static void update_bcn_vendor_spec_ie(struct rtw_adapter *padapter, u8*oui)
 {
 	DBG_8723A("%s\n", __func__);
 
-	if (!memcmp(RTW_WPA_OUI23A, oui, 4))
-	{
+	if (!memcmp(RTW_WPA_OUI23A_TYPE, oui, 4))
 		update_bcn_wpa_ie(padapter);
-	}
 	else if (!memcmp(WMM_OUI23A, oui, 4))
-	{
 		update_bcn_wmm_ie(padapter);
-	}
 	else if (!memcmp(WPS_OUI23A, oui, 4))
-	{
 		update_bcn_wps_ie(padapter);
-	}
 	else if (!memcmp(P2P_OUI23A, oui, 4))
-	{
 		update_bcn_p2p_ie(padapter);
-	}
 	else
-	{
 		DBG_8723A("unknown OUI type!\n");
-	}
 }
 
 void update_beacon23a(struct rtw_adapter *padapter, u8 ie_id, u8 *oui, u8 tx)
@@ -1350,50 +1316,37 @@ void update_beacon23a(struct rtw_adapter *padapter, u8 ie_id, u8 *oui, u8 tx)
 
 	switch (ie_id)
 	{
-		case 0xFF:
-
-			update_bcn_fixed_ie(padapter);/* 8: TimeStamp, 2: Beacon Interval 2:Capability */
-
-			break;
-
-		case _TIM_IE_:
-
-			update_BCNTIM(padapter);
-
-			break;
-
-		case _ERPINFO_IE_:
-
-			update_bcn_erpinfo_ie(padapter);
-
-			break;
-
-		case _HT_CAPABILITY_IE_:
-
-			update_bcn_htcap_ie(padapter);
-
-			break;
-
-		case _RSN_IE_2_:
-
-			update_bcn_rsn_ie(padapter);
-
-			break;
+	case 0xFF:
+		/* 8: TimeStamp, 2: Beacon Interval 2:Capability */
+		update_bcn_fixed_ie(padapter);
+		break;
 
-		case _HT_ADD_INFO_IE_:
+	case WLAN_EID_TIM:
+		update_BCNTIM(padapter);
+		break;
 
-			update_bcn_htinfo_ie(padapter);
+	case WLAN_EID_ERP_INFO:
+		update_bcn_erpinfo_ie(padapter);
+		break;
 
-			break;
+	case WLAN_EID_HT_CAPABILITY:
+		update_bcn_htcap_ie(padapter);
+		break;
 
-		case _VENDOR_SPECIFIC_IE_:
+	case WLAN_EID_RSN:
+		update_bcn_rsn_ie(padapter);
+		break;
 
-			update_bcn_vendor_spec_ie(padapter, oui);
+	case WLAN_EID_HT_OPERATION:
+		update_bcn_htinfo_ie(padapter);
+		break;
 
-			break;
+	case WLAN_EID_VENDOR_SPECIFIC:
+		update_bcn_vendor_spec_ie(padapter, oui);
+		break;
 
-		default:
-			break;
+	default:
+		break;
 	}
 
 	pmlmepriv->update_bcn = true;
@@ -1432,28 +1385,28 @@ static int rtw_ht_operation_update(struct rtw_adapter *padapter)
 	DBG_8723A("%s current operation mode = 0x%X\n",
 		   __func__, pmlmepriv->ht_op_mode);
 
-	if (!(pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT)
+	if (!(pmlmepriv->ht_op_mode & IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT)
 	    && pmlmepriv->num_sta_ht_no_gf) {
 		pmlmepriv->ht_op_mode |=
-			HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT;
+			IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT;
 		op_mode_changes++;
 	} else if ((pmlmepriv->ht_op_mode &
-		    HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT) &&
+		    IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT) &&
 		   pmlmepriv->num_sta_ht_no_gf == 0) {
 		pmlmepriv->ht_op_mode &=
-			~HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT;
+			~IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT;
 		op_mode_changes++;
 	}
 
-	if (!(pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT) &&
+	if (!(pmlmepriv->ht_op_mode & IEEE80211_HT_OP_MODE_NON_HT_STA_PRSNT) &&
 	    (pmlmepriv->num_sta_no_ht || pmlmepriv->olbc_ht)) {
-		pmlmepriv->ht_op_mode |= HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT;
+		pmlmepriv->ht_op_mode |= IEEE80211_HT_OP_MODE_NON_HT_STA_PRSNT;
 		op_mode_changes++;
 	} else if ((pmlmepriv->ht_op_mode &
-		    HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT) &&
+		    IEEE80211_HT_OP_MODE_NON_HT_STA_PRSNT) &&
 		   (pmlmepriv->num_sta_no_ht == 0 && !pmlmepriv->olbc_ht)) {
 		pmlmepriv->ht_op_mode &=
-			~HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT;
+			~IEEE80211_HT_OP_MODE_NON_HT_STA_PRSNT;
 		op_mode_changes++;
 	}
 
@@ -1461,21 +1414,21 @@ static int rtw_ht_operation_update(struct rtw_adapter *padapter)
 	 * station is associated. Probably it's a theoretical case, since
 	 * it looks like all known HT STAs support greenfield.
 	 */
-	new_op_mode = 0;
 	if (pmlmepriv->num_sta_no_ht ||
-	    (pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT))
-		new_op_mode = OP_MODE_MIXED;
-	else if ((phtpriv_ap->ht_cap.cap_info & IEEE80211_HT_CAP_SUP_WIDTH_20_40)
-		 && pmlmepriv->num_sta_ht_20mhz)
-		new_op_mode = OP_MODE_20MHZ_HT_STA_ASSOCED;
+	    (pmlmepriv->ht_op_mode & IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT))
+		new_op_mode = IEEE80211_HT_OP_MODE_PROTECTION_NONHT_MIXED;
+	else if ((le16_to_cpu(phtpriv_ap->ht_cap.cap_info) &
+		  IEEE80211_HT_CAP_SUP_WIDTH_20_40) &&
+		 pmlmepriv->num_sta_ht_20mhz)
+		new_op_mode = IEEE80211_HT_OP_MODE_PROTECTION_20MHZ;
 	else if (pmlmepriv->olbc_ht)
-		new_op_mode = OP_MODE_MAY_BE_LEGACY_STAS;
+		new_op_mode = IEEE80211_HT_OP_MODE_PROTECTION_NONMEMBER;
 	else
-		new_op_mode = OP_MODE_PURE;
+		new_op_mode = IEEE80211_HT_OP_MODE_PROTECTION_NONE;
 
-	cur_op_mode = pmlmepriv->ht_op_mode & HT_INFO_OPERATION_MODE_OP_MODE_MASK;
+	cur_op_mode = pmlmepriv->ht_op_mode & IEEE80211_HT_OP_MODE_PROTECTION;
 	if (cur_op_mode != new_op_mode) {
-		pmlmepriv->ht_op_mode &= ~HT_INFO_OPERATION_MODE_OP_MODE_MASK;
+		pmlmepriv->ht_op_mode &= ~IEEE80211_HT_OP_MODE_PROTECTION;
 		pmlmepriv->ht_op_mode |= new_op_mode;
 		op_mode_changes++;
 	}
@@ -1562,7 +1515,7 @@ void bss_cap_update_on_sta_join23a(struct rtw_adapter *padapter, struct sta_info
 			if (pmlmepriv->num_sta_non_erp == 1)
 			{
 				beacon_updated = true;
-				update_beacon23a(padapter, _ERPINFO_IE_, NULL, true);
+				update_beacon23a(padapter, WLAN_EID_ERP_INFO, NULL, true);
 			}
 		}
 
@@ -1578,7 +1531,7 @@ void bss_cap_update_on_sta_join23a(struct rtw_adapter *padapter, struct sta_info
 			if (pmlmepriv->num_sta_non_erp == 0)
 			{
 				beacon_updated = true;
-				update_beacon23a(padapter, _ERPINFO_IE_, NULL, true);
+				update_beacon23a(padapter, WLAN_EID_ERP_INFO, NULL, true);
 			}
 		}
 
@@ -1669,8 +1622,8 @@ void bss_cap_update_on_sta_join23a(struct rtw_adapter *padapter, struct sta_info
 
 	if (rtw_ht_operation_update(padapter) > 0)
 	{
-		update_beacon23a(padapter, _HT_CAPABILITY_IE_, NULL, false);
-		update_beacon23a(padapter, _HT_ADD_INFO_IE_, NULL, true);
+		update_beacon23a(padapter, WLAN_EID_HT_CAPABILITY, NULL, false);
+		update_beacon23a(padapter, WLAN_EID_HT_OPERATION, NULL, true);
 	}
 
 	/* update associcated stations cap. */
@@ -1705,7 +1658,8 @@ u8 bss_cap_update_on_sta_leave23a(struct rtw_adapter *padapter, struct sta_info
 		if (pmlmepriv->num_sta_non_erp == 0)
 		{
 			beacon_updated = true;
-			update_beacon23a(padapter, _ERPINFO_IE_, NULL, true);
+			update_beacon23a(padapter, WLAN_EID_ERP_INFO,
+					 NULL, true);
 		}
 	}
 
@@ -1737,8 +1691,8 @@ u8 bss_cap_update_on_sta_leave23a(struct rtw_adapter *padapter, struct sta_info
 
 	if (rtw_ht_operation_update(padapter) > 0)
 	{
-		update_beacon23a(padapter, _HT_CAPABILITY_IE_, NULL, false);
-		update_beacon23a(padapter, _HT_ADD_INFO_IE_, NULL, true);
+		update_beacon23a(padapter, WLAN_EID_HT_CAPABILITY, NULL, false);
+		update_beacon23a(padapter, WLAN_EID_HT_OPERATION, NULL, true);
 	}
 
 	/* update associcated stations cap. */
@@ -1806,8 +1760,8 @@ int rtw_ap_inform_ch_switch23a (struct rtw_adapter *padapter, u8 new_ch, u8 ch_o
 	if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
 		return ret;
 
-	DBG_8723A(FUNC_NDEV_FMT" with ch:%u, offset:%u\n",
-		FUNC_NDEV_ARG(padapter->pnetdev), new_ch, ch_offset);
+	DBG_8723A("%s(%s): with ch:%u, offset:%u\n", __func__,
+		  padapter->pnetdev->name, new_ch, ch_offset);
 
 	spin_lock_bh(&pstapriv->asoc_list_lock);
 	phead = &pstapriv->asoc_list;
@@ -1835,10 +1789,10 @@ int rtw_sta_flush23a(struct rtw_adapter *padapter)
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 	u8 chk_alive_num = 0;
-	char chk_alive_list[NUM_STA];
+	struct sta_info *chk_alive_list[NUM_STA];
 	int i;
 
-	DBG_8723A(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(padapter->pnetdev));
+	DBG_8723A("%s(%s)\n", __func__, padapter->pnetdev->name);
 
 	if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
 		return ret;
@@ -1847,8 +1801,6 @@ int rtw_sta_flush23a(struct rtw_adapter *padapter)
 	phead = &pstapriv->asoc_list;
 
 	list_for_each_safe(plist, ptmp, phead) {
-		int stainfo_offset;
-
 		psta = container_of(plist, struct sta_info, asoc_list);
 
 		/* Remove sta from asoc_list */
@@ -1856,18 +1808,14 @@ int rtw_sta_flush23a(struct rtw_adapter *padapter)
 		pstapriv->asoc_list_cnt--;
 
 		/* Keep sta for ap_free_sta23a() beyond this asoc_list loop */
-		stainfo_offset = rtw_stainfo_offset23a(pstapriv, psta);
-		if (stainfo_offset_valid(stainfo_offset)) {
-			chk_alive_list[chk_alive_num++] = stainfo_offset;
-		}
+		chk_alive_list[chk_alive_num++] = psta;
 	}
 	spin_unlock_bh(&pstapriv->asoc_list_lock);
 
 	/* For each sta in chk_alive_list, call ap_free_sta23a */
-	for (i = 0; i < chk_alive_num; i++) {
-		psta = rtw_get_stainfo23a_by_offset23a(pstapriv, chk_alive_list[i]);
-		ap_free_sta23a(padapter, psta, true, WLAN_REASON_DEAUTH_LEAVING);
-	}
+	for (i = 0; i < chk_alive_num; i++)
+		ap_free_sta23a(padapter, chk_alive_list[i], true,
+			       WLAN_REASON_DEAUTH_LEAVING);
 
 	issue_deauth23a(padapter, bc_addr, WLAN_REASON_DEAUTH_LEAVING);
 
@@ -1888,7 +1836,7 @@ void sta_info_update23a(struct rtw_adapter *padapter, struct sta_info *psta)
 	else
 		psta->qos_option = 0;
 
-	if (pmlmepriv->qospriv.qos_option == 0)
+	if (pmlmepriv->qos_option == 0)
 		psta->qos_option = 0;
 
 	/* update 802.11n ht cap. */
@@ -1928,25 +1876,29 @@ void rtw_ap_restore_network(struct rtw_adapter *padapter)
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 	struct list_head *phead, *plist, *ptmp;
 	u8 chk_alive_num = 0;
-	char chk_alive_list[NUM_STA];
+	struct sta_info *chk_alive_list[NUM_STA];
 	int i;
 
-	rtw_setopmode_cmd23a(padapter, Ndis802_11APMode);
+	rtw_setopmode_cmd23a(padapter, NL80211_IFTYPE_AP);
 
 	set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
 
 	start_bss_network(padapter, (u8*)&mlmepriv->cur_network.network);
 
-	if ((padapter->securitypriv.dot11PrivacyAlgrthm == _TKIP_) ||
-		(padapter->securitypriv.dot11PrivacyAlgrthm == _AES_))
-	{
+	if (padapter->securitypriv.dot11PrivacyAlgrthm ==
+	    WLAN_CIPHER_SUITE_TKIP ||
+	    padapter->securitypriv.dot11PrivacyAlgrthm ==
+	    WLAN_CIPHER_SUITE_CCMP) {
 		/* restore group key, WEP keys is restored in ips_leave23a() */
-		rtw_set_key23a(padapter, psecuritypriv, psecuritypriv->dot118021XGrpKeyid, 0);
+		rtw_set_key23a(padapter, psecuritypriv,
+			       psecuritypriv->dot118021XGrpKeyid, 0);
 	}
 
 	/* per sta pairwise key and settings */
-	if ((padapter->securitypriv.dot11PrivacyAlgrthm != _TKIP_) &&
-		(padapter->securitypriv.dot11PrivacyAlgrthm != _AES_)) {
+	if (padapter->securitypriv.dot11PrivacyAlgrthm !=
+	    WLAN_CIPHER_SUITE_TKIP &&
+	    padapter->securitypriv.dot11PrivacyAlgrthm !=
+	    WLAN_CIPHER_SUITE_CCMP) {
 		return;
 	}
 
@@ -1955,26 +1907,17 @@ void rtw_ap_restore_network(struct rtw_adapter *padapter)
 	phead = &pstapriv->asoc_list;
 
 	list_for_each_safe(plist, ptmp, phead) {
-		int stainfo_offset;
-
 		psta = container_of(plist, struct sta_info, asoc_list);
 
-		stainfo_offset = rtw_stainfo_offset23a(pstapriv, psta);
-		if (stainfo_offset_valid(stainfo_offset)) {
-			chk_alive_list[chk_alive_num++] = stainfo_offset;
-		}
+		chk_alive_list[chk_alive_num++] = psta;
 	}
 
 	spin_unlock_bh(&pstapriv->asoc_list_lock);
 
 	for (i = 0; i < chk_alive_num; i++) {
-		psta = rtw_get_stainfo23a_by_offset23a(pstapriv, chk_alive_list[i]);
+		psta = chk_alive_list[i];
 
-		if (psta == NULL) {
-			DBG_8723A(FUNC_ADPT_FMT" sta_info is null\n", FUNC_ADPT_ARG(padapter));
-		}
-		else if (psta->state &_FW_LINKED)
-		{
+		if (psta->state &_FW_LINKED) {
 			Update_RA_Entry23a(padapter, psta);
 			/* pairwise key */
 			rtw_setstakey_cmd23a(padapter, (unsigned char *)psta, true);
@@ -2083,5 +2026,3 @@ void stop_ap_mode23a(struct rtw_adapter *padapter)
 
 	rtw23a_free_mlme_priv_ie_data(pmlmepriv);
 }
-
-#endif /* CONFIG_8723AU_AP_MODE */
diff --git a/drivers/staging/rtl8723au/core/rtw_cmd.c b/drivers/staging/rtl8723au/core/rtw_cmd.c
index 5e3088a0180..1696cb8b17c 100644
--- a/drivers/staging/rtl8723au/core/rtw_cmd.c
+++ b/drivers/staging/rtl8723au/core/rtw_cmd.c
@@ -17,12 +17,9 @@
 #include <osdep_service.h>
 #include <drv_types.h>
 #include <recv_osdep.h>
-#include <cmd_osdep.h>
 #include <mlme_osdep.h>
-
-#ifdef CONFIG_8723AU_BT_COEXIST
-#include <rtl8723a_hal.h>
-#endif /*  CONFIG_8723AU_BT_COEXIST */
+#include <rtl8723a_cmd.h>
+#include <rtw_sreset.h>
 
 static struct cmd_hdl wlancmds[] = {
 	GEN_DRV_CMD_HANDLER(0, NULL) /*0*/
@@ -175,137 +172,33 @@ int rtw_init_cmd_priv23a(struct cmd_priv *pcmdpriv)
 {
 	int res = _SUCCESS;
 
-	sema_init(&pcmdpriv->cmd_queue_sema, 0);
-	sema_init(&pcmdpriv->terminate_cmdthread_sema, 0);
-
-	_rtw_init_queue23a(&pcmdpriv->cmd_queue);
-
-	pcmdpriv->cmd_seq = 1;
-
-	pcmdpriv->cmd_allocated_buf = kzalloc(MAX_CMDSZ + CMDBUFF_ALIGN_SZ,
-					      GFP_KERNEL);
-
-	if (pcmdpriv->cmd_allocated_buf == NULL) {
-		res = _FAIL;
-		goto exit;
-	}
-
-	pcmdpriv->cmd_buf = pcmdpriv->cmd_allocated_buf + CMDBUFF_ALIGN_SZ -
-			    ((unsigned long)(pcmdpriv->cmd_allocated_buf) &
-			    (CMDBUFF_ALIGN_SZ - 1));
-
-	pcmdpriv->rsp_allocated_buf = kzalloc(MAX_RSPSZ + 4, GFP_KERNEL);
-
-	if (!pcmdpriv->rsp_allocated_buf) {
-		res = _FAIL;
-		goto exit;
-	}
-
-	pcmdpriv->rsp_buf = pcmdpriv->rsp_allocated_buf + 4 -
-			    ((unsigned long)(pcmdpriv->rsp_allocated_buf) & 3);
-
 	pcmdpriv->cmd_issued_cnt = 0;
 	pcmdpriv->cmd_done_cnt = 0;
 	pcmdpriv->rsp_cnt = 0;
 
-exit:
+	pcmdpriv->wq = alloc_workqueue("rtl8723au_cmd", 0, 1);
+	if (!pcmdpriv->wq)
+		res = _FAIL;
 
 	return res;
 }
 
 /* forward definition */
 
-static void c2h_wk_callback(struct work_struct *work);
-int _rtw_init_evt_priv23a(struct evt_priv *pevtpriv)
-{
-	int res = _SUCCESS;
-
-	/* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */
-	atomic_set(&pevtpriv->event_seq, 0);
-	pevtpriv->evt_done_cnt = 0;
-
-	INIT_WORK(&pevtpriv->c2h_wk, c2h_wk_callback);
-	pevtpriv->c2h_wk_alive = false;
-	pevtpriv->c2h_queue = rtw_cbuf_alloc23a(C2H_QUEUE_MAX_LEN + 1);
-
-	return res;
-}
-
-void _rtw_free_evt_priv23a (struct evt_priv *pevtpriv)
-{
-	RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
-		 ("+_rtw_free_evt_priv23a\n"));
-	cancel_work_sync(&pevtpriv->c2h_wk);
-	while(pevtpriv->c2h_wk_alive)
-		msleep(10);
-
-	while (!rtw_cbuf_empty23a(pevtpriv->c2h_queue)) {
-		void *c2h;
-		if ((c2h = rtw_cbuf_pop23a(pevtpriv->c2h_queue)) != NULL &&
-		    c2h != (void *)pevtpriv) {
-			kfree(c2h);
-		}
-	}
-
-	RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
-		 ("-_rtw_free_evt_priv23a\n"));
-}
-
-void _rtw_free_cmd_priv23a(struct cmd_priv *pcmdpriv)
-{
-	if (pcmdpriv) {
-		kfree(pcmdpriv->cmd_allocated_buf);
-		kfree(pcmdpriv->rsp_allocated_buf);
-	}
-}
-
-/*
-Calling Context:
-rtw_enqueue_cmd23a can only be called between kernel thread,
-since only spin_lock is used.
-
-ISR/Call-Back functions can't call this sub-function.
-*/
-
-int _rtw_enqueue_cmd23a(struct rtw_queue *queue, struct cmd_obj *obj)
-{
-	unsigned long irqL;
-
-	if (obj == NULL)
-		goto exit;
-
-	spin_lock_irqsave(&queue->lock, irqL);
-
-	list_add_tail(&obj->list, &queue->queue);
-
-	spin_unlock_irqrestore(&queue->lock, irqL);
-
-exit:
-
-	return _SUCCESS;
-}
+static void rtw_irq_work(struct work_struct *work);
 
 u32 rtw_init_evt_priv23a(struct evt_priv *pevtpriv)
 {
-	int res;
+	pevtpriv->wq = alloc_workqueue("rtl8723au_evt", 0, 1);
 
-	res = _rtw_init_evt_priv23a(pevtpriv);
+	INIT_WORK(&pevtpriv->irq_wk, rtw_irq_work);
 
-	return res;
+	return _SUCCESS;
 }
 
 void rtw_free_evt_priv23a(struct evt_priv *pevtpriv)
 {
-	RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
-		 ("rtw_free_evt_priv23a\n"));
-	_rtw_free_evt_priv23a(pevtpriv);
-}
-
-void rtw_free_cmd_priv23a(struct cmd_priv *pcmdpriv)
-{
-	RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
-		 ("rtw_free_cmd_priv23a\n"));
-	_rtw_free_cmd_priv23a(pcmdpriv);
+	cancel_work_sync(&pevtpriv->irq_wk);
 }
 
 static int rtw_cmd_filter(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
@@ -329,21 +222,21 @@ static int rtw_cmd_filter(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
 	if (cmd_obj->cmdcode == GEN_CMD_CODE(_SetChannelPlan))
 		bAllow = true;
 
-	if ((pcmdpriv->padapter->hw_init_completed == false &&
-	     bAllow == false) || pcmdpriv->cmdthd_running == false)
+	if (pcmdpriv->padapter->hw_init_completed == false && bAllow == false)
 		return _FAIL;
 	return _SUCCESS;
 }
 
-u32 rtw_enqueue_cmd23a(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
+static void rtw_cmd_work(struct work_struct *work);
+
+int rtw_enqueue_cmd23a(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
 {
 	int res = _FAIL;
-	struct rtw_adapter *padapter = pcmdpriv->padapter;
 
 	if (!cmd_obj)
 		goto exit;
 
-	cmd_obj->padapter = padapter;
+	cmd_obj->padapter = pcmdpriv->padapter;
 
 	res = rtw_cmd_filter(pcmdpriv, cmd_obj);
 	if (res == _FAIL) {
@@ -351,32 +244,18 @@ u32 rtw_enqueue_cmd23a(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
 		goto exit;
 	}
 
-	res = _rtw_enqueue_cmd23a(&pcmdpriv->cmd_queue, cmd_obj);
+	INIT_WORK(&cmd_obj->work, rtw_cmd_work);
 
-	if (res == _SUCCESS)
-		up(&pcmdpriv->cmd_queue_sema);
+	res = queue_work(pcmdpriv->wq, &cmd_obj->work);
 
+	if (!res) {
+		printk(KERN_ERR "%s: Call to queue_work() failed\n", __func__);
+		res = _FAIL;
+	} else
+		res = _SUCCESS;
 exit:
-	return res;
-}
 
-static struct cmd_obj *rtw_dequeue_cmd(struct cmd_priv *pcmdpriv)
-{
-	struct cmd_obj *obj;
-	struct rtw_queue *queue = &pcmdpriv->cmd_queue;
-	unsigned long irqL;
-
-	spin_lock_irqsave(&queue->lock, irqL);
-	if (list_empty(&queue->queue))
-		obj = NULL;
-	else {
-		obj = container_of((&queue->queue)->next, struct cmd_obj, list);
-		list_del_init(&obj->list);
-	}
-
-	spin_unlock_irqrestore(&queue->lock, irqL);
-
-	return obj;
+	return res;
 }
 
 void rtw_cmd_clr_isr23a(struct	cmd_priv *pcmdpriv)
@@ -403,135 +282,72 @@ void rtw_free_cmd_obj23a(struct cmd_obj *pcmd)
 	kfree(pcmd);
 }
 
-int rtw_cmd_thread23a(void *context)
+static void rtw_cmd_work(struct work_struct *work)
 {
-	u8 ret;
-	struct cmd_obj *pcmd;
-	u8 *pcmdbuf, *prspbuf;
-	u8 (*cmd_hdl)(struct rtw_adapter *padapter, u8* pbuf);
+	int (*cmd_hdl)(struct rtw_adapter *padapter, const u8 *pbuf);
 	void (*pcmd_callback)(struct rtw_adapter *dev, struct cmd_obj *pcmd);
-	struct rtw_adapter *padapter = (struct rtw_adapter *)context;
-	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	struct cmd_priv *pcmdpriv;
+	struct cmd_obj *pcmd = container_of(work, struct cmd_obj, work);
 
-	allow_signal(SIGTERM);
+	pcmdpriv = &pcmd->padapter->cmdpriv;
 
-	pcmdbuf = pcmdpriv->cmd_buf;
-	prspbuf = pcmdpriv->rsp_buf;
+	if (rtw_cmd_filter(pcmdpriv, pcmd) == _FAIL) {
+		pcmd->res = H2C_DROPPED;
+		goto post_process;
+	}
 
-	pcmdpriv->cmdthd_running = true;
-	up(&pcmdpriv->terminate_cmdthread_sema);
+	pcmdpriv->cmd_issued_cnt++;
 
-	RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
-		 ("start r871x rtw_cmd_thread23a !!!!\n"));
-
-	while(1) {
-		if (down_interruptible(&pcmdpriv->cmd_queue_sema))
-			break;
-_next:
-		if ((padapter->bDriverStopped == true) ||
-		    (padapter->bSurpriseRemoved == true)) {
-			DBG_8723A("%s: DriverStopped(%d) SurpriseRemoved(%d) "
-				  "break at line %d\n",	__func__,
-				  padapter->bDriverStopped,
-				  padapter->bSurpriseRemoved, __LINE__);
-			break;
-		}
+	pcmd->cmdsz = ALIGN(pcmd->cmdsz, 4);
 
-		if (!(pcmd = rtw_dequeue_cmd(pcmdpriv)))
-			continue;
+	if (pcmd->cmdcode < (sizeof(wlancmds)/sizeof(struct cmd_hdl))) {
+		cmd_hdl = wlancmds[pcmd->cmdcode].h2cfuns;
 
-		if (rtw_cmd_filter(pcmdpriv, pcmd) == _FAIL) {
+		if (cmd_hdl)
+			pcmd->res = cmd_hdl(pcmd->padapter, pcmd->parmbuf);
+		else
 			pcmd->res = H2C_DROPPED;
-			goto post_process;
-		}
-
-		pcmdpriv->cmd_issued_cnt++;
-
-		pcmd->cmdsz = ALIGN(pcmd->cmdsz, 4);
-
-		memcpy(pcmdbuf, pcmd->parmbuf, pcmd->cmdsz);
-
-		if (pcmd->cmdcode < (sizeof(wlancmds)/sizeof(struct cmd_hdl))) {
-			cmd_hdl = wlancmds[pcmd->cmdcode].h2cfuns;
-
-			if (cmd_hdl) {
-				ret = cmd_hdl(pcmd->padapter, pcmdbuf);
-				pcmd->res = ret;
-			}
-
-			pcmdpriv->cmd_seq++;
-		} else
-			pcmd->res = H2C_PARAMETERS_ERROR;
-
-		cmd_hdl = NULL;
+	} else
+		pcmd->res = H2C_PARAMETERS_ERROR;
 
 post_process:
-		/* call callback function for post-processed */
-		if (pcmd->cmdcode < (sizeof(rtw_cmd_callback) /
-				     sizeof(struct _cmd_callback))) {
-			pcmd_callback =
-				rtw_cmd_callback[pcmd->cmdcode].callback;
-			if (!pcmd_callback) {
-				RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
-					 ("mlme_cmd_hdl(): pcmd_callback = "
-					  "0x%p, cmdcode = 0x%x\n",
-					  pcmd_callback, pcmd->cmdcode));
-				rtw_free_cmd_obj23a(pcmd);
-			} else {
-				/* todo: !!! fill rsp_buf to pcmd->rsp
-				   if (pcmd->rsp!= NULL) */
-				/* need conider that free cmd_obj in
-				   rtw_cmd_callback */
-				pcmd_callback(pcmd->padapter, pcmd);
-			}
-		} else {
-			RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
-				 ("%s: cmdcode = 0x%x callback not defined!\n",
-				  __func__, pcmd->cmdcode));
+	/* call callback function for post-processed */
+	if (pcmd->cmdcode < (sizeof(rtw_cmd_callback) /
+			     sizeof(struct _cmd_callback))) {
+		pcmd_callback =	rtw_cmd_callback[pcmd->cmdcode].callback;
+		if (!pcmd_callback) {
+			RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
+				 ("mlme_cmd_hdl(): pcmd_callback = 0x%p, "
+				  "cmdcode = 0x%x\n",
+				  pcmd_callback, pcmd->cmdcode));
 			rtw_free_cmd_obj23a(pcmd);
+		} else {
+			/* need conider that free cmd_obj in
+			   rtw_cmd_callback */
+			pcmd_callback(pcmd->padapter, pcmd);
 		}
-
-		if (signal_pending (current))
-			flush_signals(current);
-
-		goto _next;
-
-	}
-	pcmdpriv->cmdthd_running = false;
-
-	/*  free all cmd_obj resources */
-	do {
-		pcmd = rtw_dequeue_cmd(pcmdpriv);
-		if (!pcmd)
-			break;
-
+	} else {
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
+			 ("%s: cmdcode = 0x%x callback not defined!\n",
+			  __func__, pcmd->cmdcode));
 		rtw_free_cmd_obj23a(pcmd);
-	} while(1);
-
-	up(&pcmdpriv->terminate_cmdthread_sema);
-
-	complete_and_exit(NULL, 0);
+	}
 }
 
-u8 rtw_sitesurvey_cmd23a(struct rtw_adapter *padapter,
-			 struct cfg80211_ssid *ssid, int ssid_num,
-			 struct rtw_ieee80211_channel *ch, int ch_num)
+
+int rtw_sitesurvey_cmd23a(struct rtw_adapter *padapter,
+			  struct cfg80211_ssid *ssid, int ssid_num,
+			  struct rtw_ieee80211_channel *ch, int ch_num)
 {
-	u8 res = _FAIL;
+	int res = _FAIL;
 	struct cmd_obj *ph2c;
 	struct sitesurvey_parm *psurveyPara;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
+	if (check_fwstate(pmlmepriv, _FW_LINKED))
 		rtw_lps_ctrl_wk_cmd23a(padapter, LPS_CTRL_SCAN, 1);
 
-#ifdef CONFIG_8723AU_P2P
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
-		p2p_ps_wk_cmd23a(padapter, P2P_PS_SCAN, 1);
-	}
-#endif /* CONFIG_8723AU_P2P */
-
 	ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (!ph2c)
 		return _FAIL;
@@ -542,7 +358,7 @@ u8 rtw_sitesurvey_cmd23a(struct rtw_adapter *padapter,
 		return _FAIL;
 	}
 
-	rtw_free_network_queue23a(padapter, false);
+	rtw_free_network_queue23a(padapter);
 
 	RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
 		 ("%s: flush network queue\n", __func__));
@@ -561,11 +377,6 @@ u8 rtw_sitesurvey_cmd23a(struct rtw_adapter *padapter,
 				memcpy(&psurveyPara->ssid[i], &ssid[i],
 				       sizeof(struct cfg80211_ssid));
 				psurveyPara->ssid_num++;
-				if (0)
-				DBG_8723A(FUNC_ADPT_FMT" ssid:(%s, %d)\n",
-					  FUNC_ADPT_ARG(padapter),
-					  psurveyPara->ssid[i].ssid,
-					  psurveyPara->ssid[i].ssid_len);
 			}
 		}
 	}
@@ -579,10 +390,6 @@ u8 rtw_sitesurvey_cmd23a(struct rtw_adapter *padapter,
 				memcpy(&psurveyPara->ch[i], &ch[i],
 				       sizeof(struct rtw_ieee80211_channel));
 				psurveyPara->ch_num++;
-				if (0)
-				DBG_8723A(FUNC_ADPT_FMT" ch:%u\n",
-					  FUNC_ADPT_ARG(padapter),
-					  psurveyPara->ch[i].hw_value);
 			}
 		}
 	}
@@ -611,7 +418,7 @@ void rtw_getbbrfreg_cmdrsp_callback23a(struct rtw_adapter *padapter,
 	kfree(pcmd);
 }
 
-u8 rtw_createbss_cmd23a(struct rtw_adapter  *padapter)
+int rtw_createbss_cmd23a(struct rtw_adapter  *padapter)
 {
 	struct cmd_obj *pcmd;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
@@ -639,10 +446,9 @@ u8 rtw_createbss_cmd23a(struct rtw_adapter  *padapter)
 		goto exit;
 	}
 
-	INIT_LIST_HEAD(&pcmd->list);
 	pcmd->cmdcode = _CreateBss_CMD_;
 	pcmd->parmbuf = (unsigned char *)pdev_network;
-	pcmd->cmdsz = get_wlan_bssid_ex_sz((struct wlan_bssid_ex*)pdev_network);
+	pcmd->cmdsz = get_wlan_bssid_ex_sz(pdev_network);
 	pcmd->rsp = NULL;
 	pcmd->rspsz = 0;
 
@@ -655,24 +461,23 @@ exit:
 	return res;
 }
 
-u8 rtw_joinbss_cmd23a(struct rtw_adapter *padapter,
-		      struct wlan_network * pnetwork)
+int rtw_joinbss_cmd23a(struct rtw_adapter *padapter,
+		       struct wlan_network *pnetwork)
 {
-	u8 *auth, res = _SUCCESS;
-	uint t_len = 0;
+	u8 *auth;
+	int res = _SUCCESS;
 	struct wlan_bssid_ex *psecnetwork;
 	struct cmd_obj *pcmd;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct qos_priv *pqospriv = &pmlmepriv->qospriv;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 	struct registry_priv *pregistrypriv = &padapter->registrypriv;
 	struct ht_priv *phtpriv = &pmlmepriv->htpriv;
-	enum ndis_802_11_net_infra ndis_network_mode;
+	enum nl80211_iftype ifmode;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
-	ndis_network_mode = pnetwork->network.InfrastructureMode;
+	ifmode = pnetwork->network.ifmode;
 
 	rtw_led_control(padapter, LED_CTL_START_TO_LINK);
 
@@ -693,26 +498,23 @@ u8 rtw_joinbss_cmd23a(struct rtw_adapter *padapter,
 			  "fail!!!\n"));
 		goto exit;
 	}
-	/* for IEs is fix buf size */
-	t_len = sizeof(struct wlan_bssid_ex);
 
 	/* for hidden ap to set fw_state here */
 	if (!check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_ADHOC_STATE)) {
-		switch (ndis_network_mode) {
-		case Ndis802_11IBSS:
+		switch (ifmode) {
+		case NL80211_IFTYPE_ADHOC:
 			set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
 			break;
-		case Ndis802_11Infrastructure:
+		case NL80211_IFTYPE_P2P_CLIENT:
+		case NL80211_IFTYPE_STATION:
 			set_fwstate(pmlmepriv, WIFI_STATION_STATE);
 			break;
-		case Ndis802_11APMode:
-		case Ndis802_11AutoUnknown:
-		case Ndis802_11InfrastructureMax:
+		default:
 			break;
 		}
 	}
 
-	psecnetwork = (struct wlan_bssid_ex *)&psecuritypriv->sec_bss;
+	psecnetwork = &psecuritypriv->sec_bss;
 	if (!psecnetwork) {
 		if (pcmd)
 			kfree(pcmd);
@@ -725,7 +527,7 @@ u8 rtw_joinbss_cmd23a(struct rtw_adapter *padapter,
 		goto exit;
 	}
 
-	memset(psecnetwork, 0, t_len);
+	memset(psecnetwork, 0, sizeof(struct wlan_bssid_ex));
 
 	memcpy(psecnetwork, &pnetwork->network,
 	       get_wlan_bssid_ex_sz(&pnetwork->network));
@@ -758,7 +560,7 @@ u8 rtw_joinbss_cmd23a(struct rtw_adapter *padapter,
 				       &psecnetwork->IEs[0],
 				       pnetwork->network.IELength);
 
-	pqospriv->qos_option = 0;
+	pmlmepriv->qos_option = 0;
 
 	if (pregistrypriv->wmm_enable) {
 		u32 tmp_len;
@@ -772,22 +574,23 @@ u8 rtw_joinbss_cmd23a(struct rtw_adapter *padapter,
 		if (psecnetwork->IELength != tmp_len) {
 			psecnetwork->IELength = tmp_len;
 			/* There is WMM IE in this corresp. beacon */
-			pqospriv->qos_option = 1;
+			pmlmepriv->qos_option = 1;
 		} else {
 			/* There is no WMM IE in this corresp. beacon */
-			pqospriv->qos_option = 0;
+			pmlmepriv->qos_option = 0;
 		}
 	}
 
 	phtpriv->ht_option = false;
 	if (pregistrypriv->ht_enable) {
+		u32 algo = padapter->securitypriv.dot11PrivacyAlgrthm;
 		/*	Added by Albert 2010/06/23 */
 		/*	For the WEP mode, we will use the bg mode to do
 			the connection to avoid some IOT issue. */
 		/*	Especially for Realtek 8192u SoftAP. */
-		if ((padapter->securitypriv.dot11PrivacyAlgrthm != _WEP40_) &&
-		    (padapter->securitypriv.dot11PrivacyAlgrthm != _WEP104_) &&
-		    (padapter->securitypriv.dot11PrivacyAlgrthm != _TKIP_)) {
+		if (algo != WLAN_CIPHER_SUITE_WEP40 &&
+		    algo != WLAN_CIPHER_SUITE_WEP104 &&
+		    algo != WLAN_CIPHER_SUITE_TKIP) {
 			/* rtw_restructure_ht_ie23a */
 			rtw_restructure_ht_ie23a(padapter,
 						 &pnetwork->network.IEs[0],
@@ -813,7 +616,6 @@ u8 rtw_joinbss_cmd23a(struct rtw_adapter *padapter,
 	/* get cmdsz before endian conversion */
 	pcmd->cmdsz = get_wlan_bssid_ex_sz(psecnetwork);
 
-	INIT_LIST_HEAD(&pcmd->list);
 	pcmd->cmdcode = _JoinBss_CMD_;/* GEN_CMD_CODE(_JoinBss) */
 	pcmd->parmbuf = (unsigned char *)psecnetwork;
 	pcmd->rsp = NULL;
@@ -825,13 +627,13 @@ exit:
 	return res;
 }
 
-u8 rtw_disassoc_cmd23a(struct rtw_adapter*padapter, u32 deauth_timeout_ms,
-		       bool enqueue)
+int rtw_disassoc_cmd23a(struct rtw_adapter*padapter, u32 deauth_timeout_ms,
+			bool enqueue)
 {
 	struct cmd_obj *cmdobj = NULL;
 	struct disconnect_parm *param = NULL;
 	struct cmd_priv *cmdpriv = &padapter->cmdpriv;
-	u8 res = _SUCCESS;
+	int res = _SUCCESS;
 
 	RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_,
 		 ("+rtw_disassoc_cmd23a\n"));
@@ -866,13 +668,13 @@ exit:
 	return res;
 }
 
-u8 rtw_setopmode_cmd23a(struct rtw_adapter *padapter,
-			enum ndis_802_11_net_infra networktype)
+int rtw_setopmode_cmd23a(struct rtw_adapter *padapter,
+			 enum nl80211_iftype ifmode)
 {
 	struct	cmd_obj *ph2c;
 	struct	setopmode_parm *psetop;
 	struct	cmd_priv   *pcmdpriv = &padapter->cmdpriv;
-	u8 res = _SUCCESS;
+	int res = _SUCCESS;
 
 	ph2c = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
 	if (!ph2c) {
@@ -888,14 +690,14 @@ u8 rtw_setopmode_cmd23a(struct rtw_adapter *padapter,
 	}
 
 	init_h2fwcmd_w_parm_no_rsp(ph2c, psetop, _SetOpMode_CMD_);
-	psetop->mode = (u8)networktype;
+	psetop->mode = ifmode;
 
 	res = rtw_enqueue_cmd23a(pcmdpriv, ph2c);
 exit:
 	return res;
 }
 
-u8 rtw_setstakey_cmd23a(struct rtw_adapter *padapter, u8 *psta, u8 unicast_key)
+int rtw_setstakey_cmd23a(struct rtw_adapter *padapter, u8 *psta, u8 unicast_key)
 {
 	struct cmd_obj *ph2c;
 	struct set_stakey_parm *psetstakey_para;
@@ -904,7 +706,7 @@ u8 rtw_setstakey_cmd23a(struct rtw_adapter *padapter, u8 *psta, u8 unicast_key)
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 	struct sta_info *sta = (struct sta_info*)psta;
-	u8 res = _SUCCESS;
+	int res = _SUCCESS;
 
 	ph2c = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
 	if (!ph2c) {
@@ -950,7 +752,7 @@ u8 rtw_setstakey_cmd23a(struct rtw_adapter *padapter, u8 *psta, u8 unicast_key)
         }
 
 	/* jeff: set this becasue at least sw key is ready */
-	padapter->securitypriv.busetkipkey = true;
+	padapter->securitypriv.busetkipkey = 1;
 
 	res = rtw_enqueue_cmd23a(pcmdpriv, ph2c);
 
@@ -959,15 +761,15 @@ exit:
 	return res;
 }
 
-u8 rtw_clearstakey_cmd23a(struct rtw_adapter *padapter, u8 *psta, u8 entry,
-			  u8 enqueue)
+int rtw_clearstakey_cmd23a(struct rtw_adapter *padapter, u8 *psta, u8 entry,
+			   u8 enqueue)
 {
 	struct cmd_obj *ph2c;
 	struct set_stakey_parm *psetstakey_para;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 	struct set_stakey_rsp *psetstakey_rsp = NULL;
 	struct sta_info *sta = (struct sta_info *)psta;
-	u8 res = _SUCCESS;
+	int res = _SUCCESS;
 
 	if (!enqueue) {
 		clear_cam_entry23a(padapter, entry);
@@ -1002,7 +804,7 @@ u8 rtw_clearstakey_cmd23a(struct rtw_adapter *padapter, u8 *psta, u8 entry,
 
 		ether_addr_copy(psetstakey_para->addr, sta->hwaddr);
 
-		psetstakey_para->algorithm = _NO_PRIVACY_;
+		psetstakey_para->algorithm = 0;
 
 		psetstakey_para->id = entry;
 
@@ -1012,12 +814,17 @@ exit:
 	return res;
 }
 
-u8 rtw_addbareq_cmd23a(struct rtw_adapter*padapter, u8 tid, u8 *addr)
+int rtw_addbareq_cmd23a(struct rtw_adapter*padapter, u8 tid, u8 *addr)
 {
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 	struct cmd_obj *ph2c;
 	struct addBaReq_parm *paddbareq_parm;
-	u8 res = _SUCCESS;
+	int res = _SUCCESS;
+
+	if (tid >= MAXTID) {
+		res = _FAIL;
+		goto exit;
+	}
 
 	ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (!ph2c) {
@@ -1043,12 +850,12 @@ exit:
 	return res;
 }
 
-u8 rtw_dynamic_chk_wk_cmd23a(struct rtw_adapter*padapter)
+int rtw_dynamic_chk_wk_cmd23a(struct rtw_adapter*padapter)
 {
 	struct cmd_obj *ph2c;
 	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
-	u8 res = _SUCCESS;
+	int res = _SUCCESS;
 
 	ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (!ph2c) {
@@ -1080,17 +887,16 @@ exit:
  * This is only ever called from on_action_spct23a_ch_switch () which isn't
  * called from anywhere itself
  */
-u8 rtw_set_ch_cmd23a(struct rtw_adapter*padapter, u8 ch, u8 bw, u8 ch_offset,
-		     u8 enqueue)
+int rtw_set_ch_cmd23a(struct rtw_adapter*padapter, u8 ch, u8 bw, u8 ch_offset,
+		      u8 enqueue)
 {
 	struct cmd_obj *pcmdobj;
 	struct set_ch_parm *set_ch_parm;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
+	int res = _SUCCESS;
 
-	u8 res = _SUCCESS;
-
-	DBG_8723A(FUNC_NDEV_FMT" ch:%u, bw:%u, ch_offset:%u\n",
-		  FUNC_NDEV_ARG(padapter->pnetdev), ch, bw, ch_offset);
+	DBG_8723A("%s(%s): ch:%u, bw:%u, ch_offset:%u\n", __func__,
+		  padapter->pnetdev->name, ch, bw, ch_offset);
 
 	/* check input parameter */
 
@@ -1128,8 +934,7 @@ u8 rtw_set_ch_cmd23a(struct rtw_adapter*padapter, u8 ch, u8 bw, u8 ch_offset,
 	/* do something based on res... */
 exit:
 
-	DBG_8723A(FUNC_NDEV_FMT" res:%u\n", FUNC_NDEV_ARG(padapter->pnetdev),
-		  res);
+	DBG_8723A("%s(%s): res:%u\n", __func__, padapter->pnetdev->name, res);
 
 	return res;
 }
@@ -1141,25 +946,19 @@ static void traffic_status_watchdog(struct rtw_adapter *padapter)
 	u8 bHigherBusyTraffic = false, bHigherBusyRxTraffic = false;
 	u8 bHigherBusyTxTraffic = false;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-#ifndef CONFIG_8723AU_BT_COEXIST
 	int BusyThreshold = 100;
-#endif
 	/*  */
 	/*  Determine if our traffic is busy now */
 	/*  */
 	if (check_fwstate(pmlmepriv, _FW_LINKED)) {
-#ifdef CONFIG_8723AU_BT_COEXIST
-		if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > 50 ||
-		    pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > 50)
-#else /*  !CONFIG_8723AU_BT_COEXIST */
+		if (rtl8723a_BT_coexist(padapter))
+			BusyThreshold = 50;
+		else if (pmlmepriv->LinkDetectInfo.bBusyTraffic)
+			BusyThreshold = 75;
 		/*  if we raise bBusyTraffic in last watchdog, using
 		    lower threshold. */
-		if (pmlmepriv->LinkDetectInfo.bBusyTraffic)
-			BusyThreshold = 75;
 		if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod > BusyThreshold ||
-		    pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > BusyThreshold)
-#endif /*  !CONFIG_8723AU_BT_COEXIST */
-		{
+		    pmlmepriv->LinkDetectInfo.NumTxOkInPeriod > BusyThreshold) {
 			bBusyTraffic = true;
 
 			if (pmlmepriv->LinkDetectInfo.NumRxOkInPeriod >
@@ -1181,23 +980,21 @@ static void traffic_status_watchdog(struct rtw_adapter *padapter)
 				bHigherBusyTxTraffic = true;
 		}
 
-#ifdef CONFIG_8723AU_BT_COEXIST
-		if (BT_1Ant(padapter) == false)
-#endif
-		{
+		if (!rtl8723a_BT_coexist(padapter) ||
+		    !rtl8723a_BT_using_antenna_1(padapter)) {
 		/*  check traffic for  powersaving. */
-		if (((pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod +
-		      pmlmepriv->LinkDetectInfo.NumTxOkInPeriod) > 8) ||
-		    (pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod > 2))
-			bEnterPS = false;
-		else
-			bEnterPS = true;
+			if (((pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod +
+			      pmlmepriv->LinkDetectInfo.NumTxOkInPeriod) > 8) ||
+			    pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod >2)
+				bEnterPS = false;
+			else
+				bEnterPS = true;
 
-		/*  LeisurePS only work in infra mode. */
-		if (bEnterPS)
-			LPS_Enter23a(padapter);
-		else
-			LPS_Leave23a(padapter);
+			/*  LeisurePS only work in infra mode. */
+			if (bEnterPS)
+				LPS_Enter23a(padapter);
+			else
+				LPS_Leave23a(padapter);
 		}
 	} else
 		LPS_Leave23a(padapter);
@@ -1213,7 +1010,7 @@ static void traffic_status_watchdog(struct rtw_adapter *padapter)
 	pmlmepriv->LinkDetectInfo.bHigherBusyTxTraffic = bHigherBusyTxTraffic;
 }
 
-void dynamic_chk_wk_hdl(struct rtw_adapter *padapter, u8 *pbuf, int sz)
+static void dynamic_chk_wk_hdl(struct rtw_adapter *padapter, u8 *pbuf, int sz)
 {
 	struct mlme_priv *pmlmepriv;
 
@@ -1221,43 +1018,38 @@ void dynamic_chk_wk_hdl(struct rtw_adapter *padapter, u8 *pbuf, int sz)
 	pmlmepriv = &padapter->mlmepriv;
 
 #ifdef CONFIG_8723AU_AP_MODE
-	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true)
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
 		expire_timeout_chk23a(padapter);
 #endif
 
-	rtw_hal_sreset_xmit_status_check23a(padapter);
+	rtl8723a_sreset_xmit_status_check(padapter);
 
 	linked_status_chk23a(padapter);
 	traffic_status_watchdog(padapter);
 
-	rtw_hal_dm_watchdog23a(padapter);
+	rtl8723a_HalDmWatchDog(padapter);
 
-#ifdef CONFIG_8723AU_BT_COEXIST
 	/*  */
 	/*  BT-Coexist */
 	/*  */
-	BT_CoexistMechanism(padapter);
-#endif
+	rtl8723a_BT_do_coexist(padapter);
 }
 
-void lps_ctrl_wk_hdl(struct rtw_adapter *padapter, u8 lps_ctrl_type)
+static void lps_ctrl_wk_hdl(struct rtw_adapter *padapter, u8 lps_ctrl_type)
 {
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	u8 mstatus;
 
-	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) ||
-	    (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true))
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) ||
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_STATE))
 		return;
 
 	switch (lps_ctrl_type)
 	{
 		case LPS_CTRL_SCAN:
-#ifdef CONFIG_8723AU_BT_COEXIST
-			BT_WifiScanNotify(padapter, true);
-			if (BT_1Ant(padapter) == false)
-#endif
-			{
+			rtl8723a_BT_wifiscan_notify(padapter, true);
+			if (!rtl8723a_BT_using_antenna_1(padapter)) {
 				if (check_fwstate(pmlmepriv, _FW_LINKED))
 					LPS_Leave23a(padapter);
 			}
@@ -1269,42 +1061,26 @@ void lps_ctrl_wk_hdl(struct rtw_adapter *padapter, u8 lps_ctrl_type)
 			mstatus = 1;/* connect */
 			/*  Reset LPS Setting */
 			padapter->pwrctrlpriv.LpsIdleCount = 0;
-			rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_JOINBSSRPT,
-					     (u8 *)&mstatus);
-#ifdef CONFIG_8723AU_BT_COEXIST
-			BT_WifiMediaStatusNotify(padapter, mstatus);
-#endif
+			rtl8723a_set_FwJoinBssReport_cmd(padapter, 1);
+			rtl8723a_BT_mediastatus_notify(padapter, mstatus);
 			break;
 		case LPS_CTRL_DISCONNECT:
 			mstatus = 0;/* disconnect */
-#ifdef CONFIG_8723AU_BT_COEXIST
-			BT_WifiMediaStatusNotify(padapter, mstatus);
-			if (BT_1Ant(padapter) == false)
-#endif
-			{
+			rtl8723a_BT_mediastatus_notify(padapter, mstatus);
+			if (!rtl8723a_BT_using_antenna_1(padapter))
 				LPS_Leave23a(padapter);
-			}
-			rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_JOINBSSRPT,
-					     (u8 *)&mstatus);
+			rtl8723a_set_FwJoinBssReport_cmd(padapter, 0);
 			break;
 		case LPS_CTRL_SPECIAL_PACKET:
 			pwrpriv->DelayLPSLastTimeStamp = jiffies;
-#ifdef CONFIG_8723AU_BT_COEXIST
-			BT_SpecialPacketNotify(padapter);
-			if (BT_1Ant(padapter) == false)
-#endif
-			{
+			rtl8723a_BT_specialpacket_notify(padapter);
+			if (!rtl8723a_BT_using_antenna_1(padapter))
 				LPS_Leave23a(padapter);
-			}
 			break;
 		case LPS_CTRL_LEAVE:
-#ifdef CONFIG_8723AU_BT_COEXIST
-			BT_LpsLeave(padapter);
-			if (BT_1Ant(padapter) == false)
-#endif
-			{
+			rtl8723a_BT_lps_leave(padapter);
+			if (!rtl8723a_BT_using_antenna_1(padapter))
 				LPS_Leave23a(padapter);
-			}
 			break;
 
 		default:
@@ -1312,13 +1088,13 @@ void lps_ctrl_wk_hdl(struct rtw_adapter *padapter, u8 lps_ctrl_type)
 	}
 }
 
-u8 rtw_lps_ctrl_wk_cmd23a(struct rtw_adapter *padapter,
-			  u8 lps_ctrl_type, u8 enqueue)
+int rtw_lps_ctrl_wk_cmd23a(struct rtw_adapter *padapter,
+			   u8 lps_ctrl_type, u8 enqueue)
 {
 	struct cmd_obj *ph2c;
 	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
-	u8 res = _SUCCESS;
+	int res = _SUCCESS;
 
 	if (enqueue) {
 		ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
@@ -1355,55 +1131,12 @@ static void power_saving_wk_hdl(struct rtw_adapter *padapter, u8 *pbuf, int sz)
 	 rtw_ps_processor23a(padapter);
 }
 
-#ifdef CONFIG_8723AU_P2P
-u8 p2p_protocol_wk_cmd23a(struct rtw_adapter*padapter, int intCmdType)
-{
-	struct cmd_obj *ph2c;
-	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
-	u8 res = _SUCCESS;
-
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
-	{
-		return res;
-	}
-
-	ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
-	if (!ph2c) {
-		res = _FAIL;
-		goto exit;
-	}
-
-	pdrvextra_cmd_parm = kzalloc(sizeof(struct drvextra_cmd_parm),
-				     GFP_ATOMIC);
-	if (!pdrvextra_cmd_parm) {
-		kfree(ph2c);
-		res = _FAIL;
-		goto exit;
-	}
-
-	pdrvextra_cmd_parm->ec_id = P2P_PROTO_WK_CID;
-	pdrvextra_cmd_parm->type_size = intCmdType; /* As the command tppe. */
-	pdrvextra_cmd_parm->pbuf = NULL;	    /* Must be NULL here */
-
-	init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm,
-				   GEN_CMD_CODE(_Set_Drv_Extra));
-
-	res = rtw_enqueue_cmd23a(pcmdpriv, ph2c);
-exit:
-
-	return res;
-}
-#endif /* CONFIG_8723AU_P2P */
-
-u8 rtw_ps_cmd23a(struct rtw_adapter*padapter)
+int rtw_ps_cmd23a(struct rtw_adapter*padapter)
 {
 	struct cmd_obj *ppscmd;
 	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
-
-	u8 res = _SUCCESS;
+	int res = _SUCCESS;
 
 	ppscmd = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (!ppscmd) {
@@ -1443,11 +1176,11 @@ static void rtw_chk_hi_queue_hdl(struct rtw_adapter *padapter)
 		return;
 
 	if (psta_bmc->sleepq_len == 0) {
-		u8 val = 0;
+		bool val;
 
-		rtw23a_hal_get_hwreg(padapter, HW_VAR_CHK_HI_QUEUE_EMPTY, &val);
+		val = rtl8723a_chk_hi_queue_empty(padapter);
 
-		while(val == false) {
+		while (val == false) {
 			msleep(100);
 
 			cnt++;
@@ -1455,26 +1188,25 @@ static void rtw_chk_hi_queue_hdl(struct rtw_adapter *padapter)
 			if (cnt>10)
 				break;
 
-			rtw23a_hal_get_hwreg(padapter,
-					     HW_VAR_CHK_HI_QUEUE_EMPTY, &val);
+			val = rtl8723a_chk_hi_queue_empty(padapter);
 		}
 
 		if (cnt <= 10) {
 			pstapriv->tim_bitmap &= ~BIT(0);
 			pstapriv->sta_dz_bitmap &= ~BIT(0);
 
-			update_beacon23a(padapter, _TIM_IE_, NULL, false);
+			update_beacon23a(padapter, WLAN_EID_TIM, NULL, false);
 		} else /* re check again */
 			rtw_chk_hi_queue_cmd23a(padapter);
 	}
 }
 
-u8 rtw_chk_hi_queue_cmd23a(struct rtw_adapter*padapter)
+int rtw_chk_hi_queue_cmd23a(struct rtw_adapter*padapter)
 {
 	struct cmd_obj *ph2c;
 	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
-	u8 res = _SUCCESS;
+	int res = _SUCCESS;
 
 	ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (!ph2c) {
@@ -1504,12 +1236,12 @@ exit:
 }
 #endif
 
-u8 rtw_c2h_wk_cmd23a(struct rtw_adapter *padapter, u8 *c2h_evt)
+int rtw_c2h_wk_cmd23a(struct rtw_adapter *padapter, u8 *c2h_evt)
 {
 	struct cmd_obj *ph2c;
 	struct drvextra_cmd_parm *pdrvextra_cmd_parm;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
-	u8 res = _SUCCESS;
+	int res = _SUCCESS;
 
 	ph2c = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (!ph2c) {
@@ -1539,10 +1271,9 @@ exit:
 	return res;
 }
 
-s32 c2h_evt_hdl(struct rtw_adapter *adapter, struct c2h_evt_hdr *c2h_evt,
-		c2h_id_filter filter)
+static int c2h_evt_hdl(struct rtw_adapter *adapter, struct c2h_evt_hdr *c2h_evt)
 {
-	s32 ret = _FAIL;
+	int ret = _FAIL;
 	u8 buf[16];
 
 	if (!c2h_evt) {
@@ -1550,73 +1281,57 @@ s32 c2h_evt_hdl(struct rtw_adapter *adapter, struct c2h_evt_hdr *c2h_evt,
 		if (c2h_evt_read23a(adapter, buf) == _SUCCESS) {
 			c2h_evt = (struct c2h_evt_hdr *)buf;
 
-			if (filter && filter(c2h_evt->id) == false)
-				goto exit;
-
-			ret = rtw_hal_c2h_handler23a(adapter, c2h_evt);
+			ret = c2h_handler_8723a(adapter, c2h_evt);
 		}
-	} else {
-
-		if (filter && filter(c2h_evt->id) == false)
-			goto exit;
+	} else
+		ret = c2h_handler_8723a(adapter, c2h_evt);
 
-		ret = rtw_hal_c2h_handler23a(adapter, c2h_evt);
-	}
-exit:
 	return ret;
 }
 
-static void c2h_wk_callback(struct work_struct *work)
+static void rtw_irq_work(struct work_struct *work)
 {
 	struct evt_priv *evtpriv;
 	struct rtw_adapter *adapter;
-	struct c2h_evt_hdr *c2h_evt;
-	c2h_id_filter ccx_id_filter;
 
-	evtpriv = container_of(work, struct evt_priv, c2h_wk);
+	evtpriv = container_of(work, struct evt_priv, irq_wk);
 	adapter = container_of(evtpriv, struct rtw_adapter, evtpriv);
-	ccx_id_filter = rtw_hal_c2h_id_filter_ccx23a(adapter);
-
-	evtpriv->c2h_wk_alive = true;
-
-	while (!rtw_cbuf_empty23a(evtpriv->c2h_queue)) {
-		c2h_evt = (struct c2h_evt_hdr *)
-			rtw_cbuf_pop23a(evtpriv->c2h_queue);
-		if (c2h_evt) {
-			/* This C2H event is read, clear it */
-			c2h_evt_clear23a(adapter);
-		} else if ((c2h_evt = (struct c2h_evt_hdr *)
-			    kmalloc(16, GFP_ATOMIC))) {
-			/* This C2H event is not read, read & clear now */
-			if (c2h_evt_read23a(adapter, (u8*)c2h_evt) != _SUCCESS)
-				continue;
-		}
 
-		/* Special pointer to trigger c2h_evt_clear23a only */
-		if ((void *)c2h_evt == (void *)evtpriv)
-			continue;
+	c2h_evt_clear23a(adapter);
+}
 
-		if (!c2h_evt_exist(c2h_evt)) {
-			kfree(c2h_evt);
-			continue;
-		}
+void rtw_evt_work(struct work_struct *work)
+{
+	struct evt_work *ework;
+	struct rtw_adapter *adapter;
 
-		if (ccx_id_filter(c2h_evt->id) == true) {
-			/* Handle CCX report here */
-			rtw_hal_c2h_handler23a(adapter, c2h_evt);
-			kfree(c2h_evt);
-		} else {
-			/* Enqueue into cmd_thread for others */
-			rtw_c2h_wk_cmd23a(adapter, (u8 *)c2h_evt);
-		}
+	ework = container_of(work, struct evt_work, work);
+	adapter = ework->adapter;
+
+	c2h_evt_clear23a(adapter);
+
+	if (!c2h_evt_exist(&ework->u.c2h_evt)) {
+		kfree(ework);
+		return;
 	}
 
-	evtpriv->c2h_wk_alive = false;
+	if (c2h_id_filter_ccx_8723a(ework->u.c2h_evt.id) == true) {
+		/* Handle CCX report here */
+		c2h_handler_8723a(adapter, &ework->u.c2h_evt);
+		kfree(ework);
+	} else {
+		/*
+		 * Enqueue into cmd_thread for others.
+		 * ework will be turned into a c2h_evt and freed once it
+		 * has been consumed.
+		 */
+		rtw_c2h_wk_cmd23a(adapter, (u8 *)&ework->u.c2h_evt);
+	}
 }
 
-u8 rtw_drvextra_cmd_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
+int rtw_drvextra_cmd_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
-	struct drvextra_cmd_parm *pdrvextra_cmd;
+	const struct drvextra_cmd_parm *pdrvextra_cmd;
 
 	if (!pbuf)
 		return H2C_PARAMETERS_ERROR;
@@ -1636,16 +1351,6 @@ u8 rtw_drvextra_cmd_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
 	case LPS_CTRL_WK_CID:
 		lps_ctrl_wk_hdl(padapter, (u8)pdrvextra_cmd->type_size);
 		break;
-#ifdef CONFIG_8723AU_P2P
-	case P2P_PS_WK_CID:
-		p2p_ps_wk_hdl23a(padapter, pdrvextra_cmd->type_size);
-		break;
-	case P2P_PROTO_WK_CID:
-		/*	Commented by Albert 2011/07/01 */
-		/*	I used the type_size as the type command */
-		p2p_protocol_wk_hdl23a(padapter, pdrvextra_cmd->type_size);
-		break;
-#endif /*  CONFIG_8723AU_P2P */
 #ifdef CONFIG_8723AU_AP_MODE
 	case CHECK_HIQ_WK_CID:
 		rtw_chk_hi_queue_hdl(padapter);
@@ -1653,7 +1358,7 @@ u8 rtw_drvextra_cmd_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
 #endif /* CONFIG_8723AU_AP_MODE */
 	case C2H_WK_CID:
 		c2h_evt_hdl(padapter,
-			    (struct c2h_evt_hdr *)pdrvextra_cmd->pbuf, NULL);
+			    (struct c2h_evt_hdr *)pdrvextra_cmd->pbuf);
 		break;
 
 	default:
@@ -1662,14 +1367,18 @@ u8 rtw_drvextra_cmd_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
 
 	if (pdrvextra_cmd->pbuf && (pdrvextra_cmd->type_size > 0)) {
 		kfree(pdrvextra_cmd->pbuf);
-		pdrvextra_cmd->pbuf = NULL;
+		/*
+		 * No need to set pdrvextra_cmd->pbuf = NULL as we were
+		 * operating on a copy of the original pcmd->parmbuf
+		 * created in rtw_cmd_work().
+		 */
 	}
 
 	return H2C_SUCCESS;
 }
 
 void rtw_survey_cmd_callback23a(struct rtw_adapter *padapter,
-			     struct cmd_obj *pcmd)
+				struct cmd_obj *pcmd)
 {
 	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
@@ -1748,14 +1457,13 @@ void rtw_createbss_cmd23a_callback(struct rtw_adapter *padapter,
 
 	del_timer_sync(&pmlmepriv->assoc_timer);
 
-	spin_lock_bh(&pmlmepriv->lock);
-
 	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
 		psta = rtw_get_stainfo23a(&padapter->stapriv,
 					  pnetwork->MacAddress);
 		if (!psta) {
 			psta = rtw_alloc_stainfo23a(&padapter->stapriv,
-						 pnetwork->MacAddress);
+						    pnetwork->MacAddress,
+						    GFP_KERNEL);
 			if (!psta) {
 				RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
 					 ("\nCan't alloc sta_info when "
@@ -1764,9 +1472,11 @@ void rtw_createbss_cmd23a_callback(struct rtw_adapter *padapter,
 			}
 		}
 
+		spin_lock_bh(&pmlmepriv->lock);
 		rtw_indicate_connect23a(padapter);
+		spin_unlock_bh(&pmlmepriv->lock);
 	} else {
-		pwlan = rtw_alloc_network(pmlmepriv);
+		pwlan = rtw_alloc_network(pmlmepriv, GFP_KERNEL);
 		spin_lock_bh(&pmlmepriv->scanned_queue.lock);
 		if (!pwlan) {
 			pwlan = rtw_get_oldest_wlan_network23a(&pmlmepriv->scanned_queue);
@@ -1797,7 +1507,7 @@ void rtw_createbss_cmd23a_callback(struct rtw_adapter *padapter,
 
 		/*  reset DSConfig */
 
-		_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
+		clr_fwstate(pmlmepriv, _FW_UNDER_LINKING);
 
 		spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
 		/*  we will set _FW_LINKED when there is one more sat to
@@ -1806,8 +1516,6 @@ void rtw_createbss_cmd23a_callback(struct rtw_adapter *padapter,
 
 createbss_cmd_fail:
 
-	spin_unlock_bh(&pmlmepriv->lock);
-
 	rtw_free_cmd_obj23a(pcmd);
 }
 
@@ -1858,8 +1566,8 @@ void rtw_setassocsta_cmdrsp_callback23a(struct rtw_adapter *padapter,
 
 	spin_lock_bh(&pmlmepriv->lock);
 
-	if ((check_fwstate(pmlmepriv, WIFI_MP_STATE) == true) &&
-	    (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == true))
+	if (check_fwstate(pmlmepriv, WIFI_MP_STATE) &&
+	    check_fwstate(pmlmepriv, _FW_UNDER_LINKING))
 		_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
 
 	set_fwstate(pmlmepriv, _FW_LINKED);
@@ -1868,9 +1576,3 @@ void rtw_setassocsta_cmdrsp_callback23a(struct rtw_adapter *padapter,
 exit:
 	rtw_free_cmd_obj23a(pcmd);
 }
-
-void rtw_getrttbl_cmd_cmdrsp_callback(struct rtw_adapter *padapter,
-				      struct cmd_obj *pcmd)
-{
-	rtw_free_cmd_obj23a(pcmd);
-}
diff --git a/drivers/staging/rtl8723au/core/rtw_efuse.c b/drivers/staging/rtl8723au/core/rtw_efuse.c
index 35b177fd051..cc063cbc49b 100644
--- a/drivers/staging/rtl8723au/core/rtw_efuse.c
+++ b/drivers/staging/rtl8723au/core/rtw_efuse.c
@@ -18,6 +18,8 @@
 #include <drv_types.h>
 
 #include <rtw_efuse.h>
+#include <rtl8723a_hal.h>
+#include <usb_ops_linux.h>
 
 /*------------------------Define local variable------------------------------*/
 
@@ -26,8 +28,11 @@
 #define EFUSE_CTRL			REG_EFUSE_CTRL		/*  E-Fuse Control. */
 /*  */
 
+#define VOLTAGE_V25		0x03
+#define LDOE25_SHIFT		28
+
 /*-----------------------------------------------------------------------------
- * Function:	Efuse_PowerSwitch23a
+ * Function:	Efuse_PowerSwitch
  *
  * Overview:	When we want to enable write operation, we should change to
  *				pwr on state. When we stop write, we should switch to 500k mode
@@ -44,13 +49,55 @@
  * 11/17/2008	MHC		Create Version 0.
  *
  *---------------------------------------------------------------------------*/
-void
-Efuse_PowerSwitch23a(
-	struct rtw_adapter *	pAdapter,
-	u8		bWrite,
-	u8		PwrState)
+static void Efuse_PowerSwitch(struct rtw_adapter *padapter,
+			      u8 bWrite, u8 PwrState)
 {
-	pAdapter->HalFunc.EfusePowerSwitch(pAdapter, bWrite, PwrState);
+	u8 tempval;
+	u16 tmpV16;
+
+	if (PwrState == true) {
+		rtl8723au_write8(padapter, REG_EFUSE_ACCESS, EFUSE_ACCESS_ON);
+
+		/*  1.2V Power: From VDDON with Power
+		    Cut(0x0000h[15]), defualt valid */
+		tmpV16 = rtl8723au_read16(padapter, REG_SYS_ISO_CTRL);
+		if (!(tmpV16 & PWC_EV12V)) {
+			tmpV16 |= PWC_EV12V;
+			rtl8723au_write16(padapter, REG_SYS_ISO_CTRL, tmpV16);
+		}
+		/*  Reset: 0x0000h[28], default valid */
+		tmpV16 = rtl8723au_read16(padapter, REG_SYS_FUNC_EN);
+		if (!(tmpV16 & FEN_ELDR)) {
+			tmpV16 |= FEN_ELDR;
+			rtl8723au_write16(padapter, REG_SYS_FUNC_EN, tmpV16);
+		}
+
+		/*  Clock: Gated(0x0008h[5]) 8M(0x0008h[1]) clock
+		    from ANA, default valid */
+		tmpV16 = rtl8723au_read16(padapter, REG_SYS_CLKR);
+		if ((!(tmpV16 & LOADER_CLK_EN)) || (!(tmpV16 & ANA8M))) {
+			tmpV16 |= (LOADER_CLK_EN | ANA8M);
+			rtl8723au_write16(padapter, REG_SYS_CLKR, tmpV16);
+		}
+
+		if (bWrite == true) {
+			/*  Enable LDO 2.5V before read/write action */
+			tempval = rtl8723au_read8(padapter, EFUSE_TEST + 3);
+			tempval &= 0x0F;
+			tempval |= (VOLTAGE_V25 << 4);
+			rtl8723au_write8(padapter, EFUSE_TEST + 3,
+					 tempval | 0x80);
+		}
+	} else {
+		rtl8723au_write8(padapter, REG_EFUSE_ACCESS, EFUSE_ACCESS_OFF);
+
+		if (bWrite == true) {
+			/*  Disable LDO 2.5V after read/write action */
+			tempval = rtl8723au_read8(padapter, EFUSE_TEST + 3);
+			rtl8723au_write8(padapter, EFUSE_TEST + 3,
+					 tempval & 0x7F);
+		}
+	}
 }
 
 /*-----------------------------------------------------------------------------
@@ -74,7 +121,10 @@ Efuse_GetCurrentSize23a(struct rtw_adapter *pAdapter, u8 efuseType)
 {
 	u16 ret = 0;
 
-	ret = pAdapter->HalFunc.EfuseGetCurrentSize(pAdapter, efuseType);
+	if (efuseType == EFUSE_WIFI)
+		ret = rtl8723a_EfuseGetCurrentSize_WiFi(pAdapter);
+	else
+		ret = rtl8723a_EfuseGetCurrentSize_BT(pAdapter);
 
 	return ret;
 }
@@ -110,21 +160,22 @@ ReadEFuseByte23a(struct rtw_adapter *Adapter, u16 _offset, u8 *pbuf)
 	u16	retry;
 
 	/* Write Address */
-	rtw_write8(Adapter, EFUSE_CTRL+1, (_offset & 0xff));
-	readbyte = rtw_read8(Adapter, EFUSE_CTRL+2);
-	rtw_write8(Adapter, EFUSE_CTRL+2, ((_offset >> 8) & 0x03) | (readbyte & 0xfc));
+	rtl8723au_write8(Adapter, EFUSE_CTRL+1, (_offset & 0xff));
+	readbyte = rtl8723au_read8(Adapter, EFUSE_CTRL+2);
+	rtl8723au_write8(Adapter, EFUSE_CTRL+2,
+			 ((_offset >> 8) & 0x03) | (readbyte & 0xfc));
 
 	/* Write bit 32 0 */
-	readbyte = rtw_read8(Adapter, EFUSE_CTRL+3);
-	rtw_write8(Adapter, EFUSE_CTRL+3, (readbyte & 0x7f));
+	readbyte = rtl8723au_read8(Adapter, EFUSE_CTRL+3);
+	rtl8723au_write8(Adapter, EFUSE_CTRL+3, readbyte & 0x7f);
 
 	/* Check bit 32 read-ready */
 	retry = 0;
-	value32 = rtw_read32(Adapter, EFUSE_CTRL);
+	value32 = rtl8723au_read32(Adapter, EFUSE_CTRL);
 	/* while(!(((value32 >> 24) & 0xff) & 0x80)  && (retry<10)) */
 	while(!(((value32 >> 24) & 0xff) & 0x80)  && (retry<10000))
 	{
-		value32 = rtw_read32(Adapter, EFUSE_CTRL);
+		value32 = rtl8723au_read32(Adapter, EFUSE_CTRL);
 		retry++;
 	}
 
@@ -133,46 +184,92 @@ ReadEFuseByte23a(struct rtw_adapter *Adapter, u16 _offset, u8 *pbuf)
 	/*  Designer says that there shall be some delay after ready bit is set, or the */
 	/*  result will always stay on last data we read. */
 	udelay(50);
-	value32 = rtw_read32(Adapter, EFUSE_CTRL);
+	value32 = rtl8723au_read32(Adapter, EFUSE_CTRL);
 
 	*pbuf = (u8)(value32 & 0xff);
 }
 
-/*  */
-/*	Description: */
-/*		1. Execute E-Fuse read byte operation according as map offset and */
-/*		    save to E-Fuse table. */
-/*		2. Refered from SD1 Richard. */
-/*  */
-/*	Assumption: */
-/*		1. Boot from E-Fuse and successfully auto-load. */
-/*		2. PASSIVE_LEVEL (USB interface) */
-/*  */
-/*	Created by Roger, 2008.10.21. */
-/*  */
-/*	2008/12/12 MH	1. Reorganize code flow and reserve bytes. and add description. */
-/*					2. Add efuse utilization collect. */
-/*	2008/12/22 MH	Read Efuse must check if we write section 1 data again!!! Sec1 */
-/*					write addr must be after sec5. */
-/*  */
-
-void
-efuse_ReadEFuse(struct rtw_adapter *Adapter, u8 efuseType,
-		u16 _offset, u16 _size_byte, u8 *pbuf);
-void
-efuse_ReadEFuse(struct rtw_adapter *Adapter, u8 efuseType,
-		u16 _offset, u16 _size_byte, u8 *pbuf)
-{
-	Adapter->HalFunc.ReadEFuse(Adapter, efuseType, _offset,
-				   _size_byte, pbuf);
-}
-
 void
 EFUSE_GetEfuseDefinition23a(struct rtw_adapter *pAdapter, u8 efuseType,
-			 u8 type, void *pOut)
+			    u8 type, void *pOut)
 {
-	pAdapter->HalFunc.EFUSEGetEfuseDefinition(pAdapter, efuseType,
-						  type, pOut);
+	u8 *pu1Tmp;
+	u16 *pu2Tmp;
+	u8 *pMax_section;
+
+	switch (type) {
+	case TYPE_EFUSE_MAX_SECTION:
+		pMax_section = (u8 *) pOut;
+
+		if (efuseType == EFUSE_WIFI)
+			*pMax_section = EFUSE_MAX_SECTION_8723A;
+		else
+			*pMax_section = EFUSE_BT_MAX_SECTION;
+		break;
+
+	case TYPE_EFUSE_REAL_CONTENT_LEN:
+		pu2Tmp = (u16 *) pOut;
+
+		if (efuseType == EFUSE_WIFI)
+			*pu2Tmp = EFUSE_REAL_CONTENT_LEN_8723A;
+		else
+			*pu2Tmp = EFUSE_BT_REAL_CONTENT_LEN;
+		break;
+
+	case TYPE_AVAILABLE_EFUSE_BYTES_BANK:
+		pu2Tmp = (u16 *) pOut;
+
+		if (efuseType == EFUSE_WIFI)
+			*pu2Tmp = (EFUSE_REAL_CONTENT_LEN_8723A -
+				   EFUSE_OOB_PROTECT_BYTES);
+		else
+			*pu2Tmp = (EFUSE_BT_REAL_BANK_CONTENT_LEN -
+				   EFUSE_PROTECT_BYTES_BANK);
+		break;
+
+	case TYPE_AVAILABLE_EFUSE_BYTES_TOTAL:
+		pu2Tmp = (u16 *) pOut;
+
+		if (efuseType == EFUSE_WIFI)
+			*pu2Tmp = (EFUSE_REAL_CONTENT_LEN_8723A -
+				   EFUSE_OOB_PROTECT_BYTES);
+		else
+			*pu2Tmp = (EFUSE_BT_REAL_CONTENT_LEN -
+				   (EFUSE_PROTECT_BYTES_BANK * 3));
+		break;
+
+	case TYPE_EFUSE_MAP_LEN:
+		pu2Tmp = (u16 *) pOut;
+
+		if (efuseType == EFUSE_WIFI)
+			*pu2Tmp = EFUSE_MAP_LEN_8723A;
+		else
+			*pu2Tmp = EFUSE_BT_MAP_LEN;
+		break;
+
+	case TYPE_EFUSE_PROTECT_BYTES_BANK:
+		pu1Tmp = (u8 *) pOut;
+
+		if (efuseType == EFUSE_WIFI)
+			*pu1Tmp = EFUSE_OOB_PROTECT_BYTES;
+		else
+			*pu1Tmp = EFUSE_PROTECT_BYTES_BANK;
+		break;
+
+	case TYPE_EFUSE_CONTENT_LEN_BANK:
+		pu2Tmp = (u16 *) pOut;
+
+		if (efuseType == EFUSE_WIFI)
+			*pu2Tmp = EFUSE_REAL_CONTENT_LEN_8723A;
+		else
+			*pu2Tmp = EFUSE_BT_REAL_BANK_CONTENT_LEN;
+		break;
+
+	default:
+		pu1Tmp = (u8 *) pOut;
+		*pu1Tmp = 0;
+		break;
+	}
 }
 
 /*-----------------------------------------------------------------------------
@@ -208,22 +305,22 @@ EFUSE_Read1Byte23a(struct rtw_adapter *Adapter, u16 Address)
 	{
 		/* Write E-fuse Register address bit0~7 */
 		temp = Address & 0xFF;
-		rtw_write8(Adapter, EFUSE_CTRL+1, temp);
-		Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+2);
+		rtl8723au_write8(Adapter, EFUSE_CTRL+1, temp);
+		Bytetemp = rtl8723au_read8(Adapter, EFUSE_CTRL+2);
 		/* Write E-fuse Register address bit8~9 */
 		temp = ((Address >> 8) & 0x03) | (Bytetemp & 0xFC);
-		rtw_write8(Adapter, EFUSE_CTRL+2, temp);
+		rtl8723au_write8(Adapter, EFUSE_CTRL+2, temp);
 
 		/* Write 0x30[31]= 0 */
-		Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3);
+		Bytetemp = rtl8723au_read8(Adapter, EFUSE_CTRL+3);
 		temp = Bytetemp & 0x7F;
-		rtw_write8(Adapter, EFUSE_CTRL+3, temp);
+		rtl8723au_write8(Adapter, EFUSE_CTRL+3, temp);
 
 		/* Wait Write-ready (0x30[31]= 1) */
-		Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3);
+		Bytetemp = rtl8723au_read8(Adapter, EFUSE_CTRL+3);
 		while(!(Bytetemp & 0x80))
 		{
-			Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3);
+			Bytetemp = rtl8723au_read8(Adapter, EFUSE_CTRL+3);
 			k++;
 			if (k == 1000)
 			{
@@ -231,7 +328,7 @@ EFUSE_Read1Byte23a(struct rtw_adapter *Adapter, u16 Address)
 				break;
 			}
 		}
-		data = rtw_read8(Adapter, EFUSE_CTRL);
+		data = rtl8723au_read8(Adapter, EFUSE_CTRL);
 		return data;
 	}
 	else
@@ -278,27 +375,27 @@ EFUSE_Write1Byte(
 
 	if (Address < contentLen)	/* E-fuse 512Byte */
 	{
-		rtw_write8(Adapter, EFUSE_CTRL, Value);
+		rtl8723au_write8(Adapter, EFUSE_CTRL, Value);
 
 		/* Write E-fuse Register address bit0~7 */
 		temp = Address & 0xFF;
-		rtw_write8(Adapter, EFUSE_CTRL+1, temp);
-		Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+2);
+		rtl8723au_write8(Adapter, EFUSE_CTRL+1, temp);
+		Bytetemp = rtl8723au_read8(Adapter, EFUSE_CTRL+2);
 
 		/* Write E-fuse Register address bit8~9 */
 		temp = ((Address >> 8) & 0x03) | (Bytetemp & 0xFC);
-		rtw_write8(Adapter, EFUSE_CTRL+2, temp);
+		rtl8723au_write8(Adapter, EFUSE_CTRL+2, temp);
 
 		/* Write 0x30[31]= 1 */
-		Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3);
+		Bytetemp = rtl8723au_read8(Adapter, EFUSE_CTRL+3);
 		temp = Bytetemp | 0x80;
-		rtw_write8(Adapter, EFUSE_CTRL+3, temp);
+		rtl8723au_write8(Adapter, EFUSE_CTRL+3, temp);
 
 		/* Wait Write-ready (0x30[31]= 0) */
-		Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3);
+		Bytetemp = rtl8723au_read8(Adapter, EFUSE_CTRL+3);
 		while(Bytetemp & 0x80)
 		{
-			Bytetemp = rtw_read8(Adapter, EFUSE_CTRL+3);
+			Bytetemp = rtl8723au_read8(Adapter, EFUSE_CTRL+3);
 			k++;
 			if (k == 100)
 			{
@@ -310,38 +407,38 @@ EFUSE_Write1Byte(
 }/* EFUSE_Write1Byte */
 
 /*  11/16/2008 MH Read one byte from real Efuse. */
-u8
+int
 efuse_OneByteRead23a(struct rtw_adapter *pAdapter, u16 addr, u8 *data)
 {
 	u8	tmpidx = 0;
-	u8	bResult;
+	int	bResult;
 
 	/*  -----------------e-fuse reg ctrl --------------------------------- */
 	/* address */
-	rtw_write8(pAdapter, EFUSE_CTRL+1, (u8)(addr&0xff));
-	rtw_write8(pAdapter, EFUSE_CTRL+2, ((u8)((addr>>8) &0x03)) |
-	(rtw_read8(pAdapter, EFUSE_CTRL+2)&0xFC));
+	rtl8723au_write8(pAdapter, EFUSE_CTRL+1, (u8)(addr&0xff));
+	rtl8723au_write8(pAdapter, EFUSE_CTRL+2, ((u8)((addr>>8) &0x03)) |
+	(rtl8723au_read8(pAdapter, EFUSE_CTRL+2)&0xFC));
 
-	rtw_write8(pAdapter, EFUSE_CTRL+3,  0x72);/* read cmd */
+	rtl8723au_write8(pAdapter, EFUSE_CTRL+3,  0x72);/* read cmd */
 
-	while(!(0x80 &rtw_read8(pAdapter, EFUSE_CTRL+3)) && (tmpidx<100))
+	while(!(0x80 &rtl8723au_read8(pAdapter, EFUSE_CTRL+3)) && (tmpidx<100))
 		tmpidx++;
 	if (tmpidx < 100) {
-		*data = rtw_read8(pAdapter, EFUSE_CTRL);
-		bResult = true;
+		*data = rtl8723au_read8(pAdapter, EFUSE_CTRL);
+		bResult = _SUCCESS;
 	} else {
 		*data = 0xff;
-		bResult = false;
+		bResult = _FAIL;
 	}
 	return bResult;
 }
 
 /*  11/16/2008 MH Write one byte to reald Efuse. */
-u8
+int
 efuse_OneByteWrite23a(struct rtw_adapter *pAdapter, u16 addr, u8 data)
 {
 	u8	tmpidx = 0;
-	u8	bResult;
+	int	bResult;
 
 	/* RT_TRACE(COMP_EFUSE, DBG_LOUD, ("Addr = %x Data =%x\n", addr, data)); */
 
@@ -349,51 +446,26 @@ efuse_OneByteWrite23a(struct rtw_adapter *pAdapter, u16 addr, u8 data)
 
 	/*  -----------------e-fuse reg ctrl --------------------------------- */
 	/* address */
-	rtw_write8(pAdapter, EFUSE_CTRL+1, (u8)(addr&0xff));
-	rtw_write8(pAdapter, EFUSE_CTRL+2,
-	(rtw_read8(pAdapter, EFUSE_CTRL+2)&0xFC)|(u8)((addr>>8)&0x03));
-	rtw_write8(pAdapter, EFUSE_CTRL, data);/* data */
+	rtl8723au_write8(pAdapter, EFUSE_CTRL+1, (u8)(addr&0xff));
+	rtl8723au_write8(pAdapter, EFUSE_CTRL+2,
+	(rtl8723au_read8(pAdapter, EFUSE_CTRL+2)&0xFC)|(u8)((addr>>8)&0x03));
+	rtl8723au_write8(pAdapter, EFUSE_CTRL, data);/* data */
 
-	rtw_write8(pAdapter, EFUSE_CTRL+3, 0xF2);/* write cmd */
+	rtl8723au_write8(pAdapter, EFUSE_CTRL+3, 0xF2);/* write cmd */
 
-	while((0x80 &  rtw_read8(pAdapter, EFUSE_CTRL+3)) && (tmpidx<100)) {
+	while((0x80 & rtl8723au_read8(pAdapter, EFUSE_CTRL+3)) &&
+	      (tmpidx<100)) {
 		tmpidx++;
 	}
 
-	if (tmpidx<100)
-	{
-		bResult = true;
-	}
+	if (tmpidx < 100)
+		bResult = _SUCCESS;
 	else
-	{
-		bResult = false;
-	}
+		bResult = _FAIL;
 
 	return bResult;
 }
 
-int
-Efuse_PgPacketRead23a(struct rtw_adapter *pAdapter, u8 offset, u8 *data)
-{
-	int	ret = 0;
-
-	ret =  pAdapter->HalFunc.Efuse_PgPacketRead23a(pAdapter, offset, data);
-
-	return ret;
-}
-
-int
-Efuse_PgPacketWrite23a(struct rtw_adapter *pAdapter, u8 offset,
-		    u8 word_en, u8 *data)
-{
-	int ret;
-
-	ret =  pAdapter->HalFunc.Efuse_PgPacketWrite23a(pAdapter, offset,
-						     word_en, data);
-
-	return ret;
-}
-
 /*-----------------------------------------------------------------------------
  * Function:	efuse_WordEnableDataRead23a
  *
@@ -438,24 +510,12 @@ efuse_WordEnableDataRead23a(u8	word_en,
 	}
 }
 
-u8
-Efuse_WordEnableDataWrite23a(struct rtw_adapter *pAdapter, u16 efuse_addr,
-			  u8 word_en, u8 *data)
-{
-	u8 ret = 0;
-
-	ret = pAdapter->HalFunc.Efuse_WordEnableDataWrite23a(pAdapter, efuse_addr,
-							  word_en, data);
-
-	return ret;
-}
-
-static u8 efuse_read8(struct rtw_adapter *padapter, u16 address, u8 *value)
+static int efuse_read8(struct rtw_adapter *padapter, u16 address, u8 *value)
 {
 	return efuse_OneByteRead23a(padapter, address, value);
 }
 
-static u8 efuse_write8(struct rtw_adapter *padapter, u16 address, u8 *value)
+static int efuse_write8(struct rtw_adapter *padapter, u16 address, u8 *value)
 {
 	return efuse_OneByteWrite23a(padapter, address, *value);
 }
@@ -463,13 +523,13 @@ static u8 efuse_write8(struct rtw_adapter *padapter, u16 address, u8 *value)
 /*
  * read/wirte raw efuse data
  */
-u8 rtw_efuse_access23a(struct rtw_adapter *padapter, u8 bWrite, u16 start_addr,
-		    u16 cnts, u8 *data)
+int rtw_efuse_access23a(struct rtw_adapter *padapter, u8 bWrite, u16 start_addr,
+			u16 cnts, u8 *data)
 {
 	int i = 0;
-	u16	real_content_len = 0, max_available_size = 0;
-	u8 res = _FAIL ;
-	u8 (*rw8)(struct rtw_adapter *, u16, u8*);
+	u16 real_content_len = 0, max_available_size = 0;
+	int res = _FAIL ;
+	int (*rw8)(struct rtw_adapter *, u16, u8*);
 
 	EFUSE_GetEfuseDefinition23a(padapter, EFUSE_WIFI,
 				 TYPE_EFUSE_REAL_CONTENT_LEN,
@@ -488,7 +548,7 @@ u8 rtw_efuse_access23a(struct rtw_adapter *padapter, u8 bWrite, u16 start_addr,
 	} else
 		rw8 = &efuse_read8;
 
-	Efuse_PowerSwitch23a(padapter, bWrite, true);
+	Efuse_PowerSwitch(padapter, bWrite, true);
 
 	/*  e-fuse one byte read / write */
 	for (i = 0; i < cnts; i++) {
@@ -498,35 +558,37 @@ u8 rtw_efuse_access23a(struct rtw_adapter *padapter, u8 bWrite, u16 start_addr,
 		}
 
 		res = rw8(padapter, start_addr++, data++);
-		if (_FAIL == res) break;
+		if (res == _FAIL)
+			break;
 	}
 
-	Efuse_PowerSwitch23a(padapter, bWrite, false);
+	Efuse_PowerSwitch(padapter, bWrite, false);
 
 	return res;
 }
 /*  */
 u16 efuse_GetMaxSize23a(struct rtw_adapter *padapter)
 {
-	u16	max_size;
+	u16 max_size;
 	EFUSE_GetEfuseDefinition23a(padapter, EFUSE_WIFI,
 				 TYPE_AVAILABLE_EFUSE_BYTES_TOTAL,
 				 (void *)&max_size);
 	return max_size;
 }
 /*  */
-u8 efuse_GetCurrentSize23a(struct rtw_adapter *padapter, u16 *size)
+int efuse_GetCurrentSize23a(struct rtw_adapter *padapter, u16 *size)
 {
-	Efuse_PowerSwitch23a(padapter, false, true);
+	Efuse_PowerSwitch(padapter, false, true);
 	*size = Efuse_GetCurrentSize23a(padapter, EFUSE_WIFI);
-	Efuse_PowerSwitch23a(padapter, false, false);
+	Efuse_PowerSwitch(padapter, false, false);
 
 	return _SUCCESS;
 }
 /*  */
-u8 rtw_efuse_map_read23a(struct rtw_adapter *padapter, u16 addr, u16 cnts, u8 *data)
+int rtw_efuse_map_read23a(struct rtw_adapter *padapter,
+			  u16 addr, u16 cnts, u8 *data)
 {
-	u16	mapLen = 0;
+	u16 mapLen = 0;
 
 	EFUSE_GetEfuseDefinition23a(padapter, EFUSE_WIFI,
 				 TYPE_EFUSE_MAP_LEN, (void *)&mapLen);
@@ -534,18 +596,19 @@ u8 rtw_efuse_map_read23a(struct rtw_adapter *padapter, u16 addr, u16 cnts, u8 *d
 	if ((addr + cnts) > mapLen)
 		return _FAIL;
 
-	Efuse_PowerSwitch23a(padapter, false, true);
+	Efuse_PowerSwitch(padapter, false, true);
 
-	efuse_ReadEFuse(padapter, EFUSE_WIFI, addr, cnts, data);
+	rtl8723a_readefuse(padapter, EFUSE_WIFI, addr, cnts, data);
 
-	Efuse_PowerSwitch23a(padapter, false, false);
+	Efuse_PowerSwitch(padapter, false, false);
 
 	return _SUCCESS;
 }
 
-u8 rtw_BT_efuse_map_read23a(struct rtw_adapter *padapter, u16 addr, u16 cnts, u8 *data)
+int rtw_BT_efuse_map_read23a(struct rtw_adapter *padapter,
+			     u16 addr, u16 cnts, u8 *data)
 {
-	u16	mapLen = 0;
+	u16 mapLen = 0;
 
 	EFUSE_GetEfuseDefinition23a(padapter, EFUSE_BT,
 				 TYPE_EFUSE_MAP_LEN, (void *)&mapLen);
@@ -553,11 +616,11 @@ u8 rtw_BT_efuse_map_read23a(struct rtw_adapter *padapter, u16 addr, u16 cnts, u8
 	if ((addr + cnts) > mapLen)
 		return _FAIL;
 
-	Efuse_PowerSwitch23a(padapter, false, true);
+	Efuse_PowerSwitch(padapter, false, true);
 
-	efuse_ReadEFuse(padapter, EFUSE_BT, addr, cnts, data);
+	rtl8723a_readefuse(padapter, EFUSE_BT, addr, cnts, data);
 
-	Efuse_PowerSwitch23a(padapter, false, false);
+	Efuse_PowerSwitch(padapter, false, false);
 
 	return _SUCCESS;
 }
@@ -585,14 +648,14 @@ Efuse_ReadAllMap(struct rtw_adapter *pAdapter, u8 efuseType, u8 *Efuse)
 {
 	u16	mapLen = 0;
 
-	Efuse_PowerSwitch23a(pAdapter, false, true);
+	Efuse_PowerSwitch(pAdapter, false, true);
 
 	EFUSE_GetEfuseDefinition23a(pAdapter, efuseType, TYPE_EFUSE_MAP_LEN,
 				 (void *)&mapLen);
 
-	efuse_ReadEFuse(pAdapter, efuseType, 0, mapLen, Efuse);
+	rtl8723a_readefuse(pAdapter, efuseType, 0, mapLen, Efuse);
 
-	Efuse_PowerSwitch23a(pAdapter, false, false);
+	Efuse_PowerSwitch(pAdapter, false, false);
 }
 
 /*-----------------------------------------------------------------------------
diff --git a/drivers/staging/rtl8723au/core/rtw_ieee80211.c b/drivers/staging/rtl8723au/core/rtw_ieee80211.c
index 780631fd3b6..adb86a54bdb 100644
--- a/drivers/staging/rtl8723au/core/rtw_ieee80211.c
+++ b/drivers/staging/rtl8723au/core/rtw_ieee80211.c
@@ -46,21 +46,23 @@ u8 RSN_CIPHER_SUITE_WEP10423A[] = { 0x00, 0x0f, 0xac, 5 };
 /*  for adhoc-master to generate ie and provide supported-rate to fw */
 /*  */
 
-static u8	WIFI_CCKRATES[] =
-{(IEEE80211_CCK_RATE_1MB | IEEE80211_BASIC_RATE_MASK),
- (IEEE80211_CCK_RATE_2MB | IEEE80211_BASIC_RATE_MASK),
- (IEEE80211_CCK_RATE_5MB | IEEE80211_BASIC_RATE_MASK),
- (IEEE80211_CCK_RATE_11MB | IEEE80211_BASIC_RATE_MASK)};
-
-static u8	WIFI_OFDMRATES[] =
-{(IEEE80211_OFDM_RATE_6MB),
- (IEEE80211_OFDM_RATE_9MB),
- (IEEE80211_OFDM_RATE_12MB),
- (IEEE80211_OFDM_RATE_18MB),
- (IEEE80211_OFDM_RATE_24MB),
- IEEE80211_OFDM_RATE_36MB,
- IEEE80211_OFDM_RATE_48MB,
- IEEE80211_OFDM_RATE_54MB};
+static u8 WIFI_CCKRATES[] = {
+	IEEE80211_CCK_RATE_1MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_CCK_RATE_2MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_CCK_RATE_5MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_CCK_RATE_11MB | IEEE80211_BASIC_RATE_MASK
+};
+
+static u8 WIFI_OFDMRATES[] = {
+	IEEE80211_OFDM_RATE_6MB,
+	IEEE80211_OFDM_RATE_9MB,
+	IEEE80211_OFDM_RATE_12MB,
+	IEEE80211_OFDM_RATE_18MB,
+	IEEE80211_OFDM_RATE_24MB,
+	IEEE80211_OFDM_RATE_36MB,
+	IEEE80211_OFDM_RATE_48MB,
+	IEEE80211_OFDM_RATE_54MB
+};
 
 int rtw_get_bit_value_from_ieee_value23a(u8 val)
 {
@@ -76,13 +78,13 @@ int rtw_get_bit_value_from_ieee_value23a(u8 val)
 	return 0;
 }
 
-uint rtw_is_cckrates_included23a(u8 *rate)
+static bool rtw_is_cckrates_included(u8 *rate)
 {
 	u32 i = 0;
 
-	while (rate[i] != 0) {
-		if ((((rate[i]) & 0x7f) == 2) || (((rate[i]) & 0x7f) == 4) ||
-		    (((rate[i]) & 0x7f) == 11) || (((rate[i]) & 0x7f) == 22))
+	while (rate[i]) {
+		if ((rate[i] & 0x7f) == 2 || (rate[i] & 0x7f) == 4 ||
+		    (rate[i] & 0x7f) == 11 || (rate[i] & 0x7f) == 22)
 			return true;
 		i++;
 	}
@@ -90,13 +92,13 @@ uint rtw_is_cckrates_included23a(u8 *rate)
 	return false;
 }
 
-uint rtw_is_cckratesonly_included23a(u8 *rate)
+static bool rtw_is_cckratesonly_included(u8 *rate)
 {
 	u32 i = 0;
 
-	while (rate[i] != 0) {
-		if ((((rate[i]) & 0x7f) != 2) && (((rate[i]) & 0x7f) != 4) &&
-		    (((rate[i]) & 0x7f) != 11) && (((rate[i]) & 0x7f) != 22))
+	while (rate[i]) {
+		if ((rate[i] & 0x7f) != 2 && (rate[i] & 0x7f) != 4 &&
+		    (rate[i] & 0x7f) != 11 && (rate[i] & 0x7f) != 22)
 			return false;
 
 		i++;
@@ -108,14 +110,14 @@ uint rtw_is_cckratesonly_included23a(u8 *rate)
 int rtw_check_network_type23a(unsigned char *rate, int ratelen, int channel)
 {
 	if (channel > 14) {
-		if ((rtw_is_cckrates_included23a(rate)) == true)
+		if (rtw_is_cckrates_included(rate))
 			return WIRELESS_INVALID;
 		else
 			return WIRELESS_11A;
 	} else {  /*  could be pure B, pure G, or B/G */
-		if ((rtw_is_cckratesonly_included23a(rate)) == true)
+		if (rtw_is_cckratesonly_included(rate))
 			return WIRELESS_11B;
-		else if ((rtw_is_cckrates_included23a(rate)) == true)
+		else if (rtw_is_cckrates_included(rate))
 			return	WIRELESS_11BG;
 		else
 			return WIRELESS_11G;
@@ -131,7 +133,7 @@ u8 *rtw_set_fixed_ie23a(unsigned char *pbuf, unsigned int len,
 }
 
 /*  rtw_set_ie23a will update frame length */
-u8 *rtw_set_ie23a(u8 *pbuf, int index, uint len, u8 *source, uint *frlen)
+u8 *rtw_set_ie23a(u8 *pbuf, int index, uint len, const u8 *source, uint *frlen)
 {
 
 	*pbuf = (u8)index;
@@ -143,7 +145,6 @@ u8 *rtw_set_ie23a(u8 *pbuf, int index, uint len, u8 *source, uint *frlen)
 
 	*frlen = *frlen + (len + 2);
 
-
 	return pbuf + len + 2;
 }
 
@@ -158,18 +159,6 @@ inline u8 *rtw_set_ie23a_ch_switch (u8 *buf, u32 *buf_len, u8 ch_switch_mode,
 	return rtw_set_ie23a(buf, WLAN_EID_CHANNEL_SWITCH,  3, ie_data, buf_len);
 }
 
-inline u8 secondary_ch_offset_to_hal_ch_offset23a(u8 ch_offset)
-{
-	if (ch_offset == SCN)
-		return HAL_PRIME_CHNL_OFFSET_DONT_CARE;
-	else if (ch_offset == SCA)
-		return HAL_PRIME_CHNL_OFFSET_UPPER;
-	else if (ch_offset == SCB)
-		return HAL_PRIME_CHNL_OFFSET_LOWER;
-
-	return HAL_PRIME_CHNL_OFFSET_DONT_CARE;
-}
-
 inline u8 hal_ch_offset_to_secondary_ch_offset23a(u8 ch_offset)
 {
 	if (ch_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE)
@@ -189,19 +178,6 @@ inline u8 *rtw_set_ie23a_secondary_ch_offset(u8 *buf, u32 *buf_len,
 			  1, &secondary_ch_offset, buf_len);
 }
 
-inline u8 *rtw_set_ie23a_mesh_ch_switch_parm(u8 *buf, u32 *buf_len, u8 ttl,
-					  u8 flags, u16 reason, u16 precedence)
-{
-	u8 ie_data[6];
-
-	ie_data[0] = ttl;
-	ie_data[1] = flags;
-	put_unaligned_le16(reason, (u8*)&ie_data[2]);
-	put_unaligned_le16(precedence, (u8*)&ie_data[4]);
-
-	return rtw_set_ie23a(buf, 0x118,  6, ie_data, buf_len);
-}
-
 /*----------------------------------------------------------------------------
 index: the information element id index, limit is the limit for search
 -----------------------------------------------------------------------------*/
@@ -367,7 +343,7 @@ uint rtw_get_rateset_len23a(u8 *rateset)
 	uint i = 0;
 
 	while(1) {
-		if ((rateset[i]) == 0)
+		if (rateset[i] == 0)
 			break;
 
 		if (i > 12)
@@ -394,31 +370,31 @@ int rtw_generate_ie23a(struct registry_priv *pregistrypriv)
 
 	/* beacon interval : 2bytes */
 	/* BCN_INTERVAL; */
-	*(u16*)ie = cpu_to_le16((u16)pdev_network->Configuration.BeaconPeriod);
+	*(u16*)ie = cpu_to_le16(pdev_network->BeaconPeriod);
 	sz += 2;
 	ie += 2;
 
 	/* capability info */
 	*(u16*)ie = 0;
 
-	*(u16*)ie |= cpu_to_le16(cap_IBSS);
+	*(u16*)ie |= cpu_to_le16(WLAN_CAPABILITY_IBSS);
 
 	if (pregistrypriv->preamble == PREAMBLE_SHORT)
-		*(u16*)ie |= cpu_to_le16(cap_ShortPremble);
+		*(u16*)ie |= cpu_to_le16(WLAN_CAPABILITY_SHORT_PREAMBLE);
 
 	if (pdev_network->Privacy)
-		*(u16*)ie |= cpu_to_le16(cap_Privacy);
+		*(u16*)ie |= cpu_to_le16(WLAN_CAPABILITY_PRIVACY);
 
 	sz += 2;
 	ie += 2;
 
 	/* SSID */
-	ie = rtw_set_ie23a(ie, _SSID_IE_, pdev_network->Ssid.ssid_len,
+	ie = rtw_set_ie23a(ie, WLAN_EID_SSID, pdev_network->Ssid.ssid_len,
 			pdev_network->Ssid.ssid, &sz);
 
 	/* supported rates */
 	if (pregistrypriv->wireless_mode == WIRELESS_11ABGN) {
-		if (pdev_network->Configuration.DSConfig > 14)
+		if (pdev_network->DSConfig > 14)
 			wireless_mode = WIRELESS_11A_5N;
 		else
 			wireless_mode = WIRELESS_11BG_24N;
@@ -431,25 +407,25 @@ int rtw_generate_ie23a(struct registry_priv *pregistrypriv)
 	rateLen = rtw_get_rateset_len23a(pdev_network->SupportedRates);
 
 	if (rateLen > 8) {
-		ie = rtw_set_ie23a(ie, _SUPPORTEDRATES_IE_, 8,
+		ie = rtw_set_ie23a(ie, WLAN_EID_SUPP_RATES, 8,
 				pdev_network->SupportedRates, &sz);
 		/* ie = rtw_set_ie23a(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (pdev_network->SupportedRates + 8), &sz); */
 	} else {
-		ie = rtw_set_ie23a(ie, _SUPPORTEDRATES_IE_, rateLen,
+		ie = rtw_set_ie23a(ie, WLAN_EID_SUPP_RATES, rateLen,
 				pdev_network->SupportedRates, &sz);
 	}
 
 	/* DS parameter set */
-	ie = rtw_set_ie23a(ie, _DSSET_IE_, 1,
-			   (u8 *)&pdev_network->Configuration.DSConfig, &sz);
+	ie = rtw_set_ie23a(ie, WLAN_EID_DS_PARAMS, 1,
+			   (u8 *)&pdev_network->DSConfig, &sz);
 
 	/* IBSS Parameter Set */
 
-	ie = rtw_set_ie23a(ie, _IBSS_PARA_IE_, 2,
-			   (u8 *)&pdev_network->Configuration.ATIMWindow, &sz);
+	ie = rtw_set_ie23a(ie, WLAN_EID_IBSS_PARAMS, 2,
+			   (u8 *)&pdev_network->ATIMWindow, &sz);
 
 	if (rateLen > 8) {
-		ie = rtw_set_ie23a(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8),
+		ie = rtw_set_ie23a(ie, WLAN_EID_EXT_SUPP_RATES, (rateLen - 8),
 				(pdev_network->SupportedRates + 8), &sz);
 	}
 
@@ -460,60 +436,7 @@ int rtw_generate_ie23a(struct registry_priv *pregistrypriv)
 	return sz;
 }
 
-unsigned char *rtw_get_wpa_ie23a(unsigned char *pie, int *wpa_ie_len, int limit)
-{
-	int len;
-	u16 val16;
-	unsigned char wpa_oui_type[] = {0x00, 0x50, 0xf2, 0x01};
-	u8 *pbuf = pie;
-	int limit_new = limit;
-
-	while(1) {
-		pbuf = rtw_get_ie23a(pbuf, _WPA_IE_ID_, &len, limit_new);
-
-		if (pbuf) {
-			/* check if oui matches... */
-			if (memcmp((pbuf + 2), wpa_oui_type,
-				   sizeof(wpa_oui_type))) {
-				goto check_next_ie;
-			}
-
-			/* check version... */
-			memcpy((u8 *)&val16, (pbuf + 6), sizeof(val16));
-
-			val16 = le16_to_cpu(val16);
-			if (val16 != 0x0001)
-				goto check_next_ie;
-
-			*wpa_ie_len = *(pbuf + 1);
-
-			return pbuf;
-		} else {
-			*wpa_ie_len = 0;
-			return NULL;
-		}
-
-check_next_ie:
-
-		limit_new = limit - (pbuf - pie) - 2 - len;
-
-		if (limit_new <= 0)
-			break;
-
-		pbuf += (2 + len);
-	}
-
-	*wpa_ie_len = 0;
-
-	return NULL;
-}
-
-unsigned char *rtw_get_wpa2_ie23a(unsigned char *pie, int *rsn_ie_len, int limit)
-{
-	return rtw_get_ie23a(pie, _WPA2_IE_ID_, rsn_ie_len, limit);
-}
-
-int rtw_get_wpa_cipher_suite23a(u8 *s)
+int rtw_get_wpa_cipher_suite23a(const u8 *s)
 {
 	if (!memcmp(s, WPA_CIPHER_SUITE_NONE23A, WPA_SELECTOR_LEN))
 		return WPA_CIPHER_NONE;
@@ -529,7 +452,7 @@ int rtw_get_wpa_cipher_suite23a(u8 *s)
 	return 0;
 }
 
-int rtw_get_wpa2_cipher_suite23a(u8 *s)
+int rtw_get_wpa2_cipher_suite23a(const u8 *s)
 {
 	if (!memcmp(s, RSN_CIPHER_SUITE_NONE23A, RSN_SELECTOR_LEN))
 		return WPA_CIPHER_NONE;
@@ -545,22 +468,19 @@ int rtw_get_wpa2_cipher_suite23a(u8 *s)
 	return 0;
 }
 
-int rtw_parse_wpa_ie23a(u8* wpa_ie, int wpa_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x)
+int rtw_parse_wpa_ie23a(const u8* wpa_ie, int wpa_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x)
 {
 	int i, ret = _SUCCESS;
 	int left, count;
-	u8 *pos;
-	u8 SUITE_1X[4] = {0x00, 0x50, 0xf2, 1};
+	const u8 *pos;
 
 	if (wpa_ie_len <= 0) {
 		/* No WPA IE - fail silently */
 		return _FAIL;
 	}
 
-	if ((*wpa_ie != _WPA_IE_ID_) || (*(wpa_ie+1) != (u8)(wpa_ie_len - 2)) ||
-	    memcmp(wpa_ie + 2, RTW_WPA_OUI23A_TYPE, WPA_SELECTOR_LEN)) {
+	if (wpa_ie[1] != (u8)(wpa_ie_len - 2))
 		return _FAIL;
-	}
 
 	pos = wpa_ie;
 
@@ -612,7 +532,7 @@ int rtw_parse_wpa_ie23a(u8* wpa_ie, int wpa_ie_len, int *group_cipher, int *pair
 	if (is_8021x) {
 		if (left >= 6) {
 			pos += 2;
-			if (!memcmp(pos, SUITE_1X, 4)) {
+			if (!memcmp(pos, RTW_WPA_OUI23A_TYPE, 4)) {
 				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
 					 ("%s : there has 802.1x auth\n",
 					  __func__));
@@ -624,12 +544,12 @@ int rtw_parse_wpa_ie23a(u8* wpa_ie, int wpa_ie_len, int *group_cipher, int *pair
 	return ret;
 }
 
-int rtw_parse_wpa2_ie23a(u8* rsn_ie, int rsn_ie_len, int *group_cipher,
+int rtw_parse_wpa2_ie23a(const u8* rsn_ie, int rsn_ie_len, int *group_cipher,
 		      int *pairwise_cipher, int *is_8021x)
 {
 	int i, ret = _SUCCESS;
 	int left, count;
-	u8 *pos;
+	const u8 *pos;
 	u8 SUITE_1X[4] = {0x00, 0x0f, 0xac, 0x01};
 
 	if (rsn_ie_len <= 0) {
@@ -637,7 +557,7 @@ int rtw_parse_wpa2_ie23a(u8* rsn_ie, int rsn_ie_len, int *group_cipher,
 		return _FAIL;
 	}
 
-	if ((*rsn_ie!= _WPA2_IE_ID_) || (*(rsn_ie+1) != (u8)(rsn_ie_len - 2))) {
+	if (*rsn_ie != _WPA2_IE_ID_ || *(rsn_ie+1) != (u8)(rsn_ie_len - 2)) {
 		return _FAIL;
 	}
 
@@ -705,11 +625,9 @@ int rtw_get_sec_ie23a(u8 *in_ie, uint in_len, u8 *rsn_ie, u16 *rsn_len,
 		   u8 *wpa_ie, u16 *wpa_len)
 {
 	u8 authmode, sec_idx, i;
-	u8 wpa_oui[4] = {0x0, 0x50, 0xf2, 0x01};
 	uint cnt;
 
 
-
 	/* Search required WPA or WPA2 IE and copy to sec_ie[ ] */
 
 	cnt = (_TIMESTAMP_ + _BEACON_ITERVAL_ + _CAPABILITY_);
@@ -719,8 +637,8 @@ int rtw_get_sec_ie23a(u8 *in_ie, uint in_len, u8 *rsn_ie, u16 *rsn_len,
 	while(cnt < in_len) {
 		authmode = in_ie[cnt];
 
-		if ((authmode == _WPA_IE_ID_) &&
-		    !memcmp(&in_ie[cnt+2], &wpa_oui[0], 4)) {
+		if ((authmode == WLAN_EID_VENDOR_SPECIFIC) &&
+		    !memcmp(&in_ie[cnt+2], RTW_WPA_OUI23A_TYPE, 4)) {
 				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
 					 ("\n rtw_get_wpa_ie23a: sec_idx =%d "
 					  "in_ie[cnt+1]+2 =%d\n",
@@ -778,24 +696,6 @@ int rtw_get_sec_ie23a(u8 *in_ie, uint in_len, u8 *rsn_ie, u16 *rsn_len,
 	return *rsn_len + *wpa_len;
 }
 
-u8 rtw_is_wps_ie23a(u8 *ie_ptr, uint *wps_ielen)
-{
-	u8 match = false;
-	u8 eid, wps_oui[4]= {0x0, 0x50, 0xf2, 0x04};
-
-	if (!ie_ptr)
-		return match;
-
-	eid = ie_ptr[0];
-
-	if ((eid == _WPA_IE_ID_) && !memcmp(&ie_ptr[2], wps_oui, 4)) {
-		/* DBG_8723A("==> found WPS_IE.....\n"); */
-		*wps_ielen = ie_ptr[1] + 2;
-		match = true;
-	}
-	return match;
-}
-
 /**
  * rtw_get_wps_ie23a - Search WPS IE from a series of IEs
  * @in_ie: Address of IEs to search
@@ -824,7 +724,8 @@ u8 *rtw_get_wps_ie23a(u8 *in_ie, uint in_len, u8 *wps_ie, uint *wps_ielen)
 	while (cnt < in_len) {
 		eid = in_ie[cnt];
 
-		if ((eid == _WPA_IE_ID_) && !memcmp(&in_ie[cnt+2], wps_oui, 4)) {
+		if (eid == WLAN_EID_VENDOR_SPECIFIC &&
+		    !memcmp(&in_ie[cnt+2], wps_oui, 4)) {
 			wpsie_ptr = &in_ie[cnt];
 
 			if (wps_ie)
@@ -866,7 +767,7 @@ u8 *rtw_get_wps_attr23a(u8 *wps_ie, uint wps_ielen, u16 target_attr_id,
 	if (len_attr)
 		*len_attr = 0;
 
-	if ((wps_ie[0] != _VENDOR_SPECIFIC_IE_) ||
+	if (wps_ie[0] != WLAN_EID_VENDOR_SPECIFIC ||
 	    memcmp(wps_ie + 2, wps_oui, 4)) {
 		return attr_ptr;
 	}
@@ -937,686 +838,26 @@ u8 *rtw_get_wps_attr_content23a(u8 *wps_ie, uint wps_ielen, u16 target_attr_id,
 	return NULL;
 }
 
-static int
-rtw_ieee802_11_parse_vendor_specific(u8 *pos, uint elen,
-				     struct rtw_ieee802_11_elems *elems,
-				     int show_errors)
-{
-	unsigned int oui;
-
-	/* first 3 bytes in vendor specific information element are the IEEE
-	 * OUI of the vendor. The following byte is used a vendor specific
-	 * sub-type. */
-	if (elen < 4) {
-		if (show_errors) {
-			DBG_8723A("short vendor specific "
-				   "information element ignored (len =%lu)\n",
-				   (unsigned long) elen);
-		}
-		return -1;
-	}
-
-	oui = RTW_GET_BE24(pos);
-	switch (oui) {
-	case WLAN_OUI_MICROSOFT:
-		/* Microsoft/Wi-Fi information elements are further typed and
-		 * subtyped */
-		switch (pos[3]) {
-		case 1:
-			/* Microsoft OUI (00:50:F2) with OUI Type 1:
-			 * real WPA information element */
-			elems->wpa_ie = pos;
-			elems->wpa_ie_len = elen;
-			break;
-		case WME_OUI_TYPE: /* this is a Wi-Fi WME info. element */
-			if (elen < 5) {
-				DBG_8723A("short WME "
-					   "information element ignored "
-					   "(len =%lu)\n",
-					   (unsigned long) elen);
-				return -1;
-			}
-			switch (pos[4]) {
-			case WME_OUI_SUBTYPE_INFORMATION_ELEMENT:
-			case WME_OUI_SUBTYPE_PARAMETER_ELEMENT:
-				elems->wme = pos;
-				elems->wme_len = elen;
-				break;
-			case WME_OUI_SUBTYPE_TSPEC_ELEMENT:
-				elems->wme_tspec = pos;
-				elems->wme_tspec_len = elen;
-				break;
-			default:
-				DBG_8723A("unknown WME "
-					   "information element ignored "
-					   "(subtype =%d len =%lu)\n",
-					   pos[4], (unsigned long) elen);
-				return -1;
-			}
-			break;
-		case 4:
-			/* Wi-Fi Protected Setup (WPS) IE */
-			elems->wps_ie = pos;
-			elems->wps_ie_len = elen;
-			break;
-		default:
-			DBG_8723A("Unknown Microsoft "
-				   "information element ignored "
-				   "(type =%d len =%lu)\n",
-				   pos[3], (unsigned long) elen);
-			return -1;
-		}
-		break;
-
-	case OUI_BROADCOM:
-		switch (pos[3]) {
-		case VENDOR_HT_CAPAB_OUI_TYPE:
-			elems->vendor_ht_cap = pos;
-			elems->vendor_ht_cap_len = elen;
-			break;
-		default:
-			DBG_8723A("Unknown Broadcom "
-				   "information element ignored "
-				   "(type =%d len =%lu)\n",
-				   pos[3], (unsigned long) elen);
-			return -1;
-		}
-		break;
-
-	default:
-		DBG_8723A("unknown vendor specific information "
-			   "element ignored (vendor OUI %02x:%02x:%02x "
-			   "len =%lu)\n",
-			   pos[0], pos[1], pos[2], (unsigned long) elen);
-		return -1;
-	}
-
-	return 0;
-}
-
-/**
- * ieee802_11_parse_elems - Parse information elements in management frames
- * @start: Pointer to the start of IEs
- * @len: Length of IE buffer in octets
- * @elems: Data structure for parsed elements
- * @show_errors: Whether to show parsing errors in debug log
- * Returns: Parsing result
- */
-enum parse_res rtw_ieee802_11_parse_elems23a(u8 *start, uint len,
-				struct rtw_ieee802_11_elems *elems,
-				int show_errors)
-{
-	uint left = len;
-	u8 *pos = start;
-	int unknown = 0;
-
-	memset(elems, 0, sizeof(*elems));
-
-	while (left >= 2) {
-		u8 id, elen;
-
-		id = *pos++;
-		elen = *pos++;
-		left -= 2;
-
-		if (elen > left) {
-			if (show_errors) {
-				DBG_8723A("IEEE 802.11 element "
-					   "parse failed (id =%d elen =%d "
-					   "left =%lu)\n",
-					   id, elen, (unsigned long) left);
-			}
-			return ParseFailed;
-		}
-
-		switch (id) {
-		case WLAN_EID_SSID:
-			elems->ssid = pos;
-			elems->ssid_len = elen;
-			break;
-		case WLAN_EID_SUPP_RATES:
-			elems->supp_rates = pos;
-			elems->supp_rates_len = elen;
-			break;
-		case WLAN_EID_FH_PARAMS:
-			elems->fh_params = pos;
-			elems->fh_params_len = elen;
-			break;
-		case WLAN_EID_DS_PARAMS:
-			elems->ds_params = pos;
-			elems->ds_params_len = elen;
-			break;
-		case WLAN_EID_CF_PARAMS:
-			elems->cf_params = pos;
-			elems->cf_params_len = elen;
-			break;
-		case WLAN_EID_TIM:
-			elems->tim = pos;
-			elems->tim_len = elen;
-			break;
-		case WLAN_EID_IBSS_PARAMS:
-			elems->ibss_params = pos;
-			elems->ibss_params_len = elen;
-			break;
-		case WLAN_EID_CHALLENGE:
-			elems->challenge = pos;
-			elems->challenge_len = elen;
-			break;
-		case WLAN_EID_ERP_INFO:
-			elems->erp_info = pos;
-			elems->erp_info_len = elen;
-			break;
-		case WLAN_EID_EXT_SUPP_RATES:
-			elems->ext_supp_rates = pos;
-			elems->ext_supp_rates_len = elen;
-			break;
-		case WLAN_EID_VENDOR_SPECIFIC:
-			if (rtw_ieee802_11_parse_vendor_specific(pos, elen,
-								 elems,
-								 show_errors))
-				unknown++;
-			break;
-		case WLAN_EID_RSN:
-			elems->rsn_ie = pos;
-			elems->rsn_ie_len = elen;
-			break;
-		case WLAN_EID_PWR_CAPABILITY:
-			elems->power_cap = pos;
-			elems->power_cap_len = elen;
-			break;
-		case WLAN_EID_SUPPORTED_CHANNELS:
-			elems->supp_channels = pos;
-			elems->supp_channels_len = elen;
-			break;
-		case WLAN_EID_MOBILITY_DOMAIN:
-			elems->mdie = pos;
-			elems->mdie_len = elen;
-			break;
-		case WLAN_EID_FAST_BSS_TRANSITION:
-			elems->ftie = pos;
-			elems->ftie_len = elen;
-			break;
-		case WLAN_EID_TIMEOUT_INTERVAL:
-			elems->timeout_int = pos;
-			elems->timeout_int_len = elen;
-			break;
-		case WLAN_EID_HT_CAPABILITY:
-			elems->ht_capabilities = pos;
-			elems->ht_capabilities_len = elen;
-			break;
-		case WLAN_EID_HT_OPERATION:
-			elems->ht_operation = pos;
-			elems->ht_operation_len = elen;
-			break;
-		default:
-			unknown++;
-			if (!show_errors)
-				break;
-			DBG_8723A("IEEE 802.11 element parse "
-				   "ignored unknown element (id =%d elen =%d)\n",
-				   id, elen);
-			break;
-		}
-
-		left -= elen;
-		pos += elen;
-	}
-
-	if (left)
-		return ParseFailed;
-
-	return unknown ? ParseUnknown : ParseOK;
-}
-
-static u8 key_char2num(u8 ch)
-{
-	if ((ch >= '0') && (ch <= '9'))
-		return ch - '0';
-	else if ((ch >= 'a') && (ch <= 'f'))
-		return ch - 'a' + 10;
-	else if ((ch >= 'A') && (ch <= 'F'))
-		return ch - 'A' + 10;
-	else
-		return 0xff;
-}
-
-u8 str_2char2num23a(u8 hch, u8 lch)
-{
-	return (key_char2num(hch) * 10) + key_char2num(lch);
-}
-
-u8 key_2char2num23a(u8 hch, u8 lch)
-{
-	return (key_char2num(hch) << 4) | key_char2num(lch);
-}
-
-void rtw_macaddr_cfg23a(u8 *mac_addr)
-{
-	u8 mac[ETH_ALEN];
-	if (!mac_addr)
-		return;
-
-	memcpy(mac, mac_addr, ETH_ALEN);
-
-	if (is_broadcast_ether_addr(mac) || is_zero_ether_addr(mac)) {
-		mac[0] = 0x00;
-		mac[1] = 0xe0;
-		mac[2] = 0x4c;
-		mac[3] = 0x87;
-		mac[4] = 0x00;
-		mac[5] = 0x00;
-		/*  use default mac addresss */
-		memcpy(mac_addr, mac, ETH_ALEN);
-		DBG_8723A("MAC Address from efuse error, assign default "
-			  "one !!!\n");
-	}
-	DBG_8723A("rtw_macaddr_cfg23a MAC Address  = "MAC_FMT"\n",
-		  MAC_ARG(mac_addr));
-}
-
-void dump_ies23a(u8 *buf, u32 buf_len) {
-	u8* pos = (u8*)buf;
-	u8 id, len;
-
-	while (pos-buf <= buf_len) {
-		id = *pos;
-		len = *(pos + 1);
-
-		DBG_8723A("%s ID:%u, LEN:%u\n", __func__, id, len);
-#ifdef CONFIG_8723AU_P2P
-		dump_p2p_ie23a(pos, len);
-#endif
-		dump_wps_ie23a(pos, len);
-
-		pos += (2 + len);
-	}
-}
-
-void dump_wps_ie23a(u8 *ie, u32 ie_len) {
-	u8* pos = (u8*)ie;
-	u16 id;
-	u16 len;
-
-	u8 *wps_ie;
-	uint wps_ielen;
-
-	wps_ie = rtw_get_wps_ie23a(ie, ie_len, NULL, &wps_ielen);
-	if (wps_ie != ie || wps_ielen == 0)
-		return;
-
-	pos+= 6;
-	while (pos-ie < ie_len) {
-		id = get_unaligned_be16(pos);
-		len = get_unaligned_be16(pos + 2);
-
-		DBG_8723A("%s ID:0x%04x, LEN:%u\n", __func__, id, len);
-
-		pos += (4 + len);
-	}
-}
-
-#ifdef CONFIG_8723AU_P2P
-void dump_p2p_ie23a(u8 *ie, u32 ie_len) {
-	u8* pos = (u8*)ie;
-	u8 id;
-	u16 len;
-
-	u8 *p2p_ie;
-	uint p2p_ielen;
-
-	p2p_ie = rtw_get_p2p_ie23a(ie, ie_len, NULL, &p2p_ielen);
-	if (p2p_ie != ie || p2p_ielen == 0)
-		return;
-
-	pos += 6;
-	while (pos-ie < ie_len) {
-		id = *pos;
-		len = get_unaligned_le16(pos+1);
-
-		DBG_8723A("%s ID:%u, LEN:%u\n", __func__, id, len);
-
-		pos+= (3+len);
-	}
-}
-
-/**
- * rtw_get_p2p_ie23a - Search P2P IE from a series of IEs
- * @in_ie: Address of IEs to search
- * @in_len: Length limit from in_ie
- * @p2p_ie: If not NULL and P2P IE is found, P2P IE will be copied to the
- *          buf starting from p2p_ie
- * @p2p_ielen: If not NULL and P2P IE is found, will set to the length of
- *             the entire P2P IE
- *
- * Returns: The address of the P2P IE found, or NULL
- */
-u8 *rtw_get_p2p_ie23a(u8 *in_ie, int in_len, u8 *p2p_ie, uint *p2p_ielen)
-{
-	uint cnt = 0;
-	u8 *p2p_ie_ptr;
-	u8 eid, p2p_oui[4]={0x50, 0x6F, 0x9A, 0x09};
-
-	if (p2p_ielen)
-		*p2p_ielen = 0;
-
-	while (cnt<in_len) {
-		eid = in_ie[cnt];
-		if ((in_len < 0) || (cnt > MAX_IE_SZ)) {
-			dump_stack();
-			return NULL;
-		}
-		if ((eid == _VENDOR_SPECIFIC_IE_) &&
-		    !memcmp(&in_ie[cnt + 2], p2p_oui, 4)) {
-			p2p_ie_ptr = in_ie + cnt;
-
-			if (p2p_ie != NULL) {
-				memcpy(p2p_ie, &in_ie[cnt],
-				       in_ie[cnt + 1] + 2);
-			}
-
-			if (p2p_ielen != NULL) {
-				*p2p_ielen = in_ie[cnt + 1] + 2;
-			}
-
-			return p2p_ie_ptr;
-
-			break;
-		} else {
-			cnt += in_ie[cnt + 1] + 2; /* goto next */
-		}
-	}
-
-	return NULL;
-}
-
-/**
- * rtw_get_p2p_attr23a - Search a specific P2P attribute from a given P2P IE
- * @p2p_ie: Address of P2P IE to search
- * @p2p_ielen: Length limit from p2p_ie
- * @target_attr_id: The attribute ID of P2P attribute to search
- * @buf_attr: If not NULL and the P2P attribute is found, P2P attribute will
- *            be copied to the buf starting from buf_attr
- * @len_attr: If not NULL and the P2P attribute is found, will set to the
- *            length of the entire P2P attribute
- *
- * Returns: the address of the specific WPS attribute found, or NULL
- */
-u8 *rtw_get_p2p_attr23a(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id,
-		     u8 *buf_attr, u32 *len_attr)
-{
-	u8 *attr_ptr = NULL;
-	u8 *target_attr_ptr = NULL;
-	u8 p2p_oui[4]={0x50, 0x6F, 0x9A, 0x09};
-
-	if (len_attr)
-		*len_attr = 0;
-
-	if (!p2p_ie || (p2p_ie[0] != _VENDOR_SPECIFIC_IE_) ||
-	    memcmp(p2p_ie + 2, p2p_oui, 4)) {
-		return attr_ptr;
-	}
-
-	/*  6 = 1(Element ID) + 1(Length) + 3 (OUI) + 1(OUI Type) */
-	attr_ptr = p2p_ie + 6; /* goto first attr */
-
-	while (attr_ptr - p2p_ie < p2p_ielen) {
-		/*  3 = 1(Attribute ID) + 2(Length) */
-		u8 attr_id = *attr_ptr;
-		u16 attr_data_len = get_unaligned_le16(attr_ptr + 1);
-		u16 attr_len = attr_data_len + 3;
-
-		/* DBG_8723A("%s attr_ptr:%p, id:%u, length:%u\n", __func__, attr_ptr, attr_id, attr_data_len); */
-		if (attr_id == target_attr_id) {
-			target_attr_ptr = attr_ptr;
-
-			if (buf_attr)
-				memcpy(buf_attr, attr_ptr, attr_len);
-
-			if (len_attr)
-				*len_attr = attr_len;
-
-			break;
-		} else {
-			attr_ptr += attr_len; /* goto next */
-		}
-	}
-
-	return target_attr_ptr;
-}
-
-/**
- * rtw_get_p2p_attr23a_content - Search a specific P2P attribute content from
- * a given P2P IE
- * @p2p_ie: Address of P2P IE to search
- * @p2p_ielen: Length limit from p2p_ie
- * @target_attr_id: The attribute ID of P2P attribute to search
- * @buf_content: If not NULL and the P2P attribute is found, P2P attribute
- *               content will be copied to the buf starting from buf_content
- * @len_content: If not NULL and the P2P attribute is found, will set to the
- *               length of the P2P attribute content
- *
- * Returns: the address of the specific P2P attribute content found, or NULL
- */
-u8 *rtw_get_p2p_attr23a_content(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id,
-			     u8 *buf_content, uint *len_content)
-{
-	u8 *attr_ptr;
-	u32 attr_len;
-
-	if (len_content)
-		*len_content = 0;
-
-	attr_ptr = rtw_get_p2p_attr23a(p2p_ie, p2p_ielen, target_attr_id,
-				    NULL, &attr_len);
-
-	if (attr_ptr && attr_len) {
-		if (buf_content)
-			memcpy(buf_content, attr_ptr + 3, attr_len - 3);
-
-		if (len_content)
-			*len_content = attr_len - 3;
-
-		return attr_ptr+3;
-	}
-
-	return NULL;
-}
-
-u32 rtw_set_p2p_attr_content23a(u8 *pbuf, u8 attr_id, u16 attr_len, u8 *pdata_attr)
-{
-	u32 a_len;
-
-	*pbuf = attr_id;
-
-	/* u16*)(pbuf + 1) = cpu_to_le16(attr_len); */
-	put_unaligned_le16(attr_len, pbuf + 1);
-
-	if (pdata_attr)
-		memcpy(pbuf + 3, pdata_attr, attr_len);
-
-	a_len = attr_len + 3;
-
-	return a_len;
-}
-
-static uint rtw_p2p_attr_remove(u8 *ie, uint ielen_ori, u8 attr_id)
-{
-	u8 *target_attr;
-	u32 target_attr_len;
-	uint ielen = ielen_ori;
-
-	while(1) {
-		target_attr = rtw_get_p2p_attr23a(ie, ielen, attr_id, NULL,
-					     &target_attr_len);
-		if (target_attr && target_attr_len) {
-			u8 *next_attr = target_attr+target_attr_len;
-			uint remain_len = ielen-(next_attr-ie);
-			/* dump_ies23a(ie, ielen); */
-
-			memset(target_attr, 0, target_attr_len);
-			memcpy(target_attr, next_attr, remain_len);
-			memset(target_attr+remain_len, 0, target_attr_len);
-			*(ie + 1) -= target_attr_len;
-			ielen -= target_attr_len;
-		} else {
-			/* if (index>0) */
-			/*	dump_ies23a(ie, ielen); */
-			break;
-		}
-	}
-
-	return ielen;
-}
-
-void rtw_wlan_bssid_ex_remove_p2p_attr23a(struct wlan_bssid_ex *bss_ex, u8 attr_id)
-{
-	u8 *p2p_ie;
-	uint p2p_ielen, p2p_ielen_ori;
-
-	if ((p2p_ie = rtw_get_p2p_ie23a(bss_ex->IEs + _FIXED_IE_LENGTH_,
-				     bss_ex->IELength - _FIXED_IE_LENGTH_,
-				     NULL, &p2p_ielen_ori))) {
-		p2p_ielen = rtw_p2p_attr_remove(p2p_ie, p2p_ielen_ori, attr_id);
-		if (p2p_ielen != p2p_ielen_ori) {
-			u8 *next_ie_ori = p2p_ie+p2p_ielen_ori;
-			u8 *next_ie = p2p_ie+p2p_ielen;
-			uint remain_len;
-			remain_len = bss_ex->IELength-(next_ie_ori-bss_ex->IEs);
-
-			memcpy(next_ie, next_ie_ori, remain_len);
-			memset(next_ie+remain_len, 0, p2p_ielen_ori-p2p_ielen);
-			bss_ex->IELength -= p2p_ielen_ori-p2p_ielen;
-		}
-	}
-}
-
-#endif /* CONFIG_8723AU_P2P */
-
-#ifdef CONFIG_8723AU_P2P
-int rtw_get_wfd_ie(u8 *in_ie, int in_len, u8 *wfd_ie, uint *wfd_ielen)
-{
-	int match;
-	uint cnt = 0;
-	u8 eid, wfd_oui[4] = {0x50, 0x6F, 0x9A, 0x0A};
-
-	match = false;
-
-	if (in_len < 0) {
-		return match;
-	}
-
-	while (cnt < in_len)
-	{
-		eid = in_ie[cnt];
-
-		if ((eid == _VENDOR_SPECIFIC_IE_) &&
-		    !memcmp(&in_ie[cnt+2], wfd_oui, 4)) {
-			if (wfd_ie != NULL) {
-				memcpy(wfd_ie, &in_ie[cnt], in_ie[cnt + 1] + 2);
-
-			} else {
-				if (wfd_ielen != NULL) {
-					*wfd_ielen = 0;
-				}
-			}
-
-			if (wfd_ielen != NULL) {
-				*wfd_ielen = in_ie[cnt + 1] + 2;
-			}
-
-			cnt += in_ie[cnt + 1] + 2;
-
-			match = true;
-			break;
-		} else {
-			cnt += in_ie[cnt + 1] +2; /* goto next */
-		}
-	}
-
-	if (match == true) {
-		match = cnt;
-	}
-
-	return match;
-}
-
-/*	attr_content: The output buffer, contains the "body field" of
-	WFD attribute. */
-/*	attr_contentlen: The data length of the "body field" of WFD
-	attribute. */
-int rtw_get_wfd_attr_content(u8 *wfd_ie, uint wfd_ielen, u8 target_attr_id,
-			     u8 *attr_content, uint *attr_contentlen)
-{
-	int match;
-	uint cnt = 0;
-	u8 attr_id, wfd_oui[4] = {0x50, 0x6F, 0x9A, 0x0A};
-
-	match = false;
-
-	if ((wfd_ie[0] != _VENDOR_SPECIFIC_IE_) ||
-	    memcmp(wfd_ie + 2, wfd_oui, 4)) {
-		return match;
-	}
-
-	/*	1 (WFD IE) + 1 (Length) + 3 (OUI) + 1 (OUI Type) */
-	cnt = 6;
-	while (cnt < wfd_ielen) {
-		u16 attrlen = get_unaligned_be16(wfd_ie + cnt + 1);
-
-		attr_id = wfd_ie[cnt];
-		if (attr_id == target_attr_id) {
-			/*	3 -> 1 byte for attribute ID field, 2
-				bytes for length field */
-			if (attr_content)
-				memcpy(attr_content, &wfd_ie[cnt + 3], attrlen);
-
-			if (attr_contentlen)
-				*attr_contentlen = attrlen;
-
-			cnt += attrlen + 3;
-
-			match = true;
-			break;
-		} else {
-			cnt += attrlen + 3; /* goto next */
-		}
-	}
-
-	return match;
-}
-#endif /*  CONFIG_8723AU_P2P */
-
-/* Baron adds to avoid FreeBSD warning */
-int ieee80211_is_empty_essid23a(const char *essid, int essid_len)
-{
-	/* Single white space is for Linksys APs */
-	if (essid_len == 1 && essid[0] == ' ')
-		return 1;
-
-	/* Otherwise, if the entire essid is 0, we assume it is hidden */
-	while (essid_len) {
-		essid_len--;
-		if (essid[essid_len] != '\0')
-			return 0;
-	}
-
-	return 1;
-}
-
 static int rtw_get_cipher_info(struct wlan_network *pnetwork)
 {
-	u32 wpa_ielen;
-	unsigned char *pbuf;
+	const u8 *pbuf;
 	int group_cipher = 0, pairwise_cipher = 0, is8021x = 0;
 	int ret = _FAIL;
-	int r;
-	pbuf = rtw_get_wpa_ie23a(&pnetwork->network.IEs[12], &wpa_ielen,
-			      pnetwork->network.IELength - 12);
+	int r, offset, plen;
+	char *pie;
 
-	if (pbuf && (wpa_ielen > 0)) {
+	offset = offsetof(struct ieee80211_mgmt, u.beacon.variable) -
+		offsetof(struct ieee80211_mgmt, u);
+	pie = &pnetwork->network.IEs[offset];
+	plen = pnetwork->network.IELength - offset;
+
+	pbuf = cfg80211_find_vendor_ie(WLAN_OUI_MICROSOFT,
+				       WLAN_OUI_TYPE_MICROSOFT_WPA, pie, plen);
+
+	if (pbuf && pbuf[1] > 0) {
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-			 ("rtw_get_cipher_info: wpa_ielen: %d", wpa_ielen));
-		r = rtw_parse_wpa_ie23a(pbuf, wpa_ielen + 2, &group_cipher,
+			 ("rtw_get_cipher_info: wpa_ielen: %d", pbuf[1]));
+		r = rtw_parse_wpa_ie23a(pbuf, pbuf[1] + 2, &group_cipher,
 				     &pairwise_cipher, &is8021x);
 		if (r == _SUCCESS) {
 			pnetwork->BcnInfo.pairwise_cipher = pairwise_cipher;
@@ -1630,13 +871,12 @@ static int rtw_get_cipher_info(struct wlan_network *pnetwork)
 			ret = _SUCCESS;
 		}
 	} else {
-		pbuf = rtw_get_wpa2_ie23a(&pnetwork->network.IEs[12], &wpa_ielen,
-				       pnetwork->network.IELength - 12);
+		pbuf = cfg80211_find_ie(WLAN_EID_RSN, pie, plen);
 
-		if (pbuf && (wpa_ielen > 0)) {
+		if (pbuf && pbuf[1] > 0) {
 			RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
 				 ("get RSN IE\n"));
-			r = rtw_parse_wpa2_ie23a(pbuf, wpa_ielen + 2,
+			r = rtw_parse_wpa2_ie23a(pbuf, pbuf[1] + 2,
 					      &group_cipher, &pairwise_cipher,
 					      &is8021x);
 			if (r == _SUCCESS) {
@@ -1663,23 +903,22 @@ static int rtw_get_cipher_info(struct wlan_network *pnetwork)
 
 void rtw_get_bcn_info23a(struct wlan_network *pnetwork)
 {
-	unsigned short cap = 0;
+	unsigned short cap;
 	u8 bencrypt = 0;
 	/* u8 wpa_ie[255], rsn_ie[255]; */
 	u16 wpa_len = 0, rsn_len = 0;
-	struct HT_info_element *pht_info = NULL;
-	struct ieee80211_ht_cap *pht_cap = NULL;
-	unsigned int		len;
-	unsigned char		*p;
+	struct HT_info_element *pht_info;
+	struct ieee80211_ht_cap *pht_cap;
+	const u8 *p;
 
-	memcpy(&cap, rtw_get_capability23a_from_ie(pnetwork->network.IEs), 2);
-	cap = le16_to_cpu(cap);
+	cap = get_unaligned_le16(
+		rtw_get_capability23a_from_ie(pnetwork->network.IEs));
 	if (cap & WLAN_CAPABILITY_PRIVACY) {
 		bencrypt = 1;
 		pnetwork->network.Privacy = 1;
-	} else {
+	} else
 		pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_OPENSYS;
-	}
+
 	rtw_get_sec_ie23a(pnetwork->network.IEs, pnetwork->network.IELength,
 		       NULL, &rsn_len, NULL, &wpa_len);
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
@@ -1693,11 +932,11 @@ void rtw_get_bcn_info23a(struct wlan_network *pnetwork)
 		 ("rtw_get_bcn_info23a: wpa_len =%d rsn_len =%d\n",
 		  wpa_len, rsn_len));
 
-	if (rsn_len > 0) {
+	if (rsn_len > 0)
 		pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WPA2;
-	} else if (wpa_len > 0) {
+	else if (wpa_len > 0)
 		pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WPA;
-	} else {
+	else {
 		if (bencrypt)
 			pnetwork->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_WEP;
 	}
@@ -1711,25 +950,24 @@ void rtw_get_bcn_info23a(struct wlan_network *pnetwork)
 
 	/* get bwmode and ch_offset */
 	/* parsing HT_CAP_IE */
-	p = rtw_get_ie23a(pnetwork->network.IEs + _FIXED_IE_LENGTH_,
-		       _HT_CAPABILITY_IE_, &len,
-		       pnetwork->network.IELength - _FIXED_IE_LENGTH_);
-	if (p && len > 0) {
+	p = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY,
+			     pnetwork->network.IEs + _FIXED_IE_LENGTH_,
+			     pnetwork->network.IELength - _FIXED_IE_LENGTH_);
+	if (p && p[1] > 0) {
 		pht_cap = (struct ieee80211_ht_cap *)(p + 2);
 		pnetwork->BcnInfo.ht_cap_info = pht_cap->cap_info;
-	} else {
+	} else
 		pnetwork->BcnInfo.ht_cap_info = 0;
-	}
+
 	/* parsing HT_INFO_IE */
-	p = rtw_get_ie23a(pnetwork->network.IEs + _FIXED_IE_LENGTH_,
-		       _HT_ADD_INFO_IE_, &len,
+	p = cfg80211_find_ie(WLAN_EID_HT_OPERATION,
+			     pnetwork->network.IEs + _FIXED_IE_LENGTH_,
 		       pnetwork->network.IELength - _FIXED_IE_LENGTH_);
-	if (p && len > 0) {
+	if (p && p[1] > 0) {
 		pht_info = (struct HT_info_element *)(p + 2);
 		pnetwork->BcnInfo.ht_info_infos_0 = pht_info->infos[0];
-	} else {
+	} else
 		pnetwork->BcnInfo.ht_info_infos_0 = 0;
-	}
 }
 
 /* show MCS rate, unit: 100Kbps */
@@ -1803,37 +1041,6 @@ u16 rtw_mcs_rate23a(u8 rf_type, u8 bw_40MHz, u8 short_GI_20, u8 short_GI_40,
 	return max_rate;
 }
 
-int rtw_action_frame_parse23a(const u8 *frame, u32 frame_len, u8* category,
-			   u8 *action)
-{
-	const u8 *frame_body = frame + sizeof(struct ieee80211_hdr_3addr);
-	u16 fc;
-	u8 c, a = 0;
-
-	fc = le16_to_cpu(((struct ieee80211_hdr_3addr *)frame)->frame_control);
-
-	if ((fc & (IEEE80211_FCTL_FTYPE|IEEE80211_FCTL_STYPE)) !=
-	    (IEEE80211_FTYPE_MGMT|IEEE80211_STYPE_ACTION)) {
-		return false;
-	}
-
-	c = frame_body[0];
-
-	switch (c) {
-	case WLAN_CATEGORY_VENDOR_SPECIFIC: /* vendor-specific */
-		break;
-	default:
-		a = frame_body[1];
-	}
-
-	if (category)
-		*category = c;
-	if (action)
-		*action = a;
-
-	return true;
-}
-
 static const char *_action_public_str23a[] = {
 	"ACT_PUB_BSSCOEXIST",
 	"ACT_PUB_DSE_ENABLE",
diff --git a/drivers/staging/rtl8723au/core/rtw_io.c b/drivers/staging/rtl8723au/core/rtw_io.c
deleted file mode 100644
index 1cae8d7659b..00000000000
--- a/drivers/staging/rtl8723au/core/rtw_io.c
+++ /dev/null
@@ -1,266 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
- *
- * 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.
- *
- * 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.
- *
- ******************************************************************************/
-/*
-
-The purpose of rtw_io.c
-
-a. provides the API
-
-b. provides the protocol engine
-
-c. provides the software interface between caller and the hardware interface
-
-Compiler Flag Option:
-
-1. For USB:
-   a. USE_ASYNC_IRP: Both sync/async operations are provided.
-
-Only sync read/rtw_write_mem operations are provided.
-
-jackson@realtek.com.tw
-
-*/
-
-#define _RTW_IO_C_
-#include <osdep_service.h>
-#include <drv_types.h>
-#include <rtw_io.h>
-#include <osdep_intf.h>
-
-#include <usb_ops.h>
-
-u8 _rtw_read823a(struct rtw_adapter *adapter, u32 addr)
-{
-	u8 r_val;
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl *pintfhdl = &pio_priv->intf;
-
-	r_val = pintfhdl->io_ops._read8(pintfhdl, addr);
-
-	return r_val;
-}
-
-u16 _rtw_read1623a(struct rtw_adapter *adapter, u32 addr)
-{
-	u16 r_val;
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl *pintfhdl = &pio_priv->intf;
-
-	r_val = pintfhdl->io_ops._read16(pintfhdl, addr);
-
-	return le16_to_cpu(r_val);
-}
-
-u32 _rtw_read3223a(struct rtw_adapter *adapter, u32 addr)
-{
-	u32 r_val;
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl *pintfhdl = &pio_priv->intf;
-
-	r_val = pintfhdl->io_ops._read32(pintfhdl, addr);
-
-	return le32_to_cpu(r_val);
-}
-
-int _rtw_write823a(struct rtw_adapter *adapter, u32 addr, u8 val)
-{
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl		*pintfhdl = &pio_priv->intf;
-	int ret;
-
-	ret = pintfhdl->io_ops._write8(pintfhdl, addr, val);
-
-	return RTW_STATUS_CODE23a(ret);
-}
-
-int _rtw_write1623a(struct rtw_adapter *adapter, u32 addr, u16 val)
-{
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl		*pintfhdl = &pio_priv->intf;
-	int ret;
-
-	val = cpu_to_le16(val);
-	ret = pintfhdl->io_ops._write16(pintfhdl, addr, val);
-
-	return RTW_STATUS_CODE23a(ret);
-}
-int _rtw_write3223a(struct rtw_adapter *adapter, u32 addr, u32 val)
-{
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl *pintfhdl = &pio_priv->intf;
-	int ret;
-
-	val = cpu_to_le32(val);
-	ret = pintfhdl->io_ops._write32(pintfhdl, addr, val);
-
-	return RTW_STATUS_CODE23a(ret);
-}
-
-int _rtw_writeN23a(struct rtw_adapter *adapter, u32 addr , u32 length , u8 *pdata)
-{
-	struct io_priv *pio_priv = &adapter->iopriv;
-        struct intf_hdl *pintfhdl = (struct intf_hdl*)&pio_priv->intf;
-	int ret;
-
-	ret = pintfhdl->io_ops._writeN(pintfhdl, addr, length, pdata);
-
-	return RTW_STATUS_CODE23a(ret);
-}
-int _rtw_write823a_async23a(struct rtw_adapter *adapter, u32 addr, u8 val)
-{
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl *pintfhdl = &pio_priv->intf;
-	int ret;
-
-	ret = pintfhdl->io_ops._write8_async(pintfhdl, addr, val);
-
-	return RTW_STATUS_CODE23a(ret);
-}
-int _rtw_write1623a_async(struct rtw_adapter *adapter, u32 addr, u16 val)
-{
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl *pintfhdl = &pio_priv->intf;
-	int ret;
-
-	val = cpu_to_le16(val);
-	ret = pintfhdl->io_ops._write16_async(pintfhdl, addr, val);
-
-	return RTW_STATUS_CODE23a(ret);
-}
-int _rtw_write3223a_async23a(struct rtw_adapter *adapter, u32 addr, u32 val)
-{
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl *pintfhdl = &pio_priv->intf;
-	int ret;
-
-	val = cpu_to_le32(val);
-	ret = pintfhdl->io_ops._write32_async(pintfhdl, addr, val);
-
-	return RTW_STATUS_CODE23a(ret);
-}
-
-void _rtw_read_mem23a(struct rtw_adapter *adapter, u32 addr, u32 cnt, u8 *pmem)
-{
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl *pintfhdl = &pio_priv->intf;
-
-	if ((adapter->bDriverStopped == true) ||
-	    (adapter->bSurpriseRemoved == true)) {
-	     RT_TRACE(_module_rtl871x_io_c_, _drv_info_,
-		      ("rtw_read_mem:bDriverStopped(%d) OR "
-		       "bSurpriseRemoved(%d)", adapter->bDriverStopped,
-		       adapter->bSurpriseRemoved));
-	     return;
-	}
-
-	pintfhdl->io_ops._read_mem(pintfhdl, addr, cnt, pmem);
-}
-
-void _rtw_write_mem23a(struct rtw_adapter *adapter, u32 addr, u32 cnt, u8 *pmem)
-{
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl *pintfhdl = &pio_priv->intf;
-
-	pintfhdl->io_ops._write_mem(pintfhdl, addr, cnt, pmem);
-}
-
-void _rtw_read_port23a(struct rtw_adapter *adapter, u32 addr, u32 cnt,
-		    struct recv_buf *rbuf)
-{
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl *pintfhdl = &pio_priv->intf;
-
-	if ((adapter->bDriverStopped == true) ||
-	    (adapter->bSurpriseRemoved == true)) {
-	     RT_TRACE(_module_rtl871x_io_c_, _drv_info_,
-		      ("rtw_read_port:bDriverStopped(%d) OR "
-		       "bSurpriseRemoved(%d)", adapter->bDriverStopped,
-		       adapter->bSurpriseRemoved));
-	     return;
-	}
-
-	pintfhdl->io_ops._read_port(pintfhdl, addr, cnt, rbuf);
-}
-
-void _rtw_read_port23a_cancel(struct rtw_adapter *adapter)
-{
-	void (*_read_port_cancel)(struct intf_hdl *pintfhdl);
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl *pintfhdl = &pio_priv->intf;
-
-	_read_port_cancel = pintfhdl->io_ops._read_port_cancel;
-
-	if (_read_port_cancel)
-		_read_port_cancel(pintfhdl);
-}
-
-u32 _rtw_write_port23a(struct rtw_adapter *adapter, u32 addr, u32 cnt,
-		    struct xmit_buf *xbuf)
-{
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl *pintfhdl = &pio_priv->intf;
-	u32 ret = _SUCCESS;
-
-	ret = pintfhdl->io_ops._write_port(pintfhdl, addr, cnt, xbuf);
-
-	return ret;
-}
-
-u32 _rtw_write_port23a_and_wait23a(struct rtw_adapter *adapter, u32 addr, u32 cnt,
-			     struct xmit_buf *pxmitbuf, int timeout_ms)
-{
-	int ret = _SUCCESS;
-	struct submit_ctx sctx;
-
-	rtw_sctx_init23a(&sctx, timeout_ms);
-	pxmitbuf->sctx = &sctx;
-
-	ret = _rtw_write_port23a(adapter, addr, cnt, pxmitbuf);
-
-	if (ret == _SUCCESS)
-		ret = rtw_sctx_wait23a(&sctx);
-
-	return ret;
-}
-
-void _rtw_write_port23a_cancel(struct rtw_adapter *adapter)
-{
-	void (*_write_port_cancel)(struct intf_hdl *pintfhdl);
-	struct io_priv *pio_priv = &adapter->iopriv;
-	struct intf_hdl *pintfhdl = &pio_priv->intf;
-
-	_write_port_cancel = pintfhdl->io_ops._write_port_cancel;
-
-	if (_write_port_cancel)
-		_write_port_cancel(pintfhdl);
-}
-
-int rtw_init_io_priv23a(struct rtw_adapter *padapter,
-		     void (*set_intf_ops)(struct _io_ops *pops))
-{
-	struct io_priv	*piopriv = &padapter->iopriv;
-	struct intf_hdl *pintf = &piopriv->intf;
-
-	if (set_intf_ops == NULL)
-		return _FAIL;
-
-	piopriv->padapter = padapter;
-	pintf->padapter = padapter;
-	pintf->pintf_dev = adapter_to_dvobj(padapter);
-
-	set_intf_ops(&pintf->io_ops);
-
-	return _SUCCESS;
-}
diff --git a/drivers/staging/rtl8723au/core/rtw_ioctl_set.c b/drivers/staging/rtl8723au/core/rtw_ioctl_set.c
index 30d7185e563..cf897c723f4 100644
--- a/drivers/staging/rtl8723au/core/rtw_ioctl_set.c
+++ b/drivers/staging/rtl8723au/core/rtw_ioctl_set.c
@@ -19,17 +19,16 @@
 #include <rtw_ioctl_set.h>
 #include <hal_intf.h>
 
-#include <usb_osintf.h>
 #include <usb_ops.h>
 #include <linux/ieee80211.h>
 
-u8 rtw_do_join23a(struct rtw_adapter *padapter)
+int rtw_do_join23a(struct rtw_adapter *padapter)
 {
 	struct list_head *plist, *phead;
 	u8* pibss = NULL;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct rtw_queue *queue = &pmlmepriv->scanned_queue;
-	u8 ret = _SUCCESS;
+	int ret = _SUCCESS;
 
 	spin_lock_bh(&pmlmepriv->scanned_queue.lock);
 	phead = get_list_head(queue);
@@ -45,7 +44,7 @@ u8 rtw_do_join23a(struct rtw_adapter *padapter)
 
 	pmlmepriv->to_join = true;
 
-	if (_rtw_queue_empty23a(queue) == true) {
+	if (list_empty(&queue->queue)) {
 		spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
 		_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
 
@@ -54,7 +53,7 @@ u8 rtw_do_join23a(struct rtw_adapter *padapter)
 		/* we try to issue sitesurvey firstly */
 
 		if (pmlmepriv->LinkDetectInfo.bBusyTraffic == false ||
-		    rtw_to_roaming(padapter) > 0) {
+		    padapter->mlmepriv.to_roaming > 0) {
 			RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
 				 ("rtw_do_join23a(): site survey if scanned_queue "
 				  "is empty\n."));
@@ -83,7 +82,7 @@ u8 rtw_do_join23a(struct rtw_adapter *padapter)
 			mod_timer(&pmlmepriv->assoc_timer,
 				  jiffies + msecs_to_jiffies(MAX_JOIN_TIMEOUT));
 		} else {
-			if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true) {
+			if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
 				struct wlan_bssid_ex *pdev_network;
 				/*  submit createbss_cmd to change to a
 				    ADHOC_MASTER */
@@ -130,7 +129,7 @@ u8 rtw_do_join23a(struct rtw_adapter *padapter)
 				   queue */
 				/* we try to issue sitesurvey firstly */
 				if (pmlmepriv->LinkDetectInfo.bBusyTraffic ==
-				    false || rtw_to_roaming(padapter) > 0) {
+				    false || padapter->mlmepriv.to_roaming > 0){
 					/* DBG_8723A("rtw_do_join23a() when   no "
 					   "desired bss in scanning queue\n");
 					*/
@@ -152,9 +151,10 @@ exit:
 	return ret;
 }
 
-u8 rtw_set_802_11_ssid23a(struct rtw_adapter* padapter, struct cfg80211_ssid *ssid)
+int rtw_set_802_11_ssid23a(struct rtw_adapter* padapter,
+			   struct cfg80211_ssid *ssid)
 {
-	u8 status = _SUCCESS;
+	int status = _SUCCESS;
 	u32 cur_time = 0;
 
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
@@ -175,22 +175,19 @@ u8 rtw_set_802_11_ssid23a(struct rtw_adapter* padapter, struct cfg80211_ssid *ss
 	spin_lock_bh(&pmlmepriv->lock);
 
 	DBG_8723A("Set SSID under fw_state = 0x%08x\n", get_fwstate(pmlmepriv));
-	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true) {
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY))
 		goto handle_tkip_countermeasure;
-	} else if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == true) {
+	else if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING))
 		goto release_mlme_lock;
-	}
 
-	if (check_fwstate(pmlmepriv, _FW_LINKED|WIFI_ADHOC_MASTER_STATE) == true)
-	{
+	if (check_fwstate(pmlmepriv, _FW_LINKED|WIFI_ADHOC_MASTER_STATE)) {
 		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
 			 ("set_ssid: _FW_LINKED||WIFI_ADHOC_MASTER_STATE\n"));
 
 		if ((pmlmepriv->assoc_ssid.ssid_len == ssid->ssid_len) &&
 		    !memcmp(&pmlmepriv->assoc_ssid.ssid, ssid->ssid,
 			    ssid->ssid_len)) {
-			if ((check_fwstate(pmlmepriv, WIFI_STATION_STATE) == false))
-			{
+			if (!check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
 				RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
 					 ("Set SSID is the same ssid, fw_state = 0x%08x\n",
 					  get_fwstate(pmlmepriv)));
@@ -200,12 +197,12 @@ u8 rtw_set_802_11_ssid23a(struct rtw_adapter* padapter, struct cfg80211_ssid *ss
 					/* if in WIFI_ADHOC_MASTER_STATE | WIFI_ADHOC_STATE, create bss or rejoin again */
 					rtw_disassoc_cmd23a(padapter, 0, true);
 
-					if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
+					if (check_fwstate(pmlmepriv, _FW_LINKED))
 						rtw_indicate_disconnect23a(padapter);
 
 					rtw_free_assoc_resources23a(padapter, 1);
 
-					if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) {
+					if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
 						_clr_fwstate_(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
 						set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
 					}
@@ -228,12 +225,12 @@ u8 rtw_set_802_11_ssid23a(struct rtw_adapter* padapter, struct cfg80211_ssid *ss
 
 			rtw_disassoc_cmd23a(padapter, 0, true);
 
-			if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
+			if (check_fwstate(pmlmepriv, _FW_LINKED))
 				rtw_indicate_disconnect23a(padapter);
 
 			rtw_free_assoc_resources23a(padapter, 1);
 
-			if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) {
+			if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
 				_clr_fwstate_(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
 				set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
 			}
@@ -260,12 +257,10 @@ handle_tkip_countermeasure:
 	memcpy(&pmlmepriv->assoc_ssid, ssid, sizeof(struct cfg80211_ssid));
 	pmlmepriv->assoc_by_bssid = false;
 
-	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true) {
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY))
 		pmlmepriv->to_join = true;
-	}
-	else {
+	else
 		status = rtw_do_join23a(padapter);
-	}
 
 release_mlme_lock:
 	spin_unlock_bh(&pmlmepriv->lock);
@@ -279,108 +274,23 @@ exit:
 	return status;
 }
 
-u8 rtw_set_802_11_infrastructure_mode23a(struct rtw_adapter* padapter,
-	enum ndis_802_11_net_infra networktype)
-{
-	struct	mlme_priv	*pmlmepriv = &padapter->mlmepriv;
-	struct	wlan_network	*cur_network = &pmlmepriv->cur_network;
-	enum ndis_802_11_net_infra* pold_state = &cur_network->network.InfrastructureMode;
-
-
-
-	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_notice_,
-		 ("+rtw_set_802_11_infrastructure_mode23a: old =%d new =%d fw_state = 0x%08x\n",
-		  *pold_state, networktype, get_fwstate(pmlmepriv)));
-
-	if (*pold_state != networktype)
-	{
-		spin_lock_bh(&pmlmepriv->lock);
-
-		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, (" change mode!"));
-		/* DBG_8723A("change mode, old_mode =%d, new_mode =%d, fw_state = 0x%x\n", *pold_state, networktype, get_fwstate(pmlmepriv)); */
-
-		if (*pold_state == Ndis802_11APMode)
-		{
-			/* change to other mode from Ndis802_11APMode */
-			cur_network->join_res = -1;
-
-#ifdef CONFIG_8723AU_AP_MODE
-			stop_ap_mode23a(padapter);
-#endif
-		}
-
-		if ((check_fwstate(pmlmepriv, _FW_LINKED) == true) ||(*pold_state == Ndis802_11IBSS))
-			rtw_disassoc_cmd23a(padapter, 0, true);
-
-		if ((check_fwstate(pmlmepriv, _FW_LINKED) == true) ||
-			(check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true))
-			rtw_free_assoc_resources23a(padapter, 1);
-
-		if ((*pold_state == Ndis802_11Infrastructure) ||(*pold_state == Ndis802_11IBSS))
-	       {
-			if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-			{
-				rtw_indicate_disconnect23a(padapter); /* will clr Linked_state; before this function, we must have chked whether  issue dis-assoc_cmd or not */
-			}
-	       }
-
-		*pold_state = networktype;
-
-		_clr_fwstate_(pmlmepriv, ~WIFI_NULL_STATE);
-
-		switch (networktype)
-		{
-			case Ndis802_11IBSS:
-				set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
-				break;
-
-			case Ndis802_11Infrastructure:
-				set_fwstate(pmlmepriv, WIFI_STATION_STATE);
-				break;
-
-			case Ndis802_11APMode:
-				set_fwstate(pmlmepriv, WIFI_AP_STATE);
-#ifdef CONFIG_8723AU_AP_MODE
-				start_ap_mode23a(padapter);
-				/* rtw_indicate_connect23a(padapter); */
-#endif
-
-				break;
-
-			case Ndis802_11AutoUnknown:
-			case Ndis802_11InfrastructureMax:
-				break;
-		}
-
-		/* SecClearAllKeys(adapter); */
-
-		/* RT_TRACE(COMP_OID_SET, DBG_LOUD, ("set_infrastructure: fw_state:%x after changing mode\n", */
-		/*									get_fwstate(pmlmepriv))); */
-
-		spin_unlock_bh(&pmlmepriv->lock);
-	}
-
-
-
-	return true;
-}
-
-u8 rtw_set_802_11_bssid23a_list_scan(struct rtw_adapter *padapter,
-				  struct cfg80211_ssid *pssid, int ssid_max_num)
+int rtw_set_802_11_bssid23a_list_scan(struct rtw_adapter *padapter,
+				      struct cfg80211_ssid *pssid,
+				      int ssid_max_num)
 {
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	u8 res = true;
+	int res = _SUCCESS;
 
 	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
 		 ("+rtw_set_802_11_bssid23a_list_scan(), fw_state =%x\n",
 		  get_fwstate(pmlmepriv)));
 
 	if (!padapter) {
-		res = false;
+		res = _FAIL;
 		goto exit;
 	}
 	if (padapter->hw_init_completed == false) {
-		res = false;
+		res = _FAIL;
 		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
 			 ("\n === rtw_set_802_11_bssid23a_list_scan:"
 			  "hw_init_completed == false ===\n"));
@@ -393,7 +303,6 @@ u8 rtw_set_802_11_bssid23a_list_scan(struct rtw_adapter *padapter,
 		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
 			 ("rtw_set_802_11_bssid23a_list_scan fail since fw_state "
 			  "= %x\n", get_fwstate(pmlmepriv)));
-		res = true;
 
 		if (check_fwstate(pmlmepriv,
 				  (_FW_UNDER_SURVEY|_FW_UNDER_LINKING))) {
@@ -406,8 +315,8 @@ u8 rtw_set_802_11_bssid23a_list_scan(struct rtw_adapter *padapter,
 		}
 	} else {
 		if (rtw_is_scan_deny(padapter)) {
-			DBG_8723A(FUNC_ADPT_FMT": scan deny\n",
-				  FUNC_ADPT_ARG(padapter));
+			DBG_8723A("%s(%s): scan deny\n",
+				  __func__, padapter->pnetdev->name);
 			return _SUCCESS;
 		}
 
@@ -422,12 +331,11 @@ exit:
 	return res;
 }
 
-u8 rtw_set_802_11_authentication_mode23a(struct rtw_adapter* padapter,
-				      enum ndis_802_11_auth_mode authmode)
+int rtw_set_802_11_authentication_mode23a(struct rtw_adapter* padapter,
+					  enum ndis_802_11_auth_mode authmode)
 {
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 	int res;
-	u8 ret;
 
 	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
 		 ("set_802_11_auth.mode(): mode =%x\n", authmode));
@@ -444,90 +352,7 @@ u8 rtw_set_802_11_authentication_mode23a(struct rtw_adapter* padapter,
 
 	res = rtw_set_auth23a(padapter, psecuritypriv);
 
-	if (res == _SUCCESS)
-		ret = true;
-	else
-		ret = false;
-
-	return ret;
-}
-
-u8 rtw_set_802_11_add_wep23a(struct rtw_adapter* padapter,
-			  struct ndis_802_11_wep *wep)
-{
-	u8 bdefaultkey;
-	u8 btransmitkey;
-	int keyid, res;
-	struct security_priv *psecuritypriv = &padapter->securitypriv;
-	u8 ret = _SUCCESS;
-
-	bdefaultkey = (wep->KeyIndex & 0x40000000) > 0 ? false : true;
-	btransmitkey = (wep->KeyIndex & 0x80000000) > 0 ? true  : false;
-	keyid = wep->KeyIndex & 0x3fffffff;
-
-	if (keyid >= 4) {
-		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
-			 ("MgntActrtw_set_802_11_add_wep23a:keyid>4 =>fail\n"));
-		ret = false;
-		goto exit;
-	}
-
-	switch (wep->KeyLength)
-	{
-	case 5:
-		psecuritypriv->dot11PrivacyAlgrthm = _WEP40_;
-		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
-			 ("MgntActrtw_set_802_11_add_wep23a:wep->KeyLength = 5\n"));
-		break;
-	case 13:
-		psecuritypriv->dot11PrivacyAlgrthm = _WEP104_;
-		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
-			 ("MgntActrtw_set_802_11_add_wep23a:wep->KeyLength = 13\n"));
-		break;
-	default:
-		psecuritypriv->dot11PrivacyAlgrthm = _NO_PRIVACY_;
-		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
-			 ("MgntActrtw_set_802_11_add_wep23a:wep->KeyLength!= 5 "
-			  "or 13\n"));
-			break;
-	}
-
-	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
-		 ("rtw_set_802_11_add_wep23a:befor memcpy, wep->KeyLength = 0x%x "
-		  "wep->KeyIndex = 0x%x  keyid =%x\n",
-		  wep->KeyLength, wep->KeyIndex, keyid));
-
-	memcpy(&psecuritypriv->dot11DefKey[keyid].skey[0],
-	       &wep->KeyMaterial, wep->KeyLength);
-
-	psecuritypriv->dot11DefKeylen[keyid] = wep->KeyLength;
-
-	psecuritypriv->dot11PrivacyKeyIndex = keyid;
-
-	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
-		 ("rtw_set_802_11_add_wep23a:security key material : %x %x %x %x "
-		  "%x %x %x %x %x %x %x %x %x\n",
-		  psecuritypriv->dot11DefKey[keyid].skey[0],
-		  psecuritypriv->dot11DefKey[keyid].skey[1],
-		  psecuritypriv->dot11DefKey[keyid].skey[2],
-		  psecuritypriv->dot11DefKey[keyid].skey[3],
-		  psecuritypriv->dot11DefKey[keyid].skey[4],
-		  psecuritypriv->dot11DefKey[keyid].skey[5],
-		  psecuritypriv->dot11DefKey[keyid].skey[6],
-		  psecuritypriv->dot11DefKey[keyid].skey[7],
-		  psecuritypriv->dot11DefKey[keyid].skey[8],
-		  psecuritypriv->dot11DefKey[keyid].skey[9],
-		  psecuritypriv->dot11DefKey[keyid].skey[10],
-		  psecuritypriv->dot11DefKey[keyid].skey[11],
-		  psecuritypriv->dot11DefKey[keyid].skey[12]));
-
-	res = rtw_set_key23a(padapter, psecuritypriv, keyid, 1);
-
-	if (res == _FAIL)
-		ret = false;
-exit:
-
-	return ret;
+	return res;
 }
 
 /*
@@ -539,7 +364,7 @@ exit:
 u16 rtw_get_cur_max_rate23a(struct rtw_adapter *adapter)
 {
 	int i = 0;
-	u8 *p;
+	const u8 *p;
 	u16 rate = 0, max_rate = 0;
 	struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
@@ -550,16 +375,16 @@ u16 rtw_get_cur_max_rate23a(struct rtw_adapter *adapter)
 	u8 rf_type = 0;
 	u8 bw_40MHz = 0, short_GI_20 = 0, short_GI_40 = 0;
 	u16 mcs_rate = 0;
-	u32 ht_ielen = 0;
 
 	if (!check_fwstate(pmlmepriv, _FW_LINKED) &&
 	    !check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE))
 		return 0;
 
 	if (pmlmeext->cur_wireless_mode & (WIRELESS_11_24N|WIRELESS_11_5N)) {
-		p = rtw_get_ie23a(&pcur_bss->IEs[12], _HT_CAPABILITY_IE_,
-			       &ht_ielen, pcur_bss->IELength - 12);
-		if (p && ht_ielen > 0) {
+		p = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY,
+				     &pcur_bss->IEs[12],
+				     pcur_bss->IELength - 12);
+		if (p && p[1] > 0) {
 			pht_capie = (struct ieee80211_ht_cap *)(p + 2);
 
 			memcpy(&mcs_rate, &pht_capie->mcs, 2);
@@ -569,7 +394,7 @@ u16 rtw_get_cur_max_rate23a(struct rtw_adapter *adapter)
 			/* cur_bwmod is updated by beacon, pmlmeinfo is
 			   updated by association response */
 			bw_40MHz = (pmlmeext->cur_bwmode &&
-				    (HT_INFO_HT_PARAM_REC_TRANS_CHNL_WIDTH &
+				    (IEEE80211_HT_PARAM_CHAN_WIDTH_ANY &
 				     pmlmeinfo->HT_info.infos[0])) ? 1:0;
 
 			/* short_GI = (pht_capie->cap_info & (IEEE80211_HT_CAP
@@ -577,8 +402,7 @@ u16 rtw_get_cur_max_rate23a(struct rtw_adapter *adapter)
 			short_GI_20 = (pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info&IEEE80211_HT_CAP_SGI_20) ? 1:0;
 			short_GI_40 = (pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info&IEEE80211_HT_CAP_SGI_40) ? 1:0;
 
-			rtw23a_hal_get_hwreg(adapter, HW_VAR_RF_TYPE,
-					  (u8 *)(&rf_type));
+			rf_type = rtl8723a_get_rf_type(adapter);
 			max_rate = rtw_mcs_rate23a(rf_type, bw_40MHz &
 						pregistrypriv->cbw40_enable,
 						short_GI_20, short_GI_40,
diff --git a/drivers/staging/rtl8723au/core/rtw_led.c b/drivers/staging/rtl8723au/core/rtw_led.c
index 68532a3b2c1..e21a42c5418 100644
--- a/drivers/staging/rtl8723au/core/rtw_led.c
+++ b/drivers/staging/rtl8723au/core/rtw_led.c
@@ -225,7 +225,7 @@ static void SwLedBlink1(struct led_8723a *pLed)
 		if (pLed->BlinkTimes == 0)
 			bStopBlinking = true;
 		if (bStopBlinking) {
-			if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
+			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
 				pLed->bLedLinkBlinkInProgress = true;
 				pLed->CurrLedState = LED_BLINK_NORMAL;
 				if (pLed->bLedOn)
@@ -234,7 +234,7 @@ static void SwLedBlink1(struct led_8723a *pLed)
 					pLed->BlinkingLedState = RTW_LED_ON;
 				delay = LED_BLINK_LINK_INTERVAL_ALPHA;
 				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-			} else if (check_fwstate(pmlmepriv, _FW_LINKED) == false) {
+			} else {
 				pLed->bLedNoLinkBlinkInProgress = true;
 				pLed->CurrLedState = LED_BLINK_SLOWLY;
 				if (pLed->bLedOn)
@@ -258,7 +258,7 @@ static void SwLedBlink1(struct led_8723a *pLed)
 		if (pLed->BlinkTimes == 0)
 			bStopBlinking = true;
 		if (bStopBlinking) {
-			if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
+			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
 				pLed->bLedLinkBlinkInProgress = true;
 				pLed->CurrLedState = LED_BLINK_NORMAL;
 				if (pLed->bLedOn)
@@ -267,8 +267,7 @@ static void SwLedBlink1(struct led_8723a *pLed)
 					pLed->BlinkingLedState = RTW_LED_ON;
 				delay = LED_BLINK_LINK_INTERVAL_ALPHA;
 				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-			} else if (check_fwstate(pmlmepriv,
-						 _FW_LINKED) == false) {
+			} else {
 				pLed->bLedNoLinkBlinkInProgress = true;
 				pLed->CurrLedState = LED_BLINK_SLOWLY;
 				if (pLed->bLedOn)
@@ -354,7 +353,7 @@ static void SwLedBlink2(struct led_8723a *pLed)
 				RT_TRACE(_module_rtl8712_led_c_, _drv_info_,
 					 ("stop scan blink CurrLedState %d\n",
 					 pLed->CurrLedState));
-			} else if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
+			} else {
 				pLed->CurrLedState = RTW_LED_OFF;
 				pLed->BlinkingLedState = RTW_LED_OFF;
 				SwLedOff23a(padapter, pLed);
@@ -390,7 +389,7 @@ static void SwLedBlink2(struct led_8723a *pLed)
 				RT_TRACE(_module_rtl8712_led_c_, _drv_info_,
 					 ("stop CurrLedState %d\n", pLed->CurrLedState));
 
-			} else if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
+			} else {
 				pLed->CurrLedState = RTW_LED_OFF;
 				pLed->BlinkingLedState = RTW_LED_OFF;
 				SwLedOff23a(padapter, pLed);
@@ -450,17 +449,14 @@ static void SwLedBlink3(struct led_8723a *pLed)
 				{
 					SwLedOff23a(padapter, pLed);
 				}
-				else if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-				{
+				else if (check_fwstate(pmlmepriv, _FW_LINKED)) {
 					pLed->CurrLedState = RTW_LED_ON;
 					pLed->BlinkingLedState = RTW_LED_ON;
 					if (!pLed->bLedOn)
 						SwLedOn23a(padapter, pLed);
 
 					RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-				}
-				else if (check_fwstate(pmlmepriv, _FW_LINKED) == false)
-				{
+				} else {
 					pLed->CurrLedState = RTW_LED_OFF;
 					pLed->BlinkingLedState = RTW_LED_OFF;
 					if (pLed->bLedOn)
@@ -499,9 +495,8 @@ static void SwLedBlink3(struct led_8723a *pLed)
 				if (padapter->pwrctrlpriv.rf_pwrstate != rf_on)
 				{
 					SwLedOff23a(padapter, pLed);
-				}
-				else if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-				{
+				} else if (check_fwstate(pmlmepriv,
+							 _FW_LINKED)) {
 					pLed->CurrLedState = RTW_LED_ON;
 					pLed->BlinkingLedState = RTW_LED_ON;
 
@@ -509,9 +504,7 @@ static void SwLedBlink3(struct led_8723a *pLed)
 						SwLedOn23a(padapter, pLed);
 
 					RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
-				}
-				else if (check_fwstate(pmlmepriv, _FW_LINKED) == false)
-				{
+				} else {
 					pLed->CurrLedState = RTW_LED_OFF;
 					pLed->BlinkingLedState = RTW_LED_OFF;
 
@@ -914,8 +907,8 @@ SwLedControlMode1(struct rtw_adapter *padapter, enum led_ctl_mode LedAction)
 			break;
 
 		case LED_CTL_SITE_SURVEY:
-			 if ((pmlmepriv->LinkDetectInfo.bBusyTraffic) &&
-			     (check_fwstate(pmlmepriv, _FW_LINKED) == true))
+			 if (pmlmepriv->LinkDetectInfo.bBusyTraffic &&
+			     check_fwstate(pmlmepriv, _FW_LINKED))
 				;
 			 else if (pLed->bLedScanBlinkInProgress == false) {
 				if (IS_LED_WPS_BLINKING(pLed))
@@ -1118,8 +1111,8 @@ SwLedControlMode2(struct rtw_adapter *padapter, enum led_ctl_mode LedAction)
 		 break;
 	case LED_CTL_TX:
 	case LED_CTL_RX:
-		if ((pLed->bLedBlinkInProgress == false) &&
-		    (check_fwstate(pmlmepriv, _FW_LINKED) == true)) {
+		if (pLed->bLedBlinkInProgress == false &&
+		    check_fwstate(pmlmepriv, _FW_LINKED)) {
 			if (pLed->CurrLedState == LED_BLINK_SCAN ||
 			    IS_LED_WPS_BLINKING(pLed)) {
 				return;
@@ -1261,8 +1254,8 @@ SwLedControlMode3(struct rtw_adapter *padapter, enum led_ctl_mode LedAction)
 
 		case LED_CTL_TX:
 		case LED_CTL_RX:
-			if ((pLed->bLedBlinkInProgress == false) &&
-			    (check_fwstate(pmlmepriv, _FW_LINKED) == true)) {
+			if (pLed->bLedBlinkInProgress == false &&
+			    check_fwstate(pmlmepriv, _FW_LINKED)) {
 				if (pLed->CurrLedState == LED_BLINK_SCAN ||
 				    IS_LED_WPS_BLINKING(pLed)) {
 					return;
@@ -1477,8 +1470,8 @@ SwLedControlMode4(struct rtw_adapter *padapter, enum led_ctl_mode LedAction)
 			break;
 
 		case LED_CTL_SITE_SURVEY:
-			if ((pmlmepriv->LinkDetectInfo.bBusyTraffic) &&
-			    (check_fwstate(pmlmepriv, _FW_LINKED) == true))
+			if (pmlmepriv->LinkDetectInfo.bBusyTraffic &&
+			    check_fwstate(pmlmepriv, _FW_LINKED))
 				;
 			else if (pLed->bLedScanBlinkInProgress == false) {
 				if (IS_LED_WPS_BLINKING(pLed))
@@ -1714,7 +1707,8 @@ SwLedControlMode5(struct rtw_adapter *padapter, enum led_ctl_mode LedAction)
 			break;
 
 		case LED_CTL_SITE_SURVEY:
-			if ((pmlmepriv->LinkDetectInfo.bBusyTraffic) && (check_fwstate(pmlmepriv, _FW_LINKED) == true))
+			if (pmlmepriv->LinkDetectInfo.bBusyTraffic &&
+			    check_fwstate(pmlmepriv, _FW_LINKED))
 				;
 			else if (pLed->bLedScanBlinkInProgress == false)
 			{
diff --git a/drivers/staging/rtl8723au/core/rtw_mlme.c b/drivers/staging/rtl8723au/core/rtw_mlme.c
index 6cee78785bd..7170258d260 100644
--- a/drivers/staging/rtl8723au/core/rtw_mlme.c
+++ b/drivers/staging/rtl8723au/core/rtw_mlme.c
@@ -25,8 +25,7 @@
 #include <wifi.h>
 #include <wlan_bssdef.h>
 #include <rtw_ioctl_set.h>
-
-extern u8 rtw_do_join23a(struct rtw_adapter * padapter);
+#include <rtw_sreset.h>
 
 static void rtw_init_mlme_timer(struct rtw_adapter *padapter)
 {
@@ -45,7 +44,7 @@ static void rtw_init_mlme_timer(struct rtw_adapter *padapter)
 		    rtw_set_scan_deny_timer_hdl, (unsigned long)padapter);
 }
 
-int _rtw_init_mlme_priv23a(struct rtw_adapter *padapter)
+int rtw_init_mlme_priv23a(struct rtw_adapter *padapter)
 {
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	int res = _SUCCESS;
@@ -53,16 +52,16 @@ int _rtw_init_mlme_priv23a(struct rtw_adapter *padapter)
 	pmlmepriv->nic_hdl = padapter;
 
 	pmlmepriv->fw_state = 0;
-	pmlmepriv->cur_network.network.InfrastructureMode = Ndis802_11AutoUnknown;
-	pmlmepriv->scan_mode=SCAN_ACTIVE;/*  1: active, 0: pasive. Maybe someday we should rename this varable to "active_mode" (Jeff) */
+	pmlmepriv->cur_network.network.ifmode = NL80211_IFTYPE_UNSPECIFIED;
+	/*  1: active, 0: pasive. Maybe someday we should rename this
+	    varable to "active_mode" (Jeff) */
+	pmlmepriv->scan_mode = SCAN_ACTIVE;
 
 	spin_lock_init(&pmlmepriv->lock);
 	_rtw_init_queue23a(&pmlmepriv->scanned_queue);
 
 	memset(&pmlmepriv->assoc_ssid, 0, sizeof(struct cfg80211_ssid));
 
-	/* allocate DMA-able/Non-Page memory for cmd_buf and rsp_buf */
-
 	rtw_clear_scan_deny(padapter);
 
 	rtw_init_mlme_timer(padapter);
@@ -72,11 +71,10 @@ int _rtw_init_mlme_priv23a(struct rtw_adapter *padapter)
 #ifdef CONFIG_8723AU_AP_MODE
 static void rtw_free_mlme_ie_data(u8 **ppie, u32 *plen)
 {
-	if(*ppie)
-	{
+	if (*ppie) {
 		kfree(*ppie);
 		*plen = 0;
-		*ppie=NULL;
+		*ppie = NULL;
 	}
 }
 #endif
@@ -86,37 +84,52 @@ void rtw23a_free_mlme_priv_ie_data(struct mlme_priv *pmlmepriv)
 #ifdef CONFIG_8723AU_AP_MODE
 	kfree(pmlmepriv->assoc_req);
 	kfree(pmlmepriv->assoc_rsp);
-	rtw_free_mlme_ie_data(&pmlmepriv->wps_beacon_ie, &pmlmepriv->wps_beacon_ie_len);
-	rtw_free_mlme_ie_data(&pmlmepriv->wps_probe_req_ie, &pmlmepriv->wps_probe_req_ie_len);
-	rtw_free_mlme_ie_data(&pmlmepriv->wps_probe_resp_ie, &pmlmepriv->wps_probe_resp_ie_len);
-	rtw_free_mlme_ie_data(&pmlmepriv->wps_assoc_resp_ie, &pmlmepriv->wps_assoc_resp_ie_len);
-
-	rtw_free_mlme_ie_data(&pmlmepriv->p2p_beacon_ie, &pmlmepriv->p2p_beacon_ie_len);
-	rtw_free_mlme_ie_data(&pmlmepriv->p2p_probe_req_ie, &pmlmepriv->p2p_probe_req_ie_len);
-	rtw_free_mlme_ie_data(&pmlmepriv->p2p_probe_resp_ie, &pmlmepriv->p2p_probe_resp_ie_len);
-	rtw_free_mlme_ie_data(&pmlmepriv->p2p_go_probe_resp_ie, &pmlmepriv->p2p_go_probe_resp_ie_len);
-	rtw_free_mlme_ie_data(&pmlmepriv->p2p_assoc_req_ie, &pmlmepriv->p2p_assoc_req_ie_len);
-
-	rtw_free_mlme_ie_data(&pmlmepriv->wfd_beacon_ie, &pmlmepriv->wfd_beacon_ie_len);
-	rtw_free_mlme_ie_data(&pmlmepriv->wfd_probe_req_ie, &pmlmepriv->wfd_probe_req_ie_len);
-	rtw_free_mlme_ie_data(&pmlmepriv->wfd_probe_resp_ie, &pmlmepriv->wfd_probe_resp_ie_len);
-	rtw_free_mlme_ie_data(&pmlmepriv->wfd_go_probe_resp_ie, &pmlmepriv->wfd_go_probe_resp_ie_len);
-	rtw_free_mlme_ie_data(&pmlmepriv->wfd_assoc_req_ie, &pmlmepriv->wfd_assoc_req_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wps_beacon_ie,
+			      &pmlmepriv->wps_beacon_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wps_probe_req_ie,
+			      &pmlmepriv->wps_probe_req_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wps_probe_resp_ie,
+			      &pmlmepriv->wps_probe_resp_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wps_assoc_resp_ie,
+			      &pmlmepriv->wps_assoc_resp_ie_len);
+
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_beacon_ie,
+			      &pmlmepriv->p2p_beacon_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_probe_req_ie,
+			      &pmlmepriv->p2p_probe_req_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_probe_resp_ie,
+			      &pmlmepriv->p2p_probe_resp_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_go_probe_resp_ie,
+			      &pmlmepriv->p2p_go_probe_resp_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->p2p_assoc_req_ie,
+			      &pmlmepriv->p2p_assoc_req_ie_len);
+
+	rtw_free_mlme_ie_data(&pmlmepriv->wfd_beacon_ie,
+			      &pmlmepriv->wfd_beacon_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wfd_probe_req_ie,
+			      &pmlmepriv->wfd_probe_req_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wfd_probe_resp_ie,
+			      &pmlmepriv->wfd_probe_resp_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wfd_go_probe_resp_ie,
+			      &pmlmepriv->wfd_go_probe_resp_ie_len);
+	rtw_free_mlme_ie_data(&pmlmepriv->wfd_assoc_req_ie,
+			      &pmlmepriv->wfd_assoc_req_ie_len);
 #endif
 }
 
-void _rtw_free_mlme_priv23a(struct mlme_priv *pmlmepriv)
+void rtw_free_mlme_priv23a(struct mlme_priv *pmlmepriv)
 {
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
+		 ("rtw_free_mlme_priv23a\n"));
 
 	rtw23a_free_mlme_priv_ie_data(pmlmepriv);
-
 }
 
-struct wlan_network *rtw_alloc_network(struct mlme_priv *pmlmepriv)
+struct wlan_network *rtw_alloc_network(struct mlme_priv *pmlmepriv, int gfp)
 {
 	struct wlan_network *pnetwork;
 
-	pnetwork = kzalloc(sizeof(struct wlan_network), GFP_ATOMIC);
+	pnetwork = kzalloc(sizeof(struct wlan_network), gfp);
 	if (pnetwork) {
 		INIT_LIST_HEAD(&pnetwork->list);
 		pnetwork->network_type = 0;
@@ -129,48 +142,27 @@ struct wlan_network *rtw_alloc_network(struct mlme_priv *pmlmepriv)
 	return pnetwork;
 }
 
-void _rtw_free_network23a(struct mlme_priv *pmlmepriv,
-		       struct wlan_network *pnetwork, u8 isfreeall)
+static void _rtw_free_network23a(struct mlme_priv *pmlmepriv,
+				 struct wlan_network *pnetwork)
 {
-	u32 lifetime = SCANQUEUE_LIFETIME;
-
 	if (!pnetwork)
 		return;
 
 	if (pnetwork->fixed == true)
 		return;
 
-	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) ||
-	    (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true))
-		lifetime = 1;
-
-	list_del_init(&pnetwork->list);
-
-	kfree(pnetwork);
-}
-
-void _rtw_free_network23a_nolock23a(struct mlme_priv *pmlmepriv,
-			      struct wlan_network *pnetwork)
-{
-
-	if (pnetwork == NULL)
-		return;
-
-	if (pnetwork->fixed == true)
-		return;
-
 	list_del_init(&pnetwork->list);
 
 	kfree(pnetwork);
 }
 
 /*
-	return the wlan_network with the matching addr
+ return the wlan_network with the matching addr
 
-	Shall be calle under atomic context... to avoid possible racing condition...
+ Shall be calle under atomic context... to avoid possible racing condition...
 */
 struct wlan_network *
-_rtw_find_network23a(struct rtw_queue *scanned_queue, u8 *addr)
+rtw_find_network23a(struct rtw_queue *scanned_queue, u8 *addr)
 {
 	struct list_head *phead, *plist;
 	struct wlan_network *pnetwork = NULL;
@@ -194,7 +186,7 @@ _rtw_find_network23a(struct rtw_queue *scanned_queue, u8 *addr)
 		plist = plist->next;
         }
 
-	if(plist == phead)
+	if (plist == phead)
 		pnetwork = NULL;
 
 	/* spin_unlock_bh(&scanned_queue->lock); */
@@ -204,11 +196,11 @@ exit:
 	return pnetwork;
 }
 
-void _rtw_free_network23a_queue23a(struct rtw_adapter *padapter, u8 isfreeall)
+void rtw_free_network_queue23a(struct rtw_adapter *padapter)
 {
 	struct list_head *phead, *plist, *ptmp;
 	struct wlan_network *pnetwork;
-	struct mlme_priv* pmlmepriv = &padapter->mlmepriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct rtw_queue *scanned_queue = &pmlmepriv->scanned_queue;
 
 	spin_lock_bh(&scanned_queue->lock);
@@ -218,24 +210,25 @@ void _rtw_free_network23a_queue23a(struct rtw_adapter *padapter, u8 isfreeall)
 	list_for_each_safe(plist, ptmp, phead) {
 		pnetwork = container_of(plist, struct wlan_network, list);
 
-		_rtw_free_network23a(pmlmepriv,pnetwork, isfreeall);
+		_rtw_free_network23a(pmlmepriv, pnetwork);
 	}
 
 	spin_unlock_bh(&scanned_queue->lock);
-
 }
 
-int rtw_if_up23a(struct rtw_adapter *padapter)	{
-
+int rtw_if_up23a(struct rtw_adapter *padapter)
+{
 	int res;
 
-	if(padapter->bDriverStopped || padapter->bSurpriseRemoved ||
-		(check_fwstate(&padapter->mlmepriv, _FW_LINKED)== false)) {
-		RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_if_up23a:bDriverStopped(%d) OR bSurpriseRemoved(%d)", padapter->bDriverStopped, padapter->bSurpriseRemoved));
-		res=false;
-	}
-	else
-		res=  true;
+	if (padapter->bDriverStopped || padapter->bSurpriseRemoved ||
+	    !check_fwstate(&padapter->mlmepriv, _FW_LINKED)) {
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+			 ("rtw_if_up23a:bDriverStopped(%d) OR "
+			  "bSurpriseRemoved(%d)", padapter->bDriverStopped,
+			  padapter->bSurpriseRemoved));
+		res = false;
+	} else
+		res =  true;
 
 	return res;
 }
@@ -247,115 +240,114 @@ void rtw_generate_random_ibss23a(u8* pibss)
 	pibss[0] = 0x02;  /* in ad-hoc mode bit1 must set to 1 */
 	pibss[1] = 0x11;
 	pibss[2] = 0x87;
-	pibss[3] = (u8)(curtime & 0xff) ;/* p[0]; */
-	pibss[4] = (u8)((curtime>>8) & 0xff) ;/* p[1]; */
-	pibss[5] = (u8)((curtime>>16) & 0xff) ;/* p[2]; */
+	pibss[3] = curtime & 0xff;/* p[0]; */
+	pibss[4] = (curtime >> 8) & 0xff;/* p[1]; */
+	pibss[5] = (curtime >> 16) & 0xff;/* p[2]; */
 
 	return;
 }
 
-u8 *rtw_get_capability23a_from_ie(u8 *ie)
+void rtw_set_roaming(struct rtw_adapter *adapter, u8 to_roaming)
 {
-	return ie + 8 + 2;
+	if (to_roaming == 0)
+		adapter->mlmepriv.to_join = false;
+	adapter->mlmepriv.to_roaming = to_roaming;
 }
 
-u16 rtw_get_capability23a(struct wlan_bssid_ex *bss)
+static void _rtw_roaming(struct rtw_adapter *padapter,
+			 struct wlan_network *tgt_network)
 {
-	u16	val;
-
-	memcpy((u8 *)&val, rtw_get_capability23a_from_ie(bss->IEs), 2);
-
-	return le16_to_cpu(val);
-}
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_network *pnetwork;
+	int do_join_r;
 
-u8 *rtw_get_timestampe_from_ie23a(u8 *ie)
-{
-	return ie + 0;
-}
+	if (tgt_network)
+		pnetwork = tgt_network;
+	else
+		pnetwork = &pmlmepriv->cur_network;
 
-u8 *rtw_get_beacon_interval23a_from_ie(u8 *ie)
-{
-	return ie + 8;
-}
+	if (padapter->mlmepriv.to_roaming > 0) {
+		DBG_8723A("roaming from %s("MAC_FMT"), length:%d\n",
+			  pnetwork->network.Ssid.ssid,
+			  MAC_ARG(pnetwork->network.MacAddress),
+			  pnetwork->network.Ssid.ssid_len);
+		memcpy(&pmlmepriv->assoc_ssid, &pnetwork->network.Ssid,
+		       sizeof(struct cfg80211_ssid));
 
-int	rtw_init_mlme_priv23a (struct rtw_adapter *padapter)/* struct	mlme_priv *pmlmepriv) */
-{
-	int	res;
+		pmlmepriv->assoc_by_bssid = false;
 
-	res = _rtw_init_mlme_priv23a(padapter);/*  (pmlmepriv); */
+		while (1) {
+			do_join_r = rtw_do_join23a(padapter);
+			if (do_join_r == _SUCCESS)
+				break;
+			else {
+				DBG_8723A("roaming do_join return %d\n",
+					  do_join_r);
+				pmlmepriv->to_roaming--;
 
-	return res;
+				if (padapter->mlmepriv.to_roaming > 0)
+					continue;
+				else {
+					DBG_8723A("%s(%d) -to roaming fail, "
+						  "indicate_disconnect\n",
+						  __func__, __LINE__);
+					rtw_indicate_disconnect23a(padapter);
+					break;
+				}
+			}
+		}
+	}
 }
 
-void rtw_free_mlme_priv23a (struct mlme_priv *pmlmepriv)
+void rtw23a_roaming(struct rtw_adapter *padapter,
+		    struct wlan_network *tgt_network)
 {
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
-	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("rtw_free_mlme_priv23a\n"));
-	_rtw_free_mlme_priv23a(pmlmepriv);
-
+	spin_lock_bh(&pmlmepriv->lock);
+	_rtw_roaming(padapter, tgt_network);
+	spin_unlock_bh(&pmlmepriv->lock);
 }
 
-void rtw_free_network(struct mlme_priv *pmlmepriv, struct	wlan_network *pnetwork, u8 is_freeall);
-void rtw_free_network(struct mlme_priv *pmlmepriv, struct	wlan_network *pnetwork, u8 is_freeall)/* struct	wlan_network *pnetwork, _queue	*free_queue) */
+__le16 *rtw_get_capability23a_from_ie(u8 *ie)
 {
-
-	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-		 ("rtw_free_network ==> ssid = %s\n\n" ,
-		  pnetwork->network.Ssid.ssid));
-	_rtw_free_network23a(pmlmepriv, pnetwork, is_freeall);
-
+	return (__le16 *)(ie + 8 + 2);
 }
 
-void rtw_free_network_nolock(struct mlme_priv *pmlmepriv, struct wlan_network *pnetwork);
-void rtw_free_network_nolock(struct mlme_priv *pmlmepriv, struct wlan_network *pnetwork)
+u16 rtw_get_capability23a(struct wlan_bssid_ex *bss)
 {
+	u16 val;
 
-	/* RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("rtw_free_network ==> ssid = %s\n\n" , pnetwork->network.Ssid.ssid)); */
-	_rtw_free_network23a_nolock23a(pmlmepriv, pnetwork);
+	memcpy(&val, rtw_get_capability23a_from_ie(bss->IEs), 2);
 
+	return le16_to_cpu(val);
 }
 
-void rtw_free_network_queue23a(struct rtw_adapter* dev, u8 isfreeall)
+__le16 *rtw_get_beacon_interval23a_from_ie(u8 *ie)
 {
-
-	_rtw_free_network23a_queue23a(dev, isfreeall);
-
+	return (__le16 *)(ie + 8);
 }
 
-/*
-	return the wlan_network with the matching addr
-
-	Shall be calle under atomic context... to avoid possible racing condition...
-*/
-struct	wlan_network *
-rtw_find_network23a(struct rtw_queue *scanned_queue, u8 *addr)
+static void rtw_free_network_nolock(struct mlme_priv *pmlmepriv,
+				    struct wlan_network *pnetwork)
 {
-	struct wlan_network *pnetwork;
-
-	pnetwork = _rtw_find_network23a(scanned_queue, addr);
-
-	return pnetwork;
+	_rtw_free_network23a(pmlmepriv, pnetwork);
 }
 
-int rtw_is_same_ibss23a(struct rtw_adapter *adapter, struct wlan_network *pnetwork)
+int rtw_is_same_ibss23a(struct rtw_adapter *adapter,
+			struct wlan_network *pnetwork)
 {
 	int ret = true;
 	struct security_priv *psecuritypriv = &adapter->securitypriv;
 
-	if ((psecuritypriv->dot11PrivacyAlgrthm != _NO_PRIVACY_) &&
-		    (pnetwork->network.Privacy == 0))
-	{
+	if (psecuritypriv->dot11PrivacyAlgrthm != 0 &&
+	    pnetwork->network.Privacy == 0)
 		ret = false;
-	}
-	else if ((psecuritypriv->dot11PrivacyAlgrthm == _NO_PRIVACY_) &&
-		 (pnetwork->network.Privacy == 1))
-	{
+	else if (psecuritypriv->dot11PrivacyAlgrthm == 0 &&
+		 pnetwork->network.Privacy == 1)
 		ret = false;
-	}
 	else
-	{
 		ret = true;
-	}
 
 	return ret;
 }
@@ -363,24 +355,19 @@ int rtw_is_same_ibss23a(struct rtw_adapter *adapter, struct wlan_network *pnetwo
 inline int is_same_ess(struct wlan_bssid_ex *a, struct wlan_bssid_ex *b);
 inline int is_same_ess(struct wlan_bssid_ex *a, struct wlan_bssid_ex *b)
 {
-	/* RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("(%s,%d)(%s,%d)\n", */
-	/*		a->Ssid.Ssid, a->Ssid.SsidLength, b->Ssid.Ssid, b->Ssid.SsidLength)); */
 	return (a->Ssid.ssid_len == b->Ssid.ssid_len) &&
 		!memcmp(a->Ssid.ssid, b->Ssid.ssid, a->Ssid.ssid_len);
 }
 
 int is_same_network23a(struct wlan_bssid_ex *src, struct wlan_bssid_ex *dst)
 {
-	 u16 s_cap, d_cap;
-
-	memcpy((u8 *)&s_cap, rtw_get_capability23a_from_ie(src->IEs), 2);
-	memcpy((u8 *)&d_cap, rtw_get_capability23a_from_ie(dst->IEs), 2);
+	u16 s_cap, d_cap;
 
-	s_cap = le16_to_cpu(s_cap);
-	d_cap = le16_to_cpu(d_cap);
+	s_cap = get_unaligned_le16(rtw_get_capability23a_from_ie(src->IEs));
+	d_cap = get_unaligned_le16(rtw_get_capability23a_from_ie(dst->IEs));
 
 	return ((src->Ssid.ssid_len == dst->Ssid.ssid_len) &&
-		/*	(src->Configuration.DSConfig == dst->Configuration.DSConfig) && */
+		/*	(src->DSConfig == dst->DSConfig) && */
 		ether_addr_equal(src->MacAddress, dst->MacAddress) &&
 		((!memcmp(src->Ssid.ssid, dst->Ssid.ssid, src->Ssid.ssid_len))) &&
 		((s_cap & WLAN_CAPABILITY_IBSS) ==
@@ -389,10 +376,10 @@ int is_same_network23a(struct wlan_bssid_ex *src, struct wlan_bssid_ex *dst)
 		 (d_cap & WLAN_CAPABILITY_ESS)));
 }
 
-struct wlan_network *rtw_get_oldest_wlan_network23a(struct rtw_queue *scanned_queue)
+struct wlan_network *
+rtw_get_oldest_wlan_network23a(struct rtw_queue *scanned_queue)
 {
 	struct list_head *plist, *phead;
-
 	struct wlan_network *pwlan;
 	struct wlan_network *oldest = NULL;
 
@@ -412,7 +399,7 @@ struct wlan_network *rtw_get_oldest_wlan_network23a(struct rtw_queue *scanned_qu
 }
 
 void update_network23a(struct wlan_bssid_ex *dst, struct wlan_bssid_ex *src,
-	struct rtw_adapter * padapter, bool update_ie)
+		       struct rtw_adapter *padapter, bool update_ie)
 {
 	u8 ss_ori = dst->PhyInfo.SignalStrength;
 	u8 sq_ori = dst->PhyInfo.SignalQuality;
@@ -426,30 +413,35 @@ void update_network23a(struct wlan_bssid_ex *dst, struct wlan_bssid_ex *src,
 	u8 sq_final;
 	long rssi_final;
 
-	DBG_8723A("%s %s(%pM, ch%u) ss_ori:%3u, sq_ori:%3u, rssi_ori:%3ld, ss_smp:%3u, sq_smp:%3u, rssi_smp:%3ld\n",
+	DBG_8723A("%s %s(%pM, ch%u) ss_ori:%3u, sq_ori:%3u, rssi_ori:%3ld, "
+		  "ss_smp:%3u, sq_smp:%3u, rssi_smp:%3ld\n",
 		  __func__, src->Ssid.ssid, src->MacAddress,
-		  src->Configuration.DSConfig, ss_ori, sq_ori, rssi_ori,
+		  src->DSConfig, ss_ori, sq_ori, rssi_ori,
 		  ss_smp, sq_smp, rssi_smp
 	);
 
 	/* The rule below is 1/5 for sample value, 4/5 for history value */
-	if (check_fwstate(&padapter->mlmepriv, _FW_LINKED) && is_same_network23a(&padapter->mlmepriv.cur_network.network, src)) {
+	if (check_fwstate(&padapter->mlmepriv, _FW_LINKED) &&
+	    is_same_network23a(&padapter->mlmepriv.cur_network.network, src)) {
 		/* Take the recvpriv's value for the connected AP*/
 		ss_final = padapter->recvpriv.signal_strength;
 		sq_final = padapter->recvpriv.signal_qual;
-		/* the rssi value here is undecorated, and will be used for antenna diversity */
+		/* the rssi value here is undecorated, and will be
+		   used for antenna diversity */
 		if (sq_smp != 101) /* from the right channel */
 			rssi_final = (src->Rssi+dst->Rssi*4)/5;
 		else
 			rssi_final = rssi_ori;
-	}
-	else {
+	} else {
 		if (sq_smp != 101) { /* from the right channel */
-			ss_final = ((u32)(src->PhyInfo.SignalStrength)+(u32)(dst->PhyInfo.SignalStrength)*4)/5;
-			sq_final = ((u32)(src->PhyInfo.SignalQuality)+(u32)(dst->PhyInfo.SignalQuality)*4)/5;
-			rssi_final = (src->Rssi+dst->Rssi*4)/5;
+			ss_final = ((u32)src->PhyInfo.SignalStrength +
+				    (u32)dst->PhyInfo.SignalStrength * 4) / 5;
+			sq_final = ((u32)src->PhyInfo.SignalQuality +
+				    (u32)dst->PhyInfo.SignalQuality * 4) / 5;
+			rssi_final = src->Rssi+dst->Rssi * 4 / 5;
 		} else {
-			/* bss info not receving from the right channel, use the original RX signal infos */
+			/* bss info not receving from the right channel, use
+			   the original RX signal infos */
 			ss_final = dst->PhyInfo.SignalStrength;
 			sq_final = dst->PhyInfo.SignalQuality;
 			rssi_final = dst->Rssi;
@@ -458,35 +450,37 @@ void update_network23a(struct wlan_bssid_ex *dst, struct wlan_bssid_ex *src,
 	}
 
 	if (update_ie)
-		memcpy((u8 *)dst, (u8 *)src, get_wlan_bssid_ex_sz(src));
+		memcpy(dst, src, get_wlan_bssid_ex_sz(src));
 
 	dst->PhyInfo.SignalStrength = ss_final;
 	dst->PhyInfo.SignalQuality = sq_final;
 	dst->Rssi = rssi_final;
 
-	DBG_8723A("%s %s(%pM), SignalStrength:%u, SignalQuality:%u, RawRSSI:%ld\n",
-		  __func__, dst->Ssid.ssid, dst->MacAddress,
+	DBG_8723A("%s %s(%pM), SignalStrength:%u, SignalQuality:%u, "
+		  "RawRSSI:%ld\n",  __func__, dst->Ssid.ssid, dst->MacAddress,
 		  dst->PhyInfo.SignalStrength,
 		  dst->PhyInfo.SignalQuality, dst->Rssi);
-
 }
 
-static void update_current_network(struct rtw_adapter *adapter, struct wlan_bssid_ex *pnetwork)
+static void update_current_network(struct rtw_adapter *adapter,
+				   struct wlan_bssid_ex *pnetwork)
 {
-	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
 
-	if ((check_fwstate(pmlmepriv, _FW_LINKED)== true) && (is_same_network23a(&pmlmepriv->cur_network.network, pnetwork)))
-	{
-		/* RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,"Same Network\n"); */
+	if (check_fwstate(pmlmepriv, _FW_LINKED) &&
+	    is_same_network23a(&pmlmepriv->cur_network.network, pnetwork)) {
+		int bcn_size;
+		update_network23a(&pmlmepriv->cur_network.network,
+				  pnetwork,adapter, true);
 
-		/* if(pmlmepriv->cur_network.network.IELength<= pnetwork->IELength) */
-		{
-			update_network23a(&pmlmepriv->cur_network.network, pnetwork,adapter, true);
-			rtw_update_protection23a(adapter, (pmlmepriv->cur_network.network.IEs) + sizeof (struct ndis_802_11_fixed_ies),
-									pmlmepriv->cur_network.network.IELength);
-		}
-	}
+		bcn_size = offsetof(struct ieee80211_mgmt, u.beacon.variable) -
+			offsetof(struct ieee80211_mgmt, u.beacon);
 
+		rtw_update_protection23a(adapter,
+					 pmlmepriv->cur_network.network.IEs +
+					 bcn_size,
+					 pmlmepriv->cur_network.network.IELength);
+	}
 }
 
 /*
@@ -494,7 +488,8 @@ static void update_current_network(struct rtw_adapter *adapter, struct wlan_bssi
 Caller must hold pmlmepriv->lock first.
 
 */
-void rtw_update_scanned_network23a(struct rtw_adapter *adapter, struct wlan_bssid_ex *target)
+void rtw_update_scanned_network23a(struct rtw_adapter *adapter,
+				   struct wlan_bssid_ex *target)
 {
 	struct list_head *plist, *phead;
 	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
@@ -522,7 +517,7 @@ void rtw_update_scanned_network23a(struct rtw_adapter *adapter, struct wlan_bssi
 	/* If we didn't find a match, then get a new network slot to initialize
 	 * with this beacon's information */
 	if (!found) {
-		pnetwork = rtw_alloc_network(pmlmepriv);
+		pnetwork = rtw_alloc_network(pmlmepriv, GFP_ATOMIC);
 		if (!pnetwork) {
 			if (!oldest) {
 				RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
@@ -561,19 +556,20 @@ void rtw_update_scanned_network23a(struct rtw_adapter *adapter, struct wlan_bssi
 
 		/* target.reserved == 1, means that scanned network is
 		 * a bcn frame. */
-		if ((pnetwork->network.IELength>target->IELength) &&
-		    (target->reserved == 1))
+		if (pnetwork->network.IELength > target->IELength &&
+		    target->reserved == 1)
 			update_ie = false;
 
-		update_network23a(&pnetwork->network, target,adapter, update_ie);
+		update_network23a(&pnetwork->network, target,adapter,
+				  update_ie);
 	}
 
 exit:
 	spin_unlock_bh(&queue->lock);
-
 }
 
-void rtw_add_network(struct rtw_adapter *adapter, struct wlan_bssid_ex *pnetwork)
+static void rtw_add_network(struct rtw_adapter *adapter,
+			    struct wlan_bssid_ex *pnetwork)
 {
 	update_current_network(adapter, pnetwork);
 	rtw_update_scanned_network23a(adapter, pnetwork);
@@ -585,7 +581,8 @@ void rtw_add_network(struct rtw_adapter *adapter, struct wlan_bssid_ex *pnetwork
 /*			   (3) WMM */
 /*			   (4) HT */
 /*                      (5) others */
-int rtw_is_desired_network(struct rtw_adapter *adapter, struct wlan_network *pnetwork)
+static int rtw_is_desired_network(struct rtw_adapter *adapter,
+				  struct wlan_network *pnetwork)
 {
 	struct security_priv *psecuritypriv = &adapter->securitypriv;
 	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
@@ -600,31 +597,31 @@ int rtw_is_desired_network(struct rtw_adapter *adapter, struct wlan_network *pne
 	desired_encmode = psecuritypriv->ndisencryptstatus;
 	privacy = pnetwork->network.Privacy;
 
-	if (check_fwstate(pmlmepriv, WIFI_UNDER_WPS))
-	{
-		if (rtw_get_wps_ie23a(pnetwork->network.IEs+_FIXED_IE_LENGTH_, pnetwork->network.IELength-_FIXED_IE_LENGTH_, NULL, &wps_ielen)!= NULL)
-		{
+	if (check_fwstate(pmlmepriv, WIFI_UNDER_WPS)) {
+		if (rtw_get_wps_ie23a(pnetwork->network.IEs + _FIXED_IE_LENGTH_,
+				      pnetwork->network.IELength -
+				      _FIXED_IE_LENGTH_, NULL, &wps_ielen))
 			return true;
-		}
 		else
-		{
 			return false;
-		}
 	}
-	if (adapter->registrypriv.wifi_spec == 1) /* for  correct flow of 8021X  to do.... */
-	{
-		if ((desired_encmode == Ndis802_11EncryptionDisabled) && (privacy != 0))
+	if (adapter->registrypriv.wifi_spec == 1) {
+		/* for  correct flow of 8021X  to do.... */
+		if (desired_encmode == Ndis802_11EncryptionDisabled &&
+		    privacy != 0)
 	            bselected = false;
 	}
 
-	if ((desired_encmode != Ndis802_11EncryptionDisabled) && (privacy == 0)) {
-		DBG_8723A("desired_encmode: %d, privacy: %d\n", desired_encmode, privacy);
+	if (desired_encmode != Ndis802_11EncryptionDisabled &&
+	    privacy == 0) {
+		DBG_8723A("desired_encmode: %d, privacy: %d\n",
+			  desired_encmode, privacy);
 		bselected = false;
 	}
 
-	if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true)
-	{
-		if (pnetwork->network.InfrastructureMode != pmlmepriv->cur_network.network.InfrastructureMode)
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
+		if (pnetwork->network.ifmode !=
+		    pmlmepriv->cur_network.network.ifmode)
 			bselected = false;
 	}
 
@@ -632,15 +629,14 @@ int rtw_is_desired_network(struct rtw_adapter *adapter, struct wlan_network *pne
 }
 
 /* TODO: Perry : For Power Management */
-void rtw_atimdone_event_callback23a(struct rtw_adapter	*adapter , u8 *pbuf)
+void rtw_atimdone_event_callback23a(struct rtw_adapter *adapter, const u8 *pbuf)
 {
-
 	RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("receive atimdone_evet\n"));
 
 	return;
 }
 
-void rtw_survey_event_cb23a(struct rtw_adapter	*adapter, u8 *pbuf)
+void rtw_survey_event_cb23a(struct rtw_adapter *adapter, const u8 *pbuf)
 {
 	u32 len;
 	struct wlan_bssid_ex *pnetwork;
@@ -648,31 +644,36 @@ void rtw_survey_event_cb23a(struct rtw_adapter	*adapter, u8 *pbuf)
 
 	pnetwork = (struct wlan_bssid_ex *)pbuf;
 
-	RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("rtw_survey_event_cb23a, ssid=%s\n",  pnetwork->Ssid.ssid));
+	RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,
+		 ("rtw_survey_event_cb23a, ssid=%s\n", pnetwork->Ssid.ssid));
 
 	len = get_wlan_bssid_ex_sz(pnetwork);
-	if(len > (sizeof(struct wlan_bssid_ex)))
-	{
-		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("\n ****rtw_survey_event_cb23a: return a wrong bss ***\n"));
+	if (len > (sizeof(struct wlan_bssid_ex))) {
+		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,
+			 ("\n ****rtw_survey_event_cb23a: return a wrong "
+			  "bss ***\n"));
 		return;
 	}
 
 	spin_lock_bh(&pmlmepriv->lock);
 
 	/*  update IBSS_network 's timestamp */
-	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) == true)
-	{
-		/* RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,"rtw_survey_event_cb23a : WIFI_ADHOC_MASTER_STATE\n\n"); */
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
+		/* RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
+		   "rtw_survey_event_cb23a : WIFI_ADHOC_MASTER_STATE\n\n"); */
 		if (ether_addr_equal(pmlmepriv->cur_network.network.MacAddress,
 				     pnetwork->MacAddress)) {
-			struct wlan_network* ibss_wlan = NULL;
+			struct wlan_network* ibss_wlan;
 
-			memcpy(pmlmepriv->cur_network.network.IEs, pnetwork->IEs, 8);
+			memcpy(pmlmepriv->cur_network.network.IEs,
+			       pnetwork->IEs, 8);
 			spin_lock_bh(&pmlmepriv->scanned_queue.lock);
-			ibss_wlan = rtw_find_network23a(&pmlmepriv->scanned_queue,  pnetwork->MacAddress);
-			if (ibss_wlan)
-			{
-				memcpy(ibss_wlan->network.IEs , pnetwork->IEs, 8);
+			ibss_wlan = rtw_find_network23a(
+				&pmlmepriv->scanned_queue,
+				pnetwork->MacAddress);
+			if (ibss_wlan) {
+				memcpy(ibss_wlan->network.IEs,
+				       pnetwork->IEs, 8);
 				spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
 				goto exit;
 			}
@@ -681,8 +682,7 @@ void rtw_survey_event_cb23a(struct rtw_adapter	*adapter, u8 *pbuf)
 	}
 
 	/*  lock pmlmepriv->lock when you accessing network_q */
-	if ((check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) == false)
-	{
+	if (!check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) {
 	        if (pnetwork->Ssid.ssid[0] == 0)
 			pnetwork->Ssid.ssid_len = 0;
 
@@ -696,10 +696,13 @@ exit:
 	return;
 }
 
-void rtw_surveydone_event_callback23a(struct rtw_adapter	*adapter, u8 *pbuf)
+void
+rtw_surveydone_event_callback23a(struct rtw_adapter *adapter, const u8 *pbuf)
 {
 	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
 	struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv;
+	struct wlan_bssid_ex *pdev_network;
+	u8 *pibss;
 
 	spin_lock_bh(&pmlmepriv->lock);
 
@@ -709,47 +712,62 @@ void rtw_surveydone_event_callback23a(struct rtw_adapter	*adapter, u8 *pbuf)
 		pmlmepriv->wps_probe_req_ie = NULL;
 	}
 
-	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_surveydone_event_callback23a: fw_state:%x\n\n", get_fwstate(pmlmepriv)));
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+		 ("rtw_surveydone_event_callback23a: fw_state:%x\n\n",
+		  get_fwstate(pmlmepriv)));
 
 	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY)) {
 		del_timer_sync(&pmlmepriv->scan_to_timer);
 
 		_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
 	} else {
-
-		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("nic status =%x, survey done event comes too late!\n", get_fwstate(pmlmepriv)));
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
+			 ("nic status =%x, survey done event comes too late!\n",
+			  get_fwstate(pmlmepriv)));
 	}
 
 	rtw_set_signal_stat_timer(&adapter->recvpriv);
 
 	if (pmlmepriv->to_join == true) {
-		if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true)) {
-			if (check_fwstate(pmlmepriv, _FW_LINKED) == false) {
+		if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
+			if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
 				set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
 
-				if (rtw_select_and_join_from_scanned_queue23a(pmlmepriv) == _SUCCESS) {
+				if (rtw_select_and_join_from_scanned_queue23a(
+					    pmlmepriv) == _SUCCESS) {
 					mod_timer(&pmlmepriv->assoc_timer,
 						  jiffies + msecs_to_jiffies(MAX_JOIN_TIMEOUT));
 				} else {
-					struct wlan_bssid_ex *pdev_network = &adapter->registrypriv.dev_network;
-					u8 *pibss = adapter->registrypriv.dev_network.MacAddress;
+					pdev_network = &adapter->registrypriv.dev_network;
+					pibss = adapter->registrypriv.dev_network.MacAddress;
 
-					_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+					_clr_fwstate_(pmlmepriv,
+						      _FW_UNDER_SURVEY);
 
-					RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("switching to adhoc master\n"));
+					RT_TRACE(_module_rtl871x_mlme_c_,
+						 _drv_err_,
+						 ("switching to adhoc "
+						  "master\n"));
 
-					memset(&pdev_network->Ssid, 0, sizeof(struct cfg80211_ssid));
-					memcpy(&pdev_network->Ssid, &pmlmepriv->assoc_ssid, sizeof(struct cfg80211_ssid));
+					memset(&pdev_network->Ssid, 0,
+					       sizeof(struct cfg80211_ssid));
+					memcpy(&pdev_network->Ssid,
+					       &pmlmepriv->assoc_ssid,
+					       sizeof(struct cfg80211_ssid));
 
-					rtw_update_registrypriv_dev_network23a(adapter);
+					rtw_update_registrypriv_dev_network23a(
+						adapter);
 					rtw_generate_random_ibss23a(pibss);
 
-					pmlmepriv->fw_state = WIFI_ADHOC_MASTER_STATE;
+					pmlmepriv->fw_state =
+						WIFI_ADHOC_MASTER_STATE;
 
-					if (rtw_createbss_cmd23a(adapter)!= _SUCCESS)
-					{
-					RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Error =>rtw_createbss_cmd23a status FAIL\n"));
-					}
+					if (rtw_createbss_cmd23a(adapter) !=
+					    _SUCCESS)
+					RT_TRACE(_module_rtl871x_mlme_c_,
+						 _drv_err_,
+						 ("Error =>rtw_createbss_cmd23a"
+						  " status FAIL\n"));
 
 					pmlmepriv->to_join = false;
 				}
@@ -758,27 +776,32 @@ void rtw_surveydone_event_callback23a(struct rtw_adapter	*adapter, u8 *pbuf)
 			int ret;
 			set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
 			pmlmepriv->to_join = false;
-			ret = rtw_select_and_join_from_scanned_queue23a(pmlmepriv);
+			ret = rtw_select_and_join_from_scanned_queue23a(
+				pmlmepriv);
 			if (ret == _SUCCESS) {
 				unsigned long e;
 				e = msecs_to_jiffies(MAX_JOIN_TIMEOUT);
 				mod_timer(&pmlmepriv->assoc_timer, jiffies + e);
-			} else if (ret == 2)/* there is no need to wait for join */
-			{
+			} else if (ret == 2) {/* there is no need to wait */
 				_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
 				rtw_indicate_connect23a(adapter);
 			} else {
-				DBG_8723A("try_to_join, but select scanning queue fail, to_roaming:%d\n", rtw_to_roaming(adapter));
-				if (rtw_to_roaming(adapter) != 0) {
-					if (--pmlmepriv->to_roaming == 0
-						|| _SUCCESS != rtw_sitesurvey_cmd23a(adapter, &pmlmepriv->assoc_ssid, 1, NULL, 0)
-					) {
+				DBG_8723A("try_to_join, but select scanning "
+					  "queue fail, to_roaming:%d\n",
+					  adapter->mlmepriv.to_roaming);
+				if (adapter->mlmepriv.to_roaming) {
+					if (--pmlmepriv->to_roaming == 0 ||
+					    rtw_sitesurvey_cmd23a(
+						    adapter,
+						    &pmlmepriv->assoc_ssid, 1,
+						    NULL, 0) != _SUCCESS) {
 						rtw_set_roaming(adapter, 0);
-						rtw_free_assoc_resources23a(adapter, 1);
-						rtw_indicate_disconnect23a(adapter);
-					} else {
+						rtw_free_assoc_resources23a(
+							adapter, 1);
+						rtw_indicate_disconnect23a(
+							adapter);
+					} else
 						pmlmepriv->to_join = true;
-					}
 				}
 				_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
 			}
@@ -787,29 +810,15 @@ void rtw_surveydone_event_callback23a(struct rtw_adapter	*adapter, u8 *pbuf)
 
 	spin_unlock_bh(&pmlmepriv->lock);
 
-#ifdef CONFIG_8723AU_P2P
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-		p2p_ps_wk_cmd23a(adapter, P2P_PS_SCAN_DONE, 0);
-#endif /*  CONFIG_8723AU_P2P */
-
 	rtw_os_xmit_schedule23a(adapter);
 
-	if(pmlmeext->sitesurvey_res.bss_cnt == 0)
-		rtw_hal_sreset_reset23a(adapter);
+	if (pmlmeext->sitesurvey_res.bss_cnt == 0)
+		rtw_sreset_reset(adapter);
 
 	rtw_cfg80211_surveydone_event_callback(adapter);
-
-}
-
-void rtw_dummy_event_callback23a(struct rtw_adapter *adapter , u8 *pbuf)
-{
 }
 
-void rtw23a_fwdbg_event_callback(struct rtw_adapter *adapter , u8 *pbuf)
-{
-}
-
-static void free_scanqueue(struct	mlme_priv *pmlmepriv)
+static void free_scanqueue(struct mlme_priv *pmlmepriv)
 {
 	struct wlan_network *pnetwork;
 	struct rtw_queue *scan_queue = &pmlmepriv->scanned_queue;
@@ -827,42 +836,38 @@ static void free_scanqueue(struct	mlme_priv *pmlmepriv)
         }
 
 	spin_unlock_bh(&scan_queue->lock);
-
 }
 
 /*
-*rtw_free_assoc_resources23a: the caller has to lock pmlmepriv->lock
-*/
-void rtw_free_assoc_resources23a(struct rtw_adapter *adapter, int lock_scanned_queue)
+ *rtw_free_assoc_resources23a: the caller has to lock pmlmepriv->lock
+ */
+void rtw_free_assoc_resources23a(struct rtw_adapter *adapter,
+				 int lock_scanned_queue)
 {
-	struct wlan_network* pwlan = NULL;
-	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
-	struct	sta_priv *pstapriv = &adapter->stapriv;
+	struct wlan_network* pwlan;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct sta_priv *pstapriv = &adapter->stapriv;
 	struct wlan_network *tgt_network = &pmlmepriv->cur_network;
+	struct sta_info* psta;
 
-	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_, ("+rtw_free_assoc_resources23a\n"));
-	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("tgt_network->network.MacAddress="MAC_FMT" ssid=%s\n",
-		MAC_ARG(tgt_network->network.MacAddress), tgt_network->network.Ssid.ssid));
-
-	if(check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_AP_STATE))
-	{
-		struct sta_info* psta;
-
-		psta = rtw_get_stainfo23a(&adapter->stapriv, tgt_network->network.MacAddress);
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
+		 ("+rtw_free_assoc_resources23a\n"));
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+		 ("tgt_network->network.MacAddress="MAC_FMT" ssid=%s\n",
+		  MAC_ARG(tgt_network->network.MacAddress),
+		  tgt_network->network.Ssid.ssid));
 
-		{
-			spin_lock_bh(&pstapriv->sta_hash_lock);
-			rtw_free_stainfo23a(adapter,  psta);
-		}
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_AP_STATE)) {
+		psta = rtw_get_stainfo23a(&adapter->stapriv,
+					  tgt_network->network.MacAddress);
 
+		spin_lock_bh(&pstapriv->sta_hash_lock);
+		rtw_free_stainfo23a(adapter,  psta);
 		spin_unlock_bh(&pstapriv->sta_hash_lock);
-
 	}
 
-	if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE|WIFI_ADHOC_MASTER_STATE|WIFI_AP_STATE))
-	{
-		struct sta_info* psta;
-
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE |
+			  WIFI_ADHOC_MASTER_STATE|WIFI_AP_STATE)) {
 		rtw_free_all_stainfo23a(adapter);
 
 		psta = rtw_get_bcmc_stainfo23a(adapter);
@@ -873,23 +878,25 @@ void rtw_free_assoc_resources23a(struct rtw_adapter *adapter, int lock_scanned_q
 		rtw_init_bcmc_stainfo23a(adapter);
 	}
 
-	if(lock_scanned_queue)
+	if (lock_scanned_queue)
 		spin_lock_bh(&pmlmepriv->scanned_queue.lock);
 
-	pwlan = rtw_find_network23a(&pmlmepriv->scanned_queue, tgt_network->network.MacAddress);
-	if(pwlan)
+	pwlan = rtw_find_network23a(&pmlmepriv->scanned_queue,
+				    tgt_network->network.MacAddress);
+	if (pwlan)
 		pwlan->fixed = false;
 	else
-		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("rtw_free_assoc_resources23a : pwlan== NULL\n\n"));
+		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,
+			 ("rtw_free_assoc_resources23a : pwlan== NULL\n"));
 
-	if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) && (adapter->stapriv.asoc_sta_count == 1))
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) &&
+	    adapter->stapriv.asoc_sta_count == 1)
 		rtw_free_network_nolock(pmlmepriv, pwlan);
 
-	if(lock_scanned_queue)
+	if (lock_scanned_queue)
 		spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
 
 	pmlmepriv->key_mask = 0;
-
 }
 
 /*
@@ -897,46 +904,54 @@ void rtw_free_assoc_resources23a(struct rtw_adapter *adapter, int lock_scanned_q
 */
 void rtw_indicate_connect23a(struct rtw_adapter *padapter)
 {
-	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
-	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("+rtw_indicate_connect23a\n"));
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
+		 ("+rtw_indicate_connect23a\n"));
 
 	pmlmepriv->to_join = false;
 
-	if(!check_fwstate(&padapter->mlmepriv, _FW_LINKED)) {
+	if (!check_fwstate(&padapter->mlmepriv, _FW_LINKED)) {
 		set_fwstate(pmlmepriv, _FW_LINKED);
 
 		rtw_led_control(padapter, LED_CTL_LINK);
 
-		rtw_os_indicate_connect23a(padapter);
+		rtw_cfg80211_indicate_connect(padapter);
+
+		netif_carrier_on(padapter->pnetdev);
+
+		if (padapter->pid[2] != 0)
+			kill_pid(find_vpid(padapter->pid[2]), SIGALRM, 1);
 	}
 
 	rtw_set_roaming(padapter, 0);
 
 	rtw_set_scan_deny(padapter, 3000);
 
-	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("-rtw_indicate_connect23a: fw_state=0x%08x\n", get_fwstate(pmlmepriv)));
-
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
+		 ("-rtw_indicate_connect23a: fw_state=0x%08x\n",
+		  get_fwstate(pmlmepriv)));
 }
 
 /*
-*rtw_indicate_disconnect23a: the caller has to lock pmlmepriv->lock
-*/
+ *rtw_indicate_disconnect23a: the caller has to lock pmlmepriv->lock
+ */
 void rtw_indicate_disconnect23a(struct rtw_adapter *padapter)
 {
 	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
-	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("+rtw_indicate_disconnect23a\n"));
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
+		 ("+rtw_indicate_disconnect23a\n"));
 
 	_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING|WIFI_UNDER_WPS);
 
         /* DBG_8723A("clear wps when %s\n", __func__); */
 
-	if (rtw_to_roaming(padapter) > 0)
+	if (padapter->mlmepriv.to_roaming > 0)
 		_clr_fwstate_(pmlmepriv, _FW_LINKED);
 
 	if (check_fwstate(&padapter->mlmepriv, _FW_LINKED) ||
-	    (rtw_to_roaming(padapter) <= 0)) {
+	    padapter->mlmepriv.to_roaming <= 0) {
 		rtw_os_indicate_disconnect23a(padapter);
 
 		/* set ips_deny_time to avoid enter IPS before LPS leave */
@@ -951,17 +966,7 @@ void rtw_indicate_disconnect23a(struct rtw_adapter *padapter)
 
 	}
 
-#ifdef CONFIG_8723AU_P2P
-	p2p_ps_wk_cmd23a(padapter, P2P_PS_DISABLE, 1);
-#endif /*  CONFIG_8723AU_P2P */
-
 	rtw_lps_ctrl_wk_cmd23a(padapter, LPS_CTRL_DISCONNECT, 1);
-
-}
-
-inline void rtw_indicate_scan_done23a(struct rtw_adapter *padapter, bool aborted)
-{
-	rtw_os_indicate_scan_done23a(padapter, aborted);
 }
 
 void rtw_scan_abort23a(struct rtw_adapter *adapter)
@@ -974,96 +979,105 @@ void rtw_scan_abort23a(struct rtw_adapter *adapter)
 	pmlmeext->scan_abort = true;
 	while (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) &&
 	       jiffies_to_msecs(jiffies - start) <= 200) {
-
 		if (adapter->bDriverStopped || adapter->bSurpriseRemoved)
 			break;
 
-		DBG_8723A(FUNC_NDEV_FMT"fw_state = _FW_UNDER_SURVEY!\n", FUNC_NDEV_ARG(adapter->pnetdev));
+		DBG_8723A("%s(%s): fw_state = _FW_UNDER_SURVEY!\n",
+			  __func__, adapter->pnetdev->name);
 		msleep(20);
 	}
 
 	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY)) {
 		if (!adapter->bDriverStopped && !adapter->bSurpriseRemoved)
-			DBG_8723A(FUNC_NDEV_FMT"waiting for scan_abort time out!\n", FUNC_NDEV_ARG(adapter->pnetdev));
-		rtw_indicate_scan_done23a(adapter, true);
+			DBG_8723A("%s(%s): waiting for scan_abort time out!\n",
+				  __func__, adapter->pnetdev->name);
+		rtw_cfg80211_indicate_scan_done(wdev_to_priv(adapter->rtw_wdev),
+						true);
 	}
 	pmlmeext->scan_abort = false;
 }
 
-static struct sta_info *rtw_joinbss_update_stainfo(struct rtw_adapter *padapter, struct wlan_network *pnetwork)
+static struct sta_info *
+rtw_joinbss_update_stainfo(struct rtw_adapter *padapter,
+			   struct wlan_network *pnetwork)
 {
 	int i;
-	struct sta_info *bmc_sta, *psta = NULL;
+	struct sta_info *bmc_sta, *psta;
 	struct recv_reorder_ctrl *preorder_ctrl;
 	struct sta_priv *pstapriv = &padapter->stapriv;
 
 	psta = rtw_get_stainfo23a(pstapriv, pnetwork->network.MacAddress);
-	if (psta == NULL) {
-		psta = rtw_alloc_stainfo23a(pstapriv, pnetwork->network.MacAddress);
-	}
+	if (!psta)
+		psta = rtw_alloc_stainfo23a(pstapriv,
+					    pnetwork->network.MacAddress,
+					    GFP_ATOMIC);
 
-	if (psta) /* update ptarget_sta */
-	{
+	if (psta) { /* update ptarget_sta */
 		DBG_8723A("%s\n", __func__);
 
 		psta->aid  = pnetwork->join_res;
-			psta->mac_id = 0;
+		psta->mac_id = 0;
 
 		/* sta mode */
-		rtw_hal_set_odm_var23a(padapter, HAL_ODM_STA_INFO, psta, true);
+		rtl8723a_SetHalODMVar(padapter, HAL_ODM_STA_INFO, psta, true);
 
 		/* security related */
-		if (padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X)
-		{
-			padapter->securitypriv.binstallGrpkey = false;
-			padapter->securitypriv.busetkipkey = false;
-			padapter->securitypriv.bgrpkey_handshake = false;
+		if (padapter->securitypriv.dot11AuthAlgrthm ==
+		    dot11AuthAlgrthm_8021X) {
+			padapter->securitypriv.binstallGrpkey = 0;
+			padapter->securitypriv.busetkipkey = 0;
 
 			psta->ieee8021x_blocked = true;
-			psta->dot118021XPrivacy = padapter->securitypriv.dot11PrivacyAlgrthm;
+			psta->dot118021XPrivacy =
+				padapter->securitypriv.dot11PrivacyAlgrthm;
 
-			memset((u8 *)&psta->dot118021x_UncstKey, 0, sizeof (union Keytype));
+			memset(&psta->dot118021x_UncstKey, 0,
+			       sizeof (union Keytype));
 
-			memset((u8 *)&psta->dot11tkiprxmickey, 0, sizeof (union Keytype));
-			memset((u8 *)&psta->dot11tkiptxmickey, 0, sizeof (union Keytype));
+			memset(&psta->dot11tkiprxmickey, 0,
+			       sizeof (union Keytype));
+			memset(&psta->dot11tkiptxmickey, 0,
+			       sizeof (union Keytype));
 
-			memset((u8 *)&psta->dot11txpn, 0, sizeof (union pn48));
-			memset((u8 *)&psta->dot11rxpn, 0, sizeof (union pn48));
+			memset(&psta->dot11txpn, 0, sizeof (union pn48));
+			memset(&psta->dot11rxpn, 0, sizeof (union pn48));
 		}
 
 		/*	Commented by Albert 2012/07/21 */
 		/*	When doing the WPS, the wps_ie_len won't equal to 0 */
-		/*	And the Wi-Fi driver shouldn't allow the data packet to be tramsmitted. */
-		if (padapter->securitypriv.wps_ie_len != 0)
-		{
+		/*	And the Wi-Fi driver shouldn't allow the data packet
+			to be tramsmitted. */
+		if (padapter->securitypriv.wps_ie_len != 0) {
 			psta->ieee8021x_blocked = true;
 			padapter->securitypriv.wps_ie_len = 0;
 		}
 
-		/* for A-MPDU Rx reordering buffer control for bmc_sta & sta_info */
-		/* if A-MPDU Rx is enabled, reseting  rx_ordering_ctrl wstart_b(indicate_seq) to default value = 0xffff */
+		/* for A-MPDU Rx reordering buffer control for bmc_sta &
+		 * sta_info */
+		/* if A-MPDU Rx is enabled, reseting
+		   rx_ordering_ctrl wstart_b(indicate_seq) to default
+		   value = 0xffff */
 		/* todo: check if AP can send A-MPDU packets */
-		for (i = 0; i < 16 ; i++)
-		{
+		for (i = 0; i < 16 ; i++) {
 			/* preorder_ctrl = &precvpriv->recvreorder_ctrl[i]; */
 			preorder_ctrl = &psta->recvreorder_ctrl[i];
 			preorder_ctrl->enable = false;
 			preorder_ctrl->indicate_seq = 0xffff;
 			preorder_ctrl->wend_b = 0xffff;
-			preorder_ctrl->wsize_b = 64;/* max_ampdu_sz; ex. 32(kbytes) -> wsize_b = 32 */
+			/* max_ampdu_sz; ex. 32(kbytes) -> wsize_b = 32 */
+			preorder_ctrl->wsize_b = 64;
 		}
 
 		bmc_sta = rtw_get_bcmc_stainfo23a(padapter);
-		if (bmc_sta)
-		{
-			for (i = 0; i < 16 ; i++)
-			{
-				/* preorder_ctrl = &precvpriv->recvreorder_ctrl[i]; */
+		if (bmc_sta) {
+			for (i = 0; i < 16 ; i++) {
 				preorder_ctrl = &bmc_sta->recvreorder_ctrl[i];
 				preorder_ctrl->enable = false;
 				preorder_ctrl->indicate_seq = 0xffff;
 				preorder_ctrl->wend_b = 0xffff;
-				preorder_ctrl->wsize_b = 64;/* max_ampdu_sz; ex. 32(kbytes) -> wsize_b = 32 */
+				/* max_ampdu_sz; ex. 32(kbytes) ->
+				   wsize_b = 32 */
+				preorder_ctrl->wsize_b = 64;
 			}
 		}
 
@@ -1077,81 +1091,108 @@ static struct sta_info *rtw_joinbss_update_stainfo(struct rtw_adapter *padapter,
 
 /* pnetwork : returns from rtw23a_joinbss_event_cb */
 /* ptarget_wlan: found from scanned_queue */
-static void rtw_joinbss_update_network23a(struct rtw_adapter *padapter, struct wlan_network *ptarget_wlan, struct wlan_network  *pnetwork)
+static void
+rtw_joinbss_update_network23a(struct rtw_adapter *padapter,
+			      struct wlan_network *ptarget_wlan,
+			      struct wlan_network  *pnetwork)
 {
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct wlan_network *cur_network = &pmlmepriv->cur_network;
+	int bcn_size;
 
 	DBG_8723A("%s\n", __func__);
 
-	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("\nfw_state:%x, BSSID:"MAC_FMT"\n"
-		, get_fwstate(pmlmepriv), MAC_ARG(pnetwork->network.MacAddress)));
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+		 ("\nfw_state:%x, BSSID:"MAC_FMT"\n", get_fwstate(pmlmepriv),
+		  MAC_ARG(pnetwork->network.MacAddress)));
 
 	/*  why not use ptarget_wlan?? */
-	memcpy(&cur_network->network, &pnetwork->network, pnetwork->network.Length);
+	memcpy(&cur_network->network, &pnetwork->network,
+	       pnetwork->network.Length);
 	/*  some IEs in pnetwork is wrong, so we should use ptarget_wlan IEs */
 	cur_network->network.IELength = ptarget_wlan->network.IELength;
-	memcpy(&cur_network->network.IEs[0], &ptarget_wlan->network.IEs[0], MAX_IE_SZ);
+	memcpy(&cur_network->network.IEs[0], &ptarget_wlan->network.IEs[0],
+	       MAX_IE_SZ);
 
 	cur_network->aid = pnetwork->join_res;
 
 	rtw_set_signal_stat_timer(&padapter->recvpriv);
-	padapter->recvpriv.signal_strength = ptarget_wlan->network.PhyInfo.SignalStrength;
-	padapter->recvpriv.signal_qual = ptarget_wlan->network.PhyInfo.SignalQuality;
-	/* the ptarget_wlan->network.Rssi is raw data, we use ptarget_wlan->network.PhyInfo.SignalStrength instead (has scaled) */
-	padapter->recvpriv.rssi = translate_percentage_to_dbm(ptarget_wlan->network.PhyInfo.SignalStrength);
+	padapter->recvpriv.signal_strength =
+		ptarget_wlan->network.PhyInfo.SignalStrength;
+	padapter->recvpriv.signal_qual =
+		ptarget_wlan->network.PhyInfo.SignalQuality;
+	/*
+	 * the ptarget_wlan->network.Rssi is raw data, we use
+	 * ptarget_wlan->network.PhyInfo.SignalStrength instead (has scaled)
+	 */
+	padapter->recvpriv.rssi = translate_percentage_to_dbm(
+		ptarget_wlan->network.PhyInfo.SignalStrength);
 	DBG_8723A("%s signal_strength:%3u, rssi:%3d, signal_qual:%3u\n",
 		  __func__, padapter->recvpriv.signal_strength,
 		  padapter->recvpriv.rssi, padapter->recvpriv.signal_qual);
 	rtw_set_signal_stat_timer(&padapter->recvpriv);
 
 	/* update fw_state will clr _FW_UNDER_LINKING here indirectly */
-	switch (pnetwork->network.InfrastructureMode) {
-	case Ndis802_11Infrastructure:
-		if (pmlmepriv->fw_state&WIFI_UNDER_WPS)
+	switch (pnetwork->network.ifmode) {
+	case NL80211_IFTYPE_P2P_CLIENT:
+	case NL80211_IFTYPE_STATION:
+		if (pmlmepriv->fw_state & WIFI_UNDER_WPS)
 			pmlmepriv->fw_state = WIFI_STATION_STATE|WIFI_UNDER_WPS;
 		else
 			pmlmepriv->fw_state = WIFI_STATION_STATE;
 		break;
-	case Ndis802_11IBSS:
+	case NL80211_IFTYPE_ADHOC:
 		pmlmepriv->fw_state = WIFI_ADHOC_STATE;
 		break;
 	default:
 		pmlmepriv->fw_state = WIFI_NULL_STATE;
-		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Invalid network_mode\n"));
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
+			 ("Invalid network_mode\n"));
 		break;
 	}
 
-	rtw_update_protection23a(padapter, (cur_network->network.IEs) + sizeof (struct ndis_802_11_fixed_ies),
-									(cur_network->network.IELength));
+	bcn_size = offsetof(struct ieee80211_mgmt, u.beacon.variable) -
+		offsetof(struct ieee80211_mgmt, u.beacon);
 
-	rtw_update_ht_cap23a(padapter, cur_network->network.IEs, cur_network->network.IELength);
+	rtw_update_protection23a(padapter, cur_network->network.IEs +
+				 bcn_size, cur_network->network.IELength);
+
+	rtw_update_ht_cap23a(padapter, cur_network->network.IEs,
+			     cur_network->network.IELength);
 }
 
-/* Notes: the fucntion could be > passive_level (the same context as Rx tasklet) */
-/* pnetwork : returns from rtw23a_joinbss_event_cb */
-/* ptarget_wlan: found from scanned_queue */
-/* if join_res > 0, for (fw_state==WIFI_STATION_STATE), we check if  "ptarget_sta" & "ptarget_wlan" exist. */
-/* if join_res > 0, for (fw_state==WIFI_ADHOC_STATE), we only check if "ptarget_wlan" exist. */
-/* if join_res > 0, update "cur_network->network" from "pnetwork->network" if (ptarget_wlan !=NULL). */
+/*
+ * Notes:
+ * the fucntion could be > passive_level (the same context as Rx tasklet)
+ * pnetwork : returns from rtw23a_joinbss_event_cb
+ * ptarget_wlan: found from scanned_queue
+ * if join_res > 0, for (fw_state==WIFI_STATION_STATE),
+ * we check if  "ptarget_sta" & "ptarget_wlan" exist.
+ * if join_res > 0, for (fw_state==WIFI_ADHOC_STATE),
+ * we only check if "ptarget_wlan" exist.
+ * if join_res > 0, update "cur_network->network" from "pnetwork->network"
+ * if (ptarget_wlan !=NULL).
+ */
 
 void rtw_joinbss_event_prehandle23a(struct rtw_adapter *adapter, u8 *pbuf)
 {
-	static u8 retry=0;
-	struct sta_info *ptarget_sta= NULL, *pcur_sta = NULL;
-	struct	sta_priv *pstapriv = &adapter->stapriv;
-	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
-	struct wlan_network	*pnetwork	= (struct wlan_network *)pbuf;
+	struct sta_info *ptarget_sta, *pcur_sta;
+	struct sta_priv *pstapriv = &adapter->stapriv;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct wlan_network *pnetwork = (struct wlan_network *)pbuf;
 	struct wlan_network *cur_network = &pmlmepriv->cur_network;
-	struct wlan_network	*pcur_wlan = NULL, *ptarget_wlan = NULL;
-	unsigned int		the_same_macaddr = false;
+	struct wlan_network *pcur_wlan, *ptarget_wlan = NULL;
+	bool the_same_macaddr;
 
-	RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("joinbss event call back received with res=%d\n", pnetwork->join_res));
+	RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,
+		 ("joinbss event call back received with res=%d\n",
+		  pnetwork->join_res));
 
 	rtw_get_encrypt_decrypt_from_registrypriv23a(adapter);
 
 	if (pmlmepriv->assoc_ssid.ssid_len == 0) {
-		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("@@@@@   joinbss event call back  for Any SSid\n"));
+		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,
+			 ("@@@@@   joinbss event call back  for Any SSid\n"));
 	} else {
 		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,
 			 ("@@@@@   rtw23a_joinbss_event_cb for SSid:%s\n",
@@ -1165,93 +1206,102 @@ void rtw_joinbss_event_prehandle23a(struct rtw_adapter *adapter, u8 *pbuf)
 		the_same_macaddr = false;
 
 	pnetwork->network.Length = get_wlan_bssid_ex_sz(&pnetwork->network);
-	if(pnetwork->network.Length > sizeof(struct wlan_bssid_ex))
-	{
-		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("\n\n ***joinbss_evt_callback return a wrong bss ***\n\n"));
+	if (pnetwork->network.Length > sizeof(struct wlan_bssid_ex)) {
+		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,
+			 ("\n\n ***joinbss_evt_callback return a wrong bss "
+			  "***\n\n"));
 		return;
 	}
 
 	spin_lock_bh(&pmlmepriv->lock);
 
-	RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("\n rtw23a_joinbss_event_cb !! _enter_critical\n"));
+	RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,
+		 ("\n rtw23a_joinbss_event_cb !! _enter_critical\n"));
 
-	if(pnetwork->join_res > 0)
-	{
+	if (pnetwork->join_res > 0) {
 		spin_lock_bh(&pmlmepriv->scanned_queue.lock);
-		retry = 0;
-		if (check_fwstate(pmlmepriv,_FW_UNDER_LINKING))
-		{
+		if (check_fwstate(pmlmepriv,_FW_UNDER_LINKING)) {
 			/* s1. find ptarget_wlan */
-			if(check_fwstate(pmlmepriv, _FW_LINKED))
-			{
-				if(the_same_macaddr == true)
-				{
+			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+				if (the_same_macaddr == true) {
 					ptarget_wlan = rtw_find_network23a(&pmlmepriv->scanned_queue, cur_network->network.MacAddress);
-				}
-				else
-				{
+				} else {
 					pcur_wlan = rtw_find_network23a(&pmlmepriv->scanned_queue, cur_network->network.MacAddress);
-					if(pcur_wlan)	pcur_wlan->fixed = false;
+					if (pcur_wlan)
+						pcur_wlan->fixed = false;
 
 					pcur_sta = rtw_get_stainfo23a(pstapriv, cur_network->network.MacAddress);
-					if(pcur_sta) {
+					if (pcur_sta) {
 						spin_lock_bh(&pstapriv->sta_hash_lock);
-						rtw_free_stainfo23a(adapter,  pcur_sta);
+						rtw_free_stainfo23a(adapter,
+								    pcur_sta);
 						spin_unlock_bh(&pstapriv->sta_hash_lock);
 					}
 
 					ptarget_wlan = rtw_find_network23a(&pmlmepriv->scanned_queue, pnetwork->network.MacAddress);
-					if(check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) {
-						if(ptarget_wlan)	ptarget_wlan->fixed = true;
+					if (check_fwstate(pmlmepriv,
+							  WIFI_STATION_STATE)) {
+						if (ptarget_wlan)
+							ptarget_wlan->fixed =
+								true;
 					}
 				}
 
-			}
-			else
-			{
-				ptarget_wlan = rtw_find_network23a(&pmlmepriv->scanned_queue, pnetwork->network.MacAddress);
-				if(check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) {
-					if(ptarget_wlan)	ptarget_wlan->fixed = true;
+			} else {
+				ptarget_wlan = rtw_find_network23a(
+					&pmlmepriv->scanned_queue,
+					pnetwork->network.MacAddress);
+				if (check_fwstate(pmlmepriv,
+						  WIFI_STATION_STATE)) {
+					if (ptarget_wlan)
+						ptarget_wlan->fixed = true;
 				}
 			}
 
 			/* s2. update cur_network */
-			if(ptarget_wlan)
-			{
-				rtw_joinbss_update_network23a(adapter, ptarget_wlan, pnetwork);
-			}
-			else
-			{
-				RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("Can't find ptarget_wlan when joinbss_event callback\n"));
+			if (ptarget_wlan)
+				rtw_joinbss_update_network23a(adapter,
+							      ptarget_wlan,
+							      pnetwork);
+			else {
+				RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,
+					 ("Can't find ptarget_wlan when "
+					  "joinbss_event callback\n"));
 				spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
 				goto ignore_joinbss_callback;
 			}
 
-			/* s3. find ptarget_sta & update ptarget_sta after update cur_network only for station mode */
-			if(check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true)
-			{
-				ptarget_sta = rtw_joinbss_update_stainfo(adapter, pnetwork);
-				if(ptarget_sta==NULL)
-				{
-					RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("Can't update stainfo when joinbss_event callback\n"));
+			/* s3. find ptarget_sta & update ptarget_sta after
+			   update cur_network only for station mode */
+			if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+				ptarget_sta = rtw_joinbss_update_stainfo(
+					adapter, pnetwork);
+				if (!ptarget_sta) {
+					RT_TRACE(_module_rtl871x_mlme_c_,
+						 _drv_err_,
+						 ("Can't update stainfo when "
+						  "joinbss_event callback\n"));
 					spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
 					goto ignore_joinbss_callback;
 				}
 			}
 
 			/* s4. indicate connect */
-			if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true)
-			{
+			if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
 				rtw_indicate_connect23a(adapter);
-			} else {
-					/* adhoc mode will rtw_indicate_connect23a when rtw_stassoc_event_callback23a */
-				RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("adhoc mode, fw_state:%x", get_fwstate(pmlmepriv)));
+			else {
+				/* adhoc mode will rtw_indicate_connect23a
+				   when rtw_stassoc_event_callback23a */
+				RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,
+					 ("adhoc mode, fw_state:%x",
+					  get_fwstate(pmlmepriv)));
 			}
 
 			/* s5. Cancle assoc_timer */
 			del_timer_sync(&pmlmepriv->assoc_timer);
 
-			RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("Cancle assoc_timer\n"));
+			RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,
+				 ("Cancle assoc_timer\n"));
 		} else {
 			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
 				 ("rtw23a_joinbss_event_cb err: fw_state:%x",
@@ -1260,20 +1310,19 @@ void rtw_joinbss_event_prehandle23a(struct rtw_adapter *adapter, u8 *pbuf)
 			goto ignore_joinbss_callback;
 		}
 		spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
-	} else if(pnetwork->join_res == -4) {
+	} else if (pnetwork->join_res == -4) {
 		rtw_reset_securitypriv23a(adapter);
 		mod_timer(&pmlmepriv->assoc_timer,
 			  jiffies + msecs_to_jiffies(1));
 
 		/* rtw_free_assoc_resources23a(adapter, 1); */
 
-		if((check_fwstate(pmlmepriv, _FW_UNDER_LINKING))) {
+		if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) {
 			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-				 ("fail! clear _FW_UNDER_LINKING ^^^fw_state=%x\n",
-				 get_fwstate(pmlmepriv)));
+				 ("fail! clear _FW_UNDER_LINKING ^^^fw_state="
+				  "%x\n", get_fwstate(pmlmepriv)));
 			_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING);
 		}
-
 	} else {
 		/* if join_res < 0 (join fails), then try again */
 		mod_timer(&pmlmepriv->assoc_timer,
@@ -1286,48 +1335,33 @@ ignore_joinbss_callback:
 	spin_unlock_bh(&pmlmepriv->lock);
 }
 
-void rtw23a_joinbss_event_cb(struct rtw_adapter *adapter, u8 *pbuf)
+void rtw23a_joinbss_event_cb(struct rtw_adapter *adapter, const u8 *pbuf)
 {
-	struct wlan_network	*pnetwork	= (struct wlan_network *)pbuf;
+	struct wlan_network *pnetwork = (struct wlan_network *)pbuf;
 
 	mlmeext_joinbss_event_callback23a(adapter, pnetwork->join_res);
 
 	rtw_os_xmit_schedule23a(adapter);
-
-}
-
-/* FOR AP , AD-HOC mode */
-void rtw_stassoc_hw_rpt23a(struct rtw_adapter *adapter, struct sta_info *psta)
-{
-	u16 media_status;
-
-	if (psta == NULL)	return;
-
-	media_status = (psta->mac_id<<8)|1; /*   MACID|OPMODE:1 connect */
-	rtw_hal_set_hwreg23a(adapter, HW_VAR_H2C_MEDIA_STATUS_RPT, (u8 *)&media_status);
 }
 
-void rtw_stassoc_event_callback23a(struct rtw_adapter *adapter, u8 *pbuf)
+void rtw_stassoc_event_callback23a(struct rtw_adapter *adapter, const u8 *pbuf)
 {
 	struct sta_info *psta;
 	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
-	struct stassoc_event	*pstassoc	= (struct stassoc_event*)pbuf;
+	struct stassoc_event *pstassoc = (struct stassoc_event*)pbuf;
 	struct wlan_network *cur_network = &pmlmepriv->cur_network;
-	struct wlan_network	*ptarget_wlan = NULL;
+	struct wlan_network *ptarget_wlan;
 
-	if(rtw_access_ctrl23a(adapter, pstassoc->macaddr) == false)
+	if (rtw_access_ctrl23a(adapter, pstassoc->macaddr) == false)
 		return;
 
 #ifdef CONFIG_8723AU_AP_MODE
-	if(check_fwstate(pmlmepriv, WIFI_AP_STATE))
-	{
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
 		psta = rtw_get_stainfo23a(&adapter->stapriv, pstassoc->macaddr);
 		if (psta) {
 			/* bss_cap_update_on_sta_join23a(adapter, psta); */
 			/* sta_info_update23a(adapter, psta); */
 			ap_sta_info_defer_update23a(adapter, psta);
-
-			rtw_stassoc_hw_rpt23a(adapter,psta);
 		}
 		return;
 	}
@@ -1336,13 +1370,20 @@ void rtw_stassoc_event_callback23a(struct rtw_adapter *adapter, u8 *pbuf)
 	psta = rtw_get_stainfo23a(&adapter->stapriv, pstassoc->macaddr);
 	if (psta != NULL) {
 		/* the sta have been in sta_info_queue => do nothing */
-		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("Error: rtw_stassoc_event_callback23a: sta has been in sta_hash_queue\n"));
-		return; /* between drv has received this event before and  fw have not yet to set key to CAM_ENTRY) */
+		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,
+			 ("Error: rtw_stassoc_event_callback23a: sta has "
+			  "been in sta_hash_queue\n"));
+		/* between drv has received this event before and
+		   fw have not yet to set key to CAM_ENTRY) */
+		return;
 	}
 
-	psta = rtw_alloc_stainfo23a(&adapter->stapriv, pstassoc->macaddr);
-	if (psta == NULL) {
-		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("Can't alloc sta_info when rtw_stassoc_event_callback23a\n"));
+	psta = rtw_alloc_stainfo23a(&adapter->stapriv, pstassoc->macaddr,
+		GFP_KERNEL);
+	if (!psta) {
+		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,
+			 ("Can't alloc sta_info when "
+			  "rtw_stassoc_event_callback23a\n"));
 		return;
 	}
 
@@ -1352,25 +1393,25 @@ void rtw_stassoc_event_callback23a(struct rtw_adapter *adapter, u8 *pbuf)
 	/* psta->aid = (uint)pstassoc->cam_id; */
 	DBG_8723A("%s\n",__func__);
 	/* for ad-hoc mode */
-	rtw_hal_set_odm_var23a(adapter,HAL_ODM_STA_INFO,psta,true);
+	rtl8723a_SetHalODMVar(adapter, HAL_ODM_STA_INFO, psta, true);
 
-	rtw_stassoc_hw_rpt23a(adapter,psta);
-
-	if(adapter->securitypriv.dot11AuthAlgrthm==dot11AuthAlgrthm_8021X)
-		psta->dot118021XPrivacy = adapter->securitypriv.dot11PrivacyAlgrthm;
+	if (adapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X)
+		psta->dot118021XPrivacy =
+			adapter->securitypriv.dot11PrivacyAlgrthm;
 
 	psta->ieee8021x_blocked = false;
 
 	spin_lock_bh(&pmlmepriv->lock);
 
-	if ( (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)==true ) ||
-		(check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)==true ) )
-	{
-		if(adapter->stapriv.asoc_sta_count== 2)
-		{
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) ||
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
+		if (adapter->stapriv.asoc_sta_count == 2) {
 			spin_lock_bh(&pmlmepriv->scanned_queue.lock);
-			ptarget_wlan = rtw_find_network23a(&pmlmepriv->scanned_queue, cur_network->network.MacAddress);
-			if(ptarget_wlan)	ptarget_wlan->fixed = true;
+			ptarget_wlan =
+				rtw_find_network23a(&pmlmepriv->scanned_queue,
+						    cur_network->network.MacAddress);
+			if (ptarget_wlan)
+				ptarget_wlan->fixed = true;
 			spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
 			/*  a sta + bc/mc_stainfo (not Ibss_stainfo) */
 			rtw_indicate_connect23a(adapter);
@@ -1382,49 +1423,42 @@ void rtw_stassoc_event_callback23a(struct rtw_adapter *adapter, u8 *pbuf)
 	mlmeext_sta_add_event_callback23a(adapter, psta);
 }
 
-void rtw_stadel_event_callback23a(struct rtw_adapter *adapter, u8 *pbuf)
+void rtw_stadel_event_callback23a(struct rtw_adapter *adapter, const u8 *pbuf)
 {
-	int mac_id=-1;
+	int mac_id;
 	struct sta_info *psta;
-	struct wlan_network* pwlan = NULL;
-	struct wlan_bssid_ex    *pdev_network=NULL;
-	u8* pibss = NULL;
-	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
-	struct	stadel_event *pstadel	= (struct stadel_event*)pbuf;
-	struct	sta_priv *pstapriv = &adapter->stapriv;
+	struct wlan_network* pwlan;
+	struct wlan_bssid_ex *pdev_network;
+	u8 *pibss;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	struct stadel_event *pstadel = (struct stadel_event *)pbuf;
+	struct sta_priv *pstapriv = &adapter->stapriv;
 	struct wlan_network *tgt_network = &pmlmepriv->cur_network;
 
 	psta = rtw_get_stainfo23a(&adapter->stapriv, pstadel->macaddr);
-	if(psta)
+	if (psta)
 		mac_id = psta->mac_id;
 	else
 		mac_id = pstadel->mac_id;
 
-	DBG_8723A("%s(mac_id=%d)=" MAC_FMT "\n", __func__, mac_id, MAC_ARG(pstadel->macaddr));
-
-	if(mac_id>=0) {
-		u16 media_status;
-		media_status = (mac_id<<8)|0; /*   MACID|OPMODE:0 means disconnect */
-		/* for STA,AP,ADHOC mode, report disconnect stauts to FW */
-		rtw_hal_set_hwreg23a(adapter, HW_VAR_H2C_MEDIA_STATUS_RPT, (u8 *)&media_status);
-	}
+	DBG_8723A("%s(mac_id=%d)=" MAC_FMT "\n", __func__, mac_id,
+		  MAC_ARG(pstadel->macaddr));
 
         if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
-        {
 		return;
-        }
 
 	mlmeext_sta_del_event_callback23a(adapter);
 
 	spin_lock_bh(&pmlmepriv->lock);
 
-	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
-	{
-		if (rtw_to_roaming(adapter) > 0)
-			pmlmepriv->to_roaming--; /* this stadel_event is caused by roaming, decrease to_roaming */
-		else if (rtw_to_roaming(adapter) == 0)
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+		if (adapter->mlmepriv.to_roaming > 0) {
+			/* this stadel_event is caused by roaming,
+			   decrease to_roaming */
+			pmlmepriv->to_roaming--;
+		} else if (adapter->mlmepriv.to_roaming == 0)
 			rtw_set_roaming(adapter, adapter->registrypriv.max_roaming_times);
-		if (*((unsigned short *)(pstadel->rsvd)) != WLAN_REASON_EXPIRATION_CHK)
+		if (*((u16 *)pstadel->rsvd) != WLAN_REASON_EXPIRATION_CHK)
 			rtw_set_roaming(adapter, 0); /* don't roam */
 
 		rtw_free_uc_swdec_pending_queue23a(adapter);
@@ -1433,32 +1467,33 @@ void rtw_stadel_event_callback23a(struct rtw_adapter *adapter, u8 *pbuf)
 		rtw_indicate_disconnect23a(adapter);
 		spin_lock_bh(&pmlmepriv->scanned_queue.lock);
 		/*  remove the network entry in scanned_queue */
-		pwlan = rtw_find_network23a(&pmlmepriv->scanned_queue, tgt_network->network.MacAddress);
+		pwlan = rtw_find_network23a(&pmlmepriv->scanned_queue,
+					    tgt_network->network.MacAddress);
 		if (pwlan) {
 			pwlan->fixed = false;
 			rtw_free_network_nolock(pmlmepriv, pwlan);
 		}
 		spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
 
-		_rtw23a_roaming(adapter, tgt_network);
+		_rtw_roaming(adapter, tgt_network);
 	}
 
 	if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) ||
-	      check_fwstate(pmlmepriv, WIFI_ADHOC_STATE))
-	{
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
 
 		spin_lock_bh(&pstapriv->sta_hash_lock);
 		rtw_free_stainfo23a(adapter,  psta);
 		spin_unlock_bh(&pstapriv->sta_hash_lock);
 
-		if (adapter->stapriv.asoc_sta_count == 1) /* a sta + bc/mc_stainfo (not Ibss_stainfo) */
-		{
+		/* a sta + bc/mc_stainfo (not Ibss_stainfo) */
+		if (adapter->stapriv.asoc_sta_count == 1) {
 			spin_lock_bh(&pmlmepriv->scanned_queue.lock);
 			/* free old ibss network */
-			/* pwlan = rtw_find_network23a(&pmlmepriv->scanned_queue, pstadel->macaddr); */
-			pwlan = rtw_find_network23a(&pmlmepriv->scanned_queue, tgt_network->network.MacAddress);
-			if (pwlan)
-			{
+			/* pwlan = rtw_find_network23a(
+			   &pmlmepriv->scanned_queue, pstadel->macaddr); */
+			pwlan = rtw_find_network23a(&pmlmepriv->scanned_queue,
+						    tgt_network->network.MacAddress);
+			if (pwlan) {
 				pwlan->fixed = false;
 				rtw_free_network_nolock(pmlmepriv, pwlan);
 			}
@@ -1467,7 +1502,8 @@ void rtw_stadel_event_callback23a(struct rtw_adapter *adapter, u8 *pbuf)
 			pdev_network = &adapter->registrypriv.dev_network;
 			pibss = adapter->registrypriv.dev_network.MacAddress;
 
-			memcpy(pdev_network, &tgt_network->network, get_wlan_bssid_ex_sz(&tgt_network->network));
+			memcpy(pdev_network, &tgt_network->network,
+			       get_wlan_bssid_ex_sz(&tgt_network->network));
 
 			memset(&pdev_network->Ssid, 0,
 			       sizeof(struct cfg80211_ssid));
@@ -1478,32 +1514,22 @@ void rtw_stadel_event_callback23a(struct rtw_adapter *adapter, u8 *pbuf)
 
 			rtw_generate_random_ibss23a(pibss);
 
-			if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE))
-			{
+			if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
 				set_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE);
 				_clr_fwstate_(pmlmepriv, WIFI_ADHOC_STATE);
 			}
 
-			if (rtw_createbss_cmd23a(adapter)!= _SUCCESS)
-			{
-
-				RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("***Error =>stadel_event_callback: rtw_createbss_cmd23a status FAIL***\n "));
-
+			if (rtw_createbss_cmd23a(adapter) != _SUCCESS) {
+				RT_TRACE(_module_rtl871x_ioctl_set_c_,
+					 _drv_err_,
+					 ("***Error =>stadel_event_callback: "
+					  "rtw_createbss_cmd23a status "
+					  "FAIL***\n"));
 			}
-
 		}
-
 	}
 
 	spin_unlock_bh(&pmlmepriv->lock);
-
-}
-
-void rtw_cpwm_event_callback23a(struct rtw_adapter *padapter, u8 *pbuf)
-{
-
-	RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("+rtw_cpwm_event_callback23a !!!\n"));
-
 }
 
 /*
@@ -1518,23 +1544,28 @@ void rtw23a_join_to_handler (unsigned long data)
 
 	DBG_8723A("%s, fw_state=%x\n", __func__, get_fwstate(pmlmepriv));
 
-	if(adapter->bDriverStopped ||adapter->bSurpriseRemoved)
+	if (adapter->bDriverStopped ||adapter->bSurpriseRemoved)
 		return;
 
 	spin_lock_bh(&pmlmepriv->lock);
 
-	if (rtw_to_roaming(adapter) > 0) { /* join timeout caused by roaming */
-		while(1) {
+	if (adapter->mlmepriv.to_roaming > 0) {
+		/* join timeout caused by roaming */
+		while (1) {
 			pmlmepriv->to_roaming--;
-			if (rtw_to_roaming(adapter) != 0) { /* try another */
+			if (adapter->mlmepriv.to_roaming != 0) {
+				/* try another */
 				DBG_8723A("%s try another roaming\n", __func__);
-				if (_SUCCESS!= (do_join_r = rtw_do_join23a(adapter))) {
-					DBG_8723A("%s roaming do_join return %d\n", __func__ , do_join_r);
+				do_join_r = rtw_do_join23a(adapter);
+				if (do_join_r != _SUCCESS) {
+					DBG_8723A("%s roaming do_join return "
+						  "%d\n", __func__ , do_join_r);
 					continue;
 				}
 				break;
 			} else {
-				DBG_8723A("%s We've try roaming but fail\n", __func__);
+				DBG_8723A("%s We've try roaming but fail\n",
+					  __func__);
 				rtw_indicate_disconnect23a(adapter);
 				break;
 			}
@@ -1543,7 +1574,8 @@ void rtw23a_join_to_handler (unsigned long data)
 		rtw_indicate_disconnect23a(adapter);
 		free_scanqueue(pmlmepriv);/*  */
 
-		/* indicate disconnect for the case that join_timeout and check_fwstate != FW_LINKED */
+		/* indicate disconnect for the case that join_timeout and
+		   check_fwstate != FW_LINKED */
 		rtw_cfg80211_indicate_disconnect(adapter);
 	}
 
@@ -1560,7 +1592,8 @@ void rtw_scan_timeout_handler23a(unsigned long data)
 	struct rtw_adapter *adapter = (struct rtw_adapter *)data;
 	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
 
-	DBG_8723A(FUNC_ADPT_FMT" fw_state =%x\n", FUNC_ADPT_ARG(adapter), get_fwstate(pmlmepriv));
+	DBG_8723A("%s(%s): fw_state =%x\n", __func__, adapter->pnetdev->name,
+		  get_fwstate(pmlmepriv));
 
 	spin_lock_bh(&pmlmepriv->lock);
 
@@ -1568,7 +1601,7 @@ void rtw_scan_timeout_handler23a(unsigned long data)
 
 	spin_unlock_bh(&pmlmepriv->lock);
 
-	rtw_indicate_scan_done23a(adapter, true);
+	rtw_cfg80211_indicate_scan_done(wdev_to_priv(adapter->rtw_wdev), true);
 }
 
 static void rtw_auto_scan_handler(struct rtw_adapter *padapter)
@@ -1581,7 +1614,8 @@ static void rtw_auto_scan_handler(struct rtw_adapter *padapter)
 		if (pmlmepriv->scan_interval == 0) {
 			DBG_8723A("%s\n", __func__);
 			rtw_set_802_11_bssid23a_list_scan(padapter, NULL, 0);
-			pmlmepriv->scan_interval = SCAN_INTERVAL;/*  30*2 sec = 60sec */
+			/*  30*2 sec = 60sec */
+			pmlmepriv->scan_interval = SCAN_INTERVAL;
 		}
 	}
 }
@@ -1594,7 +1628,8 @@ void rtw_dynamic_check_timer_handler(unsigned long data)
 	if (adapter->hw_init_completed == false)
 		goto out;
 
-	if ((adapter->bDriverStopped == true)||(adapter->bSurpriseRemoved == true))
+	if (adapter->bDriverStopped == true ||
+	    adapter->bSurpriseRemoved == true)
 		goto out;
 
 	if (adapter->net_closed == true)
@@ -1602,16 +1637,9 @@ void rtw_dynamic_check_timer_handler(unsigned long data)
 
 	rtw_dynamic_chk_wk_cmd23a(adapter);
 
-	if (pregistrypriv->wifi_spec == 1)
-	{
-#ifdef CONFIG_8723AU_P2P
-		struct wifidirect_info *pwdinfo = &adapter->wdinfo;
-		if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
-#endif
-		{
-			/* auto site survey */
-			rtw_auto_scan_handler(adapter);
-		}
+	if (pregistrypriv->wifi_spec == 1) {
+		/* auto site survey */
+		rtw_auto_scan_handler(adapter);
 	}
 out:
 	mod_timer(&adapter->mlmepriv.dynamic_chk_timer,
@@ -1628,8 +1656,6 @@ void rtw_clear_scan_deny(struct rtw_adapter *adapter)
 {
 	struct mlme_priv *mlmepriv = &adapter->mlmepriv;
 	atomic_set(&mlmepriv->set_scan_deny, 0);
-	if (0)
-	DBG_8723A(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(adapter));
 }
 
 void rtw_set_scan_deny_timer_hdl(unsigned long data)
@@ -1642,12 +1668,9 @@ void rtw_set_scan_deny(struct rtw_adapter *adapter, u32 ms)
 {
 	struct mlme_priv *mlmepriv = &adapter->mlmepriv;
 
-	if (0)
-	DBG_8723A(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(adapter));
 	atomic_set(&mlmepriv->set_scan_deny, 1);
 	mod_timer(&mlmepriv->set_scan_deny_timer,
 		  jiffies + msecs_to_jiffies(ms));
-
 }
 
 #if defined(IEEE80211_SCAN_RESULT_EXPIRE)
@@ -1657,15 +1680,19 @@ void rtw_set_scan_deny(struct rtw_adapter *adapter, u32 ms)
 #endif
 
 /*
-* Select a new join candidate from the original @param candidate and @param competitor
+* Select a new join candidate from the original @param candidate and
+*     @param competitor
 * @return true: candidate is updated
 * @return false: candidate is not updated
 */
-static int rtw_check_join_candidate(struct mlme_priv *pmlmepriv
-	, struct wlan_network **candidate, struct wlan_network *competitor)
+static int rtw_check_join_candidate(struct mlme_priv *pmlmepriv,
+				    struct wlan_network **candidate,
+				    struct wlan_network *competitor)
 {
 	int updated = false;
-	struct rtw_adapter *adapter = container_of(pmlmepriv, struct rtw_adapter, mlmepriv);
+	struct rtw_adapter *adapter;
+
+	adapter = container_of(pmlmepriv, struct rtw_adapter, mlmepriv);
 
 	/* check bssid, if needed */
 	if (pmlmepriv->assoc_by_bssid == true) {
@@ -1684,10 +1711,10 @@ static int rtw_check_join_candidate(struct mlme_priv *pmlmepriv
 			goto exit;
 	}
 
-	if (rtw_is_desired_network(adapter, competitor)  == false)
+	if (rtw_is_desired_network(adapter, competitor) == false)
 		goto exit;
 
-	if (rtw_to_roaming(adapter) > 0) {
+	if (adapter->mlmepriv.to_roaming > 0) {
 		unsigned int passed;
 
 		passed = jiffies_to_msecs(jiffies - competitor->last_scanned);
@@ -1697,19 +1724,21 @@ static int rtw_check_join_candidate(struct mlme_priv *pmlmepriv
 			goto exit;
 	}
 
-	if (*candidate == NULL ||(*candidate)->network.Rssi<competitor->network.Rssi) {
+	if (!*candidate ||
+	    (*candidate)->network.Rssi<competitor->network.Rssi) {
 		*candidate = competitor;
 		updated = true;
 	}
 
 	if (updated) {
-		DBG_8723A("[by_bssid:%u][assoc_ssid:%s][to_roaming:%u] new candidate: %s("MAC_FMT") rssi:%d\n",
-			pmlmepriv->assoc_by_bssid,
-			pmlmepriv->assoc_ssid.ssid,
-			rtw_to_roaming(adapter),
-			(*candidate)->network.Ssid.ssid,
-			MAC_ARG((*candidate)->network.MacAddress),
-			(int)(*candidate)->network.Rssi);
+		DBG_8723A("[by_bssid:%u][assoc_ssid:%s][to_roaming:%u] "
+			  "new candidate: %s("MAC_FMT") rssi:%d\n",
+			  pmlmepriv->assoc_by_bssid,
+			  pmlmepriv->assoc_ssid.ssid,
+			  adapter->mlmepriv.to_roaming,
+			  (*candidate)->network.Ssid.ssid,
+			  MAC_ARG((*candidate)->network.MacAddress),
+			  (int)(*candidate)->network.Rssi);
 	}
 
 exit:
@@ -1732,8 +1761,8 @@ int rtw_select_and_join_from_scanned_queue23a(struct mlme_priv *pmlmepriv)
 	struct list_head *phead, *plist, *ptmp;
 	struct rtw_adapter *adapter;
 	struct rtw_queue *queue = &pmlmepriv->scanned_queue;
-	struct	wlan_network	*pnetwork = NULL;
-	struct	wlan_network	*candidate = NULL;
+	struct wlan_network *pnetwork;
+	struct wlan_network *candidate = NULL;
 
 	spin_lock_bh(&pmlmepriv->scanned_queue.lock);
 	phead = get_list_head(queue);
@@ -1760,11 +1789,11 @@ int rtw_select_and_join_from_scanned_queue23a(struct mlme_priv *pmlmepriv)
 		DBG_8723A("%s: candidate: %s("MAC_FMT", ch:%u)\n", __func__,
 			  candidate->network.Ssid.ssid,
 			  MAC_ARG(candidate->network.MacAddress),
-			  candidate->network.Configuration.DSConfig);
+			  candidate->network.DSConfig);
 	}
 
 	/*  check for situation of  _FW_LINKED */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
+	if (check_fwstate(pmlmepriv, _FW_LINKED)) {
 		DBG_8723A("%s: _FW_LINKED while ask_for_joinbss!!!\n",
 			  __func__);
 
@@ -1782,7 +1811,7 @@ exit:
 }
 
 int rtw_set_auth23a(struct rtw_adapter * adapter,
-		 struct security_priv *psecuritypriv)
+		    struct security_priv *psecuritypriv)
 {
 	struct cmd_obj* pcmd;
 	struct setauth_parm *psetauthparm;
@@ -1795,8 +1824,7 @@ int rtw_set_auth23a(struct rtw_adapter * adapter,
 		goto exit;
 	}
 
-	psetauthparm = (struct setauth_parm*)
-		kzalloc(sizeof(struct setauth_parm), GFP_KERNEL);
+	psetauthparm = kzalloc(sizeof(struct setauth_parm), GFP_KERNEL);
 	if (!psetauthparm) {
 		kfree(pcmd);
 		res = _FAIL;
@@ -1811,8 +1839,6 @@ int rtw_set_auth23a(struct rtw_adapter * adapter,
 	pcmd->rsp = NULL;
 	pcmd->rspsz = 0;
 
-	INIT_LIST_HEAD(&pcmd->list);
-
 	RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,
 		 ("after enqueue set_auth_cmd, auth_mode=%x\n",
 		  psecuritypriv->dot11AuthAlgrthm));
@@ -1825,7 +1851,7 @@ exit:
 }
 
 int rtw_set_key23a(struct rtw_adapter *adapter,
-		struct security_priv *psecuritypriv, int keyid, u8 set_tx)
+		   struct security_priv *psecuritypriv, int keyid, u8 set_tx)
 {
 	u8 keylen;
 	struct cmd_obj *pcmd;
@@ -1834,7 +1860,12 @@ int rtw_set_key23a(struct rtw_adapter *adapter,
 	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
 	int res = _SUCCESS;
 
-	pcmd = (struct cmd_obj *)kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
+	if (keyid >= 4) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pcmd = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
 	if (!pcmd) {
 		res = _FAIL;  /* try again */
 		goto exit;
@@ -1850,9 +1881,9 @@ int rtw_set_key23a(struct rtw_adapter *adapter,
 		psetkeyparm->algorithm = (unsigned char)
 			psecuritypriv->dot118021XGrpPrivacy;
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-			 ("\n rtw_set_key23a: psetkeyparm->algorithm = (unsigned "
-			  "char)psecuritypriv->dot118021XGrpPrivacy =%d\n",
-			  psetkeyparm->algorithm));
+			 ("\n rtw_set_key23a: psetkeyparm->algorithm = "
+			  "(unsigned char)psecuritypriv->dot118021XGrpPrivacy "
+			  "=%d\n", psetkeyparm->algorithm));
 	} else {
 		psetkeyparm->algorithm = (u8)psecuritypriv->dot11PrivacyAlgrthm;
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
@@ -1860,10 +1891,10 @@ int rtw_set_key23a(struct rtw_adapter *adapter,
 			  "psecuritypriv->dot11PrivacyAlgrthm =%d\n",
 			  psetkeyparm->algorithm));
 	}
-	psetkeyparm->keyid = (u8)keyid;/* 0~3 */
+	psetkeyparm->keyid = keyid;/* 0~3 */
 	psetkeyparm->set_tx = set_tx;
 	if (is_wep_enc(psetkeyparm->algorithm))
-		pmlmepriv->key_mask |= CHKBIT(psetkeyparm->keyid);
+		pmlmepriv->key_mask |= BIT(psetkeyparm->keyid);
 
 	DBG_8723A("==> rtw_set_key23a algorithm(%x), keyid(%x), key_mask(%x)\n",
 		  psetkeyparm->algorithm, psetkeyparm->keyid,
@@ -1873,23 +1904,23 @@ int rtw_set_key23a(struct rtw_adapter *adapter,
 		  "keyid = (u8)keyid =%d\n", psetkeyparm->algorithm, keyid));
 
 	switch (psetkeyparm->algorithm) {
-	case _WEP40_:
+	case WLAN_CIPHER_SUITE_WEP40:
 		keylen = 5;
 		memcpy(&psetkeyparm->key[0],
-		       &psecuritypriv->dot11DefKey[keyid].skey[0], keylen);
+		       &psecuritypriv->wep_key[keyid].key, keylen);
 		break;
-	case _WEP104_:
+	case WLAN_CIPHER_SUITE_WEP104:
 		keylen = 13;
 		memcpy(&psetkeyparm->key[0],
-		       &psecuritypriv->dot11DefKey[keyid].skey[0], keylen);
+		       &psecuritypriv->wep_key[keyid].key, keylen);
 		break;
-	case _TKIP_:
+	case WLAN_CIPHER_SUITE_TKIP:
 		keylen = 16;
 		memcpy(&psetkeyparm->key,
 		       &psecuritypriv->dot118021XGrpKey[keyid], keylen);
 		psetkeyparm->grpkey = 1;
 		break;
-	case _AES_:
+	case WLAN_CIPHER_SUITE_CCMP:
 		keylen = 16;
 		memcpy(&psetkeyparm->key,
 		       &psecuritypriv->dot118021XGrpKey[keyid], keylen);
@@ -1897,8 +1928,8 @@ int rtw_set_key23a(struct rtw_adapter *adapter,
 		break;
 	default:
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-			 ("\n rtw_set_key23a:psecuritypriv->dot11PrivacyAlgrthm = "
-			  "%x (must be 1 or 2 or 4 or 5)\n",
+			 ("\n rtw_set_key23a:psecuritypriv->dot11PrivacyAlgrthm"
+			  " = %x (must be 1 or 2 or 4 or 5)\n",
 			  psecuritypriv->dot11PrivacyAlgrthm));
 		res = _FAIL;
 		kfree(pcmd);
@@ -1912,8 +1943,6 @@ int rtw_set_key23a(struct rtw_adapter *adapter,
 	pcmd->rsp = NULL;
 	pcmd->rspsz = 0;
 
-	INIT_LIST_HEAD(&pcmd->list);
-
 	/* sema_init(&pcmd->cmd_sem, 0); */
 
 	res = rtw_enqueue_cmd23a(pcmdpriv, pcmd);
@@ -1931,7 +1960,7 @@ int rtw_restruct_wmm_ie23a(struct rtw_adapter *adapter, u8 *in_ie,
 	unsigned int i, j;
 
 	i = 12; /* after the fixed IE */
-	while(i < in_len) {
+	while (i < in_len) {
 		ielength = initial_out_len;
 
 		/* WMM element ID and OUI */
@@ -1982,11 +2011,11 @@ static int SecIsInPMKIDList(struct rtw_adapter *Adapter, u8 *bssid)
 			i++;
 			/* continue; */
 		}
-	} while(i < NUM_PMKID_CACHE);
+	} while (i < NUM_PMKID_CACHE);
 
-	if (i == NUM_PMKID_CACHE) {
+	if (i == NUM_PMKID_CACHE)
 		i = -1;/*  Could not find. */
-	} else {
+	else {
 		/*  There is one Pre-Authentication Key for
 		    the specific BSSID. */
 	}
@@ -2023,8 +2052,9 @@ static int rtw_append_pmkid(struct rtw_adapter *Adapter, int iEntry,
 	}
 	return ie_len;
 }
+
 int rtw_restruct_sec_ie23a(struct rtw_adapter *adapter, u8 *in_ie, u8 *out_ie,
-			uint in_len)
+			   uint in_len)
 {
 	u8 authmode;
 	uint ielength;
@@ -2035,41 +2065,39 @@ int rtw_restruct_sec_ie23a(struct rtw_adapter *adapter, u8 *in_ie, u8 *out_ie,
 	uint ndissecuritytype = psecuritypriv->ndisencryptstatus;
 
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
-		 ("+rtw_restruct_sec_ie23a: ndisauthmode=%d ndissecuritytype=%d\n",
-		  ndisauthmode, ndissecuritytype));
+		 ("+rtw_restruct_sec_ie23a: ndisauthmode=%d "
+		  "ndissecuritytype=%d\n", ndisauthmode, ndissecuritytype));
 
 	/* copy fixed ie only */
 	memcpy(out_ie, in_ie, 12);
 	ielength = 12;
-	if ((ndisauthmode==Ndis802_11AuthModeWPA) ||
-	    (ndisauthmode==Ndis802_11AuthModeWPAPSK))
-		authmode=_WPA_IE_ID_;
-	if ((ndisauthmode==Ndis802_11AuthModeWPA2) ||
-	    (ndisauthmode==Ndis802_11AuthModeWPA2PSK))
-		authmode=_WPA2_IE_ID_;
+	if (ndisauthmode == Ndis802_11AuthModeWPA ||
+	    ndisauthmode == Ndis802_11AuthModeWPAPSK)
+		authmode = WLAN_EID_VENDOR_SPECIFIC;
+	if (ndisauthmode == Ndis802_11AuthModeWPA2 ||
+	    ndisauthmode == Ndis802_11AuthModeWPA2PSK)
+		authmode = _WPA2_IE_ID_;
 
 	if (check_fwstate(pmlmepriv, WIFI_UNDER_WPS)) {
 		memcpy(out_ie + ielength, psecuritypriv->wps_ie,
 		       psecuritypriv->wps_ie_len);
 
 		ielength += psecuritypriv->wps_ie_len;
-	} else if ((authmode==_WPA_IE_ID_) || (authmode==_WPA2_IE_ID_)) {
+	} else if (authmode == WLAN_EID_VENDOR_SPECIFIC ||
+		   authmode == _WPA2_IE_ID_) {
 		/* copy RSN or SSN */
 		memcpy(&out_ie[ielength], &psecuritypriv->supplicant_ie[0],
 		       psecuritypriv->supplicant_ie[1] + 2);
 		ielength += psecuritypriv->supplicant_ie[1] + 2;
-		rtw_report_sec_ie23a(adapter, authmode,
-				  psecuritypriv->supplicant_ie);
 	}
 
 	iEntry = SecIsInPMKIDList(adapter, pmlmepriv->assoc_bssid);
-	if (iEntry < 0)	{
+	if (iEntry < 0)
 		return ielength;
-	} else {
-		if (authmode == _WPA2_IE_ID_) {
-			ielength=rtw_append_pmkid(adapter, iEntry,
-						  out_ie, ielength);
-		}
+	else {
+		if (authmode == _WPA2_IE_ID_)
+			ielength = rtw_append_pmkid(adapter, iEntry,
+						    out_ie, ielength);
 	}
 
 	return ielength;
@@ -2087,13 +2115,7 @@ void rtw_init_registrypriv_dev_network23a(struct rtw_adapter* adapter)
 	memcpy(&pdev_network->Ssid, &pregistrypriv->ssid,
 	       sizeof(struct cfg80211_ssid));
 
-	pdev_network->Configuration.Length=sizeof(struct ndis_802_11_config);
-	pdev_network->Configuration.BeaconPeriod = 100;
-	pdev_network->Configuration.FHConfig.Length = 0;
-	pdev_network->Configuration.FHConfig.HopPattern = 0;
-	pdev_network->Configuration.FHConfig.HopSet = 0;
-	pdev_network->Configuration.FHConfig.DwellTime = 0;
-
+	pdev_network->BeaconPeriod = 100;
 }
 
 void rtw_update_registrypriv_dev_network23a(struct rtw_adapter* adapter)
@@ -2110,44 +2132,15 @@ void rtw_update_registrypriv_dev_network23a(struct rtw_adapter* adapter)
 
 	pdev_network->Rssi = 0;
 
-	switch (pregistrypriv->wireless_mode)
-	{
-	case WIRELESS_11B:
-		pdev_network->NetworkTypeInUse = Ndis802_11DS;
-		break;
-	case WIRELESS_11G:
-	case WIRELESS_11BG:
-	case WIRELESS_11_24N:
-	case WIRELESS_11G_24N:
-	case WIRELESS_11BG_24N:
-		pdev_network->NetworkTypeInUse = Ndis802_11OFDM24;
-		break;
-	case WIRELESS_11A:
-	case WIRELESS_11A_5N:
-		pdev_network->NetworkTypeInUse = Ndis802_11OFDM5;
-		break;
-	case WIRELESS_11ABGN:
-		if (pregistrypriv->channel > 14)
-			pdev_network->NetworkTypeInUse = Ndis802_11OFDM5;
-		else
-			pdev_network->NetworkTypeInUse = Ndis802_11OFDM24;
-		break;
-	default :
-		/*  TODO */
-		break;
-	}
-
-	pdev_network->Configuration.DSConfig = pregistrypriv->channel;
+	pdev_network->DSConfig = pregistrypriv->channel;
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-		 ("pregistrypriv->channel =%d, pdev_network->Configuration."
-		  "DSConfig = 0x%x\n", pregistrypriv->channel,
-		  pdev_network->Configuration.DSConfig));
+		 ("pregistrypriv->channel =%d, pdev_network->DSConfig = 0x%x\n",
+		  pregistrypriv->channel, pdev_network->DSConfig));
 
-	if (cur_network->network.InfrastructureMode == Ndis802_11IBSS)
-		pdev_network->Configuration.ATIMWindow = 0;
+	if (cur_network->network.ifmode == NL80211_IFTYPE_ADHOC)
+		pdev_network->ATIMWindow = 0;
 
-	pdev_network->InfrastructureMode =
-		cur_network->network.InfrastructureMode;
+	pdev_network->ifmode = cur_network->network.ifmode;
 
 	/*  1. Supported rates */
 	/*  2. IE */
@@ -2157,12 +2150,11 @@ void rtw_update_registrypriv_dev_network23a(struct rtw_adapter* adapter)
 	pdev_network->IELength = sz;
 
 	pdev_network->Length =
-		get_wlan_bssid_ex_sz((struct wlan_bssid_ex *)pdev_network);
+		get_wlan_bssid_ex_sz(pdev_network);
 
 	/* notes: translate IELength & Length after assign the
 	   Length to cmdsz in createbss_cmd(); */
 	/* pdev_network->IELength = cpu_to_le32(sz); */
-
 }
 
 void rtw_get_encrypt_decrypt_from_registrypriv23a(struct rtw_adapter* adapter)
@@ -2193,41 +2185,38 @@ void rtw_joinbss_reset23a(struct rtw_adapter *padapter)
 			threshold = 1;
 		else
 			threshold = 0;
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_RXDMA_AGG_PG_TH,
-				  (u8 *)(&threshold));
-	} else {
+	} else
 		threshold = 1;
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_RXDMA_AGG_PG_TH,
-				  (u8 *)(&threshold));
-	}
+
+	rtl8723a_set_rxdma_agg_pg_th(padapter, threshold);
 }
 
 /* the fucntion is >= passive_level */
 unsigned int rtw_restructure_ht_ie23a(struct rtw_adapter *padapter, u8 *in_ie,
-				   u8 *out_ie, uint in_len, uint *pout_len)
+				      u8 *out_ie, uint in_len, uint *pout_len)
 {
-	u32 ielen, out_len;
+	u32 out_len;
 	int max_rx_ampdu_factor;
-	unsigned char *p, *pframe;
+	unsigned char *pframe;
+	const u8 *p;
 	struct ieee80211_ht_cap ht_capie;
 	unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01, 0x00};
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct qos_priv *pqospriv = &pmlmepriv->qospriv;
 	struct ht_priv *phtpriv = &pmlmepriv->htpriv;
 
 	phtpriv->ht_option = false;
 
-	p = rtw_get_ie23a(in_ie + 12, _HT_CAPABILITY_IE_, &ielen, in_len - 12);
+	p = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY, in_ie + 12, in_len -12);
 
-	if (p && ielen > 0) {
+	if (p && p[1] > 0) {
 		u32 rx_packet_offset, max_recvbuf_sz;
-		if (pqospriv->qos_option == 0) {
+		if (pmlmepriv->qos_option == 0) {
 			out_len = *pout_len;
 			pframe = rtw_set_ie23a(out_ie + out_len,
-					    _VENDOR_SPECIFIC_IE_,
+					    WLAN_EID_VENDOR_SPECIFIC,
 					    _WMM_IE_Length_, WMM_IE, pout_len);
 
-			pqospriv->qos_option = 1;
+			pmlmepriv->qos_option = 1;
 		}
 
 		out_len = *pout_len;
@@ -2238,33 +2227,36 @@ unsigned int rtw_restructure_ht_ie23a(struct rtw_adapter *padapter, u8 *in_ie,
 			IEEE80211_HT_CAP_SGI_20 | IEEE80211_HT_CAP_SGI_40 |
 			IEEE80211_HT_CAP_TX_STBC | IEEE80211_HT_CAP_DSSSCCK40;
 
-		rtw_hal_get_def_var23a(padapter, HAL_DEF_RX_PACKET_OFFSET,
-				    &rx_packet_offset);
-		rtw_hal_get_def_var23a(padapter, HAL_DEF_MAX_RECVBUF_SZ,
-				    &max_recvbuf_sz);
+		GetHalDefVar8192CUsb(padapter, HAL_DEF_RX_PACKET_OFFSET,
+				     &rx_packet_offset);
+		GetHalDefVar8192CUsb(padapter, HAL_DEF_MAX_RECVBUF_SZ,
+				     &max_recvbuf_sz);
 
-		rtw_hal_get_def_var23a(padapter, HW_VAR_MAX_RX_AMPDU_FACTOR,
-				    &max_rx_ampdu_factor);
+		GetHalDefVar8192CUsb(padapter, HW_VAR_MAX_RX_AMPDU_FACTOR,
+				     &max_rx_ampdu_factor);
 		ht_capie.ampdu_params_info = max_rx_ampdu_factor & 0x03;
 
-		if (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)
+		if (padapter->securitypriv.dot11PrivacyAlgrthm ==
+		    WLAN_CIPHER_SUITE_CCMP)
 			ht_capie.ampdu_params_info |=
 				(IEEE80211_HT_AMPDU_PARM_DENSITY& (0x07 << 2));
 		else
 			ht_capie.ampdu_params_info |=
 				(IEEE80211_HT_AMPDU_PARM_DENSITY & 0x00);
 
-		pframe = rtw_set_ie23a(out_ie + out_len, _HT_CAPABILITY_IE_,
+		pframe = rtw_set_ie23a(out_ie + out_len, WLAN_EID_HT_CAPABILITY,
 				    sizeof(struct ieee80211_ht_cap),
 				    (unsigned char*)&ht_capie, pout_len);
 
 		phtpriv->ht_option = true;
 
-		p = rtw_get_ie23a(in_ie + 12, _HT_ADD_INFO_IE_, &ielen, in_len-12);
-		if (p && (ielen == sizeof(struct ieee80211_ht_addt_info))) {
+		p = cfg80211_find_ie(WLAN_EID_HT_OPERATION, in_ie + 12,
+				     in_len -12);
+		if (p && (p[1] == sizeof(struct ieee80211_ht_addt_info))) {
 			out_len = *pout_len;
-			pframe = rtw_set_ie23a(out_ie + out_len, _HT_ADD_INFO_IE_,
-					    ielen, p + 2 , pout_len);
+			pframe = rtw_set_ie23a(out_ie + out_len,
+					       WLAN_EID_HT_OPERATION,
+					       p[1], p + 2 , pout_len);
 		}
 	}
 
@@ -2274,19 +2266,16 @@ unsigned int rtw_restructure_ht_ie23a(struct rtw_adapter *padapter, u8 *in_ie,
 /* the fucntion is > passive_level (in critical_section) */
 void rtw_update_ht_cap23a(struct rtw_adapter *padapter, u8 *pie, uint ie_len)
 {
-	u8 *p, max_ampdu_sz;
-	int len;
-	/* struct sta_info *bmc_sta, *psta; */
+	u8 max_ampdu_sz;
+	const u8 *p;
 	struct ieee80211_ht_cap *pht_capie;
 	struct ieee80211_ht_addt_info *pht_addtinfo;
-	/* struct recv_reorder_ctrl *preorder_ctrl; */
-	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
-	struct ht_priv		*phtpriv = &pmlmepriv->htpriv;
-	/* struct recv_priv *precvpriv = &padapter->recvpriv; */
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv *phtpriv = &pmlmepriv->htpriv;
 	struct registry_priv *pregistrypriv = &padapter->registrypriv;
-	/* struct wlan_network *pcur_network = &pmlmepriv->cur_network;; */
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
+	int bcn_fixed_size;
 
 	if (!phtpriv->ht_option)
 		return;
@@ -2296,51 +2285,54 @@ void rtw_update_ht_cap23a(struct rtw_adapter *padapter, u8 *pie, uint ie_len)
 
 	DBG_8723A("+rtw_update_ht_cap23a()\n");
 
+	bcn_fixed_size = offsetof(struct ieee80211_mgmt, u.beacon.variable) -
+		offsetof(struct ieee80211_mgmt, u.beacon);
+
+	/* Adjust pie + ie_len for our searches */
+	pie += bcn_fixed_size;
+	ie_len -= bcn_fixed_size;
+
 	/* maybe needs check if ap supports rx ampdu. */
-	if ((phtpriv->ampdu_enable == false) && (pregistrypriv->ampdu_enable == 1)) {
+	if (phtpriv->ampdu_enable == false &&
+	    pregistrypriv->ampdu_enable == 1) {
 		if (pregistrypriv->wifi_spec == 1)
 			phtpriv->ampdu_enable = false;
 		else
 			phtpriv->ampdu_enable = true;
-	} else if (pregistrypriv->ampdu_enable == 2) {
+	} else if (pregistrypriv->ampdu_enable == 2)
 		phtpriv->ampdu_enable = true;
-	}
 
 	/* check Max Rx A-MPDU Size */
-	len = 0;
-	p = rtw_get_ie23a(pie+sizeof (struct ndis_802_11_fixed_ies), _HT_CAPABILITY_IE_, &len, ie_len-sizeof (struct ndis_802_11_fixed_ies));
-	if (p && len > 0) {
-		pht_capie = (struct ieee80211_ht_cap *)(p+2);
-		max_ampdu_sz = (pht_capie->ampdu_params_info & IEEE80211_HT_AMPDU_PARM_FACTOR);
-		max_ampdu_sz = 1 << (max_ampdu_sz+3); /*  max_ampdu_sz (kbytes); */
-
-		/* DBG_8723A("rtw_update_ht_cap23a(): max_ampdu_sz =%d\n", max_ampdu_sz); */
-		phtpriv->rx_ampdu_maxlen = max_ampdu_sz;
+	p = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY, pie, ie_len);
 
+	if (p && p[1] > 0) {
+		pht_capie = (struct ieee80211_ht_cap *)(p + 2);
+		max_ampdu_sz = pht_capie->ampdu_params_info &
+			IEEE80211_HT_AMPDU_PARM_FACTOR;
+		/*  max_ampdu_sz (kbytes); */
+		max_ampdu_sz = 1 << (max_ampdu_sz + 3);
+
+		phtpriv->rx_ampdu_maxlen = max_ampdu_sz;
 	}
 
-	len = 0;
-	p = rtw_get_ie23a(pie+sizeof (struct ndis_802_11_fixed_ies), _HT_ADD_INFO_IE_, &len, ie_len-sizeof (struct ndis_802_11_fixed_ies));
-	if (p && len>0)
-	{
-		pht_addtinfo = (struct ieee80211_ht_addt_info *)(p+2);
+	p = cfg80211_find_ie(WLAN_EID_HT_OPERATION, pie, ie_len);
+	if (p && p[1] > 0) {
+		pht_addtinfo = (struct ieee80211_ht_addt_info *)(p + 2);
 		/* todo: */
 	}
 
 	/* update cur_bwmode & cur_ch_offset */
-	if ((pregistrypriv->cbw40_enable) &&
-		(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info & BIT(1)) &&
-		(pmlmeinfo->HT_info.infos[0] & BIT(2)))
-	{
+	if (pregistrypriv->cbw40_enable &&
+	    pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info & BIT(1) &&
+	    pmlmeinfo->HT_info.infos[0] & BIT(2)) {
 		int i;
-		u8	rf_type;
+		u8 rf_type;
 
-		padapter->HalFunc.GetHwRegHandler(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+		rf_type = rtl8723a_get_rf_type(padapter);
 
 		/* update the MCS rates */
-		for (i = 0; i < 16; i++)
-		{
-			if ((rf_type == RF_1T1R) || (rf_type == RF_1T2R))
+		for (i = 0; i < 16; i++) {
+			if (rf_type == RF_1T1R || rf_type == RF_1T2R)
 				pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= MCS_rate_1R23A[i];
 			else
 				pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= MCS_rate_2R23A[i];
@@ -2349,24 +2341,26 @@ void rtw_update_ht_cap23a(struct rtw_adapter *padapter, u8 *pie, uint ie_len)
 		pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_40;
 		switch ((pmlmeinfo->HT_info.infos[0] & 0x3))
 		{
-			case HT_EXTCHNL_OFFSET_UPPER:
-				pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
-				break;
+		case HT_EXTCHNL_OFFSET_UPPER:
+			pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_LOWER;
+			break;
 
-			case HT_EXTCHNL_OFFSET_LOWER:
-				pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
-				break;
+		case HT_EXTCHNL_OFFSET_LOWER:
+			pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_UPPER;
+			break;
 
-			default:
-				pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
-				break;
+		default:
+			pmlmeext->cur_ch_offset =
+				HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+			break;
 		}
 	}
 
 	/*  */
 	/*  Config SM Power Save setting */
 	/*  */
-	pmlmeinfo->SM_PS = (pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info & 0x0C) >> 2;
+	pmlmeinfo->SM_PS = (pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info &
+			    0x0C) >> 2;
 	if (pmlmeinfo->SM_PS == WLAN_HT_CAP_SM_PS_STATIC)
 		DBG_8723A("%s(): WLAN_HT_CAP_SM_PS_STATIC\n", __func__);
 
@@ -2376,124 +2370,64 @@ void rtw_update_ht_cap23a(struct rtw_adapter *padapter, u8 *pie, uint ie_len)
 	pmlmeinfo->HT_protection = pmlmeinfo->HT_info.infos[1] & 0x3;
 }
 
-void rtw_issue_addbareq_cmd23a(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe)
+void rtw_issue_addbareq_cmd23a(struct rtw_adapter *padapter,
+			       struct xmit_frame *pxmitframe)
 {
 	u8 issued;
 	int priority;
-	struct sta_info *psta = NULL;
+	struct sta_info *psta;
 	struct ht_priv	*phtpriv;
 	struct pkt_attrib *pattrib = &pxmitframe->attrib;
 	s32 bmcst = is_multicast_ether_addr(pattrib->ra);
 
-	if (bmcst || (padapter->mlmepriv.LinkDetectInfo.NumTxOkInPeriod<100))
+	if (bmcst || padapter->mlmepriv.LinkDetectInfo.NumTxOkInPeriod < 100)
 		return;
 
 	priority = pattrib->priority;
 
 	if (pattrib->psta)
 		psta = pattrib->psta;
-	else
-	{
+	else {
 		DBG_8723A("%s, call rtw_get_stainfo23a()\n", __func__);
 		psta = rtw_get_stainfo23a(&padapter->stapriv, pattrib->ra);
 	}
 
-	if (psta == NULL)
-	{
+	if (!psta) {
 		DBG_8723A("%s, psta == NUL\n", __func__);
 		return;
 	}
 
-	if (!(psta->state &_FW_LINKED))
-	{
-		DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
+	if (!(psta->state &_FW_LINKED)) {
+		DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n",
+			  __func__, psta->state);
 		return;
 	}
 
 	phtpriv = &psta->htpriv;
 
-	if ((phtpriv->ht_option == true) && (phtpriv->ampdu_enable == true))
-	{
+	if (phtpriv->ht_option == true && phtpriv->ampdu_enable == true) {
 		issued = (phtpriv->agg_enable_bitmap>>priority)&0x1;
 		issued |= (phtpriv->candidate_tid_bitmap>>priority)&0x1;
 
-		if (0 == issued)
-		{
-			DBG_8723A("rtw_issue_addbareq_cmd23a, p =%d\n", priority);
-			psta->htpriv.candidate_tid_bitmap |= CHKBIT((u8)priority);
-			rtw_addbareq_cmd23a(padapter, (u8) priority, pattrib->ra);
-		}
-	}
-}
-
-inline void rtw_set_roaming(struct rtw_adapter *adapter, u8 to_roaming)
-{
-	if (to_roaming == 0)
-		adapter->mlmepriv.to_join = false;
-	adapter->mlmepriv.to_roaming = to_roaming;
-}
-
-inline u8 rtw_to_roaming(struct rtw_adapter *adapter)
-{
-	return adapter->mlmepriv.to_roaming;
-}
-
-void rtw23a_roaming(struct rtw_adapter *padapter, struct wlan_network *tgt_network)
-{
-	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
-
-	spin_lock_bh(&pmlmepriv->lock);
-	_rtw23a_roaming(padapter, tgt_network);
-	spin_unlock_bh(&pmlmepriv->lock);
-}
-void _rtw23a_roaming(struct rtw_adapter *padapter, struct wlan_network *tgt_network)
-{
-	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
-	struct wlan_network *pnetwork;
-	int do_join_r;
-
-	if (tgt_network != NULL)
-		pnetwork = tgt_network;
-	else
-		pnetwork = &pmlmepriv->cur_network;
-
-	if (0 < rtw_to_roaming(padapter)) {
-		DBG_8723A("roaming from %s("MAC_FMT"), length:%d\n",
-			  pnetwork->network.Ssid.ssid,
-			  MAC_ARG(pnetwork->network.MacAddress),
-			  pnetwork->network.Ssid.ssid_len);
-		memcpy(&pmlmepriv->assoc_ssid, &pnetwork->network.Ssid,
-		       sizeof(struct cfg80211_ssid));
-
-		pmlmepriv->assoc_by_bssid = false;
-
-		while(1) {
-			if (_SUCCESS == (do_join_r = rtw_do_join23a(padapter))) {
-				break;
-			} else {
-				DBG_8723A("roaming do_join return %d\n", do_join_r);
-				pmlmepriv->to_roaming--;
-
-				if (0 < rtw_to_roaming(padapter)) {
-					continue;
-				} else {
-					DBG_8723A("%s(%d) -to roaming fail, indicate_disconnect\n", __func__, __LINE__);
-					rtw_indicate_disconnect23a(padapter);
-					break;
-				}
-			}
+		if (issued == 0) {
+			DBG_8723A("rtw_issue_addbareq_cmd23a, p =%d\n",
+				  priority);
+			psta->htpriv.candidate_tid_bitmap |= BIT(priority);
+			rtw_addbareq_cmd23a(padapter, (u8) priority,
+					    pattrib->ra);
 		}
 	}
 }
 
 int rtw_linked_check(struct rtw_adapter *padapter)
 {
-	if ((check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE)) ||
-	    (check_fwstate(&padapter->mlmepriv, WIFI_ADHOC_STATE|WIFI_ADHOC_MASTER_STATE))) {
+	if (check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE) ||
+	    check_fwstate(&padapter->mlmepriv,
+			  WIFI_ADHOC_STATE|WIFI_ADHOC_MASTER_STATE)) {
 		if (padapter->stapriv.asoc_sta_count > 2)
 			return true;
 	} else {	/* Station mode */
-		if (check_fwstate(&padapter->mlmepriv, _FW_LINKED) == true)
+		if (check_fwstate(&padapter->mlmepriv, _FW_LINKED))
 			return true;
 	}
 	return false;
diff --git a/drivers/staging/rtl8723au/core/rtw_mlme_ext.c b/drivers/staging/rtl8723au/core/rtw_mlme_ext.c
index 4c753639ea5..e1b28a23425 100644
--- a/drivers/staging/rtl8723au/core/rtw_mlme_ext.c
+++ b/drivers/staging/rtl8723au/core/rtw_mlme_ext.c
@@ -21,11 +21,51 @@
 #include <wlan_bssdef.h>
 #include <mlme_osdep.h>
 #include <recv_osdep.h>
-#include <ethernet.h>
 #include <linux/ieee80211.h>
-
-#ifdef CONFIG_8723AU_BT_COEXIST
 #include <rtl8723a_hal.h>
+
+static int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnAssocRsp23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnProbeReq23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnProbeRsp23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int DoReserved23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnBeacon23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnAtim23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnDisassoc23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnAuth23aClient23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnDeAuth23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnAction23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+
+static int on_action_spct23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnAction23a_qos(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnAction23a_dls(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnAction23a_back23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int on_action_public23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnAction23a_ht(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnAction23a_wmm(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static int OnAction23a_p2p(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+
+static void issue_assocreq(struct rtw_adapter *padapter);
+static void issue_probereq(struct rtw_adapter *padapter,
+			   struct cfg80211_ssid *pssid, u8 *da);
+static int issue_probereq_ex(struct rtw_adapter *padapter,
+			     struct cfg80211_ssid *pssid,
+			     u8 *da, int try_cnt, int wait_ms);
+static void issue_probersp(struct rtw_adapter *padapter, unsigned char *da,
+			   u8 is_valid_p2p_probereq);
+static void issue_auth(struct rtw_adapter *padapter, struct sta_info *psta,
+		       unsigned short status);
+static int issue_deauth_ex(struct rtw_adapter *padapter, u8 *da,
+			   unsigned short reason, int try_cnt, int wait_ms);
+static void start_clnt_assoc(struct rtw_adapter *padapter);
+static void start_clnt_auth(struct rtw_adapter *padapter);
+static void start_clnt_join(struct rtw_adapter *padapter);
+static void start_create_ibss(struct rtw_adapter *padapter);
+
+#ifdef CONFIG_8723AU_AP_MODE
+static int OnAuth23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+static void issue_assocrsp(struct rtw_adapter *padapter, unsigned short status,
+			   struct sta_info *pstat, u16 pkt_type);
 #endif
 
 static struct mlme_handler mlme_sta_tbl[]={
@@ -66,7 +106,6 @@ static u8	null_addr[ETH_ALEN]= {0, 0, 0, 0, 0, 0};
 /**************************************************
 OUI definitions for the vendor specific IE
 ***************************************************/
-unsigned char	RTW_WPA_OUI23A[] = {0x00, 0x50, 0xf2, 0x01};
 unsigned char WMM_OUI23A[] = {0x00, 0x50, 0xf2, 0x02};
 unsigned char	WPS_OUI23A[] = {0x00, 0x50, 0xf2, 0x04};
 unsigned char	P2P_OUI23A[] = {0x50, 0x6F, 0x9A, 0x09};
@@ -75,9 +114,7 @@ unsigned char	WFD_OUI23A[] = {0x50, 0x6F, 0x9A, 0x0A};
 unsigned char	WMM_INFO_OUI23A[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
 unsigned char	WMM_PARA_OUI23A[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
 
-unsigned char WPA_TKIP_CIPHER23A[4] = {0x00, 0x50, 0xf2, 0x02};
-unsigned char RSN_TKIP_CIPHER23A[4] = {0x00, 0x0f, 0xac, 0x02};
-
+static unsigned char REALTEK_96B_IE[] = {0x00, 0xe0, 0x4c, 0x02, 0x01, 0x20};
 
 /********************************************************
 MCS rate definitions
@@ -198,9 +235,13 @@ static struct rt_channel_plan_map	RTW_ChannelPlanMap[RT_CHANNEL_DOMAIN_MAX] = {
 
 static struct rt_channel_plan_map	RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE = {0x03, 0x02}; /* use the conbination for max channel numbers */
 
+static void dummy_event_callback(struct rtw_adapter *adapter, const u8 *pbuf)
+{
+}
+
 static struct fwevent wlanevents[] =
 {
-	{0, rtw_dummy_event_callback23a},	/*0*/
+	{0, &dummy_event_callback},	/*0*/
 	{0, NULL},
 	{0, NULL},
 	{0, NULL},
@@ -214,21 +255,32 @@ static struct fwevent wlanevents[] =
 	{0, &rtw23a_joinbss_event_cb},		/*10*/
 	{sizeof(struct stassoc_event), &rtw_stassoc_event_callback23a},
 	{sizeof(struct stadel_event), &rtw_stadel_event_callback23a},
-	{0, &rtw_atimdone_event_callback23a},
-	{0, rtw_dummy_event_callback23a},
+	{0, &dummy_event_callback},
+	{0, &dummy_event_callback},
 	{0, NULL},	/*15*/
 	{0, NULL},
 	{0, NULL},
 	{0, NULL},
-	{0, rtw23a_fwdbg_event_callback},
+	{0, &dummy_event_callback},
 	{0, NULL},	 /*20*/
 	{0, NULL},
 	{0, NULL},
-	{0, &rtw_cpwm_event_callback23a},
+	{0, &dummy_event_callback},
 	{0, NULL},
 };
 
 
+static void rtw_correct_TSF(struct rtw_adapter *padapter)
+{
+	hw_var_set_correct_tsf(padapter);
+}
+
+static void
+rtw_update_TSF(struct mlme_ext_priv *pmlmeext, struct ieee80211_mgmt *mgmt)
+{
+	pmlmeext->TSFValue = get_unaligned_le64(&mgmt->u.beacon.timestamp);
+}
+
 /*
  * Search the @param channel_num in given @param channel_set
  * @ch_set: the given channel set
@@ -310,7 +362,7 @@ static void init_mlme_ext_priv23a_value(struct rtw_adapter* padapter)
 	pmlmeinfo->key_index = 0;
 	pmlmeinfo->iv = 0;
 
-	pmlmeinfo->enc_algo = _NO_PRIVACY_;
+	pmlmeinfo->enc_algo = 0;
 	pmlmeinfo->authModeToggle = 0;
 
 	memset(pmlmeinfo->chg_txt, 0, 128);
@@ -388,96 +440,88 @@ static void init_channel_list(struct rtw_adapter *padapter,
 	channel_list->reg_classes = cla;
 }
 
-static u8 init_channel_set(struct rtw_adapter* padapter, u8 ChannelPlan,
-			   struct rt_channel_info *channel_set)
+static u8 init_channel_set(struct rtw_adapter* padapter, u8 cplan,
+			   struct rt_channel_info *c_set)
 {
-	u8	index, chanset_size = 0;
-	u8	b5GBand = false, b2_4GBand = false;
-	u8	Index2G = 0, Index5G = 0;
+	u8 i, ch_size = 0;
+	u8 b5GBand = false, b2_4GBand = false;
+	u8 Index2G = 0, Index5G = 0;
 
-	memset(channel_set, 0, sizeof(struct rt_channel_info)*MAX_CHANNEL_NUM);
+	memset(c_set, 0, sizeof(struct rt_channel_info) * MAX_CHANNEL_NUM);
 
-	if (ChannelPlan >= RT_CHANNEL_DOMAIN_MAX &&
-	    ChannelPlan != RT_CHANNEL_DOMAIN_REALTEK_DEFINE) {
-		DBG_8723A("ChannelPlan ID %x error !!!!!\n", ChannelPlan);
-		return chanset_size;
+	if (cplan >= RT_CHANNEL_DOMAIN_MAX &&
+	    cplan != RT_CHANNEL_DOMAIN_REALTEK_DEFINE) {
+		DBG_8723A("ChannelPlan ID %x error !!!!!\n", cplan);
+		return ch_size;
 	}
 
 	if (padapter->registrypriv.wireless_mode & WIRELESS_11G) {
 		b2_4GBand = true;
-		if (RT_CHANNEL_DOMAIN_REALTEK_DEFINE == ChannelPlan)
+		if (RT_CHANNEL_DOMAIN_REALTEK_DEFINE == cplan)
 			Index2G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.Index2G;
 		else
-			Index2G = RTW_ChannelPlanMap[ChannelPlan].Index2G;
+			Index2G = RTW_ChannelPlanMap[cplan].Index2G;
 	}
 
 	if (padapter->registrypriv.wireless_mode & WIRELESS_11A) {
 		b5GBand = true;
-		if (RT_CHANNEL_DOMAIN_REALTEK_DEFINE == ChannelPlan)
+		if (RT_CHANNEL_DOMAIN_REALTEK_DEFINE == cplan)
 			Index5G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.Index5G;
 		else
-			Index5G = RTW_ChannelPlanMap[ChannelPlan].Index5G;
+			Index5G = RTW_ChannelPlanMap[cplan].Index5G;
 	}
 
 	if (b2_4GBand) {
-		for (index = 0; index<RTW_ChannelPlan2G[Index2G].Len; index++) {
-			channel_set[chanset_size].ChannelNum =
-				RTW_ChannelPlan2G[Index2G].Channel[index];
+		for (i = 0; i < RTW_ChannelPlan2G[Index2G].Len; i++) {
+			c_set[ch_size].ChannelNum =
+				RTW_ChannelPlan2G[Index2G].Channel[i];
 
-			if ((RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN == ChannelPlan) ||
+			if ((RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN == cplan) ||
 			    /* Channel 1~11 is active, and 12~14 is passive */
-			    (RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN_2G == ChannelPlan)){
-				if (channel_set[chanset_size].ChannelNum >= 1 &&
-				    channel_set[chanset_size].ChannelNum <= 11)
-					channel_set[chanset_size].ScanType =
-						SCAN_ACTIVE;
-				else if ((channel_set[chanset_size].ChannelNum >= 12 &&
-					  channel_set[chanset_size].ChannelNum  <= 14))
-					channel_set[chanset_size].ScanType =
-						SCAN_PASSIVE;
-			} else if (RT_CHANNEL_DOMAIN_WORLD_WIDE_13 ==
-				   ChannelPlan ||
-				   RT_CHANNEL_DOMAIN_WORLD_WIDE_5G ==
-				   ChannelPlan ||
+			    RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN_2G == cplan) {
+				if (c_set[ch_size].ChannelNum >= 1 &&
+				    c_set[ch_size].ChannelNum <= 11)
+					c_set[ch_size].ScanType = SCAN_ACTIVE;
+				else if (c_set[ch_size].ChannelNum >= 12 &&
+					 c_set[ch_size].ChannelNum  <= 14)
+					c_set[ch_size].ScanType = SCAN_PASSIVE;
+			} else if (RT_CHANNEL_DOMAIN_WORLD_WIDE_13 == cplan ||
+				   RT_CHANNEL_DOMAIN_WORLD_WIDE_5G == cplan ||
 				   RT_CHANNEL_DOMAIN_2G_WORLD == Index2G) {
 				/*  channel 12~13, passive scan */
-				if (channel_set[chanset_size].ChannelNum <= 11)
-					channel_set[chanset_size].ScanType =
-						SCAN_ACTIVE;
+				if (c_set[ch_size].ChannelNum <= 11)
+					c_set[ch_size].ScanType = SCAN_ACTIVE;
 				else
-					channel_set[chanset_size].ScanType =
-						SCAN_PASSIVE;
+					c_set[ch_size].ScanType = SCAN_PASSIVE;
 			} else
-				channel_set[chanset_size].ScanType =
-					SCAN_ACTIVE;
+				c_set[ch_size].ScanType = SCAN_ACTIVE;
 
-			chanset_size++;
+			ch_size++;
 		}
 	}
 
 	if (b5GBand) {
-		for (index = 0;index<RTW_ChannelPlan5G[Index5G].Len;index++) {
-			if (RTW_ChannelPlan5G[Index5G].Channel[index] <= 48 ||
-			    RTW_ChannelPlan5G[Index5G].Channel[index] >= 149) {
-				channel_set[chanset_size].ChannelNum =
-					RTW_ChannelPlan5G[Index5G].Channel[index];
-				if (RT_CHANNEL_DOMAIN_WORLD_WIDE_5G ==
-				    ChannelPlan) {
+		for (i = 0; i < RTW_ChannelPlan5G[Index5G].Len; i++) {
+			if (RTW_ChannelPlan5G[Index5G].Channel[i] <= 48 ||
+			    RTW_ChannelPlan5G[Index5G].Channel[i] >= 149) {
+				c_set[ch_size].ChannelNum =
+					RTW_ChannelPlan5G[Index5G].Channel[i];
+				if (RT_CHANNEL_DOMAIN_WORLD_WIDE_5G == cplan) {
 					/* passive scan for all 5G channels */
-					channel_set[chanset_size].ScanType =
+					c_set[ch_size].ScanType =
 						SCAN_PASSIVE;
 				} else
-					channel_set[chanset_size].ScanType =
+					c_set[ch_size].ScanType =
 						SCAN_ACTIVE;
 				DBG_8723A("%s(): channel_set[%d].ChannelNum = "
-					  "%d\n", __func__, chanset_size,
-					  channel_set[chanset_size].ChannelNum);
-				chanset_size++;
+					  "%d\n", __func__, ch_size,
+					  c_set[ch_size].ChannelNum);
+				ch_size++;
 			}
 		}
 	}
 
-	return chanset_size;
+	return ch_size;
 }
 
 int init_mlme_ext_priv23a(struct rtw_adapter* padapter)
@@ -547,28 +591,28 @@ _mgt_dispatcher23a(struct rtw_adapter *padapter, struct mlme_handler *ptable,
 void mgt_dispatcher23a(struct rtw_adapter *padapter,
 		    struct recv_frame *precv_frame)
 {
-	int index;
 	struct mlme_handler *ptable;
 #ifdef CONFIG_8723AU_AP_MODE
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 #endif /* CONFIG_8723AU_AP_MODE */
 	struct sk_buff *skb = precv_frame->pkt;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-	u16 stype;
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
 	struct sta_info *psta;
+	u16 stype;
+	int index;
 
-	if (!ieee80211_is_mgmt(hdr->frame_control))
+	if (!ieee80211_is_mgmt(mgmt->frame_control))
 		return;
 
 	/* receive the frames that ra(a1) is my address or ra(a1) is
 	   bc address. */
-	if (!ether_addr_equal(hdr->addr1, myid(&padapter->eeprompriv)) &&
-	    !is_broadcast_ether_addr(hdr->addr1))
+	if (!ether_addr_equal(mgmt->da, myid(&padapter->eeprompriv)) &&
+	    !is_broadcast_ether_addr(mgmt->da))
 		return;
 
 	ptable = mlme_sta_tbl;
 
-	stype = le16_to_cpu(hdr->frame_control) & IEEE80211_FCTL_STYPE;
+	stype = le16_to_cpu(mgmt->frame_control) & IEEE80211_FCTL_STYPE;
 	index = stype >> 4;
 
 	if (index > 13) {
@@ -579,10 +623,10 @@ void mgt_dispatcher23a(struct rtw_adapter *padapter,
 	}
 	ptable += index;
 
-	psta = rtw_get_stainfo23a(&padapter->stapriv, hdr->addr2);
+	psta = rtw_get_stainfo23a(&padapter->stapriv, mgmt->sa);
 
 	if (psta) {
-		if (ieee80211_has_retry(hdr->frame_control)) {
+		if (ieee80211_has_retry(mgmt->frame_control)) {
 			if (precv_frame->attrib.seq_num ==
 			    psta->RxMgmtFrameSeqNum) {
 				/* drop the duplicate management frame */
@@ -599,7 +643,7 @@ void mgt_dispatcher23a(struct rtw_adapter *padapter,
 	switch (stype)
 	{
 	case IEEE80211_STYPE_AUTH:
-		if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true)
+		if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
 			ptable->func = &OnAuth23a;
 		else
 			ptable->func = &OnAuth23aClient23a;
@@ -609,7 +653,7 @@ void mgt_dispatcher23a(struct rtw_adapter *padapter,
 		_mgt_dispatcher23a(padapter, ptable, precv_frame);
 		break;
 	case IEEE80211_STYPE_PROBE_REQ:
-		if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true)
+		if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
 			_mgt_dispatcher23a(padapter, ptable, precv_frame);
 		else
 			_mgt_dispatcher23a(padapter, ptable, precv_frame);
@@ -618,13 +662,11 @@ void mgt_dispatcher23a(struct rtw_adapter *padapter,
 		_mgt_dispatcher23a(padapter, ptable, precv_frame);
 		break;
 	case IEEE80211_STYPE_ACTION:
-		/* if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) */
+		/* if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) */
 		_mgt_dispatcher23a(padapter, ptable, precv_frame);
 		break;
 	default:
 		_mgt_dispatcher23a(padapter, ptable, precv_frame);
-		if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true)
-			rtw_hostapd_mlme_rx23a(padapter, precv_frame);
 		break;
 	}
 #else
@@ -632,186 +674,63 @@ void mgt_dispatcher23a(struct rtw_adapter *padapter,
 #endif
 }
 
-#ifdef CONFIG_8723AU_P2P
-static u32 p2p_listen_state_process(struct rtw_adapter *padapter,
-				    unsigned char *da)
-{
-	bool response = true;
-
-	if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled == false ||
-	    padapter->mlmepriv.wps_probe_resp_ie == NULL ||
-	    padapter->mlmepriv.p2p_probe_resp_ie == NULL) {
-		DBG_8723A("DON'T issue_probersp23a_p2p23a: p2p_enabled:%d, "
-			  "wps_probe_resp_ie:%p, p2p_probe_resp_ie:%p\n",
-			  wdev_to_priv(padapter->rtw_wdev)->p2p_enabled,
-			  padapter->mlmepriv.wps_probe_resp_ie,
-			  padapter->mlmepriv.p2p_probe_resp_ie);
-		response = false;
-	}
-
-	if (response == true)
-		issue_probersp23a_p2p23a(padapter, da);
-
-	return _SUCCESS;
-}
-#endif /* CONFIG_8723AU_P2P */
-
 /****************************************************************************
 
 Following are the callback functions for each subtype of the management frames
 
 *****************************************************************************/
 
-unsigned int OnProbeReq23a(struct rtw_adapter *padapter,
-			   struct recv_frame *precv_frame)
+static int
+OnProbeReq23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
-	unsigned int	ielen;
-	unsigned char	*p;
+	const u8 *ie;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *cur = &pmlmeinfo->network;
 	struct sk_buff *skb = precv_frame->pkt;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-	u8 *pframe = skb->data;
-	uint len = skb->len;
-	u8 is_valid_p2p_probereq = false;
-
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-	u8 wifi_test_chk_rate = 1;
-
-	if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) &&
-	    !rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE) &&
-	    !rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT) &&
-	    !rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH) &&
-	    !rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN)) {
-		/*	mcs_rate = 0 -> CCK 1M rate */
-		/*	mcs_rate = 1 -> CCK 2M rate */
-		/*	mcs_rate = 2 -> CCK 5.5M rate */
-		/*	mcs_rate = 3 -> CCK 11M rate */
-		/*	In the P2P mode, the driver should not support
-			the CCK rate */
-
-		/*	IOT issue: Google Nexus7 use 1M rate to send
-			p2p_probe_req after GO nego completed and Nexus7
-			is client */
-		if (wifi_test_chk_rate == 1) {
-			if ((is_valid_p2p_probereq =
-			     process_probe_req_p2p_ie23a(pwdinfo, pframe,
-							 len)) == true) {
-				if (rtw_p2p_chk_role(pwdinfo,
-						     P2P_ROLE_DEVICE)) {
-					u8 *sa = ieee80211_get_SA(hdr);
-					p2p_listen_state_process(padapter, sa);
-					return _SUCCESS;
-				}
-
-				if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
-					goto _continue;
-				}
-			}
-		}
-	}
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
+	int len = skb->len;
 
-_continue:
-#endif /* CONFIG_8723AU_P2P */
-
-	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
 		return _SUCCESS;
-	}
 
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == false &&
-		check_fwstate(pmlmepriv,
-			      WIFI_ADHOC_MASTER_STATE|WIFI_AP_STATE) == false) {
+	if (!check_fwstate(pmlmepriv, _FW_LINKED) &&
+	    !check_fwstate(pmlmepriv,
+			   WIFI_ADHOC_MASTER_STATE | WIFI_AP_STATE))
 		return _SUCCESS;
-	}
 
-	p = rtw_get_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) +
-			  _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ielen,
-			  len - sizeof(struct ieee80211_hdr_3addr) -
-			  _PROBEREQ_IE_OFFSET_);
+	if (unlikely(!ieee80211_is_probe_req(mgmt->frame_control))) {
+		printk(KERN_WARNING "%s: Received non probe request frame\n",
+		       __func__);
+		return _FAIL;
+	}
 
-	/* check (wildcard) SSID */
-	if (p) {
-		if (is_valid_p2p_probereq == true) {
-			goto _issue_probersp23a;
-		}
+	len -= offsetof(struct ieee80211_mgmt, u.probe_req.variable);
 
-		if ((ielen != 0 &&
-		     memcmp((void *)(p+2), cur->Ssid.ssid,
-			    cur->Ssid.ssid_len)) ||
-		    (ielen == 0 && pmlmeinfo->hidden_ssid_mode)) {
-			return _SUCCESS;
-		}
+	ie = cfg80211_find_ie(WLAN_EID_SSID, mgmt->u.probe_req.variable, len);
 
-_issue_probersp23a:
+	/* check (wildcard) SSID */
+	if (!ie)
+		goto out;
 
-		if (check_fwstate(pmlmepriv, _FW_LINKED) == true &&
-		    pmlmepriv->cur_network.join_res == true) {
-			/* DBG_8723A("+issue_probersp23a during ap mode\n"); */
-			issue_probersp23a(padapter, ieee80211_get_SA(hdr),
-					  is_valid_p2p_probereq);
-		}
+	if ((ie[1] && memcmp(ie + 2, cur->Ssid.ssid, cur->Ssid.ssid_len)) ||
+	    (ie[1] == 0 && pmlmeinfo->hidden_ssid_mode)) {
+		return _SUCCESS;
 	}
 
+	if (check_fwstate(pmlmepriv, _FW_LINKED) &&
+	    pmlmepriv->cur_network.join_res)
+		issue_probersp(padapter, mgmt->sa, false);
+
+out:
 	return _SUCCESS;
 }
 
-unsigned int OnProbeRsp23a(struct rtw_adapter *padapter,
-			   struct recv_frame *precv_frame)
+static int
+OnProbeRsp23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-#ifdef CONFIG_8723AU_P2P
-	struct sk_buff *skb = precv_frame->pkt;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
-#endif
-
-#ifdef CONFIG_8723AU_P2P
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ))	{
-		if (pwdinfo->tx_prov_disc_info.benable == true) {
-			if (ether_addr_equal(pwdinfo->tx_prov_disc_info.peerIFAddr,
-				    hdr->addr2)) {
-				if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) {
-					pwdinfo->tx_prov_disc_info.benable = false;
-					issue_p2p_provision_request23a(padapter,
-								    				pwdinfo->tx_prov_disc_info.ssid.ssid,
-												pwdinfo->tx_prov_disc_info.ssid.ssid_len,
-												pwdinfo->tx_prov_disc_info.peerDevAddr);
-				}
-				else if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE) || rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
-				{
-					pwdinfo->tx_prov_disc_info.benable = false;
-					issue_p2p_provision_request23a(padapter,
-												NULL,
-												0,
-												pwdinfo->tx_prov_disc_info.peerDevAddr);
-				}
-			}
-		}
-		return _SUCCESS;
-	} else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING)) {
-		if (pwdinfo->nego_req_info.benable == true) {
-			DBG_8723A("[%s] P2P State is GONEGO ING!\n", __func__);
-			if (ether_addr_equal(pwdinfo->nego_req_info.peerDevAddr,
-					     hdr->addr2)) {
-				pwdinfo->nego_req_info.benable = false;
-				issue_p2p_GO_request23a(padapter, pwdinfo->nego_req_info.peerDevAddr);
-			}
-		}
-	} else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_INVITE_REQ)) {
-		if (pwdinfo->invitereq_info.benable == true) {
-			DBG_8723A("[%s] P2P_STATE_TX_INVITE_REQ!\n", __func__);
-			if (ether_addr_equal(
-				    pwdinfo->invitereq_info.peer_macaddr,
-				    hdr->addr2)) {
-				pwdinfo->invitereq_info.benable = false;
-				issue_p2p_invitation_request23a(padapter, pwdinfo->invitereq_info.peer_macaddr);
-			}
-		}
-	}
-#endif
 
 	if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) {
 		report_survey_event23a(padapter, precv_frame);
@@ -821,30 +740,30 @@ unsigned int OnProbeRsp23a(struct rtw_adapter *padapter,
 	return _SUCCESS;
 }
 
-unsigned int OnBeacon23a(struct rtw_adapter *padapter,
-			 struct recv_frame *precv_frame)
+static int
+OnBeacon23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
 	int cam_idx;
 	struct sta_info	*psta;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct sta_priv	*pstapriv = &padapter->stapriv;
 	struct sk_buff *skb = precv_frame->pkt;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
 	u8 *pframe = skb->data;
-	uint len = skb->len;
+	int pkt_len = skb->len;
 	struct wlan_bssid_ex *pbss;
 	int ret = _SUCCESS;
-	u8 *p = NULL;
+	u8 *p, *pie;
+	int pie_len;
 	u32 ielen = 0;
 
-	p = rtw_get_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) +
-			  _BEACON_IE_OFFSET_, _EXT_SUPPORTEDRATES_IE_, &ielen,
-			  len - sizeof(struct ieee80211_hdr_3addr) -
-			  _BEACON_IE_OFFSET_);
-	if ((p != NULL) && (ielen > 0)) {
-		if ((*(p + 1 + ielen) == 0x2D) && (*(p + 2 + ielen) != 0x2D)) {
+	pie = mgmt->u.beacon.variable;
+	pie_len = pkt_len - offsetof(struct ieee80211_mgmt, u.beacon.variable);
+	p = rtw_get_ie23a(pie, WLAN_EID_EXT_SUPP_RATES, &ielen, pie_len);
+	if (p && ielen > 0) {
+		if (p[1 + ielen] == 0x2D && p[2 + ielen] != 0x2D) {
 			/* Invalid value 0x2D is detected in Extended Supported
 			 * Rates (ESR) IE. Try to fix the IE length to avoid
 			 * failed Beacon parsing.
@@ -852,8 +771,8 @@ unsigned int OnBeacon23a(struct rtw_adapter *padapter,
 			DBG_8723A("[WIFIDBG] Error in ESR IE is detected in "
 				  "Beacon of BSSID: %pM. Fix the length of "
 				  "ESR IE to avoid failed Beacon parsing.\n",
-				  hdr->addr3);
-			*(p + 1) = ielen - 1;
+				  mgmt->bssid);
+			p[1] = ielen - 1;
 		}
 	}
 
@@ -862,141 +781,142 @@ unsigned int OnBeacon23a(struct rtw_adapter *padapter,
 		return _SUCCESS;
 	}
 
-	if (ether_addr_equal(hdr->addr3, get_my_bssid23a(&pmlmeinfo->network))){
-		if (pmlmeinfo->state & WIFI_FW_AUTH_NULL) {
-			/* we should update current network before auth,
-			   or some IE is wrong */
-			pbss = (struct wlan_bssid_ex *)
-				kmalloc(sizeof(struct wlan_bssid_ex),
-					GFP_ATOMIC);
-			if (pbss) {
-				if (collect_bss_info23a(padapter, precv_frame,
-							pbss) == _SUCCESS) {
-					update_network23a(&pmlmepriv->cur_network.network, pbss, padapter, true);
-					rtw_get_bcn_info23a(&pmlmepriv->cur_network);
-				}
-				kfree(pbss);
-			}
+	if (!ether_addr_equal(mgmt->bssid,
+			      get_my_bssid23a(&pmlmeinfo->network)))
+		goto out;
 
-			/* check the vendor of the assoc AP */
-			pmlmeinfo->assoc_AP_vendor = check_assoc_AP23a(pframe + sizeof(struct ieee80211_hdr_3addr), len-sizeof(struct ieee80211_hdr_3addr));
+	if (pmlmeinfo->state & WIFI_FW_AUTH_NULL) {
+		/* we should update current network before auth,
+		   or some IE is wrong */
+		pbss = (struct wlan_bssid_ex *)
+			kmalloc(sizeof(struct wlan_bssid_ex), GFP_ATOMIC);
+		if (pbss) {
+			if (collect_bss_info23a(padapter, precv_frame, pbss) ==
+			    _SUCCESS) {
+				update_network23a(
+					&pmlmepriv->cur_network.network, pbss,
+					padapter, true);
+				rtw_get_bcn_info23a(&pmlmepriv->cur_network);
+			}
+			kfree(pbss);
+		}
 
-			/* update TSF Value */
-			update_TSF23a(pmlmeext, pframe, len);
+		/* check the vendor of the assoc AP */
+		pmlmeinfo->assoc_AP_vendor =
+			check_assoc_AP23a((u8 *)&mgmt->u.beacon, pkt_len -
+					  offsetof(struct ieee80211_mgmt, u));
 
-			/* start auth */
-			start_clnt_auth23a(padapter);
+		/* update TSF Value */
+		rtw_update_TSF(pmlmeext, mgmt);
 
-			return _SUCCESS;
-		}
+		/* start auth */
+		start_clnt_auth(padapter);
 
-		if (((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) &&
-		    (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) {
-			psta = rtw_get_stainfo23a(pstapriv, hdr->addr2);
-			if (psta) {
-				ret = rtw_check_bcn_info23a(padapter, pframe,
-							    len);
-				if (!ret) {
-					DBG_8723A_LEVEL(_drv_always_,
-							"ap has changed, "
-							"disconnect now\n");
-					receive_disconnect23a(padapter, pmlmeinfo->network.MacAddress, 65535);
-					return _SUCCESS;
-				}
-				/* update WMM, ERP in the beacon */
-				/* todo: the timer is used instead of
-				   the number of the beacon received */
-				if ((sta_rx_pkts(psta) & 0xf) == 0) {
-					/* DBG_8723A("update_bcn_info\n"); */
-					update_beacon23a_info(padapter, pframe,
-							      len, psta);
-				}
+		return _SUCCESS;
+	}
 
-#ifdef CONFIG_8723AU_P2P
-				process_p2p_ps_ie23a(padapter, (pframe + sizeof(struct ieee80211_hdr_3addr)), (len - sizeof(struct ieee80211_hdr_3addr)));
-#endif /* CONFIG_8723AU_P2P */
+	if (((pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE) &&
+	    (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) {
+		psta = rtw_get_stainfo23a(pstapriv, mgmt->sa);
+		if (psta) {
+			ret = rtw_check_bcn_info23a(padapter, mgmt, pkt_len);
+			if (!ret) {
+				DBG_8723A_LEVEL(_drv_always_, "ap has changed, "
+						"disconnect now\n");
+				receive_disconnect23a(padapter, pmlmeinfo->network.MacAddress, 65535);
+				return _SUCCESS;
+			}
+			/* update WMM, ERP in the beacon */
+			/* todo: the timer is used instead of
+			   the number of the beacon received */
+			if ((sta_rx_pkts(psta) & 0xf) == 0) {
+				/* DBG_8723A("update_bcn_info\n"); */
+				update_beacon23a_info(padapter, pframe,
+						      pkt_len, psta);
+			}
+		}
+	} else if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) {
+		psta = rtw_get_stainfo23a(pstapriv, mgmt->sa);
+		if (psta) {
+			/* update WMM, ERP in the beacon */
+			/* todo: the timer is used instead of the
+			   number of the beacon received */
+			if ((sta_rx_pkts(psta) & 0xf) == 0) {
+				/* DBG_8723A("update_bcn_info\n"); */
+				update_beacon23a_info(padapter, pframe,
+						      pkt_len, psta);
+			}
+		} else {
+			/* allocate a new CAM entry for IBSS station */
+			cam_idx = allocate_fw_sta_entry23a(padapter);
+			if (cam_idx == NUM_STA)
+				goto out;
+
+			/* get supported rate */
+			if (update_sta_support_rate23a(padapter, pie, pie_len,
+						       cam_idx) == _FAIL) {
+				pmlmeinfo->FW_sta_info[cam_idx].status = 0;
+				goto out;
 			}
-		} else if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) {
-			psta = rtw_get_stainfo23a(pstapriv, hdr->addr2);
-			if (psta) {
-				/* update WMM, ERP in the beacon */
-				/* todo: the timer is used instead of the
-				   number of the beacon received */
-				if ((sta_rx_pkts(psta) & 0xf) == 0) {
-					/* DBG_8723A("update_bcn_info\n"); */
-					update_beacon23a_info(padapter, pframe,
-							      len, psta);
-				}
-			} else {
-				/* allocate a new CAM entry for IBSS station */
-				cam_idx = allocate_fw_sta_entry23a(padapter);
-				if (cam_idx == NUM_STA)
-					goto _END_ONBEACON_;
-
-				/* get supported rate */
-				if (update_sta_support_rate23a(padapter, (pframe + sizeof(struct ieee80211_hdr_3addr) + _BEACON_IE_OFFSET_), (len - sizeof(struct ieee80211_hdr_3addr) - _BEACON_IE_OFFSET_), cam_idx) == _FAIL) {
-					pmlmeinfo->FW_sta_info[cam_idx].status = 0;
-					goto _END_ONBEACON_;
-				}
 
-				/* update TSF Value */
-				update_TSF23a(pmlmeext, pframe, len);
+			/* update TSF Value */
+			rtw_update_TSF(pmlmeext, mgmt);
 
-				/* report sta add event */
-				report_add_sta_event23a(padapter, hdr->addr2,
-							cam_idx);
-			}
+			/* report sta add event */
+			report_add_sta_event23a(padapter, mgmt->sa,
+						cam_idx);
 		}
 	}
 
-_END_ONBEACON_:
+out:
 
 	return _SUCCESS;
 }
 
-unsigned int OnAuth23a(struct rtw_adapter *padapter,
-		       struct recv_frame *precv_frame)
-{
 #ifdef CONFIG_8723AU_AP_MODE
-	unsigned int	auth_mode, seq, ie_len;
-	unsigned char	*sa, *p;
-	u16	algorithm;
-	int	status;
+static int
+OnAuth23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
+{
 	static struct sta_info stat;
-	struct	sta_info	*pstat = NULL;
-	struct	sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *pstat = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct sk_buff *skb = precv_frame->pkt;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-	u8 *pframe = skb->data;
-	uint len = skb->len;
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
+	u8 *pframe;
+	const u8 *p;
+	unsigned char *sa;
+	u16 auth_mode, seq, algorithm;
+	int status, len = skb->len;
 
-	if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
+	if ((pmlmeinfo->state & 0x03) != WIFI_FW_AP_STATE)
 		return _FAIL;
 
 	DBG_8723A("+OnAuth23a\n");
 
-	sa = hdr->addr2;
+	sa = mgmt->sa;
 
 	auth_mode = psecuritypriv->dot11AuthAlgrthm;
-	seq = cpu_to_le16(*(u16*)((unsigned long)pframe +
-				  sizeof(struct ieee80211_hdr_3addr) + 2));
-	algorithm = cpu_to_le16(*(u16*)((unsigned long)pframe +
-					sizeof(struct ieee80211_hdr_3addr)));
+
+	pframe = mgmt->u.auth.variable;
+	len = skb->len - offsetof(struct ieee80211_mgmt, u.auth.variable);
+
+	seq = le16_to_cpu(mgmt->u.auth.auth_transaction);
+	algorithm = le16_to_cpu(mgmt->u.auth.auth_alg);
 
 	DBG_8723A("auth alg =%x, seq =%X\n", algorithm, seq);
 
 	if (auth_mode == 2 &&
-	    psecuritypriv->dot11PrivacyAlgrthm != _WEP40_ &&
-	    psecuritypriv->dot11PrivacyAlgrthm != _WEP104_)
+	    psecuritypriv->dot11PrivacyAlgrthm != WLAN_CIPHER_SUITE_WEP40 &&
+	    psecuritypriv->dot11PrivacyAlgrthm != WLAN_CIPHER_SUITE_WEP104)
 		auth_mode = 0;
 
 	/*  rx a shared-key auth but shared not enabled, or */
 	/*  rx a open-system auth but shared-key is enabled */
-	if ((algorithm > 0 && auth_mode == 0) ||
-	    (algorithm == 0 && auth_mode == 1)) {
+	if ((algorithm != WLAN_AUTH_OPEN && auth_mode == 0) ||
+	    (algorithm == WLAN_AUTH_OPEN && auth_mode == 1)) {
 		DBG_8723A("auth rejected due to bad alg [alg =%d, auth_mib "
 			  "=%d] %02X%02X%02X%02X%02X%02X\n",
 			  algorithm, auth_mode,
@@ -1017,7 +937,7 @@ unsigned int OnAuth23a(struct rtw_adapter *padapter,
 		/*  allocate a new one */
 		DBG_8723A("going to alloc stainfo for sa ="MAC_FMT"\n",
 			  MAC_ARG(sa));
-		pstat = rtw_alloc_stainfo23a(pstapriv, sa);
+		pstat = rtw_alloc_stainfo23a(pstapriv, sa, GFP_ATOMIC);
 		if (!pstat) {
 			DBG_8723A(" Exceed the upper limit of supported "
 				  "clients...\n");
@@ -1035,8 +955,7 @@ unsigned int OnAuth23a(struct rtw_adapter *padapter,
 		if (!list_empty(&pstat->asoc_list)) {
 			list_del_init(&pstat->asoc_list);
 			pstapriv->asoc_list_cnt--;
-			if (pstat->expire_to > 0)
-			{
+			if (pstat->expire_to > 0) {
 				/* TODO: STA re_auth within expire_to */
 			}
 		}
@@ -1064,7 +983,7 @@ unsigned int OnAuth23a(struct rtw_adapter *padapter,
 		goto auth_fail;
 	}
 
-	if (algorithm == 0 && (auth_mode == 0 || auth_mode == 2)) {
+	if (algorithm == WLAN_AUTH_OPEN && (auth_mode == 0 || auth_mode == 2)) {
 		if (seq == 1) {
 			pstat->state &= ~WIFI_FW_AUTH_NULL;
 			pstat->state |= WIFI_FW_AUTH_SUCCESS;
@@ -1088,22 +1007,16 @@ unsigned int OnAuth23a(struct rtw_adapter *padapter,
 			/* checking for challenging txt... */
 			DBG_8723A("checking for challenging txt...\n");
 
-			p = rtw_get_ie23a(pframe +
-					  sizeof(struct ieee80211_hdr_3addr) +
-					  4 + _AUTH_IE_OFFSET_, _CHLGETXT_IE_,
-					  (int *)&ie_len, len -
-					  sizeof(struct ieee80211_hdr_3addr) -
-					  _AUTH_IE_OFFSET_ - 4);
-
-			if ((p == NULL) || (ie_len<= 0)) {
+			p = cfg80211_find_ie(WLAN_EID_CHALLENGE, pframe, len);
+			if (!p || p[1] <= 0) {
 				DBG_8723A("auth rejected because challenge "
 					  "failure!(1)\n");
 				status = WLAN_STATUS_CHALLENGE_FAIL;
 				goto auth_fail;
 			}
 
-			if (!memcmp((void *)(p + 2), pstat->chg_txt, 128)) {
-				pstat->state &= (~WIFI_FW_AUTH_STATE);
+			if (!memcmp(p + 2, pstat->chg_txt, 128)) {
+				pstat->state &= ~WIFI_FW_AUTH_STATE;
 				pstat->state |= WIFI_FW_AUTH_SUCCESS;
 				/*  challenging txt is correct... */
 				pstat->expire_to =  pstapriv->assoc_to;
@@ -1122,10 +1035,10 @@ unsigned int OnAuth23a(struct rtw_adapter *padapter,
 		}
 	}
 
-	/*  Now, we are going to issue_auth23a... */
+	/*  Now, we are going to issue_auth... */
 	pstat->auth_seq = seq + 1;
 
-	issue_auth23a(padapter, pstat, (unsigned short)WLAN_STATUS_SUCCESS);
+	issue_auth(padapter, pstat, WLAN_STATUS_SUCCESS);
 
 	if (pstat->state & WIFI_FW_AUTH_SUCCESS)
 		pstat->auth_seq = 0;
@@ -1142,46 +1055,45 @@ auth_fail:
 	pstat->auth_seq = 2;
 	memcpy(pstat->hwaddr, sa, 6);
 
-	issue_auth23a(padapter, pstat, (unsigned short)status);
+	issue_auth(padapter, pstat, (unsigned short)status);
 
-#endif
 	return _FAIL;
 }
+#endif
 
-unsigned int OnAuth23aClient23a(struct rtw_adapter *padapter,
-				struct recv_frame *precv_frame)
+static int
+OnAuth23aClient23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
-	unsigned int	seq, len, status, algthm, offset;
-	unsigned char	*p;
-	unsigned int	go2asoc = 0;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	unsigned int seq, status, algthm;
+	unsigned int go2asoc = 0;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct sk_buff *skb = precv_frame->pkt;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-	u8 *pframe = skb->data;
-	uint pkt_len = skb->len;
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
+	const u8 *p;
+	u8 *pie;
+	int plen = skb->len;
 
 	DBG_8723A("%s\n", __func__);
 
 	/* check A1 matches or not */
-	if (!ether_addr_equal(myid(&padapter->eeprompriv),
-			      ieee80211_get_DA(hdr)))
+	if (!ether_addr_equal(myid(&padapter->eeprompriv), mgmt->da))
 		return _SUCCESS;
 
 	if (!(pmlmeinfo->state & WIFI_FW_AUTH_STATE))
 		return _SUCCESS;
 
-	offset = ieee80211_has_protected(hdr->frame_control) ? 4: 0;
+	pie = mgmt->u.auth.variable;
+	plen -= offsetof(struct ieee80211_mgmt, u.auth.variable);
 
-	algthm	= le16_to_cpu(*(unsigned short *)((unsigned long)pframe + sizeof(struct ieee80211_hdr_3addr) + offset));
-	seq	= le16_to_cpu(*(unsigned short *)((unsigned long)pframe + sizeof(struct ieee80211_hdr_3addr) + offset + 2));
-	status	= le16_to_cpu(*(unsigned short *)((unsigned long)pframe + sizeof(struct ieee80211_hdr_3addr) + offset + 4));
+	algthm = le16_to_cpu(mgmt->u.auth.auth_alg);
+	seq = le16_to_cpu(mgmt->u.auth.auth_transaction);
+	status = le16_to_cpu(mgmt->u.auth.status_code);
 
-	if (status != 0)
-	{
+	if (status) {
 		DBG_8723A("clnt auth fail, status: %d\n", status);
-		if (status == 13)/*  pmlmeinfo->auth_algo == dot11AuthAlgrthm_Auto) */
-		{
+		/*  pmlmeinfo->auth_algo == dot11AuthAlgrthm_Auto) */
+		if (status == WLAN_STATUS_NOT_SUPPORTED_AUTH_ALG) {
 			if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
 				pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
 			else
@@ -1193,55 +1105,41 @@ unsigned int OnAuth23aClient23a(struct rtw_adapter *padapter,
 		goto authclnt_fail;
 	}
 
-	if (seq == 2)
-	{
-		if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
-		{
-			 /*  legendary shared system */
-			p = rtw_get_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + _AUTH_IE_OFFSET_, _CHLGETXT_IE_, (int *)&len,
-				pkt_len - sizeof(struct ieee80211_hdr_3addr) - _AUTH_IE_OFFSET_);
+	if (seq == 2) {
+		if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared) {
+			/*  legendary shared system */
+			p = cfg80211_find_ie(WLAN_EID_CHALLENGE, pie, plen);
 
-			if (p == NULL)
-			{
+			if (!p) {
 				/* DBG_8723A("marc: no challenge text?\n"); */
 				goto authclnt_fail;
 			}
 
-			memcpy((void *)(pmlmeinfo->chg_txt), (void *)(p + 2), len);
+			memcpy((void *)(pmlmeinfo->chg_txt), p + 2, p[1]);
 			pmlmeinfo->auth_seq = 3;
-			issue_auth23a(padapter, NULL, 0);
+			issue_auth(padapter, NULL, 0);
 			set_link_timer(pmlmeext, REAUTH_TO);
 
 			return _SUCCESS;
-		}
-		else
-		{
+		} else {
 			/*  open system */
 			go2asoc = 1;
 		}
-	}
-	else if (seq == 4)
-	{
+	} else if (seq == 4) {
 		if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
-		{
 			go2asoc = 1;
-		}
 		else
-		{
 			goto authclnt_fail;
-		}
-	}
-	else
-	{
+	} else {
 		/*  this is also illegal */
-		/* DBG_8723A("marc: clnt auth failed due to illegal seq =%x\n", seq); */
+		/* DBG_8723A("marc: clnt auth failed due to illegal seq =%x\n",
+		   seq); */
 		goto authclnt_fail;
 	}
 
-	if (go2asoc)
-	{
+	if (go2asoc) {
 		DBG_8723A_LEVEL(_drv_always_, "auth success, start assoc\n");
-		start_clnt_assoc23a(padapter);
+		start_clnt_assoc(padapter);
 		return _SUCCESS;
 	}
 
@@ -1252,19 +1150,153 @@ authclnt_fail:
 	return _FAIL;
 }
 
-unsigned int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
+#ifdef CONFIG_8723AU_AP_MODE
+static int rtw_validate_vendor_specific_ies(const u8 *pos, int elen)
+{
+	unsigned int oui;
+
+	/* first 3 bytes in vendor specific information element are the IEEE
+	 * OUI of the vendor. The following byte is used a vendor specific
+	 * sub-type. */
+	if (elen < 4) {
+		DBG_8723A("short vendor specific information element "
+			  "ignored (len =%i)\n", elen);
+		return -EINVAL;
+	}
+
+	oui = RTW_GET_BE24(pos);
+	switch (oui) {
+	case WLAN_OUI_MICROSOFT:
+		/* Microsoft/Wi-Fi information elements are further typed and
+		 * subtyped */
+		switch (pos[3]) {
+		case 1:
+			/* Microsoft OUI (00:50:F2) with OUI Type 1:
+			 * real WPA information element */
+			break;
+		case WME_OUI_TYPE: /* this is a Wi-Fi WME info. element */
+			if (elen < 5) {
+				DBG_8723A("short WME information element "
+					  "ignored (len =%i)\n", elen);
+				return -EINVAL;
+			}
+			switch (pos[4]) {
+			case WME_OUI_SUBTYPE_INFORMATION_ELEMENT:
+			case WME_OUI_SUBTYPE_PARAMETER_ELEMENT:
+				break;
+			case WME_OUI_SUBTYPE_TSPEC_ELEMENT:
+				break;
+			default:
+				DBG_8723A("unknown WME information element "
+					  "ignored (subtype =%d len =%i)\n",
+					   pos[4], elen);
+				return -EINVAL;
+			}
+			break;
+		case 4:
+			/* Wi-Fi Protected Setup (WPS) IE */
+			break;
+		default:
+			DBG_8723A("Unknown Microsoft information element "
+				  "ignored (type =%d len =%i)\n",
+				  pos[3], elen);
+			return -EINVAL;
+		}
+		break;
+
+	case OUI_BROADCOM:
+		switch (pos[3]) {
+		case VENDOR_HT_CAPAB_OUI_TYPE:
+			break;
+		default:
+			DBG_8723A("Unknown Broadcom information element "
+				  "ignored (type =%d len =%i)\n", pos[3], elen);
+			return -EINVAL;
+		}
+		break;
+
+	default:
+		DBG_8723A("unknown vendor specific information element "
+			  "ignored (vendor OUI %02x:%02x:%02x len =%i)\n",
+			   pos[0], pos[1], pos[2], elen);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int rtw_validate_frame_ies(const u8 *start, uint len)
+{
+	const u8 *pos = start;
+	int left = len;
+	int unknown = 0;
+
+	while (left >= 2) {
+		u8 id, elen;
+
+		id = *pos++;
+		elen = *pos++;
+		left -= 2;
+
+		if (elen > left) {
+			DBG_8723A("%s: IEEE 802.11 failed (id =%d elen =%d "
+				  "left =%i)\n", __func__, id, elen, left);
+			return -EINVAL;
+		}
+
+		switch (id) {
+		case WLAN_EID_SSID:
+		case WLAN_EID_SUPP_RATES:
+		case WLAN_EID_FH_PARAMS:
+		case WLAN_EID_DS_PARAMS:
+		case WLAN_EID_CF_PARAMS:
+		case WLAN_EID_TIM:
+		case WLAN_EID_IBSS_PARAMS:
+		case WLAN_EID_CHALLENGE:
+		case WLAN_EID_ERP_INFO:
+		case WLAN_EID_EXT_SUPP_RATES:
+		case WLAN_EID_VENDOR_SPECIFIC:
+			if (rtw_validate_vendor_specific_ies(pos, elen))
+				unknown++;
+			break;
+		case WLAN_EID_RSN:
+		case WLAN_EID_PWR_CAPABILITY:
+		case WLAN_EID_SUPPORTED_CHANNELS:
+		case WLAN_EID_MOBILITY_DOMAIN:
+		case WLAN_EID_FAST_BSS_TRANSITION:
+		case WLAN_EID_TIMEOUT_INTERVAL:
+		case WLAN_EID_HT_CAPABILITY:
+		case WLAN_EID_HT_OPERATION:
+		default:
+			unknown++;
+			DBG_8723A("%s IEEE 802.11 ignored unknown element "
+				  "(id =%d elen =%d)\n", __func__, id, elen);
+			break;
+		}
+
+		left -= elen;
+		pos += elen;
+	}
+
+	if (left)
+		return -EINVAL;
+
+	return 0;
+}
+#endif
+
+static int
+OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
 #ifdef CONFIG_8723AU_AP_MODE
 	u16 capab_info, listen_interval;
-	struct rtw_ieee802_11_elems elems;
 	struct sta_info	*pstat;
-	unsigned char		reassoc, *p, *pos, *wpa_ie;
+	unsigned char reassoc;
 	unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
-	int		i, ie_len, wpa_ie_len, left;
-	unsigned char		supportRate[16];
-	int					supportRateNum;
-	unsigned short		status = WLAN_STATUS_SUCCESS;
-	unsigned short ie_offset;
+	int i, wpa_ie_len, left;
+	unsigned char supportRate[16];
+	int supportRateNum;
+	unsigned short status = WLAN_STATUS_SUCCESS;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
@@ -1272,69 +1304,56 @@ unsigned int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *prec
 	struct wlan_bssid_ex *cur = &pmlmeinfo->network;
 	struct sta_priv *pstapriv = &padapter->stapriv;
 	struct sk_buff *skb = precv_frame->pkt;
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
+	const u8 *pos, *p, *wpa_ie, *wps_ie;
 	u8 *pframe = skb->data;
 	uint pkt_len = skb->len;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-	u16 frame_control;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-	u8 p2p_status_code = P2P_STATUS_SUCCESS;
-	u8 *p2pie;
-	u32 p2pielen = 0;
-	u8	wfd_ie[ 128 ] = { 0x00 };
-	u32	wfd_ielen = 0;
-#endif /* CONFIG_8723AU_P2P */
+	int r;
 
-	if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
+	if ((pmlmeinfo->state & 0x03) != WIFI_FW_AP_STATE)
 		return _FAIL;
 
-	frame_control = hdr->frame_control;
-	if (ieee80211_is_assoc_req(frame_control)) {
+	left = pkt_len - sizeof(struct ieee80211_hdr_3addr);
+	if (ieee80211_is_assoc_req(mgmt->frame_control)) {
 		reassoc = 0;
-		ie_offset = _ASOCREQ_IE_OFFSET_;
+		pos = mgmt->u.assoc_req.variable;
+		left -= offsetof(struct ieee80211_mgmt, u.assoc_req.variable);
 	} else { /*  WIFI_REASSOCREQ */
 		reassoc = 1;
-		ie_offset = _REASOCREQ_IE_OFFSET_;
+		pos = mgmt->u.reassoc_req.variable;
+		left -= offsetof(struct ieee80211_mgmt, u.reassoc_req.variable);
 	}
 
-	if (pkt_len < sizeof(struct ieee80211_hdr_3addr) + ie_offset) {
-		DBG_8723A("handle_assoc(reassoc =%d) - too short payload (len =%lu)"
-		       "\n", reassoc, (unsigned long)pkt_len);
+	if (left < 0) {
+		DBG_8723A("handle_assoc(reassoc =%d) - too short payload "
+			  "(len =%lu)\n", reassoc, (unsigned long)pkt_len);
 		return _FAIL;
 	}
 
-	pstat = rtw_get_stainfo23a(pstapriv, hdr->addr2);
+	pstat = rtw_get_stainfo23a(pstapriv, mgmt->sa);
 	if (!pstat) {
 		status = WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA;
 		goto asoc_class2_error;
 	}
 
-	capab_info = get_unaligned_le16(pframe + sizeof(struct ieee80211_hdr_3addr));
-	/* capab_info = le16_to_cpu(*(unsigned short *)(pframe + sizeof(struct ieee80211_hdr_3addr))); */
-	/* listen_interval = le16_to_cpu(*(unsigned short *)(pframe + sizeof(struct ieee80211_hdr_3addr)+2)); */
-	listen_interval = get_unaligned_le16(pframe + sizeof(struct ieee80211_hdr_3addr)+2);
-
-	left = pkt_len - (sizeof(struct ieee80211_hdr_3addr) + ie_offset);
-	pos = pframe + (sizeof(struct ieee80211_hdr_3addr) + ie_offset);
+	/* These two are located at the same offsets whether it's an
+	 * assoc_req or a reassoc_req */
+	capab_info = get_unaligned_le16(&mgmt->u.assoc_req.capab_info);
+	listen_interval =
+		get_unaligned_le16(&mgmt->u.assoc_req.listen_interval);
 
 	DBG_8723A("%s\n", __func__);
 
 	/*  check if this stat has been successfully authenticated/assocated */
-	if (!((pstat->state) & WIFI_FW_AUTH_SUCCESS))
-	{
-		if (!((pstat->state) & WIFI_FW_ASSOC_SUCCESS))
-		{
+	if (!(pstat->state & WIFI_FW_AUTH_SUCCESS)) {
+		if (!(pstat->state & WIFI_FW_ASSOC_SUCCESS)) {
 			status = WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA;
 			goto asoc_class2_error;
-		}
-		else
-		{
+		} else {
 			pstat->state &= (~WIFI_FW_ASSOC_SUCCESS);
 			pstat->state |= WIFI_FW_ASSOC_STATE;
 		}
-	}
-	else
-	{
+	} else {
 		pstat->state &= (~WIFI_FW_AUTH_SUCCESS);
 		pstat->state |= WIFI_FW_ASSOC_STATE;
 	}
@@ -1342,41 +1361,40 @@ unsigned int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *prec
 	pstat->capability = capab_info;
 
 	/* now parse all ieee802_11 ie to point to elems */
-	if (rtw_ieee802_11_parse_elems23a(pos, left, &elems, 1) == ParseFailed ||
-	    !elems.ssid) {
+
+	if (rtw_validate_frame_ies(pos, left)) {
 		DBG_8723A("STA " MAC_FMT " sent invalid association request\n",
-		       MAC_ARG(pstat->hwaddr));
+			  MAC_ARG(pstat->hwaddr));
 		status = WLAN_STATUS_UNSPECIFIED_FAILURE;
 		goto OnAssocReq23aFail;
 	}
 
 	/*  now we should check all the fields... */
 	/*  checking SSID */
-	p = rtw_get_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + ie_offset, _SSID_IE_, &ie_len,
-		pkt_len - sizeof(struct ieee80211_hdr_3addr) - ie_offset);
-	if (p == NULL)
-	{
-		status = WLAN_STATUS_UNSPECIFIED_FAILURE;
-	}
-
-	if (ie_len == 0) /*  broadcast ssid, however it is not allowed in assocreq */
+	p = cfg80211_find_ie(WLAN_EID_SSID, pos, left);
+	if (!p || p[1] == 0) {
+		/*  broadcast ssid, however it is not allowed in assocreq */
+		DBG_8723A("STA " MAC_FMT " sent invalid association request "
+			  "lacking an SSID\n", MAC_ARG(pstat->hwaddr));
 		status = WLAN_STATUS_UNSPECIFIED_FAILURE;
-	else {
+		goto OnAssocReq23aFail;
+	} else {
 		/*  check if ssid match */
-		if (memcmp((void *)(p+2), cur->Ssid.ssid, cur->Ssid.ssid_len))
+		if (memcmp(p + 2, cur->Ssid.ssid, cur->Ssid.ssid_len))
 			status = WLAN_STATUS_UNSPECIFIED_FAILURE;
 
-		if (ie_len != cur->Ssid.ssid_len)
+		if (p[1] != cur->Ssid.ssid_len)
 			status = WLAN_STATUS_UNSPECIFIED_FAILURE;
 	}
 
-	if (WLAN_STATUS_SUCCESS != status)
+	if (status != WLAN_STATUS_SUCCESS)
 		goto OnAssocReq23aFail;
 
 	/*  check if the supported rate is ok */
-	p = rtw_get_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + ie_offset, _SUPPORTEDRATES_IE_, &ie_len, pkt_len - sizeof(struct ieee80211_hdr_3addr) - ie_offset);
-	if (p == NULL) {
-		DBG_8723A("Rx a sta assoc-req which supported rate is empty!\n");
+	p = cfg80211_find_ie(WLAN_EID_SUPP_RATES, pos, left);
+	if (!p) {
+		DBG_8723A("Rx a sta assoc-req which supported rate is "
+			  "empty!\n");
 		/*  use our own rate set as statoin used */
 		/* memcpy(supportRate, AP_BSSRATE, AP_BSSRATE_LEN); */
 		/* supportRateNum = AP_BSSRATE_LEN; */
@@ -1384,17 +1402,14 @@ unsigned int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *prec
 		status = WLAN_STATUS_UNSPECIFIED_FAILURE;
 		goto OnAssocReq23aFail;
 	} else {
-		memcpy(supportRate, p+2, ie_len);
-		supportRateNum = ie_len;
-
-		p = rtw_get_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + ie_offset, _EXT_SUPPORTEDRATES_IE_, &ie_len,
-				pkt_len - sizeof(struct ieee80211_hdr_3addr) - ie_offset);
-		if (p !=  NULL) {
-
-			if (supportRateNum<= sizeof(supportRate))
-			{
-				memcpy(supportRate+supportRateNum, p+2, ie_len);
-				supportRateNum += ie_len;
+		memcpy(supportRate, p + 2, p[1]);
+		supportRateNum = p[1];
+
+		p = cfg80211_find_ie(WLAN_EID_EXT_SUPP_RATES, pos, left);
+		if (p) {
+			if (supportRateNum <= sizeof(supportRate)) {
+				memcpy(supportRate+supportRateNum, p + 2, p[1]);
+				supportRateNum += p[1];
 			}
 		}
 	}
@@ -1415,90 +1430,97 @@ unsigned int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *prec
 	pstat->wpa_pairwise_cipher = 0;
 	pstat->wpa2_pairwise_cipher = 0;
 	memset(pstat->wpa_ie, 0, sizeof(pstat->wpa_ie));
-	if ((psecuritypriv->wpa_psk & BIT(1)) && elems.rsn_ie) {
 
+	wpa_ie = cfg80211_find_ie(WLAN_EID_RSN, pos, left);
+	if (!wpa_ie)
+		wpa_ie = cfg80211_find_vendor_ie(WLAN_OUI_MICROSOFT,
+						 WLAN_OUI_TYPE_MICROSOFT_WPA,
+						 pos, left);
+	if (wpa_ie) {
 		int group_cipher = 0, pairwise_cipher = 0;
 
-		wpa_ie = elems.rsn_ie;
-		wpa_ie_len = elems.rsn_ie_len;
-
-		if (rtw_parse_wpa2_ie23a(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
-			pstat->dot8021xalg = 1;/* psk,  todo:802.1x */
-			pstat->wpa_psk |= BIT(1);
-
-			pstat->wpa2_group_cipher = group_cipher&psecuritypriv->wpa2_group_cipher;
-			pstat->wpa2_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa2_pairwise_cipher;
-
-			if (!pstat->wpa2_group_cipher)
-				status = WLAN_REASON_INVALID_GROUP_CIPHER;
-
-			if (!pstat->wpa2_pairwise_cipher)
-				status = WLAN_REASON_INVALID_PAIRWISE_CIPHER;
+		wpa_ie_len = wpa_ie[1];
+		if (psecuritypriv->wpa_psk & BIT(1)) {
+			r = rtw_parse_wpa2_ie23a(wpa_ie, wpa_ie_len + 2,
+						 &group_cipher,
+						 &pairwise_cipher, NULL);
+			if (r == _SUCCESS) {
+				pstat->dot8021xalg = 1;/* psk,  todo:802.1x */
+				pstat->wpa_psk |= BIT(1);
+
+				pstat->wpa2_group_cipher = group_cipher &
+					psecuritypriv->wpa2_group_cipher;
+				pstat->wpa2_pairwise_cipher = pairwise_cipher &
+					psecuritypriv->wpa2_pairwise_cipher;
+			} else
+				status = WLAN_STATUS_INVALID_IE;
+		} else if (psecuritypriv->wpa_psk & BIT(0)) {
+			r = rtw_parse_wpa_ie23a(wpa_ie, wpa_ie_len + 2,
+						&group_cipher, &pairwise_cipher,
+						NULL);
+			if (r == _SUCCESS) {
+				pstat->dot8021xalg = 1;/* psk,  todo:802.1x */
+				pstat->wpa_psk |= BIT(0);
+
+				pstat->wpa_group_cipher = group_cipher &
+					psecuritypriv->wpa_group_cipher;
+				pstat->wpa_pairwise_cipher = pairwise_cipher &
+					psecuritypriv->wpa_pairwise_cipher;
+			} else
+				status = WLAN_STATUS_INVALID_IE;
 		} else {
-			status = WLAN_STATUS_INVALID_IE;
+			wpa_ie = NULL;
+			wpa_ie_len = 0;
 		}
-
-	} else if ((psecuritypriv->wpa_psk & BIT(0)) && elems.wpa_ie) {
-
-		int group_cipher = 0, pairwise_cipher = 0;
-
-		wpa_ie = elems.wpa_ie;
-		wpa_ie_len = elems.wpa_ie_len;
-
-		if (rtw_parse_wpa_ie23a(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
-			pstat->dot8021xalg = 1;/* psk,  todo:802.1x */
-			pstat->wpa_psk |= BIT(0);
-
-			pstat->wpa_group_cipher = group_cipher&psecuritypriv->wpa_group_cipher;
-			pstat->wpa_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa_pairwise_cipher;
-
+		if (wpa_ie && status == WLAN_STATUS_SUCCESS) {
 			if (!pstat->wpa_group_cipher)
 				status = WLAN_STATUS_INVALID_GROUP_CIPHER;
 
 			if (!pstat->wpa_pairwise_cipher)
 				status = WLAN_STATUS_INVALID_PAIRWISE_CIPHER;
-
-		} else {
-			status = WLAN_STATUS_INVALID_IE;
 		}
-
-	} else {
-		wpa_ie = NULL;
-		wpa_ie_len = 0;
 	}
 
-	if (WLAN_STATUS_SUCCESS != status)
+	if (status != WLAN_STATUS_SUCCESS)
 		goto OnAssocReq23aFail;
 
 	pstat->flags &= ~(WLAN_STA_WPS | WLAN_STA_MAYBE_WPS);
-	if (wpa_ie == NULL) {
-		if (elems.wps_ie) {
-			DBG_8723A("STA included WPS IE in "
-				   "(Re)Association Request - assume WPS is "
-				   "used\n");
+
+	wps_ie = cfg80211_find_vendor_ie(WLAN_OUI_MICROSOFT,
+					 WLAN_OUI_TYPE_MICROSOFT_WPS,
+					 pos, left);
+
+	if (!wpa_ie) {
+		if (wps_ie) {
+			DBG_8723A("STA included WPS IE in (Re)Association "
+				  "Request - assume WPS is used\n");
 			pstat->flags |= WLAN_STA_WPS;
 		} else {
-			DBG_8723A("STA did not include WPA/RSN IE "
-				   "in (Re)Association Request - possible WPS "
-				   "use\n");
+			DBG_8723A("STA did not include WPA/RSN IE in (Re)"
+				   "Association Request - possible WPS use\n");
 			pstat->flags |= WLAN_STA_MAYBE_WPS;
 		}
 
-		/*  AP support WPA/RSN, and sta is going to do WPS, but AP is not ready */
+		/*  AP support WPA/RSN, and sta is going to do WPS, but AP
+		    is not ready */
 		/*  that the selected registrar of AP is _FLASE */
-		if ((psecuritypriv->wpa_psk > 0) &&
-		    (pstat->flags & (WLAN_STA_WPS|WLAN_STA_MAYBE_WPS))) {
+		if (psecuritypriv->wpa_psk > 0 &&
+		    pstat->flags & (WLAN_STA_WPS|WLAN_STA_MAYBE_WPS)) {
 			if (pmlmepriv->wps_beacon_ie) {
 				u8 selected_registrar = 0;
 
-				rtw_get_wps_attr_content23a(pmlmepriv->wps_beacon_ie, pmlmepriv->wps_beacon_ie_len,
-							 WPS_ATTR_SELECTED_REGISTRAR, &selected_registrar, NULL);
+				rtw_get_wps_attr_content23a(
+					pmlmepriv->wps_beacon_ie,
+					pmlmepriv->wps_beacon_ie_len,
+					WPS_ATTR_SELECTED_REGISTRAR,
+					&selected_registrar, NULL);
 
 				if (!selected_registrar) {
-					DBG_8723A("selected_registrar is false , or AP is not ready to do WPS\n");
+					DBG_8723A("selected_registrar is false,"
+						  "or AP is not ready to do "
+						  "WPS\n");
 
 					status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA;
-
 					goto OnAssocReq23aFail;
 				}
 			}
@@ -1508,26 +1530,26 @@ unsigned int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *prec
 
 		if (psecuritypriv->wpa_psk == 0) {
 			DBG_8723A("STA " MAC_FMT ": WPA/RSN IE in association "
-			"request, but AP don't support WPA/RSN\n", MAC_ARG(pstat->hwaddr));
+			"request, but AP don't support WPA/RSN\n",
+				  MAC_ARG(pstat->hwaddr));
 
 			status = WLAN_STATUS_INVALID_IE;
 
 			goto OnAssocReq23aFail;
 		}
 
-		if (elems.wps_ie) {
-			DBG_8723A("STA included WPS IE in "
-				   "(Re)Association Request - WPS is "
-				   "used\n");
+		if (wps_ie) {
+			DBG_8723A("STA included WPS IE in (Re)Association "
+				  "Request - WPS is used\n");
 			pstat->flags |= WLAN_STA_WPS;
 			copy_len = 0;
 		} else {
-			copy_len = ((wpa_ie_len+2) > sizeof(pstat->wpa_ie)) ? (sizeof(pstat->wpa_ie)):(wpa_ie_len+2);
+			copy_len = ((wpa_ie_len + 2) > sizeof(pstat->wpa_ie)) ?
+				sizeof(pstat->wpa_ie) : (wpa_ie_len + 2);
 		}
 
-		if (copy_len>0)
-			memcpy(pstat->wpa_ie, wpa_ie-2, copy_len);
-
+		if (copy_len > 0)
+			memcpy(pstat->wpa_ie, wpa_ie - 2, copy_len);
 	}
 
 	/*  check if there is WMM IE & support WWM-PS */
@@ -1539,45 +1561,45 @@ unsigned int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *prec
 	pstat->uapsd_vi = 0;
 	pstat->uapsd_be = 0;
 	pstat->uapsd_bk = 0;
-	if (pmlmepriv->qospriv.qos_option)
-	{
-		p = pframe + sizeof(struct ieee80211_hdr_3addr) + ie_offset; ie_len = 0;
-		for (;;)
-		{
-			p = rtw_get_ie23a(p, _VENDOR_SPECIFIC_IE_, &ie_len, pkt_len - sizeof(struct ieee80211_hdr_3addr) - ie_offset);
-			if (p != NULL) {
-				if (!memcmp(p+2, WMM_IE, 6)) {
-
+	if (pmlmepriv->qos_option) {
+		const u8 *end = pos + left;
+		p = pos;
+
+		for (;;) {
+			left = end - p;
+			p = cfg80211_find_ie(WLAN_EID_VENDOR_SPECIFIC, p, left);
+			if (p) {
+				if (!memcmp(p + 2, WMM_IE, 6)) {
 					pstat->flags |= WLAN_STA_WME;
 
 					pstat->qos_option = 1;
-					pstat->qos_info = *(p+8);
+					pstat->qos_info = *(p + 8);
 
-					pstat->max_sp_len = (pstat->qos_info>>5)&0x3;
+					pstat->max_sp_len =
+						(pstat->qos_info >> 5) & 0x3;
 
-					if ((pstat->qos_info&0xf) != 0xf)
+					if ((pstat->qos_info & 0xf) != 0xf)
 						pstat->has_legacy_ac = true;
 					else
 						pstat->has_legacy_ac = false;
 
-					if (pstat->qos_info&0xf)
-					{
-						if (pstat->qos_info&BIT(0))
+					if (pstat->qos_info & 0xf) {
+						if (pstat->qos_info & BIT(0))
 							pstat->uapsd_vo = BIT(0)|BIT(1);
 						else
 							pstat->uapsd_vo = 0;
 
-						if (pstat->qos_info&BIT(1))
+						if (pstat->qos_info & BIT(1))
 							pstat->uapsd_vi = BIT(0)|BIT(1);
 						else
 							pstat->uapsd_vi = 0;
 
-						if (pstat->qos_info&BIT(2))
+						if (pstat->qos_info & BIT(2))
 							pstat->uapsd_bk = BIT(0)|BIT(1);
 						else
 							pstat->uapsd_bk = 0;
 
-						if (pstat->qos_info&BIT(3))
+						if (pstat->qos_info & BIT(3))
 							pstat->uapsd_be = BIT(0)|BIT(1);
 						else
 							pstat->uapsd_be = 0;
@@ -1586,45 +1608,42 @@ unsigned int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *prec
 
 					break;
 				}
-			}
-			else {
+			} else {
 				break;
 			}
-			p = p + ie_len + 2;
+			p = p + p[1] + 2;
 		}
 	}
 
 	/* save HT capabilities in the sta object */
 	memset(&pstat->htpriv.ht_cap, 0, sizeof(struct ieee80211_ht_cap));
-	if (elems.ht_capabilities && elems.ht_capabilities_len >= sizeof(struct ieee80211_ht_cap))
-	{
+	p = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY, pos, left);
+
+	if (p && p[1] >= sizeof(struct ieee80211_ht_cap)) {
 		pstat->flags |= WLAN_STA_HT;
 
 		pstat->flags |= WLAN_STA_WME;
 
-		memcpy(&pstat->htpriv.ht_cap, elems.ht_capabilities, sizeof(struct ieee80211_ht_cap));
-
+		memcpy(&pstat->htpriv.ht_cap, p + 2,
+		       sizeof(struct ieee80211_ht_cap));
 	} else
 		pstat->flags &= ~WLAN_STA_HT;
 
-	if ((pmlmepriv->htpriv.ht_option == false) && (pstat->flags&WLAN_STA_HT))
-	{
+	if (pmlmepriv->htpriv.ht_option == false && pstat->flags & WLAN_STA_HT){
 		status = WLAN_STATUS_UNSPECIFIED_FAILURE;
 		goto OnAssocReq23aFail;
 	}
 
-	if ((pstat->flags & WLAN_STA_HT) &&
-		    ((pstat->wpa2_pairwise_cipher&WPA_CIPHER_TKIP) ||
-		      (pstat->wpa_pairwise_cipher&WPA_CIPHER_TKIP)))
-	{
-		DBG_8723A("HT: " MAC_FMT " tried to "
-				   "use TKIP with HT association\n", MAC_ARG(pstat->hwaddr));
+	if (pstat->flags & WLAN_STA_HT &&
+	    (pstat->wpa2_pairwise_cipher & WPA_CIPHER_TKIP ||
+	     pstat->wpa_pairwise_cipher & WPA_CIPHER_TKIP)) {
+		DBG_8723A("HT: " MAC_FMT " tried to use TKIP with HT "
+			  "association\n", MAC_ARG(pstat->hwaddr));
 
 		/* status = WLAN_STATUS_CIPHER_REJECTED_PER_POLICY; */
 		/* goto OnAssocReq23aFail; */
 	}
 
-       /*  */
 	pstat->flags |= WLAN_STA_NONERP;
 	for (i = 0; i < pstat->bssratelen; i++) {
 		if ((pstat->bssrateset[i] & 0x7f) > 22) {
@@ -1641,39 +1660,6 @@ unsigned int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *prec
 	if (status != WLAN_STATUS_SUCCESS)
 		goto OnAssocReq23aFail;
 
-#ifdef CONFIG_8723AU_P2P
-	pstat->is_p2p_device = false;
-	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
-	{
-		if ((p2pie = rtw_get_p2p_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + ie_offset, pkt_len - sizeof(struct ieee80211_hdr_3addr) - ie_offset, NULL, &p2pielen)))
-		{
-			pstat->is_p2p_device = true;
-			if ((p2p_status_code = (u8)process_assoc_req_p2p_ie23a(pwdinfo, pframe, pkt_len, pstat))>0)
-			{
-				pstat->p2p_status_code = p2p_status_code;
-				status = WLAN_STATUS_CAPS_UNSUPPORTED;
-				goto OnAssocReq23aFail;
-			}
-		}
-#ifdef CONFIG_8723AU_P2P
-		if (rtw_get_wfd_ie(pframe + sizeof(struct ieee80211_hdr_3addr) + ie_offset, pkt_len - sizeof(struct ieee80211_hdr_3addr) - ie_offset, wfd_ie, &wfd_ielen))
-		{
-			u8	attr_content[ 10 ] = { 0x00 };
-			u32	attr_contentlen = 0;
-
-			DBG_8723A("[%s] WFD IE Found!!\n", __func__);
-			rtw_get_wfd_attr_content(wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, attr_content, &attr_contentlen);
-			if (attr_contentlen)
-			{
-				pwdinfo->wfd_info->peer_rtsp_ctrlport = get_unaligned_be16(attr_content + 2);
-				DBG_8723A("[%s] Peer PORT NUM = %d\n", __func__, pwdinfo->wfd_info->peer_rtsp_ctrlport);
-			}
-		}
-#endif
-	}
-	pstat->p2p_status_code = p2p_status_code;
-#endif /* CONFIG_8723AU_P2P */
-
 	/* TODO: identify_proprietary_vendor_ie(); */
 	/*  Realtek proprietary IE */
 	/*  identify if this is Broadcom sta */
@@ -1699,14 +1685,13 @@ unsigned int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *prec
 			status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA;
 
 			goto OnAssocReq23aFail;
-
 		} else {
 			pstapriv->sta_aid[pstat->aid - 1] = pstat;
 			DBG_8723A("allocate new AID = (%d)\n", pstat->aid);
 		}
 	}
 
-	pstat->state &= (~WIFI_FW_ASSOC_STATE);
+	pstat->state &= ~WIFI_FW_ASSOC_STATE;
 	pstat->state |= WIFI_FW_ASSOC_SUCCESS;
 
 	spin_lock_bh(&pstapriv->auth_list_lock);
@@ -1725,18 +1710,20 @@ unsigned int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *prec
 	spin_unlock_bh(&pstapriv->asoc_list_lock);
 
 	/*  now the station is qualified to join our BSS... */
-	if (pstat && (pstat->state & WIFI_FW_ASSOC_SUCCESS) &&
-	    (WLAN_STATUS_SUCCESS == status)) {
+	if (pstat && pstat->state & WIFI_FW_ASSOC_SUCCESS &&
+	    status == WLAN_STATUS_SUCCESS) {
 #ifdef CONFIG_8723AU_AP_MODE
 		/* 1 bss_cap_update & sta_info_update23a */
 		bss_cap_update_on_sta_join23a(padapter, pstat);
 		sta_info_update23a(padapter, pstat);
 
 		/* issue assoc rsp before notify station join event. */
-		if (ieee80211_is_assoc_req(frame_control))
-			issue_asocrsp23a(padapter, status, pstat, WIFI_ASSOCRSP);
+		if (ieee80211_is_assoc_req(mgmt->frame_control))
+			issue_assocrsp(padapter, status, pstat,
+				       IEEE80211_STYPE_ASSOC_RESP);
 		else
-			issue_asocrsp23a(padapter, status, pstat, WIFI_REASSOCRSP);
+			issue_assocrsp(padapter, status, pstat,
+				       IEEE80211_STYPE_REASSOC_RESP);
 
 		/* 2 - report to upper layer */
 		DBG_8723A("indicate_sta_join_event to upper layer - hostapd\n");
@@ -1752,19 +1739,20 @@ unsigned int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *prec
 asoc_class2_error:
 
 #ifdef CONFIG_8723AU_AP_MODE
-	issue_deauth23a(padapter, hdr->addr2, status);
+	issue_deauth23a(padapter, mgmt->sa, status);
 #endif
-
 	return _FAIL;
 
 OnAssocReq23aFail:
 
 #ifdef CONFIG_8723AU_AP_MODE
 	pstat->aid = 0;
-	if (ieee80211_is_assoc_req(frame_control))
-		issue_asocrsp23a(padapter, status, pstat, WIFI_ASSOCRSP);
+	if (ieee80211_is_assoc_req(mgmt->frame_control))
+		issue_assocrsp(padapter, status, pstat,
+			       IEEE80211_STYPE_ASSOC_RESP);
 	else
-		issue_asocrsp23a(padapter, status, pstat, WIFI_REASSOCRSP);
+		issue_assocrsp(padapter, status, pstat,
+			       IEEE80211_STYPE_REASSOC_RESP);
 #endif
 
 #endif /* CONFIG_8723AU_AP_MODE */
@@ -1772,25 +1760,24 @@ OnAssocReq23aFail:
 	return _FAIL;
 }
 
-unsigned int OnAssocRsp23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
+static int
+OnAssocRsp23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
-	uint i;
-	int res;
-	unsigned short	status;
-	struct ndis_802_11_var_ies *pIE;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct sk_buff *skb = precv_frame->pkt;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
+	struct ieee80211_mgmt *pmgmt = (struct ieee80211_mgmt *) skb->data;
+	int res, i;
+	unsigned short status;
+	u8 *p;
 	u8 *pframe = skb->data;
-	uint pkt_len = skb->len;
+	int pkt_len = skb->len;
 
 	DBG_8723A("%s\n", __func__);
 
 	/* check A1 matches or not */
-	if (!ether_addr_equal(myid(&padapter->eeprompriv),
-			      ieee80211_get_DA(hdr)))
+	if (!ether_addr_equal(myid(&padapter->eeprompriv), pmgmt->da))
 		return _SUCCESS;
 
 	if (!(pmlmeinfo->state & (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE)))
@@ -1802,8 +1789,8 @@ unsigned int OnAssocRsp23a(struct rtw_adapter *padapter, struct recv_frame *prec
 	del_timer_sync(&pmlmeext->link_timer);
 
 	/* status */
-	if ((status = le16_to_cpu(*(unsigned short *)(pframe + sizeof(struct ieee80211_hdr_3addr) + 2))) > 0)
-	{
+	status = le16_to_cpu(pmgmt->u.assoc_resp.status_code);
+	if (status > 0)	{
 		DBG_8723A("assoc reject, status code: %d\n", status);
 		pmlmeinfo->state = WIFI_FW_NULL_STATE;
 		res = -4;
@@ -1811,52 +1798,47 @@ unsigned int OnAssocRsp23a(struct rtw_adapter *padapter, struct recv_frame *prec
 	}
 
 	/* get capabilities */
-	pmlmeinfo->capability = le16_to_cpu(*(unsigned short *)(pframe + sizeof(struct ieee80211_hdr_3addr)));
+	pmlmeinfo->capability = le16_to_cpu(pmgmt->u.assoc_resp.capab_info);
 
 	/* set slot time */
 	pmlmeinfo->slotTime = (pmlmeinfo->capability & BIT(10))? 9: 20;
 
 	/* AID */
-	res = pmlmeinfo->aid = (int)(le16_to_cpu(*(unsigned short *)(pframe + sizeof(struct ieee80211_hdr_3addr) + 4))&0x3fff);
+	res = pmlmeinfo->aid = le16_to_cpu(pmgmt->u.assoc_resp.aid) & 0x3fff;
 
 	/* following are moved to join event callback function */
 	/* to handle HT, WMM, rate adaptive, update MAC reg */
 	/* for not to handle the synchronous IO in the tasklet */
-	for (i = (6 + sizeof(struct ieee80211_hdr_3addr)); i < pkt_len;) {
-		pIE = (struct ndis_802_11_var_ies *)(pframe + i);
+	for (i = offsetof(struct ieee80211_mgmt, u.assoc_resp.variable);
+	     i < pkt_len;) {
+		p = pframe + i;
 
-		switch (pIE->ElementID)
+		switch (p[0])
 		{
-		case _VENDOR_SPECIFIC_IE_:
-			if (!memcmp(pIE->data, WMM_PARA_OUI23A, 6))/* WMM */
-					WMM_param_handler23a(padapter, pIE);
-#if defined(CONFIG_8723AU_P2P)
-			else if (!memcmp(pIE->data, WFD_OUI23A, 4)) { /* WFD */
-				DBG_8723A("[%s] Found WFD IE\n", __func__);
-				WFD_info_handler(padapter, pIE);
-			}
-#endif
+		case WLAN_EID_VENDOR_SPECIFIC:
+			if (!memcmp(p + 2, WMM_PARA_OUI23A, 6))/* WMM */
+				WMM_param_handler23a(padapter, p);
 			break;
 
-		case _HT_CAPABILITY_IE_:	/* HT caps */
-			HT_caps_handler23a(padapter, pIE);
+		case WLAN_EID_HT_CAPABILITY:	/* HT caps */
+			HT_caps_handler23a(padapter, p);
 			break;
 
-		case _HT_EXTRA_INFO_IE_:	/* HT info */
-			HT_info_handler23a(padapter, pIE);
+		case WLAN_EID_HT_OPERATION:	/* HT info */
+			HT_info_handler23a(padapter, p);
 			break;
 
-		case _ERPINFO_IE_:
-			ERP_IE_handler23a(padapter, pIE);
+		case WLAN_EID_ERP_INFO:
+			ERP_IE_handler23a(padapter, p);
 
 		default:
 			break;
 		}
 
-		i += (pIE->Length + 2);
+		i += (p[1] + 2);
 	}
 
-	pmlmeinfo->state &= (~WIFI_FW_ASSOC_STATE);
+	pmlmeinfo->state &= ~WIFI_FW_ASSOC_STATE;
 	pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
 
 	/* Update Basic Rate Table for spec, 2010-12-28 , by thomas */
@@ -1879,44 +1861,33 @@ report_assoc_result:
 	return _SUCCESS;
 }
 
-unsigned int OnDeAuth23a(struct rtw_adapter *padapter,
-			 struct recv_frame *precv_frame)
+static int
+OnDeAuth23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
-	unsigned short	reason;
+	unsigned short reason;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct sk_buff *skb = precv_frame->pkt;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-	u8 *pframe = skb->data;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-#endif /* CONFIG_8723AU_P2P */
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
 
-	/* check A3 */
-	if (!ether_addr_equal(hdr->addr3, get_my_bssid23a(&pmlmeinfo->network)))
+	if (!ether_addr_equal(mgmt->bssid,
+			      get_my_bssid23a(&pmlmeinfo->network)))
 		return _SUCCESS;
 
-#ifdef CONFIG_8723AU_P2P
-	if (pwdinfo->rx_invitereq_info.scan_op_ch_only) {
-		mod_timer(&pwdinfo->reset_ch_sitesurvey,
-			  jiffies + msecs_to_jiffies(10));
-	}
-#endif /* CONFIG_8723AU_P2P */
-
-	reason = le16_to_cpu(*(unsigned short *)(pframe + sizeof(struct ieee80211_hdr_3addr)));
+	reason = le16_to_cpu(mgmt->u.deauth.reason_code);
 
 	DBG_8723A("%s Reason code(%d)\n", __func__, reason);
 
 #ifdef CONFIG_8723AU_AP_MODE
-	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) {
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
 		struct sta_info *psta;
 		struct sta_priv *pstapriv = &padapter->stapriv;
 
 		DBG_8723A_LEVEL(_drv_always_, "ap recv deauth reason code(%d) "
-				"sta:%pM\n", reason, hdr->addr2);
+				"sta:%pM\n", reason, mgmt->sa);
 
-		psta = rtw_get_stainfo23a(pstapriv, hdr->addr2);
+		psta = rtw_get_stainfo23a(pstapriv, mgmt->sa);
 		if (psta) {
 			u8 updated = 0;
 
@@ -1933,46 +1904,34 @@ unsigned int OnDeAuth23a(struct rtw_adapter *padapter,
 		}
 
 		return _SUCCESS;
-	}
-	else
+	} else
 #endif
 	{
 		DBG_8723A_LEVEL(_drv_always_, "sta recv deauth reason code(%d) "
-				"sta:%pM\n", reason, hdr->addr3);
+				"sta:%pM\n", reason, mgmt->bssid);
 
-		receive_disconnect23a(padapter, hdr->addr3, reason);
+		receive_disconnect23a(padapter, mgmt->bssid, reason);
 	}
 	pmlmepriv->LinkDetectInfo.bBusyTraffic = false;
+
 	return _SUCCESS;
 }
 
-unsigned int OnDisassoc23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
+static int
+OnDisassoc23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
 	unsigned short	reason;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct sk_buff *skb = precv_frame->pkt;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-	u8 *pframe = skb->data;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-#endif /* CONFIG_8723AU_P2P */
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
 
-	/* check A3 */
-	if (!ether_addr_equal(hdr->addr3, get_my_bssid23a(&pmlmeinfo->network)))
+	if (!ether_addr_equal(mgmt->bssid,
+			      get_my_bssid23a(&pmlmeinfo->network)))
 		return _SUCCESS;
 
-#ifdef CONFIG_8723AU_P2P
-	if (pwdinfo->rx_invitereq_info.scan_op_ch_only)
-	{
-		mod_timer(&pwdinfo->reset_ch_sitesurvey,
-			  jiffies + msecs_to_jiffies(10));
-	}
-#endif /* CONFIG_8723AU_P2P */
-
-	reason = le16_to_cpu(*(unsigned short *)
-			     (pframe + sizeof(struct ieee80211_hdr_3addr)));
+	reason = le16_to_cpu(mgmt->u.disassoc.reason_code);
 
         DBG_8723A("%s Reason code(%d)\n", __func__, reason);
 
@@ -1982,9 +1941,9 @@ unsigned int OnDisassoc23a(struct rtw_adapter *padapter, struct recv_frame *prec
 		struct sta_priv *pstapriv = &padapter->stapriv;
 
 		DBG_8723A_LEVEL(_drv_always_, "ap recv disassoc reason code(%d)"
-				" sta:%pM\n", reason, hdr->addr2);
+				" sta:%pM\n", reason, mgmt->sa);
 
-		psta = rtw_get_stainfo23a(pstapriv, hdr->addr2);
+		psta = rtw_get_stainfo23a(pstapriv, mgmt->sa);
 		if (psta) {
 			u8 updated = 0;
 
@@ -1993,7 +1952,7 @@ unsigned int OnDisassoc23a(struct rtw_adapter *padapter, struct recv_frame *prec
 				list_del_init(&psta->asoc_list);
 				pstapriv->asoc_list_cnt--;
 				updated = ap_free_sta23a(padapter, psta,
-						      false, reason);
+							 false, reason);
 			}
 			spin_unlock_bh(&pstapriv->asoc_list_lock);
 
@@ -2001,57 +1960,59 @@ unsigned int OnDisassoc23a(struct rtw_adapter *padapter, struct recv_frame *prec
 		}
 
 		return _SUCCESS;
-	}
-	else
+	} else
 #endif
 	{
 		DBG_8723A_LEVEL(_drv_always_, "ap recv disassoc reason "
-				"code(%d) sta:%pM\n", reason, hdr->addr3);
+				"code(%d) sta:%pM\n", reason, mgmt->bssid);
 
-		receive_disconnect23a(padapter, hdr->addr3, reason);
+		receive_disconnect23a(padapter, mgmt->bssid, reason);
 	}
 	pmlmepriv->LinkDetectInfo.bBusyTraffic = false;
 	return _SUCCESS;
 }
 
-unsigned int OnAtim23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
+static int
+OnAtim23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
 	DBG_8723A("%s\n", __func__);
 	return _SUCCESS;
 }
 
-unsigned int on_action_spct23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
+static int
+on_action_spct23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
 	return _FAIL;
 }
 
-unsigned int OnAction23a_qos(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
+static int
+OnAction23a_qos(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
 	return _SUCCESS;
 }
 
-unsigned int OnAction23a_dls(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
+static int
+OnAction23a_dls(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
 	return _SUCCESS;
 }
 
-unsigned int OnAction23a_back23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
+static int OnAction23a_back23a(struct rtw_adapter *padapter,
+			       struct recv_frame *precv_frame)
 {
 	u8 *addr;
 	struct sta_info *psta = NULL;
 	struct recv_reorder_ctrl *preorder_ctrl;
-	unsigned char		*frame_body;
-	unsigned char		category, action;
-	unsigned short	tid, status, reason_code = 0;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	unsigned char category, action;
+	unsigned short tid, status, capab, params, reason_code = 0;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct sk_buff *skb = precv_frame->pkt;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-	u8 *pframe = skb->data;
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
 	struct sta_priv *pstapriv = &padapter->stapriv;
 
 	/* check RA matches or not */
-	if (!ether_addr_equal(myid(&padapter->eeprompriv), hdr->addr1))
+	if (!ether_addr_equal(myid(&padapter->eeprompriv), mgmt->da))
 		return _SUCCESS;
 
 	DBG_8723A("%s\n", __func__);
@@ -2060,24 +2021,24 @@ unsigned int OnAction23a_back23a(struct rtw_adapter *padapter, struct recv_frame
 		if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
 			return _SUCCESS;
 
-	addr = hdr->addr2;
+	addr = mgmt->sa;
 	psta = rtw_get_stainfo23a(pstapriv, addr);
 
 	if (!psta)
 		return _SUCCESS;
 
-	frame_body = (unsigned char *)
-		(pframe + sizeof(struct ieee80211_hdr_3addr));
-
-	category = frame_body[0];
+	category = mgmt->u.action.category;
 	if (category == WLAN_CATEGORY_BACK) { /*  representing Block Ack */
 		if (!pmlmeinfo->HT_enable)
 			return _SUCCESS;
-		action = frame_body[1];
+		/* action_code is located in the same place for all
+		   action events, so pick any */
+		action = mgmt->u.action.u.wme_action.action_code;
 		DBG_8723A("%s, action =%d\n", __func__, action);
 		switch (action) {
 		case WLAN_ACTION_ADDBA_REQ: /* ADDBA request */
-			memcpy(&pmlmeinfo->ADDBA_req, &frame_body[2],
+			memcpy(&pmlmeinfo->ADDBA_req,
+			       &mgmt->u.action.u.addba_req.dialog_token,
 			       sizeof(struct ADDBA_request));
 			process_addba_req23a(padapter,
 					     (u8 *)&pmlmeinfo->ADDBA_req, addr);
@@ -2091,36 +2052,34 @@ unsigned int OnAction23a_back23a(struct rtw_adapter *padapter, struct recv_frame
 			}
 			break;
 		case WLAN_ACTION_ADDBA_RESP: /* ADDBA response */
-			status = get_unaligned_le16(&frame_body[3]);
-			tid = ((frame_body[5] >> 2) & 0x7);
+			status = get_unaligned_le16(
+				&mgmt->u.action.u.addba_resp.status);
+			capab = get_unaligned_le16(
+				&mgmt->u.action.u.addba_resp.capab);
+			tid = (capab & IEEE80211_ADDBA_PARAM_TID_MASK) >> 2;
 			if (status == 0) {	/* successful */
 				DBG_8723A("agg_enable for TID =%d\n", tid);
-				psta->htpriv.agg_enable_bitmap |= 1 << tid;
-				psta->htpriv.candidate_tid_bitmap &=
-					~CHKBIT(tid);
+				psta->htpriv.agg_enable_bitmap |= BIT(tid);
+				psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
 			} else
-				psta->htpriv.agg_enable_bitmap &= ~CHKBIT(tid);
+				psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
 			break;
 
 		case WLAN_ACTION_DELBA: /* DELBA */
-			if ((frame_body[3] & BIT(3)) == 0) {
-				psta->htpriv.agg_enable_bitmap &=
-					~(1 << ((frame_body[3] >> 4) & 0xf));
-				psta->htpriv.candidate_tid_bitmap &=
-					~(1 << ((frame_body[3] >> 4) & 0xf));
-
-				/* reason_code = frame_body[4] | (frame_body[5] << 8); */
-				reason_code = get_unaligned_le16(&frame_body[4]);
-			} else if ((frame_body[3] & BIT(3)) == BIT(3)) {
-				tid = (frame_body[3] >> 4) & 0x0F;
-
-				preorder_ctrl =  &psta->recvreorder_ctrl[tid];
+			params = get_unaligned_le16(
+				&mgmt->u.action.u.delba.params);
+			tid = params >> 12;
+
+			if (params & IEEE80211_DELBA_PARAM_INITIATOR_MASK) {
+				preorder_ctrl = &psta->recvreorder_ctrl[tid];
 				preorder_ctrl->enable = false;
 				preorder_ctrl->indicate_seq = 0xffff;
+			} else {
+				psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
+				psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
 			}
-
-			DBG_8723A("%s(): DELBA: %x(%x)\n", __func__,
-				  pmlmeinfo->agg_enable_bitmap, reason_code);
+			reason_code = get_unaligned_le16(
+				&mgmt->u.action.u.delba.reason_code);
 			/* todo: how to notify the host while receiving
 			   DELETE BA */
 			break;
@@ -2131,2365 +2090,7 @@ unsigned int OnAction23a_back23a(struct rtw_adapter *padapter, struct recv_frame
 	return _SUCCESS;
 }
 
-#ifdef CONFIG_8723AU_P2P
-
-static int get_reg_classes_full_count(struct p2p_channels channel_list) {
-	int cnt = 0;
-	int i;
-
-	for (i = 0; i < channel_list.reg_classes; i++)
-		cnt += channel_list.reg_class[i].channels;
-
-	return cnt;
-}
-
-void issue_p2p_GO_request23a(struct rtw_adapter *padapter, u8* raddr)
-{
-	unsigned char category = WLAN_CATEGORY_PUBLIC;
-	u8			action = P2P_PUB_ACTION_ACTION;
-	u32			p2poui = cpu_to_be32(P2POUI);
-	u8			oui_subtype = P2P_GO_NEGO_REQ;
-	u8			wpsie[ 255 ] = { 0x00 }, p2pie[ 255 ] = { 0x00 };
-	u8			wpsielen = 0, p2pielen = 0;
-	u16			len_channellist_attr = 0;
-#ifdef CONFIG_8723AU_P2P
-	u32					wfdielen = 0;
-#endif /* CONFIG_8723AU_P2P */
-
-	struct xmit_frame			*pmgntframe;
-	struct pkt_attrib			*pattrib;
-	unsigned char					*pframe;
-	struct ieee80211_hdr	*pwlanhdr;
-	unsigned short				*fctrl;
-	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
-		return;
-
-	DBG_8723A("[%s] In\n", __func__);
-	/* update attribute */
-	pattrib = &pmgntframe->attrib;
-	update_mgntframe_attrib23a(padapter, pattrib);
-
-	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
-
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
-	pwlanhdr = (struct ieee80211_hdr *)pframe;
-
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
-
-	ether_addr_copy(pwlanhdr->addr1, raddr);
-	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
-	ether_addr_copy(pwlanhdr->addr3, myid(&padapter->eeprompriv));
-
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
-	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ACTION);
-
-	pframe += sizeof(struct ieee80211_hdr_3addr);
-	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
-
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *)&p2poui,
-				     &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen);
-	pwdinfo->negotiation_dialog_token = 1;	/*Initialize the dialog value*/
-	pframe = rtw_set_fixed_ie23a(pframe, 1,
-				     &pwdinfo->negotiation_dialog_token,
-				     &pattrib->pktlen);
-
-	/*	WPS Section */
-	wpsielen = 0;
-	/*	WPS OUI */
-	*(u32*) (wpsie) = cpu_to_be32(WPSOUI);
-	wpsielen += 4;
-
-	/*	WPS version */
-	/*	Type: */
-	*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
-	wpsielen += 2;
-
-	/*	Length: */
-	*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0001);
-	wpsielen += 2;
-
-	/*	Value: */
-	wpsie[wpsielen++] = WPS_VERSION_1;	/*	Version 1.0 */
-
-	/*	Device Password ID */
-	/*	Type: */
-	*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_PWID);
-	wpsielen += 2;
-
-	/*	Length: */
-	*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0002);
-	wpsielen += 2;
-
-	/*	Value: */
-
-	if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PEER_DISPLAY_PIN)
-	{
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_DPID_USER_SPEC);
-	}
-	else if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_SELF_DISPLAY_PIN)
-	{
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_DPID_REGISTRAR_SPEC);
-	}
-	else if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PBC)
-	{
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_DPID_PBC);
-	}
-
-	wpsielen += 2;
-
-	pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen);
-
-	/*	P2P IE Section. */
-
-	/*	P2P OUI */
-	p2pielen = 0;
-	p2pie[p2pielen++] = 0x50;
-	p2pie[p2pielen++] = 0x6F;
-	p2pie[p2pielen++] = 0x9A;
-	p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
-
-	/*	Commented by Albert 20110306 */
-	/*	According to the P2P Specification, the group negoitation request frame should contain 9 P2P attributes */
-	/*	1. P2P Capability */
-	/*	2. Group Owner Intent */
-	/*	3. Configuration Timeout */
-	/*	4. Listen Channel */
-	/*	5. Extended Listen Timing */
-	/*	6. Intended P2P Interface Address */
-	/*	7. Channel List */
-	/*	8. P2P Device Info */
-	/*	9. Operating Channel */
-
-	/*	P2P Capability */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Device Capability Bitmap, 1 byte */
-	p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
-
-	/*	Group Capability Bitmap, 1 byte */
-	if (pwdinfo->persistent_supported)
-	{
-		p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN | P2P_GRPCAP_PERSISTENT_GROUP;
-	}
-	else
-	{
-		p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN;
-	}
-
-	/*	Group Owner Intent */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_GO_INTENT;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0001);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Todo the tie breaker bit. */
-	p2pie[p2pielen++] = ((pwdinfo->intent << 1) | BIT(0));
-
-	/*	Configuration Timeout */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002);
-	p2pielen += 2;
-
-	/*	Value: */
-	p2pie[p2pielen++] = 200;	/*	2 seconds needed to be the P2P GO */
-	p2pie[p2pielen++] = 200;	/*	2 seconds needed to be the P2P Client */
-
-	/*	Listen Channel */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_LISTEN_CH;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0005);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Country String */
-	p2pie[p2pielen++] = 'X';
-	p2pie[p2pielen++] = 'X';
-
-	/*	The third byte should be set to 0x04. */
-	/*	Described in the "Operating Channel Attribute" section. */
-	p2pie[p2pielen++] = 0x04;
-
-	/*	Operating Class */
-	p2pie[p2pielen++] = 0x51;	/*	Copy from SD7 */
-
-	/*	Channel Number */
-	p2pie[p2pielen++] = pwdinfo->listen_channel;	/*	listening channel number */
-
-	/*	Extended Listen Timing ATTR */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_EX_LISTEN_TIMING;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0004);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Availability Period */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF);
-	p2pielen += 2;
-
-	/*	Availability Interval */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF);
-	p2pielen += 2;
-
-	/*	Intended P2P Interface Address */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_INTENTED_IF_ADDR;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
-	p2pielen += 2;
-
-	/*	Value: */
-	memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
-	p2pielen += ETH_ALEN;
-
-	/*	Channel List */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
-
-	/*  Length: */
-	/*  Country String(3) */
-	/*  + (Operating Class (1) + Number of Channels(1)) * Operation Classes (?) */
-	/*  + number of channels in all classes */
-	len_channellist_attr = 3
-	   + (1 + 1) * (u16)(pmlmeext->channel_list.reg_classes)
-	   + get_reg_classes_full_count(pmlmeext->channel_list);
-
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Country String */
-	p2pie[p2pielen++] = 'X';
-	p2pie[p2pielen++] = 'X';
-
-	/*	The third byte should be set to 0x04. */
-	/*	Described in the "Operating Channel Attribute" section. */
-	p2pie[p2pielen++] = 0x04;
-
-	/*	Channel Entry List */
-
-	{
-		int i, j;
-		for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
-			/*	Operating Class */
-			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].reg_class;
-
-			/*	Number of Channels */
-			p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channels;
-
-			/*	Channel List */
-			for (i = 0; i < pmlmeext->channel_list.reg_class[j].channels; i++) {
-				p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
-			}
-		}
-	}
-
-	/*	Device Info */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
-	/*	+ NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	P2P Device Address */
-	memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
-	p2pielen += ETH_ALEN;
-
-	/*	Config Method */
-	/*	This field should be big endian. Noted by P2P specification. */
-
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->supported_wps_cm);
-
-	p2pielen += 2;
-
-	/*	Primary Device Type */
-	/*	Category ID */
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
-	p2pielen += 2;
-
-	/*	OUI */
-	*(u32*) (p2pie + p2pielen) = cpu_to_be32(WPSOUI);
-	p2pielen += 4;
-
-	/*	Sub Category ID */
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
-	p2pielen += 2;
-
-	/*	Number of Secondary Device Types */
-	p2pie[p2pielen++] = 0x00;	/*	No Secondary Device Type List */
-
-	/*	Device Name */
-	/*	Type: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
-	p2pielen += 2;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len);
-	p2pielen += 2;
-
-	/*	Value: */
-	memcpy(p2pie + p2pielen, pwdinfo->device_name,
-	       pwdinfo->device_name_len);
-	p2pielen += pwdinfo->device_name_len;
-
-	/*	Operating Channel */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0005);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Country String */
-	p2pie[p2pielen++] = 'X';
-	p2pie[p2pielen++] = 'X';
-
-	/*	The third byte should be set to 0x04. */
-	/*	Described in the "Operating Channel Attribute" section. */
-	p2pie[p2pielen++] = 0x04;
-
-	/*	Operating Class */
-	if (pwdinfo->operating_channel <= 14)
-	{
-		/*	Operating Class */
-		p2pie[p2pielen++] = 0x51;
-	}
-	else if ((pwdinfo->operating_channel >= 36) && (pwdinfo->operating_channel <= 48))
-	{
-		/*	Operating Class */
-		p2pie[p2pielen++] = 0x73;
-	}
-	else
-	{
-		/*	Operating Class */
-		p2pie[p2pielen++] = 0x7c;
-	}
-
-	/*	Channel Number */
-	p2pie[p2pielen++] = pwdinfo->operating_channel;	/*	operating channel number */
-
-	pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &pattrib->pktlen);
-
-#ifdef CONFIG_8723AU_P2P
-	wfdielen = build_nego_req_wfd_ie(pwdinfo, pframe);
-	pframe += wfdielen;
-	pattrib->pktlen += wfdielen;
-#endif /* CONFIG_8723AU_P2P */
-
-	pattrib->last_txcmdsz = pattrib->pktlen;
-
-	dump_mgntframe23a(padapter, pmgntframe);
-
-	return;
-}
-
-static void issue_p2p_GO_response(struct rtw_adapter *padapter, u8* raddr, u8* frame_body, uint len, u8 result)
-{
-
-	unsigned char category = WLAN_CATEGORY_PUBLIC;
-	u8 action = P2P_PUB_ACTION_ACTION;
-	u32 p2poui = cpu_to_be32(P2POUI);
-	u8 oui_subtype = P2P_GO_NEGO_RESP;
-	u8 wpsie[255] = { 0x00 }, p2pie[255] = { 0x00 };
-	u8 p2pielen = 0;
-	uint wpsielen = 0;
-	u16 wps_devicepassword_id = 0x0000;
-	uint wps_devicepassword_id_len = 0;
-	u16 len_channellist_attr = 0;
-	int i, j;
-	struct xmit_frame *pmgntframe;
-	struct pkt_attrib *pattrib;
-	unsigned char *pframe;
-	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
-	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-#ifdef CONFIG_8723AU_P2P
-	u32 wfdielen = 0;
-#endif /* CONFIG_8723AU_P2P */
-
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
-		return;
-
-	DBG_8723A("[%s] In, result = %d\n", __func__,  result);
-	/* update attribute */
-	pattrib = &pmgntframe->attrib;
-	update_mgntframe_attrib23a(padapter, pattrib);
-
-	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
-
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
-	pwlanhdr = (struct ieee80211_hdr *)pframe;
-
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
-
-	ether_addr_copy(pwlanhdr->addr1, raddr);
-	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
-	ether_addr_copy(pwlanhdr->addr3, myid(&padapter->eeprompriv));
-
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
-	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ACTION);
-
-	pframe += sizeof(struct ieee80211_hdr_3addr);
-	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
-
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *) &p2poui,
-				     &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen);
-	/*	The Dialog Token of provisioning discovery request frame. */
-	pwdinfo->negotiation_dialog_token = frame_body[7];
-	pframe = rtw_set_fixed_ie23a(pframe, 1,
-				     &pwdinfo->negotiation_dialog_token,
-				     &pattrib->pktlen);
-
-	/*	Commented by Albert 20110328 */
-	/*	Try to get the device password ID from the WPS IE of group
-		negotiation request frame */
-	/*	WiFi Direct test plan 5.1.15 */
-	rtw_get_wps_ie23a(frame_body + _PUBLIC_ACTION_IE_OFFSET_,
-			  len - _PUBLIC_ACTION_IE_OFFSET_, wpsie, &wpsielen);
-	rtw_get_wps_attr_content23a(wpsie, wpsielen, WPS_ATTR_DEVICE_PWID,
-				    (u8 *)&wps_devicepassword_id,
-				    &wps_devicepassword_id_len);
-	wps_devicepassword_id = be16_to_cpu(wps_devicepassword_id);
-
-	memset(wpsie, 0x00, 255);
-	wpsielen = 0;
-
-	/*	WPS Section */
-	wpsielen = 0;
-	/*	WPS OUI */
-	*(u32*) (wpsie) = cpu_to_be32(WPSOUI);
-	wpsielen += 4;
-
-	/*	WPS version */
-	/*	Type: */
-	*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
-	wpsielen += 2;
-
-	/*	Length: */
-	*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0001);
-	wpsielen += 2;
-
-	/*	Value: */
-	wpsie[wpsielen++] = WPS_VERSION_1;	/*	Version 1.0 */
-
-	/*	Device Password ID */
-	/*	Type: */
-	*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_PWID);
-	wpsielen += 2;
-
-	/*	Length: */
-	*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0002);
-	wpsielen += 2;
-
-	/*	Value: */
-	if (wps_devicepassword_id == WPS_DPID_USER_SPEC) {
-		*(u16*) (wpsie + wpsielen) =
-			cpu_to_be16(WPS_DPID_REGISTRAR_SPEC);
-	} else if (wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC) {
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_DPID_USER_SPEC);
-	} else {
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_DPID_PBC);
-	}
-	wpsielen += 2;
-
-	/*	Commented by Kurt 20120113 */
-	/*	If some device wants to do p2p handshake without sending prov_disc_req */
-	/*	We have to get peer_req_cm from here. */
-	if (!memcmp(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "000", 3)) {
-		if (wps_devicepassword_id == WPS_DPID_USER_SPEC) {
-			memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3);
-		} else if (wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC) {
-			memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3);
-		} else {
-			memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3);
-		}
-	}
-
-	pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, wpsielen,
-			       (unsigned char *) wpsie, &pattrib->pktlen);
-
-	/*	P2P IE Section. */
-
-	/*	P2P OUI */
-	p2pielen = 0;
-	p2pie[p2pielen++] = 0x50;
-	p2pie[p2pielen++] = 0x6F;
-	p2pie[p2pielen++] = 0x9A;
-	p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
-
-	/*	Commented by Albert 20100908 */
-	/*	According to the P2P Specification, the group negoitation
-		response frame should contain 9 P2P attributes */
-	/*	1. Status */
-	/*	2. P2P Capability */
-	/*	3. Group Owner Intent */
-	/*	4. Configuration Timeout */
-	/*	5. Operating Channel */
-	/*	6. Intended P2P Interface Address */
-	/*	7. Channel List */
-	/*	8. Device Info */
-	/*	9. Group ID	(Only GO) */
-
-	/*	ToDo: */
-
-	/*	P2P Status */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_STATUS;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0001);
-	p2pielen += 2;
-
-	/*	Value: */
-	p2pie[p2pielen++] = result;
-
-	/*	P2P Capability */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Device Capability Bitmap, 1 byte */
-
-	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) {
-		/*	Commented by Albert 2011/03/08 */
-		/*	According to the P2P specification */
-		/*	if the sending device will be client, the P2P
-			Capability should be reserved of group negotation
-			response frame */
-		p2pie[p2pielen++] = 0;
-	} else {
-		/*	Be group owner or meet the error case */
-		p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
-	}
-
-	/*	Group Capability Bitmap, 1 byte */
-	if (pwdinfo->persistent_supported) {
-		p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN |
-			P2P_GRPCAP_PERSISTENT_GROUP;
-	} else {
-		p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN;
-	}
-
-	/*	Group Owner Intent */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_GO_INTENT;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0001);
-	p2pielen += 2;
-
-	/*	Value: */
-	if (pwdinfo->peer_intent & 0x01) {
-		/*	Peer's tie breaker bit is 1, our tie breaker
-			bit should be 0 */
-		p2pie[p2pielen++] = (pwdinfo->intent << 1);
-	} else {
-		/* Peer's tie breaker bit is 0, our tie breaker bit
-		   should be 1 */
-		p2pie[p2pielen++] = ((pwdinfo->intent << 1) | BIT(0));
-	}
-
-	/*	Configuration Timeout */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	2 seconds needed to be the P2P GO */
-	p2pie[p2pielen++] = 200;
-	/*	2 seconds needed to be the P2P Client */
-	p2pie[p2pielen++] = 200;
-
-	/*	Operating Channel */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0005);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Country String */
-	p2pie[p2pielen++] = 'X';
-	p2pie[p2pielen++] = 'X';
-
-	/*	The third byte should be set to 0x04. */
-	/*	Described in the "Operating Channel Attribute" section. */
-	p2pie[p2pielen++] = 0x04;
-
-	/*	Operating Class */
-	if (pwdinfo->operating_channel <= 14) {
-		/*	Operating Class */
-		p2pie[p2pielen++] = 0x51;
-	} else if ((pwdinfo->operating_channel >= 36) &&
-		   (pwdinfo->operating_channel <= 48)) {
-		/*	Operating Class */
-		p2pie[p2pielen++] = 0x73;
-	} else {
-		/*	Operating Class */
-		p2pie[p2pielen++] = 0x7c;
-	}
-
-	/*	Channel Number */
-	/*	operating channel number */
-	p2pie[p2pielen++] = pwdinfo->operating_channel;
-
-	/*	Intended P2P Interface Address */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_INTENTED_IF_ADDR;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
-	p2pielen += 2;
-
-	/*	Value: */
-	memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
-	p2pielen += ETH_ALEN;
-
-	/*	Channel List */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
-
-	/*  Country String(3) */
-	/*  + (Operating Class (1) + Number of Channels(1)) *
-	    Operation Classes (?) */
-	/*  + number of channels in all classes */
-	len_channellist_attr = 3 +
-		(1 + 1) * (u16)pmlmeext->channel_list.reg_classes +
-		get_reg_classes_full_count(pmlmeext->channel_list);
-
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
-
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Country String */
-	p2pie[p2pielen++] = 'X';
-	p2pie[p2pielen++] = 'X';
-
-	/*	The third byte should be set to 0x04. */
-	/*	Described in the "Operating Channel Attribute" section. */
-	p2pie[p2pielen++] = 0x04;
-
-	/*	Channel Entry List */
-
-	for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
-		/*	Operating Class */
-		p2pie[p2pielen++] =
-			pmlmeext->channel_list.reg_class[j].reg_class;
-
-		/*	Number of Channels */
-		p2pie[p2pielen++] =
-			pmlmeext->channel_list.reg_class[j].channels;
-
-		/*	Channel List */
-		for (i = 0;
-		     i < pmlmeext->channel_list.reg_class[j].channels; i++) {
-			p2pie[p2pielen++] =
-				pmlmeext->channel_list.reg_class[j].channel[i];
-		}
-	}
-
-	/*	Device Info */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	21 -> P2P Device Address (6bytes) + Config Methods (2bytes) +
-		Primary Device Type (8bytes) */
-	/*	+ NumofSecondDevType (1byte) + WPS Device Name ID field
-		(2bytes) + WPS Device Name Len field (2bytes) */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	P2P Device Address */
-	memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
-	p2pielen += ETH_ALEN;
-
-	/*	Config Method */
-	/*	This field should be big endian. Noted by P2P specification. */
-
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->supported_wps_cm);
-
-	p2pielen += 2;
-
-	/*	Primary Device Type */
-	/*	Category ID */
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
-	p2pielen += 2;
-
-	/*	OUI */
-	*(u32*) (p2pie + p2pielen) = cpu_to_be32(WPSOUI);
-	p2pielen += 4;
-
-	/*	Sub Category ID */
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
-	p2pielen += 2;
-
-	/*	Number of Secondary Device Types */
-	p2pie[p2pielen++] = 0x00;	/*	No Secondary Device Type List */
-
-	/*	Device Name */
-	/*	Type: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
-	p2pielen += 2;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len);
-	p2pielen += 2;
-
-	/*	Value: */
-	memcpy(p2pie + p2pielen, pwdinfo->device_name,
-	       pwdinfo->device_name_len);
-	p2pielen += pwdinfo->device_name_len;
-
-	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
-	{
-		/*	Group ID Attribute */
-		/*	Type: */
-		p2pie[p2pielen++] = P2P_ATTR_GROUP_ID;
-
-		/*	Length: */
-		*(u16*) (p2pie + p2pielen) =
-			cpu_to_le16(ETH_ALEN + pwdinfo->nego_ssidlen);
-		p2pielen += 2;
-
-		/*	Value: */
-		/*	p2P Device Address */
-		memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN);
-		p2pielen += ETH_ALEN;
-
-		/*	SSID */
-		memcpy(p2pie + p2pielen, pwdinfo->nego_ssid,
-		       pwdinfo->nego_ssidlen);
-		p2pielen += pwdinfo->nego_ssidlen;
-
-	}
-
-	pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen,
-			       (unsigned char *) p2pie, &pattrib->pktlen);
-
-#ifdef CONFIG_8723AU_P2P
-	wfdielen = build_nego_resp_wfd_ie(pwdinfo, pframe);
-	pframe += wfdielen;
-	pattrib->pktlen += wfdielen;
-#endif /* CONFIG_8723AU_P2P */
-
-	pattrib->last_txcmdsz = pattrib->pktlen;
-
-	dump_mgntframe23a(padapter, pmgntframe);
-
-	return;
-}
-
-static void issue_p2p_GO_confirm(struct rtw_adapter *padapter, u8* raddr,
-				 u8 result)
-{
-
-	unsigned char category = WLAN_CATEGORY_PUBLIC;
-	u8 action = P2P_PUB_ACTION_ACTION;
-	u32 p2poui = cpu_to_be32(P2POUI);
-	u8 oui_subtype = P2P_GO_NEGO_CONF;
-	u8 p2pie[ 255 ] = { 0x00 };
-	u8 p2pielen = 0;
-	struct xmit_frame *pmgntframe;
-	struct pkt_attrib *pattrib;
-	unsigned char *pframe;
-	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
-	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-#ifdef CONFIG_8723AU_P2P
-	u32 wfdielen = 0;
-#endif /* CONFIG_8723AU_P2P */
-
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
-		return;
-
-	DBG_8723A("[%s] In\n", __func__);
-	/* update attribute */
-	pattrib = &pmgntframe->attrib;
-	update_mgntframe_attrib23a(padapter, pattrib);
-
-	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
-
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
-	pwlanhdr = (struct ieee80211_hdr *)pframe;
-
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
-
-	ether_addr_copy(pwlanhdr->addr1, raddr);
-	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
-	ether_addr_copy(pwlanhdr->addr3, myid(&padapter->eeprompriv));
-
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
-	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ACTION);
-
-	pframe += sizeof(struct ieee80211_hdr_3addr);
-	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
-
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *)&p2poui,
-				     &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1,
-				     &pwdinfo->negotiation_dialog_token,
-				  &pattrib->pktlen);
-	/*	P2P OUI */
-	p2pielen = 0;
-	p2pie[p2pielen++] = 0x50;
-	p2pie[p2pielen++] = 0x6F;
-	p2pie[p2pielen++] = 0x9A;
-	p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
-
-	/*	Commented by Albert 20110306 */
-	/*	According to the P2P Specification, the group negoitation
-		request frame should contain 5 P2P attributes */
-	/*	1. Status */
-	/*	2. P2P Capability */
-	/*	3. Operating Channel */
-	/*	4. Channel List */
-	/*	5. Group ID	(if this WiFi is GO) */
-
-	/*	P2P Status */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_STATUS;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0001);
-	p2pielen += 2;
-
-	/*	Value: */
-	p2pie[p2pielen++] = result;
-
-	/*	P2P Capability */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Device Capability Bitmap, 1 byte */
-	p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
-
-	/*	Group Capability Bitmap, 1 byte */
-	if (pwdinfo->persistent_supported) {
-		p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN |
-			P2P_GRPCAP_PERSISTENT_GROUP;
-	} else {
-		p2pie[p2pielen++] = P2P_GRPCAP_CROSS_CONN;
-	}
-
-	/*	Operating Channel */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0005);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Country String */
-	p2pie[p2pielen++] = 'X';
-	p2pie[p2pielen++] = 'X';
-
-	/*	The third byte should be set to 0x04. */
-	/*	Described in the "Operating Channel Attribute" section. */
-	p2pie[p2pielen++] = 0x04;
-
-	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) {
-		if (pwdinfo->peer_operating_ch <= 14) {
-			/*	Operating Class */
-			p2pie[p2pielen++] = 0x51;
-		} else if ((pwdinfo->peer_operating_ch >= 36) &&
-			 (pwdinfo->peer_operating_ch <= 48)) {
-			/*	Operating Class */
-			p2pie[p2pielen++] = 0x73;
-		} else {
-			/*	Operating Class */
-			p2pie[p2pielen++] = 0x7c;
-		}
-
-		p2pie[p2pielen++] = pwdinfo->peer_operating_ch;
-	} else {
-		if (pwdinfo->operating_channel <= 14) {
-			/*	Operating Class */
-			p2pie[p2pielen++] = 0x51;
-		}
-		else if ((pwdinfo->operating_channel >= 36) &&
-			 (pwdinfo->operating_channel <= 48)) {
-			/*	Operating Class */
-			p2pie[p2pielen++] = 0x73;
-		} else {
-			/*	Operating Class */
-			p2pie[p2pielen++] = 0x7c;
-		}
-
-		/*	Channel Number */
-		/*	Use the listen channel as the operating channel */
-		p2pie[p2pielen++] = pwdinfo->operating_channel;
-	}
-
-	/*	Channel List */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) =
-		cpu_to_le16(pwdinfo->channel_list_attr_len);
-	p2pielen += 2;
-
-	/*	Value: */
-	memcpy(p2pie + p2pielen, pwdinfo->channel_list_attr,
-	       pwdinfo->channel_list_attr_len);
-	p2pielen += pwdinfo->channel_list_attr_len;
-
-	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
-		/*	Group ID Attribute */
-		/*	Type: */
-		p2pie[p2pielen++] = P2P_ATTR_GROUP_ID;
-
-		/*	Length: */
-		*(u16*) (p2pie + p2pielen) =
-			cpu_to_le16(ETH_ALEN + pwdinfo->nego_ssidlen);
-		p2pielen += 2;
-
-		/*	Value: */
-		/*	p2P Device Address */
-		memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN);
-		p2pielen += ETH_ALEN;
-
-		/*	SSID */
-		memcpy(p2pie + p2pielen, pwdinfo->nego_ssid,
-		       pwdinfo->nego_ssidlen);
-		p2pielen += pwdinfo->nego_ssidlen;
-	}
-
-	pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen,
-			       (unsigned char *)p2pie, &pattrib->pktlen);
-
-#ifdef CONFIG_8723AU_P2P
-	wfdielen = build_nego_confirm_wfd_ie(pwdinfo, pframe);
-	pframe += wfdielen;
-	pattrib->pktlen += wfdielen;
-#endif /* CONFIG_8723AU_P2P */
-
-	pattrib->last_txcmdsz = pattrib->pktlen;
-
-	dump_mgntframe23a(padapter, pmgntframe);
-
-	return;
-}
-
-void issue_p2p_invitation_request23a(struct rtw_adapter *padapter, u8* raddr)
-{
-	unsigned char category = WLAN_CATEGORY_PUBLIC;
-	u8 action = P2P_PUB_ACTION_ACTION;
-	u32 p2poui = cpu_to_be32(P2POUI);
-	u8 oui_subtype = P2P_INVIT_REQ;
-	u8 p2pie[ 255 ] = { 0x00 };
-	u8 p2pielen = 0;
-	u8 dialogToken = 3;
-	u16 len_channellist_attr = 0;
-#ifdef CONFIG_8723AU_P2P
-	u32 wfdielen = 0;
-#endif /* CONFIG_8723AU_P2P */
-	int i, j;
-	struct xmit_frame *pmgntframe;
-	struct pkt_attrib *pattrib;
-	unsigned char *pframe;
-	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
-	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
-		return;
-
-	/* update attribute */
-	pattrib = &pmgntframe->attrib;
-	update_mgntframe_attrib23a(padapter, pattrib);
-
-	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
-
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
-	pwlanhdr = (struct ieee80211_hdr *)pframe;
-
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
-
-	ether_addr_copy(pwlanhdr->addr1, raddr);
-	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
-	ether_addr_copy(pwlanhdr->addr3, raddr);
-
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
-	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ACTION);
-
-	pframe += sizeof(struct ieee80211_hdr_3addr);
-	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
-
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *) &p2poui,
-				     &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &dialogToken, &pattrib->pktlen);
-
-	/*	P2P IE Section. */
-
-	/*	P2P OUI */
-	p2pielen = 0;
-	p2pie[p2pielen++] = 0x50;
-	p2pie[p2pielen++] = 0x6F;
-	p2pie[p2pielen++] = 0x9A;
-	p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
-
-	/*	Commented by Albert 20101011 */
-	/*	According to the P2P Specification, the P2P Invitation
-		request frame should contain 7 P2P attributes */
-	/*	1. Configuration Timeout */
-	/*	2. Invitation Flags */
-	/*	3. Operating Channel	(Only GO) */
-	/*	4. P2P Group BSSID	(Should be included if I am the GO) */
-	/*	5. Channel List */
-	/*	6. P2P Group ID */
-	/*	7. P2P Device Info */
-
-	/*	Configuration Timeout */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	2 seconds needed to be the P2P GO */
-	p2pie[p2pielen++] = 200;
-	/*	2 seconds needed to be the P2P Client */
-	p2pie[p2pielen++] = 200;
-
-	/*	Invitation Flags */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_INVITATION_FLAGS;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0001);
-	p2pielen += 2;
-
-	/*	Value: */
-	p2pie[p2pielen++] = P2P_INVITATION_FLAGS_PERSISTENT;
-
-	/*	Operating Channel */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0005);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Country String */
-	p2pie[p2pielen++] = 'X';
-	p2pie[p2pielen++] = 'X';
-
-	/*	The third byte should be set to 0x04. */
-	/*	Described in the "Operating Channel Attribute" section. */
-	p2pie[p2pielen++] = 0x04;
-
-	/*	Operating Class */
-	if (pwdinfo->invitereq_info.operating_ch <= 14)
-		p2pie[p2pielen++] = 0x51;
-	else if ((pwdinfo->invitereq_info.operating_ch >= 36) &&
-		 (pwdinfo->invitereq_info.operating_ch <= 48))
-		p2pie[p2pielen++] = 0x73;
-	else
-		p2pie[p2pielen++] = 0x7c;
-
-	/*	Channel Number */
-	/*	operating channel number */
-	p2pie[p2pielen++] = pwdinfo->invitereq_info.operating_ch;
-
-	if (ether_addr_equal(myid(&padapter->eeprompriv),
-			     pwdinfo->invitereq_info.go_bssid)) {
-		/*	P2P Group BSSID */
-		/*	Type: */
-		p2pie[p2pielen++] = P2P_ATTR_GROUP_BSSID;
-
-		/*	Length: */
-		*(u16*) (p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
-		p2pielen += 2;
-
-		/*	Value: */
-		/*	P2P Device Address for GO */
-		memcpy(p2pie + p2pielen, pwdinfo->invitereq_info.go_bssid,
-		       ETH_ALEN);
-		p2pielen += ETH_ALEN;
-	}
-
-	/*	Channel List */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
-
-	/*	Length: */
-	/*  Country String(3) */
-	/*  + (Operating Class (1) + Number of Channels(1)) *
-	    Operation Classes (?) */
-	/*  + number of channels in all classes */
-	len_channellist_attr = 3 +
-		(1 + 1) * (u16)pmlmeext->channel_list.reg_classes +
-		get_reg_classes_full_count(pmlmeext->channel_list);
-
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Country String */
-	p2pie[p2pielen++] = 'X';
-	p2pie[p2pielen++] = 'X';
-
-	/*	The third byte should be set to 0x04. */
-	/*	Described in the "Operating Channel Attribute" section. */
-	p2pie[p2pielen++] = 0x04;
-
-	/*	Channel Entry List */
-	for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
-		/*	Operating Class */
-		p2pie[p2pielen++] =
-			pmlmeext->channel_list.reg_class[j].reg_class;
-
-		/*	Number of Channels */
-		p2pie[p2pielen++] =
-			pmlmeext->channel_list.reg_class[j].channels;
-
-		/*	Channel List */
-		for (i = 0;
-		     i < pmlmeext->channel_list.reg_class[j].channels; i++) {
-			p2pie[p2pielen++] =
-				pmlmeext->channel_list.reg_class[j].channel[i];
-		}
-	}
-
-	/*	P2P Group ID */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_GROUP_ID;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) =
-		cpu_to_le16(6 + pwdinfo->invitereq_info.ssidlen);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	P2P Device Address for GO */
-	memcpy(p2pie + p2pielen, pwdinfo->invitereq_info.go_bssid, ETH_ALEN);
-	p2pielen += ETH_ALEN;
-
-	/*	SSID */
-	memcpy(p2pie + p2pielen, pwdinfo->invitereq_info.go_ssid,
-	       pwdinfo->invitereq_info.ssidlen);
-	p2pielen += pwdinfo->invitereq_info.ssidlen;
-
-	/*	Device Info */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	21 -> P2P Device Address (6bytes) + Config Methods (2bytes) +
-		Primary Device Type (8bytes) */
-	/*	+ NumofSecondDevType (1byte) + WPS Device Name ID field
-		(2bytes) + WPS Device Name Len field (2bytes) */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	P2P Device Address */
-	memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv), ETH_ALEN);
-	p2pielen += ETH_ALEN;
-
-	/*	Config Method */
-	/*	This field should be big endian. Noted by P2P specification. */
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_DISPLAY);
-	p2pielen += 2;
-
-	/*	Primary Device Type */
-	/*	Category ID */
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
-	p2pielen += 2;
-
-	/*	OUI */
-	*(u32*) (p2pie + p2pielen) = cpu_to_be32(WPSOUI);
-	p2pielen += 4;
-
-	/*	Sub Category ID */
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
-	p2pielen += 2;
-
-	/*	Number of Secondary Device Types */
-	p2pie[p2pielen++] = 0x00;	/*	No Secondary Device Type List */
-
-	/*	Device Name */
-	/*	Type: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
-	p2pielen += 2;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len);
-	p2pielen += 2;
-
-	/*	Value: */
-	memcpy(p2pie + p2pielen, pwdinfo->device_name,
-	       pwdinfo->device_name_len);
-	p2pielen += pwdinfo->device_name_len;
-
-	pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen,
-			       (unsigned char *) p2pie, &pattrib->pktlen);
-
-#ifdef CONFIG_8723AU_P2P
-	wfdielen = build_invitation_req_wfd_ie(pwdinfo, pframe);
-	pframe += wfdielen;
-	pattrib->pktlen += wfdielen;
-#endif /* CONFIG_8723AU_P2P */
-
-	pattrib->last_txcmdsz = pattrib->pktlen;
-
-	dump_mgntframe23a(padapter, pmgntframe);
-
-	return;
-}
-
-void issue_p2p_invitation_response23a(struct rtw_adapter *padapter, u8 *raddr,
-				      u8 dialogToken, u8 status_code)
-{
-	unsigned char category = WLAN_CATEGORY_PUBLIC;
-	u8 action = P2P_PUB_ACTION_ACTION;
-	u32 p2poui = cpu_to_be32(P2POUI);
-	u8 oui_subtype = P2P_INVIT_RESP;
-	u8 p2pie[ 255 ] = { 0x00 };
-	u8 p2pielen = 0;
-	u16 len_channellist_attr = 0;
-#ifdef CONFIG_8723AU_P2P
-	u32 wfdielen = 0;
-#endif /* CONFIG_8723AU_P2P */
-	int i, j;
-
-	struct xmit_frame *pmgntframe;
-	struct pkt_attrib *pattrib;
-	unsigned char *pframe;
-	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
-	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
-		return;
-
-	/* update attribute */
-	pattrib = &pmgntframe->attrib;
-	update_mgntframe_attrib23a(padapter, pattrib);
-
-	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
-
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
-	pwlanhdr = (struct ieee80211_hdr *)pframe;
-
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
-
-	ether_addr_copy(pwlanhdr->addr1, raddr);
-	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
-	ether_addr_copy(pwlanhdr->addr3, raddr);
-
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
-	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ACTION);
-
-	pframe += sizeof(struct ieee80211_hdr_3addr);
-	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
-
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *)&p2poui,
-				     &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &dialogToken, &pattrib->pktlen);
-
-	/*	P2P IE Section. */
-
-	/*	P2P OUI */
-	p2pielen = 0;
-	p2pie[p2pielen++] = 0x50;
-	p2pie[p2pielen++] = 0x6F;
-	p2pie[p2pielen++] = 0x9A;
-	p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
-
-	/*	Commented by Albert 20101005 */
-	/*	According to the P2P Specification, the P2P Invitation
-		response frame should contain 5 P2P attributes */
-	/*	1. Status */
-	/*	2. Configuration Timeout */
-	/*	3. Operating Channel	(Only GO) */
-	/*	4. P2P Group BSSID	(Only GO) */
-	/*	5. Channel List */
-
-	/*	P2P Status */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_STATUS;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0001);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	When status code is P2P_STATUS_FAIL_INFO_UNAVAILABLE. */
-	/*	Sent the event receiving the P2P Invitation Req frame
-		to DMP UI. */
-	/*	DMP had to compare the MAC address to find out the profile. */
-	/*	So, the WiFi driver will send the
-		P2P_STATUS_FAIL_INFO_UNAVAILABLE to NB. */
-	/*	If the UI found the corresponding profile, the WiFi driver
-		sends the P2P Invitation Req */
-	/*	to NB to rebuild the persistent group. */
-	p2pie[p2pielen++] = status_code;
-
-	/*	Configuration Timeout */
-	/*	Type: */
-	p2pie[p2pielen++] = P2P_ATTR_CONF_TIMEOUT;
-
-	/*	Length: */
-	*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	2 seconds needed to be the P2P GO */
-	p2pie[p2pielen++] = 200;
-	/*	2 seconds needed to be the P2P Client */
-	p2pie[p2pielen++] = 200;
-
-	if (status_code == P2P_STATUS_SUCCESS) {
-		if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
-			/* The P2P Invitation request frame asks this
-			   Wi-Fi device to be the P2P GO */
-			/* In this case, the P2P Invitation response
-			   frame should carry the two more P2P attributes. */
-			/* First one is operating channel attribute. */
-			/* Second one is P2P Group BSSID attribute. */
-
-			/* Operating Channel */
-			/* Type: */
-			p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
-
-			/* Length: */
-			*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0005);
-			p2pielen += 2;
-
-			/* Value: */
-			/* Country String */
-			p2pie[p2pielen++] = 'X';
-			p2pie[p2pielen++] = 'X';
-
-			/* The third byte should be set to 0x04. */
-			/* Described in the "Operating Channel Attribute"
-			   section. */
-			p2pie[p2pielen++] = 0x04;
-
-			/* Operating Class */
-			/*	Copy from SD7 */
-			p2pie[p2pielen++] = 0x51;
-
-			/* Channel Number */
-			/*	operating channel number */
-			p2pie[p2pielen++] = pwdinfo->operating_channel;
-
-			/*	P2P Group BSSID */
-			/*	Type: */
-			p2pie[p2pielen++] = P2P_ATTR_GROUP_BSSID;
-
-			/*	Length: */
-			*(u16*) (p2pie + p2pielen) = cpu_to_le16(ETH_ALEN);
-			p2pielen += 2;
-
-			/*	Value: */
-			/*	P2P Device Address for GO */
-			memcpy(p2pie + p2pielen, myid(&padapter->eeprompriv),
-			       ETH_ALEN);
-			p2pielen += ETH_ALEN;
-		}
-
-		/*	Channel List */
-		/*	Type: */
-		p2pie[p2pielen++] = P2P_ATTR_CH_LIST;
-
-		/*	Length: */
-		/*  Country String(3) */
-		/*  + (Operating Class (1) + Number of Channels(1)) *
-		    Operation Classes (?) */
-		/*  + number of channels in all classes */
-		len_channellist_attr = 3 +
-			(1 + 1) * (u16)pmlmeext->channel_list.reg_classes +
-			get_reg_classes_full_count(pmlmeext->channel_list);
-
-		*(u16*) (p2pie + p2pielen) = cpu_to_le16(len_channellist_attr);
-		p2pielen += 2;
-
-		/*	Value: */
-		/*	Country String */
-		p2pie[p2pielen++] = 'X';
-		p2pie[p2pielen++] = 'X';
-
-		/* The third byte should be set to 0x04. */
-		/* Described in the "Operating Channel Attribute" section. */
-		p2pie[p2pielen++] = 0x04;
-
-		/*	Channel Entry List */
-		for (j = 0; j < pmlmeext->channel_list.reg_classes; j++) {
-			/*	Operating Class */
-			p2pie[p2pielen++] =
-				pmlmeext->channel_list.reg_class[j].reg_class;
-
-			/*	Number of Channels */
-			p2pie[p2pielen++] =
-				pmlmeext->channel_list.reg_class[j].channels;
-
-			/*	Channel List */
-			for (i = 0;
-			     i < pmlmeext->channel_list.reg_class[j].channels;
-			     i++) {
-				p2pie[p2pielen++] = pmlmeext->channel_list.reg_class[j].channel[i];
-			}
-		}
-	}
-
-	pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen,
-			       (unsigned char *)p2pie, &pattrib->pktlen);
-
-#ifdef CONFIG_8723AU_P2P
-	wfdielen = build_invitation_resp_wfd_ie(pwdinfo, pframe);
-	pframe += wfdielen;
-	pattrib->pktlen += wfdielen;
-#endif /* CONFIG_8723AU_P2P */
-
-	pattrib->last_txcmdsz = pattrib->pktlen;
-
-	dump_mgntframe23a(padapter, pmgntframe);
-
-	return;
-}
-
-void issue_p2p_provision_request23a(struct rtw_adapter *padapter, u8 *pssid,
-				    u8 ussidlen, u8 *pdev_raddr)
-{
-	unsigned char category = WLAN_CATEGORY_PUBLIC;
-	u8 action = P2P_PUB_ACTION_ACTION;
-	u8 dialogToken = 1;
-	u32 p2poui = cpu_to_be32(P2POUI);
-	u8 oui_subtype = P2P_PROVISION_DISC_REQ;
-	u8 wpsie[100] = { 0x00 };
-	u8 wpsielen = 0;
-	u32 p2pielen = 0;
-#ifdef CONFIG_8723AU_P2P
-	u32 wfdielen = 0;
-#endif /* CONFIG_8723AU_P2P */
-	struct xmit_frame *pmgntframe;
-	struct pkt_attrib *pattrib;
-	unsigned char *pframe;
-	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
-	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
-		return;
-
-	DBG_8723A("[%s] In\n", __func__);
-	/* update attribute */
-	pattrib = &pmgntframe->attrib;
-	update_mgntframe_attrib23a(padapter, pattrib);
-
-	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
-
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
-	pwlanhdr = (struct ieee80211_hdr *)pframe;
-
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
-
-	ether_addr_copy(pwlanhdr->addr1, pdev_raddr);
-	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
-	ether_addr_copy(pwlanhdr->addr3, pdev_raddr);
-
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
-	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ACTION);
-
-	pframe += sizeof(struct ieee80211_hdr_3addr);
-	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
-
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *)&p2poui,
-				     &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &dialogToken, &pattrib->pktlen);
-
-	p2pielen = build_prov_disc_request_p2p_ie23a(pwdinfo, pframe, pssid,
-						     ussidlen, pdev_raddr);
-
-	pframe += p2pielen;
-	pattrib->pktlen += p2pielen;
-
-	wpsielen = 0;
-	/*	WPS OUI */
-	*(u32*) (wpsie) = cpu_to_be32(WPSOUI);
-	wpsielen += 4;
-
-	/*	WPS version */
-	/*	Type: */
-	*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
-	wpsielen += 2;
-
-	/*	Length: */
-	*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0001);
-	wpsielen += 2;
-
-	/*	Value: */
-	wpsie[wpsielen++] = WPS_VERSION_1;	/*	Version 1.0 */
-
-	/*	Config Method */
-	/*	Type: */
-	*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD);
-	wpsielen += 2;
-
-	/*	Length: */
-	*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0002);
-	wpsielen += 2;
-
-	/*	Value: */
-	*(u16*) (wpsie + wpsielen) = cpu_to_be16(pwdinfo->tx_prov_disc_info.wps_config_method_request);
-	wpsielen += 2;
-
-	pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, wpsielen,
-			       (unsigned char *) wpsie, &pattrib->pktlen);
-
-#ifdef CONFIG_8723AU_P2P
-	wfdielen = build_provdisc_req_wfd_ie(pwdinfo, pframe);
-	pframe += wfdielen;
-	pattrib->pktlen += wfdielen;
-#endif /* CONFIG_8723AU_P2P */
-
-	pattrib->last_txcmdsz = pattrib->pktlen;
-
-	dump_mgntframe23a(padapter, pmgntframe);
-
-	return;
-}
-
-static u8 is_matched_in_profilelist(u8 *peermacaddr,
-				    struct profile_info *profileinfo)
-{
-	u8 i, match_result = 0;
-
-	DBG_8723A("[%s] peermac = %.2X %.2X %.2X %.2X %.2X %.2X\n", __func__,
-		  peermacaddr[0], peermacaddr[1], peermacaddr[2],
-		  peermacaddr[3], peermacaddr[4], peermacaddr[5]);
-
-	for (i = 0; i < P2P_MAX_PERSISTENT_GROUP_NUM; i++, profileinfo++) {
-	       DBG_8723A("[%s] profileinfo_mac = %.2X %.2X %.2X %.2X %.2X "
-			 "%.2X\n", __func__, profileinfo->peermac[0],
-			 profileinfo->peermac[1], profileinfo->peermac[2],
-			 profileinfo->peermac[3], profileinfo->peermac[4],
-			 profileinfo->peermac[5]);
-		if (ether_addr_equal(peermacaddr, profileinfo->peermac)) {
-			match_result = 1;
-			DBG_8723A("[%s] Match!\n", __func__);
-			break;
-		}
-	}
-
-	return match_result;
-}
-
-void issue_probersp23a_p2p23a(struct rtw_adapter *padapter, unsigned char *da)
-{
-	struct xmit_frame *pmgntframe;
-	struct pkt_attrib *pattrib;
-	unsigned char *pframe;
-	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
-	unsigned char *mac;
-	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	u16 beacon_interval = 100;
-	u16 capInfo = 0;
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-	u8 wpsie[255] = { 0x00 };
-	u32 wpsielen = 0, p2pielen = 0;
-#ifdef CONFIG_8723AU_P2P
-	u32 wfdielen = 0;
-#endif /* CONFIG_8723AU_P2P */
-	struct cfg80211_wifidirect_info *pcfg80211_wdinfo =
-		&padapter->cfg80211_wdinfo;
-	struct ieee80211_channel *ieee_ch =
-		&pcfg80211_wdinfo->remain_on_ch_channel;
-	u8 listen_channel =
-		(u8)ieee80211_frequency_to_channel(ieee_ch->center_freq);
-
-	/* DBG_8723A("%s\n", __func__); */
-
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
-	{
-		return;
-	}
-
-	/* update attribute */
-	pattrib = &pmgntframe->attrib;
-	update_mgntframe_attrib23a(padapter, pattrib);
-
-	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
-
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
-	pwlanhdr = (struct ieee80211_hdr *)pframe;
-
-	mac = myid(&padapter->eeprompriv);
-
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
-	ether_addr_copy(pwlanhdr->addr1, da);
-	ether_addr_copy(pwlanhdr->addr2, mac);
-
-	/*	Use the device address for BSSID field. */
-	ether_addr_copy(pwlanhdr->addr3, mac);
-
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
-	pmlmeext->mgnt_seq++;
-	SetFrameSubType(fctrl, WIFI_PROBERSP);
-
-	pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr);
-	pattrib->pktlen = pattrib->hdrlen;
-	pframe += pattrib->hdrlen;
-
-	/* timestamp will be inserted by hardware */
-	pframe += 8;
-	pattrib->pktlen += 8;
-
-	/*  beacon interval: 2 bytes */
-	memcpy(pframe, (unsigned char *) &beacon_interval, 2);
-	pframe += 2;
-	pattrib->pktlen += 2;
-
-	/*	capability info: 2 bytes */
-	/*	ESS and IBSS bits must be 0 (defined in the 3.1.2.1.1 of
-		WiFi Direct Spec) */
-	capInfo |= cap_ShortPremble;
-	capInfo |= cap_ShortSlot;
-
-	memcpy(pframe, (unsigned char *) &capInfo, 2);
-	pframe += 2;
-	pattrib->pktlen += 2;
-
-	/*  SSID */
-	pframe = rtw_set_ie23a(pframe, _SSID_IE_, 7, pwdinfo->p2p_wildcard_ssid,
-			       &pattrib->pktlen);
-
-	/*  supported rates... */
-	/*	Use the OFDM rate in the P2P probe response frame.
-		(6(B), 9(B), 12, 18, 24, 36, 48, 54) */
-	pframe = rtw_set_ie23a(pframe, _SUPPORTEDRATES_IE_, 8,
-			       pwdinfo->support_rate, &pattrib->pktlen);
-
-	/*  DS parameter set */
-	if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled &&
-	    listen_channel != 0) {
-		pframe = rtw_set_ie23a(pframe, _DSSET_IE_, 1, (unsigned char *)
-				       &listen_channel, &pattrib->pktlen);
-	} else {
-		pframe = rtw_set_ie23a(pframe, _DSSET_IE_, 1, (unsigned char *)
-				       &pwdinfo->listen_channel,
-				       &pattrib->pktlen);
-	}
-
-	if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled) {
-		if (pmlmepriv->wps_probe_resp_ie &&
-		    pmlmepriv->p2p_probe_resp_ie) {
-			/* WPS IE */
-			memcpy(pframe, pmlmepriv->wps_probe_resp_ie,
-			       pmlmepriv->wps_probe_resp_ie_len);
-			pattrib->pktlen += pmlmepriv->wps_probe_resp_ie_len;
-			pframe += pmlmepriv->wps_probe_resp_ie_len;
-
-			/* P2P IE */
-			memcpy(pframe, pmlmepriv->p2p_probe_resp_ie,
-			       pmlmepriv->p2p_probe_resp_ie_len);
-			pattrib->pktlen += pmlmepriv->p2p_probe_resp_ie_len;
-			pframe += pmlmepriv->p2p_probe_resp_ie_len;
-		}
-	} else {
-
-		/*	Todo: WPS IE */
-		/*	Noted by Albert 20100907 */
-		/*	According to the WPS specification, all the WPS
-			attribute is presented by Big Endian. */
-
-		wpsielen = 0;
-		/*	WPS OUI */
-		*(u32*) (wpsie) = cpu_to_be32(WPSOUI);
-		wpsielen += 4;
-
-		/*	WPS version */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0001);
-		wpsielen += 2;
-
-		/*	Value: */
-		wpsie[wpsielen++] = WPS_VERSION_1;	/*	Version 1.0 */
-
-		/*	WiFi Simple Config State */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) =
-			cpu_to_be16(WPS_ATTR_SIMPLE_CONF_STATE);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0001);
-		wpsielen += 2;
-
-		/*	Value: */
-		wpsie[wpsielen++] = WPS_WSC_STATE_NOT_CONFIG;
-
-		/*	Response Type */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_RESP_TYPE);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0001);
-		wpsielen += 2;
-
-		/*	Value: */
-		wpsie[wpsielen++] = WPS_RESPONSE_TYPE_8021X;
-
-		/*	UUID-E */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_UUID_E);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0010);
-		wpsielen += 2;
-
-		/*	Value: */
-		memcpy(wpsie + wpsielen, myid(&padapter->eeprompriv), ETH_ALEN);
-		wpsielen += 0x10;
-
-		/*	Manufacturer */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_MANUFACTURER);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0007);
-		wpsielen += 2;
-
-		/*	Value: */
-		memcpy(wpsie + wpsielen, "Realtek", 7);
-		wpsielen += 7;
-
-		/*	Model Name */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_MODEL_NAME);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0006);
-		wpsielen += 2;
-
-		/*	Value: */
-		memcpy(wpsie + wpsielen, "8192CU", 6);
-		wpsielen += 6;
-
-		/*	Model Number */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_MODEL_NUMBER);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0001);
-		wpsielen += 2;
-
-		/*	Value: */
-		wpsie[ wpsielen++ ] = 0x31;		/*	character 1 */
-
-		/*	Serial Number */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) =
-			cpu_to_be16(WPS_ATTR_SERIAL_NUMBER);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(ETH_ALEN);
-		wpsielen += 2;
-
-		/*	Value: */
-		memcpy(wpsie + wpsielen, "123456", ETH_ALEN);
-		wpsielen += ETH_ALEN;
-
-		/*	Primary Device Type */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) =
-			cpu_to_be16(WPS_ATTR_PRIMARY_DEV_TYPE);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0008);
-		wpsielen += 2;
-
-		/*	Value: */
-		/*	Category ID */
-		*(u16*) (wpsie + wpsielen) =
-			cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
-		wpsielen += 2;
-
-		/*	OUI */
-		*(u32*) (wpsie + wpsielen) = cpu_to_be32(WPSOUI);
-		wpsielen += 4;
-
-		/*	Sub Category ID */
-		*(u16*) (wpsie + wpsielen) =
-			cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
-		wpsielen += 2;
-
-		/*	Device Name */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) =
-			cpu_to_be16(pwdinfo->device_name_len);
-		wpsielen += 2;
-
-		/*	Value: */
-		if (pwdinfo->device_name_len) {
-			memcpy(wpsie + wpsielen, pwdinfo->device_name,
-			       pwdinfo->device_name_len);
-			wpsielen += pwdinfo->device_name_len;
-		}
-
-		/*	Config Method */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0002);
-		wpsielen += 2;
-
-		/*	Value: */
-		*(u16*) (wpsie + wpsielen) =
-			cpu_to_be16(pwdinfo->supported_wps_cm);
-		wpsielen += 2;
-
-		pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, wpsielen,
-				       (unsigned char *)wpsie,
-				       &pattrib->pktlen);
-
-		p2pielen = build_probe_resp_p2p_ie23a(pwdinfo, pframe);
-		pframe += p2pielen;
-		pattrib->pktlen += p2pielen;
-	}
-
-#ifdef CONFIG_8723AU_P2P
-	if (pwdinfo->wfd_info->wfd_enable) {
-		wfdielen = build_probe_resp_wfd_ie(pwdinfo, pframe, 0);
-		pframe += wfdielen;
-		pattrib->pktlen += wfdielen;
-	} else if (pmlmepriv->wfd_probe_resp_ie &&
-		 pmlmepriv->wfd_probe_resp_ie_len > 0) {
-		/* WFD IE */
-		memcpy(pframe, pmlmepriv->wfd_probe_resp_ie,
-		       pmlmepriv->wfd_probe_resp_ie_len);
-		pattrib->pktlen += pmlmepriv->wfd_probe_resp_ie_len;
-		pframe += pmlmepriv->wfd_probe_resp_ie_len;
-	}
-#endif /* CONFIG_8723AU_P2P */
-
-	pattrib->last_txcmdsz = pattrib->pktlen;
-
-	dump_mgntframe23a(padapter, pmgntframe);
-
-	return;
-}
-
-static int _issue23a_probereq_p2p(struct rtw_adapter *padapter, u8 *da,
-				  int wait_ack)
-{
-	int ret = _FAIL;
-	struct xmit_frame *pmgntframe;
-	struct pkt_attrib *pattrib;
-	unsigned char *pframe;
-	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
-	unsigned char *mac;
-	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	u8	bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
-	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
-	u8 wpsie[255] = {0x00}, p2pie[255] = {0x00};
-	u16 wpsielen = 0, p2pielen = 0;
-#ifdef CONFIG_8723AU_P2P
-	u32 wfdielen = 0;
-#endif /* CONFIG_8723AU_P2P */
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
-		goto exit;
-
-	/* update attribute */
-	pattrib = &pmgntframe->attrib;
-	update_mgntframe_attrib23a(padapter, pattrib);
-
-	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
-
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
-	pwlanhdr = (struct ieee80211_hdr *)pframe;
-
-	mac = myid(&padapter->eeprompriv);
-
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
-
-	if (da) {
-		ether_addr_copy(pwlanhdr->addr1, da);
-		ether_addr_copy(pwlanhdr->addr3, da);
-	} else {
-		if ((pwdinfo->p2p_info.scan_op_ch_only) ||
-		    (pwdinfo->rx_invitereq_info.scan_op_ch_only)) {
-			/*	This two flags will be set when this is
-				only the P2P client mode. */
-			ether_addr_copy(pwlanhdr->addr1,
-					pwdinfo->p2p_peer_interface_addr);
-			ether_addr_copy(pwlanhdr->addr3,
-					pwdinfo->p2p_peer_interface_addr);
-		} else {
-			/*	broadcast probe request frame */
-			ether_addr_copy(pwlanhdr->addr1, bc_addr);
-			ether_addr_copy(pwlanhdr->addr3, bc_addr);
-		}
-	}
-	ether_addr_copy(pwlanhdr->addr2, mac);
-
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
-	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_PROBEREQ);
-
-	pframe += sizeof (struct ieee80211_hdr_3addr);
-	pattrib->pktlen = sizeof (struct ieee80211_hdr_3addr);
-
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ)) {
-		pframe = rtw_set_ie23a(pframe, _SSID_IE_,
-				    pwdinfo->tx_prov_disc_info.ssid.ssid_len,
-				    pwdinfo->tx_prov_disc_info.ssid.ssid,
-				    &pattrib->pktlen);
-	} else {
-		pframe = rtw_set_ie23a(pframe, _SSID_IE_,
-				       P2P_WILDCARD_SSID_LEN,
-				       pwdinfo->p2p_wildcard_ssid,
-				       &pattrib->pktlen);
-	}
-	/*	Use the OFDM rate in the P2P probe request frame.
-		(6(B), 9(B), 12(B), 24(B), 36, 48, 54) */
-	pframe = rtw_set_ie23a(pframe, _SUPPORTEDRATES_IE_, 8,
-			       pwdinfo->support_rate, &pattrib->pktlen);
-
-	if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled) {
-		if (pmlmepriv->wps_probe_req_ie &&
-		    pmlmepriv->p2p_probe_req_ie) {
-			/* WPS IE */
-			memcpy(pframe, pmlmepriv->wps_probe_req_ie,
-			       pmlmepriv->wps_probe_req_ie_len);
-			pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len;
-			pframe += pmlmepriv->wps_probe_req_ie_len;
-
-			/* P2P IE */
-			memcpy(pframe, pmlmepriv->p2p_probe_req_ie,
-			       pmlmepriv->p2p_probe_req_ie_len);
-			pattrib->pktlen += pmlmepriv->p2p_probe_req_ie_len;
-			pframe += pmlmepriv->p2p_probe_req_ie_len;
-		}
-	} else {
-
-		/*	WPS IE */
-		/*	Noted by Albert 20110221 */
-		/*	According to the WPS specification, all the WPS
-			attribute is presented by Big Endian. */
-
-		wpsielen = 0;
-		/*	WPS OUI */
-		*(u32*) (wpsie) = cpu_to_be32(WPSOUI);
-		wpsielen += 4;
-
-		/*	WPS version */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0001);
-		wpsielen += 2;
-
-		/*	Value: */
-		wpsie[wpsielen++] = WPS_VERSION_1;	/*	Version 1.0 */
-
-		if (pmlmepriv->wps_probe_req_ie == NULL) {
-			/*	UUID-E */
-			/*	Type: */
-			*(u16*) (wpsie + wpsielen) =
-				cpu_to_be16(WPS_ATTR_UUID_E);
-			wpsielen += 2;
-
-			/*	Length: */
-			*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0010);
-			wpsielen += 2;
-
-			/*	Value: */
-			memcpy(wpsie + wpsielen, myid(&padapter->eeprompriv),
-			       ETH_ALEN);
-			wpsielen += 0x10;
-
-			/*	Config Method */
-			/*	Type: */
-			*(u16*) (wpsie + wpsielen) =
-				cpu_to_be16(WPS_ATTR_CONF_METHOD);
-			wpsielen += 2;
-
-			/*	Length: */
-			*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0002);
-			wpsielen += 2;
-
-			/*	Value: */
-			*(u16*) (wpsie + wpsielen) =
-				cpu_to_be16(pwdinfo->supported_wps_cm);
-			wpsielen += 2;
-		}
-
-		/*	Device Name */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) =
-			cpu_to_be16(pwdinfo->device_name_len);
-		wpsielen += 2;
-
-		/*	Value: */
-		memcpy(wpsie + wpsielen, pwdinfo->device_name,
-		       pwdinfo->device_name_len);
-		wpsielen += pwdinfo->device_name_len;
-
-		/*	Primary Device Type */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) =
-			cpu_to_be16(WPS_ATTR_PRIMARY_DEV_TYPE);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0008);
-		wpsielen += 2;
-
-		/*	Value: */
-		/*	Category ID */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_PDT_CID_RTK_WIDI);
-		wpsielen += 2;
-
-		/*	OUI */
-		*(u32*) (wpsie + wpsielen) = cpu_to_be32(WPSOUI);
-		wpsielen += 4;
-
-		/*	Sub Category ID */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_PDT_SCID_RTK_DMP);
-		wpsielen += 2;
-
-		/*	Device Password ID */
-		/*	Type: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_DEVICE_PWID);
-		wpsielen += 2;
-
-		/*	Length: */
-		*(u16*) (wpsie + wpsielen) = cpu_to_be16(0x0002);
-		wpsielen += 2;
-
-		/*	Value: */
-		/*	Registrar-specified */
-		*(u16*) (wpsie + wpsielen) =
-			cpu_to_be16(WPS_DPID_REGISTRAR_SPEC);
-		wpsielen += 2;
-
-		pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, wpsielen,
-				       (unsigned char *)wpsie,
-				       &pattrib->pktlen);
-
-		/*	P2P OUI */
-		p2pielen = 0;
-		p2pie[p2pielen++] = 0x50;
-		p2pie[p2pielen++] = 0x6F;
-		p2pie[p2pielen++] = 0x9A;
-		p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
-
-		/*	Commented by Albert 20110221 */
-		/*	According to the P2P Specification, the probe request
-			frame should contain 5 P2P attributes */
-		/*	1. P2P Capability */
-		/*	2. P2P Device ID if this probe request wants to
-			find the specific P2P device */
-		/*	3. Listen Channel */
-		/*	4. Extended Listen Timing */
-		/*	5. Operating Channel if this WiFi is working as
-			the group owner now */
-
-		/*	P2P Capability */
-		/*	Type: */
-		p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
-
-		/*	Length: */
-		*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002);
-		p2pielen += 2;
-
-		/*	Value: */
-		/*	Device Capability Bitmap, 1 byte */
-		p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
-
-		/*	Group Capability Bitmap, 1 byte */
-		if (pwdinfo->persistent_supported)
-			p2pie[p2pielen++] = P2P_GRPCAP_PERSISTENT_GROUP |
-				DMP_P2P_GRPCAP_SUPPORT;
-		else
-			p2pie[p2pielen++] = DMP_P2P_GRPCAP_SUPPORT;
-
-		/*	Listen Channel */
-		/*	Type: */
-		p2pie[p2pielen++] = P2P_ATTR_LISTEN_CH;
-
-		/*	Length: */
-		*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0005);
-		p2pielen += 2;
-
-		/*	Value: */
-		/*	Country String */
-		p2pie[p2pielen++] = 'X';
-		p2pie[p2pielen++] = 'X';
-
-		/* The third byte should be set to 0x04. */
-		/* Described in the "Operating Channel Attribute" section. */
-		p2pie[p2pielen++] = 0x04;
-
-		/*	Operating Class */
-		p2pie[p2pielen++] = 0x51;	/*	Copy from SD7 */
-
-		/*	Channel Number */
-		/*	listen channel */
-		p2pie[p2pielen++] = pwdinfo->listen_channel;
-
-		/*	Extended Listen Timing */
-		/*	Type: */
-		p2pie[p2pielen++] = P2P_ATTR_EX_LISTEN_TIMING;
-
-		/*	Length: */
-		*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0004);
-		p2pielen += 2;
-
-		/*	Value: */
-		/*	Availability Period */
-		*(u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF);
-		p2pielen += 2;
-
-		/*	Availability Interval */
-		*(u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF);
-		p2pielen += 2;
-
-		if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
-			/* Operating Channel (if this WiFi is working as
-			   the group owner now) */
-			/* Type: */
-			p2pie[p2pielen++] = P2P_ATTR_OPERATING_CH;
-
-			/*	Length: */
-			*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0005);
-			p2pielen += 2;
-
-			/*	Value: */
-			/*	Country String */
-			p2pie[p2pielen++] = 'X';
-			p2pie[p2pielen++] = 'X';
-
-			/* The third byte should be set to 0x04. */
-			/* Described in the "Operating Channel Attribute"
-			   section. */
-			p2pie[p2pielen++] = 0x04;
-
-			/*	Operating Class */
-			p2pie[p2pielen++] = 0x51;	/*	Copy from SD7 */
-
-			/*	Channel Number */
-			/*	operating channel number */
-			p2pie[p2pielen++] = pwdinfo->operating_channel;
-		}
-
-		pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen,
-				       (unsigned char *)p2pie,
-				       &pattrib->pktlen);
-
-		if (pmlmepriv->wps_probe_req_ie) {
-			/* WPS IE */
-			memcpy(pframe, pmlmepriv->wps_probe_req_ie,
-			       pmlmepriv->wps_probe_req_ie_len);
-			pattrib->pktlen += pmlmepriv->wps_probe_req_ie_len;
-			pframe += pmlmepriv->wps_probe_req_ie_len;
-		}
-	}
-
-#ifdef CONFIG_8723AU_P2P
-	if (pwdinfo->wfd_info->wfd_enable) {
-		wfdielen = build_probe_req_wfd_ie(pwdinfo, pframe);
-		pframe += wfdielen;
-		pattrib->pktlen += wfdielen;
-	} else if (pmlmepriv->wfd_probe_req_ie &&
-		   pmlmepriv->wfd_probe_req_ie_len>0) {
-		/* WFD IE */
-		memcpy(pframe, pmlmepriv->wfd_probe_req_ie,
-		       pmlmepriv->wfd_probe_req_ie_len);
-		pattrib->pktlen += pmlmepriv->wfd_probe_req_ie_len;
-		pframe += pmlmepriv->wfd_probe_req_ie_len;
-	}
-#endif /* CONFIG_8723AU_P2P */
-
-	pattrib->last_txcmdsz = pattrib->pktlen;
-
-	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-		 ("issuing probe_req, tx_len =%d\n", pattrib->last_txcmdsz));
-
-	if (wait_ack) {
-		ret = dump_mgntframe23a_and_wait_ack23a(padapter, pmgntframe);
-	} else {
-		dump_mgntframe23a(padapter, pmgntframe);
-		ret = _SUCCESS;
-	}
-
-exit:
-	return ret;
-}
-
-inline void issue23a_probereq_p2p(struct rtw_adapter *adapter, u8 *da)
-{
-	_issue23a_probereq_p2p(adapter, da, false);
-}
-
-int issue23a_probereq_p2p_ex(struct rtw_adapter *adapter, u8 *da,
-			     int try_cnt, int wait_ms)
-{
-	int ret;
-	int i = 0;
-	unsigned long start = jiffies;
-
-	do {
-		ret = _issue23a_probereq_p2p(adapter, da,
-					     wait_ms > 0 ? true : false);
-
-		i++;
-
-		if (adapter->bDriverStopped || adapter->bSurpriseRemoved)
-			break;
-
-		if (i < try_cnt && wait_ms > 0 && ret == _FAIL)
-			msleep(wait_ms);
-
-	} while ((i < try_cnt) && ((ret == _FAIL) || (wait_ms == 0)));
-
-	if (ret != _FAIL) {
-		ret = _SUCCESS;
-		goto exit;
-	}
-
-	if (try_cnt && wait_ms) {
-		if (da)
-			DBG_8723A(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d "
-				  "in %u ms\n", FUNC_ADPT_ARG(adapter),
-				  MAC_ARG(da), rtw_get_oper_ch23a(adapter),
-				  ret == _SUCCESS?", acked":"", i, try_cnt,
-				  jiffies_to_msecs(jiffies - start));
-		else
-			DBG_8723A(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
-				  FUNC_ADPT_ARG(adapter),
-				  rtw_get_oper_ch23a(adapter),
-				  ret == _SUCCESS?", acked":"", i, try_cnt,
-				  jiffies_to_msecs(jiffies - start));
-	}
-exit:
-	return ret;
-}
-
-#endif /* CONFIG_8723AU_P2P */
-
-static s32 rtw_action_public_decache(struct recv_frame *recv_frame, s32 token)
+static int rtw_action_public_decache(struct recv_frame *recv_frame, s32 token)
 {
 	struct rtw_adapter *adapter = recv_frame->adapter;
 	struct mlme_ext_priv *mlmeext = &adapter->mlmeextpriv;
@@ -4504,17 +2105,17 @@ static s32 rtw_action_public_decache(struct recv_frame *recv_frame, s32 token)
 		if (token >= 0) {
 			if ((seq_ctrl == mlmeext->action_public_rxseq) &&
 			    (token == mlmeext->action_public_dialog_token)) {
-				DBG_8723A(FUNC_ADPT_FMT" seq_ctrl = 0x%x, "
-					  "rxseq = 0x%x, token:%d\n",
-					  FUNC_ADPT_ARG(adapter), seq_ctrl,
+				DBG_8723A("%s(%s): seq_ctrl = 0x%x, "
+					  "rxseq = 0x%x, token:%d\n", __func__,
+					  adapter->pnetdev->name, seq_ctrl,
 					  mlmeext->action_public_rxseq, token);
 				return _FAIL;
 			}
 		} else {
 			if (seq_ctrl == mlmeext->action_public_rxseq) {
-				DBG_8723A(FUNC_ADPT_FMT" seq_ctrl = 0x%x, "
-					  "rxseq = 0x%x\n",
-					  FUNC_ADPT_ARG(adapter), seq_ctrl,
+				DBG_8723A("%s(%s): seq_ctrl = 0x%x, "
+					  "rxseq = 0x%x\n", __func__,
+					  adapter->pnetdev->name, seq_ctrl,
 					  mlmeext->action_public_rxseq);
 				return _FAIL;
 			}
@@ -4529,21 +2130,12 @@ static s32 rtw_action_public_decache(struct recv_frame *recv_frame, s32 token)
 	return _SUCCESS;
 }
 
-static unsigned int on_action_public23a_p2p(struct recv_frame *precv_frame)
+static int on_action_public23a_p2p(struct recv_frame *precv_frame)
 {
 	struct sk_buff *skb = precv_frame->pkt;
 	u8 *pframe = skb->data;
 	u8 *frame_body;
 	u8 dialogToken = 0;
-#ifdef CONFIG_8723AU_P2P
-	struct rtw_adapter *padapter = precv_frame->adapter;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-	uint len = skb->len;
-	u8 *p2p_ie;
-	u32	p2p_ielen;
-	struct	wifidirect_info	*pwdinfo = &padapter->wdinfo;
-	u8	result = P2P_STATUS_SUCCESS;
-#endif /* CONFIG_8723AU_P2P */
 
 	frame_body = (unsigned char *)
 		(pframe + sizeof(struct ieee80211_hdr_3addr));
@@ -4553,349 +2145,10 @@ static unsigned int on_action_public23a_p2p(struct recv_frame *precv_frame)
 	if (rtw_action_public_decache(precv_frame, dialogToken) == _FAIL)
 		return _FAIL;
 
-#ifdef CONFIG_8723AU_P2P
-	del_timer_sync(&pwdinfo->reset_ch_sitesurvey);
-	if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled) {
-		rtw_cfg80211_rx_p2p_action_public(padapter, pframe, len);
-	} else {
-		/*	Do nothing if the driver doesn't enable the P2P function. */
-		if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE))
-			return _SUCCESS;
-
-		len -= sizeof(struct ieee80211_hdr_3addr);
-
-		switch (frame_body[ 6 ])/* OUI Subtype */
-		{
-			case P2P_GO_NEGO_REQ:
-				DBG_8723A("[%s] Got GO Nego Req Frame\n", __func__);
-				memset(&pwdinfo->groupid_info, 0x00, sizeof(struct group_id_info));
-
-				if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ))
-				{
-					rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
-				}
-
-				if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL))
-				{
-					/*	Commented by Albert 20110526 */
-					/*	In this case, this means the previous nego fail doesn't be reset yet. */
-					del_timer_sync(&pwdinfo->restore_p2p_state_timer);
-					/*	Restore the previous p2p state */
-					rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
-					DBG_8723A("[%s] Restore the previous p2p state to %d\n", __func__, rtw_p2p_state(pwdinfo));
-				}
-
-				/*	Commented by Kurt 20110902 */
-				/* Add if statement to avoid receiving duplicate prov disc req. such that pre_p2p_state would be covered. */
-				if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING))
-					rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
-
-				/*	Commented by Kurt 20120113 */
-				/*	Get peer_dev_addr here if peer doesn't issue prov_disc frame. */
-				if (is_zero_ether_addr(pwdinfo->rx_prov_disc_info.peerDevAddr))
-					ether_addr_copy(pwdinfo->rx_prov_disc_info.peerDevAddr, hdr->addr2);
-
-				result = process_p2p_group_negotation_req23a(pwdinfo, frame_body, len);
-				issue_p2p_GO_response(padapter, hdr->addr2,
-						      frame_body, len, result);
-
-				/*	Commented by Albert 20110718 */
-				/*	No matter negotiating or negotiation failure, the driver should set up the restore P2P state timer. */
-				mod_timer(&pwdinfo->restore_p2p_state_timer,
-					  jiffies + msecs_to_jiffies(5000));
-				break;
-
-			case P2P_GO_NEGO_RESP:
-				DBG_8723A("[%s] Got GO Nego Resp Frame\n", __func__);
-
-				if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING))
-				{
-					/*	Commented by Albert 20110425 */
-					/*	The restore timer is enabled when issuing the nego request frame of rtw_p2p_connect function. */
-					del_timer_sync(&pwdinfo->restore_p2p_state_timer);
-					pwdinfo->nego_req_info.benable = false;
-					result = process_p2p_group_negotation_resp23a(pwdinfo, frame_body, len);
-					issue_p2p_GO_confirm(pwdinfo->padapter,
-							     hdr->addr2,
-							     result);
-					if (result == P2P_STATUS_SUCCESS) {
-						if (rtw_p2p_role(pwdinfo) ==
-						    P2P_ROLE_CLIENT) {
-							pwdinfo->p2p_info.operation_ch[ 0 ] = pwdinfo->peer_operating_ch;
-							pwdinfo->p2p_info.scan_op_ch_only = 1;
-							mod_timer(&pwdinfo->reset_ch_sitesurvey2, jiffies + msecs_to_jiffies(P2P_RESET_SCAN_CH));
-						}
-					}
-
-					/*	Reset the dialog token for group negotiation frames. */
-					pwdinfo->negotiation_dialog_token = 1;
-
-					if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL))
-					{
-						mod_timer(&pwdinfo->restore_p2p_state_timer, jiffies + msecs_to_jiffies(5000));
-					}
-				} else {
-					DBG_8723A("[%s] Skipped GO Nego Resp Frame (p2p_state != P2P_STATE_GONEGO_ING)\n", __func__);
-				}
-
-				break;
-
-			case P2P_GO_NEGO_CONF:
-
-				DBG_8723A("[%s] Got GO Nego Confirm Frame\n", __func__);
-				result = process_p2p_group_negotation_confirm23a(pwdinfo, frame_body, len);
-				if (P2P_STATUS_SUCCESS == result)
-				{
-					if (rtw_p2p_role(pwdinfo) == P2P_ROLE_CLIENT)
-					{
-						pwdinfo->p2p_info.operation_ch[ 0 ] = pwdinfo->peer_operating_ch;
-						pwdinfo->p2p_info.scan_op_ch_only = 1;
-						mod_timer(&pwdinfo->reset_ch_sitesurvey2, jiffies + msecs_to_jiffies(P2P_RESET_SCAN_CH));
-					}
-				}
-				break;
-
-			case P2P_INVIT_REQ:
-				/*	Added by Albert 2010/10/05 */
-				/*	Received the P2P Invite Request frame. */
-
-				DBG_8723A("[%s] Got invite request frame!\n", __func__);
-				if ((p2p_ie = rtw_get_p2p_ie23a(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen)))
-				{
-					/*	Parse the necessary information from the P2P Invitation Request frame. */
-					/*	For example: The MAC address of sending this P2P Invitation Request frame. */
-					u32	attr_contentlen = 0;
-					u8	status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
-					struct group_id_info group_id;
-					u8	invitation_flag = 0;
-
-					rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_INVITATION_FLAGS, &invitation_flag, &attr_contentlen);
-					if (attr_contentlen)
-					{
-
-						rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_BSSID, pwdinfo->p2p_peer_interface_addr, &attr_contentlen);
-						/*	Commented by Albert 20120510 */
-						/*	Copy to the pwdinfo->p2p_peer_interface_addr. */
-						/*	So that the WFD UI (or Sigma) can get the peer interface address by using the following command. */
-						/*	#> iwpriv wlan0 p2p_get peer_ifa */
-						/*	After having the peer interface address, the sigma can find the correct conf file for wpa_supplicant. */
-
-						if (attr_contentlen)
-						{
-							DBG_8723A("[%s] GO's BSSID = %.2X %.2X %.2X %.2X %.2X %.2X\n", __func__,
-									pwdinfo->p2p_peer_interface_addr[0], pwdinfo->p2p_peer_interface_addr[1],
-									pwdinfo->p2p_peer_interface_addr[2], pwdinfo->p2p_peer_interface_addr[3],
-									pwdinfo->p2p_peer_interface_addr[4], pwdinfo->p2p_peer_interface_addr[5]);
-						}
-
-						if (invitation_flag & P2P_INVITATION_FLAGS_PERSISTENT)
-						{
-							/*	Re-invoke the persistent group. */
-
-							memset(&group_id, 0x00, sizeof(struct group_id_info));
-							rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, (u8*) &group_id, &attr_contentlen);
-							if (attr_contentlen) {
-								if (ether_addr_equal(group_id.go_device_addr, myid(&padapter->eeprompriv))) {
-									/*	The p2p device sending this p2p invitation request wants this Wi-Fi device to be the persistent GO. */
-									rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_GO);
-									rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
-									status_code = P2P_STATUS_SUCCESS;
-								}
-								else
-								{
-									/*	The p2p device sending this p2p invitation request wants to be the persistent GO. */
-									if (is_matched_in_profilelist(pwdinfo->p2p_peer_interface_addr, &pwdinfo->profileinfo[ 0 ]))
-									{
-										u8 operatingch_info[5] = { 0x00 };
-										if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen))
-										{
-											if (rtw_ch_set_search_ch23a(padapter->mlmeextpriv.channel_set, (u32)operatingch_info[4]))
-											{
-												/*	The operating channel is acceptable for this device. */
-												pwdinfo->rx_invitereq_info.operation_ch[0]= operatingch_info[4];
-												pwdinfo->rx_invitereq_info.scan_op_ch_only = 1;
-												mod_timer(&pwdinfo->reset_ch_sitesurvey, jiffies + msecs_to_jiffies(P2P_RESET_SCAN_CH));
-												rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_MATCH);
-												rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
-												status_code = P2P_STATUS_SUCCESS;
-												}
-											else
-											{
-												/*	The operating channel isn't supported by this device. */
-												rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH);
-												rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
-												status_code = P2P_STATUS_FAIL_NO_COMMON_CH;
-												mod_timer(&pwdinfo->restore_p2p_state_timer, jiffies + msecs_to_jiffies(3000));
-											}
-										}
-										else {
-											/*	Commented by Albert 20121130 */
-											/*	Intel will use the different P2P IE to store the operating channel information */
-											/*	Workaround for Intel WiDi 3.5 */
-											rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_MATCH);
-											rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
-											status_code = P2P_STATUS_SUCCESS;
-										}
-									}
-									else
-									{
-										rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH);
-
-										status_code = P2P_STATUS_FAIL_UNKNOWN_P2PGROUP;
-									}
-								}
-							}
-							else
-							{
-								DBG_8723A("[%s] P2P Group ID Attribute NOT FOUND!\n", __func__);
-								status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
-							}
-						}
-						else
-						{
-							/*	Received the invitation to join a P2P group. */
-
-							memset(&group_id, 0x00, sizeof(struct group_id_info));
-							rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, (u8*) &group_id, &attr_contentlen);
-							if (attr_contentlen)
-							{
-								if (ether_addr_equal(group_id.go_device_addr, myid(&padapter->eeprompriv))) {
-									/*	In this case, the GO can't be myself. */
-									rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_DISMATCH);
-									status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
-								}
-								else
-								{
-									/*	The p2p device sending this p2p invitation request wants to join an existing P2P group */
-									/*	Commented by Albert 2012/06/28 */
-									/*	In this case, this Wi-Fi device should use the iwpriv command to get the peer device address. */
-									/*	The peer device address should be the destination address for the provisioning discovery request. */
-									/*	Then, this Wi-Fi device should use the iwpriv command to get the peer interface address. */
-									/*	The peer interface address should be the address for WPS mac address */
-									ether_addr_copy(pwdinfo->p2p_peer_device_addr, group_id.go_device_addr);
-									rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
-									rtw_p2p_set_state(pwdinfo, P2P_STATE_RECV_INVITE_REQ_JOIN);
-									status_code = P2P_STATUS_SUCCESS;
-								}
-							}
-							else
-							{
-								DBG_8723A("[%s] P2P Group ID Attribute NOT FOUND!\n", __func__);
-								status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
-							}
-						}
-					}
-					else
-					{
-						DBG_8723A("[%s] P2P Invitation Flags Attribute NOT FOUND!\n", __func__);
-						status_code = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
-					}
-
-					DBG_8723A("[%s] status_code = %d\n", __func__, status_code);
-
-					pwdinfo->inviteresp_info.token = frame_body[ 7 ];
-					issue_p2p_invitation_response23a(padapter, hdr->addr2, pwdinfo->inviteresp_info.token, status_code);
-				}
-				break;
-
-			case P2P_INVIT_RESP:
-			{
-				u8	attr_content = 0x00;
-				u32	attr_contentlen = 0;
-
-				DBG_8723A("[%s] Got invite response frame!\n", __func__);
-				del_timer_sync(&pwdinfo->restore_p2p_state_timer);
-				if ((p2p_ie = rtw_get_p2p_ie23a(frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &p2p_ielen)))
-				{
-					rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
-
-					if (attr_contentlen == 1)
-					{
-						DBG_8723A("[%s] Status = %d\n", __func__, attr_content);
-						pwdinfo->invitereq_info.benable = false;
-
-						if (attr_content == P2P_STATUS_SUCCESS)
-						{
-							if (ether_addr_equal(pwdinfo->invitereq_info.go_bssid, myid(&padapter->eeprompriv))) {
-								rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
-							}
-							else
-							{
-								rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
-							}
-							rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_OK);
-						}
-						else
-						{
-							rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
-							rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL);
-						}
-					}
-					else
-					{
-						rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
-						rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL);
-					}
-				}
-				else
-				{
-					rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
-					rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL);
-				}
-
-				if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_INVITE_RESP_FAIL)) {
-					mod_timer(&pwdinfo->restore_p2p_state_timer, jiffies + msecs_to_jiffies(5000));
-				}
-				break;
-			}
-			case P2P_DEVDISC_REQ:
-
-				process_p2p_devdisc_req23a(pwdinfo, pframe, len);
-
-				break;
-
-			case P2P_DEVDISC_RESP:
-
-				process_p2p_devdisc_resp23a(pwdinfo, pframe, len);
-
-				break;
-
-			case P2P_PROVISION_DISC_REQ:
-				DBG_8723A("[%s] Got Provisioning Discovery Request Frame\n", __func__);
-				process_p2p_provdisc_req23a(pwdinfo, pframe, len);
-				ether_addr_copy(pwdinfo->rx_prov_disc_info.peerDevAddr, hdr->addr2);
-
-				/* 20110902 Kurt */
-				/* Add the following statement to avoid receiving duplicate prov disc req. such that pre_p2p_state would be covered. */
-				if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ))
-					rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
-
-				rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_REQ);
-				mod_timer(&pwdinfo->restore_p2p_state_timer,
-					  jiffies + msecs_to_jiffies(P2P_PROVISION_TIMEOUT));
-				break;
-
-			case P2P_PROVISION_DISC_RESP:
-				/*	Commented by Albert 20110707 */
-				/*	Should we check the pwdinfo->tx_prov_disc_info.bsent flag here?? */
-				DBG_8723A("[%s] Got Provisioning Discovery Response Frame\n", __func__);
-				/*	Commented by Albert 20110426 */
-				/*	The restore timer is enabled when issuing the provisioing request frame in rtw_p2p_prov_disc function. */
-				del_timer_sync(&pwdinfo->restore_p2p_state_timer);
-				rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_PROVISION_DIS_RSP);
-				process_p2p_provdisc_resp23a(pwdinfo, pframe);
-				mod_timer(&pwdinfo->restore_p2p_state_timer,
-					  jiffies + msecs_to_jiffies(P2P_PROVISION_TIMEOUT));
-				break;
-
-		}
-	}
-#endif /* CONFIG_8723AU_P2P */
-
 	return _SUCCESS;
 }
 
-static unsigned int on_action_public23a_vendor(struct recv_frame *precv_frame)
+static int on_action_public23a_vendor(struct recv_frame *precv_frame)
 {
 	unsigned int ret = _FAIL;
 	struct sk_buff *skb = precv_frame->pkt;
@@ -4937,10 +2190,10 @@ exit:
 	return ret;
 }
 
-unsigned int on_action_public23a(struct rtw_adapter *padapter,
-				 struct recv_frame *precv_frame)
+static int on_action_public23a(struct rtw_adapter *padapter,
+			       struct recv_frame *precv_frame)
 {
-	unsigned int ret = _FAIL;
+	int ret = _FAIL;
 	struct sk_buff *skb = precv_frame->pkt;
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
 	u8 *pframe = skb->data;
@@ -4969,92 +2222,34 @@ exit:
 	return ret;
 }
 
-unsigned int OnAction23a_ht(struct rtw_adapter *padapter,
-			    struct recv_frame *precv_frame)
+static int
+OnAction23a_ht(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
 	return _SUCCESS;
 }
 
-unsigned int OnAction23a_wmm(struct rtw_adapter *padapter,
-			     struct recv_frame *precv_frame)
+static int
+OnAction23a_wmm(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
 	return _SUCCESS;
 }
 
-unsigned int OnAction23a_p2p(struct rtw_adapter *padapter,
-			     struct recv_frame *precv_frame)
+static int
+OnAction23a_p2p(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
-#ifdef CONFIG_8723AU_P2P
-	u8 *frame_body;
-	u8 category, OUI_Subtype, dialogToken = 0;
-	struct sk_buff *skb = precv_frame->pkt;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-	u8 *pframe = skb->data;
-	uint len = skb->len;
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-
-	DBG_8723A("%s\n", __func__);
-
-	/* check RA matches or not */
-	if (!ether_addr_equal(myid(&padapter->eeprompriv), hdr->addr1))
-		return _SUCCESS;
-
-	frame_body = (unsigned char *)
-		(pframe + sizeof(struct ieee80211_hdr_3addr));
-
-	category = frame_body[0];
-	if (category != WLAN_CATEGORY_VENDOR_SPECIFIC)
-		return _SUCCESS;
-
-	if (cpu_to_be32(*((u32*) (frame_body + 1))) != P2POUI)
-		return _SUCCESS;
-
-	if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled) {
-		rtw_cfg80211_rx_action_p2p(padapter, pframe, len);
-		return _SUCCESS;
-	} else {
-		len -= sizeof(struct ieee80211_hdr_3addr);
-		OUI_Subtype = frame_body[5];
-		dialogToken = frame_body[6];
-
-		switch (OUI_Subtype)
-		{
-		case P2P_NOTICE_OF_ABSENCE:
-			break;
-
-		case P2P_PRESENCE_REQUEST:
-			process_p2p_presence_req23a(pwdinfo, pframe, len);
-			break;
-
-		case P2P_PRESENCE_RESPONSE:
-			break;
-
-		case P2P_GO_DISC_REQUEST:
-			break;
-
-		default:
-			break;
-		}
-	}
-#endif /* CONFIG_8723AU_P2P */
-
 	return _SUCCESS;
 }
 
-unsigned int OnAction23a(struct rtw_adapter *padapter,
-			 struct recv_frame *precv_frame)
+static int
+OnAction23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 {
 	int i;
-	unsigned char	category;
+	u8 category;
 	struct action_handler *ptable;
-	unsigned char	*frame_body;
 	struct sk_buff *skb = precv_frame->pkt;
-	u8 *pframe = skb->data;
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
 
-	frame_body = (unsigned char *)
-		(pframe + sizeof(struct ieee80211_hdr_3addr));
-
-	category = frame_body[0];
+	category = mgmt->u.action.category;
 
 	for (i = 0;
 	     i < sizeof(OnAction23a_tbl) / sizeof(struct action_handler); i++) {
@@ -5067,8 +2262,8 @@ unsigned int OnAction23a(struct rtw_adapter *padapter,
 	return _SUCCESS;
 }
 
-unsigned int DoReserved23a(struct rtw_adapter *padapter,
-			struct recv_frame *precv_frame)
+static int DoReserved23a(struct rtw_adapter *padapter,
+			 struct recv_frame *precv_frame)
 {
 	return _SUCCESS;
 }
@@ -5081,15 +2276,15 @@ struct xmit_frame *alloc_mgtxmitframe23a(struct xmit_priv *pxmitpriv)
 	pmgntframe = rtw_alloc_xmitframe23a_ext(pxmitpriv);
 
 	if (!pmgntframe) {
-		DBG_8723A(FUNC_ADPT_FMT" alloc xmitframe fail\n",
-			  FUNC_ADPT_ARG(pxmitpriv->adapter));
+		DBG_8723A("%s(%s): alloc xmitframe fail\n", __func__,
+			  pxmitpriv->adapter->pnetdev->name);
 		goto exit;
 	}
 
 	pxmitbuf = rtw_alloc_xmitbuf23a_ext(pxmitpriv);
 	if (!pxmitbuf) {
-		DBG_8723A(FUNC_ADPT_FMT" alloc xmitbuf fail\n",
-			  FUNC_ADPT_ARG(pxmitpriv->adapter));
+		DBG_8723A("%s(%s): alloc xmitbuf fail\n", __func__,
+			  pxmitpriv->adapter->pnetdev->name);
 		rtw_free_xmitframe23a(pxmitpriv, pmgntframe);
 		pmgntframe = NULL;
 		goto exit;
@@ -5138,7 +2333,7 @@ void update_mgntframe_attrib23a(struct rtw_adapter *padapter,
 	else
 		pattrib->raid = 5;/* a/g mode */
 
-	pattrib->encrypt = _NO_PRIVACY_;
+	pattrib->encrypt = 0;
 	pattrib->bswenc = false;
 
 	pattrib->qos_en = false;
@@ -5159,13 +2354,13 @@ void dump_mgntframe23a(struct rtw_adapter *padapter,
 	    padapter->bDriverStopped == true)
 		return;
 
-	rtw_hal_mgnt_xmit23a(padapter, pmgntframe);
+	rtl8723au_mgnt_xmit(padapter, pmgntframe);
 }
 
-s32 dump_mgntframe23a_and_wait(struct rtw_adapter *padapter,
+int dump_mgntframe23a_and_wait(struct rtw_adapter *padapter,
 			       struct xmit_frame *pmgntframe, int timeout_ms)
 {
-	s32 ret = _FAIL;
+	int ret = _FAIL;
 	unsigned long irqL;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct xmit_buf *pxmitbuf = pmgntframe->pxmitbuf;
@@ -5178,7 +2373,7 @@ s32 dump_mgntframe23a_and_wait(struct rtw_adapter *padapter,
 	rtw_sctx_init23a(&sctx, timeout_ms);
 	pxmitbuf->sctx = &sctx;
 
-	ret = rtw_hal_mgnt_xmit23a(padapter, pmgntframe);
+	ret = rtl8723au_mgnt_xmit(padapter, pmgntframe);
 
 	if (ret == _SUCCESS)
 		ret = rtw_sctx_wait23a(&sctx);
@@ -5190,24 +2385,23 @@ s32 dump_mgntframe23a_and_wait(struct rtw_adapter *padapter,
 	return ret;
 }
 
-s32 dump_mgntframe23a_and_wait_ack23a(struct rtw_adapter *padapter,
+int dump_mgntframe23a_and_wait_ack23a(struct rtw_adapter *padapter,
 				      struct xmit_frame *pmgntframe)
 {
-	s32 ret = _FAIL;
+	int ret = _FAIL;
 	u32 timeout_ms = 500;/*   500ms */
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 
 	if (padapter->bSurpriseRemoved == true ||
 	    padapter->bDriverStopped == true)
-		return -1;
+		return _FAIL;
 
 	mutex_lock(&pxmitpriv->ack_tx_mutex);
 	pxmitpriv->ack_tx = true;
 
 	pmgntframe->ack_report = 1;
-	if (rtw_hal_mgnt_xmit23a(padapter, pmgntframe) == _SUCCESS) {
+	if (rtl8723au_mgnt_xmit(padapter, pmgntframe) == _SUCCESS)
 		ret = rtw_ack_tx_wait23a(pxmitpriv, timeout_ms);
-	}
 
 	pxmitpriv->ack_tx = false;
 	mutex_unlock(&pxmitpriv->ack_tx_mutex);
@@ -5259,7 +2453,6 @@ void issue_beacon23a(struct rtw_adapter *padapter, int timeout_ms)
 	struct pkt_attrib *pattrib;
 	unsigned char *pframe;
 	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
 	unsigned int rate_len;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
@@ -5267,9 +2460,6 @@ void issue_beacon23a(struct rtw_adapter *padapter, int timeout_ms)
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *cur_network = &pmlmeinfo->network;
 	u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-#endif /* CONFIG_8723AU_P2P */
 	u8 *wps_ie;
 	u32 wps_ielen;
 	u8 sr = 0;
@@ -5277,7 +2467,8 @@ void issue_beacon23a(struct rtw_adapter *padapter, int timeout_ms)
 
 	/* DBG_8723A("%s\n", __func__); */
 
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) {
+	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
+	if (!pmgntframe) {
 		DBG_8723A("%s, alloc mgnt frame fail\n", __func__);
 		return;
 	}
@@ -5295,135 +2486,20 @@ void issue_beacon23a(struct rtw_adapter *padapter, int timeout_ms)
 	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
 
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
+	pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+					      IEEE80211_STYPE_BEACON);
+	pwlanhdr->seq_ctrl = 0;
 
 	ether_addr_copy(pwlanhdr->addr1, bc_addr);
 	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
 	ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(cur_network));
 
-	SetSeqNum(pwlanhdr, 0 /*pmlmeext->mgnt_seq*/);
-	/* pmlmeext->mgnt_seq++; */
-	SetFrameSubType(pframe, WIFI_BEACON);
-
 	pframe += sizeof(struct ieee80211_hdr_3addr);
 	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
 
 	if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) {
 		/* DBG_8723A("ie len =%d\n", cur_network->IELength); */
-#ifdef CONFIG_8723AU_P2P
-		/*  for P2P : Primary Device Type & Device Name */
-		u32 insert_len = 0;
-		wps_ie = rtw_get_wps_ie23a(cur_network->IEs + _FIXED_IE_LENGTH_,
-					   cur_network->IELength -
-					   _FIXED_IE_LENGTH_, NULL, &wps_ielen);
-
-		if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && wps_ie &&
-		    wps_ielen > 0) {
-			uint wps_offset, remainder_ielen;
-			u8 *premainder_ie, *pframe_wscie;
-
-			wps_offset = (uint)(wps_ie - cur_network->IEs);
-
-			premainder_ie = wps_ie + wps_ielen;
-
-			remainder_ielen = cur_network->IELength - wps_offset -
-				wps_ielen;
-
-			if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled) {
-				if (pmlmepriv->wps_beacon_ie &&
-				    pmlmepriv->wps_beacon_ie_len>0) {
-					memcpy(pframe, cur_network->IEs,
-					       wps_offset);
-					pframe += wps_offset;
-					pattrib->pktlen += wps_offset;
-
-					memcpy(pframe, pmlmepriv->wps_beacon_ie,
-					       pmlmepriv->wps_beacon_ie_len);
-					pframe += pmlmepriv->wps_beacon_ie_len;
-					pattrib->pktlen +=
-						pmlmepriv->wps_beacon_ie_len;
-
-					/* copy remainder_ie to pframe */
-					memcpy(pframe, premainder_ie,
-					       remainder_ielen);
-					pframe += remainder_ielen;
-					pattrib->pktlen += remainder_ielen;
-				} else {
-					memcpy(pframe, cur_network->IEs,
-					       cur_network->IELength);
-					pframe += cur_network->IELength;
-					pattrib->pktlen +=
-						cur_network->IELength;
-				}
-			} else {
-				pframe_wscie = pframe + wps_offset;
-				memcpy(pframe, cur_network->IEs,
-				       wps_offset + wps_ielen);
-				pframe += (wps_offset + wps_ielen);
-				pattrib->pktlen += (wps_offset + wps_ielen);
-
-				/* now pframe is end of wsc ie, insert Primary
-				   Device Type & Device Name */
-				/*	Primary Device Type */
-				/*	Type: */
-				*(u16*) (pframe + insert_len) =
-					cpu_to_be16(WPS_ATTR_PRIMARY_DEV_TYPE);
-				insert_len += 2;
-
-				/*	Length: */
-				*(u16*) (pframe + insert_len) =
-					cpu_to_be16(0x0008);
-				insert_len += 2;
-
-				/*	Value: */
-				/*	Category ID */
-				*(u16*) (pframe + insert_len) =
-					cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
-				insert_len += 2;
-
-				/*	OUI */
-				*(u32*) (pframe + insert_len) =
-					cpu_to_be32(WPSOUI);
-				insert_len += 4;
-
-				/*	Sub Category ID */
-				*(u16*) (pframe + insert_len) =
-					cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
-				insert_len += 2;
-
-				/*	Device Name */
-				/*	Type: */
-				*(u16*) (pframe + insert_len) =
-					cpu_to_be16(WPS_ATTR_DEVICE_NAME);
-				insert_len += 2;
-
-				/*	Length: */
-				*(u16*) (pframe + insert_len) =
-					cpu_to_be16(pwdinfo->device_name_len);
-				insert_len += 2;
-
-				/*	Value: */
-				memcpy(pframe + insert_len,
-				       pwdinfo->device_name,
-				       pwdinfo->device_name_len);
-				insert_len += pwdinfo->device_name_len;
-
-				/* update wsc ie length */
-				*(pframe_wscie+1) = (wps_ielen -2) + insert_len;
-
-				/* pframe move to end */
-				pframe+= insert_len;
-				pattrib->pktlen += insert_len;
-
-				/* copy remainder_ie to pframe */
-				memcpy(pframe, premainder_ie, remainder_ielen);
-				pframe += remainder_ielen;
-				pattrib->pktlen += remainder_ielen;
-			}
-		} else
-#endif /* CONFIG_8723AU_P2P */
-			memcpy(pframe, cur_network->IEs, cur_network->IELength);
+		memcpy(pframe, cur_network->IEs, cur_network->IELength);
 		len_diff = update_hidden_ssid(pframe + _BEACON_IE_OFFSET_,
 					      cur_network->IELength -
 					      _BEACON_IE_OFFSET_,
@@ -5447,36 +2523,6 @@ void issue_beacon23a(struct rtw_adapter *padapter, int timeout_ms)
 		else
 			_clr_fwstate_(pmlmepriv, WIFI_UNDER_WPS);
 
-#ifdef CONFIG_8723AU_P2P
-		if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
-			u32 len;
-			if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled) {
-				len = pmlmepriv->p2p_beacon_ie_len;
-				if (pmlmepriv->p2p_beacon_ie && len > 0)
-					memcpy(pframe,
-					       pmlmepriv->p2p_beacon_ie, len);
-			} else
-				len = build_beacon_p2p_ie23a(pwdinfo, pframe);
-
-			pframe += len;
-			pattrib->pktlen += len;
-
-			if (true == pwdinfo->wfd_info->wfd_enable) {
-				len = build_beacon_wfd_ie(pwdinfo, pframe);
-			} else {
-				len = 0;
-				if (pmlmepriv->wfd_beacon_ie &&
-				    pmlmepriv->wfd_beacon_ie_len>0) {
-					len = pmlmepriv->wfd_beacon_ie_len;
-					memcpy(pframe,
-					       pmlmepriv->wfd_beacon_ie, len);
-				}
-			}
-			pframe += len;
-			pattrib->pktlen += len;
-		}
-#endif /* CONFIG_8723AU_P2P */
-
 		goto _issue_bcn;
 	}
 
@@ -5503,39 +2549,39 @@ void issue_beacon23a(struct rtw_adapter *padapter, int timeout_ms)
 	pattrib->pktlen += 2;
 
 	/*  SSID */
-	pframe = rtw_set_ie23a(pframe, _SSID_IE_, cur_network->Ssid.ssid_len,
+	pframe = rtw_set_ie23a(pframe, WLAN_EID_SSID,
+			       cur_network->Ssid.ssid_len,
 			       cur_network->Ssid.ssid, &pattrib->pktlen);
 
 	/*  supported rates... */
 	rate_len = rtw_get_rateset_len23a(cur_network->SupportedRates);
-	pframe = rtw_set_ie23a(pframe, _SUPPORTEDRATES_IE_,
+	pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES,
 			       ((rate_len > 8)? 8: rate_len),
 			       cur_network->SupportedRates, &pattrib->pktlen);
 
 	/*  DS parameter set */
-	pframe = rtw_set_ie23a(pframe, _DSSET_IE_, 1, (unsigned char *)
-			       &cur_network->Configuration.DSConfig,
-			       &pattrib->pktlen);
+	pframe = rtw_set_ie23a(pframe, WLAN_EID_DS_PARAMS, 1, (unsigned char *)
+			       &cur_network->DSConfig, &pattrib->pktlen);
 
 	/* if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) */
 	{
 		u8 erpinfo = 0;
 		u32 ATIMWindow;
 		/*  IBSS Parameter Set... */
-		/* ATIMWindow = cur->Configuration.ATIMWindow; */
+		/* ATIMWindow = cur->ATIMWindow; */
 		ATIMWindow = 0;
-		pframe = rtw_set_ie23a(pframe, _IBSS_PARA_IE_, 2,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_IBSS_PARAMS, 2,
 				       (unsigned char *)&ATIMWindow,
 				       &pattrib->pktlen);
 
 		/* ERP IE */
-		pframe = rtw_set_ie23a(pframe, _ERPINFO_IE_, 1,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_ERP_INFO, 1,
 				       &erpinfo, &pattrib->pktlen);
 	}
 
 	/*  EXTERNDED SUPPORTED RATE */
 	if (rate_len > 8)
-		pframe = rtw_set_ie23a(pframe, _EXT_SUPPORTEDRATES_IE_,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_EXT_SUPP_RATES,
 				       rate_len - 8,
 				       cur_network->SupportedRates + 8,
 				       &pattrib->pktlen);
@@ -5564,14 +2610,13 @@ _issue_bcn:
 		dump_mgntframe23a(padapter, pmgntframe);
 }
 
-void issue_probersp23a(struct rtw_adapter *padapter, unsigned char *da,
-		       u8 is_valid_p2p_probereq)
+static void issue_probersp(struct rtw_adapter *padapter, unsigned char *da,
+			   u8 is_valid_p2p_probereq)
 {
 	struct xmit_frame *pmgntframe;
 	struct pkt_attrib *pattrib;
 	unsigned char *pframe;
 	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
 	unsigned char *mac, *bssid;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 #ifdef CONFIG_8723AU_AP_MODE
@@ -5582,22 +2627,17 @@ void issue_probersp23a(struct rtw_adapter *padapter, unsigned char *da,
 	int ssid_ielen_diff;
 	u8 buf[MAX_IE_SZ];
 	u8 *ies;
-#endif
-#if defined(CONFIG_8723AU_AP_MODE) || defined(CONFIG_8723AU_P2P)
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 #endif
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *cur_network = &pmlmeinfo->network;
 	unsigned int rate_len;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-#endif /* CONFIG_8723AU_P2P */
 
 	/* DBG_8723A("%s\n", __func__); */
 
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
-	{
+	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
+	if (!pmgntframe) {
 		DBG_8723A("%s, alloc mgnt frame fail\n", __func__);
 		return;
 	}
@@ -5608,21 +2648,22 @@ void issue_probersp23a(struct rtw_adapter *padapter, unsigned char *da,
 
 	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
 
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pframe = (u8 *)pmgntframe->buf_addr + TXDESC_OFFSET;
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
 
 	mac = myid(&padapter->eeprompriv);
 	bssid = cur_network->MacAddress;
 
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
+	pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+					      IEEE80211_STYPE_PROBE_RESP);
+
 	ether_addr_copy(pwlanhdr->addr1, da);
 	ether_addr_copy(pwlanhdr->addr2, mac);
 	ether_addr_copy(pwlanhdr->addr3, bssid);
 
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pwlanhdr->seq_ctrl =
+		cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
 	pmlmeext->mgnt_seq++;
-	SetFrameSubType(fctrl, WIFI_PROBERSP);
 
 	pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr);
 	pattrib->pktlen = pattrib->hdrlen;
@@ -5640,8 +2681,7 @@ void issue_probersp23a(struct rtw_adapter *padapter, unsigned char *da,
 					    &wps_ielen);
 
 		/* inerset & update wps_probe_resp_ie */
-		if ((pmlmepriv->wps_probe_resp_ie != NULL) && pwps_ie &&
-		    (wps_ielen > 0)) {
+		if (pmlmepriv->wps_probe_resp_ie && pwps_ie && wps_ielen > 0) {
 			uint wps_offset, remainder_ielen;
 			u8 *premainder_ie;
 
@@ -5658,14 +2698,14 @@ void issue_probersp23a(struct rtw_adapter *padapter, unsigned char *da,
 
 			/* to get ie data len */
 			wps_ielen = (uint)pmlmepriv->wps_probe_resp_ie[1];
-			if ((wps_offset+wps_ielen+2)<= MAX_IE_SZ) {
+			if (wps_offset + wps_ielen + 2 <= MAX_IE_SZ) {
 				memcpy(pframe, pmlmepriv->wps_probe_resp_ie,
 				       wps_ielen+2);
 				pframe += wps_ielen+2;
 				pattrib->pktlen += wps_ielen+2;
 			}
 
-			if ((wps_offset+wps_ielen+2+remainder_ielen) <=
+			if (wps_offset + wps_ielen + 2 + remainder_ielen <=
 			    MAX_IE_SZ) {
 				memcpy(pframe, premainder_ie, remainder_ielen);
 				pframe += remainder_ielen;
@@ -5681,9 +2721,9 @@ void issue_probersp23a(struct rtw_adapter *padapter, unsigned char *da,
 		ies = pmgntframe->buf_addr + TXDESC_OFFSET +
 			sizeof(struct ieee80211_hdr_3addr);
 
-		ssid_ie = rtw_get_ie23a(ies+_FIXED_IE_LENGTH_, _SSID_IE_,
+		ssid_ie = rtw_get_ie23a(ies + _FIXED_IE_LENGTH_, WLAN_EID_SSID,
 					&ssid_ielen,
-					(pframe-ies)-_FIXED_IE_LENGTH_);
+					pframe - ies - _FIXED_IE_LENGTH_);
 
 		ssid_ielen_diff = cur_network->Ssid.ssid_len - ssid_ielen;
 
@@ -5691,20 +2731,19 @@ void issue_probersp23a(struct rtw_adapter *padapter, unsigned char *da,
 			uint remainder_ielen;
 			u8 *remainder_ie;
 			remainder_ie = ssid_ie + 2;
-			remainder_ielen = (pframe-remainder_ie);
+			remainder_ielen = pframe - remainder_ie;
 
-			DBG_8723A_LEVEL(_drv_warning_, FUNC_ADPT_FMT
-					" remainder_ielen > MAX_IE_SZ\n",
-					FUNC_ADPT_ARG(padapter));
-			if (remainder_ielen > MAX_IE_SZ) {
+			DBG_8723A_LEVEL(_drv_warning_, "%s(%s): "
+					"remainder_ielen > MAX_IE_SZ\n",
+					__func__, padapter->pnetdev->name);
+			if (remainder_ielen > MAX_IE_SZ)
 				remainder_ielen = MAX_IE_SZ;
-			}
 
 			memcpy(buf, remainder_ie, remainder_ielen);
-			memcpy(remainder_ie+ssid_ielen_diff, buf,
+			memcpy(remainder_ie + ssid_ielen_diff, buf,
 			       remainder_ielen);
-			*(ssid_ie+1) = cur_network->Ssid.ssid_len;
-			memcpy(ssid_ie+2, cur_network->Ssid.ssid,
+			*(ssid_ie + 1) = cur_network->Ssid.ssid_len;
+			memcpy(ssid_ie + 2, cur_network->Ssid.ssid,
 			       cur_network->Ssid.ssid_len);
 
 			pframe += ssid_ielen_diff;
@@ -5737,40 +2776,41 @@ void issue_probersp23a(struct rtw_adapter *padapter, unsigned char *da,
 		/* below for ad-hoc mode */
 
 		/*  SSID */
-		pframe = rtw_set_ie23a(pframe, _SSID_IE_,
-				    cur_network->Ssid.ssid_len,
-				    cur_network->Ssid.ssid, &pattrib->pktlen);
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_SSID,
+				       cur_network->Ssid.ssid_len,
+				       cur_network->Ssid.ssid,
+				       &pattrib->pktlen);
 
 		/*  supported rates... */
 		rate_len = rtw_get_rateset_len23a(cur_network->SupportedRates);
-		pframe = rtw_set_ie23a(pframe, _SUPPORTEDRATES_IE_,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES,
 				       ((rate_len > 8)? 8: rate_len),
 				       cur_network->SupportedRates,
 				       &pattrib->pktlen);
 
 		/*  DS parameter set */
-		pframe = rtw_set_ie23a(pframe, _DSSET_IE_, 1, (unsigned char *)
-				       &cur_network->Configuration.DSConfig,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_DS_PARAMS, 1,
+				       (unsigned char *)&cur_network->DSConfig,
 				       &pattrib->pktlen);
 
-		if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) {
+		if ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) {
 			u8 erpinfo = 0;
 			u32 ATIMWindow;
 			/*  IBSS Parameter Set... */
-			/* ATIMWindow = cur->Configuration.ATIMWindow; */
+			/* ATIMWindow = cur->ATIMWindow; */
 			ATIMWindow = 0;
-			pframe = rtw_set_ie23a(pframe, _IBSS_PARA_IE_, 2,
+			pframe = rtw_set_ie23a(pframe, WLAN_EID_IBSS_PARAMS, 2,
 					       (unsigned char *)&ATIMWindow,
 					       &pattrib->pktlen);
 
 			/* ERP IE */
-			pframe = rtw_set_ie23a(pframe, _ERPINFO_IE_, 1,
+			pframe = rtw_set_ie23a(pframe, WLAN_EID_ERP_INFO, 1,
 					       &erpinfo, &pattrib->pktlen);
 		}
 
 		/*  EXTERNDED SUPPORTED RATE */
 		if (rate_len > 8)
-			pframe = rtw_set_ie23a(pframe, _EXT_SUPPORTEDRATES_IE_,
+			pframe = rtw_set_ie23a(pframe, WLAN_EID_EXT_SUPP_RATES,
 					       rate_len - 8,
 					       cur_network->SupportedRates + 8,
 					       &pattrib->pktlen);
@@ -5778,38 +2818,6 @@ void issue_probersp23a(struct rtw_adapter *padapter, unsigned char *da,
 		/* todo:HT for adhoc */
 	}
 
-#ifdef CONFIG_8723AU_P2P
-	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) && is_valid_p2p_probereq) {
-		u32 len;
-		if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled) {
-			/* if pwdinfo->role == P2P_ROLE_DEVICE will call
-			   issue_probersp23a_p2p23a() */
-			len = pmlmepriv->p2p_go_probe_resp_ie_len;
-			if (pmlmepriv->p2p_go_probe_resp_ie && len>0)
-				memcpy(pframe, pmlmepriv->p2p_go_probe_resp_ie,
-				       len);
-		} else
-			len = build_probe_resp_p2p_ie23a(pwdinfo, pframe);
-
-		pframe += len;
-		pattrib->pktlen += len;
-
-		if (true == pwdinfo->wfd_info->wfd_enable) {
-			len = build_probe_resp_wfd_ie(pwdinfo, pframe, 0);
-		} else {
-			len = 0;
-			if (pmlmepriv->wfd_probe_resp_ie &&
-			    pmlmepriv->wfd_probe_resp_ie_len > 0) {
-				len = pmlmepriv->wfd_probe_resp_ie_len;
-				memcpy(pframe, pmlmepriv->wfd_probe_resp_ie,
-				       len);
-			}
-		}
-		pframe += len;
-		pattrib->pktlen += len;
-	}
-#endif /* CONFIG_8723AU_P2P */
-
 	pattrib->last_txcmdsz = pattrib->pktlen;
 
 	dump_mgntframe23a(padapter, pmgntframe);
@@ -5817,15 +2825,14 @@ void issue_probersp23a(struct rtw_adapter *padapter, unsigned char *da,
 	return;
 }
 
-static int _issue_probereq23a(struct rtw_adapter *padapter,
-			      struct cfg80211_ssid *pssid, u8 *da, int wait_ack)
+static int _issue_probereq(struct rtw_adapter *padapter,
+			   struct cfg80211_ssid *pssid, u8 *da, int wait_ack)
 {
 	int ret = _FAIL;
 	struct xmit_frame		*pmgntframe;
 	struct pkt_attrib		*pattrib;
 	unsigned char			*pframe;
 	struct ieee80211_hdr	*pwlanhdr;
-	unsigned short		*fctrl;
 	unsigned char			*mac;
 	unsigned char			bssrate[NumRates];
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
@@ -5835,9 +2842,10 @@ static int _issue_probereq23a(struct rtw_adapter *padapter,
 	u8	bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
-		 ("+issue_probereq23a\n"));
+		 ("+%s\n", __func__));
 
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
+	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
+	if (!pmgntframe)
 		goto exit;
 
 	/* update attribute */
@@ -5851,8 +2859,8 @@ static int _issue_probereq23a(struct rtw_adapter *padapter,
 
 	mac = myid(&padapter->eeprompriv);
 
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
+	pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+					      IEEE80211_STYPE_PROBE_REQ);
 
 	if (da) {
 		/*	unicast probe request frame */
@@ -5866,30 +2874,31 @@ static int _issue_probereq23a(struct rtw_adapter *padapter,
 
 	ether_addr_copy(pwlanhdr->addr2, mac);
 
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pwlanhdr->seq_ctrl =
+		cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
+
 	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_PROBEREQ);
 
 	pframe += sizeof (struct ieee80211_hdr_3addr);
 	pattrib->pktlen = sizeof (struct ieee80211_hdr_3addr);
 
 	if (pssid)
-		pframe = rtw_set_ie23a(pframe, _SSID_IE_, pssid->ssid_len,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_SSID, pssid->ssid_len,
 				       pssid->ssid, &pattrib->pktlen);
 	else
-		pframe = rtw_set_ie23a(pframe, _SSID_IE_, 0, NULL,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_SSID, 0, NULL,
 				       &pattrib->pktlen);
 
 	get_rate_set23a(padapter, bssrate, &bssrate_len);
 
 	if (bssrate_len > 8) {
-		pframe = rtw_set_ie23a(pframe, _SUPPORTEDRATES_IE_, 8,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES, 8,
 				       bssrate, &pattrib->pktlen);
-		pframe = rtw_set_ie23a(pframe, _EXT_SUPPORTEDRATES_IE_,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_EXT_SUPP_RATES,
 				       (bssrate_len - 8), (bssrate + 8),
 				       &pattrib->pktlen);
 	} else {
-		pframe = rtw_set_ie23a(pframe, _SUPPORTEDRATES_IE_,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES,
 				       bssrate_len, bssrate, &pattrib->pktlen);
 	}
 
@@ -5917,23 +2926,23 @@ exit:
 	return ret;
 }
 
-inline void issue_probereq23a(struct rtw_adapter *padapter,
-			      struct cfg80211_ssid *pssid, u8 *da)
+static inline void issue_probereq(struct rtw_adapter *padapter,
+				  struct cfg80211_ssid *pssid, u8 *da)
 {
-	_issue_probereq23a(padapter, pssid, da, false);
+	_issue_probereq(padapter, pssid, da, false);
 }
 
-int issue_probereq23a_ex23a(struct rtw_adapter *padapter,
-		      struct cfg80211_ssid *pssid, u8 *da,
-		      int try_cnt, int wait_ms)
+static int issue_probereq_ex(struct rtw_adapter *padapter,
+			     struct cfg80211_ssid *pssid, u8 *da,
+			     int try_cnt, int wait_ms)
 {
 	int ret;
 	int i = 0;
 	unsigned long start = jiffies;
 
 	do {
-		ret = _issue_probereq23a(padapter, pssid, da,
-					 wait_ms > 0 ? true : false);
+		ret = _issue_probereq(padapter, pssid, da,
+				      wait_ms > 0 ? true : false);
 
 		i++;
 
@@ -5952,14 +2961,15 @@ int issue_probereq23a_ex23a(struct rtw_adapter *padapter,
 
 	if (try_cnt && wait_ms) {
 		if (da)
-			DBG_8723A(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d "
-				  "in %u ms\n",	FUNC_ADPT_ARG(padapter),
+			DBG_8723A("%s(%s): to "MAC_FMT", ch:%u%s, %d/%d "
+				  "in %u ms\n",	__func__,
+				  padapter->pnetdev->name,
 				  MAC_ARG(da), rtw_get_oper_ch23a(padapter),
 				  ret == _SUCCESS?", acked":"", i, try_cnt,
 				  jiffies_to_msecs(jiffies - start));
 		else
-			DBG_8723A(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
-				  FUNC_ADPT_ARG(padapter),
+			DBG_8723A("%s(%s):, ch:%u%s, %d/%d in %u ms\n",
+				  __func__, padapter->pnetdev->name,
 				  rtw_get_oper_ch23a(padapter),
 				  ret == _SUCCESS?", acked":"", i, try_cnt,
 				  jiffies_to_msecs(jiffies - start));
@@ -5969,14 +2979,13 @@ exit:
 }
 
 /*  if psta == NULL, indiate we are station(client) now... */
-void issue_auth23a(struct rtw_adapter *padapter, struct sta_info *psta,
-		   unsigned short status)
+static void issue_auth(struct rtw_adapter *padapter, struct sta_info *psta,
+		       unsigned short status)
 {
 	struct xmit_frame *pmgntframe;
 	struct pkt_attrib *pattrib;
 	unsigned char *pframe;
 	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
 	unsigned int val32;
 	unsigned short val16;
 	int use_shared_key = 0;
@@ -5984,7 +2993,8 @@ void issue_auth23a(struct rtw_adapter *padapter, struct sta_info *psta,
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
+	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
+	if (!pmgntframe)
 		return;
 
 	/* update attribute */
@@ -5996,12 +3006,11 @@ void issue_auth23a(struct rtw_adapter *padapter, struct sta_info *psta,
 	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
 
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
-
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+					      IEEE80211_STYPE_AUTH);
+	pwlanhdr->seq_ctrl =
+		cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
 	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_AUTH);
 
 	pframe += sizeof(struct ieee80211_hdr_3addr);
 	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
@@ -6045,7 +3054,7 @@ void issue_auth23a(struct rtw_adapter *padapter, struct sta_info *psta,
 		/*  added challenging text... */
 		if ((psta->auth_seq == 2) &&
 		    (psta->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1))
-			pframe = rtw_set_ie23a(pframe, _CHLGETXT_IE_, 128,
+			pframe = rtw_set_ie23a(pframe, WLAN_EID_CHALLENGE, 128,
 					       psta->chg_txt, &pattrib->pktlen);
 #endif
 	} else {
@@ -6079,7 +3088,7 @@ void issue_auth23a(struct rtw_adapter *padapter, struct sta_info *psta,
 						     (unsigned char *)&val32,
 						     &pattrib->pktlen);
 
-			pattrib->iv_len = 4;
+			pattrib->iv_len = IEEE80211_WEP_IV_LEN;
 		}
 
 		pframe = rtw_set_fixed_ie23a(pframe, _AUTH_ALGM_NUM_,
@@ -6104,17 +3113,18 @@ void issue_auth23a(struct rtw_adapter *padapter, struct sta_info *psta,
 		if ((pmlmeinfo->auth_seq == 3) &&
 		    (pmlmeinfo->state & WIFI_FW_AUTH_STATE) &&
 		    (use_shared_key == 1)) {
-			pframe = rtw_set_ie23a(pframe, _CHLGETXT_IE_, 128,
+			pframe = rtw_set_ie23a(pframe, WLAN_EID_CHALLENGE, 128,
 					       pmlmeinfo->chg_txt,
 					       &pattrib->pktlen);
 
-			SetPrivacy(fctrl);
+			pwlanhdr->frame_control |=
+				cpu_to_le16(IEEE80211_FCTL_PROTECTED);
 
 			pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr);
 
-			pattrib->encrypt = _WEP40_;
+			pattrib->encrypt = WLAN_CIPHER_SUITE_WEP40;
 
-			pattrib->icv_len = 4;
+			pattrib->icv_len = IEEE80211_WEP_ICV_LEN;
 
 			pattrib->pktlen += pattrib->icv_len;
 		}
@@ -6129,29 +3139,27 @@ void issue_auth23a(struct rtw_adapter *padapter, struct sta_info *psta,
 	return;
 }
 
-void issue_asocrsp23a(struct rtw_adapter *padapter, unsigned short status,
-		      struct sta_info *pstat, int pkt_type)
-{
 #ifdef CONFIG_8723AU_AP_MODE
+static void issue_assocrsp(struct rtw_adapter *padapter, unsigned short status,
+			   struct sta_info *pstat, u16 pkt_type)
+{
 	struct xmit_frame *pmgntframe;
 	struct ieee80211_hdr *pwlanhdr;
 	struct pkt_attrib *pattrib;
-	unsigned char *pbuf, *pframe;
+	unsigned char *pframe;
 	unsigned short val;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;
+	const u8 *p;
 	u8 *ie = pnetwork->IEs;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-	u32 wfdielen = 0;
-#endif /* CONFIG_8723AU_P2P */
 
 	DBG_8723A("%s\n", __func__);
 
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
+	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
+	if (!pmgntframe)
 		return;
 
 	/* update attribute */
@@ -6163,18 +3171,16 @@ void issue_asocrsp23a(struct rtw_adapter *padapter, unsigned short status,
 	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
 
-	pwlanhdr->frame_control = 0;
+	pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT | pkt_type);
 
 	ether_addr_copy(pwlanhdr->addr1, pstat->hwaddr);
 	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
 	ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(&pmlmeinfo->network));
 
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pwlanhdr->seq_ctrl =
+		cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
+
 	pmlmeext->mgnt_seq++;
-	if ((pkt_type == WIFI_ASSOCRSP) || (pkt_type == WIFI_REASSOCRSP))
-		SetFrameSubType(pwlanhdr, pkt_type);
-	else
-		return;
 
 	pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr);
 	pattrib->pktlen += pattrib->hdrlen;
@@ -6196,70 +3202,71 @@ void issue_asocrsp23a(struct rtw_adapter *padapter, unsigned short status,
 				     &pattrib->pktlen);
 
 	if (pstat->bssratelen <= 8) {
-		pframe = rtw_set_ie23a(pframe, _SUPPORTEDRATES_IE_,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES,
 				       pstat->bssratelen, pstat->bssrateset,
 				       &pattrib->pktlen);
 	} else {
-		pframe = rtw_set_ie23a(pframe, _SUPPORTEDRATES_IE_, 8,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES, 8,
 				       pstat->bssrateset, &pattrib->pktlen);
-		pframe = rtw_set_ie23a(pframe, _EXT_SUPPORTEDRATES_IE_,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_EXT_SUPP_RATES,
 				       pstat->bssratelen - 8,
 				       pstat->bssrateset + 8, &pattrib->pktlen);
 	}
 
-	if ((pstat->flags & WLAN_STA_HT) && (pmlmepriv->htpriv.ht_option)) {
-		uint ie_len = 0;
-
+	if (pstat->flags & WLAN_STA_HT && pmlmepriv->htpriv.ht_option) {
 		/* FILL HT CAP INFO IE */
 		/* p = hostapd_eid_ht_capabilities_info(hapd, p); */
-		pbuf = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_,
-				     _HT_CAPABILITY_IE_, &ie_len,
-				     pnetwork->IELength - _BEACON_IE_OFFSET_);
-		if (pbuf && ie_len>0) {
-			memcpy(pframe, pbuf, ie_len + 2);
-			pframe += (ie_len + 2);
-			pattrib->pktlen += (ie_len + 2);
+		p = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY,
+				     ie + _BEACON_IE_OFFSET_,
+				     pnetwork->IELength -_BEACON_IE_OFFSET_);
+		if (p && p[1]) {
+			memcpy(pframe, p, p[1] + 2);
+			pframe += (p[1] + 2);
+			pattrib->pktlen += (p[1] + 2);
 		}
 
 		/* FILL HT ADD INFO IE */
 		/* p = hostapd_eid_ht_operation(hapd, p); */
-		pbuf = rtw_get_ie23a(ie + _BEACON_IE_OFFSET_, _HT_ADD_INFO_IE_,
-				     &ie_len,
+		p = cfg80211_find_ie(WLAN_EID_HT_OPERATION,
+				     ie + _BEACON_IE_OFFSET_,
 				     pnetwork->IELength - _BEACON_IE_OFFSET_);
-		if (pbuf && ie_len > 0) {
-			memcpy(pframe, pbuf, ie_len + 2);
-			pframe += (ie_len + 2);
-			pattrib->pktlen += (ie_len + 2);
+		if (p && p[1] > 0) {
+			memcpy(pframe, p, p[1] + 2);
+			pframe += (p[1] + 2);
+			pattrib->pktlen += (p[1] + 2);
 		}
 	}
 
 	/* FILL WMM IE */
-	if ((pstat->flags & WLAN_STA_WME) && pmlmepriv->qospriv.qos_option) {
-		uint ie_len = 0;
+	if (pstat->flags & WLAN_STA_WME && pmlmepriv->qos_option) {
 		unsigned char WMM_PARA_IE[] = {0x00, 0x50, 0xf2, 0x02,
 					       0x01, 0x01};
-
-		for (pbuf = ie + _BEACON_IE_OFFSET_; ; pbuf += (ie_len + 2)) {
-			pbuf = rtw_get_ie23a(pbuf, _VENDOR_SPECIFIC_IE_,
-					     &ie_len, (pnetwork->IELength -
-						       _BEACON_IE_OFFSET_ -
-						       (ie_len + 2)));
-			if (pbuf && !memcmp(pbuf + 2, WMM_PARA_IE, 6)) {
-				memcpy(pframe, pbuf, ie_len + 2);
+		int ie_len = 0;
+
+		for (p = ie + _BEACON_IE_OFFSET_; ; p += (ie_len + 2)) {
+			p = cfg80211_find_ie(WLAN_EID_VENDOR_SPECIFIC, p,
+					     pnetwork->IELength -
+					     _BEACON_IE_OFFSET_ - (ie_len + 2));
+			if (p)
+				ie_len = p[1];
+			else
+				ie_len = 0;
+			if (p && !memcmp(p + 2, WMM_PARA_IE, 6)) {
+				memcpy(pframe, p, ie_len + 2);
 				pframe += (ie_len + 2);
 				pattrib->pktlen += (ie_len + 2);
 
 				break;
 			}
 
-			if ((!pbuf) || (ie_len == 0))
+			if (!p || ie_len == 0)
 				break;
 		}
 	}
 
 	if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK) {
-		pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, 6,
-				       REALTEK_96B_IE23A, &pattrib->pktlen);
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_VENDOR_SPECIFIC, 6,
+				       REALTEK_96B_IE, &pattrib->pktlen);
 	}
 
 	/* add WPS IE ie for wps 2.0 */
@@ -6272,47 +3279,32 @@ void issue_asocrsp23a(struct rtw_adapter *padapter, unsigned short status,
 		pattrib->pktlen += pmlmepriv->wps_assoc_resp_ie_len;
 	}
 
-#ifdef CONFIG_8723AU_P2P
-	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO) &&
-	    pwdinfo->wfd_info->wfd_enable) {
-		wfdielen = build_assoc_resp_wfd_ie(pwdinfo, pframe);
-		pframe += wfdielen;
-		pattrib->pktlen += wfdielen;
-	}
-#endif /* CONFIG_8723AU_P2P */
-
 	pattrib->last_txcmdsz = pattrib->pktlen;
 
 	dump_mgntframe23a(padapter, pmgntframe);
-#endif
 }
+#endif
 
-void issue_assocreq23a(struct rtw_adapter *padapter)
+static void issue_assocreq(struct rtw_adapter *padapter)
 {
 	int ret = _FAIL;
 	struct xmit_frame *pmgntframe;
 	struct pkt_attrib *pattrib;
-	unsigned char *pframe, *p;
+	unsigned char *pframe;
+	const u8 *p;
 	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
-	unsigned short val16;
-	unsigned int i, j, ie_len, index = 0;
+	unsigned int i, j, index = 0;
 	unsigned char rf_type, bssrate[NumRates], sta_bssrate[NumRates];
-	struct ndis_802_11_var_ies *pIE;
 	struct registry_priv *pregpriv = &padapter->registrypriv;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
-	int bssrate_len = 0, sta_bssrate_len = 0;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-	u8 p2pie[255] = { 0x00 };
-	u16 p2pielen = 0;
-	u32 wfdielen = 0;
-#endif /* CONFIG_8723AU_P2P */
-
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
+	int bssrate_len = 0, sta_bssrate_len = 0, pie_len, bcn_fixed_size;
+	u8 *pie;
+
+	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
+	if (!pmgntframe)
 		goto exit;
 
 	/* update attribute */
@@ -6321,38 +3313,38 @@ void issue_assocreq23a(struct rtw_adapter *padapter)
 
 	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
 
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pframe = (u8 *)pmgntframe->buf_addr + TXDESC_OFFSET;
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
 
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
+	pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+					      IEEE80211_STYPE_ASSOC_REQ);
+
 	ether_addr_copy(pwlanhdr->addr1, get_my_bssid23a(&pmlmeinfo->network));
 	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
 	ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(&pmlmeinfo->network));
 
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pwlanhdr->seq_ctrl =
+		cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
 	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ASSOCREQ);
 
 	pframe += sizeof(struct ieee80211_hdr_3addr);
 	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
 
 	/* caps */
-	memcpy(pframe, rtw_get_capability23a_from_ie(pmlmeinfo->network.IEs),
-	       2);
+	memcpy(pframe,
+	       rtw_get_capability23a_from_ie(pmlmeinfo->network.IEs), 2);
 
 	pframe += 2;
 	pattrib->pktlen += 2;
 
 	/* listen interval */
 	/* todo: listen interval for power saving */
-	val16 = cpu_to_le16(3);
-	memcpy(pframe, (unsigned char *)&val16, 2);
+	put_unaligned_le16(3, pframe);
 	pframe += 2;
 	pattrib->pktlen += 2;
 
 	/* SSID */
-	pframe = rtw_set_ie23a(pframe, _SSID_IE_,
+	pframe = rtw_set_ie23a(pframe, WLAN_EID_SSID,
 			       pmlmeinfo->network.Ssid.ssid_len,
 			       pmlmeinfo->network.Ssid.ssid, &pattrib->pktlen);
 
@@ -6387,7 +3379,7 @@ void issue_assocreq23a(struct rtw_adapter *padapter)
 			     Handlink WSG-4000 AP */
 			if ((pmlmeinfo->network.SupportedRates[i] |
 			     IEEE80211_BASIC_RATE_MASK) ==
-			    (sta_bssrate[j]|IEEE80211_BASIC_RATE_MASK)) {
+			    (sta_bssrate[j] | IEEE80211_BASIC_RATE_MASK)) {
 				/* DBG_8723A("match i = %d, j =%d\n", i, j); */
 				break;
 			}
@@ -6414,33 +3406,33 @@ void issue_assocreq23a(struct rtw_adapter *padapter)
 	}
 
 	if (bssrate_len > 8) {
-		pframe = rtw_set_ie23a(pframe, _SUPPORTEDRATES_IE_, 8,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES, 8,
 				       bssrate, &pattrib->pktlen);
-		pframe = rtw_set_ie23a(pframe, _EXT_SUPPORTEDRATES_IE_,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_EXT_SUPP_RATES,
 				       (bssrate_len - 8), (bssrate + 8),
 				       &pattrib->pktlen);
 	} else
-		pframe = rtw_set_ie23a(pframe, _SUPPORTEDRATES_IE_,
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES,
 				       bssrate_len, bssrate, &pattrib->pktlen);
 
 	/* RSN */
-	p = rtw_get_ie23a((pmlmeinfo->network.IEs +
-			   sizeof(struct ndis_802_11_fixed_ies)), _RSN_IE_2_,
-			  &ie_len, (pmlmeinfo->network.IELength -
-				    sizeof(struct ndis_802_11_fixed_ies)));
+	bcn_fixed_size = offsetof(struct ieee80211_mgmt, u.beacon.variable) -
+		offsetof(struct ieee80211_mgmt, u.beacon);
+
+	pie = pmlmeinfo->network.IEs + bcn_fixed_size;
+	pie_len = pmlmeinfo->network.IELength - bcn_fixed_size;
+
+	p = cfg80211_find_ie(WLAN_EID_RSN, pie, pie_len);
 	if (p)
-		pframe = rtw_set_ie23a(pframe, _RSN_IE_2_, ie_len, (p + 2),
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_RSN, p[1], p + 2,
 				       &pattrib->pktlen);
 
 	/* HT caps */
 	if (padapter->mlmepriv.htpriv.ht_option == true) {
-		p = rtw_get_ie23a((pmlmeinfo->network.IEs +
-				   sizeof(struct ndis_802_11_fixed_ies)),
-				  _HT_CAPABILITY_IE_, &ie_len,
-				  (pmlmeinfo->network.IELength -
-				   sizeof(struct ndis_802_11_fixed_ies)));
-		if ((p != NULL) && (!(is_ap_in_tkip23a(padapter)))) {
-			memcpy(&pmlmeinfo->HT_caps, (p + 2),
+		p = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY, pie, pie_len);
+
+		if (p && !is_ap_in_tkip23a(padapter)) {
+			memcpy(&pmlmeinfo->HT_caps, p + 2,
 			       sizeof(struct HT_caps_element));
 
 			/* to disable 40M Hz support while gd_bw_40MHz_en = 0 */
@@ -6454,15 +3446,13 @@ void issue_assocreq23a(struct rtw_adapter *padapter)
 			pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |=
 				0x000c;
 
-			rtw23a_hal_get_hwreg(padapter, HW_VAR_RF_TYPE,
-					     (u8 *)(&rf_type));
+			rf_type = rtl8723a_get_rf_type(padapter);
 			/* switch (pregpriv->rf_config) */
-			switch (rf_type)
-			{
+			switch (rf_type) {
 			case RF_1T1R:
-
+				/* RX STBC One spatial stream */
 				if (pregpriv->rx_stbc)
-					pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= cpu_to_le16(0x0100);/* RX STBC One spatial stream */
+					pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= cpu_to_le16(0x0100);
 
 				memcpy(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_rate_1R23A, 16);
 				break;
@@ -6470,18 +3460,17 @@ void issue_assocreq23a(struct rtw_adapter *padapter)
 			case RF_2T2R:
 			case RF_1T2R:
 			default:
-
 				/* enable for 2.4/5 GHz */
-				if ((pregpriv->rx_stbc == 0x3) ||
-				    ((pmlmeext->cur_wireless_mode &
-				      WIRELESS_11_24N) &&
+				if (pregpriv->rx_stbc == 0x3 ||
+				    (pmlmeext->cur_wireless_mode &
+				     WIRELESS_11_24N &&
 				     /* enable for 2.4GHz */
-				     (pregpriv->rx_stbc == 0x1)) ||
-				    ((pmlmeext->cur_wireless_mode &
-				      WIRELESS_11_5N) &&
-				     (pregpriv->rx_stbc == 0x2)) ||
+				     pregpriv->rx_stbc == 0x1) ||
+				    (pmlmeext->cur_wireless_mode &
+				     WIRELESS_11_5N &&
+				     pregpriv->rx_stbc == 0x2) ||
 				    /* enable for 5GHz */
-				    (pregpriv->wifi_spec == 1)) {
+				    pregpriv->wifi_spec == 1) {
 					DBG_8723A("declare supporting RX "
 						  "STBC\n");
 					pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info |= cpu_to_le16(0x0200);/* RX STBC two spatial stream */
@@ -6492,44 +3481,40 @@ void issue_assocreq23a(struct rtw_adapter *padapter)
 			pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info =
 				cpu_to_le16(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info);
 
-#ifdef CONFIG_8723AU_BT_COEXIST
-			if (BT_1Ant(padapter) == true) {
+			if (rtl8723a_BT_coexist(padapter) &&
+			    rtl8723a_BT_using_antenna_1(padapter)) {
 				/*  set to 8K */
 				pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para &= (u8)~IEEE80211_HT_AMPDU_PARM_FACTOR;
 /*				pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para |= MAX_AMPDU_FACTOR_8K */
 			}
-#endif
 
-			pframe = rtw_set_ie23a(pframe, _HT_CAPABILITY_IE_,
-					       ie_len,
-					       (u8 *)&pmlmeinfo->HT_caps,
+			pframe = rtw_set_ie23a(pframe, WLAN_EID_HT_CAPABILITY,
+					       p[1], (u8 *)&pmlmeinfo->HT_caps,
 					       &pattrib->pktlen);
 		}
 	}
 
 	/* vendor specific IE, such as WPA, WMM, WPS */
-	for (i = sizeof(struct ndis_802_11_fixed_ies);
-	     i < pmlmeinfo->network.IELength;) {
-		pIE = (struct ndis_802_11_var_ies *)
-			(pmlmeinfo->network.IEs + i);
-
-		switch (pIE->ElementID)
-		{
-		case _VENDOR_SPECIFIC_IE_:
-			if (!memcmp(pIE->data, RTW_WPA_OUI23A, 4) ||
-			    !memcmp(pIE->data, WMM_OUI23A, 4) ||
-			    !memcmp(pIE->data, WPS_OUI23A, 4)) {
+	for (i = bcn_fixed_size;  i < pmlmeinfo->network.IELength;) {
+		p = pmlmeinfo->network.IEs + i;
+
+		switch (p[0]) {
+		case WLAN_EID_VENDOR_SPECIFIC:
+			if (!memcmp(p + 2, RTW_WPA_OUI23A_TYPE, 4) ||
+			    !memcmp(p + 2, WMM_OUI23A, 4) ||
+			    !memcmp(p + 2, WPS_OUI23A, 4)) {
+				u8 plen = p[1];
 				if (!padapter->registrypriv.wifi_spec) {
 					/* Commented by Kurt 20110629 */
 					/* In some older APs, WPS handshake */
 					/* would be fail if we append vender
 					   extensions informations to AP */
-					if (!memcmp(pIE->data, WPS_OUI23A, 4))
-						pIE->Length = 14;
+					if (!memcmp(p + 2, WPS_OUI23A, 4))
+						plen = 14;
 				}
 				pframe = rtw_set_ie23a(pframe,
-						       _VENDOR_SPECIFIC_IE_,
-						       pIE->Length, pIE->data,
+						       WLAN_EID_VENDOR_SPECIFIC,
+						       plen, p + 2,
 						       &pattrib->pktlen);
 			}
 			break;
@@ -6538,197 +3523,12 @@ void issue_assocreq23a(struct rtw_adapter *padapter)
 			break;
 		}
 
-		i += (pIE->Length + 2);
+		i += p[1] + 2;
 	}
 
 	if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_REALTEK)
-		pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, 6,
-				       REALTEK_96B_IE23A, &pattrib->pktlen);
-
-#ifdef CONFIG_8723AU_P2P
-
-	if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled) {
-		if (pmlmepriv->p2p_assoc_req_ie &&
-		    pmlmepriv->p2p_assoc_req_ie_len>0) {
-			memcpy(pframe, pmlmepriv->p2p_assoc_req_ie,
-			       pmlmepriv->p2p_assoc_req_ie_len);
-			pframe += pmlmepriv->p2p_assoc_req_ie_len;
-			pattrib->pktlen += pmlmepriv->p2p_assoc_req_ie_len;
-		}
-	} else {
-		if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) &&
-		    !rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE)) {
-			/*	Should add the P2P IE in the association
-				request frame. */
-			/*	P2P OUI */
-
-			p2pielen = 0;
-			p2pie[p2pielen++] = 0x50;
-			p2pie[p2pielen++] = 0x6F;
-			p2pie[p2pielen++] = 0x9A;
-			p2pie[p2pielen++] = 0x09;	/*	WFA P2P v1.0 */
-
-			/*	Commented by Albert 20101109 */
-			/*	According to the P2P Specification, the
-				association request frame should contain
-				3 P2P attributes */
-			/*	1. P2P Capability */
-			/*	2. Extended Listen Timing */
-			/*	3. Device Info */
-			/*	Commented by Albert 20110516 */
-			/*	4. P2P Interface */
-
-			/*	P2P Capability */
-			/*	Type: */
-			p2pie[p2pielen++] = P2P_ATTR_CAPABILITY;
-
-			/*	Length: */
-			*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002);
-			p2pielen += 2;
-
-			/*	Value: */
-			/*	Device Capability Bitmap, 1 byte */
-			p2pie[p2pielen++] = DMP_P2P_DEVCAP_SUPPORT;
-
-			/*	Group Capability Bitmap, 1 byte */
-			if (pwdinfo->persistent_supported)
-				p2pie[p2pielen++] =
-					P2P_GRPCAP_PERSISTENT_GROUP |
-					DMP_P2P_GRPCAP_SUPPORT;
-			else
-				p2pie[p2pielen++] = DMP_P2P_GRPCAP_SUPPORT;
-
-			/*	Extended Listen Timing */
-			/*	Type: */
-			p2pie[p2pielen++] = P2P_ATTR_EX_LISTEN_TIMING;
-
-			/*	Length: */
-			*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x0004);
-			p2pielen += 2;
-
-			/*	Value: */
-			/*	Availability Period */
-			*(u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF);
-			p2pielen += 2;
-
-			/*	Availability Interval */
-			*(u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF);
-			p2pielen += 2;
-
-			/*	Device Info */
-			/*	Type: */
-			p2pie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
-
-			/*	Length: */
-			/*	21 -> P2P Device Address (6bytes) + Config
-				Methods (2bytes) + Primary Device
-				Type (8bytes) */
-			/*	+ NumofSecondDevType (1byte) + WPS Device
-				Name ID field (2bytes) + WPS Device Name
-				Len field (2bytes) */
-			*(u16*) (p2pie + p2pielen) =
-				cpu_to_le16(21 + pwdinfo->device_name_len);
-			p2pielen += 2;
-
-			/*	Value: */
-			/*	P2P Device Address */
-			memcpy(p2pie + p2pielen,
-			       myid(&padapter->eeprompriv), ETH_ALEN);
-			p2pielen += ETH_ALEN;
-
-			/*	Config Method */
-			/*	This field should be big endian.
-				Noted by P2P specification. */
-			if ((pwdinfo->ui_got_wps_info ==
-			     P2P_GOT_WPSINFO_PEER_DISPLAY_PIN) ||
-			    (pwdinfo->ui_got_wps_info ==
-			     P2P_GOT_WPSINFO_SELF_DISPLAY_PIN))
-				*(u16*) (p2pie + p2pielen) =
-					cpu_to_be16(WPS_CONFIG_METHOD_DISPLAY);
-			else
-				*(u16*) (p2pie + p2pielen) =
-					cpu_to_be16(WPS_CONFIG_METHOD_PBC);
-
-			p2pielen += 2;
-
-			/*	Primary Device Type */
-			/*	Category ID */
-			*(u16*) (p2pie + p2pielen) =
-				cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA);
-			p2pielen += 2;
-
-			/*	OUI */
-			*(u32*) (p2pie + p2pielen) = cpu_to_be32(WPSOUI);
-			p2pielen += 4;
-
-			/*	Sub Category ID */
-			*(u16*) (p2pie + p2pielen) =
-				cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER);
-			p2pielen += 2;
-
-			/*	Number of Secondary Device Types */
-			/*	No Secondary Device Type List */
-			p2pie[p2pielen++] = 0x00;
-
-			/*	Device Name */
-			/*	Type: */
-			*(u16*) (p2pie + p2pielen) =
-				cpu_to_be16(WPS_ATTR_DEVICE_NAME);
-			p2pielen += 2;
-
-			/*	Length: */
-			*(u16*) (p2pie + p2pielen) =
-				cpu_to_be16(pwdinfo->device_name_len);
-			p2pielen += 2;
-
-			/*	Value: */
-			memcpy(p2pie + p2pielen, pwdinfo->device_name,
-			       pwdinfo->device_name_len);
-			p2pielen += pwdinfo->device_name_len;
-
-			/*	P2P Interface */
-			/*	Type: */
-			p2pie[p2pielen++] = P2P_ATTR_INTERFACE;
-
-			/*	Length: */
-			*(u16*) (p2pie + p2pielen) = cpu_to_le16(0x000D);
-			p2pielen += 2;
-
-			/*	Value: */
-			memcpy(p2pie + p2pielen, pwdinfo->device_addr,
-			       ETH_ALEN);	/* P2P Device Address */
-			p2pielen += ETH_ALEN;
-
-			/* P2P Interface Address Count */
-			p2pie[p2pielen++] = 1;
-
-			memcpy(p2pie + p2pielen, pwdinfo->device_addr,
-			       ETH_ALEN);	/* P2P Interface Address List */
-			p2pielen += ETH_ALEN;
-
-			pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_,
-					       p2pielen, (unsigned char *)p2pie,
-					       &pattrib->pktlen);
-
-			/* wfdielen = build_assoc_req_wfd_ie(pwdinfo, pframe);*/
-			/* pframe += wfdielen; */
-			/* pattrib->pktlen += wfdielen; */
-		}
-	}
-
-	if (true == pwdinfo->wfd_info->wfd_enable) {
-		wfdielen = build_assoc_req_wfd_ie(pwdinfo, pframe);
-		pframe += wfdielen;
-		pattrib->pktlen += wfdielen;
-	} else if (pmlmepriv->wfd_assoc_req_ie != NULL &&
-		   pmlmepriv->wfd_assoc_req_ie_len > 0) {
-		/* WFD IE */
-		memcpy(pframe, pmlmepriv->wfd_assoc_req_ie,
-		       pmlmepriv->wfd_assoc_req_ie_len);
-		pattrib->pktlen += pmlmepriv->wfd_assoc_req_ie_len;
-		pframe += pmlmepriv->wfd_assoc_req_ie_len;
-	}
-#endif /* CONFIG_8723AU_P2P */
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_VENDOR_SPECIFIC, 6,
+				       REALTEK_96B_IE, &pattrib->pktlen);
 
 	pattrib->last_txcmdsz = pattrib->pktlen;
 	dump_mgntframe23a(padapter, pmgntframe);
@@ -6741,8 +3541,7 @@ exit:
 		kfree(pmlmepriv->assoc_req);
 		pmlmepriv->assoc_req = kmalloc(pattrib->pktlen, GFP_ATOMIC);
 		if (pmlmepriv->assoc_req) {
-			memcpy(pmlmepriv->assoc_req, pwlanhdr,
-			       pattrib->pktlen);
+			memcpy(pmlmepriv->assoc_req, pwlanhdr, pattrib->pktlen);
 			pmlmepriv->assoc_req_len = pattrib->pktlen;
 		}
 	} else
@@ -6760,7 +3559,6 @@ static int _issue_nulldata23a(struct rtw_adapter *padapter, unsigned char *da,
 	struct pkt_attrib *pattrib;
 	unsigned char *pframe;
 	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
 	struct xmit_priv *pxmitpriv;
 	struct mlme_ext_priv *pmlmeext;
 	struct mlme_ext_info *pmlmeinfo;
@@ -6774,7 +3572,8 @@ static int _issue_nulldata23a(struct rtw_adapter *padapter, unsigned char *da,
 	pmlmeext = &padapter->mlmeextpriv;
 	pmlmeinfo = &pmlmeext->mlmext_info;
 
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
+	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
+	if (!pmgntframe)
 		goto exit;
 
 	/* update attribute */
@@ -6787,24 +3586,24 @@ static int _issue_nulldata23a(struct rtw_adapter *padapter, unsigned char *da,
 	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
 
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
+	pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_DATA |
+					      IEEE80211_STYPE_NULLFUNC);
 
 	if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
-		SetFrDs(fctrl);
+		pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_FROMDS);
 	else if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
-		SetToDs(fctrl);
+		pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_TODS);
 
 	if (power_mode)
-		SetPwrMgt(fctrl);
+		pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_PM);
 
 	ether_addr_copy(pwlanhdr->addr1, da);
 	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
 	ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(&pmlmeinfo->network));
 
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pwlanhdr->seq_ctrl =
+		cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
 	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_DATA_NULL);
 
 	pframe += sizeof(struct ieee80211_hdr_3addr);
 	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
@@ -6858,14 +3657,15 @@ int issue_nulldata23a(struct rtw_adapter *padapter, unsigned char *da,
 
 	if (try_cnt && wait_ms) {
 		if (da)
-			DBG_8723A(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d "
-				  "in %u ms\n", FUNC_ADPT_ARG(padapter),
+			DBG_8723A("%s(%s): to "MAC_FMT", ch:%u%s, %d/%d "
+				  "in %u ms\n", __func__,
+				  padapter->pnetdev->name,
 				  MAC_ARG(da), rtw_get_oper_ch23a(padapter),
 				  ret == _SUCCESS?", acked":"", i, try_cnt,
 				  jiffies_to_msecs(jiffies - start));
 		else
-			DBG_8723A(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
-				  FUNC_ADPT_ARG(padapter),
+			DBG_8723A("%s(%s):, ch:%u%s, %d/%d in %u ms\n",
+				  __func__, padapter->pnetdev->name,
 				  rtw_get_oper_ch23a(padapter),
 				  ret == _SUCCESS?", acked":"", i, try_cnt,
 				  jiffies_to_msecs(jiffies - start));
@@ -6882,15 +3682,15 @@ static int _issue_qos_nulldata23a(struct rtw_adapter *padapter,
 	struct xmit_frame *pmgntframe;
 	struct pkt_attrib *pattrib;
 	unsigned char *pframe;
-	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl, *qc;
+	struct ieee80211_qos_hdr *pwlanhdr;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
 	DBG_8723A("%s\n", __func__);
 
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
+	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
+	if (!pmgntframe)
 		goto exit;
 
 	/* update attribute */
@@ -6906,34 +3706,32 @@ static int _issue_qos_nulldata23a(struct rtw_adapter *padapter,
 	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
 
 	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
-	pwlanhdr = (struct ieee80211_hdr *)pframe;
+	pwlanhdr = (struct ieee80211_qos_hdr *)pframe;
 
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
+	pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_DATA |
+					      IEEE80211_STYPE_QOS_NULLFUNC);
 
 	if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
-		SetFrDs(fctrl);
+		pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_FROMDS);
 	else if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
-		SetToDs(fctrl);
+		pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_TODS);
 
 	if (pattrib->mdata)
-		SetMData(fctrl);
-
-	qc = (unsigned short *)(pframe + pattrib->hdrlen - 2);
+		pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_MOREDATA);
 
-	SetPriority(qc, tid);
-
-	SetEOSP(qc, pattrib->eosp);
-
-	SetAckpolicy(qc, pattrib->ack_policy);
+	pwlanhdr->qos_ctrl = cpu_to_le16(tid & IEEE80211_QOS_CTL_TID_MASK);
+	pwlanhdr->qos_ctrl |= cpu_to_le16((pattrib->ack_policy << 5) &
+					  IEEE80211_QOS_CTL_ACK_POLICY_MASK);
+	if (pattrib->eosp)
+		pwlanhdr->qos_ctrl |= cpu_to_le16(IEEE80211_QOS_CTL_EOSP);
 
 	ether_addr_copy(pwlanhdr->addr1, da);
 	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
 	ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(&pmlmeinfo->network));
 
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pwlanhdr->seq_ctrl =
+		cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
 	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_QOS_DATA_NULL);
 
 	pframe += sizeof(struct ieee80211_qos_hdr);
 	pattrib->pktlen = sizeof(struct ieee80211_qos_hdr);
@@ -6986,14 +3784,15 @@ int issue_qos_nulldata23a(struct rtw_adapter *padapter, unsigned char *da,
 
 	if (try_cnt && wait_ms) {
 		if (da)
-			DBG_8723A(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d "
-				  "in %u ms\n", FUNC_ADPT_ARG(padapter),
+			DBG_8723A("%s(%s): to "MAC_FMT", ch:%u%s, %d/%d "
+				  "in %u ms\n", __func__,
+				  padapter->pnetdev->name,
 				  MAC_ARG(da), rtw_get_oper_ch23a(padapter),
 				  ret == _SUCCESS?", acked":"", i, try_cnt,
 				  jiffies_to_msecs(jiffies - start));
 		else
-			DBG_8723A(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
-				  FUNC_ADPT_ARG(padapter),
+			DBG_8723A("%s(%s):, ch:%u%s, %d/%d in %u ms\n",
+				  __func__, padapter->pnetdev->name,
 				  rtw_get_oper_ch23a(padapter),
 				  ret == _SUCCESS?", acked":"", i, try_cnt,
 				  jiffies_to_msecs(jiffies - start));
@@ -7002,33 +3801,22 @@ exit:
 	return ret;
 }
 
-static int _issue_deauth23a(struct rtw_adapter *padapter, unsigned char *da,
-			    unsigned short reason, u8 wait_ack)
+static int _issue_deauth(struct rtw_adapter *padapter, unsigned char *da,
+			 unsigned short reason, u8 wait_ack)
 {
 	struct xmit_frame *pmgntframe;
 	struct pkt_attrib *pattrib;
 	unsigned char *pframe;
 	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	int ret = _FAIL;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-#endif /* CONFIG_8723AU_P2P */
 
 	/* DBG_8723A("%s to "MAC_FMT"\n", __func__, MAC_ARG(da)); */
 
-#ifdef CONFIG_8723AU_P2P
-	if (!(rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) &&
-	    (pwdinfo->rx_invitereq_info.scan_op_ch_only)) {
-		mod_timer(&pwdinfo->reset_ch_sitesurvey,
-			  jiffies + msecs_to_jiffies(10));
-	}
-#endif /* CONFIG_8723AU_P2P */
-
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
+	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
+	if (!pmgntframe)
 		goto exit;
 
 	/* update attribute */
@@ -7041,16 +3829,16 @@ static int _issue_deauth23a(struct rtw_adapter *padapter, unsigned char *da,
 	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
 
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
+	pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+					      IEEE80211_STYPE_DEAUTH);
 
 	ether_addr_copy(pwlanhdr->addr1, da);
 	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
 	ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(&pmlmeinfo->network));
 
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pwlanhdr->seq_ctrl =
+		cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
 	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_DEAUTH);
 
 	pframe += sizeof(struct ieee80211_hdr_3addr);
 	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
@@ -7077,19 +3865,19 @@ int issue_deauth23a(struct rtw_adapter *padapter, unsigned char *da,
 		    unsigned short reason)
 {
 	DBG_8723A("%s to "MAC_FMT"\n", __func__, MAC_ARG(da));
-	return _issue_deauth23a(padapter, da, reason, false);
+	return _issue_deauth(padapter, da, reason, false);
 }
 
-int issue_deauth23a_ex23a(struct rtw_adapter *padapter, u8 *da,
-			  unsigned short reason, int try_cnt, int wait_ms)
+static int issue_deauth_ex(struct rtw_adapter *padapter, u8 *da,
+			   unsigned short reason, int try_cnt, int wait_ms)
 {
 	int ret;
 	int i = 0;
 	unsigned long start = jiffies;
 
 	do {
-		ret = _issue_deauth23a(padapter, da, reason,
-				       wait_ms >0 ? true : false);
+		ret = _issue_deauth(padapter, da, reason,
+				    wait_ms >0 ? true : false);
 
 		i++;
 
@@ -7108,14 +3896,15 @@ int issue_deauth23a_ex23a(struct rtw_adapter *padapter, u8 *da,
 
 	if (try_cnt && wait_ms) {
 		if (da)
-			DBG_8723A(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d "
-				  "in %u ms\n", FUNC_ADPT_ARG(padapter),
+			DBG_8723A("%s(%s): to "MAC_FMT", ch:%u%s, %d/%d "
+				  "in %u ms\n", __func__,
+				  padapter->pnetdev->name,
 				  MAC_ARG(da), rtw_get_oper_ch23a(padapter),
 				  ret == _SUCCESS?", acked":"", i, try_cnt,
 				  jiffies_to_msecs(jiffies - start));
 		else
-			DBG_8723A(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
-				  FUNC_ADPT_ARG(padapter),
+			DBG_8723A("%s(%s):, ch:%u%s, %d/%d in %u ms\n",
+				  __func__, padapter->pnetdev->name,
 				  rtw_get_oper_ch23a(padapter),
 				  ret == _SUCCESS?", acked":"", i, try_cnt,
 				  jiffies_to_msecs(jiffies - start));
@@ -7131,16 +3920,15 @@ void issue_action_spct_ch_switch23a(struct rtw_adapter *padapter,
 	struct pkt_attrib *pattrib;
 	unsigned char *pframe;
 	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	u8 category, action;
 
-	DBG_8723A(FUNC_NDEV_FMT" ra ="MAC_FMT", ch:%u, offset:%u\n",
-		FUNC_NDEV_ARG(padapter->pnetdev), MAC_ARG(ra),
-		  new_ch, ch_offset);
+	DBG_8723A("%s(%s): ra ="MAC_FMT", ch:%u, offset:%u\n", __func__,
+		  padapter->pnetdev->name, MAC_ARG(ra), new_ch, ch_offset);
 
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
+	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
+	if (!pmgntframe)
 		return;
 
 	/* update attribute */
@@ -7152,16 +3940,16 @@ void issue_action_spct_ch_switch23a(struct rtw_adapter *padapter,
 	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
 
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
+	pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+					      IEEE80211_STYPE_ACTION);
 
 	ether_addr_copy(pwlanhdr->addr1, ra); /* RA */
 	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv)); /* TA */
 	ether_addr_copy(pwlanhdr->addr3, ra); /* DA = RA */
 
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pwlanhdr->seq_ctrl =
+		cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
 	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ACTION);
 
 	pframe += sizeof(struct ieee80211_hdr_3addr);
 	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
@@ -7183,7 +3971,8 @@ void issue_action_spct_ch_switch23a(struct rtw_adapter *padapter,
 	dump_mgntframe23a(padapter, pmgntframe);
 }
 
-void issue_action_BA23a(struct rtw_adapter *padapter, unsigned char *raddr,
+void issue_action_BA23a(struct rtw_adapter *padapter,
+			const unsigned char *raddr,
 			unsigned char action, unsigned short status)
 {
 	u8 category = WLAN_CATEGORY_BACK;
@@ -7197,21 +3986,19 @@ void issue_action_BA23a(struct rtw_adapter *padapter, unsigned char *raddr,
 	struct pkt_attrib *pattrib;
 	u8 *pframe;
 	struct ieee80211_hdr *pwlanhdr;
-	u16 *fctrl;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct sta_info *psta;
 	struct sta_priv *pstapriv = &padapter->stapriv;
 	struct registry_priv *pregpriv = &padapter->registrypriv;
-#ifdef CONFIG_8723AU_BT_COEXIST
 	u8 tendaAPMac[] = {0xC8, 0x3A, 0x35};
-#endif
 
 	DBG_8723A("%s, category =%d, action =%d, status =%d\n",
 		  __func__, category, action, status);
 
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
+	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
+	if (!pmgntframe)
 		return;
 
 	/* update attribute */
@@ -7223,17 +4010,17 @@ void issue_action_BA23a(struct rtw_adapter *padapter, unsigned char *raddr,
 	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
 
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
+	pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+					      IEEE80211_STYPE_ACTION);
 
 	/* memcpy(pwlanhdr->addr1, get_my_bssid23a(&pmlmeinfo->network), ETH_ALEN); */
 	ether_addr_copy(pwlanhdr->addr1, raddr);
 	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
 	ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(&pmlmeinfo->network));
 
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pwlanhdr->seq_ctrl =
+		cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
 	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ACTION);
 
 	pframe += sizeof(struct ieee80211_hdr_3addr);
 	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
@@ -7255,9 +4042,9 @@ void issue_action_BA23a(struct rtw_adapter *padapter, unsigned char *raddr,
 		pframe = rtw_set_fixed_ie23a(pframe, 1, &pmlmeinfo->dialogToken,
 					     &pattrib->pktlen);
 
-#ifdef CONFIG_8723AU_BT_COEXIST
-		if ((BT_1Ant(padapter) == true) &&
-		    ((pmlmeinfo->assoc_AP_vendor != broadcomAP) ||
+		if (rtl8723a_BT_coexist(padapter) &&
+		    rtl8723a_BT_using_antenna_1(padapter) &&
+		    (pmlmeinfo->assoc_AP_vendor != broadcomAP ||
 		     memcmp(raddr, tendaAPMac, 3))) {
 			/*  A-MSDU NOT Supported */
 			BA_para_set = 0;
@@ -7270,9 +4057,7 @@ void issue_action_BA23a(struct rtw_adapter *padapter, unsigned char *raddr,
 			/*  max buffer size is 8 MSDU */
 			BA_para_set |= (8 << 6) &
 				IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
-		} else
-#endif
-		{
+		} else {
 			/* immediate ack & 64 buffer size */
 			BA_para_set = (0x1002 | ((status & 0xf) << 2));
 		}
@@ -7309,8 +4094,8 @@ void issue_action_BA23a(struct rtw_adapter *padapter, unsigned char *raddr,
 		pframe = rtw_set_fixed_ie23a(pframe, 2,
 					     (unsigned char *)&status,
 					     &pattrib->pktlen);
-		rtw_hal_get_def_var23a(padapter, HW_VAR_MAX_RX_AMPDU_FACTOR,
-				       &max_rx_ampdu_factor);
+		GetHalDefVar8192CUsb(padapter, HW_VAR_MAX_RX_AMPDU_FACTOR,
+				     &max_rx_ampdu_factor);
 		if (max_rx_ampdu_factor == IEEE80211_HT_MAX_AMPDU_64K)
 			BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x1000); /* 64 buffer size */
 		else if (max_rx_ampdu_factor == IEEE80211_HT_MAX_AMPDU_32K)
@@ -7322,16 +4107,15 @@ void issue_action_BA23a(struct rtw_adapter *padapter, unsigned char *raddr,
 		else
 			BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x1000); /* 64 buffer size */
 
-#ifdef CONFIG_8723AU_BT_COEXIST
-		if ((BT_1Ant(padapter) == true) &&
-		    ((pmlmeinfo->assoc_AP_vendor != broadcomAP) ||
+		if (rtl8723a_BT_coexist(padapter) &&
+		    rtl8723a_BT_using_antenna_1(padapter) &&
+		    (pmlmeinfo->assoc_AP_vendor != broadcomAP ||
 		     memcmp(raddr, tendaAPMac, 3))) {
 			/*  max buffer size is 8 MSDU */
 			BA_para_set &= ~IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
 			BA_para_set |= (8 << 6) &
 				IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
 		}
-#endif
 
 		if (pregpriv->ampdu_amsdu == 0)/* disabled */
 			BA_para_set = cpu_to_le16(BA_para_set & ~BIT(0));
@@ -7373,24 +4157,25 @@ static void issue_action_BSSCoexistPacket(struct rtw_adapter *padapter)
 {
 	struct list_head *plist, *phead, *ptmp;
 	unsigned char category, action;
-	struct xmit_frame			*pmgntframe;
-	struct pkt_attrib			*pattrib;
-	unsigned char				*pframe;
-	struct ieee80211_hdr	*pwlanhdr;
-	unsigned short			*fctrl;
-	struct	wlan_network	*pnetwork = NULL;
+	struct xmit_frame *pmgntframe;
+	struct pkt_attrib *pattrib;
+	u8 *pframe;
+	struct ieee80211_hdr *pwlanhdr;
+	struct wlan_network *pnetwork;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
-	struct rtw_queue	*queue	= &pmlmepriv->scanned_queue;
+	struct rtw_queue *queue	= &pmlmepriv->scanned_queue;
 	u8 InfoContent[16] = {0};
 	u8 ICS[8][15];
+	int i;
 
-	if ((pmlmepriv->num_FortyMHzIntolerant == 0) || (pmlmepriv->num_sta_no_ht == 0))
+	if (pmlmepriv->num_FortyMHzIntolerant == 0 ||
+	    pmlmepriv->num_sta_no_ht == 0)
 		return;
 
-	if (true == pmlmeinfo->bwmode_updated)
+	if (pmlmeinfo->bwmode_updated)
 		return;
 
 	DBG_8723A("%s\n", __func__);
@@ -7398,10 +4183,9 @@ static void issue_action_BSSCoexistPacket(struct rtw_adapter *padapter)
 	category = WLAN_CATEGORY_PUBLIC;
 	action = ACT_PUBLIC_BSSCOEXIST;
 
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
-	{
+	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
+	if (!pmgntframe)
 		return;
-	}
 
 	/* update attribute */
 	pattrib = &pmgntframe->attrib;
@@ -7409,19 +4193,19 @@ static void issue_action_BSSCoexistPacket(struct rtw_adapter *padapter)
 
 	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
 
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
+	pframe = (u8 *)pmgntframe->buf_addr + TXDESC_OFFSET;
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
 
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
+	pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+					      IEEE80211_STYPE_ACTION);
 
 	ether_addr_copy(pwlanhdr->addr1, get_my_bssid23a(&pmlmeinfo->network));
 	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
 	ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(&pmlmeinfo->network));
 
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pwlanhdr->seq_ctrl =
+		cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
 	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ACTION);
 
 	pframe += sizeof(struct ieee80211_hdr_3addr);
 	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
@@ -7429,91 +4213,80 @@ static void issue_action_BSSCoexistPacket(struct rtw_adapter *padapter)
 	pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen);
 	pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen);
 
-	/*  */
-	if (pmlmepriv->num_FortyMHzIntolerant>0)
-	{
-		u8 iedata = 0;
-
-		iedata |= BIT(2);/* 20 MHz BSS Width Request */
-
-		pframe = rtw_set_ie23a(pframe, EID_BSSCoexistence,  1, &iedata, &pattrib->pktlen);
+	if (pmlmepriv->num_FortyMHzIntolerant > 0) {
+		u8 iedata = BIT(2);/* 20 MHz BSS Width Request */
 
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_BSS_COEX_2040, 1,
+				       &iedata, &pattrib->pktlen);
 	}
 
-	/*  */
-	memset(ICS, 0, sizeof(ICS));
-	if (pmlmepriv->num_sta_no_ht>0)
-	{
-		int i;
+	if (pmlmepriv->num_sta_no_ht <= 0)
+		goto out;
 
-		spin_lock_bh(&pmlmepriv->scanned_queue.lock);
+	memset(ICS, 0, sizeof(ICS));
 
-		phead = get_list_head(queue);
-		plist = phead->next;
+	spin_lock_bh(&pmlmepriv->scanned_queue.lock);
 
-		list_for_each_safe(plist, ptmp, phead) {
-			int len;
-			u8 *p;
-			struct wlan_bssid_ex *pbss_network;
+	phead = get_list_head(queue);
+	plist = phead->next;
 
-			pnetwork = container_of(plist, struct wlan_network,
-						list);
+	list_for_each_safe(plist, ptmp, phead) {
+		const u8 *p;
+		struct wlan_bssid_ex *pbss_network;
 
-			pbss_network = &pnetwork->network;
+		pnetwork = container_of(plist, struct wlan_network, list);
 
-			p = rtw_get_ie23a(pbss_network->IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, pbss_network->IELength - _FIXED_IE_LENGTH_);
-			if ((p == NULL) || (len == 0))/* non-HT */
-			{
-				if ((pbss_network->Configuration.DSConfig<= 0) || (pbss_network->Configuration.DSConfig>14))
-					continue;
+		pbss_network = &pnetwork->network;
 
-				ICS[0][pbss_network->Configuration.DSConfig]= 1;
+		p = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY,
+				     pbss_network->IEs + _FIXED_IE_LENGTH_,
+				     pbss_network->IELength -_FIXED_IE_LENGTH_);
+		if (!p || !p[1]) { /* non-HT */
+			if (pbss_network->DSConfig <= 0 ||
+			    pbss_network->DSConfig > 14)
+				continue;
 
-				if (ICS[0][0] == 0)
-					ICS[0][0] = 1;
-			}
+			ICS[0][pbss_network->DSConfig] = 1;
 
+			if (ICS[0][0] == 0)
+				ICS[0][0] = 1;
 		}
 
-		spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
+	}
 
-		for (i = 0;i<8;i++)
-		{
-			if (ICS[i][0] == 1)
-			{
-				int j, k = 0;
+	spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
 
-				InfoContent[k] = i;
-				/* SET_BSS_INTOLERANT_ELE_REG_CLASS(InfoContent, i); */
-				k++;
+	for (i = 0; i < 8;i++) {
+		if (ICS[i][0] == 1) {
+			int j, k = 0;
 
-				for (j = 1;j<= 14;j++)
-				{
-					if (ICS[i][j]== 1)
-					{
-						if (k<16)
-						{
-							InfoContent[k] = j; /* channel number */
-							/* SET_BSS_INTOLERANT_ELE_CHANNEL(InfoContent+k, j); */
-							k++;
-						}
+			InfoContent[k] = i;
+			/* SET_BSS_INTOLERANT_ELE_REG_CLASS(InfoContent, i); */
+			k++;
+
+			for (j = 1; j <= 14; j++) {
+				if (ICS[i][j] == 1) {
+					if (k < 16) {
+						/* channel number */
+						InfoContent[k] = j;
+						k++;
 					}
 				}
-
-				pframe = rtw_set_ie23a(pframe, EID_BSSIntolerantChlReport, k, InfoContent, &pattrib->pktlen);
-
 			}
 
+			pframe = rtw_set_ie23a(pframe,
+					       EID_BSSIntolerantChlReport, k,
+					       InfoContent, &pattrib->pktlen);
 		}
-
 	}
 
+out:
 	pattrib->last_txcmdsz = pattrib->pktlen;
 
 	dump_mgntframe23a(padapter, pmgntframe);
 }
 
-unsigned int send_delba23a(struct rtw_adapter *padapter, u8 initiator, u8 *addr)
+int send_delba23a(struct rtw_adapter *padapter, u8 initiator, u8 *addr)
 {
 	struct sta_priv *pstapriv = &padapter->stapriv;
 	struct sta_info *psta = NULL;
@@ -7553,23 +4326,23 @@ unsigned int send_delba23a(struct rtw_adapter *padapter, u8 initiator, u8 *addr)
 	return _SUCCESS;
 }
 
-unsigned int send_beacon23a(struct rtw_adapter *padapter)
+int send_beacon23a(struct rtw_adapter *padapter)
 {
-	u8	bxmitok = false;
+	bool	bxmitok;
 	int	issue = 0;
 	int poll = 0;
 	unsigned long start = jiffies;
 	unsigned int passing_time;
 
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_BCN_VALID, NULL);
+	rtl8723a_bcn_valid(padapter);
 	do {
 		issue_beacon23a(padapter, 100);
 		issue++;
 		do {
 			yield();
-			rtw23a_hal_get_hwreg(padapter, HW_VAR_BCN_VALID, (u8 *)(&bxmitok));
+			bxmitok = rtl8723a_get_bcn_valid(padapter);
 			poll++;
-		} while ((poll%10)!= 0 && false == bxmitok &&
+		} while ((poll % 10) != 0 && bxmitok == false &&
 			 !padapter->bSurpriseRemoved &&
 			 !padapter->bDriverStopped);
 
@@ -7613,48 +4386,24 @@ bool IsLegal5GChannel(struct rtw_adapter *Adapter, u8 channel)
 	return false;
 }
 
-void site_survey23a(struct rtw_adapter *padapter)
+static void rtw_site_survey(struct rtw_adapter *padapter)
 {
-	unsigned char survey_channel = 0, val8;
+	unsigned char survey_channel = 0;
 	enum rt_scan_type ScanType = SCAN_PASSIVE;
 	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
-	u32 initialgain = 0;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-
-	if ((pwdinfo->rx_invitereq_info.scan_op_ch_only) ||
-	    (pwdinfo->p2p_info.scan_op_ch_only)) {
-		if (pwdinfo->rx_invitereq_info.scan_op_ch_only)
-			survey_channel = pwdinfo->rx_invitereq_info.operation_ch[pmlmeext->sitesurvey_res.channel_idx];
-		else
-			survey_channel = pwdinfo->p2p_info.operation_ch[pmlmeext->sitesurvey_res.channel_idx];
-		ScanType = SCAN_ACTIVE;
-	} else if (rtw_p2p_findphase_ex_is_social(pwdinfo)) {
-		/* The driver is in the find phase, it should go through the social channel. */
-		int ch_set_idx;
-		survey_channel = pwdinfo->social_chan[pmlmeext->sitesurvey_res.channel_idx];
-		ch_set_idx = rtw_ch_set_search_ch23a(pmlmeext->channel_set, survey_channel);
-		if (ch_set_idx >= 0)
-			ScanType = pmlmeext->channel_set[ch_set_idx].ScanType;
-		else
-			ScanType = SCAN_ACTIVE;
-	} else
-#endif /* CONFIG_8723AU_P2P */
-	{
-		struct rtw_ieee80211_channel *ch;
-		if (pmlmeext->sitesurvey_res.channel_idx < pmlmeext->sitesurvey_res.ch_num) {
-			ch = &pmlmeext->sitesurvey_res.ch[pmlmeext->sitesurvey_res.channel_idx];
-			survey_channel = ch->hw_value;
-			ScanType = (ch->flags & IEEE80211_CHAN_NO_IR) ? SCAN_PASSIVE : SCAN_ACTIVE;
-}
+	struct rtw_ieee80211_channel *ch;
+
+	if (pmlmeext->sitesurvey_res.channel_idx <
+	    pmlmeext->sitesurvey_res.ch_num) {
+		ch = &pmlmeext->sitesurvey_res.ch[pmlmeext->sitesurvey_res.channel_idx];
+		survey_channel = ch->hw_value;
+		ScanType = (ch->flags & IEEE80211_CHAN_NO_IR) ?
+			SCAN_PASSIVE : SCAN_ACTIVE;
 	}
 
 	if (survey_channel != 0) {
-		/* PAUSE 4-AC Queue when site_survey23a */
-		/* rtw23a_hal_get_hwreg(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8)); */
-		/* val8 |= 0x0f; */
-		/* rtw_hal_set_hwreg23a(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8)); */
+		/* PAUSE 4-AC Queue when site_survey */
 		if (pmlmeext->sitesurvey_res.channel_idx == 0)
 			set_channel_bwmode23a(padapter, survey_channel,
 					      HAL_PRIME_CHNL_OFFSET_DONT_CARE,
@@ -7664,317 +4413,251 @@ void site_survey23a(struct rtw_adapter *padapter)
 
 		if (ScanType == SCAN_ACTIVE) /* obey the channel plan setting... */
 		{
-#ifdef CONFIG_8723AU_P2P
-			if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) ||
-				rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH)
-			)
-			{
-				issue23a_probereq_p2p(padapter, NULL);
-				issue23a_probereq_p2p(padapter, NULL);
-				issue23a_probereq_p2p(padapter, NULL);
-			}
-			else
-#endif /* CONFIG_8723AU_P2P */
-			{
-				int i;
-				for (i = 0;i<RTW_SSID_SCAN_AMOUNT;i++) {
-					if (pmlmeext->sitesurvey_res.ssid[i].ssid_len) {
-						/* todo: to issue two probe req??? */
-						issue_probereq23a(padapter, &pmlmeext->sitesurvey_res.ssid[i], NULL);
-						/* msleep(SURVEY_TO>>1); */
-						issue_probereq23a(padapter, &pmlmeext->sitesurvey_res.ssid[i], NULL);
-					}
-				}
-
-				if (pmlmeext->sitesurvey_res.scan_mode == SCAN_ACTIVE) {
+			int i;
+			for (i = 0;i<RTW_SSID_SCAN_AMOUNT;i++) {
+				if (pmlmeext->sitesurvey_res.ssid[i].ssid_len) {
 					/* todo: to issue two probe req??? */
-					issue_probereq23a(padapter, NULL, NULL);
+					issue_probereq(padapter, &pmlmeext->sitesurvey_res.ssid[i], NULL);
 					/* msleep(SURVEY_TO>>1); */
-					issue_probereq23a(padapter, NULL, NULL);
+					issue_probereq(padapter, &pmlmeext->sitesurvey_res.ssid[i], NULL);
 				}
 			}
+
+			if (pmlmeext->sitesurvey_res.scan_mode == SCAN_ACTIVE) {
+				/* todo: to issue two probe req??? */
+				issue_probereq(padapter, NULL, NULL);
+				/* msleep(SURVEY_TO>>1); */
+				issue_probereq(padapter, NULL, NULL);
+			}
 		}
 
 		set_survey_timer(pmlmeext, pmlmeext->chan_scan_time);
 	} else {
-
 		/*	channel number is 0 or this channel is not valid. */
+		pmlmeext->sitesurvey_res.state = SCAN_COMPLETE;
 
+		/* switch back to the original channel */
 
-#ifdef CONFIG_8723AU_P2P
-		if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH))
-		{
-			if ((pwdinfo->rx_invitereq_info.scan_op_ch_only) || (pwdinfo->p2p_info.scan_op_ch_only))
-			{
-				/*	Set the find_phase_state_exchange_cnt to P2P_FINDPHASE_EX_CNT. */
-				/*	This will let the following flow to run the scanning end. */
-				rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_MAX);
-			}
-		}
-
-		if (rtw_p2p_findphase_ex_is_needed(pwdinfo))
-		{
-			/*	Set the P2P State to the listen state of find phase and set the current channel to the listen channel */
-			set_channel_bwmode23a(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
-			rtw_p2p_set_state(pwdinfo, P2P_STATE_FIND_PHASE_LISTEN);
-			pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
-
-			initialgain = 0xff; /* restore RX GAIN */
-			rtw_hal_set_hwreg23a(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
-			/* turn on dynamic functions */
-			Restore_DM_Func_Flag23a(padapter);
-			/* Switch_DM_Func23a(padapter, DYNAMIC_FUNC_DIG|DYNAMIC_FUNC_HP|DYNAMIC_FUNC_SS, true); */
-
-			mod_timer(&pwdinfo->find_phase_timer, jiffies +
-				  msecs_to_jiffies(pwdinfo->listen_dwell * 100));
-		} else
-#endif /* CONFIG_8723AU_P2P */
-		{
-#ifdef CONFIG_8723AU_P2P
-			if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_SCAN) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH))
-				rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
-			rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_NONE);
-#endif /* CONFIG_8723AU_P2P */
-
-			pmlmeext->sitesurvey_res.state = SCAN_COMPLETE;
-
-			/* switch back to the original channel */
-
-			set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+		set_channel_bwmode23a(padapter, pmlmeext->cur_channel,
+				      pmlmeext->cur_ch_offset,
+				      pmlmeext->cur_bwmode);
 
-			/* flush 4-AC Queue after site_survey23a */
-			/* val8 = 0; */
-			/* rtw_hal_set_hwreg23a(padapter, HW_VAR_TXPAUSE, (u8 *)(&val8)); */
+		/* flush 4-AC Queue after rtw_site_survey */
+		/* val8 = 0; */
 
-			/* config MSR */
-			Set_MSR23a(padapter, (pmlmeinfo->state & 0x3));
-
-			initialgain = 0xff; /* restore RX GAIN */
-			rtw_hal_set_hwreg23a(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain));
-			/* turn on dynamic functions */
-			Restore_DM_Func_Flag23a(padapter);
-			/* Switch_DM_Func23a(padapter, DYNAMIC_ALL_FUNC_ENABLE, true); */
+		/* config MSR */
+		Set_MSR23a(padapter, (pmlmeinfo->state & 0x3));
 
-			if (is_client_associated_to_ap23a(padapter) == true)
-			{
-				issue_nulldata23a(padapter, NULL, 0, 3, 500);
+		/* restore RX GAIN */
+		rtl8723a_set_initial_gain(padapter, 0xff);
+		/* turn on dynamic functions */
+		rtl8723a_odm_support_ability_restore(padapter);
 
-			}
+		if (is_client_associated_to_ap23a(padapter) == true)
+			issue_nulldata23a(padapter, NULL, 0, 3, 500);
 
-			val8 = 0; /* survey done */
-			rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
+		rtl8723a_mlme_sitesurvey(padapter, 0);
 
-			report_surveydone_event23a(padapter);
+		report_surveydone_event23a(padapter);
 
-			pmlmeext->chan_scan_time = SURVEY_TO;
-			pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
+		pmlmeext->chan_scan_time = SURVEY_TO;
+		pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
 
-			issue_action_BSSCoexistPacket(padapter);
-			issue_action_BSSCoexistPacket(padapter);
-			issue_action_BSSCoexistPacket(padapter);
-
-		}
+		issue_action_BSSCoexistPacket(padapter);
+		issue_action_BSSCoexistPacket(padapter);
+		issue_action_BSSCoexistPacket(padapter);
 	}
 
 	return;
 }
 
 /* collect bss info from Beacon and Probe request/response frames. */
-u8 collect_bss_info23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame, struct wlan_bssid_ex *bssid)
+int collect_bss_info23a(struct rtw_adapter *padapter,
+			struct recv_frame *precv_frame,
+			struct wlan_bssid_ex *bssid)
 {
-	int	i;
-	u32	len;
-	u8	*p;
-	u16	val16;
+	int i;
+	const u8 *p;
 	struct sk_buff *skb = precv_frame->pkt;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-	u8	*pframe = skb->data;
-	u32	packet_len = skb->len;
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
+	unsigned int length;
 	u8 ie_offset;
-	struct registry_priv	*pregistrypriv = &padapter->registrypriv;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
+	u16 capab_info;
 
-	len = packet_len - sizeof(struct ieee80211_hdr_3addr);
+	length = skb->len - sizeof(struct ieee80211_hdr_3addr);
 
-	if (len > MAX_IE_SZ)
-	{
+	if (length > MAX_IE_SZ) {
 		/* DBG_8723A("IE too long for survey event\n"); */
 		return _FAIL;
 	}
 
 	memset(bssid, 0, sizeof(struct wlan_bssid_ex));
 
-	if (ieee80211_is_beacon(hdr->frame_control)) {
+	if (ieee80211_is_beacon(mgmt->frame_control)) {
 		bssid->reserved = 1;
-		ie_offset = _BEACON_IE_OFFSET_;
+		ie_offset = offsetof(struct ieee80211_mgmt, u.beacon.variable);
+		capab_info = mgmt->u.beacon.capab_info;
+	} else  if (ieee80211_is_probe_req(mgmt->frame_control)) {
+		ie_offset = offsetof(struct ieee80211_mgmt,
+				     u.probe_req.variable);
+		bssid->reserved = 2;
+		capab_info = 0;
+	} else if (ieee80211_is_probe_resp(mgmt->frame_control)) {
+		ie_offset = offsetof(struct ieee80211_mgmt,
+				     u.probe_resp.variable);
+		bssid->reserved = 3;
+		capab_info = mgmt->u.probe_resp.capab_info;
 	} else {
-		/*  FIXME : more type */
-		if (ieee80211_is_probe_req(hdr->frame_control)) {
-			ie_offset = _PROBEREQ_IE_OFFSET_;
-			bssid->reserved = 2;
-		} else if (ieee80211_is_probe_resp(hdr->frame_control)) {
-			ie_offset = _PROBERSP_IE_OFFSET_;
-			bssid->reserved = 3;
-		} else {
-			bssid->reserved = 0;
-			ie_offset = _FIXED_IE_LENGTH_;
-		}
+		bssid->reserved = 0;
+		ie_offset = offsetof(struct ieee80211_mgmt, u.beacon.variable);
+		capab_info = mgmt->u.beacon.capab_info;
 	}
+	ie_offset -= offsetof(struct ieee80211_mgmt, u);
 
-	bssid->Length = sizeof(struct wlan_bssid_ex) - MAX_IE_SZ + len;
+	bssid->Length = offsetof(struct wlan_bssid_ex, IEs) + length;
 
 	/* below is to copy the information element */
-	bssid->IELength = len;
-	memcpy(bssid->IEs, (pframe + sizeof(struct ieee80211_hdr_3addr)), bssid->IELength);
+	bssid->IELength = length;
+	memcpy(bssid->IEs, &mgmt->u, bssid->IELength);
 
 	/* get the signal strength */
-	bssid->Rssi = precv_frame->attrib.phy_info.RecvSignalPower; /*  in dBM.raw data */
-	bssid->PhyInfo.SignalQuality = precv_frame->attrib.phy_info.SignalQuality;/* in percentage */
-	bssid->PhyInfo.SignalStrength = precv_frame->attrib.phy_info.SignalStrength;/* in percentage */
+	/*  in dBM.raw data */
+	bssid->Rssi = precv_frame->attrib.phy_info.RecvSignalPower;
+	bssid->PhyInfo.SignalQuality =
+		precv_frame->attrib.phy_info.SignalQuality;/* in percentage */
+	bssid->PhyInfo.SignalStrength =
+		precv_frame->attrib.phy_info.SignalStrength;/* in percentage */
 
 	/*  checking SSID */
-	if ((p = rtw_get_ie23a(bssid->IEs + ie_offset, _SSID_IE_, &len, bssid->IELength - ie_offset)) == NULL)
-	{
+	p = cfg80211_find_ie(WLAN_EID_SSID, bssid->IEs + ie_offset,
+			     bssid->IELength - ie_offset);
+
+	if (!p) {
 		DBG_8723A("marc: cannot find SSID for survey event\n");
 		return _FAIL;
 	}
 
-	if (*(p + 1)) {
-		if (len > IEEE80211_MAX_SSID_LEN) {
-			DBG_8723A("%s()-%d: IE too long (%d) for survey "
-				  "event\n", __func__, __LINE__, len);
-			return _FAIL;
-		}
-		memcpy(bssid->Ssid.ssid, (p + 2), *(p + 1));
-		bssid->Ssid.ssid_len = *(p + 1);
-	} else {
-		bssid->Ssid.ssid_len = 0;
+	if (p[1] > IEEE80211_MAX_SSID_LEN) {
+		DBG_8723A("%s()-%d: IE too long (%d) for survey "
+			  "event\n", __func__, __LINE__, p[1]);
+		return _FAIL;
 	}
+	memcpy(bssid->Ssid.ssid, p + 2, p[1]);
+	bssid->Ssid.ssid_len = p[1];
 
 	memset(bssid->SupportedRates, 0, NDIS_802_11_LENGTH_RATES_EX);
 
 	/* checking rate info... */
 	i = 0;
-	p = rtw_get_ie23a(bssid->IEs + ie_offset, _SUPPORTEDRATES_IE_, &len, bssid->IELength - ie_offset);
-	if (p != NULL)
-	{
-		if (len > NDIS_802_11_LENGTH_RATES_EX)
-		{
-			DBG_8723A("%s()-%d: IE too long (%d) for survey event\n", __func__, __LINE__, len);
+	p = cfg80211_find_ie(WLAN_EID_SUPP_RATES, bssid->IEs + ie_offset,
+			     bssid->IELength - ie_offset);
+	if (p) {
+		if (p[1] > NDIS_802_11_LENGTH_RATES_EX) {
+			DBG_8723A("%s()-%d: IE too long (%d) for survey "
+				  "event\n", __func__, __LINE__, p[1]);
 			return _FAIL;
 		}
-		memcpy(bssid->SupportedRates, (p + 2), len);
-		i = len;
+		memcpy(bssid->SupportedRates, p + 2, p[1]);
+		i = p[1];
 	}
 
-	p = rtw_get_ie23a(bssid->IEs + ie_offset, _EXT_SUPPORTEDRATES_IE_, &len, bssid->IELength - ie_offset);
-	if (p != NULL)
-	{
-		if (len > (NDIS_802_11_LENGTH_RATES_EX-i))
-		{
-			DBG_8723A("%s()-%d: IE too long (%d) for survey event\n", __func__, __LINE__, len);
+	p = cfg80211_find_ie(WLAN_EID_EXT_SUPP_RATES, bssid->IEs + ie_offset,
+			     bssid->IELength - ie_offset);
+	if (p) {
+		if (p[1] > (NDIS_802_11_LENGTH_RATES_EX-i)) {
+			DBG_8723A("%s()-%d: IE too long (%d) for survey "
+				  "event\n", __func__, __LINE__, p[1]);
 			return _FAIL;
 		}
-		memcpy(bssid->SupportedRates + i, (p + 2), len);
-	}
-
-	/* todo: */
-	{
-		bssid->NetworkTypeInUse = Ndis802_11OFDM24;
+		memcpy(bssid->SupportedRates + i, p + 2, p[1]);
 	}
 
 	if (bssid->IELength < 12)
 		return _FAIL;
 
 	/*  Checking for DSConfig */
-	p = rtw_get_ie23a(bssid->IEs + ie_offset, _DSSET_IE_, &len, bssid->IELength - ie_offset);
+	p = cfg80211_find_ie(WLAN_EID_DS_PARAMS, bssid->IEs + ie_offset,
+			     bssid->IELength - ie_offset);
 
-	bssid->Configuration.DSConfig = 0;
-	bssid->Configuration.Length = 0;
+	bssid->DSConfig = 0;
 
-	if (p)
-	{
-		bssid->Configuration.DSConfig = *(p + 2);
-	}
-	else
-	{/*  In 5G, some ap do not have DSSET IE */
+	if (p) {
+		bssid->DSConfig = p[2];
+	} else {/*  In 5G, some ap do not have DSSET IE */
 		/*  checking HT info for channel */
-		p = rtw_get_ie23a(bssid->IEs + ie_offset, _HT_ADD_INFO_IE_, &len, bssid->IELength - ie_offset);
-		if (p)
-		{
-			struct HT_info_element *HT_info = (struct HT_info_element *)(p + 2);
-			bssid->Configuration.DSConfig = HT_info->primary_channel;
-		}
-		else
-		{ /*  use current channel */
-			bssid->Configuration.DSConfig = rtw_get_oper_ch23a(padapter);
-		}
-	}
-
-	if (ieee80211_is_probe_req(hdr->frame_control)) {
+		p = cfg80211_find_ie(WLAN_EID_HT_OPERATION,
+				     bssid->IEs + ie_offset,
+				     bssid->IELength - ie_offset);
+		if (p) {
+			struct HT_info_element *HT_info =
+				(struct HT_info_element *)(p + 2);
+			bssid->DSConfig = HT_info->primary_channel;
+		} else /*  use current channel */
+			bssid->DSConfig = rtw_get_oper_ch23a(padapter);
+	}
+
+	if (ieee80211_is_probe_req(mgmt->frame_control)) {
 		/*  FIXME */
-		bssid->InfrastructureMode = Ndis802_11Infrastructure;
-		ether_addr_copy(bssid->MacAddress, hdr->addr2);
+		bssid->ifmode = NL80211_IFTYPE_STATION;
+		ether_addr_copy(bssid->MacAddress, mgmt->sa);
 		bssid->Privacy = 1;
 		return _SUCCESS;
 	}
 
-	memcpy(&bssid->Configuration.BeaconPeriod, rtw_get_beacon_interval23a_from_ie(bssid->IEs), 2);
-	bssid->Configuration.BeaconPeriod = le32_to_cpu(bssid->Configuration.BeaconPeriod);
+	bssid->BeaconPeriod = get_unaligned_le16(
+		rtw_get_beacon_interval23a_from_ie(bssid->IEs));
 
-	val16 = rtw_get_capability23a(bssid);
-
-	if (val16 & BIT(0)) {
-		bssid->InfrastructureMode = Ndis802_11Infrastructure;
-		ether_addr_copy(bssid->MacAddress, hdr->addr2);
+	if (capab_info & BIT(0)) {
+		bssid->ifmode = NL80211_IFTYPE_STATION;
+		ether_addr_copy(bssid->MacAddress, mgmt->sa);
 	} else {
-		bssid->InfrastructureMode = Ndis802_11IBSS;
-		ether_addr_copy(bssid->MacAddress, hdr->addr3);
+		bssid->ifmode = NL80211_IFTYPE_ADHOC;
+		ether_addr_copy(bssid->MacAddress, mgmt->bssid);
 	}
 
-	if (val16 & BIT(4))
+	if (capab_info & BIT(4))
 		bssid->Privacy = 1;
 	else
 		bssid->Privacy = 0;
 
-	bssid->Configuration.ATIMWindow = 0;
+	bssid->ATIMWindow = 0;
 
 	/* 20/40 BSS Coexistence check */
-	if ((pregistrypriv->wifi_spec == 1) && (false == pmlmeinfo->bwmode_updated))
-	{
+	if (pregistrypriv->wifi_spec == 1 &&
+	    pmlmeinfo->bwmode_updated == false) {
 		struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
-		p = rtw_get_ie23a(bssid->IEs + ie_offset, _HT_CAPABILITY_IE_, &len, bssid->IELength - ie_offset);
-		if (p && len > 0) {
-			struct HT_caps_element	*pHT_caps;
-			pHT_caps = (struct HT_caps_element	*)(p + 2);
+		p = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY,
+				     bssid->IEs + ie_offset,
+				     bssid->IELength - ie_offset);
+		if (p && p[1] > 0) {
+			struct HT_caps_element *pHT_caps;
+			pHT_caps = (struct HT_caps_element *)(p + 2);
 
 			if (pHT_caps->u.HT_cap_element.HT_caps_info & BIT(14))
 				pmlmepriv->num_FortyMHzIntolerant++;
 		} else
-		{
 			pmlmepriv->num_sta_no_ht++;
-		}
 	}
 
 
 	/*  mark bss info receving from nearby channel as SignalQuality 101 */
-	if (bssid->Configuration.DSConfig != rtw_get_oper_ch23a(padapter))
+	if (bssid->DSConfig != rtw_get_oper_ch23a(padapter))
 		bssid->PhyInfo.SignalQuality = 101;
 
 	return _SUCCESS;
 }
 
-void start_create_ibss23a(struct rtw_adapter* padapter)
+static void start_create_ibss(struct rtw_adapter* padapter)
 {
 	unsigned short	caps;
-	u8	val8;
-	u8	join_type;
 	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;
-	pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig;
+	pmlmeext->cur_channel = (u8)pnetwork->DSConfig;
 	pmlmeinfo->bcn_interval = get_beacon_interval23a(pnetwork);
 
 	/* update wireless mode */
@@ -7983,16 +4666,14 @@ void start_create_ibss23a(struct rtw_adapter* padapter)
 	/* udpate capability */
 	caps = rtw_get_capability23a(pnetwork);
 	update_capinfo23a(padapter, caps);
-	if (caps&cap_IBSS)/* adhoc master */
-	{
-		val8 = 0xcf;
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
+	if (caps & WLAN_CAPABILITY_IBSS) {	/* adhoc master */
+		rtl8723a_set_sec_cfg(padapter, 0xcf);
 
 		/* switch channel */
 		/* SelectChannel23a(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE); */
 		set_channel_bwmode23a(padapter, pmlmeext->cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
 
-		beacon_timing_control23a(padapter);
+		rtl8723a_SetBeaconRelatedRegisters(padapter);
 
 		/* set msr to WIFI_FW_ADHOC_STATE */
 		pmlmeinfo->state = WIFI_FW_ADHOC_STATE;
@@ -8008,9 +4689,8 @@ void start_create_ibss23a(struct rtw_adapter* padapter)
 		}
 		else
 		{
-			rtw_hal_set_hwreg23a(padapter, HW_VAR_BSSID, padapter->registrypriv.dev_network.MacAddress);
-			join_type = 0;
-			rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+			hw_var_set_bssid(padapter, padapter->registrypriv.dev_network.MacAddress);
+			hw_var_set_mlme_join(padapter, 0);
 
 			report_join_res23a(padapter, 1);
 			pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
@@ -8018,12 +4698,12 @@ void start_create_ibss23a(struct rtw_adapter* padapter)
 	}
 	else
 	{
-		DBG_8723A("start_create_ibss23a, invalid cap:%x\n", caps);
+		DBG_8723A("%s: invalid cap:%x\n", __func__, caps);
 		return;
 	}
 }
 
-void start_clnt_join23a(struct rtw_adapter* padapter)
+static void start_clnt_join(struct rtw_adapter* padapter)
 {
 	unsigned short	caps;
 	u8	val8;
@@ -8032,7 +4712,7 @@ void start_clnt_join23a(struct rtw_adapter* padapter)
 	struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;
 	int beacon_timeout;
 
-	pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig;
+	pmlmeext->cur_channel = (u8)pnetwork->DSConfig;
 	pmlmeinfo->bcn_interval = get_beacon_interval23a(pnetwork);
 
 	/* update wireless mode */
@@ -8041,15 +4721,16 @@ void start_clnt_join23a(struct rtw_adapter* padapter)
 	/* udpate capability */
 	caps = rtw_get_capability23a(pnetwork);
 	update_capinfo23a(padapter, caps);
-	if (caps&cap_ESS) {
+	if (caps & WLAN_CAPABILITY_ESS) {
 		/* switch channel */
 		set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
 
 		Set_MSR23a(padapter, WIFI_FW_STATION_STATE);
 
-		val8 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_8021X)? 0xcc: 0xcf;
+		val8 = (pmlmeinfo->auth_algo == dot11AuthAlgrthm_8021X) ?
+			0xcc: 0xcf;
 
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
+		rtl8723a_set_sec_cfg(padapter, val8);
 
 		/* switch channel */
 		/* set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); */
@@ -8061,18 +4742,15 @@ void start_clnt_join23a(struct rtw_adapter* padapter)
 		mod_timer(&padapter->mlmepriv.assoc_timer, jiffies +
 			  msecs_to_jiffies((REAUTH_TO * REAUTH_LIMIT) + (REASSOC_TO*REASSOC_LIMIT) + beacon_timeout));
 		pmlmeinfo->state = WIFI_FW_AUTH_NULL | WIFI_FW_STATION_STATE;
-	}
-	else if (caps&cap_IBSS) /* adhoc client */
-	{
+	} else if (caps & WLAN_CAPABILITY_IBSS) {	/* adhoc client */
 		Set_MSR23a(padapter, WIFI_FW_ADHOC_STATE);
 
-		val8 = 0xcf;
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
+		rtl8723a_set_sec_cfg(padapter, 0xcf);
 
 		/* switch channel */
 		set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
 
-		beacon_timing_control23a(padapter);
+		rtl8723a_SetBeaconRelatedRegisters(padapter);
 
 		pmlmeinfo->state = WIFI_FW_ADHOC_STATE;
 
@@ -8085,7 +4763,7 @@ void start_clnt_join23a(struct rtw_adapter* padapter)
 	}
 }
 
-void start_clnt_auth23a(struct rtw_adapter* padapter)
+static void start_clnt_auth(struct rtw_adapter* padapter)
 {
 	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
@@ -8109,12 +4787,12 @@ void start_clnt_auth23a(struct rtw_adapter* padapter)
 	issue_deauth23a(padapter, (&pmlmeinfo->network)->MacAddress, WLAN_REASON_DEAUTH_LEAVING);
 
 	DBG_8723A_LEVEL(_drv_always_, "start auth\n");
-	issue_auth23a(padapter, NULL, 0);
+	issue_auth(padapter, NULL, 0);
 
 	set_link_timer(pmlmeext, REAUTH_TO);
 }
 
-void start_clnt_assoc23a(struct rtw_adapter* padapter)
+static void start_clnt_assoc(struct rtw_adapter* padapter)
 {
 	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
@@ -8124,12 +4802,13 @@ void start_clnt_assoc23a(struct rtw_adapter* padapter)
 	pmlmeinfo->state &= (~(WIFI_FW_AUTH_NULL | WIFI_FW_AUTH_STATE));
 	pmlmeinfo->state |= (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE);
 
-	issue_assocreq23a(padapter);
+	issue_assocreq(padapter);
 
 	set_link_timer(pmlmeext, REASSOC_TO);
 }
 
-unsigned int receive_disconnect23a(struct rtw_adapter *padapter, unsigned char *MacAddr, unsigned short reason)
+int receive_disconnect23a(struct rtw_adapter *padapter,
+			  unsigned char *MacAddr, unsigned short reason)
 {
 	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
@@ -8158,7 +4837,8 @@ unsigned int receive_disconnect23a(struct rtw_adapter *padapter, unsigned char *
 	return _SUCCESS;
 }
 
-static void process_80211d(struct rtw_adapter *padapter, struct wlan_bssid_ex *bssid)
+static void process_80211d(struct rtw_adapter *padapter,
+			   struct wlan_bssid_ex *bssid)
 {
 	struct registry_priv *pregistrypriv;
 	struct mlme_ext_priv *pmlmeext;
@@ -8171,10 +4851,8 @@ static void process_80211d(struct rtw_adapter *padapter, struct wlan_bssid_ex *b
 
 	/*  Adjust channel plan by AP Country IE */
 	if (pregistrypriv->enable80211d &&
-		(!pmlmeext->update_channel_plan_by_ap_done))
-	{
-		u8 *ie, *p;
-		u32 len;
+	    !pmlmeext->update_channel_plan_by_ap_done) {
+		const u8 *ie, *p;
 		struct rt_channel_plan chplan_ap;
 		struct rt_channel_info chplan_sta[MAX_CHANNEL_NUM];
 		u8 country[4];
@@ -8182,31 +4860,34 @@ static void process_80211d(struct rtw_adapter *padapter, struct wlan_bssid_ex *b
 		u8 noc; /*  number of channel */
 		u8 j, k;
 
-		ie = rtw_get_ie23a(bssid->IEs + _FIXED_IE_LENGTH_, _COUNTRY_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
-		if (!ie) return;
-		if (len < 6) return;
+		ie = cfg80211_find_ie(WLAN_EID_COUNTRY,
+				      bssid->IEs + _FIXED_IE_LENGTH_,
+				      bssid->IELength - _FIXED_IE_LENGTH_);
+		if (!ie || ie[1] < IEEE80211_COUNTRY_IE_MIN_LEN)
+			return;
 
+		p = ie + 2;
+		ie += ie[1];
 		ie += 2;
-		p = ie;
-		ie += len;
 
-		memset(country, 0, 4);
 		memcpy(country, p, 3);
+		country[3] = '\0';
+
 		p += 3;
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
-				("%s: 802.11d country =%s\n", __func__, country));
+			 ("%s: 802.11d country =%s\n", __func__, country));
 
 		i = 0;
-		while ((ie - p) >= 3)
-		{
+		while ((ie - p) >= 3) {
 			fcn = *(p++);
 			noc = *(p++);
 			p++;
 
-			for (j = 0; j < noc; j++)
-			{
-				if (fcn <= 14) channel = fcn + j; /*  2.4 GHz */
-				else channel = fcn + j*4; /*  5 GHz */
+			for (j = 0; j < noc; j++) {
+				if (fcn <= 14)
+					channel = fcn + j; /*  2.4 GHz */
+				else
+					channel = fcn + j * 4; /*  5 GHz */
 
 				chplan_ap.Channel[i++] = channel;
 			}
@@ -8220,45 +4901,55 @@ static void process_80211d(struct rtw_adapter *padapter, struct wlan_bssid_ex *b
 		i = j = k = 0;
 		if (pregistrypriv->wireless_mode & WIRELESS_11G) {
 			do {
-				if ((i == MAX_CHANNEL_NUM) ||
-					(chplan_sta[i].ChannelNum == 0) ||
-					(chplan_sta[i].ChannelNum > 14))
+				if (i == MAX_CHANNEL_NUM ||
+				    chplan_sta[i].ChannelNum == 0 ||
+				    chplan_sta[i].ChannelNum > 14)
 					break;
 
-				if ((j == chplan_ap.Len) || (chplan_ap.Channel[j] > 14))
+				if (j == chplan_ap.Len ||
+				    chplan_ap.Channel[j] > 14)
 					break;
 
-				if (chplan_sta[i].ChannelNum == chplan_ap.Channel[j]) {
-					chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+				if (chplan_sta[i].ChannelNum ==
+				    chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum =
+						chplan_ap.Channel[j];
 					chplan_new[k].ScanType = SCAN_ACTIVE;
 					i++;
 					j++;
 					k++;
-				} else if (chplan_sta[i].ChannelNum < chplan_ap.Channel[j]) {
-					chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
-					chplan_new[k].ScanType = SCAN_PASSIVE;
+				} else if (chplan_sta[i].ChannelNum <
+					   chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum =
+						chplan_sta[i].ChannelNum;
+					chplan_new[k].ScanType =
+						SCAN_PASSIVE;
 					i++;
 					k++;
-				} else if (chplan_sta[i].ChannelNum > chplan_ap.Channel[j]) {
-					chplan_new[k].ChannelNum = chplan_ap.Channel[j];
-					chplan_new[k].ScanType = SCAN_ACTIVE;
+				} else if (chplan_sta[i].ChannelNum >
+					   chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum =
+						chplan_ap.Channel[j];
+					chplan_new[k].ScanType =
+						SCAN_ACTIVE;
 					j++;
 					k++;
 				}
 			} while (1);
 
 			/*  change AP not support channel to Passive scan */
-			while ((i < MAX_CHANNEL_NUM) &&
-				(chplan_sta[i].ChannelNum != 0) &&
-				(chplan_sta[i].ChannelNum <= 14)) {
-				chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+			while (i < MAX_CHANNEL_NUM &&
+			       chplan_sta[i].ChannelNum != 0 &&
+			       chplan_sta[i].ChannelNum <= 14) {
+				chplan_new[k].ChannelNum =
+					chplan_sta[i].ChannelNum;
 				chplan_new[k].ScanType = SCAN_PASSIVE;
 				i++;
 				k++;
 			}
 
 			/*  add channel AP supported */
-			while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14)) {
+			while (j < chplan_ap.Len && chplan_ap.Channel[j] <= 14){
 				chplan_new[k].ChannelNum = chplan_ap.Channel[j];
 				chplan_new[k].ScanType = SCAN_ACTIVE;
 				j++;
@@ -8266,49 +4957,50 @@ static void process_80211d(struct rtw_adapter *padapter, struct wlan_bssid_ex *b
 			}
 		} else {
 			/*  keep original STA 2.4G channel plan */
-			while ((i < MAX_CHANNEL_NUM) &&
-				(chplan_sta[i].ChannelNum != 0) &&
-				(chplan_sta[i].ChannelNum <= 14)) {
-				chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+			while (i < MAX_CHANNEL_NUM &&
+			       chplan_sta[i].ChannelNum != 0 &&
+			       chplan_sta[i].ChannelNum <= 14) {
+				chplan_new[k].ChannelNum =
+					chplan_sta[i].ChannelNum;
 				chplan_new[k].ScanType = chplan_sta[i].ScanType;
 				i++;
 				k++;
 			}
 
 			/*  skip AP 2.4G channel plan */
-			while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] <= 14)) {
+			while (j < chplan_ap.Len && chplan_ap.Channel[j] <= 14)
 				j++;
-			}
 		}
 
 		if (pregistrypriv->wireless_mode & WIRELESS_11A) {
 			do {
-				if ((i == MAX_CHANNEL_NUM) ||
-				    (chplan_sta[i].ChannelNum == 0))
+				if (i == MAX_CHANNEL_NUM ||
+				    chplan_sta[i].ChannelNum == 0)
 					break;
 
-				if ((j == chplan_ap.Len) || (chplan_ap.Channel[j] == 0))
+				if (j == chplan_ap.Len ||
+				    chplan_ap.Channel[j] == 0)
 					break;
 
-				if (chplan_sta[i].ChannelNum == chplan_ap.Channel[j])
-				{
-					chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+				if (chplan_sta[i].ChannelNum ==
+				    chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum =
+						chplan_ap.Channel[j];
 					chplan_new[k].ScanType = SCAN_ACTIVE;
 					i++;
 					j++;
 					k++;
-				}
-				else if (chplan_sta[i].ChannelNum < chplan_ap.Channel[j])
-				{
-					chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
-/*					chplan_new[k].ScanType = chplan_sta[i].ScanType; */
+				} else if (chplan_sta[i].ChannelNum <
+					   chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum =
+						chplan_sta[i].ChannelNum;
 					chplan_new[k].ScanType = SCAN_PASSIVE;
 					i++;
 					k++;
-				}
-				else if (chplan_sta[i].ChannelNum > chplan_ap.Channel[j])
-				{
-					chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+				} else if (chplan_sta[i].ChannelNum >
+					   chplan_ap.Channel[j]) {
+					chplan_new[k].ChannelNum =
+						chplan_ap.Channel[j];
 					chplan_new[k].ScanType = SCAN_ACTIVE;
 					j++;
 					k++;
@@ -8316,15 +5008,17 @@ static void process_80211d(struct rtw_adapter *padapter, struct wlan_bssid_ex *b
 			} while (1);
 
 			/*  change AP not support channel to Passive scan */
-			while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0)) {
-				chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+			while (i < MAX_CHANNEL_NUM &&
+			       chplan_sta[i].ChannelNum != 0) {
+				chplan_new[k].ChannelNum =
+					chplan_sta[i].ChannelNum;
 				chplan_new[k].ScanType = SCAN_PASSIVE;
 				i++;
 				k++;
 			}
 
 			/*  add channel AP supported */
-			while ((j < chplan_ap.Len) && (chplan_ap.Channel[j] != 0)) {
+			while (j < chplan_ap.Len && chplan_ap.Channel[j] != 0) {
 				chplan_new[k].ChannelNum = chplan_ap.Channel[j];
 				chplan_new[k].ScanType = SCAN_ACTIVE;
 				j++;
@@ -8332,8 +5026,10 @@ static void process_80211d(struct rtw_adapter *padapter, struct wlan_bssid_ex *b
 			}
 		} else {
 			/*  keep original STA 5G channel plan */
-			while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0)) {
-				chplan_new[k].ChannelNum = chplan_sta[i].ChannelNum;
+			while (i < MAX_CHANNEL_NUM &&
+			       chplan_sta[i].ChannelNum != 0) {
+				chplan_new[k].ChannelNum =
+					chplan_sta[i].ChannelNum;
 				chplan_new[k].ScanType = chplan_sta[i].ScanType;
 				i++;
 				k++;
@@ -8343,21 +5039,22 @@ static void process_80211d(struct rtw_adapter *padapter, struct wlan_bssid_ex *b
 	}
 
 	/*  If channel is used by AP, set channel scan type to active */
-	channel = bssid->Configuration.DSConfig;
+	channel = bssid->DSConfig;
 	chplan_new = pmlmeext->channel_set;
 	i = 0;
-	while ((i < MAX_CHANNEL_NUM) && (chplan_new[i].ChannelNum != 0)) {
-		if (chplan_new[i].ChannelNum == channel)
-		{
+	while (i < MAX_CHANNEL_NUM && chplan_new[i].ChannelNum != 0) {
+		if (chplan_new[i].ChannelNum == channel) {
 			if (chplan_new[i].ScanType == SCAN_PASSIVE) {
-				/* 5G Bnad 2, 3 (DFS) doesn't change to active scan */
+				/* 5G Bnad 2, 3 (DFS) doesn't change
+				   to active scan */
 				if (channel >= 52 && channel <= 144)
 					break;
 
 				chplan_new[i].ScanType = SCAN_ACTIVE;
 				RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
-						 ("%s: change channel %d scan type from passive to active\n",
-						  __func__, channel));
+					 ("%s: change channel %d scan type "
+					  "from passive to active\n",
+					  __func__, channel));
 			}
 			break;
 		}
@@ -8399,8 +5096,6 @@ void report_survey_event23a(struct rtw_adapter *padapter, struct recv_frame *pre
 		return;
 	}
 
-	INIT_LIST_HEAD(&pcmd_obj->list);
-
 	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
 	pcmd_obj->cmdsz = cmdsz;
 	pcmd_obj->parmbuf = pevtcmd;
@@ -8452,8 +5147,6 @@ void report_surveydone_event23a(struct rtw_adapter *padapter)
 		return;
 	}
 
-	INIT_LIST_HEAD(&pcmd_obj->list);
-
 	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
 	pcmd_obj->cmdsz = cmdsz;
 	pcmd_obj->parmbuf = pevtcmd;
@@ -8499,8 +5192,6 @@ void report_join_res23a(struct rtw_adapter *padapter, int res)
 		return;
 	}
 
-	INIT_LIST_HEAD(&pcmd_obj->list);
-
 	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
 	pcmd_obj->cmdsz = cmdsz;
 	pcmd_obj->parmbuf = pevtcmd;
@@ -8551,8 +5242,6 @@ void report_del_sta_event23a(struct rtw_adapter *padapter, unsigned char* MacAdd
 		return;
 	}
 
-	INIT_LIST_HEAD(&pcmd_obj->list);
-
 	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
 	pcmd_obj->cmdsz = cmdsz;
 	pcmd_obj->parmbuf = pevtcmd;
@@ -8607,8 +5296,6 @@ void report_add_sta_event23a(struct rtw_adapter *padapter, unsigned char* MacAdd
 		return;
 	}
 
-	INIT_LIST_HEAD(&pcmd_obj->list);
-
 	pcmd_obj->cmdcode = GEN_CMD_CODE(_Set_MLME_EVT);
 	pcmd_obj->cmdsz = cmdsz;
 	pcmd_obj->parmbuf = pevtcmd;
@@ -8678,7 +5365,7 @@ void update_sta_info23a(struct rtw_adapter *padapter, struct sta_info *psta)
 	psta->htpriv.candidate_tid_bitmap = 0x0;/* reset */
 
 	/* QoS */
-	if (pmlmepriv->qospriv.qos_option)
+	if (pmlmepriv->qos_option)
 		psta->qos_option = true;
 
 	psta->state = _FW_LINKED;
@@ -8691,17 +5378,14 @@ void mlmeext_joinbss_event_callback23a(struct rtw_adapter *padapter, int join_re
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *cur_network = &pmlmeinfo->network;
 	struct sta_priv		*pstapriv = &padapter->stapriv;
-	u8	join_type;
-	u16 media_status;
 
-	if (join_res < 0)
-	{
-		join_type = 1;
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_BSSID, null_addr);
+	if (join_res < 0) {
+		hw_var_set_mlme_join(padapter, 1);
+		hw_var_set_bssid(padapter, null_addr);
 
 		/* restore to initial setting. */
-		update_tx_basic_rate23a(padapter, padapter->registrypriv.wireless_mode);
+		update_tx_basic_rate23a(padapter,
+					padapter->registrypriv.wireless_mode);
 
 		goto exit_mlmeext_joinbss_event_callback23a;
 	}
@@ -8719,15 +5403,15 @@ void mlmeext_joinbss_event_callback23a(struct rtw_adapter *padapter, int join_re
 	}
 
 	/* turn on dynamic functions */
-	Switch_DM_Func23a(padapter, DYNAMIC_ALL_FUNC_ENABLE, true);
+	rtl8723a_odm_support_ability_set(padapter, DYNAMIC_ALL_FUNC_ENABLE);
 
 	/*  update IOT-releated issue */
 	update_IOT_info23a(padapter);
 
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_BASIC_RATE, cur_network->SupportedRates);
+	HalSetBrateCfg23a(padapter, cur_network->SupportedRates);
 
 	/* BCN interval */
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_BEACON_INTERVAL, (u8 *)(&pmlmeinfo->bcn_interval));
+	rtl8723a_set_beacon_interval(padapter, pmlmeinfo->bcn_interval);
 
 	/* udpate capability */
 	update_capinfo23a(padapter, pmlmeinfo->capability);
@@ -8752,18 +5436,13 @@ void mlmeext_joinbss_event_callback23a(struct rtw_adapter *padapter, int join_re
 
 		/* set per sta rate after updating HT cap. */
 		set_sta_rate23a(padapter, psta);
-
-		media_status = (psta->mac_id<<8)|1; /*   MACID|OPMODE: 1 means connect */
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_MEDIA_STATUS_RPT, (u8 *)&media_status);
 	}
 
-	join_type = 2;
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+	hw_var_set_mlme_join(padapter, 2);
 
-	if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
-	{
+	if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) {
 		/*  correcting TSF */
-		correct_TSF23a(padapter, pmlmeext);
+		rtw_correct_TSF(padapter);
 
 		/* set_link_timer(pmlmeext, DISCONNECT_TO); */
 	}
@@ -8778,27 +5457,20 @@ void mlmeext_sta_add_event_callback23a(struct rtw_adapter *padapter, struct sta_
 {
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
-	u8	join_type;
 
 	DBG_8723A("%s\n", __func__);
 
-	if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE)
-	{
-		if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)/* adhoc master or sta_count>1 */
+	if ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) {
+	/* adhoc master or sta_count>1 */
+		if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
 		{
 			/* nothing to do */
-		}
-		else/* adhoc client */
-		{
-			/* update TSF Value */
-			/* update_TSF23a(pmlmeext, pframe, len); */
-
+		} else { /* adhoc client */
 			/*  correcting TSF */
-			correct_TSF23a(padapter, pmlmeext);
+			rtw_correct_TSF(padapter);
 
 			/* start beacon */
-			if (send_beacon23a(padapter) == _FAIL)
-			{
+			if (send_beacon23a(padapter) != _SUCCESS) {
 				pmlmeinfo->FW_sta_info[psta->mac_id].status = 0;
 
 				pmlmeinfo->state ^= WIFI_FW_ADHOC_STATE;
@@ -8807,11 +5479,8 @@ void mlmeext_sta_add_event_callback23a(struct rtw_adapter *padapter, struct sta_
 			}
 
 			pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
-
 		}
-
-		join_type = 2;
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+		hw_var_set_mlme_join(padapter, 2);
 	}
 
 	pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
@@ -8825,25 +5494,27 @@ void mlmeext_sta_add_event_callback23a(struct rtw_adapter *padapter, struct sta_
 
 void mlmeext_sta_del_event_callback23a(struct rtw_adapter *padapter)
 {
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
-	if (is_client_associated_to_ap23a(padapter) || is_IBSS_empty23a(padapter))
-	{
+	if (is_client_associated_to_ap23a(padapter) ||
+	    is_IBSS_empty23a(padapter)) {
 		/* set_opmode_cmd(padapter, infra_client_with_mlme); */
 
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_DISCONNECT, NULL);
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_BSSID, null_addr);
+		hw_var_set_mlme_disconnect(padapter);
+		hw_var_set_bssid(padapter, null_addr);
 
 		/* restore to initial setting. */
-		update_tx_basic_rate23a(padapter, padapter->registrypriv.wireless_mode);
+		update_tx_basic_rate23a(padapter,
+					padapter->registrypriv.wireless_mode);
 
 		/* switch to the 20M Hz mode after disconnect */
 		pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
 		pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
 
-		/* SelectChannel23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset); */
-		set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+		set_channel_bwmode23a(padapter, pmlmeext->cur_channel,
+				      pmlmeext->cur_ch_offset,
+				      pmlmeext->cur_bwmode);
 
 		flush_all_cam_entry23a(padapter);
 
@@ -8856,33 +5527,11 @@ void mlmeext_sta_del_event_callback23a(struct rtw_adapter *padapter)
 	}
 }
 
-/****************************************************************************
-
-Following are the functions for the timer handlers
-
-*****************************************************************************/
-void linked23a_rx_sig_stren_disp(struct rtw_adapter *padapter)
-{
-	struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
-	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
-	u8 mac_id;
-	int UndecoratedSmoothedPWDB;
-	if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE)
-		mac_id = 0;
-	else if ((pmlmeinfo->state&0x03) == _HW_STATE_AP_)
-		mac_id = 2;
-
-	rtw_hal_get_def_var23a(padapter, HW_DEF_RA_INFO_DUMP,&mac_id);
-
-	rtw_hal_get_def_var23a(padapter, HAL_DEF_UNDERCORATEDSMOOTHEDPWDB, &UndecoratedSmoothedPWDB);
-	DBG_8723A("UndecoratedSmoothedPWDB:%d\n", UndecoratedSmoothedPWDB);
-}
-
 static u8 chk_ap_is_alive(struct rtw_adapter *padapter, struct sta_info *psta)
 {
 	u8 ret = false;
 
-	if ((sta_rx_data_pkts(psta) == sta_last_rx_data_pkts(psta)) &&
+	if (sta_rx_data_pkts(psta) == sta_last_rx_data_pkts(psta) &&
 	    sta_rx_beacon_pkts(psta) == sta_last_rx_beacon_pkts(psta) &&
 	    sta_rx_probersp_pkts(psta) == sta_last_rx_probersp_pkts(psta))
 		ret = false;
@@ -8895,20 +5544,16 @@ static u8 chk_ap_is_alive(struct rtw_adapter *padapter, struct sta_info *psta)
 
 void linked_status_chk23a(struct rtw_adapter *padapter)
 {
-	u32	i;
-	struct sta_info		*psta;
+	u32 i;
+	struct sta_info *psta;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
-	struct sta_priv		*pstapriv = &padapter->stapriv;
-
-	if (padapter->bRxRSSIDisplay)
-		 linked23a_rx_sig_stren_disp(padapter);
+	struct sta_priv *pstapriv = &padapter->stapriv;
 
-	rtw_hal_sreset_linked_status_check23a(padapter);
+	rtl8723a_sreset_linked_status_check(padapter);
 
-	if (is_client_associated_to_ap23a(padapter))
-	{
+	if (is_client_associated_to_ap23a(padapter)) {
 		/* linked infrastructure client mode */
 
 		int tx_chk = _SUCCESS, rx_chk = _SUCCESS;
@@ -8916,12 +5561,10 @@ void linked_status_chk23a(struct rtw_adapter *padapter)
 
 		rx_chk_limit = 4;
 
-		if ((psta = rtw_get_stainfo23a(pstapriv, pmlmeinfo->network.MacAddress)) != NULL)
-		{
+		psta = rtw_get_stainfo23a(pstapriv,
+					  pmlmeinfo->network.MacAddress);
+		if (psta) {
 			bool is_p2p_enable = false;
-#ifdef CONFIG_8723AU_P2P
-			is_p2p_enable = !rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE);
-#endif
 
 			if (chk_ap_is_alive(padapter, psta) == false)
 				rx_chk = _FAIL;
@@ -8929,102 +5572,108 @@ void linked_status_chk23a(struct rtw_adapter *padapter)
 			if (pxmitpriv->last_tx_pkts == pxmitpriv->tx_pkts)
 				tx_chk = _FAIL;
 
-			if (pmlmeext->active_keep_alive_check && (rx_chk == _FAIL || tx_chk == _FAIL)) {
+			if (pmlmeext->active_keep_alive_check &&
+			    (rx_chk == _FAIL || tx_chk == _FAIL)) {
 				u8 backup_oper_channel = 0;
 
-				/* switch to correct channel of current network  before issue keep-alive frames */
-				if (rtw_get_oper_ch23a(padapter) != pmlmeext->cur_channel) {
-					backup_oper_channel = rtw_get_oper_ch23a(padapter);
-					SelectChannel23a(padapter, pmlmeext->cur_channel);
+				/* switch to correct channel of current
+				   network  before issue keep-alive frames */
+				if (rtw_get_oper_ch23a(padapter) !=
+				    pmlmeext->cur_channel) {
+					backup_oper_channel =
+						rtw_get_oper_ch23a(padapter);
+					SelectChannel23a(padapter,
+							 pmlmeext->cur_channel);
 				}
 
 				if (rx_chk != _SUCCESS)
-					issue_probereq23a_ex23a(padapter, &pmlmeinfo->network.Ssid, psta->hwaddr, 3, 1);
-
-				if ((tx_chk != _SUCCESS && pmlmeinfo->link_count++ == 0xf) || rx_chk != _SUCCESS) {
-					tx_chk = issue_nulldata23a(padapter, psta->hwaddr, 0, 3, 1);
-					/* if tx acked and p2p disabled, set rx_chk _SUCCESS to reset retry count */
-					if (tx_chk == _SUCCESS && !is_p2p_enable)
+					issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, psta->hwaddr, 3, 1);
+
+				if ((tx_chk != _SUCCESS &&
+				     pmlmeinfo->link_count++ == 0xf) ||
+				    rx_chk != _SUCCESS) {
+					tx_chk = issue_nulldata23a(padapter,
+								   psta->hwaddr,
+								   0, 3, 1);
+					/* if tx acked and p2p disabled,
+					   set rx_chk _SUCCESS to reset retry
+					   count */
+					if (tx_chk == _SUCCESS &&
+					    !is_p2p_enable)
 						rx_chk = _SUCCESS;
 				}
 
 				/* back to the original operation channel */
 				if (backup_oper_channel>0)
-					SelectChannel23a(padapter, backup_oper_channel);
-
+					SelectChannel23a(padapter,
+							 backup_oper_channel);
 			} else {
 				if (rx_chk != _SUCCESS) {
 					if (pmlmeext->retry == 0) {
-						issue_probereq23a(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
-						issue_probereq23a(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
-						issue_probereq23a(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
+						issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
+						issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
+						issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
 					}
 				}
 
-				if (tx_chk != _SUCCESS && pmlmeinfo->link_count++ == 0xf)
-					tx_chk = issue_nulldata23a(padapter, NULL, 0, 1, 0);
+				if (tx_chk != _SUCCESS &&
+				    pmlmeinfo->link_count++ == 0xf)
+					tx_chk = issue_nulldata23a(padapter,
+								   NULL, 0, 1,
+								   0);
 			}
 
 			if (rx_chk == _FAIL) {
 				pmlmeext->retry++;
 				if (pmlmeext->retry > rx_chk_limit) {
-					DBG_8723A_LEVEL(_drv_always_, FUNC_ADPT_FMT" disconnect or roaming\n",
-						FUNC_ADPT_ARG(padapter));
+					DBG_8723A_LEVEL(_drv_always_,
+							"%s(%s): disconnect or "
+							"roaming\n", __func__,
+							padapter->pnetdev->name);
 					receive_disconnect23a(padapter, pmlmeinfo->network.MacAddress,
 						WLAN_REASON_EXPIRATION_CHK);
 					return;
 				}
-			} else {
+			} else
 				pmlmeext->retry = 0;
-			}
 
-			if (tx_chk == _FAIL) {
+			if (tx_chk == _FAIL)
 				pmlmeinfo->link_count &= 0xf;
-			} else {
+			else {
 				pxmitpriv->last_tx_pkts = pxmitpriv->tx_pkts;
 				pmlmeinfo->link_count = 0;
 			}
 
-		} /* end of if ((psta = rtw_get_stainfo23a(pstapriv, passoc_res->network.MacAddress)) != NULL) */
-	}
-	else if (is_client_associated_to_ibss23a(padapter))
-	{
+		}
+	} else if (is_client_associated_to_ibss23a(padapter)) {
 		/* linked IBSS mode */
 		/* for each assoc list entry to check the rx pkt counter */
-		for (i = IBSS_START_MAC_ID; i < NUM_STA; i++)
-		{
-			if (pmlmeinfo->FW_sta_info[i].status == 1)
-			{
+		for (i = IBSS_START_MAC_ID; i < NUM_STA; i++) {
+			if (pmlmeinfo->FW_sta_info[i].status == 1) {
 				psta = pmlmeinfo->FW_sta_info[i].psta;
 
-				if (NULL == psta) continue;
+				if (!psta)
+					continue;
 
-				if (pmlmeinfo->FW_sta_info[i].rx_pkt == sta_rx_pkts(psta))
-				{
+				if (pmlmeinfo->FW_sta_info[i].rx_pkt ==
+				    sta_rx_pkts(psta)) {
 
-					if (pmlmeinfo->FW_sta_info[i].retry<3)
-					{
+					if (pmlmeinfo->FW_sta_info[i].retry<3) {
 						pmlmeinfo->FW_sta_info[i].retry++;
-					}
-					else
-					{
+					} else {
 						pmlmeinfo->FW_sta_info[i].retry = 0;
 						pmlmeinfo->FW_sta_info[i].status = 0;
 						report_del_sta_event23a(padapter, psta->hwaddr,
 							65535/*  indicate disconnect caused by no rx */
 						);
 					}
-				}
-				else
-				{
+				} else {
 					pmlmeinfo->FW_sta_info[i].retry = 0;
 					pmlmeinfo->FW_sta_info[i].rx_pkt = (u32)sta_rx_pkts(psta);
 				}
 			}
 		}
-
 		/* set_link_timer(pmlmeext, DISCONNECT_TO); */
-
 	}
 }
 
@@ -9035,32 +5684,17 @@ static void survey_timer_hdl(unsigned long data)
 	struct sitesurvey_parm *psurveyPara;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-#endif
 
 	/* issue rtw_sitesurvey_cmd23a */
 	if (pmlmeext->sitesurvey_res.state > SCAN_START) {
 		if (pmlmeext->sitesurvey_res.state ==  SCAN_PROCESS)
 			pmlmeext->sitesurvey_res.channel_idx++;
 
-		if (pmlmeext->scan_abort == true)
-		{
-#ifdef CONFIG_8723AU_P2P
-			if (!rtw_p2p_chk_state(&padapter->wdinfo, P2P_STATE_NONE))
-			{
-				rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_MAX);
-				pmlmeext->sitesurvey_res.channel_idx = 3;
-				DBG_8723A("%s idx:%d, cnt:%u\n", __func__,
-					  pmlmeext->sitesurvey_res.channel_idx,
-					  pwdinfo->find_phase_state_exchange_cnt);
-			} else
-			#endif
-			{
-				pmlmeext->sitesurvey_res.channel_idx = pmlmeext->sitesurvey_res.ch_num;
-				DBG_8723A("%s idx:%d\n", __func__,
-					  pmlmeext->sitesurvey_res.channel_idx);
-			}
+		if (pmlmeext->scan_abort == true) {
+			pmlmeext->sitesurvey_res.channel_idx =
+				pmlmeext->sitesurvey_res.ch_num;
+			DBG_8723A("%s idx:%d\n", __func__,
+				  pmlmeext->sitesurvey_res.channel_idx);
 
 			pmlmeext->scan_abort = false;/* reset */
 		}
@@ -9077,7 +5711,8 @@ static void survey_timer_hdl(unsigned long data)
 			goto exit_survey_timer_hdl;
 		}
 
-		init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara, GEN_CMD_CODE(_SiteSurvey));
+		init_h2fwcmd_w_parm_no_rsp(ph2c, psurveyPara,
+					   GEN_CMD_CODE(_SiteSurvey));
 		rtw_enqueue_cmd23a(pcmdpriv, ph2c);
 	}
 
@@ -9095,17 +5730,13 @@ static void link_timer_hdl(unsigned long data)
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	/* struct sta_priv		*pstapriv = &padapter->stapriv; */
 
-	if (pmlmeinfo->state & WIFI_FW_AUTH_NULL)
-	{
+	if (pmlmeinfo->state & WIFI_FW_AUTH_NULL) {
 		DBG_8723A("link_timer_hdl:no beacon while connecting\n");
 		pmlmeinfo->state = WIFI_FW_NULL_STATE;
 		report_join_res23a(padapter, -3);
-	}
-	else if (pmlmeinfo->state & WIFI_FW_AUTH_STATE)
-	{
+	} else if (pmlmeinfo->state & WIFI_FW_AUTH_STATE) {
 		/* re-auth timer */
-		if (++pmlmeinfo->reauth_count > REAUTH_LIMIT)
-		{
+		if (++pmlmeinfo->reauth_count > REAUTH_LIMIT) {
 			/* if (pmlmeinfo->auth_algo != dot11AuthAlgrthm_Auto) */
 			/*  */
 				pmlmeinfo->state = 0;
@@ -9114,28 +5745,25 @@ static void link_timer_hdl(unsigned long data)
 			/*  */
 			/* else */
 			/*  */
-			/*	pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared; */
-			/*	pmlmeinfo->reauth_count = 0; */
+			/* pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared; */
+			/* pmlmeinfo->reauth_count = 0; */
 			/*  */
 		}
 
 		DBG_8723A("link_timer_hdl: auth timeout and try again\n");
 		pmlmeinfo->auth_seq = 1;
-		issue_auth23a(padapter, NULL, 0);
+		issue_auth(padapter, NULL, 0);
 		set_link_timer(pmlmeext, REAUTH_TO);
-	}
-	else if (pmlmeinfo->state & WIFI_FW_ASSOC_STATE)
-	{
+	} else if (pmlmeinfo->state & WIFI_FW_ASSOC_STATE) {
 		/* re-assoc timer */
-		if (++pmlmeinfo->reassoc_count > REASSOC_LIMIT)
-		{
+		if (++pmlmeinfo->reassoc_count > REASSOC_LIMIT) {
 			pmlmeinfo->state = WIFI_FW_NULL_STATE;
 			report_join_res23a(padapter, -2);
 			return;
 		}
 
 		DBG_8723A("link_timer_hdl: assoc timeout and try again\n");
-		issue_assocreq23a(padapter);
+		issue_assocreq(padapter);
 		set_link_timer(pmlmeext, REASSOC_TO);
 	}
 
@@ -9152,11 +5780,9 @@ static void addba_timer_hdl(unsigned long data)
 
 	phtpriv = &psta->htpriv;
 
-	if ((phtpriv->ht_option == true) && (phtpriv->ampdu_enable == true))
-	{
+	if (phtpriv->ht_option == true && phtpriv->ampdu_enable == true) {
 		if (phtpriv->candidate_tid_bitmap)
 			phtpriv->candidate_tid_bitmap = 0x0;
-
 	}
 }
 
@@ -9177,57 +5803,58 @@ void init_mlme_ext_timer23a(struct rtw_adapter *padapter)
 		    (unsigned long)padapter);
 }
 
-u8 NULL_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
+int NULL_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
 	return H2C_SUCCESS;
 }
 
-u8 setopmode_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
+int setopmode_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
-	u8	type;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	enum nl80211_iftype type;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
-	struct setopmode_parm *psetop = (struct setopmode_parm *)pbuf;
+	const struct setopmode_parm *psetop = (struct setopmode_parm *)pbuf;
 
-	if (psetop->mode == Ndis802_11APMode)
-	{
+	switch (psetop->mode) {
+	case NL80211_IFTYPE_P2P_GO:
+	case NL80211_IFTYPE_AP:
 		pmlmeinfo->state = WIFI_FW_AP_STATE;
 		type = _HW_STATE_AP_;
-	}
-	else if (psetop->mode == Ndis802_11Infrastructure)
-	{
-		pmlmeinfo->state &= ~(BIT(0)|BIT(1));/*  clear state */
-		pmlmeinfo->state |= WIFI_FW_STATION_STATE;/* set to	STATION_STATE */
+		break;
+	case NL80211_IFTYPE_P2P_CLIENT:
+	case NL80211_IFTYPE_STATION:
+		/*  clear state */
+		pmlmeinfo->state &= ~(BIT(0)|BIT(1));
+		/* set to STATION_STATE */
+		pmlmeinfo->state |= WIFI_FW_STATION_STATE;
 		type = _HW_STATE_STATION_;
-	}
-	else if (psetop->mode == Ndis802_11IBSS)
-	{
+		break;
+	case NL80211_IFTYPE_ADHOC:
 		type = _HW_STATE_ADHOC_;
-	}
-	else
-	{
+		break;
+	default:
 		type = _HW_STATE_NOLINK_;
+		break;
 	}
 
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_SET_OPMODE, (u8 *)(&type));
+	hw_var_set_opmode(padapter, type);
 	/* Set_NETYPE0_MSR(padapter, type); */
 
 	return H2C_SUCCESS;
 }
 
-u8 createbss_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
+int createbss_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
 	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;
-	struct wlan_bssid_ex *pparm = (struct wlan_bssid_ex *)pbuf;
+	const struct wlan_bssid_ex *pparm = (struct wlan_bssid_ex *)pbuf;
 	/* u32	initialgain; */
 
-	if (pparm->InfrastructureMode == Ndis802_11APMode) {
+	if (pparm->ifmode == NL80211_IFTYPE_AP ||
+	    pparm->ifmode == NL80211_IFTYPE_P2P_GO) {
 #ifdef CONFIG_8723AU_AP_MODE
-
-		if (pmlmeinfo->state == WIFI_FW_AP_STATE)
-		{
+		if (pmlmeinfo->state == WIFI_FW_AP_STATE) {
 			/* todo: */
 			return H2C_SUCCESS;
 		}
@@ -9235,7 +5862,7 @@ u8 createbss_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
 	}
 
 	/* below is for ad-hoc master */
-	if (pparm->InfrastructureMode == Ndis802_11IBSS) {
+	if (pparm->ifmode == NL80211_IFTYPE_ADHOC) {
 		rtw_joinbss_reset23a(padapter);
 
 		pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
@@ -9245,16 +5872,12 @@ u8 createbss_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
 		pmlmeinfo->HT_enable = 0;
 		pmlmeinfo->HT_caps_enable = 0;
 		pmlmeinfo->HT_info_enable = 0;
-		pmlmeinfo->agg_enable_bitmap = 0;
-		pmlmeinfo->candidate_tid_bitmap = 0;
 
 		/* disable dynamic functions, such as high power, DIG */
-		Save_DM_Func_Flag23a(padapter);
-		Switch_DM_Func23a(padapter, DYNAMIC_FUNC_DISABLE, false);
+		rtl8723a_odm_support_ability_backup(padapter);
 
-		/* config the initial gain under linking, need to write the BB registers */
-		/* initialgain = 0x1E; */
-		/* rtw_hal_set_hwreg23a(padapter, HW_VAR_INITIAL_GAIN, (u8 *)(&initialgain)); */
+		rtl8723a_odm_support_ability_clr(padapter,
+						 DYNAMIC_FUNC_DISABLE);
 
 		/* cancel link timer */
 		del_timer_sync(&pmlmeext->link_timer);
@@ -9267,31 +5890,30 @@ u8 createbss_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
 
 		memcpy(pnetwork, pparm, sizeof(struct wlan_bssid_ex));
 
-		start_create_ibss23a(padapter);
+		start_create_ibss(padapter);
 	}
 
 	return H2C_SUCCESS;
 }
 
-u8 join_cmd_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
+int join_cmd_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
-	u8	join_type;
-	struct ndis_802_11_var_ies *	pIE;
-	struct registry_priv	*pregpriv = &padapter->registrypriv;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;
-	struct wlan_bssid_ex *pparm = (struct wlan_bssid_ex *)pbuf;
+	const struct wlan_bssid_ex *pparm = (struct wlan_bssid_ex *)pbuf;
 	struct HT_info_element *pht_info;
 	u32 i;
+	int bcn_fixed_size;
+	u8 *p;
         /* u32	initialgain; */
 	/* u32	acparm; */
 
 	/* check already connecting to AP or not */
-	if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
-	{
+	if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) {
 		if (pmlmeinfo->state & WIFI_FW_STATION_STATE)
-			issue_deauth23a_ex23a(padapter, pnetwork->MacAddress,
+			issue_deauth_ex(padapter, pnetwork->MacAddress,
 					WLAN_REASON_DEAUTH_LEAVING, 5, 100);
 
 		pmlmeinfo->state = WIFI_FW_NULL_STATE;
@@ -9305,7 +5927,7 @@ u8 join_cmd_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
 		/* Set_MSR23a(padapter, _HW_STATE_NOLINK_); */
 		Set_MSR23a(padapter, _HW_STATE_STATION_);
 
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_DISCONNECT, NULL);
+		hw_var_set_mlme_disconnect(padapter);
 	}
 
 	rtw_joinbss_reset23a(padapter);
@@ -9317,8 +5939,6 @@ u8 join_cmd_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
 	pmlmeinfo->HT_enable = 0;
 	pmlmeinfo->HT_caps_enable = 0;
 	pmlmeinfo->HT_info_enable = 0;
-	pmlmeinfo->agg_enable_bitmap = 0;
-	pmlmeinfo->candidate_tid_bitmap = 0;
 	pmlmeinfo->bwmode_updated = false;
 	/* pmlmeinfo->assoc_AP_vendor = HT_IOT_PEER_MAX; */
 
@@ -9331,27 +5951,28 @@ u8 join_cmd_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
 	/* pmlmeinfo->assoc_AP_vendor = check_assoc_AP23a(pnetwork->IEs,
 	   pnetwork->IELength); */
 
-	for (i = sizeof(struct ndis_802_11_fixed_ies); i < pnetwork->IELength;)
-	{
-		pIE = (struct ndis_802_11_var_ies *)(pnetwork->IEs + i);
+	bcn_fixed_size = offsetof(struct ieee80211_mgmt, u.beacon.variable) -
+		offsetof(struct ieee80211_mgmt, u.beacon);
 
-		switch (pIE->ElementID)
-		{
-		case _VENDOR_SPECIFIC_IE_:/* Get WMM IE. */
-			if (!memcmp(pIE->data, WMM_OUI23A, 4))
+	for (i = bcn_fixed_size; i < pnetwork->IELength;) {
+		p = pnetwork->IEs + i;
+
+		switch (p[0]) {
+		case WLAN_EID_VENDOR_SPECIFIC:/* Get WMM IE. */
+			if (!memcmp(p + 2, WMM_OUI23A, 4))
 				pmlmeinfo->WMM_enable = 1;
 			break;
 
-		case _HT_CAPABILITY_IE_:	/* Get HT Cap IE. */
+		case WLAN_EID_HT_CAPABILITY:	/* Get HT Cap IE. */
 			pmlmeinfo->HT_caps_enable = 1;
 			break;
 
-		case _HT_EXTRA_INFO_IE_:	/* Get HT Info IE. */
+		case WLAN_EID_HT_OPERATION:	/* Get HT Info IE. */
 			pmlmeinfo->HT_info_enable = 1;
 
 			/* spec case only for cisco's ap because cisco's ap
 			 * issue assoc rsp using mcs rate @40MHz or @20MHz */
-			pht_info = (struct HT_info_element *)(pIE->data);
+			pht_info = (struct HT_info_element *)(p + 2);
 
 			if ((pregpriv->cbw40_enable) &&
 			    (pht_info->infos[0] & BIT(2))) {
@@ -9383,59 +6004,46 @@ u8 join_cmd_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
 			break;
 		}
 
-		i += (pIE->Length + 2);
+		i += (p[1] + 2);
 	}
-	/* disable dynamic functions, such as high power, DIG */
-	/* Switch_DM_Func23a(padapter, DYNAMIC_FUNC_DISABLE, false); */
-
-	/* config the initial gain under linking, need to write the BB
-	   registers */
-	/* initialgain = 0x1E; */
-	/* rtw_hal_set_hwreg23a(padapter, HW_VAR_INITIAL_GAIN,
-	   (u8 *)(&initialgain)); */
 
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_BSSID,
-			  pmlmeinfo->network.MacAddress);
-	join_type = 0;
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+	hw_var_set_bssid(padapter, pmlmeinfo->network.MacAddress);
+	hw_var_set_mlme_join(padapter, 0);
 
 	/* cancel link timer */
 	del_timer_sync(&pmlmeext->link_timer);
 
-	start_clnt_join23a(padapter);
+	start_clnt_join(padapter);
 
 	return H2C_SUCCESS;
 }
 
-u8 disconnect_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
+int disconnect_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
-	struct disconnect_parm *param = (struct disconnect_parm *)pbuf;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	const struct disconnect_parm *param = (struct disconnect_parm *)pbuf;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *pnetwork = &pmlmeinfo->network;
-	u8	val8;
 
-	if (is_client_associated_to_ap23a(padapter))
-	{
-		issue_deauth23a_ex23a(padapter, pnetwork->MacAddress, WLAN_REASON_DEAUTH_LEAVING, param->deauth_timeout_ms/100, 100);
+	if (is_client_associated_to_ap23a(padapter)) {
+		issue_deauth_ex(padapter, pnetwork->MacAddress,
+				WLAN_REASON_DEAUTH_LEAVING,
+				param->deauth_timeout_ms/100, 100);
 	}
 
 	/* set_opmode_cmd(padapter, infra_client_with_mlme); */
 
 	/* pmlmeinfo->state = WIFI_FW_NULL_STATE; */
 
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_DISCONNECT, NULL);
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_BSSID, null_addr);
+	hw_var_set_mlme_disconnect(padapter);
+	hw_var_set_bssid(padapter, null_addr);
 
 	/* restore to initial setting. */
 	update_tx_basic_rate23a(padapter, padapter->registrypriv.wireless_mode);
 
-	if (((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) || ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE))
-	{
-		/* Stop BCN */
-		val8 = 0;
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_BCN_FUNC, (u8 *)(&val8));
-	}
+	if ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE ||
+	    (pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE)
+		rtl8723a_set_bcn_func(padapter, 0);	/* Stop BCN */
 
 	/* set MSR to no link state -> infra. mode */
 	Set_MSR23a(padapter, _HW_STATE_STATION_);
@@ -9446,7 +6054,8 @@ u8 disconnect_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
 	pmlmeext->cur_bwmode = HT_CHANNEL_WIDTH_20;
 	pmlmeext->cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
 
-	set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+	set_channel_bwmode23a(padapter, pmlmeext->cur_channel,
+			      pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
 
 	flush_all_cam_entry23a(padapter);
 
@@ -9457,8 +6066,10 @@ u8 disconnect_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
 	return	H2C_SUCCESS;
 }
 
-static int rtw_scan_ch_decision(struct rtw_adapter *padapter, struct rtw_ieee80211_channel *out,
-	u32 out_num, struct rtw_ieee80211_channel *in, u32 in_num)
+static int
+rtw_scan_ch_decision(struct rtw_adapter *padapter,
+		     struct rtw_ieee80211_channel *out, u32 out_num,
+		     const struct rtw_ieee80211_channel *in, u32 in_num)
 {
 	int i, j;
 	int scan_ch_num = 0;
@@ -9471,15 +6082,15 @@ static int rtw_scan_ch_decision(struct rtw_adapter *padapter, struct rtw_ieee802
 	/* acquire channels from in */
 	j = 0;
 	for (i = 0;i<in_num;i++) {
-		if (0)
-		DBG_8723A(FUNC_ADPT_FMT" "CHAN_FMT"\n", FUNC_ADPT_ARG(padapter), CHAN_ARG(&in[i]));
-		if (in[i].hw_value && !(in[i].flags & IEEE80211_CHAN_DISABLED)
-			&& (set_idx = rtw_ch_set_search_ch23a(pmlmeext->channel_set, in[i].hw_value)) >= 0
-		)
-		{
-			memcpy(&out[j], &in[i], sizeof(struct rtw_ieee80211_channel));
-
-			if (pmlmeext->channel_set[set_idx].ScanType == SCAN_PASSIVE)
+		if (in[i].hw_value &&
+		    !(in[i].flags & IEEE80211_CHAN_DISABLED) &&
+		    (set_idx = rtw_ch_set_search_ch23a(pmlmeext->channel_set,
+						       in[i].hw_value)) >= 0) {
+			memcpy(&out[j], &in[i],
+			       sizeof(struct rtw_ieee80211_channel));
+
+			if (pmlmeext->channel_set[set_idx].ScanType ==
+			    SCAN_PASSIVE)
 				out[j].flags &= IEEE80211_CHAN_NO_IR;
 
 			j++;
@@ -9500,7 +6111,7 @@ static int rtw_scan_ch_decision(struct rtw_adapter *padapter, struct rtw_ieee802
 		}
 	}
 
-	if (padapter->setband == GHZ_24) {				/*  2.4G */
+	if (padapter->setband == GHZ_24) {			/*  2.4G */
 		for (i = 0; i < j ; i++) {
 			if (out[i].hw_value > 35)
 				memset(&out[i], 0,
@@ -9509,10 +6120,11 @@ static int rtw_scan_ch_decision(struct rtw_adapter *padapter, struct rtw_ieee802
 				scan_ch_num++;
 		}
 		j = scan_ch_num;
-	} else if  (padapter->setband == GHZ_50) {			/*  5G */
+	} else if  (padapter->setband == GHZ_50) {		/*  5G */
 		for (i = 0; i < j ; i++) {
 			if (out[i].hw_value > 35) {
-				memcpy(&out[scan_ch_num++], &out[i], sizeof(struct rtw_ieee80211_channel));
+				memcpy(&out[scan_ch_num++], &out[i],
+				       sizeof(struct rtw_ieee80211_channel));
 			}
 		}
 		j = scan_ch_num;
@@ -9522,19 +6134,15 @@ static int rtw_scan_ch_decision(struct rtw_adapter *padapter, struct rtw_ieee802
 	return j;
 }
 
-u8 sitesurvey_cmd_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
+int sitesurvey_cmd_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	struct sitesurvey_parm *pparm = (struct sitesurvey_parm *)pbuf;
+	const struct sitesurvey_parm *pparm = (struct sitesurvey_parm *)pbuf;
 	u8 bdelayscan = false;
-	u8 val8;
 	u32 initialgain;
 	u32 i;
 
 	if (pmlmeext->sitesurvey_res.state == SCAN_DISABLE) {
-		/* for first time sitesurvey_cmd */
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_CHECK_TXBUF, NULL);
-
 		pmlmeext->sitesurvey_res.state = SCAN_START;
 		pmlmeext->sitesurvey_res.bss_cnt = 0;
 		pmlmeext->sitesurvey_res.channel_idx = 0;
@@ -9564,8 +6172,10 @@ u8 sitesurvey_cmd_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
 
 			/* switch to correct channel of current network
 			   before issue keep-alive frames */
-			if (rtw_get_oper_ch23a(padapter) != pmlmeext->cur_channel)
-				SelectChannel23a(padapter, pmlmeext->cur_channel);
+			if (rtw_get_oper_ch23a(padapter) !=
+			    pmlmeext->cur_channel)
+				SelectChannel23a(padapter,
+						 pmlmeext->cur_channel);
 
 			issue_nulldata23a(padapter, NULL, 1, 3, 500);
 
@@ -9579,83 +6189,80 @@ u8 sitesurvey_cmd_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
 		}
 	}
 
-	if ((pmlmeext->sitesurvey_res.state == SCAN_START) ||
-	    (pmlmeext->sitesurvey_res.state == SCAN_TXNULL)) {
+	if (pmlmeext->sitesurvey_res.state == SCAN_START ||
+	    pmlmeext->sitesurvey_res.state == SCAN_TXNULL) {
 		/* disable dynamic functions, such as high power, DIG */
-		Save_DM_Func_Flag23a(padapter);
-		Switch_DM_Func23a(padapter, DYNAMIC_FUNC_DISABLE, false);
+		rtl8723a_odm_support_ability_backup(padapter);
+		rtl8723a_odm_support_ability_clr(padapter,
+						 DYNAMIC_FUNC_DISABLE);
 
 		/* config the initial gain under scaning, need to
 		   write the BB registers */
-		if ((wdev_to_priv(padapter->rtw_wdev))->p2p_enabled == true) {
+		if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled == true)
 			initialgain = 0x30;
-		} else
+		else
 			initialgain = 0x1E;
 
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_INITIAL_GAIN,
-				  (u8 *)(&initialgain));
+		rtl8723a_set_initial_gain(padapter, initialgain);
 
 		/* set MSR to no link state */
 		Set_MSR23a(padapter, _HW_STATE_NOLINK_);
 
-		val8 = 1; /* under site survey */
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_SITESURVEY,
-				  (u8 *)(&val8));
+		rtl8723a_mlme_sitesurvey(padapter, 1);
 
 		pmlmeext->sitesurvey_res.state = SCAN_PROCESS;
 	}
 
-	site_survey23a(padapter);
+	rtw_site_survey(padapter);
 
 	return H2C_SUCCESS;
 }
 
-u8 setauth_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
+int setauth_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
-	struct setauth_parm		*pparm = (struct setauth_parm *)pbuf;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	const struct setauth_parm *pparm = (struct setauth_parm *)pbuf;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
 	if (pparm->mode < 4)
-	{
 		pmlmeinfo->auth_algo = pparm->mode;
-	}
 
 	return	H2C_SUCCESS;
 }
 
-u8 setkey_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
+int setkey_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
-	unsigned short				ctrl;
-	struct setkey_parm		*pparm = (struct setkey_parm *)pbuf;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	unsigned short ctrl;
+	const struct setkey_parm *pparm = (struct setkey_parm *)pbuf;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
-	unsigned char					null_sta[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+	unsigned char null_sta[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
 
 	/* main tx key for wep. */
 	if (pparm->set_tx)
 		pmlmeinfo->key_index = pparm->keyid;
 
 	/* write cam */
-	ctrl = BIT(15) | ((pparm->algorithm) << 2) | pparm->keyid;
+	ctrl = BIT(15) | (pparm->algorithm) << 2 | pparm->keyid;
 
-	DBG_8723A_LEVEL(_drv_always_, "set group key to hw: alg:%d(WEP40-1 WEP104-5 TKIP-2 AES-4) "
-			"keyid:%d\n", pparm->algorithm, pparm->keyid);
-	write_cam23a(padapter, pparm->keyid, ctrl, null_sta, pparm->key);
+	DBG_8723A_LEVEL(_drv_always_, "set group key to hw: alg:%d(WEP40-1 "
+			"WEP104-5 TKIP-2 AES-4) keyid:%d\n",
+			pparm->algorithm, pparm->keyid);
+	rtl8723a_cam_write(padapter, pparm->keyid, ctrl, null_sta, pparm->key);
 
 	/* allow multicast packets to driver */
-        padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_ON_RCR_AM, null_addr);
+	rtl8723a_on_rcr_am(padapter);
 
 	return H2C_SUCCESS;
 }
 
-u8 set_stakey_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
+int set_stakey_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
 	u16 ctrl = 0;
 	u8 cam_id;/* cam_entry */
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
-	struct set_stakey_parm	*pparm = (struct set_stakey_parm *)pbuf;
+	const struct set_stakey_parm *pparm = (struct set_stakey_parm *)pbuf;
 
 	/* cam_entry: */
 	/* 0~3 for default key */
@@ -9672,105 +6279,104 @@ u8 set_stakey_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
 
 	cam_id = 4;
 
-	DBG_8723A_LEVEL(_drv_always_, "set pairwise key to hw: alg:%d(WEP40-1 WEP104-5 TKIP-2 AES-4) camid:%d\n",
+	DBG_8723A_LEVEL(_drv_always_, "set pairwise key to hw: alg:%d(WEP40-1 "
+			"WEP104-5 TKIP-2 AES-4) camid:%d\n",
 			pparm->algorithm, cam_id);
-	if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE)
-	{
-
+	if ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) {
 		struct sta_info *psta;
 		struct sta_priv *pstapriv = &padapter->stapriv;
 
-		if (pparm->algorithm == _NO_PRIVACY_)	/*  clear cam entry */
-		{
+		if (pparm->algorithm == 0) {	/*  clear cam entry */
 			clear_cam_entry23a(padapter, pparm->id);
 			return H2C_SUCCESS_RSP;
 		}
 
 		psta = rtw_get_stainfo23a(pstapriv, pparm->addr);
-		if (psta)
-		{
-			ctrl = (BIT(15) | ((pparm->algorithm) << 2));
+		if (psta) {
+			ctrl = BIT(15) | (pparm->algorithm << 2);
 
-			DBG_8723A("r871x_set_stakey_hdl23a(): enc_algorithm =%d\n", pparm->algorithm);
+			DBG_8723A("r871x_set_stakey_hdl23a(): enc_algorithm "
+				  "=%d\n", pparm->algorithm);
 
-			if ((psta->mac_id<1) || (psta->mac_id>(NUM_STA-4)))
-			{
-				DBG_8723A("r871x_set_stakey_hdl23a():set_stakey failed, mac_id(aid) =%d\n", psta->mac_id);
+			if (psta->mac_id < 1 || psta->mac_id > (NUM_STA - 4)) {
+				DBG_8723A("r871x_set_stakey_hdl23a():set_stakey"
+					  " failed, mac_id(aid) =%d\n",
+					  psta->mac_id);
 				return H2C_REJECTED;
 			}
 
-			cam_id = (psta->mac_id + 3);/* 0~3 for default key, cmd_id = macid + 3, macid = aid+1; */
+			/* 0~3 for default key, cmd_id = macid + 3,
+			   macid = aid+1; */
+			cam_id = (psta->mac_id + 3);
 
-			DBG_8723A("Write CAM, mac_addr =%x:%x:%x:%x:%x:%x, cam_entry =%d\n", pparm->addr[0],
-						pparm->addr[1], pparm->addr[2], pparm->addr[3], pparm->addr[4],
-						pparm->addr[5], cam_id);
+			DBG_8723A("Write CAM, mac_addr =%x:%x:%x:%x:%x:%x, "
+				  "cam_entry =%d\n", pparm->addr[0],
+				  pparm->addr[1], pparm->addr[2],
+				  pparm->addr[3], pparm->addr[4],
+				  pparm->addr[5], cam_id);
 
-			write_cam23a(padapter, cam_id, ctrl, pparm->addr, pparm->key);
+			rtl8723a_cam_write(padapter, cam_id, ctrl,
+					   pparm->addr, pparm->key);
 
 			return H2C_SUCCESS_RSP;
-
-		}
-		else
-		{
-			DBG_8723A("r871x_set_stakey_hdl23a(): sta has been free\n");
+		} else {
+			DBG_8723A("r871x_set_stakey_hdl23a(): sta has been "
+				  "free\n");
 			return H2C_REJECTED;
 		}
-
 	}
 
 	/* below for sta mode */
 
-	if (pparm->algorithm == _NO_PRIVACY_)	/*  clear cam entry */
-	{
+	if (pparm->algorithm == 0) {	/*  clear cam entry */
 		clear_cam_entry23a(padapter, pparm->id);
 		return H2C_SUCCESS;
 	}
 
-	ctrl = BIT(15) | ((pparm->algorithm) << 2);
+	ctrl = BIT(15) | (pparm->algorithm << 2);
 
-	write_cam23a(padapter, cam_id, ctrl, pparm->addr, pparm->key);
+	rtl8723a_cam_write(padapter, cam_id, ctrl, pparm->addr, pparm->key);
 
 	pmlmeinfo->enc_algo = pparm->algorithm;
 
 	return H2C_SUCCESS;
 }
 
-u8 add_ba_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
+int add_ba_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
-	struct addBaReq_parm	*pparm = (struct addBaReq_parm *)pbuf;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	const struct addBaReq_parm *pparm = (struct addBaReq_parm *)pbuf;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
+	struct sta_info *psta;
 
-	struct sta_info *psta = rtw_get_stainfo23a(&padapter->stapriv, pparm->addr);
+	psta = rtw_get_stainfo23a(&padapter->stapriv, pparm->addr);
 
 	if (!psta)
 		return	H2C_SUCCESS;
 
 	if (((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) &&
-	     (pmlmeinfo->HT_enable)) ||
-	    ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE)) {
+	     pmlmeinfo->HT_enable) ||
+	    (pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) {
 		issue_action_BA23a(padapter, pparm->addr,
-				WLAN_ACTION_ADDBA_REQ, (u16)pparm->tid);
+				   WLAN_ACTION_ADDBA_REQ, (u16)pparm->tid);
 		mod_timer(&psta->addba_retry_timer,
 			  jiffies + msecs_to_jiffies(ADDBA_TO));
-	} else {
-		psta->htpriv.candidate_tid_bitmap &= ~CHKBIT(pparm->tid);
-	}
+	} else
+		psta->htpriv.candidate_tid_bitmap &= ~BIT(pparm->tid);
+
 	return	H2C_SUCCESS;
 }
 
-u8 set_tx_beacon_cmd23a(struct rtw_adapter* padapter)
+int set_tx_beacon_cmd23a(struct rtw_adapter* padapter)
 {
-	struct cmd_obj	*ph2c;
+	struct cmd_obj *ph2c;
 	struct Tx_Beacon_param	*ptxBeacon_parm;
 	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
 	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
-	u8	res = _SUCCESS;
+	u8 res = _SUCCESS;
 	int len_diff = 0;
 
-
-
 	ph2c = (struct cmd_obj *)kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
 	if (!ph2c) {
 		res = _FAIL;
@@ -9794,61 +6400,52 @@ u8 set_tx_beacon_cmd23a(struct rtw_adapter* padapter)
 		pmlmeinfo->hidden_ssid_mode);
 	ptxBeacon_parm->network.IELength += len_diff;
 
-	init_h2fwcmd_w_parm_no_rsp(ph2c, ptxBeacon_parm, GEN_CMD_CODE(_TX_Beacon));
+	init_h2fwcmd_w_parm_no_rsp(ph2c, ptxBeacon_parm,
+				   GEN_CMD_CODE(_TX_Beacon));
 
 	res = rtw_enqueue_cmd23a(pcmdpriv, ph2c);
 
 exit:
-
-
-
 	return res;
 }
 
-u8 mlme_evt_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
+int mlme_evt_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
 	u8 evt_code, evt_seq;
 	u16 evt_sz;
-	uint	*peventbuf;
-	void (*event_callback)(struct rtw_adapter *dev, u8 *pbuf);
-	struct evt_priv *pevt_priv = &padapter->evtpriv;
+	const struct C2HEvent_Header *c2h;
+	void (*event_callback)(struct rtw_adapter *dev, const u8 *pbuf);
 
-	peventbuf = (uint*)pbuf;
-	evt_sz = (u16)(*peventbuf&0xffff);
-	evt_seq = (u8)((*peventbuf>>24)&0x7f);
-	evt_code = (u8)((*peventbuf>>16)&0xff);
+	c2h = (struct C2HEvent_Header *)pbuf;
+	evt_sz = c2h->len;
+	evt_seq = c2h->seq;
+	evt_code = c2h->ID;
 
 	/*  checking if event code is valid */
 	if (evt_code >= MAX_C2HEVT) {
-		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nEvent Code(%d) mismatch!\n", evt_code));
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
+			 ("\nEvent Code(%d) mismatch!\n", evt_code));
 		goto _abort_event_;
 	}
 
 	/*  checking if event size match the event parm size */
-	if ((wlanevents[evt_code].parmsize != 0) &&
-	    (wlanevents[evt_code].parmsize != evt_sz)) {
-		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nEvent(%d) Parm Size mismatch (%d vs %d)!\n",
-			evt_code, wlanevents[evt_code].parmsize, evt_sz));
+	if (wlanevents[evt_code].parmsize != 0 &&
+	    wlanevents[evt_code].parmsize != evt_sz) {
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_,
+			 ("\nEvent(%d) Parm Size mismatch (%d vs %d)!\n",
+			  evt_code, wlanevents[evt_code].parmsize, evt_sz));
 		goto _abort_event_;
 	}
 
-	atomic_inc(&pevt_priv->event_seq);
-
-	peventbuf += 2;
-
-	if (peventbuf) {
-		event_callback = wlanevents[evt_code].event_callback;
-		event_callback(padapter, (u8*)peventbuf);
-
-		pevt_priv->evt_done_cnt++;
-	}
+	event_callback = wlanevents[evt_code].event_callback;
+	event_callback(padapter, pbuf + sizeof(struct C2HEvent_Header));
 
 _abort_event_:
 
 	return H2C_SUCCESS;
 }
 
-u8 h2c_msg_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
+int h2c_msg_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
 	if (!pbuf)
 		return H2C_PARAMETERS_ERROR;
@@ -9856,16 +6453,14 @@ u8 h2c_msg_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
 	return H2C_SUCCESS;
 }
 
-u8 tx_beacon_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
+int tx_beacon_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
-	if (send_beacon23a(padapter) == _FAIL)
-	{
+	if (send_beacon23a(padapter) == _FAIL) {
 		DBG_8723A("issue_beacon23a, fail!\n");
 		return H2C_PARAMETERS_ERROR;
 	}
 #ifdef CONFIG_8723AU_AP_MODE
-	else /* tx bc/mc frames after update TIM */
-	{
+	else { /* tx bc/mc frames after update TIM */
 		struct sta_info *psta_bmc;
 		struct list_head *plist, *phead, *ptmp;
 		struct xmit_frame *pxmitframe;
@@ -9877,8 +6472,7 @@ u8 tx_beacon_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
 		if (!psta_bmc)
 			return H2C_SUCCESS;
 
-		if ((pstapriv->tim_bitmap&BIT(0)) && (psta_bmc->sleepq_len>0))
-		{
+		if (pstapriv->tim_bitmap & BIT(0) && psta_bmc->sleepq_len > 0) {
 			msleep(10);/*  10ms, ATIM(HIQ) Windows */
 			/* spin_lock_bh(&psta_bmc->sleep_q.lock); */
 			spin_lock_bh(&pxmitpriv->lock);
@@ -9902,22 +6496,22 @@ u8 tx_beacon_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
 
 				pxmitframe->attrib.qsel = 0x11;/* HIQ */
 
-				rtw_hal_xmit23aframe_enqueue(padapter, pxmitframe);
+				rtl8723au_hal_xmitframe_enqueue(padapter,
+								pxmitframe);
 			}
 
 			/* spin_unlock_bh(&psta_bmc->sleep_q.lock); */
 			spin_unlock_bh(&pxmitpriv->lock);
 		}
-
 	}
 #endif
 
 	return H2C_SUCCESS;
 }
 
-u8 set_ch_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
+int set_ch_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
-	struct set_ch_parm *set_ch_parm;
+	const struct set_ch_parm *set_ch_parm;
 	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
 
 	if (!pbuf)
@@ -9925,22 +6519,23 @@ u8 set_ch_hdl23a(struct rtw_adapter *padapter, u8 *pbuf)
 
 	set_ch_parm = (struct set_ch_parm *)pbuf;
 
-	DBG_8723A(FUNC_NDEV_FMT" ch:%u, bw:%u, ch_offset:%u\n",
-		FUNC_NDEV_ARG(padapter->pnetdev),
-		set_ch_parm->ch, set_ch_parm->bw, set_ch_parm->ch_offset);
+	DBG_8723A("%s(%s): ch:%u, bw:%u, ch_offset:%u\n", __func__,
+		  padapter->pnetdev->name, set_ch_parm->ch,
+		  set_ch_parm->bw, set_ch_parm->ch_offset);
 
 	pmlmeext->cur_channel = set_ch_parm->ch;
 	pmlmeext->cur_ch_offset = set_ch_parm->ch_offset;
 	pmlmeext->cur_bwmode = set_ch_parm->bw;
 
-	set_channel_bwmode23a(padapter, set_ch_parm->ch, set_ch_parm->ch_offset, set_ch_parm->bw);
+	set_channel_bwmode23a(padapter, set_ch_parm->ch,
+			      set_ch_parm->ch_offset, set_ch_parm->bw);
 
 	return	H2C_SUCCESS;
 }
 
-u8 set_chplan_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
+int set_chplan_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
-	struct SetChannelPlan_param *setChannelPlan_param;
+	const struct SetChannelPlan_param *setChannelPlan_param;
 	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
 
 	if (!pbuf)
@@ -9948,13 +6543,16 @@ u8 set_chplan_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
 
 	setChannelPlan_param = (struct SetChannelPlan_param *)pbuf;
 
-	pmlmeext->max_chan_nums = init_channel_set(padapter, setChannelPlan_param->channel_plan, pmlmeext->channel_set);
-	init_channel_list(padapter, pmlmeext->channel_set, pmlmeext->max_chan_nums, &pmlmeext->channel_list);
+	pmlmeext->max_chan_nums =
+		init_channel_set(padapter, setChannelPlan_param->channel_plan,
+				 pmlmeext->channel_set);
+	init_channel_list(padapter, pmlmeext->channel_set,
+			  pmlmeext->max_chan_nums, &pmlmeext->channel_list);
 
 	return	H2C_SUCCESS;
 }
 
-u8 led_blink_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
+int led_blink_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
 	struct LedBlink_param *ledBlink_param;
 
@@ -9966,25 +6564,28 @@ u8 led_blink_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
 	return	H2C_SUCCESS;
 }
 
-u8 set_csa_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
+int set_csa_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
 	return	H2C_REJECTED;
 }
 
 /*  TDLS_WRCR		: write RCR DATA BIT */
 /*  TDLS_SD_PTI		: issue peer traffic indication */
-/*  TDLS_CS_OFF		: go back to the channel linked with AP, terminating channel switch procedure */
-/*  TDLS_INIT_CH_SEN	: init channel sensing, receive all data and mgnt frame */
-/*  TDLS_DONE_CH_SEN: channel sensing and report candidate channel */
+/*  TDLS_CS_OFF		: go back to the channel linked with AP,
+			  terminating channel switch procedure */
+/*  TDLS_INIT_CH_SEN	: init channel sensing, receive all data and
+			  mgnt frame */
+/*  TDLS_DONE_CH_SEN	: channel sensing and report candidate channel */
 /*  TDLS_OFF_CH		: first time set channel to off channel */
-/*  TDLS_BASE_CH		: go back tp the channel linked with AP when set base channel as target channel */
+/*  TDLS_BASE_CH	: go back tp the channel linked with AP when set
+			  base channel as target channel */
 /*  TDLS_P_OFF_CH	: periodically go to off channel */
 /*  TDLS_P_BASE_CH	: periodically go back to base channel */
 /*  TDLS_RS_RCR		: restore RCR */
 /*  TDLS_CKALV_PH1	: check alive timer phase1 */
 /*  TDLS_CKALV_PH2	: check alive timer phase2 */
 /*  TDLS_FREE_STA	: free tdls sta */
-u8 tdls_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf)
+int tdls_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 {
 	return H2C_REJECTED;
 }
diff --git a/drivers/staging/rtl8723au/core/rtw_p2p.c b/drivers/staging/rtl8723au/core/rtw_p2p.c
deleted file mode 100644
index 27a6cc76973..00000000000
--- a/drivers/staging/rtl8723au/core/rtw_p2p.c
+++ /dev/null
@@ -1,4001 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
- *
- * 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.
- *
- * 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 _RTW_P2P_C_
-
-#include <drv_types.h>
-#include <rtw_p2p.h>
-#include <wifi.h>
-
-#ifdef CONFIG_8723AU_P2P
-
-static int rtw_p2p_is_channel_list_ok(u8 desired_ch, u8* ch_list, u8 ch_cnt)
-{
-	int found = 0, i = 0;
-
-	for (i = 0; i < ch_cnt; i++)
-	{
-		if (ch_list[ i ] == desired_ch)
-		{
-			found = 1;
-			break;
-		}
-	}
-	return found;
-}
-
-static int is_any_client_associated(struct rtw_adapter *padapter)
-{
-	return padapter->stapriv.asoc_list_cnt ? true : false;
-}
-
-static u32 go_add_group_info_attr(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	struct list_head *phead, *plist;
-	u32 len = 0;
-	u16 attr_len = 0;
-	u8 tmplen, *pdata_attr, *pstart, *pcur;
-	struct sta_info *psta;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct sta_priv *pstapriv = &padapter->stapriv;
-
-	DBG_8723A("%s\n", __func__);
-
-	pdata_attr = kzalloc(MAX_P2P_IE_LEN, GFP_ATOMIC);
-
-	pstart = pdata_attr;
-	pcur = pdata_attr;
-
-	spin_lock_bh(&pstapriv->asoc_list_lock);
-	phead = &pstapriv->asoc_list;
-
-	list_for_each(plist, phead) {
-		psta = container_of(plist, struct sta_info, asoc_list);
-
-		if (psta->is_p2p_device)
-		{
-			tmplen = 0;
-
-			pcur++;
-
-			/* P2P device address */
-			memcpy(pcur, psta->dev_addr, ETH_ALEN);
-			pcur += ETH_ALEN;
-
-			/* P2P interface address */
-			memcpy(pcur, psta->hwaddr, ETH_ALEN);
-			pcur += ETH_ALEN;
-
-			*pcur = psta->dev_cap;
-			pcur++;
-
-			/* u16*)(pcur) = cpu_to_be16(psta->config_methods); */
-			put_unaligned_be16(psta->config_methods, pcur);
-			pcur += 2;
-
-			memcpy(pcur, psta->primary_dev_type, 8);
-			pcur += 8;
-
-			*pcur = psta->num_of_secdev_type;
-			pcur++;
-
-			memcpy(pcur, psta->secdev_types_list, psta->num_of_secdev_type*8);
-			pcur += psta->num_of_secdev_type*8;
-
-			if (psta->dev_name_len>0)
-			{
-				/* u16*)(pcur) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); */
-				put_unaligned_be16(WPS_ATTR_DEVICE_NAME, pcur);
-				pcur += 2;
-
-				/* u16*)(pcur) = cpu_to_be16(psta->dev_name_len); */
-				put_unaligned_be16(psta->dev_name_len, pcur);
-				pcur += 2;
-
-				memcpy(pcur, psta->dev_name, psta->dev_name_len);
-				pcur += psta->dev_name_len;
-			}
-
-			tmplen = (u8)(pcur-pstart);
-
-			*pstart = (tmplen-1);
-
-			attr_len += tmplen;
-
-			/* pstart += tmplen; */
-			pstart = pcur;
-
-		}
-
-	}
-	spin_unlock_bh(&pstapriv->asoc_list_lock);
-
-	if (attr_len>0)
-	{
-		len = rtw_set_p2p_attr_content23a(pbuf, P2P_ATTR_GROUP_INFO, attr_len, pdata_attr);
-	}
-
-	kfree(pdata_attr);
-
-	return len;
-}
-
-static void issue_group_disc_req(struct wifidirect_info *pwdinfo, u8 *da)
-{
-	struct xmit_frame			*pmgntframe;
-	struct pkt_attrib			*pattrib;
-	unsigned char					*pframe;
-	struct ieee80211_hdr	*pwlanhdr;
-	unsigned short				*fctrl;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	unsigned char category = WLAN_CATEGORY_VENDOR_SPECIFIC;/* P2P action frame */
-	u32	p2poui = cpu_to_be32(P2POUI);
-	u8	oui_subtype = P2P_GO_DISC_REQUEST;
-	u8	dialogToken = 0;
-
-	DBG_8723A("[%s]\n", __func__);
-
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
-	{
-		return;
-	}
-
-	/* update attribute */
-	pattrib = &pmgntframe->attrib;
-	update_mgntframe_attrib23a(padapter, pattrib);
-
-	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
-
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
-	pwlanhdr = (struct ieee80211_hdr *)pframe;
-
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
-
-	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
-	memcpy(pwlanhdr->addr2, pwdinfo->interface_addr, ETH_ALEN);
-	memcpy(pwlanhdr->addr3, pwdinfo->interface_addr, ETH_ALEN);
-
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
-	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ACTION);
-
-	pframe += sizeof(struct ieee80211_hdr_3addr);
-	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
-
-	/* Build P2P action frame header */
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *)&p2poui, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &dialogToken, &pattrib->pktlen);
-
-	/* there is no IE in this P2P action frame */
-
-	pattrib->last_txcmdsz = pattrib->pktlen;
-
-	dump_mgntframe23a(padapter, pmgntframe);
-}
-
-static void issue_p2p_devdisc_resp(struct wifidirect_info *pwdinfo, u8 *da, u8 status, u8 dialogToken)
-{
-	struct xmit_frame			*pmgntframe;
-	struct pkt_attrib			*pattrib;
-	unsigned char					*pframe;
-	struct ieee80211_hdr	*pwlanhdr;
-	unsigned short				*fctrl;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	unsigned char category = WLAN_CATEGORY_PUBLIC;
-	u8			action = P2P_PUB_ACTION_ACTION;
-	u32			p2poui = cpu_to_be32(P2POUI);
-	u8			oui_subtype = P2P_DEVDISC_RESP;
-	u8 p2pie[8] = { 0x00 };
-	u32 p2pielen = 0;
-
-	DBG_8723A("[%s]\n", __func__);
-
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
-	{
-		return;
-	}
-
-	/* update attribute */
-	pattrib = &pmgntframe->attrib;
-	update_mgntframe_attrib23a(padapter, pattrib);
-
-	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
-
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
-	pwlanhdr = (struct ieee80211_hdr *)pframe;
-
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
-
-	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
-	memcpy(pwlanhdr->addr2, pwdinfo->device_addr, ETH_ALEN);
-	memcpy(pwlanhdr->addr3, pwdinfo->device_addr, ETH_ALEN);
-
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
-	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ACTION);
-
-	pframe += sizeof(struct ieee80211_hdr_3addr);
-	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
-
-	/* Build P2P public action frame header */
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *) &p2poui,
-				     &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &dialogToken, &pattrib->pktlen);
-
-	/* Build P2P IE */
-	/*	P2P OUI */
-	p2pielen = 0;
-	p2pie[ p2pielen++ ] = 0x50;
-	p2pie[ p2pielen++ ] = 0x6F;
-	p2pie[ p2pielen++ ] = 0x9A;
-	p2pie[ p2pielen++ ] = 0x09;	/*	WFA P2P v1.0 */
-
-	/*  P2P_ATTR_STATUS */
-	p2pielen += rtw_set_p2p_attr_content23a(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status);
-
-	pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, p2pie, &pattrib->pktlen);
-
-	pattrib->last_txcmdsz = pattrib->pktlen;
-
-	dump_mgntframe23a(padapter, pmgntframe);
-}
-
-static void issue_p2p_provision_resp(struct wifidirect_info *pwdinfo, u8* raddr, u8* frame_body, u16 config_method)
-{
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	unsigned char category = WLAN_CATEGORY_PUBLIC;
-	u8			action = P2P_PUB_ACTION_ACTION;
-	u8			dialogToken = frame_body[7];	/*	The Dialog Token of provisioning discovery request frame. */
-	u32			p2poui = cpu_to_be32(P2POUI);
-	u8			oui_subtype = P2P_PROVISION_DISC_RESP;
-	u8			wpsie[ 100 ] = { 0x00 };
-	u8			wpsielen = 0;
-#ifdef CONFIG_8723AU_P2P
-	u32					wfdielen = 0;
-#endif /* CONFIG_8723AU_P2P */
-
-	struct xmit_frame			*pmgntframe;
-	struct pkt_attrib			*pattrib;
-	unsigned char					*pframe;
-	struct ieee80211_hdr	*pwlanhdr;
-	unsigned short				*fctrl;
-	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
-		return;
-
-	/* update attribute */
-	pattrib = &pmgntframe->attrib;
-	update_mgntframe_attrib23a(padapter, pattrib);
-
-	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
-
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
-	pwlanhdr = (struct ieee80211_hdr *)pframe;
-
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
-
-	memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
-	memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
-	memcpy(pwlanhdr->addr3, myid(&padapter->eeprompriv), ETH_ALEN);
-
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
-	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ACTION);
-
-	pframe += sizeof(struct ieee80211_hdr_3addr);
-	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
-
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *) &p2poui,
-				     &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &dialogToken, &pattrib->pktlen);
-
-	wpsielen = 0;
-	/*	WPS OUI */
-	/* u32*) (wpsie) = cpu_to_be32(WPSOUI); */
-	put_unaligned_be32(WPSOUI, wpsie);
-	wpsielen += 4;
-
-	/*	Config Method */
-	/*	Type: */
-	/* u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD); */
-	put_unaligned_be16(WPS_ATTR_CONF_METHOD, wpsie + wpsielen);
-	wpsielen += 2;
-
-	/*	Length: */
-	/* u16*) (wpsie + wpsielen) = cpu_to_be16(0x0002); */
-	put_unaligned_be16(0x0002, wpsie + wpsielen);
-	wpsielen += 2;
-
-	/*	Value: */
-	/* u16*) (wpsie + wpsielen) = cpu_to_be16(config_method); */
-	put_unaligned_be16(config_method, wpsie + wpsielen);
-	wpsielen += 2;
-
-	pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen);
-
-#ifdef CONFIG_8723AU_P2P
-	wfdielen = build_provdisc_resp_wfd_ie(pwdinfo, pframe);
-	pframe += wfdielen;
-	pattrib->pktlen += wfdielen;
-#endif /* CONFIG_8723AU_P2P */
-
-	pattrib->last_txcmdsz = pattrib->pktlen;
-
-	dump_mgntframe23a(padapter, pmgntframe);
-
-	return;
-}
-
-static void issue_p2p_presence_resp(struct wifidirect_info *pwdinfo, u8 *da, u8 status, u8 dialogToken)
-{
-	struct xmit_frame			*pmgntframe;
-	struct pkt_attrib			*pattrib;
-	unsigned char					*pframe;
-	struct ieee80211_hdr	*pwlanhdr;
-	unsigned short				*fctrl;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	unsigned char category = WLAN_CATEGORY_VENDOR_SPECIFIC;/* P2P action frame */
-	u32	p2poui = cpu_to_be32(P2POUI);
-	u8	oui_subtype = P2P_PRESENCE_RESPONSE;
-	u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 };
-	u8 noa_attr_content[32] = { 0x00 };
-	u32 p2pielen = 0;
-
-	DBG_8723A("[%s]\n", __func__);
-
-	if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL)
-	{
-		return;
-	}
-
-	/* update attribute */
-	pattrib = &pmgntframe->attrib;
-	update_mgntframe_attrib23a(padapter, pattrib);
-
-	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
-
-	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
-	pwlanhdr = (struct ieee80211_hdr *)pframe;
-
-	fctrl = &pwlanhdr->frame_control;
-	*fctrl = 0;
-
-	memcpy(pwlanhdr->addr1, da, ETH_ALEN);
-	memcpy(pwlanhdr->addr2, pwdinfo->interface_addr, ETH_ALEN);
-	memcpy(pwlanhdr->addr3, pwdinfo->interface_addr, ETH_ALEN);
-
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
-	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ACTION);
-
-	pframe += sizeof(struct ieee80211_hdr_3addr);
-	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
-
-	/* Build P2P action frame header */
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *) &p2poui,
-				     &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &dialogToken, &pattrib->pktlen);
-
-	/* Add P2P IE header */
-	/*	P2P OUI */
-	p2pielen = 0;
-	p2pie[ p2pielen++ ] = 0x50;
-	p2pie[ p2pielen++ ] = 0x6F;
-	p2pie[ p2pielen++ ] = 0x9A;
-	p2pie[ p2pielen++ ] = 0x09;	/*	WFA P2P v1.0 */
-
-	/* Add Status attribute in P2P IE */
-	p2pielen += rtw_set_p2p_attr_content23a(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status);
-
-	/* Add NoA attribute in P2P IE */
-	noa_attr_content[0] = 0x1;/* index */
-	noa_attr_content[1] = 0x0;/* CTWindow and OppPS Parameters */
-
-	/* todo: Notice of Absence Descriptor(s) */
-
-	p2pielen += rtw_set_p2p_attr_content23a(&p2pie[p2pielen], P2P_ATTR_NOA, 2, noa_attr_content);
-
-	pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, p2pie,
-			       &pattrib->pktlen);
-
-	pattrib->last_txcmdsz = pattrib->pktlen;
-
-	dump_mgntframe23a(padapter, pmgntframe);
-}
-
-u32 build_beacon_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 };
-	u16 capability = 0;
-	u32 len = 0, p2pielen = 0;
-
-	/*	P2P OUI */
-	p2pielen = 0;
-	p2pie[ p2pielen++ ] = 0x50;
-	p2pie[ p2pielen++ ] = 0x6F;
-	p2pie[ p2pielen++ ] = 0x9A;
-	p2pie[ p2pielen++ ] = 0x09;	/*	WFA P2P v1.0 */
-
-	/*	According to the P2P Specification, the beacon frame should contain 3 P2P attributes */
-	/*	1. P2P Capability */
-	/*	2. P2P Device ID */
-	/*	3. Notice of Absence (NOA) */
-
-	/*	P2P Capability ATTR */
-	/*	Type: */
-	/*	Length: */
-	/*	Value: */
-	/*	Device Capability Bitmap, 1 byte */
-	/*	Be able to participate in additional P2P Groups and */
-	/*	support the P2P Invitation Procedure */
-	/*	Group Capability Bitmap, 1 byte */
-	capability = P2P_DEVCAP_INVITATION_PROC|P2P_DEVCAP_CLIENT_DISCOVERABILITY;
-	capability |=  ((P2P_GRPCAP_GO | P2P_GRPCAP_INTRABSS) << 8);
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_PROVISIONING_ING))
-		capability |= (P2P_GRPCAP_GROUP_FORMATION<<8);
-
-	capability = cpu_to_le16(capability);
-
-	p2pielen += rtw_set_p2p_attr_content23a(&p2pie[p2pielen], P2P_ATTR_CAPABILITY, 2, (u8*)&capability);
-
-	/*  P2P Device ID ATTR */
-	p2pielen += rtw_set_p2p_attr_content23a(&p2pie[p2pielen], P2P_ATTR_DEVICE_ID, ETH_ALEN, pwdinfo->device_addr);
-
-	/*  Notice of Absence ATTR */
-	/*	Type: */
-	/*	Length: */
-	/*	Value: */
-
-	/* go_add_noa_attr(pwdinfo); */
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len);
-
-	return len;
-}
-
-#ifdef CONFIG_8723AU_P2P
-u32 build_beacon_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
-	u32 len = 0, wfdielen = 0;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
-	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
-
-	/*	WFD OUI */
-	wfdielen = 0;
-	wfdie[ wfdielen++ ] = 0x50;
-	wfdie[ wfdielen++ ] = 0x6F;
-	wfdie[ wfdielen++ ] = 0x9A;
-	wfdie[ wfdielen++ ] = 0x0A;	/*	WFA WFD v1.0 */
-
-	/*	Commented by Albert 20110812 */
-	/*	According to the WFD Specification, the beacon frame should contain 4 WFD attributes */
-	/*	1. WFD Device Information */
-	/*	2. Associated BSSID */
-	/*	3. Coupled Sink Information */
-
-	/*	WFD Device Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value1: */
-	/*	WFD device information */
-
-	if (P2P_ROLE_GO == pwdinfo->role)
-	{
-		if (is_any_client_associated(pwdinfo->padapter))
-		{
-			/*	WFD primary sink + WiFi Direct mode + WSD (WFD Service Discovery) */
-			put_unaligned_be16(pwfd_info->wfd_device_type |
-					   WFD_DEVINFO_WSD, wfdie + wfdielen);
-		}
-		else
-		{
-			/*	WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
-			put_unaligned_be16(pwfd_info->wfd_device_type |
-					   WFD_DEVINFO_SESSION_AVAIL |
-					   WFD_DEVINFO_WSD, wfdie + wfdielen);
-		}
-
-	}
-	else
-	{
-		/*	WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
-		put_unaligned_be16(pwfd_info->wfd_device_type |
-				   WFD_DEVINFO_SESSION_AVAIL |
-				   WFD_DEVINFO_WSD, wfdie + wfdielen);
-	}
-
-	wfdielen += 2;
-
-	/*	Value2: */
-	/*	Session Management Control Port */
-	/*	Default TCP port for RTSP messages is 554 */
-	put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value3: */
-	/*	WFD Device Maximum Throughput */
-	/*	300Mbps is the maximum throughput */
-	put_unaligned_be16(300, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Associated BSSID ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Associated BSSID */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-	{
-		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
-	}
-	else
-	{
-		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
-	}
-
-	wfdielen += ETH_ALEN;
-
-	/*	Coupled Sink Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0007, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Coupled Sink Status bitmap */
-	/*	Not coupled/available for Coupling */
-	wfdie[ wfdielen++ ] = 0;
-	/*   MAC Addr. */
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
-
-	return len;
-}
-
-u32 build_probe_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
-	u32 len = 0, wfdielen = 0;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
-	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
-
-	/*	WFD OUI */
-	wfdielen = 0;
-	wfdie[ wfdielen++ ] = 0x50;
-	wfdie[ wfdielen++ ] = 0x6F;
-	wfdie[ wfdielen++ ] = 0x9A;
-	wfdie[ wfdielen++ ] = 0x0A;	/*	WFA WFD v1.0 */
-
-	/*	Commented by Albert 20110812 */
-	/*	According to the WFD Specification, the probe request frame should contain 4 WFD attributes */
-	/*	1. WFD Device Information */
-	/*	2. Associated BSSID */
-	/*	3. Coupled Sink Information */
-
-	/*	WFD Device Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value1: */
-	/*	WFD device information */
-
-	if (1 == pwdinfo->wfd_tdls_enable)
-	{
-		/*	WFD primary sink + available for WFD session + WiFi TDLS mode + WSC (WFD Service Discovery) */
-		put_unaligned_be16(pwfd_info->wfd_device_type |
-				   WFD_DEVINFO_SESSION_AVAIL |
-				   WFD_DEVINFO_WSD |
-				   WFD_DEVINFO_PC_TDLS, wfdie + wfdielen);
-	}
-	else
-	{
-		/*	WFD primary sink + available for WFD session + WiFi Direct mode + WSC (WFD Service Discovery) */
-		put_unaligned_be16(pwfd_info->wfd_device_type |
-				   WFD_DEVINFO_SESSION_AVAIL |
-				   WFD_DEVINFO_WSD, wfdie + wfdielen);
-	}
-
-	wfdielen += 2;
-
-	/*	Value2: */
-	/*	Session Management Control Port */
-	/*	Default TCP port for RTSP messages is 554 */
-	put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value3: */
-	/*	WFD Device Maximum Throughput */
-	/*	300Mbps is the maximum throughput */
-	put_unaligned_be16(300, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Associated BSSID ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Associated BSSID */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-	{
-		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
-	}
-	else
-	{
-		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
-	}
-
-	wfdielen += ETH_ALEN;
-
-	/*	Coupled Sink Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0007, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Coupled Sink Status bitmap */
-	/*	Not coupled/available for Coupling */
-	wfdie[ wfdielen++ ] = 0;
-	/*   MAC Addr. */
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
-
-	return len;
-}
-
-u32 build_probe_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf, u8 tunneled)
-{
-	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
-	u32 len = 0, wfdielen = 0;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
-	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
-
-	/*	WFD OUI */
-	wfdielen = 0;
-	wfdie[ wfdielen++ ] = 0x50;
-	wfdie[ wfdielen++ ] = 0x6F;
-	wfdie[ wfdielen++ ] = 0x9A;
-	wfdie[ wfdielen++ ] = 0x0A;	/*	WFA WFD v1.0 */
-
-	/*	Commented by Albert 20110812 */
-	/*	According to the WFD Specification, the probe response frame should contain 4 WFD attributes */
-	/*	1. WFD Device Information */
-	/*	2. Associated BSSID */
-	/*	3. Coupled Sink Information */
-	/*	4. WFD Session Information */
-
-	/*	WFD Device Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value1: */
-	/*	WFD device information */
-	/*	WFD primary sink + available for WFD session + WiFi Direct mode */
-
-	if (true == pwdinfo->session_available)
-	{
-		if (P2P_ROLE_GO == pwdinfo->role)
-		{
-			if (is_any_client_associated(pwdinfo->padapter))
-			{
-				if (pwdinfo->wfd_tdls_enable)
-				{
-					/*	TDLS mode + WSD (WFD Service Discovery) */
-					put_unaligned_be16(pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_HDCP_SUPPORT, wfdie + wfdielen);
-				}
-				else
-				{
-					/*	WiFi Direct mode + WSD (WFD Service Discovery) */
-					put_unaligned_be16(pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_HDCP_SUPPORT, wfdie + wfdielen);
-				}
-			}
-			else
-			{
-				if (pwdinfo->wfd_tdls_enable)
-				{
-					/*	available for WFD session + TDLS mode + WSD (WFD Service Discovery) */
-					put_unaligned_be16(pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD | WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_HDCP_SUPPORT, wfdie + wfdielen);
-				}
-				else
-				{
-					/*	available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
-					put_unaligned_be16(pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD | WFD_DEVINFO_HDCP_SUPPORT, wfdie + wfdielen);
-				}
-			}
-		}
-		else
-		{
-			if (pwdinfo->wfd_tdls_enable)
-			{
-				/*	available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
-				put_unaligned_be16(pwfd_info->wfd_device_type |
-						   WFD_DEVINFO_SESSION_AVAIL |
-						   WFD_DEVINFO_WSD |
-						   WFD_DEVINFO_PC_TDLS |
-						   WFD_DEVINFO_HDCP_SUPPORT,
-						   wfdie + wfdielen);
-			}
-			else
-			{
-
-				/*	available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
-				put_unaligned_be16(pwfd_info->wfd_device_type |
-						   WFD_DEVINFO_SESSION_AVAIL |
-						   WFD_DEVINFO_WSD |
-						   WFD_DEVINFO_HDCP_SUPPORT,
-						   wfdie + wfdielen);
-			}
-		}
-	}
-	else
-	{
-		if (pwdinfo->wfd_tdls_enable)
-		{
-			put_unaligned_be16(pwfd_info->wfd_device_type |
-					   WFD_DEVINFO_WSD |
-					   WFD_DEVINFO_PC_TDLS |
-					   WFD_DEVINFO_HDCP_SUPPORT,
-					   wfdie + wfdielen);
-		}
-		else
-		{
-			put_unaligned_be16(pwfd_info->wfd_device_type |
-					   WFD_DEVINFO_WSD |
-					   WFD_DEVINFO_HDCP_SUPPORT,
-					   wfdie + wfdielen);
-		}
-
-	}
-
-	wfdielen += 2;
-
-	/*	Value2: */
-	/*	Session Management Control Port */
-	/*	Default TCP port for RTSP messages is 554 */
-	put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value3: */
-	/*	WFD Device Maximum Throughput */
-	/*	300Mbps is the maximum throughput */
-	put_unaligned_be16(300, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Associated BSSID ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Associated BSSID */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-	{
-		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
-	}
-	else
-	{
-		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
-	}
-
-	wfdielen += ETH_ALEN;
-
-	/*	Coupled Sink Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0007, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Coupled Sink Status bitmap */
-	/*	Not coupled/available for Coupling */
-	wfdie[ wfdielen++ ] = 0;
-	/*   MAC Addr. */
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-
-	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
-	{
-		/*	WFD Session Information ATTR */
-		/*	Type: */
-		wfdie[ wfdielen++ ] = WFD_ATTR_SESSION_INFO;
-
-		/*	Length: */
-		/*	Note: In the WFD specification, the size of length field is 2. */
-		put_unaligned_be16(0x0000, wfdie + wfdielen);
-		wfdielen += 2;
-
-		/*	Todo: to add the list of WFD device info descriptor in WFD group. */
-
-	}
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
-
-	return len;
-}
-
-u32 build_assoc_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
-	u32 len = 0, wfdielen = 0;
-	struct rtw_adapter					*padapter = NULL;
-	struct mlme_priv			*pmlmepriv = NULL;
-	struct wifi_display_info		*pwfd_info = NULL;
-
-	/*	WFD OUI */
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE))
-	{
-		return 0;
-	}
-
-	padapter = pwdinfo->padapter;
-	pmlmepriv = &padapter->mlmepriv;
-	pwfd_info = padapter->wdinfo.wfd_info;
-
-	wfdielen = 0;
-	wfdie[ wfdielen++ ] = 0x50;
-	wfdie[ wfdielen++ ] = 0x6F;
-	wfdie[ wfdielen++ ] = 0x9A;
-	wfdie[ wfdielen++ ] = 0x0A;	/*	WFA WFD v1.0 */
-
-	/*	Commented by Albert 20110812 */
-	/*	According to the WFD Specification, the probe request frame should contain 4 WFD attributes */
-	/*	1. WFD Device Information */
-	/*	2. Associated BSSID */
-	/*	3. Coupled Sink Information */
-
-	/*	WFD Device Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value1: */
-	/*	WFD device information */
-	/*	WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
-	put_unaligned_be16(pwfd_info->wfd_device_type |
-			   WFD_DEVINFO_SESSION_AVAIL |
-			   WFD_DEVINFO_WSD, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value2: */
-	/*	Session Management Control Port */
-	/*	Default TCP port for RTSP messages is 554 */
-	put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value3: */
-	/*	WFD Device Maximum Throughput */
-	/*	300Mbps is the maximum throughput */
-	put_unaligned_be16(300, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Associated BSSID ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Associated BSSID */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-	{
-		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
-	}
-	else
-	{
-		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
-	}
-
-	wfdielen += ETH_ALEN;
-
-	/*	Coupled Sink Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0007, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Coupled Sink Status bitmap */
-	/*	Not coupled/available for Coupling */
-	wfdie[ wfdielen++ ] = 0;
-	/*   MAC Addr. */
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
-
-	return len;
-}
-
-u32 build_assoc_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
-	u32 len = 0, wfdielen = 0;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
-	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
-
-	/*	WFD OUI */
-	wfdielen = 0;
-	wfdie[ wfdielen++ ] = 0x50;
-	wfdie[ wfdielen++ ] = 0x6F;
-	wfdie[ wfdielen++ ] = 0x9A;
-	wfdie[ wfdielen++ ] = 0x0A;	/*	WFA WFD v1.0 */
-
-	/*	Commented by Albert 20110812 */
-	/*	According to the WFD Specification, the probe request frame should contain 4 WFD attributes */
-	/*	1. WFD Device Information */
-	/*	2. Associated BSSID */
-	/*	3. Coupled Sink Information */
-
-	/*	WFD Device Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value1: */
-	/*	WFD device information */
-	/*	WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
-	put_unaligned_be16(pwfd_info->wfd_device_type |
-			   WFD_DEVINFO_SESSION_AVAIL |
-			   WFD_DEVINFO_WSD, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value2: */
-	/*	Session Management Control Port */
-	/*	Default TCP port for RTSP messages is 554 */
-	put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value3: */
-	/*	WFD Device Maximum Throughput */
-	/*	300Mbps is the maximum throughput */
-	put_unaligned_be16(300, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Associated BSSID ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Associated BSSID */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-	{
-		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
-	}
-	else
-	{
-		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
-	}
-
-	wfdielen += ETH_ALEN;
-
-	/*	Coupled Sink Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0007, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Coupled Sink Status bitmap */
-	/*	Not coupled/available for Coupling */
-	wfdie[ wfdielen++ ] = 0;
-	/*   MAC Addr. */
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
-
-	return len;
-}
-
-u32 build_nego_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
-	u32 len = 0, wfdielen = 0;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
-	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
-
-	/*	WFD OUI */
-	wfdielen = 0;
-	wfdie[ wfdielen++ ] = 0x50;
-	wfdie[ wfdielen++ ] = 0x6F;
-	wfdie[ wfdielen++ ] = 0x9A;
-	wfdie[ wfdielen++ ] = 0x0A;	/*	WFA WFD v1.0 */
-
-	/*	Commented by Albert 20110825 */
-	/*	According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes */
-	/*	1. WFD Device Information */
-	/*	2. Associated BSSID (Optional) */
-	/*	3. Local IP Adress (Optional) */
-
-	/*	WFD Device Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value1: */
-	/*	WFD device information */
-	/*	WFD primary sink + WiFi Direct mode + WSD (WFD Service Discovery) + WFD Session Available */
-	put_unaligned_be16(pwfd_info->wfd_device_type |
-			   WFD_DEVINFO_WSD | WFD_DEVINFO_SESSION_AVAIL,
-			   wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value2: */
-	/*	Session Management Control Port */
-	/*	Default TCP port for RTSP messages is 554 */
-	put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value3: */
-	/*	WFD Device Maximum Throughput */
-	/*	300Mbps is the maximum throughput */
-	put_unaligned_be16(300, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Associated BSSID ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Associated BSSID */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-	{
-		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
-	}
-	else
-	{
-		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
-	}
-
-	wfdielen += ETH_ALEN;
-
-	/*	Coupled Sink Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0007, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Coupled Sink Status bitmap */
-	/*	Not coupled/available for Coupling */
-	wfdie[ wfdielen++ ] = 0;
-	/*   MAC Addr. */
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
-
-	return len;
-}
-
-u32 build_nego_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
-	u32 len = 0, wfdielen = 0;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
-	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
-
-	/*	WFD OUI */
-	wfdielen = 0;
-	wfdie[ wfdielen++ ] = 0x50;
-	wfdie[ wfdielen++ ] = 0x6F;
-	wfdie[ wfdielen++ ] = 0x9A;
-	wfdie[ wfdielen++ ] = 0x0A;	/*	WFA WFD v1.0 */
-
-	/*	Commented by Albert 20110825 */
-	/*	According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes */
-	/*	1. WFD Device Information */
-	/*	2. Associated BSSID (Optional) */
-	/*	3. Local IP Adress (Optional) */
-
-	/*	WFD Device Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value1: */
-	/*	WFD device information */
-	/*	WFD primary sink + WiFi Direct mode + WSD (WFD Service Discovery) + WFD Session Available */
-	put_unaligned_be16(pwfd_info->wfd_device_type |
-			   WFD_DEVINFO_WSD | WFD_DEVINFO_SESSION_AVAIL,
-			   wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value2: */
-	/*	Session Management Control Port */
-	/*	Default TCP port for RTSP messages is 554 */
-	put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value3: */
-	/*	WFD Device Maximum Throughput */
-	/*	300Mbps is the maximum throughput */
-	put_unaligned_be16(300, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Associated BSSID ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Associated BSSID */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-	{
-		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
-	}
-	else
-	{
-		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
-	}
-
-	wfdielen += ETH_ALEN;
-
-	/*	Coupled Sink Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0007, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Coupled Sink Status bitmap */
-	/*	Not coupled/available for Coupling */
-	wfdie[ wfdielen++ ] = 0;
-	/*   MAC Addr. */
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
-
-	return len;
-}
-
-u32 build_nego_confirm_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
-	u32 len = 0, wfdielen = 0;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
-	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
-
-	/*	WFD OUI */
-	wfdielen = 0;
-	wfdie[ wfdielen++ ] = 0x50;
-	wfdie[ wfdielen++ ] = 0x6F;
-	wfdie[ wfdielen++ ] = 0x9A;
-	wfdie[ wfdielen++ ] = 0x0A;	/*	WFA WFD v1.0 */
-
-	/*	Commented by Albert 20110825 */
-	/*	According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes */
-	/*	1. WFD Device Information */
-	/*	2. Associated BSSID (Optional) */
-	/*	3. Local IP Adress (Optional) */
-
-	/*	WFD Device Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value1: */
-	/*	WFD device information */
-	/*	WFD primary sink + WiFi Direct mode + WSD (WFD Service Discovery) + WFD Session Available */
-	put_unaligned_be16(pwfd_info->wfd_device_type | WFD_DEVINFO_WSD |
-			   WFD_DEVINFO_SESSION_AVAIL, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value2: */
-	/*	Session Management Control Port */
-	/*	Default TCP port for RTSP messages is 554 */
-	put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value3: */
-	/*	WFD Device Maximum Throughput */
-	/*	300Mbps is the maximum throughput */
-	put_unaligned_be16(300, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Associated BSSID ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Associated BSSID */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-	{
-		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
-	}
-	else
-	{
-		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
-	}
-
-	wfdielen += ETH_ALEN;
-
-	/*	Coupled Sink Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0007, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Coupled Sink Status bitmap */
-	/*	Not coupled/available for Coupling */
-	wfdie[ wfdielen++ ] = 0;
-	/*   MAC Addr. */
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
-
-	return len;
-}
-
-u32 build_invitation_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
-	u32 len = 0, wfdielen = 0;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
-	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
-
-	/*	WFD OUI */
-	wfdielen = 0;
-	wfdie[ wfdielen++ ] = 0x50;
-	wfdie[ wfdielen++ ] = 0x6F;
-	wfdie[ wfdielen++ ] = 0x9A;
-	wfdie[ wfdielen++ ] = 0x0A;	/*	WFA WFD v1.0 */
-
-	/*	Commented by Albert 20110825 */
-	/*	According to the WFD Specification, the provision discovery request frame should contain 3 WFD attributes */
-	/*	1. WFD Device Information */
-	/*	2. Associated BSSID (Optional) */
-	/*	3. Local IP Adress (Optional) */
-
-	/*	WFD Device Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value1: */
-	/*	WFD device information */
-	/*	WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
-	put_unaligned_be16(pwfd_info->wfd_device_type |
-			   WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD,
-			   wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value2: */
-	/*	Session Management Control Port */
-	/*	Default TCP port for RTSP messages is 554 */
-	put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value3: */
-	/*	WFD Device Maximum Throughput */
-	/*	300Mbps is the maximum throughput */
-	put_unaligned_be16(300, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Associated BSSID ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Associated BSSID */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-	{
-		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
-	}
-	else
-	{
-		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
-	}
-
-	wfdielen += ETH_ALEN;
-
-	/*	Coupled Sink Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0007, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Coupled Sink Status bitmap */
-	/*	Not coupled/available for Coupling */
-	wfdie[ wfdielen++ ] = 0;
-	/*   MAC Addr. */
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-
-	if (P2P_ROLE_GO == pwdinfo->role)
-	{
-		/*	WFD Session Information ATTR */
-		/*	Type: */
-		wfdie[ wfdielen++ ] = WFD_ATTR_SESSION_INFO;
-
-		/*	Length: */
-		/*	Note: In the WFD specification, the size of length field is 2. */
-		put_unaligned_be16(0x0000, wfdie + wfdielen);
-		wfdielen += 2;
-
-		/*	Todo: to add the list of WFD device info descriptor in WFD group. */
-
-	}
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
-
-	return len;
-}
-
-u32 build_invitation_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
-	u32 len = 0, wfdielen = 0;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
-	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
-
-	/*	WFD OUI */
-	wfdielen = 0;
-	wfdie[ wfdielen++ ] = 0x50;
-	wfdie[ wfdielen++ ] = 0x6F;
-	wfdie[ wfdielen++ ] = 0x9A;
-	wfdie[ wfdielen++ ] = 0x0A;	/*	WFA WFD v1.0 */
-
-	/*	Commented by Albert 20110825 */
-	/*	According to the WFD Specification, the provision discovery request frame should contain 3 WFD attributes */
-	/*	1. WFD Device Information */
-	/*	2. Associated BSSID (Optional) */
-	/*	3. Local IP Adress (Optional) */
-
-	/*	WFD Device Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value1: */
-	/*	WFD device information */
-	/*	WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
-	put_unaligned_be16(pwfd_info->wfd_device_type |
-			   WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD,
-			   wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value2: */
-	/*	Session Management Control Port */
-	/*	Default TCP port for RTSP messages is 554 */
-	put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value3: */
-	/*	WFD Device Maximum Throughput */
-	/*	300Mbps is the maximum throughput */
-	put_unaligned_be16(300, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Associated BSSID ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Associated BSSID */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-	{
-		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
-	}
-	else
-	{
-		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
-	}
-
-	wfdielen += ETH_ALEN;
-
-	/*	Coupled Sink Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0007, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Coupled Sink Status bitmap */
-	/*	Not coupled/available for Coupling */
-	wfdie[ wfdielen++ ] = 0;
-	/*   MAC Addr. */
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-
-	if (P2P_ROLE_GO == pwdinfo->role)
-	{
-		/*	WFD Session Information ATTR */
-		/*	Type: */
-		wfdie[ wfdielen++ ] = WFD_ATTR_SESSION_INFO;
-
-		/*	Length: */
-		/*	Note: In the WFD specification, the size of length field is 2. */
-		put_unaligned_be16(0x0000, wfdie + wfdielen);
-		wfdielen += 2;
-
-		/*	Todo: to add the list of WFD device info descriptor in WFD group. */
-
-	}
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
-
-	return len;
-}
-
-u32 build_provdisc_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
-	u32 len = 0, wfdielen = 0;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
-	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
-
-	/*	WFD OUI */
-	wfdielen = 0;
-	wfdie[ wfdielen++ ] = 0x50;
-	wfdie[ wfdielen++ ] = 0x6F;
-	wfdie[ wfdielen++ ] = 0x9A;
-	wfdie[ wfdielen++ ] = 0x0A;	/*	WFA WFD v1.0 */
-
-	/*	Commented by Albert 20110825 */
-	/*	According to the WFD Specification, the provision discovery request frame should contain 3 WFD attributes */
-	/*	1. WFD Device Information */
-	/*	2. Associated BSSID (Optional) */
-	/*	3. Local IP Adress (Optional) */
-
-	/*	WFD Device Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value1: */
-	/*	WFD device information */
-	/*	WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
-	put_unaligned_be16(pwfd_info->wfd_device_type |
-			   WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD,
-			   wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value2: */
-	/*	Session Management Control Port */
-	/*	Default TCP port for RTSP messages is 554 */
-	put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value3: */
-	/*	WFD Device Maximum Throughput */
-	/*	300Mbps is the maximum throughput */
-	put_unaligned_be16(300, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Associated BSSID ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Associated BSSID */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-	{
-		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
-	}
-	else
-	{
-		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
-	}
-
-	wfdielen += ETH_ALEN;
-
-	/*	Coupled Sink Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0007, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Coupled Sink Status bitmap */
-	/*	Not coupled/available for Coupling */
-	wfdie[ wfdielen++ ] = 0;
-	/*   MAC Addr. */
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
-
-	return len;
-}
-
-u32 build_provdisc_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 };
-	u32 len = 0, wfdielen = 0;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
-	struct wifi_display_info*	pwfd_info = padapter->wdinfo.wfd_info;
-
-	/*	WFD OUI */
-	wfdielen = 0;
-	wfdie[ wfdielen++ ] = 0x50;
-	wfdie[ wfdielen++ ] = 0x6F;
-	wfdie[ wfdielen++ ] = 0x9A;
-	wfdie[ wfdielen++ ] = 0x0A;	/*	WFA WFD v1.0 */
-
-	/*	Commented by Albert 20110825 */
-	/*	According to the WFD Specification, the provision discovery response frame should contain 3 WFD attributes */
-	/*	1. WFD Device Information */
-	/*	2. Associated BSSID (Optional) */
-	/*	3. Local IP Adress (Optional) */
-
-	/*	WFD Device Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value1: */
-	/*	WFD device information */
-	/*	WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */
-	put_unaligned_be16(pwfd_info->wfd_device_type |
-			   WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD,
-			   wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value2: */
-	/*	Session Management Control Port */
-	/*	Default TCP port for RTSP messages is 554 */
-	put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value3: */
-	/*	WFD Device Maximum Throughput */
-	/*	300Mbps is the maximum throughput */
-	put_unaligned_be16(300, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Associated BSSID ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0006, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Associated BSSID */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-	{
-		memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN);
-	}
-	else
-	{
-		memset(wfdie + wfdielen, 0x00, ETH_ALEN);
-	}
-
-	wfdielen += ETH_ALEN;
-
-	/*	Coupled Sink Information ATTR */
-	/*	Type: */
-	wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO;
-
-	/*	Length: */
-	/*	Note: In the WFD specification, the size of length field is 2. */
-	put_unaligned_be16(0x0007, wfdie + wfdielen);
-	wfdielen += 2;
-
-	/*	Value: */
-	/*	Coupled Sink Status bitmap */
-	/*	Not coupled/available for Coupling */
-	wfdie[ wfdielen++ ] = 0;
-	/*   MAC Addr. */
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-	wfdie[ wfdielen++ ] = 0;
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len);
-
-	return len;
-}
-
-#endif /* CONFIG_8723AU_P2P */
-
-u32 build_probe_resp_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 };
-	u32 len = 0, p2pielen = 0;
-
-	/*	P2P OUI */
-	p2pielen = 0;
-	p2pie[ p2pielen++ ] = 0x50;
-	p2pie[ p2pielen++ ] = 0x6F;
-	p2pie[ p2pielen++ ] = 0x9A;
-	p2pie[ p2pielen++ ] = 0x09;	/*	WFA P2P v1.0 */
-
-	/*	Commented by Albert 20100907 */
-	/*	According to the P2P Specification, the probe response frame should contain 5 P2P attributes */
-	/*	1. P2P Capability */
-	/*	2. Extended Listen Timing */
-	/*	3. Notice of Absence (NOA)	(Only GO needs this) */
-	/*	4. Device Info */
-	/*	5. Group Info	(Only GO need this) */
-
-	/*	P2P Capability ATTR */
-	/*	Type: */
-	p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
-
-	/*	Length: */
-	/* u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002); */
-	put_unaligned_le16(0x0002, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Device Capability Bitmap, 1 byte */
-	p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
-
-	/*	Group Capability Bitmap, 1 byte */
-	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
-	{
-		p2pie[ p2pielen ] = (P2P_GRPCAP_GO | P2P_GRPCAP_INTRABSS);
-
-		if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_PROVISIONING_ING))
-			p2pie[ p2pielen ] |= P2P_GRPCAP_GROUP_FORMATION;
-
-		p2pielen++;
-	}
-	else if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE))
-	{
-		/*	Group Capability Bitmap, 1 byte */
-		if (pwdinfo->persistent_supported)
-			p2pie[ p2pielen++ ] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
-		else
-			p2pie[ p2pielen++ ] = DMP_P2P_GRPCAP_SUPPORT;
-	}
-
-	/*	Extended Listen Timing ATTR */
-	/*	Type: */
-	p2pie[ p2pielen++ ] = P2P_ATTR_EX_LISTEN_TIMING;
-
-	/*	Length: */
-	/* u16*) (p2pie + p2pielen) = cpu_to_le16(0x0004); */
-	put_unaligned_le16(0x0004, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Availability Period */
-	/* u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF); */
-	put_unaligned_le16(0xFFFF, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	Availability Interval */
-	/* u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF); */
-	put_unaligned_le16(0xFFFF, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*  Notice of Absence ATTR */
-	/*	Type: */
-	/*	Length: */
-	/*	Value: */
-	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
-	{
-		/* go_add_noa_attr(pwdinfo); */
-	}
-
-	/*	Device Info ATTR */
-	/*	Type: */
-	p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
-	/*	+ NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
-	/* u16*) (p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len); */
-	put_unaligned_le16(21 + pwdinfo->device_name_len, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	P2P Device Address */
-	memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN);
-	p2pielen += ETH_ALEN;
-
-	/*	Config Method */
-	/*	This field should be big endian. Noted by P2P specification. */
-	/* u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->supported_wps_cm); */
-	put_unaligned_be16(pwdinfo->supported_wps_cm, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	Primary Device Type */
-	/*	Category ID */
-	/* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA); */
-	put_unaligned_be16(WPS_PDT_CID_MULIT_MEDIA, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	OUI */
-	/* u32*) (p2pie + p2pielen) = cpu_to_be32(WPSOUI); */
-	put_unaligned_be32(WPSOUI, p2pie + p2pielen);
-	p2pielen += 4;
-
-	/*	Sub Category ID */
-	/* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER); */
-	put_unaligned_be16(WPS_PDT_SCID_MEDIA_SERVER, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	Number of Secondary Device Types */
-	p2pie[ p2pielen++ ] = 0x00;	/*	No Secondary Device Type List */
-
-	/*	Device Name */
-	/*	Type: */
-	/* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); */
-	put_unaligned_be16(WPS_ATTR_DEVICE_NAME, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	Length: */
-	/* u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len); */
-	put_unaligned_be16(pwdinfo->device_name_len, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	Value: */
-	memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len);
-	p2pielen += pwdinfo->device_name_len;
-
-	/*  Group Info ATTR */
-	/*	Type: */
-	/*	Length: */
-	/*	Value: */
-	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
-	{
-		p2pielen += go_add_group_info_attr(pwdinfo, p2pie + p2pielen);
-	}
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len);
-
-	return len;
-}
-
-u32 build_prov_disc_request_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf, u8* pssid, u8 ussidlen, u8* pdev_raddr)
-{
-	u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 };
-	u32 len = 0, p2pielen = 0;
-
-	/*	P2P OUI */
-	p2pielen = 0;
-	p2pie[ p2pielen++ ] = 0x50;
-	p2pie[ p2pielen++ ] = 0x6F;
-	p2pie[ p2pielen++ ] = 0x9A;
-	p2pie[ p2pielen++ ] = 0x09;	/*	WFA P2P v1.0 */
-
-	/*	Commented by Albert 20110301 */
-	/*	According to the P2P Specification, the provision discovery request frame should contain 3 P2P attributes */
-	/*	1. P2P Capability */
-	/*	2. Device Info */
-	/*	3. Group ID (When joining an operating P2P Group) */
-
-	/*	P2P Capability ATTR */
-	/*	Type: */
-	p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
-
-	/*	Length: */
-	/* u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002); */
-	put_unaligned_le16(0x0002, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	Device Capability Bitmap, 1 byte */
-	p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT;
-
-	/*	Group Capability Bitmap, 1 byte */
-	if (pwdinfo->persistent_supported)
-		p2pie[ p2pielen++ ] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT;
-	else
-		p2pie[ p2pielen++ ] = DMP_P2P_GRPCAP_SUPPORT;
-
-	/*	Device Info ATTR */
-	/*	Type: */
-	p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
-
-	/*	Length: */
-	/*	21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */
-	/*	+ NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */
-	/* u16*) (p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len); */
-	put_unaligned_le16(21 + pwdinfo->device_name_len, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	Value: */
-	/*	P2P Device Address */
-	memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN);
-	p2pielen += ETH_ALEN;
-
-	/*	Config Method */
-	/*	This field should be big endian. Noted by P2P specification. */
-	if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PBC)
-	{
-		/* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_PBC); */
-		put_unaligned_be16(WPS_CONFIG_METHOD_PBC, p2pie + p2pielen);
-	}
-	else
-	{
-		/* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_DISPLAY); */
-		put_unaligned_be16(WPS_CONFIG_METHOD_DISPLAY, p2pie + p2pielen);
-	}
-
-	p2pielen += 2;
-
-	/*	Primary Device Type */
-	/*	Category ID */
-	/* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA); */
-	put_unaligned_be16(WPS_PDT_CID_MULIT_MEDIA, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	OUI */
-	/* u32*) (p2pie + p2pielen) = cpu_to_be32(WPSOUI); */
-	put_unaligned_be32(WPSOUI, p2pie + p2pielen);
-	p2pielen += 4;
-
-	/*	Sub Category ID */
-	/* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER); */
-	put_unaligned_be16(WPS_PDT_SCID_MEDIA_SERVER, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	Number of Secondary Device Types */
-	p2pie[ p2pielen++ ] = 0x00;	/*	No Secondary Device Type List */
-
-	/*	Device Name */
-	/*	Type: */
-	/* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); */
-	put_unaligned_be16(WPS_ATTR_DEVICE_NAME, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	Length: */
-	/* u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len); */
-	put_unaligned_be16(pwdinfo->device_name_len, p2pie + p2pielen);
-	p2pielen += 2;
-
-	/*	Value: */
-	memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len);
-	p2pielen += pwdinfo->device_name_len;
-
-	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT))
-	{
-		/*	Added by Albert 2011/05/19 */
-		/*	In this case, the pdev_raddr is the device address of the group owner. */
-
-		/*	P2P Group ID ATTR */
-		/*	Type: */
-		p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_ID;
-
-		/*	Length: */
-		/* u16*) (p2pie + p2pielen) = cpu_to_le16(ETH_ALEN + ussidlen); */
-		put_unaligned_le16(ETH_ALEN + ussidlen, p2pie + p2pielen);
-		p2pielen += 2;
-
-		/*	Value: */
-		memcpy(p2pie + p2pielen, pdev_raddr, ETH_ALEN);
-		p2pielen += ETH_ALEN;
-
-		memcpy(p2pie + p2pielen, pssid, ussidlen);
-		p2pielen += ussidlen;
-
-	}
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len);
-
-	return len;
-}
-
-u32 build_assoc_resp_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf, u8 status_code)
-{
-	u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 };
-	u32 len = 0, p2pielen = 0;
-
-	/*	P2P OUI */
-	p2pielen = 0;
-	p2pie[ p2pielen++ ] = 0x50;
-	p2pie[ p2pielen++ ] = 0x6F;
-	p2pie[ p2pielen++ ] = 0x9A;
-	p2pie[ p2pielen++ ] = 0x09;	/*	WFA P2P v1.0 */
-
-	/*  According to the P2P Specification, the Association response frame should contain 2 P2P attributes */
-	/*	1. Status */
-	/*	2. Extended Listen Timing (optional) */
-
-	/*	Status ATTR */
-	p2pielen += rtw_set_p2p_attr_content23a(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status_code);
-
-	/*  Extended Listen Timing ATTR */
-	/*	Type: */
-	/*	Length: */
-	/*	Value: */
-
-	pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len);
-
-	return len;
-}
-
-u32 build_deauth_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf)
-{
-	u32 len = 0;
-
-	return len;
-}
-
-u32 process_probe_req_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
-{
-	u8 *p;
-	u32 ret = false;
-	u8 *p2pie;
-	u32	p2pielen = 0;
-	int ssid_len = 0, rate_cnt = 0;
-
-	p = rtw_get_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + _PROBEREQ_IE_OFFSET_, _SUPPORTEDRATES_IE_, (int *)&rate_cnt,
-			len - sizeof(struct ieee80211_hdr_3addr) - _PROBEREQ_IE_OFFSET_);
-
-	if (rate_cnt <= 4)
-	{
-		int i, g_rate = 0;
-
-		for (i = 0; i < rate_cnt; i++)
-		{
-			if (((*(p + 2 + i) & 0xff) != 0x02) &&
-				((*(p + 2 + i) & 0xff) != 0x04) &&
-				((*(p + 2 + i) & 0xff) != 0x0B) &&
-				((*(p + 2 + i) & 0xff) != 0x16))
-			{
-				g_rate = 1;
-			}
-		}
-
-		if (g_rate == 0)
-		{
-			/*	There is no OFDM rate included in SupportedRates IE of this probe request frame */
-			/*	The driver should response this probe request. */
-			return ret;
-		}
-	}
-	else
-	{
-		/*	rate_cnt > 4 means the SupportRates IE contains the OFDM rate because the count of CCK rates are 4. */
-		/*	We should proceed the following check for this probe request. */
-	}
-
-	/*	Added comments by Albert 20100906 */
-	/*	There are several items we should check here. */
-	/*	1. This probe request frame must contain the P2P IE. (Done) */
-	/*	2. This probe request frame must contain the wildcard SSID. (Done) */
-	/*	3. Wildcard BSSID. (Todo) */
-	/*	4. Destination Address. (Done in mgt_dispatcher23a function) */
-	/*	5. Requested Device Type in WSC IE. (Todo) */
-	/*	6. Device ID attribute in P2P IE. (Todo) */
-
-	p = rtw_get_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ssid_len,
-			len - sizeof(struct ieee80211_hdr_3addr) - _PROBEREQ_IE_OFFSET_);
-
-	ssid_len &= 0xff;	/*	Just last 1 byte is valid for ssid len of the probe request */
-	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE) || rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
-	{
-		if ((p2pie = rtw_get_p2p_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + _PROBEREQ_IE_OFFSET_, len - sizeof(struct ieee80211_hdr_3addr) - _PROBEREQ_IE_OFFSET_, NULL, &p2pielen)))
-		{
-			if ((p) && !memcmp((void *)(p+2), (void *)pwdinfo->p2p_wildcard_ssid, 7))
-			{
-				/* todo: */
-				/* Check Requested Device Type attributes in WSC IE. */
-				/* Check Device ID attribute in P2P IE */
-
-				ret = true;
-			}
-			else if ((p != NULL) && (ssid_len == 0))
-			{
-				ret = true;
-			}
-		}
-		else
-		{
-			/* non -p2p device */
-		}
-
-	}
-
-	return ret;
-}
-
-u32 process_assoc_req_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len, struct sta_info *psta)
-{
-	u8 status_code = P2P_STATUS_SUCCESS;
-	u8 *pbuf, *pattr_content = NULL;
-	u32 attr_contentlen = 0;
-	u16 cap_attr = 0;
-	unsigned short ie_offset;
-	u8 * ies;
-	u32 ies_len;
-	u8 * p2p_ie;
-	u32	p2p_ielen = 0;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)pframe;
-
-	if (!rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
-		return P2P_STATUS_FAIL_REQUEST_UNABLE;
-
-	if (ieee80211_is_assoc_req(hdr->frame_control))
-		ie_offset = _ASOCREQ_IE_OFFSET_;
-	else /*  WIFI_REASSOCREQ */
-		ie_offset = _REASOCREQ_IE_OFFSET_;
-
-	ies = pframe + sizeof(struct ieee80211_hdr_3addr) + ie_offset;
-	ies_len = len - sizeof(struct ieee80211_hdr_3addr) - ie_offset;
-
-	p2p_ie = rtw_get_p2p_ie23a(ies, ies_len, NULL, &p2p_ielen);
-
-	if (!p2p_ie)
-	{
-		DBG_8723A("[%s] P2P IE not Found!!\n", __func__);
-		status_code =  P2P_STATUS_FAIL_INVALID_PARAM;
-	}
-	else
-	{
-		DBG_8723A("[%s] P2P IE Found!!\n", __func__);
-	}
-
-	while (p2p_ie)
-	{
-		/* Check P2P Capability ATTR */
-		if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8*)&cap_attr, (uint*) &attr_contentlen))
-		{
-			DBG_8723A("[%s] Got P2P Capability Attr!!\n", __func__);
-			cap_attr = le16_to_cpu(cap_attr);
-			psta->dev_cap = cap_attr&0xff;
-		}
-
-		/* Check Extended Listen Timing ATTR */
-
-		/* Check P2P Device Info ATTR */
-		if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_INFO, NULL, (uint*)&attr_contentlen))
-		{
-			DBG_8723A("[%s] Got P2P DEVICE INFO Attr!!\n", __func__);
-			pattr_content = pbuf = kzalloc(attr_contentlen,
-						       GFP_ATOMIC);
-			if (pattr_content) {
-				u8 num_of_secdev_type;
-				u16 dev_name_len;
-
-				rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_INFO, pattr_content, (uint*)&attr_contentlen);
-
-				memcpy(psta->dev_addr,	pattr_content, ETH_ALEN);/* P2P Device Address */
-
-				pattr_content += ETH_ALEN;
-
-				memcpy(&psta->config_methods, pattr_content, 2);/* Config Methods */
-				psta->config_methods = be16_to_cpu(psta->config_methods);
-
-				pattr_content += 2;
-
-				memcpy(psta->primary_dev_type, pattr_content, 8);
-
-				pattr_content += 8;
-
-				num_of_secdev_type = *pattr_content;
-				pattr_content += 1;
-
-				if (num_of_secdev_type == 0)
-				{
-					psta->num_of_secdev_type = 0;
-				}
-				else
-				{
-					u32 len;
-
-					psta->num_of_secdev_type = num_of_secdev_type;
-
-					len = (sizeof(psta->secdev_types_list)<(num_of_secdev_type*8)) ? (sizeof(psta->secdev_types_list)) : (num_of_secdev_type*8);
-
-					memcpy(psta->secdev_types_list, pattr_content, len);
-
-					pattr_content += (num_of_secdev_type*8);
-				}
-
-				/* dev_name_len = attr_contentlen - ETH_ALEN - 2 - 8 - 1 - (num_of_secdev_type*8); */
-				psta->dev_name_len = 0;
-				if (WPS_ATTR_DEVICE_NAME == be16_to_cpu(*(u16*)pattr_content))
-				{
-					dev_name_len = be16_to_cpu(*(u16*)(pattr_content+2));
-
-					psta->dev_name_len = (sizeof(psta->dev_name)<dev_name_len) ? sizeof(psta->dev_name):dev_name_len;
-
-					memcpy(psta->dev_name, pattr_content+4, psta->dev_name_len);
-				}
-
-				kfree(pbuf);
-
-			}
-
-		}
-
-		/* Get the next P2P IE */
-		p2p_ie = rtw_get_p2p_ie23a(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
-
-	}
-
-	return status_code;
-}
-
-u32 process_p2p_devdisc_req23a(struct wifidirect_info *pwdinfo, u8 *pframe,
-			    uint len)
-{
-	u8 *frame_body;
-	u8 status, dialogToken;
-	struct sta_info *psta = NULL;
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	struct sta_priv *pstapriv = &padapter->stapriv;
-	u8 *p2p_ie;
-	u32	p2p_ielen = 0;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) pframe;
-
-	frame_body = (unsigned char *)
-		(pframe + sizeof(struct ieee80211_hdr_3addr));
-
-	dialogToken = frame_body[7];
-	status = P2P_STATUS_FAIL_UNKNOWN_P2PGROUP;
-
-	if ((p2p_ie = rtw_get_p2p_ie23a(frame_body + _PUBLIC_ACTION_IE_OFFSET_,
-				     len - _PUBLIC_ACTION_IE_OFFSET_, NULL,
-				     &p2p_ielen))) {
-		u8 groupid[38] = { 0x00 };
-		u8 dev_addr[ETH_ALEN] = { 0x00 };
-		u32 attr_contentlen = 0;
-
-		if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen,
-					     P2P_ATTR_GROUP_ID, groupid,
-					     &attr_contentlen)) {
-			if (!memcmp(pwdinfo->device_addr, groupid, ETH_ALEN) &&
-			    !memcmp(pwdinfo->p2p_group_ssid, groupid + ETH_ALEN,
-				    pwdinfo->p2p_group_ssid_len)) {
-				attr_contentlen = 0;
-
-				if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen,
-							     P2P_ATTR_DEVICE_ID,
-							     dev_addr,
-							     &attr_contentlen)) {
-					struct list_head *phead, *plist, *ptmp;
-
-					spin_lock_bh(&pstapriv->asoc_list_lock);
-					phead = &pstapriv->asoc_list;
-
-					list_for_each_safe(plist, ptmp, phead) {
-						psta = container_of(plist, struct sta_info, asoc_list);
-
-						if (psta->is_p2p_device && (psta->dev_cap&P2P_DEVCAP_CLIENT_DISCOVERABILITY) &&
-						   !memcmp(psta->dev_addr, dev_addr, ETH_ALEN))
-						{
-							/* spin_unlock_bh(&pstapriv->asoc_list_lock); */
-							/* issue GO Discoverability Request */
-							issue_group_disc_req(pwdinfo, psta->hwaddr);
-							/* spin_lock_bh(&pstapriv->asoc_list_lock); */
-							status = P2P_STATUS_SUCCESS;
-							break;
-						} else {
-							status = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
-						}
-					}
-					spin_unlock_bh(&pstapriv->asoc_list_lock);
-				} else {
-					status = P2P_STATUS_FAIL_INVALID_PARAM;
-				}
-			} else {
-				status = P2P_STATUS_FAIL_INVALID_PARAM;
-			}
-		}
-	}
-
-	/* issue Device Discoverability Response */
-	issue_p2p_devdisc_resp(pwdinfo, hdr->addr2, status, dialogToken);
-
-	return (status == P2P_STATUS_SUCCESS) ? true:false;
-}
-
-u32 process_p2p_devdisc_resp23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
-{
-	return true;
-}
-
-u8 process_p2p_provdisc_req23a(struct wifidirect_info *pwdinfo,
-			    u8 *pframe, uint len)
-{
-	u8 *frame_body;
-	u8 *wpsie;
-	u8 *ptr = NULL;
-	uint	wps_ielen = 0, attr_contentlen = 0;
-	u16	uconfig_method = 0;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)pframe;
-
-	frame_body = (pframe + sizeof(struct ieee80211_hdr_3addr));
-
-	wpsie = rtw_get_wps_ie23a(frame_body + _PUBLIC_ACTION_IE_OFFSET_,
-			       len - _PUBLIC_ACTION_IE_OFFSET_, NULL,
-			       &wps_ielen);
-	if (!wpsie)
-		goto out;
-
-	if (!rtw_get_wps_attr_content23a(wpsie, wps_ielen, WPS_ATTR_CONF_METHOD,
-				     (u8 *)&uconfig_method, &attr_contentlen))
-		goto out;
-
-	uconfig_method = be16_to_cpu(uconfig_method);
-	ptr = pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req;
-
-	switch (uconfig_method)
-	{
-	case WPS_CM_DISPLYA:
-		memcpy(ptr, "dis", 3);
-		break;
-
-	case WPS_CM_LABEL:
-		memcpy(ptr, "lab", 3);
-		break;
-
-	case WPS_CM_PUSH_BUTTON:
-		memcpy(ptr, "pbc", 3);
-		break;
-
-	case WPS_CM_KEYPAD:
-		memcpy(ptr, "pad", 3);
-		break;
-	}
-	issue_p2p_provision_resp(pwdinfo, hdr->addr2, frame_body,
-				 uconfig_method);
-
-out:
-	DBG_8723A("[%s] config method = %s\n", __func__, ptr);
-
-	return true;
-}
-
-u8 process_p2p_provdisc_resp23a(struct wifidirect_info *pwdinfo,  u8 *pframe)
-{
-
-	return true;
-}
-
-static u8 rtw_p2p_get_peer_ch_list(struct wifidirect_info *pwdinfo, u8 *ch_content, u8 ch_cnt, u8 *peer_ch_list)
-{
-	u8 i = 0, j = 0;
-	u8 temp = 0;
-	u8 ch_no = 0;
-	ch_content += 3;
-	ch_cnt -= 3;
-
-	while(ch_cnt > 0)
-	{
-		ch_content += 1;
-		ch_cnt -= 1;
-		temp = *ch_content;
-		for (i = 0 ; i < temp ; i++, j++)
-		{
-			peer_ch_list[j] = *(ch_content + 1 + i);
-		}
-		ch_content += (temp + 1);
-		ch_cnt -= (temp + 1);
-		ch_no += temp ;
-	}
-
-	return ch_no;
-}
-
-static u8 rtw_p2p_ch_inclusion(struct mlme_ext_priv *pmlmeext, u8 *peer_ch_list, u8 peer_ch_num, u8 *ch_list_inclusioned)
-{
-	int	i = 0, j = 0, temp = 0;
-	u8 ch_no = 0;
-
-	for (i = 0; i < peer_ch_num; i++)
-	{
-		for (j = temp; j < pmlmeext->max_chan_nums; j++)
-		{
-			if (*(peer_ch_list + i) == pmlmeext->channel_set[ j ].ChannelNum)
-			{
-				ch_list_inclusioned[ ch_no++ ] = *(peer_ch_list + i);
-				temp = j;
-				break;
-			}
-		}
-	}
-
-	return ch_no;
-}
-
-u8 process_p2p_group_negotation_req23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
-{
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	u8	result = P2P_STATUS_SUCCESS;
-	u32	p2p_ielen = 0, wps_ielen = 0;
-	u8 * ies;
-	u32 ies_len;
-	u8 *p2p_ie;
-	u8 *wpsie;
-	u16		wps_devicepassword_id = 0x0000;
-	uint	wps_devicepassword_id_len = 0;
-#ifdef CONFIG_8723AU_P2P
-	u8	wfd_ie[ 128 ] = { 0x00 };
-	u32	wfd_ielen = 0;
-#endif /*  CONFIG_8723AU_P2P */
-
-	if ((wpsie = rtw_get_wps_ie23a(pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &wps_ielen)))
-	{
-		/*	Commented by Kurt 20120113 */
-		/*	If some device wants to do p2p handshake without sending prov_disc_req */
-		/*	We have to get peer_req_cm from here. */
-		if (!memcmp(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "000", 3))
-		{
-			rtw_get_wps_attr_content23a(wpsie, wps_ielen, WPS_ATTR_DEVICE_PWID, (u8*) &wps_devicepassword_id, &wps_devicepassword_id_len);
-			wps_devicepassword_id = be16_to_cpu(wps_devicepassword_id);
-
-			if (wps_devicepassword_id == WPS_DPID_USER_SPEC)
-			{
-				memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3);
-			}
-			else if (wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC)
-			{
-				memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3);
-			}
-			else
-			{
-				memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3);
-			}
-		}
-	}
-	else
-	{
-		DBG_8723A("[%s] WPS IE not Found!!\n", __func__);
-		result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
-		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
-		return result;
-	}
-
-	if (pwdinfo->ui_got_wps_info == P2P_NO_WPSINFO)
-	{
-		result = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
-		rtw_p2p_set_state(pwdinfo, P2P_STATE_TX_INFOR_NOREADY);
-		return result;
-	}
-
-	ies = pframe + _PUBLIC_ACTION_IE_OFFSET_;
-	ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
-
-	p2p_ie = rtw_get_p2p_ie23a(ies, ies_len, NULL, &p2p_ielen);
-
-	if (!p2p_ie)
-	{
-		DBG_8723A("[%s] P2P IE not Found!!\n", __func__);
-		result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
-		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
-	}
-
-	while (p2p_ie)
-	{
-		u8	attr_content = 0x00;
-		u32	attr_contentlen = 0;
-		u8	ch_content[50] = { 0x00 };
-		uint	ch_cnt = 0;
-		u8	peer_ch_list[50] = { 0x00 };
-		u8	peer_ch_num = 0;
-		u8	ch_list_inclusioned[50] = { 0x00 };
-		u8	ch_num_inclusioned = 0;
-		u16	cap_attr;
-
-		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_ING);
-
-		/* Check P2P Capability ATTR */
-		if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8*)&cap_attr, (uint*)&attr_contentlen))
-			cap_attr = le16_to_cpu(cap_attr);
-
-		if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT, &attr_content, &attr_contentlen))
-		{
-			DBG_8723A("[%s] GO Intent = %d, tie = %d\n", __func__, attr_content >> 1, attr_content & 0x01);
-			pwdinfo->peer_intent = attr_content;	/*	include both intent and tie breaker values. */
-
-			if (pwdinfo->intent == (pwdinfo->peer_intent >> 1))
-			{
-				/*	Try to match the tie breaker value */
-				if (pwdinfo->intent == P2P_MAX_INTENT)
-				{
-					rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
-					result = P2P_STATUS_FAIL_BOTH_GOINTENT_15;
-				}
-				else
-				{
-					if (attr_content & 0x01)
-					{
-						rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
-					}
-					else
-					{
-						rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
-					}
-				}
-			}
-			else if (pwdinfo->intent > (pwdinfo->peer_intent >> 1))
-			{
-				rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
-			}
-			else
-			{
-				rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
-			}
-
-			if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
-			{
-				/*	Store the group id information. */
-				memcpy(pwdinfo->groupid_info.go_device_addr, pwdinfo->device_addr, ETH_ALEN);
-				memcpy(pwdinfo->groupid_info.ssid, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen);
-			}
-		}
-
-		attr_contentlen = 0;
-		if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_INTENTED_IF_ADDR, pwdinfo->p2p_peer_interface_addr, &attr_contentlen))
-		{
-			if (attr_contentlen != ETH_ALEN)
-			{
-				memset(pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN);
-			}
-		}
-
-		if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, ch_content, &ch_cnt))
-		{
-			peer_ch_num = rtw_p2p_get_peer_ch_list(pwdinfo, ch_content, ch_cnt, peer_ch_list);
-			ch_num_inclusioned = rtw_p2p_ch_inclusion(&padapter->mlmeextpriv, peer_ch_list, peer_ch_num, ch_list_inclusioned);
-
-			if (ch_num_inclusioned == 0)
-			{
-				DBG_8723A("[%s] No common channel in channel list!\n", __func__);
-				result = P2P_STATUS_FAIL_NO_COMMON_CH;
-				rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
-				break;
-			}
-
-			if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
-			{
-				if (!rtw_p2p_is_channel_list_ok(pwdinfo->operating_channel,
-												ch_list_inclusioned, ch_num_inclusioned))
-				{
-					{
-						u8 operatingch_info[5] = { 0x00 }, peer_operating_ch = 0;
-						attr_contentlen = 0;
-
-						if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen))
-						{
-							peer_operating_ch = operatingch_info[4];
-						}
-
-						if (rtw_p2p_is_channel_list_ok(peer_operating_ch,
-														ch_list_inclusioned, ch_num_inclusioned))
-						{
-							/**
-							 *	Change our operating channel as peer's for compatibility.
-							 */
-							pwdinfo->operating_channel = peer_operating_ch;
-							DBG_8723A("[%s] Change op ch to %02x as peer's\n", __func__, pwdinfo->operating_channel);
-						}
-						else
-						{
-							/*  Take first channel of ch_list_inclusioned as operating channel */
-							pwdinfo->operating_channel = ch_list_inclusioned[0];
-							DBG_8723A("[%s] Change op ch to %02x\n", __func__, pwdinfo->operating_channel);
-						}
-					}
-
-				}
-			}
-		}
-
-		/* Get the next P2P IE */
-		p2p_ie = rtw_get_p2p_ie23a(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
-	}
-
-#ifdef CONFIG_8723AU_P2P
-	/*	Added by Albert 20110823 */
-	/*	Try to get the TCP port information when receiving the negotiation request. */
-	if (rtw_get_wfd_ie(pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, wfd_ie, &wfd_ielen))
-	{
-		u8	attr_content[ 10 ] = { 0x00 };
-		u32	attr_contentlen = 0;
-
-		DBG_8723A("[%s] WFD IE Found!!\n", __func__);
-		rtw_get_wfd_attr_content(wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, attr_content, &attr_contentlen);
-		if (attr_contentlen)
-		{
-			pwdinfo->wfd_info->peer_rtsp_ctrlport = get_unaligned_be16(attr_content + 2);
-			DBG_8723A("[%s] Peer PORT NUM = %d\n", __func__, pwdinfo->wfd_info->peer_rtsp_ctrlport);
-		}
-	}
-#endif /*  CONFIG_8723AU_P2P */
-
-	return result;
-}
-
-u8 process_p2p_group_negotation_resp23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
-{
-	struct rtw_adapter *padapter = pwdinfo->padapter;
-	u8	result = P2P_STATUS_SUCCESS;
-	u32	p2p_ielen, wps_ielen;
-	u8 * ies;
-	u32 ies_len;
-	u8 * p2p_ie;
-#ifdef CONFIG_8723AU_P2P
-	u8	wfd_ie[ 128 ] = { 0x00 };
-	u32	wfd_ielen = 0;
-#endif /*  CONFIG_8723AU_P2P */
-
-	ies = pframe + _PUBLIC_ACTION_IE_OFFSET_;
-	ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
-
-	/*	Be able to know which one is the P2P GO and which one is P2P client. */
-
-	if (rtw_get_wps_ie23a(ies, ies_len, NULL, &wps_ielen))
-	{
-
-	}
-	else
-	{
-		DBG_8723A("[%s] WPS IE not Found!!\n", __func__);
-		result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
-		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
-	}
-
-	p2p_ie = rtw_get_p2p_ie23a(ies, ies_len, NULL, &p2p_ielen);
-	if (!p2p_ie)
-	{
-		rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
-		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
-		result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM;
-	}
-	else
-	{
-
-		u8	attr_content = 0x00;
-		u32	attr_contentlen = 0;
-		u8	operatingch_info[5] = { 0x00 };
-		u8	groupid[ 38 ];
-		u16	cap_attr;
-		u8	peer_ch_list[50] = { 0x00 };
-		u8	peer_ch_num = 0;
-		u8	ch_list_inclusioned[50] = { 0x00 };
-		u8	ch_num_inclusioned = 0;
-
-		while (p2p_ie)	/*	Found the P2P IE. */
-		{
-
-			/* Check P2P Capability ATTR */
-			if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8*)&cap_attr, (uint*)&attr_contentlen))
-				cap_attr = le16_to_cpu(cap_attr);
-
-			rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
-			if (attr_contentlen == 1)
-			{
-				DBG_8723A("[%s] Status = %d\n", __func__, attr_content);
-				if (attr_content == P2P_STATUS_SUCCESS)
-				{
-					/*	Do nothing. */
-				}
-				else
-				{
-					if (P2P_STATUS_FAIL_INFO_UNAVAILABLE == attr_content) {
-						rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INFOR_NOREADY);
-					} else {
-						rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
-					}
-					rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
-					result = attr_content;
-					break;
-				}
-			}
-
-			/*	Try to get the peer's interface address */
-			attr_contentlen = 0;
-			if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_INTENTED_IF_ADDR, pwdinfo->p2p_peer_interface_addr, &attr_contentlen))
-			{
-				if (attr_contentlen != ETH_ALEN)
-				{
-					memset(pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN);
-				}
-			}
-
-			/*	Try to get the peer's intent and tie breaker value. */
-			attr_content = 0x00;
-			attr_contentlen = 0;
-			if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT, &attr_content, &attr_contentlen))
-			{
-				DBG_8723A("[%s] GO Intent = %d, tie = %d\n", __func__, attr_content >> 1, attr_content & 0x01);
-				pwdinfo->peer_intent = attr_content;	/*	include both intent and tie breaker values. */
-
-				if (pwdinfo->intent == (pwdinfo->peer_intent >> 1))
-				{
-					/*	Try to match the tie breaker value */
-					if (pwdinfo->intent == P2P_MAX_INTENT)
-					{
-						rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
-						result = P2P_STATUS_FAIL_BOTH_GOINTENT_15;
-						rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
-					}
-					else
-					{
-						rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
-						rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
-						if (attr_content & 0x01)
-						{
-							rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
-						}
-						else
-						{
-							rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
-						}
-					}
-				}
-				else if (pwdinfo->intent > (pwdinfo->peer_intent >> 1))
-				{
-					rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
-					rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
-					rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
-				}
-				else
-				{
-					rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
-					rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
-					rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
-				}
-
-				if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
-				{
-					/*	Store the group id information. */
-					memcpy(pwdinfo->groupid_info.go_device_addr, pwdinfo->device_addr, ETH_ALEN);
-					memcpy(pwdinfo->groupid_info.ssid, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen);
-
-				}
-			}
-
-			/*	Try to get the operation channel information */
-
-			attr_contentlen = 0;
-			if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen))
-			{
-				DBG_8723A("[%s] Peer's operating channel = %d\n", __func__, operatingch_info[4]);
-				pwdinfo->peer_operating_ch = operatingch_info[4];
-			}
-
-			/*	Try to get the channel list information */
-			if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, pwdinfo->channel_list_attr, &pwdinfo->channel_list_attr_len))
-			{
-				DBG_8723A("[%s] channel list attribute found, len = %d\n", __func__,  pwdinfo->channel_list_attr_len);
-
-				peer_ch_num = rtw_p2p_get_peer_ch_list(pwdinfo, pwdinfo->channel_list_attr, pwdinfo->channel_list_attr_len, peer_ch_list);
-				ch_num_inclusioned = rtw_p2p_ch_inclusion(&padapter->mlmeextpriv, peer_ch_list, peer_ch_num, ch_list_inclusioned);
-
-				if (ch_num_inclusioned == 0)
-				{
-					DBG_8723A("[%s] No common channel in channel list!\n", __func__);
-					result = P2P_STATUS_FAIL_NO_COMMON_CH;
-					rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
-					break;
-				}
-
-				if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO))
-				{
-					if (!rtw_p2p_is_channel_list_ok(pwdinfo->operating_channel,
-													ch_list_inclusioned, ch_num_inclusioned))
-					{
-						{
-							u8 operatingch_info[5] = { 0x00 }, peer_operating_ch = 0;
-							attr_contentlen = 0;
-
-							if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen))
-							{
-								peer_operating_ch = operatingch_info[4];
-							}
-
-							if (rtw_p2p_is_channel_list_ok(peer_operating_ch,
-															ch_list_inclusioned, ch_num_inclusioned))
-							{
-								/**
-								 *	Change our operating channel as peer's for compatibility.
-								 */
-								pwdinfo->operating_channel = peer_operating_ch;
-								DBG_8723A("[%s] Change op ch to %02x as peer's\n", __func__, pwdinfo->operating_channel);
-							}
-							else
-							{
-								/*  Take first channel of ch_list_inclusioned as operating channel */
-								pwdinfo->operating_channel = ch_list_inclusioned[0];
-								DBG_8723A("[%s] Change op ch to %02x\n", __func__, pwdinfo->operating_channel);
-							}
-						}
-
-					}
-				}
-
-			}
-			else
-			{
-				DBG_8723A("[%s] channel list attribute not found!\n", __func__);
-			}
-
-			/*	Try to get the group id information if peer is GO */
-			attr_contentlen = 0;
-			memset(groupid, 0x00, 38);
-			if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, groupid, &attr_contentlen))
-			{
-				memcpy(pwdinfo->groupid_info.go_device_addr, &groupid[0], ETH_ALEN);
-				memcpy(pwdinfo->groupid_info.ssid, &groupid[6], attr_contentlen - ETH_ALEN);
-			}
-
-			/* Get the next P2P IE */
-			p2p_ie = rtw_get_p2p_ie23a(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
-		}
-
-	}
-
-#ifdef CONFIG_8723AU_P2P
-	/*	Added by Albert 20111122 */
-	/*	Try to get the TCP port information when receiving the negotiation response. */
-	if (rtw_get_wfd_ie(pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, wfd_ie, &wfd_ielen))
-	{
-		u8	attr_content[ 10 ] = { 0x00 };
-		u32	attr_contentlen = 0;
-
-		DBG_8723A("[%s] WFD IE Found!!\n", __func__);
-		rtw_get_wfd_attr_content(wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, attr_content, &attr_contentlen);
-		if (attr_contentlen)
-		{
-			pwdinfo->wfd_info->peer_rtsp_ctrlport = get_unaligned_be16(attr_content + 2);
-			DBG_8723A("[%s] Peer PORT NUM = %d\n", __func__, pwdinfo->wfd_info->peer_rtsp_ctrlport);
-		}
-	}
-#endif /*  CONFIG_8723AU_P2P */
-
-	return result;
-}
-
-u8 process_p2p_group_negotation_confirm23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
-{
-	u8 * ies;
-	u32 ies_len;
-	u8 * p2p_ie;
-	u32	p2p_ielen = 0;
-	u8	result = P2P_STATUS_SUCCESS;
-	ies = pframe + _PUBLIC_ACTION_IE_OFFSET_;
-	ies_len = len - _PUBLIC_ACTION_IE_OFFSET_;
-
-	p2p_ie = rtw_get_p2p_ie23a(ies, ies_len, NULL, &p2p_ielen);
-	while (p2p_ie)	/*	Found the P2P IE. */
-	{
-		u8	attr_content = 0x00, operatingch_info[5] = { 0x00 };
-		u8	groupid[ 38 ] = { 0x00 };
-		u32	attr_contentlen = 0;
-
-		pwdinfo->negotiation_dialog_token = 1;
-		rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen);
-		if (attr_contentlen == 1)
-		{
-			DBG_8723A("[%s] Status = %d\n", __func__, attr_content);
-			result = attr_content;
-
-			if (attr_content == P2P_STATUS_SUCCESS)
-			{
-				del_timer_sync(&pwdinfo->restore_p2p_state_timer);
-
-				/*	Commented by Albert 20100911 */
-				/*	Todo: Need to handle the case which both Intents are the same. */
-				rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
-				rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
-				if ((pwdinfo->intent) > (pwdinfo->peer_intent >> 1))
-				{
-					rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
-				}
-				else if ((pwdinfo->intent) < (pwdinfo->peer_intent >> 1))
-				{
-					rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
-				}
-				else
-				{
-					/*	Have to compare the Tie Breaker */
-					if (pwdinfo->peer_intent & 0x01)
-					{
-						rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
-					}
-					else
-					{
-						rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
-					}
-				}
-			}
-			else
-			{
-				rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
-				rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL);
-				break;
-			}
-		}
-
-		/*	Try to get the group id information */
-		attr_contentlen = 0;
-		memset(groupid, 0x00, 38);
-		if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, groupid, &attr_contentlen))
-		{
-			DBG_8723A("[%s] Ssid = %s, ssidlen = %zu\n", __func__, &groupid[ETH_ALEN], strlen(&groupid[ETH_ALEN]));
-			memcpy(pwdinfo->groupid_info.go_device_addr, &groupid[0], ETH_ALEN);
-			memcpy(pwdinfo->groupid_info.ssid, &groupid[6], attr_contentlen - ETH_ALEN);
-		}
-
-		attr_contentlen = 0;
-		if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen))
-		{
-			DBG_8723A("[%s] Peer's operating channel = %d\n", __func__, operatingch_info[4]);
-			pwdinfo->peer_operating_ch = operatingch_info[4];
-		}
-
-		/* Get the next P2P IE */
-		p2p_ie = rtw_get_p2p_ie23a(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
-
-	}
-
-	return result;
-}
-
-u8 process_p2p_presence_req23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
-{
-	u8 *frame_body;
-	u8 dialogToken = 0;
-	u8 status = P2P_STATUS_SUCCESS;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) pframe;
-
-	frame_body = (unsigned char *)(pframe + sizeof(struct ieee80211_hdr_3addr));
-
-	dialogToken = frame_body[6];
-
-	/* todo: check NoA attribute */
-
-	issue_p2p_presence_resp(pwdinfo, hdr->addr2, status, dialogToken);
-
-	return true;
-}
-
-static void find_phase_handler(struct rtw_adapter *padapter)
-{
-	struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
-	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
-	struct cfg80211_ssid ssid;
-	u8					_status = 0;
-
-
-
-	memset((unsigned char*)&ssid, 0, sizeof(struct cfg80211_ssid));
-	memcpy(ssid.ssid, pwdinfo->p2p_wildcard_ssid, P2P_WILDCARD_SSID_LEN);
-	ssid.ssid_len = P2P_WILDCARD_SSID_LEN;
-
-	rtw_p2p_set_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH);
-
-	spin_lock_bh(&pmlmepriv->lock);
-	_status = rtw_sitesurvey_cmd23a(padapter, &ssid, 1, NULL, 0);
-	spin_unlock_bh(&pmlmepriv->lock);
-
-
-}
-
-void p2p_concurrent_handler(struct rtw_adapter* padapter);
-
-static void restore_p2p_state_handler(struct rtw_adapter *padapter)
-{
-	struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
-
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL))
-		rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
-
-	rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
-
-	if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE)) {
-		/*	In the P2P client mode, the driver should not switch back to its listen channel */
-		/*	because this P2P client should stay at the operating channel of P2P GO. */
-		set_channel_bwmode23a(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
-	}
-}
-
-static void pre_tx_invitereq_handler(struct rtw_adapter *padapter)
-{
-	struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
-	u8	val8 = 1;
-
-	set_channel_bwmode23a(padapter, pwdinfo->invitereq_info.peer_ch, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
-	padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
-	issue23a_probereq_p2p(padapter, NULL);
-	mod_timer(&pwdinfo->pre_tx_scan_timer,
-		  jiffies + msecs_to_jiffies(P2P_TX_PRESCAN_TIMEOUT));
-
-
-}
-
-static void pre_tx_provdisc_handler(struct rtw_adapter *padapter)
-{
-	struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
-	u8	val8 = 1;
-
-
-	set_channel_bwmode23a(padapter, pwdinfo->tx_prov_disc_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
-	issue23a_probereq_p2p(padapter, NULL);
-	mod_timer(&pwdinfo->pre_tx_scan_timer,
-		  jiffies + msecs_to_jiffies(P2P_TX_PRESCAN_TIMEOUT));
-
-
-}
-
-static void pre_tx_negoreq_handler(struct rtw_adapter *padapter)
-{
-	struct wifidirect_info  *pwdinfo = &padapter->wdinfo;
-	u8	val8 = 1;
-
-
-	set_channel_bwmode23a(padapter, pwdinfo->nego_req_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8));
-	issue23a_probereq_p2p(padapter, NULL);
-	mod_timer(&pwdinfo->pre_tx_scan_timer,
-		  jiffies + msecs_to_jiffies(P2P_TX_PRESCAN_TIMEOUT));
-
-
-}
-
-static void ro_ch_handler(struct rtw_adapter *padapter)
-{
-	struct cfg80211_wifidirect_info *pcfg80211_wdinfo = &padapter->cfg80211_wdinfo;
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-
-	if (pcfg80211_wdinfo->restore_channel != pmlmeext->cur_channel) {
-		if (!check_fwstate(&padapter->mlmepriv, _FW_LINKED))
-			pmlmeext->cur_channel = pcfg80211_wdinfo->restore_channel;
-
-		set_channel_bwmode23a(padapter, pmlmeext->cur_channel,
-				      HAL_PRIME_CHNL_OFFSET_DONT_CARE,
-				      HT_CHANNEL_WIDTH_20);
-	}
-
-	rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
-
-	pcfg80211_wdinfo->is_ro_ch = false;
-
-	DBG_8723A("cfg80211_remain_on_channel_expired\n");
-
-	rtw_cfg80211_remain_on_channel_expired(padapter,
-		pcfg80211_wdinfo->remain_on_ch_cookie,
-		&pcfg80211_wdinfo->remain_on_ch_channel,
-		pcfg80211_wdinfo->remain_on_ch_type, GFP_KERNEL);
-}
-
-static void ro_ch_timer_process (unsigned long data)
-{
-	struct rtw_adapter *adapter = (struct rtw_adapter *)data;
-
-	p2p_protocol_wk_cmd23a(adapter, P2P_RO_CH_WK);
-}
-
-#ifdef CONFIG_8723AU_P2P
-void rtw_append_wfd_ie(struct rtw_adapter *padapter, u8 *buf, u32* len)
-{
-	unsigned char	*frame_body;
-	u8 category, action, OUI_Subtype, dialogToken = 0;
-	u32	wfdielen = 0;
-
-	frame_body = (unsigned char *)(buf + sizeof(struct ieee80211_hdr_3addr));
-	category = frame_body[0];
-
-	if (category == WLAN_CATEGORY_PUBLIC) {
-		action = frame_body[1];
-		if (action == ACT_PUBLIC_VENDOR &&
-		    !memcmp(frame_body+2, P2P_OUI23A, 4)) {
-			OUI_Subtype = frame_body[6];
-			dialogToken = frame_body[7];
-			switch (OUI_Subtype)/* OUI Subtype */ {
-			case P2P_GO_NEGO_REQ:
-				wfdielen = build_nego_req_wfd_ie(&padapter->wdinfo, buf + (*len));
-				(*len) += wfdielen;
-				break;
-			case P2P_GO_NEGO_RESP:
-				wfdielen = build_nego_resp_wfd_ie(&padapter->wdinfo, buf + (*len));
-				(*len) += wfdielen;
-				break;
-			case P2P_GO_NEGO_CONF:
-				wfdielen = build_nego_confirm_wfd_ie(&padapter->wdinfo, buf + (*len));
-				(*len) += wfdielen;
-				break;
-			case P2P_INVIT_REQ:
-				wfdielen = build_invitation_req_wfd_ie(&padapter->wdinfo, buf + (*len));
-				(*len) += wfdielen;
-				break;
-			case P2P_INVIT_RESP:
-				wfdielen = build_invitation_resp_wfd_ie(&padapter->wdinfo, buf + (*len));
-				(*len) += wfdielen;
-				break;
-			case P2P_DEVDISC_REQ:
-				break;
-			case P2P_DEVDISC_RESP:
-				break;
-			case P2P_PROVISION_DISC_REQ:
-				wfdielen = build_provdisc_req_wfd_ie(&padapter->wdinfo, buf + (*len));
-				(*len) += wfdielen;
-				break;
-			case P2P_PROVISION_DISC_RESP:
-				wfdielen = build_provdisc_resp_wfd_ie(&padapter->wdinfo, buf + (*len));
-				(*len) += wfdielen;
-				break;
-			default:
-				break;
-			}
-		}
-	} else if (category == WLAN_CATEGORY_VENDOR_SPECIFIC) {
-		OUI_Subtype = frame_body[5];
-		dialogToken = frame_body[6];
-	} else {
-		DBG_8723A("%s, action frame category =%d\n", __func__, category);
-	}
-}
-#endif
-
-int rtw_p2p_check_frames(struct rtw_adapter *padapter, const u8 *buf, u32 len, u8 tx)
-{
-	int is_p2p_frame = (-1);
-	unsigned char	*frame_body;
-	u8 category, action, OUI_Subtype, dialogToken = 0;
-	u8 *p2p_ie = NULL;
-	uint p2p_ielen = 0;
-	struct rtw_wdev_priv *pwdev_priv = wdev_to_priv(padapter->rtw_wdev);
-
-	frame_body = (unsigned char *)(buf + sizeof(struct ieee80211_hdr_3addr));
-	category = frame_body[0];
-	/* just for check */
-	if (category == WLAN_CATEGORY_PUBLIC)
-	{
-		action = frame_body[1];
-		if (action == ACT_PUBLIC_VENDOR &&
-		    !memcmp(frame_body+2, P2P_OUI23A, 4)) {
-			OUI_Subtype = frame_body[6];
-			dialogToken = frame_body[7];
-			is_p2p_frame = OUI_Subtype;
-			p2p_ie = rtw_get_p2p_ie23a(
-				(u8 *)buf+sizeof(struct ieee80211_hdr_3addr)+_PUBLIC_ACTION_IE_OFFSET_,
-				len-sizeof(struct ieee80211_hdr_3addr)-_PUBLIC_ACTION_IE_OFFSET_,
-				NULL, &p2p_ielen);
-
-			switch (OUI_Subtype) {/* OUI Subtype */
-			u8 *cont;
-			uint cont_len;
-			case P2P_GO_NEGO_REQ:
-				DBG_8723A("RTW_%s:P2P_GO_NEGO_REQ, dialogToken =%d\n", (tx == true)?"Tx":"Rx", dialogToken);
-				break;
-			case P2P_GO_NEGO_RESP:
-				cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len);
-				DBG_8723A("RTW_%s:P2P_GO_NEGO_RESP, dialogToken =%d, status:%d\n", (tx == true)?"Tx":"Rx", dialogToken, cont?*cont:-1);
-
-				if (!tx)
-					pwdev_priv->provdisc_req_issued = false;
-				break;
-			case P2P_GO_NEGO_CONF:
-				cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len);
-				DBG_8723A("RTW_%s:P2P_GO_NEGO_CONF, dialogToken =%d, status:%d\n",
-					  (tx == true)?"Tx":"Rx", dialogToken, cont?*cont:-1);
-				break;
-			case P2P_INVIT_REQ:
-			{
-				struct rtw_wdev_invit_info* invit_info = &pwdev_priv->invit_info;
-				int flags = -1;
-				int op_ch = 0;
-
-				if ((cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_INVITATION_FLAGS, NULL, &cont_len)))
-					flags = *cont;
-				if ((cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, &cont_len)))
-					op_ch = *(cont+4);
-
-				if (invit_info->token != dialogToken)
-					rtw_wdev_invit_info_init(invit_info);
-
-				invit_info->token = dialogToken;
-				invit_info->flags = (flags ==-1) ? 0x0 : flags;
-				invit_info->req_op_ch = op_ch;
-
-				DBG_8723A("RTW_%s:P2P_INVIT_REQ, dialogToken =%d, flags:0x%02x, op_ch:%d\n",
-					  (tx) ? "Tx" : "Rx", dialogToken, flags, op_ch);
-				break;
-			}
-			case P2P_INVIT_RESP:
-			{
-				struct rtw_wdev_invit_info* invit_info = &pwdev_priv->invit_info;
-				int status = -1;
-				int op_ch = 0;
-
-				if ((cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len)))
-					status = *cont;
-				if ((cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, &cont_len)))
-					op_ch = *(cont+4);
-
-				if (invit_info->token != dialogToken) {
-					rtw_wdev_invit_info_init(invit_info);
-				} else {
-					invit_info->token = 0;
-					invit_info->status = (status ==-1) ? 0xff : status;
-					invit_info->rsp_op_ch = op_ch;
-				}
-
-				DBG_8723A("RTW_%s:P2P_INVIT_RESP, dialogToken =%d, status:%d, op_ch:%d\n",
-					  (tx == true)?"Tx":"Rx", dialogToken, status, op_ch);
-				break;
-			}
-			case P2P_DEVDISC_REQ:
-				DBG_8723A("RTW_%s:P2P_DEVDISC_REQ, dialogToken =%d\n", (tx == true)?"Tx":"Rx", dialogToken);
-				break;
-			case P2P_DEVDISC_RESP:
-				cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len);
-				DBG_8723A("RTW_%s:P2P_DEVDISC_RESP, dialogToken =%d, status:%d\n", (tx == true)?"Tx":"Rx", dialogToken, cont?*cont:-1);
-				break;
-			case P2P_PROVISION_DISC_REQ:
-			{
-				size_t frame_body_len = len - sizeof(struct ieee80211_hdr_3addr);
-				u8 *p2p_ie;
-				uint p2p_ielen = 0;
-				uint contentlen = 0;
-
-				DBG_8723A("RTW_%s:P2P_PROVISION_DISC_REQ, dialogToken =%d\n", (tx == true)?"Tx":"Rx", dialogToken);
-
-				pwdev_priv->provdisc_req_issued = false;
-
-				p2p_ie = rtw_get_p2p_ie23a(frame_body + _PUBLIC_ACTION_IE_OFFSET_,
-							   frame_body_len - _PUBLIC_ACTION_IE_OFFSET_,
-							   NULL, &p2p_ielen);
-				if (p2p_ie) {
-					if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, NULL, &contentlen))
-						pwdev_priv->provdisc_req_issued = false;/* case: p2p_client join p2p GO */
-					else
-						pwdev_priv->provdisc_req_issued = true;/* case: p2p_devices connection before Nego req. */
-				}
-			}
-				break;
-			case P2P_PROVISION_DISC_RESP:
-				DBG_8723A("RTW_%s:P2P_PROVISION_DISC_RESP, dialogToken =%d\n", (tx == true)?"Tx":"Rx", dialogToken);
-				break;
-			default:
-				DBG_8723A("RTW_%s:OUI_Subtype =%d, dialogToken =%d\n", (tx == true)?"Tx":"Rx", OUI_Subtype, dialogToken);
-				break;
-			}
-
-		}
-
-	}
-	else if (category == WLAN_CATEGORY_VENDOR_SPECIFIC)
-	{
-		OUI_Subtype = frame_body[5];
-		dialogToken = frame_body[6];
-
-		is_p2p_frame = OUI_Subtype;
-
-		switch (OUI_Subtype) {
-		case P2P_NOTICE_OF_ABSENCE:
-			DBG_8723A("RTW_%s:P2P_NOTICE_OF_ABSENCE, dialogToken =%d\n", (tx == true)?"TX":"RX", dialogToken);
-			break;
-		case P2P_PRESENCE_REQUEST:
-			DBG_8723A("RTW_%s:P2P_PRESENCE_REQUEST, dialogToken =%d\n", (tx == true)?"TX":"RX", dialogToken);
-			break;
-		case P2P_PRESENCE_RESPONSE:
-			DBG_8723A("RTW_%s:P2P_PRESENCE_RESPONSE, dialogToken =%d\n", (tx == true)?"TX":"RX", dialogToken);
-			break;
-		case P2P_GO_DISC_REQUEST:
-			DBG_8723A("RTW_%s:P2P_GO_DISC_REQUEST, dialogToken =%d\n", (tx == true)?"TX":"RX", dialogToken);
-			break;
-		default:
-			DBG_8723A("RTW_%s:OUI_Subtype =%d, dialogToken =%d\n", (tx == true)?"TX":"RX", OUI_Subtype, dialogToken);
-			break;
-		}
-
-	} else {
-		DBG_8723A("RTW_%s:action frame category =%d\n", (tx == true)?"TX":"RX", category);
-	}
-	return is_p2p_frame;
-}
-
-void rtw_init_cfg80211_wifidirect_info(struct rtw_adapter *padapter)
-{
-	struct cfg80211_wifidirect_info *pcfg80211_wdinfo = &padapter->cfg80211_wdinfo;
-
-	memset(pcfg80211_wdinfo, 0x00, sizeof(struct cfg80211_wifidirect_info));
-
-	setup_timer(&pcfg80211_wdinfo->remain_on_ch_timer,
-		    ro_ch_timer_process, (unsigned long)padapter);
-}
-
-void p2p_protocol_wk_hdl23a(struct rtw_adapter *padapter, int intCmdType)
-{
-	switch (intCmdType) {
-	case P2P_FIND_PHASE_WK:
-		find_phase_handler(padapter);
-		break;
-	case P2P_RESTORE_STATE_WK:
-		restore_p2p_state_handler(padapter);
-		break;
-	case P2P_PRE_TX_PROVDISC_PROCESS_WK:
-		pre_tx_provdisc_handler(padapter);
-		break;
-	case P2P_PRE_TX_INVITEREQ_PROCESS_WK:
-		pre_tx_invitereq_handler(padapter);
-		break;
-	case P2P_PRE_TX_NEGOREQ_PROCESS_WK:
-		pre_tx_negoreq_handler(padapter);
-		break;
-	case P2P_RO_CH_WK:
-		ro_ch_handler(padapter);
-		break;
-	}
-}
-
-#ifdef CONFIG_8723AU_P2P
-void process_p2p_ps_ie23a(struct rtw_adapter *padapter, u8 *IEs, u32 IELength)
-{
-	u8 * ies;
-	u32 ies_len;
-	u8 * p2p_ie;
-	u32	p2p_ielen = 0;
-	u8	noa_attr[MAX_P2P_IE_LEN] = { 0x00 };/*  NoA length should be n*(13) + 2 */
-	u32	attr_contentlen = 0;
-
-	struct wifidirect_info	*pwdinfo = &padapter->wdinfo;
-	u8	find_p2p = false, find_p2p_ps = false;
-	u8	noa_offset, noa_num, noa_index;
-
-
-
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
-	{
-		return;
-	}
-	if (IELength <= _BEACON_IE_OFFSET_)
-		return;
-
-	ies = IEs + _BEACON_IE_OFFSET_;
-	ies_len = IELength - _BEACON_IE_OFFSET_;
-
-	p2p_ie = rtw_get_p2p_ie23a(ies, ies_len, NULL, &p2p_ielen);
-
-	while(p2p_ie)
-	{
-		find_p2p = true;
-		/*  Get Notice of Absence IE. */
-		if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_NOA, noa_attr, &attr_contentlen))
-		{
-			find_p2p_ps = true;
-			noa_index = noa_attr[0];
-
-			if ((pwdinfo->p2p_ps_mode == P2P_PS_NONE) ||
-				(noa_index != pwdinfo->noa_index))/*  if index change, driver should reconfigure related setting. */
-			{
-				pwdinfo->noa_index = noa_index;
-				pwdinfo->opp_ps = noa_attr[1] >> 7;
-				pwdinfo->ctwindow = noa_attr[1] & 0x7F;
-
-				noa_offset = 2;
-				noa_num = 0;
-				/*  NoA length should be n*(13) + 2 */
-				if (attr_contentlen > 2)
-				{
-					while(noa_offset < attr_contentlen)
-					{
-						/* memcpy(&wifidirect_info->noa_count[noa_num], &noa_attr[noa_offset], 1); */
-						pwdinfo->noa_count[noa_num] = noa_attr[noa_offset];
-						noa_offset += 1;
-
-						memcpy(&pwdinfo->noa_duration[noa_num], &noa_attr[noa_offset], 4);
-						noa_offset += 4;
-
-						memcpy(&pwdinfo->noa_interval[noa_num], &noa_attr[noa_offset], 4);
-						noa_offset += 4;
-
-						memcpy(&pwdinfo->noa_start_time[noa_num], &noa_attr[noa_offset], 4);
-						noa_offset += 4;
-
-						noa_num++;
-					}
-				}
-				pwdinfo->noa_num = noa_num;
-
-				if (pwdinfo->opp_ps == 1)
-				{
-					pwdinfo->p2p_ps_mode = P2P_PS_CTWINDOW;
-					/*  driver should wait LPS for entering CTWindow */
-					if (padapter->pwrctrlpriv.bFwCurrentInPSMode == true)
-					{
-						p2p_ps_wk_cmd23a(padapter, P2P_PS_ENABLE, 1);
-					}
-				}
-				else if (pwdinfo->noa_num > 0)
-				{
-					pwdinfo->p2p_ps_mode = P2P_PS_NOA;
-					p2p_ps_wk_cmd23a(padapter, P2P_PS_ENABLE, 1);
-				}
-				else if (pwdinfo->p2p_ps_mode > P2P_PS_NONE)
-				{
-					p2p_ps_wk_cmd23a(padapter, P2P_PS_DISABLE, 1);
-				}
-			}
-
-			break; /*  find target, just break. */
-		}
-
-		/* Get the next P2P IE */
-		p2p_ie = rtw_get_p2p_ie23a(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen);
-
-	}
-
-	if (find_p2p == true)
-	{
-		if ((pwdinfo->p2p_ps_mode > P2P_PS_NONE) && (find_p2p_ps == false))
-		{
-			p2p_ps_wk_cmd23a(padapter, P2P_PS_DISABLE, 1);
-		}
-	}
-
-
-}
-
-void p2p_ps_wk_hdl23a(struct rtw_adapter *padapter, u8 p2p_ps_state)
-{
-	struct pwrctrl_priv		*pwrpriv = &padapter->pwrctrlpriv;
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-
-
-
-	/*  Pre action for p2p state */
-	switch (p2p_ps_state)
-	{
-		case P2P_PS_DISABLE:
-			pwdinfo->p2p_ps_state = p2p_ps_state;
-
-			rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_P2P_PS_OFFLOAD, (u8 *)(&p2p_ps_state));
-
-			pwdinfo->noa_index = 0;
-			pwdinfo->ctwindow = 0;
-			pwdinfo->opp_ps = 0;
-			pwdinfo->noa_num = 0;
-			pwdinfo->p2p_ps_mode = P2P_PS_NONE;
-			if (padapter->pwrctrlpriv.bFwCurrentInPSMode == true)
-			{
-				if (pwrpriv->smart_ps == 0)
-				{
-					pwrpriv->smart_ps = 2;
-					rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)&padapter->pwrctrlpriv.pwr_mode);
-				}
-			}
-			break;
-		case P2P_PS_ENABLE:
-			if (pwdinfo->p2p_ps_mode > P2P_PS_NONE) {
-				pwdinfo->p2p_ps_state = p2p_ps_state;
-
-				if (pwdinfo->ctwindow > 0)
-				{
-					if (pwrpriv->smart_ps != 0)
-					{
-						pwrpriv->smart_ps = 0;
-						DBG_8723A("%s(): Enter CTW, change SmartPS\n", __func__);
-						rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)&padapter->pwrctrlpriv.pwr_mode);
-					}
-				}
-				rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_P2P_PS_OFFLOAD, (u8 *)(&p2p_ps_state));
-			}
-			break;
-		case P2P_PS_SCAN:
-		case P2P_PS_SCAN_DONE:
-		case P2P_PS_ALLSTASLEEP:
-			if (pwdinfo->p2p_ps_mode > P2P_PS_NONE) {
-				pwdinfo->p2p_ps_state = p2p_ps_state;
-				rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_P2P_PS_OFFLOAD, (u8 *)(&p2p_ps_state));
-			}
-			break;
-		default:
-			break;
-	}
-
-
-}
-
-u8 p2p_ps_wk_cmd23a(struct rtw_adapter*padapter, u8 p2p_ps_state, u8 enqueue)
-{
-	struct cmd_obj	*ph2c;
-	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
-	u8	res = _SUCCESS;
-
-
-
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
-		return res;
-
-	if (enqueue) {
-		ph2c = (struct cmd_obj *)kzalloc(sizeof(struct cmd_obj),
-						 GFP_ATOMIC);
-		if (!ph2c) {
-			res = _FAIL;
-			goto exit;
-		}
-
-		pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)
-			kzalloc(sizeof(struct drvextra_cmd_parm), GFP_ATOMIC);
-		if (pdrvextra_cmd_parm == NULL) {
-			kfree(ph2c);
-			res = _FAIL;
-			goto exit;
-		}
-
-		pdrvextra_cmd_parm->ec_id = P2P_PS_WK_CID;
-		pdrvextra_cmd_parm->type_size = p2p_ps_state;
-		pdrvextra_cmd_parm->pbuf = NULL;
-
-		init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
-
-		res = rtw_enqueue_cmd23a(pcmdpriv, ph2c);
-	}
-	else
-	{
-		p2p_ps_wk_hdl23a(padapter, p2p_ps_state);
-	}
-
-exit:
-
-
-
-	return res;
-}
-#endif /*  CONFIG_8723AU_P2P */
-
-static void reset_ch_sitesurvey_timer_process(unsigned long data)
-{
-	struct rtw_adapter *adapter = (struct rtw_adapter *)data;
-	struct  wifidirect_info *pwdinfo = &adapter->wdinfo;
-
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
-		return;
-
-	DBG_8723A("[%s] In\n", __func__);
-	/*	Reset the operation channel information */
-	pwdinfo->rx_invitereq_info.operation_ch[0] = 0;
-	pwdinfo->rx_invitereq_info.scan_op_ch_only = 0;
-}
-
-static void reset_ch_sitesurvey_timer_process2(unsigned long data)
-{
-	struct rtw_adapter *adapter = (struct rtw_adapter *)data;
-	struct wifidirect_info *pwdinfo = &adapter->wdinfo;
-
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
-		return;
-
-	DBG_8723A("[%s] In\n", __func__);
-	/*	Reset the operation channel information */
-	pwdinfo->p2p_info.operation_ch[0] = 0;
-	pwdinfo->p2p_info.scan_op_ch_only = 0;
-}
-
-static void restore_p2p_state_timer_process (unsigned long data)
-{
-	struct rtw_adapter *adapter = (struct rtw_adapter *)data;
-	struct	wifidirect_info		*pwdinfo = &adapter->wdinfo;
-
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
-		return;
-
-	p2p_protocol_wk_cmd23a(adapter, P2P_RESTORE_STATE_WK);
-}
-
-static void pre_tx_scan_timer_process (unsigned long data)
-{
-	struct rtw_adapter *adapter = (struct rtw_adapter *)data;
-	struct wifidirect_info *pwdinfo = &adapter->wdinfo;
-	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
-
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
-		return;
-
-	spin_lock_bh(&pmlmepriv->lock);
-
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ))
-	{
-		if (true == pwdinfo->tx_prov_disc_info.benable)	/*	the provision discovery request frame is trigger to send or not */
-		{
-			p2p_protocol_wk_cmd23a(adapter, P2P_PRE_TX_PROVDISC_PROCESS_WK);
-			/* issue23a_probereq_p2p(adapter, NULL); */
-			/* _set_timer(&pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT); */
-		}
-	}
-	else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING))
-	{
-		if (true == pwdinfo->nego_req_info.benable)
-		{
-			p2p_protocol_wk_cmd23a(adapter, P2P_PRE_TX_NEGOREQ_PROCESS_WK);
-		}
-	}
-	else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_INVITE_REQ))
-	{
-		if (true == pwdinfo->invitereq_info.benable)
-		{
-			p2p_protocol_wk_cmd23a(adapter, P2P_PRE_TX_INVITEREQ_PROCESS_WK);
-		}
-	}
-	else
-	{
-		DBG_8723A("[%s] p2p_state is %d, ignore!!\n", __func__, rtw_p2p_state(pwdinfo));
-	}
-
-	spin_unlock_bh(&pmlmepriv->lock);
-}
-
-static void find_phase_timer_process (unsigned long data)
-{
-	struct rtw_adapter *adapter = (struct rtw_adapter *)data;
-	struct	wifidirect_info		*pwdinfo = &adapter->wdinfo;
-
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
-		return;
-
-	adapter->wdinfo.find_phase_state_exchange_cnt++;
-
-	p2p_protocol_wk_cmd23a(adapter, P2P_FIND_PHASE_WK);
-}
-
-void reset_global_wifidirect_info23a(struct rtw_adapter *padapter)
-{
-	struct wifidirect_info	*pwdinfo;
-
-	pwdinfo = &padapter->wdinfo;
-	pwdinfo->persistent_supported = 0;
-	pwdinfo->session_available = true;
-	pwdinfo->wfd_tdls_enable = 0;
-	pwdinfo->wfd_tdls_weaksec = 0;
-}
-
-#ifdef CONFIG_8723AU_P2P
-int rtw_init_wifi_display_info(struct rtw_adapter* padapter)
-{
-	int	res = _SUCCESS;
-	struct wifi_display_info *pwfd_info = &padapter->wfd_info;
-
-	/*  Used in P2P and TDLS */
-	pwfd_info->rtsp_ctrlport = 554;
-	pwfd_info->peer_rtsp_ctrlport = 0;	/*	Reset to 0 */
-	pwfd_info->wfd_enable = false;
-	pwfd_info->wfd_device_type = WFD_DEVINFO_PSINK;
-	pwfd_info->scan_result_type = SCAN_RESULT_P2P_ONLY;
-
-	/*  Used in P2P */
-	pwfd_info->peer_session_avail = true;
-	pwfd_info->wfd_pc = false;
-
-	/*  Used in TDLS */
-	memset(pwfd_info->ip_address, 0x00, 4);
-	memset(pwfd_info->peer_ip_address, 0x00, 4);
-	return res;
-}
-#endif /* CONFIG_8723AU_P2P */
-
-void rtw_init_wifidirect_timers23a(struct rtw_adapter* padapter)
-{
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-
-	setup_timer(&pwdinfo->find_phase_timer, find_phase_timer_process,
-		    (unsigned long)padapter);
-	setup_timer(&pwdinfo->restore_p2p_state_timer,
-		    restore_p2p_state_timer_process, (unsigned long)padapter);
-	setup_timer(&pwdinfo->pre_tx_scan_timer, pre_tx_scan_timer_process,
-		    (unsigned long)padapter);
-	setup_timer(&pwdinfo->reset_ch_sitesurvey,
-		    reset_ch_sitesurvey_timer_process, (unsigned long)padapter);
-	setup_timer(&pwdinfo->reset_ch_sitesurvey2,
-		    reset_ch_sitesurvey_timer_process2,
-		    (unsigned long)padapter);
-}
-
-void rtw_init_wifidirect_addrs23a(struct rtw_adapter* padapter, u8 *dev_addr, u8 *iface_addr)
-{
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-
-	/*init device&interface address */
-	if (dev_addr) {
-		memcpy(pwdinfo->device_addr, dev_addr, ETH_ALEN);
-	}
-	if (iface_addr) {
-		memcpy(pwdinfo->interface_addr, iface_addr, ETH_ALEN);
-	}
-#endif
-}
-
-void init_wifidirect_info23a(struct rtw_adapter *padapter, enum P2P_ROLE role)
-{
-	struct wifidirect_info	*pwdinfo;
-#ifdef CONFIG_8723AU_P2P
-	struct wifi_display_info	*pwfd_info = &padapter->wfd_info;
-#endif
-
-	pwdinfo = &padapter->wdinfo;
-
-	pwdinfo->padapter = padapter;
-
-	/*	1, 6, 11 are the social channel defined in the WiFi Direct specification. */
-	pwdinfo->social_chan[0] = 1;
-	pwdinfo->social_chan[1] = 6;
-	pwdinfo->social_chan[2] = 11;
-	pwdinfo->social_chan[3] = 0;	/*	channel 0 for scanning ending in site survey function. */
-
-	/*	Use the channel 11 as the listen channel */
-	pwdinfo->listen_channel = 11;
-
-	if (role == P2P_ROLE_DEVICE)
-	{
-		rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
-		rtw_p2p_set_state(pwdinfo, P2P_STATE_LISTEN);
-		pwdinfo->intent = 1;
-		rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_LISTEN);
-	}
-	else if (role == P2P_ROLE_CLIENT)
-	{
-		rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
-		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
-		pwdinfo->intent = 1;
-		rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
-	}
-	else if (role == P2P_ROLE_GO)
-	{
-		rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
-		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
-		pwdinfo->intent = 15;
-		rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK);
-	}
-
-/*	Use the OFDM rate in the P2P probe response frame. (6(B), 9(B), 12, 18, 24, 36, 48, 54) */
-	pwdinfo->support_rate[0] = 0x8c;	/*	6(B) */
-	pwdinfo->support_rate[1] = 0x92;	/*	9(B) */
-	pwdinfo->support_rate[2] = 0x18;	/*	12 */
-	pwdinfo->support_rate[3] = 0x24;	/*	18 */
-	pwdinfo->support_rate[4] = 0x30;	/*	24 */
-	pwdinfo->support_rate[5] = 0x48;	/*	36 */
-	pwdinfo->support_rate[6] = 0x60;	/*	48 */
-	pwdinfo->support_rate[7] = 0x6c;	/*	54 */
-
-	memcpy((void*) pwdinfo->p2p_wildcard_ssid, "DIRECT-", 7);
-
-	memset(pwdinfo->device_name, 0x00, WPS_MAX_DEVICE_NAME_LEN);
-	pwdinfo->device_name_len = 0;
-
-	memset(&pwdinfo->invitereq_info, 0x00, sizeof(struct tx_invite_req_info));
-	pwdinfo->invitereq_info.token = 3;	/*	Token used for P2P invitation request frame. */
-
-	memset(&pwdinfo->inviteresp_info, 0x00, sizeof(struct tx_invite_resp_info));
-	pwdinfo->inviteresp_info.token = 0;
-
-	pwdinfo->profileindex = 0;
-	memset(&pwdinfo->profileinfo[ 0 ], 0x00, sizeof(struct profile_info) * P2P_MAX_PERSISTENT_GROUP_NUM);
-
-	rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_NONE);
-
-	pwdinfo->listen_dwell = (u8) ((jiffies % 3) + 1);
-	/* DBG_8723A("[%s] listen_dwell time is %d00ms\n", __func__, pwdinfo->listen_dwell); */
-
-	memset(&pwdinfo->tx_prov_disc_info, 0x00, sizeof(struct tx_provdisc_req_info));
-	pwdinfo->tx_prov_disc_info.wps_config_method_request = WPS_CM_NONE;
-
-	memset(&pwdinfo->nego_req_info, 0x00, sizeof(struct tx_nego_req_info));
-
-	pwdinfo->device_password_id_for_nego = WPS_DPID_PBC;
-	pwdinfo->negotiation_dialog_token = 1;
-
-	memset(pwdinfo->nego_ssid, 0x00, IEEE80211_MAX_SSID_LEN);
-	pwdinfo->nego_ssidlen = 0;
-
-	pwdinfo->ui_got_wps_info = P2P_NO_WPSINFO;
-#ifdef CONFIG_8723AU_P2P
-	pwdinfo->supported_wps_cm = WPS_CONFIG_METHOD_DISPLAY  | WPS_CONFIG_METHOD_PBC;
-	pwdinfo->wfd_info = pwfd_info;
-#else
-	pwdinfo->supported_wps_cm = WPS_CONFIG_METHOD_DISPLAY | WPS_CONFIG_METHOD_PBC | WPS_CONFIG_METHOD_KEYPAD;
-#endif /* CONFIG_8723AU_P2P */
-	pwdinfo->channel_list_attr_len = 0;
-	memset(pwdinfo->channel_list_attr, 0x00, 100);
-
-	memset(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, 0x00, 4);
-	memset(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, '0', 3);
-	memset(&pwdinfo->groupid_info, 0x00, sizeof(struct group_id_info));
-	pwdinfo->wfd_tdls_enable = 0;
-	memset(pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN);
-	memset(pwdinfo->p2p_peer_device_addr, 0x00, ETH_ALEN);
-
-	pwdinfo->rx_invitereq_info.operation_ch[0] = 0;
-	pwdinfo->rx_invitereq_info.operation_ch[1] = 0;	/*	Used to indicate the scan end in site survey function */
-	pwdinfo->rx_invitereq_info.scan_op_ch_only = 0;
-	pwdinfo->p2p_info.operation_ch[0] = 0;
-	pwdinfo->p2p_info.operation_ch[1] = 0;			/*	Used to indicate the scan end in site survey function */
-	pwdinfo->p2p_info.scan_op_ch_only = 0;
-}
-
-int rtw_p2p_enable23a(struct rtw_adapter *padapter, enum P2P_ROLE role)
-{
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-	int ret = _SUCCESS;
-
-	if (role == P2P_ROLE_DEVICE || role == P2P_ROLE_CLIENT ||
-	    role == P2P_ROLE_GO) {
-		/* leave IPS/Autosuspend */
-		if (_FAIL == rtw_pwr_wakeup(padapter)) {
-			ret = _FAIL;
-			goto exit;
-		}
-
-		/*	Added by Albert 2011/03/22 */
-		/*	In the P2P mode, the driver should not support the b mode. */
-		/*	So, the Tx packet shouldn't use the CCK rate */
-		update_tx_basic_rate23a(padapter, WIRELESS_11AGN);
-
-		/* Enable P2P function */
-		init_wifidirect_info23a(padapter, role);
-
-		rtw_hal_set_odm_var23a(padapter, HAL_ODM_P2P_STATE, NULL, true);
-		#ifdef CONFIG_8723AU_P2P
-		rtw_hal_set_odm_var23a(padapter, HAL_ODM_WIFI_DISPLAY_STATE, NULL, true);
-		#endif
-
-	}
-	else if (role == P2P_ROLE_DISABLE)
-	{
-		if (_FAIL == rtw_pwr_wakeup(padapter)) {
-			ret = _FAIL;
-			goto exit;
-		}
-
-		/* Disable P2P function */
-		if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
-		{
-			del_timer_sync(&pwdinfo->find_phase_timer);
-			del_timer_sync(&pwdinfo->restore_p2p_state_timer);
-			del_timer_sync(&pwdinfo->pre_tx_scan_timer);
-			del_timer_sync(&pwdinfo->reset_ch_sitesurvey);
-			del_timer_sync(&pwdinfo->reset_ch_sitesurvey2);
-			reset_ch_sitesurvey_timer_process((unsigned long)padapter);
-			reset_ch_sitesurvey_timer_process2((unsigned long)padapter);
-			rtw_p2p_set_state(pwdinfo, P2P_STATE_NONE);
-			rtw_p2p_set_role(pwdinfo, P2P_ROLE_DISABLE);
-			memset(&pwdinfo->rx_prov_disc_info, 0x00, sizeof(struct rx_provdisc_req_info));
-		}
-
-		rtw_hal_set_odm_var23a(padapter, HAL_ODM_P2P_STATE, NULL, false);
-		#ifdef CONFIG_8723AU_P2P
-		rtw_hal_set_odm_var23a(padapter, HAL_ODM_WIFI_DISPLAY_STATE, NULL, false);
-		#endif
-
-		/* Restore to initial setting. */
-		update_tx_basic_rate23a(padapter, padapter->registrypriv.wireless_mode);
-	}
-
-exit:
-	return ret;
-}
-
-#endif /* CONFIG_8723AU_P2P */
diff --git a/drivers/staging/rtl8723au/core/rtw_pwrctrl.c b/drivers/staging/rtl8723au/core/rtw_pwrctrl.c
index 8ddd67f194b..dbd01b652e0 100644
--- a/drivers/staging/rtl8723au/core/rtw_pwrctrl.c
+++ b/drivers/staging/rtl8723au/core/rtw_pwrctrl.c
@@ -17,10 +17,11 @@
 #include <osdep_service.h>
 #include <drv_types.h>
 #include <osdep_intf.h>
+#include <rtl8723a_cmd.h>
+#include <rtw_sreset.h>
 
-#ifdef CONFIG_8723AU_BT_COEXIST
-#include <rtl8723a_hal.h>
-#endif
+#include <rtl8723a_bt_intf.h>
+#include <usb_ops_linux.h>
 
 void ips_enter23a(struct rtw_adapter * padapter)
 {
@@ -35,11 +36,9 @@ void ips_enter23a(struct rtw_adapter * padapter)
 
 	pwrpriv->ips_enter23a_cnts++;
 	DBG_8723A("==>ips_enter23a cnts:%d\n", pwrpriv->ips_enter23a_cnts);
-#ifdef CONFIG_8723AU_BT_COEXIST
-	BTDM_TurnOffBtCoexistBeforeEnterIPS(padapter);
-#endif
-	if (rf_off == pwrpriv->change_rfpwrstate)
-	{
+	rtl8723a_BT_disable_coexist(padapter);
+
+	if (pwrpriv->change_rfpwrstate == rf_off) {
 		pwrpriv->bpower_saving = true;
 		DBG_8723A_LEVEL(_drv_always_, "nolinked power save enter\n");
 
@@ -64,25 +63,34 @@ int ips_leave23a(struct rtw_adapter * padapter)
 
 	down(&pwrpriv->lock);
 
-	if ((pwrpriv->rf_pwrstate == rf_off) &&!pwrpriv->bips_processing)
-	{
+	if (pwrpriv->rf_pwrstate == rf_off && !pwrpriv->bips_processing) {
 		pwrpriv->bips_processing = true;
 		pwrpriv->change_rfpwrstate = rf_on;
 		pwrpriv->ips_leave23a_cnts++;
-		DBG_8723A("==>ips_leave23a cnts:%d\n", pwrpriv->ips_leave23a_cnts);
+		DBG_8723A("==>ips_leave23a cnts:%d\n",
+			  pwrpriv->ips_leave23a_cnts);
 
-		if ((result = rtw_ips_pwr_up23a(padapter)) == _SUCCESS) {
+		result = rtw_ips_pwr_up23a(padapter);
+		if (result == _SUCCESS)
 			pwrpriv->rf_pwrstate = rf_on;
-		}
+
 		DBG_8723A_LEVEL(_drv_always_, "nolinked power save leave\n");
 
-		if ((_WEP40_ == psecuritypriv->dot11PrivacyAlgrthm) ||(_WEP104_ == psecuritypriv->dot11PrivacyAlgrthm))
-		{
-			DBG_8723A("==>%s, channel(%d), processing(%x)\n", __func__, padapter->mlmeextpriv.cur_channel, pwrpriv->bips_processing);
-			set_channel_bwmode23a(padapter, padapter->mlmeextpriv.cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
-			for (keyid = 0;keyid<4;keyid++) {
-				if (pmlmepriv->key_mask & CHKBIT(keyid)) {
-					if (keyid == psecuritypriv->dot11PrivacyKeyIndex)
+		if (psecuritypriv->dot11PrivacyAlgrthm ==
+		    WLAN_CIPHER_SUITE_WEP40 ||
+		    psecuritypriv->dot11PrivacyAlgrthm ==
+		    WLAN_CIPHER_SUITE_WEP104) {
+			DBG_8723A("==>%s, channel(%d), processing(%x)\n",
+				  __func__, padapter->mlmeextpriv.cur_channel,
+				  pwrpriv->bips_processing);
+			set_channel_bwmode23a(padapter,
+					      padapter->mlmeextpriv.cur_channel,
+					      HAL_PRIME_CHNL_OFFSET_DONT_CARE,
+					      HT_CHANNEL_WIDTH_20);
+			for (keyid = 0; keyid < 4; keyid++) {
+				if (pmlmepriv->key_mask & BIT(keyid)) {
+					if (keyid ==
+					    psecuritypriv->dot11PrivacyKeyIndex)
 						result = rtw_set_key23a(padapter, psecuritypriv, keyid, 1);
 					else
 						result = rtw_set_key23a(padapter, psecuritypriv, keyid, 0);
@@ -90,7 +98,8 @@ int ips_leave23a(struct rtw_adapter * padapter)
 			}
 		}
 
-		DBG_8723A("==> ips_leave23a.....LED(0x%08x)...\n", rtw_read32(padapter, 0x4c));
+		DBG_8723A("==> ips_leave23a.....LED(0x%08x)...\n",
+			  rtl8723au_read32(padapter, 0x4c));
 		pwrpriv->bips_processing = false;
 
 		pwrpriv->bkeepfwalive = false;
@@ -108,42 +117,42 @@ static bool rtw_pwr_unassociated_idle(struct rtw_adapter *adapter)
 	struct rtw_adapter *buddy = adapter->pbuddy_adapter;
 	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
 	struct xmit_priv *pxmit_priv = &adapter->xmitpriv;
-	struct wifidirect_info *pwdinfo = &adapter->wdinfo;
 
 	bool ret = false;
 
 	if (time_after_eq(adapter->pwrctrlpriv.ips_deny_time, jiffies))
 		goto exit;
 
-	if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE|WIFI_SITE_MONITOR)
-		|| check_fwstate(pmlmepriv, WIFI_UNDER_LINKING|WIFI_UNDER_WPS)
-		|| check_fwstate(pmlmepriv, WIFI_AP_STATE)
-		|| check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_ADHOC_STATE)
-		|| !rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)
-	) {
+	if (check_fwstate(pmlmepriv, WIFI_ASOC_STATE|WIFI_SITE_MONITOR) ||
+	    check_fwstate(pmlmepriv, WIFI_UNDER_LINKING|WIFI_UNDER_WPS) ||
+	    check_fwstate(pmlmepriv, WIFI_AP_STATE) ||
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_ADHOC_STATE)){
 		goto exit;
 	}
 
 	/* consider buddy, if exist */
 	if (buddy) {
 		struct mlme_priv *b_pmlmepriv = &buddy->mlmepriv;
-		struct wifidirect_info *b_pwdinfo = &buddy->wdinfo;
-
-		if (check_fwstate(b_pmlmepriv, WIFI_ASOC_STATE|WIFI_SITE_MONITOR)
-			|| check_fwstate(b_pmlmepriv, WIFI_UNDER_LINKING|WIFI_UNDER_WPS)
-			|| check_fwstate(b_pmlmepriv, WIFI_AP_STATE)
-			|| check_fwstate(b_pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_ADHOC_STATE)
-			|| !rtw_p2p_chk_state(b_pwdinfo, P2P_STATE_NONE)
-		) {
+
+		if (check_fwstate(b_pmlmepriv,
+				  WIFI_ASOC_STATE|WIFI_SITE_MONITOR) ||
+		    check_fwstate(b_pmlmepriv,
+				  WIFI_UNDER_LINKING|WIFI_UNDER_WPS) ||
+		    check_fwstate(b_pmlmepriv, WIFI_AP_STATE) ||
+		    check_fwstate(b_pmlmepriv,
+				  WIFI_ADHOC_MASTER_STATE|WIFI_ADHOC_STATE)) {
 			goto exit;
 		}
 	}
 
 	if (pxmit_priv->free_xmitbuf_cnt != NR_XMITBUFF ||
 		pxmit_priv->free_xmit_extbuf_cnt != NR_XMIT_EXTBUFF) {
-		DBG_8723A_LEVEL(_drv_always_, "There are some pkts to transmit\n");
-		DBG_8723A_LEVEL(_drv_info_, "free_xmitbuf_cnt: %d, free_xmit_extbuf_cnt: %d\n",
-			pxmit_priv->free_xmitbuf_cnt, pxmit_priv->free_xmit_extbuf_cnt);
+		DBG_8723A_LEVEL(_drv_always_,
+				"There are some pkts to transmit\n");
+		DBG_8723A_LEVEL(_drv_info_, "free_xmitbuf_cnt: %d, "
+				"free_xmit_extbuf_cnt: %d\n",
+				pxmit_priv->free_xmitbuf_cnt,
+				pxmit_priv->free_xmit_extbuf_cnt);
 		goto exit;
 	}
 
@@ -166,7 +175,8 @@ void rtw_ps_processor23a(struct rtw_adapter*padapter)
 
 	if (padapter->pwrctrlpriv.bHWPwrPindetect) {
 		rfpwrstate = RfOnOffDetect23a(padapter);
-		DBG_8723A("@@@@- #2  %s ==> rfstate:%s\n", __func__, (rfpwrstate == rf_on)?"rf_on":"rf_off");
+		DBG_8723A("@@@@- #2  %s ==> rfstate:%s\n", __func__,
+			  (rfpwrstate == rf_on)?"rf_on":"rf_off");
 
 		if (rfpwrstate!= pwrpriv->rf_pwrstate) {
 			if (rfpwrstate == rf_off) {
@@ -178,7 +188,9 @@ void rtw_ps_processor23a(struct rtw_adapter*padapter)
 				pwrpriv->change_rfpwrstate = rf_on;
 				rtw_hw_resume23a(padapter);
 			}
-			DBG_8723A("current rf_pwrstate(%s)\n", (pwrpriv->rf_pwrstate == rf_off)?"rf_off":"rf_on");
+			DBG_8723A("current rf_pwrstate(%s)\n",
+				  (pwrpriv->rf_pwrstate == rf_off) ?
+				  "rf_off":"rf_on");
 		}
 		pwrpriv->pwr_state_check_cnts ++;
 	}
@@ -189,9 +201,10 @@ void rtw_ps_processor23a(struct rtw_adapter*padapter)
 	if (rtw_pwr_unassociated_idle(padapter) == false)
 		goto exit;
 
-	if ((pwrpriv->rf_pwrstate == rf_on) && ((pwrpriv->pwr_state_check_cnts%4) == 0))
-	{
-		DBG_8723A("==>%s .fw_state(%x)\n", __func__, get_fwstate(pmlmepriv));
+	if (pwrpriv->rf_pwrstate == rf_on &&
+	    (pwrpriv->pwr_state_check_cnts % 4) == 0) {
+		DBG_8723A("==>%s .fw_state(%x)\n", __func__,
+			  get_fwstate(pmlmepriv));
 		pwrpriv->change_rfpwrstate = rf_off;
 		ips_enter23a(padapter);
 	}
@@ -210,36 +223,35 @@ static void pwr_state_check_handler(unsigned long data)
 /*
  *
  * Parameters
- *	padapter
- *	pslv			power state level, only could be PS_STATE_S0 ~ PS_STATE_S4
+ *   padapter
+ *   pslv	power state level, only could be PS_STATE_S0 ~ PS_STATE_S4
  *
  */
 void rtw_set_rpwm23a(struct rtw_adapter *padapter, u8 pslv)
 {
-	u8	rpwm;
+	u8 rpwm;
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
 
-
-
 	pslv = PS_STATE(pslv);
 
-	if (true == pwrpriv->btcoex_rfon)
-	{
+	if (pwrpriv->btcoex_rfon) {
 		if (pslv < PS_STATE_S4)
 			pslv = PS_STATE_S3;
 	}
 
 	if (pwrpriv->rpwm == pslv) {
 		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
-			("%s: Already set rpwm[0x%02X], new = 0x%02X!\n", __func__, pwrpriv->rpwm, pslv));
+			("%s: Already set rpwm[0x%02X], new = 0x%02X!\n",
+			 __func__, pwrpriv->rpwm, pslv));
 		return;
 	}
 
-	if ((padapter->bSurpriseRemoved == true) ||
-	    (padapter->hw_init_completed == false)) {
+	if (padapter->bSurpriseRemoved == true ||
+	    padapter->hw_init_completed == false) {
 		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
-				 ("%s: SurpriseRemoved(%d) hw_init_completed(%d)\n",
-				  __func__, padapter->bSurpriseRemoved, padapter->hw_init_completed));
+			 ("%s: SurpriseRemoved(%d) hw_init_completed(%d)\n",
+			  __func__, padapter->bSurpriseRemoved,
+			  padapter->hw_init_completed));
 
 		pwrpriv->cpwm = PS_STATE_S4;
 
@@ -248,52 +260,52 @@ void rtw_set_rpwm23a(struct rtw_adapter *padapter, u8 pslv)
 
 	if (padapter->bDriverStopped == true) {
 		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
-				 ("%s: change power state(0x%02X) when DriverStopped\n", __func__, pslv));
+			 ("%s: change power state(0x%02X) when DriverStopped\n",
+			  __func__, pslv));
 
 		if (pslv < PS_STATE_S2) {
 			RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
-					 ("%s: Reject to enter PS_STATE(0x%02X) lower than S2 when DriverStopped!!\n", __func__, pslv));
+				 ("%s: Reject to enter PS_STATE(0x%02X) lower "
+				  "than S2 when DriverStopped!!\n",
+				  __func__, pslv));
 			return;
 		}
 	}
 
 	rpwm = pslv | pwrpriv->tog;
 	RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
-			 ("rtw_set_rpwm23a: rpwm = 0x%02x cpwm = 0x%02x\n", rpwm, pwrpriv->cpwm));
+		 ("rtw_set_rpwm23a: rpwm = 0x%02x cpwm = 0x%02x\n",
+		  rpwm, pwrpriv->cpwm));
 
 	pwrpriv->rpwm = pslv;
 
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_SET_RPWM, (u8 *)(&rpwm));
+	rtl8723a_set_rpwm(padapter, rpwm);
 
 	pwrpriv->tog += 0x80;
 	pwrpriv->cpwm = pslv;
-
-
 }
 
-u8 PS_RDY_CHECK(struct rtw_adapter * padapter)
+static bool PS_RDY_CHECK(struct rtw_adapter * padapter)
 {
 	unsigned long delta_time;
-	struct pwrctrl_priv	*pwrpriv = &padapter->pwrctrlpriv;
+	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
 	delta_time = jiffies - pwrpriv->DelayLPSLastTimeStamp;
 
 	if (delta_time < LPS_DELAY_TIME)
-	{
 		return false;
-	}
 
-	if ((check_fwstate(pmlmepriv, _FW_LINKED) == false) ||
-		(check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true) ||
-		(check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) ||
-		(check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) ||
-		(check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true))
+	if (!check_fwstate(pmlmepriv, _FW_LINKED) ||
+	    check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) ||
+	    check_fwstate(pmlmepriv, WIFI_AP_STATE) ||
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) ||
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_STATE))
 		return false;
-	if (true == pwrpriv->bInSuspend)
+	if (pwrpriv->bInSuspend)
 		return false;
-	if ((padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) && (padapter->securitypriv.binstallGrpkey == false))
-	{
+	if (padapter->securitypriv.dot11AuthAlgrthm == dot11AuthAlgrthm_8021X &&
+	    !padapter->securitypriv.binstallGrpkey) {
 		DBG_8723A("Group handshake still in progress !!!\n");
 		return false;
 	}
@@ -303,75 +315,51 @@ u8 PS_RDY_CHECK(struct rtw_adapter * padapter)
 	return true;
 }
 
-void rtw_set_ps_mode23a(struct rtw_adapter *padapter, u8 ps_mode, u8 smart_ps, u8 bcn_ant_mode)
+void rtw_set_ps_mode23a(struct rtw_adapter *padapter, u8 ps_mode,
+			u8 smart_ps, u8 bcn_ant_mode)
 {
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-#endif /* CONFIG_8723AU_P2P */
-
-
 
 	RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
 			 ("%s: PowerMode =%d Smart_PS =%d\n",
 			  __func__, ps_mode, smart_ps));
 
 	if (ps_mode > PM_Card_Disable) {
-		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_, ("ps_mode:%d error\n", ps_mode));
+		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
+			 ("ps_mode:%d error\n", ps_mode));
 		return;
 	}
 
-	if (pwrpriv->pwr_mode == ps_mode)
-	{
-		if (PS_MODE_ACTIVE == ps_mode) return;
+	if (pwrpriv->pwr_mode == ps_mode) {
+		if (PS_MODE_ACTIVE == ps_mode)
+			return;
 
-		if ((pwrpriv->smart_ps == smart_ps) &&
-			(pwrpriv->bcn_ant_mode == bcn_ant_mode))
-		{
+		if (pwrpriv->smart_ps == smart_ps &&
+		    pwrpriv->bcn_ant_mode == bcn_ant_mode)
 			return;
-		}
 	}
 
 	if (ps_mode == PS_MODE_ACTIVE) {
-#ifdef CONFIG_8723AU_P2P
-		if (pwdinfo->opp_ps == 0)
-#endif /* CONFIG_8723AU_P2P */
-		{
-			DBG_8723A("rtw_set_ps_mode23a: Leave 802.11 power save\n");
-
-			pwrpriv->pwr_mode = ps_mode;
-			rtw_set_rpwm23a(padapter, PS_STATE_S4);
-			rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&ps_mode));
-			pwrpriv->bFwCurrentInPSMode = false;
-		}
-	}
-	else
-	{
-		if (PS_RDY_CHECK(padapter)
-#ifdef CONFIG_8723AU_BT_COEXIST
-			|| (BT_1Ant(padapter) == true)
-#endif
-			)
-		{
+		DBG_8723A("rtw_set_ps_mode23a: Leave 802.11 power save\n");
+
+		pwrpriv->pwr_mode = ps_mode;
+		rtw_set_rpwm23a(padapter, PS_STATE_S4);
+		rtl8723a_set_FwPwrMode_cmd(padapter, ps_mode);
+		pwrpriv->bFwCurrentInPSMode = false;
+	} else {
+		if (PS_RDY_CHECK(padapter) ||
+		    rtl8723a_BT_using_antenna_1(padapter)) {
 			DBG_8723A("%s: Enter 802.11 power save\n", __func__);
 
 			pwrpriv->bFwCurrentInPSMode = true;
 			pwrpriv->pwr_mode = ps_mode;
 			pwrpriv->smart_ps = smart_ps;
 			pwrpriv->bcn_ant_mode = bcn_ant_mode;
-			rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&ps_mode));
-
-#ifdef CONFIG_8723AU_P2P
-			/*  Set CTWindow after LPS */
-			if (pwdinfo->opp_ps == 1)
-				p2p_ps_wk_cmd23a(padapter, P2P_PS_ENABLE, 0);
-#endif /* CONFIG_8723AU_P2P */
+			rtl8723a_set_FwPwrMode_cmd(padapter, ps_mode);
 
 			rtw_set_rpwm23a(padapter, PS_STATE_S2);
 		}
 	}
-
-
 }
 
 /*
@@ -391,12 +379,11 @@ s32 LPS_RF_ON_check23a(struct rtw_adapter *padapter, u32 delay_ms)
 
 	while (1)
 	{
-		rtw23a_hal_get_hwreg(padapter, HW_VAR_FWLPS_RF_ON, &bAwake);
-		if (true == bAwake)
+		bAwake = rtl8723a_get_fwlps_rf_on(padapter);
+		if (bAwake == true)
 			break;
 
-		if (true == padapter->bSurpriseRemoved)
-		{
+		if (padapter->bSurpriseRemoved == true) {
 			err = -2;
 			DBG_8723A("%s: device surprise removed!!\n", __func__);
 			break;
@@ -404,7 +391,8 @@ s32 LPS_RF_ON_check23a(struct rtw_adapter *padapter, u32 delay_ms)
 
 		if (time_after(jiffies, end_time)) {
 			err = -1;
-			DBG_8723A("%s: Wait for FW LPS leave more than %u ms!!!\n", __func__, delay_ms);
+			DBG_8723A("%s: Wait for FW LPS leave more than %u "
+				  "ms!\n", __func__, delay_ms);
 			break;
 		}
 		udelay(100);
@@ -417,7 +405,7 @@ s32 LPS_RF_ON_check23a(struct rtw_adapter *padapter, u32 delay_ms)
 /*		Enter the leisure power save mode. */
 void LPS_Enter23a(struct rtw_adapter *padapter)
 {
-	struct pwrctrl_priv	*pwrpriv = &padapter->pwrctrlpriv;
+	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
 
 	if (!PS_RDY_CHECK(padapter))
 		return;
@@ -427,13 +415,15 @@ void LPS_Enter23a(struct rtw_adapter *padapter)
 		if (pwrpriv->LpsIdleCount >= 2) { /*   4 Sec */
 			if (pwrpriv->pwr_mode == PS_MODE_ACTIVE) {
 				pwrpriv->bpower_saving = true;
-				DBG_8723A("%s smart_ps:%d\n", __func__, pwrpriv->smart_ps);
+				DBG_8723A("%s smart_ps:%d\n", __func__,
+					  pwrpriv->smart_ps);
 				/* For Tenda W311R IOT issue */
-				rtw_set_ps_mode23a(padapter, pwrpriv->power_mgnt, pwrpriv->smart_ps, 0);
+				rtw_set_ps_mode23a(padapter,
+						   pwrpriv->power_mgnt,
+						   pwrpriv->smart_ps, 0);
 			}
-		} else {
+		} else
 			pwrpriv->LpsIdleCount++;
-		}
 	}
 }
 
@@ -443,14 +433,15 @@ void LPS_Leave23a(struct rtw_adapter *padapter)
 {
 #define LPS_LEAVE_TIMEOUT_MS 100
 
-	struct pwrctrl_priv	*pwrpriv = &padapter->pwrctrlpriv;
+	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
 
 	if (pwrpriv->bLeisurePs) {
 		if (pwrpriv->pwr_mode != PS_MODE_ACTIVE) {
 			rtw_set_ps_mode23a(padapter, PS_MODE_ACTIVE, 0, 0);
 
 			if (pwrpriv->pwr_mode == PS_MODE_ACTIVE)
-				LPS_RF_ON_check23a(padapter, LPS_LEAVE_TIMEOUT_MS);
+				LPS_RF_ON_check23a(padapter,
+						   LPS_LEAVE_TIMEOUT_MS);
 		}
 	}
 
@@ -462,21 +453,11 @@ void LPS_Leave23a(struct rtw_adapter *padapter)
 void LeaveAllPowerSaveMode23a(struct rtw_adapter *Adapter)
 {
 	struct mlme_priv *pmlmepriv = &Adapter->mlmepriv;
-	u8	enqueue = 0;
-
-
+	u8 enqueue = 0;
 
 	/* DBG_8723A("%s.....\n", __func__); */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-	{ /* connect */
-#ifdef CONFIG_8723AU_P2P
-		p2p_ps_wk_cmd23a(Adapter, P2P_PS_DISABLE, enqueue);
-#endif /* CONFIG_8723AU_P2P */
-
+	if (check_fwstate(pmlmepriv, _FW_LINKED))
 		rtw_lps_ctrl_wk_cmd23a(Adapter, LPS_CTRL_LEAVE, enqueue);
-	}
-
-
 }
 
 void rtw_init_pwrctrl_priv23a(struct rtw_adapter *padapter)
@@ -494,13 +475,15 @@ void rtw_init_pwrctrl_priv23a(struct rtw_adapter *padapter)
 
 	pwrctrlpriv->pwr_state_check_interval = RTW_PWR_STATE_CHK_INTERVAL;
 	pwrctrlpriv->pwr_state_check_cnts = 0;
-	pwrctrlpriv->bInternalAutoSuspend = false;
 	pwrctrlpriv->bInSuspend = false;
 	pwrctrlpriv->bkeepfwalive = false;
 
 	pwrctrlpriv->LpsIdleCount = 0;
-	pwrctrlpriv->power_mgnt = padapter->registrypriv.power_mgnt;/*  PS_MODE_MIN; */
-	pwrctrlpriv->bLeisurePs = (PS_MODE_ACTIVE != pwrctrlpriv->power_mgnt)?true:false;
+
+	/*  PS_MODE_MIN; */
+	pwrctrlpriv->power_mgnt = padapter->registrypriv.power_mgnt;
+	pwrctrlpriv->bLeisurePs =
+		(PS_MODE_ACTIVE != pwrctrlpriv->power_mgnt)?true:false;
 
 	pwrctrlpriv->bFwCurrentInPSMode = false;
 
@@ -517,22 +500,12 @@ void rtw_init_pwrctrl_priv23a(struct rtw_adapter *padapter)
 
 	setup_timer(&pwrctrlpriv->pwr_state_check_timer,
 		    pwr_state_check_handler, (unsigned long)padapter);
-
-
 }
 
 void rtw_free_pwrctrl_priv(struct rtw_adapter *adapter)
 {
 }
 
-u8 rtw_interface_ps_func23a(struct rtw_adapter *padapter, enum hal_intf_ps_func efunc_id, u8* val)
-{
-	u8 bResult = true;
-	rtw_hal_intf_ps_func23a(padapter, efunc_id, val);
-
-	return bResult;
-}
-
 inline void rtw_set_ips_deny23a(struct rtw_adapter *padapter, u32 ms)
 {
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
@@ -570,18 +543,19 @@ int _rtw_pwr_wakeup23a(struct rtw_adapter *padapter, u32 ips_deffer_ms, const ch
 			DBG_8723A("%s wait ps_processing done\n", __func__);
 	}
 
-	if (rtw_hal_sreset_inprogress(padapter)) {
+	if (rtw_sreset_inprogress(padapter)) {
 		DBG_8723A("%s wait sreset_inprogress...\n", __func__);
-		while (rtw_hal_sreset_inprogress(padapter) &&
+		while (rtw_sreset_inprogress(padapter) &&
 		       jiffies_to_msecs(jiffies - start) <= 4000)
 			msleep(10);
-		if (rtw_hal_sreset_inprogress(padapter))
-			DBG_8723A("%s wait sreset_inprogress timeout\n", __func__);
+		if (rtw_sreset_inprogress(padapter))
+			DBG_8723A("%s wait sreset_inprogress timeout\n",
+				  __func__);
 		else
 			DBG_8723A("%s wait sreset_inprogress done\n", __func__);
 	}
 
-	if (pwrpriv->bInternalAutoSuspend == false && pwrpriv->bInSuspend) {
+	if (pwrpriv->bInSuspend) {
 		DBG_8723A("%s wait bInSuspend...\n", __func__);
 		while (pwrpriv->bInSuspend &&
 		       (jiffies_to_msecs(jiffies - start) <= 3000)) {
@@ -594,27 +568,20 @@ int _rtw_pwr_wakeup23a(struct rtw_adapter *padapter, u32 ips_deffer_ms, const ch
 	}
 
 	/* System suspend is not allowed to wakeup */
-	if ((pwrpriv->bInternalAutoSuspend == false) && (true == pwrpriv->bInSuspend)) {
-		ret = _FAIL;
-		goto exit;
-	}
-
-	/* block??? */
-	if ((pwrpriv->bInternalAutoSuspend == true)  && (padapter->net_closed == true)) {
+	if (pwrpriv->bInSuspend) {
 		ret = _FAIL;
 		goto exit;
 	}
 
 	/* I think this should be check in IPS, LPS, autosuspend functions... */
-	if (check_fwstate(pmlmepriv, _FW_LINKED) == true)
-	{
+	if (check_fwstate(pmlmepriv, _FW_LINKED)) {
 		ret = _SUCCESS;
 		goto exit;
 	}
 
 	if (rf_off == pwrpriv->rf_pwrstate) {
 		DBG_8723A("%s call ips_leave23a....\n", __func__);
-		if (_FAIL ==  ips_leave23a(padapter)) {
+		if (ips_leave23a(padapter)== _FAIL) {
 			DBG_8723A("======> ips_leave23a fail.............\n");
 			ret = _FAIL;
 			goto exit;
@@ -627,7 +594,7 @@ int _rtw_pwr_wakeup23a(struct rtw_adapter *padapter, u32 ips_deffer_ms, const ch
 		DBG_8723A("%s: bDriverStopped =%d, bup =%d, hw_init_completed "
 			  "=%u\n", caller, padapter->bDriverStopped,
 			  padapter->bup, padapter->hw_init_completed);
-		ret = false;
+		ret = _FAIL;
 		goto exit;
 	}
 
@@ -640,29 +607,22 @@ exit:
 
 int rtw_pm_set_lps23a(struct rtw_adapter *padapter, u8 mode)
 {
-	int	ret = 0;
+	int ret = 0;
 	struct pwrctrl_priv *pwrctrlpriv = &padapter->pwrctrlpriv;
 
-	if (mode < PS_MODE_NUM)
-	{
-		if (pwrctrlpriv->power_mgnt != mode)
-		{
+	if (mode < PS_MODE_NUM) {
+		if (pwrctrlpriv->power_mgnt != mode) {
 			if (PS_MODE_ACTIVE == mode)
-			{
 				LeaveAllPowerSaveMode23a(padapter);
-			}
 			else
-			{
 				pwrctrlpriv->LpsIdleCount = 2;
-			}
 			pwrctrlpriv->power_mgnt = mode;
-			pwrctrlpriv->bLeisurePs = (PS_MODE_ACTIVE != pwrctrlpriv->power_mgnt)?true:false;
+			pwrctrlpriv->bLeisurePs =
+				(PS_MODE_ACTIVE != pwrctrlpriv->power_mgnt) ?
+				true:false;
 		}
-	}
-	else
-	{
+	} else
 		ret = -EINVAL;
-	}
 
 	return ret;
 }
@@ -673,17 +633,17 @@ int rtw_pm_set_ips23a(struct rtw_adapter *padapter, u8 mode)
 
 	if (mode == IPS_NORMAL || mode == IPS_LEVEL_2) {
 		rtw_ips_mode_req(pwrctrlpriv, mode);
-		DBG_8723A("%s %s\n", __func__, mode == IPS_NORMAL?"IPS_NORMAL":"IPS_LEVEL_2");
+		DBG_8723A("%s %s\n", __func__,
+			  mode == IPS_NORMAL?"IPS_NORMAL":"IPS_LEVEL_2");
 		return 0;
-	}
-	else if (mode == IPS_NONE) {
+	} else if (mode == IPS_NONE) {
 		rtw_ips_mode_req(pwrctrlpriv, mode);
 		DBG_8723A("%s %s\n", __func__, "IPS_NONE");
-		if ((padapter->bSurpriseRemoved == 0)&&_FAIL == rtw_pwr_wakeup(padapter))
+		if (padapter->bSurpriseRemoved == 0 &&
+		    rtw_pwr_wakeup(padapter) == _FAIL)
 			return -EFAULT;
-	}
-	else {
+	} else
 		return -EINVAL;
-	}
+
 	return 0;
 }
diff --git a/drivers/staging/rtl8723au/core/rtw_recv.c b/drivers/staging/rtl8723au/core/rtw_recv.c
index 0b2455e4f5b..690970efc22 100644
--- a/drivers/staging/rtl8723au/core/rtw_recv.c
+++ b/drivers/staging/rtl8723au/core/rtw_recv.c
@@ -19,10 +19,11 @@
 #include <mlme_osdep.h>
 #include <linux/ip.h>
 #include <linux/if_ether.h>
-#include <ethernet.h>
 #include <usb_ops.h>
 #include <linux/ieee80211.h>
 #include <wifi.h>
+#include <rtl8723a_recv.h>
+#include <rtl8723a_xmit.h>
 
 void rtw_signal_stat_timer_hdl23a(unsigned long data);
 
@@ -48,12 +49,6 @@ int _rtw_init_recv_priv23a(struct recv_priv *precvpriv,
 	int i;
 	int res = _SUCCESS;
 
-
-
-	/*  We don't need to memset padapter->XXX to zero, because
-	    adapter is allocated by rtw_zvmalloc(). */
-	/* memset((unsigned char *)precvpriv, 0, sizeof (struct  recv_priv)); */
-
 	spin_lock_init(&precvpriv->lock);
 
 	_rtw_init_queue23a(&precvpriv->free_recv_queue);
@@ -62,35 +57,23 @@ int _rtw_init_recv_priv23a(struct recv_priv *precvpriv,
 
 	precvpriv->adapter = padapter;
 
-	precvpriv->free_recvframe_cnt = NR_RECVFRAME;
-
-	precvpriv->pallocated_frame_buf =
-		rtw_zvmalloc(NR_RECVFRAME * sizeof(struct recv_frame));
-
-	if (precvpriv->pallocated_frame_buf == NULL) {
-		res = _FAIL;
-		goto exit;
-	}
-
-	precvframe = precvpriv->pallocated_frame_buf;
-
 	for (i = 0; i < NR_RECVFRAME ; i++) {
+		precvframe = kzalloc(sizeof(struct recv_frame), GFP_KERNEL);
+		if (!precvframe)
+			break;
 		INIT_LIST_HEAD(&precvframe->list);
 
 		list_add_tail(&precvframe->list,
 			      &precvpriv->free_recv_queue.queue);
 
-		res = rtw_os_recv_resource_alloc23a(padapter, precvframe);
-
 		precvframe->adapter = padapter;
 		precvframe++;
 	}
 
+	precvpriv->free_recvframe_cnt = i;
 	precvpriv->rx_pending_cnt = 1;
 
-	sema_init(&precvpriv->allrxreturnevt, 0);
-
-	res = rtw_hal_init23a_recv_priv(padapter);
+	res = rtl8723au_init_recv_priv(padapter);
 
 	setup_timer(&precvpriv->signal_stat_timer, rtw_signal_stat_timer_hdl23a,
 		    (unsigned long)padapter);
@@ -99,29 +82,24 @@ int _rtw_init_recv_priv23a(struct recv_priv *precvpriv,
 
 	rtw_set_signal_stat_timer(precvpriv);
 
-exit:
-
-
-
 	return res;
 }
 
 void _rtw_free_recv_priv23a (struct recv_priv *precvpriv)
 {
 	struct rtw_adapter *padapter = precvpriv->adapter;
-
-
+	struct recv_frame *precvframe;
+	struct list_head *plist, *ptmp;
 
 	rtw_free_uc_swdec_pending_queue23a(padapter);
 
-	if (precvpriv->pallocated_frame_buf) {
-		rtw_vmfree(precvpriv->pallocated_frame_buf,
-			   NR_RECVFRAME * sizeof(struct recv_frame));
+	list_for_each_safe(plist, ptmp, &precvpriv->free_recv_queue.queue) {
+		precvframe = container_of(plist, struct recv_frame, list);
+		list_del_init(&precvframe->list);
+		kfree(precvframe);
 	}
 
-	rtw_hal_free_recv_priv23a(padapter);
-
-
+	rtl8723au_free_recv_priv(padapter);
 }
 
 struct recv_frame *rtw_alloc_recvframe23a(struct rtw_queue *pfree_recv_queue)
@@ -133,7 +111,7 @@ struct recv_frame *rtw_alloc_recvframe23a(struct rtw_queue *pfree_recv_queue)
 
 	spin_lock_bh(&pfree_recv_queue->lock);
 
-	if (_rtw_queue_empty23a(pfree_recv_queue) == true)
+	if (list_empty(&pfree_recv_queue->queue))
 		pframe = NULL;
 	else {
 		phead = get_list_head(pfree_recv_queue);
@@ -156,18 +134,18 @@ struct recv_frame *rtw_alloc_recvframe23a(struct rtw_queue *pfree_recv_queue)
 	return pframe;
 }
 
-int rtw_free_recvframe23a(struct recv_frame *precvframe, struct rtw_queue *pfree_recv_queue)
+int rtw_free_recvframe23a(struct recv_frame *precvframe)
 {
 	struct rtw_adapter *padapter = precvframe->adapter;
 	struct recv_priv *precvpriv = &padapter->recvpriv;
-
-
+	struct rtw_queue *pfree_recv_queue;
 
 	if (precvframe->pkt) {
 		dev_kfree_skb_any(precvframe->pkt);/* free skb by driver */
 		precvframe->pkt = NULL;
 	}
 
+	pfree_recv_queue = &precvpriv->free_recv_queue;
 	spin_lock_bh(&pfree_recv_queue->lock);
 
 	list_del_init(&precvframe->list);
@@ -215,12 +193,11 @@ using spinlock to protect
 
 */
 
-void rtw_free_recvframe23a_queue(struct rtw_queue *pframequeue,  struct rtw_queue *pfree_recv_queue)
+static void rtw_free_recvframe23a_queue(struct rtw_queue *pframequeue)
 {
 	struct recv_frame *hdr;
 	struct list_head *plist, *phead, *ptmp;
 
-
 	spin_lock(&pframequeue->lock);
 
 	phead = get_list_head(pframequeue);
@@ -228,12 +205,10 @@ void rtw_free_recvframe23a_queue(struct rtw_queue *pframequeue,  struct rtw_queu
 
 	list_for_each_safe(plist, ptmp, phead) {
 		hdr = container_of(plist, struct recv_frame, list);
-		rtw_free_recvframe23a(hdr, pfree_recv_queue);
+		rtw_free_recvframe23a(hdr);
 	}
 
 	spin_unlock(&pframequeue->lock);
-
-
 }
 
 u32 rtw_free_uc_swdec_pending_queue23a(struct rtw_adapter *adapter)
@@ -241,8 +216,7 @@ u32 rtw_free_uc_swdec_pending_queue23a(struct rtw_adapter *adapter)
 	u32 cnt = 0;
 	struct recv_frame *pending_frame;
 	while ((pending_frame = rtw_alloc_recvframe23a(&adapter->recvpriv.uc_swdec_pending_queue))) {
-		rtw_free_recvframe23a(pending_frame,
-				   &adapter->recvpriv.free_recv_queue);
+		rtw_free_recvframe23a(pending_frame);
 		DBG_8723A("%s: dequeue uc_swdec_pending_queue\n", __func__);
 		cnt++;
 	}
@@ -282,7 +256,7 @@ struct recv_buf *rtw_dequeue_recvbuf23a (struct rtw_queue *queue)
 
 	spin_lock_irqsave(&queue->lock, irqL);
 
-	if (_rtw_queue_empty23a(queue) == true) {
+	if (list_empty(&queue->queue)) {
 		precvbuf = NULL;
 	} else {
 		phead = get_list_head(queue);
@@ -321,9 +295,9 @@ int recvframe_chkmic(struct rtw_adapter *adapter,
 
 	stainfo = rtw_get_stainfo23a(&adapter->stapriv, &prxattrib->ta[0]);
 
-	if (prxattrib->encrypt == _TKIP_) {
+	if (prxattrib->encrypt == WLAN_CIPHER_SUITE_TKIP) {
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
-			 ("\n recvframe_chkmic:prxattrib->encrypt == _TKIP_\n"));
+			 ("\n recvframe_chkmic:prxattrib->encrypt == WLAN_CIPHER_SUITE_TKIP\n"));
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
 			 ("\n recvframe_chkmic:da = 0x%02x:0x%02x:0x%02x:0x%02x:"
 			  "0x%02x:0x%02x\n", prxattrib->ra[0],
@@ -338,7 +312,7 @@ int recvframe_chkmic(struct rtw_adapter *adapter,
 				RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
 					 ("\n recvframe_chkmic: bcmc key\n"));
 
-				if (psecuritypriv->binstallGrpkey == false) {
+				if (!psecuritypriv->binstallGrpkey) {
 					res = _FAIL;
 					RT_TRACE(_module_rtl871x_recv_c_,
 						 _drv_err_,
@@ -467,9 +441,9 @@ int recvframe_chkmic(struct rtw_adapter *adapter,
 				res = _FAIL;
 			} else {
 				/* mic checked ok */
-				if ((psecuritypriv->bcheck_grpkey == false) &&
-				    (is_multicast_ether_addr(prxattrib->ra))) {
-					psecuritypriv->bcheck_grpkey = true;
+				if (!psecuritypriv->bcheck_grpkey &&
+				    is_multicast_ether_addr(prxattrib->ra)) {
+					psecuritypriv->bcheck_grpkey = 1;
 					RT_TRACE(_module_rtl871x_recv_c_,
 						 _drv_err_,
 						 ("psecuritypriv->bcheck_grp"
@@ -501,8 +475,7 @@ struct recv_frame *decryptor(struct rtw_adapter *padapter,
 	struct rx_pkt_attrib *prxattrib = &precv_frame->attrib;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 	struct recv_frame *return_packet = precv_frame;
-	u32 res = _SUCCESS;
-
+	int res = _SUCCESS;
 
 	RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
 		 ("prxstat->decrypted =%x prxattrib->encrypt = 0x%03x\n",
@@ -517,13 +490,13 @@ struct recv_frame *decryptor(struct rtw_adapter *padapter,
 				  prxattrib->key_index);
 
 			switch (prxattrib->encrypt) {
-			case _WEP40_:
-			case _WEP104_:
+			case WLAN_CIPHER_SUITE_WEP40:
+			case WLAN_CIPHER_SUITE_WEP104:
 				prxattrib->key_index =
 					psecuritypriv->dot11PrivacyKeyIndex;
 				break;
-			case _TKIP_:
-			case _AES_:
+			case WLAN_CIPHER_SUITE_TKIP:
+			case WLAN_CIPHER_SUITE_CCMP:
 			default:
 				prxattrib->key_index =
 					psecuritypriv->dot118021XGrpKeyid;
@@ -533,16 +506,16 @@ struct recv_frame *decryptor(struct rtw_adapter *padapter,
 	}
 
 	if ((prxattrib->encrypt > 0) && ((prxattrib->bdecrypted == 0))) {
-		psecuritypriv->hw_decrypted = false;
+		psecuritypriv->hw_decrypted = 0;
 		switch (prxattrib->encrypt) {
-		case _WEP40_:
-		case _WEP104_:
+		case WLAN_CIPHER_SUITE_WEP40:
+		case WLAN_CIPHER_SUITE_WEP104:
 			rtw_wep_decrypt23a(padapter, precv_frame);
 			break;
-		case _TKIP_:
+		case WLAN_CIPHER_SUITE_TKIP:
 			res = rtw_tkip_decrypt23a(padapter, precv_frame);
 			break;
-		case _AES_:
+		case WLAN_CIPHER_SUITE_CCMP:
 			res = rtw_aes_decrypt23a(padapter, precv_frame);
 			break;
 		default:
@@ -550,13 +523,12 @@ struct recv_frame *decryptor(struct rtw_adapter *padapter,
 		}
 	} else if (prxattrib->bdecrypted == 1 && prxattrib->encrypt > 0 &&
 		   (psecuritypriv->busetkipkey == 1 ||
-		    prxattrib->encrypt != _TKIP_)) {
-			psecuritypriv->hw_decrypted = true;
+		    prxattrib->encrypt != WLAN_CIPHER_SUITE_TKIP)) {
+			psecuritypriv->hw_decrypted = 1;
 	}
 
 	if (res == _FAIL) {
-		rtw_free_recvframe23a(return_packet,
-				   &padapter->recvpriv.free_recv_queue);
+		rtw_free_recvframe23a(return_packet);
 		return_packet = NULL;
 	}
 
@@ -569,31 +541,35 @@ struct recv_frame *decryptor(struct rtw_adapter *padapter,
 static struct recv_frame *portctrl(struct rtw_adapter *adapter,
 				   struct recv_frame *precv_frame)
 {
-	u8 *psta_addr = NULL, *ptr;
+	u8 *psta_addr, *ptr;
 	uint auth_alg;
 	struct recv_frame *pfhdr;
 	struct sta_info *psta;
 	struct sta_priv *pstapriv ;
 	struct recv_frame *prtnframe;
-	u16 ether_type = 0;
-	u16 eapol_type = 0x888e;/* for Funia BD's WPA issue */
+	u16 ether_type;
+	u16 eapol_type = ETH_P_PAE;/* for Funia BD's WPA issue */
 	struct rx_pkt_attrib *pattrib;
 
 	pstapriv = &adapter->stapriv;
-	psta = rtw_get_stainfo23a(pstapriv, psta_addr);
 
 	auth_alg = adapter->securitypriv.dot11AuthAlgrthm;
 
-	ptr = precv_frame->pkt->data;
 	pfhdr = precv_frame;
 	pattrib = &pfhdr->attrib;
 	psta_addr = pattrib->ta;
+	psta = rtw_get_stainfo23a(pstapriv, psta_addr);
 
 	RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
 		 ("########portctrl:adapter->securitypriv.dot11AuthAlgrthm ="
 		  "%d\n", adapter->securitypriv.dot11AuthAlgrthm));
 
-	if (auth_alg == 2) {
+	if (auth_alg == dot11AuthAlgrthm_8021X) {
+		/* get ether_type */
+		ptr = pfhdr->pkt->data + pfhdr->attrib.hdrlen;
+
+		ether_type = (ptr[6] << 8) | ptr[7];
+
 		if ((psta != NULL) && (psta->ieee8021x_blocked)) {
 			/* blocked */
 			/* only accept EAPOL frame */
@@ -601,19 +577,11 @@ static struct recv_frame *portctrl(struct rtw_adapter *adapter,
 				 ("########portctrl:psta->ieee8021x_blocked =="
 				  "1\n"));
 
-			prtnframe = precv_frame;
-
-			/* get ether_type */
-			ptr = ptr + pfhdr->attrib.hdrlen + LLC_HEADER_SIZE;
-			memcpy(&ether_type, ptr, 2);
-			ether_type = ntohs((unsigned short)ether_type);
-
 		        if (ether_type == eapol_type) {
 				prtnframe = precv_frame;
 			} else {
 				/* free this frame */
-				rtw_free_recvframe23a(precv_frame,
-						   &adapter->recvpriv.free_recv_queue);
+				rtw_free_recvframe23a(precv_frame);
 				prtnframe = NULL;
 			}
 		} else {
@@ -651,9 +619,7 @@ static struct recv_frame *portctrl(struct rtw_adapter *adapter,
 		prtnframe = precv_frame;
 	}
 
-
-
-		return prtnframe;
+	return prtnframe;
 }
 
 int recv_decache(struct recv_frame *precv_frame, u8 bretry,
@@ -833,8 +799,8 @@ static int sta2sta_data_frame(struct rtw_adapter *adapter,
 
 
 
-	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true) ||
-		(check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true)) {
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) ||
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
 
 		/*  filter packets that SA is myself or multicast or broadcast */
 		if (ether_addr_equal(myhwaddr, pattrib->src)) {
@@ -857,7 +823,7 @@ static int sta2sta_data_frame(struct rtw_adapter *adapter,
 		}
 
 		sta_addr = pattrib->src;
-	} else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) {
+	} else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
 		/*  For Station mode, sa and bssid should always be BSSID,
 		    and DA is my mac-address */
 		if (!ether_addr_equal(pattrib->bssid, pattrib->src)) {
@@ -870,7 +836,7 @@ static int sta2sta_data_frame(struct rtw_adapter *adapter,
 
 		sta_addr = pattrib->bssid;
 
-	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) {
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
 		if (bmcast) {
 			/*  For AP mode, if DA == MCAST, then BSSID should be also MCAST */
 			if (!is_multicast_ether_addr(pattrib->bssid)) {
@@ -887,7 +853,7 @@ static int sta2sta_data_frame(struct rtw_adapter *adapter,
 
 			sta_addr = pattrib->src;
 		}
-	} else if (check_fwstate(pmlmepriv, WIFI_MP_STATE) == true) {
+	} else if (check_fwstate(pmlmepriv, WIFI_MP_STATE)) {
 		ether_addr_copy(pattrib->dst, hdr->addr1);
 		ether_addr_copy(pattrib->src, hdr->addr2);
 		ether_addr_copy(pattrib->bssid, hdr->addr3);
@@ -934,9 +900,9 @@ int ap2sta_data_frame(struct rtw_adapter *adapter,
 
 
 
-	if ((check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) &&
-	    (check_fwstate(pmlmepriv, _FW_LINKED) == true ||
-	     check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == true)) {
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) &&
+	    (check_fwstate(pmlmepriv, _FW_LINKED) ||
+	     check_fwstate(pmlmepriv, _FW_UNDER_LINKING))) {
 
 		/* filter packets that SA is myself or multicast or broadcast */
 		if (ether_addr_equal(myhwaddr, pattrib->src)) {
@@ -999,8 +965,8 @@ int ap2sta_data_frame(struct rtw_adapter *adapter,
 			goto exit;
 		}
 
-	} else if ((check_fwstate(pmlmepriv, WIFI_MP_STATE) == true) &&
-		   (check_fwstate(pmlmepriv, _FW_LINKED) == true)) {
+	} else if (check_fwstate(pmlmepriv, WIFI_MP_STATE) &&
+		   check_fwstate(pmlmepriv, _FW_LINKED)) {
 		ether_addr_copy(pattrib->dst, hdr->addr1);
 		ether_addr_copy(pattrib->src, hdr->addr2);
 		ether_addr_copy(pattrib->bssid, hdr->addr3);
@@ -1018,7 +984,7 @@ int ap2sta_data_frame(struct rtw_adapter *adapter,
 			ret = _FAIL;
 			goto exit;
 		}
-	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) {
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
 		/* Special case */
 		ret = RTW_RX_HANDLED;
 		goto exit;
@@ -1062,7 +1028,7 @@ int sta2ap_data_frame(struct rtw_adapter *adapter,
 
 
 
-	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) {
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
 		/* For AP mode, RA = BSSID, TX = STA(SRC_ADDR), A3 = DST_ADDR */
 		if (!ether_addr_equal(pattrib->bssid, mybssid)) {
 			ret = _FAIL;
@@ -1119,17 +1085,14 @@ exit:
 	return ret;
 }
 
-int validate_recv_ctrl_frame(struct rtw_adapter *padapter,
-			     struct recv_frame *precv_frame);
-int validate_recv_ctrl_frame(struct rtw_adapter *padapter,
-			     struct recv_frame *precv_frame)
+static int validate_recv_ctrl_frame(struct rtw_adapter *padapter,
+				    struct recv_frame *precv_frame)
 {
 #ifdef CONFIG_8723AU_AP_MODE
 	struct rx_pkt_attrib *pattrib = &precv_frame->attrib;
 	struct sta_priv *pstapriv = &padapter->stapriv;
 	struct sk_buff *skb = precv_frame->pkt;
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-	u8 *pframe = skb->data;
 
 	if (!ieee80211_is_ctl(hdr->frame_control))
 		return _FAIL;
@@ -1140,14 +1103,15 @@ int validate_recv_ctrl_frame(struct rtw_adapter *padapter,
 
 	/* only handle ps-poll */
 	if (ieee80211_is_pspoll(hdr->frame_control)) {
+		struct ieee80211_pspoll *psp = (struct ieee80211_pspoll *)hdr;
 		u16 aid;
 		u8 wmmps_ac = 0;
 		struct sta_info *psta = NULL;
 
-		aid = GetAid(pframe);
+		aid = le16_to_cpu(psp->aid) & 0x3fff;
 		psta = rtw_get_stainfo23a(pstapriv, hdr->addr2);
 
-		if ((!psta) || (psta->aid != aid))
+		if (!psta || psta->aid != aid)
 			return _FAIL;
 
 		/* for rx pkt statistics */
@@ -1213,7 +1177,8 @@ int validate_recv_ctrl_frame(struct rtw_adapter *padapter,
 
 	                        /* DBG_8723A("handling ps-poll, q_len =%d, tim =%x\n", psta->sleepq_len, pstapriv->tim_bitmap); */
 
-				rtw_hal_xmit23aframe_enqueue(padapter, pxmitframe);
+				rtl8723au_hal_xmitframe_enqueue(padapter,
+								pxmitframe);
 
 				if (psta->sleepq_len == 0) {
 					pstapriv->tim_bitmap &= ~CHKBIT(psta->aid);
@@ -1222,8 +1187,8 @@ int validate_recv_ctrl_frame(struct rtw_adapter *padapter,
 
 					/* upate BCN for TIM IE */
 					/* update_BCNTIM(padapter); */
-					update_beacon23a(padapter, _TIM_IE_,
-						      NULL, false);
+					update_beacon23a(padapter, WLAN_EID_TIM,
+							 NULL, false);
 				}
 
 				/* spin_unlock_bh(&psta->sleep_q.lock); */
@@ -1254,8 +1219,8 @@ int validate_recv_ctrl_frame(struct rtw_adapter *padapter,
 
 					/* upate BCN for TIM IE */
 					/* update_BCNTIM(padapter); */
-					update_beacon23a(padapter, _TIM_IE_,
-						      NULL, false);
+					update_beacon23a(padapter, WLAN_EID_TIM,
+							 NULL, false);
 				}
 			}
 		}
@@ -1267,10 +1232,8 @@ int validate_recv_ctrl_frame(struct rtw_adapter *padapter,
 
 struct recv_frame* recvframe_chk_defrag23a(struct rtw_adapter *padapter,
 					struct recv_frame *precv_frame);
-int validate_recv_mgnt_frame(struct rtw_adapter *padapter,
-			     struct recv_frame *precv_frame);
-int validate_recv_mgnt_frame(struct rtw_adapter *padapter,
-			     struct recv_frame *precv_frame)
+static int validate_recv_mgnt_frame(struct rtw_adapter *padapter,
+				    struct recv_frame *precv_frame)
 {
 	struct sta_info *psta;
 	struct sk_buff *skb;
@@ -1316,15 +1279,12 @@ int validate_recv_mgnt_frame(struct rtw_adapter *padapter,
 	return _SUCCESS;
 }
 
-int validate_recv_data_frame(struct rtw_adapter *adapter,
-			     struct recv_frame *precv_frame);
-int validate_recv_data_frame(struct rtw_adapter *adapter,
-			     struct recv_frame *precv_frame)
+static int validate_recv_data_frame(struct rtw_adapter *adapter,
+				    struct recv_frame *precv_frame)
 {
 	u8 bretry;
-	u8 *psa, *pda, *pbssid;
+	u8 *psa, *pda;
 	struct sta_info *psta = NULL;
-	u8 *ptr = precv_frame->pkt->data;
 	struct rx_pkt_attrib *pattrib = & precv_frame->attrib;
 	struct security_priv *psecuritypriv = &adapter->securitypriv;
 	int ret = _SUCCESS;
@@ -1336,48 +1296,44 @@ int validate_recv_data_frame(struct rtw_adapter *adapter,
 	bretry = ieee80211_has_retry(hdr->frame_control);
 	pda = ieee80211_get_DA(hdr);
 	psa = ieee80211_get_SA(hdr);
-	pbssid = get_hdr_bssid(ptr);
-
-	if (pbssid == NULL) {
-		ret = _FAIL;
-		goto exit;
-	}
 
 	ether_addr_copy(pattrib->dst, pda);
 	ether_addr_copy(pattrib->src, psa);
 
-	ether_addr_copy(pattrib->bssid, pbssid);
-
-	switch (pattrib->to_fr_ds)
-	{
-	case 0:
+	switch (hdr->frame_control &
+		cpu_to_le16(IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) {
+	case cpu_to_le16(0):
+		ether_addr_copy(pattrib->bssid, hdr->addr3);
 		ether_addr_copy(pattrib->ra, pda);
 		ether_addr_copy(pattrib->ta, psa);
 		ret = sta2sta_data_frame(adapter, precv_frame, &psta);
 		break;
 
-	case 1:
+	case cpu_to_le16(IEEE80211_FCTL_FROMDS):
+		ether_addr_copy(pattrib->bssid, hdr->addr2);
 		ether_addr_copy(pattrib->ra, pda);
-		ether_addr_copy(pattrib->ta, pbssid);
+		ether_addr_copy(pattrib->ta, hdr->addr2);
 		ret = ap2sta_data_frame(adapter, precv_frame, &psta);
 		break;
 
-	case 2:
-		ether_addr_copy(pattrib->ra, pbssid);
+	case cpu_to_le16(IEEE80211_FCTL_TODS):
+		ether_addr_copy(pattrib->bssid, hdr->addr1);
+		ether_addr_copy(pattrib->ra, hdr->addr1);
 		ether_addr_copy(pattrib->ta, psa);
 		ret = sta2ap_data_frame(adapter, precv_frame, &psta);
 		break;
 
-	case 3:
+	case cpu_to_le16(IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS):
+		/*
+		 * There is no BSSID in this case, but the driver has been
+		 * using addr1 so far, so keep it for now.
+		 */
+		ether_addr_copy(pattrib->bssid, hdr->addr1);
 		ether_addr_copy(pattrib->ra, hdr->addr1);
 		ether_addr_copy(pattrib->ta, hdr->addr2);
 		ret = _FAIL;
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, (" case 3\n"));
 		break;
-
-	default:
-		ret = _FAIL;
-		break;
 	}
 
 	if ((ret == _FAIL) || (ret == RTW_RX_HANDLED))
@@ -1451,22 +1407,18 @@ int validate_recv_data_frame(struct rtw_adapter *adapter,
 
 		switch (pattrib->encrypt)
 		{
-		case _WEP40_:
-		case _WEP104_:
-			pattrib->iv_len = 4;
-			pattrib->icv_len = 4;
+		case WLAN_CIPHER_SUITE_WEP40:
+		case WLAN_CIPHER_SUITE_WEP104:
+			pattrib->iv_len = IEEE80211_WEP_IV_LEN;
+			pattrib->icv_len = IEEE80211_WEP_ICV_LEN;
 			break;
-		case _TKIP_:
-			pattrib->iv_len = 8;
-			pattrib->icv_len = 4;
+		case WLAN_CIPHER_SUITE_TKIP:
+			pattrib->iv_len = IEEE80211_TKIP_IV_LEN;
+			pattrib->icv_len = IEEE80211_TKIP_ICV_LEN;
 			break;
-		case _AES_:
-			pattrib->iv_len = 8;
-			pattrib->icv_len = 8;
-			break;
-		case _SMS4_:
-			pattrib->iv_len = 18;
-			pattrib->icv_len = 16;
+		case WLAN_CIPHER_SUITE_CCMP:
+			pattrib->iv_len = IEEE80211_CCMP_HDR_LEN;
+			pattrib->icv_len = IEEE80211_CCMP_MIC_LEN;
 			break;
 		default:
 			pattrib->iv_len = 0;
@@ -1538,8 +1490,6 @@ static int validate_recv_frame(struct rtw_adapter *adapter,
 		goto exit;
 	}
 
-	pattrib->to_fr_ds = get_tofr_ds(hdr->frame_control);
-
 	seq_ctrl = le16_to_cpu(hdr->seq_ctrl);
 	pattrib->frag_num = seq_ctrl & IEEE80211_SCTL_FRAG;
 	pattrib->seq_num = seq_ctrl >> 4;
@@ -1550,7 +1500,7 @@ static int validate_recv_frame(struct rtw_adapter *adapter,
 	pattrib->privacy = ieee80211_has_protected(hdr->frame_control);
 	pattrib->order = ieee80211_has_order(hdr->frame_control);
 
-	rtw_hal_get_def_var23a(adapter, HAL_DEF_DBG_DUMP_RXPKT, &bDumpRxPkt);
+	GetHalDefVar8192CUsb(adapter, HAL_DEF_DBG_DUMP_RXPKT, &bDumpRxPkt);
 
 	if (unlikely(bDumpRxPkt == 1))
 		dump_rx_pkt(skb, type, bDumpRxPkt);
@@ -1638,7 +1588,7 @@ static int wlanhdr_to_ethhdr (struct recv_frame *precvframe)
 		  pattrib->hdrlen,  pattrib->iv_len));
 
 	pattrib->eth_type = eth_type;
-	if ((check_fwstate(pmlmepriv, WIFI_MP_STATE) == true)) {
+	if (check_fwstate(pmlmepriv, WIFI_MP_STATE)) {
 		ptr += hdrlen;
 		*ptr = 0x87;
 		*(ptr + 1) = 0x12;
@@ -1694,8 +1644,8 @@ struct recv_frame *recvframe_defrag(struct rtw_adapter *adapter,
 	if (curfragnum != prframe->attrib.frag_num) {
 		/* the first fragment number must be 0 */
 		/* free the whole queue */
-		rtw_free_recvframe23a(prframe, pfree_recv_queue);
-		rtw_free_recvframe23a_queue(defrag_q, pfree_recv_queue);
+		rtw_free_recvframe23a(prframe);
+		rtw_free_recvframe23a_queue(defrag_q);
 
 		return NULL;
 	}
@@ -1715,8 +1665,8 @@ struct recv_frame *recvframe_defrag(struct rtw_adapter *adapter,
 			/* the fragment number must be increasing
 			   (after decache) */
 			/* release the defrag_q & prframe */
-			rtw_free_recvframe23a(prframe, pfree_recv_queue);
-			rtw_free_recvframe23a_queue(defrag_q, pfree_recv_queue);
+			rtw_free_recvframe23a(prframe);
+			rtw_free_recvframe23a_queue(defrag_q);
 			return NULL;
 		}
 
@@ -1744,7 +1694,7 @@ struct recv_frame *recvframe_defrag(struct rtw_adapter *adapter,
 	};
 
 	/* free the defrag_q queue and return the prframe */
-	rtw_free_recvframe23a_queue(defrag_q, pfree_recv_queue);
+	rtw_free_recvframe23a_queue(defrag_q);
 
 	RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
 		 ("Performance defrag!!!!!\n"));
@@ -1803,9 +1753,9 @@ struct recv_frame* recvframe_chk_defrag23a(struct rtw_adapter *padapter,
 		if (pdefrag_q != NULL) {
 			if (fragnum == 0) {
 				/* the first fragment */
-				if (_rtw_queue_empty23a(pdefrag_q) == false) {
+				if (!list_empty(&pdefrag_q->queue)) {
 					/* free current defrag_q */
-					rtw_free_recvframe23a_queue(pdefrag_q, pfree_recv_queue);
+					rtw_free_recvframe23a_queue(pdefrag_q);
 				}
 			}
 
@@ -1826,7 +1776,7 @@ struct recv_frame* recvframe_chk_defrag23a(struct rtw_adapter *padapter,
 		} else {
 			/* can't find this ta's defrag_queue,
 			   so free this recv_frame */
-			rtw_free_recvframe23a(precv_frame, pfree_recv_queue);
+			rtw_free_recvframe23a(precv_frame);
 			prtnframe = NULL;
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
 				 ("Free because pdefrag_q == NULL: ismfrag = "
@@ -1852,7 +1802,7 @@ struct recv_frame* recvframe_chk_defrag23a(struct rtw_adapter *padapter,
 		} else {
 			/* can't find this ta's defrag_queue,
 			   so free this recv_frame */
-			rtw_free_recvframe23a(precv_frame, pfree_recv_queue);
+			rtw_free_recvframe23a(precv_frame);
 			prtnframe = NULL;
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
 				 ("Free because pdefrag_q == NULL: ismfrag = "
@@ -1867,7 +1817,7 @@ struct recv_frame* recvframe_chk_defrag23a(struct rtw_adapter *padapter,
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
 				 ("recvframe_chkmic(padapter,  prtnframe) =="
 				  "_FAIL\n"));
-			rtw_free_recvframe23a(prtnframe, pfree_recv_queue);
+			rtw_free_recvframe23a(prtnframe);
 			prtnframe = NULL;
 		}
 	}
@@ -1883,8 +1833,6 @@ int amsdu_to_msdu(struct rtw_adapter *padapter, struct recv_frame *prframe)
 	struct rx_pkt_attrib *pattrib;
 	struct sk_buff *skb, *sub_skb;
 	struct sk_buff_head skb_list;
-	struct recv_priv *precvpriv = &padapter->recvpriv;
-	struct rtw_queue *pfree_recv_queue = &precvpriv->free_recv_queue;
 
 	pattrib = &prframe->attrib;
 
@@ -1906,7 +1854,7 @@ int amsdu_to_msdu(struct rtw_adapter *padapter, struct recv_frame *prframe)
 	}
 
 	prframe->pkt = NULL;
-	rtw_free_recvframe23a(prframe, pfree_recv_queue);
+	rtw_free_recvframe23a(prframe);
 	return _SUCCESS;
 }
 
@@ -1942,8 +1890,8 @@ int check_indicate_seq(struct recv_reorder_ctrl *preorder_ctrl, u16 seq_num)
 	return true;
 }
 
-int enqueue_reorder_recvframe23a(struct recv_reorder_ctrl *preorder_ctrl,
-			      struct recv_frame *prframe)
+static int enqueue_reorder_recvframe23a(struct recv_reorder_ctrl *preorder_ctrl,
+				        struct recv_frame *prframe)
 {
 	struct rx_pkt_attrib *pattrib = &prframe->attrib;
 	struct rtw_queue *ppending_recvframe_queue;
@@ -2061,10 +2009,8 @@ int recv_indicatepkts_in_order(struct rtw_adapter *padapter,
 				}
 			} else {
 				if (amsdu_to_msdu(padapter, prframe) !=
-				    _SUCCESS) {
-					rtw_free_recvframe23a(prframe,
-							   &precvpriv->free_recv_queue);
-				}
+				    _SUCCESS)
+					rtw_free_recvframe23a(prframe);
 			}
 
 			/* Update local variables. */
@@ -2102,7 +2048,7 @@ int recv_indicatepkt_reorder(struct rtw_adapter *padapter,
 		/* s1. */
 		wlanhdr_to_ethhdr(prframe);
 
-		if ((pattrib->qos!= 1) || (pattrib->eth_type == 0x0806) ||
+		if ((pattrib->qos!= 1) || (pattrib->eth_type == ETH_P_ARP) ||
 		    (pattrib->ack_policy != 0)) {
 			if ((padapter->bDriverStopped == false) &&
 			    (padapter->bSurpriseRemoved == false)) {
@@ -2269,7 +2215,6 @@ int process_recv_indicatepkts(struct rtw_adapter *padapter,
 static int recv_func_prehandle(struct rtw_adapter *padapter,
 			       struct recv_frame *rframe)
 {
-	struct rtw_queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
 	int ret = _SUCCESS;
 
 	/* check the frame crtl field and decache */
@@ -2277,7 +2222,7 @@ static int recv_func_prehandle(struct rtw_adapter *padapter,
 	if (ret != _SUCCESS) {
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
 			 ("recv_func: validate_recv_frame fail! drop pkt\n"));
-		rtw_free_recvframe23a(rframe, pfree_recv_queue);
+		rtw_free_recvframe23a(rframe);
 		goto exit;
 	}
 
@@ -2291,7 +2236,6 @@ static int recv_func_posthandle(struct rtw_adapter *padapter,
 	int ret = _SUCCESS;
 	struct recv_frame *orig_prframe = prframe;
 	struct recv_priv *precvpriv = &padapter->recvpriv;
-	struct rtw_queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
 
 	/*  DATA FRAME */
 	rtw_led_control(padapter, LED_CTL_RX);
@@ -2337,7 +2281,7 @@ static int recv_func_posthandle(struct rtw_adapter *padapter,
 	if (ret != _SUCCESS) {
 		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
 			 ("recv_func: process_recv_indicatepkts fail!\n"));
-		rtw_free_recvframe23a(orig_prframe, pfree_recv_queue);/* free this recv_frame */
+		rtw_free_recvframe23a(orig_prframe);/* free this recv_frame */
 		goto _recv_data_drop;
 	}
 	return ret;
@@ -2434,8 +2378,7 @@ void rtw_signal_stat_timer_hdl23a(unsigned long data)
 		}
 
 		/* update value of signal_strength, rssi, signal_qual */
-		if (check_fwstate(&adapter->mlmepriv, _FW_UNDER_SURVEY) ==
-		    false) {
+		if (!check_fwstate(&adapter->mlmepriv, _FW_UNDER_SURVEY)) {
 			tmp_s = (avg_signal_strength + (_alpha - 1) *
 				 recvpriv->signal_strength);
 			if (tmp_s %_alpha)
diff --git a/drivers/staging/rtl8723au/core/rtw_security.c b/drivers/staging/rtl8723au/core/rtw_security.c
index fd43e71bf6d..d6c07adc7bf 100644
--- a/drivers/staging/rtl8723au/core/rtw_security.c
+++ b/drivers/staging/rtl8723au/core/rtw_security.c
@@ -170,16 +170,17 @@ void rtw_wep_encrypt23a(struct rtw_adapter *padapter,
 	pframe = pxmitframe->buf_addr + hw_hdr_offset;
 
 	/* start to encrypt each fragment */
-	if ((pattrib->encrypt != _WEP40_) && (pattrib->encrypt != _WEP104_))
+	if (pattrib->encrypt != WLAN_CIPHER_SUITE_WEP40 &&
+	    pattrib->encrypt != WLAN_CIPHER_SUITE_WEP104)
 		return;
 
 	index = psecuritypriv->dot11PrivacyKeyIndex;
-	keylength = psecuritypriv->dot11DefKeylen[index];
+	keylength = psecuritypriv->wep_key[index].keylen;
 
 	for (curfragnum = 0; curfragnum < pattrib->nr_frags ; curfragnum++) {
 		iv = pframe + pattrib->hdrlen;
 		memcpy(&wepkey[0], iv, 3);
-		memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[index].skey[0],
+		memcpy(&wepkey[3], &psecuritypriv->wep_key[index].key,
 		       keylength);
 		payload = pframe + pattrib->iv_len + pattrib->hdrlen;
 
@@ -225,17 +226,17 @@ void rtw_wep_decrypt23a(struct rtw_adapter *padapter,
 	pframe = skb->data;
 
 	/* start to decrypt recvframe */
-	if ((prxattrib->encrypt != _WEP40_) && (prxattrib->encrypt != _WEP104_))
+	if (prxattrib->encrypt != WLAN_CIPHER_SUITE_WEP40 &&
+	    prxattrib->encrypt != WLAN_CIPHER_SUITE_WEP104)
 		return;
 
 	iv = pframe + prxattrib->hdrlen;
 	/* keyindex = (iv[3]&0x3); */
 	keyindex = prxattrib->key_index;
-	keylength = psecuritypriv->dot11DefKeylen[keyindex];
+	keylength = psecuritypriv->wep_key[keyindex].keylen;
 	memcpy(&wepkey[0], iv, 3);
 	/* memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[psecuritypriv->dot11PrivacyKeyIndex].skey[0], keylength); */
-	memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[keyindex].skey[0],
-		   keylength);
+	memcpy(&wepkey[3], &psecuritypriv->wep_key[keyindex].key, keylength);
 	length = skb->len - prxattrib->hdrlen - prxattrib->iv_len;
 
 	payload = pframe + prxattrib->iv_len + prxattrib->hdrlen;
@@ -248,7 +249,7 @@ void rtw_wep_decrypt23a(struct rtw_adapter *padapter,
 	*((u32 *)crc) = le32_to_cpu(getcrc32(payload, length - 4));
 
 	if (crc[3] != payload[length - 1] || crc[2] != payload[length - 2] ||
-		crc[1] != payload[length - 3] || crc[0] != payload[length - 4]) {
+	    crc[1] != payload[length - 3] || crc[0] != payload[length - 4]) {
 		RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
 			 ("rtw_wep_decrypt23a:icv error crc[3](%x)!= payload"
 			  "[length-1](%x) || crc[2](%x)!= payload[length-2](%x)"
@@ -610,9 +611,9 @@ static void phase2(u8 *rc4key, const u8 *tk, const u16 *p1k, u16 iv16)
 }
 
 /* The hlen isn't include the IV */
-u32 rtw_tkip_encrypt23a(struct rtw_adapter *padapter,
-		     struct xmit_frame *pxmitframe)
-{																	/*  exclude ICV */
+int rtw_tkip_encrypt23a(struct rtw_adapter *padapter,
+			struct xmit_frame *pxmitframe)
+{
 	u16	pnl;
 	u32	pnh;
 	u8	rc4key[16];
@@ -622,14 +623,13 @@ u32 rtw_tkip_encrypt23a(struct rtw_adapter *padapter,
 	struct arc4context mycontext;
 	int			curfragnum, length;
 	u32	prwskeylen;
-
 	u8	*pframe, *payload,*iv,*prwskey;
 	union pn48 dot11txpn;
 	struct	sta_info		*stainfo;
 	struct	pkt_attrib	 *pattrib = &pxmitframe->attrib;
 	struct	security_priv	*psecuritypriv = &padapter->securitypriv;
 	struct	xmit_priv		*pxmitpriv = &padapter->xmitpriv;
-	u32	res = _SUCCESS;
+	int res = _SUCCESS;
 
 	if (!pxmitframe->buf_addr)
 		return _FAIL;
@@ -638,14 +638,10 @@ u32 rtw_tkip_encrypt23a(struct rtw_adapter *padapter,
 
 	pframe = pxmitframe->buf_addr + hw_hdr_offset;
 	/* 4 start to encrypt each fragment */
-	if (pattrib->encrypt == _TKIP_) {
-
+	if (pattrib->encrypt == WLAN_CIPHER_SUITE_TKIP) {
 		if (pattrib->psta)
-		{
 			stainfo = pattrib->psta;
-		}
-		else
-		{
+		else {
 			DBG_8723A("%s, call rtw_get_stainfo()\n", __func__);
 			stainfo = rtw_get_stainfo23a(&padapter->stapriv,
 						     &pattrib->ra[0]);
@@ -716,10 +712,9 @@ u32 rtw_tkip_encrypt23a(struct rtw_adapter *padapter,
 }
 
 /* The hlen isn't include the IV */
-u32 rtw_tkip_decrypt23a(struct rtw_adapter *padapter,
-		     struct recv_frame *precvframe)
+int rtw_tkip_decrypt23a(struct rtw_adapter *padapter,
+			struct recv_frame *precvframe)
 {
-	/*  exclude ICV */
 	u16 pnl;
 	u32 pnh;
 	u8   rc4key[16];
@@ -734,19 +729,19 @@ u32 rtw_tkip_decrypt23a(struct rtw_adapter *padapter,
 	struct	rx_pkt_attrib *prxattrib = &precvframe->attrib;
 	struct	security_priv *psecuritypriv = &padapter->securitypriv;
 	struct sk_buff * skb = precvframe->pkt;
-	u32	res = _SUCCESS;
+	int res = _SUCCESS;
 
 	pframe = skb->data;
 
 	/* 4 start to decrypt recvframe */
-	if (prxattrib->encrypt == _TKIP_) {
+	if (prxattrib->encrypt == WLAN_CIPHER_SUITE_TKIP) {
 
 		stainfo = rtw_get_stainfo23a(&padapter->stapriv,
 					     &prxattrib->ta[0]);
 		if (stainfo!= NULL) {
 
 			if (is_multicast_ether_addr(prxattrib->ra)) {
-				if (psecuritypriv->binstallGrpkey == false) {
+				if (psecuritypriv->binstallGrpkey == 0) {
 					res = _FAIL;
 					DBG_8723A("%s:rx bc/mc packets, but didn't install group key!!!!!!!!!!\n", __func__);
 					goto exit;
@@ -1303,7 +1298,8 @@ static int aes_cipher(u8 *key, uint hdrlen, u8 *pframe, uint plen)
 	return _SUCCESS;
 }
 
-u32 rtw_aes_encrypt23a(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe)
+int rtw_aes_encrypt23a(struct rtw_adapter *padapter,
+		       struct xmit_frame *pxmitframe)
 {	/*  exclude ICV */
 	/* Intermediate Buffers */
 	int curfragnum, length;
@@ -1314,7 +1310,7 @@ u32 rtw_aes_encrypt23a(struct rtw_adapter *padapter, struct xmit_frame *pxmitfra
 	struct pkt_attrib *pattrib = &pxmitframe->attrib;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	u32 res = _SUCCESS;
+	int res = _SUCCESS;
 
 	if (!pxmitframe->buf_addr)
 		return _FAIL;
@@ -1324,7 +1320,7 @@ u32 rtw_aes_encrypt23a(struct rtw_adapter *padapter, struct xmit_frame *pxmitfra
 	pframe = pxmitframe->buf_addr + hw_hdr_offset;
 
 	/* 4 start to encrypt each fragment */
-	if (pattrib->encrypt != _AES_)
+	if (pattrib->encrypt != WLAN_CIPHER_SUITE_CCMP)
 		return _FAIL;
 
 	if (pattrib->psta) {
@@ -1586,7 +1582,8 @@ static int aes_decipher(u8 *key, uint	hdrlen,
 	return res;
 }
 
-u32 rtw_aes_decrypt23a(struct rtw_adapter *padapter, struct recv_frame *precvframe)
+int rtw_aes_decrypt23a(struct rtw_adapter *padapter,
+		       struct recv_frame *precvframe)
 {	/*  exclude ICV */
 	struct sta_info *stainfo;
 	struct rx_pkt_attrib *prxattrib = &precvframe->attrib;
@@ -1594,11 +1591,11 @@ u32 rtw_aes_decrypt23a(struct rtw_adapter *padapter, struct recv_frame *precvfra
 	struct sk_buff *skb = precvframe->pkt;
 	int length;
 	u8 *pframe, *prwskey;	/*  *payload,*iv */
-	u32 res = _SUCCESS;
+	int res = _SUCCESS;
 
 	pframe = skb->data;
 	/* 4 start to encrypt each fragment */
-	if (prxattrib->encrypt != _AES_)
+	if (prxattrib->encrypt != WLAN_CIPHER_SUITE_CCMP)
 		return _FAIL;
 
 	stainfo = rtw_get_stainfo23a(&padapter->stapriv, &prxattrib->ta[0]);
@@ -1645,7 +1642,7 @@ void rtw_use_tkipkey_handler23a(void *FunctionContext)
 	struct rtw_adapter *padapter = (struct rtw_adapter *)FunctionContext;
 
 	RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("^^^rtw_use_tkipkey_handler23a ^^^\n"));
-	padapter->securitypriv.busetkipkey = true;
+	padapter->securitypriv.busetkipkey = 1;
 	RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
 		 ("^^^rtw_use_tkipkey_handler23a padapter->securitypriv.busetkipkey =%d^^^\n",
 		 padapter->securitypriv.busetkipkey));
diff --git a/drivers/staging/rtl8723au/core/rtw_sreset.c b/drivers/staging/rtl8723au/core/rtw_sreset.c
index 4f745920339..18a42a27b48 100644
--- a/drivers/staging/rtl8723au/core/rtw_sreset.c
+++ b/drivers/staging/rtl8723au/core/rtw_sreset.c
@@ -13,11 +13,12 @@
  *
  ******************************************************************************/
 
-#include<rtw_sreset.h>
+#include <rtw_sreset.h>
+#include <usb_ops_linux.h>
 
-void sreset_init_value23a(struct rtw_adapter *padapter)
+void rtw_sreset_init(struct rtw_adapter *padapter)
 {
-	struct hal_data_8723a	*pHalData = GET_HAL_DATA(padapter);
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 	struct sreset_priv *psrtpriv = &pHalData->srestpriv;
 
 	mutex_init(&psrtpriv->silentreset_mutex);
@@ -26,9 +27,10 @@ void sreset_init_value23a(struct rtw_adapter *padapter)
 	psrtpriv->last_tx_time = 0;
 	psrtpriv->last_tx_complete_time = 0;
 }
-void sreset_reset_value23a(struct rtw_adapter *padapter)
+
+void rtw_sreset_reset_value(struct rtw_adapter *padapter)
 {
-	struct hal_data_8723a	*pHalData = GET_HAL_DATA(padapter);
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 	struct sreset_priv *psrtpriv = &pHalData->srestpriv;
 
 	psrtpriv->silent_reset_inprogress = false;
@@ -37,16 +39,16 @@ void sreset_reset_value23a(struct rtw_adapter *padapter)
 	psrtpriv->last_tx_complete_time = 0;
 }
 
-u8 sreset_get_wifi_status23a(struct rtw_adapter *padapter)
+u8 rtw_sreset_get_wifi_status(struct rtw_adapter *padapter)
 {
-	struct hal_data_8723a	*pHalData = GET_HAL_DATA(padapter);
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 	struct sreset_priv *psrtpriv = &pHalData->srestpriv;
 	u8 status = WIFI_STATUS_SUCCESS;
 	u32 val32 = 0;
 
 	if (psrtpriv->silent_reset_inprogress)
 		return status;
-	val32 = rtw_read32(padapter, REG_TXDMA_STATUS);
+	val32 = rtl8723au_read32(padapter, REG_TXDMA_STATUS);
 	if (val32 == 0xeaeaeaea) {
 		psrtpriv->Wifi_Error_Status = WIFI_IF_NOT_EXIST;
 	} else if (val32 != 0) {
@@ -80,9 +82,10 @@ void sreset_set_trigger_point(struct rtw_adapter *padapter, s32 tgp)
 	pHalData->srestpriv.dbg_trigger_point = tgp;
 }
 
-bool sreset_inprogress(struct rtw_adapter *padapter)
+bool rtw_sreset_inprogress(struct rtw_adapter *padapter)
 {
-	struct hal_data_8723a	*pHalData = GET_HAL_DATA(padapter);
+	struct rtw_adapter *primary_adapter = GET_PRIMARY_ADAPTER(padapter);
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(primary_adapter);
 
 	return pHalData->srestpriv.silent_reset_inprogress;
 }
@@ -99,10 +102,13 @@ static void sreset_restore_security_station(struct rtw_adapter *padapter)
 		val8 = 0xcc;
 	else
 		val8 = 0xcf;
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
 
-	if ((padapter->securitypriv.dot11PrivacyAlgrthm == _TKIP_) ||
-	    (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)) {
+	rtl8723a_set_sec_cfg(padapter, val8);
+
+	if (padapter->securitypriv.dot11PrivacyAlgrthm ==
+	    WLAN_CIPHER_SUITE_TKIP ||
+	    padapter->securitypriv.dot11PrivacyAlgrthm ==
+	    WLAN_CIPHER_SUITE_CCMP) {
 		psta = rtw_get_stainfo23a(pstapriv, get_bssid(mlmepriv));
 		if (psta == NULL) {
 			/* DEBUG_ERR(("Set wpa_set_encryption: Obtain Sta_info fail\n")); */
@@ -122,7 +128,7 @@ static void sreset_restore_network_station(struct rtw_adapter *padapter)
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	u8 threshold;
 
-	rtw_setopmode_cmd23a(padapter, Ndis802_11Infrastructure);
+	rtw_setopmode_cmd23a(padapter, NL80211_IFTYPE_STATION);
 
 	/*  TH = 1 => means that invalidate usb rx aggregation */
 	/*  TH = 0 => means that validate usb rx aggregation, use init value. */
@@ -131,29 +137,22 @@ static void sreset_restore_network_station(struct rtw_adapter *padapter)
 			threshold = 1;
 		else
 			threshold = 0;
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_RXDMA_AGG_PG_TH, (u8 *)(&threshold));
-	} else {
+	} else
 		threshold = 1;
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_RXDMA_AGG_PG_TH, (u8 *)(&threshold));
-	}
 
-	set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+	rtl8723a_set_rxdma_agg_pg_th(padapter, threshold);
 
-	/* disable dynamic functions, such as high power, DIG */
-	/* Switch_DM_Func23a(padapter, DYNAMIC_FUNC_DISABLE, false); */
+	set_channel_bwmode23a(padapter, pmlmeext->cur_channel,
+			      pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
 
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_BSSID, pmlmeinfo->network.MacAddress);
-
-	{
-		u8	join_type = 0;
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
-	}
+	hw_var_set_bssid(padapter, pmlmeinfo->network.MacAddress);
+	hw_var_set_mlme_join(padapter, 0);
 
 	Set_MSR23a(padapter, (pmlmeinfo->state & 0x3));
 
 	mlmeext_joinbss_event_callback23a(padapter, 1);
 	/* restore Sequence No. */
-	rtw_write8(padapter, 0x4dc, padapter->xmitpriv.nqos_ssn);
+	rtl8723au_write8(padapter, 0x4dc, padapter->xmitpriv.nqos_ssn);
 
 	sreset_restore_security_station(padapter);
 }
@@ -163,17 +162,24 @@ static void sreset_restore_network_status(struct rtw_adapter *padapter)
 	struct mlme_priv *mlmepriv = &padapter->mlmepriv;
 
 	if (check_fwstate(mlmepriv, WIFI_STATION_STATE)) {
-		DBG_8723A(FUNC_ADPT_FMT" fwstate:0x%08x - WIFI_STATION_STATE\n", FUNC_ADPT_ARG(padapter), get_fwstate(mlmepriv));
+		DBG_8723A("%s(%s): fwstate:0x%08x - WIFI_STATION_STATE\n",
+			  __func__, padapter->pnetdev->name,
+			  get_fwstate(mlmepriv));
 		sreset_restore_network_station(padapter);
 #ifdef CONFIG_8723AU_AP_MODE
 	} else if (check_fwstate(mlmepriv, WIFI_AP_STATE)) {
-		DBG_8723A(FUNC_ADPT_FMT" fwstate:0x%08x - WIFI_AP_STATE\n", FUNC_ADPT_ARG(padapter), get_fwstate(mlmepriv));
+		DBG_8723A("%s(%s): fwstate:0x%08x - WIFI_AP_STATE\n",
+			  __func__, padapter->pnetdev->name,
+			  get_fwstate(mlmepriv));
 		rtw_ap_restore_network(padapter);
 #endif
 	} else if (check_fwstate(mlmepriv, WIFI_ADHOC_STATE)) {
-		DBG_8723A(FUNC_ADPT_FMT" fwstate:0x%08x - WIFI_ADHOC_STATE\n", FUNC_ADPT_ARG(padapter), get_fwstate(mlmepriv));
+		DBG_8723A("%s(%s): fwstate:0x%08x - WIFI_ADHOC_STATE\n",
+			  __func__, padapter->pnetdev->name,
+			  get_fwstate(mlmepriv));
 	} else {
-		DBG_8723A(FUNC_ADPT_FMT" fwstate:0x%08x - ???\n", FUNC_ADPT_ARG(padapter), get_fwstate(mlmepriv));
+		DBG_8723A("%s(%s): fwstate:0x%08x - ???\n", __func__,
+			  padapter->pnetdev->name, get_fwstate(mlmepriv));
 	}
 }
 
@@ -185,7 +191,7 @@ static void sreset_stop_adapter(struct rtw_adapter *padapter)
 	if (padapter == NULL)
 		return;
 
-	DBG_8723A(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter));
+	DBG_8723A("%s(%s)\n", __func__, padapter->pnetdev->name);
 
 	if (!rtw_netif_queue_stopped(padapter->pnetdev))
 		netif_tx_stop_all_queues(padapter->pnetdev);
@@ -210,7 +216,7 @@ static void sreset_start_adapter(struct rtw_adapter *padapter)
 	if (padapter == NULL)
 		return;
 
-	DBG_8723A(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter));
+	DBG_8723A("%s(%s)\n", __func__, padapter->pnetdev->name);
 
 	if (check_fwstate(pmlmepriv, _FW_LINKED)) {
 		sreset_restore_network_status(padapter);
@@ -226,9 +232,10 @@ static void sreset_start_adapter(struct rtw_adapter *padapter)
 		netif_tx_wake_all_queues(padapter->pnetdev);
 }
 
-void sreset_reset(struct rtw_adapter *padapter)
+void rtw_sreset_reset(struct rtw_adapter *active_adapter)
 {
-	struct hal_data_8723a	*pHalData = GET_HAL_DATA(padapter);
+	struct rtw_adapter *padapter = GET_PRIMARY_ADAPTER(active_adapter);
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 	struct sreset_priv *psrtpriv = &pHalData->srestpriv;
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
 	unsigned long start = jiffies;
diff --git a/drivers/staging/rtl8723au/core/rtw_sta_mgt.c b/drivers/staging/rtl8723au/core/rtw_sta_mgt.c
index 451b58f4728..14a82bea826 100644
--- a/drivers/staging/rtl8723au/core/rtw_sta_mgt.c
+++ b/drivers/staging/rtl8723au/core/rtw_sta_mgt.c
@@ -20,8 +20,9 @@
 #include <xmit_osdep.h>
 #include <mlme_osdep.h>
 #include <sta_info.h>
+#include <rtl8723a_hal.h>
 
-void _rtw_init_stainfo(struct sta_info *psta)
+static void _rtw_init_stainfo(struct sta_info *psta)
 {
 	memset((u8 *)psta, 0, sizeof (struct sta_info));
 	spin_lock_init(&psta->lock);
@@ -48,31 +49,15 @@ void _rtw_init_stainfo(struct sta_info *psta)
 #endif	/*  CONFIG_8723AU_AP_MODE */
 }
 
-u32 _rtw_init_sta_priv23a(struct sta_priv *pstapriv)
+int _rtw_init_sta_priv23a(struct sta_priv *pstapriv)
 {
-	struct sta_info *psta;
-	s32 i;
-
-	pstapriv->pallocated_stainfo_buf = rtw_zvmalloc(sizeof(struct sta_info) * NUM_STA+ 4);
-
-	if (!pstapriv->pallocated_stainfo_buf)
-		return _FAIL;
+	int i;
 
-	pstapriv->pstainfo_buf = pstapriv->pallocated_stainfo_buf + 4 -
-		((unsigned long)(pstapriv->pallocated_stainfo_buf) & 3);
-	_rtw_init_queue23a(&pstapriv->free_sta_queue);
 	spin_lock_init(&pstapriv->sta_hash_lock);
 	pstapriv->asoc_sta_count = 0;
-	_rtw_init_queue23a(&pstapriv->sleep_q);
-	_rtw_init_queue23a(&pstapriv->wakeup_q);
-	psta = (struct sta_info *)(pstapriv->pstainfo_buf);
-
-	for (i = 0; i < NUM_STA; i++) {
-		_rtw_init_stainfo(psta);
+	for (i = 0; i < NUM_STA; i++)
 		INIT_LIST_HEAD(&pstapriv->sta_hash[i]);
-		list_add_tail(&psta->list, get_list_head(&pstapriv->free_sta_queue));
-		psta++;
-	}
+
 #ifdef CONFIG_8723AU_AP_MODE
 	pstapriv->sta_dz_bitmap = 0;
 	pstapriv->tim_bitmap = 0;
@@ -92,49 +77,12 @@ u32 _rtw_init_sta_priv23a(struct sta_priv *pstapriv)
 	return _SUCCESS;
 }
 
-inline int rtw_stainfo_offset23a(struct sta_priv *stapriv, struct sta_info *sta)
-{
-	int offset = (((u8 *)sta) - stapriv->pstainfo_buf)/sizeof(struct sta_info);
-
-	if (!stainfo_offset_valid(offset))
-		DBG_8723A("%s invalid offset(%d), out of range!!!", __func__, offset);
-	return offset;
-}
-
-inline struct sta_info *rtw_get_stainfo23a_by_offset23a(struct sta_priv *stapriv, int offset)
-{
-	if (!stainfo_offset_valid(offset))
-		DBG_8723A("%s invalid offset(%d), out of range!!!", __func__, offset);
-	return (struct sta_info *)(stapriv->pstainfo_buf + offset * sizeof(struct sta_info));
-}
-
-/*  this function is used to free the memory of lock || sema for all stainfos */
-void rtw_mfree_all_stainfo(struct sta_priv *pstapriv)
-{
-	struct list_head *plist, *phead;
-	struct sta_info *psta;
-
-	spin_lock_bh(&pstapriv->sta_hash_lock);
-
-	phead = get_list_head(&pstapriv->free_sta_queue);
-
-	/* we really achieve a lot in this loop .... */
-	list_for_each(plist, phead)
-		psta = container_of(plist, struct sta_info, list);
-	spin_unlock_bh(&pstapriv->sta_hash_lock);
-}
-
-void rtw_mfree_sta_priv_lock(struct	sta_priv *pstapriv)
-{
-	rtw_mfree_all_stainfo(pstapriv); /* be done before free sta_hash_lock */
-}
-
-u32	_rtw_free_sta_priv23a(struct	sta_priv *pstapriv)
+int _rtw_free_sta_priv23a(struct sta_priv *pstapriv)
 {
 	struct list_head *phead, *plist, *ptmp;
 	struct sta_info *psta;
 	struct recv_reorder_ctrl *preorder_ctrl;
-	int	index;
+	int index;
 
 	if (pstapriv) {
 		/*	delete all reordering_ctrl_timer		*/
@@ -154,39 +102,25 @@ u32	_rtw_free_sta_priv23a(struct	sta_priv *pstapriv)
 		}
 		spin_unlock_bh(&pstapriv->sta_hash_lock);
 		/*===============================*/
-
-		rtw_mfree_sta_priv_lock(pstapriv);
-
-		if (pstapriv->pallocated_stainfo_buf)
-			rtw_vmfree(pstapriv->pallocated_stainfo_buf, sizeof(struct sta_info)*NUM_STA+4);
 	}
 	return _SUCCESS;
 }
 
-struct	sta_info *rtw_alloc_stainfo23a(struct sta_priv *pstapriv, u8 *hwaddr)
+struct sta_info *
+rtw_alloc_stainfo23a(struct sta_priv *pstapriv, u8 *hwaddr, gfp_t gfp)
 {
 	struct list_head	*phash_list;
 	struct sta_info	*psta;
-	struct rtw_queue *pfree_sta_queue;
 	struct recv_reorder_ctrl *preorder_ctrl;
-	uint tmp_aid;
 	s32	index;
 	int i = 0;
 	u16  wRxSeqInitialValue = 0xffff;
 
-	pfree_sta_queue = &pstapriv->free_sta_queue;
-
-	spin_lock_bh(&pstapriv->sta_hash_lock);
-
-	if (_rtw_queue_empty23a(pfree_sta_queue)) {
-		spin_unlock_bh(&pstapriv->sta_hash_lock);
+	psta = kmalloc(sizeof(struct sta_info), gfp);
+	if (!psta)
 		return NULL;
-	}
-	psta = container_of((&pfree_sta_queue->queue)->next, struct sta_info, list);
 
-	list_del_init(&psta->list);
-
-	tmp_aid = psta->aid;
+	spin_lock_bh(&pstapriv->sta_hash_lock);
 
 	_rtw_init_stainfo(psta);
 
@@ -252,9 +186,8 @@ exit:
 }
 
 /*  using pstapriv->sta_hash_lock to protect */
-u32	rtw_free_stainfo23a(struct rtw_adapter *padapter, struct sta_info *psta)
+int rtw_free_stainfo23a(struct rtw_adapter *padapter, struct sta_info *psta)
 {
-	struct rtw_queue *pfree_sta_queue;
 	struct recv_reorder_ctrl *preorder_ctrl;
 	struct	sta_xmit_priv	*pstaxmitpriv;
 	struct	xmit_priv	*pxmitpriv = &padapter->xmitpriv;
@@ -269,8 +202,6 @@ u32	rtw_free_stainfo23a(struct rtw_adapter *padapter, struct sta_info *psta)
 	psta->state &= ~_FW_LINKED;
 	spin_unlock_bh(&psta->lock);
 
-	pfree_sta_queue = &pstapriv->free_sta_queue;
-
 	pstaxmitpriv = &psta->sta_xmitpriv;
 
 	spin_lock_bh(&pxmitpriv->lock);
@@ -323,7 +254,6 @@ u32	rtw_free_stainfo23a(struct rtw_adapter *padapter, struct sta_info *psta)
 		struct list_head	*phead, *plist;
 		struct recv_frame *prframe;
 		struct rtw_queue *ppending_recvframe_queue;
-		struct rtw_queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
 
 		preorder_ctrl = &psta->recvreorder_ctrl[i];
 
@@ -339,12 +269,12 @@ u32	rtw_free_stainfo23a(struct rtw_adapter *padapter, struct sta_info *psta)
 			prframe = container_of(plist, struct recv_frame, list);
 			plist = plist->next;
 			list_del_init(&prframe->list);
-			rtw_free_recvframe23a(prframe, pfree_recv_queue);
+			rtw_free_recvframe23a(prframe);
 		}
 		spin_unlock_bh(&ppending_recvframe_queue->lock);
 	}
 	if (!(psta->state & WIFI_AP_STATE))
-		rtw_hal_set_odm_var23a(padapter, HAL_ODM_STA_INFO, psta, false);
+		rtl8723a_SetHalODMVar(padapter, HAL_ODM_STA_INFO, psta, false);
 #ifdef CONFIG_8723AU_AP_MODE
 	spin_lock_bh(&pstapriv->auth_list_lock);
 	if (!list_empty(&psta->auth_list)) {
@@ -374,7 +304,8 @@ u32	rtw_free_stainfo23a(struct rtw_adapter *padapter, struct sta_info *psta)
 		psta->aid = 0;
 	}
 #endif	/*  CONFIG_8723AU_AP_MODE */
-	list_add_tail(&psta->list, get_list_head(pfree_sta_queue));
+
+	kfree(psta);
 exit:
 	return _SUCCESS;
 }
@@ -384,9 +315,11 @@ void rtw_free_all_stainfo23a(struct rtw_adapter *padapter)
 {
 	struct list_head *plist, *phead, *ptmp;
 	struct sta_info *psta;
-	struct	sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_priv *pstapriv = &padapter->stapriv;
 	struct sta_info* pbcmc_stainfo = rtw_get_bcmc_stainfo23a(padapter);
-	s32	index;	if (pstapriv->asoc_sta_count == 1)
+	s32 index;
+
+	if (pstapriv->asoc_sta_count == 1)
 		return;
 
 	spin_lock_bh(&pstapriv->sta_hash_lock);
@@ -405,12 +338,12 @@ void rtw_free_all_stainfo23a(struct rtw_adapter *padapter)
 }
 
 /* any station allocated can be searched by hash list */
-struct sta_info *rtw_get_stainfo23a(struct sta_priv *pstapriv, u8 *hwaddr)
+struct sta_info *rtw_get_stainfo23a(struct sta_priv *pstapriv, const u8 *hwaddr)
 {
 	struct list_head *plist, *phead;
 	struct sta_info *psta = NULL;
 	u32	index;
-	u8 *addr;
+	const u8 *addr;
 	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 
 	if (hwaddr == NULL)
@@ -440,15 +373,15 @@ struct sta_info *rtw_get_stainfo23a(struct sta_priv *pstapriv, u8 *hwaddr)
 	return psta;
 }
 
-u32 rtw_init_bcmc_stainfo23a(struct rtw_adapter* padapter)
+int rtw_init_bcmc_stainfo23a(struct rtw_adapter* padapter)
 {
 	struct	sta_priv *pstapriv = &padapter->stapriv;
 	struct sta_info		*psta;
 	struct tx_servq	*ptxservq;
-	u32 res = _SUCCESS;
+	int res = _SUCCESS;
 	unsigned char bcast_addr[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 
-	psta = rtw_alloc_stainfo23a(pstapriv, bcast_addr);
+	psta = rtw_alloc_stainfo23a(pstapriv, bcast_addr, GFP_KERNEL);
 	if (psta == NULL) {
 		res = _FAIL;
 		RT_TRACE(_module_rtl871x_sta_mgt_c_, _drv_err_,
@@ -472,13 +405,13 @@ struct sta_info *rtw_get_bcmc_stainfo23a(struct rtw_adapter *padapter)
 	return psta;
 }
 
-u8 rtw_access_ctrl23a(struct rtw_adapter *padapter, u8 *mac_addr)
+bool rtw_access_ctrl23a(struct rtw_adapter *padapter, u8 *mac_addr)
 {
-	u8 res = true;
+	bool res = true;
 #ifdef CONFIG_8723AU_AP_MODE
 	struct list_head *plist, *phead;
 	struct rtw_wlan_acl_node *paclnode;
-	u8 match = false;
+	bool match = false;
 	struct sta_priv *pstapriv = &padapter->stapriv;
 	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
 	struct rtw_queue *pacl_node_q = &pacl_list->acl_node_q;
diff --git a/drivers/staging/rtl8723au/core/rtw_wlan_util.c b/drivers/staging/rtl8723au/core/rtw_wlan_util.c
index 0dfcfbce3b5..579a4a8c827 100644
--- a/drivers/staging/rtl8723au/core/rtw_wlan_util.c
+++ b/drivers/staging/rtl8723au/core/rtw_wlan_util.c
@@ -32,29 +32,36 @@ static unsigned char REALTEK_OUI[] = {0x00, 0xe0, 0x4c};
 static unsigned char AIRGOCAP_OUI[] = {0x00, 0x0a, 0xf5};
 static unsigned char EPIGRAM_OUI[] = {0x00, 0x90, 0x4c};
 
-unsigned char REALTEK_96B_IE23A[] = {0x00, 0xe0, 0x4c, 0x02, 0x01, 0x20};
+static unsigned char WPA_TKIP_CIPHER[4] = {0x00, 0x50, 0xf2, 0x02};
+static unsigned char RSN_TKIP_CIPHER[4] = {0x00, 0x0f, 0xac, 0x02};
 
-#define R2T_PHY_DELAY	(0)
+#define R2T_PHY_DELAY		0
 
-/* define WAIT_FOR_BCN_TO_MIN	(3000) */
-#define WAIT_FOR_BCN_TO_MIN	(6000)
-#define WAIT_FOR_BCN_TO_MAX	(20000)
+/* define WAIT_FOR_BCN_TO_MIN	3000 */
+#define WAIT_FOR_BCN_TO_MIN	6000
+#define WAIT_FOR_BCN_TO_MAX	20000
 
 static u8 rtw_basic_rate_cck[4] = {
-	IEEE80211_CCK_RATE_1MB|IEEE80211_BASIC_RATE_MASK, IEEE80211_CCK_RATE_2MB|IEEE80211_BASIC_RATE_MASK,
-	IEEE80211_CCK_RATE_5MB|IEEE80211_BASIC_RATE_MASK, IEEE80211_CCK_RATE_11MB|IEEE80211_BASIC_RATE_MASK
+	IEEE80211_CCK_RATE_1MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_CCK_RATE_2MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_CCK_RATE_5MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_CCK_RATE_11MB | IEEE80211_BASIC_RATE_MASK
 };
 
 static u8 rtw_basic_rate_ofdm[3] = {
-	IEEE80211_OFDM_RATE_6MB|IEEE80211_BASIC_RATE_MASK, IEEE80211_OFDM_RATE_12MB|IEEE80211_BASIC_RATE_MASK,
-	IEEE80211_OFDM_RATE_24MB|IEEE80211_BASIC_RATE_MASK
+	IEEE80211_OFDM_RATE_6MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_OFDM_RATE_12MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_OFDM_RATE_24MB | IEEE80211_BASIC_RATE_MASK
 };
 
 static u8 rtw_basic_rate_mix[7] = {
-	IEEE80211_CCK_RATE_1MB|IEEE80211_BASIC_RATE_MASK, IEEE80211_CCK_RATE_2MB|IEEE80211_BASIC_RATE_MASK,
-	IEEE80211_CCK_RATE_5MB|IEEE80211_BASIC_RATE_MASK, IEEE80211_CCK_RATE_11MB|IEEE80211_BASIC_RATE_MASK,
-	IEEE80211_OFDM_RATE_6MB|IEEE80211_BASIC_RATE_MASK, IEEE80211_OFDM_RATE_12MB|IEEE80211_BASIC_RATE_MASK,
-	IEEE80211_OFDM_RATE_24MB|IEEE80211_BASIC_RATE_MASK
+	IEEE80211_CCK_RATE_1MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_CCK_RATE_2MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_CCK_RATE_5MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_CCK_RATE_11MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_OFDM_RATE_6MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_OFDM_RATE_12MB | IEEE80211_BASIC_RATE_MASK,
+	IEEE80211_OFDM_RATE_24MB | IEEE80211_BASIC_RATE_MASK
 };
 
 int cckrates_included23a(unsigned char *rate, int ratelen)
@@ -62,8 +69,8 @@ int cckrates_included23a(unsigned char *rate, int ratelen)
 	int	i;
 
 	for (i = 0; i < ratelen; i++) {
-		if  ((((rate[i]) & 0x7f) == 2)	|| (((rate[i]) & 0x7f) == 4) ||
-		     (((rate[i]) & 0x7f) == 11)  || (((rate[i]) & 0x7f) == 22))
+		if  (((rate[i]) & 0x7f) == 2 || ((rate[i]) & 0x7f) == 4 ||
+		     ((rate[i]) & 0x7f) == 11  || ((rate[i]) & 0x7f) == 22)
 			return true;
 	}
 
@@ -75,8 +82,8 @@ int cckratesonly_included23a(unsigned char *rate, int ratelen)
 	int	i;
 
 	for (i = 0; i < ratelen; i++) {
-		if  ((((rate[i]) & 0x7f) != 2) && (((rate[i]) & 0x7f) != 4) &&
-			   (((rate[i]) & 0x7f) != 11)  && (((rate[i]) & 0x7f) != 22))
+		if  (((rate[i]) & 0x7f) != 2 && ((rate[i]) & 0x7f) != 4 &&
+		     ((rate[i]) & 0x7f) != 11 && ((rate[i]) & 0x7f) != 22)
 		return false;
 	}
 
@@ -116,10 +123,11 @@ unsigned char networktype_to_raid23a(unsigned char network_type)
 	return raid;
 }
 
-u8 judge_network_type23a(struct rtw_adapter *padapter, unsigned char *rate, int ratelen)
+u8 judge_network_type23a(struct rtw_adapter *padapter,
+			 unsigned char *rate, int ratelen)
 {
 	u8 network_type = 0;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
 	if (pmlmeext->cur_channel > 14) {
@@ -140,7 +148,7 @@ u8 judge_network_type23a(struct rtw_adapter *padapter, unsigned char *rate, int
 	return	network_type;
 }
 
-unsigned char ratetbl_val_2wifirate(unsigned char rate)
+static unsigned char ratetbl_val_2wifirate(unsigned char rate)
 {
 	unsigned char val = 0;
 
@@ -185,7 +193,7 @@ unsigned char ratetbl_val_2wifirate(unsigned char rate)
 	return val;
 }
 
-int is_basicrate(struct rtw_adapter *padapter, unsigned char rate)
+static int is_basicrate(struct rtw_adapter *padapter, unsigned char rate)
 {
 	int i;
 	unsigned char val;
@@ -194,7 +202,7 @@ int is_basicrate(struct rtw_adapter *padapter, unsigned char rate)
 	for (i = 0; i < NumRates; i++) {
 		val = pmlmeext->basicrate[i];
 
-		if ((val != 0xff) && (val != 0xfe)) {
+		if (val != 0xff && val != 0xfe) {
 			if (rate == ratetbl_val_2wifirate(val))
 				return true;
 		}
@@ -203,11 +211,12 @@ int is_basicrate(struct rtw_adapter *padapter, unsigned char rate)
 	return false;
 }
 
-unsigned int ratetbl2rateset(struct rtw_adapter *padapter, unsigned char *rateset)
+static unsigned int ratetbl2rateset(struct rtw_adapter *padapter,
+				    unsigned char *rateset)
 {
 	int i;
 	unsigned char rate;
-	unsigned int	len = 0;
+	unsigned int len = 0;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 
 	for (i = 0; i < NumRates; i++) {
@@ -232,7 +241,8 @@ unsigned int ratetbl2rateset(struct rtw_adapter *padapter, unsigned char *ratese
 	return len;
 }
 
-void get_rate_set23a(struct rtw_adapter *padapter, unsigned char *pbssrate, int *bssrate_len)
+void get_rate_set23a(struct rtw_adapter *padapter,
+		     unsigned char *pbssrate, int *bssrate_len)
 {
 	unsigned char supportedrates[NumRates];
 
@@ -243,8 +253,8 @@ void get_rate_set23a(struct rtw_adapter *padapter, unsigned char *pbssrate, int
 
 void UpdateBrateTbl23a(struct rtw_adapter *Adapter, u8 *mBratesOS)
 {
-	u8	i;
-	u8	rate;
+	u8 i;
+	u8 rate;
 
 	/*  1M, 2M, 5.5M, 11M, 6M, 12M, 24M are mandatory. */
 	for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
@@ -267,8 +277,8 @@ void UpdateBrateTbl23a(struct rtw_adapter *Adapter, u8 *mBratesOS)
 
 void Update23aTblForSoftAP(u8 *bssrateset, u32 bssratelen)
 {
-	u8	i;
-	u8	rate;
+	u8 i;
+	u8 rate;
 
 	for (i = 0; i < bssratelen; i++) {
 		rate = bssrateset[i] & 0x7f;
@@ -283,35 +293,9 @@ void Update23aTblForSoftAP(u8 *bssrateset, u32 bssratelen)
 	}
 }
 
-void Save_DM_Func_Flag23a(struct rtw_adapter *padapter)
-{
-	u8	bSaveFlag = true;
-
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_DM_FUNC_OP, (u8 *)(&bSaveFlag));
-}
-
-void Restore_DM_Func_Flag23a(struct rtw_adapter *padapter)
-{
-	u8	bSaveFlag = false;
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_DM_FUNC_OP, (u8 *)(&bSaveFlag));
-}
-
-void Switch_DM_Func23a(struct rtw_adapter *padapter, unsigned long mode, u8 enable)
-{
-	if (enable == true)
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_DM_FUNC_SET, (u8 *)(&mode));
-	else
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_DM_FUNC_CLR, (u8 *)(&mode));
-}
-
-static void Set_NETYPE0_MSR(struct rtw_adapter *padapter, u8 type)
-{
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_MEDIA_STATUS, (u8 *)(&type));
-}
-
 void Set_MSR23a(struct rtw_adapter *padapter, u8 type)
 {
-		Set_NETYPE0_MSR(padapter, type);
+	rtl8723a_set_media_status(padapter, type);
 }
 
 inline u8 rtw_get_oper_ch23a(struct rtw_adapter *adapter)
@@ -351,12 +335,13 @@ void SelectChannel23a(struct rtw_adapter *padapter, unsigned char channel)
 	/* saved channel info */
 	rtw_set_oper_ch23a(padapter, channel);
 
-	rtw_hal_set_chan23a(padapter, channel);
+	PHY_SwChnl8723A(padapter, channel);
 
 	mutex_unlock(&adapter_to_dvobj(padapter)->setch_mutex);
 }
 
-void SetBWMode23a(struct rtw_adapter *padapter, unsigned short bwmode, unsigned char channel_offset)
+static void set_bwmode(struct rtw_adapter *padapter, unsigned short bwmode,
+		       unsigned char channel_offset)
 {
 	mutex_lock(&adapter_to_dvobj(padapter)->setbw_mutex);
 
@@ -364,8 +349,8 @@ void SetBWMode23a(struct rtw_adapter *padapter, unsigned short bwmode, unsigned
 	rtw_set_oper_bw23a(padapter, bwmode);
 	rtw_set_oper_ch23aoffset23a(padapter, channel_offset);
 
-	rtw_hal_set_bwmode23a(padapter, (enum ht_channel_width)bwmode,
-			   channel_offset);
+	PHY_SetBWMode23a8723A(padapter, (enum ht_channel_width)bwmode,
+			      channel_offset);
 
 	mutex_unlock(&adapter_to_dvobj(padapter)->setbw_mutex);
 }
@@ -376,10 +361,11 @@ void set_channel_bwmode23a(struct rtw_adapter *padapter, unsigned char channel,
 	u8 center_ch;
 
 	if (padapter->bNotifyChannelChange)
-		DBG_8723A("[%s] ch = %d, offset = %d, bwmode = %d\n", __func__, channel, channel_offset, bwmode);
+		DBG_8723A("[%s] ch = %d, offset = %d, bwmode = %d\n",
+			  __func__, channel, channel_offset, bwmode);
 
-	if ((bwmode == HT_CHANNEL_WIDTH_20) ||
-	    (channel_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE)) {
+	if (bwmode == HT_CHANNEL_WIDTH_20 ||
+	    channel_offset == HAL_PRIME_CHNL_OFFSET_DONT_CARE) {
 		/* SelectChannel23a(padapter, channel); */
 		center_ch = channel;
 	} else {
@@ -401,20 +387,11 @@ void set_channel_bwmode23a(struct rtw_adapter *padapter, unsigned char channel,
 	rtw_set_oper_bw23a(padapter, bwmode);
 	rtw_set_oper_ch23aoffset23a(padapter, channel_offset);
 
-	rtw_hal_set_chan23a(padapter, center_ch); /*  set center channel */
+	PHY_SwChnl8723A(padapter, center_ch); /*  set center channel */
 
 	mutex_unlock(&adapter_to_dvobj(padapter)->setch_mutex);
 
-	SetBWMode23a(padapter, bwmode, channel_offset);
-}
-
-int get_bsstype23a(unsigned short capability)
-{
-	if (capability & BIT(0))
-		return WIFI_FW_AP_STATE;
-	else if (capability & BIT(1))
-		return WIFI_FW_ADHOC_STATE;
-	return 0;
+	set_bwmode(padapter, bwmode, channel_offset);
 }
 
 inline u8 *get_my_bssid23a(struct wlan_bssid_ex *pnetwork)
@@ -425,49 +402,50 @@ inline u8 *get_my_bssid23a(struct wlan_bssid_ex *pnetwork)
 u16 get_beacon_interval23a(struct wlan_bssid_ex *bss)
 {
 	unsigned short val;
-	memcpy((unsigned char *)&val, rtw_get_beacon_interval23a_from_ie(bss->IEs), 2);
+	memcpy(&val, rtw_get_beacon_interval23a_from_ie(bss->IEs), 2);
 
 	return le16_to_cpu(val);
 }
 
-int is_client_associated_to_ap23a(struct rtw_adapter *padapter)
+bool is_client_associated_to_ap23a(struct rtw_adapter *padapter)
 {
-	struct mlme_ext_priv	*pmlmeext;
-	struct mlme_ext_info	*pmlmeinfo;
+	struct mlme_ext_priv *pmlmeext;
+	struct mlme_ext_info *pmlmeinfo;
 
 	if (!padapter)
-		return _FAIL;
+		return false;
 
 	pmlmeext = &padapter->mlmeextpriv;
 	pmlmeinfo = &pmlmeext->mlmext_info;
 
-	if ((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) && ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE))
+	if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS &&
+	    (pmlmeinfo->state & 0x03) == WIFI_FW_STATION_STATE)
 		return true;
 	else
-		return _FAIL;
+		return false;
 }
 
-int is_client_associated_to_ibss23a(struct rtw_adapter *padapter)
+bool is_client_associated_to_ibss23a(struct rtw_adapter *padapter)
 {
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
-	if ((pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS) &&
-	    ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE))
+	if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS &&
+	    (pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE)
 		return true;
 	else
-		return _FAIL;
+		return false;
 }
 
-int is_IBSS_empty23a(struct rtw_adapter *padapter)
+bool is_IBSS_empty23a(struct rtw_adapter *padapter)
 {
 	unsigned int i;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
 	for (i = IBSS_START_MAC_ID; i < NUM_STA; i++) {
 		if (pmlmeinfo->FW_sta_info[i].status == 1)
-			return _FAIL;
+			return false;
 	}
 
 	return true;
@@ -483,66 +461,26 @@ unsigned int decide_wait_for_beacon_timeout23a(unsigned int bcn_interval)
 		return bcn_interval << 2;
 }
 
-void CAM_empty_entry23a(struct rtw_adapter *Adapter, u8 ucIndex)
-{
-	rtw_hal_set_hwreg23a(Adapter, HW_VAR_CAM_EMPTY_ENTRY, (u8 *)(&ucIndex));
-}
-
 void invalidate_cam_all23a(struct rtw_adapter *padapter)
 {
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_CAM_INVALID_ALL, NULL);
-}
-
-void write_cam23a(struct rtw_adapter *padapter, u8 entry, u16 ctrl, u8 *mac, u8 *key)
-{
-	unsigned int	i, val, addr;
-	int j;
-	u32	cam_val[2];
-
-	addr = entry << 3;
-
-	for (j = 5; j >= 0; j--) {
-		switch (j) {
-		case 0:
-			val = (ctrl | (mac[0] << 16) | (mac[1] << 24));
-			break;
-		case 1:
-			val = (mac[2] | (mac[3] << 8) | (mac[4] << 16) | (mac[5] << 24));
-			break;
-		default:
-			i = (j - 2) << 2;
-			val = (key[i] | (key[i+1] << 8) | (key[i+2] << 16) | (key[i+3] << 24));
-			break;
-		}
-
-		cam_val[0] = val;
-		cam_val[1] = addr + (unsigned int)j;
-
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_CAM_WRITE, (u8 *)cam_val);
-
-		/* rtw_write32(padapter, WCAMI, val); */
-
-		/* cmd = CAM_POLLINIG | CAM_WRITE | (addr + j); */
-		/* rtw_write32(padapter, RWCAM, cmd); */
-
-		/* DBG_8723A("%s => cam write: %x, %x\n", __func__, cmd, val); */
-
-	}
+	rtl8723a_cam_invalid_all(padapter);
 }
 
 void clear_cam_entry23a(struct rtw_adapter *padapter, u8 entry)
 {
 	unsigned char null_sta[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
 
-	unsigned char null_key[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+	unsigned char null_key[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+				    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+				    0x00, 0x00, 0x00, 0x00};
 
-	write_cam23a(padapter, entry, 0, null_sta, null_key);
+	rtl8723a_cam_write(padapter, entry, 0, null_sta, null_key);
 }
 
 int allocate_fw_sta_entry23a(struct rtw_adapter *padapter)
 {
 	unsigned int mac_id;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
 	for (mac_id = IBSS_START_MAC_ID; mac_id < NUM_STA; mac_id++) {
@@ -558,69 +496,43 @@ int allocate_fw_sta_entry23a(struct rtw_adapter *padapter)
 
 void flush_all_cam_entry23a(struct rtw_adapter *padapter)
 {
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_CAM_INVALID_ALL, NULL);
+	rtl8723a_cam_invalid_all(padapter);
 
-	memset((u8 *)(pmlmeinfo->FW_sta_info), 0, sizeof(pmlmeinfo->FW_sta_info));
+	memset(pmlmeinfo->FW_sta_info, 0, sizeof(pmlmeinfo->FW_sta_info));
 }
 
-#if defined(CONFIG_8723AU_P2P) && defined(CONFIG_8723AU_P2P)
-int WFD_info_handler(struct rtw_adapter *padapter, struct ndis_802_11_var_ies *	pIE)
-{
-	struct wifidirect_info	*pwdinfo;
-	u8	wfd_ie[128] = {0x00};
-	u32	wfd_ielen = 0;
-
-	pwdinfo = &padapter->wdinfo;
-	if (rtw_get_wfd_ie((u8 *) pIE, pIE->Length, wfd_ie, &wfd_ielen)) {
-		u8	attr_content[ 10 ] = { 0x00 };
-		u32	attr_contentlen = 0;
-
-		DBG_8723A("[%s] Found WFD IE\n", __func__);
-		rtw_get_wfd_attr_content(wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, attr_content, &attr_contentlen);
-		if (attr_contentlen) {
-			pwdinfo->wfd_info->peer_rtsp_ctrlport = get_unaligned_be16(attr_content + 2);
-			DBG_8723A("[%s] Peer PORT NUM = %d\n", __func__, pwdinfo->wfd_info->peer_rtsp_ctrlport);
-			return true;
-		}
-	} else {
-		DBG_8723A("[%s] NO WFD IE\n", __func__);
-	}
-	return _FAIL;
-}
-#endif
-
-int WMM_param_handler23a(struct rtw_adapter *padapter, struct ndis_802_11_var_ies *	pIE)
+int WMM_param_handler23a(struct rtw_adapter *padapter, u8 *p)
 {
 	/* struct registry_priv	*pregpriv = &padapter->registrypriv; */
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
-	if (pmlmepriv->qospriv.qos_option == 0) {
+	if (pmlmepriv->qos_option == 0) {
 		pmlmeinfo->WMM_enable = 0;
 		return _FAIL;
 	}
 
 	pmlmeinfo->WMM_enable = 1;
-	memcpy(&pmlmeinfo->WMM_param, (pIE->data + 6),
+	memcpy(&pmlmeinfo->WMM_param, p + 2 + 6,
 	       sizeof(struct WMM_para_element));
 	return true;
 }
 
 void WMMOnAssocRsp23a(struct rtw_adapter *padapter)
 {
-	u8	ACI, ACM, AIFS, ECWMin, ECWMax, aSifsTime;
-	u8	acm_mask;
-	u16	TXOP;
-	u32	acParm, i;
-	u32	edca[4], inx[4];
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	u8 ACI, ACM, AIFS, ECWMin, ECWMax, aSifsTime;
+	u8 acm_mask;
+	u16 TXOP;
+	u32 acParm, i;
+	u32 edca[4], inx[4];
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
-	struct xmit_priv		*pxmitpriv = &padapter->xmitpriv;
-	struct registry_priv	*pregpriv = &padapter->registrypriv;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct registry_priv *pregpriv = &padapter->registrypriv;
 
 	if (pmlmeinfo->WMM_enable == 0) {
 		padapter->mlmepriv.acm_mask = 0;
@@ -639,9 +551,10 @@ void WMMOnAssocRsp23a(struct rtw_adapter *padapter)
 		ACM = (pmlmeinfo->WMM_param.ac_param[i].ACI_AIFSN >> 4) & 0x01;
 
 		/* AIFS = AIFSN * slot time + SIFS - r2t phy delay */
-		AIFS = (pmlmeinfo->WMM_param.ac_param[i].ACI_AIFSN & 0x0f) * pmlmeinfo->slotTime + aSifsTime;
+		AIFS = (pmlmeinfo->WMM_param.ac_param[i].ACI_AIFSN & 0x0f) *
+			pmlmeinfo->slotTime + aSifsTime;
 
-		ECWMin = (pmlmeinfo->WMM_param.ac_param[i].CW & 0x0f);
+		ECWMin = pmlmeinfo->WMM_param.ac_param[i].CW & 0x0f;
 		ECWMax = (pmlmeinfo->WMM_param.ac_param[i].CW & 0xf0) >> 4;
 		TXOP = le16_to_cpu(pmlmeinfo->WMM_param.ac_param[i].TXOP_limit);
 
@@ -649,22 +562,22 @@ void WMMOnAssocRsp23a(struct rtw_adapter *padapter)
 
 		switch (ACI) {
 		case 0x0:
-			rtw_hal_set_hwreg23a(padapter, HW_VAR_AC_PARAM_BE, (u8 *)(&acParm));
+			rtl8723a_set_ac_param_be(padapter, acParm);
 			acm_mask |= (ACM? BIT(1):0);
 			edca[XMIT_BE_QUEUE] = acParm;
 			break;
 		case 0x1:
-			rtw_hal_set_hwreg23a(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acParm));
+			rtl8723a_set_ac_param_bk(padapter, acParm);
 			/* acm_mask |= (ACM? BIT(0):0); */
 			edca[XMIT_BK_QUEUE] = acParm;
 			break;
 		case 0x2:
-			rtw_hal_set_hwreg23a(padapter, HW_VAR_AC_PARAM_VI, (u8 *)(&acParm));
+			rtl8723a_set_ac_param_vi(padapter, acParm);
 			acm_mask |= (ACM? BIT(2):0);
 			edca[XMIT_VI_QUEUE] = acParm;
 			break;
 		case 0x3:
-			rtw_hal_set_hwreg23a(padapter, HW_VAR_AC_PARAM_VO, (u8 *)(&acParm));
+			rtl8723a_set_ac_param_vo(padapter, acParm);
 			acm_mask |= (ACM? BIT(3):0);
 			edca[XMIT_VO_QUEUE] = acParm;
 			break;
@@ -674,14 +587,14 @@ void WMMOnAssocRsp23a(struct rtw_adapter *padapter)
 	}
 
 	if (padapter->registrypriv.acm_method == 1)
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_ACM_CTRL, (u8 *)(&acm_mask));
+		rtl8723a_set_acm_ctrl(padapter, acm_mask);
 	else
 		padapter->mlmepriv.acm_mask = acm_mask;
 
 	inx[0] = 0; inx[1] = 1; inx[2] = 2; inx[3] = 3;
 
 	if (pregpriv->wifi_spec == 1) {
-		u32	j, tmp, change_inx;
+		u32 j, tmp, change_inx = false;
 
 		/* entry indx: 0->vo, 1->vi, 2->be, 3->bk. */
 		for (i = 0; i < 4; i++) {
@@ -689,7 +602,8 @@ void WMMOnAssocRsp23a(struct rtw_adapter *padapter)
 				/* compare CW and AIFS */
 				if ((edca[j] & 0xFFFF) < (edca[i] & 0xFFFF)) {
 					change_inx = true;
-				} else if ((edca[j] & 0xFFFF) == (edca[i] & 0xFFFF)) {
+				} else if ((edca[j] & 0xFFFF) ==
+					   (edca[i] & 0xFFFF)) {
 					/* compare TXOP */
 					if ((edca[j] >> 16) > (edca[i] >> 16))
 						change_inx = true;
@@ -712,31 +626,32 @@ void WMMOnAssocRsp23a(struct rtw_adapter *padapter)
 
 	for (i = 0; i<4; i++) {
 		pxmitpriv->wmm_para_seq[i] = inx[i];
-		DBG_8723A("wmm_para_seq(%d): %d\n", i, pxmitpriv->wmm_para_seq[i]);
+		DBG_8723A("wmm_para_seq(%d): %d\n", i,
+			  pxmitpriv->wmm_para_seq[i]);
 	}
 
 	return;
 }
 
-static void bwmode_update_check(struct rtw_adapter *padapter, struct ndis_802_11_var_ies * pIE)
+static void bwmode_update_check(struct rtw_adapter *padapter, u8 *p)
 {
-	struct HT_info_element	 *pHT_info;
+	struct HT_info_element *pHT_info;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct registry_priv *pregistrypriv = &padapter->registrypriv;
-	struct ht_priv	*phtpriv = &pmlmepriv->htpriv;
-	unsigned char	 new_bwmode;
-	unsigned char  new_ch_offset;
+	struct ht_priv *phtpriv = &pmlmepriv->htpriv;
+	unsigned char new_bwmode;
+	unsigned char new_ch_offset;
 
-	if (!pIE)
+	if (!p)
 		return;
 	if (!phtpriv->ht_option)
 		return;
-	if (pIE->Length > sizeof(struct HT_info_element))
+	if (p[1] > sizeof(struct HT_info_element))
 		return;
 
-	pHT_info = (struct HT_info_element *)pIE->data;
+	pHT_info = (struct HT_info_element *)(p + 2);
 
 	if ((pHT_info->infos[0] & BIT(2)) && pregistrypriv->cbw40_enable) {
 		new_bwmode = HT_CHANNEL_WIDTH_40;
@@ -757,133 +672,144 @@ static void bwmode_update_check(struct rtw_adapter *padapter, struct ndis_802_11
 		new_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
 	}
 
-	if ((new_bwmode!= pmlmeext->cur_bwmode) ||
-	    (new_ch_offset!= pmlmeext->cur_ch_offset)) {
+	if (new_bwmode != pmlmeext->cur_bwmode ||
+	    new_ch_offset != pmlmeext->cur_ch_offset) {
 		pmlmeinfo->bwmode_updated = true;
 
 		pmlmeext->cur_bwmode = new_bwmode;
 		pmlmeext->cur_ch_offset = new_ch_offset;
 
 		/* update HT info also */
-		HT_info_handler23a(padapter, pIE);
-	} else {
+		HT_info_handler23a(padapter, p);
+	} else
 		pmlmeinfo->bwmode_updated = false;
-	}
 
 	if (pmlmeinfo->bwmode_updated) {
 		struct sta_info *psta;
 		struct wlan_bssid_ex *cur_network = &pmlmeinfo->network;
 		struct sta_priv	*pstapriv = &padapter->stapriv;
 
-		/* set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); */
+		/* set_channel_bwmode23a(padapter, pmlmeext->cur_channel,
+		   pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); */
 
 		/* update ap's stainfo */
 		psta = rtw_get_stainfo23a(pstapriv, cur_network->MacAddress);
 		if (psta) {
-			struct ht_priv	*phtpriv_sta = &psta->htpriv;
+			struct ht_priv *phtpriv_sta = &psta->htpriv;
 
 			if (phtpriv_sta->ht_option) {
 				/*  bwmode */
 				phtpriv_sta->bwmode = pmlmeext->cur_bwmode;
-				phtpriv_sta->ch_offset = pmlmeext->cur_ch_offset;
+				phtpriv_sta->ch_offset =
+					pmlmeext->cur_ch_offset;
 			} else {
 				phtpriv_sta->bwmode = HT_CHANNEL_WIDTH_20;
-				phtpriv_sta->ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+				phtpriv_sta->ch_offset =
+					HAL_PRIME_CHNL_OFFSET_DONT_CARE;
 			}
-
 		}
 	}
 }
 
-void HT_caps_handler23a(struct rtw_adapter *padapter, struct ndis_802_11_var_ies * pIE)
+void HT_caps_handler23a(struct rtw_adapter *padapter, u8 *p)
 {
-	unsigned int	i;
-	u8	rf_type;
-	u8	max_AMPDU_len, min_MPDU_spacing;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	unsigned int i;
+	u8 rf_type;
+	u8 max_AMPDU_len, min_MPDU_spacing;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
-	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
-	struct ht_priv			*phtpriv = &pmlmepriv->htpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv *phtpriv = &pmlmepriv->htpriv;
 
-	if (pIE == NULL) return;
+	if (!p)
+		return;
 
-	if (phtpriv->ht_option == false)	return;
+	if (phtpriv->ht_option == false)
+		return;
 
 	pmlmeinfo->HT_caps_enable = 1;
 
-	for (i = 0; i < (pIE->Length); i++) {
+	for (i = 0; i < p[1]; i++) {
 		if (i != 2) {
 			/*	Commented by Albert 2010/07/12 */
 			/*	Got the endian issue here. */
-			pmlmeinfo->HT_caps.u.HT_cap[i] &= (pIE->data[i]);
+			pmlmeinfo->HT_caps.u.HT_cap[i] &= p[i + 2];
 		} else {
 			/* modify from  fw by Thomas 2010/11/17 */
-			if ((pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3) > (pIE->data[i] & 0x3))
-				max_AMPDU_len = (pIE->data[i] & 0x3);
+			if ((pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3) > (p[i + 2] & 0x3))
+				max_AMPDU_len = p[i + 2] & 0x3;
 			else
-				max_AMPDU_len = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3);
+				max_AMPDU_len = pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x3;
 
-			if ((pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) > (pIE->data[i] & 0x1c))
-				min_MPDU_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c);
+			if ((pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) > (p[i + 2] & 0x1c))
+				min_MPDU_spacing = pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c;
 			else
-				min_MPDU_spacing = (pIE->data[i] & 0x1c);
+				min_MPDU_spacing = p[i + 2] & 0x1c;
 
-			pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para = max_AMPDU_len | min_MPDU_spacing;
+			pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para =
+				max_AMPDU_len | min_MPDU_spacing;
 		}
 	}
 
 	/*	Commented by Albert 2010/07/12 */
 	/*	Have to handle the endian issue after copying. */
 	/*	HT_ext_caps didn't be used yet. */
-	pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info = le16_to_cpu(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info);
-	pmlmeinfo->HT_caps.u.HT_cap_element.HT_ext_caps = le16_to_cpu(pmlmeinfo->HT_caps.u.HT_cap_element.HT_ext_caps);
+	pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info =
+		le16_to_cpu(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info);
+	pmlmeinfo->HT_caps.u.HT_cap_element.HT_ext_caps =
+		le16_to_cpu(pmlmeinfo->HT_caps.u.HT_cap_element.HT_ext_caps);
 
-	rtw23a_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+	rf_type = rtl8723a_get_rf_type(padapter);
 
 	/* update the MCS rates */
 	for (i = 0; i < 16; i++) {
-		if ((rf_type == RF_1T1R) || (rf_type == RF_1T2R))
-			pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= MCS_rate_1R23A[i];
+		if (rf_type == RF_1T1R || rf_type == RF_1T2R)
+			pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &=
+				MCS_rate_1R23A[i];
 		else
-			pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &= MCS_rate_2R23A[i];
+			pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate[i] &=
+				MCS_rate_2R23A[i];
 	}
 	return;
 }
 
-void HT_info_handler23a(struct rtw_adapter *padapter, struct ndis_802_11_var_ies * pIE)
+void HT_info_handler23a(struct rtw_adapter *padapter, u8 *p)
 {
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
-	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
-	struct ht_priv			*phtpriv = &pmlmepriv->htpriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct ht_priv *phtpriv = &pmlmepriv->htpriv;
 
-	if (pIE == NULL) return;
+	if (!p)
+		return;
 
-	if (phtpriv->ht_option == false)	return;
+	if (phtpriv->ht_option == false)
+		return;
 
-	if (pIE->Length > sizeof(struct HT_info_element))
+	if (p[1] > sizeof(struct HT_info_element))
 		return;
 
 	pmlmeinfo->HT_info_enable = 1;
-	memcpy(&pmlmeinfo->HT_info, pIE->data, pIE->Length);
+	memcpy(&pmlmeinfo->HT_info, p + 2, p[1]);
 	return;
 }
 
 void HTOnAssocRsp23a(struct rtw_adapter *padapter)
 {
-	unsigned char		max_AMPDU_len;
-	unsigned char		min_MPDU_spacing;
+	unsigned char max_AMPDU_len;
+	unsigned char min_MPDU_spacing;
 	/* struct registry_priv	 *pregpriv = &padapter->registrypriv; */
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
 	DBG_8723A("%s\n", __func__);
 
-	if ((pmlmeinfo->HT_info_enable) && (pmlmeinfo->HT_caps_enable)) {
+	if (pmlmeinfo->HT_info_enable && pmlmeinfo->HT_caps_enable)
 		pmlmeinfo->HT_enable = 1;
-	} else {
+	else {
 		pmlmeinfo->HT_enable = 0;
-		/* set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); */
+		/* set_channel_bwmode23a(padapter, pmlmeext->cur_channel,
+		   pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode); */
 		return;
 	}
 
@@ -894,29 +820,29 @@ void HTOnAssocRsp23a(struct rtw_adapter *padapter)
 	*/
 	max_AMPDU_len = pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x03;
 
-	min_MPDU_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) >> 2;
+	min_MPDU_spacing =
+		(pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) >> 2;
 
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_AMPDU_MIN_SPACE, (u8 *)(&min_MPDU_spacing));
-
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_AMPDU_FACTOR, (u8 *)(&max_AMPDU_len));
+	rtl8723a_set_ampdu_min_space(padapter, min_MPDU_spacing);
+	rtl8723a_set_ampdu_factor(padapter, max_AMPDU_len);
 }
 
-void ERP_IE_handler23a(struct rtw_adapter *padapter, struct ndis_802_11_var_ies * pIE)
+void ERP_IE_handler23a(struct rtw_adapter *padapter, u8 *p)
 {
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
-	if (pIE->Length>1)
+	if (p[1] > 1)
 		return;
 
 	pmlmeinfo->ERP_enable = 1;
-	memcpy(&pmlmeinfo->ERP_IE, pIE->data, pIE->Length);
+	memcpy(&pmlmeinfo->ERP_IE, p + 2, p[1]);
 }
 
 void VCS_update23a(struct rtw_adapter *padapter, struct sta_info *psta)
 {
-	struct registry_priv	 *pregpriv = &padapter->registrypriv;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
 	switch (pregpriv->vrtl_carrier_sense) { /* 0:off 1:on 2:auto */
@@ -935,7 +861,7 @@ void VCS_update23a(struct rtw_adapter *padapter, struct sta_info *psta)
 		break;
 	case 2: /* auto */
 	default:
-		if ((pmlmeinfo->ERP_enable) && (pmlmeinfo->ERP_IE & BIT(1))) {
+		if (pmlmeinfo->ERP_enable && pmlmeinfo->ERP_IE & BIT(1)) {
 			if (pregpriv->vcs_type == 1) {
 				psta->rtsen = 1;
 				psta->cts2self = 0;
@@ -951,116 +877,134 @@ void VCS_update23a(struct rtw_adapter *padapter, struct sta_info *psta)
 	}
 }
 
-int rtw_check_bcn_info23a(struct rtw_adapter *Adapter, u8 *pframe, u32 packet_len)
+int rtw_check_bcn_info23a(struct rtw_adapter *Adapter,
+			  struct ieee80211_mgmt *mgmt, u32 pkt_len)
 {
-	unsigned int		len;
-	unsigned char		*p;
-	unsigned short	val16;
 	struct wlan_network *cur_network = &Adapter->mlmepriv.cur_network;
-	u16 wpa_len = 0, rsn_len = 0;
-	u8 encryp_protocol = 0;
+	struct HT_info_element *pht_info;
+	struct ieee80211_ht_cap *pht_cap;
 	struct wlan_bssid_ex *bssid;
-	int group_cipher = 0, pairwise_cipher = 0, is_8021x = 0;
-	unsigned char *pbuf;
-	u32 wpa_ielen = 0;
-	u32 hidden_ssid = 0;
-	struct HT_info_element *pht_info = NULL;
-	struct ieee80211_ht_cap *pht_cap = NULL;
+	unsigned short val16;
+	u16 wpa_len = 0, rsn_len = 0;
+	u8 encryp_protocol;
+	int group_cipher = 0, pairwise_cipher = 0, is_8021x = 0, r;
 	u32 bcn_channel;
-	unsigned short	ht_cap_info;
-	unsigned char	ht_info_infos_0;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) pframe;
-	u8 *pbssid = hdr->addr3;
+	unsigned short ht_cap_info;
+	unsigned char ht_info_infos_0;
+	int len, pie_len, ie_offset;
+	const u8 *p;
+	u8 *pie;
 
 	if (is_client_associated_to_ap23a(Adapter) == false)
 		return true;
 
-	len = packet_len - sizeof(struct ieee80211_hdr_3addr);
+	if (unlikely(!ieee80211_is_beacon(mgmt->frame_control))) {
+		printk(KERN_WARNING "%s: received a non beacon frame!\n",
+		       __func__);
+		return false;
+	}
+
+	len = pkt_len - sizeof(struct ieee80211_hdr_3addr);
 
 	if (len > MAX_IE_SZ) {
 		DBG_8723A("%s IE too long for survey event\n", __func__);
 		return _FAIL;
 	}
 
-	if (memcmp(cur_network->network.MacAddress, pbssid, 6)) {
-		DBG_8723A("Oops: rtw_check_network_encrypt linked but recv other bssid bcn\n" MAC_FMT MAC_FMT,
-				MAC_ARG(pbssid), MAC_ARG(cur_network->network.MacAddress));
+	if (memcmp(cur_network->network.MacAddress, mgmt->bssid, 6)) {
+		DBG_8723A("Oops: rtw_check_network_encrypt linked but recv "
+			  "other bssid bcn\n" MAC_FMT MAC_FMT,
+			  MAC_ARG(mgmt->bssid),
+			  MAC_ARG(cur_network->network.MacAddress));
 		return true;
 	}
 
-	bssid = (struct wlan_bssid_ex *)kzalloc(sizeof(struct wlan_bssid_ex),
-		GFP_ATOMIC);
+	bssid = kzalloc(sizeof(struct wlan_bssid_ex), GFP_ATOMIC);
+	if (!bssid)
+		return _FAIL;
 
-	if (ieee80211_is_beacon(hdr->frame_control))
-		bssid->reserved = 1;
+	bssid->reserved = 1;
 
-	bssid->Length = sizeof(struct wlan_bssid_ex) - MAX_IE_SZ + len;
+	bssid->Length = offsetof(struct wlan_bssid_ex, IEs) + len;
 
 	/* below is to copy the information element */
 	bssid->IELength = len;
-	memcpy(bssid->IEs, (pframe + sizeof(struct ieee80211_hdr_3addr)), bssid->IELength);
+	memcpy(bssid->IEs, &mgmt->u, len);
 
 	/* check bw and channel offset */
 	/* parsing HT_CAP_IE */
-	p = rtw_get_ie23a(bssid->IEs + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
-	if (p && len>0) {
-			pht_cap = (struct ieee80211_ht_cap *)(p + 2);
-			ht_cap_info = pht_cap->cap_info;
+	ie_offset = offsetof(struct ieee80211_mgmt, u.beacon.variable) -
+		offsetof(struct ieee80211_mgmt, u);
+	pie = bssid->IEs + ie_offset;
+	pie_len = pkt_len - ie_offset;
+
+	p = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY, pie, pie_len);
+	if (p && p[1] > 0) {
+		pht_cap = (struct ieee80211_ht_cap *)(p + 2);
+		ht_cap_info = pht_cap->cap_info;
 	} else {
-			ht_cap_info = 0;
+		pht_cap = NULL;
+		ht_cap_info = 0;
 	}
+
 	/* parsing HT_INFO_IE */
-	p = rtw_get_ie23a(bssid->IEs + _FIXED_IE_LENGTH_, _HT_ADD_INFO_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
-	if (p && len>0) {
-			pht_info = (struct HT_info_element *)(p + 2);
-			ht_info_infos_0 = pht_info->infos[0];
+	p = cfg80211_find_ie(WLAN_EID_HT_OPERATION, pie, pie_len);
+	if (p && p[1] > 0) {
+		pht_info = (struct HT_info_element *)(p + 2);
+		ht_info_infos_0 = pht_info->infos[0];
 	} else {
-			ht_info_infos_0 = 0;
+		pht_info = NULL;
+		ht_info_infos_0 = 0;
 	}
+
 	if (ht_cap_info != cur_network->BcnInfo.ht_cap_info ||
-		((ht_info_infos_0&0x03) != (cur_network->BcnInfo.ht_info_infos_0&0x03))) {
-			DBG_8723A("%s bcn now: ht_cap_info:%x ht_info_infos_0:%x\n", __func__,
-							ht_cap_info, ht_info_infos_0);
-			DBG_8723A("%s bcn link: ht_cap_info:%x ht_info_infos_0:%x\n", __func__,
-							cur_network->BcnInfo.ht_cap_info, cur_network->BcnInfo.ht_info_infos_0);
-			DBG_8723A("%s bw mode change, disconnect\n", __func__);
-			/* bcn_info_update */
-			cur_network->BcnInfo.ht_cap_info = ht_cap_info;
-			cur_network->BcnInfo.ht_info_infos_0 = ht_info_infos_0;
-			/* to do : need to check that whether modify related register of BB or not */
+	    ((ht_info_infos_0 & 0x03) !=
+	     (cur_network->BcnInfo.ht_info_infos_0 & 0x03))) {
+		DBG_8723A("%s bcn now: ht_cap_info:%x ht_info_infos_0:%x\n",
+			  __func__, ht_cap_info, ht_info_infos_0);
+		DBG_8723A("%s bcn link: ht_cap_info:%x ht_info_infos_0:%x\n",
+			  __func__, cur_network->BcnInfo.ht_cap_info,
+			  cur_network->BcnInfo.ht_info_infos_0);
+		DBG_8723A("%s bw mode change, disconnect\n", __func__);
+		/* bcn_info_update */
+		cur_network->BcnInfo.ht_cap_info = ht_cap_info;
+		cur_network->BcnInfo.ht_info_infos_0 = ht_info_infos_0;
+		/* to do : need to check that whether modify related
+		   register of BB or not */
 	}
 
 	/* Checking for channel */
-	p = rtw_get_ie23a(bssid->IEs + _FIXED_IE_LENGTH_, _DSSET_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
-	if (p) {
-			bcn_channel = *(p + 2);
-	} else {/* In 5G, some ap do not have DSSET IE checking HT info for channel */
-			p = rtw_get_ie23a(bssid->IEs + _FIXED_IE_LENGTH_, _HT_ADD_INFO_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
-			if (pht_info) {
-					bcn_channel = pht_info->primary_channel;
-			} else { /* we don't find channel IE, so don't check it */
-					DBG_8723A("Oops: %s we don't find channel IE, so don't check it\n", __func__);
-					bcn_channel = Adapter->mlmeextpriv.cur_channel;
-			}
+	p = cfg80211_find_ie(WLAN_EID_DS_PARAMS, pie, pie_len);
+	if (p)
+		bcn_channel = p[2];
+	else {
+		/* In 5G, some ap do not have DSSET IE checking HT
+		   info for channel */
+		p = cfg80211_find_ie(WLAN_EID_HT_OPERATION, pie, pie_len);
+
+		if (pht_info)
+			bcn_channel = pht_info->primary_channel;
+		else { /* we don't find channel IE, so don't check it */
+			DBG_8723A("Oops: %s we don't find channel IE, so don't "
+				  "check it\n", __func__);
+			bcn_channel = Adapter->mlmeextpriv.cur_channel;
+		}
 	}
 	if (bcn_channel != Adapter->mlmeextpriv.cur_channel) {
-			DBG_8723A("%s beacon channel:%d cur channel:%d disconnect\n", __func__,
-						   bcn_channel, Adapter->mlmeextpriv.cur_channel);
-			goto _mismatch;
+		DBG_8723A("%s beacon channel:%d cur channel:%d disconnect\n",
+			  __func__, bcn_channel,
+			  Adapter->mlmeextpriv.cur_channel);
+		goto _mismatch;
 	}
 
 	/* checking SSID */
-	if ((p = rtw_get_ie23a(bssid->IEs + _FIXED_IE_LENGTH_, _SSID_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_)) == NULL) {
-		DBG_8723A("%s marc: cannot find SSID for survey event\n", __func__);
-		hidden_ssid = true;
-	} else {
-		hidden_ssid = false;
-	}
-
-	if ((NULL != p) && (false == hidden_ssid && (*(p + 1)))) {
-		memcpy(bssid->Ssid.ssid, (p + 2), *(p + 1));
-		bssid->Ssid.ssid_len = *(p + 1);
+	p = cfg80211_find_ie(WLAN_EID_SSID, pie, pie_len);
+	if (p && p[1]) {
+		memcpy(bssid->Ssid.ssid, p + 2, p[1]);
+		bssid->Ssid.ssid_len = p[1];
 	} else {
+		DBG_8723A("%s marc: cannot find SSID for survey event\n",
+			  __func__);
 		bssid->Ssid.ssid_len = 0;
 		bssid->Ssid.ssid[0] = '\0';
 	}
@@ -1083,7 +1027,7 @@ int rtw_check_bcn_info23a(struct rtw_adapter *Adapter, u8 *pframe, u32 packet_le
 	}
 
 	/* check encryption info */
-	val16 = rtw_get_capability23a((struct wlan_bssid_ex *)bssid);
+	val16 = rtw_get_capability23a(bssid);
 
 	if (val16 & BIT(4))
 		bssid->Privacy = 1;
@@ -1091,60 +1035,83 @@ int rtw_check_bcn_info23a(struct rtw_adapter *Adapter, u8 *pframe, u32 packet_le
 		bssid->Privacy = 0;
 
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-			("%s(): cur_network->network.Privacy is %d, bssid.Privacy is %d\n",
-			 __func__, cur_network->network.Privacy, bssid->Privacy));
+		 ("%s(): cur_network->network.Privacy is %d, bssid.Privacy "
+		  "is %d\n", __func__, cur_network->network.Privacy,
+		  bssid->Privacy));
 	if (cur_network->network.Privacy != bssid->Privacy) {
 		DBG_8723A("%s(), privacy is not match return FAIL\n", __func__);
 		goto _mismatch;
 	}
 
-	rtw_get_sec_ie23a(bssid->IEs, bssid->IELength, NULL,&rsn_len, NULL,&wpa_len);
+	rtw_get_sec_ie23a(bssid->IEs, bssid->IELength, NULL, &rsn_len, NULL,
+			  &wpa_len);
 
-	if (rsn_len > 0) {
+	if (rsn_len > 0)
 		encryp_protocol = ENCRYP_PROTOCOL_WPA2;
-	} else if (wpa_len > 0) {
+	else if (wpa_len > 0)
 		encryp_protocol = ENCRYP_PROTOCOL_WPA;
-	} else {
+	else {
 		if (bssid->Privacy)
 			encryp_protocol = ENCRYP_PROTOCOL_WEP;
+		else
+			encryp_protocol = ENCRYP_PROTOCOL_OPENSYS;
 	}
 
 	if (cur_network->BcnInfo.encryp_protocol != encryp_protocol) {
-		DBG_8723A("%s(): enctyp is not match , return FAIL\n", __func__);
+		DBG_8723A("%s(): enctyp is not match, return FAIL\n", __func__);
 		goto _mismatch;
 	}
 
-	if (encryp_protocol == ENCRYP_PROTOCOL_WPA || encryp_protocol == ENCRYP_PROTOCOL_WPA2) {
-		pbuf = rtw_get_wpa_ie23a(&bssid->IEs[12], &wpa_ielen, bssid->IELength-12);
-		if (pbuf && (wpa_ielen>0)) {
-			if (_SUCCESS == rtw_parse_wpa_ie23a(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is_8021x)) {
+	if (encryp_protocol == ENCRYP_PROTOCOL_WPA ||
+	    encryp_protocol == ENCRYP_PROTOCOL_WPA2) {
+		p = cfg80211_find_vendor_ie(WLAN_OUI_MICROSOFT,
+					    WLAN_OUI_TYPE_MICROSOFT_WPA,
+					    pie, pie_len);
+		if (p && p[1] > 0) {
+			r = rtw_parse_wpa_ie23a(p, p[1] + 2, &group_cipher,
+						&pairwise_cipher, &is_8021x);
+			if (r == _SUCCESS)
 				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-						("%s pnetwork->pairwise_cipher: %d, group_cipher is %d, is_8021x is %d\n", __func__,
-						 pairwise_cipher, group_cipher, is_8021x));
-			}
+					 ("%s pnetwork->pairwise_cipher: %d, "
+					  "group_cipher is %d, is_8021x is "
+					  "%d\n", __func__, pairwise_cipher,
+					  group_cipher, is_8021x));
 		} else {
-			pbuf = rtw_get_wpa2_ie23a(&bssid->IEs[12], &wpa_ielen, bssid->IELength-12);
-
-			if (pbuf && (wpa_ielen>0)) {
-				if (_SUCCESS == rtw_parse_wpa2_ie23a(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is_8021x)) {
-					RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-							("%s pnetwork->pairwise_cipher: %d, pnetwork->group_cipher is %d, is_802x is %d\n",
-							 __func__, pairwise_cipher, group_cipher, is_8021x));
-				}
+			p = cfg80211_find_ie(WLAN_EID_RSN, pie, pie_len);
+
+			if (p && p[1] > 0) {
+				r = rtw_parse_wpa2_ie23a(p, p[1] + 2,
+							 &group_cipher,
+							 &pairwise_cipher,
+							 &is_8021x);
+				if (r == _SUCCESS)
+					RT_TRACE(_module_rtl871x_mlme_c_,
+						 _drv_info_,
+						 ("%s pnetwork->pairwise_cipher"
+						  ": %d, pnetwork->group_cipher"
+						  " is %d, is_802x is %d\n",
+						  __func__, pairwise_cipher,
+						  group_cipher, is_8021x));
 			}
 		}
 
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
-				("%s cur_network->group_cipher is %d: %d\n", __func__, cur_network->BcnInfo.group_cipher, group_cipher));
-		if (pairwise_cipher != cur_network->BcnInfo.pairwise_cipher || group_cipher != cur_network->BcnInfo.group_cipher) {
-			DBG_8723A("%s pairwise_cipher(%x:%x) or group_cipher(%x:%x) is not match , return FAIL\n", __func__,
-					pairwise_cipher, cur_network->BcnInfo.pairwise_cipher,
-					group_cipher, cur_network->BcnInfo.group_cipher);
+			 ("%s cur_network->group_cipher is %d: %d\n", __func__,
+			  cur_network->BcnInfo.group_cipher, group_cipher));
+		if (pairwise_cipher != cur_network->BcnInfo.pairwise_cipher ||
+		    group_cipher != cur_network->BcnInfo.group_cipher) {
+			DBG_8723A("%s pairwise_cipher(%x:%x) or group_cipher "
+				  "(%x:%x) is not match, return FAIL\n",
+				  __func__, pairwise_cipher,
+				  cur_network->BcnInfo.pairwise_cipher,
+				  group_cipher,
+				  cur_network->BcnInfo.group_cipher);
 			goto _mismatch;
 		}
 
 		if (is_8021x != cur_network->BcnInfo.is_8021x) {
-			DBG_8723A("%s authentication is not match , return FAIL\n", __func__);
+			DBG_8723A("%s authentication is not match, return "
+				  "FAIL\n", __func__);
 			goto _mismatch;
 		}
 	}
@@ -1158,93 +1125,106 @@ _mismatch:
 	return _FAIL;
 }
 
-void update_beacon23a_info(struct rtw_adapter *padapter, u8 *pframe, uint pkt_len, struct sta_info *psta)
+void update_beacon23a_info(struct rtw_adapter *padapter, u8 *pframe,
+			   uint pkt_len, struct sta_info *psta)
 {
 	unsigned int i;
 	unsigned int len;
-	struct ndis_802_11_var_ies *	pIE;
+	u8 *p;
 
 	len = pkt_len -
 		(_BEACON_IE_OFFSET_ + sizeof(struct ieee80211_hdr_3addr));
 
 	for (i = 0; i < len;) {
-		pIE = (struct ndis_802_11_var_ies *)(pframe + (_BEACON_IE_OFFSET_ + sizeof(struct ieee80211_hdr_3addr)) + i);
+		p = (u8 *)(pframe + (_BEACON_IE_OFFSET_ + sizeof(struct ieee80211_hdr_3addr)) + i);
 
-		switch (pIE->ElementID) {
-		case _HT_EXTRA_INFO_IE_:	/* HT info */
+		switch (p[0]) {
+		case WLAN_EID_HT_OPERATION:	/* HT info */
 			/* HT_info_handler23a(padapter, pIE); */
-			bwmode_update_check(padapter, pIE);
+			bwmode_update_check(padapter, p);
 			break;
-		case _ERPINFO_IE_:
-			ERP_IE_handler23a(padapter, pIE);
+		case WLAN_EID_ERP_INFO:
+			ERP_IE_handler23a(padapter, p);
 			VCS_update23a(padapter, psta);
 			break;
 		default:
 			break;
 		}
-		i += (pIE->Length + 2);
+		i += (p[1] + 2);
 	}
 }
 
-unsigned int is_ap_in_tkip23a(struct rtw_adapter *padapter)
+bool is_ap_in_tkip23a(struct rtw_adapter *padapter)
 {
 	u32 i;
-	struct ndis_802_11_var_ies *	pIE;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *cur_network = &pmlmeinfo->network;
+	const u8 *p;
+	int bcn_fixed_size;
+
+	bcn_fixed_size = offsetof(struct ieee80211_mgmt, u.beacon.variable) -
+		offsetof(struct ieee80211_mgmt, u.beacon);
 
-	if (rtw_get_capability23a((struct wlan_bssid_ex *)cur_network) & WLAN_CAPABILITY_PRIVACY) {
-		for (i = sizeof(struct ndis_802_11_fixed_ies); i < pmlmeinfo->network.IELength;) {
-			pIE = (struct ndis_802_11_var_ies *)(pmlmeinfo->network.IEs + i);
+	if (rtw_get_capability23a(cur_network) & WLAN_CAPABILITY_PRIVACY) {
+		for (i = bcn_fixed_size; i < pmlmeinfo->network.IELength;) {
+			p = pmlmeinfo->network.IEs + i;
 
-			switch (pIE->ElementID) {
-			case _VENDOR_SPECIFIC_IE_:
-				if ((!memcmp(pIE->data, RTW_WPA_OUI23A, 4)) && (!memcmp((pIE->data + 12), WPA_TKIP_CIPHER23A, 4)))
+			switch (p[0]) {
+			case WLAN_EID_VENDOR_SPECIFIC:
+				if (!memcmp(p + 2, RTW_WPA_OUI23A_TYPE, 4) &&
+				    !memcmp(p + 2 + 12, WPA_TKIP_CIPHER, 4))
 					return true;
 				break;
-			case _RSN_IE_2_:
-				if (!memcmp((pIE->data + 8), RSN_TKIP_CIPHER23A, 4))
+			case WLAN_EID_RSN:
+				if (!memcmp(p + 2 + 8, RSN_TKIP_CIPHER, 4))
 					return true;
 				break;
 			default:
 				break;
 			}
-			i += (pIE->Length + 2);
+			i += (p[1] + 2);
 		}
 		return false;
-	} else {
+	} else
 		return false;
-	}
 }
 
-unsigned int should_forbid_n_rate23a(struct rtw_adapter * padapter)
+bool should_forbid_n_rate23a(struct rtw_adapter * padapter)
 {
 	u32 i;
-	struct ndis_802_11_var_ies *	pIE;
-	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct wlan_bssid_ex  *cur_network = &pmlmepriv->cur_network.network;
-
-	if (rtw_get_capability23a((struct wlan_bssid_ex *)cur_network) & WLAN_CAPABILITY_PRIVACY) {
-		for (i = sizeof(struct ndis_802_11_fixed_ies); i < cur_network->IELength;) {
-			pIE = (struct ndis_802_11_var_ies *)(cur_network->IEs + i);
-
-			switch (pIE->ElementID) {
-			case _VENDOR_SPECIFIC_IE_:
-				if (!memcmp(pIE->data, RTW_WPA_OUI23A, 4) &&
-					((!memcmp((pIE->data + 12), WPA_CIPHER_SUITE_CCMP23A, 4)) ||
-					  (!memcmp((pIE->data + 16), WPA_CIPHER_SUITE_CCMP23A, 4))))
+	const u8 *p;
+	int bcn_fixed_size;
+
+	bcn_fixed_size = offsetof(struct ieee80211_mgmt, u.beacon.variable) -
+		offsetof(struct ieee80211_mgmt, u.beacon);
+
+	if (rtw_get_capability23a(cur_network) & WLAN_CAPABILITY_PRIVACY) {
+		for (i = bcn_fixed_size; i < cur_network->IELength;) {
+			p = cur_network->IEs + i;
+
+			switch (p[0]) {
+			case WLAN_EID_VENDOR_SPECIFIC:
+				if (!memcmp(p + 2, RTW_WPA_OUI23A_TYPE, 4) &&
+				    (!memcmp(p + 2 + 12,
+					     WPA_CIPHER_SUITE_CCMP23A, 4) ||
+				     !memcmp(p + 2 + 16,
+					     WPA_CIPHER_SUITE_CCMP23A, 4)))
 					return false;
 				break;
-			case _RSN_IE_2_:
-				if  ((!memcmp((pIE->data + 8), RSN_CIPHER_SUITE_CCMP23A, 4))  ||
-				       (!memcmp((pIE->data + 12), RSN_CIPHER_SUITE_CCMP23A, 4)))
+			case WLAN_EID_RSN:
+				if (!memcmp(p + 2 + 8,
+					    RSN_CIPHER_SUITE_CCMP23A, 4) ||
+				    !memcmp(p + 2 + 12,
+					    RSN_CIPHER_SUITE_CCMP23A, 4))
 				return false;
 			default:
 				break;
 			}
 
-			i += (pIE->Length + 2);
+			i += (p[1] + 2);
 		}
 		return true;
 	} else {
@@ -1252,42 +1232,45 @@ unsigned int should_forbid_n_rate23a(struct rtw_adapter * padapter)
 	}
 }
 
-unsigned int is_ap_in_wep23a(struct rtw_adapter *padapter)
+bool is_ap_in_wep23a(struct rtw_adapter *padapter)
 {
 	u32 i;
-	struct ndis_802_11_var_ies *	pIE;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *cur_network = &pmlmeinfo->network;
+	const u8 *p;
+	int bcn_fixed_size;
 
-	if (rtw_get_capability23a((struct wlan_bssid_ex *)cur_network) & WLAN_CAPABILITY_PRIVACY) {
-		for (i = sizeof(struct ndis_802_11_fixed_ies); i < pmlmeinfo->network.IELength;) {
-			pIE = (struct ndis_802_11_var_ies *)(pmlmeinfo->network.IEs + i);
+	bcn_fixed_size = offsetof(struct ieee80211_mgmt, u.beacon.variable) -
+		offsetof(struct ieee80211_mgmt, u.beacon);
 
-			switch (pIE->ElementID) {
-			case _VENDOR_SPECIFIC_IE_:
-				if (!memcmp(pIE->data, RTW_WPA_OUI23A, 4))
+	if (rtw_get_capability23a(cur_network) & WLAN_CAPABILITY_PRIVACY) {
+		for (i = bcn_fixed_size; i < pmlmeinfo->network.IELength;) {
+			p = pmlmeinfo->network.IEs + i;
+
+			switch (p[0]) {
+			case WLAN_EID_VENDOR_SPECIFIC:
+				if (!memcmp(p + 2, RTW_WPA_OUI23A_TYPE, 4))
 					return false;
 				break;
-			case _RSN_IE_2_:
+			case WLAN_EID_RSN:
 				return false;
 
 			default:
 				break;
 			}
 
-			i += (pIE->Length + 2);
+			i += (p[1] + 2);
 		}
 
 		return true;
-	} else {
+	} else
 		return false;
-	}
 }
 
-int wifirate2_ratetbl_inx23a(unsigned char rate)
+static int wifirate2_ratetbl_inx23a(unsigned char rate)
 {
-	int	inx = 0;
+	int inx = 0;
 	rate = rate & 0x7f;
 
 	switch (rate) {
@@ -1350,7 +1333,7 @@ unsigned int update_supported_rate23a(unsigned char *ptn, unsigned int ptn_sz)
 	unsigned int i, num_of_rate;
 	unsigned int mask = 0;
 
-	num_of_rate = (ptn_sz > NumRates)? NumRates: ptn_sz;
+	num_of_rate = (ptn_sz > NumRates) ? NumRates : ptn_sz;
 
 	for (i = 0; i < num_of_rate; i++)
 		mask |= 0x1 << wifirate2_ratetbl_inx23a(*(ptn + i));
@@ -1361,21 +1344,22 @@ unsigned int update_MSC_rate23a(struct HT_caps_element *pHT_caps)
 {
 	unsigned int mask = 0;
 
-	mask = ((pHT_caps->u.HT_cap_element.MCS_rate[0] << 12) | (pHT_caps->u.HT_cap_element.MCS_rate[1] << 20));
+	mask = pHT_caps->u.HT_cap_element.MCS_rate[0] << 12 |
+		pHT_caps->u.HT_cap_element.MCS_rate[1] << 20;
 
 	return mask;
 }
 
 int support_short_GI23a(struct rtw_adapter *padapter,
-		     struct HT_caps_element *pHT_caps)
+			struct HT_caps_element *pHT_caps)
 {
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	unsigned char bit_offset;
 
-	if (!(pmlmeinfo->HT_enable))
+	if (!pmlmeinfo->HT_enable)
 		return _FAIL;
-	if ((pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_RALINK))
+	if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_RALINK)
 		return _FAIL;
 	bit_offset = (pmlmeext->cur_bwmode & HT_CHANNEL_WIDTH_40)? 6: 5;
 
@@ -1399,25 +1383,13 @@ unsigned char get_highest_rate_idx23a(u32 mask)
 	return rate_idx;
 }
 
-unsigned char get_highest_mcs_rate(struct HT_caps_element *pHT_caps)
-{
-	int i, mcs_rate;
-
-	mcs_rate = (pHT_caps->u.HT_cap_element.MCS_rate[0] | (pHT_caps->u.HT_cap_element.MCS_rate[1] << 8));
-
-	for (i = 15; i >= 0; i--) {
-		if (mcs_rate & (0x1 << i))
-			break;
-	}
-	return i;
-}
-
 void Update_RA_Entry23a(struct rtw_adapter *padapter, struct sta_info *psta)
 {
 	rtw_hal_update_ra_mask23a(psta, 0);
 }
 
-void enable_rate_adaptive(struct rtw_adapter *padapter, struct sta_info *psta)
+static void enable_rate_adaptive(struct rtw_adapter *padapter,
+				 struct sta_info *psta)
 {
 	Update_RA_Entry23a(padapter, psta);
 }
@@ -1432,19 +1404,10 @@ void set_sta_rate23a(struct rtw_adapter *padapter, struct sta_info *psta)
 void update_tx_basic_rate23a(struct rtw_adapter *padapter, u8 wirelessmode)
 {
 	unsigned char supported_rates[NDIS_802_11_LENGTH_RATES_EX];
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info*	pwdinfo = &padapter->wdinfo;
-
-	/*	Added by Albert 2011/03/22 */
-	/*	In the P2P mode, the driver should not support the b mode. */
-	/*	So, the Tx packet shouldn't use the CCK rate */
-	if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE))
-		return;
-#endif /* CONFIG_8723AU_P2P */
 
 	memset(supported_rates, 0, NDIS_802_11_LENGTH_RATES_EX);
 
-	if ((wirelessmode & WIRELESS_11B) && (wirelessmode == WIRELESS_11B)) {
+	if (wirelessmode == WIRELESS_11B) {
 		memcpy(supported_rates, rtw_basic_rate_cck, 4);
 	} else if (wirelessmode & WIRELESS_11B) {
 		memcpy(supported_rates, rtw_basic_rate_mix, 7);
@@ -1457,67 +1420,68 @@ void update_tx_basic_rate23a(struct rtw_adapter *padapter, u8 wirelessmode)
 	else
 		update_mgnt_tx_rate23a(padapter, IEEE80211_OFDM_RATE_6MB);
 
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_BASIC_RATE, supported_rates);
+	HalSetBrateCfg23a(padapter, supported_rates);
 }
 
 unsigned char check_assoc_AP23a(u8 *pframe, uint len)
 {
-	unsigned int	i;
-	struct ndis_802_11_var_ies *	pIE;
-	u8	epigram_vendor_flag;
-	u8	ralink_vendor_flag;
+	int i, bcn_fixed_size;
+	u8 epigram_vendor_flag;
+	u8 ralink_vendor_flag;
+	const u8 *p;
 	epigram_vendor_flag = 0;
 	ralink_vendor_flag = 0;
 
-	for (i = sizeof(struct ndis_802_11_fixed_ies); i < len;) {
-		pIE = (struct ndis_802_11_var_ies *)(pframe + i);
+	bcn_fixed_size = offsetof(struct ieee80211_mgmt, u.beacon.variable) -
+		offsetof(struct ieee80211_mgmt, u.beacon);
+
+	for (i = bcn_fixed_size; i < len;) {
+		p = pframe + i;
 
-		switch (pIE->ElementID) {
-		case _VENDOR_SPECIFIC_IE_:
-			if ((!memcmp(pIE->data, ARTHEROS_OUI1, 3)) ||
-			    (!memcmp(pIE->data, ARTHEROS_OUI2, 3))) {
+		switch (p[0]) {
+		case WLAN_EID_VENDOR_SPECIFIC:
+			if (!memcmp(p + 2, ARTHEROS_OUI1, 3) ||
+			    !memcmp(p + 2, ARTHEROS_OUI2, 3)) {
 				DBG_8723A("link to Artheros AP\n");
 				return HT_IOT_PEER_ATHEROS;
-			} else if ((!memcmp(pIE->data, BROADCOM_OUI1, 3)) ||
-				   !memcmp(pIE->data, BROADCOM_OUI2, 3) ||
-				   !memcmp(pIE->data, BROADCOM_OUI2, 3)) {
+			} else if (!memcmp(p + 2, BROADCOM_OUI1, 3) ||
+				   !memcmp(p + 2, BROADCOM_OUI2, 3) ||
+				   !memcmp(p + 2, BROADCOM_OUI2, 3)) {
 				DBG_8723A("link to Broadcom AP\n");
 				return HT_IOT_PEER_BROADCOM;
-			} else if (!memcmp(pIE->data, MARVELL_OUI, 3)) {
+			} else if (!memcmp(p + 2, MARVELL_OUI, 3)) {
 				DBG_8723A("link to Marvell AP\n");
 				return HT_IOT_PEER_MARVELL;
-			} else if (!memcmp(pIE->data, RALINK_OUI, 3)) {
-				if (!ralink_vendor_flag) {
+			} else if (!memcmp(p + 2, RALINK_OUI, 3)) {
+				if (!ralink_vendor_flag)
 					ralink_vendor_flag = 1;
-				} else {
+				else {
 					DBG_8723A("link to Ralink AP\n");
 					return HT_IOT_PEER_RALINK;
 				}
-			} else if (!memcmp(pIE->data, CISCO_OUI, 3)) {
+			} else if (!memcmp(p + 2, CISCO_OUI, 3)) {
 				DBG_8723A("link to Cisco AP\n");
 				return HT_IOT_PEER_CISCO;
-			} else if (!memcmp(pIE->data, REALTEK_OUI, 3)) {
+			} else if (!memcmp(p + 2, REALTEK_OUI, 3)) {
 				DBG_8723A("link to Realtek 96B\n");
 				return HT_IOT_PEER_REALTEK;
-			} else if (!memcmp(pIE->data, AIRGOCAP_OUI, 3)) {
+			} else if (!memcmp(p + 2, AIRGOCAP_OUI, 3)) {
 				DBG_8723A("link to Airgo Cap\n");
 				return HT_IOT_PEER_AIRGO;
-			} else if (!memcmp(pIE->data, EPIGRAM_OUI, 3)) {
+			} else if (!memcmp(p + 2, EPIGRAM_OUI, 3)) {
 				epigram_vendor_flag = 1;
 				if (ralink_vendor_flag) {
 					DBG_8723A("link to Tenda W311R AP\n");
 					return HT_IOT_PEER_TENDA;
-				} else {
+				} else
 					DBG_8723A("Capture EPIGRAM_OUI\n");
-				}
-			} else {
+			} else
 				break;
-			}
 		default:
 			break;
 		}
 
-		i += (pIE->Length + 2);
+		i += (p[1] + 2);
 	}
 
 	if (ralink_vendor_flag && !epigram_vendor_flag) {
@@ -1534,7 +1498,7 @@ unsigned char check_assoc_AP23a(u8 *pframe, uint len)
 
 void update_IOT_info23a(struct rtw_adapter *padapter)
 {
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
 	switch (pmlmeinfo->assoc_AP_vendor) {
@@ -1546,15 +1510,15 @@ void update_IOT_info23a(struct rtw_adapter *padapter)
 		pmlmeinfo->turboMode_cts2self = 0;
 		pmlmeinfo->turboMode_rtsen = 1;
 		/* disable high power */
-		Switch_DM_Func23a(padapter, ~DYNAMIC_BB_DYNAMIC_TXPWR,
-			       false);
+		rtl8723a_odm_support_ability_clr(padapter, (u32)
+						 ~DYNAMIC_BB_DYNAMIC_TXPWR);
 		break;
 	case HT_IOT_PEER_REALTEK:
 		/* rtw_write16(padapter, 0x4cc, 0xffff); */
 		/* rtw_write16(padapter, 0x546, 0x01c0); */
 		/* disable high power */
-		Switch_DM_Func23a(padapter, ~DYNAMIC_BB_DYNAMIC_TXPWR,
-			       false);
+		rtl8723a_odm_support_ability_clr(padapter, (u32)
+						 ~DYNAMIC_BB_DYNAMIC_TXPWR);
 		break;
 	default:
 		pmlmeinfo->turboMode_cts2self = 0;
@@ -1565,24 +1529,21 @@ void update_IOT_info23a(struct rtw_adapter *padapter)
 
 void update_capinfo23a(struct rtw_adapter *Adapter, u16 updateCap)
 {
-	struct mlme_ext_priv	*pmlmeext = &Adapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &Adapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
-	bool		ShortPreamble;
 
 	if (updateCap & cShortPreamble) {
 		/*  Short Preamble */
 		if (pmlmeinfo->preamble_mode != PREAMBLE_SHORT) {
 			/*  PREAMBLE_LONG or PREAMBLE_AUTO */
-			ShortPreamble = true;
 			pmlmeinfo->preamble_mode = PREAMBLE_SHORT;
-			rtw_hal_set_hwreg23a(Adapter, HW_VAR_ACK_PREAMBLE, (u8 *)&ShortPreamble);
+			rtl8723a_ack_preamble(Adapter, true);
 		}
 	} else { /*  Long Preamble */
 		if (pmlmeinfo->preamble_mode != PREAMBLE_LONG) {
 			/*  PREAMBLE_SHORT or PREAMBLE_AUTO */
-			ShortPreamble = false;
 			pmlmeinfo->preamble_mode = PREAMBLE_LONG;
-			rtw_hal_set_hwreg23a(Adapter, HW_VAR_ACK_PREAMBLE, (u8 *)&ShortPreamble);
+			rtl8723a_ack_preamble(Adapter, false);
 		}
 	}
 	if (updateCap & cIBSS) {
@@ -1590,32 +1551,34 @@ void update_capinfo23a(struct rtw_adapter *Adapter, u16 updateCap)
 		pmlmeinfo->slotTime = NON_SHORT_SLOT_TIME;
 	} else {
 		/* Filen: See 802.11-2007 p.90 */
-		if (pmlmeext->cur_wireless_mode & (WIRELESS_11G | WIRELESS_11_24N)) {
+		if (pmlmeext->cur_wireless_mode &
+		    (WIRELESS_11G | WIRELESS_11_24N)) {
 			if (updateCap & cShortSlotTime) { /*  Short Slot Time */
 				if (pmlmeinfo->slotTime != SHORT_SLOT_TIME)
 					pmlmeinfo->slotTime = SHORT_SLOT_TIME;
 			} else { /*  Long Slot Time */
 				if (pmlmeinfo->slotTime != NON_SHORT_SLOT_TIME)
-					pmlmeinfo->slotTime = NON_SHORT_SLOT_TIME;
+					pmlmeinfo->slotTime =
+						NON_SHORT_SLOT_TIME;
 			}
-		} else if (pmlmeext->cur_wireless_mode & (WIRELESS_11A | WIRELESS_11_5N)) {
+		} else if (pmlmeext->cur_wireless_mode &
+			   (WIRELESS_11A | WIRELESS_11_5N)) {
 			pmlmeinfo->slotTime = SHORT_SLOT_TIME;
 		} else {
 			/* B Mode */
 			pmlmeinfo->slotTime = NON_SHORT_SLOT_TIME;
 		}
 	}
-	rtw_hal_set_hwreg23a(Adapter, HW_VAR_SLOT_TIME, &pmlmeinfo->slotTime);
+	rtl8723a_set_slot_time(Adapter, pmlmeinfo->slotTime);
 }
 
 void update_wireless_mode23a(struct rtw_adapter *padapter)
 {
 	int ratelen, network_type = 0;
-	u32 SIFS_Timer;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *cur_network = &pmlmeinfo->network;
-	unsigned char			*rate = cur_network->SupportedRates;
+	unsigned char *rate = cur_network->SupportedRates;
 
 	ratelen = rtw_get_rateset_len23a(cur_network->SupportedRates);
 
@@ -1630,20 +1593,20 @@ void update_wireless_mode23a(struct rtw_adapter *padapter)
 		if (pmlmeinfo->HT_enable)
 			network_type = WIRELESS_11_24N;
 
-		if ((cckratesonly_included23a(rate, ratelen)) == true)
+		if (cckratesonly_included23a(rate, ratelen) == true)
 			network_type |= WIRELESS_11B;
-		else if ((cckrates_included23a(rate, ratelen)) == true)
+		else if (cckrates_included23a(rate, ratelen) == true)
 			network_type |= WIRELESS_11BG;
 		else
 			network_type |= WIRELESS_11G;
 	}
 
-	pmlmeext->cur_wireless_mode = network_type & padapter->registrypriv.wireless_mode;
+	pmlmeext->cur_wireless_mode =
+		network_type & padapter->registrypriv.wireless_mode;
 
-	SIFS_Timer = 0x0a0a0808; /* 0x0808 -> for CCK, 0x0a0a -> for OFDM */
-                             /* change this value if having IOT issues. */
-
-	padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_RESP_SIFS,  (u8 *)&SIFS_Timer);
+	/* 0x0808 -> for CCK, 0x0a0a -> for OFDM */
+	/* change this value if having IOT issues. */
+	rtl8723a_set_resp_sifs(padapter, 0x08, 0x08, 0x0a, 0x0a);
 
 	if (pmlmeext->cur_wireless_mode & WIRELESS_11B)
 		update_mgnt_tx_rate23a(padapter, IEEE80211_CCK_RATE_1MB);
@@ -1658,41 +1621,45 @@ void update_bmc_sta_support_rate23a(struct rtw_adapter *padapter, u32 mac_id)
 
 	if (pmlmeext->cur_wireless_mode & WIRELESS_11B) {
 		/*  Only B, B/G, and B/G/N AP could use CCK rate */
-		memcpy((pmlmeinfo->FW_sta_info[mac_id].SupportedRates), rtw_basic_rate_cck, 4);
+		memcpy((pmlmeinfo->FW_sta_info[mac_id].SupportedRates),
+		       rtw_basic_rate_cck, 4);
 	} else {
-		memcpy((pmlmeinfo->FW_sta_info[mac_id].SupportedRates), rtw_basic_rate_ofdm, 4);
+		memcpy(pmlmeinfo->FW_sta_info[mac_id].SupportedRates,
+		       rtw_basic_rate_ofdm, 3);
 	}
 }
 
-int update_sta_support_rate23a(struct rtw_adapter *padapter, u8 *pvar_ie, uint var_ie_len, int cam_idx)
+int update_sta_support_rate23a(struct rtw_adapter *padapter, u8 *pvar_ie,
+			       uint var_ie_len, int cam_idx)
 {
-	unsigned int	ie_len;
-	struct ndis_802_11_var_ies *pIE;
-	int	supportRateNum = 0;
+	int supportRateNum = 0;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
+	const u8 *p;
 
-	pIE = (struct ndis_802_11_var_ies *)rtw_get_ie23a(pvar_ie, _SUPPORTEDRATES_IE_, &ie_len, var_ie_len);
-	if (pIE == NULL)
+	p = cfg80211_find_ie(WLAN_EID_SUPP_RATES, pvar_ie, var_ie_len);
+	if (!p)
 		return _FAIL;
 
-	memcpy(pmlmeinfo->FW_sta_info[cam_idx].SupportedRates, pIE->data, ie_len);
-	supportRateNum = ie_len;
+	memcpy(pmlmeinfo->FW_sta_info[cam_idx].SupportedRates, p + 2, p[1]);
+	supportRateNum = p[1];
 
-	pIE = (struct ndis_802_11_var_ies *)rtw_get_ie23a(pvar_ie, _EXT_SUPPORTEDRATES_IE_, &ie_len, var_ie_len);
-	if (pIE)
-		memcpy((pmlmeinfo->FW_sta_info[cam_idx].SupportedRates + supportRateNum), pIE->data, ie_len);
+	p = cfg80211_find_ie(WLAN_EID_EXT_SUPP_RATES, pvar_ie, var_ie_len);
+	if (p)
+		memcpy(pmlmeinfo->FW_sta_info[cam_idx].SupportedRates +
+		       supportRateNum, p + 2, p[1]);
 	return _SUCCESS;
 }
 
-void process_addba_req23a(struct rtw_adapter *padapter, u8 *paddba_req, u8 *addr)
+void process_addba_req23a(struct rtw_adapter *padapter,
+			  u8 *paddba_req, u8 *addr)
 {
 	struct sta_info *psta;
 	u16 tid, start_seq, param;
 	struct recv_reorder_ctrl *preorder_ctrl;
 	struct sta_priv *pstapriv = &padapter->stapriv;
-	struct ADDBA_request	*preq = (struct ADDBA_request*)paddba_req;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct ADDBA_request *preq = (struct ADDBA_request*)paddba_req;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
 	psta = rtw_get_stainfo23a(pstapriv, addr);
@@ -1701,41 +1668,17 @@ void process_addba_req23a(struct rtw_adapter *padapter, u8 *paddba_req, u8 *addr
 		start_seq = le16_to_cpu(preq->BA_starting_seqctrl) >> 4;
 
 		param = le16_to_cpu(preq->BA_para_set);
-		tid = (param>>2)&0x0f;
+		tid = (param >> 2) & 0x0f;
 
 		preorder_ctrl = &psta->recvreorder_ctrl[tid];
 
 		preorder_ctrl->indicate_seq = 0xffff;
 
-		preorder_ctrl->enable = (pmlmeinfo->bAcceptAddbaReq == true)? true :false;
+		preorder_ctrl->enable = (pmlmeinfo->bAcceptAddbaReq == true) ?
+			true : false;
 	}
 }
 
-void update_TSF23a(struct mlme_ext_priv *pmlmeext, u8 *pframe, uint len)
-{
-	u8 *pIE;
-	u32 *pbuf;
-
-	pIE = pframe + sizeof(struct ieee80211_hdr_3addr);
-	pbuf = (u32 *)pIE;
-
-	pmlmeext->TSFValue = le32_to_cpu(*(pbuf+1));
-
-	pmlmeext->TSFValue = pmlmeext->TSFValue << 32;
-
-	pmlmeext->TSFValue |= le32_to_cpu(*pbuf);
-}
-
-void correct_TSF23a(struct rtw_adapter *padapter, struct mlme_ext_priv *pmlmeext)
-{
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_CORRECT_TSF, NULL);
-}
-
-void beacon_timing_control23a(struct rtw_adapter *padapter)
-{
-	rtw_hal_bcn_related_reg_setting23a(padapter);
-}
-
 static struct rtw_adapter *pbuddy_padapter;
 
 int rtw_handle_dualmac23a(struct rtw_adapter *adapter, bool init)
@@ -1745,16 +1688,18 @@ int rtw_handle_dualmac23a(struct rtw_adapter *adapter, bool init)
 	if (init) {
 		if (pbuddy_padapter == NULL) {
 			pbuddy_padapter = adapter;
-			DBG_8723A("%s(): pbuddy_padapter == NULL, Set pbuddy_padapter\n", __func__);
+			DBG_8723A("%s(): pbuddy_padapter == NULL, "
+				  "Set pbuddy_padapter\n", __func__);
 		} else {
 			adapter->pbuddy_adapter = pbuddy_padapter;
 			pbuddy_padapter->pbuddy_adapter = adapter;
 			/*  clear global value */
 			pbuddy_padapter = NULL;
-			DBG_8723A("%s(): pbuddy_padapter exist, Exchange Information\n", __func__);
+			DBG_8723A("%s(): pbuddy_padapter exist, "
+				  "Exchange Information\n", __func__);
 		}
-	  } else {
+	} else
 		pbuddy_padapter = NULL;
-	}
+
 	return status;
 }
diff --git a/drivers/staging/rtl8723au/core/rtw_xmit.c b/drivers/staging/rtl8723au/core/rtw_xmit.c
index 0f10cfa10d3..b917526f570 100644
--- a/drivers/staging/rtl8723au/core/rtw_xmit.c
+++ b/drivers/staging/rtl8723au/core/rtw_xmit.c
@@ -20,6 +20,7 @@
 #include <osdep_intf.h>
 #include <linux/ip.h>
 #include <usb_ops.h>
+#include <rtl8723a_xmit.h>
 
 static u8 P802_1H_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0xf8 };
 static u8 RFC1042_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0x00 };
@@ -50,27 +51,21 @@ void	_rtw_init_sta_xmit_priv23a(struct sta_xmit_priv *psta_xmitpriv)
 
 }
 
-s32	_rtw_init_xmit_priv23a(struct xmit_priv *pxmitpriv, struct rtw_adapter *padapter)
+int _rtw_init_xmit_priv23a(struct xmit_priv *pxmitpriv,
+			   struct rtw_adapter *padapter)
 {
 	int i;
 	struct xmit_buf *pxmitbuf;
 	struct xmit_frame *pxframe;
-	int	res = _SUCCESS;
+	int res = _SUCCESS;
 	u32 max_xmit_extbuf_size = MAX_XMIT_EXTBUF_SZ;
 	u32 num_xmit_extbuf = NR_XMIT_EXTBUFF;
 
-	/*  We don't need to memset padapter->XXX to zero, because adapter is allocated by rtw_zvmalloc(). */
-	/* memset((unsigned char *)pxmitpriv, 0, sizeof(struct xmit_priv)); */
-
 	spin_lock_init(&pxmitpriv->lock);
 	spin_lock_init(&pxmitpriv->lock_sctx);
 	sema_init(&pxmitpriv->xmit_sema, 0);
 	sema_init(&pxmitpriv->terminate_xmitthread_sema, 0);
 
-	/*
-	Please insert all the queue initializaiton using _rtw_init_queue23a below
-	*/
-
 	pxmitpriv->adapter = padapter;
 
 	_rtw_init_queue23a(&pxmitpriv->be_pending);
@@ -81,42 +76,20 @@ s32	_rtw_init_xmit_priv23a(struct xmit_priv *pxmitpriv, struct rtw_adapter *pada
 
 	_rtw_init_queue23a(&pxmitpriv->free_xmit_queue);
 
-	/*
-	Please allocate memory with the sz = (struct xmit_frame) * NR_XMITFRAME,
-	and initialize free_xmit_frame below.
-	Please also apply  free_txobj to link_up all the xmit_frames...
-	*/
-
-	pxmitpriv->pallocated_frame_buf = rtw_zvmalloc(NR_XMITFRAME * sizeof(struct xmit_frame) + 4);
-
-	if (pxmitpriv->pallocated_frame_buf  == NULL) {
-		pxmitpriv->pxmit_frame_buf = NULL;
-		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("alloc xmit_frame fail!\n"));
-		res = _FAIL;
-		goto exit;
-	}
-	pxmitpriv->pxmit_frame_buf = PTR_ALIGN(pxmitpriv->pallocated_frame_buf, 4);
-
-	pxframe = (struct xmit_frame*) pxmitpriv->pxmit_frame_buf;
-
 	for (i = 0; i < NR_XMITFRAME; i++) {
+		pxframe = kzalloc(sizeof(struct xmit_frame), GFP_KERNEL);
+		if (!pxframe)
+			break;
 		INIT_LIST_HEAD(&pxframe->list);
 
 		pxframe->padapter = padapter;
 		pxframe->frame_tag = NULL_FRAMETAG;
 
-		pxframe->pkt = NULL;
-
-		pxframe->buf_addr = NULL;
-		pxframe->pxmitbuf = NULL;
-
 		list_add_tail(&pxframe->list,
 			      &pxmitpriv->free_xmit_queue.queue);
-
-		pxframe++;
 	}
 
-	pxmitpriv->free_xmitframe_cnt = NR_XMITFRAME;
+	pxmitpriv->free_xmitframe_cnt = i;
 
 	pxmitpriv->frag_len = MAX_FRAG_THRESHOLD;
 
@@ -152,18 +125,10 @@ s32	_rtw_init_xmit_priv23a(struct xmit_priv *pxmitpriv, struct rtw_adapter *pada
 	/* init xframe_ext queue,  the same count as extbuf  */
 	_rtw_init_queue23a(&pxmitpriv->free_xframe_ext_queue);
 
-	pxmitpriv->xframe_ext_alloc_addr = rtw_zvmalloc(num_xmit_extbuf * sizeof(struct xmit_frame) + 4);
-
-	if (pxmitpriv->xframe_ext_alloc_addr  == NULL) {
-		pxmitpriv->xframe_ext = NULL;
-		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("alloc xframe_ext fail!\n"));
-		res = _FAIL;
-		goto exit;
-	}
-	pxmitpriv->xframe_ext = PTR_ALIGN(pxmitpriv->xframe_ext_alloc_addr, 4);
-	pxframe = (struct xmit_frame*)pxmitpriv->xframe_ext;
-
 	for (i = 0; i < num_xmit_extbuf; i++) {
+		pxframe = kzalloc(sizeof(struct xmit_frame), GFP_KERNEL);
+		if (!pxframe)
+			break;
 		INIT_LIST_HEAD(&pxframe->list);
 
 		pxframe->padapter = padapter;
@@ -178,10 +143,8 @@ s32	_rtw_init_xmit_priv23a(struct xmit_priv *pxmitpriv, struct rtw_adapter *pada
 
 		list_add_tail(&pxframe->list,
 			      &pxmitpriv->free_xframe_ext_queue.queue);
-
-		pxframe++;
 	}
-	pxmitpriv->free_xframe_ext_cnt = num_xmit_extbuf;
+	pxmitpriv->free_xframe_ext_cnt = i;
 
 	/*  Init xmit extension buff */
 	_rtw_init_queue23a(&pxmitpriv->free_xmit_extbuf_queue);
@@ -230,7 +193,9 @@ s32	_rtw_init_xmit_priv23a(struct xmit_priv *pxmitpriv, struct rtw_adapter *pada
 	pxmitpriv->ack_tx = false;
 	mutex_init(&pxmitpriv->ack_tx_mutex);
 	rtw_sctx_init23a(&pxmitpriv->ack_tx_ops, 0);
-	rtw_hal_init23a_xmit_priv(padapter);
+	tasklet_init(&padapter->xmitpriv.xmit_tasklet,
+		     (void(*)(unsigned long))rtl8723au_xmit_tasklet,
+		     (unsigned long)padapter);
 
 exit:
 
@@ -242,19 +207,15 @@ fail:
 void _rtw_free_xmit_priv23a (struct xmit_priv *pxmitpriv)
 {
 	struct rtw_adapter *padapter = pxmitpriv->adapter;
-	struct xmit_frame *pxmitframe = (struct xmit_frame*) pxmitpriv->pxmit_frame_buf;
+	struct xmit_frame *pxframe;
 	struct xmit_buf *pxmitbuf;
 	struct list_head *plist, *ptmp;
-	u32 num_xmit_extbuf = NR_XMIT_EXTBUFF;
-	int i;
-
-	rtw_hal_free_xmit_priv23a(padapter);
 
-	if (pxmitpriv->pxmit_frame_buf == NULL)
-		return;
-	for (i = 0; i < NR_XMITFRAME; i++) {
-		rtw_os_xmit_complete23a(padapter, pxmitframe);
-		pxmitframe++;
+	list_for_each_safe(plist, ptmp, &pxmitpriv->free_xmit_queue.queue) {
+		pxframe = container_of(plist, struct xmit_frame, list);
+		list_del_init(&pxframe->list);
+		rtw_os_xmit_complete23a(padapter, pxframe);
+		kfree(pxframe);
 	}
 
 	list_for_each_safe(plist, ptmp, &pxmitpriv->xmitbuf_list) {
@@ -264,19 +225,14 @@ void _rtw_free_xmit_priv23a (struct xmit_priv *pxmitpriv)
 		kfree(pxmitbuf);
 	}
 
-	if (pxmitpriv->pallocated_frame_buf) {
-		rtw_vmfree(pxmitpriv->pallocated_frame_buf, NR_XMITFRAME * sizeof(struct xmit_frame) + 4);
-	}
-
 	/* free xframe_ext queue,  the same count as extbuf  */
-	if ((pxmitframe = (struct xmit_frame*)pxmitpriv->xframe_ext)) {
-		for (i = 0; i<num_xmit_extbuf; i++) {
-			rtw_os_xmit_complete23a(padapter, pxmitframe);
-			pxmitframe++;
-		}
+	list_for_each_safe(plist, ptmp,
+			   &pxmitpriv->free_xframe_ext_queue.queue) {
+		pxframe = container_of(plist, struct xmit_frame, list);
+		list_del_init(&pxframe->list);
+		rtw_os_xmit_complete23a(padapter, pxframe);
+		kfree(pxframe);
 	}
-	if (pxmitpriv->xframe_ext_alloc_addr)
-		rtw_vmfree(pxmitpriv->xframe_ext_alloc_addr, num_xmit_extbuf * sizeof(struct xmit_frame) + 4);
 
 	/*  free xmit extension buff */
 	list_for_each_safe(plist, ptmp, &pxmitpriv->xmitextbuf_list) {
@@ -338,9 +294,10 @@ static void update_attrib_vcs_info(struct rtw_adapter *padapter, struct xmit_fra
 	} else {
 		while (true) {
 			/* IOT action */
-			if ((pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_ATHEROS) &&
-			    (pattrib->ampdu_en) &&
-			    (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)) {
+			if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_ATHEROS &&
+			    pattrib->ampdu_en &&
+			    padapter->securitypriv.dot11PrivacyAlgrthm ==
+			    WLAN_CIPHER_SUITE_CCMP) {
 				pattrib->vcs_mode = CTS_TO_SELF;
 				break;
 			}
@@ -437,27 +394,24 @@ u8 qos_acm23a(u8 acm_mask, u8 priority)
 	default:
 		DBG_8723A("qos_acm23a(): invalid pattrib->priority: %d!!!\n",
 			  priority);
+		change_priority = 0;
 		break;
 	}
 
 	return change_priority;
 }
 
-static void set_qos(struct pkt_file *ppktfile, struct pkt_attrib *pattrib)
+static void set_qos(struct sk_buff *skb, struct pkt_attrib *pattrib)
 {
-	struct ethhdr etherhdr;
-	struct iphdr ip_hdr;
-	s32 UserPriority = 0;
-
-	_rtw_open_pktfile23a(ppktfile->pkt, ppktfile);
-	_rtw_pktfile_read23a(ppktfile, (unsigned char*)&etherhdr, ETH_HLEN);
+	u8 *pframe = skb->data;
+	struct iphdr *ip_hdr;
+	u8 UserPriority = 0;
 
 	/*  get UserPriority from IP hdr */
-	if (pattrib->ether_type == 0x0800) {
-		_rtw_pktfile_read23a(ppktfile, (u8*)&ip_hdr, sizeof(ip_hdr));
-/*		UserPriority = (ntohs(ip_hdr.tos) >> 5) & 0x3; */
-		UserPriority = ip_hdr.tos >> 5;
-	} else if (pattrib->ether_type == 0x888e) {
+	if (pattrib->ether_type == ETH_P_IP) {
+		ip_hdr = (struct iphdr *)(pframe + ETH_HLEN);
+		UserPriority = ip_hdr->tos >> 5;
+	} else if (pattrib->ether_type == ETH_P_PAE) {
 		/*  "When priority processing of data frames is supported, */
 		/*  a STA's SME should send EAPOL-Key frames at the highest
 		    priority." */
@@ -466,61 +420,54 @@ static void set_qos(struct pkt_file *ppktfile, struct pkt_attrib *pattrib)
 
 	pattrib->priority = UserPriority;
 	pattrib->hdrlen = sizeof(struct ieee80211_qos_hdr);
-	pattrib->subtype = WIFI_QOS_DATA_TYPE;
+	pattrib->type = IEEE80211_FTYPE_DATA | IEEE80211_STYPE_QOS_DATA;
 }
 
-static s32 update_attrib(struct rtw_adapter *padapter,
-			 struct sk_buff *pkt, struct pkt_attrib *pattrib)
+static int update_attrib(struct rtw_adapter *padapter,
+			 struct sk_buff *skb, struct pkt_attrib *pattrib)
 {
-	uint i;
-	struct pkt_file pktfile;
 	struct sta_info *psta = NULL;
-	struct ethhdr etherhdr;
-
 	int bmcast;
 	struct sta_priv	*pstapriv = &padapter->stapriv;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct qos_priv	*pqospriv = &pmlmepriv->qospriv;
 	int res = _SUCCESS;
+	struct ethhdr *ehdr = (struct ethhdr *) skb->data;
 
-	_rtw_open_pktfile23a(pkt, &pktfile);
-	i = _rtw_pktfile_read23a(&pktfile, (u8*)&etherhdr, ETH_HLEN);
-
-	pattrib->ether_type = ntohs(etherhdr.h_proto);
+	pattrib->ether_type = ntohs(ehdr->h_proto);
 
-	memcpy(pattrib->dst, &etherhdr.h_dest, ETH_ALEN);
-	memcpy(pattrib->src, &etherhdr.h_source, ETH_ALEN);
+	ether_addr_copy(pattrib->dst, ehdr->h_dest);
+	ether_addr_copy(pattrib->src, ehdr->h_source);
 
 	pattrib->pctrl = 0;
 
-	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true) ||
-		(check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true)) {
-		memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
-		memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
-	}
-	else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
-		memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);
-		memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
-	}
-	else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
-		memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
-		memcpy(pattrib->ta, get_bssid(pmlmepriv), ETH_ALEN);
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) ||
+	    check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
+		ether_addr_copy(pattrib->ra, pattrib->dst);
+		ether_addr_copy(pattrib->ta, pattrib->src);
+	} else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+		ether_addr_copy(pattrib->ra, get_bssid(pmlmepriv));
+		ether_addr_copy(pattrib->ta, pattrib->src);
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		ether_addr_copy(pattrib->ra, pattrib->dst);
+		ether_addr_copy(pattrib->ta, get_bssid(pmlmepriv));
 	}
 
-	pattrib->pktlen = pktfile.pkt_len;
+	pattrib->pktlen = skb->len - ETH_HLEN;
 
 	if (pattrib->ether_type == ETH_P_IP) {
 		/*  The following is for DHCP and ARP packet, we use cck1M
 		    to tx these packets and let LPS awake some time */
 		/*  to prevent DHCP protocol fail */
-		u8 tmp[24];
-		_rtw_pktfile_read23a(&pktfile, &tmp[0], 24);
 		pattrib->dhcp_pkt = 0;
-		if (pktfile.pkt_len > 282) {/* MINIMUM_DHCP_PACKET_SIZE) { */
-			if (ETH_P_IP == pattrib->ether_type) {/*  IP header */
-				if (((tmp[21] == 68) && (tmp[23] == 67)) ||
-				    ((tmp[21] == 67) && (tmp[23] == 68))) {
+		/* MINIMUM_DHCP_PACKET_SIZE) { */
+		if (pattrib->pktlen > 282 + 24) {
+			if (pattrib->ether_type == ETH_P_IP) {/*  IP header */
+				u8 *pframe = skb->data;
+				pframe += ETH_HLEN;
+
+				if ((pframe[21] == 68 && pframe[23] == 67) ||
+				    (pframe[21] == 67 && pframe[23] == 68)) {
 					/*  68 : UDP BOOTP client */
 					/*  67 : UDP BOOTP server */
 					RT_TRACE(_module_rtl871x_xmit_c_,
@@ -532,17 +479,17 @@ static s32 update_attrib(struct rtw_adapter *padapter,
 				}
 			}
 		}
-	} else if (0x888e == pattrib->ether_type) {
+	} else if (pattrib->ether_type == ETH_P_PAE) {
 		DBG_8723A_LEVEL(_drv_always_, "send eapol packet\n");
 	}
 
-	if ((pattrib->ether_type == 0x888e) || (pattrib->dhcp_pkt == 1)) {
+	if ((pattrib->ether_type == ETH_P_PAE) || (pattrib->dhcp_pkt == 1)) {
 		rtw_set_scan_deny(padapter, 3000);
 	}
 
 	/*  If EAPOL , ARP , OR DHCP packet, driver must be in active mode. */
-	if ((pattrib->ether_type == 0x0806) ||
-	    (pattrib->ether_type == 0x888e) || (pattrib->dhcp_pkt == 1)) {
+	if ((pattrib->ether_type == ETH_P_ARP) ||
+	    (pattrib->ether_type == ETH_P_PAE) || (pattrib->dhcp_pkt == 1)) {
 		rtw_lps_ctrl_wk_cmd23a(padapter, LPS_CTRL_SPECIAL_PACKET, 1);
 	}
 
@@ -559,7 +506,7 @@ static s32 update_attrib(struct rtw_adapter *padapter,
 				  MAC_FMT"\n", MAC_ARG(pattrib->ra)));
 			res = _FAIL;
 			goto exit;
-		} else if ((check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) &&
+		} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE) &&
 			   (!(psta->state & _FW_LINKED))) {
 			res = _FAIL;
 			goto exit;
@@ -586,16 +533,16 @@ static s32 update_attrib(struct rtw_adapter *padapter,
 	pattrib->pkt_hdrlen = ETH_HLEN;
 
 	pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr);
-	pattrib->subtype = WIFI_DATA_TYPE;
+	pattrib->type = IEEE80211_FTYPE_DATA;
 	pattrib->priority = 0;
 
 	if (check_fwstate(pmlmepriv, WIFI_AP_STATE | WIFI_ADHOC_STATE |
 			  WIFI_ADHOC_MASTER_STATE)) {
 		if (psta->qos_option)
-			set_qos(&pktfile, pattrib);
+			set_qos(skb, pattrib);
 	} else {
-		if (pqospriv->qos_option) {
-			set_qos(&pktfile, pattrib);
+		if (pmlmepriv->qos_option) {
+			set_qos(skb, pattrib);
 
 			if (pmlmepriv->acm_mask != 0) {
 				pattrib->priority = qos_acm23a(pmlmepriv->acm_mask,
@@ -610,8 +557,8 @@ static s32 update_attrib(struct rtw_adapter *padapter,
 
 		pattrib->encrypt = 0;
 
-		if ((pattrib->ether_type != 0x888e) &&
-		    (check_fwstate(pmlmepriv, WIFI_MP_STATE) == false)) {
+		if ((pattrib->ether_type != ETH_P_PAE) &&
+		    !check_fwstate(pmlmepriv, WIFI_MP_STATE)) {
 			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
 				 ("\npsta->ieee8021x_blocked == true,  "
 				  "pattrib->ether_type(%.4x) != 0x888e\n",
@@ -644,31 +591,32 @@ static s32 update_attrib(struct rtw_adapter *padapter,
 	}
 
 	switch (pattrib->encrypt) {
-	case _WEP40_:
-	case _WEP104_:
-		pattrib->iv_len = 4;
-		pattrib->icv_len = 4;
+	case WLAN_CIPHER_SUITE_WEP40:
+	case WLAN_CIPHER_SUITE_WEP104:
+		pattrib->iv_len = IEEE80211_WEP_IV_LEN;
+		pattrib->icv_len = IEEE80211_WEP_ICV_LEN;
 		break;
 
-	case _TKIP_:
-		pattrib->iv_len = 8;
-		pattrib->icv_len = 4;
+	case WLAN_CIPHER_SUITE_TKIP:
+		pattrib->iv_len = IEEE80211_TKIP_IV_LEN;
+		pattrib->icv_len = IEEE80211_TKIP_ICV_LEN;
 
-		if (padapter->securitypriv.busetkipkey == _FAIL) {
+		if (!padapter->securitypriv.busetkipkey) {
 			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
 				 ("\npadapter->securitypriv.busetkip"
-				  "key(%d) == _FAIL drop packet\n",
+				  "key(%d) == false drop packet\n",
 				  padapter->securitypriv.busetkipkey));
 			res = _FAIL;
 			goto exit;
 		}
 
 		break;
-	case _AES_:
+	case WLAN_CIPHER_SUITE_CCMP:
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
-			 ("pattrib->encrypt =%d (_AES_)\n", pattrib->encrypt));
-		pattrib->iv_len = 8;
-		pattrib->icv_len = 8;
+			 ("pattrib->encrypt =%d (WLAN_CIPHER_SUITE_CCMP)\n",
+			  pattrib->encrypt));
+		pattrib->iv_len = IEEE80211_CCMP_HDR_LEN;
+		pattrib->icv_len = IEEE80211_CCMP_MIC_LEN;
 		break;
 
 	default:
@@ -680,7 +628,7 @@ static s32 update_attrib(struct rtw_adapter *padapter,
 	RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
 		 ("update_attrib: encrypt =%d\n", pattrib->encrypt));
 
-	if (pattrib->encrypt && psecuritypriv->hw_decrypted == false) {
+	if (pattrib->encrypt && !psecuritypriv->hw_decrypted) {
 		pattrib->bswenc = true;
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
 			 ("update_attrib: encrypt =%d bswenc = true\n",
@@ -697,7 +645,7 @@ exit:
 	return res;
 }
 
-static s32 xmitframe_addmic(struct rtw_adapter *padapter,
+static int xmitframe_addmic(struct rtw_adapter *padapter,
 			    struct xmit_frame *pxmitframe) {
 	struct mic_data micdata;
 	struct sta_info *stainfo;
@@ -730,7 +678,7 @@ static s32 xmitframe_addmic(struct rtw_adapter *padapter,
 
 	hw_hdr_offset = TXDESC_OFFSET;
 
-	if (pattrib->encrypt == _TKIP_) {
+	if (pattrib->encrypt == WLAN_CIPHER_SUITE_TKIP) {
 		/* encode mic code */
 		if (stainfo) {
 			u8 null_key[16]={0x0, 0x0, 0x0, 0x0,
@@ -775,7 +723,7 @@ static s32 xmitframe_addmic(struct rtw_adapter *padapter,
 							 &pframe[10], 6);
 			}
 
-			/* if (pqospriv->qos_option == 1) */
+			/* if (pmlmepriv->qos_option == 1) */
 			if (pattrib->qos_en)
 				priority[0] = (u8)pxmitframe->attrib.priority;
 
@@ -873,7 +821,7 @@ static s32 xmitframe_addmic(struct rtw_adapter *padapter,
 	return _SUCCESS;
 }
 
-static s32 xmitframe_swencrypt(struct rtw_adapter *padapter,
+static int xmitframe_swencrypt(struct rtw_adapter *padapter,
 			       struct xmit_frame *pxmitframe)
 {
 	struct pkt_attrib *pattrib = &pxmitframe->attrib;
@@ -884,14 +832,14 @@ static s32 xmitframe_swencrypt(struct rtw_adapter *padapter,
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_alert_,
 			 ("### xmitframe_swencrypt\n"));
 		switch (pattrib->encrypt) {
-		case _WEP40_:
-		case _WEP104_:
+		case WLAN_CIPHER_SUITE_WEP40:
+		case WLAN_CIPHER_SUITE_WEP104:
 			rtw_wep_encrypt23a(padapter, pxmitframe);
 			break;
-		case _TKIP_:
+		case WLAN_CIPHER_SUITE_TKIP:
 			rtw_tkip_encrypt23a(padapter, pxmitframe);
 			break;
-		case _AES_:
+		case WLAN_CIPHER_SUITE_CCMP:
 			rtw_aes_encrypt23a(padapter, pxmitframe);
 			break;
 		default:
@@ -906,17 +854,14 @@ static s32 xmitframe_swencrypt(struct rtw_adapter *padapter,
 	return _SUCCESS;
 }
 
-s32 rtw_make_wlanhdr23a(struct rtw_adapter *padapter, u8 *hdr,
-		        struct pkt_attrib *pattrib)
+static int rtw_make_wlanhdr(struct rtw_adapter *padapter, u8 *hdr,
+			    struct pkt_attrib *pattrib)
 {
-	u16 *qc;
-
 	struct ieee80211_hdr *pwlanhdr = (struct ieee80211_hdr *)hdr;
+	struct ieee80211_qos_hdr *qoshdr;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct qos_priv *pqospriv = &pmlmepriv->qospriv;
 	u8 qos_option = false;
 	int res = _SUCCESS;
-	u16 *fctrl = &pwlanhdr->frame_control;
 
 	struct sta_info *psta;
 
@@ -945,36 +890,36 @@ s32 rtw_make_wlanhdr23a(struct rtw_adapter *padapter, u8 *hdr,
 
 	memset(hdr, 0, WLANHDR_OFFSET);
 
-	SetFrameSubType(fctrl, pattrib->subtype);
+	pwlanhdr->frame_control = cpu_to_le16(pattrib->type);
 
-	if (pattrib->subtype & WIFI_DATA_TYPE) {
-		if ((check_fwstate(pmlmepriv,  WIFI_STATION_STATE) == true)) {
+	if (pattrib->type & IEEE80211_FTYPE_DATA) {
+		if (check_fwstate(pmlmepriv,  WIFI_STATION_STATE)) {
 			/* to_ds = 1, fr_ds = 0; */
 			/* Data transfer to AP */
-			SetToDs(fctrl);
-			memcpy(pwlanhdr->addr1, get_bssid(pmlmepriv), ETH_ALEN);
-			memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
-			memcpy(pwlanhdr->addr3, pattrib->dst, ETH_ALEN);
+			pwlanhdr->frame_control |=
+				cpu_to_le16(IEEE80211_FCTL_TODS);
+			ether_addr_copy(pwlanhdr->addr1, get_bssid(pmlmepriv));
+			ether_addr_copy(pwlanhdr->addr2, pattrib->src);
+			ether_addr_copy(pwlanhdr->addr3, pattrib->dst);
 
-			if (pqospriv->qos_option)
+			if (pmlmepriv->qos_option)
 				qos_option = true;
 
-		}
-		else if ((check_fwstate(pmlmepriv,  WIFI_AP_STATE) == true)) {
+		} else if (check_fwstate(pmlmepriv,  WIFI_AP_STATE)) {
 			/* to_ds = 0, fr_ds = 1; */
-			SetFrDs(fctrl);
-			memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
-			memcpy(pwlanhdr->addr2, get_bssid(pmlmepriv), ETH_ALEN);
-			memcpy(pwlanhdr->addr3, pattrib->src, ETH_ALEN);
+			pwlanhdr->frame_control |=
+				cpu_to_le16(IEEE80211_FCTL_FROMDS);
+			ether_addr_copy(pwlanhdr->addr1, pattrib->dst);
+			ether_addr_copy(pwlanhdr->addr2, get_bssid(pmlmepriv));
+			ether_addr_copy(pwlanhdr->addr3, pattrib->src);
 
 			if (psta->qos_option)
 				qos_option = true;
-		}
-		else if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true) ||
-		(check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true)) {
-			memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
-			memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
-			memcpy(pwlanhdr->addr3, get_bssid(pmlmepriv), ETH_ALEN);
+		} else if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) ||
+			   check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
+			ether_addr_copy(pwlanhdr->addr1, pattrib->dst);
+			ether_addr_copy(pwlanhdr->addr2, pattrib->src);
+			ether_addr_copy(pwlanhdr->addr3, get_bssid(pmlmepriv));
 
 			if (psta->qos_option)
 				qos_option = true;
@@ -985,15 +930,24 @@ s32 rtw_make_wlanhdr23a(struct rtw_adapter *padapter, u8 *hdr,
 			goto exit;
 		}
 		if (pattrib->mdata)
-			SetMData(fctrl);
+			pwlanhdr->frame_control |=
+				cpu_to_le16(IEEE80211_FCTL_MOREDATA);
 		if (pattrib->encrypt)
-			SetPrivacy(fctrl);
+			pwlanhdr->frame_control |=
+				cpu_to_le16(IEEE80211_FCTL_PROTECTED);
 		if (qos_option) {
-			qc = (unsigned short *)(hdr + pattrib->hdrlen - 2);
-			if (pattrib->priority)
-				SetPriority(qc, pattrib->priority);
-			SetEOSP(qc, pattrib->eosp);
-			SetAckpolicy(qc, pattrib->ack_policy);
+			qoshdr = (struct ieee80211_qos_hdr *)hdr;
+
+			qoshdr->qos_ctrl = cpu_to_le16(
+				pattrib->priority & IEEE80211_QOS_CTL_TID_MASK);
+
+			qoshdr->qos_ctrl |= cpu_to_le16(
+				(pattrib->ack_policy << 5) &
+				IEEE80211_QOS_CTL_ACK_POLICY_MASK);
+
+			if (pattrib->eosp)
+				qoshdr->qos_ctrl |=
+					cpu_to_le16(IEEE80211_QOS_CTL_EOSP);
 		}
 		/* TODO: fill HT Control Field */
 
@@ -1002,11 +956,18 @@ s32 rtw_make_wlanhdr23a(struct rtw_adapter *padapter, u8 *hdr,
 			psta->sta_xmitpriv.txseq_tid[pattrib->priority]++;
 			psta->sta_xmitpriv.txseq_tid[pattrib->priority] &= 0xFFF;
 			pattrib->seqnum = psta->sta_xmitpriv.txseq_tid[pattrib->priority];
-			SetSeqNum(hdr, pattrib->seqnum);
+			/* We dont need to worry about frag bits here */
+			pwlanhdr->seq_ctrl = cpu_to_le16(IEEE80211_SN_TO_SEQ(
+							      pattrib->seqnum));
 			/* check if enable ampdu */
 			if (pattrib->ht_en && psta->htpriv.ampdu_enable) {
-				if (psta->htpriv.agg_enable_bitmap & CHKBIT(pattrib->priority))
-				pattrib->ampdu_en = true;
+				if (pattrib->priority >= 16)
+					printk(KERN_WARNING "%s: Invalid "
+					       "pattrib->priority %i\n",
+					       __func__, pattrib->priority);
+				if (psta->htpriv.agg_enable_bitmap &
+				    BIT(pattrib->priority))
+					pattrib->ampdu_en = true;
 			}
 			/* re-check if enable ampdu by BA_starting_seqctrl */
 			if (pattrib->ampdu_en) {
@@ -1037,10 +998,10 @@ s32 rtw_txframes_pending23a(struct rtw_adapter *padapter)
 {
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 
-	return (!_rtw_queue_empty23a(&pxmitpriv->be_pending)) ||
-	       (!_rtw_queue_empty23a(&pxmitpriv->bk_pending)) ||
-	       (!_rtw_queue_empty23a(&pxmitpriv->vi_pending)) ||
-	       (!_rtw_queue_empty23a(&pxmitpriv->vo_pending));
+	return (!list_empty(&pxmitpriv->be_pending.queue)) ||
+		(!list_empty(&pxmitpriv->bk_pending.queue)) ||
+		(!list_empty(&pxmitpriv->vi_pending.queue)) ||
+		(!list_empty(&pxmitpriv->vo_pending.queue));
 }
 
 s32 rtw_txframes_sta_ac_pending23a(struct rtw_adapter *padapter,
@@ -1098,7 +1059,7 @@ u32 rtw_calculate_wlan_pkt_size_by_attribue23a(struct pkt_attrib *pattrib)
 	len = pattrib->hdrlen + pattrib->iv_len; /*  WLAN Header and IV */
 	len += SNAP_SIZE + sizeof(u16); /*  LLC */
 	len += pattrib->pktlen;
-	if (pattrib->encrypt == _TKIP_) len += 8; /*  MIC */
+	if (pattrib->encrypt == WLAN_CIPHER_SUITE_TKIP) len += 8; /*  MIC */
 	len += ((pattrib->bswenc) ? pattrib->icv_len : 0); /*  ICV */
 
 	return len;
@@ -1116,39 +1077,41 @@ This sub-routine will perform all the following:
 6. apply sw-encrypt, if necessary.
 
 */
-s32 rtw_xmitframe_coalesce23a(struct rtw_adapter *padapter, struct sk_buff *pkt,
+int rtw_xmitframe_coalesce23a(struct rtw_adapter *padapter, struct sk_buff *skb,
 			      struct xmit_frame *pxmitframe)
 {
-	struct pkt_file pktfile;
-	struct sta_info		*psta;
-	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
-	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	struct sta_info *psta;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct pkt_attrib *pattrib = &pxmitframe->attrib;
+	struct ieee80211_hdr *hdr;
 	s32 frg_inx, frg_len, mpdu_len, llc_sz, mem_sz;
 	u8 *pframe, *mem_start;
 	u8 hw_hdr_offset;
 	u8 *pbuf_start;
-
+	u8 *pdata = skb->data;
+	int data_len = skb->len;
 	s32 bmcst = is_multicast_ether_addr(pattrib->ra);
-	s32 res = _SUCCESS;
+	int res = _SUCCESS;
 
-	if (pattrib->psta) {
+	if (pattrib->psta)
 		psta = pattrib->psta;
-	} else {
+	else {
 		DBG_8723A("%s, call rtw_get_stainfo23a()\n", __func__);
 		psta = rtw_get_stainfo23a(&padapter->stapriv, pattrib->ra);
 	}
 
-	if (psta == NULL) {
+	if (!psta) {
 		DBG_8723A("%s, psta == NUL\n", __func__);
 		return _FAIL;
 	}
 
 	if (!(psta->state &_FW_LINKED)) {
-		DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
+		DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n",
+			  __func__, psta->state);
 		return _FAIL;
 	}
 
-	if (pxmitframe->buf_addr == NULL) {
+	if (!pxmitframe->buf_addr) {
 		DBG_8723A("==> %s buf_addr == NULL\n", __func__);
 		return _FAIL;
 	}
@@ -1157,18 +1120,17 @@ s32 rtw_xmitframe_coalesce23a(struct rtw_adapter *padapter, struct sk_buff *pkt,
 
 	hw_hdr_offset = TXDESC_OFFSET;
 
-	mem_start = pbuf_start +	hw_hdr_offset;
+	mem_start = pbuf_start + hw_hdr_offset;
 
-	if (rtw_make_wlanhdr23a(padapter, mem_start, pattrib) == _FAIL) {
+	if (rtw_make_wlanhdr(padapter, mem_start, pattrib) == _FAIL) {
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
-			 ("rtw_xmitframe_coalesce23a: rtw_make_wlanhdr23a "
-			  "fail; drop pkt\n"));
+			 ("%s: rtw_make_wlanhdr fail; drop pkt\n", __func__));
 		res = _FAIL;
 		goto exit;
 	}
 
-	_rtw_open_pktfile23a(pkt, &pktfile);
-	_rtw_pktfile_read23a(&pktfile, NULL, pattrib->pkt_hdrlen);
+	pdata += pattrib->pkt_hdrlen;
+	data_len -= pattrib->pkt_hdrlen;
 
 	frg_inx = 0;
 	frg_len = pxmitpriv->frag_len - 4;/* 2346-4 = 2342 */
@@ -1179,31 +1141,37 @@ s32 rtw_xmitframe_coalesce23a(struct rtw_adapter *padapter, struct sk_buff *pkt,
 		mpdu_len = frg_len;
 
 		pframe = mem_start;
-
-		SetMFrag(mem_start);
+		hdr = (struct ieee80211_hdr *)mem_start;
 
 		pframe += pattrib->hdrlen;
 		mpdu_len -= pattrib->hdrlen;
 
 		/* adding icv, if necessary... */
 		if (pattrib->iv_len) {
-			if (psta != NULL) {
+			if (psta) {
 				switch (pattrib->encrypt) {
-				case _WEP40_:
-				case _WEP104_:
-					WEP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
+				case WLAN_CIPHER_SUITE_WEP40:
+				case WLAN_CIPHER_SUITE_WEP104:
+					WEP_IV(pattrib->iv, psta->dot11txpn,
+					       pattrib->key_idx);
 					break;
-				case _TKIP_:
+				case WLAN_CIPHER_SUITE_TKIP:
 					if (bmcst)
-						TKIP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
+						TKIP_IV(pattrib->iv,
+							psta->dot11txpn,
+							pattrib->key_idx);
 					else
-						TKIP_IV(pattrib->iv, psta->dot11txpn, 0);
+						TKIP_IV(pattrib->iv,
+							psta->dot11txpn, 0);
 					break;
-				case _AES_:
+				case WLAN_CIPHER_SUITE_CCMP:
 					if (bmcst)
-						AES_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
+						AES_IV(pattrib->iv,
+						       psta->dot11txpn,
+						       pattrib->key_idx);
 					else
-						AES_IV(pattrib->iv, psta->dot11txpn, 0);
+						AES_IV(pattrib->iv,
+						       psta->dot11txpn, 0);
 					break;
 				}
 			}
@@ -1225,16 +1193,20 @@ s32 rtw_xmitframe_coalesce23a(struct rtw_adapter *padapter, struct sk_buff *pkt,
 			mpdu_len -= llc_sz;
 		}
 
-		if ((pattrib->icv_len >0) && (pattrib->bswenc))
+		if (pattrib->icv_len > 0 && pattrib->bswenc)
 			mpdu_len -= pattrib->icv_len;
 
-		if (bmcst) {
+		if (bmcst)
 			/*  don't do fragment to broadcat/multicast packets */
-			mem_sz = _rtw_pktfile_read23a(&pktfile, pframe, pattrib->pktlen);
-		} else {
-			mem_sz = _rtw_pktfile_read23a(&pktfile, pframe, mpdu_len);
-		}
+			mem_sz = min_t(s32, data_len, pattrib->pktlen);
+		else
+			mem_sz = min_t(s32, data_len, mpdu_len);
+
+		memcpy(pframe, pdata, mem_sz);
+
 		pframe += mem_sz;
+		pdata += mem_sz;
+		data_len -= mem_sz;
 
 		if ((pattrib->icv_len >0) && (pattrib->bswenc)) {
 			memcpy(pframe, pattrib->icv, pattrib->icv_len);
@@ -1243,7 +1215,7 @@ s32 rtw_xmitframe_coalesce23a(struct rtw_adapter *padapter, struct sk_buff *pkt,
 
 		frg_inx++;
 
-		if (bmcst || (rtw_endofpktfile23a(&pktfile))) {
+		if (bmcst || data_len <= 0) {
 			pattrib->nr_frags = frg_inx;
 
 			pattrib->last_txcmdsz = pattrib->hdrlen +
@@ -1252,21 +1224,24 @@ s32 rtw_xmitframe_coalesce23a(struct rtw_adapter *padapter, struct sk_buff *pkt,
 						llc_sz : 0) +
 						((pattrib->bswenc) ?
 						pattrib->icv_len : 0) + mem_sz;
-
-			ClearMFrag(mem_start);
+			hdr->frame_control &=
+				~cpu_to_le16(IEEE80211_FCTL_MOREFRAGS);
 
 			break;
 		} else {
-			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("%s: There're still something in packet!\n", __func__));
+			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
+				 ("%s: There're still something in packet!\n",
+				  __func__));
 		}
+		hdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_MOREFRAGS);
 
 		mem_start = PTR_ALIGN(pframe, 4) + hw_hdr_offset;
 		memcpy(mem_start, pbuf_start + hw_hdr_offset, pattrib->hdrlen);
-
 	}
 
 	if (xmitframe_addmic(padapter, pxmitframe) == _FAIL) {
-		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("xmitframe_addmic(padapter, pxmitframe) == _FAIL\n"));
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
+			 ("xmitframe_addmic(padapter, pxmitframe) == _FAIL\n"));
 		DBG_8723A("xmitframe_addmic(padapter, pxmitframe) == _FAIL\n");
 		res = _FAIL;
 		goto exit;
@@ -1313,11 +1288,10 @@ s32 rtw_put_snap23a(u8 *data, u16 h_proto)
 
 void rtw_update_protection23a(struct rtw_adapter *padapter, u8 *ie, uint ie_len)
 {
-	struct	xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct	registry_priv *pregistrypriv = &padapter->registrypriv;
-	uint	protection;
-	u8	*perp;
-	int	 erp_len;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	uint protection;
+	const u8 *p;
 
 	switch (pxmitpriv->vcs_setting) {
 	case DISABLE_VCS:
@@ -1327,11 +1301,11 @@ void rtw_update_protection23a(struct rtw_adapter *padapter, u8 *ie, uint ie_len)
 		break;
 	case AUTO_VCS:
 	default:
-		perp = rtw_get_ie23a(ie, _ERPINFO_IE_, &erp_len, ie_len);
-		if (perp == NULL) {
+		p = cfg80211_find_ie(WLAN_EID_ERP_INFO, ie, ie_len);
+		if (!p)
 			pxmitpriv->vcs = NONE_VCS;
-		} else {
-			protection = (*(perp + 2)) & BIT(1);
+		else {
+			protection = (*(p + 2)) & BIT(1);
 			if (protection) {
 				if (pregistrypriv->vcs_type == RTS_CTS)
 					pxmitpriv->vcs = RTS_CTS;
@@ -1396,7 +1370,8 @@ struct xmit_buf *rtw_alloc_xmitbuf23a_ext(struct xmit_priv *pxmitpriv)
 	return pxmitbuf;
 }
 
-s32 rtw_free_xmitbuf_ext23a(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
+int rtw_free_xmitbuf_ext23a(struct xmit_priv *pxmitpriv,
+			    struct xmit_buf *pxmitbuf)
 {
 	unsigned long irqL;
 	struct rtw_queue *pfree_queue = &pxmitpriv->free_xmit_extbuf_queue;
@@ -1450,7 +1425,7 @@ struct xmit_buf *rtw_alloc_xmitbuf23a(struct xmit_priv *pxmitpriv)
 	return pxmitbuf;
 }
 
-s32 rtw_free_xmitbuf23a(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
+int rtw_free_xmitbuf23a(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
 {
 	unsigned long irqL;
 	struct rtw_queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue;
@@ -1512,22 +1487,18 @@ Otherwise, we must use _enter/_exit critical to protect free_xmit_queue...
 Must be very very cautious...
 
 */
-struct xmit_frame *rtw_alloc_xmitframe23a(struct xmit_priv *pxmitpriv)/* _queue *pfree_xmit_queue) */
+static struct xmit_frame *rtw_alloc_xmitframe(struct xmit_priv *pxmitpriv)
 {
-	/*
-		Please remember to use all the osdep_service api,
-		and lock/unlock or _enter/_exit critical to protect
-		pfree_xmit_queue
-	*/
-
 	struct xmit_frame *pxframe = NULL;
 	struct list_head *plist, *phead;
 	struct rtw_queue *pfree_xmit_queue = &pxmitpriv->free_xmit_queue;
 
 	spin_lock_bh(&pfree_xmit_queue->lock);
 
-	if (_rtw_queue_empty23a(pfree_xmit_queue) == true) {
-		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe23a:%d\n", pxmitpriv->free_xmitframe_cnt));
+	if (list_empty(&pfree_xmit_queue->queue)) {
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
+			 ("rtw_alloc_xmitframe:%d\n",
+			  pxmitpriv->free_xmitframe_cnt));
 		pxframe =  NULL;
 	} else {
 		phead = get_list_head(pfree_xmit_queue);
@@ -1538,7 +1509,9 @@ struct xmit_frame *rtw_alloc_xmitframe23a(struct xmit_priv *pxmitpriv)/* _queue
 
 		list_del_init(&pxframe->list);
 		pxmitpriv->free_xmitframe_cnt--;
-		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe23a():free_xmitframe_cnt =%d\n", pxmitpriv->free_xmitframe_cnt));
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
+			 ("rtw_alloc_xmitframe():free_xmitframe_cnt =%d\n",
+			  pxmitpriv->free_xmitframe_cnt));
 	}
 
 	spin_unlock_bh(&pfree_xmit_queue->lock);
@@ -1556,7 +1529,7 @@ struct xmit_frame *rtw_alloc_xmitframe23a_ext(struct xmit_priv *pxmitpriv)
 
 	spin_lock_bh(&queue->lock);
 
-	if (_rtw_queue_empty23a(queue) == true) {
+	if (list_empty(&queue->queue)) {
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe23a_ext:%d\n", pxmitpriv->free_xframe_ext_cnt));
 		pxframe =  NULL;
 	} else {
@@ -1642,7 +1615,7 @@ void rtw_free_xmitframe_queue23a(struct xmit_priv *pxmitpriv,
 
 }
 
-s32 rtw_xmitframe_enqueue23a(struct rtw_adapter *padapter,
+int rtw_xmitframe_enqueue23a(struct rtw_adapter *padapter,
 			     struct xmit_frame *pxmitframe)
 {
 	if (rtw_xmit23a_classifier(padapter, pxmitframe) == _FAIL) {
@@ -1715,7 +1688,9 @@ rtw_dequeue_xframe23a(struct xmit_priv *pxmitpriv, struct hw_xmit *phwxmit_i,
 				phwxmit->accnt--;
 
 				/* Remove sta node when there is no pending packets. */
-				if (_rtw_queue_empty23a(pframe_queue)) /* must be done after get_next and before break */
+				/* must be done after get_next and
+				   before break */
+				if (list_empty(&pframe_queue->queue))
 					list_del_init(&ptxservq->tx_pending);
 				goto exit;
 			}
@@ -1764,8 +1739,8 @@ struct tx_servq *rtw_get_sta_pending23a(struct rtw_adapter *padapter, struct sta
  * Will enqueue pxmitframe to the proper queue,
  * and indicate it to xx_pending list.....
  */
-s32 rtw_xmit23a_classifier(struct rtw_adapter *padapter,
-			struct xmit_frame *pxmitframe)
+int rtw_xmit23a_classifier(struct rtw_adapter *padapter,
+			   struct xmit_frame *pxmitframe)
 {
 	struct sta_info	*psta;
 	struct tx_servq	*ptxservq;
@@ -1942,9 +1917,9 @@ int rtw_xmit23a(struct rtw_adapter *padapter, struct sk_buff *skb)
 {
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct xmit_frame *pxmitframe = NULL;
-	s32 res;
+	int res;
 
-	pxmitframe = rtw_alloc_xmitframe23a(pxmitpriv);
+	pxmitframe = rtw_alloc_xmitframe(pxmitpriv);
 
 	if (pxmitframe == NULL) {
 		RT_TRACE(_module_xmit_osdep_c_, _drv_err_,
@@ -1974,7 +1949,7 @@ int rtw_xmit23a(struct rtw_adapter *padapter, struct sk_buff *skb)
 	spin_unlock_bh(&pxmitpriv->lock);
 #endif
 
-	if (rtw_hal_xmit23a(padapter, pxmitframe) == false)
+	if (rtl8723au_hal_xmit(padapter, pxmitframe) == false)
 		return 1;
 
 	return 0;
@@ -1991,8 +1966,8 @@ int xmitframe_enqueue_for_sleeping_sta23a(struct rtw_adapter *padapter, struct x
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	int bmcst = is_multicast_ether_addr(pattrib->ra);
 
-	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == false)
-	    return ret;
+	if (!check_fwstate(pmlmepriv, WIFI_AP_STATE))
+		return ret;
 
 	if (pattrib->psta) {
 		psta = pattrib->psta;
@@ -2036,7 +2011,8 @@ int xmitframe_enqueue_for_sleeping_sta23a(struct rtw_adapter *padapter, struct x
 
 			/* DBG_8723A("enqueue, sq_len =%d, tim =%x\n", psta->sleepq_len, pstapriv->tim_bitmap); */
 
-			update_beacon23a(padapter, _TIM_IE_, NULL, false);/* tx bc/mc packets after upate bcn */
+			/* tx bc/mc packets after upate bcn */
+			update_beacon23a(padapter, WLAN_EID_TIM, NULL, false);
 
 			/* spin_unlock_bh(&psta->sleep_q.lock); */
 
@@ -2093,7 +2069,8 @@ int xmitframe_enqueue_for_sleeping_sta23a(struct rtw_adapter *padapter, struct x
 
 				if (psta->sleepq_len == 1) {
 					/* upate BCN for TIM IE */
-					update_beacon23a(padapter, _TIM_IE_, NULL, false);
+					update_beacon23a(padapter, WLAN_EID_TIM,
+							 NULL, false);
 				}
 			}
 
@@ -2244,7 +2221,7 @@ void wakeup_sta_to_xmit23a(struct rtw_adapter *padapter, struct sta_info *psta)
 		}
 
 		pxmitframe->attrib.triggered = 1;
-		rtw_hal_xmit23aframe_enqueue(padapter, pxmitframe);
+		rtl8723au_hal_xmitframe_enqueue(padapter, pxmitframe);
 	}
 
 	if (psta->sleepq_len == 0) {
@@ -2291,7 +2268,7 @@ void wakeup_sta_to_xmit23a(struct rtw_adapter *padapter, struct sta_info *psta)
 				pxmitframe->attrib.mdata = 0;
 
 			pxmitframe->attrib.triggered = 1;
-			rtw_hal_xmit23aframe_enqueue(padapter, pxmitframe);
+			rtl8723au_hal_xmitframe_enqueue(padapter, pxmitframe);
 		}
 		if (psta_bmc->sleepq_len == 0) {
 			pstapriv->tim_bitmap &= ~BIT(0);
@@ -2307,7 +2284,7 @@ void wakeup_sta_to_xmit23a(struct rtw_adapter *padapter, struct sta_info *psta)
 	}
 
 	if (update_mask)
-		update_beacon23a(padapter, _TIM_IE_, NULL, false);
+		update_beacon23a(padapter, WLAN_EID_TIM, NULL, false);
 }
 
 void xmit_delivery_enabled_frames23a(struct rtw_adapter *padapter,
@@ -2365,14 +2342,14 @@ void xmit_delivery_enabled_frames23a(struct rtw_adapter *padapter,
 
 		pxmitframe->attrib.triggered = 1;
 
-		rtw_hal_xmit23aframe_enqueue(padapter, pxmitframe);
+		rtl8723au_hal_xmitframe_enqueue(padapter, pxmitframe);
 
 		if ((psta->sleepq_ac_len == 0) && (!psta->has_legacy_ac) &&
 		    (wmmps_ac)) {
 			pstapriv->tim_bitmap &= ~CHKBIT(psta->aid);
 
 			/* upate BCN for TIM IE */
-			update_beacon23a(padapter, _TIM_IE_, NULL, false);
+			update_beacon23a(padapter, WLAN_EID_TIM, NULL, false);
 		}
 	}
 	spin_unlock_bh(&pxmitpriv->lock);
diff --git a/drivers/staging/rtl8723au/hal/HalDMOutSrc8723A_CE.c b/drivers/staging/rtl8723au/hal/HalDMOutSrc8723A_CE.c
index 9796f2e5c68..4b41bc4ce1e 100644
--- a/drivers/staging/rtl8723au/hal/HalDMOutSrc8723A_CE.c
+++ b/drivers/staging/rtl8723au/hal/HalDMOutSrc8723A_CE.c
@@ -18,6 +18,7 @@
 /*  include files */
 
 #include "odm_precomp.h"
+#include <usb_ops_linux.h>
 
 #define		DPK_DELTA_MAPPING_NUM	13
 #define		index_mapping_HP_NUM	15
@@ -252,35 +253,41 @@ odm_TXPowerTrackingCallback_ThermalMeter_92C(
 				PHY_SetBBReg(Adapter, rOFDM0_XCTxAFE, bMaskH4Bits, value32);
 
 				value32 = ((X * ele_D)>>7)&0x01;
-				PHY_SetBBReg(Adapter, rOFDM0_ECCAThreshold, BIT31, value32);
+				PHY_SetBBReg(Adapter, rOFDM0_ECCAThreshold,
+					     BIT(31), value32);
 
 				value32 = ((Y * ele_D)>>7)&0x01;
-				PHY_SetBBReg(Adapter, rOFDM0_ECCAThreshold, BIT29, value32);
+				PHY_SetBBReg(Adapter, rOFDM0_ECCAThreshold,
+					     BIT(29), value32);
 			} else {
-				PHY_SetBBReg(Adapter, rOFDM0_XATxIQImbalance, bMaskDWord, OFDMSwingTable23A[OFDM_index[0]]);
-				PHY_SetBBReg(Adapter, rOFDM0_XCTxAFE, bMaskH4Bits, 0x00);
-				PHY_SetBBReg(Adapter, rOFDM0_ECCAThreshold, BIT31|BIT29, 0x00);
+				PHY_SetBBReg(Adapter, rOFDM0_XATxIQImbalance,
+					     bMaskDWord,
+					     OFDMSwingTable23A[OFDM_index[0]]);
+				PHY_SetBBReg(Adapter, rOFDM0_XCTxAFE,
+					     bMaskH4Bits, 0x00);
+				PHY_SetBBReg(Adapter, rOFDM0_ECCAThreshold,
+					     BIT(31) | BIT(29), 0x00);
 			}
 
 			/* Adjust CCK according to IQK result */
 			if (!pdmpriv->bCCKinCH14) {
-				rtw_write8(Adapter, 0xa22, CCKSwingTable_Ch1_Ch1323A[CCK_index][0]);
-				rtw_write8(Adapter, 0xa23, CCKSwingTable_Ch1_Ch1323A[CCK_index][1]);
-				rtw_write8(Adapter, 0xa24, CCKSwingTable_Ch1_Ch1323A[CCK_index][2]);
-				rtw_write8(Adapter, 0xa25, CCKSwingTable_Ch1_Ch1323A[CCK_index][3]);
-				rtw_write8(Adapter, 0xa26, CCKSwingTable_Ch1_Ch1323A[CCK_index][4]);
-				rtw_write8(Adapter, 0xa27, CCKSwingTable_Ch1_Ch1323A[CCK_index][5]);
-				rtw_write8(Adapter, 0xa28, CCKSwingTable_Ch1_Ch1323A[CCK_index][6]);
-				rtw_write8(Adapter, 0xa29, CCKSwingTable_Ch1_Ch1323A[CCK_index][7]);
+				rtl8723au_write8(Adapter, 0xa22, CCKSwingTable_Ch1_Ch1323A[CCK_index][0]);
+				rtl8723au_write8(Adapter, 0xa23, CCKSwingTable_Ch1_Ch1323A[CCK_index][1]);
+				rtl8723au_write8(Adapter, 0xa24, CCKSwingTable_Ch1_Ch1323A[CCK_index][2]);
+				rtl8723au_write8(Adapter, 0xa25, CCKSwingTable_Ch1_Ch1323A[CCK_index][3]);
+				rtl8723au_write8(Adapter, 0xa26, CCKSwingTable_Ch1_Ch1323A[CCK_index][4]);
+				rtl8723au_write8(Adapter, 0xa27, CCKSwingTable_Ch1_Ch1323A[CCK_index][5]);
+				rtl8723au_write8(Adapter, 0xa28, CCKSwingTable_Ch1_Ch1323A[CCK_index][6]);
+				rtl8723au_write8(Adapter, 0xa29, CCKSwingTable_Ch1_Ch1323A[CCK_index][7]);
 			} else {
-				rtw_write8(Adapter, 0xa22, CCKSwingTable_Ch1423A[CCK_index][0]);
-				rtw_write8(Adapter, 0xa23, CCKSwingTable_Ch1423A[CCK_index][1]);
-				rtw_write8(Adapter, 0xa24, CCKSwingTable_Ch1423A[CCK_index][2]);
-				rtw_write8(Adapter, 0xa25, CCKSwingTable_Ch1423A[CCK_index][3]);
-				rtw_write8(Adapter, 0xa26, CCKSwingTable_Ch1423A[CCK_index][4]);
-				rtw_write8(Adapter, 0xa27, CCKSwingTable_Ch1423A[CCK_index][5]);
-				rtw_write8(Adapter, 0xa28, CCKSwingTable_Ch1423A[CCK_index][6]);
-				rtw_write8(Adapter, 0xa29, CCKSwingTable_Ch1423A[CCK_index][7]);
+				rtl8723au_write8(Adapter, 0xa22, CCKSwingTable_Ch1423A[CCK_index][0]);
+				rtl8723au_write8(Adapter, 0xa23, CCKSwingTable_Ch1423A[CCK_index][1]);
+				rtl8723au_write8(Adapter, 0xa24, CCKSwingTable_Ch1423A[CCK_index][2]);
+				rtl8723au_write8(Adapter, 0xa25, CCKSwingTable_Ch1423A[CCK_index][3]);
+				rtl8723au_write8(Adapter, 0xa26, CCKSwingTable_Ch1423A[CCK_index][4]);
+				rtl8723au_write8(Adapter, 0xa27, CCKSwingTable_Ch1423A[CCK_index][5]);
+				rtl8723au_write8(Adapter, 0xa28, CCKSwingTable_Ch1423A[CCK_index][6]);
+				rtl8723au_write8(Adapter, 0xa29, CCKSwingTable_Ch1423A[CCK_index][7]);
 			}
 
 			if (is2T) {
@@ -308,14 +315,25 @@ odm_TXPowerTrackingCallback_ThermalMeter_92C(
 					PHY_SetBBReg(Adapter, rOFDM0_XDTxAFE, bMaskH4Bits, value32);
 
 					value32 = ((X * ele_D)>>7)&0x01;
-					PHY_SetBBReg(Adapter, rOFDM0_ECCAThreshold, BIT27, value32);
+					PHY_SetBBReg(Adapter,
+						     rOFDM0_ECCAThreshold,
+						     BIT(27), value32);
 
 					value32 = ((Y * ele_D)>>7)&0x01;
-					PHY_SetBBReg(Adapter, rOFDM0_ECCAThreshold, BIT25, value32);
+					PHY_SetBBReg(Adapter,
+						     rOFDM0_ECCAThreshold,
+						     BIT(25), value32);
 				} else {
-					PHY_SetBBReg(Adapter, rOFDM0_XBTxIQImbalance, bMaskDWord, OFDMSwingTable23A[OFDM_index[1]]);
-					PHY_SetBBReg(Adapter, rOFDM0_XDTxAFE, bMaskH4Bits, 0x00);
-					PHY_SetBBReg(Adapter, rOFDM0_ECCAThreshold, BIT27|BIT25, 0x00);
+					PHY_SetBBReg(Adapter,
+						     rOFDM0_XBTxIQImbalance,
+						     bMaskDWord,
+						     OFDMSwingTable23A[OFDM_index[1]]);
+					PHY_SetBBReg(Adapter,
+						     rOFDM0_XDTxAFE,
+						     bMaskH4Bits, 0x00);
+					PHY_SetBBReg(Adapter,
+						     rOFDM0_ECCAThreshold,
+						     BIT(27) | BIT(25), 0x00);
 				}
 			}
 
@@ -410,14 +428,14 @@ static u8 _PHY_PathA_IQK(struct rtw_adapter *pAdapter, bool configPathB)
 	regE9C = PHY_QueryBBReg(pAdapter, rTx_Power_After_IQK_A, bMaskDWord);
 	regEA4 = PHY_QueryBBReg(pAdapter, rRx_Power_Before_IQK_A_2, bMaskDWord);
 
-	if (!(regEAC & BIT28) &&
+	if (!(regEAC & BIT(28)) &&
 	    (((regE94 & 0x03FF0000)>>16) != 0x142) &&
 	    (((regE9C & 0x03FF0000)>>16) != 0x42))
 		result |= 0x01;
 	else			/* if Tx not OK, ignore Rx */
 		return result;
 
-	if (!(regEAC & BIT27) &&		/* if Tx is OK, check whether Rx is OK */
+	if (!(regEAC & BIT(27)) &&		/* if Tx is OK, check whether Rx is OK */
 	    (((regEA4 & 0x03FF0000)>>16) != 0x132) &&
 	    (((regEAC & 0x03FF0000)>>16) != 0x36))
 		result |= 0x02;
@@ -445,14 +463,14 @@ static u8 _PHY_PathB_IQK(struct rtw_adapter *pAdapter)
 	regEC4 = PHY_QueryBBReg(pAdapter, rRx_Power_Before_IQK_B_2, bMaskDWord);
 	regECC = PHY_QueryBBReg(pAdapter, rRx_Power_After_IQK_B_2, bMaskDWord);
 
-	if (!(regEAC & BIT31) &&
+	if (!(regEAC & BIT(31)) &&
 	    (((regEB4 & 0x03FF0000)>>16) != 0x142) &&
 	    (((regEBC & 0x03FF0000)>>16) != 0x42))
 		result |= 0x01;
 	else
 		return result;
 
-	if (!(regEAC & BIT30) &&
+	if (!(regEAC & BIT(30)) &&
 	    (((regEC4 & 0x03FF0000)>>16) != 0x132) &&
 	    (((regECC & 0x03FF0000)>>16) != 0x36))
 		result |= 0x02;
@@ -564,9 +582,9 @@ static void _PHY_SaveMACRegisters(struct rtw_adapter *pAdapter, u32 *MACReg, u32
 	u32 i;
 
 	for (i = 0 ; i < (IQK_MAC_REG_NUM - 1); i++) {
-		MACBackup[i] = rtw_read8(pAdapter, MACReg[i]);
+		MACBackup[i] = rtl8723au_read8(pAdapter, MACReg[i]);
 	}
-	MACBackup[i] = rtw_read32(pAdapter, MACReg[i]);
+	MACBackup[i] = rtl8723au_read32(pAdapter, MACReg[i]);
 }
 
 static void _PHY_ReloadADDARegisters(struct rtw_adapter *pAdapter, u32 *ADDAReg, u32 *ADDABackup, u32 RegiesterNum)
@@ -582,10 +600,10 @@ static void _PHY_ReloadMACRegisters(struct rtw_adapter *pAdapter, u32 *MACReg, u
 {
 	u32 i;
 
-	for (i = 0 ; i < (IQK_MAC_REG_NUM - 1); i++) {
-		rtw_write8(pAdapter, MACReg[i], (u8)MACBackup[i]);
-	}
-	rtw_write32(pAdapter, MACReg[i], MACBackup[i]);
+	for (i = 0 ; i < (IQK_MAC_REG_NUM - 1); i++)
+		rtl8723au_write8(pAdapter, MACReg[i], (u8)MACBackup[i]);
+
+	rtl8723au_write32(pAdapter, MACReg[i], MACBackup[i]);
 }
 
 static void _PHY_PathADDAOn(struct rtw_adapter *pAdapter, u32 *ADDAReg, bool isPathAOn, bool is2T)
@@ -609,12 +627,13 @@ static void _PHY_MACSettingCalibration(struct rtw_adapter *pAdapter, u32 *MACReg
 {
 	u32 i = 0;
 
-	rtw_write8(pAdapter, MACReg[i], 0x3F);
+	rtl8723au_write8(pAdapter, MACReg[i], 0x3F);
 
 	for (i = 1 ; i < (IQK_MAC_REG_NUM - 1); i++) {
-		rtw_write8(pAdapter, MACReg[i], (u8)(MACBackup[i]&(~BIT3)));
+		rtl8723au_write8(pAdapter, MACReg[i],
+				 (u8)(MACBackup[i] & ~BIT(3)));
 	}
-	rtw_write8(pAdapter, MACReg[i], (u8)(MACBackup[i]&(~BIT5)));
+	rtl8723au_write8(pAdapter, MACReg[i], (u8)(MACBackup[i] & ~BIT(5)));
 }
 
 static void _PHY_PathAStandBy(struct rtw_adapter *pAdapter)
@@ -737,21 +756,23 @@ static void _PHY_IQCalibrate(struct rtw_adapter *pAdapter, int result[][8], u8 t
 	_PHY_PathADDAOn(pAdapter, ADDA_REG, true, is2T);
 
 	if (t == 0)
-		pdmpriv->bRfPiEnable = (u8)PHY_QueryBBReg(pAdapter, rFPGA0_XA_HSSIParameter1, BIT(8));
+		pdmpriv->bRfPiEnable = (u8)
+			PHY_QueryBBReg(pAdapter, rFPGA0_XA_HSSIParameter1,
+				       BIT(8));
 
 	if (!pdmpriv->bRfPiEnable) {
 		/*  Switch BB to PI mode to do IQ Calibration. */
 		_PHY_PIModeSwitch(pAdapter, true);
 	}
 
-	PHY_SetBBReg(pAdapter, rFPGA0_RFMOD, BIT24, 0x00);
+	PHY_SetBBReg(pAdapter, rFPGA0_RFMOD, BIT(24), 0x00);
 	PHY_SetBBReg(pAdapter, rOFDM0_TRxPathEnable, bMaskDWord, 0x03a05600);
 	PHY_SetBBReg(pAdapter, rOFDM0_TRMuxPar, bMaskDWord, 0x000800e4);
 	PHY_SetBBReg(pAdapter, rFPGA0_XCD_RFInterfaceSW, bMaskDWord, 0x22204000);
-	PHY_SetBBReg(pAdapter, rFPGA0_XAB_RFInterfaceSW, BIT10, 0x01);
-	PHY_SetBBReg(pAdapter, rFPGA0_XAB_RFInterfaceSW, BIT26, 0x01);
-	PHY_SetBBReg(pAdapter, rFPGA0_XA_RFInterfaceOE, BIT10, 0x00);
-	PHY_SetBBReg(pAdapter, rFPGA0_XB_RFInterfaceOE, BIT10, 0x00);
+	PHY_SetBBReg(pAdapter, rFPGA0_XAB_RFInterfaceSW, BIT(10), 0x01);
+	PHY_SetBBReg(pAdapter, rFPGA0_XAB_RFInterfaceSW, BIT(26), 0x01);
+	PHY_SetBBReg(pAdapter, rFPGA0_XA_RFInterfaceOE, BIT(10), 0x00);
+	PHY_SetBBReg(pAdapter, rFPGA0_XB_RFInterfaceOE, BIT(10), 0x00);
 
 	if (is2T) {
 		PHY_SetBBReg(pAdapter, rFPGA0_XA_LSSIParameter, bMaskDWord, 0x00010000);
@@ -859,12 +880,17 @@ static void _PHY_LCCalibrate(struct rtw_adapter *pAdapter, bool is2T)
 	u32 RF_Amode = 0, RF_Bmode = 0, LC_Cal;
 
 	/* Check continuous TX and Packet TX */
-	tmpReg = rtw_read8(pAdapter, 0xd03);
+	tmpReg = rtl8723au_read8(pAdapter, 0xd03);
 
-	if ((tmpReg&0x70) != 0)			/* Deal with contisuous TX case */
-		rtw_write8(pAdapter, 0xd03, tmpReg&0x8F);	/* disable all continuous TX */
-	else							/*  Deal with Packet TX case */
-		rtw_write8(pAdapter, REG_TXPAUSE, 0xFF);			/*  block all queues */
+	if ((tmpReg&0x70) != 0) {
+		/* Deal with contisuous TX case */
+		/* disable all continuous TX */
+		rtl8723au_write8(pAdapter, 0xd03, tmpReg&0x8F);
+	} else {
+		/*  Deal with Packet TX case */
+		/*  block all queues */
+		rtl8723au_write8(pAdapter, REG_TXPAUSE, 0xFF);
+	}
 
 	if ((tmpReg&0x70) != 0) {
 		/* 1. Read original RF mode */
@@ -895,15 +921,14 @@ static void _PHY_LCCalibrate(struct rtw_adapter *pAdapter, bool is2T)
 	/* Restore original situation */
 	if ((tmpReg&0x70) != 0) {	/* Deal with contuous TX case  */
 		/* Path-A */
-		rtw_write8(pAdapter, 0xd03, tmpReg);
+		rtl8723au_write8(pAdapter, 0xd03, tmpReg);
 		PHY_SetRFReg(pAdapter, RF_PATH_A, RF_AC, bMask12Bits, RF_Amode);
 
 		/* Path-B */
 		if (is2T)
 			PHY_SetRFReg(pAdapter, RF_PATH_B, RF_AC, bMask12Bits, RF_Bmode);
-	} else { /*  Deal with Packet TX case */
-		rtw_write8(pAdapter, REG_TXPAUSE, 0x00);
-	}
+	} else /*  Deal with Packet TX case */
+		rtl8723au_write8(pAdapter, REG_TXPAUSE, 0x00);
 }
 
 /* Analog Pre-distortion calibration */
diff --git a/drivers/staging/rtl8723au/hal/HalPwrSeqCmd.c b/drivers/staging/rtl8723au/hal/HalPwrSeqCmd.c
index 4f6b4b72f92..1da4eece6f9 100644
--- a/drivers/staging/rtl8723au/hal/HalPwrSeqCmd.c
+++ b/drivers/staging/rtl8723au/hal/HalPwrSeqCmd.c
@@ -30,6 +30,7 @@ Major Change History:
 
 --*/
 #include <HalPwrSeqCmd.h>
+#include <usb_ops_linux.h>
 
 /*  */
 /*	Description: */
@@ -89,14 +90,14 @@ u8 HalPwrSeqCmdParsing23a(struct rtw_adapter *padapter, u8 CutVersion,
 				offset = GET_PWR_CFG_OFFSET(PwrCfgCmd);
 
 				/*  Read the value from system register */
-				value = rtw_read8(padapter, offset);
+				value = rtl8723au_read8(padapter, offset);
 
 				value &= ~(GET_PWR_CFG_MASK(PwrCfgCmd));
 				value |= (GET_PWR_CFG_VALUE(PwrCfgCmd) &
 					  GET_PWR_CFG_MASK(PwrCfgCmd));
 
 				/*  Write the value back to sytem register */
-				rtw_write8(padapter, offset, value);
+				rtl8723au_write8(padapter, offset, value);
 				break;
 
 			case PWR_CMD_POLLING:
@@ -107,7 +108,8 @@ u8 HalPwrSeqCmdParsing23a(struct rtw_adapter *padapter, u8 CutVersion,
 				bPollingBit = false;
 				offset = GET_PWR_CFG_OFFSET(PwrCfgCmd);
 				do {
-					value = rtw_read8(padapter, offset);
+					value = rtl8723au_read8(padapter,
+								offset);
 
 					value &= GET_PWR_CFG_MASK(PwrCfgCmd);
 					if (value ==
diff --git a/drivers/staging/rtl8723au/hal/hal_com.c b/drivers/staging/rtl8723au/hal/hal_com.c
index 0640f352213..9fba0494515 100644
--- a/drivers/staging/rtl8723au/hal/hal_com.c
+++ b/drivers/staging/rtl8723au/hal/hal_com.c
@@ -18,6 +18,7 @@
 #include <hal_intf.h>
 #include <hal_com.h>
 #include <rtl8723a_hal.h>
+#include <usb_ops_linux.h>
 
 #define _HAL_INIT_C_
 
@@ -222,10 +223,10 @@ void HalSetBrateCfg23a(struct rtw_adapter *padapter, u8 *mBratesOS)
 	DBG_8723A("HW_VAR_BASIC_RATE: BrateCfg(%#x)\n", brate_cfg);
 
 	/*  Set RRSR rate table. */
-	rtw_write8(padapter, REG_RRSR, brate_cfg & 0xff);
-	rtw_write8(padapter, REG_RRSR + 1, (brate_cfg >> 8) & 0xff);
-	rtw_write8(padapter, REG_RRSR + 2,
-		   rtw_read8(padapter, REG_RRSR + 2) & 0xf0);
+	rtl8723au_write8(padapter, REG_RRSR, brate_cfg & 0xff);
+	rtl8723au_write8(padapter, REG_RRSR + 1, (brate_cfg >> 8) & 0xff);
+	rtl8723au_write8(padapter, REG_RRSR + 2,
+			 rtl8723au_read8(padapter, REG_RRSR + 2) & 0xf0);
 
 	rate_index = 0;
 	/*  Set RTS initial rate */
@@ -234,7 +235,7 @@ void HalSetBrateCfg23a(struct rtw_adapter *padapter, u8 *mBratesOS)
 		rate_index++;
 	}
 		/*  Ziv - Check */
-	rtw_write8(padapter, REG_INIRTS_RATE_SEL, rate_index);
+	rtl8723au_write8(padapter, REG_INIRTS_RATE_SEL, rate_index);
 
 	return;
 }
@@ -344,12 +345,6 @@ bool Hal_MappingOutPipe23a(struct rtw_adapter *pAdapter, u8 NumOutPipe)
 	return result;
 }
 
-void hal_init_macaddr23a(struct rtw_adapter *adapter)
-{
-	rtw_hal_set_hwreg23a(adapter, HW_VAR_MAC_ADDR,
-			  adapter->eeprompriv.mac_addr);
-}
-
 /*
 * C2H event format:
 * Field	 TRIGGER		CONTENT	   CMD_SEQ	CMD_LEN		 CMD_ID
@@ -358,12 +353,12 @@ void hal_init_macaddr23a(struct rtw_adapter *adapter)
 
 void c2h_evt_clear23a(struct rtw_adapter *adapter)
 {
-	rtw_write8(adapter, REG_C2HEVT_CLEAR, C2H_EVT_HOST_CLOSE);
+	rtl8723au_write8(adapter, REG_C2HEVT_CLEAR, C2H_EVT_HOST_CLOSE);
 }
 
-s32 c2h_evt_read23a(struct rtw_adapter *adapter, u8 *buf)
+int c2h_evt_read23a(struct rtw_adapter *adapter, u8 *buf)
 {
-	s32 ret = _FAIL;
+	int ret = _FAIL;
 	struct c2h_evt_hdr *c2h_evt;
 	int i;
 	u8 trigger;
@@ -371,7 +366,7 @@ s32 c2h_evt_read23a(struct rtw_adapter *adapter, u8 *buf)
 	if (buf == NULL)
 		goto exit;
 
-	trigger = rtw_read8(adapter, REG_C2HEVT_CLEAR);
+	trigger = rtl8723au_read8(adapter, REG_C2HEVT_CLEAR);
 
 	if (trigger == C2H_EVT_HOST_CLOSE)
 		goto exit;	/* Not ready */
@@ -382,8 +377,8 @@ s32 c2h_evt_read23a(struct rtw_adapter *adapter, u8 *buf)
 
 	memset(c2h_evt, 0, 16);
 
-	*buf = rtw_read8(adapter, REG_C2HEVT_MSG_NORMAL);
-	*(buf + 1) = rtw_read8(adapter, REG_C2HEVT_MSG_NORMAL + 1);
+	*buf = rtl8723au_read8(adapter, REG_C2HEVT_MSG_NORMAL);
+	*(buf + 1) = rtl8723au_read8(adapter, REG_C2HEVT_MSG_NORMAL + 1);
 
 	RT_PRINT_DATA(_module_hal_init_c_, _drv_info_, "c2h_evt_read23a(): ",
 		      &c2h_evt, sizeof(c2h_evt));
@@ -396,7 +391,7 @@ s32 c2h_evt_read23a(struct rtw_adapter *adapter, u8 *buf)
 
 	/* Read the content */
 	for (i = 0; i < c2h_evt->plen; i++)
-		c2h_evt->payload[i] = rtw_read8(adapter,
+		c2h_evt->payload[i] = rtl8723au_read8(adapter,
 						REG_C2HEVT_MSG_NORMAL +
 						sizeof(*c2h_evt) + i);
 
@@ -424,15 +419,14 @@ rtl8723a_set_ampdu_min_space(struct rtw_adapter *padapter, u8 MinSpacingToSet)
 
 	if (MinSpacingToSet <= 7) {
 		switch (padapter->securitypriv.dot11PrivacyAlgrthm) {
-		case _NO_PRIVACY_:
-		case _AES_:
+		case 0:
+		case WLAN_CIPHER_SUITE_CCMP:
 			SecMinSpace = 0;
 			break;
 
-		case _WEP40_:
-		case _WEP104_:
-		case _TKIP_:
-		case _TKIP_WTMIC_:
+		case WLAN_CIPHER_SUITE_WEP40:
+		case WLAN_CIPHER_SUITE_WEP104:
+		case WLAN_CIPHER_SUITE_TKIP:
 			SecMinSpace = 6;
 			break;
 		default:
@@ -447,9 +441,9 @@ rtl8723a_set_ampdu_min_space(struct rtw_adapter *padapter, u8 MinSpacingToSet)
 		   ("Set HW_VAR_AMPDU_MIN_SPACE: %#x\n",
 		   padapter->MgntInfo.MinSpaceCfg)); */
 		MinSpacingToSet |=
-			rtw_read8(padapter, REG_AMPDU_MIN_SPACE) & 0xf8;
-		rtw_write8(padapter, REG_AMPDU_MIN_SPACE,
-			   MinSpacingToSet);
+			rtl8723au_read8(padapter, REG_AMPDU_MIN_SPACE) & 0xf8;
+		rtl8723au_write8(padapter, REG_AMPDU_MIN_SPACE,
+				 MinSpacingToSet);
 	}
 }
 
@@ -461,15 +455,12 @@ void rtl8723a_set_ampdu_factor(struct rtw_adapter *padapter, u8 FactorToSet)
 	u8 index = 0;
 
 	pRegToSet = RegToSet_Normal;	/*  0xb972a841; */
-#ifdef CONFIG_8723AU_BT_COEXIST
-	if ((BT_IsBtDisabled(padapter) == false) &&
-	    (BT_1Ant(padapter) == true)) {
+
+	if (rtl8723a_BT_enabled(padapter) &&
+	    rtl8723a_BT_using_antenna_1(padapter))
 		MaxAggNum = 0x8;
-	} else
-#endif /*  CONFIG_8723AU_BT_COEXIST */
-	{
+	else
 		MaxAggNum = 0xF;
-	}
 
 	if (FactorToSet <= 3) {
 		FactorToSet = (1 << (FactorToSet + 2));
@@ -485,8 +476,8 @@ void rtl8723a_set_ampdu_factor(struct rtw_adapter *padapter, u8 FactorToSet)
 				pRegToSet[index] = (pRegToSet[index] & 0xf0) |
 					FactorToSet;
 
-			rtw_write8(padapter, REG_AGGLEN_LMT + index,
-				   pRegToSet[index]);
+			rtl8723au_write8(padapter, REG_AGGLEN_LMT + index,
+					 pRegToSet[index]);
 		}
 
 		/* RT_TRACE(COMP_MLME, DBG_LOUD,
@@ -512,25 +503,25 @@ void rtl8723a_set_acm_ctrl(struct rtw_adapter *padapter, u8 ctrl)
 	}
 
 	DBG_8723A("[HW_VAR_ACM_CTRL] Write 0x%02X\n", hwctrl);
-	rtw_write8(padapter, REG_ACMHWCTRL, hwctrl);
+	rtl8723au_write8(padapter, REG_ACMHWCTRL, hwctrl);
 }
 
 void rtl8723a_set_media_status(struct rtw_adapter *padapter, u8 status)
 {
 	u8 val8;
 
-	val8 = rtw_read8(padapter, MSR) & 0x0c;
+	val8 = rtl8723au_read8(padapter, MSR) & 0x0c;
 	val8 |= status;
-	rtw_write8(padapter, MSR, val8);
+	rtl8723au_write8(padapter, MSR, val8);
 }
 
 void rtl8723a_set_media_status1(struct rtw_adapter *padapter, u8 status)
 {
 	u8 val8;
 
-	val8 = rtw_read8(padapter, MSR) & 0x03;
+	val8 = rtl8723au_read8(padapter, MSR) & 0x03;
 	val8 |= status << 2;
-	rtw_write8(padapter, MSR, val8);
+	rtl8723au_write8(padapter, MSR, val8);
 }
 
 void rtl8723a_set_bcn_func(struct rtw_adapter *padapter, u8 val)
@@ -544,12 +535,12 @@ void rtl8723a_set_bcn_func(struct rtw_adapter *padapter, u8 val)
 void rtl8723a_check_bssid(struct rtw_adapter *padapter, u8 val)
 {
 	u32 val32;
-	val32 = rtw_read32(padapter, REG_RCR);
+	val32 = rtl8723au_read32(padapter, REG_RCR);
 	if (val)
 		val32 |= RCR_CBSSID_DATA | RCR_CBSSID_BCN;
 	else
 		val32 &= ~(RCR_CBSSID_DATA | RCR_CBSSID_BCN);
-	rtw_write32(padapter, REG_RCR, val32);
+	rtl8723au_write32(padapter, REG_RCR, val32);
 }
 
 void rtl8723a_mlme_sitesurvey(struct rtw_adapter *padapter, u8 flag)
@@ -559,11 +550,11 @@ void rtl8723a_mlme_sitesurvey(struct rtw_adapter *padapter, u8 flag)
 
 		/*  config RCR to receive different BSSID & not
 		    to receive data frame */
-		v32 = rtw_read32(padapter, REG_RCR);
+		v32 = rtl8723au_read32(padapter, REG_RCR);
 		v32 &= ~(RCR_CBSSID_BCN);
-		rtw_write32(padapter, REG_RCR, v32);
+		rtl8723au_write32(padapter, REG_RCR, v32);
 		/*  reject all data frame */
-		rtw_write16(padapter, REG_RXFLTMAP2, 0);
+		rtl8723au_write16(padapter, REG_RXFLTMAP2, 0);
 
 		/*  disable update TSF */
 		SetBcnCtrlReg23a(padapter, DIS_TSF_UDT, 0);
@@ -579,35 +570,34 @@ void rtl8723a_mlme_sitesurvey(struct rtw_adapter *padapter, u8 flag)
 		    ((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) ||
 		    ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE)) {
 			/*  enable to rx data frame */
-			rtw_write16(padapter, REG_RXFLTMAP2, 0xFFFF);
+			rtl8723au_write16(padapter, REG_RXFLTMAP2, 0xFFFF);
 
 			/*  enable update TSF */
 			SetBcnCtrlReg23a(padapter, 0, DIS_TSF_UDT);
 		}
 
-		v32 = rtw_read32(padapter, REG_RCR);
+		v32 = rtl8723au_read32(padapter, REG_RCR);
 		v32 |= RCR_CBSSID_BCN;
-		rtw_write32(padapter, REG_RCR, v32);
+		rtl8723au_write32(padapter, REG_RCR, v32);
 	}
 
-#ifdef CONFIG_8723AU_BT_COEXIST
-	BT_WifiScanNotify(padapter, flag ? true : false);
-#endif
+	rtl8723a_BT_wifiscan_notify(padapter, flag ? true : false);
 }
 
 void rtl8723a_on_rcr_am(struct rtw_adapter *padapter)
 {
-	rtw_write32(padapter, REG_RCR, rtw_read32(padapter, REG_RCR) | RCR_AM);
-	DBG_8723A("%s, %d, RCR = %x \n", __FUNCTION__, __LINE__,
-		  rtw_read32(padapter, REG_RCR));
+	rtl8723au_write32(padapter, REG_RCR,
+		    rtl8723au_read32(padapter, REG_RCR) | RCR_AM);
+	DBG_8723A("%s, %d, RCR = %x \n", __func__, __LINE__,
+		  rtl8723au_read32(padapter, REG_RCR));
 }
 
 void rtl8723a_off_rcr_am(struct rtw_adapter *padapter)
 {
-	rtw_write32(padapter, REG_RCR,
-		    rtw_read32(padapter, REG_RCR) & (~RCR_AM));
-	DBG_8723A("%s, %d, RCR = %x \n", __FUNCTION__, __LINE__,
-		  rtw_read32(padapter, REG_RCR));
+	rtl8723au_write32(padapter, REG_RCR,
+		    rtl8723au_read32(padapter, REG_RCR) & (~RCR_AM));
+	DBG_8723A("%s, %d, RCR = %x \n", __func__, __LINE__,
+		  rtl8723au_read32(padapter, REG_RCR));
 }
 
 void rtl8723a_set_slot_time(struct rtw_adapter *padapter, u8 slottime)
@@ -616,7 +606,7 @@ void rtl8723a_set_slot_time(struct rtw_adapter *padapter, u8 slottime)
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
-	rtw_write8(padapter, REG_SLOT, slottime);
+	rtl8723au_write8(padapter, REG_SLOT, slottime);
 
 	if (pmlmeinfo->WMM_enable == 0) {
 		if (pmlmeext->cur_wireless_mode == WIRELESS_11B)
@@ -627,10 +617,10 @@ void rtl8723a_set_slot_time(struct rtw_adapter *padapter, u8 slottime)
 		u1bAIFS = aSifsTime + (2 * pmlmeinfo->slotTime);
 
 		/*  <Roger_EXP> Temporary removed, 2008.06.20. */
-		rtw_write8(padapter, REG_EDCA_VO_PARAM, u1bAIFS);
-		rtw_write8(padapter, REG_EDCA_VI_PARAM, u1bAIFS);
-		rtw_write8(padapter, REG_EDCA_BE_PARAM, u1bAIFS);
-		rtw_write8(padapter, REG_EDCA_BK_PARAM, u1bAIFS);
+		rtl8723au_write8(padapter, REG_EDCA_VO_PARAM, u1bAIFS);
+		rtl8723au_write8(padapter, REG_EDCA_VI_PARAM, u1bAIFS);
+		rtl8723au_write8(padapter, REG_EDCA_BE_PARAM, u1bAIFS);
+		rtl8723au_write8(padapter, REG_EDCA_BK_PARAM, u1bAIFS);
 	}
 }
 
@@ -645,12 +635,12 @@ void rtl8723a_ack_preamble(struct rtw_adapter *padapter, u8 bShortPreamble)
 	/* regTmp = 0; */
 	if (bShortPreamble)
 		regTmp |= 0x80;
-	rtw_write8(padapter, REG_RRSR + 2, regTmp);
+	rtl8723au_write8(padapter, REG_RRSR + 2, regTmp);
 }
 
 void rtl8723a_set_sec_cfg(struct rtw_adapter *padapter, u8 sec)
 {
-	rtw_write8(padapter, REG_SECCFG, sec);
+	rtl8723au_write8(padapter, REG_SECCFG, sec);
 }
 
 void rtl8723a_cam_empty_entry(struct rtw_adapter *padapter, u8 ucIndex)
@@ -674,29 +664,54 @@ void rtl8723a_cam_empty_entry(struct rtw_adapter *padapter, u8 ucIndex)
 		ulCommand = ulCommand | CAM_POLLINIG | CAM_WRITE;
 		/*  write content 0 is equall to mark invalid */
 		/* delay_ms(40); */
-		rtw_write32(padapter, WCAMI, ulContent);
+		rtl8723au_write32(padapter, WCAMI, ulContent);
 		/* RT_TRACE(COMP_SEC, DBG_LOUD,
-		   ("CAM_empty_entry23a(): WRITE A4: %lx \n", ulContent));*/
+		   ("rtl8723a_cam_empty_entry(): WRITE A4: %lx \n",
+		   ulContent));*/
 		/* delay_ms(40); */
-		rtw_write32(padapter, RWCAM, ulCommand);
+		rtl8723au_write32(padapter, RWCAM, ulCommand);
 		/* RT_TRACE(COMP_SEC, DBG_LOUD,
-		   ("CAM_empty_entry23a(): WRITE A0: %lx \n", ulCommand));*/
+		   ("rtl8723a_cam_empty_entry(): WRITE A0: %lx \n",
+		   ulCommand));*/
 	}
 }
 
 void rtl8723a_cam_invalid_all(struct rtw_adapter *padapter)
 {
-	rtw_write32(padapter, RWCAM, BIT(31) | BIT(30));
+	rtl8723au_write32(padapter, RWCAM, BIT(31) | BIT(30));
 }
 
-void rtl8723a_cam_write(struct rtw_adapter *padapter, u32 val1, u32 val2)
+void rtl8723a_cam_write(struct rtw_adapter *padapter,
+			u8 entry, u16 ctrl, const u8 *mac, const u8 *key)
 {
 	u32 cmd;
+	unsigned int i, val, addr;
+	int j;
+
+	addr = entry << 3;
+
+	for (j = 5; j >= 0; j--) {
+		switch (j) {
+		case 0:
+			val = ctrl | (mac[0] << 16) | (mac[1] << 24);
+			break;
+		case 1:
+			val = mac[2] | (mac[3] << 8) |
+				(mac[4] << 16) | (mac[5] << 24);
+			break;
+		default:
+			i = (j - 2) << 2;
+			val = key[i] | (key[i+1] << 8) |
+				(key[i+2] << 16) | (key[i+3] << 24);
+			break;
+		}
 
-	rtw_write32(padapter, WCAMI, val1);
+		rtl8723au_write32(padapter, WCAMI, val);
+		cmd = CAM_POLLINIG | CAM_WRITE | (addr + j);
+		rtl8723au_write32(padapter, RWCAM, cmd);
 
-	cmd = CAM_POLLINIG | CAM_WRITE | val2;
-	rtw_write32(padapter, RWCAM, cmd);
+		/* DBG_8723A("%s => cam write: %x, %x\n", __func__, cmd, val);*/
+	}
 }
 
 void rtl8723a_fifo_cleanup(struct rtw_adapter *padapter)
@@ -708,20 +723,21 @@ void rtl8723a_fifo_cleanup(struct rtw_adapter *padapter)
 	u8 trycnt = 100;
 
 	/*  pause tx */
-	rtw_write8(padapter, REG_TXPAUSE, 0xff);
+	rtl8723au_write8(padapter, REG_TXPAUSE, 0xff);
 
 	/*  keep sn */
-	padapter->xmitpriv.nqos_ssn = rtw_read16(padapter, REG_NQOS_SEQ);
+	padapter->xmitpriv.nqos_ssn = rtl8723au_read16(padapter, REG_NQOS_SEQ);
 
 	if (pwrpriv->bkeepfwalive != true) {
 		u32 v32;
 
 		/*  RX DMA stop */
-		v32 = rtw_read32(padapter, REG_RXPKT_NUM);
+		v32 = rtl8723au_read32(padapter, REG_RXPKT_NUM);
 		v32 |= RW_RELEASE_EN;
-		rtw_write32(padapter, REG_RXPKT_NUM, v32);
+		rtl8723au_write32(padapter, REG_RXPKT_NUM, v32);
 		do {
-			v32 = rtw_read32(padapter, REG_RXPKT_NUM) & RXDMA_IDLE;
+			v32 = rtl8723au_read32(padapter,
+					       REG_RXPKT_NUM) & RXDMA_IDLE;
 			if (!v32)
 				break;
 		} while (trycnt--);
@@ -730,36 +746,32 @@ void rtl8723a_fifo_cleanup(struct rtw_adapter *padapter)
 		}
 
 		/*  RQPN Load 0 */
-		rtw_write16(padapter, REG_RQPN_NPQ, 0);
-		rtw_write32(padapter, REG_RQPN, 0x80000000);
+		rtl8723au_write16(padapter, REG_RQPN_NPQ, 0);
+		rtl8723au_write32(padapter, REG_RQPN, 0x80000000);
 		mdelay(10);
 	}
 }
 
-void rtl8723a_set_apfm_on_mac(struct rtw_adapter *padapter, u8 val)
-{
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
-
-	pHalData->bMacPwrCtrlOn = val;
-	DBG_8723A("%s: bMacPwrCtrlOn =%d\n", __func__, pHalData->bMacPwrCtrlOn);
-}
-
 void rtl8723a_bcn_valid(struct rtw_adapter *padapter)
 {
 	/* BCN_VALID, BIT16 of REG_TDECTRL = BIT0 of REG_TDECTRL+2,
 	   write 1 to clear, Clear by sw */
-	rtw_write8(padapter, REG_TDECTRL + 2,
-		   rtw_read8(padapter, REG_TDECTRL + 2) | BIT0);
+	rtl8723au_write8(padapter, REG_TDECTRL + 2,
+			 rtl8723au_read8(padapter, REG_TDECTRL + 2) | BIT(0));
 }
 
-void rtl8723a_set_tx_pause(struct rtw_adapter *padapter, u8 pause)
+bool rtl8723a_get_bcn_valid(struct rtw_adapter *padapter)
 {
-	rtw_write8(padapter, REG_TXPAUSE, pause);
+	bool retval;
+
+	retval = (rtl8723au_read8(padapter, REG_TDECTRL + 2) & BIT(0)) ? true : false;
+
+	return retval;
 }
 
 void rtl8723a_set_beacon_interval(struct rtw_adapter *padapter, u16 interval)
 {
-	rtw_write16(padapter, REG_BCN_INTERVAL, interval);
+	rtl8723au_write16(padapter, REG_BCN_INTERVAL, interval);
 }
 
 void rtl8723a_set_resp_sifs(struct rtw_adapter *padapter,
@@ -768,24 +780,24 @@ void rtl8723a_set_resp_sifs(struct rtw_adapter *padapter,
 	/* SIFS_Timer = 0x0a0a0808; */
 	/* RESP_SIFS for CCK */
 	/*  SIFS_T2T_CCK (0x08) */
-	rtw_write8(padapter, REG_R2T_SIFS, r2t1);
+	rtl8723au_write8(padapter, REG_R2T_SIFS, r2t1);
 	/* SIFS_R2T_CCK(0x08) */
-	rtw_write8(padapter, REG_R2T_SIFS + 1, r2t2);
+	rtl8723au_write8(padapter, REG_R2T_SIFS + 1, r2t2);
 	/* RESP_SIFS for OFDM */
 	/* SIFS_T2T_OFDM (0x0a) */
-	rtw_write8(padapter, REG_T2T_SIFS, t2t1);
+	rtl8723au_write8(padapter, REG_T2T_SIFS, t2t1);
 	/* SIFS_R2T_OFDM(0x0a) */
-	rtw_write8(padapter, REG_T2T_SIFS + 1, t2t2);
+	rtl8723au_write8(padapter, REG_T2T_SIFS + 1, t2t2);
 }
 
 void rtl8723a_set_ac_param_vo(struct rtw_adapter *padapter, u32 vo)
 {
-	rtw_write32(padapter, REG_EDCA_VO_PARAM, vo);
+	rtl8723au_write32(padapter, REG_EDCA_VO_PARAM, vo);
 }
 
 void rtl8723a_set_ac_param_vi(struct rtw_adapter *padapter, u32 vi)
 {
-	rtw_write32(padapter, REG_EDCA_VI_PARAM, vi);
+	rtl8723au_write32(padapter, REG_EDCA_VI_PARAM, vi);
 }
 
 void rtl8723a_set_ac_param_be(struct rtw_adapter *padapter, u32 be)
@@ -793,17 +805,17 @@ void rtl8723a_set_ac_param_be(struct rtw_adapter *padapter, u32 be)
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
 	pHalData->AcParam_BE = be;
-	rtw_write32(padapter, REG_EDCA_BE_PARAM, be);
+	rtl8723au_write32(padapter, REG_EDCA_BE_PARAM, be);
 }
 
 void rtl8723a_set_ac_param_bk(struct rtw_adapter *padapter, u32 bk)
 {
-	rtw_write32(padapter, REG_EDCA_BK_PARAM, bk);
+	rtl8723au_write32(padapter, REG_EDCA_BK_PARAM, bk);
 }
 
 void rtl8723a_set_rxdma_agg_pg_th(struct rtw_adapter *padapter, u8 val)
 {
-	rtw_write8(padapter, REG_RXDMA_AGG_PG_TH, val);
+	rtl8723au_write8(padapter, REG_RXDMA_AGG_PG_TH, val);
 }
 
 void rtl8723a_set_nav_upper(struct rtw_adapter *padapter, u32 usNavUpper)
@@ -821,7 +833,7 @@ void rtl8723a_set_nav_upper(struct rtw_adapter *padapter, u32 usNavUpper)
 	/*  is getting the upper integer. */
 	usNavUpper = (usNavUpper + HAL_8723A_NAV_UPPER_UNIT - 1) /
 		HAL_8723A_NAV_UPPER_UNIT;
-	rtw_write8(padapter, REG_NAV_UPPER, (u8) usNavUpper);
+	rtl8723au_write8(padapter, REG_NAV_UPPER, (u8) usNavUpper);
 }
 
 void rtl8723a_set_initial_gain(struct rtw_adapter *padapter, u32 rx_gain)
@@ -837,23 +849,18 @@ void rtl8723a_set_initial_gain(struct rtw_adapter *padapter, u32 rx_gain)
 	}
 }
 
-void rtl8723a_odm_support_ability_write(struct rtw_adapter *padapter, u32 val)
+void rtl8723a_odm_support_ability_restore(struct rtw_adapter *padapter)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
-	pHalData->odmpriv.SupportAbility = val;
+	pHalData->odmpriv.SupportAbility = pHalData->odmpriv.BK_SupportAbility;
 }
 
-void rtl8723a_odm_support_ability_backup(struct rtw_adapter *padapter, u8 val)
+void rtl8723a_odm_support_ability_backup(struct rtw_adapter *padapter)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
-	if (val)	/*  save dm flag */
-		pHalData->odmpriv.BK_SupportAbility =
-			pHalData->odmpriv.SupportAbility;
-	else		/*  restore dm flag */
-		pHalData->odmpriv.SupportAbility =
-			pHalData->odmpriv.BK_SupportAbility;
+	pHalData->odmpriv.BK_SupportAbility = pHalData->odmpriv.SupportAbility;
 }
 
 void rtl8723a_odm_support_ability_set(struct rtw_adapter *padapter, u32 val)
@@ -877,5 +884,44 @@ void rtl8723a_odm_support_ability_clr(struct rtw_adapter *padapter, u32 val)
 
 void rtl8723a_set_rpwm(struct rtw_adapter *padapter, u8 val)
 {
-	rtw_write8(padapter, REG_USB_HRPWM, val);
+	rtl8723au_write8(padapter, REG_USB_HRPWM, val);
+}
+
+u8 rtl8723a_get_rf_type(struct rtw_adapter *padapter)
+{
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
+
+	return pHalData->rf_type;
+}
+
+bool rtl8723a_get_fwlps_rf_on(struct rtw_adapter *padapter)
+{
+	bool retval;
+	u32 valRCR;
+
+	/*  When we halt NIC, we should check if FW LPS is leave. */
+
+	if ((padapter->bSurpriseRemoved == true) ||
+	    (padapter->pwrctrlpriv.rf_pwrstate == rf_off)) {
+		/*  If it is in HW/SW Radio OFF or IPS state, we do
+		    not check Fw LPS Leave, because Fw is unload. */
+		retval = true;
+	} else {
+		valRCR = rtl8723au_read32(padapter, REG_RCR);
+		if (valRCR & 0x00070000)
+			retval = false;
+		else
+			retval = true;
+	}
+
+	return retval;
+}
+
+bool rtl8723a_chk_hi_queue_empty(struct rtw_adapter *padapter)
+{
+	u32 hgq;
+
+	hgq = rtl8723au_read32(padapter, REG_HGQ_INFORMATION);
+
+	return ((hgq & 0x0000ff00) == 0) ? true : false;
 }
diff --git a/drivers/staging/rtl8723au/hal/hal_intf.c b/drivers/staging/rtl8723au/hal/hal_intf.c
index de3608b4010..5383e692546 100644
--- a/drivers/staging/rtl8723au/hal/hal_intf.c
+++ b/drivers/staging/rtl8723au/hal/hal_intf.c
@@ -19,232 +19,7 @@
 
 #include <hal_intf.h>
 
-#include <usb_hal.h>
-
-void rtw_hal_chip_configure23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.intf_chip_configure)
-		padapter->HalFunc.intf_chip_configure(padapter);
-}
-
-void rtw_hal_read_chip_info23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.read_adapter_info)
-		padapter->HalFunc.read_adapter_info(padapter);
-}
-
-void rtw_hal_read_chip_version23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.read_chip_version)
-		padapter->HalFunc.read_chip_version(padapter);
-}
-
-void rtw_hal_def_value_init23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.init_default_value)
-		padapter->HalFunc.init_default_value(padapter);
-}
-void	rtw_hal_free_data23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.free_hal_data)
-		padapter->HalFunc.free_hal_data(padapter);
-}
-void	rtw_hal_dm_init23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.dm_init)
-		padapter->HalFunc.dm_init(padapter);
-}
-void rtw_hal_dm_deinit23a(struct rtw_adapter *padapter)
-{
-	/*  cancel dm  timer */
-	if (padapter->HalFunc.dm_deinit)
-		padapter->HalFunc.dm_deinit(padapter);
-}
-void	rtw_hal_sw_led_init23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.InitSwLeds)
-		padapter->HalFunc.InitSwLeds(padapter);
-}
-
-void rtw_hal_sw_led_deinit23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.DeInitSwLeds)
-		padapter->HalFunc.DeInitSwLeds(padapter);
-}
-
-u32 rtw_hal_power_on23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.hal_power_on)
-		return padapter->HalFunc.hal_power_on(padapter);
-	return _FAIL;
-}
-
-uint	 rtw_hal_init23a(struct rtw_adapter *padapter)
-{
-	uint	status = _SUCCESS;
-
-	padapter->hw_init_completed = false;
-
-	status = padapter->HalFunc.hal_init(padapter);
-
-	if (status == _SUCCESS) {
-		padapter->hw_init_completed = true;
-
-		if (padapter->registrypriv.notch_filter == 1)
-			rtw_hal_notch_filter23a(padapter, 1);
-
-		rtw_hal_reset_security_engine23a(padapter);
-	} else {
-		padapter->hw_init_completed = false;
-		DBG_8723A("rtw_hal_init23a: hal__init fail\n");
-	}
-
-	RT_TRACE(_module_hal_init_c_, _drv_err_, ("-rtl871x_hal_init:status = 0x%x\n", status));
-
-	return status;
-}
-
-uint rtw_hal_deinit23a(struct rtw_adapter *padapter)
-{
-	uint	status = _SUCCESS;
-
-	status = padapter->HalFunc.hal_deinit(padapter);
-
-	if (status == _SUCCESS)
-		padapter->hw_init_completed = false;
-	else
-		DBG_8723A("\n rtw_hal_deinit23a: hal_init fail\n");
-	return status;
-}
-
-void rtw_hal_set_hwreg23a(struct rtw_adapter *padapter, u8 variable, u8 *val)
-{
-	if (padapter->HalFunc.SetHwRegHandler)
-		padapter->HalFunc.SetHwRegHandler(padapter, variable, val);
-}
-
-void rtw23a_hal_get_hwreg(struct rtw_adapter *padapter, u8 variable, u8 *val)
-{
-	if (padapter->HalFunc.GetHwRegHandler)
-		padapter->HalFunc.GetHwRegHandler(padapter, variable, val);
-}
-
-u8 rtw_hal_set_def_var23a(struct rtw_adapter *padapter, enum hal_def_variable eVariable, void *pValue)
-{
-	if (padapter->HalFunc.SetHalDefVarHandler)
-		return padapter->HalFunc.SetHalDefVarHandler(padapter, eVariable, pValue);
-	return _FAIL;
-}
-u8 rtw_hal_get_def_var23a(struct rtw_adapter *padapter, enum hal_def_variable eVariable, void *pValue)
-{
-	if (padapter->HalFunc.GetHalDefVarHandler)
-		return padapter->HalFunc.GetHalDefVarHandler(padapter, eVariable, pValue);
-	return _FAIL;
-}
-
-void rtw_hal_set_odm_var23a(struct rtw_adapter *padapter, enum hal_odm_variable eVariable, void *pValue1, bool bSet)
-{
-	if (padapter->HalFunc.SetHalODMVarHandler)
-		padapter->HalFunc.SetHalODMVarHandler(padapter, eVariable, pValue1, bSet);
-}
-void	rtw_hal_get_odm_var23a(struct rtw_adapter *padapter, enum hal_odm_variable eVariable, void *pValue1, bool bSet)
-{
-	if (padapter->HalFunc.GetHalODMVarHandler)
-		padapter->HalFunc.GetHalODMVarHandler(padapter, eVariable, pValue1, bSet);
-}
-
-void rtw_hal_enable_interrupt23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.enable_interrupt)
-		padapter->HalFunc.enable_interrupt(padapter);
-	else
-		DBG_8723A("%s: HalFunc.enable_interrupt is NULL!\n", __FUNCTION__);
-
-}
-void rtw_hal_disable_interrupt23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.disable_interrupt)
-		padapter->HalFunc.disable_interrupt(padapter);
-	else
-		DBG_8723A("%s: HalFunc.disable_interrupt is NULL!\n", __FUNCTION__);
-
-}
-
-u32	rtw_hal_inirp_init23a(struct rtw_adapter *padapter)
-{
-	u32 rst = _FAIL;
-	if (padapter->HalFunc.inirp_init)
-		rst = padapter->HalFunc.inirp_init(padapter);
-	else
-		DBG_8723A(" %s HalFunc.inirp_init is NULL!!!\n", __FUNCTION__);
-	return rst;
-}
-
-u32	rtw_hal_inirp_deinit23a(struct rtw_adapter *padapter)
-{
-
-	if (padapter->HalFunc.inirp_deinit)
-		return padapter->HalFunc.inirp_deinit(padapter);
-
-	return _FAIL;
-
-}
-
-u8	rtw_hal_intf_ps_func23a(struct rtw_adapter *padapter, enum hal_intf_ps_func efunc_id, u8 *val)
-{
-	if (padapter->HalFunc.interface_ps_func)
-		return padapter->HalFunc.interface_ps_func(padapter, efunc_id, val);
-	return _FAIL;
-}
-
-s32	rtw_hal_xmit23aframe_enqueue(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe)
-{
-	if (padapter->HalFunc.hal_xmitframe_enqueue)
-		return padapter->HalFunc.hal_xmitframe_enqueue(padapter, pxmitframe);
-
-	return false;
-}
-
-s32	rtw_hal_xmit23a(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe)
-{
-	if (padapter->HalFunc.hal_xmit)
-		return padapter->HalFunc.hal_xmit(padapter, pxmitframe);
-
-	return false;
-}
-
-s32	rtw_hal_mgnt_xmit23a(struct rtw_adapter *padapter, struct xmit_frame *pmgntframe)
-{
-	s32 ret = _FAIL;
-	if (padapter->HalFunc.mgnt_xmit)
-		ret = padapter->HalFunc.mgnt_xmit(padapter, pmgntframe);
-	return ret;
-}
-
-s32	rtw_hal_init23a_xmit_priv(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.init_xmit_priv != NULL)
-		return padapter->HalFunc.init_xmit_priv(padapter);
-	return _FAIL;
-}
-void	rtw_hal_free_xmit_priv23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.free_xmit_priv != NULL)
-		padapter->HalFunc.free_xmit_priv(padapter);
-}
-
-s32	rtw_hal_init23a_recv_priv(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.init_recv_priv)
-		return padapter->HalFunc.init_recv_priv(padapter);
-
-	return _FAIL;
-}
-void	rtw_hal_free_recv_priv23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.free_recv_priv)
-		padapter->HalFunc.free_recv_priv(padapter);
-}
+#include <rtl8723a_hal.h>
 
 void rtw_hal_update_ra_mask23a(struct sta_info *psta, u8 rssi_level)
 {
@@ -262,159 +37,6 @@ void rtw_hal_update_ra_mask23a(struct sta_info *psta, u8 rssi_level)
 #ifdef CONFIG_8723AU_AP_MODE
 		add_RATid23a(padapter, psta, rssi_level);
 #endif
-	} else {
-		if (padapter->HalFunc.UpdateRAMaskHandler)
-			padapter->HalFunc.UpdateRAMaskHandler(padapter, psta->mac_id, rssi_level);
-	}
-}
-
-void	rtw_hal_add_ra_tid23a(struct rtw_adapter *padapter, u32 bitmap, u8 arg, u8 rssi_level)
-{
-	if (padapter->HalFunc.Add_RateATid)
-		padapter->HalFunc.Add_RateATid(padapter, bitmap, arg, rssi_level);
-}
-
-/*	Start specifical interface thread		*/
-void	rtw_hal_start_thread23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.run_thread)
-		padapter->HalFunc.run_thread(padapter);
-}
-/*	Start specifical interface thread		*/
-void	rtw_hal_stop_thread23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.cancel_thread)
-		padapter->HalFunc.cancel_thread(padapter);
-}
-
-u32	rtw_hal_read_bbreg23a(struct rtw_adapter *padapter, u32 RegAddr, u32 BitMask)
-{
-	u32 data = 0;
-	if (padapter->HalFunc.read_bbreg)
-		 data = padapter->HalFunc.read_bbreg(padapter, RegAddr, BitMask);
-	return data;
-}
-void	rtw_hal_write_bbreg23a(struct rtw_adapter *padapter, u32 RegAddr, u32 BitMask, u32 Data)
-{
-	if (padapter->HalFunc.write_bbreg)
-		padapter->HalFunc.write_bbreg(padapter, RegAddr, BitMask, Data);
-}
-
-u32	rtw_hal_read_rfreg23a(struct rtw_adapter *padapter, u32 eRFPath, u32 RegAddr, u32 BitMask)
-{
-	u32 data = 0;
-	if (padapter->HalFunc.read_rfreg)
-		data = padapter->HalFunc.read_rfreg(padapter, eRFPath, RegAddr, BitMask);
-	return data;
-}
-void	rtw_hal_write_rfreg23a(struct rtw_adapter *padapter, u32 eRFPath, u32 RegAddr, u32 BitMask, u32 Data)
-{
-	if (padapter->HalFunc.write_rfreg)
-		padapter->HalFunc.write_rfreg(padapter, eRFPath, RegAddr, BitMask, Data);
-}
-
-s32	rtw_hal_interrupt_handler23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.interrupt_handler)
-		return padapter->HalFunc.interrupt_handler(padapter);
-	return _FAIL;
-}
-
-void	rtw_hal_set_bwmode23a(struct rtw_adapter *padapter,
-			   enum ht_channel_width Bandwidth, u8 offset)
-{
-	if (padapter->HalFunc.set_bwmode_handler)
-		padapter->HalFunc.set_bwmode_handler(padapter, Bandwidth,
-						     offset);
-}
-
-void	rtw_hal_set_chan23a(struct rtw_adapter *padapter, u8 channel)
-{
-	if (padapter->HalFunc.set_channel_handler)
-		padapter->HalFunc.set_channel_handler(padapter, channel);
-}
-
-void	rtw_hal_dm_watchdog23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.hal_dm_watchdog)
-		padapter->HalFunc.hal_dm_watchdog(padapter);
-}
-
-void rtw_hal_bcn_related_reg_setting23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.SetBeaconRelatedRegistersHandler)
-		padapter->HalFunc.SetBeaconRelatedRegistersHandler(padapter);
-}
-
-void	rtw_hal_sreset_init23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.sreset_init_value23a)
-		padapter->HalFunc.sreset_init_value23a(padapter);
-}
-void rtw_hal_sreset_reset23a(struct rtw_adapter *padapter)
-{
-	padapter = GET_PRIMARY_ADAPTER(padapter);
-
-	if (padapter->HalFunc.silentreset)
-		padapter->HalFunc.silentreset(padapter);
-}
-
-void rtw_hal_sreset_reset23a_value23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.sreset_reset_value23a)
-		padapter->HalFunc.sreset_reset_value23a(padapter);
-}
-
-void rtw_hal_sreset_xmit_status_check23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.sreset_xmit_status_check)
-		padapter->HalFunc.sreset_xmit_status_check(padapter);
-}
-void rtw_hal_sreset_linked_status_check23a(struct rtw_adapter *padapter)
-{
-	if (padapter->HalFunc.sreset_linked_status_check)
-		padapter->HalFunc.sreset_linked_status_check(padapter);
-}
-u8   rtw_hal_sreset_get_wifi_status23a(struct rtw_adapter *padapter)
-{
-	u8 status = 0;
-	if (padapter->HalFunc.sreset_get_wifi_status23a)
-		status = padapter->HalFunc.sreset_get_wifi_status23a(padapter);
-	return status;
-}
-
-bool rtw_hal_sreset_inprogress(struct rtw_adapter *padapter)
-{
-	bool inprogress = false;
-
-	padapter = GET_PRIMARY_ADAPTER(padapter);
-
-	if (padapter->HalFunc.sreset_inprogress)
-		inprogress = padapter->HalFunc.sreset_inprogress(padapter);
-	return inprogress;
-}
-
-void rtw_hal_notch_filter23a(struct rtw_adapter *adapter, bool enable)
-{
-	if (adapter->HalFunc.hal_notch_filter)
-		adapter->HalFunc.hal_notch_filter(adapter, enable);
-}
-
-void rtw_hal_reset_security_engine23a(struct rtw_adapter *adapter)
-{
-	if (adapter->HalFunc.hal_reset_security_engine)
-		adapter->HalFunc.hal_reset_security_engine(adapter);
-}
-
-s32 rtw_hal_c2h_handler23a(struct rtw_adapter *adapter, struct c2h_evt_hdr *c2h_evt)
-{
-	s32 ret = _FAIL;
-	if (adapter->HalFunc.c2h_handler)
-		ret = adapter->HalFunc.c2h_handler(adapter, c2h_evt);
-	return ret;
-}
-
-c2h_id_filter rtw_hal_c2h_id_filter_ccx23a(struct rtw_adapter *adapter)
-{
-	return adapter->HalFunc.c2h_id_filter_ccx;
+	} else
+		rtl8723a_update_ramask(padapter, psta->mac_id, rssi_level);
 }
diff --git a/drivers/staging/rtl8723au/hal/odm.c b/drivers/staging/rtl8723au/hal/odm.c
index 584a74ed294..e15ebfe1881 100644
--- a/drivers/staging/rtl8723au/hal/odm.c
+++ b/drivers/staging/rtl8723au/hal/odm.c
@@ -14,6 +14,7 @@
  ******************************************************************************/
 
 #include "odm_precomp.h"
+#include "usb_ops_linux.h"
 
 static const u16 dB_Invert_Table[8][12] = {
 	{1, 1, 1, 2, 2, 2, 2, 3, 3, 3, 4, 4},
@@ -199,17 +200,6 @@ void odm_RefreshRateAdaptiveMask23aAPADSL23a(struct dm_odm_t *pDM_Odm);
 
 void odm_DynamicTxPower23aInit(struct dm_odm_t *pDM_Odm);
 
-void odm_DynamicTxPower23aRestorePowerIndex(struct dm_odm_t *pDM_Odm);
-
-void odm_DynamicTxPower23aSavePowerIndex(struct dm_odm_t *pDM_Odm);
-
-void odm_DynamicTxPower23aWritePowerIndex(struct dm_odm_t *pDM_Odm,
-	u8 Value);
-
-void odm_DynamicTxPower23a_92C(struct dm_odm_t *pDM_Odm);
-
-void odm_DynamicTxPower23a_92D(struct dm_odm_t *pDM_Odm);
-
 void odm_RSSIMonitorInit(struct dm_odm_t *pDM_Odm);
 
 void odm_RSSIMonitorCheck23aMP(struct dm_odm_t *pDM_Odm);
@@ -232,8 +222,6 @@ void odm_SwAntDivChkAntSwitchNIC(struct dm_odm_t *pDM_Odm,
 
 void odm_SwAntDivChkAntSwitchCallback23a(unsigned long data);
 
-void odm_GlobalAdapterCheck(void);
-
 void odm_RefreshRateAdaptiveMask23a(struct dm_odm_t *pDM_Odm);
 
 void ODM_TXPowerTrackingCheck23a(struct dm_odm_t *pDM_Odm);
@@ -306,7 +294,6 @@ void ODM23a_DMInit(struct dm_odm_t *pDM_Odm)
 void ODM_DMWatchdog23a(struct dm_odm_t *pDM_Odm)
 {
 	/* 2012.05.03 Luke: For all IC series */
-	odm_GlobalAdapterCheck();
 	odm_CmnInfoHook_Debug23a(pDM_Odm);
 	odm_CmnInfoUpdate_Debug23a(pDM_Odm);
 	odm_CommonInfoSelfUpdate23a(pDM_Odm);
@@ -343,7 +330,6 @@ void ODM_DMWatchdog23a(struct dm_odm_t *pDM_Odm)
 	if (pDM_Odm->SupportICType & ODM_IC_11N_SERIES) {
 		ODM_TXPowerTrackingCheck23a(pDM_Odm);
 	      odm_EdcaTurboCheck23a(pDM_Odm);
-		odm_DynamicTxPower23a(pDM_Odm);
 	}
 
 	odm_dtc(pDM_Odm);
@@ -563,7 +549,7 @@ void ODM_CmnInfoUpdate23a(struct dm_odm_t *pDM_Odm, u32 CmnInfo, u64 Value)
 void odm_CommonInfoSelfInit23a(struct dm_odm_t *pDM_Odm
 	)
 {
-	pDM_Odm->bCckHighPower = (bool) ODM_GetBBReg(pDM_Odm, 0x824, BIT9);
+	pDM_Odm->bCckHighPower = (bool) ODM_GetBBReg(pDM_Odm, 0x824, BIT(9));
 	pDM_Odm->RFPathRxEnable = (u8) ODM_GetBBReg(pDM_Odm, 0xc04, 0x0F);
 	if (pDM_Odm->SupportICType & (ODM_RTL8723A))
 		pDM_Odm->AntDivType = CGCS_RX_SW_ANTDIV;
@@ -941,8 +927,8 @@ void odm_FalseAlarmCounterStatistics23a(struct dm_odm_t *pDM_Odm)
 
 	if (pDM_Odm->SupportICType & ODM_IC_11N_SERIES) {
 		/* hold ofdm counter */
-		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_HOLDC_11N, BIT31, 1); /* hold page C counter */
-		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTD_11N, BIT31, 1); /* hold page D counter */
+		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_HOLDC_11N, BIT(31), 1); /* hold page C counter */
+		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTD_11N, BIT(31), 1); /* hold page D counter */
 
 		ret_value = ODM_GetBBReg(pDM_Odm, ODM_REG_OFDM_FA_TYPE1_11N, bMaskDWord);
 		FalseAlmCnt->Cnt_Fast_Fsync = (ret_value&0xffff);
@@ -962,9 +948,9 @@ void odm_FalseAlarmCounterStatistics23a(struct dm_odm_t *pDM_Odm)
 					     FalseAlmCnt->Cnt_Mcs_fail +
 					     FalseAlmCnt->Cnt_Fast_Fsync +
 					     FalseAlmCnt->Cnt_SB_Search_fail;
-	/* hold cck counter */
-	ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N, BIT12, 1);
-	ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N, BIT14, 1);
+		/* hold cck counter */
+		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N, BIT(12), 1);
+		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N, BIT(14), 1);
 
 	ret_value = ODM_GetBBReg(pDM_Odm, ODM_REG_CCK_FA_LSB_11N, bMaskByte0);
 	FalseAlmCnt->Cnt_Cck_fail = ret_value;
@@ -986,20 +972,24 @@ void odm_FalseAlarmCounterStatistics23a(struct dm_odm_t *pDM_Odm)
 
 	if (pDM_Odm->SupportICType >= ODM_RTL8723A) {
 		/* reset false alarm counter registers */
-		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTC_11N, BIT31, 1);
-		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTC_11N, BIT31, 0);
-		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTD_11N, BIT27, 1);
-		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTD_11N, BIT27, 0);
+		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTC_11N, BIT(31), 1);
+		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTC_11N, BIT(31), 0);
+		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTD_11N, BIT(27), 1);
+		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTD_11N, BIT(27), 0);
 		/* update ofdm counter */
-		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_HOLDC_11N, BIT31, 0); /* update page C counter */
-		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTD_11N, BIT31, 0); /* update page D counter */
+		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_HOLDC_11N, BIT(31), 0); /* update page C counter */
+		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RSTD_11N, BIT(31), 0); /* update page D counter */
 
 		/* reset CCK CCA counter */
-		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N, BIT13|BIT12, 0);
-		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N, BIT13|BIT12, 2);
+		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N,
+			     BIT(13) | BIT(12), 0);
+		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N,
+			     BIT(13) | BIT(12), 2);
 		/* reset CCK FA counter */
-		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N, BIT15|BIT14, 0);
-		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N, BIT15|BIT14, 2);
+		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N,
+			     BIT(15) | BIT(14), 0);
+		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11N,
+			     BIT(15) | BIT(14), 2);
 	}
 
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_FA_CNT, ODM_DBG_LOUD, ("Enter odm_FalseAlarmCounterStatistics23a\n"));
@@ -1016,11 +1006,11 @@ void odm_FalseAlarmCounterStatistics23a(struct dm_odm_t *pDM_Odm)
 		FalseAlmCnt->Cnt_all = FalseAlmCnt->Cnt_Ofdm_fail + FalseAlmCnt->Cnt_Cck_fail;
 
 		/*  reset OFDM FA coutner */
-		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RST_11AC, BIT17, 1);
-		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RST_11AC, BIT17, 0);
+		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RST_11AC, BIT(17), 1);
+		ODM_SetBBReg(pDM_Odm, ODM_REG_OFDM_FA_RST_11AC, BIT(17), 0);
 		/*  reset CCK FA counter */
-		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11AC, BIT15, 0);
-		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11AC, BIT15, 1);
+		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11AC, BIT(15), 0);
+		ODM_SetBBReg(pDM_Odm, ODM_REG_CCK_FA_RST_11AC, BIT(15), 1);
 	}
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_FA_CNT, ODM_DBG_LOUD, ("Cnt_Cck_fail =%d\n", FalseAlmCnt->Cnt_Cck_fail));
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_FA_CNT, ODM_DBG_LOUD, ("Cnt_Ofdm_fail =%d\n", FalseAlmCnt->Cnt_Ofdm_fail));
@@ -1140,7 +1130,8 @@ void ODM_RF_Saving23a(struct dm_odm_t *pDM_Odm, u8 bForceInNormal)
 	if (pDM_PSTable->initialize == 0) {
 
 		pDM_PSTable->Reg874 = (ODM_GetBBReg(pDM_Odm, 0x874, bMaskDWord)&0x1CC000)>>14;
-		pDM_PSTable->RegC70 = (ODM_GetBBReg(pDM_Odm, 0xc70, bMaskDWord)&BIT3)>>3;
+		pDM_PSTable->RegC70 =
+			(ODM_GetBBReg(pDM_Odm, 0xc70, bMaskDWord) & BIT(3)) >>3;
 		pDM_PSTable->Reg85C = (ODM_GetBBReg(pDM_Odm, 0x85c, bMaskDWord)&0xFF000000)>>24;
 		pDM_PSTable->RegA74 = (ODM_GetBBReg(pDM_Odm, 0xa74, bMaskDWord)&0xF000)>>12;
 		/* Reg818 = PHY_QueryBBReg(pAdapter, 0x818, bMaskDWord); */
@@ -1172,23 +1163,23 @@ void ODM_RF_Saving23a(struct dm_odm_t *pDM_Odm, u8 bForceInNormal)
 			/*  <tynli_note> 8723 RSSI report will be wrong. Set 0x874[5]= 1 when enter BB power saving mode. */
 			/*  Suggested by SD3 Yu-Nan. 2011.01.20. */
 			if (pDM_Odm->SupportICType == ODM_RTL8723A)
-				ODM_SetBBReg(pDM_Odm, 0x874, BIT5, 0x1); /* Reg874[5]= 1b'1 */
+				ODM_SetBBReg(pDM_Odm, 0x874, BIT(5), 0x1); /* Reg874[5]= 1b'1 */
 			ODM_SetBBReg(pDM_Odm, 0x874, 0x1C0000, 0x2); /* Reg874[20:18]= 3'b010 */
-			ODM_SetBBReg(pDM_Odm, 0xc70, BIT3, 0); /* RegC70[3]= 1'b0 */
+			ODM_SetBBReg(pDM_Odm, 0xc70, BIT(3), 0); /* RegC70[3]= 1'b0 */
 			ODM_SetBBReg(pDM_Odm, 0x85c, 0xFF000000, 0x63); /* Reg85C[31:24]= 0x63 */
 			ODM_SetBBReg(pDM_Odm, 0x874, 0xC000, 0x2); /* Reg874[15:14]= 2'b10 */
 			ODM_SetBBReg(pDM_Odm, 0xa74, 0xF000, 0x3); /* RegA75[7:4]= 0x3 */
-			ODM_SetBBReg(pDM_Odm, 0x818, BIT28, 0x0); /* Reg818[28]= 1'b0 */
-			ODM_SetBBReg(pDM_Odm, 0x818, BIT28, 0x1); /* Reg818[28]= 1'b1 */
+			ODM_SetBBReg(pDM_Odm, 0x818, BIT(28), 0x0); /* Reg818[28]= 1'b0 */
+			ODM_SetBBReg(pDM_Odm, 0x818, BIT(28), 0x1); /* Reg818[28]= 1'b1 */
 		} else {
 			ODM_SetBBReg(pDM_Odm, 0x874, 0x1CC000, pDM_PSTable->Reg874);
-			ODM_SetBBReg(pDM_Odm, 0xc70, BIT3, pDM_PSTable->RegC70);
+			ODM_SetBBReg(pDM_Odm, 0xc70, BIT(3), pDM_PSTable->RegC70);
 			ODM_SetBBReg(pDM_Odm, 0x85c, 0xFF000000, pDM_PSTable->Reg85C);
 			ODM_SetBBReg(pDM_Odm, 0xa74, 0xF000, pDM_PSTable->RegA74);
-			ODM_SetBBReg(pDM_Odm, 0x818, BIT28, 0x0);
+			ODM_SetBBReg(pDM_Odm, 0x818, BIT(28), 0x0);
 
 			if (pDM_Odm->SupportICType == ODM_RTL8723A)
-				ODM_SetBBReg(pDM_Odm, 0x874, BIT5, 0x0); /* Reg874[5]= 1b'0 */
+				ODM_SetBBReg(pDM_Odm, 0x874, BIT(5), 0x0); /* Reg874[5]= 1b'0 */
 		}
 		pDM_PSTable->PreRFState = pDM_PSTable->CurRFState;
 	}
@@ -1289,7 +1280,7 @@ u32 ODM_Get_Rate_Bitmap23a(struct dm_odm_t *pDM_Odm,
 		break;
 	}
 
-	/* printk("%s ==> rssi_level:0x%02x, WirelessMode:0x%02x, rate_bitmap:0x%08x \n", __FUNCTION__, rssi_level, WirelessMode, rate_bitmap); */
+	/* printk("%s ==> rssi_level:0x%02x, WirelessMode:0x%02x, rate_bitmap:0x%08x \n", __func__, rssi_level, WirelessMode, rate_bitmap); */
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_RA_MASK, ODM_DBG_LOUD, (" ==> rssi_level:0x%02x, WirelessMode:0x%02x, rate_bitmap:0x%08x \n", rssi_level, WirelessMode, rate_bitmap));
 
 	return rate_bitmap;
@@ -1345,7 +1336,7 @@ void odm_RefreshRateAdaptiveMask23aCE23a(struct dm_odm_t *pDM_Odm)
 		return;
 	}
 
-	/* printk("==> %s \n", __FUNCTION__); */
+	/* printk("==> %s \n", __func__); */
 
 	for (i = 0; i < ODM_ASSOCIATE_ENTRY_NUM; i++) {
 		struct sta_info *pstat = pDM_Odm->pODM_StaInfo[i];
@@ -1424,60 +1415,13 @@ void odm_DynamicTxPower23aInit(struct dm_odm_t *pDM_Odm)
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
 	struct dm_priv *pdmpriv = &pHalData->dmpriv;
 
-	pdmpriv->bDynamicTxPowerEnable = false;
-
-	pdmpriv->LastDTPLvl = TxHighPwrLevel_Normal;
+	/*
+	 * This is never changed, so we should be able to clean up the
+	 * code checking for different values in rtl8723a_rf6052.c
+	 */
 	pdmpriv->DynamicTxHighPowerLvl = TxHighPwrLevel_Normal;
 }
 
-void odm_DynamicTxPower23aSavePowerIndex(struct dm_odm_t *pDM_Odm)
-{
-	u8 index;
-	u32 Power_Index_REG[6] = {0xc90, 0xc91, 0xc92, 0xc98, 0xc99, 0xc9a};
-
-	struct rtw_adapter *Adapter = pDM_Odm->Adapter;
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
-	struct dm_priv *pdmpriv = &pHalData->dmpriv;
-	for (index = 0; index < 6; index++)
-		pdmpriv->PowerIndex_backup[index] = rtw_read8(Adapter, Power_Index_REG[index]);
-}
-
-void odm_DynamicTxPower23aRestorePowerIndex(struct dm_odm_t *pDM_Odm)
-{
-	u8 index;
-	struct rtw_adapter *Adapter = pDM_Odm->Adapter;
-
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
-	u32 Power_Index_REG[6] = {0xc90, 0xc91, 0xc92, 0xc98, 0xc99, 0xc9a};
-	struct dm_priv *pdmpriv = &pHalData->dmpriv;
-	for (index = 0; index < 6; index++)
-		rtw_write8(Adapter, Power_Index_REG[index], pdmpriv->PowerIndex_backup[index]);
-}
-
-void odm_DynamicTxPower23aWritePowerIndex(struct dm_odm_t *pDM_Odm,
-	u8 Value)
-{
-
-	u8 index;
-	u32 Power_Index_REG[6] = {0xc90, 0xc91, 0xc92, 0xc98, 0xc99, 0xc9a};
-
-	for (index = 0; index < 6; index++)
-		ODM_Write1Byte(pDM_Odm, Power_Index_REG[index], Value);
-
-}
-
-void odm_DynamicTxPower23a(struct dm_odm_t *pDM_Odm)
-{
-}
-
-void odm_DynamicTxPower23a_92C(struct dm_odm_t *pDM_Odm)
-{
-}
-
-void odm_DynamicTxPower23a_92D(struct dm_odm_t *pDM_Odm)
-{
-}
-
 /* 3 ============================================================ */
 /* 3 RSSI Monitor */
 /* 3 ============================================================ */
@@ -1576,22 +1520,6 @@ void odm_RSSIMonitorCheck23aAP(struct dm_odm_t *pDM_Odm)
 {
 }
 
-void ODM_InitAllTimers(struct dm_odm_t *pDM_Odm)
-{
-	setup_timer(&pDM_Odm->DM_SWAT_Table.SwAntennaSwitchTimer,
-		    odm_SwAntDivChkAntSwitchCallback23a, (unsigned long)pDM_Odm);
-}
-
-void ODM_CancelAllTimers(struct dm_odm_t *pDM_Odm)
-{
-	del_timer_sync(&pDM_Odm->DM_SWAT_Table.SwAntennaSwitchTimer);
-}
-
-void ODM_ReleaseAllTimers(struct dm_odm_t *pDM_Odm)
-{
-	ODM_ReleaseTimer(pDM_Odm, &pDM_Odm->DM_SWAT_Table.SwAntennaSwitchTimer);
-}
-
 /* endif */
 /* 3 ============================================================ */
 /* 3 Tx Power Tracking */
@@ -1655,7 +1583,8 @@ void odm_SwAntDivInit(struct dm_odm_t *pDM_Odm)
 {
 }
 
-void ODM_SwAntDivChkPerPktRssi(struct dm_odm_t *pDM_Odm, u8 StationID, struct odm_phy_info *pPhyInfo)
+void ODM_SwAntDivChkPerPktRssi(struct dm_odm_t *pDM_Odm, u8 StationID,
+			       struct phy_info *pPhyInfo)
 {
 }
 
@@ -1739,10 +1668,8 @@ void odm_EdcaTurboCheck23aCE23a(struct dm_odm_t *pDM_Odm)
 	if (pmlmeinfo->assoc_AP_vendor >=  HT_IOT_PEER_MAX)
 		goto dm_CheckEdcaTurbo_EXIT;
 
-#ifdef CONFIG_8723AU_BT_COEXIST
-	if (BT_DisableEDCATurbo(Adapter))
+	if (rtl8723a_BT_disable_EDCA_turbo(Adapter))
 		goto dm_CheckEdcaTurbo_EXIT;
-#endif
 
 	/*  Check if the status needs to be changed. */
 	if ((bbtchange) || (!precvpriv->bIsAnyNonBEPkts)) {
@@ -1774,7 +1701,8 @@ void odm_EdcaTurboCheck23aCE23a(struct dm_odm_t *pDM_Odm)
 				edca_param = EDCAParam[pmlmeinfo->assoc_AP_vendor][trafficIndex];
 			else
 				edca_param = EDCAParam[HT_IOT_PEER_UNKNOWN][trafficIndex];
-			rtw_write32(Adapter, REG_EDCA_BE_PARAM, edca_param);
+			rtl8723au_write32(Adapter, REG_EDCA_BE_PARAM,
+					  edca_param);
 
 			pDM_Odm->DM_EDCA_Table.prv_traffic_idx = trafficIndex;
 		}
@@ -1784,7 +1712,8 @@ void odm_EdcaTurboCheck23aCE23a(struct dm_odm_t *pDM_Odm)
 		/*  Turn Off EDCA turbo here. */
 		/*  Restore original EDCA according to the declaration of AP. */
 		if (pDM_Odm->DM_EDCA_Table.bCurrentTurboEDCA) {
-			rtw_write32(Adapter, REG_EDCA_BE_PARAM, pHalData->AcParam_BE);
+			rtl8723au_write32(Adapter, REG_EDCA_BE_PARAM,
+					  pHalData->AcParam_BE);
 			pDM_Odm->DM_EDCA_Table.bCurrentTurboEDCA = false;
 		}
 	}
@@ -1804,10 +1733,10 @@ u32 GetPSDData(struct dm_odm_t *pDM_Odm, unsigned int point, u8 initial_gain_psd
 	ODM_SetBBReg(pDM_Odm, 0x808, 0x3FF, point);
 
 	/* Start PSD calculation, Reg808[22]= 0->1 */
-	ODM_SetBBReg(pDM_Odm, 0x808, BIT22, 1);
+	ODM_SetBBReg(pDM_Odm, 0x808, BIT(22), 1);
 	/* Need to wait for HW PSD report */
 	udelay(30);
-	ODM_SetBBReg(pDM_Odm, 0x808, BIT22, 0);
+	ODM_SetBBReg(pDM_Odm, 0x808, BIT(22), 0);
 	/* Read PSD report, Reg8B4[15:0] */
 	psd_report = ODM_GetBBReg(pDM_Odm, 0x8B4, bMaskDWord) & 0x0000FFFF;
 
@@ -1845,16 +1774,6 @@ ConvertTo_dB23a(
 }
 
 /*  */
-/*  2011/09/22 MH Add for 92D global spin lock utilization. */
-/*  */
-void
-odm_GlobalAdapterCheck(
-		void
-	)
-{
-}	/*  odm_GlobalAdapterCheck */
-
-/*  */
 /*  Description: */
 /*Set Single/Dual Antenna default setting for products that do not do detection in advance. */
 /*  */
@@ -1941,7 +1860,7 @@ bool ODM_SingleDualAntennaDetection(struct dm_odm_t *pDM_Odm, u8 mode)
 	odm_PHY_SaveAFERegisters(pDM_Odm, AFE_REG_8723A, AFE_Backup, 16);
 
 	/* Set PSD 128 pts */
-	ODM_SetBBReg(pDM_Odm, rFPGA0_PSDFunction, BIT14|BIT15, 0x0);  /* 128 pts */
+	ODM_SetBBReg(pDM_Odm, rFPGA0_PSDFunction, BIT(14) | BIT(15), 0x0);
 
 	/*  To SET CH1 to do */
 	ODM_SetRFReg(pDM_Odm, RF_PATH_A, ODM_CHANNEL, bRFRegOffsetMask, 0x01);     /* Channel 1 */
diff --git a/drivers/staging/rtl8723au/hal/odm_HWConfig.c b/drivers/staging/rtl8723au/hal/odm_HWConfig.c
index 72441709697..82b1b834889 100644
--- a/drivers/staging/rtl8723au/hal/odm_HWConfig.c
+++ b/drivers/staging/rtl8723au/hal/odm_HWConfig.c
@@ -91,7 +91,7 @@ odm_EVMdbToPercentage(
 }
 
 static void odm_RxPhyStatus92CSeries_Parsing(struct dm_odm_t *pDM_Odm,
-					     struct odm_phy_info *pPhyInfo,
+					     struct phy_info *pPhyInfo,
 					     u8 *pPhyStatus,
 					     struct odm_packet_info *pPktinfo)
 {
@@ -281,7 +281,7 @@ void odm_Init_RSSIForDM23a(struct dm_odm_t *pDM_Odm)
 }
 
 static void odm_Process_RSSIForDM(struct dm_odm_t *pDM_Odm,
-				  struct odm_phy_info *pPhyInfo,
+				  struct phy_info *pPhyInfo,
 				  struct odm_packet_info *pPktinfo)
 {
 	s32 UndecoratedSmoothedPWDB, UndecoratedSmoothedCCK;
@@ -347,7 +347,8 @@ static void odm_Process_RSSIForDM(struct dm_odm_t *pDM_Odm,
 							(RSSI_Ave)) / (Rx_Smooth_Factor);
 				}
 			}
-			pEntry->rssi_stat.PacketMap = (pEntry->rssi_stat.PacketMap<<1) | BIT0;
+			pEntry->rssi_stat.PacketMap =
+				(pEntry->rssi_stat.PacketMap<<1) | BIT(0);
 		} else {
 			RSSI_Ave = pPhyInfo->RxPWDBAll;
 
@@ -377,7 +378,8 @@ static void odm_Process_RSSIForDM(struct dm_odm_t *pDM_Odm,
 			pEntry->rssi_stat.ValidBit++;
 
 		for (i = 0; i < pEntry->rssi_stat.ValidBit; i++)
-			OFDM_pkt += (u8)(pEntry->rssi_stat.PacketMap>>i)&BIT0;
+			OFDM_pkt +=
+				(u8)(pEntry->rssi_stat.PacketMap>>i) & BIT(0);
 
 		if (pEntry->rssi_stat.ValidBit == 64) {
 			Weighting = ((OFDM_pkt<<4) > 64)?64:(OFDM_pkt<<4);
@@ -396,7 +398,7 @@ static void odm_Process_RSSIForDM(struct dm_odm_t *pDM_Odm,
 
 /*  Endianness before calling this API */
 static void ODM_PhyStatusQuery23a_92CSeries(struct dm_odm_t *pDM_Odm,
-					 struct odm_phy_info *pPhyInfo,
+					 struct phy_info *pPhyInfo,
 					 u8 *pPhyStatus,
 					 struct odm_packet_info *pPktinfo)
 {
@@ -411,7 +413,7 @@ static void ODM_PhyStatusQuery23a_92CSeries(struct dm_odm_t *pDM_Odm,
 	}
 }
 
-void ODM_PhyStatusQuery23a(struct dm_odm_t *pDM_Odm, struct odm_phy_info *pPhyInfo,
+void ODM_PhyStatusQuery23a(struct dm_odm_t *pDM_Odm, struct phy_info *pPhyInfo,
 			   u8 *pPhyStatus, struct odm_packet_info *pPktinfo)
 {
 	ODM_PhyStatusQuery23a_92CSeries(pDM_Odm, pPhyInfo, pPhyStatus, pPktinfo);
@@ -426,12 +428,9 @@ void ODM_MacStatusQuery23a(struct dm_odm_t *pDM_Odm, u8 *pMacStatus, u8 MacID,
 
 }
 
-enum hal_status
-ODM_ConfigRFWithHeaderFile23a(
-	struct dm_odm_t *pDM_Odm,
-	enum RF_RADIO_PATH	Content,
-	enum RF_RADIO_PATH	eRFPath
-  )
+int ODM_ConfigRFWithHeaderFile23a(struct dm_odm_t *pDM_Odm,
+				  enum RF_RADIO_PATH Content,
+				  enum RF_RADIO_PATH eRFPath)
 {
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_LOUD,
 		     ("===>ODM_ConfigRFWithHeaderFile23a\n"));
@@ -446,14 +445,11 @@ ODM_ConfigRFWithHeaderFile23a(
 	}
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_TRACE,
 		     ("ODM_ConfigRFWithHeaderFile23a: Radio No %x\n", eRFPath));
-	return HAL_STATUS_SUCCESS;
+	return _SUCCESS;
 }
 
-enum hal_status
-ODM_ConfigBBWithHeaderFile23a(
-	struct dm_odm_t *pDM_Odm,
-	enum odm_bb_config_type		ConfigType
-	)
+int ODM_ConfigBBWithHeaderFile23a(struct dm_odm_t *pDM_Odm,
+				  enum odm_bb_config_type ConfigType)
 {
 	if (pDM_Odm->SupportICType == ODM_RTL8723A) {
 		if (ConfigType == CONFIG_BB_PHY_REG)
@@ -465,17 +461,12 @@ ODM_ConfigBBWithHeaderFile23a(
 		ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_LOUD,
 			     (" ===> phy_ConfigBBWithHeaderFile() agc:Rtl8723PHY_REG_1TArray\n"));
 	}
-	return HAL_STATUS_SUCCESS;
+	return _SUCCESS;
 }
 
-enum hal_status
-ODM_ConfigMACWithHeaderFile23a(
-	struct dm_odm_t *pDM_Odm
-	)
+int ODM_ConfigMACWithHeaderFile23a(struct dm_odm_t *pDM_Odm)
 {
-	u8 result = HAL_STATUS_SUCCESS;
-
 	if (pDM_Odm->SupportICType == ODM_RTL8723A)
 		READ_AND_CONFIG_MP(8723A, _MAC_REG_);
-	return result;
+	return _SUCCESS;
 }
diff --git a/drivers/staging/rtl8723au/hal/odm_interface.c b/drivers/staging/rtl8723au/hal/odm_interface.c
index bef1269749d..f03f6d4a388 100644
--- a/drivers/staging/rtl8723au/hal/odm_interface.c
+++ b/drivers/staging/rtl8723au/hal/odm_interface.c
@@ -21,6 +21,7 @@
 /*  */
 /*  ODM IO Relative API. */
 /*  */
+#include <usb_ops_linux.h>
 
 u8 ODM_Read1Byte(struct dm_odm_t *pDM_Odm,
 	u32			RegAddr
@@ -28,59 +29,42 @@ u8 ODM_Read1Byte(struct dm_odm_t *pDM_Odm,
 {
 	struct rtw_adapter *Adapter = pDM_Odm->Adapter;
 
-	return rtw_read8(Adapter, RegAddr);
+	return rtl8723au_read8(Adapter, RegAddr);
 }
 
-u16 ODM_Read2Byte(struct dm_odm_t *pDM_Odm,
-	u32			RegAddr
-	)
+u16 ODM_Read2Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr)
 {
 	struct rtw_adapter *Adapter = pDM_Odm->Adapter;
 
-	return rtw_read16(Adapter, RegAddr);
+	return rtl8723au_read16(Adapter, RegAddr);
 }
 
-u32 ODM_Read4Byte(struct dm_odm_t *pDM_Odm,
-	u32			RegAddr
-	)
+u32 ODM_Read4Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr)
 {
 	struct rtw_adapter *Adapter = pDM_Odm->Adapter;
 
-	return rtw_read32(Adapter, RegAddr);
+	return rtl8723au_read32(Adapter, RegAddr);
 }
 
-void ODM_Write1Byte(
-	struct dm_odm_t *pDM_Odm,
-	u32			RegAddr,
-	u8			Data
-	)
+void ODM_Write1Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr, u8 Data)
 {
 	struct rtw_adapter *Adapter = pDM_Odm->Adapter;
 
-	rtw_write8(Adapter, RegAddr, Data);
+	rtl8723au_write8(Adapter, RegAddr, Data);
 }
 
-void ODM_Write2Byte(
-	struct dm_odm_t *pDM_Odm,
-	u32			RegAddr,
-	u16			Data
-	)
+void ODM_Write2Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr, u16 Data)
 {
 	struct rtw_adapter *Adapter = pDM_Odm->Adapter;
 
-	rtw_write16(Adapter, RegAddr, Data);
+	rtl8723au_write16(Adapter, RegAddr, Data);
 }
 
-void ODM_Write4Byte(
-	struct dm_odm_t *pDM_Odm,
-	u32			RegAddr,
-	u32			Data
-	)
+void ODM_Write4Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr, u32 Data)
 {
 	struct rtw_adapter *Adapter = pDM_Odm->Adapter;
 
-	rtw_write32(Adapter, RegAddr, Data);
-
+	rtl8723au_write32(Adapter, RegAddr, Data);
 }
 
 void ODM_SetMACReg(
@@ -153,84 +137,3 @@ u32 ODM_GetRFReg(
 
 	return PHY_QueryRFReg(Adapter, eRFPath, RegAddr, BitMask);
 }
-
-/*  */
-/*  ODM Memory relative API. */
-/*  */
-void ODM_AllocateMemory(
-	struct dm_odm_t *pDM_Odm,
-	void **pPtr,
-	u32		length
-	)
-{
-	*pPtr = rtw_zvmalloc(length);
-}
-
-/*  length could be ignored, used to detect memory leakage. */
-void ODM_FreeMemory(
-	struct dm_odm_t *pDM_Odm,
-	void *pPtr,
-	u32		length
-	)
-{
-	rtw_vmfree(pPtr, length);
-}
-
-/*  */
-/*  ODM MISC relative API. */
-/*  */
-void
-ODM_AcquireSpinLock(
-	struct dm_odm_t *pDM_Odm,
-	enum rt_spinlock_type	type
-	)
-{
-}
-
-void ODM_ReleaseSpinLock(
-	struct dm_odm_t *pDM_Odm,
-	enum rt_spinlock_type	type
-	)
-{
-}
-
-/*  */
-/*  Work item relative API. FOr MP driver only~! */
-/*  */
-void ODM_InitializeWorkItem(
-	struct dm_odm_t *pDM_Odm,
-	void *pRtWorkItem,
-	RT_WORKITEM_CALL_BACK		RtWorkItemCallback,
-	void *pContext,
-	const char *szID
-	)
-{
-}
-
-/*  */
-/*  ODM Timer relative API. */
-/*  */
-void ODM_SetTimer(struct dm_odm_t *pDM_Odm, struct timer_list *pTimer, u32 msDelay)
-{
-	mod_timer(pTimer, jiffies + msecs_to_jiffies(msDelay)); /* ms */
-}
-
-void ODM_ReleaseTimer(struct dm_odm_t *pDM_Odm, struct timer_list *pTimer)
-{
-}
-
-/*  */
-/*  ODM FW relative API. */
-/*  */
-u32 ODM_FillH2CCmd(
-	u8 *pH2CBuffer,
-	u32		H2CBufferLen,
-	u32		CmdNum,
-	u32 *pElementID,
-	u32 *pCmdLen,
-	u8 **pCmbBuffer,
-	u8 *CmdStartSeq
-	)
-{
-	return	true;
-}
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_bt-coexist.c b/drivers/staging/rtl8723au/hal/rtl8723a_bt-coexist.c
index 9d738d79de4..c0010538909 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723a_bt-coexist.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723a_bt-coexist.c
@@ -15,11 +15,10 @@
 #include <drv_types.h>
 #include <rtl8723a_hal.h>
 #include <rtw_ioctl_set.h>
+#include <usb_ops_linux.h>
 
 #define DIS_PS_RX_BCN
 
-#ifdef CONFIG_8723AU_BT_COEXIST
-
 u32 BTCoexDbgLevel = _bt_dbg_off_;
 
 #define RTPRINT(_Comp, _Level, Fmt)\
@@ -70,7 +69,6 @@ if ((BTCoexDbgLevel == _bt_dbg_on_)) {\
 		printk(_TitleString);					\
 		printk(": %d, <%s>\n", _Len, buffer);			\
 	}
-#endif
 
 #define DCMD_Printf(...)
 #define RT_ASSERT(...)
@@ -100,7 +98,6 @@ enum {
 
 /*  power saving */
 
-#ifdef __BT_C__ /*  COMMOM/BT.c */
 /*  ===== Below this line is sync from SD7 driver COMMOM/BT.c ===== */
 
 static u8 BT_Operation(struct rtw_adapter *padapter)
@@ -138,14 +135,14 @@ void BT_SignalCompensation(struct rtw_adapter *padapter, u8 *rssi_wifi, u8 *rssi
 	BTDM_SignalCompensation(padapter, rssi_wifi, rssi_bt);
 }
 
-void BT_WifiScanNotify(struct rtw_adapter *padapter, u8 scanType)
+void rtl8723a_BT_wifiscan_notify(struct rtw_adapter *padapter, u8 scanType)
 {
 	BTHCI_WifiScanNotify(padapter, scanType);
 	BTDM_CheckAntSelMode(padapter);
 	BTDM_WifiScanNotify(padapter, scanType);
 }
 
-void BT_WifiAssociateNotify(struct rtw_adapter *padapter, u8 action)
+void rtl8723a_BT_wifiassociate_notify(struct rtw_adapter *padapter, u8 action)
 {
 	/*  action : */
 	/*  true = associate start */
@@ -156,36 +153,12 @@ void BT_WifiAssociateNotify(struct rtw_adapter *padapter, u8 action)
 	BTDM_WifiAssociateNotify(padapter, action);
 }
 
-void BT_WifiMediaStatusNotify(struct rtw_adapter *padapter, enum rt_media_status mstatus)
-{
-	BTDM_MediaStatusNotify(padapter, mstatus);
-}
-
-void BT_SpecialPacketNotify(struct rtw_adapter *padapter)
-{
-	BTDM_ForDhcp(padapter);
-}
-
 void BT_HaltProcess(struct rtw_adapter *padapter)
 {
 	BTDM_ForHalt(padapter);
 }
 
-void BT_LpsLeave(struct rtw_adapter *padapter)
-{
-	BTDM_LpsLeave(padapter);
-}
-
 /*  ===== End of sync from SD7 driver COMMOM/BT.c ===== */
-#endif
-
-#ifdef __BT_HANDLEPACKET_C__ /*  COMMOM/bt_handlepacket.c */
-/*  ===== Below this line is sync from SD7 driver COMMOM/bt_handlepacket.c ===== */
-
-/*  ===== End of sync from SD7 driver COMMOM/bt_handlepacket.c ===== */
-#endif
-
-#ifdef __BT_HCI_C__ /*  COMMOM/bt_hci.c */
 
 #define i64fmt		"ll"
 #define UINT64_C(v)  (v)
@@ -3944,7 +3917,6 @@ bthci_CmdAMPTestEnd(struct rtw_adapter *padapter,
 	enum hci_status status = HCI_STATUS_SUCCESS;
 	struct bt_30info *pBTInfo = GET_BT_INFO(padapter);
 	struct bt_hci_info *pBtHciInfo = &pBTInfo->BtHciInfo;
-	u8 bFilterOutNonAssociatedBSSID = true;
 
 	if (!pBtHciInfo->bInTestMode) {
 		RTPRINT(FIOCTL, (IOCTL_BT_EVENT|IOCTL_BT_LOGO), ("Not in Test mode, return status = HCI_STATUS_CMD_DISALLOW\n"));
@@ -3956,7 +3928,7 @@ bthci_CmdAMPTestEnd(struct rtw_adapter *padapter,
 
 	del_timer_sync(&pBTInfo->BTTestSendPacketTimer);
 
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_CHECK_BSSID, (u8 *)(&bFilterOutNonAssociatedBSSID));
+	rtl8723a_check_bssid(padapter, true);
 
 	/* send command complete event here when all data are received. */
 	{
@@ -4057,8 +4029,7 @@ bthci_CmdAMPTestCommand(struct rtw_adapter *padapter,
 			  jiffies + msecs_to_jiffies(50));
 		RTPRINT(FIOCTL, (IOCTL_BT_EVENT|IOCTL_BT_LOGO), ("TX Single Test \n"));
 	} else if (pBtHciInfo->TestScenario == 0x02) {
-		u8 bFilterOutNonAssociatedBSSID = false;
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_CHECK_BSSID, (u8 *)(&bFilterOutNonAssociatedBSSID));
+		rtl8723a_check_bssid(padapter, false);
 		RTPRINT(FIOCTL, (IOCTL_BT_EVENT|IOCTL_BT_LOGO), ("Receive Frame Test \n"));
 	}
 
@@ -4674,10 +4645,10 @@ bthci_StateConnected(struct rtw_adapter *padapter,
 
 		/*  for rate adaptive */
 
-		if (padapter->HalFunc.UpdateRAMaskHandler)
-			padapter->HalFunc.UpdateRAMaskHandler(padapter, MAX_FW_SUPPORT_MACID_NUM-1-EntryNum, 0);
+		rtl8723a_update_ramask(padapter,
+				       MAX_FW_SUPPORT_MACID_NUM-1-EntryNum, 0);
 
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_BASIC_RATE, padapter->mlmepriv.cur_network.network.SupportedRates);
+		HalSetBrateCfg23a(padapter, padapter->mlmepriv.cur_network.network.SupportedRates);
 		BTDM_SetFwChnlInfo(padapter, RT_MEDIA_CONNECT);
 		break;
 	default:
@@ -5215,9 +5186,6 @@ BTHCI_HandleHCICMD(
 }
 
 /*  ===== End of sync from SD7 driver COMMOM/bt_hci.c ===== */
-#endif
-
-#ifdef __HALBTC87231ANT_C__ /*  HAL/BTCoexist/HalBtc87231Ant.c */
 
 static const char *const BtStateString[] = {
 	"BT_DISABLED",
@@ -5286,7 +5254,7 @@ static void btdm_1AntTSFSwitch(struct rtw_adapter *padapter, u8 enable)
 {
 	u8 oldVal, newVal;
 
-	oldVal = rtw_read8(padapter, 0x550);
+	oldVal = rtl8723au_read8(padapter, 0x550);
 
 	if (enable)
 		newVal = oldVal | EN_BCN_FUNCTION;
@@ -5294,7 +5262,7 @@ static void btdm_1AntTSFSwitch(struct rtw_adapter *padapter, u8 enable)
 		newVal = oldVal & ~EN_BCN_FUNCTION;
 
 	if (oldVal != newVal)
-		rtw_write8(padapter, 0x550, newVal);
+		rtl8723au_write8(padapter, 0x550, newVal);
 }
 
 static u8 btdm_Is1AntPsTdmaStateChange(struct rtw_adapter *padapter)
@@ -5393,8 +5361,8 @@ btdm_1AntPsTdma(
 		case 29: /*  WiFi DHCP/Site Survey & BT ACL busy */
 			if (btdm_Is1AntPsTdmaStateChange(padapter)) {
 				BTDM_SetFw3a(padapter, 0xeb, 0x1a, 0x1a, 0x01, 0x18);
-				rtw_write32(padapter, 0x6c0, 0x5afa5afa);
-				rtw_write32(padapter, 0x6c4, 0x5afa5afa);
+				rtl8723au_write32(padapter, 0x6c0, 0x5afa5afa);
+				rtl8723au_write32(padapter, 0x6c4, 0x5afa5afa);
 			}
 			break;
 		case 30: /*  WiFi idle & BT Inquiry */
@@ -5437,7 +5405,8 @@ btdm_1AntPsTdma(
 				/*  Antenna control by PTA, 0x870 = 0x310 */
 				BTDM_SetFw3a(padapter, 0x0, 0x0, 0x0, 0x8, 0x0);
 			}
-			rtw_write16(padapter, 0x860, 0x210); /*  Switch Antenna to BT */
+			/*  Switch Antenna to BT */
+			rtl8723au_write16(padapter, 0x860, 0x210);
 			RTPRINT(FBT, BT_TRACE, ("[BTCoex], 0x860 = 0x210, Switch Antenna to BT\n"));
 			break;
 		case 9:
@@ -5445,7 +5414,8 @@ btdm_1AntPsTdma(
 				/*  Antenna control by PTA, 0x870 = 0x310 */
 				BTDM_SetFw3a(padapter, 0x0, 0x0, 0x0, 0x8, 0x0);
 			}
-			rtw_write16(padapter, 0x860, 0x110); /*  Switch Antenna to WiFi */
+			/*  Switch Antenna to WiFi */
+			rtl8723au_write16(padapter, 0x860, 0x110);
 			RTPRINT(FBT, BT_TRACE, ("[BTCoex], 0x860 = 0x110, Switch Antenna to WiFi\n"));
 			break;
 		}
@@ -5531,7 +5501,7 @@ _btdm_1AntSetPSTDMA(struct rtw_adapter *padapter, u8 bPSEn, u8 smartps,
 			if (!bTDMAOn) {
 				btdm_1AntPsTdma(padapter, false, tdmaType);
 			} else {
-				if ((BT_IsBtDisabled(padapter)) ||
+				if (!rtl8723a_BT_enabled(padapter) ||
 				    (pHalData->bt_coexist.halCoex8723.c2hBtInfo == BT_INFO_STATE_NO_CONNECTION) ||
 				    (pHalData->bt_coexist.halCoex8723.c2hBtInfo == BT_INFO_STATE_CONNECT_IDLE) ||
 				    (tdmaType == 29))
@@ -5575,24 +5545,28 @@ static void btdm_1AntWifiParaAdjust(struct rtw_adapter *padapter, u8 bEnable)
 static void btdm_1AntPtaParaReload(struct rtw_adapter *padapter)
 {
 	/*  PTA parameter */
-	rtw_write8(padapter, 0x6cc, 0x0);			/*  1-Ant coex */
-	rtw_write32(padapter, 0x6c8, 0xffff);		/*  wifi break table */
-	rtw_write32(padapter, 0x6c4, 0x55555555);	/*  coex table */
+	rtl8723au_write8(padapter, 0x6cc, 0x0);		/*  1-Ant coex */
+	rtl8723au_write32(padapter, 0x6c8, 0xffff);	/*  wifi break table */
+	rtl8723au_write32(padapter, 0x6c4, 0x55555555);	/*  coex table */
 
 	/*  Antenna switch control parameter */
-	rtw_write32(padapter, 0x858, 0xaaaaaaaa);
+	rtl8723au_write32(padapter, 0x858, 0xaaaaaaaa);
 	if (IS_8723A_A_CUT(GET_HAL_DATA(padapter)->VersionID)) {
-		rtw_write32(padapter, 0x870, 0x0);	/*  SPDT(connected with TRSW) control by hardware PTA */
-		rtw_write8(padapter, 0x40, 0x24);
+		/*  SPDT(connected with TRSW) control by hardware PTA */
+		rtl8723au_write32(padapter, 0x870, 0x0);
+		rtl8723au_write8(padapter, 0x40, 0x24);
 	} else {
-		rtw_write8(padapter, 0x40, 0x20);
-		rtw_write16(padapter, 0x860, 0x210);	/*  set antenna at bt side if ANTSW is software control */
-		rtw_write32(padapter, 0x870, 0x300);	/*  SPDT(connected with TRSW) control by hardware PTA */
-		rtw_write32(padapter, 0x874, 0x22804000);	/*  ANTSW keep by GNT_BT */
+		rtl8723au_write8(padapter, 0x40, 0x20);
+		/*  set antenna at bt side if ANTSW is software control */
+		rtl8723au_write16(padapter, 0x860, 0x210);
+		/*  SPDT(connected with TRSW) control by hardware PTA */
+		rtl8723au_write32(padapter, 0x870, 0x300);
+		/*  ANTSW keep by GNT_BT */
+		rtl8723au_write32(padapter, 0x874, 0x22804000);
 	}
 
 	/*  coexistence parameters */
-	rtw_write8(padapter, 0x778, 0x1);	/*  enable RTK mode PTA */
+	rtl8723au_write8(padapter, 0x778, 0x1);	/*  enable RTK mode PTA */
 
 	/*  BT don't ignore WLAN_Act */
 	btdm_SetFwIgnoreWlanAct(padapter, false);
@@ -5764,13 +5738,17 @@ static void btdm_1AntCoexProcessForWifiConnect(struct rtw_adapter *padapter)
 	pBtdm8723 = &pBtCoex->btdm1Ant;
 	BtState = pBtCoex->c2hBtInfo;
 
-	RTPRINT(FBT, BT_TRACE, ("[BTCoex], WiFi is %s\n", BTDM_IsWifiBusy(padapter)?"Busy":"IDLE"));
-	RTPRINT(FBT, BT_TRACE, ("[BTCoex], BT is %s\n", BtStateString[BtState]));
+	RTPRINT(FBT, BT_TRACE, ("[BTCoex], WiFi is %s\n",
+				BTDM_IsWifiBusy(padapter)?"Busy":"IDLE"));
+	RTPRINT(FBT, BT_TRACE, ("[BTCoex], BT is %s\n",
+				BtStateString[BtState]));
 
 	padapter->pwrctrlpriv.btcoex_rfon = false;
 
-	if ((!BTDM_IsWifiBusy(padapter)) && (!check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE)) &&
-	    ((BtState == BT_INFO_STATE_NO_CONNECTION) || (BtState == BT_INFO_STATE_CONNECT_IDLE))) {
+	if (!BTDM_IsWifiBusy(padapter) &&
+	    !check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE) &&
+	    (BtState == BT_INFO_STATE_NO_CONNECTION ||
+	     BtState == BT_INFO_STATE_CONNECT_IDLE)) {
 		switch (BtState) {
 		case BT_INFO_STATE_NO_CONNECTION:
 			_btdm_1AntSetPSTDMA(padapter, true, 2, 0x26, false, 9);
@@ -5785,45 +5763,59 @@ static void btdm_1AntCoexProcessForWifiConnect(struct rtw_adapter *padapter)
 		case BT_INFO_STATE_CONNECT_IDLE:
 			/*  WiFi is Busy */
 			btdm_1AntSetPSTDMA(padapter, false, 0, true, 5);
-			rtw_write32(padapter, 0x6c0, 0x5a5a5a5a);
-			rtw_write32(padapter, 0x6c4, 0x5a5a5a5a);
+			rtl8723au_write32(padapter, 0x6c0, 0x5a5a5a5a);
+			rtl8723au_write32(padapter, 0x6c4, 0x5a5a5a5a);
 			break;
 		case BT_INFO_STATE_ACL_INQ_OR_PAG:
-			RTPRINT(FBT, BT_TRACE, ("[BTCoex], BT PROFILE is BT_INFO_STATE_ACL_INQ_OR_PAG\n"));
+			RTPRINT(FBT, BT_TRACE,
+				("[BTCoex], BT PROFILE is "
+				 "BT_INFO_STATE_ACL_INQ_OR_PAG\n"));
 		case BT_INFO_STATE_INQ_OR_PAG:
 			padapter->pwrctrlpriv.btcoex_rfon = true;
 			btdm_1AntSetPSTDMA(padapter, true, 0, true, 30);
 			break;
 		case BT_INFO_STATE_SCO_ONLY_BUSY:
 		case BT_INFO_STATE_ACL_SCO_BUSY:
-			if (true == pBtCoex->bC2hBtInquiryPage) {
-				btdm_1AntSetPSTDMA(padapter, false, 0, true, 32);
-			} else {
+			if (true == pBtCoex->bC2hBtInquiryPage)
+				btdm_1AntSetPSTDMA(padapter, false, 0,
+						   true, 32);
+			else {
 #ifdef BTCOEX_CMCC_TEST
-				btdm_1AntSetPSTDMA(padapter, false, 0, true, 23);
+				btdm_1AntSetPSTDMA(padapter, false, 0,
+						   true, 23);
 #else /*  !BTCOEX_CMCC_TEST */
-				btdm_1AntSetPSTDMA(padapter, false, 0, false, 8);
-				rtw_write32(padapter, 0x6c0, 0x5a5a5a5a);
-				rtw_write32(padapter, 0x6c4, 0x5a5a5a5a);
+				btdm_1AntSetPSTDMA(padapter, false, 0,
+						   false, 8);
+				rtl8723au_write32(padapter, 0x6c0, 0x5a5a5a5a);
+				rtl8723au_write32(padapter, 0x6c4, 0x5a5a5a5a);
 #endif /*  !BTCOEX_CMCC_TEST */
 			}
 			break;
 		case BT_INFO_STATE_ACL_ONLY_BUSY:
 			padapter->pwrctrlpriv.btcoex_rfon = true;
 			if (pBtCoex->c2hBtProfile == BT_INFO_HID) {
-				RTPRINT(FBT, BT_TRACE, ("[BTCoex], BT PROFILE is HID\n"));
+				RTPRINT(FBT, BT_TRACE,
+					("[BTCoex], BT PROFILE is HID\n"));
 				btdm_1AntSetPSTDMA(padapter, true, 0, true, 31);
 			} else if (pBtCoex->c2hBtProfile == BT_INFO_FTP) {
-				RTPRINT(FBT, BT_TRACE, ("[BTCoex], BT PROFILE is FTP/OPP\n"));
+				RTPRINT(FBT, BT_TRACE,
+					("[BTCoex], BT PROFILE is FTP/OPP\n"));
 				btdm_1AntSetPSTDMA(padapter, true, 0, true, 3);
 			} else if (pBtCoex->c2hBtProfile == (BT_INFO_A2DP|BT_INFO_FTP)) {
-				RTPRINT(FBT, BT_TRACE, ("[BTCoex], BT PROFILE is A2DP_FTP\n"));
+				RTPRINT(FBT, BT_TRACE,
+					("[BTCoex], BT PROFILE is A2DP_FTP\n"));
 				btdm_1AntSetPSTDMA(padapter, true, 0, true, 11);
 			} else {
 				if (pBtCoex->c2hBtProfile == BT_INFO_A2DP)
-					RTPRINT(FBT, BT_TRACE, ("[BTCoex], BT PROFILE is A2DP\n"));
+					RTPRINT(FBT, BT_TRACE,
+						("[BTCoex], BT PROFILE is "
+						 "A2DP\n"));
 				else
-					RTPRINT(FBT, BT_TRACE, ("[BTCoex], BT PROFILE is UNKNOWN(0x%02X)! Use A2DP Profile\n", pBtCoex->c2hBtProfile));
+					RTPRINT(FBT, BT_TRACE,
+						("[BTCoex], BT PROFILE is "
+						 "UNKNOWN(0x%02X)! Use A2DP "
+						 "Profile\n",
+						 pBtCoex->c2hBtProfile));
 				btdm_1AntTdmaDurationAdjustForACL(padapter);
 			}
 			break;
@@ -5833,13 +5825,14 @@ static void btdm_1AntCoexProcessForWifiConnect(struct rtw_adapter *padapter)
 	pBtdm8723->psTdmaGlobalCnt++;
 }
 
-static void btdm_1AntUpdateHalRAMask(struct rtw_adapter *padapter, u32 mac_id, u32 filter)
+static void
+btdm_1AntUpdateHalRAMask(struct rtw_adapter *padapter, u32 mac_id, u32 filter)
 {
 	u8 init_rate = 0;
 	u8 raid;
 	u32 mask;
 	u8 shortGIrate = false;
-	int	supportRateNum = 0;
+	int supportRateNum = 0;
 	struct sta_info	*psta;
 	struct hal_data_8723a *pHalData;
 	struct dm_priv *pdmpriv;
@@ -5847,7 +5840,8 @@ static void btdm_1AntUpdateHalRAMask(struct rtw_adapter *padapter, u32 mac_id, u
 	struct mlme_ext_info *pmlmeinfo;
 	struct wlan_bssid_ex *cur_network;
 
-	RTPRINT(FBT, BT_TRACE, ("[BTCoex], %s, MACID =%d, filter = 0x%08x!!\n", __func__, mac_id, filter));
+	RTPRINT(FBT, BT_TRACE, ("[BTCoex], %s, MACID =%d, filter = 0x%08x!!\n",
+				__func__, mac_id, filter));
 
 	pHalData = GET_HAL_DATA(padapter);
 	pdmpriv = &pHalData->dmpriv;
@@ -5856,13 +5850,15 @@ static void btdm_1AntUpdateHalRAMask(struct rtw_adapter *padapter, u32 mac_id, u
 	cur_network = &pmlmeinfo->network;
 
 	if (mac_id >= NUM_STA) { /* CAM_SIZE */
-		RTPRINT(FBT, BT_TRACE, ("[BTCoex], %s, MACID =%d illegal!!\n", __func__, mac_id));
+		RTPRINT(FBT, BT_TRACE, ("[BTCoex], %s, MACID =%d illegal!!\n",
+					__func__, mac_id));
 		return;
 	}
 
 	psta = pmlmeinfo->FW_sta_info[mac_id].psta;
-	if (psta == NULL) {
-		RTPRINT(FBT, BT_TRACE, ("[BTCoex], %s, Can't find station!!\n", __func__));
+	if (!psta) {
+		RTPRINT(FBT, BT_TRACE, ("[BTCoex], %s, Can't find station!!\n",
+					__func__));
 		return;
 	}
 
@@ -5870,19 +5866,26 @@ static void btdm_1AntUpdateHalRAMask(struct rtw_adapter *padapter, u32 mac_id, u
 
 	switch (mac_id) {
 	case 0:/*  for infra mode */
-		supportRateNum = rtw_get_rateset_len23a(cur_network->SupportedRates);
-		mask = update_supported_rate23a(cur_network->SupportedRates, supportRateNum);
-		mask |= (pmlmeinfo->HT_enable) ? update_MSC_rate23a(&pmlmeinfo->HT_caps):0;
+		supportRateNum =
+			rtw_get_rateset_len23a(cur_network->SupportedRates);
+		mask = update_supported_rate23a(cur_network->SupportedRates,
+						supportRateNum);
+		mask |= (pmlmeinfo->HT_enable) ?
+			update_MSC_rate23a(&pmlmeinfo->HT_caps):0;
 		if (support_short_GI23a(padapter, &pmlmeinfo->HT_caps))
 			shortGIrate = true;
 		break;
 	case 1:/* for broadcast/multicast */
-		supportRateNum = rtw_get_rateset_len23a(pmlmeinfo->FW_sta_info[mac_id].SupportedRates);
-		mask = update_basic_rate23a(cur_network->SupportedRates, supportRateNum);
+		supportRateNum = rtw_get_rateset_len23a(
+			pmlmeinfo->FW_sta_info[mac_id].SupportedRates);
+		mask = update_basic_rate23a(cur_network->SupportedRates,
+					    supportRateNum);
 		break;
 	default: /* for each sta in IBSS */
-		supportRateNum = rtw_get_rateset_len23a(pmlmeinfo->FW_sta_info[mac_id].SupportedRates);
-		mask = update_supported_rate23a(cur_network->SupportedRates, supportRateNum);
+		supportRateNum = rtw_get_rateset_len23a(
+			pmlmeinfo->FW_sta_info[mac_id].SupportedRates);
+		mask = update_supported_rate23a(cur_network->SupportedRates,
+						supportRateNum);
 		break;
 	}
 	mask |= ((raid<<28)&0xf0000000);
@@ -5899,22 +5902,24 @@ static void btdm_1AntUpdateHalRAMask(struct rtw_adapter *padapter, u32 mac_id, u
 			arg |= BIT(5);
 
 		RTPRINT(FBT, BT_TRACE,
-			("[BTCoex], Update FW RAID entry, MASK = 0x%08x, arg = 0x%02x\n",
-			mask, arg));
+			("[BTCoex], Update FW RAID entry, MASK = 0x%08x, "
+			 "arg = 0x%02x\n", mask, arg));
 
 		rtl8723a_set_raid_cmd(padapter, mask, arg);
 	} else {
 		if (shortGIrate)
 			init_rate |= BIT(6);
 
-		rtw_write8(padapter, (REG_INIDATA_RATE_SEL+mac_id), init_rate);
+		rtl8723au_write8(padapter, REG_INIDATA_RATE_SEL + mac_id,
+				 init_rate);
 	}
 
 	psta->init_rate = init_rate;
 	pdmpriv->INIDATA_RATE[mac_id] = init_rate;
 }
 
-static void btdm_1AntUpdateHalRAMaskForSCO(struct rtw_adapter *padapter, u8 forceUpdate)
+static void
+btdm_1AntUpdateHalRAMaskForSCO(struct rtw_adapter *padapter, u8 forceUpdate)
 {
 	struct btdm_8723a_1ant *pBtdm8723;
 	struct sta_priv *pstapriv;
@@ -5925,7 +5930,7 @@ static void btdm_1AntUpdateHalRAMaskForSCO(struct rtw_adapter *padapter, u8 forc
 
 	pBtdm8723 = &GET_HAL_DATA(padapter)->bt_coexist.halCoex8723.btdm1Ant;
 
-	if ((pBtdm8723->bRAChanged == true) && (forceUpdate == false))
+	if (pBtdm8723->bRAChanged == true && forceUpdate == false)
 		return;
 
 	pstapriv = &padapter->stapriv;
@@ -5968,9 +5973,13 @@ static void btdm_1AntRecoverHalRAMask(struct rtw_adapter *padapter)
 
 static void
 btdm_1AntBTStateChangeHandler(struct rtw_adapter *padapter,
-			      enum bt_state_1ant oldState, enum bt_state_1ant newState)
+			      enum bt_state_1ant oldState,
+			      enum bt_state_1ant newState)
 {
-	RTPRINT(FBT, BT_TRACE, ("[BTCoex], BT state change, %s => %s\n", BtStateString[oldState], BtStateString[newState]));
+	struct hal_data_8723a *phaldata;
+	RTPRINT(FBT, BT_TRACE, ("[BTCoex], BT state change, %s => %s\n",
+				BtStateString[oldState],
+				BtStateString[newState]));
 
 	/*  BT default ignore wlan active, */
 	/*  WiFi MUST disable this when BT is enable */
@@ -5987,22 +5996,23 @@ btdm_1AntBTStateChangeHandler(struct rtw_adapter *padapter,
 			btdm_1AntRecoverHalRAMask(padapter);
 		}
 	} else {
-		GET_HAL_DATA(padapter)->bt_coexist.halCoex8723.btdm1Ant.bRAChanged = false;
+		phaldata = GET_HAL_DATA(padapter);
+		phaldata->bt_coexist.halCoex8723.btdm1Ant.bRAChanged = false;
 	}
 
 	if (oldState == newState)
 		return;
 
 	if (oldState == BT_INFO_STATE_ACL_ONLY_BUSY) {
-		struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
-		pHalData->bt_coexist.halCoex8723.btdm1Ant.psTdmaMonitorCnt = 0;
-		pHalData->bt_coexist.halCoex8723.btdm1Ant.psTdmaMonitorCntForSCO = 0;
+		struct hal_data_8723a *Hal = GET_HAL_DATA(padapter);
+		Hal->bt_coexist.halCoex8723.btdm1Ant.psTdmaMonitorCnt = 0;
+		Hal->bt_coexist.halCoex8723.btdm1Ant.psTdmaMonitorCntForSCO = 0;
 	}
 
 	if ((oldState == BT_INFO_STATE_SCO_ONLY_BUSY) ||
 	    (oldState == BT_INFO_STATE_ACL_SCO_BUSY)) {
-		struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
-		pHalData->bt_coexist.halCoex8723.btdm1Ant.psTdmaMonitorCntForSCO = 0;
+		struct hal_data_8723a *Hal = GET_HAL_DATA(padapter);
+		Hal->bt_coexist.halCoex8723.btdm1Ant.psTdmaMonitorCntForSCO = 0;
 	}
 
 	/*  Active 2Ant mechanism when BT Connected */
@@ -6010,14 +6020,16 @@ btdm_1AntBTStateChangeHandler(struct rtw_adapter *padapter,
 	    (oldState == BT_INFO_STATE_NO_CONNECTION)) {
 		if ((newState != BT_INFO_STATE_DISABLED) &&
 		    (newState != BT_INFO_STATE_NO_CONNECTION)) {
-			BTDM_SetSwRfRxLpfCorner(padapter, BT_RF_RX_LPF_CORNER_SHRINK);
+			BTDM_SetSwRfRxLpfCorner(padapter,
+						BT_RF_RX_LPF_CORNER_SHRINK);
 			BTDM_AGCTable(padapter, BT_AGCTABLE_ON);
 			BTDM_BBBackOffLevel(padapter, BT_BB_BACKOFF_ON);
 		}
 	} else {
 		if ((newState == BT_INFO_STATE_DISABLED) ||
 		    (newState == BT_INFO_STATE_NO_CONNECTION)) {
-			BTDM_SetSwRfRxLpfCorner(padapter, BT_RF_RX_LPF_CORNER_RESUME);
+			BTDM_SetSwRfRxLpfCorner(padapter,
+						BT_RF_RX_LPF_CORNER_RESUME);
 			BTDM_AGCTable(padapter, BT_AGCTABLE_OFF);
 			BTDM_BBBackOffLevel(padapter, BT_BB_BACKOFF_OFF);
 		}
@@ -6034,21 +6046,27 @@ static void btdm_1AntBtCoexistHandler(struct rtw_adapter *padapter)
 	pBtCoex8723 = &pHalData->bt_coexist.halCoex8723;
 	pBtdm8723 = &pBtCoex8723->btdm1Ant;
 	padapter->pwrctrlpriv.btcoex_rfon = false;
-	if (BT_IsBtDisabled(padapter)) {
+	if (!rtl8723a_BT_enabled(padapter)) {
 		RTPRINT(FBT, BT_TRACE, ("[BTCoex], BT is disabled\n"));
 
 		if (BTDM_IsWifiConnectionExist(padapter)) {
-			RTPRINT(FBT, BT_TRACE, ("[BTCoex], wifi is connected\n"));
+			RTPRINT(FBT, BT_TRACE,
+				("[BTCoex], wifi is connected\n"));
 
 			if (BTDM_IsWifiBusy(padapter)) {
-				RTPRINT(FBT, BT_TRACE, ("[BTCoex], Wifi is busy\n"));
-				btdm_1AntSetPSTDMA(padapter, false, 0, false, 9);
+				RTPRINT(FBT, BT_TRACE,
+					("[BTCoex], Wifi is busy\n"));
+				btdm_1AntSetPSTDMA(padapter, false, 0,
+						   false, 9);
 			} else {
-				RTPRINT(FBT, BT_TRACE, ("[BTCoex], Wifi is idle\n"));
-				_btdm_1AntSetPSTDMA(padapter, true, 2, 1, false, 9);
+				RTPRINT(FBT, BT_TRACE,
+					("[BTCoex], Wifi is idle\n"));
+				_btdm_1AntSetPSTDMA(padapter, true, 2, 1,
+						    false, 9);
 			}
 		} else {
-			RTPRINT(FBT, BT_TRACE, ("[BTCoex], wifi is disconnected\n"));
+			RTPRINT(FBT, BT_TRACE,
+				("[BTCoex], wifi is disconnected\n"));
 
 			btdm_1AntSetPSTDMA(padapter, false, 0, false, 9);
 		}
@@ -6056,24 +6074,29 @@ static void btdm_1AntBtCoexistHandler(struct rtw_adapter *padapter)
 		RTPRINT(FBT, BT_TRACE, ("[BTCoex], BT is enabled\n"));
 
 		if (BTDM_IsWifiConnectionExist(padapter)) {
-			RTPRINT(FBT, BT_TRACE, ("[BTCoex], wifi is connected\n"));
+			RTPRINT(FBT, BT_TRACE,
+				("[BTCoex], wifi is connected\n"));
 
 			btdm_1AntWifiParaAdjust(padapter, true);
 			btdm_1AntCoexProcessForWifiConnect(padapter);
 		} else {
-			RTPRINT(FBT, BT_TRACE, ("[BTCoex], wifi is disconnected\n"));
+			RTPRINT(FBT, BT_TRACE,
+				("[BTCoex], wifi is disconnected\n"));
 
-			/*  Antenna switch at BT side(0x870 = 0x300, 0x860 = 0x210) after PSTDMA off */
+			/*  Antenna switch at BT side(0x870 = 0x300,
+			    0x860 = 0x210) after PSTDMA off */
 			btdm_1AntWifiParaAdjust(padapter, false);
 			btdm_1AntSetPSTDMA(padapter, false, 0, false, 0);
 		}
 	}
 
-	btdm_1AntBTStateChangeHandler(padapter, pBtCoex8723->prec2hBtInfo, pBtCoex8723->c2hBtInfo);
+	btdm_1AntBTStateChangeHandler(padapter, pBtCoex8723->prec2hBtInfo,
+				      pBtCoex8723->c2hBtInfo);
 	pBtCoex8723->prec2hBtInfo = pBtCoex8723->c2hBtInfo;
 }
 
-void BTDM_1AntSignalCompensation(struct rtw_adapter *padapter, u8 *rssi_wifi, u8 *rssi_bt)
+void BTDM_1AntSignalCompensation(struct rtw_adapter *padapter,
+				 u8 *rssi_wifi, u8 *rssi_bt)
 {
 	struct hal_data_8723a *pHalData;
 	struct btdm_8723a_1ant *pBtdm8723;
@@ -6119,14 +6142,19 @@ void BTDM_1AntSignalCompensation(struct rtw_adapter *padapter, u8 *rssi_wifi, u8
 	}
 
 	if (rssi_wifi && RSSI_WiFi_Cmpnstn) {
-		RTPRINT(FBT, BT_TRACE, ("[BTCoex], 1AntSgnlCmpnstn, case %d, WiFiCmpnstn =%d(%d => %d)\n",
-				pBtdm8723->curPsTdma, RSSI_WiFi_Cmpnstn, *rssi_wifi, *rssi_wifi+RSSI_WiFi_Cmpnstn));
+		RTPRINT(FBT, BT_TRACE,
+			("[BTCoex], 1AntSgnlCmpnstn, case %d, WiFiCmpnstn "
+			 "=%d(%d => %d)\n", pBtdm8723->curPsTdma,
+			 RSSI_WiFi_Cmpnstn, *rssi_wifi,
+			 *rssi_wifi+RSSI_WiFi_Cmpnstn));
 		*rssi_wifi += RSSI_WiFi_Cmpnstn;
 	}
 
 	if (rssi_bt && RSSI_BT_Cmpnstn) {
-		RTPRINT(FBT, BT_TRACE, ("[BTCoex], 1AntSgnlCmpnstn, case %d, BTCmpnstn =%d(%d => %d)\n",
-				pBtdm8723->curPsTdma, RSSI_BT_Cmpnstn, *rssi_bt, *rssi_bt+RSSI_BT_Cmpnstn));
+		RTPRINT(FBT, BT_TRACE,
+			("[BTCoex], 1AntSgnlCmpnstn, case %d, BTCmpnstn "
+			 "=%d(%d => %d)\n", pBtdm8723->curPsTdma,
+			 RSSI_BT_Cmpnstn, *rssi_bt, *rssi_bt+RSSI_BT_Cmpnstn));
 		*rssi_bt += RSSI_BT_Cmpnstn;
 	}
 }
@@ -6142,7 +6170,7 @@ static void BTDM_1AntParaInit(struct rtw_adapter *padapter)
 	pBtdm8723 = &pBtCoex->btdm1Ant;
 
 	/*  Enable counter statistics */
-	rtw_write8(padapter, 0x76e, 0x4);
+	rtl8723au_write8(padapter, 0x76e, 0x4);
 	btdm_1AntPtaParaReload(padapter);
 
 	pBtdm8723->wifiRssiThresh = 48;
@@ -6162,7 +6190,8 @@ static void BTDM_1AntForHalt(struct rtw_adapter *padapter)
 {
 	RTPRINT(FBT, BT_TRACE, ("\n[BTCoex], 1Ant for halt\n"));
 
-	GET_HAL_DATA(padapter)->bt_coexist.halCoex8723.btdm1Ant.bWiFiHalt = true;
+	GET_HAL_DATA(padapter)->bt_coexist.halCoex8723.btdm1Ant.bWiFiHalt =
+		true;
 
 	btdm_1AntWifiParaAdjust(padapter, false);
 
@@ -6180,7 +6209,8 @@ static void BTDM_1AntLpsLeave(struct rtw_adapter *padapter)
 	RTPRINT(FBT, BT_TRACE, ("\n[BTCoex], 1Ant for LPS Leave\n"));
 
 	/*  Prevent from entering LPS again */
-	GET_HAL_DATA(padapter)->bt_coexist.halCoex8723.btdm1Ant.bWiFiHalt = true;
+	GET_HAL_DATA(padapter)->bt_coexist.halCoex8723.btdm1Ant.bWiFiHalt =
+		true;
 
 	btdm_1AntSetPSTDMA(padapter, false, 0, false, 8);
 /*btdm_1AntPsTdma(padapter, false, 8); */
@@ -6190,13 +6220,14 @@ static void BTDM_1AntWifiAssociateNotify(struct rtw_adapter *padapter, u8 type)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
-	RTPRINT(FBT, BT_TRACE, ("\n[BTCoex], 1Ant for associate, type =%d\n", type));
+	RTPRINT(FBT, BT_TRACE,
+		("\n[BTCoex], 1Ant for associate, type =%d\n", type));
 
 	if (type) {
 		rtl8723a_CheckAntenna_Selection(padapter);
-		if (BT_IsBtDisabled(padapter)) {
+		if (!rtl8723a_BT_enabled(padapter))
 			btdm_1AntSetPSTDMA(padapter, false, 0, false, 9);
-		} else {
+		else {
 			struct bt_coexist_8723a *pBtCoex;
 			u8 BtState;
 
@@ -6205,24 +6236,28 @@ static void BTDM_1AntWifiAssociateNotify(struct rtw_adapter *padapter, u8 type)
 
 			btdm_1AntTSFSwitch(padapter, true);
 
-			if ((BtState == BT_INFO_STATE_NO_CONNECTION) ||
-			    (BtState == BT_INFO_STATE_CONNECT_IDLE)) {
-				btdm_1AntSetPSTDMA(padapter, false, 0, true, 28);
-			} else if ((BtState == BT_INFO_STATE_SCO_ONLY_BUSY) ||
-				   (BtState == BT_INFO_STATE_ACL_SCO_BUSY)) {
-				btdm_1AntSetPSTDMA(padapter, false, 0, false, 8);
-				rtw_write32(padapter, 0x6c0, 0x5a5a5a5a);
-				rtw_write32(padapter, 0x6c4, 0x5a5a5a5a);
-			} else if ((BtState == BT_INFO_STATE_ACL_ONLY_BUSY) ||
-				   (BtState == BT_INFO_STATE_ACL_INQ_OR_PAG)) {
+			if (BtState == BT_INFO_STATE_NO_CONNECTION ||
+			    BtState == BT_INFO_STATE_CONNECT_IDLE) {
+				btdm_1AntSetPSTDMA(padapter, false, 0,
+						   true, 28);
+			} else if (BtState == BT_INFO_STATE_SCO_ONLY_BUSY ||
+				   BtState == BT_INFO_STATE_ACL_SCO_BUSY) {
+				btdm_1AntSetPSTDMA(padapter, false, 0,
+						   false, 8);
+				rtl8723au_write32(padapter, 0x6c0, 0x5a5a5a5a);
+				rtl8723au_write32(padapter, 0x6c4, 0x5a5a5a5a);
+			} else if (BtState == BT_INFO_STATE_ACL_ONLY_BUSY ||
+				   BtState == BT_INFO_STATE_ACL_INQ_OR_PAG) {
 				if (pBtCoex->c2hBtProfile == BT_INFO_HID)
-					btdm_1AntSetPSTDMA(padapter, false, 0, true, 35);
+					btdm_1AntSetPSTDMA(padapter, false, 0,
+							   true, 35);
 				else
-					btdm_1AntSetPSTDMA(padapter, false, 0, true, 29);
+					btdm_1AntSetPSTDMA(padapter, false, 0,
+							   true, 29);
 			}
 		}
 	} else {
-		if (BT_IsBtDisabled(padapter)) {
+		if (!rtl8723a_BT_enabled(padapter)) {
 			if (!BTDM_IsWifiConnectionExist(padapter)) {
 				btdm_1AntPsTdma(padapter, false, 0);
 				btdm_1AntTSFSwitch(padapter, false);
@@ -6241,22 +6276,24 @@ BTDM_1AntMediaStatusNotify(struct rtw_adapter *padapter,
 
 	pBtCoex = &GET_HAL_DATA(padapter)->bt_coexist.halCoex8723;
 
-	RTPRINT(FBT, BT_TRACE, ("\n\n[BTCoex]******************************\n"));
+	RTPRINT(FBT, BT_TRACE,
+		("\n\n[BTCoex]******************************\n"));
 	RTPRINT(FBT, BT_TRACE, ("[BTCoex], MediaStatus, WiFi %s !!\n",
 			mstatus == RT_MEDIA_CONNECT?"CONNECT":"DISCONNECT"));
 	RTPRINT(FBT, BT_TRACE, ("[BTCoex]******************************\n"));
 
 	if (RT_MEDIA_CONNECT == mstatus) {
 		if (check_fwstate(&padapter->mlmepriv, WIFI_STATION_STATE)) {
-			if ((pBtCoex->c2hBtInfo == BT_INFO_STATE_SCO_ONLY_BUSY) ||
-			    (pBtCoex->c2hBtInfo == BT_INFO_STATE_ACL_SCO_BUSY))
+			if (pBtCoex->c2hBtInfo == BT_INFO_STATE_SCO_ONLY_BUSY ||
+			    pBtCoex->c2hBtInfo == BT_INFO_STATE_ACL_SCO_BUSY)
 				btdm_1AntUpdateHalRAMaskForSCO(padapter, true);
 		}
 
 		padapter->pwrctrlpriv.DelayLPSLastTimeStamp = jiffies;
 		BTDM_1AntForDhcp(padapter);
 	} else {
-		/* DBG_8723A("%s rtl8723a_DeinitAntenna_Selection\n", __func__); */
+		/* DBG_8723A("%s rtl8723a_DeinitAntenna_Selection\n",
+		   __func__); */
 		rtl8723a_DeinitAntenna_Selection(padapter);
 		btdm_1AntBtCoexistHandler(padapter);
 		pBtCoex->btdm1Ant.bRAChanged = false;
@@ -6276,8 +6313,10 @@ void BTDM_1AntForDhcp(struct rtw_adapter *padapter)
 	pBtdm8723 = &pBtCoex->btdm1Ant;
 
 	RTPRINT(FBT, BT_TRACE, ("\n[BTCoex], 1Ant for DHCP\n"));
-	RTPRINT(FBT, BT_TRACE, ("[BTCoex], 1Ant for DHCP, WiFi is %s\n", BTDM_IsWifiBusy(padapter)?"Busy":"IDLE"));
-	RTPRINT(FBT, BT_TRACE, ("[BTCoex], 1Ant for DHCP, %s\n", BtStateString[BtState]));
+	RTPRINT(FBT, BT_TRACE, ("[BTCoex], 1Ant for DHCP, WiFi is %s\n",
+				BTDM_IsWifiBusy(padapter)?"Busy":"IDLE"));
+	RTPRINT(FBT, BT_TRACE, ("[BTCoex], 1Ant for DHCP, %s\n",
+				BtStateString[BtState]));
 
 	BTDM_1AntWifiAssociateNotify(padapter, true);
 }
@@ -6294,13 +6333,16 @@ static void BTDM_1AntWifiScanNotify(struct rtw_adapter *padapter, u8 scanType)
 	pBtCoex = &pHalData->bt_coexist.halCoex8723;
 	pBtdm8723 = &pBtCoex->btdm1Ant;
 
-	RTPRINT(FBT, BT_TRACE, ("\n[BTCoex], 1Ant for wifi scan =%d!!\n", scanType));
-	RTPRINT(FBT, BT_TRACE, ("[BTCoex], 1Ant for wifi scan, WiFi is %s\n", BTDM_IsWifiBusy(padapter)?"Busy":"IDLE"));
-	RTPRINT(FBT, BT_TRACE, ("[BTCoex], 1Ant for wifi scan, %s\n", BtStateString[BtState]));
+	RTPRINT(FBT, BT_TRACE, ("\n[BTCoex], 1Ant for wifi scan =%d!!\n",
+				scanType));
+	RTPRINT(FBT, BT_TRACE, ("[BTCoex], 1Ant for wifi scan, WiFi is %s\n",
+				BTDM_IsWifiBusy(padapter)?"Busy":"IDLE"));
+	RTPRINT(FBT, BT_TRACE, ("[BTCoex], 1Ant for wifi scan, %s\n",
+				BtStateString[BtState]));
 
 	if (scanType) {
 		rtl8723a_CheckAntenna_Selection(padapter);
-		if (BT_IsBtDisabled(padapter)) {
+		if (!rtl8723a_BT_enabled(padapter)) {
 			btdm_1AntSetPSTDMA(padapter, false, 0, false, 9);
 		} else if (BTDM_IsWifiConnectionExist(padapter) == false) {
 			BTDM_1AntWifiAssociateNotify(padapter, true);
@@ -6308,10 +6350,13 @@ static void BTDM_1AntWifiScanNotify(struct rtw_adapter *padapter, u8 scanType)
 			if ((BtState == BT_INFO_STATE_SCO_ONLY_BUSY) ||
 			    (BtState == BT_INFO_STATE_ACL_SCO_BUSY)) {
 				if (pBtCoex->bC2hBtInquiryPage) {
-					btdm_1AntSetPSTDMA(padapter, false, 0, true, 32);
+					btdm_1AntSetPSTDMA(padapter, false, 0,
+							   true, 32);
 				} else {
-					padapter->pwrctrlpriv.btcoex_rfon = true;
-					btdm_1AntSetPSTDMA(padapter, true, 0, true, 33);
+					padapter->pwrctrlpriv.btcoex_rfon =
+						true;
+					btdm_1AntSetPSTDMA(padapter, true, 0,
+							   true, 33);
 				}
 			} else if (true == pBtCoex->bC2hBtInquiryPage) {
 				padapter->pwrctrlpriv.btcoex_rfon = true;
@@ -6319,9 +6364,11 @@ static void BTDM_1AntWifiScanNotify(struct rtw_adapter *padapter, u8 scanType)
 			} else if (BtState == BT_INFO_STATE_ACL_ONLY_BUSY) {
 				padapter->pwrctrlpriv.btcoex_rfon = true;
 				if (pBtCoex->c2hBtProfile == BT_INFO_HID)
-					btdm_1AntSetPSTDMA(padapter, true, 0, true, 34);
+					btdm_1AntSetPSTDMA(padapter, true, 0,
+							   true, 34);
 				else
-					btdm_1AntSetPSTDMA(padapter, true, 0, true, 4);
+					btdm_1AntSetPSTDMA(padapter, true, 0,
+							   true, 4);
 			} else {
 				padapter->pwrctrlpriv.btcoex_rfon = true;
 				btdm_1AntSetPSTDMA(padapter, true, 0, true, 5);
@@ -6365,16 +6412,18 @@ static void BTDM_1AntFwC2hBtInfo8723A(struct rtw_adapter *padapter)
 		pBtCoex->bC2hBtInquiryPage = false;
 	btState &= ~BIT(2);
 
-	if (!(btState & BIT(0))) {
+	if (!(btState & BIT(0)))
 		pBtCoex->c2hBtInfo = BT_INFO_STATE_NO_CONNECTION;
-	} else {
-		if (btState == 0x1) {
+	else {
+		if (btState == 0x1)
 			pBtCoex->c2hBtInfo = BT_INFO_STATE_CONNECT_IDLE;
-		} else if (btState == 0x9) {
+		else if (btState == 0x9) {
 			if (pBtCoex->bC2hBtInquiryPage == true)
-				pBtCoex->c2hBtInfo = BT_INFO_STATE_ACL_INQ_OR_PAG;
+				pBtCoex->c2hBtInfo =
+					BT_INFO_STATE_ACL_INQ_OR_PAG;
 			else
-				pBtCoex->c2hBtInfo = BT_INFO_STATE_ACL_ONLY_BUSY;
+				pBtCoex->c2hBtInfo =
+					BT_INFO_STATE_ACL_ONLY_BUSY;
 			pBtMgnt->ExtConfig.bBTBusy = true;
 		} else if (btState == 0x3) {
 			pBtCoex->c2hBtInfo = BT_INFO_STATE_SCO_ONLY_BUSY;
@@ -6382,15 +6431,15 @@ static void BTDM_1AntFwC2hBtInfo8723A(struct rtw_adapter *padapter)
 		} else if (btState == 0xb) {
 			pBtCoex->c2hBtInfo = BT_INFO_STATE_ACL_SCO_BUSY;
 			pBtMgnt->ExtConfig.bBTBusy = true;
-		} else {
+		} else
 			pBtCoex->c2hBtInfo = BT_INFO_STATE_MAX;
-		}
 		if (pBtMgnt->ExtConfig.bBTBusy)
-			pHalData->bt_coexist.CurrentState &= ~BT_COEX_STATE_BT_IDLE;
+			pHalData->bt_coexist.CurrentState &=
+				~BT_COEX_STATE_BT_IDLE;
 	}
 
-	if ((BT_INFO_STATE_NO_CONNECTION == pBtCoex->c2hBtInfo) ||
-	    (BT_INFO_STATE_CONNECT_IDLE == pBtCoex->c2hBtInfo)) {
+	if (BT_INFO_STATE_NO_CONNECTION == pBtCoex->c2hBtInfo ||
+	    BT_INFO_STATE_CONNECT_IDLE == pBtCoex->c2hBtInfo) {
 		if (pBtCoex->bC2hBtInquiryPage)
 			pBtCoex->c2hBtInfo = BT_INFO_STATE_INQ_OR_PAG;
 	}
@@ -6413,12 +6462,14 @@ void BTDM_1AntBtCoexist8723A(struct rtw_adapter *padapter)
 
 	if (check_fwstate(pmlmepriv, WIFI_SITE_MONITOR)) {
 		/*  already done in BTDM_1AntForScan() */
-		RTPRINT(FBT, BT_TRACE, ("[BTCoex], wifi is under scan progress!!\n"));
+		RTPRINT(FBT, BT_TRACE,
+			("[BTCoex], wifi is under scan progress!!\n"));
 		return;
 	}
 
 	if (check_fwstate(pmlmepriv, WIFI_UNDER_LINKING)) {
-		RTPRINT(FBT, BT_TRACE, ("[BTCoex], wifi is under link progress!!\n"));
+		RTPRINT(FBT, BT_TRACE,
+			("[BTCoex], wifi is under link progress!!\n"));
 		return;
 	}
 
@@ -6437,9 +6488,7 @@ void BTDM_1AntBtCoexist8723A(struct rtw_adapter *padapter)
 }
 
 /*  ===== End of sync from SD7 driver HAL/BTCoexist/HalBtc87231Ant.c ===== */
-#endif
 
-#ifdef __HALBTC87232ANT_C__ /*  HAL/BTCoexist/HalBtc87232Ant.c */
 /*  ===== Below this line is sync from SD7 driver HAL/BTCoexist/HalBtc87232Ant.c ===== */
 
 /*  local function start with btdm_ */
@@ -6691,13 +6740,13 @@ btdm_SetCoexTable(struct rtw_adapter *padapter, u32 val0x6c0,
 		  u32 val0x6c8, u8 val0x6cc)
 {
 	RTPRINT(FBT, BT_TRACE, ("set coex table, set 0x6c0 = 0x%x\n", val0x6c0));
-	rtw_write32(padapter, 0x6c0, val0x6c0);
+	rtl8723au_write32(padapter, 0x6c0, val0x6c0);
 
 	RTPRINT(FBT, BT_TRACE, ("set coex table, set 0x6c8 = 0x%x\n", val0x6c8));
-	rtw_write32(padapter, 0x6c8, val0x6c8);
+	rtl8723au_write32(padapter, 0x6c8, val0x6c8);
 
 	RTPRINT(FBT, BT_TRACE, ("set coex table, set 0x6cc = 0x%x\n", val0x6cc));
-	rtw_write8(padapter, 0x6cc, val0x6cc);
+	rtl8723au_write8(padapter, 0x6cc, val0x6cc);
 }
 
 static void
@@ -8520,9 +8569,9 @@ static void BTDM_2AntParaInit(struct rtw_adapter *padapter)
 	RTPRINT(FBT, BT_TRACE, ("[BTCoex], 2Ant Parameter Init!!\n"));
 
 	/*  Enable counter statistics */
-	rtw_write8(padapter, 0x76e, 0x4);
-	rtw_write8(padapter, 0x778, 0x3);
-	rtw_write8(padapter, 0x40, 0x20);
+	rtl8723au_write8(padapter, 0x76e, 0x4);
+	rtl8723au_write8(padapter, 0x778, 0x3);
+	rtl8723au_write8(padapter, 0x40, 0x20);
 
 	/*  force to reset coex mechanism */
 	pBtdm8723->preVal0x6c0 = 0x0;
@@ -8919,9 +8968,7 @@ void BTDM_2AntBtCoexist8723A(struct rtw_adapter *padapter)
 }
 
 /*  ===== End of sync from SD7 driver HAL/BTCoexist/HalBtc87232Ant.c ===== */
-#endif
 
-#ifdef __HALBTC8723_C__ /*  HAL/BTCoexist/HalBtc8723.c */
 /*  ===== Below this line is sync from SD7 driver HAL/BTCoexist/HalBtc8723.c ===== */
 
 static u8 btCoexDbgBuf[BT_TMP_BUF_SIZE];
@@ -8974,11 +9021,11 @@ static void btdm_BtHwCountersMonitor(struct rtw_adapter *padapter)
 	regHPTxRx = REG_HIGH_PRIORITY_TXRX;
 	regLPTxRx = REG_LOW_PRIORITY_TXRX;
 
-	u4Tmp = rtw_read32(padapter, regHPTxRx);
+	u4Tmp = rtl8723au_read32(padapter, regHPTxRx);
 	regHPTx = u4Tmp & bMaskLWord;
 	regHPRx = (u4Tmp & bMaskHWord)>>16;
 
-	u4Tmp = rtw_read32(padapter, regLPTxRx);
+	u4Tmp = rtl8723au_read32(padapter, regLPTxRx);
 	regLPTx = u4Tmp & bMaskLWord;
 	regLPRx = (u4Tmp & bMaskHWord)>>16;
 
@@ -8991,7 +9038,7 @@ static void btdm_BtHwCountersMonitor(struct rtw_adapter *padapter)
 	RTPRINT(FBT, BT_TRACE, ("Low Priority Tx/Rx = %d / %d\n", regLPTx, regLPRx));
 
 	/*  reset counter */
-	rtw_write8(padapter, 0x76e, 0xc);
+	rtl8723au_write8(padapter, 0x76e, 0xc);
 }
 
 /*  This function check if 8723 bt is disabled */
@@ -9004,7 +9051,7 @@ static void btdm_BtEnableDisableCheck8723A(struct rtw_adapter *padapter)
 	u8 val8;
 
 	/*  ox68[28]= 1 => BT enable; otherwise disable */
-	val8 = rtw_read8(padapter, 0x6B);
+	val8 = rtl8723au_read8(padapter, 0x6B);
 	if (!(val8 & BIT(4)))
 		btAlife = false;
 
@@ -9197,7 +9244,7 @@ void BTDM_SetFw3a(
 {
 	u8 H2C_Parameter[5] = {0};
 
-	if (BTDM_1Ant8723A(padapter)) {
+	if (rtl8723a_BT_using_antenna_1(padapter)) {
 		if ((!check_fwstate(&padapter->mlmepriv, WIFI_STATION_STATE)) &&
 		    (get_fwstate(&padapter->mlmepriv) != WIFI_NULL_STATE)) {
 			/*  for softap mode */
@@ -9241,7 +9288,7 @@ void BTDM_QueryBtInformation(struct rtw_adapter *padapter)
 	pHalData = GET_HAL_DATA(padapter);
 	pBtCoex = &pHalData->bt_coexist.halCoex8723;
 
-	if (BT_IsBtDisabled(padapter)) {
+	if (!rtl8723a_BT_enabled(padapter)) {
 		pBtCoex->c2hBtInfo = BT_INFO_STATE_DISABLED;
 		pBtCoex->bC2hBtInfoReqSent = false;
 		return;
@@ -9288,7 +9335,7 @@ BTDM_SetSwPenaltyTxRateAdaptive(
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 	u8 tmpU1;
 
-	tmpU1 = rtw_read8(padapter, 0x4fd);
+	tmpU1 = rtl8723au_read8(padapter, 0x4fd);
 	tmpU1 |= BIT(0);
 	if (BT_TX_RATE_ADAPTIVE_LOW_PENALTY == raType) {
 		tmpU1 &= ~BIT(2);
@@ -9297,7 +9344,7 @@ BTDM_SetSwPenaltyTxRateAdaptive(
 		tmpU1 |= BIT(2);
 	}
 
-	rtw_write8(padapter, 0x4fd, tmpU1);
+	rtl8723au_write8(padapter, 0x4fd, tmpU1);
 }
 
 void BTDM_SetFwDecBtPwr(struct rtw_adapter *padapter, u8 bDecBtPwr)
@@ -9349,8 +9396,8 @@ static void BTDM_FwC2hBtRssi8723A(struct rtw_adapter *padapter, u8 *tmpBuf)
 /*RTPRINT(FBT, BT_TRACE, ("[BTC2H], BT RSSI =%d\n", percent)); */
 }
 
-static void
-BTDM_FwC2hBtInfo8723A(struct rtw_adapter *padapter, u8 *tmpBuf, u8 length)
+void
+rtl8723a_fw_c2h_BT_info(struct rtw_adapter *padapter, u8 *tmpBuf, u8 length)
 {
 	struct hal_data_8723a *pHalData;
 	struct bt_30info *pBTInfo;
@@ -9419,7 +9466,7 @@ static void BTDM_Display8723ABtCoexInfo(struct rtw_adapter *padapter)
 	rsprintf(btCoexDbgBuf, BT_TMP_BUF_SIZE, "\r\n ============[BT Coexist info]============");
 	DCMD_Printf(btCoexDbgBuf);
 
-	if (!pHalData->bt_coexist.BluetoothCoexist) {
+	if (!rtl8723a_BT_coexist(padapter)) {
 		rsprintf(btCoexDbgBuf, BT_TMP_BUF_SIZE, "\r\n BT not exists !!!");
 		DCMD_Printf(btCoexDbgBuf);
 		return;
@@ -9473,7 +9520,8 @@ static void BTDM_Display8723ABtCoexInfo(struct rtw_adapter *padapter)
 
 					btInfoExt = pHalData->bt_coexist.halCoex8723.btInfoExt;
 					rsprintf(btCoexDbgBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s", "A2DP rate", \
-						(btInfoExt&BIT0) ? "Basic rate" : "EDR rate");
+						 (btInfoExt & BIT(0)) ?
+						 "Basic rate" : "EDR rate");
 					DCMD_Printf(btCoexDbgBuf);
 				} else {
 					rsprintf(btCoexDbgBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s/ %s", "Bt link type/spec", \
@@ -9527,9 +9575,9 @@ static void BTDM_Display8723ABtCoexInfo(struct rtw_adapter *padapter)
 			pBtCoex->btdm2Ant.bCurDecBtPwr);
 		DCMD_Printf(btCoexDbgBuf);
 	}
-	u1Tmp = rtw_read8(padapter, 0x778);
-	u1Tmp1 = rtw_read8(padapter, 0x783);
-	u1Tmp2 = rtw_read8(padapter, 0x796);
+	u1Tmp = rtl8723au_read8(padapter, 0x778);
+	u1Tmp1 = rtl8723au_read8(padapter, 0x783);
+	u1Tmp2 = rtl8723au_read8(padapter, 0x796);
 	rsprintf(btCoexDbgBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x", "0x778/ 0x783/ 0x796", \
 		u1Tmp, u1Tmp1, u1Tmp2);
 	DCMD_Printf(btCoexDbgBuf);
@@ -9539,7 +9587,7 @@ static void BTDM_Display8723ABtCoexInfo(struct rtw_adapter *padapter)
 			pBtCoex->btdm2Ant.bCurDacSwingOn, pBtCoex->btdm2Ant.curDacSwingLvl);
 		DCMD_Printf(btCoexDbgBuf);
 	}
-	u4Tmp[0] =  rtw_read32(padapter, 0x880);
+	u4Tmp[0] =  rtl8723au_read32(padapter, 0x880);
 	rsprintf(btCoexDbgBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x880", \
 		u4Tmp[0]);
 	DCMD_Printf(btCoexDbgBuf);
@@ -9550,56 +9598,56 @@ static void BTDM_Display8723ABtCoexInfo(struct rtw_adapter *padapter)
 		DCMD_Printf(btCoexDbgBuf);
 	}
 
-	u1Tmp = rtw_read8(padapter, 0x40);
+	u1Tmp = rtl8723au_read8(padapter, 0x40);
 	rsprintf(btCoexDbgBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x40", \
 		u1Tmp);
 	DCMD_Printf(btCoexDbgBuf);
 
-	u4Tmp[0] = rtw_read32(padapter, 0x550);
-	u1Tmp = rtw_read8(padapter, 0x522);
+	u4Tmp[0] = rtl8723au_read32(padapter, 0x550);
+	u1Tmp = rtl8723au_read8(padapter, 0x522);
 	rsprintf(btCoexDbgBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/0x%x", "0x550(bcn contrl)/0x522", \
 		u4Tmp[0], u1Tmp);
 	DCMD_Printf(btCoexDbgBuf);
 
-	u4Tmp[0] = rtw_read32(padapter, 0x484);
+	u4Tmp[0] = rtl8723au_read32(padapter, 0x484);
 	rsprintf(btCoexDbgBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x484(rate adaptive)", \
 		u4Tmp[0]);
 	DCMD_Printf(btCoexDbgBuf);
 
-	u4Tmp[0] = rtw_read32(padapter, 0x50);
+	u4Tmp[0] = rtl8723au_read32(padapter, 0x50);
 	rsprintf(btCoexDbgBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0xc50(dig)", \
 		u4Tmp[0]);
 	DCMD_Printf(btCoexDbgBuf);
 
-	u4Tmp[0] = rtw_read32(padapter, 0xda0);
-	u4Tmp[1] = rtw_read32(padapter, 0xda4);
-	u4Tmp[2] = rtw_read32(padapter, 0xda8);
-	u4Tmp[3] = rtw_read32(padapter, 0xdac);
+	u4Tmp[0] = rtl8723au_read32(padapter, 0xda0);
+	u4Tmp[1] = rtl8723au_read32(padapter, 0xda4);
+	u4Tmp[2] = rtl8723au_read32(padapter, 0xda8);
+	u4Tmp[3] = rtl8723au_read32(padapter, 0xdac);
 	rsprintf(btCoexDbgBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x", "0xda0/0xda4/0xda8/0xdac(FA cnt)", \
 		u4Tmp[0], u4Tmp[1], u4Tmp[2], u4Tmp[3]);
 	DCMD_Printf(btCoexDbgBuf);
 
-	u4Tmp[0] = rtw_read32(padapter, 0x6c0);
-	u4Tmp[1] = rtw_read32(padapter, 0x6c4);
-	u4Tmp[2] = rtw_read32(padapter, 0x6c8);
-	u1Tmp = rtw_read8(padapter, 0x6cc);
+	u4Tmp[0] = rtl8723au_read32(padapter, 0x6c0);
+	u4Tmp[1] = rtl8723au_read32(padapter, 0x6c4);
+	u4Tmp[2] = rtl8723au_read32(padapter, 0x6c8);
+	u1Tmp = rtl8723au_read8(padapter, 0x6cc);
 	rsprintf(btCoexDbgBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x", "0x6c0/0x6c4/0x6c8/0x6cc(coexTable)", \
 		u4Tmp[0], u4Tmp[1], u4Tmp[2], u1Tmp);
 	DCMD_Printf(btCoexDbgBuf);
 
-	/* u4Tmp = rtw_read32(padapter, 0x770); */
+	/* u4Tmp = rtl8723au_read32(padapter, 0x770); */
 	rsprintf(btCoexDbgBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d", "0x770(Hi pri Rx[31:16]/Tx[15:0])", \
 		pHalData->bt_coexist.halCoex8723.highPriorityRx,
 		pHalData->bt_coexist.halCoex8723.highPriorityTx);
 	DCMD_Printf(btCoexDbgBuf);
-	/* u4Tmp = rtw_read32(padapter, 0x774); */
+	/* u4Tmp = rtl8723au_read32(padapter, 0x774); */
 	rsprintf(btCoexDbgBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d", "0x774(Lo pri Rx[31:16]/Tx[15:0])", \
 		pHalData->bt_coexist.halCoex8723.lowPriorityRx,
 		pHalData->bt_coexist.halCoex8723.lowPriorityTx);
 	DCMD_Printf(btCoexDbgBuf);
 
 	/*  Tx mgnt queue hang or not, 0x41b should = 0xf, ex: 0xd ==>hang */
-	u1Tmp = rtw_read8(padapter, 0x41b);
+	u1Tmp = rtl8723au_read8(padapter, 0x41b);
 	rsprintf(btCoexDbgBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x41b (hang chk == 0xf)", \
 		u1Tmp);
 	DCMD_Printf(btCoexDbgBuf);
@@ -9672,7 +9720,7 @@ BTDM_Set8723ABtCoexCurrAntNum(struct rtw_adapter *padapter, u8 antNum)
 		pBtCoex->TotalAntNum = Ant_x2;
 }
 
-void BTDM_LpsLeave(struct rtw_adapter *padapter)
+void rtl8723a_BT_lps_leave(struct rtw_adapter *padapter)
 {
 	struct bt_30info *pBTInfo = GET_BT_INFO(padapter);
 	struct bt_mgnt *pBtMgnt = &pBTInfo->BtMgnt;
@@ -9752,7 +9800,7 @@ static void BTDM_ForDhcp8723A(struct rtw_adapter *padapter)
 		BTDM_1AntForDhcp(padapter);
 }
 
-u8 BTDM_1Ant8723A(struct rtw_adapter *padapter)
+bool rtl8723a_BT_using_antenna_1(struct rtw_adapter *padapter)
 {
 	if (btdm_BtWifiAntNum(padapter) == Ant_x1)
 		return true;
@@ -9785,7 +9833,7 @@ static void BTDM_BTCoexist8723A(struct rtw_adapter *padapter)
 	}
 
 	if (pBtCoex->bC2hBtInfoReqSent) {
-		if (BT_IsBtDisabled(padapter)) {
+		if (!rtl8723a_BT_enabled(padapter)) {
 			pBtCoex->c2hBtInfo = BT_INFO_STATE_DISABLED;
 		} else {
 			if (pBtCoex->c2hBtInfo == BT_INFO_STATE_DISABLED)
@@ -9793,7 +9841,7 @@ static void BTDM_BTCoexist8723A(struct rtw_adapter *padapter)
 		}
 
 		btdm_BTCoexist8723AHandler(padapter);
-	} else if (BT_IsBtDisabled(padapter) == true) {
+	} else if (!rtl8723a_BT_enabled(padapter)) {
 		pBtCoex->c2hBtInfo = BT_INFO_STATE_DISABLED;
 		btdm_BTCoexist8723AHandler(padapter);
 	}
@@ -9802,9 +9850,7 @@ static void BTDM_BTCoexist8723A(struct rtw_adapter *padapter)
 }
 
 /*  ===== End of sync from SD7 driver HAL/BTCoexist/HalBtc8723.c ===== */
-#endif
 
-#ifdef __HALBTCCSR1ANT_C__ /*  HAL/BTCoexist/HalBtcCsr1Ant.c */
 /*  ===== Below this line is sync from SD7 driver HAL/BTCoexist/HalBtcCsr1Ant.c ===== */
 
 /*  local function start with btdm_ */
@@ -9866,7 +9912,7 @@ void BTDM_CheckBTIdleChange1Ant(struct rtw_adapter *padapter)
 	u32				BT_Active, BT_State;
 	u32				regBTActive = 0, regBTState = 0, regBTPolling = 0;
 
-	if (!pHalData->bt_coexist.BluetoothCoexist)
+	if (!rtl8723a_BT_coexist(padapter))
 		return;
 	if (pBtMgnt->ExtConfig.bManualControl)
 		return;
@@ -9885,15 +9931,15 @@ void BTDM_CheckBTIdleChange1Ant(struct rtw_adapter *padapter)
 	else
 		regBTPolling = REG_BT_POLLING;
 
-	BT_Active = rtw_read32(padapter, regBTActive);
+	BT_Active = rtl8723au_read32(padapter, regBTActive);
 	RTPRINT(FBT, BT_TRACE, ("[DM][BT], BT_Active(0x%x) =%x\n", regBTActive, BT_Active));
 	BT_Active = BT_Active & 0x00ffffff;
 
-	BT_State = rtw_read32(padapter, regBTState);
+	BT_State = rtl8723au_read32(padapter, regBTState);
 	RTPRINT(FBT, BT_TRACE, ("[DM][BT], BT_State(0x%x) =%x\n", regBTState, BT_State));
 	BT_State = BT_State & 0x00ffffff;
 
-	BT_Polling = rtw_read32(padapter, regBTPolling);
+	BT_Polling = rtl8723au_read32(padapter, regBTPolling);
 	RTPRINT(FBT, BT_TRACE, ("[DM][BT], BT_Polling(0x%x) =%x\n", regBTPolling, BT_Polling));
 
 	if (BT_Active == 0xffffffff && BT_State == 0xffffffff && BT_Polling == 0xffffffff)
@@ -9991,9 +10037,7 @@ void BTDM_CheckBTIdleChange1Ant(struct rtw_adapter *padapter)
 }
 
 /*  ===== End of sync from SD7 driver HAL/BTCoexist/HalBtcCsr1Ant.c ===== */
-#endif
 
-#ifdef __HALBTCCSR2ANT_C__ /*  HAL/BTCoexist/HalBtcCsr2Ant.c */
 /*  ===== Below this line is sync from SD7 driver HAL/BTCoexist/HalBtcCsr2Ant.c ===== */
 
 /*  local function start with btdm_ */
@@ -10048,9 +10092,7 @@ BTDM_DiminishWiFi(
 }
 
 /*  ===== End of sync from SD7 driver HAL/BTCoexist/HalBtcCsr2Ant.c ===== */
-#endif
 
-#ifdef __HALBTCOEXIST_C__ /*  HAL/BTCoexist/HalBtCoexist.c */
 /*  ===== Below this line is sync from SD7 driver HAL/BTCoexist/HalBtCoexist.c ===== */
 
 /*  local function */
@@ -10089,11 +10131,6 @@ void BTDM_FwC2hBtRssi(struct rtw_adapter *padapter, u8 *tmpBuf)
 	BTDM_FwC2hBtRssi8723A(padapter, tmpBuf);
 }
 
-void BTDM_FwC2hBtInfo(struct rtw_adapter *padapter, u8 *tmpBuf, u8 length)
-{
-	BTDM_FwC2hBtInfo8723A(padapter, tmpBuf, length);
-}
-
 void BTDM_DisplayBtCoexInfo(struct rtw_adapter *padapter)
 {
 	BTDM_Display8723ABtCoexInfo(padapter);
@@ -10489,18 +10526,18 @@ u8 BTDM_CheckCoexRSSIState(struct rtw_adapter *padapter, u8 levelNum,
 	return btRssiState;
 }
 
-u8 BTDM_DisableEDCATurbo(struct rtw_adapter *padapter)
+bool rtl8723a_BT_disable_EDCA_turbo(struct rtw_adapter *padapter)
 {
 	struct bt_mgnt *pBtMgnt;
 	struct hal_data_8723a *pHalData;
 	u8 bBtChangeEDCA = false;
 	u32 EDCA_BT_BE = 0x5ea42b, cur_EDCA_reg;
-	u8 bRet = false;
+	bool bRet = false;
 
 	pHalData = GET_HAL_DATA(padapter);
 	pBtMgnt = &pHalData->BtInfo.BtMgnt;
 
-	if (!pHalData->bt_coexist.BluetoothCoexist) {
+	if (!rtl8723a_BT_coexist(padapter)) {
 		bRet = false;
 		pHalData->bt_coexist.lastBtEdca = 0;
 		return bRet;
@@ -10512,7 +10549,7 @@ u8 BTDM_DisableEDCATurbo(struct rtw_adapter *padapter)
 		return bRet;
 	}
 
-	if (BT_1Ant(padapter)) {
+	if (rtl8723a_BT_using_antenna_1(padapter)) {
 		bRet = false;
 		pHalData->bt_coexist.lastBtEdca = 0;
 		return bRet;
@@ -10533,12 +10570,13 @@ u8 BTDM_DisableEDCATurbo(struct rtw_adapter *padapter)
 			pHalData->odmpriv.DM_EDCA_Table.bCurrentTurboEDCA = false;
 			pHalData->dmpriv.prv_traffic_idx = 3;
 		}
-		cur_EDCA_reg = rtw_read32(padapter, REG_EDCA_BE_PARAM);
+		cur_EDCA_reg = rtl8723au_read32(padapter, REG_EDCA_BE_PARAM);
 
 		if (cur_EDCA_reg != EDCA_BT_BE)
 			bBtChangeEDCA = true;
 		if (bBtChangeEDCA || !pHalData->bt_coexist.bEDCAInitialized) {
-			rtw_write32(padapter, REG_EDCA_BE_PARAM, EDCA_BT_BE);
+			rtl8723au_write32(padapter, REG_EDCA_BE_PARAM,
+					  EDCA_BT_BE);
 			pHalData->bt_coexist.lastBtEdca = EDCA_BT_BE;
 		}
 		bRet = true;
@@ -10585,11 +10623,11 @@ void BTDM_AGCTable(struct rtw_adapter *padapter, u8 type)
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 	if (type == BT_AGCTABLE_OFF) {
 		RTPRINT(FBT, BT_TRACE, ("[BT]AGCTable Off!\n"));
-		rtw_write32(padapter, 0xc78, 0x641c0001);
-		rtw_write32(padapter, 0xc78, 0x631d0001);
-		rtw_write32(padapter, 0xc78, 0x621e0001);
-		rtw_write32(padapter, 0xc78, 0x611f0001);
-		rtw_write32(padapter, 0xc78, 0x60200001);
+		rtl8723au_write32(padapter, 0xc78, 0x641c0001);
+		rtl8723au_write32(padapter, 0xc78, 0x631d0001);
+		rtl8723au_write32(padapter, 0xc78, 0x621e0001);
+		rtl8723au_write32(padapter, 0xc78, 0x611f0001);
+		rtl8723au_write32(padapter, 0xc78, 0x60200001);
 
 		PHY_SetRFReg(padapter, PathA, RF_RX_AGC_HP, bRFRegOffsetMask, 0x32000);
 		PHY_SetRFReg(padapter, PathA, RF_RX_AGC_HP, bRFRegOffsetMask, 0x71000);
@@ -10600,11 +10638,11 @@ void BTDM_AGCTable(struct rtw_adapter *padapter, u8 type)
 		pHalData->bt_coexist.b8723aAgcTableOn = false;
 	} else if (type == BT_AGCTABLE_ON) {
 		RTPRINT(FBT, BT_TRACE, ("[BT]AGCTable On!\n"));
-		rtw_write32(padapter, 0xc78, 0x4e1c0001);
-		rtw_write32(padapter, 0xc78, 0x4d1d0001);
-		rtw_write32(padapter, 0xc78, 0x4c1e0001);
-		rtw_write32(padapter, 0xc78, 0x4b1f0001);
-		rtw_write32(padapter, 0xc78, 0x4a200001);
+		rtl8723au_write32(padapter, 0xc78, 0x4e1c0001);
+		rtl8723au_write32(padapter, 0xc78, 0x4d1d0001);
+		rtl8723au_write32(padapter, 0xc78, 0x4c1e0001);
+		rtl8723au_write32(padapter, 0xc78, 0x4b1f0001);
+		rtl8723au_write32(padapter, 0xc78, 0x4a200001);
 
 		PHY_SetRFReg(padapter, PathA, RF_RX_AGC_HP, bRFRegOffsetMask, 0xdc000);
 		PHY_SetRFReg(padapter, PathA, RF_RX_AGC_HP, bRFRegOffsetMask, 0x90000);
@@ -10624,10 +10662,10 @@ void BTDM_BBBackOffLevel(struct rtw_adapter *padapter, u8 type)
 
 	if (type == BT_BB_BACKOFF_OFF) {
 		RTPRINT(FBT, BT_TRACE, ("[BT]BBBackOffLevel Off!\n"));
-		rtw_write32(padapter, 0xc04, 0x3a05611);
+		rtl8723au_write32(padapter, 0xc04, 0x3a05611);
 	} else if (type == BT_BB_BACKOFF_ON) {
 		RTPRINT(FBT, BT_TRACE, ("[BT]BBBackOffLevel On!\n"));
-		rtw_write32(padapter, 0xc04, 0x3a07611);
+		rtl8723au_write32(padapter, 0xc04, 0x3a07611);
 		pHalData->bt_coexist.bSWCoexistAllOff = false;
 	}
 }
@@ -10680,16 +10718,15 @@ void BTDM_CoexAllOff(struct rtw_adapter *padapter)
 	BTDM_HWCoexAllOff(padapter);
 }
 
-void BTDM_TurnOffBtCoexistBeforeEnterIPS(struct rtw_adapter *padapter)
+void rtl8723a_BT_disable_coexist(struct rtw_adapter *padapter)
 {
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 	struct pwrctrl_priv *ppwrctrl = &padapter->pwrctrlpriv;
 
-	if (!pHalData->bt_coexist.BluetoothCoexist)
+	if (!rtl8723a_BT_coexist(padapter))
 		return;
 
 	/*  8723 1Ant doesn't need to turn off bt coexist mechanism. */
-	if (BTDM_1Ant8723A(padapter))
+	if (rtl8723a_BT_using_antenna_1(padapter))
 		return;
 
 	/*  Before enter IPS, turn off FW BT Co-exist mechanism */
@@ -10706,11 +10743,11 @@ void BTDM_SignalCompensation(struct rtw_adapter *padapter, u8 *rssi_wifi, u8 *rs
 	BTDM_8723ASignalCompensation(padapter, rssi_wifi, rssi_bt);
 }
 
-void BTDM_Coexist(struct rtw_adapter *padapter)
+void rtl8723a_BT_do_coexist(struct rtw_adapter *padapter)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
-	if (!pHalData->bt_coexist.BluetoothCoexist) {
+	if (!rtl8723a_BT_coexist(padapter)) {
 		RTPRINT(FBT, BT_TRACE, ("[DM][BT], BT not exists!!\n"));
 		return;
 	}
@@ -10920,9 +10957,7 @@ void BTDM_SetBtCoexCurrAntNum(struct rtw_adapter *padapter, u8 antNum)
 
 void BTDM_ForHalt(struct rtw_adapter *padapter)
 {
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
-
-	if (!pHalData->bt_coexist.BluetoothCoexist)
+	if (!rtl8723a_BT_coexist(padapter))
 		return;
 
 	BTDM_ForHalt8723A(padapter);
@@ -10931,9 +10966,7 @@ void BTDM_ForHalt(struct rtw_adapter *padapter)
 
 void BTDM_WifiScanNotify(struct rtw_adapter *padapter, u8 scanType)
 {
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
-
-	if (!pHalData->bt_coexist.BluetoothCoexist)
+	if (!rtl8723a_BT_coexist(padapter))
 		return;
 
 	BTDM_WifiScanNotify8723A(padapter, scanType);
@@ -10941,29 +10974,24 @@ void BTDM_WifiScanNotify(struct rtw_adapter *padapter, u8 scanType)
 
 void BTDM_WifiAssociateNotify(struct rtw_adapter *padapter, u8 action)
 {
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
-
-	if (!pHalData->bt_coexist.BluetoothCoexist)
+	if (!rtl8723a_BT_coexist(padapter))
 		return;
 
 	BTDM_WifiAssociateNotify8723A(padapter, action);
 }
 
-void BTDM_MediaStatusNotify(struct rtw_adapter *padapter, enum rt_media_status mstatus)
+void rtl8723a_BT_mediastatus_notify(struct rtw_adapter *padapter,
+				    enum rt_media_status mstatus)
 {
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
-
-	if (!pHalData->bt_coexist.BluetoothCoexist)
+	if (!rtl8723a_BT_coexist(padapter))
 		return;
 
 	BTDM_MediaStatusNotify8723A(padapter, mstatus);
 }
 
-void BTDM_ForDhcp(struct rtw_adapter *padapter)
+void rtl8723a_BT_specialpacket_notify(struct rtw_adapter *padapter)
 {
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
-
-	if (!pHalData->bt_coexist.BluetoothCoexist)
+	if (!rtl8723a_BT_coexist(padapter))
 		return;
 
 	BTDM_ForDhcp8723A(padapter);
@@ -11175,20 +11203,18 @@ u8 BTDM_IsActionPANA2DP(struct rtw_adapter *padapter)
 	return bRet;
 }
 
-u8 BTDM_IsBtDisabled(struct rtw_adapter *padapter)
+bool rtl8723a_BT_enabled(struct rtw_adapter *padapter)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
 	if (pHalData->bt_coexist.bCurBtDisabled)
-		return true;
-	else
 		return false;
+	else
+		return true;
 }
 
 /*  ===== End of sync from SD7 driver HAL/BTCoexist/HalBtCoexist.c ===== */
-#endif
 
-#ifdef __HALBT_C__ /*  HAL/HalBT.c */
 /*  ===== Below this line is sync from SD7 driver HAL/HalBT.c ===== */
 
 /*  */
@@ -11221,7 +11247,9 @@ void HALBT_SetKey(struct rtw_adapter *padapter, u8 EntryNum)
 	pBtAssocEntry->HwCAMIndex = BT_HWCAM_STAR + EntryNum;
 
 	usConfig = CAM_VALID | (CAM_AES << 2);
-	write_cam23a(padapter, pBtAssocEntry->HwCAMIndex, usConfig, pBtAssocEntry->BTRemoteMACAddr, pBtAssocEntry->PTK + TKIP_ENC_KEY_POS);
+	rtl8723a_cam_write(padapter, pBtAssocEntry->HwCAMIndex, usConfig,
+			   pBtAssocEntry->BTRemoteMACAddr,
+			   pBtAssocEntry->PTK + TKIP_ENC_KEY_POS);
 }
 
 void HALBT_RemoveKey(struct rtw_adapter *padapter, u8 EntryNum)
@@ -11234,13 +11262,15 @@ void HALBT_RemoveKey(struct rtw_adapter *padapter, u8 EntryNum)
 
 	if (pBTinfo->BtAsocEntry[EntryNum].HwCAMIndex != 0) {
 		/*  ToDo : add New HALBT_RemoveKey function !! */
-		if (pBtAssocEntry->HwCAMIndex >= BT_HWCAM_STAR && pBtAssocEntry->HwCAMIndex < HALF_CAM_ENTRY)
-			CAM_empty_entry23a(padapter, pBtAssocEntry->HwCAMIndex);
+		if (pBtAssocEntry->HwCAMIndex >= BT_HWCAM_STAR &&
+		    pBtAssocEntry->HwCAMIndex < HALF_CAM_ENTRY)
+			rtl8723a_cam_empty_entry(padapter,
+						 pBtAssocEntry->HwCAMIndex);
 		pBTinfo->BtAsocEntry[EntryNum].HwCAMIndex = 0;
 	}
 }
 
-void HALBT_InitBTVars8723A(struct rtw_adapter *padapter)
+void rtl8723a_BT_init_hal_vars(struct rtw_adapter *padapter)
 {
 	struct hal_data_8723a *pHalData;
 
@@ -11252,8 +11282,10 @@ void HALBT_InitBTVars8723A(struct rtw_adapter *padapter)
 	pHalData->bt_coexist.BT_Ant_isolation = pHalData->EEPROMBluetoothAntIsolation;
 	pHalData->bt_coexist.bt_radiosharedtype = pHalData->EEPROMBluetoothRadioShared;
 
-	RT_TRACE(_module_hal_init_c_, _drv_info_, ("BT Coexistance = 0x%x\n", pHalData->bt_coexist.BluetoothCoexist));
-	if (pHalData->bt_coexist.BluetoothCoexist) {
+	RT_TRACE(_module_hal_init_c_, _drv_info_,
+		 ("BT Coexistance = 0x%x\n", rtl8723a_BT_coexist(padapter)));
+
+	if (rtl8723a_BT_coexist(padapter)) {
 		if (pHalData->bt_coexist.BT_Ant_Num == Ant_x2) {
 			BTDM_SetBtCoexCurrAntNum(padapter, 2);
 			RT_TRACE(_module_hal_init_c_, _drv_info_, ("BlueTooth BT_Ant_Num = Antx2\n"));
@@ -11273,7 +11305,7 @@ void HALBT_InitBTVars8723A(struct rtw_adapter *padapter)
 	}
 }
 
-u8 HALBT_IsBTExist(struct rtw_adapter *padapter)
+bool rtl8723a_BT_coexist(struct rtw_adapter *padapter)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
@@ -11290,10 +11322,10 @@ u8 HALBT_BTChipType(struct rtw_adapter *padapter)
 	return pHalData->bt_coexist.BT_CoexistType;
 }
 
-void HALBT_InitHwConfig(struct rtw_adapter *padapter)
+void rtl8723a_BT_init_hwconfig(struct rtw_adapter *padapter)
 {
 	halbt_InitHwConfig8723A(padapter);
-	BTDM_Coexist(padapter);
+	rtl8723a_BT_do_coexist(padapter);
 }
 
 void HALBT_SetRtsCtsNoLenLimit(struct rtw_adapter *padapter)
@@ -11301,4 +11333,49 @@ void HALBT_SetRtsCtsNoLenLimit(struct rtw_adapter *padapter)
 }
 
 /*  ===== End of sync from SD7 driver HAL/HalBT.c ===== */
-#endif
+
+void rtl8723a_dual_antenna_detection(struct rtw_adapter *padapter)
+{
+	struct hal_data_8723a *pHalData;
+	struct dm_odm_t *pDM_Odm;
+	struct sw_ant_sw *pDM_SWAT_Table;
+	u8 i;
+
+	pHalData = GET_HAL_DATA(padapter);
+	pDM_Odm = &pHalData->odmpriv;
+	pDM_SWAT_Table = &pDM_Odm->DM_SWAT_Table;
+
+	/*  */
+	/*  <Roger_Notes> RTL8723A Single and Dual antenna dynamic detection
+	    mechanism when RF power state is on. */
+	/*  We should take power tracking, IQK, LCK, RCK RF read/write
+	    operation into consideration. */
+	/*  2011.12.15. */
+	/*  */
+	if (!pHalData->bAntennaDetected) {
+		u8 btAntNum = BT_GetPGAntNum(padapter);
+
+		/*  Set default antenna B status */
+		if (btAntNum == Ant_x2)
+			pDM_SWAT_Table->ANTB_ON = true;
+		else if (btAntNum == Ant_x1)
+			pDM_SWAT_Table->ANTB_ON = false;
+		else
+			pDM_SWAT_Table->ANTB_ON = true;
+
+		if (pHalData->CustomerID != RT_CID_TOSHIBA) {
+			for (i = 0; i < MAX_ANTENNA_DETECTION_CNT; i++) {
+				if (ODM_SingleDualAntennaDetection
+				    (&pHalData->odmpriv, ANTTESTALL) == true)
+					break;
+			}
+
+			/*  Set default antenna number for BT coexistence */
+			if (btAntNum == Ant_x2)
+				BT_SetBtCoexCurrAntNum(padapter,
+						       pDM_SWAT_Table->
+						       ANTB_ON ? 2 : 1);
+		}
+		pHalData->bAntennaDetected = true;
+	}
+}
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_cmd.c b/drivers/staging/rtl8723au/hal/rtl8723a_cmd.c
index 0b205e1204f..e8737916c7b 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723a_cmd.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723a_cmd.c
@@ -17,10 +17,10 @@
 #include <osdep_service.h>
 #include <drv_types.h>
 #include <recv_osdep.h>
-#include <cmd_osdep.h>
 #include <mlme_osdep.h>
 #include <rtw_ioctl_set.h>
 #include <rtl8723a_hal.h>
+#include <usb_ops_linux.h>
 
 #define RTL92C_MAX_H2C_BOX_NUMS		4
 #define RTL92C_MAX_CMD_LEN		5
@@ -34,7 +34,7 @@ static u8 _is_fw_read_cmd_down(struct rtw_adapter *padapter, u8 msgbox_num)
 	u8 valid;
 
 	do {
-		valid = rtw_read8(padapter, REG_HMETFR) & BIT(msgbox_num);
+		valid = rtl8723au_read8(padapter, REG_HMETFR) & BIT(msgbox_num);
 		if (0 == valid)
 			read_down = true;
 	} while ((!read_down) && (retry_cnts--));
@@ -48,7 +48,8 @@ static u8 _is_fw_read_cmd_down(struct rtw_adapter *padapter, u8 msgbox_num)
 *| h2c_msg	|Ext_bit	|CMD_ID	|
 *
 ******************************************/
-s32 FillH2CCmd(struct rtw_adapter *padapter, u8 ElementID, u32 CmdLen, u8 *pCmdBuffer)
+int FillH2CCmd(struct rtw_adapter *padapter, u8 ElementID, u32 CmdLen,
+	       u8 *pCmdBuffer)
 {
 	u8 bcmd_down = false;
 	s32 retry_cnts = 100;
@@ -58,7 +59,7 @@ s32 FillH2CCmd(struct rtw_adapter *padapter, u8 ElementID, u32 CmdLen, u8 *pCmdB
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 	u32 h2c_cmd = 0;
 	u16 h2c_cmd_ex = 0;
-	s32 ret = _FAIL;
+	int ret = _FAIL;
 
 	padapter = GET_PRIMARY_ADAPTER(padapter);
 	pHalData = GET_HAL_DATA(padapter);
@@ -94,11 +95,11 @@ s32 FillH2CCmd(struct rtw_adapter *padapter, u8 ElementID, u32 CmdLen, u8 *pCmdB
 		if (h2c_cmd & BIT(7)) {
 			msgbox_ex_addr = REG_HMEBOX_EXT_0 + (h2c_box_num * EX_MESSAGE_BOX_SIZE);
 			h2c_cmd_ex = le16_to_cpu(h2c_cmd_ex);
-			rtw_write16(padapter, msgbox_ex_addr, h2c_cmd_ex);
+			rtl8723au_write16(padapter, msgbox_ex_addr, h2c_cmd_ex);
 		}
 		msgbox_addr = REG_HMEBOX_0 + (h2c_box_num * MESSAGE_BOX_SIZE);
 		h2c_cmd = le32_to_cpu(h2c_cmd);
-		rtw_write32(padapter, msgbox_addr, h2c_cmd);
+		rtl8723au_write32(padapter, msgbox_addr, h2c_cmd);
 
 		bcmd_down = true;
 
@@ -113,9 +114,9 @@ exit:
 	return ret;
 }
 
-u8 rtl8723a_set_rssi_cmd(struct rtw_adapter *padapter, u8 *param)
+int rtl8723a_set_rssi_cmd(struct rtw_adapter *padapter, u8 *param)
 {
-	u8 res = _SUCCESS;
+	int res = _SUCCESS;
 
 	*((u32 *)param) = cpu_to_le32(*((u32 *)param));
 
@@ -124,10 +125,10 @@ u8 rtl8723a_set_rssi_cmd(struct rtw_adapter *padapter, u8 *param)
 	return res;
 }
 
-u8 rtl8723a_set_raid_cmd(struct rtw_adapter *padapter, u32 mask, u8 arg)
+int rtl8723a_set_raid_cmd(struct rtw_adapter *padapter, u32 mask, u8 arg)
 {
 	u8 buf[5];
-	u8 res = _SUCCESS;
+	int res = _SUCCESS;
 
 	memset(buf, 0, 5);
 	mask = cpu_to_le32(mask);
@@ -137,7 +138,6 @@ u8 rtl8723a_set_raid_cmd(struct rtw_adapter *padapter, u32 mask, u8 arg)
 	FillH2CCmd(padapter, MACID_CONFIG_EID, 5, buf);
 
 	return res;
-
 }
 
 /* bitmap[0:27] = tx_rate_bitmap */
@@ -168,7 +168,8 @@ void rtl8723a_add_rateatid(struct rtw_adapter *pAdapter, u32 bitmap, u8 arg, u8
 		if (shortGIrate == true)
 			init_rate |= BIT(6);
 
-		rtw_write8(pAdapter, (REG_INIDATA_RATE_SEL+macid), (u8)init_rate);
+		rtl8723au_write8(pAdapter, REG_INIDATA_RATE_SEL + macid,
+				 init_rate);
 	}
 }
 
@@ -178,7 +179,7 @@ void rtl8723a_set_FwPwrMode_cmd(struct rtw_adapter *padapter, u8 Mode)
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
-	DBG_8723A("%s: Mode =%d SmartPS =%d UAPSD =%d BcnMode = 0x%02x\n", __FUNCTION__,
+	DBG_8723A("%s: Mode =%d SmartPS =%d UAPSD =%d BcnMode = 0x%02x\n", __func__,
 			Mode, pwrpriv->smart_ps, padapter->registrypriv.uapsd_enable, pwrpriv->bcn_ant_mode);
 
 	/*  Forece leave RF low power mode for 1T1R to
@@ -203,27 +204,26 @@ void rtl8723a_set_FwPwrMode_cmd(struct rtw_adapter *padapter, u8 Mode)
 static void ConstructBeacon(struct rtw_adapter *padapter, u8 *pframe, u32 *pLength)
 {
 	struct ieee80211_hdr *pwlanhdr;
-	u16 *fctrl;
 	u32 rate_len, pktlen;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *cur_network = &pmlmeinfo->network;
 	u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	int bcn_fixed_size;
 
-	/* DBG_8723A("%s\n", __FUNCTION__); */
+	/* DBG_8723A("%s\n", __func__); */
 
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
 
-	fctrl = &pwlanhdr->frame_control;
-	*(fctrl) = 0;
+	pwlanhdr->frame_control =
+		cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_BEACON);
 
 	memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
 	memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
 	memcpy(pwlanhdr->addr3, get_my_bssid23a(cur_network), ETH_ALEN);
 
-	SetSeqNum(pwlanhdr, 0/*pmlmeext->mgnt_seq*/);
-	/* pmlmeext->mgnt_seq++; */
-	SetFrameSubType(pframe, WIFI_BEACON);
+	/* A Beacon frame shouldn't have fragment bits set */
+	pwlanhdr->seq_ctrl = 0;
 
 	pframe += sizeof(struct ieee80211_hdr_3addr);
 	pktlen = sizeof (struct ieee80211_hdr_3addr);
@@ -245,9 +245,13 @@ static void ConstructBeacon(struct rtw_adapter *padapter, u8 *pframe, u32 *pLeng
 	pktlen += 2;
 
 	if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) {
+		bcn_fixed_size =
+			offsetof(struct ieee80211_mgmt, u.beacon.variable) -
+			offsetof(struct ieee80211_mgmt, u.beacon);
+
 		/* DBG_8723A("ie len =%d\n", cur_network->IELength); */
-		pktlen += cur_network->IELength - sizeof(struct ndis_802_11_fixed_ies);
-		memcpy(pframe, cur_network->IEs+sizeof(struct ndis_802_11_fixed_ies), pktlen);
+		pktlen += cur_network->IELength - bcn_fixed_size;
+		memcpy(pframe, cur_network->IEs + bcn_fixed_size, pktlen);
 
 		goto _ConstructBeacon;
 	}
@@ -255,30 +259,36 @@ static void ConstructBeacon(struct rtw_adapter *padapter, u8 *pframe, u32 *pLeng
 	/* below for ad-hoc mode */
 
 	/*  SSID */
-	pframe = rtw_set_ie23a(pframe, _SSID_IE_, cur_network->Ssid.ssid_len,
-			    cur_network->Ssid.ssid, &pktlen);
+	pframe = rtw_set_ie23a(pframe, WLAN_EID_SSID,
+			       cur_network->Ssid.ssid_len,
+			       cur_network->Ssid.ssid, &pktlen);
 
 	/*  supported rates... */
 	rate_len = rtw_get_rateset_len23a(cur_network->SupportedRates);
-	pframe = rtw_set_ie23a(pframe, _SUPPORTEDRATES_IE_, ((rate_len > 8) ?
+	pframe = rtw_set_ie23a(pframe, WLAN_EID_SUPP_RATES, ((rate_len > 8) ?
 			       8 : rate_len), cur_network->SupportedRates, &pktlen);
 
 	/*  DS parameter set */
-	pframe = rtw_set_ie23a(pframe, _DSSET_IE_, 1, (unsigned char *)&cur_network->Configuration.DSConfig, &pktlen);
+	pframe = rtw_set_ie23a(pframe, WLAN_EID_DS_PARAMS, 1, (unsigned char *)
+			       &cur_network->DSConfig, &pktlen);
 
 	if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) {
 		u32 ATIMWindow;
 		/*  IBSS Parameter Set... */
-		/* ATIMWindow = cur->Configuration.ATIMWindow; */
+		/* ATIMWindow = cur->ATIMWindow; */
 		ATIMWindow = 0;
-		pframe = rtw_set_ie23a(pframe, _IBSS_PARA_IE_, 2, (unsigned char *)(&ATIMWindow), &pktlen);
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_IBSS_PARAMS, 2,
+				       (unsigned char *)&ATIMWindow, &pktlen);
 	}
 
 	/* todo: ERP IE */
 
 	/*  EXTERNDED SUPPORTED RATE */
 	if (rate_len > 8)
-		pframe = rtw_set_ie23a(pframe, _EXT_SUPPORTEDRATES_IE_, (rate_len - 8), (cur_network->SupportedRates + 8), &pktlen);
+		pframe = rtw_set_ie23a(pframe, WLAN_EID_EXT_SUPP_RATES,
+				       (rate_len - 8),
+				       (cur_network->SupportedRates + 8),
+				       &pktlen);
 
 	/* todo:HT for adhoc */
 
@@ -291,27 +301,26 @@ _ConstructBeacon:
 
 	*pLength = pktlen;
 
-	/* DBG_8723A("%s bcn_sz =%d\n", __FUNCTION__, pktlen); */
+	/* DBG_8723A("%s bcn_sz =%d\n", __func__, pktlen); */
 
 }
 
-static void ConstructPSPoll(struct rtw_adapter *padapter, u8 *pframe, u32 *pLength)
+static void ConstructPSPoll(struct rtw_adapter *padapter,
+			    u8 *pframe, u32 *pLength)
 {
 	struct ieee80211_hdr *pwlanhdr;
-	u16 *fctrl;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
 
 	/*  Frame control. */
-	fctrl = &pwlanhdr->frame_control;
-	*(fctrl) = 0;
-	SetPwrMgt(fctrl);
-	SetFrameSubType(pframe, WIFI_PSPOLL);
+	pwlanhdr->frame_control =
+		cpu_to_le16(IEEE80211_FTYPE_CTL | IEEE80211_STYPE_PSPOLL);
+	pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_PM);
 
 	/*  AID. */
-	SetDuration(pframe, (pmlmeinfo->aid | 0xc000));
+	pwlanhdr->duration_id = cpu_to_le16(pmlmeinfo->aid | 0xc000);
 
 	/*  BSSID. */
 	memcpy(pwlanhdr->addr1, get_my_bssid23a(&pmlmeinfo->network), ETH_ALEN);
@@ -322,49 +331,46 @@ static void ConstructPSPoll(struct rtw_adapter *padapter, u8 *pframe, u32 *pLeng
 	*pLength = 16;
 }
 
-static void ConstructNullFunctionData(
-	struct rtw_adapter *padapter,
-	u8 *pframe,
-	u32 *pLength,
-	u8 *StaAddr,
-	u8 bQoS,
-	u8 AC,
-	u8 bEosp,
-	u8 bForcePowerSave)
+static void
+ConstructNullFunctionData(struct rtw_adapter *padapter, u8 *pframe,
+			  u32 *pLength, u8 *StaAddr, u8 bQoS, u8 AC,
+			  u8 bEosp, u8 bForcePowerSave)
 {
 	struct ieee80211_hdr *pwlanhdr;
-	u16 *fctrl;
 	u32 pktlen;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct wlan_network		*cur_network = &pmlmepriv->cur_network;
+	struct wlan_network *cur_network = &pmlmepriv->cur_network;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
 
-	fctrl = &pwlanhdr->frame_control;
-	*(fctrl) = 0;
+	pwlanhdr->frame_control = 0;
+	pwlanhdr->seq_ctrl = 0;
+
 	if (bForcePowerSave)
-		SetPwrMgt(fctrl);
+		pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_PM);
 
-	switch (cur_network->network.InfrastructureMode) {
-	case Ndis802_11Infrastructure:
-		SetToDs(fctrl);
+	switch (cur_network->network.ifmode) {
+	case NL80211_IFTYPE_P2P_CLIENT:
+	case NL80211_IFTYPE_STATION:
+		pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_TODS);
 		memcpy(pwlanhdr->addr1,
 		       get_my_bssid23a(&pmlmeinfo->network), ETH_ALEN);
 		memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv),
 		       ETH_ALEN);
 		memcpy(pwlanhdr->addr3, StaAddr, ETH_ALEN);
 		break;
-	case Ndis802_11APMode:
-		SetFrDs(fctrl);
+	case NL80211_IFTYPE_P2P_GO:
+	case NL80211_IFTYPE_AP:
+		pwlanhdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_FROMDS);
 		memcpy(pwlanhdr->addr1, StaAddr, ETH_ALEN);
 		memcpy(pwlanhdr->addr2,
 		       get_my_bssid23a(&pmlmeinfo->network), ETH_ALEN);
 		memcpy(pwlanhdr->addr3, myid(&padapter->eeprompriv),
 		       ETH_ALEN);
 		break;
-	case Ndis802_11IBSS:
+	case NL80211_IFTYPE_ADHOC:
 	default:
 		memcpy(pwlanhdr->addr1, StaAddr, ETH_ALEN);
 		memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
@@ -373,20 +379,23 @@ static void ConstructNullFunctionData(
 		break;
 	}
 
-	SetSeqNum(pwlanhdr, 0);
-
 	if (bQoS == true) {
-		struct ieee80211_qos_hdr *pwlanqoshdr;
+		struct ieee80211_qos_hdr *qoshdr;
+		qoshdr = (struct ieee80211_qos_hdr *)pframe;
 
-		SetFrameSubType(pframe, WIFI_QOS_DATA_NULL);
+		qoshdr->frame_control |=
+			cpu_to_le16(IEEE80211_FTYPE_DATA |
+				    IEEE80211_STYPE_QOS_NULLFUNC);
 
-		pwlanqoshdr = (struct ieee80211_qos_hdr *)pframe;
-		SetPriority(&pwlanqoshdr->qos_ctrl, AC);
-		SetEOSP(&pwlanqoshdr->qos_ctrl, bEosp);
+		qoshdr->qos_ctrl = cpu_to_le16(AC & IEEE80211_QOS_CTL_TID_MASK);
+		if (bEosp)
+			qoshdr->qos_ctrl |= cpu_to_le16(IEEE80211_QOS_CTL_EOSP);
 
 		pktlen = sizeof(struct ieee80211_qos_hdr);
 	} else {
-		SetFrameSubType(pframe, WIFI_DATA_NULL);
+		pwlanhdr->frame_control |=
+			cpu_to_le16(IEEE80211_FTYPE_DATA |
+				    IEEE80211_STYPE_NULLFUNC);
 
 		pktlen = sizeof(struct ieee80211_hdr_3addr);
 	}
@@ -394,32 +403,32 @@ static void ConstructNullFunctionData(
 	*pLength = pktlen;
 }
 
-static void ConstructProbeRsp(struct rtw_adapter *padapter, u8 *pframe, u32 *pLength, u8 *StaAddr, bool bHideSSID)
+static void ConstructProbeRsp(struct rtw_adapter *padapter, u8 *pframe,
+			      u32 *pLength, u8 *StaAddr, bool bHideSSID)
 {
 	struct ieee80211_hdr *pwlanhdr;
-	u16 *fctrl;
 	u8 *mac, *bssid;
 	u32 pktlen;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct wlan_bssid_ex *cur_network = &pmlmeinfo->network;
 
-	/* DBG_8723A("%s\n", __FUNCTION__); */
+	/* DBG_8723A("%s\n", __func__); */
 
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
 
 	mac = myid(&padapter->eeprompriv);
 	bssid = cur_network->MacAddress;
 
-	fctrl = &pwlanhdr->frame_control;
-	*(fctrl) = 0;
+	pwlanhdr->frame_control =
+		cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_PROBE_RESP);
+
+	pwlanhdr->seq_ctrl = 0;
+
 	memcpy(pwlanhdr->addr1, StaAddr, ETH_ALEN);
 	memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
 	memcpy(pwlanhdr->addr3, bssid, ETH_ALEN);
 
-	SetSeqNum(pwlanhdr, 0);
-	SetFrameSubType(fctrl, WIFI_PROBERSP);
-
 	pktlen = sizeof(struct ieee80211_hdr_3addr);
 	pframe += pktlen;
 
@@ -464,11 +473,11 @@ static void SetFwRsvdPagePkt(struct rtw_adapter *padapter, bool bDLFinished)
 	u32 TotalPacketLen;
 	struct rsvdpage_loc	RsvdPageLoc;
 
-	DBG_8723A("%s\n", __FUNCTION__);
+	DBG_8723A("%s\n", __func__);
 
 	ReservedPagePacket = kzalloc(1000, GFP_KERNEL);
 	if (ReservedPagePacket == NULL) {
-		DBG_8723A("%s: alloc ReservedPagePacket fail!\n", __FUNCTION__);
+		DBG_8723A("%s: alloc ReservedPagePacket fail!\n", __func__);
 		return;
 	}
 
@@ -507,13 +516,13 @@ static void SetFwRsvdPagePkt(struct rtw_adapter *padapter, bool bDLFinished)
 
 	/* 3 (3) null data */
 	RsvdPageLoc.LocNullData = PageNum;
-	ConstructNullFunctionData(
-		padapter,
-		&ReservedPagePacket[BufIndex],
-		&NullDataLength,
-		get_my_bssid23a(&pmlmeinfo->network),
-		false, 0, 0, false);
-	rtl8723a_fill_fake_txdesc(padapter, &ReservedPagePacket[BufIndex-TxDescLen], NullDataLength, false, false);
+	ConstructNullFunctionData(padapter, &ReservedPagePacket[BufIndex],
+				  &NullDataLength,
+				  get_my_bssid23a(&pmlmeinfo->network),
+				  false, 0, 0, false);
+	rtl8723a_fill_fake_txdesc(padapter,
+				  &ReservedPagePacket[BufIndex-TxDescLen],
+				  NullDataLength, false, false);
 
 	PageNeed = (u8)PageNum_128(TxDescLen + NullDataLength);
 	PageNum += PageNeed;
@@ -573,9 +582,9 @@ static void SetFwRsvdPagePkt(struct rtw_adapter *padapter, bool bDLFinished)
 	pattrib->pktlen = pattrib->last_txcmdsz = TotalPacketLen - TXDESC_OFFSET;
 	memcpy(pmgntframe->buf_addr, ReservedPagePacket, TotalPacketLen);
 
-	rtw_hal_mgnt_xmit23a(padapter, pmgntframe);
+	rtl8723au_mgnt_xmit(padapter, pmgntframe);
 
-	DBG_8723A("%s: Set RSVD page location to Fw\n", __FUNCTION__);
+	DBG_8723A("%s: Set RSVD page location to Fw\n", __func__);
 	FillH2CCmd(padapter, RSVD_PAGE_EID, sizeof(RsvdPageLoc), (u8 *)&RsvdPageLoc);
 
 exit:
@@ -589,7 +598,7 @@ void rtl8723a_set_FwJoinBssReport_cmd(struct rtw_adapter *padapter, u8 mstatus)
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
-	DBG_8723A("%s mstatus(%x)\n", __FUNCTION__, mstatus);
+	DBG_8723A("%s mstatus(%x)\n", __func__, mstatus);
 
 	if (mstatus == 1) {
 		bool bRecover = false;
@@ -597,17 +606,18 @@ void rtl8723a_set_FwJoinBssReport_cmd(struct rtw_adapter *padapter, u8 mstatus)
 
 		/*  We should set AID, correct TSF, HW seq enable before set JoinBssReport to Fw in 88/92C. */
 		/*  Suggested by filen. Added by tynli. */
-		rtw_write16(padapter, REG_BCN_PSR_RPT, (0xC000|pmlmeinfo->aid));
+		rtl8723au_write16(padapter, REG_BCN_PSR_RPT,
+				  0xC000|pmlmeinfo->aid);
 		/*  Do not set TSF again here or vWiFi beacon DMA INT will not work. */
 		/* correct_TSF23a(padapter, pmlmeext); */
 		/*  Hw sequende enable by dedault. 2010.06.23. by tynli. */
-		/* rtw_write16(padapter, REG_NQOS_SEQ, ((pmlmeext->mgnt_seq+100)&0xFFF)); */
-		/* rtw_write8(padapter, REG_HWSEQ_CTRL, 0xFF); */
+		/* rtl8723au_write16(padapter, REG_NQOS_SEQ, ((pmlmeext->mgnt_seq+100)&0xFFF)); */
+		/* rtl8723au_write8(padapter, REG_HWSEQ_CTRL, 0xFF); */
 
 		/*  set REG_CR bit 8 */
-		v8 = rtw_read8(padapter, REG_CR+1);
+		v8 = rtl8723au_read8(padapter, REG_CR+1);
 		v8 |= BIT(0); /*  ENSWBCN */
-		rtw_write8(padapter,  REG_CR+1, v8);
+		rtl8723au_write8(padapter,  REG_CR+1, v8);
 
 		/*  Disable Hw protection for a time which revserd for Hw sending beacon. */
 		/*  Fix download reserved page packet fail that access collision with the protection time. */
@@ -621,8 +631,9 @@ void rtl8723a_set_FwJoinBssReport_cmd(struct rtw_adapter *padapter, u8 mstatus)
 			bRecover = true;
 
 		/*  To tell Hw the packet is not a real beacon frame. */
-		/* U1bTmp = rtw_read8(padapter, REG_FWHW_TXQ_CTRL+2); */
-		rtw_write8(padapter, REG_FWHW_TXQ_CTRL+2, pHalData->RegFwHwTxQCtrl & ~BIT(6));
+		/* U1bTmp = rtl8723au_read8(padapter, REG_FWHW_TXQ_CTRL+2); */
+		rtl8723au_write8(padapter, REG_FWHW_TXQ_CTRL + 2,
+				 pHalData->RegFwHwTxQCtrl & ~BIT(6));
 		pHalData->RegFwHwTxQCtrl &= ~BIT(6);
 		SetFwRsvdPagePkt(padapter, 0);
 
@@ -635,14 +646,15 @@ void rtl8723a_set_FwJoinBssReport_cmd(struct rtw_adapter *padapter, u8 mstatus)
 		/*  the beacon cannot be sent by HW. */
 		/*  2010.06.23. Added by tynli. */
 		if (bRecover) {
-			rtw_write8(padapter, REG_FWHW_TXQ_CTRL+2, pHalData->RegFwHwTxQCtrl | BIT(6));
+			rtl8723au_write8(padapter, REG_FWHW_TXQ_CTRL + 2,
+					 pHalData->RegFwHwTxQCtrl | BIT(6));
 			pHalData->RegFwHwTxQCtrl |= BIT(6);
 		}
 
 		/*  Clear CR[8] or beacon packet will not be send to TxBuf anymore. */
-		v8 = rtw_read8(padapter, REG_CR+1);
+		v8 = rtl8723au_read8(padapter, REG_CR+1);
 		v8 &= ~BIT(0); /*  ~ENSWBCN */
-		rtw_write8(padapter, REG_CR+1, v8);
+		rtl8723au_write8(padapter, REG_CR+1, v8);
 	}
 
 	JoinBssRptParm.OpMode = mstatus;
@@ -668,11 +680,11 @@ static void SetFwRsvdPagePkt_BTCoex(struct rtw_adapter *padapter)
 	u32 TotalPacketLen;
 	struct rsvdpage_loc	RsvdPageLoc;
 
-	DBG_8723A("+%s\n", __FUNCTION__);
+	DBG_8723A("+%s\n", __func__);
 
 	ReservedPagePacket = kzalloc(1024, GFP_KERNEL);
 	if (ReservedPagePacket == NULL) {
-		DBG_8723A("%s: alloc ReservedPagePacket fail!\n", __FUNCTION__);
+		DBG_8723A("%s: alloc ReservedPagePacket fail!\n", __func__);
 		return;
 	}
 
@@ -732,9 +744,9 @@ static void SetFwRsvdPagePkt_BTCoex(struct rtw_adapter *padapter)
 	pattrib->pktlen = pattrib->last_txcmdsz = TotalPacketLen - TXDESC_OFFSET;
 	memcpy(pmgntframe->buf_addr, ReservedPagePacket, TotalPacketLen);
 
-	rtw_hal_mgnt_xmit23a(padapter, pmgntframe);
+	rtl8723au_mgnt_xmit(padapter, pmgntframe);
 
-	DBG_8723A("%s: Set RSVD page location to Fw\n", __FUNCTION__);
+	DBG_8723A("%s: Set RSVD page location to Fw\n", __func__);
 	FillH2CCmd(padapter, RSVD_PAGE_EID, sizeof(RsvdPageLoc), (u8 *)&RsvdPageLoc);
 
 exit:
@@ -746,7 +758,7 @@ void rtl8723a_set_BTCoex_AP_mode_FwRsvdPkt_cmd(struct rtw_adapter *padapter)
 	struct hal_data_8723a *pHalData;
 	u8 bRecover = false;
 
-	DBG_8723A("+%s\n", __FUNCTION__);
+	DBG_8723A("+%s\n", __func__);
 
 	pHalData = GET_HAL_DATA(padapter);
 
@@ -756,7 +768,8 @@ void rtl8723a_set_BTCoex_AP_mode_FwRsvdPkt_cmd(struct rtw_adapter *padapter)
 
 	/*  To tell Hw the packet is not a real beacon frame. */
 	pHalData->RegFwHwTxQCtrl &= ~BIT(6);
-	rtw_write8(padapter, REG_FWHW_TXQ_CTRL+2, pHalData->RegFwHwTxQCtrl);
+	rtl8723au_write8(padapter, REG_FWHW_TXQ_CTRL + 2,
+			 pHalData->RegFwHwTxQCtrl);
 	SetFwRsvdPagePkt_BTCoex(padapter);
 
 	/*  To make sure that if there exists an adapter which would like to send beacon. */
@@ -766,80 +779,8 @@ void rtl8723a_set_BTCoex_AP_mode_FwRsvdPkt_cmd(struct rtw_adapter *padapter)
 	/*  2010.06.23. Added by tynli. */
 	if (bRecover) {
 		pHalData->RegFwHwTxQCtrl |= BIT(6);
-		rtw_write8(padapter, REG_FWHW_TXQ_CTRL+2, pHalData->RegFwHwTxQCtrl);
+		rtl8723au_write8(padapter, REG_FWHW_TXQ_CTRL + 2,
+				 pHalData->RegFwHwTxQCtrl);
 	}
 }
 #endif
-
-#ifdef CONFIG_8723AU_P2P
-void rtl8723a_set_p2p_ps_offload_cmd(struct rtw_adapter *padapter, u8 p2p_ps_state)
-{
-	struct hal_data_8723a	*pHalData = GET_HAL_DATA(padapter);
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-	struct P2P_PS_Offload_t	*p2p_ps_offload = &pHalData->p2p_ps_offload;
-	u8 i;
-
-	switch (p2p_ps_state) {
-	case P2P_PS_DISABLE:
-		DBG_8723A("P2P_PS_DISABLE \n");
-		memset(p2p_ps_offload, 0, 1);
-		break;
-	case P2P_PS_ENABLE:
-		DBG_8723A("P2P_PS_ENABLE \n");
-		/*  update CTWindow value. */
-		if (pwdinfo->ctwindow > 0) {
-			p2p_ps_offload->CTWindow_En = 1;
-			rtw_write8(padapter, REG_P2P_CTWIN, pwdinfo->ctwindow);
-		}
-
-		/*  hw only support 2 set of NoA */
-		for (i = 0; i < pwdinfo->noa_num; i++) {
-			/*  To control the register setting for which NOA */
-			rtw_write8(padapter, REG_NOA_DESC_SEL, (i << 4));
-			if (i == 0)
-				p2p_ps_offload->NoA0_En = 1;
-			else
-				p2p_ps_offload->NoA1_En = 1;
-
-			/*  config P2P NoA Descriptor Register */
-			rtw_write32(padapter, REG_NOA_DESC_DURATION, pwdinfo->noa_duration[i]);
-
-			rtw_write32(padapter, REG_NOA_DESC_INTERVAL, pwdinfo->noa_interval[i]);
-
-			rtw_write32(padapter, REG_NOA_DESC_START, pwdinfo->noa_start_time[i]);
-
-			rtw_write8(padapter, REG_NOA_DESC_COUNT, pwdinfo->noa_count[i]);
-		}
-
-		if ((pwdinfo->opp_ps == 1) || (pwdinfo->noa_num > 0)) {
-			/*  rst p2p circuit */
-			rtw_write8(padapter, REG_DUAL_TSF_RST, BIT(4));
-
-			p2p_ps_offload->Offload_En = 1;
-
-			if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
-				p2p_ps_offload->role = 1;
-				p2p_ps_offload->AllStaSleep = 0;
-			} else {
-				p2p_ps_offload->role = 0;
-			}
-
-			p2p_ps_offload->discovery = 0;
-		}
-		break;
-	case P2P_PS_SCAN:
-		DBG_8723A("P2P_PS_SCAN \n");
-		p2p_ps_offload->discovery = 1;
-		break;
-	case P2P_PS_SCAN_DONE:
-		DBG_8723A("P2P_PS_SCAN_DONE \n");
-		p2p_ps_offload->discovery = 0;
-		pwdinfo->p2p_ps_state = P2P_PS_ENABLE;
-		break;
-	default:
-		break;
-	}
-
-	FillH2CCmd(padapter, P2P_PS_OFFLOAD_EID, 1, (u8 *)p2p_ps_offload);
-}
-#endif /* CONFIG_8723AU_P2P */
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_dm.c b/drivers/staging/rtl8723au/hal/rtl8723a_dm.c
index f204ab1714e..ac47a971108 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723a_dm.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723a_dm.c
@@ -28,6 +28,7 @@
 #include <drv_types.h>
 
 #include <rtl8723a_hal.h>
+#include <usb_ops_linux.h>
 
 /*  */
 /*  Global var */
@@ -45,18 +46,21 @@ static void dm_CheckPbcGPIO(struct rtw_adapter *padapter)
 	if (!padapter->registrypriv.hw_wps_pbc)
 		return;
 
-	tmp1byte = rtw_read8(padapter, GPIO_IO_SEL);
+	tmp1byte = rtl8723au_read8(padapter, GPIO_IO_SEL);
 	tmp1byte |= (HAL_8192C_HW_GPIO_WPS_BIT);
-	rtw_write8(padapter, GPIO_IO_SEL, tmp1byte);	/* enable GPIO[2] as output mode */
+	/* enable GPIO[2] as output mode */
+	rtl8723au_write8(padapter, GPIO_IO_SEL, tmp1byte);
 
 	tmp1byte &= ~(HAL_8192C_HW_GPIO_WPS_BIT);
-	rtw_write8(padapter,  GPIO_IN, tmp1byte);		/* reset the floating voltage level */
+	/* reset the floating voltage level */
+	rtl8723au_write8(padapter,  GPIO_IN, tmp1byte);
 
-	tmp1byte = rtw_read8(padapter, GPIO_IO_SEL);
+	tmp1byte = rtl8723au_read8(padapter, GPIO_IO_SEL);
 	tmp1byte &= ~(HAL_8192C_HW_GPIO_WPS_BIT);
-	rtw_write8(padapter, GPIO_IO_SEL, tmp1byte);	/* enable GPIO[2] as input mode */
+	/* enable GPIO[2] as input mode */
+	rtl8723au_write8(padapter, GPIO_IO_SEL, tmp1byte);
 
-	tmp1byte = rtw_read8(padapter, GPIO_IN);
+	tmp1byte = rtl8723au_read8(padapter, GPIO_IN);
 
 	if (tmp1byte == 0xff)
 		return;
@@ -76,7 +80,7 @@ static void dm_CheckPbcGPIO(struct rtw_adapter *padapter)
 			return;
 		}
 
-		rtw_signal_process(padapter->pid[0], SIGUSR1);
+		kill_pid(find_vpid(padapter->pid[0]), SIGUSR1, 1);
 	}
 }
 
@@ -197,7 +201,7 @@ void rtl8723a_InitHalDm(struct rtw_adapter *Adapter)
 	ODM23a_DMInit(pDM_Odm);
 	/*  Save REG_INIDATA_RATE_SEL value for TXDESC. */
 	for (i = 0; i < 32; i++)
-		pdmpriv->INIDATA_RATE[i] = rtw_read8(Adapter, REG_INIDATA_RATE_SEL+i) & 0x3f;
+		pdmpriv->INIDATA_RATE[i] = rtl8723au_read8(Adapter, REG_INIDATA_RATE_SEL+i) & 0x3f;
 }
 
 void
@@ -217,14 +221,7 @@ rtl8723a_HalDmWatchDog(
 		goto skip_dm;
 
 	bFwCurrentInPSMode = Adapter->pwrctrlpriv.bFwCurrentInPSMode;
-	rtw23a_hal_get_hwreg(Adapter, HW_VAR_FWLPS_RF_ON, (u8 *)(&bFwPSAwake));
-
-#ifdef CONFIG_8723AU_P2P
-	/*  Fw is under p2p powersaving mode, driver should stop dynamic mechanism. */
-	/*  modifed by thomas. 2011.06.11. */
-	if (Adapter->wdinfo.p2p_ps_mode)
-		bFwPSAwake = false;
-#endif /* CONFIG_8723AU_P2P */
+	bFwPSAwake = rtl8723a_get_fwlps_rf_on(Adapter);
 
 	if ((hw_init_completed) && ((!bFwCurrentInPSMode) && bFwPSAwake)) {
 		/*  Calculate Tx/Rx statistics. */
@@ -232,11 +229,11 @@ rtl8723a_HalDmWatchDog(
 
 		/*  Read REG_INIDATA_RATE_SEL value for TXDESC. */
 		if (check_fwstate(&Adapter->mlmepriv, WIFI_STATION_STATE)) {
-			pdmpriv->INIDATA_RATE[0] = rtw_read8(Adapter, REG_INIDATA_RATE_SEL) & 0x3f;
+			pdmpriv->INIDATA_RATE[0] = rtl8723au_read8(Adapter, REG_INIDATA_RATE_SEL) & 0x3f;
 		} else {
 			u8	i;
 			for (i = 1 ; i < (Adapter->stapriv.asoc_sta_count + 1); i++)
-				pdmpriv->INIDATA_RATE[i] = rtw_read8(Adapter, (REG_INIDATA_RATE_SEL+i)) & 0x3f;
+				pdmpriv->INIDATA_RATE[i] = rtl8723au_read8(Adapter, (REG_INIDATA_RATE_SEL+i)) & 0x3f;
 		}
 	}
 
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_hal_init.c b/drivers/staging/rtl8723au/hal/rtl8723a_hal_init.c
index 0982b0a4ab9..0acacab95a4 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723a_hal_init.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723a_hal_init.c
@@ -19,6 +19,7 @@
 #include <rtw_efuse.h>
 
 #include <rtl8723a_hal.h>
+#include <usb_ops_linux.h>
 
 static void _FWDownloadEnable(struct rtw_adapter *padapter, bool enable)
 {
@@ -26,23 +27,23 @@ static void _FWDownloadEnable(struct rtw_adapter *padapter, bool enable)
 
 	if (enable) {
 		/*  8051 enable */
-		tmp = rtw_read8(padapter, REG_SYS_FUNC_EN + 1);
-		rtw_write8(padapter, REG_SYS_FUNC_EN + 1, tmp | 0x04);
+		tmp = rtl8723au_read8(padapter, REG_SYS_FUNC_EN + 1);
+		rtl8723au_write8(padapter, REG_SYS_FUNC_EN + 1, tmp | 0x04);
 
 		/*  MCU firmware download enable. */
-		tmp = rtw_read8(padapter, REG_MCUFWDL);
-		rtw_write8(padapter, REG_MCUFWDL, tmp | 0x01);
+		tmp = rtl8723au_read8(padapter, REG_MCUFWDL);
+		rtl8723au_write8(padapter, REG_MCUFWDL, tmp | 0x01);
 
 		/*  8051 reset */
-		tmp = rtw_read8(padapter, REG_MCUFWDL + 2);
-		rtw_write8(padapter, REG_MCUFWDL + 2, tmp & 0xf7);
+		tmp = rtl8723au_read8(padapter, REG_MCUFWDL + 2);
+		rtl8723au_write8(padapter, REG_MCUFWDL + 2, tmp & 0xf7);
 	} else {
 		/*  MCU firmware download disable. */
-		tmp = rtw_read8(padapter, REG_MCUFWDL);
-		rtw_write8(padapter, REG_MCUFWDL, tmp & 0xfe);
+		tmp = rtl8723au_read8(padapter, REG_MCUFWDL);
+		rtl8723au_write8(padapter, REG_MCUFWDL, tmp & 0xfe);
 
 		/*  Reserved for fw extension. */
-		rtw_write8(padapter, REG_MCUFWDL + 1, 0x00);
+		rtl8723au_write8(padapter, REG_MCUFWDL + 1, 0x00);
 	}
 }
 
@@ -75,9 +76,10 @@ static int _BlockWrite(struct rtw_adapter *padapter, void *buffer, u32 buffSize)
 	}
 
 	for (i = 0; i < blockCount_p1; i++) {
-		ret = rtw_writeN(padapter,
-				 (FW_8723A_START_ADDRESS + i * blockSize_p1),
-				 blockSize_p1, (bufferPtr + i * blockSize_p1));
+		ret = rtl8723au_writeN(padapter, (FW_8723A_START_ADDRESS +
+						  i * blockSize_p1),
+				       blockSize_p1,
+				       (bufferPtr + i * blockSize_p1));
 		if (ret == _FAIL)
 			goto exit;
 	}
@@ -99,11 +101,12 @@ static int _BlockWrite(struct rtw_adapter *padapter, void *buffer, u32 buffSize)
 		}
 
 		for (i = 0; i < blockCount_p2; i++) {
-			ret = rtw_writeN(padapter,
-					 (FW_8723A_START_ADDRESS + offset +
-					  i * blockSize_p2), blockSize_p2,
-					 (bufferPtr + offset +
-					  i * blockSize_p2));
+			ret = rtl8723au_writeN(padapter,
+					       (FW_8723A_START_ADDRESS +
+						offset + i * blockSize_p2),
+					       blockSize_p2,
+					       (bufferPtr + offset +
+						i * blockSize_p2));
 
 			if (ret == _FAIL)
 				goto exit;
@@ -123,9 +126,9 @@ static int _BlockWrite(struct rtw_adapter *padapter, void *buffer, u32 buffSize)
 			  (buffSize - offset), blockSize_p3, blockCount_p3));
 
 		for (i = 0; i < blockCount_p3; i++) {
-			ret = rtw_write8(padapter,
-					 (FW_8723A_START_ADDRESS + offset + i),
-					 *(bufferPtr + offset + i));
+			ret = rtl8723au_write8(padapter,
+					       (FW_8723A_START_ADDRESS + offset + i),
+					       *(bufferPtr + offset + i));
 
 			if (ret == _FAIL)
 				goto exit;
@@ -142,8 +145,8 @@ _PageWrite(struct rtw_adapter *padapter, u32 page, void *buffer, u32 size)
 	u8 value8;
 	u8 u8Page = (u8) (page & 0x07);
 
-	value8 = (rtw_read8(padapter, REG_MCUFWDL + 2) & 0xF8) | u8Page;
-	rtw_write8(padapter, REG_MCUFWDL + 2, value8);
+	value8 = (rtl8723au_read8(padapter, REG_MCUFWDL + 2) & 0xF8) | u8Page;
+	rtl8723au_write8(padapter, REG_MCUFWDL + 2, value8);
 
 	return _BlockWrite(padapter, buffer, size);
 }
@@ -187,14 +190,14 @@ exit:
 	return ret;
 }
 
-static s32 _FWFreeToGo(struct rtw_adapter *padapter)
+static int _FWFreeToGo(struct rtw_adapter *padapter)
 {
 	u32 counter = 0;
 	u32 value32;
 
 	/*  polling CheckSum report */
 	do {
-		value32 = rtw_read32(padapter, REG_MCUFWDL);
+		value32 = rtl8723au_read32(padapter, REG_MCUFWDL);
 		if (value32 & FWDL_ChkSum_rpt)
 			break;
 	} while (counter++ < POLLING_READY_TIMEOUT_COUNT);
@@ -209,15 +212,15 @@ static s32 _FWFreeToGo(struct rtw_adapter *padapter)
 		 ("%s: Checksum report OK! REG_MCUFWDL:0x%08x\n", __func__,
 		  value32));
 
-	value32 = rtw_read32(padapter, REG_MCUFWDL);
+	value32 = rtl8723au_read32(padapter, REG_MCUFWDL);
 	value32 |= MCUFWDL_RDY;
 	value32 &= ~WINTINI_RDY;
-	rtw_write32(padapter, REG_MCUFWDL, value32);
+	rtl8723au_write32(padapter, REG_MCUFWDL, value32);
 
 	/*  polling for FW ready */
 	counter = 0;
 	do {
-		value32 = rtw_read32(padapter, REG_MCUFWDL);
+		value32 = rtl8723au_read32(padapter, REG_MCUFWDL);
 		if (value32 & WINTINI_RDY) {
 			RT_TRACE(_module_hal_init_c_, _drv_info_,
 				 ("%s: Polling FW ready success!! "
@@ -248,15 +251,15 @@ void rtl8723a_FirmwareSelfReset(struct rtw_adapter *padapter)
 		pHalData->FirmwareSubVersion < 0x01)))) {
 		/*  after 88C Fw v33.1 */
 		/* 0x1cf = 0x20. Inform 8051 to reset. 2009.12.25. tynli_test */
-		rtw_write8(padapter, REG_HMETFR + 3, 0x20);
+		rtl8723au_write8(padapter, REG_HMETFR + 3, 0x20);
 
-		u1bTmp = rtw_read8(padapter, REG_SYS_FUNC_EN + 1);
-		while (u1bTmp & BIT2) {
+		u1bTmp = rtl8723au_read8(padapter, REG_SYS_FUNC_EN + 1);
+		while (u1bTmp & BIT(2)) {
 			Delay--;
 			if (Delay == 0)
 				break;
 			udelay(50);
-			u1bTmp = rtw_read8(padapter, REG_SYS_FUNC_EN + 1);
+			u1bTmp = rtl8723au_read8(padapter, REG_SYS_FUNC_EN + 1);
 		}
 		RT_TRACE(_module_hal_init_c_, _drv_info_,
 			 ("-%s: 8051 reset success (%d)\n", __func__,
@@ -264,9 +267,9 @@ void rtl8723a_FirmwareSelfReset(struct rtw_adapter *padapter)
 
 		if ((Delay == 0)) {
 			/* force firmware reset */
-			u1bTmp = rtw_read8(padapter, REG_SYS_FUNC_EN + 1);
-			rtw_write8(padapter, REG_SYS_FUNC_EN + 1,
-				   u1bTmp & (~BIT2));
+			u1bTmp = rtl8723au_read8(padapter, REG_SYS_FUNC_EN + 1);
+			rtl8723au_write8(padapter, REG_SYS_FUNC_EN + 1,
+					 u1bTmp & ~BIT(2));
 		}
 	}
 }
@@ -276,9 +279,9 @@ void rtl8723a_FirmwareSelfReset(struct rtw_adapter *padapter)
 /*		Download 8192C firmware code. */
 /*  */
 /*  */
-s32 rtl8723a_FirmwareDownload(struct rtw_adapter *padapter)
+int rtl8723a_FirmwareDownload(struct rtw_adapter *padapter)
 {
-	s32 rtStatus = _SUCCESS;
+	int rtStatus = _SUCCESS;
 	u8 writeFW_retry = 0;
 	unsigned long fwdl_start_time;
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
@@ -306,15 +309,15 @@ s32 rtl8723a_FirmwareDownload(struct rtw_adapter *padapter)
 			DBG_8723A(" Rtl8723_FwUMCBCutImageArrayWithoutBT for "
 				  "RTL8723A B CUT\n");
 		} else {
-#ifdef CONFIG_8723AU_BT_COEXIST
-			fw_name = "rtlwifi/rtl8723aufw_B.bin";
-			DBG_8723A(" Rtl8723_FwUMCBCutImageArrayWithBT for "
-				  "RTL8723A B CUT\n");
-#else
-			fw_name = "rtlwifi/rtl8723aufw_B_NoBT.bin";
-			DBG_8723A(" Rtl8723_FwUMCBCutImageArrayWithoutBT for "
-				  "RTL8723A B CUT\n");
-#endif
+			if (rtl8723a_BT_coexist(padapter)) {
+				fw_name = "rtlwifi/rtl8723aufw_B.bin";
+				DBG_8723A(" Rtl8723_FwUMCBCutImageArrayWithBT "
+					  "for RTL8723A B CUT\n");
+			} else {
+				fw_name = "rtlwifi/rtl8723aufw_B_NoBT.bin";
+				DBG_8723A(" Rtl8723_FwUMCBCutImageArrayWithout "
+					  "BT for RTL8723A B CUT\n");
+			}
 		}
 	} else {
 		/*  <Roger_TODO> We should download proper RAM Code here
@@ -336,12 +339,11 @@ s32 rtl8723a_FirmwareDownload(struct rtw_adapter *padapter)
 		rtStatus = _FAIL;
 		goto Exit;
 	}
-	firmware_buf = kzalloc(fw->size, GFP_KERNEL);
+	firmware_buf = kmemdup(fw->data, fw->size, GFP_KERNEL);
 	if (!firmware_buf) {
 		rtStatus = _FAIL;
 		goto Exit;
 	}
-	memcpy(firmware_buf, fw->data, fw->size);
 	buf = firmware_buf;
 	fw_size = fw->size;
 	release_firmware(fw);
@@ -372,18 +374,19 @@ s32 rtl8723a_FirmwareDownload(struct rtw_adapter *padapter)
 	/*  Suggested by Filen. If 8051 is running in RAM code, driver should
 	    inform Fw to reset by itself, */
 	/*  or it will cause download Fw fail. 2010.02.01. by tynli. */
-	if (rtw_read8(padapter, REG_MCUFWDL) & RAM_DL_SEL) {
+	if (rtl8723au_read8(padapter, REG_MCUFWDL) & RAM_DL_SEL) {
 		/* 8051 RAM code */
 		rtl8723a_FirmwareSelfReset(padapter);
-		rtw_write8(padapter, REG_MCUFWDL, 0x00);
+		rtl8723au_write8(padapter, REG_MCUFWDL, 0x00);
 	}
 
 	_FWDownloadEnable(padapter, true);
 	fwdl_start_time = jiffies;
 	while (1) {
 		/* reset the FWDL chksum */
-		rtw_write8(padapter, REG_MCUFWDL,
-			   rtw_read8(padapter, REG_MCUFWDL) | FWDL_ChkSum_rpt);
+		rtl8723au_write8(padapter, REG_MCUFWDL,
+				 rtl8723au_read8(padapter, REG_MCUFWDL) |
+				 FWDL_ChkSum_rpt);
 
 		rtStatus = _WriteFW(padapter, buf, fw_size);
 
@@ -427,14 +430,6 @@ void rtl8723a_InitializeFirmwareVars(struct rtw_adapter *padapter)
 	pHalData->LastHMEBoxNum = 0;
 }
 
-static void rtl8723a_free_hal_data(struct rtw_adapter *padapter)
-{
-
-	kfree(padapter->HalData);
-	padapter->HalData = NULL;
-
-}
-
 /*  */
 /*				Efuse related code */
 /*  */
@@ -445,7 +440,7 @@ hal_EfuseSwitchToBank(struct rtw_adapter *padapter, u8 bank)
 	u32 value32 = 0;
 
 	DBG_8723A("%s: Efuse switch bank to %d\n", __func__, bank);
-	value32 = rtw_read32(padapter, EFUSE_TEST);
+	value32 = rtl8723au_read32(padapter, EFUSE_TEST);
 	bRet = true;
 	switch (bank) {
 	case 0:
@@ -470,147 +465,12 @@ hal_EfuseSwitchToBank(struct rtw_adapter *padapter, u8 bank)
 		bRet = false;
 		break;
 	}
-	rtw_write32(padapter, EFUSE_TEST, value32);
+	rtl8723au_write32(padapter, EFUSE_TEST, value32);
 
 	return bRet;
 }
 
 static void
-Hal_GetEfuseDefinition(struct rtw_adapter *padapter,
-		       u8 efuseType, u8 type, void *pOut)
-{
-	u8 *pu1Tmp;
-	u16 *pu2Tmp;
-	u8 *pMax_section;
-
-	switch (type) {
-	case TYPE_EFUSE_MAX_SECTION:
-		pMax_section = (u8 *) pOut;
-
-		if (efuseType == EFUSE_WIFI)
-			*pMax_section = EFUSE_MAX_SECTION_8723A;
-		else
-			*pMax_section = EFUSE_BT_MAX_SECTION;
-		break;
-
-	case TYPE_EFUSE_REAL_CONTENT_LEN:
-		pu2Tmp = (u16 *) pOut;
-
-		if (efuseType == EFUSE_WIFI)
-			*pu2Tmp = EFUSE_REAL_CONTENT_LEN_8723A;
-		else
-			*pu2Tmp = EFUSE_BT_REAL_CONTENT_LEN;
-		break;
-
-	case TYPE_AVAILABLE_EFUSE_BYTES_BANK:
-		pu2Tmp = (u16 *) pOut;
-
-		if (efuseType == EFUSE_WIFI)
-			*pu2Tmp = (EFUSE_REAL_CONTENT_LEN_8723A -
-				   EFUSE_OOB_PROTECT_BYTES);
-		else
-			*pu2Tmp = (EFUSE_BT_REAL_BANK_CONTENT_LEN -
-				   EFUSE_PROTECT_BYTES_BANK);
-		break;
-
-	case TYPE_AVAILABLE_EFUSE_BYTES_TOTAL:
-		pu2Tmp = (u16 *) pOut;
-
-		if (efuseType == EFUSE_WIFI)
-			*pu2Tmp = (EFUSE_REAL_CONTENT_LEN_8723A -
-				   EFUSE_OOB_PROTECT_BYTES);
-		else
-			*pu2Tmp = (EFUSE_BT_REAL_CONTENT_LEN -
-				   (EFUSE_PROTECT_BYTES_BANK * 3));
-		break;
-
-	case TYPE_EFUSE_MAP_LEN:
-		pu2Tmp = (u16 *) pOut;
-
-		if (efuseType == EFUSE_WIFI)
-			*pu2Tmp = EFUSE_MAP_LEN_8723A;
-		else
-			*pu2Tmp = EFUSE_BT_MAP_LEN;
-		break;
-
-	case TYPE_EFUSE_PROTECT_BYTES_BANK:
-		pu1Tmp = (u8 *) pOut;
-
-		if (efuseType == EFUSE_WIFI)
-			*pu1Tmp = EFUSE_OOB_PROTECT_BYTES;
-		else
-			*pu1Tmp = EFUSE_PROTECT_BYTES_BANK;
-		break;
-
-	case TYPE_EFUSE_CONTENT_LEN_BANK:
-		pu2Tmp = (u16 *) pOut;
-
-		if (efuseType == EFUSE_WIFI)
-			*pu2Tmp = EFUSE_REAL_CONTENT_LEN_8723A;
-		else
-			*pu2Tmp = EFUSE_BT_REAL_BANK_CONTENT_LEN;
-		break;
-
-	default:
-		pu1Tmp = (u8 *) pOut;
-		*pu1Tmp = 0;
-		break;
-	}
-}
-
-#define VOLTAGE_V25		0x03
-#define LDOE25_SHIFT	28
-
-static void
-Hal_EfusePowerSwitch(struct rtw_adapter *padapter, u8 bWrite, u8 PwrState)
-{
-	u8 tempval;
-	u16 tmpV16;
-
-	if (PwrState == true) {
-		rtw_write8(padapter, REG_EFUSE_ACCESS, EFUSE_ACCESS_ON);
-
-		/*  1.2V Power: From VDDON with Power
-		    Cut(0x0000h[15]), defualt valid */
-		tmpV16 = rtw_read16(padapter, REG_SYS_ISO_CTRL);
-		if (!(tmpV16 & PWC_EV12V)) {
-			tmpV16 |= PWC_EV12V;
-			rtw_write16(padapter, REG_SYS_ISO_CTRL, tmpV16);
-		}
-		/*  Reset: 0x0000h[28], default valid */
-		tmpV16 = rtw_read16(padapter, REG_SYS_FUNC_EN);
-		if (!(tmpV16 & FEN_ELDR)) {
-			tmpV16 |= FEN_ELDR;
-			rtw_write16(padapter, REG_SYS_FUNC_EN, tmpV16);
-		}
-
-		/*  Clock: Gated(0x0008h[5]) 8M(0x0008h[1]) clock
-		    from ANA, default valid */
-		tmpV16 = rtw_read16(padapter, REG_SYS_CLKR);
-		if ((!(tmpV16 & LOADER_CLK_EN)) || (!(tmpV16 & ANA8M))) {
-			tmpV16 |= (LOADER_CLK_EN | ANA8M);
-			rtw_write16(padapter, REG_SYS_CLKR, tmpV16);
-		}
-
-		if (bWrite == true) {
-			/*  Enable LDO 2.5V before read/write action */
-			tempval = rtw_read8(padapter, EFUSE_TEST + 3);
-			tempval &= 0x0F;
-			tempval |= (VOLTAGE_V25 << 4);
-			rtw_write8(padapter, EFUSE_TEST + 3, (tempval | 0x80));
-		}
-	} else {
-		rtw_write8(padapter, REG_EFUSE_ACCESS, EFUSE_ACCESS_OFF);
-
-		if (bWrite == true) {
-			/*  Disable LDO 2.5V after read/write action */
-			tempval = rtw_read8(padapter, EFUSE_TEST + 3);
-			rtw_write8(padapter, EFUSE_TEST + 3, (tempval & 0x7F));
-		}
-	}
-}
-
-static void
 hal_ReadEFuse_WiFi(struct rtw_adapter *padapter,
 		   u16 _offset, u16 _size_byte, u8 *pbuf)
 {
@@ -619,6 +479,7 @@ hal_ReadEFuse_WiFi(struct rtw_adapter *padapter,
 	u8 offset, wden;
 	u8 efuseHeader, efuseExtHdr, efuseData;
 	u16 i, total, used;
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
 	/*  Do NOT excess total size of EFuse table.
 	    Added by Roger, 2008.11.10. */
@@ -628,7 +489,7 @@ hal_ReadEFuse_WiFi(struct rtw_adapter *padapter,
 		return;
 	}
 
-	efuseTbl = (u8 *) kmalloc(EFUSE_MAP_LEN_8723A, GFP_KERNEL);
+	efuseTbl = kmalloc(EFUSE_MAP_LEN_8723A, GFP_KERNEL);
 	if (efuseTbl == NULL) {
 		DBG_8723A("%s: alloc efuseTbl fail!\n", __func__);
 		return;
@@ -696,7 +557,7 @@ hal_ReadEFuse_WiFi(struct rtw_adapter *padapter,
 	EFUSE_GetEfuseDefinition23a(padapter, EFUSE_WIFI,
 				 TYPE_AVAILABLE_EFUSE_BYTES_TOTAL, &total);
 	used = eFuse_Addr - 1;
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_EFUSE_BYTES, (u8 *)&used);
+	pHalData->EfuseUsedBytes = used;
 
 	kfree(efuseTbl);
 }
@@ -711,6 +572,7 @@ hal_ReadEFuse_BT(struct rtw_adapter *padapter,
 	u8 efuseHeader, efuseExtHdr, efuseData;
 	u8 offset, wden;
 	u16 i, total, used;
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
 	/*  Do NOT excess total size of EFuse table.
 	    Added by Roger, 2008.11.10. */
@@ -812,15 +674,15 @@ hal_ReadEFuse_BT(struct rtw_adapter *padapter,
 	EFUSE_GetEfuseDefinition23a(padapter, EFUSE_BT,
 				 TYPE_AVAILABLE_EFUSE_BYTES_TOTAL, &total);
 	used = (EFUSE_BT_REAL_BANK_CONTENT_LEN * (bank - 1)) + eFuse_Addr - 1;
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_EFUSE_BT_BYTES, (u8 *) &used);
+	pHalData->BTEfuseUsedBytes = used;
 
 exit:
 	kfree(efuseTbl);
 }
 
-static void
-Hal_ReadEFuse(struct rtw_adapter *padapter,
-	      u8 efuseType, u16 _offset, u16 _size_byte, u8 *pbuf)
+void
+rtl8723a_readefuse(struct rtw_adapter *padapter,
+		   u8 efuseType, u16 _offset, u16 _size_byte, u8 *pbuf)
 {
 	if (efuseType == EFUSE_WIFI)
 		hal_ReadEFuse_WiFi(padapter, _offset, _size_byte, pbuf);
@@ -828,14 +690,14 @@ Hal_ReadEFuse(struct rtw_adapter *padapter,
 		hal_ReadEFuse_BT(padapter, _offset, _size_byte, pbuf);
 }
 
-static u16
-hal_EfuseGetCurrentSize_WiFi(struct rtw_adapter *padapter)
+u16 rtl8723a_EfuseGetCurrentSize_WiFi(struct rtw_adapter *padapter)
 {
 	u16 efuse_addr = 0;
 	u8 hoffset = 0, hworden = 0;
 	u8 efuse_data, word_cnts = 0;
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
-	rtw23a_hal_get_hwreg(padapter, HW_VAR_EFUSE_BYTES, (u8 *) &efuse_addr);
+	efuse_addr = pHalData->EfuseUsedBytes;
 
 	DBG_8723A("%s: start_efuse_addr = 0x%X\n", __func__, efuse_addr);
 
@@ -844,7 +706,7 @@ hal_EfuseGetCurrentSize_WiFi(struct rtw_adapter *padapter)
 
 	while (AVAILABLE_EFUSE_ADDR(efuse_addr)) {
 		if (efuse_OneByteRead23a(padapter, efuse_addr, &efuse_data) ==
-		    false) {
+		    _FAIL) {
 			DBG_8723A(KERN_ERR "%s: efuse_OneByteRead23a Fail! "
 				  "addr = 0x%X !!\n", __func__, efuse_addr);
 			break;
@@ -872,15 +734,14 @@ hal_EfuseGetCurrentSize_WiFi(struct rtw_adapter *padapter)
 		efuse_addr += (word_cnts * 2) + 1;
 	}
 
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_EFUSE_BYTES, (u8 *) &efuse_addr);
+	pHalData->EfuseUsedBytes = efuse_addr;
 
 	DBG_8723A("%s: CurrentSize =%d\n", __func__, efuse_addr);
 
 	return efuse_addr;
 }
 
-static u16
-hal_EfuseGetCurrentSize_BT(struct rtw_adapter *padapter)
+u16 rtl8723a_EfuseGetCurrentSize_BT(struct rtw_adapter *padapter)
 {
 	u16 btusedbytes;
 	u16 efuse_addr;
@@ -888,8 +749,9 @@ hal_EfuseGetCurrentSize_BT(struct rtw_adapter *padapter)
 	u8 hoffset = 0, hworden = 0;
 	u8 efuse_data, word_cnts = 0;
 	u16 retU2 = 0;
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
-	rtw23a_hal_get_hwreg(padapter, HW_VAR_EFUSE_BT_BYTES, (u8 *) &btusedbytes);
+	btusedbytes = pHalData->BTEfuseUsedBytes;
 
 	efuse_addr = (u16) ((btusedbytes % EFUSE_BT_REAL_BANK_CONTENT_LEN));
 	startBank = (u8) (1 + (btusedbytes / EFUSE_BT_REAL_BANK_CONTENT_LEN));
@@ -915,7 +777,7 @@ hal_EfuseGetCurrentSize_BT(struct rtw_adapter *padapter)
 
 		while (AVAILABLE_EFUSE_ADDR(efuse_addr)) {
 			if (efuse_OneByteRead23a(padapter, efuse_addr,
-					      &efuse_data) == false) {
+					      &efuse_data) == _FAIL) {
 				DBG_8723A(KERN_ERR "%s: efuse_OneByteRead23a Fail!"
 					  " addr = 0x%X !!\n",
 					  __func__, efuse_addr);
@@ -954,86 +816,14 @@ hal_EfuseGetCurrentSize_BT(struct rtw_adapter *padapter)
 	}
 
 	retU2 = ((bank - 1) * EFUSE_BT_REAL_BANK_CONTENT_LEN) + efuse_addr;
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_EFUSE_BT_BYTES, (u8 *)&retU2);
+	pHalData->BTEfuseUsedBytes = retU2;
 
 	DBG_8723A("%s: CurrentSize =%d\n", __func__, retU2);
 	return retU2;
 }
 
-static u16
-Hal_EfuseGetCurrentSize(struct rtw_adapter *pAdapter, u8 efuseType)
-{
-	u16 ret = 0;
-
-	if (efuseType == EFUSE_WIFI)
-		ret = hal_EfuseGetCurrentSize_WiFi(pAdapter);
-	else
-		ret = hal_EfuseGetCurrentSize_BT(pAdapter);
-
-	return ret;
-}
-
-static u8
-Hal_EfuseWordEnableDataWrite(struct rtw_adapter *padapter,
-			     u16 efuse_addr, u8 word_en, u8 *data)
-{
-	u16 tmpaddr = 0;
-	u16 start_addr = efuse_addr;
-	u8 badworden = 0x0F;
-	u8 tmpdata[PGPKT_DATA_SIZE];
-
-	memset(tmpdata, 0xFF, PGPKT_DATA_SIZE);
-
-	if (!(word_en & BIT(0))) {
-		tmpaddr = start_addr;
-		efuse_OneByteWrite23a(padapter, start_addr++, data[0]);
-		efuse_OneByteWrite23a(padapter, start_addr++, data[1]);
-
-		efuse_OneByteRead23a(padapter, tmpaddr, &tmpdata[0]);
-		efuse_OneByteRead23a(padapter, tmpaddr + 1, &tmpdata[1]);
-		if ((data[0] != tmpdata[0]) || (data[1] != tmpdata[1])) {
-			badworden &= (~BIT(0));
-		}
-	}
-	if (!(word_en & BIT(1))) {
-		tmpaddr = start_addr;
-		efuse_OneByteWrite23a(padapter, start_addr++, data[2]);
-		efuse_OneByteWrite23a(padapter, start_addr++, data[3]);
-
-		efuse_OneByteRead23a(padapter, tmpaddr, &tmpdata[2]);
-		efuse_OneByteRead23a(padapter, tmpaddr + 1, &tmpdata[3]);
-		if ((data[2] != tmpdata[2]) || (data[3] != tmpdata[3])) {
-			badworden &= (~BIT(1));
-		}
-	}
-	if (!(word_en & BIT(2))) {
-		tmpaddr = start_addr;
-		efuse_OneByteWrite23a(padapter, start_addr++, data[4]);
-		efuse_OneByteWrite23a(padapter, start_addr++, data[5]);
-
-		efuse_OneByteRead23a(padapter, tmpaddr, &tmpdata[4]);
-		efuse_OneByteRead23a(padapter, tmpaddr + 1, &tmpdata[5]);
-		if ((data[4] != tmpdata[4]) || (data[5] != tmpdata[5])) {
-			badworden &= (~BIT(2));
-		}
-	}
-	if (!(word_en & BIT(3))) {
-		tmpaddr = start_addr;
-		efuse_OneByteWrite23a(padapter, start_addr++, data[6]);
-		efuse_OneByteWrite23a(padapter, start_addr++, data[7]);
-
-		efuse_OneByteRead23a(padapter, tmpaddr, &tmpdata[6]);
-		efuse_OneByteRead23a(padapter, tmpaddr + 1, &tmpdata[7]);
-		if ((data[6] != tmpdata[6]) || (data[7] != tmpdata[7])) {
-			badworden &= (~BIT(3));
-		}
-	}
-
-	return badworden;
-}
-
-static s32
-Hal_EfusePgPacketRead(struct rtw_adapter *padapter, u8 offset, u8 *data)
+bool
+rtl8723a_EfusePgPacketRead(struct rtw_adapter *padapter, u8 offset, u8 *data)
 {
 	u8 efuse_data, word_cnts = 0;
 	u16 efuse_addr = 0;
@@ -1064,7 +854,7 @@ Hal_EfusePgPacketRead(struct rtw_adapter *padapter, u8 offset, u8 *data)
 	/*  */
 	while (AVAILABLE_EFUSE_ADDR(efuse_addr)) {
 		if (efuse_OneByteRead23a(padapter, efuse_addr++, &efuse_data) ==
-		    false) {
+		    _FAIL) {
 			ret = false;
 			break;
 		}
@@ -1110,281 +900,7 @@ Hal_EfusePgPacketRead(struct rtw_adapter *padapter, u8 offset, u8 *data)
 	return ret;
 }
 
-static u8
-hal_EfusePgCheckAvailableAddr(struct rtw_adapter *pAdapter, u8 efuseType)
-{
-	u16 max_available = 0;
-	u16 current_size;
-
-	EFUSE_GetEfuseDefinition23a(pAdapter, efuseType,
-				 TYPE_AVAILABLE_EFUSE_BYTES_TOTAL,
-				 &max_available);
-
-	current_size = Efuse_GetCurrentSize23a(pAdapter, efuseType);
-	if (current_size >= max_available) {
-		DBG_8723A("%s: Error!! current_size(%d)>max_available(%d)\n",
-			  __func__, current_size, max_available);
-		return false;
-	}
-	return true;
-}
-
-static void
-hal_EfuseConstructPGPkt(u8 offset, u8 word_en, u8 *pData,
-			struct pg_pkt_struct *pTargetPkt)
-{
-	memset(pTargetPkt->data, 0xFF, PGPKT_DATA_SIZE);
-	pTargetPkt->offset = offset;
-	pTargetPkt->word_en = word_en;
-	efuse_WordEnableDataRead23a(word_en, pData, pTargetPkt->data);
-	pTargetPkt->word_cnts = Efuse_CalculateWordCnts23a(pTargetPkt->word_en);
-}
-
-static u8
-hal_EfusePartialWriteCheck(struct rtw_adapter *padapter, u8 efuseType,
-			   u16 *pAddr, struct pg_pkt_struct *pTargetPkt)
-{
-	u8 bRet = false;
-	u16 startAddr = 0, efuse_max_available_len = 0, efuse_max = 0;
-	u8 efuse_data = 0;
-
-	EFUSE_GetEfuseDefinition23a(padapter, efuseType,
-				 TYPE_AVAILABLE_EFUSE_BYTES_TOTAL,
-				 &efuse_max_available_len);
-	EFUSE_GetEfuseDefinition23a(padapter, efuseType,
-				 TYPE_EFUSE_CONTENT_LEN_BANK, &efuse_max);
-
-	if (efuseType == EFUSE_WIFI) {
-		rtw23a_hal_get_hwreg(padapter, HW_VAR_EFUSE_BYTES,
-				  (u8 *) &startAddr);
-	} else {
-		rtw23a_hal_get_hwreg(padapter, HW_VAR_EFUSE_BT_BYTES,
-				  (u8 *) &startAddr);
-	}
-	startAddr %= efuse_max;
-
-	while (1) {
-		if (startAddr >= efuse_max_available_len) {
-			bRet = false;
-			DBG_8723A("%s: startAddr(%d) >= efuse_max_available_"
-				  "len(%d)\n", __func__, startAddr,
-				  efuse_max_available_len);
-			break;
-		}
-
-		if (efuse_OneByteRead23a(padapter, startAddr, &efuse_data) &&
-		    (efuse_data != 0xFF)) {
-			bRet = false;
-			DBG_8723A("%s: Something Wrong! last bytes(%#X = 0x%02X) "
-				  "is not 0xFF\n", __func__,
-				  startAddr, efuse_data);
-			break;
-		} else {
-			/*  not used header, 0xff */
-			*pAddr = startAddr;
-			bRet = true;
-			break;
-		}
-	}
-
-	return bRet;
-}
-
-static u8
-hal_EfusePgPacketWrite1ByteHeader(struct rtw_adapter *pAdapter, u8 efuseType,
-				  u16 *pAddr, struct pg_pkt_struct *pTargetPkt)
-{
-	u8 pg_header = 0, tmp_header = 0;
-	u16 efuse_addr = *pAddr;
-	u8 repeatcnt = 0;
-
-	pg_header = ((pTargetPkt->offset << 4) & 0xf0) | pTargetPkt->word_en;
-
-	do {
-		efuse_OneByteWrite23a(pAdapter, efuse_addr, pg_header);
-		efuse_OneByteRead23a(pAdapter, efuse_addr, &tmp_header);
-		if (tmp_header != 0xFF)
-			break;
-		if (repeatcnt++ > EFUSE_REPEAT_THRESHOLD_) {
-			DBG_8723A("%s: Repeat over limit for pg_header!!\n",
-				  __func__);
-			return false;
-		}
-	} while (1);
-
-	if (tmp_header != pg_header) {
-		DBG_8723A(KERN_ERR "%s: PG Header Fail!!(pg = 0x%02X "
-			  "read = 0x%02X)\n", __func__,
-			  pg_header, tmp_header);
-		return false;
-	}
-
-	*pAddr = efuse_addr;
-
-	return true;
-}
-
-static u8
-hal_EfusePgPacketWrite2ByteHeader(struct rtw_adapter *padapter, u8 efuseType,
-				  u16 *pAddr, struct pg_pkt_struct *pTargetPkt)
-{
-	u16 efuse_addr, efuse_max_available_len = 0;
-	u8 pg_header = 0, tmp_header = 0;
-	u8 repeatcnt = 0;
-
-	EFUSE_GetEfuseDefinition23a(padapter, efuseType,
-				 TYPE_AVAILABLE_EFUSE_BYTES_BANK,
-				 &efuse_max_available_len);
-
-	efuse_addr = *pAddr;
-	if (efuse_addr >= efuse_max_available_len) {
-		DBG_8723A("%s: addr(%d) over avaliable(%d)!!\n", __func__,
-			  efuse_addr, efuse_max_available_len);
-		return false;
-	}
-
-	pg_header = ((pTargetPkt->offset & 0x07) << 5) | 0x0F;
-
-	do {
-		efuse_OneByteWrite23a(padapter, efuse_addr, pg_header);
-		efuse_OneByteRead23a(padapter, efuse_addr, &tmp_header);
-		if (tmp_header != 0xFF)
-			break;
-		if (repeatcnt++ > EFUSE_REPEAT_THRESHOLD_) {
-			DBG_8723A("%s: Repeat over limit for pg_header!!\n",
-				  __func__);
-			return false;
-		}
-	} while (1);
-
-	if (tmp_header != pg_header) {
-		DBG_8723A(KERN_ERR
-			  "%s: PG Header Fail!!(pg = 0x%02X read = 0x%02X)\n",
-			  __func__, pg_header, tmp_header);
-		return false;
-	}
-
-	/*  to write ext_header */
-	efuse_addr++;
-	pg_header = ((pTargetPkt->offset & 0x78) << 1) | pTargetPkt->word_en;
-
-	do {
-		efuse_OneByteWrite23a(padapter, efuse_addr, pg_header);
-		efuse_OneByteRead23a(padapter, efuse_addr, &tmp_header);
-		if (tmp_header != 0xFF)
-			break;
-		if (repeatcnt++ > EFUSE_REPEAT_THRESHOLD_) {
-			DBG_8723A("%s: Repeat over limit for ext_header!!\n",
-				  __func__);
-			return false;
-		}
-	} while (1);
-
-	if (tmp_header != pg_header) {	/* offset PG fail */
-		DBG_8723A(KERN_ERR
-			  "%s: PG EXT Header Fail!!(pg = 0x%02X read = 0x%02X)\n",
-			  __func__, pg_header, tmp_header);
-		return false;
-	}
-
-	*pAddr = efuse_addr;
-
-	return true;
-}
-
-static u8
-hal_EfusePgPacketWriteHeader(struct rtw_adapter *padapter, u8 efuseType,
-			     u16 *pAddr, struct pg_pkt_struct *pTargetPkt)
-{
-	u8 bRet = false;
-
-	if (pTargetPkt->offset >= EFUSE_MAX_SECTION_BASE) {
-		bRet = hal_EfusePgPacketWrite2ByteHeader(padapter, efuseType,
-							 pAddr, pTargetPkt);
-	} else {
-		bRet = hal_EfusePgPacketWrite1ByteHeader(padapter, efuseType,
-							 pAddr, pTargetPkt);
-	}
-
-	return bRet;
-}
-
-static u8
-hal_EfusePgPacketWriteData(struct rtw_adapter *pAdapter, u8 efuseType,
-			   u16 *pAddr, struct pg_pkt_struct *pTargetPkt)
-{
-	u16 efuse_addr;
-	u8 badworden;
-
-	efuse_addr = *pAddr;
-	badworden =
-	    Efuse_WordEnableDataWrite23a(pAdapter, efuse_addr + 1,
-				      pTargetPkt->word_en, pTargetPkt->data);
-	if (badworden != 0x0F) {
-		DBG_8723A("%s: Fail!!\n", __func__);
-		return false;
-	}
-
-	return true;
-}
-
-static s32
-Hal_EfusePgPacketWrite(struct rtw_adapter *padapter,
-		       u8 offset, u8 word_en, u8 *pData)
-{
-	struct pg_pkt_struct targetPkt;
-	u16 startAddr = 0;
-	u8 efuseType = EFUSE_WIFI;
-
-	if (!hal_EfusePgCheckAvailableAddr(padapter, efuseType))
-		return false;
-
-	hal_EfuseConstructPGPkt(offset, word_en, pData, &targetPkt);
-
-	if (!hal_EfusePartialWriteCheck(padapter, efuseType,
-					&startAddr, &targetPkt))
-		return false;
-
-	if (!hal_EfusePgPacketWriteHeader(padapter, efuseType,
-					  &startAddr, &targetPkt))
-		return false;
-
-	if (!hal_EfusePgPacketWriteData(padapter, efuseType,
-					&startAddr, &targetPkt))
-		return false;
-
-	return true;
-}
-
-static bool
-Hal_EfusePgPacketWrite_BT(struct rtw_adapter *pAdapter,
-			  u8 offset, u8 word_en, u8 *pData)
-{
-	struct pg_pkt_struct targetPkt;
-	u16 startAddr = 0;
-	u8 efuseType = EFUSE_BT;
-
-	if (!hal_EfusePgCheckAvailableAddr(pAdapter, efuseType))
-		return false;
-
-	hal_EfuseConstructPGPkt(offset, word_en, pData, &targetPkt);
-
-	if (!hal_EfusePartialWriteCheck(pAdapter, efuseType,
-					&startAddr, &targetPkt))
-		return false;
-
-	if (!hal_EfusePgPacketWriteHeader(pAdapter, efuseType,
-					  &startAddr, &targetPkt))
-		return false;
-
-	if (!hal_EfusePgPacketWriteData(pAdapter, efuseType,
-					&startAddr, &targetPkt))
-		return false;
-
-	return true;
-}
-
-static struct hal_version ReadChipVersion8723A(struct rtw_adapter *padapter)
+void rtl8723a_read_chip_version(struct rtw_adapter *padapter)
 {
 	u32 value32;
 	struct hal_version ChipVersion;
@@ -1392,7 +908,7 @@ static struct hal_version ReadChipVersion8723A(struct rtw_adapter *padapter)
 
 	pHalData = GET_HAL_DATA(padapter);
 
-	value32 = rtw_read32(padapter, REG_SYS_CFG);
+	value32 = rtl8723au_read32(padapter, REG_SYS_CFG);
 	ChipVersion.ICType = CHIP_8723A;
 	ChipVersion.ChipType = ((value32 & RTL_ID) ? TEST_CHIP : NORMAL_CHIP);
 	ChipVersion.RFType = RF_TYPE_1T1R;
@@ -1404,13 +920,13 @@ static struct hal_version ReadChipVersion8723A(struct rtw_adapter *padapter)
 	pHalData->RegulatorMode = ((value32 & SPS_SEL) ?
 				   RT_LDO_REGULATOR : RT_SWITCHING_REGULATOR);
 
-	value32 = rtw_read32(padapter, REG_GPIO_OUTSTS);
+	value32 = rtl8723au_read32(padapter, REG_GPIO_OUTSTS);
 	/*  ROM code version. */
 	ChipVersion.ROMVer = ((value32 & RF_RL_ID) >> 20);
 
 	/*  For multi-function consideration. Added by Roger, 2010.10.06. */
 	pHalData->MultiFunc = RT_MULTI_FUNC_NONE;
-	value32 = rtw_read32(padapter, REG_MULTI_FUNC_CTRL);
+	value32 = rtl8723au_read32(padapter, REG_MULTI_FUNC_CTRL);
 	pHalData->MultiFunc |=
 		((value32 & WL_FUNC_EN) ? RT_MULTI_FUNC_WIFI : 0);
 	pHalData->MultiFunc |= ((value32 & BT_FUNC_EN) ? RT_MULTI_FUNC_BT : 0);
@@ -1430,13 +946,6 @@ static struct hal_version ReadChipVersion8723A(struct rtw_adapter *padapter)
 		pHalData->rf_type = RF_1T1R;
 
 	MSG_8723A("RF_Type is %x!!\n", pHalData->rf_type);
-
-	return ChipVersion;
-}
-
-static void rtl8723a_read_chip_version(struct rtw_adapter *padapter)
-{
-	ReadChipVersion8723A(padapter);
 }
 
 /*  */
@@ -1461,33 +970,34 @@ void SetBcnCtrlReg23a(struct rtw_adapter *padapter, u8 SetBits, u8 ClearBits)
 
 	addr = REG_BCN_CTRL;
 
-	*pRegBcnCtrlVal = rtw_read8(padapter, addr);
+	*pRegBcnCtrlVal = rtl8723au_read8(padapter, addr);
 	*pRegBcnCtrlVal |= SetBits;
 	*pRegBcnCtrlVal &= ~ClearBits;
 
-	rtw_write8(padapter, addr, *pRegBcnCtrlVal);
+	rtl8723au_write8(padapter, addr, *pRegBcnCtrlVal);
 }
 
 void rtl8723a_InitBeaconParameters(struct rtw_adapter *padapter)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
-	rtw_write16(padapter, REG_BCN_CTRL, 0x1010);
+	rtl8723au_write16(padapter, REG_BCN_CTRL, 0x1010);
 	pHalData->RegBcnCtrlVal = 0x1010;
 
 	/*  TODO: Remove these magic number */
-	rtw_write16(padapter, REG_TBTT_PROHIBIT, 0x6404);	/*  ms */
+	rtl8723au_write16(padapter, REG_TBTT_PROHIBIT, 0x6404);	/*  ms */
 	/*  Firmware will control REG_DRVERLYINT when power saving is enable, */
 	/*  so don't set this register on STA mode. */
 	if (check_fwstate(&padapter->mlmepriv, WIFI_STATION_STATE) == false)
-		rtw_write8(padapter, REG_DRVERLYINT, DRIVER_EARLY_INT_TIME);
+		rtl8723au_write8(padapter, REG_DRVERLYINT,
+				 DRIVER_EARLY_INT_TIME);
 	/*  2ms */
-	rtw_write8(padapter, REG_BCNDMATIM, BCN_DMA_ATIME_INT_TIME);
+	rtl8723au_write8(padapter, REG_BCNDMATIM, BCN_DMA_ATIME_INT_TIME);
 
 	/*  Suggested by designer timchen. Change beacon AIFS to the
 	    largest number beacause test chip does not contension before
 	    sending beacon. by tynli. 2009.11.03 */
-	rtw_write16(padapter, REG_BCNTCFG, 0x660F);
+	rtl8723au_write16(padapter, REG_BCNTCFG, 0x660F);
 }
 
 static void ResumeTxBeacon(struct rtw_adapter *padapter)
@@ -1501,10 +1011,11 @@ static void ResumeTxBeacon(struct rtw_adapter *padapter)
 	RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("+ResumeTxBeacon\n"));
 
 	pHalData->RegFwHwTxQCtrl |= BIT(6);
-	rtw_write8(padapter, REG_FWHW_TXQ_CTRL + 2, pHalData->RegFwHwTxQCtrl);
-	rtw_write8(padapter, REG_TBTT_PROHIBIT + 1, 0xff);
+	rtl8723au_write8(padapter, REG_FWHW_TXQ_CTRL + 2,
+			 pHalData->RegFwHwTxQCtrl);
+	rtl8723au_write8(padapter, REG_TBTT_PROHIBIT + 1, 0xff);
 	pHalData->RegReg542 |= BIT(0);
-	rtw_write8(padapter, REG_TBTT_PROHIBIT + 2, pHalData->RegReg542);
+	rtl8723au_write8(padapter, REG_TBTT_PROHIBIT + 2, pHalData->RegReg542);
 }
 
 static void StopTxBeacon(struct rtw_adapter *padapter)
@@ -1518,10 +1029,11 @@ static void StopTxBeacon(struct rtw_adapter *padapter)
 	RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("+StopTxBeacon\n"));
 
 	pHalData->RegFwHwTxQCtrl &= ~BIT(6);
-	rtw_write8(padapter, REG_FWHW_TXQ_CTRL + 2, pHalData->RegFwHwTxQCtrl);
-	rtw_write8(padapter, REG_TBTT_PROHIBIT + 1, 0x64);
+	rtl8723au_write8(padapter, REG_FWHW_TXQ_CTRL + 2,
+			 pHalData->RegFwHwTxQCtrl);
+	rtl8723au_write8(padapter, REG_TBTT_PROHIBIT + 1, 0x64);
 	pHalData->RegReg542 &= ~BIT(0);
-	rtw_write8(padapter, REG_TBTT_PROHIBIT + 2, pHalData->RegReg542);
+	rtl8723au_write8(padapter, REG_TBTT_PROHIBIT + 2, pHalData->RegReg542);
 
 	CheckFwRsvdPageContent23a(padapter); /*  2010.06.23. Added by tynli. */
 }
@@ -1531,10 +1043,10 @@ static void _BeaconFunctionEnable(struct rtw_adapter *padapter, u8 Enable,
 {
 	SetBcnCtrlReg23a(padapter, DIS_TSF_UDT | EN_BCN_FUNCTION | DIS_BCNQ_SUB,
 		      0);
-	rtw_write8(padapter, REG_RD_CTRL + 1, 0x6F);
+	rtl8723au_write8(padapter, REG_RD_CTRL + 1, 0x6F);
 }
 
-static void rtl8723a_SetBeaconRelatedRegisters(struct rtw_adapter *padapter)
+void rtl8723a_SetBeaconRelatedRegisters(struct rtw_adapter *padapter)
 {
 	u32 value32;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
@@ -1555,32 +1067,32 @@ static void rtl8723a_SetBeaconRelatedRegisters(struct rtw_adapter *padapter)
 	/*  */
 	/*  ATIM window */
 	/*  */
-	rtw_write16(padapter, REG_ATIMWND, 2);
+	rtl8723au_write16(padapter, REG_ATIMWND, 2);
 
 	/*  */
 	/*  Beacon interval (in unit of TU). */
 	/*  */
-	rtw_write16(padapter, REG_BCN_INTERVAL, pmlmeinfo->bcn_interval);
+	rtl8723au_write16(padapter, REG_BCN_INTERVAL, pmlmeinfo->bcn_interval);
 
 	rtl8723a_InitBeaconParameters(padapter);
 
-	rtw_write8(padapter, REG_SLOT, 0x09);
+	rtl8723au_write8(padapter, REG_SLOT, 0x09);
 
 	/*  */
 	/*  Reset TSF Timer to zero, added by Roger. 2008.06.24 */
 	/*  */
-	value32 = rtw_read32(padapter, REG_TCR);
+	value32 = rtl8723au_read32(padapter, REG_TCR);
 	value32 &= ~TSFRST;
-	rtw_write32(padapter, REG_TCR, value32);
+	rtl8723au_write32(padapter, REG_TCR, value32);
 
 	value32 |= TSFRST;
-	rtw_write32(padapter, REG_TCR, value32);
+	rtl8723au_write32(padapter, REG_TCR, value32);
 
 	/*  NOTE: Fix test chip's bug (about contention windows's randomness) */
 	if (check_fwstate(&padapter->mlmepriv, WIFI_ADHOC_STATE |
 			  WIFI_ADHOC_MASTER_STATE | WIFI_AP_STATE) == true) {
-		rtw_write8(padapter, REG_RXTSF_OFFSET_CCK, 0x50);
-		rtw_write8(padapter, REG_RXTSF_OFFSET_OFDM, 0x50);
+		rtl8723au_write8(padapter, REG_RXTSF_OFFSET_CCK, 0x50);
+		rtl8723au_write8(padapter, REG_RXTSF_OFFSET_OFDM, 0x50);
 	}
 
 	_BeaconFunctionEnable(padapter, true, true);
@@ -1589,21 +1101,9 @@ static void rtl8723a_SetBeaconRelatedRegisters(struct rtw_adapter *padapter)
 	SetBcnCtrlReg23a(padapter, DIS_BCNQ_SUB, 0);
 }
 
-static void rtl8723a_GetHalODMVar(struct rtw_adapter *Adapter,
-				  enum hal_odm_variable eVariable,
-				  void *pValue1, bool bSet)
-{
-	switch (eVariable) {
-	case HAL_ODM_STA_INFO:
-		break;
-	default:
-		break;
-	}
-}
-
-static void rtl8723a_SetHalODMVar(struct rtw_adapter *Adapter,
-				  enum hal_odm_variable eVariable,
-				  void *pValue1, bool bSet)
+void rtl8723a_SetHalODMVar(struct rtw_adapter *Adapter,
+			   enum hal_odm_variable eVariable,
+			   void *pValue1, bool bSet)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
 	struct dm_odm_t *podmpriv = &pHalData->odmpriv;
@@ -1636,32 +1136,33 @@ static void rtl8723a_SetHalODMVar(struct rtw_adapter *Adapter,
 	}
 }
 
-static void hal_notch_filter_8723a(struct rtw_adapter *adapter, bool enable)
+void rtl8723a_notch_filter(struct rtw_adapter *adapter, bool enable)
 {
 	if (enable) {
 		DBG_8723A("Enable notch filter\n");
-		rtw_write8(adapter, rOFDM0_RxDSP + 1,
-			   rtw_read8(adapter, rOFDM0_RxDSP + 1) | BIT1);
+		rtl8723au_write8(adapter, rOFDM0_RxDSP + 1,
+				 rtl8723au_read8(adapter, rOFDM0_RxDSP + 1) |
+				 BIT(1));
 	} else {
 		DBG_8723A("Disable notch filter\n");
-		rtw_write8(adapter, rOFDM0_RxDSP + 1,
-			   rtw_read8(adapter, rOFDM0_RxDSP + 1) & ~BIT1);
+		rtl8723au_write8(adapter, rOFDM0_RxDSP + 1,
+			   rtl8723au_read8(adapter, rOFDM0_RxDSP + 1) &
+				 ~BIT(1));
 	}
 }
 
-s32 c2h_id_filter_ccx_8723a(u8 id)
+bool c2h_id_filter_ccx_8723a(u8 id)
 {
-	s32 ret = false;
+	bool ret = false;
 	if (id == C2H_CCX_TX_RPT)
 		ret = true;
 
 	return ret;
 }
 
-static s32 c2h_handler_8723a(struct rtw_adapter *padapter,
-			     struct c2h_evt_hdr *c2h_evt)
+int c2h_handler_8723a(struct rtw_adapter *padapter, struct c2h_evt_hdr *c2h_evt)
 {
-	s32 ret = _SUCCESS;
+	int ret = _SUCCESS;
 	u8 i = 0;
 
 	if (c2h_evt == NULL) {
@@ -1701,12 +1202,11 @@ static s32 c2h_handler_8723a(struct rtw_adapter *padapter,
 			  c2h_evt->payload[3], c2h_evt->payload[4]));
 		break;
 
-#ifdef CONFIG_8723AU_BT_COEXIST
 	case C2H_BT_INFO:
 		DBG_8723A("%s ,  Got  C2H_BT_INFO \n", __func__);
-		BT_FwC2hBtInfo(padapter, c2h_evt->payload, c2h_evt->plen);
+		rtl8723a_fw_c2h_BT_info(padapter,
+					c2h_evt->payload, c2h_evt->plen);
 		break;
-#endif
 
 	default:
 		ret = _FAIL;
@@ -1717,97 +1217,36 @@ exit:
 	return ret;
 }
 
-void rtl8723a_set_hal_ops(struct hal_ops *pHalFunc)
-{
-	pHalFunc->free_hal_data = &rtl8723a_free_hal_data;
-
-	pHalFunc->dm_init = &rtl8723a_init_dm_priv;
-	pHalFunc->dm_deinit = &rtl8723a_deinit_dm_priv;
-
-	pHalFunc->read_chip_version = &rtl8723a_read_chip_version;
-
-	pHalFunc->set_bwmode_handler = &PHY_SetBWMode23a8723A;
-	pHalFunc->set_channel_handler = &PHY_SwChnl8723A;
-
-	pHalFunc->hal_dm_watchdog = &rtl8723a_HalDmWatchDog;
-
-	pHalFunc->SetBeaconRelatedRegistersHandler =
-		&rtl8723a_SetBeaconRelatedRegisters;
-
-	pHalFunc->Add_RateATid = &rtl8723a_add_rateatid;
-	pHalFunc->run_thread = &rtl8723a_start_thread;
-	pHalFunc->cancel_thread = &rtl8723a_stop_thread;
-
-	pHalFunc->read_bbreg = &PHY_QueryBBReg;
-	pHalFunc->write_bbreg = &PHY_SetBBReg;
-	pHalFunc->read_rfreg = &PHY_QueryRFReg;
-	pHalFunc->write_rfreg = &PHY_SetRFReg;
-
-	/*  Efuse related function */
-	pHalFunc->EfusePowerSwitch = &Hal_EfusePowerSwitch;
-	pHalFunc->ReadEFuse = &Hal_ReadEFuse;
-	pHalFunc->EFUSEGetEfuseDefinition = &Hal_GetEfuseDefinition;
-	pHalFunc->EfuseGetCurrentSize = &Hal_EfuseGetCurrentSize;
-	pHalFunc->Efuse_PgPacketRead23a = &Hal_EfusePgPacketRead;
-	pHalFunc->Efuse_PgPacketWrite23a = &Hal_EfusePgPacketWrite;
-	pHalFunc->Efuse_WordEnableDataWrite23a = &Hal_EfuseWordEnableDataWrite;
-	pHalFunc->Efuse_PgPacketWrite23a_BT = &Hal_EfusePgPacketWrite_BT;
-
-	pHalFunc->sreset_init_value23a = &sreset_init_value23a;
-	pHalFunc->sreset_reset_value23a = &sreset_reset_value23a;
-	pHalFunc->silentreset = &sreset_reset;
-	pHalFunc->sreset_xmit_status_check = &rtl8723a_sreset_xmit_status_check;
-	pHalFunc->sreset_linked_status_check =
-		&rtl8723a_sreset_linked_status_check;
-	pHalFunc->sreset_get_wifi_status23a = &sreset_get_wifi_status23a;
-	pHalFunc->sreset_inprogress = &sreset_inprogress;
-	pHalFunc->GetHalODMVarHandler = &rtl8723a_GetHalODMVar;
-	pHalFunc->SetHalODMVarHandler = &rtl8723a_SetHalODMVar;
-
-	pHalFunc->hal_notch_filter = &hal_notch_filter_8723a;
-
-	pHalFunc->c2h_handler = c2h_handler_8723a;
-	pHalFunc->c2h_id_filter_ccx = c2h_id_filter_ccx_8723a;
-}
-
 void rtl8723a_InitAntenna_Selection(struct rtw_adapter *padapter)
 {
-	struct hal_data_8723a *pHalData;
 	u8 val;
 
-	pHalData = GET_HAL_DATA(padapter);
-
-	val = rtw_read8(padapter, REG_LEDCFG2);
+	val = rtl8723au_read8(padapter, REG_LEDCFG2);
 	/*  Let 8051 take control antenna settting */
 	val |= BIT(7);		/*  DPDT_SEL_EN, 0x4C[23] */
-	rtw_write8(padapter, REG_LEDCFG2, val);
+	rtl8723au_write8(padapter, REG_LEDCFG2, val);
 }
 
 void rtl8723a_CheckAntenna_Selection(struct rtw_adapter *padapter)
 {
-	struct hal_data_8723a *pHalData;
 	u8 val;
 
-	pHalData = GET_HAL_DATA(padapter);
-
-	val = rtw_read8(padapter, REG_LEDCFG2);
+	val = rtl8723au_read8(padapter, REG_LEDCFG2);
 	/*  Let 8051 take control antenna settting */
 	if (!(val & BIT(7))) {
 		val |= BIT(7);	/*  DPDT_SEL_EN, 0x4C[23] */
-		rtw_write8(padapter, REG_LEDCFG2, val);
+		rtl8723au_write8(padapter, REG_LEDCFG2, val);
 	}
 }
 
 void rtl8723a_DeinitAntenna_Selection(struct rtw_adapter *padapter)
 {
-	struct hal_data_8723a *pHalData;
 	u8 val;
 
-	pHalData = GET_HAL_DATA(padapter);
-	val = rtw_read8(padapter, REG_LEDCFG2);
+	val = rtl8723au_read8(padapter, REG_LEDCFG2);
 	/*  Let 8051 take control antenna settting */
 	val &= ~BIT(7);		/*  DPDT_SEL_EN, clear 0x4C[23] */
-	rtw_write8(padapter, REG_LEDCFG2, val);
+	rtl8723au_write8(padapter, REG_LEDCFG2, val);
 }
 
 void rtl8723a_init_default_value(struct rtw_adapter *padapter)
@@ -1847,7 +1286,7 @@ u8 GetEEPROMSize8723A(struct rtw_adapter *padapter)
 	u8 size = 0;
 	u32 cr;
 
-	cr = rtw_read16(padapter, REG_9346CR);
+	cr = rtl8723au_read16(padapter, REG_9346CR);
 	/*  6: EEPROM used is 93C46, 4: boot from E-Fuse. */
 	size = (cr & BOOT_FROM_EEPROM) ? 6 : 4;
 
@@ -1861,19 +1300,19 @@ u8 GetEEPROMSize8723A(struct rtw_adapter *padapter)
 /*  LLT R/W/Init function */
 /*  */
 /*  */
-static s32 _LLTWrite(struct rtw_adapter *padapter, u32 address, u32 data)
+static int _LLTWrite(struct rtw_adapter *padapter, u32 address, u32 data)
 {
-	s32 status = _SUCCESS;
+	int status = _SUCCESS;
 	s32 count = 0;
 	u32 value = _LLT_INIT_ADDR(address) | _LLT_INIT_DATA(data) |
 		    _LLT_OP(_LLT_WRITE_ACCESS);
 	u16 LLTReg = REG_LLT_INIT;
 
-	rtw_write32(padapter, LLTReg, value);
+	rtl8723au_write32(padapter, LLTReg, value);
 
 	/* polling */
 	do {
-		value = rtw_read32(padapter, LLTReg);
+		value = rtl8723au_read32(padapter, LLTReg);
 		if (_LLT_NO_ACTIVE == _LLT_OP_VALUE(value)) {
 			break;
 		}
@@ -1890,23 +1329,23 @@ static s32 _LLTWrite(struct rtw_adapter *padapter, u32 address, u32 data)
 	return status;
 }
 
-s32 InitLLTTable23a(struct rtw_adapter *padapter, u32 boundary)
+int InitLLTTable23a(struct rtw_adapter *padapter, u32 boundary)
 {
-	s32 status = _SUCCESS;
+	int status = _SUCCESS;
 	u32 i;
 	u32 txpktbuf_bndy = boundary;
 	u32 Last_Entry_Of_TxPktBuf = LAST_ENTRY_OF_TX_PKT_BUFFER;
 
 	for (i = 0; i < (txpktbuf_bndy - 1); i++) {
 		status = _LLTWrite(padapter, i, i + 1);
-		if (_SUCCESS != status) {
+		if (status != _SUCCESS) {
 			return status;
 		}
 	}
 
 	/*  end of list */
 	status = _LLTWrite(padapter, (txpktbuf_bndy - 1), 0xFF);
-	if (_SUCCESS != status) {
+	if (status != _SUCCESS) {
 		return status;
 	}
 
@@ -1923,7 +1362,7 @@ s32 InitLLTTable23a(struct rtw_adapter *padapter, u32 boundary)
 
 	/*  Let last entry point to the start entry of ring buffer */
 	status = _LLTWrite(padapter, Last_Entry_Of_TxPktBuf, txpktbuf_bndy);
-	if (_SUCCESS != status) {
+	if (status != _SUCCESS) {
 		return status;
 	}
 
@@ -1943,11 +1382,11 @@ n. LEDCFG 0x4C[15:0] = 0x8080
 	u32 u4bTmp;
 
 	/* 1. Disable GPIO[7:0] */
-	rtw_write16(padapter, REG_GPIO_PIN_CTRL + 2, 0x0000);
-	value32 = rtw_read32(padapter, REG_GPIO_PIN_CTRL) & 0xFFFF00FF;
+	rtl8723au_write16(padapter, REG_GPIO_PIN_CTRL + 2, 0x0000);
+	value32 = rtl8723au_read32(padapter, REG_GPIO_PIN_CTRL) & 0xFFFF00FF;
 	u4bTmp = value32 & 0x000000FF;
 	value32 |= ((u4bTmp << 8) | 0x00FF0000);
-	rtw_write32(padapter, REG_GPIO_PIN_CTRL, value32);
+	rtl8723au_write32(padapter, REG_GPIO_PIN_CTRL, value32);
 
 	/*  */
 	/*  <Roger_Notes> For RTL8723u multi-function configuration which
@@ -1958,15 +1397,15 @@ n. LEDCFG 0x4C[15:0] = 0x8080
 	/* 2. Disable GPIO[8] and GPIO[12] */
 
 	/*  Configure all pins as input mode. */
-	rtw_write16(padapter, REG_GPIO_IO_SEL_2, 0x0000);
-	value32 = rtw_read32(padapter, REG_GPIO_PIN_CTRL_2) & 0xFFFF001F;
+	rtl8723au_write16(padapter, REG_GPIO_IO_SEL_2, 0x0000);
+	value32 = rtl8723au_read32(padapter, REG_GPIO_PIN_CTRL_2) & 0xFFFF001F;
 	u4bTmp = value32 & 0x0000001F;
 	/*  Set pin 8, 10, 11 and pin 12 to output mode. */
 	value32 |= ((u4bTmp << 8) | 0x001D0000);
-	rtw_write32(padapter, REG_GPIO_PIN_CTRL_2, value32);
+	rtl8723au_write32(padapter, REG_GPIO_PIN_CTRL_2, value32);
 
 	/* 3. Disable LED0 & 1 */
-	rtw_write16(padapter, REG_LEDCFG0, 0x8080);
+	rtl8723au_write16(padapter, REG_LEDCFG0, 0x8080);
 }				/* end of _DisableGPIO() */
 
 static void _DisableRFAFEAndResetBB8192C(struct rtw_adapter *padapter)
@@ -1980,21 +1419,21 @@ e.	SYS_FUNC_EN 0x02[7:0] = 0x14		reset BB state machine
 ***************************************/
 	u8 eRFPath = 0, value8 = 0;
 
-	rtw_write8(padapter, REG_TXPAUSE, 0xFF);
+	rtl8723au_write8(padapter, REG_TXPAUSE, 0xFF);
 
 	PHY_SetRFReg(padapter, (enum RF_RADIO_PATH) eRFPath, 0x0, bMaskByte0, 0x0);
 
 	value8 |= APSDOFF;
-	rtw_write8(padapter, REG_APSD_CTRL, value8);	/* 0x40 */
+	rtl8723au_write8(padapter, REG_APSD_CTRL, value8);	/* 0x40 */
 
 	/*  Set BB reset at first */
 	value8 = 0;
 	value8 |= (FEN_USBD | FEN_USBA | FEN_BB_GLB_RSTn);
-	rtw_write8(padapter, REG_SYS_FUNC_EN, value8);	/* 0x16 */
+	rtl8723au_write8(padapter, REG_SYS_FUNC_EN, value8);	/* 0x16 */
 
 	/*  Set global reset. */
 	value8 &= ~FEN_BB_GLB_RSTn;
-	rtw_write8(padapter, REG_SYS_FUNC_EN, value8);	/* 0x14 */
+	rtl8723au_write8(padapter, REG_SYS_FUNC_EN, value8);	/* 0x14 */
 
 	/*  2010/08/12 MH We need to set BB/GLBAL reset to save power
 	    for SS mode. */
@@ -2021,19 +1460,22 @@ static void _ResetDigitalProcedure1_92C(struct rtw_adapter *padapter,
 						(8051 enable)
 	******************************/
 		u16 valu16 = 0;
-		rtw_write8(padapter, REG_MCUFWDL, 0);
+		rtl8723au_write8(padapter, REG_MCUFWDL, 0);
 
-		valu16 = rtw_read16(padapter, REG_SYS_FUNC_EN);
+		valu16 = rtl8723au_read16(padapter, REG_SYS_FUNC_EN);
 		/* reset MCU , 8051 */
-		rtw_write16(padapter, REG_SYS_FUNC_EN, (valu16 & (~FEN_CPUEN)));
+		rtl8723au_write16(padapter, REG_SYS_FUNC_EN,
+				  valu16 & (~FEN_CPUEN));
 
-		valu16 = rtw_read16(padapter, REG_SYS_FUNC_EN) & 0x0FFF;
-		rtw_write16(padapter, REG_SYS_FUNC_EN,
-			    (valu16 | (FEN_HWPDN | FEN_ELDR)));	/* reset MAC */
+		valu16 = rtl8723au_read16(padapter, REG_SYS_FUNC_EN) & 0x0FFF;
+		/* reset MAC */
+		rtl8723au_write16(padapter, REG_SYS_FUNC_EN,
+				  valu16 | (FEN_HWPDN | FEN_ELDR));
 
-		valu16 = rtw_read16(padapter, REG_SYS_FUNC_EN);
+		valu16 = rtl8723au_read16(padapter, REG_SYS_FUNC_EN);
 		/* enable MCU , 8051 */
-		rtw_write16(padapter, REG_SYS_FUNC_EN, (valu16 | FEN_CPUEN));
+		rtl8723au_write16(padapter, REG_SYS_FUNC_EN,
+				  valu16 | FEN_CPUEN);
 	} else {
 		u8 retry_cnts = 0;
 
@@ -2041,41 +1483,43 @@ static void _ResetDigitalProcedure1_92C(struct rtw_adapter *padapter,
 		    are trying to enter IPS/HW&SW radio off. For
 		    S3/S4/S5/Disable, we can stop 8051 because */
 		/*  we will init FW when power on again. */
-		/* if (!pDevice->RegUsbSS) */
 		/*  If we want to SS mode, we can not reset 8051. */
-		if (rtw_read8(padapter, REG_MCUFWDL) & BIT1) {
+		if (rtl8723au_read8(padapter, REG_MCUFWDL) & BIT(1)) {
 			/* IF fw in RAM code, do reset */
 			if (padapter->bFWReady) {
 				/*  2010/08/25 MH Accordign to RD alfred's
 				    suggestion, we need to disable other */
 				/*  HRCV INT to influence 8051 reset. */
-				rtw_write8(padapter, REG_FWIMR, 0x20);
+				rtl8723au_write8(padapter, REG_FWIMR, 0x20);
 				/*  2011/02/15 MH According to Alex's
 				    suggestion, close mask to prevent
 				    incorrect FW write operation. */
-				rtw_write8(padapter, REG_FTIMR, 0x00);
-				rtw_write8(padapter, REG_FSIMR, 0x00);
+				rtl8723au_write8(padapter, REG_FTIMR, 0x00);
+				rtl8723au_write8(padapter, REG_FSIMR, 0x00);
 
 				/* 8051 reset by self */
-				rtw_write8(padapter, REG_HMETFR + 3, 0x20);
+				rtl8723au_write8(padapter, REG_HMETFR + 3,
+						 0x20);
 
 				while ((retry_cnts++ < 100) &&
 				       (FEN_CPUEN &
-					rtw_read16(padapter, REG_SYS_FUNC_EN))) {
+					rtl8723au_read16(padapter,
+							 REG_SYS_FUNC_EN))) {
 					udelay(50);	/* us */
 				}
 
 				if (retry_cnts >= 100) {
 					/* Reset MAC and Enable 8051 */
-					rtw_write8(padapter,
-						   REG_SYS_FUNC_EN + 1, 0x50);
+					rtl8723au_write8(padapter,
+							 REG_SYS_FUNC_EN + 1,
+							 0x50);
 					mdelay(10);
 				}
 			}
 		}
 		/* Reset MAC and Enable 8051 */
-		rtw_write8(padapter, REG_SYS_FUNC_EN + 1, 0x54);
-		rtw_write8(padapter, REG_MCUFWDL, 0);
+		rtl8723au_write8(padapter, REG_SYS_FUNC_EN + 1, 0x54);
+		rtl8723au_write8(padapter, REG_MCUFWDL, 0);
 	}
 
 	if (bWithoutHWSM) {
@@ -2087,13 +1531,13 @@ static void _ResetDigitalProcedure1_92C(struct rtw_adapter *padapter,
 	j.	SYS_ISO_CTRL 0x00[7:0] = 0xF9		isolated digital to PON
 	******************************/
 		/* modify to 0x70A3 by Scott. */
-		rtw_write16(padapter, REG_SYS_CLKR, 0x70A3);
-		rtw_write8(padapter, REG_AFE_PLL_CTRL, 0x80);
-		rtw_write16(padapter, REG_AFE_XTAL_CTRL, 0x880F);
-		rtw_write8(padapter, REG_SYS_ISO_CTRL, 0xF9);
+		rtl8723au_write16(padapter, REG_SYS_CLKR, 0x70A3);
+		rtl8723au_write8(padapter, REG_AFE_PLL_CTRL, 0x80);
+		rtl8723au_write16(padapter, REG_AFE_XTAL_CTRL, 0x880F);
+		rtl8723au_write8(padapter, REG_SYS_ISO_CTRL, 0xF9);
 	} else {
 		/*  Disable all RF/BB power */
-		rtw_write8(padapter, REG_RF_CTRL, 0x00);
+		rtl8723au_write8(padapter, REG_RF_CTRL, 0x00);
 	}
 }
 
@@ -2111,9 +1555,9 @@ l.	SYS_CLKR 0x08[15:0] = 0x3083		disable ELDR clock
 m.	SYS_ISO_CTRL 0x01[7:0] = 0x83		isolated ELDR to PON
 ******************************/
 	/* modify to 0x70a3 by Scott. */
-	rtw_write16(padapter, REG_SYS_CLKR, 0x70a3);
+	rtl8723au_write16(padapter, REG_SYS_CLKR, 0x70a3);
 	/* modify to 0x82 by Scott. */
-	rtw_write8(padapter, REG_SYS_ISO_CTRL + 1, 0x82);
+	rtl8723au_write8(padapter, REG_SYS_ISO_CTRL + 1, 0x82);
 }
 
 static void _DisableAnalog(struct rtw_adapter *padapter, bool bWithoutHWSM)
@@ -2130,12 +1574,12 @@ static void _DisableAnalog(struct rtw_adapter *padapter, bool bWithoutHWSM)
 		clock automatically
 	******************************/
 
-		rtw_write8(padapter, REG_LDOA15_CTRL, 0x04);
-		/* rtw_write8(padapter, REG_LDOV12D_CTRL, 0x54); */
+		rtl8723au_write8(padapter, REG_LDOA15_CTRL, 0x04);
+		/* rtl8723au_write8(padapter, REG_LDOV12D_CTRL, 0x54); */
 
-		value8 = rtw_read8(padapter, REG_LDOV12D_CTRL);
+		value8 = rtl8723au_read8(padapter, REG_LDOV12D_CTRL);
 		value8 &= (~LDV12_EN);
-		rtw_write8(padapter, REG_LDOV12D_CTRL, value8);
+		rtl8723au_write8(padapter, REG_LDOV12D_CTRL, value8);
 /*		RT_TRACE(COMP_INIT, DBG_LOUD,
 		(" REG_LDOV12D_CTRL Reg0x21:0x%02x.\n", value8)); */
 	}
@@ -2146,9 +1590,9 @@ static void _DisableAnalog(struct rtw_adapter *padapter, bool bWithoutHWSM)
 	******************************/
 	value8 = 0x23;
 	if (IS_81xxC_VENDOR_UMC_B_CUT(pHalData->VersionID))
-		value8 |= BIT3;
+		value8 |= BIT(3);
 
-	rtw_write8(padapter, REG_SPS0_CTRL, value8);
+	rtl8723au_write8(padapter, REG_SPS0_CTRL, value8);
 
 	if (bWithoutHWSM) {
 		/* value16 |= (APDM_HOST | FSM_HSUS |/PFM_ALDN); */
@@ -2161,13 +1605,13 @@ static void _DisableAnalog(struct rtw_adapter *padapter, bool bWithoutHWSM)
 		value16 |= (APDM_HOST | AFSM_HSUS | PFM_ALDN);
 	}
 
-	rtw_write16(padapter, REG_APS_FSMCO, value16);	/* 0x4802 */
+	rtl8723au_write16(padapter, REG_APS_FSMCO, value16);	/* 0x4802 */
 
-	rtw_write8(padapter, REG_RSV_CTRL, 0x0e);
+	rtl8723au_write8(padapter, REG_RSV_CTRL, 0x0e);
 }
 
 /*  HW Auto state machine */
-s32 CardDisableHWSM(struct rtw_adapter *padapter, u8 resetMCU)
+int CardDisableHWSM(struct rtw_adapter *padapter, u8 resetMCU)
 {
 	int rtStatus = _SUCCESS;
 
@@ -2193,9 +1637,9 @@ s32 CardDisableHWSM(struct rtw_adapter *padapter, u8 resetMCU)
 }
 
 /*  without HW Auto state machine */
-s32 CardDisableWithoutHWSM(struct rtw_adapter *padapter)
+int CardDisableWithoutHWSM(struct rtw_adapter *padapter)
 {
-	s32 rtStatus = _SUCCESS;
+	int rtStatus = _SUCCESS;
 
 	/* RT_TRACE(COMP_INIT, DBG_LOUD,
 	   ("======> Card Disable Without HWSM .\n")); */
@@ -2274,9 +1718,9 @@ static void Hal_EEValueCheck(u8 EEType, void *pInValue, void *pOutValue)
 		u8 *pIn, *pOut;
 		pIn = (u8 *) pInValue;
 		pOut = (u8 *) pOutValue;
-		if (*pIn >= 0 && *pIn <= 63) {
+		if (*pIn <= 63)
 			*pOut = *pIn;
-		} else {
+		else {
 			RT_TRACE(_module_hci_hal_init_c_, _drv_err_,
 				 ("EETYPE_TX_PWR, value =%d is invalid, set "
 				  "to default = 0x%x\n",
@@ -2346,7 +1790,7 @@ Hal_ReadPowerValueFromPROM_8723A(struct txpowerinfo *pwrInfo,
 				 [EEPROM_HT20_TX_PWR_INX_DIFF_8723A +
 				  group] >> (rfPath * 4)) & 0xF;
 			/* 4bit sign number to 8 bit sign number */
-			if (pwrInfo->HT20IndexDiff[rfPath][group] & BIT3)
+			if (pwrInfo->HT20IndexDiff[rfPath][group] & BIT(3))
 				pwrInfo->HT20IndexDiff[rfPath][group] |= 0xF0;
 
 			pwrInfo->OFDMIndexDiff[rfPath][group] =
@@ -2478,7 +1922,7 @@ Hal_EfuseParseBTCoexistInfo_8723A(struct rtw_adapter *padapter,
 	u32 tmpu4;
 
 	if (!AutoLoadFail) {
-		tmpu4 = rtw_read32(padapter, REG_MULTI_FUNC_CTRL);
+		tmpu4 = rtl8723au_read32(padapter, REG_MULTI_FUNC_CTRL);
 		if (tmpu4 & BT_FUNC_EN)
 			pHalData->EEPROMBluetoothCoexist = 1;
 		else
@@ -2498,9 +1942,8 @@ Hal_EfuseParseBTCoexistInfo_8723A(struct rtw_adapter *padapter,
 		pHalData->EEPROMBluetoothAntIsolation = 0;
 		pHalData->EEPROMBluetoothRadioShared = BT_Radio_Shared;
 	}
-#ifdef CONFIG_8723AU_BT_COEXIST
-	BT_InitHalVars(padapter);
-#endif
+
+	rtl8723a_BT_init_hal_vars(padapter);
 }
 
 void
@@ -2586,20 +2029,20 @@ Hal_EfuseParseXtal_8723A(struct rtw_adapter *pAdapter,
 
 void
 Hal_EfuseParseThermalMeter_8723A(struct rtw_adapter *padapter,
-				 u8 *PROMContent, u8 AutoloadFail)
+				 u8 *PROMContent, bool AutoloadFail)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 
 	/*  */
 	/*  ThermalMeter from EEPROM */
 	/*  */
-	if (false == AutoloadFail)
+	if (AutoloadFail == false)
 		pHalData->EEPROMThermalMeter =
 		    PROMContent[EEPROM_THERMAL_METER_8723A];
 	else
 		pHalData->EEPROMThermalMeter = EEPROM_Default_ThermalMeter;
 
-	if ((pHalData->EEPROMThermalMeter == 0xff) || (true == AutoloadFail)) {
+	if ((pHalData->EEPROMThermalMeter == 0xff) || (AutoloadFail == true)) {
 		pHalData->bAPKThermalMeterIgnore = true;
 		pHalData->EEPROMThermalMeter = EEPROM_Default_ThermalMeter;
 	}
@@ -2635,18 +2078,17 @@ static void fill_txdesc_sectype(struct pkt_attrib *pattrib,
 	if ((pattrib->encrypt > 0) && !pattrib->bswenc) {
 		switch (pattrib->encrypt) {
 			/*  SEC_TYPE */
-		case _WEP40_:
-		case _WEP104_:
-		case _TKIP_:
-		case _TKIP_WTMIC_:
+		case WLAN_CIPHER_SUITE_WEP40:
+		case WLAN_CIPHER_SUITE_WEP104:
+		case WLAN_CIPHER_SUITE_TKIP:
 			ptxdesc->sectype = 1;
 			break;
 
-		case _AES_:
+		case WLAN_CIPHER_SUITE_CCMP:
 			ptxdesc->sectype = 3;
 			break;
 
-		case _NO_PRIVACY_:
+		case 0:
 		default:
 			break;
 		}
@@ -2935,7 +2377,7 @@ void rtl8723a_fill_fake_txdesc(struct rtw_adapter *padapter, u8 *pDesc,
 	rtl8723a_cal_txdesc_chksum(ptxdesc);
 }
 
-static void hw_var_set_opmode(struct rtw_adapter *padapter, u8 mode)
+void hw_var_set_opmode(struct rtw_adapter *padapter, u8 mode)
 {
 	u8 val8;
 
@@ -2951,10 +2393,8 @@ static void hw_var_set_opmode(struct rtw_adapter *padapter, u8 mode)
 		val8 = DIS_TSF_UDT | EN_BCN_FUNCTION | DIS_BCNQ_SUB;
 		SetBcnCtrlReg23a(padapter, val8, ~val8);
 	} else if (mode == _HW_STATE_AP_) {
-#ifdef CONFIG_8723AU_BT_COEXIST
 		/*  add NULL Data and BT NULL Data Packets to FW RSVD Page */
 		rtl8723a_set_BTCoex_AP_mode_FwRsvdPkt_cmd(padapter);
-#endif
 
 		ResumeTxBeacon(padapter);
 
@@ -2962,26 +2402,29 @@ static void hw_var_set_opmode(struct rtw_adapter *padapter, u8 mode)
 		SetBcnCtrlReg23a(padapter, val8, ~val8);
 
 		/*  Set RCR */
-		/* rtw_write32(padapter, REG_RCR, 0x70002a8e);
+		/* rtl8723au_write32(padapter, REG_RCR, 0x70002a8e);
 		   CBSSID_DATA must set to 0 */
 		/* CBSSID_DATA must set to 0 */
-		rtw_write32(padapter, REG_RCR, 0x7000228e);
+		rtl8723au_write32(padapter, REG_RCR, 0x7000228e);
 		/*  enable to rx data frame */
-		rtw_write16(padapter, REG_RXFLTMAP2, 0xFFFF);
+		rtl8723au_write16(padapter, REG_RXFLTMAP2, 0xFFFF);
 		/*  enable to rx ps-poll */
-		rtw_write16(padapter, REG_RXFLTMAP1, 0x0400);
+		rtl8723au_write16(padapter, REG_RXFLTMAP1, 0x0400);
 
 		/*  Beacon Control related register for first time */
-		rtw_write8(padapter, REG_BCNDMATIM, 0x02);	/*  2ms */
-		rtw_write8(padapter, REG_DRVERLYINT, 0x05);	/*  5ms */
-		rtw_write8(padapter, REG_ATIMWND, 0x0a); /*  10ms for port0 */
-		rtw_write16(padapter, REG_BCNTCFG, 0x00);
-		rtw_write16(padapter, REG_TBTT_PROHIBIT, 0xff04);
+		/*  2ms */
+		rtl8723au_write8(padapter, REG_BCNDMATIM, 0x02);
+		/*  5ms */
+		rtl8723au_write8(padapter, REG_DRVERLYINT, 0x05);
+		/*  10ms for port0 */
+		rtl8723au_write8(padapter, REG_ATIMWND, 0x0a);
+		rtl8723au_write16(padapter, REG_BCNTCFG, 0x00);
+		rtl8723au_write16(padapter, REG_TBTT_PROHIBIT, 0xff04);
 		/*  +32767 (~32ms) */
-		rtw_write16(padapter, REG_TSFTR_SYN_OFFSET, 0x7fff);
+		rtl8723au_write16(padapter, REG_TSFTR_SYN_OFFSET, 0x7fff);
 
 		/*  reset TSF */
-		rtw_write8(padapter, REG_DUAL_TSF_RST, BIT(0));
+		rtl8723au_write8(padapter, REG_DUAL_TSF_RST, BIT(0));
 
 		/*  enable BCN Function */
 		/*  don't enable update TSF (due to TSF update when
@@ -2991,12 +2434,12 @@ static void hw_var_set_opmode(struct rtw_adapter *padapter, u8 mode)
 		SetBcnCtrlReg23a(padapter, val8, ~val8);
 	}
 
-	val8 = rtw_read8(padapter, MSR);
+	val8 = rtl8723au_read8(padapter, MSR);
 	val8 = (val8 & 0xC) | mode;
-	rtw_write8(padapter, MSR, val8);
+	rtl8723au_write8(padapter, MSR, val8);
 }
 
-static void hw_var_set_macaddr(struct rtw_adapter *padapter, u8 *val)
+void hw_var_set_macaddr(struct rtw_adapter *padapter, u8 *val)
 {
 	u8 idx = 0;
 	u32 reg_macid;
@@ -3004,10 +2447,10 @@ static void hw_var_set_macaddr(struct rtw_adapter *padapter, u8 *val)
 	reg_macid = REG_MACID;
 
 	for (idx = 0; idx < 6; idx++)
-		rtw_write8(padapter, (reg_macid + idx), val[idx]);
+		rtl8723au_write8(padapter, (reg_macid + idx), val[idx]);
 }
 
-static void hw_var_set_bssid(struct rtw_adapter *padapter, u8 *val)
+void hw_var_set_bssid(struct rtw_adapter *padapter, u8 *val)
 {
 	u8 idx = 0;
 	u32 reg_bssid;
@@ -3015,10 +2458,10 @@ static void hw_var_set_bssid(struct rtw_adapter *padapter, u8 *val)
 	reg_bssid = REG_BSSID;
 
 	for (idx = 0; idx < 6; idx++)
-		rtw_write8(padapter, (reg_bssid + idx), val[idx]);
+		rtl8723au_write8(padapter, (reg_bssid + idx), val[idx]);
 }
 
-static void hw_var_set_correct_tsf(struct rtw_adapter *padapter)
+void hw_var_set_correct_tsf(struct rtw_adapter *padapter)
 {
 	u64 tsf;
 	u32 reg_tsftr;
@@ -3028,14 +2471,14 @@ static void hw_var_set_correct_tsf(struct rtw_adapter *padapter)
 	/* tsf = pmlmeext->TSFValue - ((u32)pmlmeext->TSFValue %
 	   (pmlmeinfo->bcn_interval*1024)) - 1024; us */
 	tsf = pmlmeext->TSFValue -
-		rtw_modular6423a(pmlmeext->TSFValue,
-			      (pmlmeinfo->bcn_interval * 1024)) - 1024;	/* us */
+		do_div(pmlmeext->TSFValue,
+		       (pmlmeinfo->bcn_interval * 1024)) - 1024;	/* us */
 
 	if (((pmlmeinfo->state & 0x03) == WIFI_FW_ADHOC_STATE) ||
 	    ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE)) {
 		/* pHalData->RegTxPause |= STOP_BCNQ;BIT(6) */
-		/* rtw_write8(padapter, REG_TXPAUSE,
-		   (rtw_read8(Adapter, REG_TXPAUSE)|BIT(6))); */
+		/* rtl8723au_write8(padapter, REG_TXPAUSE,
+		   (rtl8723au_read8(Adapter, REG_TXPAUSE)|BIT(6))); */
 		StopTxBeacon(padapter);
 	}
 
@@ -3044,8 +2487,8 @@ static void hw_var_set_correct_tsf(struct rtw_adapter *padapter)
 	/*  disable related TSF function */
 	SetBcnCtrlReg23a(padapter, 0, EN_BCN_FUNCTION);
 
-	rtw_write32(padapter, reg_tsftr, tsf);
-	rtw_write32(padapter, reg_tsftr + 4, tsf >> 32);
+	rtl8723au_write32(padapter, reg_tsftr, tsf);
+	rtl8723au_write32(padapter, reg_tsftr + 4, tsf >> 32);
 
 	/* enable related TSF function */
 	SetBcnCtrlReg23a(padapter, EN_BCN_FUNCTION, 0);
@@ -3055,19 +2498,19 @@ static void hw_var_set_correct_tsf(struct rtw_adapter *padapter)
 		ResumeTxBeacon(padapter);
 }
 
-static void hw_var_set_mlme_disconnect(struct rtw_adapter *padapter)
+void hw_var_set_mlme_disconnect(struct rtw_adapter *padapter)
 {
 	/*  reject all data frames */
-	rtw_write16(padapter, REG_RXFLTMAP2, 0);
+	rtl8723au_write16(padapter, REG_RXFLTMAP2, 0);
 
 	/*  reset TSF */
-	rtw_write8(padapter, REG_DUAL_TSF_RST, BIT(0));
+	rtl8723au_write8(padapter, REG_DUAL_TSF_RST, BIT(0));
 
 	/*  disable update TSF */
 	SetBcnCtrlReg23a(padapter, DIS_TSF_UDT, 0);
 }
 
-static void hw_var_set_mlme_join(struct rtw_adapter *padapter, u8 type)
+void hw_var_set_mlme_join(struct rtw_adapter *padapter, u8 type)
 {
 	u8 RetryLimit = 0x30;
 
@@ -3078,13 +2521,13 @@ static void hw_var_set_mlme_join(struct rtw_adapter *padapter, u8 type)
 		u32 v32;
 
 		/*  enable to rx data frame.Accept all data frame */
-		/* rtw_write32(padapter, REG_RCR,
-		   rtw_read32(padapter, REG_RCR)|RCR_ADF); */
-		rtw_write16(padapter, REG_RXFLTMAP2, 0xFFFF);
+		/* rtl8723au_write32(padapter, REG_RCR,
+		   rtl8723au_read32(padapter, REG_RCR)|RCR_ADF); */
+		rtl8723au_write16(padapter, REG_RXFLTMAP2, 0xFFFF);
 
-		v32 = rtw_read32(padapter, REG_RCR);
+		v32 = rtl8723au_read32(padapter, REG_RCR);
 		v32 |= RCR_CBSSID_DATA | RCR_CBSSID_BCN;
-		rtw_write32(padapter, REG_RCR, v32);
+		rtl8723au_write32(padapter, REG_RCR, v32);
 
 		if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true)
 			RetryLimit =
@@ -3094,7 +2537,7 @@ static void hw_var_set_mlme_join(struct rtw_adapter *padapter, u8 type)
 	} else if (type == 1) {	/*  joinbss_event callback when join res < 0 */
 		/*  config RCR to receive different BSSID & not to
 		    receive data frame during linking */
-		rtw_write16(padapter, REG_RXFLTMAP2, 0);
+		rtl8723au_write16(padapter, REG_RXFLTMAP2, 0);
 	} else if (type == 2) {	/*  sta add event callback */
 		/*  enable update TSF */
 		SetBcnCtrlReg23a(padapter, 0, DIS_TSF_UDT);
@@ -3102,351 +2545,27 @@ static void hw_var_set_mlme_join(struct rtw_adapter *padapter, u8 type)
 		if (check_fwstate(pmlmepriv,
 				  WIFI_ADHOC_STATE | WIFI_ADHOC_MASTER_STATE)) {
 			/*  fixed beacon issue for 8191su........... */
-			rtw_write8(padapter, 0x542, 0x02);
+			rtl8723au_write8(padapter, 0x542, 0x02);
 			RetryLimit = 0x7;
 		}
 	}
 
-	rtw_write16(padapter, REG_RL,
-		    RetryLimit << RETRY_LIMIT_SHORT_SHIFT | RetryLimit <<
-		    RETRY_LIMIT_LONG_SHIFT);
+	rtl8723au_write16(padapter, REG_RL,
+			  RetryLimit << RETRY_LIMIT_SHORT_SHIFT | RetryLimit <<
+			  RETRY_LIMIT_LONG_SHIFT);
 
-#ifdef CONFIG_8723AU_BT_COEXIST
 	switch (type) {
 	case 0:
 		/*  prepare to join */
-		BT_WifiAssociateNotify(padapter, true);
+		rtl8723a_BT_wifiassociate_notify(padapter, true);
 		break;
 	case 1:
 		/*  joinbss_event callback when join res < 0 */
-		BT_WifiAssociateNotify(padapter, false);
+		rtl8723a_BT_wifiassociate_notify(padapter, false);
 		break;
 	case 2:
 		/*  sta add event callback */
 /*		BT_WifiMediaStatusNotify(padapter, RT_MEDIA_CONNECT); */
 		break;
 	}
-#endif
-}
-
-void SetHwReg8723A(struct rtw_adapter *padapter, u8 variable, u8 *val)
-{
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
-	u32 *val32 = (u32 *)val;
-
-	switch (variable) {
-	case HW_VAR_MEDIA_STATUS:
-		rtl8723a_set_media_status(padapter, *val);
-		break;
-
-	case HW_VAR_MEDIA_STATUS1:
-		rtl8723a_set_media_status1(padapter, *val);
-		break;
-
-	case HW_VAR_SET_OPMODE:
-		hw_var_set_opmode(padapter, *val);
-		break;
-
-	case HW_VAR_MAC_ADDR:
-		hw_var_set_macaddr(padapter, val);
-		break;
-
-	case HW_VAR_BSSID:
-		hw_var_set_bssid(padapter, val);
-		break;
-
-	case HW_VAR_BASIC_RATE:
-		HalSetBrateCfg23a(padapter, val);
-		break;
-
-	case HW_VAR_TXPAUSE:
-		rtl8723a_set_tx_pause(padapter, *val);
-		break;
-
-	case HW_VAR_BCN_FUNC:
-		rtl8723a_set_bcn_func(padapter, *val);
-		break;
-
-	case HW_VAR_CORRECT_TSF:
-		hw_var_set_correct_tsf(padapter);
-		break;
-
-	case HW_VAR_CHECK_BSSID:
-		rtl8723a_check_bssid(padapter, *val);
-		break;
-
-	case HW_VAR_MLME_DISCONNECT:
-		hw_var_set_mlme_disconnect(padapter);
-		break;
-
-	case HW_VAR_MLME_SITESURVEY:
-		rtl8723a_mlme_sitesurvey(padapter, *val);
-		break;
-
-	case HW_VAR_MLME_JOIN:
-		hw_var_set_mlme_join(padapter, *val);
-		break;
-
-	case HW_VAR_ON_RCR_AM:
-		rtl8723a_on_rcr_am(padapter);
-		break;
-
-	case HW_VAR_OFF_RCR_AM:
-		rtl8723a_off_rcr_am(padapter);
-		break;
-
-	case HW_VAR_BEACON_INTERVAL:
-		rtl8723a_set_beacon_interval(padapter, *((u16 *) val));
-		break;
-
-	case HW_VAR_SLOT_TIME:
-		rtl8723a_set_slot_time(padapter, *val);
-		break;
-
-	case HW_VAR_RESP_SIFS:
-		rtl8723a_set_resp_sifs(padapter, val[0], val[1],
-				       val[2], val[3]);
-		break;
-
-	case HW_VAR_ACK_PREAMBLE:
-		rtl8723a_ack_preamble(padapter, *val);
-		break;
-
-	case HW_VAR_SEC_CFG:
-		rtl8723a_set_sec_cfg(padapter, *val);
-		break;
-
-	case HW_VAR_DM_FLAG:
-		rtl8723a_odm_support_ability_write(padapter, *val32);
-		break;
-	case HW_VAR_DM_FUNC_OP:
-		rtl8723a_odm_support_ability_backup(padapter, *val);
-		break;
-	case HW_VAR_DM_FUNC_SET:
-		rtl8723a_odm_support_ability_set(padapter, *val32);
-		break;
-
-	case HW_VAR_DM_FUNC_CLR:
-		rtl8723a_odm_support_ability_clr(padapter, *val32);
-		break;
-
-	case HW_VAR_CAM_EMPTY_ENTRY:
-		rtl8723a_cam_empty_entry(padapter, *val);
-		break;
-
-	case HW_VAR_CAM_INVALID_ALL:
-		rtl8723a_cam_invalid_all(padapter);
-		break;
-
-	case HW_VAR_CAM_WRITE:
-		rtl8723a_cam_write(padapter, val32[0], val32[1]);
-		break;
-
-	case HW_VAR_AC_PARAM_VO:
-		rtl8723a_set_ac_param_vo(padapter, *val32);
-		break;
-
-	case HW_VAR_AC_PARAM_VI:
-		rtl8723a_set_ac_param_vi(padapter, *val32);
-		break;
-
-	case HW_VAR_AC_PARAM_BE:
-		rtl8723a_set_ac_param_be(padapter, *val32);
-		break;
-
-	case HW_VAR_AC_PARAM_BK:
-		rtl8723a_set_ac_param_bk(padapter, *val32);
-		break;
-
-	case HW_VAR_ACM_CTRL:
-		rtl8723a_set_acm_ctrl(padapter, *val);
-		break;
-
-	case HW_VAR_AMPDU_MIN_SPACE:
-		rtl8723a_set_ampdu_min_space(padapter, *val);
-		break;
-
-	case HW_VAR_AMPDU_FACTOR:
-		rtl8723a_set_ampdu_factor(padapter, *val);
-		break;
-
-	case HW_VAR_RXDMA_AGG_PG_TH:
-		rtl8723a_set_rxdma_agg_pg_th(padapter, *val);
-		break;
-
-	case HW_VAR_H2C_FW_PWRMODE:
-		rtl8723a_set_FwPwrMode_cmd(padapter, *val);
-		break;
-
-	case HW_VAR_H2C_FW_JOINBSSRPT:
-		rtl8723a_set_FwJoinBssReport_cmd(padapter, *val);
-		break;
-
-#ifdef CONFIG_8723AU_P2P
-	case HW_VAR_H2C_FW_P2P_PS_OFFLOAD:
-		rtl8723a_set_p2p_ps_offload_cmd(padapter, *val);
-		break;
-#endif /* CONFIG_8723AU_P2P */
-
-	case HW_VAR_INITIAL_GAIN:
-		rtl8723a_set_initial_gain(padapter, *val32);
-		break;
-	case HW_VAR_EFUSE_BYTES:
-		pHalData->EfuseUsedBytes = *((u16 *) val);
-		break;
-	case HW_VAR_EFUSE_BT_BYTES:
-		pHalData->BTEfuseUsedBytes = *((u16 *) val);
-		break;
-	case HW_VAR_FIFO_CLEARN_UP:
-		rtl8723a_fifo_cleanup(padapter);
-		break;
-	case HW_VAR_CHECK_TXBUF:
-		break;
-	case HW_VAR_APFM_ON_MAC:
-		rtl8723a_set_apfm_on_mac(padapter, *val);
-		break;
-
-	case HW_VAR_NAV_UPPER:
-		rtl8723a_set_nav_upper(padapter, *val32);
-		break;
-	case HW_VAR_BCN_VALID:
-		rtl8723a_bcn_valid(padapter);
-		break;
-	default:
-		break;
-	}
-
-}
-
-void GetHwReg8723A(struct rtw_adapter *padapter, u8 variable, u8 *val)
-{
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
-
-	switch (variable) {
-	case HW_VAR_BASIC_RATE:
-		*((u16 *) val) = pHalData->BasicRateSet;
-		break;
-
-	case HW_VAR_TXPAUSE:
-		*val = rtw_read8(padapter, REG_TXPAUSE);
-		break;
-
-	case HW_VAR_BCN_VALID:
-		/* BCN_VALID, BIT16 of REG_TDECTRL = BIT0 of REG_TDECTRL+2 */
-		val[0] = (BIT0 & rtw_read8(padapter, REG_TDECTRL + 2)) ? true :
-			false;
-		break;
-
-	case HW_VAR_RF_TYPE:
-		*val = pHalData->rf_type;
-		break;
-
-	case HW_VAR_DM_FLAG:
-	{
-		struct dm_odm_t *podmpriv = &pHalData->odmpriv;
-		*((u32 *) val) = podmpriv->SupportAbility;
-	}
-		break;
-
-	case HW_VAR_FWLPS_RF_ON:
-	{
-		/*  When we halt NIC, we should check if FW LPS is leave. */
-		u32 valRCR;
-
-		if ((padapter->bSurpriseRemoved == true) ||
-		    (padapter->pwrctrlpriv.rf_pwrstate == rf_off)) {
-			/*  If it is in HW/SW Radio OFF or IPS state, we do
-			    not check Fw LPS Leave, because Fw is unload. */
-			*val = true;
-		} else {
-			valRCR = rtw_read32(padapter, REG_RCR);
-			valRCR &= 0x00070000;
-			if (valRCR)
-				*val = false;
-			else
-				*val = true;
-		}
-	}
-		break;
-	case HW_VAR_EFUSE_BYTES:
-		*((u16 *) val) = pHalData->EfuseUsedBytes;
-		break;
-
-	case HW_VAR_EFUSE_BT_BYTES:
-		*((u16 *) val) = pHalData->BTEfuseUsedBytes;
-		break;
-
-	case HW_VAR_APFM_ON_MAC:
-		*val = pHalData->bMacPwrCtrlOn;
-		break;
-	case HW_VAR_CHK_HI_QUEUE_EMPTY:
-		*val =
-		    ((rtw_read32(padapter, REG_HGQ_INFORMATION) & 0x0000ff00) ==
-		     0) ? true : false;
-		break;
-	}
-}
-
-#ifdef CONFIG_8723AU_BT_COEXIST
-
-void rtl8723a_SingleDualAntennaDetection(struct rtw_adapter *padapter)
-{
-	struct hal_data_8723a *pHalData;
-	struct dm_odm_t *pDM_Odm;
-	struct sw_ant_sw *pDM_SWAT_Table;
-	u8 i;
-
-	pHalData = GET_HAL_DATA(padapter);
-	pDM_Odm = &pHalData->odmpriv;
-	pDM_SWAT_Table = &pDM_Odm->DM_SWAT_Table;
-
-	/*  */
-	/*  <Roger_Notes> RTL8723A Single and Dual antenna dynamic detection
-	    mechanism when RF power state is on. */
-	/*  We should take power tracking, IQK, LCK, RCK RF read/write
-	    operation into consideration. */
-	/*  2011.12.15. */
-	/*  */
-	if (!pHalData->bAntennaDetected) {
-		u8 btAntNum = BT_GetPGAntNum(padapter);
-
-		/*  Set default antenna B status */
-		if (btAntNum == Ant_x2)
-			pDM_SWAT_Table->ANTB_ON = true;
-		else if (btAntNum == Ant_x1)
-			pDM_SWAT_Table->ANTB_ON = false;
-		else
-			pDM_SWAT_Table->ANTB_ON = true;
-
-		if (pHalData->CustomerID != RT_CID_TOSHIBA) {
-			for (i = 0; i < MAX_ANTENNA_DETECTION_CNT; i++) {
-				if (ODM_SingleDualAntennaDetection
-				    (&pHalData->odmpriv, ANTTESTALL) == true)
-					break;
-			}
-
-			/*  Set default antenna number for BT coexistence */
-			if (btAntNum == Ant_x2)
-				BT_SetBtCoexCurrAntNum(padapter,
-						       pDM_SWAT_Table->
-						       ANTB_ON ? 2 : 1);
-		}
-		pHalData->bAntennaDetected = true;
-	}
-}
-#endif /*  CONFIG_8723AU_BT_COEXIST */
-
-void rtl8723a_clone_haldata(struct rtw_adapter *dst_adapter,
-			    struct rtw_adapter *src_adapter)
-{
-	memcpy(dst_adapter->HalData, src_adapter->HalData,
-	       dst_adapter->hal_data_sz);
-}
-
-void rtl8723a_start_thread(struct rtw_adapter *padapter)
-{
-}
-
-void rtl8723a_stop_thread(struct rtw_adapter *padapter)
-{
 }
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_phycfg.c b/drivers/staging/rtl8723au/hal/rtl8723a_phycfg.c
index 8400e6e2fca..d23525e664f 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723a_phycfg.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723a_phycfg.c
@@ -18,6 +18,7 @@
 #include <drv_types.h>
 
 #include <rtl8723a_hal.h>
+#include <usb_ops_linux.h>
 
 /*---------------------------Define Local Constant---------------------------*/
 /* Channel switch:The size of command tables for switch channel*/
@@ -87,7 +88,7 @@ PHY_QueryBBReg(struct rtw_adapter *Adapter, u32 RegAddr, u32 BitMask)
 {
 	u32	ReturnValue = 0, OriginalValue, BitShift;
 
-	OriginalValue = rtw_read32(Adapter, RegAddr);
+	OriginalValue = rtl8723au_read32(Adapter, RegAddr);
 	BitShift = phy_CalculateBitShift(BitMask);
 	ReturnValue = (OriginalValue & BitMask) >> BitShift;
 	return ReturnValue;
@@ -123,12 +124,12 @@ PHY_SetBBReg(struct rtw_adapter *Adapter, u32 RegAddr, u32 BitMask, u32	Data)
 	/* RT_TRACE(COMP_RF, DBG_TRACE, ("--->PHY_SetBBReg(): RegAddr(%#lx), BitMask(%#lx), Data(%#lx)\n", RegAddr, BitMask, Data)); */
 
 	if (BitMask != bMaskDWord) {/* if not "double word" write */
-		OriginalValue = rtw_read32(Adapter, RegAddr);
+		OriginalValue = rtl8723au_read32(Adapter, RegAddr);
 		BitShift = phy_CalculateBitShift(BitMask);
 		Data = ((OriginalValue & (~BitMask)) | (Data << BitShift));
 	}
 
-	rtw_write32(Adapter, RegAddr, Data);
+	rtl8723au_write32(Adapter, RegAddr, Data);
 
 	/* RTPRINT(FPHY, PHY_BBW, ("BBW MASK = 0x%lx Addr[0x%lx]= 0x%lx\n", BitMask, RegAddr, Data)); */
 	/* RT_TRACE(COMP_RF, DBG_TRACE, ("<---PHY_SetBBReg(): RegAddr(%#lx), BitMask(%#lx), Data(%#lx)\n", RegAddr, BitMask, Data)); */
@@ -212,10 +213,12 @@ phy_RFSerialRead(struct rtw_adapter *Adapter, enum RF_RADIO_PATH eRFPath,
 
 	if (eRFPath == RF_PATH_A)
 		RfPiEnable = (u8)PHY_QueryBBReg(Adapter,
-						rFPGA0_XA_HSSIParameter1, BIT8);
+						rFPGA0_XA_HSSIParameter1,
+						BIT(8));
 	else if (eRFPath == RF_PATH_B)
 		RfPiEnable = (u8)PHY_QueryBBReg(Adapter,
-						rFPGA0_XB_HSSIParameter1, BIT8);
+						rFPGA0_XB_HSSIParameter1,
+						BIT(8));
 
 	if (RfPiEnable)	{
 		/* Read from BBreg8b8, 12 bits for 8190, 20bits for T65 RF */
@@ -413,28 +416,23 @@ PHY_SetRFReg(struct rtw_adapter *Adapter, enum RF_RADIO_PATH eRFPath,
  *  08/12/2008	MHC		Create Version 0.
  *
  *---------------------------------------------------------------------------*/
-s32 PHY_MACConfig8723A(struct rtw_adapter *Adapter)
+int PHY_MACConfig8723A(struct rtw_adapter *Adapter)
 {
 	int rtStatus = _SUCCESS;
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
-	s8 *pszMACRegFile;
-	s8 sz8723MACRegFile[] = RTL8723_PHY_MACREG;
 	bool is92C = IS_92C_SERIAL(pHalData->VersionID);
 
-	pszMACRegFile = sz8723MACRegFile;
-
 	/*  */
 	/*  Config MAC */
 	/*  */
-	if (HAL_STATUS_FAILURE ==
-	    ODM_ConfigMACWithHeaderFile23a(&pHalData->odmpriv))
+	if (ODM_ConfigMACWithHeaderFile23a(&pHalData->odmpriv) == _FAIL)
 		rtStatus = _FAIL;
 
 	/*  2010.07.13 AMPDU aggregation number 9 */
 	/* rtw_write16(Adapter, REG_MAX_AGGR_NUM, MAX_AGGR_NUM); */
-	rtw_write8(Adapter, REG_MAX_AGGR_NUM, 0x0A); /* By tynli. 2010.11.18. */
+	rtl8723au_write8(Adapter, REG_MAX_AGGR_NUM, 0x0A);
 	if (is92C && (BOARD_USB_DONGLE == pHalData->BoardType))
-		rtw_write8(Adapter, 0x40, 0x04);
+		rtl8723au_write8(Adapter, 0x40, 0x04);
 
 	return rtStatus;
 }
@@ -751,27 +749,12 @@ phy_BB8723a_Config_ParaFile(struct rtw_adapter *Adapter)
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
 	int rtStatus = _SUCCESS;
 
-	u8 sz8723BBRegFile[] = RTL8723_PHY_REG;
-	u8 sz8723AGCTableFile[] = RTL8723_AGC_TAB;
-	u8 sz8723BBRegPgFile[] = RTL8723_PHY_REG_PG;
-	u8 sz8723BBRegMpFile[] = RTL8723_PHY_REG_MP;
-
-	u8 *pszBBRegFile = NULL, *pszAGCTableFile = NULL;
-	u8 *pszBBRegPgFile = NULL, *pszBBRegMpFile = NULL;
-
-	/* RT_TRACE(COMP_INIT, DBG_TRACE, ("==>phy_BB8192S_Config_ParaFile\n")); */
-
-	pszBBRegFile = sz8723BBRegFile ;
-	pszAGCTableFile = sz8723AGCTableFile;
-	pszBBRegPgFile = sz8723BBRegPgFile;
-	pszBBRegMpFile = sz8723BBRegMpFile;
-
 	/*  */
 	/*  1. Read PHY_REG.TXT BB INIT!! */
 	/*  We will seperate as 88C / 92C according to chip version */
 	/*  */
-	if (HAL_STATUS_FAILURE == ODM_ConfigBBWithHeaderFile23a(&pHalData->odmpriv,
-							     CONFIG_BB_PHY_REG))
+	if (ODM_ConfigBBWithHeaderFile23a(&pHalData->odmpriv,
+					  CONFIG_BB_PHY_REG) == _FAIL)
 		rtStatus = _FAIL;
 	if (rtStatus != _SUCCESS)
 		goto phy_BB8190_Config_ParaFile_Fail;
@@ -801,8 +784,8 @@ phy_BB8723a_Config_ParaFile(struct rtw_adapter *Adapter)
 	/*  */
 	/*  3. BB AGC table Initialization */
 	/*  */
-	if (HAL_STATUS_FAILURE == ODM_ConfigBBWithHeaderFile23a(&pHalData->odmpriv,
-							     CONFIG_BB_AGC_TAB))
+	if (ODM_ConfigBBWithHeaderFile23a(&pHalData->odmpriv,
+					  CONFIG_BB_AGC_TAB) == _FAIL)
 		rtStatus = _FAIL;
 
 phy_BB8190_Config_ParaFile_Fail:
@@ -822,30 +805,30 @@ PHY_BBConfig8723A(struct rtw_adapter *Adapter)
 
 	/*  Suggested by Scott. tynli_test. 2010.12.30. */
 	/* 1. 0x28[1] = 1 */
-	TmpU1B = rtw_read8(Adapter, REG_AFE_PLL_CTRL);
+	TmpU1B = rtl8723au_read8(Adapter, REG_AFE_PLL_CTRL);
 	udelay(2);
-	rtw_write8(Adapter, REG_AFE_PLL_CTRL, (TmpU1B|BIT1));
+	rtl8723au_write8(Adapter, REG_AFE_PLL_CTRL, TmpU1B | BIT(1));
 	udelay(2);
 
 	/* 2. 0x29[7:0] = 0xFF */
-	rtw_write8(Adapter, REG_AFE_PLL_CTRL+1, 0xff);
+	rtl8723au_write8(Adapter, REG_AFE_PLL_CTRL+1, 0xff);
 	udelay(2);
 
 	/* 3. 0x02[1:0] = 2b'11 */
-	TmpU1B = rtw_read8(Adapter, REG_SYS_FUNC_EN);
-	rtw_write8(Adapter, REG_SYS_FUNC_EN,
-		   (TmpU1B | FEN_BB_GLB_RSTn | FEN_BBRSTB));
+	TmpU1B = rtl8723au_read8(Adapter, REG_SYS_FUNC_EN);
+	rtl8723au_write8(Adapter, REG_SYS_FUNC_EN,
+			 (TmpU1B | FEN_BB_GLB_RSTn | FEN_BBRSTB));
 
 	/* 4. 0x25[6] = 0 */
-	TmpU1B = rtw_read8(Adapter, REG_AFE_XTAL_CTRL + 1);
-	rtw_write8(Adapter, REG_AFE_XTAL_CTRL+1, (TmpU1B & (~BIT6)));
+	TmpU1B = rtl8723au_read8(Adapter, REG_AFE_XTAL_CTRL + 1);
+	rtl8723au_write8(Adapter, REG_AFE_XTAL_CTRL+1, TmpU1B & ~BIT(6));
 
 	/* 5. 0x24[20] = 0	Advised by SD3 Alex Wang. 2011.02.09. */
-	TmpU1B = rtw_read8(Adapter, REG_AFE_XTAL_CTRL+2);
-	rtw_write8(Adapter, REG_AFE_XTAL_CTRL+2, (TmpU1B & (~BIT4)));
+	TmpU1B = rtl8723au_read8(Adapter, REG_AFE_XTAL_CTRL+2);
+	rtl8723au_write8(Adapter, REG_AFE_XTAL_CTRL+2, TmpU1B & ~BIT(4));
 
 	/* 6. 0x1f[7:0] = 0x07 */
-	rtw_write8(Adapter, REG_RF_CTRL, 0x07);
+	rtl8723au_write8(Adapter, REG_RF_CTRL, 0x07);
 
 	/*  */
 	/*  Config BB and AGC */
@@ -974,20 +957,20 @@ _PHY_SetBWMode23a92C(struct rtw_adapter *Adapter)
 	/* 3<1>Set MAC register */
 	/* 3 */
 
-	regBwOpMode = rtw_read8(Adapter, REG_BWOPMODE);
-	regRRSR_RSC = rtw_read8(Adapter, REG_RRSR+2);
+	regBwOpMode = rtl8723au_read8(Adapter, REG_BWOPMODE);
+	regRRSR_RSC = rtl8723au_read8(Adapter, REG_RRSR+2);
 
 	switch (pHalData->CurrentChannelBW) {
 	case HT_CHANNEL_WIDTH_20:
 		regBwOpMode |= BW_OPMODE_20MHZ;
-		rtw_write8(Adapter, REG_BWOPMODE, regBwOpMode);
+		rtl8723au_write8(Adapter, REG_BWOPMODE, regBwOpMode);
 		break;
 	case HT_CHANNEL_WIDTH_40:
 		regBwOpMode &= ~BW_OPMODE_20MHZ;
-		rtw_write8(Adapter, REG_BWOPMODE, regBwOpMode);
+		rtl8723au_write8(Adapter, REG_BWOPMODE, regBwOpMode);
 		regRRSR_RSC = (regRRSR_RSC & 0x90) |
 			(pHalData->nCur40MhzPrimeSC << 5);
-		rtw_write8(Adapter, REG_RRSR+2, regRRSR_RSC);
+		rtl8723au_write8(Adapter, REG_RRSR+2, regRRSR_RSC);
 		break;
 
 	default:
@@ -1002,7 +985,7 @@ _PHY_SetBWMode23a92C(struct rtw_adapter *Adapter)
 	case HT_CHANNEL_WIDTH_20:
 		PHY_SetBBReg(Adapter, rFPGA0_RFMOD, bRFMOD, 0x0);
 		PHY_SetBBReg(Adapter, rFPGA1_RFMOD, bRFMOD, 0x0);
-		PHY_SetBBReg(Adapter, rFPGA0_AnalogParameter2, BIT10, 1);
+		PHY_SetBBReg(Adapter, rFPGA0_AnalogParameter2, BIT(10), 1);
 
 		break;
 
@@ -1017,9 +1000,9 @@ _PHY_SetBWMode23a92C(struct rtw_adapter *Adapter)
 			     (pHalData->nCur40MhzPrimeSC >> 1));
 		PHY_SetBBReg(Adapter, rOFDM1_LSTF, 0xC00,
 			     pHalData->nCur40MhzPrimeSC);
-		PHY_SetBBReg(Adapter, rFPGA0_AnalogParameter2, BIT10, 0);
+		PHY_SetBBReg(Adapter, rFPGA0_AnalogParameter2, BIT(10), 0);
 
-		PHY_SetBBReg(Adapter, 0x818, (BIT26 | BIT27),
+		PHY_SetBBReg(Adapter, 0x818, BIT(26) | BIT(27),
 			     (pHalData->nCur40MhzPrimeSC ==
 			      HAL_PRIME_CHNL_OFFSET_LOWER) ? 2:1);
 		break;
@@ -1116,7 +1099,7 @@ static void _PHY_SwChnl8723A(struct rtw_adapter *Adapter, u8 channel)
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
 
 	if (Adapter->bNotifyChannelChange)
-		DBG_8723A("[%s] ch = %d\n", __FUNCTION__, channel);
+		DBG_8723A("[%s] ch = %d\n", __func__, channel);
 
 	/* s1. pre common command - CmdID_SetTxPowerLevel */
 	PHY_SetTxPowerLevel8723A(Adapter, channel);
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_rf6052.c b/drivers/staging/rtl8723au/hal/rtl8723a_rf6052.c
index ed39c18c3f8..c30bd232bf6 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723a_rf6052.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723a_rf6052.c
@@ -40,6 +40,7 @@
 #include <drv_types.h>
 
 #include <rtl8723a_hal.h>
+#include <usb_ops_linux.h>
 
 /*---------------------------Define Local Constant---------------------------*/
 /*  Define local structure for debug!!!!! */
@@ -368,7 +369,8 @@ static void writeOFDMPowerReg(struct rtw_adapter *Adapter, u8 index, u32 *pValue
 					writeVal = (writeVal > 8) ? (writeVal-8) : 0;
 				else
 					writeVal = (writeVal > 6) ? (writeVal-6) : 0;
-				rtw_write8(Adapter, (u32)(RegOffset+i), (u8)writeVal);
+				rtl8723au_write8(Adapter, RegOffset + i,
+						 (u8)writeVal);
 			}
 		}
 	}
@@ -417,12 +419,6 @@ static int phy_RF6052_Config_ParaFile(struct rtw_adapter *Adapter)
 	struct bb_reg_define	*pPhyReg;
 	int rtStatus = _SUCCESS;
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
-	static char sz8723RadioAFile[] = RTL8723_PHY_RADIO_A;
-	static char sz8723RadioBFile[] = RTL8723_PHY_RADIO_B;
-	char *pszRadioAFile, *pszRadioBFile;
-
-	pszRadioAFile = sz8723RadioAFile;
-	pszRadioBFile = sz8723RadioBFile;
 
 	/* 3----------------------------------------------------------------- */
 	/* 3 <2> Initialize RF */
@@ -459,11 +455,19 @@ static int phy_RF6052_Config_ParaFile(struct rtw_adapter *Adapter)
 		/*----Initialize RF fom connfiguration file----*/
 		switch (eRFPath) {
 		case RF_PATH_A:
-			if (HAL_STATUS_FAILURE == ODM_ConfigRFWithHeaderFile23a(&pHalData->odmpriv, (enum RF_RADIO_PATH)eRFPath, (enum RF_RADIO_PATH)eRFPath))
+			if (ODM_ConfigRFWithHeaderFile23a(&pHalData->odmpriv,
+							  (enum RF_RADIO_PATH)
+							  eRFPath,
+							  (enum RF_RADIO_PATH)
+							  eRFPath) == _FAIL)
 				rtStatus = _FAIL;
 			break;
 		case RF_PATH_B:
-			if (HAL_STATUS_FAILURE == ODM_ConfigRFWithHeaderFile23a(&pHalData->odmpriv, (enum RF_RADIO_PATH)eRFPath, (enum RF_RADIO_PATH)eRFPath))
+			if (ODM_ConfigRFWithHeaderFile23a(&pHalData->odmpriv,
+							  (enum RF_RADIO_PATH)
+							  eRFPath,
+							  (enum RF_RADIO_PATH)
+							  eRFPath) == _FAIL)
 				rtStatus = _FAIL;
 			break;
 		}
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_sreset.c b/drivers/staging/rtl8723au/hal/rtl8723a_sreset.c
index c0218e734b9..0680e29a552 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723a_sreset.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723a_sreset.c
@@ -16,6 +16,7 @@
 
 #include <rtl8723a_sreset.h>
 #include <rtl8723a_hal.h>
+#include <usb_ops_linux.h>
 
 void rtl8723a_sreset_xmit_status_check(struct rtw_adapter *padapter)
 {
@@ -27,10 +28,10 @@ void rtl8723a_sreset_xmit_status_check(struct rtw_adapter *padapter)
 	unsigned int diff_time;
 	u32 txdma_status;
 
-	txdma_status = rtw_read32(padapter, REG_TXDMA_STATUS);
+	txdma_status = rtl8723au_read32(padapter, REG_TXDMA_STATUS);
 	if (txdma_status != 0) {
 		DBG_8723A("%s REG_TXDMA_STATUS:0x%08x\n", __func__, txdma_status);
-		rtw_hal_sreset_reset23a(padapter);
+		rtw_sreset_reset(padapter);
 	}
 
 	current_time = jiffies;
@@ -47,7 +48,7 @@ void rtl8723a_sreset_xmit_status_check(struct rtw_adapter *padapter)
 				if (diff_time > 4000) {
 					/* padapter->Wifi_Error_Status = WIFI_TX_HANG; */
 					DBG_8723A("%s tx hang\n", __func__);
-					rtw_hal_sreset_reset23a(padapter);
+					rtw_sreset_reset(padapter);
 				}
 			}
 		}
@@ -55,7 +56,7 @@ void rtl8723a_sreset_xmit_status_check(struct rtw_adapter *padapter)
 
 	if (psrtpriv->dbg_trigger_point == SRESET_TGP_XMIT_STATUS) {
 		psrtpriv->dbg_trigger_point = SRESET_TGP_NULL;
-		rtw_hal_sreset_reset23a(padapter);
+		rtw_sreset_reset(padapter);
 		return;
 	}
 }
@@ -67,7 +68,7 @@ void rtl8723a_sreset_linked_status_check(struct rtw_adapter *padapter)
 
 	if (psrtpriv->dbg_trigger_point == SRESET_TGP_LINK_STATUS) {
 		psrtpriv->dbg_trigger_point = SRESET_TGP_NULL;
-		rtw_hal_sreset_reset23a(padapter);
+		rtw_sreset_reset(padapter);
 		return;
 	}
 }
diff --git a/drivers/staging/rtl8723au/hal/rtl8723au_led.c b/drivers/staging/rtl8723au/hal/rtl8723au_led.c
index 4d5c909487f..b946636af9b 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723au_led.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723au_led.c
@@ -16,6 +16,7 @@
 #include "drv_types.h"
 #include "rtl8723a_hal.h"
 #include "rtl8723a_led.h"
+#include "usb_ops_linux.h"
 
 /*  */
 /*  LED object. */
@@ -41,14 +42,18 @@ void SwLedOn23a(struct rtw_adapter *padapter, struct led_8723a *pLed)
 	case LED_PIN_GPIO0:
 		break;
 	case LED_PIN_LED0:
-		rtw_write8(padapter, REG_LEDCFG0, (LedCfg&0xf0)|BIT5|BIT6); /*  SW control led0 on. */
+		/*  SW control led0 on. */
+		rtl8723au_write8(padapter, REG_LEDCFG0,
+				 (LedCfg&0xf0)|BIT(5)|BIT(6));
 		break;
 	case LED_PIN_LED1:
-		rtw_write8(padapter, REG_LEDCFG1, (LedCfg&0x00)|BIT6); /*  SW control led1 on. */
+		 /*  SW control led1 on. */
+		rtl8723au_write8(padapter, REG_LEDCFG1, (LedCfg&0x00)|BIT(6));
 		break;
 	case LED_PIN_LED2:
-		LedCfg = rtw_read8(padapter, REG_LEDCFG2);
-		rtw_write8(padapter, REG_LEDCFG2, (LedCfg&0x80)|BIT5); /*  SW control led1 on. */
+		LedCfg = rtl8723au_read8(padapter, REG_LEDCFG2);
+		 /*  SW control led1 on. */
+		rtl8723au_write8(padapter, REG_LEDCFG2, (LedCfg&0x80)|BIT(5));
 		break;
 	default:
 		break;
@@ -70,14 +75,20 @@ void SwLedOff23a(struct rtw_adapter *padapter, struct led_8723a *pLed)
 	case LED_PIN_GPIO0:
 		break;
 	case LED_PIN_LED0:
-		rtw_write8(padapter, REG_LEDCFG0, (LedCfg&0xf0)|BIT5|BIT6); /*  SW control led0 on. */
+		/*  SW control led0 on. */
+		rtl8723au_write8(padapter, REG_LEDCFG0,
+				 (LedCfg&0xf0)|BIT(5)|BIT(6));
 		break;
 	case LED_PIN_LED1:
-		rtw_write8(padapter, REG_LEDCFG1, (LedCfg&0x00)|BIT5|BIT6); /*  SW control led1 on. */
+		/*  SW control led1 on. */
+		rtl8723au_write8(padapter, REG_LEDCFG1,
+				 (LedCfg&0x00)|BIT(5)|BIT(6));
 		break;
 	case LED_PIN_LED2:
-		LedCfg = rtw_read8(padapter, REG_LEDCFG2);
-		rtw_write8(padapter, REG_LEDCFG2, (LedCfg&0x80)|BIT3|BIT5); /*  SW control led1 on. */
+		LedCfg = rtl8723au_read8(padapter, REG_LEDCFG2);
+		/*  SW control led1 on. */
+		rtl8723au_write8(padapter, REG_LEDCFG2,
+				 (LedCfg&0x80)|BIT(3)|BIT(5));
 		break;
 	default:
 		break;
diff --git a/drivers/staging/rtl8723au/hal/rtl8723au_recv.c b/drivers/staging/rtl8723au/hal/rtl8723au_recv.c
index 213d1936109..6075b6dc1be 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723au_recv.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723au_recv.c
@@ -19,16 +19,10 @@
 #include <mlme_osdep.h>
 #include <linux/ip.h>
 #include <linux/if_ether.h>
-#include <ethernet.h>
 #include <usb_ops.h>
 #include <wifi.h>
 #include <rtl8723a_hal.h>
 
-void rtl8723au_init_recvbuf(struct rtw_adapter *padapter,
-			    struct recv_buf *precvbuf)
-{
-}
-
 int rtl8723au_init_recv_priv(struct rtw_adapter *padapter)
 {
 	struct recv_priv *precvpriv = &padapter->recvpriv;
@@ -49,9 +43,6 @@ int rtl8723au_init_recv_priv(struct rtw_adapter *padapter)
 	if (!precvpriv->int_in_buf)
 		DBG_8723A("alloc_mem for interrupt in endpoint fail !!!!\n");
 
-	/* init recv_buf */
-	_rtw_init_queue23a(&precvpriv->free_recv_buf_queue);
-
 	size = NR_RECVBUFF * sizeof(struct recv_buf);
 	precvpriv->precv_buf = kzalloc(size, GFP_KERNEL);
 	if (!precvpriv->precv_buf) {
@@ -66,8 +57,8 @@ int rtl8723au_init_recv_priv(struct rtw_adapter *padapter)
 	for (i = 0; i < NR_RECVBUFF; i++) {
 		INIT_LIST_HEAD(&precvbuf->list);
 
-		res = rtw_os_recvbuf_resource_alloc23a(padapter, precvbuf);
-		if (res == _FAIL)
+		precvbuf->purb = usb_alloc_urb(0, GFP_KERNEL);
+		if (!precvbuf->purb)
 			break;
 
 		precvbuf->adapter = padapter;
@@ -75,8 +66,6 @@ int rtl8723au_init_recv_priv(struct rtw_adapter *padapter)
 		precvbuf++;
 	}
 
-	precvpriv->free_recv_buf_queue_cnt = NR_RECVBUFF;
-
 	skb_queue_head_init(&precvpriv->rx_skb_queue);
 	skb_queue_head_init(&precvpriv->free_recv_skb_queue);
 
@@ -110,7 +99,11 @@ void rtl8723au_free_recv_priv(struct rtw_adapter *padapter)
 	precvbuf = (struct recv_buf *)precvpriv->precv_buf;
 
 	for (i = 0; i < NR_RECVBUFF; i++) {
-		rtw_os_recvbuf_resource_free23a(padapter, precvbuf);
+		usb_free_urb(precvbuf->purb);
+
+		if (precvbuf->pskb)
+			dev_kfree_skb_any(precvbuf->pskb);
+
 		precvbuf++;
 	}
 
@@ -132,11 +125,20 @@ void rtl8723au_free_recv_priv(struct rtw_adapter *padapter)
 	skb_queue_purge(&precvpriv->free_recv_skb_queue);
 }
 
+struct recv_stat_cpu {
+	u32 rxdw0;
+	u32 rxdw1;
+	u32 rxdw2;
+	u32 rxdw3;
+	u32 rxdw4;
+	u32 rxdw5;
+};
+
 void update_recvframe_attrib(struct recv_frame *precvframe,
 			     struct recv_stat *prxstat)
 {
 	struct rx_pkt_attrib *pattrib;
-	struct recv_stat report;
+	struct recv_stat_cpu report;
 	struct rxreport_8723a *prxreport;
 
 	report.rxdw0 = le32_to_cpu(prxstat->rxdw0);
@@ -182,25 +184,43 @@ void update_recvframe_phyinfo(struct recv_frame *precvframe,
 	struct rtw_adapter *padapter = precvframe->adapter;
 	struct rx_pkt_attrib *pattrib = &precvframe->attrib;
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
-	struct odm_phy_info *pPHYInfo = (struct odm_phy_info *)(&pattrib->phy_info);
+	struct phy_info *pPHYInfo = &pattrib->phy_info;
 	struct odm_packet_info pkt_info;
 	u8 *sa = NULL, *da;
 	struct sta_priv *pstapriv;
 	struct sta_info *psta;
 	struct sk_buff *skb = precvframe->pkt;
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-	u8 *wlanhdr = skb->data;
+	bool matchbssid = false;
+	u8 *bssid;
+
+	matchbssid = (!ieee80211_is_ctl(hdr->frame_control) &&
+		      !pattrib->icv_err && !pattrib->crc_err);
+
+	if (matchbssid) {
+		switch (hdr->frame_control &
+			cpu_to_le16(IEEE80211_FCTL_TODS |
+				    IEEE80211_FCTL_FROMDS)) {
+		case cpu_to_le16(IEEE80211_FCTL_TODS):
+			bssid = hdr->addr1;
+			break;
+		case cpu_to_le16(IEEE80211_FCTL_FROMDS):
+			bssid = hdr->addr2;
+			break;
+		case cpu_to_le16(0):
+			bssid = hdr->addr3;
+			break;
+		default:
+			bssid = NULL;
+			matchbssid = false;
+		}
 
-	pkt_info.bPacketMatchBSSID = false;
-	pkt_info.bPacketToSelf = false;
-	pkt_info.bPacketBeacon = false;
+		if (bssid)
+			matchbssid = ether_addr_equal(
+				get_bssid(&padapter->mlmepriv), bssid);
+	}
 
-	pkt_info.bPacketMatchBSSID =
-		(!ieee80211_is_ctl(hdr->frame_control) &&
-		 !pattrib->icv_err &&
-		 !pattrib->crc_err &&
-		 !memcmp(get_hdr_bssid(wlanhdr),
-			 get_bssid(&padapter->mlmepriv), ETH_ALEN));
+	pkt_info.bPacketMatchBSSID = matchbssid;
 
 	da = ieee80211_get_DA(hdr);
 	pkt_info.bPacketToSelf = pkt_info.bPacketMatchBSSID &&
@@ -222,12 +242,12 @@ void update_recvframe_phyinfo(struct recv_frame *precvframe,
 	psta = rtw_get_stainfo23a(pstapriv, sa);
 	if (psta) {
 		pkt_info.StationID = psta->mac_id;
-		/* printk("%s ==> StationID(%d)\n", __FUNCTION__, pkt_info.StationID); */
+		/* printk("%s ==> StationID(%d)\n", __func__, pkt_info.StationID); */
 	}
 	pkt_info.Rate = pattrib->mcs_rate;
 
 	ODM_PhyStatusQuery23a(&pHalData->odmpriv, pPHYInfo,
-			   (u8 *)pphy_status, &pkt_info);
+			      (u8 *)pphy_status, &pkt_info);
 	precvframe->psta = NULL;
 	if (pkt_info.bPacketMatchBSSID &&
 	    (check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE) == true)) {
diff --git a/drivers/staging/rtl8723au/hal/rtl8723au_xmit.c b/drivers/staging/rtl8723au/hal/rtl8723au_xmit.c
index 2af2e3ee1ab..a67850fe6e5 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723au_xmit.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723au_xmit.c
@@ -21,20 +21,6 @@
 /* include <rtl8192c_hal.h> */
 #include <rtl8723a_hal.h>
 
-s32	rtl8723au_init_xmit_priv(struct rtw_adapter *padapter)
-{
-	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
-
-	tasklet_init(&pxmitpriv->xmit_tasklet,
-	     (void(*)(unsigned long))rtl8723au_xmit_tasklet,
-	     (unsigned long)padapter);
-	return _SUCCESS;
-}
-
-void	rtl8723au_free_xmit_priv(struct rtw_adapter *padapter)
-{
-}
-
 static void do_queue_select(struct rtw_adapter	*padapter, struct pkt_attrib *pattrib)
 {
 	u8 qsel;
@@ -87,19 +73,18 @@ static void fill_txdesc_sectype(struct pkt_attrib *pattrib, struct tx_desc *ptxd
 	if ((pattrib->encrypt > 0) && !pattrib->bswenc) {
 		switch (pattrib->encrypt) {
 		/* SEC_TYPE */
-		case _WEP40_:
-		case _WEP104_:
+		case WLAN_CIPHER_SUITE_WEP40:
+		case WLAN_CIPHER_SUITE_WEP104:
 			ptxdesc->txdw1 |= cpu_to_le32((0x01<<22)&0x00c00000);
 			break;
-		case _TKIP_:
-		case _TKIP_WTMIC_:
+		case WLAN_CIPHER_SUITE_TKIP:
 			/* ptxdesc->txdw1 |= cpu_to_le32((0x02<<22)&0x00c00000); */
 			ptxdesc->txdw1 |= cpu_to_le32((0x01<<22)&0x00c00000);
 			break;
-		case _AES_:
+		case WLAN_CIPHER_SUITE_CCMP:
 			ptxdesc->txdw1 |= cpu_to_le32((0x03<<22)&0x00c00000);
 			break;
-		case _NO_PRIVACY_:
+		case 0:
 		default:
 			break;
 		}
@@ -309,10 +294,11 @@ static s32 update_txdesc(struct xmit_frame *pxmitframe, u8 *pmem, s32 sz, u8 bag
 	return pull;
 }
 
-static s32 rtw_dump_xframe(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe)
+static int rtw_dump_xframe(struct rtw_adapter *padapter,
+			   struct xmit_frame *pxmitframe)
 {
-	s32 ret = _SUCCESS;
-	s32 inner_ret = _SUCCESS;
+	int ret = _SUCCESS;
+	int inner_ret = _SUCCESS;
 	int t, sz, w_sz, pull = 0;
 	u8 *mem_addr;
 	u32 ff_hwaddr;
@@ -358,7 +344,8 @@ static s32 rtw_dump_xframe(struct rtw_adapter *padapter, struct xmit_frame *pxmi
 		}
 
 		ff_hwaddr = rtw_get_ff_hwaddr23a(pxmitframe);
-		inner_ret = rtw_write_port(padapter, ff_hwaddr, w_sz, pxmitbuf);
+		inner_ret = rtl8723au_write_port(padapter, ff_hwaddr,
+						 w_sz, pxmitbuf);
 		rtw_count_tx_stats23a(padapter, pxmitframe, sz);
 
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
@@ -377,8 +364,9 @@ static s32 rtw_dump_xframe(struct rtw_adapter *padapter, struct xmit_frame *pxmi
 	return ret;
 }
 
-s32 rtl8723au_xmitframe_complete(struct rtw_adapter *padapter,
-				 struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
+bool rtl8723au_xmitframe_complete(struct rtw_adapter *padapter,
+				  struct xmit_priv *pxmitpriv,
+				  struct xmit_buf *pxmitbuf)
 {
 	struct hw_xmit *phwxmits;
 	struct xmit_frame *pxmitframe;
@@ -427,9 +415,10 @@ s32 rtl8723au_xmitframe_complete(struct rtw_adapter *padapter,
 	return true;
 }
 
-static s32 xmitframe_direct(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe)
+static int xmitframe_direct(struct rtw_adapter *padapter,
+			    struct xmit_frame *pxmitframe)
 {
-	s32 res = _SUCCESS;
+	int res;
 
 	res = rtw_xmitframe_coalesce23a(padapter, pxmitframe->pkt, pxmitframe);
 	if (res == _SUCCESS)
@@ -442,9 +431,10 @@ static s32 xmitframe_direct(struct rtw_adapter *padapter, struct xmit_frame *pxm
  *	true	dump packet directly
  *	false	enqueue packet
  */
-static s32 pre_xmitframe(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe)
+bool rtl8723au_hal_xmit(struct rtw_adapter *padapter,
+			struct xmit_frame *pxmitframe)
 {
-	s32 res;
+	int res;
 	struct xmit_buf *pxmitbuf = NULL;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct pkt_attrib *pattrib = &pxmitframe->attrib;
@@ -513,26 +503,17 @@ enqueue:
 	return false;
 }
 
-s32 rtl8723au_mgnt_xmit(struct rtw_adapter *padapter, struct xmit_frame *pmgntframe)
+int rtl8723au_mgnt_xmit(struct rtw_adapter *padapter,
+			struct xmit_frame *pmgntframe)
 {
 	return rtw_dump_xframe(padapter, pmgntframe);
 }
 
-/*
- * Return
- *	true	dump packet directly ok
- *	false	temporary can't transmit packets to hardware
- */
-s32 rtl8723au_hal_xmit(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe)
-{
-	return pre_xmitframe(padapter, pxmitframe);
-}
-
-s32	rtl8723au_hal_xmitframe_enqueue(struct rtw_adapter *padapter,
-					struct xmit_frame *pxmitframe)
+int rtl8723au_hal_xmitframe_enqueue(struct rtw_adapter *padapter,
+				    struct xmit_frame *pxmitframe)
 {
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	s32 err;
+	int err;
 
 	err = rtw_xmitframe_enqueue23a(padapter, pxmitframe);
 	if (err != _SUCCESS) {
diff --git a/drivers/staging/rtl8723au/hal/usb_halinit.c b/drivers/staging/rtl8723au/hal/usb_halinit.c
index e206829d50f..6a7fb28e03d 100644
--- a/drivers/staging/rtl8723au/hal/usb_halinit.c
+++ b/drivers/staging/rtl8723au/hal/usb_halinit.c
@@ -25,8 +25,6 @@
 #include <linux/ieee80211.h>
 
 #include <usb_ops.h>
-#include <usb_hal.h>
-#include <usb_osintf.h>
 
 static void
 _ConfigChipOutEP(struct rtw_adapter *pAdapter, u8 NumOutPipe)
@@ -38,7 +36,7 @@ _ConfigChipOutEP(struct rtw_adapter *pAdapter, u8 NumOutPipe)
 	pHalData->OutEpNumber = 0;
 
 	/*  Normal and High queue */
-	value8 = rtw_read8(pAdapter, (REG_NORMAL_SIE_EP + 1));
+	value8 = rtl8723au_read8(pAdapter, (REG_NORMAL_SIE_EP + 1));
 
 	if (value8 & USB_NORMAL_SIE_EP_MASK) {
 		pHalData->OutEpQueueSel |= TX_SELE_HQ;
@@ -51,7 +49,7 @@ _ConfigChipOutEP(struct rtw_adapter *pAdapter, u8 NumOutPipe)
 	}
 
 	/*  Low queue */
-	value8 = rtw_read8(pAdapter, (REG_NORMAL_SIE_EP + 2));
+	value8 = rtl8723au_read8(pAdapter, (REG_NORMAL_SIE_EP + 2));
 	if (value8 & USB_NORMAL_SIE_EP_MASK) {
 		pHalData->OutEpQueueSel |= TX_SELE_LQ;
 		pHalData->OutEpNumber++;
@@ -82,19 +80,11 @@ static bool rtl8723au_set_queue_pipe_mapping(struct rtw_adapter *pAdapter,
 	return result;
 }
 
-static void rtl8723au_interface_configure(struct rtw_adapter *padapter)
+void rtl8723au_chip_configure(struct rtw_adapter *padapter)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
 	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter);
 
-	if (pdvobjpriv->ishighspeed == true) {
-		/* 512 bytes */
-		pHalData->UsbBulkOutSize = USB_HIGH_SPEED_BULK_SIZE;
-	} else {
-		/* 64 bytes */
-		pHalData->UsbBulkOutSize = USB_FULL_SPEED_BULK_SIZE;
-	}
-
 	pHalData->interfaceIndex = pdvobjpriv->InterfaceNumber;
 
 	rtl8723au_set_queue_pipe_mapping(padapter,
@@ -102,15 +92,15 @@ static void rtl8723au_interface_configure(struct rtw_adapter *padapter)
 					 pdvobjpriv->RtNumOutPipes);
 }
 
-static u8 _InitPowerOn(struct rtw_adapter *padapter)
+static int _InitPowerOn(struct rtw_adapter *padapter)
 {
-	u8 status = _SUCCESS;
+	int status = _SUCCESS;
 	u16 value16 = 0;
 	u8 value8 = 0;
 
 	/*  RSV_CTRL 0x1C[7:0] = 0x00
 	    unlock ISO/CLK/Power control register */
-	rtw_write8(padapter, REG_RSV_CTRL, 0x0);
+	rtl8723au_write8(padapter, REG_RSV_CTRL, 0x0);
 
 	/*  HW Power on sequence */
 	if (!HalPwrSeqCmdParsing23a(padapter, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
@@ -118,20 +108,20 @@ static u8 _InitPowerOn(struct rtw_adapter *padapter)
 		return _FAIL;
 
 	/*  0x04[19] = 1, suggest by Jackie 2011.05.09, reset 8051 */
-	value8 = rtw_read8(padapter, REG_APS_FSMCO+2);
-	rtw_write8(padapter, REG_APS_FSMCO + 2, (value8 | BIT3));
+	value8 = rtl8723au_read8(padapter, REG_APS_FSMCO+2);
+	rtl8723au_write8(padapter, REG_APS_FSMCO + 2, value8 | BIT(3));
 
 	/*  Enable MAC DMA/WMAC/SCHEDULE/SEC block */
 	/*  Set CR bit10 to enable 32k calibration. Suggested by SD1 Gimmy.
 	    Added by tynli. 2011.08.31. */
-	value16 = rtw_read16(padapter, REG_CR);
+	value16 = rtl8723au_read16(padapter, REG_CR);
 	value16 |= (HCI_TXDMA_EN | HCI_RXDMA_EN | TXDMA_EN | RXDMA_EN |
 		    PROTOCOL_EN | SCHEDULE_EN | MACTXEN | MACRXEN |
 		    ENSEC | CALTMR_EN);
-	rtw_write16(padapter, REG_CR, value16);
+	rtl8723au_write16(padapter, REG_CR, value16);
 
 	/* for Efuse PG, suggest by Jackie 2011.11.23 */
-	PHY_SetBBReg(padapter, REG_EFUSE_CTRL, BIT28|BIT29|BIT30, 0x06);
+	PHY_SetBBReg(padapter, REG_EFUSE_CTRL, BIT(28)|BIT(29)|BIT(30), 0x06);
 
 	return status;
 }
@@ -143,10 +133,10 @@ static void _InitInterrupt(struct rtw_adapter *Adapter)
 
 	/*  HISR - turn all on */
 	value32 = 0xFFFFFFFF;
-	rtw_write32(Adapter, REG_HISR, value32);
+	rtl8723au_write32(Adapter, REG_HISR, value32);
 
 	/*  HIMR - turn all on */
-	rtw_write32(Adapter, REG_HIMR, value32);
+	rtl8723au_write32(Adapter, REG_HIMR, value32);
 }
 
 static void _InitQueueReservedPage(struct rtw_adapter *Adapter)
@@ -160,37 +150,33 @@ static void _InitQueueReservedPage(struct rtw_adapter *Adapter)
 	u32 value32;
 	u8 value8;
 	bool bWiFiConfig = pregistrypriv->wifi_spec;
-	/* u32			txQPageNum, txQPageUnit, txQRemainPage; */
 
-	{ /* for WMM */
-		/* RT_ASSERT((outEPNum>= 2), ("for WMM , number of out-ep "
-		   "must more than or equal to 2!\n")); */
+	/* RT_ASSERT((outEPNum>= 2), ("for WMM , number of out-ep "
+	   "must more than or equal to 2!\n")); */
 
-		numPubQ = bWiFiConfig ?
-			WMM_NORMAL_PAGE_NUM_PUBQ : NORMAL_PAGE_NUM_PUBQ;
+	numPubQ = bWiFiConfig ? WMM_NORMAL_PAGE_NUM_PUBQ : NORMAL_PAGE_NUM_PUBQ;
 
-		if (pHalData->OutEpQueueSel & TX_SELE_HQ) {
-			numHQ = bWiFiConfig ?
-				WMM_NORMAL_PAGE_NUM_HPQ : NORMAL_PAGE_NUM_HPQ;
-		}
+	if (pHalData->OutEpQueueSel & TX_SELE_HQ) {
+		numHQ = bWiFiConfig ?
+			WMM_NORMAL_PAGE_NUM_HPQ : NORMAL_PAGE_NUM_HPQ;
+	}
 
-		if (pHalData->OutEpQueueSel & TX_SELE_LQ) {
-			numLQ = bWiFiConfig ?
-				WMM_NORMAL_PAGE_NUM_LPQ : NORMAL_PAGE_NUM_LPQ;
-		}
-		/*  NOTE: This step shall be proceed before
-		    writting REG_RQPN. */
-		if (pHalData->OutEpQueueSel & TX_SELE_NQ) {
-			numNQ = bWiFiConfig ?
-				WMM_NORMAL_PAGE_NUM_NPQ : NORMAL_PAGE_NUM_NPQ;
-		}
-		value8 = (u8)_NPQ(numNQ);
-		rtw_write8(Adapter, REG_RQPN_NPQ, value8);
+	if (pHalData->OutEpQueueSel & TX_SELE_LQ) {
+		numLQ = bWiFiConfig ?
+			WMM_NORMAL_PAGE_NUM_LPQ : NORMAL_PAGE_NUM_LPQ;
 	}
+	/*  NOTE: This step shall be proceed before
+	    writting REG_RQPN. */
+	if (pHalData->OutEpQueueSel & TX_SELE_NQ) {
+		numNQ = bWiFiConfig ?
+			WMM_NORMAL_PAGE_NUM_NPQ : NORMAL_PAGE_NUM_NPQ;
+	}
+	value8 = (u8)_NPQ(numNQ);
+	rtl8723au_write8(Adapter, REG_RQPN_NPQ, value8);
 
 	/*  TX DMA */
 	value32 = _HPQ(numHQ) | _LPQ(numLQ) | _PUBQ(numPubQ) | LD_RQPN;
-	rtw_write32(Adapter, REG_RQPN, value32);
+	rtl8723au_write32(Adapter, REG_RQPN, value32);
 }
 
 static void _InitTxBufferBoundary(struct rtw_adapter *Adapter)
@@ -204,11 +190,11 @@ static void _InitTxBufferBoundary(struct rtw_adapter *Adapter)
 	else /* for WMM */
 		txpktbuf_bndy = WMM_NORMAL_TX_PAGE_BOUNDARY;
 
-	rtw_write8(Adapter, REG_TXPKTBUF_BCNQ_BDNY, txpktbuf_bndy);
-	rtw_write8(Adapter, REG_TXPKTBUF_MGQ_BDNY, txpktbuf_bndy);
-	rtw_write8(Adapter, REG_TXPKTBUF_WMAC_LBK_BF_HD, txpktbuf_bndy);
-	rtw_write8(Adapter, REG_TRXFF_BNDY, txpktbuf_bndy);
-	rtw_write8(Adapter, REG_TDECTRL+1, txpktbuf_bndy);
+	rtl8723au_write8(Adapter, REG_TXPKTBUF_BCNQ_BDNY, txpktbuf_bndy);
+	rtl8723au_write8(Adapter, REG_TXPKTBUF_MGQ_BDNY, txpktbuf_bndy);
+	rtl8723au_write8(Adapter, REG_TXPKTBUF_WMAC_LBK_BF_HD, txpktbuf_bndy);
+	rtl8723au_write8(Adapter, REG_TRXFF_BNDY, txpktbuf_bndy);
+	rtl8723au_write8(Adapter, REG_TDECTRL+1, txpktbuf_bndy);
 }
 
 static void _InitPageBoundary(struct rtw_adapter *Adapter)
@@ -217,7 +203,7 @@ static void _InitPageBoundary(struct rtw_adapter *Adapter)
 	/* srand(static_cast<unsigned int>(time(NULL))); */
 	u16 rxff_bndy = 0x27FF;/* rand() % 1) ? 0x27FF : 0x23FF; */
 
-	rtw_write16(Adapter, (REG_TRXFF_BNDY + 2), rxff_bndy);
+	rtl8723au_write16(Adapter, (REG_TRXFF_BNDY + 2), rxff_bndy);
 
 	/*  TODO: ?? shall we set tx boundary? */
 }
@@ -226,13 +212,13 @@ static void
 _InitNormalChipRegPriority(struct rtw_adapter *Adapter, u16 beQ, u16 bkQ,
 			   u16 viQ, u16 voQ, u16 mgtQ, u16 hiQ)
 {
-	u16 value16 = rtw_read16(Adapter, REG_TRXDMA_CTRL) & 0x7;
+	u16 value16 = rtl8723au_read16(Adapter, REG_TRXDMA_CTRL) & 0x7;
 
 	value16 |= _TXDMA_BEQ_MAP(beQ) | _TXDMA_BKQ_MAP(bkQ) |
 		_TXDMA_VIQ_MAP(viQ) | _TXDMA_VOQ_MAP(voQ) |
 		_TXDMA_MGQ_MAP(mgtQ) | _TXDMA_HIQ_MAP(hiQ);
 
-	rtw_write16(Adapter, REG_TRXDMA_CTRL, value16);
+	rtl8723au_write16(Adapter, REG_TRXDMA_CTRL, value16);
 }
 
 static void _InitNormalChipOneOutEpPriority(struct rtw_adapter *Adapter)
@@ -356,11 +342,11 @@ static void _InitNetworkType(struct rtw_adapter *Adapter)
 {
 	u32 value32;
 
-	value32 = rtw_read32(Adapter, REG_CR);
+	value32 = rtl8723au_read32(Adapter, REG_CR);
 
 	/*  TODO: use the other function to set network type */
 	value32 = (value32 & ~MASK_NETTYPE) | _NETTYPE(NT_LINK_AP);
-	rtw_write32(Adapter, REG_CR, value32);
+	rtl8723au_write32(Adapter, REG_CR, value32);
 }
 
 static void _InitTransferPageSize(struct rtw_adapter *Adapter)
@@ -369,12 +355,12 @@ static void _InitTransferPageSize(struct rtw_adapter *Adapter)
 
 	u8 value8;
 	value8 = _PSRX(PBP_128) | _PSTX(PBP_128);
-	rtw_write8(Adapter, REG_PBP, value8);
+	rtl8723au_write8(Adapter, REG_PBP, value8);
 }
 
 static void _InitDriverInfoSize(struct rtw_adapter *Adapter, u8 drvInfoSize)
 {
-	rtw_write8(Adapter, REG_RX_DRVINFO_SZ, drvInfoSize);
+	rtl8723au_write8(Adapter, REG_RX_DRVINFO_SZ, drvInfoSize);
 }
 
 static void _InitWMACSetting(struct rtw_adapter *Adapter)
@@ -389,15 +375,15 @@ static void _InitWMACSetting(struct rtw_adapter *Adapter)
 
 	/*  some REG_RCR will be modified later by
 	    phy_ConfigMACWithHeaderFile() */
-	rtw_write32(Adapter, REG_RCR, pHalData->ReceiveConfig);
+	rtl8723au_write32(Adapter, REG_RCR, pHalData->ReceiveConfig);
 
 	/*  Accept all multicast address */
-	rtw_write32(Adapter, REG_MAR, 0xFFFFFFFF);
-	rtw_write32(Adapter, REG_MAR + 4, 0xFFFFFFFF);
+	rtl8723au_write32(Adapter, REG_MAR, 0xFFFFFFFF);
+	rtl8723au_write32(Adapter, REG_MAR + 4, 0xFFFFFFFF);
 
 	/*  Accept all data frames */
 	/* value16 = 0xFFFF; */
-	/* rtw_write16(Adapter, REG_RXFLTMAP2, value16); */
+	/* rtl8723au_write16(Adapter, REG_RXFLTMAP2, value16); */
 
 	/*  2010.09.08 hpfan */
 	/*  Since ADF is removed from RCR, ps-poll will not be indicate
@@ -405,14 +391,14 @@ static void _InitWMACSetting(struct rtw_adapter *Adapter)
 	/*  RxFilterMap should mask ps-poll to gurantee AP mode can
 	    rx ps-poll. */
 	/* value16 = 0x400; */
-	/* rtw_write16(Adapter, REG_RXFLTMAP1, value16); */
+	/* rtl8723au_write16(Adapter, REG_RXFLTMAP1, value16); */
 
 	/*  Accept all management frames */
 	/* value16 = 0xFFFF; */
-	/* rtw_write16(Adapter, REG_RXFLTMAP0, value16); */
+	/* rtl8723au_write16(Adapter, REG_RXFLTMAP0, value16); */
 
 	/* enable RX_SHIFT bits */
-	/* rtw_write8(Adapter, REG_TRXDMA_CTRL, rtw_read8(Adapter,
+	/* rtl8723au_write8(Adapter, REG_TRXDMA_CTRL, rtl8723au_read8(Adapter,
 	   REG_TRXDMA_CTRL)|BIT(1)); */
 }
 
@@ -422,49 +408,49 @@ static void _InitAdaptiveCtrl(struct rtw_adapter *Adapter)
 	u32 value32;
 
 	/*  Response Rate Set */
-	value32 = rtw_read32(Adapter, REG_RRSR);
+	value32 = rtl8723au_read32(Adapter, REG_RRSR);
 	value32 &= ~RATE_BITMAP_ALL;
 	value32 |= RATE_RRSR_CCK_ONLY_1M;
-	rtw_write32(Adapter, REG_RRSR, value32);
+	rtl8723au_write32(Adapter, REG_RRSR, value32);
 
 	/*  CF-END Threshold */
-	/* m_spIoBase->rtw_write8(REG_CFEND_TH, 0x1); */
+	/* m_spIoBase->rtl8723au_write8(REG_CFEND_TH, 0x1); */
 
 	/*  SIFS (used in NAV) */
 	value16 = _SPEC_SIFS_CCK(0x10) | _SPEC_SIFS_OFDM(0x10);
-	rtw_write16(Adapter, REG_SPEC_SIFS, value16);
+	rtl8723au_write16(Adapter, REG_SPEC_SIFS, value16);
 
 	/*  Retry Limit */
 	value16 = _LRL(0x30) | _SRL(0x30);
-	rtw_write16(Adapter, REG_RL, value16);
+	rtl8723au_write16(Adapter, REG_RL, value16);
 }
 
 static void _InitRateFallback(struct rtw_adapter *Adapter)
 {
 	/*  Set Data Auto Rate Fallback Retry Count register. */
-	rtw_write32(Adapter, REG_DARFRC, 0x00000000);
-	rtw_write32(Adapter, REG_DARFRC+4, 0x10080404);
-	rtw_write32(Adapter, REG_RARFRC, 0x04030201);
-	rtw_write32(Adapter, REG_RARFRC+4, 0x08070605);
+	rtl8723au_write32(Adapter, REG_DARFRC, 0x00000000);
+	rtl8723au_write32(Adapter, REG_DARFRC+4, 0x10080404);
+	rtl8723au_write32(Adapter, REG_RARFRC, 0x04030201);
+	rtl8723au_write32(Adapter, REG_RARFRC+4, 0x08070605);
 }
 
 static void _InitEDCA(struct rtw_adapter *Adapter)
 {
 	/*  Set Spec SIFS (used in NAV) */
-	rtw_write16(Adapter, REG_SPEC_SIFS, 0x100a);
-	rtw_write16(Adapter, REG_MAC_SPEC_SIFS, 0x100a);
+	rtl8723au_write16(Adapter, REG_SPEC_SIFS, 0x100a);
+	rtl8723au_write16(Adapter, REG_MAC_SPEC_SIFS, 0x100a);
 
 	/*  Set SIFS for CCK */
-	rtw_write16(Adapter, REG_SIFS_CTX, 0x100a);
+	rtl8723au_write16(Adapter, REG_SIFS_CTX, 0x100a);
 
 	/*  Set SIFS for OFDM */
-	rtw_write16(Adapter, REG_SIFS_TRX, 0x100a);
+	rtl8723au_write16(Adapter, REG_SIFS_TRX, 0x100a);
 
 	/*  TXOP */
-	rtw_write32(Adapter, REG_EDCA_BE_PARAM, 0x005EA42B);
-	rtw_write32(Adapter, REG_EDCA_BK_PARAM, 0x0000A44F);
-	rtw_write32(Adapter, REG_EDCA_VI_PARAM, 0x005EA324);
-	rtw_write32(Adapter, REG_EDCA_VO_PARAM, 0x002FA226);
+	rtl8723au_write32(Adapter, REG_EDCA_BE_PARAM, 0x005EA42B);
+	rtl8723au_write32(Adapter, REG_EDCA_BK_PARAM, 0x0000A44F);
+	rtl8723au_write32(Adapter, REG_EDCA_VI_PARAM, 0x005EA324);
+	rtl8723au_write32(Adapter, REG_EDCA_VO_PARAM, 0x002FA226);
 }
 
 static void _InitHWLed(struct rtw_adapter *Adapter)
@@ -481,21 +467,21 @@ static void _InitHWLed(struct rtw_adapter *Adapter)
 
 static void _InitRDGSetting(struct rtw_adapter *Adapter)
 {
-	rtw_write8(Adapter, REG_RD_CTRL, 0xFF);
-	rtw_write16(Adapter, REG_RD_NAV_NXT, 0x200);
-	rtw_write8(Adapter, REG_RD_RESP_PKT_TH, 0x05);
+	rtl8723au_write8(Adapter, REG_RD_CTRL, 0xFF);
+	rtl8723au_write16(Adapter, REG_RD_NAV_NXT, 0x200);
+	rtl8723au_write8(Adapter, REG_RD_RESP_PKT_TH, 0x05);
 }
 
 static void _InitRetryFunction(struct rtw_adapter *Adapter)
 {
 	u8 value8;
 
-	value8 = rtw_read8(Adapter, REG_FWHW_TXQ_CTRL);
+	value8 = rtl8723au_read8(Adapter, REG_FWHW_TXQ_CTRL);
 	value8 |= EN_AMPDU_RTY_NEW;
-	rtw_write8(Adapter, REG_FWHW_TXQ_CTRL, value8);
+	rtl8723au_write8(Adapter, REG_FWHW_TXQ_CTRL, value8);
 
 	/*  Set ACK timeout */
-	rtw_write8(Adapter, REG_ACKTO, 0x40);
+	rtl8723au_write8(Adapter, REG_ACKTO, 0x40);
 }
 
 /*-----------------------------------------------------------------------------
@@ -599,35 +585,33 @@ enum rt_rf_power_state RfOnOffDetect23a(struct rtw_adapter *pAdapter)
 	enum rt_rf_power_state rfpowerstate = rf_off;
 
 	if (pAdapter->pwrctrlpriv.bHWPowerdown) {
-		val8 = rtw_read8(pAdapter, REG_HSISR);
+		val8 = rtl8723au_read8(pAdapter, REG_HSISR);
 		DBG_8723A("pwrdown, 0x5c(BIT7) =%02x\n", val8);
-		rfpowerstate = (val8 & BIT7) ? rf_off : rf_on;
+		rfpowerstate = (val8 & BIT(7)) ? rf_off : rf_on;
 	} else { /*  rf on/off */
-		rtw_write8(pAdapter, REG_MAC_PINMUX_CFG,
-			   rtw_read8(pAdapter, REG_MAC_PINMUX_CFG) & ~BIT3);
-		val8 = rtw_read8(pAdapter, REG_GPIO_IO_SEL);
+		rtl8723au_write8(pAdapter, REG_MAC_PINMUX_CFG,
+				 rtl8723au_read8(pAdapter, REG_MAC_PINMUX_CFG) &
+				 ~BIT(3));
+		val8 = rtl8723au_read8(pAdapter, REG_GPIO_IO_SEL);
 		DBG_8723A("GPIO_IN =%02x\n", val8);
-		rfpowerstate = (val8 & BIT3) ? rf_on : rf_off;
+		rfpowerstate = (val8 & BIT(3)) ? rf_on : rf_off;
 	}
 	return rfpowerstate;
 }	/*  HalDetectPwrDownMode */
 
 void _ps_open_RF23a(struct rtw_adapter *padapter);
 
-static u32 rtl8723au_hal_init(struct rtw_adapter *Adapter)
+static int rtl8723au_hal_init(struct rtw_adapter *Adapter)
 {
-	u8	val8 = 0;
-	u32	boundary, status = _SUCCESS;
+	u8 val8 = 0;
+	u32 boundary;
+	int status = _SUCCESS;
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
 	struct pwrctrl_priv *pwrctrlpriv = &Adapter->pwrctrlpriv;
 	struct registry_priv *pregistrypriv = &Adapter->registrypriv;
-	u32 NavUpper = WiFiNavUpperUs;
 
 	unsigned long init_start_time = jiffies;
 
-#define HAL_INIT_PROFILE_TAG(stage) do {} while (0)
-
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_BEGIN);
 	if (Adapter->pwrctrlpriv.bkeepfwalive) {
 		_ps_open_RF23a(Adapter);
 
@@ -644,7 +628,7 @@ static u32 rtl8723au_hal_init(struct rtw_adapter *Adapter)
 	}
 
 	/*  Check if MAC has already power on. by tynli. 2011.05.27. */
-	val8 = rtw_read8(Adapter, REG_CR);
+	val8 = rtl8723au_read8(Adapter, REG_CR);
 	RT_TRACE(_module_hci_hal_init_c_, _drv_info_,
 		 ("%s: REG_CR 0x100 = 0x%02x\n", __func__, val8));
 	/* Fix 92DU-VC S3 hang with the reason is that secondary mac is not
@@ -659,7 +643,6 @@ static u32 rtl8723au_hal_init(struct rtw_adapter *Adapter)
 			 ("%s: MAC has already power on\n", __func__));
 	}
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_INIT_PW_ON);
 	status = _InitPowerOn(Adapter);
 	if (status == _FAIL) {
 		RT_TRACE(_module_hci_hal_init_c_, _drv_err_,
@@ -667,7 +650,6 @@ static u32 rtl8723au_hal_init(struct rtw_adapter *Adapter)
 		goto exit;
 	}
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_INIT_LLTT);
 	if (!pregistrypriv->wifi_spec) {
 		boundary = TX_PAGE_BOUNDARY;
 	} else {
@@ -684,11 +666,9 @@ static u32 rtl8723au_hal_init(struct rtw_adapter *Adapter)
 		}
 	}
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_MISC01);
 	if (pHalData->bRDGEnable)
 		_InitRDGSetting(Adapter);
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_DOWNLOAD_FW);
 	status = rtl8723a_FirmwareDownload(Adapter);
 	if (status != _SUCCESS) {
 		Adapter->bFWReady = false;
@@ -720,14 +700,12 @@ static u32 rtl8723au_hal_init(struct rtw_adapter *Adapter)
 	/*  <Roger_Notes> Current Channel will be updated again later. */
 	pHalData->CurrentChannel = 6;/* default set to 6 */
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_MAC);
 	status = PHY_MACConfig8723A(Adapter);
 	if (status == _FAIL) {
 		DBG_8723A("PHY_MACConfig8723A fault !!\n");
 		goto exit;
 	}
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_BB);
 	/*  */
 	/* d. Initialize BB related configurations. */
 	/*  */
@@ -740,7 +718,6 @@ static u32 rtl8723au_hal_init(struct rtw_adapter *Adapter)
 	/*  Add for tx power by rate fine tune. We need to call the function after BB config. */
 	/*  Because the tx power by rate table is inited in BB config. */
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_RF);
 	status = PHY_RFConfig8723A(Adapter);
 	if (status == _FAIL) {
 		DBG_8723A("PHY_RFConfig8723A fault !!\n");
@@ -766,7 +743,6 @@ static u32 rtl8723au_hal_init(struct rtw_adapter *Adapter)
 	pHalData->RfRegChnlVal[0] = PHY_QueryRFReg(Adapter, (enum RF_RADIO_PATH)0, RF_CHNLBW, bRFRegOffsetMask);
 	pHalData->RfRegChnlVal[1] = PHY_QueryRFReg(Adapter, (enum RF_RADIO_PATH)1, RF_CHNLBW, bRFRegOffsetMask);
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_MISC02);
 	if (!pHalData->bMACFuncEnable) {
 		_InitQueueReservedPage(Adapter);
 		_InitTxBufferBoundary(Adapter);
@@ -779,7 +755,7 @@ static u32 rtl8723au_hal_init(struct rtw_adapter *Adapter)
 	_InitDriverInfoSize(Adapter, DRVINFO_SZ);
 
 	_InitInterrupt(Adapter);
-	hal_init_macaddr23a(Adapter);/* set mac_address */
+	hw_var_set_macaddr(Adapter, Adapter->eeprompriv.mac_addr);
 	_InitNetworkType(Adapter);/* set msr */
 	_InitWMACSetting(Adapter);
 	_InitAdaptiveCtrl(Adapter);
@@ -792,14 +768,11 @@ static u32 rtl8723au_hal_init(struct rtw_adapter *Adapter)
 
 	_InitHWLed(Adapter);
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_TURN_ON_BLOCK);
 	_BBTurnOnBlock(Adapter);
 	/* NicIFSetMacAddress(padapter, padapter->PermanentAddress); */
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_INIT_SECURITY);
 	invalidate_cam_all23a(Adapter);
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_MISC11);
 	/*  2010/12/17 MH We need to set TX power according to EFUSE content at first. */
 	PHY_SetTxPowerLevel8723A(Adapter, pHalData->CurrentChannel);
 
@@ -807,101 +780,89 @@ static u32 rtl8723au_hal_init(struct rtw_adapter *Adapter)
 
 	/*  HW SEQ CTRL */
 	/* set 0x0 to 0xFF by tynli. Default enable HW SEQ NUM. */
-	rtw_write8(Adapter, REG_HWSEQ_CTRL, 0xFF);
+	rtl8723au_write8(Adapter, REG_HWSEQ_CTRL, 0xFF);
 
 	/*  */
 	/*  Disable BAR, suggested by Scott */
 	/*  2010.04.09 add by hpfan */
 	/*  */
-	rtw_write32(Adapter, REG_BAR_MODE_CTRL, 0x0201ffff);
+	rtl8723au_write32(Adapter, REG_BAR_MODE_CTRL, 0x0201ffff);
 
 	if (pregistrypriv->wifi_spec)
-		rtw_write16(Adapter, REG_FAST_EDCA_CTRL, 0);
+		rtl8723au_write16(Adapter, REG_FAST_EDCA_CTRL, 0);
 
 	/*  Move by Neo for USB SS from above setp */
 	_RfPowerSave(Adapter);
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_IQK);
-		/*  2010/08/26 MH Merge from 8192CE. */
-		/* sherry masked that it has been done in _RfPowerSave */
-		/* 20110927 */
-		/* recovery for 8192cu and 9723Au 20111017 */
-		if (pwrctrlpriv->rf_pwrstate == rf_on) {
-			if (pHalData->bIQKInitialized) {
-				rtl8723a_phy_iq_calibrate(Adapter, true);
-			} else {
-				rtl8723a_phy_iq_calibrate(Adapter, false);
-				pHalData->bIQKInitialized = true;
-			}
+	/*  2010/08/26 MH Merge from 8192CE. */
+	/* sherry masked that it has been done in _RfPowerSave */
+	/* 20110927 */
+	/* recovery for 8192cu and 9723Au 20111017 */
+	if (pwrctrlpriv->rf_pwrstate == rf_on) {
+		if (pHalData->bIQKInitialized) {
+			rtl8723a_phy_iq_calibrate(Adapter, true);
+		} else {
+			rtl8723a_phy_iq_calibrate(Adapter, false);
+			pHalData->bIQKInitialized = true;
+		}
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_PW_TRACK);
-			rtl8723a_odm_check_tx_power_tracking(Adapter);
+		rtl8723a_odm_check_tx_power_tracking(Adapter);
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_LCK);
-			rtl8723a_phy_lc_calibrate(Adapter);
+		rtl8723a_phy_lc_calibrate(Adapter);
 
-#ifdef CONFIG_8723AU_BT_COEXIST
-			rtl8723a_SingleDualAntennaDetection(Adapter);
-#endif
-		}
+		rtl8723a_dual_antenna_detection(Adapter);
+	}
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_MISC21);
- /* fixed USB interface interference issue */
-	rtw_write8(Adapter, 0xfe40, 0xe0);
-	rtw_write8(Adapter, 0xfe41, 0x8d);
-	rtw_write8(Adapter, 0xfe42, 0x80);
-	rtw_write32(Adapter, 0x20c, 0xfd0320);
+	/* fixed USB interface interference issue */
+	rtl8723au_write8(Adapter, 0xfe40, 0xe0);
+	rtl8723au_write8(Adapter, 0xfe41, 0x8d);
+	rtl8723au_write8(Adapter, 0xfe42, 0x80);
+	rtl8723au_write32(Adapter, 0x20c, 0xfd0320);
 	/* Solve too many protocol error on USB bus */
 	if (!IS_81xxC_VENDOR_UMC_A_CUT(pHalData->VersionID)) {
 		/*  0xE6 = 0x94 */
-		rtw_write8(Adapter, 0xFE40, 0xE6);
-		rtw_write8(Adapter, 0xFE41, 0x94);
-		rtw_write8(Adapter, 0xFE42, 0x80);
+		rtl8723au_write8(Adapter, 0xFE40, 0xE6);
+		rtl8723au_write8(Adapter, 0xFE41, 0x94);
+		rtl8723au_write8(Adapter, 0xFE42, 0x80);
 
 		/*  0xE0 = 0x19 */
-		rtw_write8(Adapter, 0xFE40, 0xE0);
-		rtw_write8(Adapter, 0xFE41, 0x19);
-		rtw_write8(Adapter, 0xFE42, 0x80);
+		rtl8723au_write8(Adapter, 0xFE40, 0xE0);
+		rtl8723au_write8(Adapter, 0xFE41, 0x19);
+		rtl8723au_write8(Adapter, 0xFE42, 0x80);
 
 		/*  0xE5 = 0x91 */
-		rtw_write8(Adapter, 0xFE40, 0xE5);
-		rtw_write8(Adapter, 0xFE41, 0x91);
-		rtw_write8(Adapter, 0xFE42, 0x80);
+		rtl8723au_write8(Adapter, 0xFE40, 0xE5);
+		rtl8723au_write8(Adapter, 0xFE41, 0x91);
+		rtl8723au_write8(Adapter, 0xFE42, 0x80);
 
 		/*  0xE2 = 0x81 */
-		rtw_write8(Adapter, 0xFE40, 0xE2);
-		rtw_write8(Adapter, 0xFE41, 0x81);
-		rtw_write8(Adapter, 0xFE42, 0x80);
+		rtl8723au_write8(Adapter, 0xFE40, 0xE2);
+		rtl8723au_write8(Adapter, 0xFE41, 0x81);
+		rtl8723au_write8(Adapter, 0xFE42, 0x80);
 
 	}
 
-/* HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_INIT_PABIAS); */
 /*	_InitPABias(Adapter); */
 
-#ifdef CONFIG_8723AU_BT_COEXIST
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_BT_COEXIST);
 	/*  Init BT hw config. */
-	BT_InitHwConfig(Adapter);
-#endif
+	rtl8723a_BT_init_hwconfig(Adapter);
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_INIT_HAL_DM);
 	rtl8723a_InitHalDm(Adapter);
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_MISC31);
-	rtw_hal_set_hwreg23a(Adapter, HW_VAR_NAV_UPPER, (u8 *)&NavUpper);
+	rtl8723a_set_nav_upper(Adapter, WiFiNavUpperUs);
 
 	/*  2011/03/09 MH debug only, UMC-B cut pass 2500 S5 test, but we need to fin root cause. */
-	if (((rtw_read32(Adapter, rFPGA0_RFMOD) & 0xFF000000) != 0x83000000)) {
+	if (((rtl8723au_read32(Adapter, rFPGA0_RFMOD) & 0xFF000000) !=
+	     0x83000000)) {
 		PHY_SetBBReg(Adapter, rFPGA0_RFMOD, BIT(24), 1);
 		RT_TRACE(_module_hci_hal_init_c_, _drv_err_, ("%s: IQK fail recorver\n", __func__));
 	}
 
 	/* ack for xmit mgmt frames. */
-	rtw_write32(Adapter, REG_FWHW_TXQ_CTRL, rtw_read32(Adapter, REG_FWHW_TXQ_CTRL)|BIT(12));
+	rtl8723au_write32(Adapter, REG_FWHW_TXQ_CTRL,
+			  rtl8723au_read32(Adapter, REG_FWHW_TXQ_CTRL)|BIT(12));
 
 exit:
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_END);
-
 	DBG_8723A("%s in %dms\n", __func__,
 		  jiffies_to_msecs(jiffies - init_start_time));
 	return status;
@@ -923,9 +884,9 @@ static void phy_SsPwrSwitch92CU(struct rtw_adapter *Adapter,
 			   ReadXBYTE(REG_SYS_CLKR+1) | BIT(3)); */
 
 			/*  2. Force PWM, Enable SPS18_LDO_Marco_Block */
-			rtw_write8(Adapter, REG_SPS0_CTRL,
-				   rtw_read8(Adapter, REG_SPS0_CTRL) |
-				   (BIT0|BIT3));
+			rtl8723au_write8(Adapter, REG_SPS0_CTRL,
+					 rtl8723au_read8(Adapter, REG_SPS0_CTRL) |
+					 BIT(0) | BIT(3));
 
 			/*  3. restore BB, AFE control register. */
 			/* RF */
@@ -936,7 +897,7 @@ static void phy_SsPwrSwitch92CU(struct rtw_adapter *Adapter,
 				PHY_SetBBReg(Adapter, rFPGA0_XAB_RFParameter,
 					     0x38, 1);
 			PHY_SetBBReg(Adapter, rOFDM0_TRxPathEnable, 0xf0, 1);
-			PHY_SetBBReg(Adapter, rFPGA0_RFMOD, BIT1, 0);
+			PHY_SetBBReg(Adapter, rFPGA0_RFMOD, BIT(1), 0);
 
 			/* AFE */
 			if (pHalData->rf_type ==  RF_2T2R)
@@ -959,17 +920,17 @@ static void phy_SsPwrSwitch92CU(struct rtw_adapter *Adapter,
 		} else {		/*  Level 2 or others. */
 			/* h.	AFE_PLL_CTRL 0x28[7:0] = 0x80
 			   disable AFE PLL */
-			rtw_write8(Adapter, REG_AFE_PLL_CTRL, 0x81);
+			rtl8723au_write8(Adapter, REG_AFE_PLL_CTRL, 0x81);
 
 			/*  i.	AFE_XTAL_CTRL 0x24[15:0] = 0x880F
 			    gated AFE DIG_CLOCK */
-			rtw_write16(Adapter, REG_AFE_XTAL_CTRL, 0x800F);
+			rtl8723au_write16(Adapter, REG_AFE_XTAL_CTRL, 0x800F);
 			mdelay(1);
 
 			/*  2. Force PWM, Enable SPS18_LDO_Marco_Block */
-			rtw_write8(Adapter, REG_SPS0_CTRL,
-				   rtw_read8(Adapter, REG_SPS0_CTRL) |
-				   (BIT0|BIT3));
+			rtl8723au_write8(Adapter, REG_SPS0_CTRL,
+					 rtl8723au_read8(Adapter, REG_SPS0_CTRL) |
+					 BIT(0) | BIT(3));
 
 			/*  3. restore BB, AFE control register. */
 			/* RF */
@@ -980,7 +941,7 @@ static void phy_SsPwrSwitch92CU(struct rtw_adapter *Adapter,
 				PHY_SetBBReg(Adapter, rFPGA0_XAB_RFParameter,
 					     0x38, 1);
 			PHY_SetBBReg(Adapter, rOFDM0_TRxPathEnable, 0xf0, 1);
-			PHY_SetBBReg(Adapter, rFPGA0_RFMOD, BIT1, 0);
+			PHY_SetBBReg(Adapter, rFPGA0_RFMOD, BIT(1), 0);
 
 			/* AFE */
 			if (pHalData->rf_type ==  RF_2T2R)
@@ -1002,25 +963,27 @@ static void phy_SsPwrSwitch92CU(struct rtw_adapter *Adapter,
 			}
 
 			/*  5. gated MAC Clock */
-			bytetmp = rtw_read8(Adapter, REG_APSD_CTRL);
-			rtw_write8(Adapter, REG_APSD_CTRL, bytetmp & ~BIT6);
+			bytetmp = rtl8723au_read8(Adapter, REG_APSD_CTRL);
+			rtl8723au_write8(Adapter, REG_APSD_CTRL,
+					 bytetmp & ~BIT(6));
 
 			mdelay(10);
 
 			/*  Set BB reset at first */
-			rtw_write8(Adapter, REG_SYS_FUNC_EN, 0x17); /* 0x16 */
+			/* 0x16 */
+			rtl8723au_write8(Adapter, REG_SYS_FUNC_EN, 0x17);
 
 			/*  Enable TX */
-			rtw_write8(Adapter, REG_TXPAUSE, 0x0);
+			rtl8723au_write8(Adapter, REG_TXPAUSE, 0x0);
 		}
 		break;
 	case rf_sleep:
 	case rf_off:
-		value8 = rtw_read8(Adapter, REG_SPS0_CTRL) ;
+		value8 = rtl8723au_read8(Adapter, REG_SPS0_CTRL) ;
 		if (IS_81xxC_VENDOR_UMC_B_CUT(pHalData->VersionID))
-			value8 &= ~(BIT0);
+			value8 &= ~BIT(0);
 		else
-			value8 &= ~(BIT0|BIT3);
+			value8 &= ~(BIT(0) | BIT(3));
 		if (bRegSSPwrLvl == 1) {
 			RT_TRACE(_module_hal_init_c_, _drv_err_, ("SS LVL1\n"));
 			/*  Disable RF and BB only for SelectSuspend. */
@@ -1050,7 +1013,7 @@ static void phy_SsPwrSwitch92CU(struct rtw_adapter *Adapter,
 					     0x38, 0);
 			}
 			PHY_SetBBReg(Adapter, rOFDM0_TRxPathEnable, 0xf0, 0);
-			PHY_SetBBReg(Adapter, rFPGA0_RFMOD, BIT1, 1);
+			PHY_SetBBReg(Adapter, rFPGA0_RFMOD, BIT(1), 1);
 
 			/*  2 .AFE control register to power down. bit[30:22] */
 			Adapter->pwrctrlpriv.PS_BBRegBackup[PSBBREG_AFE0] =
@@ -1070,18 +1033,19 @@ static void phy_SsPwrSwitch92CU(struct rtw_adapter *Adapter,
 					     bRFRegOffsetMask, 0);
 
 			/*  4. Force PFM , disable SPS18_LDO_Marco_Block */
-			rtw_write8(Adapter, REG_SPS0_CTRL, value8);
+			rtl8723au_write8(Adapter, REG_SPS0_CTRL, value8);
 		} else {	/*  Level 2 or others. */
 			RT_TRACE(_module_hal_init_c_, _drv_err_, ("SS LVL2\n"));
 			{
 				u8 eRFPath = RF_PATH_A, value8 = 0;
-				rtw_write8(Adapter, REG_TXPAUSE, 0xFF);
+				rtl8723au_write8(Adapter, REG_TXPAUSE, 0xFF);
 				PHY_SetRFReg(Adapter,
 					     (enum RF_RADIO_PATH)eRFPath,
 					     0x0, bMaskByte0, 0x0);
 				value8 |= APSDOFF;
 				/* 0x40 */
-				rtw_write8(Adapter, REG_APSD_CTRL, value8);
+				rtl8723au_write8(Adapter, REG_APSD_CTRL,
+						 value8);
 
 				/*  After switch APSD, we need to delay
 				    for stability */
@@ -1092,7 +1056,8 @@ static void phy_SsPwrSwitch92CU(struct rtw_adapter *Adapter,
 				value8 |= (FEN_USBD | FEN_USBA |
 					   FEN_BB_GLB_RSTn);
 				/* 0x16 */
-				rtw_write8(Adapter, REG_SYS_FUNC_EN, value8);
+				rtl8723au_write8(Adapter, REG_SYS_FUNC_EN,
+						 value8);
 			}
 
 			/*  Disable RF and BB only for SelectSuspend. */
@@ -1121,7 +1086,7 @@ static void phy_SsPwrSwitch92CU(struct rtw_adapter *Adapter,
 				PHY_SetBBReg(Adapter, rFPGA0_XAB_RFParameter,
 					     0x38, 0);
 			PHY_SetBBReg(Adapter, rOFDM0_TRxPathEnable, 0xf0, 0);
-			PHY_SetBBReg(Adapter, rFPGA0_RFMOD, BIT1, 1);
+			PHY_SetBBReg(Adapter, rFPGA0_RFMOD, BIT(1), 1);
 
 			/*  2 .AFE control register to power down. bit[30:22] */
 			Adapter->pwrctrlpriv.PS_BBRegBackup[PSBBREG_AFE0] =
@@ -1141,17 +1106,17 @@ static void phy_SsPwrSwitch92CU(struct rtw_adapter *Adapter,
 					     bRFRegOffsetMask, 0);
 
 			/*  4. Force PFM , disable SPS18_LDO_Marco_Block */
-			rtw_write8(Adapter, REG_SPS0_CTRL, value8);
+			rtl8723au_write8(Adapter, REG_SPS0_CTRL, value8);
 
 			/*  2010/10/13 MH/Isaachsu exchange sequence. */
 			/* h.	AFE_PLL_CTRL 0x28[7:0] = 0x80
 				disable AFE PLL */
-			rtw_write8(Adapter, REG_AFE_PLL_CTRL, 0x80);
+			rtl8723au_write8(Adapter, REG_AFE_PLL_CTRL, 0x80);
 			mdelay(1);
 
 			/*  i.	AFE_XTAL_CTRL 0x24[15:0] = 0x880F
 				gated AFE DIG_CLOCK */
-			rtw_write16(Adapter, REG_AFE_XTAL_CTRL, 0xA80F);
+			rtl8723au_write16(Adapter, REG_AFE_XTAL_CTRL, 0xA80F);
 		}
 		break;
 	default:
@@ -1177,19 +1142,19 @@ static void CardDisableRTL8723U(struct rtw_adapter *Adapter)
 			    PWR_INTF_USB_MSK, rtl8723AU_enter_lps_flow);
 
 	/*  2. 0x1F[7:0] = 0		turn off RF */
-	rtw_write8(Adapter, REG_RF_CTRL, 0x00);
+	rtl8723au_write8(Adapter, REG_RF_CTRL, 0x00);
 
 	/*	==== Reset digital sequence   ====== */
-	if ((rtw_read8(Adapter, REG_MCUFWDL)&BIT7) &&
+	if ((rtl8723au_read8(Adapter, REG_MCUFWDL) & BIT(7)) &&
 	    Adapter->bFWReady) /* 8051 RAM code */
 		rtl8723a_FirmwareSelfReset(Adapter);
 
 	/*  Reset MCU. Suggested by Filen. 2011.01.26. by tynli. */
-	u1bTmp = rtw_read8(Adapter, REG_SYS_FUNC_EN+1);
-	rtw_write8(Adapter, REG_SYS_FUNC_EN+1, (u1bTmp & (~BIT2)));
+	u1bTmp = rtl8723au_read8(Adapter, REG_SYS_FUNC_EN+1);
+	rtl8723au_write8(Adapter, REG_SYS_FUNC_EN+1, u1bTmp & ~BIT(2));
 
 	/*  g.	MCUFWDL 0x80[1:0]= 0		reset MCU ready status */
-	rtw_write8(Adapter, REG_MCUFWDL, 0x00);
+	rtl8723au_write8(Adapter, REG_MCUFWDL, 0x00);
 
 	/*	==== Reset digital sequence end ====== */
 	/*  Card disable power action flow */
@@ -1198,16 +1163,16 @@ static void CardDisableRTL8723U(struct rtw_adapter *Adapter)
 			       rtl8723AU_card_disable_flow);
 
 	/*  Reset MCU IO Wrapper, added by Roger, 2011.08.30. */
-	u1bTmp = rtw_read8(Adapter, REG_RSV_CTRL + 1);
-	rtw_write8(Adapter, REG_RSV_CTRL+1, (u1bTmp & (~BIT0)));
-	u1bTmp = rtw_read8(Adapter, REG_RSV_CTRL + 1);
-	rtw_write8(Adapter, REG_RSV_CTRL+1, u1bTmp | BIT0);
+	u1bTmp = rtl8723au_read8(Adapter, REG_RSV_CTRL + 1);
+	rtl8723au_write8(Adapter, REG_RSV_CTRL+1, u1bTmp & ~BIT(0));
+	u1bTmp = rtl8723au_read8(Adapter, REG_RSV_CTRL + 1);
+	rtl8723au_write8(Adapter, REG_RSV_CTRL+1, u1bTmp | BIT(0));
 
 	/*  7. RSV_CTRL 0x1C[7:0] = 0x0E  lock ISO/CLK/Power control register */
-	rtw_write8(Adapter, REG_RSV_CTRL, 0x0e);
+	rtl8723au_write8(Adapter, REG_RSV_CTRL, 0x0e);
 }
 
-static u32 rtl8723au_hal_deinit(struct rtw_adapter *padapter)
+static int rtl8723au_hal_deinit(struct rtw_adapter *padapter)
 {
 	DBG_8723A("==> %s\n", __func__);
 
@@ -1223,67 +1188,57 @@ static u32 rtl8723au_hal_deinit(struct rtw_adapter *padapter)
 	return _SUCCESS;
 }
 
-static unsigned int rtl8723au_inirp_init(struct rtw_adapter *Adapter)
+int rtl8723au_inirp_init(struct rtw_adapter *Adapter)
 {
 	u8 i;
 	struct recv_buf *precvbuf;
-	uint	status;
-	struct intf_hdl *pintfhdl = &Adapter->iopriv.intf;
+	int status;
 	struct recv_priv *precvpriv = &Adapter->recvpriv;
-	u32 (*_read_port)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt,
-			  struct recv_buf *rbuf);
-	u32 (*_read_interrupt)(struct intf_hdl *pintfhdl, u32 addr);
-	struct hal_data_8723a	*pHalData = GET_HAL_DATA(Adapter);
-
-	_read_port = pintfhdl->io_ops._read_port;
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
 
 	status = _SUCCESS;
 
 	RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("===> usb_inirp_init\n"));
 
-	precvpriv->ff_hwaddr = RECV_BULK_IN_ADDR;
-
 	/* issue Rx irp to receive data */
 	precvbuf = (struct recv_buf *)precvpriv->precv_buf;
 	for (i = 0; i < NR_RECVBUFF; i++) {
-		if (_read_port(pintfhdl, precvpriv->ff_hwaddr, 0, precvbuf) ==
-		    false) {
+		if (rtl8723au_read_port(Adapter, RECV_BULK_IN_ADDR, 0,
+					precvbuf) == _FAIL) {
 			RT_TRACE(_module_hci_hal_init_c_, _drv_err_,
 				 ("usb_rx_init: usb_read_port error\n"));
 			status = _FAIL;
 			goto exit;
 		}
 		precvbuf++;
-		precvpriv->free_recv_buf_queue_cnt--;
 	}
-	_read_interrupt = pintfhdl->io_ops._read_interrupt;
-	if (_read_interrupt(pintfhdl, RECV_INT_IN_ADDR) == false) {
+	if (rtl8723au_read_interrupt(Adapter, RECV_INT_IN_ADDR) == _FAIL) {
 		RT_TRACE(_module_hci_hal_init_c_, _drv_err_,
 			 ("usb_rx_init: usb_read_interrupt error\n"));
 		status = _FAIL;
 	}
-	pHalData->IntrMask[0] = rtw_read32(Adapter, REG_USB_HIMR);
+	pHalData->IntrMask[0] = rtl8723au_read32(Adapter, REG_USB_HIMR);
 	MSG_8723A("pHalData->IntrMask = 0x%04x\n", pHalData->IntrMask[0]);
 	pHalData->IntrMask[0] |= UHIMR_C2HCMD|UHIMR_CPWM;
-	rtw_write32(Adapter, REG_USB_HIMR, pHalData->IntrMask[0]);
+	rtl8723au_write32(Adapter, REG_USB_HIMR, pHalData->IntrMask[0]);
 exit:
 	RT_TRACE(_module_hci_hal_init_c_, _drv_info_,
 		 ("<=== usb_inirp_init\n"));
 	return status;
 }
 
-static unsigned int rtl8723au_inirp_deinit(struct rtw_adapter *Adapter)
+int rtl8723au_inirp_deinit(struct rtw_adapter *Adapter)
 {
 	struct hal_data_8723a	*pHalData = GET_HAL_DATA(Adapter);
 
 	RT_TRACE(_module_hci_hal_init_c_, _drv_info_,
 		 ("\n ===> usb_rx_deinit\n"));
-	rtw_read_port_cancel(Adapter);
-	pHalData->IntrMask[0] = rtw_read32(Adapter, REG_USB_HIMR);
+	rtl8723au_read_port_cancel(Adapter);
+	pHalData->IntrMask[0] = rtl8723au_read32(Adapter, REG_USB_HIMR);
 	MSG_8723A("%s pHalData->IntrMask = 0x%04x\n", __func__,
 		  pHalData->IntrMask[0]);
 	pHalData->IntrMask[0] = 0x0;
-	rtw_write32(Adapter, REG_USB_HIMR, pHalData->IntrMask[0]);
+	rtl8723au_write32(Adapter, REG_USB_HIMR, pHalData->IntrMask[0]);
 	RT_TRACE(_module_hci_hal_init_c_, _drv_info_,
 		 ("\n <=== usb_rx_deinit\n"));
 	return _SUCCESS;
@@ -1420,7 +1375,7 @@ static void _ReadPROMContent(struct rtw_adapter *Adapter)
 	struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(Adapter);
 	u8 eeValue;
 
-	eeValue = rtw_read8(Adapter, REG_9346CR);
+	eeValue = rtl8723au_read8(Adapter, REG_9346CR);
 	/*  To check system boot selection. */
 	pEEPROM->EepromOrEfuse = (eeValue & BOOT_FROM_EEPROM) ? true : false;
 	pEEPROM->bautoload_fail_flag = (eeValue & EEPROM_EN) ? false : true;
@@ -1459,16 +1414,18 @@ static void hal_EfuseCellSel(struct rtw_adapter *Adapter)
 {
 	u32 value32;
 
-	value32 = rtw_read32(Adapter, EFUSE_TEST);
+	value32 = rtl8723au_read32(Adapter, EFUSE_TEST);
 	value32 = (value32 & ~EFUSE_SEL_MASK) | EFUSE_SEL(EFUSE_WIFI_SEL_0);
-	rtw_write32(Adapter, EFUSE_TEST, value32);
+	rtl8723au_write32(Adapter, EFUSE_TEST, value32);
 }
 
-static int _ReadAdapterInfo8723AU(struct rtw_adapter *Adapter)
+void rtl8723a_read_adapter_info(struct rtw_adapter *Adapter)
 {
-	/* struct hal_data_8723a	*pHalData = GET_HAL_DATA(Adapter); */
 	unsigned long start = jiffies;
 
+	/*  Read EEPROM size before call any EEPROM function */
+	Adapter->EepromAddressSize = GetEEPROMSize8723A(Adapter);
+
 	MSG_8723A("====> _ReadAdapterInfo8723AU\n");
 
 	hal_EfuseCellSel(Adapter);
@@ -1485,73 +1442,17 @@ static int _ReadAdapterInfo8723AU(struct rtw_adapter *Adapter)
 
 	MSG_8723A("<==== _ReadAdapterInfo8723AU in %d ms\n",
 		  jiffies_to_msecs(jiffies - start));
-
-	return _SUCCESS;
-}
-
-static void ReadAdapterInfo8723AU(struct rtw_adapter *Adapter)
-{
-	/*  Read EEPROM size before call any EEPROM function */
-	Adapter->EepromAddressSize = GetEEPROMSize8723A(Adapter);
-
-	_ReadAdapterInfo8723AU(Adapter);
-}
-
-#define GPIO_DEBUG_PORT_NUM 0
-static void rtl8723au_trigger_gpio_0(struct rtw_adapter *padapter)
-{
-	u32 gpioctrl;
-	DBG_8723A("==> trigger_gpio_0...\n");
-	rtw_write16_async(padapter, REG_GPIO_PIN_CTRL, 0);
-	rtw_write8_async(padapter, REG_GPIO_PIN_CTRL+2, 0xFF);
-	gpioctrl = (BIT(GPIO_DEBUG_PORT_NUM) << 24)|
-		(BIT(GPIO_DEBUG_PORT_NUM) << 16);
-	rtw_write32_async(padapter, REG_GPIO_PIN_CTRL, gpioctrl);
-	gpioctrl |= (BIT(GPIO_DEBUG_PORT_NUM)<<8);
-	rtw_write32_async(padapter, REG_GPIO_PIN_CTRL, gpioctrl);
-	DBG_8723A("<=== trigger_gpio_0...\n");
-}
-
-/*
- * If variable not handled here,
- * some variables will be processed in SetHwReg8723A()
- */
-static void SetHwReg8723AU(struct rtw_adapter *Adapter, u8 variable, u8 *val)
-{
-	switch (variable) {
-	case HW_VAR_RXDMA_AGG_PG_TH:
-		break;
-	case HW_VAR_SET_RPWM:
-		rtl8723a_set_rpwm(Adapter, *val);
-		break;
-	case HW_VAR_TRIGGER_GPIO_0:
-		rtl8723au_trigger_gpio_0(Adapter);
-		break;
-	default:
-		SetHwReg8723A(Adapter, variable, val);
-		break;
-	}
-
-}
-
-/*
- * If variable not handled here,
- * some variables will be processed in GetHwReg8723A()
- */
-static void GetHwReg8723AU(struct rtw_adapter *Adapter, u8 variable, u8 *val)
-{
-	GetHwReg8723A(Adapter, variable, val);
 }
 
 /*  */
 /*	Description: */
 /*		Query setting of specified variable. */
 /*  */
-static u8 GetHalDefVar8192CUsb(struct rtw_adapter *Adapter,
-			       enum hal_def_variable eVariable, void *pValue)
+int GetHalDefVar8192CUsb(struct rtw_adapter *Adapter,
+			 enum hal_def_variable eVariable, void *pValue)
 {
-	struct hal_data_8723a	*pHalData = GET_HAL_DATA(Adapter);
-	u8			bResult = _SUCCESS;
+	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
+	int bResult = _SUCCESS;
 
 	switch (eVariable) {
 	case HAL_DEF_UNDERCORATEDSMOOTHEDPWDB:
@@ -1596,79 +1497,8 @@ static u8 GetHalDefVar8192CUsb(struct rtw_adapter *Adapter,
 	return bResult;
 }
 
-/*	Change default setting of specified variable. */
-static u8 SetHalDefVar8192CUsb(struct rtw_adapter *Adapter,
-			       enum hal_def_variable eVariable, void *pValue)
-{
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
-	u8 bResult = _SUCCESS;
-
-	switch (eVariable) {
-	case HAL_DEF_DBG_DUMP_RXPKT:
-		pHalData->bDumpRxPkt = *((u8 *)pValue);
-		break;
-	case HAL_DEF_DBG_DM_FUNC:
-	{
-		u8 dm_func = *((u8 *)pValue);
-		struct dm_priv	*pdmpriv = &pHalData->dmpriv;
-		struct dm_odm_t *podmpriv = &pHalData->odmpriv;
-
-		if (dm_func == 0) { /* disable all dynamic func */
-			podmpriv->SupportAbility = DYNAMIC_FUNC_DISABLE;
-			DBG_8723A("==> Disable all dynamic function...\n");
-		} else if (dm_func == 1) {/* disable DIG */
-			podmpriv->SupportAbility &= (~DYNAMIC_BB_DIG);
-			DBG_8723A("==> Disable DIG...\n");
-		} else if (dm_func == 2) {/* disable High power */
-			podmpriv->SupportAbility &= (~DYNAMIC_BB_DYNAMIC_TXPWR);
-		} else if (dm_func == 3) {/* disable tx power tracking */
-			podmpriv->SupportAbility &= (~DYNAMIC_RF_CALIBRATION);
-			DBG_8723A("==> Disable tx power tracking...\n");
-		} else if (dm_func == 4) {/* disable BT coexistence */
-			pdmpriv->DMFlag &= (~DYNAMIC_FUNC_BT);
-		} else if (dm_func == 5) {/* disable antenna diversity */
-			podmpriv->SupportAbility &= (~DYNAMIC_BB_ANT_DIV);
-		} else if (dm_func == 6) {/* turn on all dynamic func */
-			if (!(podmpriv->SupportAbility & DYNAMIC_BB_DIG)) {
-				struct dig_t *pDigTable =
-					&podmpriv->DM_DigTable;
-				pDigTable->CurIGValue = rtw_read8(Adapter, 0xc50);
-			}
-			pdmpriv->DMFlag |= DYNAMIC_FUNC_BT;
-			podmpriv->SupportAbility = DYNAMIC_ALL_FUNC_ENABLE;
-			DBG_8723A("==> Turn on all dynamic function...\n");
-		}
-	}
-		break;
-	case HW_DEF_FA_CNT_DUMP:
-	{
-		u8 bRSSIDump = *((u8 *)pValue);
-		struct dm_odm_t *pDM_Odm = &pHalData->odmpriv;
-		if (bRSSIDump)
-			pDM_Odm->DebugComponents = ODM_COMP_DIG|ODM_COMP_FA_CNT;
-		else
-			pDM_Odm->DebugComponents = 0;
-	}
-		break;
-	case HW_DEF_ODM_DBG_FLAG:
-	{
-		u64 DebugComponents = *((u64 *)pValue);
-		struct dm_odm_t *pDM_Odm = &pHalData->odmpriv;
-		pDM_Odm->DebugComponents = DebugComponents;
-	}
-		break;
-	default:
-		/* RT_TRACE(COMP_INIT, DBG_TRACE, ("SetHalDefVar819xUsb(): "
-		   "Unkown variable: %d!\n", eVariable)); */
-		bResult = _FAIL;
-		break;
-	}
-
-	return bResult;
-}
-
-static void UpdateHalRAMask8192CUsb(struct rtw_adapter *padapter,
-				    u32 mac_id, u8 rssi_level)
+void rtl8723a_update_ramask(struct rtw_adapter *padapter,
+			    u32 mac_id, u8 rssi_level)
 {
 	u8	init_rate = 0;
 	u8	networkType, raid;
@@ -1741,10 +1571,9 @@ static void UpdateHalRAMask8192CUsb(struct rtw_adapter *padapter,
 	rate_bitmap = 0x0fffffff;
 	rate_bitmap = ODM_Get_Rate_Bitmap23a(&pHalData->odmpriv,
 					  mac_id, mask, rssi_level);
-	printk(KERN_DEBUG "%s => mac_id:%d, networkType:0x%02x, "
-	       "mask:0x%08x\n\t ==> rssi_level:%d, rate_bitmap:0x%08x\n",
-	       __func__,
-	       mac_id, networkType, mask, rssi_level, rate_bitmap);
+	DBG_8723A("%s => mac_id:%d, networkType:0x%02x, "
+		  "mask:0x%08x\n\t ==> rssi_level:%d, rate_bitmap:0x%08x\n",
+		  __func__, mac_id, networkType, mask, rssi_level, rate_bitmap);
 
 	mask &= rate_bitmap;
 	mask |= ((raid<<28)&0xf0000000);
@@ -1770,7 +1599,8 @@ static void UpdateHalRAMask8192CUsb(struct rtw_adapter *padapter,
 		if (shortGIrate == true)
 			init_rate |= BIT(6);
 
-		rtw_write8(padapter, (REG_INIDATA_RATE_SEL+mac_id), init_rate);
+		rtl8723au_write8(padapter, (REG_INIDATA_RATE_SEL+mac_id),
+				 init_rate);
 	}
 
 	/* set ra_id */
@@ -1781,54 +1611,39 @@ static void UpdateHalRAMask8192CUsb(struct rtw_adapter *padapter,
 	pdmpriv->INIDATA_RATE[mac_id] = init_rate;
 }
 
-static void rtl8723au_init_default_value(struct rtw_adapter *padapter)
+int rtw_hal_init23a(struct rtw_adapter *padapter)
 {
-	rtl8723a_init_default_value(padapter);
-}
+	int status;
 
-static u8 rtl8192cu_ps_func(struct rtw_adapter *Adapter,
-			    enum hal_intf_ps_func efunc_id, u8 *val)
-{
-	return true;
+	padapter->hw_init_completed = false;
+
+	status = rtl8723au_hal_init(padapter);
+
+	if (status == _SUCCESS) {
+		padapter->hw_init_completed = true;
+
+		if (padapter->registrypriv.notch_filter == 1)
+			rtl8723a_notch_filter(padapter, 1);
+	} else {
+		padapter->hw_init_completed = false;
+		DBG_8723A("rtw_hal_init23a: hal__init fail\n");
+	}
+
+	RT_TRACE(_module_hal_init_c_, _drv_err_,
+		 ("-rtl871x_hal_init:status = 0x%x\n", status));
+
+	return status;
 }
 
-int rtl8723au_set_hal_ops(struct rtw_adapter *padapter)
+int rtw_hal_deinit23a(struct rtw_adapter *padapter)
 {
-	struct hal_ops	*pHalFunc = &padapter->HalFunc;
+	int status;
 
-	padapter->HalData = kzalloc(sizeof(struct hal_data_8723a), GFP_KERNEL);
-	if (!padapter->HalData) {
-		DBG_8723A("cannot alloc memory for HAL DATA\n");
-		return -ENOMEM;
-	}
-	padapter->hal_data_sz = sizeof(struct hal_data_8723a);
-
-	pHalFunc->hal_init = &rtl8723au_hal_init;
-	pHalFunc->hal_deinit = &rtl8723au_hal_deinit;
-
-	pHalFunc->inirp_init = &rtl8723au_inirp_init;
-	pHalFunc->inirp_deinit = &rtl8723au_inirp_deinit;
-
-	pHalFunc->init_xmit_priv = &rtl8723au_init_xmit_priv;
-	pHalFunc->free_xmit_priv = &rtl8723au_free_xmit_priv;
-
-	pHalFunc->init_recv_priv = &rtl8723au_init_recv_priv;
-	pHalFunc->free_recv_priv = &rtl8723au_free_recv_priv;
-	pHalFunc->InitSwLeds = NULL;
-	pHalFunc->DeInitSwLeds = NULL;
-
-	pHalFunc->init_default_value = &rtl8723au_init_default_value;
-	pHalFunc->intf_chip_configure = &rtl8723au_interface_configure;
-	pHalFunc->read_adapter_info = &ReadAdapterInfo8723AU;
-	pHalFunc->SetHwRegHandler = &SetHwReg8723AU;
-	pHalFunc->GetHwRegHandler = &GetHwReg8723AU;
-	pHalFunc->GetHalDefVarHandler = &GetHalDefVar8192CUsb;
-	pHalFunc->SetHalDefVarHandler = &SetHalDefVar8192CUsb;
-	pHalFunc->UpdateRAMaskHandler = &UpdateHalRAMask8192CUsb;
-	pHalFunc->hal_xmit = &rtl8723au_hal_xmit;
-	pHalFunc->mgnt_xmit = &rtl8723au_mgnt_xmit;
-	pHalFunc->hal_xmitframe_enqueue = &rtl8723au_hal_xmitframe_enqueue;
-	pHalFunc->interface_ps_func = &rtl8192cu_ps_func;
-	rtl8723a_set_hal_ops(pHalFunc);
-	return 0;
+	status = rtl8723au_hal_deinit(padapter);
+
+	if (status == _SUCCESS)
+		padapter->hw_init_completed = false;
+	else
+		DBG_8723A("\n rtw_hal_deinit23a: hal_init fail\n");
+	return status;
 }
diff --git a/drivers/staging/rtl8723au/hal/usb_ops_linux.c b/drivers/staging/rtl8723au/hal/usb_ops_linux.c
index 0311cdf77ff..8e9e61c74bd 100644
--- a/drivers/staging/rtl8723au/hal/usb_ops_linux.c
+++ b/drivers/staging/rtl8723au/hal/usb_ops_linux.c
@@ -22,9 +22,10 @@
 #include <rtl8723a_hal.h>
 #include <rtl8723a_recv.h>
 
-static int usbctrl_vendorreq(struct intf_hdl *pintfhdl, u8 request, u16 value, u16 index, void *pdata, u16 len, u8 requesttype)
+static int usbctrl_vendorreq(struct rtw_adapter *padapter, u8 request,
+			     u16 value, u16 index, void *pdata, u16 len,
+			     u8 requesttype)
 {
-	struct rtw_adapter		*padapter = pintfhdl->padapter ;
 	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter);
 	struct usb_device *udev = pdvobjpriv->pusbdev;
 
@@ -34,15 +35,17 @@ static int usbctrl_vendorreq(struct intf_hdl *pintfhdl, u8 request, u16 value, u
 	u8 *pIo_buf;
 	int vendorreq_times = 0;
 
-	if ((padapter->bSurpriseRemoved) || (padapter->pwrctrlpriv.pnp_bstop_trx)) {
+	if (padapter->bSurpriseRemoved || padapter->pwrctrlpriv.pnp_bstop_trx) {
 		RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-			 ("usbctrl_vendorreq:(padapter->bSurpriseRemoved||adapter->pwrctrlpriv.pnp_bstop_trx)!!!\n"));
+			 ("usbctrl_vendorreq:(padapter->bSurpriseRemoved||"
+			  "adapter->pwrctrlpriv.pnp_bstop_trx)!!!\n"));
 		status = -EPERM;
 		goto exit;
 	}
 
 	if (len > MAX_VENDOR_REQ_CMD_SIZE) {
-		DBG_8723A("[%s] Buffer len error , vendor request failed\n", __FUNCTION__);
+		DBG_8723A("[%s] Buffer len error , vendor request failed\n",
+			  __func__);
 		status = -EINVAL;
 		goto exit;
 	}
@@ -53,7 +56,7 @@ static int usbctrl_vendorreq(struct intf_hdl *pintfhdl, u8 request, u16 value, u
 	pIo_buf = pdvobjpriv->usb_vendor_req_buf;
 
 	if (pIo_buf == NULL) {
-		DBG_8723A("[%s] pIo_buf == NULL \n", __FUNCTION__);
+		DBG_8723A("[%s] pIo_buf == NULL \n", __func__);
 		status = -ENOMEM;
 		goto release_mutex;
 	}
@@ -70,9 +73,9 @@ static int usbctrl_vendorreq(struct intf_hdl *pintfhdl, u8 request, u16 value, u
 			memcpy(pIo_buf, pdata, len);
 		}
 
-		status = rtw_usb_control_msg(udev, pipe, request, reqtype,
-					     value, index, pIo_buf, len,
-					     RTW_USB_CONTROL_MSG_TIMEOUT);
+		status = usb_control_msg(udev, pipe, request, reqtype,
+					 value, index, pIo_buf, len,
+					 RTW_USB_CONTROL_MSG_TIMEOUT);
 
 		if (status == len) {   /*  Success this control transfer. */
 			rtw_reset_continual_urb_error(pdvobjpriv);
@@ -85,21 +88,26 @@ static int usbctrl_vendorreq(struct intf_hdl *pintfhdl, u8 request, u16 value, u
 		} else { /*  error cases */
 			DBG_8723A("reg 0x%x, usb %s %u fail, status:%d value ="
 				  " 0x%x, vendorreq_times:%d\n",
-				  value, (requesttype == 0x01) ? "read" : "write",
+				  value, (requesttype == 0x01) ?
+				  "read" : "write",
 				  len, status, *(u32 *)pdata, vendorreq_times);
 
 			if (status < 0) {
-				if (status == (-ESHUTDOWN) || status == -ENODEV) {
+				if (status == -ESHUTDOWN || status == -ENODEV)
 					padapter->bSurpriseRemoved = true;
-				} else {
-					struct hal_data_8723a	*pHalData = GET_HAL_DATA(padapter);
-					pHalData->srestpriv.Wifi_Error_Status = USB_VEN_REQ_CMD_FAIL;
+				else {
+					struct hal_data_8723a *pHalData;
+					pHalData = GET_HAL_DATA(padapter);
+					pHalData->srestpriv.Wifi_Error_Status =
+						USB_VEN_REQ_CMD_FAIL;
 				}
 			} else { /*  status != len && status >= 0 */
 				if (status > 0) {
 					if (requesttype == 0x01) {
-						/*  For Control read transfer, we have to copy
-						 * the read data from pIo_buf to pdata.
+						/*
+						 * For Control read transfer,
+						 * we have to copy the read
+						 * data from pIo_buf to pdata.
 						 */
 						memcpy(pdata, pIo_buf,  len);
 					}
@@ -110,11 +118,11 @@ static int usbctrl_vendorreq(struct intf_hdl *pintfhdl, u8 request, u16 value, u
 				padapter->bSurpriseRemoved = true;
 				break;
 			}
-
 		}
 
 		/*  firmware download is checksumed, don't retry */
-		if ((value >= FW_8723A_START_ADDRESS && value <= FW_8723A_END_ADDRESS) || status == len)
+		if ((value >= FW_8723A_START_ADDRESS &&
+		     value <= FW_8723A_END_ADDRESS) || status == len)
 			break;
 	}
 
@@ -124,7 +132,7 @@ exit:
 	return status;
 }
 
-static u8 usb_read8(struct intf_hdl *pintfhdl, u32 addr)
+u8 rtl8723au_read8(struct rtw_adapter *padapter, u32 addr)
 {
 	u8 request;
 	u8 requesttype;
@@ -140,19 +148,20 @@ static u8 usb_read8(struct intf_hdl *pintfhdl, u32 addr)
 	wvalue = (u16)(addr&0x0000ffff);
 	len = 1;
 
-	usbctrl_vendorreq(pintfhdl, request, wvalue, index, &data, len, requesttype);
+	usbctrl_vendorreq(padapter, request, wvalue, index, &data,
+			  len, requesttype);
 
 	return data;
 }
 
-static u16 usb_read16(struct intf_hdl *pintfhdl, u32 addr)
+u16 rtl8723au_read16(struct rtw_adapter *padapter, u32 addr)
 {
 	u8 request;
 	u8 requesttype;
 	u16 wvalue;
 	u16 index;
 	u16 len;
-	u16 data = 0;
+	__le16 data;
 
 	request = 0x05;
 	requesttype = 0x01;/* read_in */
@@ -161,19 +170,20 @@ static u16 usb_read16(struct intf_hdl *pintfhdl, u32 addr)
 	wvalue = (u16)(addr&0x0000ffff);
 	len = 2;
 
-	usbctrl_vendorreq(pintfhdl, request, wvalue, index, &data, len, requesttype);
+	usbctrl_vendorreq(padapter, request, wvalue, index, &data,
+			  len, requesttype);
 
-	return data;
+	return le16_to_cpu(data);
 }
 
-static u32 usb_read32(struct intf_hdl *pintfhdl, u32 addr)
+u32 rtl8723au_read32(struct rtw_adapter *padapter, u32 addr)
 {
 	u8 request;
 	u8 requesttype;
 	u16 wvalue;
 	u16 index;
 	u16 len;
-	u32 data = 0;
+	__le32 data;
 
 	request = 0x05;
 	requesttype = 0x01;/* read_in */
@@ -182,12 +192,13 @@ static u32 usb_read32(struct intf_hdl *pintfhdl, u32 addr)
 	wvalue = (u16)(addr&0x0000ffff);
 	len = 4;
 
-	usbctrl_vendorreq(pintfhdl, request, wvalue, index, &data, len, requesttype);
+	usbctrl_vendorreq(padapter, request, wvalue, index, &data,
+			  len, requesttype);
 
-	return data;
+	return le32_to_cpu(data);
 }
 
-static int usb_write8(struct intf_hdl *pintfhdl, u32 addr, u8 val)
+int rtl8723au_write8(struct rtw_adapter *padapter, u32 addr, u8 val)
 {
 	u8 request;
 	u8 requesttype;
@@ -206,19 +217,20 @@ static int usb_write8(struct intf_hdl *pintfhdl, u32 addr, u8 val)
 
 	data = val;
 
-	ret = usbctrl_vendorreq(pintfhdl, request, wvalue, index, &data, len, requesttype);
+	ret = usbctrl_vendorreq(padapter, request, wvalue, index, &data,
+				len, requesttype);
 
 	return ret;
 }
 
-static int usb_write16(struct intf_hdl *pintfhdl, u32 addr, u16 val)
+int rtl8723au_write16(struct rtw_adapter *padapter, u32 addr, u16 val)
 {
 	u8 request;
 	u8 requesttype;
 	u16 wvalue;
 	u16 index;
 	u16 len;
-	u16 data;
+	__le16 data;
 	int ret;
 
 	request = 0x05;
@@ -228,20 +240,21 @@ static int usb_write16(struct intf_hdl *pintfhdl, u32 addr, u16 val)
 	wvalue = (u16)(addr&0x0000ffff);
 	len = 2;
 
-	data = val;
+	data = cpu_to_le16(val);
 
-	ret = usbctrl_vendorreq(pintfhdl, request, wvalue, index, &data, len, requesttype);
+	ret = usbctrl_vendorreq(padapter, request, wvalue, index, &data,
+				len, requesttype);
 	return ret;
 }
 
-static int usb_write32(struct intf_hdl *pintfhdl, u32 addr, u32 val)
+int rtl8723au_write32(struct rtw_adapter *padapter, u32 addr, u32 val)
 {
 	u8 request;
 	u8 requesttype;
 	u16 wvalue;
 	u16 index;
 	u16 len;
-	u32 data;
+	__le32 data;
 	int ret;
 
 	request = 0x05;
@@ -250,14 +263,16 @@ static int usb_write32(struct intf_hdl *pintfhdl, u32 addr, u32 val)
 
 	wvalue = (u16)(addr&0x0000ffff);
 	len = 4;
-	data = val;
+	data = cpu_to_le32(val);
 
-	ret = usbctrl_vendorreq(pintfhdl, request, wvalue, index, &data, len, requesttype);
+	ret = usbctrl_vendorreq(padapter, request, wvalue, index, &data,
+				len, requesttype);
 
 	return ret;
 }
 
-static int usb_writeN(struct intf_hdl *pintfhdl, u32 addr, u32 length, u8 *pdata)
+int rtl8723au_writeN(struct rtw_adapter *padapter,
+		     u32 addr, u32 length, u8 *pdata)
 {
 	u8 request;
 	u8 requesttype;
@@ -273,9 +288,10 @@ static int usb_writeN(struct intf_hdl *pintfhdl, u32 addr, u32 length, u8 *pdata
 
 	wvalue = (u16)(addr&0x0000ffff);
 	len = length;
-	 memcpy(buf, pdata, len);
+	memcpy(buf, pdata, len);
 
-	ret = usbctrl_vendorreq(pintfhdl, request, wvalue, index, buf, len, requesttype);
+	ret = usbctrl_vendorreq(padapter, request, wvalue, index, buf,
+				len, requesttype);
 
 	return ret;
 }
@@ -323,11 +339,11 @@ InterruptRecognized8723AU(struct rtw_adapter *Adapter, void *pContent,
 
 	memcpy(&report.state, &buffer[USB_INTR_CPWM_OFFSET], 1);
 
-	return ((pHalData->IntArray[0])&pHalData->IntrMask[0]) != 0 ||
-		((pHalData->IntArray[1])&pHalData->IntrMask[1]) != 0;
+	return (pHalData->IntArray[0] & pHalData->IntrMask[0]) != 0 ||
+		(pHalData->IntArray[1] & pHalData->IntrMask[1]) != 0;
 }
 
-static void usb_read_interrupt_complete(struct urb *purb, struct pt_regs *regs)
+static void usb_read_interrupt_complete(struct urb *purb)
 {
 	int err;
 	struct rtw_adapter *padapter = (struct rtw_adapter *)purb->context;
@@ -336,7 +352,7 @@ static void usb_read_interrupt_complete(struct urb *purb, struct pt_regs *regs)
 	    padapter->bReadPortCancel) {
 		DBG_8723A("%s() RX Warning! bDriverStopped(%d) OR "
 			  "bSurpriseRemoved(%d) bReadPortCancel(%d)\n",
-			  __FUNCTION__, padapter->bDriverStopped,
+			  __func__, padapter->bDriverStopped,
 			  padapter->bSurpriseRemoved,
 			  padapter->bReadPortCancel);
 		return;
@@ -359,30 +375,37 @@ static void usb_read_interrupt_complete(struct urb *purb, struct pt_regs *regs)
 		if (c2h_evt_exist(c2h_evt)) {
 			if (c2h_id_filter_ccx_8723a(c2h_evt->id)) {
 				/* Handle CCX report here */
-				handle_txrpt_ccx_8723a(padapter, (void *)(c2h_evt->payload));
-				/* Replace with special pointer to
-				   trigger c2h_evt_clear23a */
-				if (rtw_cbuf_push23a(padapter->evtpriv.c2h_queue,
-						  (void *)&padapter->evtpriv) !=
-				    _SUCCESS)
-					DBG_8723A("%s rtw_cbuf_push23a fail\n",
-						  __func__);
-				schedule_work(&padapter->evtpriv.c2h_wk);
-			} else if ((c2h_evt = (struct c2h_evt_hdr *)
-				    kmalloc(16, GFP_ATOMIC))) {
-				memcpy(c2h_evt, purb->transfer_buffer, 16);
-				if (rtw_cbuf_push23a(padapter->evtpriv.c2h_queue,
-						  (void *)c2h_evt) != _SUCCESS)
-					DBG_8723A("%s rtw_cbuf_push23a fail\n",
-						  __func__);
-				schedule_work(&padapter->evtpriv.c2h_wk);
+				handle_txrpt_ccx_8723a(padapter, (void *)
+						       c2h_evt->payload);
+				schedule_work(&padapter->evtpriv.irq_wk);
 			} else {
-				/* Error handling for malloc fail */
-				if (rtw_cbuf_push23a(padapter->evtpriv.c2h_queue,
-						  (void *)NULL) != _SUCCESS)
-					DBG_8723A("%s rtw_cbuf_push23a fail\n",
-						  __func__);
-				schedule_work(&padapter->evtpriv.c2h_wk);
+				struct evt_work *c2w;
+				int res;
+
+				c2w = kmalloc(sizeof(struct evt_work),
+						GFP_ATOMIC);
+
+				if (!c2w) {
+					printk(KERN_WARNING "%s: unable to "
+					       "allocate work buffer\n",
+					       __func__);
+					goto urb_submit;
+				}
+
+				c2w->adapter = padapter;
+				INIT_WORK(&c2w->work, rtw_evt_work);
+				memcpy(c2w->u.buf, purb->transfer_buffer, 16);
+
+				res = queue_work(padapter->evtpriv.wq,
+						 &c2w->work);
+
+				if (!res) {
+					printk(KERN_ERR "%s: Call to "
+					       "queue_work() failed\n",
+					       __func__);
+					kfree(c2w);
+					goto urb_submit;
+				}
 			}
 		}
 
@@ -415,7 +438,7 @@ urb_submit:
 		case -EPROTO:
 			break;
 		case -EINPROGRESS:
-			DBG_8723A("ERROR: URB IS IN PROGRESS!/n");
+			DBG_8723A("ERROR: URB IS IN PROGRESS!\n");
 			break;
 		default:
 			break;
@@ -423,18 +446,17 @@ urb_submit:
 	}
 }
 
-static u32 usb_read_interrupt(struct intf_hdl *pintfhdl, u32 addr)
+int rtl8723au_read_interrupt(struct rtw_adapter *adapter, u32 addr)
 {
 	int err;
 	unsigned int pipe;
-	u32 ret = _SUCCESS;
-	struct rtw_adapter *adapter = pintfhdl->padapter;
+	int ret = _SUCCESS;
 	struct dvobj_priv *pdvobj = adapter_to_dvobj(adapter);
 	struct recv_priv *precvpriv = &adapter->recvpriv;
 	struct usb_device *pusbd = pdvobj->pusbdev;
 
 	/* translate DMA FIFO addr to pipehandle */
-	pipe = ffaddr2pipehdl23a(pdvobj, addr);
+	pipe = usb_rcvintpipe(pusbd, pdvobj->RtInPipe[1]);
 
 	usb_fill_int_urb(precvpriv->int_in_urb, pusbd, pipe,
 			 precvpriv->int_in_buf, USB_INTR_CONTENT_LENGTH,
@@ -457,16 +479,16 @@ static int recvbuf2recvframe(struct rtw_adapter *padapter, struct sk_buff *pskb)
 	u8	shift_sz = 0;
 	u16	pkt_cnt;
 	u32	pkt_offset, skb_len, alloc_sz;
-	s32	transfer_len;
-	struct recv_stat	*prxstat;
-	struct phy_stat	*pphy_info = NULL;
-	struct sk_buff		*pkt_copy = NULL;
-	struct recv_frame	*precvframe = NULL;
-	struct rx_pkt_attrib	*pattrib = NULL;
-	struct recv_priv	*precvpriv = &padapter->recvpriv;
-	struct rtw_queue	*pfree_recv_queue = &precvpriv->free_recv_queue;
-
-	transfer_len = (s32)pskb->len;
+	int	transfer_len;
+	struct recv_stat *prxstat;
+	struct phy_stat	*pphy_info;
+	struct sk_buff *pkt_copy;
+	struct recv_frame *precvframe;
+	struct rx_pkt_attrib *pattrib;
+	struct recv_priv *precvpriv = &padapter->recvpriv;
+	struct rtw_queue *pfree_recv_queue = &precvpriv->free_recv_queue;
+
+	transfer_len = (int)pskb->len;
 	pbuf = pskb->data;
 
 	prxstat = (struct recv_stat *)pbuf;
@@ -485,7 +507,7 @@ static int recvbuf2recvframe(struct rtw_adapter *padapter, struct sk_buff *pskb)
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
 				 ("recvbuf2recvframe: precvframe == NULL\n"));
 			DBG_8723A("%s()-%d: rtw_alloc_recvframe23a() failed! RX "
-				  "Drop!\n", __FUNCTION__, __LINE__);
+				  "Drop!\n", __func__, __LINE__);
 			goto _exit_recvbuf2recvframe;
 		}
 
@@ -497,31 +519,30 @@ static int recvbuf2recvframe(struct rtw_adapter *padapter, struct sk_buff *pskb)
 
 		if (pattrib->crc_err) {
 			DBG_8723A("%s()-%d: RX Warning! rx CRC ERROR !!\n",
-				  __FUNCTION__, __LINE__);
-			rtw_free_recvframe23a(precvframe, pfree_recv_queue);
+				  __func__, __LINE__);
+			rtw_free_recvframe23a(precvframe);
 			goto _exit_recvbuf2recvframe;
 		}
 
 		pkt_offset = RXDESC_SIZE + pattrib->drvinfo_sz +
 			pattrib->shift_sz + pattrib->pkt_len;
 
-		if ((pattrib->pkt_len <= 0) || (pkt_offset > transfer_len)) {
+		if (pattrib->pkt_len <= 0 || pkt_offset > transfer_len) {
 			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
 				 ("recvbuf2recvframe: pkt_len<= 0\n"));
 			DBG_8723A("%s()-%d: RX Warning!\n",
-				  __FUNCTION__, __LINE__);
-			rtw_free_recvframe23a(precvframe, pfree_recv_queue);
+				  __func__, __LINE__);
+			rtw_free_recvframe23a(precvframe);
 			goto _exit_recvbuf2recvframe;
 		}
 
 		/*	Modified by Albert 20101213 */
 		/*	For 8 bytes IP header alignment. */
 		/*	Qos data, wireless lan header length is 26 */
-		if (pattrib->qos) {
+		if (pattrib->qos)
 			shift_sz = 6;
-		} else {
+		else
 			shift_sz = 0;
-		}
 
 		skb_len = pattrib->pkt_len;
 
@@ -529,7 +550,7 @@ static int recvbuf2recvframe(struct rtw_adapter *padapter, struct sk_buff *pskb)
 		 * 1536+drvinfo_sz+RXDESC_SIZE to defrag packet.
 		 * modify alloc_sz for recvive crc error packet
 		 * by thomas 2011-06-02 */
-		if ((pattrib->mfrag == 1) && (pattrib->frag_num == 0)) {
+		if (pattrib->mfrag == 1 && pattrib->frag_num == 0) {
 			/* alloc_sz = 1664;	1664 is 128 alignment. */
 			if (skb_len <= 1650)
 				alloc_sz = 1664;
@@ -546,17 +567,20 @@ static int recvbuf2recvframe(struct rtw_adapter *padapter, struct sk_buff *pskb)
 		if (pkt_copy) {
 			pkt_copy->dev = padapter->pnetdev;
 			precvframe->pkt = pkt_copy;
-			skb_reserve(pkt_copy, 8 - ((unsigned long)(pkt_copy->data) & 7));/* force pkt_copy->data at 8-byte alignment address */
-	/*force ip_hdr at 8-byte alignment address according to shift_sz. */
+			/* force pkt_copy->data at 8-byte alignment address */
+			skb_reserve(pkt_copy, 8 -
+				    ((unsigned long)(pkt_copy->data) & 7));
+			/*force ip_hdr at 8-byte alignment address
+			  according to shift_sz. */
 			skb_reserve(pkt_copy, shift_sz);
-			memcpy(pkt_copy->data, (pbuf + pattrib->shift_sz + pattrib->drvinfo_sz + RXDESC_SIZE), skb_len);
+			memcpy(pkt_copy->data, pbuf + pattrib->shift_sz +
+			       pattrib->drvinfo_sz + RXDESC_SIZE, skb_len);
 			skb_put(pkt_copy, skb_len);
 		} else {
-			if ((pattrib->mfrag == 1) && (pattrib->frag_num == 0)) {
+			if (pattrib->mfrag == 1 && pattrib->frag_num == 0) {
 				DBG_8723A("recvbuf2recvframe: alloc_skb fail, "
 					  "drop frag frame \n");
-				rtw_free_recvframe23a(precvframe,
-						   pfree_recv_queue);
+				rtw_free_recvframe23a(precvframe);
 				goto _exit_recvbuf2recvframe;
 			}
 
@@ -564,8 +588,7 @@ static int recvbuf2recvframe(struct rtw_adapter *padapter, struct sk_buff *pskb)
 			if (!precvframe->pkt) {
 				DBG_8723A("recvbuf2recvframe: skb_clone "
 					  "fail\n");
-				rtw_free_recvframe23a(precvframe,
-						   pfree_recv_queue);
+				rtw_free_recvframe23a(precvframe);
 				goto _exit_recvbuf2recvframe;
 			}
 		}
@@ -589,7 +612,7 @@ static int recvbuf2recvframe(struct rtw_adapter *padapter, struct sk_buff *pskb)
 		if (transfer_len > 0 && pkt_cnt == 0)
 			pkt_cnt = (le32_to_cpu(prxstat->rxdw2)>>16) & 0xff;
 
-	} while ((transfer_len > 0) && (pkt_cnt > 0));
+	} while (transfer_len > 0 && pkt_cnt > 0);
 
 _exit_recvbuf2recvframe:
 
@@ -603,8 +626,7 @@ void rtl8723au_recv_tasklet(void *priv)
 	struct recv_priv *precvpriv = &padapter->recvpriv;
 
 	while (NULL != (pskb = skb_dequeue(&precvpriv->rx_skb_queue))) {
-		if ((padapter->bDriverStopped) ||
-		    (padapter->bSurpriseRemoved)) {
+		if (padapter->bDriverStopped || padapter->bSurpriseRemoved) {
 			DBG_8723A("recv_tasklet => bDriverStopped or "
 				  "bSurpriseRemoved \n");
 			dev_kfree_skb_any(pskb);
@@ -620,7 +642,7 @@ void rtl8723au_recv_tasklet(void *priv)
 	}
 }
 
-static void usb_read_port_complete(struct urb *purb, struct pt_regs *regs)
+static void usb_read_port_complete(struct urb *purb)
 {
 	struct recv_buf *precvbuf = (struct recv_buf *)purb->context;
 	struct rtw_adapter *padapter = (struct rtw_adapter *)precvbuf->adapter;
@@ -641,22 +663,22 @@ static void usb_read_port_complete(struct urb *purb, struct pt_regs *regs)
 
 		DBG_8723A("%s()-%d: RX Warning! bDriverStopped(%d) OR "
 			  "bSurpriseRemoved(%d) bReadPortCancel(%d)\n",
-			  __FUNCTION__, __LINE__, padapter->bDriverStopped,
+			  __func__, __LINE__, padapter->bDriverStopped,
 			  padapter->bSurpriseRemoved, padapter->bReadPortCancel);
 		return;
 	}
 
 	if (purb->status == 0) {
-		if ((purb->actual_length > MAX_RECVBUF_SZ) ||
-		    (purb->actual_length < RXDESC_SIZE)) {
+		if (purb->actual_length > MAX_RECVBUF_SZ ||
+		    purb->actual_length < RXDESC_SIZE) {
 			RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
 				 ("usb_read_port_complete: (purb->actual_"
 				  "length > MAX_RECVBUF_SZ) || (purb->actual_"
 				  "length < RXDESC_SIZE)\n"));
-			rtw_read_port(padapter, precvpriv->ff_hwaddr, 0,
-				      precvbuf);
+			rtl8723au_read_port(padapter, RECV_BULK_IN_ADDR, 0,
+					    precvbuf);
 			DBG_8723A("%s()-%d: RX Warning!\n",
-				  __FUNCTION__, __LINE__);
+				  __func__, __LINE__);
 		} else {
 			rtw_reset_continual_urb_error(
 				adapter_to_dvobj(padapter));
@@ -669,8 +691,8 @@ static void usb_read_port_complete(struct urb *purb, struct pt_regs *regs)
 				tasklet_schedule(&precvpriv->recv_tasklet);
 
 			precvbuf->pskb = NULL;
-			rtw_read_port(padapter, precvpriv->ff_hwaddr, 0,
-				      precvbuf);
+			rtl8723au_read_port(padapter, RECV_BULK_IN_ADDR, 0,
+					    precvbuf);
 		}
 	} else {
 		RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
@@ -707,32 +729,30 @@ static void usb_read_port_complete(struct urb *purb, struct pt_regs *regs)
 			pHalData = GET_HAL_DATA(padapter);
 			pHalData->srestpriv.Wifi_Error_Status =
 				USB_READ_PORT_FAIL;
-			rtw_read_port(padapter, precvpriv->ff_hwaddr,
-				      0, precvbuf);
+			rtl8723au_read_port(padapter, RECV_BULK_IN_ADDR, 0,
+					    precvbuf);
 			break;
 		case -EINPROGRESS:
-			DBG_8723A("ERROR: URB IS IN PROGRESS!/n");
+			DBG_8723A("ERROR: URB IS IN PROGRESS!\n");
 			break;
 		default:
 			break;
 		}
-
 	}
 }
 
-static u32 usb_read_port(struct intf_hdl *pintfhdl, u32 addr, u32 cnt,
-			 struct recv_buf *precvbuf)
+int rtl8723au_read_port(struct rtw_adapter *adapter, u32 addr, u32 cnt,
+			struct recv_buf *precvbuf)
 {
 	int err;
 	unsigned int pipe;
-	unsigned long tmpaddr = 0;
-	unsigned long alignment = 0;
-	u32 ret = _SUCCESS;
-	struct urb *purb = NULL;
-	struct rtw_adapter		*adapter = pintfhdl->padapter;
-	struct dvobj_priv	*pdvobj = adapter_to_dvobj(adapter);
-	struct recv_priv	*precvpriv = &adapter->recvpriv;
-	struct usb_device	*pusbd = pdvobj->pusbdev;
+	unsigned long tmpaddr;
+	unsigned long alignment;
+	int ret = _SUCCESS;
+	struct urb *purb;
+	struct dvobj_priv *pdvobj = adapter_to_dvobj(adapter);
+	struct recv_priv *precvpriv = &adapter->recvpriv;
+	struct usb_device *pusbd = pdvobj->pusbdev;
 
 	if (adapter->bDriverStopped || adapter->bSurpriseRemoved ||
 	    adapter->pwrctrlpriv.pnp_bstop_trx) {
@@ -752,8 +772,6 @@ static u32 usb_read_port(struct intf_hdl *pintfhdl, u32 addr, u32 cnt,
 	if (!precvbuf->pskb)
 		precvbuf->pskb = skb_dequeue(&precvpriv->free_recv_skb_queue);
 
-	rtl8723au_init_recvbuf(adapter, precvbuf);
-
 	/* re-assign for linux based on skb */
 	if (!precvbuf->pskb) {
 		precvbuf->pskb = netdev_alloc_skb(adapter->pnetdev, MAX_RECVBUF_SZ + RECVBUFF_ALIGN_SZ);
@@ -772,7 +790,7 @@ static u32 usb_read_port(struct intf_hdl *pintfhdl, u32 addr, u32 cnt,
 	purb = precvbuf->purb;
 
 	/* translate DMA FIFO addr to pipehandle */
-	pipe = ffaddr2pipehdl23a(pdvobj, addr);
+	pipe = usb_rcvbulkpipe(pusbd, pdvobj->RtInPipe[0]);
 
 	usb_fill_bulk_urb(purb, pusbd, pipe, precvbuf->pskb->data,
 			  MAX_RECVBUF_SZ, usb_read_port_complete,
@@ -792,7 +810,7 @@ static u32 usb_read_port(struct intf_hdl *pintfhdl, u32 addr, u32 cnt,
 
 void rtl8723au_xmit_tasklet(void *priv)
 {
-	int ret = false;
+	int ret;
 	struct rtw_adapter *padapter = (struct rtw_adapter *)priv;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 
@@ -800,9 +818,8 @@ void rtl8723au_xmit_tasklet(void *priv)
 		return;
 
 	while (1) {
-		if ((padapter->bDriverStopped) ||
-		    (padapter->bSurpriseRemoved) ||
-		    (padapter->bWritePortCancel)) {
+		if (padapter->bDriverStopped || padapter->bSurpriseRemoved ||
+		    padapter->bWritePortCancel) {
 			DBG_8723A("xmit_tasklet => bDriverStopped or "
 				  "bSurpriseRemoved or bWritePortCancel\n");
 			break;
@@ -815,31 +832,6 @@ void rtl8723au_xmit_tasklet(void *priv)
 	}
 }
 
-void rtl8723au_set_intf_ops(struct _io_ops *pops)
-{
-
-	memset((u8 *)pops, 0, sizeof(struct _io_ops));
-
-	pops->_read8 = &usb_read8;
-	pops->_read16 = &usb_read16;
-	pops->_read32 = &usb_read32;
-	pops->_read_mem = &usb_read_mem23a;
-	pops->_read_port = &usb_read_port;
-
-	pops->_write8 = &usb_write8;
-	pops->_write16 = &usb_write16;
-	pops->_write32 = &usb_write32;
-	pops->_writeN = &usb_writeN;
-
-	pops->_write_mem = &usb_write_mem23a;
-	pops->_write_port = &usb_write_port23a;
-
-	pops->_read_port_cancel = &usb_read_port_cancel23a;
-	pops->_write_port_cancel = &usb_write_port23a_cancel;
-
-	pops->_read_interrupt = &usb_read_interrupt;
-}
-
 void rtl8723au_set_hw_type(struct rtw_adapter *padapter)
 {
 	padapter->chip_type = RTL8723A;
diff --git a/drivers/staging/rtl8723au/include/Hal8723APhyCfg.h b/drivers/staging/rtl8723au/include/Hal8723APhyCfg.h
index 4b7f3479c0a..299598b5387 100644
--- a/drivers/staging/rtl8723au/include/Hal8723APhyCfg.h
+++ b/drivers/staging/rtl8723au/include/Hal8723APhyCfg.h
@@ -68,13 +68,13 @@ enum RF_RADIO_PATH {
 
 enum WIRELESS_MODE {
 	WIRELESS_MODE_UNKNOWN	= 0x00,
-	WIRELESS_MODE_A		= BIT2,
-	WIRELESS_MODE_B		= BIT0,
-	WIRELESS_MODE_G		= BIT1,
-	WIRELESS_MODE_AUTO	= BIT5,
-	WIRELESS_MODE_N_24G	= BIT3,
-	WIRELESS_MODE_N_5G	= BIT4,
-	WIRELESS_MODE_AC	= BIT6
+	WIRELESS_MODE_A		= BIT(2),
+	WIRELESS_MODE_B		= BIT(0),
+	WIRELESS_MODE_G		= BIT(1),
+	WIRELESS_MODE_AUTO	= BIT(5),
+	WIRELESS_MODE_N_24G	= BIT(3),
+	WIRELESS_MODE_N_5G	= BIT(4),
+	WIRELESS_MODE_AC	= BIT(6)
 };
 
 enum baseband_config_type {
diff --git a/drivers/staging/rtl8723au/include/Hal8723PwrSeq.h b/drivers/staging/rtl8723au/include/Hal8723PwrSeq.h
index 7f3bdea6a55..4a1f58f2982 100644
--- a/drivers/staging/rtl8723au/include/Hal8723PwrSeq.h
+++ b/drivers/staging/rtl8723au/include/Hal8723PwrSeq.h
@@ -39,71 +39,71 @@
  * { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },  comments here
  */
 #define RTL8723A_TRANS_CARDEMU_TO_ACT														\
-	{0x0020, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, BIT0}, /*0x20[0] = 1b'1 enable LDOA12 MACRO block for all interface*/   \
-	{0x0067, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, 0}, /*0x67[0] = 0 to disable BT_GPS_SEL pins*/	\
+	{0x0020, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), BIT(0)}, /*0x20[0] = 1b'1 enable LDOA12 MACRO block for all interface*/ \
+	{0x0067, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), 0}, /*0x67[0] = 0 to disable BT_GPS_SEL pins*/	\
 	{0x0001, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_DELAY, 1, PWRSEQ_DELAY_MS},/*Delay 1ms*/   \
-	{0x0000, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT5, 0}, /*0x00[5] = 1b'0 release analog Ips to digital , 1:isolation*/   \
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT2, 0},/* disable SW LPS 0x04[10]= 0*/	\
-	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT1, BIT1},/* wait till 0x04[17] = 1    power ready*/	\
-	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, BIT0},/* release WLON reset  0x04[16]= 1*/	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT7, 0},/* disable HWPDN 0x04[15]= 0*/	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, (BIT4|BIT3), 0},/* disable WL suspend*/	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, BIT0},/* polling until return 0*/	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT0, 0},/**/	\
-	{0x004E, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT7, 1},/*0x4C[23] = 0x4E[7] = 1, switch DPDT_SEL_P output from WL BB */\
+	{0x0000, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(5), 0}, /*0x00[5] = 1b'0 release analog Ips to digital , 1:isolation*/   \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(2), 0},/* disable SW LPS 0x04[10]= 0*/	\
+	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(1), BIT(1)},/* wait till 0x04[17] = 1    power ready*/	\
+	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), BIT(0)},/* release WLON reset  0x04[16]= 1*/ \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), 0},/* disable HWPDN 0x04[15]= 0*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, (BIT(4)|BIT(3)), 0},/* disable WL suspend*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), BIT(0)},/* polling until return 0*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(0), 0},/**/	\
+	{0x004E, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), 1},/*0x4C[23] = 0x4E[7] = 1, switch DPDT_SEL_P output from WL BB */\
 
 #define RTL8723A_TRANS_ACT_TO_CARDEMU													\
 	{0x001F, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0},/*0x1F[7:0] = 0 turn off RF*/	\
-	{0x004E, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT7, 0},/*0x4C[23] = 0x4E[7] = 0, switch DPDT_SEL_P output from register 0x65[2] */\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, BIT1}, /*0x04[9] = 1 turn off MAC by HW state machine*/	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT1, 0}, /*wait till 0x04[9] = 0 polling until return 0 to disable*/	\
-	{0x0000, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT5, BIT5}, /*0x00[5] = 1b'1 analog Ips to digital , 1:isolation*/   \
-	{0x0020, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 0}, /*0x20[0] = 1b'0 disable LDOA12 MACRO block*/   \
+	{0x004E, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), 0},/*0x4C[23] = 0x4E[7] = 0, switch DPDT_SEL_P output from register 0x65[2] */\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), BIT(1)}, /*0x04[9] = 1 turn off MAC by HW state machine*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(1), 0}, /*wait till 0x04[9] = 0 polling until return 0 to disable*/	\
+	{0x0000, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(5), BIT(5)}, /*0x00[5] = 1b'1 analog Ips to digital , 1:isolation*/   \
+	{0x0020, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), 0}, /*0x20[0] = 1b'0 disable LDOA12 MACRO block*/   \
 
 
 #define RTL8723A_TRANS_CARDEMU_TO_SUS													\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4|BIT3, (BIT4|BIT3)}, /*0x04[12:11] = 2b'11 enable WL suspend for PCIe*/	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3|BIT4, BIT3}, /*0x04[12:11] = 2b'01 enable WL suspend*/	\
-	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, BIT4}, /*0x23[4] = 1b'1 12H LDO enter sleep mode*/   \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4)|BIT(3), (BIT(4)|BIT(3))}, /*0x04[12:11] = 2b'11 enable WL suspend for PCIe*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3)|BIT(4), BIT(3)}, /*0x04[12:11] = 2b'01 enable WL suspend*/	\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), BIT(4)}, /*0x23[4] = 1b'1 12H LDO enter sleep mode*/   \
 	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x20}, /*0x07[7:0] = 0x20 SDIO SOP option to disable BG/MB/ACK/SWR*/   \
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3|BIT4, BIT3|BIT4}, /*0x04[12:11] = 2b'11 enable WL suspend for PCIe*/	\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT0, BIT0}, /*Set SDIO suspend local register*/	\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT1, 0}, /*wait power state to suspend*/
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3)|BIT(4), BIT(3)|BIT(4)}, /*0x04[12:11] = 2b'11 enable WL suspend for PCIe*/	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT(0), BIT(0)}, /*Set SDIO suspend local register*/	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT(1), 0}, /*wait power state to suspend*/
 
 #define RTL8723A_TRANS_SUS_TO_CARDEMU													\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3 | BIT7, 0}, /*clear suspend enable and power down enable*/	\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT0, 0}, /*Set SDIO suspend local register*/	\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT1, BIT1}, /*wait power state to suspend*/\
-	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, 0}, /*0x23[4] = 1b'0 12H LDO enter normal mode*/   \
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3|BIT4, 0}, /*0x04[12:11] = 2b'01enable WL suspend*/
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3) | BIT(7), 0}, /*clear suspend enable and power down enable*/	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT(0), 0}, /*Set SDIO suspend local register*/	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT(1), BIT(1)}, /*wait power state to suspend*/\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), 0}, /*0x23[4] = 1b'0 12H LDO enter normal mode*/   \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3)|BIT(4), 0}, /*0x04[12:11] = 2b'01enable WL suspend*/
 
 #define RTL8723A_TRANS_CARDEMU_TO_CARDDIS													\
 	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x20}, /*0x07 = 0x20 , SOP option to disable BG/MB*/	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3|BIT4, BIT3}, /*0x04[12:11] = 2b'01 enable WL suspend*/	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT2, BIT2}, /*0x04[10] = 1, enable SW LPS*/	\
-	{0x004A, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 1}, /*0x48[16] = 1 to enable GPIO9 as EXT WAKEUP*/   \
-	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, BIT4}, /*0x23[4] = 1b'1 12H LDO enter sleep mode*/   \
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT0, BIT0}, /*Set SDIO suspend local register*/	\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT1, 0}, /*wait power state to suspend*/
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3)|BIT(4), BIT(3)}, /*0x04[12:11] = 2b'01 enable WL suspend*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(2), BIT(2)}, /*0x04[10] = 1, enable SW LPS*/	\
+	{0x004A, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), 1}, /*0x48[16] = 1 to enable GPIO9 as EXT WAKEUP*/   \
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), BIT(4)}, /*0x23[4] = 1b'1 12H LDO enter sleep mode*/   \
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT(0), BIT(0)}, /*Set SDIO suspend local register*/	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT(1), 0}, /*wait power state to suspend*/
 
 #define RTL8723A_TRANS_CARDDIS_TO_CARDEMU													\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3 | BIT7, 0}, /*clear suspend enable and power down enable*/	\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT0, 0}, /*Set SDIO suspend local register*/	\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT1, BIT1}, /*wait power state to suspend*/\
-	{0x004A, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 0}, /*0x48[16] = 0 to disable GPIO9 as EXT WAKEUP*/   \
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3|BIT4, 0}, /*0x04[12:11] = 2b'01enable WL suspend*/\
-	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, 0}, /*0x23[4] = 1b'0 12H LDO enter normal mode*/   \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3) | BIT(7), 0}, /*clear suspend enable and power down enable*/	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT(0), 0}, /*Set SDIO suspend local register*/	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT(1), BIT(1)}, /*wait power state to suspend*/\
+	{0x004A, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), 0}, /*0x48[16] = 0 to disable GPIO9 as EXT WAKEUP*/   \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3)|BIT(4), 0}, /*0x04[12:11] = 2b'01enable WL suspend*/\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), 0}, /*0x23[4] = 1b'0 12H LDO enter normal mode*/   \
 	{0x0301, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0},/*PCIe DMA start*/
 
 
 #define RTL8723A_TRANS_CARDEMU_TO_PDN												\
-	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, BIT4}, /*0x23[4] = 1b'1 12H LDO enter sleep mode*/   \
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), BIT(4)}, /*0x23[4] = 1b'1 12H LDO enter sleep mode*/   \
 	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK|PWR_INTF_USB_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x20}, /*0x07[7:0] = 0x20 SOP option to disable BG/MB/ACK/SWR*/   \
-	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 0},/* 0x04[16] = 0*/\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT7, BIT7},/* 0x04[15] = 1*/
+	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), 0},/* 0x04[16] = 0*/\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), BIT(7)},/* 0x04[15] = 1*/
 
 #define RTL8723A_TRANS_PDN_TO_CARDEMU												\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT7, 0},/* 0x04[15] = 0*/
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), 0},/* 0x04[15] = 0*/
 
 #define RTL8723A_TRANS_ACT_TO_LPS														\
 	{0x0301, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0xFF},/*PCIe DMA stop*/	\
@@ -112,25 +112,25 @@
 	{0x05F9, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},/*Should be zero if no packet is transmitting*/	\
 	{0x05FA, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},/*Should be zero if no packet is transmitting*/	\
 	{0x05FB, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},/*Should be zero if no packet is transmitting*/	\
-	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 0},/*CCK and OFDM are disabled, and clock are gated*/	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), 0},/*CCK and OFDM are disabled, and clock are gated*/	\
 	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_DELAY, 0, PWRSEQ_DELAY_US},/*Delay 1us*/	\
-	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, 0},/*Whole BB is reset*/	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), 0},/*Whole BB is reset*/	\
 	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x03},/*Reset MAC TRX*/	\
-	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, 0},/*check if removed later*/	\
+	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), 0},/*check if removed later*/	\
 	{0x0093, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x00},/*When driver enter Sus/ Disable, enable LOP for BT*/	\
-	{0x0553, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT5, BIT5},/*Respond TxOK to scheduler*/
+	{0x0553, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(5), BIT(5)},/*Respond TxOK to scheduler*/
 
 #define RTL8723A_TRANS_LPS_TO_ACT															\
 	{0x0080, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_WRITE, 0xFF, 0x84}, /*SDIO RPWM*/\
 	{0xFE58, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x84}, /*USB RPWM*/\
 	{0x0361, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x84}, /*PCIe RPWM*/\
 	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_DELAY, 0, PWRSEQ_DELAY_MS}, /*Delay*/\
-	{0x0008, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, 0}, /*.	0x08[4] = 0		 switch TSF to 40M*/\
-	{0x0109, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT7, 0}, /*Polling 0x109[7]= 0  TSF in 40M*/\
-	{0x0029, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT6|BIT7, 0}, /*.	0x29[7:6] = 2b'00	 enable BB clock*/\
-	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, BIT1}, /*.	0x101[1] = 1*/\
+	{0x0008, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), 0}, /*.	0x08[4] = 0		 switch TSF to 40M*/\
+	{0x0109, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(7), 0}, /*Polling 0x109[7]= 0  TSF in 40M*/\
+	{0x0029, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(6)|BIT(7), 0}, /*.	0x29[7:6] = 2b'00	 enable BB clock*/\
+	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), BIT(1)}, /*.	0x101[1] = 1*/\
 	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0xFF}, /*.	0x100[7:0] = 0xFF	 enable WMAC TRX*/\
-	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1|BIT0, BIT1|BIT0}, /*.	0x02[1:0] = 2b'11	 enable BB macro*/\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1)|BIT(0), BIT(1)|BIT(0)}, /*.	0x02[1:0] = 2b'11	 enable BB macro*/\
 	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0}, /*.	0x522 = 0*/
 
 #define RTL8723A_TRANS_END															\
diff --git a/drivers/staging/rtl8723au/include/cmd_osdep.h b/drivers/staging/rtl8723au/include/cmd_osdep.h
deleted file mode 100644
index 4866bee0405..00000000000
--- a/drivers/staging/rtl8723au/include/cmd_osdep.h
+++ /dev/null
@@ -1,26 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
- *
- * 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.
- *
- * 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 __CMD_OSDEP_H_
-#define __CMD_OSDEP_H_
-
-#include <osdep_service.h>
-#include <drv_types.h>
-
-int _rtw_init_evt_priv23a(struct evt_priv *pevtpriv);
-void _rtw_free_evt_priv23a(struct	evt_priv *pevtpriv);
-void _rtw_free_cmd_priv23a(struct	cmd_priv *pcmdpriv);
-int _rtw_enqueue_cmd23a(struct rtw_queue *queue, struct cmd_obj *obj);
-
-#endif
diff --git a/drivers/staging/rtl8723au/include/drv_types.h b/drivers/staging/rtl8723au/include/drv_types.h
index 53eecea48ce..a94857df2de 100644
--- a/drivers/staging/rtl8723au/include/drv_types.h
+++ b/drivers/staging/rtl8723au/include/drv_types.h
@@ -38,12 +38,10 @@ enum _NIC_VERSION {
 #include <rtw_ht.h>
 
 #include <rtw_cmd.h>
-#include <wlan_bssdef.h>
 #include <rtw_xmit.h>
 #include <rtw_recv.h>
 #include <hal_intf.h>
 #include <hal_com.h>
-#include <rtw_qos.h>
 #include <rtw_security.h>
 #include <rtw_pwrctrl.h>
 #include <rtw_io.h>
@@ -55,26 +53,10 @@ enum _NIC_VERSION {
 #include <rtw_event.h>
 #include <rtw_led.h>
 #include <rtw_mlme_ext.h>
-#include <rtw_p2p.h>
 #include <rtw_ap.h>
 
 #include "ioctl_cfg80211.h"
 
-#define SPEC_DEV_ID_NONE BIT(0)
-#define SPEC_DEV_ID_DISABLE_HT BIT(1)
-#define SPEC_DEV_ID_ENABLE_PS BIT(2)
-#define SPEC_DEV_ID_RF_CONFIG_1T1R BIT(3)
-#define SPEC_DEV_ID_RF_CONFIG_2T2R BIT(4)
-#define SPEC_DEV_ID_ASSIGN_IFNAME BIT(5)
-
-struct specific_device_id {
-	u32		flags;
-
-	u16		idVendor;
-	u16		idProduct;
-
-};
-
 struct registry_priv {
 	u8	chip_version;
 	u8	rfintfs;
@@ -187,8 +169,6 @@ struct dvobj_priv {
 	int	RtOutPipe[3];
 	u8	Queue2Pipe[HW_QUEUE_ENTRY];/* for out pipe mapping */
 
-	u8	irq_alloc;
-
 /*-------- below is for USB INTERFACE --------*/
 
 	u8	nr_endpoint;
@@ -197,10 +177,6 @@ struct dvobj_priv {
 	u8	RtNumOutPipes;
 	int	ep_num[5]; /* endpoint number */
 
-	int	RegUsbSS;
-
-	struct semaphore usb_suspend_sema;
-
 	struct mutex  usb_vendor_req_mutex;
 
 	u8 *usb_alloc_vendor_req_buf;
@@ -243,8 +219,6 @@ struct rtw_adapter {
 	struct	mlme_ext_priv mlmeextpriv;
 	struct	cmd_priv	cmdpriv;
 	struct	evt_priv	evtpriv;
-	/* struct	io_queue	*pio_queue; */
-	struct	io_priv	iopriv;
 	struct	xmit_priv	xmitpriv;
 	struct	recv_priv	recvpriv;
 	struct	sta_priv	stapriv;
@@ -254,21 +228,9 @@ struct rtw_adapter {
 	struct	eeprom_priv eeprompriv;
 	struct	led_priv	ledpriv;
 
-#ifdef CONFIG_8723AU_AP_MODE
-	struct	hostapd_priv	*phostapdpriv;
-#endif
-
-	struct cfg80211_wifidirect_info	cfg80211_wdinfo;
 	u32	setband;
-	struct wifidirect_info	wdinfo;
-
-#ifdef CONFIG_8723AU_P2P
-	struct wifi_display_info wfd_info;
-#endif /* CONFIG_8723AU_P2P */
 
 	void *HalData;
-	u32 hal_data_sz;
-	struct hal_ops	HalFunc;
 
 	s32	bDriverStopped;
 	s32	bSurpriseRemoved;
@@ -283,69 +245,26 @@ struct rtw_adapter {
 	u8	init_adpt_in_progress;
 	u8	bHaltInProgress;
 
-	void *cmdThread;
-	void *evtThread;
-	void *xmitThread;
-	void *recvThread;
-
-	void (*intf_start)(struct rtw_adapter *adapter);
-	void (*intf_stop)(struct rtw_adapter *adapter);
-
 	struct net_device *pnetdev;
 
 	/*  used by rtw_rereg_nd_name related function */
-	struct rereg_nd_name_data {
-		struct net_device *old_pnetdev;
-		char old_ifname[IFNAMSIZ];
-		u8 old_ips_mode;
-		u8 old_bRegUseLed;
-	} rereg_nd_name_priv;
-
 	int bup;
 	struct net_device_stats stats;
-	struct iw_statistics iwstats;
-	struct proc_dir_entry *dir_dev;/*  for proc directory */
 
 	struct wireless_dev *rtw_wdev;
 	int net_closed;
 
 	u8 bFWReady;
-	u8 bBTFWReady;
 	u8 bReadPortCancel;
 	u8 bWritePortCancel;
-	u8 bRxRSSIDisplay;
 	/* The driver will show the desired chan nor when this flag is 1. */
 	u8 bNotifyChannelChange;
-#ifdef CONFIG_8723AU_P2P
-	/* driver will show current P2P status when the  application reads it*/
-	u8 bShowGetP2PState;
-#endif
 	struct rtw_adapter *pbuddy_adapter;
 
 	/* extend to support multi interface */
 	/* IFACE_ID0 is equals to PRIMARY_ADAPTER */
 	/* IFACE_ID1 is equals to SECONDARY_ADAPTER */
 	u8 iface_id;
-
-#ifdef CONFIG_BR_EXT
-	_lock					br_ext_lock;
-	/* unsigned int			macclone_completed; */
-	struct nat25_network_db_entry	*nethash[NAT25_HASH_SIZE];
-	int				pppoe_connection_in_progress;
-	unsigned char			pppoe_addr[MACADDRLEN];
-	unsigned char			scdb_mac[MACADDRLEN];
-	unsigned char			scdb_ip[4];
-	struct nat25_network_db_entry	*scdb_entry;
-	unsigned char			br_mac[MACADDRLEN];
-	unsigned char			br_ip[4];
-
-	struct br_ext_info		ethBrExtInfo;
-#endif	/*  CONFIG_BR_EXT */
-
-	u8    fix_rate;
-
-	unsigned char     in_cta_test;
-
 };
 
 #define adapter_to_dvobj(adapter) (adapter->dvobj)
diff --git a/drivers/staging/rtl8723au/include/ethernet.h b/drivers/staging/rtl8723au/include/ethernet.h
deleted file mode 100644
index 39fc6df8818..00000000000
--- a/drivers/staging/rtl8723au/include/ethernet.h
+++ /dev/null
@@ -1,22 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
- *
- * 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.
- *
- * 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.
- *
- *
- ******************************************************************************/
-/*! \file */
-#ifndef __INC_ETHERNET_H
-#define __INC_ETHERNET_H
-
-#define LLC_HEADER_SIZE			6	/*  LLC Header Length */
-
-#endif /*  #ifndef __INC_ETHERNET_H */
diff --git a/drivers/staging/rtl8723au/include/hal_com.h b/drivers/staging/rtl8723au/include/hal_com.h
index 20f983cfc2b..4a161e26c8d 100644
--- a/drivers/staging/rtl8723au/include/hal_com.h
+++ b/drivers/staging/rtl8723au/include/hal_com.h
@@ -165,8 +165,6 @@ void HalSetBrateCfg23a(struct rtw_adapter *padapter, u8 *mBratesOS);
 bool
 Hal_MappingOutPipe23a(struct rtw_adapter *pAdapter, u8 NumOutPipe);
 
-void hal_init_macaddr23a(struct rtw_adapter *adapter);
-
 void c2h_evt_clear23a(struct rtw_adapter *adapter);
 s32 c2h_evt_read23a(struct rtw_adapter *adapter, u8 *buf);
 
@@ -185,11 +183,12 @@ void rtl8723a_ack_preamble(struct rtw_adapter *padapter, u8 bShortPreamble);
 void rtl8723a_set_sec_cfg(struct rtw_adapter *padapter, u8 sec);
 void rtl8723a_cam_empty_entry(struct rtw_adapter *padapter, u8 ucIndex);
 void rtl8723a_cam_invalid_all(struct rtw_adapter *padapter);
-void rtl8723a_cam_write(struct rtw_adapter *padapter, u32 val1, u32 val2);
+void rtl8723a_cam_write(struct rtw_adapter *padapter,
+			u8 entry, u16 ctrl, const u8 *mac, const u8 *key);
 void rtl8723a_fifo_cleanup(struct rtw_adapter *padapter);
 void rtl8723a_set_apfm_on_mac(struct rtw_adapter *padapter, u8 val);
 void rtl8723a_bcn_valid(struct rtw_adapter *padapter);
-void rtl8723a_set_tx_pause(struct rtw_adapter *padapter, u8 pause);
+bool rtl8723a_get_bcn_valid(struct rtw_adapter *padapter);
 void rtl8723a_set_beacon_interval(struct rtw_adapter *padapter, u16 interval);
 void rtl8723a_set_resp_sifs(struct rtw_adapter *padapter,
 			    u8 r2t1, u8 r2t2, u8 t2t1, u8 t2t2);
@@ -202,10 +201,14 @@ void rtl8723a_set_nav_upper(struct rtw_adapter *padapter, u32 usNavUpper);
 void rtl8723a_set_initial_gain(struct rtw_adapter *padapter, u32 rx_gain);
 
 void rtl8723a_odm_support_ability_write(struct rtw_adapter *padapter, u32 val);
-void rtl8723a_odm_support_ability_backup(struct rtw_adapter *padapter, u8 val);
+void rtl8723a_odm_support_ability_backup(struct rtw_adapter *padapter);
+void rtl8723a_odm_support_ability_restore(struct rtw_adapter *padapter);
 void rtl8723a_odm_support_ability_set(struct rtw_adapter *padapter, u32 val);
 void rtl8723a_odm_support_ability_clr(struct rtw_adapter *padapter, u32 val);
 
 void rtl8723a_set_rpwm(struct rtw_adapter *padapter, u8 val);
+u8 rtl8723a_get_rf_type(struct rtw_adapter *padapter);
+bool rtl8723a_get_fwlps_rf_on(struct rtw_adapter *padapter);
+bool rtl8723a_chk_hi_queue_empty(struct rtw_adapter *padapter);
 
 #endif /* __HAL_COMMON_H__ */
diff --git a/drivers/staging/rtl8723au/include/hal_intf.h b/drivers/staging/rtl8723au/include/hal_intf.h
index d183f4ba1ec..04223b5c047 100644
--- a/drivers/staging/rtl8723au/include/hal_intf.h
+++ b/drivers/staging/rtl8723au/include/hal_intf.h
@@ -19,10 +19,10 @@
 #include <drv_types.h>
 
 enum RTL871X_HCI_TYPE {
-	RTW_PCIE	= BIT0,
-	RTW_USB		= BIT1,
-	RTW_SDIO	= BIT2,
-	RTW_GSPI	= BIT3,
+	RTW_PCIE	= BIT(0),
+	RTW_USB		= BIT(1),
+	RTW_SDIO	= BIT(2),
+	RTW_GSPI	= BIT(3),
 };
 
 enum _CHIP_TYPE {
@@ -35,78 +35,6 @@ enum _CHIP_TYPE {
 	MAX_CHIP_TYPE
 };
 
-enum HW_VARIABLES {
-	HW_VAR_MEDIA_STATUS,
-	HW_VAR_MEDIA_STATUS1,
-	HW_VAR_SET_OPMODE,
-	HW_VAR_MAC_ADDR,
-	HW_VAR_BSSID,
-	HW_VAR_INIT_RTS_RATE,
-	HW_VAR_BASIC_RATE,
-	HW_VAR_TXPAUSE,
-	HW_VAR_BCN_FUNC,
-	HW_VAR_CORRECT_TSF,
-	HW_VAR_CHECK_BSSID,
-	HW_VAR_MLME_DISCONNECT,
-	HW_VAR_MLME_SITESURVEY,
-	HW_VAR_MLME_JOIN,
-	HW_VAR_ON_RCR_AM,
-	HW_VAR_OFF_RCR_AM,
-	HW_VAR_BEACON_INTERVAL,
-	HW_VAR_SLOT_TIME,
-	HW_VAR_RESP_SIFS,
-	HW_VAR_ACK_PREAMBLE,
-	HW_VAR_SEC_CFG,
-	HW_VAR_BCN_VALID,
-	HW_VAR_RF_TYPE,
-	HW_VAR_DM_FLAG,
-	HW_VAR_DM_FUNC_OP,
-	HW_VAR_DM_FUNC_SET,
-	HW_VAR_DM_FUNC_CLR,
-	HW_VAR_CAM_EMPTY_ENTRY,
-	HW_VAR_CAM_INVALID_ALL,
-	HW_VAR_CAM_WRITE,
-	HW_VAR_CAM_READ,
-	HW_VAR_AC_PARAM_VO,
-	HW_VAR_AC_PARAM_VI,
-	HW_VAR_AC_PARAM_BE,
-	HW_VAR_AC_PARAM_BK,
-	HW_VAR_ACM_CTRL,
-	HW_VAR_AMPDU_MIN_SPACE,
-	HW_VAR_AMPDU_FACTOR,
-	HW_VAR_RXDMA_AGG_PG_TH,
-	HW_VAR_SET_RPWM,
-	HW_VAR_H2C_FW_PWRMODE,
-	HW_VAR_H2C_FW_JOINBSSRPT,
-	HW_VAR_FWLPS_RF_ON,
-	HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
-	HW_VAR_TDLS_WRCR,
-	HW_VAR_TDLS_INIT_CH_SEN,
-	HW_VAR_TDLS_RS_RCR,
-	HW_VAR_TDLS_DONE_CH_SEN,
-	HW_VAR_INITIAL_GAIN,
-	HW_VAR_TRIGGER_GPIO_0,
-	HW_VAR_BT_SET_COEXIST,
-	HW_VAR_BT_ISSUE_DELBA,
-	HW_VAR_CURRENT_ANTENNA,
-	HW_VAR_ANTENNA_DIVERSITY_LINK,
-	HW_VAR_ANTENNA_DIVERSITY_SELECT,
-	HW_VAR_SWITCH_EPHY_WoWLAN,
-	HW_VAR_EFUSE_BYTES,
-	HW_VAR_EFUSE_BT_BYTES,
-	HW_VAR_FIFO_CLEARN_UP,
-	HW_VAR_CHECK_TXBUF,
-	HW_VAR_APFM_ON_MAC, /* Auto FSM to Turn On, include clock, isolation, power control for MAC only */
-	/*  The valid upper nav range for the HW updating, if the true value is larger than the upper range, the HW won't update it. */
-	/*  Unit in microsecond. 0 means disable this function. */
-	HW_VAR_NAV_UPPER,
-	HW_VAR_RPT_TIMER_SETTING,
-	HW_VAR_TX_RPT_MAX_MACID,
-	HW_VAR_H2C_MEDIA_STATUS_RPT,
-	HW_VAR_CHK_HI_QUEUE_EMPTY,
-	HW_VAR_READ_LLT_TAB,
-};
-
 enum hal_def_variable {
 	HAL_DEF_UNDERCORATEDSMOOTHEDPWDB,
 	HAL_DEF_IS_SUPPORT_ANT_DIV,
@@ -132,127 +60,6 @@ enum hal_odm_variable {
 	HAL_ODM_WIFI_DISPLAY_STATE,
 };
 
-enum hal_intf_ps_func {
-	HAL_USB_SELECT_SUSPEND,
-	HAL_MAX_ID,
-};
-
-struct hal_ops {
-	u32 (*hal_power_on)(struct rtw_adapter *padapter);
-	u32 (*hal_init)(struct rtw_adapter *padapter);
-	u32 (*hal_deinit)(struct rtw_adapter *padapter);
-
-	void (*free_hal_data)(struct rtw_adapter *padapter);
-
-	u32 (*inirp_init)(struct rtw_adapter *padapter);
-	u32 (*inirp_deinit)(struct rtw_adapter *padapter);
-
-	s32 (*init_xmit_priv)(struct rtw_adapter *padapter);
-	void (*free_xmit_priv)(struct rtw_adapter *padapter);
-
-	s32 (*init_recv_priv)(struct rtw_adapter *padapter);
-	void (*free_recv_priv)(struct rtw_adapter *padapter);
-
-	void (*InitSwLeds)(struct rtw_adapter *padapter);
-	void (*DeInitSwLeds)(struct rtw_adapter *padapter);
-
-	void (*dm_init)(struct rtw_adapter *padapter);
-	void (*dm_deinit)(struct rtw_adapter *padapter);
-	void (*read_chip_version)(struct rtw_adapter *padapter);
-
-	void (*init_default_value)(struct rtw_adapter *padapter);
-
-	void (*intf_chip_configure)(struct rtw_adapter *padapter);
-
-	void (*read_adapter_info)(struct rtw_adapter *padapter);
-
-	void (*enable_interrupt)(struct rtw_adapter *padapter);
-	void (*disable_interrupt)(struct rtw_adapter *padapter);
-	s32 (*interrupt_handler)(struct rtw_adapter *padapter);
-	void (*set_bwmode_handler)(struct rtw_adapter *padapter,
-				   enum ht_channel_width Bandwidth, u8 Offset);
-	void (*set_channel_handler)(struct rtw_adapter *padapter, u8 channel);
-
-	void (*hal_dm_watchdog)(struct rtw_adapter *padapter);
-
-	void (*SetHwRegHandler)(struct rtw_adapter *padapter,
-				u8 variable, u8 *val);
-	void (*GetHwRegHandler)(struct rtw_adapter *padapter,
-				u8 variable, u8 *val);
-
-	u8 (*GetHalDefVarHandler)(struct rtw_adapter *padapter,
-				  enum hal_def_variable eVariable,
-				  void *pValue);
-	u8 (*SetHalDefVarHandler)(struct rtw_adapter *padapter,
-				  enum hal_def_variable eVariable,
-				  void *pValue);
-
-	void (*GetHalODMVarHandler)(struct rtw_adapter *padapter,
-				    enum hal_odm_variable eVariable,
-				    void *pValue1, bool bSet);
-	void (*SetHalODMVarHandler)(struct rtw_adapter *padapter,
-				    enum hal_odm_variable eVariable,
-				    void *pValue1, bool bSet);
-
-	void (*UpdateRAMaskHandler)(struct rtw_adapter *padapter,
-				    u32 mac_id, u8 rssi_level);
-	void (*SetBeaconRelatedRegistersHandler)(struct rtw_adapter *padapter);
-
-	void (*Add_RateATid)(struct rtw_adapter *padapter, u32 bitmap,
-			     u8 arg, u8 rssi_level);
-	void (*run_thread)(struct rtw_adapter *padapter);
-	void (*cancel_thread)(struct rtw_adapter *padapter);
-
-	u8 (*interface_ps_func)(struct rtw_adapter *padapter,
-				enum hal_intf_ps_func efunc_id, u8 *val);
-
-	s32 (*hal_xmit)(struct rtw_adapter *padapter,
-			struct xmit_frame *pxmitframe);
-	s32 (*mgnt_xmit)(struct rtw_adapter *padapter,
-			 struct xmit_frame *pmgntframe);
-	s32 (*hal_xmitframe_enqueue)(struct rtw_adapter *padapter,
-				     struct xmit_frame *pxmitframe);
-
-	u32 (*read_bbreg)(struct rtw_adapter *padapter, u32 RegAddr,
-			  u32 BitMask);
-	void (*write_bbreg)(struct rtw_adapter *padapter, u32 RegAddr,
-			    u32 BitMask, u32 Data);
-	u32 (*read_rfreg)(struct rtw_adapter *padapter, u32 eRFPath,
-			  u32 RegAddr, u32 BitMask);
-	void (*write_rfreg)(struct rtw_adapter *padapter, u32 eRFPath,
-			    u32 RegAddr, u32 BitMask, u32 Data);
-
-	void (*EfusePowerSwitch)(struct rtw_adapter *padapter, u8 bWrite,
-				 u8 PwrState);
-	void (*ReadEFuse)(struct rtw_adapter *padapter, u8 efuseType,
-			  u16 _offset, u16 _size_byte, u8 *pbuf);
-	void (*EFUSEGetEfuseDefinition)(struct rtw_adapter *padapter,
-					u8 efuseType, u8 type, void *pOut);
-	u16 (*EfuseGetCurrentSize)(struct rtw_adapter *padapter, u8 efuseType);
-	int (*Efuse_PgPacketRead23a)(struct rtw_adapter *padapter,
-				     u8 offset, u8 *data);
-	int (*Efuse_PgPacketWrite23a)(struct rtw_adapter *padapter,
-				      u8 offset, u8 word_en, u8 *data);
-	u8 (*Efuse_WordEnableDataWrite23a)(struct rtw_adapter *padapter,
-					   u16 efuse_addr, u8 word_en,
-					   u8 *data);
-	bool (*Efuse_PgPacketWrite23a_BT)(struct rtw_adapter *padapter,
-					  u8 offset, u8 word_en, u8 *data);
-
-	void (*sreset_init_value23a)(struct rtw_adapter *padapter);
-	void (*sreset_reset_value23a)(struct rtw_adapter *padapter);
-	void (*silentreset)(struct rtw_adapter *padapter);
-	void (*sreset_xmit_status_check)(struct rtw_adapter *padapter);
-	void (*sreset_linked_status_check) (struct rtw_adapter *padapter);
-	u8 (*sreset_get_wifi_status23a)(struct rtw_adapter *padapter);
-	bool (*sreset_inprogress)(struct rtw_adapter *padapter);
-
-	void (*hal_notch_filter)(struct rtw_adapter *adapter, bool enable);
-	void (*hal_reset_security_engine)(struct rtw_adapter *adapter);
-	s32 (*c2h_handler)(struct rtw_adapter *padapter, struct c2h_evt_hdr *c2h_evt);
-	c2h_id_filter c2h_id_filter_ccx;
-};
-
 enum rt_eeprom_type {
 	EEPROM_93C46,
 	EEPROM_93C56,
@@ -262,10 +69,10 @@ enum rt_eeprom_type {
 
 
 #define RF_CHANGE_BY_INIT	0
-#define RF_CHANGE_BY_IPS	BIT28
-#define RF_CHANGE_BY_PS		BIT29
-#define RF_CHANGE_BY_HW		BIT30
-#define RF_CHANGE_BY_SW		BIT31
+#define RF_CHANGE_BY_IPS	BIT(28)
+#define RF_CHANGE_BY_PS		BIT(29)
+#define RF_CHANGE_BY_HW		BIT(30)
+#define RF_CHANGE_BY_SW		BIT(31)
 
 enum hardware_type {
 	HARDWARE_TYPE_RTL8180,
@@ -291,102 +98,26 @@ enum hardware_type {
 };
 
 #define GET_EEPROM_EFUSE_PRIV(adapter) (&adapter->eeprompriv)
-#define is_boot_from_eeprom(adapter) (adapter->eeprompriv.EepromOrEfuse)
-
-extern int rtw_ht_enable23A;
-extern int rtw_cbw40_enable23A;
-extern int rtw_ampdu_enable23A;/* for enable tx_ampdu */
 
 void rtw_hal_def_value_init23a(struct rtw_adapter *padapter);
 int pm_netdev_open23a(struct net_device *pnetdev, u8 bnormal);
 int rtw_resume_process23a(struct rtw_adapter *padapter);
 
-void	rtw_hal_free_data23a(struct rtw_adapter *padapter);
-
-void rtw_hal_dm_init23a(struct rtw_adapter *padapter);
-void rtw_hal_dm_deinit23a(struct rtw_adapter *padapter);
-void rtw_hal_sw_led_init23a(struct rtw_adapter *padapter);
-void rtw_hal_sw_led_deinit23a(struct rtw_adapter *padapter);
-
-u32 rtw_hal_power_on23a(struct rtw_adapter *padapter);
-uint rtw_hal_init23a(struct rtw_adapter *padapter);
-uint rtw_hal_deinit23a(struct rtw_adapter *padapter);
+int rtw_hal_init23a(struct rtw_adapter *padapter);
+int rtw_hal_deinit23a(struct rtw_adapter *padapter);
 void rtw_hal_stop(struct rtw_adapter *padapter);
-void rtw_hal_set_hwreg23a(struct rtw_adapter *padapter, u8 variable, u8 *val);
-void rtw23a_hal_get_hwreg(struct rtw_adapter *padapter, u8 variable, u8 *val);
-
-void rtw_hal_chip_configure23a(struct rtw_adapter *padapter);
-void rtw_hal_read_chip_info23a(struct rtw_adapter *padapter);
-void rtw_hal_read_chip_version23a(struct rtw_adapter *padapter);
-
-u8 rtw_hal_set_def_var23a(struct rtw_adapter *padapter,
-			  enum hal_def_variable eVariable,
-			  void *pValue);
-u8 rtw_hal_get_def_var23a(struct rtw_adapter *padapter,
-			  enum hal_def_variable eVariable,
-			  void *pValue);
-
-void rtw_hal_set_odm_var23a(struct rtw_adapter *padapter,
-			    enum hal_odm_variable eVariable,
-			    void *pValue1, bool bSet);
-void rtw_hal_get_odm_var23a(struct rtw_adapter *padapter,
-			    enum hal_odm_variable eVariable,
-			    void *pValue1, bool bSet);
-
-void rtw_hal_enable_interrupt23a(struct rtw_adapter *padapter);
-void rtw_hal_disable_interrupt23a(struct rtw_adapter *padapter);
-
-u32 rtw_hal_inirp_init23a(struct rtw_adapter *padapter);
-u32 rtw_hal_inirp_deinit23a(struct rtw_adapter *padapter);
-
-u8 rtw_hal_intf_ps_func23a(struct rtw_adapter *padapter,
-			   enum hal_intf_ps_func efunc_id, u8 *val);
-
-s32 rtw_hal_xmit23aframe_enqueue(struct rtw_adapter *padapter,
-				 struct xmit_frame *pxmitframe);
-s32 rtw_hal_xmit23a(struct rtw_adapter *padapter,
-		    struct xmit_frame *pxmitframe);
-s32 rtw_hal_mgnt_xmit23a(struct rtw_adapter *padapter,
-			 struct xmit_frame *pmgntframe);
-
-s32	rtw_hal_init23a_xmit_priv(struct rtw_adapter *padapter);
-void	rtw_hal_free_xmit_priv23a(struct rtw_adapter *padapter);
-
-s32	rtw_hal_init23a_recv_priv(struct rtw_adapter *padapter);
-void	rtw_hal_free_recv_priv23a(struct rtw_adapter *padapter);
 
 void rtw_hal_update_ra_mask23a(struct sta_info *psta, u8 rssi_level);
-void	rtw_hal_add_ra_tid23a(struct rtw_adapter *padapter, u32 bitmap, u8 arg, u8 rssi_level);
 void	rtw_hal_clone_data(struct rtw_adapter *dst_padapter, struct rtw_adapter *src_padapter);
-void	rtw_hal_start_thread23a(struct rtw_adapter *padapter);
-void	rtw_hal_stop_thread23a(struct rtw_adapter *padapter);
-
-void rtw_hal_bcn_related_reg_setting23a(struct rtw_adapter *padapter);
-
-u32	rtw_hal_read_bbreg23a(struct rtw_adapter *padapter, u32 RegAddr, u32 BitMask);
-void	rtw_hal_write_bbreg23a(struct rtw_adapter *padapter, u32 RegAddr, u32 BitMask, u32 Data);
-u32	rtw_hal_read_rfreg23a(struct rtw_adapter *padapter, u32 eRFPath, u32 RegAddr, u32 BitMask);
-void	rtw_hal_write_rfreg23a(struct rtw_adapter *padapter, u32 eRFPath, u32 RegAddr, u32 BitMask, u32 Data);
-
-s32	rtw_hal_interrupt_handler23a(struct rtw_adapter *padapter);
-
-void	rtw_hal_set_bwmode23a(struct rtw_adapter *padapter,
-			   enum ht_channel_width Bandwidth, u8 Offset);
-void	rtw_hal_set_chan23a(struct rtw_adapter *padapter, u8 channel);
-void	rtw_hal_dm_watchdog23a(struct rtw_adapter *padapter);
-
-void rtw_hal_sreset_init23a(struct rtw_adapter *padapter);
-void rtw_hal_sreset_reset23a(struct rtw_adapter *padapter);
-void rtw_hal_sreset_reset23a_value23a(struct rtw_adapter *padapter);
-void rtw_hal_sreset_xmit_status_check23a(struct rtw_adapter *padapter);
-void rtw_hal_sreset_linked_status_check23a (struct rtw_adapter *padapter);
-u8   rtw_hal_sreset_get_wifi_status23a(struct rtw_adapter *padapter);
-bool rtw_hal_sreset_inprogress(struct rtw_adapter *padapter);
 
-void rtw_hal_notch_filter23a(struct rtw_adapter *adapter, bool enable);
-void rtw_hal_reset_security_engine23a(struct rtw_adapter *adapter);
+void hw_var_set_correct_tsf(struct rtw_adapter *padapter);
+void hw_var_set_mlme_disconnect(struct rtw_adapter *padapter);
+void hw_var_set_opmode(struct rtw_adapter *padapter, u8 mode);
+void hw_var_set_macaddr(struct rtw_adapter *padapter, u8 *val);
+void hw_var_set_bssid(struct rtw_adapter *padapter, u8 *val);
+void hw_var_set_mlme_join(struct rtw_adapter *padapter, u8 type);
 
-s32 rtw_hal_c2h_handler23a(struct rtw_adapter *adapter, struct c2h_evt_hdr *c2h_evt);
-c2h_id_filter rtw_hal_c2h_id_filter_ccx23a(struct rtw_adapter *adapter);
+int GetHalDefVar8192CUsb(struct rtw_adapter *Adapter,
+			 enum hal_def_variable eVariable, void *pValue);
 
 #endif /* __HAL_INTF_H__ */
diff --git a/drivers/staging/rtl8723au/include/ieee80211.h b/drivers/staging/rtl8723au/include/ieee80211.h
index 28e4ab239fb..69c0f5c94fc 100644
--- a/drivers/staging/rtl8723au/include/ieee80211.h
+++ b/drivers/staging/rtl8723au/include/ieee80211.h
@@ -58,7 +58,7 @@
 
 #define WPA_CIPHER_NONE		BIT(0)
 #define WPA_CIPHER_WEP40	BIT(1)
-#define WPA_CIPHER_WEP104 BIT(2)
+#define WPA_CIPHER_WEP104	BIT(2)
 #define WPA_CIPHER_TKIP		BIT(3)
 #define WPA_CIPHER_CCMP		BIT(4)
 
@@ -105,41 +105,53 @@ enum ratr_table_mode {
 
 enum NETWORK_TYPE
 {
-    WIRELESS_INVALID = 0,
-    /* Sub-Element */
-    WIRELESS_11B = BIT(0), /*  tx: cck only , rx: cck only, hw: cck */
-    WIRELESS_11G = BIT(1), /*  tx: ofdm only, rx: ofdm & cck, hw: cck & ofdm */
-    WIRELESS_11A = BIT(2), /*  tx: ofdm only, rx: ofdm only, hw: ofdm only */
-    WIRELESS_11_24N = BIT(3), /*  tx: MCS only, rx: MCS & cck, hw: MCS & cck */
-    WIRELESS_11_5N = BIT(4), /*  tx: MCS only, rx: MCS & ofdm, hw: ofdm only */
+	WIRELESS_INVALID = 0,
+	/* Sub-Element */
+	/*  tx: cck only , rx: cck only, hw: cck */
+	WIRELESS_11B = BIT(0),
+	/*  tx: ofdm only, rx: ofdm & cck, hw: cck & ofdm */
+	WIRELESS_11G = BIT(1),
+	/*  tx: ofdm only, rx: ofdm only, hw: ofdm only */
+	WIRELESS_11A = BIT(2),
+	/*  tx: MCS only, rx: MCS & cck, hw: MCS & cck */
+	WIRELESS_11_24N = BIT(3),
+	/*  tx: MCS only, rx: MCS & ofdm, hw: ofdm only */
+	WIRELESS_11_5N = BIT(4),
 	/* WIRELESS_AUTO		= BIT(5), */
-	WIRELESS_AC		= BIT(6),
-
-    /* Combination */
-    WIRELESS_11BG = (WIRELESS_11B|WIRELESS_11G), /*  tx: cck & ofdm, rx: cck & ofdm & MCS, hw: cck & ofdm */
-    WIRELESS_11G_24N = (WIRELESS_11G|WIRELESS_11_24N), /*  tx: ofdm & MCS, rx: ofdm & cck & MCS, hw: cck & ofdm */
-    WIRELESS_11A_5N = (WIRELESS_11A|WIRELESS_11_5N), /*  tx: ofdm & MCS, rx: ofdm & MCS, hw: ofdm only */
-    WIRELESS_11BG_24N = (WIRELESS_11B|WIRELESS_11G|WIRELESS_11_24N), /*  tx: ofdm & cck & MCS, rx: ofdm & cck & MCS, hw: ofdm & cck */
-    WIRELESS_11AGN = (WIRELESS_11A|WIRELESS_11G|WIRELESS_11_24N|WIRELESS_11_5N), /*  tx: ofdm & MCS, rx: ofdm & MCS, hw: ofdm only */
-    WIRELESS_11ABGN = (WIRELESS_11A|WIRELESS_11B|WIRELESS_11G|WIRELESS_11_24N|WIRELESS_11_5N),
+	WIRELESS_AC = BIT(6),
+
+	/* Combination */
+	 /*  tx: cck & ofdm, rx: cck & ofdm & MCS, hw: cck & ofdm */
+	WIRELESS_11BG = WIRELESS_11B|WIRELESS_11G,
+	/*  tx: ofdm & MCS, rx: ofdm & cck & MCS, hw: cck & ofdm */
+	WIRELESS_11G_24N = WIRELESS_11G | WIRELESS_11_24N,
+	/*  tx: ofdm & MCS, rx: ofdm & MCS, hw: ofdm only */
+	WIRELESS_11A_5N = WIRELESS_11A | WIRELESS_11_5N,
+	/*  tx: ofdm & cck & MCS, rx: ofdm & cck & MCS, hw: ofdm & cck */
+	WIRELESS_11BG_24N = WIRELESS_11B | WIRELESS_11G | WIRELESS_11_24N,
+	/*  tx: ofdm & MCS, rx: ofdm & MCS, hw: ofdm only */
+	WIRELESS_11AGN = WIRELESS_11A | WIRELESS_11G | WIRELESS_11_24N |
+				WIRELESS_11_5N,
+	WIRELESS_11ABGN = WIRELESS_11A | WIRELESS_11B | WIRELESS_11G |
+				WIRELESS_11_24N | WIRELESS_11_5N,
 };
 
 #define SUPPORTED_24G_NETTYPE_MSK (WIRELESS_11B | WIRELESS_11G | WIRELESS_11_24N)
 #define SUPPORTED_5G_NETTYPE_MSK (WIRELESS_11A | WIRELESS_11_5N)
 
-#define IsSupported24G(NetType) ((NetType) & SUPPORTED_24G_NETTYPE_MSK ? true : false)
-#define IsSupported5G(NetType) ((NetType) & SUPPORTED_5G_NETTYPE_MSK ? true : false)
+#define IsSupported24G(NetType) (NetType & SUPPORTED_24G_NETTYPE_MSK ? true : false)
+#define IsSupported5G(NetType) (NetType & SUPPORTED_5G_NETTYPE_MSK ? true : false)
 
 #define IsEnableHWCCK(NetType) IsSupported24G(NetType)
-#define IsEnableHWOFDM(NetType) ((NetType) & (WIRELESS_11G|WIRELESS_11_24N|SUPPORTED_5G_NETTYPE_MSK) ? true : false)
+#define IsEnableHWOFDM(NetType) (NetType & (WIRELESS_11G|WIRELESS_11_24N|SUPPORTED_5G_NETTYPE_MSK) ? true : false)
 
 #define IsSupportedRxCCK(NetType) IsEnableHWCCK(NetType)
 #define IsSupportedRxOFDM(NetType) IsEnableHWOFDM(NetType)
 #define IsSupportedRxMCS(NetType) IsEnableHWOFDM(NetType)
 
-#define IsSupportedTxCCK(NetType) ((NetType) & (WIRELESS_11B) ? true : false)
-#define IsSupportedTxOFDM(NetType) ((NetType) & (WIRELESS_11G|WIRELESS_11A) ? true : false)
-#define IsSupportedTxMCS(NetType) ((NetType) & (WIRELESS_11_24N|WIRELESS_11_5N) ? true : false)
+#define IsSupportedTxCCK(NetType) (NetType & (WIRELESS_11B) ? true : false)
+#define IsSupportedTxOFDM(NetType) (NetType & (WIRELESS_11G|WIRELESS_11A) ? true : false)
+#define IsSupportedTxMCS(NetType) (NetType & (WIRELESS_11_24N|WIRELESS_11_5N) ? true : false)
 
 
 struct ieee_param {
@@ -192,7 +204,7 @@ struct ieee_param {
 /* QoS,QOS */
 #define NORMAL_ACK			0
 #define NO_ACK				1
-#define NON_EXPLICIT_ACK	2
+#define NON_EXPLICIT_ACK		2
 #define BLOCK_ACK			3
 
 /* IEEE 802.11 defines */
@@ -200,24 +212,22 @@ struct ieee_param {
 #define P80211_OUI_LEN 3
 
 struct ieee80211_snap_hdr {
-
         u8    dsap;   /* always 0xAA */
         u8    ssap;   /* always 0xAA */
         u8    ctrl;   /* always 0x03 */
         u8    oui[P80211_OUI_LEN];    /* organizational universal id */
-
 } __attribute__ ((packed));
 
 
 #define SNAP_SIZE sizeof(struct ieee80211_snap_hdr)
 
-#define WLAN_FC_GET_TYPE(fc) ((fc) & IEEE80211_FCTL_FTYPE)
-#define WLAN_FC_GET_STYPE(fc) ((fc) & IEEE80211_FCTL_STYPE)
+#define WLAN_FC_GET_TYPE(fc)		(fc & IEEE80211_FCTL_FTYPE)
+#define WLAN_FC_GET_STYPE(fc)		(fc & IEEE80211_FCTL_STYPE)
 
-#define WLAN_QC_GET_TID(qc) ((qc) & 0x0f)
+#define WLAN_QC_GET_TID(qc)		(qc & 0x0f)
 
-#define WLAN_GET_SEQ_FRAG(seq) ((seq) & RTW_IEEE80211_SCTL_FRAG)
-#define WLAN_GET_SEQ_SEQ(seq)  ((seq) & RTW_IEEE80211_SCTL_SEQ)
+#define WLAN_GET_SEQ_FRAG(seq)		(seq & RTW_IEEE80211_SCTL_FRAG)
+#define WLAN_GET_SEQ_SEQ(seq)		(seq & RTW_IEEE80211_SCTL_SEQ)
 
 
 #define WLAN_REASON_JOIN_WRONG_CHANNEL       65534
@@ -298,67 +308,20 @@ struct ieee80211_snap_hdr {
 #define WEP_KEY_LEN 13
 
 
-
-/*
-
- 802.11 data frame from AP
-
-      ,-------------------------------------------------------------------.
-Bytes |  2   |  2   |    6    |    6    |    6    |  2   | 0..2312 |   4  |
-      |------|------|---------|---------|---------|------|---------|------|
-Desc. | ctrl | dura |  DA/RA  |   TA    |    SA   | Sequ |  frame  |  fcs |
-      |      | tion | (BSSID) |         |         | ence |  data   |      |
-      `-------------------------------------------------------------------'
-
-Total: 28-2340 bytes
-
-*/
-
-struct ieee80211_header_data {
-	u16 frame_ctl;
-	u16 duration_id;
-	u8 addr1[6];
-	u8 addr2[6];
-	u8 addr3[6];
-	u16 seq_ctrl;
-};
-
-struct ieee80211_info_element_hdr {
-	u8 id;
-	u8 len;
-} __attribute__ ((packed));
-
-struct ieee80211_info_element {
-	u8 id;
-	u8 len;
-	u8 data[0];
-} __attribute__ ((packed));
-
-
-struct ieee80211_txb {
-	u8 nr_frags;
-	u8 encrypted;
-	u16 reserved;
-	u16 frag_size;
-	u16 payload_size;
-	struct sk_buff *fragments[0];
-};
-
-
 /* MAX_RATES_LENGTH needs to be 12.  The spec says 8, and many APs
  * only use 8, and then use extended rates for the remaining supported
  * rates.  Other APs, however, stick all of their supported rates on the
  * main rates information element... */
-#define MAX_RATES_LENGTH                  ((u8)12)
-#define MAX_RATES_EX_LENGTH               ((u8)16)
-#define MAX_CHANNEL_NUMBER                 161
+#define MAX_RATES_LENGTH	12
+#define MAX_RATES_EX_LENGTH	16
+#define MAX_CHANNEL_NUMBER	161
 
-#define MAX_WPA_IE_LEN (256)
-#define MAX_WPS_IE_LEN (512)
-#define MAX_P2P_IE_LEN (256)
-#define MAX_WFD_IE_LEN (128)
+#define MAX_WPA_IE_LEN		256
+#define MAX_WPS_IE_LEN		512
+#define MAX_P2P_IE_LEN		256
+#define MAX_WFD_IE_LEN		128
 
-#define IW_ESSID_MAX_SIZE 32
+#define IW_ESSID_MAX_SIZE	32
 
 /*
 join_res:
@@ -367,24 +330,11 @@ join_res:
 > 0: TID
 */
 
-#define DEFAULT_MAX_SCAN_AGE (15 * HZ)
-#define DEFAULT_FTS 2346
 #define MAC_FMT "%02x:%02x:%02x:%02x:%02x:%02x"
 #define MAC_ARG(x) ((u8*)(x))[0],((u8*)(x))[1],((u8*)(x))[2],((u8*)(x))[3],((u8*)(x))[4],((u8*)(x))[5]
 
-#define CFG_IEEE80211_RESERVE_FCS (1<<0)
-#define CFG_IEEE80211_COMPUTE_FCS (1<<1)
-
 #define MAXTID	16
 
-#define IEEE_A            (1<<0)
-#define IEEE_B            (1<<1)
-#define IEEE_G            (1<<2)
-#define IEEE_MODE_MASK    (IEEE_A|IEEE_B|IEEE_G)
-
-/* Baron move to ieee80211.c */
-int ieee80211_is_empty_essid23a(const char *essid, int essid_len);
-
 enum _PUBLIC_ACTION{
 	ACT_PUBLIC_BSSCOEXIST = 0, /*  20/40 BSS Coexistence */
 	ACT_PUBLIC_DSE_ENABLE = 1,
@@ -457,79 +407,17 @@ struct rtw_ieee80211_channel {
 	/*, (channel)->orig_mag*/ \
 	/*, (channel)->orig_mpwr*/ \
 
-/* Parsed Information Elements */
-struct rtw_ieee802_11_elems {
-	u8 *ssid;
-	u8 ssid_len;
-	u8 *supp_rates;
-	u8 supp_rates_len;
-	u8 *fh_params;
-	u8 fh_params_len;
-	u8 *ds_params;
-	u8 ds_params_len;
-	u8 *cf_params;
-	u8 cf_params_len;
-	u8 *tim;
-	u8 tim_len;
-	u8 *ibss_params;
-	u8 ibss_params_len;
-	u8 *challenge;
-	u8 challenge_len;
-	u8 *erp_info;
-	u8 erp_info_len;
-	u8 *ext_supp_rates;
-	u8 ext_supp_rates_len;
-	u8 *wpa_ie;
-	u8 wpa_ie_len;
-	u8 *rsn_ie;
-	u8 rsn_ie_len;
-	u8 *wme;
-	u8 wme_len;
-	u8 *wme_tspec;
-	u8 wme_tspec_len;
-	u8 *wps_ie;
-	u8 wps_ie_len;
-	u8 *power_cap;
-	u8 power_cap_len;
-	u8 *supp_channels;
-	u8 supp_channels_len;
-	u8 *mdie;
-	u8 mdie_len;
-	u8 *ftie;
-	u8 ftie_len;
-	u8 *timeout_int;
-	u8 timeout_int_len;
-	u8 *ht_capabilities;
-	u8 ht_capabilities_len;
-	u8 *ht_operation;
-	u8 ht_operation_len;
-	u8 *vendor_ht_cap;
-	u8 vendor_ht_cap_len;
-};
-
-enum parse_res {
-	ParseOK = 0,
-	ParseUnknown = 1,
-	ParseFailed = -1
-};
-
-enum parse_res rtw_ieee802_11_parse_elems23a(u8 *start, uint len,
-				struct rtw_ieee802_11_elems *elems,
-				int show_errors);
-
 u8 *rtw_set_fixed_ie23a(unsigned char *pbuf, unsigned int len, unsigned char *source, unsigned int *frlen);
-u8 *rtw_set_ie23a(u8 *pbuf, int index, uint len, u8 *source, uint *frlen);
+u8 *rtw_set_ie23a(u8 *pbuf, int index, uint len, const u8 *source, uint *frlen);
 
 enum secondary_ch_offset {
 	SCN = 0, /* no secondary channel */
 	SCA = 1, /* secondary channel above */
 	SCB = 3,  /* secondary channel below */
 };
-u8 secondary_ch_offset_to_hal_ch_offset23a(u8 ch_offset);
 u8 hal_ch_offset_to_secondary_ch_offset23a(u8 ch_offset);
 u8 *rtw_set_ie23a_ch_switch(u8 *buf, u32 *buf_len, u8 ch_switch_mode, u8 new_ch, u8 ch_switch_cnt);
 u8 *rtw_set_ie23a_secondary_ch_offset(u8 *buf, u32 *buf_len, u8 secondary_ch_offset);
-u8 *rtw_set_ie23a_mesh_ch_switch_parm(u8 *buf, u32 *buf_len, u8 ttl, u8 flags, u16 reason, u16 precedence);
 
 u8 *rtw_get_ie23a(u8*pbuf, int index, int *len, int limit);
 u8 *rtw_get_ie23a_ex(u8 *in_ie, uint in_len, u8 eid, u8 *oui, u8 oui_len, u8 *ie, uint *ielen);
@@ -537,46 +425,17 @@ int rtw_ies_remove_ie23a(u8 *ies, uint *ies_len, uint offset, u8 eid, u8 *oui, u
 
 void rtw_set_supported_rate23a(u8* SupportedRates, uint mode) ;
 
-unsigned char *rtw_get_wpa_ie23a(unsigned char *pie, int *wpa_ie_len, int limit);
-unsigned char *rtw_get_wpa2_ie23a(unsigned char *pie, int *rsn_ie_len, int limit);
-int rtw_get_wpa_cipher_suite23a(u8 *s);
-int rtw_get_wpa2_cipher_suite23a(u8 *s);
-int rtw_parse_wpa_ie23a(u8* wpa_ie, int wpa_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x);
-int rtw_parse_wpa2_ie23a(u8* wpa_ie, int wpa_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x);
+int rtw_get_wpa_cipher_suite23a(const u8 *s);
+int rtw_get_wpa2_cipher_suite23a(const u8 *s);
+int rtw_parse_wpa_ie23a(const u8* wpa_ie, int wpa_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x);
+int rtw_parse_wpa2_ie23a(const u8* wpa_ie, int wpa_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x);
 
 int rtw_get_sec_ie23a(u8 *in_ie,uint in_len,u8 *rsn_ie,u16 *rsn_len,u8 *wpa_ie,u16 *wpa_len);
 
-u8 rtw_is_wps_ie23a(u8 *ie_ptr, uint *wps_ielen);
 u8 *rtw_get_wps_ie23a(u8 *in_ie, uint in_len, u8 *wps_ie, uint *wps_ielen);
 u8 *rtw_get_wps_attr23a(u8 *wps_ie, uint wps_ielen, u16 target_attr_id ,u8 *buf_attr, u32 *len_attr);
 u8 *rtw_get_wps_attr_content23a(u8 *wps_ie, uint wps_ielen, u16 target_attr_id ,u8 *buf_content, uint *len_content);
 
-/**
- * for_each_ie - iterate over continuous IEs
- * @ie:
- * @buf:
- * @buf_len:
- */
-#define for_each_ie(ie, buf, buf_len) \
-	for (ie = (void*)buf; (((u8*)ie) - ((u8*)buf) + 1) < buf_len; ie = (void*)(((u8*)ie) + *(((u8*)ie)+1) + 2))
-
-void dump_ies23a(u8 *buf, u32 buf_len);
-void dump_wps_ie23a(u8 *ie, u32 ie_len);
-
-#ifdef CONFIG_8723AU_P2P
-void dump_p2p_ie23a(u8 *ie, u32 ie_len);
-u8 *rtw_get_p2p_ie23a(u8 *in_ie, int in_len, u8 *p2p_ie, uint *p2p_ielen);
-u8 *rtw_get_p2p_attr23a(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id ,u8 *buf_attr, u32 *len_attr);
-u8 *rtw_get_p2p_attr23a_content(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id ,u8 *buf_content, uint *len_content);
-u32 rtw_set_p2p_attr_content23a(u8 *pbuf, u8 attr_id, u16 attr_len, u8 *pdata_attr);
-void rtw_wlan_bssid_ex_remove_p2p_attr23a(struct wlan_bssid_ex *bss_ex, u8 attr_id);
-#endif
-
-#ifdef CONFIG_8723AU_P2P
-int rtw_get_wfd_ie(u8 *in_ie, int in_len, u8 *wfd_ie, uint *wfd_ielen);
-int rtw_get_wfd_attr_content(u8 *wfd_ie, uint wfd_ielen, u8 target_attr_id ,u8 *attr_content, uint *attr_contentlen);
-#endif /*  CONFIG_8723AU_P2P */
-
 uint	rtw_get_rateset_len23a(u8	*rateset);
 
 struct registry_priv;
@@ -585,19 +444,12 @@ int rtw_generate_ie23a(struct registry_priv *pregistrypriv);
 
 int rtw_get_bit_value_from_ieee_value23a(u8 val);
 
-uint rtw_is_cckrates_included23a(u8 *rate);
-
-uint rtw_is_cckratesonly_included23a(u8 *rate);
-
 int rtw_check_network_type23a(unsigned char *rate, int ratelen, int channel);
 
 void rtw_get_bcn_info23a(struct wlan_network *pnetwork);
 
-void rtw_macaddr_cfg23a(u8 *mac_addr);
-
 u16 rtw_mcs_rate23a(u8 rf_type, u8 bw_40MHz, u8 short_GI_20, u8 short_GI_40, unsigned char * MCS_rate);
 
-int rtw_action_frame_parse23a(const u8 *frame, u32 frame_len, u8* category, u8 *action);
 const char *action_public_str23a(u8 action);
 
 #endif /* IEEE80211_H */
diff --git a/drivers/staging/rtl8723au/include/ioctl_cfg80211.h b/drivers/staging/rtl8723au/include/ioctl_cfg80211.h
index 0eb9036d725..63e921f921d 100644
--- a/drivers/staging/rtl8723au/include/ioctl_cfg80211.h
+++ b/drivers/staging/rtl8723au/include/ioctl_cfg80211.h
@@ -15,23 +15,6 @@
 #ifndef __IOCTL_CFG80211_H__
 #define __IOCTL_CFG80211_H__
 
-struct rtw_wdev_invit_info {
-	u8 token;
-	u8 flags;
-	u8 status;
-	u8 req_op_ch;
-	u8 rsp_op_ch;
-};
-
-#define rtw_wdev_invit_info_init(invit_info) \
-	do { \
-		(invit_info)->token = 0; \
-		(invit_info)->flags = 0x00; \
-		(invit_info)->status = 0xff; \
-		(invit_info)->req_op_ch = 0; \
-		(invit_info)->rsp_op_ch = 0; \
-	} while (0)
-
 struct rtw_wdev_priv {
 	struct wireless_dev *rtw_wdev;
 
@@ -45,11 +28,6 @@ struct rtw_wdev_priv {
 
 	u8 p2p_enabled;
 
-	u8 provdisc_req_issued;
-
-	struct rtw_wdev_invit_info invit_info;
-
-	bool block;
 	bool power_mgmt;
 };
 
@@ -83,37 +61,9 @@ void rtw_cfg80211_indicate_sta_disassoc(struct rtw_adapter *padapter,
 					unsigned char *da, unsigned short reason);
 #endif /* CONFIG_8723AU_AP_MODE */
 
-void rtw_cfg80211_issue_p2p_provision_request23a(struct rtw_adapter *padapter,
-					      const u8 *buf, size_t len);
-void rtw_cfg80211_rx_p2p_action_public(struct rtw_adapter *padapter,
-				       u8 *pmgmt_frame, uint frame_len);
-void rtw_cfg80211_rx_action_p2p(struct rtw_adapter *padapter,
-				u8 *pmgmt_frame, uint frame_len);
 void rtw_cfg80211_rx_action(struct rtw_adapter *adapter, u8 *frame,
 			    uint frame_len, const char*msg);
 
-int rtw_cfg80211_set_mgnt_wpsp2pie(struct net_device *net, char *buf, int len,
-				   int type);
-
 bool rtw_cfg80211_pwr_mgmt(struct rtw_adapter *adapter);
 
-#define rtw_cfg80211_rx_mgmt(adapter, freq, sig_dbm, buf, len, gfp)	\
-	cfg80211_rx_mgmt((adapter)->rtw_wdev, freq, sig_dbm, buf, len, 0, gfp)
-
-#define rtw_cfg80211_send_rx_assoc(adapter, bss, buf, len)		\
-	cfg80211_send_rx_assoc((adapter)->pnetdev, bss, buf, len)
-
-#define rtw_cfg80211_mgmt_tx_status(adapter, cookie, buf, len, ack, gfp) \
-	cfg80211_mgmt_tx_status((adapter)->rtw_wdev, cookie, buf,	\
-				len, ack, gfp)
-
-#define rtw_cfg80211_ready_on_channel(adapter, cookie, chan,		\
-				      channel_type, duration, gfp)	\
-	cfg80211_ready_on_channel((adapter)->rtw_wdev, cookie, chan,	\
-				  duration, gfp)
-#define rtw_cfg80211_remain_on_channel_expired(adapter, cookie, chan,	\
-					       chan_type, gfp)		\
-	cfg80211_remain_on_channel_expired((adapter)->rtw_wdev,		\
-					   cookie, chan, gfp)
-
 #endif /* __IOCTL_CFG80211_H__ */
diff --git a/drivers/staging/rtl8723au/include/mlme_osdep.h b/drivers/staging/rtl8723au/include/mlme_osdep.h
index b7132a9a137..4bb5525b7a6 100644
--- a/drivers/staging/rtl8723au/include/mlme_osdep.h
+++ b/drivers/staging/rtl8723au/include/mlme_osdep.h
@@ -19,10 +19,6 @@
 #include <drv_types.h>
 
 void rtw_os_indicate_disconnect23a(struct rtw_adapter *adapter);
-void rtw_os_indicate_connect23a(struct rtw_adapter *adapter);
-void rtw_os_indicate_scan_done23a(struct rtw_adapter *padapter, bool aborted);
-void rtw_report_sec_ie23a(struct rtw_adapter *adapter, u8 authmode, u8 *sec_ie);
-
 void rtw_reset_securitypriv23a(struct rtw_adapter *adapter);
 
 #endif	/* _MLME_OSDEP_H_ */
diff --git a/drivers/staging/rtl8723au/include/mp_custom_oid.h b/drivers/staging/rtl8723au/include/mp_custom_oid.h
deleted file mode 100644
index da197cf678a..00000000000
--- a/drivers/staging/rtl8723au/include/mp_custom_oid.h
+++ /dev/null
@@ -1,342 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
- *
- * 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.
- *
- * 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	__CUSTOM_OID_H
-#define __CUSTOM_OID_H
-
-/*  0xFF818000 - 0xFF81802F		RTL8180 Mass Production Kit */
-/*  0xFF818500 - 0xFF81850F		RTL8185 Setup Utility */
-/*  0xFF818580 - 0xFF81858F		RTL8185 Phy Status Utility */
-
-/*  For Production Kit with Agilent Equipments */
-/*  in order to make our custom oids hopefully somewhat unique */
-/*  we will use 0xFF (indicating implementation specific OID) */
-/*                81(first byte of non zero Realtek unique identifier) */
-/*                80 (second byte of non zero Realtek unique identifier) */
-/*                XX (the custom OID number - providing 255 possible custom oids) */
-
-#define OID_RT_PRO_RESET_DUT		0xFF818000
-#define OID_RT_PRO_SET_DATA_RATE		0xFF818001
-#define OID_RT_PRO_START_TEST		0xFF818002
-#define OID_RT_PRO_STOP_TEST		0xFF818003
-#define OID_RT_PRO_SET_PREAMBLE		0xFF818004
-#define OID_RT_PRO_SET_SCRAMBLER		0xFF818005
-#define OID_RT_PRO_SET_FILTER_BB		0xFF818006
-#define OID_RT_PRO_SET_MANUAL_DIVERSITY_BB		0xFF818007
-#define OID_RT_PRO_SET_CHANNEL_DIRECT_CALL		0xFF818008
-#define OID_RT_PRO_SET_SLEEP_MODE_DIRECT_CALL		0xFF818009
-#define OID_RT_PRO_SET_WAKE_MODE_DIRECT_CALL		0xFF81800A
-
-#define OID_RT_PRO_SET_TX_ANTENNA_BB		0xFF81800D
-#define OID_RT_PRO_SET_ANTENNA_BB		0xFF81800E
-#define OID_RT_PRO_SET_CR_SCRAMBLER		0xFF81800F
-#define OID_RT_PRO_SET_CR_NEW_FILTER		0xFF818010
-#define OID_RT_PRO_SET_TX_POWER_CONTROL		0xFF818011
-#define OID_RT_PRO_SET_CR_TX_CONFIG		0xFF818012
-#define OID_RT_PRO_GET_TX_POWER_CONTROL		0xFF818013
-#define OID_RT_PRO_GET_CR_SIGNAL_QUALITY		0xFF818014
-#define OID_RT_PRO_SET_CR_SETPOINT		0xFF818015
-#define OID_RT_PRO_SET_INTEGRATOR		0xFF818016
-#define OID_RT_PRO_SET_SIGNAL_QUALITY		0xFF818017
-#define OID_RT_PRO_GET_INTEGRATOR		0xFF818018
-#define OID_RT_PRO_GET_SIGNAL_QUALITY		0xFF818019
-#define OID_RT_PRO_QUERY_EEPROM_TYPE		0xFF81801A
-#define OID_RT_PRO_WRITE_MAC_ADDRESS		0xFF81801B
-#define OID_RT_PRO_READ_MAC_ADDRESS		0xFF81801C
-#define OID_RT_PRO_WRITE_CIS_DATA		0xFF81801D
-#define OID_RT_PRO_READ_CIS_DATA		0xFF81801E
-#define OID_RT_PRO_WRITE_POWER_CONTROL		0xFF81801F
-#define OID_RT_PRO_READ_POWER_CONTROL		0xFF818020
-#define OID_RT_PRO_WRITE_EEPROM		0xFF818021
-#define OID_RT_PRO_READ_EEPROM		0xFF818022
-#define OID_RT_PRO_RESET_TX_PACKET_SENT		0xFF818023
-#define OID_RT_PRO_QUERY_TX_PACKET_SENT		0xFF818024
-#define OID_RT_PRO_RESET_RX_PACKET_RECEIVED		0xFF818025
-#define OID_RT_PRO_QUERY_RX_PACKET_RECEIVED		0xFF818026
-#define OID_RT_PRO_QUERY_RX_PACKET_CRC32_ERROR		0xFF818027
-#define OID_RT_PRO_QUERY_CURRENT_ADDRESS		0xFF818028
-#define OID_RT_PRO_QUERY_PERMANENT_ADDRESS		0xFF818029
-#define OID_RT_PRO_SET_PHILIPS_RF_PARAMETERS		0xFF81802A
-#define OID_RT_PRO_RECEIVE_PACKET		0xFF81802C
-/*  added by Owen on 04/08/03 for Cameo's request */
-#define OID_RT_PRO_WRITE_EEPROM_BYTE		0xFF81802D
-#define OID_RT_PRO_READ_EEPROM_BYTE		0xFF81802E
-#define OID_RT_PRO_SET_MODULATION		0xFF81802F
-/*  */
-
-#define OID_RT_DRIVER_OPTION		0xFF818080
-#define OID_RT_RF_OFF		0xFF818081
-#define OID_RT_AUTH_STATUS		0xFF818082
-
-/*  */
-#define OID_RT_PRO_SET_CONTINUOUS_TX		0xFF81800B
-#define OID_RT_PRO_SET_SINGLE_CARRIER_TX		0xFF81800C
-#define OID_RT_PRO_SET_CARRIER_SUPPRESSION_TX		0xFF81802B
-#define OID_RT_PRO_SET_SINGLE_TONE_TX		0xFF818043
-/*  */
-
-
-/*  by Owen for RTL8185 Phy Status Report Utility */
-#define OID_RT_UTILITYfalse_ALARM_COUNTERS		0xFF818580
-#define OID_RT_UTILITY_SELECT_DEBUG_MODE		0xFF818581
-#define OID_RT_UTILITY_SELECT_SUBCARRIER_NUMBER		0xFF818582
-#define OID_RT_UTILITY_GET_RSSI_STATUS		0xFF818583
-#define OID_RT_UTILITY_GET_FRAME_DETECTION_STATUS		0xFF818584
-#define OID_RT_UTILITY_GET_AGC_AND_FREQUENCY_OFFSET_ESTIMATION_STATUS	0xFF818585
-#define OID_RT_UTILITY_GET_CHANNEL_ESTIMATION_STATUS		0xFF818586
-
-/*  by Owen on 03/09/19-03/09/22 for RTL8185 */
-#define OID_RT_WIRELESS_MODE		0xFF818500
-#define OID_RT_SUPPORTED_RATES		0xFF818501
-#define OID_RT_DESIRED_RATES		0xFF818502
-#define OID_RT_WIRELESS_MODE_STARTING_ADHOC		0xFF818503
-/*  */
-
-#define OID_RT_GET_CONNECT_STATE		0xFF030001
-#define OID_RT_RESCAN		0xFF030002
-#define OID_RT_SET_KEY_LENGTH		0xFF030003
-#define OID_RT_SET_DEFAULT_KEY_ID		0xFF030004
-
-#define OID_RT_SET_CHANNEL		0xFF010182
-#define OID_RT_SET_SNIFFER_MODE		0xFF010183
-#define OID_RT_GET_SIGNAL_QUALITY		0xFF010184
-#define OID_RT_GET_SMALL_PACKET_CRC		0xFF010185
-#define OID_RT_GET_MIDDLE_PACKET_CRC		0xFF010186
-#define OID_RT_GET_LARGE_PACKET_CRC		0xFF010187
-#define OID_RT_GET_TX_RETRY		0xFF010188
-#define OID_RT_GET_RX_RETRY		0xFF010189
-#define OID_RT_PRO_SET_FW_DIG_STATE		0xFF01018A/* S */
-#define OID_RT_PRO_SET_FW_RA_STATE		0xFF01018B/* S */
-
-#define OID_RT_GET_RX_TOTAL_PACKET		0xFF010190
-#define OID_RT_GET_TX_BEACON_OK		0xFF010191
-#define OID_RT_GET_TX_BEACON_ERR		0xFF010192
-#define OID_RT_GET_RX_ICV_ERR		0xFF010193
-#define OID_RT_SET_ENCRYPTION_ALGORITHM		0xFF010194
-#define OID_RT_SET_NO_AUTO_RESCAN		0xFF010195
-#define OID_RT_GET_PREAMBLE_MODE		0xFF010196
-#define OID_RT_GET_DRIVER_UP_DELTA_TIME		0xFF010197
-#define OID_RT_GET_AP_IP		0xFF010198
-#define OID_RT_GET_CHANNELPLAN		0xFF010199
-#define OID_RT_SET_PREAMBLE_MODE		0xFF01019A
-#define OID_RT_SET_BCN_INTVL		0xFF01019B
-#define OID_RT_GET_RF_VENDER		0xFF01019C
-#define OID_RT_DEDICATE_PROBE		0xFF01019D
-#define OID_RT_PRO_RX_FILTER_PATTERN		0xFF01019E
-
-#define OID_RT_GET_DCST_CURRENT_THRESHOLD		0xFF01019F
-
-#define OID_RT_GET_CCA_ERR		0xFF0101A0
-#define OID_RT_GET_CCA_UPGRADE_THRESHOLD		0xFF0101A1
-#define OID_RT_GET_CCA_FALLBACK_THRESHOLD		0xFF0101A2
-
-#define OID_RT_GET_CCA_UPGRADE_EVALUATE_TIMES		0xFF0101A3
-#define OID_RT_GET_CCA_FALLBACK_EVALUATE_TIMES		0xFF0101A4
-
-/*  by Owen on 03/31/03 for Cameo's request */
-#define OID_RT_SET_RATE_ADAPTIVE		0xFF0101A5
-/*  */
-#define OID_RT_GET_DCST_EVALUATE_PERIOD		0xFF0101A5
-#define OID_RT_GET_DCST_TIME_UNIT_INDEX		0xFF0101A6
-#define OID_RT_GET_TOTAL_TX_BYTES		0xFF0101A7
-#define OID_RT_GET_TOTAL_RX_BYTES		0xFF0101A8
-#define OID_RT_CURRENT_TX_POWER_LEVEL		0xFF0101A9
-#define OID_RT_GET_ENC_KEY_MISMATCH_COUNT		0xFF0101AA
-#define OID_RT_GET_ENC_KEY_MATCH_COUNT		0xFF0101AB
-#define OID_RT_GET_CHANNEL		0xFF0101AC
-
-#define OID_RT_SET_CHANNELPLAN		0xFF0101AD
-#define OID_RT_GET_HARDWARE_RADIO_OFF		0xFF0101AE
-#define OID_RT_CHANNELPLAN_BY_COUNTRY		0xFF0101AF
-#define OID_RT_SCAN_AVAILABLE_BSSID		0xFF0101B0
-#define OID_RT_GET_HARDWARE_VERSION		0xFF0101B1
-#define OID_RT_GET_IS_ROAMING		0xFF0101B2
-#define OID_RT_GET_IS_PRIVACY		0xFF0101B3
-#define OID_RT_GET_KEY_MISMATCH		0xFF0101B4
-#define OID_RT_SET_RSSI_ROAM_TRAFFIC_TH		0xFF0101B5
-#define OID_RT_SET_RSSI_ROAM_SIGNAL_TH		0xFF0101B6
-#define OID_RT_RESET_LOG		0xFF0101B7
-#define OID_RT_GET_LOG		0xFF0101B8
-#define OID_RT_SET_INDICATE_HIDDEN_AP		0xFF0101B9
-#define OID_RT_GET_HEADER_FAIL		0xFF0101BA
-#define OID_RT_SUPPORTED_WIRELESS_MODE		0xFF0101BB
-#define OID_RT_GET_CHANNEL_LIST		0xFF0101BC
-#define OID_RT_GET_SCAN_IN_PROGRESS		0xFF0101BD
-#define OID_RT_GET_TX_INFO		0xFF0101BE
-#define OID_RT_RF_READ_WRITE_OFFSET		0xFF0101BF
-#define OID_RT_RF_READ_WRITE		0xFF0101C0
-
-/*  For Netgear request. 2005.01.13, by rcnjko. */
-#define OID_RT_FORCED_DATA_RATE		0xFF0101C1
-#define OID_RT_WIRELESS_MODE_FOR_SCAN_LIST	0xFF0101C2
-/*  For Netgear request. 2005.02.17, by rcnjko. */
-#define OID_RT_GET_BSS_WIRELESS_MODE		0xFF0101C3
-/*  For AZ project. 2005.06.27, by rcnjko. */
-#define OID_RT_SCAN_WITH_MAGIC_PACKET		0xFF0101C4
-
-/*  Vincent 8185MP */
-#define OID_RT_PRO_RX_FILTER		0xFF0111C0
-
-/* Andy TEST */
-/* define OID_RT_PRO_WRITE_REGISTRY		0xFF0111C1 */
-/* define OID_RT_PRO_READ_REGISTRY		0xFF0111C2 */
-#define OID_CE_USB_WRITE_REGISTRY		0xFF0111C1
-#define OID_CE_USB_READ_REGISTRY		0xFF0111C2
-
-
-#define OID_RT_PRO_SET_INITIAL_GAIN		0xFF0111C3
-#define OID_RT_PRO_SET_BB_RF_STANDBY_MODE		0xFF0111C4
-#define OID_RT_PRO_SET_BB_RF_SHUTDOWN_MODE		0xFF0111C5
-#define OID_RT_PRO_SET_TX_CHARGE_PUMP		0xFF0111C6
-#define OID_RT_PRO_SET_RX_CHARGE_PUMP		0xFF0111C7
-#define OID_RT_PRO_RF_WRITE_REGISTRY		0xFF0111C8
-#define OID_RT_PRO_RF_READ_REGISTRY		0xFF0111C9
-#define OID_RT_PRO_QUERY_RF_TYPE		0xFF0111CA
-
-/*  AP OID */
-#define OID_RT_AP_GET_ASSOCIATED_STATION_LIST		0xFF010300
-#define OID_RT_AP_GET_CURRENT_TIME_STAMP		0xFF010301
-#define OID_RT_AP_SWITCH_INTO_AP_MODE		0xFF010302
-#define OID_RT_AP_SET_DTIM_PERIOD		0xFF010303
-#define OID_RT_AP_SUPPORTED		0xFF010304	/*  Determine if driver supports AP mode. 2004.08.27, by rcnjko. */
-#define OID_RT_AP_SET_PASSPHRASE		0xFF010305	/*  Set WPA-PSK passphrase into authenticator. 2005.07.08, byrcnjko. */
-
-/*  8187MP. 2004.09.06, by rcnjko. */
-#define OID_RT_PRO8187_WI_POLL		0xFF818780
-#define OID_RT_PRO_WRITE_BB_REG		0xFF818781
-#define OID_RT_PRO_READ_BB_REG		0xFF818782
-#define OID_RT_PRO_WRITE_RF_REG		0xFF818783
-#define OID_RT_PRO_READ_RF_REG		0xFF818784
-
-/*  Meeting House. added by Annie, 2005-07-20. */
-#define OID_RT_MH_VENDER_ID		0xFFEDC100
-
-/* 8711 MP OID added 20051230. */
-#define OID_RT_PRO8711_JOIN_BSS		0xFF871100/* S */
-
-#define OID_RT_PRO_READ_REGISTER		0xFF871101 /* Q */
-#define OID_RT_PRO_WRITE_REGISTER		0xFF871102 /* S */
-
-#define OID_RT_PRO_BURST_READ_REGISTER		0xFF871103 /* Q */
-#define OID_RT_PRO_BURST_WRITE_REGISTER		0xFF871104 /* S */
-
-#define OID_RT_PRO_WRITE_TXCMD		0xFF871105 /* S */
-
-#define OID_RT_PRO_READ16_EEPROM		0xFF871106 /* Q */
-#define OID_RT_PRO_WRITE16_EEPROM		0xFF871107 /* S */
-
-#define OID_RT_PRO_H2C_SET_COMMAND		0xFF871108 /* S */
-#define OID_RT_PRO_H2C_QUERY_RESULT		0xFF871109 /* Q */
-
-#define OID_RT_PRO8711_WI_POLL		0xFF87110A /* Q */
-#define OID_RT_PRO8711_PKT_LOSS		0xFF87110B /* Q */
-#define OID_RT_RD_ATTRIB_MEM		0xFF87110C/* Q */
-#define OID_RT_WR_ATTRIB_MEM		0xFF87110D/* S */
-
-
-/* Method 2 for H2C/C2H */
-#define OID_RT_PRO_H2C_CMD_MODE		0xFF871110 /* S */
-#define OID_RT_PRO_H2C_CMD_RSP_MODE		0xFF871111 /* Q */
-#define OID_RT_PRO_H2C_CMD_EVENT_MODE		0xFF871112 /* S */
-#define OID_RT_PRO_WAIT_C2H_EVENT		0xFF871113 /* Q */
-#define OID_RT_PRO_RW_ACCESS_PROTOCOL_TEST		0xFF871114/* Q */
-
-#define OID_RT_PRO_SCSI_ACCESS_TEST		0xFF871115 /* Q, S */
-
-#define OID_RT_PRO_SCSI_TCPIPOFFLOAD_OUT		0xFF871116 /* S */
-#define OID_RT_PRO_SCSI_TCPIPOFFLOAD_IN		0xFF871117 /* Q,S */
-#define OID_RT_RRO_RX_PKT_VIA_IOCTRL		0xFF871118 /* Q */
-#define OID_RT_RRO_RX_PKTARRAY_VIA_IOCTRL		0xFF871119 /* Q */
-
-#define OID_RT_RPO_SET_PWRMGT_TEST		0xFF87111A /* S */
-#define OID_RT_PRO_QRY_PWRMGT_TEST		0XFF87111B /* Q */
-#define OID_RT_RPO_ASYNC_RWIO_TEST		0xFF87111C /* S */
-#define OID_RT_RPO_ASYNC_RWIO_POLL		0xFF87111D /* Q */
-#define OID_RT_PRO_SET_RF_INTFS		0xFF87111E /* S */
-#define OID_RT_POLL_RX_STATUS		0xFF87111F /* Q */
-
-#define OID_RT_PRO_CFG_DEBUG_MESSAGE		0xFF871120 /* Q,S */
-#define OID_RT_PRO_SET_DATA_RATE_EX		0xFF871121/* S */
-#define OID_RT_PRO_SET_BASIC_RATE		0xFF871122/* S */
-#define OID_RT_PRO_READ_TSSI		0xFF871123/* S */
-#define OID_RT_PRO_SET_POWER_TRACKING		0xFF871124/* S */
-
-
-#define OID_RT_PRO_QRY_PWRSTATE		0xFF871150 /* Q */
-#define OID_RT_PRO_SET_PWRSTATE		0xFF871151 /* S */
-
-/* Method 2 , using workitem */
-#define OID_RT_SET_READ_REG		0xFF871181 /* S */
-#define OID_RT_SET_WRITE_REG		0xFF871182 /* S */
-#define OID_RT_SET_BURST_READ_REG		0xFF871183 /* S */
-#define OID_RT_SET_BURST_WRITE_REG		0xFF871184 /* S */
-#define OID_RT_SET_WRITE_TXCMD		0xFF871185 /* S */
-#define OID_RT_SET_READ16_EEPROM		0xFF871186 /* S */
-#define OID_RT_SET_WRITE16_EEPROM		0xFF871187 /* S */
-#define OID_RT_QRY_POLL_WKITEM		0xFF871188 /* Q */
-
-/* For SDIO INTERFACE only */
-#define OID_RT_PRO_SYNCPAGERW_SRAM		0xFF8711A0 /* Q, S */
-#define OID_RT_PRO_871X_DRV_EXT		0xFF8711A1
-
-/* For USB INTERFACE only */
-#define OID_RT_PRO_USB_VENDOR_REQ		0xFF8711B0 /* Q, S */
-#define OID_RT_PRO_SCSI_AUTO_TEST		0xFF8711B1 /* S */
-#define OID_RT_PRO_USB_MAC_AC_FIFO_WRITE		0xFF8711B2 /* S */
-#define OID_RT_PRO_USB_MAC_RX_FIFO_READ		0xFF8711B3 /* Q */
-#define OID_RT_PRO_USB_MAC_RX_FIFO_POLLING		0xFF8711B4 /* Q */
-
-#define OID_RT_PRO_H2C_SET_RATE_TABLE		0xFF8711FB /* S */
-#define OID_RT_PRO_H2C_GET_RATE_TABLE		0xFF8711FC /* S */
-#define OID_RT_PRO_H2C_C2H_LBK_TEST		0xFF8711FE
-
-#define OID_RT_PRO_ENCRYPTION_CTRL		0xFF871200 /* Q, S */
-#define OID_RT_PRO_ADD_STA_INFO		0xFF871201 /* S */
-#define OID_RT_PRO_DELE_STA_INFO		0xFF871202 /* S */
-#define OID_RT_PRO_QUERY_DR_VARIABLE		0xFF871203 /* Q */
-
-#define OID_RT_PRO_RX_PACKET_TYPE		0xFF871204 /* Q, S */
-
-#define OID_RT_PRO_READ_EFUSE		0xFF871205 /* Q */
-#define OID_RT_PRO_WRITE_EFUSE		0xFF871206 /* S */
-#define OID_RT_PRO_RW_EFUSE_PGPKT		0xFF871207 /* Q, S */
-#define OID_RT_GET_EFUSE_CURRENT_SIZE		0xFF871208 /* Q */
-
-#define OID_RT_SET_BANDWIDTH		0xFF871209 /* S */
-#define OID_RT_SET_CRYSTAL_CAP		0xFF87120A /* S */
-
-#define OID_RT_SET_RX_PACKET_TYPE		0xFF87120B /* S */
-
-#define OID_RT_GET_EFUSE_MAX_SIZE		0xFF87120C /* Q */
-
-#define OID_RT_PRO_SET_TX_AGC_OFFSET		0xFF87120D /* S */
-
-#define OID_RT_PRO_SET_PKT_TEST_MODE		0xFF87120E /* S */
-
-#define OID_RT_PRO_FOR_EVM_TEST_SETTING		0xFF87120F /* S */
-
-#define OID_RT_PRO_GET_THERMAL_METER		0xFF871210 /* Q */
-
-#define OID_RT_RESET_PHY_RX_PACKET_COUNT	0xFF871211 /* S */
-#define OID_RT_GET_PHY_RX_PACKET_RECEIVED	0xFF871212 /* Q */
-#define OID_RT_GET_PHY_RX_PACKET_CRC32_ERROR	0xFF871213 /* Q */
-
-#define OID_RT_SET_POWER_DOWN		0xFF871214 /* S */
-
-#define OID_RT_GET_POWER_MODE		0xFF871215 /* Q */
-
-#define OID_RT_PRO_EFUSE		0xFF871216 /* Q, S */
-#define OID_RT_PRO_EFUSE_MAP		0xFF871217 /* Q, S */
-
-#endif /* ifndef	__CUSTOM_OID_H */
diff --git a/drivers/staging/rtl8723au/include/odm.h b/drivers/staging/rtl8723au/include/odm.h
index dfedfbb48fc..183d8ff1ab9 100644
--- a/drivers/staging/rtl8723au/include/odm.h
+++ b/drivers/staging/rtl8723au/include/odm.h
@@ -86,15 +86,6 @@
 /*  structure and define */
 /*  */
 
-/*  */
-/*  2011/09/20 MH Add for AP/ADSLpseudo DM structuer requirement. */
-/*  We need to remove to other position??? */
-/*  */
-struct rtl8723a_priv {
-	u8		temp;
-};
-
-
 struct  dig_t {
 	u8		Dig_Enable_Flag;
 	u8		Dig_Ext_Port_Stage;
@@ -187,7 +178,6 @@ struct rx_hp {
 	bool		First_time_enter;
 	bool		RXHP_enable;
 	u8		TP_Mode;
-	struct timer_list PSDTimer;
 };
 
 #define ASSOCIATE_ENTRY_NUM					32 /*  Max size of AsocEntry[]. */
@@ -234,14 +224,12 @@ struct sw_ant_sw {
 	u64		RXByteCnt_A;
 	u64		RXByteCnt_B;
 	u8		TrafficLoad;
-	struct timer_list SwAntennaSwitchTimer;
 };
 
 struct edca_turbo {
 	bool bCurrentTurboEDCA;
 	bool bIsCurRDLState;
 	u32	prv_traffic_idx; /*  edca turbo */
-
 };
 
 struct odm_rate_adapt {
@@ -267,20 +255,6 @@ struct odm_rate_adapt {
 
 /*  Declare for common info */
 
-struct odm_phy_info {
-	u8		RxPWDBAll;
-	u8		SignalQuality;	 /*  in 0-100 index. */
-	u8		RxMIMOSignalQuality[RF_PATH_MAX]; /* EVM */
-	u8		RxMIMOSignalStrength[RF_PATH_MAX];/*  in 0~100 index */
-	s8		RxPower; /*  in dBm Translate from PWdB */
-	s8		RecvSignalPower;/*  Real power in dBm for this packet, no beautification and aggregation. Keep this raw info to be used for the other procedures. */
-	u8		BTRxRSSIPercentage;
-	u8		SignalStrength; /*  in 0-100 index. */
-	u8		RxPwr[RF_PATH_MAX];/* per-path's pwdb */
-	u8		RxSNR[RF_PATH_MAX];/* per-path's SNR */
-};
-
-
 struct odm_phy_dbg_info {
 	/* ODM Write,debug info */
 	s8		RxSNRdB[RF_PATH_MAX];
@@ -300,11 +274,6 @@ struct odm_packet_info {
 	bool		bPacketBeacon;
 };
 
-struct odm_mac_info {
-	u8	test;
-
-};
-
 
 enum {
 	/*  BB Team */
@@ -323,32 +292,6 @@ enum {
 };
 
 /*  */
-/*  2011/20/20 MH For MP driver RT_WLAN_STA =  struct sta_info */
-/*  Please declare below ODM relative info in your STA info structure. */
-/*  */
-struct odm_sta_info {
-	/*  Driver Write */
-	bool		bUsed;				/*  record the sta status link or not? */
-	u8		IOTPeer;			/*  Enum value.	HT_IOT_PEER_E */
-
-	/*  ODM Write */
-	/* 1 PHY_STATUS_INFO */
-	u8		RSSI_Path[4];		/*  */
-	u8		RSSI_Ave;
-	u8		RXEVM[4];
-	u8		RXSNR[4];
-
-	/*  ODM Write */
-	/* 1 TX_INFO (may changed by IC) */
-
-	/*  */
-	/*	Please use compile flag to disable the structure for other IC except 88E. */
-	/*	Move To lower layer. */
-	/*  */
-	/*  ODM Write Wilson will handle this part(said by Luke.Lee) */
-};
-
-/*  */
 /*  2011/10/20 MH Define Common info enum for all team. */
 /*  */
 
@@ -428,28 +371,28 @@ enum odm_cmninfo {
 /*  Define ODM support ability.  ODM_CMNINFO_ABILITY */
 enum {
 	/*  BB ODM section BIT 0-15 */
-	ODM_BB_DIG				= BIT0,
-	ODM_BB_RA_MASK				= BIT1,
-	ODM_BB_DYNAMIC_TXPWR			= BIT2,
-	ODM_BB_FA_CNT				= BIT3,
-	ODM_BB_RSSI_MONITOR			= BIT4,
-	ODM_BB_CCK_PD				= BIT5,
-	ODM_BB_ANT_DIV				= BIT6,
-	ODM_BB_PWR_SAVE				= BIT7,
-	ODM_BB_PWR_TRAIN			= BIT8,
-	ODM_BB_RATE_ADAPTIVE			= BIT9,
-	ODM_BB_PATH_DIV				= BIT10,
-	ODM_BB_PSD				= BIT11,
-	ODM_BB_RXHP				= BIT12,
+	ODM_BB_DIG				= BIT(0),
+	ODM_BB_RA_MASK				= BIT(1),
+	ODM_BB_DYNAMIC_TXPWR			= BIT(2),
+	ODM_BB_FA_CNT				= BIT(3),
+	ODM_BB_RSSI_MONITOR			= BIT(4),
+	ODM_BB_CCK_PD				= BIT(5),
+	ODM_BB_ANT_DIV				= BIT(6),
+	ODM_BB_PWR_SAVE				= BIT(7),
+	ODM_BB_PWR_TRAIN			= BIT(8),
+	ODM_BB_RATE_ADAPTIVE			= BIT(9),
+	ODM_BB_PATH_DIV				= BIT(10),
+	ODM_BB_PSD				= BIT(11),
+	ODM_BB_RXHP				= BIT(12),
 
 	/*  MAC DM section BIT 16-23 */
-	ODM_MAC_EDCA_TURBO			= BIT16,
-	ODM_MAC_EARLY_MODE			= BIT17,
+	ODM_MAC_EDCA_TURBO			= BIT(16),
+	ODM_MAC_EARLY_MODE			= BIT(17),
 
 	/*  RF ODM section BIT 24-31 */
-	ODM_RF_TX_PWR_TRACK			= BIT24,
-	ODM_RF_RX_GAIN_TRACK			= BIT25,
-	ODM_RF_CALIBRATION			= BIT26,
+	ODM_RF_TX_PWR_TRACK			= BIT(24),
+	ODM_RF_RX_GAIN_TRACK			= BIT(25),
+	ODM_RF_CALIBRATION			= BIT(26),
 
 };
 
@@ -463,13 +406,13 @@ enum odm_interface_def {
 
 /*  ODM_CMNINFO_IC_TYPE */
 enum odm_ic_type_def {
-	ODM_RTL8192S	=	BIT0,
-	ODM_RTL8192C	=	BIT1,
-	ODM_RTL8192D	=	BIT2,
-	ODM_RTL8723A	=	BIT3,
-	ODM_RTL8188E	=	BIT4,
-	ODM_RTL8812	=	BIT5,
-	ODM_RTL8821	=	BIT6,
+	ODM_RTL8192S	=	BIT(0),
+	ODM_RTL8192C	=	BIT(1),
+	ODM_RTL8192D	=	BIT(2),
+	ODM_RTL8723A	=	BIT(3),
+	ODM_RTL8188E	=	BIT(4),
+	ODM_RTL8812	=	BIT(5),
+	ODM_RTL8821	=	BIT(6),
 };
 
 #define ODM_IC_11N_SERIES			\
@@ -496,14 +439,14 @@ enum odm_fab_version {
 /*  ODM_CMNINFO_RF_TYPE */
 /*  For example 1T2R (A+AB = BIT0|BIT4|BIT5) */
 enum rf_path_def {
-	ODM_RF_TX_A	=	BIT0,
-	ODM_RF_TX_B	=	BIT1,
-	ODM_RF_TX_C	=	BIT2,
-	ODM_RF_TX_D	=	BIT3,
-	ODM_RF_RX_A	=	BIT4,
-	ODM_RF_RX_B	=	BIT5,
-	ODM_RF_RX_C	=	BIT6,
-	ODM_RF_RX_D	=	BIT7,
+	ODM_RF_TX_A	=	BIT(0),
+	ODM_RF_TX_B	=	BIT(1),
+	ODM_RF_TX_C	=	BIT(2),
+	ODM_RF_TX_D	=	BIT(3),
+	ODM_RF_RX_A	=	BIT(4),
+	ODM_RF_RX_B	=	BIT(5),
+	ODM_RF_RX_C	=	BIT(6),
+	ODM_RF_RX_D	=	BIT(7),
 };
 
 
@@ -536,33 +479,33 @@ enum odm_bt_coexist {
 
 /*  ODM_CMNINFO_OP_MODE */
 enum odm_operation_mode {
-	ODM_NO_LINK		= BIT0,
-	ODM_LINK		= BIT1,
-	ODM_SCAN		= BIT2,
-	ODM_POWERSAVE		= BIT3,
-	ODM_AP_MODE		= BIT4,
-	ODM_CLIENT_MODE		= BIT5,
-	ODM_AD_HOC		= BIT6,
-	ODM_WIFI_DIRECT		= BIT7,
-	ODM_WIFI_DISPLAY	= BIT8,
+	ODM_NO_LINK		= BIT(0),
+	ODM_LINK		= BIT(1),
+	ODM_SCAN		= BIT(2),
+	ODM_POWERSAVE		= BIT(3),
+	ODM_AP_MODE		= BIT(4),
+	ODM_CLIENT_MODE		= BIT(5),
+	ODM_AD_HOC		= BIT(6),
+	ODM_WIFI_DIRECT		= BIT(7),
+	ODM_WIFI_DISPLAY	= BIT(8),
 };
 
 /*  ODM_CMNINFO_WM_MODE */
 enum odm_wireless_mode {
 	ODM_WM_UNKNOW		= 0x0,
-	ODM_WM_B		= BIT0,
-	ODM_WM_G		= BIT1,
-	ODM_WM_A		= BIT2,
-	ODM_WM_N24G		= BIT3,
-	ODM_WM_N5G		= BIT4,
-	ODM_WM_AUTO		= BIT5,
-	ODM_WM_AC		= BIT6,
+	ODM_WM_B		= BIT(0),
+	ODM_WM_G		= BIT(1),
+	ODM_WM_A		= BIT(2),
+	ODM_WM_N24G		= BIT(3),
+	ODM_WM_N5G		= BIT(4),
+	ODM_WM_AUTO		= BIT(5),
+	ODM_WM_AC		= BIT(6),
 };
 
 /*  ODM_CMNINFO_BAND */
 enum odm_band_type {
-	ODM_BAND_2_4G		= BIT0,
-	ODM_BAND_5G		= BIT1,
+	ODM_BAND_2_4G		= BIT(0),
+	ODM_BAND_5G		= BIT(1),
 
 };
 
@@ -573,18 +516,6 @@ enum odm_sec_chnl_offset {
 	ODM_ABOVE		= 2
 };
 
-/*  ODM_CMNINFO_SEC_MODE */
-enum odm_security {
-	ODM_SEC_OPEN		= 0,
-	ODM_SEC_WEP40		= 1,
-	ODM_SEC_TKIP		= 2,
-	ODM_SEC_RESERVE		= 3,
-	ODM_SEC_AESCCMP		= 4,
-	ODM_SEC_WEP104		= 5,
-	ODM_WEP_WPA_MIXED	= 6, /*  WEP + WPA */
-	ODM_SEC_SMS4		= 7,
-};
-
 /*  ODM_CMNINFO_BW */
 enum odm_band_width {
 	ODM_BW20M		= 0,
@@ -613,38 +544,6 @@ enum odm_cca_path {
 	ODM_CCA_1R_B			= 2,
 };
 
-struct odm_ra_info {
-	u8 RateID;
-	u32 RateMask;
-	u32 RAUseRate;
-	u8 RateSGI;
-	u8 RssiStaRA;
-	u8 PreRssiStaRA;
-	u8 SGIEnable;
-	u8 DecisionRate;
-	u8 PreRate;
-	u8 HighestRate;
-	u8 LowestRate;
-	u32 NscUp;
-	u32 NscDown;
-	u16 RTY[5];
-	u32 TOTAL;
-	u16 DROP;
-	u8 Active;
-	u16 RptTime;
-	u8 RAWaitingCounter;
-	u8 RAPendingCounter;
-	u8 PTActive;  /*  on or off */
-	u8 PTTryState;  /*  0 trying state, 1 for decision state */
-	u8 PTStage;  /*  0~6 */
-	u8 PTStopCount; /* Stop PT counter */
-	u8 PTPreRate;  /*  if rate change do PT */
-	u8 PTPreRssi; /*  if RSSI change 5% do PT */
-	u8 PTModeSS;  /*  decide whitch rate should do PT */
-	u8 RAstage;  /*  StageRA, decide how many times RA will be done between PT */
-	u8 PTSmoothFactor;
-};
-
 struct iqk_matrix_regs_set {
 	bool	bIQKDone;
 	s32	Value[1][IQK_Matrix_REG_NUM];
@@ -762,16 +661,10 @@ enum ant_dif_type {
 
 /*  2011/09/22 MH Copy from SD4 defined structure. We use to support PHY DM integration. */
 struct dm_odm_t {
-	/* struct timer_list FastAntTrainingTimer; */
 	/*  */
 	/*	Add for different team use temporarily */
 	/*  */
 	struct rtw_adapter	*Adapter;		/*  For CE/NIC team */
-	struct rtl8723a_priv	*priv;			/*  For AP/ADSL team */
-	/*  WHen you use Adapter or priv pointer, you must make sure the pointer is ready. */
-	bool			odm_ready;
-
-	struct rtl8723a_priv fake_priv;
 
 	u64			DebugComponents;
 	u32			DebugLevel;
@@ -891,7 +784,6 @@ struct dm_odm_t {
 	/* PHY_INFO_88E		PhyInfo; */
 
 	/*  Latest packet phy info (ODM write) */
-	struct odm_mac_info		*pMacInfo;
 	/* MAC_INFO_88E		MacInfo; */
 
 	/*  Different Team independt structure?? */
@@ -928,7 +820,6 @@ struct dm_odm_t {
 
 	/* PSD */
 	bool			bUserAssignLevel;
-	struct timer_list	PSDTimer;
 	u8			RSSI_BT;			/* come from BT */
 	bool			bPSDinProcess;
 	bool			bDMInitialGainEnable;
@@ -955,14 +846,6 @@ struct dm_odm_t {
 	/*  */
 	/*  ODM system resource. */
 	/*  */
-
-	/*  ODM relative time. */
-	struct timer_list PathDivSwitchTimer;
-	/* 2011.09.27 add for Path Diversity */
-	struct timer_list CCKPathDiversityTimer;
-	struct timer_list FastAntTrainingTimer;
-
-	/*  ODM relative workitem. */
 };	/*  DM_Dynamic_Mechanism_Structure */
 
 enum odm_rf_content {
@@ -1163,7 +1046,7 @@ bool ODM_RAStateCheck23a(struct dm_odm_t *pDM_Odm, s32 RSSI, bool bForceUpdate,
 
 #define dm_SWAW_RSSI_Check	ODM_SwAntDivChkPerPktRssi
 void ODM_SwAntDivChkPerPktRssi(struct dm_odm_t *pDM_Odm, u8 StationID,
-			       struct odm_phy_info *pPhyInfo);
+			       struct phy_info *pPhyInfo);
 
 u32 ConvertTo_dB23a(u32 Value);
 
@@ -1186,12 +1069,6 @@ void ODM_CmnInfoPtrArrayHook23a(struct dm_odm_t *pDM_Odm, enum odm_cmninfo	CmnIn
 
 void ODM_CmnInfoUpdate23a(struct dm_odm_t *pDM_Odm, u32 CmnInfo, u64 Value);
 
-void ODM_InitAllTimers(struct dm_odm_t *pDM_Odm);
-
-void ODM_CancelAllTimers(struct dm_odm_t *pDM_Odm);
-
-void ODM_ReleaseAllTimers(struct dm_odm_t *pDM_Odm);
-
 void ODM_ResetIQKResult(struct dm_odm_t *pDM_Odm);
 
 void ODM_AntselStatistics_88C(struct dm_odm_t *pDM_Odm, u8 MacId, u32 PWDBAll, bool isCCKrate);
diff --git a/drivers/staging/rtl8723au/include/odm_HWConfig.h b/drivers/staging/rtl8723au/include/odm_HWConfig.h
index 147855c96ad..6b98e05d046 100644
--- a/drivers/staging/rtl8723au/include/odm_HWConfig.h
+++ b/drivers/staging/rtl8723au/include/odm_HWConfig.h
@@ -147,7 +147,7 @@ void odm_Init_RSSIForDM23a(struct dm_odm_t *pDM_Odm);
 void
 ODM_PhyStatusQuery23a(
 	struct dm_odm_t *pDM_Odm,
-	struct odm_phy_info *pPhyInfo,
+	struct phy_info *pPhyInfo,
 	u8 *						pPhyStatus,
 	struct odm_packet_info *pPktinfo
 	);
@@ -160,15 +160,13 @@ void ODM_MacStatusQuery23a(struct dm_odm_t *pDM_Odm,
 	bool bPacketBeacon
 );
 
-enum hal_status ODM_ConfigRFWithHeaderFile23a(struct dm_odm_t *pDM_Odm,
-	enum RF_RADIO_PATH	Content,
-	enum RF_RADIO_PATH	eRFPath
-);
+int ODM_ConfigRFWithHeaderFile23a(struct dm_odm_t *pDM_Odm,
+				  enum RF_RADIO_PATH Content,
+				  enum RF_RADIO_PATH eRFPath);
 
-enum hal_status ODM_ConfigBBWithHeaderFile23a(struct dm_odm_t *pDM_Odm,
-	enum odm_bb_config_type		ConfigType
-);
+int ODM_ConfigBBWithHeaderFile23a(struct dm_odm_t *pDM_Odm,
+				  enum odm_bb_config_type ConfigType);
 
-enum hal_status ODM_ConfigMACWithHeaderFile23a(struct dm_odm_t *pDM_Odm);
+int ODM_ConfigMACWithHeaderFile23a(struct dm_odm_t *pDM_Odm);
 
 #endif
diff --git a/drivers/staging/rtl8723au/include/odm_debug.h b/drivers/staging/rtl8723au/include/odm_debug.h
index 5bc51d09e52..4d935a33ccb 100644
--- a/drivers/staging/rtl8723au/include/odm_debug.h
+++ b/drivers/staging/rtl8723au/include/odm_debug.h
@@ -65,30 +65,30 @@
 /*  */
 /*  */
 /* BB Functions */
-#define ODM_COMP_DIG				BIT0
-#define ODM_COMP_RA_MASK			BIT1
-#define ODM_COMP_DYNAMIC_TXPWR			BIT2
-#define ODM_COMP_FA_CNT				BIT3
-#define ODM_COMP_RSSI_MONITOR			BIT4
-#define ODM_COMP_CCK_PD				BIT5
-#define ODM_COMP_ANT_DIV			BIT6
-#define ODM_COMP_PWR_SAVE			BIT7
-#define ODM_COMP_PWR_TRAIN			BIT8
-#define ODM_COMP_RATE_ADAPTIVE			BIT9
-#define ODM_COMP_PATH_DIV			BIT10
-#define ODM_COMP_PSD				BIT11
-#define ODM_COMP_DYNAMIC_PRICCA			BIT12
-#define ODM_COMP_RXHP				BIT13
+#define ODM_COMP_DIG				BIT(0)
+#define ODM_COMP_RA_MASK			BIT(1)
+#define ODM_COMP_DYNAMIC_TXPWR			BIT(2)
+#define ODM_COMP_FA_CNT				BIT(3)
+#define ODM_COMP_RSSI_MONITOR			BIT(4)
+#define ODM_COMP_CCK_PD				BIT(5)
+#define ODM_COMP_ANT_DIV			BIT(6)
+#define ODM_COMP_PWR_SAVE			BIT(7)
+#define ODM_COMP_PWR_TRAIN			BIT(8)
+#define ODM_COMP_RATE_ADAPTIVE			BIT(9)
+#define ODM_COMP_PATH_DIV			BIT(10)
+#define ODM_COMP_PSD				BIT(11)
+#define ODM_COMP_DYNAMIC_PRICCA			BIT(12)
+#define ODM_COMP_RXHP				BIT(13)
 /* MAC Functions */
-#define ODM_COMP_EDCA_TURBO			BIT16
-#define ODM_COMP_EARLY_MODE			BIT17
+#define ODM_COMP_EDCA_TURBO			BIT(16)
+#define ODM_COMP_EARLY_MODE			BIT(17)
 /* RF Functions */
-#define ODM_COMP_TX_PWR_TRACK			BIT24
-#define ODM_COMP_RX_GAIN_TRACK			BIT25
-#define ODM_COMP_CALIBRATION			BIT26
+#define ODM_COMP_TX_PWR_TRACK			BIT(24)
+#define ODM_COMP_RX_GAIN_TRACK			BIT(25)
+#define ODM_COMP_CALIBRATION			BIT(26)
 /* Common Functions */
-#define ODM_COMP_COMMON				BIT30
-#define ODM_COMP_INIT				BIT31
+#define ODM_COMP_COMMON				BIT(30)
+#define ODM_COMP_INIT				BIT(31)
 
 /*------------------------Export Macro Definition---------------------------*/
 	#define DbgPrint	printk
diff --git a/drivers/staging/rtl8723au/include/odm_interface.h b/drivers/staging/rtl8723au/include/odm_interface.h
index f216b5846f9..c260d6bd8ff 100644
--- a/drivers/staging/rtl8723au/include/odm_interface.h
+++ b/drivers/staging/rtl8723au/include/odm_interface.h
@@ -18,22 +18,6 @@
 #define __ODM_INTERFACE_H__
 
 
-
-/*  */
-/*  =========== Constant/Structure/Enum/... Define */
-/*  */
-
-
-
-/*  */
-/*  =========== Macro Define */
-/*  */
-
-#define _reg_all(_name)			ODM_##_name
-#define _reg_ic(_name, _ic)		ODM_##_name##_ic
-#define _bit_all(_name)			BIT_##_name
-#define _bit_ic(_name, _ic)		BIT_##_name##_ic
-
 /*  _cat: implemented by Token-Pasting Operator. */
 
 /*===================================
@@ -68,64 +52,23 @@ ODM_REG(DIG,_pDM_Odm)
 typedef void (*RT_WORKITEM_CALL_BACK)(struct work_struct *pContext);
 
 /*  */
-/*  =========== Extern Variable ??? It should be forbidden. */
-/*  */
-
-
-/*  */
 /*  =========== EXtern Function Prototype */
 /*  */
 
 
 u8 ODM_Read1Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr);
-
 u16 ODM_Read2Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr);
-
 u32 ODM_Read4Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr);
-
 void ODM_Write1Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr, u8 Data);
-
 void ODM_Write2Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr, u16 Data);
-
 void ODM_Write4Byte(struct dm_odm_t *pDM_Odm, u32 RegAddr, u32 Data);
-
 void ODM_SetMACReg(struct dm_odm_t *pDM_Odm, u32 RegAddr, u32 BitMask, u32 Data);
-
 u32 ODM_GetMACReg(struct dm_odm_t *pDM_Odm, u32 RegAddr, u32 BitMask);
-
 void ODM_SetBBReg(struct dm_odm_t *pDM_Odm, u32 RegAddr, u32 BitMask, u32 Data);
-
 u32 ODM_GetBBReg(struct dm_odm_t *pDM_Odm, u32 RegAddr, u32 BitMask);
-
 void ODM_SetRFReg(struct dm_odm_t *pDM_Odm, enum RF_RADIO_PATH eRFPath,
 		  u32 RegAddr, u32 BitMask, u32 Data);
-
 u32 ODM_GetRFReg(struct dm_odm_t *pDM_Odm, enum RF_RADIO_PATH eRFPath,
 		 u32 RegAddr, u32 BitMask);
 
-/*  Memory Relative Function. */
-void ODM_AllocateMemory(struct dm_odm_t *pDM_Odm, void **pPtr, u32 length);
-void ODM_FreeMemory(struct dm_odm_t *pDM_Odm, void *pPtr, u32 length);
-
-s32 ODM_CompareMemory(struct dm_odm_t *pDM_Odm, void *pBuf1, void *pBuf2, u32 length);
-
-/*  ODM MISC-spin lock relative API. */
-void ODM_AcquireSpinLock(struct dm_odm_t *pDM_Odm, enum rt_spinlock_type type);
-
-void ODM_ReleaseSpinLock(struct dm_odm_t *pDM_Odm, enum rt_spinlock_type type);
-
-/*  ODM MISC-workitem relative API. */
-void ODM_InitializeWorkItem(struct dm_odm_t *pDM_Odm, void *pRtWorkItem,
-			    RT_WORKITEM_CALL_BACK RtWorkItemCallback, void *pContext, const char *szID);
-
-/*  ODM Timer relative API. */
-void ODM_SetTimer(struct dm_odm_t *pDM_Odm, struct timer_list *pTimer, u32 msDelay);
-
-void ODM_ReleaseTimer(struct dm_odm_t *pDM_Odm, struct timer_list *pTimer);
-
-/*  ODM FW relative API. */
-u32 ODM_FillH2CCmd(u8 *pH2CBuffer, u32 H2CBufferLen, u32 CmdNum,
-		   u32 *pElementID, u32 *pCmdLen, u8 **pCmbBuffer,
-		   u8 *CmdStartSeq);
-
 #endif	/*  __ODM_INTERFACE_H__ */
diff --git a/drivers/staging/rtl8723au/include/odm_precomp.h b/drivers/staging/rtl8723au/include/odm_precomp.h
index f3fc2fad988..69de888d884 100644
--- a/drivers/staging/rtl8723au/include/odm_precomp.h
+++ b/drivers/staging/rtl8723au/include/odm_precomp.h
@@ -16,10 +16,6 @@
 #ifndef	__ODM_PRECOMP_H__
 #define __ODM_PRECOMP_H__
 
-#include "odm_types.h"
-
-#define		TEST_FALG___		1
-
 /* 2 Config Flags and Structs - defined by each ODM Type */
 
 #include <osdep_service.h>
diff --git a/drivers/staging/rtl8723au/include/odm_reg.h b/drivers/staging/rtl8723au/include/odm_reg.h
index 56191e9fdcd..c18433120fe 100644
--- a/drivers/staging/rtl8723au/include/odm_reg.h
+++ b/drivers/staging/rtl8723au/include/odm_reg.h
@@ -102,13 +102,10 @@
 #define	PATHDIV_REG					0xB30
 #define	PATHDIV_TRI					0xBA0
 
-
 /*  */
 /*  Bitmap Definition */
 /*  */
 
-#define	BIT_FA_RESET					BIT0
-
-
+#define	BIT_FA_RESET					BIT(0)
 
 #endif
diff --git a/drivers/staging/rtl8723au/include/odm_types.h b/drivers/staging/rtl8723au/include/odm_types.h
deleted file mode 100644
index a866769ea17..00000000000
--- a/drivers/staging/rtl8723au/include/odm_types.h
+++ /dev/null
@@ -1,36 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
- *
- * 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.
- *
- * 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 __ODM_TYPES_H__
-#define __ODM_TYPES_H__
-
-/*  Define Different SW team support */
-
-enum hal_status {
-	HAL_STATUS_SUCCESS,
-	HAL_STATUS_FAILURE,
-};
-
-enum rt_spinlock_type {
-	RT_TEMP =1,
-};
-
-#define SET_TX_DESC_ANTSEL_A_88E(__pTxDesc, __Value)		\
-	SET_BITS_TO_LE_4BYTE(__pTxDesc+8, 24, 1, __Value)
-#define SET_TX_DESC_ANTSEL_B_88E(__pTxDesc, __Value)		\
-	SET_BITS_TO_LE_4BYTE(__pTxDesc+8, 25, 1, __Value)
-#define SET_TX_DESC_ANTSEL_C_88E(__pTxDesc, __Value)		\
-	SET_BITS_TO_LE_4BYTE(__pTxDesc+28, 29, 1, __Value)
-
-#endif /*  __ODM_TYPES_H__ */
diff --git a/drivers/staging/rtl8723au/include/osdep_intf.h b/drivers/staging/rtl8723au/include/osdep_intf.h
index b603cf53290..33afa62f31b 100644
--- a/drivers/staging/rtl8723au/include/osdep_intf.h
+++ b/drivers/staging/rtl8723au/include/osdep_intf.h
@@ -22,12 +22,10 @@
 int rtw_hw_suspend23a(struct rtw_adapter *padapter);
 int rtw_hw_resume23a(struct rtw_adapter *padapter);
 
-u8 rtw_init_drv_sw23a(struct rtw_adapter *padapter);
-u8 rtw_free_drv_sw23a(struct rtw_adapter *padapter);
-u8 rtw_reset_drv_sw23a(struct rtw_adapter *padapter);
+int rtw_init_drv_sw23a(struct rtw_adapter *padapter);
+int rtw_free_drv_sw23a(struct rtw_adapter *padapter);
+int rtw_reset_drv_sw23a(struct rtw_adapter *padapter);
 
-u32 rtw_start_drv_threads23a(struct rtw_adapter *padapter);
-void rtw_stop_drv_threads23a (struct rtw_adapter *padapter);
 void rtw_cancel_all_timer23a(struct rtw_adapter *padapter);
 
 int rtw_init_netdev23a_name23a(struct net_device *pnetdev, const char *ifname);
@@ -43,4 +41,8 @@ void rtw_ips_pwr_down23a(struct rtw_adapter *padapter);
 int rtw_drv_register_netdev(struct rtw_adapter *padapter);
 void rtw_ndev_destructor(struct net_device *ndev);
 
+int rtl8723au_inirp_init(struct rtw_adapter *Adapter);
+int rtl8723au_inirp_deinit(struct rtw_adapter *Adapter);
+void rtl8723a_usb_intf_stop(struct rtw_adapter *padapter);
+
 #endif	/* _OSDEP_INTF_H_ */
diff --git a/drivers/staging/rtl8723au/include/osdep_service.h b/drivers/staging/rtl8723au/include/osdep_service.h
index 039bc7285ed..a5ebdb81ce3 100644
--- a/drivers/staging/rtl8723au/include/osdep_service.h
+++ b/drivers/staging/rtl8723au/include/osdep_service.h
@@ -19,7 +19,6 @@
 #define _SUCCESS	1
 #define RTW_RX_HANDLED 2
 
-#include <linux/version.h>
 #include <linux/spinlock.h>
 #include <linux/compiler.h>
 #include <linux/kernel.h>
@@ -30,7 +29,6 @@
 #include <linux/kref.h>
 #include <linux/netdevice.h>
 #include <linux/skbuff.h>
-#include <linux/circ_buf.h>
 #include <asm/uaccess.h>
 #include <asm/byteorder.h>
 #include <asm/atomic.h>
@@ -40,26 +38,14 @@
 #include <linux/sched.h>
 #include <linux/etherdevice.h>
 #include <linux/wireless.h>
-#include <net/iw_handler.h>
 #include <linux/if_arp.h>
 #include <linux/rtnetlink.h>
 #include <linux/delay.h>
-#include <linux/proc_fs.h>	/*  Necessary because we use the proc fs */
 #include <linux/interrupt.h>	/*  for struct tasklet_struct */
 #include <linux/ip.h>
-#include <linux/kthread.h>
 
-
-/*	#include <linux/ieee80211.h> */
 #include <net/ieee80211_radiotap.h>
 #include <net/cfg80211.h>
-#include <linux/usb.h>
-#include <linux/usb/ch9.h>
-
-struct rtw_adapter;
-struct c2h_evt_hdr;
-
-typedef s32 (*c2h_id_filter)(u8 id);
 
 struct rtw_queue {
 	struct	list_head	queue;
@@ -79,9 +65,6 @@ static inline int rtw_netif_queue_stopped(struct net_device *pnetdev)
 		netif_tx_queue_stopped(netdev_get_tx_queue(pnetdev, 3)) );
 }
 
-#ifndef BIT
-#define BIT(x)	( 1 << (x))
-#endif
 static inline u32 CHKBIT(u32 x)
 {
 	WARN_ON(x >= 32);
@@ -90,118 +73,16 @@ static inline u32 CHKBIT(u32 x)
 	return BIT(x);
 }
 
-#define BIT0	0x00000001
-#define BIT1	0x00000002
-#define BIT2	0x00000004
-#define BIT3	0x00000008
-#define BIT4	0x00000010
-#define BIT5	0x00000020
-#define BIT6	0x00000040
-#define BIT7	0x00000080
-#define BIT8	0x00000100
-#define BIT9	0x00000200
-#define BIT10	0x00000400
-#define BIT11	0x00000800
-#define BIT12	0x00001000
-#define BIT13	0x00002000
-#define BIT14	0x00004000
-#define BIT15	0x00008000
-#define BIT16	0x00010000
-#define BIT17	0x00020000
-#define BIT18	0x00040000
-#define BIT19	0x00080000
-#define BIT20	0x00100000
-#define BIT21	0x00200000
-#define BIT22	0x00400000
-#define BIT23	0x00800000
-#define BIT24	0x01000000
-#define BIT25	0x02000000
-#define BIT26	0x04000000
-#define BIT27	0x08000000
-#define BIT28	0x10000000
-#define BIT29	0x20000000
-#define BIT30	0x40000000
-#define BIT31	0x80000000
-#define BIT32	0x0100000000
-#define BIT33	0x0200000000
-#define BIT34	0x0400000000
-#define BIT35	0x0800000000
-#define BIT36	0x1000000000
-
-int RTW_STATUS_CODE23a(int error_code);
-
-u8*	_rtw_vmalloc(u32 sz);
-u8*	_rtw_zvmalloc(u32 sz);
-void	_rtw_vmfree(u8 *pbuf, u32 sz);
-#define rtw_vmalloc(sz)			_rtw_vmalloc((sz))
-#define rtw_zvmalloc(sz)			_rtw_zvmalloc((sz))
-#define rtw_vmfree(pbuf, sz)		_rtw_vmfree((pbuf), (sz))
-
-extern unsigned char REALTEK_96B_IE23A[];
 extern unsigned char MCS_rate_2R23A[16];
-extern unsigned char RTW_WPA_OUI23A[];
-extern unsigned char WPA_TKIP_CIPHER23A[4];
-extern unsigned char RSN_TKIP_CIPHER23A[4];
 
 extern unsigned char	MCS_rate_2R23A[16];
 extern unsigned char	MCS_rate_1R23A[16];
 
 void	_rtw_init_queue23a(struct rtw_queue *pqueue);
-u32	_rtw_queue_empty23a(struct rtw_queue *pqueue);
-
-static inline u32 bitshift(u32 bitmask)
-{
-	u32 i;
-
-	for (i = 0; i <= 31; i++)
-		if (((bitmask>>i) &  0x1) == 1) break;
-
-	return i;
-}
-
-void rtw_suspend_lock_init(void);
-void rtw_suspend_lock_uninit(void);
-void rtw_lock_suspend(void);
-void rtw_unlock_suspend(void);
-
-
-#define NDEV_FMT "%s"
-#define NDEV_ARG(ndev) ndev->name
-#define ADPT_FMT "%s"
-#define ADPT_ARG(adapter) adapter->pnetdev->name
-#define FUNC_NDEV_FMT "%s(%s)"
-#define FUNC_NDEV_ARG(ndev) __func__, ndev->name
-#define FUNC_ADPT_FMT "%s(%s)"
-#define FUNC_ADPT_ARG(adapter) __func__, adapter->pnetdev->name
-
-#define rtw_signal_process(pid, sig) kill_pid(find_vpid((pid)),(sig), 1)
-
-u64 rtw_modular6423a(u64 x, u64 y);
-u64 rtw_division6423a(u64 x, u64 y);
-
 
 /* Macros for handling unaligned memory accesses */
 
 #define RTW_GET_BE24(a) ((((u32) (a)[0]) << 16) | (((u32) (a)[1]) << 8) | \
 			 ((u32) (a)[2]))
 
-
-struct rtw_cbuf {
-	u32 write;
-	u32 read;
-	u32 size;
-	void *bufs[0];
-};
-
-bool rtw_cbuf_full23a(struct rtw_cbuf *cbuf);
-bool rtw_cbuf_empty23a(struct rtw_cbuf *cbuf);
-bool rtw_cbuf_push23a(struct rtw_cbuf *cbuf, void *buf);
-void *rtw_cbuf_pop23a(struct rtw_cbuf *cbuf);
-struct rtw_cbuf *rtw_cbuf_alloc23a(u32 size);
-void rtw_cbuf_free(struct rtw_cbuf *cbuf);
-int rtw_change_ifname(struct rtw_adapter *padapter, const char *ifname);
-s32 c2h_evt_hdl(struct rtw_adapter *adapter, struct c2h_evt_hdr *c2h_evt, c2h_id_filter filter);
-void indicate_wx_scan_complete_event(struct rtw_adapter *padapter);
-u8 rtw_do_join23a(struct rtw_adapter *padapter);
-
 #endif
diff --git a/drivers/staging/rtl8723au/include/recv_osdep.h b/drivers/staging/rtl8723au/include/recv_osdep.h
index 15c94b6168b..c2d3f1bd594 100644
--- a/drivers/staging/rtl8723au/include/recv_osdep.h
+++ b/drivers/staging/rtl8723au/include/recv_osdep.h
@@ -23,22 +23,13 @@ void _rtw_free_recv_priv23a (struct recv_priv *precvpriv);
 
 int rtw_recv_entry23a(struct recv_frame *precv_frame);
 int rtw_recv_indicatepkt23a(struct rtw_adapter *adapter, struct recv_frame *precv_frame);
-void rtw_recv_returnpacket(struct net_device *cnxt, struct sk_buff *preturnedpkt);
 
-void rtw_hostapd_mlme_rx23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
 void rtw_handle_tkip_mic_err23a(struct rtw_adapter *padapter, u8 bgroup);
 
 int	rtw_init_recv_priv(struct recv_priv *precvpriv, struct rtw_adapter *padapter);
 void rtw_free_recv_priv (struct recv_priv *precvpriv);
 
 int rtw_os_recv_resource_init(struct recv_priv *precvpriv, struct rtw_adapter *padapter);
-int rtw_os_recv_resource_alloc23a(struct rtw_adapter *padapter, struct recv_frame *precvframe);
-void rtw_os_recv_resource_free(struct recv_priv *precvpriv);
-
-int rtw_os_recvbuf_resource_alloc23a(struct rtw_adapter *padapter, struct recv_buf *precvbuf);
-int rtw_os_recvbuf_resource_free23a(struct rtw_adapter *padapter, struct recv_buf *precvbuf);
-
-void rtw_os_read_port23a(struct rtw_adapter *padapter, struct recv_buf *precvbuf);
 
 void rtw_init_recv_timer23a(struct recv_reorder_ctrl *preorder_ctrl);
 
diff --git a/drivers/staging/rtl8723au/include/rtl8723a_bt-coexist.h b/drivers/staging/rtl8723au/include/rtl8723a_bt-coexist.h
index 6d1edc6ef84..05069652bae 100644
--- a/drivers/staging/rtl8723au/include/rtl8723a_bt-coexist.h
+++ b/drivers/staging/rtl8723au/include/rtl8723a_bt-coexist.h
@@ -19,19 +19,6 @@
 #include "odm_precomp.h"
 
 
-#define __BT_C__ 1
-#define __BT_HANDLEPACKET_C__ 1
-#define __BT_HCI_C__ 1
-#define __HALBTC87231ANT_C__ 1
-#define __HALBTC87232ANT_C__ 1
-#define __HALBTC8723_C__ 1
-#define __HALBTCCSR1ANT_C__ 1
-#define __HALBTCCSR2ANT_C__ 1
-#define __HALBTCOEXIST_C__ 1
-#define __HALBT_C__ 1
-
-#ifdef __BT_C__ /*  COMMON/BT.h */
-
 /*  HEADER/PlatformDef.h */
 enum rt_media_status {
 	RT_MEDIA_DISCONNECT	= 0,
@@ -44,20 +31,12 @@ enum rt_media_status {
 
 void BT_SignalCompensation(struct rtw_adapter *padapter,
 			   u8 *rssi_wifi, u8 *rssi_bt);
-void BT_WifiScanNotify(struct rtw_adapter *padapter, u8 scanType);
-void BT_WifiAssociateNotify(struct rtw_adapter *padapter, u8 action);
-void BT_WifiMediaStatusNotify(struct rtw_adapter *padapter,
-			      enum rt_media_status mstatus);
-void BT_SpecialPacketNotify(struct rtw_adapter * padapter);
 void BT_HaltProcess(struct rtw_adapter * padapter);
 void BT_LpsLeave(struct rtw_adapter * padapter);
 
 
 #define	BT_HsConnectionEstablished(Adapter)		false
 /*  ===== End of sync from SD7 driver COMMON/BT.h ===== */
-#endif /*  __BT_C__ */
-
-#ifdef __BT_HCI_C__ /*  COMMON/bt_hci.h */
 
 /*  HEADER/SecurityType.h */
 #define TKIP_ENC_KEY_POS		32		/* KEK_LEN+KEK_LEN) */
@@ -1107,15 +1086,9 @@ enum hci_ext_bp_operation {
 	HCI_BT_OP_MAX
 };
 
-/*	Function proto type */
-struct btdata_entry {
-	struct list_head	List;
-	void			*pDataBlock;
-};
-
 #define BTHCI_SM_WITH_INFO(_Adapter, _StateToEnter, _StateCmd, _EntryNum)	\
 {										\
-	RTPRINT(FIOCTL, IOCTL_STATE, ("[BT state change] caused by ""%s"", line =%d\n", __FUNCTION__, __LINE__));							\
+	RTPRINT(FIOCTL, IOCTL_STATE, ("[BT state change] caused by ""%s"", line =%d\n", __func__, __LINE__));							\
 	BTHCI_StateMachine(_Adapter, _StateToEnter, _StateCmd, _EntryNum);\
 }
 
@@ -1132,9 +1105,7 @@ void BTHCI_DisconnectAll(struct rtw_adapter * padapter);
 enum hci_status BTHCI_HandleHCICMD(struct rtw_adapter * padapter, struct packet_irp_hcicmd_data *pHciCmd);
 
 /*  ===== End of sync from SD7 driver COMMON/bt_hci.h ===== */
-#endif /*  __BT_HCI_C__ */
 
-#ifdef __HALBTC87231ANT_C__ /*  HAL/BTCoexist/HalBtc87231Ant.h */
 /*  ===== Below this line is sync from SD7 driver HAL/BTCoexist/HalBtc87231Ant.h ===== */
 #define GET_BT_INFO(padapter)	(&GET_HAL_DATA(padapter)->BtInfo)
 
@@ -1191,9 +1162,7 @@ void BTDM_1AntForDhcp(struct rtw_adapter * padapter);
 void BTDM_1AntBtCoexist8723A(struct rtw_adapter * padapter);
 
 /*  ===== End of sync from SD7 driver HAL/BTCoexist/HalBtc87231Ant.h ===== */
-#endif /*  __HALBTC87231ANT_C__ */
 
-#ifdef __HALBTC87232ANT_C__ /*  HAL/BTCoexist/HalBtc87232Ant.h */
 /*  ===== Below this line is sync from SD7 driver HAL/BTCoexist/HalBtc87232Ant.h ===== */
 enum bt_2ant_bt_status {
 	BT_2ANT_BT_STATUS_IDLE			= 0x0,
@@ -1274,9 +1243,7 @@ struct btdm_8723a_2ant {
 
 void BTDM_2AntBtCoexist8723A(struct rtw_adapter * padapter);
 /*  ===== End of sync from SD7 driver HAL/BTCoexist/HalBtc87232Ant.h ===== */
-#endif /*  __HALBTC87232ANT_C__ */
 
-#ifdef __HALBTC8723_C__ /*  HAL/BTCoexist/HalBtc8723.h */
 /*  ===== Below this line is sync from SD7 driver HAL/BTCoexist/HalBtc8723.h ===== */
 
 #define	BT_Q_PKT_OFF		0
@@ -1352,13 +1319,9 @@ void BTDM_SetSwPenaltyTxRateAdaptive(struct rtw_adapter * padapter, u8 raType);
 void BTDM_SetFwDecBtPwr(struct rtw_adapter * padapter, u8 bDecBtPwr);
 u8 BTDM_BtProfileSupport(struct rtw_adapter * padapter);
 void BTDM_LpsLeave(struct rtw_adapter * padapter);
-u8 BTDM_1Ant8723A(struct rtw_adapter * padapter);
-#define BT_1Ant BTDM_1Ant8723A
 
 /*  ===== End of sync from SD7 driver HAL/BTCoexist/HalBtc8723.h ===== */
-#endif /*  __HALBTC8723_C__ */
 
-#ifdef __HALBTCCSR1ANT_C__ /*  HAL/BTCoexist/HalBtcCsr1Ant.h */
 /*  ===== Below this line is sync from SD7 driver HAL/BTCoexist/HalBtcCsr1Ant.h ===== */
 
 enum BT_A2DP_INDEX{
@@ -1381,19 +1344,17 @@ void BTDM_SingleAnt(struct rtw_adapter * padapter, u8 bSingleAntOn, u8 bInterrup
 void BTDM_CheckBTIdleChange1Ant(struct rtw_adapter * padapter);
 
 /*  ===== End of sync from SD7 driver HAL/BTCoexist/HalBtcCsr1Ant.h ===== */
-#endif /*  __HALBTCCSR1ANT_C__ */
 
-#ifdef __HALBTCCSR2ANT_C__ /*  HAL/BTCoexist/HalBtcCsr2Ant.h */
 /*  ===== Below this line is sync from SD7 driver HAL/BTCoexist/HalBtcCsr2Ant.h ===== */
 
 /*  */
 /*  For old core stack before v251 */
 /*  */
-#define BT_RSSI_STATE_NORMAL_POWER	BIT0
-#define BT_RSSI_STATE_AMDPU_OFF		BIT1
-#define BT_RSSI_STATE_SPECIAL_LOW	BIT2
-#define BT_RSSI_STATE_BG_EDCA_LOW	BIT3
-#define BT_RSSI_STATE_TXPOWER_LOW	BIT4
+#define BT_RSSI_STATE_NORMAL_POWER	BIT(0)
+#define BT_RSSI_STATE_AMDPU_OFF		BIT(1)
+#define BT_RSSI_STATE_SPECIAL_LOW	BIT(2)
+#define BT_RSSI_STATE_BG_EDCA_LOW	BIT(3)
+#define BT_RSSI_STATE_TXPOWER_LOW	BIT(4)
 
 #define	BT_DACSWING_OFF				0
 #define	BT_DACSWING_M4				1
@@ -1403,9 +1364,6 @@ void BTDM_CheckBTIdleChange1Ant(struct rtw_adapter * padapter);
 void BTDM_DiminishWiFi(struct rtw_adapter * Adapter, u8 bDACOn, u8 bInterruptOn, u8 DACSwingLevel, u8 bNAVOn);
 
 /*  ===== End of sync from SD7 driver HAL/BTCoexist/HalBtcCsr2Ant.h ===== */
-#endif /*  __HALBTCCSR2ANT_C__ */
-
-#ifdef __HALBTCOEXIST_C__ /*  HAL/BTCoexist/HalBtCoexist.h */
 
 /*  HEADER/TypeDef.h */
 #define MAX_FW_SUPPORT_MACID_NUM			64
@@ -1440,61 +1398,61 @@ void BTDM_DiminishWiFi(struct rtw_adapter * Adapter, u8 bDACOn, u8 bInterruptOn,
 #define BT_FW_COEX_THRESH_55				55
 #define BT_FW_COEX_THRESH_65				65
 
-#define BT_COEX_STATE_BT30					BIT(0)
+#define BT_COEX_STATE_BT30			BIT(0)
 #define BT_COEX_STATE_WIFI_HT20			BIT(1)
 #define BT_COEX_STATE_WIFI_HT40			BIT(2)
-#define BT_COEX_STATE_WIFI_LEGACY			BIT(3)
+#define BT_COEX_STATE_WIFI_LEGACY		BIT(3)
 
 #define BT_COEX_STATE_WIFI_RSSI_LOW		BIT(4)
-#define BT_COEX_STATE_WIFI_RSSI_MEDIUM	BIT(5)
+#define BT_COEX_STATE_WIFI_RSSI_MEDIUM		BIT(5)
 #define BT_COEX_STATE_WIFI_RSSI_HIGH		BIT(6)
 #define BT_COEX_STATE_DEC_BT_POWER		BIT(7)
 
 #define BT_COEX_STATE_WIFI_IDLE			BIT(8)
-#define BT_COEX_STATE_WIFI_UPLINK			BIT(9)
+#define BT_COEX_STATE_WIFI_UPLINK		BIT(9)
 #define BT_COEX_STATE_WIFI_DOWNLINK		BIT(10)
 
-#define BT_COEX_STATE_BT_INQ_PAGE	BIT(11)
-#define BT_COEX_STATE_BT_IDLE				BIT(12)
+#define BT_COEX_STATE_BT_INQ_PAGE		BIT(11)
+#define BT_COEX_STATE_BT_IDLE			BIT(12)
 #define BT_COEX_STATE_BT_UPLINK			BIT(13)
 #define BT_COEX_STATE_BT_DOWNLINK		BIT(14)
 /*  */
 /*  Todo: Remove these definitions */
-#define BT_COEX_STATE_BT_PAN_IDLE			BIT(15)
+#define BT_COEX_STATE_BT_PAN_IDLE		BIT(15)
 #define BT_COEX_STATE_BT_PAN_UPLINK		BIT(16)
-#define BT_COEX_STATE_BT_PAN_DOWNLINK	BIT(17)
+#define BT_COEX_STATE_BT_PAN_DOWNLINK		BIT(17)
 #define BT_COEX_STATE_BT_A2DP_IDLE		BIT(18)
 /*  */
-#define BT_COEX_STATE_BT_RSSI_LOW			BIT(19)
+#define BT_COEX_STATE_BT_RSSI_LOW		BIT(19)
 
-#define BT_COEX_STATE_PROFILE_HID			BIT(20)
+#define BT_COEX_STATE_PROFILE_HID		BIT(20)
 #define BT_COEX_STATE_PROFILE_A2DP		BIT(21)
-#define BT_COEX_STATE_PROFILE_PAN			BIT(22)
-#define BT_COEX_STATE_PROFILE_SCO			BIT(23)
+#define BT_COEX_STATE_PROFILE_PAN		BIT(22)
+#define BT_COEX_STATE_PROFILE_SCO		BIT(23)
 
 #define BT_COEX_STATE_WIFI_RSSI_1_LOW		BIT(24)
 #define BT_COEX_STATE_WIFI_RSSI_1_MEDIUM	BIT(25)
 #define BT_COEX_STATE_WIFI_RSSI_1_HIGH		BIT(26)
 
-#define BT_COEX_STATE_WIFI_RSSI_BEACON_LOW		BIT(27)
-#define BT_COEX_STATE_WIFI_RSSI_BEACON_MEDIUM		BIT(28)
-#define BT_COEX_STATE_WIFI_RSSI_BEACON_HIGH		BIT(29)
+#define BT_COEX_STATE_WIFI_RSSI_BEACON_LOW	BIT(27)
+#define BT_COEX_STATE_WIFI_RSSI_BEACON_MEDIUM	BIT(28)
+#define BT_COEX_STATE_WIFI_RSSI_BEACON_HIGH	BIT(29)
 
 
-#define BT_COEX_STATE_BTINFO_COMMON				BIT30
-#define BT_COEX_STATE_BTINFO_B_HID_SCOESCO		BIT31
-#define BT_COEX_STATE_BTINFO_B_FTP_A2DP			BIT32
+#define BT_COEX_STATE_BTINFO_COMMON		BIT(30)
+#define BT_COEX_STATE_BTINFO_B_HID_SCOESCO	BIT(31)
+#define BT_COEX_STATE_BTINFO_B_FTP_A2DP		BIT(32)
 
-#define BT_COEX_STATE_BT_CNT_LEVEL_0				BIT33
-#define BT_COEX_STATE_BT_CNT_LEVEL_1				BIT34
-#define BT_COEX_STATE_BT_CNT_LEVEL_2				BIT35
-#define BT_COEX_STATE_BT_CNT_LEVEL_3				BIT36
+#define BT_COEX_STATE_BT_CNT_LEVEL_0		BIT(33)
+#define BT_COEX_STATE_BT_CNT_LEVEL_1		BIT(34)
+#define BT_COEX_STATE_BT_CNT_LEVEL_2		BIT(35)
+#define BT_COEX_STATE_BT_CNT_LEVEL_3		BIT(36)
 
-#define BT_RSSI_STATE_HIGH					0
-#define BT_RSSI_STATE_MEDIUM				1
-#define BT_RSSI_STATE_LOW					2
+#define BT_RSSI_STATE_HIGH			0
+#define BT_RSSI_STATE_MEDIUM			1
+#define BT_RSSI_STATE_LOW			2
 #define BT_RSSI_STATE_STAY_HIGH			3
-#define BT_RSSI_STATE_STAY_MEDIUM			4
+#define BT_RSSI_STATE_STAY_MEDIUM		4
 #define BT_RSSI_STATE_STAY_LOW			5
 
 #define	BT_AGCTABLE_OFF				0
@@ -1579,8 +1537,6 @@ struct bt_coexist_str {
 void BTDM_CheckAntSelMode(struct rtw_adapter * padapter);
 void BTDM_FwC2hBtRssi(struct rtw_adapter * padapter, u8 *tmpBuf);
 #define BT_FwC2hBtRssi BTDM_FwC2hBtRssi
-void BTDM_FwC2hBtInfo(struct rtw_adapter * padapter, u8 *tmpBuf, u8 length);
-#define BT_FwC2hBtInfo BTDM_FwC2hBtInfo
 void BTDM_DisplayBtCoexInfo(struct rtw_adapter * padapter);
 #define BT_DisplayBtCoexInfo BTDM_DisplayBtCoexInfo
 void BTDM_RejectAPAggregatedPacket(struct rtw_adapter * padapter, u8 bReject);
@@ -1591,8 +1547,6 @@ s32 BTDM_GetRxSS(struct rtw_adapter * padapter);
 u8 BTDM_CheckCoexBcnRssiState(struct rtw_adapter * padapter, u8 levelNum, u8 RssiThresh, u8 RssiThresh1);
 u8 BTDM_CheckCoexRSSIState1(struct rtw_adapter * padapter, u8 levelNum, u8 RssiThresh, u8 RssiThresh1);
 u8 BTDM_CheckCoexRSSIState(struct rtw_adapter * padapter, u8 levelNum, u8 RssiThresh, u8 RssiThresh1);
-u8 BTDM_DisableEDCATurbo(struct rtw_adapter * padapter);
-#define BT_DisableEDCATurbo BTDM_DisableEDCATurbo
 void BTDM_Balance(struct rtw_adapter * padapter, u8 bBalanceOn, u8 ms0, u8 ms1);
 void BTDM_AGCTable(struct rtw_adapter * padapter, u8 type);
 void BTDM_BBBackOffLevel(struct rtw_adapter * padapter, u8 type);
@@ -1602,8 +1556,6 @@ void BTDM_HWCoexAllOff(struct rtw_adapter * padapter);
 void BTDM_CoexAllOff(struct rtw_adapter * padapter);
 void BTDM_TurnOffBtCoexistBeforeEnterIPS(struct rtw_adapter * padapter);
 void BTDM_SignalCompensation(struct rtw_adapter * padapter, u8 *rssi_wifi, u8 *rssi_bt);
-void BTDM_Coexist(struct rtw_adapter * padapter);
-#define BT_CoexistMechanism BTDM_Coexist
 void BTDM_UpdateCoexState(struct rtw_adapter * padapter);
 u8 BTDM_IsSameCoexistState(struct rtw_adapter * padapter);
 void BTDM_PWDBMonitor(struct rtw_adapter * padapter);
@@ -1632,15 +1584,11 @@ u8 BTDM_IsActionPAN(struct rtw_adapter * padapter);
 u8 BTDM_IsActionHIDA2DP(struct rtw_adapter * padapter);
 u8 BTDM_IsActionHIDPAN(struct rtw_adapter * padapter);
 u8 BTDM_IsActionPANA2DP(struct rtw_adapter * padapter);
-u8 BTDM_IsBtDisabled(struct rtw_adapter * padapter);
-#define BT_IsBtDisabled BTDM_IsBtDisabled
 u32 BTDM_BtTxRxCounterH(struct rtw_adapter * padapter);
 u32 BTDM_BtTxRxCounterL(struct rtw_adapter * padapter);
 
 /*  ===== End of sync from SD7 driver HAL/BTCoexist/HalBtCoexist.h ===== */
-#endif /*  __HALBTCOEXIST_C__ */
 
-#ifdef __HALBT_C__ /*  HAL/HalBT.h */
 /*  ===== Below this line is sync from SD7 driver HAL/HalBT.h ===== */
 
 #define RTS_CTS_NO_LEN_LIMIT	0
@@ -1649,18 +1597,12 @@ u8 HALBT_GetPGAntNum(struct rtw_adapter * padapter);
 #define BT_GetPGAntNum HALBT_GetPGAntNum
 void HALBT_SetKey(struct rtw_adapter * padapter, u8 EntryNum);
 void HALBT_RemoveKey(struct rtw_adapter * padapter, u8 EntryNum);
-void HALBT_InitBTVars8723A(struct rtw_adapter * padapter);
-#define HALBT_InitHalVars HALBT_InitBTVars8723A
-#define BT_InitHalVars HALBT_InitHalVars
 u8 HALBT_IsBTExist(struct rtw_adapter * padapter);
 #define BT_IsBtExist HALBT_IsBTExist
 u8 HALBT_BTChipType(struct rtw_adapter * padapter);
-void HALBT_InitHwConfig(struct rtw_adapter * padapter);
-#define BT_InitHwConfig HALBT_InitHwConfig
 void HALBT_SetRtsCtsNoLenLimit(struct rtw_adapter * padapter);
 
 /*  ===== End of sync from SD7 driver HAL/HalBT.c ===== */
-#endif /*  __HALBT_C__ */
 
 #define _bt_dbg_off_		0
 #define _bt_dbg_on_		1
diff --git a/drivers/staging/rtl8723au/include/rtl8723a_bt_intf.h b/drivers/staging/rtl8723au/include/rtl8723a_bt_intf.h
new file mode 100644
index 00000000000..4733559970e
--- /dev/null
+++ b/drivers/staging/rtl8723au/include/rtl8723a_bt_intf.h
@@ -0,0 +1,69 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ * Copyright(c) 2014, Jes Sorensen <Jes.Sorensen@redhat.com>
+ *
+ * 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.
+ *
+ * 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 __RTL8723A_BT_INTF_H__
+#define __RTL8723A_BT_INTF_H__
+
+#include <drv_types.h>
+
+#ifdef CONFIG_8723AU_BT_COEXIST
+enum rt_media_status;
+bool rtl8723a_BT_using_antenna_1(struct rtw_adapter *padapter);
+bool rtl8723a_BT_enabled(struct rtw_adapter *padapter);
+bool rtl8723a_BT_coexist(struct rtw_adapter *padapter);
+void rtl8723a_BT_do_coexist(struct rtw_adapter *padapter);
+void rtl8723a_BT_wifiscan_notify(struct rtw_adapter *padapter, u8 scanType);
+void rtl8723a_BT_mediastatus_notify(struct rtw_adapter *padapter,
+				    enum rt_media_status mstatus);
+void rtl8723a_BT_specialpacket_notify(struct rtw_adapter *padapter);
+void rtl8723a_BT_lps_leave(struct rtw_adapter *padapter);
+void rtl8723a_BT_disable_coexist(struct rtw_adapter *padapter);
+bool rtl8723a_BT_disable_EDCA_turbo(struct rtw_adapter *padapter);
+void rtl8723a_dual_antenna_detection(struct rtw_adapter *padapter);
+void rtl8723a_BT_init_hwconfig(struct rtw_adapter *padapter);
+void rtl8723a_BT_wifiassociate_notify(struct rtw_adapter *padapter, u8 action);
+void rtl8723a_BT_init_hal_vars(struct rtw_adapter *padapter);
+void rtl8723a_fw_c2h_BT_info(struct rtw_adapter *padapter, u8 *tmpBuf, u8 length);
+#else
+static inline bool rtl8723a_BT_using_antenna_1(struct rtw_adapter *padapter)
+{
+	return false;
+}
+static inline bool rtl8723a_BT_enabled(struct rtw_adapter *padapter)
+{
+	return false;
+}
+static inline bool rtl8723a_BT_coexist(struct rtw_adapter *padapter)
+{
+	return false;
+}
+#define rtl8723a_BT_do_coexist(padapter)			do {} while(0)
+#define rtl8723a_BT_wifiscan_notify(padapter, scanType)		do {} while(0)
+#define rtl8723a_BT_mediastatus_notify(padapter, mstatus)	do {} while(0)
+#define rtl8723a_BT_specialpacket_notify(padapter)		do {} while(0)
+#define rtl8723a_BT_lps_leave(padapter)				do {} while(0)
+#define rtl8723a_BT_disable_coexist(padapter)			do {} while(0)
+static inline bool rtl8723a_BT_disable_EDCA_turbo(struct rtw_adapter *padapter)
+{
+	return false;
+}
+#define rtl8723a_dual_antenna_detection(padapter)		do {} while(0)
+#define rtl8723a_BT_init_hwconfig(padapter)			do {} while(0)
+#define rtl8723a_BT_wifiassociate_notify(padapter, action)	do {} while(0)
+#define rtl8723a_BT_init_hal_vars(padapter)			do {} while(0)
+#define rtl8723a_fw_c2h_BT_info(padapter, tmpBuf, length)	do {} while(0)
+#endif
+
+#endif
diff --git a/drivers/staging/rtl8723au/include/rtl8723a_cmd.h b/drivers/staging/rtl8723au/include/rtl8723a_cmd.h
index 8fccbfc3a45..900baccc3c2 100644
--- a/drivers/staging/rtl8723au/include/rtl8723a_cmd.h
+++ b/drivers/staging/rtl8723au/include/rtl8723a_cmd.h
@@ -142,18 +142,16 @@ struct lowpwr_lps_parm{
 
 
 /*  host message to firmware cmd */
-void rtl8723a_set_FwPwrMode_cmd(struct rtw_adapter * padapter, u8 Mode);
-void rtl8723a_set_FwJoinBssReport_cmd(struct rtw_adapter * padapter, u8 mstatus);
+void rtl8723a_set_FwPwrMode_cmd(struct rtw_adapter *padapter, u8 Mode);
+void rtl8723a_set_FwJoinBssReport_cmd(struct rtw_adapter *padapter, u8 mstatus);
 #ifdef CONFIG_8723AU_BT_COEXIST
-void rtl8723a_set_BTCoex_AP_mode_FwRsvdPkt_cmd(struct rtw_adapter * padapter);
+void rtl8723a_set_BTCoex_AP_mode_FwRsvdPkt_cmd(struct rtw_adapter *padapter);
+#else
+#define rtl8723a_set_BTCoex_AP_mode_FwRsvdPkt_cmd(padapter) do {} while(0)
 #endif
-u8 rtl8723a_set_rssi_cmd(struct rtw_adapter * padapter, u8 *param);
-u8 rtl8723a_set_raid_cmd(struct rtw_adapter * padapter, u32 mask, u8 arg);
-void rtl8723a_add_rateatid(struct rtw_adapter * padapter, u32 bitmap, u8 arg, u8 rssi_level);
-
-#ifdef CONFIG_8723AU_P2P
-void rtl8723a_set_p2p_ps_offload_cmd(struct rtw_adapter * padapter, u8 p2p_ps_state);
-#endif /* CONFIG_8723AU_P2P */
+int rtl8723a_set_rssi_cmd(struct rtw_adapter *padapter, u8 *param);
+int rtl8723a_set_raid_cmd(struct rtw_adapter *padapter, u32 mask, u8 arg);
+void rtl8723a_add_rateatid(struct rtw_adapter *padapter, u32 bitmap, u8 arg, u8 rssi_level);
 
 void CheckFwRsvdPageContent23a(struct rtw_adapter *padapter);
 
diff --git a/drivers/staging/rtl8723au/include/rtl8723a_dm.h b/drivers/staging/rtl8723au/include/rtl8723a_dm.h
index 47e887f5bb2..ccac672c9c0 100644
--- a/drivers/staging/rtl8723au/include/rtl8723a_dm.h
+++ b/drivers/staging/rtl8723au/include/rtl8723a_dm.h
@@ -59,8 +59,6 @@ struct dm_priv
 	#endif
 
 	/* for High Power */
-	u8 bDynamicTxPowerEnable;
-	u8 LastDTPLvl;
 	u8 DynamicTxHighPowerLvl;/* Add by Jacken Tx Power Control for Near/Far Range 2008/03/06 */
 
 	/* for tx power tracking */
diff --git a/drivers/staging/rtl8723au/include/rtl8723a_hal.h b/drivers/staging/rtl8723au/include/rtl8723a_hal.h
index c20248bab71..344b7085f87 100644
--- a/drivers/staging/rtl8723au/include/rtl8723a_hal.h
+++ b/drivers/staging/rtl8723au/include/rtl8723a_hal.h
@@ -20,6 +20,7 @@
 #include "Hal8723APhyReg.h"
 #include "Hal8723APhyCfg.h"
 #include "rtl8723a_rf.h"
+#include "rtl8723a_bt_intf.h"
 #ifdef CONFIG_8723AU_BT_COEXIST
 #include "rtl8723a_bt-coexist.h"
 #endif
@@ -29,25 +30,16 @@
 #include "rtl8723a_cmd.h"
 #include "rtl8723a_sreset.h"
 #include "rtw_efuse.h"
+#include "rtw_eeprom.h"
 
 #include "odm_precomp.h"
+#include "odm.h"
 
 
 /* 2TODO: We should define 8192S firmware related macro settings here!! */
 #define RTL819X_DEFAULT_RF_TYPE			RF_1T2R
 #define RTL819X_TOTAL_RF_PATH				2
 
-/* TODO:  The following need to check!! */
-#define RTL8723_FW_UMC_IMG				"rtl8192CU\\rtl8723fw.bin"
-#define RTL8723_FW_UMC_B_IMG			"rtl8192CU\\rtl8723fw_B.bin"
-#define RTL8723_PHY_REG					"rtl8723S\\PHY_REG_1T.txt"
-#define RTL8723_PHY_RADIO_A				"rtl8723S\\radio_a_1T.txt"
-#define RTL8723_PHY_RADIO_B				"rtl8723S\\radio_b_1T.txt"
-#define RTL8723_AGC_TAB					"rtl8723S\\AGC_TAB_1T.txt"
-#define RTL8723_PHY_MACREG				"rtl8723S\\MAC_REG.txt"
-#define RTL8723_PHY_REG_PG				"rtl8723S\\PHY_REG_PG.txt"
-#define RTL8723_PHY_REG_MP				"rtl8723S\\PHY_REG_MP.txt"
-
 /*  */
 /*		RTL8723S From header */
 /*  */
@@ -424,27 +416,9 @@ struct hal_data_8723a {
 
 	bool				bMACFuncEnable;
 
-#ifdef CONFIG_8723AU_P2P
-	struct P2P_PS_Offload_t	p2p_ps_offload;
-#endif
-
-
-	/*  */
-	/*  For USB Interface HAL related */
-	/*  */
-	u32	UsbBulkOutSize;
-
 	/*  Interrupt related register information. */
 	u32	IntArray[2];
 	u32	IntrMask[2];
-
-	/*  */
-	/*  For SDIO Interface HAL related */
-	/*  */
-
-	/*  Auto FSM to Turn On, include clock, isolation, power control for MAC only */
-	u8			bMacPwrCtrlOn;
-
 };
 
 #define GET_HAL_DATA(__pAdapter)	((struct hal_data_8723a *)((__pAdapter)->HalData))
@@ -551,25 +525,33 @@ void Hal_EfuseParseCustomerID(struct rtw_adapter *padapter, u8 *hwinfo, bool Aut
 void Hal_EfuseParseAntennaDiversity(struct rtw_adapter *padapter, u8 *hwinfo, bool AutoLoadFail);
 void Hal_EfuseParseRateIndicationOption(struct rtw_adapter *padapter, u8 *hwinfo, bool AutoLoadFail);
 void Hal_EfuseParseXtal_8723A(struct rtw_adapter *pAdapter, u8 *hwinfo, u8 AutoLoadFail);
-void Hal_EfuseParseThermalMeter_8723A(struct rtw_adapter *padapter, u8 *hwinfo, u8 AutoLoadFail);
+void Hal_EfuseParseThermalMeter_8723A(struct rtw_adapter *padapter, u8 *hwinfo, bool AutoLoadFail);
 
 void Hal_InitChannelPlan23a(struct rtw_adapter *padapter);
 
-void rtl8723a_set_hal_ops(struct hal_ops *pHalFunc);
-void SetHwReg8723A(struct rtw_adapter *padapter, u8 variable, u8 *val);
-void GetHwReg8723A(struct rtw_adapter *padapter, u8 variable, u8 *val);
-#ifdef CONFIG_8723AU_BT_COEXIST
-void rtl8723a_SingleDualAntennaDetection(struct rtw_adapter *padapter);
-#endif
-
 /*  register */
 void SetBcnCtrlReg23a(struct rtw_adapter *padapter, u8 SetBits, u8 ClearBits);
 void rtl8723a_InitBeaconParameters(struct rtw_adapter *padapter);
 
-void rtl8723a_clone_haldata(struct rtw_adapter *dst_adapter, struct rtw_adapter *src_adapter);
 void rtl8723a_start_thread(struct rtw_adapter *padapter);
 void rtl8723a_stop_thread(struct rtw_adapter *padapter);
 
-s32 c2h_id_filter_ccx_8723a(u8 id);
+bool c2h_id_filter_ccx_8723a(u8 id);
+int c2h_handler_8723a(struct rtw_adapter *padapter, struct c2h_evt_hdr *c2h_evt);
+
+void rtl8723a_read_adapter_info(struct rtw_adapter *Adapter);
+void rtl8723a_read_chip_version(struct rtw_adapter *padapter);
+void rtl8723a_notch_filter(struct rtw_adapter *adapter, bool enable);
+void rtl8723a_SetBeaconRelatedRegisters(struct rtw_adapter *padapter);
+void rtl8723a_SetHalODMVar(struct rtw_adapter *Adapter,
+			   enum hal_odm_variable eVariable,
+			   void *pValue1, bool bSet);
+void
+rtl8723a_readefuse(struct rtw_adapter *padapter,
+		   u8 efuseType, u16 _offset, u16 _size_byte, u8 *pbuf);
+u16 rtl8723a_EfuseGetCurrentSize_WiFi(struct rtw_adapter *padapter);
+u16 rtl8723a_EfuseGetCurrentSize_BT(struct rtw_adapter *padapter);
+void rtl8723a_update_ramask(struct rtw_adapter *padapter,
+			    u32 mac_id, u8 rssi_level);
 
 #endif
diff --git a/drivers/staging/rtl8723au/include/rtl8723a_recv.h b/drivers/staging/rtl8723au/include/rtl8723a_recv.h
index 6bf6904f4d4..885d4d3859f 100644
--- a/drivers/staging/rtl8723au/include/rtl8723a_recv.h
+++ b/drivers/staging/rtl8723au/include/rtl8723a_recv.h
@@ -18,21 +18,21 @@
 #include <osdep_service.h>
 #include <drv_types.h>
 
-#define NR_RECVBUFF (4)
+#define NR_RECVBUFF			4
 
-#define NR_PREALLOC_RECV_SKB (8)
+#define NR_PREALLOC_RECV_SKB		8
 
-#define RECV_BLK_SZ 512
-#define RECV_BLK_CNT 16
-#define RECV_BLK_TH RECV_BLK_CNT
+#define RECV_BLK_SZ			512
+#define RECV_BLK_CNT			16
+#define RECV_BLK_TH			RECV_BLK_CNT
 
-#define MAX_RECVBUF_SZ (15360) /*  15k < 16k */
+#define MAX_RECVBUF_SZ			15360 /*  15k < 16k */
 
 #define RECV_BULK_IN_ADDR		0x80
 #define RECV_INT_IN_ADDR		0x81
 
-#define PHY_RSSI_SLID_WIN_MAX				100
-#define PHY_LINKQUALITY_SLID_WIN_MAX		20
+#define PHY_RSSI_SLID_WIN_MAX		100
+#define PHY_LINKQUALITY_SLID_WIN_MAX	20
 
 
 struct phy_stat
@@ -48,7 +48,7 @@ struct phy_stat
 };
 
 /*  Rx smooth factor */
-#define	Rx_Smooth_Factor (20)
+#define	Rx_Smooth_Factor		20
 
 struct interrupt_msg_format {
 	unsigned int C2H_MSG0;
@@ -60,8 +60,7 @@ struct interrupt_msg_format {
 	unsigned int  MSG_EX;
 };
 
-void rtl8723au_init_recvbuf(struct rtw_adapter *padapter, struct recv_buf *precvbuf);
-int	rtl8723au_init_recv_priv(struct rtw_adapter * padapter);
+int rtl8723au_init_recv_priv(struct rtw_adapter * padapter);
 void rtl8723au_free_recv_priv(struct rtw_adapter * padapter);
 void rtl8723a_process_phy_info(struct rtw_adapter *padapter, void *prframe);
 void update_recvframe_attrib(struct recv_frame *precvframe, struct recv_stat *prxstat);
diff --git a/drivers/staging/rtl8723au/include/rtl8723a_spec.h b/drivers/staging/rtl8723au/include/rtl8723a_spec.h
index 3595c27907d..b5d7123f787 100644
--- a/drivers/staging/rtl8723au/include/rtl8723a_spec.h
+++ b/drivers/staging/rtl8723au/include/rtl8723a_spec.h
@@ -481,11 +481,11 @@
 /* 8192C Cmd9346CR bits			(Offset 0xA, 16bit) */
 /*  */
 	 /*  EEPROM enable when set 1 */
-#define	CmdEEPROM_En			BIT5
+#define	CmdEEPROM_En			BIT(5)
 	/*  System EEPROM select, 0: boot from E-FUSE,
 	    1: The EEPROM used is 9346 */
-#define	CmdEERPOMSEL			BIT4
-#define	Cmd9346CR_9356SEL		BIT4
+#define	CmdEERPOMSEL			BIT(4)
+#define	Cmd9346CR_9356SEL		BIT(4)
 #define	AutoLoadEEPROM			(CmdEEPROM_En|CmdEERPOMSEL)
 #define	AutoLoadEFUSE			CmdEEPROM_En
 
@@ -493,7 +493,7 @@
 /* 8192C GPIO MUX Configuration Register (offset 0x40, 4 byte) */
 /*  */
 #define	GPIOSEL_GPIO			0
-#define	GPIOSEL_ENBT			BIT5
+#define	GPIOSEL_ENBT			BIT(5)
 
 /*  */
 /*        8192C GPIO PIN Control Register (offset 0x44, 4 byte) */
@@ -535,43 +535,43 @@ Default: 00b.
 #define	RRSR_RSC_UPSUBCHNL		0x400000
 #define	RRSR_RSC_LOWSUBCHNL		0x200000
 #define	RRSR_SHORT			0x800000
-#define	RRSR_1M				BIT0
-#define	RRSR_2M				BIT1
-#define	RRSR_5_5M			BIT2
-#define	RRSR_11M			BIT3
-#define	RRSR_6M				BIT4
-#define	RRSR_9M				BIT5
-#define	RRSR_12M			BIT6
-#define	RRSR_18M			BIT7
-#define	RRSR_24M			BIT8
-#define	RRSR_36M			BIT9
-#define	RRSR_48M			BIT10
-#define	RRSR_54M			BIT11
-#define	RRSR_MCS0			BIT12
-#define	RRSR_MCS1			BIT13
-#define	RRSR_MCS2			BIT14
-#define	RRSR_MCS3			BIT15
-#define	RRSR_MCS4			BIT16
-#define	RRSR_MCS5			BIT17
-#define	RRSR_MCS6			BIT18
-#define	RRSR_MCS7			BIT19
-#define	BRSR_AckShortPmb		BIT23
+#define	RRSR_1M				BIT(0)
+#define	RRSR_2M				BIT(1)
+#define	RRSR_5_5M			BIT(2)
+#define	RRSR_11M			BIT(3)
+#define	RRSR_6M				BIT(4)
+#define	RRSR_9M				BIT(5)
+#define	RRSR_12M			BIT(6)
+#define	RRSR_18M			BIT(7)
+#define	RRSR_24M			BIT(8)
+#define	RRSR_36M			BIT(9)
+#define	RRSR_48M			BIT(10)
+#define	RRSR_54M			BIT(11)
+#define	RRSR_MCS0			BIT(12)
+#define	RRSR_MCS1			BIT(13)
+#define	RRSR_MCS2			BIT(14)
+#define	RRSR_MCS3			BIT(15)
+#define	RRSR_MCS4			BIT(16)
+#define	RRSR_MCS5			BIT(17)
+#define	RRSR_MCS6			BIT(18)
+#define	RRSR_MCS7			BIT(19)
+#define	BRSR_AckShortPmb		BIT(23)
 /*  CCK ACK: use Short Preamble or not */
 
 /*  */
 /* 8192C BW_OPMODE bits			(Offset 0x203, 8bit) */
 /*  */
-#define	BW_OPMODE_20MHZ			BIT2
-#define	BW_OPMODE_5G			BIT1
-#define	BW_OPMODE_11J			BIT0
+#define	BW_OPMODE_20MHZ			BIT(2)
+#define	BW_OPMODE_5G			BIT(1)
+#define	BW_OPMODE_11J			BIT(0)
 
 
 /*  */
 /* 8192C CAM Config Setting (offset 0x250, 1 byte) */
 /*  */
-#define	CAM_VALID			BIT15
+#define	CAM_VALID			BIT(15)
 #define	CAM_NOTVALID			0x0000
-#define	CAM_USEDK			BIT5
+#define	CAM_USEDK			BIT(5)
 
 #define	CAM_CONTENT_COUNT		8
 
@@ -587,9 +587,9 @@ Default: 00b.
 #define	CAM_CONFIG_USEDK		true
 #define	CAM_CONFIG_NO_USEDK		false
 
-#define	CAM_WRITE			BIT16
+#define	CAM_WRITE			BIT(16)
 #define	CAM_READ			0x00000000
-#define	CAM_POLLINIG			BIT31
+#define	CAM_POLLINIG			BIT(31)
 
 #define	SCR_UseDK			0x01
 #define	SCR_TxSecEnable			0x02
@@ -605,54 +605,54 @@ Default: 00b.
 #define	IMR8190_DISABLED		0x0
 /*  IMR DW0 Bit 0-31 */
 
-#define	IMR_BCNDMAINT6			BIT31	/*  Beacon DMA Interrupt 6 */
-#define	IMR_BCNDMAINT5			BIT30	/*  Beacon DMA Interrupt 5 */
-#define	IMR_BCNDMAINT4			BIT29	/*  Beacon DMA Interrupt 4 */
-#define	IMR_BCNDMAINT3			BIT28	/*  Beacon DMA Interrupt 3 */
-#define	IMR_BCNDMAINT2			BIT27	/*  Beacon DMA Interrupt 2 */
-#define	IMR_BCNDMAINT1			BIT26	/*  Beacon DMA Interrupt 1 */
-#define	IMR_BCNDOK8			BIT25	/*  Beacon Queue DMA OK
+#define	IMR_BCNDMAINT6			BIT(31)	/*  Beacon DMA Interrupt 6 */
+#define	IMR_BCNDMAINT5			BIT(30)	/*  Beacon DMA Interrupt 5 */
+#define	IMR_BCNDMAINT4			BIT(29)	/*  Beacon DMA Interrupt 4 */
+#define	IMR_BCNDMAINT3			BIT(28)	/*  Beacon DMA Interrupt 3 */
+#define	IMR_BCNDMAINT2			BIT(27)	/*  Beacon DMA Interrupt 2 */
+#define	IMR_BCNDMAINT1			BIT(26)	/*  Beacon DMA Interrupt 1 */
+#define	IMR_BCNDOK8			BIT(25)	/*  Beacon Queue DMA OK
 						    Interrupt 8 */
-#define	IMR_BCNDOK7			BIT24	/*  Beacon Queue DMA OK
+#define	IMR_BCNDOK7			BIT(24)	/*  Beacon Queue DMA OK
 						    Interrupt 7 */
-#define	IMR_BCNDOK6			BIT23	/*  Beacon Queue DMA OK
+#define	IMR_BCNDOK6			BIT(23)	/*  Beacon Queue DMA OK
 						    Interrupt 6 */
-#define	IMR_BCNDOK5			BIT22	/*  Beacon Queue DMA OK
+#define	IMR_BCNDOK5			BIT(22)	/*  Beacon Queue DMA OK
 						    Interrupt 5 */
-#define	IMR_BCNDOK4			BIT21	/*  Beacon Queue DMA OK
+#define	IMR_BCNDOK4			BIT(21)	/*  Beacon Queue DMA OK
 						    Interrupt 4 */
-#define	IMR_BCNDOK3			BIT20	/*  Beacon Queue DMA OK
+#define	IMR_BCNDOK3			BIT(20)	/*  Beacon Queue DMA OK
 						    Interrupt 3 */
-#define	IMR_BCNDOK2			BIT19	/*  Beacon Queue DMA OK
+#define	IMR_BCNDOK2			BIT(19)	/*  Beacon Queue DMA OK
 						    Interrupt 2 */
-#define	IMR_BCNDOK1			BIT18	/*  Beacon Queue DMA OK
+#define	IMR_BCNDOK1			BIT(18)	/*  Beacon Queue DMA OK
 						    Interrupt 1 */
-#define	IMR_TIMEOUT2			BIT17	/*  Timeout interrupt 2 */
-#define	IMR_TIMEOUT1			BIT16	/*  Timeout interrupt 1 */
-#define	IMR_TXFOVW			BIT15	/*  Transmit FIFO Overflow */
-#define	IMR_PSTIMEOUT			BIT14	/*  Power save time out
+#define	IMR_TIMEOUT2			BIT(17)	/*  Timeout interrupt 2 */
+#define	IMR_TIMEOUT1			BIT(16)	/*  Timeout interrupt 1 */
+#define	IMR_TXFOVW			BIT(15)	/*  Transmit FIFO Overflow */
+#define	IMR_PSTIMEOUT			BIT(14)	/*  Power save time out
 						    interrupt */
-#define	IMR_BcnInt			BIT13	/*  Beacon DMA Interrupt 0 */
-#define	IMR_RXFOVW			BIT12	/*  Receive FIFO Overflow */
-#define	IMR_RDU				BIT11	/*  Receive Descriptor
+#define	IMR_BcnInt			BIT(13)	/*  Beacon DMA Interrupt 0 */
+#define	IMR_RXFOVW			BIT(12)	/*  Receive FIFO Overflow */
+#define	IMR_RDU				BIT(11)	/*  Receive Descriptor
 						    Unavailable */
-#define	IMR_ATIMEND			BIT10	/*  For 92C,ATIM Window
+#define	IMR_ATIMEND			BIT(10)	/*  For 92C,ATIM Window
 						    End Interrupt */
-#define	IMR_BDOK			BIT9	/*  Beacon Queue DMA OK
+#define	IMR_BDOK			BIT(9)	/*  Beacon Queue DMA OK
 						    Interrup */
-#define	IMR_HIGHDOK			BIT8	/*  High Queue DMA OK
+#define	IMR_HIGHDOK			BIT(8)	/*  High Queue DMA OK
 						    Interrupt */
-#define	IMR_TBDOK			BIT7	/*  Transmit Beacon OK
+#define	IMR_TBDOK			BIT(7)	/*  Transmit Beacon OK
 						    interrup */
-#define	IMR_MGNTDOK			BIT6	/*  Management Queue DMA OK
+#define	IMR_MGNTDOK			BIT(6)	/*  Management Queue DMA OK
 						    Interrupt */
-#define	IMR_TBDER			BIT5	/*  For 92C,Transmit Beacon
+#define	IMR_TBDER			BIT(5)	/*  For 92C,Transmit Beacon
 						    Error Interrupt */
-#define	IMR_BKDOK			BIT4	/*  AC_BK DMA OK Interrupt */
-#define	IMR_BEDOK			BIT3	/*  AC_BE DMA OK Interrupt */
-#define	IMR_VIDOK			BIT2	/*  AC_VI DMA OK Interrupt */
-#define	IMR_VODOK			BIT1	/*  AC_VO DMA Interrupt */
-#define	IMR_ROK				BIT0	/*  Receive DMA OK Interrupt */
+#define	IMR_BKDOK			BIT(4)	/*  AC_BK DMA OK Interrupt */
+#define	IMR_BEDOK			BIT(3)	/*  AC_BE DMA OK Interrupt */
+#define	IMR_VIDOK			BIT(2)	/*  AC_VI DMA OK Interrupt */
+#define	IMR_VODOK			BIT(1)	/*  AC_VO DMA Interrupt */
+#define	IMR_ROK				BIT(0)	/*  Receive DMA OK Interrupt */
 
 #define	IMR_RX_MASK			(IMR_ROK|IMR_RDU|IMR_RXFOVW)
 #define	IMR_TX_MASK			(IMR_VODOK|IMR_VIDOK|IMR_BEDOK| \
@@ -660,14 +660,14 @@ Default: 00b.
 					 IMR_BDOK)
 
 /*  13. Host Interrupt Status Extension Register (Offset: 0x012C-012Eh) */
-#define	IMR_BcnInt_E			BIT12
-#define	IMR_TXERR			BIT11
-#define	IMR_RXERR			BIT10
-#define	IMR_C2HCMD			BIT9
-#define	IMR_CPWM			BIT8
+#define	IMR_BcnInt_E			BIT(12)
+#define	IMR_TXERR			BIT(11)
+#define	IMR_RXERR			BIT(10)
+#define	IMR_C2HCMD			BIT(9)
+#define	IMR_CPWM			BIT(8)
 /* RSVD [2-7] */
-#define	IMR_OCPINT			BIT1
-#define	IMR_WLANOFF			BIT0
+#define	IMR_OCPINT			BIT(1)
+#define	IMR_WLANOFF			BIT(0)
 
 
 /*        8192C EEPROM/EFUSE share register definition. */
@@ -913,35 +913,35 @@ Current IOREG MAP
 /*  */
 /* 8192C (RCR) Receive Configuration Register	(Offset 0x608, 32 bits) */
 /*  */
-#define	RCR_APPFCS			BIT31 /* WMAC append FCS after payload*/
-#define	RCR_APP_MIC			BIT30
-#define	RCR_APP_PHYSTS			BIT28
-#define	RCR_APP_ICV			BIT29
-#define	RCR_APP_PHYST_RXFF		BIT28
-#define	RCR_APP_BA_SSN			BIT27 /* Accept BA SSN */
-#define	RCR_ENMBID			BIT24 /* Enable Multiple BssId. */
-#define	RCR_LSIGEN			BIT23
-#define	RCR_MFBEN			BIT22
-#define	RCR_HTC_LOC_CTRL		BIT14 /* MFC<--HTC=1 MFC-->HTC=0 */
-#define	RCR_AMF				BIT13 /* Accept management type frame */
-#define	RCR_ACF				BIT12 /* Accept control type frame */
-#define	RCR_ADF				BIT11 /* Accept data type frame */
-#define	RCR_AICV			BIT9  /* Accept ICV error packet */
-#define	RCR_ACRC32			BIT8  /* Accept CRC32 error packet */
-#define	RCR_CBSSID_BCN			BIT7  /* Accept BSSID match packet
+#define	RCR_APPFCS			BIT(31) /* WMAC append FCS after payload*/
+#define	RCR_APP_MIC			BIT(30)
+#define	RCR_APP_PHYSTS			BIT(28)
+#define	RCR_APP_ICV			BIT(29)
+#define	RCR_APP_PHYST_RXFF		BIT(28)
+#define	RCR_APP_BA_SSN			BIT(27) /* Accept BA SSN */
+#define	RCR_ENMBID			BIT(24) /* Enable Multiple BssId. */
+#define	RCR_LSIGEN			BIT(23)
+#define	RCR_MFBEN			BIT(22)
+#define	RCR_HTC_LOC_CTRL		BIT(14) /* MFC<--HTC=1 MFC-->HTC=0 */
+#define	RCR_AMF				BIT(13) /* Accept management type frame */
+#define	RCR_ACF				BIT(12) /* Accept control type frame */
+#define	RCR_ADF				BIT(11) /* Accept data type frame */
+#define	RCR_AICV			BIT(9)  /* Accept ICV error packet */
+#define	RCR_ACRC32			BIT(8)  /* Accept CRC32 error packet */
+#define	RCR_CBSSID_BCN			BIT(7)  /* Accept BSSID match packet
 						 (Rx beacon, probe rsp) */
-#define	RCR_CBSSID_DATA			BIT6  /* Accept BSSID match packet
+#define	RCR_CBSSID_DATA			BIT(6)  /* Accept BSSID match packet
 						 (Data) */
 #define	RCR_CBSSID			RCR_CBSSID_DATA	/* Accept BSSID match
 							   packet */
-#define	RCR_APWRMGT			BIT5  /* Accept power management
+#define	RCR_APWRMGT			BIT(5)  /* Accept power management
 						 packet */
-#define	RCR_ADD3			BIT4  /* Accept address 3 match
+#define	RCR_ADD3			BIT(4)  /* Accept address 3 match
 						 packet */
-#define	RCR_AB				BIT3  /* Accept broadcast packet */
-#define	RCR_AM				BIT2  /* Accept multicast packet */
-#define	RCR_APM				BIT1  /* Accept physical match packet */
-#define	RCR_AAP				BIT0  /* Accept all unicast packet */
+#define	RCR_AB				BIT(3)  /* Accept broadcast packet */
+#define	RCR_AM				BIT(2)  /* Accept multicast packet */
+#define	RCR_APM				BIT(1)  /* Accept physical match packet */
+#define	RCR_AAP				BIT(0)  /* Accept all unicast packet */
 #define	RCR_MXDMA_OFFSET		8
 #define	RCR_FIFO_OFFSET			13
 
@@ -1714,33 +1714,33 @@ Current IOREG MAP
 
 /* 2REG_MULTI_FUNC_CTRL(For RTL8723 Only) */
 	/*  Enable GPIO[9] as WiFi HW PDn source */
-#define	WL_HWPDN_EN			BIT0
+#define	WL_HWPDN_EN			BIT(0)
 	/*  WiFi HW PDn polarity control */
-#define	WL_HWPDN_SL			BIT1
+#define	WL_HWPDN_SL			BIT(1)
 	/*  WiFi function enable */
-#define	WL_FUNC_EN			BIT2
+#define	WL_FUNC_EN			BIT(2)
 	/*  Enable GPIO[9] as WiFi RF HW PDn source */
-#define	WL_HWROF_EN			BIT3
+#define	WL_HWROF_EN			BIT(3)
 	/*  Enable GPIO[11] as BT HW PDn source */
-#define	BT_HWPDN_EN			BIT16
+#define	BT_HWPDN_EN			BIT(16)
 	/*  BT HW PDn polarity control */
-#define	BT_HWPDN_SL			BIT17
+#define	BT_HWPDN_SL			BIT(17)
 	/*  BT function enable */
-#define	BT_FUNC_EN			BIT18
+#define	BT_FUNC_EN			BIT(18)
 	/*  Enable GPIO[11] as BT/GPS RF HW PDn source */
-#define	BT_HWROF_EN			BIT19
+#define	BT_HWROF_EN			BIT(19)
 	/*  Enable GPIO[10] as GPS HW PDn source */
-#define	GPS_HWPDN_EN			BIT20
+#define	GPS_HWPDN_EN			BIT(20)
 	/*  GPS HW PDn polarity control */
-#define	GPS_HWPDN_SL			BIT21
+#define	GPS_HWPDN_SL			BIT(21)
 	/*  GPS function enable */
-#define	GPS_FUNC_EN			BIT22
+#define	GPS_FUNC_EN			BIT(22)
 
 /* 3 REG_LIFECTRL_CTRL */
-#define	HAL92C_EN_PKT_LIFE_TIME_BK	BIT3
-#define	HAL92C_EN_PKT_LIFE_TIME_BE	BIT2
-#define	HAL92C_EN_PKT_LIFE_TIME_VI	BIT1
-#define	HAL92C_EN_PKT_LIFE_TIME_VO	BIT0
+#define	HAL92C_EN_PKT_LIFE_TIME_BK	BIT(3)
+#define	HAL92C_EN_PKT_LIFE_TIME_BE	BIT(2)
+#define	HAL92C_EN_PKT_LIFE_TIME_VI	BIT(1)
+#define	HAL92C_EN_PKT_LIFE_TIME_VO	BIT(0)
 
 #define	HAL92C_MSDU_LIFE_TIME_UNIT	128	/*  in us, said by Tim. */
 
@@ -1933,7 +1933,7 @@ Current IOREG MAP
 					 HSISR_GPIO9_INT)
 
 /* 2 MCUFWDL */
-#define RAM_DL_SEL			BIT7	/*  1:RAM, 0:ROM */
+#define RAM_DL_SEL			BIT(7)	/*  1:RAM, 0:ROM */
 
 /* 2 HPON_FSM */
 
@@ -2083,60 +2083,60 @@ Current IOREG MAP
 
 
 /* USB interrupt */
-#define	UHIMR_TIMEOUT2			BIT31
-#define	UHIMR_TIMEOUT1			BIT30
-#define	UHIMR_PSTIMEOUT			BIT29
-#define	UHIMR_GTINT4			BIT28
-#define	UHIMR_GTINT3			BIT27
-#define	UHIMR_TXBCNERR			BIT26
-#define	UHIMR_TXBCNOK			BIT25
-#define	UHIMR_TSF_BIT32_TOGGLE		BIT24
-#define	UHIMR_BCNDMAINT3		BIT23
-#define	UHIMR_BCNDMAINT2		BIT22
-#define	UHIMR_BCNDMAINT1		BIT21
-#define	UHIMR_BCNDMAINT0		BIT20
-#define	UHIMR_BCNDOK3			BIT19
-#define	UHIMR_BCNDOK2			BIT18
-#define	UHIMR_BCNDOK1			BIT17
-#define	UHIMR_BCNDOK0			BIT16
-#define	UHIMR_HSISR_IND			BIT15
-#define	UHIMR_BCNDMAINT_E		BIT14
-/* RSVD	BIT13 */
-#define	UHIMR_CTW_END			BIT12
-/* RSVD	BIT11 */
-#define	UHIMR_C2HCMD			BIT10
-#define	UHIMR_CPWM2			BIT9
-#define	UHIMR_CPWM			BIT8
-#define	UHIMR_HIGHDOK			BIT7	/*  High Queue DMA OK
+#define	UHIMR_TIMEOUT2			BIT(31)
+#define	UHIMR_TIMEOUT1			BIT(30)
+#define	UHIMR_PSTIMEOUT			BIT(29)
+#define	UHIMR_GTINT4			BIT(28)
+#define	UHIMR_GTINT3			BIT(27)
+#define	UHIMR_TXBCNERR			BIT(26)
+#define	UHIMR_TXBCNOK			BIT(25)
+#define	UHIMR_TSF_BIT32_TOGGLE		BIT(24)
+#define	UHIMR_BCNDMAINT3		BIT(23)
+#define	UHIMR_BCNDMAINT2		BIT(22)
+#define	UHIMR_BCNDMAINT1		BIT(21)
+#define	UHIMR_BCNDMAINT0		BIT(20)
+#define	UHIMR_BCNDOK3			BIT(19)
+#define	UHIMR_BCNDOK2			BIT(18)
+#define	UHIMR_BCNDOK1			BIT(17)
+#define	UHIMR_BCNDOK0			BIT(16)
+#define	UHIMR_HSISR_IND			BIT(15)
+#define	UHIMR_BCNDMAINT_E		BIT(14)
+/* RSVD	BIT(13) */
+#define	UHIMR_CTW_END			BIT(12)
+/* RSVD	BIT(11) */
+#define	UHIMR_C2HCMD			BIT(10)
+#define	UHIMR_CPWM2			BIT(9)
+#define	UHIMR_CPWM			BIT(8)
+#define	UHIMR_HIGHDOK			BIT(7)	/*  High Queue DMA OK
 						    Interrupt */
-#define	UHIMR_MGNTDOK			BIT6	/*  Management Queue DMA OK
+#define	UHIMR_MGNTDOK			BIT(6)	/*  Management Queue DMA OK
 						    Interrupt */
-#define	UHIMR_BKDOK			BIT5	/*  AC_BK DMA OK Interrupt */
-#define	UHIMR_BEDOK			BIT4	/*  AC_BE DMA OK Interrupt */
-#define	UHIMR_VIDOK			BIT3	/*  AC_VI DMA OK Interrupt */
-#define	UHIMR_VODOK			BIT2	/*  AC_VO DMA Interrupt */
-#define	UHIMR_RDU			BIT1	/*  Receive Descriptor
+#define	UHIMR_BKDOK			BIT(5)	/*  AC_BK DMA OK Interrupt */
+#define	UHIMR_BEDOK			BIT(4)	/*  AC_BE DMA OK Interrupt */
+#define	UHIMR_VIDOK			BIT(3)	/*  AC_VI DMA OK Interrupt */
+#define	UHIMR_VODOK			BIT(2)	/*  AC_VO DMA Interrupt */
+#define	UHIMR_RDU			BIT(1)	/*  Receive Descriptor
 						    Unavailable */
-#define	UHIMR_ROK			BIT0	/*  Receive DMA OK Interrupt */
+#define	UHIMR_ROK			BIT(0)	/*  Receive DMA OK Interrupt */
 
 /*  USB Host Interrupt Status Extension bit */
-#define	UHIMR_BCNDMAINT7		BIT23
-#define	UHIMR_BCNDMAINT6		BIT22
-#define	UHIMR_BCNDMAINT5		BIT21
-#define	UHIMR_BCNDMAINT4		BIT20
-#define	UHIMR_BCNDOK7			BIT19
-#define	UHIMR_BCNDOK6			BIT18
-#define	UHIMR_BCNDOK5			BIT17
-#define	UHIMR_BCNDOK4			BIT16
+#define	UHIMR_BCNDMAINT7		BIT(23)
+#define	UHIMR_BCNDMAINT6		BIT(22)
+#define	UHIMR_BCNDMAINT5		BIT(21)
+#define	UHIMR_BCNDMAINT4		BIT(20)
+#define	UHIMR_BCNDOK7			BIT(19)
+#define	UHIMR_BCNDOK6			BIT(18)
+#define	UHIMR_BCNDOK5			BIT(17)
+#define	UHIMR_BCNDOK4			BIT(16)
 /*  bit14-15: RSVD */
-#define	UHIMR_ATIMEND_E			BIT13
-#define	UHIMR_ATIMEND			BIT12
-#define	UHIMR_TXERR			BIT11
-#define	UHIMR_RXERR			BIT10
-#define	UHIMR_TXFOVW			BIT9
-#define	UHIMR_RXFOVW			BIT8
+#define	UHIMR_ATIMEND_E			BIT(13)
+#define	UHIMR_ATIMEND			BIT(12)
+#define	UHIMR_TXERR			BIT(11)
+#define	UHIMR_RXERR			BIT(10)
+#define	UHIMR_TXFOVW			BIT(9)
+#define	UHIMR_RXFOVW			BIT(8)
 /*  bit2-7: RSVD */
-#define	UHIMR_OCPINT			BIT1
+#define	UHIMR_OCPINT			BIT(1)
 /*  bit0: RSVD */
 
 #define	REG_USB_HIMR			0xFE38
diff --git a/drivers/staging/rtl8723au/include/rtl8723a_xmit.h b/drivers/staging/rtl8723au/include/rtl8723a_xmit.h
index 3b6fdc3a9b7..79883d7b867 100644
--- a/drivers/staging/rtl8723au/include/rtl8723a_xmit.h
+++ b/drivers/staging/rtl8723au/include/rtl8723a_xmit.h
@@ -216,14 +216,12 @@ void handle_txrpt_ccx_8723a(struct rtw_adapter *adapter, void *buf);
 void rtl8723a_update_txdesc(struct xmit_frame *pxmitframe, u8 *pmem);
 void rtl8723a_fill_fake_txdesc(struct rtw_adapter *padapter, u8 *pDesc, u32 BufferLen, u8 IsPsPoll, u8 IsBTQosNull);
 
-s32	rtl8723au_hal_xmitframe_enqueue(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe);
+int rtl8723au_hal_xmitframe_enqueue(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe);
 s32 rtl8723au_xmit_buf_handler(struct rtw_adapter *padapter);
 #define hal_xmit_handler rtl8723au_xmit_buf_handler
-s32	rtl8723au_init_xmit_priv(struct rtw_adapter * padapter);
-void	rtl8723au_free_xmit_priv(struct rtw_adapter * padapter);
-s32 rtl8723au_hal_xmit(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe);
-s32 rtl8723au_mgnt_xmit(struct rtw_adapter *padapter, struct xmit_frame *pmgntframe);
-s32 rtl8723au_xmitframe_complete(struct rtw_adapter *padapter, struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf);
+bool rtl8723au_hal_xmit(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe);
+int rtl8723au_mgnt_xmit(struct rtw_adapter *padapter, struct xmit_frame *pmgntframe);
+bool rtl8723au_xmitframe_complete(struct rtw_adapter *padapter, struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf);
 
 
 #endif
diff --git a/drivers/staging/rtl8723au/include/rtw_ap.h b/drivers/staging/rtl8723au/include/rtw_ap.h
index 76f82d68f63..8d9be5a36ea 100644
--- a/drivers/staging/rtl8723au/include/rtw_ap.h
+++ b/drivers/staging/rtl8723au/include/rtw_ap.h
@@ -22,8 +22,6 @@
 #ifdef CONFIG_8723AU_AP_MODE
 
 /* external function */
-void rtw_indicate_sta_assoc_event23a(struct rtw_adapter *padapter, struct sta_info *psta);
-void rtw_indicate_sta_disassoc_event23a(struct rtw_adapter *padapter, struct sta_info *psta);
 
 void init_mlme_ap_info23a(struct rtw_adapter *padapter);
 void free_mlme_ap_info23a(struct rtw_adapter *padapter);
@@ -34,7 +32,7 @@ void update_beacon23a(struct rtw_adapter *padapter, u8 ie_id, u8 *oui, u8 tx);
 void add_RATid23a(struct rtw_adapter *padapter, struct sta_info *psta, u8 rssi_level);
 void expire_timeout_chk23a(struct rtw_adapter *padapter);
 void update_sta_info23a_apmode23a(struct rtw_adapter *padapter, struct sta_info *psta);
-int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf,  int len);
+int rtw_check_beacon_data23a(struct rtw_adapter *padapter, u8 *pbuf, unsigned int len);
 void rtw_ap_restore_network(struct rtw_adapter *padapter);
 void rtw_set_macaddr_acl23a(struct rtw_adapter *padapter, int mode);
 int rtw_acl_add_sta23a(struct rtw_adapter *padapter, u8 *addr);
diff --git a/drivers/staging/rtl8723au/include/rtw_cmd.h b/drivers/staging/rtl8723au/include/rtw_cmd.h
index f9caa3e35f5..c49237b04a3 100644
--- a/drivers/staging/rtl8723au/include/rtw_cmd.h
+++ b/drivers/staging/rtl8723au/include/rtw_cmd.h
@@ -25,8 +25,6 @@
 #include <ieee80211.h> /*  <ieee80211/ieee80211.h> */
 
 
-#define FREE_CMDOBJ_SZ	128
-
 #define MAX_CMDSZ	1024
 #define MAX_RSPSZ	512
 #define MAX_EVTSZ	1024
@@ -34,50 +32,33 @@
 #define CMDBUFF_ALIGN_SZ 512
 
 struct cmd_obj {
+	struct work_struct work;
 	struct rtw_adapter *padapter;
 	u16	cmdcode;
-	u8	res;
-	u8	*parmbuf;
+	int	res;
 	u32	cmdsz;
+	u8	*parmbuf;
 	u8	*rsp;
 	u32	rspsz;
-	/* struct semaphore		cmd_sem; */
-	struct list_head	list;
 };
 
 struct cmd_priv {
-	struct semaphore	cmd_queue_sema;
-	/* struct semaphore	cmd_done_sema; */
-	struct semaphore	terminate_cmdthread_sema;
-	struct rtw_queue	cmd_queue;
-	u8	cmd_seq;
-	u8	*cmd_buf;	/* shall be non-paged, and 4 bytes aligned */
-	u8	*cmd_allocated_buf;
-	u8	*rsp_buf;	/* shall be non-paged, and 4 bytes aligned */
-	u8	*rsp_allocated_buf;
+	struct workqueue_struct *wq;
 	u32	cmd_issued_cnt;
 	u32	cmd_done_cnt;
 	u32	rsp_cnt;
-	u8 cmdthd_running;
 	struct rtw_adapter *padapter;
 };
 
 #define C2H_QUEUE_MAX_LEN 10
 
 struct	evt_priv {
-	struct work_struct c2h_wk;
-	bool c2h_wk_alive;
-	struct rtw_cbuf *c2h_queue;
-
-	atomic_t event_seq;
-	u8	*evt_buf;	/* shall be non-paged, and 4 bytes aligned */
-	u8	*evt_allocated_buf;
-	u32	evt_done_cnt;
+	struct workqueue_struct *wq;
+	struct work_struct irq_wk;
 };
 
 #define init_h2fwcmd_w_parm_no_rsp(pcmd, pparm, code) \
 do {\
-	INIT_LIST_HEAD(&pcmd->list);\
 	pcmd->cmdcode = code;\
 	pcmd->parmbuf = (u8 *)(pparm);\
 	pcmd->cmdsz = sizeof (*pparm);\
@@ -92,23 +73,35 @@ struct c2h_evt_hdr {
 	u8 payload[0];
 };
 
+/*
+ * Do not reorder - this allows for struct evt_work to be passed on to
+ * rtw_c2h_wk_cmd23a() as a 'struct c2h_evt_hdr *' without making an
+ * additional copy.
+ */
+struct evt_work {
+	union {
+		struct c2h_evt_hdr c2h_evt;
+		u8 buf[16];
+	} u;
+	struct work_struct work;
+	struct rtw_adapter *adapter;
+};
+
 #define c2h_evt_exist(c2h_evt) ((c2h_evt)->id || (c2h_evt)->plen)
 
-u32 rtw_enqueue_cmd23a(struct cmd_priv *pcmdpriv, struct cmd_obj *obj);
+void rtw_evt_work(struct work_struct *work);
+
+int rtw_enqueue_cmd23a(struct cmd_priv *pcmdpriv, struct cmd_obj *obj);
 void rtw_free_cmd_obj23a(struct cmd_obj *pcmd);
 
 int rtw_cmd_thread23a(void *context);
 
 int rtw_init_cmd_priv23a(struct cmd_priv *pcmdpriv);
-void rtw_free_cmd_priv23a (struct cmd_priv *pcmdpriv);
 
 u32 rtw_init_evt_priv23a (struct evt_priv *pevtpriv);
 void rtw_free_evt_priv23a (struct evt_priv *pevtpriv);
 void rtw_cmd_clr_isr23a(struct cmd_priv *pcmdpriv);
 void rtw_evt_notify_isr(struct evt_priv *pevtpriv);
-#ifdef CONFIG_8723AU_P2P
-u8 p2p_protocol_wk_cmd23a(struct rtw_adapter*padapter, int intCmdType );
-#endif /* CONFIG_8723AU_P2P */
 
 enum rtw_drvextra_cmd_id
 {
@@ -177,8 +170,7 @@ struct disconnect_parm {
 };
 
 struct	setopmode_parm {
-	u8	mode;
-	u8	rsvd[3];
+	enum nl80211_iftype mode;
 };
 
 /*
@@ -227,7 +219,7 @@ when 802.1x ==> keyid > 2 ==> unicast key
 
 */
 struct setkey_parm {
-	u8	algorithm;	/*  encryption algorithm, could be none, wep40, TKIP, CCMP, wep104 */
+	u32	algorithm;	/*  encryption algorithm, could be none, wep40, TKIP, CCMP, wep104 */
 	u8	keyid;
 	u8	grpkey;		/*  1: this is the grpkey for 802.1x. 0: this is the unicast key for 802.1x */
 	u8	set_tx;		/*  1: main tx key for wep. 0: other key. */
@@ -245,8 +237,8 @@ when shared key ==> algorithm/keyid
 */
 struct set_stakey_parm {
 	u8	addr[ETH_ALEN];
-	u8	algorithm;
 	u8	id;/*  currently for erasing cam entry if algorithm == _NO_PRIVACY_ */
+	u32	algorithm;
 	u8	key[16];
 };
 
@@ -686,52 +678,51 @@ Result:
 #define H2C_CMD_OVERFLOW		0x06
 #define H2C_RESERVED			0x07
 
-u8 rtw_setassocsta_cmd(struct rtw_adapter  *padapter, u8 *mac_addr);
-u8 rtw_setstandby_cmd(struct rtw_adapter *padapter, uint action);
-u8 rtw_sitesurvey_cmd23a(struct rtw_adapter  *padapter, struct cfg80211_ssid *ssid, int ssid_num, struct rtw_ieee80211_channel *ch, int ch_num);
-u8 rtw_createbss_cmd23a(struct rtw_adapter  *padapter);
-u8 rtw_createbss_cmd23a_ex(struct rtw_adapter  *padapter, unsigned char *pbss, unsigned int sz);
-u8 rtw_setphy_cmd(struct rtw_adapter  *padapter, u8 modem, u8 ch);
-u8 rtw_setstakey_cmd23a(struct rtw_adapter  *padapter, u8 *psta, u8 unicast_key);
-u8 rtw_clearstakey_cmd23a(struct rtw_adapter *padapter, u8 *psta, u8 entry, u8 enqueue);
-u8 rtw_joinbss_cmd23a(struct rtw_adapter  *padapter, struct wlan_network* pnetwork);
-u8 rtw_disassoc_cmd23a(struct rtw_adapter *padapter, u32 deauth_timeout_ms, bool enqueue);
-u8 rtw_setopmode_cmd23a(struct rtw_adapter  *padapter, enum ndis_802_11_net_infra networktype);
-u8 rtw_setdatarate_cmd(struct rtw_adapter  *padapter, u8 *rateset);
-u8 rtw_setbasicrate_cmd(struct rtw_adapter  *padapter, u8 *rateset);
-u8 rtw_setbbreg_cmd(struct rtw_adapter * padapter, u8 offset, u8 val);
-u8 rtw_setrfreg_cmd(struct rtw_adapter * padapter, u8 offset, u32 val);
-u8 rtw_getbbreg_cmd(struct rtw_adapter * padapter, u8 offset, u8 * pval);
-u8 rtw_getrfreg_cmd(struct rtw_adapter * padapter, u8 offset, u8 * pval);
-u8 rtw_setrfintfs_cmd(struct rtw_adapter  *padapter, u8 mode);
-u8 rtw_setrttbl_cmd(struct rtw_adapter  *padapter, struct setratable_parm *prate_table);
-u8 rtw_getrttbl_cmd(struct rtw_adapter  *padapter, struct getratable_rsp *pval);
-
-u8 rtw_gettssi_cmd(struct rtw_adapter  *padapter, u8 offset,u8 *pval);
-u8 rtw_setfwdig_cmd(struct rtw_adapter*padapter, u8 type);
-u8 rtw_setfwra_cmd(struct rtw_adapter*padapter, u8 type);
-
-u8 rtw_addbareq_cmd23a(struct rtw_adapter*padapter, u8 tid, u8 *addr);
-
-u8 rtw_dynamic_chk_wk_cmd23a(struct rtw_adapter *adapter);
-
-u8 rtw_lps_ctrl_wk_cmd23a(struct rtw_adapter*padapter, u8 lps_ctrl_type, u8 enqueue);
-
-u8 rtw_ps_cmd23a(struct rtw_adapter*padapter);
+int rtw_setassocsta_cmd(struct rtw_adapter  *padapter, u8 *mac_addr);
+int rtw_setstandby_cmd(struct rtw_adapter *padapter, uint action);
+int rtw_sitesurvey_cmd23a(struct rtw_adapter  *padapter, struct cfg80211_ssid *ssid, int ssid_num, struct rtw_ieee80211_channel *ch, int ch_num);
+int rtw_createbss_cmd23a(struct rtw_adapter  *padapter);
+int rtw_createbss_cmd23a_ex(struct rtw_adapter  *padapter, unsigned char *pbss, unsigned int sz);
+int rtw_setphy_cmd(struct rtw_adapter  *padapter, u8 modem, u8 ch);
+int rtw_setstakey_cmd23a(struct rtw_adapter  *padapter, u8 *psta, u8 unicast_key);
+int rtw_clearstakey_cmd23a(struct rtw_adapter *padapter, u8 *psta, u8 entry, u8 enqueue);
+int rtw_joinbss_cmd23a(struct rtw_adapter  *padapter, struct wlan_network* pnetwork);
+int rtw_disassoc_cmd23a(struct rtw_adapter *padapter, u32 deauth_timeout_ms, bool enqueue);
+int rtw_setopmode_cmd23a(struct rtw_adapter *padapter, enum nl80211_iftype ifmode);
+int rtw_setdatarate_cmd(struct rtw_adapter  *padapter, u8 *rateset);
+int rtw_setbasicrate_cmd(struct rtw_adapter  *padapter, u8 *rateset);
+int rtw_setbbreg_cmd(struct rtw_adapter * padapter, u8 offset, u8 val);
+int rtw_setrfreg_cmd(struct rtw_adapter * padapter, u8 offset, u32 val);
+int rtw_getbbreg_cmd(struct rtw_adapter * padapter, u8 offset, u8 * pval);
+int rtw_getrfreg_cmd(struct rtw_adapter * padapter, u8 offset, u8 * pval);
+int rtw_setrfintfs_cmd(struct rtw_adapter  *padapter, u8 mode);
+int rtw_setrttbl_cmd(struct rtw_adapter  *padapter, struct setratable_parm *prate_table);
+int rtw_getrttbl_cmd(struct rtw_adapter  *padapter, struct getratable_rsp *pval);
+
+int rtw_gettssi_cmd(struct rtw_adapter  *padapter, u8 offset,u8 *pval);
+int rtw_setfwdig_cmd(struct rtw_adapter*padapter, u8 type);
+int rtw_setfwra_cmd(struct rtw_adapter*padapter, u8 type);
+
+int rtw_addbareq_cmd23a(struct rtw_adapter*padapter, u8 tid, u8 *addr);
+
+int rtw_dynamic_chk_wk_cmd23a(struct rtw_adapter *adapter);
+
+int rtw_lps_ctrl_wk_cmd23a(struct rtw_adapter*padapter, u8 lps_ctrl_type, u8 enqueue);
+
+int rtw_ps_cmd23a(struct rtw_adapter*padapter);
 
 #ifdef CONFIG_8723AU_AP_MODE
-u8 rtw_chk_hi_queue_cmd23a(struct rtw_adapter*padapter);
+int rtw_chk_hi_queue_cmd23a(struct rtw_adapter*padapter);
 #endif
 
-u8 rtw_set_ch_cmd23a(struct rtw_adapter*padapter, u8 ch, u8 bw, u8 ch_offset, u8 enqueue);
-u8 rtw_set_chplan_cmd(struct rtw_adapter*padapter, u8 chplan, u8 enqueue);
-u8 rtw_led_blink_cmd(struct rtw_adapter*padapter, struct led_8723a *pLed);
-u8 rtw_set_csa_cmd(struct rtw_adapter*padapter, u8 new_ch_no);
-u8 rtw_tdls_cmd(struct rtw_adapter*padapter, u8 *addr, u8 option);
+int rtw_set_ch_cmd23a(struct rtw_adapter*padapter, u8 ch, u8 bw, u8 ch_offset, u8 enqueue);
+int rtw_set_chplan_cmd(struct rtw_adapter*padapter, u8 chplan, u8 enqueue);
+int rtw_led_blink_cmd(struct rtw_adapter*padapter, struct led_8723a *pLed);
+int rtw_set_csa_cmd(struct rtw_adapter*padapter, u8 new_ch_no);
 
-u8 rtw_c2h_wk_cmd23a(struct rtw_adapter *padapter, u8 *c2h_evt);
+int rtw_c2h_wk_cmd23a(struct rtw_adapter *padapter, u8 *c2h_evt);
 
-u8 rtw_drvextra_cmd_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf);
+int rtw_drvextra_cmd_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
 
 void rtw_survey_cmd_callback23a(struct rtw_adapter  *padapter, struct cmd_obj *pcmd);
 void rtw_disassoc_cmd23a_callback(struct rtw_adapter  *padapter, struct cmd_obj *pcmd);
@@ -825,11 +816,6 @@ enum rtw_h2c_cmd {
 	MAX_H2CCMD
 };
 
-#define _GetBBReg_CMD_		_Read_BBREG_CMD_
-#define _SetBBReg_CMD_		_Write_BBREG_CMD_
-#define _GetRFReg_CMD_		_Read_RFREG_CMD_
-#define _SetRFReg_CMD_		_Write_RFREG_CMD_
-
 extern struct _cmd_callback	rtw_cmd_callback[];
 
 #endif /*  _CMD_H_ */
diff --git a/drivers/staging/rtl8723au/include/rtw_efuse.h b/drivers/staging/rtl8723au/include/rtw_efuse.h
index a7755056163..07bdc346444 100644
--- a/drivers/staging/rtl8723au/include/rtw_efuse.h
+++ b/drivers/staging/rtl8723au/include/rtw_efuse.h
@@ -81,20 +81,19 @@ struct pg_pkt_struct {
 
 /*------------------------Export global variable----------------------------*/
 
-u8	efuse_GetCurrentSize23a(struct rtw_adapter *padapter, u16 *size);
 u16	efuse_GetMaxSize23a(struct rtw_adapter *padapter);
-u8	rtw_efuse_access23a(struct rtw_adapter *padapter, u8 bRead, u16 start_addr, u16 cnts, u8 *data);
-u8	rtw_efuse_map_read23a(struct rtw_adapter *padapter, u16 addr, u16 cnts, u8 *data);
+int	rtw_efuse_access23a(struct rtw_adapter *padapter, u8 bRead, u16 start_addr, u16 cnts, u8 *data);
+int	rtw_efuse_map_read23a(struct rtw_adapter *padapter, u16 addr, u16 cnts, u8 *data);
 u8	rtw_efuse_map_write(struct rtw_adapter *padapter, u16 addr, u16 cnts, u8 *data);
-u8	rtw_BT_efuse_map_read23a(struct rtw_adapter *padapter, u16 addr, u16 cnts, u8 *data);
+int	rtw_BT_efuse_map_read23a(struct rtw_adapter *padapter, u16 addr, u16 cnts, u8 *data);
 u8	rtw_BT_efuse_map_write(struct rtw_adapter *padapter, u16 addr, u16 cnts, u8 *data);
 
 u16	Efuse_GetCurrentSize23a(struct rtw_adapter *pAdapter, u8 efuseType);
 u8	Efuse_CalculateWordCnts23a(u8 word_en);
 void	ReadEFuseByte23a(struct rtw_adapter *Adapter, u16 _offset, u8 *pbuf);
 void	EFUSE_GetEfuseDefinition23a(struct rtw_adapter *pAdapter, u8 efuseType, u8 type, void *pOut);
-u8	efuse_OneByteRead23a(struct rtw_adapter *pAdapter, u16 addr, u8 *data);
-u8	efuse_OneByteWrite23a(struct rtw_adapter *pAdapter, u16 addr, u8 data);
+int	efuse_OneByteRead23a(struct rtw_adapter *pAdapter, u16 addr, u8 *data);
+int	efuse_OneByteWrite23a(struct rtw_adapter *pAdapter, u16 addr, u8 data);
 
 void	Efuse_PowerSwitch23a(struct rtw_adapter *pAdapter,u8	bWrite,u8	 PwrState);
 int	Efuse_PgPacketRead23a(struct rtw_adapter *pAdapter, u8 offset, u8 *data);
diff --git a/drivers/staging/rtl8723au/include/rtw_event.h b/drivers/staging/rtl8723au/include/rtw_event.h
index bb20640e685..807cc83bc71 100644
--- a/drivers/staging/rtl8723au/include/rtw_event.h
+++ b/drivers/staging/rtl8723au/include/rtw_event.h
@@ -16,99 +16,59 @@
 #define _RTW_EVENT_H_
 
 #include <osdep_service.h>
-
 #include <wlan_bssdef.h>
 
 /*
 Used to report a bss has been scanned
-
 */
-struct survey_event	{
+struct survey_event {
 	struct wlan_bssid_ex bss;
 };
 
 /*
 Used to report that the requested site survey has been done.
-
 bss_cnt indicates the number of bss that has been reported.
-
-
 */
 struct surveydone_event {
-	unsigned int	bss_cnt;
-
+	unsigned int bss_cnt;
 };
 
 /*
 Used to report the link result of joinning the given bss
-
-
 join_res:
 -1: authentication fail
 -2: association fail
 > 0: TID
-
 */
 struct joinbss_event {
-	struct	wlan_network	network;
+	struct wlan_network network;
 };
 
 /*
 Used to report a given STA has joinned the created BSS.
 It is used in AP/Ad-HoC(M) mode.
-
-
 */
 struct stassoc_event {
 	unsigned char macaddr[6];
 	unsigned char rsvd[2];
-	int    cam_id;
-
+	int cam_id;
 };
 
 struct stadel_event {
- unsigned char macaddr[6];
- unsigned char rsvd[2]; /* for reason */
- int mac_id;
+	unsigned char macaddr[6];
+	unsigned char rsvd[2]; /* for reason */
+	int mac_id;
 };
 
-struct addba_event
-{
+struct addba_event {
 	unsigned int tid;
 };
 
 #define GEN_EVT_CODE(event)	event ## _EVT_
 
 struct fwevent {
-	u32	parmsize;
-	void (*event_callback)(struct rtw_adapter *dev, u8 *pbuf);
-};
-
-
-#define C2HEVENT_SZ			32
-
-struct event_node{
-	unsigned char *node;
-	unsigned char evt_code;
-	unsigned short evt_sz;
-	volatile int	*caller_ff_tail;
-	int	caller_ff_sz;
+	u32 parmsize;
+	void (*event_callback)(struct rtw_adapter *dev, const u8 *pbuf);
 };
 
-struct c2hevent_queue {
-	volatile int	head;
-	volatile int	tail;
-	struct	event_node	nodes[C2HEVENT_SZ];
-	unsigned char	seq;
-};
-
-#define NETWORK_QUEUE_SZ	4
-
-struct network_queue {
-	volatile int	head;
-	volatile int	tail;
-	struct wlan_bssid_ex networks[NETWORK_QUEUE_SZ];
-};
-
-
 #endif /*  _WLANEVENT_H_ */
diff --git a/drivers/staging/rtl8723au/include/rtw_ht.h b/drivers/staging/rtl8723au/include/rtw_ht.h
index 7fe0aa46f70..86ce86b6b69 100644
--- a/drivers/staging/rtl8723au/include/rtw_ht.h
+++ b/drivers/staging/rtl8723au/include/rtw_ht.h
@@ -33,9 +33,9 @@ struct ht_priv
 	u8	sgi;/* short GI */
 
 	/* for processing Tx A-MPDU */
-	u8	agg_enable_bitmap;
+	u16	agg_enable_bitmap;
 	/* u8	ADDBA_retry_count; */
-	u8	candidate_tid_bitmap;
+	u16	candidate_tid_bitmap;
 
 	struct ieee80211_ht_cap ht_cap;
 };
diff --git a/drivers/staging/rtl8723au/include/rtw_io.h b/drivers/staging/rtl8723au/include/rtw_io.h
index 8d39d800267..cc151b08d31 100644
--- a/drivers/staging/rtl8723au/include/rtw_io.h
+++ b/drivers/staging/rtl8723au/include/rtw_io.h
@@ -97,40 +97,6 @@
 #define _INTF_ASYNC_	BIT(0)	/* support async io */
 
 struct intf_priv;
-struct intf_hdl;
-struct io_queue;
-
-struct _io_ops
-{
-		u8 (*_read8)(struct intf_hdl *pintfhdl, u32 addr);
-		u16 (*_read16)(struct intf_hdl *pintfhdl, u32 addr);
-		u32 (*_read32)(struct intf_hdl *pintfhdl, u32 addr);
-
-		int (*_write8)(struct intf_hdl *pintfhdl, u32 addr, u8 val);
-		int (*_write16)(struct intf_hdl *pintfhdl, u32 addr, u16 val);
-		int (*_write32)(struct intf_hdl *pintfhdl, u32 addr, u32 val);
-		int (*_writeN)(struct intf_hdl *pintfhdl, u32 addr, u32 length, u8 *pdata);
-
-		int (*_write8_async)(struct intf_hdl *pintfhdl, u32 addr, u8 val);
-		int (*_write16_async)(struct intf_hdl *pintfhdl, u32 addr, u16 val);
-		int (*_write32_async)(struct intf_hdl *pintfhdl, u32 addr, u32 val);
-
-		void (*_read_mem)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
-		void (*_write_mem)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
-
-		void (*_sync_irp_protocol_rw)(struct io_queue *pio_q);
-
-		u32 (*_read_interrupt)(struct intf_hdl *pintfhdl, u32 addr);
-
-		u32 (*_read_port)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, struct recv_buf *rbuf);
-		u32 (*_write_port)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, struct xmit_buf *pmem);
-
-		u32 (*_write_scsi)(struct intf_hdl *pintfhdl,u32 cnt, u8 *pmem);
-
-		void (*_read_port_cancel)(struct intf_hdl *pintfhdl);
-		void (*_write_port_cancel)(struct intf_hdl *pintfhdl);
-
-};
 
 struct io_req {
 	struct list_head	list;
@@ -145,14 +111,6 @@ struct io_req {
 	u8 *cnxt;
 };
 
-struct	intf_hdl {
-	struct rtw_adapter *padapter;
-	struct dvobj_priv *pintf_dev;/* 	pointer to &(padapter->dvobjpriv); */
-
-	struct _io_ops	io_ops;
-
-};
-
 struct reg_protocol_rd {
 
 #ifdef __LITTLE_ENDIAN
@@ -265,152 +223,15 @@ struct reg_protocol_wt {
 
 };
 
-
-
-/*
-Below is the data structure used by _io_handler
-
-*/
-
-struct io_queue {
-	spinlock_t	lock;
-	struct list_head free_ioreqs;
-	struct list_head pending;		/* The io_req list that will be served in the single protocol read/write. */
-	struct list_head processing;
-	u8	*free_ioreqs_buf; /*  4-byte aligned */
-	u8	*pallocated_free_ioreqs_buf;
-	struct	intf_hdl	intf;
-};
-
-struct io_priv{
-
-	struct rtw_adapter *padapter;
-
-	struct intf_hdl intf;
-
-};
-
-uint ioreq_flush(struct rtw_adapter *adapter, struct io_queue *ioqueue);
-void sync_ioreq_enqueue(struct io_req *preq,struct io_queue *ioqueue);
-uint sync_ioreq_flush(struct rtw_adapter *adapter, struct io_queue *ioqueue);
-
-uint free_ioreq(struct io_req *preq, struct io_queue *pio_queue);
-struct io_req *alloc_ioreq(struct io_queue *pio_q);
-
-uint register_intf_hdl(u8 *dev, struct intf_hdl *pintfhdl);
-void unregister_intf_hdl(struct intf_hdl *pintfhdl);
-
-void _rtw_attrib_read(struct rtw_adapter *adapter, u32 addr, u32 cnt, u8 *pmem);
-void _rtw_attrib_write(struct rtw_adapter *adapter, u32 addr, u32 cnt, u8 *pmem);
-
-u8 _rtw_read823a(struct rtw_adapter *adapter, u32 addr);
-u16 _rtw_read1623a(struct rtw_adapter *adapter, u32 addr);
-u32 _rtw_read3223a(struct rtw_adapter *adapter, u32 addr);
-void _rtw_read_mem23a(struct rtw_adapter *adapter, u32 addr, u32 cnt, u8 *pmem);
-void _rtw_read_port23a(struct rtw_adapter *adapter, u32 addr, u32 cnt, struct recv_buf *rbuf);
-void _rtw_read_port23a_cancel(struct rtw_adapter *adapter);
-
-int _rtw_write823a(struct rtw_adapter *adapter, u32 addr, u8 val);
-int _rtw_write1623a(struct rtw_adapter *adapter, u32 addr, u16 val);
-int _rtw_write3223a(struct rtw_adapter *adapter, u32 addr, u32 val);
-int _rtw_writeN23a(struct rtw_adapter *adapter, u32 addr, u32 length, u8 *pdata);
-
-int _rtw_write823a_async23a(struct rtw_adapter *adapter, u32 addr, u8 val);
-int _rtw_write1623a_async(struct rtw_adapter *adapter, u32 addr, u16 val);
-int _rtw_write3223a_async23a(struct rtw_adapter *adapter, u32 addr, u32 val);
-
-void _rtw_write_mem23a(struct rtw_adapter *adapter, u32 addr, u32 cnt, u8 *pmem);
-u32 _rtw_write_port23a(struct rtw_adapter *adapter, u32 addr, u32 cnt, struct xmit_buf *pmem);
-u32 _rtw_write_port23a_and_wait23a(struct rtw_adapter *adapter, u32 addr, u32 cnt, struct xmit_buf *pmem, int timeout_ms);
-void _rtw_write_port23a_cancel(struct rtw_adapter *adapter);
-
-#ifdef DBG_IO
-bool match_read_sniff_ranges(u16 addr, u16 len);
-bool match_write_sniff_ranges(u16 addr, u16 len);
-
-u8 dbg_rtw_read823a(struct rtw_adapter *adapter, u32 addr, const char *caller, const int line);
-u16 dbg_rtw_read1623a(struct rtw_adapter *adapter, u32 addr, const char *caller, const int line);
-u32 dbg_rtw_read3223a(struct rtw_adapter *adapter, u32 addr, const char *caller, const int line);
-
-int dbg_rtw_write823a(struct rtw_adapter *adapter, u32 addr, u8 val, const char *caller, const int line);
-int dbg_rtw_write1623a(struct rtw_adapter *adapter, u32 addr, u16 val, const char *caller, const int line);
-int dbg_rtw_write3223a(struct rtw_adapter *adapter, u32 addr, u32 val, const char *caller, const int line);
-int dbg_rtw_writeN23a(struct rtw_adapter *adapter, u32 addr ,u32 length , u8 *data, const char *caller, const int line);
-
-#define rtw_read8(adapter, addr) dbg_rtw_read823a((adapter), (addr), __FUNCTION__, __LINE__)
-#define rtw_read16(adapter, addr) dbg_rtw_read1623a((adapter), (addr), __FUNCTION__, __LINE__)
-#define rtw_read32(adapter, addr) dbg_rtw_read3223a((adapter), (addr), __FUNCTION__, __LINE__)
-#define rtw_read_mem(adapter, addr, cnt, mem) _rtw_read_mem23a((adapter), (addr), (cnt), (mem))
-#define rtw_read_port(adapter, addr, cnt, mem) _rtw_read_port23a((adapter), (addr), (cnt), (mem))
-#define rtw_read_port_cancel(adapter) _rtw_read_port23a_cancel((adapter))
-
-#define  rtw_write8(adapter, addr, val) dbg_rtw_write823a((adapter), (addr), (val), __FUNCTION__, __LINE__)
-#define  rtw_write16(adapter, addr, val) dbg_rtw_write1623a((adapter), (addr), (val), __FUNCTION__, __LINE__)
-#define  rtw_write32(adapter, addr, val) dbg_rtw_write3223a((adapter), (addr), (val), __FUNCTION__, __LINE__)
-#define  rtw_writeN(adapter, addr, length, data) dbg_rtw_writeN23a((adapter), (addr), (length), (data), __FUNCTION__, __LINE__)
-
-#define rtw_write8_async(adapter, addr, val) _rtw_write823a_async23a((adapter), (addr), (val))
-#define rtw_write16_async(adapter, addr, val) _rtw_write1623a_async((adapter), (addr), (val))
-#define rtw_write32_async(adapter, addr, val) _rtw_write3223a_async23a((adapter), (addr), (val))
-
-#define rtw_write_mem(adapter, addr, cnt, mem) _rtw_write_mem23a((adapter), addr, cnt, mem)
-#define rtw_write_port(adapter, addr, cnt, mem) _rtw_write_port23a(adapter, addr, cnt, mem)
-#define rtw_write_port_and_wait(adapter, addr, cnt, mem, timeout_ms) _rtw_write_port23a_and_wait23a((adapter), (addr), (cnt), (mem), (timeout_ms))
-#define rtw_write_port_cancel(adapter) _rtw_write_port23a_cancel(adapter)
-#else /* DBG_IO */
-#define rtw_read8(adapter, addr) _rtw_read823a((adapter), (addr))
-#define rtw_read16(adapter, addr) _rtw_read1623a((adapter), (addr))
-#define rtw_read32(adapter, addr) _rtw_read3223a((adapter), (addr))
-#define rtw_read_mem(adapter, addr, cnt, mem) _rtw_read_mem23a((adapter), (addr), (cnt), (mem))
-#define rtw_read_port(adapter, addr, cnt, mem) _rtw_read_port23a((adapter), (addr), (cnt), (mem))
-#define rtw_read_port_cancel(adapter) _rtw_read_port23a_cancel((adapter))
-
-#define  rtw_write8(adapter, addr, val) _rtw_write823a((adapter), (addr), (val))
-#define  rtw_write16(adapter, addr, val) _rtw_write1623a((adapter), (addr), (val))
-#define  rtw_write32(adapter, addr, val) _rtw_write3223a((adapter), (addr), (val))
-#define  rtw_writeN(adapter, addr, length, data) _rtw_writeN23a((adapter), (addr), (length), (data))
-
-#define rtw_write8_async(adapter, addr, val) _rtw_write823a_async23a((adapter), (addr), (val))
-#define rtw_write16_async(adapter, addr, val) _rtw_write1623a_async((adapter), (addr), (val))
-#define rtw_write32_async(adapter, addr, val) _rtw_write3223a_async23a((adapter), (addr), (val))
-
-#define rtw_write_mem(adapter, addr, cnt, mem) _rtw_write_mem23a((adapter), (addr), (cnt), (mem))
-#define rtw_write_port(adapter, addr, cnt, mem) _rtw_write_port23a((adapter), (addr), (cnt), (mem))
-#define rtw_write_port_and_wait(adapter, addr, cnt, mem, timeout_ms) _rtw_write_port23a_and_wait23a((adapter), (addr), (cnt), (mem), (timeout_ms))
-#define rtw_write_port_cancel(adapter) _rtw_write_port23a_cancel((adapter))
-#endif /* DBG_IO */
-
-void rtw_write_scsi(struct rtw_adapter *adapter, u32 cnt, u8 *pmem);
-
-/* ioreq */
-void ioreq_read8(struct rtw_adapter *adapter, u32 addr, u8 *pval);
-void ioreq_read16(struct rtw_adapter *adapter, u32 addr, u16 *pval);
-void ioreq_read32(struct rtw_adapter *adapter, u32 addr, u32 *pval);
-void ioreq_write8(struct rtw_adapter *adapter, u32 addr, u8 val);
-void ioreq_write16(struct rtw_adapter *adapter, u32 addr, u16 val);
-void ioreq_write32(struct rtw_adapter *adapter, u32 addr, u32 val);
-
-int rtw_init_io_priv23a(struct rtw_adapter *padapter, void (*set_intf_ops)(struct _io_ops *pops));
-
-uint alloc_io_queue(struct rtw_adapter *adapter);
-void free_io_queue(struct rtw_adapter *adapter);
-void async_bus_io(struct io_queue *pio_q);
-void bus_sync_io(struct io_queue *pio_q);
-u32 _ioreq2rwmem(struct io_queue *pio_q);
-void dev_power_down(struct rtw_adapter * Adapter, u8 bpwrup);
-
 #define PlatformEFIOWrite1Byte(_a,_b,_c)		\
-	rtw_write8(_a,_b,_c)
+	rtl8723au_write8(_a,_b,_c)
 #define PlatformEFIOWrite2Byte(_a,_b,_c)		\
-	rtw_write16(_a,_b,_c)
+	rtl8723au_write16(_a,_b,_c)
 #define PlatformEFIOWrite4Byte(_a,_b,_c)		\
-	rtw_write32(_a,_b,_c)
-
-#define PlatformEFIORead1Byte(_a,_b)		\
-		rtw_read8(_a,_b)
-#define PlatformEFIORead2Byte(_a,_b)		\
-		rtw_read16(_a,_b)
-#define PlatformEFIORead4Byte(_a,_b)		\
-		rtw_read32(_a,_b)
+	rtl8723au_write32(_a,_b,_c)
+
+#define PlatformEFIORead1Byte(_a,_b)		rtl8723au_read8(_a,_b)
+#define PlatformEFIORead2Byte(_a,_b)		rtl8723au_read16(_a,_b)
+#define PlatformEFIORead4Byte(_a,_b)		rtl8723au_read32(_a,_b)
 
 #endif	/* _RTL8711_IO_H_ */
diff --git a/drivers/staging/rtl8723au/include/rtw_ioctl.h b/drivers/staging/rtl8723au/include/rtw_ioctl.h
deleted file mode 100644
index 629eec8a702..00000000000
--- a/drivers/staging/rtl8723au/include/rtw_ioctl.h
+++ /dev/null
@@ -1,26 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
- *
- * 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.
- *
- * 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 _RTW_IOCTL_H_
-#define _RTW_IOCTL_H_
-
-#include <osdep_service.h>
-#include <drv_types.h>
-
-
-#if defined(CONFIG_WIRELESS_EXT)
-extern struct iw_handler_def  rtw_handlers_def;
-#endif
-
-#endif /*  #ifndef __INC_CEINFO_ */
diff --git a/drivers/staging/rtl8723au/include/rtw_ioctl_set.h b/drivers/staging/rtl8723au/include/rtw_ioctl_set.h
index 18ad2a87335..040543bff1b 100644
--- a/drivers/staging/rtl8723au/include/rtw_ioctl_set.h
+++ b/drivers/staging/rtl8723au/include/rtw_ioctl_set.h
@@ -17,23 +17,16 @@
 
 #include <drv_types.h>
 
-
-struct bssid_info {
-	unsigned char  BSSID[6];
-	u8  PMKID[16];
-};
-
-u8 rtw_set_802_11_authentication_mode23a(struct rtw_adapter *pdapter,
-				      enum ndis_802_11_auth_mode authmode);
-u8 rtw_set_802_11_add_wep23a(struct rtw_adapter * padapter,
-			  struct ndis_802_11_wep *wep);
-u8 rtw_set_802_11_bssid23a_list_scan(struct rtw_adapter *padapter,
-				  struct cfg80211_ssid *pssid, int ssid_max_num);
-u8 rtw_set_802_11_infrastructure_mode23a(struct rtw_adapter *padapter,
-				      enum ndis_802_11_net_infra networktype);
-u8 rtw_set_802_11_ssid23a(struct rtw_adapter * padapter, struct cfg80211_ssid * ssid);
+int rtw_set_802_11_authentication_mode23a(struct rtw_adapter *pdapter,
+					  enum ndis_802_11_auth_mode authmode);
+int rtw_set_802_11_bssid23a_list_scan(struct rtw_adapter *padapter,
+				      struct cfg80211_ssid *pssid,
+				      int ssid_max_num);
+int rtw_set_802_11_ssid23a(struct rtw_adapter * padapter,
+			   struct cfg80211_ssid * ssid);
 
 u16 rtw_get_cur_max_rate23a(struct rtw_adapter *adapter);
 s32 FillH2CCmd(struct rtw_adapter *padapter, u8 ElementID, u32 CmdLen, u8 *pCmdBuffer);
+int rtw_do_join23a(struct rtw_adapter *padapter);
 
 #endif
diff --git a/drivers/staging/rtl8723au/include/rtw_mlme.h b/drivers/staging/rtl8723au/include/rtw_mlme.h
index 31f96f39b49..4d327ba775b 100644
--- a/drivers/staging/rtl8723au/include/rtw_mlme.h
+++ b/drivers/staging/rtl8723au/include/rtw_mlme.h
@@ -83,13 +83,6 @@ enum {
 	GHZ_24,
 };
 
-enum SCAN_RESULT_TYPE {
-	SCAN_RESULT_P2P_ONLY = 0,	/*	Will return all the P2P devices. */
-	SCAN_RESULT_ALL = 1,		/*	Will return all the scanned device, include AP. */
-	SCAN_RESULT_WFD_TYPE = 2	/*	Will just return the correct WFD device. */
-					/*	If this device is Miracast sink device, it will just return all the Miracast source devices. */
-};
-
 /*
 
 there are several "locks" in mlme_priv,
@@ -104,16 +97,6 @@ To avoid possible dead lock, any thread trying to modifiying mlme_priv
 SHALL not lock up more than one locks at a time!
 */
 
-#define traffic_threshold	10
-#define	traffic_scan_period	500
-
-struct sitesurvey_ctrl {
-	u64	last_tx_pkts;
-	uint	last_rx_pkts;
-	int	traffic_busy;
-	struct timer_list	sitesurvey_ctrl_timer;
-};
-
 struct rt_link_detect {
 	u32	NumTxOkInPeriod;
 	u32	NumRxOkInPeriod;
@@ -126,209 +109,6 @@ struct rt_link_detect {
 	bool	bHigherBusyTxTraffic; /*  We may disable Tx interrupt according as Tx traffic. */
 };
 
-struct profile_info {
-	u8	ssidlen;
-	u8	ssid[IEEE80211_MAX_SSID_LEN];
-	u8	peermac[ETH_ALEN];
-};
-
-struct tx_invite_req_info {
-	u8	token;
-	u8	benable;
-	u8	go_ssid[IEEE80211_MAX_SSID_LEN];
-	u8	ssidlen;
-	u8	go_bssid[ETH_ALEN];
-	u8	peer_macaddr[ETH_ALEN];
-	u8	operating_ch;	/* This information will be set by using the p2p_set op_ch = x */
-	u8	peer_ch;	/* The listen channel for peer P2P device */
-
-};
-
-struct tx_invite_resp_info {
-	u8	token;	/*	Used to record the dialog token of p2p invitation request frame. */
-};
-
-#ifdef CONFIG_8723AU_P2P
-
-struct wifi_display_info {
-	u16	wfd_enable;		/* Enable/Disable the WFD function. */
-	u16	rtsp_ctrlport;		/* TCP port number at which the this WFD device listens for RTSP messages */
-	u16	peer_rtsp_ctrlport;	/* TCP port number at which the peer WFD device listens for RTSP messages */
-					/* This filed should be filled when receiving the gropu negotiation request */
-
-	u8	peer_session_avail;	/* WFD session is available or not for the peer wfd device. */
-					/* This variable will be set when sending the provisioning discovery request to peer WFD device. */
-					/* And this variable will be reset when it is read by using the iwpriv p2p_get wfd_sa command. */
-	u8	ip_address[4];
-	u8	peer_ip_address[4];
-	u8	wfd_pc;		/* WFD preferred connection */
-				/* 0 -> Prefer to use the P2P for WFD connection on peer side. */
-				/* 1 -> Prefer to use the TDLS for WFD connection on peer side. */
-
-	u8	wfd_device_type;/* WFD Device Type */
-				/* 0 -> WFD Source Device */
-				/* 1 -> WFD Primary Sink Device */
-	enum	SCAN_RESULT_TYPE scan_result_type;	/* Used when P2P is enable. This parameter will impact the scan result. */
-};
-#endif /* CONFIG_8723AU_P2P */
-
-struct tx_provdisc_req_info {
-	u16	wps_config_method_request;	/* Used when sending the provisioning request frame */
-	u16	peer_channel_num[2];		/* The channel number which the receiver stands. */
-	struct	cfg80211_ssid ssid;
-	u8	peerDevAddr[ETH_ALEN];	/* Peer device address */
-	u8	peerIFAddr[ETH_ALEN];		/* Peer interface address */
-	u8	benable;			/* This provision discovery request frame is trigger to send or not */
-};
-
-struct rx_provdisc_req_info {	/* When peer device issue prov_disc_req first, we should store the following informations */
-	u8	peerDevAddr[ETH_ALEN];		/*	Peer device address */
-	u8	strconfig_method_desc_of_prov_disc_req[4];	/*	description for the config method located in the provisioning discovery request frame. */
-																	/*	The UI must know this information to know which config method the remote p2p device is requiring. */
-};
-
-struct tx_nego_req_info {
-	u16	peer_channel_num[2];	/* The channel number which the receiver stands. */
-	u8	peerDevAddr[ETH_ALEN];/* Peer device address */
-	u8	benable;		/* This negoitation request frame is trigger to send or not */
-};
-
-struct group_id_info {
-	u8 go_device_addr[ETH_ALEN]; /*The GO's device address of P2P group */
-	u8 ssid[IEEE80211_MAX_SSID_LEN]; /* The SSID of this P2P group */
-};
-
-struct scan_limit_info {
-	u8	scan_op_ch_only;	/* When this flag is set, the driver should just scan the operation channel */
-	u8	operation_ch[2];	/* Store the operation channel of invitation request frame */
-};
-
-struct cfg80211_wifidirect_info {
-	struct timer_list		remain_on_ch_timer;
-	u8		restore_channel;
-	struct ieee80211_channel	remain_on_ch_channel;
-	enum nl80211_channel_type	remain_on_ch_type;
-	u64	remain_on_ch_cookie;
-	bool is_ro_ch;
-};
-
-struct wifidirect_info {
-	struct rtw_adapter	*padapter;
-	struct timer_list	find_phase_timer;
-	struct timer_list	restore_p2p_state_timer;
-
-	/*	Used to do the scanning. After confirming the peer is availalble, the driver transmits the P2P frame to peer. */
-	struct timer_list	pre_tx_scan_timer;
-	struct timer_list	reset_ch_sitesurvey;
-	struct timer_list	reset_ch_sitesurvey2;	/*	Just for resetting the scan limit function by using p2p nego */
-	struct tx_provdisc_req_info	tx_prov_disc_info;
-	struct rx_provdisc_req_info rx_prov_disc_info;
-	struct tx_invite_req_info	invitereq_info;
-	struct profile_info	profileinfo[P2P_MAX_PERSISTENT_GROUP_NUM];	/*	Store the profile information of persistent group */
-	struct tx_invite_resp_info	inviteresp_info;
-	struct tx_nego_req_info	nego_req_info;
-	struct group_id_info	groupid_info;	/*	Store the group id information when doing the group negotiation handshake. */
-	struct scan_limit_info	rx_invitereq_info;	/*	Used for get the limit scan channel from the Invitation procedure */
-	struct scan_limit_info	p2p_info;		/*	Used for get the limit scan channel from the P2P negotiation handshake */
-#ifdef CONFIG_8723AU_P2P
-	struct wifi_display_info	*wfd_info;
-#endif
-	enum P2P_ROLE	role;
-	enum P2P_STATE	pre_p2p_state;
-	enum P2P_STATE	p2p_state;
-	u8	device_addr[ETH_ALEN];	/*	The device address should be the mac address of this device. */
-	u8	interface_addr[ETH_ALEN];
-	u8	social_chan[4];
-	u8	listen_channel;
-	u8	operating_channel;
-	u8	listen_dwell;		/*	This value should be between 1 and 3 */
-	u8	support_rate[8];
-	u8	p2p_wildcard_ssid[P2P_WILDCARD_SSID_LEN];
-	u8	intent;		/*	should only include the intent value. */
-	u8	p2p_peer_interface_addr[ETH_ALEN];
-	u8	p2p_peer_device_addr[ETH_ALEN];
-	u8	peer_intent;	/*	Included the intent value and tie breaker value. */
-	u8	device_name[WPS_MAX_DEVICE_NAME_LEN];	/*	Device name for displaying on searching device screen */
-	u8	device_name_len;
-	u8	profileindex;	/*	Used to point to the index of profileinfo array */
-	u8	peer_operating_ch;
-	u8	find_phase_state_exchange_cnt;
-	u16	device_password_id_for_nego;	/*	The device password ID for group negotation */
-	u8	negotiation_dialog_token;
-	/*	SSID information for group negotitation */
-	u8 nego_ssid[IEEE80211_MAX_SSID_LEN];
-	u8 nego_ssidlen;
-	u8 p2p_group_ssid[IEEE80211_MAX_SSID_LEN];
-	u8 p2p_group_ssid_len;
-	u8	persistent_supported;	/*	Flag to know the persistent function should be supported or not. */
-					/*	In the Sigma test, the Sigma will provide this enable from the sta_set_p2p CAPI. */
-					/*	0: disable */
-					/*	1: enable */
-	u8	session_available;	/*	Flag to set the WFD session available to enable or disable "by Sigma" */
-					/*	In the Sigma test, the Sigma will disable the session available by using the sta_preset CAPI. */
-					/*	0: disable */
-					/*	1: enable */
-
-	u8	wfd_tdls_enable;	/*	Flag to enable or disable the TDLS by WFD Sigma */
-					/*	0: disable */
-					/*	1: enable */
-	u8	wfd_tdls_weaksec;	/*	Flag to enable or disable the weak security function for TDLS by WFD Sigma */
-				/*	0: disable */
-				/*	In this case, the driver can't issue the tdsl setup request frame. */
-				/*	1: enable */
-				/*	In this case, the driver can issue the tdls setup request frame */
-				/*	even the current security is weak security. */
-
-	enum	P2P_WPSINFO		ui_got_wps_info;			/*	This field will store the WPS value (PIN value or PBC) that UI had got from the user. */
-	u16	supported_wps_cm;			/*	This field describes the WPS config method which this driver supported. */
-														/*	The value should be the combination of config method defined in page104 of WPS v2.0 spec. */
-	uint	channel_list_attr_len;		/*	This field will contain the length of body of P2P Channel List attribute of group negotitation response frame. */
-	u8	channel_list_attr[100];		/*	This field will contain the body of P2P Channel List attribute of group negotitation response frame. */
-														/*	We will use the channel_cnt and channel_list fields when constructing the group negotitation confirm frame. */
-#ifdef CONFIG_8723AU_P2P
-	enum P2P_PS_MODE	p2p_ps_mode; /*  indicate p2p ps mode */
-	enum P2P_PS_STATE	p2p_ps_state; /*  indicate p2p ps state */
-	u8	noa_index; /*  Identifies and instance of Notice of Absence timing. */
-	u8	ctwindow; /*  Client traffic window. A period of time in TU after TBTT. */
-	u8	opp_ps; /*  opportunistic power save. */
-	u8	noa_num; /*  number of NoA descriptor in P2P IE. */
-	u8	noa_count[P2P_MAX_NOA_NUM]; /*  Count for owner, Type of client. */
-	u32	noa_duration[P2P_MAX_NOA_NUM]; /*  Max duration for owner, preferred or min acceptable duration for client. */
-	u32	noa_interval[P2P_MAX_NOA_NUM]; /*  Length of interval for owner, preferred or max acceptable interval of client. */
-	u32	noa_start_time[P2P_MAX_NOA_NUM]; /*  schedule expressed in terms of the lower 4 bytes of the TSF timer. */
-#endif /*  CONFIG_8723AU_P2P */
-};
-
-struct tdls_ss_record {	/* signal strength record */
-	u8	macaddr[ETH_ALEN];
-	u8	RxPWDBAll;
-	u8	is_tdls_sta;	/*  true: direct link sta, false: else */
-};
-
-struct tdls_info {
-	u8	ap_prohibited;
-	uint	setup_state;
-	u8	sta_cnt;
-	/* 1:tdls sta == (NUM_STA-1), reach max direct link no; 0: else; */
-	u8	sta_maximum;
-	struct tdls_ss_record	ss_record;
-	u8	macid_index;	/* macid entry that is ready to write */
-	/* cam entry that is trying to clear, using it in direct link teardown*/
-	u8	clear_cam;
-	u8	ch_sensing;
-	u8	cur_channel;
-	u8	candidate_ch;
-	u8	collect_pkt_num[MAX_CHANNEL_NUM];
-	spinlock_t	cmd_lock;
-	spinlock_t	hdl_lock;
-	u8	watchdog_count;
-	u8	dev_discovered;		/* WFD_TDLS: for sigma test */
-	u8	enable;
-#ifdef CONFIG_8723AU_P2P
-	struct wifi_display_info		*wfd_info;
-#endif
-};
-
 struct mlme_priv {
 	spinlock_t	lock;
 	int	fw_state;
@@ -360,7 +140,7 @@ struct mlme_priv {
 	struct timer_list set_scan_deny_timer;
 	atomic_t set_scan_deny; /* 0: allowed, 1: deny */
 
-	struct qos_priv qospriv;
+	unsigned int qos_option;
 
 	/* Number of non-HT AP/stations */
 	int num_sta_no_ht;
@@ -445,30 +225,17 @@ struct mlme_priv {
 	u32 wfd_go_probe_resp_ie_len; /* for GO */
 };
 
-#ifdef CONFIG_8723AU_AP_MODE
-
-struct hostapd_priv {
-	struct rtw_adapter *padapter;
-};
-
-int hostapd_mode_init(struct rtw_adapter *padapter);
-void hostapd_mode_unload(struct rtw_adapter *padapter);
-#endif
-
 void rtw_joinbss_event_prehandle23a(struct rtw_adapter *adapter, u8 *pbuf);
-void rtw_survey_event_cb23a(struct rtw_adapter *adapter, u8 *pbuf);
-void rtw_surveydone_event_callback23a(struct rtw_adapter *adapter, u8 *pbuf);
-void rtw23a_joinbss_event_cb(struct rtw_adapter *adapter, u8 *pbuf);
-void rtw_stassoc_event_callback23a(struct rtw_adapter *adapter, u8 *pbuf);
-void rtw_stadel_event_callback23a(struct rtw_adapter *adapter, u8 *pbuf);
-void rtw_atimdone_event_callback23a(struct rtw_adapter *adapter, u8 *pbuf);
-void rtw_cpwm_event_callback23a(struct rtw_adapter *adapter, u8 *pbuf);
-
+void rtw_survey_event_cb23a(struct rtw_adapter *adapter, const u8 *pbuf);
+void rtw_surveydone_event_callback23a(struct rtw_adapter *adapter, const u8 *pbuf);
+void rtw23a_joinbss_event_cb(struct rtw_adapter *adapter, const u8 *pbuf);
+void rtw_stassoc_event_callback23a(struct rtw_adapter *adapter, const u8 *pbuf);
+void rtw_stadel_event_callback23a(struct rtw_adapter *adapter, const u8 *pbuf);
 
 int event_thread(void *context);
 void rtw23a_join_to_handler(unsigned long);
 
-void rtw_free_network_queue23a(struct rtw_adapter *adapter, u8 isfreeall);
+void rtw_free_network_queue23a(struct rtw_adapter *adapter);
 int rtw_init_mlme_priv23a(struct rtw_adapter *adapter);
 
 void rtw_free_mlme_priv23a(struct mlme_priv *pmlmepriv);
@@ -485,7 +252,7 @@ static inline u8 *get_bssid(struct mlme_priv *pmlmepriv)
 	return pmlmepriv->cur_network.network.MacAddress;
 }
 
-static inline int check_fwstate(struct mlme_priv *pmlmepriv, int state)
+static inline bool check_fwstate(struct mlme_priv *pmlmepriv, int state)
 {
 	if (pmlmepriv->fw_state & state)
 		return true;
@@ -553,7 +320,6 @@ void rtw_free_assoc_resources23a(struct rtw_adapter *adapter,
 			      int lock_scanned_queue);
 void rtw_indicate_disconnect23a(struct rtw_adapter *adapter);
 void rtw_indicate_connect23a(struct rtw_adapter *adapter);
-void rtw_indicate_scan_done23a(struct rtw_adapter *padapter, bool aborted);
 void rtw_scan_abort23a(struct rtw_adapter *adapter);
 
 int rtw_restruct_sec_ie23a(struct rtw_adapter *adapter, u8 *in_ie, u8 *out_ie,
@@ -574,30 +340,18 @@ void rtw_clear_scan_deny(struct rtw_adapter *adapter);
 void rtw_set_scan_deny_timer_hdl(unsigned long data);
 void rtw_set_scan_deny(struct rtw_adapter *adapter, u32 ms);
 
-int _rtw_init_mlme_priv23a(struct rtw_adapter *padapter);
-
 void rtw23a_free_mlme_priv_ie_data(struct mlme_priv *pmlmepriv);
 
 void _rtw_free_mlme_priv23a(struct mlme_priv *pmlmepriv);
 
-struct wlan_network *rtw_alloc_network(struct mlme_priv *pmlmepriv);
-
-void _rtw_free_network23a(struct mlme_priv *pmlmepriv,
-		       struct wlan_network *pnetwork, u8 isfreeall);
-void _rtw_free_network23a_nolock23a(struct mlme_priv *pmlmepriv,
-			      struct wlan_network *pnetwork);
-
-struct wlan_network *_rtw_find_network23a(struct rtw_queue *scanned_queue, u8 *addr);
-
-void _rtw_free_network23a_queue23a(struct rtw_adapter *padapter, u8 isfreeall);
+struct wlan_network *rtw_alloc_network(struct mlme_priv *pmlmepriv, int gfp);
 
 int rtw_if_up23a(struct rtw_adapter *padapter);
 
 int rtw_linked_check(struct rtw_adapter *padapter);
 
-u8 *rtw_get_capability23a_from_ie(u8 *ie);
-u8 *rtw_get_timestampe_from_ie23a(u8 *ie);
-u8 *rtw_get_beacon_interval23a_from_ie(u8 *ie);
+__le16 *rtw_get_capability23a_from_ie(u8 *ie);
+__le16 *rtw_get_beacon_interval23a_from_ie(u8 *ie);
 
 
 void rtw_joinbss_reset23a(struct rtw_adapter *padapter);
@@ -613,12 +367,8 @@ int rtw_is_same_ibss23a(struct rtw_adapter *adapter,
 		     struct wlan_network *pnetwork);
 int is_same_network23a(struct wlan_bssid_ex *src, struct wlan_bssid_ex *dst);
 
-void _rtw23a_roaming(struct rtw_adapter *adapter,
-		  struct wlan_network *tgt_network);
 void rtw23a_roaming(struct rtw_adapter *adapter,
 		 struct wlan_network *tgt_network);
 void rtw_set_roaming(struct rtw_adapter *adapter, u8 to_roaming);
-u8 rtw_to_roaming(struct rtw_adapter *adapter);
-void rtw_stassoc_hw_rpt23a(struct rtw_adapter *adapter, struct sta_info *psta);
 
 #endif /* __RTL871X_MLME_H_ */
diff --git a/drivers/staging/rtl8723au/include/rtw_mlme_ext.h b/drivers/staging/rtl8723au/include/rtw_mlme_ext.h
index 0aaf0d5d8ae..badbce09559 100644
--- a/drivers/staging/rtl8723au/include/rtw_mlme_ext.h
+++ b/drivers/staging/rtl8723au/include/rtw_mlme_ext.h
@@ -89,7 +89,6 @@
 #define		_54M_RATE_	11
 
 
-extern unsigned char RTW_WPA_OUI23A[];
 extern unsigned char WMM_OUI23A[];
 extern unsigned char WPS_OUI23A[];
 extern unsigned char WFD_OUI23A[];
@@ -262,13 +261,13 @@ enum SCAN_STATE {
 
 struct mlme_handler {
 	char *str;
-	unsigned int (*func)(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+	int (*func)(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
 };
 
 struct action_handler {
 	unsigned int   num;
 	char* str;
-	unsigned int (*func)(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
+	int (*func)(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
 };
 
 struct	ss_res
@@ -358,9 +357,7 @@ struct mlme_ext_info
 	u8	turboMode_cts2self;
 	u8	turboMode_rtsen;
 	u8	SM_PS;
-	u8	agg_enable_bitmap;
 	u8	ADDBA_retry_count;
-	u8	candidate_tid_bitmap;
 	u8	dialogToken;
 	/*  Accept ADDBA Request */
 	bool bAcceptAddbaReq;
@@ -474,10 +471,6 @@ void get_rate_set23a(struct rtw_adapter *padapter, unsigned char *pbssrate,
 void UpdateBrateTbl23a(struct rtw_adapter *padapter,u8 *mBratesOS);
 void Update23aTblForSoftAP(u8 *bssrateset, u32 bssratelen);
 
-void Save_DM_Func_Flag23a(struct rtw_adapter *padapter);
-void Restore_DM_Func_Flag23a(struct rtw_adapter *padapter);
-void Switch_DM_Func23a(struct rtw_adapter *padapter, unsigned long mode, u8 enable);
-
 void Set_MSR23a(struct rtw_adapter *padapter, u8 type);
 
 u8 rtw_get_oper_ch23a(struct rtw_adapter *adapter);
@@ -490,61 +483,47 @@ void rtw_set_oper_ch23aoffset23a(struct rtw_adapter *adapter, u8 offset);
 void set_channel_bwmode23a(struct rtw_adapter *padapter, unsigned char channel,
 			unsigned char channel_offset, unsigned short bwmode);
 void SelectChannel23a(struct rtw_adapter *padapter, unsigned char channel);
-void SetBWMode23a(struct rtw_adapter *padapter, unsigned short bwmode,
-	       unsigned char channel_offset);
 
 unsigned int decide_wait_for_beacon_timeout23a(unsigned int bcn_interval);
 
-void write_cam23a(struct rtw_adapter *padapter, u8 entry, u16 ctrl,
-	       u8 *mac, u8 *key);
 void clear_cam_entry23a(struct rtw_adapter *padapter, u8 entry);
 
 void invalidate_cam_all23a(struct rtw_adapter *padapter);
-void CAM_empty_entry23a(struct rtw_adapter *Adapter, u8 ucIndex);
 
 int allocate_fw_sta_entry23a(struct rtw_adapter *padapter);
 void flush_all_cam_entry23a(struct rtw_adapter *padapter);
 
 bool IsLegal5GChannel(struct rtw_adapter *Adapter, u8 channel);
 
-void site_survey23a(struct rtw_adapter *padapter);
-u8 collect_bss_info23a(struct rtw_adapter *padapter,
-		    struct recv_frame *precv_frame,
-		    struct wlan_bssid_ex *bssid);
+int collect_bss_info23a(struct rtw_adapter *padapter,
+			struct recv_frame *precv_frame,
+			struct wlan_bssid_ex *bssid);
 void update_network23a(struct wlan_bssid_ex *dst, struct wlan_bssid_ex *src,
 		    struct rtw_adapter *padapter, bool update_ie);
 
-int get_bsstype23a(unsigned short capability);
 u8 *get_my_bssid23a(struct wlan_bssid_ex *pnetwork);
 u16 get_beacon_interval23a(struct wlan_bssid_ex *bss);
 
-int is_client_associated_to_ap23a(struct rtw_adapter *padapter);
-int is_client_associated_to_ibss23a(struct rtw_adapter *padapter);
-int is_IBSS_empty23a(struct rtw_adapter *padapter);
+bool is_client_associated_to_ap23a(struct rtw_adapter *padapter);
+bool is_client_associated_to_ibss23a(struct rtw_adapter *padapter);
+bool is_IBSS_empty23a(struct rtw_adapter *padapter);
 
 unsigned char check_assoc_AP23a(u8 *pframe, uint len);
 
-int WMM_param_handler23a(struct rtw_adapter *padapter,
-		      struct ndis_802_11_var_ies *pIE);
-#ifdef CONFIG_8723AU_P2P
-int WFD_info_handler(struct rtw_adapter *padapter,
-		     struct ndis_802_11_var_ies *pIE);
-#endif
+int WMM_param_handler23a(struct rtw_adapter *padapter, u8 *p);
 void WMMOnAssocRsp23a(struct rtw_adapter *padapter);
 
-void HT_caps_handler23a(struct rtw_adapter *padapter,
-		     struct ndis_802_11_var_ies *pIE);
-void HT_info_handler23a(struct rtw_adapter *padapter,
-		     struct ndis_802_11_var_ies *pIE);
+void HT_caps_handler23a(struct rtw_adapter *padapter, u8 *p);
+void HT_info_handler23a(struct rtw_adapter *padapter, u8 *p);
 void HTOnAssocRsp23a(struct rtw_adapter *padapter);
 
-void ERP_IE_handler23a(struct rtw_adapter *padapter,
-		    struct ndis_802_11_var_ies *pIE);
+void ERP_IE_handler23a(struct rtw_adapter *padapter, u8 *p);
 void VCS_update23a(struct rtw_adapter *padapter, struct sta_info *psta);
 
 void update_beacon23a_info(struct rtw_adapter *padapter, u8 *pframe, uint len,
 			struct sta_info *psta);
-int rtw_check_bcn_info23a(struct rtw_adapter *Adapter, u8 *pframe, u32 packet_len);
+int rtw_check_bcn_info23a(struct rtw_adapter *Adapter,
+			  struct ieee80211_mgmt *mgmt, u32 packet_len);
 void update_IOT_info23a(struct rtw_adapter *padapter);
 void update_capinfo23a(struct rtw_adapter *Adapter, u16 updateCap);
 void update_wireless_mode23a(struct rtw_adapter * padapter);
@@ -561,15 +540,15 @@ unsigned int update_MSC_rate23a(struct HT_caps_element *pHT_caps);
 void Update_RA_Entry23a(struct rtw_adapter *padapter, struct sta_info *psta);
 void set_sta_rate23a(struct rtw_adapter *padapter, struct sta_info *psta);
 
-unsigned int receive_disconnect23a(struct rtw_adapter *padapter,
-				unsigned char *MacAddr, unsigned short reason);
+int receive_disconnect23a(struct rtw_adapter *padapter,
+			  unsigned char *MacAddr, unsigned short reason);
 
 unsigned char get_highest_rate_idx23a(u32 mask);
 int support_short_GI23a(struct rtw_adapter *padapter,
 		     struct HT_caps_element *pHT_caps);
-unsigned int is_ap_in_tkip23a(struct rtw_adapter *padapter);
-unsigned int is_ap_in_wep23a(struct rtw_adapter *padapter);
-unsigned int should_forbid_n_rate23a(struct rtw_adapter *padapter);
+bool is_ap_in_tkip23a(struct rtw_adapter *padapter);
+bool is_ap_in_wep23a(struct rtw_adapter *padapter);
+bool should_forbid_n_rate23a(struct rtw_adapter *padapter);
 
 void report_join_res23a(struct rtw_adapter *padapter, int res);
 void report_survey_event23a(struct rtw_adapter *padapter,
@@ -580,8 +559,7 @@ void report_del_sta_event23a(struct rtw_adapter *padapter,
 void report_add_sta_event23a(struct rtw_adapter *padapter,
 			  unsigned char *MacAddr, int cam_idx);
 
-void beacon_timing_control23a(struct rtw_adapter *padapter);
-u8 set_tx_beacon_cmd23a(struct rtw_adapter*padapter);
+int set_tx_beacon_cmd23a(struct rtw_adapter*padapter);
 unsigned int setup_beacon_frame(struct rtw_adapter *padapter,
 				unsigned char *beacon_frame);
 void update_mgnt_tx_rate23a(struct rtw_adapter *padapter, u8 rate);
@@ -594,72 +572,20 @@ s32 dump_mgntframe23a_and_wait(struct rtw_adapter *padapter,
 s32 dump_mgntframe23a_and_wait_ack23a(struct rtw_adapter *padapter,
 				struct xmit_frame *pmgntframe);
 
-#ifdef CONFIG_8723AU_P2P
-void issue_probersp23a_p2p23a(struct rtw_adapter *padapter, unsigned char *da);
-void issue_p2p_provision_request23a(struct rtw_adapter *padapter, u8 *pssid,
-				 u8 ussidlen, u8* pdev_raddr);
-void issue_p2p_GO_request23a(struct rtw_adapter *padapter, u8* raddr);
-void issue23a_probereq_p2p(struct rtw_adapter *padapter, u8 *da);
-int issue23a_probereq_p2p_ex(struct rtw_adapter *adapter, u8 *da, int try_cnt,
-			  int wait_ms);
-void issue_p2p_invitation_response23a(struct rtw_adapter *padapter, u8* raddr,
-				   u8 dialogToken, u8 success);
-void issue_p2p_invitation_request23a(struct rtw_adapter *padapter, u8* raddr);
-#endif /* CONFIG_8723AU_P2P */
 void issue_beacon23a(struct rtw_adapter *padapter, int timeout_ms);
-void issue_probersp23a(struct rtw_adapter *padapter, unsigned char *da,
-		    u8 is_valid_p2p_probereq);
-void issue_assocreq23a(struct rtw_adapter *padapter);
-void issue_asocrsp23a(struct rtw_adapter *padapter, unsigned short status,
-		   struct sta_info *pstat, int pkt_type);
-void issue_auth23a(struct rtw_adapter *padapter, struct sta_info *psta,
-		unsigned short status);
-void issue_probereq23a(struct rtw_adapter *padapter, struct cfg80211_ssid *pssid,
-		    u8 *da);
-s32 issue_probereq23a_ex23a(struct rtw_adapter *padapter, struct cfg80211_ssid *pssid,
-		      u8 *da, int try_cnt, int wait_ms);
 int issue_nulldata23a(struct rtw_adapter *padapter, unsigned char *da,
 		   unsigned int power_mode, int try_cnt, int wait_ms);
 int issue_qos_nulldata23a(struct rtw_adapter *padapter, unsigned char *da, u16 tid,
 		       int try_cnt, int wait_ms);
 int issue_deauth23a(struct rtw_adapter *padapter, unsigned char *da,
 		 unsigned short reason);
-int issue_deauth23a_ex23a(struct rtw_adapter *padapter, u8 *da, unsigned short reason,
-		    int try_cnt, int wait_ms);
 void issue_action_spct_ch_switch23a(struct rtw_adapter *padapter, u8 *ra,
 				 u8 new_ch, u8 ch_offset);
-void issue_action_BA23a(struct rtw_adapter *padapter, unsigned char *raddr,
-		     unsigned char action, unsigned short status);
-unsigned int send_delba23a(struct rtw_adapter *padapter, u8 initiator, u8 *addr);
-unsigned int send_beacon23a(struct rtw_adapter *padapter);
-
-void start_clnt_assoc23a(struct rtw_adapter *padapter);
-void start_clnt_auth23a(struct rtw_adapter *padapter);
-void start_clnt_join23a(struct rtw_adapter *padapter);
-void start_create_ibss23a(struct rtw_adapter *padapter);
-
-unsigned int OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnAssocRsp23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnProbeReq23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnProbeRsp23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int DoReserved23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnBeacon23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnAtim23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnDisassoc23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnAuth23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnAuth23aClient23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnDeAuth23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnAction23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-
-unsigned int on_action_spct23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnAction23a_qos(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnAction23a_dls(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnAction23a_back23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int on_action_public23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnAction23a_ht(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnAction23a_wmm(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-unsigned int OnAction23a_p2p(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
-
+void issue_action_BA23a(struct rtw_adapter *padapter,
+			const unsigned char *raddr,
+			unsigned char action, unsigned short status);
+int send_delba23a(struct rtw_adapter *padapter, u8 initiator, u8 *addr);
+int send_beacon23a(struct rtw_adapter *padapter);
 
 void mlmeext_joinbss_event_callback23a(struct rtw_adapter *padapter, int join_res);
 void mlmeext_sta_del_event_callback23a(struct rtw_adapter *padapter);
@@ -668,11 +594,11 @@ void mlmeext_sta_add_event_callback23a(struct rtw_adapter *padapter, struct sta_
 void linked_status_chk23a(struct rtw_adapter *padapter);
 
 #define set_survey_timer(mlmeext, ms) \
-	/*DBG_8723A("%s set_survey_timer(%p, %d)\n", __FUNCTION__, (mlmeext), (ms));*/ \
+	/*DBG_8723A("%s set_survey_timer(%p, %d)\n", __func__, (mlmeext), (ms));*/ \
 	mod_timer(&mlmeext->survey_timer, jiffies + msecs_to_jiffies(ms));
 
 #define set_link_timer(mlmeext, ms) \
-	/*DBG_8723A("%s set_link_timer(%p, %d)\n", __FUNCTION__, (mlmeext), (ms));*/ \
+	/*DBG_8723A("%s set_link_timer(%p, %d)\n", __func__, (mlmeext), (ms));*/ \
 	mod_timer(&mlmeext->link_timer, jiffies + msecs_to_jiffies(ms));
 
 int cckrates_included23a(unsigned char *rate, int ratelen);
@@ -680,44 +606,43 @@ int cckratesonly_included23a(unsigned char *rate, int ratelen);
 
 void process_addba_req23a(struct rtw_adapter *padapter, u8 *paddba_req, u8 *addr);
 
-void update_TSF23a(struct mlme_ext_priv *pmlmeext, u8 *pframe, uint len);
 void correct_TSF23a(struct rtw_adapter *padapter, struct mlme_ext_priv *pmlmeext);
 
 struct cmd_hdl {
 	uint	parmsize;
-	u8 (*h2cfuns)(struct rtw_adapter *padapter, u8 *pbuf);
+	int (*h2cfuns)(struct rtw_adapter *padapter, const u8 *pbuf);
 };
 
 
-u8 read_macreg_hdl(struct rtw_adapter *padapter, u8 *pbuf);
-u8 write_macreg_hdl(struct rtw_adapter *padapter, u8 *pbuf);
-u8 read_bbreg_hdl(struct rtw_adapter *padapter, u8 *pbuf);
-u8 write_bbreg_hdl(struct rtw_adapter *padapter, u8 *pbuf);
-u8 read_rfreg_hdl(struct rtw_adapter *padapter, u8 *pbuf);
-u8 write_rfreg_hdl(struct rtw_adapter *padapter, u8 *pbuf);
-
-
-u8 NULL_hdl23a(struct rtw_adapter *padapter, u8 *pbuf);
-u8 join_cmd_hdl23a(struct rtw_adapter *padapter, u8 *pbuf);
-u8 disconnect_hdl23a(struct rtw_adapter *padapter, u8 *pbuf);
-u8 createbss_hdl23a(struct rtw_adapter *padapter, u8 *pbuf);
-u8 setopmode_hdl23a(struct rtw_adapter *padapter, u8 *pbuf);
-u8 sitesurvey_cmd_hdl23a(struct rtw_adapter *padapter, u8 *pbuf);
-u8 setauth_hdl23a(struct rtw_adapter *padapter, u8 *pbuf);
-u8 setkey_hdl23a(struct rtw_adapter *padapter, u8 *pbuf);
-u8 set_stakey_hdl23a(struct rtw_adapter *padapter, u8 *pbuf);
-u8 set_assocsta_hdl(struct rtw_adapter *padapter, u8 *pbuf);
-u8 del_assocsta_hdl(struct rtw_adapter *padapter, u8 *pbuf);
-u8 add_ba_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf);
-
-u8 mlme_evt_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf);
-u8 h2c_msg_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf);
-u8 tx_beacon_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf);
-u8 set_ch_hdl23a(struct rtw_adapter *padapter, u8 *pbuf);
-u8 set_chplan_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf);
-u8 led_blink_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf);
-u8 set_csa_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf);	/* Kurt: Handling DFS channel switch announcement ie. */
-u8 tdls_hdl23a(struct rtw_adapter *padapter, unsigned char *pbuf);
+int read_macreg_hdl(struct rtw_adapter *padapter, u8 *pbuf);
+int write_macreg_hdl(struct rtw_adapter *padapter, u8 *pbuf);
+int read_bbreg_hdl(struct rtw_adapter *padapter, u8 *pbuf);
+int write_bbreg_hdl(struct rtw_adapter *padapter, u8 *pbuf);
+int read_rfreg_hdl(struct rtw_adapter *padapter, u8 *pbuf);
+int write_rfreg_hdl(struct rtw_adapter *padapter, u8 *pbuf);
+
+
+int NULL_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int join_cmd_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int disconnect_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int createbss_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int setopmode_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int sitesurvey_cmd_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int setauth_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int setkey_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int set_stakey_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int set_assocsta_hdl(struct rtw_adapter *padapter, const u8 *pbuf);
+int del_assocsta_hdl(struct rtw_adapter *padapter, const u8 *pbuf);
+int add_ba_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+
+int mlme_evt_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int h2c_msg_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int tx_beacon_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int set_ch_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int set_chplan_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int led_blink_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
+int set_csa_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);	/* Kurt: Handling DFS channel switch announcement ie. */
+int tdls_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf);
 
 #define GEN_DRV_CMD_HANDLER(size, cmd)	{size, &cmd ## _hdl23a},
 #define GEN_MLME_EXT_HANDLER(size, cmd)	{size, cmd},
@@ -744,9 +669,6 @@ struct C2HEvent_Header {
 	unsigned int rsvd;
 };
 
-void rtw_dummy_event_callback23a(struct rtw_adapter *adapter , u8 *pbuf);
-void rtw23a_fwdbg_event_callback(struct rtw_adapter *adapter , u8 *pbuf);
-
 enum rtw_c2h_event {
 	GEN_EVT_CODE(_Read_MACREG) = 0, /*0*/
 	GEN_EVT_CODE(_Read_BBREG),
diff --git a/drivers/staging/rtl8723au/include/rtw_p2p.h b/drivers/staging/rtl8723au/include/rtw_p2p.h
deleted file mode 100644
index 93fdc658ff4..00000000000
--- a/drivers/staging/rtl8723au/include/rtw_p2p.h
+++ /dev/null
@@ -1,158 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
- *
- * 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.
- *
- * 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 __RTW_P2P_H_
-#define __RTW_P2P_H_
-
-#include <drv_types.h>
-
-u32 build_beacon_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf);
-u32 build_probe_resp_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf);
-u32 build_prov_disc_request_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf,
-				   u8 *pssid, u8 ussidlen, u8 *pdev_raddr);
-u32 build_assoc_resp_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf,
-			    u8 status_code);
-u32 build_deauth_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf);
-#ifdef CONFIG_8723AU_P2P
-u32 build_probe_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf);
-u32 build_probe_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf,
-			    u8 tunneled);
-u32 build_beacon_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf);
-u32 build_nego_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf);
-u32 build_nego_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf);
-u32 build_nego_confirm_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf);
-u32 build_invitation_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf);
-u32 build_invitation_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf);
-u32 build_assoc_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf);
-u32 build_assoc_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf);
-u32 build_provdisc_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf);
-u32 build_provdisc_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf);
-#endif /* CONFIG_8723AU_P2P */
-
-u32 process_probe_req_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pframe,
-			     uint len);
-u32 process_assoc_req_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pframe,
-			     uint len, struct sta_info *psta);
-u32 process_p2p_devdisc_req23a(struct wifidirect_info *pwdinfo, u8 *pframe,
-			    uint len);
-u32 process_p2p_devdisc_resp23a(struct wifidirect_info *pwdinfo, u8 *pframe,
-			     uint len);
-u8 process_p2p_provdisc_req23a(struct wifidirect_info *pwdinfo, u8 *pframe,
-			    uint len);
-u8 process_p2p_provdisc_resp23a(struct wifidirect_info *pwdinfo, u8 *pframe);
-u8 process_p2p_group_negotation_req23a(struct wifidirect_info *pwdinfo,
-				    u8 *pframe, uint len);
-u8 process_p2p_group_negotation_resp23a(struct wifidirect_info *pwdinfo,
-				     u8 *pframe, uint len);
-u8 process_p2p_group_negotation_confirm23a(struct wifidirect_info *pwdinfo,
-					u8 *pframe, uint len);
-u8 process_p2p_presence_req23a(struct wifidirect_info *pwdinfo,
-			    u8 *pframe, uint len);
-
-void p2p_protocol_wk_hdl23a(struct rtw_adapter *padapter, int cmdtype);
-
-#ifdef CONFIG_8723AU_P2P
-void	process_p2p_ps_ie23a(struct rtw_adapter *padapter, u8 *IEs, u32 IELength);
-void	p2p_ps_wk_hdl23a(struct rtw_adapter *padapter, u8 p2p_ps_state);
-u8 p2p_ps_wk_cmd23a(struct rtw_adapter *padapter, u8 p2p_ps_state, u8 enqueue);
-#endif /*  CONFIG_8723AU_P2P */
-
-void rtw_init_cfg80211_wifidirect_info(struct rtw_adapter *padapter);
-int rtw_p2p_check_frames(struct rtw_adapter *padapter, const u8 *buf,
-			 u32 len, u8 tx);
-void rtw_append_wfd_ie(struct rtw_adapter *padapter, u8 *buf, u32 *len);
-
-void reset_global_wifidirect_info23a(struct rtw_adapter *padapter);
-int rtw_init_wifi_display_info(struct rtw_adapter *padapter);
-void rtw_init_wifidirect_timers23a(struct rtw_adapter *padapter);
-void rtw_init_wifidirect_addrs23a(struct rtw_adapter *padapter, u8 *dev_addr,
-			       u8 *iface_addr);
-void init_wifidirect_info23a(struct rtw_adapter *padapter, enum P2P_ROLE role);
-int rtw_p2p_enable23a(struct rtw_adapter *padapter, enum P2P_ROLE role);
-
-static inline void _rtw_p2p_set_state(struct wifidirect_info *wdinfo,
-				      enum P2P_STATE state)
-{
-	if (wdinfo->p2p_state != state) {
-		/* wdinfo->pre_p2p_state = wdinfo->p2p_state; */
-		wdinfo->p2p_state = state;
-	}
-}
-
-static inline void _rtw_p2p_set_pre_state(struct wifidirect_info *wdinfo,
-					  enum P2P_STATE state)
-{
-	if (wdinfo->pre_p2p_state != state)
-		wdinfo->pre_p2p_state = state;
-}
-
-static inline void _rtw_p2p_set_role(struct wifidirect_info *wdinfo,
-				     enum P2P_ROLE role)
-{
-	if (wdinfo->role != role)
-		wdinfo->role = role;
-}
-
-static inline int _rtw_p2p_state(struct wifidirect_info *wdinfo)
-{
-	return wdinfo->p2p_state;
-}
-
-static inline int _rtw_p2p_pre_state(struct wifidirect_info *wdinfo)
-{
-	return wdinfo->pre_p2p_state;
-}
-
-static inline int _rtw_p2p_role(struct wifidirect_info *wdinfo)
-{
-	return wdinfo->role;
-}
-
-static inline bool _rtw_p2p_chk_state(struct wifidirect_info *wdinfo,
-				      enum P2P_STATE state)
-{
-	return wdinfo->p2p_state == state;
-}
-
-static inline bool _rtw_p2p_chk_role(struct wifidirect_info *wdinfo,
-				     enum P2P_ROLE role)
-{
-	return wdinfo->role == role;
-}
-
-#define rtw_p2p_set_state(wdinfo, state) _rtw_p2p_set_state(wdinfo, state)
-#define rtw_p2p_set_pre_state(wdinfo, state)			\
-	_rtw_p2p_set_pre_state(wdinfo, state)
-#define rtw_p2p_set_role(wdinfo, role) _rtw_p2p_set_role(wdinfo, role)
-
-#define rtw_p2p_state(wdinfo) _rtw_p2p_state(wdinfo)
-#define rtw_p2p_pre_state(wdinfo) _rtw_p2p_pre_state(wdinfo)
-#define rtw_p2p_role(wdinfo) _rtw_p2p_role(wdinfo)
-#define rtw_p2p_chk_state(wdinfo, state) _rtw_p2p_chk_state(wdinfo, state)
-#define rtw_p2p_chk_role(wdinfo, role) _rtw_p2p_chk_role(wdinfo, role)
-
-#define rtw_p2p_findphase_ex_set(wdinfo, value) \
-	((wdinfo)->find_phase_state_exchange_cnt = (value))
-
-/* is this find phase exchange for social channel scan? */
-#define rtw_p2p_findphase_ex_is_social(wdinfo)			\
-	((wdinfo)->find_phase_state_exchange_cnt >=		\
-	 P2P_FINDPHASE_EX_SOCIAL_FIRST)
-
-/* should we need find phase exchange anymore? */
-#define rtw_p2p_findphase_ex_is_needed(wdinfo) \
-	((wdinfo)->find_phase_state_exchange_cnt < P2P_FINDPHASE_EX_MAX && \
-	(wdinfo)->find_phase_state_exchange_cnt != P2P_FINDPHASE_EX_NONE)
-
-#endif
diff --git a/drivers/staging/rtl8723au/include/rtw_pwrctrl.h b/drivers/staging/rtl8723au/include/rtw_pwrctrl.h
index e0da87d4d3d..a458af97814 100644
--- a/drivers/staging/rtl8723au/include/rtw_pwrctrl.h
+++ b/drivers/staging/rtl8723au/include/rtw_pwrctrl.h
@@ -165,7 +165,6 @@ struct pwrctrl_priv {
 	struct work_struct cpwm_event;
 	u8	bpower_saving;
 
-	u8	b_hw_radio_off;
 	u8	reg_rfoff;
 	u8	reg_pdnmode; /* powerdown mode */
 	u32	rfoff_reason;
@@ -192,7 +191,6 @@ struct pwrctrl_priv {
 	s32		pnp_current_pwr_state;
 	u8		pnp_bstop_trx;
 
-	u8		bInternalAutoSuspend;
 	u8		bInSuspend;
 #ifdef	CONFIG_8723AU_BT_COEXIST
 	u8		bAutoResume;
@@ -250,8 +248,6 @@ s32 LPS_RF_ON_check23a(struct rtw_adapter *padapter, u32 delay_ms);
 void LPS_Enter23a(struct rtw_adapter *padapter);
 void LPS_Leave23a(struct rtw_adapter *padapter);
 
-u8 rtw_interface_ps_func23a(struct rtw_adapter *padapter,
-			 enum hal_intf_ps_func efunc_id, u8 *val);
 void rtw_set_ips_deny23a(struct rtw_adapter *padapter, u32 ms);
 int _rtw_pwr_wakeup23a(struct rtw_adapter *padapter, u32 ips_deffer_ms,
 		    const char *caller);
diff --git a/drivers/staging/rtl8723au/include/rtw_qos.h b/drivers/staging/rtl8723au/include/rtw_qos.h
deleted file mode 100644
index 68fc5ba1844..00000000000
--- a/drivers/staging/rtl8723au/include/rtw_qos.h
+++ /dev/null
@@ -1,26 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
- *
- * 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.
- *
- * 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 _RTW_QOS_H_
-#define _RTW_QOS_H_
-
-#include <osdep_service.h>
-
-struct	qos_priv	{
-	/* bit mask option: u-apsd, s-apsd, ts, block ack... */
-	unsigned int	  qos_option;
-};
-
-#endif	/* _RTL871X_QOS_H_ */
diff --git a/drivers/staging/rtl8723au/include/rtw_recv.h b/drivers/staging/rtl8723au/include/rtw_recv.h
index d1866a6e831..f846bb5e7ab 100644
--- a/drivers/staging/rtl8723au/include/rtw_recv.h
+++ b/drivers/staging/rtl8723au/include/rtw_recv.h
@@ -99,7 +99,6 @@ struct rx_pkt_attrib	{
 	u8	drvinfo_sz;
 	u8	shift_sz;
 	u8	hdrlen; /* the WLAN Header Len */
-	u8	to_fr_ds;
 	u8	amsdu;
 	u8	qos;
 	u8	priority;
@@ -112,7 +111,7 @@ struct rx_pkt_attrib	{
 	u8	privacy; /* in frame_ctrl field */
 	u8	bdecrypted;
 	/* when 0 indicate no encrypt. when non-zero, indicate the algorith */
-	u8	encrypt;
+	u32	encrypt;
 	u8	iv_len;
 	u8	icv_len;
 	u8	crc_err;
@@ -152,12 +151,12 @@ struct rx_pkt_attrib	{
 #define RXDESC_OFFSET RXDESC_SIZE
 
 struct recv_stat {
-	unsigned int rxdw0;
-	unsigned int rxdw1;
-	unsigned int rxdw2;
-	unsigned int rxdw3;
-	unsigned int rxdw4;
-	unsigned int rxdw5;
+	__le32 rxdw0;
+	__le32 rxdw1;
+	__le32 rxdw2;
+	__le32 rxdw3;
+	__le32 rxdw4;
+	__le32 rxdw5;
 };
 
 /* accesser of recv_priv: rtw_recv_entry23a(dispatch / passive level);	\
@@ -172,9 +171,7 @@ struct recv_priv {
 	struct rtw_queue	recv_pending_queue;
 	struct rtw_queue	uc_swdec_pending_queue;
 
-	void *pallocated_frame_buf;
-
-	uint free_recvframe_cnt;
+	int free_recvframe_cnt;
 
 	struct rtw_adapter	*adapter;
 
@@ -190,8 +187,6 @@ struct recv_priv {
 	uint  rx_middlepacket_crcerr;
 
 	/* u8 *pallocated_urb_buf; */
-	struct semaphore allrxreturnevt;
-	uint	ff_hwaddr;
 	u8	rx_pending_cnt;
 
 	struct urb *int_in_urb;
@@ -203,8 +198,6 @@ struct recv_priv {
 	struct sk_buff_head free_recv_skb_queue;
 	struct sk_buff_head rx_skb_queue;
 	u8 *precv_buf;
-	struct rtw_queue	free_recv_buf_queue;
-	u32	free_recv_buf_queue_cnt;
 
 	/* For display the phy informatiom */
 	u8 is_signal_dbg;	/*  for debug */
@@ -283,11 +276,10 @@ struct recv_frame {
 
 /* get a free recv_frame from pfree_recv_queue */
 struct recv_frame *rtw_alloc_recvframe23a(struct rtw_queue *pfree_recv_queue);
-int rtw_free_recvframe23a(struct recv_frame *precvframe, struct rtw_queue *pfree_recv_queue);
+int rtw_free_recvframe23a(struct recv_frame *precvframe);
 
 int rtw_enqueue_recvframe23a(struct recv_frame *precvframe, struct rtw_queue *queue);
 
-void rtw_free_recvframe23a_queue(struct rtw_queue *pframequeue, struct rtw_queue *pfree_recv_queue);
 u32 rtw_free_uc_swdec_pending_queue23a(struct rtw_adapter *adapter);
 
 int rtw_enqueue_recvbuf23a_to_head(struct recv_buf *precvbuf, struct rtw_queue *queue);
diff --git a/drivers/staging/rtl8723au/include/rtw_security.h b/drivers/staging/rtl8723au/include/rtw_security.h
index 75bbb934a53..8b8433365e3 100644
--- a/drivers/staging/rtl8723au/include/rtw_security.h
+++ b/drivers/staging/rtl8723au/include/rtw_security.h
@@ -17,20 +17,12 @@
 
 #include <osdep_service.h>
 #include <drv_types.h>
+#include <net/lib80211.h>
 
 
-#define _NO_PRIVACY_		0x0
-#define _WEP40_			0x1
-#define _TKIP_			0x2
-#define _TKIP_WTMIC_		0x3
-#define _AES_			0x4
-#define _WEP104_		0x5
-#define _WEP_WPA_MIXED_	0x07  /*  WEP + WPA */
-#define _SMS4_			0x06
+#define is_wep_enc(alg) (alg == WLAN_CIPHER_SUITE_WEP40 || \
+			 alg == WLAN_CIPHER_SUITE_WEP104)
 
-#define is_wep_enc(alg) (((alg) == _WEP40_) || ((alg) == _WEP104_))
-
-#define _WPA_IE_ID_	0xdd
 #define _WPA2_IE_ID_	0x30
 
 #define SHA256_MAC_LEN 32
@@ -93,6 +85,10 @@ union Keytype {
 	u32    lkey[4];
 };
 
+struct rtw_wep_key {
+	u8 key[WLAN_KEY_LEN_WEP104 + 1]; /* 14 */
+	u16 keylen;
+};
 
 struct rt_pmkid_list {
 	u8	bUsed;
@@ -113,8 +109,7 @@ struct security_priv {
 	u32	  dot11PrivacyKeyIndex;	/*  this is only valid for legendary
 					 * wep, 0~3 for key id. (tx key index)
 					 */
-	union Keytype dot11DefKey[4];	/*  this is only valid for def. key */
-	u32	dot11DefKeylen[4];
+	struct rtw_wep_key wep_key[NUM_WEP_KEYS];
 
 	u32 dot118021XGrpPrivacy;	/* specify the privacy algthm.
 					 * used for Grp key
@@ -140,15 +135,13 @@ struct security_priv {
 
 	u8 wps_ie[MAX_WPS_IE_LEN];/* added in assoc req */
 	int wps_ie_len;
-	u8	binstallGrpkey;
-	u8	busetkipkey;
-	u8	bcheck_grpkey;
-	u8	bgrpkey_handshake;
-	s32	hw_decrypted;
+	unsigned int binstallGrpkey:1;
+	unsigned int busetkipkey:1;
+	unsigned int bcheck_grpkey:1;
+	unsigned int hw_decrypted:1;
 	u32 ndisauthtype;	/*  enum ndis_802_11_auth_mode */
 	u32 ndisencryptstatus;	/*  NDIS_802_11_ENCRYPTION_STATUS */
 	struct wlan_bssid_ex sec_bss;  /* for joinbss (h2c buffer) usage */
-	struct ndis_802_11_wep ndiswep;
 	u8 assoc_info[600];
 	u8 szofcapability[256]; /* for wpa2 usage */
 	u8 oidassociation[512]; /* for wpa/wpa2 usage */
@@ -179,13 +172,13 @@ do {\
 	case dot11AuthAlgrthm_Open:\
 	case dot11AuthAlgrthm_Shared:\
 	case dot11AuthAlgrthm_Auto:\
-		encry_algo = (u8)psecuritypriv->dot11PrivacyAlgrthm;\
+		encry_algo = psecuritypriv->dot11PrivacyAlgrthm;\
 		break;\
 	case dot11AuthAlgrthm_8021X:\
 		if (bmcst)\
-			encry_algo = (u8)psecuritypriv->dot118021XGrpPrivacy;\
+			encry_algo = psecuritypriv->dot118021XGrpPrivacy;\
 		else\
-			encry_algo = (u8)psta->dot118021XPrivacy;\
+			encry_algo = psta->dot118021XPrivacy;\
 		break;\
 	}	\
 } while (0)
@@ -316,22 +309,6 @@ static const unsigned long K[64] = {
 	0x90befffaUL, 0xa4506cebUL, 0xbef9a3f7UL, 0xc67178f2UL
 };
 
-/* Various logical functions */
-#define RORc(x, y) \
-(((((unsigned long)(x) & 0xFFFFFFFFUL) >> (unsigned long) ((y) & 31)) | \
-((unsigned long)(x) << (unsigned long) (32 - ((y) & 31)))) & 0xFFFFFFFFUL)
-#define Ch(x, y, z)     (z ^ (x & (y ^ z)))
-#define Maj(x, y, z)    (((x | y) & z) | (x & y))
-#define S(x, n)         RORc((x), (n))
-#define R(x, n)         (((x)&0xFFFFFFFFUL)>>(n))
-#define Sigma0(x)       (S(x, 2) ^ S(x, 13) ^ S(x, 22))
-#define Sigma1(x)       (S(x, 6) ^ S(x, 11) ^ S(x, 25))
-#define Gamma0(x)       (S(x, 7) ^ S(x, 18) ^ R(x, 3))
-#define Gamma1(x)       (S(x, 17) ^ S(x, 19) ^ R(x, 10))
-#ifndef MIN
-#define MIN(x, y) (((x) < (y)) ? (x) : (y))
-#endif
-
 void rtw_secmicsetkey23a(struct mic_data *pmicdata, u8 *key);
 void rtw_secmicappend23abyte23a(struct mic_data *pmicdata, u8 b);
 void rtw_secmicappend23a(struct mic_data *pmicdata, u8 *src, u32 nbBytes);
@@ -340,15 +317,15 @@ void rtw_secgetmic23a(struct mic_data *pmicdata, u8 *dst);
 void rtw_seccalctkipmic23a(u8 *key, u8 *header, u8 *data, u32 data_len,
 			u8 *Miccode, u8 priorityi);
 
-u32 rtw_aes_encrypt23a(struct rtw_adapter *padapter,
+int rtw_aes_encrypt23a(struct rtw_adapter *padapter,
 		    struct xmit_frame *pxmitframe);
-u32 rtw_tkip_encrypt23a(struct rtw_adapter *padapter,
+int rtw_tkip_encrypt23a(struct rtw_adapter *padapter,
 		     struct xmit_frame *pxmitframe);
 void rtw_wep_encrypt23a(struct rtw_adapter *padapter,
 		     struct xmit_frame *pxmitframe);
-u32 rtw_aes_decrypt23a(struct rtw_adapter *padapter,
+int rtw_aes_decrypt23a(struct rtw_adapter *padapter,
 		    struct recv_frame *precvframe);
-u32 rtw_tkip_decrypt23a(struct rtw_adapter *padapter,
+int rtw_tkip_decrypt23a(struct rtw_adapter *padapter,
 		     struct recv_frame *precvframe);
 void rtw_wep_decrypt23a(struct rtw_adapter *padapter, struct recv_frame *precvframe);
 
diff --git a/drivers/staging/rtl8723au/include/rtw_sreset.h b/drivers/staging/rtl8723au/include/rtw_sreset.h
index 4c523722dd1..77cc7941b54 100644
--- a/drivers/staging/rtl8723au/include/rtw_sreset.h
+++ b/drivers/staging/rtl8723au/include/rtw_sreset.h
@@ -36,21 +36,21 @@ struct sreset_priv {
 
 #include <rtl8723a_hal.h>
 
-#define	WIFI_STATUS_SUCCESS		0
-#define	USB_VEN_REQ_CMD_FAIL	BIT0
-#define	USB_READ_PORT_FAIL		BIT1
-#define	USB_WRITE_PORT_FAIL		BIT2
-#define	WIFI_MAC_TXDMA_ERROR	BIT3
-#define   WIFI_TX_HANG				BIT4
-#define	WIFI_RX_HANG				BIT5
-#define		WIFI_IF_NOT_EXIST			BIT6
-
-void sreset_init_value23a(struct rtw_adapter *padapter);
-void sreset_reset_value23a(struct rtw_adapter *padapter);
-u8 sreset_get_wifi_status23a(struct rtw_adapter *padapter);
+#define	WIFI_STATUS_SUCCESS	0
+#define	USB_VEN_REQ_CMD_FAIL	BIT(0)
+#define	USB_READ_PORT_FAIL	BIT(1)
+#define	USB_WRITE_PORT_FAIL	BIT(2)
+#define	WIFI_MAC_TXDMA_ERROR	BIT(3)
+#define	WIFI_TX_HANG		BIT(4)
+#define	WIFI_RX_HANG		BIT(5)
+#define	WIFI_IF_NOT_EXIST	BIT(6)
+
+void rtw_sreset_init(struct rtw_adapter *padapter);
+void rtw_sreset_reset_value(struct rtw_adapter *padapter);
+u8 rtw_sreset_get_wifi_status(struct rtw_adapter *padapter);
+bool rtw_sreset_inprogress(struct rtw_adapter *padapter);
 void sreset_set_wifi_error_status23a(struct rtw_adapter *padapter, u32 status);
 void sreset_set_trigger_point(struct rtw_adapter *padapter, s32 tgp);
-bool sreset_inprogress(struct rtw_adapter *padapter);
-void sreset_reset(struct rtw_adapter *padapter);
+void rtw_sreset_reset(struct rtw_adapter *active_adapter);
 
 #endif
diff --git a/drivers/staging/rtl8723au/include/rtw_xmit.h b/drivers/staging/rtl8723au/include/rtw_xmit.h
index 65a33a07c8e..32a84417032 100644
--- a/drivers/staging/rtl8723au/include/rtw_xmit.h
+++ b/drivers/staging/rtl8723au/include/rtw_xmit.h
@@ -92,14 +92,14 @@ do {									\
 
 struct tx_desc {
 	/* DWORD 0 */
-	unsigned int txdw0;
-	unsigned int txdw1;
-	unsigned int txdw2;
-	unsigned int txdw3;
-	unsigned int txdw4;
-	unsigned int txdw5;
-	unsigned int txdw6;
-	unsigned int txdw7;
+	__le32 txdw0;
+	__le32 txdw1;
+	__le32 txdw2;
+	__le32 txdw3;
+	__le32 txdw4;
+	__le32 txdw5;
+	__le32 txdw6;
+	__le32 txdw7;
 };
 
 union txdesc {
@@ -114,8 +114,7 @@ struct	hw_xmit	{
 
 /* reduce size */
 struct pkt_attrib {
-	u8	type;
-	u8	subtype;
+	u16	type;
 	u8	bswenc;
 	u8	dhcp_pkt;
 	u16	ether_type;
@@ -124,8 +123,8 @@ struct pkt_attrib {
 	u16	hdrlen;		/* the WLAN Header Len */
 	u32	pktlen;		/* the original 802.3 pkt raw_data len */
 	u32	last_txcmdsz;
+	u32	encrypt;	/* when 0 indicate no encrypt. */
 	u8	nr_frags;
-	u8	encrypt;	/* when 0 indicate no encrypt. */
 	u8	iv_len;
 	u8	icv_len;
 	u8	iv[18];
@@ -286,14 +285,10 @@ struct	xmit_priv {
 	struct rtw_queue	vo_pending;
 	struct rtw_queue	bm_pending;
 
-	u8 *pallocated_frame_buf;
-	u8 *pxmit_frame_buf;
-	uint free_xmitframe_cnt;
+	int free_xmitframe_cnt;
 	struct rtw_queue	free_xmit_queue;
 
-	u8 *xframe_ext_alloc_addr;
-	u8 *xframe_ext;
-	uint free_xframe_ext_cnt;
+	int free_xframe_ext_cnt;
 	struct rtw_queue free_xframe_ext_queue;
 
 	uint	frag_len;
@@ -354,10 +349,7 @@ s32 rtw_free_xmitbuf23a(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf);
 void rtw_count_tx_stats23a(struct rtw_adapter *padapter,
 			struct xmit_frame *pxmitframe, int sz);
 void rtw_update_protection23a(struct rtw_adapter *padapter, u8 *ie, uint ie_len);
-s32 rtw_make_wlanhdr23a(struct rtw_adapter *padapter, u8 *hdr,
-		     struct pkt_attrib *pattrib);
 s32 rtw_put_snap23a(u8 *data, u16 h_proto);
-struct xmit_frame *rtw_alloc_xmitframe23a(struct xmit_priv *pxmitpriv);
 struct xmit_frame *rtw_alloc_xmitframe23a_ext(struct xmit_priv *pxmitpriv);
 struct xmit_frame *rtw_alloc_xmitframe23a_once(struct xmit_priv *pxmitpriv);
 s32 rtw_free_xmitframe23a(struct xmit_priv *pxmitpriv,
@@ -382,7 +374,7 @@ s32 rtw_txframes_pending23a(struct rtw_adapter *padapter);
 s32 rtw_txframes_sta_ac_pending23a(struct rtw_adapter *padapter,
 				struct pkt_attrib *pattrib);
 void rtw_init_hwxmits23a(struct hw_xmit *phwxmit, int entry);
-s32 _rtw_init_xmit_priv23a(struct xmit_priv *pxmitpriv,
+int _rtw_init_xmit_priv23a(struct xmit_priv *pxmitpriv,
 			struct rtw_adapter *padapter);
 void _rtw_free_xmit_priv23a(struct xmit_priv *pxmitpriv);
 void rtw_alloc_hwxmits23a(struct rtw_adapter *padapter);
diff --git a/drivers/staging/rtl8723au/include/sta_info.h b/drivers/staging/rtl8723au/include/sta_info.h
index ffbc9e3f215..d1939a67390 100644
--- a/drivers/staging/rtl8723au/include/sta_info.h
+++ b/drivers/staging/rtl8723au/include/sta_info.h
@@ -97,7 +97,7 @@ struct sta_info {
 	u8	hwaddr[ETH_ALEN];
 
 	uint	ieee8021x_blocked;	/* 0: allowed, 1:blocked */
-	uint	dot118021XPrivacy; /* aes, tkip... */
+	u32	dot118021XPrivacy; /* aes, tkip... */
 	union Keytype	dot11tkiptxmickey;
 	union Keytype	dot11tkiprxmickey;
 	union Keytype	dot118021x_UncstKey;
@@ -211,7 +211,6 @@ struct sta_info {
 	/*  */
 	/*  Driver Write */
 	u8		bValid;				/*  record the sta status link or not? */
-	u8		IOTPeer;			/*  Enum value.	HT_IOT_PEER_E */
 	u8		rssi_level;			/* for Refresh RA mask */
 	/*  ODM Write */
 	/* 1 PHY_STATUS_INFO */
@@ -310,16 +309,10 @@ struct sta_info {
 
 #define STA_PKTS_FMT "(m:%llu, c:%llu, d:%llu)"
 
-struct	sta_priv {
-	u8 *pallocated_stainfo_buf;
-	u8 *pstainfo_buf;
-	struct rtw_queue	free_sta_queue;
-
+struct sta_priv {
 	spinlock_t sta_hash_lock;
-	struct list_head   sta_hash[NUM_STA];
+	struct list_head sta_hash[NUM_STA];
 	int asoc_sta_count;
-	struct rtw_queue sleep_q;
-	struct rtw_queue wakeup_q;
 
 	struct rtw_adapter *padapter;
 	struct list_head asoc_list;
@@ -349,7 +342,7 @@ struct	sta_priv {
 	struct wlan_acl_pool acl_list;
 };
 
-static inline u32 wifi_mac_hash(u8 *mac)
+static inline u32 wifi_mac_hash(const u8 *mac)
 {
 	u32 x;
 
@@ -366,20 +359,15 @@ static inline u32 wifi_mac_hash(u8 *mac)
 	return x;
 }
 
-u32	_rtw_init_sta_priv23a(struct sta_priv *pstapriv);
-u32	_rtw_free_sta_priv23a(struct sta_priv *pstapriv);
-
-#define stainfo_offset_valid(offset) (offset < NUM_STA && offset >= 0)
-int rtw_stainfo_offset23a(struct sta_priv *stapriv, struct sta_info *sta);
-struct sta_info *rtw_get_stainfo23a_by_offset23a(struct sta_priv *stapriv,
-					   int offset);
+int _rtw_init_sta_priv23a(struct sta_priv *pstapriv);
+int _rtw_free_sta_priv23a(struct sta_priv *pstapriv);
 
-struct sta_info *rtw_alloc_stainfo23a(struct sta_priv *pstapriv, u8 *hwaddr);
-u32 rtw_free_stainfo23a(struct rtw_adapter *padapter, struct sta_info *psta);
+struct sta_info *rtw_alloc_stainfo23a(struct sta_priv *pstapriv, u8 *hwaddr, gfp_t gfp);
+int rtw_free_stainfo23a(struct rtw_adapter *padapter, struct sta_info *psta);
 void rtw_free_all_stainfo23a(struct rtw_adapter *padapter);
-struct sta_info *rtw_get_stainfo23a(struct sta_priv *pstapriv, u8 *hwaddr);
-u32 rtw_init_bcmc_stainfo23a(struct rtw_adapter *padapter);
+struct sta_info *rtw_get_stainfo23a(struct sta_priv *pstapriv, const u8 *hwaddr);
+int rtw_init_bcmc_stainfo23a(struct rtw_adapter *padapter);
 struct sta_info *rtw_get_bcmc_stainfo23a(struct rtw_adapter *padapter);
-u8 rtw_access_ctrl23a(struct rtw_adapter *padapter, u8 *mac_addr);
+bool rtw_access_ctrl23a(struct rtw_adapter *padapter, u8 *mac_addr);
 
 #endif /* _STA_INFO_H_ */
diff --git a/drivers/staging/rtl8723au/include/usb_hal.h b/drivers/staging/rtl8723au/include/usb_hal.h
deleted file mode 100644
index 4edec3b539b..00000000000
--- a/drivers/staging/rtl8723au/include/usb_hal.h
+++ /dev/null
@@ -1,20 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
- *
- * 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.
- *
- * 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 __USB_HAL_H__
-#define __USB_HAL_H__
-
-int rtl8723au_set_hal_ops(struct rtw_adapter *padapter);
-
-#endif /* __USB_HAL_H__ */
diff --git a/drivers/staging/rtl8723au/include/usb_ops.h b/drivers/staging/rtl8723au/include/usb_ops.h
index 55d1380f903..ade8bc71572 100644
--- a/drivers/staging/rtl8723au/include/usb_ops.h
+++ b/drivers/staging/rtl8723au/include/usb_ops.h
@@ -34,19 +34,7 @@ enum {
 #define MAX_VENDOR_REQ_CMD_SIZE	254		/* 8188cu SIE Support */
 #define MAX_USB_IO_CTL_SIZE	(MAX_VENDOR_REQ_CMD_SIZE +ALIGNMENT_UNIT)
 
-#define rtw_usb_control_msg(dev, pipe, request, requesttype, value,	\
-			    index, data, size, timeout_ms)		\
-	usb_control_msg((dev), (pipe), (request), (requesttype),	\
-			(value), (index), (data), (size), (timeout_ms))
-#define rtw_usb_bulk_msg(usb_dev, pipe, data, len, actual_length, timeout_ms) \
-	usb_bulk_msg((usb_dev), (pipe), (data), (len), (actual_length),	\
-		     (timeout_ms))
-
 void rtl8723au_set_hw_type(struct rtw_adapter *padapter);
-#define hal_set_hw_type rtl8723au_set_hw_type
-
-void rtl8723au_set_intf_ops(struct _io_ops *pops);
-#define usb_set_intf_ops rtl8723au_set_intf_ops
 
 void rtl8723au_recv_tasklet(void *priv);
 
@@ -75,23 +63,6 @@ static inline void rtw_reset_continual_urb_error(struct dvobj_priv *dvobj)
 	atomic_set(&dvobj->continual_urb_error, 0);
 }
 
-#define USB_HIGH_SPEED_BULK_SIZE	512
-#define USB_FULL_SPEED_BULK_SIZE	64
-
-static inline u8 rtw_usb_bulk_size_boundary(struct rtw_adapter *padapter,
-					    int buf_len)
-{
-	u8 rst = true;
-	struct dvobj_priv *pdvobjpriv = adapter_to_dvobj(padapter);
-
-	if (pdvobjpriv->ishighspeed)
-		rst = (0 == (buf_len) % USB_HIGH_SPEED_BULK_SIZE) ?
-		      true : false;
-	else
-		rst = (0 == (buf_len) % USB_FULL_SPEED_BULK_SIZE) ?
-		      true : false;
-	return rst;
-}
-
+void rtl8723au_chip_configure(struct rtw_adapter *padapter);
 
 #endif /* __USB_OPS_H_ */
diff --git a/drivers/staging/rtl8723au/include/usb_ops_linux.h b/drivers/staging/rtl8723au/include/usb_ops_linux.h
index 8f5c59eace5..e540a4bad08 100644
--- a/drivers/staging/rtl8723au/include/usb_ops_linux.h
+++ b/drivers/staging/rtl8723au/include/usb_ops_linux.h
@@ -17,30 +17,25 @@
 
 #define VENDOR_CMD_MAX_DATA_LEN	254
 
-#define RTW_USB_CONTROL_MSG_TIMEOUT_TEST	10/* ms */
 #define RTW_USB_CONTROL_MSG_TIMEOUT	500/* ms */
 
 #define MAX_USBCTRL_VENDORREQ_TIMES	10
 
-#define RTW_USB_BULKOUT_TIMEOUT	5000/* ms */
-
-#define _usbctrl_vendorreq_async_callback(urb, regs)		\
-	_usbctrl_vendorreq_async_callback(urb)
-#define usb_write_mem23a_complete(purb, regs)	usb_write_mem23a_complete(purb)
-#define usb_write_port23a_complete(purb, regs)	usb_write_port23a_complete(purb)
-#define usb_read_port_complete(purb, regs)	usb_read_port_complete(purb)
-#define usb_read_interrupt_complete(purb, regs)			\
-	usb_read_interrupt_complete(purb)
-
-unsigned int ffaddr2pipehdl23a(struct dvobj_priv *pdvobj, u32 addr);
-
-void usb_read_mem23a(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *rmem);
-void usb_write_mem23a(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *wmem);
-
-void usb_read_port_cancel23a(struct intf_hdl *pintfhdl);
-
-u32 usb_write_port23a(struct intf_hdl *pintfhdl, u32 addr, u32 cnt,
-		   struct xmit_buf *wmem);
-void usb_write_port23a_cancel(struct intf_hdl *pintfhdl);
+int rtl8723au_read_port(struct rtw_adapter *adapter, u32 addr, u32 cnt,
+			struct recv_buf *precvbuf);
+void rtl8723au_read_port_cancel(struct rtw_adapter *padapter);
+int rtl8723au_write_port(struct rtw_adapter *padapter, u32 addr, u32 cnt,
+			 struct xmit_buf *pxmitbuf);
+void rtl8723au_write_port_cancel(struct rtw_adapter *padapter);
+int rtl8723au_read_interrupt(struct rtw_adapter *adapter, u32 addr);
+
+u8 rtl8723au_read8(struct rtw_adapter *padapter, u32 addr);
+u16 rtl8723au_read16(struct rtw_adapter *padapter, u32 addr);
+u32 rtl8723au_read32(struct rtw_adapter *padapter, u32 addr);
+int rtl8723au_write8(struct rtw_adapter *padapter, u32 addr, u8 val);
+int rtl8723au_write16(struct rtw_adapter *padapter, u32 addr, u16 val);
+int rtl8723au_write32(struct rtw_adapter *padapter, u32 addr, u32 val);
+int rtl8723au_writeN(struct rtw_adapter *padapter,
+		     u32 addr, u32 length, u8 *pdata);
 
 #endif
diff --git a/drivers/staging/rtl8723au/include/usb_osintf.h b/drivers/staging/rtl8723au/include/usb_osintf.h
deleted file mode 100644
index 46087662834..00000000000
--- a/drivers/staging/rtl8723au/include/usb_osintf.h
+++ /dev/null
@@ -1,24 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
- *
- * 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.
- *
- * 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 __USB_OSINTF_H
-#define __USB_OSINTF_H
-
-#include <osdep_service.h>
-#include <drv_types.h>
-#include <usb_vendor_req.h>
-
-#define USBD_HALTED(_status) ((u32)(_status) >> 30 == 3)
-
-#endif
diff --git a/drivers/staging/rtl8723au/include/usb_vendor_req.h b/drivers/staging/rtl8723au/include/usb_vendor_req.h
deleted file mode 100644
index eb4508ef191..00000000000
--- a/drivers/staging/rtl8723au/include/usb_vendor_req.h
+++ /dev/null
@@ -1,31 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
- *
- * 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.
- *
- * 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 _USB_VENDOR_REQUEST_H_
-#define _USB_VENDOR_REQUEST_H_
-
-/* 4	Set/Get Register related wIndex/Data */
-#define	RT_USB_RESET_MASK_OFF		0
-#define	RT_USB_RESET_MASK_ON		1
-#define	RT_USB_SLEEP_MASK_OFF		0
-#define	RT_USB_SLEEP_MASK_ON		1
-#define	RT_USB_LDO_ON			1
-#define	RT_USB_LDO_OFF			0
-
-/* 4	Set/Get SYSCLK related	wValue or Data */
-#define	RT_USB_SYSCLK_32KHZ		0
-#define	RT_USB_SYSCLK_40MHZ		1
-#define	RT_USB_SYSCLK_60MHZ		2
-
-#endif
diff --git a/drivers/staging/rtl8723au/include/wifi.h b/drivers/staging/rtl8723au/include/wifi.h
index b5034c6ef1d..cccea6a50b3 100644
--- a/drivers/staging/rtl8723au/include/wifi.h
+++ b/drivers/staging/rtl8723au/include/wifi.h
@@ -23,189 +23,6 @@
  */
 #define	WiFiNavUpperUs		30000	/*  30 ms */
 
-enum WIFI_FRAME_TYPE {
-	WIFI_MGT_TYPE  =	(0),
-	WIFI_CTRL_TYPE =	(BIT(2)),
-	WIFI_DATA_TYPE =	(BIT(3)),
-	WIFI_QOS_DATA_TYPE	= (BIT(7)|BIT(3)),	/*  QoS Data */
-};
-
-enum WIFI_FRAME_SUBTYPE {
-	/*  below is for mgt frame */
-	WIFI_ASSOCREQ = (0 | WIFI_MGT_TYPE),
-	WIFI_ASSOCRSP = (BIT(4) | WIFI_MGT_TYPE),
-	WIFI_REASSOCREQ = (BIT(5) | WIFI_MGT_TYPE),
-	WIFI_REASSOCRSP = (BIT(5) | BIT(4) | WIFI_MGT_TYPE),
-	WIFI_PROBEREQ = (BIT(6) | WIFI_MGT_TYPE),
-	WIFI_PROBERSP = (BIT(6) | BIT(4) | WIFI_MGT_TYPE),
-	WIFI_BEACON = (BIT(7) | WIFI_MGT_TYPE),
-	WIFI_ATIM = (BIT(7) | BIT(4) | WIFI_MGT_TYPE),
-	WIFI_DISASSOC = (BIT(7) | BIT(5) | WIFI_MGT_TYPE),
-	WIFI_AUTH = (BIT(7) | BIT(5) | BIT(4) | WIFI_MGT_TYPE),
-	WIFI_DEAUTH = (BIT(7) | BIT(6) | WIFI_MGT_TYPE),
-	WIFI_ACTION = (BIT(7) | BIT(6) | BIT(4) | WIFI_MGT_TYPE),
-
-	/*  below is for control frame */
-	WIFI_PSPOLL = (BIT(7) | BIT(5) | WIFI_CTRL_TYPE),
-	WIFI_RTS = (BIT(7) | BIT(5) | BIT(4) | WIFI_CTRL_TYPE),
-	WIFI_CTS = (BIT(7) | BIT(6) | WIFI_CTRL_TYPE),
-	WIFI_ACK = (BIT(7) | BIT(6) | BIT(4) | WIFI_CTRL_TYPE),
-	WIFI_CFEND = (BIT(7) | BIT(6) | BIT(5) | WIFI_CTRL_TYPE),
-	WIFI_CFEND_CFACK = (BIT(7) | BIT(6) | BIT(5) | BIT(4) | WIFI_CTRL_TYPE),
-
-	/*  below is for data frame */
-	WIFI_DATA = (0 | WIFI_DATA_TYPE),
-	WIFI_DATA_CFACK = (BIT(4) | WIFI_DATA_TYPE),
-	WIFI_DATA_CFPOLL = (BIT(5) | WIFI_DATA_TYPE),
-	WIFI_DATA_CFACKPOLL = (BIT(5) | BIT(4) | WIFI_DATA_TYPE),
-	WIFI_DATA_NULL = (BIT(6) | WIFI_DATA_TYPE),
-	WIFI_CF_ACK = (BIT(6) | BIT(4) | WIFI_DATA_TYPE),
-	WIFI_CF_POLL = (BIT(6) | BIT(5) | WIFI_DATA_TYPE),
-	WIFI_CF_ACKPOLL = (BIT(6) | BIT(5) | BIT(4) | WIFI_DATA_TYPE),
-	WIFI_QOS_DATA_NULL = (BIT(6) | WIFI_QOS_DATA_TYPE),
-};
-
-
-enum WIFI_REG_DOMAIN {
-	DOMAIN_FCC		= 1,
-	DOMAIN_IC		= 2,
-	DOMAIN_ETSI		= 3,
-	DOMAIN_SPAIN		= 4,
-	DOMAIN_FRANCE		= 5,
-	DOMAIN_MKK		= 6,
-	DOMAIN_ISRAEL		= 7,
-	DOMAIN_MKK1		= 8,
-	DOMAIN_MKK2		= 9,
-	DOMAIN_MKK3		= 10,
-	DOMAIN_MAX
-};
-
-
-#define SetToDs(pbuf)	\
-	(*(unsigned short *)(pbuf) |= cpu_to_le16(IEEE80211_FCTL_TODS))
-
-#define SetFrDs(pbuf)	\
-	(*(unsigned short *)(pbuf) |= cpu_to_le16(IEEE80211_FCTL_FROMDS))
-
-#define get_tofr_ds(pframe)	((ieee80211_has_tods(pframe) << 1) | \
-				 ieee80211_has_fromds(pframe))
-
-#define SetMFrag(pbuf)	\
-	(*(unsigned short *)(pbuf) |= cpu_to_le16(IEEE80211_FCTL_MOREFRAGS))
-
-#define ClearMFrag(pbuf)	\
-	(*(unsigned short *)(pbuf) &= (~cpu_to_le16(IEEE80211_FCTL_MOREFRAGS)))
-
-#define SetRetry(pbuf)	\
-	(*(unsigned short *)(pbuf) |= cpu_to_le16(IEEE80211_FCTL_RETRY))
-
-#define SetPwrMgt(pbuf)	\
-	(*(unsigned short *)(pbuf) |= cpu_to_le16(IEEE80211_FCTL_PM))
-
-#define SetMData(pbuf)	\
-	(*(unsigned short *)(pbuf) |= cpu_to_le16(IEEE80211_FCTL_MOREDATA))
-
-#define SetPrivacy(pbuf)	\
-	(*(unsigned short *)(pbuf) |= cpu_to_le16(IEEE80211_FCTL_PROTECTED))
-
-#define SetFrameType(pbuf, type)	\
-	do {	\
-		*(unsigned short *)(pbuf) &= __constant_cpu_to_le16(~(BIT(3) | BIT(2))); \
-		*(unsigned short *)(pbuf) |= __constant_cpu_to_le16(type); \
-	} while (0)
-
-#define SetFrameSubType(pbuf, type) \
-	do {    \
-		*(unsigned short *)(pbuf) &= cpu_to_le16(~(BIT(7) | BIT(6) | BIT(5) | BIT(4) | BIT(3) | BIT(2))); \
-		*(unsigned short *)(pbuf) |= cpu_to_le16(type); \
-	} while (0)
-
-#define GetTupleCache(pbuf)	(cpu_to_le16(*(unsigned short *)((unsigned long)(pbuf) + 22)))
-
-#define SetFragNum(pbuf, num) \
-	do {    \
-		*(unsigned short *)((unsigned long)(pbuf) + 22) = \
-			((*(unsigned short *)((unsigned long)(pbuf) + 22)) & le16_to_cpu(~(0x000f))) | \
-			cpu_to_le16(0x0f & (num));     \
-	} while (0)
-
-#define SetSeqNum(pbuf, num) \
-	do {    \
-		*(unsigned short *)((unsigned long)(pbuf) + 22) = \
-			((*(unsigned short *)((unsigned long)(pbuf) + 22)) & le16_to_cpu((unsigned short)0x000f)) | \
-			le16_to_cpu((unsigned short)(0xfff0 & (num << 4))); \
-	} while (0)
-
-#define SetDuration(pbuf, dur) \
-	(*(unsigned short *)((unsigned long)(pbuf) + 2) =		\
-	 cpu_to_le16(0xffff & (dur)))
-
-#define SetPriority(pbuf, tid)	\
-	(*(unsigned short *)(pbuf) |= cpu_to_le16(tid & 0xf))
-
-#define SetEOSP(pbuf, eosp)	\
-	(*(unsigned short *)(pbuf) |= cpu_to_le16((eosp & 1) << 4))
-
-#define SetAckpolicy(pbuf, ack)	\
-	(*(unsigned short *)(pbuf) |= cpu_to_le16((ack & 3) << 5))
-
-#define SetAMsdu(pbuf, amsdu)	\
-	(*(unsigned short *)(pbuf) |= cpu_to_le16((amsdu & 1) << 7))
-
-#define GetAid(pbuf)							\
-	(cpu_to_le16(*(unsigned short *)((unsigned long)(pbuf) + 2)) &	\
-	 0x3fff)
-
-#define GetTid(pbuf)							\
-	(cpu_to_le16(*(unsigned short *)((unsigned long)(pbuf) +	\
-	 (((ieee80211_has_tods(pbuf)<<1) |				\
-	 ieee80211_has_fromds(pbuf)) == 3 ? 30 : 24))) & 0x000f)
-
-static inline unsigned char *get_hdr_bssid(unsigned char *pframe)
-{
-	unsigned char	*sa;
-	unsigned int	to_fr_ds;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) pframe;
-
-	to_fr_ds = (ieee80211_has_tods(hdr->frame_control) << 1) |
-		    ieee80211_has_fromds(hdr->frame_control);
-
-	switch (to_fr_ds) {
-	case 0x00:	/*  ToDs=0, FromDs=0 */
-		sa = hdr->addr3;
-		break;
-	case 0x01:	/*  ToDs=0, FromDs=1 */
-		sa = hdr->addr2;
-		break;
-	case 0x02:	/*  ToDs=1, FromDs=0 */
-		sa = hdr->addr1;
-		break;
-	case 0x03:	/*  ToDs=1, FromDs=1 */
-		sa = hdr->addr1;
-		break;
-	default:
-		sa = NULL; /*  */
-		break;
-	}
-	return sa;
-}
-
-/*-----------------------------------------------------------------------------
-			Below is for the security related definition
-------------------------------------------------------------------------------*/
-#define _RESERVED_FRAME_TYPE_		0
-#define _SKB_FRAME_TYPE_		2
-#define _PRE_ALLOCMEM_			1
-#define _PRE_ALLOCHDR_			3
-#define _PRE_ALLOCLLCHDR_		4
-#define _PRE_ALLOCICVHDR_		5
-#define _PRE_ALLOCMICHDR_		6
-
-#define _SIFSTIME_					\
-	((priv->pmib->dot11BssType.net_work_type & WIRELESS_11A) ? 16 : 10)
-#define _ACKCTSLNG_			14	/* 14 bytes long, including crclng */
-#define _CRCLNG_			4
-
 #define _ASOCREQ_IE_OFFSET_		4	/*  excluding wlan_hdr */
 #define	_ASOCRSP_IE_OFFSET_		6
 #define _REASOCREQ_IE_OFFSET_		10
@@ -219,39 +36,8 @@ static inline unsigned char *get_hdr_bssid(unsigned char *pframe)
 
 #define _FIXED_IE_LENGTH_		_BEACON_IE_OFFSET_
 
-#define _SSID_IE_			0
-#define _SUPPORTEDRATES_IE_		1
-#define _DSSET_IE_			3
-#define _TIM_IE_			5
-#define _IBSS_PARA_IE_			6
-#define _COUNTRY_IE_			7
-#define _CHLGETXT_IE_			16
-#define _SUPPORTED_CH_IE_		36
-#define _CH_SWTICH_ANNOUNCE_	37	/* Secondary Channel Offset */
-#define _RSN_IE_2_			48
-#define _SSN_IE_1_			221
-#define _ERPINFO_IE_			42
-#define _EXT_SUPPORTEDRATES_IE_		50
-
-#define _HT_CAPABILITY_IE_		45
-#define _FTIE_				55
-#define _TIMEOUT_ITVL_IE_		56
-#define _SRC_IE_			59
-#define _HT_EXTRA_INFO_IE_		61
-#define _HT_ADD_INFO_IE_		61 /* _HT_EXTRA_INFO_IE_ */
 
-
-#define	EID_BSSCoexistence		72 /*  20/40 BSS Coexistence */
 #define	EID_BSSIntolerantChlReport	73
-#define _RIC_Descriptor_IE_		75
-
-#define _LINK_ID_IE_		101
-#define _CH_SWITCH_TIMING_	104
-#define _PTI_BUFFER_STATUS_	106
-#define _EXT_CAP_IE_		127
-#define _VENDOR_SPECIFIC_IE_	221
-
-#define	_RESERVED47_		47
 
 /* ---------------------------------------------------------------------------
 					Below is the fixed elements...
@@ -266,41 +52,6 @@ static inline unsigned char *get_hdr_bssid(unsigned char *pframe)
 #define _STATUS_CODE_		2
 #define _TIMESTAMP_		8
 
-#define AUTH_ODD_TO		0
-#define AUTH_EVEN_TO		1
-
-#define WLAN_ETHCONV_ENCAP	1
-#define WLAN_ETHCONV_RFC1042	2
-#define WLAN_ETHCONV_8021h	3
-
-#define cap_ESS		BIT(0)
-#define cap_IBSS	BIT(1)
-#define cap_CFPollable	BIT(2)
-#define cap_CFRequest	BIT(3)
-#define cap_Privacy	BIT(4)
-#define cap_ShortPremble BIT(5)
-#define cap_PBCC	BIT(6)
-#define cap_ChAgility	BIT(7)
-#define cap_SpecMgmt	BIT(8)
-#define cap_QoS		BIT(9)
-#define cap_ShortSlot	BIT(10)
-
-/*-----------------------------------------------------------------------------
-				Below is the definition for 802.11i / 802.1x
-------------------------------------------------------------------------------*/
-#define _IEEE8021X_MGT_			1	/*  WPA */
-#define _IEEE8021X_PSK_			2	/*  WPA with pre-shared key */
-
-/*
-#define _NO_PRIVACY_			0
-#define _WEP_40_PRIVACY_		1
-#define _TKIP_PRIVACY_			2
-#define _WRAP_PRIVACY_			3
-#define _CCMP_PRIVACY_			4
-#define _WEP_104_PRIVACY_		5
-#define _WEP_WPA_MIXED_PRIVACY_ 6	WEP + WPA
-*/
-
 /*-----------------------------------------------------------------------------
 				Below is the definition for WMM
 ------------------------------------------------------------------------------*/
@@ -312,13 +63,6 @@ static inline unsigned char *get_hdr_bssid(unsigned char *pframe)
 				Below is the definition for 802.11n
 ------------------------------------------------------------------------------*/
 
-#define SetOrderBit(pbuf)						\
-	(*(unsigned short *)(pbuf) |= cpu_to_le16(_ORDER_))
-
-#define GetOrderBit(pbuf)		\
-	(((*(unsigned short *)(pbuf)) & le16_to_cpu(_ORDER_)) != 0)
-
-
 /* struct rtw_ieee80211_ht_cap - HT additional information
  *
  * This structure refers to "HT information element" as
@@ -372,38 +116,7 @@ struct ADDBA_request {
 }  __packed;
 
 
-#define OP_MODE_PURE                    0
-#define OP_MODE_MAY_BE_LEGACY_STAS      1
-#define OP_MODE_20MHZ_HT_STA_ASSOCED    2
-#define OP_MODE_MIXED                   3
-
-#define HT_INFO_HT_PARAM_SECONDARY_CHNL_OFF_MASK	((u8) BIT(0) | BIT(1))
-#define HT_INFO_HT_PARAM_SECONDARY_CHNL_ABOVE		((u8) BIT(0))
-#define HT_INFO_HT_PARAM_SECONDARY_CHNL_BELOW		((u8) BIT(0) | BIT(1))
-#define HT_INFO_HT_PARAM_REC_TRANS_CHNL_WIDTH		((u8) BIT(2))
-#define HT_INFO_HT_PARAM_RIFS_MODE			((u8) BIT(3))
-#define HT_INFO_HT_PARAM_CTRL_ACCESS_ONLY		((u8) BIT(4))
-#define HT_INFO_HT_PARAM_SRV_INTERVAL_GRANULARITY	((u8) BIT(5))
-
-#define HT_INFO_OPERATION_MODE_OP_MODE_MASK	\
-		((u16) (0x0001 | 0x0002))
-#define HT_INFO_OPERATION_MODE_OP_MODE_OFFSET		0
-#define HT_INFO_OPERATION_MODE_NON_GF_DEVS_PRESENT	((u8) BIT(2))
-#define HT_INFO_OPERATION_MODE_TRANSMIT_BURST_LIMIT	((u8) BIT(3))
-#define HT_INFO_OPERATION_MODE_NON_HT_STA_PRESENT	((u8) BIT(4))
-
-#define HT_INFO_STBC_PARAM_DUAL_BEACON		((u16) BIT(6))
-#define HT_INFO_STBC_PARAM_DUAL_STBC_PROTECT	((u16) BIT(7))
-#define HT_INFO_STBC_PARAM_SECONDARY_BCN	((u16) BIT(8))
-#define HT_INFO_STBC_PARAM_LSIG_TXOP_PROTECT_ALLOWED	((u16) BIT(9))
-#define HT_INFO_STBC_PARAM_PCO_ACTIVE		((u16) BIT(10))
-#define HT_INFO_STBC_PARAM_PCO_PHASE		((u16) BIT(11))
-
-
-
 /*	===============WPS Section=============== */
-/*	For WPSv1.0 */
-#define WPSOUI					0x0050f204
 /*	WPS attribute ID */
 #define WPS_ATTR_VER1				0x104A
 #define WPS_ATTR_SIMPLE_CONF_STATE		0x1044
@@ -425,182 +138,6 @@ struct ADDBA_request {
 #define WPS_ATTR_VENDOR_EXT			0x1049
 #define WPS_ATTR_SELECTED_REGISTRAR		0x1041
 
-/*	Value of WPS attribute "WPS_ATTR_DEVICE_NAME */
-#define WPS_MAX_DEVICE_NAME_LEN			32
-
-/*	Value of WPS Request Type Attribute */
-#define WPS_REQ_TYPE_ENROLLEE_INFO_ONLY		0x00
-#define WPS_REQ_TYPE_ENROLLEE_OPEN_8021X	0x01
-#define WPS_REQ_TYPE_REGISTRAR			0x02
-#define WPS_REQ_TYPE_WLAN_MANAGER_REGISTRAR	0x03
-
-/*	Value of WPS Response Type Attribute */
-#define WPS_RESPONSE_TYPE_INFO_ONLY		0x00
-#define WPS_RESPONSE_TYPE_8021X			0x01
-#define WPS_RESPONSE_TYPE_REGISTRAR		0x02
-#define WPS_RESPONSE_TYPE_AP			0x03
-
-/*	Value of WPS WiFi Simple Configuration State Attribute */
-#define WPS_WSC_STATE_NOT_CONFIG		0x01
-#define WPS_WSC_STATE_CONFIG			0x02
-
-/*	Value of WPS Version Attribute */
-#define WPS_VERSION_1				0x10
-
-/*	Value of WPS Configuration Method Attribute */
-#define WPS_CONFIG_METHOD_FLASH			0x0001
-#define WPS_CONFIG_METHOD_ETHERNET		0x0002
-#define WPS_CONFIG_METHOD_LABEL			0x0004
-#define WPS_CONFIG_METHOD_DISPLAY		0x0008
-#define WPS_CONFIG_METHOD_E_NFC			0x0010
-#define WPS_CONFIG_METHOD_I_NFC			0x0020
-#define WPS_CONFIG_METHOD_NFC			0x0040
-#define WPS_CONFIG_METHOD_PBC			0x0080
-#define WPS_CONFIG_METHOD_KEYPAD		0x0100
-#define WPS_CONFIG_METHOD_VPBC			0x0280
-#define WPS_CONFIG_METHOD_PPBC			0x0480
-#define WPS_CONFIG_METHOD_VDISPLAY		0x2008
-#define WPS_CONFIG_METHOD_PDISPLAY		0x4008
-
-/*	Value of Category ID of WPS Primary Device Type Attribute */
-#define WPS_PDT_CID_DISPLAYS			0x0007
-#define WPS_PDT_CID_MULIT_MEDIA			0x0008
-#define WPS_PDT_CID_RTK_WIDI			WPS_PDT_CID_MULIT_MEDIA
-
-/*	Value of Sub Category ID of WPS Primary Device Type Attribute */
-#define WPS_PDT_SCID_MEDIA_SERVER		0x0005
-#define WPS_PDT_SCID_RTK_DMP			WPS_PDT_SCID_MEDIA_SERVER
-
-/*	Value of Device Password ID */
-#define WPS_DPID_PIN				0x0000
-#define WPS_DPID_USER_SPEC			0x0001
-#define WPS_DPID_MACHINE_SPEC			0x0002
-#define WPS_DPID_REKEY				0x0003
-#define WPS_DPID_PBC				0x0004
-#define WPS_DPID_REGISTRAR_SPEC			0x0005
-
-/*	Value of WPS RF Bands Attribute */
-#define WPS_RF_BANDS_2_4_GHZ			0x01
-#define WPS_RF_BANDS_5_GHZ			0x02
-
-/*	Value of WPS Association State Attribute */
-#define WPS_ASSOC_STATE_NOT_ASSOCIATED		0x00
-#define WPS_ASSOC_STATE_CONNECTION_SUCCESS	0x01
-#define WPS_ASSOC_STATE_CONFIGURATION_FAILURE	0x02
-#define WPS_ASSOC_STATE_ASSOCIATION_FAILURE	0x03
-#define WPS_ASSOC_STATE_IP_FAILURE		0x04
-
-/*	=====================P2P Section===================== */
-/*	For P2P */
-#define	P2POUI					0x506F9A09
-
-/*	P2P Attribute ID */
-#define	P2P_ATTR_STATUS				0x00
-#define	P2P_ATTR_MINOR_REASON_CODE		0x01
-#define	P2P_ATTR_CAPABILITY			0x02
-#define	P2P_ATTR_DEVICE_ID			0x03
-#define	P2P_ATTR_GO_INTENT			0x04
-#define	P2P_ATTR_CONF_TIMEOUT			0x05
-#define	P2P_ATTR_LISTEN_CH			0x06
-#define	P2P_ATTR_GROUP_BSSID			0x07
-#define	P2P_ATTR_EX_LISTEN_TIMING		0x08
-#define	P2P_ATTR_INTENTED_IF_ADDR		0x09
-#define	P2P_ATTR_MANAGEABILITY			0x0A
-#define	P2P_ATTR_CH_LIST			0x0B
-#define	P2P_ATTR_NOA				0x0C
-#define	P2P_ATTR_DEVICE_INFO			0x0D
-#define	P2P_ATTR_GROUP_INFO			0x0E
-#define	P2P_ATTR_GROUP_ID			0x0F
-#define	P2P_ATTR_INTERFACE			0x10
-#define	P2P_ATTR_OPERATING_CH			0x11
-#define	P2P_ATTR_INVITATION_FLAGS		0x12
-
-/*	Value of Status Attribute */
-#define	P2P_STATUS_SUCCESS			0x00
-#define	P2P_STATUS_FAIL_INFO_UNAVAILABLE	0x01
-#define	P2P_STATUS_FAIL_INCOMPATIBLE_PARAM	0x02
-#define	P2P_STATUS_FAIL_LIMIT_REACHED		0x03
-#define	P2P_STATUS_FAIL_INVALID_PARAM		0x04
-#define	P2P_STATUS_FAIL_REQUEST_UNABLE		0x05
-#define	P2P_STATUS_FAIL_PREVOUS_PROTO_ERR	0x06
-#define	P2P_STATUS_FAIL_NO_COMMON_CH		0x07
-#define	P2P_STATUS_FAIL_UNKNOWN_P2PGROUP	0x08
-#define	P2P_STATUS_FAIL_BOTH_GOINTENT_15	0x09
-#define	P2P_STATUS_FAIL_INCOMPATIBLE_PROVSION	0x0A
-#define	P2P_STATUS_FAIL_USER_REJECT		0x0B
-
-/*	Value of Inviation Flags Attribute */
-#define	P2P_INVITATION_FLAGS_PERSISTENT		BIT(0)
-
-#define	DMP_P2P_DEVCAP_SUPPORT	(P2P_DEVCAP_SERVICE_DISCOVERY | \
-				 P2P_DEVCAP_CLIENT_DISCOVERABILITY | \
-				 P2P_DEVCAP_CONCURRENT_OPERATION | \
-				 P2P_DEVCAP_INVITATION_PROC)
-
-#define	DMP_P2P_GRPCAP_SUPPORT	(P2P_GRPCAP_INTRABSS)
-
-/*	Value of Device Capability Bitmap */
-#define	P2P_DEVCAP_SERVICE_DISCOVERY		BIT(0)
-#define	P2P_DEVCAP_CLIENT_DISCOVERABILITY	BIT(1)
-#define	P2P_DEVCAP_CONCURRENT_OPERATION		BIT(2)
-#define	P2P_DEVCAP_INFRA_MANAGED		BIT(3)
-#define	P2P_DEVCAP_DEVICE_LIMIT			BIT(4)
-#define	P2P_DEVCAP_INVITATION_PROC		BIT(5)
-
-/*	Value of Group Capability Bitmap */
-#define	P2P_GRPCAP_GO				BIT(0)
-#define	P2P_GRPCAP_PERSISTENT_GROUP		BIT(1)
-#define	P2P_GRPCAP_GROUP_LIMIT			BIT(2)
-#define	P2P_GRPCAP_INTRABSS			BIT(3)
-#define	P2P_GRPCAP_CROSS_CONN			BIT(4)
-#define	P2P_GRPCAP_PERSISTENT_RECONN		BIT(5)
-#define	P2P_GRPCAP_GROUP_FORMATION		BIT(6)
-
-/*	P2P Public Action Frame ( Management Frame ) */
-#define	P2P_PUB_ACTION_ACTION			0x09
-
-/*	P2P Public Action Frame Type */
-#define	P2P_GO_NEGO_REQ				0
-#define	P2P_GO_NEGO_RESP			1
-#define	P2P_GO_NEGO_CONF			2
-#define	P2P_INVIT_REQ				3
-#define	P2P_INVIT_RESP				4
-#define	P2P_DEVDISC_REQ				5
-#define	P2P_DEVDISC_RESP			6
-#define	P2P_PROVISION_DISC_REQ			7
-#define	P2P_PROVISION_DISC_RESP			8
-
-/*	P2P Action Frame Type */
-#define	P2P_NOTICE_OF_ABSENCE			0
-#define	P2P_PRESENCE_REQUEST			1
-#define	P2P_PRESENCE_RESPONSE			2
-#define	P2P_GO_DISC_REQUEST			3
-
-
-#define	P2P_MAX_PERSISTENT_GROUP_NUM		10
-
-#define	P2P_PROVISIONING_SCAN_CNT		3
-
-#define	P2P_WILDCARD_SSID_LEN			7
-
-#define	P2P_FINDPHASE_EX_NONE			0	/*  default value, used when: (1)p2p disabed or (2)p2p enabled but only do 1 scan phase */
-#define	P2P_FINDPHASE_EX_FULL			1	/*  used when p2p enabled and want to do 1 scan phase and P2P_FINDPHASE_EX_MAX-1 find phase */
-#define	P2P_FINDPHASE_EX_SOCIAL_FIRST		(P2P_FINDPHASE_EX_FULL+1)
-#define	P2P_FINDPHASE_EX_MAX					4
-#define	P2P_FINDPHASE_EX_SOCIAL_LAST		P2P_FINDPHASE_EX_MAX
-
-#define	P2P_PROVISION_TIMEOUT			5000	/*5 sec timeout for sending the provision discovery request */
-#define	P2P_CONCURRENT_PROVISION_TIMEOUT	3000	/*3 sec timeout for sending the provision discovery request under concurrent mode */
-#define	P2P_GO_NEGO_TIMEOUT			5000	/*5 sec timeout for receiving the group negotation response */
-#define	P2P_CONCURRENT_GO_NEGO_TIMEOUT		3000	/*3 sec timeout for sending the negotiation request under concurrent mode */
-#define	P2P_TX_PRESCAN_TIMEOUT			100	/*100ms */
-#define	P2P_INVITE_TIMEOUT			5000	/*5 sec timeout for sending the invitation request */
-#define	P2P_CONCURRENT_INVITE_TIMEOUT		3000	/*3 sec timeout for sending the invitation request under concurrent mode */
-#define	P2P_RESET_SCAN_CH			25000	/*25 sec t/o to reset the scan channel ( based on channel plan ) */
-#define	P2P_MAX_INTENT				15
-
-#define	P2P_MAX_NOA_NUM				2
-
 /*	WPS Configuration Method */
 #define	WPS_CM_NONE					0x0000
 #define	WPS_CM_LABEL					0x0004
@@ -615,93 +152,4 @@ struct ADDBA_request {
 #define	WPS_CM_SW_DISPLAY_PIN				0x2008
 #define	WPS_CM_LCD_DISPLAY_PIN				0x4008
 
-enum P2P_ROLE {
-	P2P_ROLE_DISABLE = 0,
-	P2P_ROLE_DEVICE = 1,
-	P2P_ROLE_CLIENT = 2,
-	P2P_ROLE_GO = 3
-};
-
-enum P2P_STATE {
-	P2P_STATE_NONE = 0,			/*P2P disable */
-	P2P_STATE_IDLE = 1,			/*P2P had enabled and do nothing */
-	P2P_STATE_LISTEN = 2,			/*In pure listen state */
-	P2P_STATE_SCAN = 3,			/*In scan phase */
-	P2P_STATE_FIND_PHASE_LISTEN = 4,	/*In the listen state of find phase */
-	P2P_STATE_FIND_PHASE_SEARCH = 5,	/*In the search state of find phase */
-	P2P_STATE_TX_PROVISION_DIS_REQ = 6,	/*In P2P provisioning discovery */
-	P2P_STATE_RX_PROVISION_DIS_RSP = 7,
-	P2P_STATE_RX_PROVISION_DIS_REQ = 8,
-	P2P_STATE_GONEGO_ING = 9,		/*Doing the group owner negoitation handshake */
-	P2P_STATE_GONEGO_OK = 10,		/*finish the group negoitation handshake with success */
-	P2P_STATE_GONEGO_FAIL = 11,		/*finish the group negoitation handshake with failure */
-	P2P_STATE_RECV_INVITE_REQ_MATCH = 12,	/*receiving the P2P Inviation request and match with the profile. */
-	P2P_STATE_PROVISIONING_ING = 13,	/*Doing the P2P WPS */
-	P2P_STATE_PROVISIONING_DONE = 14,	/*Finish the P2P WPS */
-	P2P_STATE_TX_INVITE_REQ = 15,		/*Transmit the P2P Invitation request */
-	P2P_STATE_RX_INVITE_RESP_OK = 16,	/*Receiving the P2P Invitation response */
-	P2P_STATE_RECV_INVITE_REQ_DISMATCH = 17,/*receiving the P2P Inviation request and dismatch with the profile. */
-	P2P_STATE_RECV_INVITE_REQ_GO = 18,	/*receiving the P2P Inviation request and this wifi is GO. */
-	P2P_STATE_RECV_INVITE_REQ_JOIN = 19,	/*receiving the P2P Inviation request to join an existing P2P Group. */
-	P2P_STATE_RX_INVITE_RESP_FAIL = 20,	/*receiving the P2P Inviation response with failure */
-	P2P_STATE_RX_INFOR_NOREADY = 21,	/*receiving p2p negotiation response with information is not available */
-	P2P_STATE_TX_INFOR_NOREADY = 22,	/*sending p2p negotiation response with information is not available */
-};
-
-enum P2P_WPSINFO {
-	P2P_NO_WPSINFO				= 0,
-	P2P_GOT_WPSINFO_PEER_DISPLAY_PIN	= 1,
-	P2P_GOT_WPSINFO_SELF_DISPLAY_PIN	= 2,
-	P2P_GOT_WPSINFO_PBC			= 3,
-};
-
-#define	P2P_PRIVATE_IOCTL_SET_LEN		64
-
-enum P2P_PROTO_WK_ID {
-	P2P_FIND_PHASE_WK = 0,
-	P2P_RESTORE_STATE_WK = 1,
-	P2P_PRE_TX_PROVDISC_PROCESS_WK = 2,
-	P2P_PRE_TX_NEGOREQ_PROCESS_WK = 3,
-	P2P_PRE_TX_INVITEREQ_PROCESS_WK = 4,
-	P2P_AP_P2P_CH_SWITCH_PROCESS_WK = 5,
-	P2P_RO_CH_WK = 6,
-};
-
-#ifdef CONFIG_8723AU_P2P
-enum P2P_PS_STATE {
-	P2P_PS_DISABLE = 0,
-	P2P_PS_ENABLE = 1,
-	P2P_PS_SCAN = 2,
-	P2P_PS_SCAN_DONE = 3,
-	P2P_PS_ALLSTASLEEP = 4, /*  for P2P GO */
-};
-
-enum P2P_PS_MODE {
-	P2P_PS_NONE = 0,
-	P2P_PS_CTWINDOW = 1,
-	P2P_PS_NOA	 = 2,
-	P2P_PS_MIX = 3, /*  CTWindow and NoA */
-};
-#endif /*  CONFIG_8723AU_P2P */
-
-/*	=====================WFD Section===================== */
-/*	For Wi-Fi Display */
-#define	WFD_ATTR_DEVICE_INFO			0x00
-#define	WFD_ATTR_ASSOC_BSSID			0x01
-#define	WFD_ATTR_COUPLED_SINK_INFO	0x06
-#define	WFD_ATTR_LOCAL_IP_ADDR		0x08
-#define	WFD_ATTR_SESSION_INFO		0x09
-#define	WFD_ATTR_ALTER_MAC			0x0a
-
-/*	For WFD Device Information Attribute */
-#define	WFD_DEVINFO_SOURCE					0x0000
-#define	WFD_DEVINFO_PSINK					0x0001
-#define	WFD_DEVINFO_SSINK					0x0002
-#define	WFD_DEVINFO_DUAL					0x0003
-
-#define	WFD_DEVINFO_SESSION_AVAIL			0x0010
-#define	WFD_DEVINFO_WSD						0x0040
-#define	WFD_DEVINFO_PC_TDLS					0x0080
-#define	WFD_DEVINFO_HDCP_SUPPORT			0x0100
-
 #endif /*  _WIFI_H_ */
diff --git a/drivers/staging/rtl8723au/include/wlan_bssdef.h b/drivers/staging/rtl8723au/include/wlan_bssdef.h
index 92287ebe5b9..664015d049e 100644
--- a/drivers/staging/rtl8723au/include/wlan_bssdef.h
+++ b/drivers/staging/rtl8723au/include/wlan_bssdef.h
@@ -22,54 +22,6 @@
 #define NDIS_802_11_LENGTH_RATES        8
 #define NDIS_802_11_LENGTH_RATES_EX     16
 
-enum ndis_802_11_net_type {
-	Ndis802_11FH,
-	Ndis802_11DS,
-	Ndis802_11OFDM5,
-	Ndis802_11OFDM24,
-	Ndis802_11NetworkTypeMax    /*  just an upper bound */
-};
-
-struct ndis_802_11_configuration_fh {
-	u32           Length;             /*  Length of structure */
-	u32           HopPattern;         /*  As defined by 802.11, MSB set */
-	u32           HopSet;             /*  to one if non-802.11 */
-	u32           DwellTime;          /*  units are Kusec */
-};
-
-
-/*
-	FW will only save the channel number in DSConfig.
-	ODI Handler will convert the channel number to freq. number.
-*/
-struct ndis_802_11_config {
-	u32           Length;             /*  Length of structure */
-	u32           BeaconPeriod;       /*  units are Kusec */
-	u32           ATIMWindow;         /*  units are Kusec */
-	u32           DSConfig;           /*  Frequency, units are kHz */
-	struct ndis_802_11_configuration_fh    FHConfig;
-};
-
-enum ndis_802_11_net_infra {
-	Ndis802_11IBSS,
-	Ndis802_11Infrastructure,
-	Ndis802_11AutoUnknown,
-	Ndis802_11InfrastructureMax,     /*  Not a real value, defined as upper bound */
-	Ndis802_11APMode
-};
-
-struct ndis_802_11_fixed_ies {
-	u8  Timestamp[8];
-	u16  BeaconInterval;
-	u16  Capabilities;
-};
-
-struct ndis_802_11_var_ies {
-	u8  ElementID;
-	u8  Length;
-	u8  data[1];
-};
-
 /* Length is the 4 bytes multiples of the sum of
  * sizeof(6 * sizeof(unsigned char)) + 2 + sizeof(struct ndis_802_11_ssid) +
  * sizeof(u32) + sizeof(long) + sizeof(enum ndis_802_11_net_type) +
@@ -115,34 +67,6 @@ struct ndis_802_11_key {
 	u8 KeyMaterial[32]; /*  variable length depending on above field */
 };
 
-struct ndis_802_11_wep {
-	u32     Length;        /*  Length of this structure */
-	u32     KeyIndex;      /*  0 is the per-client key, 1-N are global */
-	u32     KeyLength;     /*  length of key in bytes */
-	u8     KeyMaterial[16];/*  variable length depending on above field */
-};
-
-enum NDIS_802_11_STATUS_TYPE {
-	Ndis802_11StatusType_Authentication,
-	Ndis802_11StatusType_MediaStreamMode,
-	Ndis802_11StatusType_PMKID_CandidateList,
-	Ndis802_11StatusTypeMax    /*  not a real type, just an upper bound */
-};
-
-/*  mask for authentication/integrity fields */
-#define NDIS_802_11_AUTH_REQUEST_AUTH_FIELDS        0x0f
-#define NDIS_802_11_AUTH_REQUEST_REAUTH			0x01
-#define NDIS_802_11_AUTH_REQUEST_KEYUPDATE		0x02
-#define NDIS_802_11_AUTH_REQUEST_PAIRWISE_ERROR		0x06
-#define NDIS_802_11_AUTH_REQUEST_GROUP_ERROR		0x0E
-
-/*  MIC check time, 60 seconds. */
-#define MIC_CHECK_TIME	60000000
-
-#ifndef Ndis802_11APMode
-#define Ndis802_11APMode (Ndis802_11InfrastructureMax+1)
-#endif
-
 struct wlan_phy_info {
 	u8	SignalStrength;/* in percentage) */
 	u8	SignalQuality;/* in percentage) */
@@ -170,9 +94,10 @@ struct wlan_bssid_ex {
 	struct cfg80211_ssid Ssid;
 	u32  Privacy;
 	long  Rssi;/* in dBM, raw data , get from PHY) */
-	enum ndis_802_11_net_type  NetworkTypeInUse;
-	struct ndis_802_11_config  Configuration;
-	enum ndis_802_11_net_infra  InfrastructureMode;
+	u16 BeaconPeriod;       /*  units are Kusec */
+	u32 ATIMWindow;         /*  units are Kusec */
+	u32 DSConfig;           /*  Frequency, units are kHz */
+	enum nl80211_iftype ifmode;
 	unsigned char SupportedRates[NDIS_802_11_LENGTH_RATES_EX];
 	struct wlan_phy_info	PhyInfo;
 	u32  IELength;
diff --git a/drivers/staging/rtl8723au/include/xmit_osdep.h b/drivers/staging/rtl8723au/include/xmit_osdep.h
index 0eca53ece75..2be04c48656 100644
--- a/drivers/staging/rtl8723au/include/xmit_osdep.h
+++ b/drivers/staging/rtl8723au/include/xmit_osdep.h
@@ -18,24 +18,9 @@
 #include <osdep_service.h>
 #include <drv_types.h>
 
-struct pkt_file {
-	struct sk_buff *pkt;
-	__kernel_size_t pkt_len; /* the remainder length of the open_file */
-	unsigned char *cur_buffer;
-	u8 *buf_start;
-	u8 *cur_addr;
-	__kernel_size_t buf_len;
-};
-
 
 #define NR_XMITFRAME	256
 
-struct xmit_priv;
-struct pkt_attrib;
-struct sta_xmit_priv;
-struct xmit_frame;
-struct xmit_buf;
-
 int rtw_xmit23a_entry23a(struct sk_buff *pkt, struct net_device *pnetdev);
 
 void rtw_os_xmit_schedule23a(struct rtw_adapter *padapter);
@@ -44,10 +29,6 @@ int rtw_os_xmit_resource_alloc23a(struct rtw_adapter *padapter,
 			       struct xmit_buf *pxmitbuf, u32 alloc_sz);
 void rtw_os_xmit_resource_free23a(struct rtw_adapter *padapter,
 			       struct xmit_buf *pxmitbuf);
-uint rtw_remainder_len23a(struct pkt_file *pfile);
-void _rtw_open_pktfile23a(struct sk_buff *pkt, struct pkt_file *pfile);
-uint _rtw_pktfile_read23a(struct pkt_file *pfile, u8 *rmem, uint rlen);
-int rtw_endofpktfile23a(struct pkt_file *pfile);
 
 void rtw_os_pkt_complete23a(struct rtw_adapter *padapter, struct sk_buff *pkt);
 void rtw_os_xmit_complete23a(struct rtw_adapter *padapter,
diff --git a/drivers/staging/rtl8723au/os_dep/ioctl_cfg80211.c b/drivers/staging/rtl8723au/os_dep/ioctl_cfg80211.c
index 50840b9a11f..0c9f5cebfb4 100644
--- a/drivers/staging/rtl8723au/os_dep/ioctl_cfg80211.c
+++ b/drivers/staging/rtl8723au/os_dep/ioctl_cfg80211.c
@@ -20,7 +20,6 @@
 #include <xmit_osdep.h>
 
 #include "ioctl_cfg80211.h"
-#include <linux/version.h>
 
 #define RTW_MAX_MGMT_TX_CNT 8
 
@@ -260,26 +259,21 @@ static int rtw_cfg80211_inform_bss(struct rtw_adapter *padapter,
 	size_t notify_ielen;
 	s32 notify_signal;
 	u8 buf[MAX_BSSINFO_LEN], *pbuf;
-	size_t len, bssinf_len = 0;
+	size_t len;
 	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
-	u8 bc_addr[] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
-
 	struct wireless_dev *wdev = padapter->rtw_wdev;
 	struct wiphy *wiphy = wdev->wiphy;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
 	/* DBG_8723A("%s\n", __func__); */
 
-	bssinf_len =
-		pnetwork->network.IELength + sizeof(struct ieee80211_hdr_3addr);
-	if (bssinf_len > MAX_BSSINFO_LEN) {
+	if (pnetwork->network.IELength > MAX_IE_SZ) {
 		DBG_8723A("%s IE Length too long > %d byte\n", __func__,
-			  MAX_BSSINFO_LEN);
+			  MAX_IE_SZ);
 		goto exit;
 	}
 
-	channel = pnetwork->network.Configuration.DSConfig;
+	channel = pnetwork->network.DSConfig;
 	if (channel <= RTW_CH_MAX_2G_CHANNEL)
 		freq = ieee80211_channel_to_frequency(channel,
 						      IEEE80211_BAND_2GHZ);
@@ -289,14 +283,13 @@ static int rtw_cfg80211_inform_bss(struct rtw_adapter *padapter,
 
 	notify_channel = ieee80211_get_channel(wiphy, freq);
 
-	/* rtw_get_timestampe_from_ie23a() */
 	notify_timestamp = jiffies_to_msecs(jiffies) * 1000;	/* uSec */
 
 	notify_interval =
-	    le16_to_cpu(*(u16 *)
+		get_unaligned_le16(
 			rtw_get_beacon_interval23a_from_ie(pnetwork->network.IEs));
 	notify_capability =
-	    le16_to_cpu(*(u16 *)
+		get_unaligned_le16(
 			rtw_get_capability23a_from_ie(pnetwork->network.IEs));
 
 	notify_ie = pnetwork->network.IEs + _FIXED_IE_LENGTH_;
@@ -315,21 +308,22 @@ static int rtw_cfg80211_inform_bss(struct rtw_adapter *padapter,
 	pbuf = buf;
 
 	pwlanhdr = (struct ieee80211_hdr *)pbuf;
-	fctrl = &pwlanhdr->frame_control;
-	*(fctrl) = 0;
 
-	SetSeqNum(pwlanhdr, 0);
+	pwlanhdr->seq_ctrl = 0;
 
 	if (pnetwork->network.reserved == 1) {	/*  WIFI_BEACON */
-		memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
-		SetFrameSubType(pbuf, WIFI_BEACON);
+		eth_broadcast_addr(pwlanhdr->addr1);
+		pwlanhdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+						      IEEE80211_STYPE_BEACON);
 	} else {
-		memcpy(pwlanhdr->addr1, myid(&padapter->eeprompriv), ETH_ALEN);
-		SetFrameSubType(pbuf, WIFI_PROBERSP);
+		ether_addr_copy(pwlanhdr->addr1, myid(&padapter->eeprompriv));
+		pwlanhdr->frame_control =
+			cpu_to_le16(IEEE80211_FTYPE_MGMT |
+				    IEEE80211_STYPE_PROBE_RESP);
 	}
 
-	memcpy(pwlanhdr->addr2, pnetwork->network.MacAddress, ETH_ALEN);
-	memcpy(pwlanhdr->addr3, pnetwork->network.MacAddress, ETH_ALEN);
+	ether_addr_copy(pwlanhdr->addr2, pnetwork->network.MacAddress);
+	ether_addr_copy(pwlanhdr->addr3, pnetwork->network.MacAddress);
 
 	pbuf += sizeof(struct ieee80211_hdr_3addr);
 	len = sizeof(struct ieee80211_hdr_3addr);
@@ -357,9 +351,6 @@ void rtw_cfg80211_indicate_connect(struct rtw_adapter *padapter)
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct wlan_network *cur_network = &pmlmepriv->cur_network;
 	struct wireless_dev *pwdev = padapter->rtw_wdev;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-#endif
 
 	DBG_8723A("%s(padapter =%p)\n", __func__, padapter);
 
@@ -370,22 +361,11 @@ void rtw_cfg80211_indicate_connect(struct rtw_adapter *padapter)
 	if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
 		return;
 
-#ifdef CONFIG_8723AU_P2P
-	if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) {
-		rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
-		rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT);
-		rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
-		DBG_8723A("%s, role =%d, p2p_state =%d, pre_p2p_state =%d\n",
-			  __func__, rtw_p2p_role(pwdinfo),
-			  rtw_p2p_state(pwdinfo), rtw_p2p_pre_state(pwdinfo));
-	}
-#endif /* CONFIG_8723AU_P2P */
-
-	if (rtw_to_roaming(padapter) > 0) {
+	if (padapter->mlmepriv.to_roaming > 0) {
 		struct wiphy *wiphy = pwdev->wiphy;
 		struct ieee80211_channel *notify_channel;
 		u32 freq;
-		u16 channel = cur_network->network.Configuration.DSConfig;
+		u16 channel = cur_network->network.DSConfig;
 
 		if (channel <= RTW_CH_MAX_2G_CHANNEL)
 			freq =
@@ -429,9 +409,6 @@ void rtw_cfg80211_indicate_disconnect(struct rtw_adapter *padapter)
 {
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct wireless_dev *pwdev = padapter->rtw_wdev;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-#endif
 
 	DBG_8723A("%s(padapter =%p)\n", __func__, padapter);
 
@@ -442,21 +419,6 @@ void rtw_cfg80211_indicate_disconnect(struct rtw_adapter *padapter)
 	if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
 		return;
 
-#ifdef CONFIG_8723AU_P2P
-	if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) {
-		del_timer_sync(&pwdinfo->find_phase_timer);
-		del_timer_sync(&pwdinfo->restore_p2p_state_timer);
-		del_timer_sync(&pwdinfo->pre_tx_scan_timer);
-
-		rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
-		rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
-
-		DBG_8723A("%s, role =%d, p2p_state =%d, pre_p2p_state =%d\n",
-			  __func__, rtw_p2p_role(pwdinfo),
-			  rtw_p2p_state(pwdinfo), rtw_p2p_pre_state(pwdinfo));
-	}
-#endif /* CONFIG_8723AU_P2P */
-
 	if (!padapter->mlmepriv.not_indic_disco) {
 		if (check_fwstate(&padapter->mlmepriv, WIFI_UNDER_LINKING)) {
 			cfg80211_connect_result(padapter->pnetdev, NULL, NULL,
@@ -471,12 +433,12 @@ void rtw_cfg80211_indicate_disconnect(struct rtw_adapter *padapter)
 }
 
 #ifdef CONFIG_8723AU_AP_MODE
-static u8 set_pairwise_key(struct rtw_adapter *padapter, struct sta_info *psta)
+static int set_pairwise_key(struct rtw_adapter *padapter, struct sta_info *psta)
 {
 	struct cmd_obj *ph2c;
 	struct set_stakey_parm *psetstakey_para;
 	struct cmd_priv *pcmdpriv = &padapter->cmdpriv;
-	u8 res = _SUCCESS;
+	int res = _SUCCESS;
 
 	ph2c = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
 	if (ph2c == NULL) {
@@ -493,9 +455,9 @@ static u8 set_pairwise_key(struct rtw_adapter *padapter, struct sta_info *psta)
 
 	init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para, _SetStaKey_CMD_);
 
-	psetstakey_para->algorithm = (u8) psta->dot118021XPrivacy;
+	psetstakey_para->algorithm = psta->dot118021XPrivacy;
 
-	memcpy(psetstakey_para->addr, psta->hwaddr, ETH_ALEN);
+	ether_addr_copy(psetstakey_para->addr, psta->hwaddr);
 
 	memcpy(psetstakey_para->key, &psta->dot118021x_UncstKey, 16);
 
@@ -505,8 +467,8 @@ exit:
 	return res;
 }
 
-static int set_group_key(struct rtw_adapter *padapter, u8 *key, u8 alg,
-			 int keyid)
+static int set_group_key(struct rtw_adapter *padapter, u8 *key, u32 alg,
+			 u8 keyid)
 {
 	u8 keylen;
 	struct cmd_obj *pcmd;
@@ -516,6 +478,11 @@ static int set_group_key(struct rtw_adapter *padapter, u8 *key, u8 alg,
 
 	DBG_8723A("%s\n", __func__);
 
+	if (keyid >= 4) {
+		res = _FAIL;
+		goto exit;
+	}
+
 	pcmd = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
 	if (!pcmd) {
 		res = _FAIL;
@@ -528,24 +495,23 @@ static int set_group_key(struct rtw_adapter *padapter, u8 *key, u8 alg,
 		goto exit;
 	}
 
-	psetkeyparm->keyid = (u8) keyid;
+	psetkeyparm->keyid = keyid;
 	if (is_wep_enc(alg))
-		padapter->mlmepriv.key_mask |= CHKBIT(psetkeyparm->keyid);
+		padapter->mlmepriv.key_mask |= BIT(psetkeyparm->keyid);
 
 	psetkeyparm->algorithm = alg;
 
 	psetkeyparm->set_tx = 1;
 
 	switch (alg) {
-	case _WEP40_:
+	case WLAN_CIPHER_SUITE_WEP40:
 		keylen = 5;
 		break;
-	case _WEP104_:
+	case WLAN_CIPHER_SUITE_WEP104:
 		keylen = 13;
 		break;
-	case _TKIP_:
-	case _TKIP_WTMIC_:
-	case _AES_:
+	case WLAN_CIPHER_SUITE_TKIP:
+	case WLAN_CIPHER_SUITE_CCMP:
 	default:
 		keylen = 16;
 	}
@@ -558,28 +524,26 @@ static int set_group_key(struct rtw_adapter *padapter, u8 *key, u8 alg,
 	pcmd->rsp = NULL;
 	pcmd->rspsz = 0;
 
-	INIT_LIST_HEAD(&pcmd->list);
-
 	res = rtw_enqueue_cmd23a(pcmdpriv, pcmd);
 
 exit:
 	return res;
 }
 
-static int set_wep_key(struct rtw_adapter *padapter, u8 *key, u8 keylen,
-		       int keyid)
+static int set_wep_key(struct rtw_adapter *padapter, u8 *key, u16 keylen,
+		       u8 keyid)
 {
-	u8 alg;
+	u32 alg;
 
 	switch (keylen) {
 	case 5:
-		alg = _WEP40_;
+		alg = WLAN_CIPHER_SUITE_WEP40;
 		break;
 	case 13:
-		alg = _WEP104_;
+		alg = WLAN_CIPHER_SUITE_WEP104;
 		break;
 	default:
-		alg = _NO_PRIVACY_;
+		alg = 0;
 	}
 
 	return set_group_key(padapter, key, alg, keyid);
@@ -590,7 +554,8 @@ static int rtw_cfg80211_ap_set_encryption(struct net_device *dev,
 					  u32 param_len)
 {
 	int ret = 0;
-	u32 wep_key_idx, wep_key_len;
+	u16 wep_key_len;
+	u8 wep_key_idx;
 	struct sta_info *psta = NULL, *pbcmc_sta = NULL;
 	struct rtw_adapter *padapter = netdev_priv(dev);
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
@@ -658,21 +623,21 @@ static int rtw_cfg80211_ap_set_encryption(struct net_device *dev,
 
 			psecuritypriv->ndisencryptstatus =
 				Ndis802_11Encryption1Enabled;
-			psecuritypriv->dot11PrivacyAlgrthm = _WEP40_;
-			psecuritypriv->dot118021XGrpPrivacy = _WEP40_;
+			psecuritypriv->dot11PrivacyAlgrthm = WLAN_CIPHER_SUITE_WEP40;
+			psecuritypriv->dot118021XGrpPrivacy = WLAN_CIPHER_SUITE_WEP40;
 
 			if (wep_key_len == 13) {
-				psecuritypriv->dot11PrivacyAlgrthm = _WEP104_;
-				psecuritypriv->dot118021XGrpPrivacy = _WEP104_;
+				psecuritypriv->dot11PrivacyAlgrthm = WLAN_CIPHER_SUITE_WEP104;
+				psecuritypriv->dot118021XGrpPrivacy = WLAN_CIPHER_SUITE_WEP104;
 			}
 
 			psecuritypriv->dot11PrivacyKeyIndex = wep_key_idx;
 		}
 
-		memcpy(&psecuritypriv->dot11DefKey[wep_key_idx].skey[0],
+		memcpy(&psecuritypriv->wep_key[wep_key_idx].key,
 		       param->u.crypt.key, wep_key_len);
 
-		psecuritypriv->dot11DefKeylen[wep_key_idx] = wep_key_len;
+		psecuritypriv->wep_key[wep_key_idx].keylen = wep_key_len;
 
 		set_wep_key(padapter, param->u.crypt.key, wep_key_len,
 			    wep_key_idx);
@@ -684,8 +649,7 @@ static int rtw_cfg80211_ap_set_encryption(struct net_device *dev,
 	if (!psta && check_fwstate(pmlmepriv, WIFI_AP_STATE)) {	/*  group key */
 		if (param->u.crypt.set_tx == 0) {	/* group key */
 			if (strcmp(param->u.crypt.alg, "WEP") == 0) {
-				DBG_8723A("%s, set group_key, WEP\n",
-					  __func__);
+				DBG_8723A("%s, set group_key, WEP\n", __func__);
 
 				memcpy(psecuritypriv->
 				       dot118021XGrpKey[param->u.crypt.idx].
@@ -693,17 +657,17 @@ static int rtw_cfg80211_ap_set_encryption(struct net_device *dev,
 				       (param->u.crypt.key_len >
 					16 ? 16 : param->u.crypt.key_len));
 
-				psecuritypriv->dot118021XGrpPrivacy = _WEP40_;
+				psecuritypriv->dot118021XGrpPrivacy = WLAN_CIPHER_SUITE_WEP40;
 				if (param->u.crypt.key_len == 13) {
 					psecuritypriv->dot118021XGrpPrivacy =
-					    _WEP104_;
+					    WLAN_CIPHER_SUITE_WEP104;
 				}
 
 			} else if (strcmp(param->u.crypt.alg, "TKIP") == 0) {
 				DBG_8723A("%s, set group_key, TKIP\n",
 					  __func__);
 
-				psecuritypriv->dot118021XGrpPrivacy = _TKIP_;
+				psecuritypriv->dot118021XGrpPrivacy = WLAN_CIPHER_SUITE_TKIP;
 
 				memcpy(psecuritypriv->
 				       dot118021XGrpKey[param->u.crypt.idx].
@@ -722,13 +686,13 @@ static int rtw_cfg80211_ap_set_encryption(struct net_device *dev,
 							     idx].skey,
 				       &param->u.crypt.key[24], 8);
 
-				psecuritypriv->busetkipkey = true;
+				psecuritypriv->busetkipkey = 1;
 
 			} else if (strcmp(param->u.crypt.alg, "CCMP") == 0) {
 				DBG_8723A("%s, set group_key, CCMP\n",
 					  __func__);
 
-				psecuritypriv->dot118021XGrpPrivacy = _AES_;
+				psecuritypriv->dot118021XGrpPrivacy = WLAN_CIPHER_SUITE_CCMP;
 
 				memcpy(psecuritypriv->
 				       dot118021XGrpKey[param->u.crypt.idx].
@@ -740,12 +704,12 @@ static int rtw_cfg80211_ap_set_encryption(struct net_device *dev,
 					  __func__);
 
 				psecuritypriv->dot118021XGrpPrivacy =
-				    _NO_PRIVACY_;
+				    0;
 			}
 
 			psecuritypriv->dot118021XGrpKeyid = param->u.crypt.idx;
 
-			psecuritypriv->binstallGrpkey = true;
+			psecuritypriv->binstallGrpkey = 1;
 
 			psecuritypriv->dot11PrivacyAlgrthm =
 				psecuritypriv->dot118021XGrpPrivacy;
@@ -782,16 +746,16 @@ static int rtw_cfg80211_ap_set_encryption(struct net_device *dev,
 					DBG_8723A("%s, set pairwise key, WEP\n",
 						  __func__);
 
-					psta->dot118021XPrivacy = _WEP40_;
+					psta->dot118021XPrivacy = WLAN_CIPHER_SUITE_WEP40;
 					if (param->u.crypt.key_len == 13) {
 						psta->dot118021XPrivacy =
-							_WEP104_;
+							WLAN_CIPHER_SUITE_WEP104;
 					}
 				} else if (!strcmp(param->u.crypt.alg, "TKIP")) {
 					DBG_8723A("%s, set pairwise key, "
 						  "TKIP\n", __func__);
 
-					psta->dot118021XPrivacy = _TKIP_;
+					psta->dot118021XPrivacy = WLAN_CIPHER_SUITE_TKIP;
 
 					/* DEBUG_ERR("set key length :param->u.crypt.key_len =%d\n", param->u.crypt.key_len); */
 					/* set mic key */
@@ -800,19 +764,19 @@ static int rtw_cfg80211_ap_set_encryption(struct net_device *dev,
 					memcpy(psta->dot11tkiprxmickey.skey,
 					       &param->u.crypt.key[24], 8);
 
-					psecuritypriv->busetkipkey = true;
+					psecuritypriv->busetkipkey = 1;
 
 				} else if (!strcmp(param->u.crypt.alg, "CCMP")) {
 
 					DBG_8723A("%s, set pairwise key, "
 						  "CCMP\n", __func__);
 
-					psta->dot118021XPrivacy = _AES_;
+					psta->dot118021XPrivacy = WLAN_CIPHER_SUITE_CCMP;
 				} else {
 					DBG_8723A("%s, set pairwise key, "
 						  "none\n", __func__);
 
-					psta->dot118021XPrivacy = _NO_PRIVACY_;
+					psta->dot118021XPrivacy = 0;
 				}
 
 				set_pairwise_key(padapter, psta);
@@ -831,15 +795,15 @@ static int rtw_cfg80211_ap_set_encryption(struct net_device *dev,
 						key_len));
 
 					psecuritypriv->dot118021XGrpPrivacy =
-						_WEP40_;
+						WLAN_CIPHER_SUITE_WEP40;
 					if (param->u.crypt.key_len == 13) {
 						psecuritypriv->
 						    dot118021XGrpPrivacy =
-							_WEP104_;
+							WLAN_CIPHER_SUITE_WEP104;
 					}
 				} else if (!strcmp(param->u.crypt.alg, "TKIP")) {
 					psecuritypriv->dot118021XGrpPrivacy =
-					    _TKIP_;
+					    WLAN_CIPHER_SUITE_TKIP;
 
 					memcpy(psecuritypriv->
 					       dot118021XGrpKey[param->u.crypt.
@@ -864,11 +828,11 @@ static int rtw_cfg80211_ap_set_encryption(struct net_device *dev,
 					       skey, &param->u.crypt.key[24],
 					       8);
 
-					psecuritypriv->busetkipkey = true;
+					psecuritypriv->busetkipkey = 1;
 
 				} else if (!strcmp(param->u.crypt.alg, "CCMP")) {
 					psecuritypriv->dot118021XGrpPrivacy =
-						_AES_;
+						WLAN_CIPHER_SUITE_CCMP;
 
 					memcpy(psecuritypriv->
 					       dot118021XGrpKey[param->u.crypt.
@@ -879,13 +843,13 @@ static int rtw_cfg80211_ap_set_encryption(struct net_device *dev,
 						key_len));
 				} else {
 					psecuritypriv->dot118021XGrpPrivacy =
-						_NO_PRIVACY_;
+						0;
 				}
 
 				psecuritypriv->dot118021XGrpKeyid =
 					param->u.crypt.idx;
 
-				psecuritypriv->binstallGrpkey = true;
+				psecuritypriv->binstallGrpkey = 1;
 
 				psecuritypriv->dot11PrivacyAlgrthm =
 					psecuritypriv->dot118021XGrpPrivacy;
@@ -917,15 +881,11 @@ static int rtw_cfg80211_set_encryption(struct net_device *dev,
 				       struct ieee_param *param, u32 param_len)
 {
 	int ret = 0;
-	u32 wep_key_idx, wep_key_len;
+	u32 wep_key_idx;
+	u16 wep_key_len;
 	struct rtw_adapter *padapter = netdev_priv(dev);
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-#endif /* CONFIG_8723AU_P2P */
-
-
 
 	DBG_8723A("%s\n", __func__);
 
@@ -970,21 +930,21 @@ static int rtw_cfg80211_set_encryption(struct net_device *dev,
 
 			psecuritypriv->ndisencryptstatus =
 				Ndis802_11Encryption1Enabled;
-			psecuritypriv->dot11PrivacyAlgrthm = _WEP40_;
-			psecuritypriv->dot118021XGrpPrivacy = _WEP40_;
+			psecuritypriv->dot11PrivacyAlgrthm = WLAN_CIPHER_SUITE_WEP40;
+			psecuritypriv->dot118021XGrpPrivacy = WLAN_CIPHER_SUITE_WEP40;
 
 			if (wep_key_len == 13) {
-				psecuritypriv->dot11PrivacyAlgrthm = _WEP104_;
-				psecuritypriv->dot118021XGrpPrivacy = _WEP104_;
+				psecuritypriv->dot11PrivacyAlgrthm = WLAN_CIPHER_SUITE_WEP104;
+				psecuritypriv->dot118021XGrpPrivacy = WLAN_CIPHER_SUITE_WEP104;
 			}
 
 			psecuritypriv->dot11PrivacyKeyIndex = wep_key_idx;
 		}
 
-		memcpy(&psecuritypriv->dot11DefKey[wep_key_idx].skey[0],
+		memcpy(&psecuritypriv->wep_key[wep_key_idx].key,
 		       param->u.crypt.key, wep_key_len);
 
-		psecuritypriv->dot11DefKeylen[wep_key_idx] = wep_key_len;
+		psecuritypriv->wep_key[wep_key_idx].keylen = wep_key_len;
 
 		rtw_set_key23a(padapter, psecuritypriv, wep_key_idx, 0);
 
@@ -1041,7 +1001,7 @@ static int rtw_cfg80211_set_encryption(struct net_device *dev,
 						       8);
 
 						padapter->securitypriv.
-							busetkipkey = false;
+							busetkipkey = 0;
 					}
 					DBG_8723A(" ~~~~set sta key:unicastkey\n");
 
@@ -1067,7 +1027,7 @@ static int rtw_cfg80211_set_encryption(struct net_device *dev,
 					       skey, &param->u.crypt.key[24],
 					       8);
 					padapter->securitypriv.binstallGrpkey =
-					    true;
+						1;
 					/* DEBUG_ERR((" param->u.crypt.key_len"
 					   "=%d\n", param->u.crypt.key_len)); */
 					DBG_8723A
@@ -1080,15 +1040,6 @@ static int rtw_cfg80211_set_encryption(struct net_device *dev,
 					rtw_set_key23a(padapter,
 						    &padapter->securitypriv,
 						    param->u.crypt.idx, 1);
-#ifdef CONFIG_8723AU_P2P
-					if (rtw_p2p_chk_state
-					    (pwdinfo,
-					     P2P_STATE_PROVISIONING_ING)) {
-						rtw_p2p_set_state(pwdinfo,
-								  P2P_STATE_PROVISIONING_DONE);
-					}
-#endif /* CONFIG_8723AU_P2P */
-
 				}
 			}
 
@@ -1127,13 +1078,13 @@ static int cfg80211_rtw_add_key(struct wiphy *wiphy, struct net_device *ndev,
 {
 	char *alg_name;
 	u32 param_len;
-	struct ieee_param *param = NULL;
+	struct ieee_param *param;
 	int ret = 0;
 	struct wireless_dev *rtw_wdev = wiphy_to_wdev(wiphy);
 	struct rtw_adapter *padapter = wiphy_to_adapter(wiphy);
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
-	DBG_8723A(FUNC_NDEV_FMT " adding key for %pM\n", FUNC_NDEV_ARG(ndev),
+	DBG_8723A("%s(%s): adding key for %pM\n", __func__, ndev->name,
 		  mac_addr);
 	DBG_8723A("cipher = 0x%x\n", params->cipher);
 	DBG_8723A("key_len = 0x%x\n", params->key_len);
@@ -1143,11 +1094,11 @@ static int cfg80211_rtw_add_key(struct wiphy *wiphy, struct net_device *ndev,
 
 	param_len = sizeof(struct ieee_param) + params->key_len;
 	param = kzalloc(param_len, GFP_KERNEL);
-	if (param == NULL)
-		return -1;
+	if (!param)
+		return -ENOMEM;
 
 	param->cmd = IEEE_CMD_SET_ENCRYPTION;
-	memset(param->sta_addr, 0xff, ETH_ALEN);
+	eth_broadcast_addr(param->sta_addr);
 
 	switch (params->cipher) {
 	case IW_AUTH_CIPHER_NONE:
@@ -1196,7 +1147,7 @@ static int cfg80211_rtw_add_key(struct wiphy *wiphy, struct net_device *ndev,
 	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
 #ifdef CONFIG_8723AU_AP_MODE
 		if (mac_addr)
-			memcpy(param->sta_addr, (void *)mac_addr, ETH_ALEN);
+			ether_addr_copy(param->sta_addr, mac_addr);
 
 		ret = rtw_cfg80211_ap_set_encryption(ndev, param, param_len);
 #endif
@@ -1218,7 +1169,7 @@ cfg80211_rtw_get_key(struct wiphy *wiphy, struct net_device *ndev,
 		     void *cookie,
 		     void (*callback) (void *cookie, struct key_params *))
 {
-	DBG_8723A(FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("%s(%s)\n", __func__, ndev->name);
 	return 0;
 }
 
@@ -1229,8 +1180,7 @@ static int cfg80211_rtw_del_key(struct wiphy *wiphy, struct net_device *ndev,
 	struct rtw_adapter *padapter = netdev_priv(ndev);
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 
-	DBG_8723A(FUNC_NDEV_FMT " key_index =%d\n", FUNC_NDEV_ARG(ndev),
-		  key_index);
+	DBG_8723A("%s(%s): key_index =%d\n", __func__, ndev->name, key_index);
 
 	if (key_index == psecuritypriv->dot11PrivacyKeyIndex) {
 		/* clear the flag of wep default key set. */
@@ -1247,23 +1197,24 @@ static int cfg80211_rtw_set_default_key(struct wiphy *wiphy,
 	struct rtw_adapter *padapter = netdev_priv(ndev);
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 
-	DBG_8723A(FUNC_NDEV_FMT " key_index =%d"
-		  ", unicast =%d, multicast =%d.\n", FUNC_NDEV_ARG(ndev),
-		  key_index, unicast, multicast);
+	DBG_8723A("%s(%s): key_index =%d, unicast =%d, multicast =%d.\n",
+		  __func__, ndev->name, key_index, unicast, multicast);
 
-	if ((key_index < WEP_KEYS) &&
-	    ((psecuritypriv->dot11PrivacyAlgrthm == _WEP40_) ||
-	     (psecuritypriv->dot11PrivacyAlgrthm == _WEP104_))) {
+	if (key_index < NUM_WEP_KEYS &&
+	    (psecuritypriv->dot11PrivacyAlgrthm == WLAN_CIPHER_SUITE_WEP40 ||
+	     psecuritypriv->dot11PrivacyAlgrthm == WLAN_CIPHER_SUITE_WEP104)) {
 		/* set wep default key */
 		psecuritypriv->ndisencryptstatus = Ndis802_11Encryption1Enabled;
 
 		psecuritypriv->dot11PrivacyKeyIndex = key_index;
 
-		psecuritypriv->dot11PrivacyAlgrthm = _WEP40_;
-		psecuritypriv->dot118021XGrpPrivacy = _WEP40_;
-		if (psecuritypriv->dot11DefKeylen[key_index] == 13) {
-			psecuritypriv->dot11PrivacyAlgrthm = _WEP104_;
-			psecuritypriv->dot118021XGrpPrivacy = _WEP104_;
+		psecuritypriv->dot11PrivacyAlgrthm = WLAN_CIPHER_SUITE_WEP40;
+		psecuritypriv->dot118021XGrpPrivacy = WLAN_CIPHER_SUITE_WEP40;
+		if (psecuritypriv->wep_key[key_index].keylen == 13) {
+			psecuritypriv->dot11PrivacyAlgrthm =
+				WLAN_CIPHER_SUITE_WEP104;
+			psecuritypriv->dot118021XGrpPrivacy =
+				WLAN_CIPHER_SUITE_WEP104;
 		}
 
 		/* set the flag to represent that wep default key
@@ -1287,7 +1238,7 @@ static int cfg80211_rtw_get_station(struct wiphy *wiphy,
 	sinfo->filled = 0;
 
 	if (!mac) {
-		DBG_8723A(FUNC_NDEV_FMT " mac ==%p\n", FUNC_NDEV_ARG(ndev), mac);
+		DBG_8723A("%s(%s): mac ==%p\n", __func__, ndev->name, mac);
 		ret = -ENOENT;
 		goto exit;
 	}
@@ -1298,17 +1249,15 @@ static int cfg80211_rtw_get_station(struct wiphy *wiphy,
 		ret = -ENOENT;
 		goto exit;
 	}
-#ifdef CONFIG_DEBUG_CFG80211
-	DBG_8723A(FUNC_NDEV_FMT " mac =" MAC_FMT "\n", FUNC_NDEV_ARG(ndev),
+	DBG_8723A("%s(%s): mac =" MAC_FMT "\n", __func__, ndev->name,
 		  MAC_ARG(mac));
-#endif
 
 	/* for infra./P2PClient mode */
 	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) &&
 	    check_fwstate(pmlmepriv, _FW_LINKED)) {
 		struct wlan_network *cur_network = &pmlmepriv->cur_network;
 
-		if (memcmp(mac, cur_network->network.MacAddress, ETH_ALEN)) {
+		if (!ether_addr_equal(mac, cur_network->network.MacAddress)) {
 			DBG_8723A("%s, mismatch bssid =" MAC_FMT "\n", __func__,
 				  MAC_ARG(cur_network->network.MacAddress));
 			ret = -ENOENT;
@@ -1342,78 +1291,122 @@ exit:
 	return ret;
 }
 
+int cfg80211_infrastructure_mode(struct rtw_adapter* padapter,
+				 enum nl80211_iftype ifmode)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct wlan_network *cur_network = &pmlmepriv->cur_network;
+	enum nl80211_iftype old_mode;
+
+	old_mode = cur_network->network.ifmode;
+
+	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_notice_,
+		 ("+%s: old =%d new =%d fw_state = 0x%08x\n", __func__,
+		  old_mode, ifmode, get_fwstate(pmlmepriv)));
+
+	if (old_mode != ifmode) {
+		spin_lock_bh(&pmlmepriv->lock);
+
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
+			 (" change mode!"));
+
+		if (old_mode == NL80211_IFTYPE_AP ||
+		    old_mode == NL80211_IFTYPE_P2P_GO) {
+			/* change to other mode from Ndis802_11APMode */
+			cur_network->join_res = -1;
+
+#ifdef CONFIG_8723AU_AP_MODE
+			stop_ap_mode23a(padapter);
+#endif
+		}
+
+		if (check_fwstate(pmlmepriv, _FW_LINKED) ||
+		    old_mode == NL80211_IFTYPE_ADHOC)
+			rtw_disassoc_cmd23a(padapter, 0, true);
+
+		if (check_fwstate(pmlmepriv, _FW_LINKED) ||
+		    check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE))
+			rtw_free_assoc_resources23a(padapter, 1);
+
+		if (old_mode == NL80211_IFTYPE_STATION ||
+		    old_mode == NL80211_IFTYPE_P2P_CLIENT ||
+		    old_mode == NL80211_IFTYPE_ADHOC) {
+			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+				/* will clr Linked_state; before this function,
+				   we must have chked whether issue
+				   dis-assoc_cmd or not */
+				rtw_indicate_disconnect23a(padapter);
+			}
+	       }
+
+		cur_network->network.ifmode = ifmode;
+
+		_clr_fwstate_(pmlmepriv, ~WIFI_NULL_STATE);
+
+		switch (ifmode) {
+		case NL80211_IFTYPE_ADHOC:
+			set_fwstate(pmlmepriv, WIFI_ADHOC_STATE);
+			break;
+
+		case NL80211_IFTYPE_P2P_CLIENT:
+		case NL80211_IFTYPE_STATION:
+			set_fwstate(pmlmepriv, WIFI_STATION_STATE);
+			break;
+
+		case NL80211_IFTYPE_P2P_GO:
+		case NL80211_IFTYPE_AP:
+			set_fwstate(pmlmepriv, WIFI_AP_STATE);
+#ifdef CONFIG_8723AU_AP_MODE
+			start_ap_mode23a(padapter);
+			/* rtw_indicate_connect23a(padapter); */
+#endif
+			break;
+
+		default:
+			break;
+		}
+
+		/* SecClearAllKeys(adapter); */
+
+		/* RT_TRACE(COMP_OID_SET, DBG_LOUD,
+		   ("set_infrastructure: fw_state:%x after changing mode\n", */
+		/* get_fwstate(pmlmepriv))); */
+
+		spin_unlock_bh(&pmlmepriv->lock);
+	}
+
+	return _SUCCESS;
+}
+
 static int cfg80211_rtw_change_iface(struct wiphy *wiphy,
 				     struct net_device *ndev,
 				     enum nl80211_iftype type, u32 *flags,
 				     struct vif_params *params)
 {
 	enum nl80211_iftype old_type;
-	enum ndis_802_11_net_infra networkType;
 	struct rtw_adapter *padapter = wiphy_to_adapter(wiphy);
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct wireless_dev *rtw_wdev = wiphy_to_wdev(wiphy);
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-#endif
 	int ret = 0;
-	u8 change = false;
 
-	DBG_8723A(FUNC_NDEV_FMT " call netdev_open23a\n", FUNC_NDEV_ARG(ndev));
-	if (netdev_open23a(ndev) != 0) {
-		ret = -EPERM;
-		goto exit;
-	}
-
-	if (_FAIL == rtw_pwr_wakeup(padapter)) {
-		ret = -EPERM;
-		goto exit;
-	}
+	DBG_8723A("%s(%s): call netdev_open23a\n", __func__, ndev->name);
 
 	old_type = rtw_wdev->iftype;
-	DBG_8723A(FUNC_NDEV_FMT " old_iftype =%d, new_iftype =%d\n",
-		  FUNC_NDEV_ARG(ndev), old_type, type);
+	DBG_8723A("%s(%s): old_iftype =%d, new_iftype =%d\n",
+		  __func__, ndev->name, old_type, type);
 
 	if (old_type != type) {
-		change = true;
 		pmlmeext->action_public_rxseq = 0xffff;
 		pmlmeext->action_public_dialog_token = 0xff;
 	}
 
 	switch (type) {
 	case NL80211_IFTYPE_ADHOC:
-		networkType = Ndis802_11IBSS;
-		break;
 	case NL80211_IFTYPE_P2P_CLIENT:
 	case NL80211_IFTYPE_STATION:
-		networkType = Ndis802_11Infrastructure;
-#ifdef CONFIG_8723AU_P2P
-		if (change && rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) {
-			del_timer_sync(&pwdinfo->find_phase_timer);
-			del_timer_sync(&pwdinfo->restore_p2p_state_timer);
-			del_timer_sync(&pwdinfo->pre_tx_scan_timer);
-
-			/* it means remove GO and change mode from AP(GO)
-			   to station(P2P DEVICE) */
-			rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE);
-			rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
-
-			DBG_8723A("%s, role =%d, p2p_state =%d, pre_p2p_state ="
-				  "%d\n", __func__, rtw_p2p_role(pwdinfo),
-				  rtw_p2p_state(pwdinfo),
-				  rtw_p2p_pre_state(pwdinfo));
-		}
-#endif /* CONFIG_8723AU_P2P */
-		break;
 	case NL80211_IFTYPE_P2P_GO:
 	case NL80211_IFTYPE_AP:
-		networkType = Ndis802_11APMode;
-#ifdef CONFIG_8723AU_P2P
-		if (change && !rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) {
-			/* it means P2P Group created, we will be GO
-			   and change mode from  P2P DEVICE to AP(GO) */
-			rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
-		}
-#endif /* CONFIG_8723AU_P2P */
+	case NL80211_IFTYPE_UNSPECIFIED:
 		break;
 	default:
 		return -EOPNOTSUPP;
@@ -1421,13 +1414,13 @@ static int cfg80211_rtw_change_iface(struct wiphy *wiphy,
 
 	rtw_wdev->iftype = type;
 
-	if (rtw_set_802_11_infrastructure_mode23a(padapter, networkType) == false) {
+	if (cfg80211_infrastructure_mode(padapter, type) != _SUCCESS) {
 		rtw_wdev->iftype = old_type;
 		ret = -EPERM;
 		goto exit;
 	}
 
-	rtw_setopmode_cmd23a(padapter, networkType);
+	rtw_setopmode_cmd23a(padapter, type);
 
 exit:
 	return ret;
@@ -1438,9 +1431,8 @@ void rtw_cfg80211_indicate_scan_done(struct rtw_wdev_priv *pwdev_priv,
 {
 	spin_lock_bh(&pwdev_priv->scan_req_lock);
 	if (pwdev_priv->scan_request != NULL) {
-#ifdef CONFIG_DEBUG_CFG80211
 		DBG_8723A("%s with scan req\n", __func__);
-#endif
+
 		if (pwdev_priv->scan_request->wiphy !=
 		    pwdev_priv->rtw_wdev->wiphy)
 			DBG_8723A("error wiphy compare\n");
@@ -1449,9 +1441,7 @@ void rtw_cfg80211_indicate_scan_done(struct rtw_wdev_priv *pwdev_priv,
 
 		pwdev_priv->scan_request = NULL;
 	} else {
-#ifdef CONFIG_DEBUG_CFG80211
 		DBG_8723A("%s without scan req\n", __func__);
-#endif
 	}
 	spin_unlock_bh(&pwdev_priv->scan_req_lock);
 }
@@ -1463,10 +1453,6 @@ void rtw_cfg80211_surveydone_event_callback(struct rtw_adapter *padapter)
 	struct rtw_queue *queue = &pmlmepriv->scanned_queue;
 	struct wlan_network *pnetwork;
 
-#ifdef CONFIG_DEBUG_CFG80211
-	DBG_8723A("%s\n", __func__);
-#endif
-
 	spin_lock_bh(&pmlmepriv->scanned_queue.lock);
 
 	phead = get_list_head(queue);
@@ -1478,7 +1464,7 @@ void rtw_cfg80211_surveydone_event_callback(struct rtw_adapter *padapter)
 		   contains the channel to which this network belongs */
 		if (rtw_ch_set_search_ch23a
 		    (padapter->mlmeextpriv.channel_set,
-		     pnetwork->network.Configuration.DSConfig) >= 0)
+		     pnetwork->network.DSConfig) >= 0)
 			rtw_cfg80211_inform_bss(padapter, pnetwork);
 	}
 
@@ -1495,93 +1481,31 @@ static int rtw_cfg80211_set_probe_req_wpsp2pie(struct rtw_adapter *padapter,
 	int ret = 0;
 	uint wps_ielen = 0;
 	u8 *wps_ie;
-#ifdef CONFIG_8723AU_P2P
-	u32 p2p_ielen = 0;
-	u8 *p2p_ie;
-	u32 wfd_ielen = 0;
-#endif
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
-#ifdef CONFIG_DEBUG_CFG80211
 	DBG_8723A("%s, ielen =%d\n", __func__, len);
-#endif
 
 	if (len > 0) {
 		wps_ie = rtw_get_wps_ie23a(buf, len, NULL, &wps_ielen);
 		if (wps_ie) {
-#ifdef CONFIG_DEBUG_CFG80211
 			DBG_8723A("probe_req_wps_ielen =%d\n", wps_ielen);
-#endif
+
 			if (pmlmepriv->wps_probe_req_ie) {
 				pmlmepriv->wps_probe_req_ie_len = 0;
 				kfree(pmlmepriv->wps_probe_req_ie);
 				pmlmepriv->wps_probe_req_ie = NULL;
 			}
 
-			pmlmepriv->wps_probe_req_ie =
-				kmalloc(wps_ielen, GFP_KERNEL);
+			pmlmepriv->wps_probe_req_ie = kmemdup(wps_ie,
+							      wps_ielen,
+							      GFP_KERNEL);
 			if (pmlmepriv->wps_probe_req_ie == NULL) {
 				DBG_8723A("%s()-%d: kmalloc() ERROR!\n",
 					  __func__, __LINE__);
 				return -EINVAL;
 			}
-			memcpy(pmlmepriv->wps_probe_req_ie, wps_ie, wps_ielen);
 			pmlmepriv->wps_probe_req_ie_len = wps_ielen;
 		}
-#ifdef CONFIG_8723AU_P2P
-		p2p_ie = rtw_get_p2p_ie23a(buf, len, NULL, &p2p_ielen);
-		if (p2p_ie) {
-#ifdef CONFIG_DEBUG_CFG80211
-			DBG_8723A("probe_req_p2p_ielen =%d\n", p2p_ielen);
-#endif
-
-			if (pmlmepriv->p2p_probe_req_ie) {
-				pmlmepriv->p2p_probe_req_ie_len = 0;
-				kfree(pmlmepriv->p2p_probe_req_ie);
-				pmlmepriv->p2p_probe_req_ie = NULL;
-			}
-
-			pmlmepriv->p2p_probe_req_ie =
-				kmalloc(p2p_ielen, GFP_KERNEL);
-			if (pmlmepriv->p2p_probe_req_ie == NULL) {
-				DBG_8723A("%s()-%d: kmalloc() ERROR!\n",
-					  __func__, __LINE__);
-				return -EINVAL;
-
-			}
-			memcpy(pmlmepriv->p2p_probe_req_ie, p2p_ie, p2p_ielen);
-			pmlmepriv->p2p_probe_req_ie_len = p2p_ielen;
-		}
-#endif /* CONFIG_8723AU_P2P */
-
-		/* buf += p2p_ielen; */
-		/* len -= p2p_ielen; */
-
-#ifdef CONFIG_8723AU_P2P
-		if (rtw_get_wfd_ie(buf, len, NULL, &wfd_ielen)) {
-#ifdef CONFIG_DEBUG_CFG80211
-			DBG_8723A("probe_req_wfd_ielen =%d\n", wfd_ielen);
-#endif
-
-			if (pmlmepriv->wfd_probe_req_ie) {
-				pmlmepriv->wfd_probe_req_ie_len = 0;
-				kfree(pmlmepriv->wfd_probe_req_ie);
-				pmlmepriv->wfd_probe_req_ie = NULL;
-			}
-
-			pmlmepriv->wfd_probe_req_ie =
-				kmalloc(wfd_ielen, GFP_KERNEL);
-			if (pmlmepriv->wfd_probe_req_ie == NULL) {
-				DBG_8723A("%s()-%d: kmalloc() ERROR!\n",
-					  __func__, __LINE__);
-				return -EINVAL;
-
-			}
-			rtw_get_wfd_ie(buf, len, pmlmepriv->wfd_probe_req_ie,
-				       &pmlmepriv->wfd_probe_req_ie_len);
-		}
-#endif /* CONFIG_8723AU_P2P */
-
 	}
 
 	return ret;
@@ -1599,25 +1523,16 @@ static int cfg80211_rtw_scan(struct wiphy *wiphy,
 	struct rtw_ieee80211_channel ch[RTW_CHANNEL_SCAN_AMOUNT];
 	struct rtw_wdev_priv *pwdev_priv = wdev_to_priv(padapter->rtw_wdev);
 	struct cfg80211_ssid *ssids = request->ssids;
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-	int social_channel = 0;
-#endif /* CONFIG_8723AU_P2P */
 	bool need_indicate_scan_done = false;
 
-#ifdef CONFIG_DEBUG_CFG80211
-	DBG_8723A(FUNC_ADPT_FMT "\n", FUNC_ADPT_ARG(padapter));
-#endif
+	DBG_8723A("%s(%s)\n", __func__, padapter->pnetdev->name);
 
 	spin_lock_bh(&pwdev_priv->scan_req_lock);
 	pwdev_priv->scan_request = request;
 	spin_unlock_bh(&pwdev_priv->scan_req_lock);
 
 	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
-
-#ifdef CONFIG_DEBUG_CFG80211
 		DBG_8723A("%s under WIFI_AP_STATE\n", __func__);
-#endif
 		/* need_indicate_scan_done = true; */
 		/* goto check_need_indicate_scan_done; */
 	}
@@ -1626,29 +1541,6 @@ static int cfg80211_rtw_scan(struct wiphy *wiphy,
 		need_indicate_scan_done = true;
 		goto check_need_indicate_scan_done;
 	}
-#ifdef CONFIG_8723AU_P2P
-	if (!memcmp(ssids->ssid, "DIRECT-", 7) &&
-	    rtw_get_p2p_ie23a((u8 *) request->ie, request->ie_len, NULL, NULL)) {
-		if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) {
-			rtw_p2p_enable23a(padapter, P2P_ROLE_DEVICE);
-			wdev_to_priv(padapter->rtw_wdev)->p2p_enabled = true;
-		} else {
-			rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
-#ifdef CONFIG_DEBUG_CFG80211
-			DBG_8723A("%s, role =%d, p2p_state =%d\n", __func__,
-				  rtw_p2p_role(pwdinfo),
-				  rtw_p2p_state(pwdinfo));
-#endif
-		}
-		rtw_p2p_set_state(pwdinfo, P2P_STATE_LISTEN);
-
-		if (request->n_channels == 3 &&
-		    request->channels[0]->hw_value == 1 &&
-		    request->channels[1]->hw_value == 6 &&
-		    request->channels[2]->hw_value == 11)
-			social_channel = 1;
-	}
-#endif /* CONFIG_8723AU_P2P */
 
 	if (request->ie && request->ie_len > 0) {
 		rtw_cfg80211_set_probe_req_wpsp2pie(padapter,
@@ -1662,8 +1554,8 @@ static int cfg80211_rtw_scan(struct wiphy *wiphy,
 		goto check_need_indicate_scan_done;
 	}
 	if (rtw_is_scan_deny(padapter)) {
-		DBG_8723A(FUNC_ADPT_FMT ": scan deny\n",
-			  FUNC_ADPT_ARG(padapter));
+		DBG_8723A("%s(%s): scan deny\n", __func__,
+			  padapter->pnetdev->name);
 		need_indicate_scan_done = true;
 		goto check_need_indicate_scan_done;
 	}
@@ -1674,28 +1566,12 @@ static int cfg80211_rtw_scan(struct wiphy *wiphy,
 		need_indicate_scan_done = true;
 		goto check_need_indicate_scan_done;
 	}
-#ifdef CONFIG_8723AU_P2P
-	if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) &&
-	    !rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE)) {
-		rtw_p2p_set_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH);
-		rtw_free_network_queue23a(padapter, true);
-
-		if (social_channel == 0)
-			rtw_p2p_findphase_ex_set(pwdinfo,
-						 P2P_FINDPHASE_EX_NONE);
-		else
-			rtw_p2p_findphase_ex_set(pwdinfo,
-						 P2P_FINDPHASE_EX_SOCIAL_LAST);
-	}
-#endif /* CONFIG_8723AU_P2P */
 
 	memset(ssid, 0, sizeof(struct cfg80211_ssid) * RTW_SSID_SCAN_AMOUNT);
 	/* parsing request ssids, n_ssids */
 	for (i = 0; i < request->n_ssids && i < RTW_SSID_SCAN_AMOUNT; i++) {
-#ifdef CONFIG_DEBUG_CFG80211
 		DBG_8723A("ssid =%s, len =%d\n", ssids[i].ssid,
 			  ssids[i].ssid_len);
-#endif
 		memcpy(ssid[i].ssid, ssids[i].ssid, ssids[i].ssid_len);
 		ssid[i].ssid_len = ssids[i].ssid_len;
 	}
@@ -1707,11 +1583,9 @@ static int cfg80211_rtw_scan(struct wiphy *wiphy,
 	if (request->n_channels == 1) {
 		for (i = 0; i < request->n_channels &&
 		     i < RTW_CHANNEL_SCAN_AMOUNT; i++) {
-#ifdef CONFIG_DEBUG_CFG80211
-			DBG_8723A(FUNC_ADPT_FMT CHAN_FMT "\n",
-				  FUNC_ADPT_ARG(padapter),
+			DBG_8723A("%s:(%s):" CHAN_FMT "\n",
+				  __func__, padapter->pnetdev->name,
 				  CHAN_ARG(request->channels[i]));
-#endif
 			ch[i].hw_value = request->channels[i]->hw_value;
 			ch[i].flags = request->channels[i]->flags;
 		}
@@ -1747,13 +1621,13 @@ static int cfg80211_rtw_set_wiphy_params(struct wiphy *wiphy, u32 changed)
 static int cfg80211_rtw_join_ibss(struct wiphy *wiphy, struct net_device *ndev,
 				  struct cfg80211_ibss_params *params)
 {
-	DBG_8723A(FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("%s(%s)\n", __func__, ndev->name);
 	return 0;
 }
 
 static int cfg80211_rtw_leave_ibss(struct wiphy *wiphy, struct net_device *ndev)
 {
-	DBG_8723A(FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("%s(%s)\n", __func__, ndev->name);
 	return 0;
 }
 
@@ -1822,30 +1696,30 @@ static int rtw_cfg80211_set_cipher(struct security_priv *psecuritypriv,
 	DBG_8723A("%s, ucast =%d, cipher = 0x%x\n", __func__, ucast, cipher);
 
 	if (!cipher) {
-		*profile_cipher = _NO_PRIVACY_;
+		*profile_cipher = 0;
 		psecuritypriv->ndisencryptstatus = ndisencryptstatus;
 		return 0;
 	}
 
 	switch (cipher) {
 	case IW_AUTH_CIPHER_NONE:
-		*profile_cipher = _NO_PRIVACY_;
+		*profile_cipher = 0;
 		ndisencryptstatus = Ndis802_11EncryptionDisabled;
 		break;
 	case WLAN_CIPHER_SUITE_WEP40:
-		*profile_cipher = _WEP40_;
+		*profile_cipher = WLAN_CIPHER_SUITE_WEP40;
 		ndisencryptstatus = Ndis802_11Encryption1Enabled;
 		break;
 	case WLAN_CIPHER_SUITE_WEP104:
-		*profile_cipher = _WEP104_;
+		*profile_cipher = WLAN_CIPHER_SUITE_WEP104;
 		ndisencryptstatus = Ndis802_11Encryption1Enabled;
 		break;
 	case WLAN_CIPHER_SUITE_TKIP:
-		*profile_cipher = _TKIP_;
+		*profile_cipher = WLAN_CIPHER_SUITE_TKIP;
 		ndisencryptstatus = Ndis802_11Encryption2Enabled;
 		break;
 	case WLAN_CIPHER_SUITE_CCMP:
-		*profile_cipher = _AES_;
+		*profile_cipher = WLAN_CIPHER_SUITE_CCMP;
 		ndisencryptstatus = Ndis802_11Encryption3Enabled;
 		break;
 	default:
@@ -1877,13 +1751,10 @@ static int rtw_cfg80211_set_key_mgt(struct security_priv *psecuritypriv,
 static int rtw_cfg80211_set_wpa_ie(struct rtw_adapter *padapter, const u8 *pie,
 				   size_t ielen)
 {
-	u8 *buf = NULL, *pos = NULL;
+	u8 *buf = NULL;
 	int group_cipher = 0, pairwise_cipher = 0;
 	int ret = 0;
-	int wpa_ielen = 0;
-	int wpa2_ielen = 0;
-	u8 *pwpa, *pwpa2;
-	u8 null_addr[] = { 0, 0, 0, 0, 0, 0 };
+	const u8 *pwpa, *pwpa2;
 	int i;
 
 	if (!pie || !ielen) {
@@ -1896,12 +1767,11 @@ static int rtw_cfg80211_set_wpa_ie(struct rtw_adapter *padapter, const u8 *pie,
 		ret = -EINVAL;
 		goto exit;
 	}
-	buf = kzalloc(ielen, GFP_KERNEL);
+	buf = kmemdup(pie, ielen, GFP_KERNEL);
 	if (buf == NULL) {
 		ret = -ENOMEM;
 		goto exit;
 	}
-	memcpy(buf, pie, ielen);
 
 	/* dump */
 	DBG_8723A("set wpa_ie(length:%zu):\n", ielen);
@@ -1910,7 +1780,6 @@ static int rtw_cfg80211_set_wpa_ie(struct rtw_adapter *padapter, const u8 *pie,
 			  buf[i], buf[i + 1],
 			  buf[i + 2], buf[i + 3], buf[i + 4],
 			  buf[i + 5], buf[i + 6], buf[i + 7]);
-	pos = buf;
 	if (ielen < RSN_HEADER_LEN) {
 		RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_err_,
 			 ("Ie len too short %d\n", (int)ielen));
@@ -1918,33 +1787,35 @@ static int rtw_cfg80211_set_wpa_ie(struct rtw_adapter *padapter, const u8 *pie,
 		goto exit;
 	}
 
-	pwpa = rtw_get_wpa_ie23a(buf, &wpa_ielen, ielen);
-	if (pwpa && wpa_ielen > 0) {
-		if (rtw_parse_wpa_ie23a(pwpa, wpa_ielen + 2, &group_cipher,
-				     &pairwise_cipher, NULL) == _SUCCESS) {
+	pwpa = cfg80211_find_vendor_ie(WLAN_OUI_MICROSOFT,
+				       WLAN_OUI_TYPE_MICROSOFT_WPA,
+				       buf, ielen);
+	if (pwpa && pwpa[1] > 0) {
+		if (rtw_parse_wpa_ie23a(pwpa, pwpa[1] + 2, &group_cipher,
+					&pairwise_cipher, NULL) == _SUCCESS) {
 			padapter->securitypriv.dot11AuthAlgrthm =
 				dot11AuthAlgrthm_8021X;
 			padapter->securitypriv.ndisauthtype =
 				Ndis802_11AuthModeWPAPSK;
-			memcpy(padapter->securitypriv.supplicant_ie, &pwpa[0],
-			       wpa_ielen + 2);
+			memcpy(padapter->securitypriv.supplicant_ie, pwpa,
+			       pwpa[1] + 2);
 
-			DBG_8723A("got wpa_ie, wpa_ielen:%u\n", wpa_ielen);
+			DBG_8723A("got wpa_ie, wpa_ielen:%u\n", pwpa[1]);
 		}
 	}
 
-	pwpa2 = rtw_get_wpa2_ie23a(buf, &wpa2_ielen, ielen);
-	if (pwpa2 && wpa2_ielen > 0) {
-		if (rtw_parse_wpa2_ie23a (pwpa2, wpa2_ielen + 2, &group_cipher,
-				       &pairwise_cipher, NULL) == _SUCCESS) {
+	pwpa2 = cfg80211_find_ie(WLAN_EID_RSN, buf, ielen);
+	if (pwpa2 && pwpa2[1] > 0) {
+		if (rtw_parse_wpa2_ie23a (pwpa2, pwpa2[1] + 2, &group_cipher,
+					  &pairwise_cipher, NULL) == _SUCCESS) {
 			padapter->securitypriv.dot11AuthAlgrthm =
 				dot11AuthAlgrthm_8021X;
 			padapter->securitypriv.ndisauthtype =
 				Ndis802_11AuthModeWPA2PSK;
-			memcpy(padapter->securitypriv.supplicant_ie, &pwpa2[0],
-			       wpa2_ielen + 2);
+			memcpy(padapter->securitypriv.supplicant_ie, pwpa2,
+			       pwpa2[1] + 2);
 
-			DBG_8723A("got wpa2_ie, wpa2_ielen:%u\n", wpa2_ielen);
+			DBG_8723A("got wpa2_ie, wpa2_ielen:%u\n", pwpa2[1]);
 		}
 	}
 
@@ -1957,27 +1828,27 @@ static int rtw_cfg80211_set_wpa_ie(struct rtw_adapter *padapter, const u8 *pie,
 
 	switch (group_cipher) {
 	case WPA_CIPHER_NONE:
-		padapter->securitypriv.dot118021XGrpPrivacy = _NO_PRIVACY_;
+		padapter->securitypriv.dot118021XGrpPrivacy = 0;
 		padapter->securitypriv.ndisencryptstatus =
 			Ndis802_11EncryptionDisabled;
 		break;
 	case WPA_CIPHER_WEP40:
-		padapter->securitypriv.dot118021XGrpPrivacy = _WEP40_;
+		padapter->securitypriv.dot118021XGrpPrivacy = WLAN_CIPHER_SUITE_WEP40;
 		padapter->securitypriv.ndisencryptstatus =
 			Ndis802_11Encryption1Enabled;
 		break;
 	case WPA_CIPHER_TKIP:
-		padapter->securitypriv.dot118021XGrpPrivacy = _TKIP_;
+		padapter->securitypriv.dot118021XGrpPrivacy = WLAN_CIPHER_SUITE_TKIP;
 		padapter->securitypriv.ndisencryptstatus =
 			Ndis802_11Encryption2Enabled;
 		break;
 	case WPA_CIPHER_CCMP:
-		padapter->securitypriv.dot118021XGrpPrivacy = _AES_;
+		padapter->securitypriv.dot118021XGrpPrivacy = WLAN_CIPHER_SUITE_CCMP;
 		padapter->securitypriv.ndisencryptstatus =
 			Ndis802_11Encryption3Enabled;
 		break;
 	case WPA_CIPHER_WEP104:
-		padapter->securitypriv.dot118021XGrpPrivacy = _WEP104_;
+		padapter->securitypriv.dot118021XGrpPrivacy = WLAN_CIPHER_SUITE_WEP104;
 		padapter->securitypriv.ndisencryptstatus =
 			Ndis802_11Encryption1Enabled;
 		break;
@@ -1985,27 +1856,27 @@ static int rtw_cfg80211_set_wpa_ie(struct rtw_adapter *padapter, const u8 *pie,
 
 	switch (pairwise_cipher) {
 	case WPA_CIPHER_NONE:
-		padapter->securitypriv.dot11PrivacyAlgrthm = _NO_PRIVACY_;
+		padapter->securitypriv.dot11PrivacyAlgrthm = 0;
 		padapter->securitypriv.ndisencryptstatus =
 			Ndis802_11EncryptionDisabled;
 		break;
 	case WPA_CIPHER_WEP40:
-		padapter->securitypriv.dot11PrivacyAlgrthm = _WEP40_;
+		padapter->securitypriv.dot11PrivacyAlgrthm = WLAN_CIPHER_SUITE_WEP40;
 		padapter->securitypriv.ndisencryptstatus =
 			Ndis802_11Encryption1Enabled;
 		break;
 	case WPA_CIPHER_TKIP:
-		padapter->securitypriv.dot11PrivacyAlgrthm = _TKIP_;
+		padapter->securitypriv.dot11PrivacyAlgrthm = WLAN_CIPHER_SUITE_TKIP;
 		padapter->securitypriv.ndisencryptstatus =
 			Ndis802_11Encryption2Enabled;
 		break;
 	case WPA_CIPHER_CCMP:
-		padapter->securitypriv.dot11PrivacyAlgrthm = _AES_;
+		padapter->securitypriv.dot11PrivacyAlgrthm = WLAN_CIPHER_SUITE_CCMP;
 		padapter->securitypriv.ndisencryptstatus =
 			Ndis802_11Encryption3Enabled;
 		break;
 	case WPA_CIPHER_WEP104:
-		padapter->securitypriv.dot11PrivacyAlgrthm = _WEP104_;
+		padapter->securitypriv.dot11PrivacyAlgrthm = WLAN_CIPHER_SUITE_WEP104;
 		padapter->securitypriv.ndisencryptstatus =
 			Ndis802_11Encryption1Enabled;
 		break;
@@ -2029,75 +1900,14 @@ static int rtw_cfg80211_set_wpa_ie(struct rtw_adapter *padapter, const u8 *pie,
 		}
 	}
 
-#ifdef CONFIG_8723AU_P2P
-	{			/* check p2p_ie for assoc req; */
-		uint p2p_ielen = 0;
-		u8 *p2p_ie;
-		struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-
-		p2p_ie = rtw_get_p2p_ie23a(buf, ielen, NULL, &p2p_ielen);
-		if (p2p_ie) {
-#ifdef CONFIG_DEBUG_CFG80211
-			DBG_8723A("%s p2p_assoc_req_ielen =%d\n", __func__,
-				  p2p_ielen);
-#endif
-
-			if (pmlmepriv->p2p_assoc_req_ie) {
-				pmlmepriv->p2p_assoc_req_ie_len = 0;
-				kfree(pmlmepriv->p2p_assoc_req_ie);
-				pmlmepriv->p2p_assoc_req_ie = NULL;
-			}
-
-			pmlmepriv->p2p_assoc_req_ie =
-				kmalloc(p2p_ielen, GFP_KERNEL);
-			if (pmlmepriv->p2p_assoc_req_ie == NULL) {
-				DBG_8723A("%s()-%d: kmalloc() ERROR!\n",
-					  __func__, __LINE__);
-				goto exit;
-			}
-			memcpy(pmlmepriv->p2p_assoc_req_ie, p2p_ie, p2p_ielen);
-			pmlmepriv->p2p_assoc_req_ie_len = p2p_ielen;
-		}
-	}
-#endif /* CONFIG_8723AU_P2P */
-
-#ifdef CONFIG_8723AU_P2P
-	{			/* check wfd_ie for assoc req; */
-		uint wfd_ielen = 0;
-		struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-
-		if (rtw_get_wfd_ie(buf, ielen, NULL, &wfd_ielen)) {
-#ifdef CONFIG_DEBUG_CFG80211
-			DBG_8723A("%s wfd_assoc_req_ielen =%d\n", __func__,
-				  wfd_ielen);
-#endif
-
-			if (pmlmepriv->wfd_assoc_req_ie) {
-				pmlmepriv->wfd_assoc_req_ie_len = 0;
-				kfree(pmlmepriv->wfd_assoc_req_ie);
-				pmlmepriv->wfd_assoc_req_ie = NULL;
-			}
-
-			pmlmepriv->wfd_assoc_req_ie =
-				kmalloc(wfd_ielen, GFP_KERNEL);
-			if (pmlmepriv->wfd_assoc_req_ie == NULL) {
-				DBG_8723A("%s()-%d: kmalloc() ERROR!\n",
-					  __func__, __LINE__);
-				goto exit;
-			}
-			rtw_get_wfd_ie(buf, ielen, pmlmepriv->wfd_assoc_req_ie,
-				       &pmlmepriv->wfd_assoc_req_ie_len);
-		}
-	}
-#endif /* CONFIG_8723AU_P2P */
-
 	/* TKIP and AES disallow multicast packets until installing group key */
-	if (padapter->securitypriv.dot11PrivacyAlgrthm == _TKIP_ ||
-	    padapter->securitypriv.dot11PrivacyAlgrthm == _TKIP_WTMIC_ ||
-	    padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)
+	if (padapter->securitypriv.dot11PrivacyAlgrthm ==
+	    WLAN_CIPHER_SUITE_TKIP ||
+	    padapter->securitypriv.dot11PrivacyAlgrthm ==
+	    WLAN_CIPHER_SUITE_CCMP)
 		/* WPS open need to enable multicast */
 		/* check_fwstate(&padapter->mlmepriv, WIFI_UNDER_WPS) == true)*/
-		rtw_hal_set_hwreg23a(padapter, HW_VAR_OFF_RCR_AM, null_addr);
+		rtl8723a_off_rcr_am(padapter);
 
 	RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_info_,
 		 ("rtw_set_wpa_ie: pairwise_cipher = 0x%08x padapter->"
@@ -2113,6 +1923,70 @@ exit:
 	return ret;
 }
 
+static int rtw_cfg80211_add_wep(struct rtw_adapter *padapter,
+				struct rtw_wep_key *wep, u8 keyid)
+{
+	int res;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+
+	if (keyid >= NUM_WEP_KEYS) {
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+			 ("%s:keyid>4 =>fail\n", __func__));
+		res = _FAIL;
+		goto exit;
+	}
+
+	switch (wep->keylen) {
+	case 5:
+		psecuritypriv->dot11PrivacyAlgrthm = WLAN_CIPHER_SUITE_WEP40;
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
+			 ("%s:wep->KeyLength = 5\n", __func__));
+		break;
+	case 13:
+		psecuritypriv->dot11PrivacyAlgrthm = WLAN_CIPHER_SUITE_WEP104;
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
+			 ("%s:wep->KeyLength = 13\n", __func__));
+		break;
+	default:
+		psecuritypriv->dot11PrivacyAlgrthm = 0;
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
+			 ("%s:wep->KeyLength!= 5 or 13\n", __func__));
+		res = _FAIL;
+		goto exit;
+	}
+
+	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
+		 ("%s:before memcpy, wep->KeyLength = 0x%x keyid =%x\n",
+		  __func__, wep->keylen, keyid));
+
+	memcpy(&psecuritypriv->wep_key[keyid], wep, sizeof(struct rtw_wep_key));
+
+	psecuritypriv->dot11PrivacyKeyIndex = keyid;
+
+	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
+		 ("%s:security key material : "
+		  "%x %x %x %x %x %x %x %x %x %x %x %x %x\n", __func__,
+		  psecuritypriv->wep_key[keyid].key[0],
+		  psecuritypriv->wep_key[keyid].key[1],
+		  psecuritypriv->wep_key[keyid].key[2],
+		  psecuritypriv->wep_key[keyid].key[3],
+		  psecuritypriv->wep_key[keyid].key[4],
+		  psecuritypriv->wep_key[keyid].key[5],
+		  psecuritypriv->wep_key[keyid].key[6],
+		  psecuritypriv->wep_key[keyid].key[7],
+		  psecuritypriv->wep_key[keyid].key[8],
+		  psecuritypriv->wep_key[keyid].key[9],
+		  psecuritypriv->wep_key[keyid].key[10],
+		  psecuritypriv->wep_key[keyid].key[11],
+		  psecuritypriv->wep_key[keyid].key[12]));
+
+	res = rtw_set_key23a(padapter, psecuritypriv, keyid, 1);
+
+exit:
+
+	return res;
+}
+
 static int cfg80211_rtw_connect(struct wiphy *wiphy, struct net_device *ndev,
 				struct cfg80211_connect_params *sme)
 {
@@ -2133,16 +2007,10 @@ static int cfg80211_rtw_connect(struct wiphy *wiphy, struct net_device *ndev,
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 	struct rtw_queue *queue = &pmlmepriv->scanned_queue;
 
-	DBG_8723A("=>" FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("=>" "%s(%s)\n", __func__, ndev->name);
 	DBG_8723A("privacy =%d, key =%p, key_len =%d, key_idx =%d\n",
 		  sme->privacy, sme->key, sme->key_len, sme->key_idx);
 
-	if (wdev_to_priv(padapter->rtw_wdev)->block) {
-		ret = -EBUSY;
-		DBG_8723A("%s wdev_priv.block is set\n", __func__);
-		goto exit;
-	}
-
 	if (_FAIL == rtw_pwr_wakeup(padapter)) {
 		ret = -EPERM;
 		goto exit;
@@ -2193,8 +2061,8 @@ static int cfg80211_rtw_connect(struct wiphy *wiphy, struct net_device *ndev,
 		dst_bssid = pnetwork->network.MacAddress;
 
 		if (sme->bssid) {
-			if (memcmp(pnetwork->network.MacAddress,
-				   sme->bssid, ETH_ALEN))
+			if (!ether_addr_equal(pnetwork->network.MacAddress,
+					      sme->bssid))
 				continue;
 		}
 
@@ -2208,7 +2076,7 @@ static int cfg80211_rtw_connect(struct wiphy *wiphy, struct net_device *ndev,
 		if (sme->bssid) {
 			src_bssid = sme->bssid;
 
-			if ((!memcmp(dst_bssid, src_bssid, ETH_ALEN))) {
+			if (ether_addr_equal(dst_bssid, src_bssid)) {
 				DBG_8723A("matched by bssid\n");
 
 				ndis_ssid.ssid_len =
@@ -2242,15 +2110,15 @@ static int cfg80211_rtw_connect(struct wiphy *wiphy, struct net_device *ndev,
 		goto exit;
 	}
 
-	if (rtw_set_802_11_infrastructure_mode23a
-	    (padapter, pnetwork->network.InfrastructureMode) == false) {
+	if (cfg80211_infrastructure_mode(
+		    padapter, pnetwork->network.ifmode) != _SUCCESS) {
 		ret = -EPERM;
 		goto exit;
 	}
 
 	psecuritypriv->ndisencryptstatus = Ndis802_11EncryptionDisabled;
-	psecuritypriv->dot11PrivacyAlgrthm = _NO_PRIVACY_;
-	psecuritypriv->dot118021XGrpPrivacy = _NO_PRIVACY_;
+	psecuritypriv->dot11PrivacyAlgrthm = 0;
+	psecuritypriv->dot118021XGrpPrivacy = 0;
 	psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_Open;
 	psecuritypriv->ndisauthtype = Ndis802_11AuthModeOpen;
 
@@ -2283,58 +2151,42 @@ static int cfg80211_rtw_connect(struct wiphy *wiphy, struct net_device *ndev,
 	if ((psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_Shared ||
 	     psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_Auto) &&
 	    sme->key) {
-		u32 wep_key_idx, wep_key_len, wep_total_len;
-		struct ndis_802_11_wep *pwep = NULL;
+		struct rtw_wep_key wep_key;
+		u8 wep_key_idx, wep_key_len;
 		DBG_8723A("%s(): Shared/Auto WEP\n", __func__);
 
 		wep_key_idx = sme->key_idx;
 		wep_key_len = sme->key_len;
 
-		if (sme->key_idx > WEP_KEYS) {
+		if (wep_key_idx > WEP_KEYS || !wep_key_len ||
+		    wep_key_len > WLAN_KEY_LEN_WEP104) {
 			ret = -EINVAL;
 			goto exit;
 		}
 
-		if (wep_key_len > 0) {
-			wep_key_len = wep_key_len <= 5 ? 5 : 13;
-			wep_total_len =
-				wep_key_len +
-				offsetof(struct ndis_802_11_wep, KeyMaterial);
-			pwep = (struct ndis_802_11_wep *)kmalloc(wep_total_len,
-								 GFP_KERNEL);
-			if (pwep == NULL) {
-				DBG_8723A(" wpa_set_encryption: pwep "
-					  "allocate fail !!!\n");
-				ret = -ENOMEM;
-				goto exit;
-			}
+		wep_key_len = wep_key_len <= 5 ? 5 : 13;
 
-			memset(pwep, 0, wep_total_len);
+		memset(&wep_key, 0, sizeof(struct rtw_wep_key));
 
-			pwep->KeyLength = wep_key_len;
-			pwep->Length = wep_total_len;
+		wep_key.keylen = wep_key_len;
 
-			if (wep_key_len == 13) {
-				padapter->securitypriv.dot11PrivacyAlgrthm =
-				    _WEP104_;
-				padapter->securitypriv.dot118021XGrpPrivacy =
-				    _WEP104_;
-			}
+		if (wep_key_len == 13) {
+			padapter->securitypriv.dot11PrivacyAlgrthm =
+				WLAN_CIPHER_SUITE_WEP104;
+			padapter->securitypriv.dot118021XGrpPrivacy =
+				WLAN_CIPHER_SUITE_WEP104;
 		} else {
-			ret = -EINVAL;
-			goto exit;
+			padapter->securitypriv.dot11PrivacyAlgrthm =
+				WLAN_CIPHER_SUITE_WEP40;
+			padapter->securitypriv.dot118021XGrpPrivacy =
+				WLAN_CIPHER_SUITE_WEP40;
 		}
 
-		pwep->KeyIndex = wep_key_idx;
-		pwep->KeyIndex |= 0x80000000;
+		memcpy(wep_key.key, (void *)sme->key, wep_key.keylen);
 
-		memcpy(pwep->KeyMaterial, (void *)sme->key, pwep->KeyLength);
-
-		if (rtw_set_802_11_add_wep23a(padapter, pwep) == (u8) _FAIL) {
+		if (rtw_cfg80211_add_wep(padapter, &wep_key, wep_key_idx) !=
+		    _SUCCESS)
 			ret = -EOPNOTSUPP;
-		}
-
-		kfree(pwep);
 
 		if (ret < 0)
 			goto exit;
@@ -2380,7 +2232,7 @@ static int cfg80211_rtw_disconnect(struct wiphy *wiphy, struct net_device *ndev,
 {
 	struct rtw_adapter *padapter = wiphy_to_adapter(wiphy);
 
-	DBG_8723A(FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("%s(%s)\n", __func__, ndev->name);
 
 	rtw_set_roaming(padapter, 0);
 
@@ -2430,8 +2282,8 @@ static int cfg80211_rtw_set_power_mgmt(struct wiphy *wiphy,
 	struct rtw_adapter *padapter = wiphy_to_adapter(wiphy);
 	struct rtw_wdev_priv *rtw_wdev_priv = wdev_to_priv(padapter->rtw_wdev);
 
-	DBG_8723A(FUNC_NDEV_FMT " enabled:%u, timeout:%d\n",
-		  FUNC_NDEV_ARG(ndev), enabled, timeout);
+	DBG_8723A("%s(%s): enabled:%u, timeout:%d\n",
+		  __func__, ndev->name, enabled, timeout);
 
 	rtw_wdev_priv->power_mgmt = enabled;
 
@@ -2448,25 +2300,22 @@ static int cfg80211_rtw_set_pmksa(struct wiphy *wiphy,
 	u8 index, blInserted = false;
 	struct rtw_adapter *padapter = wiphy_to_adapter(wiphy);
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
-	u8 strZeroMacAddress[ETH_ALEN] = { 0x00 };
 
-	DBG_8723A(FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(netdev));
+	DBG_8723A("%s(%s)\n", __func__, netdev->name);
 
-	if (!memcmp(pmksa->bssid, strZeroMacAddress, ETH_ALEN)) {
+	if (is_zero_ether_addr(pmksa->bssid))
 		return -EINVAL;
-	}
 
 	blInserted = false;
 
 	/* overwrite PMKID */
 	for (index = 0; index < NUM_PMKID_CACHE; index++) {
-		if (!memcmp(psecuritypriv->PMKIDList[index].Bssid,
-			    pmksa->bssid, ETH_ALEN)) {
+		if (ether_addr_equal(psecuritypriv->PMKIDList[index].Bssid,
+				     pmksa->bssid)) {
 			/* BSSID is matched, the same AP => rewrite with
 			   new PMKID. */
-			DBG_8723A(FUNC_NDEV_FMT
-				  " BSSID exists in the PMKList.\n",
-				  FUNC_NDEV_ARG(netdev));
+			DBG_8723A("%s(%s):  BSSID exists in the PMKList.\n",
+				  __func__, netdev->name);
 
 			memcpy(psecuritypriv->PMKIDList[index].PMKID,
 			       pmksa->pmkid, WLAN_PMKID_LEN);
@@ -2479,12 +2328,12 @@ static int cfg80211_rtw_set_pmksa(struct wiphy *wiphy,
 
 	if (!blInserted) {
 		/*  Find a new entry */
-		DBG_8723A(FUNC_NDEV_FMT
-			  " Use the new entry index = %d for this PMKID.\n",
-			  FUNC_NDEV_ARG(netdev), psecuritypriv->PMKIDIndex);
+		DBG_8723A("%s(%s): Use new entry index = %d for this PMKID\n",
+			  __func__, netdev->name, psecuritypriv->PMKIDIndex);
 
-		memcpy(psecuritypriv->PMKIDList[psecuritypriv->PMKIDIndex].
-		       Bssid, pmksa->bssid, ETH_ALEN);
+		ether_addr_copy(
+			psecuritypriv->PMKIDList[psecuritypriv->PMKIDIndex].
+			Bssid, pmksa->bssid);
 		memcpy(psecuritypriv->PMKIDList[psecuritypriv->PMKIDIndex].
 		       PMKID, pmksa->pmkid, WLAN_PMKID_LEN);
 
@@ -2507,14 +2356,14 @@ static int cfg80211_rtw_del_pmksa(struct wiphy *wiphy,
 	struct rtw_adapter *padapter = wiphy_to_adapter(wiphy);
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 
-	DBG_8723A(FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(netdev));
+	DBG_8723A("%s(%s)\n", __func__, netdev->name);
 
 	for (index = 0; index < NUM_PMKID_CACHE; index++) {
-		if (!memcmp(psecuritypriv->PMKIDList[index].Bssid,
-			    pmksa->bssid, ETH_ALEN)) {
-			/* BSSID is matched, the same AP => Remove this PMKID information and reset it. */
-			memset(psecuritypriv->PMKIDList[index].Bssid, 0x00,
-			       ETH_ALEN);
+		if (ether_addr_equal(psecuritypriv->PMKIDList[index].Bssid,
+				     pmksa->bssid)) {
+			/* BSSID is matched, the same AP => Remove this PMKID
+			   information and reset it. */
+			eth_zero_addr(psecuritypriv->PMKIDList[index].Bssid);
 			memset(psecuritypriv->PMKIDList[index].PMKID, 0x00,
 			       WLAN_PMKID_LEN);
 			psecuritypriv->PMKIDList[index].bUsed = false;
@@ -2524,8 +2373,8 @@ static int cfg80211_rtw_del_pmksa(struct wiphy *wiphy,
 	}
 
 	if (false == bMatched) {
-		DBG_8723A(FUNC_NDEV_FMT " do not have matched BSSID\n",
-			  FUNC_NDEV_ARG(netdev));
+		DBG_8723A("%s(%s): do not have matched BSSID\n", __func__,
+			  netdev->name);
 		return -EINVAL;
 	}
 
@@ -2538,7 +2387,7 @@ static int cfg80211_rtw_flush_pmksa(struct wiphy *wiphy,
 	struct rtw_adapter *padapter = wiphy_to_adapter(wiphy);
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 
-	DBG_8723A(FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(netdev));
+	DBG_8723A("%s(%s)\n", __func__, netdev->name);
 
 	memset(&psecuritypriv->PMKIDList[0], 0x00,
 	       sizeof(struct rt_pmkid_list) * NUM_PMKID_CACHE);
@@ -2583,8 +2432,8 @@ void rtw_cfg80211_indicate_sta_assoc(struct rtw_adapter *padapter,
 		freq = ieee80211_channel_to_frequency(channel,
 						      IEEE80211_BAND_5GHZ);
 
-	rtw_cfg80211_rx_mgmt(padapter, freq, 0, pmgmt_frame, frame_len,
-			     GFP_ATOMIC);
+	cfg80211_rx_mgmt(padapter->rtw_wdev, freq, 0, pmgmt_frame, frame_len,
+			 0, GFP_ATOMIC);
 #endif /* defined(RTW_USE_CFG80211_STA_EVENT) */
 }
 
@@ -2597,14 +2446,15 @@ void rtw_cfg80211_indicate_sta_disassoc(struct rtw_adapter *padapter,
 	u8 *pmgmt_frame;
 	uint frame_len;
 	struct ieee80211_hdr *pwlanhdr;
-	unsigned short *fctrl;
-	u8 mgmt_buf[128] = { 0 };
+	u8 mgmt_buf[128];
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 	struct net_device *ndev = padapter->pnetdev;
 
 	DBG_8723A("%s(padapter =%p,%s)\n", __func__, padapter, ndev->name);
 
+	memset(mgmt_buf, 0, 128);
+
 #if defined(RTW_USE_CFG80211_STA_EVENT)
 	cfg80211_del_sta(ndev, da, GFP_ATOMIC);
 #else /* defined(RTW_USE_CFG80211_STA_EVENT) */
@@ -2619,16 +2469,16 @@ void rtw_cfg80211_indicate_sta_disassoc(struct rtw_adapter *padapter,
 	pmgmt_frame = mgmt_buf;
 	pwlanhdr = (struct ieee80211_hdr *)pmgmt_frame;
 
-	fctrl = &pwlanhdr->frame_control;
-	*(fctrl) = 0;
+	pwlanhdr->frame_control =
+		cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_DEAUTH);
 
-	memcpy(pwlanhdr->addr1, myid(&padapter->eeprompriv), ETH_ALEN);
-	memcpy(pwlanhdr->addr2, da, ETH_ALEN);
-	memcpy(pwlanhdr->addr3, get_my_bssid23a(&pmlmeinfo->network), ETH_ALEN);
+	ether_addr_copy(pwlanhdr->addr1, myid(&padapter->eeprompriv));
+	ether_addr_copy(pwlanhdr->addr2, da);
+	ether_addr_copy(pwlanhdr->addr3, get_my_bssid23a(&pmlmeinfo->network));
 
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
+	pwlanhdr->seq_ctrl =
+		cpu_to_le16(IEEE80211_SN_TO_SEQ(pmlmeext->mgnt_seq));
 	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pmgmt_frame, WIFI_DEAUTH);
 
 	pmgmt_frame += sizeof(struct ieee80211_hdr_3addr);
 	frame_len = sizeof(struct ieee80211_hdr_3addr);
@@ -2638,8 +2488,8 @@ void rtw_cfg80211_indicate_sta_disassoc(struct rtw_adapter *padapter,
 				       WLAN_REASON_PREV_AUTH_NOT_VALID,
 				       (unsigned char *)&reason, &frame_len);
 
-	rtw_cfg80211_rx_mgmt(padapter, freq, 0, mgmt_buf, frame_len,
-			     GFP_ATOMIC);
+	cfg80211_rx_mgmt(padapter->rtw_wdev, freq, 0, mgmt_buf, frame_len,
+			 0, GFP_ATOMIC);
 #endif /* defined(RTW_USE_CFG80211_STA_EVENT) */
 }
 
@@ -2676,7 +2526,7 @@ static int rtw_cfg80211_monitor_if_xmit_entry(struct sk_buff *skb,
 	struct ieee80211_radiotap_header *rtap_hdr;
 	struct rtw_adapter *padapter = netdev_priv(ndev);
 
-	DBG_8723A(FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("%s(%s)\n", __func__, ndev->name);
 
 	if (unlikely(skb->len < sizeof(struct ieee80211_radiotap_header)))
 		goto fail;
@@ -2718,8 +2568,8 @@ static int rtw_cfg80211_monitor_if_xmit_entry(struct sk_buff *skb,
 		skb_pull(skb, dot11_hdr_len + qos_len + snap_len -
 			 ETH_ALEN * 2);
 		pdata = (unsigned char *)skb->data;
-		memcpy(pdata, dst_mac_addr, ETH_ALEN);
-		memcpy(pdata + ETH_ALEN, src_mac_addr, ETH_ALEN);
+		ether_addr_copy(pdata, dst_mac_addr);
+		ether_addr_copy(pdata + ETH_ALEN, src_mac_addr);
 
 		DBG_8723A("should be eapol packet\n");
 
@@ -2729,6 +2579,7 @@ static int rtw_cfg80211_monitor_if_xmit_entry(struct sk_buff *skb,
 		return ret;
 
 	} else if (ieee80211_is_action(dot11_hdr->frame_control)) {
+		struct ieee80211_mgmt *mgmt;
 		/* only for action frames */
 		struct xmit_frame *pmgntframe;
 		struct pkt_attrib *pattrib;
@@ -2739,33 +2590,19 @@ static int rtw_cfg80211_monitor_if_xmit_entry(struct sk_buff *skb,
 		struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 		u32 len = skb->len;
 		u8 category, action;
-#ifdef CONFIG_8723AU_P2P
-		int type = -1;
-#endif
 
-		if (rtw_action_frame_parse23a(skb->data, len, &category,
-					   &action) == false) {
-			DBG_8723A(FUNC_NDEV_FMT " frame_control:0x%x\n",
-				  FUNC_NDEV_ARG(ndev),
-				  le16_to_cpu(dot11_hdr->frame_control));
-			goto fail;
-		}
+		mgmt = (struct ieee80211_mgmt *)dot11_hdr;
 
-		DBG_8723A("RTW_Tx:da =" MAC_FMT " via " FUNC_NDEV_FMT "\n",
-			  MAC_ARG(dot11_hdr->addr1), FUNC_NDEV_ARG(ndev));
-#ifdef CONFIG_8723AU_P2P
-		type = rtw_p2p_check_frames(padapter, skb->data, len, true);
-		if (type >= 0)
-			goto dump;
-#endif
-		if (category == WLAN_CATEGORY_PUBLIC)
+		DBG_8723A("RTW_Tx:da =" MAC_FMT " via %s(%s)\n",
+			  MAC_ARG(mgmt->da), __func__, ndev->name);
+		category = mgmt->u.action.category;
+		action = mgmt->u.action.u.wme_action.action_code;
+		if (mgmt->u.action.category == WLAN_CATEGORY_PUBLIC)
 			DBG_8723A("RTW_Tx:%s\n", action_public_str23a(action));
 		else
 			DBG_8723A("RTW_Tx:category(%u), action(%u)\n", category,
 				  action);
-#ifdef CONFIG_8723AU_P2P
-dump:
-#endif
+
 		/* starting alloc mgmt frame to dump it */
 		pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
 		if (pmgntframe == NULL)
@@ -2781,16 +2618,6 @@ dump:
 		pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 
 		memcpy(pframe, skb->data, len);
-#ifdef CONFIG_8723AU_P2P
-		if (type >= 0) {
-			struct wifi_display_info *pwfd_info;
-
-			pwfd_info = padapter->wdinfo.wfd_info;
-
-			if (pwfd_info->wfd_enable)
-				rtw_append_wfd_ie(padapter, pframe, &len);
-		}
-#endif /*  CONFIG_8723AU_P2P */
 		pattrib->pktlen = len;
 
 		/* update seq number */
@@ -2836,24 +2663,23 @@ static int rtw_cfg80211_add_monitor_if(struct rtw_adapter *padapter, char *name,
 	struct rtw_wdev_priv *pwdev_priv = wdev_to_priv(padapter->rtw_wdev);
 
 	if (!name) {
-		DBG_8723A(FUNC_ADPT_FMT " without specific name\n",
-			  FUNC_ADPT_ARG(padapter));
+		DBG_8723A("%s(%s): without specific name\n",
+			  __func__, padapter->pnetdev->name);
 		ret = -EINVAL;
 		goto out;
 	}
 
 	if (pwdev_priv->pmon_ndev) {
-		DBG_8723A(FUNC_ADPT_FMT " monitor interface exist: " NDEV_FMT
-			  "\n", FUNC_ADPT_ARG(padapter),
-			  NDEV_ARG(pwdev_priv->pmon_ndev));
+		DBG_8723A("%s(%s): monitor interface exist: %s\n", __func__,
+			  padapter->pnetdev->name, pwdev_priv->pmon_ndev->name);
 		ret = -EBUSY;
 		goto out;
 	}
 
 	mon_ndev = alloc_etherdev(sizeof(struct rtw_adapter));
 	if (!mon_ndev) {
-		DBG_8723A(FUNC_ADPT_FMT " allocate ndev fail\n",
-			  FUNC_ADPT_ARG(padapter));
+		DBG_8723A("%s(%s): allocate ndev fail\n", __func__,
+			  padapter->pnetdev->name);
 		ret = -ENOMEM;
 		goto out;
 	}
@@ -2868,8 +2694,8 @@ static int rtw_cfg80211_add_monitor_if(struct rtw_adapter *padapter, char *name,
 	/*  wdev */
 	mon_wdev = kzalloc(sizeof(struct wireless_dev), GFP_KERNEL);
 	if (!mon_wdev) {
-		DBG_8723A(FUNC_ADPT_FMT " allocate mon_wdev fail\n",
-			  FUNC_ADPT_ARG(padapter));
+		DBG_8723A("%s(%s): allocate mon_wdev fail\n", __func__,
+			  padapter->pnetdev->name);
 		ret = -ENOMEM;
 		goto out;
 	}
@@ -2910,8 +2736,8 @@ cfg80211_rtw_add_virtual_intf(struct wiphy *wiphy, const char *name,
 	struct net_device *ndev = NULL;
 	struct rtw_adapter *padapter = wiphy_to_adapter(wiphy);
 
-	DBG_8723A(FUNC_ADPT_FMT " wiphy:%s, name:%s, type:%d\n",
-		  FUNC_ADPT_ARG(padapter), wiphy_name(wiphy), name, type);
+	DBG_8723A("%s(%s): wiphy:%s, name:%s, type:%d\n", __func__,
+		  padapter->pnetdev->name, wiphy_name(wiphy), name, type);
 
 	switch (type) {
 	case NL80211_IFTYPE_ADHOC:
@@ -2940,7 +2766,8 @@ cfg80211_rtw_add_virtual_intf(struct wiphy *wiphy, const char *name,
 		break;
 	}
 
-	DBG_8723A(FUNC_ADPT_FMT " ndev:%p, ret:%d\n", FUNC_ADPT_ARG(padapter),
+	DBG_8723A("%s(%s): ndev:%p, ret:%d\n", __func__,
+		  padapter->pnetdev->name,
 		  ndev, ret);
 
 	return ndev ? ndev->ieee80211_ptr : ERR_PTR(ret);
@@ -2962,8 +2789,8 @@ static int cfg80211_rtw_del_virtual_intf(struct wiphy *wiphy,
 	if (ndev == pwdev_priv->pmon_ndev) {
 		pwdev_priv->pmon_ndev = NULL;
 		pwdev_priv->ifname_mon[0] = '\0';
-		DBG_8723A(FUNC_NDEV_FMT " remove monitor interface\n",
-			  FUNC_NDEV_ARG(ndev));
+		DBG_8723A("%s(%s): remove monitor interface\n",
+			  __func__, ndev->name);
 	}
 
 exit:
@@ -2976,10 +2803,6 @@ static int rtw_add_beacon(struct rtw_adapter *adapter, const u8 *head,
 	int ret = 0;
 	u8 *pbuf = NULL;
 	uint len, wps_ielen = 0;
-#ifdef CONFIG_8723AU_P2P
-	uint p2p_ielen = 0;
-	u8 got_p2p_ie = false;
-#endif
 	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
 	/* struct sta_priv *pstapriv = &padapter->stapriv; */
 
@@ -3007,55 +2830,14 @@ static int rtw_add_beacon(struct rtw_adapter *adapter, const u8 *head,
 	     &wps_ielen))
 		DBG_8723A("add bcn, wps_ielen =%d\n", wps_ielen);
 
-#ifdef CONFIG_8723AU_P2P
-	/* check p2p ie if inclued */
-	if (rtw_get_p2p_ie23a
-	    (pbuf + _FIXED_IE_LENGTH_, len - _FIXED_IE_LENGTH_, NULL,
-	     &p2p_ielen)) {
-		DBG_8723A("got p2p_ie, len =%d\n", p2p_ielen);
-		got_p2p_ie = true;
-	}
-#endif
-
 	/* pbss_network->IEs will not include p2p_ie, wfd ie */
-	rtw_ies_remove_ie23a(pbuf, &len, _BEACON_IE_OFFSET_, _VENDOR_SPECIFIC_IE_,
-			  P2P_OUI23A, 4);
-	rtw_ies_remove_ie23a(pbuf, &len, _BEACON_IE_OFFSET_, _VENDOR_SPECIFIC_IE_,
-			  WFD_OUI23A, 4);
+	rtw_ies_remove_ie23a(pbuf, &len, _BEACON_IE_OFFSET_,
+			     WLAN_EID_VENDOR_SPECIFIC, P2P_OUI23A, 4);
+	rtw_ies_remove_ie23a(pbuf, &len, _BEACON_IE_OFFSET_,
+			     WLAN_EID_VENDOR_SPECIFIC, WFD_OUI23A, 4);
 
 	if (rtw_check_beacon_data23a(adapter, pbuf, len) == _SUCCESS) {
-#ifdef CONFIG_8723AU_P2P
-		/* check p2p if enable */
-		if (got_p2p_ie == true) {
-			struct mlme_ext_priv *pmlmeext = &adapter->mlmeextpriv;
-			struct wifidirect_info *pwdinfo = &adapter->wdinfo;
-
-			if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) {
-				DBG_8723A("Enable P2P function for the first "
-					  "time\n");
-				rtw_p2p_enable23a(adapter, P2P_ROLE_GO);
-				wdev_to_priv(adapter->rtw_wdev)->p2p_enabled =
-					true;
-			} else {
-				del_timer_sync(&pwdinfo->find_phase_timer);
-				del_timer_sync(&pwdinfo->
-					       restore_p2p_state_timer);
-				del_timer_sync(&pwdinfo->pre_tx_scan_timer);
-
-				DBG_8723A("enter GO Mode, p2p_ielen =%d\n",
-					  p2p_ielen);
-
-				rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO);
-				rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK);
-				pwdinfo->intent = 15;
-			}
-
-			pwdinfo->operating_channel = pmlmeext->cur_channel;
-		}
-#endif /* CONFIG_8723AU_P2P */
-
 		ret = 0;
-
 	} else {
 		ret = -EINVAL;
 	}
@@ -3071,8 +2853,8 @@ static int cfg80211_rtw_start_ap(struct wiphy *wiphy, struct net_device *ndev,
 	int ret = 0;
 	struct rtw_adapter *adapter = wiphy_to_adapter(wiphy);
 
-	DBG_8723A(FUNC_NDEV_FMT " hidden_ssid:%d, auth_type:%d\n",
-		  FUNC_NDEV_ARG(ndev), settings->hidden_ssid,
+	DBG_8723A("%s(%s): hidden_ssid:%d, auth_type:%d\n",
+		  __func__, ndev->name, settings->hidden_ssid,
 		  settings->auth_type);
 
 	ret = rtw_add_beacon(adapter, settings->beacon.head,
@@ -3088,29 +2870,12 @@ static int cfg80211_rtw_start_ap(struct wiphy *wiphy, struct net_device *ndev,
 		struct wlan_bssid_ex *pbss_network_ext =
 			&adapter->mlmeextpriv.mlmext_info.network;
 
-		if (0)
-			DBG_8723A(FUNC_ADPT_FMT
-				  " ssid:(%s,%d), from ie:(%s,%d)\n",
-				  FUNC_ADPT_ARG(adapter), settings->ssid,
-				  (int)settings->ssid_len,
-				  pbss_network->Ssid.ssid,
-				  pbss_network->Ssid.ssid_len);
-
 		memcpy(pbss_network->Ssid.ssid, (void *)settings->ssid,
 		       settings->ssid_len);
 		pbss_network->Ssid.ssid_len = settings->ssid_len;
 		memcpy(pbss_network_ext->Ssid.ssid, (void *)settings->ssid,
 		       settings->ssid_len);
 		pbss_network_ext->Ssid.ssid_len = settings->ssid_len;
-
-		if (0)
-			DBG_8723A(FUNC_ADPT_FMT
-				  " after ssid:(%s,%d), (%s,%d)\n",
-				  FUNC_ADPT_ARG(adapter),
-				  pbss_network->Ssid.ssid,
-				  pbss_network->Ssid.ssid_len,
-				  pbss_network_ext->Ssid.ssid,
-				  pbss_network_ext->Ssid.ssid_len);
 	}
 
 	return ret;
@@ -3123,7 +2888,7 @@ static int cfg80211_rtw_change_beacon(struct wiphy *wiphy,
 	int ret = 0;
 	struct rtw_adapter *adapter = wiphy_to_adapter(wiphy);
 
-	DBG_8723A(FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("%s(%s)\n", __func__, ndev->name);
 
 	ret = rtw_add_beacon(adapter, info->head, info->head_len, info->tail,
 			     info->tail_len);
@@ -3133,7 +2898,7 @@ static int cfg80211_rtw_change_beacon(struct wiphy *wiphy,
 
 static int cfg80211_rtw_stop_ap(struct wiphy *wiphy, struct net_device *ndev)
 {
-	DBG_8723A(FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("%s(%s)\n", __func__, ndev->name);
 	return 0;
 }
 
@@ -3141,7 +2906,7 @@ static int cfg80211_rtw_add_station(struct wiphy *wiphy,
 				    struct net_device *ndev, u8 *mac,
 				    struct station_parameters *params)
 {
-	DBG_8723A(FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("%s(%s)\n", __func__, ndev->name);
 
 	return 0;
 }
@@ -3157,7 +2922,7 @@ static int cfg80211_rtw_del_station(struct wiphy *wiphy,
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct sta_priv *pstapriv = &padapter->stapriv;
 
-	DBG_8723A("+" FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("+%s(%s)\n", __func__, ndev->name);
 
 	if (check_fwstate(pmlmepriv, (_FW_LINKED | WIFI_AP_STATE)) != true) {
 		DBG_8723A("%s, fw_state != FW_LINKED|WIFI_AP_STATE\n",
@@ -3188,7 +2953,7 @@ static int cfg80211_rtw_del_station(struct wiphy *wiphy,
 	list_for_each_safe(plist, ptmp, phead) {
 		psta = container_of(plist, struct sta_info, asoc_list);
 
-		if (!memcmp(mac, psta->hwaddr, ETH_ALEN)) {
+		if (ether_addr_equal(mac, psta->hwaddr)) {
 			if (psta->dot8021xalg == 1 &&
 			    psta->bpairwise_key_installed == false) {
 				DBG_8723A("%s, sta's dot8021xalg = 1 and "
@@ -3217,7 +2982,7 @@ static int cfg80211_rtw_del_station(struct wiphy *wiphy,
 
 	associated_clients_update23a(padapter, updated);
 
-	DBG_8723A("-" FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("-%s(%s)\n", __func__, ndev->name);
 
 	return ret;
 }
@@ -3226,7 +2991,7 @@ static int cfg80211_rtw_change_station(struct wiphy *wiphy,
 				       struct net_device *ndev, u8 *mac,
 				       struct station_parameters *params)
 {
-	DBG_8723A(FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("%s(%s)\n", __func__, ndev->name);
 	return 0;
 }
 
@@ -3234,7 +2999,7 @@ static int cfg80211_rtw_dump_station(struct wiphy *wiphy,
 				     struct net_device *ndev, int idx, u8 *mac,
 				     struct station_info *sinfo)
 {
-	DBG_8723A(FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("%s(%s)\n", __func__, ndev->name);
 
 	/* TODO: dump scanned queue */
 
@@ -3244,104 +3009,27 @@ static int cfg80211_rtw_dump_station(struct wiphy *wiphy,
 static int cfg80211_rtw_change_bss(struct wiphy *wiphy, struct net_device *ndev,
 				   struct bss_parameters *params)
 {
-	DBG_8723A(FUNC_NDEV_FMT "\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("%s(%s)\n", __func__, ndev->name);
 	return 0;
 }
 #endif /* CONFIG_8723AU_AP_MODE */
 
-void rtw_cfg80211_rx_action_p2p(struct rtw_adapter *padapter, u8 *pmgmt_frame,
-				uint frame_len)
-{
-#ifdef CONFIG_8723AU_P2P
-	int type;
-#endif
-	s32 freq;
-	int channel;
-	u8 category, action;
-
-	channel = rtw_get_oper_ch23a(padapter);
-
-	DBG_8723A("RTW_Rx:cur_ch =%d\n", channel);
-#ifdef CONFIG_8723AU_P2P
-	type = rtw_p2p_check_frames(padapter, pmgmt_frame, frame_len, false);
-	if (type >= 0)
-		goto indicate;
-#endif
-	rtw_action_frame_parse23a(pmgmt_frame, frame_len, &category, &action);
-	DBG_8723A("RTW_Rx:category(%u), action(%u)\n", category, action);
-
-#ifdef CONFIG_8723AU_P2P
-indicate:
-#endif
-	if (channel <= RTW_CH_MAX_2G_CHANNEL)
-		freq = ieee80211_channel_to_frequency(channel,
-						      IEEE80211_BAND_2GHZ);
-	else
-		freq = ieee80211_channel_to_frequency(channel,
-						      IEEE80211_BAND_5GHZ);
-
-	rtw_cfg80211_rx_mgmt(padapter, freq, 0, pmgmt_frame, frame_len,
-			     GFP_ATOMIC);
-}
-
-void rtw_cfg80211_rx_p2p_action_public(struct rtw_adapter *padapter,
-				       u8 *pmgmt_frame, uint frame_len)
-{
-#ifdef CONFIG_8723AU_P2P
-	int type;
-#endif
-	s32 freq;
-	int channel;
-	u8 category, action;
-
-	channel = rtw_get_oper_ch23a(padapter);
-
-	DBG_8723A("RTW_Rx:cur_ch =%d\n", channel);
-#ifdef CONFIG_8723AU_P2P
-	type = rtw_p2p_check_frames(padapter, pmgmt_frame, frame_len, false);
-	if (type >= 0) {
-		switch (type) {
-		case P2P_GO_NEGO_CONF:
-		case P2P_PROVISION_DISC_RESP:
-			rtw_clear_scan_deny(padapter);
-		}
-		goto indicate;
-	}
-#endif
-	rtw_action_frame_parse23a(pmgmt_frame, frame_len, &category, &action);
-	DBG_8723A("RTW_Rx:category(%u), action(%u)\n", category, action);
-
-#ifdef CONFIG_8723AU_P2P
-indicate:
-#endif
-	if (channel <= RTW_CH_MAX_2G_CHANNEL)
-		freq = ieee80211_channel_to_frequency(channel,
-						      IEEE80211_BAND_2GHZ);
-	else
-		freq = ieee80211_channel_to_frequency(channel,
-						      IEEE80211_BAND_5GHZ);
-
-	rtw_cfg80211_rx_mgmt(padapter, freq, 0, pmgmt_frame, frame_len,
-			     GFP_ATOMIC);
-}
-
 void rtw_cfg80211_rx_action(struct rtw_adapter *adapter, u8 *frame,
 			    uint frame_len, const char *msg)
 {
+	struct ieee80211_mgmt *hdr = (struct ieee80211_mgmt *)frame;
 	s32 freq;
 	int channel;
-	u8 category, action;
 
 	channel = rtw_get_oper_ch23a(adapter);
 
-	rtw_action_frame_parse23a(frame, frame_len, &category, &action);
-
 	DBG_8723A("RTW_Rx:cur_ch =%d\n", channel);
 	if (msg)
 		DBG_8723A("RTW_Rx:%s\n", msg);
 	else
-		DBG_8723A("RTW_Rx:category(%u), action(%u)\n", category,
-			  action);
+		DBG_8723A("RTW_Rx:category(%u), action(%u)\n",
+			  hdr->u.action.category,
+			  hdr->u.action.u.wme_action.action_code);
 
 	if (channel <= RTW_CH_MAX_2G_CHANNEL)
 		freq = ieee80211_channel_to_frequency(channel,
@@ -3350,353 +3038,10 @@ void rtw_cfg80211_rx_action(struct rtw_adapter *adapter, u8 *frame,
 		freq = ieee80211_channel_to_frequency(channel,
 						      IEEE80211_BAND_5GHZ);
 
-	rtw_cfg80211_rx_mgmt(adapter, freq, 0, frame, frame_len, GFP_ATOMIC);
+	cfg80211_rx_mgmt(adapter->rtw_wdev, freq, 0, frame, frame_len,
+			 0, GFP_ATOMIC);
 }
 
-#ifdef CONFIG_8723AU_P2P
-void rtw_cfg80211_issue_p2p_provision_request23a(struct rtw_adapter *padapter,
-					      const u8 *buf, size_t len)
-{
-	u16 wps_devicepassword_id = 0x0000;
-	uint wps_devicepassword_id_len = 0;
-	u8 wpsie[255] = { 0x00 }, p2p_ie[255] = { 0x00 };
-	uint p2p_ielen = 0;
-	uint wpsielen = 0;
-	u32 devinfo_contentlen = 0;
-	u8 devinfo_content[64] = { 0x00 };
-	u16 capability = 0;
-	uint capability_len = 0;
-
-	unsigned char category = WLAN_CATEGORY_PUBLIC;
-	u8 action = P2P_PUB_ACTION_ACTION;
-	u8 dialogToken = 1;
-	u32 p2poui = cpu_to_be32(P2POUI);
-	u8 oui_subtype = P2P_PROVISION_DISC_REQ;
-	u32 p2pielen = 0;
-#ifdef CONFIG_8723AU_P2P
-	u32 wfdielen = 0;
-#endif /* CONFIG_8723AU_P2P */
-
-	struct xmit_frame *pmgntframe;
-	struct pkt_attrib *pattrib;
-	unsigned char *pframe;
-	struct ieee80211_hdr *pwlanhdr, *hdr;
-	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-	u8 *frame_body =
-	    (unsigned char *)(buf + sizeof(struct ieee80211_hdr_3addr));
-	size_t frame_body_len = len - sizeof(struct ieee80211_hdr_3addr);
-
-	DBG_8723A("[%s] In\n", __func__);
-
-	hdr = (struct ieee80211_hdr *)buf;
-	/* prepare for building provision_request frame */
-	memcpy(pwdinfo->tx_prov_disc_info.peerIFAddr, hdr->addr1, ETH_ALEN);
-	memcpy(pwdinfo->tx_prov_disc_info.peerDevAddr, hdr->addr1, ETH_ALEN);
-
-	pwdinfo->tx_prov_disc_info.wps_config_method_request =
-	    WPS_CM_PUSH_BUTTON;
-
-	rtw_get_wps_ie23a(frame_body + _PUBLIC_ACTION_IE_OFFSET_,
-		       frame_body_len - _PUBLIC_ACTION_IE_OFFSET_, wpsie,
-		       &wpsielen);
-	rtw_get_wps_attr_content23a(wpsie, wpsielen, WPS_ATTR_DEVICE_PWID,
-				    (u8 *)&wps_devicepassword_id,
-				    &wps_devicepassword_id_len);
-	wps_devicepassword_id = be16_to_cpu(wps_devicepassword_id);
-
-	switch (wps_devicepassword_id) {
-	case WPS_DPID_PIN:
-		pwdinfo->tx_prov_disc_info.wps_config_method_request =
-			WPS_CM_LABEL;
-		break;
-	case WPS_DPID_USER_SPEC:
-		pwdinfo->tx_prov_disc_info.wps_config_method_request =
-			WPS_CM_DISPLYA;
-		break;
-	case WPS_DPID_MACHINE_SPEC:
-		break;
-	case WPS_DPID_REKEY:
-		break;
-	case WPS_DPID_PBC:
-		pwdinfo->tx_prov_disc_info.wps_config_method_request =
-			WPS_CM_PUSH_BUTTON;
-		break;
-	case WPS_DPID_REGISTRAR_SPEC:
-		pwdinfo->tx_prov_disc_info.wps_config_method_request =
-			WPS_CM_KEYPAD;
-		break;
-	default:
-		break;
-	}
-
-	if (rtw_get_p2p_ie23a(frame_body + _PUBLIC_ACTION_IE_OFFSET_,
-			   frame_body_len - _PUBLIC_ACTION_IE_OFFSET_,
-			   p2p_ie, &p2p_ielen)) {
-		rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen,
-					 P2P_ATTR_DEVICE_INFO, devinfo_content,
-					 &devinfo_contentlen);
-		rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY,
-					 (u8 *)&capability, &capability_len);
-	}
-
-	/* start to build provision_request frame */
-	memset(wpsie, 0, sizeof(wpsie));
-	memset(p2p_ie, 0, sizeof(p2p_ie));
-	p2p_ielen = 0;
-
-	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
-	if (pmgntframe == NULL)
-		return;
-	/* update attribute */
-	pattrib = &pmgntframe->attrib;
-	update_mgntframe_attrib23a(padapter, pattrib);
-
-	memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
-
-	pframe = (u8 *) (pmgntframe->buf_addr) + TXDESC_OFFSET;
-	pwlanhdr = (struct ieee80211_hdr *)pframe;
-
-	pwlanhdr->frame_control = 0;
-
-	memcpy(pwlanhdr->addr1, pwdinfo->tx_prov_disc_info.peerDevAddr,
-	       ETH_ALEN);
-	memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
-	memcpy(pwlanhdr->addr3, pwdinfo->tx_prov_disc_info.peerDevAddr,
-	       ETH_ALEN);
-
-	SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
-	pmlmeext->mgnt_seq++;
-	SetFrameSubType(pframe, WIFI_ACTION);
-
-	pframe += sizeof(struct ieee80211_hdr_3addr);
-	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
-
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *)&p2poui,
-				  &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen);
-	pframe = rtw_set_fixed_ie23a(pframe, 1, &dialogToken, &pattrib->pktlen);
-
-	/* build_prov_disc_request_p2p_ie23a */
-	/*      P2P OUI */
-	p2pielen = 0;
-	p2p_ie[p2pielen++] = 0x50;
-	p2p_ie[p2pielen++] = 0x6F;
-	p2p_ie[p2pielen++] = 0x9A;
-	p2p_ie[p2pielen++] = 0x09;	/*      WFA P2P v1.0 */
-
-	/*      Commented by Albert 20110301 */
-	/*      According to the P2P Specification, the provision discovery request frame should contain 3 P2P attributes */
-	/*      1. P2P Capability */
-	/*      2. Device Info */
-	/*      3. Group ID ( When joining an operating P2P Group ) */
-
-	/*      P2P Capability ATTR */
-	/*      Type: */
-	p2p_ie[p2pielen++] = P2P_ATTR_CAPABILITY;
-
-	/*      Length: */
-	put_unaligned_le16(0x0002, p2p_ie + p2pielen);
-	p2pielen += 2;
-
-	/*      Value: */
-	/*      Device Capability Bitmap, 1 byte */
-	/*      Group Capability Bitmap, 1 byte */
-	memcpy(p2p_ie + p2pielen, &capability, 2);
-	p2pielen += 2;
-
-	/*      Device Info ATTR */
-	/*      Type: */
-	p2p_ie[p2pielen++] = P2P_ATTR_DEVICE_INFO;
-
-	/*      Length: */
-	put_unaligned_le16(devinfo_contentlen, p2p_ie + p2pielen);
-	p2pielen += 2;
-
-	/*      Value: */
-	memcpy(p2p_ie + p2pielen, devinfo_content, devinfo_contentlen);
-	p2pielen += devinfo_contentlen;
-
-	pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen,
-			    (unsigned char *)p2p_ie, &p2p_ielen);
-	pattrib->pktlen += p2p_ielen;
-
-	wpsielen = 0;
-	/*      WPS OUI */
-	*(u32 *)(wpsie) = cpu_to_be32(WPSOUI);
-	wpsielen += 4;
-
-	/*      WPS version */
-	/*      Type: */
-	*(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_VER1);
-	wpsielen += 2;
-
-	/*      Length: */
-	*(u16 *)(wpsie + wpsielen) = cpu_to_be16(0x0001);
-	wpsielen += 2;
-
-	/*      Value: */
-	wpsie[wpsielen++] = WPS_VERSION_1;	/*      Version 1.0 */
-
-	/*      Config Method */
-	/*      Type: */
-	*(u16 *)(wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD);
-	wpsielen += 2;
-
-	/*      Length: */
-	*(u16 *)(wpsie + wpsielen) = cpu_to_be16(0x0002);
-	wpsielen += 2;
-
-	/*      Value: */
-	*(u16 *)(wpsie + wpsielen) =
-	    cpu_to_be16(pwdinfo->tx_prov_disc_info.wps_config_method_request);
-	wpsielen += 2;
-
-	pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, wpsielen,
-			    (unsigned char *)wpsie, &pattrib->pktlen);
-
-#ifdef CONFIG_8723AU_P2P
-	wfdielen = build_provdisc_req_wfd_ie(pwdinfo, pframe);
-	pframe += wfdielen;
-	pattrib->pktlen += wfdielen;
-#endif /* CONFIG_8723AU_P2P */
-
-	pattrib->last_txcmdsz = pattrib->pktlen;
-
-	/* dump_mgntframe23a(padapter, pmgntframe); */
-	if (dump_mgntframe23a_and_wait_ack23a(padapter, pmgntframe) != _SUCCESS)
-		DBG_8723A("%s, ack to\n", __func__);
-}
-
-static s32 cfg80211_rtw_remain_on_channel(struct wiphy *wiphy,
-					  struct wireless_dev *wdev,
-					  struct ieee80211_channel *channel,
-					  unsigned int duration, u64 *cookie)
-{
-	s32 err = 0;
-	struct rtw_adapter *padapter = wiphy_to_adapter(wiphy);
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-	struct cfg80211_wifidirect_info *pcfg80211_wdinfo =
-	    &padapter->cfg80211_wdinfo;
-	u8 remain_ch =
-		(u8) ieee80211_frequency_to_channel(channel->center_freq);
-	u8 ready_on_channel = false;
-
-	DBG_8723A(FUNC_ADPT_FMT " ch:%u duration:%d\n", FUNC_ADPT_ARG(padapter),
-		  remain_ch, duration);
-
-	if (pcfg80211_wdinfo->is_ro_ch == true) {
-		DBG_8723A("%s, cancel ro ch timer\n", __func__);
-
-		del_timer_sync(&padapter->cfg80211_wdinfo.remain_on_ch_timer);
-
-#ifdef CONFIG_8723AU_P2P
-		p2p_protocol_wk_hdl23a(padapter, P2P_RO_CH_WK);
-#endif
-	}
-
-	pcfg80211_wdinfo->is_ro_ch = true;
-
-	if (_FAIL == rtw_pwr_wakeup(padapter)) {
-		err = -EFAULT;
-		goto exit;
-	}
-
-	memcpy(&pcfg80211_wdinfo->remain_on_ch_channel, channel,
-	       sizeof(struct ieee80211_channel));
-	pcfg80211_wdinfo->remain_on_ch_cookie = *cookie;
-
-	rtw_scan_abort23a(padapter);
-	if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) {
-		rtw_p2p_enable23a(padapter, P2P_ROLE_DEVICE);
-		wdev_to_priv(padapter->rtw_wdev)->p2p_enabled = true;
-	} else {
-		rtw_p2p_set_pre_state(pwdinfo, rtw_p2p_state(pwdinfo));
-#ifdef CONFIG_DEBUG_CFG80211
-		DBG_8723A("%s, role =%d, p2p_state =%d\n", __func__,
-			  rtw_p2p_role(pwdinfo), rtw_p2p_state(pwdinfo));
-#endif
-	}
-
-	rtw_p2p_set_state(pwdinfo, P2P_STATE_LISTEN);
-
-	if (duration < 400)
-		duration = duration * 3;	/* extend from exper. */
-
-	pcfg80211_wdinfo->restore_channel = pmlmeext->cur_channel;
-
-	if (rtw_ch_set_search_ch23a(pmlmeext->channel_set, remain_ch) >= 0) {
-		if (remain_ch != pmlmeext->cur_channel) {
-			ready_on_channel = true;
-		}
-	} else {
-		DBG_8723A("%s remain_ch:%u not in channel plan!!!!\n",
-			  __func__, remain_ch);
-	}
-
-	/* call this after other things have been done */
-	if (ready_on_channel == true) {
-		if (!check_fwstate(&padapter->mlmepriv, _FW_LINKED)) {
-			pmlmeext->cur_channel = remain_ch;
-
-			set_channel_bwmode23a(padapter, remain_ch,
-					   HAL_PRIME_CHNL_OFFSET_DONT_CARE,
-					   HT_CHANNEL_WIDTH_20);
-		}
-	}
-	DBG_8723A("%s, set ro ch timer, duration =%d\n", __func__, duration);
-	mod_timer(&pcfg80211_wdinfo->remain_on_ch_timer,
-		  jiffies + msecs_to_jiffies(duration));
-
-	rtw_cfg80211_ready_on_channel(padapter, *cookie, channel, channel_type,
-				      duration, GFP_KERNEL);
-
-	pwdinfo->listen_channel = pmlmeext->cur_channel;
-
-exit:
-	if (err)
-		pcfg80211_wdinfo->is_ro_ch = false;
-
-	return err;
-}
-
-static s32 cfg80211_rtw_cancel_remain_on_channel(struct wiphy *wiphy,
-						 struct wireless_dev *wdev,
-						 u64 cookie)
-{
-	s32 err = 0;
-	struct rtw_adapter *padapter = wiphy_to_adapter(wiphy);
-	struct wifidirect_info *pwdinfo = &padapter->wdinfo;
-	struct cfg80211_wifidirect_info *pcfg80211_wdinfo =
-	    &padapter->cfg80211_wdinfo;
-
-	DBG_8723A(FUNC_ADPT_FMT "\n", FUNC_ADPT_ARG(padapter));
-
-	if (pcfg80211_wdinfo->is_ro_ch == true) {
-		DBG_8723A("%s, cancel ro ch timer\n", __func__);
-		del_timer_sync(&padapter->cfg80211_wdinfo.remain_on_ch_timer);
-#ifdef CONFIG_8723AU_P2P
-		p2p_protocol_wk_hdl23a(padapter, P2P_RO_CH_WK);
-#endif
-	}
-
-	rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo));
-#ifdef CONFIG_DEBUG_CFG80211
-	DBG_8723A("%s, role =%d, p2p_state =%d\n", __func__,
-		  rtw_p2p_role(pwdinfo), rtw_p2p_state(pwdinfo));
-#endif
-	pcfg80211_wdinfo->is_ro_ch = false;
-
-	return err;
-}
-
-#endif /* CONFIG_8723AU_P2P */
-
 static int _cfg80211_rtw_mgmt_tx(struct rtw_adapter *padapter, u8 tx_ch,
 				 const u8 *buf, size_t len)
 {
@@ -3704,11 +3049,9 @@ static int _cfg80211_rtw_mgmt_tx(struct rtw_adapter *padapter, u8 tx_ch,
 	struct pkt_attrib *pattrib;
 	unsigned char *pframe;
 	int ret = _FAIL;
-	bool ack = true;
 	struct ieee80211_hdr *pwlanhdr;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	/* struct cfg80211_wifidirect_info *pcfg80211_wdinfo = &padapter->cfg80211_wdinfo; */
 
 	if (_FAIL == rtw_pwr_wakeup(padapter)) {
 		ret = -EFAULT;
@@ -3729,7 +3072,7 @@ static int _cfg80211_rtw_mgmt_tx(struct rtw_adapter *padapter, u8 tx_ch,
 
 	/* starting alloc mgmt frame to dump it */
 	pmgntframe = alloc_mgtxmitframe23a(pxmitpriv);
-	if (pmgntframe == NULL) {
+	if (!pmgntframe) {
 		/* ret = -ENOMEM; */
 		ret = _FAIL;
 		goto exit;
@@ -3753,139 +3096,76 @@ static int _cfg80211_rtw_mgmt_tx(struct rtw_adapter *padapter, u8 tx_ch,
 	pattrib->seqnum = pmlmeext->mgnt_seq;
 	pmlmeext->mgnt_seq++;
 
-#ifdef CONFIG_8723AU_P2P
-	{
-		struct wifi_display_info *pwfd_info;
-
-		pwfd_info = padapter->wdinfo.wfd_info;
-
-		if (true == pwfd_info->wfd_enable) {
-			rtw_append_wfd_ie(padapter, pframe, &pattrib->pktlen);
-		}
-	}
-#endif /*  CONFIG_8723AU_P2P */
-
 	pattrib->last_txcmdsz = pattrib->pktlen;
 
-	if (dump_mgntframe23a_and_wait_ack23a(padapter, pmgntframe) != _SUCCESS) {
-		ack = false;
-		ret = _FAIL;
+	ret = dump_mgntframe23a_and_wait_ack23a(padapter, pmgntframe);
 
-#ifdef CONFIG_DEBUG_CFG80211
-		DBG_8723A("%s, ack == _FAIL\n", __func__);
-#endif
-	} else {
-#ifdef CONFIG_DEBUG_CFG80211
-		DBG_8723A("%s, ack =%d, ok!\n", __func__, ack);
-#endif
-		ret = _SUCCESS;
-	}
+	if (ret  != _SUCCESS)
+		DBG_8723A("%s, ack == false\n", __func__);
+	else
+		DBG_8723A("%s, ack == true\n", __func__);
 
 exit:
 
-#ifdef CONFIG_DEBUG_CFG80211
 	DBG_8723A("%s, ret =%d\n", __func__, ret);
-#endif
 
 	return ret;
 }
 
 static int cfg80211_rtw_mgmt_tx(struct wiphy *wiphy, struct wireless_dev *wdev,
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3, 14, 0))
-				struct ieee80211_channel *chan,
-				bool offchan,
-				unsigned int wait,
-				const u8 *buf, size_t len,
-				bool no_cck, bool dont_wait_for_ack,
-#else
 				struct cfg80211_mgmt_tx_params *params,
-#endif
 				u64 *cookie)
 {
 	struct rtw_adapter *padapter =
 		(struct rtw_adapter *)wiphy_to_adapter(wiphy);
-	struct rtw_wdev_priv *pwdev_priv = wdev_to_priv(padapter->rtw_wdev);
 	int ret = 0;
 	int tx_ret;
 	u32 dump_limit = RTW_MAX_MGMT_TX_CNT;
 	u32 dump_cnt = 0;
 	bool ack = true;
 	u8 category, action;
-	int type = (-1);
 	unsigned long start = jiffies;
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 14, 0))
 	size_t len = params->len;
 	struct ieee80211_channel *chan = params->chan;
 	const u8 *buf = params->buf;
-#endif
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)buf;
+	struct ieee80211_mgmt *hdr = (struct ieee80211_mgmt *)buf;
 	u8 tx_ch = (u8) ieee80211_frequency_to_channel(chan->center_freq);
 
+	if (!ieee80211_is_action(hdr->frame_control))
+		return -EINVAL;
+
 	/* cookie generation */
 	*cookie = (unsigned long)buf;
 
-#ifdef CONFIG_DEBUG_CFG80211
-	DBG_8723A(FUNC_ADPT_FMT " len =%zu, ch =%d"
-		  "\n", FUNC_ADPT_ARG(padapter), len, tx_ch);
-#endif /* CONFIG_DEBUG_CFG80211 */
+	DBG_8723A("%s(%s): len =%zu, ch =%d\n", __func__,
+		  padapter->pnetdev->name, len, tx_ch);
 
 	/* indicate ack before issue frame to avoid racing with rsp frame */
-	rtw_cfg80211_mgmt_tx_status(padapter, *cookie, buf, len, ack,
-				    GFP_KERNEL);
-
-	if (rtw_action_frame_parse23a(buf, len, &category, &action) == false) {
-		DBG_8723A(FUNC_ADPT_FMT " frame_control:0x%x\n",
-			  FUNC_ADPT_ARG(padapter),
-			  le16_to_cpu(hdr->frame_control));
-		goto exit;
-	}
+	cfg80211_mgmt_tx_status(padapter->rtw_wdev, *cookie, buf, len, ack,
+				GFP_KERNEL);
 
 	DBG_8723A("RTW_Tx:tx_ch =%d, da =" MAC_FMT "\n", tx_ch,
-		  MAC_ARG(hdr->addr1));
-#ifdef CONFIG_8723AU_P2P
-	type = rtw_p2p_check_frames(padapter, buf, len, true);
-	if (type >= 0)
-		goto dump;
-#endif
+		  MAC_ARG(hdr->da));
+	category = hdr->u.action.category;
+	action = hdr->u.action.u.wme_action.action_code;
 	if (category == WLAN_CATEGORY_PUBLIC)
 		DBG_8723A("RTW_Tx:%s\n", action_public_str23a(action));
 	else
 		DBG_8723A("RTW_Tx:category(%u), action(%u)\n",
 			  category, action);
 
-#ifdef CONFIG_8723AU_P2P
-dump:
-#endif
 	do {
 		dump_cnt++;
 		tx_ret = _cfg80211_rtw_mgmt_tx(padapter, tx_ch, buf, len);
 	} while (dump_cnt < dump_limit && tx_ret != _SUCCESS);
 
 	if (tx_ret != _SUCCESS || dump_cnt > 1) {
-		DBG_8723A(FUNC_ADPT_FMT " %s (%d/%d) in %d ms\n",
-			  FUNC_ADPT_ARG(padapter),
+		DBG_8723A("%s(%s): %s (%d/%d) in %d ms\n",
+			  __func__, padapter->pnetdev->name,
 			  tx_ret == _SUCCESS ? "OK" : "FAIL", dump_cnt,
 			  dump_limit, jiffies_to_msecs(jiffies - start));
 	}
 
-	switch (type) {
-	case P2P_GO_NEGO_CONF:
-		rtw_clear_scan_deny(padapter);
-		break;
-	case P2P_INVIT_RESP:
-		if (pwdev_priv->invit_info.flags & BIT(0)
-		    && pwdev_priv->invit_info.status == 0) {
-			DBG_8723A(FUNC_ADPT_FMT " agree with invitation of "
-				  "persistent group\n",
-				  FUNC_ADPT_ARG(padapter));
-			rtw_set_scan_deny(padapter, 5000);
-			rtw_pwr_wakeup_ex(padapter, 5000);
-			rtw_clear_scan_deny(padapter);
-		}
-		break;
-	}
-
-exit:
 	return ret;
 }
 
@@ -3893,358 +3173,12 @@ static void cfg80211_rtw_mgmt_frame_register(struct wiphy *wiphy,
 					     struct wireless_dev *wdev,
 					     u16 frame_type, bool reg)
 {
-
-#ifdef CONFIG_DEBUG_CFG80211
-	DBG_8723A(FUNC_ADPT_FMT " frame_type:%x, reg:%d\n",
-		  FUNC_ADPT_ARG(adapter), frame_type, reg);
-#endif
-
 	if (frame_type != (IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_PROBE_REQ))
 		return;
 
 	return;
 }
 
-static int rtw_cfg80211_set_beacon_wpsp2pie(struct net_device *ndev, char *buf,
-					    int len)
-{
-	int ret = 0;
-	uint wps_ielen = 0;
-	u8 *wps_ie;
-#ifdef CONFIG_8723AU_P2P
-	u32 p2p_ielen = 0;
-	u32 wfd_ielen = 0;
-	u8 *p2p_ie;
-#endif
-#ifdef CONFIG_8723AU_AP_MODE
-	u8 wps_oui[8] = { 0x0, 0x50, 0xf2, 0x04 };
-#endif
-	struct rtw_adapter *padapter = netdev_priv(ndev);
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-
-	DBG_8723A(FUNC_NDEV_FMT " ielen =%d\n", FUNC_NDEV_ARG(ndev), len);
-
-	if (len > 0) {
-		wps_ie = rtw_get_wps_ie23a(buf, len, NULL, &wps_ielen);
-		if (wps_ie) {
-#ifdef CONFIG_DEBUG_CFG80211
-			DBG_8723A("bcn_wps_ielen =%d\n", wps_ielen);
-#endif
-
-			if (pmlmepriv->wps_beacon_ie) {
-				pmlmepriv->wps_beacon_ie_len = 0;
-				kfree(pmlmepriv->wps_beacon_ie);
-				pmlmepriv->wps_beacon_ie = NULL;
-			}
-
-			pmlmepriv->wps_beacon_ie =
-				kmalloc(wps_ielen, GFP_KERNEL);
-			if (pmlmepriv->wps_beacon_ie == NULL) {
-				DBG_8723A("%s()-%d: kmalloc() ERROR!\n",
-					  __func__, __LINE__);
-				return -EINVAL;
-			}
-			memcpy(pmlmepriv->wps_beacon_ie, wps_ie, wps_ielen);
-			pmlmepriv->wps_beacon_ie_len = wps_ielen;
-
-#ifdef CONFIG_8723AU_AP_MODE
-			update_beacon23a(padapter, _VENDOR_SPECIFIC_IE_, wps_oui,
-				      true);
-#endif
-		}
-#ifdef CONFIG_8723AU_P2P
-		p2p_ie = rtw_get_p2p_ie23a(buf, len, NULL, &p2p_ielen);
-		if (p2p_ie) {
-#ifdef CONFIG_DEBUG_CFG80211
-			DBG_8723A("bcn_p2p_ielen =%d\n", p2p_ielen);
-#endif
-
-			if (pmlmepriv->p2p_beacon_ie) {
-				pmlmepriv->p2p_beacon_ie_len = 0;
-				kfree(pmlmepriv->p2p_beacon_ie);
-				pmlmepriv->p2p_beacon_ie = NULL;
-			}
-
-			pmlmepriv->p2p_beacon_ie =
-				kmalloc(p2p_ielen, GFP_KERNEL);
-			if (pmlmepriv->p2p_beacon_ie == NULL) {
-				DBG_8723A("%s()-%d: kmalloc() ERROR!\n",
-					  __func__, __LINE__);
-				return -EINVAL;
-			}
-
-			memcpy(pmlmepriv->p2p_beacon_ie, p2p_ie, p2p_ielen);
-			pmlmepriv->p2p_beacon_ie_len = p2p_ielen;
-		}
-#endif /* CONFIG_8723AU_P2P */
-
-		/* buf += p2p_ielen; */
-		/* len -= p2p_ielen; */
-
-#ifdef CONFIG_8723AU_P2P
-		if (rtw_get_wfd_ie(buf, len, NULL, &wfd_ielen)) {
-#ifdef CONFIG_DEBUG_CFG80211
-			DBG_8723A("bcn_wfd_ielen =%d\n", wfd_ielen);
-#endif
-
-			if (pmlmepriv->wfd_beacon_ie) {
-				pmlmepriv->wfd_beacon_ie_len = 0;
-				kfree(pmlmepriv->wfd_beacon_ie);
-				pmlmepriv->wfd_beacon_ie = NULL;
-			}
-
-			pmlmepriv->wfd_beacon_ie =
-				kmalloc(wfd_ielen, GFP_KERNEL);
-			if (pmlmepriv->wfd_beacon_ie == NULL) {
-				DBG_8723A("%s()-%d: kmalloc() ERROR!\n",
-					  __func__, __LINE__);
-				return -EINVAL;
-
-			}
-			rtw_get_wfd_ie(buf, len, pmlmepriv->wfd_beacon_ie,
-				       &pmlmepriv->wfd_beacon_ie_len);
-		}
-#endif /* CONFIG_8723AU_P2P */
-
-		pmlmeext->bstart_bss = true;
-
-	}
-
-	return ret;
-}
-
-static int rtw_cfg80211_set_probe_resp_wpsp2pie(struct net_device *net,
-						char *buf, int len)
-{
-	struct rtw_adapter *padapter = netdev_priv(net);
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-#ifdef CONFIG_8723AU_P2P
-	u32 p2p_ielen = 0;
-	u8 *p2p_ie;
-	u32 wfd_ielen = 0;
-#endif
-	int ret = 0;
-	uint wps_ielen = 0;
-	u8 *wps_ie;
-
-	if (len > 0) {
-		wps_ie = rtw_get_wps_ie23a(buf, len, NULL, &wps_ielen);
-		if (wps_ie) {
-			uint attr_contentlen = 0;
-			u16 uconfig_method, *puconfig_method = NULL;
-
-			if (pmlmepriv->wps_probe_resp_ie) {
-				pmlmepriv->wps_probe_resp_ie_len = 0;
-				kfree(pmlmepriv->wps_probe_resp_ie);
-				pmlmepriv->wps_probe_resp_ie = NULL;
-			}
-
-			pmlmepriv->wps_probe_resp_ie =
-				kmalloc(wps_ielen, GFP_KERNEL);
-			if (pmlmepriv->wps_probe_resp_ie == NULL) {
-				DBG_8723A("%s()-%d: kmalloc() ERROR!\n",
-					  __func__, __LINE__);
-				return -EINVAL;
-
-			}
-
-			/* add PUSH_BUTTON config_method by driver self in
-			   wpsie of probe_resp at GO Mode */
-			puconfig_method = (u16 *)rtw_get_wps_attr_content23a(wps_ie, wps_ielen,
-							      WPS_ATTR_CONF_METHOD,
-							      NULL,
-							      &attr_contentlen);
-			if (puconfig_method) {
-				uconfig_method = WPS_CM_PUSH_BUTTON;
-				uconfig_method = cpu_to_be16(uconfig_method);
-
-				*puconfig_method |= uconfig_method;
-			}
-
-			memcpy(pmlmepriv->wps_probe_resp_ie, wps_ie, wps_ielen);
-			pmlmepriv->wps_probe_resp_ie_len = wps_ielen;
-
-		}
-
-		/* buf += wps_ielen; */
-		/* len -= wps_ielen; */
-
-#ifdef CONFIG_8723AU_P2P
-		p2p_ie = rtw_get_p2p_ie23a(buf, len, NULL, &p2p_ielen);
-		if (p2p_ie) {
-			u8 is_GO = false;
-			u32 attr_contentlen = 0;
-			u16 cap_attr = 0;
-
-#ifdef CONFIG_DEBUG_CFG80211
-			DBG_8723A("probe_resp_p2p_ielen =%d\n", p2p_ielen);
-#endif
-
-			/* Check P2P Capability ATTR */
-			if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen,
-						     P2P_ATTR_CAPABILITY,
-						     (u8 *) &cap_attr,
-						     (uint *) &attr_contentlen)) {
-				u8 grp_cap = 0;
-				/* DBG_8723A( "[%s] Got P2P Capability Attr!!\n", __func__ ); */
-				cap_attr = le16_to_cpu(cap_attr);
-				grp_cap = (u8) ((cap_attr >> 8) & 0xff);
-
-				is_GO = (grp_cap & BIT(0)) ? true : false;
-
-				if (is_GO)
-					DBG_8723A
-					    ("Got P2P Capability Attr, grp_cap"
-					     "= 0x%x, is_GO\n", grp_cap);
-			}
-
-			if (is_GO == false) {
-				if (pmlmepriv->p2p_probe_resp_ie) {
-					pmlmepriv->p2p_probe_resp_ie_len = 0;
-					kfree(pmlmepriv->p2p_probe_resp_ie);
-					pmlmepriv->p2p_probe_resp_ie = NULL;
-				}
-
-				pmlmepriv->p2p_probe_resp_ie =
-				    kmalloc(p2p_ielen, GFP_KERNEL);
-				if (pmlmepriv->p2p_probe_resp_ie == NULL) {
-					DBG_8723A("%s()-%d: kmalloc() ERROR!\n",
-						  __func__, __LINE__);
-					return -EINVAL;
-				}
-				memcpy(pmlmepriv->p2p_probe_resp_ie, p2p_ie,
-				       p2p_ielen);
-				pmlmepriv->p2p_probe_resp_ie_len = p2p_ielen;
-			} else {
-				if (pmlmepriv->p2p_go_probe_resp_ie) {
-					pmlmepriv->p2p_go_probe_resp_ie_len = 0;
-					kfree(pmlmepriv->p2p_go_probe_resp_ie);
-					pmlmepriv->p2p_go_probe_resp_ie = NULL;
-				}
-
-				pmlmepriv->p2p_go_probe_resp_ie =
-				    kmalloc(p2p_ielen, GFP_KERNEL);
-				if (pmlmepriv->p2p_go_probe_resp_ie == NULL) {
-					DBG_8723A("%s()-%d: kmalloc() ERROR!\n",
-						  __func__, __LINE__);
-					return -EINVAL;
-
-				}
-				memcpy(pmlmepriv->p2p_go_probe_resp_ie,
-				       p2p_ie, p2p_ielen);
-				pmlmepriv->p2p_go_probe_resp_ie_len = p2p_ielen;
-			}
-		}
-#endif /* CONFIG_8723AU_P2P */
-
-		/* buf += p2p_ielen; */
-		/* len -= p2p_ielen; */
-
-#ifdef CONFIG_8723AU_P2P
-		if (rtw_get_wfd_ie(buf, len, NULL, &wfd_ielen)) {
-#ifdef CONFIG_DEBUG_CFG80211
-			DBG_8723A("probe_resp_wfd_ielen =%d\n", wfd_ielen);
-#endif
-
-			if (pmlmepriv->wfd_probe_resp_ie) {
-				pmlmepriv->wfd_probe_resp_ie_len = 0;
-				kfree(pmlmepriv->wfd_probe_resp_ie);
-				pmlmepriv->wfd_probe_resp_ie = NULL;
-			}
-
-			pmlmepriv->wfd_probe_resp_ie =
-			    kmalloc(wfd_ielen, GFP_KERNEL);
-			if (pmlmepriv->wfd_probe_resp_ie == NULL) {
-				DBG_8723A("%s()-%d: kmalloc() ERROR!\n",
-					  __func__, __LINE__);
-				return -EINVAL;
-
-			}
-			rtw_get_wfd_ie(buf, len, pmlmepriv->wfd_probe_resp_ie,
-				       &pmlmepriv->wfd_probe_resp_ie_len);
-		}
-#endif /* CONFIG_8723AU_P2P */
-	}
-
-	return ret;
-}
-
-static int rtw_cfg80211_set_assoc_resp_wpsp2pie(struct net_device *net,
-						char *buf, int len)
-{
-	int ret = 0;
-	struct rtw_adapter *padapter = netdev_priv(net);
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-
-	DBG_8723A("%s, ielen =%d\n", __func__, len);
-
-	if (len > 0) {
-		if (pmlmepriv->wps_assoc_resp_ie) {
-			pmlmepriv->wps_assoc_resp_ie_len = 0;
-			kfree(pmlmepriv->wps_assoc_resp_ie);
-			pmlmepriv->wps_assoc_resp_ie = NULL;
-		}
-
-		pmlmepriv->wps_assoc_resp_ie = kmalloc(len, GFP_KERNEL);
-		if (pmlmepriv->wps_assoc_resp_ie == NULL) {
-			DBG_8723A("%s()-%d: kmalloc() ERROR!\n",
-				  __func__, __LINE__);
-			return -EINVAL;
-
-		}
-		memcpy(pmlmepriv->wps_assoc_resp_ie, buf, len);
-		pmlmepriv->wps_assoc_resp_ie_len = len;
-	}
-
-	return ret;
-}
-
-int rtw_cfg80211_set_mgnt_wpsp2pie(struct net_device *net, char *buf, int len,
-				   int type)
-{
-	int ret = 0;
-	uint wps_ielen = 0;
-#ifdef CONFIG_8723AU_P2P
-	u32 p2p_ielen = 0;
-#endif
-
-#ifdef CONFIG_DEBUG_CFG80211
-	DBG_8723A("%s, ielen =%d\n", __func__, len);
-#endif
-
-	if ((rtw_get_wps_ie23a(buf, len, NULL, &wps_ielen) && (wps_ielen > 0))
-#ifdef CONFIG_8723AU_P2P
-	    || (rtw_get_p2p_ie23a(buf, len, NULL, &p2p_ielen) && (p2p_ielen > 0))
-#endif
-	    ) {
-		if (net) {
-			switch (type) {
-			case 0x1:	/* BEACON */
-				ret =
-				    rtw_cfg80211_set_beacon_wpsp2pie(net, buf,
-								     len);
-				break;
-			case 0x2:	/* PROBE_RESP */
-				ret =
-				    rtw_cfg80211_set_probe_resp_wpsp2pie(net,
-									 buf,
-									 len);
-				break;
-			case 0x4:	/* ASSOC_RESP */
-				ret =
-				    rtw_cfg80211_set_assoc_resp_wpsp2pie(net,
-									 buf,
-									 len);
-				break;
-			}
-		}
-	}
-
-	return ret;
-
-}
-
 static struct cfg80211_ops rtw_cfg80211_ops = {
 	.change_virtual_intf = cfg80211_rtw_change_iface,
 	.add_key = cfg80211_rtw_add_key,
@@ -4280,11 +3214,6 @@ static struct cfg80211_ops rtw_cfg80211_ops = {
 	.change_bss = cfg80211_rtw_change_bss,
 #endif /* CONFIG_8723AU_AP_MODE */
 
-#ifdef CONFIG_8723AU_P2P
-	.remain_on_channel = cfg80211_rtw_remain_on_channel,
-	.cancel_remain_on_channel = cfg80211_rtw_cancel_remain_on_channel,
-#endif
-
 	.mgmt_tx = cfg80211_rtw_mgmt_tx,
 	.mgmt_frame_register = cfg80211_rtw_mgmt_frame_register,
 };
@@ -4348,7 +3277,7 @@ void rtw_cfg80211_init_wiphy(struct rtw_adapter *padapter)
 	struct wireless_dev *pwdev = padapter->rtw_wdev;
 	struct wiphy *wiphy = pwdev->wiphy;
 
-	rtw23a_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+	rf_type = rtl8723a_get_rf_type(padapter);
 
 	DBG_8723A("%s:rf_type =%d\n", __func__, rf_type);
 
@@ -4388,9 +3317,6 @@ static void rtw_cfg80211_preinit_wiphy(struct rtw_adapter *padapter,
 #ifdef CONFIG_8723AU_AP_MODE
 	    BIT(NL80211_IFTYPE_AP) | BIT(NL80211_IFTYPE_MONITOR) |
 #endif
-#if defined(CONFIG_8723AU_P2P)
-	    BIT(NL80211_IFTYPE_P2P_CLIENT) | BIT(NL80211_IFTYPE_P2P_GO) |
-#endif
 	    0;
 
 #ifdef CONFIG_8723AU_AP_MODE
@@ -4440,22 +3366,24 @@ int rtw_wdev_alloc(struct rtw_adapter *padapter, struct device *dev)
 		ret = -ENOMEM;
 		goto exit;
 	}
+
+	/*  wdev */
+	wdev = kzalloc(sizeof(struct wireless_dev), GFP_KERNEL);
+	if (!wdev) {
+		DBG_8723A("Couldn't allocate wireless device\n");
+		ret = -ENOMEM;
+		goto free_wiphy;
+	}
+
 	set_wiphy_dev(wiphy, dev);
 	rtw_cfg80211_preinit_wiphy(padapter, wiphy);
 
 	ret = wiphy_register(wiphy);
 	if (ret < 0) {
 		DBG_8723A("Couldn't register wiphy device\n");
-		goto free_wiphy;
+		goto free_wdev;
 	}
 
-	/*  wdev */
-	wdev = kzalloc(sizeof(struct wireless_dev), GFP_KERNEL);
-	if (!wdev) {
-		DBG_8723A("Couldn't allocate wireless device\n");
-		ret = -ENOMEM;
-		goto unregister_wiphy;
-	}
 	wdev->wiphy = wiphy;
 	wdev->netdev = pnetdev;
 	/* wdev->iftype = NL80211_IFTYPE_STATION; */
@@ -4474,8 +3402,6 @@ int rtw_wdev_alloc(struct rtw_adapter *padapter, struct device *dev)
 	spin_lock_init(&pwdev_priv->scan_req_lock);
 
 	pwdev_priv->p2p_enabled = false;
-	pwdev_priv->provdisc_req_issued = false;
-	rtw_wdev_invit_info_init(&pwdev_priv->invit_info);
 
 	if (padapter->registrypriv.power_mgnt != PS_MODE_ACTIVE)
 		pwdev_priv->power_mgmt = true;
@@ -4483,8 +3409,8 @@ int rtw_wdev_alloc(struct rtw_adapter *padapter, struct device *dev)
 		pwdev_priv->power_mgmt = false;
 
 	return ret;
-unregister_wiphy:
-	wiphy_unregister(wiphy);
+free_wdev:
+	kfree(wdev);
 free_wiphy:
 	wiphy_free(wiphy);
 exit:
@@ -4493,15 +3419,11 @@ exit:
 
 void rtw_wdev_free(struct wireless_dev *wdev)
 {
-	struct rtw_wdev_priv *pwdev_priv;
-
 	DBG_8723A("%s(wdev =%p)\n", __func__, wdev);
 
 	if (!wdev)
 		return;
 
-	pwdev_priv = wdev_to_priv(wdev);
-
 	kfree(wdev->wiphy->bands[IEEE80211_BAND_2GHZ]);
 	kfree(wdev->wiphy->bands[IEEE80211_BAND_5GHZ]);
 
diff --git a/drivers/staging/rtl8723au/os_dep/mlme_linux.c b/drivers/staging/rtl8723au/os_dep/mlme_linux.c
index b30d4d37556..ac618fb8b76 100644
--- a/drivers/staging/rtl8723au/os_dep/mlme_linux.c
+++ b/drivers/staging/rtl8723au/os_dep/mlme_linux.c
@@ -20,22 +20,6 @@
 #include <mlme_osdep.h>
 #include <rtw_ioctl_set.h>
 
-void rtw_os_indicate_connect23a(struct rtw_adapter *adapter)
-{
-	rtw_cfg80211_indicate_connect(adapter);
-
-	netif_carrier_on(adapter->pnetdev);
-
-	if (adapter->pid[2] != 0)
-		rtw_signal_process(adapter->pid[2], SIGALRM);
-}
-
-void rtw_os_indicate_scan_done23a(struct rtw_adapter *padapter, bool aborted)
-{
-	rtw_cfg80211_indicate_scan_done(wdev_to_priv(padapter->rtw_wdev),
-					aborted);
-}
-
 static struct rt_pmkid_list backupPMKIDList[NUM_PMKID_CACHE];
 
 void rtw_reset_securitypriv23a(struct rtw_adapter *adapter)
@@ -52,9 +36,6 @@ void rtw_reset_securitypriv23a(struct rtw_adapter *adapter)
 		 *  When the countermeasure is trigger, the driver have to
 		 *  disconnect with AP for 60 seconds.
 		 */
-		memset(&backupPMKIDList[0], 0x00, sizeof(struct rt_pmkid_list) *
-		       NUM_PMKID_CACHE);
-
 		memcpy(&backupPMKIDList[0], &adapter->securitypriv.PMKIDList[0],
 		       sizeof(struct rt_pmkid_list) * NUM_PMKID_CACHE);
 		backupPMKIDIndex = adapter->securitypriv.PMKIDIndex;
@@ -79,10 +60,10 @@ void rtw_reset_securitypriv23a(struct rtw_adapter *adapter)
 
 		/* open system */
 		psec_priv->dot11AuthAlgrthm = dot11AuthAlgrthm_Open;
-		psec_priv->dot11PrivacyAlgrthm = _NO_PRIVACY_;
+		psec_priv->dot11PrivacyAlgrthm = 0;
 		psec_priv->dot11PrivacyKeyIndex = 0;
 
-		psec_priv->dot118021XGrpPrivacy = _NO_PRIVACY_;
+		psec_priv->dot118021XGrpPrivacy = 0;
 		psec_priv->dot118021XGrpKeyid = 1;
 
 		psec_priv->ndisauthtype = Ndis802_11AuthModeOpen;
@@ -99,89 +80,3 @@ void rtw_os_indicate_disconnect23a(struct rtw_adapter *adapter)
 
 	rtw_reset_securitypriv23a(adapter);
 }
-
-void rtw_report_sec_ie23a(struct rtw_adapter *adapter, u8 authmode, u8 *sec_ie)
-{
-	uint	len;
-	u8	*buff, *p, i;
-	union iwreq_data wrqu;
-
-	RT_TRACE(_module_mlme_osdep_c_, _drv_info_,
-		 ("+rtw_report_sec_ie23a, authmode =%d\n", authmode));
-
-	buff = NULL;
-	if (authmode == _WPA_IE_ID_) {
-		RT_TRACE(_module_mlme_osdep_c_, _drv_info_,
-			 ("rtw_report_sec_ie23a, authmode =%d\n", authmode));
-
-		buff = kzalloc(IW_CUSTOM_MAX, GFP_KERNEL);
-		if (!buff)
-			return;
-		p = buff;
-
-		p += sprintf(p, "ASSOCINFO(ReqIEs =");
-
-		len = sec_ie[1]+2;
-		len =  (len < IW_CUSTOM_MAX) ? len : IW_CUSTOM_MAX;
-
-		for (i = 0; i < len; i++)
-			p += sprintf(p, "%02x", sec_ie[i]);
-
-		p += sprintf(p, ")");
-
-		memset(&wrqu, 0, sizeof(wrqu));
-
-		wrqu.data.length = p-buff;
-
-		wrqu.data.length = (wrqu.data.length < IW_CUSTOM_MAX) ?
-				   wrqu.data.length : IW_CUSTOM_MAX;
-
-		kfree(buff);
-	}
-}
-
-#ifdef CONFIG_8723AU_AP_MODE
-void rtw_indicate_sta_assoc_event23a(struct rtw_adapter *padapter,
-				  struct sta_info *psta)
-{
-	struct sta_priv *pstapriv = &padapter->stapriv;
-	union iwreq_data wrqu;
-
-	if (psta == NULL)
-		return;
-
-	if (psta->aid > NUM_STA)
-		return;
-
-	if (pstapriv->sta_aid[psta->aid - 1] != psta)
-		return;
-
-	wrqu.addr.sa_family = ARPHRD_ETHER;
-
-	memcpy(wrqu.addr.sa_data, psta->hwaddr, ETH_ALEN);
-
-	DBG_8723A("+rtw_indicate_sta_assoc_event23a\n");
-}
-
-void rtw_indicate_sta_disassoc_event23a(struct rtw_adapter *padapter,
-				     struct sta_info *psta)
-{
-	struct sta_priv *pstapriv = &padapter->stapriv;
-	union iwreq_data wrqu;
-
-	if (psta == NULL)
-		return;
-
-	if (psta->aid > NUM_STA)
-		return;
-
-	if (pstapriv->sta_aid[psta->aid - 1] != psta)
-		return;
-
-	wrqu.addr.sa_family = ARPHRD_ETHER;
-
-	memcpy(wrqu.addr.sa_data, psta->hwaddr, ETH_ALEN);
-
-	DBG_8723A("+rtw_indicate_sta_disassoc_event23a\n");
-}
-#endif
diff --git a/drivers/staging/rtl8723au/os_dep/os_intfs.c b/drivers/staging/rtl8723au/os_dep/os_intfs.c
index 57eca7a4567..4e32003a443 100644
--- a/drivers/staging/rtl8723au/os_dep/os_intfs.c
+++ b/drivers/staging/rtl8723au/os_dep/os_intfs.c
@@ -20,10 +20,8 @@
 #include <recv_osdep.h>
 #include <hal_intf.h>
 #include <rtw_version.h>
-#include <ethernet.h>
 
-#include <usb_osintf.h>
-#include <linux/version.h>
+#include <rtl8723a_hal.h>
 
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("Realtek Wireless Lan Driver");
@@ -66,10 +64,10 @@ static int rtw_acm_method;/*  0:By SW 1:By HW. */
 static int rtw_wmm_enable = 1;/*  default is set to enable the wmm. */
 static int rtw_uapsd_enable;
 
-int rtw_ht_enable23A = 1;
+static int rtw_ht_enable = 1;
 /* 0 :diable, bit(0): enable 2.4g, bit(1): enable 5g */
-int rtw_cbw40_enable23A = 3;
-int rtw_ampdu_enable23A = 1;/* for enable tx_ampdu */
+static int rtw_cbw40_enable = 3;
+static int rtw_ampdu_enable = 1;/* for enable tx_ampdu */
 /*  0: disable, bit(0):enable 2.4g, bit(1):enable 5g, default is set to enable
  * 2.4GHZ for IOT issue with bufflao's AP at 5GHZ
  */
@@ -130,9 +128,9 @@ module_param(rtw_wmm_enable, int, 0644);
 module_param(rtw_vrtl_carrier_sense, int, 0644);
 module_param(rtw_vcs_type, int, 0644);
 module_param(rtw_busy_thresh, int, 0644);
-module_param(rtw_ht_enable23A, int, 0644);
-module_param(rtw_cbw40_enable23A, int, 0644);
-module_param(rtw_ampdu_enable23A, int, 0644);
+module_param(rtw_ht_enable, int, 0644);
+module_param(rtw_cbw40_enable, int, 0644);
+module_param(rtw_ampdu_enable, int, 0644);
 module_param(rtw_rx_stbc, int, 0644);
 module_param(rtw_ampdu_amsdu, int, 0644);
 
@@ -172,10 +170,10 @@ MODULE_PARM_DESC(debug, "Set debug level (1-9) (default 1)");
 
 static int netdev_close(struct net_device *pnetdev);
 
-static uint loadparam(struct rtw_adapter *padapter,  struct net_device *pnetdev)
+static int loadparam(struct rtw_adapter *padapter,  struct net_device *pnetdev)
 {
 	struct registry_priv  *registry_par = &padapter->registrypriv;
-	uint status = _SUCCESS;
+	int status = _SUCCESS;
 
 	GlobalDebugLevel23A = rtw_debug;
 	registry_par->chip_version = (u8)rtw_chip_version;
@@ -203,9 +201,9 @@ static uint loadparam(struct rtw_adapter *padapter,  struct net_device *pnetdev)
 	 /* UAPSD */
 	registry_par->wmm_enable = (u8)rtw_wmm_enable;
 	registry_par->uapsd_enable = (u8)rtw_uapsd_enable;
-	registry_par->ht_enable = (u8)rtw_ht_enable23A;
-	registry_par->cbw40_enable = (u8)rtw_cbw40_enable23A;
-	registry_par->ampdu_enable = (u8)rtw_ampdu_enable23A;
+	registry_par->ht_enable = (u8)rtw_ht_enable;
+	registry_par->cbw40_enable = (u8)rtw_cbw40_enable;
+	registry_par->ampdu_enable = (u8)rtw_ampdu_enable;
 	registry_par->rx_stbc = (u8)rtw_rx_stbc;
 	registry_par->ampdu_amsdu = (u8)rtw_ampdu_amsdu;
 	registry_par->lowrate_two_xmit = (u8)rtw_lowrate_two_xmit;
@@ -274,9 +272,9 @@ static struct net_device_stats *rtw_net_get_stats(struct net_device *pnetdev)
 static const u16 rtw_1d_to_queue[8] = { 2, 3, 3, 2, 1, 1, 0, 0 };
 
 /* Given a data frame determine the 802.1p/1d tag to use. */
-static unsigned int rtw_classify8021d(struct sk_buff *skb)
+static u32 rtw_classify8021d(struct sk_buff *skb)
 {
-	unsigned int dscp;
+	u32 dscp;
 
 	/* skb->priority values from 256->263 are magic values to
 	 * directly indicate a specific 802.1d priority.  This is used
@@ -312,14 +310,14 @@ static u16 rtw_select_queue(struct net_device *dev, struct sk_buff *skb,
 u16 rtw_recv_select_queue23a(struct sk_buff *skb)
 {
 	struct iphdr *piphdr;
+	struct ethhdr *eth = (struct ethhdr *)skb->data;
 	unsigned int dscp;
-	u16 eth_type;
+	u16 eth_type = get_unaligned_be16(&eth->h_proto);
 	u32 priority;
 	u8 *pdata = skb->data;
 
-	memcpy(&eth_type, pdata + (ETH_ALEN << 1), 2);
 	switch (eth_type) {
-	case htons(ETH_P_IP):
+	case ETH_P_IP:
 		piphdr = (struct iphdr *)(pdata + ETH_HLEN);
 		dscp = piphdr->tos & 0xfc;
 		priority = dscp >> 5;
@@ -378,42 +376,13 @@ struct net_device *rtw_init_netdev23a(struct rtw_adapter *old_padapter)
 	return pnetdev;
 }
 
-u32 rtw_start_drv_threads23a(struct rtw_adapter *padapter)
-{
-	u32 _status = _SUCCESS;
-
-	RT_TRACE(_module_os_intfs_c_, _drv_info_,
-		 ("+rtw_start_drv_threads23a\n"));
-	padapter->cmdThread = kthread_run(rtw_cmd_thread23a, padapter,
-					  "RTW_CMD_THREAD");
-	if (IS_ERR(padapter->cmdThread)) {
-		_status = _FAIL;
-	} else {
-		/* wait for cmd_thread to run */
-		down(&padapter->cmdpriv.terminate_cmdthread_sema);
-	}
-	rtw_hal_start_thread23a(padapter);
-	return _status;
-}
-
-void rtw_stop_drv_threads23a(struct rtw_adapter *padapter)
-{
-	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("+rtw_stop_drv_threads23a\n"));
-
-	/* Below is to termindate rtw_cmd_thread23a & event_thread... */
-	up(&padapter->cmdpriv.cmd_queue_sema);
-	if (padapter->cmdThread)
-		down(&padapter->cmdpriv.terminate_cmdthread_sema);
-	rtw_hal_stop_thread23a(padapter);
-}
-
-static u8 rtw_init_default_value(struct rtw_adapter *padapter)
+static int rtw_init_default_value(struct rtw_adapter *padapter)
 {
 	struct registry_priv *pregistrypriv = &padapter->registrypriv;
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
-	u8 ret = _SUCCESS;
+	int ret = _SUCCESS;
 
 	/* xmit_priv */
 	pxmitpriv->vcs_setting = pregistrypriv->vrtl_carrier_sense;
@@ -430,15 +399,15 @@ static u8 rtw_init_default_value(struct rtw_adapter *padapter)
 	pmlmepriv->htpriv.ampdu_enable = false;/* set to disabled */
 
 	/* security_priv */
-	psecuritypriv->binstallGrpkey = _FAIL;
+	psecuritypriv->binstallGrpkey = 0;
 
 	 /* open system */
 	psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_Open;
-	psecuritypriv->dot11PrivacyAlgrthm = _NO_PRIVACY_;
+	psecuritypriv->dot11PrivacyAlgrthm = 0;
 
 	psecuritypriv->dot11PrivacyKeyIndex = 0;
 
-	psecuritypriv->dot118021XGrpPrivacy = _NO_PRIVACY_;
+	psecuritypriv->dot118021XGrpPrivacy = 0;
 	psecuritypriv->dot118021XGrpKeyid = 1;
 
 	psecuritypriv->ndisauthtype = Ndis802_11AuthModeOpen;
@@ -449,30 +418,24 @@ static u8 rtw_init_default_value(struct rtw_adapter *padapter)
 	rtw_update_registrypriv_dev_network23a(padapter);
 
 	/* hal_priv */
-	rtw_hal_def_value_init23a(padapter);
+	rtl8723a_init_default_value(padapter);
 
 	/* misc. */
 	padapter->bReadPortCancel = false;
 	padapter->bWritePortCancel = false;
-	padapter->bRxRSSIDisplay = 0;
 	padapter->bNotifyChannelChange = 0;
-#ifdef CONFIG_8723AU_P2P
-	padapter->bShowGetP2PState = 1;
-#endif
 	return ret;
 }
 
-u8 rtw_reset_drv_sw23a(struct rtw_adapter *padapter)
+int rtw_reset_drv_sw23a(struct rtw_adapter *padapter)
 {
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct pwrctrl_priv *pwrctrlpriv = &padapter->pwrctrlpriv;
-	u8 ret8 = _SUCCESS;
 
 	/* hal_priv */
-	rtw_hal_def_value_init23a(padapter);
+	rtl8723a_init_default_value(padapter);
 	padapter->bReadPortCancel = false;
 	padapter->bWritePortCancel = false;
-	padapter->bRxRSSIDisplay = 0;
 	pmlmepriv->scan_interval = SCAN_INTERVAL;/*  30*2 sec = 60sec */
 
 	padapter->xmitpriv.tx_pkts = 0;
@@ -482,23 +445,23 @@ u8 rtw_reset_drv_sw23a(struct rtw_adapter *padapter)
 
 	_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY | _FW_UNDER_LINKING);
 
-	rtw_hal_sreset_reset23a_value23a(padapter);
+	rtw_sreset_reset_value(padapter);
 	pwrctrlpriv->pwr_state_check_cnts = 0;
 
 	/* mlmeextpriv */
 	padapter->mlmeextpriv.sitesurvey_res.state = SCAN_DISABLE;
 
 	rtw_set_signal_stat_timer(&padapter->recvpriv);
-	return ret8;
+	return _SUCCESS;
 }
 
-u8 rtw_init_drv_sw23a(struct rtw_adapter *padapter)
+int rtw_init_drv_sw23a(struct rtw_adapter *padapter)
 {
-	u8 ret8 = _SUCCESS;
+	int ret8 = _SUCCESS;
 
 	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("+rtw_init_drv_sw23a\n"));
 
-	if ((rtw_init_cmd_priv23a(&padapter->cmdpriv)) == _FAIL) {
+	if (rtw_init_cmd_priv23a(&padapter->cmdpriv) == _FAIL) {
 		RT_TRACE(_module_os_intfs_c_, _drv_err_,
 			 ("\n Can't init cmd_priv\n"));
 		ret8 = _FAIL;
@@ -521,17 +484,6 @@ u8 rtw_init_drv_sw23a(struct rtw_adapter *padapter)
 		goto exit;
 	}
 
-#ifdef CONFIG_8723AU_P2P
-	rtw_init_wifidirect_timers23a(padapter);
-	init_wifidirect_info23a(padapter, P2P_ROLE_DISABLE);
-	reset_global_wifidirect_info23a(padapter);
-	rtw_init_cfg80211_wifidirect_info(padapter);
-#ifdef CONFIG_8723AU_P2P
-	if (rtw_init_wifi_display_info(padapter) == _FAIL)
-		RT_TRACE(_module_os_intfs_c_, _drv_err_,
-			 ("\n Can't init init_wifi_display_info\n"));
-#endif
-#endif /* CONFIG_8723AU_P2P */
 
 	if (init_mlme_ext_priv23a(padapter) == _FAIL) {
 		RT_TRACE(_module_os_intfs_c_, _drv_err_,
@@ -566,10 +518,9 @@ u8 rtw_init_drv_sw23a(struct rtw_adapter *padapter)
 
 	ret8 = rtw_init_default_value(padapter);
 
-	rtw_hal_dm_init23a(padapter);
-	rtw_hal_sw_led_init23a(padapter);
+	rtl8723a_init_dm_priv(padapter);
 
-	rtw_hal_sreset_init23a(padapter);
+	rtw_sreset_init(padapter);
 
 exit:
 
@@ -579,89 +530,60 @@ exit:
 
 void rtw_cancel_all_timer23a(struct rtw_adapter *padapter)
 {
-	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("+rtw_cancel_all_timer23a\n"));
+	RT_TRACE(_module_os_intfs_c_, _drv_info_,
+		 ("+rtw_cancel_all_timer23a\n"));
 
 	del_timer_sync(&padapter->mlmepriv.assoc_timer);
 	RT_TRACE(_module_os_intfs_c_, _drv_info_,
-		 ("rtw_cancel_all_timer23a:cancel association timer complete!\n"));
+		 ("%s:cancel association timer complete!\n", __func__));
 
 	del_timer_sync(&padapter->mlmepriv.scan_to_timer);
 	RT_TRACE(_module_os_intfs_c_, _drv_info_,
-		 ("rtw_cancel_all_timer23a:cancel scan_to_timer!\n"));
+		 ("%s:cancel scan_to_timer!\n", __func__));
 
 	del_timer_sync(&padapter->mlmepriv.dynamic_chk_timer);
 	RT_TRACE(_module_os_intfs_c_, _drv_info_,
-		 ("rtw_cancel_all_timer23a:cancel dynamic_chk_timer!\n"));
+		 ("%s:cancel dynamic_chk_timer!\n", __func__));
 
-	/*  cancel sw led timer */
-	rtw_hal_sw_led_deinit23a(padapter);
 	RT_TRACE(_module_os_intfs_c_, _drv_info_,
-		 ("rtw_cancel_all_timer23a:cancel DeInitSwLeds!\n"));
+		 ("%s:cancel DeInitSwLeds!\n", __func__));
 
 	del_timer_sync(&padapter->pwrctrlpriv.pwr_state_check_timer);
 
-#ifdef CONFIG_8723AU_P2P
-	del_timer_sync(&padapter->cfg80211_wdinfo.remain_on_ch_timer);
-#endif /* CONFIG_8723AU_P2P */
-
 	del_timer_sync(&padapter->mlmepriv.set_scan_deny_timer);
 	rtw_clear_scan_deny(padapter);
 	RT_TRACE(_module_os_intfs_c_, _drv_info_,
-		 ("rtw_cancel_all_timer23a:cancel set_scan_deny_timer!\n"));
+		 ("%s:cancel set_scan_deny_timer!\n", __func__));
 
 	del_timer_sync(&padapter->recvpriv.signal_stat_timer);
 	/* cancel dm timer */
-	rtw_hal_dm_deinit23a(padapter);
+	rtl8723a_deinit_dm_priv(padapter);
 }
 
-u8 rtw_free_drv_sw23a(struct rtw_adapter *padapter)
+int rtw_free_drv_sw23a(struct rtw_adapter *padapter)
 {
-#ifdef CONFIG_8723AU_P2P
-	struct wifidirect_info *pwdinfo;
-#endif
-
 	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("==>rtw_free_drv_sw23a"));
 
-	/* we can call rtw_p2p_enable23a here, but:
-	 *  1. rtw_p2p_enable23a may have IO operation
-	 *  2. rtw_p2p_enable23a is bundled with wext interface
-	 */
-#ifdef CONFIG_8723AU_P2P
-	pwdinfo = &padapter->wdinfo;
-	if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) {
-		del_timer_sync(&pwdinfo->find_phase_timer);
-		del_timer_sync(&pwdinfo->restore_p2p_state_timer);
-		del_timer_sync(&pwdinfo->pre_tx_scan_timer);
-		rtw_p2p_set_state(pwdinfo, P2P_STATE_NONE);
-	}
-#endif
-
 	free_mlme_ext_priv23a(&padapter->mlmeextpriv);
 
-	rtw_free_cmd_priv23a(&padapter->cmdpriv);
-
 	rtw_free_evt_priv23a(&padapter->evtpriv);
 
 	rtw_free_mlme_priv23a(&padapter->mlmepriv);
 
 	_rtw_free_xmit_priv23a(&padapter->xmitpriv);
 
-	_rtw_free_sta_priv23a(&padapter->stapriv);/* will free bcmc_stainfo here */
+	/* will free bcmc_stainfo here */
+	_rtw_free_sta_priv23a(&padapter->stapriv);
 
 	_rtw_free_recv_priv23a(&padapter->recvpriv);
 
 	rtw_free_pwrctrl_priv(padapter);
 
-	rtw_hal_free_data23a(padapter);
+	kfree(padapter->HalData);
+	padapter->HalData = NULL;
 
 	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("<== rtw_free_drv_sw23a\n"));
 
-	/* free the old_pnetdev */
-	if (padapter->rereg_nd_name_priv.old_pnetdev) {
-		free_netdev(padapter->rereg_nd_name_priv.old_pnetdev);
-		padapter->rereg_nd_name_priv.old_pnetdev = NULL;
-	}
-
 	/*  clear pbuddy_adapter to avoid access wrong pointer. */
 	if (padapter->pbuddy_adapter != NULL)
 		padapter->pbuddy_adapter->pbuddy_adapter = NULL;
@@ -681,7 +603,7 @@ static int _rtw_drv_register_netdev(struct rtw_adapter *padapter, char *name)
 
 	/* Tell the network stack we exist */
 	if (register_netdev(pnetdev)) {
-		DBG_8723A(FUNC_NDEV_FMT "Failed!\n", FUNC_NDEV_ARG(pnetdev));
+		DBG_8723A("%s(%s): Failed!\n", __func__, pnetdev->name);
 		ret = _FAIL;
 		goto error_register_netdev;
 	}
@@ -704,26 +626,30 @@ int rtw_drv_register_netdev(struct rtw_adapter *if1)
 	struct dvobj_priv *dvobj = if1->dvobj;
 	int i, status = _SUCCESS;
 
-	if (dvobj->iface_nums < IFACE_ID_MAX) {
-		for (i = 0; i < dvobj->iface_nums; i++) {
-			struct rtw_adapter *padapter = dvobj->padapters[i];
-
-			if (padapter) {
-				char *name;
-
-				if (padapter->iface_id == IFACE_ID0)
-					name = if1->registrypriv.ifname;
-				else if (padapter->iface_id == IFACE_ID1)
-					name = if1->registrypriv.if2name;
-				else
-					name = "wlan%d";
-				status = _rtw_drv_register_netdev(padapter,
-								  name);
-				if (status != _SUCCESS)
-					break;
-			}
+	if (dvobj->iface_nums >= IFACE_ID_MAX) {
+		status = _FAIL; /* -EINVAL */
+		goto exit;
+	}
+
+	for (i = 0; i < dvobj->iface_nums; i++) {
+		struct rtw_adapter *padapter = dvobj->padapters[i];
+
+		if (padapter) {
+			char *name;
+
+			if (padapter->iface_id == IFACE_ID0)
+				name = if1->registrypriv.ifname;
+			else if (padapter->iface_id == IFACE_ID1)
+				name = if1->registrypriv.if2name;
+			else
+				name = "wlan%d";
+			status = _rtw_drv_register_netdev(padapter, name);
+			if (status != _SUCCESS)
+				break;
 		}
 	}
+
+exit:
 	return status;
 }
 
@@ -732,7 +658,7 @@ int netdev_open23a(struct net_device *pnetdev)
 	struct rtw_adapter *padapter = netdev_priv(pnetdev);
 	struct pwrctrl_priv *pwrctrlpriv;
 	int ret = 0;
-	uint status;
+	int status;
 
 	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("+871x_drv - dev_open\n"));
 	DBG_8723A("+871x_drv - drv_open, bup =%d\n", padapter->bup);
@@ -760,19 +686,12 @@ int netdev_open23a(struct net_device *pnetdev)
 		DBG_8723A("MAC Address = "MAC_FMT"\n",
 			  MAC_ARG(pnetdev->dev_addr));
 
-		status = rtw_start_drv_threads23a(padapter);
-		if (status == _FAIL) {
-			DBG_8723A("Initialize driver software resource Failed!\n");
-			goto netdev_open23a_error;
-		}
-
 		if (init_hw_mlme_ext23a(padapter) == _FAIL) {
 			DBG_8723A("can't init mlme_ext_priv\n");
 			goto netdev_open23a_error;
 		}
 
-		if (padapter->intf_start)
-			padapter->intf_start(padapter);
+		rtl8723au_inirp_init(padapter);
 
 		rtw_cfg80211_init_wiphy(padapter);
 
@@ -816,7 +735,7 @@ netdev_open23a_error:
 	goto exit;
 }
 
-static int  ips_netdrv_open(struct rtw_adapter *padapter)
+static int ips_netdrv_open(struct rtw_adapter *padapter)
 {
 	int status = _SUCCESS;
 
@@ -834,8 +753,7 @@ static int  ips_netdrv_open(struct rtw_adapter *padapter)
 		goto netdev_open23a_error;
 	}
 
-	if (padapter->intf_start)
-		padapter->intf_start(padapter);
+	rtl8723au_inirp_init(padapter);
 
 	rtw_set_pwr_state_check_timer(&padapter->pwrctrlpriv);
 	mod_timer(&padapter->mlmepriv.dynamic_chk_timer,
@@ -887,10 +805,9 @@ void rtw_ips_pwr_down23a(struct rtw_adapter *padapter)
 
 void rtw_ips_dev_unload23a(struct rtw_adapter *padapter)
 {
-	rtw_hal_set_hwreg23a(padapter, HW_VAR_FIFO_CLEARN_UP, NULL);
+	rtl8723a_fifo_cleanup(padapter);
 
-	if (padapter->intf_stop)
-		padapter->intf_stop(padapter);
+	rtl8723a_usb_intf_stop(padapter);
 
 	/* s5. */
 	if (!padapter->bSurpriseRemoved)
@@ -916,15 +833,11 @@ static int netdev_close(struct net_device *pnetdev)
 
 	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("+871x_drv - drv_close\n"));
 
-	if (padapter->pwrctrlpriv.bInternalAutoSuspend) {
-		if (padapter->pwrctrlpriv.rf_pwrstate == rf_off)
-			padapter->pwrctrlpriv.ps_flag = true;
-	}
 	padapter->net_closed = true;
 
 	if (padapter->pwrctrlpriv.rf_pwrstate == rf_on) {
-		DBG_8723A("(2)871x_drv - drv_close, bup =%d, hw_init_completed =%d\n",
-			  padapter->bup,
+		DBG_8723A("(2)871x_drv - drv_close, bup =%d, "
+			  "hw_init_completed =%d\n", padapter->bup,
 			  padapter->hw_init_completed);
 
 		/* s1. */
@@ -941,20 +854,12 @@ static int netdev_close(struct net_device *pnetdev)
 		/* s2-3. */
 		rtw_free_assoc_resources23a(padapter, 1);
 		/* s2-4. */
-		rtw_free_network_queue23a(padapter, true);
+		rtw_free_network_queue23a(padapter);
 		/*  Close LED */
 		rtw_led_control(padapter, LED_CTL_POWER_OFF);
 	}
 
-#ifdef CONFIG_8723AU_P2P
-	if (wdev_to_priv(padapter->rtw_wdev)->p2p_enabled)
-		wdev_to_priv(padapter->rtw_wdev)->p2p_enabled = false;
-	rtw_p2p_enable23a(padapter, P2P_ROLE_DISABLE);
-#endif /* CONFIG_8723AU_P2P */
-
 	rtw_scan_abort23a(padapter);
-	 /* set this at the end */
-	padapter->rtw_wdev->iftype = NL80211_IFTYPE_MONITOR;
 
 	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("-871x_drv - drv_close\n"));
 	DBG_8723A("-871x_drv - drv_close, bup =%d\n", padapter->bup);
@@ -964,7 +869,13 @@ static int netdev_close(struct net_device *pnetdev)
 
 void rtw_ndev_destructor(struct net_device *ndev)
 {
-	DBG_8723A(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(ndev));
+	DBG_8723A("%s(%s)\n", __func__, ndev->name);
 	kfree(ndev->ieee80211_ptr);
 	free_netdev(ndev);
 }
+
+void _rtw_init_queue23a(struct rtw_queue *pqueue)
+{
+	INIT_LIST_HEAD(&pqueue->queue);
+	spin_lock_init(&pqueue->lock);
+}
diff --git a/drivers/staging/rtl8723au/os_dep/osdep_service.c b/drivers/staging/rtl8723au/os_dep/osdep_service.c
deleted file mode 100644
index 97fc27dce19..00000000000
--- a/drivers/staging/rtl8723au/os_dep/osdep_service.c
+++ /dev/null
@@ -1,175 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
- *
- * 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.
- *
- * 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 _OSDEP_SERVICE_C_
-
-#include <osdep_service.h>
-#include <drv_types.h>
-#include <recv_osdep.h>
-#include <linux/vmalloc.h>
-
-#define RT_TAG	('1178')
-
-/*
-* Translate the OS dependent @param error_code to OS independent RTW_STATUS_CODE23a
-* @return: one of RTW_STATUS_CODE23a
-*/
-inline int RTW_STATUS_CODE23a(int error_code)
-{
-	if (error_code >= 0)
-		return _SUCCESS;
-	return _FAIL;
-}
-
-inline u8 *_rtw_vmalloc(u32 sz)
-{
-	u8	*pbuf;
-	pbuf = vmalloc(sz);
-
-	return pbuf;
-}
-
-inline u8 *_rtw_zvmalloc(u32 sz)
-{
-	u8	*pbuf;
-	pbuf = _rtw_vmalloc(sz);
-	if (pbuf != NULL)
-		memset(pbuf, 0, sz);
-
-	return pbuf;
-}
-
-inline void _rtw_vmfree(u8 *pbuf, u32 sz)
-{
-	vfree(pbuf);
-}
-
-void _rtw_init_queue23a(struct rtw_queue *pqueue)
-{
-	INIT_LIST_HEAD(&pqueue->queue);
-	spin_lock_init(&pqueue->lock);
-}
-
-u32 _rtw_queue_empty23a(struct rtw_queue *pqueue)
-{
-	if (list_empty(&pqueue->queue))
-		return true;
-	else
-		return false;
-}
-
-u64 rtw_modular6423a(u64 x, u64 y)
-{
-	return do_div(x, y);
-}
-
-u64 rtw_division6423a(u64 x, u64 y)
-{
-	do_div(x, y);
-	return x;
-}
-
-/* rtw_cbuf_full23a - test if cbuf is full
- * @cbuf: pointer of struct rtw_cbuf
- *
- * Returns: true if cbuf is full
- */
-inline bool rtw_cbuf_full23a(struct rtw_cbuf *cbuf)
-{
-	return (cbuf->write == cbuf->read-1) ? true : false;
-}
-
-/* rtw_cbuf_empty23a - test if cbuf is empty
- * @cbuf: pointer of struct rtw_cbuf
- *
- * Returns: true if cbuf is empty
- */
-inline bool rtw_cbuf_empty23a(struct rtw_cbuf *cbuf)
-{
-	return (cbuf->write == cbuf->read) ? true : false;
-}
-
-/**
- * rtw_cbuf_push23a - push a pointer into cbuf
- * @cbuf: pointer of struct rtw_cbuf
- * @buf: pointer to push in
- *
- * Lock free operation, be careful of the use scheme
- * Returns: true push success
- */
-bool rtw_cbuf_push23a(struct rtw_cbuf *cbuf, void *buf)
-{
-	if (rtw_cbuf_full23a(cbuf))
-		return _FAIL;
-
-	if (0)
-		DBG_8723A("%s on %u\n", __func__, cbuf->write);
-	cbuf->bufs[cbuf->write] = buf;
-	cbuf->write = (cbuf->write+1)%cbuf->size;
-
-	return _SUCCESS;
-}
-
-/**
- * rtw_cbuf_pop23a - pop a pointer from cbuf
- * @cbuf: pointer of struct rtw_cbuf
- *
- * Lock free operation, be careful of the use scheme
- * Returns: pointer popped out
- */
-void *rtw_cbuf_pop23a(struct rtw_cbuf *cbuf)
-{
-	void *buf;
-	if (rtw_cbuf_empty23a(cbuf))
-		return NULL;
-
-	if (0)
-		DBG_8723A("%s on %u\n", __func__, cbuf->read);
-	buf = cbuf->bufs[cbuf->read];
-	cbuf->read = (cbuf->read+1)%cbuf->size;
-
-	return buf;
-}
-
-/**
- * rtw_cbuf_alloc23a - allocte a rtw_cbuf with given size and do initialization
- * @size: size of pointer
- *
- * Returns: pointer of srtuct rtw_cbuf, NULL for allocation failure
- */
-struct rtw_cbuf *rtw_cbuf_alloc23a(u32 size)
-{
-	struct rtw_cbuf *cbuf;
-
-	cbuf = kmalloc(sizeof(*cbuf) + sizeof(void *)*size, GFP_KERNEL);
-
-	if (cbuf) {
-		cbuf->write = 0;
-		cbuf->read = 0;
-		cbuf->size = size;
-	}
-
-	return cbuf;
-}
-
-/**
- * rtw_cbuf_free - free the given rtw_cbuf
- * @cbuf: pointer of struct rtw_cbuf to free
- */
-void rtw_cbuf_free(struct rtw_cbuf *cbuf)
-{
-	kfree(cbuf);
-}
diff --git a/drivers/staging/rtl8723au/os_dep/recv_linux.c b/drivers/staging/rtl8723au/os_dep/recv_linux.c
index 84402a589f2..bcd970baf40 100644
--- a/drivers/staging/rtl8723au/os_dep/recv_linux.c
+++ b/drivers/staging/rtl8723au/os_dep/recv_linux.c
@@ -21,50 +21,9 @@
 #include <recv_osdep.h>
 
 #include <osdep_intf.h>
-#include <ethernet.h>
 
 #include <usb_ops.h>
 
-/* alloc os related resource in struct recv_frame */
-int rtw_os_recv_resource_alloc23a(struct rtw_adapter *padapter,
-			       struct recv_frame *precvframe)
-{
-	int res = _SUCCESS;
-
-	precvframe->pkt = NULL;
-
-	return res;
-}
-
-/* alloc os related resource in struct recv_buf */
-int rtw_os_recvbuf_resource_alloc23a(struct rtw_adapter *padapter,
-				  struct recv_buf *precvbuf)
-{
-	int res = _SUCCESS;
-
-	precvbuf->purb = usb_alloc_urb(0, GFP_KERNEL);
-	if (precvbuf->purb == NULL)
-		res = _FAIL;
-
-	precvbuf->pskb = NULL;
-
-	return res;
-}
-
-/* free os related resource in struct recv_buf */
-int rtw_os_recvbuf_resource_free23a(struct rtw_adapter *padapter,
-				 struct recv_buf *precvbuf)
-{
-	int ret = _SUCCESS;
-
-	usb_free_urb(precvbuf->purb);
-
-	if (precvbuf->pskb)
-		dev_kfree_skb_any(precvbuf->pskb);
-
-	return ret;
-}
-
 void rtw_handle_tkip_mic_err23a(struct rtw_adapter *padapter, u8 bgroup)
 {
 	enum nl80211_key_type key_type = 0;
@@ -110,21 +69,14 @@ void rtw_handle_tkip_mic_err23a(struct rtw_adapter *padapter, u8 bgroup)
 	wrqu.data.length = sizeof(ev);
 }
 
-void rtw_hostapd_mlme_rx23a(struct rtw_adapter *padapter,
-			 struct recv_frame *precv_frame)
-{
-}
-
 int rtw_recv_indicatepkt23a(struct rtw_adapter *padapter,
 			 struct recv_frame *precv_frame)
 {
 	struct recv_priv *precvpriv;
-	struct rtw_queue *pfree_recv_queue;
 	struct sk_buff *skb;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
-	precvpriv = &(padapter->recvpriv);
-	pfree_recv_queue = &(precvpriv->free_recv_queue);
+	precvpriv = &padapter->recvpriv;
 
 	skb = precv_frame->pkt;
 	if (!skb) {
@@ -193,7 +145,7 @@ _recv_indicatepkt_end:
 
 	precv_frame->pkt = NULL; /*  pointers to NULL before rtw_free_recvframe23a() */
 
-	rtw_free_recvframe23a(precv_frame, pfree_recv_queue);
+	rtw_free_recvframe23a(precv_frame);
 
 	RT_TRACE(_module_recv_osdep_c_, _drv_info_,
 		 ("\n rtw_recv_indicatepkt23a :after netif_rx!!!!\n"));
@@ -201,22 +153,10 @@ _recv_indicatepkt_end:
 
 _recv_indicatepkt_drop:
 
-	 rtw_free_recvframe23a(precv_frame, pfree_recv_queue);
+	 rtw_free_recvframe23a(precv_frame);
 	 return _FAIL;
 }
 
-void rtw_os_read_port23a(struct rtw_adapter *padapter, struct recv_buf *precvbuf)
-{
-	struct recv_priv *precvpriv = &padapter->recvpriv;
-
-	/* free skb in recv_buf */
-	dev_kfree_skb_any(precvbuf->pskb);
-
-	precvbuf->pskb = NULL;
-
-	rtw_read_port(padapter, precvpriv->ff_hwaddr, 0, precvbuf);
-}
-
 void rtw_init_recv_timer23a(struct recv_reorder_ctrl *preorder_ctrl)
 {
 	setup_timer(&preorder_ctrl->reordering_ctrl_timer,
diff --git a/drivers/staging/rtl8723au/os_dep/usb_intf.c b/drivers/staging/rtl8723au/os_dep/usb_intf.c
index 612806e0de2..8b25c1aa202 100644
--- a/drivers/staging/rtl8723au/os_dep/usb_intf.c
+++ b/drivers/staging/rtl8723au/os_dep/usb_intf.c
@@ -21,10 +21,8 @@
 #include <hal_intf.h>
 #include <rtw_version.h>
 #include <osdep_intf.h>
-#include <usb_vendor_req.h>
 #include <usb_ops.h>
-#include <usb_osintf.h>
-#include <usb_hal.h>
+#include <rtl8723a_hal.h>
 
 static int rtw_suspend(struct usb_interface *intf, pm_message_t message);
 static int rtw_resume(struct usb_interface *intf);
@@ -101,15 +99,15 @@ static inline int RT_usb_endpoint_num(const struct usb_endpoint_descriptor *epd)
 	return epd->bEndpointAddress & USB_ENDPOINT_NUMBER_MASK;
 }
 
-static u8 rtw_init_intf_priv(struct dvobj_priv *dvobj)
+static int rtw_init_intf_priv(struct dvobj_priv *dvobj)
 {
-	u8 rst = _SUCCESS;
+	int rst = _SUCCESS;
 
 	mutex_init(&dvobj->usb_vendor_req_mutex);
 	dvobj->usb_alloc_vendor_req_buf = kzalloc(MAX_USB_IO_CTL_SIZE,
 						  GFP_KERNEL);
 	if (dvobj->usb_alloc_vendor_req_buf == NULL) {
-		DBG_8723A("alloc usb_vendor_req_buf failed... /n");
+		DBG_8723A("alloc usb_vendor_req_buf failed...\n");
 		rst = _FAIL;
 		goto exit;
 	}
@@ -119,9 +117,9 @@ exit:
 	return rst;
 }
 
-static u8 rtw_deinit_intf_priv(struct dvobj_priv *dvobj)
+static int rtw_deinit_intf_priv(struct dvobj_priv *dvobj)
 {
-	u8 rst = _SUCCESS;
+	int rst = _SUCCESS;
 
 	kfree(dvobj->usb_alloc_vendor_req_buf);
 
@@ -229,7 +227,6 @@ static struct dvobj_priv *usb_dvobj_init(struct usb_interface *usb_intf)
 		goto free_dvobj;
 	}
 	/* 3 misc */
-	sema_init(&(pdvobjpriv->usb_suspend_sema), 0);
 	rtw_reset_continual_urb_error(pdvobjpriv);
 	usb_get_dev(pusbd);
 	status = _SUCCESS;
@@ -278,21 +275,7 @@ static void usb_dvobj_deinit(struct usb_interface *usb_intf)
 	usb_put_dev(interface_to_usbdev(usb_intf));
 }
 
-static void decide_chip_type_by_usb_device_id(struct rtw_adapter *padapter,
-					      const struct usb_device_id *pdid)
-{
-	padapter->chip_type = NULL_CHIP_TYPE;
-	hal_set_hw_type(padapter);
-}
-
-static void usb_intf_start(struct rtw_adapter *padapter)
-{
-	RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("+usb_intf_start\n"));
-	rtw_hal_inirp_init23a(padapter);
-	RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("-usb_intf_start\n"));
-}
-
-static void usb_intf_stop(struct rtw_adapter *padapter)
+void rtl8723a_usb_intf_stop(struct rtw_adapter *padapter)
 {
 	RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("+usb_intf_stop\n"));
 
@@ -306,10 +289,10 @@ static void usb_intf_stop(struct rtw_adapter *padapter)
 	}
 
 	/* cancel in irp */
-	rtw_hal_inirp_deinit23a(padapter);
+	rtl8723au_inirp_deinit(padapter);
 
 	/* cancel out irp */
-	rtw_write_port_cancel(padapter);
+	rtl8723au_write_port_cancel(padapter);
 
 	/* todo:cancel other irps */
 	RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("-usb_intf_stop\n"));
@@ -328,12 +311,10 @@ static void rtw_dev_unload(struct rtw_adapter *padapter)
 					RTW_SCTX_DONE_DRV_STOP);
 
 		/* s3. */
-		if (padapter->intf_stop)
-			padapter->intf_stop(padapter);
+		rtl8723a_usb_intf_stop(padapter);
 
 		/* s4. */
-		if (!padapter->pwrctrlpriv.bInternalAutoSuspend)
-			rtw_stop_drv_threads23a(padapter);
+		flush_workqueue(padapter->cmdpriv.wq);
 
 		/* s5. */
 		if (!padapter->bSurpriseRemoved) {
@@ -353,6 +334,7 @@ int rtw_hw_suspend23a(struct rtw_adapter *padapter)
 {
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
 	struct net_device *pnetdev = padapter->pnetdev;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
 	if ((!padapter->bup) || (padapter->bDriverStopped) ||
 	    (padapter->bSurpriseRemoved)) {
@@ -380,26 +362,22 @@ int rtw_hw_suspend23a(struct rtw_adapter *padapter)
 
 		/* s2-2.  indicate disconnect to os */
 		/* rtw_indicate_disconnect23a(padapter); */
-		{
-			struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+		if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+			_clr_fwstate_(pmlmepriv, _FW_LINKED);
 
-			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
-				_clr_fwstate_(pmlmepriv, _FW_LINKED);
+			rtw_led_control(padapter, LED_CTL_NO_LINK);
 
-				rtw_led_control(padapter, LED_CTL_NO_LINK);
+			rtw_os_indicate_disconnect23a(padapter);
 
-				rtw_os_indicate_disconnect23a(padapter);
-
-				/* donnot enqueue cmd */
-				rtw_lps_ctrl_wk_cmd23a(padapter,
-						    LPS_CTRL_DISCONNECT, 0);
-			}
+			/* donnot enqueue cmd */
+			rtw_lps_ctrl_wk_cmd23a(padapter,
+					       LPS_CTRL_DISCONNECT, 0);
 		}
 		/* s2-3. */
 		rtw_free_assoc_resources23a(padapter, 1);
 
 		/* s2-4. */
-		rtw_free_network_queue23a(padapter, true);
+		rtw_free_network_queue23a(padapter);
 		rtw_ips_dev_unload23a(padapter);
 		pwrpriv->rf_pwrstate = rf_off;
 		pwrpriv->bips_processing = false;
@@ -503,13 +481,14 @@ static int rtw_suspend(struct usb_interface *pusb_intf, pm_message_t message)
 	/* s2-3. */
 	rtw_free_assoc_resources23a(padapter, 1);
 	/* s2-4. */
-	rtw_free_network_queue23a(padapter, true);
+	rtw_free_network_queue23a(padapter);
 
 	rtw_dev_unload(padapter);
 	up(&pwrpriv->lock);
 
 	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY))
-		rtw_indicate_scan_done23a(padapter, 1);
+		rtw_cfg80211_indicate_scan_done(
+			wdev_to_priv(padapter->rtw_wdev), true);
 
 	if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING))
 		rtw_indicate_disconnect23a(padapter);
@@ -525,13 +504,9 @@ static int rtw_resume(struct usb_interface *pusb_intf)
 {
 	struct dvobj_priv *dvobj = usb_get_intfdata(pusb_intf);
 	struct rtw_adapter *padapter = dvobj->if1;
-	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
-	int ret = 0;
+	int ret;
 
-	if (pwrpriv->bInternalAutoSuspend)
-		ret = rtw_resume_process23a(padapter);
-	else
-		ret = rtw_resume_process23a(padapter);
+	ret = rtw_resume_process23a(padapter);
 
 	return ret;
 }
@@ -565,7 +540,7 @@ int rtw_resume_process23a(struct rtw_adapter *padapter)
 
 	if (padapter->pid[1] != 0) {
 		DBG_8723A("pid[1]:%d\n", padapter->pid[1]);
-		rtw_signal_process(padapter->pid[1], SIGUSR2);
+		kill_pid(find_vpid(padapter->pid[1]), SIGUSR2, 1);
 	}
 
 	rtw23a_roaming(padapter, NULL);
@@ -606,8 +581,7 @@ static struct rtw_adapter *rtw_usb_if1_init(struct dvobj_priv *dvobj,
 	dvobj->padapters[dvobj->iface_nums++] = padapter;
 	padapter->iface_id = IFACE_ID0;
 
-	/* step 1-1., decide the chip_type via vid/pid */
-	decide_chip_type_by_usb_device_id(padapter, pdid);
+	rtl8723au_set_hw_type(padapter);
 
 	if (rtw_handle_dualmac23a(padapter, 1) != _SUCCESS)
 		goto free_adapter;
@@ -617,24 +591,19 @@ static struct rtw_adapter *rtw_usb_if1_init(struct dvobj_priv *dvobj,
 	if (rtw_wdev_alloc(padapter, dvobj_to_dev(dvobj)))
 		goto handle_dualmac;
 
-	/* step 2. hook HalFunc, allocate HalData */
-	if (rtl8723au_set_hal_ops(padapter))
-		return NULL;
-
-	padapter->intf_start = &usb_intf_start;
-	padapter->intf_stop = &usb_intf_stop;
-
-	/* step init_io_priv */
-	rtw_init_io_priv23a(padapter, usb_set_intf_ops);
+	/* step 2. allocate HalData */
+	padapter->HalData = kzalloc(sizeof(struct hal_data_8723a), GFP_KERNEL);
+	if (!padapter->HalData)
+		goto free_wdev;
 
 	/* step read_chip_version */
-	rtw_hal_read_chip_version23a(padapter);
+	rtl8723a_read_chip_version(padapter);
 
 	/* step usb endpoint mapping */
-	rtw_hal_chip_configure23a(padapter);
+	rtl8723au_chip_configure(padapter);
 
 	/* step read efuse/eeprom data and get mac_addr */
-	rtw_hal_read_chip_info23a(padapter);
+	rtl8723a_read_adapter_info(padapter);
 
 	/* step 5. */
 	if (rtw_init_drv_sw23a(padapter) == _FAIL) {
@@ -662,12 +631,10 @@ static struct rtw_adapter *rtw_usb_if1_init(struct dvobj_priv *dvobj,
 	padapter->pwrctrlpriv.autopm_cnt = 1;
 #endif
 
-	/*  set mac addr */
-	rtw_macaddr_cfg23a(padapter->eeprompriv.mac_addr);
-#ifdef CONFIG_8723AU_P2P
-	rtw_init_wifidirect_addrs23a(padapter, padapter->eeprompriv.mac_addr,
-				  padapter->eeprompriv.mac_addr);
-#endif
+	/* If the eeprom mac address is corrupted, assign a random address */
+	if (is_broadcast_ether_addr(padapter->eeprompriv.mac_addr) ||
+	    is_zero_ether_addr(padapter->eeprompriv.mac_addr))
+		eth_random_addr(padapter->eeprompriv.mac_addr);
 
 	DBG_8723A("bDriverStopped:%d, bSurpriseRemoved:%d, bup:%d, hw_init_completed:%d\n",
 		  padapter->bDriverStopped, padapter->bSurpriseRemoved,
@@ -678,6 +645,7 @@ static struct rtw_adapter *rtw_usb_if1_init(struct dvobj_priv *dvobj,
 free_hal_data:
 	if (status != _SUCCESS)
 		kfree(padapter->HalData);
+free_wdev:
 	if (status != _SUCCESS) {
 		rtw_wdev_unregister(padapter->rtw_wdev);
 		rtw_wdev_free(padapter->rtw_wdev);
diff --git a/drivers/staging/rtl8723au/os_dep/usb_ops_linux.c b/drivers/staging/rtl8723au/os_dep/usb_ops_linux.c
index c49160e477d..d081449318c 100644
--- a/drivers/staging/rtl8723au/os_dep/usb_ops_linux.c
+++ b/drivers/staging/rtl8723au/os_dep/usb_ops_linux.c
@@ -18,22 +18,6 @@
 #include <usb_ops_linux.h>
 #include <rtw_sreset.h>
 
-unsigned int ffaddr2pipehdl23a(struct dvobj_priv *pdvobj, u32 addr)
-{
-	struct usb_device *pusbd = pdvobj->pusbdev;
-	unsigned int pipe = 0, ep_num = 0;
-
-	if (addr == RECV_BULK_IN_ADDR) {
-		pipe = usb_rcvbulkpipe(pusbd, pdvobj->RtInPipe[0]);
-	} else if (addr == RECV_INT_IN_ADDR) {
-		pipe = usb_rcvbulkpipe(pusbd, pdvobj->RtInPipe[1]);
-	} else if (addr < HW_QUEUE_ENTRY) {
-		ep_num = pdvobj->Queue2Pipe[addr];
-		pipe = usb_sndbulkpipe(pusbd, ep_num);
-	}
-	return pipe;
-}
-
 struct zero_bulkout_context {
 	void *pbuf;
 	void *purb;
@@ -41,18 +25,9 @@ struct zero_bulkout_context {
 	void *padapter;
 };
 
-void usb_read_mem23a(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *rmem)
-{
-}
-
-void usb_write_mem23a(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *wmem)
-{
-}
-
-void usb_read_port_cancel23a(struct intf_hdl *pintfhdl)
+void rtl8723au_read_port_cancel(struct rtw_adapter *padapter)
 {
 	struct recv_buf *precvbuf;
-	struct rtw_adapter *padapter = pintfhdl->padapter;
 	int i;
 
 	precvbuf = (struct recv_buf *)padapter->recvpriv.precv_buf;
@@ -69,7 +44,7 @@ void usb_read_port_cancel23a(struct intf_hdl *pintfhdl)
 	usb_kill_urb(padapter->recvpriv.int_in_urb);
 }
 
-static void usb_write_port23a_complete(struct urb *purb, struct pt_regs *regs)
+static void usb_write_port23a_complete(struct urb *purb)
 {
 	struct xmit_buf *pxmitbuf = (struct xmit_buf *)purb->context;
 	struct rtw_adapter *padapter = pxmitbuf->padapter;
@@ -102,23 +77,25 @@ static void usb_write_port23a_complete(struct urb *purb, struct pt_regs *regs)
 	if (padapter->bSurpriseRemoved || padapter->bDriverStopped ||
 	    padapter->bWritePortCancel) {
 		RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-			 ("usb_write_port23a_complete:bDriverStopped(%d) OR bSurpriseRemoved(%d)",
-			 padapter->bDriverStopped, padapter->bSurpriseRemoved));
-		DBG_8723A("%s(): TX Warning! bDriverStopped(%d) OR bSurpriseRemoved(%d) bWritePortCancel(%d) pxmitbuf->ext_tag(%x)\n",
-			  __func__, padapter->bDriverStopped,
-			  padapter->bSurpriseRemoved, padapter->bReadPortCancel,
-			  pxmitbuf->ext_tag);
+			 ("usb_write_port23a_complete:bDriverStopped(%d) OR "
+			  "bSurpriseRemoved(%d)", padapter->bDriverStopped,
+			  padapter->bSurpriseRemoved));
+		DBG_8723A("%s(): TX Warning! bDriverStopped(%d) OR "
+			  "bSurpriseRemoved(%d) bWritePortCancel(%d) "
+			  "pxmitbuf->ext_tag(%x)\n", __func__,
+			  padapter->bDriverStopped, padapter->bSurpriseRemoved,
+			  padapter->bReadPortCancel, pxmitbuf->ext_tag);
 
 		goto check_completion;
 	}
 
 	if (purb->status) {
 		RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-			 ("usb_write_port23a_complete : purb->status(%d) != 0\n",
-			 purb->status));
+			 ("usb_write_port23a_complete : purb->status(%d) "
+			  "!= 0\n", purb->status));
 		DBG_8723A("###=> urb_write_port_complete status(%d)\n",
 			  purb->status);
-		if ((purb->status == -EPIPE) || (purb->status == -EPROTO)) {
+		if (purb->status == -EPIPE || purb->status == -EPROTO) {
 			sreset_set_wifi_error_status23a(padapter,
 						     USB_WRITE_PORT_FAIL);
 		} else if (purb->status == -EINPROGRESS) {
@@ -136,13 +113,15 @@ static void usb_write_port23a_complete(struct urb *purb, struct pt_regs *regs)
 				 ("usb_write_port23a_complete: ESHUTDOWN\n"));
 			padapter->bDriverStopped = true;
 			RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-				 ("usb_write_port23a_complete:bDriverStopped = true\n"));
+				 ("usb_write_port23a_complete:bDriverStopped "
+				  "= true\n"));
 			goto check_completion;
 		} else {
 			padapter->bSurpriseRemoved = true;
 			DBG_8723A("bSurpriseRemoved = true\n");
 			RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-				 ("usb_write_port23a_complete:bSurpriseRemoved = true\n"));
+				 ("usb_write_port23a_complete:bSurpriseRemoved "
+				  "= true\n"));
 			goto check_completion;
 		}
 	}
@@ -152,8 +131,8 @@ static void usb_write_port23a_complete(struct urb *purb, struct pt_regs *regs)
 check_completion:
 	spin_lock_irqsave(&pxmitpriv->lock_sctx, irqL);
 	rtw23a_sctx_done_err(&pxmitbuf->sctx,
-			  purb->status ? RTW_SCTX_DONE_WRITE_PORT_ERR :
-			  RTW_SCTX_DONE_SUCCESS);
+			     purb->status ? RTW_SCTX_DONE_WRITE_PORT_ERR :
+			     RTW_SCTX_DONE_SUCCESS);
 	spin_unlock_irqrestore(&pxmitpriv->lock_sctx, irqL);
 
 	rtw_free_xmitbuf23a(pxmitpriv, pxmitbuf);
@@ -161,30 +140,32 @@ check_completion:
 	tasklet_hi_schedule(&pxmitpriv->xmit_tasklet);
 }
 
-u32 usb_write_port23a(struct intf_hdl *pintfhdl, u32 addr, u32 cnt,
-		   struct xmit_buf *pxmitbuf)
+int rtl8723au_write_port(struct rtw_adapter *padapter, u32 addr, u32 cnt,
+			 struct xmit_buf *pxmitbuf)
 {
 	struct urb *purb = NULL;
-	struct rtw_adapter *padapter = (struct rtw_adapter *)pintfhdl->padapter;
 	struct dvobj_priv *pdvobj = adapter_to_dvobj(padapter);
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
-	struct xmit_frame *pxmitframe = (struct xmit_frame *)pxmitbuf->priv_data;
+	struct xmit_frame *pxmitframe;
 	struct usb_device *pusbd = pdvobj->pusbdev;
 	unsigned long irqL;
-	unsigned int pipe;
+	unsigned int pipe, ep_num;
 	int status;
-	u32 ret = _FAIL;
+	int ret = _FAIL;
 
 	RT_TRACE(_module_hci_ops_os_c_, _drv_err_, ("+usb_write_port23a\n"));
 
-	if ((padapter->bDriverStopped) || (padapter->bSurpriseRemoved) ||
-	    (padapter->pwrctrlpriv.pnp_bstop_trx)) {
+	if (padapter->bDriverStopped || padapter->bSurpriseRemoved ||
+	    padapter->pwrctrlpriv.pnp_bstop_trx) {
 		RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
-			 ("usb_write_port23a:( padapter->bDriverStopped ||padapter->bSurpriseRemoved ||adapter->pwrctrlpriv.pnp_bstop_trx)!!!\n"));
+			 ("usb_write_port23a:( padapter->bDriverStopped || "
+			  "padapter->bSurpriseRemoved || "
+			  "adapter->pwrctrlpriv.pnp_bstop_trx)!!!\n"));
 		rtw23a_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_TX_DENY);
 		goto exit;
 	}
 
+	pxmitframe = (struct xmit_frame *)pxmitbuf->priv_data;
 	spin_lock_irqsave(&pxmitpriv->lock, irqL);
 
 	switch (addr) {
@@ -214,10 +195,11 @@ u32 usb_write_port23a(struct intf_hdl *pintfhdl, u32 addr, u32 cnt,
 
 	spin_unlock_irqrestore(&pxmitpriv->lock, irqL);
 
-	purb	= pxmitbuf->pxmit_urb[0];
+	purb = pxmitbuf->pxmit_urb[0];
 
 	/* translate DMA FIFO addr to pipehandle */
-	pipe = ffaddr2pipehdl23a(pdvobj, addr);
+	ep_num = pdvobj->Queue2Pipe[addr];
+	pipe = usb_sndbulkpipe(pusbd, ep_num);
 
 	usb_fill_bulk_urb(purb, pusbd, pipe,
 			  pxmitframe->buf_addr, /*  pxmitbuf->pbuf */
@@ -230,7 +212,7 @@ u32 usb_write_port23a(struct intf_hdl *pintfhdl, u32 addr, u32 cnt,
 		phaldata->srestpriv.last_tx_time = jiffies;
 	} else {
 		rtw23a_sctx_done_err(&pxmitbuf->sctx,
-				  RTW_SCTX_DONE_WRITE_PORT_ERR);
+				     RTW_SCTX_DONE_WRITE_PORT_ERR);
 		DBG_8723A("usb_write_port23a, status =%d\n", status);
 		RT_TRACE(_module_hci_ops_os_c_, _drv_err_,
 			 ("usb_write_port23a(): usb_submit_urb, status =%x\n",
@@ -255,9 +237,8 @@ exit:
 	return ret;
 }
 
-void usb_write_port23a_cancel(struct intf_hdl *pintfhdl)
+void rtl8723au_write_port_cancel(struct rtw_adapter *padapter)
 {
-	struct rtw_adapter *padapter = pintfhdl->padapter;
 	struct xmit_buf *pxmitbuf;
 	struct list_head *plist;
 	int j;
diff --git a/drivers/staging/rtl8723au/os_dep/xmit_linux.c b/drivers/staging/rtl8723au/os_dep/xmit_linux.c
index e1c6fc74623..69f88848715 100644
--- a/drivers/staging/rtl8723au/os_dep/xmit_linux.c
+++ b/drivers/staging/rtl8723au/os_dep/xmit_linux.c
@@ -24,47 +24,6 @@
 #include <xmit_osdep.h>
 #include <osdep_intf.h>
 
-uint rtw_remainder_len23a(struct pkt_file *pfile)
-{
-	return pfile->buf_len - ((unsigned long)(pfile->cur_addr) -
-	       (unsigned long)(pfile->buf_start));
-}
-
-void _rtw_open_pktfile23a(struct sk_buff *pktptr, struct pkt_file *pfile)
-{
-	pfile->pkt = pktptr;
-	pfile->buf_start = pktptr->data;
-	pfile->cur_addr = pktptr->data;
-	pfile->buf_len = pktptr->len;
-	pfile->pkt_len = pktptr->len;
-
-	pfile->cur_buffer = pfile->buf_start;
-}
-
-uint _rtw_pktfile_read23a(struct pkt_file *pfile, u8 *rmem, uint rlen)
-{
-	uint	len = 0;
-
-	len =  rtw_remainder_len23a(pfile);
-	len = (rlen > len) ? len : rlen;
-
-	if (rmem)
-		skb_copy_bits(pfile->pkt, pfile->buf_len-pfile->pkt_len,
-			      rmem, len);
-
-	pfile->cur_addr += len;
-	pfile->pkt_len -= len;
-
-	return len;
-}
-
-int rtw_endofpktfile23a(struct pkt_file *pfile)
-{
-	if (pfile->pkt_len == 0)
-		return true;
-	return false;
-}
-
 int rtw_os_xmit_resource_alloc23a(struct rtw_adapter *padapter,
 			       struct xmit_buf *pxmitbuf, u32 alloc_sz)
 {
diff --git a/drivers/staging/rtl8821ae/base.c b/drivers/staging/rtl8821ae/base.c
index e5073fe2477..4a36da06c01 100644
--- a/drivers/staging/rtl8821ae/base.c
+++ b/drivers/staging/rtl8821ae/base.c
@@ -320,9 +320,6 @@ static void _rtl_init_mac80211(struct ieee80211_hw *hw)
 	/* <5> set hw caps */
 	hw->flags = IEEE80211_HW_SIGNAL_DBM |
 	    IEEE80211_HW_RX_INCLUDES_FCS |
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3, 4, 0))
-	    IEEE80211_HW_BEACON_FILTER |
-#endif
 	    IEEE80211_HW_AMPDU_AGGREGATION |
 	    IEEE80211_HW_REPORTS_TX_ACK_STATUS |
 	    IEEE80211_HW_CONNECTION_MONITOR |
@@ -335,8 +332,6 @@ static void _rtl_init_mac80211(struct ieee80211_hw *hw)
 			IEEE80211_HW_PS_NULLFUNC_STACK |
 			/* IEEE80211_HW_SUPPORTS_DYNAMIC_PS | */
 			0;
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 37))
 	hw->wiphy->interface_modes =
 	    BIT(NL80211_IFTYPE_AP) |
 	    BIT(NL80211_IFTYPE_STATION) |
@@ -344,23 +339,10 @@ static void _rtl_init_mac80211(struct ieee80211_hw *hw)
 	    BIT(NL80211_IFTYPE_MESH_POINT) |
 	    BIT(NL80211_IFTYPE_P2P_CLIENT) |
 	    BIT(NL80211_IFTYPE_P2P_GO);
-#else
-/*<delete in kernel end>*/
-	hw->wiphy->interface_modes =
-	    BIT(NL80211_IFTYPE_AP) |
-	    BIT(NL80211_IFTYPE_STATION) |
-	    BIT(NL80211_IFTYPE_ADHOC) |
-	    BIT(NL80211_IFTYPE_MESH_POINT) ;
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
-#if (LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 39))
+
 	hw->wiphy->flags |= WIPHY_FLAG_IBSS_RSN;
-#endif
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 3, 0))
 	hw->wiphy->flags |= WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL;
-#endif
 
 	hw->wiphy->rts_threshold = 2347;
 
@@ -388,7 +370,7 @@ static void _rtl_init_mac80211(struct ieee80211_hw *hw)
 
 }
 
-static void _rtl_init_deferred_work(struct ieee80211_hw *hw)
+static int _rtl_init_deferred_work(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
@@ -401,15 +383,11 @@ static void _rtl_init_deferred_work(struct ieee80211_hw *hw)
 		    rtl_easy_concurrent_retrytimer_callback, (unsigned long)hw);
 	/* <2> work queue */
 	rtlpriv->works.hw = hw;
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 37))
-/*<delete in kernel end>*/
-	rtlpriv->works.rtl_wq = alloc_workqueue(rtlpriv->cfg->name, 0, 0);
-/*<delete in kernel start>*/
-#else
-	rtlpriv->works.rtl_wq = create_workqueue(rtlpriv->cfg->name);
-#endif
-/*<delete in kernel end>*/
+	rtlpriv->works.rtl_wq = alloc_workqueue("%s", 0, 0,
+						rtlpriv->cfg->name);
+	if (!rtlpriv->works.rtl_wq)
+		return -ENOMEM;
+
 	INIT_DELAYED_WORK(&rtlpriv->works.watchdog_wq,
 			  (void *)rtl_watchdog_wq_callback);
 	INIT_DELAYED_WORK(&rtlpriv->works.ips_nic_off_wq,
@@ -421,6 +399,8 @@ static void _rtl_init_deferred_work(struct ieee80211_hw *hw)
 	INIT_DELAYED_WORK(&rtlpriv->works.fwevt_wq,
 			  (void *)rtl_fwevt_wq_callback);
 
+	return 0;
+
 }
 
 void rtl_deinit_deferred_work(struct ieee80211_hw *hw)
@@ -519,7 +499,8 @@ int rtl_init_core(struct ieee80211_hw *hw)
 	INIT_LIST_HEAD(&rtlpriv->entry_list);
 
 	/* <6> init deferred work */
-	_rtl_init_deferred_work(hw);
+	if (_rtl_init_deferred_work(hw))
+		return 1;
 
 	/* <7> */
 #ifdef VIF_TODO
@@ -894,13 +875,8 @@ bool rtl_action_proc(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx)
 								    hdr->addr3,
 								    tid);
 					if (skb_delba) {
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 10, 0))
 						rx_status.freq = hw->conf.chandef.chan->center_freq;
 						rx_status.band = hw->conf.chandef.chan->band;
-#else
-						rx_status.freq = hw->conf.channel->center_freq;
-						rx_status.band = hw->conf.channel->band;
-#endif
 						rx_status.flag |= RX_FLAG_DECRYPTED;
 						rx_status.flag |= RX_FLAG_MACTIME_MPDU;
 						rx_status.rate_idx = 0;
@@ -1475,21 +1451,8 @@ int rtl_send_smps_action(struct ieee80211_hw *hw,
 		/* rtlpriv->cfg->ops->update_rate_tbl(hw, sta, 0); */
 
 		info->control.rates[0].idx = 0;
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 10, 0))
 		info->band = hw->conf.chandef.chan->band;
-#else
-		info->band = hw->conf.channel->band;
-#endif
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3, 7, 0))
-		info->control.sta = sta;
-		rtlpriv->intf_ops->adapter_tx(hw, skb, &tcb_desc);
-#else
-/*<delete in kernel end>*/
 		rtlpriv->intf_ops->adapter_tx(hw, sta, skb, &tcb_desc);
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
 	}
 	return 1;
 
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtc8192e2ant.c b/drivers/staging/rtl8821ae/btcoexist/halbtc8192e2ant.c
index 115908928ae..d2381794ecc 100644
--- a/drivers/staging/rtl8821ae/btcoexist/halbtc8192e2ant.c
+++ b/drivers/staging/rtl8821ae/btcoexist/halbtc8192e2ant.c
@@ -12,7 +12,7 @@
  *   include files
  **************************************************************/
 #include "halbt_precomp.h"
-#if 1
+
 /**************************************************************
  *   Global variables, these are static variables
  **************************************************************/
@@ -21,14 +21,14 @@ static struct coex_dm_8192e_2ant *coex_dm = &glcoex_dm_8192e_2ant;
 static struct coex_sta_8192e_2ant glcoex_sta_8192e_2ant;
 static struct coex_sta_8192e_2ant *coex_sta = &glcoex_sta_8192e_2ant;
 
-const char *const GLBtInfoSrc8192e2Ant[]={
+static const char *const GLBtInfoSrc8192e2Ant[] = {
 	"BT Info[wifi fw]",
 	"BT Info[bt rsp]",
 	"BT Info[bt auto report]",
 };
 
-u32 glcoex_ver_date_8192e_2ant = 20130902;
-u32 glcoex_ver_8192e_2ant = 0x34;
+static u32 glcoex_ver_date_8192e_2ant = 20130902;
+static u32 glcoex_ver_8192e_2ant = 0x34;
 
 /**************************************************************
  *   local function proto type if needed
@@ -36,13 +36,12 @@ u32 glcoex_ver_8192e_2ant = 0x34;
 /**************************************************************
  *   local function start with halbtc8192e2ant_
  **************************************************************/
-u8 halbtc8192e2ant_btrssi_state(u8 level_num, u8 rssi_thresh, u8 rssi_thresh1)
+static u8 halbtc8192e2ant_btrssi_state(u8 level_num, u8 rssi_thresh,
+				       u8 rssi_thresh1)
 {
-	int btrssi=0;
+	int btrssi = coex_sta->bt_rssi;
 	u8 btrssi_state = coex_sta->pre_bt_rssi_state;
 
-	btrssi = coex_sta->bt_rssi;
-
 	if (level_num == 2) {
 		if ((coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_LOW) ||
 		    (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW)) {
@@ -82,7 +81,7 @@ u8 halbtc8192e2ant_btrssi_state(u8 level_num, u8 rssi_thresh, u8 rssi_thresh1)
 		    (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW)) {
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
 				  "BT Rssi pre state=LOW\n");
-			if(btrssi >= (rssi_thresh +
+			if (btrssi >= (rssi_thresh +
 				      BTC_RSSI_COEX_THRESH_TOL_8192E_2ANT)) {
 				btrssi_state = BTC_RSSI_STATE_MEDIUM;
 				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
@@ -132,8 +131,9 @@ u8 halbtc8192e2ant_btrssi_state(u8 level_num, u8 rssi_thresh, u8 rssi_thresh1)
 	return btrssi_state;
 }
 
-u8 halbtc8192e2ant_wifirssi_state(struct btc_coexist * btcoexist, u8 index,
-				  u8 level_num, u8 rssi_thresh, u8 rssi_thresh1)
+static u8 halbtc8192e2ant_wifirssi_state(struct btc_coexist *btcoexist,
+					 u8 index, u8 level_num,
+					 u8 rssi_thresh, u8 rssi_thresh1)
 {
 	int wifirssi = 0;
 	u8 wifirssi_state = coex_sta->pre_wifi_rssi_state[index];
@@ -156,8 +156,8 @@ u8 halbtc8192e2ant_wifirssi_state(struct btc_coexist * btcoexist, u8 index,
 				BTC_PRINT(BTC_MSG_ALGORITHM,
 					  ALGO_WIFI_RSSI_STATE,
 					  "wifi RSSI state stay at Low\n");
- 			}
- 		} else {
+			}
+		} else {
 			if (wifirssi < rssi_thresh) {
 				wifirssi_state = BTC_RSSI_STATE_LOW;
 				BTC_PRINT(BTC_MSG_ALGORITHM,
@@ -234,10 +234,10 @@ u8 halbtc8192e2ant_wifirssi_state(struct btc_coexist * btcoexist, u8 index,
 	return wifirssi_state;
 }
 
-void halbtc8192e2ant_monitor_bt_enable_disable(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_monitor_bt_enable_disable(struct btc_coexist *btcoexist)
 {
-	static bool pre_bt_disabled = false;
-	static u32 bt_disable_cnt = 0;
+	static bool pre_bt_disabled;
+	static u32 bt_disable_cnt;
 	bool bt_active = true, bt_disabled = false;
 
 	/* This function check if bt is disabled */
@@ -283,7 +283,7 @@ void halbtc8192e2ant_monitor_bt_enable_disable(struct btc_coexist *btcoexist)
 	}
 }
 
-u32 halbtc8192e2ant_decidera_mask(struct btc_coexist *btcoexist,
+static u32 halbtc8192e2ant_decidera_mask(struct btc_coexist *btcoexist,
 				  u8 sstype, u32 ra_masktype)
 {
 	u32 disra_mask = 0x0;
@@ -296,13 +296,13 @@ u32 halbtc8192e2ant_decidera_mask(struct btc_coexist *btcoexist,
 			disra_mask = 0xfff00000;/* disable 2ss */
 		break;
 	case 1: /* disable cck 1/2 */
-		if(sstype == 2)
+		if (sstype == 2)
 			disra_mask = 0x00000003;/* enable 2ss */
 		else
 			disra_mask = 0xfff00003;/* disable 2ss */
 		break;
 	case 2: /* disable cck 1/2/5.5, ofdm 6/9/12/18/24, mcs 0/1/2/3/4 */
-		if(sstype == 2)
+		if (sstype == 2)
 			disra_mask = 0x0001f1f7;/* enable 2ss */
 		else
 			disra_mask = 0xfff1f1f7;/* disable 2ss */
@@ -314,8 +314,8 @@ u32 halbtc8192e2ant_decidera_mask(struct btc_coexist *btcoexist,
 	return disra_mask;
 }
 
-void halbtc8192e2ant_Updatera_mask(struct btc_coexist *btcoexist,
-				   bool force_exec, u32 dis_ratemask)
+static void halbtc8192e2ant_Updatera_mask(struct btc_coexist *btcoexist,
+					  bool force_exec, u32 dis_ratemask)
 {
 	coex_dm->curra_mask = dis_ratemask;
 
@@ -325,8 +325,8 @@ void halbtc8192e2ant_Updatera_mask(struct btc_coexist *btcoexist,
 	coex_dm->prera_mask = coex_dm->curra_mask;
 }
 
-void halbtc8192e2ant_autorate_fallback_retry(struct btc_coexist *btcoexist,
-					     bool force_exec, u8 type)
+static void halbtc8192e2ant_autorate_fallback_retry(struct btc_coexist *btcoexist,
+						    bool force_exec, u8 type)
 {
 	bool wifi_under_bmode = false;
 
@@ -364,32 +364,32 @@ void halbtc8192e2ant_autorate_fallback_retry(struct btc_coexist *btcoexist,
 	coex_dm->pre_arfrtype = coex_dm->cur_arfrtype;
 }
 
-void halbtc8192e2ant_retrylimit(struct btc_coexist *btcoexist,
-				bool force_exec, u8 type)
+static void halbtc8192e2ant_retrylimit(struct btc_coexist *btcoexist,
+				       bool force_exec, u8 type)
 {
 	coex_dm->cur_retrylimit_type = type;
 
 	if (force_exec || (coex_dm->pre_retrylimit_type !=
 			   coex_dm->cur_retrylimit_type)) {
 		switch (coex_dm->cur_retrylimit_type) {
-			case 0:	/* normal mode */
-				btcoexist->btc_write_2byte(btcoexist, 0x42a,
-						    coex_dm->backup_retrylimit);
-				break;
-			case 1:	/* retry limit=8 */
-				btcoexist->btc_write_2byte(btcoexist, 0x42a,
-							   0x0808);
-				break;
-			default:
-				break;
+		case 0:	/* normal mode */
+			btcoexist->btc_write_2byte(btcoexist, 0x42a,
+					    coex_dm->backup_retrylimit);
+			break;
+		case 1:	/* retry limit=8 */
+			btcoexist->btc_write_2byte(btcoexist, 0x42a,
+						   0x0808);
+			break;
+		default:
+			break;
 		}
 	}
 
 	coex_dm->pre_retrylimit_type = coex_dm->cur_retrylimit_type;
 }
 
-void halbtc8192e2ant_ampdu_maxtime(struct btc_coexist *btcoexist,
-				   bool force_exec, u8 type)
+static void halbtc8192e2ant_ampdu_maxtime(struct btc_coexist *btcoexist,
+					  bool force_exec, u8 type)
 {
 	coex_dm->cur_ampdutime_type = type;
 
@@ -411,9 +411,10 @@ void halbtc8192e2ant_ampdu_maxtime(struct btc_coexist *btcoexist,
 	coex_dm->pre_ampdutime_type = coex_dm->cur_ampdutime_type;
 }
 
-void halbtc8192e2ant_limited_tx(struct btc_coexist *btcoexist,
-				bool force_exec, u8 ra_masktype, u8 arfr_type,
-				u8 retrylimit_type, u8 ampdutime_type)
+static void halbtc8192e2ant_limited_tx(struct btc_coexist *btcoexist,
+				       bool force_exec, u8 ra_masktype,
+				       u8 arfr_type, u8 retrylimit_type,
+				       u8 ampdutime_type)
 {
 	u32 disra_mask = 0x0;
 
@@ -429,10 +430,10 @@ void halbtc8192e2ant_limited_tx(struct btc_coexist *btcoexist,
 	halbtc8192e2ant_ampdu_maxtime(btcoexist, force_exec, ampdutime_type);
 }
 
-void halbtc8192e2ant_limited_rx(struct btc_coexist *btcoexist,
-				bool force_exec, bool rej_ap_agg_pkt,
-				bool b_bt_ctrl_agg_buf_size,
-				u8 agg_buf_size)
+static void halbtc8192e2ant_limited_rx(struct btc_coexist *btcoexist,
+				       bool force_exec, bool rej_ap_agg_pkt,
+				       bool b_bt_ctrl_agg_buf_size,
+				       u8 agg_buf_size)
 {
 	bool reject_rx_agg = rej_ap_agg_pkt;
 	bool bt_ctrl_rx_agg_size = b_bt_ctrl_agg_buf_size;
@@ -455,7 +456,7 @@ void halbtc8192e2ant_limited_rx(struct btc_coexist *btcoexist,
 
 }
 
-void halbtc8192e2ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
 {
 	u32 reg_hp_txrx, reg_lp_txrx, u32tmp;
 	u32 reg_hp_tx = 0, reg_hp_rx = 0, reg_lp_tx = 0, reg_lp_rx = 0;
@@ -487,9 +488,9 @@ void halbtc8192e2ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
 	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
 }
 
-void halbtc8192e2ant_querybt_info(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_querybt_info(struct btc_coexist *btcoexist)
 {
-	u8 h2c_parameter[1] ={0};
+	u8 h2c_parameter[1] = {0};
 
 	coex_sta->c2h_bt_info_req_sent = true;
 
@@ -502,39 +503,7 @@ void halbtc8192e2ant_querybt_info(struct btc_coexist *btcoexist)
 	btcoexist->btc_fill_h2c(btcoexist, 0x61, 1, h2c_parameter);
 }
 
-bool halbtc8192e2ant_iswifi_status_changed(struct btc_coexist *btcoexist)
-{
-	static bool pre_wifi_busy = false;
-	static bool pre_under_4way = false, pre_bt_hson = false;
-	bool wifi_busy = false, under_4way = false, bt_hson = false;
-	bool wifi_connected = false;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
-			   &wifi_connected);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hson);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS,
-			   &under_4way);
-
-	if (wifi_connected) {
-		if (wifi_busy != pre_wifi_busy) {
-			pre_wifi_busy = wifi_busy;
-			return true;
-		}
-		if (under_4way != pre_under_4way) {
-			pre_under_4way = under_4way;
-			return true;
-		}
-		if (bt_hson != pre_bt_hson) {
-			pre_bt_hson = bt_hson;
-			return true;
-		}
-	}
-
-	return false;
-}
-
-void halbtc8192e2ant_update_btlink_info(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_update_btlink_info(struct btc_coexist *btcoexist)
 {
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
 	bool bt_hson = false;
@@ -590,11 +559,11 @@ void halbtc8192e2ant_update_btlink_info(struct btc_coexist *btcoexist)
 		bt_link_info->hid_only = false;
 }
 
-u8 halbtc8192e2ant_action_algorithm(struct btc_coexist *btcoexist)
+static u8 halbtc8192e2ant_action_algorithm(struct btc_coexist *btcoexist)
 {
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
 	struct btc_stack_info *stack_info = &btcoexist->stack_info;
-	bool bt_hson=false;
+	bool bt_hson = false;
 	u8 algorithm = BT_8192E_2ANT_COEX_ALGO_UNDEFINED;
 	u8 numOfDiffProfile = 0;
 
@@ -777,10 +746,10 @@ u8 halbtc8192e2ant_action_algorithm(struct btc_coexist *btcoexist)
 	return algorithm;
 }
 
-void halbtc8192e2ant_setfw_dac_swinglevel(struct btc_coexist *btcoexist,
-					  u8 dac_swinglvl)
+static void halbtc8192e2ant_setfw_dac_swinglevel(struct btc_coexist *btcoexist,
+						 u8 dac_swinglvl)
 {
-	u8 h2c_parameter[1] ={0};
+	u8 h2c_parameter[1] = {0};
 
 	/* There are several type of dacswing
 	 * 0x18/ 0x10/ 0xc/ 0x8/ 0x4/ 0x6 */
@@ -794,10 +763,10 @@ void halbtc8192e2ant_setfw_dac_swinglevel(struct btc_coexist *btcoexist,
 	btcoexist->btc_fill_h2c(btcoexist, 0x64, 1, h2c_parameter);
 }
 
-void halbtc8192e2ant_set_fwdec_btpwr(struct btc_coexist *btcoexist,
-				     u8 dec_btpwr_lvl)
+static void halbtc8192e2ant_set_fwdec_btpwr(struct btc_coexist *btcoexist,
+					    u8 dec_btpwr_lvl)
 {
-	u8 h2c_parameter[1] ={0};
+	u8 h2c_parameter[1] = {0};
 
 	h2c_parameter[0] = dec_btpwr_lvl;
 
@@ -808,12 +777,12 @@ void halbtc8192e2ant_set_fwdec_btpwr(struct btc_coexist *btcoexist,
 	btcoexist->btc_fill_h2c(btcoexist, 0x62, 1, h2c_parameter);
 }
 
-void halbtc8192e2ant_dec_btpwr(struct btc_coexist *btcoexist,
-			       bool force_exec, u8 dec_btpwr_lvl)
+static void halbtc8192e2ant_dec_btpwr(struct btc_coexist *btcoexist,
+				      bool force_exec, u8 dec_btpwr_lvl)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
 		  "[BTCoex], %s Dec BT power level = %d\n",
-		  (force_exec? "force to":""), dec_btpwr_lvl);
+		  (force_exec ? "force to":""), dec_btpwr_lvl);
 	coex_dm->cur_dec_bt_pwr = dec_btpwr_lvl;
 
 	if (!force_exec) {
@@ -826,10 +795,10 @@ void halbtc8192e2ant_dec_btpwr(struct btc_coexist *btcoexist,
 	coex_dm->pre_dec_bt_pwr = coex_dm->cur_dec_bt_pwr;
 }
 
-void halbtc8192e2ant_set_bt_autoreport(struct btc_coexist *btcoexist,
-				       bool enable_autoreport)
+static void halbtc8192e2ant_set_bt_autoreport(struct btc_coexist *btcoexist,
+					      bool enable_autoreport)
 {
-	u8 h2c_parameter[1] ={0};
+	u8 h2c_parameter[1] = {0};
 
 	h2c_parameter[0] = 0;
 
@@ -838,19 +807,20 @@ void halbtc8192e2ant_set_bt_autoreport(struct btc_coexist *btcoexist,
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
 		  "[BTCoex], BT FW auto report : %s, FW write 0x68=0x%x\n",
-		  (enable_autoreport? "Enabled!!":"Disabled!!"),
+		  (enable_autoreport ? "Enabled!!":"Disabled!!"),
 		  h2c_parameter[0]);
 
 	btcoexist->btc_fill_h2c(btcoexist, 0x68, 1, h2c_parameter);
 }
 
-void halbtc8192e2ant_bt_autoreport(struct btc_coexist *btcoexist,
-				   bool force_exec, bool enable_autoreport)
+static void halbtc8192e2ant_bt_autoreport(struct btc_coexist *btcoexist,
+					  bool force_exec,
+					  bool enable_autoreport)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
 		  "[BTCoex], %s BT Auto report = %s\n",
-		  (force_exec? "force to":""),
-		  ((enable_autoreport)? "Enabled":"Disabled"));
+		  (force_exec ? "force to":""),
+		  ((enable_autoreport) ? "Enabled":"Disabled"));
 	coex_dm->cur_bt_auto_report = enable_autoreport;
 
 	if (!force_exec) {
@@ -868,12 +838,12 @@ void halbtc8192e2ant_bt_autoreport(struct btc_coexist *btcoexist,
 	coex_dm->pre_bt_auto_report = coex_dm->cur_bt_auto_report;
 }
 
-void halbtc8192e2ant_fw_dac_swinglvl(struct btc_coexist *btcoexist,
-				     bool force_exec, u8 fw_dac_swinglvl)
+static void halbtc8192e2ant_fw_dac_swinglvl(struct btc_coexist *btcoexist,
+					    bool force_exec, u8 fw_dac_swinglvl)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
 		  "[BTCoex], %s set FW Dac Swing level = %d\n",
-		  (force_exec? "force to":""), fw_dac_swinglvl);
+		  (force_exec ? "force to":""), fw_dac_swinglvl);
 	coex_dm->cur_fw_dac_swing_lvl = fw_dac_swinglvl;
 
 	if (!force_exec) {
@@ -893,8 +863,8 @@ void halbtc8192e2ant_fw_dac_swinglvl(struct btc_coexist *btcoexist,
 	coex_dm->pre_fw_dac_swing_lvl = coex_dm->cur_fw_dac_swing_lvl;
 }
 
-void halbtc8192e2ant_set_sw_rf_rx_lpf_corner(struct btc_coexist *btcoexist,
-					     bool rx_rf_shrink_on)
+static void halbtc8192e2ant_set_sw_rf_rx_lpf_corner(struct btc_coexist *btcoexist,
+						    bool rx_rf_shrink_on)
 {
 	if (rx_rf_shrink_on) {
 		/* Shrink RF Rx LPF corner */
@@ -915,12 +885,12 @@ void halbtc8192e2ant_set_sw_rf_rx_lpf_corner(struct btc_coexist *btcoexist,
 	}
 }
 
-void halbtc8192e2ant_rf_shrink(struct btc_coexist *btcoexist,
-			       bool force_exec, bool rx_rf_shrink_on)
+static void halbtc8192e2ant_rf_shrink(struct btc_coexist *btcoexist,
+				      bool force_exec, bool rx_rf_shrink_on)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
 		  "[BTCoex], %s turn Rx RF Shrink = %s\n",
-		  (force_exec? "force to":""), ((rx_rf_shrink_on)? "ON":"OFF"));
+		  (force_exec ? "force to":""), ((rx_rf_shrink_on) ? "ON":"OFF"));
 	coex_dm->cur_rf_rx_lpf_shrink = rx_rf_shrink_on;
 
 	if (!force_exec) {
@@ -939,56 +909,8 @@ void halbtc8192e2ant_rf_shrink(struct btc_coexist *btcoexist,
 	coex_dm->pre_rf_rx_lpf_shrink = coex_dm->cur_rf_rx_lpf_shrink;
 }
 
-void halbtc8192e2ant_set_sw_penalty_tx_rateadaptive(
-						struct btc_coexist *btcoexist,
-						bool low_penalty_ra)
-{
-	u8 h2c_parameter[6] ={0};
-
-	h2c_parameter[0] = 0x6;	/* opCode, 0x6= Retry_Penalty */
-
-	if (low_penalty_ra) {
-		h2c_parameter[1] |= BIT0;
-		/* normal rate except MCS7/6/5, OFDM54/48/36 */
-		h2c_parameter[2] = 0x00;
-		h2c_parameter[3] = 0xf7;  /* MCS7 or OFDM54 */
-		h2c_parameter[4] = 0xf8;  /* MCS6 or OFDM48 */
-		h2c_parameter[5] = 0xf9;  /* MCS5 or OFDM36 */
-	}
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], set WiFi Low-Penalty Retry: %s",
-		  (low_penalty_ra? "ON!!":"OFF!!"));
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x69, 6, h2c_parameter);
-}
-
-void halbtc8192e2ant_low_penalty_ra(struct btc_coexist *btcoexist,
-				    bool force_exec, bool low_penalty_ra)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
-		  "[BTCoex], %s turn LowPenaltyRA = %s\n",
-		  (force_exec? "force to":""), ((low_penalty_ra)? "ON":"OFF"));
-	coex_dm->cur_low_penalty_ra = low_penalty_ra;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex] bPreLowPenaltyRa=%d, bCurLowPenaltyRa=%d\n",
-			  coex_dm->pre_low_penalty_ra,
-			  coex_dm->cur_low_penalty_ra);
-
-		if (coex_dm->pre_low_penalty_ra ==
-		    coex_dm->cur_low_penalty_ra)
-			return;
-	}
-	halbtc8192e2ant_set_sw_penalty_tx_rateadaptive(btcoexist,
-						coex_dm->cur_low_penalty_ra);
-
-	coex_dm->pre_low_penalty_ra = coex_dm->cur_low_penalty_ra;
-}
-
-void halbtc8192e2ant_set_dac_swingreg(struct btc_coexist *btcoexist,
-				      u32 level)
+static void halbtc8192e2ant_set_dac_swingreg(struct btc_coexist *btcoexist,
+					     u32 level)
 {
 	u8 val = (u8)level;
 
@@ -997,9 +919,9 @@ void halbtc8192e2ant_set_dac_swingreg(struct btc_coexist *btcoexist,
 	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x883, 0x3e, val);
 }
 
-void halbtc8192e2ant_setsw_fulltime_dacswing(struct btc_coexist *btcoexist,
-					     bool sw_dac_swingon,
-					     u32 sw_dac_swinglvl)
+static void halbtc8192e2ant_setsw_fulltime_dacswing(struct btc_coexist *btcoexist,
+						    bool sw_dac_swingon,
+						    u32 sw_dac_swinglvl)
 {
 	if (sw_dac_swingon)
 		halbtc8192e2ant_set_dac_swingreg(btcoexist, sw_dac_swinglvl);
@@ -1007,15 +929,14 @@ void halbtc8192e2ant_setsw_fulltime_dacswing(struct btc_coexist *btcoexist,
 		halbtc8192e2ant_set_dac_swingreg(btcoexist, 0x18);
 }
 
-
-void halbtc8192e2ant_DacSwing(struct btc_coexist *btcoexist,
-			      bool force_exec, bool dac_swingon,
-			      u32 dac_swinglvl)
+static void halbtc8192e2ant_DacSwing(struct btc_coexist *btcoexist,
+				     bool force_exec, bool dac_swingon,
+				     u32 dac_swinglvl)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
 		  "[BTCoex], %s turn DacSwing=%s, dac_swinglvl=0x%x\n",
-		  (force_exec? "force to":""),
-		  ((dac_swingon)? "ON":"OFF"), dac_swinglvl);
+		  (force_exec ? "force to":""),
+		  ((dac_swingon) ? "ON":"OFF"), dac_swinglvl);
 	coex_dm->cur_dac_swing_on = dac_swingon;
 	coex_dm->cur_dac_swing_lvl = dac_swinglvl;
 
@@ -1041,46 +962,8 @@ void halbtc8192e2ant_DacSwing(struct btc_coexist *btcoexist,
 	coex_dm->pre_dac_swing_lvl = coex_dm->cur_dac_swing_lvl;
 }
 
-void halbtc8192e2ant_set_adc_backoff(struct btc_coexist *btcoexist,
-				     bool adc_backoff)
-{
-	if(adc_backoff)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-			  "[BTCoex], BB BackOff Level On!\n");
-		btcoexist->btc_write_1byte_bitmask(btcoexist, 0xc05, 0x30, 0x3);
-	}
-	else
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-			  "[BTCoex], BB BackOff Level Off!\n");
-		btcoexist->btc_write_1byte_bitmask(btcoexist, 0xc05, 0x30, 0x1);
-	}
-}
-
-void halbtc8192e2ant_adc_backoff(struct btc_coexist *btcoexist,
-				 bool force_exec, bool adc_backoff)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
-		  "[BTCoex], %s turn AdcBackOff = %s\n",
-		  (force_exec? "force to":""), ((adc_backoff)? "ON":"OFF"));
-	coex_dm->cur_adc_back_off = adc_backoff;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], bPreAdcBackOff=%d, bCurAdcBackOff=%d\n",
-			  coex_dm->pre_adc_back_off, coex_dm->cur_adc_back_off);
-
-		if (coex_dm->pre_adc_back_off == coex_dm->cur_adc_back_off)
-			return;
-	}
-	halbtc8192e2ant_set_adc_backoff(btcoexist, coex_dm->cur_adc_back_off);
-
-	coex_dm->pre_adc_back_off = coex_dm->cur_adc_back_off;
-}
-
-void halbtc8192e2ant_set_agc_table(struct btc_coexist *btcoexist,
-				   bool agc_table_en)
+static void halbtc8192e2ant_set_agc_table(struct btc_coexist *btcoexist,
+					  bool agc_table_en)
 {
 
 	/* BB AGC Gain Table */
@@ -1096,7 +979,7 @@ void halbtc8192e2ant_set_agc_table(struct btc_coexist *btcoexist,
 	} else {
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
 			  "[BTCoex], BB Agc Table Off!\n");
-	 	btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xaa1A0001);
+		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xaa1A0001);
 		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa91B0001);
 		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa81C0001);
 		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa71D0001);
@@ -1105,13 +988,13 @@ void halbtc8192e2ant_set_agc_table(struct btc_coexist *btcoexist,
 	}
 }
 
-void halbtc8192e2ant_AgcTable(struct btc_coexist *btcoexist,
-			      bool force_exec, bool agc_table_en)
+static void halbtc8192e2ant_AgcTable(struct btc_coexist *btcoexist,
+				     bool force_exec, bool agc_table_en)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
 		  "[BTCoex], %s %s Agc Table\n",
-		  (force_exec? "force to":""),
-		  ((agc_table_en)? "Enable":"Disable"));
+		  (force_exec ? "force to":""),
+		  ((agc_table_en) ? "Enable":"Disable"));
 	coex_dm->cur_agc_table_en = agc_table_en;
 
 	if (!force_exec) {
@@ -1127,9 +1010,9 @@ void halbtc8192e2ant_AgcTable(struct btc_coexist *btcoexist,
 	coex_dm->pre_agc_table_en = coex_dm->cur_agc_table_en;
 }
 
-void halbtc8192e2ant_set_coex_table(struct btc_coexist *btcoexist,
-				    u32 val0x6c0, u32 val0x6c4,
-				    u32 val0x6c8, u8 val0x6cc)
+static void halbtc8192e2ant_set_coex_table(struct btc_coexist *btcoexist,
+					   u32 val0x6c0, u32 val0x6c4,
+					   u32 val0x6c8, u8 val0x6cc)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
 		  "[BTCoex], set coex table, set 0x6c0=0x%x\n", val0x6c0);
@@ -1148,13 +1031,14 @@ void halbtc8192e2ant_set_coex_table(struct btc_coexist *btcoexist,
 	btcoexist->btc_write_1byte(btcoexist, 0x6cc, val0x6cc);
 }
 
-void halbtc8192e2ant_coex_table(struct btc_coexist *btcoexist, bool force_exec,
-				u32 val0x6c0, u32 val0x6c4,
-				u32 val0x6c8, u8 val0x6cc)
+static void halbtc8192e2ant_coex_table(struct btc_coexist *btcoexist,
+				       bool force_exec,
+				       u32 val0x6c0, u32 val0x6c4,
+				       u32 val0x6c8, u8 val0x6cc)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
 		  "[BTCoex], %s write Coex Table 0x6c0=0x%x, ",
-		  (force_exec? "force to":""), val0x6c0);
+		  (force_exec ? "force to":""), val0x6c0);
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
 		  "0x6c4=0x%x, 0x6c8=0x%x, 0x6cc=0x%x\n",
 		  val0x6c4, val0x6c8, val0x6cc);
@@ -1171,7 +1055,7 @@ void halbtc8192e2ant_coex_table(struct btc_coexist *btcoexist, bool force_exec,
 			  "preVal0x6c8=0x%x, preVal0x6cc=0x%x !!\n",
 			  coex_dm->pre_val0x6c8, coex_dm->pre_val0x6cc);
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], curVal0x6c0=0x%x, curVal0x6c4=0x%x, \n",
+			  "[BTCoex], curVal0x6c0=0x%x, curVal0x6c4=0x%x,\n",
 			  coex_dm->cur_val0x6c0, coex_dm->cur_val0x6c4);
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
 			  "curVal0x6c8=0x%x, curVal0x6cc=0x%x !!\n",
@@ -1192,8 +1076,8 @@ void halbtc8192e2ant_coex_table(struct btc_coexist *btcoexist, bool force_exec,
 	coex_dm->pre_val0x6cc = coex_dm->cur_val0x6cc;
 }
 
-void halbtc8192e2ant_coex_table_with_type(struct btc_coexist *btcoexist,
-					  bool force_exec, u8 type)
+static void halbtc8192e2ant_coex_table_with_type(struct btc_coexist *btcoexist,
+						 bool force_exec, u8 type)
 {
 	switch (type) {
 	case 0:
@@ -1221,10 +1105,10 @@ void halbtc8192e2ant_coex_table_with_type(struct btc_coexist *btcoexist,
 	}
 }
 
-void halbtc8192e2ant_set_fw_ignore_wlanact(struct btc_coexist *btcoexist,
-					   bool enable)
+static void halbtc8192e2ant_set_fw_ignore_wlanact(struct btc_coexist *btcoexist,
+						  bool enable)
 {
-	u8 h2c_parameter[1] ={0};
+	u8 h2c_parameter[1] = {0};
 
 	if (enable)
 		h2c_parameter[0] |= BIT0; /* function enable */
@@ -1236,12 +1120,12 @@ void halbtc8192e2ant_set_fw_ignore_wlanact(struct btc_coexist *btcoexist,
 	btcoexist->btc_fill_h2c(btcoexist, 0x63, 1, h2c_parameter);
 }
 
-void halbtc8192e2ant_IgnoreWlanAct(struct btc_coexist *btcoexist,
-				   bool force_exec, bool enable)
+static void halbtc8192e2ant_IgnoreWlanAct(struct btc_coexist *btcoexist,
+					  bool force_exec, bool enable)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
 		  "[BTCoex], %s turn Ignore WlanAct %s\n",
-		  (force_exec? "force to":""), (enable? "ON":"OFF"));
+		  (force_exec ? "force to":""), (enable ? "ON":"OFF"));
 	coex_dm->cur_ignore_wlan_act = enable;
 
 	if (!force_exec) {
@@ -1261,10 +1145,10 @@ void halbtc8192e2ant_IgnoreWlanAct(struct btc_coexist *btcoexist,
 	coex_dm->pre_ignore_wlan_act = coex_dm->cur_ignore_wlan_act;
 }
 
-void halbtc8192e2ant_SetFwPstdma(struct btc_coexist *btcoexist, u8 byte1,
-				 u8 byte2, u8 byte3, u8 byte4, u8 byte5)
+static void halbtc8192e2ant_SetFwPstdma(struct btc_coexist *btcoexist, u8 byte1,
+					u8 byte2, u8 byte3, u8 byte4, u8 byte5)
 {
-	u8 h2c_parameter[5] ={0};
+	u8 h2c_parameter[5] = {0};
 
 	h2c_parameter[0] = byte1;
 	h2c_parameter[1] = byte2;
@@ -1287,29 +1171,33 @@ void halbtc8192e2ant_SetFwPstdma(struct btc_coexist *btcoexist, u8 byte1,
 	btcoexist->btc_fill_h2c(btcoexist, 0x60, 5, h2c_parameter);
 }
 
-void halbtc8192e2ant_sw_mechanism1(struct btc_coexist *btcoexist,
-				   bool shrink_rx_lpf, bool low_penalty_ra,
-				   bool limited_dig, bool btlan_constrain)
+static void halbtc8192e2ant_sw_mechanism1(struct btc_coexist *btcoexist,
+					  bool shrink_rx_lpf,
+					  bool low_penalty_ra,
+					  bool limited_dig,
+					  bool btlan_constrain)
 {
 	halbtc8192e2ant_rf_shrink(btcoexist, NORMAL_EXEC, shrink_rx_lpf);
 }
 
-void halbtc8192e2ant_sw_mechanism2(struct btc_coexist *btcoexist,
-				   bool agc_table_shift, bool adc_backoff,
-				   bool sw_dac_swing, u32 dac_swinglvl)
+static void halbtc8192e2ant_sw_mechanism2(struct btc_coexist *btcoexist,
+					  bool agc_table_shift,
+					  bool adc_backoff,
+					  bool sw_dac_swing,
+					  u32 dac_swinglvl)
 {
 	halbtc8192e2ant_AgcTable(btcoexist, NORMAL_EXEC, agc_table_shift);
 	halbtc8192e2ant_DacSwing(btcoexist, NORMAL_EXEC, sw_dac_swing,
 				 dac_swinglvl);
 }
 
-void halbtc8192e2ant_ps_tdma(struct btc_coexist *btcoexist,
-			     bool force_exec, bool turn_on, u8 type)
+static void halbtc8192e2ant_ps_tdma(struct btc_coexist *btcoexist,
+				    bool force_exec, bool turn_on, u8 type)
 {
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
 		  "[BTCoex], %s turn %s PS TDMA, type=%d\n",
-		  (force_exec? "force to":""), (turn_on? "ON":"OFF"), type);
+		  (force_exec ? "force to":""), (turn_on ? "ON":"OFF"), type);
 	coex_dm->cur_ps_tdma_on = turn_on;
 	coex_dm->cur_ps_tdma = type;
 
@@ -1440,7 +1328,8 @@ void halbtc8192e2ant_ps_tdma(struct btc_coexist *btcoexist,
 	coex_dm->pre_ps_tdma = coex_dm->cur_ps_tdma;
 }
 
-void halbtc8192e2ant_set_switch_sstype(struct btc_coexist *btcoexist, u8 sstype)
+static void halbtc8192e2ant_set_switch_sstype(struct btc_coexist *btcoexist,
+					      u8 sstype)
 {
 	u8 mimops = BTC_MIMO_PS_DYNAMIC;
 	u32 disra_mask = 0x0;
@@ -1461,7 +1350,7 @@ void halbtc8192e2ant_set_switch_sstype(struct btc_coexist *btcoexist, u8 sstype)
 		/* switch cck patch */
 		btcoexist->btc_write_1byte_bitmask(btcoexist, 0xe77, 0x4, 0x1);
 		btcoexist->btc_write_1byte(btcoexist, 0xa07, 0x81);
-		mimops=BTC_MIMO_PS_STATIC;
+		mimops = BTC_MIMO_PS_STATIC;
 	} else if (sstype == 2) {
 		halbtc8192e2ant_ps_tdma(btcoexist, FORCE_EXEC, false, 0);
 		btcoexist->btc_write_1byte(btcoexist, 0xc04, 0x33);
@@ -1469,18 +1358,18 @@ void halbtc8192e2ant_set_switch_sstype(struct btc_coexist *btcoexist, u8 sstype)
 		btcoexist->btc_write_4byte(btcoexist, 0x90c, 0x81121313);
 		btcoexist->btc_write_1byte_bitmask(btcoexist, 0xe77, 0x4, 0x0);
 		btcoexist->btc_write_1byte(btcoexist, 0xa07, 0x41);
-		mimops=BTC_MIMO_PS_DYNAMIC;
+		mimops = BTC_MIMO_PS_DYNAMIC;
 	}
 	/* set rx 1ss or 2ss */
 	btcoexist->btc_set(btcoexist, BTC_SET_ACT_SEND_MIMO_PS, &mimops);
 }
 
-void halbtc8192e2ant_switch_sstype(struct btc_coexist *btcoexist,
+static void halbtc8192e2ant_switch_sstype(struct btc_coexist *btcoexist,
 				   bool force_exec, u8 new_sstype)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 		  "[BTCoex], %s Switch SS Type = %d\n",
-		  (force_exec? "force to":""), new_sstype);
+		  (force_exec ? "force to":""), new_sstype);
 	coex_dm->cur_sstype = new_sstype;
 
 	if (!force_exec) {
@@ -1492,7 +1381,7 @@ void halbtc8192e2ant_switch_sstype(struct btc_coexist *btcoexist,
 	coex_dm->pre_sstype = coex_dm->cur_sstype;
 }
 
-void halbtc8192e2ant_coex_alloff(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_coex_alloff(struct btc_coexist *btcoexist)
 {
 	/* fw all off */
 	halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
@@ -1507,7 +1396,7 @@ void halbtc8192e2ant_coex_alloff(struct btc_coexist *btcoexist)
 	halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
 }
 
-void halbtc8192e2ant_init_coex_dm(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_init_coex_dm(struct btc_coexist *btcoexist)
 {
 	/* force to reset coex mechanism */
 
@@ -1522,7 +1411,7 @@ void halbtc8192e2ant_init_coex_dm(struct btc_coexist *btcoexist)
 	halbtc8192e2ant_sw_mechanism2(btcoexist, false, false, false, 0x18);
 }
 
-void halbtc8192e2ant_action_bt_inquiry(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_action_bt_inquiry(struct btc_coexist *btcoexist)
 {
 	bool low_pwr_disable = true;
 
@@ -1540,7 +1429,7 @@ void halbtc8192e2ant_action_bt_inquiry(struct btc_coexist *btcoexist)
 	halbtc8192e2ant_sw_mechanism2(btcoexist, false, false, false, 0x18);
 }
 
-bool halbtc8192e2ant_is_common_action(struct btc_coexist *btcoexist)
+static bool halbtc8192e2ant_is_common_action(struct btc_coexist *btcoexist)
 {
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
 	bool common = false, wifi_connected = false, wifi_busy = false;
@@ -1586,7 +1475,7 @@ bool halbtc8192e2ant_is_common_action(struct btc_coexist *btcoexist)
 		halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
 		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
 
- 		halbtc8192e2ant_sw_mechanism1(btcoexist, false, false, false,
+		halbtc8192e2ant_sw_mechanism1(btcoexist, false, false, false,
 					      false);
 		halbtc8192e2ant_sw_mechanism2(btcoexist, false, false, false,
 					      0x18);
@@ -1685,9 +1574,9 @@ bool halbtc8192e2ant_is_common_action(struct btc_coexist *btcoexist)
 	return common;
 }
 
-void halbtc8192e2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
-					  bool sco_hid, bool tx_pause,
-					  u8 max_interval)
+static void halbtc8192e2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
+						 bool sco_hid, bool tx_pause,
+						 u8 max_interval)
 {
 	static int up, dn, m, n, wait_cnt;
 	/* 0: no change, +1: increase WiFi duration,
@@ -1799,7 +1688,7 @@ void halbtc8192e2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
 		up = 0;
 		dn = 0;
 		m = 1;
-		n= 3;
+		n = 3;
 		result = 0;
 		wait_cnt = 0;
 	} else {
@@ -2116,7 +2005,7 @@ void halbtc8192e2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
 									     12;
 					}
 				} else if (result == 1) {
- 					if (coex_dm->cur_ps_tdma == 4) {
+					if (coex_dm->cur_ps_tdma == 4) {
 						halbtc8192e2ant_ps_tdma(
 								    btcoexist,
 								    NORMAL_EXEC,
@@ -2416,21 +2305,21 @@ void halbtc8192e2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
 								    true, 2);
 						coex_dm->ps_tdma_du_adj_type =
 									     2;
-					} else if(coex_dm->cur_ps_tdma == 12) {
+					} else if (coex_dm->cur_ps_tdma == 12) {
 						halbtc8192e2ant_ps_tdma(
 								    btcoexist,
 								    NORMAL_EXEC,
 								    true, 11);
 						coex_dm->ps_tdma_du_adj_type =
 									     11;
-					} else if(coex_dm->cur_ps_tdma == 11) {
+					} else if (coex_dm->cur_ps_tdma == 11) {
 						halbtc8192e2ant_ps_tdma(
 								    btcoexist,
 								    NORMAL_EXEC,
 								    true, 10);
 						coex_dm->ps_tdma_du_adj_type =
 									     10;
-					} else if(coex_dm->cur_ps_tdma == 10) {
+					} else if (coex_dm->cur_ps_tdma == 10) {
 						halbtc8192e2ant_ps_tdma(
 								    btcoexist,
 								    NORMAL_EXEC,
@@ -2721,7 +2610,7 @@ void halbtc8192e2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
 	if (coex_dm->cur_ps_tdma != coex_dm->ps_tdma_du_adj_type) {
 		bool scan = false, link = false, roam = false;
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], PsTdma type dismatch!!!, " );
+			  "[BTCoex], PsTdma type dismatch!!!, ");
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
 			  "curPsTdma=%d, recordPsTdma=%d\n",
 			  coex_dm->cur_ps_tdma, coex_dm->ps_tdma_du_adj_type);
@@ -2730,7 +2619,7 @@ void halbtc8192e2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
 		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
 		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
 
-		if ( !scan && !link && !roam)
+		if (!scan && !link && !roam)
 			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
 						true,
 						coex_dm->ps_tdma_du_adj_type);
@@ -2742,7 +2631,7 @@ void halbtc8192e2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
 }
 
 /* SCO only or SCO+PAN(HS) */
-void halbtc8192e2ant_action_sco(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_action_sco(struct btc_coexist *btcoexist)
 {
 	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_STAY_LOW;
 	u32 wifi_bw;
@@ -2804,7 +2693,7 @@ void halbtc8192e2ant_action_sco(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8192e2ant_action_sco_pan(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_action_sco_pan(struct btc_coexist *btcoexist)
 {
 	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_STAY_LOW;
 	u32 wifi_bw;
@@ -2866,9 +2755,9 @@ void halbtc8192e2ant_action_sco_pan(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8192e2ant_action_hid(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_action_hid(struct btc_coexist *btcoexist)
 {
-	u8 wifirssi_state, btrssi_state=BTC_RSSI_STATE_HIGH;
+	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
 	u32 wifi_bw;
 
 	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
@@ -2906,7 +2795,7 @@ void halbtc8192e2ant_action_hid(struct btc_coexist *btcoexist)
 			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
 						      false, 0x18);
 		} else {
- 			halbtc8192e2ant_sw_mechanism1(btcoexist, true, true,
+			halbtc8192e2ant_sw_mechanism1(btcoexist, true, true,
 						      false, false);
 			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
 						      false, 0x18);
@@ -2918,8 +2807,8 @@ void halbtc8192e2ant_action_hid(struct btc_coexist *btcoexist)
 						      false, false);
 			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
 						      false, 0x18);
- 		} else {
- 			halbtc8192e2ant_sw_mechanism1(btcoexist, false, true,
+		} else {
+			halbtc8192e2ant_sw_mechanism1(btcoexist, false, true,
 						      false, false);
 			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
 						      false, 0x18);
@@ -2928,7 +2817,7 @@ void halbtc8192e2ant_action_hid(struct btc_coexist *btcoexist)
 }
 
 /* A2DP only / PAN(EDR) only/ A2DP+PAN(HS) */
-void halbtc8192e2ant_action_a2dp(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_action_a2dp(struct btc_coexist *btcoexist)
 {
 	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
 	u32 wifi_bw;
@@ -2991,7 +2880,7 @@ void halbtc8192e2ant_action_a2dp(struct btc_coexist *btcoexist)
 	if (BTC_WIFI_BW_HT40 == wifi_bw) {
 		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
- 			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
+			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
 						      false, false);
 			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
 						      false, 0x18);
@@ -3017,7 +2906,7 @@ void halbtc8192e2ant_action_a2dp(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8192e2ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
 {
 	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
 	u32 wifi_bw;
@@ -3052,7 +2941,7 @@ void halbtc8192e2ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
 	if (BTC_WIFI_BW_HT40 == wifi_bw) {
 		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
- 			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
+			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
 						      false, false);
 			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
 						      true, 0x6);
@@ -3078,7 +2967,7 @@ void halbtc8192e2ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8192e2ant_action_pan_edr(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_action_pan_edr(struct btc_coexist *btcoexist)
 {
 	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
 	u32 wifi_bw;
@@ -3139,7 +3028,7 @@ void halbtc8192e2ant_action_pan_edr(struct btc_coexist *btcoexist)
 }
 
 /* PAN(HS) only */
-void halbtc8192e2ant_action_pan_hs(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_action_pan_hs(struct btc_coexist *btcoexist)
 {
 	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
 	u32 wifi_bw;
@@ -3197,9 +3086,9 @@ void halbtc8192e2ant_action_pan_hs(struct btc_coexist *btcoexist)
 }
 
 /* PAN(EDR)+A2DP */
-void halbtc8192e2ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
 {
-	u8 wifirssi_state, btrssi_state=BTC_RSSI_STATE_HIGH;
+	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
 	u32 wifi_bw;
 
 	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
@@ -3260,7 +3149,7 @@ void halbtc8192e2ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8192e2ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
 {
 	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
 	u32 wifi_bw;
@@ -3280,15 +3169,15 @@ void halbtc8192e2ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
 	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
 	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
 		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
-	        halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 14);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 14);
 	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
 		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
 		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
-	        halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 10);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 10);
 	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
 		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
 		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
-	        halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 10);
+		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 10);
 	}
 
 	/* sw mechanism */
@@ -3322,7 +3211,7 @@ void halbtc8192e2ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
 }
 
 /* HID+A2DP+PAN(EDR) */
-void halbtc8192e2ant_action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
 {
 	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
 	u32 wifi_bw;
@@ -3383,7 +3272,7 @@ void halbtc8192e2ant_action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8192e2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
 {
 	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
 	u32 wifi_bw;
@@ -3403,7 +3292,7 @@ void halbtc8192e2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
 		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
 		halbtc8192e2ant_tdma_duration_adjust(btcoexist, true, true, 2);
 	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
-		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) 	{
+		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
 		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
 		halbtc8192e2ant_tdma_duration_adjust(btcoexist, true, false, 2);
 	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
@@ -3442,7 +3331,7 @@ void halbtc8192e2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8192e2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
+static void halbtc8192e2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 {
 	u8 algorithm = 0;
 
@@ -3472,7 +3361,7 @@ void halbtc8192e2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 
 	coex_dm->cur_algorithm = algorithm;
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-		  "[BTCoex], Algorithm = %d \n", coex_dm->cur_algorithm);
+		  "[BTCoex], Algorithm = %d\n", coex_dm->cur_algorithm);
 
 	if (halbtc8192e2ant_is_common_action(btcoexist)) {
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
@@ -3552,7 +3441,8 @@ void halbtc8192e2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8192e2ant_init_hwconfig(struct btc_coexist *btcoexist, bool backup)
+static void halbtc8192e2ant_init_hwconfig(struct btc_coexist *btcoexist,
+					  bool backup)
 {
 	u16 u16tmp = 0;
 	u8 u8tmp = 0;
@@ -3649,7 +3539,7 @@ void ex_halbtc8192e2ant_init_coex_dm(struct btc_coexist *btcoexist)
 void ex_halbtc8192e2ant_display_coex_info(struct btc_coexist *btcoexist)
 {
 	struct btc_board_info *board_info = &btcoexist->board_info;
-	struct btc_stack_info*stack_info = &btcoexist->stack_info;
+	struct btc_stack_info *stack_info = &btcoexist->stack_info;
 	u8 *cli_buf = btcoexist->cli_buf;
 	u8 u8tmp[4], i, bt_info_ext, ps_tdma_case = 0;
 	u16 u16tmp[4];
@@ -3735,9 +3625,9 @@ void ex_halbtc8192e2ant_display_coex_info(struct btc_coexist *btcoexist)
 	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %s/ %s ",
 		   "Wifi status", (wifi_under_5g ? "5G" : "2.4G"),
 		   ((BTC_WIFI_BW_LEGACY == wifi_bw) ? "Legacy" :
-		   	(((BTC_WIFI_BW_HT40 == wifi_bw) ? "HT40" : "HT20"))),
+			(((BTC_WIFI_BW_HT40 == wifi_bw) ? "HT40" : "HT20"))),
 		   ((!wifi_busy) ? "idle" :
-		   	((BTC_WIFI_TRAFFIC_TX == wifi_traffic_dir) ?
+			((BTC_WIFI_TRAFFIC_TX == wifi_traffic_dir) ?
 				"uplink" : "downlink")));
 	CL_PRINTF(cli_buf);
 
@@ -3766,7 +3656,7 @@ void ex_halbtc8192e2ant_display_coex_info(struct btc_coexist *btcoexist)
 		   (bt_info_ext&BIT0) ? "Basic rate" : "EDR rate");
 	CL_PRINTF(cli_buf);
 
-	for (i=0; i<BT_INFO_SRC_8192E_2ANT_MAX; i++) {
+	for (i = 0; i < BT_INFO_SRC_8192E_2ANT_MAX; i++) {
 		if (coex_sta->bt_info_c2h_cnt[i]) {
 			CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
 				   "\r\n %-35s = %02x %02x %02x %02x ",
@@ -3911,9 +3801,6 @@ void ex_halbtc8192e2ant_display_coex_info(struct btc_coexist *btcoexist)
 		   "0x774(lp rx[31:16]/tx[15:0])",
 		   coex_sta->low_priority_rx, coex_sta->low_priority_tx);
 	CL_PRINTF(cli_buf);
-#if(BT_AUTO_REPORT_ONLY_8192E_2ANT == 1)
-	halbtc8192e2ant_monitor_bt_ctr(btcoexist);
-#endif
 	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_COEX_STATISTICS);
 }
 
@@ -3950,7 +3837,7 @@ void ex_halbtc8192e2ant_scan_notify(struct btc_coexist *btcoexist, u8 type)
 	if (BTC_SCAN_START == type)
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], SCAN START notify\n");
-	else if(BTC_SCAN_FINISH == type)
+	else if (BTC_SCAN_FINISH == type)
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], SCAN FINISH notify\n");
 }
@@ -3960,7 +3847,7 @@ void ex_halbtc8192e2ant_connect_notify(struct btc_coexist *btcoexist, u8 type)
 	if (BTC_ASSOCIATE_START == type)
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], CONNECT START notify\n");
-	else if(BTC_ASSOCIATE_FINISH == type)
+	else if (BTC_ASSOCIATE_FINISH == type)
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], CONNECT FINISH notify\n");
 }
@@ -3968,7 +3855,7 @@ void ex_halbtc8192e2ant_connect_notify(struct btc_coexist *btcoexist, u8 type)
 void ex_halbtc8192e2ant_media_status_notify(struct btc_coexist *btcoexist,
 					    u8 type)
 {
-	u8 h2c_parameter[3] ={0};
+	u8 h2c_parameter[3] = {0};
 	u32 wifi_bw;
 	u8 wifi_center_chnl;
 
@@ -4014,12 +3901,12 @@ void ex_halbtc8192e2ant_special_packet_notify(struct btc_coexist *btcoexist,
 					      u8 type)
 {
 	if (type == BTC_PACKET_DHCP)
- 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], DHCP Packet notify\n");
- }
+}
 
 void ex_halbtc8192e2ant_bt_info_notify(struct btc_coexist *btcoexist,
-				       u8 *tmp_buf, u8 length )
+				       u8 *tmp_buf, u8 length)
 {
 	u8 bt_info = 0;
 	u8 i, rspSource = 0;
@@ -4089,18 +3976,16 @@ void ex_halbtc8192e2ant_bt_info_notify(struct btc_coexist *btcoexist,
 			 * do nothing here. */
 		}
 
-#if(BT_AUTO_REPORT_ONLY_8192E_2ANT == 0)
 		if ((coex_sta->bt_info_ext & BIT4)) {
 			/* BT auto report already enabled, do nothing */
 		} else {
 			halbtc8192e2ant_bt_autoreport(btcoexist, FORCE_EXEC,
 						      true);
 		}
-#endif
 	}
 
 	/* check BIT2 first ==> check if bt is under inquiry or page scan */
-	if(bt_info & BT_INFO_8192E_2ANT_B_INQ_PAGE)
+	if (bt_info & BT_INFO_8192E_2ANT_B_INQ_PAGE)
 		coex_sta->c2h_bt_inquiry_page = true;
 	else
 		coex_sta->c2h_bt_inquiry_page = false;
@@ -4180,10 +4065,10 @@ void ex_halbtc8192e2ant_stack_operation_notify(struct btc_coexist *btcoexist,
 {
 	if (BTC_STACK_OP_INQ_PAGE_PAIR_START == type)
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-		          "[BTCoex] StackOP Inquiry/page/pair start notify\n");
-	else if(BTC_STACK_OP_INQ_PAGE_PAIR_FINISH == type)
+			  "[BTCoex] StackOP Inquiry/page/pair start notify\n");
+	else if (BTC_STACK_OP_INQ_PAGE_PAIR_FINISH == type)
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-		          "[BTCoex] StackOP Inquiry/page/pair finish notify\n");
+			  "[BTCoex] StackOP Inquiry/page/pair finish notify\n");
 }
 
 void ex_halbtc8192e2ant_halt_notify(struct btc_coexist *btcoexist)
@@ -4196,10 +4081,10 @@ void ex_halbtc8192e2ant_halt_notify(struct btc_coexist *btcoexist)
 
 void ex_halbtc8192e2ant_periodical(struct btc_coexist *btcoexist)
 {
-	static u8 dis_ver_info_cnt = 0;
+	static u8 dis_ver_info_cnt;
 	u32 fw_ver = 0, bt_patch_ver = 0;
-	struct btc_board_info *board_info=&btcoexist->board_info;
-	struct btc_stack_info *stack_info=&btcoexist->stack_info;
+	struct btc_board_info *board_info = &btcoexist->board_info;
+	struct btc_stack_info *stack_info = &btcoexist->stack_info;
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 		  "=======================Periodical=======================\n");
@@ -4226,17 +4111,8 @@ void ex_halbtc8192e2ant_periodical(struct btc_coexist *btcoexist)
 			  "************************************************\n");
 	}
 
-#if(BT_AUTO_REPORT_ONLY_8192E_2ANT == 0)
 	halbtc8192e2ant_querybt_info(btcoexist);
 	halbtc8192e2ant_monitor_bt_ctr(btcoexist);
 	halbtc8192e2ant_monitor_bt_enable_disable(btcoexist);
-#else
-	if (halbtc8192e2ant_iswifi_status_changed(btcoexist) ||
-	    coex_dm->auto_tdma_adjust)
-		halbtc8192e2ant_run_coexist_mechanism(btcoexist);
-#endif
 }
 
-
-#endif
-
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtc8192e2ant.h b/drivers/staging/rtl8821ae/btcoexist/halbtc8192e2ant.h
index 6d109edb895..416d3ddaed3 100644
--- a/drivers/staging/rtl8821ae/btcoexist/halbtc8192e2ant.h
+++ b/drivers/staging/rtl8821ae/btcoexist/halbtc8192e2ant.h
@@ -1,8 +1,6 @@
 /*****************************************************************
  *   The following is for 8192E 2Ant BT Co-exist definition
  *****************************************************************/
-#define	BT_AUTO_REPORT_ONLY_8192E_2ANT			0
-
 #define	BT_INFO_8192E_2ANT_B_FTP			BIT7
 #define	BT_INFO_8192E_2ANT_B_A2DP			BIT6
 #define	BT_INFO_8192E_2ANT_B_HID			BIT5
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtc8723b1ant.c b/drivers/staging/rtl8821ae/btcoexist/halbtc8723b1ant.c
index 9677943fc20..c9693944c13 100644
--- a/drivers/staging/rtl8821ae/btcoexist/halbtc8723b1ant.c
+++ b/drivers/staging/rtl8821ae/btcoexist/halbtc8723b1ant.c
@@ -22,14 +22,14 @@ static struct coex_dm_8723b_1ant *coex_dm = &glcoex_dm_8723b_1ant;
 static struct coex_sta_8723b_1ant glcoex_sta_8723b_1ant;
 static struct coex_sta_8723b_1ant *coex_sta = &glcoex_sta_8723b_1ant;
 
-const char *const GLBtInfoSrc8723b1Ant[]={
+static const char *const GLBtInfoSrc8723b1Ant[] = {
 	"BT Info[wifi fw]",
 	"BT Info[bt rsp]",
 	"BT Info[bt auto report]",
 };
 
-u32 glcoex_ver_date_8723b_1ant = 20130906;
-u32 glcoex_ver_8723b_1ant = 0x45;
+static u32 glcoex_ver_date_8723b_1ant = 20130906;
+static u32 glcoex_ver_8723b_1ant = 0x45;
 
 /***************************************************************
  * local function proto type if needed
@@ -37,7 +37,8 @@ u32 glcoex_ver_8723b_1ant = 0x45;
 /***************************************************************
  * local function start with halbtc8723b1ant_
  ***************************************************************/
-u8 halbtc8723b1ant_bt_rssi_state(u8 level_num, u8 rssi_thresh, u8 rssi_thresh1)
+static u8 halbtc8723b1ant_bt_rssi_state(u8 level_num, u8 rssi_thresh,
+					u8 rssi_thresh1)
 {
 	s32 bt_rssi=0;
 	u8 bt_rssi_state = coex_sta->pre_bt_rssi_state;
@@ -134,9 +135,9 @@ u8 halbtc8723b1ant_bt_rssi_state(u8 level_num, u8 rssi_thresh, u8 rssi_thresh1)
 	return bt_rssi_state;
 }
 
-u8 halbtc8723b1ant_wifi_rssi_state(struct btc_coexist *btcoexist,
-				   u8 index, u8 level_num,
-				   u8 rssi_thresh, u8 rssi_thresh1)
+static u8 halbtc8723b1ant_wifi_rssi_state(struct btc_coexist *btcoexist,
+					u8 index, u8 level_num,
+					u8 rssi_thresh, u8 rssi_thresh1)
 {
 	s32 wifi_rssi=0;
 	u8 wifi_rssi_state = coex_sta->pre_wifi_rssi_state[index];
@@ -249,8 +250,8 @@ u8 halbtc8723b1ant_wifi_rssi_state(struct btc_coexist *btcoexist,
 	return wifi_rssi_state;
 }
 
-void halbtc8723b1ant_updatera_mask(struct btc_coexist *btcoexist,
-				   bool force_exec, u32 dis_rate_mask)
+static void halbtc8723b1ant_updatera_mask(struct btc_coexist *btcoexist,
+					bool force_exec, u32 dis_rate_mask)
 {
 	coex_dm->curra_mask = dis_rate_mask;
 
@@ -261,8 +262,9 @@ void halbtc8723b1ant_updatera_mask(struct btc_coexist *btcoexist,
 	coex_dm->prera_mask = coex_dm->curra_mask;
 }
 
-void halbtc8723b1ant_auto_rate_fallback_retry(struct btc_coexist *btcoexist,
-					      bool force_exec, u8 type)
+static void halbtc8723b1ant_auto_rate_fallback_retry(
+						struct btc_coexist *btcoexist,
+						bool force_exec, u8 type)
 {
 	bool wifi_under_bmode = false;
 
@@ -300,8 +302,8 @@ void halbtc8723b1ant_auto_rate_fallback_retry(struct btc_coexist *btcoexist,
 	coex_dm->pre_arfr_type = coex_dm->cur_arfr_type;
 }
 
-void halbtc8723b1ant_retry_limit(struct btc_coexist *btcoexist,
-				 bool force_exec, u8 type)
+static void halbtc8723b1ant_retry_limit(struct btc_coexist *btcoexist,
+					bool force_exec, u8 type)
 {
 	coex_dm->cur_retry_limit_type = type;
 
@@ -324,8 +326,8 @@ void halbtc8723b1ant_retry_limit(struct btc_coexist *btcoexist,
 	coex_dm->pre_retry_limit_type = coex_dm->cur_retry_limit_type;
 }
 
-void halbtc8723b1ant_ampdu_maxtime(struct btc_coexist *btcoexist,
-				   bool force_exec, u8 type)
+static void halbtc8723b1ant_ampdu_maxtime(struct btc_coexist *btcoexist,
+					bool force_exec, u8 type)
 {
 	coex_dm->cur_ampdu_time_type = type;
 
@@ -348,7 +350,7 @@ void halbtc8723b1ant_ampdu_maxtime(struct btc_coexist *btcoexist,
 	coex_dm->pre_ampdu_time_type = coex_dm->cur_ampdu_time_type;
 }
 
-void halbtc8723b1ant_limited_tx(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_limited_tx(struct btc_coexist *btcoexist,
 				bool force_exec, u8 ra_maskType, u8 arfr_type,
 				u8 retry_limit_type, u8 ampdu_time_type)
 {
@@ -375,7 +377,7 @@ void halbtc8723b1ant_limited_tx(struct btc_coexist *btcoexist,
 	halbtc8723b1ant_ampdu_maxtime(btcoexist, force_exec, ampdu_time_type);
 }
 
-void halbtc8723b1ant_limited_rx(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_limited_rx(struct btc_coexist *btcoexist,
 				bool force_exec, bool rej_ap_agg_pkt,
 				bool b_bt_ctrl_agg_buf_size, u8 agg_buf_size)
 {
@@ -398,7 +400,7 @@ void halbtc8723b1ant_limited_rx(struct btc_coexist *btcoexist,
 	btcoexist->btc_set(btcoexist, BTC_SET_ACT_AGGREGATE_CTRL, NULL);
 }
 
-void halbtc8723b1ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
 {
 	u32 reg_hp_txrx, reg_lp_txrx, u32tmp;
 	u32 reg_hp_tx = 0, reg_hp_rx = 0;
@@ -424,7 +426,7 @@ void halbtc8723b1ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
 	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
 }
 
-void halbtc8723b1ant_query_bt_info(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_query_bt_info(struct btc_coexist *btcoexist)
 {
 	u8 h2c_parameter[1] = {0};
 
@@ -439,7 +441,8 @@ void halbtc8723b1ant_query_bt_info(struct btc_coexist *btcoexist)
 	btcoexist->btc_fill_h2c(btcoexist, 0x61, 1, h2c_parameter);
 }
 
-bool halbtc8723b1ant_is_wifi_status_changed(struct btc_coexist *btcoexist)
+static bool halbtc8723b1ant_is_wifi_status_changed(
+						struct btc_coexist *btcoexist)
 {
 	static bool pre_wifi_busy = false;
 	static bool pre_under_4way = false, pre_bt_hs_on = false;
@@ -471,7 +474,7 @@ bool halbtc8723b1ant_is_wifi_status_changed(struct btc_coexist *btcoexist)
 	return false;
 }
 
-void halbtc8723b1ant_update_bt_link_info(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_update_bt_link_info(struct btc_coexist *btcoexist)
 {
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
 	bool bt_hs_on = false;
@@ -519,7 +522,7 @@ void halbtc8723b1ant_update_bt_link_info(struct btc_coexist *btcoexist)
 		bt_link_info->hid_only = false;
 }
 
-u8 halbtc8723b1ant_action_algorithm(struct btc_coexist *btcoexist)
+static u8 halbtc8723b1ant_action_algorithm(struct btc_coexist *btcoexist)
 {
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
 	bool bt_hs_on = false;
@@ -719,7 +722,7 @@ u8 halbtc8723b1ant_action_algorithm(struct btc_coexist *btcoexist)
 	return algorithm;
 }
 
-bool halbtc8723b1ant_need_to_dec_bt_pwr(struct btc_coexist *btcoexist)
+static bool halbtc8723b1ant_need_to_dec_bt_pwr(struct btc_coexist *btcoexist)
 {
 	bool ret = false;
 	bool bt_hs_on = false, wifi_connected = false;
@@ -750,8 +753,9 @@ bool halbtc8723b1ant_need_to_dec_bt_pwr(struct btc_coexist *btcoexist)
 	return ret;
 }
 
-void halbtc8723b1ant_set_fw_dac_swing_level(struct btc_coexist *btcoexist,
-					    u8 dac_swing_lvl)
+static void halbtc8723b1ant_set_fw_dac_swing_level(
+						struct btc_coexist *btcoexist,
+						u8 dac_swing_lvl)
 {
 	u8 h2c_parameter[1] = {0};
 
@@ -767,7 +771,7 @@ void halbtc8723b1ant_set_fw_dac_swing_level(struct btc_coexist *btcoexist,
 	btcoexist->btc_fill_h2c(btcoexist, 0x64, 1, h2c_parameter);
 }
 
-void halbtc8723b1ant_set_fw_dec_bt_pwr(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_set_fw_dec_bt_pwr(struct btc_coexist *btcoexist,
 				       bool dec_bt_pwr)
 {
 	u8 h2c_parameter[1] = {0};
@@ -784,7 +788,7 @@ void halbtc8723b1ant_set_fw_dec_bt_pwr(struct btc_coexist *btcoexist,
 	btcoexist->btc_fill_h2c(btcoexist, 0x62, 1, h2c_parameter);
 }
 
-void halbtc8723b1ant_dec_bt_pwr(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_dec_bt_pwr(struct btc_coexist *btcoexist,
 				bool force_exec, bool dec_bt_pwr)
 {
 	return;
@@ -806,7 +810,8 @@ void halbtc8723b1ant_dec_bt_pwr(struct btc_coexist *btcoexist,
 	coex_dm->pre_dec_bt_pwr = coex_dm->cur_dec_bt_pwr;
 }
 
-void halbtc8723b1ant_set_bt_auto_report(struct btc_coexist *btcoexist,
+#if (BT_AUTO_REPORT_ONLY_8723B_1ANT == 0)
+static void halbtc8723b1ant_set_bt_auto_report(struct btc_coexist *btcoexist,
 					bool enable_auto_report)
 {
 	u8 h2c_parameter[1] = {0};
@@ -824,7 +829,7 @@ void halbtc8723b1ant_set_bt_auto_report(struct btc_coexist *btcoexist,
 	btcoexist->btc_fill_h2c(btcoexist, 0x68, 1, h2c_parameter);
 }
 
-void halbtc8723b1ant_bt_auto_report(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_bt_auto_report(struct btc_coexist *btcoexist,
 				    bool force_exec, bool enable_auto_report)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
@@ -848,8 +853,9 @@ void halbtc8723b1ant_bt_auto_report(struct btc_coexist *btcoexist,
 
 	coex_dm->pre_bt_auto_report = coex_dm->cur_bt_auto_report;
 }
+#endif
 
-void halbtc8723b1ant_fw_dac_swing_lvl(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_fw_dac_swing_lvl(struct btc_coexist *btcoexist,
 				      bool force_exec, u8 fw_dac_swing_lvl)
 {
 	return;
@@ -876,55 +882,7 @@ void halbtc8723b1ant_fw_dac_swing_lvl(struct btc_coexist *btcoexist,
 	coex_dm->pre_fw_dac_swing_lvl = coex_dm->cur_fw_dac_swing_lvl;
 }
 
-void halbtc8723b1ant_set_sw_rf_rx_lpf_corner(struct btc_coexist *btcoexist,
-					     bool rx_rf_shrink_on)
-{
-	if (rx_rf_shrink_on) {
-		/*Shrink RF Rx LPF corner */
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-			  "[BTCoex], Shrink RF Rx LPF corner!!\n");
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1e,
-					  0xfffff, 0xffff7);
-	} else {
-		/*Resume RF Rx LPF corner
-		 * After initialized, we can use coex_dm->btRf0x1eBackup */
-		if (btcoexist->initilized) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-				  "[BTCoex], Resume RF Rx LPF corner!!\n");
-			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A,
-						  0x1e, 0xfffff,
-						  coex_dm->bt_rf0x1e_backup);
-		}
-	}
-}
-
-void halbtc8723b1ant_rf_shrink(struct btc_coexist *btcoexist,
-			       bool force_exec, bool rx_rf_shrink_on)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
-		  "[BTCoex], %s turn Rx RF Shrink = %s\n",
-		  (force_exec? "force to":""),
-		  ((rx_rf_shrink_on)? "ON":"OFF"));
-	coex_dm->cur_rf_rx_lpf_shrink = rx_rf_shrink_on;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], bPreRfRxLpfShrink=%d, "
-			  "bCurRfRxLpfShrink=%d\n",
-			  coex_dm->pre_rf_rx_lpf_shrink,
-			  coex_dm->cur_rf_rx_lpf_shrink);
-
-		if (coex_dm->pre_rf_rx_lpf_shrink ==
-		    coex_dm->cur_rf_rx_lpf_shrink)
-			return;
-	}
-	halbtc8723b1ant_set_sw_rf_rx_lpf_corner(btcoexist,
-						coex_dm->cur_rf_rx_lpf_shrink);
-
-	coex_dm->pre_rf_rx_lpf_shrink = coex_dm->cur_rf_rx_lpf_shrink;
-}
-
-void halbtc8723b1ant_set_sw_penalty_tx_rate_adaptive(
+static void halbtc8723b1ant_set_sw_penalty_tx_rate_adaptive(
 					struct btc_coexist *btcoexist,
 					bool low_penalty_ra)
 {
@@ -948,7 +906,7 @@ void halbtc8723b1ant_set_sw_penalty_tx_rate_adaptive(
 	btcoexist->btc_fill_h2c(btcoexist, 0x69, 6, h2c_parameter);
 }
 
-void halbtc8723b1ant_low_penalty_ra(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_low_penalty_ra(struct btc_coexist *btcoexist,
 				    bool force_exec, bool low_penalty_ra)
 {
 	coex_dm->cur_low_penalty_ra = low_penalty_ra;
@@ -963,152 +921,7 @@ void halbtc8723b1ant_low_penalty_ra(struct btc_coexist *btcoexist,
 	coex_dm->pre_low_penalty_ra = coex_dm->cur_low_penalty_ra;
 }
 
-void halbtc8723b1ant_set_dac_swing_reg(struct btc_coexist *btcoexist, u32 level)
-{
-	u8 val = (u8) level;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-		  "[BTCoex], Write SwDacSwing = 0x%x\n", level);
-	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x883, 0x3e, val);
-}
-
-void halbtc8723b1ant_set_sw_full_time_dac_swing(struct btc_coexist *btcoexist,
-						bool sw_dac_swing_on,
-						u32 sw_dac_swing_lvl)
-{
-	if (sw_dac_swing_on)
-		halbtc8723b1ant_set_dac_swing_reg(btcoexist, sw_dac_swing_lvl);
-	else
-		halbtc8723b1ant_set_dac_swing_reg(btcoexist, 0x18);
-}
-
-
-void halbtc8723b1ant_dac_swing(struct btc_coexist *btcoexist, bool force_exec,
-			       bool dac_swing_on, u32 dac_swing_lvl)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
-		  "[BTCoex], %s turn DacSwing=%s, dac_swing_lvl=0x%x\n",
-		  (force_exec ? "force to" : ""), (dac_swing_on ? "ON" : "OFF"),
-		  dac_swing_lvl);
-
-	coex_dm->cur_dac_swing_on = dac_swing_on;
-	coex_dm->cur_dac_swing_lvl = dac_swing_lvl;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], bPreDacSwingOn=%d, preDacSwingLvl=0x%x, "
-			  "bCurDacSwingOn=%d, curDacSwingLvl=0x%x\n",
-			  coex_dm->pre_dac_swing_on, coex_dm->pre_dac_swing_lvl,
-			  coex_dm->cur_dac_swing_on,
-			  coex_dm->cur_dac_swing_lvl);
-
-		if ((coex_dm->pre_dac_swing_on == coex_dm->cur_dac_swing_on) &&
-		    (coex_dm->pre_dac_swing_lvl == coex_dm->cur_dac_swing_lvl))
-			return;
-	}
-	mdelay(30);
-	halbtc8723b1ant_set_sw_full_time_dac_swing(btcoexist, dac_swing_on,
-						   dac_swing_lvl);
-
-	coex_dm->pre_dac_swing_on = coex_dm->cur_dac_swing_on;
-	coex_dm->pre_dac_swing_lvl = coex_dm->cur_dac_swing_lvl;
-}
-
-void halbtc8723b1ant_set_adc_backoff(struct btc_coexist *btcoexist,
-				     bool adc_backoff)
-{
-	if (adc_backoff) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-			  "[BTCoex], BB BackOff Level On!\n");
-		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x8db, 0x60, 0x3);
-	} else {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-			  "[BTCoex], BB BackOff Level Off!\n");
-		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x8db, 0x60, 0x1);
-	}
-}
-
-void halbtc8723b1ant_adc_backoff(struct btc_coexist *btcoexist,
-				 bool force_exec, bool adc_backoff)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
-		  "[BTCoex], %s turn AdcBackOff = %s\n",
-		  (force_exec ? "force to" : ""), (adc_backoff ? "ON" : "OFF"));
-	coex_dm->cur_adc_backoff = adc_backoff;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], bPreAdcBackOff=%d, bCurAdcBackOff=%d\n",
-			  coex_dm->pre_adc_backoff, coex_dm->cur_adc_backoff);
-
-		if(coex_dm->pre_adc_backoff == coex_dm->cur_adc_backoff)
-			return;
-	}
-	halbtc8723b1ant_set_adc_backoff(btcoexist, coex_dm->cur_adc_backoff);
-
-	coex_dm->pre_adc_backoff =
-		coex_dm->cur_adc_backoff;
-}
-
-void halbtc8723b1ant_set_agc_table(struct btc_coexist *btcoexist,
-				   bool adc_table_en)
-{
-	u8 rssi_adjust_val = 0;
-
-	btcoexist->btc_set_rf_reg(btcoexist,
-		BTC_RF_A, 0xef, 0xfffff, 0x02000);
-	if (adc_table_en) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-			  "[BTCoex], Agc Table On!\n");
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b,
-					  0xfffff, 0x3fa58);
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b,
-					  0xfffff, 0x37a58);
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b,
-					  0xfffff, 0x2fa58);
-		rssi_adjust_val = 8;
-	} else {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-			  "[BTCoex], Agc Table Off!\n");
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b,
-					  0xfffff, 0x39258);
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b,
-					  0xfffff, 0x31258);
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b,
-					  0xfffff, 0x29258);
-	}
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0xef, 0xfffff, 0x0);
-
-	/* set rssi_adjust_val for wifi module. */
-	btcoexist->btc_set(btcoexist, BTC_SET_U1_RSSI_ADJ_VAL_FOR_AGC_TABLE_ON,
-			   &rssi_adjust_val);
-}
-
-
-void halbtc8723b1ant_agc_table(struct btc_coexist *btcoexist,
-			       bool force_exec, bool adc_table_en)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
-		  "[BTCoex], %s %s Agc Table\n",
-		  (force_exec ? "force to" : ""),
-		  (adc_table_en ? "Enable" : "Disable"));
-	coex_dm->cur_agc_table_en = adc_table_en;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], bPreAgcTableEn=%d, bCurAgcTableEn=%d\n",
-			  coex_dm->pre_agc_table_en,
-			  coex_dm->cur_agc_table_en);
-
-		if(coex_dm->pre_agc_table_en == coex_dm->cur_agc_table_en)
-			return;
-	}
-	halbtc8723b1ant_set_agc_table(btcoexist, adc_table_en);
-
-	coex_dm->pre_agc_table_en = coex_dm->cur_agc_table_en;
-}
-
-void halbtc8723b1ant_set_coex_table(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_set_coex_table(struct btc_coexist *btcoexist,
 				    u32 val0x6c0, u32 val0x6c4,
 				    u32 val0x6c8, u8 val0x6cc)
 {
@@ -1129,7 +942,7 @@ void halbtc8723b1ant_set_coex_table(struct btc_coexist *btcoexist,
 	btcoexist->btc_write_1byte(btcoexist, 0x6cc, val0x6cc);
 }
 
-void halbtc8723b1ant_coex_table(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_coex_table(struct btc_coexist *btcoexist,
  				bool force_exec, u32 val0x6c0,
  				u32 val0x6c4, u32 val0x6c8,
  				u8 val0x6cc)
@@ -1159,7 +972,7 @@ void halbtc8723b1ant_coex_table(struct btc_coexist *btcoexist,
 	coex_dm->pre_val0x6cc = coex_dm->cur_val0x6cc;
 }
 
-void halbtc8723b1ant_coex_table_with_type(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_coex_table_with_type(struct btc_coexist *btcoexist,
 					  bool force_exec, u8 type)
 {
 	switch (type) {
@@ -1200,7 +1013,7 @@ void halbtc8723b1ant_coex_table_with_type(struct btc_coexist *btcoexist,
 	}
 }
 
-void halbtc8723b1ant_SetFwIgnoreWlanAct(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_SetFwIgnoreWlanAct(struct btc_coexist *btcoexist,
 					bool enable)
 {
 	u8 h2c_parameter[1] = {0};
@@ -1215,7 +1028,7 @@ void halbtc8723b1ant_SetFwIgnoreWlanAct(struct btc_coexist *btcoexist,
 	btcoexist->btc_fill_h2c(btcoexist, 0x63, 1, h2c_parameter);
 }
 
-void halbtc8723b1ant_ignore_wlan_act(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_ignore_wlan_act(struct btc_coexist *btcoexist,
 				     bool force_exec, bool enable)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
@@ -1239,7 +1052,7 @@ void halbtc8723b1ant_ignore_wlan_act(struct btc_coexist *btcoexist,
 	coex_dm->pre_ignore_wlan_act = coex_dm->cur_ignore_wlan_act;
 }
 
-void halbtc8723b1ant_set_fw_ps_tdma(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_set_fw_ps_tdma(struct btc_coexist *btcoexist,
 				    u8 byte1, u8 byte2, u8 byte3,
 				    u8 byte4, u8 byte5)
 {
@@ -1285,7 +1098,7 @@ void halbtc8723b1ant_set_fw_ps_tdma(struct btc_coexist *btcoexist,
 	btcoexist->btc_fill_h2c(btcoexist, 0x60, 5, h2c_parameter);
 }
 
-void halbtc8723b1ant_SetLpsRpwm(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_SetLpsRpwm(struct btc_coexist *btcoexist,
 				u8 lps_val, u8 rpwm_val)
 {
 	u8 lps = lps_val;
@@ -1295,8 +1108,8 @@ void halbtc8723b1ant_SetLpsRpwm(struct btc_coexist *btcoexist,
 	btcoexist->btc_set(btcoexist, BTC_SET_U1_1ANT_RPWM, &rpwm);
 }
 
-void halbtc8723b1ant_LpsRpwm(struct btc_coexist *btcoexist, bool force_exec,
-			     u8 lps_val, u8 rpwm_val)
+static void halbtc8723b1ant_LpsRpwm(struct btc_coexist *btcoexist,
+				bool force_exec, u8 lps_val, u8 rpwm_val)
 {
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
@@ -1326,7 +1139,7 @@ void halbtc8723b1ant_LpsRpwm(struct btc_coexist *btcoexist, bool force_exec,
 	coex_dm->pre_rpwm = coex_dm->cur_rpwm;
 }
 
-void halbtc8723b1ant_sw_mechanism1(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_sw_mechanism1(struct btc_coexist *btcoexist,
 				   bool shrink_rx_lpf, bool low_penalty_ra,
 				   bool limited_dig, bool bt_lna_constrain)
 {
@@ -1337,7 +1150,7 @@ void halbtc8723b1ant_sw_mechanism1(struct btc_coexist *btcoexist,
 	halbtc8723b1ant_low_penalty_ra(btcoexist, NORMAL_EXEC, low_penalty_ra);
 }
 
-void halbtc8723b1ant_sw_mechanism2(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_sw_mechanism2(struct btc_coexist *btcoexist,
 				   bool agc_table_shift, bool adc_backoff,
 				   bool sw_dac_swing, u32 dac_swing_lvl)
 {
@@ -1346,7 +1159,7 @@ void halbtc8723b1ant_sw_mechanism2(struct btc_coexist *btcoexist,
 		  agc_table_shift, adc_backoff, sw_dac_swing);
 }
 
-void halbtc8723b1ant_SetAntPath(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_SetAntPath(struct btc_coexist *btcoexist,
 				u8 ant_pos_type, bool init_hw_cfg,
 				bool wifi_off)
 {
@@ -1537,8 +1350,8 @@ void halbtc8723b1ant_SetAntPath(struct btc_coexist *btcoexist,
 	}
 }
 
-void halbtc8723b1ant_ps_tdma(struct btc_coexist *btcoexist, bool force_exec,
-			     bool turn_on, u8 type)
+static void halbtc8723b1ant_ps_tdma(struct btc_coexist *btcoexist,
+					bool force_exec, bool turn_on, u8 type)
 {
 	bool wifi_busy = false;
 	u8 rssi_adjust_val = 0;
@@ -1783,22 +1596,7 @@ void halbtc8723b1ant_ps_tdma(struct btc_coexist *btcoexist, bool force_exec,
 	coex_dm->pre_ps_tdma = coex_dm->cur_ps_tdma;
 }
 
-void halbtc8723b1ant_coex_alloff(struct btc_coexist *btcoexist)
-{
-	/* fw all off */
-	halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-	halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	/* sw all off */
-	halbtc8723b1ant_sw_mechanism1(btcoexist, false, false, false, false);
-	halbtc8723b1ant_sw_mechanism2(btcoexist, false, false, false, 0x18);
-
-
-	/* hw all off */
-	halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
-}
-
-bool halbtc8723b1ant_is_common_action(struct btc_coexist *btcoexist)
+static bool halbtc8723b1ant_is_common_action(struct btc_coexist *btcoexist)
 {
 	bool commom = false, wifi_connected = false;
 	bool wifi_busy = false;
@@ -1896,8 +1694,9 @@ bool halbtc8723b1ant_is_common_action(struct btc_coexist *btcoexist)
 }
 
 
-void halbtc8723b1ant_tdma_duration_adjust_for_acl(struct btc_coexist *btcoexist,
-						  u8 wifi_status)
+static void halbtc8723b1ant_tdma_duration_adjust_for_acl(
+						struct btc_coexist *btcoexist,
+						u8 wifi_status)
 {
 	static s32 up, dn, m, n, wait_count;
 	/* 0: no change, +1: increase WiFi duration,
@@ -2082,28 +1881,8 @@ void halbtc8723b1ant_tdma_duration_adjust_for_acl(struct btc_coexist *btcoexist,
 	}
 }
 
-u8 halbtc8723b1ant_ps_tdma_type_by_wifi_rssi(s32 wifi_rssi, s32 pre_wifi_rssi,
-					     u8 wifi_rssi_thresh)
-{
-	u8 ps_tdma_type=0;
-
-	if (wifi_rssi > pre_wifi_rssi) {
-		if (wifi_rssi > (wifi_rssi_thresh + 5))
-			ps_tdma_type = 26;
-		else
-			ps_tdma_type = 25;
-	} else  {
-		if (wifi_rssi > wifi_rssi_thresh)
- 			ps_tdma_type = 26;
- 		else
- 			ps_tdma_type = 25;
- 	}
-
-	return ps_tdma_type;
-}
-
-void halbtc8723b1ant_PsTdmaCheckForPowerSaveState(struct btc_coexist *btcoexist,
-						  bool new_ps_state)
+static void halbtc8723b1ant_PsTdmaCheckForPowerSaveState(
+			struct btc_coexist *btcoexist, bool new_ps_state)
 {
 	u8 lps_mode = 0x0;
 
@@ -2128,7 +1907,7 @@ void halbtc8723b1ant_PsTdmaCheckForPowerSaveState(struct btc_coexist *btcoexist,
 	}
 }
 
-void halbtc8723b1ant_power_save_state(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_power_save_state(struct btc_coexist *btcoexist,
 				      u8 ps_type, u8 lps_val,
 				      u8 rpwm_val)
 {
@@ -2162,13 +1941,15 @@ void halbtc8723b1ant_power_save_state(struct btc_coexist *btcoexist,
 	}
 }
 
-void halbtc8723b1ant_action_wifi_only(struct btc_coexist *btcoexist)
+#if (BT_AUTO_REPORT_ONLY_8723B_1ANT == 0)
+static void halbtc8723b1ant_action_wifi_only(struct btc_coexist *btcoexist)
 {
 	halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
 	halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 9);
 }
 
-void halbtc8723b1ant_monitor_bt_enable_disable(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_monitor_bt_enable_disable(
+						struct btc_coexist *btcoexist)
 {
 	static bool pre_bt_disabled = false;
 	static u32 bt_disable_cnt = 0;
@@ -2224,6 +2005,7 @@ void halbtc8723b1ant_monitor_bt_enable_disable(struct btc_coexist *btcoexist)
 		}
 	}
 }
+#endif
 
 /***************************************************
  *
@@ -2231,7 +2013,7 @@ void halbtc8723b1ant_monitor_bt_enable_disable(struct btc_coexist *btcoexist)
  *
  ***************************************************/
 /* SCO only or SCO+PAN(HS) */
-void halbtc8723b1ant_action_sco(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_action_sco(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state;
 	u32 wifi_bw;
@@ -2280,7 +2062,7 @@ void halbtc8723b1ant_action_sco(struct btc_coexist *btcoexist)
 }
 
 
-void halbtc8723b1ant_action_hid(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_action_hid(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, bt_rssi_state;
 	u32 wifi_bw;
@@ -2331,7 +2113,7 @@ void halbtc8723b1ant_action_hid(struct btc_coexist *btcoexist)
 }
 
 /*A2DP only / PAN(EDR) only/ A2DP+PAN(HS) */
-void halbtc8723b1ant_action_a2dp(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_action_a2dp(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, bt_rssi_state;
 	u32 wifi_bw;
@@ -2380,7 +2162,7 @@ void halbtc8723b1ant_action_a2dp(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b1ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, bt_rssi_state, bt_info_ext;
 	u32 wifi_bw;
@@ -2430,7 +2212,7 @@ void halbtc8723b1ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b1ant_action_pan_edr(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_action_pan_edr(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, bt_rssi_state;
 	u32 wifi_bw;
@@ -2481,7 +2263,7 @@ void halbtc8723b1ant_action_pan_edr(struct btc_coexist *btcoexist)
 
 
 /* PAN(HS) only */
-void halbtc8723b1ant_action_pan_hs(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_action_pan_hs(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, bt_rssi_state;
 	u32 wifi_bw;
@@ -2544,7 +2326,7 @@ void halbtc8723b1ant_action_pan_hs(struct btc_coexist *btcoexist)
 }
 
 /*PAN(EDR)+A2DP */
-void halbtc8723b1ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, bt_rssi_state, bt_info_ext;
 	u32 wifi_bw;
@@ -2595,7 +2377,7 @@ void halbtc8723b1ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b1ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, bt_rssi_state;
 	u32 wifi_bw;
@@ -2645,7 +2427,8 @@ void halbtc8723b1ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
 }
 
 /* HID+A2DP+PAN(EDR) */
-void halbtc8723b1ant_action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_action_hid_a2dp_pan_edr(
+						struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, bt_rssi_state, bt_info_ext;
 	u32 wifi_bw;
@@ -2695,7 +2478,7 @@ void halbtc8723b1ant_action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b1ant_action_hid_a2dp(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_action_hid_a2dp(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, bt_rssi_state, bt_info_ext;
 	u32 wifi_bw;
@@ -2748,13 +2531,13 @@ void halbtc8723b1ant_action_hid_a2dp(struct btc_coexist *btcoexist)
  *	Non-Software Coex Mechanism start
  *
  *****************************************************/
-void halbtc8723b1ant_action_hs(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_action_hs(struct btc_coexist *btcoexist)
 {
 	halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
 	halbtc8723b1ant_coex_table_with_type(btcoexist, FORCE_EXEC, 2);
 }
 
-void halbtc8723b1ant_action_bt_inquiry(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_action_bt_inquiry(struct btc_coexist *btcoexist)
 {
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
 	bool wifi_connected = false, ap_enable = false;
@@ -2790,8 +2573,9 @@ void halbtc8723b1ant_action_bt_inquiry(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b1ant_action_bt_sco_hid_only_busy(struct btc_coexist * btcoexist,
-						 u8 wifi_status)
+static void halbtc8723b1ant_action_bt_sco_hid_only_busy(
+						struct btc_coexist *btcoexist,
+						u8 wifi_status)
 {
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
 	bool wifi_connected = false;
@@ -2810,7 +2594,7 @@ void halbtc8723b1ant_action_bt_sco_hid_only_busy(struct btc_coexist * btcoexist,
 	}
 }
 
-void halbtc8723b1ant_action_wifi_connected_bt_acl_busy(
+static void halbtc8723b1ant_action_wifi_connected_bt_acl_busy(
 					struct btc_coexist *btcoexist,
 					u8 wifi_status)
 {
@@ -2870,7 +2654,8 @@ void halbtc8723b1ant_action_wifi_connected_bt_acl_busy(
 	}
 }
 
-void halbtc8723b1ant_action_wifi_not_connected(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_action_wifi_not_connected(
+						struct btc_coexist *btcoexist)
 {
 	/* power save state */
 	halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
@@ -2881,7 +2666,7 @@ void halbtc8723b1ant_action_wifi_not_connected(struct btc_coexist *btcoexist)
 	halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
 }
 
-void halbtc8723b1ant_action_wifi_not_connected_asso_auth_scan(
+static void halbtc8723b1ant_action_wifi_not_connected_asso_auth_scan(
 						struct btc_coexist *btcoexist)
 {
 	halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
@@ -2891,7 +2676,8 @@ void halbtc8723b1ant_action_wifi_not_connected_asso_auth_scan(
 	halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
 }
 
-void halbtc8723b1ant_ActionWifiConnectedScan(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_ActionWifiConnectedScan(
+						struct btc_coexist *btcoexist)
 {
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
 
@@ -2921,7 +2707,7 @@ void halbtc8723b1ant_ActionWifiConnectedScan(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b1ant_action_wifi_connected_special_packet(
+static void halbtc8723b1ant_action_wifi_connected_special_packet(
 						struct btc_coexist *btcoexist)
 {
 	bool hs_connecting = false;
@@ -2951,7 +2737,7 @@ void halbtc8723b1ant_action_wifi_connected_special_packet(
 	}
 }
 
-void halbtc8723b1ant_action_wifi_connected(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_action_wifi_connected(struct btc_coexist *btcoexist)
 {
 	bool wifi_busy = false;
 	bool scan = false, link = false, roam = false;
@@ -3031,7 +2817,8 @@ void halbtc8723b1ant_action_wifi_connected(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b1ant_run_sw_coexist_mechanism(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_run_sw_coexist_mechanism(
+						struct btc_coexist *btcoexist)
 {
 	u8 algorithm = 0;
 
@@ -3104,7 +2891,7 @@ void halbtc8723b1ant_run_sw_coexist_mechanism(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b1ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 {
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
 	bool wifi_connected = false, bt_hs_on = false;
@@ -3218,7 +3005,7 @@ void halbtc8723b1ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b1ant_init_coex_dm(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_init_coex_dm(struct btc_coexist *btcoexist)
 {
 	/* force to reset coex mechanism */
 	halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, FORCE_EXEC, 6);
@@ -3232,7 +3019,8 @@ void halbtc8723b1ant_init_coex_dm(struct btc_coexist *btcoexist)
 	halbtc8723b1ant_coex_table_with_type(btcoexist, FORCE_EXEC, 0);
 }
 
-void halbtc8723b1ant_init_hw_config(struct btc_coexist *btcoexist, bool backup)
+static void halbtc8723b1ant_init_hw_config(struct btc_coexist *btcoexist,
+					bool backup)
 {
 	u32 u32tmp = 0;
 	u8 u8tmp = 0;
@@ -3304,7 +3092,7 @@ void halbtc8723b1ant_init_hw_config(struct btc_coexist *btcoexist, bool backup)
 
 }
 
-void halbtc8723b1ant_wifi_off_hw_cfg(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_wifi_off_hw_cfg(struct btc_coexist *btcoexist)
 {
 	/* set wlan_act to low */
 	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0);
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtc8723b2ant.c b/drivers/staging/rtl8821ae/btcoexist/halbtc8723b2ant.c
index d337bd0b3c1..da3f62da4e4 100644
--- a/drivers/staging/rtl8821ae/btcoexist/halbtc8723b2ant.c
+++ b/drivers/staging/rtl8821ae/btcoexist/halbtc8723b2ant.c
@@ -20,14 +20,14 @@ static struct coex_dm_8723b_2ant *coex_dm = &glcoex_dm_8723b_2ant;
 static struct coex_sta_8723b_2ant glcoex_sta_8723b_2ant;
 static struct coex_sta_8723b_2ant *coex_sta = &glcoex_sta_8723b_2ant;
 
-const char *const glbt_info_src_8723b_2ant[] = {
+static const char *const glbt_info_src_8723b_2ant[] = {
 	"BT Info[wifi fw]",
 	"BT Info[bt rsp]",
 	"BT Info[bt auto report]",
 };
 
-u32 glcoex_ver_date_8723b_2ant = 20131113;
-u32 glcoex_ver_8723b_2ant = 0x3f;
+static u32 glcoex_ver_date_8723b_2ant = 20131113;
+static u32 glcoex_ver_8723b_2ant = 0x3f;
 
 /**************************************************************
  * local function proto type if needed
@@ -35,7 +35,8 @@ u32 glcoex_ver_8723b_2ant = 0x3f;
 /**************************************************************
  * local function start with halbtc8723b2ant_
  **************************************************************/
-u8 halbtc8723b2ant_bt_rssi_state(u8 level_num, u8 rssi_thresh, u8 rssi_thresh1)
+static u8 halbtc8723b2ant_bt_rssi_state(u8 level_num, u8 rssi_thresh,
+					u8 rssi_thresh1)
 {
 	s32 bt_rssi = 0;
 	u8 bt_rssi_state = coex_sta->pre_bt_rssi_state;
@@ -132,7 +133,7 @@ u8 halbtc8723b2ant_bt_rssi_state(u8 level_num, u8 rssi_thresh, u8 rssi_thresh1)
 	return bt_rssi_state;
 }
 
-u8 halbtc8723b2ant_wifi_rssi_state(struct btc_coexist *btcoexist,
+static u8 halbtc8723b2ant_wifi_rssi_state(struct btc_coexist *btcoexist,
 				   u8 index, u8 level_num,
 				   u8 rssi_thresh, u8 rssi_thresh1)
 {
@@ -246,7 +247,9 @@ u8 halbtc8723b2ant_wifi_rssi_state(struct btc_coexist *btcoexist,
 	return wifi_rssi_state;
 }
 
-void halbtc8723b2ant_monitor_bt_enable_disable(struct btc_coexist *btcoexist)
+#if (BT_AUTO_REPORT_ONLY_8723B_2ANT == 0)
+static void halbtc8723b2ant_monitor_bt_enable_disable(
+						struct btc_coexist *btcoexist)
 {
 	static bool pre_bt_disabled = false;
 	static u32 bt_disable_cnt = 0;
@@ -298,8 +301,9 @@ void halbtc8723b2ant_monitor_bt_enable_disable(struct btc_coexist *btcoexist)
 		}
 	}
 }
+#endif
 
-void halbtc8723b2ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
 {
 	u32 reg_hp_txrx, reg_lp_txrx, u32tmp;
 	u32 reg_hp_tx = 0, reg_hp_rx = 0;
@@ -332,7 +336,7 @@ void halbtc8723b2ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
 	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
 }
 
-void halbtc8723b2ant_query_bt_info(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_query_bt_info(struct btc_coexist *btcoexist)
 {
 	u8 h2c_parameter[1] ={0};
 
@@ -347,7 +351,8 @@ void halbtc8723b2ant_query_bt_info(struct btc_coexist *btcoexist)
 	btcoexist->btc_fill_h2c(btcoexist, 0x61, 1, h2c_parameter);
 }
 
-bool halbtc8723b2ant_is_wifi_status_changed(struct btc_coexist *btcoexist)
+static bool halbtc8723b2ant_is_wifi_status_changed(
+						struct btc_coexist *btcoexist)
 {
 	static bool pre_wifi_busy = false;
 	static bool pre_under_4way = false;
@@ -382,7 +387,7 @@ bool halbtc8723b2ant_is_wifi_status_changed(struct btc_coexist *btcoexist)
 	return false;
 }
 
-void halbtc8723b2ant_update_bt_link_info(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_update_bt_link_info(struct btc_coexist *btcoexist)
 {
 	/*struct btc_stack_info *stack_info = &btcoexist->stack_info;*/
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
@@ -446,7 +451,7 @@ void halbtc8723b2ant_update_bt_link_info(struct btc_coexist *btcoexist)
 		bt_link_info->hid_only = false;
 }
 
-u8 halbtc8723b2ant_action_algorithm(struct btc_coexist *btcoexist)
+static u8 halbtc8723b2ant_action_algorithm(struct btc_coexist *btcoexist)
 {
 	struct btc_bt_link_info *bt_link_info=&btcoexist->bt_link_info;
 	bool bt_hs_on = false;
@@ -634,7 +639,7 @@ u8 halbtc8723b2ant_action_algorithm(struct btc_coexist *btcoexist)
 	return algorithm;
 }
 
-bool halbtc8723b2ant_need_to_dec_bt_pwr(struct btc_coexist *btcoexist)
+static bool halbtc8723b2ant_need_to_dec_bt_pwr(struct btc_coexist *btcoexist)
 {
 	bool bRet = false;
 	bool bt_hs_on = false, wifi_connected = false;
@@ -675,8 +680,9 @@ bool halbtc8723b2ant_need_to_dec_bt_pwr(struct btc_coexist *btcoexist)
 	return bRet;
 }
 
-void halbtc8723b2ant_set_fw_dac_swing_level(struct btc_coexist *btcoexist,
-					    u8 dac_swing_lvl)
+static void halbtc8723b2ant_set_fw_dac_swing_level(
+						struct btc_coexist *btcoexist,
+						u8 dac_swing_lvl)
 {
 	u8 h2c_parameter[1] ={0};
 
@@ -692,7 +698,7 @@ void halbtc8723b2ant_set_fw_dac_swing_level(struct btc_coexist *btcoexist,
 	btcoexist->btc_fill_h2c(btcoexist, 0x64, 1, h2c_parameter);
 }
 
-void halbtc8723b2ant_set_fw_dec_bt_pwr(struct btc_coexist *btcoexist,
+static void halbtc8723b2ant_set_fw_dec_bt_pwr(struct btc_coexist *btcoexist,
 				       bool dec_bt_pwr)
 {
 	u8 h2c_parameter[1] = {0};
@@ -709,8 +715,8 @@ void halbtc8723b2ant_set_fw_dec_bt_pwr(struct btc_coexist *btcoexist,
 	btcoexist->btc_fill_h2c(btcoexist, 0x62, 1, h2c_parameter);
 }
 
-void halbtc8723b2ant_dec_bt_pwr(struct btc_coexist *btcoexist,
-				bool force_exec, bool dec_bt_pwr)
+static void halbtc8723b2ant_dec_bt_pwr(struct btc_coexist *btcoexist,
+					bool force_exec, bool dec_bt_pwr)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
 		  "[BTCoex], %s Dec BT power = %s\n",
@@ -730,7 +736,8 @@ void halbtc8723b2ant_dec_bt_pwr(struct btc_coexist *btcoexist,
 	coex_dm->pre_dec_bt_pwr = coex_dm->cur_dec_bt_pwr;
 }
 
-void halbtc8723b2ant_set_bt_auto_report(struct btc_coexist *btcoexist,
+#if (BT_AUTO_REPORT_ONLY_8723B_2ANT == 0)
+static void halbtc8723b2ant_set_bt_auto_report(struct btc_coexist *btcoexist,
 					bool enable_auto_report)
 {
 	u8 h2c_parameter[1] = {0};
@@ -747,7 +754,7 @@ void halbtc8723b2ant_set_bt_auto_report(struct btc_coexist *btcoexist,
 	btcoexist->btc_fill_h2c(btcoexist, 0x68, 1, h2c_parameter);
 }
 
-void halbtc8723b2ant_bt_auto_report(struct btc_coexist *btcoexist,
+static void halbtc8723b2ant_bt_auto_report(struct btc_coexist *btcoexist,
 				    bool force_exec, bool enable_auto_report)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
@@ -771,8 +778,9 @@ void halbtc8723b2ant_bt_auto_report(struct btc_coexist *btcoexist,
 
 	coex_dm->pre_bt_auto_report = coex_dm->cur_bt_auto_report;
 }
+#endif
 
-void halbtc8723b2ant_fw_dac_swing_lvl(struct btc_coexist *btcoexist,
+static void halbtc8723b2ant_fw_dac_swing_lvl(struct btc_coexist *btcoexist,
 				      bool force_exec, u8 fw_dac_swing_lvl)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
@@ -797,8 +805,9 @@ void halbtc8723b2ant_fw_dac_swing_lvl(struct btc_coexist *btcoexist,
 	coex_dm->pre_fw_dac_swing_lvl = coex_dm->cur_fw_dac_swing_lvl;
 }
 
-void halbtc8723b2ant_set_sw_rf_rx_lpf_corner(struct btc_coexist *btcoexist,
-					     bool rx_rf_shrink_on)
+static void halbtc8723b2ant_set_sw_rf_rx_lpf_corner(
+						struct btc_coexist *btcoexist,
+						bool rx_rf_shrink_on)
 {
 	if (rx_rf_shrink_on) {
 		/* Shrink RF Rx LPF corner */
@@ -819,7 +828,7 @@ void halbtc8723b2ant_set_sw_rf_rx_lpf_corner(struct btc_coexist *btcoexist,
 	}
 }
 
-void halbtc8723b2ant_rf_shrink(struct btc_coexist *btcoexist,
+static void halbtc8723b2ant_rf_shrink(struct btc_coexist *btcoexist,
 			       bool force_exec, bool rx_rf_shrink_on)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
@@ -844,7 +853,7 @@ void halbtc8723b2ant_rf_shrink(struct btc_coexist *btcoexist,
 	coex_dm->pre_rf_rx_lpf_shrink = coex_dm->cur_rf_rx_lpf_shrink;
 }
 
-void halbtc8723b2ant_set_sw_penalty_txrate_adaptive(
+static void halbtc8723b2ant_set_sw_penalty_txrate_adaptive(
 						struct btc_coexist *btcoexist,
 						bool low_penalty_ra)
 {
@@ -868,7 +877,7 @@ void halbtc8723b2ant_set_sw_penalty_txrate_adaptive(
 	btcoexist->btc_fill_h2c(btcoexist, 0x69, 6, h2c_parameter);
 }
 
-void halbtc8723b2ant_low_penalty_ra(struct btc_coexist *btcoexist,
+static void halbtc8723b2ant_low_penalty_ra(struct btc_coexist *btcoexist,
 				    bool force_exec, bool low_penalty_ra)
 {
 	/*return; */
@@ -893,7 +902,7 @@ void halbtc8723b2ant_low_penalty_ra(struct btc_coexist *btcoexist,
 	coex_dm->pre_low_penalty_ra = coex_dm->cur_low_penalty_ra;
 }
 
-void halbtc8723b2ant_set_dac_swing_reg(struct btc_coexist * btcoexist,
+static void halbtc8723b2ant_set_dac_swing_reg(struct btc_coexist *btcoexist,
 				       u32 level)
 {
 	u8 val = (u8) level;
@@ -902,9 +911,10 @@ void halbtc8723b2ant_set_dac_swing_reg(struct btc_coexist * btcoexist,
 	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x883, 0x3e, val);
 }
 
-void halbtc8723b2ant_set_sw_fulltime_dac_swing(struct btc_coexist *btcoexist,
-					       bool sw_dac_swing_on,
-					       u32 sw_dac_swing_lvl)
+static void halbtc8723b2ant_set_sw_fulltime_dac_swing(
+						struct btc_coexist *btcoexist,
+						bool sw_dac_swing_on,
+						u32 sw_dac_swing_lvl)
 {
 	if(sw_dac_swing_on)
 		halbtc8723b2ant_set_dac_swing_reg(btcoexist, sw_dac_swing_lvl);
@@ -913,7 +923,7 @@ void halbtc8723b2ant_set_sw_fulltime_dac_swing(struct btc_coexist *btcoexist,
 }
 
 
-void halbtc8723b2ant_dac_swing(struct btc_coexist *btcoexist,
+static void halbtc8723b2ant_dac_swing(struct btc_coexist *btcoexist,
 			       bool force_exec, bool dac_swing_on,
 			       u32 dac_swing_lvl)
 {
@@ -944,7 +954,8 @@ void halbtc8723b2ant_dac_swing(struct btc_coexist *btcoexist,
 	coex_dm->pre_dac_swing_lvl = coex_dm->cur_dac_swing_lvl;
 }
 
-void halbtc8723b2ant_set_adc_backoff(struct btc_coexist *btcoexist,
+#if 0
+static void halbtc8723b2ant_set_adc_backoff(struct btc_coexist *btcoexist,
 				     bool adc_backoff)
 {
 	if (adc_backoff) {
@@ -958,7 +969,7 @@ void halbtc8723b2ant_set_adc_backoff(struct btc_coexist *btcoexist,
 	}
 }
 
-void halbtc8723b2ant_adc_backoff(struct btc_coexist *btcoexist,
+static void halbtc8723b2ant_adc_backoff(struct btc_coexist *btcoexist,
 				 bool force_exec, bool adc_backoff)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
@@ -979,8 +990,9 @@ void halbtc8723b2ant_adc_backoff(struct btc_coexist *btcoexist,
 
 	coex_dm->pre_adc_back_off = coex_dm->cur_adc_back_off;
 }
+#endif
 
-void halbtc8723b2ant_set_agc_table(struct btc_coexist *btcoexist,
+static void halbtc8723b2ant_set_agc_table(struct btc_coexist *btcoexist,
 				   bool agc_table_en)
 {
 	u8 rssi_adjust_val = 0;
@@ -1054,7 +1066,7 @@ void halbtc8723b2ant_set_agc_table(struct btc_coexist *btcoexist,
 			   &rssi_adjust_val);
 }
 
-void halbtc8723b2ant_agc_table(struct btc_coexist *btcoexist,
+static void halbtc8723b2ant_agc_table(struct btc_coexist *btcoexist,
 			       bool force_exec, bool agc_table_en)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
@@ -1076,7 +1088,7 @@ void halbtc8723b2ant_agc_table(struct btc_coexist *btcoexist,
 	coex_dm->pre_agc_table_en = coex_dm->cur_agc_table_en;
 }
 
-void halbtc8723b2ant_set_coex_table(struct btc_coexist *btcoexist,
+static void halbtc8723b2ant_set_coex_table(struct btc_coexist *btcoexist,
 				    u32 val0x6c0, u32 val0x6c4,
 				    u32 val0x6c8, u8 val0x6cc)
 {
@@ -1097,7 +1109,7 @@ void halbtc8723b2ant_set_coex_table(struct btc_coexist *btcoexist,
 	btcoexist->btc_write_1byte(btcoexist, 0x6cc, val0x6cc);
 }
 
-void halbtc8723b2ant_coex_table(struct btc_coexist *btcoexist,
+static void halbtc8723b2ant_coex_table(struct btc_coexist *btcoexist,
 				bool force_exec, u32 val0x6c0,
 				u32 val0x6c4, u32 val0x6c8,
 				u8 val0x6cc)
@@ -1141,7 +1153,7 @@ void halbtc8723b2ant_coex_table(struct btc_coexist *btcoexist,
 	coex_dm->pre_val0x6cc = coex_dm->cur_val0x6cc;
 }
 
-void halbtc8723b2ant_coex_table_with_type(struct btc_coexist *btcoexist,
+static void halbtc8723b2ant_coex_table_with_type(struct btc_coexist *btcoexist,
 					  bool force_exec, u8 type)
 {
 	switch (type) {
@@ -1202,8 +1214,9 @@ void halbtc8723b2ant_coex_table_with_type(struct btc_coexist *btcoexist,
 	}
 }
 
-void halbtc8723b2ant_set_fw_ignore_wlan_act(struct btc_coexist *btcoexist,
-					    bool enable)
+static void halbtc8723b2ant_set_fw_ignore_wlan_act(
+						struct btc_coexist *btcoexist,
+						bool enable)
 {
 	u8 h2c_parameter[1] ={0};
 
@@ -1217,7 +1230,7 @@ void halbtc8723b2ant_set_fw_ignore_wlan_act(struct btc_coexist *btcoexist,
 	btcoexist->btc_fill_h2c(btcoexist, 0x63, 1, h2c_parameter);
 }
 
-void halbtc8723b2ant_ignore_wlan_act(struct btc_coexist *btcoexist,
+static void halbtc8723b2ant_ignore_wlan_act(struct btc_coexist *btcoexist,
 				     bool force_exec, bool enable)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
@@ -1241,8 +1254,8 @@ void halbtc8723b2ant_ignore_wlan_act(struct btc_coexist *btcoexist,
 	coex_dm->pre_ignore_wlan_act = coex_dm->cur_ignore_wlan_act;
 }
 
-void halbtc8723b2ant_set_fw_ps_tdma(struct btc_coexist *btcoexist, u8 byte1,
-				    u8 byte2, u8 byte3, u8 byte4, u8 byte5)
+static void halbtc8723b2ant_set_fw_ps_tdma(struct btc_coexist *btcoexist,
+			u8 byte1, u8 byte2, u8 byte3, u8 byte4, u8 byte5)
 {
 	u8 h2c_parameter[5] ={0};
 
@@ -1267,7 +1280,7 @@ void halbtc8723b2ant_set_fw_ps_tdma(struct btc_coexist *btcoexist, u8 byte1,
 	btcoexist->btc_fill_h2c(btcoexist, 0x60, 5, h2c_parameter);
 }
 
-void halbtc8723b2ant_sw_mechanism1(struct btc_coexist *btcoexist,
+static void halbtc8723b2ant_sw_mechanism1(struct btc_coexist *btcoexist,
 				   bool shrink_rx_lpf, bool low_penalty_ra,
 				   bool limited_dig, bool bt_lna_constrain)
 {
@@ -1287,7 +1300,7 @@ void halbtc8723b2ant_sw_mechanism1(struct btc_coexist *btcoexist,
 	halbtc8723b2ant_low_penalty_ra(btcoexist, NORMAL_EXEC, low_penalty_ra);
 }
 
-void halbtc8723b2ant_sw_mechanism2(struct btc_coexist *btcoexist,
+static void halbtc8723b2ant_sw_mechanism2(struct btc_coexist *btcoexist,
 				   bool agc_table_shift, bool adc_backoff,
 				   bool sw_dac_swing, u32 dac_swing_lvl)
 {
@@ -1297,7 +1310,7 @@ void halbtc8723b2ant_sw_mechanism2(struct btc_coexist *btcoexist,
 				  dac_swing_lvl);
 }
 
-void halbtc8723b2ant_set_ant_path(struct btc_coexist *btcoexist,
+static void halbtc8723b2ant_set_ant_path(struct btc_coexist *btcoexist,
 				  u8 antpos_type, bool init_hwcfg,
 				  bool wifi_off)
 {
@@ -1377,8 +1390,9 @@ void halbtc8723b2ant_set_ant_path(struct btc_coexist *btcoexist,
 }
 
 
-void halbtc8723b2ant_ps_tdma(struct btc_coexist *btcoexist, bool force_exec,
-			     bool turn_on, u8 type)
+static void halbtc8723b2ant_ps_tdma(struct btc_coexist *btcoexist,
+					bool force_exec,
+					bool turn_on, u8 type)
 {
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
@@ -1514,7 +1528,7 @@ void halbtc8723b2ant_ps_tdma(struct btc_coexist *btcoexist, bool force_exec,
 	coex_dm->pre_ps_tdma = coex_dm->cur_ps_tdma;
 }
 
-void halbtc8723b2ant_coex_alloff(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_coex_alloff(struct btc_coexist *btcoexist)
 {
 	/* fw all off */
 	halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
@@ -1530,7 +1544,7 @@ void halbtc8723b2ant_coex_alloff(struct btc_coexist *btcoexist)
 	halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
 }
 
-void halbtc8723b2ant_init_coex_dm(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_init_coex_dm(struct btc_coexist *btcoexist)
 {
 	/* force to reset coex mechanism*/
 
@@ -1542,7 +1556,7 @@ void halbtc8723b2ant_init_coex_dm(struct btc_coexist *btcoexist)
 	halbtc8723b2ant_sw_mechanism2(btcoexist, false, false, false, 0x18);
 }
 
-void halbtc8723b2ant_action_bt_inquiry(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_action_bt_inquiry(struct btc_coexist *btcoexist)
 {
 	bool wifi_connected = false;
 	bool low_pwr_disable = true;
@@ -1572,7 +1586,7 @@ void halbtc8723b2ant_action_bt_inquiry(struct btc_coexist *btcoexist)
 				     false, false);
 }
 
-bool halbtc8723b2ant_is_common_action(struct btc_coexist *btcoexist)
+static bool halbtc8723b2ant_is_common_action(struct btc_coexist *btcoexist)
 {
 	bool bCommon = false, wifi_connected = false;
 	bool wifi_busy = false;
@@ -1713,7 +1727,8 @@ bool halbtc8723b2ant_is_common_action(struct btc_coexist *btcoexist)
 
 	return bCommon;
 }
-void halbtc8723b2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
+
+static void halbtc8723b2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
 					  bool sco_hid, bool tx_pause,
 					  u8 max_interval)
 {
@@ -2772,7 +2787,7 @@ void halbtc8723b2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
 }
 
 /* SCO only or SCO+PAN(HS) */
-void halbtc8723b2ant_action_sco(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_action_sco(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state;
 	u32 wifi_bw;
@@ -2830,8 +2845,7 @@ void halbtc8723b2ant_action_sco(struct btc_coexist *btcoexist)
 	}
 }
 
-
-void halbtc8723b2ant_action_hid(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_action_hid(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, bt_rssi_state;
 	u32 wifi_bw;
@@ -2893,7 +2907,7 @@ void halbtc8723b2ant_action_hid(struct btc_coexist *btcoexist)
 }
 
 /*A2DP only / PAN(EDR) only/ A2DP+PAN(HS)*/
-void halbtc8723b2ant_action_a2dp(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_action_a2dp(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, wifi_rssi_state1, bt_rssi_state;
 	u32 wifi_bw;
@@ -2981,7 +2995,7 @@ void halbtc8723b2ant_action_a2dp(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b2ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state;
 	u32 wifi_bw;
@@ -3034,7 +3048,7 @@ void halbtc8723b2ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b2ant_action_pan_edr(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_action_pan_edr(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, bt_rssi_state;
 	u32 wifi_bw;
@@ -3091,9 +3105,8 @@ void halbtc8723b2ant_action_pan_edr(struct btc_coexist *btcoexist)
 	}
 }
 
-
 /*PAN(HS) only*/
-void halbtc8723b2ant_action_pan_hs(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_action_pan_hs(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state;
 	u32 wifi_bw;
@@ -3146,7 +3159,7 @@ void halbtc8723b2ant_action_pan_hs(struct btc_coexist *btcoexist)
 }
 
 /*PAN(EDR)+A2DP*/
-void halbtc8723b2ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, bt_rssi_state;
 	u32 wifi_bw;
@@ -3210,7 +3223,7 @@ void halbtc8723b2ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b2ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, bt_rssi_state;
 	u32 wifi_bw;
@@ -3282,7 +3295,8 @@ void halbtc8723b2ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
 }
 
 /* HID+A2DP+PAN(EDR) */
-void halbtc8723b2ant_action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_action_hid_a2dp_pan_edr(
+						struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, bt_rssi_state;
 	u32 wifi_bw;
@@ -3346,7 +3360,7 @@ void halbtc8723b2ant_action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
 {
 	u8 wifi_rssi_state, bt_rssi_state;
 	u32 wifi_bw;
@@ -3404,7 +3418,8 @@ void halbtc8723b2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_run_coexist_mechanism(
+						struct btc_coexist *btcoexist)
 {
 	u8 algorithm = 0;
 
@@ -3525,7 +3540,7 @@ void halbtc8723b2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 	}
 }
 
-void halbtc8723b2ant_wifioff_hwcfg(struct btc_coexist *btcoexist)
+static void halbtc8723b2ant_wifioff_hwcfg(struct btc_coexist *btcoexist)
 {
 	/* set wlan_act to low */
 	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0x4);
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtcoutsrc.c b/drivers/staging/rtl8821ae/btcoexist/halbtcoutsrc.c
index c4e83773ec9..01f5a74028a 100644
--- a/drivers/staging/rtl8821ae/btcoexist/halbtcoutsrc.c
+++ b/drivers/staging/rtl8821ae/btcoexist/halbtcoutsrc.c
@@ -25,7 +25,7 @@
 /***********************************************
  *		Global variables
  ***********************************************/
-const char *const bt_profile_string[]={
+static const char *const bt_profile_string[] = {
 	"NONE",
 	"A2DP",
 	"PAN",
@@ -33,7 +33,7 @@ const char *const bt_profile_string[]={
 	"SCO",
 };
 
-const char *const bt_spec_string[]={
+static const char *const bt_spec_string[] = {
 	"1.0b",
 	"1.1",
 	"1.2",
@@ -43,19 +43,19 @@ const char *const bt_spec_string[]={
 	"4.0",
 };
 
-const char *const bt_link_role_string[]={
+static const char *const bt_link_role_string[] = {
 	"Master",
 	"Slave",
 };
 
-const char *const h2c_state_string[]={
+static const char *const h2c_state_string[] = {
 	"successful",
 	"h2c busy",
 	"rf off",
 	"fw not read",
 };
 
-const char *const io_state_string[]={
+static const char *const io_state_string[] = {
 	"IO_STATUS_SUCCESS",
 	"IO_STATUS_FAIL_CANNOT_IO",
 	"IO_STATUS_FAIL_RF_OFF",
@@ -72,12 +72,12 @@ const char *const io_state_string[]={
 struct btc_coexist gl_bt_coexist;
 
 u32 btc_dbg_type[BTC_MSG_MAX];
-u8 btc_dbg_buf[100];
+static u8 btc_dbg_buf[100];
 
 /***************************************************
  *		Debug related function
  ***************************************************/
-bool halbtc_is_bt_coexist_available(struct btc_coexist *btcoexist)
+static bool halbtc_is_bt_coexist_available(struct btc_coexist *btcoexist)
 {
 	if (!btcoexist->binded || NULL == btcoexist->adapter)
 		return false;
@@ -85,7 +85,7 @@ bool halbtc_is_bt_coexist_available(struct btc_coexist *btcoexist)
 	return true;
 }
 
-bool halbtc_is_wifi_busy(struct rtl_priv *rtlpriv)
+static bool halbtc_is_wifi_busy(struct rtl_priv *rtlpriv)
 {
 
 	if (rtlpriv->link_info.b_busytraffic)
@@ -95,7 +95,7 @@ bool halbtc_is_wifi_busy(struct rtl_priv *rtlpriv)
 }
 
 
-void halbtc_dbg_init(void)
+static void halbtc_dbg_init(void)
 {
 	u8 i;
 
@@ -121,12 +121,7 @@ void halbtc_dbg_init(void)
 			0;
 }
 
-bool halbtc_is_hw_mailbox_exist(struct btc_coexist *btcoexist)
-{
-	return true;
-}
-
-bool halbtc_is_bt40(struct rtl_priv *adapter)
+static bool halbtc_is_bt40(struct rtl_priv *adapter)
 {
 	struct rtl_priv *rtlpriv = adapter;
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
@@ -142,7 +137,7 @@ bool halbtc_is_bt40(struct rtl_priv *adapter)
 	return is_ht40;
 }
 
-bool halbtc_legacy(struct rtl_priv *adapter)
+static bool halbtc_legacy(struct rtl_priv *adapter)
 {
 	struct rtl_priv *rtlpriv = adapter;
 	struct rtl_mac *mac = rtl_mac(rtlpriv);
@@ -165,7 +160,7 @@ bool halbtc_is_wifi_uplink(struct rtl_priv *adapter)
 		return false;
 }
 
-u32 halbtc_get_wifi_bw(struct btc_coexist *btcoexist)
+static u32 halbtc_get_wifi_bw(struct btc_coexist *btcoexist)
 {
 	struct rtl_priv *rtlpriv = 
 		(struct rtl_priv *)btcoexist->adapter;
@@ -182,7 +177,7 @@ u32 halbtc_get_wifi_bw(struct btc_coexist *btcoexist)
 	return wifi_bw;
 }
 
-u8 halbtc_get_wifi_central_chnl(struct btc_coexist *btcoexist)
+static u8 halbtc_get_wifi_central_chnl(struct btc_coexist *btcoexist)
 {
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
 	struct rtl_phy 	*rtlphy = &(rtlpriv->phy);
@@ -196,7 +191,7 @@ u8 halbtc_get_wifi_central_chnl(struct btc_coexist *btcoexist)
 	return chnl;
 }
 
-void halbtc_leave_lps(struct btc_coexist *btcoexist)
+static void halbtc_leave_lps(struct btc_coexist *btcoexist)
 {
 	struct rtl_priv *rtlpriv;
 	struct rtl_ps_ctl *ppsc;
@@ -217,7 +212,7 @@ void halbtc_leave_lps(struct btc_coexist *btcoexist)
 	btcoexist->bt_info.bt_lps_on = false;
 }
 
-void halbtc_enter_lps(struct btc_coexist *btcoexist)
+static void halbtc_enter_lps(struct btc_coexist *btcoexist)
 {
 	struct rtl_priv *rtlpriv;
 	struct rtl_ps_ctl *ppsc;
@@ -238,7 +233,7 @@ void halbtc_enter_lps(struct btc_coexist *btcoexist)
 	btcoexist->bt_info.bt_lps_on = false;
 }
 
-void halbtc_normal_lps(struct btc_coexist *btcoexist)
+static void halbtc_normal_lps(struct btc_coexist *btcoexist)
 {
 	if (btcoexist->bt_info.bt_ctrl_lps) {
 		btcoexist->bt_info.bt_lps_on = false;
@@ -247,29 +242,29 @@ void halbtc_normal_lps(struct btc_coexist *btcoexist)
 		
 }
 
-void halbtc_leave_low_power(void)
+static void halbtc_leave_low_power(void)
 {
 }
 
-void halbtc_nomal_low_power(void)
+static void halbtc_nomal_low_power(void)
 {
 }
 
-void halbtc_disable_low_power(void)
+static void halbtc_disable_low_power(void)
 {
 }
 
-void halbtc_aggregation_check(void)
+static void halbtc_aggregation_check(void)
 {
 }
 
 
-u32 halbtc_get_bt_patch_version(struct btc_coexist *btcoexist)
+static u32 halbtc_get_bt_patch_version(struct btc_coexist *btcoexist)
 {
 	return 0;
 }
 
-s32 halbtc_get_wifi_rssi(struct rtl_priv *adapter)
+static s32 halbtc_get_wifi_rssi(struct rtl_priv *adapter)
 {
 	struct rtl_priv *rtlpriv = adapter;
 	s32	undecorated_smoothed_pwdb = 0;
@@ -283,7 +278,7 @@ s32 halbtc_get_wifi_rssi(struct rtl_priv *adapter)
 	return undecorated_smoothed_pwdb;
 }
 
-bool halbtc_get(void *void_btcoexist, u8 get_type, void *out_buf)
+static bool halbtc_get(void *void_btcoexist, u8 get_type, void *out_buf)
 {
 	struct btc_coexist *btcoexist = (struct btc_coexist *)void_btcoexist;
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
@@ -422,7 +417,7 @@ bool halbtc_get(void *void_btcoexist, u8 get_type, void *out_buf)
 	return true;
 }
 
-bool halbtc_set(void *void_btcoexist, u8 set_type, void *in_buf)
+static bool halbtc_set(void *void_btcoexist, u8 set_type, void *in_buf)
 {
 	struct btc_coexist *btcoexist = (struct btc_coexist *)void_btcoexist;
 	bool *bool_tmp = (bool *)in_buf;
@@ -516,26 +511,26 @@ bool halbtc_set(void *void_btcoexist, u8 set_type, void *in_buf)
 	return true;
 }
 
-void halbtc_display_coex_statistics(struct btc_coexist *btcoexist)
+static void halbtc_display_coex_statistics(struct btc_coexist *btcoexist)
 {
 }
 
-void halbtc_display_bt_link_info(struct btc_coexist *btcoexist)
+static void halbtc_display_bt_link_info(struct btc_coexist *btcoexist)
 {
 }
 
-void halbtc_display_bt_fw_info(struct btc_coexist *btcoexist)
+static void halbtc_display_bt_fw_info(struct btc_coexist *btcoexist)
 {
 }
 
-void halbtc_display_fw_pwr_mode_cmd(struct btc_coexist *btcoexist)
+static void halbtc_display_fw_pwr_mode_cmd(struct btc_coexist *btcoexist)
 {
 }
 
 /************************************************************
  *		IO related function
  ************************************************************/
-u8 halbtc_read_1byte(void *bt_context, u32 reg_addr)
+static u8 halbtc_read_1byte(void *bt_context, u32 reg_addr)
 {
 	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
@@ -544,7 +539,7 @@ u8 halbtc_read_1byte(void *bt_context, u32 reg_addr)
 }
 
 
-u16 halbtc_read_2byte(void *bt_context, u32 reg_addr)
+static u16 halbtc_read_2byte(void *bt_context, u32 reg_addr)
 {
 	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
@@ -553,7 +548,7 @@ u16 halbtc_read_2byte(void *bt_context, u32 reg_addr)
 }
 
 
-u32 halbtc_read_4byte(void *bt_context, u32 reg_addr)
+static u32 halbtc_read_4byte(void *bt_context, u32 reg_addr)
 {
 	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
@@ -562,7 +557,7 @@ u32 halbtc_read_4byte(void *bt_context, u32 reg_addr)
 }
 
 
-void halbtc_write_1byte(void *bt_context, u32 reg_addr, u8 data)
+static void halbtc_write_1byte(void *bt_context, u32 reg_addr, u8 data)
 {
 	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
@@ -570,7 +565,7 @@ void halbtc_write_1byte(void *bt_context, u32 reg_addr, u8 data)
 	rtl_write_byte(rtlpriv, reg_addr, data);
 }
 
-void halbtc_bitmask_write_1byte(void *bt_context, u32 reg_addr, 
+static void halbtc_bitmask_write_1byte(void *bt_context, u32 reg_addr,
 				u8 bit_mask, u8 data)
 {
 	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
@@ -592,7 +587,7 @@ void halbtc_bitmask_write_1byte(void *bt_context, u32 reg_addr,
 }
 
 
-void halbtc_write_2byte(void *bt_context, u32 reg_addr, u16 data)
+static void halbtc_write_2byte(void *bt_context, u32 reg_addr, u16 data)
 {
 	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
@@ -601,7 +596,7 @@ void halbtc_write_2byte(void *bt_context, u32 reg_addr, u16 data)
 }
 
 
-void halbtc_write_4byte(void *bt_context, u32 reg_addr, u32 data)
+static void halbtc_write_4byte(void *bt_context, u32 reg_addr, u32 data)
 {
 	struct btc_coexist *btcoexist = 
 		(struct btc_coexist *)bt_context;
@@ -610,26 +605,8 @@ void halbtc_write_4byte(void *bt_context, u32 reg_addr, u32 data)
 	rtl_write_dword(rtlpriv, reg_addr, data);
 }
 
-
-void halbtc_set_macreg(void *bt_context, u32 reg_addr, u32 bit_mask, u32 data)
-{
-	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-
-	rtl_set_bbreg(rtlpriv->mac80211.hw, reg_addr, bit_mask, data);
-}
-
-
-u32 halbtc_get_macreg(void *bt_context, u32 reg_addr, u32 bit_mask)
-{
-	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-
-	return rtl_get_bbreg(rtlpriv->mac80211.hw, reg_addr, bit_mask);
-}
-
-
-void halbtc_set_bbreg(void *bt_context, u32 reg_addr, u32 bit_mask, u32 data)
+static void halbtc_set_bbreg(void *bt_context, u32 reg_addr,
+				u32 bit_mask, u32 data)
 {
 	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
@@ -638,7 +615,7 @@ void halbtc_set_bbreg(void *bt_context, u32 reg_addr, u32 bit_mask, u32 data)
 }
 
 
-u32 halbtc_get_bbreg(void *bt_context, u32 reg_addr, u32 bit_mask)
+static u32 halbtc_get_bbreg(void *bt_context, u32 reg_addr, u32 bit_mask)
 {
 	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
@@ -647,7 +624,7 @@ u32 halbtc_get_bbreg(void *bt_context, u32 reg_addr, u32 bit_mask)
 }
 
 
-void halbtc_set_rfreg(void *bt_context, u8 rf_path, u32 reg_addr, 
+static void halbtc_set_rfreg(void *bt_context, u8 rf_path, u32 reg_addr,
 		      u32 bit_mask, u32 data)
 {
 	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
@@ -657,7 +634,8 @@ void halbtc_set_rfreg(void *bt_context, u8 rf_path, u32 reg_addr,
 }
 
 
-u32 halbtc_get_rfreg(void *bt_context, u8 rf_path, u32 reg_addr, u32 bit_mask)
+static u32 halbtc_get_rfreg(void *bt_context, u8 rf_path,
+			u32 reg_addr, u32 bit_mask)
 {
 	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
@@ -666,7 +644,7 @@ u32 halbtc_get_rfreg(void *bt_context, u8 rf_path, u32 reg_addr, u32 bit_mask)
 }
 
 
-void halbtc_fill_h2c_cmd(void *bt_context, u8 element_id, 
+static void halbtc_fill_h2c_cmd(void *bt_context, u8 element_id,
 			 u32 cmd_len, u8 *cmd_buf)
 {
 	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
@@ -676,7 +654,7 @@ void halbtc_fill_h2c_cmd(void *bt_context, u8 element_id,
 					cmd_len, cmd_buf);
 }
 
-void halbtc_display_dbg_msg(void *bt_context, u8 disp_type)
+static void halbtc_display_dbg_msg(void *bt_context, u8 disp_type)
 {
 	struct btc_coexist *btcoexist =	(struct btc_coexist *)bt_context;
 	switch (disp_type) {
@@ -697,25 +675,6 @@ void halbtc_display_dbg_msg(void *bt_context, u8 disp_type)
 	}
 }
 
-bool halbtc_under_ips(struct btc_coexist *btcoexist)
-{
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtlpriv);
-	enum rf_pwrstate rtstate;
-	
-	if (ppsc->b_inactiveps) {
-		rtstate = ppsc->rfpwr_state;
-
-		if (rtstate != ERFON &&
-		    ppsc->rfoff_reason == RF_CHANGE_BY_IPS) {
-
-			return true;
-		}
-	}
-
-	return false;
-}
-
 /*****************************************************************
  *         Extern functions called by other module
  *****************************************************************/
diff --git a/drivers/staging/rtl8821ae/btcoexist/rtl_btc.c b/drivers/staging/rtl8821ae/btcoexist/rtl_btc.c
index 6653f147757..6be0acafba5 100644
--- a/drivers/staging/rtl8821ae/btcoexist/rtl_btc.c
+++ b/drivers/staging/rtl8821ae/btcoexist/rtl_btc.c
@@ -32,7 +32,7 @@
 #include "rtl_btc.h"
 #include "halbt_precomp.h"
 
-struct rtl_btc_ops rtl_btc_operation ={
+static struct rtl_btc_ops rtl_btc_operation = {
 	.btc_init_variables = rtl_btc_init_variables,
 	.btc_init_hal_vars = rtl_btc_init_hal_vars,
 	.btc_init_hw_config = rtl_btc_init_hw_config,
diff --git a/drivers/staging/rtl8821ae/cam.c b/drivers/staging/rtl8821ae/cam.c
index 72743e78954..3bc6b3d0651 100644
--- a/drivers/staging/rtl8821ae/cam.c
+++ b/drivers/staging/rtl8821ae/cam.c
@@ -28,9 +28,7 @@
  *****************************************************************************/
 #include "wifi.h"
 #include "cam.h"
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,2,0))
 #include <linux/export.h>
-#endif
 
 void rtl_cam_reset_sec_info(struct ieee80211_hw *hw)
 {
diff --git a/drivers/staging/rtl8821ae/compat.h b/drivers/staging/rtl8821ae/compat.h
index 68269cc2d47..ffb5f8b07bd 100644
--- a/drivers/staging/rtl8821ae/compat.h
+++ b/drivers/staging/rtl8821ae/compat.h
@@ -1,65 +1,8 @@
 #ifndef __RTL_COMPAT_H__
 #define __RTL_COMPAT_H__
 
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,29))
-/*
- * Use this if you want to use the same suspend and resume callbacks for suspend
- * to RAM and hibernation.
- */
-#define SIMPLE_DEV_PM_OPS(name, suspend_fn, resume_fn) \
-struct dev_pm_ops name = { \
-	.suspend = suspend_fn, \
-	.resume = resume_fn, \
-	.freeze = suspend_fn, \
-	.thaw = resume_fn, \
-	.poweroff = suspend_fn, \
-	.restore = resume_fn, \
-}
-
-#define compat_pci_suspend(fn)						\
-	int fn##_compat(struct pci_dev *pdev, pm_message_t state) 	\
-	{								\
-		int r;							\
-									\
-		r = fn(&pdev->dev);					\
-		if (r)							\
-			return r;					\
-									\
-		pci_save_state(pdev);					\
-		pci_disable_device(pdev);				\
-		pci_set_power_state(pdev, PCI_D3hot);			\
-									\
-		return 0;						\
-	}
-
-#define compat_pci_resume(fn)						\
-	int fn##_compat(struct pci_dev *pdev)				\
-	{								\
-		int r;							\
-									\
-		pci_set_power_state(pdev, PCI_D0);			\
-		r = pci_enable_device(pdev);				\
-		if (r)							\
-			return r;					\
-		pci_restore_state(pdev);				\
-									\
-		return fn(&pdev->dev);					\
-	}
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,39))
-#define RX_FLAG_MACTIME_MPDU RX_FLAG_TSFT
-#else
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,8,0))
 #define RX_FLAG_MACTIME_MPDU RX_FLAG_MACTIME_START
-#else
-#endif
-//#define NETDEV_TX_OK
-#endif
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,7,0))
 #define IEEE80211_KEY_FLAG_SW_MGMT IEEE80211_KEY_FLAG_SW_MGMT_TX
-#endif
 
 struct ieee80211_mgmt_compat {
 	__le16 frame_control;
diff --git a/drivers/staging/rtl8821ae/core.c b/drivers/staging/rtl8821ae/core.c
index ff3139b6da6..9a37408708f 100644
--- a/drivers/staging/rtl8821ae/core.c
+++ b/drivers/staging/rtl8821ae/core.c
@@ -88,42 +88,9 @@ static void rtl_op_stop(struct ieee80211_hw *hw)
 	mutex_unlock(&rtlpriv->locks.conf_mutex);
 }
 
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,39))
-static int rtl_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_tcb_desc tcb_desc;
-	memset(&tcb_desc, 0, sizeof(struct rtl_tcb_desc));
-
-	if (unlikely(is_hal_stop(rtlhal) || ppsc->rfpwr_state != ERFON))
-		goto err_free;
-
-	if (!test_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status))
-		goto err_free;
-
-	if (!rtlpriv->intf_ops->waitq_insert(hw, skb))
-		rtlpriv->intf_ops->adapter_tx(hw, skb, &tcb_desc);
-
-	return NETDEV_TX_OK;
-
-err_free:
-	dev_kfree_skb_any(skb);
-	return NETDEV_TX_OK;
-}
-#else
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0))
-static void rtl_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
-#else
-/*<delete in kernel end>*/
 static void rtl_op_tx(struct ieee80211_hw *hw,
 		      struct ieee80211_tx_control *control,
 		      struct sk_buff *skb)
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
@@ -137,26 +104,14 @@ static void rtl_op_tx(struct ieee80211_hw *hw,
 	if (!test_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status))
 		goto err_free;
 
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0))
-	if (!rtlpriv->intf_ops->waitq_insert(hw, skb))
-		rtlpriv->intf_ops->adapter_tx(hw, skb, &tcb_desc);
-#else
-/*<delete in kernel end>*/
 	if (!rtlpriv->intf_ops->waitq_insert(hw, control->sta, skb))
 	        rtlpriv->intf_ops->adapter_tx(hw, control->sta, skb, &tcb_desc);
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
 	return;
 
 err_free:
 	dev_kfree_skb_any(skb);
 	return;
 }
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
 
 static int rtl_op_add_interface(struct ieee80211_hw *hw,
 		struct ieee80211_vif *vif)
@@ -171,26 +126,15 @@ static int rtl_op_add_interface(struct ieee80211_hw *hw,
 		return -EOPNOTSUPP;
 	}
 
-/*This flag is not defined before kernel 3.4*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,4,0))
 	vif->driver_flags |= IEEE80211_VIF_BEACON_FILTER;
-#endif
 
 	rtl_ips_nic_on(hw);
 
 	mutex_lock(&rtlpriv->locks.conf_mutex);
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,37))
 	switch (ieee80211_vif_type_p2p(vif)) {
 	case NL80211_IFTYPE_P2P_CLIENT:
 		mac->p2p = P2P_ROLE_CLIENT;
 		/*fall through*/
-#else
-/*<delete in kernel end>*/
-	switch (vif->type) {
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
 	case NL80211_IFTYPE_STATION:
 		if (mac->beacon_enabled == 1) {
 			RT_TRACE(COMP_MAC80211, DBG_LOUD,
@@ -214,13 +158,9 @@ static int rtl_op_add_interface(struct ieee80211_hw *hw,
 				(u8 *) (&mac->basic_rates));
 
 		break;
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,37))
 	case NL80211_IFTYPE_P2P_GO:
 		mac->p2p = P2P_ROLE_GO;
 		/*fall through*/
-#endif
-/*<delete in kernel end>*/
 	case NL80211_IFTYPE_AP:
 		RT_TRACE(COMP_MAC80211, DBG_LOUD,
 			 ("NL80211_IFTYPE_AP \n"));
@@ -310,9 +250,7 @@ static void rtl_op_remove_interface(struct ieee80211_hw *hw,
 
 	mutex_unlock(&rtlpriv->locks.conf_mutex);
 }
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,37))
-/*<delete in kernel end>*/
+
 static int rtl_op_change_interface(struct ieee80211_hw *hw,
 				   struct ieee80211_vif *vif,
 				   enum nl80211_iftype new_type, bool p2p)
@@ -328,9 +266,7 @@ static int rtl_op_change_interface(struct ieee80211_hw *hw,
 		 (" p2p  %x\n",p2p));
 	return ret;
 }
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
+
 static int rtl_op_config(struct ieee80211_hw *hw, u32 changed)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -398,14 +334,9 @@ static int rtl_op_config(struct ieee80211_hw *hw, u32 changed)
 	}
 
 	if (changed & IEEE80211_CONF_CHANGE_CHANNEL) {
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,10,0))
 		struct ieee80211_channel *channel = hw->conf.chandef.chan;
 		enum nl80211_channel_type channel_type =
 				cfg80211_get_chandef_type(&(hw->conf.chandef));
-#else
-		struct ieee80211_channel *channel = hw->conf.channel;
-		enum nl80211_channel_type channel_type = hw->conf.channel_type;
-#endif
 		u8 wide_chan = (u8) channel->hw_value;
 
 		if (mac->act_scanning)
@@ -661,14 +592,9 @@ static int _rtl_get_hal_qnum(u16 queue)
  *for mac80211 VO=0, VI=1, BE=2, BK=3
  *for rtl819x  BE=0, BK=1, VI=2, VO=3
  */
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,2,0))
 static int rtl_op_conf_tx(struct ieee80211_hw *hw,
 			  struct ieee80211_vif *vif, u16 queue,
 			  const struct ieee80211_tx_queue_params *param)
-#else
-static int rtl_op_conf_tx(struct ieee80211_hw *hw, u16 queue,
-			  const struct ieee80211_tx_queue_params *param)
-#endif
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
@@ -964,11 +890,7 @@ out:
 	mutex_unlock(&rtlpriv->locks.conf_mutex);
 }
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,2,0))
 static u64 rtl_op_get_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
-#else
-static u64 rtl_op_get_tsf(struct ieee80211_hw *hw)
-#endif
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u64 tsf;
@@ -977,12 +899,8 @@ static u64 rtl_op_get_tsf(struct ieee80211_hw *hw)
 	return tsf;
 }
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,2,0))
 static void rtl_op_set_tsf(struct ieee80211_hw *hw,
 			   struct ieee80211_vif *vif, u64 tsf)
-#else
-static void rtl_op_set_tsf(struct ieee80211_hw *hw, u64 tsf)
-#endif
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
@@ -992,11 +910,7 @@ static void rtl_op_set_tsf(struct ieee80211_hw *hw, u64 tsf)
 	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_CORRECT_TSF, (u8 *) (&bibss));
 }
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,2,0))
 static void rtl_op_reset_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
-#else
-static void rtl_op_reset_tsf(struct ieee80211_hw *hw)
-#endif
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 tmp = 0;
@@ -1023,13 +937,7 @@ static int rtl_op_ampdu_action(struct ieee80211_hw *hw,
 			       struct ieee80211_vif *vif,
 			       enum ieee80211_ampdu_mlme_action action,
 			       struct ieee80211_sta *sta, u16 tid, u16 * ssn
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,39))
-/*<delete in kernel end>*/
 			       ,u8 buf_size
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
 			       )
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -1040,13 +948,9 @@ static int rtl_op_ampdu_action(struct ieee80211_hw *hw,
 			 ("IEEE80211_AMPDU_TX_START: TID:%d\n", tid));
 		return rtl_tx_agg_start(hw, vif, sta, tid, ssn);
 		break;
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,9,0))
 	case IEEE80211_AMPDU_TX_STOP_CONT:
 	case IEEE80211_AMPDU_TX_STOP_FLUSH:
 	case IEEE80211_AMPDU_TX_STOP_FLUSH_CONT:
-#else
-	case IEEE80211_AMPDU_TX_STOP:
-#endif
 		RT_TRACE(COMP_MAC80211, DBG_TRACE,
 			 ("IEEE80211_AMPDU_TX_STOP: TID:%d\n", tid));
 		return rtl_tx_agg_stop(hw, vif, sta, tid);
@@ -1174,9 +1078,6 @@ static int rtl_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 	mutex_lock(&rtlpriv->locks.conf_mutex);
 	/* <1> get encryption alg */
 
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,37))
-/*<delete in kernel end>*/
 	switch (key->cipher) {
 	case WLAN_CIPHER_SUITE_WEP40:
 		key_type = WEP40_ENCRYPTION;
@@ -1209,43 +1110,6 @@ static int rtl_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 			 ("alg_err:%x!!!!:\n", key->cipher));
 		goto out_unlock;
 	}
-/*<delete in kernel start>*/
-#else
-	switch (key->alg) {
-	case ALG_WEP:
-		if (key->keylen == WLAN_KEY_LEN_WEP40) {
-			key_type = WEP40_ENCRYPTION;
-			RT_TRACE(COMP_SEC, DBG_DMESG, ("alg:WEP40\n"));
-		} else {
-			RT_TRACE(COMP_SEC, DBG_DMESG,
-				 ("alg:WEP104\n"));
-			key_type = WEP104_ENCRYPTION;
-		}
-		break;
-	case ALG_TKIP:
-		key_type = TKIP_ENCRYPTION;
-		RT_TRACE(COMP_SEC, DBG_DMESG, ("alg:TKIP\n"));
-		break;
-	case ALG_CCMP:
-		key_type = AESCCMP_ENCRYPTION;
-		RT_TRACE(COMP_SEC, DBG_DMESG, ("alg:CCMP\n"));
-		break;
-	case ALG_AES_CMAC:
-		/*HW don't support CMAC encryption, use software CMAC encryption */
-		key_type = AESCMAC_ENCRYPTION;
-		RT_TRACE(COMP_SEC, DBG_DMESG, ("alg:CMAC\n"));
-		RT_TRACE(COMP_SEC, DBG_DMESG,
-			 ("HW don't support CMAC encryption, "
-			  "use software CMAC encryption\n"));
-		err = -EOPNOTSUPP;
-		goto out_unlock;
-	default:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("alg_err:%x!!!!:\n", key->alg));
-		goto out_unlock;
-	}
-#endif
-/*<delete in kernel end>*/
 	if(key_type == WEP40_ENCRYPTION ||
 			key_type == WEP104_ENCRYPTION ||
 			vif->type == NL80211_IFTYPE_ADHOC)
@@ -1414,7 +1278,6 @@ static void rtl_op_rfkill_poll(struct ieee80211_hw *hw)
  * before switch channel or power save, or tx buffer packet
  * maybe send after offchannel or rf sleep, this may cause
  * dis-association by AP */
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,10,0))
 static void rtl_op_flush(struct ieee80211_hw *hw, u32 queues, bool drop)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -1422,15 +1285,6 @@ static void rtl_op_flush(struct ieee80211_hw *hw, u32 queues, bool drop)
 	if (rtlpriv->intf_ops->flush)
 		rtlpriv->intf_ops->flush(hw, queues, drop);
 }
-#else
-static void rtl_op_flush(struct ieee80211_hw *hw, bool drop)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	if (rtlpriv->intf_ops->flush)
-		rtlpriv->intf_ops->flush(hw, drop);
-}
-#endif
 
 const struct ieee80211_ops rtl_ops = {
 	.start = rtl_op_start,
@@ -1438,13 +1292,7 @@ const struct ieee80211_ops rtl_ops = {
 	.tx = rtl_op_tx,
 	.add_interface = rtl_op_add_interface,
 	.remove_interface = rtl_op_remove_interface,
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,37))
-/*<delete in kernel end>*/
 	.change_interface = rtl_op_change_interface,
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
 	.config = rtl_op_config,
 	.configure_filter = rtl_op_configure_filter,
 	.set_key = rtl_op_set_key,
diff --git a/drivers/staging/rtl8821ae/debug.c b/drivers/staging/rtl8821ae/debug.c
index 8a6c794bda4..8aefbf10a45 100644
--- a/drivers/staging/rtl8821ae/debug.c
+++ b/drivers/staging/rtl8821ae/debug.c
@@ -30,12 +30,7 @@
 #include "wifi.h"
 #include "cam.h"
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,10,0))
 #define GET_INODE_DATA(__node)		PDE_DATA(__node)
-#else
-#define GET_INODE_DATA(__node)		PDE(__node)->data
-#endif
-
 
 void rtl_dbgp_flag_init(struct ieee80211_hw *hw)
 {
diff --git a/drivers/staging/rtl8821ae/efuse.c b/drivers/staging/rtl8821ae/efuse.c
index 250aae1ce63..206012cf360 100644
--- a/drivers/staging/rtl8821ae/efuse.c
+++ b/drivers/staging/rtl8821ae/efuse.c
@@ -29,9 +29,7 @@
 #include "wifi.h"
 #include "efuse.h"
 #include "btcoexist/halbt_precomp.h"
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,2,0))
 #include <linux/export.h>
-#endif
 
 static const u8 MAX_PGPKT_SIZE = 9;
 static const u8 PGPKT_DATA_SIZE = 8;
diff --git a/drivers/staging/rtl8821ae/pci.c b/drivers/staging/rtl8821ae/pci.c
index a562aa60d59..e194ffe58ac 100644
--- a/drivers/staging/rtl8821ae/pci.c
+++ b/drivers/staging/rtl8821ae/pci.c
@@ -33,9 +33,7 @@
 #include "base.h"
 #include "ps.h"
 #include "efuse.h"
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,2,0))
 #include <linux/export.h>
-#endif
 
 static const u16 pcibridge_vendors[PCI_BRIDGE_VENDOR_MAX] = {
 	INTEL_VENDOR_ID,
@@ -364,47 +362,6 @@ static bool rtl_pci_get_amd_l1_patch(struct ieee80211_hw *hw)
 	return status;
 }
 
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,35))
-static u8 _rtl_pci_get_pciehdr_offset(struct ieee80211_hw *hw)
-{
-	u8 capability_offset;
-	u8 num4bytes = 0x34/4;
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	u32 pcicfg_addr_port = (pcipriv->ndis_adapter.pcibridge_busnum << 16)|
-			       (pcipriv->ndis_adapter.pcibridge_devnum << 11)|
-			       (pcipriv->ndis_adapter.pcibridge_funcnum << 8)|
-			       (1 << 31);
-
-	rtl_pci_raw_write_port_ulong(PCI_CONF_ADDRESS , pcicfg_addr_port
-							+ (num4bytes << 2));
-	rtl_pci_raw_read_port_uchar(PCI_CONF_DATA, &capability_offset);
-	while (capability_offset != 0) {
-		struct rtl_pci_capabilities_header capability_hdr;
-
-		num4bytes = capability_offset / 4;
-		/* Read the header of the capability at  this offset.
-		 * If the retrieved capability is not the power management
-		 * capability that we are looking for, follow the link to
-		 * the next capability and continue looping.
-		 */
-		rtl_pci_raw_write_port_ulong(PCI_CONF_ADDRESS ,
-					     pcicfg_addr_port +
-					     (num4bytes << 2));
-		rtl_pci_raw_read_port_ushort(PCI_CONF_DATA,
-					     (u16*)&capability_hdr);
-		/* Found the PCI express capability. */
-		if (capability_hdr.capability_id ==
-		    PCI_CAPABILITY_ID_PCI_EXPRESS)
-			break;
-		else
-        		capability_offset = capability_hdr.next;
-	}
-	return capability_offset;
-}
-#endif
-/*<delete in kernel end>*/
-
 bool rtl_pci_check_buddy_priv(struct ieee80211_hw *hw,
         		      struct rtl_priv **buddy_priv)
 {
@@ -610,14 +567,7 @@ static void _rtl_pci_tx_chk_waitq(struct ieee80211_hw *hw)
 				_rtl_pci_update_earlymode_info(hw, skb,
 							       &tcb_desc, tid);
 
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0))
-			rtlpriv->intf_ops->adapter_tx(hw, skb, &tcb_desc);
-#else
-/*<delete in kernel end>*/
 			rtlpriv->intf_ops->adapter_tx(hw, NULL, skb, &tcb_desc);
-#endif
-/*<delete in kernel end>*/
 		}
 	}
 }
@@ -1201,19 +1151,9 @@ static void _rtl_pci_prepare_bcn_tasklet(struct ieee80211_hw *hw)
 	if (rtlpriv->use_new_trx_flow)
 		pbuffer_desc = &ring->buffer_desc[0];
 
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0))
-	rtlpriv->cfg->ops->fill_tx_desc(hw, hdr, (u8 *) pdesc,
-				        (u8 *)pbuffer_desc, info, pskb,
-				        BEACON_QUEUE, &tcb_desc);
-#else
-/*<delete in kernel end>*/
 	rtlpriv->cfg->ops->fill_tx_desc(hw, hdr, (u8 *) pdesc,
 					(u8 *)pbuffer_desc, info, NULL, pskb,
 					BEACON_QUEUE, &tcb_desc);
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
 
 	__skb_queue_tail(&ring->queue, pskb);
 
@@ -1616,26 +1556,11 @@ int rtl_pci_reset_trx_ring(struct ieee80211_hw *hw)
 	return 0;
 }
 
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0))
-static bool rtl_pci_tx_chk_waitq_insert(struct ieee80211_hw *hw,
-					struct sk_buff *skb)
-#else
-/*<delete in kernel end>*/
 static bool rtl_pci_tx_chk_waitq_insert(struct ieee80211_hw *hw,
 					struct ieee80211_sta *sta,
 					struct sk_buff *skb)
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0))
-	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
-	struct ieee80211_sta *sta = info->control.sta;
-#endif
-/*<delete in kernel end>*/
 	struct rtl_sta_info *sta_entry = NULL;
 	u8 tid = rtl_get_tid(skb);
 	u16 fc = rtl_get_fc(skb);
@@ -1671,28 +1596,14 @@ static bool rtl_pci_tx_chk_waitq_insert(struct ieee80211_hw *hw,
 	return true;
 }
 
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0))
-int rtl_pci_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
-	       struct rtl_tcb_desc *ptcb_desc)
-#else
-/*<delete in kernel end>*/
 static int rtl_pci_tx(struct ieee80211_hw *hw,
 		      struct ieee80211_sta *sta,
 		      struct sk_buff *skb,
 		      struct rtl_tcb_desc *ptcb_desc)
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_sta_info *sta_entry = NULL;
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0))
-	struct ieee80211_sta *sta = info->control.sta;
-#endif
-/*<delete in kernel end>*/
 	struct rtl8192_tx_ring *ring;
 	struct rtl_tx_desc *pdesc;
 	struct rtl_tx_buffer_desc *ptx_bd_desc = NULL;
@@ -1777,19 +1688,9 @@ static int rtl_pci_tx(struct ieee80211_hw *hw,
 	if (ieee80211_is_data(fc))
 		rtlpriv->cfg->ops->led_control(hw, LED_CTL_TX);
 
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0))
-	rtlpriv->cfg->ops->fill_tx_desc(hw, hdr, (u8 *) pdesc,
-					(u8 *)ptx_bd_desc, info, skb,
-					hw_queue, ptcb_desc);
-#else
-/*<delete in kernel end>*/
 	rtlpriv->cfg->ops->fill_tx_desc(hw, hdr, (u8 *) pdesc,
 					(u8 *)ptx_bd_desc, info, sta, skb,
 					hw_queue, ptcb_desc);
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
 
 	__skb_queue_tail(&ring->queue, skb);
 	if (rtlpriv->use_new_trx_flow) {
@@ -1819,11 +1720,7 @@ static int rtl_pci_tx(struct ieee80211_hw *hw,
 
 	return 0;
 }
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,10,0))
 static void rtl_pci_flush(struct ieee80211_hw *hw, u32 queues, bool drop)
-#else
-static void rtl_pci_flush(struct ieee80211_hw *hw, bool drop)
-#endif
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
@@ -1838,12 +1735,10 @@ static void rtl_pci_flush(struct ieee80211_hw *hw, bool drop)
 
 	for (queue_id = RTL_PCI_MAX_TX_QUEUE_COUNT - 1; queue_id >= 0;) {
 		u32 queue_len;
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,10,0))
 		if (((queues >> queue_id) & 0x1) == 0) {
 			queue_id--;
 			continue;
 		}
-#endif
 		ring = &pcipriv->dev.tx_ring[queue_id];
 		queue_len = skb_queue_len(&ring->queue);
 		if (queue_len == 0 || queue_id == BEACON_QUEUE ||
@@ -2122,17 +2017,8 @@ static bool _rtl_pci_find_adapter(struct pci_dev *pdev,
 		    (pcipriv->ndis_adapter.pcibridge_busnum << 16) |
 		    (pcipriv->ndis_adapter.pcibridge_devnum << 11) |
 		    (pcipriv->ndis_adapter.pcibridge_funcnum << 8) | (1 << 31);
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,35))
-/*<delete in kernel end>*/
 		pcipriv->ndis_adapter.pcibridge_pciehdr_offset =
 		    pci_pcie_cap(bridge_pdev);
-/*<delete in kernel start>*/
-#else
-		pcipriv->ndis_adapter.pcibridge_pciehdr_offset =
-			_rtl_pci_get_pciehdr_offset(hw);
-#endif
-/*<delete in kernel end>*/
 		pcipriv->ndis_adapter.num4bytes =
 		    (pcipriv->ndis_adapter.pcibridge_pciehdr_offset + 0x10) / 4;
 
@@ -2230,14 +2116,8 @@ static int rtl_pci_intr_mode_decide(struct ieee80211_hw *hw)
  * hw pointer in rtl_pci_get_hw_pointer */
 struct ieee80211_hw *hw_export = NULL;
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,8,0))
 int rtl_pci_probe(struct pci_dev *pdev,
                   const struct pci_device_id *id)
-
-#else
-int __devinit rtl_pci_probe(struct pci_dev *pdev,
-			    const struct pci_device_id *id)
-#endif
 {
 	struct ieee80211_hw *hw = NULL;
 
@@ -2313,16 +2193,16 @@ int __devinit rtl_pci_probe(struct pci_dev *pdev,
 
 	/*shared mem start */
 	rtlpriv->io.pci_mem_start =
-			(unsigned long)pci_iomap(pdev,
+			pci_iomap(pdev,
 			rtlpriv->cfg->bar_id, pmem_len);
-	if (rtlpriv->io.pci_mem_start == 0) {
+	if (rtlpriv->io.pci_mem_start == NULL) {
 		RT_ASSERT(false, ("Can't map PCI mem\n"));
 		goto fail2;
 	}
 
 	RT_TRACE(COMP_INIT, DBG_DMESG,
 		 ("mem mapped space: start: 0x%08lx len:%08lx "
-		  "flags:%08lx, after map:0x%08lx\n",
+		  "flags:%08lx, after map:0x%p\n",
 		  pmem_start, pmem_len, pmem_flags,
 		  rtlpriv->io.pci_mem_start));
 
@@ -2415,8 +2295,8 @@ fail3:
 	rtl_deinit_core(hw);
 	ieee80211_free_hw(hw);
 
-	if (rtlpriv->io.pci_mem_start != 0)
-		pci_iounmap(pdev, (void *)rtlpriv->io.pci_mem_start);
+	if (rtlpriv->io.pci_mem_start != NULL)
+		pci_iounmap(pdev, rtlpriv->io.pci_mem_start);
 
 fail2:
 	pci_release_regions(pdev);
@@ -2478,8 +2358,8 @@ void rtl_pci_disconnect(struct pci_dev *pdev)
 		pci_disable_msi(rtlpci->pdev);
 
 	list_del(&rtlpriv->list);
-	if (rtlpriv->io.pci_mem_start != 0) {
-		pci_iounmap(pdev, (void *)rtlpriv->io.pci_mem_start);
+	if (rtlpriv->io.pci_mem_start != NULL) {
+		pci_iounmap(pdev, rtlpriv->io.pci_mem_start);
 		pci_release_regions(pdev);
 	}
 
diff --git a/drivers/staging/rtl8821ae/pci.h b/drivers/staging/rtl8821ae/pci.h
index 06eaa521e0e..3f16ec90296 100644
--- a/drivers/staging/rtl8821ae/pci.h
+++ b/drivers/staging/rtl8821ae/pci.h
@@ -282,13 +282,8 @@ int rtl_pci_reset_trx_ring(struct ieee80211_hw *hw);
 
 extern struct rtl_intf_ops rtl_pci_ops;
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,8,0))
 int rtl_pci_probe(struct pci_dev *pdev,
                   const struct pci_device_id *id);
-#else
-int __devinit rtl_pci_probe(struct pci_dev *pdev,
-			    const struct pci_device_id *id);
-#endif
 void rtl_pci_disconnect(struct pci_dev *pdev);
 int rtl_pci_suspend(struct device *dev);
 int rtl_pci_resume(struct device *dev);
diff --git a/drivers/staging/rtl8821ae/ps.c b/drivers/staging/rtl8821ae/ps.c
index 7876442417f..5a9bbf025e2 100644
--- a/drivers/staging/rtl8821ae/ps.c
+++ b/drivers/staging/rtl8821ae/ps.c
@@ -30,9 +30,7 @@
 #include "wifi.h"
 #include "base.h"
 #include "ps.h"
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,2,0))
 #include <linux/export.h>
-#endif
 #include "btcoexist/rtl_btc.h"
 
 bool rtl_ps_enable_nic(struct ieee80211_hw *hw)
@@ -542,17 +540,8 @@ void rtl_swlps_beacon(struct ieee80211_hw *hw, void *data, unsigned int len)
 	tim_len = tim[1];
 	tim_ie = (struct ieee80211_tim_ie *) &tim[2];
 
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,35))
-/*<delete in kernel end>*/
 	if (!WARN_ON_ONCE(!hw->conf.ps_dtim_period))
 		rtlpriv->psc.dtim_counter = tim_ie->dtim_count;
-/*<delete in kernel start>*/
-#else
-	if (!WARN_ON_ONCE(!mac->vif->bss_conf.dtim_period))
-		rtlpriv->psc.dtim_counter = tim_ie->dtim_count;
-#endif
-/*<delete in kernel end>*/
 
 	/* Check whenever the PHY can be turned off again. */
 
@@ -656,9 +645,6 @@ void rtl_swlps_rf_sleep(struct ieee80211_hw *hw)
 	 * time to sleep_intv = rtlpriv->psc.dtim_counter or
 	 * MAX_SW_LPS_SLEEP_INTV(default set to 5) */
 
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,35))
-/*<delete in kernel end>*/
 	if (rtlpriv->psc.dtim_counter == 0) {
 		if (hw->conf.ps_dtim_period == 1)
 			sleep_intv = hw->conf.ps_dtim_period * 2;
@@ -667,18 +653,6 @@ void rtl_swlps_rf_sleep(struct ieee80211_hw *hw)
 	} else {
 		sleep_intv = rtlpriv->psc.dtim_counter;
 	}
-/*<delete in kernel start>*/
-#else
-	if (rtlpriv->psc.dtim_counter == 0) {
-		if (mac->vif->bss_conf.dtim_period == 1)
-			sleep_intv = mac->vif->bss_conf.dtim_period * 2;
-		else
-			sleep_intv = mac->vif->bss_conf.dtim_period;
-	} else {
-		sleep_intv = rtlpriv->psc.dtim_counter;
-	}
-#endif
-/*<delete in kernel end>*/
 
 	if (sleep_intv > MAX_SW_LPS_SLEEP_INTV)
 		sleep_intv = MAX_SW_LPS_SLEEP_INTV;
diff --git a/drivers/staging/rtl8821ae/rc.c b/drivers/staging/rtl8821ae/rc.c
index 0cc32c60dde..0b4f3218508 100644
--- a/drivers/staging/rtl8821ae/rc.c
+++ b/drivers/staging/rtl8821ae/rc.c
@@ -210,16 +210,8 @@ static void rtl_tx_status(void *ppriv,
 						       tid)) {
 					sta_entry->tids[tid].agg.agg_state =
 						RTL_AGG_PROGRESS;
-					/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,38))
-					/*<delete in kernel end>*/
 					ieee80211_start_tx_ba_session(sta, tid,
 								      5000);
-					/*<delete in kernel start>*/
-#else
-					ieee80211_start_tx_ba_session(sta, tid);
-#endif
-					/*<delete in kernel end>*/
 				}
 			}
 		}
@@ -232,15 +224,6 @@ static void rtl_rate_init(void *ppriv,
 			  struct ieee80211_sta *sta, void *priv_sta)
 {
 }
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,5,0))
-static void rtl_rate_update(void *ppriv,
-			    struct ieee80211_supported_band *sband,
-			    struct ieee80211_sta *sta, void *priv_sta,
-			    u32 changed,
-			    enum nl80211_channel_type oper_chan_type)
-{
-}
-#else
 static void rtl_rate_update(void *ppriv,
 			    struct ieee80211_supported_band *sband,
 			    struct cfg80211_chan_def *chandef,
@@ -248,7 +231,6 @@ static void rtl_rate_update(void *ppriv,
 			    u32 changed)
 {
 }
-#endif
 static void *rtl_rate_alloc(struct ieee80211_hw *hw, struct dentry *debugfsdir)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -286,7 +268,7 @@ static void rtl_rate_free_sta(void *rtlpriv,
 }
 
 static struct rate_control_ops rtl_rate_ops = {
-	.name = "rtl_rc",
+	.name = "rtl_rc_21ae",
 	.alloc = rtl_rate_alloc,
 	.free = rtl_rate_free,
 	.alloc_sta = rtl_rate_alloc_sta,
diff --git a/drivers/staging/rtl8821ae/regd.c b/drivers/staging/rtl8821ae/regd.c
index 0a4b3984b7e..2efa5f31822 100644
--- a/drivers/staging/rtl8821ae/regd.c
+++ b/drivers/staging/rtl8821ae/regd.c
@@ -158,11 +158,6 @@ static void _rtl_reg_apply_beaconing_flags(struct wiphy *wiphy,
 	const struct ieee80211_reg_rule *reg_rule;
 	struct ieee80211_channel *ch;
 	unsigned int i;
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,9,0))
-	u32 bandwidth = 0;
-	int r;
-#endif
-
 	for (band = 0; band < IEEE80211_NUM_BANDS; band++) {
 
 		if (!wiphy->bands[band])
@@ -176,16 +171,9 @@ static void _rtl_reg_apply_beaconing_flags(struct wiphy *wiphy,
 			    (ch->flags & IEEE80211_CHAN_RADAR))
 				continue;
 			if (initiator == NL80211_REGDOM_SET_BY_COUNTRY_IE) {
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,9,0))
 				reg_rule = freq_reg_info(wiphy, ch->center_freq);
 				if (IS_ERR(reg_rule))
 					continue;
-#else
-				r = freq_reg_info(wiphy, ch->center_freq,
-						  bandwidth, &reg_rule);
-				if (r)
-					continue;
-#endif
 
 				/*
 				 *If 11d had a rule for this channel ensure
@@ -219,10 +207,6 @@ static void _rtl_reg_apply_active_scan_flags(struct wiphy *wiphy,
 	struct ieee80211_supported_band *sband;
 	struct ieee80211_channel *ch;
 	const struct ieee80211_reg_rule *reg_rule;
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,9,0))
-	u32 bandwidth = 0;
-	int r;
-#endif
 
 	if (!wiphy->bands[IEEE80211_BAND_2GHZ])
 		return;
@@ -250,26 +234,16 @@ static void _rtl_reg_apply_active_scan_flags(struct wiphy *wiphy,
 	 */
 
 	ch = &sband->channels[11];	/* CH 12 */
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,9,0))
 	reg_rule = freq_reg_info(wiphy, ch->center_freq);
 	if (!IS_ERR(reg_rule)) {
-#else
-	r = freq_reg_info(wiphy, ch->center_freq, bandwidth, &reg_rule);
-	if (!r) {
-#endif
 		if (!(reg_rule->flags & NL80211_RRF_PASSIVE_SCAN))
 			if (ch->flags & IEEE80211_CHAN_PASSIVE_SCAN)
 				ch->flags &= ~IEEE80211_CHAN_PASSIVE_SCAN;
 	}
 
 	ch = &sband->channels[12];	/* CH 13 */
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,9,0))
 	reg_rule = freq_reg_info(wiphy, ch->center_freq);
 	if (!IS_ERR(reg_rule)) {
-#else
-	r = freq_reg_info(wiphy, ch->center_freq, bandwidth, &reg_rule);
-	if (!r) {
-#endif
 		if (!(reg_rule->flags & NL80211_RRF_PASSIVE_SCAN))
 			if (ch->flags & IEEE80211_CHAN_PASSIVE_SCAN)
 				ch->flags &= ~IEEE80211_CHAN_PASSIVE_SCAN;
@@ -389,19 +363,11 @@ static const struct ieee80211_regdomain *_rtl_regdomain_select(
 	}
 }
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,9,0))
 static int _rtl_regd_init_wiphy(struct rtl_regulatory *reg,
 				struct wiphy *wiphy,
 				void (*reg_notifier) (struct wiphy * wiphy,
 						     struct regulatory_request *
 						     request))
-#else
-static int _rtl_regd_init_wiphy(struct rtl_regulatory *reg,
-				struct wiphy *wiphy,
-				int (*reg_notifier) (struct wiphy * wiphy,
-						     struct regulatory_request *
-						     request))
-#endif
 {
 	const struct ieee80211_regdomain *regd;
 
@@ -429,15 +395,9 @@ static struct country_code_to_enum_rd *_rtl_regd_find_country(u16 countrycode)
 	return NULL;
 }
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,9,0))
 int rtl_regd_init(struct ieee80211_hw *hw,
 		  void (*reg_notifier) (struct wiphy *wiphy,
 				        struct regulatory_request *request))
-#else
-int rtl_regd_init(struct ieee80211_hw *hw,
-		  int (*reg_notifier) (struct wiphy *wiphy,
-				       struct regulatory_request *request))
-#endif
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct wiphy *wiphy = hw->wiphy;
@@ -480,7 +440,6 @@ int rtl_regd_init(struct ieee80211_hw *hw,
 	return 0;
 }
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,9,0))
 void rtl_reg_notifier(struct wiphy *wiphy, struct regulatory_request *request)
 {
 	struct ieee80211_hw *hw = wiphy_to_ieee80211_hw(wiphy);
@@ -490,14 +449,3 @@ void rtl_reg_notifier(struct wiphy *wiphy, struct regulatory_request *request)
 
 	_rtl_reg_notifier_apply(wiphy, request, &rtlpriv->regd);
 }
-#else
-int rtl_reg_notifier(struct wiphy *wiphy, struct regulatory_request *request)
-{
-	struct ieee80211_hw *hw = wiphy_to_ieee80211_hw(wiphy);
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	RT_TRACE(COMP_REGD, DBG_LOUD, ("\n"));
-
-	return _rtl_reg_notifier_apply(wiphy, request, &rtlpriv->regd);
-}
-#endif
diff --git a/drivers/staging/rtl8821ae/regd.h b/drivers/staging/rtl8821ae/regd.h
index dceb3f18200..1cfcb97b0ab 100644
--- a/drivers/staging/rtl8821ae/regd.h
+++ b/drivers/staging/rtl8821ae/regd.h
@@ -60,16 +60,8 @@ enum country_code_type_t {
 	COUNTRY_CODE_MAX
 };
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,9,0))
 int rtl_regd_init(struct ieee80211_hw *hw,
 		  void (*reg_notifier) (struct wiphy *wiphy,
 				        struct regulatory_request *request));
 void rtl_reg_notifier(struct wiphy *wiphy, struct regulatory_request *request);
-#else
-int rtl_regd_init(struct ieee80211_hw *hw,
-		  int (*reg_notifier) (struct wiphy *wiphy,
-				       struct regulatory_request *request));
-int rtl_reg_notifier(struct wiphy *wiphy, struct regulatory_request *request);
-#endif
-
 #endif
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/hw.h b/drivers/staging/rtl8821ae/rtl8821ae/hw.h
index 4fb6bf0d1da..256e514f424 100644
--- a/drivers/staging/rtl8821ae/rtl8821ae/hw.h
+++ b/drivers/staging/rtl8821ae/rtl8821ae/hw.h
@@ -11,10 +11,6 @@
  * 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, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -34,42 +30,43 @@ void rtl8821ae_get_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val);
 void rtl8821ae_read_eeprom_info(struct ieee80211_hw *hw);
 
 void rtl8821ae_interrupt_recognized(struct ieee80211_hw *hw,
-											 u32 *p_inta, u32 *p_intb);
+				    u32 *p_inta, u32 *p_intb);
 int rtl8821ae_hw_init(struct ieee80211_hw *hw);
 void rtl8821ae_card_disable(struct ieee80211_hw *hw);
 void rtl8821ae_enable_interrupt(struct ieee80211_hw *hw);
 void rtl8821ae_disable_interrupt(struct ieee80211_hw *hw);
-int rtl8821ae_set_network_type(struct ieee80211_hw *hw, enum nl80211_iftype type);
+int rtl8821ae_set_network_type(struct ieee80211_hw *hw,
+			       enum nl80211_iftype type);
 void rtl8821ae_set_check_bssid(struct ieee80211_hw *hw, bool check_bssid);
 void rtl8821ae_set_qos(struct ieee80211_hw *hw, int aci);
 void rtl8821ae_set_beacon_related_registers(struct ieee80211_hw *hw);
 void rtl8821ae_set_beacon_interval(struct ieee80211_hw *hw);
 void rtl8821ae_update_interrupt_mask(struct ieee80211_hw *hw,
-												u32 add_msr, u32 rm_msr);
+				     u32 add_msr, u32 rm_msr);
 void rtl8821ae_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val);
 void rtl8821ae_update_hal_rate_tbl(struct ieee80211_hw *hw,
-											struct ieee80211_sta *sta,
-											u8 rssi_level);
+				   struct ieee80211_sta *sta,
+				   u8 rssi_level);
 void rtl8821ae_update_channel_access_setting(struct ieee80211_hw *hw);
 bool rtl8821ae_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 *valid);
 void rtl8821ae_enable_hw_security_config(struct ieee80211_hw *hw);
 void rtl8821ae_set_key(struct ieee80211_hw *hw, u32 key_index,
-		     				u8 *p_macaddr, bool is_group, u8 enc_algo,
-		     				bool is_wepkey, bool clear_all);
+		       u8 *p_macaddr, bool is_group, u8 enc_algo,
+		       bool is_wepkey, bool clear_all);
 
 void rtl8821ae_read_bt_coexist_info_from_hwpg(struct ieee80211_hw *hw,
-															bool autoload_fail,
-															u8* hwinfo);
+					      bool autoload_fail,
+					      u8 *hwinfo);
 void rtl8812ae_read_bt_coexist_info_from_hwpg(struct ieee80211_hw *hw,
-															bool autoload_fail,
-															u8* hwinfo);
-void rtl8821ae_bt_reg_init(struct ieee80211_hw* hw);
-void rtl8821ae_bt_hw_init(struct ieee80211_hw* hw);
+					      bool autoload_fail,
+					      u8 *hwinfo);
+void rtl8821ae_bt_reg_init(struct ieee80211_hw *hw);
+void rtl8821ae_bt_hw_init(struct ieee80211_hw *hw);
 void rtl8821ae_suspend(struct ieee80211_hw *hw);
 void rtl8821ae_resume(struct ieee80211_hw *hw);
 void rtl8821ae_allow_all_destaddr(struct ieee80211_hw *hw,
-										  bool allow_all_da,
-										  bool write_into_reg);
+				  bool allow_all_da,
+				  bool write_into_reg);
 void _rtl8821ae_stop_tx_beacon(struct ieee80211_hw *hw);
 void _rtl8821ae_resume_tx_beacon(struct ieee80211_hw *hw);
 #endif
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/sw.c b/drivers/staging/rtl8821ae/rtl8821ae/sw.c
index a8d17556977..26212755f88 100644
--- a/drivers/staging/rtl8821ae/rtl8821ae/sw.c
+++ b/drivers/staging/rtl8821ae/rtl8821ae/sw.c
@@ -45,7 +45,7 @@
 #include "hal_btc.h"
 #include "../btcoexist/rtl_btc.h"
 
-void rtl8821ae_init_aspm_vars(struct ieee80211_hw *hw)
+static void rtl8821ae_init_aspm_vars(struct ieee80211_hw *hw)
 {
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 
@@ -225,7 +225,7 @@ void rtl8821ae_deinit_sw_vars(struct ieee80211_hw *hw)
 	//printk("<=========rtl8821ae_deinit_sw_vars().\n");
 }
 
-u32 rtl8812ae_rx_command_packet_handler(
+static u32 rtl8812ae_rx_command_packet_handler(
 	struct ieee80211_hw *hw,
 	struct rtl_stats status,
 	struct sk_buff *skb
@@ -260,7 +260,7 @@ bool rtl8821ae_get_btc_status(void)
 	return true;
 }
 
-struct rtl_hal_ops rtl8821ae_hal_ops = {
+static struct rtl_hal_ops rtl8821ae_hal_ops = {
 	.init_sw_vars = rtl8821ae_init_sw_vars,
 	.deinit_sw_vars = rtl8821ae_deinit_sw_vars,
 	.read_eeprom_info = rtl8821ae_read_eeprom_info,
@@ -311,14 +311,14 @@ struct rtl_hal_ops rtl8821ae_hal_ops = {
 	.rx_command_packet_handler = rtl8812ae_rx_command_packet_handler,
 };
 
-struct rtl_mod_params rtl8821ae_mod_params = {
+static struct rtl_mod_params rtl8821ae_mod_params = {
 	.sw_crypto = false,
 	.b_inactiveps = false,//true,
 	.b_swctrl_lps = false,
 	.b_fwctrl_lps = false, //true,
 };
 
-struct rtl_hal_cfg rtl8821ae_hal_cfg = {
+static struct rtl_hal_cfg rtl8821ae_hal_cfg = {
 	.bar_id = 2,
 	.write_readback = true,
 	.name = "rtl8821ae_pci",
@@ -415,19 +415,11 @@ struct rtl_hal_cfg rtl8821ae_hal_cfg = {
 	.maps[RTL_RC_HT_RATEMCS15] =  DESC_RATEMCS15,
 };
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,8,0))
 static struct pci_device_id rtl8821ae_pci_ids[] = {
         {RTL_PCI_DEVICE(PCI_VENDOR_ID_REALTEK, 0x8812, rtl8821ae_hal_cfg)},
         {RTL_PCI_DEVICE(PCI_VENDOR_ID_REALTEK, 0x8821, rtl8821ae_hal_cfg)},
         {},
 };
-#else
-static struct pci_device_id rtl8821ae_pci_ids[] __devinitdata = {
-	{RTL_PCI_DEVICE(PCI_VENDOR_ID_REALTEK, 0x8812, rtl8821ae_hal_cfg)},
-	{RTL_PCI_DEVICE(PCI_VENDOR_ID_REALTEK, 0x8821, rtl8821ae_hal_cfg)},
-	{},
-};
-#endif
 
 MODULE_DEVICE_TABLE(pci, rtl8821ae_pci_ids);
 
@@ -445,14 +437,7 @@ MODULE_PARM_DESC(swenc, "using hardware crypto (default 0 [hardware])\n");
 MODULE_PARM_DESC(ips, "using no link power save (default 1 is open)\n");
 MODULE_PARM_DESC(fwlps, "using linked fw control power save (default 1 is open)\n");
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,29))
-static const SIMPLE_DEV_PM_OPS(rtlwifi_pm_ops, rtl_pci_suspend, rtl_pci_resume);
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,29))
-compat_pci_suspend(rtl_pci_suspend)
-compat_pci_resume(rtl_pci_resume)
-#endif
+static SIMPLE_DEV_PM_OPS(rtlwifi_pm_ops, rtl_pci_suspend, rtl_pci_resume);
 
 static struct pci_driver rtl8821ae_driver = {
 	.name = KBUILD_MODNAME,
@@ -460,13 +445,7 @@ static struct pci_driver rtl8821ae_driver = {
 	.probe = rtl_pci_probe,
 	.remove = rtl_pci_disconnect,
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,29))
 	.driver.pm = &rtlwifi_pm_ops,
-#elif defined(CONFIG_PM)
-	.suspend = rtl_pci_suspend_compat,
-	.resume = rtl_pci_resume_compat,
-#endif
-
 };
 
 
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/trx.c b/drivers/staging/rtl8821ae/rtl8821ae/trx.c
index f40a93c0d2b..b4943e8b7f8 100644
--- a/drivers/staging/rtl8821ae/rtl8821ae/trx.c
+++ b/drivers/staging/rtl8821ae/rtl8821ae/trx.c
@@ -77,11 +77,7 @@ static int _rtl8821ae_rate_mapping(struct ieee80211_hw *hw,
 	int rate_idx;
 
 	if (false == isht) {
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,10,0))
 		if (IEEE80211_BAND_2GHZ == hw->conf.chandef.chan->band) {
-#else
-		if (IEEE80211_BAND_2GHZ == hw->conf.channel->band) {
-#endif
 			switch (desc_rate) {
 			case DESC_RATE1M:
 				rate_idx = 0;
@@ -579,13 +575,8 @@ bool rtl8821ae_rx_query_desc(struct ieee80211_hw *hw,
 	if (status->wake_match)
 		RT_TRACE(COMP_RXDESC,DBG_LOUD,
 		("GGGGGGGGGGGGGet Wakeup Packet!! WakeMatch=%d\n",status->wake_match ));
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,10,0))
 	rx_status->freq = hw->conf.chandef.chan->center_freq;
 	rx_status->band = hw->conf.chandef.chan->band;
-#else
-	rx_status->freq = hw->conf.channel->center_freq;
-	rx_status->band = hw->conf.channel->band;
-#endif
 
 	hdr = (struct ieee80211_hdr *)(skb->data + status->rx_drvinfo_size
 			+ status->rx_bufshift);
@@ -650,33 +641,17 @@ bool rtl8821ae_rx_query_desc(struct ieee80211_hw *hw,
 	return true;
 }
 
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0))
-void rtl8821ae_tx_fill_desc(struct ieee80211_hw *hw,
-			  struct ieee80211_hdr *hdr, u8 *pdesc_tx, u8 *txbd,
-			  struct ieee80211_tx_info *info, struct sk_buff *skb,
-			  u8 hw_queue, struct rtl_tcb_desc *ptcb_desc)
-#else
-/*<delete in kernel end>*/
 void rtl8821ae_tx_fill_desc(struct ieee80211_hw *hw,
 			  struct ieee80211_hdr *hdr, u8 *pdesc_tx, u8 *txbd,
 			  struct ieee80211_tx_info *info,
 			  struct ieee80211_sta *sta,
 			  struct sk_buff *skb,
 			  u8 hw_queue, struct rtl_tcb_desc *ptcb_desc)
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0))
-	struct ieee80211_sta *sta = info->control.sta;
-#endif
-/*<delete in kernel end>*/
 	u8 *pdesc = (u8 *) pdesc_tx;
 	u16 seq_number;
 	u16 fc = le16_to_cpu(hdr->frame_control);
@@ -783,9 +758,6 @@ void rtl8821ae_tx_fill_desc(struct ieee80211_hw *hw,
 		}
 		if (info->control.hw_key) {
 			struct ieee80211_key_conf *keyconf = info->control.hw_key;
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,37))
-/*<delete in kernel end>*/
 			switch (keyconf->cipher) {
 			case WLAN_CIPHER_SUITE_WEP40:
 			case WLAN_CIPHER_SUITE_WEP104:
@@ -800,23 +772,6 @@ void rtl8821ae_tx_fill_desc(struct ieee80211_hw *hw,
 				break;
 
 			}
-/*<delete in kernel start>*/
-#else
-			switch (keyconf->alg) {
-			case ALG_WEP:
-			case ALG_TKIP:
-				SET_TX_DESC_SEC_TYPE(pdesc, 0x1);
-				break;
-			case ALG_CCMP:
-				SET_TX_DESC_SEC_TYPE(pdesc, 0x3);
-				break;
-			default:
-				SET_TX_DESC_SEC_TYPE(pdesc, 0x0);
-				break;
-
-			}
-#endif
-/*<delete in kernel end>*/
 		}
 
 		SET_TX_DESC_QUEUE_SEL(pdesc, fw_qsel);
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/trx.h b/drivers/staging/rtl8821ae/rtl8821ae/trx.h
index da93e5c7ece..af017844d82 100644
--- a/drivers/staging/rtl8821ae/rtl8821ae/trx.h
+++ b/drivers/staging/rtl8821ae/rtl8821ae/trx.h
@@ -609,23 +609,12 @@ struct rx_desc_8821ae {
 
 } __packed;
 
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0))
-void rtl8821ae_tx_fill_desc(struct ieee80211_hw *hw,
-			  struct ieee80211_hdr *hdr, u8 *pdesc_tx, u8 *txbd,
-			  struct ieee80211_tx_info *info, struct sk_buff *skb,
-			  u8 hw_queue, struct rtl_tcb_desc *ptcb_desc);
-#else
-/*<delete in kernel end>*/
 void rtl8821ae_tx_fill_desc(struct ieee80211_hw *hw,
 			  struct ieee80211_hdr *hdr, u8 *pdesc_tx, u8 *txbd,
 			  struct ieee80211_tx_info *info,
 			  struct ieee80211_sta *sta,
 			  struct sk_buff *skb,
 			  u8 hw_queue, struct rtl_tcb_desc *ptcb_desc);
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
 bool rtl8821ae_rx_query_desc(struct ieee80211_hw *hw,
 			   struct rtl_stats *status,
 			   struct ieee80211_rx_status *rx_status,
diff --git a/drivers/staging/rtl8821ae/stats.c b/drivers/staging/rtl8821ae/stats.c
index a20c0f8f65e..4d383d11574 100644
--- a/drivers/staging/rtl8821ae/stats.c
+++ b/drivers/staging/rtl8821ae/stats.c
@@ -28,9 +28,7 @@
  *****************************************************************************/
 #include "wifi.h"
 #include "stats.h"
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,2,0))
 #include <linux/export.h>
-#endif
 
 u8 rtl_query_rxpwrpercentage(char antpower)
 {
diff --git a/drivers/staging/rtl8821ae/wifi.h b/drivers/staging/rtl8821ae/wifi.h
index 17a9d9f8781..e8250dad613 100644
--- a/drivers/staging/rtl8821ae/wifi.h
+++ b/drivers/staging/rtl8821ae/wifi.h
@@ -1086,8 +1086,8 @@ struct rtl_io {
 	struct device *dev;
 
 	/*PCI MEM map */
-	unsigned long pci_mem_end;	/*shared mem end        */
-	unsigned long pci_mem_start;	/*shared mem start */
+	void __iomem *pci_mem_end;	/*shared mem end        */
+	void __iomem *pci_mem_start;	/*shared mem start */
 
 	/*PCI IO map */
 	unsigned long pci_base_addr;	/*device I/O address */
diff --git a/drivers/staging/rts5139/Kconfig b/drivers/staging/rts5139/Kconfig
deleted file mode 100644
index afd526bf422..00000000000
--- a/drivers/staging/rts5139/Kconfig
+++ /dev/null
@@ -1,16 +0,0 @@
-config RTS5139
-	tristate "Realtek RTS5139 USB card reader support"
-	depends on USB && SCSI
-	help
-	  Say Y here to include driver code to support the Realtek
-	  RTS5139 USB card readers.
-
-	  If this driver is compiled as a module, it will be named rts5139.
-
-config RTS5139_DEBUG
-	bool "Realtek RTS5139 Card Reader verbose debug"
-	depends on RTS5139
-	help
-	  Say Y here in order to have the rts5139 code generate
-	  verbose debugging messages.
-
diff --git a/drivers/staging/rts5139/Makefile b/drivers/staging/rts5139/Makefile
deleted file mode 100644
index 75dd31224e6..00000000000
--- a/drivers/staging/rts5139/Makefile
+++ /dev/null
@@ -1,43 +0,0 @@
-# Driver for Realtek RTS51xx USB card reader
-#
-# Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
-#
-# Author:
-#   wwang (wei_wang@realsil.com.cn)
-#   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
-# Maintainer:
-#   Edwin Rong (edwin_rong@realsil.com.cn)
-#   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
-#
-# Makefile for the RTS51xx USB Card Reader drivers.
-#
-
-obj-$(CONFIG_RTS5139) := rts5139.o
-
-ccflags-y := -Idrivers/scsi
-
-rts5139-y :=				\
-		rts51x_transport.o 	\
-		rts51x_scsi.o		\
-		rts51x_fop.o		\
-		rts51x.o		\
-		rts51x_chip.o		\
-		rts51x_card.o		\
-		xd.o			\
-		sd.o			\
-		ms.o			\
-		sd_cprm.o		\
-		ms_mg.o
diff --git a/drivers/staging/rts5139/TODO b/drivers/staging/rts5139/TODO
deleted file mode 100644
index dd5fabb8ea7..00000000000
--- a/drivers/staging/rts5139/TODO
+++ /dev/null
@@ -1,9 +0,0 @@
-TODO:
-- support more USB card reader of Realtek family
-- use kernel coding style
-- checkpatch.pl fixes
-- stop having thousands of lines of code duplicated with staging/rts_pstor
-- This driver contains an entire SD/MMC stack -- it should use the stack in
-  drivers/mmc instead, as a host driver e.g. drivers/mmc/host/realtek-usb.c;
-  see drivers/mmc/host/ushc.c as an example.
-- This driver presents cards as SCSI devices, but they should be MMC devices.
diff --git a/drivers/staging/rts5139/debug.h b/drivers/staging/rts5139/debug.h
deleted file mode 100644
index 73dec133a1b..00000000000
--- a/drivers/staging/rts5139/debug.h
+++ /dev/null
@@ -1,46 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- * Header file
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#ifndef __RTS51X_DEBUG_H
-#define __RTS51X_DEBUG_H
-
-#include <linux/kernel.h>
-
-#define RTS51X_TIP "rts51x: "
-
-#ifdef CONFIG_RTS5139_DEBUG
-#define RTS51X_DEBUGP(x...) printk(KERN_DEBUG RTS51X_TIP x)
-#define RTS51X_DEBUGPN(x...) printk(KERN_DEBUG x)
-#define RTS51X_DEBUGPX(x...) printk(x)
-#define RTS51X_DEBUG(x) x
-#else
-#define RTS51X_DEBUGP(x...)
-#define RTS51X_DEBUGPN(x...)
-#define RTS51X_DEBUGPX(x...)
-#define RTS51X_DEBUG(x)
-#endif
-
-#endif /* __RTS51X_DEBUG_H */
diff --git a/drivers/staging/rts5139/ms.c b/drivers/staging/rts5139/ms.c
deleted file mode 100644
index 390292ad456..00000000000
--- a/drivers/staging/rts5139/ms.c
+++ /dev/null
@@ -1,4185 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#include <linux/blkdev.h>
-#include <linux/kthread.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
-#include <linux/vmalloc.h>
-
-#include "debug.h"
-#include "trace.h"
-#include "rts51x.h"
-#include "rts51x_transport.h"
-#include "rts51x_scsi.h"
-#include "rts51x_card.h"
-#include "ms.h"
-
-static inline void ms_set_err_code(struct rts51x_chip *chip, u8 err_code)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-
-	ms_card->err_code = err_code;
-}
-
-static inline int ms_check_err_code(struct rts51x_chip *chip, u8 err_code)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-
-	return ms_card->err_code == err_code;
-}
-
-static int ms_parse_err_code(struct rts51x_chip *chip)
-{
-	TRACE_RET(chip, STATUS_FAIL);
-}
-
-static int ms_transfer_tpc(struct rts51x_chip *chip, u8 trans_mode, u8 tpc,
-			   u8 cnt, u8 cfg)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-
-	RTS51X_DEBUGP("ms_transfer_tpc: tpc = 0x%x\n", tpc);
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_BYTE_CNT, 0xFF, cnt);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-		       PINGPONG_BUFFER);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
-		       MS_TRANSFER_START | trans_mode);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER, MS_TRANSFER_END,
-		       MS_TRANSFER_END);
-
-	rts51x_add_cmd(chip, READ_REG_CMD, MS_TRANS_CFG, 0, 0);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, 2, 5000);
-	if (CHECK_MS_TRANS_FAIL(chip, retval)) {
-		rts51x_clear_ms_error(chip);
-		ms_set_err_code(chip, MS_TO_ERROR);
-		TRACE_RET(chip, ms_parse_err_code(chip));
-	}
-
-	if (!(tpc & 0x08)) {	/* Read Packet */
-		/* Check CRC16 & Ready Timeout */
-		if (chip->rsp_buf[1] & MS_CRC16_ERR) {
-			ms_set_err_code(chip, MS_CRC16_ERROR);
-			TRACE_RET(chip, ms_parse_err_code(chip));
-		}
-	} else { /* Write Packet */
-		if (CHK_MSPRO(ms_card) && !(chip->rsp_buf[1] & 0x80)) {
-			if (chip->rsp_buf[1] & (MS_INT_ERR | MS_INT_CMDNK)) {
-				ms_set_err_code(chip, MS_CMD_NK);
-				TRACE_RET(chip, ms_parse_err_code(chip));
-			}
-		}
-	}
-
-	/* Check Timeout of Ready Signal */
-	if (chip->rsp_buf[1] & MS_RDY_TIMEOUT) {
-		rts51x_clear_ms_error(chip);
-		ms_set_err_code(chip, MS_TO_ERROR);
-		TRACE_RET(chip, ms_parse_err_code(chip));
-	}
-
-	return STATUS_SUCCESS;
-}
-
-int ms_transfer_data(struct rts51x_chip *chip, u8 trans_mode, u8 tpc,
-		     u16 sec_cnt, u8 cfg, int mode_2k, int use_sg, void *buf,
-		     int buf_len)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-	u8 val, err_code = 0, flag = 0;
-	enum dma_data_direction dir;
-	unsigned int pipe;
-
-	if (!buf || !buf_len)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	if (trans_mode == MS_TM_AUTO_READ) {
-		pipe = RCV_BULK_PIPE(chip);
-		dir = DMA_FROM_DEVICE;
-		flag = MODE_CDIR;
-		err_code = MS_FLASH_READ_ERROR;
-	} else if (trans_mode == MS_TM_AUTO_WRITE) {
-		pipe = SND_BULK_PIPE(chip);
-		dir = DMA_TO_DEVICE;
-		flag = MODE_CDOR;
-		err_code = MS_FLASH_WRITE_ERROR;
-	} else {
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_SECTOR_CNT_H, 0xFF,
-		       (u8) (sec_cnt >> 8));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_SECTOR_CNT_L, 0xFF,
-		       (u8) sec_cnt);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-		       RING_BUFFER);
-
-	if (mode_2k)
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_CFG, MS_2K_SECTOR_MODE,
-			       MS_2K_SECTOR_MODE);
-	else
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_CFG, MS_2K_SECTOR_MODE,
-			       0);
-
-	rts51x_trans_dma_enable(dir, chip, sec_cnt * 512, DMA_512);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
-		       MS_TRANSFER_START | trans_mode);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER, MS_TRANSFER_END,
-		       MS_TRANSFER_END);
-
-	retval = rts51x_send_cmd(chip, flag | STAGE_MS_STATUS, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval =
-	    rts51x_transfer_data_rcc(chip, pipe, buf, buf_len, use_sg, NULL,
-				     15000, flag);
-	if (retval != STATUS_SUCCESS) {
-		ms_set_err_code(chip, err_code);
-		rts51x_clear_ms_error(chip);
-		TRACE_RET(chip, retval);
-	}
-
-	retval = rts51x_get_rsp(chip, 3, 15000);
-	if (CHECK_MS_TRANS_FAIL(chip, retval)) {
-		ms_set_err_code(chip, err_code);
-		rts51x_clear_ms_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	ms_card->last_rw_int = val = chip->rsp_buf[1];
-	if (val & (MS_INT_CMDNK | MS_INT_ERR | MS_CRC16_ERR | MS_RDY_TIMEOUT))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	return STATUS_SUCCESS;
-}
-
-int ms_write_bytes(struct rts51x_chip *chip, u8 tpc, u8 cnt, u8 cfg, u8 *data,
-		   int data_len)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, i;
-
-	if (!data || (data_len < cnt))
-		TRACE_RET(chip, STATUS_ERROR);
-
-	rts51x_init_cmd(chip);
-
-	for (i = 0; i < cnt; i++) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, PPBUF_BASE2 + i, 0xFF,
-			       data[i]);
-	}
-	if (cnt % 2)
-		rts51x_add_cmd(chip, WRITE_REG_CMD, PPBUF_BASE2 + i,
-				0xFF, 0xFF);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_BYTE_CNT, 0xFF, cnt);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-		       PINGPONG_BUFFER);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
-		       MS_TRANSFER_START | MS_TM_WRITE_BYTES);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER, MS_TRANSFER_END,
-		       MS_TRANSFER_END);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, 1, 5000);
-	if (CHECK_MS_TRANS_FAIL(chip, retval)) {
-		u8 val = 0;
-
-		rts51x_ep0_read_register(chip, MS_TRANS_CFG, &val);
-		RTS51X_DEBUGP("MS_TRANS_CFG: 0x%02x\n", val);
-
-		rts51x_clear_ms_error(chip);
-
-		if (!(tpc & 0x08)) { /* Read Packet */
-			/* Check CRC16 & Ready Timeout */
-			if (val & MS_CRC16_ERR) {
-				ms_set_err_code(chip, MS_CRC16_ERROR);
-				TRACE_RET(chip, ms_parse_err_code(chip));
-			}
-		} else { /* Write Packet */
-			if (CHK_MSPRO(ms_card) && !(val & 0x80)) {
-				if (val & (MS_INT_ERR | MS_INT_CMDNK)) {
-					ms_set_err_code(chip, MS_CMD_NK);
-					TRACE_RET(chip,
-						  ms_parse_err_code(chip));
-				}
-			}
-		}
-
-		/* Check Timeout of Ready Signal */
-		if (val & MS_RDY_TIMEOUT) {
-			ms_set_err_code(chip, MS_TO_ERROR);
-			TRACE_RET(chip, ms_parse_err_code(chip));
-		}
-
-		ms_set_err_code(chip, MS_TO_ERROR);
-		TRACE_RET(chip, ms_parse_err_code(chip));
-	}
-
-	return STATUS_SUCCESS;
-}
-
-int ms_read_bytes(struct rts51x_chip *chip, u8 tpc, u8 cnt, u8 cfg, u8 *data,
-		  int data_len)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, i;
-
-	if (!data)
-		TRACE_RET(chip, STATUS_ERROR);
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_BYTE_CNT, 0xFF, cnt);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-		       PINGPONG_BUFFER);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
-		       MS_TRANSFER_START | MS_TM_READ_BYTES);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER, MS_TRANSFER_END,
-		       MS_TRANSFER_END);
-
-	for (i = 0; i < data_len - 1; i++)
-		rts51x_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + i, 0, 0);
-
-	if (data_len % 2)
-		rts51x_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + data_len, 0,
-			       0);
-	else
-		rts51x_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + data_len - 1,
-			       0, 0);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, data_len + 1, 5000);
-	if (CHECK_MS_TRANS_FAIL(chip, retval)) {
-		u8 val = 0;
-
-		rts51x_ep0_read_register(chip, MS_TRANS_CFG, &val);
-		RTS51X_DEBUGP("MS_TRANS_CFG: 0x%02x\n", val);
-
-		rts51x_clear_ms_error(chip);
-
-		if (!(tpc & 0x08)) { /* Read Packet */
-			/* Check CRC16 & Ready Timeout */
-			if (val & MS_CRC16_ERR) {
-				ms_set_err_code(chip, MS_CRC16_ERROR);
-				TRACE_RET(chip, ms_parse_err_code(chip));
-			}
-		} else { /* Write Packet */
-			if (CHK_MSPRO(ms_card) && !(val & 0x80)) {
-				if (val & (MS_INT_ERR | MS_INT_CMDNK)) {
-					ms_set_err_code(chip, MS_CMD_NK);
-					TRACE_RET(chip,
-						  ms_parse_err_code(chip));
-				}
-			}
-		}
-
-		/* Check Timeout of Ready Signal */
-		if (val & MS_RDY_TIMEOUT) {
-			ms_set_err_code(chip, MS_TO_ERROR);
-			TRACE_RET(chip, ms_parse_err_code(chip));
-		}
-
-		ms_set_err_code(chip, MS_TO_ERROR);
-		TRACE_RET(chip, ms_parse_err_code(chip));
-	}
-
-	rts51x_read_rsp_buf(chip, 1, data, data_len);
-
-	return STATUS_SUCCESS;
-}
-
-int ms_set_rw_reg_addr(struct rts51x_chip *chip,
-		       u8 read_start, u8 read_cnt, u8 write_start, u8 write_cnt)
-{
-	int retval, i;
-	u8 data[4];
-
-	data[0] = read_start;
-	data[1] = read_cnt;
-	data[2] = write_start;
-	data[3] = write_cnt;
-
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval =
-		    ms_write_bytes(chip, SET_RW_REG_ADRS, 4, NO_WAIT_INT, data,
-				   4);
-		if (retval == STATUS_SUCCESS)
-			return STATUS_SUCCESS;
-		rts51x_clear_ms_error(chip);
-	}
-
-	TRACE_RET(chip, STATUS_FAIL);
-}
-
-static int ms_send_cmd(struct rts51x_chip *chip, u8 cmd, u8 cfg)
-{
-	u8 data[2];
-
-	data[0] = cmd;
-	data[1] = 0;
-
-	return ms_write_bytes(chip, PRO_SET_CMD, 1, cfg, data, 1);
-}
-
-static int ms_set_cmd(struct rts51x_chip *chip,
-		      u8 read_start, u8 read_count,
-		      u8 write_start, u8 write_count,
-		      u8 cmd, u8 cfg, u8 *data, int data_len, u8 *int_stat)
-{
-	int retval, i;
-	u8 val;
-
-	if (!data || (data_len <= 0) || (data_len > 128)) {
-		RTS51X_DEBUGP("ms_set_cmd (data_len = %d)\n", data_len);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	retval =
-	    ms_set_rw_reg_addr(chip, read_start, read_count, write_start,
-			       write_count);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval =
-		    ms_write_bytes(chip, WRITE_REG, write_count, NO_WAIT_INT,
-				   data, data_len);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	ms_set_err_code(chip, MS_NO_ERROR);
-
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval = ms_send_cmd(chip, cmd, WAIT_INT);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
-	/* GET_INT Register */
-	ms_set_err_code(chip, MS_NO_ERROR);
-	retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (int_stat)
-		*int_stat = val;
-
-	return STATUS_SUCCESS;
-}
-
-#ifdef MS_SPEEDUP
-static int ms_auto_set_cmd(struct rts51x_chip *chip,
-			   u8 read_start, u8 read_count,
-			   u8 write_start, u8 write_count,
-			   u8 cmd, u8 cfg, u8 *data, int data_len,
-			   u8 *int_stat)
-{
-	int retval;
-	int i;
-
-	if (!data || (data_len <= 0) || (data_len > 128)) {
-		RTS51X_DEBUGP("ms_auto_set_cmd (data_len = %d)\n", data_len);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_READ_START, 0xFF, read_start);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_READ_COUNT, 0xFF, read_count);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_WRITE_START, 0xFF, write_start);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_WRITE_COUNT, 0xFF, write_count);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_COMMAND, 0xFF, cmd);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-		       PINGPONG_BUFFER);
-
-	for (i = 0; i < data_len; i++) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, PPBUF_BASE2 + i, 0xFF,
-			       data[i]);
-	}
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
-		       MS_TRANSFER_START | MS_TM_SET_CMD);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER, MS_TRANSFER_END,
-		       MS_TRANSFER_END);
-
-	retval = rts51x_send_cmd(chip, MODE_CR | STAGE_MS_STATUS, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, 3, 5000);
-
-	if (CHECK_MS_TRANS_FAIL(chip, retval)) {
-		rts51x_clear_ms_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if (int_stat)
-		*int_stat = chip->rsp_buf[2];
-
-	return STATUS_SUCCESS;
-}
-#endif
-
-static int ms_set_init_para(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-
-	if (CHK_HG8BIT(ms_card)) {
-		if (chip->asic_code)
-			ms_card->ms_clock = chip->option.asic_ms_hg_clk;
-		else
-			ms_card->ms_clock = chip->option.fpga_ms_hg_clk;
-	} else if (CHK_MSPRO(ms_card) || CHK_MS4BIT(ms_card)) {
-		if (chip->asic_code)
-			ms_card->ms_clock = chip->option.asic_ms_4bit_clk;
-		else
-			ms_card->ms_clock = chip->option.fpga_ms_4bit_clk;
-	} else {
-		if (chip->asic_code)
-			ms_card->ms_clock = 38;
-		else
-			ms_card->ms_clock = CLK_40;
-	}
-
-	retval = switch_clock(chip, ms_card->ms_clock);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_select_card(chip, MS_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-int ms_switch_clock(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-
-	retval = rts51x_select_card(chip, MS_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = switch_clock(chip, ms_card->ms_clock);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-static void ms_pull_ctl_disable(struct rts51x_chip *chip)
-{
-	if (CHECK_PKG(chip, LQFP48)) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0xA5);
-	} else {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0x65);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x56);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0x59);
-	}
-}
-
-static void ms_pull_ctl_enable(struct rts51x_chip *chip)
-{
-	if (CHECK_PKG(chip, LQFP48)) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0xA5);
-	} else {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0x65);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0x59);
-	}
-}
-
-static int ms_prepare_reset(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-
-	ms_card->ms_type = 0;
-	ms_card->check_ms_flow = 0;
-	ms_card->switch_8bit_fail = 0;
-	ms_card->delay_write.delay_write_flag = 0;
-
-	ms_card->pro_under_formatting = 0;
-
-	rts51x_init_cmd(chip);
-
-	if (chip->asic_code) {
-		ms_pull_ctl_enable(chip);
-	} else {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, FPGA_PULL_CTL,
-			       FPGA_MS_PULL_CTL_BIT | 0x20, 0);
-	}
-	/* Tri-state MS output */
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_OE, MS_OUTPUT_EN, 0);
-
-	if (!chip->option.FT2_fast_mode) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, POWER_MASK,
-			       POWER_OFF);
-	}
-
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (!chip->option.FT2_fast_mode) {
-		wait_timeout(250);
-
-		rts51x_card_power_on(chip, MS_CARD);
-		wait_timeout(150);
-
-#ifdef SUPPORT_OCP
-		rts51x_get_card_status(chip, &(chip->card_status));
-		/* get OCP status */
-		chip->ocp_stat = (chip->card_status >> 4) & 0x03;
-
-		if (chip->ocp_stat & (MS_OCP_NOW | MS_OCP_EVER)) {
-			RTS51X_DEBUGP("Over current, OCPSTAT is 0x%x\n",
-				       chip->ocp_stat);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-#endif
-	}
-
-	rts51x_init_cmd(chip);
-
-	/* Enable MS Output */
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_OE, MS_OUTPUT_EN,
-		       MS_OUTPUT_EN);
-
-	/* Reset Registers */
-	if (chip->asic_code)
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_CFG, 0xFF,
-			       SAMPLE_TIME_RISING | PUSH_TIME_DEFAULT |
-			       NO_EXTEND_TOGGLE | MS_BUS_WIDTH_1);
-	else
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_CFG, 0xFF,
-			       SAMPLE_TIME_FALLING | PUSH_TIME_DEFAULT |
-			       NO_EXTEND_TOGGLE | MS_BUS_WIDTH_1);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF,
-		       NO_WAIT_INT | NO_AUTO_READ_INT_REG);
-
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return ms_set_init_para(chip);
-}
-
-static int ms_identify_media_type(struct rts51x_chip *chip, int switch_8bit_bus)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, i;
-	u8 val;
-
-	retval = ms_set_rw_reg_addr(chip, Pro_StatusReg, 6, SystemParm, 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	/* Get Register form MS-PRO card */
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval =
-		    ms_transfer_tpc(chip, MS_TM_READ_BYTES, READ_REG, 6,
-				    NO_WAIT_INT);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	RTS51X_READ_REG(chip, PPBUF_BASE2 + 2, &val);
-	RTS51X_DEBUGP("Type register: 0x%x\n", val);
-	if (val != 0x01) {
-		if (val != 0x02)
-			ms_card->check_ms_flow = 1;
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-	/* Category Register */
-	RTS51X_READ_REG(chip, PPBUF_BASE2 + 4, &val);
-	RTS51X_DEBUGP("Category register: 0x%x\n", val);
-	if (val != 0) {
-		ms_card->check_ms_flow = 1;
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-	/* Class Register */
-	RTS51X_READ_REG(chip, PPBUF_BASE2 + 5, &val);
-	RTS51X_DEBUGP("Class register: 0x%x\n", val);
-	if (val == 0) {
-		RTS51X_READ_REG(chip, PPBUF_BASE2, &val);
-		if (val & WRT_PRTCT)
-			chip->card_wp |= MS_CARD;
-		else
-			chip->card_wp &= ~MS_CARD;
-	} else if ((val == 0x01) || (val == 0x02) || (val == 0x03)) {
-		chip->card_wp |= MS_CARD;
-	} else {
-		ms_card->check_ms_flow = 1;
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	ms_card->ms_type |= TYPE_MSPRO;
-
-	/* Check MSPro-HG Card, use IF Mode Register to distinguish */
-	RTS51X_READ_REG(chip, PPBUF_BASE2 + 3, &val);
-	RTS51X_DEBUGP("IF Mode register: 0x%x\n", val);
-	if (val == 0) {
-		ms_card->ms_type &= 0x0F;
-	} else if (val == 7) {
-		if (switch_8bit_bus)
-			ms_card->ms_type |= MS_HG;
-		else
-			ms_card->ms_type &= 0x0F;
-	} else {
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	/* end Procedure to identify Media Type */
-	return STATUS_SUCCESS;
-}
-
-static int ms_confirm_cpu_startup(struct rts51x_chip *chip)
-{
-	int retval, i, k;
-	u8 val;
-
-	/* Confirm CPU StartUp */
-	k = 0;
-	do {
-		if (monitor_card_cd(chip, MS_CARD) == CD_NOT_EXIST)
-			TRACE_RET(chip, STATUS_FAIL);
-
-		for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-			retval =
-			    ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val,
-					  1);
-			if (retval == STATUS_SUCCESS)
-				break;
-		}
-		if (i == MS_MAX_RETRY_COUNT)
-			TRACE_RET(chip, STATUS_FAIL);
-
-		if (k > 100)
-			TRACE_RET(chip, STATUS_FAIL);
-		k++;
-		wait_timeout(100);
-	} while (!(val & INT_REG_CED));
-
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	if (val & INT_REG_ERR) {
-		if (val & INT_REG_CMDNK) {	/* CMDNK = 1 */
-			chip->card_wp |= (MS_CARD);
-		} else {	/* CMDNK = 0 */
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	}
-	/*--  end confirm CPU startup */
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_switch_parallel_bus(struct rts51x_chip *chip)
-{
-	int retval, i;
-	u8 data[2];
-
-	data[0] = PARALLEL_4BIT_IF;
-	data[1] = 0;
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval =
-		    ms_write_bytes(chip, WRITE_REG, 1, NO_WAIT_INT, data, 2);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_switch_8bit_bus(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, i;
-	u8 data[2];
-
-	data[0] = PARALLEL_8BIT_IF;
-	data[1] = 0;
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval =
-		    ms_write_bytes(chip, WRITE_REG, 1, NO_WAIT_INT, data, 2);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	RTS51X_WRITE_REG(chip, MS_CFG, 0x98,
-			 MS_BUS_WIDTH_8 | SAMPLE_TIME_FALLING);
-	ms_card->ms_type |= MS_8BIT;
-
-	retval = ms_set_init_para(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval =
-		    ms_transfer_tpc(chip, MS_TM_READ_BYTES, GET_INT, 1,
-				    NO_WAIT_INT);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_pro_reset_flow(struct rts51x_chip *chip, int switch_8bit_bus)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, i;
-
-	for (i = 0; i < 3; i++) {
-		retval = ms_prepare_reset(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		retval = ms_identify_media_type(chip, switch_8bit_bus);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		retval = ms_confirm_cpu_startup(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		retval = ms_switch_parallel_bus(chip);
-		if (retval != STATUS_SUCCESS) {
-			if (monitor_card_cd(chip, MS_CARD) == CD_NOT_EXIST)
-				TRACE_RET(chip, STATUS_FAIL);
-			continue;
-		} else {
-			break;
-		}
-	}
-
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	RTS51X_WRITE_REG(chip, MS_CFG, 0x18, MS_BUS_WIDTH_4);
-
-	RTS51X_WRITE_REG(chip, MS_CFG, PUSH_TIME_ODD, PUSH_TIME_ODD);
-
-	retval = ms_set_init_para(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (CHK_MSHG(ms_card) && switch_8bit_bus) {
-		retval = ms_switch_8bit_bus(chip);
-		if (retval != STATUS_SUCCESS) {
-			ms_card->switch_8bit_fail = 1;
-			TRACE_RET(chip, retval);
-		}
-	}
-
-	return STATUS_SUCCESS;
-}
-
-#ifdef XC_POWERCLASS
-static int msxc_change_power(struct rts51x_chip *chip, u8 mode)
-{
-	int retval;
-	u8 buf[6];
-
-	rts51x_ms_cleanup_work(chip);
-
-	/* Set Parameter Register */
-	retval = ms_set_rw_reg_addr(chip, 0, 0, Pro_DataCount1, 6);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	buf[0] = 0;
-	buf[1] = mode;
-	buf[2] = 0;
-	buf[3] = 0;
-	buf[4] = 0;
-	buf[5] = 0;
-
-	retval = ms_write_bytes(chip, PRO_WRITE_REG, 6, NO_WAIT_INT, buf, 6);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = ms_send_cmd(chip, XC_CHG_POWER, WAIT_INT);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	RTS51X_READ_REG(chip, MS_TRANS_CFG, buf);
-	if (buf[0] & (MS_INT_CMDNK | MS_INT_ERR))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	return STATUS_SUCCESS;
-}
-#endif
-
-static int ms_read_attribute_info(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, i;
-	u8 val, *buf, class_code, device_type, sub_class, data[16];
-	u16 total_blk = 0, blk_size = 0;
-#ifdef SUPPORT_MSXC
-	u32 xc_total_blk = 0, xc_blk_size = 0;
-#endif
-	u32 sys_info_addr = 0, sys_info_size;
-#ifdef SUPPORT_PCGL_1P18
-	u32 model_name_addr = 0, model_name_size;
-	int found_sys_info = 0, found_model_name = 0;
-#endif
-
-	retval = ms_set_rw_reg_addr(chip, Pro_IntReg, 2, Pro_SystemParm, 7);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (CHK_MS8BIT(ms_card))
-		data[0] = PARALLEL_8BIT_IF;
-	else
-		data[0] = PARALLEL_4BIT_IF;
-	data[1] = 0;
-
-	data[2] = 0x40;
-	data[3] = 0;
-	data[4] = 0;
-	data[5] = 0;
-	/* Start address 0 */
-	data[6] = 0;
-	data[7] = 0;
-
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval =
-		    ms_write_bytes(chip, PRO_WRITE_REG, 7, NO_WAIT_INT, data,
-				   8);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	buf = kmalloc(64 * 512, GFP_KERNEL);
-	if (buf == NULL)
-		TRACE_RET(chip, STATUS_NOMEM);
-
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval = ms_send_cmd(chip, PRO_READ_ATRB, WAIT_INT);
-		if (retval != STATUS_SUCCESS)
-			continue;
-
-		retval = rts51x_read_register(chip, MS_TRANS_CFG, &val);
-		if (retval != STATUS_SUCCESS) {
-			kfree(buf);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-		if (!(val & MS_INT_BREQ)) {
-			kfree(buf);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		retval =
-		    ms_transfer_data(chip, MS_TM_AUTO_READ, PRO_READ_LONG_DATA,
-				     0x40, WAIT_INT, 0, 0, buf, 64 * 512);
-		if (retval == STATUS_SUCCESS)
-			break;
-		else
-			rts51x_clear_ms_error(chip);
-	}
-	if (retval != STATUS_SUCCESS) {
-		kfree(buf);
-		TRACE_RET(chip, retval);
-	}
-
-	i = 0;
-	do {
-		retval = rts51x_read_register(chip, MS_TRANS_CFG, &val);
-		if (retval != STATUS_SUCCESS) {
-			kfree(buf);
-			TRACE_RET(chip, retval);
-		}
-
-		if ((val & MS_INT_CED) || !(val & MS_INT_BREQ))
-			break;
-
-		retval =
-		    ms_transfer_tpc(chip, MS_TM_NORMAL_READ, PRO_READ_LONG_DATA,
-				    0, WAIT_INT);
-		if (retval != STATUS_SUCCESS) {
-			kfree(buf);
-			TRACE_RET(chip, retval);
-		}
-
-		i++;
-	} while (i < 1024);
-
-	if (retval != STATUS_SUCCESS) {
-		kfree(buf);
-		TRACE_RET(chip, retval);
-	}
-
-	if ((buf[0] != 0xa5) && (buf[1] != 0xc3)) {
-		/* Signature code is wrong */
-		kfree(buf);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if ((buf[4] < 1) || (buf[4] > 12)) {
-		kfree(buf);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	for (i = 0; i < buf[4]; i++) {
-		int cur_addr_off = 16 + i * 12;
-
-#ifdef SUPPORT_MSXC
-		if ((buf[cur_addr_off + 8] == 0x10)
-		    || (buf[cur_addr_off + 8] == 0x13)) {
-#else
-		if (buf[cur_addr_off + 8] == 0x10) {
-#endif
-			sys_info_addr = ((u32) buf[cur_addr_off + 0] << 24) |
-			    ((u32) buf[cur_addr_off + 1] << 16) |
-			    ((u32) buf[cur_addr_off + 2] << 8) |
-			    buf[cur_addr_off + 3];
-			sys_info_size =
-			    ((u32) buf[cur_addr_off + 4] << 24) |
-			    ((u32) buf[cur_addr_off + 5] << 16) |
-			    ((u32) buf[cur_addr_off + 6] << 8) |
-			    buf[cur_addr_off + 7];
-			RTS51X_DEBUGP("sys_info_addr = 0x%x, sys_info_size = 0x%x\n",
-						sys_info_addr, sys_info_size);
-			if (sys_info_size != 96) {
-				kfree(buf);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-			if (sys_info_addr < 0x1A0) {
-				kfree(buf);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-			if ((sys_info_size + sys_info_addr) > 0x8000) {
-				kfree(buf);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-#ifdef SUPPORT_MSXC
-			if (buf[cur_addr_off + 8] == 0x13)
-				ms_card->ms_type |= MS_XC;
-#endif
-#ifdef SUPPORT_PCGL_1P18
-			found_sys_info = 1;
-#else
-			break;
-#endif
-		}
-#ifdef SUPPORT_PCGL_1P18
-		if (buf[cur_addr_off + 8] == 0x15) {
-			model_name_addr = ((u32) buf[cur_addr_off + 0] << 24) |
-			    ((u32) buf[cur_addr_off + 1] << 16) |
-			    ((u32) buf[cur_addr_off + 2] << 8) |
-			    buf[cur_addr_off + 3];
-			model_name_size =
-			    ((u32) buf[cur_addr_off + 4] << 24) |
-			    ((u32) buf[cur_addr_off + 5] << 16) |
-			    ((u32) buf[cur_addr_off + 6] << 8) |
-			    buf[cur_addr_off + 7];
-			RTS51X_DEBUGP("model_name_addr = 0x%x, model_name_size = 0x%x\n",
-					model_name_addr, model_name_size);
-			if (model_name_size != 48) {
-				kfree(buf);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-			if (model_name_addr < 0x1A0) {
-				kfree(buf);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-			if ((model_name_size + model_name_addr) > 0x8000) {
-				kfree(buf);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-
-			found_model_name = 1;
-		}
-
-		if (found_sys_info && found_model_name)
-			break;
-#endif
-	}
-
-	if (i == buf[4]) {
-		kfree(buf);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	class_code = buf[sys_info_addr + 0];
-	device_type = buf[sys_info_addr + 56];
-	sub_class = buf[sys_info_addr + 46];
-#ifdef SUPPORT_MSXC
-	if (CHK_MSXC(ms_card)) {
-		xc_total_blk = ((u32) buf[sys_info_addr + 6] << 24) |
-		    ((u32) buf[sys_info_addr + 7] << 16) |
-		    ((u32) buf[sys_info_addr + 8] << 8) |
-		    buf[sys_info_addr + 9];
-		xc_blk_size = ((u32) buf[sys_info_addr + 32] << 24) |
-		    ((u32) buf[sys_info_addr + 33] << 16) |
-		    ((u32) buf[sys_info_addr + 34] << 8) |
-		    buf[sys_info_addr + 35];
-		RTS51X_DEBUGP("xc_total_blk = 0x%x, xc_blk_size = 0x%x\n",
-			       xc_total_blk, xc_blk_size);
-	} else {
-		total_blk =
-		    ((u16) buf[sys_info_addr + 6] << 8) | buf[sys_info_addr +
-							      7];
-		blk_size =
-		    ((u16) buf[sys_info_addr + 2] << 8) | buf[sys_info_addr +
-							      3];
-		RTS51X_DEBUGP("total_blk = 0x%x, blk_size = 0x%x\n", total_blk,
-			       blk_size);
-	}
-#else
-	total_blk =
-	    ((u16) buf[sys_info_addr + 6] << 8) | buf[sys_info_addr + 7];
-	blk_size = ((u16) buf[sys_info_addr + 2] << 8) | buf[sys_info_addr + 3];
-	RTS51X_DEBUGP("total_blk = 0x%x, blk_size = 0x%x\n", total_blk,
-		       blk_size);
-#endif
-
-	RTS51X_DEBUGP("class_code = 0x%x, device_type = 0x%x,"
-			"sub_class = 0x%x\n",
-				class_code, device_type, sub_class);
-
-	memcpy(ms_card->raw_sys_info, buf + sys_info_addr, 96);
-#ifdef SUPPORT_PCGL_1P18
-	memcpy(ms_card->raw_model_name, buf + model_name_addr, 48);
-#endif
-
-	kfree(buf);
-
-	/* Confirm System Information */
-#ifdef SUPPORT_MSXC
-	if (CHK_MSXC(ms_card)) {
-		if (class_code != 0x03)
-			TRACE_RET(chip, STATUS_FAIL);
-	} else {
-		if (class_code != 0x02)
-			TRACE_RET(chip, STATUS_FAIL);
-	}
-#else
-	if (class_code != 0x02)
-		TRACE_RET(chip, STATUS_FAIL);
-#endif
-
-	if (device_type != 0x00) {
-		if ((device_type == 0x01) || (device_type == 0x02)
-		    || (device_type == 0x03))
-			chip->card_wp |= MS_CARD;
-		else
-			TRACE_RET(chip, STATUS_FAIL);
-	}
-	if (sub_class & 0xC0)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	RTS51X_DEBUGP("class_code: 0x%x, device_type: 0x%x, sub_class: 0x%x\n",
-		       class_code, device_type, sub_class);
-
-#ifdef SUPPORT_MSXC
-	if (CHK_MSXC(ms_card)) {
-		chip->capacity[chip->card2lun[MS_CARD]] = ms_card->capacity =
-		    xc_total_blk * xc_blk_size;
-	} else {
-		chip->capacity[chip->card2lun[MS_CARD]] = ms_card->capacity =
-		    total_blk * blk_size;
-	}
-#else
-	chip->capacity[chip->card2lun[MS_CARD]] = ms_card->capacity =
-	    total_blk * blk_size;
-#endif
-
-	return STATUS_SUCCESS;
-}
-
-#ifdef SUPPORT_MAGIC_GATE
-int mg_set_tpc_para_sub(struct rts51x_chip *chip, int type, u8 mg_entry_num);
-#endif
-
-static int reset_ms_pro(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-#ifdef XC_POWERCLASS
-	u8 change_power_class = 2;
-#endif
-
-#ifdef XC_POWERCLASS
-Retry:
-#endif
-	retval = ms_pro_reset_flow(chip, 1);
-	if (retval != STATUS_SUCCESS) {
-		if (ms_card->switch_8bit_fail) {
-			retval = ms_pro_reset_flow(chip, 0);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-		} else {
-			TRACE_RET(chip, retval);
-		}
-	}
-
-	retval = ms_read_attribute_info(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-#ifdef XC_POWERCLASS
-	if (CHK_HG8BIT(ms_card))
-		change_power_class = 0;
-
-	if (change_power_class && CHK_MSXC(ms_card)) {
-		u8 power_class_mode = (ms_card->raw_sys_info[46] & 0x18) >> 3;
-		RTS51X_DEBUGP("power_class_mode = 0x%x", power_class_mode);
-		if (change_power_class > power_class_mode)
-			change_power_class = power_class_mode;
-		if (change_power_class) {
-			retval = msxc_change_power(chip, change_power_class);
-			if (retval != STATUS_SUCCESS) {
-				change_power_class--;
-				goto Retry;
-			}
-		}
-	}
-#endif
-
-#ifdef SUPPORT_MAGIC_GATE
-	retval = mg_set_tpc_para_sub(chip, 0, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-#endif
-
-	if (CHK_HG8BIT(ms_card))
-		chip->card_bus_width[chip->card2lun[MS_CARD]] = 8;
-	else
-		chip->card_bus_width[chip->card2lun[MS_CARD]] = 4;
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_read_status_reg(struct rts51x_chip *chip)
-{
-	int retval;
-	u8 val[2];
-
-	retval = ms_set_rw_reg_addr(chip, StatusReg0, 2, 0, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = ms_read_bytes(chip, READ_REG, 2, NO_WAIT_INT, val, 2);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	if (val[1] & (STS_UCDT | STS_UCEX | STS_UCFG)) {
-		ms_set_err_code(chip, MS_FLASH_READ_ERROR);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_check_boot_block(struct rts51x_chip *chip, u16 block_addr)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-	u8 extra[MS_EXTRA_SIZE], data[10], val = 0;
-
-	if (CHK_MS4BIT(ms_card)) {
-		/* Parallel interface */
-		data[0] = 0x88;
-	} else {
-		/* Serial interface */
-		data[0] = 0x80;
-	}
-	/* Block Address */
-	data[1] = 0;
-	data[2] = (u8) (block_addr >> 8);
-	data[3] = (u8) block_addr;
-	/* Page Number
-	 * Extra data access mode */
-	data[4] = 0x40;
-	data[5] = 0;
-
-	retval = ms_set_cmd(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm, 6,
-			    BLOCK_READ, WAIT_INT, data, 6, &val);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (val & INT_REG_CMDNK) {
-		ms_set_err_code(chip, MS_CMD_NK);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-	if (val & INT_REG_CED) {
-		if (val & INT_REG_ERR) {
-			retval = ms_read_status_reg(chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-			retval =
-			    ms_set_rw_reg_addr(chip, OverwriteFlag,
-					       MS_EXTRA_SIZE, SystemParm, 6);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-		}
-	}
-
-	retval =
-	    ms_read_bytes(chip, READ_REG, MS_EXTRA_SIZE, NO_WAIT_INT, extra,
-			  MS_EXTRA_SIZE);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (!(extra[0] & BLOCK_OK) || (extra[1] & NOT_BOOT_BLOCK))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_read_extra_data(struct rts51x_chip *chip,
-			      u16 block_addr, u8 page_num, u8 *buf,
-			      int buf_len)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-	u8 val = 0, data[10];
-
-	if (CHK_MS4BIT(ms_card)) {
-		/* Parallel interface */
-		data[0] = 0x88;
-	} else {
-		/* Serial interface */
-		data[0] = 0x80;
-	}
-	/* Block Address */
-	data[1] = 0;
-	data[2] = (u8) (block_addr >> 8);
-	data[3] = (u8) block_addr;
-	/* Page Number
-	 * Extra data access mode */
-	data[4] = 0x40;
-	data[5] = page_num;
-
-#ifdef MS_SPEEDUP
-	retval =
-	    ms_auto_set_cmd(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm, 6,
-			    BLOCK_READ, WAIT_INT, data, 6, &val);
-#else
-	retval = ms_set_cmd(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm, 6,
-			    BLOCK_READ, WAIT_INT, data, 6, &val);
-#endif
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (val & INT_REG_CMDNK) {
-		ms_set_err_code(chip, MS_CMD_NK);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-	if (val & INT_REG_CED) {
-		if (val & INT_REG_ERR) {
-			retval = ms_read_status_reg(chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-			retval =
-			    ms_set_rw_reg_addr(chip, OverwriteFlag,
-					       MS_EXTRA_SIZE, SystemParm, 6);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-		}
-	}
-
-	retval =
-	    ms_read_bytes(chip, READ_REG, MS_EXTRA_SIZE, NO_WAIT_INT, data,
-			  MS_EXTRA_SIZE);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (buf && buf_len) {
-		if (buf_len > MS_EXTRA_SIZE)
-			buf_len = MS_EXTRA_SIZE;
-		memcpy(buf, data, buf_len);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_write_extra_data(struct rts51x_chip *chip,
-			       u16 block_addr, u8 page_num, u8 *buf,
-			       int buf_len)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, i;
-	u8 val = 0, data[16];
-
-	if (!buf || (buf_len < MS_EXTRA_SIZE))
-		TRACE_RET(chip, STATUS_FAIL);
-	/* Write REG */
-	if (CHK_MS4BIT(ms_card)) {
-		/* Parallel interface */
-		data[0] = 0x88;
-	} else {
-		/* Serial interface */
-		data[0] = 0x80;
-	}
-	/* Block Address */
-	data[1] = 0;
-	data[2] = (u8) (block_addr >> 8);
-	data[3] = (u8) block_addr;
-	/* Page Number
-	 * Extra data access mode */
-	data[4] = 0x40;
-	data[5] = page_num;
-
-	for (i = 6; i < MS_EXTRA_SIZE + 6; i++)
-		data[i] = buf[i - 6];
-
-#ifdef MS_SPEEDUP
-	retval =
-	    ms_auto_set_cmd(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm,
-			    6 + MS_EXTRA_SIZE, BLOCK_WRITE, WAIT_INT, data, 16,
-			    &val);
-#else
-	retval =
-	    ms_set_cmd(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm,
-		       6 + MS_EXTRA_SIZE, BLOCK_WRITE, WAIT_INT, data, 16,
-		       &val);
-#endif
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (val & INT_REG_CMDNK) {
-		ms_set_err_code(chip, MS_CMD_NK);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-	if (val & INT_REG_CED) {
-		if (val & INT_REG_ERR) {
-			ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_read_page(struct rts51x_chip *chip, u16 block_addr, u8 page_num)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-	u8 val = 0, data[6];
-
-	if (CHK_MS4BIT(ms_card)) {
-		/* Parallel interface */
-		data[0] = 0x88;
-	} else {
-		/* Serial interface */
-		data[0] = 0x80;
-	}
-	/* Block Address */
-	data[1] = 0;
-	data[2] = (u8) (block_addr >> 8);
-	data[3] = (u8) block_addr;
-	/* Page Number
-	 * Single page access mode */
-	data[4] = 0x20;
-	data[5] = page_num;
-
-	retval = ms_set_cmd(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm, 6,
-			    BLOCK_READ, WAIT_INT, data, 6, &val);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (val & INT_REG_CMDNK) {
-		ms_set_err_code(chip, MS_CMD_NK);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if (val & INT_REG_CED) {
-		if (val & INT_REG_ERR) {
-			if (!(val & INT_REG_BREQ)) {
-				ms_set_err_code(chip, MS_FLASH_READ_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-			retval = ms_read_status_reg(chip);
-			if (retval != STATUS_SUCCESS)
-				ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
-		} else {
-			if (!(val & INT_REG_BREQ)) {
-				ms_set_err_code(chip, MS_BREQ_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-	}
-
-	retval =
-	    ms_transfer_tpc(chip, MS_TM_NORMAL_READ, READ_PAGE_DATA, 0,
-			    NO_WAIT_INT);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	if (ms_check_err_code(chip, MS_FLASH_WRITE_ERROR))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_set_bad_block(struct rts51x_chip *chip, u16 phy_blk)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-	u8 val = 0, data[8], extra[MS_EXTRA_SIZE];
-
-	retval = ms_read_extra_data(chip, phy_blk, 0, extra, MS_EXTRA_SIZE);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	ms_set_err_code(chip, MS_NO_ERROR);
-
-	if (CHK_MS4BIT(ms_card)) {
-		/* Parallel interface */
-		data[0] = 0x88;
-	} else {
-		/* Serial interface */
-		data[0] = 0x80;
-	}
-	/* Block Address */
-	data[1] = 0;
-	data[2] = (u8) (phy_blk >> 8);
-	data[3] = (u8) phy_blk;
-	data[4] = 0x80;
-	data[5] = 0;
-	data[6] = extra[0] & 0x7F;
-	data[7] = 0xFF;
-
-#ifdef MS_SPEEDUP
-	retval = ms_set_cmd(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm, 7,
-			    BLOCK_WRITE, WAIT_INT, data, 7, &val);
-#else
-	retval = ms_set_cmd(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm, 7,
-			    BLOCK_WRITE, WAIT_INT, data, 7, &val);
-#endif
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (val & INT_REG_CMDNK) {
-		ms_set_err_code(chip, MS_CMD_NK);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if (val & INT_REG_CED) {
-		if (val & INT_REG_ERR) {
-			ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_erase_block(struct rts51x_chip *chip, u16 phy_blk)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, i = 0;
-	u8 val = 0, data[6];
-
-	retval =
-	    ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm,
-			       6);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	ms_set_err_code(chip, MS_NO_ERROR);
-
-	if (CHK_MS4BIT(ms_card)) {
-		/* Parallel interface */
-		data[0] = 0x88;
-	} else {
-		/* Serial interface */
-		data[0] = 0x80;
-	}
-	/* Block Address */
-	data[1] = 0;
-	data[2] = (u8) (phy_blk >> 8);
-	data[3] = (u8) phy_blk;
-	data[4] = 0;
-	data[5] = 0;
-
-ERASE_RTY:
-#ifdef MS_SPEEDUP
-	retval =
-	    ms_auto_set_cmd(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm, 6,
-			    BLOCK_ERASE, WAIT_INT, data, 6, &val);
-#else
-	retval = ms_set_cmd(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm, 6,
-			    BLOCK_ERASE, WAIT_INT, data, 6, &val);
-#endif
-
-	if (val & INT_REG_CMDNK) {
-		if (i < 3) {
-			i++;
-			goto ERASE_RTY;
-		}
-		ms_set_err_code(chip, MS_CMD_NK);
-		ms_set_bad_block(chip, phy_blk);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if (val & INT_REG_CED) {
-		if (val & INT_REG_ERR) {
-			ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static void ms_set_page_status(u16 log_blk, u8 type, u8 *extra, int extra_len)
-{
-	if (!extra || (extra_len < MS_EXTRA_SIZE))
-		return;
-
-	memset(extra, 0xFF, MS_EXTRA_SIZE);
-
-	if (type == setPS_NG)
-		extra[0] = 0xB8;
-	else
-		extra[0] = 0x98;
-
-	extra[2] = (u8) (log_blk >> 8);
-	extra[3] = (u8) log_blk;
-}
-
-static int ms_init_page(struct rts51x_chip *chip, u16 phy_blk, u16 log_blk,
-			u8 start_page, u8 end_page)
-{
-	int retval;
-	u8 extra[MS_EXTRA_SIZE], i;
-
-	memset(extra, 0xff, MS_EXTRA_SIZE);
-
-	extra[0] = 0xf8; /* Block, page OK, data erased */
-	extra[1] = 0xff;
-	extra[2] = (u8) (log_blk >> 8);
-	extra[3] = (u8) log_blk;
-
-	for (i = start_page; i < end_page; i++) {
-		if (monitor_card_cd(chip, MS_CARD) == CD_NOT_EXIST)
-			TRACE_RET(chip, STATUS_FAIL);
-
-		retval =
-		    ms_write_extra_data(chip, phy_blk, i, extra, MS_EXTRA_SIZE);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_copy_page(struct rts51x_chip *chip, u16 old_blk, u16 new_blk,
-			u16 log_blk, u8 start_page, u8 end_page)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, rty_cnt, uncorrect_flag = 0;
-	u8 extra[MS_EXTRA_SIZE], val, i, j, data[16];
-
-	RTS51X_DEBUGP("Copy page from 0x%x to 0x%x, logical block is 0x%x\n",
-		       old_blk, new_blk, log_blk);
-	RTS51X_DEBUGP("start_page = %d, end_page = %d\n", start_page,
-		       end_page);
-
-	retval = ms_read_extra_data(chip, new_blk, 0, extra, MS_EXTRA_SIZE);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = ms_read_status_reg(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	RTS51X_READ_REG(chip, PPBUF_BASE2, &val);
-
-	if (val & BUF_FULL) {
-		/* Clear Buffer */
-		retval = ms_send_cmd(chip, CLEAR_BUF, WAIT_INT);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-		/* GET_INT Register */
-		retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		if (!(val & INT_REG_CED)) {
-			ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	}
-
-	for (i = start_page; i < end_page; i++) {
-		if (monitor_card_cd(chip, MS_CARD) == CD_NOT_EXIST)
-			TRACE_RET(chip, STATUS_FAIL);
-
-		ms_read_extra_data(chip, old_blk, i, extra, MS_EXTRA_SIZE);
-
-		retval =
-		    ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE,
-				       SystemParm, 6);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-		/* Write REG */
-		ms_set_err_code(chip, MS_NO_ERROR);
-
-		if (CHK_MS4BIT(ms_card)) {
-			/* Parallel interface */
-			data[0] = 0x88;
-		} else {
-			/* Serial interface */
-			data[0] = 0x80;
-		}
-		/* Block Address */
-		data[1] = 0;
-		data[2] = (u8) (old_blk >> 8);
-		data[3] = (u8) old_blk;
-		data[4] = 0x20;
-		data[5] = i;
-
-		retval =
-		    ms_write_bytes(chip, WRITE_REG, 6, NO_WAIT_INT, data, 6);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		retval = ms_send_cmd(chip, BLOCK_READ, WAIT_INT);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		ms_set_err_code(chip, MS_NO_ERROR);
-		retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		if (val & INT_REG_CMDNK) {
-			ms_set_err_code(chip, MS_CMD_NK);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		if (val & INT_REG_CED) {
-			if (val & INT_REG_ERR) {
-				retval = ms_read_status_reg(chip);
-				if (retval != STATUS_SUCCESS) {
-					uncorrect_flag = 1;
-					RTS51X_DEBUGP("Uncorrectable error\n");
-				} else {
-					uncorrect_flag = 0;
-				}
-
-				retval =
-				    ms_transfer_tpc(chip, MS_TM_NORMAL_READ,
-					READ_PAGE_DATA, 0, NO_WAIT_INT);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, retval);
-				if (uncorrect_flag) {
-					ms_set_page_status(log_blk, setPS_NG,
-						extra, MS_EXTRA_SIZE);
-					if (i == 0)
-						extra[0] &= 0xEF;
-					ms_write_extra_data(chip, old_blk, i,
-							    extra,
-							    MS_EXTRA_SIZE);
-					RTS51X_DEBUGP("page %d : extra[0] = 0x%x\n",
-							i, extra[0]);
-					MS_SET_BAD_BLOCK_FLG(ms_card);
-
-					ms_set_page_status(log_blk, setPS_Error,
-							extra, MS_EXTRA_SIZE);
-					ms_write_extra_data(chip, new_blk, i,
-						extra, MS_EXTRA_SIZE);
-					continue;
-				}
-
-				for (rty_cnt = 0; rty_cnt < MS_MAX_RETRY_COUNT;
-				     rty_cnt++) {
-					retval =
-					    ms_transfer_tpc(chip,
-							    MS_TM_NORMAL_WRITE,
-							    WRITE_PAGE_DATA, 0,
-							    NO_WAIT_INT);
-					if (retval == STATUS_SUCCESS)
-						break;
-				}
-				if (rty_cnt == MS_MAX_RETRY_COUNT)
-					TRACE_RET(chip, STATUS_FAIL);
-			}
-
-			if (!(val & INT_REG_BREQ)) {
-				ms_set_err_code(chip, MS_BREQ_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-
-		retval = ms_set_rw_reg_addr(chip, OverwriteFlag,
-					    MS_EXTRA_SIZE, SystemParm,
-					    (6 + MS_EXTRA_SIZE));
-
-		/* Write REG */
-		ms_set_err_code(chip, MS_NO_ERROR);
-
-		if (CHK_MS4BIT(ms_card)) {
-			/* Parallel interface */
-			data[0] = 0x88;
-		} else {
-			/* Serial interface */
-			data[0] = 0x80;
-		}
-		/* Block Address */
-		data[1] = 0;
-		data[2] = (u8) (new_blk >> 8);
-		data[3] = (u8) new_blk;
-		data[4] = 0x20;
-		data[5] = i;
-
-		/* for MS check procedure */
-		if ((extra[0] & 0x60) != 0x60)
-			data[6] = extra[0];
-		else
-			data[6] = 0xF8;
-
-		data[6 + 1] = 0xFF;
-		data[6 + 2] = (u8) (log_blk >> 8);
-		data[6 + 3] = (u8) log_blk;
-
-		for (j = 4; j <= MS_EXTRA_SIZE; j++)
-			data[6 + j] = 0xFF;
-
-		retval =
-		    ms_write_bytes(chip, WRITE_REG, (6 + MS_EXTRA_SIZE),
-				   NO_WAIT_INT, data, 16);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-		/* GET_INT Register */
-		ms_set_err_code(chip, MS_NO_ERROR);
-		retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		if (val & INT_REG_CMDNK) {
-			ms_set_err_code(chip, MS_CMD_NK);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		if (val & INT_REG_CED) {
-			if (val & INT_REG_ERR) {
-				ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-
-		if (i == 0) {
-			retval =
-			    ms_set_rw_reg_addr(chip, OverwriteFlag,
-					       MS_EXTRA_SIZE, SystemParm, 7);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-
-			ms_set_err_code(chip, MS_NO_ERROR);
-
-			if (CHK_MS4BIT(ms_card)) {
-				/* Parallel interface */
-				data[0] = 0x88;
-			} else {
-				/* Serial interface */
-				data[0] = 0x80;
-			}
-			/* Block Address */
-			data[1] = 0;
-			data[2] = (u8) (old_blk >> 8);
-			data[3] = (u8) old_blk;
-			data[4] = 0x80;
-			data[5] = 0;
-			data[6] = 0xEF;
-			data[7] = 0xFF;
-
-			retval =
-			    ms_write_bytes(chip, WRITE_REG, 7, NO_WAIT_INT,
-					   data, 8);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-
-			retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-
-			ms_set_err_code(chip, MS_NO_ERROR);
-			retval =
-			    ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val,
-					  1);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-
-			if (val & INT_REG_CMDNK) {
-				ms_set_err_code(chip, MS_CMD_NK);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-
-			if (val & INT_REG_CED) {
-				if (val & INT_REG_ERR) {
-					ms_set_err_code(chip,
-							MS_FLASH_WRITE_ERROR);
-					TRACE_RET(chip, STATUS_FAIL);
-				}
-			}
-		}
-	}
-
-	return STATUS_SUCCESS;
-}
-
-#ifdef MS_SPEEDUP
-static int ms_auto_copy_page(struct rts51x_chip *chip, u16 old_blk, u16 new_blk,
-			     u16 log_blk, u8 start_page, u8 end_page)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-	u8 page_len, bus_width, val = 0;
-	u8 extra[MS_EXTRA_SIZE];
-
-	RTS51X_DEBUGP("Auto copy page from 0x%x to 0x%x, logical block is 0x%x\n",
-				old_blk, new_blk, log_blk);
-	RTS51X_DEBUGP("start_page = %d, end_page = %d\n", start_page,
-		       end_page);
-
-	page_len = end_page - start_page;
-
-	retval = ms_read_extra_data(chip, new_blk, 0, extra, MS_EXTRA_SIZE);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = ms_read_status_reg(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	RTS51X_READ_REG(chip, PPBUF_BASE2, &val);
-
-	if (val & BUF_FULL) {
-		retval = ms_send_cmd(chip, CLEAR_BUF, WAIT_INT);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		if (!(val & INT_REG_CED)) {
-			ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	}
-
-	if (CHK_MS4BIT(ms_card)) {
-		/* Parallel interface */
-		bus_width = 0x88;
-	} else {
-		/* Serial interface */
-		bus_width = 0x80;
-	}
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_OLD_BLOCK_0, 0xFF, (u8) old_blk);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_OLD_BLOCK_1, 0xFF,
-		       (u8) (old_blk >> 8));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_NEW_BLOCK_0, 0xFF, (u8) new_blk);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_NEW_BLOCK_1, 0xFF,
-		       (u8) (new_blk >> 8));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_LOG_BLOCK_0, 0xFF, (u8) log_blk);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_LOG_BLOCK_1, 0xFF,
-		       (u8) (log_blk >> 8));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_PAGE_START, 0xFF, start_page);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_PAGE_LENGTH, 0xFF, page_len);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_BUS_WIDTH, 0xFF, bus_width);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
-		       MS_TRANSFER_START | MS_TM_COPY_PAGE);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER, MS_TRANSFER_END,
-		       MS_TRANSFER_END);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_ms_error(chip);
-		TRACE_RET(chip, retval);
-	}
-
-	retval = rts51x_get_rsp(chip, 1, 5000);
-
-	if (CHECK_MS_TRANS_FAIL(chip, retval)) {
-		rts51x_clear_ms_error(chip);
-		if (retval == STATUS_TIMEDOUT)
-			TRACE_RET(chip, retval);
-		TRACE_GOTO(chip, Fail);
-	}
-
-	return STATUS_SUCCESS;
-
-Fail:
-	retval = ms_erase_block(chip, new_blk);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval =
-	    ms_copy_page(chip, old_blk, new_blk, log_blk, start_page, end_page);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-#endif
-
-static int reset_ms(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-	u16 i, reg_addr, block_size;
-	u8 val, j, *ptr;
-#ifndef SUPPORT_MAGIC_GATE
-	u16 eblock_cnt;
-#endif
-
-	retval = ms_prepare_reset(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	ms_card->ms_type |= TYPE_MS;
-
-	retval = ms_send_cmd(chip, MS_RESET, NO_WAIT_INT);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = ms_read_status_reg(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	RTS51X_READ_REG(chip, PPBUF_BASE2, &val);
-	if (val & WRT_PRTCT)
-		chip->card_wp |= MS_CARD;
-	else
-		chip->card_wp &= ~MS_CARD;
-
-	i = 0;
-
-RE_SEARCH:
-	/* Search For Boot Block */
-	while (i < (MAX_DEFECTIVE_BLOCK + 2)) {
-		if (monitor_card_cd(chip, MS_CARD) == CD_NOT_EXIST)
-			TRACE_RET(chip, STATUS_FAIL);
-
-		retval = ms_check_boot_block(chip, i);
-		if (retval != STATUS_SUCCESS) {
-			i++;
-			continue;
-		}
-
-		ms_card->boot_block = i;
-		break;
-	}
-
-	if (i == (MAX_DEFECTIVE_BLOCK + 2)) {
-		RTS51X_DEBUGP("No boot block found!");
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-	for (j = 0; j < 3; j++) {
-		retval = ms_read_page(chip, ms_card->boot_block, j);
-		if (retval != STATUS_SUCCESS) {
-			if (ms_check_err_code(chip, MS_FLASH_WRITE_ERROR)) {
-				i = ms_card->boot_block + 1;
-				ms_set_err_code(chip, MS_NO_ERROR);
-				goto RE_SEARCH;
-			}
-		}
-	}
-
-	/* Read boot block contents */
-	retval = ms_read_page(chip, ms_card->boot_block, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	/* Read MS system information as sys_info */
-	retval =
-	    rts51x_seq_read_register(chip, PPBUF_BASE2 + 0x1A0, 96,
-				     ms_card->raw_sys_info);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	/* Read useful block contents */
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, READ_REG_CMD, HEADER_ID0, 0, 0);
-	rts51x_add_cmd(chip, READ_REG_CMD, HEADER_ID1, 0, 0);
-
-	for (reg_addr = DISABLED_BLOCK0; reg_addr <= DISABLED_BLOCK3;
-	     reg_addr++) {
-		rts51x_add_cmd(chip, READ_REG_CMD, reg_addr, 0, 0);
-	}
-
-	for (reg_addr = BLOCK_SIZE_0; reg_addr <= PAGE_SIZE_1; reg_addr++)
-		rts51x_add_cmd(chip, READ_REG_CMD, reg_addr, 0, 0);
-
-	rts51x_add_cmd(chip, READ_REG_CMD, MS_Device_Type, 0, 0);
-	rts51x_add_cmd(chip, READ_REG_CMD, MS_4bit_Support, 0, 0);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, 16, 100);
-
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	ptr = rts51x_get_rsp_data(chip);
-
-	RTS51X_DEBUGP("Boot block data:\n");
-	RTS51X_DUMP(ptr, 16);
-
-	if (ptr[0] != 0x00 || ptr[1] != 0x01) {
-		i = ms_card->boot_block + 1;
-		goto RE_SEARCH;
-	}
-	if (ptr[12] != 0x02 || ptr[13] != 0x00) {
-		i = ms_card->boot_block + 1;
-		goto RE_SEARCH;
-	}
-	if ((ptr[14] == 1) || (ptr[14] == 3))
-		chip->card_wp |= MS_CARD;
-	block_size = ((u16) ptr[6] << 8) | ptr[7];
-	if (block_size == 0x0010) {
-		ms_card->block_shift = 5;
-		ms_card->page_off = 0x1F;
-	} else if (block_size == 0x0008) {
-		ms_card->block_shift = 4;
-		ms_card->page_off = 0x0F;
-	}
-	ms_card->total_block = ((u16) ptr[8] << 8) | ptr[9];
-
-#ifdef SUPPORT_MAGIC_GATE
-	j = ptr[10];
-
-	if (ms_card->block_shift == 4) {
-		if (j < 2)
-			ms_card->capacity = 0x1EE0;
-		else
-			ms_card->capacity = 0x3DE0;
-	} else {
-		if (j < 5)
-			ms_card->capacity = 0x7BC0;
-		else if (j < 0xA)
-			ms_card->capacity = 0xF7C0;
-		else if (j < 0x11)
-			ms_card->capacity = 0x1EF80;
-		else
-			ms_card->capacity = 0x3DF00;
-	}
-#else
-	eblock_cnt = ((u16) ptr[10] << 8) | ptr[11];
-
-	ms_card->capacity = ((u32) eblock_cnt - 2) << ms_card->block_shift;
-#endif
-
-	chip->capacity[chip->card2lun[MS_CARD]] = ms_card->capacity;
-
-	if (ptr[15]) {
-		retval = ms_set_rw_reg_addr(chip, 0, 0, SystemParm, 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
-		RTS51X_WRITE_REG(chip, PPBUF_BASE2, 0xFF, 0x88);
-		RTS51X_WRITE_REG(chip, PPBUF_BASE2 + 1, 0xFF, 0);
-
-		retval =
-		    ms_transfer_tpc(chip, MS_TM_WRITE_BYTES, WRITE_REG, 1,
-				    NO_WAIT_INT);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
-		RTS51X_WRITE_REG(chip, MS_CFG, 0x58 | MS_NO_CHECK_INT,
-				 MS_BUS_WIDTH_4 | PUSH_TIME_ODD |
-				 MS_NO_CHECK_INT);
-
-		ms_card->ms_type |= MS_4BIT;
-	}
-
-	if (CHK_MS4BIT(ms_card))
-		chip->card_bus_width[chip->card2lun[MS_CARD]] = 4;
-	else
-		chip->card_bus_width[chip->card2lun[MS_CARD]] = 1;
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_init_l2p_tbl(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int size, i, seg_no, retval;
-	u16 defect_block, reg_addr;
-	u8 val1, val2;
-
-	ms_card->segment_cnt = ms_card->total_block >> 9;
-	RTS51X_DEBUGP("ms_card->segment_cnt = %d\n", ms_card->segment_cnt);
-
-	size = ms_card->segment_cnt * sizeof(struct zone_entry);
-	ms_card->segment = vmalloc(size);
-	if (ms_card->segment == NULL)
-		TRACE_RET(chip, STATUS_FAIL);
-	memset(ms_card->segment, 0, size);
-
-	retval = ms_read_page(chip, ms_card->boot_block, 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_GOTO(chip, INIT_FAIL);
-
-	reg_addr = PPBUF_BASE2;
-	for (i = 0; i < (((ms_card->total_block >> 9) * 10) + 1); i++) {
-		retval = rts51x_read_register(chip, reg_addr++, &val1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, INIT_FAIL);
-		retval = rts51x_read_register(chip, reg_addr++, &val2);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, INIT_FAIL);
-
-		defect_block = ((u16) val1 << 8) | val2;
-		if (defect_block == 0xFFFF)
-			break;
-		seg_no = defect_block / 512;
-		ms_card->segment[seg_no].defect_list[ms_card->segment[seg_no].
-						     disable_count++] =
-		    defect_block;
-	}
-
-	for (i = 0; i < ms_card->segment_cnt; i++) {
-		ms_card->segment[i].build_flag = 0;
-		ms_card->segment[i].l2p_table = NULL;
-		ms_card->segment[i].free_table = NULL;
-		ms_card->segment[i].get_index = 0;
-		ms_card->segment[i].set_index = 0;
-		ms_card->segment[i].unused_blk_cnt = 0;
-
-		RTS51X_DEBUGP("defective block count of segment %d is %d\n",
-			       i, ms_card->segment[i].disable_count);
-	}
-
-	return STATUS_SUCCESS;
-
-INIT_FAIL:
-	if (ms_card->segment) {
-		vfree(ms_card->segment);
-		ms_card->segment = NULL;
-	}
-
-	return STATUS_FAIL;
-}
-
-static u16 ms_get_l2p_tbl(struct rts51x_chip *chip, int seg_no, u16 log_off)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	struct zone_entry *segment;
-
-	if (ms_card->segment == NULL)
-		return 0xFFFF;
-
-	segment = &(ms_card->segment[seg_no]);
-
-	if (segment->l2p_table)
-		return segment->l2p_table[log_off];
-
-	return 0xFFFF;
-}
-
-static void ms_set_l2p_tbl(struct rts51x_chip *chip, int seg_no, u16 log_off,
-			   u16 phy_blk)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	struct zone_entry *segment;
-
-	if (ms_card->segment == NULL)
-		return;
-
-	segment = &(ms_card->segment[seg_no]);
-	if (segment->l2p_table)
-		segment->l2p_table[log_off] = phy_blk;
-}
-
-static void ms_set_unused_block(struct rts51x_chip *chip, u16 phy_blk)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	struct zone_entry *segment;
-	int seg_no;
-
-	seg_no = (int)phy_blk >> 9;
-	segment = &(ms_card->segment[seg_no]);
-
-	segment->free_table[segment->set_index++] = phy_blk;
-	if (segment->set_index >= MS_FREE_TABLE_CNT)
-		segment->set_index = 0;
-	segment->unused_blk_cnt++;
-}
-
-static u16 ms_get_unused_block(struct rts51x_chip *chip, int seg_no)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	struct zone_entry *segment;
-	u16 phy_blk;
-
-	segment = &(ms_card->segment[seg_no]);
-
-	if (segment->unused_blk_cnt <= 0)
-		return 0xFFFF;
-
-	phy_blk = segment->free_table[segment->get_index];
-	segment->free_table[segment->get_index++] = 0xFFFF;
-	if (segment->get_index >= MS_FREE_TABLE_CNT)
-		segment->get_index = 0;
-	segment->unused_blk_cnt--;
-
-	return phy_blk;
-}
-
-static const unsigned short ms_start_idx[] = {
-	0, 494, 990, 1486, 1982, 2478, 2974, 3470,
-	3966, 4462, 4958, 5454, 5950, 6446, 6942, 7438, 7934
-};
-
-static int ms_arbitrate_l2p(struct rts51x_chip *chip, u16 phy_blk, u16 log_off,
-			    u8 us1, u8 us2)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	struct zone_entry *segment;
-	int seg_no;
-	u16 tmp_blk;
-
-	seg_no = (int)phy_blk >> 9;
-	segment = &(ms_card->segment[seg_no]);
-	tmp_blk = segment->l2p_table[log_off];
-
-	if (us1 != us2) {
-		if (us1 == 0) {
-			if (!(chip->card_wp & MS_CARD))
-				ms_erase_block(chip, tmp_blk);
-			ms_set_unused_block(chip, tmp_blk);
-			segment->l2p_table[log_off] = phy_blk;
-		} else {
-			if (!(chip->card_wp & MS_CARD))
-				ms_erase_block(chip, phy_blk);
-			ms_set_unused_block(chip, phy_blk);
-		}
-	} else {
-		if (phy_blk < tmp_blk) {
-			if (!(chip->card_wp & MS_CARD))
-				ms_erase_block(chip, phy_blk);
-			ms_set_unused_block(chip, phy_blk);
-		} else {
-			if (!(chip->card_wp & MS_CARD))
-				ms_erase_block(chip, tmp_blk);
-			ms_set_unused_block(chip, tmp_blk);
-			segment->l2p_table[log_off] = phy_blk;
-		}
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_build_l2p_tbl(struct rts51x_chip *chip, int seg_no)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	struct zone_entry *segment;
-	int retval, table_size, disable_cnt, defect_flag, i;
-	u16 start, end, phy_blk, log_blk, tmp_blk;
-	u8 extra[MS_EXTRA_SIZE], us1, us2;
-
-	RTS51X_DEBUGP("ms_build_l2p_tbl: %d\n", seg_no);
-
-	if (ms_card->segment == NULL) {
-		retval = ms_init_l2p_tbl(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	if (ms_card->segment[seg_no].build_flag) {
-		RTS51X_DEBUGP("l2p table of segment %d has been built\n",
-			       seg_no);
-		return STATUS_SUCCESS;
-	}
-
-	if (seg_no == 0)
-		table_size = 494;
-	else
-		table_size = 496;
-
-	segment = &(ms_card->segment[seg_no]);
-
-	if (segment->l2p_table == NULL) {
-		segment->l2p_table = vmalloc(table_size * 2);
-		if (segment->l2p_table == NULL)
-			TRACE_GOTO(chip, BUILD_FAIL);
-	}
-	memset((u8 *) (segment->l2p_table), 0xff, table_size * 2);
-
-	if (segment->free_table == NULL) {
-		segment->free_table = vmalloc(MS_FREE_TABLE_CNT * 2);
-		if (segment->free_table == NULL)
-			TRACE_GOTO(chip, BUILD_FAIL);
-	}
-	memset((u8 *) (segment->free_table), 0xff, MS_FREE_TABLE_CNT * 2);
-
-	start = (u16) seg_no << 9;
-	end = (u16) (seg_no + 1) << 9;
-
-	disable_cnt = segment->disable_count;
-
-	segment->get_index = segment->set_index = 0;
-	segment->unused_blk_cnt = 0;
-
-	for (phy_blk = start; phy_blk < end; phy_blk++) {
-		if (disable_cnt) {
-			defect_flag = 0;
-			for (i = 0; i < segment->disable_count; i++) {
-				if (phy_blk == segment->defect_list[i]) {
-					defect_flag = 1;
-					break;
-				}
-			}
-			if (defect_flag) {
-				disable_cnt--;
-				continue;
-			}
-		}
-
-		retval =
-		    ms_read_extra_data(chip, phy_blk, 0, extra, MS_EXTRA_SIZE);
-		if (retval != STATUS_SUCCESS) {
-			RTS51X_DEBUGP("read extra data fail\n");
-			ms_set_bad_block(chip, phy_blk);
-			continue;
-		}
-
-		if (seg_no == ms_card->segment_cnt - 1) {
-			if (!(extra[1] & NOT_TRANSLATION_TABLE)) {
-				if (!(chip->card_wp & MS_CARD)) {
-					retval = ms_erase_block(chip, phy_blk);
-					if (retval != STATUS_SUCCESS)
-						continue;
-					extra[2] = 0xff;
-					extra[3] = 0xff;
-				}
-			}
-		}
-
-		if (!(extra[0] & BLOCK_OK))
-			continue;
-		if (!(extra[1] & NOT_BOOT_BLOCK))
-			continue;
-		if ((extra[0] & PAGE_OK) != PAGE_OK)
-			continue;
-
-		log_blk = ((u16) extra[2] << 8) | extra[3];
-
-		if (log_blk == 0xFFFF) {
-			if (!(chip->card_wp & MS_CARD)) {
-				retval = ms_erase_block(chip, phy_blk);
-				if (retval != STATUS_SUCCESS)
-					continue;
-			}
-			ms_set_unused_block(chip, phy_blk);
-			continue;
-		}
-
-		if ((log_blk < ms_start_idx[seg_no]) ||
-		    (log_blk >= ms_start_idx[seg_no + 1])) {
-			if (!(chip->card_wp & MS_CARD)) {
-				retval = ms_erase_block(chip, phy_blk);
-				if (retval != STATUS_SUCCESS)
-					continue;
-			}
-			ms_set_unused_block(chip, phy_blk);
-			continue;
-		}
-
-		if (segment->l2p_table[log_blk - ms_start_idx[seg_no]] ==
-		    0xFFFF) {
-			segment->l2p_table[log_blk - ms_start_idx[seg_no]] =
-			    phy_blk;
-			continue;
-		}
-
-		us1 = extra[0] & 0x10;
-		tmp_blk = segment->l2p_table[log_blk - ms_start_idx[seg_no]];
-		retval =
-		    ms_read_extra_data(chip, tmp_blk, 0, extra, MS_EXTRA_SIZE);
-		if (retval != STATUS_SUCCESS)
-			continue;
-		us2 = extra[0] & 0x10;
-
-		(void)ms_arbitrate_l2p(chip, phy_blk,
-				       log_blk - ms_start_idx[seg_no], us1,
-				       us2);
-		continue;
-	}
-
-	segment->build_flag = 1;
-
-	RTS51X_DEBUGP("unused block count: %d\n", segment->unused_blk_cnt);
-
-	if (seg_no == ms_card->segment_cnt - 1) {
-		if (segment->unused_blk_cnt < 2)
-			chip->card_wp |= MS_CARD;
-	} else {
-		if (segment->unused_blk_cnt < 1)
-			chip->card_wp |= MS_CARD;
-	}
-
-	if (chip->card_wp & MS_CARD)
-		return STATUS_SUCCESS;
-
-	for (log_blk = ms_start_idx[seg_no]; log_blk < ms_start_idx[seg_no + 1];
-	     log_blk++) {
-		if (segment->l2p_table[log_blk - ms_start_idx[seg_no]] ==
-		    0xFFFF) {
-			phy_blk = ms_get_unused_block(chip, seg_no);
-			if (phy_blk == 0xFFFF) {
-				chip->card_wp |= MS_CARD;
-				return STATUS_SUCCESS;
-			}
-			retval = ms_init_page(chip, phy_blk, log_blk, 0, 1);
-			if (retval != STATUS_SUCCESS)
-				TRACE_GOTO(chip, BUILD_FAIL);
-			segment->l2p_table[log_blk - ms_start_idx[seg_no]] =
-			    phy_blk;
-			if (seg_no == ms_card->segment_cnt - 1) {
-				if (segment->unused_blk_cnt < 2) {
-					chip->card_wp |= MS_CARD;
-					return STATUS_SUCCESS;
-				}
-			} else {
-				if (segment->unused_blk_cnt < 1) {
-					chip->card_wp |= MS_CARD;
-					return STATUS_SUCCESS;
-				}
-			}
-		}
-	}
-
-	if (seg_no == 0) {
-		for (log_blk = 0; log_blk < 494; log_blk++) {
-			tmp_blk = segment->l2p_table[log_blk];
-			if (tmp_blk < ms_card->boot_block) {
-				RTS51X_DEBUGP("Boot block is not the first normal block.\n");
-
-				if (chip->card_wp & MS_CARD)
-					break;
-
-				phy_blk = ms_get_unused_block(chip, 0);
-#ifdef MS_SPEEDUP
-				retval =
-				    ms_auto_copy_page(chip, tmp_blk, phy_blk,
-						      log_blk, 0,
-						      ms_card->page_off + 1);
-#else
-				retval = ms_copy_page(chip, tmp_blk, phy_blk,
-						      log_blk, 0,
-						      ms_card->page_off + 1);
-#endif
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, retval);
-
-				segment->l2p_table[log_blk] = phy_blk;
-
-				retval = ms_set_bad_block(chip, tmp_blk);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, retval);
-			}
-		}
-	}
-
-	return STATUS_SUCCESS;
-
-BUILD_FAIL:
-	segment->build_flag = 0;
-	if (segment->l2p_table) {
-		vfree(segment->l2p_table);
-		segment->l2p_table = NULL;
-	}
-	if (segment->free_table) {
-		vfree(segment->free_table);
-		segment->free_table = NULL;
-	}
-
-	return STATUS_FAIL;
-}
-
-int rts51x_reset_ms_card(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-
-	memset(ms_card, 0, sizeof(struct ms_info));
-
-	rts51x_enable_card_clock(chip, MS_CARD);
-
-	retval = rts51x_select_card(chip, MS_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	ms_card->ms_type = 0;
-	ms_card->last_rw_int = 0;
-
-	retval = reset_ms_pro(chip);
-	if (retval != STATUS_SUCCESS) {
-		if (ms_card->check_ms_flow) {
-			retval = reset_ms(chip);
-			if (retval != STATUS_SUCCESS) {
-				if (chip->option.reset_or_rw_fail_set_pad_drive) {
-					rts51x_write_register(chip,
-						CARD_DRIVE_SEL, SD20_DRIVE_MASK,
-						DRIVE_8mA);
-				}
-				TRACE_RET(chip, retval);
-			}
-		} else {
-			if (chip->option.reset_or_rw_fail_set_pad_drive) {
-				rts51x_write_register(chip, CARD_DRIVE_SEL,
-						      SD20_DRIVE_MASK,
-						      DRIVE_8mA);
-			}
-			TRACE_RET(chip, retval);
-		}
-	}
-
-	retval = ms_set_init_para(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (!CHK_MSPRO(ms_card)) {
-		retval = ms_build_l2p_tbl(chip, ms_card->total_block / 512 - 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	RTS51X_DEBUGP("ms_card->ms_type = 0x%x\n", ms_card->ms_type);
-
-	return STATUS_SUCCESS;
-}
-
-static int mspro_set_rw_cmd(struct rts51x_chip *chip, u32 start_sec,
-			    u16 sec_cnt, u8 cmd)
-{
-	int retval, i;
-	u8 data[8];
-
-	data[0] = cmd;
-	data[1] = (u8) (sec_cnt >> 8);
-	data[2] = (u8) sec_cnt;
-	data[3] = (u8) (start_sec >> 24);
-	data[4] = (u8) (start_sec >> 16);
-	data[5] = (u8) (start_sec >> 8);
-	data[6] = (u8) start_sec;
-	data[7] = 0;
-
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval =
-		    ms_write_bytes(chip, PRO_EX_SET_CMD, 7, WAIT_INT, data, 8);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	return STATUS_SUCCESS;
-}
-
-static void mspro_stop_seq_mode(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-
-	if (ms_card->seq_mode) {
-		retval = ms_switch_clock(chip);
-		if (retval != STATUS_SUCCESS)
-			return;
-
-		ms_card->seq_mode = 0;
-		ms_card->total_sec_cnt = 0;
-		ms_card->last_rw_int = 0;
-		ms_send_cmd(chip, PRO_STOP, WAIT_INT);
-
-		rts51x_ep0_write_register(chip, MC_FIFO_CTL, FIFO_FLUSH,
-					  FIFO_FLUSH);
-	}
-}
-
-static inline int ms_auto_tune_clock(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-
-	if (chip->asic_code) {
-		if (ms_card->ms_clock > 30)
-			ms_card->ms_clock -= 20;
-	} else {
-		if (ms_card->ms_clock == CLK_80)
-			ms_card->ms_clock = CLK_60;
-		else if (ms_card->ms_clock == CLK_60)
-			ms_card->ms_clock = CLK_40;
-	}
-
-	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-static int mspro_rw_multi_sector(struct scsi_cmnd *srb,
-				 struct rts51x_chip *chip, u32 start_sector,
-				 u16 sector_cnt)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, mode_2k = 0;
-	u16 count;
-	u8 val, trans_mode, rw_tpc, rw_cmd;
-
-	ms_set_err_code(chip, MS_NO_ERROR);
-
-	ms_card->counter = 0;
-
-	if (CHK_MSHG(ms_card)) {
-		if ((start_sector % 4) || (sector_cnt % 4)) {
-			if (srb->sc_data_direction == DMA_FROM_DEVICE) {
-				rw_tpc = PRO_READ_LONG_DATA;
-				rw_cmd = PRO_READ_DATA;
-			} else {
-				rw_tpc = PRO_WRITE_LONG_DATA;
-				rw_cmd = PRO_WRITE_DATA;
-			}
-		} else {
-			if (srb->sc_data_direction == DMA_FROM_DEVICE) {
-				rw_tpc = PRO_READ_QUAD_DATA;
-				rw_cmd = PRO_READ_2K_DATA;
-			} else {
-				rw_tpc = PRO_WRITE_QUAD_DATA;
-				rw_cmd = PRO_WRITE_2K_DATA;
-			}
-			mode_2k = 1;
-		}
-	} else {
-		if (srb->sc_data_direction == DMA_FROM_DEVICE) {
-			rw_tpc = PRO_READ_LONG_DATA;
-			rw_cmd = PRO_READ_DATA;
-		} else {
-			rw_tpc = PRO_WRITE_LONG_DATA;
-			rw_cmd = PRO_WRITE_DATA;
-		}
-	}
-
-	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (srb->sc_data_direction == DMA_FROM_DEVICE)
-		trans_mode = MS_TM_AUTO_READ;
-	else
-		trans_mode = MS_TM_AUTO_WRITE;
-
-	val = ms_card->last_rw_int;
-
-	if (ms_card->seq_mode) {
-		if ((ms_card->pre_dir != srb->sc_data_direction)
-		    || ((ms_card->pre_sec_addr + ms_card->pre_sec_cnt) !=
-			start_sector)
-		    || (mode_2k && (ms_card->seq_mode & MODE_512_SEQ))
-		    || (!mode_2k && (ms_card->seq_mode & MODE_2K_SEQ))
-		    || !(val & MS_INT_BREQ)
-		    || ((ms_card->total_sec_cnt + sector_cnt) > 0xFE00)) {
-			ms_card->seq_mode = 0;
-			ms_card->total_sec_cnt = 0;
-			ms_card->last_rw_int = 0;
-			if (val & MS_INT_BREQ) {
-				retval = ms_send_cmd(chip, PRO_STOP, WAIT_INT);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, retval);
-
-				rts51x_ep0_write_register(chip, MC_FIFO_CTL,
-					FIFO_FLUSH, FIFO_FLUSH);
-			}
-		}
-	}
-
-	if (!ms_card->seq_mode) {
-		ms_card->total_sec_cnt = 0;
-		if (sector_cnt >= 0x80) {
-			if ((ms_card->capacity - start_sector) > 0xFE00)
-				count = 0xFE00;
-			else
-				count =
-				    (u16) (ms_card->capacity - start_sector);
-			if (count > sector_cnt) {
-				if (mode_2k)
-					ms_card->seq_mode |= MODE_2K_SEQ;
-				else
-					ms_card->seq_mode |= MODE_512_SEQ;
-			}
-		} else {
-			count = sector_cnt;
-		}
-		retval = mspro_set_rw_cmd(chip, start_sector, count, rw_cmd);
-		if (retval != STATUS_SUCCESS) {
-			ms_card->seq_mode = 0;
-			TRACE_RET(chip, retval);
-		}
-	}
-
-	retval =
-	    ms_transfer_data(chip, trans_mode, rw_tpc, sector_cnt, WAIT_INT,
-			     mode_2k, scsi_sg_count(srb), scsi_sglist(srb),
-			     scsi_bufflen(srb));
-	if (retval != STATUS_SUCCESS) {
-		ms_card->seq_mode = 0;
-		rts51x_ep0_read_register(chip, MS_TRANS_CFG, &val);
-		rts51x_clear_ms_error(chip);
-		if (val & MS_INT_BREQ)
-			ms_send_cmd(chip, PRO_STOP, WAIT_INT);
-		if (val & (MS_CRC16_ERR | MS_RDY_TIMEOUT)) {
-			RTS51X_DEBUGP("MSPro CRC error, tune clock!\n");
-			ms_auto_tune_clock(chip);
-		}
-
-		TRACE_RET(chip, retval);
-	}
-
-	ms_card->pre_sec_addr = start_sector;
-	ms_card->pre_sec_cnt = sector_cnt;
-	ms_card->pre_dir = srb->sc_data_direction;
-	ms_card->total_sec_cnt += sector_cnt;
-
-	return STATUS_SUCCESS;
-}
-
-static int mspro_read_format_progress(struct rts51x_chip *chip,
-				      const int short_data_len)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, i;
-	u32 total_progress, cur_progress;
-	u8 cnt, tmp;
-	u8 data[8];
-
-	ms_card->format_status = FORMAT_FAIL;
-
-	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	RTS51X_READ_REG(chip, MS_TRANS_CFG, &tmp);
-
-	if ((tmp & (MS_INT_CED | MS_INT_CMDNK | MS_INT_ERR)) == MS_INT_CED) {
-		ms_card->format_status = FORMAT_SUCCESS;
-		ms_card->pro_under_formatting = 0;
-		return STATUS_SUCCESS;
-	}
-	if (!
-	    ((tmp & (MS_INT_BREQ | MS_INT_CED | MS_INT_CMDNK | MS_INT_ERR)) ==
-	     MS_INT_BREQ)) {
-		ms_card->pro_under_formatting = 0;
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if (short_data_len >= 256)
-		cnt = 0;
-	else
-		cnt = (u8) short_data_len;
-
-	retval =
-	    ms_read_bytes(chip, PRO_READ_SHORT_DATA, cnt, WAIT_INT, data, 8);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	total_progress =
-	    (data[0] << 24) | (data[1] << 16) | (data[2] << 8) | data[3];
-	cur_progress =
-	    (data[4] << 24) | (data[5] << 16) | (data[6] << 8) | data[7];
-
-	RTS51X_DEBUGP("total_progress = %d, cur_progress = %d\n",
-		       total_progress, cur_progress);
-
-	if (total_progress == 0) {
-		ms_card->progress = 0;
-	} else {
-		u64 ulltmp = (u64) cur_progress * (u64) 65535;
-		do_div(ulltmp, total_progress);
-		ms_card->progress = (u16) ulltmp;
-	}
-	RTS51X_DEBUGP("progress = %d\n", ms_card->progress);
-
-	for (i = 0; i < 2500; i++) {
-		RTS51X_READ_REG(chip, MS_TRANS_CFG, &tmp);
-		if (tmp &
-		    (MS_INT_CED | MS_INT_CMDNK | MS_INT_BREQ | MS_INT_ERR))
-			break;
-
-		wait_timeout(1);
-	}
-
-	if (i == 2500)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	RTS51X_DEBUGP("MSPro format tmp:%d\n", tmp);
-
-	if (tmp & (MS_INT_CMDNK | MS_INT_ERR))
-		TRACE_RET(chip, STATUS_FAIL);
-	if (tmp & MS_INT_CED) {
-		ms_card->format_status = FORMAT_SUCCESS;
-		ms_card->pro_under_formatting = 0;
-	} else if (tmp & MS_INT_BREQ) {
-		ms_card->format_status = FORMAT_IN_PROGRESS;
-	} else {
-		ms_card->format_status = FORMAT_FAIL;
-		ms_card->pro_under_formatting = 0;
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	RTS51X_DEBUGP("MSPro format format_status:%d\n",
-		       ms_card->format_status);
-
-	return STATUS_SUCCESS;
-}
-
-void rts51x_mspro_polling_format_status(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int i;
-
-	if (ms_card->pro_under_formatting) {
-		for (i = 0; i < 65535; i++) {
-			mspro_read_format_progress(chip, MS_SHORT_DATA_LEN);
-			if (ms_card->format_status != FORMAT_IN_PROGRESS)
-				break;
-		}
-	}
-
-	return;
-}
-
-void rts51x_mspro_format_sense(struct rts51x_chip *chip, unsigned int lun)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-
-	if (CHK_FORMAT_STATUS(ms_card, FORMAT_SUCCESS)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_NO_SENSE);
-		ms_card->pro_under_formatting = 0;
-		ms_card->progress = 0;
-	} else if (CHK_FORMAT_STATUS(ms_card, FORMAT_IN_PROGRESS)) {
-		rts51x_set_sense_data(chip, lun, CUR_ERR, 0x02, 0, 0x04, 0x04,
-			       0, (u16) (ms_card->progress));
-	} else {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_FORMAT_CMD_FAILED);
-		ms_card->pro_under_formatting = 0;
-		ms_card->progress = 0;
-	}
-}
-
-int rts51x_mspro_format(struct scsi_cmnd *srb, struct rts51x_chip *chip,
-		 int short_data_len, int quick_format)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, i;
-	u8 buf[8], tmp;
-	u16 para;
-
-	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = ms_set_rw_reg_addr(chip, 0x00, 0x00, Pro_TPCParm, 0x01);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	memset(buf, 0, 2);
-	switch (short_data_len) {
-	case 32:
-		buf[0] = 0;
-		break;
-	case 64:
-		buf[0] = 1;
-		break;
-	case 128:
-		buf[0] = 2;
-		break;
-	case 256:
-	default:
-		buf[0] = 3;
-		break;
-	}
-
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval =
-		    ms_write_bytes(chip, PRO_WRITE_REG, 1, NO_WAIT_INT, buf, 2);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
-	/* Format command */
-	if (quick_format)
-		para = 0x0000;
-	else
-		para = 0x0001;
-	retval = mspro_set_rw_cmd(chip, 0, para, PRO_FORMAT);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	/* Check INT */
-	RTS51X_READ_REG(chip, MS_TRANS_CFG, &tmp);
-	if (tmp & (MS_INT_CMDNK | MS_INT_ERR))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	if ((tmp & (MS_INT_BREQ | MS_INT_CED)) == MS_INT_BREQ) {
-		ms_card->pro_under_formatting = 1;
-		ms_card->progress = 0;
-		ms_card->format_status = FORMAT_IN_PROGRESS;
-		return STATUS_SUCCESS;
-	}
-
-	if (tmp & MS_INT_CED) {
-		ms_card->pro_under_formatting = 0;
-		ms_card->progress = 0;
-		ms_card->format_status = FORMAT_SUCCESS;
-		rts51x_set_sense_type(chip, SCSI_LUN(srb), SENSE_TYPE_NO_SENSE);
-		return STATUS_SUCCESS;
-	}
-
-	TRACE_RET(chip, STATUS_FAIL);
-}
-
-#ifdef MS_SPEEDUP
-static int ms_read_multiple_pages(struct rts51x_chip *chip, u16 phy_blk,
-				  u16 log_blk, u8 start_page, u8 end_page,
-				  u8 *buf, void **ptr, unsigned int *offset)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-	int send_blkend;
-	u8 extra[MS_EXTRA_SIZE], val1, val2, data[6];
-	u8 page_cnt = end_page - start_page, page_addr, sec_cnt;
-
-	if (end_page != (ms_card->page_off + 1))
-		send_blkend = 1;
-	else
-		send_blkend = 0;
-
-	retval =
-	    ms_read_extra_data(chip, phy_blk, start_page, extra, MS_EXTRA_SIZE);
-	if (retval == STATUS_SUCCESS) {
-		if ((extra[1] & 0x30) != 0x30) {
-			ms_set_err_code(chip, MS_FLASH_READ_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	}
-
-	if (CHK_MS4BIT(ms_card)) {
-		/* Parallel interface */
-		data[0] = 0x88;
-	} else {
-		/* Serial interface */
-		data[0] = 0x80;
-	}
-	/* Block Address */
-	data[1] = 0;
-	data[2] = (u8) (phy_blk >> 8);
-	data[3] = (u8) phy_blk;
-	/* Page Number
-	 * Extra data access mode */
-	data[4] = 0;
-	data[5] = start_page;
-
-	retval =
-	    ms_auto_set_cmd(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm, 6,
-			    BLOCK_READ, WAIT_INT, data, 6, &val1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	rts51x_init_cmd(chip);
-
-	if (send_blkend)
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_BLKEND, SET_BLKEND,
-			       SET_BLKEND);
-	else
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_BLKEND, SET_BLKEND, 0);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, WAIT_INT,
-		       NO_WAIT_INT);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_SECTOR_CNT_L, 0xFF,
-		       (u8) page_cnt);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_SECTOR_CNT_H, 0xFF, 0);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, READ_PAGE_DATA);
-
-	rts51x_trans_dma_enable(DMA_FROM_DEVICE, chip, 512 * page_cnt, DMA_512);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
-		       MS_TRANSFER_START | MS_TM_MULTI_READ);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER, MS_TRANSFER_END,
-		       MS_TRANSFER_END);
-
-	retval = rts51x_send_cmd(chip, MODE_CDIR | STAGE_MS_STATUS, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval =
-	    rts51x_transfer_data_partial(chip, RCV_BULK_PIPE(chip), (void *)buf,
-					 ptr, offset, 512 * page_cnt,
-					 scsi_sg_count(chip->srb), NULL, 2000);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_ms_error(chip);
-		if (retval == STATUS_TIMEDOUT)
-			TRACE_RET(chip, retval);
-		TRACE_GOTO(chip, Fail);
-	}
-	retval = rts51x_get_rsp(chip, 3, 200);
-	if (CHECK_MS_TRANS_FAIL(chip, retval)) {
-		rts51x_clear_ms_error(chip);
-		if (retval == STATUS_TIMEDOUT)
-			TRACE_RET(chip, retval);
-		TRACE_GOTO(chip, Fail);
-	}
-
-	return STATUS_SUCCESS;
-
-Fail:
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, READ_REG_CMD, MS_SECTOR_CNT_L, 0, 0);
-
-	retval = rts51x_send_cmd(chip, MODE_CR | STAGE_MS_STATUS, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, 3, 200);
-
-	if (CHECK_MS_TRANS_FAIL(chip, retval))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	sec_cnt = chip->rsp_buf[0];
-	RTS51X_DEBUGP("%d pages need be transferred, %d pages remained\n",
-		       (int)page_cnt, (int)sec_cnt);
-	page_addr = start_page + (page_cnt - sec_cnt);
-
-	if (CHK_MS4BIT(ms_card)) {
-		val1 = chip->rsp_buf[1];
-		RTS51X_DEBUGP("MS_TRANS_CFG: 0x%x\n", val1);
-	} else {
-		val1 = 0;
-	}
-
-	val2 = chip->rsp_buf[2];
-	RTS51X_DEBUGP("GET_INT: 0x%x\n", val2);
-
-	if ((val1 & INT_CMDNK) || (val2 & INT_REG_CMDNK)) {
-		ms_set_err_code(chip, MS_CMD_NK);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if ((val1 & INT_ERR) || (val2 & INT_REG_ERR)) {
-		if ((val1 & INT_BREQ) || (val2 & INT_REG_BREQ)) {
-			retval = ms_read_status_reg(chip);
-			if (retval != STATUS_SUCCESS) {
-				if (!(chip->card_wp & MS_CARD)) {
-					reset_ms(chip);
-					ms_set_page_status(log_blk, setPS_NG,
-						extra, MS_EXTRA_SIZE);
-					ms_write_extra_data(chip, phy_blk,
-						page_addr, extra,
-						MS_EXTRA_SIZE);
-				}
-				ms_set_err_code(chip, MS_FLASH_READ_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		} else {
-			ms_set_err_code(chip, MS_FLASH_READ_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	} else {
-		if (CHK_MS4BIT(ms_card)) {
-			if (!(val1 & INT_BREQ) && !(val2 & INT_REG_BREQ)) {
-				ms_set_err_code(chip, MS_BREQ_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		} else {
-			if (!(val2 & INT_REG_BREQ)) {
-				ms_set_err_code(chip, MS_BREQ_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-	}
-
-	TRACE_RET(chip, STATUS_FAIL);
-}
-
-static int ms_write_multiple_pages(struct rts51x_chip *chip, u16 old_blk,
-				   u16 new_blk, u16 log_blk, u8 start_page,
-				   u8 end_page, u8 *buf, void **ptr,
-				   unsigned int *offset)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, i;
-	int send_blkend;
-	u8 val, data[16];
-	u8 page_cnt = end_page - start_page;
-
-	if ((end_page == (ms_card->page_off + 1)) || (page_cnt == 1))
-		send_blkend = 0;
-	else
-		send_blkend = 1;
-
-	if (!start_page) {
-		if (CHK_MS4BIT(ms_card)) {
-			/* Parallel interface */
-			data[0] = 0x88;
-		} else {
-			/* Serial interface */
-			data[0] = 0x80;
-		}
-		/* Block Address */
-		data[1] = 0;
-		data[2] = (u8) (old_blk >> 8);
-		data[3] = (u8) old_blk;
-		data[4] = 0x80;
-		data[5] = 0;
-		data[6] = 0xEF;
-		data[7] = 0xFF;
-
-		retval =
-		    ms_auto_set_cmd(chip, OverwriteFlag, MS_EXTRA_SIZE,
-				    SystemParm, 7, BLOCK_WRITE, WAIT_INT, data,
-				    7, &val);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	retval =
-	    ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm,
-			       (6 + MS_EXTRA_SIZE));
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	ms_set_err_code(chip, MS_NO_ERROR);
-
-	if (CHK_MS4BIT(ms_card)) {
-		/* Parallel interface */
-		data[0] = 0x88;
-	} else {
-		/* Serial interface */
-		data[0] = 0x80;
-	}
-	/* Block Address */
-	data[1] = 0;
-	data[2] = (u8) (new_blk >> 8);
-	data[3] = (u8) new_blk;
-	/* Page Number
-	 * Extra data access mode */
-	if (page_cnt == 1) {
-		/* Single page access mode */
-		data[4] = 0x20;
-	} else {
-		/* Block access mode */
-		data[4] = 0;
-	}
-	data[5] = start_page;
-	data[6] = 0xF8;
-	data[7] = 0xFF;
-	data[8] = (u8) (log_blk >> 8);
-	data[9] = (u8) log_blk;
-
-	for (i = 0x0A; i < 0x10; i++) {
-		/* ECC */
-		data[i] = 0xFF;
-	}
-
-	retval =
-	    ms_auto_set_cmd(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm,
-			    (6 + MS_EXTRA_SIZE), BLOCK_WRITE, WAIT_INT, data,
-			    16, &val);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	rts51x_init_cmd(chip);
-
-	if (send_blkend)
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_BLKEND, SET_BLKEND,
-			       SET_BLKEND);
-	else
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_BLKEND, SET_BLKEND, 0);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, WAIT_INT,
-		       NO_WAIT_INT);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_SECTOR_CNT_L, 0xFF,
-		       (u8) page_cnt);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_SECTOR_CNT_H, 0xFF, 0);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, WRITE_PAGE_DATA);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-		       RING_BUFFER);
-
-	rts51x_trans_dma_enable(DMA_TO_DEVICE, chip, 512 * page_cnt, DMA_512);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
-		       MS_TRANSFER_START | MS_TM_MULTI_WRITE);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER, MS_TRANSFER_END,
-		       MS_TRANSFER_END);
-
-	retval = rts51x_send_cmd(chip, MODE_CDOR | STAGE_MS_STATUS, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval =
-	    rts51x_transfer_data_partial(chip, SND_BULK_PIPE(chip), (void *)buf,
-					 ptr, offset, 512 * page_cnt,
-					 scsi_sg_count(chip->srb), NULL, 2000);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_ms_error(chip);
-		TRACE_RET(chip, retval);
-	}
-
-	retval = rts51x_get_rsp(chip, 3, 2000);
-
-
-	if (CHECK_MS_TRANS_FAIL(chip, retval)) {
-		rts51x_clear_ms_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-#else
-
-static int ms_read_multiple_pages(struct rts51x_chip *chip, u16 phy_blk,
-				  u16 log_blk, u8 start_page, u8 end_page,
-				  u8 *buf, void **ptr, unsigned int *offset)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, i;
-	u8 extra[MS_EXTRA_SIZE], page_addr, val, trans_cfg, data[6];
-
-	retval =
-	    ms_read_extra_data(chip, phy_blk, start_page, extra, MS_EXTRA_SIZE);
-	if (retval == STATUS_SUCCESS) {
-		if ((extra[1] & 0x30) != 0x30) {
-			ms_set_err_code(chip, MS_FLASH_READ_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	}
-
-	retval =
-	    ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm,
-			       6);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	/* Write REG */
-	if (CHK_MS4BIT(ms_card)) {
-		/* Parallel interface */
-		data[0] = 0x88;
-	} else {
-		/* Serial interface */
-		data[0] = 0x80;
-	}
-	/* Block Address */
-	data[1] = 0;
-	data[2] = (u8) (phy_blk >> 8);
-	data[3] = (u8) phy_blk;
-	/* Page Number
-	 * Extra data access mode */
-	data[4] = 0;
-	data[5] = start_page;
-
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval =
-		    ms_write_bytes(chip, WRITE_REG, 6, NO_WAIT_INT, data, 6);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	ms_set_err_code(chip, MS_NO_ERROR);
-
-	retval = ms_send_cmd(chip, BLOCK_READ, WAIT_INT);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	for (page_addr = start_page; page_addr < end_page; page_addr++) {
-		ms_set_err_code(chip, MS_NO_ERROR);
-
-		if (monitor_card_cd(chip, MS_CARD) == CD_NOT_EXIST) {
-			ms_set_err_code(chip, MS_NO_CARD);
-			chip->card_exist &= ~MS_CARD;
-			chip->card_ready &= ~MS_CARD;
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-		/* GET_INT Register */
-		retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-		if (val & INT_REG_CMDNK) {
-			ms_set_err_code(chip, MS_CMD_NK);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-		if (val & INT_REG_ERR) {
-			if (val & INT_REG_BREQ) {
-				retval = ms_read_status_reg(chip);
-				if (retval != STATUS_SUCCESS) {
-					if (!(chip->card_wp & MS_CARD)) {
-						reset_ms(chip);
-						ms_set_page_status(log_blk,
-							setPS_NG, extra,
-							MS_EXTRA_SIZE);
-						ms_write_extra_data(chip,
-							phy_blk, page_addr,
-							extra, MS_EXTRA_SIZE);
-					}
-					ms_set_err_code(chip,
-							MS_FLASH_READ_ERROR);
-					TRACE_RET(chip, STATUS_FAIL);
-				}
-			} else {
-				ms_set_err_code(chip, MS_FLASH_READ_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		} else {
-			if (!(val & INT_REG_BREQ)) {
-				ms_set_err_code(chip, MS_BREQ_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-
-		if (page_addr == (end_page - 1)) {
-			if (!(val & INT_REG_CED)) {
-				retval = ms_send_cmd(chip, BLOCK_END, WAIT_INT);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, retval);
-			}
-			retval =
-			    ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val,
-					  1);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-			if (!(val & INT_REG_CED)) {
-				ms_set_err_code(chip, MS_FLASH_READ_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-
-			trans_cfg = NO_WAIT_INT;
-		} else {
-			trans_cfg = WAIT_INT;
-		}
-
-		rts51x_init_cmd(chip);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF,
-			       READ_PAGE_DATA);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF,
-			       trans_cfg);
-
-		rts51x_trans_dma_enable(DMA_FROM_DEVICE, chip, 512, DMA_512);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
-			       MS_TRANSFER_START | MS_TM_NORMAL_READ);
-		rts51x_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER,
-			       MS_TRANSFER_END, MS_TRANSFER_END);
-
-		retval = rts51x_send_cmd(chip, MODE_CDIR, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		retval =
-		    rts51x_transfer_data_partial(chip, RCV_BULK_PIPE(chip),
-						 (void *)buf, ptr, offset, 512,
-						 scsi_sg_count(chip->srb), NULL,
-						 2000);
-		if (retval != STATUS_SUCCESS) {
-			if (retval == STATUS_TIMEDOUT) {
-				ms_set_err_code(chip, MS_TO_ERROR);
-				rts51x_clear_ms_error(chip);
-				TRACE_RET(chip, retval);
-			}
-
-			retval =
-			    rts51x_ep0_read_register(chip, MS_TRANS_CFG, &val);
-			if (retval != STATUS_SUCCESS) {
-				ms_set_err_code(chip, MS_TO_ERROR);
-				rts51x_clear_ms_error(chip);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-			if (val & (MS_CRC16_ERR | MS_RDY_TIMEOUT)) {
-				ms_set_err_code(chip, MS_CRC16_ERROR);
-				rts51x_clear_ms_error(chip);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-
-		retval = rts51x_get_rsp(chip, 1, 2000);
-		if (CHECK_MS_TRANS_FAIL(chip, retval)) {
-			if (retval == STATUS_TIMEDOUT) {
-				ms_set_err_code(chip, MS_TO_ERROR);
-				rts51x_clear_ms_error(chip);
-				TRACE_RET(chip, retval);
-			}
-
-			retval =
-			    rts51x_ep0_read_register(chip, MS_TRANS_CFG, &val);
-			if (retval != STATUS_SUCCESS) {
-				ms_set_err_code(chip, MS_TO_ERROR);
-				rts51x_clear_ms_error(chip);
-				TRACE_RET(chip, retval);
-			}
-			if (val & (MS_CRC16_ERR | MS_RDY_TIMEOUT)) {
-				ms_set_err_code(chip, MS_CRC16_ERROR);
-				rts51x_clear_ms_error(chip);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_write_multiple_pages(struct rts51x_chip *chip, u16 old_blk,
-				   u16 new_blk, u16 log_blk, u8 start_page,
-				   u8 end_page, u8 *buf, void **ptr,
-				   unsigned int *offset)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, i;
-	u8 page_addr, val, data[16];
-
-	if (!start_page) {
-		retval =
-		    ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE,
-				       SystemParm, 7);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		if (CHK_MS4BIT(ms_card)) {
-			/* Parallel interface */
-			data[0] = 0x88;
-		} else {
-			/* Serial interface */
-			data[0] = 0x80;
-		}
-		/* Block Address */
-		data[1] = 0;
-		data[2] = (u8) (old_blk >> 8);
-		data[3] = (u8) old_blk;
-		data[4] = 0x80;
-		data[5] = 0;
-		data[6] = 0xEF;
-		data[7] = 0xFF;
-
-		retval =
-		    ms_write_bytes(chip, WRITE_REG, 7, NO_WAIT_INT, data, 8);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-		/* GET_INT Register */
-		ms_set_err_code(chip, MS_NO_ERROR);
-		retval =
-		    ms_transfer_tpc(chip, MS_TM_READ_BYTES, GET_INT, 1,
-				    NO_WAIT_INT);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	retval =
-	    ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE, SystemParm,
-			       (6 + MS_EXTRA_SIZE));
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	ms_set_err_code(chip, MS_NO_ERROR);
-
-	if (CHK_MS4BIT(ms_card)) {
-		/* Parallel interface */
-		data[0] = 0x88;
-	} else {
-		/* Serial interface */
-		data[0] = 0x80;
-	}
-	/* Block Address */
-	data[1] = 0;
-	data[2] = (u8) (new_blk >> 8);
-	data[3] = (u8) new_blk;
-	/* Page Number
-	 * Extra data access mode */
-	if ((end_page - start_page) == 1) {
-		/* Single page access mode */
-		data[4] = 0x20;
-	} else {
-		/* Block access mode */
-		data[4] = 0;
-	}
-	data[5] = start_page;
-	data[6] = 0xF8;
-	data[7] = 0xFF;
-	data[8] = (u8) (log_blk >> 8);
-	data[9] = (u8) log_blk;
-
-	for (i = 0x0A; i < 0x10; i++) {
-		/* ECC */
-		data[i] = 0xFF;
-	}
-
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval =
-		    ms_write_bytes(chip, WRITE_REG, 6 + MS_EXTRA_SIZE,
-				   NO_WAIT_INT, data, 16);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
-	/* GET_INT Register */
-	retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	for (page_addr = start_page; page_addr < end_page; page_addr++) {
-		ms_set_err_code(chip, MS_NO_ERROR);
-
-		if (monitor_card_cd(chip, MS_CARD) == CD_NOT_EXIST) {
-			ms_set_err_code(chip, MS_NO_CARD);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		if (val & INT_REG_CMDNK) {
-			ms_set_err_code(chip, MS_CMD_NK);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-		if (val & INT_REG_ERR) {
-			ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-		if (!(val & INT_REG_BREQ)) {
-			ms_set_err_code(chip, MS_BREQ_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		udelay(30);
-
-		rts51x_init_cmd(chip);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF,
-			       WRITE_PAGE_DATA);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF,
-			       WAIT_INT);
-
-		rts51x_trans_dma_enable(DMA_TO_DEVICE, chip, 512, DMA_512);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
-			       MS_TRANSFER_START | MS_TM_NORMAL_WRITE);
-		rts51x_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER,
-			       MS_TRANSFER_END, MS_TRANSFER_END);
-
-		retval = rts51x_send_cmd(chip, MODE_CDOR, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		retval =
-		    rts51x_transfer_data_partial(chip, SND_BULK_PIPE(chip),
-						 (void *)buf, ptr, offset, 512,
-						 scsi_sg_count(chip->srb), NULL,
-						 2000);
-		if (retval != STATUS_SUCCESS) {
-			ms_set_err_code(chip, MS_TO_ERROR);
-			rts51x_clear_ms_error(chip);
-
-			if (retval == STATUS_TIMEDOUT)
-				TRACE_RET(chip, STATUS_TIMEDOUT);
-			else
-				TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		retval = rts51x_get_rsp(chip, 1, 2000);
-		if (CHECK_MS_TRANS_FAIL(chip, retval)) {
-			ms_set_err_code(chip, MS_TO_ERROR);
-			rts51x_clear_ms_error(chip);
-
-			if (retval == STATUS_TIMEDOUT)
-				TRACE_RET(chip, STATUS_TIMEDOUT);
-			else
-				TRACE_RET(chip, STATUS_FAIL);
-		}
-		/* GET_INT Register */
-		retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		if ((end_page - start_page) == 1) {
-			if (!(val & INT_REG_CED)) {
-				/* Command can not be executed */
-				ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		} else {
-			if (page_addr == (end_page - 1)) {
-				if (!(val & INT_REG_CED)) {
-					retval =
-					    ms_send_cmd(chip, BLOCK_END,
-							WAIT_INT);
-					if (retval != STATUS_SUCCESS)
-						TRACE_RET(chip, retval);
-				}
-				/* GET_INT Register */
-				retval =
-				    ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT,
-						  &val, 1);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, retval);
-			}
-
-			if ((page_addr == (end_page - 1))
-			    || (page_addr == ms_card->page_off)) {
-				if (!(val & INT_REG_CED)) {
-					ms_set_err_code(chip,
-							MS_FLASH_WRITE_ERROR);
-					TRACE_RET(chip, STATUS_FAIL);
-				}
-			}
-		}
-	}
-
-	return STATUS_SUCCESS;
-}
-#endif
-
-static int ms_finish_write(struct rts51x_chip *chip, u16 old_blk, u16 new_blk,
-			   u16 log_blk, u8 page_off)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, seg_no;
-
-#ifdef MS_SPEEDUP
-	retval = ms_auto_copy_page(chip, old_blk, new_blk, log_blk,
-				   page_off, ms_card->page_off + 1);
-#else
-	retval = ms_copy_page(chip, old_blk, new_blk, log_blk,
-			      page_off, ms_card->page_off + 1);
-#endif
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	seg_no = old_blk >> 9;
-
-	if (MS_TST_BAD_BLOCK_FLG(ms_card)) {
-		MS_CLR_BAD_BLOCK_FLG(ms_card);
-		ms_set_bad_block(chip, old_blk);
-	} else {
-		retval = ms_erase_block(chip, old_blk);
-		if (retval == STATUS_SUCCESS)
-			ms_set_unused_block(chip, old_blk);
-	}
-
-	ms_set_l2p_tbl(chip, seg_no, log_blk - ms_start_idx[seg_no], new_blk);
-
-	return STATUS_SUCCESS;
-}
-
-static int ms_prepare_write(struct rts51x_chip *chip, u16 old_blk, u16 new_blk,
-			    u16 log_blk, u8 start_page)
-{
-	int retval;
-
-	if (start_page) {
-#ifdef MS_SPEEDUP
-		retval =
-		    ms_auto_copy_page(chip, old_blk, new_blk, log_blk, 0,
-				      start_page);
-#else
-		retval =
-		    ms_copy_page(chip, old_blk, new_blk, log_blk, 0,
-				 start_page);
-#endif
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_ms_delay_write(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	struct rts51x_ms_delay_write_tag *delay_write = &(ms_card->delay_write);
-	int retval;
-
-	if (delay_write->delay_write_flag) {
-		retval = ms_set_init_para(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		delay_write->delay_write_flag = 0;
-		retval = ms_finish_write(chip,
-					 delay_write->old_phyblock,
-					 delay_write->new_phyblock,
-					 delay_write->logblock,
-					 delay_write->pageoff);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static inline void rts51x_ms_rw_fail(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	if (srb->sc_data_direction == DMA_FROM_DEVICE)
-		rts51x_set_sense_type(chip, SCSI_LUN(srb),
-			       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-	else
-		rts51x_set_sense_type(chip, SCSI_LUN(srb), SENSE_TYPE_MEDIA_WRITE_ERR);
-}
-
-static int rts51x_ms_rw_multi_sector(struct scsi_cmnd *srb, struct rts51x_chip *chip,
-			      u32 start_sector, u16 sector_cnt)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	unsigned int lun = SCSI_LUN(srb);
-	int retval, seg_no;
-	unsigned int offset = 0;
-	u16 old_blk = 0, new_blk = 0, log_blk, total_sec_cnt = sector_cnt;
-	u8 start_page, end_page = 0, page_cnt;
-	u8 *buf;
-	void *ptr = NULL;
-	struct rts51x_ms_delay_write_tag *delay_write = &(ms_card->delay_write);
-
-	ms_set_err_code(chip, MS_NO_ERROR);
-
-	ms_card->counter = 0;
-
-	buf = (u8 *) scsi_sglist(srb);
-
-	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_ms_rw_fail(srb, chip);
-		TRACE_RET(chip, retval);
-	}
-
-	log_blk = (u16) (start_sector >> ms_card->block_shift);
-	start_page = (u8) (start_sector & ms_card->page_off);
-
-	for (seg_no = 0; seg_no < ARRAY_SIZE(ms_start_idx) - 1; seg_no++) {
-		if (log_blk < ms_start_idx[seg_no + 1])
-			break;
-	}
-
-	if (ms_card->segment[seg_no].build_flag == 0) {
-		retval = ms_build_l2p_tbl(chip, seg_no);
-		if (retval != STATUS_SUCCESS) {
-			chip->card_fail |= MS_CARD;
-			rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-			TRACE_RET(chip, retval);
-		}
-	}
-
-	if (srb->sc_data_direction == DMA_TO_DEVICE) {
-		if (delay_write->delay_write_flag &&
-		    (delay_write->logblock == log_blk) &&
-		    (start_page > delay_write->pageoff)) {
-			delay_write->delay_write_flag = 0;
-#ifdef MS_SPEEDUP
-			retval = ms_auto_copy_page(chip,
-						   delay_write->old_phyblock,
-						   delay_write->new_phyblock,
-						   log_blk,
-						   delay_write->pageoff,
-						   start_page);
-#else
-			retval = ms_copy_page(chip,
-					      delay_write->old_phyblock,
-					      delay_write->new_phyblock,
-					      log_blk, delay_write->pageoff,
-					      start_page);
-#endif
-			if (retval != STATUS_SUCCESS) {
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, retval);
-			}
-			old_blk = delay_write->old_phyblock;
-			new_blk = delay_write->new_phyblock;
-		} else if (delay_write->delay_write_flag &&
-			   (delay_write->logblock == log_blk) &&
-			   (start_page == delay_write->pageoff)) {
-			delay_write->delay_write_flag = 0;
-			old_blk = delay_write->old_phyblock;
-			new_blk = delay_write->new_phyblock;
-		} else {
-			retval = rts51x_ms_delay_write(chip);
-			if (retval != STATUS_SUCCESS) {
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, retval);
-			}
-			old_blk =
-			    ms_get_l2p_tbl(chip, seg_no,
-					   log_blk - ms_start_idx[seg_no]);
-			new_blk = ms_get_unused_block(chip, seg_no);
-			if ((old_blk == 0xFFFF) || (new_blk == 0xFFFF)) {
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-
-			retval =
-			    ms_prepare_write(chip, old_blk, new_blk, log_blk,
-					     start_page);
-			if (retval != STATUS_SUCCESS) {
-				if (monitor_card_cd(chip, MS_CARD) ==
-				    CD_NOT_EXIST) {
-					rts51x_set_sense_type(chip, lun,
-						SENSE_TYPE_MEDIA_NOT_PRESENT);
-					TRACE_RET(chip, STATUS_FAIL);
-				}
-
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, retval);
-			}
-		}
-	} else {
-		retval = rts51x_ms_delay_write(chip);
-		if (retval != STATUS_SUCCESS) {
-			if (monitor_card_cd(chip, MS_CARD) == CD_NOT_EXIST) {
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_NOT_PRESENT);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-			TRACE_RET(chip, retval);
-		}
-		old_blk =
-		    ms_get_l2p_tbl(chip, seg_no,
-				   log_blk - ms_start_idx[seg_no]);
-		if (old_blk == 0xFFFF) {
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	}
-
-	RTS51X_DEBUGP("seg_no = %d, old_blk = 0x%x, new_blk = 0x%x\n", seg_no,
-		       old_blk, new_blk);
-
-	while (total_sec_cnt) {
-		if ((start_page + total_sec_cnt) > (ms_card->page_off + 1))
-			end_page = ms_card->page_off + 1;
-		else
-			end_page = start_page + (u8) total_sec_cnt;
-		page_cnt = end_page - start_page;
-
-		RTS51X_DEBUGP("start_page = %d, end_page = %d, page_cnt = %d\n",
-					start_page, end_page, page_cnt);
-
-		if (srb->sc_data_direction == DMA_FROM_DEVICE)
-			retval = ms_read_multiple_pages(chip,
-							old_blk, log_blk,
-							start_page, end_page,
-							buf, &ptr, &offset);
-		else
-			retval = ms_write_multiple_pages(chip, old_blk,
-							 new_blk, log_blk,
-							 start_page, end_page,
-							 buf, &ptr, &offset);
-
-		if (retval != STATUS_SUCCESS) {
-			if (monitor_card_cd(chip, MS_CARD) == CD_NOT_EXIST) {
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_NOT_PRESENT);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-
-			rts51x_ms_rw_fail(srb, chip);
-			TRACE_RET(chip, retval);
-		}
-		/* Update L2P table if need */
-		if (srb->sc_data_direction == DMA_TO_DEVICE) {
-			if (end_page == (ms_card->page_off + 1)) {
-				retval = ms_erase_block(chip, old_blk);
-				if (retval == STATUS_SUCCESS)
-					ms_set_unused_block(chip, old_blk);
-				ms_set_l2p_tbl(chip, seg_no,
-					       log_blk - ms_start_idx[seg_no],
-					       new_blk);
-			}
-		}
-
-		total_sec_cnt -= page_cnt;
-
-		if (total_sec_cnt == 0)
-			break;
-
-		log_blk++;
-
-		for (seg_no = 0; seg_no < ARRAY_SIZE(ms_start_idx) - 1;
-				seg_no++) {
-			if (log_blk < ms_start_idx[seg_no + 1])
-				break;
-		}
-
-		if (ms_card->segment[seg_no].build_flag == 0) {
-			retval = ms_build_l2p_tbl(chip, seg_no);
-			if (retval != STATUS_SUCCESS) {
-				chip->card_fail |= MS_CARD;
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_NOT_PRESENT);
-				TRACE_RET(chip, retval);
-			}
-		}
-
-		old_blk =
-		    ms_get_l2p_tbl(chip, seg_no,
-				   log_blk - ms_start_idx[seg_no]);
-		if (old_blk == 0xFFFF) {
-			rts51x_ms_rw_fail(srb, chip);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		if (srb->sc_data_direction == DMA_TO_DEVICE) {
-			new_blk = ms_get_unused_block(chip, seg_no);
-			if (new_blk == 0xFFFF) {
-				rts51x_ms_rw_fail(srb, chip);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-
-		RTS51X_DEBUGP("seg_no = %d, old_blk = 0x%x, new_blk = 0x%x\n",
-			       seg_no, old_blk, new_blk);
-
-		start_page = 0;
-	}
-
-	if (srb->sc_data_direction == DMA_TO_DEVICE) {
-		if (end_page < (ms_card->page_off + 1)) {
-			delay_write->delay_write_flag = 1;
-			delay_write->old_phyblock = old_blk;
-			delay_write->new_phyblock = new_blk;
-			delay_write->logblock = log_blk;
-			delay_write->pageoff = end_page;
-		}
-	}
-
-	scsi_set_resid(srb, 0);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_ms_rw(struct scsi_cmnd *srb, struct rts51x_chip *chip, u32 start_sector,
-	  u16 sector_cnt)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-
-	if (CHK_MSPRO(ms_card))
-		retval =
-		    mspro_rw_multi_sector(srb, chip, start_sector, sector_cnt);
-	else
-		retval =
-		    rts51x_ms_rw_multi_sector(srb, chip, start_sector, sector_cnt);
-
-	return retval;
-}
-
-void rts51x_ms_free_l2p_tbl(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int i = 0;
-
-	if (ms_card->segment != NULL) {
-		for (i = 0; i < ms_card->segment_cnt; i++) {
-			if (ms_card->segment[i].l2p_table != NULL) {
-				vfree(ms_card->segment[i].l2p_table);
-				ms_card->segment[i].l2p_table = NULL;
-			}
-			if (ms_card->segment[i].free_table != NULL) {
-				vfree(ms_card->segment[i].free_table);
-				ms_card->segment[i].free_table = NULL;
-			}
-		}
-		vfree(ms_card->segment);
-		ms_card->segment = NULL;
-	}
-}
-
-void rts51x_ms_cleanup_work(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-
-	if (CHK_MSPRO(ms_card)) {
-		if (ms_card->seq_mode) {
-			RTS51X_DEBUGP("MS Pro: stop transmission\n");
-			mspro_stop_seq_mode(chip);
-			ms_card->counter = 0;
-		}
-		if (CHK_MSHG(ms_card)) {
-			u8 value;
-			rts51x_read_register(chip, MS_CFG, &value);
-			if (value & MS_2K_SECTOR_MODE)
-				rts51x_write_register(chip, MS_CFG,
-						      MS_2K_SECTOR_MODE, 0x00);
-		}
-	} else if ((!CHK_MSPRO(ms_card))
-		   && ms_card->delay_write.delay_write_flag) {
-		RTS51X_DEBUGP("MS: delay write\n");
-		rts51x_ms_delay_write(chip);
-		ms_card->counter = 0;
-	}
-}
-
-static int ms_power_off_card3v3(struct rts51x_chip *chip)
-{
-	int retval;
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_CLK_EN, MS_CLK_EN, 0);
-	if (chip->asic_code)
-		ms_pull_ctl_disable(chip);
-	else
-		rts51x_add_cmd(chip, WRITE_REG_CMD, FPGA_PULL_CTL,
-			       FPGA_MS_PULL_CTL_BIT | 0x20,
-			       FPGA_MS_PULL_CTL_BIT);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_OE, MS_OUTPUT_EN, 0);
-	if (!chip->option.FT2_fast_mode) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, POWER_MASK,
-			       POWER_OFF);
-	}
-
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_release_ms_card(struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-
-	RTS51X_DEBUGP("rts51x_release_ms_card\n");
-
-	ms_card->delay_write.delay_write_flag = 0;
-	ms_card->pro_under_formatting = 0;
-
-	chip->card_ready &= ~MS_CARD;
-	chip->card_fail &= ~MS_CARD;
-	chip->card_wp &= ~MS_CARD;
-
-	rts51x_ms_free_l2p_tbl(chip);
-
-	rts51x_write_register(chip, SFSM_ED, HW_CMD_STOP, HW_CMD_STOP);
-
-	memset(ms_card->raw_sys_info, 0, 96);
-#ifdef SUPPORT_PCGL_1P18
-	memset(ms_card->raw_model_name, 0, 48);
-#endif
-
-	retval = ms_power_off_card3v3(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
diff --git a/drivers/staging/rts5139/ms.h b/drivers/staging/rts5139/ms.h
deleted file mode 100644
index 857c1974ef2..00000000000
--- a/drivers/staging/rts5139/ms.h
+++ /dev/null
@@ -1,261 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- * Header file
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#ifndef __RTS51X_MS_H
-#define __RTS51X_MS_H
-
-#include "rts51x_chip.h"
-
-#define	MS_MAX_RETRY_COUNT		3
-
-#define	MS_EXTRA_SIZE			0x9
-
-#define	WRT_PRTCT			0x01
-
-/* Error Code */
-#define	MS_NO_ERROR				0x00
-#define	MS_CRC16_ERROR				0x80
-#define	MS_TO_ERROR				0x40
-#define	MS_NO_CARD				0x20
-#define	MS_NO_MEMORY				0x10
-#define	MS_CMD_NK				0x08
-#define	MS_FLASH_READ_ERROR			0x04
-#define	MS_FLASH_WRITE_ERROR			0x02
-#define	MS_BREQ_ERROR				0x01
-#define	MS_NOT_FOUND				0x03
-
-/* Transfer Protocol Command */
-#define READ_PAGE_DATA				0x02
-#define READ_REG				0x04
-#define	GET_INT					0x07
-#define WRITE_PAGE_DATA				0x0D
-#define WRITE_REG				0x0B
-#define SET_RW_REG_ADRS				0x08
-#define SET_CMD					0x0E
-
-#define	PRO_READ_LONG_DATA			0x02
-#define	PRO_READ_SHORT_DATA			0x03
-#define PRO_READ_REG				0x04
-#define	PRO_READ_QUAD_DATA			0x05
-#define PRO_GET_INT				0x07
-#define	PRO_WRITE_LONG_DATA			0x0D
-#define	PRO_WRITE_SHORT_DATA			0x0C
-#define	PRO_WRITE_QUAD_DATA			0x0A
-#define PRO_WRITE_REG				0x0B
-#define PRO_SET_RW_REG_ADRS			0x08
-#define PRO_SET_CMD				0x0E
-#define PRO_EX_SET_CMD				0x09
-
-#ifdef SUPPORT_MAGIC_GATE
-#define MG_GET_ID		0x40
-#define MG_SET_LID		0x41
-#define MG_GET_LEKB		0x42
-#define MG_SET_RD		0x43
-#define MG_MAKE_RMS		0x44
-#define MG_MAKE_KSE		0x45
-#define MG_SET_IBD		0x46
-#define MG_GET_IBD		0x47
-#endif
-
-#ifdef XC_POWERCLASS
-#define XC_CHG_POWER		0x16
-#endif
-
-/* ++ CMD over Memory Stick */
-/*  Flash CMD */
-#define BLOCK_READ		0xAA
-#define	BLOCK_WRITE		0x55
-#define BLOCK_END		0x33
-#define BLOCK_ERASE		0x99
-#define FLASH_STOP		0xCC
-
-/*  Function CMD */
-#define SLEEP			0x5A
-#define CLEAR_BUF		0xC3
-#define MS_RESET		0x3C
-/* -- CMD over Memory Stick */
-
-/* ++ CMD over Memory Stick Pro */
-/*  Flash CMD */
-#define PRO_READ_DATA		0x20
-#define	PRO_WRITE_DATA		0x21
-#define PRO_READ_ATRB		0x24
-#define PRO_STOP		0x25
-#define PRO_ERASE		0x26
-#define	PRO_READ_2K_DATA	0x27
-#define	PRO_WRITE_2K_DATA	0x28
-
-/*  Function CMD */
-#define PRO_FORMAT		0x10
-#define PRO_SLEEP		0x11
-/* -- CMD over Memory Stick Pro */
-
-/*  register inside memory stick */
-#define	IntReg			0x01
-#define StatusReg0		0x02
-#define StatusReg1		0x03
-
-#define SystemParm		0x10
-#define BlockAdrs		0x11
-#define CMDParm			0x14
-#define PageAdrs		0x15
-
-#define OverwriteFlag		0x16
-#define ManagemenFlag		0x17
-#define LogicalAdrs		0x18
-#define ReserveArea		0x1A
-
-/*  register inside memory pro */
-#define	Pro_IntReg		0x01
-#define Pro_StatusReg		0x02
-#define Pro_TypeReg		0x04
-#define	Pro_IFModeReg		0x05
-#define Pro_CatagoryReg		0x06
-#define Pro_ClassReg		0x07
-
-#define Pro_SystemParm		0x10
-#define Pro_DataCount1		0x11
-#define Pro_DataCount0		0x12
-#define Pro_DataAddr3		0x13
-#define Pro_DataAddr2		0x14
-#define Pro_DataAddr1		0x15
-#define Pro_DataAddr0		0x16
-
-#define Pro_TPCParm		0x17
-#define Pro_CMDParm		0x18
-
-/*  define for INT Register */
-#define	INT_REG_CED		0x80
-#define	INT_REG_ERR		0x40
-#define	INT_REG_BREQ		0x20
-#define	INT_REG_CMDNK		0x01
-
-/*  INT signal */
-#define	INT_CED			0x01
-#define	INT_ERR			0x02
-#define	INT_BREQ		0x04
-#define	INT_CMDNK		0x08
-
-/*  define for OverwriteFlag Register */
-#define	BLOCK_BOOT		0xC0
-#define	BLOCK_OK		0x80
-#define	PAGE_OK			0x60
-#define	DATA_COMPL		0x10
-
-/*  define for ManagemenFlag Register */
-#define	NOT_BOOT_BLOCK		0x4
-#define	NOT_TRANSLATION_TABLE	0x8
-
-/*  Header */
-#define	HEADER_ID0		(PPBUF_BASE2)			/* 0 */
-#define	HEADER_ID1		(PPBUF_BASE2 + 1)		/* 1 */
-/*  System Entry */
-#define	DISABLED_BLOCK0		(PPBUF_BASE2 + 0x170 + 4)	/* 2 */
-#define	DISABLED_BLOCK1		(PPBUF_BASE2 + 0x170 + 5)	/* 3 */
-#define	DISABLED_BLOCK2		(PPBUF_BASE2 + 0x170 + 6)	/* 4 */
-#define	DISABLED_BLOCK3		(PPBUF_BASE2 + 0x170 + 7)	/* 5 */
-/*  Boot & Attribute Information */
-#define	BLOCK_SIZE_0		(PPBUF_BASE2 + 0x1a0 + 2)	/* 6 */
-#define	BLOCK_SIZE_1		(PPBUF_BASE2 + 0x1a0 + 3)	/* 7 */
-#define	BLOCK_COUNT_0		(PPBUF_BASE2 + 0x1a0 + 4)	/* 8 */
-#define	BLOCK_COUNT_1		(PPBUF_BASE2 + 0x1a0 + 5)	/* 9 */
-#define	EBLOCK_COUNT_0		(PPBUF_BASE2 + 0x1a0 + 6)	/* 10 */
-#define	EBLOCK_COUNT_1		(PPBUF_BASE2 + 0x1a0 + 7)	/* 11 */
-#define	PAGE_SIZE_0		(PPBUF_BASE2 + 0x1a0 + 8)	/* 12 */
-#define	PAGE_SIZE_1		(PPBUF_BASE2 + 0x1a0 + 9)	/* 13 */
-
-/* joey 2004-08-07 for MS check Procedure */
-#define MS_Device_Type	(PPBUF_BASE2 + 0x1D8)	/* 14 */
-/* end */
-
-/* joey 2004-05-03 */
-#define	MS_4bit_Support	(PPBUF_BASE2 + 0x1D3)	/* 15 */
-/* end */
-
-#define setPS_NG	1
-#define setPS_Error	0
-
-/*  define for Pro_SystemParm Register */
-#define	PARALLEL_8BIT_IF	0x40
-#define	PARALLEL_4BIT_IF	0x00
-#define	SERIAL_IF		0x80
-
-/*  define for StatusReg0 Register */
-#define BUF_FULL	0x10
-#define BUF_EMPTY	0x20
-
-/*  define for StatusReg1 Register */
-#define	MEDIA_BUSY	0x80
-#define	FLASH_BUSY	0x40
-#define	DATA_ERROR	0x20
-#define	STS_UCDT	0x10
-#define	EXTRA_ERROR	0x08
-#define	STS_UCEX	0x04
-#define	FLAG_ERROR	0x02
-#define	STS_UCFG	0x01
-
-#define MS_SHORT_DATA_LEN	32
-
-#define FORMAT_SUCCESS		0
-#define FORMAT_FAIL		1
-#define FORMAT_IN_PROGRESS	2
-
-#define	MS_SET_BAD_BLOCK_FLG(ms_card)	((ms_card)->multi_flag |= 0x80)
-#define MS_CLR_BAD_BLOCK_FLG(ms_card)	((ms_card)->multi_flag &= 0x7F)
-#define MS_TST_BAD_BLOCK_FLG(ms_card)	((ms_card)->multi_flag & 0x80)
-
-#define CHECK_MS_TRANS_FAIL(chip, retval)	\
-	(((retval) != STATUS_SUCCESS) || \
-	(chip->rsp_buf[0] & MS_TRANSFER_ERR))
-
-void rts51x_mspro_polling_format_status(struct rts51x_chip *chip);
-void rts51x_mspro_format_sense(struct rts51x_chip *chip, unsigned int lun);
-
-int rts51x_reset_ms_card(struct rts51x_chip *chip);
-int rts51x_ms_rw(struct scsi_cmnd *srb, struct rts51x_chip *chip, u32 start_sector,
-	  u16 sector_cnt);
-int rts51x_mspro_format(struct scsi_cmnd *srb, struct rts51x_chip *chip,
-		 int short_data_len, int quick_format);
-void rts51x_ms_free_l2p_tbl(struct rts51x_chip *chip);
-void rts51x_ms_cleanup_work(struct rts51x_chip *chip);
-int rts51x_release_ms_card(struct rts51x_chip *chip);
-int rts51x_ms_delay_write(struct rts51x_chip *chip);
-
-#ifdef SUPPORT_MAGIC_GATE
-
-int ms_switch_clock(struct rts51x_chip *chip);
-int ms_write_bytes(struct rts51x_chip *chip, u8 tpc, u8 cnt, u8 cfg, u8 *data,
-		   int data_len);
-int ms_read_bytes(struct rts51x_chip *chip, u8 tpc, u8 cnt, u8 cfg, u8 *data,
-		  int data_len);
-int ms_set_rw_reg_addr(struct rts51x_chip *chip, u8 read_start, u8 read_cnt,
-		       u8 write_start, u8 write_cnt);
-int ms_transfer_data(struct rts51x_chip *chip, u8 trans_mode, u8 tpc,
-		     u16 sec_cnt, u8 cfg, int mode_2k, int use_sg, void *buf,
-		     int buf_len);
-#endif
-
-#endif /* __RTS51X_MS_H */
diff --git a/drivers/staging/rts5139/ms_mg.c b/drivers/staging/rts5139/ms_mg.c
deleted file mode 100644
index 00862c1a36d..00000000000
--- a/drivers/staging/rts5139/ms_mg.c
+++ /dev/null
@@ -1,643 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#include <linux/blkdev.h>
-#include <linux/kthread.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
-
-#include "debug.h"
-#include "trace.h"
-#include "rts51x.h"
-#include "rts51x_transport.h"
-#include "rts51x_scsi.h"
-#include "rts51x_card.h"
-#include "ms.h"
-#include "ms_mg.h"
-
-#ifdef SUPPORT_MAGIC_GATE
-
-static int mg_check_int_error(struct rts51x_chip *chip)
-{
-	u8 value;
-
-	rts51x_read_register(chip, MS_TRANS_CFG, &value);
-	if (value & (INT_ERR | INT_CMDNK))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	return STATUS_SUCCESS;
-}
-
-static int mg_send_ex_cmd(struct rts51x_chip *chip, u8 cmd, u8 entry_num)
-{
-	int retval, i;
-	u8 data[8];
-
-	data[0] = cmd;
-	data[1] = 0;
-	data[2] = 0;
-	data[3] = 0;
-	data[4] = 0;
-	data[5] = 0;
-	data[6] = entry_num;
-	data[7] = 0;
-
-	for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
-		retval =
-		    ms_write_bytes(chip, PRO_EX_SET_CMD, 7, WAIT_INT, data, 8);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (i == MS_MAX_RETRY_COUNT)
-		TRACE_RET(chip, STATUS_FAIL);
-	retval = mg_check_int_error(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	return STATUS_SUCCESS;
-}
-
-int mg_set_tpc_para_sub(struct rts51x_chip *chip, int type, u8 mg_entry_num)
-{
-	int retval;
-	u8 buf[6];
-
-	RTS51X_DEBUGP("--%s--\n", __func__);
-
-	if (type == 0)
-		retval = ms_set_rw_reg_addr(chip, 0, 0, Pro_TPCParm, 1);
-	else
-		retval = ms_set_rw_reg_addr(chip, 0, 0, Pro_DataCount1, 6);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	buf[0] = 0;
-	buf[1] = 0;
-	if (type == 1) {
-		buf[2] = 0;
-		buf[3] = 0;
-		buf[4] = 0;
-		buf[5] = mg_entry_num;
-	}
-	retval =
-	    ms_write_bytes(chip, PRO_WRITE_REG, (type == 0) ? 1 : 6,
-			   NO_WAIT_INT, buf, 6);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-/**
-  * Get MagciGate ID and set Leaf ID to medium.
-
-  * After receiving this SCSI command, adapter shall fulfill 2 tasks
-  * below in order:
-  * 1. send GET_ID TPC command to get MagicGate ID and hold it till
-  * Response&challenge CMD.
-  * 2. send SET_ID TPC command to medium with Leaf ID released by host
-  * in this SCSI CMD.
-  */
-int rts51x_mg_set_leaf_id(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	int retval;
-	int i;
-	unsigned int lun = SCSI_LUN(srb);
-	u8 buf1[32], buf2[12];
-
-	RTS51X_DEBUGP("--%s--\n", __func__);
-
-	if (scsi_bufflen(srb) < 12) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-	rts51x_ms_cleanup_work(chip);
-
-	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = mg_send_ex_cmd(chip, MG_SET_LID, 0);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
-		TRACE_RET(chip, retval);
-	}
-
-	memset(buf1, 0, 32);
-	rts51x_get_xfer_buf(buf2, min(12, (int)scsi_bufflen(srb)), srb);
-	for (i = 0; i < 8; i++)
-		buf1[8 + i] = buf2[4 + i];
-	retval =
-	    ms_write_bytes(chip, PRO_WRITE_SHORT_DATA, 32, WAIT_INT, buf1, 32);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
-		TRACE_RET(chip, retval);
-	}
-	retval = mg_check_int_error(chip);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
-		TRACE_RET(chip, retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-/**
-  * Send Local EKB to host.
-
-  * After receiving this SCSI command, adapter shall read the divided
-  * data(1536 bytes totally) from medium by using READ_LONG_DATA TPC
-  * for 3 times, and report data to host with data-length is 1052 bytes.
-  */
-int rts51x_mg_get_local_EKB(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	int retval = STATUS_FAIL;
-	int bufflen;
-	unsigned int lun = SCSI_LUN(srb);
-	u8 *buf = NULL;
-
-	RTS51X_DEBUGP("--%s--\n", __func__);
-
-	rts51x_ms_cleanup_work(chip);
-
-	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	buf = kmalloc(1540, GFP_KERNEL);
-	if (!buf)
-		TRACE_RET(chip, STATUS_NOMEM);
-
-	buf[0] = 0x04;
-	buf[1] = 0x1A;
-	buf[2] = 0x00;
-	buf[3] = 0x00;
-
-	retval = mg_send_ex_cmd(chip, MG_GET_LEKB, 0);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
-		TRACE_GOTO(chip, GetEKBFinish);
-	}
-
-	retval = ms_transfer_data(chip, MS_TM_AUTO_READ, PRO_READ_LONG_DATA,
-				  3, WAIT_INT, 0, 0, buf + 4, 1536);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
-		rts51x_write_register(chip, CARD_STOP, MS_STOP | MS_CLR_ERR,
-				      MS_STOP | MS_CLR_ERR);
-		TRACE_GOTO(chip, GetEKBFinish);
-	}
-	retval = mg_check_int_error(chip);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
-		TRACE_GOTO(chip, GetEKBFinish);
-	}
-
-	bufflen = min(1052, (int)scsi_bufflen(srb));
-	rts51x_set_xfer_buf(buf, bufflen, srb);
-
-GetEKBFinish:
-	kfree(buf);
-	return retval;
-}
-
-/**
-  * Send challenge(host) to medium.
-
-  * After receiving this SCSI command, adapter shall sequentially issues
-  * TPC commands to the medium for writing 8-bytes data as challenge
-  * by host within a short data packet.
-  */
-int rts51x_mg_chg(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-	int bufflen;
-	int i;
-	unsigned int lun = SCSI_LUN(srb);
-	u8 buf[32], tmp;
-
-	RTS51X_DEBUGP("--%s--\n", __func__);
-
-	rts51x_ms_cleanup_work(chip);
-
-	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = mg_send_ex_cmd(chip, MG_GET_ID, 0);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
-		TRACE_RET(chip, retval);
-	}
-
-	retval =
-	    ms_read_bytes(chip, PRO_READ_SHORT_DATA, 32, WAIT_INT, buf, 32);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
-		TRACE_RET(chip, retval);
-	}
-	retval = mg_check_int_error(chip);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
-		TRACE_RET(chip, retval);
-	}
-
-	memcpy(ms_card->magic_gate_id, buf, 16);
-
-	for (i = 0; i < 2500; i++) {
-		RTS51X_READ_REG(chip, MS_TRANS_CFG, &tmp);
-		if (tmp &
-		    (MS_INT_CED | MS_INT_CMDNK | MS_INT_BREQ | MS_INT_ERR))
-			break;
-
-		wait_timeout(1);
-	}
-
-	if (i == 2500) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	retval = mg_send_ex_cmd(chip, MG_SET_RD, 0);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
-		TRACE_RET(chip, retval);
-	}
-
-	bufflen = min(12, (int)scsi_bufflen(srb));
-	rts51x_get_xfer_buf(buf, bufflen, srb);
-
-	for (i = 0; i < 8; i++)
-		buf[i] = buf[4 + i];
-	for (i = 0; i < 24; i++)
-		buf[8 + i] = 0;
-	retval =
-	    ms_write_bytes(chip, PRO_WRITE_SHORT_DATA, 32, WAIT_INT, buf, 32);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
-		TRACE_RET(chip, retval);
-	}
-	retval = mg_check_int_error(chip);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
-		TRACE_RET(chip, retval);
-	}
-
-	ms_card->mg_auth = 0;
-
-	return STATUS_SUCCESS;
-}
-
-/**
-  * Send Response and Challenge data  to host.
-
-  * After receiving this SCSI command, adapter shall communicates with
-  * the medium, get parameters(HRd, Rms, MagicGateID) by using READ_SHORT_DATA
-  * TPC and send the data to host according to certain format required by
-  * MG-R specification.
-  * The paremeter MagicGateID is the one that adapter has obtained from
-  * the medium by TPC commands in Set Leaf ID command phase previously.
-  */
-int rts51x_mg_get_rsp_chg(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval, i;
-	int bufflen;
-	unsigned int lun = SCSI_LUN(srb);
-	u8 buf1[32], buf2[36], tmp;
-
-	RTS51X_DEBUGP("--%s--\n", __func__);
-
-	rts51x_ms_cleanup_work(chip);
-
-	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = mg_send_ex_cmd(chip, MG_MAKE_RMS, 0);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
-		TRACE_RET(chip, retval);
-	}
-
-	retval =
-	    ms_read_bytes(chip, PRO_READ_SHORT_DATA, 32, WAIT_INT, buf1, 32);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
-		TRACE_RET(chip, retval);
-	}
-	retval = mg_check_int_error(chip);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
-		TRACE_RET(chip, retval);
-	}
-
-	buf2[0] = 0x00;
-	buf2[1] = 0x22;
-	buf2[2] = 0x00;
-	buf2[3] = 0x00;
-
-	memcpy(buf2 + 4, ms_card->magic_gate_id, 16);
-	memcpy(buf2 + 20, buf1, 16);
-
-	bufflen = min(36, (int)scsi_bufflen(srb));
-	rts51x_set_xfer_buf(buf2, bufflen, srb);
-
-	for (i = 0; i < 2500; i++) {
-		RTS51X_READ_REG(chip, MS_TRANS_CFG, &tmp);
-		if (tmp & (MS_INT_CED | MS_INT_CMDNK |
-				MS_INT_BREQ | MS_INT_ERR))
-			break;
-
-		wait_timeout(1);
-	}
-
-	if (i == 2500) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-/**
-  * Send response(host) to medium.
-
-  * After receiving this SCSI command, adapter shall sequentially
-  * issues TPC commands to the medium for writing 8-bytes data as
-  * challenge by host within a short data packet.
-  */
-int rts51x_mg_rsp(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-	int i;
-	int bufflen;
-	unsigned int lun = SCSI_LUN(srb);
-	u8 buf[32];
-
-	RTS51X_DEBUGP("--%s--\n", __func__);
-
-	rts51x_ms_cleanup_work(chip);
-
-	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = mg_send_ex_cmd(chip, MG_MAKE_KSE, 0);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
-		TRACE_RET(chip, retval);
-	}
-
-	bufflen = min(12, (int)scsi_bufflen(srb));
-	rts51x_get_xfer_buf(buf, bufflen, srb);
-
-	for (i = 0; i < 8; i++)
-		buf[i] = buf[4 + i];
-	for (i = 0; i < 24; i++)
-		buf[8 + i] = 0;
-	retval =
-	    ms_write_bytes(chip, PRO_WRITE_SHORT_DATA, 32, WAIT_INT, buf, 32);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
-		TRACE_RET(chip, retval);
-	}
-	retval = mg_check_int_error(chip);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
-		TRACE_RET(chip, retval);
-	}
-
-	ms_card->mg_auth = 1;
-
-	return STATUS_SUCCESS;
-}
-
-/** * Send ICV data to host.
-
-  * After receiving this SCSI command, adapter shall read the divided
-  * data(1024 bytes totally) from medium by using READ_LONG_DATA TPC
-  * for 2 times, and report data to host with data-length is 1028 bytes.
-  *
-  * Since the extra 4 bytes data is just only a prefix to original data
-  * that read from medium, so that the 4-byte data pushed into Ring buffer
-  * precedes data transmission from medium to Ring buffer by DMA mechanism
-  * in order to get maximum performance and minimum code size simultaneously.
-  */
-int rts51x_mg_get_ICV(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-	int bufflen;
-	unsigned int lun = SCSI_LUN(srb);
-	u8 *buf = NULL;
-
-	RTS51X_DEBUGP("--%s--\n", __func__);
-
-	rts51x_ms_cleanup_work(chip);
-
-	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	buf = kmalloc(1028, GFP_KERNEL);
-	if (!buf)
-		TRACE_RET(chip, STATUS_NOMEM);
-
-	buf[0] = 0x04;
-	buf[1] = 0x02;
-	buf[2] = 0x00;
-	buf[3] = 0x00;
-
-	retval = mg_send_ex_cmd(chip, MG_GET_IBD, ms_card->mg_entry_num);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-		TRACE_GOTO(chip, GetICVFinish);
-	}
-
-	retval = ms_transfer_data(chip, MS_TM_AUTO_READ, PRO_READ_LONG_DATA,
-				  2, WAIT_INT, 0, 0, buf + 4, 1024);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-		rts51x_write_register(chip, CARD_STOP, MS_STOP | MS_CLR_ERR,
-				      MS_STOP | MS_CLR_ERR);
-		TRACE_GOTO(chip, GetICVFinish);
-	}
-	retval = mg_check_int_error(chip);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-		TRACE_GOTO(chip, GetICVFinish);
-	}
-
-	bufflen = min(1028, (int)scsi_bufflen(srb));
-	rts51x_set_xfer_buf(buf, bufflen, srb);
-
-GetICVFinish:
-	kfree(buf);
-	return retval;
-}
-
-/**
-  * Send ICV data to medium.
-
-  * After receiving this SCSI command, adapter shall receive 1028 bytes
-  * and write the later 1024 bytes to medium by WRITE_LONG_DATA TPC
-  * consecutively.
-  *
-  * Since the first 4-bytes data is just only a prefix to original data
-  * that sent by host, and it should be skipped by shifting DMA pointer
-  * before writing 1024 bytes to medium.
-  */
-int rts51x_mg_set_ICV(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-	int bufflen;
-#ifdef MG_SET_ICV_SLOW
-	int i;
-#endif
-	unsigned int lun = SCSI_LUN(srb);
-	u8 *buf = NULL;
-
-	RTS51X_DEBUGP("--%s--\n", __func__);
-
-	rts51x_ms_cleanup_work(chip);
-
-	retval = ms_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	buf = kmalloc(1028, GFP_KERNEL);
-	if (!buf)
-		TRACE_RET(chip, STATUS_NOMEM);
-
-	bufflen = min(1028, (int)scsi_bufflen(srb));
-	rts51x_get_xfer_buf(buf, bufflen, srb);
-
-	retval = mg_send_ex_cmd(chip, MG_SET_IBD, ms_card->mg_entry_num);
-	if (retval != STATUS_SUCCESS) {
-		if (ms_card->mg_auth == 0) {
-			if ((buf[5] & 0xC0) != 0)
-				rts51x_set_sense_type(chip, lun,
-					SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
-			else
-				rts51x_set_sense_type(chip, lun,
-					SENSE_TYPE_MG_WRITE_ERR);
-		} else {
-			rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_WRITE_ERR);
-		}
-		TRACE_GOTO(chip, SetICVFinish);
-	}
-
-#ifdef MG_SET_ICV_SLOW
-	for (i = 0; i < 2; i++) {
-		udelay(50);
-
-		rts51x_init_cmd(chip);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF,
-			       PRO_WRITE_LONG_DATA);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF,
-			       WAIT_INT);
-
-		rts51x_trans_dma_enable(DMA_TO_DEVICE, chip, 512, DMA_512);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
-			       MS_TRANSFER_START | MS_TM_NORMAL_WRITE);
-		rts51x_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER,
-			       MS_TRANSFER_END, MS_TRANSFER_END);
-
-		retval = rts51x_send_cmd(chip, MODE_CDOR, 100);
-		if (retval != STATUS_SUCCESS) {
-			rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_WRITE_ERR);
-			TRACE_GOTO(chip, SetICVFinish);
-		}
-
-		retval = rts51x_transfer_data_rcc(chip, SND_BULK_PIPE(chip),
-						  buf + 4 + i * 512, 512, 0,
-						  NULL, 3000, STAGE_DO);
-		if (retval != STATUS_SUCCESS) {
-			rts51x_clear_ms_error(chip);
-			if (ms_card->mg_auth == 0) {
-				if ((buf[5] & 0xC0) != 0)
-					rts51x_set_sense_type(chip, lun,
-						SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
-				else
-					rts51x_set_sense_type(chip, lun,
-						SENSE_TYPE_MG_WRITE_ERR);
-			} else {
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MG_WRITE_ERR);
-			}
-			retval = STATUS_FAIL;
-			TRACE_GOTO(chip, SetICVFinish);
-		}
-
-		retval = rts51x_get_rsp(chip, 1, 3000);
-		if (CHECK_MS_TRANS_FAIL(chip, retval)
-		    || mg_check_int_error(chip)) {
-			rts51x_clear_ms_error(chip);
-			if (ms_card->mg_auth == 0) {
-				if ((buf[5] & 0xC0) != 0)
-					rts51x_set_sense_type(chip, lun,
-						SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
-				else
-					rts51x_set_sense_type(chip, lun,
-						SENSE_TYPE_MG_WRITE_ERR);
-			} else {
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MG_WRITE_ERR);
-			}
-			retval = STATUS_FAIL;
-			TRACE_GOTO(chip, SetICVFinish);
-		}
-	}
-#else
-	retval = ms_transfer_data(chip, MS_TM_AUTO_WRITE, PRO_WRITE_LONG_DATA,
-				  2, WAIT_INT, 0, 0, buf + 4, 1024);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_ms_error(chip);
-		if (ms_card->mg_auth == 0) {
-			if ((buf[5] & 0xC0) != 0)
-				rts51x_set_sense_type(chip, lun,
-					SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
-			else
-				rts51x_set_sense_type(chip, lun,
-					SENSE_TYPE_MG_WRITE_ERR);
-		} else {
-			rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_WRITE_ERR);
-		}
-		TRACE_GOTO(chip, SetICVFinish);
-	}
-#endif
-
-SetICVFinish:
-	kfree(buf);
-	return retval;
-}
-
-#endif /* SUPPORT_MAGIC_GATE */
diff --git a/drivers/staging/rts5139/ms_mg.h b/drivers/staging/rts5139/ms_mg.h
deleted file mode 100644
index d15733a992a..00000000000
--- a/drivers/staging/rts5139/ms_mg.h
+++ /dev/null
@@ -1,41 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- * Header file
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#ifndef __RTS51X_MS_MG_H
-#define __RTS51X_MS_MG_H
-
-#include "rts51x_chip.h"
-#include "ms.h"
-
-int rts51x_mg_set_leaf_id(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-int rts51x_mg_get_local_EKB(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-int rts51x_mg_chg(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-int rts51x_mg_get_rsp_chg(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-int rts51x_mg_rsp(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-int rts51x_mg_get_ICV(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-int rts51x_mg_set_ICV(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-
-#endif /* __RTS51X_MS_MG_H */
diff --git a/drivers/staging/rts5139/rts51x.c b/drivers/staging/rts5139/rts51x.c
deleted file mode 100644
index c8d06d4fddd..00000000000
--- a/drivers/staging/rts5139/rts51x.c
+++ /dev/null
@@ -1,857 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#include <linux/blkdev.h>
-#include <linux/kthread.h>
-#include <linux/sched.h>
-#include <linux/workqueue.h>
-#include <linux/errno.h>
-#include <linux/freezer.h>
-#include <linux/module.h>
-#include <linux/slab.h>
-#include <linux/mutex.h>
-#include <linux/utsname.h>
-#include <linux/usb.h>
-
-#include <scsi/scsi.h>
-#include <scsi/scsi_cmnd.h>
-#include <scsi/scsi_device.h>
-#include <scsi/scsi_devinfo.h>
-#include <scsi/scsi_eh.h>
-#include <scsi/scsi_host.h>
-
-#include "debug.h"
-#include "ms.h"
-#include "rts51x.h"
-#include "rts51x_chip.h"
-#include "rts51x_card.h"
-#include "rts51x_scsi.h"
-#include "rts51x_transport.h"
-#include "rts51x_fop.h"
-
-MODULE_DESCRIPTION(RTS51X_DESC);
-MODULE_LICENSE("GPL");
-MODULE_VERSION(DRIVER_VERSION);
-
-static int auto_delink_en;
-module_param(auto_delink_en, int, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(auto_delink_en, "enable auto delink");
-
-static int ss_en;
-module_param(ss_en, int, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(ss_en, "enable selective suspend");
-
-static int ss_delay = 50;
-module_param(ss_delay, int, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(ss_delay,
-		 "seconds to delay before entering selective suspend");
-
-static int needs_remote_wakeup;
-module_param(needs_remote_wakeup, int, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(needs_remote_wakeup, "ss state needs remote wakeup supported");
-
-#ifdef SUPPORT_FILE_OP
-static const struct file_operations rts51x_fops = {
-	.owner = THIS_MODULE,
-	.read = rts51x_read,
-	.write = rts51x_write,
-	.unlocked_ioctl = rts51x_ioctl,
-	.open = rts51x_open,
-	.release = rts51x_release,
-};
-
-/*
- * usb class driver info in order to get a minor number from the usb core,
- * and to have the device registered with the driver core
- */
-static struct usb_class_driver rts51x_class = {
-	.name = "rts51x%d",
-	.fops = &rts51x_fops,
-	.minor_base = 192,
-};
-#endif
-
-#ifdef CONFIG_PM		/* Minimal support for suspend and resume */
-
-static inline void usb_autopm_enable(struct usb_interface *intf)
-{
-	atomic_set(&intf->pm_usage_cnt, 1);
-	usb_autopm_put_interface(intf);
-}
-
-static inline void usb_autopm_disable(struct usb_interface *intf)
-{
-	atomic_set(&intf->pm_usage_cnt, 0);
-	usb_autopm_get_interface(intf);
-}
-
-static void rts51x_try_to_enter_ss(struct rts51x_chip *chip)
-{
-	RTS51X_DEBUGP("Ready to enter SS state\n");
-	usb_autopm_enable(chip->usb->pusb_intf);
-}
-
-void rts51x_try_to_exit_ss(struct rts51x_chip *chip)
-{
-	RTS51X_DEBUGP("Exit from SS state\n");
-	usb_autopm_disable(chip->usb->pusb_intf);
-}
-
-int rts51x_suspend(struct usb_interface *iface, pm_message_t message)
-{
-	struct rts51x_chip *chip = usb_get_intfdata(iface);
-
-	RTS51X_DEBUGP("%s, message.event = 0x%x\n", __func__, message.event);
-
-	/* Wait until no command is running */
-	mutex_lock(&chip->usb->dev_mutex);
-
-	chip->fake_card_ready = chip->card_ready;
-	rts51x_do_before_power_down(chip);
-
-	if (message.event == PM_EVENT_AUTO_SUSPEND) {
-		RTS51X_DEBUGP("Enter SS state");
-		chip->resume_from_scsi = 0;
-		RTS51X_SET_STAT(chip, STAT_SS);
-	} else {
-		RTS51X_DEBUGP("Enter SUSPEND state");
-		RTS51X_SET_STAT(chip, STAT_SUSPEND);
-	}
-
-	/* When runtime PM is working, we'll set a flag to indicate
-	 * whether we should autoresume when a SCSI request arrives. */
-
-	mutex_unlock(&chip->usb->dev_mutex);
-	return 0;
-}
-
-int rts51x_resume(struct usb_interface *iface)
-{
-	struct rts51x_chip *chip = usb_get_intfdata(iface);
-
-	RTS51X_DEBUGP("%s\n", __func__);
-
-	if (!RTS51X_CHK_STAT(chip, STAT_SS) || !chip->resume_from_scsi) {
-		mutex_lock(&chip->usb->dev_mutex);
-
-		if (chip->option.ss_en) {
-			if (GET_PM_USAGE_CNT(chip) <= 0) {
-				/* Remote wake up, increase pm_usage_cnt */
-				RTS51X_DEBUGP("Incr pm_usage_cnt\n");
-				SET_PM_USAGE_CNT(chip, 1);
-			}
-		}
-
-		RTS51X_SET_STAT(chip, STAT_RUN);
-
-		rts51x_init_chip(chip);
-		rts51x_init_cards(chip);
-
-		mutex_unlock(&chip->usb->dev_mutex);
-	}
-
-	return 0;
-}
-
-int rts51x_reset_resume(struct usb_interface *iface)
-{
-	struct rts51x_chip *chip = usb_get_intfdata(iface);
-
-	RTS51X_DEBUGP("%s\n", __func__);
-
-	mutex_lock(&chip->usb->dev_mutex);
-
-	RTS51X_SET_STAT(chip, STAT_RUN);
-
-	if (chip->option.ss_en)
-		SET_PM_USAGE_CNT(chip, 1);
-
-	rts51x_init_chip(chip);
-	rts51x_init_cards(chip);
-
-	mutex_unlock(&chip->usb->dev_mutex);
-
-	/* FIXME: Notify the subdrivers that they need to reinitialize
-	 * the device */
-	return 0;
-}
-
-#else /* CONFIG_PM */
-
-static void rts51x_try_to_enter_ss(struct rts51x_chip *chip)
-{
-}
-
-void rts51x_try_to_exit_ss(struct rts51x_chip *chip)
-{
-}
-
-#endif /* CONFIG_PM */
-
-/*
- * The next two routines get called just before and just after
- * a USB port reset, whether from this driver or a different one.
- */
-
-static int rts51x_pre_reset(struct usb_interface *iface)
-{
-	struct rts51x_chip *chip = usb_get_intfdata(iface);
-
-	RTS51X_DEBUGP("%s\n", __func__);
-
-	/* Make sure no command runs during the reset */
-	mutex_lock(&chip->usb->dev_mutex);
-	return 0;
-}
-
-static int rts51x_post_reset(struct usb_interface *iface)
-{
-	struct rts51x_chip *chip = usb_get_intfdata(iface);
-
-	RTS51X_DEBUGP("%s\n", __func__);
-
-	/* Report the reset to the SCSI core */
-	/* usb_stor_report_bus_reset(us); */
-
-	/* FIXME: Notify the subdrivers that they need to reinitialize
-	 * the device */
-
-	mutex_unlock(&chip->usb->dev_mutex);
-	return 0;
-}
-
-static int rts51x_control_thread(void *__chip)
-{
-	struct rts51x_chip *chip = (struct rts51x_chip *)__chip;
-	struct Scsi_Host *host = rts51x_to_host(chip);
-
-	for (;;) {
-		if (wait_for_completion_interruptible(&chip->usb->cmnd_ready))
-			break;
-
-		if (test_bit(FLIDX_DISCONNECTING, &chip->usb->dflags)) {
-			RTS51X_DEBUGP("-- exiting from rts51x-control\n");
-			break;
-		}
-
-		/* lock the device pointers */
-		mutex_lock(&(chip->usb->dev_mutex));
-
-		/* lock access to the state */
-		scsi_lock(host);
-
-		/* When we are called with no command pending, we're done */
-		if (chip->srb == NULL) {
-			scsi_unlock(host);
-			mutex_unlock(&chip->usb->dev_mutex);
-			RTS51X_DEBUGP("-- exiting from control thread\n");
-			break;
-		}
-
-		/* has the command timed out *already* ? */
-		if (test_bit(FLIDX_TIMED_OUT, &chip->usb->dflags)) {
-			chip->srb->result = DID_ABORT << 16;
-			goto SkipForAbort;
-		}
-
-		scsi_unlock(host);
-
-		/* reject the command if the direction indicator
-		 * is UNKNOWN
-		 */
-		if (chip->srb->sc_data_direction == DMA_BIDIRECTIONAL) {
-			RTS51X_DEBUGP("UNKNOWN data direction\n");
-			chip->srb->result = DID_ERROR << 16;
-		}
-
-		/* reject if target != 0 or if LUN is higher than
-		 * the maximum known LUN
-		 */
-		else if (chip->srb->device->id) {
-			RTS51X_DEBUGP("Bad target number (%d:%d)\n",
-				       chip->srb->device->id,
-				       chip->srb->device->lun);
-			chip->srb->result = DID_BAD_TARGET << 16;
-		}
-
-		else if (chip->srb->device->lun > chip->max_lun) {
-			RTS51X_DEBUGP("Bad LUN (%d:%d)\n",
-				       chip->srb->device->id,
-				       chip->srb->device->lun);
-			chip->srb->result = DID_BAD_TARGET << 16;
-		}
-
-		/* we've got a command, let's do it! */
-		else {
-			RTS51X_DEBUG(rts51x_scsi_show_command(chip->srb));
-			rts51x_invoke_transport(chip->srb, chip);
-		}
-
-		/* lock access to the state */
-		scsi_lock(host);
-
-		/* indicate that the command is done */
-		if (chip->srb->result != DID_ABORT << 16)
-			chip->srb->scsi_done(chip->srb);
-		else
-SkipForAbort :
-			RTS51X_DEBUGP("scsi command aborted\n");
-
-		/* If an abort request was received we need to signal that
-		 * the abort has finished.  The proper test for this is
-		 * the TIMED_OUT flag, not srb->result == DID_ABORT, because
-		 * the timeout might have occurred after the command had
-		 * already completed with a different result code. */
-		if (test_bit(FLIDX_TIMED_OUT, &chip->usb->dflags)) {
-			complete(&(chip->usb->notify));
-
-			/* Allow USB transfers to resume */
-			clear_bit(FLIDX_ABORTING, &chip->usb->dflags);
-			clear_bit(FLIDX_TIMED_OUT, &chip->usb->dflags);
-		}
-
-		/* finished working on this command */
-		chip->srb = NULL;
-		scsi_unlock(host);
-
-		/* unlock the device pointers */
-		mutex_unlock(&chip->usb->dev_mutex);
-	}			/* for (;;) */
-
-	complete(&chip->usb->control_exit);
-
-	/* Wait until we are told to stop */
-/*	for (;;) {
-		set_current_state(TASK_INTERRUPTIBLE);
-		if (kthread_should_stop())
-			break;
-		schedule();
-	}
-	__set_current_state(TASK_RUNNING);*/
-	return 0;
-}
-
-static int rts51x_polling_thread(void *__chip)
-{
-	struct rts51x_chip *chip = (struct rts51x_chip *)__chip;
-
-	for (;;) {
-		wait_timeout(POLLING_INTERVAL);
-
-		/* if the device has disconnected, we are free to exit */
-		if (test_bit(FLIDX_DISCONNECTING, &chip->usb->dflags)) {
-			RTS51X_DEBUGP("-- exiting from rts51x-polling\n");
-			break;
-		}
-
-		/* if the device has disconnected, we are free to exit */
-		/* if (kthread_should_stop()) {
-			printk(KERN_INFO "Stop polling thread!\n");
-			break;
-		} */
-
-#ifdef CONFIG_PM
-		if (RTS51X_CHK_STAT(chip, STAT_SS) ||
-		    RTS51X_CHK_STAT(chip, STAT_SS_PRE) ||
-		    RTS51X_CHK_STAT(chip, STAT_SUSPEND)) {
-			continue;
-		}
-
-		if (ss_en) {
-			if (RTS51X_CHK_STAT(chip, STAT_IDLE)) {
-				if (chip->ss_counter <
-				    (ss_delay * 1000 / POLLING_INTERVAL)) {
-					chip->ss_counter++;
-				} else {
-					/* Prepare SS state */
-					RTS51X_SET_STAT(chip, STAT_SS_PRE);
-					rts51x_try_to_enter_ss(chip);
-					continue;
-				}
-			} else {
-				chip->ss_counter = 0;
-			}
-		}
-#endif
-
-		rts51x_mspro_polling_format_status(chip);
-
-		/* lock the device pointers */
-		mutex_lock(&(chip->usb->dev_mutex));
-
-		rts51x_polling_func(chip);
-
-		/* unlock the device pointers */
-		mutex_unlock(&chip->usb->dev_mutex);
-	}			/* for (;;) */
-
-	complete(&chip->usb->polling_exit);
-
-	/* Wait until we are told to stop */
-	/* for (;;) {
-		set_current_state(TASK_INTERRUPTIBLE);
-		if (kthread_should_stop())
-		break;
-		schedule();
-		}
-	__set_current_state(TASK_RUNNING); */
-	return 0;
-}
-
-/* Associate our private data with the USB device */
-static int associate_dev(struct rts51x_chip *chip, struct usb_interface *intf)
-{
-	struct rts51x_usb *rts51x = chip->usb;
-#ifdef SUPPORT_FILE_OP
-	int retval;
-#endif
-
-	/* Fill in the device-related fields */
-	rts51x->pusb_dev = interface_to_usbdev(intf);
-	rts51x->pusb_intf = intf;
-	rts51x->ifnum = intf->cur_altsetting->desc.bInterfaceNumber;
-	RTS51X_DEBUGP("Vendor: 0x%04x, Product: 0x%04x, Revision: 0x%04x\n",
-		       le16_to_cpu(rts51x->pusb_dev->descriptor.idVendor),
-		       le16_to_cpu(rts51x->pusb_dev->descriptor.idProduct),
-		       le16_to_cpu(rts51x->pusb_dev->descriptor.bcdDevice));
-	RTS51X_DEBUGP("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, chip);
-
-#ifdef SUPPORT_FILE_OP
-	/* we can register the device now, as it is ready */
-	retval = usb_register_dev(intf, &rts51x_class);
-	if (retval) {
-		/* something prevented us from registering this driver */
-		RTS51X_DEBUGP("Not able to get a minor for this device.");
-		usb_set_intfdata(intf, NULL);
-		return -ENOMEM;
-	}
-#endif
-
-	/* Allocate the device-related DMA-mapped buffers */
-	rts51x->cr = usb_buffer_alloc(rts51x->pusb_dev, sizeof(*rts51x->cr),
-				      GFP_KERNEL, &rts51x->cr_dma);
-	if (!rts51x->cr) {
-		RTS51X_DEBUGP("usb_ctrlrequest allocation failed\n");
-		usb_set_intfdata(intf, NULL);
-		return -ENOMEM;
-	}
-
-	rts51x->iobuf = usb_buffer_alloc(rts51x->pusb_dev, RTS51X_IOBUF_SIZE,
-					 GFP_KERNEL, &rts51x->iobuf_dma);
-	if (!rts51x->iobuf) {
-		RTS51X_DEBUGP("I/O buffer allocation failed\n");
-		usb_set_intfdata(intf, NULL);
-		return -ENOMEM;
-	}
-	return 0;
-}
-
-static void rts51x_init_options(struct rts51x_chip *chip)
-{
-	struct rts51x_option *option = &(chip->option);
-
-	option->rts51x_mspro_formatter_enable = 1;
-
-	option->fpga_sd_sdr104_clk = CLK_100;
-	option->fpga_sd_sdr50_clk = CLK_100;
-	option->fpga_sd_ddr50_clk = CLK_100;
-	option->fpga_sd_hs_clk = CLK_100;
-	option->fpga_mmc_52m_clk = CLK_80;
-	option->fpga_ms_hg_clk = CLK_80;
-	option->fpga_ms_4bit_clk = CLK_80;
-
-	option->asic_sd_sdr104_clk = 98;
-	option->asic_sd_sdr50_clk = 98;
-	option->asic_sd_ddr50_clk = 98;
-	option->asic_sd_hs_clk = 97;
-	option->asic_mmc_52m_clk = 95;
-	option->asic_ms_hg_clk = 116;
-	option->asic_ms_4bit_clk = 77;
-
-	option->sd_ddr_tx_phase = 0;
-	option->mmc_ddr_tx_phase = 1;
-
-	option->sd_speed_prior = 0;
-	option->sd_ctl =
-	    SD_PUSH_POINT_AUTO | SD_SAMPLE_POINT_AUTO | SUPPORT_UHS50_MMC44;
-
-	option->ss_en = ss_en;
-	option->ss_delay = ss_delay;
-
-	option->auto_delink_en = auto_delink_en;
-
-	option->FT2_fast_mode = 0;
-	option->pwr_delay = 800;
-	option->rts51x_xd_rw_step = 0;
-	option->D3318_off_delay = 50;
-	option->delink_delay = 100;
-	option->rts5129_D3318_off_enable = 0;
-	option->sd20_pad_drive = 0;
-	option->reset_or_rw_fail_set_pad_drive = 1;
-	option->debounce_num = 2;
-	option->led_toggle_interval = 6;
-	option->rts51x_xd_rwn_step = 0;
-	option->sd_send_status_en = 0;
-	option->sdr50_tx_phase = 0x01;
-	option->sdr50_rx_phase = 0x05;
-	option->ddr50_tx_phase = 0x09;
-	option->ddr50_rx_phase = 0x06;
-	option->sdr50_phase_sel = 0;
-	option->sd30_pad_drive = 1;
-	option->ms_errreg_fix = 0;
-	option->reset_mmc_first = 0;
-	option->speed_mmc = 1;
-	option->led_always_on = 0;
-}
-
-/* Get the pipe settings */
-static int get_pipes(struct rts51x_chip *chip)
-{
-	struct rts51x_usb *rts51x = chip->usb;
-	struct usb_host_interface *altsetting =
-	    rts51x->pusb_intf->cur_altsetting;
-	int i;
-	struct usb_endpoint_descriptor *ep;
-	struct usb_endpoint_descriptor *ep_in = NULL;
-	struct usb_endpoint_descriptor *ep_out = NULL;
-	struct usb_endpoint_descriptor *ep_int = NULL;
-
-	/*
-	 * Find the first endpoint of each type we need.
-	 * We are expecting a minimum of 2 endpoints - in and out (bulk).
-	 * An optional interrupt-in is OK (necessary for CBI protocol).
-	 * We will ignore any others.
-	 */
-	for (i = 0; i < altsetting->desc.bNumEndpoints; i++) {
-		ep = &altsetting->endpoint[i].desc;
-
-		if (usb_endpoint_xfer_bulk(ep)) {
-			if (usb_endpoint_dir_in(ep)) {
-				if (!ep_in)
-					ep_in = ep;
-			} else {
-				if (!ep_out)
-					ep_out = ep;
-			}
-		}
-
-		else if (usb_endpoint_is_int_in(ep)) {
-			if (!ep_int)
-				ep_int = ep;
-		}
-	}
-
-	if (!ep_in || !ep_out) {
-		RTS51X_DEBUGP("Endpoint sanity check failed!"
-					"Rejecting dev.\n");
-		return -EIO;
-	}
-
-	/* Calculate and store the pipe values */
-	rts51x->send_ctrl_pipe = usb_sndctrlpipe(rts51x->pusb_dev, 0);
-	rts51x->recv_ctrl_pipe = usb_rcvctrlpipe(rts51x->pusb_dev, 0);
-	rts51x->send_bulk_pipe = usb_sndbulkpipe(rts51x->pusb_dev,
-						 usb_endpoint_num(ep_out));
-	rts51x->recv_bulk_pipe = usb_rcvbulkpipe(rts51x->pusb_dev,
-						 usb_endpoint_num(ep_in));
-	if (ep_int) {
-		rts51x->recv_intr_pipe = usb_rcvintpipe(rts51x->pusb_dev,
-							usb_endpoint_num
-							(ep_int));
-		rts51x->ep_bInterval = ep_int->bInterval;
-	}
-	return 0;
-}
-
-/* Initialize all the dynamic resources we need */
-static int rts51x_acquire_resources(struct rts51x_chip *chip)
-{
-	struct rts51x_usb *rts51x = chip->usb;
-	int retval;
-
-	rts51x->current_urb = usb_alloc_urb(0, GFP_KERNEL);
-	if (!rts51x->current_urb) {
-		RTS51X_DEBUGP("URB allocation failed\n");
-		return -ENOMEM;
-	}
-
-	rts51x->intr_urb = usb_alloc_urb(0, GFP_KERNEL);
-	if (!rts51x->intr_urb) {
-		RTS51X_DEBUGP("URB allocation failed\n");
-		return -ENOMEM;
-	}
-
-	chip->cmd_buf = chip->rsp_buf = rts51x->iobuf;
-
-	rts51x_init_options(chip);
-
-	/* Init rts51xx device */
-	retval = rts51x_init_chip(chip);
-	if (retval != STATUS_SUCCESS)
-		return -EIO;
-
-	return 0;
-}
-
-/* Release all our dynamic resources */
-static void rts51x_release_resources(struct rts51x_chip *chip)
-{
-	RTS51X_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.
-	 */
-	RTS51X_DEBUGP("-- sending exit command to thread\n");
-	complete(&chip->usb->cmnd_ready);
-	if (chip->usb->ctl_thread)
-		wait_for_completion(&chip->usb->control_exit);
-		/* kthread_stop(chip->usb->ctl_thread); */
-	if (chip->usb->polling_thread)
-		wait_for_completion(&chip->usb->polling_exit);
-
-	/* if (chip->usb->polling_thread)
-		kthread_stop(chip->usb->polling_thread); */
-
-	wait_timeout(200);
-
-	/* Release rts51xx device here */
-	rts51x_release_chip(chip);
-
-	usb_free_urb(chip->usb->current_urb);
-	usb_free_urb(chip->usb->intr_urb);
-}
-
-/* Dissociate from the USB device */
-static void dissociate_dev(struct rts51x_chip *chip)
-{
-	struct rts51x_usb *rts51x = chip->usb;
-
-	RTS51X_DEBUGP("-- %s\n", __func__);
-
-	/* Free the device-related DMA-mapped buffers */
-	if (rts51x->cr)
-		usb_buffer_free(rts51x->pusb_dev, sizeof(*rts51x->cr),
-				rts51x->cr, rts51x->cr_dma);
-	if (rts51x->iobuf)
-		usb_buffer_free(rts51x->pusb_dev, RTS51X_IOBUF_SIZE,
-				rts51x->iobuf, rts51x->iobuf_dma);
-
-	/* Remove our private data from the interface */
-	usb_set_intfdata(rts51x->pusb_intf, NULL);
-
-#ifdef SUPPORT_FILE_OP
-	/* give back our minor */
-	usb_deregister_dev(rts51x->pusb_intf, &rts51x_class);
-#endif
-
-	kfree(rts51x);
-	chip->usb = NULL;
-}
-
-/* First stage of disconnect processing: stop SCSI scanning,
- * remove the host, and stop accepting new commands
- */
-static void quiesce_and_remove_host(struct rts51x_chip *chip)
-{
-	struct rts51x_usb *rts51x = chip->usb;
-	struct Scsi_Host *host = rts51x_to_host(chip);
-
-	/* If the device is really gone, cut short reset delays */
-	if (rts51x->pusb_dev->state == USB_STATE_NOTATTACHED)
-		set_bit(FLIDX_DISCONNECTING, &rts51x->dflags);
-
-	/* Removing the host will perform an orderly shutdown: caches
-	 * synchronized, disks spun down, etc.
-	 */
-	scsi_remove_host(host);
-
-	/* Prevent any new commands from being accepted and cut short
-	 * reset delays.
-	 */
-	scsi_lock(host);
-	set_bit(FLIDX_DISCONNECTING, &rts51x->dflags);
-	scsi_unlock(host);
-}
-
-/* Second stage of disconnect processing: deallocate all resources */
-static void release_everything(struct rts51x_chip *chip)
-{
-	rts51x_release_resources(chip);
-	dissociate_dev(chip);
-
-	/* Drop our reference to the host; the SCSI core will free it
-	 * (and "chip" along with it) when the refcount becomes 0. */
-	scsi_host_put(rts51x_to_host(chip));
-}
-
-static int rts51x_probe(struct usb_interface *intf,
-			const struct usb_device_id *id)
-{
-	struct Scsi_Host *host;
-	struct rts51x_chip *chip;
-	struct rts51x_usb *rts51x;
-	int result;
-	struct task_struct *th;
-
-	RTS51X_DEBUGP("%s detected\n", RTS51X_NAME);
-
-	rts51x = kzalloc(sizeof(struct rts51x_usb), GFP_KERNEL);
-	if (!rts51x) {
-		printk(KERN_WARNING RTS51X_TIP
-		       "Unable to allocate rts51x_usb\n");
-		return -ENOMEM;
-	}
-
-	/*
-	 * Ask the SCSI layer to allocate a host structure, with extra
-	 * space at the end for our private us_data structure.
-	 */
-	host = scsi_host_alloc(&rts51x_host_template, sizeof(*chip));
-	if (!host) {
-		printk(KERN_WARNING RTS51X_TIP
-		       "Unable to allocate the scsi host\n");
-		kfree(rts51x);
-		return -ENOMEM;
-	}
-
-	/*
-	 * Allow 16-byte CDBs and thus > 2TB
-	 */
-	host->max_cmd_len = 16;
-	chip = host_to_rts51x(host);
-	memset(chip, 0, sizeof(struct rts51x_chip));
-
-	chip->vendor_id = id->idVendor;
-	chip->product_id = id->idProduct;
-
-	mutex_init(&(rts51x->dev_mutex));
-	init_completion(&rts51x->cmnd_ready);
-	init_completion(&rts51x->control_exit);
-	init_completion(&rts51x->polling_exit);
-	init_completion(&(rts51x->notify));
-
-	chip->usb = rts51x;
-
-	/* Associate the us_data structure with the USB device */
-	result = associate_dev(chip, intf);
-	if (result)
-		goto BadDevice;
-
-	/* Find the endpoints and calculate pipe values */
-	result = get_pipes(chip);
-	if (result)
-		goto BadDevice;
-
-	/* Acquire all the other resources and add the host */
-	result = rts51x_acquire_resources(chip);
-	if (result)
-		goto BadDevice;
-
-	/* Start up our control thread */
-	th = kthread_run(rts51x_control_thread, chip, RTS51X_CTL_THREAD);
-	if (IS_ERR(th)) {
-		printk(KERN_WARNING RTS51X_TIP
-		       "Unable to start control thread\n");
-		result = PTR_ERR(th);
-		goto BadDevice;
-	}
-	rts51x->ctl_thread = th;
-
-	result = scsi_add_host(rts51x_to_host(chip), &rts51x->pusb_intf->dev);
-	if (result) {
-		printk(KERN_WARNING RTS51X_TIP "Unable to add the scsi host\n");
-		goto BadDevice;
-	}
-	scsi_scan_host(rts51x_to_host(chip));
-
-	/* Start up our polling thread */
-	th = kthread_run(rts51x_polling_thread, chip, RTS51X_POLLING_THREAD);
-	if (IS_ERR(th)) {
-		printk(KERN_WARNING RTS51X_TIP
-		       "Unable to start polling thread\n");
-		result = PTR_ERR(th);
-		goto BadDevice;
-	}
-	rts51x->polling_thread = th;
-
-#ifdef CONFIG_PM
-	if (ss_en) {
-		rts51x->pusb_intf->needs_remote_wakeup = needs_remote_wakeup;
-		SET_PM_USAGE_CNT(chip, 1);
-		RTS51X_DEBUGP("pm_usage_cnt = %d\n", GET_PM_USAGE_CNT(chip));
-	}
-#endif
-
-	return 0;
-
-	/* We come here if there are any problems */
-BadDevice:
-	RTS51X_DEBUGP("rts51x_probe() failed\n");
-	release_everything(chip);
-	return result;
-}
-
-static void rts51x_disconnect(struct usb_interface *intf)
-{
-	struct rts51x_chip *chip = (struct rts51x_chip *)usb_get_intfdata(intf);
-
-	RTS51X_DEBUGP("rts51x_disconnect() called\n");
-	quiesce_and_remove_host(chip);
-	release_everything(chip);
-}
-
-/***********************************************************************
- * Initialization and registration
- ***********************************************************************/
-
-static struct usb_device_id rts5139_usb_ids[] = {
-	{USB_DEVICE(0x0BDA, 0x0139)},
-	{USB_DEVICE(0x0BDA, 0x0129)},
-	{}			/* Terminating entry */
-};
-EXPORT_SYMBOL_GPL(rts5139_usb_ids);
-
-MODULE_DEVICE_TABLE(usb, rts5139_usb_ids);
-
-struct usb_driver rts51x_driver = {
-	.name = RTS51X_NAME,
-	.probe = rts51x_probe,
-	.disconnect = rts51x_disconnect,
-	.suspend = rts51x_suspend,
-	.resume = rts51x_resume,
-	.reset_resume = rts51x_reset_resume,
-	.pre_reset = rts51x_pre_reset,
-	.post_reset = rts51x_post_reset,
-	.id_table = rts5139_usb_ids,
-	.soft_unbind = 1,
-};
-
-module_usb_driver(rts51x_driver);
diff --git a/drivers/staging/rts5139/rts51x.h b/drivers/staging/rts5139/rts51x.h
deleted file mode 100644
index ecc0109a5b1..00000000000
--- a/drivers/staging/rts5139/rts51x.h
+++ /dev/null
@@ -1,194 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- * Header file
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#ifndef __RTS51X_H
-#define __RTS51X_H
-
-#include <linux/usb.h>
-#include <linux/usb_usual.h>
-#include <linux/blkdev.h>
-#include <linux/completion.h>
-#include <linux/mutex.h>
-#include <linux/cdrom.h>
-#include <linux/kernel.h>
-
-#include <scsi/scsi.h>
-#include <scsi/scsi_cmnd.h>
-#include <scsi/scsi_device.h>
-#include <scsi/scsi_devinfo.h>
-#include <scsi/scsi_eh.h>
-#include <scsi/scsi_host.h>
-
-#define DRIVER_VERSION		"v1.04"
-
-#define RTS51X_DESC		"Realtek RTS5139/29 USB card reader driver"
-#define RTS51X_NAME		"rts5139"
-#define RTS51X_CTL_THREAD	"rts5139-control"
-#define RTS51X_POLLING_THREAD	"rts5139-polling"
-
-#define POLLING_IN_THREAD
-#define SUPPORT_FILE_OP
-
-#define wait_timeout_x(task_state, msecs)	\
-do {						\
-	set_current_state((task_state));	\
-	schedule_timeout((msecs) * HZ / 1000);	\
-} while (0)
-
-#define wait_timeout(msecs)	wait_timeout_x(TASK_INTERRUPTIBLE, (msecs))
-
-#define SCSI_LUN(srb)		((srb)->device->lun)
-
-/* Size of the DMA-mapped I/O buffer */
-#define RTS51X_IOBUF_SIZE	1024
-
-/* Dynamic bitflag definitions (dflags): used in set_bit() etc. */
-#define FLIDX_URB_ACTIVE	0	/* current_urb is in use    */
-#define FLIDX_SG_ACTIVE		1	/* current_sg is in use     */
-#define FLIDX_ABORTING		2	/* abort is in progress     */
-#define FLIDX_DISCONNECTING	3	/* disconnect in progress   */
-#define FLIDX_RESETTING		4	/* device reset in progress */
-#define FLIDX_TIMED_OUT		5	/* SCSI midlayer timed out  */
-
-struct rts51x_chip;
-
-struct rts51x_usb {
-	/* The device we're working with
-	 * It's important to note:
-	 *    (o) you must hold dev_mutex to change pusb_dev
-	 */
-	struct mutex dev_mutex;	/* protect pusb_dev */
-	struct usb_device *pusb_dev;	/* this usb_device */
-	struct usb_interface *pusb_intf;	/* this interface */
-
-	unsigned long dflags;	/* dynamic atomic bitflags */
-
-	unsigned int send_bulk_pipe;	/* cached pipe values */
-	unsigned int recv_bulk_pipe;
-	unsigned int send_ctrl_pipe;
-	unsigned int recv_ctrl_pipe;
-	unsigned int recv_intr_pipe;
-
-	u8 ifnum;		/* interface number   */
-	u8 ep_bInterval;	/* interrupt interval */
-
-	/* control and bulk communications data */
-	struct urb *current_urb;	/* USB requests         */
-	struct urb *intr_urb;	/* Interrupt USB request */
-	struct usb_ctrlrequest *cr;	/* control requests     */
-	struct usb_sg_request current_sg;	/* scatter-gather req.  */
-	unsigned char *iobuf;	/* I/O buffer           */
-	dma_addr_t cr_dma;	/* buffer DMA addresses */
-	dma_addr_t iobuf_dma;
-	struct task_struct *ctl_thread;	/* the control thread   */
-	struct task_struct *polling_thread;	/* the polling thread   */
-
-	/* mutual exclusion and synchronization structures */
-	struct completion cmnd_ready;	/* to sleep thread on      */
-	struct completion control_exit;	/* control thread exit     */
-	struct completion polling_exit;	/* polling thread exit     */
-	struct completion notify;	/* thread begin/end        */
-};
-
-extern struct usb_driver rts51x_driver;
-
-static inline void get_current_time(u8 *timeval_buf, int buf_len)
-{
-	struct timeval tv;
-
-	if (!timeval_buf || (buf_len < 8))
-		return;
-
-	do_gettimeofday(&tv);
-
-	timeval_buf[0] = (u8) (tv.tv_sec >> 24);
-	timeval_buf[1] = (u8) (tv.tv_sec >> 16);
-	timeval_buf[2] = (u8) (tv.tv_sec >> 8);
-	timeval_buf[3] = (u8) (tv.tv_sec);
-	timeval_buf[4] = (u8) (tv.tv_usec >> 24);
-	timeval_buf[5] = (u8) (tv.tv_usec >> 16);
-	timeval_buf[6] = (u8) (tv.tv_usec >> 8);
-	timeval_buf[7] = (u8) (tv.tv_usec);
-}
-
-#define SND_CTRL_PIPE(chip)	((chip)->usb->send_ctrl_pipe)
-#define RCV_CTRL_PIPE(chip)	((chip)->usb->recv_ctrl_pipe)
-#define SND_BULK_PIPE(chip)	((chip)->usb->send_bulk_pipe)
-#define RCV_BULK_PIPE(chip)	((chip)->usb->recv_bulk_pipe)
-#define RCV_INTR_PIPE(chip)	((chip)->usb->recv_intr_pipe)
-
-/* The scsi_lock() and scsi_unlock() macros protect the sm_state and the
- * single queue element srb for write access */
-#define scsi_unlock(host)	spin_unlock_irq(host->host_lock)
-#define scsi_lock(host)		spin_lock_irq(host->host_lock)
-
-#define GET_PM_USAGE_CNT(chip)	\
-	atomic_read(&((chip)->usb->pusb_intf->pm_usage_cnt))
-#define SET_PM_USAGE_CNT(chip, cnt)	\
-	atomic_set(&((chip)->usb->pusb_intf->pm_usage_cnt), (cnt))
-
-/* Compatible macros while we switch over */
-static inline void *usb_buffer_alloc(struct usb_device *dev, size_t size,
-				     gfp_t mem_flags, dma_addr_t *dma)
-{
-	return usb_alloc_coherent(dev, size, mem_flags, dma);
-}
-
-static inline void usb_buffer_free(struct usb_device *dev, size_t size,
-				   void *addr, dma_addr_t dma)
-{
-	return usb_free_coherent(dev, size, addr, dma);
-}
-
-/* Convert between us_data and the corresponding Scsi_Host */
-static inline struct Scsi_Host *rts51x_to_host(struct rts51x_chip *chip)
-{
-	return container_of((void *)chip, struct Scsi_Host, hostdata);
-}
-
-static inline struct rts51x_chip *host_to_rts51x(struct Scsi_Host *host)
-{
-	return (struct rts51x_chip *)(host->hostdata);
-}
-
-/* struct scsi_cmnd transfer buffer access utilities */
-enum xfer_buf_dir { TO_XFER_BUF, FROM_XFER_BUF };
-
-/* General routines provided by the usb-storage standard core */
-#ifdef CONFIG_PM
-void rts51x_try_to_exit_ss(struct rts51x_chip *chip);
-int rts51x_suspend(struct usb_interface *iface, pm_message_t message);
-int rts51x_resume(struct usb_interface *iface);
-int rts51x_reset_resume(struct usb_interface *iface);
-#else
-#define rts51x_suspend		NULL
-#define rts51x_resume		NULL
-#define rts51x_reset_resume	NULL
-#endif
-
-extern struct scsi_host_template rts51x_host_template;
-
-#endif /* __RTS51X_H */
diff --git a/drivers/staging/rts5139/rts51x_card.c b/drivers/staging/rts5139/rts51x_card.c
deleted file mode 100644
index 03456d9873e..00000000000
--- a/drivers/staging/rts5139/rts51x_card.c
+++ /dev/null
@@ -1,940 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#include <linux/blkdev.h>
-#include <linux/kthread.h>
-#include <linux/sched.h>
-#include <linux/workqueue.h>
-
-#include <scsi/scsi.h>
-#include <scsi/scsi_eh.h>
-#include <scsi/scsi_device.h>
-
-#include "debug.h"
-#include "rts51x.h"
-#include "rts51x_chip.h"
-#include "rts51x_card.h"
-#include "rts51x_transport.h"
-#include "xd.h"
-#include "sd.h"
-#include "ms.h"
-
-void rts51x_do_remaining_work(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	struct xd_info *xd_card = &(chip->xd_card);
-	struct ms_info *ms_card = &(chip->ms_card);
-
-	if (chip->card_ready & SD_CARD) {
-		if (sd_card->seq_mode) {
-			RTS51X_SET_STAT(chip, STAT_RUN);
-			sd_card->counter++;
-		} else {
-			sd_card->counter = 0;
-		}
-	}
-
-	if (chip->card_ready & XD_CARD) {
-		if (xd_card->delay_write.delay_write_flag) {
-			RTS51X_SET_STAT(chip, STAT_RUN);
-			xd_card->counter++;
-		} else {
-			xd_card->counter = 0;
-		}
-	}
-
-	if (chip->card_ready & MS_CARD) {
-		if (CHK_MSPRO(ms_card)) {
-			if (ms_card->seq_mode) {
-				RTS51X_SET_STAT(chip, STAT_RUN);
-				ms_card->counter++;
-			} else {
-				ms_card->counter = 0;
-			}
-		} else {
-			if (ms_card->delay_write.delay_write_flag) {
-				RTS51X_SET_STAT(chip, STAT_RUN);
-				ms_card->counter++;
-			} else {
-				ms_card->counter = 0;
-			}
-		}
-	}
-
-	if (sd_card->counter > POLLING_WAIT_CNT)
-		rts51x_sd_cleanup_work(chip);
-
-	if (xd_card->counter > POLLING_WAIT_CNT)
-		rts51x_xd_cleanup_work(chip);
-
-	if (ms_card->counter > POLLING_WAIT_CNT)
-		rts51x_ms_cleanup_work(chip);
-}
-
-static void do_rts51x_reset_xd_card(struct rts51x_chip *chip)
-{
-	int retval;
-
-	if (chip->card2lun[XD_CARD] >= MAX_ALLOWED_LUN_CNT)
-		return;
-
-	retval = rts51x_reset_xd_card(chip);
-	if (retval == STATUS_SUCCESS) {
-		chip->card_ready |= XD_CARD;
-		chip->card_fail &= ~XD_CARD;
-		chip->rw_card[chip->card2lun[XD_CARD]] = rts51x_xd_rw;
-	} else {
-		chip->card_ready &= ~XD_CARD;
-		chip->card_fail |= XD_CARD;
-		chip->capacity[chip->card2lun[XD_CARD]] = 0;
-		chip->rw_card[chip->card2lun[XD_CARD]] = NULL;
-
-		rts51x_init_cmd(chip);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_OE, XD_OUTPUT_EN, 0);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, POWER_MASK,
-			       POWER_OFF);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_CLK_EN, XD_CLK_EN, 0);
-		rts51x_send_cmd(chip, MODE_C, 100);
-	}
-}
-
-void rts51x_do_rts51x_reset_sd_card(struct rts51x_chip *chip)
-{
-	int retval;
-
-	if (chip->card2lun[SD_CARD] >= MAX_ALLOWED_LUN_CNT)
-		return;
-
-	retval = rts51x_reset_sd_card(chip);
-	if (retval == STATUS_SUCCESS) {
-		chip->card_ready |= SD_CARD;
-		chip->card_fail &= ~SD_CARD;
-		chip->rw_card[chip->card2lun[SD_CARD]] = rts51x_sd_rw;
-	} else {
-		chip->card_ready &= ~SD_CARD;
-		chip->card_fail |= SD_CARD;
-		chip->capacity[chip->card2lun[SD_CARD]] = 0;
-		chip->rw_card[chip->card2lun[SD_CARD]] = NULL;
-
-		rts51x_init_cmd(chip);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_OE, SD_OUTPUT_EN, 0);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, POWER_MASK,
-			       POWER_OFF);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_CLK_EN, SD_CLK_EN, 0);
-		rts51x_send_cmd(chip, MODE_C, 100);
-	}
-}
-
-static void do_rts51x_reset_ms_card(struct rts51x_chip *chip)
-{
-	int retval;
-
-	if (chip->card2lun[MS_CARD] >= MAX_ALLOWED_LUN_CNT)
-		return;
-
-	retval = rts51x_reset_ms_card(chip);
-	if (retval == STATUS_SUCCESS) {
-		chip->card_ready |= MS_CARD;
-		chip->card_fail &= ~MS_CARD;
-		chip->rw_card[chip->card2lun[MS_CARD]] = rts51x_ms_rw;
-	} else {
-		chip->card_ready &= ~MS_CARD;
-		chip->card_fail |= MS_CARD;
-		chip->capacity[chip->card2lun[MS_CARD]] = 0;
-		chip->rw_card[chip->card2lun[MS_CARD]] = NULL;
-
-		rts51x_init_cmd(chip);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_OE, MS_OUTPUT_EN, 0);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, POWER_MASK,
-			       POWER_OFF);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_CLK_EN, MS_CLK_EN, 0);
-		rts51x_send_cmd(chip, MODE_C, 100);
-	}
-}
-
-static void card_cd_debounce(struct rts51x_chip *chip, u8 *need_reset,
-		      u8 *need_release)
-{
-	int retval;
-	u8 release_map = 0, reset_map = 0;
-	u8 value;
-
-	retval = rts51x_get_card_status(chip, &(chip->card_status));
-#ifdef SUPPORT_OCP
-	chip->ocp_stat = (chip->card_status >> 4) & 0x03;
-#endif
-
-	if (retval != STATUS_SUCCESS)
-		goto Exit_Debounce;
-
-	if (chip->card_exist) {
-		retval = rts51x_read_register(chip, CARD_INT_PEND, &value);
-		if (retval != STATUS_SUCCESS) {
-			rts51x_ep0_write_register(chip, MC_FIFO_CTL, FIFO_FLUSH,
-						  FIFO_FLUSH);
-			rts51x_ep0_write_register(chip, SFSM_ED, 0xf8, 0xf8);
-			value = 0;
-		}
-
-		if (chip->card_exist & XD_CARD) {
-			if (!(chip->card_status & XD_CD))
-				release_map |= XD_CARD;
-		} else if (chip->card_exist & SD_CARD) {
-			/* if (!(chip->card_status & SD_CD)) { */
-			if (!(chip->card_status & SD_CD) || (value & SD_INT))
-				release_map |= SD_CARD;
-		} else if (chip->card_exist & MS_CARD) {
-			/* if (!(chip->card_status & MS_CD)) { */
-			if (!(chip->card_status & MS_CD) || (value & MS_INT))
-				release_map |= MS_CARD;
-		}
-	} else {
-		if (chip->card_status & XD_CD)
-			reset_map |= XD_CARD;
-		else if (chip->card_status & SD_CD)
-			reset_map |= SD_CARD;
-		else if (chip->card_status & MS_CD)
-			reset_map |= MS_CARD;
-	}
-
-	if (CHECK_PKG(chip, QFN24) && reset_map) {
-		if (chip->card_exist & XD_CARD) {
-			reset_map = 0;
-			goto Exit_Debounce;
-		}
-	}
-
-	if (reset_map) {
-		int xd_cnt = 0, sd_cnt = 0, ms_cnt = 0;
-		int i;
-
-		for (i = 0; i < (chip->option.debounce_num); i++) {
-			retval =
-			    rts51x_get_card_status(chip, &(chip->card_status));
-			if (retval != STATUS_SUCCESS) {
-				reset_map = release_map = 0;
-				goto Exit_Debounce;
-			}
-			if (chip->card_status & XD_CD)
-				xd_cnt++;
-			else
-				xd_cnt = 0;
-			if (chip->card_status & SD_CD)
-				sd_cnt++;
-			else
-				sd_cnt = 0;
-			if (chip->card_status & MS_CD)
-				ms_cnt++;
-			else
-				ms_cnt = 0;
-			wait_timeout(30);
-		}
-
-		reset_map = 0;
-		if (!(chip->card_exist & XD_CARD)
-		    && (xd_cnt > (chip->option.debounce_num - 1))) {
-			reset_map |= XD_CARD;
-		}
-		if (!(chip->card_exist & SD_CARD)
-		    && (sd_cnt > (chip->option.debounce_num - 1))) {
-			reset_map |= SD_CARD;
-		}
-		if (!(chip->card_exist & MS_CARD)
-		    && (ms_cnt > (chip->option.debounce_num - 1))) {
-			reset_map |= MS_CARD;
-		}
-	}
-	rts51x_write_register(chip, CARD_INT_PEND, XD_INT | MS_INT | SD_INT,
-			      XD_INT | MS_INT | SD_INT);
-
-Exit_Debounce:
-	if (need_reset)
-		*need_reset = reset_map;
-	if (need_release)
-		*need_release = release_map;
-}
-
-void rts51x_init_cards(struct rts51x_chip *chip)
-{
-	u8 need_reset = 0, need_release = 0;
-
-	card_cd_debounce(chip, &need_reset, &need_release);
-
-	if (need_release) {
-		RTS51X_DEBUGP("need_release = 0x%x\n", need_release);
-
-		rts51x_prepare_run(chip);
-		RTS51X_SET_STAT(chip, STAT_RUN);
-
-#ifdef SUPPORT_OCP
-		if (chip->ocp_stat & (MS_OCP_NOW | MS_OCP_EVER)) {
-			rts51x_write_register(chip, OCPCTL, MS_OCP_CLEAR,
-					      MS_OCP_CLEAR);
-			chip->ocp_stat = 0;
-			RTS51X_DEBUGP("Clear OCP status.\n");
-		}
-#endif
-
-		if (need_release & XD_CARD) {
-			chip->card_exist &= ~XD_CARD;
-			chip->card_ejected = 0;
-			if (chip->card_ready & XD_CARD) {
-				rts51x_release_xd_card(chip);
-				chip->rw_card[chip->card2lun[XD_CARD]] = NULL;
-				clear_bit(chip->card2lun[XD_CARD],
-					  &(chip->lun_mc));
-			}
-		}
-
-		if (need_release & SD_CARD) {
-			chip->card_exist &= ~SD_CARD;
-			chip->card_ejected = 0;
-			if (chip->card_ready & SD_CARD) {
-				rts51x_release_sd_card(chip);
-				chip->rw_card[chip->card2lun[SD_CARD]] = NULL;
-				clear_bit(chip->card2lun[SD_CARD],
-					  &(chip->lun_mc));
-			}
-		}
-
-		if (need_release & MS_CARD) {
-			chip->card_exist &= ~MS_CARD;
-			chip->card_ejected = 0;
-			if (chip->card_ready & MS_CARD) {
-				rts51x_release_ms_card(chip);
-				chip->rw_card[chip->card2lun[MS_CARD]] = NULL;
-				clear_bit(chip->card2lun[MS_CARD],
-					  &(chip->lun_mc));
-			}
-		}
-	}
-
-	if (need_reset && !chip->card_ready) {
-		RTS51X_DEBUGP("need_reset = 0x%x\n", need_reset);
-
-		rts51x_prepare_run(chip);
-		RTS51X_SET_STAT(chip, STAT_RUN);
-
-		if (need_reset & XD_CARD) {
-			chip->card_exist |= XD_CARD;
-			do_rts51x_reset_xd_card(chip);
-		} else if (need_reset & SD_CARD) {
-			chip->card_exist |= SD_CARD;
-			rts51x_do_rts51x_reset_sd_card(chip);
-		} else if (need_reset & MS_CARD) {
-			chip->card_exist |= MS_CARD;
-			do_rts51x_reset_ms_card(chip);
-		}
-	}
-}
-
-void rts51x_release_cards(struct rts51x_chip *chip)
-{
-	if (chip->card_ready & SD_CARD) {
-		rts51x_sd_cleanup_work(chip);
-		rts51x_release_sd_card(chip);
-		chip->card_ready &= ~SD_CARD;
-	}
-
-	if (chip->card_ready & XD_CARD) {
-		rts51x_xd_cleanup_work(chip);
-		rts51x_release_xd_card(chip);
-		chip->card_ready &= ~XD_CARD;
-	}
-
-	if (chip->card_ready & MS_CARD) {
-		rts51x_ms_cleanup_work(chip);
-		rts51x_release_ms_card(chip);
-		chip->card_ready &= ~MS_CARD;
-	}
-}
-
-static inline u8 double_depth(u8 depth)
-{
-	return (depth > 1) ? (depth - 1) : depth;
-}
-
-int rts51x_switch_ssc_clock(struct rts51x_chip *chip, int clk)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	struct ms_info *ms_card = &(chip->ms_card);
-	int retval;
-	u8 N = (u8) (clk - 2), min_N, max_N;
-	u8 mcu_cnt, div, max_div, ssc_depth;
-	int sd_vpclk_phase_reset = 0;
-
-	if (chip->cur_clk == clk)
-		return STATUS_SUCCESS;
-
-	min_N = 60;
-	max_N = 120;
-	max_div = CLK_DIV_4;
-
-	RTS51X_DEBUGP("Switch SSC clock to %dMHz\n", clk);
-
-	if ((clk <= 2) || (N > max_N))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	mcu_cnt = (u8) (60 / clk + 3);
-	if (mcu_cnt > 15)
-		mcu_cnt = 15;
-	/* To make sure that the SSC clock div_n is
-	 * equal or greater than min_N */
-	div = CLK_DIV_1;
-	while ((N < min_N) && (div < max_div)) {
-		N = (N + 2) * 2 - 2;
-		div++;
-	}
-	RTS51X_DEBUGP("N = %d, div = %d\n", N, div);
-
-	if (chip->option.ssc_en) {
-		if (chip->cur_card == SD_CARD) {
-			if (CHK_SD_SDR104(sd_card)) {
-				ssc_depth = chip->option.ssc_depth_sd_sdr104;
-			} else if (CHK_SD_SDR50(sd_card)) {
-				ssc_depth = chip->option.ssc_depth_sd_sdr50;
-			} else if (CHK_SD_DDR50(sd_card)) {
-				ssc_depth =
-				    double_depth(chip->option.
-						 ssc_depth_sd_ddr50);
-			} else if (CHK_SD_HS(sd_card)) {
-				ssc_depth =
-				    double_depth(chip->option.ssc_depth_sd_hs);
-			} else if (CHK_MMC_52M(sd_card)
-				   || CHK_MMC_DDR52(sd_card)) {
-				ssc_depth =
-				    double_depth(chip->option.
-						 ssc_depth_mmc_52m);
-			} else {
-				ssc_depth =
-				    double_depth(chip->option.
-						 ssc_depth_low_speed);
-			}
-		} else if (chip->cur_card == MS_CARD) {
-			if (CHK_MSPRO(ms_card)) {
-				if (CHK_HG8BIT(ms_card)) {
-					ssc_depth =
-					    double_depth(chip->option.
-							 ssc_depth_ms_hg);
-				} else {
-					ssc_depth =
-					    double_depth(chip->option.
-							 ssc_depth_ms_4bit);
-				}
-			} else {
-				if (CHK_MS4BIT(ms_card)) {
-					ssc_depth =
-					    double_depth(chip->option.
-							 ssc_depth_ms_4bit);
-				} else {
-					ssc_depth =
-					    double_depth(chip->option.
-							 ssc_depth_low_speed);
-				}
-			}
-		} else {
-			ssc_depth =
-			    double_depth(chip->option.ssc_depth_low_speed);
-		}
-
-		if (ssc_depth) {
-			if (div == CLK_DIV_2) {
-				/* If clock divided by 2, ssc depth must
-				 * be multiplied by 2 */
-				if (ssc_depth > 1)
-					ssc_depth -= 1;
-				else
-					ssc_depth = SSC_DEPTH_2M;
-			} else if (div == CLK_DIV_4) {
-				/* If clock divided by 4, ssc depth must
-				 * be multiplied by 4 */
-				if (ssc_depth > 2)
-					ssc_depth -= 2;
-				else
-					ssc_depth = SSC_DEPTH_2M;
-			}
-		}
-	} else {
-		/* Disable SSC */
-		ssc_depth = 0;
-	}
-
-	RTS51X_DEBUGP("ssc_depth = %d\n", ssc_depth);
-
-	rts51x_init_cmd(chip);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CLK_DIV, CLK_CHANGE, CLK_CHANGE);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CLK_DIV, 0x3F,
-		       (div << 4) | mcu_cnt);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SSC_CTL1, SSC_RSTB, 0);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SSC_CTL2, SSC_DEPTH_MASK,
-		       ssc_depth);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SSC_DIV_N_0, 0xFF, N);
-	if (sd_vpclk_phase_reset) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_VPCLK0_CTL,
-			       PHASE_NOT_RESET, 0);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_VPCLK0_CTL,
-			       PHASE_NOT_RESET, PHASE_NOT_RESET);
-	}
-
-	retval = rts51x_send_cmd(chip, MODE_C, 2000);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	if (chip->option.ssc_en && ssc_depth)
-		rts51x_write_register(chip, SSC_CTL1, 0xff, 0xD0);
-	else
-		rts51x_write_register(chip, SSC_CTL1, 0xff, 0x50);
-	udelay(100);
-	RTS51X_WRITE_REG(chip, CLK_DIV, CLK_CHANGE, 0);
-
-	chip->cur_clk = clk;
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_switch_normal_clock(struct rts51x_chip *chip, int clk)
-{
-	int retval;
-	u8 sel, div, mcu_cnt;
-	int sd_vpclk_phase_reset = 0;
-
-	if (chip->cur_clk == clk)
-		return STATUS_SUCCESS;
-
-	if (chip->cur_card == SD_CARD) {
-		struct sd_info *sd_card = &(chip->sd_card);
-		if (CHK_SD30_SPEED(sd_card) || CHK_MMC_DDR52(sd_card))
-			sd_vpclk_phase_reset = 1;
-	}
-
-	switch (clk) {
-	case CLK_20:
-		RTS51X_DEBUGP("Switch clock to 20MHz\n");
-		sel = SSC_80;
-		div = CLK_DIV_4;
-		mcu_cnt = 5;
-		break;
-
-	case CLK_30:
-		RTS51X_DEBUGP("Switch clock to 30MHz\n");
-		sel = SSC_60;
-		div = CLK_DIV_2;
-		mcu_cnt = 4;
-		break;
-
-	case CLK_40:
-		RTS51X_DEBUGP("Switch clock to 40MHz\n");
-		sel = SSC_80;
-		div = CLK_DIV_2;
-		mcu_cnt = 3;
-		break;
-
-	case CLK_50:
-		RTS51X_DEBUGP("Switch clock to 50MHz\n");
-		sel = SSC_100;
-		div = CLK_DIV_2;
-		mcu_cnt = 3;
-		break;
-
-	case CLK_60:
-		RTS51X_DEBUGP("Switch clock to 60MHz\n");
-		sel = SSC_60;
-		div = CLK_DIV_1;
-		mcu_cnt = 3;
-		break;
-
-	case CLK_80:
-		RTS51X_DEBUGP("Switch clock to 80MHz\n");
-		sel = SSC_80;
-		div = CLK_DIV_1;
-		mcu_cnt = 2;
-		break;
-
-	case CLK_100:
-		RTS51X_DEBUGP("Switch clock to 100MHz\n");
-		sel = SSC_100;
-		div = CLK_DIV_1;
-		mcu_cnt = 2;
-		break;
-
-	/* case CLK_120:
-		RTS51X_DEBUGP("Switch clock to 120MHz\n");
-		sel = SSC_120;
-		div = CLK_DIV_1;
-		mcu_cnt = 2;
-		break;
-
-	case CLK_150:
-		RTS51X_DEBUGP("Switch clock to 150MHz\n");
-		sel = SSC_150;
-		div = CLK_DIV_1;
-		mcu_cnt = 2;
-		break; */
-
-	default:
-		RTS51X_DEBUGP("Try to switch to an illegal clock (%d)\n",
-			       clk);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if (!sd_vpclk_phase_reset) {
-		rts51x_init_cmd(chip);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CLK_DIV, CLK_CHANGE,
-			       CLK_CHANGE);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CLK_DIV, 0x3F,
-			       (div << 4) | mcu_cnt);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SSC_CLK_FPGA_SEL, 0xFF,
-			       sel);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CLK_DIV, CLK_CHANGE, 0);
-
-		retval = rts51x_send_cmd(chip, MODE_C, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	} else {
-		rts51x_init_cmd(chip);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CLK_DIV, CLK_CHANGE,
-			       CLK_CHANGE);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_VPCLK0_CTL,
-			       PHASE_NOT_RESET, 0);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_VPCLK1_CTL,
-			       PHASE_NOT_RESET, 0);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CLK_DIV, 0x3F,
-			       (div << 4) | mcu_cnt);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SSC_CLK_FPGA_SEL, 0xFF,
-			       sel);
-
-		retval = rts51x_send_cmd(chip, MODE_C, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		udelay(200);
-
-		rts51x_init_cmd(chip);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_VPCLK0_CTL,
-			       PHASE_NOT_RESET, PHASE_NOT_RESET);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_VPCLK1_CTL,
-			       PHASE_NOT_RESET, PHASE_NOT_RESET);
-
-		retval = rts51x_send_cmd(chip, MODE_C, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		udelay(200);
-
-		RTS51X_WRITE_REG(chip, CLK_DIV, CLK_CHANGE, 0);
-	}
-
-	chip->cur_clk = clk;
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_card_rw(struct scsi_cmnd *srb, struct rts51x_chip *chip,
-		u32 sec_addr, u16 sec_cnt)
-{
-	int retval;
-	unsigned int lun = SCSI_LUN(srb);
-	int i;
-
-	if (chip->rw_card[lun] == NULL)
-		return STATUS_FAIL;
-
-	RTS51X_DEBUGP("%s card, sector addr: 0x%x, sector cnt: %d\n",
-		       (srb->sc_data_direction ==
-			DMA_TO_DEVICE) ? "Write" : "Read", sec_addr, sec_cnt);
-
-	chip->rw_need_retry = 0;
-	for (i = 0; i < 3; i++) {
-		retval = chip->rw_card[lun] (srb, chip, sec_addr, sec_cnt);
-		if (retval != STATUS_SUCCESS) {
-			CATCH_TRIGGER(chip);
-			if (chip->option.reset_or_rw_fail_set_pad_drive) {
-				rts51x_write_register(chip, CARD_DRIVE_SEL,
-						      SD20_DRIVE_MASK,
-						      DRIVE_8mA);
-			}
-		}
-
-		if (!chip->rw_need_retry)
-			break;
-
-		RTS51X_DEBUGP("Retry RW, (i = %d\n)", i);
-	}
-
-	return retval;
-}
-
-u8 rts51x_get_lun_card(struct rts51x_chip *chip, unsigned int lun)
-{
-	if ((chip->card_ready & chip->lun2card[lun]) == XD_CARD)
-		return (u8) XD_CARD;
-	else if ((chip->card_ready & chip->lun2card[lun]) == SD_CARD)
-		return (u8) SD_CARD;
-	else if ((chip->card_ready & chip->lun2card[lun]) == MS_CARD)
-		return (u8) MS_CARD;
-
-	return 0;
-}
-
-static int card_share_mode(struct rts51x_chip *chip, int card)
-{
-	u8 value;
-
-	if (card == SD_CARD)
-		value = CARD_SHARE_SD;
-	else if (card == MS_CARD)
-		value = CARD_SHARE_MS;
-	else if (card == XD_CARD)
-		value = CARD_SHARE_XD;
-	else
-		TRACE_RET(chip, STATUS_FAIL);
-
-	RTS51X_WRITE_REG(chip, CARD_SHARE_MODE, CARD_SHARE_MASK, value);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_select_card(struct rts51x_chip *chip, int card)
-{
-	int retval;
-
-	if (chip->cur_card != card) {
-		u8 mod;
-
-		if (card == SD_CARD)
-			mod = SD_MOD_SEL;
-		else if (card == MS_CARD)
-			mod = MS_MOD_SEL;
-		else if (card == XD_CARD)
-			mod = XD_MOD_SEL;
-		else
-			TRACE_RET(chip, STATUS_FAIL);
-		RTS51X_WRITE_REG(chip, CARD_SELECT, 0x07, mod);
-		chip->cur_card = card;
-
-		retval = card_share_mode(chip, card);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-void rts51x_eject_card(struct rts51x_chip *chip, unsigned int lun)
-{
-	RTS51X_DEBUGP("eject card\n");
-	RTS51X_SET_STAT(chip, STAT_RUN);
-	rts51x_do_remaining_work(chip);
-
-	if ((chip->card_ready & chip->lun2card[lun]) == SD_CARD) {
-		rts51x_release_sd_card(chip);
-		chip->card_ejected |= SD_CARD;
-		chip->card_ready &= ~SD_CARD;
-		chip->capacity[lun] = 0;
-	} else if ((chip->card_ready & chip->lun2card[lun]) == XD_CARD) {
-		rts51x_release_xd_card(chip);
-		chip->card_ejected |= XD_CARD;
-		chip->card_ready &= ~XD_CARD;
-		chip->capacity[lun] = 0;
-	} else if ((chip->card_ready & chip->lun2card[lun]) == MS_CARD) {
-		rts51x_release_ms_card(chip);
-		chip->card_ejected |= MS_CARD;
-		chip->card_ready &= ~MS_CARD;
-		chip->capacity[lun] = 0;
-	}
-	rts51x_write_register(chip, CARD_INT_PEND, XD_INT | MS_INT | SD_INT,
-			      XD_INT | MS_INT | SD_INT);
-}
-
-void rts51x_trans_dma_enable(enum dma_data_direction dir,
-		struct rts51x_chip *chip, u32 byte_cnt, u8 pack_size)
-{
-	if (pack_size > DMA_1024)
-		pack_size = DMA_512;
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-		       RING_BUFFER);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MC_DMA_TC3, 0xFF,
-		       (u8) (byte_cnt >> 24));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MC_DMA_TC2, 0xFF,
-		       (u8) (byte_cnt >> 16));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MC_DMA_TC1, 0xFF,
-		       (u8) (byte_cnt >> 8));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, MC_DMA_TC0, 0xFF, (u8) byte_cnt);
-
-	if (dir == DMA_FROM_DEVICE) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MC_DMA_CTL,
-			       0x03 | DMA_PACK_SIZE_MASK,
-			       DMA_DIR_FROM_CARD | DMA_EN | pack_size);
-	} else {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, MC_DMA_CTL,
-			       0x03 | DMA_PACK_SIZE_MASK,
-			       DMA_DIR_TO_CARD | DMA_EN | pack_size);
-	}
-}
-
-int rts51x_enable_card_clock(struct rts51x_chip *chip, u8 card)
-{
-	u8 clk_en = 0;
-
-	if (card & XD_CARD)
-		clk_en |= XD_CLK_EN;
-	if (card & SD_CARD)
-		clk_en |= SD_CLK_EN;
-	if (card & MS_CARD)
-		clk_en |= MS_CLK_EN;
-
-	RTS51X_WRITE_REG(chip, CARD_CLK_EN, clk_en, clk_en);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_card_power_on(struct rts51x_chip *chip, u8 card)
-{
-	u8 mask, val1, val2;
-
-	mask = POWER_MASK;
-	val1 = PARTIAL_POWER_ON;
-	val2 = POWER_ON;
-
-#ifdef SD_XD_IO_FOLLOW_PWR
-	if ((card == SD_CARD) || (card == XD_CARD)) {
-		RTS51X_WRITE_REG(chip, CARD_PWR_CTL, mask | LDO3318_PWR_MASK,
-				 val1 | LDO_SUSPEND);
-	} else {
-#endif
-		RTS51X_WRITE_REG(chip, CARD_PWR_CTL, mask, val1);
-#ifdef SD_XD_IO_FOLLOW_PWR
-	}
-#endif
-	udelay(chip->option.pwr_delay);
-	RTS51X_WRITE_REG(chip, CARD_PWR_CTL, mask, val2);
-#ifdef SD_XD_IO_FOLLOW_PWR
-	if (card == SD_CARD) {
-		rts51x_write_register(chip, CARD_PWR_CTL, LDO3318_PWR_MASK,
-				      LDO_ON);
-	}
-#endif
-
-	return STATUS_SUCCESS;
-}
-
-int monitor_card_cd(struct rts51x_chip *chip, u8 card)
-{
-	int retval;
-	u8 card_cd[32] = { 0 };
-
-	card_cd[SD_CARD] = SD_CD;
-	card_cd[XD_CARD] = XD_CD;
-	card_cd[MS_CARD] = MS_CD;
-
-	retval = rts51x_get_card_status(chip, &(chip->card_status));
-	if (retval != STATUS_SUCCESS)
-		return CD_NOT_EXIST;
-
-	if (chip->card_status & card_cd[card])
-		return CD_EXIST;
-
-	return CD_NOT_EXIST;
-}
-
-int rts51x_toggle_gpio(struct rts51x_chip *chip, u8 gpio)
-{
-	int retval;
-	u8 temp_reg;
-	u8 gpio_output[4] = {
-		0x01,
-	};
-	u8 gpio_oe[4] = {
-		0x02,
-	};
-	if (chip->rts5179) {
-		retval = rts51x_ep0_read_register(chip, CARD_GPIO, &temp_reg);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
-		temp_reg ^= gpio_oe[gpio];
-		temp_reg &= 0xfe; /* bit 0 always set 0 */
-		retval =
-		    rts51x_ep0_write_register(chip, CARD_GPIO, 0x03, temp_reg);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
-	} else {
-		retval = rts51x_ep0_read_register(chip, CARD_GPIO, &temp_reg);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
-		temp_reg ^= gpio_output[gpio];
-		retval =
-		    rts51x_ep0_write_register(chip, CARD_GPIO, 0xFF,
-					      temp_reg | gpio_oe[gpio]);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_turn_on_led(struct rts51x_chip *chip, u8 gpio)
-{
-	int retval;
-	u8 gpio_oe[4] = {
-		0x02,
-	};
-	u8 gpio_mask[4] = {
-		0x03,
-	};
-
-	retval =
-	    rts51x_ep0_write_register(chip, CARD_GPIO, gpio_mask[gpio],
-				      gpio_oe[gpio]);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_turn_off_led(struct rts51x_chip *chip, u8 gpio)
-{
-	int retval;
-	u8 gpio_output[4] = {
-		0x01,
-	};
-	u8 gpio_oe[4] = {
-		0x02,
-	};
-	u8 gpio_mask[4] = {
-		0x03,
-	};
-
-	retval =
-	    rts51x_ep0_write_register(chip, CARD_GPIO, gpio_mask[gpio],
-				      gpio_oe[gpio] | gpio_output[gpio]);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	return STATUS_SUCCESS;
-}
diff --git a/drivers/staging/rts5139/rts51x_card.h b/drivers/staging/rts5139/rts51x_card.h
deleted file mode 100644
index df8816e0f84..00000000000
--- a/drivers/staging/rts5139/rts51x_card.h
+++ /dev/null
@@ -1,870 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- * Header file
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#ifndef __RTS51X_CARD_H
-#define __RTS51X_CARD_H
-
-#include "rts51x_chip.h"
-
-/* Register bit definition */
-
-/* Card Power Control Register */
-#define POWER_OFF			0x03
-#define PARTIAL_POWER_ON		0x02
-#define POWER_ON			0x00
-#define POWER_MASK			0x03
-#define LDO3318_PWR_MASK		0x0C
-#define LDO_ON				0x00
-#define LDO_SUSPEND			0x08
-#define LDO_OFF				0x0C
-#define DV3318_AUTO_PWR_OFF		0x10
-#define FORCE_LDO_POWERB	0x60
-
-/* Card Output Enable Register */
-#define XD_OUTPUT_EN			0x02
-#define SD_OUTPUT_EN			0x04
-#define MS_OUTPUT_EN			0x08
-
-/* System Clock Control Register */
-
-/* System Clock Divider Register */
-#define CLK_CHANGE			0x80
-#define CLK_DIV_1			0x00
-#define CLK_DIV_2			0x01
-#define CLK_DIV_4			0x02
-#define CLK_DIV_8			0x03
-
-/* System Clock Select Register */
-#define SSC_60				0
-#define SSC_80				1
-#define SSC_100				2
-#define SSC_120				3
-#define SSC_150				4
-
-/* Card Clock Enable Register */
-#define XD_CLK_EN			0x02
-#define SD_CLK_EN			0x04
-#define MS_CLK_EN			0x08
-
-/* Card Select Register */
-#define XD_MOD_SEL			1
-#define SD_MOD_SEL			2
-#define MS_MOD_SEL			3
-
-/* Card Transfer Reset Register */
-#define XD_STOP				0x02
-#define SD_STOP				0x04
-#define MS_STOP				0x08
-#define XD_CLR_ERR			0x20
-#define SD_CLR_ERR			0x40
-#define MS_CLR_ERR			0x80
-
-/* SD30_drive_sel */
-#define SD30_DRIVE_MASK	0x07
-
-/* CARD_DRIVE_SEL */
-#define SD20_DRIVE_MASK	0x03
-#define DRIVE_4mA			0x00
-#define DRIVE_8mA			0x01
-#define DRIVE_12mA			0x02
-
-/* FPGA_PULL_CTL */
-#define FPGA_MS_PULL_CTL_EN		0xEF
-#define FPGA_SD_PULL_CTL_EN		0xF7
-#define FPGA_XD_PULL_CTL_EN1		0xFE
-#define FPGA_XD_PULL_CTL_EN2		0xFD
-#define FPGA_XD_PULL_CTL_EN3		0xFB
-
-#define FPGA_MS_PULL_CTL_BIT		0x10
-#define FPGA_SD_PULL_CTL_BIT		0x08
-
-/* Card Data Source Register */
-#define PINGPONG_BUFFER			0x01
-#define RING_BUFFER			0x00
-
-/* SFSM_ED */
-#define HW_CMD_STOP			0x80
-#define CLR_STAGE_STALL			0x08
-#define CARD_ERR				0x10
-
-/* CARD_SHARE_MODE */
-#define	CARD_SHARE_LQFP48		0x04
-#define	CARD_SHARE_QFN24		0x00
-#define CARD_SHARE_LQFP_SEL		0x04
-#define	CARD_SHARE_XD			0x00
-#define	CARD_SHARE_SD			0x01
-#define	CARD_SHARE_MS			0x02
-#define CARD_SHARE_MASK			0x03
-
-/* CARD_AUTO_BLINK */
-#define BLINK_ENABLE			0x08
-#define BLINK_SPEED_MASK		0x07
-
-/* CARD_GPIO */
-#define GPIO_OE				0x02
-#define GPIO_OUTPUT			0x01
-
-/* CARD_CLK_SOURCE */
-#define CRC_FIX_CLK			(0x00 << 0)
-#define CRC_VAR_CLK0			(0x01 << 0)
-#define CRC_VAR_CLK1			(0x02 << 0)
-#define SD30_FIX_CLK			(0x00 << 2)
-#define SD30_VAR_CLK0			(0x01 << 2)
-#define SD30_VAR_CLK1			(0x02 << 2)
-#define SAMPLE_FIX_CLK			(0x00 << 4)
-#define SAMPLE_VAR_CLK0			(0x01 << 4)
-#define SAMPLE_VAR_CLK1			(0x02 << 4)
-
-/* DCM_DRP_CTL */
-#define DCM_RESET			0x08
-#define DCM_LOCKED			0x04
-#define DCM_208M			0x00
-#define DCM_TX			        0x01
-#define DCM_RX			        0x02
-
-/* DCM_DRP_TRIG */
-#define DRP_START			0x80
-#define DRP_DONE			0x40
-
-/* DCM_DRP_CFG */
-#define DRP_WRITE			0x80
-#define DRP_READ			0x00
-#define DCM_WRITE_ADDRESS_50		0x50
-#define DCM_WRITE_ADDRESS_51		0x51
-#define DCM_READ_ADDRESS_00		0x00
-#define DCM_READ_ADDRESS_51		0x51
-
-/* HW_VERSION */
-#define FPGA_VER			0x80
-#define HW_VER_MASK			0x0F
-
-/* CD_DEGLITCH_EN */
-#define DISABLE_SD_CD			0x08
-#define DISABLE_MS_CD			0x10
-#define DISABLE_XD_CD			0x20
-#define SD_CD_DEGLITCH_EN		0x01
-#define MS_CD_DEGLITCH_EN		0x02
-#define XD_CD_DEGLITCH_EN		0x04
-
-/* OCPCTL */
-#define CARD_OC_DETECT_EN		0x08
-#define CARD_OC_CLR			0x01
-
-/* CARD_DMA1_CTL */
-#define EXTEND_DMA1_ASYNC_SIGNAL	0x02
-
-/* HS_USB_STAT */
-#define USB_HI_SPEED			0x01
-
-/* CFG_MODE_1 */
-#define RTS5179				0x02
-
-/* SYS_DUMMY0 */
-#define NYET_EN				0x01
-#define NYET_MSAK			0x01
-
-/* SSC_CTL1 */
-#define SSC_RSTB			0x80
-#define SSC_8X_EN			0x40
-#define SSC_FIX_FRAC			0x20
-#define SSC_SEL_1M			0x00
-#define SSC_SEL_2M			0x08
-#define SSC_SEL_4M			0x10
-#define SSC_SEL_8M			0x18
-
-/* SSC_CTL2 */
-#define SSC_DEPTH_MASK			0x03
-#define SSC_DEPTH_DISALBE		0x00
-#define SSC_DEPTH_2M			0x01
-#define SSC_DEPTH_1M			0x02
-#define SSC_DEPTH_512K			0x03
-
-/* LDO_POWER_CFG */
-#define TUNE_SD18_MASK			0x1C
-#define TUNE_SD18_1V8			(0x01 << 2)
-#define TUNE_SD18_3V3			(0x07 << 2)
-
-/* XD_CP_WAITTIME */
-#define WAIT_1F				0x00
-#define WAIT_3F				0x01
-#define WAIT_7F				0x02
-#define WAIT_FF				0x03
-
-/* XD_INIT */
-#define	XD_PWR_OFF_DELAY0		0x00
-#define	XD_PWR_OFF_DELAY1		0x02
-#define	XD_PWR_OFF_DELAY2		0x04
-#define	XD_PWR_OFF_DELAY3		0x06
-#define	XD_AUTO_PWR_OFF_EN		0xF7
-#define	XD_NO_AUTO_PWR_OFF		0x08
-
-/* XD_DTCTL */
-/* XD_CATCTL */
-#define	XD_TIME_RWN_1			0x00
-#define	XD_TIME_RWN_STEP		0x20
-#define	XD_TIME_RW_1			0x00
-#define	XD_TIME_RW_STEP			0x04
-#define	XD_TIME_SETUP_1			0x00
-#define	XD_TIME_SETUP_STEP		0x01
-
-/* XD_CTL */
-#define	XD_ECC2_UNCORRECTABLE		0x80
-#define	XD_ECC2_ERROR			0x40
-#define	XD_ECC1_UNCORRECTABLE		0x20
-#define	XD_ECC1_ERROR			0x10
-#define	XD_RDY				0x04
-#define	XD_CE_EN			0xFD
-#define	XD_CE_DISEN			0x02
-#define	XD_WP_EN			0xFE
-#define	XD_WP_DISEN			0x01
-
-/* XD_TRANSFER */
-#define	XD_TRANSFER_START		0x80
-#define	XD_TRANSFER_END			0x40
-#define	XD_PPB_EMPTY			0x20
-#define	XD_ERR				0x10
-#define	XD_RESET			0x00
-#define	XD_ERASE			0x01
-#define	XD_READ_STATUS			0x02
-#define	XD_READ_ID			0x03
-#define	XD_READ_REDUNDANT		0x04
-#define	XD_READ_PAGES			0x05
-#define	XD_SET_CMD			0x06
-#define	XD_NORMAL_READ			0x07
-#define	XD_WRITE_PAGES			0x08
-#define	XD_NORMAL_WRITE			0x09
-#define	XD_WRITE_REDUNDANT		0x0A
-#define	XD_SET_ADDR			0x0B
-#define XD_COPY_PAGES			0x0C
-
-/* XD_CFG */
-#define	XD_PPB_TO_SIE			0x80
-#define	XD_TO_PPB_ONLY			0x00
-#define	XD_BA_TRANSFORM			0x40
-#define	XD_BA_NO_TRANSFORM		0x00
-#define	XD_NO_CALC_ECC			0x20
-#define	XD_CALC_ECC			0x00
-#define	XD_IGNORE_ECC			0x10
-#define	XD_CHECK_ECC			0x00
-#define	XD_DIRECT_TO_RB			0x08
-#define XD_ADDR_MASK			0x07
-#define	XD_ADDR_LENGTH_0		0x00
-#define	XD_ADDR_LENGTH_1		0x01
-#define	XD_ADDR_LENGTH_2		0x02
-#define	XD_ADDR_LENGTH_3		0x03
-#define	XD_ADDR_LENGTH_4		0x04
-
-/* XD_PAGE_STATUS */
-#define	XD_GPG				0xFF
-#define	XD_BPG				0x00
-
-/* XD_BLOCK_STATUS */
-#define	XD_GBLK				0xFF
-#define	XD_LATER_BBLK			0xF0
-
-/* XD_PARITY */
-#define	XD_ECC2_ALL1			0x80
-#define	XD_ECC1_ALL1			0x40
-#define	XD_BA2_ALL0			0x20
-#define	XD_BA1_ALL0			0x10
-#define	XD_BA1_BA2_EQL			0x04
-#define	XD_BA2_VALID			0x02
-#define	XD_BA1_VALID			0x01
-
-/* XD_CHK_DATA_STATUS */
-#define	XD_PGSTS_ZEROBIT_OVER4		0x00
-#define	XD_PGSTS_NOT_FF			0x02
-#define	XD_AUTO_CHK_DATA_STATUS		0x01
-
-/* SD_CFG1 */
-#define SD_CLK_DIVIDE_0			0x00
-#define	SD_CLK_DIVIDE_256		0xC0
-#define	SD_CLK_DIVIDE_128		0x80
-#define SD_CLK_DIVIDE_MASK		0xC0
-#define	SD_BUS_WIDTH_1			0x00
-#define	SD_BUS_WIDTH_4			0x01
-#define	SD_BUS_WIDTH_8			0x02
-#define	SD_ASYNC_FIFO_RST		0x10
-#define	SD_20_MODE			0x00
-#define	SD_DDR_MODE			0x04
-#define	SD_30_MODE			0x08
-
-/* SD_CFG2 */
-#define	SD_CALCULATE_CRC7		0x00
-#define	SD_NO_CALCULATE_CRC7		0x80
-#define	SD_CHECK_CRC16			0x00
-#define	SD_NO_CHECK_CRC16		0x40
-#define SD_WAIT_CRC_TO_EN		0x20
-#define	SD_WAIT_BUSY_END		0x08
-#define	SD_NO_WAIT_BUSY_END		0x00
-#define	SD_CHECK_CRC7			0x00
-#define	SD_NO_CHECK_CRC7		0x04
-#define	SD_RSP_LEN_0			0x00
-#define	SD_RSP_LEN_6			0x01
-#define	SD_RSP_LEN_17			0x02
-/* SD/MMC Response Type Definition */
-/* SD_CALCULATE_CRC7, SD_CHECK_CRC16,
- * SD_NO_WAIT_BUSY_END, SD_NO_CHECK_CRC7,
- * SD_RSP_LEN_0 */
-#define	SD_RSP_TYPE_R0			0x04
-/* SD_CALCULATE_CRC7, SD_CHECK_CRC16,
- * SD_NO_WAIT_BUSY_END, SD_CHECK_CRC7,
- * SD_RSP_LEN_6 */
-#define	SD_RSP_TYPE_R1			0x01
-/* SD_CALCULATE_CRC7, SD_CHECK_CRC16,
- * SD_WAIT_BUSY_END, SD_CHECK_CRC7,
- * SD_RSP_LEN_6 */
-#define	SD_RSP_TYPE_R1b			0x09
-/* SD_CALCULATE_CRC7, SD_CHECK_CRC16,
- * SD_NO_WAIT_BUSY_END, SD_CHECK_CRC7,
- * SD_RSP_LEN_17 */
-#define	SD_RSP_TYPE_R2			0x02
-/* SD_CALCULATE_CRC7, SD_CHECK_CRC16,
- * SD_NO_WAIT_BUSY_END, SD_NO_CHECK_CRC7,
- * SD_RSP_LEN_6 */
-#define	SD_RSP_TYPE_R3			0x05
-/* SD_CALCULATE_CRC7, SD_CHECK_CRC16,
- * SD_NO_WAIT_BUSY_END, SD_NO_CHECK_CRC7,
- * SD_RSP_LEN_6 */
-#define	SD_RSP_TYPE_R4			0x05
-/* SD_CALCULATE_CRC7, SD_CHECK_CRC16,
- * SD_NO_WAIT_BUSY_END, SD_CHECK_CRC7,
- * SD_RSP_LEN_6 */
-#define	SD_RSP_TYPE_R5			0x01
-/* SD_CALCULATE_CRC7, SD_CHECK_CRC16,
- * SD_NO_WAIT_BUSY_END, SD_CHECK_CRC7,
- * SD_RSP_LEN_6 */
-#define	SD_RSP_TYPE_R6			0x01
-/* SD_CALCULATE_CRC7, SD_CHECK_CRC16,
- * SD_NO_WAIT_BUSY_END, SD_CHECK_CRC7,
- * SD_RSP_LEN_6  */
-#define	SD_RSP_TYPE_R7			0x01
-
-/* SD_CFG3 */
-#define	SD_RSP_80CLK_TIMEOUT_EN		0x01
-
-/* SD_STAT1 */
-#define	SD_CRC7_ERR			0x80
-#define	SD_CRC16_ERR			0x40
-#define	SD_CRC_WRITE_ERR		0x20
-#define	SD_CRC_WRITE_ERR_MASK		0x1C
-#define	GET_CRC_TIME_OUT		0x02
-#define	SD_TUNING_COMPARE_ERR		0x01
-
-/* SD_STAT2 */
-#define	SD_RSP_80CLK_TIMEOUT		0x01
-
-/* SD_BUS_STAT */
-#define	SD_CLK_TOGGLE_EN		0x80
-#define	SD_CLK_FORCE_STOP	        0x40
-#define	SD_DAT3_STATUS		        0x10
-#define	SD_DAT2_STATUS		        0x08
-#define	SD_DAT1_STATUS		        0x04
-#define	SD_DAT0_STATUS		        0x02
-#define	SD_CMD_STATUS			0x01
-
-/* SD_PAD_CTL */
-#define	SD_IO_USING_1V8		        0x80
-#define	SD_IO_USING_3V3		        0x7F
-#define	TYPE_A_DRIVING		        0x00
-#define	TYPE_B_DRIVING			0x01
-#define	TYPE_C_DRIVING			0x02
-#define	TYPE_D_DRIVING		        0x03
-
-/* SD_SAMPLE_POINT_CTL */
-#define	DDR_FIX_RX_DAT			0x00
-#define	DDR_VAR_RX_DAT			0x80
-#define	DDR_FIX_RX_DAT_EDGE		0x00
-#define	DDR_FIX_RX_DAT_14_DELAY		0x40
-#define	DDR_FIX_RX_CMD			0x00
-#define	DDR_VAR_RX_CMD			0x20
-#define	DDR_FIX_RX_CMD_POS_EDGE		0x00
-#define	DDR_FIX_RX_CMD_14_DELAY		0x10
-#define	SD20_RX_POS_EDGE		0x00
-#define	SD20_RX_14_DELAY		0x08
-#define SD20_RX_SEL_MASK		0x08
-
-/* SD_PUSH_POINT_CTL */
-#define	DDR_FIX_TX_CMD_DAT		0x00
-#define	DDR_VAR_TX_CMD_DAT		0x80
-#define	DDR_FIX_TX_DAT_14_TSU		0x00
-#define	DDR_FIX_TX_DAT_12_TSU		0x40
-#define	DDR_FIX_TX_CMD_NEG_EDGE		0x00
-#define	DDR_FIX_TX_CMD_14_AHEAD		0x20
-#define	SD20_TX_NEG_EDGE		0x00
-#define	SD20_TX_14_AHEAD		0x10
-#define SD20_TX_SEL_MASK		0x10
-#define	DDR_VAR_SDCLK_POL_SWAP		0x01
-
-/* SD_TRANSFER */
-#define	SD_TRANSFER_START		0x80
-#define	SD_TRANSFER_END			0x40
-#define SD_STAT_IDLE			0x20
-#define	SD_TRANSFER_ERR			0x10
-/* SD Transfer Mode definition */
-#define	SD_TM_NORMAL_WRITE		0x00
-#define	SD_TM_AUTO_WRITE_3		0x01
-#define	SD_TM_AUTO_WRITE_4		0x02
-#define	SD_TM_AUTO_READ_3		0x05
-#define	SD_TM_AUTO_READ_4		0x06
-#define	SD_TM_CMD_RSP			0x08
-#define	SD_TM_AUTO_WRITE_1		0x09
-#define	SD_TM_AUTO_WRITE_2		0x0A
-#define	SD_TM_NORMAL_READ		0x0C
-#define	SD_TM_AUTO_READ_1		0x0D
-#define	SD_TM_AUTO_READ_2		0x0E
-#define	SD_TM_AUTO_TUNING		0x0F
-
-/* SD_VPTX_CTL / SD_VPRX_CTL */
-#define PHASE_CHANGE			0x80
-#define PHASE_NOT_RESET			0x40
-
-/* SD_DCMPS_TX_CTL / SD_DCMPS_RX_CTL */
-#define DCMPS_CHANGE			0x80
-#define DCMPS_CHANGE_DONE		0x40
-#define DCMPS_ERROR			0x20
-#define DCMPS_CURRENT_PHASE		0x1F
-
-/* SD_CMD_STATE */
-#define SD_CMD_IDLE			0x80
-
-/* SD_DATA_STATE */
-#define SD_DATA_IDLE			0x80
-
-/* MS_BLKEND */
-#define SET_BLKEND			0x01
-
-/* MS_CFG */
-#define	SAMPLE_TIME_RISING		0x00
-#define	SAMPLE_TIME_FALLING		0x80
-#define	PUSH_TIME_DEFAULT		0x00
-#define	PUSH_TIME_ODD			0x40
-#define	NO_EXTEND_TOGGLE		0x00
-#define	EXTEND_TOGGLE_CHK		0x20
-#define	MS_BUS_WIDTH_1			0x00
-#define	MS_BUS_WIDTH_4			0x10
-#define	MS_BUS_WIDTH_8			0x18
-#define	MS_2K_SECTOR_MODE		0x04
-#define	MS_512_SECTOR_MODE		0x00
-#define	MS_TOGGLE_TIMEOUT_EN		0x00
-#define	MS_TOGGLE_TIMEOUT_DISEN		0x01
-#define MS_NO_CHECK_INT			0x02
-
-/* MS_TRANS_CFG */
-#define	WAIT_INT			0x80
-#define	NO_WAIT_INT			0x00
-#define	NO_AUTO_READ_INT_REG		0x00
-#define	AUTO_READ_INT_REG		0x40
-#define	MS_CRC16_ERR			0x20
-#define	MS_RDY_TIMEOUT			0x10
-#define	MS_INT_CMDNK			0x08
-#define	MS_INT_BREQ			0x04
-#define	MS_INT_ERR			0x02
-#define	MS_INT_CED			0x01
-
-/* MS_TRANSFER */
-#define	MS_TRANSFER_START		0x80
-#define	MS_TRANSFER_END			0x40
-#define	MS_TRANSFER_ERR			0x20
-#define	MS_BS_STATE			0x10
-#define	MS_TM_READ_BYTES		0x00
-#define	MS_TM_NORMAL_READ		0x01
-#define	MS_TM_WRITE_BYTES		0x04
-#define	MS_TM_NORMAL_WRITE		0x05
-#define	MS_TM_AUTO_READ			0x08
-#define	MS_TM_AUTO_WRITE		0x0C
-#define MS_TM_SET_CMD			0x06
-#define MS_TM_COPY_PAGE			0x07
-#define MS_TM_MULTI_READ		0x02
-#define MS_TM_MULTI_WRITE		0x03
-
-/* MC_DMA_CTL */
-#define DMA_TC_EQ_0			0x80
-#define DMA_DIR_TO_CARD			0x00
-#define DMA_DIR_FROM_CARD		0x02
-#define DMA_EN				0x01
-#define DMA_128				(0 << 2)
-#define DMA_256				(1 << 2)
-#define DMA_512				(2 << 2)
-#define DMA_1024			(3 << 2)
-#define DMA_PACK_SIZE_MASK		0x0C
-
-/* CARD_INT_PEND */
-#define XD_INT				0x10
-#define MS_INT				0x08
-#define SD_INT				0x04
-
-/* MC_FIFO_CTL */
-#define FIFO_FLUSH			0x01
-
-/* AUTO_DELINK_EN */
-#define AUTO_DELINK			0x02
-#define FORCE_DELINK			0x01
-
-/* MC_DMA_RST */
-#define DMA_RESET  0x01
-
-#define SSC_POWER_MASK			0x01
-#define SSC_POWER_DOWN			0x01
-#define SSC_POWER_ON			0x00
-
-/* OCPCTL */
-#define MS_OCP_DETECT_EN		0x08
-#define	MS_OCP_INT_EN			0x04
-#define	MS_OCP_INT_CLR			0x02
-#define	MS_OCP_CLEAR			0x01
-
-/* OCPSTAT */
-#define MS_OCP_DETECT			0x80
-#define MS_OCP_NOW			0x02
-#define MS_OCP_EVER			0x01
-
-/* MC_FIFO_STAT */
-#define FIFO_FULL		0x01
-#define FIFO_EMPTY		0x02
-
-/* RCCTL */
-#define U_HW_CMD_EN_MASK		0x02
-#define U_HW_CMD_EN			0x02
-#define U_HW_CMD_DIS			0x00
-
-/* Register address */
-#define FPDCTL				0xFC00
-#define SSC_DIV_N_0			0xFC07
-#define SSC_CTL1			0xFC09
-#define SSC_CTL2			0xFC0A
-#define CFG_MODE_1		0xFC0F
-#define RCCTL			0xFC14
-#define SYS_DUMMY0			0xFC30
-#define XD_CP_WAITTIME			0xFD00
-#define XD_CP_PAGELEN			0xFD01
-#define XD_CP_READADDR0			0xFD02
-#define XD_CP_READADDR1			0xFD03
-#define XD_CP_READADDR2			0xFD04
-#define XD_CP_READADDR3			0xFD05
-#define XD_CP_READADDR4			0xFD06
-#define XD_CP_WRITEADDR0		0xFD07
-#define XD_CP_WRITEADDR1		0xFD08
-#define XD_CP_WRITEADDR2		0xFD09
-#define XD_CP_WRITEADDR3		0xFD0A
-#define XD_CP_WRITEADDR4		0xFD0B
-#define XD_INIT				0xFD10
-#define XD_DTCTL			0xFD11
-#define XD_CTL				0xFD12
-#define XD_TRANSFER			0xFD13
-#define XD_CFG				0xFD14
-#define XD_ADDRESS0			0xFD15
-#define XD_ADDRESS1			0xFD16
-#define XD_ADDRESS2			0xFD17
-#define XD_ADDRESS3			0xFD18
-#define XD_ADDRESS4			0xFD19
-#define XD_DAT				0xFD1A
-#define XD_PAGE_CNT			0xFD1B
-#define XD_PAGE_STATUS			0xFD1C
-#define XD_BLOCK_STATUS			0xFD1D
-#define XD_BLOCK_ADDR1_L		0xFD1E
-#define XD_BLOCK_ADDR1_H		0xFD1F
-#define XD_BLOCK_ADDR2_L		0xFD20
-#define XD_BLOCK_ADDR2_H		0xFD21
-#define XD_BYTE_CNT_L			0xFD22
-#define XD_BYTE_CNT_H			0xFD23
-#define	XD_PARITY			0xFD24
-#define XD_ECC_BIT1			0xFD25
-#define XD_ECC_BYTE1			0xFD26
-#define XD_ECC_BIT2			0xFD27
-#define XD_ECC_BYTE2			0xFD28
-#define XD_RESERVED0			0xFD29
-#define XD_RESERVED1			0xFD2A
-#define XD_RESERVED2			0xFD2B
-#define XD_RESERVED3			0xFD2C
-#define XD_CHK_DATA_STATUS		0xFD2D
-#define XD_CATCTL			0xFD2E
-
-#define MS_BLKEND			0xFD30
-#define MS_READ_START			0xFD31
-#define MS_READ_COUNT			0xFD32
-#define MS_WRITE_START			0xFD33
-#define MS_WRITE_COUNT			0xFD34
-#define MS_COMMAND			0xFD35
-#define MS_OLD_BLOCK_0			0xFD36
-#define MS_OLD_BLOCK_1			0xFD37
-#define MS_NEW_BLOCK_0			0xFD38
-#define MS_NEW_BLOCK_1			0xFD39
-#define MS_LOG_BLOCK_0			0xFD3A
-#define MS_LOG_BLOCK_1			0xFD3B
-#define MS_BUS_WIDTH			0xFD3C
-#define MS_PAGE_START			0xFD3D
-#define MS_PAGE_LENGTH			0xFD3E
-#define MS_CFG				0xFD40
-#define MS_TPC				0xFD41
-#define MS_TRANS_CFG			0xFD42
-#define MS_TRANSFER			0xFD43
-#define MS_INT_REG			0xFD44
-#define MS_BYTE_CNT			0xFD45
-#define MS_SECTOR_CNT_L			0xFD46
-#define MS_SECTOR_CNT_H			0xFD47
-#define MS_DBUS_H			0xFD48
-
-#define CARD_DMA1_CTL			0xFD5C
-#define CARD_PULL_CTL1			0xFD60
-#define CARD_PULL_CTL2			0xFD61
-#define CARD_PULL_CTL3			0xFD62
-#define CARD_PULL_CTL4			0xFD63
-#define CARD_PULL_CTL5			0xFD64
-#define CARD_PULL_CTL6			0xFD65
-#define CARD_EXIST				0xFD6F
-#define CARD_INT_PEND			0xFD71
-
-#define LDO_POWER_CFG			0xFD7B
-
-#define SD_CFG1				0xFDA0
-#define SD_CFG2				0xFDA1
-#define SD_CFG3				0xFDA2
-#define SD_STAT1			0xFDA3
-#define SD_STAT2			0xFDA4
-#define SD_BUS_STAT			0xFDA5
-#define SD_PAD_CTL			0xFDA6
-#define SD_SAMPLE_POINT_CTL		0xFDA7
-#define SD_PUSH_POINT_CTL		0xFDA8
-#define SD_CMD0				0xFDA9
-#define SD_CMD1				0xFDAA
-#define SD_CMD2				0xFDAB
-#define SD_CMD3				0xFDAC
-#define SD_CMD4				0xFDAD
-#define SD_CMD5				0xFDAE
-#define SD_BYTE_CNT_L			0xFDAF
-#define SD_BYTE_CNT_H			0xFDB0
-#define SD_BLOCK_CNT_L			0xFDB1
-#define SD_BLOCK_CNT_H			0xFDB2
-#define SD_TRANSFER			0xFDB3
-#define SD_CMD_STATE			0xFDB5
-#define SD_DATA_STATE			0xFDB6
-#define SD_VPCLK0_CTL			0xFC2A
-#define SD_VPCLK1_CTL			0xFC2B
-#define SD_DCMPS0_CTL			0xFC2C
-#define SD_DCMPS1_CTL			0xFC2D
-
-#define CARD_DMA1_CTL			0xFD5C
-
-#define HW_VERSION			0xFC01
-
-#define SSC_CLK_FPGA_SEL		0xFC02
-#define CLK_DIV				0xFC03
-#define SFSM_ED				0xFC04
-
-#define CD_DEGLITCH_WIDTH		0xFC20
-#define CD_DEGLITCH_EN			0xFC21
-#define AUTO_DELINK_EN			0xFC23
-
-#define FPGA_PULL_CTL			0xFC1D
-#define CARD_CLK_SOURCE			0xFC2E
-
-#define CARD_SHARE_MODE			0xFD51
-#define CARD_DRIVE_SEL			0xFD52
-#define CARD_STOP			0xFD53
-#define CARD_OE				0xFD54
-#define CARD_AUTO_BLINK			0xFD55
-#define CARD_GPIO			0xFD56
-#define SD30_DRIVE_SEL		0xFD57
-
-#define CARD_DATA_SOURCE		0xFD5D
-#define CARD_SELECT			0xFD5E
-
-#define CARD_CLK_EN			0xFD79
-#define CARD_PWR_CTL			0xFD7A
-
-#define OCPCTL				0xFD80
-#define OCPPARA1			0xFD81
-#define OCPPARA2			0xFD82
-#define OCPSTAT				0xFD83
-
-#define HS_USB_STAT			0xFE01
-#define HS_VCONTROL			0xFE26
-#define HS_VSTAIN			0xFE27
-#define HS_VLOADM			0xFE28
-#define HS_VSTAOUT			0xFE29
-
-#define MC_IRQ				0xFF00
-#define MC_IRQEN			0xFF01
-#define MC_FIFO_CTL			0xFF02
-#define MC_FIFO_BC0			0xFF03
-#define MC_FIFO_BC1			0xFF04
-#define MC_FIFO_STAT			0xFF05
-#define MC_FIFO_MODE			0xFF06
-#define MC_FIFO_RD_PTR0		0xFF07
-#define MC_FIFO_RD_PTR1		0xFF08
-#define MC_DMA_CTL			0xFF10
-#define MC_DMA_TC0			0xFF11
-#define MC_DMA_TC1			0xFF12
-#define MC_DMA_TC2			0xFF13
-#define MC_DMA_TC3			0xFF14
-#define MC_DMA_RST			0xFF15
-
-/* Memory mapping */
-#define RBUF_SIZE_MASK		0xFBFF
-#define RBUF_BASE			0xF000
-#define PPBUF_BASE1			0xF800
-#define PPBUF_BASE2			0xFA00
-
-/* int monitor_card_cd */
-#define CD_EXIST			0
-#define CD_NOT_EXIST			1
-
-#define DEBOUNCE_CNT			5
-
-int monitor_card_cd(struct rts51x_chip *chip, u8 card);
-
-void rts51x_do_remaining_work(struct rts51x_chip *chip);
-void rts51x_do_rts51x_reset_sd_card(struct rts51x_chip *chip);
-void rts51x_init_cards(struct rts51x_chip *chip);
-void rts51x_release_cards(struct rts51x_chip *chip);
-int rts51x_switch_ssc_clock(struct rts51x_chip *chip, int clk);
-int rts51x_switch_normal_clock(struct rts51x_chip *chip, int clk);
-int rts51x_card_rw(struct scsi_cmnd *srb, struct rts51x_chip *chip,
-		u32 sec_addr, u16 sec_cnt);
-u8 rts51x_get_lun_card(struct rts51x_chip *chip, unsigned int lun);
-int rts51x_select_card(struct rts51x_chip *chip, int card);
-void rts51x_eject_card(struct rts51x_chip *chip, unsigned int lun);
-void rts51x_trans_dma_enable(enum dma_data_direction dir,
-		struct rts51x_chip *chip, u32 byte_cnt, u8 pack_size);
-int rts51x_enable_card_clock(struct rts51x_chip *chip, u8 card);
-int rts51x_card_power_on(struct rts51x_chip *chip, u8 card);
-int rts51x_toggle_gpio(struct rts51x_chip *chip, u8 gpio);
-int rts51x_turn_on_led(struct rts51x_chip *chip, u8 gpio);
-int rts51x_turn_off_led(struct rts51x_chip *chip, u8 gpio);
-
-static inline int check_card_ready(struct rts51x_chip *chip, unsigned int lun)
-{
-	if (chip->card_ready & chip->lun2card[lun])
-		return 1;
-
-	return 0;
-}
-
-static inline int check_card_exist(struct rts51x_chip *chip, unsigned int lun)
-{
-	if (chip->card_exist & chip->lun2card[lun])
-		return 1;
-
-	return 0;
-}
-
-static inline int check_card_wp(struct rts51x_chip *chip, unsigned int lun)
-{
-	if (chip->card_wp & chip->lun2card[lun])
-		return 1;
-
-	return 0;
-}
-
-static inline int check_card_fail(struct rts51x_chip *chip, unsigned int lun)
-{
-	if (chip->card_fail & chip->lun2card[lun])
-		return 1;
-
-	return 0;
-}
-
-static inline int check_card_ejected(struct rts51x_chip *chip, unsigned int lun)
-{
-	if (chip->card_ejected & chip->lun2card[lun])
-		return 1;
-
-	return 0;
-}
-
-static inline int check_fake_card_ready(struct rts51x_chip *chip,
-					unsigned int lun)
-{
-	if (chip->fake_card_ready & chip->lun2card[lun])
-		return 1;
-
-	return 0;
-}
-
-static inline u8 get_lun2card(struct rts51x_chip *chip, unsigned int lun)
-{
-	return chip->lun2card[lun];
-}
-
-static inline int check_lun_mc(struct rts51x_chip *chip, unsigned int lun)
-{
-	return CHK_BIT(chip->lun_mc, lun);
-}
-
-static inline void set_lun_mc(struct rts51x_chip *chip, unsigned int lun)
-{
-	SET_BIT(chip->lun_mc, lun);
-}
-
-static inline void clear_lun_mc(struct rts51x_chip *chip, unsigned int lun)
-{
-	CLR_BIT(chip->lun_mc, lun);
-}
-
-static inline int switch_clock(struct rts51x_chip *chip, int clk)
-{
-	int retval = 0;
-
-	if (chip->asic_code)
-		retval = rts51x_switch_ssc_clock(chip, clk);
-	else
-		retval = rts51x_switch_normal_clock(chip, clk);
-
-	return retval;
-}
-
-static inline void rts51x_clear_xd_error(struct rts51x_chip *chip)
-{
-	rts51x_ep0_write_register(chip, CARD_STOP,
-				  XD_STOP | XD_CLR_ERR, XD_STOP | XD_CLR_ERR);
-
-	rts51x_ep0_write_register(chip, MC_FIFO_CTL, FIFO_FLUSH, FIFO_FLUSH);
-	rts51x_ep0_write_register(chip, MC_DMA_RST, DMA_RESET, DMA_RESET);
-	rts51x_ep0_write_register(chip, SFSM_ED, 0xf8, 0xf8);
-}
-
-static inline void rts51x_clear_sd_error(struct rts51x_chip *chip)
-{
-	rts51x_ep0_write_register(chip, CARD_STOP,
-				  SD_STOP | SD_CLR_ERR, SD_STOP | SD_CLR_ERR);
-
-	rts51x_ep0_write_register(chip, MC_FIFO_CTL, FIFO_FLUSH, FIFO_FLUSH);
-	rts51x_ep0_write_register(chip, MC_DMA_RST, DMA_RESET, DMA_RESET);
-	rts51x_ep0_write_register(chip, SFSM_ED, 0xf8, 0xf8);
-}
-
-static inline void rts51x_clear_ms_error(struct rts51x_chip *chip)
-{
-	rts51x_ep0_write_register(chip, CARD_STOP,
-				  MS_STOP | MS_CLR_ERR, MS_STOP | MS_CLR_ERR);
-
-	rts51x_ep0_write_register(chip, MC_FIFO_CTL, FIFO_FLUSH, FIFO_FLUSH);
-	rts51x_ep0_write_register(chip, MC_DMA_RST, DMA_RESET, DMA_RESET);
-	rts51x_ep0_write_register(chip, SFSM_ED, 0xf8, 0xf8);
-}
-
-#endif /* __RTS51X_CARD_H */
diff --git a/drivers/staging/rts5139/rts51x_chip.c b/drivers/staging/rts5139/rts51x_chip.c
deleted file mode 100644
index 7d7510de170..00000000000
--- a/drivers/staging/rts5139/rts51x_chip.c
+++ /dev/null
@@ -1,1014 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#include <linux/blkdev.h>
-#include <linux/kthread.h>
-#include <linux/sched.h>
-#include <linux/workqueue.h>
-
-#include "debug.h"
-#include "trace.h"
-#include "rts51x.h"
-#include "rts51x_chip.h"
-#include "rts51x_card.h"
-#include "rts51x_transport.h"
-#include "xd.h"
-#include "ms.h"
-#include "sd.h"
-
-static int check_sd_speed_prior(u32 sd_speed_prior)
-{
-	int i, fake_para = 0;
-
-	/* Check the legality of sd_speed_prior */
-	for (i = 0; i < 4; i++) {
-		u8 tmp = (u8) (sd_speed_prior >> (i * 8));
-		if ((tmp < 0x01) || (tmp > 0x04)) {
-			fake_para = 1;
-			break;
-		}
-	}
-
-	return !fake_para;
-}
-
-int rts51x_reset_chip(struct rts51x_chip *chip)
-{
-	int retval;
-
-	if (CHECK_PKG(chip, LQFP48)) {
-		RTS51X_WRITE_REG(chip, CARD_PWR_CTL, LDO3318_PWR_MASK,
-				 LDO_SUSPEND);
-		RTS51X_WRITE_REG(chip, CARD_PWR_CTL, FORCE_LDO_POWERB,
-				 FORCE_LDO_POWERB);
-		RTS51X_WRITE_REG(chip, CARD_PULL_CTL1, 0x30, 0x10);
-		RTS51X_WRITE_REG(chip, CARD_PULL_CTL5, 0x03, 0x01);
-		RTS51X_WRITE_REG(chip, CARD_PULL_CTL6, 0x0C, 0x04);
-	}
-	if (chip->asic_code) {
-		RTS51X_WRITE_REG(chip, SYS_DUMMY0, NYET_MSAK, NYET_EN);
-		RTS51X_WRITE_REG(chip, CD_DEGLITCH_WIDTH, 0xFF, 0x08);
-		rts51x_write_register(chip, CD_DEGLITCH_EN, XD_CD_DEGLITCH_EN,
-				      0x00);
-		rts51x_write_register(chip, SD30_DRIVE_SEL, SD30_DRIVE_MASK,
-				      chip->option.sd30_pad_drive);
-		rts51x_write_register(chip, CARD_DRIVE_SEL, SD20_DRIVE_MASK,
-				      chip->option.sd20_pad_drive);
-		if (chip->rts5179)
-			rts51x_write_register(chip, CARD_PULL_CTL5, 0x03, 0x01);
-		if (CHECK_PKG(chip, LQFP48)) {
-			rts51x_write_register(chip, CARD_PULL_CTL3,
-					      0x80, 0x80);
-			rts51x_write_register(chip, CARD_PULL_CTL6,
-					      0xf0, 0xA0);
-		} else {
-			rts51x_write_register(chip, CARD_PULL_CTL1,
-					      0x30, 0x20);
-			rts51x_write_register(chip, CARD_PULL_CTL3,
-					      0x80, 0x80);
-			rts51x_write_register(chip, CARD_PULL_CTL6,
-					      0x0c, 0x08);
-		}
-	}
-	if (chip->option.sd_ctl & SUPPORT_UHS50_MMC44) {
-		SET_UHS50(chip);
-		RTS51X_DEBUGP("option enable UHS50&MMC44,sd_ctl:0x%x\n",
-			chip->option.sd_ctl);
-	} else {
-		/* if(CHECK_PID(chip, 0x0139)&&CHECK_PKG(chip, LQFP48)) */
-		if ((CHECK_PID(chip, 0x0139) && CHECK_PKG(chip, LQFP48))
-		    || chip->rts5179) {
-			SET_UHS50(chip);
-			RTS51X_DEBUGP("PID enable UHS50&MMC44\n");
-		} else {
-			CLEAR_UHS50(chip);
-			RTS51X_DEBUGP("PID disable UHS50&MMC44\n");
-		}
-	}
-
-	if (chip->option.ms_errreg_fix && (chip->ic_version > 1))
-		rts51x_write_register(chip, 0xFD4D, 0x01, 0x01);
-	retval = rts51x_write_phy_register(chip, 0xC2, 0x7C);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	rts51x_init_cmd(chip);
-
-	/* GPIO OE */
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_GPIO, GPIO_OE, GPIO_OE);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DMA1_CTL,
-		       EXTEND_DMA1_ASYNC_SIGNAL, EXTEND_DMA1_ASYNC_SIGNAL);
-
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-#ifdef SUPPORT_OCP
-	if (chip->asic_code) {
-		rts51x_write_register(chip, OCPCTL, MS_OCP_DETECT_EN,
-				      MS_OCP_DETECT_EN);
-		RTS51X_DEBUGP("Enable OCP detect!\n");
-	}
-#endif
-	if (chip->option.FT2_fast_mode) {
-		rts51x_card_power_on(chip, SD_CARD | MS_CARD | XD_CARD);
-		wait_timeout(10);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_init_chip(struct rts51x_chip *chip)
-{
-	int retval;
-	u8 val;
-
-	chip->max_lun = 0;
-	chip->cur_clk = 0;
-	chip->cur_card = 0;
-
-	chip->card2lun[XD_CARD] = 0;
-	chip->card2lun[SD_CARD] = 0;
-	chip->card2lun[MS_CARD] = 0;
-	chip->card_ejected = 0;
-
-	chip->lun2card[0] = XD_CARD | SD_CARD | MS_CARD;
-#ifdef CLOSE_SSC_POWER
-	rts51x_write_register(chip, FPDCTL, SSC_POWER_MASK, SSC_POWER_ON);
-	udelay(100);
-	rts51x_write_register(chip, CLK_DIV, CLK_CHANGE, 0x00);
-#endif
-	RTS51X_SET_STAT(chip, STAT_RUN);
-
-	RTS51X_READ_REG(chip, HW_VERSION, &val);
-	RTS51X_DEBUGP("HW_VERSION: 0x%x\n", val);
-	if (val & FPGA_VER) {
-		chip->asic_code = 0;
-		RTS51X_DEBUGP("FPGA!\n");
-	} else {
-		chip->asic_code = 1;
-		RTS51X_DEBUGP("ASIC!\n");
-	}
-	chip->ic_version = val & HW_VER_MASK;
-
-	if (!check_sd_speed_prior(chip->option.sd_speed_prior))
-		chip->option.sd_speed_prior = 0x01020403;
-	RTS51X_DEBUGP("sd_speed_prior = 0x%08x\n",
-		       chip->option.sd_speed_prior);
-
-	RTS51X_READ_REG(chip, CARD_SHARE_MODE, &val);
-	if (val & CARD_SHARE_LQFP_SEL) {
-		chip->package = LQFP48;
-		RTS51X_DEBUGP("Package: LQFP48\n");
-	} else {
-		chip->package = QFN24;
-		RTS51X_DEBUGP("Package: QFN24\n");
-	}
-
-	RTS51X_READ_REG(chip, HS_USB_STAT, &val);
-	if (val & USB_HI_SPEED) {
-		chip->usb_speed = USB_20;
-		RTS51X_DEBUGP("USB High Speed\n");
-	} else {
-		chip->usb_speed = USB_11;
-		RTS51X_DEBUGP("USB Full Speed\n");
-	}
-
-	RTS51X_READ_REG(chip, CFG_MODE_1, &val);
-	if (val & RTS5179) {
-		chip->rts5179 = 1;
-		RTS51X_DEBUGP("device is rts5179\n");
-	} else {
-		chip->rts5179 = 0;
-	}
-
-	retval = rts51x_reset_chip(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_release_chip(struct rts51x_chip *chip)
-{
-	rts51x_xd_free_l2p_tbl(chip);
-	rts51x_ms_free_l2p_tbl(chip);
-	chip->card_ready = 0;
-	return STATUS_SUCCESS;
-}
-
-static inline void rts51x_blink_led(struct rts51x_chip *chip)
-{
-	/* Read/Write */
-	if (chip->card_ready) {
-		if (chip->led_toggle_counter <
-				chip->option.led_toggle_interval) {
-			chip->led_toggle_counter++;
-		} else {
-			chip->led_toggle_counter = 0;
-			rts51x_toggle_gpio(chip, LED_GPIO);
-		}
-	}
-}
-
-static void rts51x_auto_delink_cmd(struct rts51x_chip *chip)
-{
-	rts51x_write_register(chip, AUTO_DELINK_EN,
-			AUTO_DELINK, AUTO_DELINK);
-}
-
-static void rts51x_auto_delink_force_cmd(struct rts51x_chip *chip)
-{
-	rts51x_write_register(chip, AUTO_DELINK_EN,
-			AUTO_DELINK | FORCE_DELINK,
-			AUTO_DELINK | FORCE_DELINK);
-}
-
-#ifdef USING_POLLING_CYCLE_DELINK
-/* using polling cycle as delink time */
-static void rts51x_auto_delink_polling_cycle(struct rts51x_chip *chip)
-{
-	if (chip->auto_delink_counter <=
-			chip->option.delink_delay * 2) {
-		if (chip->auto_delink_counter ==
-		    chip->option.delink_delay) {
-			if (chip->card_exist) {
-				/* False card */
-				if (!chip->card_ejected) {
-					/* if card is not ejected or safely
-					 * remove,then do force delink */
-					RTS51X_DEBUGP("False card inserted,"
-							"do force delink\n");
-					rts51x_auto_delink_force_cmd(chip);
-					chip->auto_delink_counter =
-					    chip->option.delink_delay * 2 + 1;
-				}
-			} else {
-				RTS51X_DEBUGP("No card inserted, do delink\n");
-				/* rts51x_write_register(chip, CARD_PWR_CTL,
-						DV3318_AUTO_PWR_OFF, 0); */
-				rts51x_auto_delink_cmd(chip);
-			}
-		}
-		if (chip->auto_delink_counter ==
-		    chip->option.delink_delay * 2) {
-			RTS51X_DEBUGP("Try to do force delink\n");
-			rts51x_auto_delink_force_cmd(chip);
-		}
-		chip->auto_delink_counter++;
-	}
-}
-
-static void rts51x_auto_delink(struct rts51x_chip *chip)
-{
-	rts51x_auto_delink_polling_cycle(chip);
-}
-#else
-/* some of called funcs are not implemented, so comment it out */
-static void rts51x_auto_delink(struct rts51x_chip *chip)
-{
-}
-#endif
-
-void rts51x_polling_func(struct rts51x_chip *chip)
-{
-
-	rts51x_init_cards(chip);
-
-#ifdef SUPPORT_OCP
-	/* if OCP happen and card exist, then close card OE */
-	if ((chip->ocp_stat & (MS_OCP_NOW | MS_OCP_EVER)) &&
-			(chip->card_exist)) {
-
-		rts51x_prepare_run(chip);
-
-		if (chip->card_exist & SD_CARD)
-			rts51x_write_register(chip, CARD_OE, SD_OUTPUT_EN, 0);
-		else if (chip->card_exist & MS_CARD)
-			rts51x_write_register(chip, CARD_OE, MS_OUTPUT_EN, 0);
-		else if (chip->card_exist & XD_CARD)
-			rts51x_write_register(chip, CARD_OE, XD_OUTPUT_EN, 0);
-	}
-#endif
-
-	if (chip->idle_counter < IDLE_MAX_COUNT) {
-		chip->idle_counter++;
-	} else {
-		if (!RTS51X_CHK_STAT(chip, STAT_IDLE)) {
-			RTS51X_DEBUGP("Idle state!\n");
-			RTS51X_SET_STAT(chip, STAT_IDLE);
-			chip->led_toggle_counter = 0;
-			/* Idle state, turn off LED
-			 * to reduce power consumption */
-			if (chip->option.led_always_on
-			    && (chip->card_exist &
-				(SD_CARD | MS_CARD | XD_CARD))
-			    && (!chip->card_ejected)) {
-				rts51x_turn_on_led(chip, LED_GPIO);
-			} else {
-				if (chip->rts5179) {
-					rts51x_ep0_write_register(chip,
-								  CARD_GPIO,
-								  0x03, 0x00);
-				} else {
-					rts51x_turn_off_led(chip, LED_GPIO);
-				}
-
-			}
-
-#ifdef CLOSE_SSC_POWER
-			if (!chip->card_ready) {
-				rts51x_write_register(chip, CLK_DIV, CLK_CHANGE,
-						      CLK_CHANGE);
-				rts51x_write_register(chip, FPDCTL,
-						      SSC_POWER_MASK,
-						      SSC_POWER_DOWN);
-				RTS51X_DEBUGP("Close SSC clock power!\n");
-			}
-#endif
-		}
-	}
-
-	switch (RTS51X_GET_STAT(chip)) {
-	case STAT_RUN:
-		rts51x_blink_led(chip);
-		rts51x_do_remaining_work(chip);
-		break;
-
-	case STAT_IDLE:
-		break;
-
-	default:
-		break;
-	}
-
-	if (chip->option.auto_delink_en && !chip->card_ready)
-		rts51x_auto_delink(chip);
-	else
-		chip->auto_delink_counter = 0;
-}
-
-void rts51x_add_cmd(struct rts51x_chip *chip,
-		    u8 cmd_type, u16 reg_addr, u8 mask, u8 data)
-{
-	int i;
-
-	if (chip->cmd_idx < ((CMD_BUF_LEN - CMD_OFFSET) / 4)) {
-		i = CMD_OFFSET + chip->cmd_idx * 4;
-		chip->cmd_buf[i++] =
-		    ((cmd_type & 0x03) << 6) | (u8) ((reg_addr >> 8) & 0x3F);
-		chip->cmd_buf[i++] = (u8) reg_addr;
-		chip->cmd_buf[i++] = mask;
-		chip->cmd_buf[i++] = data;
-		chip->cmd_idx++;
-	}
-}
-
-int rts51x_send_cmd(struct rts51x_chip *chip, u8 flag, int timeout)
-{
-	int result;
-
-	chip->cmd_buf[CNT_H] = (u8) (chip->cmd_idx >> 8);
-	chip->cmd_buf[CNT_L] = (u8) (chip->cmd_idx);
-	chip->cmd_buf[STAGE_FLAG] = flag;
-
-	result = rts51x_transfer_data_rcc(chip, SND_BULK_PIPE(chip),
-					  (void *)(chip->cmd_buf),
-					  chip->cmd_idx * 4 + CMD_OFFSET,
-					  0, NULL, timeout, MODE_C);
-	if (result != STATUS_SUCCESS)
-		TRACE_RET(chip, result);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_get_rsp(struct rts51x_chip *chip, int rsp_len, int timeout)
-{
-	int result;
-
-	if (rsp_len <= 0)
-		TRACE_RET(chip, STATUS_ERROR);
-	/* rsp_len must aligned to dword */
-	if (rsp_len % 4)
-		rsp_len += (4 - rsp_len % 4);
-
-	result = rts51x_transfer_data_rcc(chip, RCV_BULK_PIPE(chip),
-					  (void *)chip->rsp_buf, rsp_len,
-					  0, NULL, timeout, STAGE_R);
-	if (result != STATUS_SUCCESS)
-		TRACE_RET(chip, result);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_get_card_status(struct rts51x_chip *chip, u16 *status)
-{
-	int retval;
-	u16 val;
-
-#ifdef GET_CARD_STATUS_USING_EPC
-	retval = rts51x_get_epc_status(chip, &val);
-
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-#else
-	retval = rts51x_ctrl_transfer(chip, RCV_CTRL_PIPE(chip), 0x02, 0xC0,
-				      0, 0, &val, 2, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-#endif
-
-	if (status)
-		*status = val;
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_write_register(struct rts51x_chip *chip, u16 addr, u8 mask, u8 data)
-{
-	int retval;
-
-	rts51x_init_cmd(chip);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, addr, mask, data);
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_read_register(struct rts51x_chip *chip, u16 addr, u8 *data)
-{
-	int retval;
-
-	if (data)
-		*data = 0;
-	rts51x_init_cmd(chip);
-	rts51x_add_cmd(chip, READ_REG_CMD, addr, 0, 0);
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	retval = rts51x_get_rsp(chip, 1, 100);
-
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	if (data)
-		*data = chip->rsp_buf[0];
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_ep0_write_register(struct rts51x_chip *chip, u16 addr, u8 mask,
-			      u8 data)
-{
-	int retval;
-	u16 value = 0, index = 0;
-
-	value |= (u16) (3 & 0x03) << 14;
-	value |= (u16) (addr & 0x3FFF);
-	index |= (u16) mask << 8;
-	index |= (u16) data;
-
-	retval = rts51x_ctrl_transfer(chip, SND_CTRL_PIPE(chip), 0x00, 0x40,
-				      cpu_to_be16(value), cpu_to_be16(index),
-				      NULL, 0, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_ep0_read_register(struct rts51x_chip *chip, u16 addr, u8 *data)
-{
-	int retval;
-	u16 value = 0;
-	u8 val;
-
-	if (data)
-		*data = 0;
-
-	value |= (u16) (2 & 0x03) << 14;
-	value |= (u16) (addr & 0x3FFF);
-
-	retval = rts51x_ctrl_transfer(chip, RCV_CTRL_PIPE(chip), 0x00, 0xC0,
-				      cpu_to_be16(value), 0, &val, 1, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (data)
-		*data = val;
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_seq_write_register(struct rts51x_chip *chip, u16 addr, u16 len,
-			      u8 *data)
-{
-	int result;
-	u16 cmd_len = len + 12;
-
-	if (!data)
-		TRACE_RET(chip, STATUS_ERROR);
-
-	cmd_len = (cmd_len <= CMD_BUF_LEN) ? cmd_len : CMD_BUF_LEN;
-
-	/* cmd_len must aligned to dword */
-	if (cmd_len % 4)
-		cmd_len += (4 - cmd_len % 4);
-
-	chip->cmd_buf[0] = 'R';
-	chip->cmd_buf[1] = 'T';
-	chip->cmd_buf[2] = 'C';
-	chip->cmd_buf[3] = 'R';
-	chip->cmd_buf[PACKET_TYPE] = SEQ_WRITE;
-	chip->cmd_buf[5] = (u8) (len >> 8);
-	chip->cmd_buf[6] = (u8) len;
-	chip->cmd_buf[STAGE_FLAG] = 0;
-	chip->cmd_buf[8] = (u8) (addr >> 8);
-	chip->cmd_buf[9] = (u8) addr;
-
-	memcpy(chip->cmd_buf + 12, data, len);
-
-	result = rts51x_transfer_data_rcc(chip, SND_BULK_PIPE(chip),
-					  (void *)(chip->cmd_buf), cmd_len, 0,
-					  NULL, 100, MODE_C);
-	if (result != STATUS_SUCCESS)
-		TRACE_RET(chip, result);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_seq_read_register(struct rts51x_chip *chip, u16 addr, u16 len,
-			     u8 *data)
-{
-	int result;
-	u16 rsp_len;
-
-	if (!data)
-		TRACE_RET(chip, STATUS_ERROR);
-	/* rsp_len must aligned to dword */
-	if (len % 4)
-		rsp_len = len + (4 - len % 4);
-	else
-		rsp_len = len;
-
-	chip->cmd_buf[0] = 'R';
-	chip->cmd_buf[1] = 'T';
-	chip->cmd_buf[2] = 'C';
-	chip->cmd_buf[3] = 'R';
-	chip->cmd_buf[PACKET_TYPE] = SEQ_READ;
-	chip->cmd_buf[5] = (u8) (rsp_len >> 8);
-	chip->cmd_buf[6] = (u8) rsp_len;
-	chip->cmd_buf[STAGE_FLAG] = STAGE_R;
-	chip->cmd_buf[8] = (u8) (addr >> 8);
-	chip->cmd_buf[9] = (u8) addr;
-
-	result = rts51x_transfer_data_rcc(chip, SND_BULK_PIPE(chip),
-					  (void *)(chip->cmd_buf), 12, 0, NULL,
-					  100, MODE_C);
-	if (result != STATUS_SUCCESS)
-		TRACE_RET(chip, result);
-
-	result = rts51x_transfer_data_rcc(chip, RCV_BULK_PIPE(chip),
-					  (void *)data, rsp_len, 0, NULL, 100,
-					  STAGE_DI);
-	if (result != STATUS_SUCCESS)
-		TRACE_RET(chip, result);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_read_ppbuf(struct rts51x_chip *chip, u8 *buf, int buf_len)
-{
-	int retval;
-
-	if (!buf)
-		TRACE_RET(chip, STATUS_ERROR);
-
-	retval =
-	    rts51x_seq_read_register(chip, PPBUF_BASE2, (u16) buf_len, buf);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_write_ppbuf(struct rts51x_chip *chip, u8 *buf, int buf_len)
-{
-	int retval;
-
-	if (!buf)
-		TRACE_RET(chip, STATUS_ERROR);
-
-	retval =
-	    rts51x_seq_write_register(chip, PPBUF_BASE2, (u16) buf_len, buf);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_write_phy_register(struct rts51x_chip *chip, u8 addr, u8 val)
-{
-	int retval;
-
-	RTS51X_DEBUGP("Write 0x%x to phy register 0x%x\n", val, addr);
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VSTAIN, 0xFF, val);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VCONTROL, 0xFF, addr & 0x0F);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x00);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x00);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x01);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VCONTROL, 0xFF,
-		       (addr >> 4) & 0x0F);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x00);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x00);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x01);
-
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_read_phy_register(struct rts51x_chip *chip, u8 addr, u8 *val)
-{
-	int retval;
-
-	RTS51X_DEBUGP("Read from phy register 0x%x\n", addr);
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VCONTROL, 0xFF, 0x07);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x00);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x00);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x01);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VCONTROL, 0xFF,
-		       (addr >> 4) & 0x0F);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x00);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x00);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x01);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VCONTROL, 0xFF, addr & 0x0F);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x00);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x00);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, HS_VLOADM, 0xFF, 0x01);
-	rts51x_add_cmd(chip, READ_REG_CMD, HS_VSTAOUT, 0, 0);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, 1, 100);
-
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (val)
-		*val = chip->rsp_buf[0];
-
-	RTS51X_DEBUGP("Return value: 0x%x\n", chip->rsp_buf[0]);
-
-	return STATUS_SUCCESS;
-}
-
-void rts51x_do_before_power_down(struct rts51x_chip *chip)
-{
-	RTS51X_DEBUGP("rts51x_do_before_power_down\n");
-
-	rts51x_prepare_run(chip);
-
-	rts51x_release_cards(chip);
-	if (chip->rts5179)
-		rts51x_ep0_write_register(chip, CARD_GPIO, 0x03, 0x00);
-	else
-		rts51x_turn_off_led(chip, LED_GPIO);
-
-	chip->cur_clk = 0;
-	chip->card_exist = 0;
-	chip->cur_card = 0;
-	if (chip->asic_code) {
-		if (CHECK_PKG(chip, LQFP48)) {
-			rts51x_write_register(chip, CARD_PULL_CTL3, 0x80, 0x00);
-			rts51x_write_register(chip, CARD_PULL_CTL6, 0xf0, 0x50);
-		} else {
-			rts51x_write_register(chip, CARD_PULL_CTL1, 0x30, 0x10);
-			rts51x_write_register(chip, CARD_PULL_CTL3, 0x80, 0x00);
-			rts51x_write_register(chip, CARD_PULL_CTL6, 0x0c, 0x04);
-		}
-	}
-	if (CHECK_PKG(chip, LQFP48))
-		rts51x_write_register(chip, CARD_PWR_CTL, LDO3318_PWR_MASK,
-				      LDO_OFF);
-}
-
-void rts51x_clear_hw_error(struct rts51x_chip *chip)
-{
-	rts51x_ep0_write_register(chip, SFSM_ED, 0xf8, 0xf8);
-}
-
-void rts51x_prepare_run(struct rts51x_chip *chip)
-{
-#ifdef CLOSE_SSC_POWER
-	if (RTS51X_CHK_STAT(chip, STAT_IDLE) && (!chip->card_ready)) {
-		rts51x_write_register(chip, FPDCTL, SSC_POWER_MASK,
-				      SSC_POWER_ON);
-		udelay(100);
-		RTS51X_DEBUGP("Open SSC clock power.\n");
-
-		rts51x_write_register(chip, CLK_DIV, CLK_CHANGE, 0x00);
-	}
-#endif
-}
-
-#ifdef _MSG_TRACE
-void rts51x_trace_msg(struct rts51x_chip *chip, unsigned char *buf, int clear)
-{
-	unsigned char *ptr;
-	int i, msg_cnt;
-
-	if (!buf)
-		return;
-
-	ptr = buf;
-
-	if (chip->trace_msg[chip->msg_idx].valid)
-		msg_cnt = TRACE_ITEM_CNT;
-	else
-		msg_cnt = chip->msg_idx;
-	*(ptr++) = (u8) (msg_cnt >> 24);
-	*(ptr++) = (u8) (msg_cnt >> 16);
-	*(ptr++) = (u8) (msg_cnt >> 8);
-	*(ptr++) = (u8) msg_cnt;
-	RTS51X_DEBUGP("Trace message count is %d\n", msg_cnt);
-
-	for (i = 1; i <= msg_cnt; i++) {
-		int j, idx;
-
-		idx = chip->msg_idx - i;
-		if (idx < 0)
-			idx += TRACE_ITEM_CNT;
-
-		*(ptr++) = (u8) (chip->trace_msg[idx].line >> 8);
-		*(ptr++) = (u8) (chip->trace_msg[idx].line);
-		for (j = 0; j < MSG_FUNC_LEN; j++)
-			*(ptr++) = chip->trace_msg[idx].func[j];
-		for (j = 0; j < MSG_FILE_LEN; j++)
-			*(ptr++) = chip->trace_msg[idx].file[j];
-		for (j = 0; j < TIME_VAL_LEN; j++)
-			*(ptr++) = chip->trace_msg[idx].timeval_buf[j];
-	}
-
-	if (clear) {
-		chip->msg_idx = 0;
-		for (i = 0; i < TRACE_ITEM_CNT; i++)
-			chip->trace_msg[i].valid = 0;
-	}
-}
-#endif
-
-void rts51x_pp_status(struct rts51x_chip *chip, unsigned int lun, u8 *status,
-		      u8 status_len)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	struct ms_info *ms_card = &(chip->ms_card);
-	u8 card = rts51x_get_lun_card(chip, lun);
-#ifdef SUPPORT_OC
-	u8 oc_now_mask = 0, oc_ever_mask = 0;
-#endif
-
-	if (!status || (status_len < 32))
-		return;
-	/* IC Version */
-	status[0] = (u8) RTS51X_GET_PID(chip);
-	status[1] = (u8) (chip->ic_version);
-
-	/* Auto delink mode */
-	if (chip->option.auto_delink_en)
-		status[2] = 0x10;
-	else
-		status[2] = 0x00;
-
-	/* Spec version */
-	status[3] = 20;
-	status[4] = 10;
-	status[5] = 05;
-	status[6] = 21;
-
-	/* Card WP */
-	if (chip->card_wp)
-		status[7] = 0x20;
-	else
-		status[7] = 0x00;
-
-#ifdef SUPPORT_OC
-	/* Over current status */
-	status[8] = 0;
-	oc_now_mask = MS_OCP_NOW;
-	oc_ever_mask = MS_OCP_EVER;
-
-	if (chip->ocp_stat & oc_now_mask)
-		status[8] |= 0x02;
-	if (chip->ocp_stat & oc_ever_mask)
-		status[8] |= 0x01;
-#endif
-
-	if (card == SD_CARD) {
-		if (CHK_SD(sd_card)) {
-			if (CHK_SD_HCXC(sd_card)) {
-				if (sd_card->capacity > 0x4000000)
-					/* SDXC */
-					status[0x0E] = 0x02;
-				else /* SDHC */
-					status[0x0E] = 0x01;
-			} else { /* SDSC */
-				status[0x0E] = 0x00;
-			}
-
-			if (CHK_SD_SDR104(sd_card))
-				status[0x0F] = 0x03;
-			else if (CHK_SD_DDR50(sd_card))
-				status[0x0F] = 0x04;
-			else if (CHK_SD_SDR50(sd_card))
-				status[0x0F] = 0x02;
-			else if (CHK_SD_HS(sd_card))
-				status[0x0F] = 0x01;
-			else
-				status[0x0F] = 0x00; /* Normal speed */
-		} else {
-			if (CHK_MMC_SECTOR_MODE(sd_card))
-				status[0x0E] = 0x01; /* High capacity */
-			else
-				status[0x0E] = 0x00; /* Normal capacity */
-
-			if (CHK_MMC_DDR52(sd_card))
-				status[0x0F] = 0x03; /* DDR 52M */
-			else if (CHK_MMC_52M(sd_card))
-				status[0x0F] = 0x02; /* SDR 52M */
-			else if (CHK_MMC_26M(sd_card))
-				status[0x0F] = 0x01; /* SDR 26M */
-			else
-				status[0x0F] = 0x00; /* Normal speed */
-		}
-	} else if (card == MS_CARD) {
-		if (CHK_MSPRO(ms_card)) {
-			if (CHK_MSXC(ms_card))
-				status[0x0E] = 0x01; /* XC */
-			else
-				status[0x0E] = 0x00;
-
-			if (CHK_HG8BIT(ms_card))
-				status[0x0F] = 0x01;
-			else
-				status[0x0F] = 0x00;
-		}
-	}
-
-	/* Function 0
-	 * Support Magic Gate, CPRM and PhyRegister R/W */
-	status[0x18] = 0x8A;
-
-	/* Function 2
-	 * Support OC LUN status & WP LUN status */
-	status[0x1A] = 0x28;
-
-	/* Function 2
-	 * Support OC LUN status & WP LUN status */
-	status[0x1A] = 0x28;
-}
-
-void rts51x_read_status(struct rts51x_chip *chip, unsigned int lun,
-			u8 *rts51x_status, u8 status_len)
-{
-	if (!rts51x_status || (status_len < 16))
-		return;
-	/* VID */
-	rts51x_status[0] = (u8) (RTS51X_GET_VID(chip) >> 8);
-	rts51x_status[1] = (u8) RTS51X_GET_VID(chip);
-
-	/* PID */
-	rts51x_status[2] = (u8) (RTS51X_GET_PID(chip) >> 8);
-	rts51x_status[3] = (u8) RTS51X_GET_PID(chip);
-
-	/* gbLUN */
-	rts51x_status[4] = (u8) lun;
-
-	/* Lun Card Number */
-	if (chip->card_exist) {
-		if (chip->card_exist & XD_CARD)
-			rts51x_status[5] = 4; /* xD Card */
-		else if (chip->card_exist & SD_CARD)
-			rts51x_status[5] = 2; /* SD Card */
-		else if (chip->card_exist & MS_CARD)
-			rts51x_status[5] = 3; /* MS Card */
-		else
-			rts51x_status[5] = 7; /* Multi */
-	} else {
-		rts51x_status[5] = 7;	/* Multi */
-	}
-
-	/* Total LUNs */
-	rts51x_status[6] = 1;
-
-	/* IC Version */
-	rts51x_status[7] = (u8) RTS51X_GET_PID(chip);
-	rts51x_status[8] = chip->ic_version;
-
-	/* Physical Exist */
-	if (check_card_exist(chip, lun))
-		rts51x_status[9] = 1;
-	else
-		rts51x_status[9] = 0;
-
-	/* Multi Flag */
-	rts51x_status[10] = 1;
-
-	/* LUN Valid Map */
-	rts51x_status[11] = XD_CARD | SD_CARD | MS_CARD;
-
-	/* Logical Exist */
-	if (check_card_ready(chip, lun))
-		rts51x_status[12] = 1;
-	else
-		rts51x_status[12] = 0;
-
-	/* Detailed Type */
-	if (rts51x_get_lun_card(chip, lun) == XD_CARD) {
-		rts51x_status[13] = 0x40;
-	} else if (rts51x_get_lun_card(chip, lun) == SD_CARD) {
-		struct sd_info *sd_card = &(chip->sd_card);
-
-		rts51x_status[13] = 0x20;
-		if (CHK_SD(sd_card)) {
-			if (CHK_SD_HCXC(sd_card))
-				rts51x_status[13] |= 0x04; /* Hi capacity SD */
-			if (CHK_SD_HS(sd_card))
-				rts51x_status[13] |= 0x02; /* Hi speed SD */
-		} else {
-			rts51x_status[13] |= 0x08; /* MMC card */
-			if (CHK_MMC_52M(sd_card))
-				rts51x_status[13] |= 0x02; /* Hi speed */
-			if (CHK_MMC_SECTOR_MODE(sd_card))
-				rts51x_status[13] |= 0x04; /* Hi capacity */
-		}
-	} else if (rts51x_get_lun_card(chip, lun) == MS_CARD) {
-		struct ms_info *ms_card = &(chip->ms_card);
-
-		if (CHK_MSPRO(ms_card)) {
-			rts51x_status[13] = 0x38; /* MS Pro */
-			if (CHK_HG8BIT(ms_card))
-				rts51x_status[13] |= 0x04; /* HG */
-#ifdef SUPPORT_MSXC
-			if (CHK_MSXC(ms_card))
-				rts51x_status[13] |= 0x01; /* MSXC */
-#endif
-		} else {
-			rts51x_status[13] = 0x30;
-		}
-	} else {
-		rts51x_status[13] = 0x70;
-	}
-/* Support OC, auto delink, vendor r/w, get bus width */
-	rts51x_status[14] = 0x78;
-
-	rts51x_status[15] = 0x82;
-}
-
-int rts51x_transfer_data_rcc(struct rts51x_chip *chip, unsigned int pipe,
-			     void *buf, unsigned int len, int use_sg,
-			     unsigned int *act_len, int timeout, u8 stage_flag)
-{
-	int retval;
-
-	retval =
-	    rts51x_transfer_data(chip, pipe, buf, len, use_sg, act_len,
-				 timeout);
-
-	return retval;
-
-}
diff --git a/drivers/staging/rts5139/rts51x_chip.h b/drivers/staging/rts5139/rts51x_chip.h
deleted file mode 100644
index 12deb24cfbb..00000000000
--- a/drivers/staging/rts5139/rts51x_chip.h
+++ /dev/null
@@ -1,821 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- * Header file
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#ifndef __RTS51X_CHIP_H
-#define __RTS51X_CHIP_H
-
-#include <linux/usb.h>
-#include <linux/usb_usual.h>
-#include <linux/blkdev.h>
-#include <linux/completion.h>
-#include <linux/mutex.h>
-#include <scsi/scsi_host.h>
-
-#include "trace.h"
-
-#define SUPPORT_CPRM
-#define SUPPORT_MAGIC_GATE
-#define SUPPORT_MSXC
-#define USING_POLLING_CYCLE_DELINK
-
-#ifdef SUPPORT_MAGIC_GA
-/* Using NORMAL_WRITE instead of AUTO_WRITE to set ICVTE */
-#define MG_SET_ICV_SLOW
-#endif
-
-#ifdef SUPPORT_MSXC
-#define XC_POWERCLASS
-#define SUPPORT_PCGL_1P18
-#endif
-
-#define GET_CARD_STATUS_USING_EPC
-
-#define CLOSE_SSC_POWER
-
-#define SUPPORT_OCP
-
-#define MS_SPEEDUP
-
-#define SD_XD_IO_FOLLOW_PWR
-
-#define SD_NR		2
-#define MS_NR		3
-#define XD_NR		4
-#define SD_CARD		(1 << SD_NR)
-#define MS_CARD		(1 << MS_NR)
-#define XD_CARD		(1 << XD_NR)
-
-#define SD_CD		0x01
-#define MS_CD		0x02
-#define XD_CD		0x04
-#define SD_WP		0x08
-
-#define MAX_ALLOWED_LUN_CNT	8
-#define CMD_BUF_LEN		1024
-#define POLLING_INTERVAL	50	/* 50ms */
-
-#define XD_FREE_TABLE_CNT	1200
-#define MS_FREE_TABLE_CNT	512
-
-/* Bit Operation */
-#define SET_BIT(data, idx)	((data) |= 1 << (idx))
-#define CLR_BIT(data, idx)	((data) &= ~(1 << (idx)))
-#define CHK_BIT(data, idx)	((data) & (1 << (idx)))
-
-/* Command type */
-#define READ_REG_CMD		0
-#define WRITE_REG_CMD		1
-#define CHECK_REG_CMD		2
-
-#define PACKET_TYPE		4
-#define CNT_H			5
-#define CNT_L			6
-#define STAGE_FLAG		7
-#define CMD_OFFSET		8
-
-/* Packet type */
-#define BATCH_CMD		0
-#define SEQ_READ		1
-#define SEQ_WRITE		2
-
-/* Stage flag */
-#define STAGE_R			0x01
-#define STAGE_DI		0x02
-#define STAGE_DO		0x04
-/* Return MS_TRANS_CFG, GET_INT */
-#define STAGE_MS_STATUS		0x08
-/* Return XD_CFG, XD_CTL, XD_PAGE_STATUS */
-#define STAGE_XD_STATUS		0x10
-/* Command stage mode */
-#define MODE_C			0x00
-#define MODE_CR			(STAGE_R)
-#define MODE_CDIR		(STAGE_R | STAGE_DI)
-#define MODE_CDOR		(STAGE_R | STAGE_DO)
-
-/* Function return code */
-#ifndef STATUS_SUCCESS
-#define STATUS_SUCCESS		0
-#endif
-
-#define STATUS_FAIL		1
-#define STATUS_TIMEDOUT		4
-#define STATUS_NOMEM		5
-#define STATUS_TRANS_SHORT	6
-#define STATUS_TRANS_LONG	7
-#define STATUS_STALLED		8
-#define STATUS_ERROR		10
-
-#define IDLE_MAX_COUNT		10
-#define POLLING_WAIT_CNT	1
-#define LED_GPIO		0
-
-/* package */
-#define QFN24			0
-#define LQFP48			1
-
-#define USB_11			0
-#define USB_20			1
-
-/*
- * Transport return codes
- */
-/* Transport good, command good */
-#define TRANSPORT_GOOD		0
-/* Transport good, command failed */
-#define TRANSPORT_FAILED	1
-/* Transport bad (i.e. device dead) */
-#define TRANSPORT_ERROR		3
-
-/* Supported Clock */
-enum card_clock { CLK_20 = 1, CLK_30, CLK_40, CLK_50, CLK_60, CLK_80, CLK_100 };
-
-#ifdef _MSG_TRACE
-
-#define TRACE_ITEM_CNT		64
-
-struct trace_msg_t {
-	u16 line;
-#define MSG_FUNC_LEN 64
-	char func[MSG_FUNC_LEN];
-#define MSG_FILE_LEN 32
-	char file[MSG_FILE_LEN];
-#define TIME_VAL_LEN 16
-	u8 timeval_buf[TIME_VAL_LEN];
-	u8 valid;
-};
-
-#endif /* _MSG_TRACE */
-
-/* Size of the autosense data buffer */
-#define SENSE_SIZE		18
-
-/* Sense type */
-#define	SENSE_TYPE_NO_SENSE				0
-#define	SENSE_TYPE_MEDIA_CHANGE				1
-#define	SENSE_TYPE_MEDIA_NOT_PRESENT			2
-#define	SENSE_TYPE_MEDIA_LBA_OVER_RANGE			3
-#define	SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT		4
-#define	SENSE_TYPE_MEDIA_WRITE_PROTECT			5
-#define	SENSE_TYPE_MEDIA_INVALID_CMD_FIELD		6
-#define	SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR		7
-#define	SENSE_TYPE_MEDIA_WRITE_ERR			8
-#define SENSE_TYPE_FORMAT_CMD_FAILED			10
-#ifdef SUPPORT_MAGIC_GATE
-/* COPY PROTECTION KEY EXCHANGE FAILURE - KEY NOT ESTABLISHED */
-#define SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB		0x0b
-/* COPY PROTECTION KEY EXCHANGE FAILURE - AUTHENTICATION FAILURE */
-#define SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN		0x0c
-/* INCOMPATIBLE MEDIUM INSTALLED */
-#define SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM		0x0d
-/* WRITE ERROR */
-#define SENSE_TYPE_MG_WRITE_ERR				0x0e
-#endif
-
-/*---- sense key ----*/
-#define ILGAL_REQ               0x05	/* CDB/parameter/identify msg error */
-
-/*-----------------------------------
-    SENSE_DATA
------------------------------------*/
-
-/*---- error code ----*/
-#define CUR_ERR                 0x70	/* current error                    */
-
-/*---- sense key Information ----*/
-
-#define SKSV                    0x80
-#define CDB_ILLEGAL             0x40
-
-/*---- ASC ----*/
-#define ASC_INVLD_CDB           0x24
-
-/*---- ASQC ----*/
-#define ASCQ_INVLD_CDB          0x00
-
-struct sense_data_t {
-	unsigned char err_code;	/* error code */
-	/* bit7 : valid                    */
-	/*   (1 : SCSI2)                    */
-	/*   (0 : Vendor specific)          */
-	/* bit6-0 : error code             */
-	/*  (0x70 : current error)          */
-	/*  (0x71 : specific command error) */
-	unsigned char seg_no;	/* segment No.                      */
-	unsigned char sense_key;	/* byte5 : ILI                      */
-	/* bit3-0 : sense key              */
-	unsigned char info[4];	/* information                       */
-	unsigned char ad_sense_len;	/* additional sense data length     */
-	unsigned char cmd_info[4];	/* command specific information      */
-	unsigned char asc;	/* ASC                              */
-	unsigned char ascq;	/* ASCQ                             */
-	unsigned char rfu;	/* FRU                              */
-	unsigned char sns_key_info[3];	/* sense key specific information    */
-};
-
-/* sd_ctl bit map */
-/* SD push point control, bit 0, 1 */
-#define SD_PUSH_POINT_CTL_MASK		0x03
-#define SD_PUSH_POINT_DELAY		0x01
-#define SD_PUSH_POINT_AUTO		0x02
-/* SD sample point control, bit 2, 3 */
-#define SD_SAMPLE_POINT_CTL_MASK	0x0C
-#define SD_SAMPLE_POINT_DELAY		0x04
-#define SD_SAMPLE_POINT_AUTO		0x08
-/* SD DDR Tx phase set by user, bit 4 */
-#define SD_DDR_TX_PHASE_SET_BY_USER	0x10
-/* MMC DDR Tx phase set by user, bit 5 */
-#define MMC_DDR_TX_PHASE_SET_BY_USER	0x20
-/* Support MMC DDR mode, bit 6 */
-/*#define SUPPORT_MMC_DDR_MODE          0x40 */
-#define SUPPORT_UHS50_MMC44		0x40
-
-struct rts51x_option {
-	int rts51x_mspro_formatter_enable;
-
-	/* card clock expected by user for fpga platform */
-	int fpga_sd_sdr104_clk;
-	int fpga_sd_ddr50_clk;
-	int fpga_sd_sdr50_clk;
-	int fpga_sd_hs_clk;
-	int fpga_mmc_52m_clk;
-	int fpga_ms_hg_clk;
-	int fpga_ms_4bit_clk;
-
-	/* card clock expected by user for asic platform */
-	int asic_sd_sdr104_clk;
-	int asic_sd_ddr50_clk;
-	int asic_sd_sdr50_clk;
-	int asic_sd_hs_clk;
-	int asic_mmc_52m_clk;
-	int asic_ms_hg_clk;
-	int asic_ms_4bit_clk;
-
-	u8 ssc_depth_sd_sdr104;	/* sw */
-	u8 ssc_depth_sd_ddr50;	/* sw */
-	u8 ssc_depth_sd_sdr50;	/* sw */
-	u8 ssc_depth_sd_hs;	/* sw */
-	u8 ssc_depth_mmc_52m;	/* sw */
-	u8 ssc_depth_ms_hg;	/* sw */
-	u8 ssc_depth_ms_4bit;	/* sw */
-	u8 ssc_depth_low_speed;	/* sw */
-
-	/* SD/MMC Tx phase */
-	int sd_ddr_tx_phase;	/* Enabled by bit 4 of sd_ctl */
-	int mmc_ddr_tx_phase;	/* Enabled by bit 5 of sd_ctl */
-
-	/* priority of choosing sd speed funciton */
-	u32 sd_speed_prior;
-
-	/* sd card control */
-	u32 sd_ctl;
-
-	/* Enable Selective Suspend */
-	int ss_en;
-	/* Interval to enter SS from IDLE state (second) */
-	int ss_delay;
-
-	/* Enable SSC clock */
-	int ssc_en;
-
-	int auto_delink_en;
-
-	/* sangdy2010-07-13:add FT2 fast mode */
-	int FT2_fast_mode;
-	/* sangdy2010-07-15:
-	 * add for config delay between 1/4 PMOS and 3/4 PMOS */
-	int pwr_delay;
-
-	int rts51x_xd_rw_step;		/* add to tune xd tRP */
-	int D3318_off_delay;	/* add to tune D3318 off delay time */
-	int delink_delay;	/* add to tune delink delay time */
-	/* add for rts5129 to enable/disable D3318 off */
-	u8 rts5129_D3318_off_enable;
-	u8 sd20_pad_drive;	/* add to config SD20 PAD drive */
-	u8 sd30_pad_drive;	/* add to config SD30 pad drive */
-	/*if reset or rw fail,then set SD20 pad drive again */
-	u8 reset_or_rw_fail_set_pad_drive;
-
-	u8 debounce_num;	/* debounce number */
-	u8 led_toggle_interval;	/* used to control led toggle speed */
-	int rts51x_xd_rwn_step;
-	u8 sd_send_status_en;
-	/* used to store default phase which is
-	 * used when phase tune all pass. */
-	u8 ddr50_tx_phase;
-	u8 ddr50_rx_phase;
-	u8 sdr50_tx_phase;
-	u8 sdr50_rx_phase;
-	/* used to enable select sdr50 tx phase according to proportion. */
-	u8 sdr50_phase_sel;
-	u8 ms_errreg_fix;
-	u8 reset_mmc_first;
-	u8 speed_mmc;		/* when set, then try CMD55 only twice */
-	u8 led_always_on;	/* if set, then led always on when card exist */
-	u8 dv18_voltage;	/* add to tune dv18 voltage */
-};
-
-#define MS_FORMATTER_ENABLED(chip)	((chip)->option.rts51x_mspro_formatter_enable)
-
-struct rts51x_chip;
-
-typedef int (*rts51x_card_rw_func) (struct scsi_cmnd *srb, struct rts51x_chip *chip,
-			     u32 sec_addr, u16 sec_cnt);
-
-/* For MS Card */
-#define    MAX_DEFECTIVE_BLOCK     10
-
-struct zone_entry {
-	u16 *l2p_table;
-	u16 *free_table;
-	u16 defect_list[MAX_DEFECTIVE_BLOCK];	/* For MS card only */
-	int set_index;
-	int get_index;
-	int unused_blk_cnt;
-	int disable_count;
-	/* To indicate whether the L2P table of this zone has been built. */
-	int build_flag;
-};
-
-struct xd_delay_write_tag {
-	u32 old_phyblock;
-	u32 new_phyblock;
-	u32 logblock;
-	u8 pageoff;
-	u8 delay_write_flag;
-};
-
-struct xd_info {
-	u8 maker_code;
-	u8 device_code;
-	u8 block_shift;
-	u8 page_off;
-	u8 addr_cycle;
-	u16 cis_block;
-	u8 multi_flag;
-	u8 err_code;
-	u32 capacity;
-
-	struct zone_entry *zone;
-	int zone_cnt;
-
-	struct xd_delay_write_tag delay_write;
-
-	int counter;
-
-	int xd_clock;
-};
-
-#define TYPE_SD			0x0000
-#define TYPE_MMC		0x0001
-
-/* TYPE_SD */
-#define SD_HS			0x0100
-#define SD_SDR50		0x0200
-#define SD_DDR50		0x0400
-#define SD_SDR104		0x0800
-#define SD_HCXC			0x1000
-
-/* TYPE_MMC */
-#define MMC_26M			0x0100
-#define MMC_52M			0x0200
-#define MMC_4BIT		0x0400
-#define MMC_8BIT		0x0800
-#define MMC_SECTOR_MODE		0x1000
-#define MMC_DDR52		0x2000
-
-/* SD card */
-#define CHK_SD(sd_card)			(((sd_card)->sd_type & 0xFF) == TYPE_SD)
-#define CHK_SD_HS(sd_card)	\
-	(CHK_SD(sd_card) && ((sd_card)->sd_type & SD_HS))
-#define CHK_SD_SDR50(sd_card)		\
-	(CHK_SD(sd_card) && ((sd_card)->sd_type & SD_SDR50))
-#define CHK_SD_DDR50(sd_card)	\
-	(CHK_SD(sd_card) && ((sd_card)->sd_type & SD_DDR50))
-#define CHK_SD_SDR104(sd_card)	\
-	(CHK_SD(sd_card) && ((sd_card)->sd_type & SD_SDR104))
-#define CHK_SD_HCXC(sd_card)	\
-	(CHK_SD(sd_card) && ((sd_card)->sd_type & SD_HCXC))
-#define CHK_SD30_SPEED(sd_card)	\
-	(CHK_SD_SDR50(sd_card) || CHK_SD_DDR50(sd_card) ||\
-	 CHK_SD_SDR104(sd_card))
-
-#define SET_SD(sd_card)			((sd_card)->sd_type = TYPE_SD)
-#define SET_SD_HS(sd_card)		((sd_card)->sd_type |= SD_HS)
-#define SET_SD_SDR50(sd_card)		((sd_card)->sd_type |= SD_SDR50)
-#define SET_SD_DDR50(sd_card)		((sd_card)->sd_type |= SD_DDR50)
-#define SET_SD_SDR104(sd_card)		((sd_card)->sd_type |= SD_SDR104)
-#define SET_SD_HCXC(sd_card)		((sd_card)->sd_type |= SD_HCXC)
-
-#define CLR_SD_HS(sd_card)		((sd_card)->sd_type &= ~SD_HS)
-#define CLR_SD_SDR50(sd_card)		((sd_card)->sd_type &= ~SD_SDR50)
-#define CLR_SD_DDR50(sd_card)		((sd_card)->sd_type &= ~SD_DDR50)
-#define CLR_SD_SDR104(sd_card)		((sd_card)->sd_type &= ~SD_SDR104)
-#define CLR_SD_HCXC(sd_card)		((sd_card)->sd_type &= ~SD_HCXC)
-#define CLR_SD30_SPEED(sd_card)	\
-	((sd_card)->sd_type &= ~(SD_SDR50|SD_DDR50|SD_SDR104))
-
-/* MMC card */
-#define CHK_MMC(sd_card)	\
-	(((sd_card)->sd_type & 0xFF) == TYPE_MMC)
-#define CHK_MMC_26M(sd_card)	\
-	(CHK_MMC(sd_card) && ((sd_card)->sd_type & MMC_26M))
-#define CHK_MMC_52M(sd_card)	\
-	(CHK_MMC(sd_card) && ((sd_card)->sd_type & MMC_52M))
-#define CHK_MMC_4BIT(sd_card)	\
-	(CHK_MMC(sd_card) && ((sd_card)->sd_type & MMC_4BIT))
-#define CHK_MMC_8BIT(sd_card)	\
-	(CHK_MMC(sd_card) && ((sd_card)->sd_type & MMC_8BIT))
-#define CHK_MMC_SECTOR_MODE(sd_card)\
-	(CHK_MMC(sd_card) && ((sd_card)->sd_type & MMC_SECTOR_MODE))
-#define CHK_MMC_DDR52(sd_card)	\
-	(CHK_MMC(sd_card) && ((sd_card)->sd_type & MMC_DDR52))
-
-#define SET_MMC(sd_card)		((sd_card)->sd_type = TYPE_MMC)
-#define SET_MMC_26M(sd_card)		((sd_card)->sd_type |= MMC_26M)
-#define SET_MMC_52M(sd_card)		((sd_card)->sd_type |= MMC_52M)
-#define SET_MMC_4BIT(sd_card)		((sd_card)->sd_type |= MMC_4BIT)
-#define SET_MMC_8BIT(sd_card)		((sd_card)->sd_type |= MMC_8BIT)
-#define SET_MMC_SECTOR_MODE(sd_card)	((sd_card)->sd_type |= MMC_SECTOR_MODE)
-#define SET_MMC_DDR52(sd_card)		((sd_card)->sd_type |= MMC_DDR52)
-
-#define CLR_MMC_26M(sd_card)		((sd_card)->sd_type &= ~MMC_26M)
-#define CLR_MMC_52M(sd_card)		((sd_card)->sd_type &= ~MMC_52M)
-#define CLR_MMC_4BIT(sd_card)		((sd_card)->sd_type &= ~MMC_4BIT)
-#define CLR_MMC_8BIT(sd_card)		((sd_card)->sd_type &= ~MMC_8BIT)
-#define CLR_MMC_SECTOR_MODE(sd_card)	((sd_card)->sd_type &= ~MMC_SECTOR_MODE)
-#define CLR_MMC_DDR52(sd_card)		((sd_card)->sd_type &= ~MMC_DDR52)
-
-#define CHK_MMC_HS(sd_card)	\
-	(CHK_MMC_52M(sd_card) && CHK_MMC_26M(sd_card))
-#define CLR_MMC_HS(sd_card)			\
-do {						\
-	CLR_MMC_DDR52(sd_card);			\
-	CLR_MMC_52M(sd_card);			\
-	CLR_MMC_26M(sd_card);			\
-} while (0)
-
-#define SD_SUPPORT_CLASS_TEN		0x01
-#define SD_SUPPORT_1V8			0x02
-
-#define SD_SET_CLASS_TEN(sd_card)	\
-	((sd_card)->sd_setting |= SD_SUPPORT_CLASS_TEN)
-#define SD_CHK_CLASS_TEN(sd_card)	\
-	((sd_card)->sd_setting & SD_SUPPORT_CLASS_TEN)
-#define SD_CLR_CLASS_TEN(sd_card)	\
-	((sd_card)->sd_setting &= ~SD_SUPPORT_CLASS_TEN)
-#define SD_SET_1V8(sd_card)		\
-	((sd_card)->sd_setting |= SD_SUPPORT_1V8)
-#define SD_CHK_1V8(sd_card)		\
-	((sd_card)->sd_setting & SD_SUPPORT_1V8)
-#define SD_CLR_1V8(sd_card)		\
-	((sd_card)->sd_setting &= ~SD_SUPPORT_1V8)
-#define CLR_RETRY_SD20_MODE(sd_card)		\
-	((sd_card)->retry_SD20_mode = 0)
-#define SET_RETRY_SD20_MODE(sd_card)		\
-	((sd_card)->retry_SD20_mode = 1)
-#define CHK_RETRY_SD20_MODE(sd_card)		\
-	((sd_card)->retry_SD20_mode == 1)
-
-struct sd_info {
-	u16 sd_type;
-	u8 err_code;
-	u8 sd_data_buf_ready;
-	u32 sd_addr;
-	u32 capacity;
-
-	u8 raw_csd[16];
-	u8 raw_scr[8];
-
-	/* Sequential RW */
-	int seq_mode;
-	enum dma_data_direction pre_dir;
-	u32 pre_sec_addr;
-	u16 pre_sec_cnt;
-
-	int counter;
-
-	int sd_clock;
-
-#ifdef SUPPORT_CPRM
-	int sd_pass_thru_en;
-	int pre_cmd_err;
-	u8 last_rsp_type;
-	u8 rsp[17];
-#endif
-
-	u8 func_group1_mask;
-	u8 func_group2_mask;
-	u8 func_group3_mask;
-	u8 func_group4_mask;
-
-	u8 sd_switch_fail;
-	u8 sd_read_phase;
-	u8 retry_SD20_mode;	/* sangdy2010-06-10 */
-	u8 sd_reset_fail;	/* sangdy2010-07-01 */
-	u8 sd_send_status_en;
-
-};
-
-#define MODE_512_SEQ		0x01
-#define MODE_2K_SEQ		0x02
-
-#define TYPE_MS			0x0000
-#define TYPE_MSPRO		0x0001
-
-#define MS_4BIT			0x0100
-#define MS_8BIT			0x0200
-#define MS_HG			0x0400
-#define MS_XC			0x0800
-
-#define HG8BIT			(MS_HG | MS_8BIT)
-
-#define CHK_MSPRO(ms_card)	\
-	(((ms_card)->ms_type & 0xFF) == TYPE_MSPRO)
-#define CHK_HG8BIT(ms_card)	\
-	(CHK_MSPRO(ms_card) && (((ms_card)->ms_type & HG8BIT) == HG8BIT))
-#define CHK_MSXC(ms_card)	\
-	(CHK_MSPRO(ms_card) && ((ms_card)->ms_type & MS_XC))
-#define CHK_MSHG(ms_card)	\
-	(CHK_MSPRO(ms_card) && ((ms_card)->ms_type & MS_HG))
-
-#define CHK_MS8BIT(ms_card)	(((ms_card)->ms_type & MS_8BIT))
-#define CHK_MS4BIT(ms_card)	(((ms_card)->ms_type & MS_4BIT))
-
-struct rts51x_ms_delay_write_tag {
-	u16 old_phyblock;
-	u16 new_phyblock;
-	u16 logblock;
-	u8 pageoff;
-	u8 delay_write_flag;
-};
-
-struct ms_info {
-	u16 ms_type;
-	u8 block_shift;
-	u8 page_off;
-	u16 total_block;
-	u16 boot_block;
-	u32 capacity;
-
-	u8 check_ms_flow;
-	u8 switch_8bit_fail;
-	u8 err_code;
-
-	struct zone_entry *segment;
-	int segment_cnt;
-
-	int pro_under_formatting;
-	int format_status;
-	u16 progress;
-	u8 raw_sys_info[96];
-#ifdef SUPPORT_PCGL_1P18
-	u8 raw_model_name[48];
-#endif
-
-	u8 multi_flag;
-
-	/* Sequential RW */
-	u8 seq_mode;
-	enum dma_data_direction pre_dir;
-	u32 pre_sec_addr;
-	u16 pre_sec_cnt;
-	u32 total_sec_cnt;
-	u8 last_rw_int;
-
-	struct rts51x_ms_delay_write_tag delay_write;
-
-	int counter;
-
-	int ms_clock;
-
-#ifdef SUPPORT_MAGIC_GATE
-	u8 magic_gate_id[16];
-	u8 mg_entry_num;
-	int mg_auth;		/* flag to indicate authentication process */
-#endif
-};
-
-#define PRO_UNDER_FORMATTING(ms_card)		\
-	((ms_card)->pro_under_formatting)
-#define SET_FORMAT_STATUS(ms_card, status)	\
-	((ms_card)->format_status = (status))
-#define CHK_FORMAT_STATUS(ms_card, status)	\
-	((ms_card)->format_status == (status))
-
-struct scsi_cmnd;
-
-enum CHIP_STAT { STAT_INIT, STAT_IDLE, STAT_RUN, STAT_SS_PRE, STAT_SS,
-	    STAT_SUSPEND };
-
-struct rts51x_chip {
-	u16 vendor_id;
-	u16 product_id;
-	char max_lun;
-
-	struct scsi_cmnd *srb;
-	struct sense_data_t sense_buffer[MAX_ALLOWED_LUN_CNT];
-
-	int led_toggle_counter;
-
-	int ss_counter;
-	int idle_counter;
-	int auto_delink_counter;
-	enum CHIP_STAT chip_stat;
-
-	int resume_from_scsi;
-
-	/* Card information */
-	struct xd_info xd_card;
-	struct sd_info sd_card;
-	struct ms_info ms_card;
-
-	int cur_clk;		/* current card clock */
-	int cur_card;		/* Current card module */
-
-	u8 card_exist;		/* card exist bit map (physical exist) */
-	u8 card_ready;		/* card ready bit map (reset successfully) */
-	u8 card_fail;		/* card reset fail bit map */
-	u8 card_ejected;	/* card ejected bit map */
-	u8 card_wp;		/* card write protected bit map */
-
-	u8 fake_card_ready;
-	/* flag to indicate whether to answer MediaChange */
-	unsigned long lun_mc;
-
-	/* card bus width */
-	u8 card_bus_width[MAX_ALLOWED_LUN_CNT];
-	/* card capacity */
-	u32 capacity[MAX_ALLOWED_LUN_CNT];
-
-	/* read/write card function pointer */
-	rts51x_card_rw_func rw_card[MAX_ALLOWED_LUN_CNT];
-	/* read/write capacity, used for GPIO Toggle */
-	u32 rw_cap[MAX_ALLOWED_LUN_CNT];
-	/* card to lun mapping table */
-	u8 card2lun[32];
-	/* lun to card mapping table */
-	u8 lun2card[MAX_ALLOWED_LUN_CNT];
-
-#ifdef _MSG_TRACE
-	struct trace_msg_t trace_msg[TRACE_ITEM_CNT];
-	int msg_idx;
-#endif
-
-	int rw_need_retry;
-
-	/* ASIC or FPGA */
-	int asic_code;
-
-	/* QFN24 or LQFP48 */
-	int package;
-
-	/* Full Speed or High Speed */
-	int usb_speed;
-
-	/*sangdy:enable or disable UHS50 and MMC4.4 */
-	int uhs50_mmc44_en;
-
-	u8 ic_version;
-
-	/* Command buffer */
-	u8 *cmd_buf;
-	unsigned int cmd_idx;
-	/* Response buffer */
-	u8 *rsp_buf;
-
-	u16 card_status;
-
-#ifdef SUPPORT_OCP
-	u16 ocp_stat;
-#endif
-
-	struct rts51x_option option;
-	struct rts51x_usb *usb;
-
-	u8 rcc_read_response;
-	int reset_need_retry;
-	u8 rts5179;
-};
-
-#define UHS50_EN 0x0001
-#define UHS50_DIS 0x0000
-#define SET_UHS50(chip)   ((chip)->uhs50_mmc44_en = UHS50_EN)
-#define CLEAR_UHS50(chip)  ((chip)->uhs50_mmc44_en = UHS50_DIS)
-#define CHECK_UHS50(chip)  (((chip)->uhs50_mmc44_en&0xff) == UHS50_EN)
-
-#define RTS51X_GET_VID(chip)		((chip)->vendor_id)
-#define RTS51X_GET_PID(chip)		((chip)->product_id)
-
-#define RTS51X_SET_STAT(chip, stat)			\
-do {							\
-	if ((stat) != STAT_IDLE) {			\
-		(chip)->idle_counter = 0;		\
-	}						\
-	(chip)->chip_stat = (enum CHIP_STAT)(stat);	\
-} while (0)
-#define RTS51X_CHK_STAT(chip, stat)	((chip)->chip_stat == (stat))
-#define RTS51X_GET_STAT(chip)		((chip)->chip_stat)
-
-#define CHECK_PID(chip, pid)		(RTS51X_GET_PID(chip) == (pid))
-#define CHECK_PKG(chip, pkg)		((chip)->package == (pkg))
-#define CHECK_USB(chip, speed)		((chip)->usb_speed == (speed))
-
-int rts51x_reset_chip(struct rts51x_chip *chip);
-int rts51x_init_chip(struct rts51x_chip *chip);
-int rts51x_release_chip(struct rts51x_chip *chip);
-void rts51x_polling_func(struct rts51x_chip *chip);
-
-static inline void rts51x_init_cmd(struct rts51x_chip *chip)
-{
-	chip->cmd_idx = 0;
-	chip->cmd_buf[0] = 'R';
-	chip->cmd_buf[1] = 'T';
-	chip->cmd_buf[2] = 'C';
-	chip->cmd_buf[3] = 'R';
-	chip->cmd_buf[PACKET_TYPE] = BATCH_CMD;
-}
-
-void rts51x_add_cmd(struct rts51x_chip *chip,
-		    u8 cmd_type, u16 reg_addr, u8 mask, u8 data);
-int rts51x_send_cmd(struct rts51x_chip *chip, u8 flag, int timeout);
-int rts51x_get_rsp(struct rts51x_chip *chip, int rsp_len, int timeout);
-
-static inline void rts51x_read_rsp_buf(struct rts51x_chip *chip, int offset,
-				       u8 *buf, int buf_len)
-{
-	memcpy(buf, chip->rsp_buf + offset, buf_len);
-}
-
-static inline u8 *rts51x_get_rsp_data(struct rts51x_chip *chip)
-{
-	return chip->rsp_buf;
-}
-
-int rts51x_get_card_status(struct rts51x_chip *chip, u16 *status);
-int rts51x_write_register(struct rts51x_chip *chip, u16 addr, u8 mask, u8 data);
-int rts51x_read_register(struct rts51x_chip *chip, u16 addr, u8 *data);
-int rts51x_ep0_write_register(struct rts51x_chip *chip, u16 addr, u8 mask,
-			      u8 data);
-int rts51x_ep0_read_register(struct rts51x_chip *chip, u16 addr, u8 *data);
-int rts51x_seq_write_register(struct rts51x_chip *chip, u16 addr, u16 len,
-			      u8 *data);
-int rts51x_seq_read_register(struct rts51x_chip *chip, u16 addr, u16 len,
-			     u8 *data);
-int rts51x_read_ppbuf(struct rts51x_chip *chip, u8 *buf, int buf_len);
-int rts51x_write_ppbuf(struct rts51x_chip *chip, u8 *buf, int buf_len);
-int rts51x_write_phy_register(struct rts51x_chip *chip, u8 addr, u8 val);
-int rts51x_read_phy_register(struct rts51x_chip *chip, u8 addr, u8 *val);
-void rts51x_do_before_power_down(struct rts51x_chip *chip);
-void rts51x_clear_hw_error(struct rts51x_chip *chip);
-void rts51x_prepare_run(struct rts51x_chip *chip);
-void rts51x_trace_msg(struct rts51x_chip *chip, unsigned char *buf, int clear);
-void rts51x_pp_status(struct rts51x_chip *chip, unsigned int lun, u8 *status,
-		      u8 status_len);
-void rts51x_read_status(struct rts51x_chip *chip, unsigned int lun,
-			u8 *rts51x_status, u8 status_len);
-int rts51x_transfer_data_rcc(struct rts51x_chip *chip, unsigned int pipe,
-			     void *buf, unsigned int len, int use_sg,
-			     unsigned int *act_len, int timeout, u8 stage_flag);
-
-#define RTS51X_WRITE_REG(chip, addr, mask, data)	\
-do {							\
-	int _retval = rts51x_write_register((chip),	\
-			(addr), (mask), (data));	\
-	if (_retval != STATUS_SUCCESS) {		\
-		TRACE_RET((chip), _retval);		\
-	}						\
-} while (0)
-
-#define RTS51X_READ_REG(chip, addr, data)		\
-do {							\
-	int _retval = rts51x_read_register((chip),	\
-			(addr), (data));		\
-	if (_retval != STATUS_SUCCESS) {		\
-		TRACE_RET((chip), _retval);		\
-	}						\
-} while (0)
-
-#endif /* __RTS51X_CHIP_H */
diff --git a/drivers/staging/rts5139/rts51x_fop.c b/drivers/staging/rts5139/rts51x_fop.c
deleted file mode 100644
index 677d18b3dcd..00000000000
--- a/drivers/staging/rts5139/rts51x_fop.c
+++ /dev/null
@@ -1,292 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#include "rts51x.h"
-
-#ifdef SUPPORT_FILE_OP
-
-#include <linux/types.h>
-#include <linux/stat.h>
-#include <linux/kref.h>
-#include <linux/slab.h>
-
-#include "rts51x_chip.h"
-#include "rts51x_card.h"
-#include "rts51x_fop.h"
-#include "sd_cprm.h"
-
-#define RTS5139_IOC_MAGIC		0x39
-
-#define RTS5139_IOC_SD_DIRECT		_IOWR(RTS5139_IOC_MAGIC, 0xA0, int)
-#define RTS5139_IOC_SD_GET_RSP		_IOWR(RTS5139_IOC_MAGIC, 0xA1, int)
-
-static int rts51x_sd_direct_cmnd(struct rts51x_chip *chip,
-				 struct sd_direct_cmnd *cmnd)
-{
-	int retval;
-	u8 dir, cmd12, standby, acmd, cmd_idx, rsp_code;
-	u8 *buf;
-	u32 arg, len;
-
-	dir = (cmnd->cmnd[0] >> 3) & 0x03;
-	cmd12 = (cmnd->cmnd[0] >> 2) & 0x01;
-	standby = (cmnd->cmnd[0] >> 1) & 0x01;
-	acmd = cmnd->cmnd[0] & 0x01;
-	cmd_idx = cmnd->cmnd[1];
-	arg = ((u32) (cmnd->cmnd[2]) << 24) | ((u32) (cmnd->cmnd[3]) << 16) |
-	    ((u32) (cmnd->cmnd[4]) << 8) | cmnd->cmnd[5];
-	len =
-	    ((u32) (cmnd->cmnd[6]) << 16) | ((u32) (cmnd->cmnd[7]) << 8) |
-	    cmnd->cmnd[8];
-	rsp_code = cmnd->cmnd[9];
-
-	if (dir) {
-		if (!cmnd->buf || (cmnd->buf_len < len))
-			TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	switch (dir) {
-	case 0:
-		/* No data */
-		retval = ext_rts51x_sd_execute_no_data(chip, chip->card2lun[SD_CARD],
-						cmd_idx, standby, acmd,
-						rsp_code, arg);
-		if (retval != TRANSPORT_GOOD)
-			TRACE_RET(chip, STATUS_FAIL);
-		break;
-
-	case 1:
-		/* Read from card */
-		buf = kzalloc(cmnd->buf_len, GFP_KERNEL);
-		if (!buf)
-			TRACE_RET(chip, STATUS_NOMEM);
-
-		retval = ext_rts51x_sd_execute_read_data(chip, chip->card2lun[SD_CARD],
-						  cmd_idx, cmd12, standby, acmd,
-						  rsp_code, arg, len, buf,
-						  cmnd->buf_len, 0);
-		if (retval != TRANSPORT_GOOD) {
-			kfree(buf);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		retval =
-		    copy_to_user(cmnd->buf, (void *)buf, cmnd->buf_len);
-		if (retval) {
-			kfree(buf);
-			TRACE_RET(chip, STATUS_NOMEM);
-		}
-
-		kfree(buf);
-		break;
-
-	case 2:
-		/* Write to card */
-		buf = kmalloc(cmnd->buf_len, GFP_KERNEL);
-		if (!buf)
-			TRACE_RET(chip, STATUS_NOMEM);
-
-		retval =
-		    copy_from_user((void *)buf, cmnd->buf,
-				   cmnd->buf_len);
-		if (retval) {
-			kfree(buf);
-			TRACE_RET(chip, STATUS_NOMEM);
-		}
-
-		retval =
-		    ext_rts51x_sd_execute_write_data(chip, chip->card2lun[SD_CARD],
-					      cmd_idx, cmd12, standby, acmd,
-					      rsp_code, arg, len, buf,
-					      cmnd->buf_len, 0);
-		if (retval != TRANSPORT_GOOD) {
-			kfree(buf);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		kfree(buf);
-
-		break;
-
-	default:
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int rts51x_sd_get_rsp(struct rts51x_chip *chip, struct sd_rsp *rsp)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int count = 0, retval;
-
-	if (sd_card->pre_cmd_err) {
-		sd_card->pre_cmd_err = 0;
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if (sd_card->last_rsp_type == SD_RSP_TYPE_R0)
-		TRACE_RET(chip, STATUS_FAIL);
-	else if (sd_card->last_rsp_type == SD_RSP_TYPE_R2)
-		count = (rsp->rsp_len < 17) ? rsp->rsp_len : 17;
-	else
-		count = (rsp->rsp_len < 6) ? rsp->rsp_len : 6;
-
-	retval = copy_to_user(rsp->rsp, (void *)sd_card->rsp, count);
-	if (retval)
-		TRACE_RET(chip, STATUS_NOMEM);
-
-	RTS51X_DEBUGP("Response length: %d\n", count);
-	RTS51X_DEBUGP("Response: 0x%x 0x%x 0x%x 0x%x\n",
-		       sd_card->rsp[0], sd_card->rsp[1], sd_card->rsp[2],
-		       sd_card->rsp[3]);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_open(struct inode *inode, struct file *filp)
-{
-	struct rts51x_chip *chip;
-	struct usb_interface *interface;
-	int subminor;
-	int retval = 0;
-
-	subminor = iminor(inode);
-
-	interface = usb_find_interface(&rts51x_driver, subminor);
-	if (!interface) {
-		RTS51X_DEBUGP("%s - error, can't find device for minor %d\n",
-			       __func__, subminor);
-		retval = -ENODEV;
-		goto exit;
-	}
-
-	chip = (struct rts51x_chip *)usb_get_intfdata(interface);
-	if (!chip) {
-		RTS51X_DEBUGP("Can't find chip\n");
-		retval = -ENODEV;
-		goto exit;
-	}
-
-	/* Increase our reference to the host */
-	scsi_host_get(rts51x_to_host(chip));
-
-	/* lock the device pointers */
-	mutex_lock(&(chip->usb->dev_mutex));
-
-	/* save our object in the file's private structure */
-	filp->private_data = chip;
-
-	/* unlock the device pointers */
-	mutex_unlock(&chip->usb->dev_mutex);
-
-exit:
-	return retval;
-}
-
-int rts51x_release(struct inode *inode, struct file *filp)
-{
-	struct rts51x_chip *chip;
-
-	chip = (struct rts51x_chip *)filp->private_data;
-	if (chip == NULL)
-		return -ENODEV;
-
-	/* Drop our reference to the host; the SCSI core will free it
-	 * (and "chip" along with it) when the refcount becomes 0. */
-	scsi_host_put(rts51x_to_host(chip));
-
-	return 0;
-}
-
-ssize_t rts51x_read(struct file *filp, char __user *buf, size_t count,
-		    loff_t *f_pos)
-{
-	return 0;
-}
-
-ssize_t rts51x_write(struct file *filp, const char __user *buf, size_t count,
-		     loff_t *f_pos)
-{
-	return 0;
-}
-
-long rts51x_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
-{
-	struct rts51x_chip *chip;
-	struct sd_direct_cmnd cmnd;
-	struct sd_rsp rsp;
-	int retval = 0;
-
-	chip = (struct rts51x_chip *)filp->private_data;
-	if (chip == NULL)
-		return -ENODEV;
-
-	/* lock the device pointers */
-	mutex_lock(&(chip->usb->dev_mutex));
-
-	switch (cmd) {
-	case RTS5139_IOC_SD_DIRECT:
-		retval =
-		    copy_from_user((void *)&cmnd, (void __user *)arg,
-				   sizeof(struct sd_direct_cmnd));
-		if (retval) {
-			retval = -ENOMEM;
-			TRACE_GOTO(chip, exit);
-		}
-		retval = rts51x_sd_direct_cmnd(chip, &cmnd);
-		if (retval != STATUS_SUCCESS) {
-			retval = -EIO;
-			TRACE_GOTO(chip, exit);
-		}
-		break;
-
-	case RTS5139_IOC_SD_GET_RSP:
-		retval =
-		    copy_from_user(&rsp, (void __user *)arg,
-				   sizeof(struct sd_rsp));
-		if (retval) {
-			retval = -ENOMEM;
-			TRACE_GOTO(chip, exit);
-		}
-		retval = rts51x_sd_get_rsp(chip, &rsp);
-		if (retval != STATUS_SUCCESS) {
-			retval = -EIO;
-			TRACE_GOTO(chip, exit);
-		}
-		break;
-
-	default:
-		break;
-	}
-
-exit:
-	/* unlock the device pointers */
-	mutex_unlock(&chip->usb->dev_mutex);
-
-	return retval;
-}
-
-#endif
diff --git a/drivers/staging/rts5139/rts51x_fop.h b/drivers/staging/rts5139/rts51x_fop.h
deleted file mode 100644
index c691ee99720..00000000000
--- a/drivers/staging/rts5139/rts51x_fop.h
+++ /dev/null
@@ -1,57 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#ifndef __RTS51X_FOP_H
-#define __RTS51X_FOP_H
-
-#include "rts51x.h"
-
-#ifdef SUPPORT_FILE_OP
-
-#include <linux/fs.h>
-#include <linux/types.h>
-
-struct sd_direct_cmnd {
-	u8 cmnd[12];
-	void __user *buf;
-	int buf_len;
-};
-
-struct sd_rsp {
-	void __user *rsp;
-	int rsp_len;
-};
-
-int rts51x_open(struct inode *inode, struct file *filp);
-int rts51x_release(struct inode *inode, struct file *filp);
-ssize_t rts51x_read(struct file *filp, char __user *buf, size_t count,
-		    loff_t *f_pos);
-ssize_t rts51x_write(struct file *filp, const char __user *buf, size_t count,
-		     loff_t *f_pos);
-long rts51x_ioctl(struct file *filp, unsigned int cmd, unsigned long arg);
-
-#endif
-
-#endif /* __RTS51X_FOP_H */
diff --git a/drivers/staging/rts5139/rts51x_scsi.c b/drivers/staging/rts5139/rts51x_scsi.c
deleted file mode 100644
index 75282fea38f..00000000000
--- a/drivers/staging/rts5139/rts51x_scsi.c
+++ /dev/null
@@ -1,2135 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#include <linux/blkdev.h>
-#include <linux/kthread.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
-#include <linux/vmalloc.h>
-#include <linux/export.h>
-
-#include <scsi/scsi.h>
-#include <scsi/scsi_eh.h>
-#include <scsi/scsi_device.h>
-
-#include "debug.h"
-#include "rts51x.h"
-#include "rts51x_chip.h"
-#include "rts51x_scsi.h"
-#include "rts51x_card.h"
-#include "rts51x_transport.h"
-#include "sd_cprm.h"
-#include "ms_mg.h"
-#include "trace.h"
-
-void rts51x_scsi_show_command(struct scsi_cmnd *srb)
-{
-	char *what = NULL;
-	int i, unknown_cmd = 0;
-
-	switch (srb->cmnd[0]) {
-	case TEST_UNIT_READY:
-		what = (char *)"TEST_UNIT_READY";
-		break;
-	case REZERO_UNIT:
-		what = (char *)"REZERO_UNIT";
-		break;
-	case REQUEST_SENSE:
-		what = (char *)"REQUEST_SENSE";
-		break;
-	case FORMAT_UNIT:
-		what = (char *)"FORMAT_UNIT";
-		break;
-	case READ_BLOCK_LIMITS:
-		what = (char *)"READ_BLOCK_LIMITS";
-		break;
-	case 0x07:
-		what = (char *)"REASSIGN_BLOCKS";
-		break;
-	case READ_6:
-		what = (char *)"READ_6";
-		break;
-	case WRITE_6:
-		what = (char *)"WRITE_6";
-		break;
-	case SEEK_6:
-		what = (char *)"SEEK_6";
-		break;
-	case READ_REVERSE:
-		what = (char *)"READ_REVERSE";
-		break;
-	case WRITE_FILEMARKS:
-		what = (char *)"WRITE_FILEMARKS";
-		break;
-	case SPACE:
-		what = (char *)"SPACE";
-		break;
-	case INQUIRY:
-		what = (char *)"INQUIRY";
-		break;
-	case RECOVER_BUFFERED_DATA:
-		what = (char *)"RECOVER_BUFFERED_DATA";
-		break;
-	case MODE_SELECT:
-		what = (char *)"MODE_SELECT";
-		break;
-	case RESERVE:
-		what = (char *)"RESERVE";
-		break;
-	case RELEASE:
-		what = (char *)"RELEASE";
-		break;
-	case COPY:
-		what = (char *)"COPY";
-		break;
-	case ERASE:
-		what = (char *)"ERASE";
-		break;
-	case MODE_SENSE:
-		what = (char *)"MODE_SENSE";
-		break;
-	case START_STOP:
-		what = (char *)"START_STOP";
-		break;
-	case RECEIVE_DIAGNOSTIC:
-		what = (char *)"RECEIVE_DIAGNOSTIC";
-		break;
-	case SEND_DIAGNOSTIC:
-		what = (char *)"SEND_DIAGNOSTIC";
-		break;
-	case ALLOW_MEDIUM_REMOVAL:
-		what = (char *)"ALLOW_MEDIUM_REMOVAL";
-		break;
-	case SET_WINDOW:
-		what = (char *)"SET_WINDOW";
-		break;
-	case READ_CAPACITY:
-		what = (char *)"READ_CAPACITY";
-		break;
-	case READ_10:
-		what = (char *)"READ_10";
-		break;
-	case WRITE_10:
-		what = (char *)"WRITE_10";
-		break;
-	case SEEK_10:
-		what = (char *)"SEEK_10";
-		break;
-	case WRITE_VERIFY:
-		what = (char *)"WRITE_VERIFY";
-		break;
-	case VERIFY:
-		what = (char *)"VERIFY";
-		break;
-	case SEARCH_HIGH:
-		what = (char *)"SEARCH_HIGH";
-		break;
-	case SEARCH_EQUAL:
-		what = (char *)"SEARCH_EQUAL";
-		break;
-	case SEARCH_LOW:
-		what = (char *)"SEARCH_LOW";
-		break;
-	case SET_LIMITS:
-		what = (char *)"SET_LIMITS";
-		break;
-	case READ_POSITION:
-		what = (char *)"READ_POSITION";
-		break;
-	case SYNCHRONIZE_CACHE:
-		what = (char *)"SYNCHRONIZE_CACHE";
-		break;
-	case LOCK_UNLOCK_CACHE:
-		what = (char *)"LOCK_UNLOCK_CACHE";
-		break;
-	case READ_DEFECT_DATA:
-		what = (char *)"READ_DEFECT_DATA";
-		break;
-	case MEDIUM_SCAN:
-		what = (char *)"MEDIUM_SCAN";
-		break;
-	case COMPARE:
-		what = (char *)"COMPARE";
-		break;
-	case COPY_VERIFY:
-		what = (char *)"COPY_VERIFY";
-		break;
-	case WRITE_BUFFER:
-		what = (char *)"WRITE_BUFFER";
-		break;
-	case READ_BUFFER:
-		what = (char *)"READ_BUFFER";
-		break;
-	case UPDATE_BLOCK:
-		what = (char *)"UPDATE_BLOCK";
-		break;
-	case READ_LONG:
-		what = (char *)"READ_LONG";
-		break;
-	case WRITE_LONG:
-		what = (char *)"WRITE_LONG";
-		break;
-	case CHANGE_DEFINITION:
-		what = (char *)"CHANGE_DEFINITION";
-		break;
-	case WRITE_SAME:
-		what = (char *)"WRITE_SAME";
-		break;
-	case GPCMD_READ_SUBCHANNEL:
-		what = (char *)"READ SUBCHANNEL";
-		break;
-	case READ_TOC:
-		what = (char *)"READ_TOC";
-		break;
-	case GPCMD_READ_HEADER:
-		what = (char *)"READ HEADER";
-		break;
-	case GPCMD_PLAY_AUDIO_10:
-		what = (char *)"PLAY AUDIO (10)";
-		break;
-	case GPCMD_PLAY_AUDIO_MSF:
-		what = (char *)"PLAY AUDIO MSF";
-		break;
-	case GPCMD_GET_EVENT_STATUS_NOTIFICATION:
-		what = (char *)"GET EVENT/STATUS NOTIFICATION";
-		break;
-	case GPCMD_PAUSE_RESUME:
-		what = (char *)"PAUSE/RESUME";
-		break;
-	case LOG_SELECT:
-		what = (char *)"LOG_SELECT";
-		break;
-	case LOG_SENSE:
-		what = (char *)"LOG_SENSE";
-		break;
-	case GPCMD_STOP_PLAY_SCAN:
-		what = (char *)"STOP PLAY/SCAN";
-		break;
-	case GPCMD_READ_DISC_INFO:
-		what = (char *)"READ DISC INFORMATION";
-		break;
-	case GPCMD_READ_TRACK_RZONE_INFO:
-		what = (char *)"READ TRACK INFORMATION";
-		break;
-	case GPCMD_RESERVE_RZONE_TRACK:
-		what = (char *)"RESERVE TRACK";
-		break;
-	case GPCMD_SEND_OPC:
-		what = (char *)"SEND OPC";
-		break;
-	case MODE_SELECT_10:
-		what = (char *)"MODE_SELECT_10";
-		break;
-	case GPCMD_REPAIR_RZONE_TRACK:
-		what = (char *)"REPAIR TRACK";
-		break;
-	case 0x59:
-		what = (char *)"READ MASTER CUE";
-		break;
-	case MODE_SENSE_10:
-		what = (char *)"MODE_SENSE_10";
-		break;
-	case GPCMD_CLOSE_TRACK:
-		what = (char *)"CLOSE TRACK/SESSION";
-		break;
-	case 0x5C:
-		what = (char *)"READ BUFFER CAPACITY";
-		break;
-	case 0x5D:
-		what = (char *)"SEND CUE SHEET";
-		break;
-	case GPCMD_BLANK:
-		what = (char *)"BLANK";
-		break;
-	case REPORT_LUNS:
-		what = (char *)"REPORT LUNS";
-		break;
-	case MOVE_MEDIUM:
-		what = (char *)"MOVE_MEDIUM or PLAY AUDIO (12)";
-		break;
-	case READ_12:
-		what = (char *)"READ_12";
-		break;
-	case WRITE_12:
-		what = (char *)"WRITE_12";
-		break;
-	case WRITE_VERIFY_12:
-		what = (char *)"WRITE_VERIFY_12";
-		break;
-	case SEARCH_HIGH_12:
-		what = (char *)"SEARCH_HIGH_12";
-		break;
-	case SEARCH_EQUAL_12:
-		what = (char *)"SEARCH_EQUAL_12";
-		break;
-	case SEARCH_LOW_12:
-		what = (char *)"SEARCH_LOW_12";
-		break;
-	case SEND_VOLUME_TAG:
-		what = (char *)"SEND_VOLUME_TAG";
-		break;
-	case READ_ELEMENT_STATUS:
-		what = (char *)"READ_ELEMENT_STATUS";
-		break;
-	case GPCMD_READ_CD_MSF:
-		what = (char *)"READ CD MSF";
-		break;
-	case GPCMD_SCAN:
-		what = (char *)"SCAN";
-		break;
-	case GPCMD_SET_SPEED:
-		what = (char *)"SET CD SPEED";
-		break;
-	case GPCMD_MECHANISM_STATUS:
-		what = (char *)"MECHANISM STATUS";
-		break;
-	case GPCMD_READ_CD:
-		what = (char *)"READ CD";
-		break;
-	case 0xE1:
-		what = (char *)"WRITE CONTINUE";
-		break;
-	case WRITE_LONG_2:
-		what = (char *)"WRITE_LONG_2";
-		break;
-	case VENDOR_CMND:
-		what = (char *)"Realtek's vendor command";
-		break;
-	default:
-		what = (char *)"(unknown command)";
-		unknown_cmd = 1;
-		break;
-	}
-
-	if (srb->cmnd[0] != TEST_UNIT_READY)
-		RTS51X_DEBUGP("Command %s (%d bytes)\n", what, srb->cmd_len);
-	if (unknown_cmd) {
-		RTS51X_DEBUGP("");
-		for (i = 0; i < srb->cmd_len && i < 16; i++)
-			RTS51X_DEBUGPN(" %02x", srb->cmnd[i]);
-		RTS51X_DEBUGPN("\n");
-	}
-}
-
-void rts51x_set_sense_type(struct rts51x_chip *chip, unsigned int lun, int sense_type)
-{
-	switch (sense_type) {
-	case SENSE_TYPE_MEDIA_CHANGE:
-		rts51x_set_sense_data(chip, lun, CUR_ERR, 0x06, 0, 0x28, 0, 0, 0);
-		break;
-
-	case SENSE_TYPE_MEDIA_NOT_PRESENT:
-		rts51x_set_sense_data(chip, lun, CUR_ERR, 0x02, 0, 0x3A, 0, 0, 0);
-		break;
-
-	case SENSE_TYPE_MEDIA_LBA_OVER_RANGE:
-		rts51x_set_sense_data(chip, lun, CUR_ERR, 0x05, 0, 0x21, 0, 0, 0);
-		break;
-
-	case SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT:
-		rts51x_set_sense_data(chip, lun, CUR_ERR, 0x05, 0, 0x25, 0, 0, 0);
-		break;
-
-	case SENSE_TYPE_MEDIA_WRITE_PROTECT:
-		rts51x_set_sense_data(chip, lun, CUR_ERR, 0x07, 0, 0x27, 0, 0, 0);
-		break;
-
-	case SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR:
-		rts51x_set_sense_data(chip, lun, CUR_ERR, 0x03, 0, 0x11, 0, 0, 0);
-		break;
-
-	case SENSE_TYPE_MEDIA_WRITE_ERR:
-		rts51x_set_sense_data(chip, lun, CUR_ERR, 0x03, 0, 0x0C, 0x02, 0, 0);
-		break;
-
-	case SENSE_TYPE_MEDIA_INVALID_CMD_FIELD:
-		rts51x_set_sense_data(chip, lun, CUR_ERR, ILGAL_REQ, 0,
-			       ASC_INVLD_CDB, ASCQ_INVLD_CDB, CDB_ILLEGAL, 1);
-		break;
-
-	case SENSE_TYPE_FORMAT_CMD_FAILED:
-		rts51x_set_sense_data(chip, lun, CUR_ERR, 0x03, 0, 0x31, 0x01, 0, 0);
-		break;
-
-#ifdef SUPPORT_MAGIC_GATE
-	case SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB:
-		rts51x_set_sense_data(chip, lun, CUR_ERR, 0x05, 0, 0x6F, 0x02, 0, 0);
-		break;
-
-	case SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN:
-		rts51x_set_sense_data(chip, lun, CUR_ERR, 0x05, 0, 0x6F, 0x00, 0, 0);
-		break;
-
-	case SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM:
-		rts51x_set_sense_data(chip, lun, CUR_ERR, 0x02, 0, 0x30, 0x00, 0, 0);
-		break;
-
-	case SENSE_TYPE_MG_WRITE_ERR:
-		rts51x_set_sense_data(chip, lun, CUR_ERR, 0x03, 0, 0x0C, 0x00, 0, 0);
-		break;
-#endif
-
-	case SENSE_TYPE_NO_SENSE:
-	default:
-		rts51x_set_sense_data(chip, lun, CUR_ERR, 0, 0, 0, 0, 0, 0);
-		break;
-	}
-}
-
-void rts51x_set_sense_data(struct rts51x_chip *chip, unsigned int lun, u8 err_code,
-		    u8 sense_key, u32 info, u8 asc, u8 ascq, u8 sns_key_info0,
-		    u16 sns_key_info1)
-{
-	struct sense_data_t *sense = &(chip->sense_buffer[lun]);
-
-	sense->err_code = err_code;
-	sense->sense_key = sense_key;
-	sense->info[0] = (u8) (info >> 24);
-	sense->info[1] = (u8) (info >> 16);
-	sense->info[2] = (u8) (info >> 8);
-	sense->info[3] = (u8) info;
-
-	sense->ad_sense_len = sizeof(struct sense_data_t) - 8;
-	sense->asc = asc;
-	sense->ascq = ascq;
-	if (sns_key_info0 != 0) {
-		sense->sns_key_info[0] = SKSV | sns_key_info0;
-		sense->sns_key_info[1] = (sns_key_info1 & 0xf0) >> 8;
-		sense->sns_key_info[2] = sns_key_info1 & 0x0f;
-	}
-}
-
-static int test_unit_ready(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	unsigned int lun = SCSI_LUN(srb);
-
-	rts51x_init_cards(chip);
-
-	if (!check_card_ready(chip, lun)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		return TRANSPORT_FAILED;
-	}
-
-	if (!check_lun_mc(chip, lun)) {
-		set_lun_mc(chip, lun);
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
-		return TRANSPORT_FAILED;
-	}
-
-	return TRANSPORT_GOOD;
-}
-
-static unsigned char formatter_inquiry_str[20] = {
-	'M', 'E', 'M', 'O', 'R', 'Y', 'S', 'T', 'I', 'C', 'K',
-	'-', 'M', 'G',		/* Byte[47:49] */
-	0x0B,			/* Byte[50]: MG, MS, MSPro, MSXC */
-	0x00,			/* Byte[51]: Category Specific Commands */
-	0x00,			/* Byte[52]: Access Control and feature */
-	0x20, 0x20, 0x20,	/* Byte[53:55] */
-};
-
-static int inquiry(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	unsigned int lun = SCSI_LUN(srb);
-	char *inquiry_default = (char *)"Generic-xD/SD/M.S.      1.00 ";
-	char *inquiry_string;
-	unsigned char sendbytes;
-	unsigned char *buf;
-	u8 card = rts51x_get_lun_card(chip, lun);
-	int pro_formatter_flag = 0;
-	unsigned char inquiry_buf[] = {
-		QULIFIRE | DRCT_ACCESS_DEV,
-		RMB_DISC | 0x0D,
-		0x00,
-		0x01,
-		0x1f,
-		0x02,
-		0,
-		REL_ADR | WBUS_32 | WBUS_16 | SYNC | LINKED | CMD_QUE | SFT_RE,
-	};
-
-	inquiry_string = inquiry_default;
-
-	buf = vmalloc(scsi_bufflen(srb));
-	if (buf == NULL)
-		TRACE_RET(chip, TRANSPORT_ERROR);
-
-	if (MS_FORMATTER_ENABLED(chip) && (get_lun2card(chip, lun) & MS_CARD)) {
-		if (!card || (card == MS_CARD))
-			pro_formatter_flag = 1;
-	}
-
-	if (pro_formatter_flag) {
-		if (scsi_bufflen(srb) < 56)
-			sendbytes = (unsigned char)(scsi_bufflen(srb));
-		else
-			sendbytes = 56;
-	} else {
-		if (scsi_bufflen(srb) < 36)
-			sendbytes = (unsigned char)(scsi_bufflen(srb));
-		else
-			sendbytes = 36;
-	}
-
-	if (sendbytes > 8) {
-		memcpy(buf, inquiry_buf, 8);
-		memcpy(buf + 8, inquiry_string, sendbytes - 8);
-		if (pro_formatter_flag)
-			buf[4] = 0x33;	/* Additional Length */
-	} else {
-		memcpy(buf, inquiry_buf, sendbytes);
-	}
-
-	if (pro_formatter_flag) {
-		if (sendbytes > 36)
-			memcpy(buf + 36, formatter_inquiry_str, sendbytes - 36);
-	}
-
-	scsi_set_resid(srb, 0);
-
-	rts51x_set_xfer_buf(buf, scsi_bufflen(srb), srb);
-	vfree(buf);
-
-	return TRANSPORT_GOOD;
-}
-
-static int start_stop_unit(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	unsigned int lun = SCSI_LUN(srb);
-
-	scsi_set_resid(srb, scsi_bufflen(srb));
-
-	if (srb->cmnd[1] == 1)
-		return TRANSPORT_GOOD;
-
-	switch (srb->cmnd[0x4]) {
-	case STOP_MEDIUM:
-		/* Media disabled */
-		return TRANSPORT_GOOD;
-
-	case UNLOAD_MEDIUM:
-		/* Media shall be unload */
-		if (check_card_ready(chip, lun))
-			rts51x_eject_card(chip, lun);
-		return TRANSPORT_GOOD;
-
-	case MAKE_MEDIUM_READY:
-	case LOAD_MEDIUM:
-		if (check_card_ready(chip, lun)) {
-			return TRANSPORT_GOOD;
-		} else {
-			rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-
-		break;
-	}
-
-	TRACE_RET(chip, TRANSPORT_ERROR);
-}
-
-static int allow_medium_removal(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	int prevent;
-
-	prevent = srb->cmnd[4] & 0x1;
-
-	scsi_set_resid(srb, 0);
-
-	if (prevent) {
-		rts51x_set_sense_type(chip, SCSI_LUN(srb),
-			       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	return TRANSPORT_GOOD;
-}
-
-static void ms_mode_sense(struct rts51x_chip *chip, u8 cmd,
-			  int lun, u8 *buf, int buf_len)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	int sys_info_offset;
-	int data_size = buf_len;
-	int support_format = 0;
-	int i = 0;
-
-	if (cmd == MODE_SENSE) {
-		sys_info_offset = 8;
-		if (data_size > 0x68)
-			data_size = 0x68;
-		buf[i++] = 0x67;	/* Mode Data Length */
-	} else {
-		sys_info_offset = 12;
-		if (data_size > 0x6C)
-			data_size = 0x6C;
-		buf[i++] = 0x00;	/* Mode Data Length (MSB) */
-		buf[i++] = 0x6A;	/* Mode Data Length (LSB) */
-	}
-
-	/* Medium Type Code */
-	if (check_card_ready(chip, lun)) {
-		if (CHK_MSXC(ms_card)) {
-			support_format = 1;
-			buf[i++] = 0x40;
-		} else if (CHK_MSPRO(ms_card)) {
-			support_format = 1;
-			buf[i++] = 0x20;
-		} else {
-			buf[i++] = 0x10;
-		}
-
-		/* WP */
-		if (check_card_wp(chip, lun))
-			buf[i++] = 0x80;
-		else
-			buf[i++] = 0x00;
-	} else {
-		buf[i++] = 0x00;	/* MediaType */
-		buf[i++] = 0x00;	/* WP */
-	}
-
-	buf[i++] = 0x00;	/* Reserved */
-
-	if (cmd == MODE_SENSE_10) {
-		buf[i++] = 0x00;	/* Reserved */
-		buf[i++] = 0x00;	/* Block descriptor length(MSB) */
-		buf[i++] = 0x00;	/* Block descriptor length(LSB) */
-
-		/* The Following Data is the content of "Page 0x20" */
-		if (data_size >= 9)
-			buf[i++] = 0x20;	/* Page Code */
-		if (data_size >= 10)
-			buf[i++] = 0x62;	/* Page Length */
-		if (data_size >= 11)
-			buf[i++] = 0x00;	/* No Access Control */
-		if (data_size >= 12) {
-			if (support_format)
-				buf[i++] = 0xC0;	/* SF, SGM */
-			else
-				buf[i++] = 0x00;
-		}
-	} else {
-		/* The Following Data is the content of "Page 0x20" */
-		if (data_size >= 5)
-			buf[i++] = 0x20;	/* Page Code */
-		if (data_size >= 6)
-			buf[i++] = 0x62;	/* Page Length */
-		if (data_size >= 7)
-			buf[i++] = 0x00;	/* No Access Control */
-		if (data_size >= 8) {
-			if (support_format)
-				buf[i++] = 0xC0;	/* SF, SGM */
-			else
-				buf[i++] = 0x00;
-		}
-	}
-
-	if (data_size > sys_info_offset) {
-		/* 96 Bytes Attribute Data */
-		int len = data_size - sys_info_offset;
-		len = (len < 96) ? len : 96;
-
-		memcpy(buf + sys_info_offset, ms_card->raw_sys_info, len);
-	}
-}
-
-static int mode_sense(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	unsigned int lun = SCSI_LUN(srb);
-	unsigned int dataSize;
-	int status;
-	int pro_formatter_flag;
-	unsigned char pageCode, *buf;
-	u8 card = rts51x_get_lun_card(chip, lun);
-
-	if (!check_card_ready(chip, lun)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		scsi_set_resid(srb, scsi_bufflen(srb));
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	pro_formatter_flag = 0;
-	dataSize = 8;
-	/* In Combo mode, device responses ModeSense command as a MS LUN
-	 * when no card is inserted */
-	if ((get_lun2card(chip, lun) & MS_CARD)) {
-		if (!card || (card == MS_CARD)) {
-			dataSize = 108;
-			if (chip->option.rts51x_mspro_formatter_enable)
-				pro_formatter_flag = 1;
-		}
-	}
-
-	buf = kmalloc(dataSize, GFP_KERNEL);
-	if (buf == NULL)
-		TRACE_RET(chip, TRANSPORT_ERROR);
-
-	pageCode = srb->cmnd[2] & 0x3f;
-
-	if ((pageCode == 0x3F) || (pageCode == 0x1C) ||
-	    (pageCode == 0x00) || (pro_formatter_flag && (pageCode == 0x20))) {
-		if (srb->cmnd[0] == MODE_SENSE) {
-			if ((pageCode == 0x3F) || (pageCode == 0x20)) {
-				ms_mode_sense(chip, srb->cmnd[0], lun, buf,
-					      dataSize);
-			} else {
-				dataSize = 4;
-				buf[0] = 0x03;
-				buf[1] = 0x00;
-				if (check_card_wp(chip, lun))
-					buf[2] = 0x80;
-				else
-				buf[3] = 0x00;
-			}
-		} else {
-			if ((pageCode == 0x3F) || (pageCode == 0x20)) {
-				ms_mode_sense(chip, srb->cmnd[0], lun, buf,
-					      dataSize);
-			} else {
-				dataSize = 8;
-				buf[0] = 0x00;
-				buf[1] = 0x06;
-				buf[2] = 0x00;
-				if (check_card_wp(chip, lun))
-					buf[3] = 0x80;
-				else
-					buf[3] = 0x00;
-				buf[4] = 0x00;
-				buf[5] = 0x00;
-				buf[6] = 0x00;
-				buf[7] = 0x00;
-			}
-		}
-		status = TRANSPORT_GOOD;
-	} else {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		scsi_set_resid(srb, scsi_bufflen(srb));
-		status = TRANSPORT_FAILED;
-	}
-
-	if (status == TRANSPORT_GOOD) {
-		unsigned int len = min(scsi_bufflen(srb), dataSize);
-		rts51x_set_xfer_buf(buf, len, srb);
-		scsi_set_resid(srb, scsi_bufflen(srb) - len);
-	}
-	kfree(buf);
-
-	return status;
-}
-
-static int request_sense(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct sense_data_t *sense;
-	unsigned int lun = SCSI_LUN(srb);
-	struct ms_info *ms_card = &(chip->ms_card);
-	unsigned char *tmp, *buf;
-
-	sense = &(chip->sense_buffer[lun]);
-
-	if ((rts51x_get_lun_card(chip, lun) == MS_CARD)
-	    && PRO_UNDER_FORMATTING(ms_card)) {
-		rts51x_mspro_format_sense(chip, lun);
-	}
-
-	buf = vmalloc(scsi_bufflen(srb));
-	if (buf == NULL)
-		TRACE_RET(chip, TRANSPORT_ERROR);
-
-	tmp = (unsigned char *)sense;
-	memcpy(buf, tmp, scsi_bufflen(srb));
-
-	rts51x_set_xfer_buf(buf, scsi_bufflen(srb), srb);
-	vfree(buf);
-
-	scsi_set_resid(srb, 0);
-	/* Reset Sense Data */
-	rts51x_set_sense_type(chip, lun, SENSE_TYPE_NO_SENSE);
-	return TRANSPORT_GOOD;
-}
-
-static int read_write(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	unsigned int lun = SCSI_LUN(srb);
-	int retval;
-	u32 start_sec;
-	u16 sec_cnt;
-
-	if (!check_card_ready(chip, lun) || (chip->capacity[lun] == 0)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (!check_lun_mc(chip, lun)) {
-		set_lun_mc(chip, lun);
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
-		return TRANSPORT_FAILED;
-	}
-
-	rts51x_prepare_run(chip);
-	RTS51X_SET_STAT(chip, STAT_RUN);
-
-	if ((srb->cmnd[0] == READ_10) || (srb->cmnd[0] == WRITE_10)) {
-		start_sec =
-		    ((u32) srb->cmnd[2] << 24) |
-		    ((u32) srb->cmnd[3] << 16) |
-		    ((u32) srb->cmnd[4] << 8) |
-		    ((u32) srb->cmnd[5]);
-		sec_cnt = ((u16) (srb->cmnd[7]) << 8) | srb->cmnd[8];
-	} else if ((srb->cmnd[0] == READ_6) || (srb->cmnd[0] == WRITE_6)) {
-		start_sec = ((u32) (srb->cmnd[1] & 0x1F) << 16) |
-		    ((u32) srb->cmnd[2] << 8) | ((u32) srb->cmnd[3]);
-		sec_cnt = srb->cmnd[4];
-	} else if ((srb->cmnd[0] == VENDOR_CMND) &&
-			(srb->cmnd[1] == SCSI_APP_CMD) &&
-			((srb->cmnd[2] == PP_READ10) ||
-			 (srb->cmnd[2] == PP_WRITE10))) {
-		start_sec = ((u32) srb->cmnd[4] << 24) |
-			((u32) srb->cmnd[5] << 16) |
-			((u32) srb->cmnd[6] << 8) |
-			((u32) srb->cmnd[7]);
-		sec_cnt = ((u16) (srb->cmnd[9]) << 8) | srb->cmnd[10];
-	} else {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if ((start_sec > chip->capacity[lun]) ||
-	    ((start_sec + sec_cnt) > chip->capacity[lun])) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LBA_OVER_RANGE);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (sec_cnt == 0) {
-		scsi_set_resid(srb, 0);
-		return TRANSPORT_GOOD;
-	}
-
-	if ((srb->sc_data_direction == DMA_TO_DEVICE)
-	    && check_card_wp(chip, lun)) {
-		RTS51X_DEBUGP("Write protected card!\n");
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_PROTECT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	retval = rts51x_card_rw(srb, chip, start_sec, sec_cnt);
-	if (retval != STATUS_SUCCESS) {
-		if (srb->sc_data_direction == DMA_FROM_DEVICE) {
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-		} else {
-			rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_ERR);
-		}
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	scsi_set_resid(srb, 0);
-
-	return TRANSPORT_GOOD;
-}
-
-static int read_format_capacity(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	unsigned char *buf;
-	unsigned int lun = SCSI_LUN(srb);
-	unsigned int buf_len;
-	u8 card = rts51x_get_lun_card(chip, lun);
-	int desc_cnt;
-	int i = 0;
-
-	if (!check_card_ready(chip, lun)) {
-		if (!chip->option.rts51x_mspro_formatter_enable) {
-			rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-	}
-
-	buf_len = (scsi_bufflen(srb) > 12) ? 0x14 : 12;
-
-	buf = kmalloc(buf_len, GFP_KERNEL);
-	if (buf == NULL)
-		TRACE_RET(chip, TRANSPORT_ERROR);
-
-	buf[i++] = 0;
-	buf[i++] = 0;
-	buf[i++] = 0;
-
-	/* Capacity List Length */
-	if ((buf_len > 12) && chip->option.rts51x_mspro_formatter_enable &&
-	    (chip->lun2card[lun] & MS_CARD) && (!card || (card == MS_CARD))) {
-		buf[i++] = 0x10;
-		desc_cnt = 2;
-	} else {
-		buf[i++] = 0x08;
-		desc_cnt = 1;
-	}
-
-	while (desc_cnt) {
-		if (check_card_ready(chip, lun)) {
-			buf[i++] = (unsigned char)((chip->capacity[lun]) >> 24);
-			buf[i++] = (unsigned char)((chip->capacity[lun]) >> 16);
-			buf[i++] = (unsigned char)((chip->capacity[lun]) >> 8);
-			buf[i++] = (unsigned char)(chip->capacity[lun]);
-
-			if (desc_cnt == 2)
-				/* Byte[8]: Descriptor Type: Formatted medium */
-				buf[i++] = 2;
-			else
-				buf[i++] = 0;	/* Byte[16] */
-		} else {
-			buf[i++] = 0xFF;
-			buf[i++] = 0xFF;
-			buf[i++] = 0xFF;
-			buf[i++] = 0xFF;
-
-			if (desc_cnt == 2)
-				/* Byte[8]: Descriptor Type: No medium */
-				buf[i++] = 3;
-			else
-				buf[i++] = 0;	/*Byte[16] */
-		}
-
-		buf[i++] = 0x00;
-		buf[i++] = 0x02;
-		buf[i++] = 0x00;
-
-		desc_cnt--;
-	}
-
-	buf_len = min(scsi_bufflen(srb), buf_len);
-	rts51x_set_xfer_buf(buf, buf_len, srb);
-	kfree(buf);
-
-	scsi_set_resid(srb, scsi_bufflen(srb) - buf_len);
-
-	return TRANSPORT_GOOD;
-}
-
-static int read_capacity(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	unsigned char *buf;
-	unsigned int lun = SCSI_LUN(srb);
-
-	if (!check_card_ready(chip, lun)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (!check_lun_mc(chip, lun)) {
-		set_lun_mc(chip, lun);
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
-		return TRANSPORT_FAILED;
-	}
-
-	buf = kmalloc(8, GFP_KERNEL);
-	if (buf == NULL)
-		TRACE_RET(chip, TRANSPORT_ERROR);
-
-	buf[0] = (unsigned char)((chip->capacity[lun] - 1) >> 24);
-	buf[1] = (unsigned char)((chip->capacity[lun] - 1) >> 16);
-	buf[2] = (unsigned char)((chip->capacity[lun] - 1) >> 8);
-	buf[3] = (unsigned char)(chip->capacity[lun] - 1);
-
-	buf[4] = 0x00;
-	buf[5] = 0x00;
-	buf[6] = 0x02;
-	buf[7] = 0x00;
-
-	rts51x_set_xfer_buf(buf, scsi_bufflen(srb), srb);
-	kfree(buf);
-
-	scsi_set_resid(srb, 0);
-
-	return TRANSPORT_GOOD;
-}
-
-static int get_dev_status(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	unsigned int lun = SCSI_LUN(srb);
-	unsigned int buf_len;
-	u8 status[32] = { 0 };
-
-	rts51x_pp_status(chip, lun, status, 32);
-
-	buf_len = min_t(unsigned int, scsi_bufflen(srb), sizeof(status));
-	rts51x_set_xfer_buf(status, buf_len, srb);
-	scsi_set_resid(srb, scsi_bufflen(srb) - buf_len);
-
-	return TRANSPORT_GOOD;
-}
-
-static int read_status(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	u8 rts51x_status[16];
-	unsigned int buf_len;
-	unsigned int lun = SCSI_LUN(srb);
-
-	rts51x_read_status(chip, lun, rts51x_status, 16);
-
-	buf_len = min_t(unsigned int, scsi_bufflen(srb), sizeof(rts51x_status));
-	rts51x_set_xfer_buf(rts51x_status, buf_len, srb);
-	scsi_set_resid(srb, scsi_bufflen(srb) - buf_len);
-
-	return TRANSPORT_GOOD;
-}
-
-static int read_mem(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	unsigned int lun = SCSI_LUN(srb);
-	unsigned short addr, len, i;
-	int retval;
-	u8 *buf;
-
-	rts51x_prepare_run(chip);
-	RTS51X_SET_STAT(chip, STAT_RUN);
-
-	addr = ((u16) srb->cmnd[2] << 8) | srb->cmnd[3];
-	len = ((u16) srb->cmnd[4] << 8) | srb->cmnd[5];
-
-	if (addr < 0xe000) {
-		RTS51X_DEBUGP("filter!addr=0x%x\n", addr);
-		return TRANSPORT_GOOD;
-	}
-
-	buf = vmalloc(len);
-	if (!buf)
-		TRACE_RET(chip, TRANSPORT_ERROR);
-
-	for (i = 0; i < len; i++) {
-		retval = rts51x_ep0_read_register(chip, addr + i, buf + i);
-		if (retval != STATUS_SUCCESS) {
-			vfree(buf);
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-	}
-
-	len = (unsigned short)min(scsi_bufflen(srb), (unsigned int)len);
-	rts51x_set_xfer_buf(buf, len, srb);
-	scsi_set_resid(srb, scsi_bufflen(srb) - len);
-
-	vfree(buf);
-
-	return TRANSPORT_GOOD;
-}
-
-static int write_mem(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	unsigned int lun = SCSI_LUN(srb);
-	unsigned short addr, len, i;
-	int retval;
-	u8 *buf;
-
-	rts51x_prepare_run(chip);
-	RTS51X_SET_STAT(chip, STAT_RUN);
-
-	addr = ((u16) srb->cmnd[2] << 8) | srb->cmnd[3];
-	len = ((u16) srb->cmnd[4] << 8) | srb->cmnd[5];
-
-	if (addr < 0xe000) {
-		RTS51X_DEBUGP("filter!addr=0x%x\n", addr);
-		return TRANSPORT_GOOD;
-	}
-
-	len = (unsigned short)min(scsi_bufflen(srb), (unsigned int)len);
-	buf = vmalloc(len);
-	if (!buf)
-		TRACE_RET(chip, TRANSPORT_ERROR);
-
-	rts51x_get_xfer_buf(buf, len, srb);
-
-	for (i = 0; i < len; i++) {
-		retval =
-		    rts51x_ep0_write_register(chip, addr + i, 0xFF, buf[i]);
-		if (retval != STATUS_SUCCESS) {
-			vfree(buf);
-			rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_ERR);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-	}
-
-	vfree(buf);
-	scsi_set_resid(srb, scsi_bufflen(srb) - len);
-
-	return TRANSPORT_GOOD;
-}
-
-static int get_sd_csd(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	unsigned int lun = SCSI_LUN(srb);
-
-	if (!check_card_ready(chip, lun)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (rts51x_get_lun_card(chip, lun) != SD_CARD) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	scsi_set_resid(srb, 0);
-	rts51x_set_xfer_buf(sd_card->raw_csd, scsi_bufflen(srb), srb);
-
-	return TRANSPORT_GOOD;
-}
-
-static int read_phy_register(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	int retval;
-	u8 addr, len, i;
-	u8 *buf;
-
-	rts51x_prepare_run(chip);
-	RTS51X_SET_STAT(chip, STAT_RUN);
-
-	addr = srb->cmnd[5];
-	len = srb->cmnd[7];
-
-	if (len) {
-		buf = vmalloc(len);
-		if (!buf)
-			TRACE_RET(chip, TRANSPORT_ERROR);
-
-		for (i = 0; i < len; i++) {
-			retval =
-			    rts51x_read_phy_register(chip, addr + i, buf + i);
-			if (retval != STATUS_SUCCESS) {
-				vfree(buf);
-				rts51x_set_sense_type(chip, SCSI_LUN(srb),
-					SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-				TRACE_RET(chip, TRANSPORT_FAILED);
-			}
-		}
-
-		len = min(scsi_bufflen(srb), (unsigned int)len);
-		rts51x_set_xfer_buf(buf, len, srb);
-		scsi_set_resid(srb, scsi_bufflen(srb) - len);
-
-		vfree(buf);
-	}
-
-	return TRANSPORT_GOOD;
-}
-
-static int write_phy_register(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	int retval;
-	u8 addr, len, i;
-	u8 *buf;
-
-	rts51x_prepare_run(chip);
-	RTS51X_SET_STAT(chip, STAT_RUN);
-
-	addr = srb->cmnd[5];
-	len = srb->cmnd[7];
-
-	if (len) {
-		len = min(scsi_bufflen(srb), (unsigned int)len);
-
-		buf = vmalloc(len);
-		if (buf == NULL)
-			TRACE_RET(chip, TRANSPORT_ERROR);
-
-		rts51x_get_xfer_buf(buf, len, srb);
-		scsi_set_resid(srb, scsi_bufflen(srb) - len);
-
-		for (i = 0; i < len; i++) {
-			retval =
-			    rts51x_write_phy_register(chip, addr + i, buf[i]);
-			if (retval != STATUS_SUCCESS) {
-				vfree(buf);
-				rts51x_set_sense_type(chip, SCSI_LUN(srb),
-					       SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, TRANSPORT_FAILED);
-			}
-		}
-
-		vfree(buf);
-	}
-
-	return TRANSPORT_GOOD;
-}
-
-static int get_card_bus_width(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	unsigned int lun = SCSI_LUN(srb);
-	u8 card, bus_width;
-
-	if (!check_card_ready(chip, lun)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	card = rts51x_get_lun_card(chip, lun);
-	if ((card == SD_CARD) || (card == MS_CARD)) {
-		bus_width = chip->card_bus_width[lun];
-	} else {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	scsi_set_resid(srb, 0);
-	rts51x_set_xfer_buf(&bus_width, scsi_bufflen(srb), srb);
-
-	return TRANSPORT_GOOD;
-}
-
-#ifdef _MSG_TRACE
-static int trace_msg_cmd(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	unsigned char *buf = NULL;
-	u8 clear;
-	unsigned int buf_len;
-
-	buf_len =
-	    4 +
-	    ((2 + MSG_FUNC_LEN + MSG_FILE_LEN + TIME_VAL_LEN) * TRACE_ITEM_CNT);
-
-	if ((scsi_bufflen(srb) < buf_len) || (scsi_sglist(srb) == NULL)) {
-		rts51x_set_sense_type(chip, SCSI_LUN(srb),
-			       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	clear = srb->cmnd[2];
-
-	buf = vmalloc(scsi_bufflen(srb));
-	if (buf == NULL)
-		TRACE_RET(chip, TRANSPORT_ERROR);
-
-	rts51x_trace_msg(chip, buf, clear);
-
-	rts51x_set_xfer_buf(buf, scsi_bufflen(srb), srb);
-	vfree(buf);
-
-	scsi_set_resid(srb, 0);
-	return TRANSPORT_GOOD;
-}
-#endif
-
-static int rw_mem_cmd_buf(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	int retval = STATUS_SUCCESS;
-	unsigned int lun = SCSI_LUN(srb);
-	u8 cmd_type, mask, value, idx, mode, len;
-	u16 addr;
-	u32 timeout;
-
-	rts51x_prepare_run(chip);
-	RTS51X_SET_STAT(chip, STAT_RUN);
-
-	switch (srb->cmnd[3]) {
-	case INIT_BATCHCMD:
-		rts51x_init_cmd(chip);
-		break;
-
-	case ADD_BATCHCMD:
-		cmd_type = srb->cmnd[4];
-		if (cmd_type > 2) {
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-		addr = (srb->cmnd[5] << 8) | srb->cmnd[6];
-		mask = srb->cmnd[7];
-		value = srb->cmnd[8];
-		rts51x_add_cmd(chip, cmd_type, addr, mask, value);
-		break;
-
-	case SEND_BATCHCMD:
-		mode = srb->cmnd[4];
-		len = srb->cmnd[5];
-		timeout =
-		    ((u32) srb->cmnd[6] << 24) | ((u32) srb->
-						  cmnd[7] << 16) | ((u32) srb->
-								    cmnd[8] <<
-								    8) | ((u32)
-									  srb->
-									  cmnd
-									  [9]);
-		retval = rts51x_send_cmd(chip, mode, 1000);
-		if (retval != STATUS_SUCCESS) {
-			rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_ERR);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-		if (mode & STAGE_R) {
-			retval = rts51x_get_rsp(chip, len, timeout);
-			if (retval != STATUS_SUCCESS) {
-				rts51x_set_sense_type(chip, lun,
-					SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-				TRACE_RET(chip, TRANSPORT_FAILED);
-			}
-		}
-		break;
-
-	case GET_BATCHRSP:
-		idx = srb->cmnd[4];
-		value = chip->rsp_buf[idx];
-		if (scsi_bufflen(srb) < 1) {
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-		rts51x_set_xfer_buf(&value, 1, srb);
-		scsi_set_resid(srb, 0);
-		break;
-
-	default:
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_ERR);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	return TRANSPORT_GOOD;
-}
-
-static int suit_cmd(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	int result;
-
-	switch (srb->cmnd[3]) {
-	case INIT_BATCHCMD:
-	case ADD_BATCHCMD:
-	case SEND_BATCHCMD:
-	case GET_BATCHRSP:
-		result = rw_mem_cmd_buf(srb, chip);
-		break;
-	default:
-		result = TRANSPORT_ERROR;
-	}
-
-	return result;
-}
-
-static int app_cmd(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	int result;
-
-	switch (srb->cmnd[2]) {
-	case PP_READ10:
-	case PP_WRITE10:
-		result = read_write(srb, chip);
-		break;
-
-	case SUIT_CMD:
-		result = suit_cmd(srb, chip);
-		break;
-
-	case READ_PHY:
-		result = read_phy_register(srb, chip);
-		break;
-
-	case WRITE_PHY:
-		result = write_phy_register(srb, chip);
-		break;
-
-	case GET_DEV_STATUS:
-		result = get_dev_status(srb, chip);
-		break;
-
-	default:
-		rts51x_set_sense_type(chip, SCSI_LUN(srb),
-			       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	return result;
-}
-
-static int vendor_cmnd(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	int result = TRANSPORT_GOOD;
-
-	switch (srb->cmnd[1]) {
-	case READ_STATUS:
-		result = read_status(srb, chip);
-		break;
-
-	case READ_MEM:
-		result = read_mem(srb, chip);
-		break;
-
-	case WRITE_MEM:
-		result = write_mem(srb, chip);
-		break;
-
-	case GET_BUS_WIDTH:
-		result = get_card_bus_width(srb, chip);
-		break;
-
-	case GET_SD_CSD:
-		result = get_sd_csd(srb, chip);
-		break;
-
-#ifdef _MSG_TRACE
-	case TRACE_MSG:
-		result = trace_msg_cmd(srb, chip);
-		break;
-#endif
-
-	case SCSI_APP_CMD:
-		result = app_cmd(srb, chip);
-		break;
-
-	default:
-		rts51x_set_sense_type(chip, SCSI_LUN(srb),
-			       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	return result;
-}
-
-static int ms_format_cmnd(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	unsigned int lun = SCSI_LUN(srb);
-	int retval, quick_format;
-
-	if (rts51x_get_lun_card(chip, lun) != MS_CARD) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if ((srb->cmnd[3] != 0x4D) || (srb->cmnd[4] != 0x47)
-	    || (srb->cmnd[5] != 0x66) || (srb->cmnd[6] != 0x6D)
-	    || (srb->cmnd[7] != 0x74)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (srb->cmnd[8] & 0x01)
-		quick_format = 0;
-	else
-		quick_format = 1;
-
-	if (!(chip->card_ready & MS_CARD)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (chip->card_wp & MS_CARD) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_PROTECT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (!CHK_MSPRO(ms_card)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	rts51x_prepare_run(chip);
-	RTS51X_SET_STAT(chip, STAT_RUN);
-
-	retval = rts51x_mspro_format(srb, chip, MS_SHORT_DATA_LEN, quick_format);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_FORMAT_CMD_FAILED);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	scsi_set_resid(srb, 0);
-	return TRANSPORT_GOOD;
-}
-
-#ifdef SUPPORT_PCGL_1P18
-static int get_ms_information(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	unsigned int lun = SCSI_LUN(srb);
-	u8 dev_info_id, data_len;
-	u8 *buf;
-	unsigned int buf_len;
-	int i;
-
-	if (!check_card_ready(chip, lun)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-	if ((rts51x_get_lun_card(chip, lun) != MS_CARD)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if ((srb->cmnd[2] != 0xB0) || (srb->cmnd[4] != 0x4D) ||
-	    (srb->cmnd[5] != 0x53) || (srb->cmnd[6] != 0x49) ||
-	    (srb->cmnd[7] != 0x44)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	dev_info_id = srb->cmnd[3];
-	if ((CHK_MSXC(ms_card) && (dev_info_id == 0x10)) ||
-	    (!CHK_MSXC(ms_card) && (dev_info_id == 0x13)) ||
-	    !CHK_MSPRO(ms_card)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (dev_info_id == 0x15)
-		buf_len = data_len = 0x3A;
-	else
-		buf_len = data_len = 0x6A;
-
-	buf = kmalloc(buf_len, GFP_KERNEL);
-	if (!buf)
-		TRACE_RET(chip, TRANSPORT_ERROR);
-
-	i = 0;
-	/* GET Memory Stick Media Information Response Header */
-	buf[i++] = 0x00;	/* Data length MSB */
-	buf[i++] = data_len;	/* Data length LSB */
-	/* Device Information Type Code */
-	if (CHK_MSXC(ms_card))
-		buf[i++] = 0x03;
-	else
-		buf[i++] = 0x02;
-	/* SGM bit */
-	buf[i++] = 0x01;
-	/* Reserved */
-	buf[i++] = 0x00;
-	buf[i++] = 0x00;
-	buf[i++] = 0x00;
-	/* Number of Device Information */
-	buf[i++] = 0x01;
-
-	/*  Device Information Body
-	 *  Device Information ID Number */
-	buf[i++] = dev_info_id;
-	/* Device Information Length */
-	if (dev_info_id == 0x15)
-		data_len = 0x31;
-	else
-		data_len = 0x61;
-	buf[i++] = 0x00;	/* Data length MSB */
-	buf[i++] = data_len;	/* Data length LSB */
-	/* Valid Bit */
-	buf[i++] = 0x80;
-	if ((dev_info_id == 0x10) || (dev_info_id == 0x13)) {
-		/* System Information */
-		memcpy(buf + i, ms_card->raw_sys_info, 96);
-	} else {
-		/* Model Name */
-		memcpy(buf + i, ms_card->raw_model_name, 48);
-	}
-
-	rts51x_set_xfer_buf(buf, buf_len, srb);
-
-	if (dev_info_id == 0x15)
-		scsi_set_resid(srb, scsi_bufflen(srb) - 0x3C);
-	else
-		scsi_set_resid(srb, scsi_bufflen(srb) - 0x6C);
-
-	kfree(buf);
-	return STATUS_SUCCESS;
-}
-#endif
-
-static int ms_sp_cmnd(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	int retval = TRANSPORT_ERROR;
-
-	if (srb->cmnd[2] == MS_FORMAT)
-		retval = ms_format_cmnd(srb, chip);
-#ifdef SUPPORT_PCGL_1P18
-	else if (srb->cmnd[2] == GET_MS_INFORMATION)
-		retval = get_ms_information(srb, chip);
-#endif
-
-	return retval;
-}
-
-#ifdef SUPPORT_CPRM
-static int sd_extention_cmnd(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	unsigned int lun = SCSI_LUN(srb);
-	int result;
-
-	rts51x_prepare_run(chip);
-	RTS51X_SET_STAT(chip, STAT_RUN);
-
-	rts51x_sd_cleanup_work(chip);
-
-	if (!check_card_ready(chip, lun)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-	if ((rts51x_get_lun_card(chip, lun) != SD_CARD)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	switch (srb->cmnd[0]) {
-	case SD_PASS_THRU_MODE:
-		result = rts51x_sd_pass_thru_mode(srb, chip);
-		break;
-
-	case SD_EXECUTE_NO_DATA:
-		result = rts51x_sd_execute_no_data(srb, chip);
-		break;
-
-	case SD_EXECUTE_READ:
-		result = rts51x_sd_execute_read_data(srb, chip);
-		break;
-
-	case SD_EXECUTE_WRITE:
-		result = rts51x_sd_execute_write_data(srb, chip);
-		break;
-
-	case SD_GET_RSP:
-		result = rts51x_sd_get_cmd_rsp(srb, chip);
-		break;
-
-	case SD_HW_RST:
-		result = rts51x_sd_hw_rst(srb, chip);
-		break;
-
-	default:
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	return result;
-}
-#endif
-
-#ifdef SUPPORT_MAGIC_GATE
-static int mg_report_key(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	unsigned int lun = SCSI_LUN(srb);
-	int retval;
-	u8 key_format;
-
-	rts51x_prepare_run(chip);
-	RTS51X_SET_STAT(chip, STAT_RUN);
-
-	rts51x_ms_cleanup_work(chip);
-
-	if (!check_card_ready(chip, lun)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-	if ((rts51x_get_lun_card(chip, lun) != MS_CARD)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (srb->cmnd[7] != KC_MG_R_PRO) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (!CHK_MSPRO(ms_card)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	key_format = srb->cmnd[10] & 0x3F;
-
-	switch (key_format) {
-	case KF_GET_LOC_EKB:
-		if ((scsi_bufflen(srb) == 0x41C) &&
-		    (srb->cmnd[8] == 0x04) && (srb->cmnd[9] == 0x1C)) {
-			retval = rts51x_mg_get_local_EKB(srb, chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, TRANSPORT_FAILED);
-		} else {
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-		break;
-
-	case KF_RSP_CHG:
-		if ((scsi_bufflen(srb) == 0x24) &&
-		    (srb->cmnd[8] == 0x00) && (srb->cmnd[9] == 0x24)) {
-			retval = rts51x_mg_get_rsp_chg(srb, chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, TRANSPORT_FAILED);
-		} else {
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-		break;
-
-	case KF_GET_ICV:
-		ms_card->mg_entry_num = srb->cmnd[5];
-		if ((scsi_bufflen(srb) == 0x404) &&
-		    (srb->cmnd[8] == 0x04) &&
-		    (srb->cmnd[9] == 0x04) &&
-		    (srb->cmnd[2] == 0x00) &&
-		    (srb->cmnd[3] == 0x00) &&
-		    (srb->cmnd[4] == 0x00) && (srb->cmnd[5] < 32)) {
-			retval = rts51x_mg_get_ICV(srb, chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, TRANSPORT_FAILED);
-		} else {
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-		break;
-
-	default:
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	scsi_set_resid(srb, 0);
-	return TRANSPORT_GOOD;
-}
-
-static int mg_send_key(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	unsigned int lun = SCSI_LUN(srb);
-	int retval;
-	u8 key_format;
-
-	rts51x_prepare_run(chip);
-	RTS51X_SET_STAT(chip, STAT_RUN);
-
-	rts51x_ms_cleanup_work(chip);
-
-	if (!check_card_ready(chip, lun)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-	if (check_card_wp(chip, lun)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_PROTECT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-	if ((rts51x_get_lun_card(chip, lun) != MS_CARD)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_LUN_NOT_SUPPORT);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (srb->cmnd[7] != KC_MG_R_PRO) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (!CHK_MSPRO(ms_card)) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	key_format = srb->cmnd[10] & 0x3F;
-
-	switch (key_format) {
-	case KF_SET_LEAF_ID:
-		if ((scsi_bufflen(srb) == 0x0C) &&
-		    (srb->cmnd[8] == 0x00) && (srb->cmnd[9] == 0x0C)) {
-			retval = rts51x_mg_set_leaf_id(srb, chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, TRANSPORT_FAILED);
-		} else {
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-		break;
-
-	case KF_CHG_HOST:
-		if ((scsi_bufflen(srb) == 0x0C) &&
-		    (srb->cmnd[8] == 0x00) && (srb->cmnd[9] == 0x0C)) {
-			retval = rts51x_mg_chg(srb, chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, TRANSPORT_FAILED);
-		} else {
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-		break;
-
-	case KF_RSP_HOST:
-		if ((scsi_bufflen(srb) == 0x0C) &&
-		    (srb->cmnd[8] == 0x00) && (srb->cmnd[9] == 0x0C)) {
-			retval = rts51x_mg_rsp(srb, chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, TRANSPORT_FAILED);
-		} else {
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-		break;
-
-	case KF_SET_ICV:
-		ms_card->mg_entry_num = srb->cmnd[5];
-		if ((scsi_bufflen(srb) == 0x404) &&
-		    (srb->cmnd[8] == 0x04) &&
-		    (srb->cmnd[9] == 0x04) &&
-		    (srb->cmnd[2] == 0x00) &&
-		    (srb->cmnd[3] == 0x00) &&
-		    (srb->cmnd[4] == 0x00) && (srb->cmnd[5] < 32)) {
-			retval = rts51x_mg_set_ICV(srb, chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, TRANSPORT_FAILED);
-		} else {
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-		break;
-
-	default:
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	scsi_set_resid(srb, 0);
-	return TRANSPORT_GOOD;
-}
-#endif
-
-int rts51x_scsi_handler(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct ms_info *ms_card = &(chip->ms_card);
-	unsigned int lun = SCSI_LUN(srb);
-	int result = TRANSPORT_GOOD;
-
-	if ((rts51x_get_lun_card(chip, lun) == MS_CARD) &&
-	    (ms_card->format_status == FORMAT_IN_PROGRESS)) {
-		if ((srb->cmnd[0] != REQUEST_SENSE)
-		    && (srb->cmnd[0] != INQUIRY)) {
-			/* Logical Unit Not Ready Format in Progress */
-			rts51x_set_sense_data(chip, lun, CUR_ERR, 0x02, 0, 0x04, 0x04,
-				       0, (u16) (ms_card->progress));
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-	}
-
-	switch (srb->cmnd[0]) {
-	case READ_10:
-	case WRITE_10:
-	case READ_6:
-	case WRITE_6:
-		result = read_write(srb, chip);
-		break;
-
-	case TEST_UNIT_READY:
-		result = test_unit_ready(srb, chip);
-		break;
-
-	case INQUIRY:
-		result = inquiry(srb, chip);
-		break;
-
-	case READ_CAPACITY:
-		result = read_capacity(srb, chip);
-		break;
-
-	case START_STOP:
-		result = start_stop_unit(srb, chip);
-		break;
-
-	case ALLOW_MEDIUM_REMOVAL:
-		result = allow_medium_removal(srb, chip);
-		break;
-
-	case REQUEST_SENSE:
-		result = request_sense(srb, chip);
-		break;
-
-	case MODE_SENSE:
-	case MODE_SENSE_10:
-		result = mode_sense(srb, chip);
-		break;
-
-	case 0x23:
-		result = read_format_capacity(srb, chip);
-		break;
-
-	case VENDOR_CMND:
-		result = vendor_cmnd(srb, chip);
-		break;
-
-	case MS_SP_CMND:
-		result = ms_sp_cmnd(srb, chip);
-		break;
-
-#ifdef SUPPORT_CPRM
-	case SD_PASS_THRU_MODE:
-	case SD_EXECUTE_NO_DATA:
-	case SD_EXECUTE_READ:
-	case SD_EXECUTE_WRITE:
-	case SD_GET_RSP:
-	case SD_HW_RST:
-		result = sd_extention_cmnd(srb, chip);
-		break;
-#endif
-
-#ifdef SUPPORT_MAGIC_GATE
-	case CMD_MSPRO_MG_RKEY:
-		result = mg_report_key(srb, chip);
-		break;
-
-	case CMD_MSPRO_MG_SKEY:
-		result = mg_send_key(srb, chip);
-		break;
-#endif
-
-	case FORMAT_UNIT:
-	case MODE_SELECT:
-	case VERIFY:
-		result = TRANSPORT_GOOD;
-		break;
-
-	default:
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		result = TRANSPORT_FAILED;
-	}
-
-	return result;
-}
-
-/***********************************************************************
- * Host functions
- ***********************************************************************/
-
-int slave_alloc(struct scsi_device *sdev)
-{
-	/*
-	 * Set the INQUIRY transfer length to 36.  We don't use any of
-	 * the extra data and many devices choke if asked for more or
-	 * less than 36 bytes.
-	 */
-	sdev->inquiry_len = 36;
-	return 0;
-}
-
-int slave_configure(struct scsi_device *sdev)
-{
-	/* Scatter-gather buffers (all but the last) must have a length
-	 * divisible by the bulk maxpacket size.  Otherwise a data packet
-	 * would end up being short, causing a premature end to the data
-	 * transfer.  Since high-speed bulk pipes have a maxpacket size
-	 * of 512, we'll use that as the scsi device queue's DMA alignment
-	 * mask.  Guaranteeing proper alignment of the first buffer will
-	 * have the desired effect because, except at the beginning and
-	 * the end, scatter-gather buffers follow page boundaries. */
-	blk_queue_dma_alignment(sdev->request_queue, (512 - 1));
-
-	/* Set the SCSI level to at least 2.  We'll leave it at 3 if that's
-	 * what is originally reported.  We need this to avoid confusing
-	 * the SCSI layer with devices that report 0 or 1, but need 10-byte
-	 * commands (ala ATAPI devices behind certain bridges, or devices
-	 * which simply have broken INQUIRY data).
-	 *
-	 * NOTE: This means /dev/sg programs (ala cdrecord) will get the
-	 * actual information.  This seems to be the preference for
-	 * programs like that.
-	 *
-	 * NOTE: This also means that /proc/scsi/scsi and sysfs may report
-	 * the actual value or the modified one, depending on where the
-	 * data comes from.
-	 */
-	if (sdev->scsi_level < SCSI_2)
-		sdev->scsi_level = sdev->sdev_target->scsi_level = SCSI_2;
-
-	return 0;
-}
-
-/***********************************************************************
- * /proc/scsi/ functions
- ***********************************************************************/
-
-/* we use this macro to help us write into the buffer */
-#undef SPRINTF
-#define SPRINTF(args...) seq_printf(m, ##args)
-
-static int write_info(struct Scsi_Host *host, char *buffer, int length)
-{
-	/* if someone is sending us data, just throw it away */
-	return length;
-}
-
-static int show_info(struct seq_file *m, struct Scsi_Host *host)
-{
-	/* print the controller name */
-	SPRINTF("   Host scsi%d: %s\n", host->host_no, RTS51X_NAME);
-
-	/* print product, vendor, and driver version strings */
-	SPRINTF("       Vendor: Realtek Corp.\n");
-	SPRINTF("      Product: RTS51xx USB Card Reader\n");
-	SPRINTF("      Version: %s\n", DRIVER_VERSION);
-	return 0;
-}
-
-/* queue a command */
-/* This is always called with scsi_lock(host) held */
-static int queuecommand_lck(struct scsi_cmnd *srb, void (*done) (struct scsi_cmnd *))
-{
-	struct rts51x_chip *chip = host_to_rts51x(srb->device->host);
-
-	/* check for state-transition errors */
-	if (chip->srb != NULL) {
-		RTS51X_DEBUGP("Error in %s: chip->srb = %p\n",
-			       __func__, chip->srb);
-		return SCSI_MLQUEUE_HOST_BUSY;
-	}
-
-	/* fail the command if we are disconnecting */
-	if (test_bit(FLIDX_DISCONNECTING, &chip->usb->dflags)) {
-		RTS51X_DEBUGP("Fail command during disconnect\n");
-		srb->result = DID_NO_CONNECT << 16;
-		done(srb);
-		return 0;
-	}
-
-	/* enqueue the command and wake up the control thread */
-	srb->scsi_done = done;
-	chip->srb = srb;
-	complete(&chip->usb->cmnd_ready);
-
-	return 0;
-}
-
-DEF_SCSI_QCMD(queuecommand)
-/***********************************************************************
- * Error handling functions
- ***********************************************************************/
-/* Command timeout and abort */
-int command_abort(struct scsi_cmnd *srb)
-{
-	struct rts51x_chip *chip = host_to_rts51x(srb->device->host);
-
-	RTS51X_DEBUGP("%s called\n", __func__);
-
-	/* us->srb together with the TIMED_OUT, RESETTING, and ABORTING
-	 * bits are protected by the host lock. */
-	scsi_lock(rts51x_to_host(chip));
-
-	/* Is this command still active? */
-	if (chip->srb != srb) {
-		scsi_unlock(rts51x_to_host(chip));
-		RTS51X_DEBUGP("-- nothing to abort\n");
-		return FAILED;
-	}
-
-	/* Set the TIMED_OUT bit.  Also set the ABORTING bit, but only if
-	 * a device reset isn't already in progress (to avoid interfering
-	 * with the reset).  Note that we must retain the host lock while
-	 * calling usb_stor_stop_transport(); otherwise it might interfere
-	 * with an auto-reset that begins as soon as we release the lock. */
-	set_bit(FLIDX_TIMED_OUT, &chip->usb->dflags);
-	if (!test_bit(FLIDX_RESETTING, &chip->usb->dflags)) {
-		set_bit(FLIDX_ABORTING, &chip->usb->dflags);
-		/* rts51x_stop_transport(us); */
-	}
-	scsi_unlock(rts51x_to_host(chip));
-
-	/* Wait for the aborted command to finish */
-	wait_for_completion(&chip->usb->notify);
-	return SUCCESS;
-}
-
-/* This invokes the transport reset mechanism to reset the state of the
- * device */
-static int device_reset(struct scsi_cmnd *srb)
-{
-	int result = 0;
-
-	RTS51X_DEBUGP("%s called\n", __func__);
-
-	return result < 0 ? FAILED : SUCCESS;
-}
-
-/* Simulate a SCSI bus reset by resetting the device's USB port. */
-int bus_reset(struct scsi_cmnd *srb)
-{
-	int result = 0;
-
-	RTS51X_DEBUGP("%s called\n", __func__);
-
-	return result < 0 ? FAILED : SUCCESS;
-}
-
-static const char *rts5139_info(struct Scsi_Host *host)
-{
-	return "SCSI emulation for RTS5139 USB card reader";
-}
-
-struct scsi_host_template rts51x_host_template = {
-	/* basic userland interface stuff */
-	.name = RTS51X_NAME,
-	.proc_name = RTS51X_NAME,
-	.show_info = show_info,
-	.write_info = write_info,
-	.info = rts5139_info,
-
-	/* command interface -- queued only */
-	.queuecommand = queuecommand,
-
-	/* error and abort handlers */
-	.eh_abort_handler = command_abort,
-	.eh_device_reset_handler = device_reset,
-	.eh_bus_reset_handler = bus_reset,
-
-	/* queue commands only, only one command per LUN */
-	.can_queue = 1,
-	.cmd_per_lun = 1,
-
-	/* unknown initiator id */
-	.this_id = -1,
-
-	.slave_alloc = slave_alloc,
-	.slave_configure = slave_configure,
-
-	/* lots of sg segments can be handled */
-	.sg_tablesize = SG_ALL,
-
-	/* limit the total size of a transfer to 120 KB */
-	.max_sectors = 240,
-
-	/* merge commands... this seems to help performance, but
-	 * periodically someone should test to see which setting is more
-	 * optimal.
-	 */
-	.use_clustering = 1,
-
-	/* emulated HBA */
-	.emulated = 1,
-
-	/* we do our own delay after a device or bus reset */
-	.skip_settle_delay = 1,
-
-	/* sysfs device attributes */
-	/* .sdev_attrs = sysfs_device_attr_list, */
-
-	/* module management */
-	.module = THIS_MODULE
-};
-
diff --git a/drivers/staging/rts5139/rts51x_scsi.h b/drivers/staging/rts5139/rts51x_scsi.h
deleted file mode 100644
index 1a0d7056618..00000000000
--- a/drivers/staging/rts5139/rts51x_scsi.h
+++ /dev/null
@@ -1,154 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- * Header file
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#ifndef __RTS51X_SCSI_H
-#define __RTS51X_SCSI_H
-
-#include <linux/usb.h>
-#include <linux/usb_usual.h>
-#include <linux/blkdev.h>
-#include <linux/completion.h>
-#include <linux/mutex.h>
-#include <scsi/scsi_host.h>
-
-#include "rts51x_chip.h"
-
-#define MS_SP_CMND		0xFA
-#define MS_FORMAT		0xA0
-#define GET_MS_INFORMATION	0xB0
-
-#define VENDOR_CMND		0xF0
-
-#define READ_STATUS		0x09
-
-#define READ_MEM		0x0D
-#define WRITE_MEM		0x0E
-#define GET_BUS_WIDTH		0x13
-#define GET_SD_CSD		0x14
-#define TOGGLE_GPIO		0x15
-#define TRACE_MSG		0x18
-
-#define SCSI_APP_CMD		0x10
-
-#define PP_READ10		0x1A
-#define PP_WRITE10		0x0A
-#define READ_HOST_REG		0x1D
-#define WRITE_HOST_REG		0x0D
-#define SET_VAR			0x05
-#define GET_VAR			0x15
-#define DMA_READ		0x16
-#define DMA_WRITE		0x06
-#define GET_DEV_STATUS		0x10
-#define SET_CHIP_MODE		0x27
-#define SUIT_CMD		0xE0
-#define WRITE_PHY		0x07
-#define READ_PHY		0x17
-
-#define INIT_BATCHCMD		0x41
-#define ADD_BATCHCMD		0x42
-#define SEND_BATCHCMD		0x43
-#define GET_BATCHRSP		0x44
-
-#ifdef SUPPORT_CPRM
-/* SD Pass Through Command Extension */
-#define SD_PASS_THRU_MODE	0xD0
-#define SD_EXECUTE_NO_DATA	0xD1
-#define SD_EXECUTE_READ		0xD2
-#define SD_EXECUTE_WRITE	0xD3
-#define SD_GET_RSP		0xD4
-#define SD_HW_RST		0xD6
-#endif
-
-#ifdef SUPPORT_MAGIC_GATE
-#define CMD_MSPRO_MG_RKEY	0xA4	/* Report Key Command */
-#define CMD_MSPRO_MG_SKEY	0xA3	/* Send Key Command */
-
-/* CBWCB field: key class */
-#define KC_MG_R_PRO		0xBE	/* MG-R PRO */
-
-/* CBWCB field: key format */
-#define KF_SET_LEAF_ID		0x31	/* Set Leaf ID */
-#define KF_GET_LOC_EKB		0x32	/* Get Local EKB */
-#define KF_CHG_HOST		0x33	/* Challenge (host) */
-#define KF_RSP_CHG		0x34	/* Response and Challenge (device)  */
-#define KF_RSP_HOST		0x35	/* Response (host) */
-#define KF_GET_ICV		0x36	/* Get ICV */
-#define KF_SET_ICV		0x37	/* SSet ICV */
-#endif
-
-struct rts51x_chip;
-
-/*-----------------------------------
-    Start-Stop-Unit
------------------------------------*/
-#define STOP_MEDIUM			0x00	/* access disable */
-#define MAKE_MEDIUM_READY		0x01	/* access enable */
-#define UNLOAD_MEDIUM			0x02	/* unload */
-#define LOAD_MEDIUM			0x03	/* load */
-
-/*-----------------------------------
-    STANDARD_INQUIRY
------------------------------------*/
-#define QULIFIRE                0x00
-#define AENC_FNC                0x00
-#define TRML_IOP                0x00
-#define REL_ADR                 0x00
-#define WBUS_32                 0x00
-#define WBUS_16                 0x00
-#define SYNC                    0x00
-#define LINKED                  0x00
-#define CMD_QUE                 0x00
-#define SFT_RE                  0x00
-
-#define VEN_ID_LEN              8	/* Vendor ID Length         */
-#define PRDCT_ID_LEN            16	/* Product ID Length        */
-#define PRDCT_REV_LEN           4	/* Product LOT Length       */
-
-#define DRCT_ACCESS_DEV         0x00	/* Direct Access Device             */
-#define RMB_DISC                0x80	/* The Device is Removable          */
-#define ANSI_SCSI2              0x02	/* Based on ANSI-SCSI2              */
-
-#define SCSI                    0x00	/* Interface ID                     */
-
-void rts51x_scsi_show_command(struct scsi_cmnd *srb);
-void rts51x_set_sense_type(struct rts51x_chip *chip, unsigned int lun, int sense_type);
-void rts51x_set_sense_data(struct rts51x_chip *chip, unsigned int lun, u8 err_code,
-		    u8 sense_key, u32 info, u8 asc, u8 ascq, u8 sns_key_info0,
-		    u16 sns_key_info1);
-
-int rts51x_scsi_handler(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-
-struct Scsi_Host;
-struct scsi_device;
-struct scsi_cmnd;
-
-int slave_alloc(struct scsi_device *sdev);
-int slave_configure(struct scsi_device *sdev);
-int queuecommand(struct Scsi_Host *, struct scsi_cmnd *);
-int command_abort(struct scsi_cmnd *srb);
-int bus_reset(struct scsi_cmnd *srb);
-
-#endif /* __RTS51X_SCSI_H */
diff --git a/drivers/staging/rts5139/rts51x_transport.c b/drivers/staging/rts5139/rts51x_transport.c
deleted file mode 100644
index 74588d249b3..00000000000
--- a/drivers/staging/rts5139/rts51x_transport.c
+++ /dev/null
@@ -1,736 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#include <linux/blkdev.h>
-#include <linux/kthread.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
-
-#include <scsi/scsi.h>
-#include <scsi/scsi_eh.h>
-#include <scsi/scsi_device.h>
-
-#include "debug.h"
-#include "rts51x.h"
-#include "rts51x_chip.h"
-#include "rts51x_card.h"
-#include "rts51x_scsi.h"
-#include "rts51x_transport.h"
-#include "trace.h"
-
-/***********************************************************************
- * Scatter-gather transfer buffer access routines
- ***********************************************************************/
-
-/* Copy a buffer of length buflen to/from the srb's transfer buffer.
- * Update the **sgptr and *offset variables so that the next copy will
- * pick up from where this one left off.
- */
-
-unsigned int rts51x_access_sglist(unsigned char *buffer,
-				  unsigned int buflen, void *sglist,
-				  void **sgptr, unsigned int *offset,
-				  enum xfer_buf_dir dir)
-{
-	unsigned int cnt;
-	struct scatterlist *sg = (struct scatterlist *)*sgptr;
-
-	/* We have to go through the list one entry
-	 * at a time.  Each s-g entry contains some number of pages, and
-	 * each page has to be kmap()'ed separately.  If the page is already
-	 * in kernel-addressable memory then kmap() will return its address.
-	 * If the page is not directly accessible -- such as a user buffer
-	 * located in high memory -- then kmap() will map it to a temporary
-	 * position in the kernel's virtual address space.
-	 */
-
-	if (!sg)
-		sg = (struct scatterlist *)sglist;
-
-	/* This loop handles a single s-g list entry, which may
-	 * include multiple pages.  Find the initial page structure
-	 * and the starting offset within the page, and update
-	 * the *offset and **sgptr values for the next loop.
-	 */
-	cnt = 0;
-	while (cnt < buflen && sg) {
-		struct page *page = sg_page(sg) +
-		    ((sg->offset + *offset) >> PAGE_SHIFT);
-		unsigned int poff = (sg->offset + *offset) & (PAGE_SIZE - 1);
-		unsigned int sglen = sg->length - *offset;
-
-		if (sglen > buflen - cnt) {
-
-			/* Transfer ends within this s-g entry */
-			sglen = buflen - cnt;
-			*offset += sglen;
-		} else {
-
-			/* Transfer continues to next s-g entry */
-			*offset = 0;
-			sg = sg_next(sg);
-		}
-
-		/* Transfer the data for all the pages in this
-		 * s-g entry.  For each page: call kmap(), do the
-		 * transfer, and call kunmap() immediately after. */
-		while (sglen > 0) {
-			unsigned int plen = min(sglen, (unsigned int)
-						PAGE_SIZE - poff);
-			unsigned char *ptr = kmap(page);
-
-			if (dir == TO_XFER_BUF)
-				memcpy(ptr + poff, buffer + cnt, plen);
-			else
-				memcpy(buffer + cnt, ptr + poff, plen);
-			kunmap(page);
-
-			/* Start at the beginning of the next page */
-			poff = 0;
-			++page;
-			cnt += plen;
-			sglen -= plen;
-		}
-	}
-	*sgptr = sg;
-
-	/* Return the amount actually transferred */
-	return cnt;
-}
-
-static unsigned int rts51x_access_xfer_buf(unsigned char *buffer,
-				    unsigned int buflen, struct scsi_cmnd *srb,
-				    struct scatterlist **sgptr,
-				    unsigned int *offset, enum xfer_buf_dir dir)
-{
-	return rts51x_access_sglist(buffer, buflen, (void *)scsi_sglist(srb),
-				    (void **)sgptr, offset, dir);
-}
-
-/* Store the contents of buffer into srb's transfer buffer and set the
- * SCSI residue.
- */
-void rts51x_set_xfer_buf(unsigned char *buffer,
-			 unsigned int buflen, struct scsi_cmnd *srb)
-{
-	unsigned int offset = 0;
-	struct scatterlist *sg = NULL;
-
-	buflen = min(buflen, scsi_bufflen(srb));
-	buflen = rts51x_access_xfer_buf(buffer, buflen, srb, &sg, &offset,
-					TO_XFER_BUF);
-	if (buflen < scsi_bufflen(srb))
-		scsi_set_resid(srb, scsi_bufflen(srb) - buflen);
-}
-
-void rts51x_get_xfer_buf(unsigned char *buffer,
-			 unsigned int buflen, struct scsi_cmnd *srb)
-{
-	unsigned int offset = 0;
-	struct scatterlist *sg = NULL;
-
-	buflen = min(buflen, scsi_bufflen(srb));
-	buflen = rts51x_access_xfer_buf(buffer, buflen, srb, &sg, &offset,
-					FROM_XFER_BUF);
-	if (buflen < scsi_bufflen(srb))
-		scsi_set_resid(srb, scsi_bufflen(srb) - buflen);
-}
-
-/* This is the completion handler which will wake us up when an URB
- * completes.
- */
-static void urb_done_completion(struct urb *urb)
-{
-	struct completion *urb_done_ptr = urb->context;
-
-	if (urb_done_ptr)
-		complete(urb_done_ptr);
-}
-
-/* This is the common part of the URB message submission code
- *
- * All URBs from the driver involved in handling a queued scsi
- * command _must_ pass through this function (or something like it) for the
- * abort mechanisms to work properly.
- */
-static int rts51x_msg_common(struct rts51x_chip *chip, struct urb *urb,
-			     int timeout)
-{
-	struct rts51x_usb *rts51x = chip->usb;
-	struct completion urb_done;
-	long timeleft;
-	int status;
-
-	/* don't submit URBs during abort processing */
-	if (test_bit(FLIDX_ABORTING, &rts51x->dflags))
-		TRACE_RET(chip, -EIO);
-
-	/* set up data structures for the wakeup system */
-	init_completion(&urb_done);
-
-	/* fill the common fields in the URB */
-	urb->context = &urb_done;
-	urb->actual_length = 0;
-	urb->error_count = 0;
-	urb->status = 0;
-
-	/* we assume that if transfer_buffer isn't us->iobuf then it
-	 * hasn't been mapped for DMA.  Yes, this is clunky, but it's
-	 * easier than always having the caller tell us whether the
-	 * transfer buffer has already been mapped. */
-	urb->transfer_flags = URB_NO_SETUP_DMA_MAP;
-	if (urb->transfer_buffer == rts51x->iobuf) {
-		urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
-		urb->transfer_dma = rts51x->iobuf_dma;
-	}
-	urb->setup_dma = rts51x->cr_dma;
-
-	/* submit the URB */
-	status = usb_submit_urb(urb, GFP_NOIO);
-	if (status) {
-		/* something went wrong */
-		TRACE_RET(chip, status);
-	}
-
-	/* since the URB has been submitted successfully, it's now okay
-	 * to cancel it */
-	set_bit(FLIDX_URB_ACTIVE, &rts51x->dflags);
-
-	/* did an abort occur during the submission? */
-	if (test_bit(FLIDX_ABORTING, &rts51x->dflags)) {
-
-		/* cancel the URB, if it hasn't been cancelled already */
-		if (test_and_clear_bit(FLIDX_URB_ACTIVE, &rts51x->dflags)) {
-			RTS51X_DEBUGP("-- cancelling URB\n");
-			usb_unlink_urb(urb);
-		}
-	}
-
-	/* wait for the completion of the URB */
-	timeleft =
-	    wait_for_completion_interruptible_timeout(&urb_done,
-						      (timeout * HZ /
-						       1000) ? :
-						      MAX_SCHEDULE_TIMEOUT);
-
-	clear_bit(FLIDX_URB_ACTIVE, &rts51x->dflags);
-
-	if (timeleft <= 0) {
-		RTS51X_DEBUGP("%s -- cancelling URB\n",
-			       timeleft == 0 ? "Timeout" : "Signal");
-		usb_kill_urb(urb);
-		if (timeleft == 0)
-			status = -ETIMEDOUT;
-		else
-			status = -EINTR;
-	} else {
-		status = urb->status;
-	}
-
-	return status;
-}
-
-static int rts51x_clear_halt(struct rts51x_chip *chip, unsigned int pipe);
-
-/*
- * Interpret the results of a URB transfer
- */
-static int interpret_urb_result(struct rts51x_chip *chip, unsigned int pipe,
-				unsigned int length, int result,
-				unsigned int partial)
-{
-	int retval = STATUS_SUCCESS;
-
-	/* RTS51X_DEBUGP("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) {
-			RTS51X_DEBUGP("-- short transfer\n");
-			TRACE_RET(chip, STATUS_TRANS_SHORT);
-		}
-		/* RTS51X_DEBUGP("-- transfer complete\n"); */
-		return STATUS_SUCCESS;
-		/* stalled */
-	case -EPIPE:
-		/* for control endpoints, (used by CB[I]) a stall indicates
-		 * a failed command */
-		if (usb_pipecontrol(pipe)) {
-			RTS51X_DEBUGP("-- stall on control pipe\n");
-			TRACE_RET(chip, STATUS_STALLED);
-		}
-		/* for other sorts of endpoint, clear the stall */
-		RTS51X_DEBUGP("clearing endpoint halt for pipe 0x%x\n", pipe);
-		if (rts51x_clear_halt(chip, pipe) < 0)
-			TRACE_RET(chip, STATUS_ERROR);
-		retval = STATUS_STALLED;
-		TRACE_GOTO(chip, Exit);
-
-		/* babble - the device tried to send more than
-		 * we wanted to read */
-	case -EOVERFLOW:
-		RTS51X_DEBUGP("-- babble\n");
-		retval = STATUS_TRANS_LONG;
-		TRACE_GOTO(chip, Exit);
-
-		/* the transfer was cancelled by abort,
-		 * disconnect, or timeout */
-	case -ECONNRESET:
-		RTS51X_DEBUGP("-- transfer cancelled\n");
-		retval = STATUS_ERROR;
-		TRACE_GOTO(chip, Exit);
-
-		/* short scatter-gather read transfer */
-	case -EREMOTEIO:
-		RTS51X_DEBUGP("-- short read transfer\n");
-		retval = STATUS_TRANS_SHORT;
-		TRACE_GOTO(chip, Exit);
-
-		/* abort or disconnect in progress */
-	case -EIO:
-		RTS51X_DEBUGP("-- abort or disconnect in progress\n");
-		retval = STATUS_ERROR;
-		TRACE_GOTO(chip, Exit);
-
-	case -ETIMEDOUT:
-		RTS51X_DEBUGP("-- time out\n");
-		retval = STATUS_TIMEDOUT;
-		TRACE_GOTO(chip, Exit);
-
-		/* the catch-all error case */
-	default:
-		RTS51X_DEBUGP("-- unknown error\n");
-		retval = STATUS_ERROR;
-		TRACE_GOTO(chip, Exit);
-	}
-
-Exit:
-	if ((retval != STATUS_SUCCESS) && !usb_pipecontrol(pipe))
-		rts51x_clear_hw_error(chip);
-
-	return retval;
-}
-
-int rts51x_ctrl_transfer(struct rts51x_chip *chip, unsigned int pipe,
-			 u8 request, u8 requesttype, u16 value, u16 index,
-			 void *data, u16 size, int timeout)
-{
-	struct rts51x_usb *rts51x = chip->usb;
-	int result;
-
-	RTS51X_DEBUGP("%s: rq=%02x rqtype=%02x value=%04x index=%02x len=%u\n",
-		       __func__, request, requesttype, value, index, size);
-
-	/* fill in the devrequest structure */
-	rts51x->cr->bRequestType = requesttype;
-	rts51x->cr->bRequest = request;
-	rts51x->cr->wValue = cpu_to_le16(value);
-	rts51x->cr->wIndex = cpu_to_le16(index);
-	rts51x->cr->wLength = cpu_to_le16(size);
-
-	/* fill and submit the URB */
-	usb_fill_control_urb(rts51x->current_urb, rts51x->pusb_dev, pipe,
-			     (unsigned char *)rts51x->cr, data, size,
-			     urb_done_completion, NULL);
-	result = rts51x_msg_common(chip, rts51x->current_urb, timeout);
-
-	return interpret_urb_result(chip, pipe, size, result,
-				    rts51x->current_urb->actual_length);
-}
-
-static int rts51x_clear_halt(struct rts51x_chip *chip, unsigned int pipe)
-{
-	int result;
-	int endp = usb_pipeendpoint(pipe);
-
-	if (usb_pipein(pipe))
-		endp |= USB_DIR_IN;
-
-	result = rts51x_ctrl_transfer(chip, SND_CTRL_PIPE(chip),
-				      USB_REQ_CLEAR_FEATURE, USB_RECIP_ENDPOINT,
-				      USB_ENDPOINT_HALT, endp, NULL, 0, 3000);
-	if (result != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	usb_reset_endpoint(chip->usb->pusb_dev, endp);
-
-	return STATUS_SUCCESS;
-}
-
-static void rts51x_sg_clean(struct usb_sg_request *io)
-{
-	if (io->urbs) {
-		while (io->entries--)
-			usb_free_urb(io->urbs[io->entries]);
-		kfree(io->urbs);
-		io->urbs = NULL;
-	}
-	io->dev = NULL;
-}
-
-static int rts51x_sg_init(struct usb_sg_request *io, struct usb_device *dev,
-		   unsigned pipe, unsigned period, struct scatterlist *sg,
-		   int nents, size_t length, gfp_t mem_flags)
-{
-	return usb_sg_init(io, dev, pipe, period, sg, nents, length, mem_flags);
-}
-
-static int rts51x_sg_wait(struct usb_sg_request *io, int timeout)
-{
-	long timeleft;
-	int i;
-	int entries = io->entries;
-
-	/* queue the urbs.  */
-	spin_lock_irq(&io->lock);
-	i = 0;
-	while (i < entries && !io->status) {
-		int retval;
-
-		io->urbs[i]->dev = io->dev;
-		retval = usb_submit_urb(io->urbs[i], GFP_ATOMIC);
-
-		/* after we submit, let completions or cancelations fire;
-		 * we handshake using io->status.
-		 */
-		spin_unlock_irq(&io->lock);
-		switch (retval) {
-			/* maybe the retry will recover */
-		case -ENXIO:	/* hc didn't queue this one */
-		case -EAGAIN:
-		case -ENOMEM:
-			io->urbs[i]->dev = NULL;
-			retval = 0;
-			yield();
-			break;
-
-			/* no error? continue immediately.
-			 *
-			 * NOTE: to work better with UHCI (4K I/O buffer may
-			 * need 3K of TDs) it may be good to limit how many
-			 * URBs are queued at once; N milliseconds?
-			 */
-		case 0:
-			++i;
-			cpu_relax();
-			break;
-
-			/* fail any uncompleted urbs */
-		default:
-			io->urbs[i]->dev = NULL;
-			io->urbs[i]->status = retval;
-			dev_dbg(&io->dev->dev, "%s, submit --> %d\n",
-				__func__, retval);
-			usb_sg_cancel(io);
-		}
-		spin_lock_irq(&io->lock);
-		if (retval && (io->status == 0 || io->status == -ECONNRESET))
-			io->status = retval;
-	}
-	io->count -= entries - i;
-	if (io->count == 0)
-		complete(&io->complete);
-	spin_unlock_irq(&io->lock);
-
-	timeleft =
-	    wait_for_completion_interruptible_timeout(&io->complete,
-						      (timeout * HZ /
-						       1000) ? :
-						      MAX_SCHEDULE_TIMEOUT);
-	if (timeleft <= 0) {
-		RTS51X_DEBUGP("%s -- cancelling SG request\n",
-			       timeleft == 0 ? "Timeout" : "Signal");
-		usb_sg_cancel(io);
-		if (timeleft == 0)
-			io->status = -ETIMEDOUT;
-		else
-			io->status = -EINTR;
-	}
-
-	rts51x_sg_clean(io);
-	return io->status;
-}
-
-/*
- * Transfer a scatter-gather list via bulk transfer
- *
- * This function does basically the same thing as usb_stor_bulk_transfer_buf()
- * above, but it uses the usbcore scatter-gather library.
- */
-static int rts51x_bulk_transfer_sglist(struct rts51x_chip *chip,
-				       unsigned int pipe,
-				       struct scatterlist *sg, int num_sg,
-				       unsigned int length,
-				       unsigned int *act_len, int timeout)
-{
-	int result;
-
-	/* don't submit s-g requests during abort processing */
-	if (test_bit(FLIDX_ABORTING, &chip->usb->dflags))
-		TRACE_RET(chip, STATUS_ERROR);
-
-	/* initialize the scatter-gather request block */
-	RTS51X_DEBUGP("%s: xfer %u bytes, %d entries\n", __func__,
-		       length, num_sg);
-	result =
-	    rts51x_sg_init(&chip->usb->current_sg, chip->usb->pusb_dev, pipe, 0,
-			   sg, num_sg, length, GFP_NOIO);
-	if (result) {
-		RTS51X_DEBUGP("rts51x_sg_init returned %d\n", result);
-		TRACE_RET(chip, STATUS_ERROR);
-	}
-
-	/* since the block has been initialized successfully, it's now
-	 * okay to cancel it */
-	set_bit(FLIDX_SG_ACTIVE, &chip->usb->dflags);
-
-	/* did an abort occur during the submission? */
-	if (test_bit(FLIDX_ABORTING, &chip->usb->dflags)) {
-
-		/* cancel the request, if it hasn't been cancelled already */
-		if (test_and_clear_bit(FLIDX_SG_ACTIVE, &chip->usb->dflags)) {
-			RTS51X_DEBUGP("-- cancelling sg request\n");
-			usb_sg_cancel(&chip->usb->current_sg);
-		}
-	}
-
-	/* wait for the completion of the transfer */
-	result = rts51x_sg_wait(&chip->usb->current_sg, timeout);
-
-	clear_bit(FLIDX_SG_ACTIVE, &chip->usb->dflags);
-
-	/* result = us->current_sg.status; */
-	if (act_len)
-		*act_len = chip->usb->current_sg.bytes;
-	return interpret_urb_result(chip, pipe, length, result,
-				    chip->usb->current_sg.bytes);
-}
-
-static int rts51x_bulk_transfer_buf(struct rts51x_chip *chip,
-			     unsigned int pipe,
-			     void *buf, unsigned int length,
-			     unsigned int *act_len, int timeout)
-{
-	int result;
-
-	/* fill and submit the URB */
-	usb_fill_bulk_urb(chip->usb->current_urb, chip->usb->pusb_dev, pipe,
-			  buf, length, urb_done_completion, NULL);
-	result = rts51x_msg_common(chip, chip->usb->current_urb, timeout);
-
-	/* store the actual length of the data transferred */
-	if (act_len)
-		*act_len = chip->usb->current_urb->actual_length;
-	return interpret_urb_result(chip, pipe, length, result,
-				    chip->usb->current_urb->actual_length);
-}
-
-int rts51x_transfer_data(struct rts51x_chip *chip, unsigned int pipe,
-			 void *buf, unsigned int len, int use_sg,
-			 unsigned int *act_len, int timeout)
-{
-	int result;
-
-	if (timeout < 600)
-		timeout = 600;
-
-	if (use_sg) {
-		result =
-		    rts51x_bulk_transfer_sglist(chip, pipe,
-						(struct scatterlist *)buf,
-						use_sg, len, act_len, timeout);
-	} else {
-		result =
-		    rts51x_bulk_transfer_buf(chip, pipe, buf, len, act_len,
-					     timeout);
-	}
-
-	return result;
-}
-
-int rts51x_transfer_data_partial(struct rts51x_chip *chip, unsigned int pipe,
-				 void *buf, void **ptr, unsigned int *offset,
-				 unsigned int len, int use_sg,
-				 unsigned int *act_len, int timeout)
-{
-	int result;
-
-	if (timeout < 600)
-		timeout = 600;
-
-	if (use_sg) {
-		void *tmp_buf = kmalloc(len, GFP_KERNEL);
-		if (!tmp_buf)
-			TRACE_RET(chip, STATUS_NOMEM);
-
-		if (usb_pipeout(pipe)) {
-			rts51x_access_sglist(tmp_buf, len, buf, ptr, offset,
-					     FROM_XFER_BUF);
-		}
-		result =
-		    rts51x_bulk_transfer_buf(chip, pipe, tmp_buf, len, act_len,
-					     timeout);
-		if (result == STATUS_SUCCESS) {
-			if (usb_pipein(pipe)) {
-				rts51x_access_sglist(tmp_buf, len, buf, ptr,
-						     offset, TO_XFER_BUF);
-			}
-		}
-
-		kfree(tmp_buf);
-	} else {
-		unsigned int step = 0;
-		if (offset)
-			step = *offset;
-		result =
-		    rts51x_bulk_transfer_buf(chip, pipe, buf + step, len,
-					     act_len, timeout);
-		if (act_len)
-			step += *act_len;
-		else
-			step += len;
-		if (offset)
-			*offset = step;
-	}
-
-	return result;
-}
-
-int rts51x_get_epc_status(struct rts51x_chip *chip, u16 *status)
-{
-	unsigned int pipe = RCV_INTR_PIPE(chip);
-	struct usb_host_endpoint *ep;
-	struct completion urb_done;
-	int result;
-
-	if (!status)
-		TRACE_RET(chip, STATUS_ERROR);
-
-	/* set up data structures for the wakeup system */
-	init_completion(&urb_done);
-
-	ep = chip->usb->pusb_dev->ep_in[usb_pipeendpoint(pipe)];
-
-	/* fill and submit the URB */
-	/* Set interval to 10 here to match the endpoint descriptor,
-	 * the polling interval is controlled by the polling thread */
-	usb_fill_int_urb(chip->usb->intr_urb, chip->usb->pusb_dev, pipe,
-			 status, 2, urb_done_completion, &urb_done, 10);
-
-	result = rts51x_msg_common(chip, chip->usb->intr_urb, 100);
-
-	return interpret_urb_result(chip, pipe, 2, result,
-				    chip->usb->intr_urb->actual_length);
-}
-
-static u8 media_not_present[] = {
-	0x70, 0, 0x02, 0, 0, 0, 0, 10, 0, 0, 0, 0, 0x3A, 0, 0, 0, 0, 0 };
-static u8 invalid_cmd_field[] = {
-	0x70, 0, 0x05, 0, 0, 0, 0, 10, 0, 0, 0, 0, 0x24, 0, 0, 0, 0, 0 };
-
-void rts51x_invoke_transport(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	int result;
-
-#ifdef CONFIG_PM
-	if (chip->option.ss_en) {
-		if (srb->cmnd[0] == TEST_UNIT_READY) {
-			if (RTS51X_CHK_STAT(chip, STAT_SS)) {
-				if (check_fake_card_ready(chip,
-							SCSI_LUN(srb))) {
-					srb->result = SAM_STAT_GOOD;
-				} else {
-					srb->result = SAM_STAT_CHECK_CONDITION;
-					memcpy(srb->sense_buffer,
-					       media_not_present, SENSE_SIZE);
-				}
-				return;
-			}
-		} else if (srb->cmnd[0] == ALLOW_MEDIUM_REMOVAL) {
-			if (RTS51X_CHK_STAT(chip, STAT_SS)) {
-				int prevent = srb->cmnd[4] & 0x1;
-
-				if (prevent) {
-					srb->result = SAM_STAT_CHECK_CONDITION;
-					memcpy(srb->sense_buffer,
-					       invalid_cmd_field, SENSE_SIZE);
-				} else {
-					srb->result = SAM_STAT_GOOD;
-				}
-				return;
-			}
-		} else {
-			if (RTS51X_CHK_STAT(chip, STAT_SS)
-			    || RTS51X_CHK_STAT(chip, STAT_SS_PRE)) {
-				/* Wake up device */
-				RTS51X_DEBUGP("Try to wake up device\n");
-				chip->resume_from_scsi = 1;
-
-				rts51x_try_to_exit_ss(chip);
-
-				if (RTS51X_CHK_STAT(chip, STAT_SS)) {
-					wait_timeout(3000);
-
-					rts51x_init_chip(chip);
-					rts51x_init_cards(chip);
-				}
-			}
-		}
-	}
-#endif
-
-	result = rts51x_scsi_handler(srb, chip);
-
-	/* if there is a transport error, reset and don't auto-sense */
-	if (result == TRANSPORT_ERROR) {
-		RTS51X_DEBUGP("-- transport indicates error, resetting\n");
-		srb->result = DID_ERROR << 16;
-		goto Handle_Errors;
-	}
-
-	srb->result = SAM_STAT_GOOD;
-
-	/*
-	 * If we have a failure, we're going to do a REQUEST_SENSE
-	 * automatically.  Note that we differentiate between a command
-	 * "failure" and an "error" in the transport mechanism.
-	 */
-	if (result == TRANSPORT_FAILED) {
-		/* set the result so the higher layers expect this data */
-		srb->result = SAM_STAT_CHECK_CONDITION;
-		memcpy(srb->sense_buffer,
-		       (unsigned char *)&(chip->sense_buffer[SCSI_LUN(srb)]),
-		       sizeof(struct sense_data_t));
-	}
-
-	return;
-
-	/* Error and abort processing: try to resynchronize with the device
-	 * by issuing a port reset.  If that fails, try a class-specific
-	 * device reset. */
-Handle_Errors:
-	return;
-}
diff --git a/drivers/staging/rts5139/rts51x_transport.h b/drivers/staging/rts5139/rts51x_transport.h
deleted file mode 100644
index 024f115540a..00000000000
--- a/drivers/staging/rts5139/rts51x_transport.h
+++ /dev/null
@@ -1,67 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- * Header file
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#ifndef __RTS51X_TRANSPORT_H
-#define __RTS51X_TRANSPORT_H
-
-#include <linux/kernel.h>
-
-#include "rts51x.h"
-#include "rts51x_chip.h"
-
-#if 1 /* LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 34) */
-#define URB_NO_SETUP_DMA_MAP		0
-#endif
-
-unsigned int rts51x_access_sglist(unsigned char *buffer,
-				  unsigned int buflen, void *sglist,
-				  void **sgptr, unsigned int *offset,
-				  enum xfer_buf_dir dir);
-void rts51x_set_xfer_buf(unsigned char *buffer, unsigned int buflen,
-			 struct scsi_cmnd *srb);
-void rts51x_get_xfer_buf(unsigned char *buffer, unsigned int buflen,
-			 struct scsi_cmnd *srb);
-
-int rts51x_ctrl_transfer(struct rts51x_chip *chip, unsigned int pipe,
-			 u8 request, u8 requesttype, u16 value, u16 index,
-			 void *data, u16 size, int timeout);
-int rts51x_transfer_data(struct rts51x_chip *chip, unsigned int pipe,
-			 void *buf, unsigned int len, int use_sg,
-			 unsigned int *act_len, int timeout);
-int rts51x_transfer_data_partial(struct rts51x_chip *chip, unsigned int pipe,
-				 void *buf, void **ptr, unsigned int *offset,
-				 unsigned int len, int use_sg,
-				 unsigned int *act_len, int timeout);
-
-#ifndef POLLING_IN_THREAD
-int rts51x_start_epc_transfer(struct rts51x_chip *chip);
-void rts51x_cancel_epc_transfer(struct rts51x_chip *chip);
-#endif
-
-int rts51x_get_epc_status(struct rts51x_chip *chip, u16 *status);
-void rts51x_invoke_transport(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-
-#endif /* __RTS51X_TRANSPORT_H */
diff --git a/drivers/staging/rts5139/sd.c b/drivers/staging/rts5139/sd.c
deleted file mode 100644
index da5a9b8fb69..00000000000
--- a/drivers/staging/rts5139/sd.c
+++ /dev/null
@@ -1,3274 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#include <linux/blkdev.h>
-#include <linux/kthread.h>
-#include <linux/sched.h>
-
-#include "debug.h"
-#include "trace.h"
-#include "rts51x.h"
-#include "rts51x_transport.h"
-#include "rts51x_scsi.h"
-#include "rts51x_card.h"
-#include "sd.h"
-
-static inline void sd_set_reset_fail(struct rts51x_chip *chip, u8 err_code)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-
-	sd_card->sd_reset_fail |= err_code;
-}
-
-static inline void sd_clear_reset_fail(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-
-	sd_card->sd_reset_fail = 0;
-}
-
-static inline int sd_check_reset_fail(struct rts51x_chip *chip, u8 err_code)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-
-	return sd_card->sd_reset_fail & err_code;
-}
-
-static inline void sd_set_err_code(struct rts51x_chip *chip, u8 err_code)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-
-	sd_card->err_code |= err_code;
-}
-
-static inline void sd_clr_err_code(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-
-	sd_card->err_code = 0;
-}
-
-static inline int sd_check_err_code(struct rts51x_chip *chip, u8 err_code)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-
-	return sd_card->err_code & err_code;
-}
-
-static int sd_parse_err_code(struct rts51x_chip *chip)
-{
-	TRACE_RET(chip, STATUS_FAIL);
-}
-
-int sd_check_data0_status(struct rts51x_chip *chip)
-{
-	int retval;
-	u8 stat;
-
-	retval = rts51x_ep0_read_register(chip, SD_BUS_STAT, &stat);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	if (!(stat & SD_DAT0_STATUS)) {
-		sd_set_err_code(chip, SD_BUSY);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_send_cmd_get_rsp(struct rts51x_chip *chip, u8 cmd_idx,
-			       u32 arg, u8 rsp_type, u8 *rsp, int rsp_len)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	int timeout = 50;
-	u16 reg_addr;
-	u8 buf[17], stat;
-	int len = 2;
-	int rty_cnt = 0;
-
-	sd_clr_err_code(chip);
-
-	RTS51X_DEBUGP("SD/MMC CMD %d, arg = 0x%08x\n", cmd_idx, arg);
-
-	if (rsp_type == SD_RSP_TYPE_R1b)
-		timeout = 3000;
-
-RTY_SEND_CMD:
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD0, 0xFF, 0x40 | cmd_idx);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD1, 0xFF, (u8) (arg >> 24));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD2, 0xFF, (u8) (arg >> 16));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD3, 0xFF, (u8) (arg >> 8));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD4, 0xFF, (u8) arg);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF, rsp_type);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-		       PINGPONG_BUFFER);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
-		       SD_TM_CMD_RSP | SD_TRANSFER_START);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER,
-		       SD_TRANSFER_END | SD_STAT_IDLE,
-		       SD_TRANSFER_END | SD_STAT_IDLE);
-
-	rts51x_add_cmd(chip, READ_REG_CMD, SD_STAT1, 0, 0);
-
-	if (CHECK_USB(chip, USB_20)) {
-		if (rsp_type == SD_RSP_TYPE_R2) {
-			/* Read data from ping-pong buffer */
-			for (reg_addr = PPBUF_BASE2;
-			     reg_addr < PPBUF_BASE2 + 16; reg_addr++) {
-				rts51x_add_cmd(chip, READ_REG_CMD, reg_addr, 0,
-					       0);
-			}
-			len = 18;
-		} else if (rsp_type != SD_RSP_TYPE_R0) {
-			/* Read data from SD_CMDx registers */
-			for (reg_addr = SD_CMD0; reg_addr <= SD_CMD4;
-			     reg_addr++) {
-				rts51x_add_cmd(chip, READ_REG_CMD, reg_addr, 0,
-					       0);
-			}
-			len = 7;
-		} else {
-			len = 2;
-		}
-	} else {
-		len = 2;
-	}
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, len, timeout);
-
-	if (CHECK_SD_TRANS_FAIL(chip, retval)) {
-		u8 val;
-
-		rts51x_ep0_read_register(chip, SD_STAT1, &val);
-		RTS51X_DEBUGP("SD_STAT1: 0x%x\n", val);
-
-		rts51x_ep0_read_register(chip, SD_STAT2, &val);
-		RTS51X_DEBUGP("SD_STAT2: 0x%x\n", val);
-
-		if (val & SD_RSP_80CLK_TIMEOUT)
-			sd_set_err_code(chip, SD_RSP_TIMEOUT);
-
-		rts51x_ep0_read_register(chip, SD_BUS_STAT, &val);
-		RTS51X_DEBUGP("SD_BUS_STAT: 0x%x\n", val);
-
-		if (retval == STATUS_TIMEDOUT) {
-			if (rsp_type & SD_WAIT_BUSY_END) {
-				retval = sd_check_data0_status(chip);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, retval);
-			} else {
-				sd_set_err_code(chip, SD_TO_ERR);
-			}
-		}
-		rts51x_clear_sd_error(chip);
-
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if (rsp_type == SD_RSP_TYPE_R0)
-		return STATUS_SUCCESS;
-
-	if (CHECK_USB(chip, USB_20)) {
-		rts51x_read_rsp_buf(chip, 2, buf, len - 2);
-	} else {
-		if (rsp_type == SD_RSP_TYPE_R2) {
-			reg_addr = PPBUF_BASE2;
-			len = 16;
-		} else {
-			reg_addr = SD_CMD0;
-			len = 5;
-		}
-		retval = rts51x_seq_read_register(chip, reg_addr,
-						     (unsigned short)len, buf);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-	stat = chip->rsp_buf[1];
-
-	/* Check (Start,Transmission) bit of Response */
-	if ((buf[0] & 0xC0) != 0) {
-		sd_set_err_code(chip, SD_STS_ERR);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-	/* Check CRC7 */
-	if (!(rsp_type & SD_NO_CHECK_CRC7)) {
-		if (stat & SD_CRC7_ERR) {
-			if (cmd_idx == WRITE_MULTIPLE_BLOCK) {
-				sd_set_err_code(chip, SD_CRC_ERR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-			if (rty_cnt < SD_MAX_RETRY_COUNT) {
-				wait_timeout(20);
-				rty_cnt++;
-				goto RTY_SEND_CMD;
-			} else {
-				sd_set_err_code(chip, SD_CRC_ERR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-	}
-	/* Check Status */
-	if ((rsp_type == SD_RSP_TYPE_R1) || (rsp_type == SD_RSP_TYPE_R1b)) {
-		if ((cmd_idx != SEND_RELATIVE_ADDR)
-		    && (cmd_idx != SEND_IF_COND)) {
-			if (cmd_idx != STOP_TRANSMISSION) {
-				if (buf[1] & 0x80)
-					TRACE_RET(chip, STATUS_FAIL);
-			}
-			if (buf[1] & 0x7F) {
-				RTS51X_DEBUGP("buf[1]: 0x%02x\n", buf[1]);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-			if (buf[2] & 0xFF) {
-				RTS51X_DEBUGP("buf[2]: 0x%02x\n", buf[2]);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-			if (buf[3] & 0x80) {
-				RTS51X_DEBUGP("buf[3]: 0x%02x\n", buf[3]);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-			if (buf[3] & 0x01) {
-				/* Get "READY_FOR_DATA" bit */
-				sd_card->sd_data_buf_ready = 1;
-			} else {
-				sd_card->sd_data_buf_ready = 0;
-			}
-		}
-	}
-
-	if (rsp && rsp_len)
-		memcpy(rsp, buf, rsp_len);
-
-	return STATUS_SUCCESS;
-}
-
-static inline void sd_print_debug_reg(struct rts51x_chip *chip)
-{
-#ifdef CONFIG_RTS5139_DEBUG
-	u8 val = 0;
-
-	rts51x_ep0_read_register(chip, SD_STAT1, &val);
-	RTS51X_DEBUGP("SD_STAT1: 0x%x\n", val);
-	rts51x_ep0_read_register(chip, SD_STAT2, &val);
-	RTS51X_DEBUGP("SD_STAT2: 0x%x\n", val);
-	rts51x_ep0_read_register(chip, SD_BUS_STAT, &val);
-	RTS51X_DEBUGP("SD_BUS_STAT: 0x%x\n", val);
-#endif
-}
-
-int sd_read_data(struct rts51x_chip *chip, u8 trans_mode, u8 *cmd, int cmd_len,
-		 u16 byte_cnt, u16 blk_cnt, u8 bus_width, u8 *buf, int buf_len,
-		 int timeout)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	int i;
-
-	sd_clr_err_code(chip);
-
-	if (!buf)
-		buf_len = 0;
-
-	if (buf_len > 512)
-		/* This function can't read data more than one page */
-		TRACE_RET(chip, STATUS_FAIL);
-
-	rts51x_init_cmd(chip);
-
-	if (cmd_len) {
-		RTS51X_DEBUGP("SD/MMC CMD %d\n", cmd[0] - 0x40);
-		for (i = 0; i < (cmd_len < 6 ? cmd_len : 6); i++) {
-			rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD0 + i, 0xFF,
-				       cmd[i]);
-		}
-	}
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF, (u8) byte_cnt);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_H, 0xFF,
-		       (u8) (byte_cnt >> 8));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_L, 0xFF, (u8) blk_cnt);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_H, 0xFF,
-		       (u8) (blk_cnt >> 8));
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG1, 0x03, bus_width);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF,
-		       SD_CALCULATE_CRC7 | SD_CHECK_CRC16 | SD_NO_WAIT_BUSY_END
-		       | SD_CHECK_CRC7 | SD_RSP_LEN_6);
-	if (trans_mode != SD_TM_AUTO_TUNING) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-			       PINGPONG_BUFFER);
-	}
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
-		       trans_mode | SD_TRANSFER_START);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER, SD_TRANSFER_END,
-		       SD_TRANSFER_END);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, 1, timeout);
-
-	if (CHECK_SD_TRANS_FAIL(chip, retval)) {
-		sd_print_debug_reg(chip);
-		if (retval == STATUS_TIMEDOUT) {
-			sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
-					    SD_RSP_TYPE_R1, NULL, 0);
-		}
-
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if (buf && buf_len) {
-		retval = rts51x_read_ppbuf(chip, buf, buf_len);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_write_data(struct rts51x_chip *chip, u8 trans_mode,
-			 u8 *cmd, int cmd_len, u16 byte_cnt, u16 blk_cnt,
-			 u8 bus_width, u8 *buf, int buf_len, int timeout)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	int i;
-
-	sd_clr_err_code(chip);
-
-	if (!buf)
-		buf_len = 0;
-
-	/* This function can't write data more than one page */
-	if (buf_len > 512)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	if (buf && buf_len) {
-		retval = rts51x_write_ppbuf(chip, buf, buf_len);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	rts51x_init_cmd(chip);
-
-	if (cmd_len) {
-		RTS51X_DEBUGP("SD/MMC CMD %d\n", cmd[0] - 0x40);
-		for (i = 0; i < (cmd_len < 6 ? cmd_len : 6); i++) {
-			rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD0 + i, 0xFF,
-				       cmd[i]);
-		}
-	}
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF, (u8) byte_cnt);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_H, 0xFF,
-		       (u8) (byte_cnt >> 8));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_L, 0xFF, (u8) blk_cnt);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_H, 0xFF,
-		       (u8) (blk_cnt >> 8));
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG1, 0x03, bus_width);
-
-	if (cmd_len) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF,
-			       SD_CALCULATE_CRC7 | SD_CHECK_CRC16 |
-			       SD_WAIT_BUSY_END | SD_CHECK_CRC7 | SD_RSP_LEN_6);
-
-	} else {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF,
-			       SD_CALCULATE_CRC7 | SD_CHECK_CRC16 |
-			       SD_NO_WAIT_BUSY_END | SD_CHECK_CRC7 |
-			       SD_RSP_LEN_6);
-	}
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
-		       trans_mode | SD_TRANSFER_START);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER, SD_TRANSFER_END,
-		       SD_TRANSFER_END);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, 1, timeout);
-
-	if (CHECK_SD_TRANS_FAIL(chip, retval)) {
-		sd_print_debug_reg(chip);
-
-		if (retval == STATUS_TIMEDOUT)
-			sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
-					    SD_RSP_TYPE_R1, NULL, 0);
-
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_check_csd(struct rts51x_chip *chip, char check_wp)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	int i;
-	u8 csd_ver, trans_speed;
-	u8 rsp[16];
-
-	for (i = 0; i < 6; i++) {
-		if (monitor_card_cd(chip, SD_CARD) == CD_NOT_EXIST) {
-			sd_set_reset_fail(chip, SD_RESET_FAIL);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		retval =
-		    sd_send_cmd_get_rsp(chip, SEND_CSD, sd_card->sd_addr,
-					SD_RSP_TYPE_R2, rsp, 16);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-
-	if (i == 6)
-		TRACE_RET(chip, STATUS_FAIL);
-	memcpy(sd_card->raw_csd, rsp + 1, 15);
-	/* Get CRC7 */
-	RTS51X_READ_REG(chip, SD_CMD5, sd_card->raw_csd + 15);
-
-	RTS51X_DEBUGP("CSD Response:\n");
-	RTS51X_DUMP(rsp, 16);
-
-	/* Get CSD Version */
-	csd_ver = (rsp[1] & 0xc0) >> 6;
-	RTS51X_DEBUGP("csd_ver = %d\n", csd_ver);
-
-	trans_speed = rsp[4];
-	if ((trans_speed & 0x07) == 0x02) {	/* 10Mbits/s */
-		if ((trans_speed & 0xf8) >= 0x30) {	/* >25Mbits/s */
-			if (chip->asic_code)
-				sd_card->sd_clock = 46;
-			else
-				sd_card->sd_clock = CLK_50;
-		} else if ((trans_speed & 0xf8) == 0x28) { /* 20Mbits/s */
-			if (chip->asic_code)
-				sd_card->sd_clock = 39;
-			else
-				sd_card->sd_clock = CLK_40;
-		} else if ((trans_speed & 0xf8) == 0x20) { /* 15Mbits/s */
-			if (chip->asic_code)
-				sd_card->sd_clock = 29;
-			else
-				sd_card->sd_clock = CLK_30;
-		} else if ((trans_speed & 0xf8) >= 0x10) { /* 12Mbits/s */
-			if (chip->asic_code)
-				sd_card->sd_clock = 23;
-			else
-				sd_card->sd_clock = CLK_20;
-		} else if ((trans_speed & 0x08) >= 0x08) { /* 10Mbits/s */
-			if (chip->asic_code)
-				sd_card->sd_clock = 19;
-			else
-				sd_card->sd_clock = CLK_20;
-		} /*else { */
-			/*If this ,then slow card will use 30M clock */
-			/* TRACE_RET(chip, STATUS_FAIL); */
-		/* } */
-	}
-	/*else {
-	   TRACE_RET(chip, STATUS_FAIL);
-	   } */
-	if (CHK_MMC_SECTOR_MODE(sd_card)) {
-		sd_card->capacity = 0;
-	} else {
-		/* For High-Capacity Card, CSD_STRUCTURE always be "0x1" */
-		if ((!CHK_SD_HCXC(sd_card)) || (csd_ver == 0)) {
-			/* Calculate total sector according to C_SIZE,
-			 * C_SIZE_MULT & READ_BL_LEN */
-			u8 blk_size, c_size_mult;
-			u16 c_size;
-			/* Get READ_BL_LEN */
-			blk_size = rsp[6] & 0x0F;
-			/* Get C_SIZE */
-			c_size = ((u16) (rsp[7] & 0x03) << 10)
-			    + ((u16) rsp[8] << 2)
-			    + ((u16) (rsp[9] & 0xC0) >> 6);
-			/* Get C_SIZE_MUL */
-			c_size_mult = (u8) ((rsp[10] & 0x03) << 1);
-			c_size_mult += (rsp[11] & 0x80) >> 7;
-			/* Calculate total Capacity  */
-			sd_card->capacity =
-			    (((u32) (c_size + 1)) *
-			     (1 << (c_size_mult + 2))) << (blk_size - 9);
-		} else {
-			/* High Capacity Card and Use CSD2.0 Version */
-			u32 total_sector = 0;
-			total_sector = (((u32) rsp[8] & 0x3f) << 16) |
-			    ((u32) rsp[9] << 8) | (u32) rsp[10];
-			/* Total Capacity= (C_SIZE+1) *
-			 * 512K Byte = (C_SIZE+1)K Sector,1K = 1024 Bytes */
-			sd_card->capacity = (total_sector + 1) << 10;
-		}
-	}
-
-	/* We need check Write-Protected Status by Field PERM WP or TEMP WP */
-	if (check_wp) {
-		if (rsp[15] & 0x30)
-			chip->card_wp |= SD_CARD;
-		RTS51X_DEBUGP("CSD WP Status: 0x%x\n", rsp[15]);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_set_sample_push_timing(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-
-	rts51x_init_cmd(chip);
-
-	if (CHK_SD_SDR104(sd_card) || CHK_SD_SDR50(sd_card)) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG1,
-			       0x0C | SD_ASYNC_FIFO_RST,
-			       SD_30_MODE | SD_ASYNC_FIFO_RST);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_CLK_SOURCE, 0xFF,
-			       CRC_VAR_CLK0 | SD30_FIX_CLK | SAMPLE_VAR_CLK1);
-	} else if (CHK_SD_DDR50(sd_card) || CHK_MMC_DDR52(sd_card)) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG1,
-			       0x0C | SD_ASYNC_FIFO_RST,
-			       SD_DDR_MODE | SD_ASYNC_FIFO_RST);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_CLK_SOURCE, 0xFF,
-			       CRC_VAR_CLK0 | SD30_FIX_CLK | SAMPLE_VAR_CLK1);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_PUSH_POINT_CTL,
-			       DDR_VAR_TX_CMD_DAT, DDR_VAR_TX_CMD_DAT);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_SAMPLE_POINT_CTL,
-			       DDR_VAR_RX_DAT | DDR_VAR_RX_CMD,
-			       DDR_VAR_RX_DAT | DDR_VAR_RX_CMD);
-	} else {
-		u8 val = 0;
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG1, 0x0C, SD_20_MODE);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_CLK_SOURCE, 0xFF,
-			       CRC_FIX_CLK | SD30_VAR_CLK0 | SAMPLE_VAR_CLK1);
-
-		if ((chip->option.sd_ctl & SD_PUSH_POINT_CTL_MASK) ==
-		    SD_PUSH_POINT_AUTO) {
-			val = SD20_TX_NEG_EDGE;
-		} else if ((chip->option.sd_ctl & SD_PUSH_POINT_CTL_MASK) ==
-			   SD_PUSH_POINT_DELAY) {
-			val = SD20_TX_14_AHEAD;
-		} else {
-			val = SD20_TX_NEG_EDGE;
-		}
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_PUSH_POINT_CTL,
-			       SD20_TX_SEL_MASK, val);
-
-		if ((chip->option.sd_ctl & SD_SAMPLE_POINT_CTL_MASK) ==
-		    SD_SAMPLE_POINT_AUTO) {
-			if (chip->asic_code) {
-				if (CHK_SD_HS(sd_card) || CHK_MMC_52M(sd_card))
-					val = SD20_RX_14_DELAY;
-				else
-					val = SD20_RX_POS_EDGE;
-			} else {
-				val = SD20_RX_14_DELAY;
-			}
-		} else if ((chip->option.sd_ctl & SD_SAMPLE_POINT_CTL_MASK) ==
-			   SD_SAMPLE_POINT_DELAY) {
-			val = SD20_RX_14_DELAY;
-		} else {
-			val = SD20_RX_POS_EDGE;
-		}
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_SAMPLE_POINT_CTL,
-			       SD20_RX_SEL_MASK, val);
-	}
-
-	if (CHK_MMC_DDR52(sd_card) && CHK_MMC_8BIT(sd_card)) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DMA1_CTL,
-			       EXTEND_DMA1_ASYNC_SIGNAL, 0);
-	}
-
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-static void sd_choose_proper_clock(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-
-	if (CHK_SD_SDR104(sd_card)) {
-		if (chip->asic_code)
-			sd_card->sd_clock = chip->option.asic_sd_sdr104_clk;
-		else
-			sd_card->sd_clock = chip->option.fpga_sd_sdr104_clk;
-	} else if (CHK_SD_DDR50(sd_card)) {
-		if (chip->asic_code)
-			sd_card->sd_clock = chip->option.asic_sd_ddr50_clk;
-		else
-			sd_card->sd_clock = chip->option.fpga_sd_ddr50_clk;
-	} else if (CHK_SD_SDR50(sd_card)) {
-		if (chip->asic_code)
-			sd_card->sd_clock = chip->option.asic_sd_sdr50_clk;
-		else
-			sd_card->sd_clock = chip->option.fpga_sd_sdr50_clk;
-	} else if (CHK_SD_HS(sd_card)) {
-		if (chip->asic_code)
-			sd_card->sd_clock = chip->option.asic_sd_hs_clk;
-		else
-			sd_card->sd_clock = chip->option.fpga_sd_hs_clk;
-	} else if (CHK_MMC_52M(sd_card) || CHK_MMC_DDR52(sd_card)) {
-		if (chip->asic_code)
-			sd_card->sd_clock = chip->option.asic_mmc_52m_clk;
-		else
-			sd_card->sd_clock = chip->option.fpga_mmc_52m_clk;
-	} else if (CHK_MMC_26M(sd_card)) {
-		if (chip->asic_code) {
-			sd_card->sd_clock = 46;
-			RTS51X_DEBUGP("Set MMC clock to 22.5MHz\n");
-		} else {
-			sd_card->sd_clock = CLK_50;
-		}
-	}
-}
-
-static int sd_set_init_para(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-
-	retval = sd_set_sample_push_timing(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	sd_choose_proper_clock(chip);
-
-	retval = switch_clock(chip, sd_card->sd_clock);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_sd_select_card(struct rts51x_chip *chip, int select)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	u8 cmd_idx, cmd_type;
-	u32 addr;
-
-	if (select) {
-		cmd_idx = SELECT_CARD;
-		cmd_type = SD_RSP_TYPE_R1;
-		addr = sd_card->sd_addr;
-	} else {
-		cmd_idx = DESELECT_CARD;
-		cmd_type = SD_RSP_TYPE_R0;
-		addr = 0;
-	}
-
-	retval = sd_send_cmd_get_rsp(chip, cmd_idx, addr, cmd_type, NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_wait_currentstate_dataready(struct rts51x_chip *chip, u8 statechk,
-				   u8 rdychk, u16 pollingcnt)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	u8 rsp[5];
-	u16 i;
-
-	for (i = 0; i < pollingcnt; i++) {
-
-		retval =
-		    sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
-					SD_RSP_TYPE_R1, rsp, 5);
-		if (retval == STATUS_SUCCESS) {
-			if (((rsp[3] & 0x1E) == statechk)
-			    && ((rsp[3] & 0x01) == rdychk)) {
-				return STATUS_SUCCESS;
-			}
-		} else {
-			rts51x_clear_sd_error(chip);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	}
-
-	return STATUS_TIMEDOUT;
-}
-
-static int sd_voltage_switch(struct rts51x_chip *chip)
-{
-	int retval;
-	u8 stat;
-
-	RTS51X_WRITE_REG(chip, SD_BUS_STAT,
-			 SD_CLK_TOGGLE_EN | SD_CLK_FORCE_STOP,
-			 SD_CLK_TOGGLE_EN);
-
-	retval =
-	    sd_send_cmd_get_rsp(chip, VOLTAGE_SWITCH, 0, SD_RSP_TYPE_R1, NULL,
-				0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	RTS51X_READ_REG(chip, SD_BUS_STAT, &stat);
-	if (stat & (SD_CMD_STATUS | SD_DAT3_STATUS | SD_DAT2_STATUS |
-		    SD_DAT1_STATUS | SD_DAT0_STATUS))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	rts51x_init_cmd(chip);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BUS_STAT, 0xFF,
-		       SD_CLK_FORCE_STOP);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_PAD_CTL, SD_IO_USING_1V8,
-		       SD_IO_USING_1V8);
-	if (chip->asic_code)
-		rts51x_add_cmd(chip, WRITE_REG_CMD, LDO_POWER_CFG,
-			       TUNE_SD18_MASK, TUNE_SD18_1V8);
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	wait_timeout(chip->option.D3318_off_delay);
-
-	RTS51X_WRITE_REG(chip, SD_BUS_STAT, 0xFF, SD_CLK_TOGGLE_EN);
-	wait_timeout(10);
-
-	RTS51X_READ_REG(chip, SD_BUS_STAT, &stat);
-	if ((stat & (SD_CMD_STATUS | SD_DAT3_STATUS | SD_DAT2_STATUS |
-		     SD_DAT1_STATUS | SD_DAT0_STATUS)) !=
-	    (SD_CMD_STATUS | SD_DAT3_STATUS | SD_DAT2_STATUS |
-	     SD_DAT1_STATUS | SD_DAT0_STATUS)) {
-		rts51x_init_cmd(chip);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BUS_STAT, 0xFF,
-			       SD_CLK_FORCE_STOP);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_CLK_EN, 0xFF, 0);
-		rts51x_send_cmd(chip, MODE_C, 100);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-	RTS51X_WRITE_REG(chip, SD_BUS_STAT,
-			 SD_CLK_TOGGLE_EN | SD_CLK_FORCE_STOP, 0);
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_change_phase(struct rts51x_chip *chip, u8 sample_point,
-			   u8 tune_dir)
-{
-	u16 SD_VP_CTL, SD_DCMPS_CTL;
-	u8 val;
-	int retval;
-
-	RTS51X_DEBUGP("sd_change_phase (sample_point = %d, tune_dir = %d)\n",
-		       sample_point, tune_dir);
-
-	if (tune_dir == TUNE_RX) {
-		SD_VP_CTL = SD_VPCLK1_CTL;
-		SD_DCMPS_CTL = SD_DCMPS1_CTL;
-	} else {
-		SD_VP_CTL = SD_VPCLK0_CTL;
-		SD_DCMPS_CTL = SD_DCMPS0_CTL;
-	}
-
-	if (chip->asic_code) {
-		RTS51X_WRITE_REG(chip, CLK_DIV, CLK_CHANGE, CLK_CHANGE);
-		RTS51X_WRITE_REG(chip, SD_VP_CTL, 0x1F, sample_point);
-		RTS51X_WRITE_REG(chip, SD_VPCLK0_CTL, PHASE_NOT_RESET, 0);
-		RTS51X_WRITE_REG(chip, SD_VPCLK0_CTL, PHASE_NOT_RESET,
-				 PHASE_NOT_RESET);
-		RTS51X_WRITE_REG(chip, CLK_DIV, CLK_CHANGE, 0);
-	} else {
-#ifdef CONFIG_RTS5139_DEBUG
-		RTS51X_READ_REG(chip, SD_VP_CTL, &val);
-		RTS51X_DEBUGP("SD_VP_CTL: 0x%x\n", val);
-		RTS51X_READ_REG(chip, SD_DCMPS_CTL, &val);
-		RTS51X_DEBUGP("SD_DCMPS_CTL: 0x%x\n", val);
-#endif
-
-		RTS51X_WRITE_REG(chip, CLK_DIV, CLK_CHANGE, CLK_CHANGE);
-		udelay(100);
-		RTS51X_WRITE_REG(chip, SD_VP_CTL, 0xFF,
-				 PHASE_NOT_RESET | sample_point);
-		udelay(200);
-
-		rts51x_init_cmd(chip);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_DCMPS_CTL, DCMPS_CHANGE,
-			       DCMPS_CHANGE);
-		rts51x_add_cmd(chip, CHECK_REG_CMD, SD_DCMPS_CTL,
-			       DCMPS_CHANGE_DONE, DCMPS_CHANGE_DONE);
-		retval = rts51x_send_cmd(chip, MODE_CR, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, Fail);
-
-		retval = rts51x_get_rsp(chip, 1, 500);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, Fail);
-
-		val = chip->rsp_buf[0];
-		if (val & DCMPS_ERROR)
-			TRACE_GOTO(chip, Fail);
-		if ((val & DCMPS_CURRENT_PHASE) != sample_point)
-			TRACE_GOTO(chip, Fail);
-		RTS51X_WRITE_REG(chip, SD_DCMPS_CTL, DCMPS_CHANGE, 0);
-		RTS51X_WRITE_REG(chip, CLK_DIV, CLK_CHANGE, 0);
-		udelay(100);
-	}
-
-	RTS51X_WRITE_REG(chip, SD_CFG1, SD_ASYNC_FIFO_RST, 0);
-
-	return STATUS_SUCCESS;
-
-Fail:
-#ifdef CONFIG_RTS5139_DEBUG
-	rts51x_ep0_read_register(chip, SD_VP_CTL, &val);
-	RTS51X_DEBUGP("SD_VP_CTL: 0x%x\n", val);
-	rts51x_ep0_read_register(chip, SD_DCMPS_CTL, &val);
-	RTS51X_DEBUGP("SD_DCMPS_CTL: 0x%x\n", val);
-#endif
-
-	RTS51X_WRITE_REG(chip, SD_DCMPS_CTL, DCMPS_CHANGE, 0);
-	RTS51X_WRITE_REG(chip, SD_VP_CTL, PHASE_CHANGE, 0);
-	wait_timeout(10);
-
-	return STATUS_FAIL;
-}
-
-static int sd_check_spec(struct rts51x_chip *chip, u8 bus_width)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	u8 cmd[5], buf[8];
-
-	retval =
-	    sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr, SD_RSP_TYPE_R1,
-				NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	cmd[0] = 0x40 | SEND_SCR;
-	cmd[1] = 0;
-	cmd[2] = 0;
-	cmd[3] = 0;
-	cmd[4] = 0;
-
-	retval =
-	    sd_read_data(chip, SD_TM_NORMAL_READ, cmd, 5, 8, 1, bus_width, buf,
-			 8, 250);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_sd_error(chip);
-		TRACE_RET(chip, retval);
-	}
-
-	memcpy(sd_card->raw_scr, buf, 8);
-
-	if ((buf[0] & 0x0F) == 0)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_query_switch_result(struct rts51x_chip *chip, u8 func_group,
-				  u8 func_to_switch, u8 *buf, int buf_len)
-{
-	u8 support_mask = 0, query_switch = 0, switch_busy = 0;
-	int support_offset = 0, query_switch_offset = 0, check_busy_offset = 0;
-
-	if (func_group == SD_FUNC_GROUP_1) {
-		support_offset = FUNCTION_GROUP1_SUPPORT_OFFSET;
-		query_switch_offset = FUNCTION_GROUP1_QUERY_SWITCH_OFFSET;
-		check_busy_offset = FUNCTION_GROUP1_CHECK_BUSY_OFFSET;
-
-		switch (func_to_switch) {
-		case HS_SUPPORT:
-			support_mask = HS_SUPPORT_MASK;
-			query_switch = HS_QUERY_SWITCH_OK;
-			switch_busy = HS_SWITCH_BUSY;
-			break;
-
-		case SDR50_SUPPORT:
-			support_mask = SDR50_SUPPORT_MASK;
-			query_switch = SDR50_QUERY_SWITCH_OK;
-			switch_busy = SDR50_SWITCH_BUSY;
-			break;
-
-		case SDR104_SUPPORT:
-			support_mask = SDR104_SUPPORT_MASK;
-			query_switch = SDR104_QUERY_SWITCH_OK;
-			switch_busy = SDR104_SWITCH_BUSY;
-			break;
-
-		case DDR50_SUPPORT:
-			support_mask = DDR50_SUPPORT_MASK;
-			query_switch = DDR50_QUERY_SWITCH_OK;
-			switch_busy = DDR50_SWITCH_BUSY;
-			break;
-
-		default:
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	} else if (func_group == SD_FUNC_GROUP_3) {
-		support_offset = FUNCTION_GROUP3_SUPPORT_OFFSET;
-		query_switch_offset = FUNCTION_GROUP3_QUERY_SWITCH_OFFSET;
-		check_busy_offset = FUNCTION_GROUP3_CHECK_BUSY_OFFSET;
-
-		switch (func_to_switch) {
-		case DRIVING_TYPE_A:
-			support_mask = DRIVING_TYPE_A_MASK;
-			query_switch = TYPE_A_QUERY_SWITCH_OK;
-			switch_busy = TYPE_A_SWITCH_BUSY;
-			break;
-
-		case DRIVING_TYPE_C:
-			support_mask = DRIVING_TYPE_C_MASK;
-			query_switch = TYPE_C_QUERY_SWITCH_OK;
-			switch_busy = TYPE_C_SWITCH_BUSY;
-			break;
-
-		case DRIVING_TYPE_D:
-			support_mask = DRIVING_TYPE_D_MASK;
-			query_switch = TYPE_D_QUERY_SWITCH_OK;
-			switch_busy = TYPE_D_SWITCH_BUSY;
-			break;
-
-		default:
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	} else if (func_group == SD_FUNC_GROUP_4) {
-		support_offset = FUNCTION_GROUP4_SUPPORT_OFFSET;
-		query_switch_offset = FUNCTION_GROUP4_QUERY_SWITCH_OFFSET;
-		check_busy_offset = FUNCTION_GROUP4_CHECK_BUSY_OFFSET;
-
-		switch (func_to_switch) {
-		case CURRENT_LIMIT_400:
-			support_mask = CURRENT_LIMIT_400_MASK;
-			query_switch = CURRENT_LIMIT_400_QUERY_SWITCH_OK;
-			switch_busy = CURRENT_LIMIT_400_SWITCH_BUSY;
-			break;
-
-		case CURRENT_LIMIT_600:
-			support_mask = CURRENT_LIMIT_600_MASK;
-			query_switch = CURRENT_LIMIT_600_QUERY_SWITCH_OK;
-			switch_busy = CURRENT_LIMIT_600_SWITCH_BUSY;
-			break;
-
-		case CURRENT_LIMIT_800:
-			support_mask = CURRENT_LIMIT_800_MASK;
-			query_switch = CURRENT_LIMIT_800_QUERY_SWITCH_OK;
-			switch_busy = CURRENT_LIMIT_800_SWITCH_BUSY;
-			break;
-
-		default:
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	} else {
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if (func_group == SD_FUNC_GROUP_4)
-		buf[query_switch_offset] =
-		    (buf[query_switch_offset] & 0xf0) >> 4;
-	if (!(buf[support_offset] & support_mask) ||
-	    ((buf[query_switch_offset] & 0x0F) != query_switch))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	if ((buf[DATA_STRUCTURE_VER_OFFSET] == 0x01) &&
-	    ((buf[check_busy_offset] & switch_busy) == switch_busy))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_check_switch_mode(struct rts51x_chip *chip, u8 mode,
-				u8 func_group, u8 func_to_switch, u8 bus_width)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	u8 cmd[5], buf[64];
-
-	RTS51X_DEBUGP("sd_check_switch_mode (mode = %d, func_group = %d,"
-		"func_to_switch = %d)\n", mode, func_group, func_to_switch);
-
-	cmd[0] = 0x40 | SWITCH;
-	cmd[1] = mode;
-
-	if (func_group == SD_FUNC_GROUP_1) {
-		cmd[2] = 0xFF;
-		cmd[3] = 0xFF;
-		cmd[4] = 0xF0 + func_to_switch;
-	} else if (func_group == SD_FUNC_GROUP_3) {
-		cmd[2] = 0xFF;
-		cmd[3] = 0xF0 + func_to_switch;
-		cmd[4] = 0xFF;
-	} else if (func_group == SD_FUNC_GROUP_4) {
-		cmd[2] = 0xFF;
-		cmd[3] = 0x0F + (func_to_switch << 4);
-		cmd[4] = 0xFF;
-	} else {
-		cmd[1] = SD_CHECK_MODE;
-		cmd[2] = 0xFF;
-		cmd[3] = 0xFF;
-		cmd[4] = 0xFF;
-	}
-
-	retval =
-	    sd_read_data(chip, SD_TM_NORMAL_READ, cmd, 5, 64, 1, bus_width, buf,
-			 64, 250);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_sd_error(chip);
-		TRACE_RET(chip, retval);
-	}
-
-	if (func_group == NO_ARGUMENT) {
-		sd_card->func_group1_mask = buf[0x0D];
-		sd_card->func_group2_mask = buf[0x0B];
-		sd_card->func_group3_mask = buf[0x09];
-		sd_card->func_group4_mask = buf[0x07];
-
-		RTS51X_DEBUGP("func_group1_mask = 0x%02x\n", buf[0x0D]);
-		RTS51X_DEBUGP("func_group2_mask = 0x%02x\n", buf[0x0B]);
-		RTS51X_DEBUGP("func_group3_mask = 0x%02x\n", buf[0x09]);
-		RTS51X_DEBUGP("func_group4_mask = 0x%02x\n", buf[0x07]);
-	} else {
-		if ((buf[0] == 0) && (buf[1] == 0))
-			TRACE_RET(chip, STATUS_FAIL);
-		retval =
-		    sd_query_switch_result(chip, func_group, func_to_switch,
-					   buf, 64);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_check_switch(struct rts51x_chip *chip,
-			   u8 func_group, u8 func_to_switch, u8 bus_width)
-{
-	int retval;
-	int i;
-	int switch_good = 0;
-
-	for (i = 0; i < 3; i++) {
-		if (monitor_card_cd(chip, SD_CARD) == CD_NOT_EXIST) {
-			sd_set_reset_fail(chip, SD_RESET_FAIL);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		retval = sd_check_switch_mode(chip, SD_CHECK_MODE, func_group,
-					      func_to_switch, bus_width);
-		if (retval == STATUS_SUCCESS) {
-			u8 stat;
-
-			retval = sd_check_switch_mode(chip, SD_SWITCH_MODE,
-					func_group, func_to_switch, bus_width);
-			if (retval == STATUS_SUCCESS) {
-				switch_good = 1;
-				break;
-			}
-
-			RTS51X_READ_REG(chip, SD_STAT1, &stat);
-
-			if (stat & SD_CRC16_ERR) {
-				RTS51X_DEBUGP("SD CRC16 error when switching"
-							"mode\n");
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-
-		wait_timeout(20);
-	}
-
-	if (!switch_good)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_switch_function(struct rts51x_chip *chip, u8 bus_width)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	int i;
-	u8 func_to_switch = 0;
-
-	/* Get supported functions */
-	retval = sd_check_switch_mode(chip, SD_CHECK_MODE,
-				      NO_ARGUMENT, NO_ARGUMENT, bus_width);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	sd_card->func_group1_mask &= ~(sd_card->sd_switch_fail);
-
-	for (i = 0; i < 4; i++) {
-		switch ((u8) (chip->option.sd_speed_prior >> (i * 8))) {
-		case DDR50_SUPPORT:
-			if ((sd_card->func_group1_mask & DDR50_SUPPORT_MASK)
-			    && (CHECK_UHS50(chip)))
-				func_to_switch = DDR50_SUPPORT;
-			break;
-
-		case SDR50_SUPPORT:
-			if ((sd_card->func_group1_mask & SDR50_SUPPORT_MASK)
-			    && (CHECK_UHS50(chip)))
-				func_to_switch = SDR50_SUPPORT;
-			break;
-
-		case HS_SUPPORT:
-			if (sd_card->func_group1_mask & HS_SUPPORT_MASK)
-				func_to_switch = HS_SUPPORT;
-			break;
-
-		default:
-			continue;
-		}
-
-		if (func_to_switch)
-			break;
-	}
-	RTS51X_DEBUGP("SD_FUNC_GROUP_1: func_to_switch = 0x%02x",
-		       func_to_switch);
-
-	if (func_to_switch) {
-		retval =
-		    sd_check_switch(chip, SD_FUNC_GROUP_1, func_to_switch,
-				    bus_width);
-		if (retval != STATUS_SUCCESS) {
-			if (func_to_switch == SDR104_SUPPORT)
-				sd_card->sd_switch_fail = SDR104_SUPPORT_MASK;
-			else if (func_to_switch == DDR50_SUPPORT)
-				sd_card->sd_switch_fail = DDR50_SUPPORT_MASK;
-			else if (func_to_switch == SDR50_SUPPORT)
-				sd_card->sd_switch_fail = SDR50_SUPPORT_MASK;
-			else if (func_to_switch == HS_SUPPORT)
-				sd_card->sd_switch_fail = HS_SUPPORT_MASK;
-
-			TRACE_RET(chip, retval);
-		}
-
-		if (func_to_switch == SDR104_SUPPORT)
-			SET_SD_SDR104(sd_card);
-		else if (func_to_switch == DDR50_SUPPORT)
-			SET_SD_DDR50(sd_card);
-		else if (func_to_switch == SDR50_SUPPORT)
-			SET_SD_SDR50(sd_card);
-		else
-			SET_SD_HS(sd_card);
-	}
-
-	if (CHK_SD_DDR50(sd_card))
-		RTS51X_WRITE_REG(chip, SD_CFG1, 0x0C, SD_DDR_MODE);
-
-	func_to_switch = 0;
-	if (sd_card->func_group4_mask & CURRENT_LIMIT_400_MASK)
-		func_to_switch = CURRENT_LIMIT_400;
-
-	if (func_to_switch) {
-		RTS51X_DEBUGP("Try to switch current_limit_400\n");
-		retval =
-		    sd_check_switch(chip, SD_FUNC_GROUP_4, func_to_switch,
-				    bus_width);
-		RTS51X_DEBUGP("Switch current_limit_400 status: (%d)\n",
-			       retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_wait_data_idle(struct rts51x_chip *chip)
-{
-	int retval = STATUS_TIMEDOUT;
-	int i;
-	u8 val = 0;
-
-	for (i = 0; i < 100; i++) {
-		retval = rts51x_ep0_read_register(chip, SD_DATA_STATE, &val);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, STATUS_FAIL);
-		if (val & SD_DATA_IDLE) {
-			retval = STATUS_SUCCESS;
-			break;
-		}
-		udelay(100);
-	}
-	RTS51X_DEBUGP("SD_DATA_STATE: 0x%02x\n", val);
-
-	return retval;
-}
-
-static int sd_sdr_tuning_rx_cmd(struct rts51x_chip *chip, u8 sample_point)
-{
-	int retval;
-	u8 cmd[5];
-
-	retval = sd_change_phase(chip, sample_point, TUNE_RX);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	cmd[0] = 0x40 | SEND_TUNING_PATTERN;
-	cmd[1] = 0;
-	cmd[2] = 0;
-	cmd[3] = 0;
-	cmd[4] = 0;
-
-	retval = sd_read_data(chip, SD_TM_AUTO_TUNING,
-			      cmd, 5, 0x40, 1, SD_BUS_WIDTH_4, NULL, 0, 100);
-	if (retval != STATUS_SUCCESS) {
-		/* Wait till SD DATA IDLE */
-		(void)sd_wait_data_idle(chip);
-
-		rts51x_clear_sd_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_ddr_tuning_rx_cmd(struct rts51x_chip *chip, u8 sample_point)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	u8 cmd[5];
-
-	retval = sd_change_phase(chip, sample_point, TUNE_RX);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	RTS51X_DEBUGP("sd ddr tuning rx\n");
-
-	retval =
-	    sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr, SD_RSP_TYPE_R1,
-				NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	cmd[0] = 0x40 | SD_STATUS;
-	cmd[1] = 0;
-	cmd[2] = 0;
-	cmd[3] = 0;
-	cmd[4] = 0;
-
-	retval = sd_read_data(chip, SD_TM_NORMAL_READ,
-			      cmd, 5, 64, 1, SD_BUS_WIDTH_4, NULL, 0, 100);
-	if (retval != STATUS_SUCCESS) {
-		/* Wait till SD DATA IDLE */
-		(void)sd_wait_data_idle(chip);
-
-		rts51x_clear_sd_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int mmc_ddr_tunning_rx_cmd(struct rts51x_chip *chip, u8 sample_point)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	u8 cmd[5], bus_width;
-
-	if (CHK_MMC_8BIT(sd_card))
-		bus_width = SD_BUS_WIDTH_8;
-	else if (CHK_MMC_4BIT(sd_card))
-		bus_width = SD_BUS_WIDTH_4;
-	else
-		bus_width = SD_BUS_WIDTH_1;
-
-	retval = sd_change_phase(chip, sample_point, TUNE_RX);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	RTS51X_DEBUGP("mmc ddr tuning rx\n");
-
-	cmd[0] = 0x40 | SEND_EXT_CSD;
-	cmd[1] = 0;
-	cmd[2] = 0;
-	cmd[3] = 0;
-	cmd[4] = 0;
-
-	retval = sd_read_data(chip, SD_TM_NORMAL_READ,
-			      cmd, 5, 0x200, 1, bus_width, NULL, 0, 100);
-	if (retval != STATUS_SUCCESS) {
-		/* Wait till SD DATA IDLE */
-		(void)sd_wait_data_idle(chip);
-
-		rts51x_clear_sd_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_sdr_tuning_tx_cmd(struct rts51x_chip *chip, u8 sample_point)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-
-	retval = sd_change_phase(chip, sample_point, TUNE_TX);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	RTS51X_WRITE_REG(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN,
-			 SD_RSP_80CLK_TIMEOUT_EN);
-
-	retval = sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
-				     SD_RSP_TYPE_R1, NULL, 0);
-	if (retval != STATUS_SUCCESS) {
-		if (sd_check_err_code(chip, SD_RSP_TIMEOUT)) {
-			/* Tunning TX fail */
-			rts51x_ep0_write_register(chip, SD_CFG3,
-						  SD_RSP_80CLK_TIMEOUT_EN, 0);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	}
-
-	RTS51X_WRITE_REG(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN, 0);
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_ddr_tuning_tx_cmd(struct rts51x_chip *chip, u8 sample_point)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	u8 cmd[5], bus_width;
-
-	retval = sd_change_phase(chip, sample_point, TUNE_TX);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (CHK_SD(sd_card)) {
-		bus_width = SD_BUS_WIDTH_4;
-	} else {
-		if (CHK_MMC_8BIT(sd_card))
-			bus_width = SD_BUS_WIDTH_8;
-		else if (CHK_MMC_4BIT(sd_card))
-			bus_width = SD_BUS_WIDTH_4;
-		else
-			bus_width = SD_BUS_WIDTH_1;
-	}
-	retval = sd_wait_currentstate_dataready(chip, 0x08, 1, 20);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	RTS51X_WRITE_REG(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN,
-			 SD_RSP_80CLK_TIMEOUT_EN);
-
-	cmd[0] = 0x40 | PROGRAM_CSD;
-	cmd[1] = 0;
-	cmd[2] = 0;
-	cmd[3] = 0;
-	cmd[4] = 0;
-
-	retval = sd_write_data(chip, SD_TM_AUTO_WRITE_2,
-			cmd, 5, 16, 1, bus_width, sd_card->raw_csd, 16, 100);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_sd_error(chip);
-		/* Tunning TX fail */
-		rts51x_ep0_write_register(chip, SD_CFG3,
-					  SD_RSP_80CLK_TIMEOUT_EN, 0);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	RTS51X_WRITE_REG(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN, 0);
-
-	sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr, SD_RSP_TYPE_R1,
-			    NULL, 0);
-
-	return STATUS_SUCCESS;
-}
-
-static u8 sd_search_final_phase(struct rts51x_chip *chip, u32 phase_map,
-				u8 tune_dir)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	struct timing_phase_path path[MAX_PHASE + 1];
-	int i, j, cont_path_cnt;
-	int new_block, max_len;
-	u8 final_phase = 0xFF;
-	int final_path_idx;
-
-	if (phase_map == 0xffff) {
-		if (CHK_SD_DDR50(sd_card)) {
-			if (tune_dir == TUNE_TX)
-				final_phase = chip->option.ddr50_tx_phase;
-			else
-				final_phase = chip->option.ddr50_rx_phase;
-			RTS51X_DEBUGP("DDR50 tuning dir:%d all pass,"
-					"so select default phase:0x%x.\n",
-					tune_dir, final_phase);
-		} else {
-			if (tune_dir == TUNE_TX)
-				final_phase = chip->option.sdr50_tx_phase;
-			else
-				final_phase = chip->option.sdr50_rx_phase;
-			RTS51X_DEBUGP("SDR50 tuning dir:%d all pass,"
-					"so select default phase:0x%x.\n",
-					tune_dir, final_phase);
-		}
-		goto Search_Finish;
-	}
-
-	cont_path_cnt = 0;
-	new_block = 1;
-	j = 0;
-	for (i = 0; i < MAX_PHASE + 1; i++) {
-		if (phase_map & (1 << i)) {
-			if (new_block) {
-				new_block = 0;
-				j = cont_path_cnt++;
-				path[j].start = i;
-				path[j].end = i;
-			} else {
-				path[j].end = i;
-			}
-		} else {
-			new_block = 1;
-			if (cont_path_cnt) {
-				int idx = cont_path_cnt - 1;
-				path[idx].len =
-				    path[idx].end - path[idx].start + 1;
-				path[idx].mid =
-				    path[idx].start + path[idx].len / 2;
-			}
-		}
-	}
-
-	if (cont_path_cnt == 0) {
-		RTS51X_DEBUGP("No continuous phase path\n");
-		goto Search_Finish;
-	} else {
-		int idx = cont_path_cnt - 1;
-		path[idx].len = path[idx].end - path[idx].start + 1;
-		path[idx].mid = path[idx].start + path[idx].len / 2;
-	}
-
-	if ((path[0].start == 0) &&
-			(path[cont_path_cnt - 1].end == MAX_PHASE)) {
-		path[0].start = path[cont_path_cnt - 1].start - MAX_PHASE - 1;
-		path[0].len += path[cont_path_cnt - 1].len;
-		path[0].mid = path[0].start + path[0].len / 2;
-		if (path[0].mid < 0)
-			path[0].mid += MAX_PHASE + 1;
-		cont_path_cnt--;
-	}
-	max_len = 0;
-	final_phase = 0;
-	final_path_idx = 0;
-	for (i = 0; i < cont_path_cnt; i++) {
-		if (path[i].len > max_len) {
-			max_len = path[i].len;
-			final_phase = (u8) path[i].mid;
-			final_path_idx = i;
-		}
-
-		RTS51X_DEBUGP("path[%d].start = %d\n", i, path[i].start);
-		RTS51X_DEBUGP("path[%d].end = %d\n", i, path[i].end);
-		RTS51X_DEBUGP("path[%d].len = %d\n", i, path[i].len);
-		RTS51X_DEBUGP("path[%d].mid = %d\n", i, path[i].mid);
-		RTS51X_DEBUGP("\n");
-	}
-
-	if ((tune_dir == TUNE_TX) && (CHK_SD_SDR50(sd_card))
-	    && chip->option.sdr50_phase_sel) {
-		if (max_len > 6) {
-			int temp_mid = (max_len - 6) / 2;
-			int temp_final_phase =
-			    path[final_path_idx].end - (max_len -
-							(3 + temp_mid));
-
-			if (temp_final_phase < 0)
-				final_phase = temp_final_phase + MAX_PHASE + 1;
-			else
-				final_phase = (u8) temp_final_phase;
-		}
-	}
-
-Search_Finish:
-	RTS51X_DEBUGP("Final chosen phase: %d\n", final_phase);
-	return final_phase;
-}
-
-static int sd_tuning_rx(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	int i, j;
-	u32 raw_phase_map[3], phase_map;
-	u8 final_phase;
-	int (*tuning_cmd)(struct rts51x_chip *chip, u8 sample_point);
-
-	if (CHK_SD(sd_card)) {
-		if (CHK_SD_DDR50(sd_card))
-			tuning_cmd = sd_ddr_tuning_rx_cmd;
-		else
-			tuning_cmd = sd_sdr_tuning_rx_cmd;
-	} else {
-		if (CHK_MMC_DDR52(sd_card))
-			tuning_cmd = mmc_ddr_tunning_rx_cmd;
-		else
-			TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	for (i = 0; i < 3; i++) {
-		raw_phase_map[i] = 0;
-		for (j = MAX_PHASE; j >= 0; j--) {
-			if (monitor_card_cd(chip, SD_CARD) == CD_NOT_EXIST) {
-				sd_set_reset_fail(chip, SD_RESET_FAIL);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-
-			retval = tuning_cmd(chip, (u8) j);
-			if (retval == STATUS_SUCCESS)
-				raw_phase_map[i] |= 1 << j;
-			else
-				RTS51X_DEBUGP("Tuning phase %d fail\n", j);
-		}
-	}
-
-	phase_map = raw_phase_map[0] & raw_phase_map[1] & raw_phase_map[2];
-	for (i = 0; i < 3; i++)
-		RTS51X_DEBUGP("RX raw_phase_map[%d] = 0x%04x\n", i,
-			       raw_phase_map[i]);
-	RTS51X_DEBUGP("RX phase_map = 0x%04x\n", phase_map);
-
-	final_phase = sd_search_final_phase(chip, phase_map, TUNE_RX);
-	if (final_phase == 0xFF)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	retval = tuning_cmd(chip, final_phase);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_ddr_pre_tuning_tx(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	u8 i;
-	u8 pre_tune_tx_phase;
-	u32 pre_tune_phase_map;
-
-	RTS51X_WRITE_REG(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN,
-			 SD_RSP_80CLK_TIMEOUT_EN);
-
-	pre_tune_tx_phase = 0xFF;
-	pre_tune_phase_map = 0x0000;
-	for (i = 0; i < MAX_PHASE + 1; i++) {
-		if (monitor_card_cd(chip, SD_CARD) == CD_NOT_EXIST) {
-			sd_set_reset_fail(chip, SD_RESET_FAIL);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		retval = sd_change_phase(chip, (u8) i, TUNE_TX);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		retval =
-		    sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
-					SD_RSP_TYPE_R1, NULL, 0);
-		if ((retval == STATUS_SUCCESS)
-		    || !sd_check_err_code(chip, SD_RSP_TIMEOUT))
-			pre_tune_phase_map |= (u32) 1 << i;
-	}
-
-	RTS51X_WRITE_REG(chip, SD_CFG3, SD_RSP_80CLK_TIMEOUT_EN, 0);
-
-	pre_tune_tx_phase =
-	    sd_search_final_phase(chip, pre_tune_phase_map, TUNE_TX);
-	if (pre_tune_tx_phase == 0xFF)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	sd_change_phase(chip, pre_tune_tx_phase, TUNE_TX);
-	RTS51X_DEBUGP("DDR TX pre tune phase: %d\n", (int)pre_tune_tx_phase);
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_tuning_tx(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	int i, j;
-	u32 raw_phase_map[3], phase_map;
-	u8 final_phase;
-	int (*tuning_cmd)(struct rts51x_chip *chip, u8 sample_point);
-
-	if (CHK_SD(sd_card)) {
-		if (CHK_SD_DDR50(sd_card))
-			tuning_cmd = sd_ddr_tuning_tx_cmd;
-		else
-			tuning_cmd = sd_sdr_tuning_tx_cmd;
-	} else {
-		if (CHK_MMC_DDR52(sd_card))
-			tuning_cmd = sd_ddr_tuning_tx_cmd;
-		else
-			TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	for (i = 0; i < 3; i++) {
-		raw_phase_map[i] = 0;
-		for (j = MAX_PHASE; j >= 0; j--) {
-			if (monitor_card_cd(chip, SD_CARD) == CD_NOT_EXIST) {
-				sd_set_reset_fail(chip, SD_RESET_FAIL);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-
-			retval = tuning_cmd(chip, (u8) j);
-			if (retval == STATUS_SUCCESS)
-				raw_phase_map[i] |= 1 << j;
-			else
-				RTS51X_DEBUGP("Tuning phase %d fail\n", j);
-		}
-	}
-
-	phase_map = raw_phase_map[0] & raw_phase_map[1] & raw_phase_map[2];
-	for (i = 0; i < 3; i++)
-		RTS51X_DEBUGP("TX raw_phase_map[%d] = 0x%04x\n", i,
-			       raw_phase_map[i]);
-	RTS51X_DEBUGP("TX phase_map = 0x%04x\n", phase_map);
-
-	final_phase = sd_search_final_phase(chip, phase_map, TUNE_TX);
-	if (final_phase == 0xFF)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	retval = tuning_cmd(chip, final_phase);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_sdr_tuning(struct rts51x_chip *chip)
-{
-	int retval;
-
-	retval = sd_tuning_tx(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = sd_tuning_rx(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_ddr_tuning(struct rts51x_chip *chip)
-{
-	int retval;
-
-	if (!(chip->option.sd_ctl & SD_DDR_TX_PHASE_SET_BY_USER)) {
-		retval = sd_ddr_pre_tuning_tx(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	} else {
-		retval =
-		    sd_change_phase(chip, (u8) chip->option.sd_ddr_tx_phase,
-				    TUNE_TX);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	retval = sd_tuning_rx(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (!(chip->option.sd_ctl & SD_DDR_TX_PHASE_SET_BY_USER)) {
-		retval = sd_tuning_tx(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int mmc_ddr_tuning(struct rts51x_chip *chip)
-{
-	int retval;
-
-	if (!(chip->option.sd_ctl & MMC_DDR_TX_PHASE_SET_BY_USER)) {
-		retval = sd_ddr_pre_tuning_tx(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	} else {
-		retval =
-		    sd_change_phase(chip, (u8) chip->option.mmc_ddr_tx_phase,
-				    TUNE_TX);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	retval = sd_tuning_rx(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (!(chip->option.sd_ctl & MMC_DDR_TX_PHASE_SET_BY_USER)) {
-		retval = sd_tuning_tx(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_sd_switch_clock(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	int re_tuning = 0;
-
-	retval = rts51x_select_card(chip, SD_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (CHK_SD30_SPEED(sd_card) || CHK_MMC_DDR52(sd_card)) {
-		if (sd_card->sd_clock != chip->cur_clk)
-			re_tuning = 1;
-	}
-
-	retval = switch_clock(chip, sd_card->sd_clock);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (re_tuning) {
-		if (CHK_SD(sd_card)) {
-			if (CHK_SD_DDR50(sd_card))
-				retval = sd_ddr_tuning(chip);
-			else
-				retval = sd_sdr_tuning(chip);
-		} else {
-			if (CHK_MMC_DDR52(sd_card))
-				retval = mmc_ddr_tuning(chip);
-		}
-
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_prepare_reset(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-
-	if (chip->asic_code)
-		sd_card->sd_clock = 29;
-	else
-		sd_card->sd_clock = CLK_30;
-
-	/* Set SD Clocks */
-	retval = sd_set_init_para(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG1, 0xFF,
-		       SD_CLK_DIVIDE_128 | SD_20_MODE | SD_BUS_WIDTH_1);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_SAMPLE_POINT_CTL, 0xFF,
-		       SD20_RX_POS_EDGE);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_PUSH_POINT_CTL, 0xFF, 0);
-
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_select_card(chip, SD_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-static void sd_pull_ctl_disable(struct rts51x_chip *chip)
-{
-	if (CHECK_PKG(chip, LQFP48)) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0xA5);
-	} else {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0x65);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x56);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0x59);
-	}
-}
-
-static void sd_pull_ctl_enable(struct rts51x_chip *chip)
-{
-	if (CHECK_PKG(chip, LQFP48)) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0xAA);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0xAA);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0xA9);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0xA5);
-	} else {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0xA5);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x9A);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0xA5);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x9A);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x65);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0x5A);
-	}
-}
-
-static int sd_init_power(struct rts51x_chip *chip)
-{
-	int retval;
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, LDO3318_PWR_MASK,
-		       LDO_ON);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_PAD_CTL, SD_IO_USING_1V8,
-		       SD_IO_USING_3V3);
-	if (chip->asic_code)
-		rts51x_add_cmd(chip, WRITE_REG_CMD, LDO_POWER_CFG,
-			       TUNE_SD18_MASK, TUNE_SD18_3V3);
-	if (chip->asic_code)
-		sd_pull_ctl_disable(chip);
-	else
-		rts51x_add_cmd(chip, WRITE_REG_CMD, FPGA_PULL_CTL,
-			       FPGA_SD_PULL_CTL_BIT | 0x20,
-			       FPGA_SD_PULL_CTL_BIT);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_OE, SD_OUTPUT_EN, 0);
-	if (!chip->option.FT2_fast_mode)
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, POWER_MASK,
-			       POWER_OFF);
-
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	if (!chip->option.FT2_fast_mode) {
-#ifdef SD_XD_IO_FOLLOW_PWR
-		if (CHECK_PKG(chip, LQFP48)
-		    || chip->option.rts5129_D3318_off_enable)
-			rts51x_write_register(chip, CARD_PWR_CTL,
-					LDO_OFF, LDO_OFF);
-#endif
-		wait_timeout(250);
-
-#ifdef SD_XD_IO_FOLLOW_PWR
-		if (CHECK_PKG(chip, LQFP48)
-		    || chip->option.rts5129_D3318_off_enable) {
-			rts51x_init_cmd(chip);
-			if (chip->asic_code)
-				sd_pull_ctl_enable(chip);
-			else
-				rts51x_add_cmd(chip, WRITE_REG_CMD,
-					       FPGA_PULL_CTL,
-					       FPGA_SD_PULL_CTL_BIT | 0x20, 0);
-			retval = rts51x_send_cmd(chip, MODE_C, 100);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-		} else {
-			if (chip->asic_code)
-				rts51x_write_register(chip, CARD_PULL_CTL6,
-						      0x03, 0x00);
-		}
-#endif
-
-		/* Power on card */
-		retval = rts51x_card_power_on(chip, SD_CARD);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		wait_timeout(260);
-
-#ifdef SUPPORT_OCP
-		rts51x_get_card_status(chip, &(chip->card_status));
-		chip->ocp_stat = (chip->card_status >> 4) & 0x03;
-
-		if (chip->ocp_stat & (MS_OCP_NOW | MS_OCP_EVER)) {
-			RTS51X_DEBUGP("Over current, OCPSTAT is 0x%x\n",
-				       chip->ocp_stat);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-#endif
-	}
-
-	rts51x_init_cmd(chip);
-	if (chip->asic_code) {
-		sd_pull_ctl_enable(chip);
-	} else {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, FPGA_PULL_CTL,
-			       FPGA_SD_PULL_CTL_BIT | 0x20, 0);
-	}
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-#ifdef SD_XD_IO_FOLLOW_PWR
-	rts51x_write_register(chip, CARD_INT_PEND, XD_INT | MS_INT | SD_INT,
-			      XD_INT | MS_INT | SD_INT);
-#endif
-
-	RTS51X_WRITE_REG(chip, CARD_OE, SD_OUTPUT_EN, SD_OUTPUT_EN);
-
-	return STATUS_SUCCESS;
-}
-
-static int sd_dummy_clock(struct rts51x_chip *chip)
-{
-	RTS51X_WRITE_REG(chip, SD_BUS_STAT, SD_CLK_TOGGLE_EN, SD_CLK_TOGGLE_EN);
-	wait_timeout(5);
-	RTS51X_WRITE_REG(chip, SD_BUS_STAT, SD_CLK_TOGGLE_EN, 0x00);
-
-	return STATUS_SUCCESS;
-}
-
-int reset_sd(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval, i = 0, j = 0, k = 0, hi_cap_flow = 0;
-	int sd_dont_switch = 0;
-	int support_1v8 = 0;
-	u8 rsp[16];
-	u8 switch_bus_width;
-	u32 voltage = 0;
-	u8 cmd[5], buf[64];
-	u16 sd_card_type;
-
-	SET_SD(sd_card);
-	CLR_RETRY_SD20_MODE(sd_card);
-Switch_Fail:
-	i = 0;
-	j = 0;
-	k = 0;
-	hi_cap_flow = 0;
-	support_1v8 = 0;
-
-	retval = sd_prepare_reset(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	sd_dummy_clock(chip);
-
-	/* Start Initialization Process of SD Card */
-RTY_SD_RST:
-	retval =
-	    sd_send_cmd_get_rsp(chip, GO_IDLE_STATE, 0, SD_RSP_TYPE_R0, NULL,
-				0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	wait_timeout(20);
-
-	retval =
-	    sd_send_cmd_get_rsp(chip, SEND_IF_COND, 0x000001AA, SD_RSP_TYPE_R7,
-				rsp, 5);
-	if (retval == STATUS_SUCCESS) {
-		if ((rsp[4] == 0xAA) && ((rsp[3] & 0x0f) == 0x01)) {
-			hi_cap_flow = 1;
-			if (CHK_RETRY_SD20_MODE(sd_card)) {
-				voltage =
-				    SUPPORT_VOLTAGE |
-				    SUPPORT_HIGH_AND_EXTENDED_CAPACITY;
-			} else {
-				voltage =
-				    SUPPORT_VOLTAGE |
-				    SUPPORT_HIGH_AND_EXTENDED_CAPACITY |
-				    SUPPORT_MAX_POWER_PERMANCE | SUPPORT_1V8;
-			}
-		}
-	}
-
-	if (!hi_cap_flow) {
-		voltage = SUPPORT_VOLTAGE;
-
-		retval =
-		    sd_send_cmd_get_rsp(chip, GO_IDLE_STATE, 0, SD_RSP_TYPE_R0,
-					NULL, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-		wait_timeout(20);
-	}
-
-	/* ACMD41 */
-	do {
-		{
-			u8 temp = 0;
-			rts51x_read_register(chip, CARD_INT_PEND, &temp);
-			RTS51X_DEBUGP("CARD_INT_PEND:%x\n", temp);
-			if (temp & SD_INT) {
-				chip->reset_need_retry = 1;
-				rts51x_write_register(chip, CARD_INT_PEND,
-						      XD_INT | SD_INT | MS_INT,
-						      XD_INT | SD_INT | MS_INT);
-				sd_set_reset_fail(chip, SD_RESET_FAIL);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-
-RTY_CMD55:
-		retval =
-		    sd_send_cmd_get_rsp(chip, APP_CMD, 0, SD_RSP_TYPE_R1, NULL,
-					0);
-		if (retval != STATUS_SUCCESS) {
-			if (monitor_card_cd(chip, SD_CARD) == CD_NOT_EXIST) {
-				sd_set_reset_fail(chip, SD_RESET_FAIL);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-
-			j++;
-			if (chip->option.speed_mmc) {
-				if (j < 2)
-					goto RTY_CMD55;
-				else
-					TRACE_RET(chip, STATUS_FAIL);
-			} else {
-				if (j < 3)
-					goto RTY_SD_RST;
-				else
-					TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-
-		retval =
-		    sd_send_cmd_get_rsp(chip, SD_APP_OP_COND, voltage,
-					SD_RSP_TYPE_R3, rsp, 5);
-		if (retval != STATUS_SUCCESS) {
-			k++;
-			if (k < 3)
-				goto RTY_SD_RST;
-			else
-				TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		i++;
-		wait_timeout(20);
-	} while (!(rsp[1] & 0x80) && (i < 255)); /* Not complete power on */
-
-	if (i == 255) {
-		/* Time out */
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if (hi_cap_flow) {
-		if (rsp[1] & 0x40)
-			SET_SD_HCXC(sd_card);
-		else
-			CLR_SD_HCXC(sd_card);
-		if (!CHK_RETRY_SD20_MODE(sd_card)) {
-			if ((CHK_SD_HCXC(sd_card)) && (CHECK_UHS50(chip))) {
-				support_1v8 = (rsp[1] & 0x01) ? 1 : 0;
-				RTS51X_DEBUGP("support_1v8 = %d\n",
-					       support_1v8);
-			}
-		}
-	} else {
-		CLR_SD_HCXC(sd_card);
-		support_1v8 = 0;
-	}
-
-	/* CMD11: Switch Voltage */
-	if (support_1v8 && CHECK_UHS50(chip)
-	    && !(((u8) chip->option.sd_speed_prior & SDR104_SUPPORT) ==
-		 HS_SUPPORT)) {
-		retval = sd_voltage_switch(chip);
-		if (retval != STATUS_SUCCESS) {
-			SET_RETRY_SD20_MODE(sd_card);
-			sd_init_power(chip);
-			RTS51X_DEBUGP("1.8v switch fail\n");
-			goto Switch_Fail;
-		}
-	}
-
-	/* CMD 2 */
-	retval =
-	    sd_send_cmd_get_rsp(chip, ALL_SEND_CID, 0, SD_RSP_TYPE_R2, NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	/* CMD 3 */
-	retval =
-	    sd_send_cmd_get_rsp(chip, SEND_RELATIVE_ADDR, 0, SD_RSP_TYPE_R6,
-				rsp, 5);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	sd_card->sd_addr = (u32) rsp[1] << 24;
-	sd_card->sd_addr += (u32) rsp[2] << 16;
-
-	/* Get CSD register for Calculating Timing,Capacity,
-	 * Check CSD to determine as if this is the SD ROM card */
-	retval = sd_check_csd(chip, 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	/* Select SD card */
-	retval = rts51x_sd_select_card(chip, 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	/* ACMD42 */
-	retval =
-	    sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr, SD_RSP_TYPE_R1,
-				NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval =
-	    sd_send_cmd_get_rsp(chip, SET_CLR_CARD_DETECT, 0, SD_RSP_TYPE_R1,
-				NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (support_1v8) {
-		/* ACMD6 */
-		retval =
-		    sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
-					SD_RSP_TYPE_R1, NULL, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-		/* Enable 4 bit data bus */
-		retval =
-		    sd_send_cmd_get_rsp(chip, SET_BUS_WIDTH, 2, SD_RSP_TYPE_R1,
-					NULL, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-		switch_bus_width = SD_BUS_WIDTH_4;
-	} else {
-		switch_bus_width = SD_BUS_WIDTH_1;
-	}
-
-	/* Set block length 512 bytes for all block commands */
-	retval = sd_send_cmd_get_rsp(chip, SET_BLOCKLEN,
-			0x200, SD_RSP_TYPE_R1, NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	RTS51X_WRITE_REG(chip, SD_CFG1, SD_CLK_DIVIDE_MASK, SD_CLK_DIVIDE_0);
-
-	if (!(sd_card->raw_csd[4] & 0x40)) {
-		sd_dont_switch = 1;
-		RTS51X_DEBUGP("Not support class ten\n");
-	}
-
-	if (!sd_dont_switch) {
-		/* Check the card whether flow SD1.1 spec or higher */
-		retval = sd_check_spec(chip, switch_bus_width);
-		if (retval == STATUS_SUCCESS) {
-			retval = sd_switch_function(chip, switch_bus_width);
-			if (retval != STATUS_SUCCESS) {
-				if ((sd_card->sd_switch_fail ==
-				     SDR104_SUPPORT_MASK)
-				    || (sd_card->sd_switch_fail ==
-					DDR50_SUPPORT_MASK)
-				    || (sd_card->sd_switch_fail ==
-					    SDR50_SUPPORT_MASK)) {
-					sd_init_power(chip);
-					SET_RETRY_SD20_MODE(sd_card);
-				} else if (sd_card->sd_switch_fail ==
-						HS_SUPPORT_MASK) {
-					sd_dont_switch = 1;
-				}
-				goto Switch_Fail;
-			}
-		} else {
-			if (support_1v8) {
-				SET_RETRY_SD20_MODE(sd_card);
-				sd_init_power(chip);
-				sd_dont_switch = 1;
-
-				goto Switch_Fail;
-			}
-		}
-	}
-
-	if (!support_1v8) {
-		/* ACMD6 */
-		retval =
-		    sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
-					SD_RSP_TYPE_R1, NULL, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-		/* Enable 4 bit data bus */
-		retval =
-		    sd_send_cmd_get_rsp(chip, SET_BUS_WIDTH, 2, SD_RSP_TYPE_R1,
-					NULL, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	if (CHK_SD30_SPEED(sd_card)) {
-		rts51x_write_register(chip, SD30_DRIVE_SEL, SD30_DRIVE_MASK,
-				      0x03);
-
-		retval = sd_set_init_para(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		if (CHK_SD_DDR50(sd_card))
-			retval = sd_ddr_tuning(chip);
-		else
-			retval = sd_sdr_tuning(chip);
-
-		if (retval != STATUS_SUCCESS) {
-			SET_RETRY_SD20_MODE(sd_card);
-			RTS51X_DEBUGP("tuning phase fail,goto SD20 mode\n");
-			sd_init_power(chip);
-			CLR_SD30_SPEED(sd_card);
-			goto Switch_Fail;
-		}
-		if (STATUS_SUCCESS ==
-		    sd_wait_currentstate_dataready(chip, 0x08, 1, 20)) {
-			cmd[0] = 0x40 | READ_SINGLE_BLOCK;
-			cmd[1] = 0x00;
-			cmd[2] = 0x00;
-			cmd[3] = 0x00;
-			cmd[4] = 0x00;
-			retval =
-			    sd_read_data(chip, SD_TM_NORMAL_READ, cmd, 5, 512,
-					 1, SD_BUS_WIDTH_4, NULL, 0, 600);
-			if (retval != STATUS_SUCCESS) {
-				SET_RETRY_SD20_MODE(sd_card);
-				RTS51X_DEBUGP("read lba0 fail,"
-							"goto SD20 mode\n");
-				sd_init_power(chip);
-				CLR_SD30_SPEED(sd_card);
-				goto Switch_Fail;
-			}
-		}
-	}
-	sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr, SD_RSP_TYPE_R1,
-			    NULL, 0);
-
-	retval = sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
-			SD_RSP_TYPE_R1, NULL, 0);
-	if (retval == STATUS_SUCCESS) {
-		int ret;
-		cmd[0] = 0x40 | SEND_STATUS;
-		cmd[1] = 0x00;
-		cmd[2] = 0x00;
-		cmd[3] = 0x00;
-		cmd[4] = 0x00;
-		ret =
-		    sd_read_data(chip, SD_TM_NORMAL_READ, cmd, 5, 64, 1,
-				 SD_BUS_WIDTH_4, buf, 64, 600);
-		if (ret == STATUS_SUCCESS) {
-			sd_card_type = ((u16) buf[2] << 8) | (u16) buf[3];
-			RTS51X_DEBUGP("sd_card_type:0x%4x\n", sd_card_type);
-			if ((sd_card_type == 0x0001)
-			    || (sd_card_type == 0x0002))
-				chip->card_wp |= SD_CARD;
-		} else {
-			rts51x_clear_sd_error(chip);
-			sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
-					    SD_RSP_TYPE_R1, NULL, 0);
-		}
-	} else {
-		rts51x_clear_sd_error(chip);
-		sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
-				    SD_RSP_TYPE_R1, NULL, 0);
-	}
-
-	/* Check SD Machanical Write-Protect Switch */
-	retval = rts51x_get_card_status(chip, &(chip->card_status));
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	if (chip->card_status & SD_WP)
-		chip->card_wp |= SD_CARD;
-
-	chip->card_bus_width[chip->card2lun[SD_CARD]] = 4;
-
-	return STATUS_SUCCESS;
-}
-
-static int mmc_test_switch_bus(struct rts51x_chip *chip, u8 width)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	u8 buf[8] = { 0 }, bus_width;
-	u16 byte_cnt;
-	int len;
-
-	retval =
-	    sd_send_cmd_get_rsp(chip, BUSTEST_W, 0, SD_RSP_TYPE_R1, NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (width == MMC_8BIT_BUS) {
-		buf[0] = 0x55;
-		buf[1] = 0xAA;
-		len = 8;
-		byte_cnt = 8;
-		bus_width = SD_BUS_WIDTH_8;
-	} else {
-		buf[0] = 0x5A;
-		len = 4;
-		byte_cnt = 4;
-		bus_width = SD_BUS_WIDTH_4;
-	}
-
-	retval = sd_write_data(chip, SD_TM_AUTO_WRITE_3,
-			       NULL, 0, byte_cnt, 1, bus_width, buf, len, 100);
-	if (retval != STATUS_SUCCESS) {
-		u8 val1 = 0, val2 = 0;
-		rts51x_ep0_read_register(chip, SD_STAT1, &val1);
-		rts51x_ep0_read_register(chip, SD_STAT2, &val2);
-		rts51x_clear_sd_error(chip);
-		if ((val1 & 0xE0) || val2)
-			TRACE_RET(chip, STATUS_FAIL);
-	}
-	RTS51X_DEBUGP("SD/MMC CMD %d\n", BUSTEST_R);
-
-	rts51x_init_cmd(chip);
-
-	/* CMD14 */
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD0, 0xFF, 0x40 | BUSTEST_R);
-
-	if (width == MMC_8BIT_BUS)
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF, 0x08);
-	else
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF, 0x04);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_L, 0xFF, 1);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_H, 0xFF, 0);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF,
-		       SD_CALCULATE_CRC7 | SD_NO_CHECK_CRC16 |
-		       SD_NO_WAIT_BUSY_END | SD_CHECK_CRC7 | SD_RSP_LEN_6);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-		       PINGPONG_BUFFER);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
-		       SD_TM_NORMAL_READ | SD_TRANSFER_START);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER, SD_TRANSFER_END,
-		       SD_TRANSFER_END);
-
-	rts51x_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2, 0, 0);
-	if (width == MMC_8BIT_BUS) {
-		len = 3;
-		rts51x_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 1, 0, 0);
-	} else {
-		len = 2;
-	}
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, len, 100);
-	if (CHECK_SD_TRANS_FAIL(chip, retval)) {
-		rts51x_clear_sd_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	rts51x_read_rsp_buf(chip, 1, buf, 2);
-
-	if (width == MMC_8BIT_BUS) {
-		RTS51X_DEBUGP("BUSTEST_R [8bits]: 0x%02x 0x%02x\n",
-					buf[0], buf[1]);
-		if ((buf[0] == 0xAA) && (buf[1] == 0x55)) {
-			u8 rsp[5];
-			u32 arg;
-
-			if (CHK_MMC_DDR52(sd_card))
-				arg = 0x03B70600;
-			else
-				arg = 0x03B70200;
-			/* Switch MMC to  8-bit mode */
-			retval =
-			    sd_send_cmd_get_rsp(chip, SWITCH, arg,
-						SD_RSP_TYPE_R1b, rsp, 5);
-			if ((retval == STATUS_SUCCESS)
-			    && !(rsp[4] & MMC_SWITCH_ERR))
-				return STATUS_SUCCESS;
-		}
-	} else {
-		RTS51X_DEBUGP("BUSTEST_R [4bits]: 0x%02x\n", buf[0]);
-		if (buf[0] == 0xA5) {
-			u8 rsp[5];
-			u32 arg;
-
-			if (CHK_MMC_DDR52(sd_card))
-				arg = 0x03B70500;
-			else
-				arg = 0x03B70100;
-			/* Switch MMC to  4-bit mode */
-			retval =
-			    sd_send_cmd_get_rsp(chip, SWITCH, arg,
-						SD_RSP_TYPE_R1b, rsp, 5);
-			if ((retval == STATUS_SUCCESS)
-			    && !(rsp[4] & MMC_SWITCH_ERR))
-				return STATUS_SUCCESS;
-		}
-	}
-
-	TRACE_RET(chip, STATUS_FAIL);
-}
-
-static int mmc_switch_timing_bus(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	u8 card_type, card_type_mask = 0;
-	u8 buf[6];
-
-	CLR_MMC_HS(sd_card);
-
-	RTS51X_DEBUGP("SD/MMC CMD %d\n", SEND_EXT_CSD);
-
-	rts51x_init_cmd(chip);
-
-	/* SEND_EXT_CSD command */
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD0, 0xFF,
-			0x40 | SEND_EXT_CSD);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD1, 0xFF, 0);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD2, 0xFF, 0);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD3, 0xFF, 0);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD4, 0xFF, 0);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF, 0);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_H, 0xFF, 2);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_L, 0xFF, 1);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_H, 0xFF, 0);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF,
-		       SD_CALCULATE_CRC7 | SD_CHECK_CRC16 | SD_NO_WAIT_BUSY_END
-		       | SD_CHECK_CRC7 | SD_RSP_LEN_6);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-		       PINGPONG_BUFFER);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
-		       SD_TM_NORMAL_READ | SD_TRANSFER_START);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER, SD_TRANSFER_END,
-		       SD_TRANSFER_END);
-
-	rts51x_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 196, 0xFF, 0);
-	rts51x_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 212, 0xFF, 0);
-	rts51x_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 213, 0xFF, 0);
-	rts51x_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 214, 0xFF, 0);
-	rts51x_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 215, 0xFF, 0);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, 6, 1000);
-
-	if (CHECK_SD_TRANS_FAIL(chip, retval)) {
-		if (retval == STATUS_TIMEDOUT) {
-			rts51x_clear_sd_error(chip);
-			sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
-					    SD_RSP_TYPE_R1, NULL, 0);
-		}
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	rts51x_read_rsp_buf(chip, 0, buf, 6);
-
-	if (buf[0] & SD_TRANSFER_ERR) {
-		sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
-				    SD_RSP_TYPE_R1, NULL, 0);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-	if (CHK_MMC_SECTOR_MODE(sd_card))
-		sd_card->capacity =
-		    ((u32) buf[5] << 24) | ((u32) buf[4] << 16) |
-		    ((u32) buf[3] << 8) | ((u32) buf[2]);
-	if (CHECK_UHS50(chip))
-		card_type_mask = 0x07;
-	else
-		card_type_mask = 0x03;
-
-	card_type = buf[1] & card_type_mask;
-	if (card_type) {
-		/* CARD TYPE FIELD = DDR52MHz, 52MHz or 26MHz */
-		u8 rsp[5];
-
-		if (card_type & 0x04)
-			SET_MMC_DDR52(sd_card);
-		else if (card_type & 0x02)
-			SET_MMC_52M(sd_card);
-		else
-			SET_MMC_26M(sd_card);
-
-		retval =
-		    sd_send_cmd_get_rsp(chip, SWITCH, 0x03B90100,
-					SD_RSP_TYPE_R1b, rsp, 5);
-		if ((retval != STATUS_SUCCESS) || (rsp[4] & MMC_SWITCH_ERR))
-			CLR_MMC_HS(sd_card);
-	}
-	sd_choose_proper_clock(chip);
-	retval = switch_clock(chip, sd_card->sd_clock);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	/* Test Bus Procedure */
-	if (mmc_test_switch_bus(chip, MMC_8BIT_BUS) == STATUS_SUCCESS) {
-		SET_MMC_8BIT(sd_card);
-		chip->card_bus_width[chip->card2lun[SD_CARD]] = 8;
-	} else if (mmc_test_switch_bus(chip, MMC_4BIT_BUS) == STATUS_SUCCESS) {
-		SET_MMC_4BIT(sd_card);
-		chip->card_bus_width[chip->card2lun[SD_CARD]] = 4;
-	} else {
-		CLR_MMC_8BIT(sd_card);
-		CLR_MMC_4BIT(sd_card);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int reset_mmc(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval, i = 0, j = 0, k = 0;
-	u8 rsp[16];
-	u8 spec_ver = 0;
-	u8 change_to_ddr52 = 1;
-	u8 cmd[5];
-
-MMC_DDR_FAIL:
-
-	retval = sd_prepare_reset(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	SET_MMC(sd_card);
-
-RTY_MMC_RST:
-	retval =
-	    sd_send_cmd_get_rsp(chip, GO_IDLE_STATE, 0, SD_RSP_TYPE_R0, NULL,
-				0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	do {
-		{
-			u8 temp = 0;
-			rts51x_read_register(chip, CARD_INT_PEND, &temp);
-			if (temp & SD_INT) {
-				chip->reset_need_retry = 1;
-				rts51x_write_register(chip, CARD_INT_PEND,
-						      XD_INT | SD_INT | MS_INT,
-						      XD_INT | SD_INT | MS_INT);
-				sd_set_reset_fail(chip, MMC_RESET_FAIL);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-
-		/* CMD  1 */
-		retval = sd_send_cmd_get_rsp(chip, SEND_OP_COND,
-					     (SUPPORT_VOLTAGE | 0x40000000),
-					     SD_RSP_TYPE_R3, rsp, 5);
-		if (retval != STATUS_SUCCESS) {
-			if (monitor_card_cd(chip, SD_CARD) == CD_NOT_EXIST) {
-				sd_set_reset_fail(chip, MMC_RESET_FAIL);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-
-			if (sd_check_err_code(chip, SD_BUSY)
-			    || sd_check_err_code(chip, SD_TO_ERR)) {
-				k++;
-				if (k < 20) {
-					sd_clr_err_code(chip);
-					goto RTY_MMC_RST;
-				} else {
-					TRACE_RET(chip, STATUS_FAIL);
-				}
-			} else {
-				j++;
-				if (j < 100) {
-					sd_clr_err_code(chip);
-					goto RTY_MMC_RST;
-				} else {
-					TRACE_RET(chip, STATUS_FAIL);
-				}
-			}
-		}
-
-		wait_timeout(20);
-		i++;
-	} while (!(rsp[1] & 0x80) && (i < 100)); /* Not complete power on */
-
-	if (i == 100) {
-		/* Time out */
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if ((rsp[1] & 0x60) == 0x40)
-		SET_MMC_SECTOR_MODE(sd_card);
-	else
-		CLR_MMC_SECTOR_MODE(sd_card);
-
-	/* CMD 2 */
-	retval =
-	    sd_send_cmd_get_rsp(chip, ALL_SEND_CID, 0, SD_RSP_TYPE_R2, NULL, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	/* CMD 3 */
-	sd_card->sd_addr = 0x00100000;
-	retval =
-	    sd_send_cmd_get_rsp(chip, SET_RELATIVE_ADDR, sd_card->sd_addr,
-				SD_RSP_TYPE_R6, rsp, 5);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	/* Get CSD register for Calculating Timing,Capacity
-	 * Check CSD to determine as if this is the SD ROM card */
-	retval = sd_check_csd(chip, 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	/* Get MMC Spec_Ver in the CSD register */
-	spec_ver = (sd_card->raw_csd[0] & 0x3C) >> 2;
-
-	/* Select MMC card */
-	retval = rts51x_sd_select_card(chip, 1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	/* Set block length 512 bytes for all block commands */
-	retval =
-	    sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, 0x200, SD_RSP_TYPE_R1, NULL,
-				0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	RTS51X_WRITE_REG(chip, SD_CFG1, SD_CLK_DIVIDE_MASK, SD_CLK_DIVIDE_0);
-
-	if (chip->ic_version < 2)
-		rts51x_write_register(chip, SD30_DRIVE_SEL, SD30_DRIVE_MASK,
-				      0x02);
-	rts51x_write_register(chip, CARD_DRIVE_SEL, SD20_DRIVE_MASK, DRIVE_8mA);
-
-	chip->card_bus_width[chip->card2lun[SD_CARD]] = 1;
-	if (spec_ver == 4) {
-		/* MMC 4.x Cards */
-		(void)mmc_switch_timing_bus(chip);
-	}
-
-	if (CHK_MMC_SECTOR_MODE(sd_card) && (sd_card->capacity == 0))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	if (CHK_MMC_DDR52(sd_card) && change_to_ddr52) {
-		/* Card is extracted while identifying */
-		if (monitor_card_cd(chip, SD_CARD) == CD_NOT_EXIST)
-			TRACE_RET(chip, STATUS_FAIL);
-
-		retval = sd_set_init_para(chip);
-		if (retval != STATUS_SUCCESS) {
-			CLR_MMC_DDR52(sd_card);
-			sd_init_power(chip);
-			change_to_ddr52 = 0;
-			goto MMC_DDR_FAIL;
-		}
-
-		retval = mmc_ddr_tuning(chip);
-		if (retval != STATUS_SUCCESS) {
-			CLR_MMC_DDR52(sd_card);
-			sd_init_power(chip);
-			change_to_ddr52 = 0;
-			goto MMC_DDR_FAIL;
-		}
-
-		if (STATUS_SUCCESS ==
-		    sd_wait_currentstate_dataready(chip, 0x08, 1, 20)) {
-			cmd[0] = 0x40 | READ_SINGLE_BLOCK;
-			cmd[1] = 0x00;
-			cmd[2] = 0x00;
-			cmd[3] = 0x00;
-			cmd[4] = 0x00;
-			if (CHK_MMC_8BIT(sd_card)) {
-				retval =
-				    sd_read_data(chip, SD_TM_NORMAL_READ, cmd,
-						 5, 512, 1, SD_BUS_WIDTH_8,
-						 NULL, 0, 600);
-			} else if (CHK_MMC_4BIT(sd_card)) {
-				retval =
-				    sd_read_data(chip, SD_TM_NORMAL_READ, cmd,
-						 5, 512, 1, SD_BUS_WIDTH_4,
-						 NULL, 0, 600);
-			} else {
-				retval =
-				    sd_read_data(chip, SD_TM_NORMAL_READ, cmd,
-						 5, 512, 1, SD_BUS_WIDTH_1,
-						 NULL, 0, 600);
-			}
-
-			if (retval != STATUS_SUCCESS) {
-				CLR_MMC_DDR52(sd_card);
-				change_to_ddr52 = 0;
-				RTS51X_DEBUGP("read lba0 fail,"
-							"goto SD20 mode\n");
-				sd_init_power(chip);
-				goto MMC_DDR_FAIL;
-			}
-		}
-	}
-
-	retval = rts51x_get_card_status(chip, &(chip->card_status));
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	if (chip->card_status & SD_WP)
-		chip->card_wp |= SD_CARD;
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_reset_sd_card(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	int i;
-
-	memset(sd_card, 0, sizeof(struct sd_info));
-
-	/* Init variables */
-	sd_card->sd_type = 0;
-	sd_card->seq_mode = 0;
-	sd_card->sd_data_buf_ready = 0;
-	sd_card->capacity = 0;
-	sd_card->sd_switch_fail = 0;
-
-	sd_clear_reset_fail(chip);
-	rts51x_enable_card_clock(chip, SD_CARD);
-
-	sd_init_power(chip);
-
-	chip->reset_need_retry = 0;
-	for (i = 0; i < 3; i++) {
-		if (!chip->option.reset_mmc_first) { /* reset sd first */
-			retval = reset_sd(chip);
-			if (retval != STATUS_SUCCESS) {
-				/* Switch SD bus to 3V3 signal */
-				RTS51X_WRITE_REG(chip, SD_PAD_CTL,
-						 SD_IO_USING_1V8, 0);
-				if (sd_check_reset_fail(chip, SD_RESET_FAIL))
-					sd_clear_reset_fail(chip);
-				else
-					retval = reset_mmc(chip);
-			}
-		} else { /* reset MMC first */
-			retval = reset_mmc(chip);
-			if (retval != STATUS_SUCCESS) {
-				if (sd_check_reset_fail(chip, MMC_RESET_FAIL)) {
-					sd_clear_reset_fail(chip);
-				} else {
-					retval = reset_sd(chip);
-					if (retval != STATUS_SUCCESS) {
-						/* Switch SD bus to
-						 * 3V3 signal */
-						RTS51X_WRITE_REG(chip,
-							SD_PAD_CTL,
-							SD_IO_USING_1V8, 0);
-					}
-				}
-			}
-		}
-
-		if ((retval == STATUS_SUCCESS) || (!chip->reset_need_retry)) {
-			/* if reset success or don't need retry,then break */
-			break;
-		}
-		if (monitor_card_cd(chip, SD_CARD) == CD_NOT_EXIST) {
-			/* card is extracted */
-			break;
-		}
-		RTS51X_DEBUGP("retry reset sd card,%d\n", i);
-		chip->reset_need_retry = 0;
-	}
-
-	sd_clear_reset_fail(chip);
-	chip->reset_need_retry = 0;
-
-	if (retval == STATUS_SUCCESS) {
-		rts51x_init_cmd(chip);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG1, SD_CLK_DIVIDE_MASK,
-			       SD_CLK_DIVIDE_0);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF, 0);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_H, 0xFF, 2);
-		retval = rts51x_send_cmd(chip, MODE_C, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	} else {
-		chip->capacity[chip->card2lun[SD_CARD]] = sd_card->capacity = 0;
-		if (chip->option.reset_or_rw_fail_set_pad_drive) {
-			rts51x_write_register(chip, CARD_DRIVE_SEL,
-					      SD20_DRIVE_MASK, DRIVE_8mA);
-		}
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	chip->capacity[chip->card2lun[SD_CARD]] = sd_card->capacity;
-
-	if (chip->option.sd_send_status_en) {
-		sd_card->sd_send_status_en = 1;
-	} else {
-		if (sd_card->capacity > 0x20000) { /* 64MB */
-			sd_card->sd_send_status_en = 0;
-		} else {
-			sd_card->sd_send_status_en = 1;
-		}
-	}
-	RTS51X_DEBUGP("sd_card->sd_send_status = %d\n",
-		       sd_card->sd_send_status_en);
-
-	retval = sd_set_init_para(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	RTS51X_DEBUGP("sd_card->sd_type = 0x%x\n", sd_card->sd_type);
-
-	return STATUS_SUCCESS;
-}
-
-#define WAIT_DATA_READY_RTY_CNT		255
-
-static int wait_data_buf_ready(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int i, retval;
-
-	for (i = 0; i < WAIT_DATA_READY_RTY_CNT; i++) {
-		if (monitor_card_cd(chip, SD_CARD) == CD_NOT_EXIST)
-			TRACE_RET(chip, STATUS_FAIL);
-
-		sd_card->sd_data_buf_ready = 0;
-
-		retval = sd_send_cmd_get_rsp(chip, SEND_STATUS,
-					     sd_card->sd_addr, SD_RSP_TYPE_R1,
-					     NULL, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		if (sd_card->sd_data_buf_ready)
-			return sd_send_cmd_get_rsp(chip, SEND_STATUS,
-						   sd_card->sd_addr,
-						   SD_RSP_TYPE_R1, NULL, 0);
-	}
-
-	sd_set_err_code(chip, SD_TO_ERR);
-
-	TRACE_RET(chip, STATUS_FAIL);
-}
-
-static void sd_stop_seq_mode(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-
-	if (sd_card->seq_mode) {
-		retval = rts51x_sd_switch_clock(chip);
-		if (retval != STATUS_SUCCESS)
-			return;
-
-		retval = sd_send_cmd_get_rsp(chip, STOP_TRANSMISSION, 0,
-					     SD_RSP_TYPE_R1b, NULL, 0);
-		if (retval != STATUS_SUCCESS)
-			sd_set_err_code(chip, SD_STS_ERR);
-		sd_card->seq_mode = 0;
-
-		rts51x_ep0_write_register(chip, MC_FIFO_CTL, FIFO_FLUSH,
-					  FIFO_FLUSH);
-	}
-}
-
-static inline int sd_auto_tune_clock(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-
-	if (chip->asic_code) {
-		if (sd_card->sd_clock > 30)
-			sd_card->sd_clock -= 20;
-	} else {
-		if (sd_card->sd_clock == CLK_100)
-			sd_card->sd_clock = CLK_80;
-		else if (sd_card->sd_clock == CLK_80)
-			sd_card->sd_clock = CLK_60;
-		else if (sd_card->sd_clock == CLK_60)
-			sd_card->sd_clock = CLK_50;
-	}
-
-	retval = rts51x_sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_sd_rw(struct scsi_cmnd *srb, struct rts51x_chip *chip, u32 start_sector,
-	  u16 sector_cnt)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	u32 data_addr;
-	int retval;
-	u8 flag;
-	unsigned int pipe;
-	u8 stageflag;
-
-	sd_card->counter = 0;
-
-	if (!CHK_SD_HCXC(sd_card) && !CHK_MMC_SECTOR_MODE(sd_card))
-		data_addr = start_sector << 9;
-	else
-		data_addr = start_sector;
-
-	RTS51X_DEBUGP("rts51x_sd_rw, data_addr = 0x%x\n", data_addr);
-
-	sd_clr_err_code(chip);
-
-	retval = rts51x_sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (sd_card->seq_mode && ((sd_card->pre_dir != srb->sc_data_direction)
-				  ||
-				  ((sd_card->pre_sec_addr +
-				    sd_card->pre_sec_cnt) != start_sector))) {
-		if ((sd_card->pre_dir == DMA_FROM_DEVICE)
-		    && !CHK_SD30_SPEED(sd_card)
-		    && !CHK_SD_HS(sd_card)
-		    && !CHK_MMC_HS(sd_card)
-		    && sd_card->sd_send_status_en) {
-			sd_send_cmd_get_rsp(chip, SEND_STATUS,
-					    sd_card->sd_addr, SD_RSP_TYPE_R1,
-					    NULL, 0);
-		}
-
-		retval =
-		    sd_send_cmd_get_rsp(chip, STOP_TRANSMISSION, 0,
-					SD_RSP_TYPE_R1b, NULL, 0);
-		if (retval != STATUS_SUCCESS) {
-			sd_set_err_code(chip, SD_STS_ERR);
-			TRACE_RET(chip, sd_parse_err_code(chip));
-		}
-
-		sd_card->seq_mode = 0;
-
-		RTS51X_WRITE_REG(chip, MC_FIFO_CTL, FIFO_FLUSH, FIFO_FLUSH);
-
-		if (!CHK_SD30_SPEED(sd_card)
-		    && !CHK_SD_HS(sd_card)
-		    && !CHK_MMC_HS(sd_card)
-		    && sd_card->sd_send_status_en) {
-			/* random rw, so pre_sec_cnt < 0x80 */
-			sd_send_cmd_get_rsp(chip, SEND_STATUS,
-					    sd_card->sd_addr, SD_RSP_TYPE_R1,
-					    NULL, 0);
-		}
-	}
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF, 0x00);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_H, 0xFF, 0x02);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_L, 0xFF,
-		       (u8) sector_cnt);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_H, 0xFF,
-		       (u8) (sector_cnt >> 8));
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-		       RING_BUFFER);
-
-	if (CHK_MMC_8BIT(sd_card))
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG1, 0x03,
-			       SD_BUS_WIDTH_8);
-	else if (CHK_MMC_4BIT(sd_card) || CHK_SD(sd_card))
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG1, 0x03,
-			       SD_BUS_WIDTH_4);
-	else
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG1, 0x03,
-			       SD_BUS_WIDTH_1);
-
-	if (sd_card->seq_mode) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF,
-			       SD_NO_CALCULATE_CRC7 | SD_CHECK_CRC16 |
-			       SD_NO_WAIT_BUSY_END | SD_NO_CHECK_CRC7 |
-			       SD_RSP_LEN_0);
-
-		rts51x_trans_dma_enable(srb->sc_data_direction, chip, sector_cnt * 512,
-				 DMA_512);
-
-		if (srb->sc_data_direction == DMA_FROM_DEVICE) {
-			flag = MODE_CDIR;
-			rts51x_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
-				       SD_TM_AUTO_READ_3 | SD_TRANSFER_START);
-		} else {
-			flag = MODE_CDOR;
-			rts51x_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
-				       SD_TM_AUTO_WRITE_3 | SD_TRANSFER_START);
-		}
-
-		rts51x_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER,
-			       SD_TRANSFER_END, SD_TRANSFER_END);
-
-		retval = rts51x_send_cmd(chip, flag, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	} else {
-		if (srb->sc_data_direction == DMA_FROM_DEVICE) {
-			RTS51X_DEBUGP("SD/MMC CMD %d\n", READ_MULTIPLE_BLOCK);
-			rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD0, 0xFF,
-				       0x40 | READ_MULTIPLE_BLOCK);
-			rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD1, 0xFF,
-				       (u8) (data_addr >> 24));
-			rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD2, 0xFF,
-				       (u8) (data_addr >> 16));
-			rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD3, 0xFF,
-				       (u8) (data_addr >> 8));
-			rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD4, 0xFF,
-				       (u8) data_addr);
-
-			rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF,
-				       SD_CALCULATE_CRC7 | SD_CHECK_CRC16 |
-				       SD_NO_WAIT_BUSY_END | SD_CHECK_CRC7 |
-				       SD_RSP_LEN_6);
-
-			rts51x_trans_dma_enable(srb->sc_data_direction, chip,
-					 sector_cnt * 512, DMA_512);
-
-			rts51x_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
-				       SD_TM_AUTO_READ_2 | SD_TRANSFER_START);
-			rts51x_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER,
-				       SD_TRANSFER_END, SD_TRANSFER_END);
-
-			retval = rts51x_send_cmd(chip, MODE_CDIR, 100);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-		} else {
-			retval = rts51x_send_cmd(chip, MODE_C, 50);
-			if (retval != STATUS_SUCCESS) {
-				rts51x_clear_sd_error(chip);
-
-				sd_set_err_code(chip, SD_TO_ERR);
-				TRACE_RET(chip, sd_parse_err_code(chip));
-			}
-
-			retval = wait_data_buf_ready(chip);
-			if (retval != STATUS_SUCCESS) {
-				sd_set_err_code(chip, SD_TO_ERR);
-				TRACE_RET(chip, sd_parse_err_code(chip));
-			}
-
-			retval = sd_send_cmd_get_rsp(chip, WRITE_MULTIPLE_BLOCK,
-						     data_addr, SD_RSP_TYPE_R1,
-						     NULL, 0);
-			if (retval != STATUS_SUCCESS) {
-				sd_set_err_code(chip, SD_CRC_ERR);
-				TRACE_RET(chip, sd_parse_err_code(chip));
-			}
-
-			rts51x_init_cmd(chip);
-
-			rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF,
-				       SD_NO_CALCULATE_CRC7 | SD_CHECK_CRC16 |
-				       SD_NO_WAIT_BUSY_END | SD_NO_CHECK_CRC7 |
-				       SD_RSP_LEN_0);
-
-			rts51x_trans_dma_enable(srb->sc_data_direction, chip,
-					 sector_cnt * 512, DMA_512);
-
-			rts51x_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
-				       SD_TM_AUTO_WRITE_3 | SD_TRANSFER_START);
-			rts51x_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER,
-				       SD_TRANSFER_END, SD_TRANSFER_END);
-
-			retval = rts51x_send_cmd(chip, MODE_CDOR, 100);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-		}
-
-		sd_card->seq_mode = 1;
-	}
-
-	if (srb->sc_data_direction == DMA_FROM_DEVICE) {
-		pipe = RCV_BULK_PIPE(chip);
-		stageflag = STAGE_DI;
-	} else {
-		pipe = SND_BULK_PIPE(chip);
-		stageflag = STAGE_DO;
-	}
-
-	retval =
-	    rts51x_transfer_data_rcc(chip, pipe, scsi_sglist(srb),
-				     scsi_bufflen(srb), scsi_sg_count(srb),
-				     NULL, 10000, stageflag);
-	if (retval != STATUS_SUCCESS) {
-		u8 stat = 0;
-		int err = retval;
-
-		sd_print_debug_reg(chip);
-
-		rts51x_ep0_read_register(chip, SD_STAT1, &stat);
-		RTS51X_DEBUGP("SD_STAT1: 0x%x\n", stat);
-
-		rts51x_clear_sd_error(chip);
-
-		retval =
-		    sd_send_cmd_get_rsp(chip, STOP_TRANSMISSION, 0,
-					SD_RSP_TYPE_R1b, NULL, 0);
-		if (retval != STATUS_SUCCESS) {
-			sd_set_err_code(chip, SD_STS_ERR);
-			TRACE_RET(chip, retval);
-		}
-
-		if (stat & (SD_CRC7_ERR | SD_CRC16_ERR | SD_CRC_WRITE_ERR)) {
-			RTS51X_DEBUGP("SD CRC error, tune clock!\n");
-			sd_auto_tune_clock(chip);
-		}
-
-		sd_card->seq_mode = 0;
-
-		TRACE_RET(chip, err);
-	}
-	retval = rts51x_get_rsp(chip, 1, 2000);
-	if (CHECK_SD_TRANS_FAIL(chip, retval)) {
-		rts51x_clear_sd_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	sd_card->pre_sec_addr = start_sector;
-	sd_card->pre_sec_cnt = sector_cnt;
-	sd_card->pre_dir = srb->sc_data_direction;
-
-	return STATUS_SUCCESS;
-}
-
-void rts51x_sd_cleanup_work(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-
-	if (sd_card->seq_mode) {
-		RTS51X_DEBUGP("SD: stop transmission\n");
-		sd_stop_seq_mode(chip);
-		sd_card->counter = 0;
-	}
-}
-
-static inline void sd_fill_power_off_card3v3(struct rts51x_chip *chip)
-{
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_CLK_EN, SD_CLK_EN, 0);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_OE, SD_OUTPUT_EN, 0);
-	if (!chip->option.FT2_fast_mode) {
-#ifdef SD_XD_IO_FOLLOW_PWR
-		if (CHECK_PKG(chip, LQFP48)
-		    || chip->option.rts5129_D3318_off_enable)
-			rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL,
-				       POWER_MASK | LDO_OFF,
-				       POWER_OFF | LDO_OFF);
-		else
-			rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL,
-				       POWER_MASK, POWER_OFF);
-#else
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, POWER_MASK,
-			       POWER_OFF);
-#endif
-	}
-}
-
-static int sd_power_off_card3v3(struct rts51x_chip *chip)
-{
-	int retval;
-
-	rts51x_init_cmd(chip);
-
-	sd_fill_power_off_card3v3(chip);
-
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-#ifdef SD_XD_IO_FOLLOW_PWR
-	if (!chip->option.FT2_fast_mode)
-		wait_timeout(chip->option.D3318_off_delay);
-#endif
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_release_sd_card(struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-
-	RTS51X_DEBUGP("rts51x_release_sd_card\n");
-
-	chip->card_ready &= ~SD_CARD;
-	chip->card_fail &= ~SD_CARD;
-	chip->card_wp &= ~SD_CARD;
-
-	memset(sd_card->raw_csd, 0, 16);
-	memset(sd_card->raw_scr, 0, 8);
-
-	rts51x_write_register(chip, SFSM_ED, HW_CMD_STOP, HW_CMD_STOP);
-	rts51x_write_register(chip, SD_PAD_CTL, SD_IO_USING_1V8, 0);
-	if (CHECK_PKG(chip, LQFP48) || chip->option.rts5129_D3318_off_enable)
-		sd_power_off_card3v3(chip);
-
-	rts51x_init_cmd(chip);
-	if (!(CHECK_PKG(chip, LQFP48) || chip->option.rts5129_D3318_off_enable))
-		sd_fill_power_off_card3v3(chip);
-
-	if (chip->asic_code)
-		sd_pull_ctl_disable(chip);
-	else
-		rts51x_add_cmd(chip, WRITE_REG_CMD, FPGA_PULL_CTL,
-			       FPGA_SD_PULL_CTL_BIT | 0x20,
-			       FPGA_SD_PULL_CTL_BIT);
-
-	/* Switch LDO3318 to 3.3V */
-	rts51x_add_cmd(chip, WRITE_REG_CMD, LDO_POWER_CFG, TUNE_SD18_MASK,
-		       TUNE_SD18_3V3);
-
-	if (CHK_MMC_DDR52(sd_card) && CHK_MMC_8BIT(sd_card))
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DMA1_CTL,
-			       EXTEND_DMA1_ASYNC_SIGNAL,
-			       EXTEND_DMA1_ASYNC_SIGNAL);
-	if (CHK_SD30_SPEED(sd_card) || CHK_MMC(sd_card))
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD30_DRIVE_SEL,
-			       SD30_DRIVE_MASK, chip->option.sd30_pad_drive);
-	/* Suspend LDO3318 */
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, LDO3318_PWR_MASK,
-		       LDO_SUSPEND);
-
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	wait_timeout(20);
-
-	return STATUS_SUCCESS;
-}
diff --git a/drivers/staging/rts5139/sd.h b/drivers/staging/rts5139/sd.h
deleted file mode 100644
index 7dd943f54c7..00000000000
--- a/drivers/staging/rts5139/sd.h
+++ /dev/null
@@ -1,275 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- * Header file
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#ifndef __RTS51X_SD_H
-#define __RTS51X_SD_H
-
-#include "rts51x_chip.h"
-
-#define SD_MAX_RETRY_COUNT	3
-
-#define SUPPORT_VOLTAGE	0x003C0000
-
-#define SD_RESET_FAIL	0x01
-#define MMC_RESET_FAIL  0x02
-
-/* Error Code */
-#define	SD_NO_ERROR		0x0
-#define	SD_CRC_ERR		0x80
-#define	SD_TO_ERR		0x40
-#define	SD_NO_CARD		0x20
-#define SD_BUSY			0x10
-#define	SD_STS_ERR		0x08
-#define SD_RSP_TIMEOUT		0x04
-
-/* MMC/SD Command Index */
-/* Basic command (class 0) */
-#define GO_IDLE_STATE		0
-#define	SEND_OP_COND		1 /* reserved for SD */
-#define	ALL_SEND_CID		2
-#define	SET_RELATIVE_ADDR	3
-#define	SEND_RELATIVE_ADDR	3
-#define	SET_DSR			4
-#define IO_SEND_OP_COND		5
-#define	SWITCH			6
-#define	SELECT_CARD		7
-#define	DESELECT_CARD		7
-/* CMD8 is "SEND_EXT_CSD" for MMC4.x Spec
- * while is "SEND_IF_COND" for SD 2.0 */
-#define	SEND_EXT_CSD		8
-#define	SEND_IF_COND		8
-/* end  */
-#define	SEND_CSD		9
-#define	SEND_CID		10
-#define	VOLTAGE_SWITCH		11
-#define	READ_DAT_UTIL_STOP	11 /* reserved for SD */
-#define	STOP_TRANSMISSION	12
-#define	SEND_STATUS		13
-#define	GO_INACTIVE_STATE	15
-
-/* Block oriented read commands (class 2) */
-#define	SET_BLOCKLEN		16
-#define	READ_SINGLE_BLOCK	17
-#define	READ_MULTIPLE_BLOCK	18
-#define	SEND_TUNING_PATTERN	19
-
-/* Bus Width Test */
-#define	BUSTEST_R		14
-#define	BUSTEST_W		19
-/* end */
-
-/* Block oriented write commands (class 4) */
-#define	WRITE_BLOCK		24
-#define	WRITE_MULTIPLE_BLOCK	25
-#define	PROGRAM_CSD		27
-
-/* Erase commands */
-#define	ERASE_WR_BLK_START	32
-#define	ERASE_WR_BLK_END	33
-#define	ERASE_CMD		38
-
-/* Block Oriented Write Protection Commands */
-#define LOCK_UNLOCK		42
-
-#define	IO_RW_DIRECT		52
-
-/* Application specific commands (class 8) */
-#define	APP_CMD			55
-#define	GEN_CMD			56
-
-/* SD Application command Index */
-#define	SET_BUS_WIDTH			6
-#define	SD_STATUS			13
-#define	SEND_NUM_WR_BLOCKS		22
-#define	SET_WR_BLK_ERASE_COUNT		23
-#define	SD_APP_OP_COND			41
-#define	SET_CLR_CARD_DETECT		42
-#define	SEND_SCR			51
-
-/* SD TIMEOUT function return error */
-#define	SD_READ_COMPLETE	0x00
-#define	SD_READ_TO		0x01
-#define	SD_READ_ADVENCE		0x02
-
-/* SD v1.1 CMD6 SWITCH function */
-#define	SD_CHECK_MODE		0x00
-#define	SD_SWITCH_MODE		0x80
-#define	SD_FUNC_GROUP_1		0x01
-#define	SD_FUNC_GROUP_2		0x02
-#define	SD_FUNC_GROUP_3		0x03
-#define	SD_FUNC_GROUP_4		0x04
-#define	SD_CHECK_SPEC_V1_1	0xFF
-
-/* SD Command Argument */
-#define	NO_ARGUMENT	                        0x00
-#define	CHECK_PATTERN				0x000000AA
-#define	VOLTAGE_SUPPLY_RANGE			0x00000100 /* 2.7~3.6V */
-#define	SUPPORT_HIGH_AND_EXTENDED_CAPACITY	0x40000000
-#define	SUPPORT_MAX_POWER_PERMANCE	        0x10000000
-#define	SUPPORT_1V8	                        0x01000000
-
-/* Switch Command Error Code */
-#define	SWTICH_NO_ERR	  0x00
-#define	CARD_NOT_EXIST	  0x01
-#define	SPEC_NOT_SUPPORT  0x02
-#define	CHECK_MODE_ERR	  0x03
-#define	CHECK_NOT_READY	  0x04
-#define	SWITCH_CRC_ERR	  0x05
-#define	SWITCH_MODE_ERR	  0x06
-#define	SWITCH_PASS	  0x07
-
-/* Function Group Definition */
-/* Function Group 1 */
-#define	HS_SUPPORT			0x01
-#define	SDR50_SUPPORT			0x02
-#define	SDR104_SUPPORT			0x03
-#define	DDR50_SUPPORT			0x04
-#define	HS_SUPPORT_MASK			0x02
-#define	SDR50_SUPPORT_MASK		0x04
-#define	SDR104_SUPPORT_MASK		0x08
-#define	DDR50_SUPPORT_MASK		0x10
-#define	HS_QUERY_SWITCH_OK		0x01
-#define	SDR50_QUERY_SWITCH_OK		0x02
-#define	SDR104_QUERY_SWITCH_OK		0x03
-#define	DDR50_QUERY_SWITCH_OK		0x04
-#define	HS_SWITCH_BUSY			0x02
-#define	SDR50_SWITCH_BUSY		0x04
-#define	SDR104_SWITCH_BUSY		0x08
-#define	DDR50_SWITCH_BUSY		0x10
-#define	FUNCTION_GROUP1_SUPPORT_OFFSET       0x0D
-#define FUNCTION_GROUP1_QUERY_SWITCH_OFFSET  0x10
-#define FUNCTION_GROUP1_CHECK_BUSY_OFFSET    0x1D
-/* Function Group 3 */
-#define	DRIVING_TYPE_A	        0x01
-#define	DRIVING_TYPE_B		    0x00
-#define	DRIVING_TYPE_C		    0x02
-#define	DRIVING_TYPE_D	        0x03
-#define	DRIVING_TYPE_A_MASK	    0x02
-#define	DRIVING_TYPE_B_MASK	    0x01
-#define	DRIVING_TYPE_C_MASK	    0x04
-#define	DRIVING_TYPE_D_MASK	    0x08
-#define	TYPE_A_QUERY_SWITCH_OK	0x01
-#define	TYPE_B_QUERY_SWITCH_OK	0x00
-#define	TYPE_C_QUERY_SWITCH_OK  0x02
-#define	TYPE_D_QUERY_SWITCH_OK  0x03
-#define	TYPE_A_SWITCH_BUSY	    0x02
-#define	TYPE_B_SWITCH_BUSY	    0x01
-#define	TYPE_C_SWITCH_BUSY      0x04
-#define	TYPE_D_SWITCH_BUSY      0x08
-#define	FUNCTION_GROUP3_SUPPORT_OFFSET       0x09
-#define FUNCTION_GROUP3_QUERY_SWITCH_OFFSET  0x0F
-#define FUNCTION_GROUP3_CHECK_BUSY_OFFSET    0x19
-/* Function Group 4 */
-#define	CURRENT_LIMIT_200	    0x00
-#define	CURRENT_LIMIT_400	    0x01
-#define	CURRENT_LIMIT_600	    0x02
-#define	CURRENT_LIMIT_800	    0x03
-#define	CURRENT_LIMIT_200_MASK	0x01
-#define	CURRENT_LIMIT_400_MASK	0x02
-#define	CURRENT_LIMIT_600_MASK	0x04
-#define	CURRENT_LIMIT_800_MASK	0x08
-#define	CURRENT_LIMIT_200_QUERY_SWITCH_OK    0x00
-#define	CURRENT_LIMIT_400_QUERY_SWITCH_OK    0x01
-#define	CURRENT_LIMIT_600_QUERY_SWITCH_OK    0x02
-#define	CURRENT_LIMIT_800_QUERY_SWITCH_OK    0x03
-#define	CURRENT_LIMIT_200_SWITCH_BUSY        0x01
-#define	CURRENT_LIMIT_400_SWITCH_BUSY	     0x02
-#define	CURRENT_LIMIT_600_SWITCH_BUSY        0x04
-#define	CURRENT_LIMIT_800_SWITCH_BUSY        0x08
-#define	FUNCTION_GROUP4_SUPPORT_OFFSET       0x07
-#define FUNCTION_GROUP4_QUERY_SWITCH_OFFSET  0x0F
-#define FUNCTION_GROUP4_CHECK_BUSY_OFFSET    0x17
-/* Switch Function Status Offset */
-#define	DATA_STRUCTURE_VER_OFFSET   0x11 /* The high offset */
-#define MAX_PHASE		15
-/* #define      TOTAL_READ_PHASE    0x20 */
-/* #define      TOTAL_WRITE_PHASE    0x20 */
-/* MMC v4.0 */
-/* #define MMC_52MHZ_SPEED                       0x0001 */
-/* #define MMC_26MHZ_SPEED                       0x0002 */
-#define MMC_8BIT_BUS			0x0010
-#define MMC_4BIT_BUS			0x0020
-/* #define MMC_SECTOR_MODE                       0x0100 */
-#define MMC_SWITCH_ERR			0x80
-/* Tuning direction RX or TX */
-#define TUNE_TX    0x00
-#define TUNE_RX	   0x01
-/* For Change_DCM_FreqMode Function */
-#define CHANGE_TX  0x00
-#define CHANGE_RX  0x01
-#define DCM_HIGH_FREQUENCY_MODE  0x00
-#define DCM_LOW_FREQUENCY_MODE   0x01
-#define DCM_HIGH_FREQUENCY_MODE_SET  0x0C
-#define DCM_Low_FREQUENCY_MODE_SET   0x00
-/* For Change_FPGA_SSCClock Function */
-#define MULTIPLY_BY_1    0x00
-#define MULTIPLY_BY_2    0x01
-#define MULTIPLY_BY_3    0x02
-#define MULTIPLY_BY_4    0x03
-#define MULTIPLY_BY_5    0x04
-#define MULTIPLY_BY_6    0x05
-#define MULTIPLY_BY_7    0x06
-#define MULTIPLY_BY_8    0x07
-#define MULTIPLY_BY_9    0x08
-#define MULTIPLY_BY_10   0x09
-#define DIVIDE_BY_2      0x01
-#define DIVIDE_BY_3      0x02
-#define DIVIDE_BY_4      0x03
-#define DIVIDE_BY_5      0x04
-#define DIVIDE_BY_6      0x05
-#define DIVIDE_BY_7      0x06
-#define DIVIDE_BY_8      0x07
-#define DIVIDE_BY_9      0x08
-#define DIVIDE_BY_10     0x09
-#define CHECK_SD_TRANS_FAIL(chip, retval)	\
-	(((retval) != STATUS_SUCCESS) || \
-			(chip->rsp_buf[0] & SD_TRANSFER_ERR))
-/* SD Tuning Data Structure */
-/* Record continuous timing phase path */
-struct timing_phase_path {
-	int start;
-	int end;
-	int mid;
-	int len;
-};
-
-int rts51x_sd_select_card(struct rts51x_chip *chip, int select);
-int rts51x_reset_sd_card(struct rts51x_chip *chip);
-int rts51x_sd_switch_clock(struct rts51x_chip *chip);
-int rts51x_sd_rw(struct scsi_cmnd *srb, struct rts51x_chip *chip, u32 start_sector,
-	  u16 sector_cnt);
-void rts51x_sd_cleanup_work(struct rts51x_chip *chip);
-int rts51x_release_sd_card(struct rts51x_chip *chip);
-
-#ifdef SUPPORT_CPRM
-extern int reset_sd(struct rts51x_chip *chip);
-extern int sd_check_data0_status(struct rts51x_chip *chip);
-extern int sd_read_data(struct rts51x_chip *chip, u8 trans_mode, u8 *cmd,
-		int cmd_len, u16 byte_cnt, u16 blk_cnt, u8 bus_width,
-		u8 *buf, int buf_len, int timeout);
-#endif
-
-#endif /* __RTS51X_SD_H */
diff --git a/drivers/staging/rts5139/sd_cprm.c b/drivers/staging/rts5139/sd_cprm.c
deleted file mode 100644
index cede6c07394..00000000000
--- a/drivers/staging/rts5139/sd_cprm.c
+++ /dev/null
@@ -1,1056 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#include <linux/blkdev.h>
-#include <linux/kthread.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
-
-#include "debug.h"
-#include "trace.h"
-#include "rts51x.h"
-#include "rts51x_transport.h"
-#include "rts51x_scsi.h"
-#include "rts51x_card.h"
-#include "rts51x_chip.h"
-#include "sd_cprm.h"
-#include "sd.h"
-
-#ifdef SUPPORT_CPRM
-
-static inline int get_rsp_type(u8 rsp_code, u8 *rsp_type, int *rsp_len)
-{
-	if (!rsp_type || !rsp_len)
-		return STATUS_FAIL;
-
-	switch (rsp_code) {
-	case 0x03:
-		*rsp_type = SD_RSP_TYPE_R0; /* no response */
-		*rsp_len = 0;
-		break;
-
-	case 0x04:
-		*rsp_type = SD_RSP_TYPE_R1; /* R1,R6(,R4,R5) */
-		*rsp_len = 6;
-		break;
-
-	case 0x05:
-		*rsp_type = SD_RSP_TYPE_R1b;	/* R1b */
-		*rsp_len = 6;
-		break;
-
-	case 0x06:
-		*rsp_type = SD_RSP_TYPE_R2;	/* R2 */
-		*rsp_len = 17;
-		break;
-
-	case 0x07:
-		*rsp_type = SD_RSP_TYPE_R3;	/* R3 */
-		*rsp_len = 6;
-		break;
-
-	default:
-		return STATUS_FAIL;
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int ext_sd_send_cmd_get_rsp(struct rts51x_chip *chip, u8 cmd_idx,
-			    u32 arg, u8 rsp_type, u8 *rsp, int rsp_len,
-			    int special_check)
-{
-	int retval;
-	int timeout = 50;
-	u16 reg_addr;
-	u8 buf[17], stat;
-	int len = 2;
-	int rty_cnt = 0;
-
-	RTS51X_DEBUGP("EXT SD/MMC CMD %d\n", cmd_idx);
-
-	if (rsp_type == SD_RSP_TYPE_R1b)
-		timeout = 3000;
-
-RTY_SEND_CMD:
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD0, 0xFF, 0x40 | cmd_idx);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD1, 0xFF, (u8) (arg >> 24));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD2, 0xFF, (u8) (arg >> 16));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD3, 0xFF, (u8) (arg >> 8));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD4, 0xFF, (u8) arg);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF, rsp_type);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE,
-		       0x01, PINGPONG_BUFFER);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER,
-		       0xFF, SD_TM_CMD_RSP | SD_TRANSFER_START);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER, SD_TRANSFER_END,
-		       SD_TRANSFER_END);
-
-	rts51x_add_cmd(chip, READ_REG_CMD, SD_STAT1, 0, 0);
-
-	if (CHECK_USB(chip, USB_20)) {
-		if (rsp_type == SD_RSP_TYPE_R2) {
-			for (reg_addr = PPBUF_BASE2;
-			     reg_addr < PPBUF_BASE2 + 16; reg_addr++) {
-				rts51x_add_cmd(chip, READ_REG_CMD, reg_addr, 0,
-					       0);
-			}
-			len = 19;
-		} else if (rsp_type != SD_RSP_TYPE_R0) {
-			/* Read data from SD_CMDx registers */
-			for (reg_addr = SD_CMD0; reg_addr <= SD_CMD4;
-			     reg_addr++) {
-				rts51x_add_cmd(chip, READ_REG_CMD, reg_addr, 0,
-					       0);
-			}
-			len = 8;
-		} else {
-			len = 3;
-		}
-		rts51x_add_cmd(chip, READ_REG_CMD, SD_CMD5, 0, 0);
-	} else {
-		len = 2;
-	}
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, len, timeout);
-
-	if (CHECK_SD_TRANS_FAIL(chip, retval)) {
-		rts51x_clear_sd_error(chip);
-
-		if (retval == STATUS_TIMEDOUT) {
-			if (rsp_type & SD_WAIT_BUSY_END) {
-				retval = sd_check_data0_status(chip);
-				if (retval != STATUS_SUCCESS)
-					TRACE_RET(chip, retval);
-			}
-		}
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	if (rsp_type == SD_RSP_TYPE_R0)
-		return STATUS_SUCCESS;
-
-	if (CHECK_USB(chip, USB_20)) {
-		rts51x_read_rsp_buf(chip, 2, buf, len - 2);
-	} else {
-		if (rsp_type == SD_RSP_TYPE_R2) {
-			reg_addr = PPBUF_BASE2;
-			len = 16;
-		} else {
-			reg_addr = SD_CMD0;
-			len = 5;
-		}
-		retval =
-		    rts51x_seq_read_register(chip, reg_addr,
-						     (unsigned short)len, buf);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-		RTS51X_READ_REG(chip, SD_CMD5, buf + len);
-	}
-	stat = chip->rsp_buf[1];
-
-	if ((buf[0] & 0xC0) != 0)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	if (!(rsp_type & SD_NO_CHECK_CRC7)) {
-		if (stat & SD_CRC7_ERR) {
-			if (cmd_idx == WRITE_MULTIPLE_BLOCK)
-				TRACE_RET(chip, STATUS_FAIL);
-			if (rty_cnt < SD_MAX_RETRY_COUNT) {
-				wait_timeout(20);
-				rty_cnt++;
-				goto RTY_SEND_CMD;
-			} else {
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-	}
-
-	if ((cmd_idx == SELECT_CARD) || (cmd_idx == APP_CMD) ||
-	    (cmd_idx == SEND_STATUS) || (cmd_idx == STOP_TRANSMISSION)) {
-		if ((cmd_idx != STOP_TRANSMISSION) && (special_check == 0)) {
-			if (buf[1] & 0x80)
-				TRACE_RET(chip, STATUS_FAIL);
-		}
-		if (buf[1] & 0x7F)
-			TRACE_RET(chip, STATUS_FAIL);
-		if (buf[2] & 0xF8)
-			TRACE_RET(chip, STATUS_FAIL);
-
-		if (cmd_idx == SELECT_CARD) {
-			if (rsp_type == SD_RSP_TYPE_R2) {
-				if ((buf[3] & 0x1E) != 0x04)
-					TRACE_RET(chip, STATUS_FAIL);
-			} else if (rsp_type == SD_RSP_TYPE_R2) {
-				if ((buf[3] & 0x1E) != 0x03)
-					TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-	}
-
-	if (rsp && rsp_len)
-		memcpy(rsp, buf, rsp_len);
-
-	return STATUS_SUCCESS;
-}
-
-static int ext_sd_get_rsp(struct rts51x_chip *chip, int len,
-			u8 *rsp, u8 rsp_type)
-{
-	int retval, rsp_len;
-	u16 reg_addr;
-
-	if (rsp_type == SD_RSP_TYPE_R0)
-		return STATUS_SUCCESS;
-
-	rts51x_init_cmd(chip);
-
-	if (rsp_type == SD_RSP_TYPE_R2) {
-		for (reg_addr = PPBUF_BASE2; reg_addr < PPBUF_BASE2 + 16;
-		     reg_addr++) {
-			rts51x_add_cmd(chip, READ_REG_CMD, reg_addr, 0xFF, 0);
-		}
-		rsp_len = 17;
-	} else if (rsp_type != SD_RSP_TYPE_R0) {
-		for (reg_addr = SD_CMD0; reg_addr <= SD_CMD4; reg_addr++)
-			rts51x_add_cmd(chip, READ_REG_CMD, reg_addr, 0xFF, 0);
-		rsp_len = 6;
-	}
-	rts51x_add_cmd(chip, READ_REG_CMD, SD_CMD5, 0xFF, 0);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, rsp_len, 100);
-
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (rsp) {
-		int min_len = (rsp_len < len) ? rsp_len : len;
-
-		memcpy(rsp, rts51x_get_rsp_data(chip), min_len);
-
-		RTS51X_DEBUGP("min_len = %d\n", min_len);
-		RTS51X_DEBUGP("Response in cmd buf: 0x%x 0x%x 0x%x 0x%x\n",
-			       rsp[0], rsp[1], rsp[2], rsp[3]);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-int ext_rts51x_sd_execute_no_data(struct rts51x_chip *chip, unsigned int lun,
-			   u8 cmd_idx, u8 standby, u8 acmd, u8 rsp_code,
-			   u32 arg)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval, rsp_len;
-	u8 rsp_type;
-
-	retval = rts51x_sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, TRANSPORT_FAILED);
-
-	if (sd_card->pre_cmd_err) {
-		sd_card->pre_cmd_err = 0;
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-	retval = get_rsp_type(rsp_code, &rsp_type, &rsp_len);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-	sd_card->last_rsp_type = rsp_type;
-
-	retval = rts51x_sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	/* Set H/W SD/MMC Bus Width */
-	rts51x_write_register(chip, SD_CFG1, 0x03, SD_BUS_WIDTH_4);
-
-	if (standby) {
-		retval = rts51x_sd_select_card(chip, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Cmd_Failed);
-	}
-
-	if (acmd) {
-		retval =
-		    ext_sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
-					    SD_RSP_TYPE_R1, NULL, 0, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Cmd_Failed);
-	}
-
-	retval = ext_sd_send_cmd_get_rsp(chip, cmd_idx, arg, rsp_type,
-					 sd_card->rsp, rsp_len, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_GOTO(chip, SD_Execute_Cmd_Failed);
-
-	if (standby) {
-		retval = rts51x_sd_select_card(chip, 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Cmd_Failed);
-	}
-
-	return TRANSPORT_GOOD;
-
-SD_Execute_Cmd_Failed:
-	sd_card->pre_cmd_err = 1;
-	rts51x_set_sense_type(chip, lun, SENSE_TYPE_NO_SENSE);
-	rts51x_release_sd_card(chip);
-	rts51x_do_rts51x_reset_sd_card(chip);
-	if (!(chip->card_ready & SD_CARD))
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-
-	TRACE_RET(chip, TRANSPORT_FAILED);
-}
-
-int ext_rts51x_sd_execute_read_data(struct rts51x_chip *chip, unsigned int lun,
-			     u8 cmd_idx, u8 cmd12, u8 standby,
-			     u8 acmd, u8 rsp_code, u32 arg, u32 data_len,
-			     void *data_buf, unsigned int buf_len, int use_sg)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval, rsp_len, i;
-	int cmd13_checkbit = 0, read_err = 0;
-	u8 rsp_type, bus_width;
-
-	if (sd_card->pre_cmd_err) {
-		sd_card->pre_cmd_err = 0;
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	retval = rts51x_sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-	retval = get_rsp_type(rsp_code, &rsp_type, &rsp_len);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-	sd_card->last_rsp_type = rsp_type;
-
-	retval = rts51x_sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	bus_width = SD_BUS_WIDTH_4;
-
-	if (data_len < 512) {
-		retval = ext_sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, data_len,
-						 SD_RSP_TYPE_R1, NULL, 0, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-	}
-
-	if (standby) {
-		retval = rts51x_sd_select_card(chip, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-	}
-
-	if (acmd) {
-		retval =
-		    ext_sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
-					    SD_RSP_TYPE_R1, NULL, 0, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-	}
-
-	if (data_len <= 512) {
-		int min_len;
-		u8 *buf;
-		u16 byte_cnt, blk_cnt;
-		u8 cmd[5];
-		unsigned int offset = 0;
-		void *sg = NULL;
-
-		byte_cnt = (u16) (data_len & 0x3FF);
-		blk_cnt = 1;
-
-		cmd[0] = 0x40 | cmd_idx;
-		cmd[1] = (u8) (arg >> 24);
-		cmd[2] = (u8) (arg >> 16);
-		cmd[3] = (u8) (arg >> 8);
-		cmd[4] = (u8) arg;
-
-		buf = kmalloc(data_len, GFP_KERNEL);
-		if (buf == NULL)
-			TRACE_RET(chip, TRANSPORT_ERROR);
-
-		retval = sd_read_data(chip, SD_TM_NORMAL_READ, cmd, 5, byte_cnt,
-				      blk_cnt, bus_width, buf, data_len, 2000);
-		if (retval != STATUS_SUCCESS) {
-			read_err = 1;
-			kfree(buf);
-			rts51x_write_register(chip, CARD_STOP,
-					      SD_STOP | SD_CLR_ERR,
-					      SD_STOP | SD_CLR_ERR);
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-		}
-
-		min_len = min(data_len, buf_len);
-		if (use_sg)
-			rts51x_access_sglist(buf, min_len, (void *)data_buf,
-					     &sg, &offset, TO_XFER_BUF);
-		else
-			memcpy(data_buf, buf, min_len);
-
-		kfree(buf);
-	} else if (!(data_len & 0x1FF)) {
-		rts51x_init_cmd(chip);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_H, 0xFF, 0x02);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF, 0x00);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_H,
-			       0xFF, (u8) (data_len >> 17));
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_L,
-			       0xFF, (u8) ((data_len & 0x0001FE00) >> 9));
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD0, 0xFF,
-			       0x40 | cmd_idx);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD1, 0xFF,
-			       (u8) (arg >> 24));
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD2, 0xFF,
-			       (u8) (arg >> 16));
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD3, 0xFF,
-			       (u8) (arg >> 8));
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CMD4, 0xFF, (u8) arg);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG1, 0x03, bus_width);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_CFG2, 0xFF, rsp_type);
-		rts51x_trans_dma_enable(DMA_FROM_DEVICE, chip, data_len, DMA_512);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
-			       SD_TM_AUTO_READ_2 | SD_TRANSFER_START);
-		rts51x_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER,
-			       SD_TRANSFER_END, SD_TRANSFER_END);
-		retval = rts51x_send_cmd(chip, MODE_CDIR, 100);
-		if (retval != STATUS_SUCCESS) {
-			read_err = 1;
-			rts51x_ep0_write_register(chip, CARD_STOP,
-						  SD_STOP | SD_CLR_ERR,
-						  SD_STOP | SD_CLR_ERR);
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-		}
-
-		retval =
-		    rts51x_transfer_data_rcc(chip, RCV_BULK_PIPE(chip),
-					     data_buf, buf_len, use_sg, NULL,
-					     10000, STAGE_DI);
-		if (retval != STATUS_SUCCESS) {
-			read_err = 1;
-			rts51x_ep0_write_register(chip, CARD_STOP,
-						  SD_STOP | SD_CLR_ERR,
-						  SD_STOP | SD_CLR_ERR);
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-		}
-		retval = rts51x_get_rsp(chip, 1, 500);
-		if (CHECK_SD_TRANS_FAIL(chip, retval)) {
-			read_err = 1;
-			rts51x_ep0_write_register(chip, CARD_STOP,
-						  SD_STOP | SD_CLR_ERR,
-						  SD_STOP | SD_CLR_ERR);
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-		}
-	} else {
-		TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-	}
-
-	retval = ext_sd_get_rsp(chip, rsp_len, sd_card->rsp, rsp_type);
-	if (retval != STATUS_SUCCESS)
-		TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-
-	if (standby) {
-		retval = rts51x_sd_select_card(chip, 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-	}
-
-	if (cmd12) {
-		retval = ext_sd_send_cmd_get_rsp(chip, STOP_TRANSMISSION,
-						 0, SD_RSP_TYPE_R1b, NULL, 0,
-						 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-	}
-
-	if (data_len < 512) {
-		retval = ext_sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, 0x200,
-						 SD_RSP_TYPE_R1, NULL, 0, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-
-		rts51x_write_register(chip, SD_BYTE_CNT_H, 0xFF, 0x02);
-		rts51x_write_register(chip, SD_BYTE_CNT_L, 0xFF, 0x00);
-	}
-
-	if (standby || cmd12)
-		cmd13_checkbit = 1;
-
-	for (i = 0; i < 3; i++) {
-		retval =
-		    ext_sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
-					    SD_RSP_TYPE_R1, NULL, 0,
-					    cmd13_checkbit);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (retval != STATUS_SUCCESS)
-		TRACE_GOTO(chip, SD_Execute_Read_Cmd_Failed);
-
-	return TRANSPORT_GOOD;
-
-SD_Execute_Read_Cmd_Failed:
-	sd_card->pre_cmd_err = 1;
-	rts51x_set_sense_type(chip, lun, SENSE_TYPE_NO_SENSE);
-	if (read_err)
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-	rts51x_release_sd_card(chip);
-	rts51x_do_rts51x_reset_sd_card(chip);
-	if (!(chip->card_ready & SD_CARD))
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-
-	TRACE_RET(chip, TRANSPORT_FAILED);
-}
-
-int ext_rts51x_sd_execute_write_data(struct rts51x_chip *chip, unsigned int lun,
-			      u8 cmd_idx, u8 cmd12, u8 standby, u8 acmd,
-			      u8 rsp_code, u32 arg, u32 data_len,
-			      void *data_buf, unsigned int buf_len, int use_sg)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval, rsp_len;
-	int cmd13_checkbit = 0, write_err = 0;
-	u8 rsp_type;
-	u32 i;
-
-	if (sd_card->pre_cmd_err) {
-		sd_card->pre_cmd_err = 0;
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	retval = rts51x_sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	retval = get_rsp_type(rsp_code, &rsp_type, &rsp_len);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-	sd_card->last_rsp_type = rsp_type;
-
-	retval = rts51x_sd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	rts51x_write_register(chip, SD_CFG1, 0x03, SD_BUS_WIDTH_4);
-
-	if (data_len < 512) {
-		retval = ext_sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, data_len,
-						 SD_RSP_TYPE_R1, NULL, 0, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-	}
-
-	if (standby) {
-		retval = rts51x_sd_select_card(chip, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-	}
-
-	if (acmd) {
-		retval =
-		    ext_sd_send_cmd_get_rsp(chip, APP_CMD, sd_card->sd_addr,
-					    SD_RSP_TYPE_R1, NULL, 0, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-	}
-
-	retval = ext_sd_send_cmd_get_rsp(chip, cmd_idx, arg, rsp_type,
-					 sd_card->rsp, rsp_len, 0);
-	if (retval != STATUS_SUCCESS)
-		TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-
-	if (data_len <= 512) {
-		u8 *buf;
-		unsigned int offset = 0;
-		void *sg = NULL;
-
-		buf = kmalloc(data_len, GFP_KERNEL);
-		if (buf == NULL)
-			TRACE_RET(chip, TRANSPORT_ERROR);
-
-		if (use_sg)
-			rts51x_access_sglist(buf, data_len, (void *)data_buf,
-					     &sg, &offset, FROM_XFER_BUF);
-		else
-			memcpy(buf, data_buf, data_len);
-
-
-		if (data_len > 256) {
-			rts51x_init_cmd(chip);
-			for (i = 0; i < 256; i++) {
-				rts51x_add_cmd(chip, WRITE_REG_CMD,
-					       (u16) (PPBUF_BASE2 + i), 0xFF,
-					       buf[i]);
-			}
-			retval = rts51x_send_cmd(chip, MODE_C, 250);
-			if (retval != STATUS_SUCCESS) {
-				kfree(buf);
-				TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-			}
-
-			rts51x_init_cmd(chip);
-			for (i = 256; i < data_len; i++) {
-				rts51x_add_cmd(chip, WRITE_REG_CMD,
-					       (u16) (PPBUF_BASE2 + i), 0xFF,
-					       buf[i]);
-			}
-			retval = rts51x_send_cmd(chip, MODE_C, 250);
-			if (retval != STATUS_SUCCESS) {
-				kfree(buf);
-				TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-			}
-		} else {
-			rts51x_init_cmd(chip);
-			for (i = 0; i < data_len; i++) {
-				rts51x_add_cmd(chip, WRITE_REG_CMD,
-					       (u16) (PPBUF_BASE2 + i), 0xFF,
-					       buf[i]);
-			}
-			retval = rts51x_send_cmd(chip, MODE_C, 250);
-			if (retval != STATUS_SUCCESS) {
-				kfree(buf);
-				TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-			}
-		}
-
-		kfree(buf);
-
-		rts51x_init_cmd(chip);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_H, 0xFF,
-			       (u8) ((data_len >> 8) & 0x03));
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF,
-			       (u8) data_len);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_H, 0xFF, 0x00);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_L, 0xFF, 0x01);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-			       PINGPONG_BUFFER);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
-			       SD_TM_AUTO_WRITE_3 | SD_TRANSFER_START);
-		rts51x_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER,
-			       SD_TRANSFER_END, SD_TRANSFER_END);
-
-		retval = rts51x_send_cmd(chip, MODE_CR, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-
-		retval = rts51x_get_rsp(chip, 1, 250);
-		if (CHECK_SD_TRANS_FAIL(chip, retval))
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-	} else if (!(data_len & 0x1FF)) {
-		rts51x_init_cmd(chip);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_H, 0xFF, 0x02);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BYTE_CNT_L, 0xFF, 0x00);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_H,
-			       0xFF, (u8) (data_len >> 17));
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_BLOCK_CNT_L,
-			       0xFF, (u8) ((data_len & 0x0001FE00) >> 9));
-
-		rts51x_trans_dma_enable(DMA_TO_DEVICE, chip, data_len, DMA_512);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, SD_TRANSFER, 0xFF,
-			       SD_TM_AUTO_WRITE_3 | SD_TRANSFER_START);
-		rts51x_add_cmd(chip, CHECK_REG_CMD, SD_TRANSFER,
-			       SD_TRANSFER_END, SD_TRANSFER_END);
-
-		retval = rts51x_send_cmd(chip, MODE_CDOR, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-
-		retval =
-		    rts51x_transfer_data_rcc(chip, SND_BULK_PIPE(chip),
-					     data_buf, buf_len, use_sg, NULL,
-					     10000, STAGE_DO);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-
-		retval = rts51x_get_rsp(chip, 1, 10000);
-		if (CHECK_SD_TRANS_FAIL(chip, retval))
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-
-	} else {
-		TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-	}
-
-	if (retval < 0) {
-		write_err = 1;
-		rts51x_write_register(chip, CARD_STOP, SD_STOP | SD_CLR_ERR,
-				      SD_STOP | SD_CLR_ERR);
-		TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-	}
-
-	if (standby) {
-		retval = rts51x_sd_select_card(chip, 1);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-	}
-
-	if (cmd12) {
-		retval = ext_sd_send_cmd_get_rsp(chip, STOP_TRANSMISSION,
-						 0, SD_RSP_TYPE_R1b, NULL, 0,
-						 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-	}
-
-	if (data_len < 512) {
-		retval = ext_sd_send_cmd_get_rsp(chip, SET_BLOCKLEN, 0x200,
-						 SD_RSP_TYPE_R1, NULL, 0, 0);
-		if (retval != STATUS_SUCCESS)
-			TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-
-		rts51x_write_register(chip, SD_BYTE_CNT_H, 0xFF, 0x02);
-		rts51x_write_register(chip, SD_BYTE_CNT_L, 0xFF, 0x00);
-	}
-
-	if (cmd12 || standby) {
-		/* There is CMD7 or CMD12 sent before CMD13 */
-		cmd13_checkbit = 1;
-	}
-
-	for (i = 0; i < 3; i++) {
-		retval =
-		    ext_sd_send_cmd_get_rsp(chip, SEND_STATUS, sd_card->sd_addr,
-					    SD_RSP_TYPE_R1, NULL, 0,
-					    cmd13_checkbit);
-		if (retval == STATUS_SUCCESS)
-			break;
-	}
-	if (retval != STATUS_SUCCESS)
-		TRACE_GOTO(chip, SD_Execute_Write_Cmd_Failed);
-
-	return TRANSPORT_GOOD;
-
-SD_Execute_Write_Cmd_Failed:
-	sd_card->pre_cmd_err = 1;
-	rts51x_set_sense_type(chip, lun, SENSE_TYPE_NO_SENSE);
-	if (write_err)
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_WRITE_ERR);
-	rts51x_release_sd_card(chip);
-	rts51x_do_rts51x_reset_sd_card(chip);
-	if (!(chip->card_ready & SD_CARD))
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-
-	TRACE_RET(chip, TRANSPORT_FAILED);
-}
-
-int rts51x_sd_pass_thru_mode(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	unsigned int lun = SCSI_LUN(srb);
-	int len;
-	u8 buf[18] = {
-		0x00,
-		0x00,
-		0x00,
-		0x0E,
-		0x00,		/* Version Number */
-		0x00,		/* WP | Media Type */
-		0x00,		/* RCA (Low byte) */
-		0x00,		/* RCA (High byte) */
-		0x53,		/* 'S' */
-		0x44,		/* 'D' */
-		0x20,		/* ' ' */
-		0x43,		/* 'C' */
-		0x61,		/* 'a' */
-		0x72,		/* 'r' */
-		0x64,		/* 'd' */
-		0x00,		/* Max LUN Number */
-		0x00,
-		0x00,
-	};
-
-	sd_card->pre_cmd_err = 0;
-
-	if (!(CHK_BIT(chip->lun_mc, lun))) {
-		SET_BIT(chip->lun_mc, lun);
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if ((0x53 != srb->cmnd[2]) || (0x44 != srb->cmnd[3])
-	    || (0x20 != srb->cmnd[4]) || (0x43 != srb->cmnd[5])
-	    || (0x61 != srb->cmnd[6]) || (0x72 != srb->cmnd[7])
-	    || (0x64 != srb->cmnd[8])) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	switch (srb->cmnd[1] & 0x0F) {
-	case 0:
-		sd_card->sd_pass_thru_en = 0;
-		break;
-
-	case 1:
-		sd_card->sd_pass_thru_en = 1;
-		break;
-
-	default:
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	/* 0x01:SD Memory Card; 0x02:Other Media; 0x03:Illegal Media; */
-	buf[5] = (1 == CHK_SD(sd_card)) ? 0x01 : 0x02;
-	if (chip->card_wp & SD_CARD)
-		buf[5] |= 0x80;
-
-	buf[6] = (u8) (sd_card->sd_addr >> 16);
-	buf[7] = (u8) (sd_card->sd_addr >> 24);
-
-	buf[15] = chip->max_lun;
-
-	len = min_t(unsigned, 18, scsi_bufflen(srb));
-	rts51x_set_xfer_buf(buf, len, srb);
-
-	return TRANSPORT_GOOD;
-}
-
-int rts51x_sd_execute_no_data(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	unsigned int lun = SCSI_LUN(srb);
-	int retval;
-	u8 cmd_idx, rsp_code;
-	u8 standby = 0, acmd = 0;
-	u32 arg;
-
-	if (!sd_card->sd_pass_thru_en) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	cmd_idx = srb->cmnd[2] & 0x3F;
-	if (srb->cmnd[1] & 0x02)
-		standby = 1;
-	if (srb->cmnd[1] & 0x01)
-		acmd = 1;
-
-	arg = ((u32) srb->cmnd[3] << 24) | ((u32) srb->cmnd[4] << 16) |
-	    ((u32) srb->cmnd[5] << 8) | srb->cmnd[6];
-
-	rsp_code = srb->cmnd[10];
-
-	retval =
-	    ext_rts51x_sd_execute_no_data(chip, lun, cmd_idx, standby, acmd, rsp_code,
-				   arg);
-	scsi_set_resid(srb, 0);
-	return retval;
-}
-
-int rts51x_sd_execute_read_data(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	unsigned int lun = SCSI_LUN(srb);
-	u8 cmd_idx, rsp_code, send_cmd12 = 0, standby = 0, acmd = 0;
-	u32 arg, data_len;
-
-	if (!sd_card->sd_pass_thru_en) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	cmd_idx = srb->cmnd[2] & 0x3F;
-	if (srb->cmnd[1] & 0x04)
-		send_cmd12 = 1;
-	if (srb->cmnd[1] & 0x02)
-		standby = 1;
-	if (srb->cmnd[1] & 0x01)
-		acmd = 1;
-
-	arg = ((u32) srb->cmnd[3] << 24) | ((u32) srb->cmnd[4] << 16) |
-	    ((u32) srb->cmnd[5] << 8) | srb->cmnd[6];
-
-	data_len =
-	    ((u32) srb->cmnd[7] << 16) | ((u32) srb->cmnd[8] << 8) |
-	    srb->cmnd[9];
-	rsp_code = srb->cmnd[10];
-
-	retval =
-	    ext_rts51x_sd_execute_read_data(chip, lun, cmd_idx, send_cmd12, standby,
-				     acmd, rsp_code, arg, data_len,
-				     scsi_sglist(srb), scsi_bufflen(srb),
-				     scsi_sg_count(srb));
-	scsi_set_resid(srb, 0);
-	return retval;
-}
-
-int rts51x_sd_execute_write_data(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	int retval;
-	unsigned int lun = SCSI_LUN(srb);
-	u8 cmd_idx, rsp_code, send_cmd12 = 0, standby = 0, acmd = 0;
-	u32 data_len, arg;
-
-	if (!sd_card->sd_pass_thru_en) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	cmd_idx = srb->cmnd[2] & 0x3F;
-	if (srb->cmnd[1] & 0x04)
-		send_cmd12 = 1;
-	if (srb->cmnd[1] & 0x02)
-		standby = 1;
-	if (srb->cmnd[1] & 0x01)
-		acmd = 1;
-
-	data_len =
-	    ((u32) srb->cmnd[7] << 16) | ((u32) srb->cmnd[8] << 8) |
-	    srb->cmnd[9];
-	arg =
-	    ((u32) srb->cmnd[3] << 24) | ((u32) srb->cmnd[4] << 16) |
-	    ((u32) srb->cmnd[5] << 8) | srb->cmnd[6];
-	rsp_code = srb->cmnd[10];
-
-	retval =
-	    ext_rts51x_sd_execute_write_data(chip, lun, cmd_idx, send_cmd12, standby,
-				      acmd, rsp_code, arg, data_len,
-				      scsi_sglist(srb), scsi_bufflen(srb),
-				      scsi_sg_count(srb));
-	scsi_set_resid(srb, 0);
-	return retval;
-}
-
-int rts51x_sd_get_cmd_rsp(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	unsigned int lun = SCSI_LUN(srb);
-	int count;
-	u16 data_len;
-
-	if (!sd_card->sd_pass_thru_en) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (sd_card->pre_cmd_err) {
-		sd_card->pre_cmd_err = 0;
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	data_len = ((u16) srb->cmnd[7] << 8) | srb->cmnd[8];
-
-	if (sd_card->last_rsp_type == SD_RSP_TYPE_R0) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	} else if (sd_card->last_rsp_type == SD_RSP_TYPE_R2) {
-		count = (data_len < 17) ? data_len : 17;
-	} else {
-		count = (data_len < 6) ? data_len : 6;
-	}
-	rts51x_set_xfer_buf(sd_card->rsp, count, srb);
-
-	RTS51X_DEBUGP("Response length: %d\n", data_len);
-	RTS51X_DEBUGP("Response: 0x%x 0x%x 0x%x 0x%x\n",
-		       sd_card->rsp[0], sd_card->rsp[1], sd_card->rsp[2],
-		       sd_card->rsp[3]);
-
-	scsi_set_resid(srb, 0);
-	return TRANSPORT_GOOD;
-}
-
-int rts51x_sd_hw_rst(struct scsi_cmnd *srb, struct rts51x_chip *chip)
-{
-	struct sd_info *sd_card = &(chip->sd_card);
-	unsigned int lun = SCSI_LUN(srb);
-	int retval;
-
-	if (!sd_card->sd_pass_thru_en) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if (sd_card->pre_cmd_err) {
-		sd_card->pre_cmd_err = 0;
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	if ((0x53 != srb->cmnd[2]) || (0x44 != srb->cmnd[3])
-	    || (0x20 != srb->cmnd[4]) || (0x43 != srb->cmnd[5])
-	    || (0x61 != srb->cmnd[6]) || (0x72 != srb->cmnd[7])
-	    || (0x64 != srb->cmnd[8])) {
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	switch (srb->cmnd[1] & 0x0F) {
-	case 0:
-		/* SD Card Power Off -> ON and Initialization */
-		retval = rts51x_reset_sd_card(chip);
-		if (retval != STATUS_SUCCESS) {
-			rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-			sd_card->pre_cmd_err = 1;
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-		break;
-
-	case 1:
-		/* reset CMD(CMD0) and Initialization
-		 * (without SD Card Power Off -> ON) */
-		retval = reset_sd(chip);
-		if (retval != STATUS_SUCCESS) {
-			rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-			sd_card->pre_cmd_err = 1;
-			TRACE_RET(chip, TRANSPORT_FAILED);
-		}
-		break;
-
-	default:
-		rts51x_set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
-		TRACE_RET(chip, TRANSPORT_FAILED);
-	}
-
-	scsi_set_resid(srb, 0);
-	return TRANSPORT_GOOD;
-}
-#endif
diff --git a/drivers/staging/rts5139/sd_cprm.h b/drivers/staging/rts5139/sd_cprm.h
deleted file mode 100644
index 79dfd27db41..00000000000
--- a/drivers/staging/rts5139/sd_cprm.h
+++ /dev/null
@@ -1,54 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- * Header file
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#ifndef __RTS51X_SD_CPRM_H
-#define __RTS51X_SD_CPRM_H
-
-#include "rts51x_chip.h"
-#include "sd.h"
-
-#ifdef SUPPORT_CPRM
-int ext_rts51x_sd_execute_no_data(struct rts51x_chip *chip, unsigned int lun,
-			   u8 cmd_idx, u8 standby, u8 acmd, u8 rsp_code,
-			   u32 arg);
-int ext_rts51x_sd_execute_read_data(struct rts51x_chip *chip, unsigned int lun,
-			     u8 cmd_idx, u8 cmd12, u8 standby, u8 acmd,
-			     u8 rsp_code, u32 arg, u32 data_len, void *data_buf,
-			     unsigned int buf_len, int use_sg);
-int ext_rts51x_sd_execute_write_data(struct rts51x_chip *chip, unsigned int lun,
-			      u8 cmd_idx, u8 cmd12, u8 standby, u8 acmd,
-			      u8 rsp_code, u32 arg, u32 data_len,
-			      void *data_buf, unsigned int buf_len, int use_sg);
-
-int rts51x_sd_pass_thru_mode(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-int rts51x_sd_execute_no_data(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-int rts51x_sd_execute_read_data(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-int rts51x_sd_execute_write_data(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-int rts51x_sd_get_cmd_rsp(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-int rts51x_sd_hw_rst(struct scsi_cmnd *srb, struct rts51x_chip *chip);
-#endif
-
-#endif /* __RTS51X_SD_CPRM_H */
diff --git a/drivers/staging/rts5139/trace.h b/drivers/staging/rts5139/trace.h
deleted file mode 100644
index ac58b452ecb..00000000000
--- a/drivers/staging/rts5139/trace.h
+++ /dev/null
@@ -1,95 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- * Header file
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#ifndef __RTS51X_TRACE_H
-#define __RTS51X_TRACE_H
-
-#include <linux/string.h>
-
-#include "debug.h"
-
-#define _MSG_TRACE
-
-#ifdef _MSG_TRACE
-#define TRACE_RET(chip, ret)						\
-do {									\
-	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,		\
-			__func__, MSG_FUNC_LEN-1);			\
-	strncpy((chip)->trace_msg[(chip)->msg_idx].file,		\
-			_file, MSG_FILE_LEN-1);				\
-	get_current_time((chip)->trace_msg[(chip)->msg_idx].timeval_buf,\
-			TIME_VAL_LEN);	\
-	(chip)->trace_msg[(chip)->msg_idx].valid = 1;			\
-	(chip)->msg_idx++;						\
-	if ((chip)->msg_idx >= TRACE_ITEM_CNT) {			\
-		(chip)->msg_idx = 0;					\
-	}								\
-	return ret;							\
-} while (0)
-
-#define TRACE_GOTO(chip, label)						\
-do {									\
-	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,		\
-			__func__, MSG_FUNC_LEN-1);			\
-	strncpy((chip)->trace_msg[(chip)->msg_idx].file,		\
-			_file, MSG_FILE_LEN-1);				\
-	get_current_time((chip)->trace_msg[(chip)->msg_idx].timeval_buf,\
-			TIME_VAL_LEN);					\
-	(chip)->trace_msg[(chip)->msg_idx].valid = 1;			\
-	(chip)->msg_idx++;						\
-	if ((chip)->msg_idx >= TRACE_ITEM_CNT) {			\
-		(chip)->msg_idx = 0;					\
-	}								\
-	goto label;							\
-} while (0)
-#else
-#define TRACE_RET(chip, ret)	return (ret)
-#define TRACE_GOTO(chip, label)	goto label
-#endif
-
-#ifdef CONFIG_RTS5139_DEBUG
-#define RTS51X_DUMP(buf, buf_len)					\
-	print_hex_dump(KERN_DEBUG, RTS51X_TIP, DUMP_PREFIX_NONE,	\
-				16, 1, (buf), (buf_len), false)
-
-#define CATCH_TRIGGER(chip)					\
-do {								\
-	rts51x_ep0_write_register((chip), 0xFC31, 0x01, 0x01);	\
-	RTS51X_DEBUGP("Catch trigger!\n");			\
-} while (0)
-
-#else
-#define RTS51X_DUMP(buf, buf_len)
-#define CATCH_TRIGGER(chip)
-#endif
-
-#endif /* __RTS51X_TRACE_H */
diff --git a/drivers/staging/rts5139/xd.c b/drivers/staging/rts5139/xd.c
deleted file mode 100644
index be432351be8..00000000000
--- a/drivers/staging/rts5139/xd.c
+++ /dev/null
@@ -1,2145 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#include <linux/blkdev.h>
-#include <linux/kthread.h>
-#include <linux/sched.h>
-#include <linux/vmalloc.h>
-
-#include "debug.h"
-#include "trace.h"
-#include "rts51x.h"
-#include "rts51x_transport.h"
-#include "rts51x_scsi.h"
-#include "rts51x_card.h"
-#include "xd.h"
-
-static int xd_build_l2p_tbl(struct rts51x_chip *chip, int zone_no);
-static int xd_init_page(struct rts51x_chip *chip, u32 phy_blk, u16 logoff,
-			u8 start_page, u8 end_page);
-
-static inline void xd_set_err_code(struct rts51x_chip *chip, u8 err_code)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-
-	xd_card->err_code = err_code;
-}
-
-static int xd_set_init_para(struct rts51x_chip *chip)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	int retval;
-
-	if (chip->asic_code)
-		xd_card->xd_clock = 47;
-	else
-		xd_card->xd_clock = CLK_50;
-
-	retval = switch_clock(chip, xd_card->xd_clock);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-static int xd_switch_clock(struct rts51x_chip *chip)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	int retval;
-
-	retval = rts51x_select_card(chip, XD_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = switch_clock(chip, xd_card->xd_clock);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-static int xd_read_id(struct rts51x_chip *chip, u8 id_cmd, u8 *id_buf,
-		      u8 buf_len)
-{
-	int retval, i;
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_DAT, 0xFF, id_cmd);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
-		       XD_TRANSFER_START | XD_READ_ID);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER, XD_TRANSFER_END,
-		       XD_TRANSFER_END);
-
-	for (i = 0; i < 4; i++) {
-		rts51x_add_cmd(chip, READ_REG_CMD, (u16) (XD_ADDRESS1 + i), 0,
-			       0);
-	}
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 20);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, 5, 20);
-
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_xd_error(chip);
-		TRACE_RET(chip, retval);
-	}
-
-	if (id_buf && buf_len) {
-		if (buf_len > 4)
-			buf_len = 4;
-		rts51x_read_rsp_buf(chip, 1, id_buf, buf_len);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static void xd_assign_phy_addr(struct rts51x_chip *chip, u32 addr, u8 mode)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-
-	switch (mode) {
-	case XD_RW_ADDR:
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_ADDRESS0, 0xFF, 0);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_ADDRESS1, 0xFF,
-			       (u8) addr);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_ADDRESS2, 0xFF,
-			       (u8) (addr >> 8));
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_ADDRESS3, 0xFF,
-			       (u8) (addr >> 16));
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CFG, 0xFF,
-			       xd_card->addr_cycle | XD_CALC_ECC |
-			       XD_BA_NO_TRANSFORM);
-		break;
-
-	case XD_ERASE_ADDR:
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_ADDRESS0, 0xFF,
-			       (u8) addr);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_ADDRESS1, 0xFF,
-			       (u8) (addr >> 8));
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_ADDRESS2, 0xFF,
-			       (u8) (addr >> 16));
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CFG, 0xFF,
-			       (xd_card->addr_cycle - 1) |
-			       XD_CALC_ECC | XD_BA_NO_TRANSFORM);
-		break;
-
-	default:
-		break;
-	}
-}
-
-static int xd_read_redundant(struct rts51x_chip *chip, u32 page_addr, u8 *buf,
-			     int buf_len)
-{
-	int retval, i;
-
-	rts51x_init_cmd(chip);
-
-	xd_assign_phy_addr(chip, page_addr, XD_RW_ADDR);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
-		       XD_TRANSFER_START | XD_READ_REDUNDANT);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER, XD_TRANSFER_END,
-		       XD_TRANSFER_END);
-
-	for (i = 0; i < 6; i++) {
-		rts51x_add_cmd(chip, READ_REG_CMD, (u16) (XD_PAGE_STATUS + i),
-			       0, 0);
-	}
-	for (i = 0; i < 4; i++) {
-		rts51x_add_cmd(chip, READ_REG_CMD, (u16) (XD_RESERVED0 + i), 0,
-			       0);
-	}
-	rts51x_add_cmd(chip, READ_REG_CMD, XD_PARITY, 0, 0);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, 11, 500);
-
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_xd_error(chip);
-		TRACE_RET(chip, retval);
-	}
-
-	if (buf && buf_len) {
-		if (buf_len > 11)
-			buf_len = 11;
-		rts51x_read_rsp_buf(chip, 1, buf, buf_len);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int xd_read_data_from_ppb(struct rts51x_chip *chip, int offset, u8 *buf,
-				 int buf_len)
-{
-	int retval, i;
-
-	if (!buf || (buf_len <= 0))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	rts51x_init_cmd(chip);
-
-	for (i = 0; i < buf_len; i++) {
-		rts51x_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + offset + i, 0,
-			       0);
-	}
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, buf_len, 200);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	rts51x_read_rsp_buf(chip, 0, buf, buf_len);
-
-	return STATUS_SUCCESS;
-}
-
-static int xd_read_cis(struct rts51x_chip *chip, u32 page_addr, u8 *buf,
-		       int buf_len)
-{
-	int retval;
-	u8 reg;
-
-	if (!buf || (buf_len < 10))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	rts51x_init_cmd(chip);
-
-	xd_assign_phy_addr(chip, page_addr, XD_RW_ADDR);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-		       PINGPONG_BUFFER);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_CNT, 0xFF, 1);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CHK_DATA_STATUS,
-		       XD_AUTO_CHK_DATA_STATUS, XD_AUTO_CHK_DATA_STATUS);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
-		       XD_TRANSFER_START | XD_READ_PAGES);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER,
-		       XD_TRANSFER_END | XD_PPB_EMPTY,
-		       XD_TRANSFER_END | XD_PPB_EMPTY);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, 1, 500);
-	if (retval == STATUS_TIMEDOUT) {
-		rts51x_clear_xd_error(chip);
-		TRACE_RET(chip, retval);
-	}
-
-	RTS51X_READ_REG(chip, XD_PAGE_STATUS, &reg);
-	if (reg != XD_GPG) {
-		rts51x_clear_xd_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	RTS51X_READ_REG(chip, XD_CTL, &reg);
-
-	if (!(reg & XD_ECC1_ERROR) || !(reg & XD_ECC1_UNCORRECTABLE)) {
-		retval = xd_read_data_from_ppb(chip, 0, buf, buf_len);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-		if (reg & XD_ECC1_ERROR) {	/* correctable error */
-			u8 ecc_bit, ecc_byte;
-
-			RTS51X_READ_REG(chip, XD_ECC_BIT1, &ecc_bit);
-			RTS51X_READ_REG(chip, XD_ECC_BYTE1, &ecc_byte);
-
-			RTS51X_DEBUGP("ECC_BIT1 = 0x%x, ECC_BYTE1 = 0x%x\n",
-				       ecc_bit, ecc_byte);
-			if (ecc_byte < buf_len) {
-				RTS51X_DEBUGP("Before correct: 0x%x\n",
-					       buf[ecc_byte]);
-				buf[ecc_byte] ^= (1 << ecc_bit);
-				RTS51X_DEBUGP("After correct: 0x%x\n",
-					       buf[ecc_byte]);
-			}
-		}
-	} else if (!(reg & XD_ECC2_ERROR) || !(reg & XD_ECC2_UNCORRECTABLE)) {
-		RTS51X_WRITE_REG(chip, CARD_STOP, XD_STOP | XD_CLR_ERR,
-				 XD_STOP | XD_CLR_ERR);
-
-		retval = xd_read_data_from_ppb(chip, 256, buf, buf_len);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-		if (reg & XD_ECC2_ERROR) {
-			u8 ecc_bit, ecc_byte;
-
-			RTS51X_READ_REG(chip, XD_ECC_BIT2, &ecc_bit);
-			RTS51X_READ_REG(chip, XD_ECC_BYTE2, &ecc_byte);
-
-			RTS51X_DEBUGP("ECC_BIT2 = 0x%x, ECC_BYTE2 = 0x%x\n",
-				       ecc_bit, ecc_byte);
-			if (ecc_byte < buf_len) {
-				RTS51X_DEBUGP("Before correct: 0x%x\n",
-					       buf[ecc_byte]);
-				buf[ecc_byte] ^= (1 << ecc_bit);
-				RTS51X_DEBUGP("After correct: 0x%x\n",
-					       buf[ecc_byte]);
-			}
-		}
-	} else {
-		rts51x_clear_xd_error(chip);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static void xd_pull_ctl_disable(struct rts51x_chip *chip)
-{
-	if (CHECK_PKG(chip, LQFP48)) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0xA5);
-	} else {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0x65);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x56);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0x59);
-	}
-}
-
-static void xd_pull_ctl_enable(struct rts51x_chip *chip)
-{
-	if (CHECK_PKG(chip, LQFP48)) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0xAA);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0xA5);
-	} else {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF, 0xA5);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF, 0x59);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF, 0x95);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF, 0x55);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF, 0x59);
-	}
-}
-
-static int reset_xd(struct rts51x_chip *chip)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	int retval, i, j;
-	u8 id_buf[4], redunt[11];
-
-	retval = rts51x_select_card(chip, XD_CARD);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CHK_DATA_STATUS, 0xFF,
-		       XD_PGSTS_NOT_FF);
-	if (chip->asic_code)
-		xd_pull_ctl_disable(chip);
-	else
-		rts51x_add_cmd(chip, WRITE_REG_CMD, FPGA_PULL_CTL, 0xFF,
-			       (FPGA_XD_PULL_CTL_EN1 & FPGA_XD_PULL_CTL_EN3));
-
-	if (!chip->option.FT2_fast_mode) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_INIT, XD_NO_AUTO_PWR_OFF,
-			       0);
-		if (CHECK_PKG(chip, LQFP48) ||
-				chip->option.rts5129_D3318_off_enable) {
-			rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL,
-				       DV3318_AUTO_PWR_OFF,
-				       DV3318_AUTO_PWR_OFF);
-		}
-	}
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_OE, XD_OUTPUT_EN, 0);
-	if (!chip->option.FT2_fast_mode) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, POWER_MASK,
-			       POWER_OFF);
-	}
-
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	if (!chip->option.FT2_fast_mode) {
-#ifdef SD_XD_IO_FOLLOW_PWR
-		if (CHECK_PKG(chip, LQFP48)
-		    || chip->option.rts5129_D3318_off_enable) {
-			rts51x_write_register(chip, CARD_PWR_CTL,
-					LDO_OFF, LDO_OFF);
-		}
-#endif
-
-		wait_timeout(250);
-
-#ifdef SD_XD_IO_FOLLOW_PWR
-		if (CHECK_PKG(chip, LQFP48)
-		    || chip->option.rts5129_D3318_off_enable) {
-			rts51x_init_cmd(chip);
-			if (chip->asic_code) {
-				xd_pull_ctl_enable(chip);
-			} else {
-				rts51x_add_cmd(chip, WRITE_REG_CMD,
-					FPGA_PULL_CTL, 0xFF,
-					(FPGA_XD_PULL_CTL_EN1 &
-						FPGA_XD_PULL_CTL_EN2));
-			}
-			retval = rts51x_send_cmd(chip, MODE_C, 100);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, STATUS_FAIL);
-		}
-#endif
-
-		retval = rts51x_card_power_on(chip, XD_CARD);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-#ifdef SUPPORT_OCP
-		wait_timeout(50);
-		rts51x_get_card_status(chip, &(chip->card_status));
-		chip->ocp_stat = (chip->card_status >> 4) & 0x03;
-
-		if (chip->ocp_stat & (MS_OCP_NOW | MS_OCP_EVER)) {
-			RTS51X_DEBUGP("Over current, OCPSTAT is 0x%x\n",
-				       chip->ocp_stat);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-#endif
-	}
-
-	rts51x_init_cmd(chip);
-
-	if (chip->asic_code)
-		xd_pull_ctl_enable(chip);
-	else
-		rts51x_add_cmd(chip, WRITE_REG_CMD, FPGA_PULL_CTL, 0xFF,
-			       (FPGA_XD_PULL_CTL_EN1 & FPGA_XD_PULL_CTL_EN2));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_OE, XD_OUTPUT_EN,
-		       XD_OUTPUT_EN);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CTL, XD_CE_DISEN, XD_CE_DISEN);
-
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	if (!chip->option.FT2_fast_mode)
-		wait_timeout(200);
-
-	retval = xd_set_init_para(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-	/* Read ID to check if the timing setting is right */
-	for (i = 0; i < 4; i++) {
-		u8 xd_dat, xd_ctl;
-
-		if (monitor_card_cd(chip, XD_CARD) == CD_NOT_EXIST)
-			TRACE_RET(chip, STATUS_FAIL);
-
-		rts51x_init_cmd(chip);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_DTCTL, 0xFF,
-			XD_TIME_SETUP_STEP * 3 + XD_TIME_RW_STEP *
-			(2 + i + chip->option.rts51x_xd_rw_step)
-			+ XD_TIME_RWN_STEP *
-			(i + chip->option.rts51x_xd_rwn_step));
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CATCTL, 0xFF,
-			XD_TIME_SETUP_STEP * 3 + XD_TIME_RW_STEP * (4 +
-			i) + XD_TIME_RWN_STEP * (3 + i));
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
-			       XD_TRANSFER_START | XD_RESET);
-		rts51x_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER,
-			       XD_TRANSFER_END, XD_TRANSFER_END);
-
-		rts51x_add_cmd(chip, READ_REG_CMD, XD_DAT, 0, 0);
-		rts51x_add_cmd(chip, READ_REG_CMD, XD_CTL, 0, 0);
-
-		retval = rts51x_send_cmd(chip, MODE_CR, 100);
-		if (retval != STATUS_SUCCESS) {
-			rts51x_clear_xd_error(chip);
-			TRACE_RET(chip, retval);
-		}
-
-		retval = rts51x_get_rsp(chip, 3, 100);
-		if (retval != STATUS_SUCCESS) {
-			rts51x_clear_xd_error(chip);
-			TRACE_RET(chip, retval);
-		}
-
-		xd_dat = chip->rsp_buf[1];
-		xd_ctl = chip->rsp_buf[2];
-		RTS51X_DEBUGP("XD_DAT: 0x%x, XD_CTL: 0x%x\n", xd_dat, xd_ctl);
-
-		if (((xd_dat & READY_FLAG) != READY_STATE)
-		    || !(xd_ctl & XD_RDY))
-			continue;
-
-		retval = xd_read_id(chip, READ_ID, id_buf, 4);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		RTS51X_DEBUGP("READ_ID: 0x%x 0x%x 0x%x 0x%x\n",
-			       id_buf[0], id_buf[1], id_buf[2], id_buf[3]);
-
-		xd_card->device_code = id_buf[1];
-
-		switch (xd_card->device_code) {
-		case XD_4M_X8_512_1:
-		case XD_4M_X8_512_2:
-			xd_card->block_shift = 4;	/* 16 pages per block */
-			xd_card->page_off = 0x0F;
-			xd_card->addr_cycle = 3;
-			xd_card->zone_cnt = 1;
-			xd_card->capacity = 8000;	/* 500 * 2 ^ 4 */
-			XD_SET_4MB(xd_card);
-			break;
-		case XD_8M_X8_512:
-			xd_card->block_shift = 4;
-			xd_card->page_off = 0x0F;
-			xd_card->addr_cycle = 3;
-			xd_card->zone_cnt = 1;
-			xd_card->capacity = 16000;	/* 1000 * 2 ^ 4 */
-			break;
-		case XD_16M_X8_512:
-			XD_PAGE_512(xd_card);	/* 32 pages per block */
-			xd_card->addr_cycle = 3;
-			xd_card->zone_cnt = 1;
-			xd_card->capacity = 32000;	/* 1000 * 2 ^ 5 */
-			break;
-		case XD_32M_X8_512:
-			XD_PAGE_512(xd_card);
-			xd_card->addr_cycle = 3;
-			xd_card->zone_cnt = 2;
-			xd_card->capacity = 64000;	/* 2000 * 2 ^ 5 */
-			break;
-		case XD_64M_X8_512:
-			XD_PAGE_512(xd_card);
-			xd_card->addr_cycle = 4;
-			xd_card->zone_cnt = 4;
-			xd_card->capacity = 128000;	/* 4000 * 2 ^ 5 */
-			break;
-		case XD_128M_X8_512:
-			XD_PAGE_512(xd_card);
-			xd_card->addr_cycle = 4;
-			xd_card->zone_cnt = 8;
-			xd_card->capacity = 256000;	/* 8000 * 2 ^ 5 */
-			break;
-		case XD_256M_X8_512:
-			XD_PAGE_512(xd_card);
-			xd_card->addr_cycle = 4;
-			xd_card->zone_cnt = 16;
-			xd_card->capacity = 512000;	/* 16000 * 2 ^ 5 */
-			break;
-		case XD_512M_X8:
-			XD_PAGE_512(xd_card);
-			xd_card->addr_cycle = 4;
-			xd_card->zone_cnt = 32;
-			xd_card->capacity = 1024000;	/* 32000 * 2 ^ 5 */
-			break;
-		case xD_1G_X8_512:
-			XD_PAGE_512(xd_card);
-			xd_card->addr_cycle = 4;
-			xd_card->zone_cnt = 64;
-			xd_card->capacity = 2048000;	/* 64000 * 2 ^ 5 */
-			break;
-		case xD_2G_X8_512:
-			XD_PAGE_512(xd_card);
-			xd_card->addr_cycle = 4;
-			xd_card->zone_cnt = 128;
-			xd_card->capacity = 4096000;	/* 128000 * 2 ^ 5 */
-			break;
-		default:
-			continue;
-		}
-
-		/* Confirm timing setting */
-		for (j = 0; j < 10; j++) {
-			retval = xd_read_id(chip, READ_ID, id_buf, 4);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-
-			if (id_buf[1] != xd_card->device_code)
-				break;
-		}
-
-		/* Current timing pass */
-		if (j == 10)
-			break;
-	}
-
-	if (i == 4) {
-		xd_card->block_shift = 0;
-		xd_card->page_off = 0;
-		xd_card->addr_cycle = 0;
-		xd_card->capacity = 0;
-
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	retval = xd_read_id(chip, READ_xD_ID, id_buf, 4);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-	RTS51X_DEBUGP("READ_xD_ID: 0x%x 0x%x 0x%x 0x%x\n",
-		       id_buf[0], id_buf[1], id_buf[2], id_buf[3]);
-	if (id_buf[2] != XD_ID_CODE)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	/* Search CIS block */
-	for (i = 0; i < 24; i++) {
-		u32 page_addr;
-
-		if (monitor_card_cd(chip, XD_CARD) == CD_NOT_EXIST)
-			TRACE_RET(chip, STATUS_FAIL);
-
-		page_addr = (u32) i << xd_card->block_shift;
-
-		for (j = 0; j < 3; j++) {
-			retval = xd_read_redundant(chip, page_addr, redunt, 11);
-			if (retval == STATUS_SUCCESS)
-				break;
-		}
-		if (j == 3)
-			continue;
-
-		if (redunt[BLOCK_STATUS] != XD_GBLK)
-			continue;
-
-		j = 0;
-		/* Check page status */
-		if (redunt[PAGE_STATUS] != XD_GPG) {
-			for (j = 1; j <= 8; j++) {
-				retval =
-				    xd_read_redundant(chip, page_addr + j,
-						      redunt, 11);
-				if (retval == STATUS_SUCCESS) {
-					if (redunt[PAGE_STATUS] == XD_GPG)
-						break;
-				}
-			}
-
-			if (j == 9)
-				break;
-		}
-
-		if ((redunt[BLOCK_STATUS] == XD_GBLK)
-		    && (redunt[PARITY] & XD_BA1_ALL0)) {
-			u8 buf[10];
-
-			page_addr += j;
-
-			retval = xd_read_cis(chip, page_addr, buf, 10);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-
-			if ((buf[0] == 0x01) && (buf[1] == 0x03)
-			    && (buf[2] == 0xD9)
-			    && (buf[3] == 0x01) && (buf[4] == 0xFF)
-			    && (buf[5] == 0x18) && (buf[6] == 0x02)
-			    && (buf[7] == 0xDF) && (buf[8] == 0x01)
-			    && (buf[9] == 0x20)) {
-				xd_card->cis_block = (u16) i;
-			}
-		}
-
-		break;
-	}
-
-	RTS51X_DEBUGP("CIS block: 0x%x\n", xd_card->cis_block);
-	if (xd_card->cis_block == 0xFFFF)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	chip->capacity[chip->card2lun[XD_CARD]] = xd_card->capacity;
-
-	return STATUS_SUCCESS;
-}
-
-static int xd_check_data_blank(u8 *redunt)
-{
-	int i;
-
-	for (i = 0; i < 6; i++) {
-		if (redunt[PAGE_STATUS + i] != 0xFF)
-			return 0;
-	}
-
-	if ((redunt[PARITY] & (XD_ECC1_ALL1 | XD_ECC2_ALL1)) !=
-	    (XD_ECC1_ALL1 | XD_ECC2_ALL1))
-		return 0;
-
-	for (i = 0; i < 4; i++) {
-		if (redunt[RESERVED0 + i] != 0xFF)
-			return 0;
-	}
-
-	return 1;
-}
-
-static u16 xd_load_log_block_addr(u8 *redunt)
-{
-	u16 addr = 0xFFFF;
-
-	if (redunt[PARITY] & XD_BA1_BA2_EQL)
-		addr =
-		    ((u16) redunt[BLOCK_ADDR1_H] << 8) | redunt[BLOCK_ADDR1_L];
-	else if (redunt[PARITY] & XD_BA1_VALID)
-		addr =
-		    ((u16) redunt[BLOCK_ADDR1_H] << 8) | redunt[BLOCK_ADDR1_L];
-	else if (redunt[PARITY] & XD_BA2_VALID)
-		addr =
-		    ((u16) redunt[BLOCK_ADDR2_H] << 8) | redunt[BLOCK_ADDR2_L];
-
-	return addr;
-}
-
-static int xd_init_l2p_tbl(struct rts51x_chip *chip)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	int size, i;
-
-	RTS51X_DEBUGP("xd_init_l2p_tbl: zone_cnt = %d\n", xd_card->zone_cnt);
-
-	if (xd_card->zone_cnt < 1)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	size = xd_card->zone_cnt * sizeof(struct zone_entry);
-	RTS51X_DEBUGP("Buffer size for l2p table is %d\n", size);
-
-	xd_card->zone = vmalloc(size);
-	if (!xd_card->zone)
-		TRACE_RET(chip, STATUS_NOMEM);
-
-	for (i = 0; i < xd_card->zone_cnt; i++) {
-		xd_card->zone[i].build_flag = 0;
-		xd_card->zone[i].l2p_table = NULL;
-		xd_card->zone[i].free_table = NULL;
-		xd_card->zone[i].get_index = 0;
-		xd_card->zone[i].set_index = 0;
-		xd_card->zone[i].unused_blk_cnt = 0;
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static inline void free_zone(struct zone_entry *zone)
-{
-	RTS51X_DEBUGP("free_zone\n");
-	if (!zone)
-		return;
-	zone->build_flag = 0;
-	zone->set_index = 0;
-	zone->get_index = 0;
-	zone->unused_blk_cnt = 0;
-	if (zone->l2p_table) {
-		vfree(zone->l2p_table);
-		zone->l2p_table = NULL;
-	}
-	if (zone->free_table) {
-		vfree(zone->free_table);
-		zone->free_table = NULL;
-	}
-}
-
-static void xd_set_unused_block(struct rts51x_chip *chip, u32 phy_blk)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	struct zone_entry *zone;
-	int zone_no;
-
-	zone_no = (int)phy_blk >> 10;
-	if (zone_no >= xd_card->zone_cnt) {
-		RTS51X_DEBUGP("Set unused block to invalid zone"
-					"(zone_no = %d, zone_cnt = %d)\n",
-					zone_no, xd_card->zone_cnt);
-		return;
-	}
-	zone = &(xd_card->zone[zone_no]);
-
-	if (zone->free_table == NULL) {
-		if (xd_build_l2p_tbl(chip, zone_no) != STATUS_SUCCESS)
-			return;
-	}
-
-	if ((zone->set_index >= XD_FREE_TABLE_CNT)
-	    || (zone->set_index < 0)) {
-		free_zone(zone);
-		RTS51X_DEBUGP("Set unused block fail, invalid set_index\n");
-		return;
-	}
-
-	RTS51X_DEBUGP("Set unused block to index %d\n", zone->set_index);
-
-	zone->free_table[zone->set_index++] = (u16) (phy_blk & 0x3ff);
-	if (zone->set_index >= XD_FREE_TABLE_CNT)
-		zone->set_index = 0;
-	zone->unused_blk_cnt++;
-}
-
-static u32 xd_get_unused_block(struct rts51x_chip *chip, int zone_no)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	struct zone_entry *zone;
-	u32 phy_blk;
-
-	if (zone_no >= xd_card->zone_cnt) {
-		RTS51X_DEBUGP("Get unused block from invalid zone"
-					"(zone_no = %d, zone_cnt = %d)\n",
-					zone_no, xd_card->zone_cnt);
-		TRACE_RET(chip, BLK_NOT_FOUND);
-	}
-	zone = &(xd_card->zone[zone_no]);
-
-	if ((zone->unused_blk_cnt == 0) ||
-			(zone->set_index == zone->get_index)) {
-		free_zone(zone);
-		RTS51X_DEBUGP("Get unused block fail,"
-					"no unused block available\n");
-		TRACE_RET(chip, BLK_NOT_FOUND);
-	}
-	if ((zone->get_index >= XD_FREE_TABLE_CNT) || (zone->get_index < 0)) {
-		free_zone(zone);
-		RTS51X_DEBUGP("Get unused block fail, invalid get_index\n");
-		TRACE_RET(chip, BLK_NOT_FOUND);
-	}
-
-	RTS51X_DEBUGP("Get unused block from index %d\n", zone->get_index);
-
-	phy_blk = zone->free_table[zone->get_index];
-	zone->free_table[zone->get_index++] = 0xFFFF;
-	if (zone->get_index >= XD_FREE_TABLE_CNT)
-		zone->get_index = 0;
-	zone->unused_blk_cnt--;
-
-	phy_blk += ((u32) (zone_no) << 10);
-	return phy_blk;
-}
-
-static void xd_set_l2p_tbl(struct rts51x_chip *chip, int zone_no, u16 log_off,
-			   u16 phy_off)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	struct zone_entry *zone;
-
-	zone = &(xd_card->zone[zone_no]);
-	zone->l2p_table[log_off] = phy_off;
-}
-
-static int xd_delay_write(struct rts51x_chip *chip);
-
-static u32 xd_get_l2p_tbl(struct rts51x_chip *chip, int zone_no, u16 log_off)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	struct zone_entry *zone;
-	int retval;
-
-	zone = &(xd_card->zone[zone_no]);
-	if (zone->l2p_table[log_off] == 0xFFFF) {
-		u32 phy_blk = 0;
-		int i;
-
-		retval = xd_delay_write(chip);
-		if (retval != STATUS_SUCCESS) {
-			RTS51X_DEBUGP("In xd_get_l2p_tbl,"
-						"delay write fail!\n");
-			TRACE_RET(chip, BLK_NOT_FOUND);
-		}
-
-		if (zone->unused_blk_cnt <= 0) {
-			RTS51X_DEBUGP("No unused block!\n");
-			TRACE_RET(chip, BLK_NOT_FOUND);
-		}
-
-		for (i = 0; i < zone->unused_blk_cnt; i++) {
-			phy_blk = xd_get_unused_block(chip, zone_no);
-			if (phy_blk == BLK_NOT_FOUND) {
-				RTS51X_DEBUGP("No unused block available!\n");
-				TRACE_RET(chip, BLK_NOT_FOUND);
-			}
-
-			retval =
-			    xd_init_page(chip, phy_blk, log_off, 0,
-					 xd_card->page_off + 1);
-			if (retval == STATUS_SUCCESS)
-				break;
-		}
-		if (i >= zone->unused_blk_cnt) {
-			RTS51X_DEBUGP("No good unused block available!\n");
-			TRACE_RET(chip, BLK_NOT_FOUND);
-		}
-
-		xd_set_l2p_tbl(chip, zone_no, log_off, (u16) (phy_blk & 0x3FF));
-		return phy_blk;
-	}
-
-	return (u32) zone->l2p_table[log_off] + ((u32) (zone_no) << 10);
-}
-
-int rts51x_reset_xd_card(struct rts51x_chip *chip)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	int retval;
-
-	memset(xd_card, 0, sizeof(struct xd_info));
-
-	xd_card->block_shift = 0;
-	xd_card->page_off = 0;
-	xd_card->addr_cycle = 0;
-	xd_card->capacity = 0;
-	xd_card->zone_cnt = 0;
-	xd_card->cis_block = 0xFFFF;
-	xd_card->delay_write.delay_write_flag = 0;
-
-	rts51x_enable_card_clock(chip, XD_CARD);
-
-	retval = reset_xd(chip);
-	if (retval != STATUS_SUCCESS) {
-		if (chip->option.reset_or_rw_fail_set_pad_drive) {
-			rts51x_write_register(chip, CARD_DRIVE_SEL,
-					      SD20_DRIVE_MASK, DRIVE_8mA);
-		}
-		TRACE_RET(chip, retval);
-	}
-
-	retval = xd_init_l2p_tbl(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-static int xd_mark_bad_block(struct rts51x_chip *chip, u32 phy_blk)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	int retval;
-	u32 page_addr;
-	u8 reg = 0;
-
-	RTS51X_DEBUGP("mark block 0x%x as bad block\n", phy_blk);
-
-	if (phy_blk == BLK_NOT_FOUND)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_STATUS, 0xFF, XD_GPG);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_STATUS, 0xFF,
-		       XD_LATER_BBLK);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_ADDR1_H, 0xFF, 0xFF);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_ADDR1_L, 0xFF, 0xFF);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_ADDR2_H, 0xFF, 0xFF);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_ADDR2_L, 0xFF, 0xFF);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_RESERVED0, 0xFF, 0xFF);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_RESERVED1, 0xFF, 0xFF);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_RESERVED2, 0xFF, 0xFF);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_RESERVED3, 0xFF, 0xFF);
-
-	page_addr = phy_blk << xd_card->block_shift;
-
-	xd_assign_phy_addr(chip, page_addr, XD_RW_ADDR);
-
-	/* Specify page count */
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_CNT, 0xFF,
-		       xd_card->page_off + 1);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
-		       XD_TRANSFER_START | XD_WRITE_REDUNDANT);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER, XD_TRANSFER_END,
-		       XD_TRANSFER_END);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	retval = rts51x_get_rsp(chip, 1, 100);
-
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_xd_error(chip);
-		rts51x_ep0_read_register(chip, XD_DAT, &reg);
-		if (reg & PROGRAM_ERROR)
-			xd_set_err_code(chip, XD_PRG_ERROR);
-		else
-			xd_set_err_code(chip, XD_TO_ERROR);
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int xd_init_page(struct rts51x_chip *chip, u32 phy_blk, u16 logoff,
-			u8 start_page, u8 end_page)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	int retval;
-	u32 page_addr;
-	u8 reg = 0;
-
-	RTS51X_DEBUGP("Init block 0x%x\n", phy_blk);
-
-	if (start_page > end_page)
-		TRACE_RET(chip, STATUS_FAIL);
-	if (phy_blk == BLK_NOT_FOUND)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_STATUS, 0xFF, 0xFF);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_STATUS, 0xFF, 0xFF);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_ADDR1_H, 0xFF,
-		       (u8) (logoff >> 8));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_ADDR1_L, 0xFF,
-		       (u8) logoff);
-
-	page_addr = (phy_blk << xd_card->block_shift) + start_page;
-
-	xd_assign_phy_addr(chip, page_addr, XD_RW_ADDR);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CFG, XD_BA_TRANSFORM,
-		       XD_BA_TRANSFORM);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_CNT, 0xFF,
-		       (end_page - start_page));
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
-		       XD_TRANSFER_START | XD_WRITE_REDUNDANT);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER, XD_TRANSFER_END,
-		       XD_TRANSFER_END);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	retval = rts51x_get_rsp(chip, 1, 500);
-
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_xd_error(chip);
-		rts51x_ep0_read_register(chip, XD_DAT, &reg);
-		if (reg & PROGRAM_ERROR) {
-			xd_mark_bad_block(chip, phy_blk);
-			xd_set_err_code(chip, XD_PRG_ERROR);
-		} else {
-			xd_set_err_code(chip, XD_TO_ERROR);
-		}
-		TRACE_RET(chip, STATUS_FAIL);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int xd_copy_page(struct rts51x_chip *chip,
-			u32 old_blk, u32 new_blk, u8 start_page, u8 end_page)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	u32 old_page, new_page;
-	u8 i, reg = 0;
-	int retval;
-
-	RTS51X_DEBUGP("Copy page from block 0x%x to block 0x%x\n", old_blk,
-		       new_blk);
-
-	if (start_page > end_page)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	if ((old_blk == BLK_NOT_FOUND) || (new_blk == BLK_NOT_FOUND))
-		TRACE_RET(chip, STATUS_FAIL);
-
-	old_page = (old_blk << xd_card->block_shift) + start_page;
-	new_page = (new_blk << xd_card->block_shift) + start_page;
-
-	XD_CLR_BAD_NEWBLK(xd_card);
-
-	RTS51X_WRITE_REG(chip, CARD_DATA_SOURCE, 0x01, PINGPONG_BUFFER);
-
-	for (i = start_page; i < end_page; i++) {
-		if (monitor_card_cd(chip, XD_CARD) == CD_NOT_EXIST) {
-			RTS51X_WRITE_REG(chip, CARD_STOP, XD_STOP | XD_CLR_ERR,
-					 XD_STOP | XD_CLR_ERR);
-			xd_set_err_code(chip, XD_NO_CARD);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		rts51x_init_cmd(chip);
-
-		xd_assign_phy_addr(chip, old_page, XD_RW_ADDR);
-
-		/* Single page read */
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_CNT, 0xFF, 1);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CHK_DATA_STATUS,
-			       XD_AUTO_CHK_DATA_STATUS, 0);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
-			       XD_TRANSFER_START | XD_READ_PAGES);
-		rts51x_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER,
-			       XD_TRANSFER_END, XD_TRANSFER_END);
-
-		retval = rts51x_send_cmd(chip, MODE_CR | STAGE_XD_STATUS, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		retval = rts51x_get_rsp(chip, 4, 500);
-		if ((retval != STATUS_SUCCESS) ||
-		    (chip->rsp_buf[2] & (XD_ECC1_ERROR | XD_ECC2_ERROR))) {
-			rts51x_clear_xd_error(chip);
-			reg = 0;
-			rts51x_ep0_read_register(chip, XD_CTL, &reg);
-			if (reg & (XD_ECC1_ERROR | XD_ECC2_ERROR)) {
-				wait_timeout(100);
-
-				if (monitor_card_cd(chip, XD_CARD) ==
-				    CD_NOT_EXIST) {
-					xd_set_err_code(chip, XD_NO_CARD);
-					TRACE_RET(chip, STATUS_FAIL);
-				}
-
-				if (((reg &
-				      (XD_ECC1_ERROR | XD_ECC1_UNCORRECTABLE))
-				     == (XD_ECC1_ERROR | XD_ECC1_UNCORRECTABLE))
-				    || ((reg & (XD_ECC2_ERROR |
-					XD_ECC2_UNCORRECTABLE)) ==
-				     (XD_ECC2_ERROR | XD_ECC2_UNCORRECTABLE))) {
-					RTS51X_WRITE_REG(chip, XD_PAGE_STATUS,
-							 0xFF, XD_BPG);
-					RTS51X_WRITE_REG(chip, XD_BLOCK_STATUS,
-							 0xFF, XD_GBLK);
-					XD_SET_BAD_OLDBLK(xd_card);
-					RTS51X_DEBUGP("old block 0x%x"
-						"ecc error\n", old_blk);
-				}
-			} else {
-				xd_set_err_code(chip, XD_TO_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-		if (XD_CHK_BAD_OLDBLK(xd_card))
-			rts51x_clear_xd_error(chip);
-
-		rts51x_init_cmd(chip);
-
-		xd_assign_phy_addr(chip, new_page, XD_RW_ADDR);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_CNT, 0xFF, 1);
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
-			       XD_TRANSFER_START | XD_WRITE_PAGES);
-		rts51x_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER,
-			       XD_TRANSFER_END, XD_TRANSFER_END);
-
-		retval = rts51x_send_cmd(chip, MODE_CR, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		retval = rts51x_get_rsp(chip, 1, 300);
-		if (retval != STATUS_SUCCESS) {
-			rts51x_clear_xd_error(chip);
-			reg = 0;
-			rts51x_ep0_read_register(chip, XD_DAT, &reg);
-			if (reg & PROGRAM_ERROR) {
-				xd_mark_bad_block(chip, new_blk);
-				xd_set_err_code(chip, XD_PRG_ERROR);
-				XD_SET_BAD_NEWBLK(xd_card);
-			} else {
-				xd_set_err_code(chip, XD_TO_ERROR);
-			}
-			TRACE_RET(chip, retval);
-		}
-
-		old_page++;
-		new_page++;
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int xd_reset_cmd(struct rts51x_chip *chip)
-{
-	int retval;
-	u8 xd_dat, xd_ctl;
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
-		       XD_TRANSFER_START | XD_RESET);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER, XD_TRANSFER_END,
-		       XD_TRANSFER_END);
-	rts51x_add_cmd(chip, READ_REG_CMD, XD_DAT, 0, 0);
-	rts51x_add_cmd(chip, READ_REG_CMD, XD_CTL, 0, 0);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, 3, 100);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_xd_error(chip);
-		TRACE_RET(chip, retval);
-	}
-
-	xd_dat = chip->rsp_buf[1];
-	xd_ctl = chip->rsp_buf[2];
-	if (((xd_dat & READY_FLAG) == READY_STATE) && (xd_ctl & XD_RDY))
-		return STATUS_SUCCESS;
-
-	TRACE_RET(chip, STATUS_FAIL);
-}
-
-static int xd_erase_block(struct rts51x_chip *chip, u32 phy_blk)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	u32 page_addr;
-	u8 reg = 0, xd_dat;
-	int i, retval;
-
-	if (phy_blk == BLK_NOT_FOUND)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	page_addr = phy_blk << xd_card->block_shift;
-
-	for (i = 0; i < 3; i++) {
-		rts51x_init_cmd(chip);
-
-		xd_assign_phy_addr(chip, page_addr, XD_ERASE_ADDR);
-
-		rts51x_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
-			       XD_TRANSFER_START | XD_ERASE);
-		rts51x_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER,
-			       XD_TRANSFER_END, XD_TRANSFER_END);
-		rts51x_add_cmd(chip, READ_REG_CMD, XD_DAT, 0, 0);
-
-		retval = rts51x_send_cmd(chip, MODE_CR, 100);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		retval = rts51x_get_rsp(chip, 2, 300);
-		if (retval != STATUS_SUCCESS) {
-			rts51x_clear_xd_error(chip);
-			rts51x_ep0_read_register(chip, XD_DAT, &reg);
-			if (reg & PROGRAM_ERROR) {
-				xd_mark_bad_block(chip, phy_blk);
-				xd_set_err_code(chip, XD_PRG_ERROR);
-				TRACE_RET(chip, STATUS_FAIL);
-			} else {
-				xd_set_err_code(chip, XD_ERASE_FAIL);
-			}
-			retval = xd_reset_cmd(chip);
-			if (retval != STATUS_SUCCESS)
-				TRACE_RET(chip, retval);
-			continue;
-		}
-		xd_dat = chip->rsp_buf[1];
-		if (xd_dat & PROGRAM_ERROR) {
-			xd_mark_bad_block(chip, phy_blk);
-			xd_set_err_code(chip, XD_PRG_ERROR);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		return STATUS_SUCCESS;
-	}
-
-	xd_mark_bad_block(chip, phy_blk);
-	xd_set_err_code(chip, XD_ERASE_FAIL);
-	TRACE_RET(chip, STATUS_FAIL);
-}
-
-static int xd_build_l2p_tbl(struct rts51x_chip *chip, int zone_no)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	struct zone_entry *zone;
-	int retval;
-	u32 start, end, i;
-	u16 max_logoff, cur_fst_page_logoff, cur_lst_page_logoff,
-	    ent_lst_page_logoff;
-	u8 redunt[11];
-
-	RTS51X_DEBUGP("xd_build_l2p_tbl: %d\n", zone_no);
-
-	if (xd_card->zone == NULL) {
-		retval = xd_init_l2p_tbl(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	if (xd_card->zone[zone_no].build_flag) {
-		RTS51X_DEBUGP("l2p table of zone %d has been built\n",
-			       zone_no);
-		return STATUS_SUCCESS;
-	}
-
-	zone = &(xd_card->zone[zone_no]);
-
-	if (zone->l2p_table == NULL) {
-		zone->l2p_table = vmalloc(2000);
-		if (zone->l2p_table == NULL)
-			TRACE_GOTO(chip, Build_Fail);
-	}
-	memset((u8 *) (zone->l2p_table), 0xff, 2000);
-
-	if (zone->free_table == NULL) {
-		zone->free_table = vmalloc(XD_FREE_TABLE_CNT * 2);
-		if (zone->free_table == NULL)
-			TRACE_GOTO(chip, Build_Fail);
-	}
-	memset((u8 *) (zone->free_table), 0xff, XD_FREE_TABLE_CNT * 2);
-
-	if (zone_no == 0) {
-		if (xd_card->cis_block == 0xFFFF)
-			start = 0;
-		else
-			start = xd_card->cis_block + 1;
-		if (XD_CHK_4MB(xd_card)) {
-			end = 0x200;
-			max_logoff = 499;
-		} else {
-			end = 0x400;
-			max_logoff = 999;
-		}
-	} else {
-		start = (u32) (zone_no) << 10;
-		end = (u32) (zone_no + 1) << 10;
-		max_logoff = 999;
-	}
-
-	RTS51X_DEBUGP("start block 0x%x, end block 0x%x\n", start, end);
-
-	zone->set_index = zone->get_index = 0;
-	zone->unused_blk_cnt = 0;
-
-	for (i = start; i < end; i++) {
-		u32 page_addr = i << xd_card->block_shift;
-		u32 phy_block;
-
-		retval = xd_read_redundant(chip, page_addr, redunt, 11);
-		if (retval != STATUS_SUCCESS)
-			continue;
-
-		if (redunt[BLOCK_STATUS] != 0xFF) {
-			RTS51X_DEBUGP("bad block\n");
-			continue;
-		}
-
-		if (xd_check_data_blank(redunt)) {
-			RTS51X_DEBUGP("blank block\n");
-			xd_set_unused_block(chip, i);
-			continue;
-		}
-
-		cur_fst_page_logoff = xd_load_log_block_addr(redunt);
-		if ((cur_fst_page_logoff == 0xFFFF)
-		    || (cur_fst_page_logoff > max_logoff)) {
-			retval = xd_erase_block(chip, i);
-			if (retval == STATUS_SUCCESS)
-				xd_set_unused_block(chip, i);
-			continue;
-		}
-		if ((zone_no == 0) && (cur_fst_page_logoff == 0)
-		    && (redunt[PAGE_STATUS] != XD_GPG))
-			XD_SET_MBR_FAIL(xd_card);
-
-		if (zone->l2p_table[cur_fst_page_logoff] == 0xFFFF) {
-			zone->l2p_table[cur_fst_page_logoff] =
-			    (u16) (i & 0x3FF);
-			continue;
-		}
-
-		phy_block =
-		    zone->l2p_table[cur_fst_page_logoff] +
-		    ((u32) ((zone_no) << 10));
-
-		page_addr = ((i + 1) << xd_card->block_shift) - 1;
-
-		retval = xd_read_redundant(chip, page_addr, redunt, 11);
-		if (retval != STATUS_SUCCESS)
-			continue;
-
-		cur_lst_page_logoff = xd_load_log_block_addr(redunt);
-		if (cur_lst_page_logoff == cur_fst_page_logoff) {
-			int m;
-
-			page_addr =
-			    ((phy_block + 1) << xd_card->block_shift) - 1;
-
-			for (m = 0; m < 3; m++) {
-				retval =
-				    xd_read_redundant(chip, page_addr, redunt,
-						      11);
-				if (retval == STATUS_SUCCESS)
-					break;
-			}
-
-			if (m == 3) {
-				zone->l2p_table[cur_fst_page_logoff] =
-				    (u16) (i & 0x3FF);
-				retval = xd_erase_block(chip, phy_block);
-				if (retval == STATUS_SUCCESS)
-					xd_set_unused_block(chip, phy_block);
-				continue;
-			}
-
-			ent_lst_page_logoff = xd_load_log_block_addr(redunt);
-			if (ent_lst_page_logoff != cur_fst_page_logoff) {
-				zone->l2p_table[cur_fst_page_logoff] =
-				    (u16) (i & 0x3FF);
-				retval = xd_erase_block(chip, phy_block);
-				if (retval == STATUS_SUCCESS)
-					xd_set_unused_block(chip, phy_block);
-				continue;
-			} else {
-				retval = xd_erase_block(chip, i);
-				if (retval == STATUS_SUCCESS)
-					xd_set_unused_block(chip, i);
-			}
-		} else {
-			retval = xd_erase_block(chip, i);
-			if (retval == STATUS_SUCCESS)
-				xd_set_unused_block(chip, i);
-		}
-	}
-
-	if (XD_CHK_4MB(xd_card))
-		end = 500;
-	else
-		end = 1000;
-
-	i = 0;
-	for (start = 0; start < end; start++) {
-		if (zone->l2p_table[start] == 0xFFFF)
-			i++;
-	}
-
-	RTS51X_DEBUGP("Block count %d, invalid L2P entry %d\n", end, i);
-	RTS51X_DEBUGP("Total unused block: %d\n", zone->unused_blk_cnt);
-
-	if ((zone->unused_blk_cnt - i) < 1)
-		chip->card_wp |= XD_CARD;
-
-	zone->build_flag = 1;
-
-	return STATUS_SUCCESS;
-
-Build_Fail:
-	if (zone->l2p_table) {
-		vfree(zone->l2p_table);
-		zone->l2p_table = NULL;
-	}
-	if (zone->free_table) {
-		vfree(zone->free_table);
-		zone->free_table = NULL;
-	}
-
-	return STATUS_FAIL;
-}
-
-static int xd_send_cmd(struct rts51x_chip *chip, u8 cmd)
-{
-	int retval;
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_DAT, 0xFF, cmd);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
-		       XD_TRANSFER_START | XD_SET_CMD);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER, XD_TRANSFER_END,
-		       XD_TRANSFER_END);
-
-	retval = rts51x_send_cmd(chip, MODE_CR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval = rts51x_get_rsp(chip, 1, 200);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_xd_error(chip);
-		TRACE_RET(chip, retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int xd_read_multiple_pages(struct rts51x_chip *chip, u32 phy_blk,
-				  u32 log_blk, u8 start_page, u8 end_page,
-				  u8 *buf, void **ptr, unsigned int *offset)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	u32 page_addr, new_blk;
-	u16 log_off;
-	u8 reg_val, page_cnt;
-	int zone_no, retval, i;
-
-	if (start_page > end_page)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	page_cnt = end_page - start_page;
-	zone_no = (int)(log_blk / 1000);
-	log_off = (u16) (log_blk % 1000);
-
-	if ((phy_blk & 0x3FF) == 0x3FF) {
-		for (i = 0; i < 256; i++) {
-			page_addr = ((u32) i) << xd_card->block_shift;
-
-			retval = xd_read_redundant(chip, page_addr, NULL, 0);
-			if (retval == STATUS_SUCCESS)
-				break;
-
-			if (monitor_card_cd(chip, XD_CARD) == CD_NOT_EXIST) {
-				xd_set_err_code(chip, XD_NO_CARD);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-	}
-
-	page_addr = (phy_blk << xd_card->block_shift) + start_page;
-
-	rts51x_init_cmd(chip);
-
-	xd_assign_phy_addr(chip, page_addr, XD_RW_ADDR);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CFG, XD_PPB_TO_SIE,
-		       XD_PPB_TO_SIE);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-		       RING_BUFFER);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_CNT, 0xFF, page_cnt);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CHK_DATA_STATUS,
-		       XD_AUTO_CHK_DATA_STATUS, XD_AUTO_CHK_DATA_STATUS);
-
-	rts51x_trans_dma_enable(chip->srb->sc_data_direction, chip,
-			page_cnt * 512, DMA_512);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
-		       XD_TRANSFER_START | XD_READ_PAGES);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER,
-		       XD_TRANSFER_END | XD_PPB_EMPTY,
-		       XD_TRANSFER_END | XD_PPB_EMPTY);
-
-	retval = rts51x_send_cmd(chip, MODE_CDIR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval =
-	    rts51x_transfer_data_partial(chip, RCV_BULK_PIPE(chip), (void *)buf,
-					 ptr, offset, page_cnt * 512,
-					 scsi_sg_count(chip->srb), NULL, 2000);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_xd_error(chip);
-
-		if (retval == STATUS_TIMEDOUT) {
-			xd_set_err_code(chip, XD_TO_ERROR);
-			TRACE_RET(chip, retval);
-		} else {
-			TRACE_GOTO(chip, Fail);
-		}
-	}
-	retval = rts51x_get_rsp(chip, 1, 200);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_xd_error(chip);
-
-		if (retval == STATUS_TIMEDOUT) {
-			xd_set_err_code(chip, XD_TO_ERROR);
-			TRACE_RET(chip, retval);
-		} else {
-			TRACE_GOTO(chip, Fail);
-		}
-	}
-
-	return STATUS_SUCCESS;
-
-Fail:
-	rts51x_ep0_read_register(chip, XD_PAGE_STATUS, &reg_val);
-	RTS51X_DEBUGP("XD_PAGE_STATUS: 0x%x\n", reg_val);
-
-	if (reg_val != XD_GPG)
-		xd_set_err_code(chip, XD_PRG_ERROR);
-
-	rts51x_ep0_read_register(chip, XD_CTL, &reg_val);
-	RTS51X_DEBUGP("XD_CTL: 0x%x\n", reg_val);
-
-	/* Handle uncorrectable ECC error */
-	if (((reg_val & (XD_ECC1_ERROR | XD_ECC1_UNCORRECTABLE))
-	     == (XD_ECC1_ERROR | XD_ECC1_UNCORRECTABLE))
-	    || ((reg_val & (XD_ECC2_ERROR | XD_ECC2_UNCORRECTABLE))
-		== (XD_ECC2_ERROR | XD_ECC2_UNCORRECTABLE))) {
-		wait_timeout(100);
-
-		if (monitor_card_cd(chip, XD_CARD) == CD_NOT_EXIST) {
-			xd_set_err_code(chip, XD_NO_CARD);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		xd_set_err_code(chip, XD_ECC_ERROR);
-
-		new_blk = xd_get_unused_block(chip, zone_no);
-		if (new_blk == NO_NEW_BLK) {
-			XD_CLR_BAD_OLDBLK(xd_card);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-		retval =
-		    xd_copy_page(chip, phy_blk, new_blk, 0,
-				 xd_card->page_off + 1);
-		if (retval != STATUS_SUCCESS) {
-			if (!XD_CHK_BAD_NEWBLK(xd_card)) {
-				retval = xd_erase_block(chip, new_blk);
-				if (retval == STATUS_SUCCESS)
-					xd_set_unused_block(chip, new_blk);
-			} else {
-				XD_CLR_BAD_NEWBLK(xd_card);
-			}
-			XD_CLR_BAD_OLDBLK(xd_card);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-		xd_set_l2p_tbl(chip, zone_no, log_off, (u16) (new_blk & 0x3FF));
-		xd_erase_block(chip, phy_blk);
-		xd_mark_bad_block(chip, phy_blk);
-		XD_CLR_BAD_OLDBLK(xd_card);
-	}
-
-	TRACE_RET(chip, STATUS_FAIL);
-}
-
-static int xd_finish_write(struct rts51x_chip *chip,
-			   u32 old_blk, u32 new_blk, u32 log_blk, u8 page_off)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	int retval, zone_no;
-	u16 log_off;
-
-	RTS51X_DEBUGP("xd_finish_write, old_blk = 0x%x, new_blk = 0x%x,"
-				"log_blk = 0x%x\n", old_blk, new_blk, log_blk);
-
-	if (page_off > xd_card->page_off)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	zone_no = (int)(log_blk / 1000);
-	log_off = (u16) (log_blk % 1000);
-
-	if (old_blk == BLK_NOT_FOUND) {
-		retval = xd_init_page(chip, new_blk, log_off,
-				      page_off, xd_card->page_off + 1);
-		if (retval != STATUS_SUCCESS) {
-			retval = xd_erase_block(chip, new_blk);
-			if (retval == STATUS_SUCCESS)
-				xd_set_unused_block(chip, new_blk);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	} else {
-		retval = xd_copy_page(chip, old_blk, new_blk,
-				      page_off, xd_card->page_off + 1);
-		if (retval != STATUS_SUCCESS) {
-			if (!XD_CHK_BAD_NEWBLK(xd_card)) {
-				retval = xd_erase_block(chip, new_blk);
-				if (retval == STATUS_SUCCESS)
-					xd_set_unused_block(chip, new_blk);
-			}
-			XD_CLR_BAD_NEWBLK(xd_card);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		retval = xd_erase_block(chip, old_blk);
-		if (retval == STATUS_SUCCESS) {
-			if (XD_CHK_BAD_OLDBLK(xd_card)) {
-				xd_mark_bad_block(chip, old_blk);
-				XD_CLR_BAD_OLDBLK(xd_card);
-			} else {
-				/* Add source block to unused block */
-				xd_set_unused_block(chip, old_blk);
-			}
-		} else {
-			xd_set_err_code(chip, XD_NO_ERROR);
-			XD_CLR_BAD_OLDBLK(xd_card);
-		}
-	}
-
-	/* Add target block to L2P table */
-	xd_set_l2p_tbl(chip, zone_no, log_off, (u16) (new_blk & 0x3FF));
-
-	return STATUS_SUCCESS;
-}
-
-static int xd_prepare_write(struct rts51x_chip *chip,
-			    u32 old_blk, u32 new_blk, u32 log_blk, u8 page_off)
-{
-	int retval;
-
-	RTS51X_DEBUGP("xd_prepare_write, old_blk = 0x%x, new_blk = 0x%x,"
-				"log_blk = 0x%x, page_off = %d\n",
-				old_blk, new_blk, log_blk, (int)page_off);
-
-	if (page_off) {
-		retval = xd_copy_page(chip, old_blk, new_blk, 0, page_off);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-static int xd_write_multiple_pages(struct rts51x_chip *chip, u32 old_blk,
-				   u32 new_blk, u32 log_blk, u8 start_page,
-				   u8 end_page, u8 *buf, void **ptr,
-				   unsigned int *offset)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	u32 page_addr;
-	int zone_no, retval;
-	u16 log_off;
-	u8 page_cnt, reg_val;
-
-	RTS51X_DEBUGP("xd_write_multiple_pages, old_blk = 0x%x,"
-				"new_blk = 0x%x, log_blk = 0x%x\n",
-				old_blk, new_blk, log_blk);
-
-	if (start_page > end_page)
-		TRACE_RET(chip, STATUS_FAIL);
-
-	page_cnt = end_page - start_page;
-	zone_no = (int)(log_blk / 1000);
-	log_off = (u16) (log_blk % 1000);
-
-	page_addr = (new_blk << xd_card->block_shift) + start_page;
-
-	/* Send index command */
-	retval = xd_send_cmd(chip, READ1_1);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	rts51x_init_cmd(chip);
-
-	/* Prepare redundant field */
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_ADDR1_H, 0xFF,
-		       (u8) (log_off >> 8));
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_ADDR1_L, 0xFF,
-		       (u8) log_off);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_BLOCK_STATUS, 0xFF, XD_GBLK);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_STATUS, 0xFF, XD_GPG);
-
-	xd_assign_phy_addr(chip, page_addr, XD_RW_ADDR);
-
-	/* Transform the block address by hardware */
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_CFG, XD_BA_TRANSFORM,
-		       XD_BA_TRANSFORM);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_PAGE_CNT, 0xFF, page_cnt);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE, 0x01,
-		       RING_BUFFER);
-
-	rts51x_trans_dma_enable(chip->srb->sc_data_direction, chip,
-			page_cnt * 512, DMA_512);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, XD_TRANSFER, 0xFF,
-		       XD_TRANSFER_START | XD_WRITE_PAGES);
-	rts51x_add_cmd(chip, CHECK_REG_CMD, XD_TRANSFER, XD_TRANSFER_END,
-		       XD_TRANSFER_END);
-
-	retval = rts51x_send_cmd(chip, MODE_CDOR, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	retval =
-	    rts51x_transfer_data_partial(chip, SND_BULK_PIPE(chip), (void *)buf,
-					 ptr, offset, page_cnt * 512,
-					 scsi_sg_count(chip->srb), NULL, 2000);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_xd_error(chip);
-
-		if (retval == STATUS_TIMEDOUT) {
-			xd_set_err_code(chip, XD_TO_ERROR);
-			TRACE_RET(chip, retval);
-		} else {
-			TRACE_GOTO(chip, Fail);
-		}
-	}
-	retval = rts51x_get_rsp(chip, 1, 200);
-	if (retval != STATUS_SUCCESS) {
-		rts51x_clear_xd_error(chip);
-
-		if (retval == STATUS_TIMEDOUT) {
-			xd_set_err_code(chip, XD_TO_ERROR);
-			TRACE_RET(chip, retval);
-		} else {
-			TRACE_GOTO(chip, Fail);
-		}
-	}
-
-	if (end_page == (xd_card->page_off + 1)) {
-		xd_card->delay_write.delay_write_flag = 0;
-
-		if (old_blk != BLK_NOT_FOUND) {
-			retval = xd_erase_block(chip, old_blk);
-			if (retval == STATUS_SUCCESS) {
-				if (XD_CHK_BAD_OLDBLK(xd_card)) {
-					xd_mark_bad_block(chip, old_blk);
-					XD_CLR_BAD_OLDBLK(xd_card);
-				} else {
-					xd_set_unused_block(chip, old_blk);
-				}
-			} else {
-				xd_set_err_code(chip, XD_NO_ERROR);
-				XD_CLR_BAD_OLDBLK(xd_card);
-			}
-		}
-		xd_set_l2p_tbl(chip, zone_no, log_off, (u16) (new_blk & 0x3FF));
-	}
-
-	return STATUS_SUCCESS;
-
-Fail:
-	rts51x_ep0_read_register(chip, XD_DAT, &reg_val);
-	RTS51X_DEBUGP("XD_DAT: 0x%x\n", reg_val);
-
-	if (reg_val & PROGRAM_ERROR) {
-		xd_set_err_code(chip, XD_PRG_ERROR);
-		xd_mark_bad_block(chip, new_blk);
-	}
-
-	TRACE_RET(chip, STATUS_FAIL);
-}
-
-static int xd_delay_write(struct rts51x_chip *chip)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	struct xd_delay_write_tag *delay_write = &(xd_card->delay_write);
-	int retval;
-
-	if (delay_write->delay_write_flag) {
-		RTS51X_DEBUGP("xd_delay_write\n");
-		retval = xd_switch_clock(chip);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-
-		delay_write->delay_write_flag = 0;
-		retval = xd_finish_write(chip,
-					 delay_write->old_phyblock,
-					 delay_write->new_phyblock,
-					 delay_write->logblock,
-					 delay_write->pageoff);
-		if (retval != STATUS_SUCCESS)
-			TRACE_RET(chip, retval);
-	}
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_xd_rw(struct scsi_cmnd *srb, struct rts51x_chip *chip,
-	u32 start_sector, u16 sector_cnt)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	unsigned int lun = SCSI_LUN(srb);
-	struct xd_delay_write_tag *delay_write = &(xd_card->delay_write);
-	int retval, zone_no;
-	u32 log_blk, old_blk = 0, new_blk = 0;
-	u16 log_off, total_sec_cnt = sector_cnt;
-	u8 start_page, end_page = 0, page_cnt;
-	u8 *buf;
-	void *ptr = NULL;
-	unsigned int offset = 0;
-
-	xd_set_err_code(chip, XD_NO_ERROR);
-
-	xd_card->counter = 0;
-
-	RTS51X_DEBUGP("rts51x_xd_rw: scsi_bufflen = %d, scsi_sg_count = %d\n",
-		       scsi_bufflen(srb), scsi_sg_count(srb));
-	RTS51X_DEBUGP("Data direction: %s\n",
-		       (srb->sc_data_direction ==
-			DMA_TO_DEVICE) ? "write" : "read");
-
-	buf = (u8 *) scsi_sglist(srb);
-
-	retval = xd_switch_clock(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	log_blk = start_sector >> xd_card->block_shift;
-	start_page = (u8) start_sector & xd_card->page_off;
-	zone_no = (int)(log_blk / 1000);
-	log_off = (u16) (log_blk % 1000);
-
-	RTS51X_DEBUGP("log_blk = 0x%x\n", log_blk);
-
-	if (xd_card->zone[zone_no].build_flag == 0) {
-		retval = xd_build_l2p_tbl(chip, zone_no);
-		if (retval != STATUS_SUCCESS) {
-			chip->card_fail |= XD_CARD;
-			rts51x_set_sense_type(chip, lun,
-				SENSE_TYPE_MEDIA_NOT_PRESENT);
-			TRACE_RET(chip, retval);
-		}
-	}
-
-	if (srb->sc_data_direction == DMA_TO_DEVICE) {
-		if (delay_write->delay_write_flag &&
-		    (delay_write->logblock == log_blk) &&
-		    (start_page > delay_write->pageoff)) {
-			delay_write->delay_write_flag = 0;
-			if (delay_write->old_phyblock != BLK_NOT_FOUND) {
-				retval = xd_copy_page(chip,
-						      delay_write->old_phyblock,
-						      delay_write->new_phyblock,
-						      delay_write->pageoff,
-						      start_page);
-				if (retval != STATUS_SUCCESS) {
-					rts51x_set_sense_type(chip, lun,
-						SENSE_TYPE_MEDIA_WRITE_ERR);
-					TRACE_RET(chip, retval);
-				}
-			}
-			old_blk = delay_write->old_phyblock;
-			new_blk = delay_write->new_phyblock;
-		} else if (delay_write->delay_write_flag &&
-			   (delay_write->logblock == log_blk) &&
-			   (start_page == delay_write->pageoff)) {
-			delay_write->delay_write_flag = 0;
-			old_blk = delay_write->old_phyblock;
-			new_blk = delay_write->new_phyblock;
-		} else {
-			retval = xd_delay_write(chip);
-			if (retval != STATUS_SUCCESS) {
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, retval);
-			}
-			old_blk = xd_get_l2p_tbl(chip, zone_no, log_off);
-			new_blk = xd_get_unused_block(chip, zone_no);
-			if ((old_blk == BLK_NOT_FOUND)
-			    || (new_blk == BLK_NOT_FOUND)) {
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, retval);
-			}
-
-			retval =
-			    xd_prepare_write(chip, old_blk, new_blk, log_blk,
-					     start_page);
-			if (retval != STATUS_SUCCESS) {
-				if (monitor_card_cd(chip, XD_CARD) ==
-				    CD_NOT_EXIST) {
-					rts51x_set_sense_type(chip, lun,
-						SENSE_TYPE_MEDIA_NOT_PRESENT);
-					TRACE_RET(chip, STATUS_FAIL);
-				}
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, retval);
-			}
-		}
-	} else {
-		retval = xd_delay_write(chip);
-		if (retval != STATUS_SUCCESS) {
-			if (monitor_card_cd(chip, XD_CARD) == CD_NOT_EXIST) {
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_NOT_PRESENT);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-			TRACE_RET(chip, retval);
-		}
-
-		old_blk = xd_get_l2p_tbl(chip, zone_no, log_off);
-		if (old_blk == BLK_NOT_FOUND) {
-			rts51x_set_sense_type(chip, lun,
-				       SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-	}
-
-	RTS51X_DEBUGP("old_blk = 0x%x\n", old_blk);
-	if (srb->sc_data_direction == DMA_TO_DEVICE)
-		RTS51X_DEBUGP("new_blk = 0x%x\n", new_blk);
-
-	while (total_sec_cnt) {
-		if ((start_page + total_sec_cnt) > (xd_card->page_off + 1))
-			end_page = xd_card->page_off + 1;
-		else
-			end_page = start_page + (u8) total_sec_cnt;
-		page_cnt = end_page - start_page;
-		if (srb->sc_data_direction == DMA_FROM_DEVICE) {
-			retval = xd_read_multiple_pages(chip, old_blk, log_blk,
-							start_page, end_page,
-							buf, &ptr, &offset);
-			if (retval != STATUS_SUCCESS) {
-				rts51x_set_sense_type(chip, lun,
-					SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		} else {
-			retval =
-			    xd_write_multiple_pages(chip, old_blk, new_blk,
-						    log_blk, start_page,
-						    end_page, buf, &ptr,
-						    &offset);
-			if (retval != STATUS_SUCCESS) {
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-
-		total_sec_cnt -= page_cnt;
-
-		if (total_sec_cnt == 0)
-			break;
-
-		log_blk++;
-		zone_no = (int)(log_blk / 1000);
-		log_off = (u16) (log_blk % 1000);
-
-		if (xd_card->zone[zone_no].build_flag == 0) {
-			retval = xd_build_l2p_tbl(chip, zone_no);
-			if (retval != STATUS_SUCCESS) {
-				chip->card_fail |= XD_CARD;
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_NOT_PRESENT);
-				TRACE_RET(chip, retval);
-			}
-		}
-
-		old_blk = xd_get_l2p_tbl(chip, zone_no, log_off);
-		if (old_blk == BLK_NOT_FOUND) {
-			if (srb->sc_data_direction == DMA_FROM_DEVICE) {
-				rts51x_set_sense_type(chip, lun,
-					SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
-			} else {
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_WRITE_ERR);
-			}
-			TRACE_RET(chip, STATUS_FAIL);
-		}
-
-		if (srb->sc_data_direction == DMA_TO_DEVICE) {
-			new_blk = xd_get_unused_block(chip, zone_no);
-			if (new_blk == BLK_NOT_FOUND) {
-				rts51x_set_sense_type(chip, lun,
-					       SENSE_TYPE_MEDIA_WRITE_ERR);
-				TRACE_RET(chip, STATUS_FAIL);
-			}
-		}
-
-		start_page = 0;
-	}
-
-	if ((srb->sc_data_direction == DMA_TO_DEVICE) &&
-	    (end_page != (xd_card->page_off + 1))) {
-		delay_write->delay_write_flag = 1;
-		delay_write->old_phyblock = old_blk;
-		delay_write->new_phyblock = new_blk;
-		delay_write->logblock = log_blk;
-		delay_write->pageoff = end_page;
-	}
-
-	scsi_set_resid(srb, 0);
-
-	return STATUS_SUCCESS;
-}
-
-void rts51x_xd_free_l2p_tbl(struct rts51x_chip *chip)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	int i = 0;
-
-	if (xd_card->zone != NULL) {
-		for (i = 0; i < xd_card->zone_cnt; i++) {
-			if (xd_card->zone[i].l2p_table != NULL) {
-				vfree(xd_card->zone[i].l2p_table);
-				xd_card->zone[i].l2p_table = NULL;
-			}
-			if (xd_card->zone[i].free_table != NULL) {
-				vfree(xd_card->zone[i].free_table);
-				xd_card->zone[i].free_table = NULL;
-			}
-		}
-		vfree(xd_card->zone);
-		xd_card->zone = NULL;
-	}
-}
-
-void rts51x_xd_cleanup_work(struct rts51x_chip *chip)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-
-	if (xd_card->delay_write.delay_write_flag) {
-		RTS51X_DEBUGP("xD: delay write\n");
-		xd_delay_write(chip);
-		xd_card->counter = 0;
-	}
-}
-
-static int xd_power_off_card3v3(struct rts51x_chip *chip)
-{
-	int retval;
-
-	rts51x_init_cmd(chip);
-
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_CLK_EN, XD_CLK_EN, 0);
-
-	if (chip->asic_code)
-		xd_pull_ctl_disable(chip);
-	else
-		rts51x_add_cmd(chip, WRITE_REG_CMD, FPGA_PULL_CTL, 0xFF, 0xDF);
-	rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_OE, XD_OUTPUT_EN, 0);
-	if (!chip->option.FT2_fast_mode) {
-		rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL, POWER_MASK,
-			       POWER_OFF);
-		if (CHECK_PKG(chip, LQFP48)
-		    || chip->option.rts5129_D3318_off_enable)
-			rts51x_add_cmd(chip, WRITE_REG_CMD, CARD_PWR_CTL,
-				       DV3318_AUTO_PWR_OFF, 0);
-	}
-
-	retval = rts51x_send_cmd(chip, MODE_C, 100);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	return STATUS_SUCCESS;
-}
-
-int rts51x_release_xd_card(struct rts51x_chip *chip)
-{
-	struct xd_info *xd_card = &(chip->xd_card);
-	int retval;
-
-	RTS51X_DEBUGP("rts51x_release_xd_card\n");
-
-	chip->card_ready &= ~XD_CARD;
-	chip->card_fail &= ~XD_CARD;
-	chip->card_wp &= ~XD_CARD;
-
-	xd_card->delay_write.delay_write_flag = 0;
-
-	rts51x_xd_free_l2p_tbl(chip);
-
-	rts51x_write_register(chip, SFSM_ED, HW_CMD_STOP, HW_CMD_STOP);
-
-	retval = xd_power_off_card3v3(chip);
-	if (retval != STATUS_SUCCESS)
-		TRACE_RET(chip, retval);
-
-	if (chip->asic_code && CHECK_PKG(chip, QFN24))
-		wait_timeout(20);
-
-	return STATUS_SUCCESS;
-}
diff --git a/drivers/staging/rts5139/xd.h b/drivers/staging/rts5139/xd.h
deleted file mode 100644
index 695a0b4d7e5..00000000000
--- a/drivers/staging/rts5139/xd.h
+++ /dev/null
@@ -1,191 +0,0 @@
-/* Driver for Realtek RTS51xx USB card reader
- * Header file
- *
- * Copyright(c) 2009 Realtek Semiconductor Corp. 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, 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/>.
- *
- * Author:
- *   wwang (wei_wang@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- * Maintainer:
- *   Edwin Rong (edwin_rong@realsil.com.cn)
- *   No. 450, Shenhu Road, Suzhou Industry Park, Suzhou, China
- */
-
-#ifndef __RTS51X_XD_H
-#define __RTS51X_XD_H
-
-/* Error Codes */
-#define	XD_NO_ERROR			0x00
-#define	XD_NO_MEMORY			0x80
-#define	XD_PRG_ERROR			0x40
-#define	XD_NO_CARD			0x20
-#define	XD_READ_FAIL			0x10
-#define	XD_ERASE_FAIL			0x08
-#define	XD_WRITE_FAIL			0x04
-#define	XD_ECC_ERROR			0x02
-#define	XD_TO_ERROR			0x01
-
-/* XD Commands */
-#define	READ1_1				0x00
-#define	READ1_2				0x01
-#define	READ2				0x50
-#define READ_ID				0x90
-#define RESET				0xff
-#define PAGE_PRG_1			0x80
-#define PAGE_PRG_2			0x10
-#define	BLK_ERASE_1			0x60
-#define	BLK_ERASE_2			0xD0
-#define READ_STS			0x70
-#define READ_xD_ID			0x9A
-#define	COPY_BACK_512			0x8A
-#define	COPY_BACK_2K			0x85
-#define	READ1_1_2			0x30
-#define	READ1_1_3			0x35
-#define	CHG_DAT_OUT_1			0x05
-#define RDM_DAT_OUT_1			0x05
-#define	CHG_DAT_OUT_2			0xE0
-#define RDM_DAT_OUT_2			0xE0
-#define	CHG_DAT_OUT_2			0xE0
-#define	CHG_DAT_IN_1			0x85
-#define	CACHE_PRG			0x15
-
-/* Redundant Area Related */
-#define XD_EXTRA_SIZE			0x10
-#define XD_2K_EXTRA_SIZE		0x40
-
-/* Define for XD Status  */
-#define	NOT_WRITE_PROTECTED		0x80
-#define	READY_STATE			0x40
-#define	PROGRAM_ERROR			0x01
-#define	PROGRAM_ERROR_N_1		0x02
-#define	INTERNAL_READY			0x20
-#define	READY_FLAG			0x5F
-
-/* Define for device code */
-#define	XD_8M_X8_512			0xE6
-#define	XD_16M_X8_512			0x73
-#define	XD_32M_X8_512			0x75
-#define	XD_64M_X8_512			0x76
-#define	XD_128M_X8_512			0x79
-#define	XD_256M_X8_512			0x71
-#define	XD_128M_X8_2048			0xF1
-#define	XD_256M_X8_2048			0xDA
-#define	XD_512M_X8			0xDC
-#define	XD_128M_X16_2048		0xC1
-#define	XD_4M_X8_512_1			0xE3
-#define	XD_4M_X8_512_2			0xE5
-#define	xD_1G_X8_512			0xD3
-#define	xD_2G_X8_512			0xD5
-
-#define	XD_ID_CODE			0xB5
-
-#define	VENDOR_BLOCK			0xEFFF
-#define	CIS_BLOCK			0xDFFF
-
-#define	BLK_NOT_FOUND			0xFFFFFFFF
-
-#define	NO_NEW_BLK			0xFFFFFFFF
-
-#define	PAGE_CORRECTABLE		0x0
-#define	PAGE_NOTCORRECTABLE		0x1
-
-#define	NO_OFFSET			0x0
-#define	WITH_OFFSET			0x1
-
-#define	Sect_Per_Page			4
-#define	XD_ADDR_MODE_2C			XD_ADDR_MODE_2A
-
-#define ZONE0_BAD_BLOCK			23
-#define NOT_ZONE0_BAD_BLOCK		24
-
-/* Assign address mode */
-#define	XD_RW_ADDR			0x01
-#define	XD_ERASE_ADDR			0x02
-
-/* Macro Definition */
-#define	XD_PAGE_512(xd_card)		\
-	do {	\
-		(xd_card)->block_shift = 5;	\
-		(xd_card)->page_off = 0x1F;	\
-	} while (0)
-
-#define	XD_SET_BAD_NEWBLK(xd_card)	((xd_card)->multi_flag |= 0x01)
-#define	XD_CLR_BAD_NEWBLK(xd_card)	((xd_card)->multi_flag &= ~0x01)
-#define	XD_CHK_BAD_NEWBLK(xd_card)	((xd_card)->multi_flag & 0x01)
-
-#define	XD_SET_BAD_OLDBLK(xd_card)	((xd_card)->multi_flag |= 0x02)
-#define	XD_CLR_BAD_OLDBLK(xd_card)	((xd_card)->multi_flag &= ~0x02)
-#define	XD_CHK_BAD_OLDBLK(xd_card)	((xd_card)->multi_flag & 0x02)
-
-#define	XD_SET_MBR_FAIL(xd_card)	((xd_card)->multi_flag |= 0x04)
-#define	XD_CLR_MBR_FAIL(xd_card)	((xd_card)->multi_flag &= ~0x04)
-#define	XD_CHK_MBR_FAIL(xd_card)	((xd_card)->multi_flag & 0x04)
-
-#define	XD_SET_ECC_FLD_ERR(xd_card)	((xd_card)->multi_flag |= 0x08)
-#define	XD_CLR_ECC_FLD_ERR(xd_card)	((xd_card)->multi_flag &= ~0x08)
-#define	XD_CHK_ECC_FLD_ERR(xd_card)	((xd_card)->multi_flag & 0x08)
-
-#define	XD_SET_4MB(xd_card)		((xd_card)->multi_flag |= 0x10)
-#define	XD_CLR_4MB(xd_card)		((xd_card)->multi_flag &= ~0x10)
-#define	XD_CHK_4MB(xd_card)		((xd_card)->multi_flag & 0x10)
-
-#define	XD_SET_ECC_ERR(xd_card)		((xd_card)->multi_flag |= 0x40)
-#define	XD_CLR_ECC_ERR(xd_card)		((xd_card)->multi_flag &= ~0x40)
-#define	XD_CHK_ECC_ERR(xd_card)		((xd_card)->multi_flag & 0x40)
-
-/* Offset in xD redundant buffer */
-#define PAGE_STATUS		0
-#define BLOCK_STATUS		1
-#define BLOCK_ADDR1_L		2
-#define BLOCK_ADDR1_H		3
-#define BLOCK_ADDR2_L		4
-#define BLOCK_ADDR2_H		5
-#define RESERVED0		6
-#define RESERVED1		7
-#define RESERVED2		8
-#define RESERVED3		9
-#define PARITY			10
-
-/* For CIS block */
-#define	CIS0_0			0
-#define	CIS0_1			1
-#define	CIS0_2			2
-#define	CIS0_3			3
-#define	CIS0_4			4
-#define	CIS0_5			5
-#define	CIS0_6			6
-#define	CIS0_7			7
-#define	CIS0_8			8
-#define	CIS0_9			9
-#define	CIS1_0			256
-#define	CIS1_1			(256 + 1)
-#define	CIS1_2			(256 + 2)
-#define	CIS1_3			(256 + 3)
-#define	CIS1_4			(256 + 4)
-#define	CIS1_5			(256 + 5)
-#define	CIS1_6			(256 + 6)
-#define	CIS1_7			(256 + 7)
-#define	CIS1_8			(256 + 8)
-#define	CIS1_9			(256 + 9)
-
-int rts51x_reset_xd_card(struct rts51x_chip *chip);
-int rts51x_xd_rw(struct scsi_cmnd *srb, struct rts51x_chip *chip, u32 start_sector,
-	  u16 sector_cnt);
-void rts51x_xd_free_l2p_tbl(struct rts51x_chip *chip);
-void rts51x_xd_cleanup_work(struct rts51x_chip *chip);
-int rts51x_release_xd_card(struct rts51x_chip *chip);
-
-#endif /* __RTS51X_XD_H */
diff --git a/drivers/staging/rts5208/rtsx.c b/drivers/staging/rts5208/rtsx.c
index d2d1345fb06..c0a0e601037 100644
--- a/drivers/staging/rts5208/rtsx.c
+++ b/drivers/staging/rts5208/rtsx.c
@@ -1030,7 +1030,7 @@ static void rtsx_remove(struct pci_dev *pci)
 }
 
 /* PCI IDs */
-static DEFINE_PCI_DEVICE_TABLE(rtsx_ids) = {
+static const struct pci_device_id rtsx_ids[] = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_REALTEK, 0x5208),
 		PCI_CLASS_OTHERS << 16, 0xFF0000 },
 	{ PCI_DEVICE(PCI_VENDOR_ID_REALTEK, 0x5288),
diff --git a/drivers/staging/rts5208/rtsx_chip.c b/drivers/staging/rts5208/rtsx_chip.c
index 6426807a906..7907e931a35 100644
--- a/drivers/staging/rts5208/rtsx_chip.c
+++ b/drivers/staging/rts5208/rtsx_chip.c
@@ -104,7 +104,7 @@ void rtsx_enable_bus_int(struct rtsx_chip *chip)
 	if (chip->ic_version >= IC_VER_C)
 		reg |= DELINK_INT_EN;
 #ifdef SUPPORT_OCP
-		reg |= OC_INT_EN;
+	reg |= OC_INT_EN;
 #endif
 	if (!chip->adma_mode)
 		reg |= DATA_DONE_INT_EN;
diff --git a/drivers/staging/sbe-2t3e3/2t3e3.h b/drivers/staging/sbe-2t3e3/2t3e3.h
deleted file mode 100644
index e7bf721f3fd..00000000000
--- a/drivers/staging/sbe-2t3e3/2t3e3.h
+++ /dev/null
@@ -1,889 +0,0 @@
-/*
- * SBE 2T3E3 synchronous serial card driver for Linux
- *
- * Copyright (C) 2009-2010 Krzysztof Halasa <khc@pm.waw.pl>
- *
- * 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.
- *
- * This code is based on a driver written by SBE Inc.
- */
-
-#ifndef T3E3_H
-#define T3E3_H
-
-#include <linux/hdlc.h>
-#include <linux/interrupt.h>
-#include <linux/netdevice.h>
-#include <linux/pci.h>
-#include <linux/io.h>
-#include "ctrl.h"
-
-/**************************************************************
- *  21143
- **************************************************************/
-
-/* CSR */
-#define SBE_2T3E3_21143_REG_BUS_MODE					0
-#define SBE_2T3E3_21143_REG_TRANSMIT_POLL_DEMAND			1
-#define SBE_2T3E3_21143_REG_RECEIVE_POLL_DEMAND				2
-#define SBE_2T3E3_21143_REG_RECEIVE_LIST_BASE_ADDRESS			3
-#define SBE_2T3E3_21143_REG_TRANSMIT_LIST_BASE_ADDRESS			4
-#define SBE_2T3E3_21143_REG_STATUS					5
-#define SBE_2T3E3_21143_REG_OPERATION_MODE				6
-#define SBE_2T3E3_21143_REG_INTERRUPT_ENABLE				7
-#define SBE_2T3E3_21143_REG_MISSED_FRAMES_AND_OVERFLOW_COUNTER		8
-#define SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT	9
-#define SBE_2T3E3_21143_REG_BOOT_ROM_PROGRAMMING_ADDRESS		10
-#define SBE_2T3E3_21143_REG_GENERAL_PURPOSE_TIMER_AND_INTERRUPT_MITIGATION_CONTROL 11
-#define SBE_2T3E3_21143_REG_SIA_STATUS					12
-#define SBE_2T3E3_21143_REG_SIA_CONNECTIVITY				13
-#define SBE_2T3E3_21143_REG_SIA_TRANSMIT_AND_RECEIVE			14
-#define SBE_2T3E3_21143_REG_SIA_AND_GENERAL_PURPOSE_PORT		15
-#define SBE_2T3E3_21143_REG_MAX						16
-
-/* CSR0 - BUS_MODE */
-#define SBE_2T3E3_21143_VAL_WRITE_AND_INVALIDATE_ENABLE		0x01000000
-#define SBE_2T3E3_21143_VAL_READ_LINE_ENABLE			0x00800000
-#define SBE_2T3E3_21143_VAL_READ_MULTIPLE_ENABLE		0x00200000
-#define SBE_2T3E3_21143_VAL_TRANSMIT_AUTOMATIC_POLLING_200us	0x00020000
-#define SBE_2T3E3_21143_VAL_TRANSMIT_AUTOMATIC_POLLING_DISABLED	0x00000000
-#define SBE_2T3E3_21143_VAL_CACHE_ALIGNMENT_32			0x0000c000
-#define SBE_2T3E3_21143_VAL_CACHE_ALIGNMENT_16			0x00008000
-#define SBE_2T3E3_21143_VAL_CACHE_ALIGNMENT_8			0x00004000
-#define SBE_2T3E3_21143_VAL_BUS_ARBITRATION_RR			0x00000002
-#define SBE_2T3E3_21143_VAL_SOFTWARE_RESET			0x00000001
-
-/* CSR5 - STATUS */
-#define SBE_2T3E3_21143_VAL_GENERAL_PURPOSE_PORT_INTERRUPT	0x04000000
-#define SBE_2T3E3_21143_VAL_ERROR_BITS				0x03800000
-#define SBE_2T3E3_21143_VAL_PARITY_ERROR			0x00000000
-#define SBE_2T3E3_21143_VAL_MASTER_ABORT			0x00800000
-#define SBE_2T3E3_21143_VAL_TARGET_ABORT			0x01000000
-#define SBE_2T3E3_21143_VAL_TRANSMISSION_PROCESS_STATE		0x00700000
-#define SBE_2T3E3_21143_VAL_TX_STOPPED				0x00000000
-#define SBE_2T3E3_21143_VAL_TX_SUSPENDED			0x00600000
-#define SBE_2T3E3_21143_VAL_RECEIVE_PROCESS_STATE		0x000e0000
-#define SBE_2T3E3_21143_VAL_RX_STOPPED				0x00000000
-#define SBE_2T3E3_21143_VAL_RX_SUSPENDED			0x000a0000
-#define SBE_2T3E3_21143_VAL_NORMAL_INTERRUPT_SUMMARY		0x00010000
-#define SBE_2T3E3_21143_VAL_ABNORMAL_INTERRUPT_SUMMARY		0x00008000
-#define SBE_2T3E3_21143_VAL_EARLY_RECEIVE_INTERRUPT		0x00004000
-#define SBE_2T3E3_21143_VAL_FATAL_BUS_ERROR			0x00002000
-#define SBE_2T3E3_21143_VAL_GENERAL_PURPOSE_TIMER_EXPIRED	0x00000800
-#define SBE_2T3E3_21143_VAL_EARLY_TRANSMIT_INTERRUPT		0x00000400
-#define SBE_2T3E3_21143_VAL_RECEIVE_WATCHDOG_TIMEOUT		0x00000200
-#define SBE_2T3E3_21143_VAL_RECEIVE_PROCESS_STOPPED		0x00000100
-#define SBE_2T3E3_21143_VAL_RECEIVE_BUFFER_UNAVAILABLE		0x00000080
-#define SBE_2T3E3_21143_VAL_RECEIVE_INTERRUPT			0x00000040
-#define SBE_2T3E3_21143_VAL_TRANSMIT_UNDERFLOW			0x00000020
-#define SBE_2T3E3_21143_VAL_TRANSMIT_JABBER_TIMEOUT		0x00000008
-#define SBE_2T3E3_21143_VAL_TRANSMIT_BUFFER_UNAVAILABLE		0x00000004
-#define SBE_2T3E3_21143_VAL_TRANSMIT_PROCESS_STOPPED		0x00000002
-#define SBE_2T3E3_21143_VAL_TRANSMIT_INTERRUPT			0x00000001
-
-/* CSR6 - OPERATION_MODE */
-#define SBE_2T3E3_21143_VAL_SPECIAL_CAPTURE_EFFECT_ENABLE	0x80000000
-#define SBE_2T3E3_21143_VAL_RECEIVE_ALL				0x40000000
-#define SBE_2T3E3_21143_VAL_MUST_BE_ONE				0x02000000
-#define SBE_2T3E3_21143_VAL_SCRAMBLER_MODE			0x01000000
-#define SBE_2T3E3_21143_VAL_PCS_FUNCTION			0x00800000
-#define SBE_2T3E3_21143_VAL_TRANSMIT_THRESHOLD_MODE_10Mbs	0x00400000
-#define SBE_2T3E3_21143_VAL_TRANSMIT_THRESHOLD_MODE_100Mbs	0x00000000
-#define SBE_2T3E3_21143_VAL_STORE_AND_FORWARD			0x00200000
-#define SBE_2T3E3_21143_VAL_HEARTBEAT_DISABLE			0x00080000
-#define SBE_2T3E3_21143_VAL_PORT_SELECT				0x00040000
-#define SBE_2T3E3_21143_VAL_CAPTURE_EFFECT_ENABLE		0x00020000
-#define SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS		0x0000c000
-#define SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS_1		0x00000000
-#define SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS_2		0x00004000
-#define SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS_3		0x00008000
-#define SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS_4		0x0000c000
-#define SBE_2T3E3_21143_VAL_TRANSMISSION_START			0x00002000
-#define SBE_2T3E3_21143_VAL_OPERATING_MODE			0x00000c00
-#define SBE_2T3E3_21143_VAL_LOOPBACK_OFF			0x00000000
-#define SBE_2T3E3_21143_VAL_LOOPBACK_EXTERNAL			0x00000800
-#define SBE_2T3E3_21143_VAL_LOOPBACK_INTERNAL			0x00000400
-#define SBE_2T3E3_21143_VAL_FULL_DUPLEX_MODE			0x00000200
-#define SBE_2T3E3_21143_VAL_PASS_ALL_MULTICAST			0x00000080
-#define SBE_2T3E3_21143_VAL_PROMISCUOUS_MODE			0x00000040
-#define SBE_2T3E3_21143_VAL_PASS_BAD_FRAMES			0x00000008
-#define SBE_2T3E3_21143_VAL_RECEIVE_START			0x00000002
-
-/* CSR7 - INTERRUPT_ENABLE */
-#define SBE_2T3E3_21143_VAL_LINK_CHANGED_ENABLE			0x08000000
-#define SBE_2T3E3_21143_VAL_GENERAL_PURPOSE_PORT_ENABLE		0x04000000
-#define SBE_2T3E3_21143_VAL_NORMAL_INTERRUPT_SUMMARY_ENABLE	0x00010000
-#define SBE_2T3E3_21143_VAL_ABNORMAL_INTERRUPT_SUMMARY_ENABLE	0x00008000
-#define SBE_2T3E3_21143_VAL_EARLY_RECEIVE_INTERRUPT_ENABLE	0x00004000
-#define SBE_2T3E3_21143_VAL_FATAL_BUS_ERROR_ENABLE		0x00002000
-#define SBE_2T3E3_21143_VAL_LINK_FAIL_ENABLE			0x00001000
-#define SBE_2T3E3_21143_VAL_GENERAL_PURPOSE_TIMER_ENABLE	0x00000800
-#define SBE_2T3E3_21143_VAL_EARLY_TRANSMIT_INTERRUPT_ENABLE	0x00000400
-#define SBE_2T3E3_21143_VAL_RECEIVE_WATCHDOG_TIMEOUT_ENABLE	0x00000200
-#define SBE_2T3E3_21143_VAL_RECEIVE_STOPPED_ENABLE		0x00000100
-#define SBE_2T3E3_21143_VAL_RECEIVE_BUFFER_UNAVAILABLE_ENABLE	0x00000080
-#define SBE_2T3E3_21143_VAL_RECEIVE_INTERRUPT_ENABLE		0x00000040
-#define SBE_2T3E3_21143_VAL_TRANSMIT_UNDERFLOW_INTERRUPT_ENABLE	0x00000020
-#define SBE_2T3E3_21143_VAL_TRANSMIT_JABBER_TIMEOUT_ENABLE	0x00000008
-#define SBE_2T3E3_21143_VAL_TRANSMIT_BUFFER_UNAVAILABLE_ENABLE	0x00000004
-#define SBE_2T3E3_21143_VAL_TRANSMIT_STOPPED_ENABLE		0x00000002
-#define SBE_2T3E3_21143_VAL_TRANSMIT_INTERRUPT_ENABLE		0x00000001
-
-/* CSR8 - MISSED_FRAMES_AND_OVERFLOW_COUNTER */
-#define SBE_2T3E3_21143_VAL_OVERFLOW_COUNTER_OVERFLOW		0x10000000
-#define SBE_2T3E3_21143_VAL_OVERFLOW_COUNTER			0x0ffe0000
-#define SBE_2T3E3_21143_VAL_MISSED_FRAME_OVERFLOW		0x00010000
-#define SBE_2T3E3_21143_VAL_MISSED_FRAMES_COUNTER		0x0000ffff
-
-/* CSR9 - BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT */
-#define SBE_2T3E3_21143_VAL_MII_MANAGEMENT_DATA_IN		0x00080000
-#define SBE_2T3E3_21143_VAL_MII_MANAGEMENT_READ_MODE		0x00040000
-#define SBE_2T3E3_21143_VAL_MII_MANAGEMENT_DATA_OUT		0x00020000
-#define SBE_2T3E3_21143_VAL_MII_MANAGEMENT_CLOCK		0x00010000
-#define SBE_2T3E3_21143_VAL_READ_OPERATION			0x00004000
-#define SBE_2T3E3_21143_VAL_WRITE_OPERATION			0x00002000
-#define SBE_2T3E3_21143_VAL_BOOT_ROM_SELECT			0x00001000
-#define SBE_2T3E3_21143_VAL_SERIAL_ROM_SELECT			0x00000800
-#define SBE_2T3E3_21143_VAL_BOOT_ROM_DATA			0x000000ff
-#define SBE_2T3E3_21143_VAL_SERIAL_ROM_DATA_OUT			0x00000008
-#define SBE_2T3E3_21143_VAL_SERIAL_ROM_DATA_IN			0x00000004
-#define SBE_2T3E3_21143_VAL_SERIAL_ROM_CLOCK			0x00000002
-#define SBE_2T3E3_21143_VAL_SERIAL_ROM_CHIP_SELECT		0x00000001
-
-/* CSR11 - GENERAL_PURPOSE_TIMER_AND_INTERRUPT_MITIGATION_CONTROL */
-#define SBE_2T3E3_21143_VAL_CYCLE_SIZE				0x80000000
-#define SBE_2T3E3_21143_VAL_TRANSMIT_TIMER			0x78000000
-#define SBE_2T3E3_21143_VAL_NUMBER_OF_TRANSMIT_PACKETS		0x07000000
-#define SBE_2T3E3_21143_VAL_RECEIVE_TIMER			0x00f00000
-#define SBE_2T3E3_21143_VAL_NUMBER_OF_RECEIVE_PACKETS		0x000e0000
-#define SBE_2T3E3_21143_VAL_CONTINUOUS_MODE			0x00010000
-#define SBE_2T3E3_21143_VAL_TIMER_VALUE				0x0000ffff
-
-/* CSR12 - SIA_STATUS */
-#define SBE_2T3E3_21143_VAL_10BASE_T_RECEIVE_PORT_ACTIVITY	0x00000200
-#define SBE_2T3E3_21143_VAL_AUI_RECEIVE_PORT_ACTIVITY		0x00000100
-#define SBE_2T3E3_21143_VAL_10Mbs_LINK_STATUS			0x00000004
-#define SBE_2T3E3_21143_VAL_100Mbs_LINK_STATUS			0x00000002
-#define SBE_2T3E3_21143_VAL_MII_RECEIVE_PORT_ACTIVITY		0x00000001
-
-/* CSR13 - SIA_CONNECTIVITY */
-#define SBE_2T3E3_21143_VAL_10BASE_T_OR_AUI			0x00000008
-#define SBE_2T3E3_21143_VAL_SIA_RESET				0x00000001
-
-/* CSR14 - SIA_TRANSMIT_AND_RECEIVE */
-#define SBE_2T3E3_21143_VAL_100BASE_TX_FULL_DUPLEX		0x00020000
-#define SBE_2T3E3_21143_VAL_COLLISION_DETECT_ENABLE		0x00000400
-#define SBE_2T3E3_21143_VAL_COLLISION_SQUELCH_ENABLE		0x00000200
-#define SBE_2T3E3_21143_VAL_RECEIVE_SQUELCH_ENABLE		0x00000100
-#define SBE_2T3E3_21143_VAL_LINK_PULSE_SEND_ENABLE		0x00000004
-#define SBE_2T3E3_21143_VAL_ENCODER_ENABLE			0x00000001
-
-/* CSR15 - SIA_AND_GENERAL_PURPOSE_PORT */
-#define SBE_2T3E3_21143_VAL_RECEIVE_WATCHDOG_DISABLE		0x00000010
-#define SBE_2T3E3_21143_VAL_AUI_BNC_MODE			0x00000008
-#define SBE_2T3E3_21143_VAL_HOST_UNJAB				0x00000002
-#define SBE_2T3E3_21143_VAL_JABBER_DISABLE			0x00000001
-
-/**************************************************************
- *  CPLD
- **************************************************************/
-
-/* reg_map indexes */
-#define SBE_2T3E3_CPLD_REG_PCRA				0
-#define SBE_2T3E3_CPLD_REG_PCRB				1
-#define SBE_2T3E3_CPLD_REG_PLCR				2
-#define SBE_2T3E3_CPLD_REG_PLTR				3
-#define SBE_2T3E3_CPLD_REG_PPFR				4
-#define SBE_2T3E3_CPLD_REG_BOARD_ID			5
-#define SBE_2T3E3_CPLD_REG_FPGA_VERSION			6
-#define SBE_2T3E3_CPLD_REG_FRAMER_BASE_ADDRESS		7
-#define SBE_2T3E3_CPLD_REG_SERIAL_CHIP_SELECT		8
-#define SBE_2T3E3_CPLD_REG_STATIC_RESET			9
-#define SBE_2T3E3_CPLD_REG_PULSE_RESET			10
-#define SBE_2T3E3_CPLD_REG_FPGA_RECONFIGURATION		11
-#define SBE_2T3E3_CPLD_REG_LEDR				12
-#define SBE_2T3E3_CPLD_REG_PICSR			13
-#define SBE_2T3E3_CPLD_REG_PIER				14
-#define SBE_2T3E3_CPLD_REG_PCRC				15
-#define SBE_2T3E3_CPLD_REG_PBWF				16
-#define SBE_2T3E3_CPLD_REG_PBWL				17
-
-#define SBE_2T3E3_CPLD_REG_MAX				18
-
-/**********/
-
-/* val_map indexes */
-#define SBE_2T3E3_CPLD_VAL_LIU_SELECT			0
-#define SBE_2T3E3_CPLD_VAL_DAC_SELECT			1
-#define SBE_2T3E3_CPLD_VAL_LOOP_TIMING_SOURCE		2
-#define SBE_2T3E3_CPLD_VAL_LIU_FRAMER_RESET		3
-
-/* PCRA */
-#define SBE_2T3E3_CPLD_VAL_CRC32				0x40
-#define SBE_2T3E3_CPLD_VAL_TRANSPARENT_MODE			0x20
-#define SBE_2T3E3_CPLD_VAL_REAR_PANEL				0x10
-#define SBE_2T3E3_CPLD_VAL_RAW_MODE				0x08
-#define SBE_2T3E3_CPLD_VAL_ALT					0x04
-#define SBE_2T3E3_CPLD_VAL_LOOP_TIMING				0x02
-#define SBE_2T3E3_CPLD_VAL_LOCAL_CLOCK_E3			0x01
-
-/* PCRB */
-#define SBE_2T3E3_CPLD_VAL_PAD_COUNT				0x30
-#define SBE_2T3E3_CPLD_VAL_PAD_COUNT_1				0x00
-#define SBE_2T3E3_CPLD_VAL_PAD_COUNT_2				0x10
-#define SBE_2T3E3_CPLD_VAL_PAD_COUNT_3				0x20
-#define SBE_2T3E3_CPLD_VAL_PAD_COUNT_4				0x30
-#define SBE_2T3E3_CPLD_VAL_SCRAMBLER_TYPE			0x02
-#define SBE_2T3E3_CPLD_VAL_SCRAMBLER_ENABLE			0x01
-
-/* PCRC */
-#define SBE_2T3E3_CPLD_VAL_FRACTIONAL_MODE_NONE			0x00
-#define SBE_2T3E3_CPLD_VAL_FRACTIONAL_MODE_0			0x01
-#define SBE_2T3E3_CPLD_VAL_FRACTIONAL_MODE_1			0x11
-#define SBE_2T3E3_CPLD_VAL_FRACTIONAL_MODE_2			0x21
-
-/* PLTR */
-#define SBE_2T3E3_CPLD_VAL_LCV_COUNTER				0xff
-
-/* SCSR */
-#define SBE_2T3E3_CPLD_VAL_EEPROM_SELECT			0x10
-
-/* PICSR */
-#define SBE_2T3E3_CPLD_VAL_LOSS_OF_SIGNAL_THRESHOLD_LEVEL_1	0x80
-#define SBE_2T3E3_CPLD_VAL_RECEIVE_LOSS_OF_SIGNAL_CHANGE	0x40
-#define SBE_2T3E3_CPLD_VAL_INTERRUPT_FROM_ETHERNET_ASSERTED	0x20
-#define SBE_2T3E3_CPLD_VAL_INTERRUPT_FROM_FRAMER_ASSERTED	0x10
-#define SBE_2T3E3_CPLD_VAL_LCV_LIMIT_EXCEEDED			0x08
-#define SBE_2T3E3_CPLD_VAL_DMO_SIGNAL_DETECTED			0x04
-#define SBE_2T3E3_CPLD_VAL_RECEIVE_LOSS_OF_LOCK_DETECTED	0x02
-#define SBE_2T3E3_CPLD_VAL_RECEIVE_LOSS_OF_SIGNAL_DETECTED	0x01
-
-/* PIER */
-#define SBE_2T3E3_CPLD_VAL_RECEIVE_LOS_CHANGE_ENABLE		0x40
-#define SBE_2T3E3_CPLD_VAL_INTERRUPT_FROM_ETHERNET_ENABLE	0x20
-#define SBE_2T3E3_CPLD_VAL_INTERRUPT_FROM_FRAMER_ENABLE		0x10
-#define SBE_2T3E3_CPLD_VAL_LCV_INTERRUPT_ENABLE			0x08
-#define SBE_2T3E3_CPLD_VAL_DMO_ENABLE				0x04
-#define SBE_2T3E3_CPLD_VAL_RECEIVE_LOSS_OF_LOCK_ENABLE		0x02
-#define SBE_2T3E3_CPLD_VAL_RECEIVE_LOSS_OF_SIGNAL_ENABLE	0x01
-
-/**************************************************************
- *  Framer
- **************************************************************/
-
-/* reg_map indexes */
-/* common */
-#define SBE_2T3E3_FRAMER_REG_OPERATING_MODE				0
-#define SBE_2T3E3_FRAMER_REG_IO_CONTROL					1
-#define SBE_2T3E3_FRAMER_REG_BLOCK_INTERRUPT_ENABLE			2
-#define SBE_2T3E3_FRAMER_REG_BLOCK_INTERRUPT_STATUS			3
-#define SBE_2T3E3_FRAMER_REG_PMON_LCV_EVENT_COUNT_MSB			28
-#define SBE_2T3E3_FRAMER_REG_PMON_LCV_EVENT_COUNT_LSB			29
-#define SBE_2T3E3_FRAMER_REG_PMON_FRAMING_BIT_ERROR_EVENT_COUNT_MSB	30
-#define SBE_2T3E3_FRAMER_REG_PMON_FRAMING_BIT_ERROR_EVENT_COUNT_LSB	31
-#define SBE_2T3E3_FRAMER_REG_PMON_PARITY_ERROR_EVENT_COUNT_MSB		32
-#define SBE_2T3E3_FRAMER_REG_PMON_PARITY_ERROR_EVENT_COUNT_LSB		33
-#define SBE_2T3E3_FRAMER_REG_PMON_FEBE_EVENT_COUNT_MSB			34
-#define SBE_2T3E3_FRAMER_REG_PMON_FEBE_EVENT_COUNT_LSB			35
-#define SBE_2T3E3_FRAMER_REG_PMON_CP_BIT_ERROR_EVENT_COUNT_MSB		36
-#define SBE_2T3E3_FRAMER_REG_PMON_CP_BIT_ERROR_EVENT_COUNT_LSB		37
-#define SBE_2T3E3_FRAMER_REG_PMON_HOLDING_REGISTER			38
-#define SBE_2T3E3_FRAMER_REG_ONE_SECOND_ERROR_STATUS			39
-#define SBE_2T3E3_FRAMER_REG_LCV_ONE_SECOND_ACCUMULATOR_MSB		40
-#define SBE_2T3E3_FRAMER_REG_LCV_ONE_SECOND_ACCUMULATOR_LSB		41
-#define SBE_2T3E3_FRAMER_REG_FRAME_PARITY_ERROR_ONE_SECOND_ACCUMULATOR_MSB  42
-#define SBE_2T3E3_FRAMER_REG_FRAME_PARITY_ERROR_ONE_SECOND_ACCUMULATOR_LSB  43
-#define SBE_2T3E3_FRAMER_REG_FRAME_CP_BIT_ERROR_ONE_SECOND_ACCUMULATOR_MSB  44
-#define SBE_2T3E3_FRAMER_REG_FRAME_CP_BIT_ERROR_ONE_SECOND_ACCUMULATOR_LSB  45
-#define SBE_2T3E3_FRAMER_REG_LINE_INTERFACE_DRIVE			46
-#define SBE_2T3E3_FRAMER_REG_LINE_INTERFACE_SCAN			47
-
-/* T3 */
-#define SBE_2T3E3_FRAMER_REG_T3_RX_CONFIGURATION_STATUS			4
-#define SBE_2T3E3_FRAMER_REG_T3_RX_STATUS				5
-#define SBE_2T3E3_FRAMER_REG_T3_RX_INTERRUPT_ENABLE			6
-#define SBE_2T3E3_FRAMER_REG_T3_RX_INTERRUPT_STATUS			7
-#define SBE_2T3E3_FRAMER_REG_T3_RX_SYNC_DETECT_ENABLE			8
-#define SBE_2T3E3_FRAMER_REG_T3_RX_FEAC					10
-#define SBE_2T3E3_FRAMER_REG_T3_RX_FEAC_INTERRUPT_ENABLE_STATUS		11
-#define SBE_2T3E3_FRAMER_REG_T3_RX_LAPD_CONTROL				12
-#define SBE_2T3E3_FRAMER_REG_T3_RX_LAPD_STATUS				13
-#define SBE_2T3E3_FRAMER_REG_T3_TX_CONFIGURATION			16
-#define SBE_2T3E3_FRAMER_REG_T3_TX_FEAC_CONFIGURATION_STATUS		17
-#define SBE_2T3E3_FRAMER_REG_T3_TX_FEAC					18
-#define SBE_2T3E3_FRAMER_REG_T3_TX_LAPD_CONFIGURATION			19
-#define SBE_2T3E3_FRAMER_REG_T3_TX_LAPD_STATUS				20
-#define SBE_2T3E3_FRAMER_REG_T3_TX_MBIT_MASK				21
-#define SBE_2T3E3_FRAMER_REG_T3_TX_FBIT_MASK				22
-#define SBE_2T3E3_FRAMER_REG_T3_TX_FBIT_MASK_2				23
-#define SBE_2T3E3_FRAMER_REG_T3_TX_FBIT_MASK_3				24
-
-/* E3 */
-#define SBE_2T3E3_FRAMER_REG_E3_RX_CONFIGURATION_STATUS_1		4
-#define SBE_2T3E3_FRAMER_REG_E3_RX_CONFIGURATION_STATUS_2		5
-#define SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_ENABLE_1			6
-#define SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_ENABLE_2			7
-#define SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_STATUS_1			8
-#define SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_STATUS_2			9
-#define SBE_2T3E3_FRAMER_REG_E3_RX_LAPD_CONTROL				12
-#define SBE_2T3E3_FRAMER_REG_E3_RX_LAPD_STATUS				13
-#define SBE_2T3E3_FRAMER_REG_E3_RX_NR_BYTE				14
-#define SBE_2T3E3_FRAMER_REG_E3_RX_SERVICE_BITS				14
-#define SBE_2T3E3_FRAMER_REG_E3_RX_GC_BYTE				15
-#define SBE_2T3E3_FRAMER_REG_E3_TX_CONFIGURATION			16
-#define SBE_2T3E3_FRAMER_REG_E3_TX_LAPD_CONFIGURATION			19
-#define SBE_2T3E3_FRAMER_REG_E3_TX_LAPD_STATUS				19
-#define SBE_2T3E3_FRAMER_REG_E3_TX_GC_BYTE				21
-#define SBE_2T3E3_FRAMER_REG_E3_TX_SERVICE_BITS				21
-#define SBE_2T3E3_FRAMER_REG_E3_TX_MA_BYTE				22
-#define SBE_2T3E3_FRAMER_REG_E3_TX_NR_BYTE				23
-#define SBE_2T3E3_FRAMER_REG_E3_TX_FA1_ERROR_MASK			25
-#define SBE_2T3E3_FRAMER_REG_E3_TX_FAS_ERROR_MASK_UPPER			25
-#define SBE_2T3E3_FRAMER_REG_E3_TX_FA2_ERROR_MASK			26
-#define SBE_2T3E3_FRAMER_REG_E3_TX_FAS_ERROR_MASK_LOWER			26
-#define SBE_2T3E3_FRAMER_REG_E3_TX_BIP8_MASK				27
-#define SBE_2T3E3_FRAMER_REG_E3_TX_BIP4_MASK				27
-
-#define SBE_2T3E3_FRAMER_REG_MAX					48
-
-/**********/
-
-/* OPERATING_MODE */
-#define SBE_2T3E3_FRAMER_VAL_LOCAL_LOOPBACK_MODE		0x80
-#define SBE_2T3E3_FRAMER_VAL_T3_E3_SELECT			0x40
-#define SBE_2T3E3_FRAMER_VAL_INTERNAL_LOS_ENABLE		0x20
-#define SBE_2T3E3_FRAMER_VAL_RESET				0x10
-#define SBE_2T3E3_FRAMER_VAL_INTERRUPT_ENABLE_RESET		0x08
-#define SBE_2T3E3_FRAMER_VAL_FRAME_FORMAT_SELECT		0x04
-#define SBE_2T3E3_FRAMER_VAL_TIMING_ASYNCH_TXINCLK		0x03
-#define SBE_2T3E3_FRAMER_VAL_E3_G751				0x00
-#define SBE_2T3E3_FRAMER_VAL_E3_G832				0x04
-#define SBE_2T3E3_FRAMER_VAL_T3_CBIT				0x40
-#define SBE_2T3E3_FRAMER_VAL_T3_M13				0x44
-#define SBE_2T3E3_FRAMER_VAL_LOOPBACK_ON			0x80
-#define SBE_2T3E3_FRAMER_VAL_LOOPBACK_OFF			0x00
-
-/* IO_CONTROL */
-#define SBE_2T3E3_FRAMER_VAL_DISABLE_TX_LOSS_OF_CLOCK		0x80
-#define SBE_2T3E3_FRAMER_VAL_LOSS_OF_CLOCK_STATUS		0x40
-#define SBE_2T3E3_FRAMER_VAL_DISABLE_RX_LOSS_OF_CLOCK		0x20
-#define SBE_2T3E3_FRAMER_VAL_AMI_LINE_CODE			0x10
-#define SBE_2T3E3_FRAMER_VAL_UNIPOLAR				0x08
-#define SBE_2T3E3_FRAMER_VAL_TX_LINE_CLOCK_INVERT		0x04
-#define SBE_2T3E3_FRAMER_VAL_RX_LINE_CLOCK_INVERT		0x02
-#define SBE_2T3E3_FRAMER_VAL_REFRAME				0x01
-
-/* BLOCK_INTERRUPT_ENABLE */
-#define SBE_2T3E3_FRAMER_VAL_RX_INTERRUPT_ENABLE		0x80
-#define SBE_2T3E3_FRAMER_VAL_TX_INTERRUPT_ENABLE		0x02
-#define SBE_2T3E3_FRAMER_VAL_ONE_SECOND_INTERRUPT_ENABLE	0x01
-
-/* BLOCK_INTERRUPT_STATUS */
-#define SBE_2T3E3_FRAMER_VAL_RX_INTERRUPT_STATUS		0x80
-#define SBE_2T3E3_FRAMER_VAL_TX_INTERRUPT_STATUS		0x02
-#define SBE_2T3E3_FRAMER_VAL_ONE_SECOND_INTERRUPT_STATUS	0x01
-
-/**********/
-
-/* T3_RX_CONFIGURATION_STATUS */
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_AIS				0x80
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_LOS				0x40
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_IDLE				0x20
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_OOF				0x10
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_FRAMING_ON_PARITY		0x04
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_F_SYNC_ALGO			0x02
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_M_SYNC_ALGO			0x01
-
-/* T3_RX_STATUS */
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_FERF				0x10
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_AIC				0x04
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_FEBE				0x07
-
-/* T3_RX_INTERRUPT_ENABLE */
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_CP_BIT_ERROR_INTERRUPT_ENABLE 0x80
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_LOS_INTERRUPT_ENABLE		0x40
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_AIS_INTERRUPT_ENABLE		0x20
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_IDLE_INTERRUPT_ENABLE	0x10
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_FERF_INTERRUPT_ENABLE	0x08
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_AIC_INTERRUPT_ENABLE		0x04
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_OOF_INTERRUPT_ENABLE		0x02
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_P_BIT_INTERRUPT_ENABLE	0x01
-
-/* T3_RX_INTERRUPT_STATUS */
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_CP_BIT_ERROR_INTERRUPT_STATUS 0x80
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_LOS_INTERRUPT_STATUS		0x40
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_AIS_INTERRUPT_STATUS		0x20
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_IDLE_INTERRUPT_STATUS	0x10
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_FERF_INTERRUPT_STATUS	0x08
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_AIC_INTERRUPT_STATUS		0x04
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_OOF_INTERRUPT_STATUS		0x02
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_P_BIT_INTERRUPT_STATUS	0x01
-
-/* T3_RX_FEAC_INTERRUPT_ENABLE_STATUS */
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_FEAC_VALID			0x10
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_FEAC_REMOVE_INTERRUPT_ENABLE	0x08
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_FEAC_REMOVE_INTERRUPT_STATUS	0x04
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_FEAC_VALID_INTERRUPT_ENABLE	0x02
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_FEAC_VALID_INTERRUPT_STATUS	0x01
-
-/* T3_RX_LAPD_CONTROL */
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_LAPD_ENABLE			0x04
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_LAPD_INTERRUPT_ENABLE	0x02
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_LAPD_INTERRUPT_STATUS	0x01
-
-/* T3_RX_LAPD_STATUS */
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_ABORT			0x40
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_LAPD_TYPE			0x30
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_CR_TYPE			0x08
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_FCS_ERROR			0x04
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_END_OF_MESSAGE		0x02
-#define SBE_2T3E3_FRAMER_VAL_T3_RX_FLAG_PRESENT			0x01
-
-/* T3_TX_CONFIGURATION */
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_YELLOW_ALARM			0x80
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_X_BIT			0x40
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_IDLE				0x20
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_AIS				0x10
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_LOS				0x08
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_FERF_ON_LOS			0x04
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_FERF_ON_OOF			0x02
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_FERF_ON_AIS			0x01
-
-/* T3_TX_FEAC_CONFIGURATION_STATUS */
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_FEAC_INTERRUPT_ENABLE	0x10
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_FEAC_INTERRUPT_STATUS	0x08
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_FEAC_ENABLE			0x04
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_FEAC_GO			0x02
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_FEAC_BUSY			0x01
-
-/* T3_TX_LAPD_STATUS */
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_DL_START			0x08
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_DL_BUSY			0x04
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_LAPD_INTERRUPT_ENABLE	0x02
-#define SBE_2T3E3_FRAMER_VAL_T3_TX_LAPD_INTERRUPT_STATUS	0x01
-
-/**********/
-
-/* E3_RX_CONFIGURATION_STATUS_1 */
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_PAYLOAD_TYPE			0xe0
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_FERF_ALGO			0x10
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_T_MARK_ALGO			0x08
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_PAYLOAD_EXPECTED		0x07
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_BIP4				0x01
-
-/* E3_RX_CONFIGURATION_STATUS_2 */
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_LOF_ALGO			0x80
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_LOF				0x40
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_OOF				0x20
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_LOS				0x10
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_AIS				0x08
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_PAYLOAD_UNSTABLE		0x04
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_T_MARK			0x02
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_FERF				0x01
-
-/* E3_RX_INTERRUPT_ENABLE_1 */
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_COFA_INTERRUPT_ENABLE	0x10
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_OOF_INTERRUPT_ENABLE		0x08
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_LOF_INTERRUPT_ENABLE		0x04
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_LOS_INTERRUPT_ENABLE		0x02
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_AIS_INTERRUPT_ENABLE		0x01
-
-/* E3_RX_INTERRUPT_ENABLE_2 */
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_TTB_CHANGE_INTERRUPT_ENABLE	0x40
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_FEBE_INTERRUPT_ENABLE	0x10
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_FERF_INTERRUPT_ENABLE	0x08
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_BIP8_ERROR_INTERRUPT_ENABLE	0x04
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_BIP4_ERROR_INTERRUPT_ENABLE	0x04
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_FRAMING_BYTE_ERROR_INTERRUPT_ENABLE 0x02
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_PAYLOAD_MISMATCH_INTERRUPT_ENABLE 0x01
-
-/* E3_RX_INTERRUPT_STATUS_1 */
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_COFA_INTERRUPT_STATUS	0x10
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_OOF_INTERRUPT_STATUS		0x08
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_LOF_INTERRUPT_STATUS		0x04
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_LOS_INTERRUPT_STATUS		0x02
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_AIS_INTERRUPT_STATUS		0x01
-
-/* E3_RX_INTERRUPT_STATUS_2 */
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_TTB_CHANGE_INTERRUPT_STATUS	0x40
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_FEBE_INTERRUPT_STATUS	0x10
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_FERF_INTERRUPT_STATUS	0x08
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_BIP8_ERROR_INTERRUPT_STATUS	0x04
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_BIP4_ERROR_INTERRUPT_STATUS	0x04
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_FRAMING_BYTE_ERROR_INTERRUPT_STATUS 0x02
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_PAYLOAD_MISMATCH_INTERRUPT_STATUS 0x01
-
-/* E3_RX_LAPD_CONTROL */
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_DL_FROM_NR			0x08
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_LAPD_ENABLE			0x04
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_LAPD_INTERRUPT_ENABLE	0x02
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_LAPD_INTERRUPT_STATUS	0x01
-
-/* E3_RX_LAPD_STATUS */
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_ABORT			0x40
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_LAPD_TYPE			0x30
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_CR_TYPE			0x08
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_FCS_ERROR			0x04
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_END_OF_MESSAGE		0x02
-#define SBE_2T3E3_FRAMER_VAL_E3_RX_FLAG_PRESENT			0x01
-
-/* E3_TX_CONFIGURATION */
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_BIP4_ENABLE			0x80
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_A_SOURCE_SELECT		0x60
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_DL_IN_NR			0x10
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_N_SOURCE_SELECT		0x18
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_AIS_ENABLE			0x04
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_LOS_ENABLE			0x02
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_MA_RX			0x01
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_FAS_SOURCE_SELECT		0x01
-
-/* E3_TX_LAPD_CONFIGURATION */
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_AUTO_RETRANSMIT		0x08
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_LAPD_MESSAGE_LENGTH		0x02
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_LAPD_ENABLE			0x01
-
-/* E3_TX_LAPD_STATUS_INTERRUPT */
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_DL_START			0x08
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_DL_BUSY			0x04
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_LAPD_INTERRUPT_ENABLE	0x02
-#define SBE_2T3E3_FRAMER_VAL_E3_TX_LAPD_INTERRUPT_STATUS	0x01
-
-
-
-
-
-
-/**************************************************************
- *  LIU
- **************************************************************/
-
-/* reg_map indexes */
-#define SBE_2T3E3_LIU_REG_REG0			0
-#define SBE_2T3E3_LIU_REG_REG1			1
-#define SBE_2T3E3_LIU_REG_REG2			2
-#define SBE_2T3E3_LIU_REG_REG3			3
-#define SBE_2T3E3_LIU_REG_REG4			4
-
-#define	SBE_2T3E3_LIU_REG_MAX			5
-
-/**********/
-
-/* REG0 */
-#define SBE_2T3E3_LIU_VAL_RECEIVE_LOSS_OF_LOCK_STATUS		0x10
-#define SBE_2T3E3_LIU_VAL_RECEIVE_LOSS_OF_SIGNAL_STATUS		0x08
-#define SBE_2T3E3_LIU_VAL_ANALOG_LOSS_OF_SIGNAL_STATUS		0x04
-#define SBE_2T3E3_LIU_VAL_DIGITAL_LOSS_OF_SIGNAL_STATUS		0x02
-#define SBE_2T3E3_LIU_VAL_DMO_STATUS				0x01
-
-/* REG1 */
-#define SBE_2T3E3_LIU_VAL_TRANSMITTER_OFF			0x10
-#define SBE_2T3E3_LIU_VAL_TRANSMIT_ALL_ONES			0x08
-#define SBE_2T3E3_LIU_VAL_TRANSMIT_CLOCK_INVERT			0x04
-#define SBE_2T3E3_LIU_VAL_TRANSMIT_LEVEL_SELECT			0x02
-#define SBE_2T3E3_LIU_VAL_TRANSMIT_BINARY_DATA			0x01
-
-/* REG2 */
-#define SBE_2T3E3_LIU_VAL_DECODER_DISABLE			0x10
-#define SBE_2T3E3_LIU_VAL_ENCODER_DISABLE			0x08
-#define SBE_2T3E3_LIU_VAL_ANALOG_LOSS_OF_SIGNAL_DISABLE		0x04
-#define SBE_2T3E3_LIU_VAL_DIGITAL_LOSS_OF_SIGNAL_DISABLE	0x02
-#define SBE_2T3E3_LIU_VAL_RECEIVE_EQUALIZATION_DISABLE		0x01
-
-/* REG3 */
-#define SBE_2T3E3_LIU_VAL_RECEIVE_BINARY_DATA			0x10
-#define SBE_2T3E3_LIU_VAL_RECOVERED_DATA_MUTING			0x08
-#define SBE_2T3E3_LIU_VAL_RECEIVE_CLOCK_OUTPUT_2		0x04
-#define SBE_2T3E3_LIU_VAL_INVERT_RECEIVE_CLOCK_2		0x02
-#define SBE_2T3E3_LIU_VAL_INVERT_RECEIVE_CLOCK_1		0x01
-
-/* REG4 */
-#define SBE_2T3E3_LIU_VAL_T3_MODE_SELECT			0x00
-#define SBE_2T3E3_LIU_VAL_E3_MODE_SELECT			0x04
-#define SBE_2T3E3_LIU_VAL_LOCAL_LOOPBACK			0x02
-#define SBE_2T3E3_LIU_VAL_REMOTE_LOOPBACK			0x01
-#define SBE_2T3E3_LIU_VAL_LOOPBACK_OFF				0x00
-#define SBE_2T3E3_LIU_VAL_LOOPBACK_REMOTE			0x01
-#define SBE_2T3E3_LIU_VAL_LOOPBACK_ANALOG			0x02
-#define SBE_2T3E3_LIU_VAL_LOOPBACK_DIGITAL			0x03
-
-/**********************************************************************
- *
- * descriptor list and data buffer
- *
- **********************************************************************/
-struct t3e3_rx_desc {
-	u32 rdes0;
-	u32 rdes1;
-	u32 rdes2;
-	u32 rdes3;
-};
-
-#define SBE_2T3E3_RX_DESC_RING_SIZE			64
-
-/* RDES0 */
-#define SBE_2T3E3_RX_DESC_21143_OWN			0X80000000
-#define SBE_2T3E3_RX_DESC_FRAME_LENGTH			0x3fff0000
-#define SBE_2T3E3_RX_DESC_FRAME_LENGTH_SHIFT		16
-#define SBE_2T3E3_RX_DESC_ERROR_SUMMARY			0x00008000
-#define SBE_2T3E3_RX_DESC_DESC_ERROR			0x00004000
-#define SBE_2T3E3_RX_DESC_DATA_TYPE			0x00003000
-#define SBE_2T3E3_RX_DESC_RUNT_FRAME			0x00000800
-#define SBE_2T3E3_RX_DESC_FIRST_DESC			0x00000200
-#define SBE_2T3E3_RX_DESC_LAST_DESC			0x00000100
-#define SBE_2T3E3_RX_DESC_FRAME_TOO_LONG		0x00000080
-#define SBE_2T3E3_RX_DESC_COLLISION_SEEN		0x00000040
-#define SBE_2T3E3_RX_DESC_FRAME_TYPE			0x00000020
-#define SBE_2T3E3_RX_DESC_RECEIVE_WATCHDOG		0x00000010
-#define SBE_2T3E3_RX_DESC_MII_ERROR			0x00000008
-#define SBE_2T3E3_RX_DESC_DRIBBLING_BIT			0x00000004
-#define SBE_2T3E3_RX_DESC_CRC_ERROR			0x00000002
-
-/* RDES1 */
-#define SBE_2T3E3_RX_DESC_END_OF_RING			0x02000000
-#define SBE_2T3E3_RX_DESC_SECOND_ADDRESS_CHAINED	0x01000000
-#define SBE_2T3E3_RX_DESC_BUFFER_2_SIZE			0x003ff800
-#define SBE_2T3E3_RX_DESC_BUFFER_1_SIZE			0x000007ff
-
-/*********************/
-
-struct t3e3_tx_desc {
-	u32 tdes0;
-	u32 tdes1;
-	u32 tdes2;
-	u32 tdes3;
-};
-
-#define SBE_2T3E3_TX_DESC_RING_SIZE			256
-
-/* TDES0 */
-#define SBE_2T3E3_TX_DESC_21143_OWN			0x80000000
-#define SBE_2T3E3_TX_DESC_ERROR_SUMMARY			0x00008000
-#define SBE_2T3E3_TX_DESC_TRANSMIT_JABBER_TIMEOUT	0x00004000
-#define SBE_2T3E3_TX_DESC_LOSS_OF_CARRIER		0x00000800
-#define SBE_2T3E3_TX_DESC_NO_CARRIER			0x00000400
-#define SBE_2T3E3_TX_DESC_LINK_FAIL_REPORT		0x00000004
-#define SBE_2T3E3_TX_DESC_UNDERFLOW_ERROR		0x00000002
-#define SBE_2T3E3_TX_DESC_DEFFERED			0x00000001
-
-/* TDES1 */
-#define SBE_2T3E3_TX_DESC_INTERRUPT_ON_COMPLETION	0x80000000
-#define SBE_2T3E3_TX_DESC_LAST_SEGMENT			0x40000000
-#define SBE_2T3E3_TX_DESC_FIRST_SEGMENT			0x20000000
-#define SBE_2T3E3_TX_DESC_CRC_DISABLE			0x04000000
-#define SBE_2T3E3_TX_DESC_END_OF_RING			0x02000000
-#define SBE_2T3E3_TX_DESC_SECOND_ADDRESS_CHAINED	0x01000000
-#define SBE_2T3E3_TX_DESC_DISABLE_PADDING		0x00800000
-#define SBE_2T3E3_TX_DESC_BUFFER_2_SIZE			0x003ff800
-#define SBE_2T3E3_TX_DESC_BUFFER_1_SIZE			0x000007ff
-
-
-#define SBE_2T3E3_MTU					1600
-#define SBE_2T3E3_CRC16_LENGTH				2
-#define SBE_2T3E3_CRC32_LENGTH				4
-
-#define MCLBYTES (SBE_2T3E3_MTU + 128)
-
-struct channel {
-	struct pci_dev *pdev;
-	struct net_device *dev;
-	struct card *card;
-	unsigned long addr;	/* DECchip */
-
-	int leds;
-
-	/* pci specific */
-	struct {
-		u32 slot;           /* should be 0 or 1 */
-		u32 command;
-		u8 cache_size;
-	} h;
-
-	/* statistics */
-	struct t3e3_stats s;
-
-	/* running */
-	struct {
-		u32 flags;
-	} r;
-
-	/* parameters */
-	struct t3e3_param p;
-
-	u32 liu_regs[SBE_2T3E3_LIU_REG_MAX];	   /* LIU registers */
-	u32 framer_regs[SBE_2T3E3_FRAMER_REG_MAX]; /* Framer registers */
-
-	/* Ethernet Controller */
-	struct {
-		u_int16_t card_serial_number[3];
-
-		u32 reg[SBE_2T3E3_21143_REG_MAX]; /* registers i.e. CSR */
-
-		u32 interrupt_enable_mask;
-
-		/* receive chain/ring */
-		struct t3e3_rx_desc *rx_ring;
-		struct sk_buff *rx_data[SBE_2T3E3_RX_DESC_RING_SIZE];
-		u32 rx_ring_current_read;
-
-		/* transmit chain/ring */
-		struct t3e3_tx_desc *tx_ring;
-		struct sk_buff *tx_data[SBE_2T3E3_TX_DESC_RING_SIZE];
-		u32 tx_ring_current_read;
-		u32 tx_ring_current_write;
-		int tx_full;
-		int tx_free_cnt;
-		spinlock_t tx_lock;
-	} ether;
-
-	int32_t interrupt_active;
-	int32_t rcv_count;
-};
-
-struct card {
-	spinlock_t bootrom_lock;
-	unsigned long bootrom_addr;
-	struct timer_list timer; /* for updating LEDs */
-	struct channel channels[0];
-};
-
-#define SBE_2T3E3_FLAG_NETWORK_UP		0x00000001
-#define SBE_2T3E3_FLAG_NO_ERROR_MESSAGES	0x00000002
-
-extern const u32 cpld_reg_map[][2];
-extern const u32 cpld_val_map[][2];
-extern const u32 t3e3_framer_reg_map[];
-extern const u32 t3e3_liu_reg_map[];
-
-void t3e3_init(struct channel *);
-void t3e3_if_up(struct channel *);
-void t3e3_if_down(struct channel *);
-int t3e3_if_start_xmit(struct sk_buff *skb, struct net_device *dev);
-void t3e3_if_config(struct channel *, u32, char *, struct t3e3_resp *, int *);
-void t3e3_set_frame_type(struct channel *, u32);
-u32 t3e3_eeprom_read_word(struct channel *, u32);
-void t3e3_read_card_serial_number(struct channel *);
-
-/* interrupt handlers */
-irqreturn_t t3e3_intr(int irq, void *dev_instance);
-void dc_intr(struct channel *);
-void dc_intr_rx(struct channel *);
-void dc_intr_tx(struct channel *);
-void dc_intr_tx_underflow(struct channel *);
-void exar7250_intr(struct channel *);
-void exar7250_E3_intr(struct channel *, u32);
-void exar7250_T3_intr(struct channel *, u32);
-
-/* Ethernet controller */
-u32 bootrom_read(struct channel *, u32);
-void bootrom_write(struct channel *, u32, u32);
-void dc_init(struct channel *);
-void dc_start(struct channel *);
-void dc_stop(struct channel *);
-void dc_start_intr(struct channel *);
-void dc_stop_intr(struct channel *);
-void dc_reset(struct channel *);
-void dc_restart(struct channel *);
-void dc_receiver_onoff(struct channel *, u32);
-void dc_transmitter_onoff(struct channel *, u32);
-void dc_set_loopback(struct channel *, u32);
-void dc_clear_descriptor_list(struct channel *);
-void dc_drop_descriptor_list(struct channel *);
-void dc_set_output_port(struct channel *);
-void t3e3_sc_init(struct channel *);
-
-/* CPLD */
-void cpld_init(struct channel *sc);
-u32 cpld_read(struct channel *sc, u32 reg);
-void cpld_set_crc(struct channel *, u32);
-void cpld_start_intr(struct channel *);
-void cpld_stop_intr(struct channel *);
-void cpld_set_clock(struct channel *sc, u32 mode);
-void cpld_set_scrambler(struct channel *, u32);
-void cpld_select_panel(struct channel *, u32);
-void cpld_set_frame_mode(struct channel *, u32);
-void cpld_set_frame_type(struct channel *, u32);
-void cpld_set_pad_count(struct channel *, u32);
-void cpld_set_fractional_mode(struct channel *, u32, u32, u32);
-void cpld_LOS_update(struct channel *);
-
-/* Framer */
-extern u32 exar7250_read(struct channel *, u32);
-extern void exar7250_write(struct channel *, u32, u32);
-void exar7250_init(struct channel *);
-void exar7250_start_intr(struct channel *, u32);
-void exar7250_stop_intr(struct channel *, u32);
-void exar7250_set_frame_type(struct channel *, u32);
-void exar7250_set_loopback(struct channel *, u32);
-void exar7250_unipolar_onoff(struct channel *, u32);
-
-/* LIU */
-u32 exar7300_read(struct channel *, u32);
-void exar7300_write(struct channel *, u32, u32);
-void exar7300_init(struct channel *);
-void exar7300_line_build_out_onoff(struct channel *, u32);
-void exar7300_set_frame_type(struct channel *, u32);
-void exar7300_set_loopback(struct channel *, u32);
-void exar7300_transmit_all_ones_onoff(struct channel *, u32);
-void exar7300_receive_equalization_onoff(struct channel *, u32);
-void exar7300_unipolar_onoff(struct channel *, u32);
-
-void update_led(struct channel *, int);
-int setup_device(struct net_device *dev, struct channel *sc);
-
-static inline int has_two_ports(struct pci_dev *pdev)
-{
-	return pdev->subsystem_device == PCI_SUBDEVICE_ID_SBE_2T3E3_P0;
-}
-
-#define dev_to_priv(dev) (*(struct channel **) ((hdlc_device *)(dev) + 1))
-
-static inline u32 dc_read(unsigned long addr, u32 reg)
-{
-	return inl(addr + (reg << 3));
-}
-
-static inline void dc_write(unsigned long addr, u32 reg, u32 val)
-{
-	outl(val, addr + (reg << 3));
-}
-
-static inline void dc_set_bits(unsigned long addr, u32 reg, u32 bits)
-{
-	dc_write(addr, reg, dc_read(addr, reg) | bits);
-}
-
-static inline void dc_clear_bits(unsigned long addr, u32 reg, u32 bits)
-{
-	dc_write(addr, reg, dc_read(addr, reg) & ~bits);
-}
-
-#define CPLD_MAP_REG(reg, sc)	(cpld_reg_map[(reg)][(sc)->h.slot])
-
-static inline void cpld_write(struct channel *channel, unsigned reg, u32 val)
-{
-	unsigned long flags;
-	spin_lock_irqsave(&channel->card->bootrom_lock, flags);
-	bootrom_write(channel, CPLD_MAP_REG(reg, channel), val);
-	spin_unlock_irqrestore(&channel->card->bootrom_lock, flags);
-}
-
-#define exar7250_set_bit(sc, reg, bit)			\
-	exar7250_write((sc), (reg),			\
-		       exar7250_read(sc, reg) | (bit))
-
-#define exar7250_clear_bit(sc, reg, bit)		\
-	exar7250_write((sc), (reg),			\
-		       exar7250_read(sc, reg) & ~(bit))
-
-#define exar7300_set_bit(sc, reg, bit)			\
-	exar7300_write((sc), (reg),			\
-		       exar7300_read(sc, reg) | (bit))
-
-#define exar7300_clear_bit(sc, reg, bit)		\
-	exar7300_write((sc), (reg),			\
-		       exar7300_read(sc, reg) & ~(bit))
-
-
-#endif /* T3E3_H */
diff --git a/drivers/staging/sbe-2t3e3/Kconfig b/drivers/staging/sbe-2t3e3/Kconfig
deleted file mode 100644
index 8ec86cfc6bf..00000000000
--- a/drivers/staging/sbe-2t3e3/Kconfig
+++ /dev/null
@@ -1,13 +0,0 @@
-config SBE_2T3E3
-	tristate "SBE wanPMC-2T3E3 support"
-	depends on HDLC && PCI
-	help
-	  Driver for wanPMC-2T3E3 cards by SBE Inc.
-
-	  If you have such a card, say Y here and see
-	  <http://www.kernel.org/pub/linux/utils/net/hdlc/>.
-
-	  To compile this as a module, choose M here: the
-	  module will be called sbe-2t3e3.
-
-	  If unsure, say N.
diff --git a/drivers/staging/sbe-2t3e3/Makefile b/drivers/staging/sbe-2t3e3/Makefile
deleted file mode 100644
index cce2a7af626..00000000000
--- a/drivers/staging/sbe-2t3e3/Makefile
+++ /dev/null
@@ -1,4 +0,0 @@
-obj-$(CONFIG_SBE_2T3E3) += sbe-2t3e3.o
-
-sbe-2t3e3-y := module.o netdev.o maps.o	\
-	main.o cpld.o intr.o ctrl.o io.o dc.o exar7250.o exar7300.o
diff --git a/drivers/staging/sbe-2t3e3/TODO b/drivers/staging/sbe-2t3e3/TODO
deleted file mode 100644
index 624b20f70cf..00000000000
--- a/drivers/staging/sbe-2t3e3/TODO
+++ /dev/null
@@ -1,6 +0,0 @@
-TODO:
-	- additional cleaning and tests
-	- wait for the new configuration interface in generic HDLC layer and
-	  when available, convert the driver to it
-
-Please send patches to Krzysztof Halasa <khc@pm.waw.pl>.
\ No newline at end of file
diff --git a/drivers/staging/sbe-2t3e3/cpld.c b/drivers/staging/sbe-2t3e3/cpld.c
deleted file mode 100644
index c6370d3d637..00000000000
--- a/drivers/staging/sbe-2t3e3/cpld.c
+++ /dev/null
@@ -1,351 +0,0 @@
-/*
- * SBE 2T3E3 synchronous serial card driver for Linux
- *
- * Copyright (C) 2009-2010 Krzysztof Halasa <khc@pm.waw.pl>
- *
- * 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.
- *
- * This code is based on a driver written by SBE Inc.
- */
-
-#include <linux/delay.h>
-#include "2t3e3.h"
-#include "ctrl.h"
-
-#define bootrom_set_bit(sc, reg, bit)				\
-	bootrom_write((sc), (reg),				\
-		      bootrom_read((sc), (reg)) | (bit))
-
-#define bootrom_clear_bit(sc, reg, bit)				\
-	bootrom_write((sc), (reg),				\
-		      bootrom_read((sc), (reg)) & ~(bit))
-
-static inline void cpld_set_bit(struct channel *channel, unsigned reg, u32 bit)
-{
-	unsigned long flags;
-	spin_lock_irqsave(&channel->card->bootrom_lock, flags);
-	bootrom_set_bit(channel, CPLD_MAP_REG(reg, channel), bit);
-	spin_unlock_irqrestore(&channel->card->bootrom_lock, flags);
-}
-
-static inline void cpld_clear_bit(struct channel *channel, unsigned reg, u32 bit)
-{
-	unsigned long flags;
-	spin_lock_irqsave(&channel->card->bootrom_lock, flags);
-	bootrom_clear_bit(channel, CPLD_MAP_REG(reg, channel), bit);
-	spin_unlock_irqrestore(&channel->card->bootrom_lock, flags);
-}
-
-void cpld_init(struct channel *sc)
-{
-	u32 val;
-
-	/* PCRA */
-	val = SBE_2T3E3_CPLD_VAL_CRC32 |
-		cpld_val_map[SBE_2T3E3_CPLD_VAL_LOOP_TIMING_SOURCE][sc->h.slot];
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_PCRA, val);
-
-	/* PCRB */
-	val = 0;
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_PCRB, val);
-
-	/* PCRC */
-	val = 0;
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_PCRC, val);
-
-	/* PBWF */
-	val = 0;
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_PBWF, val);
-
-	/* PBWL */
-	val = 0;
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_PBWL, val);
-
-	/* PLTR */
-	val = SBE_2T3E3_CPLD_VAL_LCV_COUNTER;
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_PLTR, val);
-	udelay(1000);
-
-	/* PLCR */
-	val = 0;
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_PLCR, val);
-	udelay(1000);
-
-	/* PPFR */
-	val = 0x55;
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_PPFR, val);
-	/* TODO: this doesn't work!!! */
-
-	/* SERIAL_CHIP_SELECT */
-	val = 0;
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_SERIAL_CHIP_SELECT, val);
-
-	/* PICSR */
-	val = SBE_2T3E3_CPLD_VAL_DMO_SIGNAL_DETECTED |
-		SBE_2T3E3_CPLD_VAL_RECEIVE_LOSS_OF_LOCK_DETECTED |
-		SBE_2T3E3_CPLD_VAL_RECEIVE_LOSS_OF_SIGNAL_DETECTED;
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_PICSR, val);
-
-	cpld_start_intr(sc);
-
-	udelay(1000);
-}
-
-void cpld_start_intr(struct channel *sc)
-{
-	u32 val;
-
-	/* PIER */
-	val = SBE_2T3E3_CPLD_VAL_INTERRUPT_FROM_ETHERNET_ENABLE |
-		SBE_2T3E3_CPLD_VAL_INTERRUPT_FROM_FRAMER_ENABLE;
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_PIER, val);
-}
-
-void cpld_stop_intr(struct channel *sc)
-{
-	u32 val;
-
-	/* PIER */
-	val = 0;
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_PIER, val);
-}
-
-void cpld_set_frame_mode(struct channel *sc, u32 mode)
-{
-	if (sc->p.frame_mode == mode)
-		return;
-
-	switch (mode) {
-	case SBE_2T3E3_FRAME_MODE_HDLC:
-		cpld_clear_bit(sc, SBE_2T3E3_CPLD_REG_PCRA,
-			       SBE_2T3E3_CPLD_VAL_TRANSPARENT_MODE |
-			       SBE_2T3E3_CPLD_VAL_RAW_MODE);
-		exar7250_unipolar_onoff(sc, SBE_2T3E3_OFF);
-		exar7300_unipolar_onoff(sc, SBE_2T3E3_OFF);
-		break;
-	case SBE_2T3E3_FRAME_MODE_TRANSPARENT:
-		cpld_clear_bit(sc, SBE_2T3E3_CPLD_REG_PCRA,
-			       SBE_2T3E3_CPLD_VAL_RAW_MODE);
-		cpld_set_bit(sc, SBE_2T3E3_CPLD_REG_PCRA,
-			     SBE_2T3E3_CPLD_VAL_TRANSPARENT_MODE);
-		exar7250_unipolar_onoff(sc, SBE_2T3E3_OFF);
-		exar7300_unipolar_onoff(sc, SBE_2T3E3_OFF);
-		break;
-	case SBE_2T3E3_FRAME_MODE_RAW:
-		cpld_set_bit(sc, SBE_2T3E3_CPLD_REG_PCRA,
-			     SBE_2T3E3_CPLD_VAL_RAW_MODE);
-		exar7250_unipolar_onoff(sc, SBE_2T3E3_ON);
-		exar7300_unipolar_onoff(sc, SBE_2T3E3_ON);
-		break;
-	default:
-		return;
-	}
-
-	sc->p.frame_mode = mode;
-}
-
-/* set rate of the local clock */
-void cpld_set_frame_type(struct channel *sc, u32 type)
-{
-	switch (type) {
-	case SBE_2T3E3_FRAME_TYPE_E3_G751:
-	case SBE_2T3E3_FRAME_TYPE_E3_G832:
-		cpld_set_bit(sc, SBE_2T3E3_CPLD_REG_PCRA,
-			     SBE_2T3E3_CPLD_VAL_LOCAL_CLOCK_E3);
-		break;
-	case SBE_2T3E3_FRAME_TYPE_T3_CBIT:
-	case SBE_2T3E3_FRAME_TYPE_T3_M13:
-		cpld_clear_bit(sc, SBE_2T3E3_CPLD_REG_PCRA,
-			       SBE_2T3E3_CPLD_VAL_LOCAL_CLOCK_E3);
-		break;
-	default:
-		return;
-	}
-}
-
-void cpld_set_scrambler(struct channel *sc, u32 mode)
-{
-	if (sc->p.scrambler == mode)
-		return;
-
-	switch (mode) {
-	case SBE_2T3E3_SCRAMBLER_OFF:
-		cpld_clear_bit(sc, SBE_2T3E3_CPLD_REG_PCRB,
-			       SBE_2T3E3_CPLD_VAL_SCRAMBLER_ENABLE);
-		break;
-	case SBE_2T3E3_SCRAMBLER_LARSCOM:
-		cpld_clear_bit(sc, SBE_2T3E3_CPLD_REG_PCRB,
-			       SBE_2T3E3_CPLD_VAL_SCRAMBLER_TYPE);
-		cpld_set_bit(sc, SBE_2T3E3_CPLD_REG_PCRB,
-			     SBE_2T3E3_CPLD_VAL_SCRAMBLER_ENABLE);
-		break;
-	case SBE_2T3E3_SCRAMBLER_ADC_KENTROX_DIGITAL:
-		cpld_set_bit(sc, SBE_2T3E3_CPLD_REG_PCRB,
-			     SBE_2T3E3_CPLD_VAL_SCRAMBLER_TYPE);
-		cpld_set_bit(sc, SBE_2T3E3_CPLD_REG_PCRB,
-			     SBE_2T3E3_CPLD_VAL_SCRAMBLER_ENABLE);
-		break;
-	default:
-		return;
-	}
-
-	sc->p.scrambler = mode;
-}
-
-
-void cpld_set_crc(struct channel *sc, u32 crc)
-{
-	if (sc->p.crc == crc)
-		return;
-
-	switch (crc) {
-	case SBE_2T3E3_CRC_16:
-		cpld_clear_bit(sc, SBE_2T3E3_CPLD_REG_PCRA,
-			       SBE_2T3E3_CPLD_VAL_CRC32);
-		break;
-	case SBE_2T3E3_CRC_32:
-		cpld_set_bit(sc, SBE_2T3E3_CPLD_REG_PCRA,
-			     SBE_2T3E3_CPLD_VAL_CRC32);
-		break;
-	default:
-		return;
-	}
-
-	sc->p.crc = crc;
-}
-
-
-void cpld_select_panel(struct channel *sc, u32 panel)
-{
-	if (sc->p.panel == panel)
-		return;
-	switch (panel) {
-	case SBE_2T3E3_PANEL_FRONT:
-		cpld_clear_bit(sc, SBE_2T3E3_CPLD_REG_PCRA,
-			       SBE_2T3E3_CPLD_VAL_REAR_PANEL);
-		break;
-	case SBE_2T3E3_PANEL_REAR:
-		cpld_set_bit(sc, SBE_2T3E3_CPLD_REG_PCRA,
-			     SBE_2T3E3_CPLD_VAL_REAR_PANEL);
-		break;
-	default:
-		return;
-	}
-
-	udelay(100);
-
-	sc->p.panel = panel;
-}
-
-
-void cpld_set_clock(struct channel *sc, u32 mode)
-{
-	if (sc->p.clock_source == mode)
-		return;
-
-	switch (mode) {
-	case SBE_2T3E3_TIMING_LOCAL:
-		cpld_set_bit(sc, SBE_2T3E3_CPLD_REG_PCRA,
-			     SBE_2T3E3_CPLD_VAL_ALT);
-		break;
-	case SBE_2T3E3_TIMING_LOOP:
-		cpld_clear_bit(sc, SBE_2T3E3_CPLD_REG_PCRA,
-			       SBE_2T3E3_CPLD_VAL_ALT);
-		break;
-	default:
-		return;
-	}
-
-	sc->p.clock_source = mode;
-}
-
-void cpld_set_pad_count(struct channel *sc, u32 count)
-{
-	u32 val;
-
-	if (sc->p.pad_count == count)
-		return;
-
-	switch (count) {
-	case SBE_2T3E3_PAD_COUNT_1:
-		val = SBE_2T3E3_CPLD_VAL_PAD_COUNT_1;
-		break;
-	case SBE_2T3E3_PAD_COUNT_2:
-		val = SBE_2T3E3_CPLD_VAL_PAD_COUNT_2;
-		break;
-	case SBE_2T3E3_PAD_COUNT_3:
-		val = SBE_2T3E3_CPLD_VAL_PAD_COUNT_3;
-		break;
-	case SBE_2T3E3_PAD_COUNT_4:
-		val = SBE_2T3E3_CPLD_VAL_PAD_COUNT_4;
-		break;
-	default:
-		return;
-	}
-
-	cpld_clear_bit(sc, SBE_2T3E3_CPLD_REG_PCRB,
-		       SBE_2T3E3_CPLD_VAL_PAD_COUNT);
-	cpld_set_bit(sc, SBE_2T3E3_CPLD_REG_PCRB, val);
-	sc->p.pad_count = count;
-}
-
-void cpld_LOS_update(struct channel *sc)
-{
-	u_int8_t los;
-
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_PICSR,
-		   SBE_2T3E3_CPLD_VAL_DMO_SIGNAL_DETECTED |
-		   SBE_2T3E3_CPLD_VAL_RECEIVE_LOSS_OF_LOCK_DETECTED |
-		   SBE_2T3E3_CPLD_VAL_RECEIVE_LOSS_OF_SIGNAL_DETECTED);
-	los = cpld_read(sc, SBE_2T3E3_CPLD_REG_PICSR) &
-		SBE_2T3E3_CPLD_VAL_RECEIVE_LOSS_OF_SIGNAL_DETECTED;
-
-	if (los != sc->s.LOS)
-		dev_info(&sc->pdev->dev, "SBE 2T3E3: LOS status: %s\n",
-			 los ? "Loss of signal" : "Signal OK");
-	sc->s.LOS = los;
-}
-
-void cpld_set_fractional_mode(struct channel *sc, u32 mode,
-			      u32 start, u32 stop)
-{
-	if (mode == SBE_2T3E3_FRACTIONAL_MODE_NONE) {
-		start = 0;
-		stop = 0;
-	}
-
-	if (sc->p.fractional_mode == mode && sc->p.bandwidth_start == start &&
-	    sc->p.bandwidth_stop == stop)
-		return;
-
-	switch (mode) {
-	case SBE_2T3E3_FRACTIONAL_MODE_NONE:
-		cpld_write(sc, SBE_2T3E3_CPLD_REG_PCRC,
-			   SBE_2T3E3_CPLD_VAL_FRACTIONAL_MODE_NONE);
-		break;
-	case SBE_2T3E3_FRACTIONAL_MODE_0:
-		cpld_write(sc, SBE_2T3E3_CPLD_REG_PCRC,
-			   SBE_2T3E3_CPLD_VAL_FRACTIONAL_MODE_0);
-		break;
-	case SBE_2T3E3_FRACTIONAL_MODE_1:
-		cpld_write(sc, SBE_2T3E3_CPLD_REG_PCRC,
-			   SBE_2T3E3_CPLD_VAL_FRACTIONAL_MODE_1);
-		break;
-	case SBE_2T3E3_FRACTIONAL_MODE_2:
-		cpld_write(sc, SBE_2T3E3_CPLD_REG_PCRC,
-			   SBE_2T3E3_CPLD_VAL_FRACTIONAL_MODE_2);
-		break;
-	default:
-		netdev_err(sc->dev, "wrong mode in set_fractional_mode\n");
-		return;
-	}
-
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_PBWF, start);
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_PBWL, stop);
-
-	sc->p.fractional_mode = mode;
-	sc->p.bandwidth_start = start;
-	sc->p.bandwidth_stop = stop;
-}
diff --git a/drivers/staging/sbe-2t3e3/ctrl.c b/drivers/staging/sbe-2t3e3/ctrl.c
deleted file mode 100644
index e0964ac9e7d..00000000000
--- a/drivers/staging/sbe-2t3e3/ctrl.c
+++ /dev/null
@@ -1,350 +0,0 @@
-/*
- * SBE 2T3E3 synchronous serial card driver for Linux
- *
- * Copyright (C) 2009-2010 Krzysztof Halasa <khc@pm.waw.pl>
- *
- * 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.
- *
- * This code is based on a driver written by SBE Inc.
- */
-
-#include <linux/types.h>
-#include "2t3e3.h"
-#include "ctrl.h"
-
-void t3e3_set_frame_type(struct channel *sc, u32 mode)
-{
-	if (sc->p.frame_type == mode)
-		return;
-
-	if (sc->r.flags & SBE_2T3E3_FLAG_NETWORK_UP) {
-		dev_err(&sc->pdev->dev, "SBE 2T3E3: changing frame type during active connection\n");
-		return;
-	}
-
-	exar7300_set_frame_type(sc, mode);
-	exar7250_set_frame_type(sc, mode);
-	cpld_set_frame_type(sc, mode);
-
-	sc->p.frame_type = mode;
-}
-
-static void t3e3_set_loopback(struct channel *sc, u32 mode)
-{
-	u32 tx, rx;
-
-	if (sc->p.loopback == mode)
-		return;
-
-	tx = sc->p.transmitter_on;
-	rx = sc->p.receiver_on;
-	if (tx == SBE_2T3E3_ON)
-		dc_transmitter_onoff(sc, SBE_2T3E3_OFF);
-	if (rx == SBE_2T3E3_ON)
-		dc_receiver_onoff(sc, SBE_2T3E3_OFF);
-
-	/* stop current loopback if any exists */
-	switch (sc->p.loopback) {
-	case SBE_2T3E3_LOOPBACK_NONE:
-		break;
-	case SBE_2T3E3_LOOPBACK_ETHERNET:
-		dc_set_loopback(sc, SBE_2T3E3_21143_VAL_LOOPBACK_OFF);
-		break;
-	case SBE_2T3E3_LOOPBACK_FRAMER:
-		exar7250_set_loopback(sc, SBE_2T3E3_FRAMER_VAL_LOOPBACK_OFF);
-		break;
-	case SBE_2T3E3_LOOPBACK_LIU_DIGITAL:
-	case SBE_2T3E3_LOOPBACK_LIU_ANALOG:
-	case SBE_2T3E3_LOOPBACK_LIU_REMOTE:
-		exar7300_set_loopback(sc, SBE_2T3E3_LIU_VAL_LOOPBACK_OFF);
-		break;
-	default:
-		return;
-	}
-
-	switch (mode) {
-	case SBE_2T3E3_LOOPBACK_NONE:
-		break;
-	case SBE_2T3E3_LOOPBACK_ETHERNET:
-		dc_set_loopback(sc, SBE_2T3E3_21143_VAL_LOOPBACK_INTERNAL);
-		break;
-	case SBE_2T3E3_LOOPBACK_FRAMER:
-		exar7250_set_loopback(sc, SBE_2T3E3_FRAMER_VAL_LOOPBACK_ON);
-		break;
-	case SBE_2T3E3_LOOPBACK_LIU_DIGITAL:
-		exar7300_set_loopback(sc, SBE_2T3E3_LIU_VAL_LOOPBACK_DIGITAL);
-		break;
-	case SBE_2T3E3_LOOPBACK_LIU_ANALOG:
-		exar7300_set_loopback(sc, SBE_2T3E3_LIU_VAL_LOOPBACK_ANALOG);
-		break;
-	case SBE_2T3E3_LOOPBACK_LIU_REMOTE:
-		exar7300_set_loopback(sc, SBE_2T3E3_LIU_VAL_LOOPBACK_REMOTE);
-		break;
-	default:
-		return;
-	}
-
-	sc->p.loopback = mode;
-
-	if (tx == SBE_2T3E3_ON)
-		dc_transmitter_onoff(sc, SBE_2T3E3_ON);
-	if (rx == SBE_2T3E3_ON)
-		dc_receiver_onoff(sc, SBE_2T3E3_ON);
-}
-
-
-static void t3e3_reg_read(struct channel *sc, u32 *reg, u32 *val)
-{
-	u32 i;
-
-	*val = 0;
-
-	switch (reg[0]) {
-	case SBE_2T3E3_CHIP_21143:
-		if (!(reg[1] & 7))
-			*val = dc_read(sc->addr, reg[1] / 8);
-		break;
-	case SBE_2T3E3_CHIP_CPLD:
-		for (i = 0; i < SBE_2T3E3_CPLD_REG_MAX; i++)
-			if (cpld_reg_map[i][sc->h.slot] == reg[1]) {
-				*val = cpld_read(sc, i);
-				break;
-			}
-		break;
-	case SBE_2T3E3_CHIP_FRAMER:
-		for (i = 0; i < SBE_2T3E3_FRAMER_REG_MAX; i++)
-			if (t3e3_framer_reg_map[i] == reg[1]) {
-				*val = exar7250_read(sc, i);
-				break;
-			}
-		break;
-	case SBE_2T3E3_CHIP_LIU:
-		for (i = 0; i < SBE_2T3E3_LIU_REG_MAX; i++)
-			if (t3e3_liu_reg_map[i] == reg[1]) {
-				*val = exar7300_read(sc, i);
-				break;
-			}
-		break;
-	default:
-		break;
-	}
-}
-
-static void t3e3_reg_write(struct channel *sc, u32 *reg)
-{
-	u32 i;
-
-	switch (reg[0]) {
-	case SBE_2T3E3_CHIP_21143:
-		dc_write(sc->addr, reg[1], reg[2]);
-		break;
-	case SBE_2T3E3_CHIP_CPLD:
-		for (i = 0; i < SBE_2T3E3_CPLD_REG_MAX; i++)
-			if (cpld_reg_map[i][sc->h.slot] == reg[1]) {
-				cpld_write(sc, i, reg[2]);
-				break;
-			}
-		break;
-	case SBE_2T3E3_CHIP_FRAMER:
-		for (i = 0; i < SBE_2T3E3_FRAMER_REG_MAX; i++)
-			if (t3e3_framer_reg_map[i] == reg[1]) {
-				exar7250_write(sc, i, reg[2]);
-				break;
-			}
-		break;
-	case SBE_2T3E3_CHIP_LIU:
-		for (i = 0; i < SBE_2T3E3_LIU_REG_MAX; i++)
-			if (t3e3_liu_reg_map[i] == reg[1]) {
-				exar7300_write(sc, i, reg[2]);
-				break;
-			}
-		break;
-	}
-}
-
-static void t3e3_port_get(struct channel *sc, struct t3e3_param *param)
-{
-	memcpy(param, &(sc->p), sizeof(struct t3e3_param));
-}
-
-static void t3e3_port_set(struct channel *sc, struct t3e3_param *param)
-{
-	if (param->frame_mode != 0xff)
-		cpld_set_frame_mode(sc, param->frame_mode);
-
-	if (param->fractional_mode != 0xff)
-		cpld_set_fractional_mode(sc, param->fractional_mode,
-					 param->bandwidth_start,
-					 param->bandwidth_stop);
-
-	if (param->pad_count != 0xff)
-		cpld_set_pad_count(sc, param->pad_count);
-
-	if (param->crc != 0xff)
-		cpld_set_crc(sc, param->crc);
-
-	if (param->receiver_on != 0xff)
-		dc_receiver_onoff(sc, param->receiver_on);
-
-	if (param->transmitter_on != 0xff)
-		dc_transmitter_onoff(sc, param->transmitter_on);
-
-	if (param->frame_type != 0xff)
-		t3e3_set_frame_type(sc, param->frame_type);
-
-	if (param->panel != 0xff)
-		cpld_select_panel(sc, param->panel);
-
-	if (param->line_build_out != 0xff)
-		exar7300_line_build_out_onoff(sc, param->line_build_out);
-
-	if (param->receive_equalization != 0xff)
-		exar7300_receive_equalization_onoff(sc, param->receive_equalization);
-
-	if (param->transmit_all_ones != 0xff)
-		exar7300_transmit_all_ones_onoff(sc, param->transmit_all_ones);
-
-	if (param->loopback != 0xff)
-		t3e3_set_loopback(sc, param->loopback);
-
-	if (param->clock_source != 0xff)
-		cpld_set_clock(sc, param->clock_source);
-
-	if (param->scrambler != 0xff)
-		cpld_set_scrambler(sc, param->scrambler);
-}
-
-static void t3e3_port_get_stats(struct channel *sc, struct t3e3_stats *stats)
-{
-	u32 result;
-
-	sc->s.LOC = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_IO_CONTROL)
-		& SBE_2T3E3_FRAMER_VAL_LOSS_OF_CLOCK_STATUS ? 1 : 0;
-
-	switch (sc->p.frame_type) {
-	case SBE_2T3E3_FRAME_TYPE_E3_G751:
-	case SBE_2T3E3_FRAME_TYPE_E3_G832:
-		result = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_RX_CONFIGURATION_STATUS_2);
-		sc->s.LOF = result & SBE_2T3E3_FRAMER_VAL_E3_RX_LOF ? 1 : 0;
-		sc->s.OOF = result & SBE_2T3E3_FRAMER_VAL_E3_RX_OOF ? 1 : 0;
-
-		cpld_LOS_update(sc);
-
-		sc->s.AIS = result & SBE_2T3E3_FRAMER_VAL_E3_RX_AIS ? 1 : 0;
-		sc->s.FERF = result & SBE_2T3E3_FRAMER_VAL_E3_RX_FERF ? 1 : 0;
-		break;
-
-	case SBE_2T3E3_FRAME_TYPE_T3_CBIT:
-	case SBE_2T3E3_FRAME_TYPE_T3_M13:
-		result = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_CONFIGURATION_STATUS);
-		sc->s.AIS = result & SBE_2T3E3_FRAMER_VAL_T3_RX_AIS ? 1 : 0;
-
-		cpld_LOS_update(sc);
-
-		sc->s.IDLE = result & SBE_2T3E3_FRAMER_VAL_T3_RX_IDLE ? 1 : 0;
-		sc->s.OOF = result & SBE_2T3E3_FRAMER_VAL_T3_RX_OOF ? 1 : 0;
-
-		result = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_STATUS);
-		sc->s.FERF = result & SBE_2T3E3_FRAMER_VAL_T3_RX_FERF ? 1 : 0;
-		sc->s.AIC = result & SBE_2T3E3_FRAMER_VAL_T3_RX_AIC ? 1 : 0;
-		sc->s.FEBE_code = result & SBE_2T3E3_FRAMER_VAL_T3_RX_FEBE;
-
-		sc->s.FEAC = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_FEAC);
-		break;
-
-	default:
-		break;
-	}
-
-	result = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_PMON_LCV_EVENT_COUNT_MSB) << 8;
-	result += exar7250_read(sc, SBE_2T3E3_FRAMER_REG_PMON_HOLDING_REGISTER);
-	sc->s.LCV += result;
-
-	result = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_PMON_FRAMING_BIT_ERROR_EVENT_COUNT_MSB) << 8;
-	result += exar7250_read(sc, SBE_2T3E3_FRAMER_REG_PMON_HOLDING_REGISTER);
-	sc->s.FRAMING_BIT += result;
-
-	result = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_PMON_PARITY_ERROR_EVENT_COUNT_MSB) << 8;
-	result += exar7250_read(sc, SBE_2T3E3_FRAMER_REG_PMON_HOLDING_REGISTER);
-	sc->s.PARITY_ERROR += result;
-
-	result = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_PMON_FEBE_EVENT_COUNT_MSB) << 8;
-	result += exar7250_read(sc, SBE_2T3E3_FRAMER_REG_PMON_HOLDING_REGISTER);
-	sc->s.FEBE_count += result;
-
-	result = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_PMON_CP_BIT_ERROR_EVENT_COUNT_MSB) << 8;
-	result += exar7250_read(sc, SBE_2T3E3_FRAMER_REG_PMON_HOLDING_REGISTER);
-	sc->s.CP_BIT += result;
-
-	memcpy(stats, &(sc->s), sizeof(struct t3e3_stats));
-}
-
-static void t3e3_port_del_stats(struct channel *sc)
-{
-	memset(&(sc->s), 0, sizeof(struct t3e3_stats));
-}
-
-void t3e3_if_config(struct channel *sc, u32 cmd, char *set,
-		    struct t3e3_resp *ret, int *rlen)
-{
-	struct t3e3_param *param = (struct t3e3_param *)set;
-	u32 *data = (u32 *)set;
-
-	/* turn off all interrupt */
-	/* cpld_stop_intr(sc); */
-
-	switch (cmd) {
-	case SBE_2T3E3_PORT_GET:
-		t3e3_port_get(sc, &(ret->u.param));
-		*rlen = sizeof(ret->u.param);
-		break;
-	case SBE_2T3E3_PORT_SET:
-		t3e3_port_set(sc, param);
-		*rlen = 0;
-		break;
-	case SBE_2T3E3_PORT_GET_STATS:
-		t3e3_port_get_stats(sc, &(ret->u.stats));
-		*rlen = sizeof(ret->u.stats);
-		break;
-	case SBE_2T3E3_PORT_DEL_STATS:
-		t3e3_port_del_stats(sc);
-		*rlen = 0;
-		break;
-	case SBE_2T3E3_PORT_READ_REGS:
-		t3e3_reg_read(sc, data, &(ret->u.data));
-		*rlen = sizeof(ret->u.data);
-		break;
-	case SBE_2T3E3_PORT_WRITE_REGS:
-		t3e3_reg_write(sc, data);
-		*rlen = 0;
-		break;
-	case SBE_2T3E3_LOG_LEVEL:
-		*rlen = 0;
-		break;
-	default:
-		*rlen = 0;
-		break;
-	}
-}
-
-void t3e3_sc_init(struct channel *sc)
-{
-	memset(sc, 0, sizeof(*sc));
-
-	sc->p.frame_mode = SBE_2T3E3_FRAME_MODE_HDLC;
-	sc->p.fractional_mode = SBE_2T3E3_FRACTIONAL_MODE_NONE;
-	sc->p.crc = SBE_2T3E3_CRC_32;
-	sc->p.receiver_on = SBE_2T3E3_OFF;
-	sc->p.transmitter_on = SBE_2T3E3_OFF;
-	sc->p.frame_type = SBE_2T3E3_FRAME_TYPE_T3_CBIT;
-	sc->p.panel = SBE_2T3E3_PANEL_FRONT;
-	sc->p.line_build_out = SBE_2T3E3_OFF;
-	sc->p.receive_equalization = SBE_2T3E3_OFF;
-	sc->p.transmit_all_ones = SBE_2T3E3_OFF;
-	sc->p.loopback = SBE_2T3E3_LOOPBACK_NONE;
-	sc->p.clock_source = SBE_2T3E3_TIMING_LOCAL;
-	sc->p.scrambler = SBE_2T3E3_SCRAMBLER_OFF;
-	sc->p.pad_count = SBE_2T3E3_PAD_COUNT_1;
-}
diff --git a/drivers/staging/sbe-2t3e3/ctrl.h b/drivers/staging/sbe-2t3e3/ctrl.h
deleted file mode 100644
index 41f144d75c3..00000000000
--- a/drivers/staging/sbe-2t3e3/ctrl.h
+++ /dev/null
@@ -1,131 +0,0 @@
-/*
- * SBE 2T3E3 synchronous serial card driver for Linux
- *
- * Copyright (C) 2009-2010 Krzysztof Halasa <khc@pm.waw.pl>
- *
- * 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.
- *
- * This code is based on a driver written by SBE Inc.
- */
-
-#ifndef CTRL_H
-#define CTRL_H
-
-#define SBE_2T3E3_OFF					0
-#define SBE_2T3E3_ON					1
-
-#define SBE_2T3E3_LED_NONE				0
-#define SBE_2T3E3_LED_GREEN				1
-#define SBE_2T3E3_LED_YELLOW				2
-
-#define SBE_2T3E3_CABLE_LENGTH_LESS_THAN_255_FEET	0
-#define SBE_2T3E3_CABLE_LENGTH_GREATER_THAN_255_FEET	1
-
-#define SBE_2T3E3_CRC_16				0
-#define SBE_2T3E3_CRC_32				1
-
-#define SBE_2T3E3_PANEL_FRONT				0
-#define SBE_2T3E3_PANEL_REAR				1
-
-#define SBE_2T3E3_FRAME_MODE_HDLC			0
-#define SBE_2T3E3_FRAME_MODE_TRANSPARENT		1
-#define SBE_2T3E3_FRAME_MODE_RAW			2
-
-#define SBE_2T3E3_FRAME_TYPE_E3_G751			0
-#define SBE_2T3E3_FRAME_TYPE_E3_G832			1
-#define SBE_2T3E3_FRAME_TYPE_T3_CBIT			2
-#define SBE_2T3E3_FRAME_TYPE_T3_M13			3
-
-#define SBE_2T3E3_FRACTIONAL_MODE_NONE			0
-#define SBE_2T3E3_FRACTIONAL_MODE_0			1
-#define SBE_2T3E3_FRACTIONAL_MODE_1			2
-#define SBE_2T3E3_FRACTIONAL_MODE_2			3
-
-#define SBE_2T3E3_SCRAMBLER_OFF				0
-#define SBE_2T3E3_SCRAMBLER_LARSCOM			1
-#define SBE_2T3E3_SCRAMBLER_ADC_KENTROX_DIGITAL		2
-
-#define SBE_2T3E3_TIMING_LOCAL				0
-#define SBE_2T3E3_TIMING_LOOP				1
-
-#define SBE_2T3E3_LOOPBACK_NONE				0
-#define SBE_2T3E3_LOOPBACK_ETHERNET			1
-#define SBE_2T3E3_LOOPBACK_FRAMER			2
-#define SBE_2T3E3_LOOPBACK_LIU_DIGITAL			3
-#define SBE_2T3E3_LOOPBACK_LIU_ANALOG			4
-#define SBE_2T3E3_LOOPBACK_LIU_REMOTE			5
-
-#define SBE_2T3E3_PAD_COUNT_1				1
-#define SBE_2T3E3_PAD_COUNT_2				2
-#define SBE_2T3E3_PAD_COUNT_3				3
-#define SBE_2T3E3_PAD_COUNT_4				4
-
-#define SBE_2T3E3_CHIP_21143				0
-#define SBE_2T3E3_CHIP_CPLD				1
-#define SBE_2T3E3_CHIP_FRAMER				2
-#define SBE_2T3E3_CHIP_LIU				3
-
-#define SBE_2T3E3_LOG_LEVEL_NONE			0
-#define SBE_2T3E3_LOG_LEVEL_ERROR			1
-#define SBE_2T3E3_LOG_LEVEL_WARNING			2
-#define SBE_2T3E3_LOG_LEVEL_INFO			3
-
-/* commands */
-#define SBE_2T3E3_PORT_GET				0
-#define SBE_2T3E3_PORT_SET				1
-#define SBE_2T3E3_PORT_GET_STATS			2
-#define SBE_2T3E3_PORT_DEL_STATS			3
-#define SBE_2T3E3_PORT_READ_REGS			4
-#define SBE_2T3E3_LOG_LEVEL				5
-#define SBE_2T3E3_PORT_WRITE_REGS			6
-
-#define NG_SBE_2T3E3_NODE_TYPE  "sbe2T3E3"
-#define NG_SBE_2T3E3_COOKIE     0x03800891
-
-struct t3e3_param {
-	u_int8_t frame_mode;		/* FRAME_MODE_* */
-	u_int8_t crc;			/* CRC_* */
-	u_int8_t receiver_on;		/* ON/OFF */
-	u_int8_t transmitter_on;	/* ON/OFF */
-	u_int8_t frame_type;		/* FRAME_TYPE_* */
-	u_int8_t panel;			/* PANEL_* */
-	u_int8_t line_build_out;	/* ON/OFF */
-	u_int8_t receive_equalization;	/* ON/OFF */
-	u_int8_t transmit_all_ones;	/* ON/OFF */
-	u_int8_t loopback;		/* LOOPBACK_* */
-	u_int8_t clock_source;		/* TIMING_* */
-	u_int8_t scrambler;		/* SCRAMBLER_* */
-	u_int8_t pad_count;		/* PAD_COUNT_* */
-	u_int8_t log_level;		/* LOG_LEVEL_* - unused */
-	u_int8_t fractional_mode;	/* FRACTIONAL_MODE_* */
-	u_int8_t bandwidth_start;	/* 0-255 */
-	u_int8_t bandwidth_stop;	/* 0-255 */
-};
-
-struct t3e3_stats {
-	u_int64_t in_bytes;
-	u32 in_packets, in_dropped;
-	u32 in_errors, in_error_desc, in_error_coll, in_error_drib,
-		in_error_crc, in_error_mii;
-	u_int64_t out_bytes;
-	u32 out_packets, out_dropped;
-	u32 out_errors, out_error_jab, out_error_lost_carr,
-		out_error_no_carr, out_error_link_fail, out_error_underflow,
-		out_error_dereferred;
-	u_int8_t LOC, LOF, OOF, LOS, AIS, FERF, IDLE, AIC, FEAC;
-	u_int16_t FEBE_code;
-	u32 LCV, FRAMING_BIT, PARITY_ERROR, FEBE_count, CP_BIT;
-};
-
-
-struct t3e3_resp {
-	union {
-		struct t3e3_param param;
-		struct t3e3_stats stats;
-		u32 data;
-	} u;
-};
-
-#endif /* CTRL_H */
diff --git a/drivers/staging/sbe-2t3e3/dc.c b/drivers/staging/sbe-2t3e3/dc.c
deleted file mode 100644
index 02510f67ac4..00000000000
--- a/drivers/staging/sbe-2t3e3/dc.c
+++ /dev/null
@@ -1,460 +0,0 @@
-/*
- * SBE 2T3E3 synchronous serial card driver for Linux
- *
- * Copyright (C) 2009-2010 Krzysztof Halasa <khc@pm.waw.pl>
- *
- * 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.
- *
- * This code is based on a driver written by SBE Inc.
- */
-
-#include <linux/netdevice.h>
-#include <linux/types.h>
-#include <linux/errno.h>
-#include <linux/io.h>
-#include "2t3e3.h"
-#include "ctrl.h"
-
-static int dc_init_descriptor_list(struct channel *sc);
-
-void dc_init(struct channel *sc)
-{
-	u32 val;
-
-	dc_stop(sc);
-	/*dc_reset(sc);*/ /* do not want to reset here */
-
-	/*
-	 * BUS_MODE (CSR0)
-	 */
-	val = SBE_2T3E3_21143_VAL_READ_LINE_ENABLE |
-		SBE_2T3E3_21143_VAL_READ_MULTIPLE_ENABLE |
-		SBE_2T3E3_21143_VAL_TRANSMIT_AUTOMATIC_POLLING_200us |
-		SBE_2T3E3_21143_VAL_BUS_ARBITRATION_RR;
-
-	if (sc->h.command & 16)
-		val |= SBE_2T3E3_21143_VAL_WRITE_AND_INVALIDATE_ENABLE;
-
-	switch (sc->h.cache_size) {
-	case 32:
-		val |= SBE_2T3E3_21143_VAL_CACHE_ALIGNMENT_32;
-		break;
-	case 16:
-		val |= SBE_2T3E3_21143_VAL_CACHE_ALIGNMENT_16;
-		break;
-	case 8:
-		val |= SBE_2T3E3_21143_VAL_CACHE_ALIGNMENT_8;
-		break;
-	default:
-		break;
-	}
-
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_BUS_MODE, val);
-
-	/* OPERATION_MODE (CSR6) */
-	val = SBE_2T3E3_21143_VAL_RECEIVE_ALL |
-		SBE_2T3E3_21143_VAL_MUST_BE_ONE |
-		SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS_1 |
-		SBE_2T3E3_21143_VAL_LOOPBACK_OFF |
-		SBE_2T3E3_21143_VAL_PASS_ALL_MULTICAST |
-		SBE_2T3E3_21143_VAL_PROMISCUOUS_MODE |
-		SBE_2T3E3_21143_VAL_PASS_BAD_FRAMES;
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE, val);
-	if (sc->p.loopback == SBE_2T3E3_LOOPBACK_ETHERNET)
-		sc->p.loopback = SBE_2T3E3_LOOPBACK_NONE;
-
-	/*
-	 * GENERAL_PURPOSE_TIMER_AND_INTERRUPT_MITIGATION_CONTROL (CSR11)
-	 */
-	val = SBE_2T3E3_21143_VAL_CYCLE_SIZE |
-		SBE_2T3E3_21143_VAL_TRANSMIT_TIMER |
-		SBE_2T3E3_21143_VAL_NUMBER_OF_TRANSMIT_PACKETS |
-		SBE_2T3E3_21143_VAL_RECEIVE_TIMER |
-		SBE_2T3E3_21143_VAL_NUMBER_OF_RECEIVE_PACKETS;
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_GENERAL_PURPOSE_TIMER_AND_INTERRUPT_MITIGATION_CONTROL, val);
-
-	/* prepare descriptors and data for receive and transmit processes */
-	if (dc_init_descriptor_list(sc) != 0)
-		return;
-
-	/* clear ethernet interrupts status */
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_STATUS, 0xFFFFFFFF);
-
-	/* SIA mode registers */
-	dc_set_output_port(sc);
-}
-
-void dc_start(struct channel *sc)
-{
-	u32 val;
-
-	if (!(sc->r.flags & SBE_2T3E3_FLAG_NETWORK_UP))
-		return;
-
-	dc_init(sc);
-
-	/* get actual LOS and OOF status */
-	switch (sc->p.frame_type) {
-	case SBE_2T3E3_FRAME_TYPE_E3_G751:
-	case SBE_2T3E3_FRAME_TYPE_E3_G832:
-		val = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_RX_CONFIGURATION_STATUS_2);
-		dev_dbg(&sc->pdev->dev, "Start Framer Rx Status = %02X\n", val);
-		sc->s.OOF = val & SBE_2T3E3_FRAMER_VAL_E3_RX_OOF ? 1 : 0;
-		break;
-	case SBE_2T3E3_FRAME_TYPE_T3_CBIT:
-	case SBE_2T3E3_FRAME_TYPE_T3_M13:
-		val = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_CONFIGURATION_STATUS);
-		dev_dbg(&sc->pdev->dev, "Start Framer Rx Status = %02X\n", val);
-		sc->s.OOF = val & SBE_2T3E3_FRAMER_VAL_T3_RX_OOF ? 1 : 0;
-		break;
-	default:
-		break;
-	}
-	cpld_LOS_update(sc);
-
-	/* start receive and transmit processes */
-	dc_transmitter_onoff(sc, SBE_2T3E3_ON);
-	dc_receiver_onoff(sc, SBE_2T3E3_ON);
-
-	/* start interrupts */
-	dc_start_intr(sc);
-}
-
-#define MAX_INT_WAIT_CNT	12000
-void dc_stop(struct channel *sc)
-{
-	int wcnt;
-
-	/* stop receive and transmit processes */
-	dc_receiver_onoff(sc, SBE_2T3E3_OFF);
-	dc_transmitter_onoff(sc, SBE_2T3E3_OFF);
-
-	/* turn off ethernet interrupts */
-	dc_stop_intr(sc);
-
-	/* wait to ensure the interrupts have been completed */
-	for (wcnt = 0; wcnt < MAX_INT_WAIT_CNT; wcnt++) {
-		udelay(5);
-		if (!sc->interrupt_active)
-			break;
-	}
-	if (wcnt >= MAX_INT_WAIT_CNT)
-		dev_warn(&sc->pdev->dev, "SBE 2T3E3: Interrupt active too long\n");
-
-	/* clear all receive/transmit data */
-	dc_drop_descriptor_list(sc);
-}
-
-void dc_start_intr(struct channel *sc)
-{
-	if (sc->p.loopback == SBE_2T3E3_LOOPBACK_NONE && sc->s.OOF)
-		return;
-
-	if (sc->p.receiver_on || sc->p.transmitter_on) {
-		if (!sc->ether.interrupt_enable_mask)
-			dc_write(sc->addr, SBE_2T3E3_21143_REG_STATUS, 0xFFFFFFFF);
-
-		sc->ether.interrupt_enable_mask =
-			SBE_2T3E3_21143_VAL_NORMAL_INTERRUPT_SUMMARY_ENABLE |
-			SBE_2T3E3_21143_VAL_ABNORMAL_INTERRUPT_SUMMARY_ENABLE |
-			SBE_2T3E3_21143_VAL_RECEIVE_STOPPED_ENABLE |
-			SBE_2T3E3_21143_VAL_RECEIVE_BUFFER_UNAVAILABLE_ENABLE |
-			SBE_2T3E3_21143_VAL_RECEIVE_INTERRUPT_ENABLE |
-			SBE_2T3E3_21143_VAL_TRANSMIT_UNDERFLOW_INTERRUPT_ENABLE |
-			SBE_2T3E3_21143_VAL_TRANSMIT_BUFFER_UNAVAILABLE_ENABLE |
-			SBE_2T3E3_21143_VAL_TRANSMIT_STOPPED_ENABLE |
-			SBE_2T3E3_21143_VAL_TRANSMIT_INTERRUPT_ENABLE;
-
-		dc_write(sc->addr, SBE_2T3E3_21143_REG_INTERRUPT_ENABLE,
-			 sc->ether.interrupt_enable_mask);
-	}
-}
-
-void dc_stop_intr(struct channel *sc)
-{
-	sc->ether.interrupt_enable_mask = 0;
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_INTERRUPT_ENABLE, 0);
-}
-
-void dc_reset(struct channel *sc)
-{
-	/* turn off ethernet interrupts */
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_INTERRUPT_ENABLE, 0);
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_STATUS, 0xFFFFFFFF);
-
-	/* software reset */
-	dc_set_bits(sc->addr, SBE_2T3E3_21143_REG_BUS_MODE,
-		   SBE_2T3E3_21143_VAL_SOFTWARE_RESET);
-	udelay(4); /* 50 PCI cycles < 2us */
-
-	/* clear hardware configuration */
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_BUS_MODE, 0);
-
-	/* clear software configuration */
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE, 0);
-
-	/* turn off SIA reset */
-	dc_set_bits(sc->addr, SBE_2T3E3_21143_REG_SIA_CONNECTIVITY,
-		   SBE_2T3E3_21143_VAL_SIA_RESET);
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_SIA_TRANSMIT_AND_RECEIVE, 0);
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_SIA_AND_GENERAL_PURPOSE_PORT, 0);
-}
-
-
-void dc_receiver_onoff(struct channel *sc, u32 mode)
-{
-	u32 i, state = 0;
-
-	if (sc->p.receiver_on == mode)
-		return;
-
-	switch (mode) {
-	case SBE_2T3E3_OFF:
-		if (dc_read(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE) &
-		    SBE_2T3E3_21143_VAL_RECEIVE_START) {
-			dc_clear_bits(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE,
-				      SBE_2T3E3_21143_VAL_RECEIVE_START);
-
-			for (i = 0; i < 16; i++) {
-				state = dc_read(sc->addr, SBE_2T3E3_21143_REG_STATUS) &
-					SBE_2T3E3_21143_VAL_RECEIVE_PROCESS_STATE;
-				if (state == SBE_2T3E3_21143_VAL_RX_STOPPED)
-					break;
-				udelay(5);
-			}
-			if (state != SBE_2T3E3_21143_VAL_RX_STOPPED)
-				dev_warn(&sc->pdev->dev, "SBE 2T3E3: Rx failed to stop\n");
-			else
-				dev_info(&sc->pdev->dev, "SBE 2T3E3: Rx off\n");
-		}
-		break;
-	case SBE_2T3E3_ON:
-		dc_set_bits(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE,
-			   SBE_2T3E3_21143_VAL_RECEIVE_START);
-		udelay(100);
-		dc_write(sc->addr, SBE_2T3E3_21143_REG_RECEIVE_POLL_DEMAND, 0xFFFFFFFF);
-		break;
-	default:
-		return;
-	}
-
-	sc->p.receiver_on = mode;
-}
-
-void dc_transmitter_onoff(struct channel *sc, u32 mode)
-{
-	u32 i, state = 0;
-
-	if (sc->p.transmitter_on == mode)
-		return;
-
-	switch (mode) {
-	case SBE_2T3E3_OFF:
-		if (dc_read(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE) &
-		    SBE_2T3E3_21143_VAL_TRANSMISSION_START) {
-			dc_clear_bits(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE,
-				      SBE_2T3E3_21143_VAL_TRANSMISSION_START);
-
-			for (i = 0; i < 16; i++) {
-				state = dc_read(sc->addr, SBE_2T3E3_21143_REG_STATUS) &
-					SBE_2T3E3_21143_VAL_TRANSMISSION_PROCESS_STATE;
-				if (state == SBE_2T3E3_21143_VAL_TX_STOPPED)
-					break;
-				udelay(5);
-			}
-			if (state != SBE_2T3E3_21143_VAL_TX_STOPPED)
-				dev_warn(&sc->pdev->dev, "SBE 2T3E3: Tx failed to stop\n");
-		}
-		break;
-	case SBE_2T3E3_ON:
-		dc_set_bits(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE,
-			   SBE_2T3E3_21143_VAL_TRANSMISSION_START);
-		udelay(100);
-		dc_write(sc->addr, SBE_2T3E3_21143_REG_TRANSMIT_POLL_DEMAND, 0xFFFFFFFF);
-		break;
-	default:
-		return;
-	}
-
-	sc->p.transmitter_on = mode;
-}
-
-
-
-void dc_set_loopback(struct channel *sc, u32 mode)
-{
-	u32 val;
-
-	switch (mode) {
-	case SBE_2T3E3_21143_VAL_LOOPBACK_OFF:
-	case SBE_2T3E3_21143_VAL_LOOPBACK_INTERNAL:
-		break;
-	default:
-		return;
-	}
-
-	/* select loopback mode */
-	val = dc_read(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE) &
-		~SBE_2T3E3_21143_VAL_OPERATING_MODE;
-	val |= mode;
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE, val);
-
-	if (mode == SBE_2T3E3_21143_VAL_LOOPBACK_OFF)
-		dc_set_bits(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE,
-			   SBE_2T3E3_21143_VAL_FULL_DUPLEX_MODE);
-	else
-		dc_clear_bits(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE,
-			      SBE_2T3E3_21143_VAL_FULL_DUPLEX_MODE);
-}
-
-static int dc_init_descriptor_list(struct channel *sc)
-{
-	u32 i, j;
-	struct sk_buff *m;
-
-	if (sc->ether.rx_ring == NULL)
-		sc->ether.rx_ring = kcalloc(SBE_2T3E3_RX_DESC_RING_SIZE,
-					    sizeof(struct t3e3_rx_desc), GFP_KERNEL);
-	if (sc->ether.rx_ring == NULL)
-		return -ENOMEM;
-
-	if (sc->ether.tx_ring == NULL)
-		sc->ether.tx_ring = kcalloc(SBE_2T3E3_TX_DESC_RING_SIZE,
-					    sizeof(struct t3e3_tx_desc), GFP_KERNEL);
-	if (sc->ether.tx_ring == NULL) {
-		kfree(sc->ether.rx_ring);
-		sc->ether.rx_ring = NULL;
-		return -ENOMEM;
-	}
-
-
-	/*
-	 * Receive ring
-	 */
-	for (i = 0; i < SBE_2T3E3_RX_DESC_RING_SIZE; i++) {
-		sc->ether.rx_ring[i].rdes0 = SBE_2T3E3_RX_DESC_21143_OWN;
-		sc->ether.rx_ring[i].rdes1 =
-			SBE_2T3E3_RX_DESC_SECOND_ADDRESS_CHAINED | SBE_2T3E3_MTU;
-
-		if (sc->ether.rx_data[i] == NULL) {
-			m = dev_alloc_skb(MCLBYTES);
-			if (!m) {
-				for (j = 0; j < i; j++) {
-					dev_kfree_skb_any(sc->ether.rx_data[j]);
-					sc->ether.rx_data[j] = NULL;
-				}
-				kfree(sc->ether.rx_ring);
-				sc->ether.rx_ring = NULL;
-				kfree(sc->ether.tx_ring);
-				sc->ether.tx_ring = NULL;
-				dev_err(&sc->pdev->dev, "SBE 2T3E3: token_alloc err:"
-					" no buffer space for RX ring\n");
-				return -ENOBUFS;
-			}
-			sc->ether.rx_data[i] = m;
-		}
-		sc->ether.rx_ring[i].rdes2 = virt_to_phys(sc->ether.rx_data[i]->data);
-
-		sc->ether.rx_ring[i].rdes3 = virt_to_phys(
-			&sc->ether.rx_ring[(i + 1) % SBE_2T3E3_RX_DESC_RING_SIZE]);
-	}
-	sc->ether.rx_ring[SBE_2T3E3_RX_DESC_RING_SIZE - 1].rdes1 |=
-		SBE_2T3E3_RX_DESC_END_OF_RING;
-	sc->ether.rx_ring_current_read = 0;
-
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_RECEIVE_LIST_BASE_ADDRESS,
-		 virt_to_phys(&sc->ether.rx_ring[0]));
-
-	/*
-	 * Transmit ring
-	 */
-	for (i = 0; i < SBE_2T3E3_TX_DESC_RING_SIZE; i++) {
-		sc->ether.tx_ring[i].tdes0 = 0;
-		sc->ether.tx_ring[i].tdes1 = SBE_2T3E3_TX_DESC_SECOND_ADDRESS_CHAINED |
-			SBE_2T3E3_TX_DESC_DISABLE_PADDING;
-
-		sc->ether.tx_ring[i].tdes2 = 0;
-		sc->ether.tx_data[i] = NULL;
-
-		sc->ether.tx_ring[i].tdes3 = virt_to_phys(
-			&sc->ether.tx_ring[(i + 1) % SBE_2T3E3_TX_DESC_RING_SIZE]);
-	}
-	sc->ether.tx_ring[SBE_2T3E3_TX_DESC_RING_SIZE - 1].tdes1 |=
-		SBE_2T3E3_TX_DESC_END_OF_RING;
-
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_TRANSMIT_LIST_BASE_ADDRESS,
-		 virt_to_phys(&sc->ether.tx_ring[0]));
-	sc->ether.tx_ring_current_read = 0;
-	sc->ether.tx_ring_current_write = 0;
-	sc->ether.tx_free_cnt = SBE_2T3E3_TX_DESC_RING_SIZE;
-	spin_lock_init(&sc->ether.tx_lock);
-
-	return 0;
-}
-
-void dc_clear_descriptor_list(struct channel *sc)
-{
-	u32 i;
-
-	/* clear CSR3 and CSR4 */
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_RECEIVE_LIST_BASE_ADDRESS, 0);
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_TRANSMIT_LIST_BASE_ADDRESS, 0);
-
-	/* free all data buffers on TX ring */
-	for (i = 0; i < SBE_2T3E3_TX_DESC_RING_SIZE; i++) {
-		if (sc->ether.tx_data[i] != NULL) {
-			dev_kfree_skb_any(sc->ether.tx_data[i]);
-			sc->ether.tx_data[i] = NULL;
-		}
-	}
-}
-
-void dc_drop_descriptor_list(struct channel *sc)
-{
-	u32 i;
-
-	dc_clear_descriptor_list(sc);
-
-	/* free all data buffers on RX ring */
-	for (i = 0; i < SBE_2T3E3_RX_DESC_RING_SIZE; i++) {
-		if (sc->ether.rx_data[i] != NULL) {
-			dev_kfree_skb_any(sc->ether.rx_data[i]);
-			sc->ether.rx_data[i] = NULL;
-		}
-	}
-
-	kfree(sc->ether.rx_ring);
-	sc->ether.rx_ring = NULL;
-	kfree(sc->ether.tx_ring);
-	sc->ether.tx_ring = NULL;
-}
-
-
-void dc_set_output_port(struct channel *sc)
-{
-	dc_clear_bits(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE,
-		      SBE_2T3E3_21143_VAL_PORT_SELECT);
-
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_SIA_STATUS, 0x00000301);
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_SIA_CONNECTIVITY, 0);
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_SIA_TRANSMIT_AND_RECEIVE, 0);
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_SIA_AND_GENERAL_PURPOSE_PORT, 0x08000011);
-
-	dc_set_bits(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE,
-		   SBE_2T3E3_21143_VAL_TRANSMIT_THRESHOLD_MODE_100Mbs |
-		   SBE_2T3E3_21143_VAL_HEARTBEAT_DISABLE |
-		   SBE_2T3E3_21143_VAL_PORT_SELECT |
-		   SBE_2T3E3_21143_VAL_FULL_DUPLEX_MODE);
-}
-
-void dc_restart(struct channel *sc)
-{
-	dev_warn(&sc->pdev->dev, "SBE 2T3E3: 21143 restart\n");
-
-	dc_stop(sc);
-	dc_reset(sc);
-	dc_init(sc);	/* stop + reset + init */
-	dc_start(sc);
-}
diff --git a/drivers/staging/sbe-2t3e3/exar7250.c b/drivers/staging/sbe-2t3e3/exar7250.c
deleted file mode 100644
index e3ddd140207..00000000000
--- a/drivers/staging/sbe-2t3e3/exar7250.c
+++ /dev/null
@@ -1,185 +0,0 @@
-/*
- * SBE 2T3E3 synchronous serial card driver for Linux
- *
- * Copyright (C) 2009-2010 Krzysztof Halasa <khc@pm.waw.pl>
- *
- * 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.
- *
- * This code is based on a driver written by SBE Inc.
- */
-
-#include "2t3e3.h"
-#include "ctrl.h"
-
-void exar7250_init(struct channel *sc)
-{
-	exar7250_write(sc, SBE_2T3E3_FRAMER_REG_OPERATING_MODE,
-		       SBE_2T3E3_FRAMER_VAL_T3_CBIT |
-		       SBE_2T3E3_FRAMER_VAL_INTERRUPT_ENABLE_RESET |
-		       SBE_2T3E3_FRAMER_VAL_TIMING_ASYNCH_TXINCLK);
-
-	exar7250_write(sc, SBE_2T3E3_FRAMER_REG_IO_CONTROL,
-		       SBE_2T3E3_FRAMER_VAL_DISABLE_TX_LOSS_OF_CLOCK |
-		       SBE_2T3E3_FRAMER_VAL_DISABLE_RX_LOSS_OF_CLOCK |
-		       SBE_2T3E3_FRAMER_VAL_AMI_LINE_CODE |
-		       SBE_2T3E3_FRAMER_VAL_RX_LINE_CLOCK_INVERT);
-
-	exar7250_set_frame_type(sc, SBE_2T3E3_FRAME_TYPE_T3_CBIT);
-}
-
-void exar7250_set_frame_type(struct channel *sc, u32 type)
-{
-	u32 val;
-
-	switch (type) {
-	case SBE_2T3E3_FRAME_TYPE_E3_G751:
-	case SBE_2T3E3_FRAME_TYPE_E3_G832:
-	case SBE_2T3E3_FRAME_TYPE_T3_CBIT:
-	case SBE_2T3E3_FRAME_TYPE_T3_M13:
-		break;
-	default:
-		return;
-	}
-
-	exar7250_stop_intr(sc, type);
-
-	val = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_OPERATING_MODE);
-	val &= ~(SBE_2T3E3_FRAMER_VAL_LOCAL_LOOPBACK_MODE |
-		 SBE_2T3E3_FRAMER_VAL_T3_E3_SELECT |
-		 SBE_2T3E3_FRAMER_VAL_FRAME_FORMAT_SELECT);
-	switch (type) {
-	case SBE_2T3E3_FRAME_TYPE_E3_G751:
-		val |= SBE_2T3E3_FRAMER_VAL_E3_G751;
-		break;
-	case SBE_2T3E3_FRAME_TYPE_E3_G832:
-		val |= SBE_2T3E3_FRAMER_VAL_E3_G832;
-		break;
-	case SBE_2T3E3_FRAME_TYPE_T3_CBIT:
-		val |= SBE_2T3E3_FRAMER_VAL_T3_CBIT;
-		break;
-	case SBE_2T3E3_FRAME_TYPE_T3_M13:
-		val |= SBE_2T3E3_FRAMER_VAL_T3_M13;
-		break;
-	default:
-		return;
-	}
-	exar7250_write(sc, SBE_2T3E3_FRAMER_REG_OPERATING_MODE, val);
-	exar7250_start_intr(sc, type);
-}
-
-
-void exar7250_start_intr(struct channel *sc, u32 type)
-{
-	u32 val;
-
-	switch (type) {
-	case SBE_2T3E3_FRAME_TYPE_E3_G751:
-	case SBE_2T3E3_FRAME_TYPE_E3_G832:
-		val = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_RX_CONFIGURATION_STATUS_2);
-
-		cpld_LOS_update(sc);
-
-		sc->s.OOF = val & SBE_2T3E3_FRAMER_VAL_E3_RX_OOF ? 1 : 0;
-		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_STATUS_1);
-		exar7250_write(sc, SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_ENABLE_1,
-			       SBE_2T3E3_FRAMER_VAL_E3_RX_OOF_INTERRUPT_ENABLE |
-			       SBE_2T3E3_FRAMER_VAL_E3_RX_LOS_INTERRUPT_ENABLE);
-
-		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_STATUS_2);
-		break;
-
-	case SBE_2T3E3_FRAME_TYPE_T3_CBIT:
-	case SBE_2T3E3_FRAME_TYPE_T3_M13:
-		val = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_CONFIGURATION_STATUS);
-
-		cpld_LOS_update(sc);
-
-		sc->s.OOF = val & SBE_2T3E3_FRAMER_VAL_T3_RX_OOF ? 1 : 0;
-
-		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_INTERRUPT_STATUS);
-		exar7250_write(sc, SBE_2T3E3_FRAMER_REG_T3_RX_INTERRUPT_ENABLE,
-			       SBE_2T3E3_FRAMER_VAL_T3_RX_LOS_INTERRUPT_ENABLE |
-			       SBE_2T3E3_FRAMER_VAL_T3_RX_OOF_INTERRUPT_ENABLE);
-
-		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_FEAC_INTERRUPT_ENABLE_STATUS);
-
-		exar7250_write(sc, SBE_2T3E3_FRAMER_REG_T3_RX_LAPD_CONTROL, 0);
-		break;
-
-	default:
-		return;
-	}
-
-	exar7250_read(sc, SBE_2T3E3_FRAMER_REG_BLOCK_INTERRUPT_STATUS);
-	exar7250_write(sc, SBE_2T3E3_FRAMER_REG_BLOCK_INTERRUPT_ENABLE,
-		       SBE_2T3E3_FRAMER_VAL_RX_INTERRUPT_ENABLE |
-		       SBE_2T3E3_FRAMER_VAL_TX_INTERRUPT_ENABLE);
-}
-
-
-void exar7250_stop_intr(struct channel *sc, u32 type)
-{
-	exar7250_write(sc, SBE_2T3E3_FRAMER_REG_BLOCK_INTERRUPT_ENABLE, 0);
-	exar7250_read(sc, SBE_2T3E3_FRAMER_REG_BLOCK_INTERRUPT_STATUS);
-
-	switch (type) {
-	case SBE_2T3E3_FRAME_TYPE_E3_G751:
-	case SBE_2T3E3_FRAME_TYPE_E3_G832:
-		exar7250_write(sc, SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_ENABLE_1, 0);
-		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_STATUS_1);
-		exar7250_write(sc, SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_ENABLE_2, 0);
-		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_STATUS_2);
-		exar7250_write(sc, SBE_2T3E3_FRAMER_REG_E3_RX_LAPD_CONTROL, 0);
-		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_RX_LAPD_CONTROL);
-		exar7250_write(sc, SBE_2T3E3_FRAMER_REG_E3_TX_LAPD_STATUS, 0);
-		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_TX_LAPD_STATUS);
-		break;
-
-	case SBE_2T3E3_FRAME_TYPE_T3_CBIT:
-	case SBE_2T3E3_FRAME_TYPE_T3_M13:
-		exar7250_write(sc, SBE_2T3E3_FRAMER_REG_T3_RX_INTERRUPT_ENABLE, 0);
-		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_INTERRUPT_STATUS);
-		exar7250_write(sc, SBE_2T3E3_FRAMER_REG_T3_RX_FEAC_INTERRUPT_ENABLE_STATUS, 0);
-		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_FEAC_INTERRUPT_ENABLE_STATUS);
-		exar7250_write(sc, SBE_2T3E3_FRAMER_REG_T3_RX_LAPD_CONTROL, 0);
-		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_LAPD_CONTROL);
-		exar7250_write(sc, SBE_2T3E3_FRAMER_REG_T3_TX_FEAC_CONFIGURATION_STATUS, 0);
-		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_TX_FEAC_CONFIGURATION_STATUS);
-		exar7250_write(sc, SBE_2T3E3_FRAMER_REG_T3_TX_LAPD_STATUS, 0);
-		exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_TX_LAPD_STATUS);
-		break;
-	}
-}
-
-
-
-
-void exar7250_unipolar_onoff(struct channel *sc, u32 mode)
-{
-	switch (mode) {
-	case SBE_2T3E3_OFF:
-		exar7300_clear_bit(sc, SBE_2T3E3_FRAMER_REG_IO_CONTROL,
-				   SBE_2T3E3_FRAMER_VAL_UNIPOLAR);
-		break;
-	case SBE_2T3E3_ON:
-		exar7300_set_bit(sc, SBE_2T3E3_FRAMER_REG_IO_CONTROL,
-				 SBE_2T3E3_FRAMER_VAL_UNIPOLAR);
-		break;
-	}
-}
-
-void exar7250_set_loopback(struct channel *sc, u32 mode)
-{
-	switch (mode) {
-	case SBE_2T3E3_FRAMER_VAL_LOOPBACK_OFF:
-		exar7300_clear_bit(sc, SBE_2T3E3_FRAMER_REG_OPERATING_MODE,
-				   SBE_2T3E3_FRAMER_VAL_LOCAL_LOOPBACK_MODE);
-		break;
-	case SBE_2T3E3_FRAMER_VAL_LOOPBACK_ON:
-		exar7300_set_bit(sc, SBE_2T3E3_FRAMER_REG_OPERATING_MODE,
-				 SBE_2T3E3_FRAMER_VAL_LOCAL_LOOPBACK_MODE);
-		break;
-	}
-}
diff --git a/drivers/staging/sbe-2t3e3/exar7300.c b/drivers/staging/sbe-2t3e3/exar7300.c
deleted file mode 100644
index cd229998a62..00000000000
--- a/drivers/staging/sbe-2t3e3/exar7300.c
+++ /dev/null
@@ -1,165 +0,0 @@
-/*
- * SBE 2T3E3 synchronous serial card driver for Linux
- *
- * Copyright (C) 2009-2010 Krzysztof Halasa <khc@pm.waw.pl>
- *
- * 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.
- *
- * This code is based on a driver written by SBE Inc.
- */
-
-#include "2t3e3.h"
-#include "ctrl.h"
-
-void exar7300_init(struct channel *sc)
-{
-	exar7300_write(sc, SBE_2T3E3_LIU_REG_REG1, 0);
-
-	/* enable line decodeer and encoder */
-	exar7300_write(sc, SBE_2T3E3_LIU_REG_REG2, 0);
-	exar7300_write(sc, SBE_2T3E3_LIU_REG_REG3, 0);
-	exar7300_write(sc, SBE_2T3E3_LIU_REG_REG4,
-		       SBE_2T3E3_LIU_VAL_T3_MODE_SELECT |
-		       SBE_2T3E3_LIU_VAL_LOOPBACK_OFF);
-}
-
-void exar7300_set_loopback(struct channel *sc, u32 mode)
-{
-	u32 val;
-
-	switch (mode) {
-	case SBE_2T3E3_LIU_VAL_LOOPBACK_OFF:
-	case SBE_2T3E3_LIU_VAL_LOOPBACK_REMOTE:
-	case SBE_2T3E3_LIU_VAL_LOOPBACK_ANALOG:
-	case SBE_2T3E3_LIU_VAL_LOOPBACK_DIGITAL:
-		break;
-	default:
-		return;
-	}
-
-	val = exar7300_read(sc, SBE_2T3E3_LIU_REG_REG4);
-	val &= ~(SBE_2T3E3_LIU_VAL_LOCAL_LOOPBACK | SBE_2T3E3_LIU_VAL_REMOTE_LOOPBACK);
-	val |= mode;
-	exar7300_write(sc, SBE_2T3E3_LIU_REG_REG4, val);
-}
-
-void exar7300_set_frame_type(struct channel *sc, u32 type)
-{
-	u32 val;
-
-	switch (type) {
-	case SBE_2T3E3_FRAME_TYPE_T3_CBIT:
-	case SBE_2T3E3_FRAME_TYPE_T3_M13:
-	case SBE_2T3E3_FRAME_TYPE_E3_G751:
-	case SBE_2T3E3_FRAME_TYPE_E3_G832:
-		break;
-	default:
-		return;
-	}
-
-	val = exar7300_read(sc, SBE_2T3E3_LIU_REG_REG4);
-	val &= ~(SBE_2T3E3_LIU_VAL_T3_MODE_SELECT |
-		 SBE_2T3E3_LIU_VAL_E3_MODE_SELECT);
-
-	switch (type) {
-	case SBE_2T3E3_FRAME_TYPE_T3_CBIT:
-	case SBE_2T3E3_FRAME_TYPE_T3_M13:
-		val |= SBE_2T3E3_LIU_VAL_T3_MODE_SELECT;
-		break;
-	case SBE_2T3E3_FRAME_TYPE_E3_G751:
-	case SBE_2T3E3_FRAME_TYPE_E3_G832:
-		val |= SBE_2T3E3_LIU_VAL_E3_MODE_SELECT;
-		break;
-	default:
-		return;
-	}
-
-	exar7300_write(sc, SBE_2T3E3_LIU_REG_REG4, val);
-}
-
-
-void exar7300_transmit_all_ones_onoff(struct channel *sc, u32 mode)
-{
-	if (sc->p.transmit_all_ones == mode)
-		return;
-
-	switch (mode) {
-	case SBE_2T3E3_ON:
-		exar7300_set_bit(sc, SBE_2T3E3_LIU_REG_REG1,
-				 SBE_2T3E3_LIU_VAL_TRANSMIT_ALL_ONES);
-		break;
-	case SBE_2T3E3_OFF:
-		exar7300_clear_bit(sc, SBE_2T3E3_LIU_REG_REG1,
-				   SBE_2T3E3_LIU_VAL_TRANSMIT_ALL_ONES);
-		break;
-	default:
-		return;
-	}
-
-	sc->p.transmit_all_ones = mode;
-}
-
-void exar7300_receive_equalization_onoff(struct channel *sc, u32 mode)
-{
-	if (sc->p.receive_equalization == mode)
-		return;
-
-	switch (mode) {
-	case SBE_2T3E3_OFF:
-		exar7300_set_bit(sc, SBE_2T3E3_LIU_REG_REG2,
-				 SBE_2T3E3_LIU_VAL_RECEIVE_EQUALIZATION_DISABLE);
-		break;
-	case SBE_2T3E3_ON:
-		exar7300_clear_bit(sc, SBE_2T3E3_LIU_REG_REG2,
-				   SBE_2T3E3_LIU_VAL_RECEIVE_EQUALIZATION_DISABLE);
-		break;
-	default:
-		return;
-	}
-
-	sc->p.receive_equalization = mode;
-}
-
-void exar7300_line_build_out_onoff(struct channel *sc, u32 mode)
-{
-	if (sc->p.line_build_out == mode)
-		return;
-
-	switch (mode) {
-	case SBE_2T3E3_OFF:
-		exar7300_set_bit(sc, SBE_2T3E3_LIU_REG_REG1,
-				 SBE_2T3E3_LIU_VAL_TRANSMIT_LEVEL_SELECT);
-		exar7300_receive_equalization_onoff(sc, SBE_2T3E3_OFF);
-		break;
-	case SBE_2T3E3_ON:
-		exar7300_clear_bit(sc, SBE_2T3E3_LIU_REG_REG1,
-				   SBE_2T3E3_LIU_VAL_TRANSMIT_LEVEL_SELECT);
-		exar7300_receive_equalization_onoff(sc, SBE_2T3E3_ON);
-		break;
-	default:
-		return;
-	}
-
-	sc->p.line_build_out = mode;
-}
-
-/* TODO - what about encoder in raw mode??? disable it too? */
-void exar7300_unipolar_onoff(struct channel *sc, u32 mode)
-{
-	switch (mode) {
-	case SBE_2T3E3_OFF:
-		exar7300_clear_bit(sc, SBE_2T3E3_LIU_REG_REG3,
-				   SBE_2T3E3_LIU_VAL_DECODER_DISABLE);
-		exar7300_clear_bit(sc, SBE_2T3E3_LIU_REG_REG1,
-				   SBE_2T3E3_LIU_VAL_TRANSMIT_BINARY_DATA);
-		break;
-	case SBE_2T3E3_ON:
-		exar7300_set_bit(sc, SBE_2T3E3_LIU_REG_REG3,
-				 SBE_2T3E3_LIU_VAL_DECODER_DISABLE);
-		exar7300_set_bit(sc, SBE_2T3E3_LIU_REG_REG1,
-				 SBE_2T3E3_LIU_VAL_TRANSMIT_BINARY_DATA);
-		break;
-	}
-}
diff --git a/drivers/staging/sbe-2t3e3/intr.c b/drivers/staging/sbe-2t3e3/intr.c
deleted file mode 100644
index 1bf74b78800..00000000000
--- a/drivers/staging/sbe-2t3e3/intr.c
+++ /dev/null
@@ -1,579 +0,0 @@
-/*
- * SBE 2T3E3 synchronous serial card driver for Linux
- *
- * Copyright (C) 2009-2010 Krzysztof Halasa <khc@pm.waw.pl>
- *
- * 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.
- *
- * This code is based on a driver written by SBE Inc.
- */
-
-#include <linux/hdlc.h>
-#include <linux/interrupt.h>
-#include <linux/netdevice.h>
-#include "2t3e3.h"
-
-irqreturn_t t3e3_intr(int irq, void *dev_instance)
-{
-	struct channel *sc = dev_to_priv(dev_instance);
-	u32 val;
-	irqreturn_t ret = IRQ_NONE;
-
-	sc->interrupt_active = 1;
-
-	val = cpld_read(sc, SBE_2T3E3_CPLD_REG_PICSR);
-
-	if (val & SBE_2T3E3_CPLD_VAL_RECEIVE_LOSS_OF_SIGNAL_CHANGE) {
-		dev_dbg(&sc->pdev->dev,
-			"Rx LOS Chng Int r=%02x (LOS|OOF=%02x)\n",
-			val, (sc->s.LOS << 4) | sc->s.OOF);
-		cpld_LOS_update(sc);
-		ret = IRQ_HANDLED;
-	}
-
-	if (val & SBE_2T3E3_CPLD_VAL_INTERRUPT_FROM_ETHERNET_ASSERTED) {
-		dc_intr(sc);
-		ret = IRQ_HANDLED;
-	}
-
-	if (val & SBE_2T3E3_CPLD_VAL_INTERRUPT_FROM_FRAMER_ASSERTED) {
-		exar7250_intr(sc);
-		ret = IRQ_HANDLED;
-	}
-
-	/*
-	  we don't care about other interrupt sources (DMO, LOS, LCV) because
-	  they are handled by Framer too
-	*/
-
-	sc->interrupt_active = 0;
-	return ret;
-}
-
-void dc_intr(struct channel *sc)
-{
-	u32 val;
-
-	/* disable ethernet interrupts */
-	/* grrr this clears interrupt summary bits !!! */
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_INTERRUPT_ENABLE, 0);
-
-	while ((val = dc_read(sc->addr, SBE_2T3E3_21143_REG_STATUS)) &
-	       (SBE_2T3E3_21143_VAL_RECEIVE_PROCESS_STOPPED |
-		SBE_2T3E3_21143_VAL_RECEIVE_BUFFER_UNAVAILABLE |
-		SBE_2T3E3_21143_VAL_RECEIVE_INTERRUPT |
-		SBE_2T3E3_21143_VAL_TRANSMIT_UNDERFLOW |
-		SBE_2T3E3_21143_VAL_TRANSMIT_BUFFER_UNAVAILABLE |
-		SBE_2T3E3_21143_VAL_TRANSMIT_PROCESS_STOPPED |
-		SBE_2T3E3_21143_VAL_TRANSMIT_INTERRUPT)) {
-		dc_write(sc->addr, SBE_2T3E3_21143_REG_STATUS, val);
-
-		dev_dbg(&sc->pdev->dev, "SBE 2T3E3: Ethernet controller interrupt! (CSR5 = %08X)\n",
-			val);
-
-		if (val & (SBE_2T3E3_21143_VAL_RECEIVE_INTERRUPT |
-			   SBE_2T3E3_21143_VAL_RECEIVE_BUFFER_UNAVAILABLE |
-			   SBE_2T3E3_21143_VAL_RECEIVE_PROCESS_STOPPED)) {
-			if (val & SBE_2T3E3_21143_VAL_RECEIVE_INTERRUPT)
-				dev_dbg(&sc->pdev->dev,
-					"Receive interrupt (LOS=%d, OOF=%d)\n",
-					sc->s.LOS, sc->s.OOF);
-			if (val & SBE_2T3E3_21143_VAL_RECEIVE_BUFFER_UNAVAILABLE)
-				dev_dbg(&sc->pdev->dev,
-					"Receive buffer unavailable\n");
-			if (val & SBE_2T3E3_21143_VAL_RECEIVE_PROCESS_STOPPED)
-				dev_dbg(&sc->pdev->dev,
-					"Receive process stopped\n");
-			dc_intr_rx(sc);
-		}
-
-		if (val & SBE_2T3E3_21143_VAL_TRANSMIT_UNDERFLOW) {
-			dev_dbg(&sc->pdev->dev, "Transmit underflow\n");
-			dc_intr_tx_underflow(sc);
-		}
-
-		if (val & (SBE_2T3E3_21143_VAL_TRANSMIT_BUFFER_UNAVAILABLE |
-			   SBE_2T3E3_21143_VAL_TRANSMIT_INTERRUPT |
-			   SBE_2T3E3_21143_VAL_TRANSMIT_PROCESS_STOPPED)) {
-			if (val & SBE_2T3E3_21143_VAL_TRANSMIT_INTERRUPT)
-				dev_dbg(&sc->pdev->dev, "Transmit interrupt\n");
-			if (val & SBE_2T3E3_21143_VAL_TRANSMIT_BUFFER_UNAVAILABLE)
-				dev_dbg(&sc->pdev->dev,
-					"Transmit buffer unavailable\n");
-			if (val & SBE_2T3E3_21143_VAL_TRANSMIT_PROCESS_STOPPED)
-				dev_dbg(&sc->pdev->dev,
-					"Transmit process stopped\n");
-			dc_intr_tx(sc);
-		}
-	}
-
-	/* enable ethernet interrupts */
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_INTERRUPT_ENABLE,
-		 sc->ether.interrupt_enable_mask);
-}
-
-void dc_intr_rx(struct channel *sc)
-{
-	u32 current_read;
-	u32 error_mask, error;
-	struct t3e3_rx_desc *current_desc;
-	struct sk_buff *m, *m2;
-	unsigned rcv_len;
-
-	sc->rcv_count++; /* for the activity LED */
-
-	current_read = sc->ether.rx_ring_current_read;
-	dev_dbg(&sc->pdev->dev, "intr_rx current_read = %d\n", current_read);
-
-	/* when ethernet loopback is set, ignore framer signals */
-	if ((sc->p.loopback != SBE_2T3E3_LOOPBACK_ETHERNET) && sc->s.OOF) {
-		while (!(sc->ether.rx_ring[current_read].rdes0 &
-			 SBE_2T3E3_RX_DESC_21143_OWN)) {
-			current_desc = &sc->ether.rx_ring[current_read];
-			current_desc->rdes1 &= SBE_2T3E3_RX_DESC_END_OF_RING |
-				SBE_2T3E3_RX_DESC_SECOND_ADDRESS_CHAINED;
-			current_desc->rdes1 |= SBE_2T3E3_MTU;
-			current_desc->rdes0 = SBE_2T3E3_RX_DESC_21143_OWN;
-			current_read = (current_read + 1) % SBE_2T3E3_RX_DESC_RING_SIZE;
-		}
-		sc->ether.rx_ring_current_read = current_read;
-		return;
-	}
-
-	while (!(sc->ether.rx_ring[current_read].rdes0 &
-		 SBE_2T3E3_RX_DESC_21143_OWN)) {
-		current_desc = &sc->ether.rx_ring[current_read];
-
-		dev_dbg(&sc->pdev->dev, "rdes0: %08X        rdes1: %08X\n",
-			current_desc->rdes0, current_desc->rdes1);
-
-		m = sc->ether.rx_data[current_read];
-		rcv_len = (current_desc->rdes0 & SBE_2T3E3_RX_DESC_FRAME_LENGTH) >>
-			SBE_2T3E3_RX_DESC_FRAME_LENGTH_SHIFT;
-
-		dev_dbg(&sc->pdev->dev, "mbuf was received (mbuf len = %d)\n",
-			rcv_len);
-
-		switch (sc->p.crc) {
-		case SBE_2T3E3_CRC_16:
-			rcv_len -= SBE_2T3E3_CRC16_LENGTH;
-			break;
-		case SBE_2T3E3_CRC_32:
-			rcv_len -= SBE_2T3E3_CRC32_LENGTH;
-			break;
-		default:
-			break;
-		}
-
-		if (current_desc->rdes0 & SBE_2T3E3_RX_DESC_LAST_DESC) {
-
-			/* TODO: is collision possible? */
-			error_mask = SBE_2T3E3_RX_DESC_DESC_ERROR |
-				SBE_2T3E3_RX_DESC_COLLISION_SEEN |
-				SBE_2T3E3_RX_DESC_DRIBBLING_BIT;
-
-			switch (sc->p.frame_mode) {
-			case SBE_2T3E3_FRAME_MODE_HDLC:
-				error_mask |= SBE_2T3E3_RX_DESC_MII_ERROR;
-				if (sc->p.crc == SBE_2T3E3_CRC_32)
-					error_mask |= SBE_2T3E3_RX_DESC_CRC_ERROR;
-				break;
-			case SBE_2T3E3_FRAME_MODE_TRANSPARENT:
-			case SBE_2T3E3_FRAME_MODE_RAW:
-				break;
-			default:
-				error_mask = 0;
-			}
-
-			if (sc->s.LOS) {
-				error_mask &= ~(SBE_2T3E3_RX_DESC_DRIBBLING_BIT |
-						SBE_2T3E3_RX_DESC_MII_ERROR);
-			}
-
-			error = current_desc->rdes0 & error_mask;
-			if (error) {
-				sc->s.in_errors++;
-				dev_dbg(&sc->pdev->dev,
-					"error interrupt: NO_ERROR_MESSAGE = %d\n",
-					sc->r.flags & SBE_2T3E3_FLAG_NO_ERROR_MESSAGES ? 1 : 0);
-
-				current_desc->rdes1 &= SBE_2T3E3_RX_DESC_END_OF_RING |
-					SBE_2T3E3_RX_DESC_SECOND_ADDRESS_CHAINED;
-				current_desc->rdes1 |= SBE_2T3E3_MTU;
-				current_desc->rdes0 = SBE_2T3E3_RX_DESC_21143_OWN;
-
-				if (error & SBE_2T3E3_RX_DESC_DESC_ERROR) {
-					if (!(sc->r.flags & SBE_2T3E3_FLAG_NO_ERROR_MESSAGES))
-						dev_err(&sc->pdev->dev,
-							"SBE 2T3E3: descriptor error\n");
-					sc->s.in_error_desc++;
-				}
-
-				if (error & SBE_2T3E3_RX_DESC_COLLISION_SEEN) {
-					if (!(sc->r.flags & SBE_2T3E3_FLAG_NO_ERROR_MESSAGES))
-						dev_err(&sc->pdev->dev,
-							"SBE 2T3E3: collision seen\n");
-					sc->s.in_error_coll++;
-				} else {
-					if (error & SBE_2T3E3_RX_DESC_DRIBBLING_BIT) {
-						if (!(sc->r.flags & SBE_2T3E3_FLAG_NO_ERROR_MESSAGES))
-							dev_err(&sc->pdev->dev,
-								"SBE 2T3E3: dribbling bits error\n");
-						sc->s.in_error_drib++;
-					}
-
-					if (error & SBE_2T3E3_RX_DESC_CRC_ERROR) {
-						if (!(sc->r.flags & SBE_2T3E3_FLAG_NO_ERROR_MESSAGES))
-							dev_err(&sc->pdev->dev,
-								"SBE 2T3E3: crc error\n");
-						sc->s.in_error_crc++;
-					}
-				}
-
-				if (error & SBE_2T3E3_RX_DESC_MII_ERROR) {
-					if (!(sc->r.flags & SBE_2T3E3_FLAG_NO_ERROR_MESSAGES))
-						dev_err(&sc->pdev->dev, "SBE 2T3E3: mii error\n");
-					sc->s.in_error_mii++;
-				}
-
-				current_read = (current_read + 1) % SBE_2T3E3_RX_DESC_RING_SIZE;
-				sc->r.flags |= SBE_2T3E3_FLAG_NO_ERROR_MESSAGES;
-				continue;
-			}
-		}
-
-		current_desc->rdes1 &= SBE_2T3E3_RX_DESC_END_OF_RING |
-			SBE_2T3E3_RX_DESC_SECOND_ADDRESS_CHAINED;
-		current_desc->rdes1 |= SBE_2T3E3_MTU;
-
-		if (rcv_len > 1600) {
-			sc->s.in_errors++;
-			sc->s.in_dropped++;
-			if (!(sc->r.flags & SBE_2T3E3_FLAG_NO_ERROR_MESSAGES))
-				dev_err(&sc->pdev->dev, "SBE 2T3E3: oversized rx: rdes0 = %08X\n",
-					current_desc->rdes0);
-		} else {
-			m2 = dev_alloc_skb(MCLBYTES);
-			if (m2 != NULL) {
-				current_desc->rdes2 = virt_to_phys(m2->data);
-				sc->ether.rx_data[current_read] = m2;
-				sc->s.in_packets++;
-				sc->s.in_bytes += rcv_len;
-				m->dev = sc->dev;
-				skb_put(m, rcv_len);
-				skb_reset_mac_header(m);
-				m->protocol = hdlc_type_trans(m, m->dev);
-				netif_rx(m);
-
-				/* good packet was received so we will show error messages again... */
-				if (sc->r.flags & SBE_2T3E3_FLAG_NO_ERROR_MESSAGES) {
-					dev_dbg(&sc->pdev->dev,
-						"setting ERROR_MESSAGES->0\n");
-					sc->r.flags &= ~SBE_2T3E3_FLAG_NO_ERROR_MESSAGES;
-				}
-
-			} else {
-				sc->s.in_errors++;
-				sc->s.in_dropped++;
-			}
-		}
-		current_desc->rdes0 = SBE_2T3E3_RX_DESC_21143_OWN;
-		current_read = (current_read + 1) % SBE_2T3E3_RX_DESC_RING_SIZE;
-	}
-
-	sc->ether.rx_ring_current_read = current_read;
-
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_RECEIVE_POLL_DEMAND, 0xFFFFFFFF);
-}
-
-void dc_intr_tx(struct channel *sc)
-{
-	u32 current_read, current_write;
-	u32 last_segment, error;
-	struct t3e3_tx_desc *current_desc;
-
-	spin_lock(&sc->ether.tx_lock);
-
-	current_read = sc->ether.tx_ring_current_read;
-	current_write = sc->ether.tx_ring_current_write;
-
-	while (current_read != current_write) {
-		current_desc = &sc->ether.tx_ring[current_read];
-
-		if (current_desc->tdes0 & SBE_2T3E3_RX_DESC_21143_OWN)
-			break;
-
-		dev_dbg(&sc->pdev->dev,
-			"txeof: tdes0 = %08X        tdes1 = %08X\n",
-			current_desc->tdes0, current_desc->tdes1);
-
-		error = current_desc->tdes0 & (SBE_2T3E3_TX_DESC_ERROR_SUMMARY |
-					       SBE_2T3E3_TX_DESC_TRANSMIT_JABBER_TIMEOUT |
-					       SBE_2T3E3_TX_DESC_LOSS_OF_CARRIER |
-					       SBE_2T3E3_TX_DESC_NO_CARRIER |
-					       SBE_2T3E3_TX_DESC_LINK_FAIL_REPORT |
-					       SBE_2T3E3_TX_DESC_UNDERFLOW_ERROR |
-					       SBE_2T3E3_TX_DESC_DEFFERED);
-
-		last_segment = current_desc->tdes1 & SBE_2T3E3_TX_DESC_LAST_SEGMENT;
-
-		current_desc->tdes0 = 0;
-		current_desc->tdes1 &= SBE_2T3E3_TX_DESC_END_OF_RING |
-			SBE_2T3E3_TX_DESC_SECOND_ADDRESS_CHAINED;
-		current_desc->tdes2 = 0;
-		sc->ether.tx_free_cnt++;
-
-		if (last_segment != SBE_2T3E3_TX_DESC_LAST_SEGMENT) {
-			current_read = (current_read + 1) % SBE_2T3E3_TX_DESC_RING_SIZE;
-			continue;
-		}
-
-
-		if (sc->ether.tx_data[current_read]) {
-			sc->s.out_packets++;
-			sc->s.out_bytes += sc->ether.tx_data[current_read]->len;
-			dev_kfree_skb_any(sc->ether.tx_data[current_read]);
-			sc->ether.tx_data[current_read] = NULL;
-		}
-
-		if (error > 0) {
-			sc->s.out_errors++;
-
-			if (error & SBE_2T3E3_TX_DESC_TRANSMIT_JABBER_TIMEOUT) {
-				dev_err(&sc->pdev->dev, "SBE 2T3E3: transmit jabber timeout\n");
-				sc->s.out_error_jab++;
-			}
-
-			if (sc->p.loopback != SBE_2T3E3_LOOPBACK_ETHERNET) {
-				if (error & SBE_2T3E3_TX_DESC_LOSS_OF_CARRIER) {
-					dev_err(&sc->pdev->dev, "SBE 2T3E3: loss of carrier\n");
-					sc->s.out_error_lost_carr++;
-				}
-
-				if (error & SBE_2T3E3_TX_DESC_NO_CARRIER) {
-					dev_err(&sc->pdev->dev, "SBE 2T3E3: no carrier\n");
-					sc->s.out_error_no_carr++;
-				}
-			}
-
-			if (error & SBE_2T3E3_TX_DESC_LINK_FAIL_REPORT) {
-				dev_err(&sc->pdev->dev, "SBE 2T3E3: link fail report\n");
-				sc->s.out_error_link_fail++;
-			}
-
-			if (error & SBE_2T3E3_TX_DESC_UNDERFLOW_ERROR) {
-				dev_err(&sc->pdev->dev, "SBE 2T3E3:"
-					" transmission underflow error\n");
-				sc->s.out_error_underflow++;
-				spin_unlock(&sc->ether.tx_lock);
-
-				dc_restart(sc);
-				return;
-			}
-
-			if (error & SBE_2T3E3_TX_DESC_DEFFERED) {
-				dev_err(&sc->pdev->dev, "SBE 2T3E3: transmission deferred\n");
-				sc->s.out_error_dereferred++;
-			}
-		}
-
-		current_read = (current_read + 1) % SBE_2T3E3_TX_DESC_RING_SIZE;
-	}
-
-	sc->ether.tx_ring_current_read = current_read;
-
-	/* Relieve flow control when the TX queue is drained at least half way */
-	if (sc->ether.tx_full &&
-	    (sc->ether.tx_free_cnt >= (SBE_2T3E3_TX_DESC_RING_SIZE / 2))) {
-		sc->ether.tx_full = 0;
-		netif_wake_queue(sc->dev);
-	}
-	spin_unlock(&sc->ether.tx_lock);
-}
-
-
-void dc_intr_tx_underflow(struct channel *sc)
-{
-	u32 val;
-
-	dc_transmitter_onoff(sc, SBE_2T3E3_OFF);
-
-	val = dc_read(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE);
-	dc_clear_bits(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE,
-		      SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS);
-
-	switch (val & SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS) {
-	case SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS_1:
-		dc_set_bits(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE,
-			    SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS_2);
-		break;
-	case SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS_2:
-		dc_set_bits(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE,
-			    SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS_3);
-		break;
-	case SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS_3:
-		dc_set_bits(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE,
-			    SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS_4);
-		break;
-	case SBE_2T3E3_21143_VAL_THRESHOLD_CONTROL_BITS_4:
-	default:
-		dc_set_bits(sc->addr, SBE_2T3E3_21143_REG_OPERATION_MODE,
-			    SBE_2T3E3_21143_VAL_STORE_AND_FORWARD);
-		break;
-	}
-
-	dc_transmitter_onoff(sc, SBE_2T3E3_ON);
-}
-
-
-
-
-void exar7250_intr(struct channel *sc)
-{
-	u32 status, old_OOF;
-
-	old_OOF = sc->s.OOF;
-
-	status = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_BLOCK_INTERRUPT_STATUS);
-	dev_dbg(&sc->pdev->dev, "SBE 2T3E3: Framer interrupt! (REG[0x05] = %02X)\n", status);
-
-	switch (sc->p.frame_type) {
-	case SBE_2T3E3_FRAME_TYPE_E3_G751:
-	case SBE_2T3E3_FRAME_TYPE_E3_G832:
-		exar7250_E3_intr(sc, status);
-		break;
-
-	case SBE_2T3E3_FRAME_TYPE_T3_CBIT:
-	case SBE_2T3E3_FRAME_TYPE_T3_M13:
-		exar7250_T3_intr(sc, status);
-		break;
-
-	default:
-		break;
-	}
-
-	if (sc->s.OOF != old_OOF) {
-		if (sc->s.OOF) {
-			if (sc->p.loopback == SBE_2T3E3_LOOPBACK_NONE) {
-				dev_dbg(&sc->pdev->dev, "SBE 2T3E3: Disabling eth interrupts\n");
-				/* turn off ethernet interrupts */
-				dc_stop_intr(sc);
-			}
-		} else if (sc->r.flags & SBE_2T3E3_FLAG_NETWORK_UP) {
-			dev_dbg(&sc->pdev->dev, "SBE 2T3E3: Enabling eth interrupts\n");
-			/* start interrupts */
-			sc->s.OOF = 1;
-			dc_intr_rx(sc);
-			sc->s.OOF = 0;
-			if (sc->p.receiver_on) {
-				dc_receiver_onoff(sc, SBE_2T3E3_OFF);
-				dc_receiver_onoff(sc, SBE_2T3E3_ON);
-			}
-			dc_start_intr(sc);
-		}
-	}
-}
-
-
-void exar7250_T3_intr(struct channel *sc, u32 block_status)
-{
-	u32 status, result;
-
-	if (block_status & SBE_2T3E3_FRAMER_VAL_RX_INTERRUPT_STATUS) {
-		status = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_INTERRUPT_STATUS);
-
-		if (status) {
-			dev_dbg(&sc->pdev->dev,
-				"Framer interrupt T3 RX (REG[0x13] = %02X)\n",
-				status);
-
-			result = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_CONFIGURATION_STATUS);
-
-			cpld_LOS_update(sc);
-
-			if (status & SBE_2T3E3_FRAMER_VAL_T3_RX_OOF_INTERRUPT_STATUS) {
-				sc->s.OOF = result & SBE_2T3E3_FRAMER_VAL_T3_RX_OOF ? 1 : 0;
-				dev_dbg(&sc->pdev->dev,
-					"Framer interrupt T3: OOF (%d)\n",
-					sc->s.OOF);
-			}
-
-			exar7250_write(sc, SBE_2T3E3_FRAMER_REG_T3_RX_INTERRUPT_ENABLE,
-				       SBE_2T3E3_FRAMER_VAL_T3_RX_LOS_INTERRUPT_ENABLE |
-				       SBE_2T3E3_FRAMER_VAL_T3_RX_OOF_INTERRUPT_ENABLE);
-				}
-
-		status = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_FEAC_INTERRUPT_ENABLE_STATUS);
-		if (status) {
-			dev_dbg(&sc->pdev->dev,
-				"Framer interrupt T3 RX (REG[0x17] = %02X)\n",
-				status);
-		}
-
-		status = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_RX_LAPD_CONTROL);
-		if (status)
-			dev_dbg(&sc->pdev->dev,
-				"Framer interrupt T3 RX (REG[0x18] = %02X)\n",
-				status);
-	}
-
-
-	if (block_status & SBE_2T3E3_FRAMER_VAL_TX_INTERRUPT_STATUS) {
-		status = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_TX_FEAC_CONFIGURATION_STATUS);
-		dev_dbg(&sc->pdev->dev, "SBE 2T3E3: Framer interrupt T3 TX (REG[0x31] = %02X)\n",
-			status);
-
-		status = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_T3_TX_LAPD_STATUS);
-		dev_dbg(&sc->pdev->dev, "SBE 2T3E3: Framer interrupt T3 TX (REG[0x34] = %02X)\n",
-			status);
-	}
-}
-
-
-void exar7250_E3_intr(struct channel *sc, u32 block_status)
-{
-	u32 status, result;
-
-	if (block_status & SBE_2T3E3_FRAMER_VAL_RX_INTERRUPT_STATUS) {
-		status = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_STATUS_1);
-
-		if (status) {
-			dev_dbg(&sc->pdev->dev,
-				"Framer interrupt E3 RX (REG[0x14] = %02X)\n",
-				status);
-
-			result = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_RX_CONFIGURATION_STATUS_2);
-
-			cpld_LOS_update(sc);
-
-			if (status & SBE_2T3E3_FRAMER_VAL_E3_RX_OOF_INTERRUPT_STATUS) {
-				sc->s.OOF = result & SBE_2T3E3_FRAMER_VAL_E3_RX_OOF ? 1 : 0;
-				dev_dbg(&sc->pdev->dev,
-					"Framer interrupt E3: OOF (%d)\n",
-					sc->s.OOF);
-			}
-
-			exar7250_write(sc, SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_ENABLE_1,
-				       SBE_2T3E3_FRAMER_VAL_E3_RX_OOF_INTERRUPT_ENABLE |
-				       SBE_2T3E3_FRAMER_VAL_E3_RX_LOS_INTERRUPT_ENABLE
-				);
-				}
-
-		status = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_RX_INTERRUPT_STATUS_2);
-		if (status) {
-			dev_dbg(&sc->pdev->dev,
-				"Framer interrupt E3 RX (REG[0x15] = %02X)\n",
-				status);
-
-		}
-
-	}
-
-	if (block_status & SBE_2T3E3_FRAMER_VAL_TX_INTERRUPT_STATUS) {
-		status = exar7250_read(sc, SBE_2T3E3_FRAMER_REG_E3_TX_LAPD_STATUS);
-		dev_dbg(&sc->pdev->dev, "SBE 2T3E3: Framer interrupt E3 TX (REG[0x34] = %02X)\n",
-			status);
-	}
-}
diff --git a/drivers/staging/sbe-2t3e3/io.c b/drivers/staging/sbe-2t3e3/io.c
deleted file mode 100644
index c9947b165b3..00000000000
--- a/drivers/staging/sbe-2t3e3/io.c
+++ /dev/null
@@ -1,330 +0,0 @@
-/*
- * SBE 2T3E3 synchronous serial card driver for Linux
- *
- * Copyright (C) 2009-2010 Krzysztof Halasa <khc@pm.waw.pl>
- *
- * 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.
- *
- * This code is based on a driver written by SBE Inc.
- */
-
-#include <linux/ip.h>
-#include "2t3e3.h"
-#include "ctrl.h"
-
-/* All access to registers done via the 21143 on port 0 must be
- * protected via the card->bootrom_lock. */
-
-/* private define to be used here only - must be protected by card->bootrom_lock */
-#define cpld_write_nolock(channel, reg, val)			\
-	bootrom_write((channel), CPLD_MAP_REG(reg, channel), val)
-
-u32 cpld_read(struct channel *channel, u32 reg)
-{
-	unsigned long flags;
-	u32 val;
-
-	spin_lock_irqsave(&channel->card->bootrom_lock, flags);
-	val = bootrom_read((channel), CPLD_MAP_REG(reg, channel));
-	spin_unlock_irqrestore(&channel->card->bootrom_lock, flags);
-	return val;
-}
-
-/****************************************
- * Access via BootROM port
- ****************************************/
-
-u32 bootrom_read(struct channel *channel, u32 reg)
-{
-	unsigned long addr = channel->card->bootrom_addr;
-	u32 result;
-
-	/* select BootROM address */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_PROGRAMMING_ADDRESS, reg & 0x3FFFF);
-
-	/* select reading from BootROM */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT,
-		 SBE_2T3E3_21143_VAL_READ_OPERATION |
-		 SBE_2T3E3_21143_VAL_BOOT_ROM_SELECT);
-
-	udelay(2); /* 20 PCI cycles */
-
-	/* read from BootROM */
-	result = dc_read(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT) & 0xff;
-
-	/* reset CSR9 */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT, 0);
-
-	return result;
-}
-
-void bootrom_write(struct channel *channel, u32 reg, u32 val)
-{
-	unsigned long addr = channel->card->bootrom_addr;
-
-	/* select BootROM address */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_PROGRAMMING_ADDRESS, reg & 0x3FFFF);
-
-	/* select writting to BootROM */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT,
-		 SBE_2T3E3_21143_VAL_WRITE_OPERATION |
-		 SBE_2T3E3_21143_VAL_BOOT_ROM_SELECT |
-		 (val & 0xff));
-
-	udelay(2); /* 20 PCI cycles */
-
-	/* reset CSR9 */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT, 0);
-}
-
-
-/****************************************
- * Access via Serial I/O port
- ****************************************/
-
-static u32 serialrom_read_bit(struct channel *channel)
-{
-	unsigned long addr = channel->card->bootrom_addr;
-	u32 bit;
-
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT,
-		 SBE_2T3E3_21143_VAL_READ_OPERATION |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_SELECT |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_CLOCK |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_CHIP_SELECT);	/* clock high */
-
-	bit = (dc_read(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT) &
-	       SBE_2T3E3_21143_VAL_SERIAL_ROM_DATA_OUT) > 0 ? 1 : 0;
-
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT,
-		 SBE_2T3E3_21143_VAL_READ_OPERATION |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_SELECT |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_CHIP_SELECT);	/* clock low */
-
-	return bit;
-}
-
-static void serialrom_write_bit(struct channel *channel, u32 bit)
-{
-	unsigned long addr = channel->card->bootrom_addr;
-	u32 lastbit = -1;
-
-	bit &= 1;
-
-	if (bit != lastbit) {
-		dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT,
-			 SBE_2T3E3_21143_VAL_WRITE_OPERATION |
-			 SBE_2T3E3_21143_VAL_SERIAL_ROM_SELECT |
-			 SBE_2T3E3_21143_VAL_SERIAL_ROM_CHIP_SELECT |
-			 (bit << 2)); /* clock low */
-
-		lastbit = bit;
-	}
-
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT,
-		 SBE_2T3E3_21143_VAL_WRITE_OPERATION |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_SELECT |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_CLOCK |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_CHIP_SELECT |
-		 (bit << 2)); /* clock high */
-
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT,
-		 SBE_2T3E3_21143_VAL_WRITE_OPERATION |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_SELECT |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_CHIP_SELECT |
-		 (bit << 2)); /* clock low */
-}
-
-/****************************************
- * Access to SerialROM (eeprom)
- ****************************************/
-
-u32 t3e3_eeprom_read_word(struct channel *channel, u32 address)
-{
-	unsigned long addr = channel->card->bootrom_addr;
-	u32 i, val;
-	unsigned long flags;
-
-	address &= 0x3f;
-
-	spin_lock_irqsave(&channel->card->bootrom_lock, flags);
-
-	/* select correct Serial Chip */
-	cpld_write_nolock(channel, SBE_2T3E3_CPLD_REG_SERIAL_CHIP_SELECT,
-			  SBE_2T3E3_CPLD_VAL_EEPROM_SELECT);
-
-	/* select reading from Serial I/O Bus */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT,
-		 SBE_2T3E3_21143_VAL_READ_OPERATION |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_SELECT |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_CHIP_SELECT);        /* clock low */
-
-	/* select read operation */
-	serialrom_write_bit(channel, 0);
-	serialrom_write_bit(channel, 1);
-	serialrom_write_bit(channel, 1);
-	serialrom_write_bit(channel, 0);
-
-	for (i = 0x20; i; i >>= 1)
-		serialrom_write_bit(channel, address & i ? 1 : 0);
-
-	val = 0;
-	for (i = 0x8000; i; i >>= 1)
-		val |= (serialrom_read_bit(channel) ? i : 0);
-
-	/* Reset 21143's CSR9 */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT,
-		 SBE_2T3E3_21143_VAL_READ_OPERATION |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_SELECT |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_CHIP_SELECT);        /* clock low */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT, 0);
-
-	/* Unselect Serial Chip */
-	cpld_write_nolock(channel, SBE_2T3E3_CPLD_REG_SERIAL_CHIP_SELECT, 0);
-
-	spin_unlock_irqrestore(&channel->card->bootrom_lock, flags);
-
-	return ntohs(val);
-}
-
-
-/****************************************
- * Access to Framer
- ****************************************/
-
-u32 exar7250_read(struct channel *channel, u32 reg)
-{
-	u32 result;
-	unsigned long flags;
-
-	spin_lock_irqsave(&channel->card->bootrom_lock, flags);
-
-	result = bootrom_read(channel, cpld_reg_map[SBE_2T3E3_CPLD_REG_FRAMER_BASE_ADDRESS]
-			      [channel->h.slot] + (t3e3_framer_reg_map[reg] << 2));
-
-	spin_unlock_irqrestore(&channel->card->bootrom_lock, flags);
-
-	return result;
-}
-
-void exar7250_write(struct channel *channel, u32 reg, u32 val)
-{
-	unsigned long flags;
-
-	val &= 0xff;
-	channel->framer_regs[reg] = val;
-
-	spin_lock_irqsave(&channel->card->bootrom_lock, flags);
-
-	bootrom_write(channel, cpld_reg_map[SBE_2T3E3_CPLD_REG_FRAMER_BASE_ADDRESS]
-		      [channel->h.slot] + (t3e3_framer_reg_map[reg] << 2), val);
-
-	spin_unlock_irqrestore(&channel->card->bootrom_lock, flags);
-}
-
-
-/****************************************
- * Access to LIU
- ****************************************/
-
-u32 exar7300_read(struct channel *channel, u32 reg)
-{
-	unsigned long addr = channel->card->bootrom_addr, flags;
-	u32 i, val;
-
-	/* select correct Serial Chip */
-
-	spin_lock_irqsave(&channel->card->bootrom_lock, flags);
-
-	cpld_write_nolock(channel, SBE_2T3E3_CPLD_REG_SERIAL_CHIP_SELECT,
-			  cpld_val_map[SBE_2T3E3_CPLD_VAL_LIU_SELECT][channel->h.slot]);
-
-	/* select reading from Serial I/O Bus */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT,
-		 SBE_2T3E3_21143_VAL_READ_OPERATION |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_SELECT |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_CHIP_SELECT);	/* clock low */
-
-	/* select read operation */
-	serialrom_write_bit(channel, 1);
-
-	/* Exar7300 register address is 4 bit long */
-	reg = t3e3_liu_reg_map[reg];
-	for (i = 0; i < 4; i++, reg >>= 1) /* 4 bits of SerialROM address */
-		serialrom_write_bit(channel, reg & 1);
-	for (i = 0; i < 3; i++)	/* remaining 3 bits of SerialROM address */
-		serialrom_write_bit(channel, 0);
-
-	val = 0; /* Exar7300 register value is 5 bit long */
-	for (i = 0; i < 8; i++)	/* 8 bits of SerialROM value */
-		val += (serialrom_read_bit(channel) << i);
-
-	/* Reset 21143's CSR9 */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT,
-		 SBE_2T3E3_21143_VAL_READ_OPERATION |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_SELECT |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_CHIP_SELECT);	/* clock low */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT, 0);
-
-	/* Unselect Serial Chip */
-	cpld_write_nolock(channel, SBE_2T3E3_CPLD_REG_SERIAL_CHIP_SELECT, 0);
-
-	spin_unlock_irqrestore(&channel->card->bootrom_lock, flags);
-
-	return val;
-}
-
-void exar7300_write(struct channel *channel, u32 reg, u32 val)
-{
-	unsigned long addr = channel->card->bootrom_addr, flags;
-	u32 i;
-
-	channel->liu_regs[reg] = val;
-
-	/* select correct Serial Chip */
-
-	spin_lock_irqsave(&channel->card->bootrom_lock, flags);
-
-	cpld_write_nolock(channel, SBE_2T3E3_CPLD_REG_SERIAL_CHIP_SELECT,
-			  cpld_val_map[SBE_2T3E3_CPLD_VAL_LIU_SELECT][channel->h.slot]);
-
-	/* select writting to Serial I/O Bus */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT,
-		 SBE_2T3E3_21143_VAL_WRITE_OPERATION |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_SELECT |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_CHIP_SELECT);	/* clock low */
-
-	/* select write operation */
-	serialrom_write_bit(channel, 0);
-
-	/* Exar7300 register address is 4 bit long */
-	reg = t3e3_liu_reg_map[reg];
-	for (i = 0; i < 4; i++) {	/* 4 bits */
-		serialrom_write_bit(channel, reg & 1);
-		reg >>= 1;
-	}
-	for (i = 0; i < 3; i++)	/* remaining 3 bits of SerialROM address */
-		serialrom_write_bit(channel, 0);
-
-	/* Exar7300 register value is 5 bit long */
-	for (i = 0; i < 5; i++) {
-		serialrom_write_bit(channel, val & 1);
-		val >>= 1;
-	}
-	for (i = 0; i < 3; i++)	/* remaining 3 bits of SerialROM value */
-		serialrom_write_bit(channel, 0);
-
-	/* Reset 21143_CSR9 */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT,
-		 SBE_2T3E3_21143_VAL_WRITE_OPERATION |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_SELECT |
-		 SBE_2T3E3_21143_VAL_SERIAL_ROM_CHIP_SELECT);	/* clock low */
-	dc_write(addr, SBE_2T3E3_21143_REG_BOOT_ROM_SERIAL_ROM_AND_MII_MANAGEMENT, 0);
-
-	/* Unselect Serial Chip */
-	cpld_write_nolock(channel, SBE_2T3E3_CPLD_REG_SERIAL_CHIP_SELECT, 0);
-
-	spin_unlock_irqrestore(&channel->card->bootrom_lock, flags);
-}
diff --git a/drivers/staging/sbe-2t3e3/main.c b/drivers/staging/sbe-2t3e3/main.c
deleted file mode 100644
index c8e039860dc..00000000000
--- a/drivers/staging/sbe-2t3e3/main.c
+++ /dev/null
@@ -1,172 +0,0 @@
-/*
- * SBE 2T3E3 synchronous serial card driver for Linux
- *
- * Copyright (C) 2009-2010 Krzysztof Halasa <khc@pm.waw.pl>
- *
- * 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.
- *
- * This code is based on a driver written by SBE Inc.
- */
-
-#include <linux/interrupt.h>
-#include <linux/netdevice.h>
-#include "2t3e3.h"
-
-void t3e3_init(struct channel *sc)
-{
-	cpld_init(sc);
-	dc_reset(sc);
-	dc_init(sc);
-	exar7250_init(sc);
-	exar7300_init(sc);
-}
-
-int t3e3_if_start_xmit(struct sk_buff *skb, struct net_device *dev)
-{
-	struct channel *sc = dev_to_priv(dev);
-	u32 current_write, last_write;
-	unsigned long flags;
-	struct sk_buff *skb2;
-
-	if (skb == NULL) {
-		sc->s.out_errors++;
-		return 0;
-	}
-
-	if (sc->p.transmitter_on != SBE_2T3E3_ON) {
-		sc->s.out_errors++;
-		sc->s.out_dropped++;
-		dev_kfree_skb_any(skb);
-		return 0;
-	}
-
-	if (sc->s.OOF && sc->p.loopback == SBE_2T3E3_LOOPBACK_NONE) {
-		sc->s.out_dropped++;
-		dev_kfree_skb_any(skb);
-		return 0;
-	}
-
-	spin_lock_irqsave(&sc->ether.tx_lock, flags);
-
-	current_write = sc->ether.tx_ring_current_write;
-	for (skb2 = skb; skb2 != NULL; skb2 = NULL) {
-		if (skb2->len) {
-			if ((sc->ether.tx_ring[current_write].tdes1 &
-			     SBE_2T3E3_TX_DESC_BUFFER_1_SIZE) > 0)
-				break;
-			current_write = (current_write + 1) % SBE_2T3E3_TX_DESC_RING_SIZE;
-			/*
-			 * Leave at least 1 tx desc free so that dc_intr_tx() can
-			 * identify empty list
-			 */
-			if (current_write == sc->ether.tx_ring_current_read)
-				break;
-		}
-	}
-	if (skb2 != NULL) {
-		netif_stop_queue(sc->dev);
-		sc->ether.tx_full = 1;
-		dev_dbg(&sc->pdev->dev, "SBE 2T3E3: out of descriptors\n");
-		spin_unlock_irqrestore(&sc->ether.tx_lock, flags);
-		return NETDEV_TX_BUSY;
-	}
-
-	current_write = last_write = sc->ether.tx_ring_current_write;
-	dev_dbg(&sc->pdev->dev, "sending mbuf (current_write = %d)\n",
-		current_write);
-
-	for (skb2 = skb; skb2 != NULL; skb2 = NULL) {
-		if (skb2->len) {
-			dev_dbg(&sc->pdev->dev,
-				"sending mbuf (len = %d, next = %p)\n",
-				skb2->len, NULL);
-
-			sc->ether.tx_free_cnt--;
-			sc->ether.tx_ring[current_write].tdes0 = 0;
-			sc->ether.tx_ring[current_write].tdes1 &=
-				SBE_2T3E3_TX_DESC_END_OF_RING |
-				SBE_2T3E3_TX_DESC_SECOND_ADDRESS_CHAINED;
-/* DISABLE_PADDING sometimes gets lost somehow, hands off... */
-			sc->ether.tx_ring[current_write].tdes1 |=
-				SBE_2T3E3_TX_DESC_DISABLE_PADDING | skb2->len;
-
-			if (current_write == sc->ether.tx_ring_current_write) {
-				sc->ether.tx_ring[current_write].tdes1 |=
-					SBE_2T3E3_TX_DESC_FIRST_SEGMENT;
-			} else {
-				sc->ether.tx_ring[current_write].tdes0 =
-					SBE_2T3E3_TX_DESC_21143_OWN;
-			}
-
-			sc->ether.tx_ring[current_write].tdes2 = virt_to_phys(skb2->data);
-			sc->ether.tx_data[current_write] = NULL;
-
-			last_write = current_write;
-			current_write = (current_write + 1) % SBE_2T3E3_TX_DESC_RING_SIZE;
-		}
-	}
-
-	sc->ether.tx_data[last_write] = skb;
-	sc->ether.tx_ring[last_write].tdes1 |=
-		SBE_2T3E3_TX_DESC_LAST_SEGMENT |
-		SBE_2T3E3_TX_DESC_INTERRUPT_ON_COMPLETION;
-	sc->ether.tx_ring[sc->ether.tx_ring_current_write].tdes0 |=
-		SBE_2T3E3_TX_DESC_21143_OWN;
-	sc->ether.tx_ring_current_write = current_write;
-
-	dev_dbg(&sc->pdev->dev, "txput: tdes0 = %08X        tdes1 = %08X\n",
-		sc->ether.tx_ring[last_write].tdes0,
-		sc->ether.tx_ring[last_write].tdes1);
-
-	dc_write(sc->addr, SBE_2T3E3_21143_REG_TRANSMIT_POLL_DEMAND,
-		 0xffffffff);
-
-	spin_unlock_irqrestore(&sc->ether.tx_lock, flags);
-	return 0;
-}
-
-
-void t3e3_read_card_serial_number(struct channel *sc)
-{
-	u32 i;
-
-	for (i = 0; i < 3; i++)
-		sc->ether.card_serial_number[i] = t3e3_eeprom_read_word(sc, 10 + i);
-
-	netdev_info(sc->dev, "SBE wanPMC-2T3E3 serial number: %04X%04X%04X\n",
-		    sc->ether.card_serial_number[0],
-		    sc->ether.card_serial_number[1],
-		    sc->ether.card_serial_number[2]);
-}
-
-/*
-  bit 0 led1 (green)
-  bit 1 led1 (yellow)
-
-  bit 2 led2 (green)
-  bit 3 led2 (yellow)
-
-  bit 4 led3 (green)
-  bit 5 led3 (yellow)
-
-  bit 6 led4 (green)
-  bit 7 led4 (yellow)
-*/
-
-void update_led(struct channel *sc, int blinker)
-{
-	int leds;
-	if (sc->s.LOS)
-		leds = 0; /* led1 = off */
-	else if (sc->s.OOF)
-		leds = 2; /* led1 = yellow */
-	else if ((blinker & 1) && sc->rcv_count) {
-		leds = 0; /* led1 = off */
-		sc->rcv_count = 0;
-	} else
-		leds = 1; /* led1 = green */
-	cpld_write(sc, SBE_2T3E3_CPLD_REG_LEDR, leds);
-	sc->leds = leds;
-}
diff --git a/drivers/staging/sbe-2t3e3/maps.c b/drivers/staging/sbe-2t3e3/maps.c
deleted file mode 100644
index e5494502cde..00000000000
--- a/drivers/staging/sbe-2t3e3/maps.c
+++ /dev/null
@@ -1,101 +0,0 @@
-/*
- * SBE 2T3E3 synchronous serial card driver for Linux
- *
- * Copyright (C) 2009-2010 Krzysztof Halasa <khc@pm.waw.pl>
- *
- * 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.
- *
- * This code is based on a driver written by SBE Inc.
- */
-
-#include <linux/kernel.h>
-#include "2t3e3.h"
-
-const u32 cpld_reg_map[][2] = {
-	{ 0x0000, 0x0080 }, /* 0 - Port Control Register A (PCRA) */
-	{ 0x0004, 0x0084 }, /* 1 - Port Control Register B (PCRB) */
-	{ 0x0008, 0x0088 }, /* 2 - LCV Count Register (PLCR) */
-	{ 0x000c, 0x008c }, /* 3 - LCV Threshold register (PLTR) */
-	{ 0x0010, 0x0090 }, /* 4 - Payload Fill Register (PPFR) */
-	{ 0x0200, 0x0200 }, /* 5 - Board ID / FPGA Programming Status Register */
-	{ 0x0204, 0x0204 }, /* 6 - FPGA Version Register */
-	{ 0x0800, 0x1000 }, /* 7 - Framer Registers Base Address */
-	{ 0x2000, 0x2000 }, /* 8 - Serial Chip Select Register */
-	{ 0x2004, 0x2004 }, /* 9 - Static Reset Register */
-	{ 0x2008, 0x2008 }, /* 10 - Pulse Reset Register */
-	{ 0x200c, 0x200c }, /* 11 - FPGA Reconfiguration Register */
-	{ 0x2010, 0x2014 }, /* 12 - LED Register (LEDR) */
-	{ 0x2018, 0x201c }, /* 13 - LIU Control and Status Register (PISCR) */
-	{ 0x2020, 0x2024 }, /* 14 - Interrupt Enable Register (PIER) */
-	{ 0x0068, 0x00e8 }, /* 15 - Port Control Register C (PCRC) */
-	{ 0x006c, 0x00ec }, /* 16 - Port Bandwidth Start (PBWF) */
-	{ 0x0070, 0x00f0 }, /* 17 - Port Bandwidth Stop (PBWL) */
-};
-
-const u32 cpld_val_map[][2] = {
-	{ 0x01, 0x02 }, /* LIU1 / LIU2 select for Serial Chip Select */
-	{ 0x04, 0x08 }, /* DAC1 / DAC2 select for Serial Chip Select */
-	{ 0x00, 0x04 }, /* LOOP1 / LOOP2 - select of loop timing source */
-	{ 0x01, 0x02 }  /* PORT1 / PORT2 - select LIU and Framer for reset */
-};
-
-const u32 t3e3_framer_reg_map[] = {
-	0x00, /* 0 - OPERATING_MODE */
-	0x01, /* 1 - IO_CONTROL */
-	0x04, /* 2 - BLOCK_INTERRUPT_ENABLE */
-	0x05, /* 3 - BLOCK_INTERRUPT_STATUS */
-	0x10, /* 4 - T3_RX_CONFIGURATION_STATUS, E3_RX_CONFIGURATION_STATUS_1 */
-	0x11, /* 5 - T3_RX_STATUS, E3_RX_CONFIGURATION_STATUS_2 */
-	0x12, /* 6 - T3_RX_INTERRUPT_ENABLE, E3_RX_INTERRUPT_ENABLE_1 */
-	0x13, /* 7 - T3_RX_INTERRUPT_STATUS, E3_RX_INTERRUPT_ENABLE_2 */
-	0x14, /* 8 - T3_RX_SYNC_DETECT_ENABLE, E3_RX_INTERRUPT_STATUS_1 */
-	0x15, /* 9 - E3_RX_INTERRUPT_STATUS_2 */
-	0x16, /* 10 - T3_RX_FEAC */
-	0x17, /* 11 - T3_RX_FEAC_INTERRUPT_ENABLE_STATUS */
-	0x18, /* 12 - T3_RX_LAPD_CONTROL, E3_RX_LAPD_CONTROL */
-	0x19, /* 13 - T3_RX_LAPD_STATUS, E3_RX_LAPD_STATUS */
-	0x1a, /* 14 - E3_RX_NR_BYTE, E3_RX_SERVICE_BITS */
-	0x1b, /* 15 - E3_RX_GC_BYTE */
-	0x30, /* 16 - T3_TX_CONFIGURATION, E3_TX_CONFIGURATION */
-	0x31, /* 17 - T3_TX_FEAC_CONFIGURATION_STATUS */
-	0x32, /* 18 - T3_TX_FEAC */
-	0x33, /* 19 - T3_TX_LAPD_CONFIGURATION, E3_TX_LAPD_CONFIGURATION */
-	0x34, /* 20 - T3_TX_LAPD_STATUS, E3_TX_LAPD_STATUS_INTERRUPT */
-	0x35, /* 21 - T3_TX_MBIT_MASK, E3_TX_GC_BYTE, E3_TX_SERVICE_BITS */
-	0x36, /* 22 - T3_TX_FBIT_MASK, E3_TX_MA_BYTE */
-	0x37, /* 23 - T3_TX_FBIT_MASK_2, E3_TX_NR_BYTE */
-	0x38, /* 24 - T3_TX_FBIT_MASK_3 */
-	0x48, /* 25 - E3_TX_FA1_ERROR_MASK, E3_TX_FAS_ERROR_MASK_UPPER */
-	0x49, /* 26 - E3_TX_FA2_ERROR_MASK, E3_TX_FAS_ERROR_MASK_LOWER */
-	0x4a, /* 27 - E3_TX_BIP8_MASK, E3_TX_BIP4_MASK */
-	0x50, /* 28 - PMON_LCV_EVENT_COUNT_MSB */
-	0x51, /* 29 - PMON_LCV_EVENT_COUNT_LSB */
-	0x52, /* 30 - PMON_FRAMING_BIT_ERROR_EVENT_COUNT_MSB */
-	0x53, /* 31 - PMON_FRAMING_BIT_ERROR_EVENT_COUNT_LSB */
-	0x54, /* 32 - PMON_PARITY_ERROR_EVENT_COUNT_MSB */
-	0x55, /* 33 - PMON_PARITY_ERROR_EVENT_COUNT_LSB */
-	0x56, /* 34 - PMON_FEBE_EVENT_COUNT_MSB */
-	0x57, /* 35 - PMON_FEBE_EVENT_COUNT_LSB */
-	0x58, /* 36 - PMON_CP_BIT_ERROR_EVENT_COUNT_MSB */
-	0x59, /* 37 - PMON_CP_BIT_ERROR_EVENT_COUNT_LSB */
-	0x6c, /* 38 - PMON_HOLDING_REGISTER */
-	0x6d, /* 39 - ONE_SECOND_ERROR_STATUS */
-	0x6e, /* 40 - LCV_ONE_SECOND_ACCUMULATOR_MSB */
-	0x6f, /* 41 - LCV_ONE_SECOND_ACCUMULATOR_LSB */
-	0x70, /* 42 - FRAME_PARITY_ERROR_ONE_SECOND_ACCUMULATOR_MSB */
-	0x71, /* 43 - FRAME_PARITY_ERROR_ONE_SECOND_ACCUMULATOR_LSB */
-	0x72, /* 44 - FRAME_CP_BIT_ERROR_ONE_SECOND_ACCUMULATOR_MSB */
-	0x73, /* 45 - FRAME_CP_BIT_ERROR_ONE_SECOND_ACCUMULATOR_LSB */
-	0x80, /* 46 - LINE_INTERFACE_DRIVE */
-	0x81  /* 47 - LINE_INTERFACE_SCAN */
-};
-
-const u32 t3e3_liu_reg_map[] = {
-	0x00, /* REG0 */
-	0x01, /* REG1 */
-	0x02, /* REG2 */
-	0x03, /* REG3 */
-	0x04 /* REG4 */
-};
diff --git a/drivers/staging/sbe-2t3e3/module.c b/drivers/staging/sbe-2t3e3/module.c
deleted file mode 100644
index a6f93a43d21..00000000000
--- a/drivers/staging/sbe-2t3e3/module.c
+++ /dev/null
@@ -1,208 +0,0 @@
-/*
- * SBE 2T3E3 synchronous serial card driver for Linux
- *
- * Copyright (C) 2009-2010 Krzysztof Halasa <khc@pm.waw.pl>
- *
- * 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.
- *
- * This code is based on a driver written by SBE Inc.
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/module.h>
-#include <linux/slab.h>
-#include <linux/delay.h>
-#include <linux/netdevice.h>
-#include <linux/pci.h>
-#include <linux/hdlc.h>
-#include <linux/if_arp.h>
-#include <linux/interrupt.h>
-#include "2t3e3.h"
-
-static void check_leds(unsigned long arg)
-{
-	struct card *card = (struct card *)arg;
-	struct channel *channel0 = &card->channels[0];
-	static int blinker;
-
-	update_led(channel0, ++blinker);
-	if (has_two_ports(channel0->pdev))
-		update_led(&card->channels[1], blinker);
-
-	card->timer.expires = jiffies + HZ / 10;
-	add_timer(&card->timer);
-}
-
-static void t3e3_remove_channel(struct channel *channel)
-{
-	struct pci_dev *pdev = channel->pdev;
-	struct net_device *dev = channel->dev;
-
-	/* system hangs if board asserts irq while module is unloaded */
-	cpld_stop_intr(channel);
-	free_irq(dev->irq, dev);
-	dc_drop_descriptor_list(channel);
-	unregister_hdlc_device(dev);
-	free_netdev(dev);
-	pci_release_regions(pdev);
-	pci_disable_device(pdev);
-	pci_set_drvdata(pdev, NULL);
-}
-
-static int t3e3_init_channel(struct channel *channel, struct pci_dev *pdev, struct card *card)
-{
-	struct net_device *dev;
-	unsigned int val;
-	int err;
-
-	err = pci_enable_device(pdev);
-	if (err)
-		return err;
-
-	err = pci_request_regions(pdev, "SBE 2T3E3");
-	if (err)
-		goto disable;
-
-	dev = alloc_hdlcdev(channel);
-	if (!dev) {
-		pr_err("Out of memory\n");
-		err = -ENOMEM;
-		goto free_regions;
-	}
-
-	t3e3_sc_init(channel);
-	dev_to_priv(dev) = channel;
-
-	channel->pdev = pdev;
-	channel->dev = dev;
-	channel->card = card;
-	channel->addr = pci_resource_start(pdev, 0);
-	if (pdev->subsystem_device == PCI_SUBDEVICE_ID_SBE_2T3E3_P1)
-		channel->h.slot = 1;
-	else
-		channel->h.slot = 0;
-
-	err = setup_device(dev, channel);
-	if (err)
-		goto free_dev;
-
-	pci_read_config_dword(channel->pdev, 0x40, &val); /* mask sleep mode */
-	pci_write_config_dword(channel->pdev, 0x40, val & 0x3FFFFFFF);
-
-	pci_read_config_byte(channel->pdev, PCI_CACHE_LINE_SIZE, &channel->h.cache_size);
-	pci_read_config_dword(channel->pdev, PCI_COMMAND, &channel->h.command);
-	t3e3_init(channel);
-
-	err = request_irq(dev->irq, &t3e3_intr, IRQF_SHARED, dev->name, dev);
-	if (err) {
-		netdev_warn(channel->dev, "%s: could not get irq: %d\n",
-			    dev->name, dev->irq);
-		goto unregister_dev;
-	}
-
-	pci_set_drvdata(pdev, channel);
-	return 0;
-
-unregister_dev:
-	unregister_hdlc_device(dev);
-free_dev:
-	free_netdev(dev);
-free_regions:
-	pci_release_regions(pdev);
-disable:
-	pci_disable_device(pdev);
-	return err;
-}
-
-static void t3e3_remove_card(struct pci_dev *pdev)
-{
-	struct channel *channel0 = pci_get_drvdata(pdev);
-	struct card *card = channel0->card;
-
-	del_timer_sync(&card->timer);
-	if (has_two_ports(channel0->pdev)) {
-		t3e3_remove_channel(&card->channels[1]);
-		pci_dev_put(card->channels[1].pdev);
-	}
-	t3e3_remove_channel(channel0);
-	kfree(card);
-}
-
-static int t3e3_init_card(struct pci_dev *pdev, const struct pci_device_id *ent)
-{
-	/* pdev points to channel #0 */
-	struct pci_dev *pdev1 = NULL;
-	struct card *card;
-	int channels = 1, err;
-
-	if (has_two_ports(pdev)) {
-		while ((pdev1 = pci_get_subsys(PCI_VENDOR_ID_DEC, PCI_DEVICE_ID_DEC_21142,
-					       PCI_VENDOR_ID_SBE, PCI_SUBDEVICE_ID_SBE_2T3E3_P1,
-					       pdev1)))
-			if (pdev1->bus == pdev->bus &&
-			    pdev1->devfn == pdev->devfn + 8 /* next device on the same bus */)
-				break; /* found the second channel */
-
-		if (!pdev1) {
-			dev_err(&pdev->dev, "Can't find the second channel\n");
-			return -EFAULT;
-		}
-		channels = 2;
-		/* holds the reference for pdev1 */
-	}
-
-	card = kzalloc(sizeof(struct card) + channels * sizeof(struct channel),
-		       GFP_KERNEL);
-	if (!card)
-		return -ENOBUFS;
-
-	spin_lock_init(&card->bootrom_lock);
-	card->bootrom_addr = pci_resource_start(pdev, 0);
-
-	err = t3e3_init_channel(&card->channels[0], pdev, card);
-	if (err)
-		goto free_card;
-
-	if (channels == 2) {
-		err = t3e3_init_channel(&card->channels[1], pdev1, card);
-		if (err) {
-			t3e3_remove_channel(&card->channels[0]);
-			goto free_card;
-		}
-	}
-
-	/* start LED timer */
-	init_timer(&card->timer);
-	card->timer.function = check_leds;
-	card->timer.expires = jiffies + HZ / 10;
-	card->timer.data = (unsigned long)card;
-	add_timer(&card->timer);
-	return 0;
-
-free_card:
-	kfree(card);
-	return err;
-}
-
-static struct pci_device_id t3e3_pci_tbl[] = {
-	{ PCI_VENDOR_ID_DEC, PCI_DEVICE_ID_DEC_21142,
-	  PCI_VENDOR_ID_SBE, PCI_SUBDEVICE_ID_SBE_T3E3, 0, 0, 0 },
-	{ PCI_VENDOR_ID_DEC, PCI_DEVICE_ID_DEC_21142,
-	  PCI_VENDOR_ID_SBE, PCI_SUBDEVICE_ID_SBE_2T3E3_P0, 0, 0, 0 },
-	/* channel 1 will be initialized after channel 0 */
-	{ 0, }
-};
-
-static struct pci_driver t3e3_pci_driver = {
-	.name     = "SBE T3E3",
-	.id_table = t3e3_pci_tbl,
-	.probe    = t3e3_init_card,
-	.remove   = t3e3_remove_card,
-};
-
-module_pci_driver(t3e3_pci_driver);
-MODULE_LICENSE("GPL");
-MODULE_DEVICE_TABLE(pci, t3e3_pci_tbl);
diff --git a/drivers/staging/sbe-2t3e3/netdev.c b/drivers/staging/sbe-2t3e3/netdev.c
deleted file mode 100644
index fe6c9513c9c..00000000000
--- a/drivers/staging/sbe-2t3e3/netdev.c
+++ /dev/null
@@ -1,143 +0,0 @@
-/*
- * SBE 2T3E3 synchronous serial card driver for Linux
- *
- * Copyright (C) 2009-2010 Krzysztof Halasa <khc@pm.waw.pl>
- *
- * 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.
- *
- * This code is based on a driver written by SBE Inc.
- */
-
-#include <linux/capability.h>
-#include <linux/module.h>
-#include <linux/slab.h>
-#include <linux/delay.h>
-#include <linux/netdevice.h>
-#include <linux/pci.h>
-#include <linux/hdlc.h>
-#include <linux/if_arp.h>
-#include <linux/interrupt.h>
-#include "2t3e3.h"
-
-static int t3e3_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
-{
-	struct channel *sc = dev_to_priv(dev);
-	int cmd_2t3e3, len, rlen;
-	struct t3e3_param param;
-	struct t3e3_resp resp;
-	void __user *data = ifr->ifr_data + sizeof(cmd_2t3e3) + sizeof(len);
-
-	if (cmd == SIOCWANDEV)
-		return hdlc_ioctl(dev, ifr, cmd);
-	if (!capable(CAP_SYS_ADMIN))
-		return -EPERM;
-	if (cmd != SIOCDEVPRIVATE + 15)
-		return -EINVAL;
-
-	if (copy_from_user(&cmd_2t3e3, ifr->ifr_data, sizeof(cmd_2t3e3)))
-		return -EFAULT;
-	if (copy_from_user(&len, ifr->ifr_data + sizeof(cmd_2t3e3), sizeof(len)))
-		return -EFAULT;
-
-	if (len > sizeof(param))
-		return -EFAULT;
-
-	if (len)
-		if (copy_from_user(&param, data, len))
-			return -EFAULT;
-
-	t3e3_if_config(sc, cmd_2t3e3, (char *)&param, &resp, &rlen);
-
-	if (rlen)
-		if (copy_to_user(data, &resp, rlen))
-			return -EFAULT;
-
-	return 0;
-}
-
-static struct net_device_stats *t3e3_get_stats(struct net_device *dev)
-{
-	struct net_device_stats *nstats = &dev->stats;
-	struct channel *sc = dev_to_priv(dev);
-	struct t3e3_stats *stats = &sc->s;
-
-	memset(nstats, 0, sizeof(struct net_device_stats));
-	nstats->rx_packets = stats->in_packets;
-	nstats->tx_packets = stats->out_packets;
-	nstats->rx_bytes = stats->in_bytes;
-	nstats->tx_bytes = stats->out_bytes;
-
-	nstats->rx_errors = stats->in_errors;
-	nstats->tx_errors = stats->out_errors;
-	nstats->rx_crc_errors = stats->in_error_crc;
-
-
-	nstats->rx_dropped = stats->in_dropped;
-	nstats->tx_dropped = stats->out_dropped;
-	nstats->tx_carrier_errors = stats->out_error_lost_carr +
-		stats->out_error_no_carr;
-
-	return nstats;
-}
-
-static int t3e3_open(struct net_device *dev)
-{
-	struct channel *sc = dev_to_priv(dev);
-	int ret = hdlc_open(dev);
-
-	if (ret)
-		return ret;
-
-	sc->r.flags |= SBE_2T3E3_FLAG_NETWORK_UP;
-	dc_start(dev_to_priv(dev));
-	netif_start_queue(dev);
-	try_module_get(THIS_MODULE);
-	return 0;
-}
-
-static int t3e3_close(struct net_device *dev)
-{
-	struct channel *sc = dev_to_priv(dev);
-	hdlc_close(dev);
-	netif_stop_queue(dev);
-	dc_stop(sc);
-	sc->r.flags &= ~SBE_2T3E3_FLAG_NETWORK_UP;
-	module_put(THIS_MODULE);
-	return 0;
-}
-
-static int t3e3_attach(struct net_device *dev, unsigned short foo1,
-		       unsigned short foo2)
-{
-	return 0;
-}
-
-static const struct net_device_ops t3e3_ops = {
-	.ndo_open       = t3e3_open,
-	.ndo_stop       = t3e3_close,
-	.ndo_change_mtu = hdlc_change_mtu,
-	.ndo_start_xmit = hdlc_start_xmit,
-	.ndo_do_ioctl   = t3e3_ioctl,
-	.ndo_get_stats  = t3e3_get_stats,
-};
-
-int setup_device(struct net_device *dev, struct channel *sc)
-{
-	hdlc_device *hdlc = dev_to_hdlc(dev);
-	int retval;
-
-	dev->base_addr = pci_resource_start(sc->pdev, 0);
-	dev->irq = sc->pdev->irq;
-	dev->netdev_ops = &t3e3_ops;
-	dev->tx_queue_len = 100;
-	hdlc->xmit = t3e3_if_start_xmit;
-	hdlc->attach = t3e3_attach;
-	retval = register_hdlc_device(dev);
-	if (retval) {
-		dev_err(&sc->pdev->dev, "error registering HDLC device\n");
-		return retval;
-	}
-	return 0;
-}
diff --git a/drivers/staging/sep/sep_crypto.c b/drivers/staging/sep/sep_crypto.c
index 965485f71fe..415f8ec5276 100644
--- a/drivers/staging/sep/sep_crypto.c
+++ b/drivers/staging/sep/sep_crypto.c
@@ -93,6 +93,7 @@ static void sep_do_callback(struct work_struct *work)
 {
 	struct sep_work_struct *sep_work = container_of(work,
 		struct sep_work_struct, work);
+
 	if (sep_work != NULL) {
 		(sep_work->callback)(sep_work->data);
 		kfree(sep_work);
@@ -649,6 +650,7 @@ weak:
 static u32 sep_sg_nents(struct scatterlist *sg)
 {
 	u32 ct1 = 0;
+
 	while (sg) {
 		ct1 += 1;
 		sg = sg_next(sg);
@@ -666,6 +668,7 @@ static u32 sep_sg_nents(struct scatterlist *sg)
 static u32 sep_start_msg(struct this_task_ctx *ta_ctx)
 {
 	u32 *word_ptr;
+
 	ta_ctx->msg_len_words = 2;
 	ta_ctx->msgptr = ta_ctx->msg;
 	memset(ta_ctx->msg, 0, SEP_DRIVER_MESSAGE_SHARED_AREA_SIZE_IN_BYTES);
@@ -740,6 +743,7 @@ static void sep_write_msg(struct this_task_ctx *ta_ctx, void *in_addr,
 {
 	u32 *word_ptr;
 	void *void_ptr;
+
 	void_ptr = ta_ctx->msgptr + *msg_offset;
 	word_ptr = (u32 *)void_ptr;
 	memcpy(void_ptr, in_addr, size);
@@ -748,6 +752,7 @@ static void sep_write_msg(struct this_task_ctx *ta_ctx, void *in_addr,
 	/* Do we need to manipulate endian? */
 	if (byte_array) {
 		u32 i;
+
 		for (i = 0; i < ((size + 3) / 4); i += 1)
 			*(word_ptr + i) = CHG_ENDIAN(*(word_ptr + i));
 	}
@@ -788,12 +793,14 @@ static void sep_read_msg(struct this_task_ctx *ta_ctx, void *in_addr,
 {
 	u32 *word_ptr;
 	void *void_ptr;
+
 	void_ptr = ta_ctx->msgptr + *msg_offset;
 	word_ptr = (u32 *)void_ptr;
 
 	/* Do we need to manipulate endian? */
 	if (byte_array) {
 		u32 i;
+
 		for (i = 0; i < ((size + 3) / 4); i += 1)
 			*(word_ptr + i) = CHG_ENDIAN(*(word_ptr + i));
 	}
@@ -865,6 +872,7 @@ static void sep_read_context(struct this_task_ctx *ta_ctx, u32 *msg_offset,
 	void *dst, u32 len)
 {
 	u32 max_length = ((len + 3) / sizeof(u32)) * sizeof(u32);
+
 	sep_read_msg(ta_ctx, dst, len, max_length, msg_offset, 0);
 }
 
@@ -884,6 +892,7 @@ static void sep_write_context(struct this_task_ctx *ta_ctx, u32 *msg_offset,
 	void *src, u32 len)
 {
 	u32 max_length = ((len + 3) / sizeof(u32)) * sizeof(u32);
+
 	sep_write_msg(ta_ctx, src, len, max_length, msg_offset, 0);
 }
 
@@ -3893,6 +3902,7 @@ static struct crypto_alg crypto_algs[] = {
 int sep_crypto_setup(void)
 {
 	int err, i, j, k;
+
 	tasklet_init(&sep_dev->finish_tasklet, sep_finish,
 		(unsigned long)sep_dev);
 
diff --git a/drivers/staging/sep/sep_dev.h b/drivers/staging/sep/sep_dev.h
index 5f6a07f59dd..bf56c06662f 100644
--- a/drivers/staging/sep/sep_dev.h
+++ b/drivers/staging/sep/sep_dev.h
@@ -152,6 +152,7 @@ static inline u32 sep_read_reg(struct sep_device *dev, int reg)
 static inline void sep_wait_sram_write(struct sep_device *dev)
 {
 	u32 reg_val;
+
 	do {
 		reg_val = sep_read_reg(dev, HW_SRAM_DATA_READY_REG_ADDR);
 	} while (!(reg_val & 1));
diff --git a/drivers/staging/sep/sep_main.c b/drivers/staging/sep/sep_main.c
index e301207eb0b..75ca15e86a2 100644
--- a/drivers/staging/sep/sep_main.c
+++ b/drivers/staging/sep/sep_main.c
@@ -1400,7 +1400,6 @@ static int sep_lli_table_secure_dma(struct sep_device *sep,
 	struct sep_dma_context *dma_ctx)
 
 {
-	int error = 0;
 	u32 count;
 	/* The the page of the end address of the user space buffer */
 	u32 end_page;
@@ -1491,7 +1490,7 @@ static int sep_lli_table_secure_dma(struct sep_device *sep,
 	dma_ctx->dma_res_arr[dma_ctx->nr_dcb_creat].out_map_array = NULL;
 	dma_ctx->dma_res_arr[dma_ctx->nr_dcb_creat].out_map_num_entries = 0;
 
-	return error;
+	return 0;
 }
 
 /**
diff --git a/drivers/staging/serqt_usb2/serqt_usb2.c b/drivers/staging/serqt_usb2/serqt_usb2.c
index 0267dd8b84b..998c3845421 100644
--- a/drivers/staging/serqt_usb2/serqt_usb2.c
+++ b/drivers/staging/serqt_usb2/serqt_usb2.c
@@ -990,18 +990,10 @@ static void qt_block_until_empty(struct tty_struct *tty,
 static void qt_close(struct usb_serial_port *port)
 {
 	struct usb_serial *serial = port->serial;
-	struct quatech_port *qt_port;
-	struct quatech_port *port0;
-	struct tty_struct *tty;
-	int status;
-	unsigned int index;
-	status = 0;
-
-	tty = tty_port_tty_get(&port->port);
-	index = port->port_number;
-
-	qt_port = qt_get_port_private(port);
-	port0 = qt_get_port_private(serial->port[0]);
+	struct tty_struct *tty = tty_port_tty_get(&port->port);
+	unsigned int index = port->port_number;
+	struct quatech_port *qt_port = qt_get_port_private(port);
+	struct quatech_port *port0 = qt_get_port_private(serial->port[0]);
 
 	/* shutdown any bulk reads that might be going on */
 	if (serial->num_bulk_out)
@@ -1015,8 +1007,7 @@ static void qt_close(struct usb_serial_port *port)
 	tty_kref_put(tty);
 
 	/* Close uart channel */
-	status = qt_close_channel(serial, index);
-	if (status < 0)
+	if (qt_close_channel(serial, index) < 0)
 		dev_dbg(&port->dev, "%s - qt_close_channel failed.\n",
 			__func__);
 
@@ -1268,9 +1259,9 @@ static void qt_set_termios(struct tty_struct *tty,
 	if (I_IXOFF(tty) || I_IXON(tty)) {
 		unsigned char stop_char = STOP_CHAR(tty);
 		unsigned char start_char = START_CHAR(tty);
-		status =
-		    box_set_sw_flow_ctrl(port->serial, index, stop_char,
-				      start_char);
+
+		status = box_set_sw_flow_ctrl(port->serial, index, stop_char,
+					      start_char);
 		if (status < 0)
 			dev_dbg(&port->dev,
 				"box_set_sw_flow_ctrl (enabled) failed\n");
diff --git a/drivers/staging/silicom/bpctl_mod.c b/drivers/staging/silicom/bpctl_mod.c
index 6b9365b28e8..765fce84d9d 100644
--- a/drivers/staging/silicom/bpctl_mod.c
+++ b/drivers/staging/silicom/bpctl_mod.c
@@ -41,7 +41,7 @@ MODULE_AUTHOR("Anna Lukin, annal@silicom.co.il");
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION(BP_MOD_DESCR);
 MODULE_VERSION(BP_MOD_VER);
-spinlock_t bpvm_lock;
+static spinlock_t bpvm_lock;
 
 #define unlock_bpctl()					\
 	up(&bpctl_sema);
@@ -119,7 +119,6 @@ static void if_scan_init(void);
 
 static int bypass_proc_create_dev_sd(struct bpctl_dev *pbp_device_block);
 static int bypass_proc_remove_dev_sd(struct bpctl_dev *pbp_device_block);
-static int bp_proc_create(void);
 
 static int is_bypass_fn(struct bpctl_dev *pbpctl_dev);
 static int get_dev_idx_bsf(int bus, int slot, int func);
@@ -220,8 +219,12 @@ static int bp_device_event(struct notifier_block *unused,
 			if (netif_carrier_ok(dev))
 				return NOTIFY_DONE;
 
-			if (((dev_num = get_dev_idx(dev->ifindex)) == -1) ||
-			    (!(pbpctl_dev = &bpctl_dev_arr[dev_num])))
+			dev_num = get_dev_idx(dev->ifindex);
+			if (dev_num == -1)
+				return NOTIFY_DONE;
+
+			pbpctl_dev = &bpctl_dev_arr[dev_num];
+			if (!pbpctl_dev)
 				return NOTIFY_DONE;
 
 			if ((is_bypass_fn(pbpctl_dev)) == 1)
@@ -749,6 +752,7 @@ static void write_reg(struct bpctl_dev *pbpctl_dev, unsigned char value,
 	uint32_t ctrl_ext = 0, ctrl = 0;
 	struct bpctl_dev *pbpctl_dev_c = NULL;
 	unsigned long flags;
+
 	if (pbpctl_dev->bp_10g9) {
 		pbpctl_dev_c = get_status_port_fn(pbpctl_dev);
 		if (!pbpctl_dev_c)
@@ -924,6 +928,7 @@ static int read_reg(struct bpctl_dev *pbpctl_dev, unsigned char addr)
 
 #ifdef BP_SYNC_FLAG
 	unsigned long flags;
+
 	spin_lock_irqsave(&pbpctl_dev->bypass_wr_lock, flags);
 #else
 	atomic_set(&pbpctl_dev->wdt_busy, 1);
@@ -1559,7 +1564,8 @@ int pulse_set_fn(struct bpctl_dev *pbpctl_dev, unsigned int counter)
 
 int zero_set_fn(struct bpctl_dev *pbpctl_dev)
 {
-	uint32_t ctrl_ext = 0, ctrl_value = 0;
+	uint32_t ctrl_ext = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -1579,12 +1585,13 @@ int zero_set_fn(struct bpctl_dev *pbpctl_dev)
 							   BPCTLI_CTRL_EXT_MDIO_DATA)));
 
 	}
-	return ctrl_value;
+	return 0;
 }
 
 int pulse_get2_fn(struct bpctl_dev *pbpctl_dev)
 {
 	uint32_t ctrl_ext = 0, ctrl_value = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -1600,6 +1607,7 @@ int pulse_get2_fn(struct bpctl_dev *pbpctl_dev)
 int pulse_get1_fn(struct bpctl_dev *pbpctl_dev)
 {
 	uint32_t ctrl_ext = 0, ctrl_value = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -1663,6 +1671,7 @@ static struct bpctl_dev *lookup_port(struct bpctl_dev *dev)
 {
 	struct bpctl_dev *p;
 	int n;
+
 	for (n = 0, p = bpctl_dev_arr; n < device_num && p->pdev; n++) {
 		if (p->bus == dev->bus
 		    && p->slot == dev->slot
@@ -1840,6 +1849,7 @@ static int bypass_off(struct bpctl_dev *pbpctl_dev)
 static int tap_off(struct bpctl_dev *pbpctl_dev)
 {
 	int ret = BP_NOT_CAP;
+
 	if ((pbpctl_dev->bp_caps & TAP_CAP)
 	    && (pbpctl_dev->bp_ext_ver >= PXG2TBPI_VER)) {
 		write_data(pbpctl_dev, TAP_OFF);
@@ -1853,6 +1863,7 @@ static int tap_off(struct bpctl_dev *pbpctl_dev)
 static int tap_on(struct bpctl_dev *pbpctl_dev)
 {
 	int ret = BP_NOT_CAP;
+
 	if ((pbpctl_dev->bp_caps & TAP_CAP)
 	    && (pbpctl_dev->bp_ext_ver >= PXG2TBPI_VER)) {
 		write_data(pbpctl_dev, TAP_ON);
@@ -1866,6 +1877,7 @@ static int tap_on(struct bpctl_dev *pbpctl_dev)
 static int disc_off(struct bpctl_dev *pbpctl_dev)
 {
 	int ret = 0;
+
 	if ((pbpctl_dev->bp_caps & DISC_CAP) && (pbpctl_dev->bp_ext_ver >= 0x8)) {
 		write_data(pbpctl_dev, DISC_OFF);
 		msec_delay_bp(LATCH_DELAY);
@@ -1878,6 +1890,7 @@ static int disc_off(struct bpctl_dev *pbpctl_dev)
 static int disc_on(struct bpctl_dev *pbpctl_dev)
 {
 	int ret = 0;
+
 	if ((pbpctl_dev->bp_caps & DISC_CAP) && (pbpctl_dev->bp_ext_ver >= 0x8)) {
 		write_data(pbpctl_dev, /*DISC_ON */ 0x85);
 		msec_delay_bp(LATCH_DELAY);
@@ -2267,6 +2280,7 @@ static int set_tx(struct bpctl_dev *pbpctl_dev, int tx_state)
 {
 	int ret = 0, ctrl = 0;
 	struct bpctl_dev *pbpctl_dev_m;
+
 	if ((is_bypass_fn(pbpctl_dev)) == 1)
 		pbpctl_dev_m = pbpctl_dev;
 	else
@@ -2799,6 +2813,7 @@ int wdt_time_left(struct bpctl_dev *pbpctl_dev)
 static int wdt_timer(struct bpctl_dev *pbpctl_dev, int *time_left)
 {
 	int ret = 0;
+
 	if (pbpctl_dev->bp_caps & WD_CTL_CAP) {
 		{
 			if (pbpctl_dev->wdt_status == WDT_STATUS_UNKNOWN)
@@ -3011,6 +3026,7 @@ static int tx_status(struct bpctl_dev *pbpctl_dev)
 {
 	uint32_t ctrl = 0;
 	struct bpctl_dev *pbpctl_dev_m;
+
 	if ((is_bypass_fn(pbpctl_dev)) == 1)
 		pbpctl_dev_m = pbpctl_dev;
 	else
@@ -3192,6 +3208,7 @@ static int bypass_change_status(struct bpctl_dev *pbpctl_dev)
 static int bypass_status(struct bpctl_dev *pbpctl_dev)
 {
 	u32 ctrl_ext = 0;
+
 	if (pbpctl_dev->bp_caps & BP_CAP) {
 
 		struct bpctl_dev *pbpctl_dev_b = NULL;
@@ -3320,6 +3337,7 @@ static int dis_bypass_cap_status(struct bpctl_dev *pbpctl_dev)
 static int wdt_programmed(struct bpctl_dev *pbpctl_dev, int *timeout)
 {
 	int ret = 0;
+
 	if (pbpctl_dev->bp_caps & WD_CTL_CAP) {
 		if (pbpctl_dev->bp_ext_ver >= PXG2BPI_VER) {
 			if ((read_reg(pbpctl_dev, STATUS_REG_ADDR)) &
@@ -3383,6 +3401,7 @@ static int tap_flag_status(struct bpctl_dev *pbpctl_dev)
 static int tap_flag_status_clear(struct bpctl_dev *pbpctl_dev)
 {
 	uint32_t status_reg = 0;
+
 	if (pbpctl_dev->bp_caps & TAP_STATUS_CAP) {
 		if (pbpctl_dev->bp_ext_ver >= PXG2TBPI_VER) {
 			status_reg = read_reg(pbpctl_dev, STATUS_TAP_REG_ADDR);
@@ -3397,6 +3416,7 @@ static int tap_flag_status_clear(struct bpctl_dev *pbpctl_dev)
 static int tap_change_status(struct bpctl_dev *pbpctl_dev)
 {
 	int ret = BP_NOT_CAP;
+
 	if (pbpctl_dev->bp_ext_ver >= PXG2TBPI_VER) {
 		if (pbpctl_dev->bp_caps & TAP_CAP) {
 			if (pbpctl_dev->bp_caps & BP_CAP) {
@@ -3487,6 +3507,7 @@ static int disc_flag_status(struct bpctl_dev *pbpctl_dev)
 static int disc_flag_status_clear(struct bpctl_dev *pbpctl_dev)
 {
 	uint32_t status_reg = 0;
+
 	if (pbpctl_dev->bp_caps & DISC_CAP) {
 		if (pbpctl_dev->bp_ext_ver >= 0x8) {
 			status_reg = read_reg(pbpctl_dev, STATUS_DISC_REG_ADDR);
@@ -3501,6 +3522,7 @@ static int disc_flag_status_clear(struct bpctl_dev *pbpctl_dev)
 static int disc_change_status(struct bpctl_dev *pbpctl_dev)
 {
 	int ret = BP_NOT_CAP;
+
 	if (pbpctl_dev->bp_caps & DISC_CAP) {
 		ret = disc_flag_status(pbpctl_dev);
 		disc_flag_status_clear(pbpctl_dev);
@@ -3596,6 +3618,7 @@ static int disc_off_status(struct bpctl_dev *pbpctl_dev)
 static int disc_status(struct bpctl_dev *pbpctl_dev)
 {
 	int ctrl = 0;
+
 	if (pbpctl_dev->bp_caps & DISC_CAP) {
 		ctrl = disc_off_status(pbpctl_dev);
 		if (ctrl < 0)
@@ -3741,6 +3764,7 @@ static void bypass_caps_init(struct bpctl_dev *pbpctl_dev)
 
 #ifdef BYPASS_DEBUG
 	int ret = 0;
+
 	if (!(INTEL_IF_SERIES(adapter->bp_device_block.subdevice))) {
 		ret = read_reg(pbpctl_dev, VER_REG_ADDR);
 		printk("VER_REG reg1=%x\n", ret);
@@ -4152,6 +4176,7 @@ static int get_bypass_change_fn(struct bpctl_dev *pbpctl_dev)
 static int set_dis_bypass_fn(struct bpctl_dev *pbpctl_dev, int dis_param)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4179,6 +4204,7 @@ static int get_dis_bypass_fn(struct bpctl_dev *pbpctl_dev)
 static int set_bypass_pwoff_fn(struct bpctl_dev *pbpctl_dev, int bypass_mode)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4206,6 +4232,7 @@ static int get_bypass_pwoff_fn(struct bpctl_dev *pbpctl_dev)
 static int set_bypass_pwup_fn(struct bpctl_dev *pbpctl_dev, int bypass_mode)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4233,6 +4260,7 @@ static int get_bypass_pwup_fn(struct bpctl_dev *pbpctl_dev)
 static int set_bypass_wd_fn(struct bpctl_dev *pbpctl_dev, int timeout)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4279,8 +4307,8 @@ static int reset_bypass_wd_timer_fn(struct bpctl_dev *pbpctl_dev)
 static int get_wd_set_caps_fn(struct bpctl_dev *pbpctl_dev)
 {
 	int bp_status = 0;
-
 	unsigned int step_value = TIMEOUT_MAX_STEP + 1, bit_cnt = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4303,6 +4331,7 @@ static int get_wd_set_caps_fn(struct bpctl_dev *pbpctl_dev)
 static int set_std_nic_fn(struct bpctl_dev *pbpctl_dev, int nic_mode)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4355,6 +4384,7 @@ static int get_tap_fn(struct bpctl_dev *pbpctl_dev)
 static int set_tap_pwup_fn(struct bpctl_dev *pbpctl_dev, int tap_mode)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4373,6 +4403,7 @@ static int set_tap_pwup_fn(struct bpctl_dev *pbpctl_dev, int tap_mode)
 static int get_tap_pwup_fn(struct bpctl_dev *pbpctl_dev)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4393,6 +4424,7 @@ static int get_tap_change_fn(struct bpctl_dev *pbpctl_dev)
 static int set_dis_tap_fn(struct bpctl_dev *pbpctl_dev, int dis_param)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4435,6 +4467,7 @@ static int set_disc_fn(struct bpctl_dev *pbpctl_dev, int disc_mode)
 static int get_disc_fn(struct bpctl_dev *pbpctl_dev)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4446,6 +4479,7 @@ static int get_disc_fn(struct bpctl_dev *pbpctl_dev)
 static int set_disc_pwup_fn(struct bpctl_dev *pbpctl_dev, int disc_mode)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4464,6 +4498,7 @@ static int set_disc_pwup_fn(struct bpctl_dev *pbpctl_dev, int disc_mode)
 static int get_disc_pwup_fn(struct bpctl_dev *pbpctl_dev)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4474,6 +4509,7 @@ static int get_disc_pwup_fn(struct bpctl_dev *pbpctl_dev)
 static int get_disc_change_fn(struct bpctl_dev *pbpctl_dev)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4484,6 +4520,7 @@ static int get_disc_change_fn(struct bpctl_dev *pbpctl_dev)
 static int set_dis_disc_fn(struct bpctl_dev *pbpctl_dev, int dis_param)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4502,6 +4539,7 @@ static int set_dis_disc_fn(struct bpctl_dev *pbpctl_dev, int dis_param)
 static int get_dis_disc_fn(struct bpctl_dev *pbpctl_dev)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4528,8 +4566,8 @@ static int set_wd_exp_mode_fn(struct bpctl_dev *pbpctl_dev, int param)
 
 static int set_tx_fn(struct bpctl_dev *pbpctl_dev, int tx_state)
 {
-
 	struct bpctl_dev *pbpctl_dev_b = NULL;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4607,6 +4645,7 @@ static int get_bypass_slave_fn(struct bpctl_dev *pbpctl_dev,
 			       struct bpctl_dev **pbpctl_dev_out)
 {
 	int idx_dev = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4650,6 +4689,7 @@ static int is_bypass(struct bpctl_dev *pbpctl_dev)
 static int get_tx_fn(struct bpctl_dev *pbpctl_dev)
 {
 	struct bpctl_dev *pbpctl_dev_b = NULL;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4732,10 +4772,9 @@ static void bp_tpl_timer_fn(unsigned long param)
 
 static void remove_bypass_tpl_auto(struct bpctl_dev *pbpctl_dev)
 {
-	struct bpctl_dev *pbpctl_dev_b = NULL;
+	struct bpctl_dev *pbpctl_dev_b;
 	if (!pbpctl_dev)
 		return;
-	pbpctl_dev_b = get_status_port_fn(pbpctl_dev);
 
 	if (pbpctl_dev->bp_caps & TPL_CAP) {
 		del_timer_sync(&pbpctl_dev->bp_tpl_timer);
@@ -4782,12 +4821,10 @@ static int set_bypass_tpl_auto(struct bpctl_dev *pbpctl_dev, unsigned int param)
 static int set_tpl_fn(struct bpctl_dev *pbpctl_dev, int tpl_mode)
 {
 
-	struct bpctl_dev *pbpctl_dev_b = NULL;
+	struct bpctl_dev *pbpctl_dev_b;
 	if (!pbpctl_dev)
 		return -1;
 
-	pbpctl_dev_b = get_status_port_fn(pbpctl_dev);
-
 	if (pbpctl_dev->bp_caps & TPL_CAP) {
 		if (tpl_mode) {
 			pbpctl_dev_b = get_status_port_fn(pbpctl_dev);
@@ -4812,6 +4849,7 @@ static int set_tpl_fn(struct bpctl_dev *pbpctl_dev, int tpl_mode)
 static int get_tpl_fn(struct bpctl_dev *pbpctl_dev)
 {
 	int ret = BP_NOT_CAP;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4846,6 +4884,7 @@ static int set_bp_wait_at_pwup_fn(struct bpctl_dev *pbpctl_dev, int tap_mode)
 static int get_bp_wait_at_pwup_fn(struct bpctl_dev *pbpctl_dev)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4879,6 +4918,7 @@ static int set_bp_hw_reset_fn(struct bpctl_dev *pbpctl_dev, int tap_mode)
 static int get_bp_hw_reset_fn(struct bpctl_dev *pbpctl_dev)
 {
 	int ret = 0;
+
 	if (!pbpctl_dev)
 		return -1;
 
@@ -4906,6 +4946,7 @@ static int get_bypass_info_fn(struct bpctl_dev *pbpctl_dev, char *dev_name,
 static int get_dev_idx_bsf(int bus, int slot, int func)
 {
 	int idx_dev = 0;
+
 	for (idx_dev = 0;
 	     ((bpctl_dev_arr[idx_dev].pdev != NULL) && (idx_dev < device_num));
 	     idx_dev++) {
@@ -6368,56 +6409,30 @@ static int __init bypass_init_module(void)
 
 	sema_init(&bpctl_sema, 1);
 	spin_lock_init(&bpvm_lock);
-	{
-
-		struct bpctl_dev *pbpctl_dev_c = NULL;
-		for (idx_dev = 0, dev = bpctl_dev_arr;
-		     idx_dev < device_num && dev->pdev;
-		     idx_dev++, dev++) {
-			if (dev->bp_10g9) {
-				pbpctl_dev_c = get_status_port_fn(dev);
-				if (is_bypass_fn(dev)) {
-					printk(KERN_INFO "%s found, ",
-					       dev->name);
-					dev->bp_fw_ver = bypass_fw_ver(dev);
-					printk("firmware version: 0x%x\n",
-					       dev->bp_fw_ver);
-				}
-				dev->wdt_status = WDT_STATUS_UNKNOWN;
-				dev->reset_time = 0;
-				atomic_set(&dev->wdt_busy, 0);
-				dev->bp_status_un = 1;
-
-				bypass_caps_init(dev);
-
-				init_bypass_wd_auto(dev);
-				init_bypass_tpl_auto(dev);
 
+	for (idx_dev = 0, dev = bpctl_dev_arr;
+	     idx_dev < device_num && dev->pdev;
+	     idx_dev++, dev++) {
+		if (dev->bp_10g9) {
+			if (is_bypass_fn(dev)) {
+				printk(KERN_INFO "%s found, ", dev->name);
+				dev->bp_fw_ver = bypass_fw_ver(dev);
+				printk("firmware version: 0x%x\n",
+				       dev->bp_fw_ver);
 			}
+			dev->wdt_status = WDT_STATUS_UNKNOWN;
+			dev->reset_time = 0;
+			atomic_set(&dev->wdt_busy, 0);
+			dev->bp_status_un = 1;
 
-		}
-	}
-
-	register_netdevice_notifier(&bp_notifier_block);
-#ifdef BP_PROC_SUPPORT
-	{
-		int i = 0;
-		/* unsigned long flags; */
-		/* rcu_read_lock(); */
-		bp_proc_create();
-		for (i = 0; i < device_num; i++) {
-			if (bpctl_dev_arr[i].ifindex) {
-				/* spin_lock_irqsave(&bpvm_lock, flags); */
-				bypass_proc_remove_dev_sd(&bpctl_dev_arr[i]);
-				bypass_proc_create_dev_sd(&bpctl_dev_arr[i]);
-				/* spin_unlock_irqrestore(&bpvm_lock, flags); */
-			}
+			bypass_caps_init(dev);
 
+			init_bypass_wd_auto(dev);
+			init_bypass_tpl_auto(dev);
 		}
-		/* rcu_read_unlock(); */
 	}
-#endif
 
+	register_netdevice_notifier(&bp_notifier_block);
 	return 0;
 }
 
@@ -6427,17 +6442,11 @@ static int __init bypass_init_module(void)
 static void __exit bypass_cleanup_module(void)
 {
 	int i;
+
 	unregister_netdevice_notifier(&bp_notifier_block);
 
 	for (i = 0; i < device_num; i++) {
 		/* unsigned long flags; */
-#ifdef BP_PROC_SUPPORT
-/*	spin_lock_irqsave(&bpvm_lock, flags);
-	rcu_read_lock(); */
-		bypass_proc_remove_dev_sd(&bpctl_dev_arr[i]);
-/*	spin_unlock_irqrestore(&bpvm_lock, flags);
-	rcu_read_unlock(); */
-#endif
 		remove_bypass_wd_auto(&bpctl_dev_arr[i]);
 		bpctl_dev_arr[i].reset_time = 0;
 
@@ -6738,6 +6747,7 @@ int get_bypass_slave_sd(int ifindex)
 {
 	struct bpctl_dev *pbpctl_dev_out;
 	int ret = get_bypass_slave_fn(get_dev_idx_p(ifindex), &pbpctl_dev_out);
+
 	if (ret == 1)
 		return pbpctl_dev_out->ifindex;
 	return -1;
@@ -6783,22 +6793,11 @@ EXPORT_SYMBOL(bp_if_scan_sd);
 
 static struct proc_dir_entry *bp_procfs_dir;
 
-static int bp_proc_create(void)
-{
-	bp_procfs_dir = proc_mkdir(BP_PROC_DIR, init_net.proc_net);
-	if (bp_procfs_dir == (struct proc_dir_entry *)0) {
-		printk(KERN_DEBUG
-		       "Could not create procfs nicinfo directory %s\n",
-		       BP_PROC_DIR);
-		return -1;
-	}
-	return 0;
-}
-
 static int procfs_add(char *proc_name, const struct file_operations *fops,
 		      struct bpctl_dev *dev)
 {
 	struct bypass_pfs_sd *pfs = &dev->bypass_pfs_set;
+
 	if (!proc_create_data(proc_name, 0644, pfs->bypass_entry, fops, dev))
 		return -1;
 	return 0;
@@ -6843,6 +6842,7 @@ static int show_bypass_slave(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	struct bpctl_dev *slave = get_status_port_fn(dev);
+
 	if (!slave)
 		slave = dev;
 	if (!slave)
@@ -6857,6 +6857,7 @@ static int show_bypass_caps(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_bypass_caps_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "-1\n");
 	else
@@ -6869,6 +6870,7 @@ static int show_wd_set_caps(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_wd_set_caps_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "-1\n");
 	else
@@ -6905,6 +6907,7 @@ static ssize_t bypass_write(struct file *file, const char __user *buffer,
 				  size_t count, loff_t *pos)
 {
 	int bypass_param = user_on_off(buffer, count);
+
 	if (bypass_param < 0)
 		return -1;
 
@@ -6915,6 +6918,7 @@ static int show_bypass(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_bypass_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 1)
@@ -6929,6 +6933,7 @@ static ssize_t tap_write(struct file *file, const char __user *buffer,
 				  size_t count, loff_t *pos)
 {
 	int tap_param = user_on_off(buffer, count);
+
 	if (tap_param < 0)
 		return -1;
 
@@ -6939,6 +6944,7 @@ static int show_tap(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_tap_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 1)
@@ -6953,6 +6959,7 @@ static ssize_t disc_write(struct file *file, const char __user *buffer,
 				  size_t count, loff_t *pos)
 {
 	int tap_param = user_on_off(buffer, count);
+
 	if (tap_param < 0)
 		return -1;
 
@@ -6963,6 +6970,7 @@ static int show_disc(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_disc_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 1)
@@ -6977,6 +6985,7 @@ static int show_bypass_change(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_bypass_change_fn(dev);
+
 	if (ret == 1)
 		seq_puts(m, "on\n");
 	else if (ret == 0)
@@ -6991,6 +7000,7 @@ static int show_tap_change(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_tap_change_fn(dev);
+
 	if (ret == 1)
 		seq_puts(m, "on\n");
 	else if (ret == 0)
@@ -7005,6 +7015,7 @@ static int show_disc_change(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_disc_change_fn(dev);
+
 	if (ret == 1)
 		seq_puts(m, "on\n");
 	else if (ret == 0)
@@ -7021,6 +7032,7 @@ static ssize_t bypass_wd_write(struct file *file, const char __user *buffer,
 	struct bpctl_dev *dev = PDE_DATA(file_inode(file));
 	int timeout;
 	int ret = kstrtoint_from_user(buffer, count, 10, &timeout);
+
 	if (ret)
 		return ret;
 	set_bypass_wd_fn(dev, timeout);
@@ -7048,6 +7060,7 @@ static int show_wd_expire_time(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = 0, timeout = 0;
+
 	ret = get_wd_expire_time_fn(dev, &timeout);
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
@@ -7066,6 +7079,7 @@ static ssize_t tpl_write(struct file *file, const char __user *buffer,
 {
 	struct bpctl_dev *dev = PDE_DATA(file_inode(file));
 	int tpl_param = user_on_off(buffer, count);
+
 	if (tpl_param < 0)
 		return -1;
 
@@ -7076,6 +7090,7 @@ static int show_tpl(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_tpl_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 1)
@@ -7092,6 +7107,7 @@ static ssize_t wait_at_pwup_write(struct file *file, const char __user *buffer,
 {
 	struct bpctl_dev *dev = PDE_DATA(file_inode(file));
 	int tpl_param = user_on_off(buffer, count);
+
 	if (tpl_param < 0)
 		return -1;
 
@@ -7102,6 +7118,7 @@ static int show_wait_at_pwup(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_bp_wait_at_pwup_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 1)
@@ -7117,6 +7134,7 @@ static ssize_t hw_reset_write(struct file *file, const char __user *buffer,
 {
 	struct bpctl_dev *dev = PDE_DATA(file_inode(file));
 	int tpl_param = user_on_off(buffer, count);
+
 	if (tpl_param < 0)
 		return -1;
 
@@ -7127,6 +7145,7 @@ static int show_hw_reset(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_bp_hw_reset_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 1)
@@ -7143,6 +7162,7 @@ static int show_reset_bypass_wd(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = reset_bypass_wd_timer_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 0)
@@ -7157,6 +7177,7 @@ static ssize_t dis_bypass_write(struct file *file, const char __user *buffer,
 				  size_t count, loff_t *pos)
 {
 	int bypass_param = user_on_off(buffer, count);
+
 	if (bypass_param < 0)
 		return -EINVAL;
 
@@ -7167,6 +7188,7 @@ static int show_dis_bypass(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_dis_bypass_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 0)
@@ -7181,6 +7203,7 @@ static ssize_t dis_tap_write(struct file *file, const char __user *buffer,
 				  size_t count, loff_t *pos)
 {
 	int tap_param = user_on_off(buffer, count);
+
 	if (tap_param < 0)
 		return -EINVAL;
 
@@ -7191,6 +7214,7 @@ static int show_dis_tap(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_dis_tap_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 0)
@@ -7205,6 +7229,7 @@ static ssize_t dis_disc_write(struct file *file, const char __user *buffer,
 				  size_t count, loff_t *pos)
 {
 	int tap_param = user_on_off(buffer, count);
+
 	if (tap_param < 0)
 		return -EINVAL;
 
@@ -7215,6 +7240,7 @@ static int show_dis_disc(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_dis_disc_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 0)
@@ -7229,6 +7255,7 @@ static ssize_t bypass_pwup_write(struct file *file, const char __user *buffer,
 				  size_t count, loff_t *pos)
 {
 	int bypass_param = user_on_off(buffer, count);
+
 	if (bypass_param < 0)
 		return -EINVAL;
 
@@ -7239,6 +7266,7 @@ static int show_bypass_pwup(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_bypass_pwup_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 0)
@@ -7253,6 +7281,7 @@ static ssize_t bypass_pwoff_write(struct file *file, const char __user *buffer,
 				  size_t count, loff_t *pos)
 {
 	int bypass_param = user_on_off(buffer, count);
+
 	if (bypass_param < 0)
 		return -EINVAL;
 
@@ -7263,6 +7292,7 @@ static int show_bypass_pwoff(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_bypass_pwoff_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 0)
@@ -7277,6 +7307,7 @@ static ssize_t tap_pwup_write(struct file *file, const char __user *buffer,
 				  size_t count, loff_t *pos)
 {
 	int tap_param = user_on_off(buffer, count);
+
 	if (tap_param < 0)
 		return -EINVAL;
 
@@ -7287,6 +7318,7 @@ static int show_tap_pwup(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_tap_pwup_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 0)
@@ -7301,6 +7333,7 @@ static ssize_t disc_pwup_write(struct file *file, const char __user *buffer,
 				  size_t count, loff_t *pos)
 {
 	int tap_param = user_on_off(buffer, count);
+
 	if (tap_param < 0)
 		return -EINVAL;
 
@@ -7311,6 +7344,7 @@ static int show_disc_pwup(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_disc_pwup_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 0)
@@ -7325,6 +7359,7 @@ static ssize_t std_nic_write(struct file *file, const char __user *buffer,
 				  size_t count, loff_t *pos)
 {
 	int bypass_param = user_on_off(buffer, count);
+
 	if (bypass_param < 0)
 		return -EINVAL;
 
@@ -7335,6 +7370,7 @@ static int show_std_nic(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_std_nic_fn(dev);
+
 	if (ret == BP_NOT_CAP)
 		seq_puts(m, "fail\n");
 	else if (ret == 0)
@@ -7377,6 +7413,7 @@ static int show_wd_exp_mode(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_wd_exp_mode_fn(dev);
+
 	if (ret == 1)
 		seq_puts(m, "tap\n");
 	else if (ret == 0)
@@ -7394,6 +7431,7 @@ static ssize_t wd_autoreset_write(struct file *file, const char __user *buffer,
 {
 	int timeout;
 	int ret = kstrtoint_from_user(buffer, count, 10, &timeout);
+
 	if (ret)
 		return ret;
 	set_wd_autoreset_fn(PDE_DATA(file_inode(file)), timeout);
@@ -7403,6 +7441,7 @@ static int show_wd_autoreset(struct seq_file *m, void *v)
 {
 	struct bpctl_dev *dev = m->private;
 	int ret = get_wd_autoreset_fn(dev);
+
 	if (ret >= 0)
 		seq_printf(m, "%d\n", ret);
 	else
@@ -7483,8 +7522,8 @@ static int bypass_proc_create_dev_sd(struct bpctl_dev *pbp_device_block)
 
 static int bypass_proc_remove_dev_sd(struct bpctl_dev *pbp_device_block)
 {
-
 	struct bypass_pfs_sd *current_pfs = &pbp_device_block->bypass_pfs_set;
+
 	remove_proc_subtree(current_pfs->dir_name, bp_procfs_dir);
 	current_pfs->bypass_entry = NULL;
 	return 0;
diff --git a/drivers/staging/silicom/bypasslib/bp_ioctl.h b/drivers/staging/silicom/bypasslib/bp_ioctl.h
index bf47f786866..a13932af733 100644
--- a/drivers/staging/silicom/bypasslib/bp_ioctl.h
+++ b/drivers/staging/silicom/bypasslib/bp_ioctl.h
@@ -101,7 +101,7 @@ enum {
 	SET_BP_WAIT_AT_PWUP,
 	GET_BP_HW_RESET,
 	SET_BP_HW_RESET,
-} CMND_TYPE;
+};
 
 enum {
 	IF_SCAN_SD,
@@ -154,7 +154,7 @@ enum {
 	GET_BP_HW_RESET_SD,
 	SET_BP_HW_RESET_SD,
 
-} CMND_TYPE_SD;
+};
 
 #define SIOCGIFBYPASS (SIOCDEVPRIVATE+10)
 
diff --git a/drivers/staging/silicom/bypasslib/bypass.c b/drivers/staging/silicom/bypasslib/bypass.c
index 09e00dac04f..8e714a8ea3a 100644
--- a/drivers/staging/silicom/bypasslib/bypass.c
+++ b/drivers/staging/silicom/bypasslib/bypass.c
@@ -135,6 +135,7 @@ static int doit(int cmd, int if_index, int *data)
 static int is_dev_sd(int if_index)
 {
 	int ret = 0;
+
 	SET_BPLIB_INT_FN(is_bypass, int, if_index, ret);
 	return ret >= 0 ? 1 : 0;
 }
@@ -149,38 +150,33 @@ static int is_bypass_dev(int if_index)
 
 	while ((pdev = pci_get_class(PCI_CLASS_NETWORK_ETHERNET << 8, pdev))) {
 		dev = pci_get_drvdata(pdev);
-		if (dev != NULL) {
-			dev = pci_get_drvdata(pdev);
-			if ((dev != NULL) && (dev->ifindex == if_index)) {
-				if ((pdev->vendor == SILICOM_VID) &&
-				    (pdev->device >= SILICOM_BP_PID_MIN) &&
-				    (pdev->device <= SILICOM_BP_PID_MAX)) {
-					goto send_cmd;
-				}
+		if ((dev != NULL) && (dev->ifindex == if_index)) {
+			if ((pdev->vendor == SILICOM_VID) &&
+			    (pdev->device >= SILICOM_BP_PID_MIN) &&
+			    (pdev->device <= SILICOM_BP_PID_MAX)) {
+				goto send_cmd;
+			}
 #if defined(BP_VENDOR_SUPPORT) && defined(ETHTOOL_GDRVINFO)
-				else {
-					struct ethtool_drvinfo info;
-					const struct ethtool_ops *ops =
-					    dev->ethtool_ops;
-					int k = 0;
-
-					if (ops->get_drvinfo) {
-						memset(&info, 0, sizeof(info));
-						info.cmd = ETHTOOL_GDRVINFO;
-						ops->get_drvinfo(dev, &info);
-						for (; bp_desc_array[k]; k++)
-							if (!
-							    (strcmp
-							     (bp_desc_array[k],
-							      info.driver)))
-								goto send_cmd;
-
-					}
+			else {
+				struct ethtool_drvinfo info;
+				const struct ethtool_ops *ops =
+					dev->ethtool_ops;
+				int k = 0;
+
+				if (ops->get_drvinfo) {
+					memset(&info, 0, sizeof(info));
+					info.cmd = ETHTOOL_GDRVINFO;
+					ops->get_drvinfo(dev, &info);
+					for (; bp_desc_array[k]; k++)
+						if (!(strcmp(bp_desc_array[k],
+							     info.driver)))
+							goto send_cmd;
 
 				}
-#endif
-				return -1;
+
 			}
+#endif
+			return -1;
 		}
 	}
  send_cmd:
@@ -191,6 +187,7 @@ static int is_bypass_dev(int if_index)
 static int is_bypass(int if_index)
 {
 	int ret = 0;
+
 	SET_BPLIB_INT_FN(is_bypass, int, if_index, ret);
 
 	if (ret < 0)
diff --git a/drivers/staging/skein/Kconfig b/drivers/staging/skein/Kconfig
new file mode 100644
index 00000000000..b9172bfcdc1
--- /dev/null
+++ b/drivers/staging/skein/Kconfig
@@ -0,0 +1,32 @@
+config CRYPTO_SKEIN
+	bool "Skein digest algorithm"
+	depends on (X86 || UML_X86) && 64BIT && CRYPTO
+	select CRYPTO_THREEFISH
+	select CRYPTO_HASH
+	help
+	  Skein secure hash algorithm is one of 5 finalists from the NIST SHA3
+	  competition.
+
+	  Skein is optimized for modern, 64bit processors and is highly
+	  customizable.  See:
+
+	  http://www.skein-hash.info/sites/default/files/skein1.3.pdf
+
+	  for more information.  This module depends on the threefish block
+	  cipher module.
+
+config CRYPTO_THREEFISH
+	bool "Threefish tweakable block cipher"
+	depends on (X86 || UML_X86) && 64BIT && CRYPTO
+	select CRYPTO_ALGAPI
+	help
+	  Threefish cipher algorithm is the tweakable block cipher underneath
+	  the Skein family of secure hash algorithms.  Skein is one of 5
+	  finalists from the NIST SHA3 competition.
+
+	  Skein is optimized for modern, 64bit processors and is highly
+	  customizable.  See:
+
+	  http://www.skein-hash.info/sites/default/files/skein1.3.pdf
+
+	  for more information.
diff --git a/drivers/staging/skein/Makefile b/drivers/staging/skein/Makefile
new file mode 100644
index 00000000000..a14aaddd829
--- /dev/null
+++ b/drivers/staging/skein/Makefile
@@ -0,0 +1,9 @@
+#
+# Makefile for the skein secure hash algorithm
+#
+obj-$(CONFIG_CRYPTO_SKEIN) +=   skein.o \
+				skein_api.o \
+				skein_block.o
+
+obj-$(CONFIG_CRYPTO_THREEFISH) += threefish_block.o \
+				  threefish_api.o
diff --git a/drivers/staging/skein/TODO b/drivers/staging/skein/TODO
new file mode 100644
index 00000000000..cd3508dd908
--- /dev/null
+++ b/drivers/staging/skein/TODO
@@ -0,0 +1,8 @@
+skein/threefish TODO
+
+ - move macros into appropriate header files
+ - add / pass test vectors
+ - module support
+
+Please send patches to Jason Cooper <jason@lakedaemon.net> in addition to the
+staging tree mailinglist.
diff --git a/drivers/staging/skein/skein.c b/drivers/staging/skein/skein.c
new file mode 100644
index 00000000000..8cc83587b1f
--- /dev/null
+++ b/drivers/staging/skein/skein.c
@@ -0,0 +1,883 @@
+/***********************************************************************
+**
+** Implementation of the Skein hash function.
+**
+** Source code author: Doug Whiting, 2008.
+**
+** This algorithm and source code is released to the public domain.
+**
+************************************************************************/
+
+#define  SKEIN_PORT_CODE /* instantiate any code in skein_port.h */
+
+#include <linux/string.h>       /* get the memcpy/memset functions */
+#include "skein.h" /* get the Skein API definitions   */
+#include "skein_iv.h"    /* get precomputed IVs */
+#include "skein_block.h"
+
+/*****************************************************************/
+/*     256-bit Skein                                             */
+/*****************************************************************/
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* init the context for a straight hashing operation  */
+int skein_256_init(struct skein_256_ctx *ctx, size_t hash_bit_len)
+{
+	union {
+		u8 b[SKEIN_256_STATE_BYTES];
+		u64 w[SKEIN_256_STATE_WORDS];
+	} cfg;                              /* config block */
+
+	skein_assert_ret(hash_bit_len > 0, SKEIN_BAD_HASHLEN);
+	ctx->h.hash_bit_len = hash_bit_len;         /* output hash bit count */
+
+	switch (hash_bit_len) { /* use pre-computed values, where available */
+	case  256:
+		memcpy(ctx->x, SKEIN_256_IV_256, sizeof(ctx->x));
+		break;
+	case  224:
+		memcpy(ctx->x, SKEIN_256_IV_224, sizeof(ctx->x));
+		break;
+	case  160:
+		memcpy(ctx->x, SKEIN_256_IV_160, sizeof(ctx->x));
+		break;
+	case  128:
+		memcpy(ctx->x, SKEIN_256_IV_128, sizeof(ctx->x));
+		break;
+	default:
+		/* here if there is no precomputed IV value available */
+		/*
+		 * build/process the config block, type == CONFIG (could be
+		 * precomputed)
+		 */
+		/* set tweaks: T0=0; T1=CFG | FINAL */
+		skein_start_new_type(ctx, CFG_FINAL);
+
+		/* set the schema, version */
+		cfg.w[0] = skein_swap64(SKEIN_SCHEMA_VER);
+		/* hash result length in bits */
+		cfg.w[1] = skein_swap64(hash_bit_len);
+		cfg.w[2] = skein_swap64(SKEIN_CFG_TREE_INFO_SEQUENTIAL);
+		/* zero pad config block */
+		memset(&cfg.w[3], 0, sizeof(cfg) - 3*sizeof(cfg.w[0]));
+
+		/* compute the initial chaining values from config block */
+		/* zero the chaining variables */
+		memset(ctx->x, 0, sizeof(ctx->x));
+		skein_256_process_block(ctx, cfg.b, 1, SKEIN_CFG_STR_LEN);
+		break;
+	}
+	/* The chaining vars ctx->x are now initialized for hash_bit_len. */
+	/* Set up to process the data message portion of the hash (default) */
+	skein_start_new_type(ctx, MSG);              /* T0=0, T1= MSG type */
+
+	return SKEIN_SUCCESS;
+}
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* init the context for a MAC and/or tree hash operation */
+/* [identical to skein_256_init() when key_bytes == 0 && \
+ *	tree_info == SKEIN_CFG_TREE_INFO_SEQUENTIAL] */
+int skein_256_init_ext(struct skein_256_ctx *ctx, size_t hash_bit_len,
+		       u64 tree_info, const u8 *key, size_t key_bytes)
+{
+	union {
+		u8  b[SKEIN_256_STATE_BYTES];
+		u64 w[SKEIN_256_STATE_WORDS];
+	} cfg; /* config block */
+
+	skein_assert_ret(hash_bit_len > 0, SKEIN_BAD_HASHLEN);
+	skein_assert_ret(key_bytes == 0 || key != NULL, SKEIN_FAIL);
+
+	/* compute the initial chaining values ctx->x[], based on key */
+	if (key_bytes == 0) { /* is there a key? */
+		/* no key: use all zeroes as key for config block */
+		memset(ctx->x, 0, sizeof(ctx->x));
+	} else { /* here to pre-process a key */
+		skein_assert(sizeof(cfg.b) >= sizeof(ctx->x));
+		/* do a mini-Init right here */
+		/* set output hash bit count = state size */
+		ctx->h.hash_bit_len = 8*sizeof(ctx->x);
+		/* set tweaks: T0 = 0; T1 = KEY type */
+		skein_start_new_type(ctx, KEY);
+		/* zero the initial chaining variables */
+		memset(ctx->x, 0, sizeof(ctx->x));
+		/* hash the key */
+		skein_256_update(ctx, key, key_bytes);
+		/* put result into cfg.b[] */
+		skein_256_final_pad(ctx, cfg.b);
+		/* copy over into ctx->x[] */
+		memcpy(ctx->x, cfg.b, sizeof(cfg.b));
+	}
+	/*
+	 * build/process the config block, type == CONFIG (could be
+	 * precomputed for each key)
+	 */
+	/* output hash bit count */
+	ctx->h.hash_bit_len = hash_bit_len;
+	skein_start_new_type(ctx, CFG_FINAL);
+
+	/* pre-pad cfg.w[] with zeroes */
+	memset(&cfg.w, 0, sizeof(cfg.w));
+	cfg.w[0] = skein_swap64(SKEIN_SCHEMA_VER);
+	/* hash result length in bits */
+	cfg.w[1] = skein_swap64(hash_bit_len);
+	/* tree hash config info (or SKEIN_CFG_TREE_INFO_SEQUENTIAL) */
+	cfg.w[2] = skein_swap64(tree_info);
+
+	skein_show_key(256, &ctx->h, key, key_bytes);
+
+	/* compute the initial chaining values from config block */
+	skein_256_process_block(ctx, cfg.b, 1, SKEIN_CFG_STR_LEN);
+
+	/* The chaining vars ctx->x are now initialized */
+	/* Set up to process the data message portion of the hash (default) */
+	skein_start_new_type(ctx, MSG);
+
+	return SKEIN_SUCCESS;
+}
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* process the input bytes */
+int skein_256_update(struct skein_256_ctx *ctx, const u8 *msg,
+		     size_t msg_byte_cnt)
+{
+	size_t n;
+
+	/* catch uninitialized context */
+	skein_assert_ret(ctx->h.b_cnt <= SKEIN_256_BLOCK_BYTES, SKEIN_FAIL);
+
+	/* process full blocks, if any */
+	if (msg_byte_cnt + ctx->h.b_cnt > SKEIN_256_BLOCK_BYTES) {
+		/* finish up any buffered message data */
+		if (ctx->h.b_cnt) {
+			/* # bytes free in buffer b[] */
+			n = SKEIN_256_BLOCK_BYTES - ctx->h.b_cnt;
+			if (n) {
+				/* check on our logic here */
+				skein_assert(n < msg_byte_cnt);
+				memcpy(&ctx->b[ctx->h.b_cnt], msg, n);
+				msg_byte_cnt  -= n;
+				msg         += n;
+				ctx->h.b_cnt += n;
+			}
+			skein_assert(ctx->h.b_cnt == SKEIN_256_BLOCK_BYTES);
+			skein_256_process_block(ctx, ctx->b, 1,
+						SKEIN_256_BLOCK_BYTES);
+			ctx->h.b_cnt = 0;
+		}
+		/*
+		 * now process any remaining full blocks, directly from input
+		 * message data
+		 */
+		if (msg_byte_cnt > SKEIN_256_BLOCK_BYTES) {
+			/* number of full blocks to process */
+			n = (msg_byte_cnt-1) / SKEIN_256_BLOCK_BYTES;
+			skein_256_process_block(ctx, msg, n,
+						SKEIN_256_BLOCK_BYTES);
+			msg_byte_cnt -= n * SKEIN_256_BLOCK_BYTES;
+			msg        += n * SKEIN_256_BLOCK_BYTES;
+		}
+		skein_assert(ctx->h.b_cnt == 0);
+	}
+
+	/* copy any remaining source message data bytes into b[] */
+	if (msg_byte_cnt) {
+		skein_assert(msg_byte_cnt + ctx->h.b_cnt <=
+			     SKEIN_256_BLOCK_BYTES);
+		memcpy(&ctx->b[ctx->h.b_cnt], msg, msg_byte_cnt);
+		ctx->h.b_cnt += msg_byte_cnt;
+	}
+
+	return SKEIN_SUCCESS;
+}
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* finalize the hash computation and output the result */
+int skein_256_final(struct skein_256_ctx *ctx, u8 *hash_val)
+{
+	size_t i, n, byte_cnt;
+	u64 x[SKEIN_256_STATE_WORDS];
+	/* catch uninitialized context */
+	skein_assert_ret(ctx->h.b_cnt <= SKEIN_256_BLOCK_BYTES, SKEIN_FAIL);
+
+	/* tag as the final block */
+	ctx->h.tweak[1] |= SKEIN_T1_FLAG_FINAL;
+	/* zero pad b[] if necessary */
+	if (ctx->h.b_cnt < SKEIN_256_BLOCK_BYTES)
+		memset(&ctx->b[ctx->h.b_cnt], 0,
+			SKEIN_256_BLOCK_BYTES - ctx->h.b_cnt);
+
+	/* process the final block */
+	skein_256_process_block(ctx, ctx->b, 1, ctx->h.b_cnt);
+
+	/* now output the result */
+	/* total number of output bytes */
+	byte_cnt = (ctx->h.hash_bit_len + 7) >> 3;
+
+	/* run Threefish in "counter mode" to generate output */
+	/* zero out b[], so it can hold the counter */
+	memset(ctx->b, 0, sizeof(ctx->b));
+	/* keep a local copy of counter mode "key" */
+	memcpy(x, ctx->x, sizeof(x));
+	for (i = 0; i*SKEIN_256_BLOCK_BYTES < byte_cnt; i++) {
+		/* build the counter block */
+		((u64 *)ctx->b)[0] = skein_swap64((u64) i);
+		skein_start_new_type(ctx, OUT_FINAL);
+		/* run "counter mode" */
+		skein_256_process_block(ctx, ctx->b, 1, sizeof(u64));
+		/* number of output bytes left to go */
+		n = byte_cnt - i*SKEIN_256_BLOCK_BYTES;
+		if (n >= SKEIN_256_BLOCK_BYTES)
+			n  = SKEIN_256_BLOCK_BYTES;
+		/* "output" the ctr mode bytes */
+		skein_put64_lsb_first(hash_val+i*SKEIN_256_BLOCK_BYTES, ctx->x,
+				      n);
+		skein_show_final(256, &ctx->h, n,
+				 hash_val+i*SKEIN_256_BLOCK_BYTES);
+		/* restore the counter mode key for next time */
+		memcpy(ctx->x, x, sizeof(x));
+	}
+	return SKEIN_SUCCESS;
+}
+
+/*****************************************************************/
+/*     512-bit Skein                                             */
+/*****************************************************************/
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* init the context for a straight hashing operation  */
+int skein_512_init(struct skein_512_ctx *ctx, size_t hash_bit_len)
+{
+	union {
+		u8 b[SKEIN_512_STATE_BYTES];
+		u64 w[SKEIN_512_STATE_WORDS];
+	} cfg;                              /* config block */
+
+	skein_assert_ret(hash_bit_len > 0, SKEIN_BAD_HASHLEN);
+	ctx->h.hash_bit_len = hash_bit_len;         /* output hash bit count */
+
+	switch (hash_bit_len) { /* use pre-computed values, where available */
+	case  512:
+		memcpy(ctx->x, SKEIN_512_IV_512, sizeof(ctx->x));
+		break;
+	case  384:
+		memcpy(ctx->x, SKEIN_512_IV_384, sizeof(ctx->x));
+		break;
+	case  256:
+		memcpy(ctx->x, SKEIN_512_IV_256, sizeof(ctx->x));
+		break;
+	case  224:
+		memcpy(ctx->x, SKEIN_512_IV_224, sizeof(ctx->x));
+		break;
+	default:
+		/* here if there is no precomputed IV value available */
+		/*
+		 * build/process the config block, type == CONFIG (could be
+		 * precomputed)
+		 */
+		/* set tweaks: T0=0; T1=CFG | FINAL */
+		skein_start_new_type(ctx, CFG_FINAL);
+
+		/* set the schema, version */
+		cfg.w[0] = skein_swap64(SKEIN_SCHEMA_VER);
+		/* hash result length in bits */
+		cfg.w[1] = skein_swap64(hash_bit_len);
+		cfg.w[2] = skein_swap64(SKEIN_CFG_TREE_INFO_SEQUENTIAL);
+		/* zero pad config block */
+		memset(&cfg.w[3], 0, sizeof(cfg) - 3*sizeof(cfg.w[0]));
+
+		/* compute the initial chaining values from config block */
+		/* zero the chaining variables */
+		memset(ctx->x, 0, sizeof(ctx->x));
+		skein_512_process_block(ctx, cfg.b, 1, SKEIN_CFG_STR_LEN);
+		break;
+	}
+
+	/*
+	 * The chaining vars ctx->x are now initialized for the given
+	 * hash_bit_len.
+	 */
+	/* Set up to process the data message portion of the hash (default) */
+	skein_start_new_type(ctx, MSG);              /* T0=0, T1= MSG type */
+
+	return SKEIN_SUCCESS;
+}
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* init the context for a MAC and/or tree hash operation */
+/* [identical to skein_512_init() when key_bytes == 0 && \
+ *	tree_info == SKEIN_CFG_TREE_INFO_SEQUENTIAL] */
+int skein_512_init_ext(struct skein_512_ctx *ctx, size_t hash_bit_len,
+		       u64 tree_info, const u8 *key, size_t key_bytes)
+{
+	union {
+		u8 b[SKEIN_512_STATE_BYTES];
+		u64 w[SKEIN_512_STATE_WORDS];
+	} cfg;                              /* config block */
+
+	skein_assert_ret(hash_bit_len > 0, SKEIN_BAD_HASHLEN);
+	skein_assert_ret(key_bytes == 0 || key != NULL, SKEIN_FAIL);
+
+	/* compute the initial chaining values ctx->x[], based on key */
+	if (key_bytes == 0) { /* is there a key? */
+		/* no key: use all zeroes as key for config block */
+		memset(ctx->x, 0, sizeof(ctx->x));
+	} else { /* here to pre-process a key */
+		skein_assert(sizeof(cfg.b) >= sizeof(ctx->x));
+		/* do a mini-Init right here */
+		/* set output hash bit count = state size */
+		ctx->h.hash_bit_len = 8*sizeof(ctx->x);
+		/* set tweaks: T0 = 0; T1 = KEY type */
+		skein_start_new_type(ctx, KEY);
+		/* zero the initial chaining variables */
+		memset(ctx->x, 0, sizeof(ctx->x));
+		/* hash the key */
+		skein_512_update(ctx, key, key_bytes);
+		/* put result into cfg.b[] */
+		skein_512_final_pad(ctx, cfg.b);
+		/* copy over into ctx->x[] */
+		memcpy(ctx->x, cfg.b, sizeof(cfg.b));
+	}
+	/*
+	 * build/process the config block, type == CONFIG (could be
+	 * precomputed for each key)
+	 */
+	ctx->h.hash_bit_len = hash_bit_len;          /* output hash bit count */
+	skein_start_new_type(ctx, CFG_FINAL);
+
+	/* pre-pad cfg.w[] with zeroes */
+	memset(&cfg.w, 0, sizeof(cfg.w));
+	cfg.w[0] = skein_swap64(SKEIN_SCHEMA_VER);
+	/* hash result length in bits */
+	cfg.w[1] = skein_swap64(hash_bit_len);
+	/* tree hash config info (or SKEIN_CFG_TREE_INFO_SEQUENTIAL) */
+	cfg.w[2] = skein_swap64(tree_info);
+
+	skein_show_key(512, &ctx->h, key, key_bytes);
+
+	/* compute the initial chaining values from config block */
+	skein_512_process_block(ctx, cfg.b, 1, SKEIN_CFG_STR_LEN);
+
+	/* The chaining vars ctx->x are now initialized */
+	/* Set up to process the data message portion of the hash (default) */
+	skein_start_new_type(ctx, MSG);
+
+	return SKEIN_SUCCESS;
+}
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* process the input bytes */
+int skein_512_update(struct skein_512_ctx *ctx, const u8 *msg,
+		     size_t msg_byte_cnt)
+{
+	size_t n;
+
+	/* catch uninitialized context */
+	skein_assert_ret(ctx->h.b_cnt <= SKEIN_512_BLOCK_BYTES, SKEIN_FAIL);
+
+	/* process full blocks, if any */
+	if (msg_byte_cnt + ctx->h.b_cnt > SKEIN_512_BLOCK_BYTES) {
+		/* finish up any buffered message data */
+		if (ctx->h.b_cnt) {
+			/* # bytes free in buffer b[] */
+			n = SKEIN_512_BLOCK_BYTES - ctx->h.b_cnt;
+			if (n) {
+				/* check on our logic here */
+				skein_assert(n < msg_byte_cnt);
+				memcpy(&ctx->b[ctx->h.b_cnt], msg, n);
+				msg_byte_cnt  -= n;
+				msg         += n;
+				ctx->h.b_cnt += n;
+			}
+			skein_assert(ctx->h.b_cnt == SKEIN_512_BLOCK_BYTES);
+			skein_512_process_block(ctx, ctx->b, 1,
+						SKEIN_512_BLOCK_BYTES);
+			ctx->h.b_cnt = 0;
+		}
+		/*
+		 * now process any remaining full blocks, directly from input
+		 * message data
+		 */
+		if (msg_byte_cnt > SKEIN_512_BLOCK_BYTES) {
+			/* number of full blocks to process */
+			n = (msg_byte_cnt-1) / SKEIN_512_BLOCK_BYTES;
+			skein_512_process_block(ctx, msg, n,
+						SKEIN_512_BLOCK_BYTES);
+			msg_byte_cnt -= n * SKEIN_512_BLOCK_BYTES;
+			msg        += n * SKEIN_512_BLOCK_BYTES;
+		}
+		skein_assert(ctx->h.b_cnt == 0);
+	}
+
+	/* copy any remaining source message data bytes into b[] */
+	if (msg_byte_cnt) {
+		skein_assert(msg_byte_cnt + ctx->h.b_cnt <=
+			     SKEIN_512_BLOCK_BYTES);
+		memcpy(&ctx->b[ctx->h.b_cnt], msg, msg_byte_cnt);
+		ctx->h.b_cnt += msg_byte_cnt;
+	}
+
+	return SKEIN_SUCCESS;
+}
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* finalize the hash computation and output the result */
+int skein_512_final(struct skein_512_ctx *ctx, u8 *hash_val)
+{
+	size_t i, n, byte_cnt;
+	u64 x[SKEIN_512_STATE_WORDS];
+	/* catch uninitialized context */
+	skein_assert_ret(ctx->h.b_cnt <= SKEIN_512_BLOCK_BYTES, SKEIN_FAIL);
+
+	/* tag as the final block */
+	ctx->h.tweak[1] |= SKEIN_T1_FLAG_FINAL;
+	/* zero pad b[] if necessary */
+	if (ctx->h.b_cnt < SKEIN_512_BLOCK_BYTES)
+		memset(&ctx->b[ctx->h.b_cnt], 0,
+			SKEIN_512_BLOCK_BYTES - ctx->h.b_cnt);
+
+	/* process the final block */
+	skein_512_process_block(ctx, ctx->b, 1, ctx->h.b_cnt);
+
+	/* now output the result */
+	/* total number of output bytes */
+	byte_cnt = (ctx->h.hash_bit_len + 7) >> 3;
+
+	/* run Threefish in "counter mode" to generate output */
+	/* zero out b[], so it can hold the counter */
+	memset(ctx->b, 0, sizeof(ctx->b));
+	/* keep a local copy of counter mode "key" */
+	memcpy(x, ctx->x, sizeof(x));
+	for (i = 0; i*SKEIN_512_BLOCK_BYTES < byte_cnt; i++) {
+		/* build the counter block */
+		((u64 *)ctx->b)[0] = skein_swap64((u64) i);
+		skein_start_new_type(ctx, OUT_FINAL);
+		/* run "counter mode" */
+		skein_512_process_block(ctx, ctx->b, 1, sizeof(u64));
+		/* number of output bytes left to go */
+		n = byte_cnt - i*SKEIN_512_BLOCK_BYTES;
+		if (n >= SKEIN_512_BLOCK_BYTES)
+			n  = SKEIN_512_BLOCK_BYTES;
+		/* "output" the ctr mode bytes */
+		skein_put64_lsb_first(hash_val+i*SKEIN_512_BLOCK_BYTES, ctx->x,
+				      n);
+		skein_show_final(512, &ctx->h, n,
+				 hash_val+i*SKEIN_512_BLOCK_BYTES);
+		/* restore the counter mode key for next time */
+		memcpy(ctx->x, x, sizeof(x));
+	}
+	return SKEIN_SUCCESS;
+}
+
+/*****************************************************************/
+/*    1024-bit Skein                                             */
+/*****************************************************************/
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* init the context for a straight hashing operation  */
+int skein_1024_init(struct skein_1024_ctx *ctx, size_t hash_bit_len)
+{
+	union {
+		u8 b[SKEIN_1024_STATE_BYTES];
+		u64 w[SKEIN_1024_STATE_WORDS];
+	} cfg;                              /* config block */
+
+	skein_assert_ret(hash_bit_len > 0, SKEIN_BAD_HASHLEN);
+	ctx->h.hash_bit_len = hash_bit_len;         /* output hash bit count */
+
+	switch (hash_bit_len) { /* use pre-computed values, where available */
+	case  512:
+		memcpy(ctx->x, SKEIN_1024_IV_512, sizeof(ctx->x));
+		break;
+	case  384:
+		memcpy(ctx->x, SKEIN_1024_IV_384, sizeof(ctx->x));
+		break;
+	case 1024:
+		memcpy(ctx->x, SKEIN_1024_IV_1024, sizeof(ctx->x));
+		break;
+	default:
+		/* here if there is no precomputed IV value available */
+		/*
+		 * build/process the config block, type == CONFIG
+		 * (could be precomputed)
+		 */
+		/* set tweaks: T0=0; T1=CFG | FINAL */
+		skein_start_new_type(ctx, CFG_FINAL);
+
+		/* set the schema, version */
+		cfg.w[0] = skein_swap64(SKEIN_SCHEMA_VER);
+		/* hash result length in bits */
+		cfg.w[1] = skein_swap64(hash_bit_len);
+		cfg.w[2] = skein_swap64(SKEIN_CFG_TREE_INFO_SEQUENTIAL);
+		/* zero pad config block */
+		memset(&cfg.w[3], 0, sizeof(cfg) - 3*sizeof(cfg.w[0]));
+
+		/* compute the initial chaining values from config block */
+		/* zero the chaining variables */
+		memset(ctx->x, 0, sizeof(ctx->x));
+		skein_1024_process_block(ctx, cfg.b, 1, SKEIN_CFG_STR_LEN);
+		break;
+	}
+
+	/* The chaining vars ctx->x are now initialized for the hash_bit_len. */
+	/* Set up to process the data message portion of the hash (default) */
+	skein_start_new_type(ctx, MSG);              /* T0=0, T1= MSG type */
+
+	return SKEIN_SUCCESS;
+}
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* init the context for a MAC and/or tree hash operation */
+/* [identical to skein_1024_init() when key_bytes == 0 && \
+ *	tree_info == SKEIN_CFG_TREE_INFO_SEQUENTIAL] */
+int skein_1024_init_ext(struct skein_1024_ctx *ctx, size_t hash_bit_len,
+			u64 tree_info, const u8 *key, size_t key_bytes)
+{
+	union {
+		u8 b[SKEIN_1024_STATE_BYTES];
+		u64 w[SKEIN_1024_STATE_WORDS];
+	} cfg;                              /* config block */
+
+	skein_assert_ret(hash_bit_len > 0, SKEIN_BAD_HASHLEN);
+	skein_assert_ret(key_bytes == 0 || key != NULL, SKEIN_FAIL);
+
+	/* compute the initial chaining values ctx->x[], based on key */
+	if (key_bytes == 0) { /* is there a key? */
+		/* no key: use all zeroes as key for config block */
+		memset(ctx->x, 0, sizeof(ctx->x));
+	} else { /* here to pre-process a key */
+		skein_assert(sizeof(cfg.b) >= sizeof(ctx->x));
+		/* do a mini-Init right here */
+		/* set output hash bit count = state size */
+		ctx->h.hash_bit_len = 8*sizeof(ctx->x);
+		/* set tweaks: T0 = 0; T1 = KEY type */
+		skein_start_new_type(ctx, KEY);
+		/* zero the initial chaining variables */
+		memset(ctx->x, 0, sizeof(ctx->x));
+		/* hash the key */
+		skein_1024_update(ctx, key, key_bytes);
+		/* put result into cfg.b[] */
+		skein_1024_final_pad(ctx, cfg.b);
+		/* copy over into ctx->x[] */
+		memcpy(ctx->x, cfg.b, sizeof(cfg.b));
+	}
+	/*
+	 * build/process the config block, type == CONFIG (could be
+	 * precomputed for each key)
+	 */
+	/* output hash bit count */
+	ctx->h.hash_bit_len = hash_bit_len;
+	skein_start_new_type(ctx, CFG_FINAL);
+
+	/* pre-pad cfg.w[] with zeroes */
+	memset(&cfg.w, 0, sizeof(cfg.w));
+	cfg.w[0] = skein_swap64(SKEIN_SCHEMA_VER);
+	/* hash result length in bits */
+	cfg.w[1] = skein_swap64(hash_bit_len);
+	/* tree hash config info (or SKEIN_CFG_TREE_INFO_SEQUENTIAL) */
+	cfg.w[2] = skein_swap64(tree_info);
+
+	skein_show_key(1024, &ctx->h, key, key_bytes);
+
+	/* compute the initial chaining values from config block */
+	skein_1024_process_block(ctx, cfg.b, 1, SKEIN_CFG_STR_LEN);
+
+	/* The chaining vars ctx->x are now initialized */
+	/* Set up to process the data message portion of the hash (default) */
+	skein_start_new_type(ctx, MSG);
+
+	return SKEIN_SUCCESS;
+}
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* process the input bytes */
+int skein_1024_update(struct skein_1024_ctx *ctx, const u8 *msg,
+		      size_t msg_byte_cnt)
+{
+	size_t n;
+
+	/* catch uninitialized context */
+	skein_assert_ret(ctx->h.b_cnt <= SKEIN_1024_BLOCK_BYTES, SKEIN_FAIL);
+
+	/* process full blocks, if any */
+	if (msg_byte_cnt + ctx->h.b_cnt > SKEIN_1024_BLOCK_BYTES) {
+		/* finish up any buffered message data */
+		if (ctx->h.b_cnt) {
+			/* # bytes free in buffer b[] */
+			n = SKEIN_1024_BLOCK_BYTES - ctx->h.b_cnt;
+			if (n) {
+				/* check on our logic here */
+				skein_assert(n < msg_byte_cnt);
+				memcpy(&ctx->b[ctx->h.b_cnt], msg, n);
+				msg_byte_cnt  -= n;
+				msg         += n;
+				ctx->h.b_cnt += n;
+			}
+			skein_assert(ctx->h.b_cnt == SKEIN_1024_BLOCK_BYTES);
+			skein_1024_process_block(ctx, ctx->b, 1,
+						 SKEIN_1024_BLOCK_BYTES);
+			ctx->h.b_cnt = 0;
+		}
+		/*
+		 * now process any remaining full blocks, directly from input
+		 * message data
+		 */
+		if (msg_byte_cnt > SKEIN_1024_BLOCK_BYTES) {
+			/* number of full blocks to process */
+			n = (msg_byte_cnt-1) / SKEIN_1024_BLOCK_BYTES;
+			skein_1024_process_block(ctx, msg, n,
+						 SKEIN_1024_BLOCK_BYTES);
+			msg_byte_cnt -= n * SKEIN_1024_BLOCK_BYTES;
+			msg        += n * SKEIN_1024_BLOCK_BYTES;
+		}
+		skein_assert(ctx->h.b_cnt == 0);
+	}
+
+	/* copy any remaining source message data bytes into b[] */
+	if (msg_byte_cnt) {
+		skein_assert(msg_byte_cnt + ctx->h.b_cnt <=
+			     SKEIN_1024_BLOCK_BYTES);
+		memcpy(&ctx->b[ctx->h.b_cnt], msg, msg_byte_cnt);
+		ctx->h.b_cnt += msg_byte_cnt;
+	}
+
+	return SKEIN_SUCCESS;
+}
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* finalize the hash computation and output the result */
+int skein_1024_final(struct skein_1024_ctx *ctx, u8 *hash_val)
+{
+	size_t i, n, byte_cnt;
+	u64 x[SKEIN_1024_STATE_WORDS];
+	/* catch uninitialized context */
+	skein_assert_ret(ctx->h.b_cnt <= SKEIN_1024_BLOCK_BYTES, SKEIN_FAIL);
+
+	/* tag as the final block */
+	ctx->h.tweak[1] |= SKEIN_T1_FLAG_FINAL;
+	/* zero pad b[] if necessary */
+	if (ctx->h.b_cnt < SKEIN_1024_BLOCK_BYTES)
+		memset(&ctx->b[ctx->h.b_cnt], 0,
+			SKEIN_1024_BLOCK_BYTES - ctx->h.b_cnt);
+
+	/* process the final block */
+	skein_1024_process_block(ctx, ctx->b, 1, ctx->h.b_cnt);
+
+	/* now output the result */
+	/* total number of output bytes */
+	byte_cnt = (ctx->h.hash_bit_len + 7) >> 3;
+
+	/* run Threefish in "counter mode" to generate output */
+	/* zero out b[], so it can hold the counter */
+	memset(ctx->b, 0, sizeof(ctx->b));
+	/* keep a local copy of counter mode "key" */
+	memcpy(x, ctx->x, sizeof(x));
+	for (i = 0; i*SKEIN_1024_BLOCK_BYTES < byte_cnt; i++) {
+		/* build the counter block */
+		((u64 *)ctx->b)[0] = skein_swap64((u64) i);
+		skein_start_new_type(ctx, OUT_FINAL);
+		/* run "counter mode" */
+		skein_1024_process_block(ctx, ctx->b, 1, sizeof(u64));
+		/* number of output bytes left to go */
+		n = byte_cnt - i*SKEIN_1024_BLOCK_BYTES;
+		if (n >= SKEIN_1024_BLOCK_BYTES)
+			n  = SKEIN_1024_BLOCK_BYTES;
+		/* "output" the ctr mode bytes */
+		skein_put64_lsb_first(hash_val+i*SKEIN_1024_BLOCK_BYTES, ctx->x,
+				      n);
+		skein_show_final(1024, &ctx->h, n,
+				 hash_val+i*SKEIN_1024_BLOCK_BYTES);
+		/* restore the counter mode key for next time */
+		memcpy(ctx->x, x, sizeof(x));
+	}
+	return SKEIN_SUCCESS;
+}
+
+/**************** Functions to support MAC/tree hashing ***************/
+/*   (this code is identical for Optimized and Reference versions)    */
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* finalize the hash computation and output the block, no OUTPUT stage */
+int skein_256_final_pad(struct skein_256_ctx *ctx, u8 *hash_val)
+{
+	/* catch uninitialized context */
+	skein_assert_ret(ctx->h.b_cnt <= SKEIN_256_BLOCK_BYTES, SKEIN_FAIL);
+
+	/* tag as the final block */
+	ctx->h.tweak[1] |= SKEIN_T1_FLAG_FINAL;
+	/* zero pad b[] if necessary */
+	if (ctx->h.b_cnt < SKEIN_256_BLOCK_BYTES)
+		memset(&ctx->b[ctx->h.b_cnt], 0,
+			SKEIN_256_BLOCK_BYTES - ctx->h.b_cnt);
+	/* process the final block */
+	skein_256_process_block(ctx, ctx->b, 1, ctx->h.b_cnt);
+
+	/* "output" the state bytes */
+	skein_put64_lsb_first(hash_val, ctx->x, SKEIN_256_BLOCK_BYTES);
+
+	return SKEIN_SUCCESS;
+}
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* finalize the hash computation and output the block, no OUTPUT stage */
+int skein_512_final_pad(struct skein_512_ctx *ctx, u8 *hash_val)
+{
+	/* catch uninitialized context */
+	skein_assert_ret(ctx->h.b_cnt <= SKEIN_512_BLOCK_BYTES, SKEIN_FAIL);
+
+	/* tag as the final block */
+	ctx->h.tweak[1] |= SKEIN_T1_FLAG_FINAL;
+	/* zero pad b[] if necessary */
+	if (ctx->h.b_cnt < SKEIN_512_BLOCK_BYTES)
+		memset(&ctx->b[ctx->h.b_cnt], 0,
+			SKEIN_512_BLOCK_BYTES - ctx->h.b_cnt);
+	/* process the final block */
+	skein_512_process_block(ctx, ctx->b, 1, ctx->h.b_cnt);
+
+	/* "output" the state bytes */
+	skein_put64_lsb_first(hash_val, ctx->x, SKEIN_512_BLOCK_BYTES);
+
+	return SKEIN_SUCCESS;
+}
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* finalize the hash computation and output the block, no OUTPUT stage */
+int skein_1024_final_pad(struct skein_1024_ctx *ctx, u8 *hash_val)
+{
+	/* catch uninitialized context */
+	skein_assert_ret(ctx->h.b_cnt <= SKEIN_1024_BLOCK_BYTES, SKEIN_FAIL);
+
+	/* tag as the final block */
+	ctx->h.tweak[1] |= SKEIN_T1_FLAG_FINAL;
+	/* zero pad b[] if necessary */
+	if (ctx->h.b_cnt < SKEIN_1024_BLOCK_BYTES)
+		memset(&ctx->b[ctx->h.b_cnt], 0,
+			SKEIN_1024_BLOCK_BYTES - ctx->h.b_cnt);
+	/* process the final block */
+	skein_1024_process_block(ctx, ctx->b, 1, ctx->h.b_cnt);
+
+	/* "output" the state bytes */
+	skein_put64_lsb_first(hash_val, ctx->x, SKEIN_1024_BLOCK_BYTES);
+
+	return SKEIN_SUCCESS;
+}
+
+#if SKEIN_TREE_HASH
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* just do the OUTPUT stage                                       */
+int skein_256_output(struct skein_256_ctx *ctx, u8 *hash_val)
+{
+	size_t i, n, byte_cnt;
+	u64 x[SKEIN_256_STATE_WORDS];
+	/* catch uninitialized context */
+	skein_assert_ret(ctx->h.b_cnt <= SKEIN_256_BLOCK_BYTES, SKEIN_FAIL);
+
+	/* now output the result */
+	/* total number of output bytes */
+	byte_cnt = (ctx->h.hash_bit_len + 7) >> 3;
+
+	/* run Threefish in "counter mode" to generate output */
+	/* zero out b[], so it can hold the counter */
+	memset(ctx->b, 0, sizeof(ctx->b));
+	/* keep a local copy of counter mode "key" */
+	memcpy(x, ctx->x, sizeof(x));
+	for (i = 0; i*SKEIN_256_BLOCK_BYTES < byte_cnt; i++) {
+		/* build the counter block */
+		((u64 *)ctx->b)[0] = skein_swap64((u64) i);
+		skein_start_new_type(ctx, OUT_FINAL);
+		/* run "counter mode" */
+		skein_256_process_block(ctx, ctx->b, 1, sizeof(u64));
+		/* number of output bytes left to go */
+		n = byte_cnt - i*SKEIN_256_BLOCK_BYTES;
+		if (n >= SKEIN_256_BLOCK_BYTES)
+			n  = SKEIN_256_BLOCK_BYTES;
+		/* "output" the ctr mode bytes */
+		skein_put64_lsb_first(hash_val+i*SKEIN_256_BLOCK_BYTES, ctx->x,
+				      n);
+		skein_show_final(256, &ctx->h, n,
+				 hash_val+i*SKEIN_256_BLOCK_BYTES);
+		/* restore the counter mode key for next time */
+		memcpy(ctx->x, x, sizeof(x));
+	}
+	return SKEIN_SUCCESS;
+}
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* just do the OUTPUT stage                                       */
+int skein_512_output(struct skein_512_ctx *ctx, u8 *hash_val)
+{
+	size_t i, n, byte_cnt;
+	u64 x[SKEIN_512_STATE_WORDS];
+	/* catch uninitialized context */
+	skein_assert_ret(ctx->h.b_cnt <= SKEIN_512_BLOCK_BYTES, SKEIN_FAIL);
+
+	/* now output the result */
+	/* total number of output bytes */
+	byte_cnt = (ctx->h.hash_bit_len + 7) >> 3;
+
+	/* run Threefish in "counter mode" to generate output */
+	/* zero out b[], so it can hold the counter */
+	memset(ctx->b, 0, sizeof(ctx->b));
+	/* keep a local copy of counter mode "key" */
+	memcpy(x, ctx->x, sizeof(x));
+	for (i = 0; i*SKEIN_512_BLOCK_BYTES < byte_cnt; i++) {
+		/* build the counter block */
+		((u64 *)ctx->b)[0] = skein_swap64((u64) i);
+		skein_start_new_type(ctx, OUT_FINAL);
+		/* run "counter mode" */
+		skein_512_process_block(ctx, ctx->b, 1, sizeof(u64));
+		/* number of output bytes left to go */
+		n = byte_cnt - i*SKEIN_512_BLOCK_BYTES;
+		if (n >= SKEIN_512_BLOCK_BYTES)
+			n  = SKEIN_512_BLOCK_BYTES;
+		/* "output" the ctr mode bytes */
+		skein_put64_lsb_first(hash_val+i*SKEIN_512_BLOCK_BYTES, ctx->x,
+				      n);
+		skein_show_final(256, &ctx->h, n,
+				 hash_val+i*SKEIN_512_BLOCK_BYTES);
+		/* restore the counter mode key for next time */
+		memcpy(ctx->x, x, sizeof(x));
+	}
+	return SKEIN_SUCCESS;
+}
+
+/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+/* just do the OUTPUT stage                                       */
+int skein_1024_output(struct skein_1024_ctx *ctx, u8 *hash_val)
+{
+	size_t i, n, byte_cnt;
+	u64 x[SKEIN_1024_STATE_WORDS];
+	/* catch uninitialized context */
+	skein_assert_ret(ctx->h.b_cnt <= SKEIN_1024_BLOCK_BYTES, SKEIN_FAIL);
+
+	/* now output the result */
+	/* total number of output bytes */
+	byte_cnt = (ctx->h.hash_bit_len + 7) >> 3;
+
+	/* run Threefish in "counter mode" to generate output */
+	/* zero out b[], so it can hold the counter */
+	memset(ctx->b, 0, sizeof(ctx->b));
+	/* keep a local copy of counter mode "key" */
+	memcpy(x, ctx->x, sizeof(x));
+	for (i = 0; i*SKEIN_1024_BLOCK_BYTES < byte_cnt; i++) {
+		/* build the counter block */
+		((u64 *)ctx->b)[0] = skein_swap64((u64) i);
+		skein_start_new_type(ctx, OUT_FINAL);
+		/* run "counter mode" */
+		skein_1024_process_block(ctx, ctx->b, 1, sizeof(u64));
+		/* number of output bytes left to go */
+		n = byte_cnt - i*SKEIN_1024_BLOCK_BYTES;
+		if (n >= SKEIN_1024_BLOCK_BYTES)
+			n  = SKEIN_1024_BLOCK_BYTES;
+		/* "output" the ctr mode bytes */
+		skein_put64_lsb_first(hash_val+i*SKEIN_1024_BLOCK_BYTES, ctx->x,
+				      n);
+		skein_show_final(256, &ctx->h, n,
+				 hash_val+i*SKEIN_1024_BLOCK_BYTES);
+		/* restore the counter mode key for next time */
+		memcpy(ctx->x, x, sizeof(x));
+	}
+	return SKEIN_SUCCESS;
+}
+#endif
diff --git a/drivers/staging/skein/skein.h b/drivers/staging/skein/skein.h
new file mode 100644
index 00000000000..e6669f196e5
--- /dev/null
+++ b/drivers/staging/skein/skein.h
@@ -0,0 +1,346 @@
+#ifndef _SKEIN_H_
+#define _SKEIN_H_     1
+/**************************************************************************
+**
+** Interface declarations and internal definitions for Skein hashing.
+**
+** Source code author: Doug Whiting, 2008.
+**
+** This algorithm and source code is released to the public domain.
+**
+***************************************************************************
+**
+** The following compile-time switches may be defined to control some
+** tradeoffs between speed, code size, error checking, and security.
+**
+** The "default" note explains what happens when the switch is not defined.
+**
+**  SKEIN_DEBUG            -- make callouts from inside Skein code
+**                            to examine/display intermediate values.
+**                            [default: no callouts (no overhead)]
+**
+**  SKEIN_ERR_CHECK        -- how error checking is handled inside Skein
+**                            code. If not defined, most error checking
+**                            is disabled (for performance). Otherwise,
+**                            the switch value is interpreted as:
+**                                0: use assert()      to flag errors
+**                                1: return SKEIN_FAIL to flag errors
+**
+***************************************************************************/
+
+#ifndef rotl_64
+#define rotl_64(x, N)    (((x) << (N)) | ((x) >> (64-(N))))
+#endif
+
+/* below two prototype assume we are handed aligned data */
+#define skein_put64_lsb_first(dst08, src64, b_cnt) memcpy(dst08, src64, b_cnt)
+#define skein_get64_lsb_first(dst64, src08, w_cnt) \
+		memcpy(dst64, src08, 8*(w_cnt))
+#define skein_swap64(w64)  (w64)
+
+enum {
+	SKEIN_SUCCESS         =      0, /* return codes from Skein calls */
+	SKEIN_FAIL            =      1,
+	SKEIN_BAD_HASHLEN     =      2
+};
+
+#define  SKEIN_MODIFIER_WORDS   (2) /* number of modifier (tweak) words */
+
+#define  SKEIN_256_STATE_WORDS  (4)
+#define  SKEIN_512_STATE_WORDS  (8)
+#define  SKEIN_1024_STATE_WORDS (16)
+#define  SKEIN_MAX_STATE_WORDS (16)
+
+#define  SKEIN_256_STATE_BYTES  (8*SKEIN_256_STATE_WORDS)
+#define  SKEIN_512_STATE_BYTES  (8*SKEIN_512_STATE_WORDS)
+#define  SKEIN_1024_STATE_BYTES  (8*SKEIN_1024_STATE_WORDS)
+
+#define  SKEIN_256_STATE_BITS  (64*SKEIN_256_STATE_WORDS)
+#define  SKEIN_512_STATE_BITS  (64*SKEIN_512_STATE_WORDS)
+#define  SKEIN_1024_STATE_BITS  (64*SKEIN_1024_STATE_WORDS)
+
+#define  SKEIN_256_BLOCK_BYTES  (8*SKEIN_256_STATE_WORDS)
+#define  SKEIN_512_BLOCK_BYTES  (8*SKEIN_512_STATE_WORDS)
+#define  SKEIN_1024_BLOCK_BYTES  (8*SKEIN_1024_STATE_WORDS)
+
+struct skein_ctx_hdr {
+	size_t hash_bit_len;		/* size of hash result, in bits */
+	size_t b_cnt;			/* current byte count in buffer b[] */
+	u64 tweak[SKEIN_MODIFIER_WORDS]; /* tweak[0]=byte cnt, tweak[1]=flags */
+};
+
+struct skein_256_ctx { /* 256-bit Skein hash context structure */
+	struct skein_ctx_hdr h;		/* common header context variables */
+	u64 x[SKEIN_256_STATE_WORDS];	/* chaining variables */
+	u8 b[SKEIN_256_BLOCK_BYTES];	/* partial block buf (8-byte aligned) */
+};
+
+struct skein_512_ctx { /* 512-bit Skein hash context structure */
+	struct skein_ctx_hdr h;		/* common header context variables */
+	u64 x[SKEIN_512_STATE_WORDS];	/* chaining variables */
+	u8 b[SKEIN_512_BLOCK_BYTES];	/* partial block buf (8-byte aligned) */
+};
+
+struct skein_1024_ctx { /* 1024-bit Skein hash context structure */
+	struct skein_ctx_hdr h;		/* common header context variables */
+	u64 x[SKEIN_1024_STATE_WORDS];	/* chaining variables */
+	u8 b[SKEIN_1024_BLOCK_BYTES];	/* partial block buf (8-byte aligned) */
+};
+
+/* Skein APIs for (incremental) "straight hashing" */
+int skein_256_init(struct skein_256_ctx *ctx, size_t hash_bit_len);
+int skein_512_init(struct skein_512_ctx *ctx, size_t hash_bit_len);
+int skein_1024_init(struct skein_1024_ctx *ctx, size_t hash_bit_len);
+
+int skein_256_update(struct skein_256_ctx *ctx, const u8 *msg,
+		     size_t msg_byte_cnt);
+int skein_512_update(struct skein_512_ctx *ctx, const u8 *msg,
+		     size_t msg_byte_cnt);
+int skein_1024_update(struct skein_1024_ctx *ctx, const u8 *msg,
+		      size_t msg_byte_cnt);
+
+int skein_256_final(struct skein_256_ctx *ctx, u8 *hash_val);
+int skein_512_final(struct skein_512_ctx *ctx, u8 *hash_val);
+int skein_1024_final(struct skein_1024_ctx *ctx, u8 *hash_val);
+
+/*
+**   Skein APIs for "extended" initialization: MAC keys, tree hashing.
+**   After an init_ext() call, just use update/final calls as with init().
+**
+**   Notes: Same parameters as _init() calls, plus tree_info/key/key_bytes.
+**          When key_bytes == 0 and tree_info == SKEIN_SEQUENTIAL,
+**              the results of init_ext() are identical to calling init().
+**          The function init() may be called once to "precompute" the IV for
+**              a given hash_bit_len value, then by saving a copy of the context
+**              the IV computation may be avoided in later calls.
+**          Similarly, the function init_ext() may be called once per MAC key
+**              to precompute the MAC IV, then a copy of the context saved and
+**              reused for each new MAC computation.
+**/
+int skein_256_init_ext(struct skein_256_ctx *ctx, size_t hash_bit_len,
+		       u64 tree_info, const u8 *key, size_t key_bytes);
+int skein_512_init_ext(struct skein_512_ctx *ctx, size_t hash_bit_len,
+		       u64 tree_info, const u8 *key, size_t key_bytes);
+int skein_1024_init_ext(struct skein_1024_ctx *ctx, size_t hash_bit_len,
+			u64 tree_info, const u8 *key, size_t key_bytes);
+
+/*
+**   Skein APIs for MAC and tree hash:
+**      final_pad:  pad, do final block, but no OUTPUT type
+**      output:     do just the output stage
+*/
+int skein_256_final_pad(struct skein_256_ctx *ctx, u8 *hash_val);
+int skein_512_final_pad(struct skein_512_ctx *ctx, u8 *hash_val);
+int skein_1024_final_pad(struct skein_1024_ctx *ctx, u8 *hash_val);
+
+#ifndef SKEIN_TREE_HASH
+#define SKEIN_TREE_HASH (1)
+#endif
+#if  SKEIN_TREE_HASH
+int skein_256_output(struct skein_256_ctx *ctx, u8 *hash_val);
+int skein_512_output(struct skein_512_ctx *ctx, u8 *hash_val);
+int skein_1024_output(struct skein_1024_ctx *ctx, u8 *hash_val);
+#endif
+
+/*****************************************************************
+** "Internal" Skein definitions
+**    -- not needed for sequential hashing API, but will be
+**           helpful for other uses of Skein (e.g., tree hash mode).
+**    -- included here so that they can be shared between
+**           reference and optimized code.
+******************************************************************/
+
+/* tweak word tweak[1]: bit field starting positions */
+#define SKEIN_T1_BIT(BIT)       ((BIT) - 64)      /* second word  */
+
+#define SKEIN_T1_POS_TREE_LVL   SKEIN_T1_BIT(112) /* 112..118 hash tree level */
+#define SKEIN_T1_POS_BIT_PAD    SKEIN_T1_BIT(119) /* 119 part. final in byte */
+#define SKEIN_T1_POS_BLK_TYPE   SKEIN_T1_BIT(120) /* 120..125 type field `*/
+#define SKEIN_T1_POS_FIRST      SKEIN_T1_BIT(126) /* 126      first blk flag */
+#define SKEIN_T1_POS_FINAL      SKEIN_T1_BIT(127) /* 127      final blk flag */
+
+/* tweak word tweak[1]: flag bit definition(s) */
+#define SKEIN_T1_FLAG_FIRST     (((u64)  1) << SKEIN_T1_POS_FIRST)
+#define SKEIN_T1_FLAG_FINAL     (((u64)  1) << SKEIN_T1_POS_FINAL)
+#define SKEIN_T1_FLAG_BIT_PAD   (((u64)  1) << SKEIN_T1_POS_BIT_PAD)
+
+/* tweak word tweak[1]: tree level bit field mask */
+#define SKEIN_T1_TREE_LVL_MASK  (((u64)0x7F) << SKEIN_T1_POS_TREE_LVL)
+#define SKEIN_T1_TREE_LEVEL(n)  (((u64) (n)) << SKEIN_T1_POS_TREE_LVL)
+
+/* tweak word tweak[1]: block type field */
+#define SKEIN_BLK_TYPE_KEY       (0) /* key, for MAC and KDF */
+#define SKEIN_BLK_TYPE_CFG       (4) /* configuration block */
+#define SKEIN_BLK_TYPE_PERS      (8) /* personalization string */
+#define SKEIN_BLK_TYPE_PK       (12) /* pubkey (for digital sigs) */
+#define SKEIN_BLK_TYPE_KDF      (16) /* key identifier for KDF */
+#define SKEIN_BLK_TYPE_NONCE    (20) /* nonce for PRNG */
+#define SKEIN_BLK_TYPE_MSG      (48) /* message processing */
+#define SKEIN_BLK_TYPE_OUT      (63) /* output stage */
+#define SKEIN_BLK_TYPE_MASK     (63) /* bit field mask */
+
+#define SKEIN_T1_BLK_TYPE(T)   (((u64) (SKEIN_BLK_TYPE_##T)) << \
+					SKEIN_T1_POS_BLK_TYPE)
+#define SKEIN_T1_BLK_TYPE_KEY   SKEIN_T1_BLK_TYPE(KEY)  /* for MAC and KDF */
+#define SKEIN_T1_BLK_TYPE_CFG   SKEIN_T1_BLK_TYPE(CFG)  /* config block */
+#define SKEIN_T1_BLK_TYPE_PERS  SKEIN_T1_BLK_TYPE(PERS) /* personalization */
+#define SKEIN_T1_BLK_TYPE_PK    SKEIN_T1_BLK_TYPE(PK)   /* pubkey (for sigs) */
+#define SKEIN_T1_BLK_TYPE_KDF   SKEIN_T1_BLK_TYPE(KDF)  /* key ident for KDF */
+#define SKEIN_T1_BLK_TYPE_NONCE SKEIN_T1_BLK_TYPE(NONCE)/* nonce for PRNG */
+#define SKEIN_T1_BLK_TYPE_MSG   SKEIN_T1_BLK_TYPE(MSG)  /* message processing */
+#define SKEIN_T1_BLK_TYPE_OUT   SKEIN_T1_BLK_TYPE(OUT)  /* output stage */
+#define SKEIN_T1_BLK_TYPE_MASK  SKEIN_T1_BLK_TYPE(MASK) /* field bit mask */
+
+#define SKEIN_T1_BLK_TYPE_CFG_FINAL    (SKEIN_T1_BLK_TYPE_CFG | \
+					SKEIN_T1_FLAG_FINAL)
+#define SKEIN_T1_BLK_TYPE_OUT_FINAL    (SKEIN_T1_BLK_TYPE_OUT | \
+					SKEIN_T1_FLAG_FINAL)
+
+#define SKEIN_VERSION           (1)
+
+#ifndef SKEIN_ID_STRING_LE      /* allow compile-time personalization */
+#define SKEIN_ID_STRING_LE      (0x33414853) /* "SHA3" (little-endian)*/
+#endif
+
+#define SKEIN_MK_64(hi32, lo32)  ((lo32) + (((u64) (hi32)) << 32))
+#define SKEIN_SCHEMA_VER        SKEIN_MK_64(SKEIN_VERSION, SKEIN_ID_STRING_LE)
+#define SKEIN_KS_PARITY         SKEIN_MK_64(0x1BD11BDA, 0xA9FC1A22)
+
+#define SKEIN_CFG_STR_LEN       (4*8)
+
+/* bit field definitions in config block tree_info word */
+#define SKEIN_CFG_TREE_LEAF_SIZE_POS  (0)
+#define SKEIN_CFG_TREE_NODE_SIZE_POS  (8)
+#define SKEIN_CFG_TREE_MAX_LEVEL_POS  (16)
+
+#define SKEIN_CFG_TREE_LEAF_SIZE_MSK (((u64)0xFF) << \
+					SKEIN_CFG_TREE_LEAF_SIZE_POS)
+#define SKEIN_CFG_TREE_NODE_SIZE_MSK (((u64)0xFF) << \
+					SKEIN_CFG_TREE_NODE_SIZE_POS)
+#define SKEIN_CFG_TREE_MAX_LEVEL_MSK (((u64)0xFF) << \
+					SKEIN_CFG_TREE_MAX_LEVEL_POS)
+
+#define SKEIN_CFG_TREE_INFO(leaf, node, max_lvl)                   \
+	((((u64)(leaf))   << SKEIN_CFG_TREE_LEAF_SIZE_POS) |    \
+	 (((u64)(node))   << SKEIN_CFG_TREE_NODE_SIZE_POS) |    \
+	 (((u64)(max_lvl)) << SKEIN_CFG_TREE_MAX_LEVEL_POS))
+
+/* use as tree_info in InitExt() call for sequential processing */
+#define SKEIN_CFG_TREE_INFO_SEQUENTIAL SKEIN_CFG_TREE_INFO(0, 0, 0)
+
+/*
+**   Skein macros for getting/setting tweak words, etc.
+**   These are useful for partial input bytes, hash tree init/update, etc.
+**/
+#define skein_get_tweak(ctx_ptr, TWK_NUM)          ((ctx_ptr)->h.tweak[TWK_NUM])
+#define skein_set_tweak(ctx_ptr, TWK_NUM, t_val) { \
+		(ctx_ptr)->h.tweak[TWK_NUM] = (t_val); \
+	}
+
+#define skein_get_T0(ctx_ptr)     skein_get_tweak(ctx_ptr, 0)
+#define skein_get_T1(ctx_ptr)     skein_get_tweak(ctx_ptr, 1)
+#define skein_set_T0(ctx_ptr, T0) skein_set_tweak(ctx_ptr, 0, T0)
+#define skein_set_T1(ctx_ptr, T1) skein_set_tweak(ctx_ptr, 1, T1)
+
+/* set both tweak words at once */
+#define skein_set_T0_T1(ctx_ptr, T0, T1)           \
+	{                                          \
+	skein_set_T0(ctx_ptr, (T0));               \
+	skein_set_T1(ctx_ptr, (T1));               \
+	}
+
+#define skein_set_type(ctx_ptr, BLK_TYPE)         \
+	skein_set_T1(ctx_ptr, SKEIN_T1_BLK_TYPE_##BLK_TYPE)
+
+/*
+ * setup for starting with a new type:
+ * h.tweak[0]=0; h.tweak[1] = NEW_TYPE; h.b_cnt=0;
+ */
+#define skein_start_new_type(ctx_ptr, BLK_TYPE) { \
+		skein_set_T0_T1(ctx_ptr, 0, SKEIN_T1_FLAG_FIRST | \
+				SKEIN_T1_BLK_TYPE_##BLK_TYPE); \
+		(ctx_ptr)->h.b_cnt = 0; \
+	}
+
+#define skein_clear_first_flag(hdr) { \
+		(hdr).tweak[1] &= ~SKEIN_T1_FLAG_FIRST; \
+	}
+#define skein_set_bit_pad_flag(hdr) { \
+		(hdr).tweak[1] |=  SKEIN_T1_FLAG_BIT_PAD; \
+	}
+
+#define skein_set_tree_level(hdr, height) { \
+		(hdr).tweak[1] |= SKEIN_T1_TREE_LEVEL(height); \
+	}
+
+/*****************************************************************
+** "Internal" Skein definitions for debugging and error checking
+******************************************************************/
+#ifdef SKEIN_DEBUG             /* examine/display intermediate values? */
+#include "skein_debug.h"
+#else                           /* default is no callouts */
+#define skein_show_block(bits, ctx, x, blk_ptr, w_ptr, ks_event_ptr, ks_odd_ptr)
+#define skein_show_round(bits, ctx, r, x)
+#define skein_show_r_ptr(bits, ctx, r, x_ptr)
+#define skein_show_final(bits, ctx, cnt, out_ptr)
+#define skein_show_key(bits, ctx, key, key_bytes)
+#endif
+
+/* ignore all asserts, for performance */
+#define skein_assert_ret(x, ret_code)
+#define skein_assert(x)
+
+/*****************************************************************
+** Skein block function constants (shared across Ref and Opt code)
+******************************************************************/
+enum {
+	    /* SKEIN_256 round rotation constants */
+	R_256_0_0 = 14, R_256_0_1 = 16,
+	R_256_1_0 = 52, R_256_1_1 = 57,
+	R_256_2_0 = 23, R_256_2_1 = 40,
+	R_256_3_0 =  5, R_256_3_1 = 37,
+	R_256_4_0 = 25, R_256_4_1 = 33,
+	R_256_5_0 = 46, R_256_5_1 = 12,
+	R_256_6_0 = 58, R_256_6_1 = 22,
+	R_256_7_0 = 32, R_256_7_1 = 32,
+
+	    /* SKEIN_512 round rotation constants */
+	R_512_0_0 = 46, R_512_0_1 = 36, R_512_0_2 = 19, R_512_0_3 = 37,
+	R_512_1_0 = 33, R_512_1_1 = 27, R_512_1_2 = 14, R_512_1_3 = 42,
+	R_512_2_0 = 17, R_512_2_1 = 49, R_512_2_2 = 36, R_512_2_3 = 39,
+	R_512_3_0 = 44, R_512_3_1 =  9, R_512_3_2 = 54, R_512_3_3 = 56,
+	R_512_4_0 = 39, R_512_4_1 = 30, R_512_4_2 = 34, R_512_4_3 = 24,
+	R_512_5_0 = 13, R_512_5_1 = 50, R_512_5_2 = 10, R_512_5_3 = 17,
+	R_512_6_0 = 25, R_512_6_1 = 29, R_512_6_2 = 39, R_512_6_3 = 43,
+	R_512_7_0 =  8, R_512_7_1 = 35, R_512_7_2 = 56, R_512_7_3 = 22,
+
+	    /* SKEIN_1024 round rotation constants */
+	R1024_0_0 = 24, R1024_0_1 = 13, R1024_0_2 =  8, R1024_0_3 = 47,
+	R1024_0_4 =  8, R1024_0_5 = 17, R1024_0_6 = 22, R1024_0_7 = 37,
+	R1024_1_0 = 38, R1024_1_1 = 19, R1024_1_2 = 10, R1024_1_3 = 55,
+	R1024_1_4 = 49, R1024_1_5 = 18, R1024_1_6 = 23, R1024_1_7 = 52,
+	R1024_2_0 = 33, R1024_2_1 =  4, R1024_2_2 = 51, R1024_2_3 = 13,
+	R1024_2_4 = 34, R1024_2_5 = 41, R1024_2_6 = 59, R1024_2_7 = 17,
+	R1024_3_0 =  5, R1024_3_1 = 20, R1024_3_2 = 48, R1024_3_3 = 41,
+	R1024_3_4 = 47, R1024_3_5 = 28, R1024_3_6 = 16, R1024_3_7 = 25,
+	R1024_4_0 = 41, R1024_4_1 =  9, R1024_4_2 = 37, R1024_4_3 = 31,
+	R1024_4_4 = 12, R1024_4_5 = 47, R1024_4_6 = 44, R1024_4_7 = 30,
+	R1024_5_0 = 16, R1024_5_1 = 34, R1024_5_2 = 56, R1024_5_3 = 51,
+	R1024_5_4 =  4, R1024_5_5 = 53, R1024_5_6 = 42, R1024_5_7 = 41,
+	R1024_6_0 = 31, R1024_6_1 = 44, R1024_6_2 = 47, R1024_6_3 = 46,
+	R1024_6_4 = 19, R1024_6_5 = 42, R1024_6_6 = 44, R1024_6_7 = 25,
+	R1024_7_0 =  9, R1024_7_1 = 48, R1024_7_2 = 35, R1024_7_3 = 52,
+	R1024_7_4 = 23, R1024_7_5 = 31, R1024_7_6 = 37, R1024_7_7 = 20
+};
+
+#ifndef SKEIN_ROUNDS
+#define SKEIN_256_ROUNDS_TOTAL (72)	/* # rounds for diff block sizes */
+#define SKEIN_512_ROUNDS_TOTAL (72)
+#define SKEIN_1024_ROUNDS_TOTAL (80)
+#else			/* allow command-line define in range 8*(5..14)   */
+#define SKEIN_256_ROUNDS_TOTAL (8*((((SKEIN_ROUNDS/100) + 5) % 10) + 5))
+#define SKEIN_512_ROUNDS_TOTAL (8*((((SKEIN_ROUNDS/10)  + 5) % 10) + 5))
+#define SKEIN_1024_ROUNDS_TOTAL (8*((((SKEIN_ROUNDS)     + 5) % 10) + 5))
+#endif
+
+#endif  /* ifndef _SKEIN_H_ */
diff --git a/drivers/staging/skein/skein_api.c b/drivers/staging/skein/skein_api.c
new file mode 100644
index 00000000000..6e700eefc00
--- /dev/null
+++ b/drivers/staging/skein/skein_api.c
@@ -0,0 +1,239 @@
+/*
+Copyright (c) 2010 Werner Dittmann
+
+Permission is hereby granted, free of charge, to any person
+obtaining a copy of this software and associated documentation
+files (the "Software"), to deal in the Software without
+restriction, including without limitation the rights to use,
+copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the
+Software is furnished to do so, subject to the following
+conditions:
+
+The above copyright notice and this permission notice shall be
+included in all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+OTHER DEALINGS IN THE SOFTWARE.
+
+*/
+
+#include <linux/string.h>
+#include "skein_api.h"
+
+int skein_ctx_prepare(struct skein_ctx *ctx, enum skein_size size)
+{
+	skein_assert_ret(ctx && size, SKEIN_FAIL);
+
+	memset(ctx , 0, sizeof(struct skein_ctx));
+	ctx->skein_size = size;
+
+	return SKEIN_SUCCESS;
+}
+
+int skein_init(struct skein_ctx *ctx, size_t hash_bit_len)
+{
+	int ret = SKEIN_FAIL;
+	size_t x_len = 0;
+	u64 *x = NULL;
+	u64 tree_info = SKEIN_CFG_TREE_INFO_SEQUENTIAL;
+
+	skein_assert_ret(ctx, SKEIN_FAIL);
+	/*
+	 * The following two lines rely of the fact that the real Skein
+	 * contexts are a union in out context and thus have tha maximum
+	 * memory available.  The beauty of C :-) .
+	 */
+	x = ctx->m.s256.x;
+	x_len = ctx->skein_size/8;
+	/*
+	 * If size is the same and hash bit length is zero then reuse
+	 * the save chaining variables.
+	 */
+	switch (ctx->skein_size) {
+	case SKEIN_256:
+		ret = skein_256_init_ext(&ctx->m.s256, hash_bit_len,
+					 tree_info, NULL, 0);
+		break;
+	case SKEIN_512:
+		ret = skein_512_init_ext(&ctx->m.s512, hash_bit_len,
+					 tree_info, NULL, 0);
+		break;
+	case SKEIN_1024:
+		ret = skein_1024_init_ext(&ctx->m.s1024, hash_bit_len,
+					  tree_info, NULL, 0);
+		break;
+	}
+
+	if (ret == SKEIN_SUCCESS) {
+		/*
+		 * Save chaining variables for this combination of size and
+		 * hash_bit_len
+		 */
+		memcpy(ctx->x_save, x, x_len);
+	}
+	return ret;
+}
+
+int skein_mac_init(struct skein_ctx *ctx, const u8 *key, size_t key_len,
+		   size_t hash_bit_len)
+{
+	int ret = SKEIN_FAIL;
+	u64 *x = NULL;
+	size_t x_len = 0;
+	u64 tree_info = SKEIN_CFG_TREE_INFO_SEQUENTIAL;
+
+	skein_assert_ret(ctx, SKEIN_FAIL);
+
+	x = ctx->m.s256.x;
+	x_len = ctx->skein_size/8;
+
+	skein_assert_ret(hash_bit_len, SKEIN_BAD_HASHLEN);
+
+	switch (ctx->skein_size) {
+	case SKEIN_256:
+		ret = skein_256_init_ext(&ctx->m.s256, hash_bit_len,
+					 tree_info,
+					 (const u8 *)key, key_len);
+
+		break;
+	case SKEIN_512:
+		ret = skein_512_init_ext(&ctx->m.s512, hash_bit_len,
+					 tree_info,
+					 (const u8 *)key, key_len);
+		break;
+	case SKEIN_1024:
+		ret = skein_1024_init_ext(&ctx->m.s1024, hash_bit_len,
+					  tree_info,
+					  (const u8 *)key, key_len);
+
+		break;
+	}
+	if (ret == SKEIN_SUCCESS) {
+		/*
+		 * Save chaining variables for this combination of key,
+		 * key_len, hash_bit_len
+		 */
+		memcpy(ctx->x_save, x, x_len);
+	}
+	return ret;
+}
+
+void skein_reset(struct skein_ctx *ctx)
+{
+	size_t x_len = 0;
+	u64 *x = NULL;
+
+	/*
+	 * The following two lines rely of the fact that the real Skein
+	 * contexts are a union in out context and thus have tha maximum
+	 * memory available.  The beautiy of C :-) .
+	 */
+	x = ctx->m.s256.x;
+	x_len = ctx->skein_size/8;
+	/* Restore the chaing variable, reset byte counter */
+	memcpy(x, ctx->x_save, x_len);
+
+	/* Setup context to process the message */
+	skein_start_new_type(&ctx->m, MSG);
+}
+
+int skein_update(struct skein_ctx *ctx, const u8 *msg,
+		 size_t msg_byte_cnt)
+{
+	int ret = SKEIN_FAIL;
+
+	skein_assert_ret(ctx, SKEIN_FAIL);
+
+	switch (ctx->skein_size) {
+	case SKEIN_256:
+		ret = skein_256_update(&ctx->m.s256, (const u8 *)msg,
+				       msg_byte_cnt);
+		break;
+	case SKEIN_512:
+		ret = skein_512_update(&ctx->m.s512, (const u8 *)msg,
+				       msg_byte_cnt);
+		break;
+	case SKEIN_1024:
+		ret = skein_1024_update(&ctx->m.s1024, (const u8 *)msg,
+					msg_byte_cnt);
+		break;
+	}
+	return ret;
+
+}
+
+int skein_update_bits(struct skein_ctx *ctx, const u8 *msg,
+		      size_t msg_bit_cnt)
+{
+	/*
+	 * I've used the bit pad implementation from skein_test.c (see NIST CD)
+	 * and modified it to use the convenience functions and added some
+	 * pointer arithmetic.
+	 */
+	size_t length;
+	u8 mask;
+	u8 *up;
+
+	/*
+	 * only the final Update() call is allowed do partial bytes, else
+	 * assert an error
+	 */
+	skein_assert_ret((ctx->m.h.T[1] & SKEIN_T1_FLAG_BIT_PAD) == 0 ||
+			 msg_bit_cnt == 0, SKEIN_FAIL);
+
+	/* if number of bits is a multiple of bytes - that's easy */
+	if ((msg_bit_cnt & 0x7) == 0)
+		return skein_update(ctx, msg, msg_bit_cnt >> 3);
+
+	skein_update(ctx, msg, (msg_bit_cnt >> 3) + 1);
+
+	/*
+	 * The next line rely on the fact that the real Skein contexts
+	 * are a union in our context. After the addition the pointer points to
+	 * Skein's real partial block buffer.
+	 * If this layout ever changes we have to adapt this as well.
+	 */
+	up = (u8 *)ctx->m.s256.x + ctx->skein_size / 8;
+
+	/* set tweak flag for the skein_final call */
+	skein_set_bit_pad_flag(ctx->m.h);
+
+	/* now "pad" the final partial byte the way NIST likes */
+	/* get the b_cnt value (same location for all block sizes) */
+	length = ctx->m.h.b_cnt;
+	/* internal sanity check: there IS a partial byte in the buffer! */
+	skein_assert(length != 0);
+	/* partial byte bit mask */
+	mask = (u8) (1u << (7 - (msg_bit_cnt & 7)));
+	/* apply bit padding on final byte (in the buffer) */
+	up[length-1]  = (u8)((up[length-1] & (0-mask))|mask);
+
+	return SKEIN_SUCCESS;
+}
+
+int skein_final(struct skein_ctx *ctx, u8 *hash)
+{
+	int ret = SKEIN_FAIL;
+
+	skein_assert_ret(ctx, SKEIN_FAIL);
+
+	switch (ctx->skein_size) {
+	case SKEIN_256:
+		ret = skein_256_final(&ctx->m.s256, (u8 *)hash);
+		break;
+	case SKEIN_512:
+		ret = skein_512_final(&ctx->m.s512, (u8 *)hash);
+		break;
+	case SKEIN_1024:
+		ret = skein_1024_final(&ctx->m.s1024, (u8 *)hash);
+		break;
+	}
+	return ret;
+}
diff --git a/drivers/staging/skein/skein_api.h b/drivers/staging/skein/skein_api.h
new file mode 100644
index 00000000000..e02fa19d945
--- /dev/null
+++ b/drivers/staging/skein/skein_api.h
@@ -0,0 +1,230 @@
+/*
+Copyright (c) 2010 Werner Dittmann
+
+Permission is hereby granted, free of charge, to any person
+obtaining a copy of this software and associated documentation
+files (the "Software"), to deal in the Software without
+restriction, including without limitation the rights to use,
+copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the
+Software is furnished to do so, subject to the following
+conditions:
+
+The above copyright notice and this permission notice shall be
+included in all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+NONINFRINGEMENT. IN NO EVENT SHALL THE 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 SKEINAPI_H
+#define SKEINAPI_H
+
+/**
+ * @file skein_api.h
+ * @brief A Skein API and its functions.
+ * @{
+ *
+ * This API and the functions that implement this API simplify the usage
+ * of Skein. The design and the way to use the functions follow the openSSL
+ * design but at the same time take care of some Skein specific behaviour
+ * and possibilities.
+ *
+ * The functions enable applications to create a normal Skein hashes and
+ * message authentication codes (MAC).
+ *
+ * Using these functions is simple and straight forward:
+ *
+ * @code
+ *
+ * #include "skein_api.h"
+ *
+ * ...
+ * struct skein_ctx ctx;             // a Skein hash or MAC context
+ *
+ * // prepare context, here for a Skein with a state size of 512 bits.
+ * skein_ctx_prepare(&ctx, SKEIN_512);
+ *
+ * // Initialize the context to set the requested hash length in bits
+ * // here request a output hash size of 31 bits (Skein supports variable
+ * // output sizes even very strange sizes)
+ * skein_init(&ctx, 31);
+ *
+ * // Now update Skein with any number of message bits. A function that
+ * // takes a number of bytes is also available.
+ * skein_update_bits(&ctx, message, msg_length);
+ *
+ * // Now get the result of the Skein hash. The output buffer must be
+ * // large enough to hold the request number of output bits. The application
+ * // may now extract the bits.
+ * skein_final(&ctx, result);
+ * ...
+ * @endcode
+ *
+ * An application may use @c skein_reset to reset a Skein context and use
+ * it for creation of another hash with the same Skein state size and output
+ * bit length. In this case the API implementation restores some internal
+ * internal state data and saves a full Skein initialization round.
+ *
+ * To create a MAC the application just uses @c skein_mac_init instead of
+ * @c skein_init. All other functions calls remain the same.
+ *
+ */
+
+#include <linux/types.h>
+#include "skein.h"
+
+/**
+ * Which Skein size to use
+ */
+enum skein_size {
+	SKEIN_256 = 256,     /*!< Skein with 256 bit state */
+	SKEIN_512 = 512,     /*!< Skein with 512 bit state */
+	SKEIN_1024 = 1024    /*!< Skein with 1024 bit state */
+};
+
+/**
+ * Context for Skein.
+ *
+ * This structure was setup with some know-how of the internal
+ * Skein structures, in particular ordering of header and size dependent
+ * variables. If Skein implementation changes this, then adapt these
+ * structures as well.
+ */
+struct skein_ctx {
+	u64 skein_size;
+	u64 x_save[SKEIN_MAX_STATE_WORDS];   /* save area for state variables */
+	union {
+		struct skein_ctx_hdr h;
+		struct skein_256_ctx s256;
+		struct skein_512_ctx s512;
+		struct skein_1024_ctx s1024;
+	} m;
+};
+
+/**
+ * Prepare a Skein context.
+ *
+ * An application must call this function before it can use the Skein
+ * context. The functions clears memory and initializes size dependent
+ * variables.
+ *
+ * @param ctx
+ *     Pointer to a Skein context.
+ * @param size
+ *     Which Skein size to use.
+ * @return
+ *     SKEIN_SUCESS of SKEIN_FAIL
+ */
+int skein_ctx_prepare(struct skein_ctx *ctx, enum skein_size size);
+
+/**
+ * Initialize a Skein context.
+ *
+ * Initializes the context with this data and saves the resulting Skein
+ * state variables for further use.
+ *
+ * @param ctx
+ *     Pointer to a Skein context.
+ * @param hash_bit_len
+ *     Number of MAC hash bits to compute
+ * @return
+ *     SKEIN_SUCESS of SKEIN_FAIL
+ * @see skein_reset
+ */
+int skein_init(struct skein_ctx *ctx, size_t hash_bit_len);
+
+/**
+ * Resets a Skein context for further use.
+ *
+ * Restores the saved chaining variables to reset the Skein context.
+ * Thus applications can reuse the same setup to  process several
+ * messages. This saves a complete Skein initialization cycle.
+ *
+ * @param ctx
+ *     Pointer to a pre-initialized Skein MAC context
+ */
+void skein_reset(struct skein_ctx *ctx);
+
+/**
+ * Initializes a Skein context for MAC usage.
+ *
+ * Initializes the context with this data and saves the resulting Skein
+ * state variables for further use.
+ *
+ * Applications call the normal Skein functions to update the MAC and
+ * get the final result.
+ *
+ * @param ctx
+ *     Pointer to an empty or preinitialized Skein MAC context
+ * @param key
+ *     Pointer to key bytes or NULL
+ * @param key_len
+ *     Length of the key in bytes or zero
+ * @param hash_bit_len
+ *     Number of MAC hash bits to compute
+ * @return
+ *     SKEIN_SUCESS of SKEIN_FAIL
+ */
+int skein_mac_init(struct skein_ctx *ctx, const u8 *key, size_t key_len,
+		   size_t hash_bit_len);
+
+/**
+ * Update Skein with the next part of the message.
+ *
+ * @param ctx
+ *     Pointer to initialized Skein context
+ * @param msg
+ *     Pointer to the message.
+ * @param msg_byte_cnt
+ *     Length of the message in @b bytes
+ * @return
+ *     Success or error code.
+ */
+int skein_update(struct skein_ctx *ctx, const u8 *msg,
+		 size_t msg_byte_cnt);
+
+/**
+ * Update the hash with a message bit string.
+ *
+ * Skein can handle data not only as bytes but also as bit strings of
+ * arbitrary length (up to its maximum design size).
+ *
+ * @param ctx
+ *     Pointer to initialized Skein context
+ * @param msg
+ *     Pointer to the message.
+ * @param msg_bit_cnt
+ *     Length of the message in @b bits.
+ */
+int skein_update_bits(struct skein_ctx *ctx, const u8 *msg,
+		      size_t msg_bit_cnt);
+
+/**
+ * Finalize Skein and return the hash.
+ *
+ * Before an application can reuse a Skein setup the application must
+ * reset the Skein context.
+ *
+ * @param ctx
+ *     Pointer to initialized Skein context
+ * @param hash
+ *     Pointer to buffer that receives the hash. The buffer must be large
+ *     enough to store @c hash_bit_len bits.
+ * @return
+ *     Success or error code.
+ * @see skein_reset
+ */
+int skein_final(struct skein_ctx *ctx, u8 *hash);
+
+/**
+ * @}
+ */
+#endif
diff --git a/drivers/staging/skein/skein_block.c b/drivers/staging/skein/skein_block.c
new file mode 100644
index 00000000000..04ce1d00547
--- /dev/null
+++ b/drivers/staging/skein/skein_block.c
@@ -0,0 +1,777 @@
+/***********************************************************************
+**
+** Implementation of the Skein block functions.
+**
+** Source code author: Doug Whiting, 2008.
+**
+** This algorithm and source code is released to the public domain.
+**
+** Compile-time switches:
+**
+**  SKEIN_USE_ASM  -- set bits (256/512/1024) to select which
+**                    versions use ASM code for block processing
+**                    [default: use C for all block sizes]
+**
+************************************************************************/
+
+#include <linux/string.h>
+#include "skein.h"
+#include "skein_block.h"
+
+#ifndef SKEIN_USE_ASM
+#define SKEIN_USE_ASM   (0) /* default is all C code (no ASM) */
+#endif
+
+#ifndef SKEIN_LOOP
+#define SKEIN_LOOP 001 /* default: unroll 256 and 512, but not 1024 */
+#endif
+
+#define BLK_BITS        (WCNT*64) /* some useful definitions for code here */
+#define KW_TWK_BASE     (0)
+#define KW_KEY_BASE     (3)
+#define ks              (kw + KW_KEY_BASE)
+#define ts              (kw + KW_TWK_BASE)
+
+#ifdef SKEIN_DEBUG
+#define debug_save_tweak(ctx) { \
+                        ctx->h.tweak[0] = ts[0]; ctx->h.tweak[1] = ts[1]; }
+#else
+#define debug_save_tweak(ctx)
+#endif
+
+/*****************************  SKEIN_256 ******************************/
+#if !(SKEIN_USE_ASM & 256)
+void skein_256_process_block(struct skein_256_ctx *ctx, const u8 *blk_ptr,
+			     size_t blk_cnt, size_t byte_cnt_add)
+	{ /* do it in C */
+	enum {
+		WCNT = SKEIN_256_STATE_WORDS
+	};
+#undef  RCNT
+#define RCNT  (SKEIN_256_ROUNDS_TOTAL/8)
+
+#ifdef SKEIN_LOOP /* configure how much to unroll the loop */
+#define SKEIN_UNROLL_256 (((SKEIN_LOOP)/100)%10)
+#else
+#define SKEIN_UNROLL_256 (0)
+#endif
+
+#if SKEIN_UNROLL_256
+#if (RCNT % SKEIN_UNROLL_256)
+#error "Invalid SKEIN_UNROLL_256" /* sanity check on unroll count */
+#endif
+	size_t  r;
+	u64  kw[WCNT+4+RCNT*2]; /* key schedule: chaining vars + tweak + "rot"*/
+#else
+	u64  kw[WCNT+4]; /* key schedule words : chaining vars + tweak */
+#endif
+	u64  X0, X1, X2, X3; /* local copy of context vars, for speed */
+	u64  w[WCNT]; /* local copy of input block */
+#ifdef SKEIN_DEBUG
+	const u64 *X_ptr[4]; /* use for debugging (help cc put Xn in regs) */
+
+	X_ptr[0] = &X0;  X_ptr[1] = &X1;  X_ptr[2] = &X2;  X_ptr[3] = &X3;
+#endif
+	skein_assert(blk_cnt != 0); /* never call with blk_cnt == 0! */
+	ts[0] = ctx->h.tweak[0];
+	ts[1] = ctx->h.tweak[1];
+	do  {
+		/*
+		 * this implementation only supports 2**64 input bytes
+		 * (no carry out here)
+		 */
+		ts[0] += byte_cnt_add; /* update processed length */
+
+		/* precompute the key schedule for this block */
+		ks[0] = ctx->x[0];
+		ks[1] = ctx->x[1];
+		ks[2] = ctx->x[2];
+		ks[3] = ctx->x[3];
+		ks[4] = ks[0] ^ ks[1] ^ ks[2] ^ ks[3] ^ SKEIN_KS_PARITY;
+
+		ts[2] = ts[0] ^ ts[1];
+
+		/* get input block in little-endian format */
+		skein_get64_lsb_first(w, blk_ptr, WCNT);
+		debug_save_tweak(ctx);
+		skein_show_block(BLK_BITS, &ctx->h, ctx->x, blk_ptr, w, ks, ts);
+
+		X0 = w[0] + ks[0]; /* do the first full key injection */
+		X1 = w[1] + ks[1] + ts[0];
+		X2 = w[2] + ks[2] + ts[1];
+		X3 = w[3] + ks[3];
+
+		/* show starting state values */
+		skein_show_r_ptr(BLK_BITS, &ctx->h, SKEIN_RND_KEY_INITIAL,
+				 x_ptr);
+
+		blk_ptr += SKEIN_256_BLOCK_BYTES;
+
+		/* run the rounds */
+
+#define ROUND256(p0, p1, p2, p3, ROT, r_num)                              \
+do { \
+	X##p0 += X##p1; X##p1 = rotl_64(X##p1, ROT##_0); X##p1 ^= X##p0; \
+	X##p2 += X##p3; X##p3 = rotl_64(X##p3, ROT##_1); X##p3 ^= X##p2; \
+} while (0)
+
+#if SKEIN_UNROLL_256 == 0
+#define R256(p0, p1, p2, p3, ROT, r_num) /* fully unrolled */ \
+do { \
+	ROUND256(p0, p1, p2, p3, ROT, r_num); \
+	skein_show_r_ptr(BLK_BITS, &ctx->h, r_num, X_ptr); \
+} while (0)
+
+#define I256(R) \
+do { \
+	/* inject the key schedule value */ \
+	X0   += ks[((R)+1) % 5]; \
+	X1   += ks[((R)+2) % 5] + ts[((R)+1) % 3]; \
+	X2   += ks[((R)+3) % 5] + ts[((R)+2) % 3]; \
+	X3   += ks[((R)+4) % 5] +     (R)+1;       \
+	skein_show_r_ptr(BLK_BITS, &ctx->h, SKEIN_RND_KEY_INJECT, X_ptr); \
+} while (0)
+#else /* looping version */
+#define R256(p0, p1, p2, p3, ROT, r_num) \
+do { \
+	ROUND256(p0, p1, p2, p3, ROT, r_num); \
+	skein_show_r_ptr(BLK_BITS, &ctx->h, 4 * (r - 1) + r_num, X_ptr); \
+} while (0)
+
+#define I256(R) \
+do { \
+	/* inject the key schedule value */ \
+	X0   += ks[r+(R)+0]; \
+	X1   += ks[r+(R)+1] + ts[r+(R)+0]; \
+	X2   += ks[r+(R)+2] + ts[r+(R)+1]; \
+	X3   += ks[r+(R)+3] +    r+(R);    \
+	/* rotate key schedule */ \
+	ks[r + (R) + 4]   = ks[r + (R) - 1]; \
+	ts[r + (R) + 2]   = ts[r + (R) - 1]; \
+	skein_show_r_ptr(BLK_BITS, &ctx->h, SKEIN_RND_KEY_INJECT, X_ptr); \
+} while (0)
+
+	for (r = 1; r < 2 * RCNT; r += 2 * SKEIN_UNROLL_256)
+#endif
+		{
+#define R256_8_ROUNDS(R)                  \
+do { \
+		R256(0, 1, 2, 3, R_256_0, 8 * (R) + 1);  \
+		R256(0, 3, 2, 1, R_256_1, 8 * (R) + 2);  \
+		R256(0, 1, 2, 3, R_256_2, 8 * (R) + 3);  \
+		R256(0, 3, 2, 1, R_256_3, 8 * (R) + 4);  \
+		I256(2 * (R));                      \
+		R256(0, 1, 2, 3, R_256_4, 8 * (R) + 5);  \
+		R256(0, 3, 2, 1, R_256_5, 8 * (R) + 6);  \
+		R256(0, 1, 2, 3, R_256_6, 8 * (R) + 7);  \
+		R256(0, 3, 2, 1, R_256_7, 8 * (R) + 8);  \
+		I256(2 * (R) + 1); \
+} while (0)
+
+		R256_8_ROUNDS(0);
+
+#define R256_UNROLL_R(NN) \
+	((SKEIN_UNROLL_256 == 0 && \
+	  SKEIN_256_ROUNDS_TOTAL/8 > (NN)) || \
+	 (SKEIN_UNROLL_256 > (NN)))
+
+	#if   R256_UNROLL_R(1)
+		R256_8_ROUNDS(1);
+	#endif
+	#if   R256_UNROLL_R(2)
+		R256_8_ROUNDS(2);
+	#endif
+	#if   R256_UNROLL_R(3)
+		R256_8_ROUNDS(3);
+	#endif
+	#if   R256_UNROLL_R(4)
+		R256_8_ROUNDS(4);
+	#endif
+	#if   R256_UNROLL_R(5)
+		R256_8_ROUNDS(5);
+	#endif
+	#if   R256_UNROLL_R(6)
+		R256_8_ROUNDS(6);
+	#endif
+	#if   R256_UNROLL_R(7)
+		R256_8_ROUNDS(7);
+	#endif
+	#if   R256_UNROLL_R(8)
+		R256_8_ROUNDS(8);
+	#endif
+	#if   R256_UNROLL_R(9)
+		R256_8_ROUNDS(9);
+	#endif
+	#if   R256_UNROLL_R(10)
+		R256_8_ROUNDS(10);
+	#endif
+	#if   R256_UNROLL_R(11)
+		R256_8_ROUNDS(11);
+	#endif
+	#if   R256_UNROLL_R(12)
+		R256_8_ROUNDS(12);
+	#endif
+	#if   R256_UNROLL_R(13)
+		R256_8_ROUNDS(13);
+	#endif
+	#if   R256_UNROLL_R(14)
+		R256_8_ROUNDS(14);
+	#endif
+	#if  (SKEIN_UNROLL_256 > 14)
+#error  "need more unrolling in skein_256_process_block"
+	#endif
+		}
+		/* do the final "feedforward" xor, update context chaining */
+		ctx->x[0] = X0 ^ w[0];
+		ctx->x[1] = X1 ^ w[1];
+		ctx->x[2] = X2 ^ w[2];
+		ctx->x[3] = X3 ^ w[3];
+
+		skein_show_round(BLK_BITS, &ctx->h, SKEIN_RND_FEED_FWD, ctx->x);
+
+		ts[1] &= ~SKEIN_T1_FLAG_FIRST;
+	} while (--blk_cnt);
+	ctx->h.tweak[0] = ts[0];
+	ctx->h.tweak[1] = ts[1];
+}
+
+#if defined(SKEIN_CODE_SIZE) || defined(SKEIN_PERF)
+size_t skein_256_process_block_code_size(void)
+{
+	return ((u8 *) skein_256_process_block_code_size) -
+		((u8 *) skein_256_process_block);
+}
+unsigned int skein_256_unroll_cnt(void)
+{
+	return SKEIN_UNROLL_256;
+}
+#endif
+#endif
+
+/*****************************  SKEIN_512 ******************************/
+#if !(SKEIN_USE_ASM & 512)
+void skein_512_process_block(struct skein_512_ctx *ctx, const u8 *blk_ptr,
+			     size_t blk_cnt, size_t byte_cnt_add)
+{ /* do it in C */
+	enum {
+		WCNT = SKEIN_512_STATE_WORDS
+	};
+#undef  RCNT
+#define RCNT  (SKEIN_512_ROUNDS_TOTAL/8)
+
+#ifdef SKEIN_LOOP /* configure how much to unroll the loop */
+#define SKEIN_UNROLL_512 (((SKEIN_LOOP)/10)%10)
+#else
+#define SKEIN_UNROLL_512 (0)
+#endif
+
+#if SKEIN_UNROLL_512
+#if (RCNT % SKEIN_UNROLL_512)
+#error "Invalid SKEIN_UNROLL_512" /* sanity check on unroll count */
+#endif
+	size_t  r;
+	u64  kw[WCNT+4+RCNT*2]; /* key sched: chaining vars + tweak + "rot"*/
+#else
+	u64  kw[WCNT+4]; /* key schedule words : chaining vars + tweak */
+#endif
+	u64  X0, X1, X2, X3, X4, X5, X6, X7; /* local copies, for speed */
+	u64  w[WCNT]; /* local copy of input block */
+#ifdef SKEIN_DEBUG
+	const u64 *X_ptr[8]; /* use for debugging (help cc put Xn in regs) */
+
+	X_ptr[0] = &X0;  X_ptr[1] = &X1;  X_ptr[2] = &X2;  X_ptr[3] = &X3;
+	X_ptr[4] = &X4;  X_ptr[5] = &X5;  X_ptr[6] = &X6;  X_ptr[7] = &X7;
+#endif
+
+	skein_assert(blk_cnt != 0); /* never call with blk_cnt == 0! */
+	ts[0] = ctx->h.tweak[0];
+	ts[1] = ctx->h.tweak[1];
+	do  {
+		/*
+		 * this implementation only supports 2**64 input bytes
+		 * (no carry out here)
+		 */
+		ts[0] += byte_cnt_add; /* update processed length */
+
+		/* precompute the key schedule for this block */
+		ks[0] = ctx->x[0];
+		ks[1] = ctx->x[1];
+		ks[2] = ctx->x[2];
+		ks[3] = ctx->x[3];
+		ks[4] = ctx->x[4];
+		ks[5] = ctx->x[5];
+		ks[6] = ctx->x[6];
+		ks[7] = ctx->x[7];
+		ks[8] = ks[0] ^ ks[1] ^ ks[2] ^ ks[3] ^
+			ks[4] ^ ks[5] ^ ks[6] ^ ks[7] ^ SKEIN_KS_PARITY;
+
+		ts[2] = ts[0] ^ ts[1];
+
+		/* get input block in little-endian format */
+		skein_get64_lsb_first(w, blk_ptr, WCNT);
+		debug_save_tweak(ctx);
+		skein_show_block(BLK_BITS, &ctx->h, ctx->x, blk_ptr, w, ks, ts);
+
+		X0   = w[0] + ks[0]; /* do the first full key injection */
+		X1   = w[1] + ks[1];
+		X2   = w[2] + ks[2];
+		X3   = w[3] + ks[3];
+		X4   = w[4] + ks[4];
+		X5   = w[5] + ks[5] + ts[0];
+		X6   = w[6] + ks[6] + ts[1];
+		X7   = w[7] + ks[7];
+
+		blk_ptr += SKEIN_512_BLOCK_BYTES;
+
+		skein_show_r_ptr(BLK_BITS, &ctx->h, SKEIN_RND_KEY_INITIAL,
+				 X_ptr);
+		/* run the rounds */
+#define ROUND512(p0, p1, p2, p3, p4, p5, p6, p7, ROT, r_num) \
+do { \
+	X##p0 += X##p1; X##p1 = rotl_64(X##p1, ROT##_0); X##p1 ^= X##p0; \
+	X##p2 += X##p3; X##p3 = rotl_64(X##p3, ROT##_1); X##p3 ^= X##p2; \
+	X##p4 += X##p5; X##p5 = rotl_64(X##p5, ROT##_2); X##p5 ^= X##p4; \
+	X##p6 += X##p7; X##p7 = rotl_64(X##p7, ROT##_3); X##p7 ^= X##p6; \
+} while (0)
+
+#if SKEIN_UNROLL_512 == 0
+#define R512(p0, p1, p2, p3, p4, p5, p6, p7, ROT, r_num) /* unrolled */ \
+do { \
+	ROUND512(p0, p1, p2, p3, p4, p5, p6, p7, ROT, r_num) \
+	skein_show_r_ptr(BLK_BITS, &ctx->h, r_num, X_ptr); \
+} while (0)
+
+#define I512(R) \
+do { \
+	/* inject the key schedule value */ \
+	X0   += ks[((R) + 1) % 9]; \
+	X1   += ks[((R) + 2) % 9]; \
+	X2   += ks[((R) + 3) % 9]; \
+	X3   += ks[((R) + 4) % 9]; \
+	X4   += ks[((R) + 5) % 9]; \
+	X5   += ks[((R) + 6) % 9] + ts[((R) + 1) % 3]; \
+	X6   += ks[((R) + 7) % 9] + ts[((R) + 2) % 3]; \
+	X7   += ks[((R) + 8) % 9] +     (R) + 1;       \
+	skein_show_r_ptr(BLK_BITS, &ctx->h, SKEIN_RND_KEY_INJECT, X_ptr); \
+} while (0)
+#else /* looping version */
+#define R512(p0, p1, p2, p3, p4, p5, p6, p7, ROT, r_num) \
+do { \
+	ROUND512(p0, p1, p2, p3, p4, p5, p6, p7, ROT, r_num); \
+	skein_show_r_ptr(BLK_BITS, &ctx->h, 4 * (r - 1) + r_num, X_ptr); \
+} while (0)
+
+#define I512(R) \
+do { \
+	/* inject the key schedule value */ \
+	X0   += ks[r + (R) + 0]; \
+	X1   += ks[r + (R) + 1]; \
+	X2   += ks[r + (R) + 2]; \
+	X3   += ks[r + (R) + 3]; \
+	X4   += ks[r + (R) + 4]; \
+	X5   += ks[r + (R) + 5] + ts[r + (R) + 0]; \
+	X6   += ks[r + (R) + 6] + ts[r + (R) + 1]; \
+	X7   += ks[r + (R) + 7] +         r + (R); \
+	/* rotate key schedule */ \
+	ks[r +         (R) + 8] = ks[r + (R) - 1]; \
+	ts[r +         (R) + 2] = ts[r + (R) - 1]; \
+	skein_show_r_ptr(BLK_BITS, &ctx->h, SKEIN_RND_KEY_INJECT, X_ptr); \
+} while (0)
+
+		for (r = 1; r < 2 * RCNT; r += 2 * SKEIN_UNROLL_512)
+#endif /* end of looped code definitions */
+		{
+#define R512_8_ROUNDS(R)  /* do 8 full rounds */  \
+do { \
+		R512(0, 1, 2, 3, 4, 5, 6, 7, R_512_0, 8 * (R) + 1);   \
+		R512(2, 1, 4, 7, 6, 5, 0, 3, R_512_1, 8 * (R) + 2);   \
+		R512(4, 1, 6, 3, 0, 5, 2, 7, R_512_2, 8 * (R) + 3);   \
+		R512(6, 1, 0, 7, 2, 5, 4, 3, R_512_3, 8 * (R) + 4);   \
+		I512(2 * (R));                              \
+		R512(0, 1, 2, 3, 4, 5, 6, 7, R_512_4, 8 * (R) + 5);   \
+		R512(2, 1, 4, 7, 6, 5, 0, 3, R_512_5, 8 * (R) + 6);   \
+		R512(4, 1, 6, 3, 0, 5, 2, 7, R_512_6, 8 * (R) + 7);   \
+		R512(6, 1, 0, 7, 2, 5, 4, 3, R_512_7, 8 * (R) + 8);   \
+		I512(2 * (R) + 1);        /* and key injection */ \
+} while (0)
+
+			R512_8_ROUNDS(0);
+
+#define R512_UNROLL_R(NN) \
+		((SKEIN_UNROLL_512 == 0 && \
+		  SKEIN_512_ROUNDS_TOTAL/8 > (NN)) || \
+		 (SKEIN_UNROLL_512 > (NN)))
+
+	#if   R512_UNROLL_R(1)
+			R512_8_ROUNDS(1);
+	#endif
+	#if   R512_UNROLL_R(2)
+			R512_8_ROUNDS(2);
+	#endif
+	#if   R512_UNROLL_R(3)
+			R512_8_ROUNDS(3);
+	#endif
+	#if   R512_UNROLL_R(4)
+			R512_8_ROUNDS(4);
+	#endif
+	#if   R512_UNROLL_R(5)
+			R512_8_ROUNDS(5);
+	#endif
+	#if   R512_UNROLL_R(6)
+			R512_8_ROUNDS(6);
+	#endif
+	#if   R512_UNROLL_R(7)
+			R512_8_ROUNDS(7);
+	#endif
+	#if   R512_UNROLL_R(8)
+			R512_8_ROUNDS(8);
+	#endif
+	#if   R512_UNROLL_R(9)
+			R512_8_ROUNDS(9);
+	#endif
+	#if   R512_UNROLL_R(10)
+			R512_8_ROUNDS(10);
+	#endif
+	#if   R512_UNROLL_R(11)
+			R512_8_ROUNDS(11);
+	#endif
+	#if   R512_UNROLL_R(12)
+			R512_8_ROUNDS(12);
+	#endif
+	#if   R512_UNROLL_R(13)
+			R512_8_ROUNDS(13);
+	#endif
+	#if   R512_UNROLL_R(14)
+			R512_8_ROUNDS(14);
+	#endif
+	#if  (SKEIN_UNROLL_512 > 14)
+#error  "need more unrolling in skein_512_process_block"
+	#endif
+		}
+
+		/* do the final "feedforward" xor, update context chaining */
+		ctx->x[0] = X0 ^ w[0];
+		ctx->x[1] = X1 ^ w[1];
+		ctx->x[2] = X2 ^ w[2];
+		ctx->x[3] = X3 ^ w[3];
+		ctx->x[4] = X4 ^ w[4];
+		ctx->x[5] = X5 ^ w[5];
+		ctx->x[6] = X6 ^ w[6];
+		ctx->x[7] = X7 ^ w[7];
+		skein_show_round(BLK_BITS, &ctx->h, SKEIN_RND_FEED_FWD, ctx->x);
+
+		ts[1] &= ~SKEIN_T1_FLAG_FIRST;
+	} while (--blk_cnt);
+	ctx->h.tweak[0] = ts[0];
+	ctx->h.tweak[1] = ts[1];
+}
+
+#if defined(SKEIN_CODE_SIZE) || defined(SKEIN_PERF)
+size_t skein_512_process_block_code_size(void)
+{
+	return ((u8 *) skein_512_process_block_code_size) -
+		((u8 *) skein_512_process_block);
+}
+unsigned int skein_512_unroll_cnt(void)
+{
+	return SKEIN_UNROLL_512;
+}
+#endif
+#endif
+
+/*****************************  SKEIN_1024 ******************************/
+#if !(SKEIN_USE_ASM & 1024)
+void skein_1024_process_block(struct skein_1024_ctx *ctx, const u8 *blk_ptr,
+			      size_t blk_cnt, size_t byte_cnt_add)
+{ /* do it in C, always looping (unrolled is bigger AND slower!) */
+	enum {
+		WCNT = SKEIN_1024_STATE_WORDS
+	};
+#undef  RCNT
+#define RCNT  (SKEIN_1024_ROUNDS_TOTAL/8)
+
+#ifdef SKEIN_LOOP /* configure how much to unroll the loop */
+#define SKEIN_UNROLL_1024 ((SKEIN_LOOP)%10)
+#else
+#define SKEIN_UNROLL_1024 (0)
+#endif
+
+#if (SKEIN_UNROLL_1024 != 0)
+#if (RCNT % SKEIN_UNROLL_1024)
+#error "Invalid SKEIN_UNROLL_1024" /* sanity check on unroll count */
+#endif
+	size_t  r;
+	u64  kw[WCNT+4+RCNT*2]; /* key sched: chaining vars + tweak + "rot" */
+#else
+	u64  kw[WCNT+4]; /* key schedule words : chaining vars + tweak */
+#endif
+
+	/* local copy of vars, for speed */
+	u64  X00, X01, X02, X03, X04, X05, X06, X07,
+	     X08, X09, X10, X11, X12, X13, X14, X15;
+	u64  w[WCNT]; /* local copy of input block */
+#ifdef SKEIN_DEBUG
+	const u64 *X_ptr[16]; /* use for debugging (help cc put Xn in regs) */
+
+	X_ptr[0]  = &X00;  X_ptr[1]  = &X01;  X_ptr[2]  = &X02;
+	X_ptr[3]  = &X03;  X_ptr[4]  = &X04;  X_ptr[5]  = &X05;
+	X_ptr[6]  = &X06;  X_ptr[7]  = &X07;  X_ptr[8]  = &X08;
+	X_ptr[9]  = &X09;  X_ptr[10] = &X10;  X_ptr[11] = &X11;
+	X_ptr[12] = &X12;  X_ptr[13] = &X13;  X_ptr[14] = &X14;
+	X_ptr[15] = &X15;
+#endif
+
+	skein_assert(blk_cnt != 0); /* never call with blk_cnt == 0! */
+	ts[0] = ctx->h.tweak[0];
+	ts[1] = ctx->h.tweak[1];
+	do  {
+		/*
+		 * this implementation only supports 2**64 input bytes
+		 * (no carry out here)
+		 */
+		ts[0] += byte_cnt_add; /* update processed length */
+
+		/* precompute the key schedule for this block */
+		ks[0]  = ctx->x[0];
+		ks[1]  = ctx->x[1];
+		ks[2]  = ctx->x[2];
+		ks[3]  = ctx->x[3];
+		ks[4]  = ctx->x[4];
+		ks[5]  = ctx->x[5];
+		ks[6]  = ctx->x[6];
+		ks[7]  = ctx->x[7];
+		ks[8]  = ctx->x[8];
+		ks[9]  = ctx->x[9];
+		ks[10] = ctx->x[10];
+		ks[11] = ctx->x[11];
+		ks[12] = ctx->x[12];
+		ks[13] = ctx->x[13];
+		ks[14] = ctx->x[14];
+		ks[15] = ctx->x[15];
+		ks[16] =  ks[0] ^  ks[1] ^  ks[2] ^  ks[3] ^
+			  ks[4] ^  ks[5] ^  ks[6] ^  ks[7] ^
+			  ks[8] ^  ks[9] ^ ks[10] ^ ks[11] ^
+			  ks[12] ^ ks[13] ^ ks[14] ^ ks[15] ^ SKEIN_KS_PARITY;
+
+		ts[2]  = ts[0] ^ ts[1];
+
+		/* get input block in little-endian format */
+		skein_get64_lsb_first(w, blk_ptr, WCNT);
+		debug_save_tweak(ctx);
+		skein_show_block(BLK_BITS, &ctx->h, ctx->x, blk_ptr, w, ks, ts);
+
+		X00    =  w[0] +  ks[0]; /* do the first full key injection */
+		X01    =  w[1] +  ks[1];
+		X02    =  w[2] +  ks[2];
+		X03    =  w[3] +  ks[3];
+		X04    =  w[4] +  ks[4];
+		X05    =  w[5] +  ks[5];
+		X06    =  w[6] +  ks[6];
+		X07    =  w[7] +  ks[7];
+		X08    =  w[8] +  ks[8];
+		X09    =  w[9] +  ks[9];
+		X10    = w[10] + ks[10];
+		X11    = w[11] + ks[11];
+		X12    = w[12] + ks[12];
+		X13    = w[13] + ks[13] + ts[0];
+		X14    = w[14] + ks[14] + ts[1];
+		X15    = w[15] + ks[15];
+
+		skein_show_r_ptr(BLK_BITS, &ctx->h, SKEIN_RND_KEY_INITIAL,
+				 X_ptr);
+
+#define ROUND1024(p0, p1, p2, p3, p4, p5, p6, p7, p8, p9, pA, pB, pC, pD, pE, \
+			pF, ROT, r_num) \
+do { \
+	X##p0 += X##p1; X##p1 = rotl_64(X##p1, ROT##_0); X##p1 ^= X##p0;   \
+	X##p2 += X##p3; X##p3 = rotl_64(X##p3, ROT##_1); X##p3 ^= X##p2;   \
+	X##p4 += X##p5; X##p5 = rotl_64(X##p5, ROT##_2); X##p5 ^= X##p4;   \
+	X##p6 += X##p7; X##p7 = rotl_64(X##p7, ROT##_3); X##p7 ^= X##p6;   \
+	X##p8 += X##p9; X##p9 = rotl_64(X##p9, ROT##_4); X##p9 ^= X##p8;   \
+	X##pA += X##pB; X##pB = rotl_64(X##pB, ROT##_5); X##pB ^= X##pA;   \
+	X##pC += X##pD; X##pD = rotl_64(X##pD, ROT##_6); X##pD ^= X##pC;   \
+	X##pE += X##pF; X##pF = rotl_64(X##pF, ROT##_7); X##pF ^= X##pE;   \
+} while (0)
+
+#if SKEIN_UNROLL_1024 == 0
+#define R1024(p0, p1, p2, p3, p4, p5, p6, p7, p8, p9, pA, pB, pC, pD, pE, pF, \
+		ROT, rn) \
+do { \
+	ROUND1024(p0, p1, p2, p3, p4, p5, p6, p7, p8, p9, pA, pB, pC, pD, pE, \
+			pF, ROT, rn); \
+	skein_show_r_ptr(BLK_BITS, &ctx->h, rn, X_ptr); \
+} while (0)
+
+#define I1024(R) \
+do { \
+	/* inject the key schedule value */ \
+	X00   += ks[((R) +  1) % 17]; \
+	X01   += ks[((R) +  2) % 17]; \
+	X02   += ks[((R) +  3) % 17]; \
+	X03   += ks[((R) +  4) % 17]; \
+	X04   += ks[((R) +  5) % 17]; \
+	X05   += ks[((R) +  6) % 17]; \
+	X06   += ks[((R) +  7) % 17]; \
+	X07   += ks[((R) +  8) % 17]; \
+	X08   += ks[((R) +  9) % 17]; \
+	X09   += ks[((R) + 10) % 17]; \
+	X10   += ks[((R) + 11) % 17]; \
+	X11   += ks[((R) + 12) % 17]; \
+	X12   += ks[((R) + 13) % 17]; \
+	X13   += ks[((R) + 14) % 17] + ts[((R) + 1) % 3]; \
+	X14   += ks[((R) + 15) % 17] + ts[((R) + 2) % 3]; \
+	X15   += ks[((R) + 16) % 17] +     (R) + 1;       \
+	skein_show_r_ptr(BLK_BITS, &ctx->h, SKEIN_RND_KEY_INJECT, X_ptr); \
+} while (0)
+#else /* looping version */
+#define R1024(p0, p1, p2, p3, p4, p5, p6, p7, p8, p9, pA, pB, pC, pD, pE, pF, \
+		ROT, rn) \
+do { \
+	ROUND1024(p0, p1, p2, p3, p4, p5, p6, p7, p8, p9, pA, pB, pC, pD, pE, \
+			pF, ROT, rn); \
+	skein_show_r_ptr(BLK_BITS, &ctx->h, 4 * (r - 1) + rn, X_ptr); \
+} while (0)
+
+#define I1024(R) \
+do { \
+	/* inject the key schedule value */ \
+	X00   += ks[r + (R) +  0]; \
+	X01   += ks[r + (R) +  1]; \
+	X02   += ks[r + (R) +  2]; \
+	X03   += ks[r + (R) +  3]; \
+	X04   += ks[r + (R) +  4]; \
+	X05   += ks[r + (R) +  5]; \
+	X06   += ks[r + (R) +  6]; \
+	X07   += ks[r + (R) +  7]; \
+	X08   += ks[r + (R) +  8]; \
+	X09   += ks[r + (R) +  9]; \
+	X10   += ks[r + (R) + 10]; \
+	X11   += ks[r + (R) + 11]; \
+	X12   += ks[r + (R) + 12]; \
+	X13   += ks[r + (R) + 13] + ts[r + (R) + 0]; \
+	X14   += ks[r + (R) + 14] + ts[r + (R) + 1]; \
+	X15   += ks[r + (R) + 15] +         r + (R); \
+	/* rotate key schedule */ \
+	ks[r  +         (R) + 16] = ks[r + (R) - 1]; \
+	ts[r  +         (R) +  2] = ts[r + (R) - 1]; \
+	skein_show_r_ptr(BLK_BITSi, &ctx->h, SKEIN_RND_KEY_INJECT, X_ptr); \
+} while (0)
+
+		for (r = 1; r <= 2 * RCNT; r += 2 * SKEIN_UNROLL_1024)
+#endif
+		{
+#define R1024_8_ROUNDS(R) \
+do { \
+	R1024(00, 01, 02, 03, 04, 05, 06, 07, 08, 09, 10, 11, 12, 13, 14, 15, \
+		R1024_0, 8*(R) + 1); \
+	R1024(00, 09, 02, 13, 06, 11, 04, 15, 10, 07, 12, 03, 14, 05, 08, 01, \
+		R1024_1, 8*(R) + 2); \
+	R1024(00, 07, 02, 05, 04, 03, 06, 01, 12, 15, 14, 13, 08, 11, 10, 09, \
+		R1024_2, 8*(R) + 3); \
+	R1024(00, 15, 02, 11, 06, 13, 04, 09, 14, 01, 08, 05, 10, 03, 12, 07, \
+		R1024_3, 8*(R) + 4); \
+	I1024(2*(R)); \
+	R1024(00, 01, 02, 03, 04, 05, 06, 07, 08, 09, 10, 11, 12, 13, 14, 15, \
+		R1024_4, 8*(R) + 5); \
+	R1024(00, 09, 02, 13, 06, 11, 04, 15, 10, 07, 12, 03, 14, 05, 08, 01, \
+		R1024_5, 8*(R) + 6); \
+	R1024(00, 07, 02, 05, 04, 03, 06, 01, 12, 15, 14, 13, 08, 11, 10, 09, \
+		R1024_6, 8*(R) + 7); \
+	R1024(00, 15, 02, 11, 06, 13, 04, 09, 14, 01, 08, 05, 10, 03, 12, 07, \
+		R1024_7, 8*(R) + 8); \
+	I1024(2*(R)+1); \
+} while (0)
+
+			R1024_8_ROUNDS(0);
+
+#define R1024_UNROLL_R(NN) \
+		((SKEIN_UNROLL_1024 == 0 && \
+		  SKEIN_1024_ROUNDS_TOTAL/8 > (NN)) || \
+		 (SKEIN_UNROLL_1024 > (NN)))
+
+	#if   R1024_UNROLL_R(1)
+			R1024_8_ROUNDS(1);
+	#endif
+	#if   R1024_UNROLL_R(2)
+			R1024_8_ROUNDS(2);
+	#endif
+	#if   R1024_UNROLL_R(3)
+			R1024_8_ROUNDS(3);
+	#endif
+	#if   R1024_UNROLL_R(4)
+			R1024_8_ROUNDS(4);
+	#endif
+	#if   R1024_UNROLL_R(5)
+			R1024_8_ROUNDS(5);
+	#endif
+	#if   R1024_UNROLL_R(6)
+			R1024_8_ROUNDS(6);
+	#endif
+	#if   R1024_UNROLL_R(7)
+			R1024_8_ROUNDS(7);
+	#endif
+	#if   R1024_UNROLL_R(8)
+			R1024_8_ROUNDS(8);
+	#endif
+	#if   R1024_UNROLL_R(9)
+			R1024_8_ROUNDS(9);
+	#endif
+	#if   R1024_UNROLL_R(10)
+			R1024_8_ROUNDS(10);
+	#endif
+	#if   R1024_UNROLL_R(11)
+			R1024_8_ROUNDS(11);
+	#endif
+	#if   R1024_UNROLL_R(12)
+			R1024_8_ROUNDS(12);
+	#endif
+	#if   R1024_UNROLL_R(13)
+			R1024_8_ROUNDS(13);
+	#endif
+	#if   R1024_UNROLL_R(14)
+			R1024_8_ROUNDS(14);
+	#endif
+#if  (SKEIN_UNROLL_1024 > 14)
+#error  "need more unrolling in Skein_1024_Process_Block"
+  #endif
+		}
+		/* do the final "feedforward" xor, update context chaining */
+
+		ctx->x[0] = X00 ^ w[0];
+		ctx->x[1] = X01 ^ w[1];
+		ctx->x[2] = X02 ^ w[2];
+		ctx->x[3] = X03 ^ w[3];
+		ctx->x[4] = X04 ^ w[4];
+		ctx->x[5] = X05 ^ w[5];
+		ctx->x[6] = X06 ^ w[6];
+		ctx->x[7] = X07 ^ w[7];
+		ctx->x[8] = X08 ^ w[8];
+		ctx->x[9] = X09 ^ w[9];
+		ctx->x[10] = X10 ^ w[10];
+		ctx->x[11] = X11 ^ w[11];
+		ctx->x[12] = X12 ^ w[12];
+		ctx->x[13] = X13 ^ w[13];
+		ctx->x[14] = X14 ^ w[14];
+		ctx->x[15] = X15 ^ w[15];
+
+		skein_show_round(BLK_BITS, &ctx->h, SKEIN_RND_FEED_FWD, ctx->x);
+
+		ts[1] &= ~SKEIN_T1_FLAG_FIRST;
+		blk_ptr += SKEIN_1024_BLOCK_BYTES;
+	} while (--blk_cnt);
+	ctx->h.tweak[0] = ts[0];
+	ctx->h.tweak[1] = ts[1];
+}
+
+#if defined(SKEIN_CODE_SIZE) || defined(SKEIN_PERF)
+size_t skein_1024_process_block_code_size(void)
+{
+	return ((u8 *) skein_1024_process_block_code_size) -
+		((u8 *) skein_1024_process_block);
+}
+unsigned int skein_1024_unroll_cnt(void)
+{
+	return SKEIN_UNROLL_1024;
+}
+#endif
+#endif
diff --git a/drivers/staging/skein/skein_block.h b/drivers/staging/skein/skein_block.h
new file mode 100644
index 00000000000..bd7bdc35df2
--- /dev/null
+++ b/drivers/staging/skein/skein_block.h
@@ -0,0 +1,22 @@
+/***********************************************************************
+**
+** Implementation of the Skein hash function.
+**
+** Source code author: Doug Whiting, 2008.
+**
+** This algorithm and source code is released to the public domain.
+**
+************************************************************************/
+#ifndef _SKEIN_BLOCK_H_
+#define _SKEIN_BLOCK_H_
+
+#include "skein.h" /* get the Skein API definitions   */
+
+void skein_256_process_block(struct skein_256_ctx *ctx, const u8 *blk_ptr,
+			     size_t blk_cnt, size_t byte_cnt_add);
+void skein_512_process_block(struct skein_512_ctx *ctx, const u8 *blk_ptr,
+			     size_t blk_cnt, size_t byte_cnt_add);
+void skein_1024_process_block(struct skein_1024_ctx *ctx, const u8 *blk_ptr,
+			      size_t blk_cnt, size_t byte_cnt_add);
+
+#endif
diff --git a/drivers/staging/skein/skein_iv.h b/drivers/staging/skein/skein_iv.h
new file mode 100644
index 00000000000..a03703deeaf
--- /dev/null
+++ b/drivers/staging/skein/skein_iv.h
@@ -0,0 +1,186 @@
+#ifndef _SKEIN_IV_H_
+#define _SKEIN_IV_H_
+
+#include "skein.h"    /* get Skein macros and types */
+
+/*
+***************** Pre-computed Skein IVs *******************
+**
+** NOTE: these values are not "magic" constants, but
+** are generated using the Threefish block function.
+** They are pre-computed here only for speed; i.e., to
+** avoid the need for a Threefish call during Init().
+**
+** The IV for any fixed hash length may be pre-computed.
+** Only the most common values are included here.
+**
+************************************************************
+**/
+
+#define MK_64 SKEIN_MK_64
+
+/* blkSize =  256 bits. hashSize =  128 bits */
+const u64 SKEIN_256_IV_128[] = {
+	MK_64(0xE1111906, 0x964D7260),
+	MK_64(0x883DAAA7, 0x7C8D811C),
+	MK_64(0x10080DF4, 0x91960F7A),
+	MK_64(0xCCF7DDE5, 0xB45BC1C2)
+};
+
+/* blkSize =  256 bits. hashSize =  160 bits */
+const u64 SKEIN_256_IV_160[] = {
+	MK_64(0x14202314, 0x72825E98),
+	MK_64(0x2AC4E9A2, 0x5A77E590),
+	MK_64(0xD47A5856, 0x8838D63E),
+	MK_64(0x2DD2E496, 0x8586AB7D)
+};
+
+/* blkSize =  256 bits. hashSize =  224 bits */
+const u64 SKEIN_256_IV_224[] = {
+	MK_64(0xC6098A8C, 0x9AE5EA0B),
+	MK_64(0x876D5686, 0x08C5191C),
+	MK_64(0x99CB88D7, 0xD7F53884),
+	MK_64(0x384BDDB1, 0xAEDDB5DE)
+};
+
+/* blkSize =  256 bits. hashSize =  256 bits */
+const u64 SKEIN_256_IV_256[] = {
+	MK_64(0xFC9DA860, 0xD048B449),
+	MK_64(0x2FCA6647, 0x9FA7D833),
+	MK_64(0xB33BC389, 0x6656840F),
+	MK_64(0x6A54E920, 0xFDE8DA69)
+};
+
+/* blkSize =  512 bits. hashSize =  128 bits */
+const u64 SKEIN_512_IV_128[] = {
+	MK_64(0xA8BC7BF3, 0x6FBF9F52),
+	MK_64(0x1E9872CE, 0xBD1AF0AA),
+	MK_64(0x309B1790, 0xB32190D3),
+	MK_64(0xBCFBB854, 0x3F94805C),
+	MK_64(0x0DA61BCD, 0x6E31B11B),
+	MK_64(0x1A18EBEA, 0xD46A32E3),
+	MK_64(0xA2CC5B18, 0xCE84AA82),
+	MK_64(0x6982AB28, 0x9D46982D)
+};
+
+/* blkSize =  512 bits. hashSize =  160 bits */
+const u64 SKEIN_512_IV_160[] = {
+	MK_64(0x28B81A2A, 0xE013BD91),
+	MK_64(0xC2F11668, 0xB5BDF78F),
+	MK_64(0x1760D8F3, 0xF6A56F12),
+	MK_64(0x4FB74758, 0x8239904F),
+	MK_64(0x21EDE07F, 0x7EAF5056),
+	MK_64(0xD908922E, 0x63ED70B8),
+	MK_64(0xB8EC76FF, 0xECCB52FA),
+	MK_64(0x01A47BB8, 0xA3F27A6E)
+};
+
+/* blkSize =  512 bits. hashSize =  224 bits */
+const u64 SKEIN_512_IV_224[] = {
+	MK_64(0xCCD06162, 0x48677224),
+	MK_64(0xCBA65CF3, 0xA92339EF),
+	MK_64(0x8CCD69D6, 0x52FF4B64),
+	MK_64(0x398AED7B, 0x3AB890B4),
+	MK_64(0x0F59D1B1, 0x457D2BD0),
+	MK_64(0x6776FE65, 0x75D4EB3D),
+	MK_64(0x99FBC70E, 0x997413E9),
+	MK_64(0x9E2CFCCF, 0xE1C41EF7)
+};
+
+/* blkSize =  512 bits. hashSize =  256 bits */
+const u64 SKEIN_512_IV_256[] = {
+	MK_64(0xCCD044A1, 0x2FDB3E13),
+	MK_64(0xE8359030, 0x1A79A9EB),
+	MK_64(0x55AEA061, 0x4F816E6F),
+	MK_64(0x2A2767A4, 0xAE9B94DB),
+	MK_64(0xEC06025E, 0x74DD7683),
+	MK_64(0xE7A436CD, 0xC4746251),
+	MK_64(0xC36FBAF9, 0x393AD185),
+	MK_64(0x3EEDBA18, 0x33EDFC13)
+};
+
+/* blkSize =  512 bits. hashSize =  384 bits */
+const u64 SKEIN_512_IV_384[] = {
+	MK_64(0xA3F6C6BF, 0x3A75EF5F),
+	MK_64(0xB0FEF9CC, 0xFD84FAA4),
+	MK_64(0x9D77DD66, 0x3D770CFE),
+	MK_64(0xD798CBF3, 0xB468FDDA),
+	MK_64(0x1BC4A666, 0x8A0E4465),
+	MK_64(0x7ED7D434, 0xE5807407),
+	MK_64(0x548FC1AC, 0xD4EC44D6),
+	MK_64(0x266E1754, 0x6AA18FF8)
+};
+
+/* blkSize =  512 bits. hashSize =  512 bits */
+const u64 SKEIN_512_IV_512[] = {
+	MK_64(0x4903ADFF, 0x749C51CE),
+	MK_64(0x0D95DE39, 0x9746DF03),
+	MK_64(0x8FD19341, 0x27C79BCE),
+	MK_64(0x9A255629, 0xFF352CB1),
+	MK_64(0x5DB62599, 0xDF6CA7B0),
+	MK_64(0xEABE394C, 0xA9D5C3F4),
+	MK_64(0x991112C7, 0x1A75B523),
+	MK_64(0xAE18A40B, 0x660FCC33)
+};
+
+/* blkSize = 1024 bits. hashSize =  384 bits */
+const u64 SKEIN_1024_IV_384[] = {
+	MK_64(0x5102B6B8, 0xC1894A35),
+	MK_64(0xFEEBC9E3, 0xFE8AF11A),
+	MK_64(0x0C807F06, 0xE32BED71),
+	MK_64(0x60C13A52, 0xB41A91F6),
+	MK_64(0x9716D35D, 0xD4917C38),
+	MK_64(0xE780DF12, 0x6FD31D3A),
+	MK_64(0x797846B6, 0xC898303A),
+	MK_64(0xB172C2A8, 0xB3572A3B),
+	MK_64(0xC9BC8203, 0xA6104A6C),
+	MK_64(0x65909338, 0xD75624F4),
+	MK_64(0x94BCC568, 0x4B3F81A0),
+	MK_64(0x3EBBF51E, 0x10ECFD46),
+	MK_64(0x2DF50F0B, 0xEEB08542),
+	MK_64(0x3B5A6530, 0x0DBC6516),
+	MK_64(0x484B9CD2, 0x167BBCE1),
+	MK_64(0x2D136947, 0xD4CBAFEA)
+};
+
+/* blkSize = 1024 bits. hashSize =  512 bits */
+const u64 SKEIN_1024_IV_512[] = {
+	MK_64(0xCAEC0E5D, 0x7C1B1B18),
+	MK_64(0xA01B0E04, 0x5F03E802),
+	MK_64(0x33840451, 0xED912885),
+	MK_64(0x374AFB04, 0xEAEC2E1C),
+	MK_64(0xDF25A0E2, 0x813581F7),
+	MK_64(0xE4004093, 0x8B12F9D2),
+	MK_64(0xA662D539, 0xC2ED39B6),
+	MK_64(0xFA8B85CF, 0x45D8C75A),
+	MK_64(0x8316ED8E, 0x29EDE796),
+	MK_64(0x053289C0, 0x2E9F91B8),
+	MK_64(0xC3F8EF1D, 0x6D518B73),
+	MK_64(0xBDCEC3C4, 0xD5EF332E),
+	MK_64(0x549A7E52, 0x22974487),
+	MK_64(0x67070872, 0x5B749816),
+	MK_64(0xB9CD28FB, 0xF0581BD1),
+	MK_64(0x0E2940B8, 0x15804974)
+};
+
+/* blkSize = 1024 bits. hashSize = 1024 bits */
+const u64 SKEIN_1024_IV_1024[] = {
+	MK_64(0xD593DA07, 0x41E72355),
+	MK_64(0x15B5E511, 0xAC73E00C),
+	MK_64(0x5180E5AE, 0xBAF2C4F0),
+	MK_64(0x03BD41D3, 0xFCBCAFAF),
+	MK_64(0x1CAEC6FD, 0x1983A898),
+	MK_64(0x6E510B8B, 0xCDD0589F),
+	MK_64(0x77E2BDFD, 0xC6394ADA),
+	MK_64(0xC11E1DB5, 0x24DCB0A3),
+	MK_64(0xD6D14AF9, 0xC6329AB5),
+	MK_64(0x6A9B0BFC, 0x6EB67E0D),
+	MK_64(0x9243C60D, 0xCCFF1332),
+	MK_64(0x1A1F1DDE, 0x743F02D4),
+	MK_64(0x0996753C, 0x10ED0BB8),
+	MK_64(0x6572DD22, 0xF2B4969A),
+	MK_64(0x61FD3062, 0xD00A579A),
+	MK_64(0x1DE0536E, 0x8682E539)
+};
+
+#endif /* _SKEIN_IV_H_ */
diff --git a/drivers/staging/skein/threefish_api.c b/drivers/staging/skein/threefish_api.c
new file mode 100644
index 00000000000..2b649abb78c
--- /dev/null
+++ b/drivers/staging/skein/threefish_api.c
@@ -0,0 +1,77 @@
+#include <linux/string.h>
+#include "threefish_api.h"
+
+void threefish_set_key(struct threefish_key *key_ctx,
+		       enum threefish_size state_size,
+		       u64 *key_data, u64 *tweak)
+{
+	int key_words = state_size / 64;
+	int i;
+	u64 parity = KEY_SCHEDULE_CONST;
+
+	key_ctx->tweak[0] = tweak[0];
+	key_ctx->tweak[1] = tweak[1];
+	key_ctx->tweak[2] = tweak[0] ^ tweak[1];
+
+	for (i = 0; i < key_words; i++) {
+		key_ctx->key[i] = key_data[i];
+		parity ^= key_data[i];
+	}
+	key_ctx->key[i] = parity;
+	key_ctx->state_size = state_size;
+}
+
+void threefish_encrypt_block_bytes(struct threefish_key *key_ctx, u8 *in,
+				   u8 *out)
+{
+	u64 plain[SKEIN_MAX_STATE_WORDS];        /* max number of words*/
+	u64 cipher[SKEIN_MAX_STATE_WORDS];
+
+	skein_get64_lsb_first(plain, in, key_ctx->state_size / 64);
+	threefish_encrypt_block_words(key_ctx, plain, cipher);
+	skein_put64_lsb_first(out, cipher, key_ctx->state_size / 8);
+}
+
+void threefish_encrypt_block_words(struct threefish_key *key_ctx, u64 *in,
+				   u64 *out)
+{
+	switch (key_ctx->state_size) {
+	case THREEFISH_256:
+		threefish_encrypt_256(key_ctx, in, out);
+		break;
+	case THREEFISH_512:
+		threefish_encrypt_512(key_ctx, in, out);
+		break;
+	case THREEFISH_1024:
+		threefish_encrypt_1024(key_ctx, in, out);
+		break;
+	}
+}
+
+void threefish_decrypt_block_bytes(struct threefish_key *key_ctx, u8 *in,
+				   u8 *out)
+{
+	u64 plain[SKEIN_MAX_STATE_WORDS];        /* max number of words*/
+	u64 cipher[SKEIN_MAX_STATE_WORDS];
+
+	skein_get64_lsb_first(cipher, in, key_ctx->state_size / 64);
+	threefish_decrypt_block_words(key_ctx, cipher, plain);
+	skein_put64_lsb_first(out, plain, key_ctx->state_size / 8);
+}
+
+void threefish_decrypt_block_words(struct threefish_key *key_ctx, u64 *in,
+				   u64 *out)
+{
+	switch (key_ctx->state_size) {
+	case THREEFISH_256:
+		threefish_decrypt_256(key_ctx, in, out);
+		break;
+	case THREEFISH_512:
+		threefish_decrypt_512(key_ctx, in, out);
+		break;
+	case THREEFISH_1024:
+		threefish_decrypt_1024(key_ctx, in, out);
+		break;
+	}
+}
+
diff --git a/drivers/staging/skein/threefish_api.h b/drivers/staging/skein/threefish_api.h
new file mode 100644
index 00000000000..8d5ddf8b3a9
--- /dev/null
+++ b/drivers/staging/skein/threefish_api.h
@@ -0,0 +1,170 @@
+
+#ifndef THREEFISHAPI_H
+#define THREEFISHAPI_H
+
+/**
+ * @file threefish_api.h
+ * @brief A Threefish cipher API and its functions.
+ * @{
+ *
+ * This API and the functions that implement this API simplify the usage
+ * of the Threefish cipher. The design and the way to use the functions
+ * follow the openSSL design but at the same time take care of some Threefish
+ * specific behaviour and possibilities.
+ *
+ * These are the low level functions that deal with Threefish blocks only.
+ * Implementations for cipher modes such as ECB, CFB, or CBC may use these
+ * functions.
+ *
+@code
+	// Threefish cipher context data
+	struct threefish_key key_ctx;
+
+	// Initialize the context
+	threefish_set_key(&key_ctx, THREEFISH_512, key, tweak);
+
+	// Encrypt
+	threefish_encrypt_block_bytes(&key_ctx, input, cipher);
+@endcode
+ */
+
+#include <linux/types.h>
+#include "skein.h"
+
+#define KEY_SCHEDULE_CONST 0x1BD11BDAA9FC1A22L
+
+/**
+ * Which Threefish size to use
+ */
+enum threefish_size {
+	THREEFISH_256 = 256,     /*!< Skein with 256 bit state */
+	THREEFISH_512 = 512,     /*!< Skein with 512 bit state */
+	THREEFISH_1024 = 1024    /*!< Skein with 1024 bit state */
+};
+
+/**
+ * Context for Threefish key and tweak words.
+ *
+ * This structure was setup with some know-how of the internal
+ * Skein structures, in particular ordering of header and size dependent
+ * variables. If Skein implementation changes this, the adapt these
+ * structures as well.
+ */
+struct threefish_key {
+	u64 state_size;
+	u64 key[SKEIN_MAX_STATE_WORDS+1];   /* max number of key words*/
+	u64 tweak[3];
+};
+
+/**
+ * Set Threefish key and tweak data.
+ *
+ * This function sets the key and tweak data for the Threefish cipher of
+ * the given size. The key data must have the same length (number of bits)
+ * as the state size
+ *
+ * @param key_ctx
+ *     Pointer to a Threefish key structure.
+ * @param size
+ *     Which Skein size to use.
+ * @param key_data
+ *     Pointer to the key words (word has 64 bits).
+ * @param tweak
+ *     Pointer to the two tweak words (word has 64 bits).
+ */
+void threefish_set_key(struct threefish_key *key_ctx,
+		       enum threefish_size state_size,
+		       u64 *key_data, u64 *tweak);
+
+/**
+ * Encrypt Threefish block (bytes).
+ *
+ * The buffer must have at least the same length (number of bits) as the
+ * state size for this key. The function uses the first @c state_size bits
+ * of the input buffer, encrypts them and stores the result in the output
+ * buffer.
+ *
+ * @param key_ctx
+ *     Pointer to a Threefish key structure.
+ * @param in
+ *     Poionter to plaintext data buffer.
+ * @param out
+ *     Pointer to cipher buffer.
+ */
+void threefish_encrypt_block_bytes(struct threefish_key *key_ctx, u8 *in,
+				   u8 *out);
+
+/**
+ * Encrypt Threefish block (words).
+ *
+ * The buffer must have at least the same length (number of bits) as the
+ * state size for this key. The function uses the first @c state_size bits
+ * of the input buffer, encrypts them and stores the result in the output
+ * buffer.
+ *
+ * The wordsize ist set to 64 bits.
+ *
+ * @param key_ctx
+ *     Pointer to a Threefish key structure.
+ * @param in
+ *     Poionter to plaintext data buffer.
+ * @param out
+ *     Pointer to cipher buffer.
+ */
+void threefish_encrypt_block_words(struct threefish_key *key_ctx, u64 *in,
+				   u64 *out);
+
+/**
+ * Decrypt Threefish block (bytes).
+ *
+ * The buffer must have at least the same length (number of bits) as the
+ * state size for this key. The function uses the first @c state_size bits
+ * of the input buffer, decrypts them and stores the result in the output
+ * buffer
+ *
+ * @param key_ctx
+ *     Pointer to a Threefish key structure.
+ * @param in
+ *     Poionter to cipher data buffer.
+ * @param out
+ *     Pointer to plaintext buffer.
+ */
+void threefish_decrypt_block_bytes(struct threefish_key *key_ctx, u8 *in,
+				   u8 *out);
+
+/**
+ * Decrypt Threefish block (words).
+ *
+ * The buffer must have at least the same length (number of bits) as the
+ * state size for this key. The function uses the first @c state_size bits
+ * of the input buffer, encrypts them and stores the result in the output
+ * buffer.
+ *
+ * The wordsize ist set to 64 bits.
+ *
+ * @param key_ctx
+ *     Pointer to a Threefish key structure.
+ * @param in
+ *     Poionter to cipher data buffer.
+ * @param out
+ *     Pointer to plaintext buffer.
+ */
+void threefish_decrypt_block_words(struct threefish_key *key_ctx, u64 *in,
+				   u64 *out);
+
+void threefish_encrypt_256(struct threefish_key *key_ctx, u64 *input,
+			   u64 *output);
+void threefish_encrypt_512(struct threefish_key *key_ctx, u64 *input,
+			   u64 *output);
+void threefish_encrypt_1024(struct threefish_key *key_ctx, u64 *input,
+			    u64 *output);
+void threefish_decrypt_256(struct threefish_key *key_ctx, u64 *input,
+			   u64 *output);
+void threefish_decrypt_512(struct threefish_key *key_ctx, u64 *input,
+			   u64 *output);
+void threefish_decrypt_1024(struct threefish_key *key_ctx, u64 *input,
+			    u64 *output);
+/**
+ * @}
+ */
+#endif
diff --git a/drivers/staging/skein/threefish_block.c b/drivers/staging/skein/threefish_block.c
new file mode 100644
index 00000000000..bd1e15caae4
--- /dev/null
+++ b/drivers/staging/skein/threefish_block.c
@@ -0,0 +1,8258 @@
+#include "threefish_api.h"
+
+void threefish_encrypt_256(struct threefish_key *key_ctx, u64 *input,
+			   u64 *output)
+{
+	u64 b0 = input[0], b1 = input[1],
+	    b2 = input[2], b3 = input[3];
+	u64 k0 = key_ctx->key[0], k1 = key_ctx->key[1],
+	    k2 = key_ctx->key[2], k3 = key_ctx->key[3],
+	    k4 = key_ctx->key[4];
+	u64 t0 = key_ctx->tweak[0], t1 = key_ctx->tweak[1],
+	    t2 = key_ctx->tweak[2];
+
+	b1 += k1 + t0;
+	b0 += b1 + k0;
+	b1 = ((b1 << 14) | (b1 >> (64 - 14))) ^ b0;
+
+	b3 += k3;
+	b2 += b3 + k2 + t1;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 52) | (b3 >> (64 - 52))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 57) | (b1 >> (64 - 57))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 40) | (b3 >> (64 - 40))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 5) | (b3 >> (64 - 5))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 37) | (b1 >> (64 - 37))) ^ b2;
+
+	b1 += k2 + t1;
+	b0 += b1 + k1;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b0;
+
+	b3 += k4 + 1;
+	b2 += b3 + k3 + t2;
+	b3 = ((b3 << 33) | (b3 >> (64 - 33))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 46) | (b3 >> (64 - 46))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 12) | (b1 >> (64 - 12))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 58) | (b1 >> (64 - 58))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 32) | (b3 >> (64 - 32))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 32) | (b1 >> (64 - 32))) ^ b2;
+
+
+	b1 += k3 + t2;
+	b0 += b1 + k2;
+	b1 = ((b1 << 14) | (b1 >> (64 - 14))) ^ b0;
+
+	b3 += k0 + 2;
+	b2 += b3 + k4 + t0;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 52) | (b3 >> (64 - 52))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 57) | (b1 >> (64 - 57))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 40) | (b3 >> (64 - 40))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 5) | (b3 >> (64 - 5))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 37) | (b1 >> (64 - 37))) ^ b2;
+
+	b1 += k4 + t0;
+	b0 += b1 + k3;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b0;
+
+	b3 += k1 + 3;
+	b2 += b3 + k0 + t1;
+	b3 = ((b3 << 33) | (b3 >> (64 - 33))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 46) | (b3 >> (64 - 46))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 12) | (b1 >> (64 - 12))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 58) | (b1 >> (64 - 58))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 32) | (b3 >> (64 - 32))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 32) | (b1 >> (64 - 32))) ^ b2;
+
+
+	b1 += k0 + t1;
+	b0 += b1 + k4;
+	b1 = ((b1 << 14) | (b1 >> (64 - 14))) ^ b0;
+
+	b3 += k2 + 4;
+	b2 += b3 + k1 + t2;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 52) | (b3 >> (64 - 52))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 57) | (b1 >> (64 - 57))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 40) | (b3 >> (64 - 40))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 5) | (b3 >> (64 - 5))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 37) | (b1 >> (64 - 37))) ^ b2;
+
+	b1 += k1 + t2;
+	b0 += b1 + k0;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b0;
+
+	b3 += k3 + 5;
+	b2 += b3 + k2 + t0;
+	b3 = ((b3 << 33) | (b3 >> (64 - 33))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 46) | (b3 >> (64 - 46))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 12) | (b1 >> (64 - 12))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 58) | (b1 >> (64 - 58))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 32) | (b3 >> (64 - 32))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 32) | (b1 >> (64 - 32))) ^ b2;
+
+
+	b1 += k2 + t0;
+	b0 += b1 + k1;
+	b1 = ((b1 << 14) | (b1 >> (64 - 14))) ^ b0;
+
+	b3 += k4 + 6;
+	b2 += b3 + k3 + t1;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 52) | (b3 >> (64 - 52))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 57) | (b1 >> (64 - 57))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 40) | (b3 >> (64 - 40))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 5) | (b3 >> (64 - 5))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 37) | (b1 >> (64 - 37))) ^ b2;
+
+	b1 += k3 + t1;
+	b0 += b1 + k2;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b0;
+
+	b3 += k0 + 7;
+	b2 += b3 + k4 + t2;
+	b3 = ((b3 << 33) | (b3 >> (64 - 33))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 46) | (b3 >> (64 - 46))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 12) | (b1 >> (64 - 12))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 58) | (b1 >> (64 - 58))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 32) | (b3 >> (64 - 32))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 32) | (b1 >> (64 - 32))) ^ b2;
+
+
+	b1 += k4 + t2;
+	b0 += b1 + k3;
+	b1 = ((b1 << 14) | (b1 >> (64 - 14))) ^ b0;
+
+	b3 += k1 + 8;
+	b2 += b3 + k0 + t0;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 52) | (b3 >> (64 - 52))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 57) | (b1 >> (64 - 57))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 40) | (b3 >> (64 - 40))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 5) | (b3 >> (64 - 5))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 37) | (b1 >> (64 - 37))) ^ b2;
+
+	b1 += k0 + t0;
+	b0 += b1 + k4;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b0;
+
+	b3 += k2 + 9;
+	b2 += b3 + k1 + t1;
+	b3 = ((b3 << 33) | (b3 >> (64 - 33))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 46) | (b3 >> (64 - 46))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 12) | (b1 >> (64 - 12))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 58) | (b1 >> (64 - 58))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 32) | (b3 >> (64 - 32))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 32) | (b1 >> (64 - 32))) ^ b2;
+
+
+	b1 += k1 + t1;
+	b0 += b1 + k0;
+	b1 = ((b1 << 14) | (b1 >> (64 - 14))) ^ b0;
+
+	b3 += k3 + 10;
+	b2 += b3 + k2 + t2;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 52) | (b3 >> (64 - 52))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 57) | (b1 >> (64 - 57))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 40) | (b3 >> (64 - 40))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 5) | (b3 >> (64 - 5))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 37) | (b1 >> (64 - 37))) ^ b2;
+
+	b1 += k2 + t2;
+	b0 += b1 + k1;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b0;
+
+	b3 += k4 + 11;
+	b2 += b3 + k3 + t0;
+	b3 = ((b3 << 33) | (b3 >> (64 - 33))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 46) | (b3 >> (64 - 46))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 12) | (b1 >> (64 - 12))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 58) | (b1 >> (64 - 58))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 32) | (b3 >> (64 - 32))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 32) | (b1 >> (64 - 32))) ^ b2;
+
+
+	b1 += k3 + t0;
+	b0 += b1 + k2;
+	b1 = ((b1 << 14) | (b1 >> (64 - 14))) ^ b0;
+
+	b3 += k0 + 12;
+	b2 += b3 + k4 + t1;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 52) | (b3 >> (64 - 52))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 57) | (b1 >> (64 - 57))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 40) | (b3 >> (64 - 40))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 5) | (b3 >> (64 - 5))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 37) | (b1 >> (64 - 37))) ^ b2;
+
+	b1 += k4 + t1;
+	b0 += b1 + k3;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b0;
+
+	b3 += k1 + 13;
+	b2 += b3 + k0 + t2;
+	b3 = ((b3 << 33) | (b3 >> (64 - 33))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 46) | (b3 >> (64 - 46))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 12) | (b1 >> (64 - 12))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 58) | (b1 >> (64 - 58))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 32) | (b3 >> (64 - 32))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 32) | (b1 >> (64 - 32))) ^ b2;
+
+
+	b1 += k0 + t2;
+	b0 += b1 + k4;
+	b1 = ((b1 << 14) | (b1 >> (64 - 14))) ^ b0;
+
+	b3 += k2 + 14;
+	b2 += b3 + k1 + t0;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 52) | (b3 >> (64 - 52))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 57) | (b1 >> (64 - 57))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 40) | (b3 >> (64 - 40))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 5) | (b3 >> (64 - 5))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 37) | (b1 >> (64 - 37))) ^ b2;
+
+	b1 += k1 + t0;
+	b0 += b1 + k0;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b0;
+
+	b3 += k3 + 15;
+	b2 += b3 + k2 + t1;
+	b3 = ((b3 << 33) | (b3 >> (64 - 33))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 46) | (b3 >> (64 - 46))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 12) | (b1 >> (64 - 12))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 58) | (b1 >> (64 - 58))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 32) | (b3 >> (64 - 32))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 32) | (b1 >> (64 - 32))) ^ b2;
+
+
+	b1 += k2 + t1;
+	b0 += b1 + k1;
+	b1 = ((b1 << 14) | (b1 >> (64 - 14))) ^ b0;
+
+	b3 += k4 + 16;
+	b2 += b3 + k3 + t2;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 52) | (b3 >> (64 - 52))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 57) | (b1 >> (64 - 57))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 40) | (b3 >> (64 - 40))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 5) | (b3 >> (64 - 5))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 37) | (b1 >> (64 - 37))) ^ b2;
+
+	b1 += k3 + t2;
+	b0 += b1 + k2;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b0;
+
+	b3 += k0 + 17;
+	b2 += b3 + k4 + t0;
+	b3 = ((b3 << 33) | (b3 >> (64 - 33))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 46) | (b3 >> (64 - 46))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 12) | (b1 >> (64 - 12))) ^ b2;
+
+	b0 += b1;
+	b1 = ((b1 << 58) | (b1 >> (64 - 58))) ^ b0;
+
+	b2 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b2;
+
+	b0 += b3;
+	b3 = ((b3 << 32) | (b3 >> (64 - 32))) ^ b0;
+
+	b2 += b1;
+	b1 = ((b1 << 32) | (b1 >> (64 - 32))) ^ b2;
+
+	output[0] = b0 + k3;
+	output[1] = b1 + k4 + t0;
+	output[2] = b2 + k0 + t1;
+	output[3] = b3 + k1 + 18;
+}
+
+void threefish_decrypt_256(struct threefish_key *key_ctx, u64 *input,
+			   u64 *output)
+{
+	u64 b0 = input[0], b1 = input[1],
+	    b2 = input[2], b3 = input[3];
+	u64 k0 = key_ctx->key[0], k1 = key_ctx->key[1],
+	    k2 = key_ctx->key[2], k3 = key_ctx->key[3],
+	    k4 = key_ctx->key[4];
+	u64 t0 = key_ctx->tweak[0], t1 = key_ctx->tweak[1],
+	    t2 = key_ctx->tweak[2];
+
+	u64 tmp;
+
+	b0 -= k3;
+	b1 -= k4 + t0;
+	b2 -= k0 + t1;
+	b3 -= k1 + 18;
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 32) | (tmp << (64 - 32));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 32) | (tmp << (64 - 32));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 58) | (tmp << (64 - 58));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 12) | (tmp << (64 - 12));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b0 -= b1 + k2;
+	b1 -= k3 + t2;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b3 + k4 + t0;
+	b3 -= k0 + 17;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 37) | (tmp << (64 - 37));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 40) | (tmp << (64 - 40));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 52) | (tmp << (64 - 52));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 57) | (tmp << (64 - 57));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 14) | (tmp << (64 - 14));
+	b0 -= b1 + k1;
+	b1 -= k2 + t1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b2 -= b3 + k3 + t2;
+	b3 -= k4 + 16;
+
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 32) | (tmp << (64 - 32));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 32) | (tmp << (64 - 32));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 58) | (tmp << (64 - 58));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 12) | (tmp << (64 - 12));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b0 -= b1 + k0;
+	b1 -= k1 + t0;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b3 + k2 + t1;
+	b3 -= k3 + 15;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 37) | (tmp << (64 - 37));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 40) | (tmp << (64 - 40));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 52) | (tmp << (64 - 52));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 57) | (tmp << (64 - 57));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 14) | (tmp << (64 - 14));
+	b0 -= b1 + k4;
+	b1 -= k0 + t2;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b2 -= b3 + k1 + t0;
+	b3 -= k2 + 14;
+
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 32) | (tmp << (64 - 32));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 32) | (tmp << (64 - 32));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 58) | (tmp << (64 - 58));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 12) | (tmp << (64 - 12));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b0 -= b1 + k3;
+	b1 -= k4 + t1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b3 + k0 + t2;
+	b3 -= k1 + 13;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 37) | (tmp << (64 - 37));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 40) | (tmp << (64 - 40));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 52) | (tmp << (64 - 52));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 57) | (tmp << (64 - 57));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 14) | (tmp << (64 - 14));
+	b0 -= b1 + k2;
+	b1 -= k3 + t0;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b2 -= b3 + k4 + t1;
+	b3 -= k0 + 12;
+
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 32) | (tmp << (64 - 32));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 32) | (tmp << (64 - 32));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 58) | (tmp << (64 - 58));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 12) | (tmp << (64 - 12));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b0 -= b1 + k1;
+	b1 -= k2 + t2;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b3 + k3 + t0;
+	b3 -= k4 + 11;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 37) | (tmp << (64 - 37));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 40) | (tmp << (64 - 40));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 52) | (tmp << (64 - 52));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 57) | (tmp << (64 - 57));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 14) | (tmp << (64 - 14));
+	b0 -= b1 + k0;
+	b1 -= k1 + t1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b2 -= b3 + k2 + t2;
+	b3 -= k3 + 10;
+
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 32) | (tmp << (64 - 32));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 32) | (tmp << (64 - 32));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 58) | (tmp << (64 - 58));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 12) | (tmp << (64 - 12));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b0 -= b1 + k4;
+	b1 -= k0 + t0;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b3 + k1 + t1;
+	b3 -= k2 + 9;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 37) | (tmp << (64 - 37));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 40) | (tmp << (64 - 40));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 52) | (tmp << (64 - 52));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 57) | (tmp << (64 - 57));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 14) | (tmp << (64 - 14));
+	b0 -= b1 + k3;
+	b1 -= k4 + t2;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b2 -= b3 + k0 + t0;
+	b3 -= k1 + 8;
+
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 32) | (tmp << (64 - 32));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 32) | (tmp << (64 - 32));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 58) | (tmp << (64 - 58));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 12) | (tmp << (64 - 12));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b0 -= b1 + k2;
+	b1 -= k3 + t1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b3 + k4 + t2;
+	b3 -= k0 + 7;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 37) | (tmp << (64 - 37));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 40) | (tmp << (64 - 40));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 52) | (tmp << (64 - 52));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 57) | (tmp << (64 - 57));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 14) | (tmp << (64 - 14));
+	b0 -= b1 + k1;
+	b1 -= k2 + t0;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b2 -= b3 + k3 + t1;
+	b3 -= k4 + 6;
+
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 32) | (tmp << (64 - 32));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 32) | (tmp << (64 - 32));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 58) | (tmp << (64 - 58));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 12) | (tmp << (64 - 12));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b0 -= b1 + k0;
+	b1 -= k1 + t2;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b3 + k2 + t0;
+	b3 -= k3 + 5;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 37) | (tmp << (64 - 37));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 40) | (tmp << (64 - 40));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 52) | (tmp << (64 - 52));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 57) | (tmp << (64 - 57));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 14) | (tmp << (64 - 14));
+	b0 -= b1 + k4;
+	b1 -= k0 + t1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b2 -= b3 + k1 + t2;
+	b3 -= k2 + 4;
+
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 32) | (tmp << (64 - 32));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 32) | (tmp << (64 - 32));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 58) | (tmp << (64 - 58));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 12) | (tmp << (64 - 12));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b0 -= b1 + k3;
+	b1 -= k4 + t0;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b3 + k0 + t1;
+	b3 -= k1 + 3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 37) | (tmp << (64 - 37));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 40) | (tmp << (64 - 40));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 52) | (tmp << (64 - 52));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 57) | (tmp << (64 - 57));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 14) | (tmp << (64 - 14));
+	b0 -= b1 + k2;
+	b1 -= k3 + t2;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b2 -= b3 + k4 + t0;
+	b3 -= k0 + 2;
+
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 32) | (tmp << (64 - 32));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 32) | (tmp << (64 - 32));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 58) | (tmp << (64 - 58));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 12) | (tmp << (64 - 12));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b0 -= b1 + k1;
+	b1 -= k2 + t1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b3 + k3 + t2;
+	b3 -= k4 + 1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 37) | (tmp << (64 - 37));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b0 -= b1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 40) | (tmp << (64 - 40));
+	b2 -= b3;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 52) | (tmp << (64 - 52));
+	b0 -= b3;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 57) | (tmp << (64 - 57));
+	b2 -= b1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 14) | (tmp << (64 - 14));
+	b0 -= b1 + k0;
+	b1 -= k1 + t0;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b2 -= b3 + k2 + t1;
+	b3 -= k3;
+
+	output[0] = b0;
+	output[1] = b1;
+	output[2] = b2;
+	output[3] = b3;
+}
+
+void threefish_encrypt_512(struct threefish_key *key_ctx, u64 *input,
+			   u64 *output)
+{
+	u64 b0 = input[0], b1 = input[1],
+	    b2 = input[2], b3 = input[3],
+	    b4 = input[4], b5 = input[5],
+	    b6 = input[6], b7 = input[7];
+	u64 k0 = key_ctx->key[0], k1 = key_ctx->key[1],
+	    k2 = key_ctx->key[2], k3 = key_ctx->key[3],
+	    k4 = key_ctx->key[4], k5 = key_ctx->key[5],
+	    k6 = key_ctx->key[6], k7 = key_ctx->key[7],
+	    k8 = key_ctx->key[8];
+	u64 t0 = key_ctx->tweak[0], t1 = key_ctx->tweak[1],
+	    t2 = key_ctx->tweak[2];
+
+	b1 += k1;
+	b0 += b1 + k0;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b0;
+
+	b3 += k3;
+	b2 += b3 + k2;
+	b3 = ((b3 << 36) | (b3 >> (64 - 36))) ^ b2;
+
+	b5 += k5 + t0;
+	b4 += b5 + k4;
+	b5 = ((b5 << 19) | (b5 >> (64 - 19))) ^ b4;
+
+	b7 += k7;
+	b6 += b7 + k6 + t1;
+	b7 = ((b7 << 37) | (b7 >> (64 - 37))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 33) | (b1 >> (64 - 33))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 27) | (b7 >> (64 - 27))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 14) | (b5 >> (64 - 14))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 42) | (b3 >> (64 - 42))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 17) | (b1 >> (64 - 17))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 49) | (b3 >> (64 - 49))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 36) | (b5 >> (64 - 36))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 39) | (b7 >> (64 - 39))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 44) | (b1 >> (64 - 44))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 9) | (b7 >> (64 - 9))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 54) | (b5 >> (64 - 54))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 56) | (b3 >> (64 - 56))) ^ b4;
+
+	b1 += k2;
+	b0 += b1 + k1;
+	b1 = ((b1 << 39) | (b1 >> (64 - 39))) ^ b0;
+
+	b3 += k4;
+	b2 += b3 + k3;
+	b3 = ((b3 << 30) | (b3 >> (64 - 30))) ^ b2;
+
+	b5 += k6 + t1;
+	b4 += b5 + k5;
+	b5 = ((b5 << 34) | (b5 >> (64 - 34))) ^ b4;
+
+	b7 += k8 + 1;
+	b6 += b7 + k7 + t2;
+	b7 = ((b7 << 24) | (b7 >> (64 - 24))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 50) | (b7 >> (64 - 50))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 10) | (b5 >> (64 - 10))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 17) | (b3 >> (64 - 17))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 29) | (b3 >> (64 - 29))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 39) | (b5 >> (64 - 39))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 43) | (b7 >> (64 - 43))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 8) | (b1 >> (64 - 8))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 35) | (b7 >> (64 - 35))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 56) | (b5 >> (64 - 56))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b4;
+
+	b1 += k3;
+	b0 += b1 + k2;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b0;
+
+	b3 += k5;
+	b2 += b3 + k4;
+	b3 = ((b3 << 36) | (b3 >> (64 - 36))) ^ b2;
+
+	b5 += k7 + t2;
+	b4 += b5 + k6;
+	b5 = ((b5 << 19) | (b5 >> (64 - 19))) ^ b4;
+
+	b7 += k0 + 2;
+	b6 += b7 + k8 + t0;
+	b7 = ((b7 << 37) | (b7 >> (64 - 37))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 33) | (b1 >> (64 - 33))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 27) | (b7 >> (64 - 27))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 14) | (b5 >> (64 - 14))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 42) | (b3 >> (64 - 42))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 17) | (b1 >> (64 - 17))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 49) | (b3 >> (64 - 49))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 36) | (b5 >> (64 - 36))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 39) | (b7 >> (64 - 39))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 44) | (b1 >> (64 - 44))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 9) | (b7 >> (64 - 9))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 54) | (b5 >> (64 - 54))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 56) | (b3 >> (64 - 56))) ^ b4;
+
+	b1 += k4;
+	b0 += b1 + k3;
+	b1 = ((b1 << 39) | (b1 >> (64 - 39))) ^ b0;
+
+	b3 += k6;
+	b2 += b3 + k5;
+	b3 = ((b3 << 30) | (b3 >> (64 - 30))) ^ b2;
+
+	b5 += k8 + t0;
+	b4 += b5 + k7;
+	b5 = ((b5 << 34) | (b5 >> (64 - 34))) ^ b4;
+
+	b7 += k1 + 3;
+	b6 += b7 + k0 + t1;
+	b7 = ((b7 << 24) | (b7 >> (64 - 24))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 50) | (b7 >> (64 - 50))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 10) | (b5 >> (64 - 10))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 17) | (b3 >> (64 - 17))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 29) | (b3 >> (64 - 29))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 39) | (b5 >> (64 - 39))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 43) | (b7 >> (64 - 43))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 8) | (b1 >> (64 - 8))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 35) | (b7 >> (64 - 35))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 56) | (b5 >> (64 - 56))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b4;
+
+	b1 += k5;
+	b0 += b1 + k4;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b0;
+
+	b3 += k7;
+	b2 += b3 + k6;
+	b3 = ((b3 << 36) | (b3 >> (64 - 36))) ^ b2;
+
+	b5 += k0 + t1;
+	b4 += b5 + k8;
+	b5 = ((b5 << 19) | (b5 >> (64 - 19))) ^ b4;
+
+	b7 += k2 + 4;
+	b6 += b7 + k1 + t2;
+	b7 = ((b7 << 37) | (b7 >> (64 - 37))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 33) | (b1 >> (64 - 33))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 27) | (b7 >> (64 - 27))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 14) | (b5 >> (64 - 14))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 42) | (b3 >> (64 - 42))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 17) | (b1 >> (64 - 17))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 49) | (b3 >> (64 - 49))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 36) | (b5 >> (64 - 36))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 39) | (b7 >> (64 - 39))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 44) | (b1 >> (64 - 44))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 9) | (b7 >> (64 - 9))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 54) | (b5 >> (64 - 54))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 56) | (b3 >> (64 - 56))) ^ b4;
+
+	b1 += k6;
+	b0 += b1 + k5;
+	b1 = ((b1 << 39) | (b1 >> (64 - 39))) ^ b0;
+
+	b3 += k8;
+	b2 += b3 + k7;
+	b3 = ((b3 << 30) | (b3 >> (64 - 30))) ^ b2;
+
+	b5 += k1 + t2;
+	b4 += b5 + k0;
+	b5 = ((b5 << 34) | (b5 >> (64 - 34))) ^ b4;
+
+	b7 += k3 + 5;
+	b6 += b7 + k2 + t0;
+	b7 = ((b7 << 24) | (b7 >> (64 - 24))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 50) | (b7 >> (64 - 50))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 10) | (b5 >> (64 - 10))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 17) | (b3 >> (64 - 17))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 29) | (b3 >> (64 - 29))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 39) | (b5 >> (64 - 39))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 43) | (b7 >> (64 - 43))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 8) | (b1 >> (64 - 8))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 35) | (b7 >> (64 - 35))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 56) | (b5 >> (64 - 56))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b4;
+
+	b1 += k7;
+	b0 += b1 + k6;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b0;
+
+	b3 += k0;
+	b2 += b3 + k8;
+	b3 = ((b3 << 36) | (b3 >> (64 - 36))) ^ b2;
+
+	b5 += k2 + t0;
+	b4 += b5 + k1;
+	b5 = ((b5 << 19) | (b5 >> (64 - 19))) ^ b4;
+
+	b7 += k4 + 6;
+	b6 += b7 + k3 + t1;
+	b7 = ((b7 << 37) | (b7 >> (64 - 37))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 33) | (b1 >> (64 - 33))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 27) | (b7 >> (64 - 27))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 14) | (b5 >> (64 - 14))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 42) | (b3 >> (64 - 42))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 17) | (b1 >> (64 - 17))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 49) | (b3 >> (64 - 49))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 36) | (b5 >> (64 - 36))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 39) | (b7 >> (64 - 39))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 44) | (b1 >> (64 - 44))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 9) | (b7 >> (64 - 9))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 54) | (b5 >> (64 - 54))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 56) | (b3 >> (64 - 56))) ^ b4;
+
+	b1 += k8;
+	b0 += b1 + k7;
+	b1 = ((b1 << 39) | (b1 >> (64 - 39))) ^ b0;
+
+	b3 += k1;
+	b2 += b3 + k0;
+	b3 = ((b3 << 30) | (b3 >> (64 - 30))) ^ b2;
+
+	b5 += k3 + t1;
+	b4 += b5 + k2;
+	b5 = ((b5 << 34) | (b5 >> (64 - 34))) ^ b4;
+
+	b7 += k5 + 7;
+	b6 += b7 + k4 + t2;
+	b7 = ((b7 << 24) | (b7 >> (64 - 24))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 50) | (b7 >> (64 - 50))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 10) | (b5 >> (64 - 10))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 17) | (b3 >> (64 - 17))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 29) | (b3 >> (64 - 29))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 39) | (b5 >> (64 - 39))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 43) | (b7 >> (64 - 43))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 8) | (b1 >> (64 - 8))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 35) | (b7 >> (64 - 35))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 56) | (b5 >> (64 - 56))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b4;
+
+	b1 += k0;
+	b0 += b1 + k8;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b0;
+
+	b3 += k2;
+	b2 += b3 + k1;
+	b3 = ((b3 << 36) | (b3 >> (64 - 36))) ^ b2;
+
+	b5 += k4 + t2;
+	b4 += b5 + k3;
+	b5 = ((b5 << 19) | (b5 >> (64 - 19))) ^ b4;
+
+	b7 += k6 + 8;
+	b6 += b7 + k5 + t0;
+	b7 = ((b7 << 37) | (b7 >> (64 - 37))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 33) | (b1 >> (64 - 33))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 27) | (b7 >> (64 - 27))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 14) | (b5 >> (64 - 14))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 42) | (b3 >> (64 - 42))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 17) | (b1 >> (64 - 17))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 49) | (b3 >> (64 - 49))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 36) | (b5 >> (64 - 36))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 39) | (b7 >> (64 - 39))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 44) | (b1 >> (64 - 44))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 9) | (b7 >> (64 - 9))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 54) | (b5 >> (64 - 54))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 56) | (b3 >> (64 - 56))) ^ b4;
+
+	b1 += k1;
+	b0 += b1 + k0;
+	b1 = ((b1 << 39) | (b1 >> (64 - 39))) ^ b0;
+
+	b3 += k3;
+	b2 += b3 + k2;
+	b3 = ((b3 << 30) | (b3 >> (64 - 30))) ^ b2;
+
+	b5 += k5 + t0;
+	b4 += b5 + k4;
+	b5 = ((b5 << 34) | (b5 >> (64 - 34))) ^ b4;
+
+	b7 += k7 + 9;
+	b6 += b7 + k6 + t1;
+	b7 = ((b7 << 24) | (b7 >> (64 - 24))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 50) | (b7 >> (64 - 50))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 10) | (b5 >> (64 - 10))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 17) | (b3 >> (64 - 17))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 29) | (b3 >> (64 - 29))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 39) | (b5 >> (64 - 39))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 43) | (b7 >> (64 - 43))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 8) | (b1 >> (64 - 8))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 35) | (b7 >> (64 - 35))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 56) | (b5 >> (64 - 56))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b4;
+
+	b1 += k2;
+	b0 += b1 + k1;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b0;
+
+	b3 += k4;
+	b2 += b3 + k3;
+	b3 = ((b3 << 36) | (b3 >> (64 - 36))) ^ b2;
+
+	b5 += k6 + t1;
+	b4 += b5 + k5;
+	b5 = ((b5 << 19) | (b5 >> (64 - 19))) ^ b4;
+
+	b7 += k8 + 10;
+	b6 += b7 + k7 + t2;
+	b7 = ((b7 << 37) | (b7 >> (64 - 37))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 33) | (b1 >> (64 - 33))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 27) | (b7 >> (64 - 27))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 14) | (b5 >> (64 - 14))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 42) | (b3 >> (64 - 42))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 17) | (b1 >> (64 - 17))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 49) | (b3 >> (64 - 49))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 36) | (b5 >> (64 - 36))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 39) | (b7 >> (64 - 39))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 44) | (b1 >> (64 - 44))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 9) | (b7 >> (64 - 9))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 54) | (b5 >> (64 - 54))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 56) | (b3 >> (64 - 56))) ^ b4;
+
+	b1 += k3;
+	b0 += b1 + k2;
+	b1 = ((b1 << 39) | (b1 >> (64 - 39))) ^ b0;
+
+	b3 += k5;
+	b2 += b3 + k4;
+	b3 = ((b3 << 30) | (b3 >> (64 - 30))) ^ b2;
+
+	b5 += k7 + t2;
+	b4 += b5 + k6;
+	b5 = ((b5 << 34) | (b5 >> (64 - 34))) ^ b4;
+
+	b7 += k0 + 11;
+	b6 += b7 + k8 + t0;
+	b7 = ((b7 << 24) | (b7 >> (64 - 24))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 50) | (b7 >> (64 - 50))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 10) | (b5 >> (64 - 10))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 17) | (b3 >> (64 - 17))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 29) | (b3 >> (64 - 29))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 39) | (b5 >> (64 - 39))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 43) | (b7 >> (64 - 43))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 8) | (b1 >> (64 - 8))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 35) | (b7 >> (64 - 35))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 56) | (b5 >> (64 - 56))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b4;
+
+	b1 += k4;
+	b0 += b1 + k3;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b0;
+
+	b3 += k6;
+	b2 += b3 + k5;
+	b3 = ((b3 << 36) | (b3 >> (64 - 36))) ^ b2;
+
+	b5 += k8 + t0;
+	b4 += b5 + k7;
+	b5 = ((b5 << 19) | (b5 >> (64 - 19))) ^ b4;
+
+	b7 += k1 + 12;
+	b6 += b7 + k0 + t1;
+	b7 = ((b7 << 37) | (b7 >> (64 - 37))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 33) | (b1 >> (64 - 33))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 27) | (b7 >> (64 - 27))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 14) | (b5 >> (64 - 14))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 42) | (b3 >> (64 - 42))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 17) | (b1 >> (64 - 17))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 49) | (b3 >> (64 - 49))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 36) | (b5 >> (64 - 36))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 39) | (b7 >> (64 - 39))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 44) | (b1 >> (64 - 44))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 9) | (b7 >> (64 - 9))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 54) | (b5 >> (64 - 54))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 56) | (b3 >> (64 - 56))) ^ b4;
+
+	b1 += k5;
+	b0 += b1 + k4;
+	b1 = ((b1 << 39) | (b1 >> (64 - 39))) ^ b0;
+
+	b3 += k7;
+	b2 += b3 + k6;
+	b3 = ((b3 << 30) | (b3 >> (64 - 30))) ^ b2;
+
+	b5 += k0 + t1;
+	b4 += b5 + k8;
+	b5 = ((b5 << 34) | (b5 >> (64 - 34))) ^ b4;
+
+	b7 += k2 + 13;
+	b6 += b7 + k1 + t2;
+	b7 = ((b7 << 24) | (b7 >> (64 - 24))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 50) | (b7 >> (64 - 50))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 10) | (b5 >> (64 - 10))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 17) | (b3 >> (64 - 17))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 29) | (b3 >> (64 - 29))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 39) | (b5 >> (64 - 39))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 43) | (b7 >> (64 - 43))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 8) | (b1 >> (64 - 8))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 35) | (b7 >> (64 - 35))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 56) | (b5 >> (64 - 56))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b4;
+
+	b1 += k6;
+	b0 += b1 + k5;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b0;
+
+	b3 += k8;
+	b2 += b3 + k7;
+	b3 = ((b3 << 36) | (b3 >> (64 - 36))) ^ b2;
+
+	b5 += k1 + t2;
+	b4 += b5 + k0;
+	b5 = ((b5 << 19) | (b5 >> (64 - 19))) ^ b4;
+
+	b7 += k3 + 14;
+	b6 += b7 + k2 + t0;
+	b7 = ((b7 << 37) | (b7 >> (64 - 37))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 33) | (b1 >> (64 - 33))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 27) | (b7 >> (64 - 27))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 14) | (b5 >> (64 - 14))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 42) | (b3 >> (64 - 42))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 17) | (b1 >> (64 - 17))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 49) | (b3 >> (64 - 49))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 36) | (b5 >> (64 - 36))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 39) | (b7 >> (64 - 39))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 44) | (b1 >> (64 - 44))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 9) | (b7 >> (64 - 9))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 54) | (b5 >> (64 - 54))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 56) | (b3 >> (64 - 56))) ^ b4;
+
+	b1 += k7;
+	b0 += b1 + k6;
+	b1 = ((b1 << 39) | (b1 >> (64 - 39))) ^ b0;
+
+	b3 += k0;
+	b2 += b3 + k8;
+	b3 = ((b3 << 30) | (b3 >> (64 - 30))) ^ b2;
+
+	b5 += k2 + t0;
+	b4 += b5 + k1;
+	b5 = ((b5 << 34) | (b5 >> (64 - 34))) ^ b4;
+
+	b7 += k4 + 15;
+	b6 += b7 + k3 + t1;
+	b7 = ((b7 << 24) | (b7 >> (64 - 24))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 50) | (b7 >> (64 - 50))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 10) | (b5 >> (64 - 10))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 17) | (b3 >> (64 - 17))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 29) | (b3 >> (64 - 29))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 39) | (b5 >> (64 - 39))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 43) | (b7 >> (64 - 43))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 8) | (b1 >> (64 - 8))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 35) | (b7 >> (64 - 35))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 56) | (b5 >> (64 - 56))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b4;
+
+	b1 += k8;
+	b0 += b1 + k7;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b0;
+
+	b3 += k1;
+	b2 += b3 + k0;
+	b3 = ((b3 << 36) | (b3 >> (64 - 36))) ^ b2;
+
+	b5 += k3 + t1;
+	b4 += b5 + k2;
+	b5 = ((b5 << 19) | (b5 >> (64 - 19))) ^ b4;
+
+	b7 += k5 + 16;
+	b6 += b7 + k4 + t2;
+	b7 = ((b7 << 37) | (b7 >> (64 - 37))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 33) | (b1 >> (64 - 33))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 27) | (b7 >> (64 - 27))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 14) | (b5 >> (64 - 14))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 42) | (b3 >> (64 - 42))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 17) | (b1 >> (64 - 17))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 49) | (b3 >> (64 - 49))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 36) | (b5 >> (64 - 36))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 39) | (b7 >> (64 - 39))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 44) | (b1 >> (64 - 44))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 9) | (b7 >> (64 - 9))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 54) | (b5 >> (64 - 54))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 56) | (b3 >> (64 - 56))) ^ b4;
+
+	b1 += k0;
+	b0 += b1 + k8;
+	b1 = ((b1 << 39) | (b1 >> (64 - 39))) ^ b0;
+
+	b3 += k2;
+	b2 += b3 + k1;
+	b3 = ((b3 << 30) | (b3 >> (64 - 30))) ^ b2;
+
+	b5 += k4 + t2;
+	b4 += b5 + k3;
+	b5 = ((b5 << 34) | (b5 >> (64 - 34))) ^ b4;
+
+	b7 += k6 + 17;
+	b6 += b7 + k5 + t0;
+	b7 = ((b7 << 24) | (b7 >> (64 - 24))) ^ b6;
+
+	b2 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b2;
+
+	b4 += b7;
+	b7 = ((b7 << 50) | (b7 >> (64 - 50))) ^ b4;
+
+	b6 += b5;
+	b5 = ((b5 << 10) | (b5 >> (64 - 10))) ^ b6;
+
+	b0 += b3;
+	b3 = ((b3 << 17) | (b3 >> (64 - 17))) ^ b0;
+
+	b4 += b1;
+	b1 = ((b1 << 25) | (b1 >> (64 - 25))) ^ b4;
+
+	b6 += b3;
+	b3 = ((b3 << 29) | (b3 >> (64 - 29))) ^ b6;
+
+	b0 += b5;
+	b5 = ((b5 << 39) | (b5 >> (64 - 39))) ^ b0;
+
+	b2 += b7;
+	b7 = ((b7 << 43) | (b7 >> (64 - 43))) ^ b2;
+
+	b6 += b1;
+	b1 = ((b1 << 8) | (b1 >> (64 - 8))) ^ b6;
+
+	b0 += b7;
+	b7 = ((b7 << 35) | (b7 >> (64 - 35))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 56) | (b5 >> (64 - 56))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 22) | (b3 >> (64 - 22))) ^ b4;
+
+	output[0] = b0 + k0;
+	output[1] = b1 + k1;
+	output[2] = b2 + k2;
+	output[3] = b3 + k3;
+	output[4] = b4 + k4;
+	output[5] = b5 + k5 + t0;
+	output[6] = b6 + k6 + t1;
+	output[7] = b7 + k7 + 18;
+}
+
+void threefish_decrypt_512(struct threefish_key *key_ctx, u64 *input,
+			   u64 *output)
+{
+	u64 b0 = input[0], b1 = input[1],
+	    b2 = input[2], b3 = input[3],
+	    b4 = input[4], b5 = input[5],
+	    b6 = input[6], b7 = input[7];
+	u64 k0 = key_ctx->key[0], k1 = key_ctx->key[1],
+	    k2 = key_ctx->key[2], k3 = key_ctx->key[3],
+	    k4 = key_ctx->key[4], k5 = key_ctx->key[5],
+	    k6 = key_ctx->key[6], k7 = key_ctx->key[7],
+	    k8 = key_ctx->key[8];
+	u64 t0 = key_ctx->tweak[0], t1 = key_ctx->tweak[1],
+	    t2 = key_ctx->tweak[2];
+
+	u64 tmp;
+
+	b0 -= k0;
+	b1 -= k1;
+	b2 -= k2;
+	b3 -= k3;
+	b4 -= k4;
+	b5 -= k5 + t0;
+	b6 -= k6 + t1;
+	b7 -= k7 + 18;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 56) | (tmp << (64 - 56));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 35) | (tmp << (64 - 35));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 8) | (tmp << (64 - 8));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 43) | (tmp << (64 - 43));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 29) | (tmp << (64 - 29));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 17) | (tmp << (64 - 17));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 50) | (tmp << (64 - 50));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 24) | (tmp << (64 - 24));
+	b6 -= b7 + k5 + t0;
+	b7 -= k6 + 17;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 34) | (tmp << (64 - 34));
+	b4 -= b5 + k3;
+	b5 -= k4 + t2;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 30) | (tmp << (64 - 30));
+	b2 -= b3 + k1;
+	b3 -= k2;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b1 + k8;
+	b1 -= k0;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 56) | (tmp << (64 - 56));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 54) | (tmp << (64 - 54));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 44) | (tmp << (64 - 44));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 39) | (tmp << (64 - 39));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 36) | (tmp << (64 - 36));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 49) | (tmp << (64 - 49));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 17) | (tmp << (64 - 17));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 42) | (tmp << (64 - 42));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 14) | (tmp << (64 - 14));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 27) | (tmp << (64 - 27));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 37) | (tmp << (64 - 37));
+	b6 -= b7 + k4 + t2;
+	b7 -= k5 + 16;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 19) | (tmp << (64 - 19));
+	b4 -= b5 + k2;
+	b5 -= k3 + t1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 36) | (tmp << (64 - 36));
+	b2 -= b3 + k0;
+	b3 -= k1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b1 + k7;
+	b1 -= k8;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 56) | (tmp << (64 - 56));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 35) | (tmp << (64 - 35));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 8) | (tmp << (64 - 8));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 43) | (tmp << (64 - 43));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 29) | (tmp << (64 - 29));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 17) | (tmp << (64 - 17));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 50) | (tmp << (64 - 50));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 24) | (tmp << (64 - 24));
+	b6 -= b7 + k3 + t1;
+	b7 -= k4 + 15;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 34) | (tmp << (64 - 34));
+	b4 -= b5 + k1;
+	b5 -= k2 + t0;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 30) | (tmp << (64 - 30));
+	b2 -= b3 + k8;
+	b3 -= k0;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b1 + k6;
+	b1 -= k7;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 56) | (tmp << (64 - 56));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 54) | (tmp << (64 - 54));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 44) | (tmp << (64 - 44));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 39) | (tmp << (64 - 39));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 36) | (tmp << (64 - 36));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 49) | (tmp << (64 - 49));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 17) | (tmp << (64 - 17));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 42) | (tmp << (64 - 42));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 14) | (tmp << (64 - 14));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 27) | (tmp << (64 - 27));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 37) | (tmp << (64 - 37));
+	b6 -= b7 + k2 + t0;
+	b7 -= k3 + 14;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 19) | (tmp << (64 - 19));
+	b4 -= b5 + k0;
+	b5 -= k1 + t2;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 36) | (tmp << (64 - 36));
+	b2 -= b3 + k7;
+	b3 -= k8;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b1 + k5;
+	b1 -= k6;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 56) | (tmp << (64 - 56));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 35) | (tmp << (64 - 35));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 8) | (tmp << (64 - 8));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 43) | (tmp << (64 - 43));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 29) | (tmp << (64 - 29));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 17) | (tmp << (64 - 17));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 50) | (tmp << (64 - 50));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 24) | (tmp << (64 - 24));
+	b6 -= b7 + k1 + t2;
+	b7 -= k2 + 13;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 34) | (tmp << (64 - 34));
+	b4 -= b5 + k8;
+	b5 -= k0 + t1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 30) | (tmp << (64 - 30));
+	b2 -= b3 + k6;
+	b3 -= k7;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b1 + k4;
+	b1 -= k5;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 56) | (tmp << (64 - 56));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 54) | (tmp << (64 - 54));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 44) | (tmp << (64 - 44));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 39) | (tmp << (64 - 39));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 36) | (tmp << (64 - 36));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 49) | (tmp << (64 - 49));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 17) | (tmp << (64 - 17));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 42) | (tmp << (64 - 42));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 14) | (tmp << (64 - 14));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 27) | (tmp << (64 - 27));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 37) | (tmp << (64 - 37));
+	b6 -= b7 + k0 + t1;
+	b7 -= k1 + 12;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 19) | (tmp << (64 - 19));
+	b4 -= b5 + k7;
+	b5 -= k8 + t0;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 36) | (tmp << (64 - 36));
+	b2 -= b3 + k5;
+	b3 -= k6;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b1 + k3;
+	b1 -= k4;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 56) | (tmp << (64 - 56));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 35) | (tmp << (64 - 35));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 8) | (tmp << (64 - 8));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 43) | (tmp << (64 - 43));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 29) | (tmp << (64 - 29));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 17) | (tmp << (64 - 17));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 50) | (tmp << (64 - 50));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 24) | (tmp << (64 - 24));
+	b6 -= b7 + k8 + t0;
+	b7 -= k0 + 11;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 34) | (tmp << (64 - 34));
+	b4 -= b5 + k6;
+	b5 -= k7 + t2;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 30) | (tmp << (64 - 30));
+	b2 -= b3 + k4;
+	b3 -= k5;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b1 + k2;
+	b1 -= k3;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 56) | (tmp << (64 - 56));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 54) | (tmp << (64 - 54));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 44) | (tmp << (64 - 44));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 39) | (tmp << (64 - 39));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 36) | (tmp << (64 - 36));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 49) | (tmp << (64 - 49));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 17) | (tmp << (64 - 17));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 42) | (tmp << (64 - 42));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 14) | (tmp << (64 - 14));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 27) | (tmp << (64 - 27));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 37) | (tmp << (64 - 37));
+	b6 -= b7 + k7 + t2;
+	b7 -= k8 + 10;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 19) | (tmp << (64 - 19));
+	b4 -= b5 + k5;
+	b5 -= k6 + t1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 36) | (tmp << (64 - 36));
+	b2 -= b3 + k3;
+	b3 -= k4;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b1 + k1;
+	b1 -= k2;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 56) | (tmp << (64 - 56));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 35) | (tmp << (64 - 35));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 8) | (tmp << (64 - 8));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 43) | (tmp << (64 - 43));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 29) | (tmp << (64 - 29));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 17) | (tmp << (64 - 17));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 50) | (tmp << (64 - 50));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 24) | (tmp << (64 - 24));
+	b6 -= b7 + k6 + t1;
+	b7 -= k7 + 9;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 34) | (tmp << (64 - 34));
+	b4 -= b5 + k4;
+	b5 -= k5 + t0;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 30) | (tmp << (64 - 30));
+	b2 -= b3 + k2;
+	b3 -= k3;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b1 + k0;
+	b1 -= k1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 56) | (tmp << (64 - 56));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 54) | (tmp << (64 - 54));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 44) | (tmp << (64 - 44));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 39) | (tmp << (64 - 39));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 36) | (tmp << (64 - 36));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 49) | (tmp << (64 - 49));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 17) | (tmp << (64 - 17));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 42) | (tmp << (64 - 42));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 14) | (tmp << (64 - 14));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 27) | (tmp << (64 - 27));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 37) | (tmp << (64 - 37));
+	b6 -= b7 + k5 + t0;
+	b7 -= k6 + 8;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 19) | (tmp << (64 - 19));
+	b4 -= b5 + k3;
+	b5 -= k4 + t2;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 36) | (tmp << (64 - 36));
+	b2 -= b3 + k1;
+	b3 -= k2;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b1 + k8;
+	b1 -= k0;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 56) | (tmp << (64 - 56));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 35) | (tmp << (64 - 35));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 8) | (tmp << (64 - 8));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 43) | (tmp << (64 - 43));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 29) | (tmp << (64 - 29));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 17) | (tmp << (64 - 17));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 50) | (tmp << (64 - 50));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 24) | (tmp << (64 - 24));
+	b6 -= b7 + k4 + t2;
+	b7 -= k5 + 7;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 34) | (tmp << (64 - 34));
+	b4 -= b5 + k2;
+	b5 -= k3 + t1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 30) | (tmp << (64 - 30));
+	b2 -= b3 + k0;
+	b3 -= k1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b1 + k7;
+	b1 -= k8;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 56) | (tmp << (64 - 56));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 54) | (tmp << (64 - 54));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 44) | (tmp << (64 - 44));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 39) | (tmp << (64 - 39));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 36) | (tmp << (64 - 36));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 49) | (tmp << (64 - 49));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 17) | (tmp << (64 - 17));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 42) | (tmp << (64 - 42));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 14) | (tmp << (64 - 14));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 27) | (tmp << (64 - 27));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 37) | (tmp << (64 - 37));
+	b6 -= b7 + k3 + t1;
+	b7 -= k4 + 6;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 19) | (tmp << (64 - 19));
+	b4 -= b5 + k1;
+	b5 -= k2 + t0;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 36) | (tmp << (64 - 36));
+	b2 -= b3 + k8;
+	b3 -= k0;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b1 + k6;
+	b1 -= k7;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 56) | (tmp << (64 - 56));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 35) | (tmp << (64 - 35));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 8) | (tmp << (64 - 8));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 43) | (tmp << (64 - 43));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 29) | (tmp << (64 - 29));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 17) | (tmp << (64 - 17));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 50) | (tmp << (64 - 50));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 24) | (tmp << (64 - 24));
+	b6 -= b7 + k2 + t0;
+	b7 -= k3 + 5;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 34) | (tmp << (64 - 34));
+	b4 -= b5 + k0;
+	b5 -= k1 + t2;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 30) | (tmp << (64 - 30));
+	b2 -= b3 + k7;
+	b3 -= k8;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b1 + k5;
+	b1 -= k6;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 56) | (tmp << (64 - 56));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 54) | (tmp << (64 - 54));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 44) | (tmp << (64 - 44));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 39) | (tmp << (64 - 39));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 36) | (tmp << (64 - 36));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 49) | (tmp << (64 - 49));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 17) | (tmp << (64 - 17));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 42) | (tmp << (64 - 42));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 14) | (tmp << (64 - 14));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 27) | (tmp << (64 - 27));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 37) | (tmp << (64 - 37));
+	b6 -= b7 + k1 + t2;
+	b7 -= k2 + 4;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 19) | (tmp << (64 - 19));
+	b4 -= b5 + k8;
+	b5 -= k0 + t1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 36) | (tmp << (64 - 36));
+	b2 -= b3 + k6;
+	b3 -= k7;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b1 + k4;
+	b1 -= k5;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 56) | (tmp << (64 - 56));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 35) | (tmp << (64 - 35));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 8) | (tmp << (64 - 8));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 43) | (tmp << (64 - 43));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 29) | (tmp << (64 - 29));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 17) | (tmp << (64 - 17));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 50) | (tmp << (64 - 50));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 24) | (tmp << (64 - 24));
+	b6 -= b7 + k0 + t1;
+	b7 -= k1 + 3;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 34) | (tmp << (64 - 34));
+	b4 -= b5 + k7;
+	b5 -= k8 + t0;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 30) | (tmp << (64 - 30));
+	b2 -= b3 + k5;
+	b3 -= k6;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b1 + k3;
+	b1 -= k4;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 56) | (tmp << (64 - 56));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 54) | (tmp << (64 - 54));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 44) | (tmp << (64 - 44));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 39) | (tmp << (64 - 39));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 36) | (tmp << (64 - 36));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 49) | (tmp << (64 - 49));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 17) | (tmp << (64 - 17));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 42) | (tmp << (64 - 42));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 14) | (tmp << (64 - 14));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 27) | (tmp << (64 - 27));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 37) | (tmp << (64 - 37));
+	b6 -= b7 + k8 + t0;
+	b7 -= k0 + 2;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 19) | (tmp << (64 - 19));
+	b4 -= b5 + k6;
+	b5 -= k7 + t2;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 36) | (tmp << (64 - 36));
+	b2 -= b3 + k4;
+	b3 -= k5;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b1 + k2;
+	b1 -= k3;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 22) | (tmp << (64 - 22));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 56) | (tmp << (64 - 56));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 35) | (tmp << (64 - 35));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 8) | (tmp << (64 - 8));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 43) | (tmp << (64 - 43));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 29) | (tmp << (64 - 29));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 25) | (tmp << (64 - 25));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 17) | (tmp << (64 - 17));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 50) | (tmp << (64 - 50));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 24) | (tmp << (64 - 24));
+	b6 -= b7 + k7 + t2;
+	b7 -= k8 + 1;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 34) | (tmp << (64 - 34));
+	b4 -= b5 + k5;
+	b5 -= k6 + t1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 30) | (tmp << (64 - 30));
+	b2 -= b3 + k3;
+	b3 -= k4;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 39) | (tmp << (64 - 39));
+	b0 -= b1 + k1;
+	b1 -= k2;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 56) | (tmp << (64 - 56));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 54) | (tmp << (64 - 54));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b7;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 44) | (tmp << (64 - 44));
+	b6 -= b1;
+
+	tmp = b7 ^ b2;
+	b7 = (tmp >> 39) | (tmp << (64 - 39));
+	b2 -= b7;
+
+	tmp = b5 ^ b0;
+	b5 = (tmp >> 36) | (tmp << (64 - 36));
+	b0 -= b5;
+
+	tmp = b3 ^ b6;
+	b3 = (tmp >> 49) | (tmp << (64 - 49));
+	b6 -= b3;
+
+	tmp = b1 ^ b4;
+	b1 = (tmp >> 17) | (tmp << (64 - 17));
+	b4 -= b1;
+
+	tmp = b3 ^ b0;
+	b3 = (tmp >> 42) | (tmp << (64 - 42));
+	b0 -= b3;
+
+	tmp = b5 ^ b6;
+	b5 = (tmp >> 14) | (tmp << (64 - 14));
+	b6 -= b5;
+
+	tmp = b7 ^ b4;
+	b7 = (tmp >> 27) | (tmp << (64 - 27));
+	b4 -= b7;
+
+	tmp = b1 ^ b2;
+	b1 = (tmp >> 33) | (tmp << (64 - 33));
+	b2 -= b1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 37) | (tmp << (64 - 37));
+	b6 -= b7 + k6 + t1;
+	b7 -= k7;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 19) | (tmp << (64 - 19));
+	b4 -= b5 + k4;
+	b5 -= k5 + t0;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 36) | (tmp << (64 - 36));
+	b2 -= b3 + k2;
+	b3 -= k3;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b0 -= b1 + k0;
+	b1 -= k1;
+
+	output[0] = b0;
+	output[1] = b1;
+	output[2] = b2;
+	output[3] = b3;
+
+	output[7] = b7;
+	output[6] = b6;
+	output[5] = b5;
+	output[4] = b4;
+}
+
+void threefish_encrypt_1024(struct threefish_key *key_ctx, u64 *input,
+			    u64 *output)
+{
+	u64 b0 = input[0], b1 = input[1],
+	    b2 = input[2], b3 = input[3],
+	    b4 = input[4], b5 = input[5],
+	    b6 = input[6], b7 = input[7],
+	    b8 = input[8], b9 = input[9],
+	    b10 = input[10], b11 = input[11],
+	    b12 = input[12], b13 = input[13],
+	    b14 = input[14], b15 = input[15];
+	u64 k0 = key_ctx->key[0], k1 = key_ctx->key[1],
+	    k2 = key_ctx->key[2], k3 = key_ctx->key[3],
+	    k4 = key_ctx->key[4], k5 = key_ctx->key[5],
+	    k6 = key_ctx->key[6], k7 = key_ctx->key[7],
+	    k8 = key_ctx->key[8], k9 = key_ctx->key[9],
+	    k10 = key_ctx->key[10], k11 = key_ctx->key[11],
+	    k12 = key_ctx->key[12], k13 = key_ctx->key[13],
+	    k14 = key_ctx->key[14], k15 = key_ctx->key[15],
+	    k16 = key_ctx->key[16];
+	u64 t0 = key_ctx->tweak[0], t1 = key_ctx->tweak[1],
+	    t2 = key_ctx->tweak[2];
+
+	b1 += k1;
+	b0 += b1 + k0;
+	b1 = ((b1 << 24) | (b1 >> (64 - 24))) ^ b0;
+
+	b3 += k3;
+	b2 += b3 + k2;
+	b3 = ((b3 << 13) | (b3 >> (64 - 13))) ^ b2;
+
+	b5 += k5;
+	b4 += b5 + k4;
+	b5 = ((b5 << 8) | (b5 >> (64 - 8))) ^ b4;
+
+	b7 += k7;
+	b6 += b7 + k6;
+	b7 = ((b7 << 47) | (b7 >> (64 - 47))) ^ b6;
+
+	b9 += k9;
+	b8 += b9 + k8;
+	b9 = ((b9 << 8) | (b9 >> (64 - 8))) ^ b8;
+
+	b11 += k11;
+	b10 += b11 + k10;
+	b11 = ((b11 << 17) | (b11 >> (64 - 17))) ^ b10;
+
+	b13 += k13 + t0;
+	b12 += b13 + k12;
+	b13 = ((b13 << 22) | (b13 >> (64 - 22))) ^ b12;
+
+	b15 += k15;
+	b14 += b15 + k14 + t1;
+	b15 = ((b15 << 37) | (b15 >> (64 - 37))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 38) | (b9 >> (64 - 38))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 19) | (b13 >> (64 - 19))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 10) | (b11 >> (64 - 10))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 55) | (b15 >> (64 - 55))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 49) | (b7 >> (64 - 49))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 18) | (b3 >> (64 - 18))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 23) | (b5 >> (64 - 23))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 52) | (b1 >> (64 - 52))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 33) | (b7 >> (64 - 33))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 4) | (b5 >> (64 - 4))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 51) | (b3 >> (64 - 51))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 34) | (b15 >> (64 - 34))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 41) | (b13 >> (64 - 41))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 59) | (b11 >> (64 - 59))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 17) | (b9 >> (64 - 17))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 5) | (b15 >> (64 - 5))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 20) | (b11 >> (64 - 20))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 48) | (b13 >> (64 - 48))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 41) | (b9 >> (64 - 41))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 47) | (b1 >> (64 - 47))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 28) | (b5 >> (64 - 28))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 25) | (b7 >> (64 - 25))) ^ b12;
+
+	b1 += k2;
+	b0 += b1 + k1;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b0;
+
+	b3 += k4;
+	b2 += b3 + k3;
+	b3 = ((b3 << 9) | (b3 >> (64 - 9))) ^ b2;
+
+	b5 += k6;
+	b4 += b5 + k5;
+	b5 = ((b5 << 37) | (b5 >> (64 - 37))) ^ b4;
+
+	b7 += k8;
+	b6 += b7 + k7;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b6;
+
+	b9 += k10;
+	b8 += b9 + k9;
+	b9 = ((b9 << 12) | (b9 >> (64 - 12))) ^ b8;
+
+	b11 += k12;
+	b10 += b11 + k11;
+	b11 = ((b11 << 47) | (b11 >> (64 - 47))) ^ b10;
+
+	b13 += k14 + t1;
+	b12 += b13 + k13;
+	b13 = ((b13 << 44) | (b13 >> (64 - 44))) ^ b12;
+
+	b15 += k16 + 1;
+	b14 += b15 + k15 + t2;
+	b15 = ((b15 << 30) | (b15 >> (64 - 30))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 16) | (b9 >> (64 - 16))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 34) | (b13 >> (64 - 34))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 56) | (b11 >> (64 - 56))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 51) | (b15 >> (64 - 51))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 4) | (b7 >> (64 - 4))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 53) | (b3 >> (64 - 53))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 42) | (b5 >> (64 - 42))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 44) | (b5 >> (64 - 44))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 47) | (b3 >> (64 - 47))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 19) | (b15 >> (64 - 19))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 42) | (b13 >> (64 - 42))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 44) | (b11 >> (64 - 44))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 25) | (b9 >> (64 - 25))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 9) | (b15 >> (64 - 9))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 48) | (b11 >> (64 - 48))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 35) | (b13 >> (64 - 35))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 52) | (b9 >> (64 - 52))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 31) | (b5 >> (64 - 31))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 37) | (b3 >> (64 - 37))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 20) | (b7 >> (64 - 20))) ^ b12;
+
+	b1 += k3;
+	b0 += b1 + k2;
+	b1 = ((b1 << 24) | (b1 >> (64 - 24))) ^ b0;
+
+	b3 += k5;
+	b2 += b3 + k4;
+	b3 = ((b3 << 13) | (b3 >> (64 - 13))) ^ b2;
+
+	b5 += k7;
+	b4 += b5 + k6;
+	b5 = ((b5 << 8) | (b5 >> (64 - 8))) ^ b4;
+
+	b7 += k9;
+	b6 += b7 + k8;
+	b7 = ((b7 << 47) | (b7 >> (64 - 47))) ^ b6;
+
+	b9 += k11;
+	b8 += b9 + k10;
+	b9 = ((b9 << 8) | (b9 >> (64 - 8))) ^ b8;
+
+	b11 += k13;
+	b10 += b11 + k12;
+	b11 = ((b11 << 17) | (b11 >> (64 - 17))) ^ b10;
+
+	b13 += k15 + t2;
+	b12 += b13 + k14;
+	b13 = ((b13 << 22) | (b13 >> (64 - 22))) ^ b12;
+
+	b15 += k0 + 2;
+	b14 += b15 + k16 + t0;
+	b15 = ((b15 << 37) | (b15 >> (64 - 37))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 38) | (b9 >> (64 - 38))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 19) | (b13 >> (64 - 19))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 10) | (b11 >> (64 - 10))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 55) | (b15 >> (64 - 55))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 49) | (b7 >> (64 - 49))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 18) | (b3 >> (64 - 18))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 23) | (b5 >> (64 - 23))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 52) | (b1 >> (64 - 52))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 33) | (b7 >> (64 - 33))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 4) | (b5 >> (64 - 4))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 51) | (b3 >> (64 - 51))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 34) | (b15 >> (64 - 34))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 41) | (b13 >> (64 - 41))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 59) | (b11 >> (64 - 59))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 17) | (b9 >> (64 - 17))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 5) | (b15 >> (64 - 5))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 20) | (b11 >> (64 - 20))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 48) | (b13 >> (64 - 48))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 41) | (b9 >> (64 - 41))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 47) | (b1 >> (64 - 47))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 28) | (b5 >> (64 - 28))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 25) | (b7 >> (64 - 25))) ^ b12;
+
+	b1 += k4;
+	b0 += b1 + k3;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b0;
+
+	b3 += k6;
+	b2 += b3 + k5;
+	b3 = ((b3 << 9) | (b3 >> (64 - 9))) ^ b2;
+
+	b5 += k8;
+	b4 += b5 + k7;
+	b5 = ((b5 << 37) | (b5 >> (64 - 37))) ^ b4;
+
+	b7 += k10;
+	b6 += b7 + k9;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b6;
+
+	b9 += k12;
+	b8 += b9 + k11;
+	b9 = ((b9 << 12) | (b9 >> (64 - 12))) ^ b8;
+
+	b11 += k14;
+	b10 += b11 + k13;
+	b11 = ((b11 << 47) | (b11 >> (64 - 47))) ^ b10;
+
+	b13 += k16 + t0;
+	b12 += b13 + k15;
+	b13 = ((b13 << 44) | (b13 >> (64 - 44))) ^ b12;
+
+	b15 += k1 + 3;
+	b14 += b15 + k0 + t1;
+	b15 = ((b15 << 30) | (b15 >> (64 - 30))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 16) | (b9 >> (64 - 16))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 34) | (b13 >> (64 - 34))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 56) | (b11 >> (64 - 56))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 51) | (b15 >> (64 - 51))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 4) | (b7 >> (64 - 4))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 53) | (b3 >> (64 - 53))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 42) | (b5 >> (64 - 42))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 44) | (b5 >> (64 - 44))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 47) | (b3 >> (64 - 47))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 19) | (b15 >> (64 - 19))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 42) | (b13 >> (64 - 42))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 44) | (b11 >> (64 - 44))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 25) | (b9 >> (64 - 25))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 9) | (b15 >> (64 - 9))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 48) | (b11 >> (64 - 48))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 35) | (b13 >> (64 - 35))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 52) | (b9 >> (64 - 52))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 31) | (b5 >> (64 - 31))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 37) | (b3 >> (64 - 37))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 20) | (b7 >> (64 - 20))) ^ b12;
+
+	b1 += k5;
+	b0 += b1 + k4;
+	b1 = ((b1 << 24) | (b1 >> (64 - 24))) ^ b0;
+
+	b3 += k7;
+	b2 += b3 + k6;
+	b3 = ((b3 << 13) | (b3 >> (64 - 13))) ^ b2;
+
+	b5 += k9;
+	b4 += b5 + k8;
+	b5 = ((b5 << 8) | (b5 >> (64 - 8))) ^ b4;
+
+	b7 += k11;
+	b6 += b7 + k10;
+	b7 = ((b7 << 47) | (b7 >> (64 - 47))) ^ b6;
+
+	b9 += k13;
+	b8 += b9 + k12;
+	b9 = ((b9 << 8) | (b9 >> (64 - 8))) ^ b8;
+
+	b11 += k15;
+	b10 += b11 + k14;
+	b11 = ((b11 << 17) | (b11 >> (64 - 17))) ^ b10;
+
+	b13 += k0 + t1;
+	b12 += b13 + k16;
+	b13 = ((b13 << 22) | (b13 >> (64 - 22))) ^ b12;
+
+	b15 += k2 + 4;
+	b14 += b15 + k1 + t2;
+	b15 = ((b15 << 37) | (b15 >> (64 - 37))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 38) | (b9 >> (64 - 38))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 19) | (b13 >> (64 - 19))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 10) | (b11 >> (64 - 10))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 55) | (b15 >> (64 - 55))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 49) | (b7 >> (64 - 49))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 18) | (b3 >> (64 - 18))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 23) | (b5 >> (64 - 23))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 52) | (b1 >> (64 - 52))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 33) | (b7 >> (64 - 33))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 4) | (b5 >> (64 - 4))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 51) | (b3 >> (64 - 51))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 34) | (b15 >> (64 - 34))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 41) | (b13 >> (64 - 41))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 59) | (b11 >> (64 - 59))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 17) | (b9 >> (64 - 17))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 5) | (b15 >> (64 - 5))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 20) | (b11 >> (64 - 20))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 48) | (b13 >> (64 - 48))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 41) | (b9 >> (64 - 41))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 47) | (b1 >> (64 - 47))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 28) | (b5 >> (64 - 28))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 25) | (b7 >> (64 - 25))) ^ b12;
+
+	b1 += k6;
+	b0 += b1 + k5;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b0;
+
+	b3 += k8;
+	b2 += b3 + k7;
+	b3 = ((b3 << 9) | (b3 >> (64 - 9))) ^ b2;
+
+	b5 += k10;
+	b4 += b5 + k9;
+	b5 = ((b5 << 37) | (b5 >> (64 - 37))) ^ b4;
+
+	b7 += k12;
+	b6 += b7 + k11;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b6;
+
+	b9 += k14;
+	b8 += b9 + k13;
+	b9 = ((b9 << 12) | (b9 >> (64 - 12))) ^ b8;
+
+	b11 += k16;
+	b10 += b11 + k15;
+	b11 = ((b11 << 47) | (b11 >> (64 - 47))) ^ b10;
+
+	b13 += k1 + t2;
+	b12 += b13 + k0;
+	b13 = ((b13 << 44) | (b13 >> (64 - 44))) ^ b12;
+
+	b15 += k3 + 5;
+	b14 += b15 + k2 + t0;
+	b15 = ((b15 << 30) | (b15 >> (64 - 30))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 16) | (b9 >> (64 - 16))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 34) | (b13 >> (64 - 34))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 56) | (b11 >> (64 - 56))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 51) | (b15 >> (64 - 51))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 4) | (b7 >> (64 - 4))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 53) | (b3 >> (64 - 53))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 42) | (b5 >> (64 - 42))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 44) | (b5 >> (64 - 44))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 47) | (b3 >> (64 - 47))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 19) | (b15 >> (64 - 19))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 42) | (b13 >> (64 - 42))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 44) | (b11 >> (64 - 44))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 25) | (b9 >> (64 - 25))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 9) | (b15 >> (64 - 9))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 48) | (b11 >> (64 - 48))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 35) | (b13 >> (64 - 35))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 52) | (b9 >> (64 - 52))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 31) | (b5 >> (64 - 31))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 37) | (b3 >> (64 - 37))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 20) | (b7 >> (64 - 20))) ^ b12;
+
+	b1 += k7;
+	b0 += b1 + k6;
+	b1 = ((b1 << 24) | (b1 >> (64 - 24))) ^ b0;
+
+	b3 += k9;
+	b2 += b3 + k8;
+	b3 = ((b3 << 13) | (b3 >> (64 - 13))) ^ b2;
+
+	b5 += k11;
+	b4 += b5 + k10;
+	b5 = ((b5 << 8) | (b5 >> (64 - 8))) ^ b4;
+
+	b7 += k13;
+	b6 += b7 + k12;
+	b7 = ((b7 << 47) | (b7 >> (64 - 47))) ^ b6;
+
+	b9 += k15;
+	b8 += b9 + k14;
+	b9 = ((b9 << 8) | (b9 >> (64 - 8))) ^ b8;
+
+	b11 += k0;
+	b10 += b11 + k16;
+	b11 = ((b11 << 17) | (b11 >> (64 - 17))) ^ b10;
+
+	b13 += k2 + t0;
+	b12 += b13 + k1;
+	b13 = ((b13 << 22) | (b13 >> (64 - 22))) ^ b12;
+
+	b15 += k4 + 6;
+	b14 += b15 + k3 + t1;
+	b15 = ((b15 << 37) | (b15 >> (64 - 37))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 38) | (b9 >> (64 - 38))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 19) | (b13 >> (64 - 19))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 10) | (b11 >> (64 - 10))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 55) | (b15 >> (64 - 55))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 49) | (b7 >> (64 - 49))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 18) | (b3 >> (64 - 18))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 23) | (b5 >> (64 - 23))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 52) | (b1 >> (64 - 52))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 33) | (b7 >> (64 - 33))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 4) | (b5 >> (64 - 4))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 51) | (b3 >> (64 - 51))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 34) | (b15 >> (64 - 34))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 41) | (b13 >> (64 - 41))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 59) | (b11 >> (64 - 59))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 17) | (b9 >> (64 - 17))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 5) | (b15 >> (64 - 5))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 20) | (b11 >> (64 - 20))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 48) | (b13 >> (64 - 48))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 41) | (b9 >> (64 - 41))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 47) | (b1 >> (64 - 47))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 28) | (b5 >> (64 - 28))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 25) | (b7 >> (64 - 25))) ^ b12;
+
+	b1 += k8;
+	b0 += b1 + k7;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b0;
+
+	b3 += k10;
+	b2 += b3 + k9;
+	b3 = ((b3 << 9) | (b3 >> (64 - 9))) ^ b2;
+
+	b5 += k12;
+	b4 += b5 + k11;
+	b5 = ((b5 << 37) | (b5 >> (64 - 37))) ^ b4;
+
+	b7 += k14;
+	b6 += b7 + k13;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b6;
+
+	b9 += k16;
+	b8 += b9 + k15;
+	b9 = ((b9 << 12) | (b9 >> (64 - 12))) ^ b8;
+
+	b11 += k1;
+	b10 += b11 + k0;
+	b11 = ((b11 << 47) | (b11 >> (64 - 47))) ^ b10;
+
+	b13 += k3 + t1;
+	b12 += b13 + k2;
+	b13 = ((b13 << 44) | (b13 >> (64 - 44))) ^ b12;
+
+	b15 += k5 + 7;
+	b14 += b15 + k4 + t2;
+	b15 = ((b15 << 30) | (b15 >> (64 - 30))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 16) | (b9 >> (64 - 16))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 34) | (b13 >> (64 - 34))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 56) | (b11 >> (64 - 56))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 51) | (b15 >> (64 - 51))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 4) | (b7 >> (64 - 4))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 53) | (b3 >> (64 - 53))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 42) | (b5 >> (64 - 42))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 44) | (b5 >> (64 - 44))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 47) | (b3 >> (64 - 47))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 19) | (b15 >> (64 - 19))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 42) | (b13 >> (64 - 42))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 44) | (b11 >> (64 - 44))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 25) | (b9 >> (64 - 25))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 9) | (b15 >> (64 - 9))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 48) | (b11 >> (64 - 48))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 35) | (b13 >> (64 - 35))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 52) | (b9 >> (64 - 52))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 31) | (b5 >> (64 - 31))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 37) | (b3 >> (64 - 37))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 20) | (b7 >> (64 - 20))) ^ b12;
+
+	b1 += k9;
+	b0 += b1 + k8;
+	b1 = ((b1 << 24) | (b1 >> (64 - 24))) ^ b0;
+
+	b3 += k11;
+	b2 += b3 + k10;
+	b3 = ((b3 << 13) | (b3 >> (64 - 13))) ^ b2;
+
+	b5 += k13;
+	b4 += b5 + k12;
+	b5 = ((b5 << 8) | (b5 >> (64 - 8))) ^ b4;
+
+	b7 += k15;
+	b6 += b7 + k14;
+	b7 = ((b7 << 47) | (b7 >> (64 - 47))) ^ b6;
+
+	b9 += k0;
+	b8 += b9 + k16;
+	b9 = ((b9 << 8) | (b9 >> (64 - 8))) ^ b8;
+
+	b11 += k2;
+	b10 += b11 + k1;
+	b11 = ((b11 << 17) | (b11 >> (64 - 17))) ^ b10;
+
+	b13 += k4 + t2;
+	b12 += b13 + k3;
+	b13 = ((b13 << 22) | (b13 >> (64 - 22))) ^ b12;
+
+	b15 += k6 + 8;
+	b14 += b15 + k5 + t0;
+	b15 = ((b15 << 37) | (b15 >> (64 - 37))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 38) | (b9 >> (64 - 38))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 19) | (b13 >> (64 - 19))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 10) | (b11 >> (64 - 10))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 55) | (b15 >> (64 - 55))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 49) | (b7 >> (64 - 49))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 18) | (b3 >> (64 - 18))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 23) | (b5 >> (64 - 23))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 52) | (b1 >> (64 - 52))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 33) | (b7 >> (64 - 33))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 4) | (b5 >> (64 - 4))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 51) | (b3 >> (64 - 51))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 34) | (b15 >> (64 - 34))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 41) | (b13 >> (64 - 41))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 59) | (b11 >> (64 - 59))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 17) | (b9 >> (64 - 17))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 5) | (b15 >> (64 - 5))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 20) | (b11 >> (64 - 20))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 48) | (b13 >> (64 - 48))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 41) | (b9 >> (64 - 41))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 47) | (b1 >> (64 - 47))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 28) | (b5 >> (64 - 28))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 25) | (b7 >> (64 - 25))) ^ b12;
+
+	b1 += k10;
+	b0 += b1 + k9;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b0;
+
+	b3 += k12;
+	b2 += b3 + k11;
+	b3 = ((b3 << 9) | (b3 >> (64 - 9))) ^ b2;
+
+	b5 += k14;
+	b4 += b5 + k13;
+	b5 = ((b5 << 37) | (b5 >> (64 - 37))) ^ b4;
+
+	b7 += k16;
+	b6 += b7 + k15;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b6;
+
+	b9 += k1;
+	b8 += b9 + k0;
+	b9 = ((b9 << 12) | (b9 >> (64 - 12))) ^ b8;
+
+	b11 += k3;
+	b10 += b11 + k2;
+	b11 = ((b11 << 47) | (b11 >> (64 - 47))) ^ b10;
+
+	b13 += k5 + t0;
+	b12 += b13 + k4;
+	b13 = ((b13 << 44) | (b13 >> (64 - 44))) ^ b12;
+
+	b15 += k7 + 9;
+	b14 += b15 + k6 + t1;
+	b15 = ((b15 << 30) | (b15 >> (64 - 30))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 16) | (b9 >> (64 - 16))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 34) | (b13 >> (64 - 34))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 56) | (b11 >> (64 - 56))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 51) | (b15 >> (64 - 51))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 4) | (b7 >> (64 - 4))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 53) | (b3 >> (64 - 53))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 42) | (b5 >> (64 - 42))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 44) | (b5 >> (64 - 44))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 47) | (b3 >> (64 - 47))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 19) | (b15 >> (64 - 19))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 42) | (b13 >> (64 - 42))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 44) | (b11 >> (64 - 44))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 25) | (b9 >> (64 - 25))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 9) | (b15 >> (64 - 9))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 48) | (b11 >> (64 - 48))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 35) | (b13 >> (64 - 35))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 52) | (b9 >> (64 - 52))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 31) | (b5 >> (64 - 31))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 37) | (b3 >> (64 - 37))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 20) | (b7 >> (64 - 20))) ^ b12;
+
+	b1 += k11;
+	b0 += b1 + k10;
+	b1 = ((b1 << 24) | (b1 >> (64 - 24))) ^ b0;
+
+	b3 += k13;
+	b2 += b3 + k12;
+	b3 = ((b3 << 13) | (b3 >> (64 - 13))) ^ b2;
+
+	b5 += k15;
+	b4 += b5 + k14;
+	b5 = ((b5 << 8) | (b5 >> (64 - 8))) ^ b4;
+
+	b7 += k0;
+	b6 += b7 + k16;
+	b7 = ((b7 << 47) | (b7 >> (64 - 47))) ^ b6;
+
+	b9 += k2;
+	b8 += b9 + k1;
+	b9 = ((b9 << 8) | (b9 >> (64 - 8))) ^ b8;
+
+	b11 += k4;
+	b10 += b11 + k3;
+	b11 = ((b11 << 17) | (b11 >> (64 - 17))) ^ b10;
+
+	b13 += k6 + t1;
+	b12 += b13 + k5;
+	b13 = ((b13 << 22) | (b13 >> (64 - 22))) ^ b12;
+
+	b15 += k8 + 10;
+	b14 += b15 + k7 + t2;
+	b15 = ((b15 << 37) | (b15 >> (64 - 37))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 38) | (b9 >> (64 - 38))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 19) | (b13 >> (64 - 19))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 10) | (b11 >> (64 - 10))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 55) | (b15 >> (64 - 55))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 49) | (b7 >> (64 - 49))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 18) | (b3 >> (64 - 18))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 23) | (b5 >> (64 - 23))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 52) | (b1 >> (64 - 52))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 33) | (b7 >> (64 - 33))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 4) | (b5 >> (64 - 4))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 51) | (b3 >> (64 - 51))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 34) | (b15 >> (64 - 34))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 41) | (b13 >> (64 - 41))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 59) | (b11 >> (64 - 59))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 17) | (b9 >> (64 - 17))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 5) | (b15 >> (64 - 5))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 20) | (b11 >> (64 - 20))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 48) | (b13 >> (64 - 48))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 41) | (b9 >> (64 - 41))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 47) | (b1 >> (64 - 47))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 28) | (b5 >> (64 - 28))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 25) | (b7 >> (64 - 25))) ^ b12;
+
+	b1 += k12;
+	b0 += b1 + k11;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b0;
+
+	b3 += k14;
+	b2 += b3 + k13;
+	b3 = ((b3 << 9) | (b3 >> (64 - 9))) ^ b2;
+
+	b5 += k16;
+	b4 += b5 + k15;
+	b5 = ((b5 << 37) | (b5 >> (64 - 37))) ^ b4;
+
+	b7 += k1;
+	b6 += b7 + k0;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b6;
+
+	b9 += k3;
+	b8 += b9 + k2;
+	b9 = ((b9 << 12) | (b9 >> (64 - 12))) ^ b8;
+
+	b11 += k5;
+	b10 += b11 + k4;
+	b11 = ((b11 << 47) | (b11 >> (64 - 47))) ^ b10;
+
+	b13 += k7 + t2;
+	b12 += b13 + k6;
+	b13 = ((b13 << 44) | (b13 >> (64 - 44))) ^ b12;
+
+	b15 += k9 + 11;
+	b14 += b15 + k8 + t0;
+	b15 = ((b15 << 30) | (b15 >> (64 - 30))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 16) | (b9 >> (64 - 16))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 34) | (b13 >> (64 - 34))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 56) | (b11 >> (64 - 56))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 51) | (b15 >> (64 - 51))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 4) | (b7 >> (64 - 4))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 53) | (b3 >> (64 - 53))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 42) | (b5 >> (64 - 42))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 44) | (b5 >> (64 - 44))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 47) | (b3 >> (64 - 47))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 19) | (b15 >> (64 - 19))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 42) | (b13 >> (64 - 42))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 44) | (b11 >> (64 - 44))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 25) | (b9 >> (64 - 25))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 9) | (b15 >> (64 - 9))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 48) | (b11 >> (64 - 48))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 35) | (b13 >> (64 - 35))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 52) | (b9 >> (64 - 52))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 31) | (b5 >> (64 - 31))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 37) | (b3 >> (64 - 37))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 20) | (b7 >> (64 - 20))) ^ b12;
+
+	b1 += k13;
+	b0 += b1 + k12;
+	b1 = ((b1 << 24) | (b1 >> (64 - 24))) ^ b0;
+
+	b3 += k15;
+	b2 += b3 + k14;
+	b3 = ((b3 << 13) | (b3 >> (64 - 13))) ^ b2;
+
+	b5 += k0;
+	b4 += b5 + k16;
+	b5 = ((b5 << 8) | (b5 >> (64 - 8))) ^ b4;
+
+	b7 += k2;
+	b6 += b7 + k1;
+	b7 = ((b7 << 47) | (b7 >> (64 - 47))) ^ b6;
+
+	b9 += k4;
+	b8 += b9 + k3;
+	b9 = ((b9 << 8) | (b9 >> (64 - 8))) ^ b8;
+
+	b11 += k6;
+	b10 += b11 + k5;
+	b11 = ((b11 << 17) | (b11 >> (64 - 17))) ^ b10;
+
+	b13 += k8 + t0;
+	b12 += b13 + k7;
+	b13 = ((b13 << 22) | (b13 >> (64 - 22))) ^ b12;
+
+	b15 += k10 + 12;
+	b14 += b15 + k9 + t1;
+	b15 = ((b15 << 37) | (b15 >> (64 - 37))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 38) | (b9 >> (64 - 38))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 19) | (b13 >> (64 - 19))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 10) | (b11 >> (64 - 10))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 55) | (b15 >> (64 - 55))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 49) | (b7 >> (64 - 49))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 18) | (b3 >> (64 - 18))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 23) | (b5 >> (64 - 23))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 52) | (b1 >> (64 - 52))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 33) | (b7 >> (64 - 33))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 4) | (b5 >> (64 - 4))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 51) | (b3 >> (64 - 51))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 34) | (b15 >> (64 - 34))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 41) | (b13 >> (64 - 41))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 59) | (b11 >> (64 - 59))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 17) | (b9 >> (64 - 17))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 5) | (b15 >> (64 - 5))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 20) | (b11 >> (64 - 20))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 48) | (b13 >> (64 - 48))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 41) | (b9 >> (64 - 41))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 47) | (b1 >> (64 - 47))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 28) | (b5 >> (64 - 28))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 25) | (b7 >> (64 - 25))) ^ b12;
+
+	b1 += k14;
+	b0 += b1 + k13;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b0;
+
+	b3 += k16;
+	b2 += b3 + k15;
+	b3 = ((b3 << 9) | (b3 >> (64 - 9))) ^ b2;
+
+	b5 += k1;
+	b4 += b5 + k0;
+	b5 = ((b5 << 37) | (b5 >> (64 - 37))) ^ b4;
+
+	b7 += k3;
+	b6 += b7 + k2;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b6;
+
+	b9 += k5;
+	b8 += b9 + k4;
+	b9 = ((b9 << 12) | (b9 >> (64 - 12))) ^ b8;
+
+	b11 += k7;
+	b10 += b11 + k6;
+	b11 = ((b11 << 47) | (b11 >> (64 - 47))) ^ b10;
+
+	b13 += k9 + t1;
+	b12 += b13 + k8;
+	b13 = ((b13 << 44) | (b13 >> (64 - 44))) ^ b12;
+
+	b15 += k11 + 13;
+	b14 += b15 + k10 + t2;
+	b15 = ((b15 << 30) | (b15 >> (64 - 30))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 16) | (b9 >> (64 - 16))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 34) | (b13 >> (64 - 34))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 56) | (b11 >> (64 - 56))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 51) | (b15 >> (64 - 51))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 4) | (b7 >> (64 - 4))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 53) | (b3 >> (64 - 53))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 42) | (b5 >> (64 - 42))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 44) | (b5 >> (64 - 44))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 47) | (b3 >> (64 - 47))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 19) | (b15 >> (64 - 19))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 42) | (b13 >> (64 - 42))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 44) | (b11 >> (64 - 44))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 25) | (b9 >> (64 - 25))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 9) | (b15 >> (64 - 9))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 48) | (b11 >> (64 - 48))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 35) | (b13 >> (64 - 35))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 52) | (b9 >> (64 - 52))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 31) | (b5 >> (64 - 31))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 37) | (b3 >> (64 - 37))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 20) | (b7 >> (64 - 20))) ^ b12;
+
+	b1 += k15;
+	b0 += b1 + k14;
+	b1 = ((b1 << 24) | (b1 >> (64 - 24))) ^ b0;
+
+	b3 += k0;
+	b2 += b3 + k16;
+	b3 = ((b3 << 13) | (b3 >> (64 - 13))) ^ b2;
+
+	b5 += k2;
+	b4 += b5 + k1;
+	b5 = ((b5 << 8) | (b5 >> (64 - 8))) ^ b4;
+
+	b7 += k4;
+	b6 += b7 + k3;
+	b7 = ((b7 << 47) | (b7 >> (64 - 47))) ^ b6;
+
+	b9 += k6;
+	b8 += b9 + k5;
+	b9 = ((b9 << 8) | (b9 >> (64 - 8))) ^ b8;
+
+	b11 += k8;
+	b10 += b11 + k7;
+	b11 = ((b11 << 17) | (b11 >> (64 - 17))) ^ b10;
+
+	b13 += k10 + t2;
+	b12 += b13 + k9;
+	b13 = ((b13 << 22) | (b13 >> (64 - 22))) ^ b12;
+
+	b15 += k12 + 14;
+	b14 += b15 + k11 + t0;
+	b15 = ((b15 << 37) | (b15 >> (64 - 37))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 38) | (b9 >> (64 - 38))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 19) | (b13 >> (64 - 19))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 10) | (b11 >> (64 - 10))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 55) | (b15 >> (64 - 55))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 49) | (b7 >> (64 - 49))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 18) | (b3 >> (64 - 18))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 23) | (b5 >> (64 - 23))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 52) | (b1 >> (64 - 52))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 33) | (b7 >> (64 - 33))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 4) | (b5 >> (64 - 4))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 51) | (b3 >> (64 - 51))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 34) | (b15 >> (64 - 34))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 41) | (b13 >> (64 - 41))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 59) | (b11 >> (64 - 59))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 17) | (b9 >> (64 - 17))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 5) | (b15 >> (64 - 5))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 20) | (b11 >> (64 - 20))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 48) | (b13 >> (64 - 48))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 41) | (b9 >> (64 - 41))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 47) | (b1 >> (64 - 47))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 28) | (b5 >> (64 - 28))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 25) | (b7 >> (64 - 25))) ^ b12;
+
+	b1 += k16;
+	b0 += b1 + k15;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b0;
+
+	b3 += k1;
+	b2 += b3 + k0;
+	b3 = ((b3 << 9) | (b3 >> (64 - 9))) ^ b2;
+
+	b5 += k3;
+	b4 += b5 + k2;
+	b5 = ((b5 << 37) | (b5 >> (64 - 37))) ^ b4;
+
+	b7 += k5;
+	b6 += b7 + k4;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b6;
+
+	b9 += k7;
+	b8 += b9 + k6;
+	b9 = ((b9 << 12) | (b9 >> (64 - 12))) ^ b8;
+
+	b11 += k9;
+	b10 += b11 + k8;
+	b11 = ((b11 << 47) | (b11 >> (64 - 47))) ^ b10;
+
+	b13 += k11 + t0;
+	b12 += b13 + k10;
+	b13 = ((b13 << 44) | (b13 >> (64 - 44))) ^ b12;
+
+	b15 += k13 + 15;
+	b14 += b15 + k12 + t1;
+	b15 = ((b15 << 30) | (b15 >> (64 - 30))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 16) | (b9 >> (64 - 16))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 34) | (b13 >> (64 - 34))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 56) | (b11 >> (64 - 56))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 51) | (b15 >> (64 - 51))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 4) | (b7 >> (64 - 4))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 53) | (b3 >> (64 - 53))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 42) | (b5 >> (64 - 42))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 44) | (b5 >> (64 - 44))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 47) | (b3 >> (64 - 47))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 19) | (b15 >> (64 - 19))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 42) | (b13 >> (64 - 42))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 44) | (b11 >> (64 - 44))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 25) | (b9 >> (64 - 25))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 9) | (b15 >> (64 - 9))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 48) | (b11 >> (64 - 48))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 35) | (b13 >> (64 - 35))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 52) | (b9 >> (64 - 52))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 31) | (b5 >> (64 - 31))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 37) | (b3 >> (64 - 37))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 20) | (b7 >> (64 - 20))) ^ b12;
+
+	b1 += k0;
+	b0 += b1 + k16;
+	b1 = ((b1 << 24) | (b1 >> (64 - 24))) ^ b0;
+
+	b3 += k2;
+	b2 += b3 + k1;
+	b3 = ((b3 << 13) | (b3 >> (64 - 13))) ^ b2;
+
+	b5 += k4;
+	b4 += b5 + k3;
+	b5 = ((b5 << 8) | (b5 >> (64 - 8))) ^ b4;
+
+	b7 += k6;
+	b6 += b7 + k5;
+	b7 = ((b7 << 47) | (b7 >> (64 - 47))) ^ b6;
+
+	b9 += k8;
+	b8 += b9 + k7;
+	b9 = ((b9 << 8) | (b9 >> (64 - 8))) ^ b8;
+
+	b11 += k10;
+	b10 += b11 + k9;
+	b11 = ((b11 << 17) | (b11 >> (64 - 17))) ^ b10;
+
+	b13 += k12 + t1;
+	b12 += b13 + k11;
+	b13 = ((b13 << 22) | (b13 >> (64 - 22))) ^ b12;
+
+	b15 += k14 + 16;
+	b14 += b15 + k13 + t2;
+	b15 = ((b15 << 37) | (b15 >> (64 - 37))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 38) | (b9 >> (64 - 38))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 19) | (b13 >> (64 - 19))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 10) | (b11 >> (64 - 10))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 55) | (b15 >> (64 - 55))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 49) | (b7 >> (64 - 49))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 18) | (b3 >> (64 - 18))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 23) | (b5 >> (64 - 23))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 52) | (b1 >> (64 - 52))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 33) | (b7 >> (64 - 33))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 4) | (b5 >> (64 - 4))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 51) | (b3 >> (64 - 51))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 34) | (b15 >> (64 - 34))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 41) | (b13 >> (64 - 41))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 59) | (b11 >> (64 - 59))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 17) | (b9 >> (64 - 17))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 5) | (b15 >> (64 - 5))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 20) | (b11 >> (64 - 20))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 48) | (b13 >> (64 - 48))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 41) | (b9 >> (64 - 41))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 47) | (b1 >> (64 - 47))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 28) | (b5 >> (64 - 28))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 25) | (b7 >> (64 - 25))) ^ b12;
+
+	b1 += k1;
+	b0 += b1 + k0;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b0;
+
+	b3 += k3;
+	b2 += b3 + k2;
+	b3 = ((b3 << 9) | (b3 >> (64 - 9))) ^ b2;
+
+	b5 += k5;
+	b4 += b5 + k4;
+	b5 = ((b5 << 37) | (b5 >> (64 - 37))) ^ b4;
+
+	b7 += k7;
+	b6 += b7 + k6;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b6;
+
+	b9 += k9;
+	b8 += b9 + k8;
+	b9 = ((b9 << 12) | (b9 >> (64 - 12))) ^ b8;
+
+	b11 += k11;
+	b10 += b11 + k10;
+	b11 = ((b11 << 47) | (b11 >> (64 - 47))) ^ b10;
+
+	b13 += k13 + t2;
+	b12 += b13 + k12;
+	b13 = ((b13 << 44) | (b13 >> (64 - 44))) ^ b12;
+
+	b15 += k15 + 17;
+	b14 += b15 + k14 + t0;
+	b15 = ((b15 << 30) | (b15 >> (64 - 30))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 16) | (b9 >> (64 - 16))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 34) | (b13 >> (64 - 34))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 56) | (b11 >> (64 - 56))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 51) | (b15 >> (64 - 51))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 4) | (b7 >> (64 - 4))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 53) | (b3 >> (64 - 53))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 42) | (b5 >> (64 - 42))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 44) | (b5 >> (64 - 44))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 47) | (b3 >> (64 - 47))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 19) | (b15 >> (64 - 19))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 42) | (b13 >> (64 - 42))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 44) | (b11 >> (64 - 44))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 25) | (b9 >> (64 - 25))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 9) | (b15 >> (64 - 9))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 48) | (b11 >> (64 - 48))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 35) | (b13 >> (64 - 35))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 52) | (b9 >> (64 - 52))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 31) | (b5 >> (64 - 31))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 37) | (b3 >> (64 - 37))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 20) | (b7 >> (64 - 20))) ^ b12;
+
+	b1 += k2;
+	b0 += b1 + k1;
+	b1 = ((b1 << 24) | (b1 >> (64 - 24))) ^ b0;
+
+	b3 += k4;
+	b2 += b3 + k3;
+	b3 = ((b3 << 13) | (b3 >> (64 - 13))) ^ b2;
+
+	b5 += k6;
+	b4 += b5 + k5;
+	b5 = ((b5 << 8) | (b5 >> (64 - 8))) ^ b4;
+
+	b7 += k8;
+	b6 += b7 + k7;
+	b7 = ((b7 << 47) | (b7 >> (64 - 47))) ^ b6;
+
+	b9 += k10;
+	b8 += b9 + k9;
+	b9 = ((b9 << 8) | (b9 >> (64 - 8))) ^ b8;
+
+	b11 += k12;
+	b10 += b11 + k11;
+	b11 = ((b11 << 17) | (b11 >> (64 - 17))) ^ b10;
+
+	b13 += k14 + t0;
+	b12 += b13 + k13;
+	b13 = ((b13 << 22) | (b13 >> (64 - 22))) ^ b12;
+
+	b15 += k16 + 18;
+	b14 += b15 + k15 + t1;
+	b15 = ((b15 << 37) | (b15 >> (64 - 37))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 38) | (b9 >> (64 - 38))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 19) | (b13 >> (64 - 19))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 10) | (b11 >> (64 - 10))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 55) | (b15 >> (64 - 55))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 49) | (b7 >> (64 - 49))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 18) | (b3 >> (64 - 18))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 23) | (b5 >> (64 - 23))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 52) | (b1 >> (64 - 52))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 33) | (b7 >> (64 - 33))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 4) | (b5 >> (64 - 4))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 51) | (b3 >> (64 - 51))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 13) | (b1 >> (64 - 13))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 34) | (b15 >> (64 - 34))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 41) | (b13 >> (64 - 41))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 59) | (b11 >> (64 - 59))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 17) | (b9 >> (64 - 17))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 5) | (b15 >> (64 - 5))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 20) | (b11 >> (64 - 20))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 48) | (b13 >> (64 - 48))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 41) | (b9 >> (64 - 41))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 47) | (b1 >> (64 - 47))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 28) | (b5 >> (64 - 28))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 16) | (b3 >> (64 - 16))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 25) | (b7 >> (64 - 25))) ^ b12;
+
+	b1 += k3;
+	b0 += b1 + k2;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b0;
+
+	b3 += k5;
+	b2 += b3 + k4;
+	b3 = ((b3 << 9) | (b3 >> (64 - 9))) ^ b2;
+
+	b5 += k7;
+	b4 += b5 + k6;
+	b5 = ((b5 << 37) | (b5 >> (64 - 37))) ^ b4;
+
+	b7 += k9;
+	b6 += b7 + k8;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b6;
+
+	b9 += k11;
+	b8 += b9 + k10;
+	b9 = ((b9 << 12) | (b9 >> (64 - 12))) ^ b8;
+
+	b11 += k13;
+	b10 += b11 + k12;
+	b11 = ((b11 << 47) | (b11 >> (64 - 47))) ^ b10;
+
+	b13 += k15 + t1;
+	b12 += b13 + k14;
+	b13 = ((b13 << 44) | (b13 >> (64 - 44))) ^ b12;
+
+	b15 += k0 + 19;
+	b14 += b15 + k16 + t2;
+	b15 = ((b15 << 30) | (b15 >> (64 - 30))) ^ b14;
+
+	b0 += b9;
+	b9 = ((b9 << 16) | (b9 >> (64 - 16))) ^ b0;
+
+	b2 += b13;
+	b13 = ((b13 << 34) | (b13 >> (64 - 34))) ^ b2;
+
+	b6 += b11;
+	b11 = ((b11 << 56) | (b11 >> (64 - 56))) ^ b6;
+
+	b4 += b15;
+	b15 = ((b15 << 51) | (b15 >> (64 - 51))) ^ b4;
+
+	b10 += b7;
+	b7 = ((b7 << 4) | (b7 >> (64 - 4))) ^ b10;
+
+	b12 += b3;
+	b3 = ((b3 << 53) | (b3 >> (64 - 53))) ^ b12;
+
+	b14 += b5;
+	b5 = ((b5 << 42) | (b5 >> (64 - 42))) ^ b14;
+
+	b8 += b1;
+	b1 = ((b1 << 41) | (b1 >> (64 - 41))) ^ b8;
+
+	b0 += b7;
+	b7 = ((b7 << 31) | (b7 >> (64 - 31))) ^ b0;
+
+	b2 += b5;
+	b5 = ((b5 << 44) | (b5 >> (64 - 44))) ^ b2;
+
+	b4 += b3;
+	b3 = ((b3 << 47) | (b3 >> (64 - 47))) ^ b4;
+
+	b6 += b1;
+	b1 = ((b1 << 46) | (b1 >> (64 - 46))) ^ b6;
+
+	b12 += b15;
+	b15 = ((b15 << 19) | (b15 >> (64 - 19))) ^ b12;
+
+	b14 += b13;
+	b13 = ((b13 << 42) | (b13 >> (64 - 42))) ^ b14;
+
+	b8 += b11;
+	b11 = ((b11 << 44) | (b11 >> (64 - 44))) ^ b8;
+
+	b10 += b9;
+	b9 = ((b9 << 25) | (b9 >> (64 - 25))) ^ b10;
+
+	b0 += b15;
+	b15 = ((b15 << 9) | (b15 >> (64 - 9))) ^ b0;
+
+	b2 += b11;
+	b11 = ((b11 << 48) | (b11 >> (64 - 48))) ^ b2;
+
+	b6 += b13;
+	b13 = ((b13 << 35) | (b13 >> (64 - 35))) ^ b6;
+
+	b4 += b9;
+	b9 = ((b9 << 52) | (b9 >> (64 - 52))) ^ b4;
+
+	b14 += b1;
+	b1 = ((b1 << 23) | (b1 >> (64 - 23))) ^ b14;
+
+	b8 += b5;
+	b5 = ((b5 << 31) | (b5 >> (64 - 31))) ^ b8;
+
+	b10 += b3;
+	b3 = ((b3 << 37) | (b3 >> (64 - 37))) ^ b10;
+
+	b12 += b7;
+	b7 = ((b7 << 20) | (b7 >> (64 - 20))) ^ b12;
+
+	output[0] = b0 + k3;
+	output[1] = b1 + k4;
+	output[2] = b2 + k5;
+	output[3] = b3 + k6;
+	output[4] = b4 + k7;
+	output[5] = b5 + k8;
+	output[6] = b6 + k9;
+	output[7] = b7 + k10;
+	output[8] = b8 + k11;
+	output[9] = b9 + k12;
+	output[10] = b10 + k13;
+	output[11] = b11 + k14;
+	output[12] = b12 + k15;
+	output[13] = b13 + k16 + t2;
+	output[14] = b14 + k0 + t0;
+	output[15] = b15 + k1 + 20;
+}
+
+void threefish_decrypt_1024(struct threefish_key *key_ctx, u64 *input,
+			    u64 *output)
+{
+	u64 b0 = input[0], b1 = input[1],
+	    b2 = input[2], b3 = input[3],
+	    b4 = input[4], b5 = input[5],
+	    b6 = input[6], b7 = input[7],
+	    b8 = input[8], b9 = input[9],
+	    b10 = input[10], b11 = input[11],
+	    b12 = input[12], b13 = input[13],
+	    b14 = input[14], b15 = input[15];
+	u64 k0 = key_ctx->key[0], k1 = key_ctx->key[1],
+	    k2 = key_ctx->key[2], k3 = key_ctx->key[3],
+	    k4 = key_ctx->key[4], k5 = key_ctx->key[5],
+	    k6 = key_ctx->key[6], k7 = key_ctx->key[7],
+	    k8 = key_ctx->key[8], k9 = key_ctx->key[9],
+	    k10 = key_ctx->key[10], k11 = key_ctx->key[11],
+	    k12 = key_ctx->key[12], k13 = key_ctx->key[13],
+	    k14 = key_ctx->key[14], k15 = key_ctx->key[15],
+	    k16 = key_ctx->key[16];
+	u64 t0 = key_ctx->tweak[0], t1 = key_ctx->tweak[1],
+	    t2 = key_ctx->tweak[2];
+	u64 tmp;
+
+	b0 -= k3;
+	b1 -= k4;
+	b2 -= k5;
+	b3 -= k6;
+	b4 -= k7;
+	b5 -= k8;
+	b6 -= k9;
+	b7 -= k10;
+	b8 -= k11;
+	b9 -= k12;
+	b10 -= k13;
+	b11 -= k14;
+	b12 -= k15;
+	b13 -= k16 + t2;
+	b14 -= k0 + t0;
+	b15 -= k1 + 20;
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 20) | (tmp << (64 - 20));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 37) | (tmp << (64 - 37));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 31) | (tmp << (64 - 31));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 52) | (tmp << (64 - 52));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 35) | (tmp << (64 - 35));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 48) | (tmp << (64 - 48));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 25) | (tmp << (64 - 25));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 44) | (tmp << (64 - 44));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 19) | (tmp << (64 - 19));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 47) | (tmp << (64 - 47));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 44) | (tmp << (64 - 44));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 53) | (tmp << (64 - 53));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 4) | (tmp << (64 - 4));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 56) | (tmp << (64 - 56));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 34) | (tmp << (64 - 34));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 16) | (tmp << (64 - 16));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 30) | (tmp << (64 - 30));
+	b14 -= b15 + k16 + t2;
+	b15 -= k0 + 19;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 44) | (tmp << (64 - 44));
+	b12 -= b13 + k14;
+	b13 -= k15 + t1;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 47) | (tmp << (64 - 47));
+	b10 -= b11 + k12;
+	b11 -= k13;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 12) | (tmp << (64 - 12));
+	b8 -= b9 + k10;
+	b9 -= k11;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b6 -= b7 + k8;
+	b7 -= k9;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 37) | (tmp << (64 - 37));
+	b4 -= b5 + k6;
+	b5 -= k7;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 9) | (tmp << (64 - 9));
+	b2 -= b3 + k4;
+	b3 -= k5;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b0 -= b1 + k2;
+	b1 -= k3;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 25) | (tmp << (64 - 25));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 28) | (tmp << (64 - 28));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 47) | (tmp << (64 - 47));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 41) | (tmp << (64 - 41));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 48) | (tmp << (64 - 48));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 20) | (tmp << (64 - 20));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 59) | (tmp << (64 - 59));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 41) | (tmp << (64 - 41));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 34) | (tmp << (64 - 34));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 4) | (tmp << (64 - 4));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 33) | (tmp << (64 - 33));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 52) | (tmp << (64 - 52));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 18) | (tmp << (64 - 18));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 49) | (tmp << (64 - 49));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 55) | (tmp << (64 - 55));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 19) | (tmp << (64 - 19));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 38) | (tmp << (64 - 38));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 37) | (tmp << (64 - 37));
+	b14 -= b15 + k15 + t1;
+	b15 -= k16 + 18;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 22) | (tmp << (64 - 22));
+	b12 -= b13 + k13;
+	b13 -= k14 + t0;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b11 + k11;
+	b11 -= k12;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 8) | (tmp << (64 - 8));
+	b8 -= b9 + k9;
+	b9 -= k10;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 47) | (tmp << (64 - 47));
+	b6 -= b7 + k7;
+	b7 -= k8;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 8) | (tmp << (64 - 8));
+	b4 -= b5 + k5;
+	b5 -= k6;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b3 + k3;
+	b3 -= k4;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 24) | (tmp << (64 - 24));
+	b0 -= b1 + k1;
+	b1 -= k2;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 20) | (tmp << (64 - 20));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 37) | (tmp << (64 - 37));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 31) | (tmp << (64 - 31));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 52) | (tmp << (64 - 52));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 35) | (tmp << (64 - 35));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 48) | (tmp << (64 - 48));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 25) | (tmp << (64 - 25));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 44) | (tmp << (64 - 44));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 19) | (tmp << (64 - 19));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 47) | (tmp << (64 - 47));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 44) | (tmp << (64 - 44));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 53) | (tmp << (64 - 53));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 4) | (tmp << (64 - 4));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 56) | (tmp << (64 - 56));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 34) | (tmp << (64 - 34));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 16) | (tmp << (64 - 16));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 30) | (tmp << (64 - 30));
+	b14 -= b15 + k14 + t0;
+	b15 -= k15 + 17;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 44) | (tmp << (64 - 44));
+	b12 -= b13 + k12;
+	b13 -= k13 + t2;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 47) | (tmp << (64 - 47));
+	b10 -= b11 + k10;
+	b11 -= k11;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 12) | (tmp << (64 - 12));
+	b8 -= b9 + k8;
+	b9 -= k9;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b6 -= b7 + k6;
+	b7 -= k7;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 37) | (tmp << (64 - 37));
+	b4 -= b5 + k4;
+	b5 -= k5;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 9) | (tmp << (64 - 9));
+	b2 -= b3 + k2;
+	b3 -= k3;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b0 -= b1 + k0;
+	b1 -= k1;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 25) | (tmp << (64 - 25));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 28) | (tmp << (64 - 28));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 47) | (tmp << (64 - 47));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 41) | (tmp << (64 - 41));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 48) | (tmp << (64 - 48));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 20) | (tmp << (64 - 20));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 59) | (tmp << (64 - 59));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 41) | (tmp << (64 - 41));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 34) | (tmp << (64 - 34));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 4) | (tmp << (64 - 4));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 33) | (tmp << (64 - 33));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 52) | (tmp << (64 - 52));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 18) | (tmp << (64 - 18));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 49) | (tmp << (64 - 49));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 55) | (tmp << (64 - 55));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 19) | (tmp << (64 - 19));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 38) | (tmp << (64 - 38));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 37) | (tmp << (64 - 37));
+	b14 -= b15 + k13 + t2;
+	b15 -= k14 + 16;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 22) | (tmp << (64 - 22));
+	b12 -= b13 + k11;
+	b13 -= k12 + t1;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b11 + k9;
+	b11 -= k10;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 8) | (tmp << (64 - 8));
+	b8 -= b9 + k7;
+	b9 -= k8;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 47) | (tmp << (64 - 47));
+	b6 -= b7 + k5;
+	b7 -= k6;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 8) | (tmp << (64 - 8));
+	b4 -= b5 + k3;
+	b5 -= k4;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b3 + k1;
+	b3 -= k2;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 24) | (tmp << (64 - 24));
+	b0 -= b1 + k16;
+	b1 -= k0;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 20) | (tmp << (64 - 20));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 37) | (tmp << (64 - 37));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 31) | (tmp << (64 - 31));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 52) | (tmp << (64 - 52));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 35) | (tmp << (64 - 35));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 48) | (tmp << (64 - 48));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 25) | (tmp << (64 - 25));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 44) | (tmp << (64 - 44));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 19) | (tmp << (64 - 19));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 47) | (tmp << (64 - 47));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 44) | (tmp << (64 - 44));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 53) | (tmp << (64 - 53));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 4) | (tmp << (64 - 4));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 56) | (tmp << (64 - 56));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 34) | (tmp << (64 - 34));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 16) | (tmp << (64 - 16));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 30) | (tmp << (64 - 30));
+	b14 -= b15 + k12 + t1;
+	b15 -= k13 + 15;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 44) | (tmp << (64 - 44));
+	b12 -= b13 + k10;
+	b13 -= k11 + t0;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 47) | (tmp << (64 - 47));
+	b10 -= b11 + k8;
+	b11 -= k9;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 12) | (tmp << (64 - 12));
+	b8 -= b9 + k6;
+	b9 -= k7;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b6 -= b7 + k4;
+	b7 -= k5;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 37) | (tmp << (64 - 37));
+	b4 -= b5 + k2;
+	b5 -= k3;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 9) | (tmp << (64 - 9));
+	b2 -= b3 + k0;
+	b3 -= k1;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b0 -= b1 + k15;
+	b1 -= k16;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 25) | (tmp << (64 - 25));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 28) | (tmp << (64 - 28));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 47) | (tmp << (64 - 47));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 41) | (tmp << (64 - 41));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 48) | (tmp << (64 - 48));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 20) | (tmp << (64 - 20));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 59) | (tmp << (64 - 59));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 41) | (tmp << (64 - 41));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 34) | (tmp << (64 - 34));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 4) | (tmp << (64 - 4));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 33) | (tmp << (64 - 33));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 52) | (tmp << (64 - 52));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 18) | (tmp << (64 - 18));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 49) | (tmp << (64 - 49));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 55) | (tmp << (64 - 55));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 19) | (tmp << (64 - 19));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 38) | (tmp << (64 - 38));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 37) | (tmp << (64 - 37));
+	b14 -= b15 + k11 + t0;
+	b15 -= k12 + 14;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 22) | (tmp << (64 - 22));
+	b12 -= b13 + k9;
+	b13 -= k10 + t2;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b11 + k7;
+	b11 -= k8;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 8) | (tmp << (64 - 8));
+	b8 -= b9 + k5;
+	b9 -= k6;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 47) | (tmp << (64 - 47));
+	b6 -= b7 + k3;
+	b7 -= k4;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 8) | (tmp << (64 - 8));
+	b4 -= b5 + k1;
+	b5 -= k2;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b3 + k16;
+	b3 -= k0;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 24) | (tmp << (64 - 24));
+	b0 -= b1 + k14;
+	b1 -= k15;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 20) | (tmp << (64 - 20));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 37) | (tmp << (64 - 37));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 31) | (tmp << (64 - 31));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 52) | (tmp << (64 - 52));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 35) | (tmp << (64 - 35));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 48) | (tmp << (64 - 48));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 25) | (tmp << (64 - 25));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 44) | (tmp << (64 - 44));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 19) | (tmp << (64 - 19));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 47) | (tmp << (64 - 47));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 44) | (tmp << (64 - 44));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 53) | (tmp << (64 - 53));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 4) | (tmp << (64 - 4));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 56) | (tmp << (64 - 56));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 34) | (tmp << (64 - 34));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 16) | (tmp << (64 - 16));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 30) | (tmp << (64 - 30));
+	b14 -= b15 + k10 + t2;
+	b15 -= k11 + 13;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 44) | (tmp << (64 - 44));
+	b12 -= b13 + k8;
+	b13 -= k9 + t1;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 47) | (tmp << (64 - 47));
+	b10 -= b11 + k6;
+	b11 -= k7;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 12) | (tmp << (64 - 12));
+	b8 -= b9 + k4;
+	b9 -= k5;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b6 -= b7 + k2;
+	b7 -= k3;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 37) | (tmp << (64 - 37));
+	b4 -= b5 + k0;
+	b5 -= k1;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 9) | (tmp << (64 - 9));
+	b2 -= b3 + k15;
+	b3 -= k16;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b0 -= b1 + k13;
+	b1 -= k14;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 25) | (tmp << (64 - 25));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 28) | (tmp << (64 - 28));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 47) | (tmp << (64 - 47));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 41) | (tmp << (64 - 41));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 48) | (tmp << (64 - 48));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 20) | (tmp << (64 - 20));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 59) | (tmp << (64 - 59));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 41) | (tmp << (64 - 41));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 34) | (tmp << (64 - 34));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 4) | (tmp << (64 - 4));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 33) | (tmp << (64 - 33));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 52) | (tmp << (64 - 52));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 18) | (tmp << (64 - 18));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 49) | (tmp << (64 - 49));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 55) | (tmp << (64 - 55));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 19) | (tmp << (64 - 19));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 38) | (tmp << (64 - 38));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 37) | (tmp << (64 - 37));
+	b14 -= b15 + k9 + t1;
+	b15 -= k10 + 12;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 22) | (tmp << (64 - 22));
+	b12 -= b13 + k7;
+	b13 -= k8 + t0;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b11 + k5;
+	b11 -= k6;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 8) | (tmp << (64 - 8));
+	b8 -= b9 + k3;
+	b9 -= k4;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 47) | (tmp << (64 - 47));
+	b6 -= b7 + k1;
+	b7 -= k2;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 8) | (tmp << (64 - 8));
+	b4 -= b5 + k16;
+	b5 -= k0;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b3 + k14;
+	b3 -= k15;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 24) | (tmp << (64 - 24));
+	b0 -= b1 + k12;
+	b1 -= k13;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 20) | (tmp << (64 - 20));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 37) | (tmp << (64 - 37));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 31) | (tmp << (64 - 31));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 52) | (tmp << (64 - 52));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 35) | (tmp << (64 - 35));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 48) | (tmp << (64 - 48));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 25) | (tmp << (64 - 25));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 44) | (tmp << (64 - 44));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 19) | (tmp << (64 - 19));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 47) | (tmp << (64 - 47));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 44) | (tmp << (64 - 44));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 53) | (tmp << (64 - 53));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 4) | (tmp << (64 - 4));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 56) | (tmp << (64 - 56));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 34) | (tmp << (64 - 34));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 16) | (tmp << (64 - 16));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 30) | (tmp << (64 - 30));
+	b14 -= b15 + k8 + t0;
+	b15 -= k9 + 11;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 44) | (tmp << (64 - 44));
+	b12 -= b13 + k6;
+	b13 -= k7 + t2;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 47) | (tmp << (64 - 47));
+	b10 -= b11 + k4;
+	b11 -= k5;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 12) | (tmp << (64 - 12));
+	b8 -= b9 + k2;
+	b9 -= k3;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b6 -= b7 + k0;
+	b7 -= k1;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 37) | (tmp << (64 - 37));
+	b4 -= b5 + k15;
+	b5 -= k16;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 9) | (tmp << (64 - 9));
+	b2 -= b3 + k13;
+	b3 -= k14;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b0 -= b1 + k11;
+	b1 -= k12;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 25) | (tmp << (64 - 25));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 28) | (tmp << (64 - 28));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 47) | (tmp << (64 - 47));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 41) | (tmp << (64 - 41));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 48) | (tmp << (64 - 48));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 20) | (tmp << (64 - 20));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 59) | (tmp << (64 - 59));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 41) | (tmp << (64 - 41));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 34) | (tmp << (64 - 34));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 4) | (tmp << (64 - 4));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 33) | (tmp << (64 - 33));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 52) | (tmp << (64 - 52));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 18) | (tmp << (64 - 18));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 49) | (tmp << (64 - 49));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 55) | (tmp << (64 - 55));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 19) | (tmp << (64 - 19));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 38) | (tmp << (64 - 38));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 37) | (tmp << (64 - 37));
+	b14 -= b15 + k7 + t2;
+	b15 -= k8 + 10;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 22) | (tmp << (64 - 22));
+	b12 -= b13 + k5;
+	b13 -= k6 + t1;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b11 + k3;
+	b11 -= k4;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 8) | (tmp << (64 - 8));
+	b8 -= b9 + k1;
+	b9 -= k2;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 47) | (tmp << (64 - 47));
+	b6 -= b7 + k16;
+	b7 -= k0;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 8) | (tmp << (64 - 8));
+	b4 -= b5 + k14;
+	b5 -= k15;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b3 + k12;
+	b3 -= k13;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 24) | (tmp << (64 - 24));
+	b0 -= b1 + k10;
+	b1 -= k11;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 20) | (tmp << (64 - 20));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 37) | (tmp << (64 - 37));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 31) | (tmp << (64 - 31));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 52) | (tmp << (64 - 52));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 35) | (tmp << (64 - 35));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 48) | (tmp << (64 - 48));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 25) | (tmp << (64 - 25));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 44) | (tmp << (64 - 44));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 19) | (tmp << (64 - 19));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 47) | (tmp << (64 - 47));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 44) | (tmp << (64 - 44));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 53) | (tmp << (64 - 53));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 4) | (tmp << (64 - 4));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 56) | (tmp << (64 - 56));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 34) | (tmp << (64 - 34));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 16) | (tmp << (64 - 16));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 30) | (tmp << (64 - 30));
+	b14 -= b15 + k6 + t1;
+	b15 -= k7 + 9;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 44) | (tmp << (64 - 44));
+	b12 -= b13 + k4;
+	b13 -= k5 + t0;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 47) | (tmp << (64 - 47));
+	b10 -= b11 + k2;
+	b11 -= k3;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 12) | (tmp << (64 - 12));
+	b8 -= b9 + k0;
+	b9 -= k1;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b6 -= b7 + k15;
+	b7 -= k16;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 37) | (tmp << (64 - 37));
+	b4 -= b5 + k13;
+	b5 -= k14;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 9) | (tmp << (64 - 9));
+	b2 -= b3 + k11;
+	b3 -= k12;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b0 -= b1 + k9;
+	b1 -= k10;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 25) | (tmp << (64 - 25));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 28) | (tmp << (64 - 28));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 47) | (tmp << (64 - 47));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 41) | (tmp << (64 - 41));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 48) | (tmp << (64 - 48));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 20) | (tmp << (64 - 20));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 59) | (tmp << (64 - 59));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 41) | (tmp << (64 - 41));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 34) | (tmp << (64 - 34));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 4) | (tmp << (64 - 4));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 33) | (tmp << (64 - 33));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 52) | (tmp << (64 - 52));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 18) | (tmp << (64 - 18));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 49) | (tmp << (64 - 49));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 55) | (tmp << (64 - 55));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 19) | (tmp << (64 - 19));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 38) | (tmp << (64 - 38));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 37) | (tmp << (64 - 37));
+	b14 -= b15 + k5 + t0;
+	b15 -= k6 + 8;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 22) | (tmp << (64 - 22));
+	b12 -= b13 + k3;
+	b13 -= k4 + t2;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b11 + k1;
+	b11 -= k2;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 8) | (tmp << (64 - 8));
+	b8 -= b9 + k16;
+	b9 -= k0;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 47) | (tmp << (64 - 47));
+	b6 -= b7 + k14;
+	b7 -= k15;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 8) | (tmp << (64 - 8));
+	b4 -= b5 + k12;
+	b5 -= k13;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b3 + k10;
+	b3 -= k11;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 24) | (tmp << (64 - 24));
+	b0 -= b1 + k8;
+	b1 -= k9;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 20) | (tmp << (64 - 20));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 37) | (tmp << (64 - 37));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 31) | (tmp << (64 - 31));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 52) | (tmp << (64 - 52));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 35) | (tmp << (64 - 35));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 48) | (tmp << (64 - 48));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 25) | (tmp << (64 - 25));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 44) | (tmp << (64 - 44));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 19) | (tmp << (64 - 19));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 47) | (tmp << (64 - 47));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 44) | (tmp << (64 - 44));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 53) | (tmp << (64 - 53));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 4) | (tmp << (64 - 4));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 56) | (tmp << (64 - 56));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 34) | (tmp << (64 - 34));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 16) | (tmp << (64 - 16));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 30) | (tmp << (64 - 30));
+	b14 -= b15 + k4 + t2;
+	b15 -= k5 + 7;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 44) | (tmp << (64 - 44));
+	b12 -= b13 + k2;
+	b13 -= k3 + t1;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 47) | (tmp << (64 - 47));
+	b10 -= b11 + k0;
+	b11 -= k1;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 12) | (tmp << (64 - 12));
+	b8 -= b9 + k15;
+	b9 -= k16;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b6 -= b7 + k13;
+	b7 -= k14;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 37) | (tmp << (64 - 37));
+	b4 -= b5 + k11;
+	b5 -= k12;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 9) | (tmp << (64 - 9));
+	b2 -= b3 + k9;
+	b3 -= k10;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b0 -= b1 + k7;
+	b1 -= k8;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 25) | (tmp << (64 - 25));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 28) | (tmp << (64 - 28));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 47) | (tmp << (64 - 47));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 41) | (tmp << (64 - 41));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 48) | (tmp << (64 - 48));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 20) | (tmp << (64 - 20));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 59) | (tmp << (64 - 59));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 41) | (tmp << (64 - 41));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 34) | (tmp << (64 - 34));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 4) | (tmp << (64 - 4));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 33) | (tmp << (64 - 33));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 52) | (tmp << (64 - 52));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 18) | (tmp << (64 - 18));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 49) | (tmp << (64 - 49));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 55) | (tmp << (64 - 55));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 19) | (tmp << (64 - 19));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 38) | (tmp << (64 - 38));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 37) | (tmp << (64 - 37));
+	b14 -= b15 + k3 + t1;
+	b15 -= k4 + 6;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 22) | (tmp << (64 - 22));
+	b12 -= b13 + k1;
+	b13 -= k2 + t0;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b11 + k16;
+	b11 -= k0;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 8) | (tmp << (64 - 8));
+	b8 -= b9 + k14;
+	b9 -= k15;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 47) | (tmp << (64 - 47));
+	b6 -= b7 + k12;
+	b7 -= k13;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 8) | (tmp << (64 - 8));
+	b4 -= b5 + k10;
+	b5 -= k11;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b3 + k8;
+	b3 -= k9;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 24) | (tmp << (64 - 24));
+	b0 -= b1 + k6;
+	b1 -= k7;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 20) | (tmp << (64 - 20));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 37) | (tmp << (64 - 37));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 31) | (tmp << (64 - 31));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 52) | (tmp << (64 - 52));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 35) | (tmp << (64 - 35));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 48) | (tmp << (64 - 48));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 25) | (tmp << (64 - 25));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 44) | (tmp << (64 - 44));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 19) | (tmp << (64 - 19));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 47) | (tmp << (64 - 47));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 44) | (tmp << (64 - 44));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 53) | (tmp << (64 - 53));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 4) | (tmp << (64 - 4));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 56) | (tmp << (64 - 56));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 34) | (tmp << (64 - 34));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 16) | (tmp << (64 - 16));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 30) | (tmp << (64 - 30));
+	b14 -= b15 + k2 + t0;
+	b15 -= k3 + 5;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 44) | (tmp << (64 - 44));
+	b12 -= b13 + k0;
+	b13 -= k1 + t2;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 47) | (tmp << (64 - 47));
+	b10 -= b11 + k15;
+	b11 -= k16;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 12) | (tmp << (64 - 12));
+	b8 -= b9 + k13;
+	b9 -= k14;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b6 -= b7 + k11;
+	b7 -= k12;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 37) | (tmp << (64 - 37));
+	b4 -= b5 + k9;
+	b5 -= k10;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 9) | (tmp << (64 - 9));
+	b2 -= b3 + k7;
+	b3 -= k8;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b0 -= b1 + k5;
+	b1 -= k6;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 25) | (tmp << (64 - 25));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 28) | (tmp << (64 - 28));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 47) | (tmp << (64 - 47));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 41) | (tmp << (64 - 41));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 48) | (tmp << (64 - 48));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 20) | (tmp << (64 - 20));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 59) | (tmp << (64 - 59));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 41) | (tmp << (64 - 41));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 34) | (tmp << (64 - 34));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 4) | (tmp << (64 - 4));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 33) | (tmp << (64 - 33));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 52) | (tmp << (64 - 52));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 18) | (tmp << (64 - 18));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 49) | (tmp << (64 - 49));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 55) | (tmp << (64 - 55));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 19) | (tmp << (64 - 19));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 38) | (tmp << (64 - 38));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 37) | (tmp << (64 - 37));
+	b14 -= b15 + k1 + t2;
+	b15 -= k2 + 4;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 22) | (tmp << (64 - 22));
+	b12 -= b13 + k16;
+	b13 -= k0 + t1;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b11 + k14;
+	b11 -= k15;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 8) | (tmp << (64 - 8));
+	b8 -= b9 + k12;
+	b9 -= k13;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 47) | (tmp << (64 - 47));
+	b6 -= b7 + k10;
+	b7 -= k11;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 8) | (tmp << (64 - 8));
+	b4 -= b5 + k8;
+	b5 -= k9;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b3 + k6;
+	b3 -= k7;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 24) | (tmp << (64 - 24));
+	b0 -= b1 + k4;
+	b1 -= k5;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 20) | (tmp << (64 - 20));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 37) | (tmp << (64 - 37));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 31) | (tmp << (64 - 31));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 52) | (tmp << (64 - 52));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 35) | (tmp << (64 - 35));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 48) | (tmp << (64 - 48));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 25) | (tmp << (64 - 25));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 44) | (tmp << (64 - 44));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 19) | (tmp << (64 - 19));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 47) | (tmp << (64 - 47));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 44) | (tmp << (64 - 44));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 53) | (tmp << (64 - 53));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 4) | (tmp << (64 - 4));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 56) | (tmp << (64 - 56));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 34) | (tmp << (64 - 34));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 16) | (tmp << (64 - 16));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 30) | (tmp << (64 - 30));
+	b14 -= b15 + k0 + t1;
+	b15 -= k1 + 3;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 44) | (tmp << (64 - 44));
+	b12 -= b13 + k15;
+	b13 -= k16 + t0;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 47) | (tmp << (64 - 47));
+	b10 -= b11 + k13;
+	b11 -= k14;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 12) | (tmp << (64 - 12));
+	b8 -= b9 + k11;
+	b9 -= k12;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b6 -= b7 + k9;
+	b7 -= k10;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 37) | (tmp << (64 - 37));
+	b4 -= b5 + k7;
+	b5 -= k8;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 9) | (tmp << (64 - 9));
+	b2 -= b3 + k5;
+	b3 -= k6;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b0 -= b1 + k3;
+	b1 -= k4;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 25) | (tmp << (64 - 25));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 28) | (tmp << (64 - 28));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 47) | (tmp << (64 - 47));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 41) | (tmp << (64 - 41));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 48) | (tmp << (64 - 48));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 20) | (tmp << (64 - 20));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 59) | (tmp << (64 - 59));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 41) | (tmp << (64 - 41));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 34) | (tmp << (64 - 34));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 4) | (tmp << (64 - 4));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 33) | (tmp << (64 - 33));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 52) | (tmp << (64 - 52));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 18) | (tmp << (64 - 18));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 49) | (tmp << (64 - 49));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 55) | (tmp << (64 - 55));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 19) | (tmp << (64 - 19));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 38) | (tmp << (64 - 38));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 37) | (tmp << (64 - 37));
+	b14 -= b15 + k16 + t0;
+	b15 -= k0 + 2;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 22) | (tmp << (64 - 22));
+	b12 -= b13 + k14;
+	b13 -= k15 + t2;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b11 + k12;
+	b11 -= k13;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 8) | (tmp << (64 - 8));
+	b8 -= b9 + k10;
+	b9 -= k11;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 47) | (tmp << (64 - 47));
+	b6 -= b7 + k8;
+	b7 -= k9;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 8) | (tmp << (64 - 8));
+	b4 -= b5 + k6;
+	b5 -= k7;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b3 + k4;
+	b3 -= k5;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 24) | (tmp << (64 - 24));
+	b0 -= b1 + k2;
+	b1 -= k3;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 20) | (tmp << (64 - 20));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 37) | (tmp << (64 - 37));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 31) | (tmp << (64 - 31));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 52) | (tmp << (64 - 52));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 35) | (tmp << (64 - 35));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 48) | (tmp << (64 - 48));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 9) | (tmp << (64 - 9));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 25) | (tmp << (64 - 25));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 44) | (tmp << (64 - 44));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 19) | (tmp << (64 - 19));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 46) | (tmp << (64 - 46));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 47) | (tmp << (64 - 47));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 44) | (tmp << (64 - 44));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 42) | (tmp << (64 - 42));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 53) | (tmp << (64 - 53));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 4) | (tmp << (64 - 4));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 56) | (tmp << (64 - 56));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 34) | (tmp << (64 - 34));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 16) | (tmp << (64 - 16));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 30) | (tmp << (64 - 30));
+	b14 -= b15 + k15 + t2;
+	b15 -= k16 + 1;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 44) | (tmp << (64 - 44));
+	b12 -= b13 + k13;
+	b13 -= k14 + t1;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 47) | (tmp << (64 - 47));
+	b10 -= b11 + k11;
+	b11 -= k12;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 12) | (tmp << (64 - 12));
+	b8 -= b9 + k9;
+	b9 -= k10;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 31) | (tmp << (64 - 31));
+	b6 -= b7 + k7;
+	b7 -= k8;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 37) | (tmp << (64 - 37));
+	b4 -= b5 + k5;
+	b5 -= k6;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 9) | (tmp << (64 - 9));
+	b2 -= b3 + k3;
+	b3 -= k4;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 41) | (tmp << (64 - 41));
+	b0 -= b1 + k1;
+	b1 -= k2;
+
+	tmp = b7 ^ b12;
+	b7 = (tmp >> 25) | (tmp << (64 - 25));
+	b12 -= b7;
+
+	tmp = b3 ^ b10;
+	b3 = (tmp >> 16) | (tmp << (64 - 16));
+	b10 -= b3;
+
+	tmp = b5 ^ b8;
+	b5 = (tmp >> 28) | (tmp << (64 - 28));
+	b8 -= b5;
+
+	tmp = b1 ^ b14;
+	b1 = (tmp >> 47) | (tmp << (64 - 47));
+	b14 -= b1;
+
+	tmp = b9 ^ b4;
+	b9 = (tmp >> 41) | (tmp << (64 - 41));
+	b4 -= b9;
+
+	tmp = b13 ^ b6;
+	b13 = (tmp >> 48) | (tmp << (64 - 48));
+	b6 -= b13;
+
+	tmp = b11 ^ b2;
+	b11 = (tmp >> 20) | (tmp << (64 - 20));
+	b2 -= b11;
+
+	tmp = b15 ^ b0;
+	b15 = (tmp >> 5) | (tmp << (64 - 5));
+	b0 -= b15;
+
+	tmp = b9 ^ b10;
+	b9 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b9;
+
+	tmp = b11 ^ b8;
+	b11 = (tmp >> 59) | (tmp << (64 - 59));
+	b8 -= b11;
+
+	tmp = b13 ^ b14;
+	b13 = (tmp >> 41) | (tmp << (64 - 41));
+	b14 -= b13;
+
+	tmp = b15 ^ b12;
+	b15 = (tmp >> 34) | (tmp << (64 - 34));
+	b12 -= b15;
+
+	tmp = b1 ^ b6;
+	b1 = (tmp >> 13) | (tmp << (64 - 13));
+	b6 -= b1;
+
+	tmp = b3 ^ b4;
+	b3 = (tmp >> 51) | (tmp << (64 - 51));
+	b4 -= b3;
+
+	tmp = b5 ^ b2;
+	b5 = (tmp >> 4) | (tmp << (64 - 4));
+	b2 -= b5;
+
+	tmp = b7 ^ b0;
+	b7 = (tmp >> 33) | (tmp << (64 - 33));
+	b0 -= b7;
+
+	tmp = b1 ^ b8;
+	b1 = (tmp >> 52) | (tmp << (64 - 52));
+	b8 -= b1;
+
+	tmp = b5 ^ b14;
+	b5 = (tmp >> 23) | (tmp << (64 - 23));
+	b14 -= b5;
+
+	tmp = b3 ^ b12;
+	b3 = (tmp >> 18) | (tmp << (64 - 18));
+	b12 -= b3;
+
+	tmp = b7 ^ b10;
+	b7 = (tmp >> 49) | (tmp << (64 - 49));
+	b10 -= b7;
+
+	tmp = b15 ^ b4;
+	b15 = (tmp >> 55) | (tmp << (64 - 55));
+	b4 -= b15;
+
+	tmp = b11 ^ b6;
+	b11 = (tmp >> 10) | (tmp << (64 - 10));
+	b6 -= b11;
+
+	tmp = b13 ^ b2;
+	b13 = (tmp >> 19) | (tmp << (64 - 19));
+	b2 -= b13;
+
+	tmp = b9 ^ b0;
+	b9 = (tmp >> 38) | (tmp << (64 - 38));
+	b0 -= b9;
+
+	tmp = b15 ^ b14;
+	b15 = (tmp >> 37) | (tmp << (64 - 37));
+	b14 -= b15 + k14 + t1;
+	b15 -= k15;
+
+	tmp = b13 ^ b12;
+	b13 = (tmp >> 22) | (tmp << (64 - 22));
+	b12 -= b13 + k12;
+	b13 -= k13 + t0;
+
+	tmp = b11 ^ b10;
+	b11 = (tmp >> 17) | (tmp << (64 - 17));
+	b10 -= b11 + k10;
+	b11 -= k11;
+
+	tmp = b9 ^ b8;
+	b9 = (tmp >> 8) | (tmp << (64 - 8));
+	b8 -= b9 + k8;
+	b9 -= k9;
+
+	tmp = b7 ^ b6;
+	b7 = (tmp >> 47) | (tmp << (64 - 47));
+	b6 -= b7 + k6;
+	b7 -= k7;
+
+	tmp = b5 ^ b4;
+	b5 = (tmp >> 8) | (tmp << (64 - 8));
+	b4 -= b5 + k4;
+	b5 -= k5;
+
+	tmp = b3 ^ b2;
+	b3 = (tmp >> 13) | (tmp << (64 - 13));
+	b2 -= b3 + k2;
+	b3 -= k3;
+
+	tmp = b1 ^ b0;
+	b1 = (tmp >> 24) | (tmp << (64 - 24));
+	b0 -= b1 + k0;
+	b1 -= k1;
+
+	output[15] = b15;
+	output[14] = b14;
+	output[13] = b13;
+	output[12] = b12;
+	output[11] = b11;
+	output[10] = b10;
+	output[9] = b9;
+	output[8] = b8;
+	output[7] = b7;
+	output[6] = b6;
+	output[5] = b5;
+	output[4] = b4;
+	output[3] = b3;
+	output[2] = b2;
+	output[1] = b1;
+	output[0] = b0;
+}
diff --git a/drivers/staging/slicoss/TODO b/drivers/staging/slicoss/TODO
index 62ff1008b1e..20cc9abdc46 100644
--- a/drivers/staging/slicoss/TODO
+++ b/drivers/staging/slicoss/TODO
@@ -18,7 +18,6 @@ TODO:
 	  use ethtool instead
 	- reorder code to elminate use of forward declarations
 	- don't keep private linked list of drivers.
-	- remove all the gratiutous debug infrastructure
 	- use PCI_DEVICE()
 	- do ethtool correctly using ethtool_ops
 	- NAPI?
diff --git a/drivers/staging/slicoss/slic.h b/drivers/staging/slicoss/slic.h
index 702902cdb46..3a5aa882b95 100644
--- a/drivers/staging/slicoss/slic.h
+++ b/drivers/staging/slicoss/slic.h
@@ -310,8 +310,6 @@ struct sliccard {
 	u32           loadtimerset;
 	uint              config_set;
 	struct slic_config  config;
-	struct dentry      *debugfs_dir;
-	struct dentry      *debugfs_cardinfo;
 	struct adapter  *master;
 	struct adapter  *adapter[SLIC_MAX_PORTS];
 	struct sliccard *next;
@@ -364,12 +362,6 @@ struct slic_shmem {
 	volatile struct slic_stats     inicstats;
 };
 
-struct slic_reg_params {
-	u32       linkspeed;
-	u32       linkduplex;
-	u32       fail_on_bad_eeprom;
-};
-
 struct slic_upr {
 	uint               adapter;
 	u32            upr_request;
@@ -409,7 +401,6 @@ struct adapter {
 	uint                card_size;
 	uint                chipid;
 	struct net_device  *netdev;
-	struct net_device  *next_netdevice;
 	struct slic_spinlock     adapter_lock;
 	struct slic_spinlock     reset_lock;
 	struct pci_dev     *pcidev;
@@ -420,8 +411,6 @@ struct adapter {
 	ushort              devid;
 	ushort              subsysid;
 	u32             irq;
-	void __iomem *memorybase;
-	u32             memorylength;
 	u32             drambase;
 	u32             dramlength;
 	uint                queues_initialized;
@@ -452,7 +441,6 @@ struct adapter {
 	u32             pingtimerset;
 	struct timer_list   loadtimer;
 	u32             loadtimerset;
-	struct dentry      *debugfs_entry;
 	struct slic_spinlock     upr_lock;
 	struct slic_spinlock     bit64reglock;
 	struct slic_rspqueue     rspqueue;
@@ -498,7 +486,6 @@ struct adapter {
 	u32             intagg_period;
 	struct inicpm_state    *inicpm_info;
 	void *pinicpm_info;
-	struct slic_reg_params   reg_params;
 	struct slic_ifevents  if_events;
 	struct slic_stats        inicstats_prev;
 	struct slicnet_stats     slic_stats;
diff --git a/drivers/staging/slicoss/slicoss.c b/drivers/staging/slicoss/slicoss.c
index e27b88f02cc..48841e7c2f7 100644
--- a/drivers/staging/slicoss/slicoss.c
+++ b/drivers/staging/slicoss/slicoss.c
@@ -81,7 +81,6 @@
 #include <linux/etherdevice.h>
 #include <linux/skbuff.h>
 #include <linux/delay.h>
-#include <linux/debugfs.h>
 #include <linux/seq_file.h>
 #include <linux/kthread.h>
 #include <linux/module.h>
@@ -104,19 +103,11 @@ static char *slic_banner = "Alacritech SLIC Technology(tm) Server "
 		"and Storage Accelerator (Non-Accelerated)";
 
 static char *slic_proc_version = "2.0.351  2006/07/14 12:26:00";
-static char *slic_product_name = "SLIC Technology(tm) Server "
-		"and Storage Accelerator (Non-Accelerated)";
-static char *slic_vendor = "Alacritech, Inc.";
-
-static int slic_debug = 1;
-static int debug = -1;
-static struct net_device *head_netdevice;
 
 static struct base_driver slic_global = { {}, 0, 0, 0, 1, NULL, NULL };
 static int intagg_delay = 100;
 static u32 dynamic_intagg;
 static unsigned int rcv_count;
-static struct dentry *slic_debugfs;
 
 #define DRV_NAME          "slicoss"
 #define DRV_VERSION       "2.0.1"
@@ -404,7 +395,7 @@ static int slic_card_download_gbrcv(struct adapter *adapter)
 	ret = request_firmware(&fw, file, &adapter->pcidev->dev);
 	if (ret) {
 		dev_err(&adapter->pcidev->dev,
-			"SLICOSS: Failed to load firmware %s\n", file);
+			"Failed to load firmware %s\n", file);
 		return ret;
 	}
 
@@ -482,7 +473,7 @@ static int slic_card_download(struct adapter *adapter)
 	ret = request_firmware(&fw, file, &adapter->pcidev->dev);
 	if (ret) {
 		dev_err(&adapter->pcidev->dev,
-			"SLICOSS: Failed to load firmware %s\n", file);
+			"Failed to load firmware %s\n", file);
 		return ret;
 	}
 	numsects = *(u32 *)(fw->data + index);
@@ -1140,116 +1131,33 @@ static void slic_upr_request_complete(struct adapter *adapter, u32 isr)
 				adapter->upr_lock.flags);
 }
 
-static void slic_config_get(struct adapter *adapter, u32 config,
-							u32 config_h)
+static int slic_config_get(struct adapter *adapter, u32 config, u32 config_h)
 {
-	int status;
-
-	status = slic_upr_request(adapter,
-				  SLIC_UPR_RCONFIG,
-				  (u32) config, (u32) config_h, 0, 0);
+	return slic_upr_request(adapter, SLIC_UPR_RCONFIG, config, config_h,
+				0, 0);
 }
 
 /*
- *  this is here to checksum the eeprom, there is some ucode bug
- *  which prevens us from using the ucode result.
- *  remove this once ucode is fixed.
+ * Compute a checksum of the EEPROM according to RFC 1071.
  */
-static ushort slic_eeprom_cksum(char *m, int len)
+static u16 slic_eeprom_cksum(void *eeprom, unsigned len)
 {
-#define ADDCARRY(x)  (x > 65535 ? x -= 65535 : x)
-#define REDUCE {l_util.l = sum; sum = l_util.s[0] + l_util.s[1]; ADDCARRY(sum);\
-		}
-
-	u16 *w;
-	u32 sum = 0;
-	u32 byte_swapped = 0;
-	u32 w_int;
+	u16 *wp = eeprom;
+	u32 checksum = 0;
 
-	union {
-		char c[2];
-		ushort s;
-	} s_util;
-
-	union {
-		ushort s[2];
-		int l;
-	} l_util;
+	while (len > 1) {
+		checksum += *(wp++);
+		len -= 2;
+	}
 
-	l_util.l = 0;
-	s_util.s = 0;
+	if (len > 0)
+		checksum += *(u8 *) wp;
 
-	w = (u16 *)m;
-#if BITS_PER_LONG == 64
-	w_int = (u32) ((ulong) w & 0x00000000FFFFFFFF);
-#else
-	w_int = (u32) (w);
-#endif
-	if ((1 & w_int) && (len > 0)) {
-		REDUCE;
-		sum <<= 8;
-		s_util.c[0] = *(unsigned char *)w;
-		w = (u16 *)((char *)w + 1);
-		len--;
-		byte_swapped = 1;
-	}
-
-	/* Unroll the loop to make overhead from branches &c small. */
-	while ((len -= 32) >= 0) {
-		sum += w[0];
-		sum += w[1];
-		sum += w[2];
-		sum += w[3];
-		sum += w[4];
-		sum += w[5];
-		sum += w[6];
-		sum += w[7];
-		sum += w[8];
-		sum += w[9];
-		sum += w[10];
-		sum += w[11];
-		sum += w[12];
-		sum += w[13];
-		sum += w[14];
-		sum += w[15];
-		w = (u16 *)((ulong) w + 16);	/* verify */
-	}
-	len += 32;
-	while ((len -= 8) >= 0) {
-		sum += w[0];
-		sum += w[1];
-		sum += w[2];
-		sum += w[3];
-		w = (u16 *)((ulong) w + 4);	/* verify */
-	}
-	len += 8;
-	if (len != 0 || byte_swapped != 0) {
-		REDUCE;
-		while ((len -= 2) >= 0)
-			sum += *w++;	/* verify */
-		if (byte_swapped) {
-			REDUCE;
-			sum <<= 8;
-			byte_swapped = 0;
-			if (len == -1) {
-				s_util.c[1] = *(char *) w;
-				sum += s_util.s;
-				len = 0;
-			} else {
-				len = -1;
-			}
 
-		} else if (len == -1) {
-			s_util.c[0] = *(char *) w;
-		}
+	while (checksum >> 16)
+		checksum = (checksum & 0xFFFF) + ((checksum >> 16) & 0xFFFF);
 
-		if (len == -1) {
-			s_util.c[1] = 0;
-			sum += s_util.s;
-		}
-	}
-	REDUCE;
-	return (ushort) sum;
+	return ~checksum;
 }
 
 static void slic_rspqueue_free(struct adapter *adapter)
@@ -1422,8 +1330,7 @@ static void slic_cmdq_addcmdpage(struct adapter *adapter, u32 *page)
 		spin_lock_irqsave(&adapter->handle_lock.lock,
 				adapter->handle_lock.flags);
 		pslic_handle  =  adapter->pfree_slic_handles;
-		if (pslic_handle)
-			adapter->pfree_slic_handles = pslic_handle->next;
+		adapter->pfree_slic_handles = pslic_handle->next;
 		spin_unlock_irqrestore(&adapter->handle_lock.lock,
 				adapter->handle_lock.flags);
 		pslic_handle->type = SLIC_HANDLE_CMD;
@@ -1802,430 +1709,6 @@ static u32 slic_rcvqueue_reinsert(struct adapter *adapter, struct sk_buff *skb)
 	return rcvq->count;
 }
 
-static int slic_debug_card_show(struct seq_file *seq, void *v)
-{
-#ifdef MOOKTODO
-	int i;
-	struct sliccard *card = seq->private;
-	struct slic_config *config = &card->config;
-	unsigned char *fru = (unsigned char *)(&card->config.atk_fru);
-	unsigned char *oemfru = (unsigned char *)(&card->config.OemFru);
-#endif
-
-	seq_printf(seq, "driver_version           : %s\n", slic_proc_version);
-	seq_puts(seq, "Microcode versions:\n");
-	seq_printf(seq, "    Gigabit (gb)         : %s %s\n",
-		    MOJAVE_UCODE_VERS_STRING, MOJAVE_UCODE_VERS_DATE);
-	seq_printf(seq, "    Gigabit Receiver     : %s %s\n",
-		    GB_RCVUCODE_VERS_STRING, GB_RCVUCODE_VERS_DATE);
-	seq_printf(seq, "Vendor                   : %s\n", slic_vendor);
-	seq_printf(seq, "Product Name             : %s\n", slic_product_name);
-#ifdef MOOKTODO
-	seq_printf(seq, "VendorId                 : %4.4X\n",
-		    config->VendorId);
-	seq_printf(seq, "DeviceId                 : %4.4X\n",
-		    config->DeviceId);
-	seq_printf(seq, "RevisionId               : %2.2x\n",
-		    config->RevisionId);
-	seq_printf(seq, "Bus    #                 : %d\n", card->busnumber);
-	seq_printf(seq, "Device #                 : %d\n", card->slotnumber);
-	seq_printf(seq, "Interfaces               : %d\n", card->card_size);
-	seq_printf(seq, "     Initialized         : %d\n",
-		    card->adapters_activated);
-	seq_printf(seq, "     Allocated           : %d\n",
-		    card->adapters_allocated);
-	for (i = 0; i < card->card_size; i++) {
-		seq_printf(seq,
-			   "     MAC%d : %2.2X %2.2X %2.2X %2.2X %2.2X %2.2X\n",
-			   i, config->macinfo[i].macaddrA[0],
-			   config->macinfo[i].macaddrA[1],
-			   config->macinfo[i].macaddrA[2],
-			   config->macinfo[i].macaddrA[3],
-			   config->macinfo[i].macaddrA[4],
-			   config->macinfo[i].macaddrA[5]);
-	}
-	seq_puts(seq, "     IF  Init State Duplex/Speed irq\n");
-	seq_puts(seq, "     -------------------------------\n");
-	for (i = 0; i < card->adapters_allocated; i++) {
-		struct adapter *adapter;
-
-		adapter = card->adapter[i];
-		if (adapter) {
-			seq_printf(seq,
-				    "     %d   %d   %s  %s  %s    0x%X\n",
-				    adapter->physport, adapter->state,
-				    SLIC_LINKSTATE(adapter->linkstate),
-				    SLIC_DUPLEX(adapter->linkduplex),
-				    SLIC_SPEED(adapter->linkspeed),
-				    (uint) adapter->irq);
-		}
-	}
-	seq_printf(seq, "Generation #             : %4.4X\n", card->gennumber);
-	seq_printf(seq, "RcvQ max entries         : %4.4X\n",
-		    SLIC_RCVQ_ENTRIES);
-	seq_printf(seq, "Ping Status              : %8.8X\n",
-		    card->pingstatus);
-	seq_printf(seq, "Minimum grant            : %2.2x\n",
-		    config->MinGrant);
-	seq_printf(seq, "Maximum Latency          : %2.2x\n", config->MaxLat);
-	seq_printf(seq, "PciStatus                : %4.4x\n",
-		    config->Pcistatus);
-	seq_printf(seq, "Debug Device Id          : %4.4x\n",
-		    config->DbgDevId);
-	seq_printf(seq, "DRAM ROM Function        : %4.4x\n",
-		    config->DramRomFn);
-	seq_printf(seq, "Network interface Pin 1  : %2.2x\n",
-		    config->NetIntPin1);
-	seq_printf(seq, "Network interface Pin 2  : %2.2x\n",
-		    config->NetIntPin1);
-	seq_printf(seq, "Network interface Pin 3  : %2.2x\n",
-		    config->NetIntPin1);
-	seq_printf(seq, "PM capabilities          : %4.4X\n",
-		    config->PMECapab);
-	seq_printf(seq, "Network Clock Controls   : %4.4X\n",
-		    config->NwClkCtrls);
-
-	switch (config->FruFormat) {
-	case ATK_FRU_FORMAT:
-		{
-			seq_puts(seq,
-			    "Vendor                   : Alacritech, Inc.\n");
-			seq_printf(seq,
-			    "Assembly #               : %c%c%c%c%c%c\n",
-				    fru[0], fru[1], fru[2], fru[3], fru[4],
-				    fru[5]);
-			seq_printf(seq,
-				    "Revision #               : %c%c\n",
-				    fru[6], fru[7]);
-
-			if (config->OEMFruFormat == VENDOR4_FRU_FORMAT) {
-				seq_printf(seq,
-					    "Serial   #               : %c%c%c%c%c%c%c%c%c%c%c%c\n",
-					    fru[8], fru[9], fru[10],
-					    fru[11], fru[12], fru[13],
-					    fru[16], fru[17], fru[18],
-					    fru[19], fru[20], fru[21]);
-			} else {
-				seq_printf(seq,
-					    "Serial   #               : %c%c%c%c%c%c%c%c%c%c%c%c%c%c\n",
-					    fru[8], fru[9], fru[10],
-					    fru[11], fru[12], fru[13],
-					    fru[14], fru[15], fru[16],
-					    fru[17], fru[18], fru[19],
-					    fru[20], fru[21]);
-			}
-			break;
-		}
-
-	default:
-		{
-			seq_puts(seq,
-			    "Vendor                   : Alacritech, Inc.\n");
-			seq_puts(seq,
-			    "Serial   #               : Empty FRU\n");
-			break;
-		}
-	}
-
-	switch (config->OEMFruFormat) {
-	case VENDOR1_FRU_FORMAT:
-		{
-			seq_puts(seq, "FRU Information:\n");
-			seq_printf(seq, "    Commodity #          : %c\n",
-				    oemfru[0]);
-			seq_printf(seq,
-				    "    Assembly #           : %c%c%c%c\n",
-				    oemfru[1], oemfru[2], oemfru[3], oemfru[4]);
-			seq_printf(seq,
-				    "    Revision #           : %c%c\n",
-				    oemfru[5], oemfru[6]);
-			seq_printf(seq,
-				    "    Supplier #           : %c%c\n",
-				    oemfru[7], oemfru[8]);
-			seq_printf(seq,
-				    "    Date                 : %c%c\n",
-				    oemfru[9], oemfru[10]);
-			seq_sprintf(seq,
-				    "    Sequence #           : %c%c%c\n",
-				    oemfru[11], oemfru[12], oemfru[13]);
-			break;
-		}
-
-	case VENDOR2_FRU_FORMAT:
-		{
-			seq_puts(seq, "FRU Information:\n");
-			seq_printf(seq,
-				    "    Part     #           : %c%c%c%c%c%c%c%c\n",
-				    oemfru[0], oemfru[1], oemfru[2],
-				    oemfru[3], oemfru[4], oemfru[5],
-				    oemfru[6], oemfru[7]);
-			seq_printf(seq,
-				    "    Supplier #           : %c%c%c%c%c\n",
-				    oemfru[8], oemfru[9], oemfru[10],
-				    oemfru[11], oemfru[12]);
-			seq_printf(seq,
-				    "    Date                 : %c%c%c\n",
-				    oemfru[13], oemfru[14], oemfru[15]);
-			seq_sprintf(seq,
-				    "    Sequence #           : %c%c%c%c\n",
-				    oemfru[16], oemfru[17], oemfru[18],
-				    oemfru[19]);
-			break;
-		}
-
-	case VENDOR3_FRU_FORMAT:
-		{
-			seq_puts(seq, "FRU Information:\n");
-		}
-
-	case VENDOR4_FRU_FORMAT:
-		{
-			seq_puts(seq, "FRU Information:\n");
-			seq_printf(seq,
-				    "    FRU Number           : %c%c%c%c%c%c%c%c\n",
-				    oemfru[0], oemfru[1], oemfru[2],
-				    oemfru[3], oemfru[4], oemfru[5],
-				    oemfru[6], oemfru[7]);
-			seq_sprintf(seq,
-				    "    Part Number          : %c%c%c%c%c%c%c%c\n",
-				    oemfru[8], oemfru[9], oemfru[10],
-				    oemfru[11], oemfru[12], oemfru[13],
-				    oemfru[14], oemfru[15]);
-			seq_printf(seq,
-				    "    EC Level             : %c%c%c%c%c%c%c%c\n",
-				    oemfru[16], oemfru[17], oemfru[18],
-				    oemfru[19], oemfru[20], oemfru[21],
-				    oemfru[22], oemfru[23]);
-			break;
-		}
-
-	default:
-		break;
-	}
-#endif
-
-	return 0;
-}
-
-static int slic_debug_adapter_show(struct seq_file *seq, void *v)
-{
-	struct adapter *adapter = seq->private;
-	struct net_device *netdev = adapter->netdev;
-
-	seq_printf(seq, "info: interface          : %s\n",
-			    adapter->netdev->name);
-	seq_printf(seq, "info: status             : %s\n",
-		SLIC_LINKSTATE(adapter->linkstate));
-	seq_printf(seq, "info: port               : %d\n",
-		adapter->physport);
-	seq_printf(seq, "info: speed              : %s\n",
-		SLIC_SPEED(adapter->linkspeed));
-	seq_printf(seq, "info: duplex             : %s\n",
-		SLIC_DUPLEX(adapter->linkduplex));
-	seq_printf(seq, "info: irq                : 0x%X\n",
-		(uint) adapter->irq);
-	seq_printf(seq, "info: Interrupt Agg Delay: %d usec\n",
-		adapter->card->loadlevel_current);
-	seq_printf(seq, "info: RcvQ max entries   : %4.4X\n",
-		SLIC_RCVQ_ENTRIES);
-	seq_printf(seq, "info: RcvQ current       : %4.4X\n",
-		    adapter->rcvqueue.count);
-	seq_printf(seq, "rx stats: packets                  : %8.8lX\n",
-		    netdev->stats.rx_packets);
-	seq_printf(seq, "rx stats: bytes                    : %8.8lX\n",
-		    netdev->stats.rx_bytes);
-	seq_printf(seq, "rx stats: broadcasts               : %8.8X\n",
-		    adapter->rcv_broadcasts);
-	seq_printf(seq, "rx stats: multicasts               : %8.8X\n",
-		    adapter->rcv_multicasts);
-	seq_printf(seq, "rx stats: unicasts                 : %8.8X\n",
-		    adapter->rcv_unicasts);
-	seq_printf(seq, "rx stats: errors                   : %8.8X\n",
-		    (u32) adapter->slic_stats.iface.rcv_errors);
-	seq_printf(seq, "rx stats: Missed errors            : %8.8X\n",
-		    (u32) adapter->slic_stats.iface.rcv_discards);
-	seq_printf(seq, "rx stats: drops                    : %8.8X\n",
-			(u32) adapter->rcv_drops);
-	seq_printf(seq, "tx stats: packets                  : %8.8lX\n",
-			netdev->stats.tx_packets);
-	seq_printf(seq, "tx stats: bytes                    : %8.8lX\n",
-			netdev->stats.tx_bytes);
-	seq_printf(seq, "tx stats: errors                   : %8.8X\n",
-			(u32) adapter->slic_stats.iface.xmt_errors);
-	seq_printf(seq, "rx stats: multicasts               : %8.8lX\n",
-			netdev->stats.multicast);
-	seq_printf(seq, "tx stats: collision errors         : %8.8X\n",
-			(u32) adapter->slic_stats.iface.xmit_collisions);
-	seq_printf(seq, "perf: Max rcv frames/isr           : %8.8X\n",
-			adapter->max_isr_rcvs);
-	seq_printf(seq, "perf: Rcv interrupt yields         : %8.8X\n",
-			adapter->rcv_interrupt_yields);
-	seq_printf(seq, "perf: Max xmit complete/isr        : %8.8X\n",
-			adapter->max_isr_xmits);
-	seq_printf(seq, "perf: error interrupts             : %8.8X\n",
-			adapter->error_interrupts);
-	seq_printf(seq, "perf: error rmiss interrupts       : %8.8X\n",
-			adapter->error_rmiss_interrupts);
-	seq_printf(seq, "perf: rcv interrupts               : %8.8X\n",
-			adapter->rcv_interrupts);
-	seq_printf(seq, "perf: xmit interrupts              : %8.8X\n",
-			adapter->xmit_interrupts);
-	seq_printf(seq, "perf: link event interrupts        : %8.8X\n",
-			adapter->linkevent_interrupts);
-	seq_printf(seq, "perf: UPR interrupts               : %8.8X\n",
-			adapter->upr_interrupts);
-	seq_printf(seq, "perf: interrupt count              : %8.8X\n",
-			adapter->num_isrs);
-	seq_printf(seq, "perf: false interrupts             : %8.8X\n",
-			adapter->false_interrupts);
-	seq_printf(seq, "perf: All register writes          : %8.8X\n",
-			adapter->all_reg_writes);
-	seq_printf(seq, "perf: ICR register writes          : %8.8X\n",
-			adapter->icr_reg_writes);
-	seq_printf(seq, "perf: ISR register writes          : %8.8X\n",
-			adapter->isr_reg_writes);
-	seq_printf(seq, "ifevents: overflow 802 errors      : %8.8X\n",
-			adapter->if_events.oflow802);
-	seq_printf(seq, "ifevents: transport overflow errors: %8.8X\n",
-			adapter->if_events.Tprtoflow);
-	seq_printf(seq, "ifevents: underflow errors         : %8.8X\n",
-			adapter->if_events.uflow802);
-	seq_printf(seq, "ifevents: receive early            : %8.8X\n",
-			adapter->if_events.rcvearly);
-	seq_printf(seq, "ifevents: buffer overflows         : %8.8X\n",
-			adapter->if_events.Bufov);
-	seq_printf(seq, "ifevents: carrier errors           : %8.8X\n",
-			adapter->if_events.Carre);
-	seq_printf(seq, "ifevents: Long                     : %8.8X\n",
-			adapter->if_events.Longe);
-	seq_printf(seq, "ifevents: invalid preambles        : %8.8X\n",
-			adapter->if_events.Invp);
-	seq_printf(seq, "ifevents: CRC errors               : %8.8X\n",
-			adapter->if_events.Crc);
-	seq_printf(seq, "ifevents: dribble nibbles          : %8.8X\n",
-			adapter->if_events.Drbl);
-	seq_printf(seq, "ifevents: Code violations          : %8.8X\n",
-			adapter->if_events.Code);
-	seq_printf(seq, "ifevents: TCP checksum errors      : %8.8X\n",
-			adapter->if_events.TpCsum);
-	seq_printf(seq, "ifevents: TCP header short errors  : %8.8X\n",
-			adapter->if_events.TpHlen);
-	seq_printf(seq, "ifevents: IP checksum errors       : %8.8X\n",
-			adapter->if_events.IpCsum);
-	seq_printf(seq, "ifevents: IP frame incompletes     : %8.8X\n",
-			adapter->if_events.IpLen);
-	seq_printf(seq, "ifevents: IP headers shorts        : %8.8X\n",
-			adapter->if_events.IpHlen);
-
-	return 0;
-}
-static int slic_debug_adapter_open(struct inode *inode, struct file *file)
-{
-	return single_open(file, slic_debug_adapter_show, inode->i_private);
-}
-
-static int slic_debug_card_open(struct inode *inode, struct file *file)
-{
-	return single_open(file, slic_debug_card_show, inode->i_private);
-}
-
-static const struct file_operations slic_debug_adapter_fops = {
-	.owner		= THIS_MODULE,
-	.open		= slic_debug_adapter_open,
-	.read		= seq_read,
-	.llseek		= seq_lseek,
-	.release	= single_release,
-};
-
-static const struct file_operations slic_debug_card_fops = {
-	.owner		= THIS_MODULE,
-	.open		= slic_debug_card_open,
-	.read		= seq_read,
-	.llseek		= seq_lseek,
-	.release	= single_release,
-};
-
-static void slic_debug_adapter_create(struct adapter *adapter)
-{
-	struct dentry *d;
-	char    name[7];
-	struct sliccard *card = adapter->card;
-
-	if (!card->debugfs_dir)
-		return;
-
-	sprintf(name, "port%d", adapter->port);
-	d = debugfs_create_file(name, S_IRUGO,
-				card->debugfs_dir, adapter,
-				&slic_debug_adapter_fops);
-	if (!d || IS_ERR(d))
-		pr_info(PFX "%s: debugfs create failed\n", name);
-	else
-		adapter->debugfs_entry = d;
-}
-
-static void slic_debug_adapter_destroy(struct adapter *adapter)
-{
-	debugfs_remove(adapter->debugfs_entry);
-	adapter->debugfs_entry = NULL;
-}
-
-static void slic_debug_card_create(struct sliccard *card)
-{
-	struct dentry *d;
-	char    name[IFNAMSIZ];
-
-	snprintf(name, sizeof(name), "slic%d", card->cardnum);
-	d = debugfs_create_dir(name, slic_debugfs);
-	if (!d || IS_ERR(d))
-		pr_info(PFX "%s: debugfs create dir failed\n",
-				name);
-	else {
-		card->debugfs_dir = d;
-		d = debugfs_create_file("cardinfo", S_IRUGO,
-				slic_debugfs, card,
-				&slic_debug_card_fops);
-		if (!d || IS_ERR(d))
-			pr_info(PFX "%s: debugfs create failed\n",
-					name);
-		else
-			card->debugfs_cardinfo = d;
-	}
-}
-
-static void slic_debug_card_destroy(struct sliccard *card)
-{
-	int i;
-
-	for (i = 0; i < card->card_size; i++) {
-		struct adapter *adapter;
-
-		adapter = card->adapter[i];
-		if (adapter)
-			slic_debug_adapter_destroy(adapter);
-	}
-	debugfs_remove(card->debugfs_cardinfo);
-	debugfs_remove(card->debugfs_dir);
-}
-
-static void slic_debug_init(void)
-{
-	struct dentry *ent;
-
-	ent = debugfs_create_dir("slic", NULL);
-	if (!ent || IS_ERR(ent)) {
-		pr_info(PFX "debugfs create directory failed\n");
-		return;
-	}
-
-	slic_debugfs = ent;
-}
-
-static void slic_debug_cleanup(void)
-{
-	debugfs_remove(slic_debugfs);
-}
-
 /*
  * slic_link_event_handler -
  *
@@ -2947,30 +2430,21 @@ static void slic_card_cleanup(struct sliccard *card)
 		del_timer_sync(&card->loadtimer);
 	}
 
-	slic_debug_card_destroy(card);
-
 	kfree(card);
 }
 
 static void slic_entry_remove(struct pci_dev *pcidev)
 {
 	struct net_device *dev = pci_get_drvdata(pcidev);
-	u32 mmio_start = 0;
-	uint mmio_len = 0;
 	struct adapter *adapter = netdev_priv(dev);
 	struct sliccard *card;
 	struct mcast_address *mcaddr, *mlist;
 
-	slic_adapter_freeresources(adapter);
-	slic_unmap_mmio_space(adapter);
 	unregister_netdev(dev);
 
-	mmio_start = pci_resource_start(pcidev, 0);
-	mmio_len = pci_resource_len(pcidev, 0);
-
-	release_mem_region(mmio_start, mmio_len);
+	slic_adapter_freeresources(adapter);
+	slic_unmap_mmio_space(adapter);
 
-	iounmap((void __iomem *)dev->base_addr);
 	/* free multicast addresses */
 	mlist = adapter->mcastaddrs;
 	while (mlist) {
@@ -3225,13 +2699,8 @@ static int slic_card_init(struct sliccard *card, struct adapter *adapter)
 
 	/* Download the microcode */
 	status = slic_card_download(adapter);
-
-	if (status != 0) {
-		dev_err(&adapter->pcidev->dev,
-			"download failed bus %d slot %d\n",
-			adapter->busnumber, adapter->slotnumber);
+	if (status)
 		return status;
-	}
 
 	if (!card->config_set) {
 		peeprom = pci_alloc_consistent(adapter->pcidev,
@@ -3243,8 +2712,7 @@ static int slic_card_init(struct sliccard *card, struct adapter *adapter)
 
 		if (!peeprom) {
 			dev_err(&adapter->pcidev->dev,
-				"eeprom read failed to get memory bus %d slot %d\n", adapter->busnumber,
-				adapter->slotnumber);
+				"Failed to allocate DMA memory for EEPROM.\n");
 			return -ENOMEM;
 		} else {
 			memset(peeprom, 0, sizeof(struct slic_eeprom));
@@ -3256,13 +2724,19 @@ static int slic_card_init(struct sliccard *card, struct adapter *adapter)
 
 		spin_lock_irqsave(&adapter->bit64reglock.lock,
 					adapter->bit64reglock.flags);
-		slic_reg32_write(&slic_regs->slic_addr_upper, 0, DONT_FLUSH);
+		slic_reg32_write(&slic_regs->slic_addr_upper,
+				 SLIC_GET_ADDR_HIGH(&pshmem->isr), DONT_FLUSH);
 		slic_reg32_write(&slic_regs->slic_isp,
 				 SLIC_GET_ADDR_LOW(&pshmem->isr), FLUSH);
 		spin_unlock_irqrestore(&adapter->bit64reglock.lock,
 					adapter->bit64reglock.flags);
 
-		slic_config_get(adapter, phys_configl, phys_configh);
+		status = slic_config_get(adapter, phys_configl, phys_configh);
+		if (status) {
+			dev_err(&adapter->pcidev->dev,
+				"Failed to fetch config data from device.\n");
+			goto card_init_err;
+		}
 
 		for (;;) {
 			if (adapter->pshmem->isr) {
@@ -3287,13 +2761,13 @@ static int slic_card_init(struct sliccard *card, struct adapter *adapter)
 				i++;
 				if (i > 5000) {
 					dev_err(&adapter->pcidev->dev,
-						"%d config data fetch timed out!\n",
-						adapter->port);
+						"Fetch of config data timed out.\n");
 					slic_reg64_write(adapter,
 						&slic_regs->slic_isp, 0,
 						&slic_regs->slic_addr_upper,
 						0, FLUSH);
-					return -EINVAL;
+					status = -EINVAL;
+					goto card_init_err;
 				}
 			}
 		}
@@ -3339,9 +2813,8 @@ static int slic_card_init(struct sliccard *card, struct adapter *adapter)
 			/*
 			    calculate the EEPROM checksum
 			*/
-			calc_chksum =
-			    ~slic_eeprom_cksum((char *) peeprom,
-					       (eecodesize - 2));
+			calc_chksum = slic_eeprom_cksum(peeprom,
+							eecodesize - 2);
 			/*
 			    if the ucdoe chksum flag bit worked,
 			    we wouldn't need this
@@ -3367,24 +2840,20 @@ static int slic_card_init(struct sliccard *card, struct adapter *adapter)
 				    sizeof(struct slic_eeprom),
 				    peeprom, phys_config);
 
-		if ((!card->config.EepromValid) &&
-		    (adapter->reg_params.fail_on_bad_eeprom)) {
+		if (!card->config.EepromValid) {
 			slic_reg64_write(adapter, &slic_regs->slic_isp, 0,
 					 &slic_regs->slic_addr_upper,
 					 0, FLUSH);
-			dev_err(&adapter->pcidev->dev,
-				"unsupported CONFIGURATION EEPROM invalid\n");
+			dev_err(&adapter->pcidev->dev, "EEPROM invalid.\n");
 			return -EINVAL;
 		}
 
 		card->config_set = 1;
 	}
 
-	if (slic_card_download_gbrcv(adapter)) {
-		dev_err(&adapter->pcidev->dev,
-			"unable to download GB receive microcode\n");
-		return -EINVAL;
-	}
+	status = slic_card_download_gbrcv(adapter);
+	if (status)
+		return status;
 
 	if (slic_global.dynamic_intagg)
 		slic_intagg_set(adapter, 0);
@@ -3403,6 +2872,11 @@ static int slic_card_init(struct sliccard *card, struct adapter *adapter)
 	card->reset_in_progress = 0;
 
 	return 0;
+
+card_init_err:
+	pci_free_consistent(adapter->pcidev, sizeof(struct slic_eeprom),
+			    peeprom, phys_config);
+	return status;
 }
 
 static void slic_init_driver(void)
@@ -3410,7 +2884,6 @@ static void slic_init_driver(void)
 	if (slic_first_init) {
 		slic_first_init = 0;
 		spin_lock_init(&slic_global.driver_lock.lock);
-		slic_debug_init();
 	}
 }
 
@@ -3430,16 +2903,12 @@ static void slic_init_adapter(struct net_device *netdev,
 	adapter->busnumber = pcidev->bus->number;
 	adapter->slotnumber = ((pcidev->devfn >> 3) & 0x1F);
 	adapter->functionnumber = (pcidev->devfn & 0x7);
-	adapter->memorylength = pci_resource_len(pcidev, 0);
 	adapter->slic_regs = (__iomem struct slic_regs *)memaddr;
 	adapter->irq = pcidev->irq;
 /*	adapter->netdev = netdev;*/
-	adapter->next_netdevice = head_netdevice;
-	head_netdevice = netdev;
 	adapter->chipid = chip_idx;
 	adapter->port = 0;	/*adapter->functionnumber;*/
 	adapter->cardindex = adapter->port;
-	adapter->memorybase = memaddr;
 	spin_lock_init(&adapter->upr_lock.lock);
 	spin_lock_init(&adapter->bit64reglock.lock);
 	spin_lock_init(&adapter->adapter_lock.lock);
@@ -3529,8 +2998,6 @@ static u32 slic_card_locate(struct adapter *adapter)
 			}
 		}
 		slic_global.num_slic_cards++;
-
-		slic_debug_card_create(card);
 	} else {
 		/* Card exists, find the card this adapter belongs to */
 		while (card) {
@@ -3595,7 +3062,6 @@ static int slic_entry_probe(struct pci_dev *pcidev,
 	struct net_device *netdev;
 	struct adapter *adapter;
 	void __iomem *memmapped_ioaddr = NULL;
-	u32 status = 0;
 	ulong mmio_start = 0;
 	ulong mmio_len = 0;
 	struct sliccard *card = NULL;
@@ -3608,9 +3074,9 @@ static int slic_entry_probe(struct pci_dev *pcidev,
 	if (err)
 		return err;
 
-	if (slic_debug > 0 && did_version++ == 0) {
-		dev_dbg(&pcidev->dev, "%s\n", slic_banner);
-		dev_dbg(&pcidev->dev, "%s\n", slic_proc_version);
+	if (did_version++ == 0) {
+		dev_info(&pcidev->dev, "%s\n", slic_banner);
+		dev_info(&pcidev->dev, "%s\n", slic_proc_version);
 	}
 
 	if (!pci_set_dma_mask(pcidev, DMA_BIT_MASK(64))) {
@@ -3686,23 +3152,16 @@ static int slic_entry_probe(struct pci_dev *pcidev,
 		adapter->allocated = 1;
 	}
 
-	status = slic_card_init(card, adapter);
+	err = slic_card_init(card, adapter);
+	if (err)
+		goto err_out_unmap;
 
-	if (status != 0) {
-		card->state = CARD_FAIL;
-		adapter->state = ADAPT_FAIL;
-		adapter->linkstate = LINK_DOWN;
-		dev_err(&pcidev->dev, "FAILED status[%x]\n", status);
-	} else {
-		slic_adapter_set_hwaddr(adapter);
-	}
+	slic_adapter_set_hwaddr(adapter);
 
-	netdev->base_addr = (unsigned long)adapter->memorybase;
+	netdev->base_addr = (unsigned long) memmapped_ioaddr;
 	netdev->irq = adapter->irq;
 	netdev->netdev_ops = &slic_netdev_ops;
 
-	slic_debug_adapter_create(adapter);
-
 	strcpy(netdev->name, "eth%d");
 	err = register_netdev(netdev);
 	if (err) {
@@ -3712,7 +3171,7 @@ static int slic_entry_probe(struct pci_dev *pcidev,
 
 	cards_found++;
 
-	return status;
+	return 0;
 
 err_out_unmap:
 	iounmap(memmapped_ioaddr);
@@ -3736,18 +3195,12 @@ static int __init slic_module_init(void)
 {
 	slic_init_driver();
 
-	if (debug >= 0 && slic_debug != debug)
-		pr_debug("debug level is %d.\n", debug);
-	if (debug >= 0)
-		slic_debug = debug;
-
 	return pci_register_driver(&slic_driver);
 }
 
 static void __exit slic_module_cleanup(void)
 {
 	pci_unregister_driver(&slic_driver);
-	slic_debug_cleanup();
 }
 
 module_init(slic_module_init);
diff --git a/drivers/staging/speakup/kobjects.c b/drivers/staging/speakup/kobjects.c
index 1ca91f7092b..2ef7f6f201a 100644
--- a/drivers/staging/speakup/kobjects.c
+++ b/drivers/staging/speakup/kobjects.c
@@ -641,7 +641,7 @@ ssize_t spk_var_store(struct kobject *kobj, struct kobj_attribute *attr,
 		if (kstrtol(cp, 10, &value) == 0)
 			ret = spk_set_num_var(value, param, len);
 		else
-			pr_warn("overflow or parsing error has occured");
+			pr_warn("overflow or parsing error has occurred");
 		if (ret == -ERANGE) {
 			var_data = param->data;
 			pr_warn("value for %s out of range, expect %d to %d\n",
@@ -851,75 +851,75 @@ static ssize_t message_store(struct kobject *kobj, struct kobj_attribute *attr,
  * Declare the attributes.
  */
 static struct kobj_attribute keymap_attribute =
-	__ATTR(keymap, S_IWUSR|S_IRUGO, keymap_show, keymap_store);
+	__ATTR_RW(keymap);
 static struct kobj_attribute silent_attribute =
-	__ATTR(silent, S_IWUGO, NULL, silent_store);
+	__ATTR_WO(silent);
 static struct kobj_attribute synth_attribute =
-	__ATTR(synth, S_IWUGO|S_IRUGO, synth_show, synth_store);
+	__ATTR_RW(synth);
 static struct kobj_attribute synth_direct_attribute =
-	__ATTR(synth_direct, S_IWUGO, NULL, synth_direct_store);
+	__ATTR_WO(synth_direct);
 static struct kobj_attribute version_attribute =
 	__ATTR_RO(version);
 
 static struct kobj_attribute delimiters_attribute =
-	__ATTR(delimiters, S_IWUGO|S_IRUGO, punc_show, punc_store);
+	__ATTR(delimiters, S_IWUSR|S_IRUGO, punc_show, punc_store);
 static struct kobj_attribute ex_num_attribute =
-	__ATTR(ex_num, S_IWUGO|S_IRUGO, punc_show, punc_store);
+	__ATTR(ex_num, S_IWUSR|S_IRUGO, punc_show, punc_store);
 static struct kobj_attribute punc_all_attribute =
-	__ATTR(punc_all, S_IWUGO|S_IRUGO, punc_show, punc_store);
+	__ATTR(punc_all, S_IWUSR|S_IRUGO, punc_show, punc_store);
 static struct kobj_attribute punc_most_attribute =
-	__ATTR(punc_most, S_IWUGO|S_IRUGO, punc_show, punc_store);
+	__ATTR(punc_most, S_IWUSR|S_IRUGO, punc_show, punc_store);
 static struct kobj_attribute punc_some_attribute =
-	__ATTR(punc_some, S_IWUGO|S_IRUGO, punc_show, punc_store);
+	__ATTR(punc_some, S_IWUSR|S_IRUGO, punc_show, punc_store);
 static struct kobj_attribute repeats_attribute =
-	__ATTR(repeats, S_IWUGO|S_IRUGO, punc_show, punc_store);
+	__ATTR(repeats, S_IWUSR|S_IRUGO, punc_show, punc_store);
 
 static struct kobj_attribute attrib_bleep_attribute =
-	__ATTR(attrib_bleep, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(attrib_bleep, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute bell_pos_attribute =
-	__ATTR(bell_pos, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(bell_pos, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute bleep_time_attribute =
-	__ATTR(bleep_time, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(bleep_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute bleeps_attribute =
-	__ATTR(bleeps, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(bleeps, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute cursor_time_attribute =
-	__ATTR(cursor_time, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(cursor_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute key_echo_attribute =
-	__ATTR(key_echo, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(key_echo, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute no_interrupt_attribute =
-	__ATTR(no_interrupt, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(no_interrupt, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute punc_level_attribute =
-	__ATTR(punc_level, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(punc_level, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute reading_punc_attribute =
-	__ATTR(reading_punc, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(reading_punc, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute say_control_attribute =
-	__ATTR(say_control, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(say_control, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute say_word_ctl_attribute =
-	__ATTR(say_word_ctl, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(say_word_ctl, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute spell_delay_attribute =
-	__ATTR(spell_delay, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(spell_delay, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 /*
  * These attributes are i18n related.
  */
 static struct kobj_attribute announcements_attribute =
-	__ATTR(announcements, S_IWUGO|S_IRUGO, message_show, message_store);
+	__ATTR(announcements, S_IWUSR|S_IRUGO, message_show, message_store);
 static struct kobj_attribute characters_attribute =
-	__ATTR(characters, S_IWUGO|S_IRUGO, chars_chartab_show, chars_chartab_store);
+	__ATTR(characters, S_IWUSR|S_IRUGO, chars_chartab_show, chars_chartab_store);
 static struct kobj_attribute chartab_attribute =
-	__ATTR(chartab, S_IWUGO|S_IRUGO, chars_chartab_show, chars_chartab_store);
+	__ATTR(chartab, S_IWUSR|S_IRUGO, chars_chartab_show, chars_chartab_store);
 static struct kobj_attribute ctl_keys_attribute =
-	__ATTR(ctl_keys, S_IWUGO|S_IRUGO, message_show, message_store);
+	__ATTR(ctl_keys, S_IWUSR|S_IRUGO, message_show, message_store);
 static struct kobj_attribute colors_attribute =
-	__ATTR(colors, S_IWUGO|S_IRUGO, message_show, message_store);
+	__ATTR(colors, S_IWUSR|S_IRUGO, message_show, message_store);
 static struct kobj_attribute formatted_attribute =
-	__ATTR(formatted, S_IWUGO|S_IRUGO, message_show, message_store);
+	__ATTR(formatted, S_IWUSR|S_IRUGO, message_show, message_store);
 static struct kobj_attribute function_names_attribute =
-	__ATTR(function_names, S_IWUGO|S_IRUGO, message_show, message_store);
+	__ATTR(function_names, S_IWUSR|S_IRUGO, message_show, message_store);
 static struct kobj_attribute key_names_attribute =
-	__ATTR(key_names, S_IWUGO|S_IRUGO, message_show, message_store);
+	__ATTR(key_names, S_IWUSR|S_IRUGO, message_show, message_store);
 static struct kobj_attribute states_attribute =
-	__ATTR(states, S_IWUGO|S_IRUGO, message_show, message_store);
+	__ATTR(states, S_IWUSR|S_IRUGO, message_show, message_store);
 
 /*
  * Create groups of attributes so that we can create and destroy them all
diff --git a/drivers/staging/speakup/main.c b/drivers/staging/speakup/main.c
index ef5933b9359..3b6e5358c72 100644
--- a/drivers/staging/speakup/main.c
+++ b/drivers/staging/speakup/main.c
@@ -1855,8 +1855,9 @@ static int handle_goto(struct vc_data *vc, u_char type, u_char ch, u_short key)
 {
 	static u_char goto_buf[8];
 	static int num;
-	int maxlen, go_pos;
+	int maxlen;
 	char *cp;
+
 	if (type == KT_SPKUP && ch == SPEAKUP_GOTO)
 		goto do_goto;
 	if (type == KT_LATIN && ch == '\n')
@@ -1891,25 +1892,24 @@ oops:
 		spk_special_handler = NULL;
 		return 1;
 	}
-	go_pos = kstrtol(goto_buf, 10, (long *)&cp);
-	goto_pos = (u_long) go_pos;
+
+	goto_pos = simple_strtoul(goto_buf, &cp, 10);
+
 	if (*cp == 'x') {
 		if (*goto_buf < '0')
 			goto_pos += spk_x;
-		else
+		else if (goto_pos > 0)
 			goto_pos--;
-		if (goto_pos < 0)
-			goto_pos = 0;
+
 		if (goto_pos >= vc->vc_cols)
 			goto_pos = vc->vc_cols - 1;
 		goto_x = 1;
 	} else {
 		if (*goto_buf < '0')
 			goto_pos += spk_y;
-		else
+		else if (goto_pos > 0)
 			goto_pos--;
-		if (goto_pos < 0)
-			goto_pos = 0;
+
 		if (goto_pos >= vc->vc_rows)
 			goto_pos = vc->vc_rows - 1;
 		goto_x = 0;
diff --git a/drivers/staging/speakup/speakup_acntpc.c b/drivers/staging/speakup/speakup_acntpc.c
index e7dfa434bd9..31f952b9049 100644
--- a/drivers/staging/speakup/speakup_acntpc.c
+++ b/drivers/staging/speakup/speakup_acntpc.c
@@ -62,22 +62,22 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/acntpc.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute tone_attribute =
-	__ATTR(tone, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(tone, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute vol_attribute =
-	__ATTR(vol, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(vol, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 static struct kobj_attribute delay_time_attribute =
 	__ATTR(delay_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
 	__ATTR(full_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
diff --git a/drivers/staging/speakup/speakup_acntsa.c b/drivers/staging/speakup/speakup_acntsa.c
index c7f014ed962..1f374845f61 100644
--- a/drivers/staging/speakup/speakup_acntsa.c
+++ b/drivers/staging/speakup/speakup_acntsa.c
@@ -47,22 +47,22 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/acntsa.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute tone_attribute =
-	__ATTR(tone, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(tone, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute vol_attribute =
-	__ATTR(vol, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(vol, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 static struct kobj_attribute delay_time_attribute =
 	__ATTR(delay_time, S_IRUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
 	__ATTR(full_time, S_IRUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
diff --git a/drivers/staging/speakup/speakup_apollo.c b/drivers/staging/speakup/speakup_apollo.c
index 38c8c2221e4..678b263e551 100644
--- a/drivers/staging/speakup/speakup_apollo.c
+++ b/drivers/staging/speakup/speakup_apollo.c
@@ -53,24 +53,24 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/apollo.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute lang_attribute =
-	__ATTR(lang, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(lang, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute voice_attribute =
-	__ATTR(voice, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(voice, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute vol_attribute =
-	__ATTR(vol, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(vol, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 static struct kobj_attribute delay_time_attribute =
 	__ATTR(delay_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
 	__ATTR(full_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
diff --git a/drivers/staging/speakup/speakup_audptr.c b/drivers/staging/speakup/speakup_audptr.c
index de5b4a5f43b..362f9747e48 100644
--- a/drivers/staging/speakup/speakup_audptr.c
+++ b/drivers/staging/speakup/speakup_audptr.c
@@ -49,24 +49,24 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/audptr.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute punct_attribute =
-	__ATTR(punct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(punct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute tone_attribute =
-	__ATTR(tone, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(tone, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute vol_attribute =
-	__ATTR(vol, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(vol, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 static struct kobj_attribute delay_time_attribute =
 	__ATTR(delay_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
 	__ATTR(full_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
diff --git a/drivers/staging/speakup/speakup_bns.c b/drivers/staging/speakup/speakup_bns.c
index 4939e8c7272..2f070282a85 100644
--- a/drivers/staging/speakup/speakup_bns.c
+++ b/drivers/staging/speakup/speakup_bns.c
@@ -44,22 +44,22 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/bns.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute tone_attribute =
-	__ATTR(tone, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(tone, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute vol_attribute =
-	__ATTR(vol, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(vol, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 static struct kobj_attribute delay_time_attribute =
 	__ATTR(delay_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
 	__ATTR(full_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
diff --git a/drivers/staging/speakup/speakup_decext.c b/drivers/staging/speakup/speakup_decext.c
index b17af980392..67b7de1d8c7 100644
--- a/drivers/staging/speakup/speakup_decext.c
+++ b/drivers/staging/speakup/speakup_decext.c
@@ -70,24 +70,24 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/decext.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute punct_attribute =
-	__ATTR(punct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(punct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute voice_attribute =
-	__ATTR(voice, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(voice, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute vol_attribute =
-	__ATTR(vol, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(vol, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 static struct kobj_attribute delay_time_attribute =
 	__ATTR(delay_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
 	__ATTR(full_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
diff --git a/drivers/staging/speakup/speakup_decpc.c b/drivers/staging/speakup/speakup_decpc.c
index cfa4bc03235..67678d8888c 100644
--- a/drivers/staging/speakup/speakup_decpc.c
+++ b/drivers/staging/speakup/speakup_decpc.c
@@ -164,24 +164,24 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/decpc.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute punct_attribute =
-	__ATTR(punct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(punct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute voice_attribute =
-	__ATTR(voice, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(voice, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute vol_attribute =
-	__ATTR(vol, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(vol, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 static struct kobj_attribute delay_time_attribute =
 	__ATTR(delay_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
 	__ATTR(full_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
diff --git a/drivers/staging/speakup/speakup_dectlk.c b/drivers/staging/speakup/speakup_dectlk.c
index 1fcae55dabb..af848686be7 100644
--- a/drivers/staging/speakup/speakup_dectlk.c
+++ b/drivers/staging/speakup/speakup_dectlk.c
@@ -70,24 +70,24 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/dectlk.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute punct_attribute =
-	__ATTR(punct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(punct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute voice_attribute =
-	__ATTR(voice, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(voice, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute vol_attribute =
-	__ATTR(vol, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(vol, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 static struct kobj_attribute delay_time_attribute =
 	__ATTR(delay_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
 	__ATTR(full_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
diff --git a/drivers/staging/speakup/speakup_dtlk.c b/drivers/staging/speakup/speakup_dtlk.c
index 5c6c34191e8..98d1f497e4e 100644
--- a/drivers/staging/speakup/speakup_dtlk.c
+++ b/drivers/staging/speakup/speakup_dtlk.c
@@ -67,28 +67,28 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/dtlk.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute freq_attribute =
-	__ATTR(freq, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(freq, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute punct_attribute =
-	__ATTR(punct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(punct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute tone_attribute =
-	__ATTR(tone, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(tone, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute voice_attribute =
-	__ATTR(voice, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(voice, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute vol_attribute =
-	__ATTR(vol, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(vol, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 static struct kobj_attribute delay_time_attribute =
 	__ATTR(delay_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
 	__ATTR(full_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
diff --git a/drivers/staging/speakup/speakup_dummy.c b/drivers/staging/speakup/speakup_dummy.c
index e19e9994bbb..362342a194a 100644
--- a/drivers/staging/speakup/speakup_dummy.c
+++ b/drivers/staging/speakup/speakup_dummy.c
@@ -46,22 +46,22 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/dummy.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute tone_attribute =
-	__ATTR(tone, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(tone, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute vol_attribute =
-	__ATTR(vol, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(vol, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 static struct kobj_attribute delay_time_attribute =
 	__ATTR(delay_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
 	__ATTR(full_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
diff --git a/drivers/staging/speakup/speakup_keypc.c b/drivers/staging/speakup/speakup_keypc.c
index 9c246d701a9..9d30c1945f9 100644
--- a/drivers/staging/speakup/speakup_keypc.c
+++ b/drivers/staging/speakup/speakup_keypc.c
@@ -59,18 +59,18 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/keypc.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 static struct kobj_attribute delay_time_attribute =
 	__ATTR(delay_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
 	__ATTR(full_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
diff --git a/drivers/staging/speakup/speakup_ltlk.c b/drivers/staging/speakup/speakup_ltlk.c
index c9be6f52c25..d6de72295d3 100644
--- a/drivers/staging/speakup/speakup_ltlk.c
+++ b/drivers/staging/speakup/speakup_ltlk.c
@@ -50,28 +50,28 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/ltlk.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute freq_attribute =
-	__ATTR(freq, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(freq, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute punct_attribute =
-	__ATTR(punct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(punct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute tone_attribute =
-	__ATTR(tone, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(tone, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute voice_attribute =
-	__ATTR(voice, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(voice, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute vol_attribute =
-	__ATTR(vol, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(vol, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 static struct kobj_attribute delay_time_attribute =
 	__ATTR(delay_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
 	__ATTR(full_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
diff --git a/drivers/staging/speakup/speakup_soft.c b/drivers/staging/speakup/speakup_soft.c
index ee6089502a9..9ed72650926 100644
--- a/drivers/staging/speakup/speakup_soft.c
+++ b/drivers/staging/speakup/speakup_soft.c
@@ -61,35 +61,35 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/soft.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute freq_attribute =
-	__ATTR(freq, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(freq, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute punct_attribute =
-	__ATTR(punct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(punct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute tone_attribute =
-	__ATTR(tone, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(tone, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute voice_attribute =
-	__ATTR(voice, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(voice, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute vol_attribute =
-	__ATTR(vol, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(vol, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 /*
  * We should uncomment the following definition, when we agree on a
  * method of passing a language designation to the software synthesizer.
  * static struct kobj_attribute lang_attribute =
- *	__ATTR(lang, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+ *	__ATTR(lang, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
  */
 
 static struct kobj_attribute delay_time_attribute =
 	__ATTR(delay_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
 	__ATTR(full_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
diff --git a/drivers/staging/speakup/speakup_spkout.c b/drivers/staging/speakup/speakup_spkout.c
index 711cf114df8..77f2dc2c3d9 100644
--- a/drivers/staging/speakup/speakup_spkout.c
+++ b/drivers/staging/speakup/speakup_spkout.c
@@ -48,24 +48,24 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/spkout.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute punct_attribute =
-	__ATTR(punct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(punct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute tone_attribute =
-	__ATTR(tone, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(tone, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute vol_attribute =
-	__ATTR(vol, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(vol, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 static struct kobj_attribute delay_time_attribute =
 	__ATTR(delay_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
 	__ATTR(full_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
diff --git a/drivers/staging/speakup/speakup_txprt.c b/drivers/staging/speakup/speakup_txprt.c
index 3f0be04df07..dbe84b13772 100644
--- a/drivers/staging/speakup/speakup_txprt.c
+++ b/drivers/staging/speakup/speakup_txprt.c
@@ -44,22 +44,22 @@ static struct var_t vars[] = {
  * These attributes will appear in /sys/accessibility/speakup/txprt.
  */
 static struct kobj_attribute caps_start_attribute =
-	__ATTR(caps_start, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_start, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute caps_stop_attribute =
-	__ATTR(caps_stop, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(caps_stop, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
-	__ATTR(pitch, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(pitch, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
-	__ATTR(rate, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(rate, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute tone_attribute =
-	__ATTR(tone, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(tone, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute vol_attribute =
-	__ATTR(vol, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(vol, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 
 static struct kobj_attribute delay_time_attribute =
 	__ATTR(delay_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute direct_attribute =
-	__ATTR(direct, S_IWUGO|S_IRUGO, spk_var_show, spk_var_store);
+	__ATTR(direct, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute full_time_attribute =
 	__ATTR(full_time, S_IWUSR|S_IRUGO, spk_var_show, spk_var_store);
 static struct kobj_attribute jiffy_delta_attribute =
diff --git a/drivers/staging/speakup/synth.c b/drivers/staging/speakup/synth.c
index 0b3549bd909..172cf62b1aa 100644
--- a/drivers/staging/speakup/synth.c
+++ b/drivers/staging/speakup/synth.c
@@ -212,6 +212,9 @@ void synth_start(void)
 
 void spk_do_flush(void)
 {
+	if (!synth)
+		return;
+
 	speakup_info.flushing = 1;
 	synth_buffer_clear();
 	if (synth->alive) {
diff --git a/drivers/staging/tidspbridge/core/dsp-clock.c b/drivers/staging/tidspbridge/core/dsp-clock.c
index 2f084e181d3..a1aca4416ca 100644
--- a/drivers/staging/tidspbridge/core/dsp-clock.c
+++ b/drivers/staging/tidspbridge/core/dsp-clock.c
@@ -226,7 +226,7 @@ int dsp_clk_enable(enum dsp_clk_id clk_id)
 	case GPT_CLK:
 		status = omap_dm_timer_start(timer[clk_id - 1]);
 		break;
-#ifdef CONFIG_OMAP_MCBSP
+#ifdef CONFIG_SND_OMAP_SOC_MCBSP
 	case MCBSP_CLK:
 		omap_mcbsp_request(MCBSP_ID(clk_id));
 		omap2_mcbsp_set_clks_src(MCBSP_ID(clk_id), MCBSP_CLKS_PAD_SRC);
@@ -302,7 +302,7 @@ int dsp_clk_disable(enum dsp_clk_id clk_id)
 	case GPT_CLK:
 		status = omap_dm_timer_stop(timer[clk_id - 1]);
 		break;
-#ifdef CONFIG_OMAP_MCBSP
+#ifdef CONFIG_SND_OMAP_SOC_MCBSP
 	case MCBSP_CLK:
 		omap2_mcbsp_set_clks_src(MCBSP_ID(clk_id), MCBSP_CLKS_PRCM_SRC);
 		omap_mcbsp_free(MCBSP_ID(clk_id));
diff --git a/drivers/staging/unisys/channels/channel.c b/drivers/staging/unisys/channels/channel.c
index f6452595b74..7223a14082b 100644
--- a/drivers/staging/unisys/channels/channel.c
+++ b/drivers/staging/unisys/channels/channel.c
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/channels/chanstub.c b/drivers/staging/unisys/channels/chanstub.c
index f504f49a436..1e7d6a78602 100644
--- a/drivers/staging/unisys/channels/chanstub.c
+++ b/drivers/staging/unisys/channels/chanstub.c
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -25,11 +25,14 @@
 
 #include "channel.h"
 #include "chanstub.h"
+#include "timskmodutils.h"
 #include "version.h"
 
 static __init int
 channel_mod_init(void)
 {
+	if (!unisys_spar_platform)
+		return -ENODEV;
 	return 0;
 }
 
diff --git a/drivers/staging/unisys/channels/chanstub.h b/drivers/staging/unisys/channels/chanstub.h
index 8d727debca6..bdee5d529f6 100644
--- a/drivers/staging/unisys/channels/chanstub.h
+++ b/drivers/staging/unisys/channels/chanstub.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/common-spar/include/channels/channel.h b/drivers/staging/unisys/common-spar/include/channels/channel.h
index aee204172b2..d19711de114 100644
--- a/drivers/staging/unisys/common-spar/include/channels/channel.h
+++ b/drivers/staging/unisys/common-spar/include/channels/channel.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -16,6 +16,8 @@
 #ifndef __CHANNEL_H__
 #define __CHANNEL_H__
 
+#include <linux/uuid.h>
+
 /*
 * Whenever this file is changed a corresponding change must be made in
 * the Console/ServicePart/visordiag_early/supervisor_channel.h file
@@ -24,7 +26,7 @@
 */
 
 /* define the following to prevent include nesting in kernel header
- * files of similar abreviated content
+ * files of similar abbreviated content
  */
 #define __SUPERVISOR_CHANNEL_H__
 
@@ -46,19 +48,6 @@
 #define COVER(v, d)   ((d)*COVERQ(v, d))
 #endif
 
-#ifndef GUID0
-#define GUID0 {0, 0, 0, {0, 0, 0, 0, 0, 0, 0, 0} }
-#endif
-
-/*  The C language is inconsistent with respect to where it allows literal
- *  constants, especially literal constant structs.  Literal constant structs
- *  are allowed for initialization only, whereas other types of literal
- *  constants are allowed anywhere.  We get around this inconsistency by
- *  declaring a "static const" variable for each GUID.  This variable can be
- *  used in expressions where the literal constant would not be allowed.
- */
-static const GUID Guid0 = GUID0;
-
 #define ULTRA_CHANNEL_PROTOCOL_SIGNATURE  SIGNATURE_32('E', 'C', 'N', 'L')
 
 typedef enum {
@@ -226,13 +215,13 @@ typedef struct _CHANNEL_HEADER {
 	U32 HeaderSize;		/* sizeof(CHANNEL_HEADER) */
 	U64 Size;		/* Total size of this channel in bytes */
 	U64 Features;		/* Flags to modify behavior */
-	GUID Type;		/* Channel type: data, bus, control, etc. */
+	uuid_le Type;		/* Channel type: data, bus, control, etc. */
 	U64 PartitionHandle;	/* ID of guest partition */
 	U64 Handle;		/* Device number of this channel in client */
 	U64 oChannelSpace;	/* Offset in bytes to channel specific area */
 	U32 VersionId;		/* CHANNEL_HEADER Version ID */
 	U32 PartitionIndex;	/* Index of guest partition */
-	GUID ZoneGuid;		/* Guid of Channel's zone */
+	uuid_le ZoneGuid;		/* Guid of Channel's zone */
 	U32 oClientString;	/* offset from channel header to
 				 * nul-terminated ClientString (0 if
 				 * ClientString not present) */
@@ -320,17 +309,17 @@ typedef struct _SIGNAL_QUEUE_HEADER {
  */
 static inline int
 ULTRA_check_channel_client(void __iomem *pChannel,
-			   GUID expectedTypeGuid,
+			   uuid_le expectedTypeGuid,
 			   char *channelName,
 			   U64 expectedMinBytes,
 			   U32 expectedVersionId,
 			   U64 expectedSignature,
 			   char *fileName, int lineNumber, void *logCtx)
 {
-	if (MEMCMP(&expectedTypeGuid, &Guid0, sizeof(GUID)) != 0)
+	if (uuid_le_cmp(expectedTypeGuid, NULL_UUID_LE) != 0)
 		/* caller wants us to verify type GUID */
 		if (MEMCMP_IO(&(((CHANNEL_HEADER __iomem *) (pChannel))->Type),
-			   &expectedTypeGuid, sizeof(GUID)) != 0) {
+			   &expectedTypeGuid, sizeof(uuid_le)) != 0) {
 			CHANNEL_GUID_MISMATCH(expectedTypeGuid, channelName,
 					      "type", expectedTypeGuid,
 					      ((CHANNEL_HEADER __iomem *)
@@ -344,8 +333,9 @@ ULTRA_check_channel_client(void __iomem *pChannel,
 			   (pChannel))->Size) < expectedMinBytes) {
 			CHANNEL_U64_MISMATCH(expectedTypeGuid, channelName,
 					     "size", expectedMinBytes,
-					     ((CHANNEL_HEADER __iomem *)
-					      (pChannel))->Size, fileName,
+					     readq(&((CHANNEL_HEADER __iomem *)
+						     (pChannel))->Size),
+					     fileName,
 					     lineNumber, logCtx);
 			return 0;
 		}
@@ -355,9 +345,9 @@ ULTRA_check_channel_client(void __iomem *pChannel,
 		    != expectedVersionId) {
 			CHANNEL_U32_MISMATCH(expectedTypeGuid, channelName,
 					     "version", expectedVersionId,
-					     ((CHANNEL_HEADER __iomem *)
-					      (pChannel))->VersionId, fileName,
-					     lineNumber, logCtx);
+					     readl(&((CHANNEL_HEADER __iomem *)
+						     (pChannel))->VersionId),
+					     fileName, lineNumber, logCtx);
 			return 0;
 		}
 	if (expectedSignature > 0)	/* caller wants us to verify
@@ -366,8 +356,9 @@ ULTRA_check_channel_client(void __iomem *pChannel,
 		    != expectedSignature) {
 			CHANNEL_U64_MISMATCH(expectedTypeGuid, channelName,
 					     "signature", expectedSignature,
-					     ((CHANNEL_HEADER __iomem *)
-					      (pChannel))->Signature, fileName,
+					     readq(&((CHANNEL_HEADER __iomem *)
+						     (pChannel))->Signature),
+					     fileName,
 					     lineNumber, logCtx);
 			return 0;
 		}
@@ -380,7 +371,7 @@ ULTRA_check_channel_client(void __iomem *pChannel,
  * is used to pass the EFI_DIAG_CAPTURE_PROTOCOL needed to log messages.
  */
 static inline int
-ULTRA_check_channel_server(GUID typeGuid,
+ULTRA_check_channel_server(uuid_le typeGuid,
 			   char *channelName,
 			   U64 expectedMinBytes,
 			   U64 actualBytes,
diff --git a/drivers/staging/unisys/common-spar/include/channels/channel_guid.h b/drivers/staging/unisys/common-spar/include/channels/channel_guid.h
index ae0dc2b2ad1..63c67ca4c9e 100644
--- a/drivers/staging/unisys/common-spar/include/channels/channel_guid.h
+++ b/drivers/staging/unisys/common-spar/include/channels/channel_guid.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -21,44 +21,44 @@
  * {414815ed-c58c-11da-95a9-00e08161165f}
  */
 #define ULTRA_VHBA_CHANNEL_PROTOCOL_GUID \
-	{ 0x414815ed, 0xc58c, 0x11da, \
-		{ 0x95, 0xa9, 0x0, 0xe0, 0x81, 0x61, 0x16, 0x5f } }
-static const GUID UltraVhbaChannelProtocolGuid =
+		UUID_LE(0x414815ed, 0xc58c, 0x11da, \
+				0x95, 0xa9, 0x0, 0xe0, 0x81, 0x61, 0x16, 0x5f)
+static const uuid_le UltraVhbaChannelProtocolGuid =
 	ULTRA_VHBA_CHANNEL_PROTOCOL_GUID;
 
 /* Used in IOChannel
  * {8cd5994d-c58e-11da-95a9-00e08161165f}
  */
 #define ULTRA_VNIC_CHANNEL_PROTOCOL_GUID \
-	{ 0x8cd5994d, 0xc58e, 0x11da, \
-		{ 0x95, 0xa9, 0x0, 0xe0, 0x81, 0x61, 0x16, 0x5f } }
-static const GUID UltraVnicChannelProtocolGuid =
+		UUID_LE(0x8cd5994d, 0xc58e, 0x11da, \
+				0x95, 0xa9, 0x0, 0xe0, 0x81, 0x61, 0x16, 0x5f)
+static const uuid_le UltraVnicChannelProtocolGuid =
 	ULTRA_VNIC_CHANNEL_PROTOCOL_GUID;
 
 /* Used in IOChannel
  * {72120008-4AAB-11DC-8530-444553544200}
  */
 #define ULTRA_SIOVM_GUID \
-	{ 0x72120008, 0x4AAB, 0x11DC,					\
-		{ 0x85, 0x30, 0x44, 0x45, 0x53, 0x54, 0x42, 0x00 } }
-static const GUID UltraSIOVMGuid = ULTRA_SIOVM_GUID;
+		UUID_LE(0x72120008, 0x4AAB, 0x11DC, \
+				0x85, 0x30, 0x44, 0x45, 0x53, 0x54, 0x42, 0x00)
+static const uuid_le UltraSIOVMGuid = ULTRA_SIOVM_GUID;
 
 
 /* Used in visornoop/visornoop_main.c
  * {5b52c5ac-e5f5-4d42-8dff-429eaecd221f}
  */
 #define ULTRA_CONTROLDIRECTOR_CHANNEL_PROTOCOL_GUID  \
-	{ 0x5b52c5ac, 0xe5f5, 0x4d42, \
-		{ 0x8d, 0xff, 0x42, 0x9e, 0xae, 0xcd, 0x22, 0x1f } }
+		UUID_LE(0x5b52c5ac, 0xe5f5, 0x4d42, \
+				0x8d, 0xff, 0x42, 0x9e, 0xae, 0xcd, 0x22, 0x1f)
 
-static const GUID UltraControlDirectorChannelProtocolGuid =
+static const uuid_le UltraControlDirectorChannelProtocolGuid =
 	ULTRA_CONTROLDIRECTOR_CHANNEL_PROTOCOL_GUID;
 
 /* Used in visorchipset/visorchipset_main.c
  * {B4E79625-AEDE-4EAA-9E11-D3EDDCD4504C}
  */
 #define ULTRA_DIAG_POOL_CHANNEL_PROTOCOL_GUID				\
-	{0xb4e79625, 0xaede, 0x4eaa,					\
-		{ 0x9e, 0x11, 0xd3, 0xed, 0xdc, 0xd4, 0x50, 0x4c } }
+		UUID_LE(0xb4e79625, 0xaede, 0x4eaa, \
+				0x9e, 0x11, 0xd3, 0xed, 0xdc, 0xd4, 0x50, 0x4c)
 
 
diff --git a/drivers/staging/unisys/common-spar/include/channels/controlframework.h b/drivers/staging/unisys/common-spar/include/channels/controlframework.h
index 51264334834..1a1c5053fcf 100644
--- a/drivers/staging/unisys/common-spar/include/channels/controlframework.h
+++ b/drivers/staging/unisys/common-spar/include/channels/controlframework.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/common-spar/include/channels/controlvmchannel.h b/drivers/staging/unisys/common-spar/include/channels/controlvmchannel.h
index 47f1c4fa1e7..d8b12a73348 100644
--- a/drivers/staging/unisys/common-spar/include/channels/controlvmchannel.h
+++ b/drivers/staging/unisys/common-spar/include/channels/controlvmchannel.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -16,6 +16,7 @@
 #ifndef __CONTROLVMCHANNEL_H__
 #define __CONTROLVMCHANNEL_H__
 
+#include <linux/uuid.h>
 #include "commontypes.h"
 #include "channel.h"
 #include "controlframework.h"
@@ -25,10 +26,10 @@ enum { INVALID_GUEST_FIRMWARE, SAMPLE_GUEST_FIRMWARE,
 
 /* {2B3C2D10-7EF5-4ad8-B966-3448B7386B3D} */
 #define ULTRA_CONTROLVM_CHANNEL_PROTOCOL_GUID	\
-	{0x2b3c2d10, 0x7ef5, 0x4ad8, \
-		{0xb9, 0x66, 0x34, 0x48, 0xb7, 0x38, 0x6b, 0x3d} }
+		UUID_LE(0x2b3c2d10, 0x7ef5, 0x4ad8, \
+				0xb9, 0x66, 0x34, 0x48, 0xb7, 0x38, 0x6b, 0x3d)
 
-static const GUID UltraControlvmChannelProtocolGuid =
+static const uuid_le UltraControlvmChannelProtocolGuid =
 	ULTRA_CONTROLVM_CHANNEL_PROTOCOL_GUID;
 
 #define ULTRA_CONTROLVM_CHANNEL_PROTOCOL_SIGNATURE \
@@ -293,8 +294,8 @@ typedef struct _CONTROLVM_PACKET_DEVICE_CREATE  {
 			*   can be dereferenced by the receiver
 			*   of this ControlVm command */
 	U64 channelBytes; /**< specifies size of the channel in bytes */
-	GUID dataTypeGuid;/**< specifies format of data in channel */
-	GUID devInstGuid; /**< instance guid for the device */
+	uuid_le dataTypeGuid;/**< specifies format of data in channel */
+	uuid_le devInstGuid; /**< instance guid for the device */
 	struct InterruptInfo intr; /**< specifies interrupt information */
 } CONTROLVM_PACKET_DEVICE_CREATE;	/* for CONTROLVM_DEVICE_CREATE */
 
@@ -333,9 +334,9 @@ typedef struct _CONTROLVM_MESSAGE_PACKET  {
 					      *   dereferenced by the receiver
 					      *   of this ControlVm command */
 			U64 channelBytes;    /*< size of the channel in bytes */
-			GUID busDataTypeGuid;/*< indicates format of data in bus
-					      * channel */
-			GUID busInstGuid;    /*< instance guid for the bus */
+			uuid_le busDataTypeGuid;/*< indicates format of data in
+						    bus channel */
+			uuid_le busInstGuid;    /*< instance guid for the bus */
 		} createBus;	/* for CONTROLVM_BUS_CREATE */
 		struct  {
 			U32 busNo;	      /*< bus # (0..n-1) from the msg
@@ -611,7 +612,7 @@ typedef struct _ULTRA_CONTROLVM_PARAMETERS_HEADER  {
 	U32 ClientLength;
 	U32 NameOffset;
 	U32 NameLength;
-	GUID Id;
+	uuid_le Id;
 	U32 Revision;
 	U32 Reserved;		/* Natural alignment */
 } ULTRA_CONTROLVM_PARAMETERS_HEADER;
diff --git a/drivers/staging/unisys/common-spar/include/channels/diagchannel.h b/drivers/staging/unisys/common-spar/include/channels/diagchannel.h
index c93515eb211..1bea2f720e4 100644
--- a/drivers/staging/unisys/common-spar/include/channels/diagchannel.h
+++ b/drivers/staging/unisys/common-spar/include/channels/diagchannel.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -38,16 +38,16 @@
 
 /* {EEA7A573-DB82-447c-8716-EFBEAAAE4858} */
 #define ULTRA_DIAG_CHANNEL_PROTOCOL_GUID \
-	{0xeea7a573, 0xdb82, 0x447c, \
-		{0x87, 0x16, 0xef, 0xbe, 0xaa, 0xae, 0x48, 0x58} }
+		UUID_LE(0xeea7a573, 0xdb82, 0x447c, \
+				0x87, 0x16, 0xef, 0xbe, 0xaa, 0xae, 0x48, 0x58)
 
-static const GUID UltraDiagChannelProtocolGuid =
+static const uuid_le UltraDiagChannelProtocolGuid =
 	ULTRA_DIAG_CHANNEL_PROTOCOL_GUID;
 
 /* {E850F968-3263-4484-8CA5-2A35D087A5A8} */
 #define ULTRA_DIAG_ROOT_CHANNEL_PROTOCOL_GUID \
-	{0xe850f968, 0x3263, 0x4484, \
-		{0x8c, 0xa5, 0x2a, 0x35, 0xd0, 0x87, 0xa5, 0xa8} }
+		UUID_LE(0xe850f968, 0x3263, 0x4484, \
+				0x8c, 0xa5, 0x2a, 0x35, 0xd0, 0x87, 0xa5, 0xa8)
 
 #define ULTRA_DIAG_CHANNEL_PROTOCOL_SIGNATURE  ULTRA_CHANNEL_PROTOCOL_SIGNATURE
 
@@ -356,7 +356,7 @@ typedef enum  {
  *IsChannelInitialized: 1 iff SignalInit was called for this channel; otherwise
  *			0, and assume the channel is not ready for use yet.
  *
- * Reserved: Padding to allign the fields in this structure.
+ * Reserved: Padding to align the fields in this structure.
  *
  *SubsystemSeverityFilter: Level of severity on a subsystem basis that controls
  *			whether events are logged.  Any event's severity for a
diff --git a/drivers/staging/unisys/common-spar/include/channels/iochannel.h b/drivers/staging/unisys/common-spar/include/channels/iochannel.h
index 8de1d249d55..6dcfa6e7837 100644
--- a/drivers/staging/unisys/common-spar/include/channels/iochannel.h
+++ b/drivers/staging/unisys/common-spar/include/channels/iochannel.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION */
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION */
 /* All rights reserved. */
 #ifndef __IOCHANNEL_H__
 #define __IOCHANNEL_H__
@@ -29,6 +29,8 @@
 *        CHANNEL_ATTACHED -> CHANNEL_OPENED (performed only by GuestPart)
 */
 
+#include <linux/uuid.h>
+
 #include "commontypes.h"
 #include "vmcallinterface.h"
 
@@ -192,7 +194,7 @@ typedef enum { NET_RCV_POST = 0,	/* submit buffer to hold receiving
 	/* uisnic -> virtnic */
 	NET_MACADDR,		/* indicates the client has requested to update
 				 * its MAC addr */
-	NET_MACADDR_ACK,	/* Mac addres  */
+	NET_MACADDR_ACK,	/* MAC address  */
 
 } NET_TYPES;
 
@@ -696,7 +698,7 @@ typedef struct _ULTRA_IO_CHANNEL_PROTOCOL {
 			U8 macaddr[MAX_MACADDR_LEN];	/* 6 bytes */
 			U32 num_rcv_bufs;	/* 4 */
 			U32 mtu;	/* 4 */
-			GUID zoneGuid;	/* 16 */
+			uuid_le zoneGuid;	/* 16 */
 		} vnic;		/* total     30 */
 	};
 
@@ -807,7 +809,7 @@ static inline int ULTRA_VHBA_init_channel(ULTRA_IO_CHANNEL_PROTOCOL *x,
 	x->ChannelHeader.HeaderSize = sizeof(x->ChannelHeader);
 	x->ChannelHeader.Size = COVER(bytes, 4096);
 	x->ChannelHeader.Type = UltraVhbaChannelProtocolGuid;
-	x->ChannelHeader.ZoneGuid = Guid0;
+	x->ChannelHeader.ZoneGuid = NULL_UUID_LE;
 	x->vhba.wwnn = *wwnn;
 	x->vhba.max = *max;
 	INIT_CLIENTSTRING(x, ULTRA_IO_CHANNEL_PROTOCOL, clientStr,
@@ -832,7 +834,7 @@ static inline void ULTRA_VHBA_set_max(ULTRA_IO_CHANNEL_PROTOCOL *x,
 static inline int ULTRA_VNIC_init_channel(ULTRA_IO_CHANNEL_PROTOCOL *x,
 						 unsigned char *macaddr,
 						 U32 num_rcv_bufs, U32 mtu,
-						 GUID zoneGuid,
+						 uuid_le zoneGuid,
 						 unsigned char *clientStr,
 						 U32 clientStrLen,
 						 U64 bytes)  {
@@ -843,7 +845,7 @@ static inline int ULTRA_VNIC_init_channel(ULTRA_IO_CHANNEL_PROTOCOL *x,
 	x->ChannelHeader.HeaderSize = sizeof(x->ChannelHeader);
 	x->ChannelHeader.Size = COVER(bytes, 4096);
 	x->ChannelHeader.Type = UltraVnicChannelProtocolGuid;
-	x->ChannelHeader.ZoneGuid = Guid0;
+	x->ChannelHeader.ZoneGuid = NULL_UUID_LE;
 	MEMCPY(x->vnic.macaddr, macaddr, MAX_MACADDR_LEN);
 	x->vnic.num_rcv_bufs = num_rcv_bufs;
 	x->vnic.mtu = mtu;
diff --git a/drivers/staging/unisys/common-spar/include/channels/vbuschannel.h b/drivers/staging/unisys/common-spar/include/channels/vbuschannel.h
index 99dbbcf3d11..0dd3e2dd0e1 100644
--- a/drivers/staging/unisys/common-spar/include/channels/vbuschannel.h
+++ b/drivers/staging/unisys/common-spar/include/channels/vbuschannel.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -23,15 +23,16 @@
  *  We currently use this for the client to provide various information about
  *  the client devices and client drivers for the server end to see.
  */
+#include <linux/uuid.h>
 #include "commontypes.h"
 #include "vbusdeviceinfo.h"
 #include "channel.h"
 
 /* {193b331b-c58f-11da-95a9-00e08161165f} */
 #define ULTRA_VBUS_CHANNEL_PROTOCOL_GUID \
-	{0x193b331b, 0xc58f, 0x11da, \
-		{0x95, 0xa9, 0x0, 0xe0, 0x81, 0x61, 0x16, 0x5f} }
-static const GUID UltraVbusChannelProtocolGuid =
+		UUID_LE(0x193b331b, 0xc58f, 0x11da, \
+				0x95, 0xa9, 0x0, 0xe0, 0x81, 0x61, 0x16, 0x5f)
+static const uuid_le UltraVbusChannelProtocolGuid =
 	ULTRA_VBUS_CHANNEL_PROTOCOL_GUID;
 
 #define ULTRA_VBUS_CHANNEL_PROTOCOL_SIGNATURE ULTRA_CHANNEL_PROTOCOL_SIGNATURE
@@ -112,8 +113,7 @@ ULTRA_VBUS_init_channel(ULTRA_VBUS_CHANNEL_PROTOCOL __iomem *x,
 	writeq(bytesAllocated, &x->ChannelHeader.Size);
 	memcpy_toio(&x->ChannelHeader.Type, &UltraVbusChannelProtocolGuid,
 		    sizeof(x->ChannelHeader.Type));
-	memcpy_toio(&x->ChannelHeader.ZoneGuid, &Guid0,
-		    sizeof(x->ChannelHeader.ZoneGuid));
+	memcpy_toio(&x->ChannelHeader.ZoneGuid, &NULL_UUID_LE, sizeof(uuid_le));
 	writel(sizeof(ULTRA_VBUS_HEADERINFO), &x->HdrInfo.structBytes);
 	writel(sizeof(ULTRA_VBUS_HEADERINFO), &x->HdrInfo.chpInfoByteOffset);
 	writel(readl(&x->HdrInfo.chpInfoByteOffset) +
diff --git a/drivers/staging/unisys/common-spar/include/controlvmcompletionstatus.h b/drivers/staging/unisys/common-spar/include/controlvmcompletionstatus.h
index de30d321d98..db77d6f626a 100644
--- a/drivers/staging/unisys/common-spar/include/controlvmcompletionstatus.h
+++ b/drivers/staging/unisys/common-spar/include/controlvmcompletionstatus.h
@@ -1,6 +1,6 @@
 /* controlvmcompletionstatus.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All Rights Reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/common-spar/include/diagnostics/appos_subsystems.h b/drivers/staging/unisys/common-spar/include/diagnostics/appos_subsystems.h
index 4c6294d2060..18cc9ed2748 100644
--- a/drivers/staging/unisys/common-spar/include/diagnostics/appos_subsystems.h
+++ b/drivers/staging/unisys/common-spar/include/diagnostics/appos_subsystems.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/common-spar/include/iovmcall_gnuc.h b/drivers/staging/unisys/common-spar/include/iovmcall_gnuc.h
index 7304e9a0648..fe9598c941a 100644
--- a/drivers/staging/unisys/common-spar/include/iovmcall_gnuc.h
+++ b/drivers/staging/unisys/common-spar/include/iovmcall_gnuc.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/common-spar/include/vbusdeviceinfo.h b/drivers/staging/unisys/common-spar/include/vbusdeviceinfo.h
index ae708faaa94..8c0259a2cbc 100644
--- a/drivers/staging/unisys/common-spar/include/vbusdeviceinfo.h
+++ b/drivers/staging/unisys/common-spar/include/vbusdeviceinfo.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/common-spar/include/version.h b/drivers/staging/unisys/common-spar/include/version.h
index 00b0ebb09ea..f25208fc3ed 100644
--- a/drivers/staging/unisys/common-spar/include/version.h
+++ b/drivers/staging/unisys/common-spar/include/version.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/common-spar/include/vmcallinterface.h b/drivers/staging/unisys/common-spar/include/vmcallinterface.h
index 14c404367fa..c5c10f3f1eb 100644
--- a/drivers/staging/unisys/common-spar/include/vmcallinterface.h
+++ b/drivers/staging/unisys/common-spar/include/vmcallinterface.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/include/commontypes.h b/drivers/staging/unisys/include/commontypes.h
index ef12af4a72d..9de6f9dc5fb 100644
--- a/drivers/staging/unisys/include/commontypes.h
+++ b/drivers/staging/unisys/include/commontypes.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -17,13 +17,14 @@
 #define _COMMONTYPES_H_
 
 /* define the following to prevent include nesting in kernel header files of
- * similar abreviated content */
+ * similar abbreviated content */
 #define _SUPERVISOR_COMMONTYPES_H_
 
 #ifdef __KERNEL__
 #include <linux/types.h>
 #include <linux/version.h>
 #include <linux/io.h>
+#include <linux/uuid.h>
 #else
 #include <stdint.h>
 #include <syslog.h>
@@ -59,16 +60,6 @@
 
 #endif
 
-typedef struct {
-	U32 data1;
-	U16 data2;
-	U16 data3;
-	U8 data4[8];
-} __attribute__ ((__packed__)) GUID;
-
-#ifndef GUID0
-#define GUID0 {0, 0, 0, {0, 0, 0, 0, 0, 0, 0, 0} }
-#endif
 typedef U64 GUEST_PHYSICAL_ADDRESS;
 
 #define MEMSET(ptr, val, len) memset(ptr, val, len)
@@ -89,18 +80,16 @@ typedef U64 GUEST_PHYSICAL_ADDRESS;
 #define CHANNEL_GUID_MISMATCH(chType, chName, field, expected, actual, fil, \
 			      lin, logCtx)				\
 	do {								\
-		char s1[50], s2[50], s3[50];				\
-		pr_err("Channel mismatch on channel=%s(%s) field=%s expected=%s actual=%s @%s:%d\n", \
-		       chName, GUID_format2(&chType, s1), field,	\
-		       GUID_format2(&expected, s2), GUID_format2(&actual, s3), \
+		pr_err("Channel mismatch on channel=%s(%pUL) field=%s expected=%pUL actual=%pUL @%s:%d\n", \
+		       chName, &chType, field,	\
+		       &expected, &actual, \
 		       fil, lin);					\
 	} while (0)
 #define CHANNEL_U32_MISMATCH(chType, chName, field, expected, actual, fil, \
 			     lin, logCtx)				\
 	do {								\
-		char s1[50];						\
-		pr_err("Channel mismatch on channel=%s(%s) field=%s expected=0x%-8.8lx actual=0x%-8.8lx @%s:%d\n", \
-		       chName, GUID_format2(&chType, s1), field,	\
+		pr_err("Channel mismatch on channel=%s(%pUL) field=%s expected=0x%-8.8lx actual=0x%-8.8lx @%s:%d\n", \
+		       chName, &chType, field,	\
 		       (unsigned long)expected, (unsigned long)actual,	\
 		       fil, lin);					\
 	} while (0)
@@ -108,9 +97,8 @@ typedef U64 GUEST_PHYSICAL_ADDRESS;
 #define CHANNEL_U64_MISMATCH(chType, chName, field, expected, actual, fil, \
 			     lin, logCtx)				\
 	do {								\
-		char s1[50];						\
-		pr_err("Channel mismatch on channel=%s(%s) field=%s expected=0x%-8.8Lx actual=0x%-8.8Lx @%s:%d\n", \
-		       chName, GUID_format2(&chType, s1), field,	\
+		pr_err("Channel mismatch on channel=%s(%pUL) field=%s expected=0x%-8.8Lx actual=0x%-8.8Lx @%s:%d\n", \
+		       chName, &chType, field,	\
 		       (unsigned long long)expected,			\
 		       (unsigned long long)actual,			\
 		       fil, lin);					\
@@ -128,21 +116,19 @@ typedef U64 GUEST_PHYSICAL_ADDRESS;
 #define CHANNEL_GUID_MISMATCH(chType, chName, field, expected, actual, fil, \
 			      lin, logCtx)				\
 	do {								\
-		char s1[50], s2[50], s3[50];				\
 		syslog(LOG_USER | LOG_ERR,				\
-		       "Channel mismatch on channel=%s(%s) field=%s expected=%s actual=%s @%s:%d", \
-		       chName, GUID_format2(&chType, s1), field,	\
-		       GUID_format2(&expected, s2), GUID_format2(&actual, s3), \
+		       "Channel mismatch on channel=%s(%pUL) field=%s expected=%pUL actual=%pUL @%s:%d", \
+		       chName, &chType, field,	\
+		       &expected, &actual, \
 		       fil, lin);					\
 	} while (0)
 
 #define CHANNEL_U32_MISMATCH(chType, chName, field, expected, actual, fil, \
 			     lin, logCtx)				\
 	do {								\
-		char s1[50];						\
 		syslog(LOG_USER | LOG_ERR,				\
-		       "Channel mismatch on channel=%s(%s) field=%s expected=0x%-8.8lx actual=0x%-8.8lx @%s:%d", \
-		       chName, GUID_format2(&chType, s1), field,	\
+		       "Channel mismatch on channel=%s(%pUL) field=%s expected=0x%-8.8lx actual=0x%-8.8lx @%s:%d", \
+		       chName, chType, field,	\
 		       (unsigned long)expected, (unsigned long)actual,	\
 		       fil, lin);					\
 	} while (0)
@@ -150,10 +136,9 @@ typedef U64 GUEST_PHYSICAL_ADDRESS;
 #define CHANNEL_U64_MISMATCH(chType, chName, field, expected, actual, fil, \
 			     lin, logCtx)				\
 	do {								\
-		char s1[50];						\
 		syslog(LOG_USER | LOG_ERR,				\
-		       "Channel mismatch on channel=%s(%s) field=%s expected=0x%-8.8Lx actual=0x%-8.8Lx @%s:%d", \
-		       chName, GUID_format2(&chType, s1), field,	\
+		       "Channel mismatch on channel=%s(%pUL) field=%s expected=0x%-8.8Lx actual=0x%-8.8Lx @%s:%d", \
+		       chName, chType, field,	\
 		       (unsigned long long)expected,			\
 		       (unsigned long long)actual,			\
 		       fil, lin);					\
@@ -167,4 +152,4 @@ typedef U64 GUEST_PHYSICAL_ADDRESS;
 #define VolatileBarrier() MEMORYBARRIER
 
 #endif
-#include "guidutils.h"
+
diff --git a/drivers/staging/unisys/include/guestlinuxdebug.h b/drivers/staging/unisys/include/guestlinuxdebug.h
index c3de8496e5d..efc4005368b 100644
--- a/drivers/staging/unisys/include/guestlinuxdebug.h
+++ b/drivers/staging/unisys/include/guestlinuxdebug.h
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -17,7 +17,7 @@
 #define __GUESTLINUXDEBUG_H__
 
 /*
-* This file contains supporting interface for "vmcallinterface.h", particuarly
+* This file contains supporting interface for "vmcallinterface.h", particularly
 * regarding adding additional structure and functionality to linux
 * ISSUE_IO_VMCALL_POSTCODE_SEVERITY */
 
@@ -142,7 +142,7 @@ typedef enum {			/* POSTCODE event identifier tuples */
  * Please also note that the resulting postcode is in hex, so if you are
  * searching for the __LINE__ number, convert it first to decimal.  The line
  * number combined with driver and type of call, will allow you to track down
- * exactly what line an error occured on, or where the last driver
+ * exactly what line an error occurred on, or where the last driver
  * entered/exited from.
  */
 
diff --git a/drivers/staging/unisys/include/guidutils.h b/drivers/staging/unisys/include/guidutils.h
deleted file mode 100644
index 75caf929cd6..00000000000
--- a/drivers/staging/unisys/include/guidutils.h
+++ /dev/null
@@ -1,203 +0,0 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
- * 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, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-/* guidutils.h
- *
- * These are GUID manipulation inlines that can be used from either
- * kernel-mode or user-mode.
- *
- */
-#ifndef __GUIDUTILS_H__
-#define __GUIDUTILS_H__
-
-#ifdef __KERNEL__
-#include <linux/kernel.h>
-#include <linux/string.h>
-#include <linux/ctype.h>
-#define GUID_STRTOUL kstrtoul
-#else
-#include <stdio.h>
-#include <ctype.h>
-#include <string.h>
-#include <stdlib.h>
-
-#define GUID_STRTOUL strtoul
-#endif
-
-static inline char *
-GUID_format1(const GUID *guid, char *s)
-{
-	sprintf(s, "{%-8.8lx-%-4.4x-%-4.4x-%-2.2x%-2.2x%-2.2x%-2.2x%-2.2x%-2.2x%-2.2x%-2.2x}",
-		(ulong) guid->data1,
-		guid->data2,
-		guid->data3,
-		guid->data4[0],
-		guid->data4[1],
-		guid->data4[2],
-		guid->data4[3],
-		guid->data4[4], guid->data4[5], guid->data4[6], guid->data4[7]);
-	return s;
-}
-
-/** Format a GUID in Microsoft's 'what in the world were they thinking'
- *  format.
- */
-static inline char *
-GUID_format2(const GUID *guid, char *s)
-{
-	sprintf(s, "{%-8.8lx-%-4.4x-%-4.4x-%-2.2x%-2.2x-%-2.2x%-2.2x%-2.2x%-2.2x%-2.2x%-2.2x}",
-		(ulong) guid->data1,
-		guid->data2,
-		guid->data3,
-		guid->data4[0],
-		guid->data4[1],
-		guid->data4[2],
-		guid->data4[3],
-		guid->data4[4], guid->data4[5], guid->data4[6], guid->data4[7]);
-	return s;
-}
-
-/**
- * Like GUID_format2 but without the curly braces and the
- * hex digits in upper case
- */
-static inline char *
-GUID_format3(const GUID *guid, char *s)
-{
-	sprintf(s, "%-8.8lX-%-4.4X-%-4.4X-%-2.2X%-2.2X-%-2.2X%-2.2X%-2.2X%-2.2X%-2.2X%-2.2X",
-		(ulong) guid->data1,
-		guid->data2,
-		guid->data3,
-		guid->data4[0],
-		guid->data4[1],
-		guid->data4[2],
-		guid->data4[3],
-		guid->data4[4], guid->data4[5], guid->data4[6], guid->data4[7]);
-	return s;
-}
-
-/** Parse a guid string in any of these forms:
- *      {11111111-2222-3333-4455-66778899aabb}
- *      {11111111-2222-3333-445566778899aabb}
- *      11111111-2222-3333-4455-66778899aabb
- *      11111111-2222-3333-445566778899aabb
- */
-static inline GUID
-GUID_scan(U8 *p)
-{
-	GUID guid = GUID0;
-	U8 x[33];
-	int count = 0;
-	int c, i = 0;
-	U8 cdata1[9];
-	U8 cdata2[5];
-	U8 cdata3[5];
-	U8 cdata4[3];
-	int dashcount = 0;
-	int brace = 0;
-	unsigned long uldata;
-
-	if (!p)
-		return guid;
-	if (*p == '{') {
-		p++;
-		brace = 1;
-	}
-	while (count < 32) {
-		if (*p == '}')
-			return guid;
-		if (*p == '\0')
-			return guid;
-		c = toupper(*p);
-		p++;
-		if (c == '-') {
-			switch (dashcount) {
-			case 0:
-				if (i != 8)
-					return guid;
-				break;
-			case 1:
-				if (i != 4)
-					return guid;
-				break;
-			case 2:
-				if (i != 4)
-					return guid;
-				break;
-			case 3:
-				if (i != 4)
-					return guid;
-				break;
-			default:
-				return guid;
-			}
-			dashcount++;
-			i = 0;
-			continue;
-		}
-		if ((c >= '0' && c <= '9') || (c >= 'A' && c <= 'F'))
-			i++;
-		else
-			return guid;
-		x[count++] = c;
-	}
-	x[count] = '\0';
-	if (brace) {
-		if (*p == '}')
-			p++;
-		else
-			return guid;
-	}
-	if (dashcount == 3 || dashcount == 4)
-		;
-	else
-		return guid;
-	memset(cdata1, 0, sizeof(cdata1));
-	memset(cdata2, 0, sizeof(cdata2));
-	memset(cdata3, 0, sizeof(cdata3));
-	memset(cdata4, 0, sizeof(cdata4));
-	memcpy(cdata1, x + 0, 8);
-	memcpy(cdata2, x + 8, 4);
-	memcpy(cdata3, x + 12, 4);
-
-	if (GUID_STRTOUL((char *) cdata1, 16, &uldata) == 0)
-		guid.data1 = (U32)uldata;
-	if (GUID_STRTOUL((char *) cdata2, 16, &uldata) == 0)
-		guid.data2 = (U16)uldata;
-	if (GUID_STRTOUL((char *) cdata3, 16, &uldata) == 0)
-		guid.data3 = (U16)uldata;
-
-	for (i = 0; i < 8; i++) {
-		memcpy(cdata4, x + 16 + (i * 2), 2);
-		if (GUID_STRTOUL((char *) cdata4, 16, &uldata) == 0)
-			guid.data4[i] = (U8) uldata;
-	}
-
-	return guid;
-}
-
-static inline char *
-GUID_sanitize(char *inputGuidStr, char *outputGuidStr)
-{
-	GUID g;
-	GUID guid0 = GUID0;
-	*outputGuidStr = '\0';
-	g = GUID_scan((U8 *) inputGuidStr);
-	if (memcmp(&g, &guid0, sizeof(GUID)) == 0)
-		return outputGuidStr;	/* bad GUID format */
-	return GUID_format1(&g, outputGuidStr);
-}
-
-#endif
diff --git a/drivers/staging/unisys/include/periodic_work.h b/drivers/staging/unisys/include/periodic_work.h
index 6c7190bdcd6..418ba634e1a 100644
--- a/drivers/staging/unisys/include/periodic_work.h
+++ b/drivers/staging/unisys/include/periodic_work.h
@@ -1,6 +1,6 @@
 /* periodic_work.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/include/procobjecttree.h b/drivers/staging/unisys/include/procobjecttree.h
index c81d11287e6..1174056ec3d 100644
--- a/drivers/staging/unisys/include/procobjecttree.h
+++ b/drivers/staging/unisys/include/procobjecttree.h
@@ -1,6 +1,6 @@
 /* procobjecttree.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/include/sparstop.h b/drivers/staging/unisys/include/sparstop.h
index 3603ac60764..05837399a74 100644
--- a/drivers/staging/unisys/include/sparstop.h
+++ b/drivers/staging/unisys/include/sparstop.h
@@ -1,6 +1,6 @@
 /* sparstop.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/include/timskmod.h b/drivers/staging/unisys/include/timskmod.h
index 5fd5ad51446..ecf1a6fac7a 100644
--- a/drivers/staging/unisys/include/timskmod.h
+++ b/drivers/staging/unisys/include/timskmod.h
@@ -1,6 +1,6 @@
 /* timskmod.h
  *
- * Copyright � 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/include/timskmodutils.h b/drivers/staging/unisys/include/timskmodutils.h
index 2d81d46bf11..c316c94ea19 100644
--- a/drivers/staging/unisys/include/timskmodutils.h
+++ b/drivers/staging/unisys/include/timskmodutils.h
@@ -1,6 +1,6 @@
 /* timskmodutils.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -72,4 +72,6 @@ char *cyclesToSomethingsPerSecond(u64 cycles, u64 cyclesPerSecond,
 struct seq_file *visor_seq_file_new_buffer(void *buf, size_t buf_size);
 void visor_seq_file_done_buffer(struct seq_file *m);
 
+extern int unisys_spar_platform;
+
 #endif
diff --git a/drivers/staging/unisys/include/uisqueue.h b/drivers/staging/unisys/include/uisqueue.h
index 6dab3900994..2a5bea3b332 100644
--- a/drivers/staging/unisys/include/uisqueue.h
+++ b/drivers/staging/unisys/include/uisqueue.h
@@ -1,6 +1,6 @@
 /* uisqueue.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -28,6 +28,7 @@
 #include "uniklog.h"
 #include <linux/atomic.h>
 #include <linux/semaphore.h>
+#include <linux/uuid.h>
 
 #include "controlvmchannel.h"
 #include "controlvmcompletionstatus.h"
@@ -136,8 +137,8 @@ struct device_info {
 	void __iomem *chanptr;
 	U64 channelAddr;
 	U64 channelBytes;
-	GUID channelTypeGuid;
-	GUID devInstGuid;
+	uuid_le channelTypeGuid;
+	uuid_le devInstGuid;
 	struct InterruptInfo intr;
 	struct switch_info *swtch;
 	char devid[30];		/* "vbus<busno>:dev<devno>" */
@@ -163,7 +164,7 @@ struct bus_info {
 	U32 busNo, deviceCount;
 	struct device_info **device;
 	U64 guestHandle, recvBusInterruptHandle;
-	GUID busInstGuid;
+	uuid_le busInstGuid;
 	ULTRA_VBUS_CHANNEL_PROTOCOL __iomem *pBusChannel;
 	int busChannelBytes;
 	struct proc_dir_entry *proc_dir;	/* proc/uislib/vbus/<x> */
@@ -356,8 +357,8 @@ struct add_vbus_guestpart {
 					 * NOT YET USED */
 	U32 busNo;		/* bus number to be created/deleted */
 	U32 deviceCount;	/* max num of devices on bus */
-	GUID busTypeGuid;	/* indicates type of bus */
-	GUID busInstGuid;	/* instance guid for device */
+	uuid_le busTypeGuid;	/* indicates type of bus */
+	uuid_le busInstGuid;	/* instance guid for device */
 };
 
 struct del_vbus_guestpart {
@@ -371,7 +372,7 @@ struct add_virt_guestpart {
 	void __iomem *chanptr;		/* pointer to data channel */
 	U32 busNo;		/* bus number for the operation */
 	U32 deviceNo;		/* number of device on the bus */
-	GUID devInstGuid;	/* instance guid for device */
+	uuid_le devInstGuid;	/* instance guid for device */
 	struct InterruptInfo intr;	/* recv/send interrupt info */
 	/* recvInterruptHandle contains info needed in order to
 	 * register to receive interrupts on the data channel.
diff --git a/drivers/staging/unisys/include/uisthread.h b/drivers/staging/unisys/include/uisthread.h
index 2b1fba75909..26837771b43 100644
--- a/drivers/staging/unisys/include/uisthread.h
+++ b/drivers/staging/unisys/include/uisthread.h
@@ -1,6 +1,6 @@
 /* uisthread.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/include/uisutils.h b/drivers/staging/unisys/include/uisutils.h
index 5fdab3a3464..70776c93cc5 100644
--- a/drivers/staging/unisys/include/uisutils.h
+++ b/drivers/staging/unisys/include/uisutils.h
@@ -1,6 +1,6 @@
 /* uisutils.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -25,6 +25,7 @@
 #include <linux/io.h>
 #include <linux/sched.h>
 #include <linux/gfp.h>
+#include <linux/uuid.h>
 
 #include "vmcallinterface.h"
 #include "channel.h"
@@ -54,7 +55,7 @@ extern atomic_t UisUtils_Registered_Services;
 
 typedef unsigned int MACARRAY[MAX_MACADDR_LEN];
 typedef struct ReqHandlerInfo_struct {
-	GUID switchTypeGuid;
+	uuid_le switchTypeGuid;
 	int (*controlfunc)(struct io_msgs *);
 	unsigned long min_channel_bytes;
 	int (*Server_Channel_Ok)(unsigned long channelBytes);
@@ -64,7 +65,7 @@ typedef struct ReqHandlerInfo_struct {
 	struct list_head list_link;	/* links into ReqHandlerInfo_list */
 } ReqHandlerInfo_t;
 
-ReqHandlerInfo_t *ReqHandlerAdd(GUID switchTypeGuid,
+ReqHandlerInfo_t *ReqHandlerAdd(uuid_le switchTypeGuid,
 				const char *switch_type_name,
 				int (*controlfunc)(struct io_msgs *),
 				unsigned long min_channel_bytes,
@@ -73,8 +74,8 @@ ReqHandlerInfo_t *ReqHandlerAdd(GUID switchTypeGuid,
 				int (*Server_Channel_Init)
 				 (void *x, unsigned char *clientStr,
 				  U32 clientStrLen, U64 bytes));
-ReqHandlerInfo_t *ReqHandlerFind(GUID switchTypeGuid);
-int ReqHandlerDel(GUID switchTypeGuid);
+ReqHandlerInfo_t *ReqHandlerFind(uuid_le switchTypeGuid);
+int ReqHandlerDel(uuid_le switchTypeGuid);
 
 #define uislib_ioremap_cache(addr, size) \
 	dbg_ioremap_cache(addr, size, __FILE__, __LINE__)
@@ -112,7 +113,7 @@ int uisutil_add_proc_line_ex(int *total, char **buffer, int *buffer_remaining,
 
 int uisctrl_register_req_handler(int type, void *fptr,
 				 ULTRA_VBUS_DEVICEINFO *chipset_DriverInfo);
-int uisctrl_register_req_handler_ex(GUID switchTypeGuid,
+int uisctrl_register_req_handler_ex(uuid_le switchTypeGuid,
 				    const char *switch_type_name,
 				    int (*fptr)(struct io_msgs *),
 				    unsigned long min_channel_bytes,
@@ -123,7 +124,7 @@ int uisctrl_register_req_handler_ex(GUID switchTypeGuid,
 				     U32 clientStrLen, U64 bytes),
 				    ULTRA_VBUS_DEVICEINFO *chipset_DriverInfo);
 
-int uisctrl_unregister_req_handler_ex(GUID switchTypeGuid);
+int uisctrl_unregister_req_handler_ex(uuid_le switchTypeGuid);
 unsigned char *util_map_virt(struct phys_info *sg);
 void util_unmap_virt(struct phys_info *sg);
 unsigned char *util_map_virt_atomic(struct phys_info *sg);
@@ -133,20 +134,20 @@ int uislib_server_inject_add_vnic(U32 switchNo, U32 BusNo, U32 numIntPorts,
 				  pCHANNEL_HEADER **chan);
 void uislib_server_inject_del_vnic(U32 switchNo, U32 busNo, U32 numIntPorts,
 				   U32 numExtPorts);
-int uislib_client_inject_add_bus(U32 busNo, GUID instGuid,
+int uislib_client_inject_add_bus(U32 busNo, uuid_le instGuid,
 				 U64 channelAddr, ulong nChannelBytes);
 int  uislib_client_inject_del_bus(U32 busNo);
 
 int uislib_client_inject_add_vhba(U32 busNo, U32 devNo,
 				  U64 phys_chan_addr, U32 chan_bytes,
-				  int is_test_addr, GUID instGuid,
+				  int is_test_addr, uuid_le instGuid,
 				  struct InterruptInfo *intr);
 int  uislib_client_inject_pause_vhba(U32 busNo, U32 devNo);
 int  uislib_client_inject_resume_vhba(U32 busNo, U32 devNo);
 int uislib_client_inject_del_vhba(U32 busNo, U32 devNo);
 int uislib_client_inject_add_vnic(U32 busNo, U32 devNo,
 				  U64 phys_chan_addr, U32 chan_bytes,
-				  int is_test_addr, GUID instGuid,
+				  int is_test_addr, uuid_le instGuid,
 				  struct InterruptInfo *intr);
 int uislib_client_inject_pause_vnic(U32 busNo, U32 devNo);
 int uislib_client_inject_resume_vnic(U32 busNo, U32 devNo);
@@ -193,14 +194,21 @@ struct chaninfo {
  * correctly at DEVICE_CREATE time, INSTEAD OF waiting until
  * DEVICE_CONFIGURE time.
  */
-#define WAIT_FOR_VALID_GUID(guid) \
-	do {						   \
-		while (MEMCMP_IO(&guid, &Guid0, sizeof(Guid0)) == 0) {	\
-			LOGERR("Waiting for non-0 GUID (why???)...\n"); \
-			UIS_THREAD_WAIT_SEC(5);				\
-		}							\
-		LOGERR("OK... GUID is non-0 now\n");			\
-	} while (0)
+static inline void
+wait_for_valid_guid(uuid_le __iomem *guid)
+{
+	uuid_le tmpguid;
+
+	while (1) {
+		memcpy_fromio((void *)&tmpguid,
+			      (void __iomem *)guid, sizeof(uuid_le));
+		if (uuid_le_cmp(tmpguid, NULL_UUID_LE) != 0)
+			break;
+		LOGERR("Waiting for non-0 GUID (why???)...\n");
+		UIS_THREAD_WAIT_SEC(5);
+	}
+	LOGERR("OK... GUID is non-0 now\n");
+}
 
 /* CopyFragsInfoFromSkb returns the number of entries added to frags array
  * Returns -1 on failure.
diff --git a/drivers/staging/unisys/include/uniklog.h b/drivers/staging/unisys/include/uniklog.h
index 4d7b87cefa6..6178cc46f7c 100644
--- a/drivers/staging/unisys/include/uniklog.h
+++ b/drivers/staging/unisys/include/uniklog.h
@@ -1,6 +1,6 @@
 /* uniklog.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/uislib/uislib.c b/drivers/staging/unisys/uislib/uislib.c
index 8ea9c46e56a..d4a7ef821ba 100644
--- a/drivers/staging/unisys/uislib/uislib.c
+++ b/drivers/staging/unisys/uislib/uislib.c
@@ -1,6 +1,6 @@
 /* uislib.c
  *
- * Copyright � 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -23,6 +23,7 @@
 #include <config/modversions.h>
 #endif
 #include <linux/module.h>
+#include <linux/debugfs.h>
 
 #include "commontypes.h"
 
@@ -45,7 +46,6 @@
 
 #define SET_PROC_OWNER(x, y)
 
-#define UISLIB_TEST_PROC
 #define POLLJIFFIES_NORMAL 1
 /* Choose whether or not you want to wakeup the request-polling thread
  * after an IO termination:
@@ -89,146 +89,34 @@ static DEFINE_SEMAPHORE(Lock_Polling_Device_Channels);	/* unlocked */
 static DECLARE_WAIT_QUEUE_HEAD(Wakeup_Polling_Device_Channels);
 static int Go_Polling_Device_Channels;
 
-static struct proc_dir_entry *uislib_proc_dir;
-static struct proc_dir_entry *uislib_proc_vbus_dir;
-static struct proc_dir_entry *vnic_proc_entry;	/* Used to be "datachan" */
-static struct proc_dir_entry *ctrlchan_proc_entry;
-static struct proc_dir_entry *pmem_proc_entry;
-static struct proc_dir_entry *info_proc_entry;
-static struct proc_dir_entry *switch_proc_entry;
-static struct proc_dir_entry *extport_proc_entry;
-static struct proc_dir_entry *platformnumber_proc_entry;
-static struct proc_dir_entry *bus_proc_entry;
-static struct proc_dir_entry *dev_proc_entry;
-static struct proc_dir_entry *chipset_proc_entry;
-static struct proc_dir_entry *cycles_before_wait_proc_entry;
-static struct proc_dir_entry *reset_counts_proc_entry;
-static struct proc_dir_entry *smart_wakeup_proc_entry;
-static struct proc_dir_entry *disable_proc_entry;
-
-#define DIR_PROC_ENTRY "uislib"
-#define DIR_VBUS_PROC_ENTRY "vbus"
-#define VNIC_PROC_ENTRY_FN "vnic"	/* Used to be "datachan" */
-#define CTRLCHAN_PROC_ENTRY_FN "ctrlchan"
-#define PMEM_PROC_ENTRY_FN "phys_to_virt"
-#define INFO_PROC_ENTRY_FN "info"
-#define SWITCH_PROC_ENTRY_FN "switch"
-#define SWITCH_COUNT_PROC_ENTRY_FN "switch_count"
-#define EXTPORT_PROC_ENTRY_FN "extport"
-#define PLATFORMNUMBER_PROC_ENTRY_FN "platform"
-#define BUS_PROC_ENTRY_FN "bus"
-#define DEV_PROC_ENTRY_FN "device"
-#define CHIPSET_PROC_ENTRY_FN "chipset"
-#define CYCLES_BEFORE_WAIT_PROC_ENTRY_FN "cycles_before_wait"
-#define RESET_COUNTS_PROC_ENTRY_FN "reset_counts"
-#define SMART_WAKEUP_PROC_ENTRY_FN "smart_wakeup"
 #define CALLHOME_PROC_ENTRY_FN "callhome"
 #define CALLHOME_THROTTLED_PROC_ENTRY_FN "callhome_throttled"
-#define DISABLE_PROC_ENTRY_FN "switch_state"
-#ifdef UISLIB_TEST_PROC
-static struct proc_dir_entry *test_proc_entry;
-#define TEST_PROC_ENTRY_FN "test"
-#endif
-static unsigned long long cycles_before_wait, wait_cycles;
 
-/*****************************************************/
-/* local functions                                   */
-/*****************************************************/
+#define DIR_DEBUGFS_ENTRY "uislib"
+static struct dentry *dir_debugfs;
 
-static int proc_info_vbus_show(struct seq_file *m, void *v);
-static int
-proc_info_vbus_open(struct inode *inode, struct file *filp)
-{
-	/* proc_info_vbus_show will grab this from seq_file.private: */
-	struct bus_info *bus = PDE_DATA(inode);
-	return single_open(filp, proc_info_vbus_show, bus);
-}
+#define PLATFORMNUMBER_DEBUGFS_ENTRY_FN "platform"
+static struct dentry *platformnumber_debugfs_read;
 
-static const struct file_operations proc_info_vbus_fops = {
-	.open = proc_info_vbus_open,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
-};
-
-static ssize_t uislib_proc_read_writeonly(struct file *file,
-					  char __user *buffer,
-					  size_t count, loff_t *ppos);
+#define CYCLES_BEFORE_WAIT_DEBUGFS_ENTRY_FN "cycles_before_wait"
+static struct dentry *cycles_before_wait_debugfs_read;
 
-static ssize_t vnic_proc_write(struct file *file, const char __user *buffer,
-			       size_t count, loff_t *ppos);
+#define SMART_WAKEUP_DEBUGFS_ENTRY_FN "smart_wakeup"
+static struct dentry *smart_wakeup_debugfs_entry;
 
-static const struct file_operations proc_vnic_fops = {
-	.read = uislib_proc_read_writeonly,
-	.write = vnic_proc_write,
-};
+#define INFO_DEBUGFS_ENTRY_FN "info"
+static struct dentry *info_debugfs_entry;
 
-static ssize_t chipset_proc_write(struct file *file, const char __user *buffer,
-				  size_t count, loff_t *ppos);
+static unsigned long long cycles_before_wait, wait_cycles;
 
-static const struct file_operations proc_chipset_fops = {
-	.read = uislib_proc_read_writeonly,
-	.write = chipset_proc_write,
-};
+/*****************************************************/
+/* local functions                                   */
+/*****************************************************/
 
-static ssize_t info_proc_read(struct file *file, char __user *buf,
+static ssize_t info_debugfs_read(struct file *file, char __user *buf,
 			      size_t len, loff_t *offset);
-static const struct file_operations proc_info_fops = {
-	.read = info_proc_read,
-};
-
-static ssize_t platformnumber_proc_read(struct file *file, char __user *buf,
-					size_t len, loff_t *offset);
-static const struct file_operations proc_platformnumber_fops = {
-	.read = platformnumber_proc_read,
-};
-
-static ssize_t cycles_before_wait_proc_write(struct file *file,
-					     const char __user *buffer,
-					     size_t count, loff_t *ppos);
-static const struct file_operations proc_cycles_before_wait_fops = {
-	.read = uislib_proc_read_writeonly,
-	.write = cycles_before_wait_proc_write,
-};
-
-static ssize_t reset_counts_proc_write(struct file *file,
-				       const char __user *buffer,
-				       size_t count, loff_t *ppos);
-static const struct file_operations proc_reset_counts_fops = {
-	.read = uislib_proc_read_writeonly,
-	.write = reset_counts_proc_write,
-};
-
-static ssize_t smart_wakeup_proc_write(struct file *file,
-				       const char __user *buffer,
-				       size_t count, loff_t *ppos);
-static const struct file_operations proc_smart_wakeup_fops = {
-	.read = uislib_proc_read_writeonly,
-	.write = smart_wakeup_proc_write,
-};
-
-static ssize_t test_proc_write(struct file *file,
-			       const char __user *buffer,
-			       size_t count, loff_t *ppos);
-static const struct file_operations proc_test_fops = {
-	.read = uislib_proc_read_writeonly,
-	.write = test_proc_write,
-};
-
-static ssize_t bus_proc_write(struct file *file,
-			      const char __user *buffer,
-			      size_t count, loff_t *ppos);
-static const struct file_operations proc_bus_fops = {
-	.read = uislib_proc_read_writeonly,
-	.write = bus_proc_write,
-};
-
-static ssize_t dev_proc_write(struct file *file,
-			      const char __user *buffer,
-			      size_t count, loff_t *ppos);
-static const struct file_operations proc_dev_fops = {
-	.read = uislib_proc_read_writeonly,
-	.write = dev_proc_write,
+static const struct file_operations debugfs_info_fops = {
+	.read = info_debugfs_read,
 };
 
 static void
@@ -240,31 +128,10 @@ init_msg_header(CONTROLVM_MESSAGE *msg, U32 id, uint rsp, uint svr)
 	msg->hdr.Flags.server = svr;
 }
 
-static void
-create_bus_proc_entries(struct bus_info *bus)
-{
-	bus->proc_dir = proc_mkdir(bus->name, uislib_proc_vbus_dir);
-	if (!bus->proc_dir) {
-		LOGERR("failed to create /proc/uislib/vbus/%s directory",
-		       bus->name);
-		return;
-	}
-	bus->proc_info = proc_create_data("info", 0, bus->proc_dir,
-					  &proc_info_vbus_fops, bus);
-	if (!bus->proc_info) {
-		LOGERR("failed to create /proc/uislib/vbus/%s/info", bus->name);
-		remove_proc_entry(bus->name, uislib_proc_vbus_dir);
-		bus->proc_dir = NULL;
-		return;
-	}
-	SET_PROC_OWNER(bus->proc_info, THIS_MODULE);
-
-}
-
 static __iomem void *
 init_vbus_channel(U64 channelAddr, U32 channelBytes, int isServer)
 {
-	void *rc = NULL;
+	void __iomem *rc = NULL;
 	void __iomem *pChan = uislib_ioremap_cache(channelAddr, channelBytes);
 	if (!pChan) {
 		LOGERR("CONTROLVM_BUS_CREATE error: ioremap_cache of channelAddr:%Lx for channelBytes:%llu failed",
@@ -381,22 +248,21 @@ create_bus(CONTROLVM_MESSAGE *msg, char *buf)
 		cmd.add_vbus.busTypeGuid = msg->cmd.createBus.busDataTypeGuid;
 		cmd.add_vbus.busInstGuid = msg->cmd.createBus.busInstGuid;
 		if (!VirtControlChanFunc) {
-			kfree(bus);
 			LOGERR("CONTROLVM_BUS_CREATE Failed: virtpci callback not registered.");
 			POSTCODE_LINUX_3(BUS_CREATE_FAILURE_PC, bus->busNo,
 					 POSTCODE_SEVERITY_ERR);
+			kfree(bus);
 			return CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_FAILURE;
 		}
 		if (!VirtControlChanFunc(&cmd)) {
-			kfree(bus);
 			LOGERR("CONTROLVM_BUS_CREATE Failed: virtpci GUEST_ADD_VBUS returned error.");
 			POSTCODE_LINUX_3(BUS_CREATE_FAILURE_PC, bus->busNo,
 					 POSTCODE_SEVERITY_ERR);
+			kfree(bus);
 			return
 			    CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_CALLBACK_ERROR;
 		}
 	}
-	create_bus_proc_entries(bus);
 
 	/* add bus at the head of our list */
 	write_lock(&BusListLock);
@@ -473,14 +339,6 @@ destroy_bus(CONTROLVM_MESSAGE *msg, char *buf)
 		read_unlock(&BusListLock);
 		return CONTROLVM_RESP_ERROR_ALREADY_DONE;
 	}
-	if (bus->proc_info) {
-		remove_proc_entry("info", bus->proc_dir);
-		bus->proc_info = NULL;
-	}
-	if (bus->proc_dir) {
-		remove_proc_entry(bus->name, uislib_proc_vbus_dir);
-		bus->proc_dir = NULL;
-	}
 	if (bus->pBusChannel) {
 		uislib_iounmap(bus->pBusChannel);
 		bus->pBusChannel = NULL;
@@ -587,11 +445,9 @@ create_device(CONTROLVM_MESSAGE *msg, char *buf)
 			 */
 			if (!msg->hdr.Flags.server) {
 				struct guest_msgs cmd;
-				if (!memcmp
-				    (&dev->channelTypeGuid,
-				     &UltraVhbaChannelProtocolGuid,
-				     sizeof(GUID))) {
-					WAIT_FOR_VALID_GUID(((CHANNEL_HEADER
+				if (!uuid_le_cmp(dev->channelTypeGuid,
+				     UltraVhbaChannelProtocolGuid)) {
+					wait_for_valid_guid(&((CHANNEL_HEADER
 							      __iomem *) (dev->
 								  chanptr))->
 							    Type);
@@ -614,11 +470,9 @@ create_device(CONTROLVM_MESSAGE *msg, char *buf)
 					    dev->devInstGuid;
 					cmd.add_vhba.intr = dev->intr;
 				} else
-				    if (!memcmp
-					(&dev->channelTypeGuid,
-					 &UltraVnicChannelProtocolGuid,
-					 sizeof(GUID))) {
-					WAIT_FOR_VALID_GUID(((CHANNEL_HEADER
+				    if (!uuid_le_cmp(dev->channelTypeGuid,
+					 UltraVnicChannelProtocolGuid)) {
+					wait_for_valid_guid(&((CHANNEL_HEADER
 							      __iomem *) (dev->
 								  chanptr))->
 							    Type);
@@ -723,16 +577,13 @@ pause_device(CONTROLVM_MESSAGE *msg)
 			/* the msg is bound for virtpci; send
 			 * guest_msgs struct to callback
 			 */
-			if (!memcmp
-			    (&dev->channelTypeGuid,
-			     &UltraVhbaChannelProtocolGuid, sizeof(GUID))) {
+			if (!uuid_le_cmp(dev->channelTypeGuid,
+					UltraVhbaChannelProtocolGuid)) {
 				cmd.msgtype = GUEST_PAUSE_VHBA;
 				cmd.pause_vhba.chanptr = dev->chanptr;
 			} else
-			    if (!memcmp
-				(&dev->channelTypeGuid,
-				 &UltraVnicChannelProtocolGuid,
-				 sizeof(GUID))) {
+			    if (!uuid_le_cmp(dev->channelTypeGuid,
+					    UltraVnicChannelProtocolGuid)) {
 				cmd.msgtype = GUEST_PAUSE_VNIC;
 				cmd.pause_vnic.chanptr = dev->chanptr;
 			} else {
@@ -798,15 +649,13 @@ resume_device(CONTROLVM_MESSAGE *msg)
 			/* the msg is bound for virtpci; send
 			 * guest_msgs struct to callback
 			 */
-			if (!memcmp(&dev->channelTypeGuid,
-				    &UltraVhbaChannelProtocolGuid,
-				    sizeof(GUID))) {
+			if (!uuid_le_cmp(dev->channelTypeGuid,
+					UltraVhbaChannelProtocolGuid)) {
 				cmd.msgtype = GUEST_RESUME_VHBA;
 				cmd.resume_vhba.chanptr = dev->chanptr;
 			} else
-			    if (!memcmp(&dev->channelTypeGuid,
-					&UltraVnicChannelProtocolGuid,
-					sizeof(GUID))) {
+			    if (!uuid_le_cmp(dev->channelTypeGuid,
+					    UltraVnicChannelProtocolGuid)) {
 				cmd.msgtype = GUEST_RESUME_VNIC;
 				cmd.resume_vnic.chanptr = dev->chanptr;
 			} else {
@@ -873,16 +722,13 @@ destroy_device(CONTROLVM_MESSAGE *msg, char *buf)
 			/* the msg is bound for virtpci; send
 			 * guest_msgs struct to callback
 			 */
-			if (!memcmp
-			    (&dev->channelTypeGuid,
-			     &UltraVhbaChannelProtocolGuid, sizeof(GUID))) {
+			if (!uuid_le_cmp(dev->channelTypeGuid,
+					UltraVhbaChannelProtocolGuid)) {
 				cmd.msgtype = GUEST_DEL_VHBA;
 				cmd.del_vhba.chanptr = dev->chanptr;
 			} else
-			    if (!memcmp
-				(&dev->channelTypeGuid,
-				 &UltraVnicChannelProtocolGuid,
-				 sizeof(GUID))) {
+			    if (!uuid_le_cmp(dev->channelTypeGuid,
+					    UltraVnicChannelProtocolGuid)) {
 				cmd.msgtype = GUEST_DEL_VNIC;
 				cmd.del_vnic.chanptr = dev->chanptr;
 			} else {
@@ -957,27 +803,6 @@ init_chipset(CONTROLVM_MESSAGE *msg, char *buf)
 }
 
 static int
-stop_chipset(CONTROLVM_MESSAGE *msg, char *buf)
-{
-	/* Check that all buses and switches have been torn down and
-	 * destroyed.
-	 */
-	if (BusListHead) {
-		/* Buses still exist. */
-		LOGERR("CONTROLVM_CHIPSET_STOP: BusListHead is not NULL");
-		return CONTROLVM_RESP_ERROR_CHIPSET_STOP_FAILED_BUS;
-	}
-	if (BusListCount) {
-		/* BusListHead is NULL, but BusListCount != 0 */
-		LOGERR("CONTROLVM_CHIPSET_STOP: BusListCount != 0");
-		return CONTROLVM_RESP_ERROR_CHIPSET_STOP_FAILED_BUS;
-	}
-
-	/* Buses are shut down. */
-	return visorchipset_chipset_notready();
-}
-
-static int
 delete_bus_glue(U32 busNo)
 {
 	CONTROLVM_MESSAGE msg;
@@ -1008,7 +833,7 @@ delete_device_glue(U32 busNo, U32 devNo)
 }
 
 int
-uislib_client_inject_add_bus(U32 busNo, GUID instGuid,
+uislib_client_inject_add_bus(U32 busNo, uuid_le instGuid,
 			     U64 channelAddr, ulong nChannelBytes)
 {
 	CONTROLVM_MESSAGE msg;
@@ -1109,7 +934,7 @@ EXPORT_SYMBOL_GPL(uislib_client_inject_resume_vhba);
 int
 uislib_client_inject_add_vhba(U32 busNo, U32 devNo,
 			      U64 phys_chan_addr, U32 chan_bytes,
-			      int is_test_addr, GUID instGuid,
+			      int is_test_addr, uuid_le instGuid,
 			      struct InterruptInfo *intr)
 {
 	CONTROLVM_MESSAGE msg;
@@ -1168,7 +993,7 @@ EXPORT_SYMBOL_GPL(uislib_client_inject_del_vhba);
 int
 uislib_client_inject_add_vnic(U32 busNo, U32 devNo,
 			      U64 phys_chan_addr, U32 chan_bytes,
-			      int is_test_addr, GUID instGuid,
+			      int is_test_addr, uuid_le instGuid,
 			      struct InterruptInfo *intr)
 {
 	CONTROLVM_MESSAGE msg;
@@ -1272,7 +1097,6 @@ uislib_client_add_vnic(U32 busNo)
 	BOOL busCreated = FALSE;
 	int devNo = 0;		/* Default to 0, since only one device
 				 * will be created for this bus... */
-	GUID dummyGuid = GUID0;
 	CONTROLVM_MESSAGE msg;
 
 	init_msg_header(&msg, CONTROLVM_BUS_CREATE, 0, 0);
@@ -1291,7 +1115,7 @@ uislib_client_add_vnic(U32 busNo)
 	msg.hdr.Flags.testMessage = 1;
 	msg.cmd.createDevice.busNo = busNo;
 	msg.cmd.createDevice.devNo = devNo;
-	msg.cmd.createDevice.devInstGuid = dummyGuid;
+	msg.cmd.createDevice.devInstGuid = NULL_UUID_LE;
 	memset(&msg.cmd.createDevice.intr, 0, sizeof(struct InterruptInfo));
 	msg.cmd.createDevice.channelAddr = PhysicalDataChan;
 	msg.cmd.createDevice.channelBytes = MIN_IO_CHANNEL_SIZE;
@@ -1376,135 +1200,11 @@ EXPORT_SYMBOL_GPL(uislib_cache_free);
 /* proc filesystem callback functions                */
 /*****************************************************/
 
-static ssize_t
-vnic_proc_write(struct file *file, const char __user *buffer,
-		size_t count, loff_t *ppos)
-{
-	int action = 0xffff, busNo = 0, i, result = 0;
-	char buf[4];
-	char direction;
-/* GUID guid; */
-	if (count >= ARRAY_SIZE(buf))
-		return -EINVAL;
-
-	if (copy_from_user(buf, buffer, count)) {
-		LOGERR("echo > /proc/uislib/vnic copy_from_user ****FAILED.\n");
-		return -EFAULT;
-	}
-
-	i = sscanf(buf, "%d%c", &action, &direction);
-	if (i != 2) {
-		LOGERR("unable to parse vnic proc parameters.\n");
-		return -EFAULT;
-	}
-
-	if ((direction != '-') && (direction != '+')) {
-		LOGERR("unable to determine whether to add or delete vnic\n");
-		return -EFAULT;
-	}
-
-	/* if (i < 1), i.e., if we didn't even read the action field,
-	* then action will default to 0xffff and the code below will
-	* fall through the switch and print usage.
-	*/
-	switch (action) {
-	case 0:
-		/* call client method... */
-		busNo = 0;	/* All client drivers use bus value of 0... */
-		if (direction == '+')
-			result = uislib_client_add_vnic(busNo);
-		else
-			result = uislib_client_delete_vnic(busNo);
-		if (!result) {
-			LOGERR("echo 0%c > /proc/uislib/vnic failed (client end)",
-			     direction);
-			return -EFAULT;
-		}
-		return count;
-
-	default:
-		break;
-	}
-
-	LOGERR("USAGE: echo <action><direction (up/down)> > /proc/uislib/vnic");
-	LOGERR(" ");
-	LOGERR("Client Syntax");
-	LOGERR("-------------");
-	LOGERR("0+    ==> add vnic");
-	LOGERR("0-    ==> delete vnic");
-	LOGERR(" ");
-	return count;
-}				/* end vnic_proc_write */
-
-static ssize_t
-chipset_proc_write(struct file *file, const char __user *buffer,
-		   size_t count, loff_t *ppos)
-{
-	int i, action = 0xffff;
-	char buf[4];
-	CONTROLVM_MESSAGE msg;
-
-	if (count >= ARRAY_SIZE(buf))
-		return -EINVAL;
-
-	memset(&msg, 0, sizeof(CONTROLVM_MESSAGE));
-
-	if (copy_from_user(buf, buffer, count)) {
-		LOGERR("copy_from_user ****FAILED.\n");
-		return -EFAULT;
-	}
-
-	if (chipset_inited) {
-		LOGINF("Chipset already initialized\n");
-		return -EFAULT;
-	}
-	i = sscanf(buf, "%x", &action);
-
-	/* if (i < 1), i.e., if we didn't even read the action field,
-	* then action will default to 0xffff and the code below will
-	* fall through the switch and print usage.
-	*/
-	switch (action) {
-	case 1:
-		/* GUEST */
-		/* step: initialize the chipset */
-		init_msg_header(&msg, CONTROLVM_CHIPSET_INIT, 0, 0);
-		msg.hdr.Flags.testMessage = 0;
-		msg.cmd.initChipset.busCount = 23;
-		msg.cmd.initChipset.switchCount = 23;
-
-		if (init_chipset(&msg, NULL) != CONTROLVM_RESP_SUCCESS) {
-			LOGERR("init_chipset failed.\n");
-			return 0;
-		}
-		return 1;
-	case 2:
-		/* BOTH */
-		init_msg_header(&msg, CONTROLVM_CHIPSET_INIT, 0, 0);
-		msg.hdr.Flags.testMessage = 1;
-		msg.cmd.initChipset.busCount = 23;
-		msg.cmd.initChipset.switchCount = 23;
-
-		if (init_chipset(&msg, NULL) != CONTROLVM_RESP_SUCCESS) {
-			LOGERR("init_chipset failed.\n");
-			return 0;
-		}
-		return 1;
-
-	default:
-		break;
-	}
-
-	LOGERR("usage: 1 ==> init_chipset client\n");
-	LOGERR("usage: 2 ==> init_chipset test\n");
-	return -EFAULT;
-}
-
 #define PLINE(...) uisutil_add_proc_line_ex(&tot, buff, \
 					       buff_len, __VA_ARGS__)
 
 static int
-info_proc_read_helper(char **buff, int *buff_len)
+info_debugfs_read_helper(char **buff, int *buff_len)
 {
 	int i, tot = 0;
 	struct bus_info *bus;
@@ -1564,7 +1264,8 @@ err_done:
 }
 
 static ssize_t
-info_proc_read(struct file *file, char __user *buf, size_t len, loff_t *offset)
+info_debugfs_read(struct file *file, char __user *buf,
+		size_t len, loff_t *offset)
 {
 	char *temp;
 	int totalBytes = 0;
@@ -1584,9 +1285,9 @@ info_proc_read(struct file *file, char __user *buf, size_t len, loff_t *offset)
 	temp = ProcReadBuffer;
 
 	if ((*offset == 0) || (!ProcReadBufferValid)) {
-		DBGINF("calling info_proc_read_helper.\n");
+		DBGINF("calling info_debugfs_read_helper.\n");
 		/* if the read fails, then -1 will be returned */
-		totalBytes = info_proc_read_helper(&temp, &remaining_bytes);
+		totalBytes = info_debugfs_read_helper(&temp, &remaining_bytes);
 		ProcReadBufferValid = 1;
 	} else
 		totalBytes = strlen(ProcReadBuffer);
@@ -1595,432 +1296,6 @@ info_proc_read(struct file *file, char __user *buf, size_t len, loff_t *offset)
 				       ProcReadBuffer, totalBytes);
 }
 
-static ssize_t
-platformnumber_proc_read(struct file *file, char __user *buf,
-			 size_t len, loff_t *offset)
-{
-	int length = 0;
-	char *vbuf;
-	loff_t pos = *offset;
-
-	if (pos < 0)
-		return -EINVAL;
-
-	if (pos > 0 || !len)
-		return 0;
-
-	vbuf = kzalloc(len, GFP_KERNEL);
-	if (!vbuf)
-		return -ENOMEM;
-
-	length = sprintf(vbuf, "%d\n", PlatformNumber);
-
-	if (copy_to_user(buf, vbuf, length)) {
-		kfree(vbuf);
-		return -EFAULT;
-	}
-
-	kfree(vbuf);
-	*offset += length;
-	return length;
-}
-
-#ifdef UISLIB_TEST_PROC
-
-/* proc/uislib/vbus/<x>/info */
-static int
-proc_info_vbus_show(struct seq_file *m, void *v)
-{
-	struct bus_info *bus = m->private;
-	int i, devInfoCount, x;
-	char buf[999];
-
-	if (bus == NULL)
-		return 0;
-	seq_printf(m, "Client device / client driver info for %s partition (vbus #%d):\n",
-		   bus->partitionName, bus->busNo);
-	if ((bus->busChannelBytes == 0) || (bus->pBusChannel == NULL))
-		return 0;
-	devInfoCount =
-	    (bus->busChannelBytes -
-	     sizeof(ULTRA_VBUS_CHANNEL_PROTOCOL)) /
-	    sizeof(ULTRA_VBUS_DEVICEINFO);
-	x = VBUSCHANNEL_devInfoToStringBuffer(&bus->pBusChannel->ChpInfo, buf,
-					      sizeof(buf) - 1, -1);
-	buf[x] = '\0';
-	seq_printf(m, "%s", buf);
-	x = VBUSCHANNEL_devInfoToStringBuffer(&bus->pBusChannel->BusInfo,
-					      buf, sizeof(buf) - 1, -1);
-	buf[x] = '\0';
-	seq_printf(m, "%s", buf);
-	for (i = 0; i < devInfoCount; i++) {
-		x = VBUSCHANNEL_devInfoToStringBuffer(&bus->pBusChannel->
-						      DevInfo[i], buf,
-						      sizeof(buf) - 1, i);
-		if (x > 0) {
-			buf[x] = '\0';
-			seq_printf(m, "%s", buf);
-		}
-	}
-	return 0;
-}
-
-static ssize_t
-bus_proc_write(struct file *file, const char __user *buffer,
-	       size_t count, loff_t *ppos)
-{
-	int server_flag = 0;
-	int i, action = 0xffff, result;
-	char buf[16];
-	CONTROLVM_MESSAGE msg;
-	U32 busNo, deviceCount;
-
-	if (count >= ARRAY_SIZE(buf))
-		return -EINVAL;
-
-	memset(&msg, 0, sizeof(CONTROLVM_MESSAGE));
-
-	if (copy_from_user(buf, buffer, count)) {
-		LOGERR("echo > /proc/uislib/bus: copy_from_user ****FAILED.");
-		return -EFAULT;
-	}
-
-	i = sscanf(buf, "%x-%d-%d", &action, &busNo, &deviceCount);
-
-	/* if (i < 1), i.e., if we didn't even read the action field,
-	* then action will default to 0xffff and the code below will
-	* fall through the switch and print usage.
-	*/
-	switch (action) {
-	case 0:
-		/* destroy a bus */
-		if (i != 2)
-			break;
-		init_msg_header(&msg, CONTROLVM_BUS_DESTROY, 0, server_flag);
-		msg.cmd.destroyBus.busNo = busNo;
-
-		result = destroy_bus(&msg, NULL);
-
-		if (result != CONTROLVM_RESP_SUCCESS) {
-			LOGERR("echo 0-%d > /proc/uislib/bus {CONTROLVM_BUS_DESTROY Failed} Result(%d)",
-			     busNo, result);
-			return -EFAULT;
-		}
-		return count;
-	case 1:
-		/* create a bus */
-		if (i != 3)
-			break;
-		init_msg_header(&msg, CONTROLVM_BUS_CREATE, 0, server_flag);
-		msg.cmd.createBus.busNo = busNo;
-		msg.cmd.createBus.deviceCount = deviceCount;
-
-		result = create_bus(&msg, NULL);
-
-		if (result != CONTROLVM_RESP_SUCCESS) {
-			LOGERR("echo 1-%d-%d > /proc/uislib/bus {CONTROLVM_BUS_CREATE Failed} Result(%d)",
-			     busNo, deviceCount, result);
-			return -EFAULT;
-		}
-
-		return count;
-	default:
-		break;
-	}
-
-	LOGERR("USAGE: echo <action>-<busNo>... > /proc/uislib/bus");
-	LOGERR(" ");
-	LOGERR("Destruct Syntax     ControlVM Message Id");
-	LOGERR("---------------     ---------------------");
-	LOGERR("0-<busNo>       ==> CONTROLVM_BUS_DESTROY");
-	LOGERR(" ");
-	LOGERR("Construct Syntax            ControlVM Message Id");
-	LOGERR("-----------------------     -------------------- ");
-	LOGERR("1-<busNo>-<deviceCount> ==> CONTROLVM_BUS_CREATE");
-
-	return -EFAULT;
-}
-
-static ssize_t
-uislib_proc_read_writeonly(struct file *file, char __user *buffer,
-	       size_t count, loff_t *ppos)
-{
-	return 0;
-}
-
-static ssize_t
-dev_proc_write(struct file *file, const char __user *buffer,
-	       size_t count, loff_t *ppos)
-{
-	int server_flag = 0;
-	CONTROLVM_MESSAGE msg;
-	U32 busNo, devNo;
-	char buf[32];
-	unsigned int chanptr;
-	int type, i, action = 0xffff, result;
-
-	if (count >= ARRAY_SIZE(buf))
-		return -EINVAL;
-
-	if (copy_from_user(buf, buffer, count)) {
-		LOGERR("echo > /proc/uislib/device: copy_from_user ****FAILED.");
-		return -EFAULT;
-	}
-
-	i = sscanf(buf, "%x-%d-%d-%x-%d",
-		   &action, &busNo, &devNo, &chanptr, &type);
-
-	switch (action) {
-	case 0:
-		if (i != 3)
-			break;
-
-		/* destroy a device */
-		init_msg_header(&msg, CONTROLVM_DEVICE_DESTROY, 0, server_flag);
-		msg.cmd.destroyDevice.busNo = busNo;
-		msg.cmd.destroyDevice.devNo = devNo;
-
-		result = destroy_device(&msg, NULL);
-
-		if (result != CONTROLVM_RESP_SUCCESS) {
-			LOGERR("echo 0-%d-%d > /proc/uislib/device {CONTROLVM_DEVICE_DESTROY Failed} Result(%d)",
-			     busNo, devNo, result);
-			return -EFAULT;
-		}
-
-		return count;
-
-	case 1:
-		if (i != 5)
-			break;
-
-		/* create a device */
-		init_msg_header(&msg, CONTROLVM_DEVICE_CREATE, 0, server_flag);
-		msg.cmd.createDevice.busNo = busNo;
-		msg.cmd.createDevice.devNo = devNo;
-		msg.cmd.createDevice.channelAddr = __pa(chanptr);
-		msg.cmd.createDevice.channelBytes = MIN_IO_CHANNEL_SIZE;
-
-		if (type == 0)
-			msg.cmd.createDevice.dataTypeGuid =
-			    UltraVhbaChannelProtocolGuid;
-		else if (type == 1)
-			msg.cmd.createDevice.dataTypeGuid =
-			    UltraVnicChannelProtocolGuid;
-		else {
-			LOGERR("echo 1-%d-%d-%x-<type> > /proc/uislib/devce failed: invalid device type %d.",
-			     busNo, devNo, chanptr, type);
-			return -EFAULT;
-		}
-
-		result = create_device(&msg, NULL);
-
-		if (result != CONTROLVM_RESP_SUCCESS) {
-			if (type == 0)
-				LOGERR("echo 1-%d-%d-%x-0 > /proc/uislib/device {CONTROLVM_DEVICE_CREATE[vHBA] Failed} Result(%d)",
-				     busNo, devNo, chanptr, result);
-			else
-				LOGERR("echo 1-%d-%d-%x-1 > /proc/uislib/device {CONTROLVM_DEVICE_CREATE[vNIC] Failed} Result(%d)",
-				     busNo, devNo, chanptr, result);
-			return -EFAULT;
-		}
-
-	default:
-		break;
-	}
-
-	LOGERR("USAGE: echo <action>-<busNo>-<devNo>... > /proc/uislib/device");
-	LOGERR(" ");
-	LOGERR("Destruct Syntax       ControlVM Message Id");
-	LOGERR("-----------------     ------------------------");
-	LOGERR("0-<busNo>-<devNo> ==> CONTROLVM_DEVICE_DESTROY");
-	LOGERR(" ");
-	LOGERR("Construct Syntax                       ControlVM Message Id");
-	LOGERR
-	    ("----------------------------------     ----------------------- ");
-	LOGERR
-	    ("1-<busNo>-<devNo>-<chanptr>-<type> ==> CONTROLVM_DEVICE_CREATE");
-	LOGERR("      <type = 0>: vHBA");
-	LOGERR("      <type = 1>: vNIC");
-	LOGERR(" ");
-
-	return -EFAULT;
-}
-
-static ssize_t
-cycles_before_wait_proc_write(struct file *file, const char __user *buffer,
-			      size_t count, loff_t *ppos)
-{
-	char buf[16];
-
-#define CYCLES_BEFORE_WAIT_USE_ERROR  { \
-	LOGERR("Incorrect Call Home Input.\n"); \
-	pr_info("Please pass Call Home Event Parameters in the form:\n"); \
-	pr_info("EventID Category Type[parameter1][parameter2][parameter3][parameter4][parameter5][parameter6]\n"); \
-	return -EFAULT; \
-}
-	if (count >= ARRAY_SIZE(buf))
-		return -EINVAL;
-
-	if (count == 0)
-		CYCLES_BEFORE_WAIT_USE_ERROR;
-
-	if (copy_from_user(buf, buffer, count)) {
-		LOGERR("copy_from_user failed.\n");
-		return -EFAULT;
-	}
-	buf[count - 1] = '\0';	/* Replace the LF at the end of the
-				 * input with a NULL */
-	/* Pull out the cycles_before_wait must be decimal integer */
-	if (sscanf(buf, "%lld", &cycles_before_wait) != 1)
-		CYCLES_BEFORE_WAIT_USE_ERROR;
-
-	return count;
-}
-
-static ssize_t
-reset_counts_proc_write(struct file *file, const char __user *buffer,
-			size_t count, loff_t *ppos)
-{
-	char buf[16];
-	unsigned long long new_value;
-	struct bus_info *bus;
-	int i;
-
-#define RESET_COUNTS_USE_ERROR  { \
-	LOGERR("Incorrect reset_counts Input.\n"); \
-	pr_info("Please pass the new value for the counters:\n"); \
-	pr_info("e.g. echo 0 > reset_counts\n"); \
-	return -EFAULT; \
-	}
-
-	if (count >= ARRAY_SIZE(buf))
-		return -EINVAL;
-
-	if (count == 0)
-		RESET_COUNTS_USE_ERROR;
-
-	if (copy_from_user(buf, buffer, count)) {
-		LOGERR("copy_from_user failed.\n");
-		return -EFAULT;
-	}
-	buf[count - 1] = '\0';	/* Replace the LF at the end of the
-				 * input with a NULL */
-	/* Pull out the reset_counts must be decimal integer */
-	if (sscanf(buf, "%llu", &new_value) != 1)
-		RESET_COUNTS_USE_ERROR;
-	read_lock(&BusListLock);
-	for (bus = BusListHead; bus; bus = bus->next) {
-
-		for (i = 0; i < bus->deviceCount; i++) {
-			if (bus->device[i]) {
-				bus->device[i]->first_busy_cnt = new_value;
-				bus->device[i]->moved_to_tail_cnt = new_value;
-				bus->device[i]->last_on_list_cnt = new_value;
-			}
-		}
-	}
-	read_unlock(&BusListLock);
-	tot_moved_to_tail_cnt = new_value;
-	tot_wait_cnt = new_value;
-	tot_wakeup_cnt = new_value;
-	tot_schedule_cnt = new_value;
-	return count;
-}
-
-static ssize_t
-smart_wakeup_proc_write(struct file *file, const char __user *buffer,
-			size_t count, loff_t *ppos)
-{
-	char buf[16];
-	int new_value;
-
-#define SMART_WAKEUP_USE_ERROR  { \
-	LOGERR("Incorrect smart_wakeup Input 0 disables smart_wakeup, and 1 enables smart_wakeup.\n"); \
-	pr_info("echo 0 > smart_wakeup\n"); \
-	pr_info("echo 1 > smart_wakeup\n"); \
-	return -EFAULT; \
-	}
-
-	if (count >= ARRAY_SIZE(buf))
-		return -EINVAL;
-
-	if (count == 0)
-		SMART_WAKEUP_USE_ERROR;
-
-	if (copy_from_user(buf, buffer, count)) {
-		LOGERR("copy_from_user failed.\n");
-		return -EFAULT;
-	}
-	buf[count - 1] = '\0';	/* Replace the LF at the end of the
-				 * input with a NULL */
-	/* Pull out the smart_wakeup must be decimal integer */
-	if (sscanf(buf, "%d", &new_value) != 1)
-		SMART_WAKEUP_USE_ERROR;
-	en_smart_wakeup = new_value;
-	return count;
-}
-
-static ssize_t
-test_proc_write(struct file *file, const char __user *buffer,
-		size_t count, loff_t *ppos)
-{
-	int i, action = 0xffff;
-	char buf[16];
-	CONTROLVM_MESSAGE msg;
-	S64 vrtc_offset;
-
-	if (count >= ARRAY_SIZE(buf))
-		return -EINVAL;
-
-	memset(&msg, 0, sizeof(CONTROLVM_MESSAGE));
-
-	if (copy_from_user(buf, buffer, count)) {
-		LOGERR("copy_from_user ****FAILED.\n");
-		return -EFAULT;
-	}
-
-	i = sscanf(buf, "%x", &action);
-
-	/* if (i < 1), i.e., if we didn't even read the action field,
-	* then action will default to 0xffff and the code below will
-	* fall through the switch and print usage. */
-	switch (action) {
-	case 6:
-		msg.hdr.Id = CONTROLVM_CHIPSET_STOP;
-		msg.hdr.Flags.responseExpected = 1;
-		stop_chipset(&msg, NULL);
-		break;
-	case 7:
-		vrtc_offset = 0;
-		LOGERR("about to issue QUERY vrtc_offset=%LX", vrtc_offset);
-		vrtc_offset = Issue_VMCALL_QUERY_GUEST_VIRTUAL_TIME_OFFSET();
-		LOGERR("result is vrtc_offset=%LX", vrtc_offset);
-		break;
-	case 8:
-		vrtc_offset = 60;
-		LOGERR("about to increase physical time by 0x%LX seconds",
-		       vrtc_offset);
-		vrtc_offset = Issue_VMCALL_UPDATE_PHYSICAL_TIME(vrtc_offset);
-		break;
-	case 9:
-		vrtc_offset = -60;
-		LOGERR("about to decrease physical time by 0x%LX seconds",
-		       vrtc_offset);
-		vrtc_offset = Issue_VMCALL_UPDATE_PHYSICAL_TIME(vrtc_offset);
-		break;
-	default:
-		LOGERR("usage: 6 for CHIPSET_STOP\n");
-		LOGERR("       7 for VMCALL_QUERY_GUEST_VIRTUAL_TIME_OFFSET()\n");
-		LOGERR("       8 for VMCALL_UPDATE_PHYSICAL_TIME(60)\n");
-		LOGERR("       9 for VMCALL_UPDATE_PHYSICAL_TIME(-60)\n");
-		return -EFAULT;
-		break;
-	}
-	return count;
-}
-
-#endif				/* UISLIB_TEST_PROC */
 static struct device_info *
 find_dev(U32 busNo, U32 devNo)
 {
@@ -2277,6 +1552,9 @@ static int __init
 uislib_mod_init(void)
 {
 
+	if (!unisys_spar_platform)
+		return -ENODEV;
+
 	LOGINF("MONITORAPIS");
 
 	LOGINF("sizeof(struct uiscmdrsp):%lu bytes\n",
@@ -2308,59 +1586,25 @@ uislib_mod_init(void)
 	 * then map this physical address to a virtual address. */
 	POSTCODE_LINUX_2(DRIVER_ENTRY_PC, POSTCODE_SEVERITY_INFO);
 
-	/* create the proc entries for the channels */
-	uislib_proc_dir = proc_mkdir(DIR_PROC_ENTRY, NULL);
-	/* (e.g., for /proc/uislib/vbus/<x>/info) */
-	uislib_proc_vbus_dir = proc_mkdir(DIR_VBUS_PROC_ENTRY, uislib_proc_dir);
-
-	vnic_proc_entry = proc_create(VNIC_PROC_ENTRY_FN, 0, uislib_proc_dir,
-				      &proc_vnic_fops);
-	SET_PROC_OWNER(vnic_proc_entry, THIS_MODULE);
-
-	/* for testing purposes only, create the proc entries for
-	 * enqueuing Control Channel messages */
-	chipset_proc_entry =
-	    proc_create(CHIPSET_PROC_ENTRY_FN, 0, uislib_proc_dir,
-			&proc_chipset_fops);
-	SET_PROC_OWNER(chipset_proc_entry, THIS_MODULE);
-
-	info_proc_entry = proc_create(INFO_PROC_ENTRY_FN, 0, uislib_proc_dir,
-				      &proc_info_fops);
-	SET_PROC_OWNER(info_proc_entry, THIS_MODULE);
-
-	platformnumber_proc_entry =
-	    proc_create(PLATFORMNUMBER_PROC_ENTRY_FN, 0, uislib_proc_dir,
-			&proc_platformnumber_fops);
-	SET_PROC_OWNER(platformnumberinfo_proc_entry, THIS_MODULE);
-
-	cycles_before_wait_proc_entry =
-	    proc_create(CYCLES_BEFORE_WAIT_PROC_ENTRY_FN, 0, uislib_proc_dir,
-			&proc_cycles_before_wait_fops);
-	SET_PROC_OWNER(cycles_before_wait_proc_entry, THIS_MODULE);
-
-	reset_counts_proc_entry =
-	    proc_create(RESET_COUNTS_PROC_ENTRY_FN, 0, uislib_proc_dir,
-			&proc_reset_counts_fops);
-	SET_PROC_OWNER(reset_counts_proc_entry, THIS_MODULE);
-
-	smart_wakeup_proc_entry =
-	    proc_create(SMART_WAKEUP_PROC_ENTRY_FN, 0, uislib_proc_dir,
-			&proc_smart_wakeup_fops);
-	SET_PROC_OWNER(smart_wakeup_proc_entry, THIS_MODULE);
-
-#ifdef UISLIB_TEST_PROC
-	test_proc_entry = proc_create(TEST_PROC_ENTRY_FN, 0, uislib_proc_dir,
-				      &proc_test_fops);
-	SET_PROC_OWNER(test_proc_entry, THIS_MODULE);
-
-	bus_proc_entry = proc_create(BUS_PROC_ENTRY_FN, 0, uislib_proc_dir,
-				     &proc_bus_fops);
-	SET_PROC_OWNER(bus_proc_entry, THIS_MODULE);
-
-	dev_proc_entry = proc_create(DEV_PROC_ENTRY_FN, 0, uislib_proc_dir,
-				     &proc_dev_fops);
-	SET_PROC_OWNER(dev_proc_entry, THIS_MODULE);
-#endif				/* UISLIB_TEST_PROC */
+	dir_debugfs = debugfs_create_dir(DIR_DEBUGFS_ENTRY, NULL);
+	if (dir_debugfs) {
+		info_debugfs_entry = debugfs_create_file(
+			INFO_DEBUGFS_ENTRY_FN, 0444, dir_debugfs, NULL,
+			&debugfs_info_fops);
+
+		platformnumber_debugfs_read = debugfs_create_u32(
+			PLATFORMNUMBER_DEBUGFS_ENTRY_FN, 0444, dir_debugfs,
+			&PlatformNumber);
+
+		cycles_before_wait_debugfs_read = debugfs_create_u64(
+			CYCLES_BEFORE_WAIT_DEBUGFS_ENTRY_FN, 0666, dir_debugfs,
+			&cycles_before_wait);
+
+		smart_wakeup_debugfs_entry = debugfs_create_bool(
+			SMART_WAKEUP_DEBUGFS_ENTRY_FN, 0666, dir_debugfs,
+			&en_smart_wakeup);
+	}
+
 	POSTCODE_LINUX_3(DRIVER_EXIT_PC, 0, POSTCODE_SEVERITY_INFO);
 	return 0;
 }
@@ -2368,46 +1612,17 @@ uislib_mod_init(void)
 static void __exit
 uislib_mod_exit(void)
 {
-	if (disable_proc_entry)
-		remove_proc_entry(DISABLE_PROC_ENTRY_FN, uislib_proc_dir);
-	if (cycles_before_wait_proc_entry)
-		remove_proc_entry(CYCLES_BEFORE_WAIT_PROC_ENTRY_FN,
-				  uislib_proc_dir);
-	if (reset_counts_proc_entry)
-		remove_proc_entry(RESET_COUNTS_PROC_ENTRY_FN, uislib_proc_dir);
-	if (smart_wakeup_proc_entry)
-		remove_proc_entry(SMART_WAKEUP_PROC_ENTRY_FN, uislib_proc_dir);
-	if (ctrlchan_proc_entry)
-		remove_proc_entry(CTRLCHAN_PROC_ENTRY_FN, uislib_proc_dir);
-	if (pmem_proc_entry)
-		remove_proc_entry(PMEM_PROC_ENTRY_FN, uislib_proc_dir);
-	if (info_proc_entry)
-		remove_proc_entry(INFO_PROC_ENTRY_FN, uislib_proc_dir);
-	if (switch_proc_entry)
-		remove_proc_entry(SWITCH_PROC_ENTRY_FN, uislib_proc_dir);
-	if (extport_proc_entry)
-		remove_proc_entry(EXTPORT_PROC_ENTRY_FN, uislib_proc_dir);
-	if (platformnumber_proc_entry)
-		remove_proc_entry(PLATFORMNUMBER_PROC_ENTRY_FN,
-				  uislib_proc_dir);
-	if (bus_proc_entry)
-		remove_proc_entry(BUS_PROC_ENTRY_FN, uislib_proc_dir);
-	if (dev_proc_entry)
-		remove_proc_entry(DEV_PROC_ENTRY_FN, uislib_proc_dir);
-	if (vnic_proc_entry)
-		remove_proc_entry(VNIC_PROC_ENTRY_FN, uislib_proc_dir);
-	if (chipset_proc_entry)
-		remove_proc_entry(CHIPSET_PROC_ENTRY_FN, uislib_proc_dir);
-	if (uislib_proc_vbus_dir)
-		remove_proc_entry(DIR_VBUS_PROC_ENTRY, uislib_proc_dir);
-	if (uislib_proc_dir)
-		remove_proc_entry(DIR_PROC_ENTRY, NULL);
-
 	if (ProcReadBuffer) {
 		vfree(ProcReadBuffer);
 		ProcReadBuffer = NULL;
 	}
 
+	debugfs_remove(info_debugfs_entry);
+	debugfs_remove(smart_wakeup_debugfs_entry);
+	debugfs_remove(cycles_before_wait_debugfs_read);
+	debugfs_remove(platformnumber_debugfs_read);
+	debugfs_remove(dir_debugfs);
+
 	DBGINF("goodbye.\n");
 	return;
 }
diff --git a/drivers/staging/unisys/uislib/uisqueue.c b/drivers/staging/unisys/uislib/uisqueue.c
index 40598ff1f4f..d4a6074cfaf 100644
--- a/drivers/staging/unisys/uislib/uisqueue.c
+++ b/drivers/staging/unisys/uislib/uisqueue.c
@@ -1,6 +1,6 @@
 /* uisqueue.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/uislib/uisthread.c b/drivers/staging/unisys/uislib/uisthread.c
index 782b06aad56..c93ab04c3cf 100644
--- a/drivers/staging/unisys/uislib/uisthread.c
+++ b/drivers/staging/unisys/uislib/uisthread.c
@@ -1,6 +1,6 @@
 /* uisthread.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/uislib/uisutils.c b/drivers/staging/unisys/uislib/uisutils.c
index 3178f75e1eb..0f1bb739975 100644
--- a/drivers/staging/unisys/uislib/uisutils.c
+++ b/drivers/staging/unisys/uislib/uisutils.c
@@ -1,6 +1,6 @@
 /* uisutils.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -24,8 +24,9 @@
 #include "uisutils.h"
 #include "version.h"
 #include "vbushelper.h"
-#include "guidutils.h"
+#include <linux/uuid.h>
 #include <linux/skbuff.h>
+#include <linux/uuid.h>
 #ifdef CONFIG_HIGHMEM
 #include <linux/highmem.h>
 #endif
@@ -104,7 +105,7 @@ uisctrl_register_req_handler(int type, void *fptr,
 EXPORT_SYMBOL_GPL(uisctrl_register_req_handler);
 
 int
-uisctrl_register_req_handler_ex(GUID switchTypeGuid,
+uisctrl_register_req_handler_ex(uuid_le switchTypeGuid,
 				const char *switch_type_name,
 				int (*controlfunc)(struct io_msgs *),
 				unsigned long min_channel_bytes,
@@ -115,24 +116,22 @@ uisctrl_register_req_handler_ex(GUID switchTypeGuid,
 				  U32 clientStrLen, U64 bytes),
 				ULTRA_VBUS_DEVICEINFO *chipset_DriverInfo)
 {
-	char s[99];
 	ReqHandlerInfo_t *pReqHandlerInfo;
 	int rc = 0;		/* assume failure */
-	LOGINF("type=%s, controlfunc=0x%p.\n",
-	       GUID_format1(&switchTypeGuid, s), controlfunc);
+	LOGINF("type=%pUL, controlfunc=0x%p.\n",
+	       &switchTypeGuid, controlfunc);
 	if (!controlfunc) {
-		LOGERR("%s: controlfunc must be supplied\n",
-		       GUID_format1(&switchTypeGuid, s));
+		LOGERR("%pUL: controlfunc must be supplied\n", &switchTypeGuid);
 		goto Away;
 	}
 	if (!Server_Channel_Ok) {
-		LOGERR("%s: Server_Channel_Ok must be supplied\n",
-		       GUID_format1(&switchTypeGuid, s));
+		LOGERR("%pUL: Server_Channel_Ok must be supplied\n",
+				&switchTypeGuid);
 		goto Away;
 	}
 	if (!Server_Channel_Init) {
-		LOGERR("%s: Server_Channel_Init must be supplied\n",
-		       GUID_format1(&switchTypeGuid, s));
+		LOGERR("%pUL: Server_Channel_Init must be supplied\n",
+				&switchTypeGuid);
 		goto Away;
 	}
 	pReqHandlerInfo = ReqHandlerAdd(switchTypeGuid,
@@ -141,8 +140,7 @@ uisctrl_register_req_handler_ex(GUID switchTypeGuid,
 					min_channel_bytes,
 					Server_Channel_Ok, Server_Channel_Init);
 	if (!pReqHandlerInfo) {
-		LOGERR("failed to add %s to server list\n",
-		       GUID_format1(&switchTypeGuid, s));
+		LOGERR("failed to add %pUL to server list\n", &switchTypeGuid);
 		goto Away;
 	}
 
@@ -156,30 +154,27 @@ Away:
 					   VERSION, NULL,
 					   __DATE__, __TIME__);
 	} else
-		LOGERR("failed to register type %s.\n",
-		       GUID_format1(&switchTypeGuid, s));
+		LOGERR("failed to register type %pUL.\n", &switchTypeGuid);
 
 	return rc;
 }
 EXPORT_SYMBOL_GPL(uisctrl_register_req_handler_ex);
 
 int
-uisctrl_unregister_req_handler_ex(GUID switchTypeGuid)
+uisctrl_unregister_req_handler_ex(uuid_le switchTypeGuid)
 {
-	char s[99];
 	int rc = 0;		/* assume failure */
-	LOGINF("type=%s.\n", GUID_format1(&switchTypeGuid, s));
+	LOGINF("type=%pUL.\n", &switchTypeGuid);
 	if (ReqHandlerDel(switchTypeGuid) < 0) {
-		LOGERR("failed to remove %s from server list\n",
-		       GUID_format1(&switchTypeGuid, s));
+		LOGERR("failed to remove %pUL from server list\n",
+				&switchTypeGuid);
 		goto Away;
 	}
 	atomic_dec(&UisUtils_Registered_Services);
 	rc = 1;			/* success */
 Away:
 	if (!rc)
-		LOGERR("failed to unregister type %s.\n",
-		       GUID_format1(&switchTypeGuid, s));
+		LOGERR("failed to unregister type %pUL.\n", &switchTypeGuid);
 	return rc;
 }
 EXPORT_SYMBOL_GPL(uisctrl_unregister_req_handler_ex);
@@ -281,7 +276,7 @@ static LIST_HEAD(ReqHandlerInfo_list);	/* list of ReqHandlerInfo_t */
 static DEFINE_SPINLOCK(ReqHandlerInfo_list_lock);
 
 ReqHandlerInfo_t *
-ReqHandlerAdd(GUID switchTypeGuid,
+ReqHandlerAdd(uuid_le switchTypeGuid,
 	      const char *switch_type_name,
 	      int (*controlfunc)(struct io_msgs *),
 	      unsigned long min_channel_bytes,
@@ -310,16 +305,14 @@ ReqHandlerAdd(GUID switchTypeGuid,
 }
 
 ReqHandlerInfo_t *
-ReqHandlerFind(GUID switchTypeGuid)
+ReqHandlerFind(uuid_le switchTypeGuid)
 {
 	struct list_head *lelt, *tmp;
 	ReqHandlerInfo_t *entry = NULL;
 	spin_lock(&ReqHandlerInfo_list_lock);
 	list_for_each_safe(lelt, tmp, &ReqHandlerInfo_list) {
 		entry = list_entry(lelt, ReqHandlerInfo_t, list_link);
-		if (memcmp
-		    (&entry->switchTypeGuid, &switchTypeGuid,
-		     sizeof(GUID)) == 0) {
+		if (uuid_le_cmp(entry->switchTypeGuid, switchTypeGuid) == 0) {
 			spin_unlock(&ReqHandlerInfo_list_lock);
 			return entry;
 		}
@@ -329,7 +322,7 @@ ReqHandlerFind(GUID switchTypeGuid)
 }
 
 int
-ReqHandlerDel(GUID switchTypeGuid)
+ReqHandlerDel(uuid_le switchTypeGuid)
 {
 	struct list_head *lelt, *tmp;
 	ReqHandlerInfo_t *entry = NULL;
@@ -337,9 +330,7 @@ ReqHandlerDel(GUID switchTypeGuid)
 	spin_lock(&ReqHandlerInfo_list_lock);
 	list_for_each_safe(lelt, tmp, &ReqHandlerInfo_list) {
 		entry = list_entry(lelt, ReqHandlerInfo_t, list_link);
-		if (memcmp
-		    (&entry->switchTypeGuid, &switchTypeGuid,
-		     sizeof(GUID)) == 0) {
+		if (uuid_le_cmp(entry->switchTypeGuid, switchTypeGuid) == 0) {
 			list_del(lelt);
 			kfree(entry);
 			rc++;
diff --git a/drivers/staging/unisys/virthba/virthba.c b/drivers/staging/unisys/virthba/virthba.c
index 817b11dfa19..5c5aa700176 100644
--- a/drivers/staging/unisys/virthba/virthba.c
+++ b/drivers/staging/unisys/virthba/virthba.c
@@ -1,6 +1,6 @@
 /* virthba.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -82,8 +82,16 @@ static int virthba_ioctl(struct scsi_device *dev, int cmd, void __user *arg);
 static int virthba_queue_command_lck(struct scsi_cmnd *scsicmd,
 				     void (*virthba_cmnd_done)(struct scsi_cmnd *));
 
+static const struct x86_cpu_id unisys_spar_ids[] = {
+	{ X86_VENDOR_INTEL, 6, 62, X86_FEATURE_ANY },
+	{}
+};
+
+/* Autoload */
+MODULE_DEVICE_TABLE(x86cpu, unisys_spar_ids);
+
 #ifdef DEF_SCSI_QCMD
-DEF_SCSI_QCMD(virthba_queue_command)
+static DEF_SCSI_QCMD(virthba_queue_command)
 #else
 #define virthba_queue_command virthba_queue_command_lck
 #endif
@@ -1046,7 +1054,7 @@ static int
 virthba_slave_alloc(struct scsi_device *scsidev)
 {
 	/* this called by the midlayer before scan for new devices -
-	 * LLD can alloc any struc & do init if needed.
+	 * LLD can alloc any struct & do init if needed.
 	 */
 	struct virtdisk_info *vdisk;
 	struct virtdisk_info *tmpvdisk;
@@ -1400,7 +1408,7 @@ info_proc_read(struct file *file, char __user *buf, size_t len, loff_t *offset)
 		virthbainfo = VirtHbasOpen[i].virthbainfo;
 		length += sprintf(vbuf + length, "CHANSOCK is not defined.\n");
 
-		length += sprintf(vbuf + length, "MaxBuffLen:%d\n", MaxBuffLen);
+		length += sprintf(vbuf + length, "MaxBuffLen:%u\n", MaxBuffLen);
 
 		length += sprintf(vbuf + length, "\nvirthba result queue poll wait:%d usecs.\n",
 				  rsltq_wait_usecs);
@@ -1534,7 +1542,7 @@ virthba_serverup(struct virtpci_dev *virtpcidev)
 	       virtpcidev->deviceNo);
 
 	if (!virthbainfo->serverdown) {
-		DBGINF("Server up message recieved while server is already up.\n");
+		DBGINF("Server up message received while server is already up.\n");
 		return 1;
 	}
 	if (virthbainfo->serverchangingstate) {
@@ -1691,6 +1699,9 @@ virthba_mod_init(void)
 	int error;
 	int i;
 
+	if (!unisys_spar_platform)
+		return -ENODEV;
+
 	LOGINF("Entering virthba_mod_init...\n");
 
 	POSTCODE_LINUX_2(VHBA_CREATE_ENTRY_PC, POSTCODE_SEVERITY_INFO);
diff --git a/drivers/staging/unisys/virthba/virthba.h b/drivers/staging/unisys/virthba/virthba.h
index 88b797439a1..d4b809b0c7b 100644
--- a/drivers/staging/unisys/virthba/virthba.h
+++ b/drivers/staging/unisys/virthba/virthba.h
@@ -1,6 +1,6 @@
 /* virthba.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/virtpci/virtpci.c b/drivers/staging/unisys/virtpci/virtpci.c
index 8e34650b00b..71246feb154 100644
--- a/drivers/staging/unisys/virtpci/virtpci.c
+++ b/drivers/staging/unisys/virtpci/virtpci.c
@@ -1,6 +1,6 @@
 /* virtpci.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -39,6 +39,7 @@
 #include <linux/version.h>
 #include "version.h"
 #include "guestlinuxdebug.h"
+#include "timskmodutils.h"
 
 struct driver_private {
 	struct kobject kobj;
@@ -364,7 +365,7 @@ static int add_vhba(struct add_virt_guestpart *addparams)
 		memcpy_fromio(&net.zoneGuid, \
 			      &((ULTRA_IO_CHANNEL_PROTOCOL __iomem *)	\
 				chanptr)->vnic.zoneGuid,		\
-			      sizeof(GUID));				\
+			      sizeof(uuid_le));				\
 }
 
 /* adds a vnic
@@ -390,14 +391,10 @@ add_vnic(struct add_virt_guestpart *addparams)
 
 	GET_BUS_DEV(addparams->busNo);
 
-	LOGINF("Adding vnic macaddr:%02x:%02x:%02x:%02x:%02x:%02x rcvbufs:%d mtu:%d chanptr:%p{%-8.8lx-%-4.4x-%-4.4x-%-2.2x%-2.2x%-2.2x%-2.2x%-2.2x%-2.2x%-2.2x%-2.2x}\n",
+	LOGINF("Adding vnic macaddr:%02x:%02x:%02x:%02x:%02x:%02x rcvbufs:%d mtu:%d chanptr:%p%pUL\n",
 	     net.mac_addr[0], net.mac_addr[1], net.mac_addr[2], net.mac_addr[3],
 	     net.mac_addr[4], net.mac_addr[5], net.num_rcv_bufs, net.mtu,
-	     addparams->chanptr, (ulong) net.zoneGuid.data1, net.zoneGuid.data2,
-	     net.zoneGuid.data3, net.zoneGuid.data4[0], net.zoneGuid.data4[1],
-	     net.zoneGuid.data4[2], net.zoneGuid.data4[3],
-	     net.zoneGuid.data4[4], net.zoneGuid.data4[5],
-	     net.zoneGuid.data4[6], net.zoneGuid.data4[7]);
+	     addparams->chanptr, &net.zoneGuid);
 	i = virtpci_device_add(vbus, VIRTNIC_TYPE, addparams, NULL, &net);
 	if (i) {
 		LOGINF("Added vnic macaddr:%02x:%02x:%02x:%02x:%02x:%02x\n",
@@ -1509,7 +1506,6 @@ static ssize_t virt_proc_write(struct file *file, const char __user *buffer,
 	struct add_virt_guestpart addparams;
 	struct del_vbus_guestpart busdelparams;
 	struct del_virt_guestpart delparams;
-	GUID dummyGuid = GUID0;
 #ifdef STORAGE_CHANNEL
 	U64 storagechannel;
 #endif
@@ -1560,7 +1556,7 @@ static ssize_t virt_proc_write(struct file *file, const char __user *buffer,
 						   __pa(chanptr),
 						   MIN_IO_CHANNEL_SIZE,
 						   1, /* test msg */
-						   dummyGuid, /* inst guid */
+						   NULL_UUID_LE, /* inst guid */
 						   NULL)) { /*interrupt info */
 			LOGERR("FAILED to inject add vnic\n");
 			return -EFAULT;
@@ -1687,6 +1683,9 @@ static int __init virtpci_mod_init(void)
 	int ret;
 
 
+	if (!unisys_spar_platform)
+		return -ENODEV;
+
 	LOGINF("Module build: Date:%s Time:%s...\n", __DATE__, __TIME__);
 
 	POSTCODE_LINUX_2(VPCI_CREATE_ENTRY_PC, POSTCODE_SEVERITY_INFO);
diff --git a/drivers/staging/unisys/virtpci/virtpci.h b/drivers/staging/unisys/virtpci/virtpci.h
index b8fd07bc543..f7be17b669c 100644
--- a/drivers/staging/unisys/virtpci/virtpci.h
+++ b/drivers/staging/unisys/virtpci/virtpci.h
@@ -1,6 +1,6 @@
 /* virtpci.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -24,6 +24,7 @@
 
 #include "uisqueue.h"
 #include <linux/version.h>
+#include <linux/uuid.h>
 
 #define PCI_DEVICE_ID_VIRTHBA 0xAA00
 #define PCI_DEVICE_ID_VIRTNIC 0xAB00
@@ -41,7 +42,7 @@ struct net_adap_info {
 	u8 mac_addr[MAX_MACADDR_LEN];
 	int num_rcv_bufs;
 	unsigned mtu;
-	GUID zoneGuid;
+	uuid_le zoneGuid;
 };
 
 typedef enum {
diff --git a/drivers/staging/unisys/visorchannel/globals.h b/drivers/staging/unisys/visorchannel/globals.h
index 668f832ca56..07653b8dea7 100644
--- a/drivers/staging/unisys/visorchannel/globals.h
+++ b/drivers/staging/unisys/visorchannel/globals.h
@@ -1,6 +1,6 @@
 /* globals.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/visorchannel/visorchannel.h b/drivers/staging/unisys/visorchannel/visorchannel.h
index 62d29a233fd..ecf0d11117e 100644
--- a/drivers/staging/unisys/visorchannel/visorchannel.h
+++ b/drivers/staging/unisys/visorchannel/visorchannel.h
@@ -1,6 +1,6 @@
 /* visorchannel.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -18,6 +18,8 @@
 #ifndef __VISORCHANNEL_H__
 #define __VISORCHANNEL_H__
 
+#include <linux/uuid.h>
+
 #include "commontypes.h"
 #include "memregion.h"
 #include "channel.h"
@@ -38,15 +40,15 @@ typedef struct VISORCHANNEL_Tag VISORCHANNEL;
  * In this case, the values can simply be read from the channel header.
  */
 VISORCHANNEL *visorchannel_create(HOSTADDRESS physaddr,
-				  ulong channelBytes, GUID guid);
+				  ulong channelBytes, uuid_le guid);
 VISORCHANNEL *visorchannel_create_overlapped(ulong channelBytes,
 					     VISORCHANNEL *parent, ulong off,
-					     GUID guid);
+					     uuid_le guid);
 VISORCHANNEL *visorchannel_create_with_lock(HOSTADDRESS physaddr,
-					    ulong channelBytes, GUID guid);
+					    ulong channelBytes, uuid_le guid);
 VISORCHANNEL *visorchannel_create_overlapped_with_lock(ulong channelBytes,
 						       VISORCHANNEL *parent,
-						       ulong off, GUID guid);
+						       ulong off, uuid_le guid);
 void visorchannel_destroy(VISORCHANNEL *channel);
 int visorchannel_read(VISORCHANNEL *channel, ulong offset,
 		      void *local, ulong nbytes);
@@ -64,43 +66,13 @@ ulong visorchannel_get_nbytes(VISORCHANNEL *channel);
 char *visorchannel_id(VISORCHANNEL *channel, char *s);
 char *visorchannel_zoneid(VISORCHANNEL *channel, char *s);
 U64 visorchannel_get_clientpartition(VISORCHANNEL *channel);
-GUID visorchannel_get_GUID(VISORCHANNEL *channel);
+uuid_le visorchannel_get_uuid(VISORCHANNEL *channel);
 MEMREGION *visorchannel_get_memregion(VISORCHANNEL *channel);
-char *visorchannel_GUID_id(GUID *guid, char *s);
+char *visorchannel_uuid_id(uuid_le *guid, char *s);
 void visorchannel_debug(VISORCHANNEL *channel, int nQueues,
 			struct seq_file *seq, U32 off);
 void visorchannel_dump_section(VISORCHANNEL *chan, char *s,
 			       int off, int len, struct seq_file *seq);
-void *visorchannel_get_header(VISORCHANNEL *channel);
-
-#define	VISORCHANNEL_CHANGE_SERVER_STATE(chan, chanId, newstate)	\
-	do {								\
-		U8 *p = (U8 *)visorchannel_get_header(chan);		\
-		if (p) {						\
-			ULTRA_CHANNEL_SERVER_TRANSITION(p, chanId, SrvState, \
-							newstate, logCtx); \
-			visorchannel_write				\
-				(chan,					\
-				 offsetof(ULTRA_CHANNEL_PROTOCOL, SrvState), \
-				 p +					\
-				 offsetof(ULTRA_CHANNEL_PROTOCOL, SrvState), \
-				 sizeof(U32));				\
-		}							\
-	} while (0)
-
-#define	VISORCHANNEL_CHANGE_CLIENT_STATE(chan, chanId, newstate)	\
-	do {								\
-		U8 *p = (U8 *)visorchannel_get_header(chan);		\
-		if (p) {						\
-			ULTRA_CHANNEL_CLIENT_TRANSITION(p, chanId,	\
-							newstate, logCtx); \
-			visorchannel_write				\
-				(chan,					\
-				 offsetof(ULTRA_CHANNEL_PROTOCOL, CliStateOS), \
-				 p +					\
-				 offsetof(ULTRA_CHANNEL_PROTOCOL, CliStateOS), \
-				 sizeof(U32));				\
-		}							\
-	} while (0)
+void __iomem *visorchannel_get_header(VISORCHANNEL *channel);
 
 #endif
diff --git a/drivers/staging/unisys/visorchannel/visorchannel_funcs.c b/drivers/staging/unisys/visorchannel/visorchannel_funcs.c
index 053681616ba..a44da7c84ae 100644
--- a/drivers/staging/unisys/visorchannel/visorchannel_funcs.c
+++ b/drivers/staging/unisys/visorchannel/visorchannel_funcs.c
@@ -1,6 +1,6 @@
 /* visorchannel_funcs.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -24,14 +24,14 @@
 
 #include "globals.h"
 #include "visorchannel.h"
-#include "guidutils.h"
+#include <linux/uuid.h>
 
 #define MYDRVNAME "visorchannel"
 
 struct VISORCHANNEL_Tag {
 	MEMREGION *memregion;	/* from visor_memregion_create() */
 	CHANNEL_HEADER chan_hdr;
-	GUID guid;
+	uuid_le guid;
 	ulong size;
 	BOOL needs_lock;
 	spinlock_t insert_lock;
@@ -50,7 +50,7 @@ struct VISORCHANNEL_Tag {
  */
 static VISORCHANNEL *
 visorchannel_create_guts(HOSTADDRESS physaddr, ulong channelBytes,
-			 VISORCHANNEL *parent, ulong off, GUID guid,
+			 VISORCHANNEL *parent, ulong off, uuid_le guid,
 			 BOOL needs_lock)
 {
 	VISORCHANNEL *p = NULL;
@@ -90,7 +90,7 @@ visorchannel_create_guts(HOSTADDRESS physaddr, ulong channelBytes,
 	if (channelBytes == 0)
 		/* we had better be a CLIENT of this channel */
 		channelBytes = (ulong) p->chan_hdr.Size;
-	if (STRUCTSEQUAL(guid, Guid0))
+	if (uuid_le_cmp(guid, NULL_UUID_LE) == 0)
 		/* we had better be a CLIENT of this channel */
 		guid = p->chan_hdr.Type;
 	if (visor_memregion_resize(p->memregion, channelBytes) < 0) {
@@ -114,7 +114,7 @@ Away:
 }
 
 VISORCHANNEL *
-visorchannel_create(HOSTADDRESS physaddr, ulong channelBytes, GUID guid)
+visorchannel_create(HOSTADDRESS physaddr, ulong channelBytes, uuid_le guid)
 {
 	return visorchannel_create_guts(physaddr, channelBytes, NULL, 0, guid,
 					FALSE);
@@ -123,7 +123,7 @@ EXPORT_SYMBOL_GPL(visorchannel_create);
 
 VISORCHANNEL *
 visorchannel_create_with_lock(HOSTADDRESS physaddr, ulong channelBytes,
-			      GUID guid)
+			      uuid_le guid)
 {
 	return visorchannel_create_guts(physaddr, channelBytes, NULL, 0, guid,
 					TRUE);
@@ -132,7 +132,7 @@ EXPORT_SYMBOL_GPL(visorchannel_create_with_lock);
 
 VISORCHANNEL *
 visorchannel_create_overlapped(ulong channelBytes,
-			       VISORCHANNEL *parent, ulong off, GUID guid)
+			       VISORCHANNEL *parent, ulong off, uuid_le guid)
 {
 	return visorchannel_create_guts(0, channelBytes, parent, off, guid,
 					FALSE);
@@ -142,7 +142,7 @@ EXPORT_SYMBOL_GPL(visorchannel_create_overlapped);
 VISORCHANNEL *
 visorchannel_create_overlapped_with_lock(ulong channelBytes,
 					 VISORCHANNEL *parent, ulong off,
-					 GUID guid)
+					 uuid_le guid)
 {
 	return visorchannel_create_guts(0, channelBytes, parent, off, guid,
 					TRUE);
@@ -177,23 +177,24 @@ visorchannel_get_nbytes(VISORCHANNEL *channel)
 EXPORT_SYMBOL_GPL(visorchannel_get_nbytes);
 
 char *
-visorchannel_GUID_id(GUID *guid, char *s)
+visorchannel_uuid_id(uuid_le *guid, char *s)
 {
-	return GUID_format1(guid, s);
+	sprintf(s, "%pUL", guid);
+	return s;
 }
-EXPORT_SYMBOL_GPL(visorchannel_GUID_id);
+EXPORT_SYMBOL_GPL(visorchannel_uuid_id);
 
 char *
 visorchannel_id(VISORCHANNEL *channel, char *s)
 {
-	return visorchannel_GUID_id(&channel->guid, s);
+	return visorchannel_uuid_id(&channel->guid, s);
 }
 EXPORT_SYMBOL_GPL(visorchannel_id);
 
 char *
 visorchannel_zoneid(VISORCHANNEL *channel, char *s)
 {
-	return visorchannel_GUID_id(&channel->chan_hdr.ZoneGuid, s);
+	return visorchannel_uuid_id(&channel->chan_hdr.ZoneGuid, s);
 }
 EXPORT_SYMBOL_GPL(visorchannel_zoneid);
 
@@ -204,12 +205,12 @@ visorchannel_get_clientpartition(VISORCHANNEL *channel)
 }
 EXPORT_SYMBOL_GPL(visorchannel_get_clientpartition);
 
-GUID
-visorchannel_get_GUID(VISORCHANNEL *channel)
+uuid_le
+visorchannel_get_uuid(VISORCHANNEL *channel)
 {
 	return channel->guid;
 }
-EXPORT_SYMBOL_GPL(visorchannel_get_GUID);
+EXPORT_SYMBOL_GPL(visorchannel_get_uuid);
 
 MEMREGION *
 visorchannel_get_memregion(VISORCHANNEL *channel)
@@ -278,10 +279,10 @@ Away:
 }
 EXPORT_SYMBOL_GPL(visorchannel_clear);
 
-void *
+void __iomem  *
 visorchannel_get_header(VISORCHANNEL *channel)
 {
-	return (void *) &(channel->chan_hdr);
+	return (void __iomem *) &(channel->chan_hdr);
 }
 EXPORT_SYMBOL_GPL(visorchannel_get_header);
 
@@ -558,7 +559,6 @@ visorchannel_debug(VISORCHANNEL *channel, int nQueues,
 	MEMREGION *memregion = NULL;
 	CHANNEL_HEADER hdr;
 	CHANNEL_HEADER *phdr = &hdr;
-	char s[99];
 	int i = 0;
 	int errcode = 0;
 
@@ -588,9 +588,8 @@ visorchannel_debug(VISORCHANNEL *channel, int nQueues,
 	nbytes = (ulong) (phdr->Size);
 	seq_printf(seq, "--- Begin channel @0x%-16.16Lx for 0x%lx bytes (region=0x%lx bytes) ---\n",
 		   addr + off, nbytes, nbytes_region);
-	seq_printf(seq, "Type            = %s\n", GUID_format2(&phdr->Type, s));
-	seq_printf(seq, "ZoneGuid        = %s\n",
-		   GUID_format2(&phdr->ZoneGuid, s));
+	seq_printf(seq, "Type            = %pUL\n", &phdr->Type);
+	seq_printf(seq, "ZoneGuid        = %pUL\n", &phdr->ZoneGuid);
 	seq_printf(seq, "Signature       = 0x%-16.16Lx\n",
 		   (long long) phdr->Signature);
 	seq_printf(seq, "LegacyState     = %lu\n", (ulong) phdr->LegacyState);
diff --git a/drivers/staging/unisys/visorchannel/visorchannel_main.c b/drivers/staging/unisys/visorchannel/visorchannel_main.c
index 482ee0ac1c1..f4be2e62c97 100644
--- a/drivers/staging/unisys/visorchannel/visorchannel_main.c
+++ b/drivers/staging/unisys/visorchannel/visorchannel_main.c
@@ -1,6 +1,6 @@
 /* visorchannel_main.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -22,13 +22,16 @@
 #include "globals.h"
 #include "channel.h"
 #include "visorchannel.h"
-#include "guidutils.h"
+#include <linux/uuid.h>
 
 #define MYDRVNAME "visorchannel"
 
 static int __init
 visorchannel_init(void)
 {
+	if (!unisys_spar_platform)
+		return -ENODEV;
+
 	INFODRV("driver version %s loaded", VERSION);
 	return 0;
 }
diff --git a/drivers/staging/unisys/visorchipset/Makefile b/drivers/staging/unisys/visorchipset/Makefile
index f5e8650e1b0..ead4b9c0271 100644
--- a/drivers/staging/unisys/visorchipset/Makefile
+++ b/drivers/staging/unisys/visorchipset/Makefile
@@ -4,7 +4,7 @@
 
 obj-$(CONFIG_UNISYS_VISORCHIPSET)	+= visorchipset.o
 
-visorchipset-y := visorchipset_main.o controlvm_direct.o file.o filexfer.o \
+visorchipset-y := visorchipset_main.o controlvm_direct.o file.o \
 			parser.o
 
 ccflags-y += -Idrivers/staging/unisys/include
diff --git a/drivers/staging/unisys/visorchipset/controlvm.h b/drivers/staging/unisys/visorchipset/controlvm.h
index 873fa12dfe6..012891c3f21 100644
--- a/drivers/staging/unisys/visorchipset/controlvm.h
+++ b/drivers/staging/unisys/visorchipset/controlvm.h
@@ -1,6 +1,6 @@
 /* controlvm.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/visorchipset/controlvm_direct.c b/drivers/staging/unisys/visorchipset/controlvm_direct.c
index b911ea85c09..cd10e3a2a07 100644
--- a/drivers/staging/unisys/visorchipset/controlvm_direct.c
+++ b/drivers/staging/unisys/visorchipset/controlvm_direct.c
@@ -1,6 +1,6 @@
 /* controlvm_direct.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/visorchipset/file.c b/drivers/staging/unisys/visorchipset/file.c
index e214a115328..fccc4f0c3a4 100644
--- a/drivers/staging/unisys/visorchipset/file.c
+++ b/drivers/staging/unisys/visorchipset/file.c
@@ -1,6 +1,6 @@
 /* file.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/visorchipset/file.h b/drivers/staging/unisys/visorchipset/file.h
index 597282aa9a0..21bb906242e 100644
--- a/drivers/staging/unisys/visorchipset/file.h
+++ b/drivers/staging/unisys/visorchipset/file.h
@@ -1,6 +1,6 @@
 /* file.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/visorchipset/filexfer.c b/drivers/staging/unisys/visorchipset/filexfer.c
deleted file mode 100644
index f950d6e85b5..00000000000
--- a/drivers/staging/unisys/visorchipset/filexfer.c
+++ /dev/null
@@ -1,506 +0,0 @@
-/* filexfer.c
- *
- * Copyright © 2013 - 2013 UNISYS CORPORATION
- * 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, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-/* Code here-in is the "glue" that connects controlvm messages with the
- * sparfilexfer driver, which is used to transfer file contents as payload
- * across the controlvm channel.
- */
-
-#include "globals.h"
-#include "controlvm.h"
-#include "visorchipset.h"
-#include "filexfer.h"
-
-#ifdef ENABLE_SPARFILEXFER /* sparfilexfer kernel module enabled in build */
-#include "sparfilexfer.h"
-
-/* Driver-global memory */
-static LIST_HEAD(Request_list);	/* list of struct any_request *, via
-				 * req_list memb */
-
-/* lock for above pool for allocation of any_request structs, and pool
-* name; note that kmem_cache_create requires that we keep the storage
-* for the pool name for the life of the pool
- */
-static DEFINE_SPINLOCK(Request_list_lock);
-
-static struct kmem_cache *Request_memory_pool;
-static const char Request_memory_pool_name[] = "filexfer_request_pool";
-size_t Caller_req_context_bytes = 0;	/* passed to filexfer_constructor() */
-
-/* This structure defines a single controlvm GETFILE conversation, which
- * consists of a single controlvm request message and 1 or more controlvm
- * response messages.
- */
-struct getfile_request {
-	CONTROLVM_MESSAGE_HEADER controlvm_header;
-	atomic_t buffers_in_use;
-	GET_CONTIGUOUS_CONTROLVM_PAYLOAD_FUNC get_contiguous_controlvm_payload;
-	CONTROLVM_RESPOND_WITH_PAYLOAD_FUNC controlvm_respond_with_payload;
-};
-
-/* This structure defines a single controlvm PUTFILE conversation, which
- * consists of a single controlvm request with a filename, and additional
- * controlvm messages with file data.
- */
-struct putfile_request {
-	GET_CONTROLVM_FILEDATA_FUNC get_controlvm_filedata;
-	CONTROLVM_RESPOND_FUNC controlvm_end_putFile;
-};
-
-/* This structure defines a single file transfer operation, which can either
- * be a GETFILE or PUTFILE.
- */
-struct any_request {
-	struct list_head req_list;
-	ulong2 file_request_number;
-	ulong2 data_sequence_number;
-	TRANSMITFILE_DUMP_FUNC dump_func;
-	BOOL is_get;
-	union {
-		struct getfile_request get;
-		struct putfile_request put;
-	};
-	/* Size of caller_context_data will be
-	 * <Caller_req_context_bytes> bytes.  I aligned this because I
-	 * am paranoid about what happens when an arbitrary data
-	 * structure with unknown alignment requirements gets copied
-	 * here.  I want caller_context_data to be aligned to the
-	 * coarsest possible alignment boundary that could be required
-	 * for any user data structure.
-	 */
-	u8 caller_context_data[1] __aligned(sizeof(ulong2));
-};
-
-/*
- * Links the any_request into the global list of allocated requests
- * (<Request_list>).
- */
-static void
-unit_tracking_create(struct list_head *dev_list_link)
-{
-	unsigned long flags;
-	spin_lock_irqsave(&Request_list_lock, flags);
-	list_add(dev_list_link, &Request_list);
-	spin_unlock_irqrestore(&Request_list_lock, flags);
-}
-
-/* Unlinks a any_request from the global list (<Request_list>).
- */
-static void
-unit_tracking_destroy(struct list_head *dev_list_link)
-{
-	unsigned long flags;
-	spin_lock_irqsave(&Request_list_lock, flags);
-	list_del(dev_list_link);
-	spin_unlock_irqrestore(&Request_list_lock, flags);
-}
-
-/* Allocate memory for and return a new any_request struct, and
- * link it to the global list of outstanding requests.
- */
-static struct any_request *
-alloc_request(char *fn, int ln)
-{
-	struct any_request *req = (struct any_request *)
-	    (visorchipset_cache_alloc(Request_memory_pool,
-				      FALSE,
-				      fn, ln));
-	if (!req)
-		return NULL;
-	memset(req, 0, sizeof(struct any_request) + Caller_req_context_bytes);
-	unit_tracking_create(&req->req_list);
-	return req;
-}
-
-/* Book-end for alloc_request().
- */
-static void
-free_request(struct any_request *req, char *fn, int ln)
-{
-	unit_tracking_destroy(&req->req_list);
-	visorchipset_cache_free(Request_memory_pool, req, fn, ln);
-}
-
-/* Constructor for filexfer.o.
- */
-int
-filexfer_constructor(size_t req_context_bytes)
-{
-	int rc = -1;
-
-	Caller_req_context_bytes = req_context_bytes;
-	Request_memory_pool =
-	    kmem_cache_create(Request_memory_pool_name,
-			      sizeof(struct any_request) +
-			      Caller_req_context_bytes,
-			      0, SLAB_HWCACHE_ALIGN, NULL);
-	if (!Request_memory_pool) {
-		LOGERR("failed to alloc Request_memory_pool");
-		rc = -ENOMEM;
-		goto Away;
-	}
-	rc = 0;
-Away:
-	if (rc < 0) {
-		if (Request_memory_pool) {
-			kmem_cache_destroy(Request_memory_pool);
-			Request_memory_pool = NULL;
-		}
-	}
-	return rc;
-}
-
-/* Destructor for filexfer.o.
- */
-void
-filexfer_destructor(void)
-{
-	if (Request_memory_pool) {
-		kmem_cache_destroy(Request_memory_pool);
-		Request_memory_pool = NULL;
-	}
-}
-
-/* This function will obtain an available chunk from the controlvm payload area,
- * store the size in bytes of the chunk in <actual_size>, and return a pointer
- * to the chunk.  The function is passed to the sparfilexfer driver, which calls
- * it whenever payload space is required to copy file data into.
- */
-static void *
-get_empty_bucket_for_getfile_data(void *context,
-				  ulong min_size, ulong max_size,
-				  ulong *actual_size)
-{
-	void *bucket;
-	struct any_request *req = (struct any_request *) context;
-
-	if (!req->is_get) {
-		LOGERR("%s - unexpected call", __func__);
-		return NULL;
-	}
-	bucket = (*req->get.get_contiguous_controlvm_payload)
-	    (min_size, max_size, actual_size);
-	if (bucket != NULL) {
-		atomic_inc(&req->get.buffers_in_use);
-		DBGINF("%s - sent %lu-byte buffer", __func__, *actual_size);
-	}
-	return bucket;
-}
-
-/* This function will send a controlvm response with data in the payload
- * (whose space was obtained with get_empty_bucket_for_getfile_data).  The
- * function is passed to the sparfilexfer driver, which calls it whenever it
- * wants to send file data back across the controlvm channel.
- */
-static int
-send_full_getfile_data_bucket(void *context, void *bucket,
-			      ulong bucket_actual_size, ulong bucket_used_size)
-{
-	struct any_request *req = (struct any_request *) context;
-
-	if (!req->is_get) {
-		LOGERR("%s - unexpected call", __func__);
-		return 0;
-	}
-	DBGINF("sending buffer for %lu/%lu",
-	       bucket_used_size, bucket_actual_size);
-	if (!(*req->get.controlvm_respond_with_payload)
-	    (&req->get.controlvm_header,
-	     req->file_request_number,
-	     req->data_sequence_number++,
-	     0, bucket, bucket_actual_size, bucket_used_size, TRUE))
-		atomic_dec(&req->get.buffers_in_use);
-	return 0;
-}
-
-/* This function will send a controlvm response indicating the end of a
- * GETFILE transfer.  The function is passed to the sparfilexfer driver.
- */
-static void
-send_end_of_getfile_data(void *context, int status)
-{
-	struct any_request *req = (struct any_request *) context;
-	if (!req->is_get) {
-		LOGERR("%s - unexpected call", __func__);
-		return;
-	}
-	LOGINF("status=%d", status);
-	(*req->get.controlvm_respond_with_payload)
-	    (&req->get.controlvm_header,
-	     req->file_request_number,
-	     req->data_sequence_number++, status, NULL, 0, 0, FALSE);
-	free_request(req, __FILE__, __LINE__);
-	module_put(THIS_MODULE);
-}
-
-/* This function supplies data for a PUTFILE transfer.
- * The function is passed to the sparfilexfer driver.
- */
-static int
-get_putfile_data(void *context, void *pbuf, size_t bufsize,
-		 BOOL buf_is_userspace, size_t *bytes_transferred)
-{
-	struct any_request *req = (struct any_request *) context;
-	if (req->is_get) {
-		LOGERR("%s - unexpected call", __func__);
-		return -1;
-	}
-	return (*req->put.get_controlvm_filedata) (&req->caller_context_data[0],
-						   pbuf, bufsize,
-						   buf_is_userspace,
-						   bytes_transferred);
-}
-
-/* This function is called to indicate the end of a PUTFILE transfer.
- * The function is passed to the sparfilexfer driver.
- */
-static void
-end_putfile(void *context, int status)
-{
-	struct any_request *req = (struct any_request *) context;
-	if (req->is_get) {
-		LOGERR("%s - unexpected call", __func__);
-		return;
-	}
-	(*req->put.controlvm_end_putFile) (&req->caller_context_data[0],
-					   status);
-	free_request(req, __FILE__, __LINE__);
-	module_put(THIS_MODULE);
-}
-
-/* Refer to filexfer.h for description. */
-BOOL
-filexfer_getFile(CONTROLVM_MESSAGE_HEADER *msgHdr,
-		 ulong2 file_request_number,
-		 uint uplink_index,
-		 uint disk_index,
-		 char *file_name,
-		 GET_CONTIGUOUS_CONTROLVM_PAYLOAD_FUNC
-		 get_contiguous_controlvm_payload,
-		 CONTROLVM_RESPOND_WITH_PAYLOAD_FUNC
-		 controlvm_respond_with_payload,
-		 TRANSMITFILE_DUMP_FUNC dump_func)
-{
-	BOOL use_count_up = FALSE;
-	BOOL failed = TRUE;
-	struct any_request *req = alloc_request(__FILE__, __LINE__);
-
-	if (!req) {
-		LOGERR("allocation of any_request failed");
-		goto Away;
-	}
-	/* We need to increment this module's use count because we're handing
-	 * off pointers to functions within this module to be used by
-	 * another module.
-	 */
-	__module_get(THIS_MODULE);
-	use_count_up = TRUE;
-	req->is_get = TRUE;
-	req->file_request_number = file_request_number;
-	req->data_sequence_number = 0;
-	req->dump_func = dump_func;
-	req->get.controlvm_header = *msgHdr;
-	atomic_set(&req->get.buffers_in_use, 0);
-	req->get.get_contiguous_controlvm_payload =
-	    get_contiguous_controlvm_payload;
-	req->get.controlvm_respond_with_payload =
-	    controlvm_respond_with_payload;
-	if (sparfilexfer_local2remote(req,	/* context, passed to
-						 * callback funcs */
-				      file_name,
-				      file_request_number,
-				      uplink_index,
-				      disk_index,
-				      get_empty_bucket_for_getfile_data,
-				      send_full_getfile_data_bucket,
-				      send_end_of_getfile_data) < 0) {
-		LOGERR("sparfilexfer_local2remote failed");
-		goto Away;
-	}
-	failed = FALSE;
-Away:
-	if (failed) {
-		if (use_count_up) {
-			module_put(THIS_MODULE);
-			use_count_up = FALSE;
-		}
-		if (req) {
-			free_request(req, __FILE__, __LINE__);
-			req = NULL;
-		}
-		return FALSE;
-	} else {
-		return TRUE;
-		/* success; send callbacks will be called for responses */
-	}
-}
-
-/* Refer to filexfer.h for description. */
-void *
-filexfer_putFile(CONTROLVM_MESSAGE_HEADER *msgHdr,
-		 ulong2 file_request_number,
-		 uint uplink_index,
-		 uint disk_index,
-		 char *file_name,
-		 TRANSMITFILE_INIT_CONTEXT_FUNC init_context,
-		 GET_CONTROLVM_FILEDATA_FUNC get_controlvm_filedata,
-		 CONTROLVM_RESPOND_FUNC controlvm_end_putFile,
-		 TRANSMITFILE_DUMP_FUNC dump_func)
-{
-	BOOL use_count_up = FALSE;
-	BOOL failed = TRUE;
-	struct any_request *req = alloc_request(__FILE__, __LINE__);
-	void *caller_ctx = NULL;
-
-	if (!req) {
-		LOGERR("allocation of any_request failed");
-		goto Away;
-	}
-	caller_ctx = (void *) (&(req->caller_context_data[0]));
-	/* We need to increment this module's use count because we're handing
-	 * off pointers to functions within this module to be used by
-	 * another module.
-	 */
-	__module_get(THIS_MODULE);
-	use_count_up = TRUE;
-	req->is_get = FALSE;
-	req->file_request_number = file_request_number;
-	req->data_sequence_number = 0;
-	req->dump_func = dump_func;
-	req->put.get_controlvm_filedata = get_controlvm_filedata;
-	req->put.controlvm_end_putFile = controlvm_end_putFile;
-	(*init_context) (caller_ctx, msgHdr, file_request_number);
-	if (sparfilexfer_remote2local(req,	/* context, passed to
-						 * callback funcs */
-				      file_name,
-				      file_request_number,
-				      uplink_index,
-				      disk_index,
-				      get_putfile_data, end_putfile) < 0) {
-		LOGERR("sparfilexfer_remote2local failed");
-		goto Away;
-	}
-	failed = FALSE;
-Away:
-	if (failed) {
-		if (use_count_up) {
-			module_put(THIS_MODULE);
-			use_count_up = FALSE;
-		}
-		if (req) {
-			free_request(req, __FILE__, __LINE__);
-			req = NULL;
-		}
-		return NULL;
-	} else {
-		return caller_ctx;
-		/* success; callbacks will be called for responses */
-	}
-}
-
-static void
-dump_get_request(struct seq_file *f, struct getfile_request *getreq)
-{
-	seq_printf(f, "  buffers_in_use=%d\n",
-		   atomic_read(&getreq->buffers_in_use));
-}
-
-static void
-dump_put_request(struct seq_file *f, struct putfile_request *putreq)
-{
-}
-
-static void
-dump_request(struct seq_file *f, struct any_request *req)
-{
-	seq_printf(f, "* %s id=%llu seq=%llu\n",
-		   ((req->is_get) ? "Get" : "Put"),
-		   req->file_request_number, req->data_sequence_number);
-	if (req->is_get)
-		dump_get_request(f, &req->get);
-	else
-		dump_put_request(f, &req->put);
-	if (req->dump_func)
-		(*req->dump_func) (f, &(req->caller_context_data[0]), "  ");
-}
-
-void
-filexfer_dump(struct seq_file *f)
-{
-	ulong flags;
-	struct list_head *entry;
-
-	seq_puts(f, "Outstanding TRANSMIT_FILE requests:\n");
-	spin_lock_irqsave(&Request_list_lock, flags);
-	list_for_each(entry, &Request_list) {
-		struct any_request *req;
-		req = list_entry(entry, struct any_request, req_list);
-		dump_request(f, req);
-	}
-	spin_unlock_irqrestore(&Request_list_lock, flags);
-}
-
-#else				/* ifdef ENABLE_SPARFILEXFER */
-int
-filexfer_constructor(size_t req_context_bytes)
-{
-	return 0;		/* success */
-}
-
-void
-filexfer_destructor(void)
-{
-}
-
-BOOL
-filexfer_getFile(CONTROLVM_MESSAGE_HEADER *msgHdr,
-		 u64 file_request_number,
-		 uint uplink_index,
-		 uint disk_index,
-		 char *file_name,
-		 GET_CONTIGUOUS_CONTROLVM_PAYLOAD_FUNC
-		 get_contiguous_controlvm_payload,
-		 CONTROLVM_RESPOND_WITH_PAYLOAD_FUNC
-		 controlvm_respond_with_payload,
-		 TRANSMITFILE_DUMP_FUNC dump_func)
-{
-	/* since no sparfilexfer module exists to call, we just fail */
-	return FALSE;
-}
-
-void *
-filexfer_putFile(CONTROLVM_MESSAGE_HEADER *msgHdr,
-		 u64 file_request_number,
-		 uint uplink_index,
-		 uint disk_index,
-		 char *file_name,
-		 TRANSMITFILE_INIT_CONTEXT_FUNC init_context,
-		 GET_CONTROLVM_FILEDATA_FUNC get_controlvm_filedata,
-		 CONTROLVM_RESPOND_FUNC controlvm_end_putFile,
-		 TRANSMITFILE_DUMP_FUNC dump_func)
-{
-	/* since no sparfilexfer module exists to call, we just fail */
-	return NULL;
-}
-
-void
-filexfer_dump(struct seq_file *f)
-{
-}
-
-#endif				/* ifdef ENABLE_SPARFILEXFER */
diff --git a/drivers/staging/unisys/visorchipset/filexfer.h b/drivers/staging/unisys/visorchipset/filexfer.h
deleted file mode 100644
index a1bfca69cdc..00000000000
--- a/drivers/staging/unisys/visorchipset/filexfer.h
+++ /dev/null
@@ -1,147 +0,0 @@
-/* filexfer.h
- *
- * Copyright © 2013 - 2013 UNISYS CORPORATION
- * 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, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-/* This header file defines the interface that filexfer.c provides to other
- * code in the visorchipset driver.
- */
-
-#ifndef __FILEXFER_H__
-#define __FILEXFER_H__
-
-#include "globals.h"
-#include "controlvmchannel.h"
-#include <linux/seq_file.h>
-
-typedef void *(*GET_CONTIGUOUS_CONTROLVM_PAYLOAD_FUNC) (ulong min_size,
-							ulong max_size,
-							ulong *actual_size);
-
-typedef BOOL
-(*CONTROLVM_RESPOND_WITH_PAYLOAD_FUNC) (CONTROLVM_MESSAGE_HEADER *msgHdr,
-					u64 fileRequestNumber,
-					u64 dataSequenceNumber,
-					int response,
-					void *bucket, ulong payloadChunkSize,
-					ulong payloadUsedBytes, BOOL partial);
-
-typedef void
-(*TRANSMITFILE_INIT_CONTEXT_FUNC)(void *ctx,
-				  const CONTROLVM_MESSAGE_HEADER *hdr,
-				  u64 file_request_number);
-typedef void (*TRANSMITFILE_DUMP_FUNC) (struct seq_file *f, void *ctx,
-					const char *pfx);
-typedef int (*GET_CONTROLVM_FILEDATA_FUNC) (void *ctx,
-					    void *buf, size_t bufsize,
-					    BOOL buf_is_userspace,
-					    size_t *bytes_transferred);
-typedef void (*CONTROLVM_RESPOND_FUNC) (void *ctx, int response);
-
-/* Call once to initialize filexfer.o.
- * req_context_bytes number of bytes the caller needs to keep track of each file
- * transfer conversation.  The <ctx_init_value> passed to filexfer_putFile() is
- * assumed to be this many bytes in size.  Code within filexfer.o will copy this
- * into a dynamically-allocated area, and pass back a pointer to that area in
- * callback functions.
- */
-int filexfer_constructor(size_t req_context_bytes);
-
-/* Call once to clean up filexfer.o */
-void filexfer_destructor(void);
-
-/* Call this to dump diagnostic info about all outstanding getFiles/putFiles */
-void filexfer_dump(struct seq_file *f);
-
-/* Call to transfer a file from the local filesystem (i.e., from the environment
- * where this driver is running) across the controlvm channel to a remote
- * environment.  1 or more controlvm responses will be sent as a result, each
- * of which whose payload contains file data.  Only the last controlvm message
- * will have Flags.partialCompletion==0.
- *
- *   msgHdr      the controlvm message header of the GETFILE request which
- *               we just received
- *   file_request_number  this is all data from the GETFILE request that
- *   uplink_index         define which file is to be transferred
- *   disk_index
- *   file_name
- *   get_contiguous_controlvm_payload  function to call when space is needed
- *                                     in the payload area
- *   controlvm_respond_with_payload    function to call to send each controlvm
- *                                     response containing file data as the
- *                                     payload; returns FALSE only if the
- *				       payload buffer was freed inline
- *   dump_func                         function to dump context data in
- *                                     human-readable format
- *
- *  Returns TRUE iff the file transfer request has been successfully initiated,
- *  or FALSE to indicate failure.
- */
-BOOL
-filexfer_getFile(CONTROLVM_MESSAGE_HEADER *msgHdr,
-		 u64 file_request_number,
-		 uint uplink_index,
-		 uint disk_index,
-		 char *file_name,
-		 GET_CONTIGUOUS_CONTROLVM_PAYLOAD_FUNC
-		 get_contiguous_controlvm_payload,
-		 CONTROLVM_RESPOND_WITH_PAYLOAD_FUNC
-		 controlvm_respond_with_payload,
-		 TRANSMITFILE_DUMP_FUNC dump_func);
-
-/* Call to create a file in the local filesystem (i.e., in the environment
- * where this driver is running) from data received as payload in
- * controlvm channel messages from a remote environment.  1 or more controlvm
- * messages will be received for this transfer, and only the last will have
- * Flags.partialCompletion==0.
- *
- *   msgHdr      the controlvm message header of the PUTFILE request which
- *               we just received
- *   file_request_number  this is all data from the PUTFILE request that
- *   uplink_index         define which file is to be created in the local
- *   disk_index           filesystem
- *   file_name
- *   init_context         function to call to initialize the
- *                        <req_context_bytes>-sized storage area returned by
- *                        this func; note that it would NOT be sufficient to
- *                        allow the caller to initialize this upon return, as
- *                        the the other user-supplied callbacks might have
- *                        already been called by then
- *   get_controlvm_filedata   function to call to obtain more data for the file
- *                            being written; refer to get_controlvm_filedata()
- *                            in visorchipset_main.c for a complete description
- *                            of parameters
- *   controlvm_end_putFile    function to call to indicate that creation of the
- *                            local file has completed;  set <response> to a
- *                            negative value to indicate an error
- *   dump_func                function to dump context data in human-readable
- *                            format
- *
- *  Returns a pointer to a dynamically-allocated storage area of size
- *  <req_context_bytes> which the caller can use, or NULL for error.  The
- *  caller should NEVER free the returned pointer, but should expect to receive
- *  it as the <ctx> argument when callback functions are called.
- */
-void *filexfer_putFile(CONTROLVM_MESSAGE_HEADER *msgHdr,
-		       u64 file_request_number,
-		       uint uplink_index,
-		       uint disk_index,
-		       char *file_name,
-		       TRANSMITFILE_INIT_CONTEXT_FUNC init_context,
-		       GET_CONTROLVM_FILEDATA_FUNC get_controlvm_filedata,
-		       CONTROLVM_RESPOND_FUNC controlvm_end_putFile,
-		       TRANSMITFILE_DUMP_FUNC dump_func);
-
-#endif
diff --git a/drivers/staging/unisys/visorchipset/globals.h b/drivers/staging/unisys/visorchipset/globals.h
index a0e6d4fc03a..0fe14599f18 100644
--- a/drivers/staging/unisys/visorchipset/globals.h
+++ b/drivers/staging/unisys/visorchipset/globals.h
@@ -1,6 +1,6 @@
 /* globals.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/visorchipset/parser.c b/drivers/staging/unisys/visorchipset/parser.c
index b408d415bd8..4274dd2dbbd 100644
--- a/drivers/staging/unisys/visorchipset/parser.c
+++ b/drivers/staging/unisys/visorchipset/parser.c
@@ -1,6 +1,6 @@
 /* parser.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -20,6 +20,7 @@
 #include "controlvmchannel.h"
 #include <linux/ctype.h>
 #include <linux/mm.h>
+#include <linux/uuid.h>
 
 #define MYDRVNAME "visorchipset_parser"
 #define CURRENT_FILE_PC VISOR_CHIPSET_PC_parser_c
@@ -191,7 +192,7 @@ parser_byteStream_get(PARSER_CONTEXT *ctx, ulong *nbytes)
 	return (void *) ctx->data;
 }
 
-GUID
+uuid_le
 parser_id_get(PARSER_CONTEXT *ctx)
 {
 	ULTRA_CONTROLVM_PARAMETERS_HEADER *phdr = NULL;
@@ -199,7 +200,7 @@ parser_id_get(PARSER_CONTEXT *ctx)
 	if (ctx == NULL) {
 		ERRDRV("%s (%s:%d) - no context",
 		       __func__, __FILE__, __LINE__);
-		return Guid0;
+		return NULL_UUID_LE;
 	}
 	phdr = (ULTRA_CONTROLVM_PARAMETERS_HEADER *) (ctx->data);
 	return phdr->Id;
diff --git a/drivers/staging/unisys/visorchipset/parser.h b/drivers/staging/unisys/visorchipset/parser.h
index a0cc50a533c..be85fd68c0c 100644
--- a/drivers/staging/unisys/visorchipset/parser.h
+++ b/drivers/staging/unisys/visorchipset/parser.h
@@ -1,6 +1,6 @@
 /* parser.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -18,6 +18,8 @@
 #ifndef __PARSER_H__
 #define __PARSER_H__
 
+#include <linux/uuid.h>
+
 #include "uniklog.h"
 #include "timskmod.h"
 #include "channel.h"
@@ -37,7 +39,7 @@ PARSER_CONTEXT *parser_init_byteStream(U64 addr, U32 bytes, BOOL isLocal,
 void parser_param_start(PARSER_CONTEXT *ctx, PARSER_WHICH_STRING which_string);
 void *parser_param_get(PARSER_CONTEXT *ctx, char *nam, int namesize);
 void *parser_string_get(PARSER_CONTEXT *ctx);
-GUID parser_id_get(PARSER_CONTEXT *ctx);
+uuid_le parser_id_get(PARSER_CONTEXT *ctx);
 char *parser_simpleString_get(PARSER_CONTEXT *ctx);
 void *parser_byteStream_get(PARSER_CONTEXT *ctx, ulong *nbytes);
 void parser_done(PARSER_CONTEXT *ctx);
diff --git a/drivers/staging/unisys/visorchipset/testing.h b/drivers/staging/unisys/visorchipset/testing.h
index a44f5556cb2..015d502cbb1 100644
--- a/drivers/staging/unisys/visorchipset/testing.h
+++ b/drivers/staging/unisys/visorchipset/testing.h
@@ -1,6 +1,6 @@
 /* testing.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -19,6 +19,7 @@
 #define __VISORCHIPSET_TESTING_H__
 
 #define VISORCHIPSET_TEST_PROC
+#include <linux/uuid.h>
 #include "globals.h"
 #include "controlvmchannel.h"
 
@@ -28,10 +29,10 @@ void test_manufacture_vnic_client_add(void *p);
 void test_manufacture_vnic_client_add_phys(HOSTADDRESS addr);
 void test_manufacture_preamble_messages(void);
 void test_manufacture_device_attach(ulong busNo, ulong devNo);
-void test_manufacture_device_add(ulong busNo, ulong devNo, GUID dataTypeGuid,
+void test_manufacture_device_add(ulong busNo, ulong devNo, uuid_le dataTypeGuid,
 				 void *pChannel);
 void test_manufacture_add_bus(ulong busNo, ulong maxDevices,
-			      GUID id, u8 *name, BOOL isServer);
+			      uuid_le id, u8 *name, BOOL isServer);
 void test_manufacture_device_destroy(ulong busNo, ulong devNo);
 void test_manufacture_bus_destroy(ulong busNo);
 void test_manufacture_detach_externalPort(ulong switchNo, ulong externalPortNo);
diff --git a/drivers/staging/unisys/visorchipset/visorchipset.h b/drivers/staging/unisys/visorchipset/visorchipset.h
index d4bf203cdfd..e01cc7207ba 100644
--- a/drivers/staging/unisys/visorchipset/visorchipset.h
+++ b/drivers/staging/unisys/visorchipset/visorchipset.h
@@ -1,6 +1,6 @@
 /* visorchipset.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -18,6 +18,8 @@
 #ifndef __VISORCHIPSET_H__
 #define __VISORCHIPSET_H__
 
+#include <linux/uuid.h>
+
 #include "timskmod.h"
 #include "channel.h"
 #include "controlvmchannel.h"
@@ -63,8 +65,8 @@ typedef struct {
 	HOSTADDRESS channelAddr;
 	struct InterruptInfo intr;
 	U64 nChannelBytes;
-	GUID channelTypeGuid;
-	GUID channelInstGuid;
+	uuid_le channelTypeGuid;
+	uuid_le channelInstGuid;
 
 } VISORCHIPSET_CHANNEL_INFO;
 
@@ -77,7 +79,7 @@ typedef struct {
 	struct list_head entry;
 	U32 busNo;
 	U32 devNo;
-	GUID devInstGuid;
+	uuid_le devInstGuid;
 	VISORCHIPSET_STATE state;
 	VISORCHIPSET_CHANNEL_INFO chanInfo;
 	U32 Reserved1;		/* CONTROLVM_ID */
@@ -104,9 +106,9 @@ finddevice(struct list_head *list, U32 busNo, U32 devNo)
 
 static inline void delbusdevices(struct list_head *list, U32 busNo)
 {
-	VISORCHIPSET_DEVICE_INFO *p;
+	VISORCHIPSET_DEVICE_INFO *p, *tmp;
 
-	list_for_each_entry(p, list, entry) {
+	list_for_each_entry_safe(p, tmp, list, entry) {
 		if (p->busNo == busNo) {
 			list_del(&p->entry);
 			kfree(p);
@@ -125,7 +127,7 @@ typedef struct {
 	U32 busNo;
 	VISORCHIPSET_STATE state;
 	VISORCHIPSET_CHANNEL_INFO chanInfo;
-	GUID partitionGuid;
+	uuid_le partitionGuid;
 	U64 partitionHandle;
 	U8 *name;		/* UTF8 */
 	U8 *description;	/* UTF8 */
@@ -161,7 +163,7 @@ findbus(struct list_head *list, U32 busNo)
 typedef struct {
 	U32 switchNo;
 	VISORCHIPSET_STATE state;
-	GUID switchTypeGuid;
+	uuid_le switchTypeGuid;
 	U8 *authService1;
 	U8 *authService2;
 	U8 *authService3;
@@ -181,7 +183,7 @@ typedef struct {
 	U32 switchNo;
 	U32 externalPortNo;
 	VISORCHIPSET_STATE state;
-	GUID networkZoneGuid;
+	uuid_le networkZoneGuid;
 	int pdPort;
 	U8 *ip;
 	U8 *ipNetmask;
@@ -224,7 +226,7 @@ typedef struct {
 	void (*device_destroy)(ulong busNo, ulong devNo);
 	void (*device_pause)(ulong busNo, ulong devNo);
 	void (*device_resume)(ulong busNo, ulong devNo);
-	int (*get_channel_info)(GUID typeGuid, ulong *minSize,
+	int (*get_channel_info)(uuid_le typeGuid, ulong *minSize,
 				 ulong *maxSize);
 } VISORCHIPSET_BUSDEV_NOTIFIERS;
 
diff --git a/drivers/staging/unisys/visorchipset/visorchipset_main.c b/drivers/staging/unisys/visorchipset/visorchipset_main.c
index 257c6e59b46..0a602b9c130 100644
--- a/drivers/staging/unisys/visorchipset/visorchipset_main.c
+++ b/drivers/staging/unisys/visorchipset/visorchipset_main.c
@@ -1,6 +1,6 @@
 /* visorchipset_main.c
  *
- * Copyright � 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -26,14 +26,13 @@
 #include "parser.h"
 #include "uniklog.h"
 #include "uisutils.h"
-#include "guidutils.h"
 #include "controlvmcompletionstatus.h"
 #include "guestlinuxdebug.h"
-#include "filexfer.h"
 
 #include <linux/nls.h>
 #include <linux/netdevice.h>
 #include <linux/platform_device.h>
+#include <linux/uuid.h>
 
 #define CURRENT_FILE_PC VISOR_CHIPSET_PC_visorchipset_main_c
 #define TEST_VNIC_PHYSITF "eth0"	/* physical network itf for
@@ -82,7 +81,7 @@ typedef struct {
 static CONTROLVM_MESSAGE_HEADER g_DiagMsgHdr;
 static CONTROLVM_MESSAGE_HEADER g_ChipSetMsgHdr;
 static CONTROLVM_MESSAGE_HEADER g_DelDumpMsgHdr;
-static const GUID UltraDiagPoolChannelProtocolGuid =
+static const uuid_le UltraDiagPoolChannelProtocolGuid =
 	ULTRA_DIAG_POOL_CHANNEL_PROTOCOL_GUID;
 /* 0xffffff is an invalid Bus/Device number */
 static ulong g_diagpoolBusNo = 0xffffff;
@@ -93,15 +92,12 @@ static CONTROLVM_MESSAGE_PACKET g_DeviceChangeStatePacket;
  * "visorhackbus")
  */
 #define FOR_VISORHACKBUS(channel_type_guid) \
-	((memcmp(&channel_type_guid, &UltraVnicChannelProtocolGuid, \
-		 sizeof(GUID)) == 0) ||				    \
-	 (memcmp(&channel_type_guid, &UltraVhbaChannelProtocolGuid, \
-		 sizeof(GUID)) == 0))
+	(((uuid_le_cmp(channel_type_guid, UltraVnicChannelProtocolGuid) == 0)\
+	|| (uuid_le_cmp(channel_type_guid, UltraVhbaChannelProtocolGuid) == 0)))
 #define FOR_VISORBUS(channel_type_guid) (!(FOR_VISORHACKBUS(channel_type_guid)))
 
 #define is_diagpool_channel(channel_type_guid) \
-	 (memcmp(&channel_type_guid, \
-		 &UltraDiagPoolChannelProtocolGuid, sizeof(GUID)) == 0)
+	 (uuid_le_cmp(channel_type_guid, UltraDiagPoolChannelProtocolGuid) == 0)
 
 typedef enum {
 	PARTPROP_invalid,
@@ -605,16 +601,16 @@ EXPORT_SYMBOL_GPL(visorchipset_register_busdev_client);
 static void
 cleanup_controlvm_structures(void)
 {
-	VISORCHIPSET_BUS_INFO *bi;
-	VISORCHIPSET_DEVICE_INFO *di;
+	VISORCHIPSET_BUS_INFO *bi, *tmp_bi;
+	VISORCHIPSET_DEVICE_INFO *di, *tmp_di;
 
-	list_for_each_entry(bi, &BusInfoList, entry) {
+	list_for_each_entry_safe(bi, tmp_bi, &BusInfoList, entry) {
 		busInfo_clear(bi);
 		list_del(&bi->entry);
 		kfree(bi);
 	}
 
-	list_for_each_entry(di, &DevInfoList, entry) {
+	list_for_each_entry_safe(di, tmp_di, &DevInfoList, entry) {
 		devInfo_clear(di);
 		list_del(&di->entry);
 		kfree(di);
@@ -1189,7 +1185,7 @@ bus_configure(CONTROLVM_MESSAGE *inmsg, PARSER_CONTEXT *parser_ctx)
 	parser_param_start(parser_ctx, PARSERSTRING_NAME);
 	pBusInfo->name = parser_string_get(parser_ctx);
 
-	visorchannel_GUID_id(&pBusInfo->partitionGuid, s);
+	visorchannel_uuid_id(&pBusInfo->partitionGuid, s);
 	pBusInfo->procObject =
 	    visor_proc_CreateObject(PartitionType, s, (void *) (pBusInfo));
 	if (pBusInfo->procObject == NULL) {
@@ -2699,6 +2695,9 @@ visorchipset_init(void)
 	struct proc_dir_entry *toolaction_file;
 	struct proc_dir_entry *bootToTool_file;
 
+	if (!unisys_spar_platform)
+		return -ENODEV;
+
 	LOGINF("chipset driver version %s loaded", VERSION);
 	/* process module options */
 	POSTCODE_LINUX_2(DRIVER_ENTRY_PC, POSTCODE_SEVERITY_INFO);
@@ -2773,12 +2772,6 @@ visorchipset_init(void)
 			ProcDir, &parahotplug_proc_fops);
 	memset(&g_DelDumpMsgHdr, 0, sizeof(CONTROLVM_MESSAGE_HEADER));
 
-	if (filexfer_constructor(sizeof(struct putfile_request)) < 0) {
-		ERRDRV("filexfer_constructor failed: (status=-1)\n");
-		POSTCODE_LINUX_2(CHIPSET_INIT_FAILURE_PC, DIAG_SEVERITY_ERR);
-		rc = -1;
-		goto Away;
-	}
 	Putfile_buffer_list_pool =
 	    kmem_cache_create(Putfile_buffer_list_pool_name,
 			      sizeof(struct putfile_buffer_entry),
@@ -2862,7 +2855,6 @@ visorchipset_exit(void)
 		kmem_cache_destroy(Putfile_buffer_list_pool);
 		Putfile_buffer_list_pool = NULL;
 	}
-	filexfer_destructor();
 	if (ControlVmObject) {
 		visor_proc_DestroyObject(ControlVmObject);
 		ControlVmObject = NULL;
diff --git a/drivers/staging/unisys/visorchipset/visorchipset_umode.h b/drivers/staging/unisys/visorchipset/visorchipset_umode.h
index 259e840376a..06ba5b7e425 100644
--- a/drivers/staging/unisys/visorchipset/visorchipset_umode.h
+++ b/drivers/staging/unisys/visorchipset/visorchipset_umode.h
@@ -1,6 +1,6 @@
 /* visorchipset_umode.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/visorutil/charqueue.c b/drivers/staging/unisys/visorutil/charqueue.c
index 0ceede129e8..22241c7b4f7 100644
--- a/drivers/staging/unisys/visorutil/charqueue.c
+++ b/drivers/staging/unisys/visorutil/charqueue.c
@@ -1,6 +1,6 @@
 /* charqueue.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -41,6 +41,7 @@ CHARQUEUE *visor_charqueue_create(ulong nslots)
 {
 	int alloc_size = sizeof(CHARQUEUE) + nslots + 1;
 	CHARQUEUE *cq = kmalloc(alloc_size, GFP_KERNEL|__GFP_NORETRY);
+
 	if (cq == NULL) {
 		ERRDRV("visor_charqueue_create allocation failed (alloc_size=%d)",
 		       alloc_size);
@@ -75,6 +76,7 @@ EXPORT_SYMBOL_GPL(visor_charqueue_enqueue);
 BOOL visor_charqueue_is_empty(CHARQUEUE *charqueue)
 {
 	BOOL b;
+
 	spin_lock(&charqueue->lock);
 	b = IS_EMPTY(charqueue);
 	spin_unlock(&charqueue->lock);
diff --git a/drivers/staging/unisys/visorutil/charqueue.h b/drivers/staging/unisys/visorutil/charqueue.h
index e82ae0bc895..d6f16587a36 100644
--- a/drivers/staging/unisys/visorutil/charqueue.h
+++ b/drivers/staging/unisys/visorutil/charqueue.h
@@ -1,6 +1,6 @@
 /* charqueue.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/visorutil/easyproc.c b/drivers/staging/unisys/visorutil/easyproc.c
index 60b6b83d1b2..3b388494e2a 100644
--- a/drivers/staging/unisys/visorutil/easyproc.c
+++ b/drivers/staging/unisys/visorutil/easyproc.c
@@ -1,4 +1,4 @@
-/* Copyright © 2010 - 2013 UNISYS CORPORATION
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -61,6 +61,7 @@ static struct proc_dir_entry *
 	createProcDir(char *name, struct proc_dir_entry *parent)
 {
 	struct proc_dir_entry *p = proc_mkdir_mode(name, S_IFDIR, parent);
+
 	if (p == NULL)
 		ERRDRV("failed to create /proc directory %s", name);
 	return p;
@@ -196,6 +197,7 @@ void visor_easyproc_InitDevice(struct easyproc_driver_info *pdriver,
 {
 	if ((pdriver->ProcDeviceDir != NULL) && (p->procDevicexDir == NULL)) {
 		char s[29];
+
 		sprintf(s, "%d", devno);
 		p->procDevicexDir = createProcDir(s, pdriver->ProcDeviceDir);
 		p->devno = devno;
@@ -267,6 +269,7 @@ void visor_easyproc_DeInitDevice(struct easyproc_driver_info *pdriver,
 				 struct easyproc_device_info *p, int devno)
 {
 	size_t i;
+
 	for (i = 0; i < ARRAY_SIZE(p->device_property_info); i++) {
 		if (p->device_property_info[i].procEntry != NULL) {
 			struct easyproc_device_property_info *px =
@@ -281,6 +284,7 @@ void visor_easyproc_DeInitDevice(struct easyproc_driver_info *pdriver,
 	}
 	if (p->procDevicexDir != NULL) {
 		char s[29];
+
 		sprintf(s, "%d", devno);
 		remove_proc_entry(s, pdriver->ProcDeviceDir);
 		p->procDevicexDir = NULL;
@@ -334,6 +338,7 @@ static ssize_t proc_write_driver(struct file *file, const char __user *buffer,
 	struct seq_file *seq = (struct seq_file *)file->private_data;
 	struct easyproc_driver_info *p = NULL;
 	char local_buf[256];
+
 	if (seq == NULL)
 		return 0;
 	p = (struct easyproc_driver_info *)(seq->private);
@@ -356,6 +361,7 @@ static ssize_t proc_write_device(struct file *file, const char __user *buffer,
 	struct seq_file *seq = (struct seq_file *)file->private_data;
 	struct easyproc_device_info *p = NULL;
 	char local_buf[256];
+
 	if (seq == NULL)
 		return 0;
 	p = (struct easyproc_device_info *)(seq->private);
diff --git a/drivers/staging/unisys/visorutil/easyproc.h b/drivers/staging/unisys/visorutil/easyproc.h
index 1cef1fd33d5..6ce7d5eb6ad 100644
--- a/drivers/staging/unisys/visorutil/easyproc.h
+++ b/drivers/staging/unisys/visorutil/easyproc.h
@@ -1,6 +1,6 @@
 /* easyproc.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/staging/unisys/visorutil/memregion.h b/drivers/staging/unisys/visorutil/memregion.h
index bb122dbeafb..f4a65d2fcf0 100644
--- a/drivers/staging/unisys/visorutil/memregion.h
+++ b/drivers/staging/unisys/visorutil/memregion.h
@@ -1,6 +1,6 @@
 /* memregion.h
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -38,6 +38,6 @@ HOSTADDRESS visor_memregion_get_physaddr(MEMREGION *memregion);
 ulong visor_memregion_get_nbytes(MEMREGION *memregion);
 void memregion_dump(MEMREGION *memregion, char *s,
 		    ulong off, ulong len, struct seq_file *seq);
-void *visor_memregion_get_pointer(MEMREGION *memregion);
+void __iomem *visor_memregion_get_pointer(MEMREGION *memregion);
 
 #endif
diff --git a/drivers/staging/unisys/visorutil/memregion_direct.c b/drivers/staging/unisys/visorutil/memregion_direct.c
index 2c1061d55ee..28dfba0490f 100644
--- a/drivers/staging/unisys/visorutil/memregion_direct.c
+++ b/drivers/staging/unisys/visorutil/memregion_direct.c
@@ -1,6 +1,6 @@
 /* memregion_direct.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -29,7 +29,7 @@
 struct MEMREGION_Tag {
 	HOSTADDRESS physaddr;
 	ulong nbytes;
-	void *mapped;
+	void __iomem *mapped;
 	BOOL requested;
 	BOOL overlapped;
 };
@@ -93,7 +93,7 @@ visor_memregion_create_overlapped(MEMREGION *parent, ulong offset, ulong nbytes)
 
 	memregion->physaddr = parent->physaddr + offset;
 	memregion->nbytes = nbytes;
-	memregion->mapped = ((u8 *) (parent->mapped)) + offset;
+	memregion->mapped = ((u8 __iomem *) (parent->mapped)) + offset;
 	memregion->requested = FALSE;
 	memregion->overlapped = TRUE;
 	return memregion;
@@ -149,7 +149,7 @@ visor_memregion_get_nbytes(MEMREGION *memregion)
 }
 EXPORT_SYMBOL_GPL(visor_memregion_get_nbytes);
 
-void *
+void __iomem *
 visor_memregion_get_pointer(MEMREGION *memregion)
 {
 	return memregion->mapped;
diff --git a/drivers/staging/unisys/visorutil/periodic_work.c b/drivers/staging/unisys/visorutil/periodic_work.c
index 0670a317468..38a60ce8c27 100644
--- a/drivers/staging/unisys/visorutil/periodic_work.c
+++ b/drivers/staging/unisys/visorutil/periodic_work.c
@@ -1,6 +1,6 @@
 /* periodic_work.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -92,6 +92,7 @@ EXPORT_SYMBOL_GPL(visor_periodic_work_destroy);
 BOOL visor_periodic_work_nextperiod(PERIODIC_WORK *periodic_work)
 {
 	BOOL rc = FALSE;
+
 	write_lock(&periodic_work->lock);
 	if (periodic_work->want_to_stop) {
 		periodic_work->is_scheduled = FALSE;
diff --git a/drivers/staging/unisys/visorutil/procobjecttree.c b/drivers/staging/unisys/visorutil/procobjecttree.c
index 67a19e1c7b0..5c8c95c5151 100644
--- a/drivers/staging/unisys/visorutil/procobjecttree.c
+++ b/drivers/staging/unisys/visorutil/procobjecttree.c
@@ -1,6 +1,6 @@
 /* procobjecttree.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -95,6 +95,7 @@ static struct proc_dir_entry *
 createProcDir(const char *name, struct proc_dir_entry *parent)
 {
 	struct proc_dir_entry *p = proc_mkdir_mode(name, S_IFDIR, parent);
+
 	if (p == NULL)
 		ERRDRV("failed to create /proc directory %s", name);
 	return p;
@@ -197,9 +198,11 @@ void visor_proc_DestroyType(MYPROCTYPE *type)
 		return;
 	if (type->procDirs != NULL) {
 		int i = type->nNames-1;
+
 		while (i >= 0) {
 			if (type->procDirs[i] != NULL) {
 				struct proc_dir_entry *parent = NULL;
+
 				if (i == 0)
 					parent = type->procDirRoot;
 				else
@@ -299,6 +302,7 @@ EXPORT_SYMBOL_GPL(visor_proc_CreateObject);
 void visor_proc_DestroyObject(MYPROCOBJECT *obj)
 {
 	MYPROCTYPE *type = NULL;
+
 	if (obj == NULL)
 		return;
 	type = obj->type;
@@ -306,6 +310,7 @@ void visor_proc_DestroyObject(MYPROCOBJECT *obj)
 		return;
 	if (obj->procDirProperties != NULL) {
 		int i = 0;
+
 		for (i = 0; i < type->nProperties; i++) {
 			if (obj->procDirProperties[i] != NULL) {
 				remove_proc_entry(type->propertyNames[i],
@@ -338,6 +343,7 @@ EXPORT_SYMBOL_GPL(visor_proc_DestroyObject);
 static int seq_show(struct seq_file *seq, void *offset)
 {
 	PROCDIRENTRYCONTEXT *ctx = (PROCDIRENTRYCONTEXT *)(seq->private);
+
 	if (ctx == NULL) {
 		ERRDRV("I don't have a freakin' clue...");
 		return 0;
diff --git a/drivers/staging/unisys/visorutil/visorkmodutils.c b/drivers/staging/unisys/visorutil/visorkmodutils.c
index a7d1e94ca3c..10d77cb6ee9 100644
--- a/drivers/staging/unisys/visorutil/visorkmodutils.c
+++ b/drivers/staging/unisys/visorutil/visorkmodutils.c
@@ -1,6 +1,6 @@
 /* timskmodutils.c
  *
- * Copyright © 2010 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -20,6 +20,22 @@
 
 #define MYDRVNAME "timskmodutils"
 
+/* s-Par uses the Intel processor's VT-X features to separate groups of
+ * processors into partitions. The firmware sets the hypervisor bit and
+ * reports an ID in the HV capabilities leaf so that the partition's OS
+ * knows s-Par is present and managing the processors.
+ */
+
+#define UNISYS_SPAR_LEAF_ID 0x40000000
+
+/* The s-Par leaf ID returns "UnisysSpar64" encoded across ebx, ecx, edx */
+#define UNISYS_SPAR_ID_EBX 0x73696e55
+#define UNISYS_SPAR_ID_ECX 0x70537379
+#define UNISYS_SPAR_ID_EDX 0x34367261
+
+int unisys_spar_platform;
+EXPORT_SYMBOL_GPL(unisys_spar_platform);
+
 /** Callers to interfaces that set __GFP_NORETRY flag below
  *  must check for a NULL (error) result as we are telling the
  *  kernel interface that it is okay to fail.
@@ -69,3 +85,42 @@ void visor_seq_file_done_buffer(struct seq_file *m)
 	kfree(m);
 }
 EXPORT_SYMBOL_GPL(visor_seq_file_done_buffer);
+
+static __init uint32_t
+visorutil_spar_detect(void)
+{
+	unsigned int eax, ebx, ecx, edx;
+
+	if (cpu_has_hypervisor) {
+		/* check the ID */
+		cpuid(UNISYS_SPAR_LEAF_ID, &eax, &ebx, &ecx, &edx);
+		return  (ebx == UNISYS_SPAR_ID_EBX) &&
+			(ecx == UNISYS_SPAR_ID_ECX) &&
+			(edx == UNISYS_SPAR_ID_EDX);
+	} else
+		return 0;
+
+}
+
+
+
+
+static __init int
+visorutil_mod_init(void)
+{
+	if (visorutil_spar_detect()) {
+		unisys_spar_platform = TRUE;
+		return 0;
+	} else
+		return -ENODEV;
+}
+
+static __exit void
+visorutil_mod_exit(void)
+{
+}
+
+module_init(visorutil_mod_init);
+module_exit(visorutil_mod_exit);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/usbip/stub_dev.c b/drivers/staging/usbip/stub_dev.c
index de692d7011a..51d0c718873 100644
--- a/drivers/staging/usbip/stub_dev.c
+++ b/drivers/staging/usbip/stub_dev.c
@@ -99,6 +99,7 @@ static ssize_t store_sockfd(struct device *dev, struct device_attribute *attr,
 
 	if (sockfd != -1) {
 		int err;
+
 		dev_info(dev, "stub up\n");
 
 		spin_lock_irq(&sdev->ud.lock);
diff --git a/drivers/staging/usbip/stub_tx.c b/drivers/staging/usbip/stub_tx.c
index 1622563823a..dbcabc9dbe0 100644
--- a/drivers/staging/usbip/stub_tx.c
+++ b/drivers/staging/usbip/stub_tx.c
@@ -179,7 +179,7 @@ static int stub_send_ret_submit(struct stub_device *sdev)
 		else
 			iovnum = 2;
 
-		iov = kzalloc(iovnum * sizeof(struct kvec), GFP_KERNEL);
+		iov = kcalloc(iovnum, sizeof(struct kvec), GFP_KERNEL);
 
 		if (!iov) {
 			usbip_event_add(&sdev->ud, SDEV_EVENT_ERROR_MALLOC);
@@ -218,6 +218,7 @@ static int stub_send_ret_submit(struct stub_device *sdev)
 			 */
 
 			int i;
+
 			for (i = 0; i < urb->number_of_packets; i++) {
 				iov[iovnum].iov_base = urb->transfer_buffer +
 					urb->iso_frame_desc[i].offset;
diff --git a/drivers/staging/usbip/usbip_common.h b/drivers/staging/usbip/usbip_common.h
index f555d834f13..4da3866a037 100644
--- a/drivers/staging/usbip/usbip_common.h
+++ b/drivers/staging/usbip/usbip_common.h
@@ -321,12 +321,14 @@ int usbip_event_happened(struct usbip_device *ud);
 static inline int interface_to_busnum(struct usb_interface *interface)
 {
 	struct usb_device *udev = interface_to_usbdev(interface);
+
 	return udev->bus->busnum;
 }
 
 static inline int interface_to_devnum(struct usb_interface *interface)
 {
 	struct usb_device *udev = interface_to_usbdev(interface);
+
 	return udev->devnum;
 }
 
diff --git a/drivers/staging/usbip/userspace/README b/drivers/staging/usbip/userspace/README
index f528ba462b5..831f49fea3c 100644
--- a/drivers/staging/usbip/userspace/README
+++ b/drivers/staging/usbip/userspace/README
@@ -195,7 +195,6 @@ Detach the imported device:
     - Shutdown firewall.
 	- usbip now uses TCP port 3240.
     - Disable SELinux.
-    - If possible, compile your kernel with CONFIG_USB_DEBUG flag and try again.
     - Check the kernel and daemon messages.
 
 
diff --git a/drivers/staging/usbip/userspace/libsrc/usbip_common.c b/drivers/staging/usbip/userspace/libsrc/usbip_common.c
index 238bf5bf7f4..ac73710473d 100644
--- a/drivers/staging/usbip/userspace/libsrc/usbip_common.c
+++ b/drivers/staging/usbip/userspace/libsrc/usbip_common.c
@@ -75,6 +75,7 @@ const char *usbip_speed_string(int num)
 void dump_usb_interface(struct usbip_usb_interface *uinf)
 {
 	char buff[100];
+
 	usbip_names_get_class(buff, sizeof(buff),
 			uinf->bInterfaceClass,
 			uinf->bInterfaceSubClass,
@@ -86,7 +87,6 @@ void dump_usb_device(struct usbip_usb_device *udev)
 {
 	char buff[100];
 
-
 	dbg("%-20s = %s", "path",  udev->path);
 	dbg("%-20s = %s", "busid", udev->busid);
 
@@ -237,7 +237,7 @@ int usbip_names_init(char *f)
 	return names_init(f);
 }
 
-void usbip_names_free()
+void usbip_names_free(void)
 {
 	names_free();
 }
diff --git a/drivers/staging/usbip/userspace/libsrc/usbip_common.h b/drivers/staging/usbip/userspace/libsrc/usbip_common.h
index 23be848f24f..5a0e95edf4d 100644
--- a/drivers/staging/usbip/userspace/libsrc/usbip_common.h
+++ b/drivers/staging/usbip/userspace/libsrc/usbip_common.h
@@ -47,7 +47,7 @@ extern int usbip_use_debug ;
 
 #define pr_fmt(fmt)	"%s: %s: " fmt "\n", PROGNAME
 #define dbg_fmt(fmt)	pr_fmt("%s:%d:[%s] " fmt), "debug",	\
-		        __FILE__, __LINE__, __FUNCTION__
+		        __FILE__, __LINE__, __func__
 
 #define err(fmt, args...)						\
 	do {								\
diff --git a/drivers/staging/usbip/userspace/libsrc/usbip_host_driver.c b/drivers/staging/usbip/userspace/libsrc/usbip_host_driver.c
index c5bf60b135b..92caef7474c 100644
--- a/drivers/staging/usbip/userspace/libsrc/usbip_host_driver.c
+++ b/drivers/staging/usbip/userspace/libsrc/usbip_host_driver.c
@@ -118,6 +118,7 @@ static int refresh_exported_devices(void)
 	struct udev_list_entry *devices, *dev_list_entry;
 	struct udev_device *dev;
 	const char *path;
+	const char *driver;
 
 	enumerate = udev_enumerate_new(udev_context);
 	udev_enumerate_add_match_subsystem(enumerate, "usb");
@@ -128,10 +129,12 @@ static int refresh_exported_devices(void)
 	udev_list_entry_foreach(dev_list_entry, devices) {
 		path = udev_list_entry_get_name(dev_list_entry);
 		dev = udev_device_new_from_syspath(udev_context, path);
+		if (dev == NULL)
+			continue;
 
 		/* Check whether device uses usbip-host driver. */
-		if (!strcmp(udev_device_get_driver(dev),
-			    USBIP_HOST_DRV_NAME)) {
+		driver = udev_device_get_driver(dev);
+		if (driver != NULL && !strcmp(driver, USBIP_HOST_DRV_NAME)) {
 			edev = usbip_exported_device_new(path);
 			if (!edev) {
 				dbg("usbip_exported_device_new failed");
diff --git a/drivers/staging/usbip/userspace/libsrc/vhci_driver.c b/drivers/staging/usbip/userspace/libsrc/vhci_driver.c
index 8901fcb7946..ad920477353 100644
--- a/drivers/staging/usbip/userspace/libsrc/vhci_driver.c
+++ b/drivers/staging/usbip/userspace/libsrc/vhci_driver.c
@@ -260,7 +260,7 @@ err:
 }
 
 
-void usbip_vhci_driver_close()
+void usbip_vhci_driver_close(void)
 {
 	if (!vhci_driver)
 		return;
diff --git a/drivers/staging/usbip/userspace/src/usbip_attach.c b/drivers/staging/usbip/userspace/src/usbip_attach.c
index 716a79e284c..d58a14dfc09 100644
--- a/drivers/staging/usbip/userspace/src/usbip_attach.c
+++ b/drivers/staging/usbip/userspace/src/usbip_attach.c
@@ -56,6 +56,7 @@ static int record_connection(char *host, char *port, char *busid, int rhport)
 		/* if VHCI_STATE_PATH exists, then it better be a directory */
 		if (errno == EEXIST) {
 			struct stat s;
+
 			ret = stat(VHCI_STATE_PATH, &s);
 			if (ret < 0)
 				return -1;
diff --git a/drivers/staging/usbip/userspace/src/usbip_port.c b/drivers/staging/usbip/userspace/src/usbip_port.c
index 52aa168b46b..a2e884fd922 100644
--- a/drivers/staging/usbip/userspace/src/usbip_port.c
+++ b/drivers/staging/usbip/userspace/src/usbip_port.c
@@ -16,7 +16,7 @@
 #include "vhci_driver.h"
 #include "usbip_common.h"
 
-static int list_imported_devices()
+static int list_imported_devices(void)
 {
 	int i;
 	struct usbip_imported_device *idev;
diff --git a/drivers/staging/usbip/userspace/src/usbipd.c b/drivers/staging/usbip/userspace/src/usbipd.c
index 2cae4ce48d1..2f87f2d348b 100644
--- a/drivers/staging/usbip/userspace/src/usbipd.c
+++ b/drivers/staging/usbip/userspace/src/usbipd.c
@@ -372,6 +372,7 @@ static int listen_all_addrinfo(struct addrinfo *ai_head, int sockfdlist[],
 
 	for (ai = ai_head; ai && nsockfd < maxsockfd; ai = ai->ai_next) {
 		int sock;
+
 		addrinfo_to_text(ai, ai_buf, ai_buf_size);
 		dbg("opening %s", ai_buf);
 		sock = socket(ai->ai_family, ai->ai_socktype, ai->ai_protocol);
@@ -457,11 +458,13 @@ static void set_signal(void)
 
 static const char *pid_file;
 
-static void write_pid_file()
+static void write_pid_file(void)
 {
 	if (pid_file) {
 		dbg("creating pid file %s", pid_file);
-		FILE *fp = fopen(pid_file, "w");
+		FILE *fp;
+
+		fp = fopen(pid_file, "w");
 		if (!fp) {
 			err("pid_file: %s: %d (%s)",
 			    pid_file, errno, strerror(errno));
@@ -472,7 +475,7 @@ static void write_pid_file()
 	}
 }
 
-static void remove_pid_file()
+static void remove_pid_file(void)
 {
 	if (pid_file) {
 		dbg("removing pid file %s", pid_file);
@@ -602,6 +605,7 @@ int main(int argc, char *argv[])
 	int daemonize = 0;
 	int ipv4 = 0, ipv6 = 0;
 	int opt, rc = -1;
+
 	pid_file = NULL;
 
 	usbip_use_stderr = 1;
diff --git a/drivers/staging/usbip/vhci_hcd.c b/drivers/staging/usbip/vhci_hcd.c
index 70e17551943..0007d30e45b 100644
--- a/drivers/staging/usbip/vhci_hcd.c
+++ b/drivers/staging/usbip/vhci_hcd.c
@@ -635,6 +635,7 @@ static int vhci_urb_dequeue(struct usb_hcd *hcd, struct urb *urb, int status)
 
 	{
 		int ret = 0;
+
 		ret = usb_hcd_check_unlink_urb(hcd, urb, status);
 		if (ret) {
 			spin_unlock(&the_controller->lock);
@@ -885,6 +886,7 @@ static int vhci_start(struct usb_hcd *hcd)
 
 	for (rhport = 0; rhport < VHCI_NPORTS; rhport++) {
 		struct vhci_device *vdev = &vhci->vdev[rhport];
+
 		vhci_device_init(vdev);
 		vdev->rhport = rhport;
 	}
diff --git a/drivers/staging/usbip/vhci_sysfs.c b/drivers/staging/usbip/vhci_sysfs.c
index 47bddcdde0a..211f43f67ea 100644
--- a/drivers/staging/usbip/vhci_sysfs.c
+++ b/drivers/staging/usbip/vhci_sysfs.c
@@ -184,7 +184,7 @@ static ssize_t store_attach(struct device *dev, struct device_attribute *attr,
 	 * @devid: unique device identifier in a remote host
 	 * @speed: usb device speed in a remote host
 	 */
-	if (sscanf(buf, "%u %u %u %u", &rhport, &sockfd, &devid, &speed) != 1)
+	if (sscanf(buf, "%u %u %u %u", &rhport, &sockfd, &devid, &speed) != 4)
 		return -EINVAL;
 
 	usbip_dbg_vhci_sysfs("rhport(%u) sockfd(%u) devid(%u) speed(%u)\n",
diff --git a/drivers/staging/vme/devices/vme_user.c b/drivers/staging/vme/devices/vme_user.c
index 79279271567..2d8497277aa 100644
--- a/drivers/staging/vme/devices/vme_user.c
+++ b/drivers/staging/vme/devices/vme_user.c
@@ -684,7 +684,7 @@ static int vme_user_match(struct vme_dev *vdev)
 static int vme_user_probe(struct vme_dev *vdev)
 {
 	int i, err;
-	char name[12];
+	char *name;
 
 	/* Save pointer to the bridge device */
 	if (vme_user_bridge != NULL) {
@@ -776,7 +776,8 @@ static int vme_user_probe(struct vme_dev *vdev)
 		image[i].kern_buf = kmalloc(image[i].size_buf, GFP_KERNEL);
 		if (image[i].kern_buf == NULL) {
 			err = -ENOMEM;
-			goto err_master_buf;
+			vme_master_free(image[i].resource);
+			goto err_master;
 		}
 	}
 
@@ -791,15 +792,16 @@ static int vme_user_probe(struct vme_dev *vdev)
 	/* Add sysfs Entries */
 	for (i = 0; i < VME_DEVS; i++) {
 		int num;
+
 		switch (type[i]) {
 		case MASTER_MINOR:
-			sprintf(name, "bus/vme/m%%d");
+			name = "bus/vme/m%d";
 			break;
 		case CONTROL_MINOR:
-			sprintf(name, "bus/vme/ctl");
+			name = "bus/vme/ctl";
 			break;
 		case SLAVE_MINOR:
-			sprintf(name, "bus/vme/s%%d");
+			name = "bus/vme/s%d";
 			break;
 		default:
 			err = -EINVAL;
@@ -819,8 +821,6 @@ static int vme_user_probe(struct vme_dev *vdev)
 
 	return 0;
 
-	/* Ensure counter set correcty to destroy all sysfs devices */
-	i = VME_DEVS;
 err_sysfs:
 	while (i > 0) {
 		i--;
@@ -830,12 +830,10 @@ err_sysfs:
 
 	/* Ensure counter set correcty to unalloc all master windows */
 	i = MASTER_MAX + 1;
-err_master_buf:
-	for (i = MASTER_MINOR; i < (MASTER_MAX + 1); i++)
-		kfree(image[i].kern_buf);
 err_master:
 	while (i > MASTER_MINOR) {
 		i--;
+		kfree(image[i].kern_buf);
 		vme_master_free(image[i].resource);
 	}
 
diff --git a/drivers/staging/vt6655/IEEE11h.c b/drivers/staging/vt6655/IEEE11h.c
index dfda3c82906..6cfad1c0402 100644
--- a/drivers/staging/vt6655/IEEE11h.c
+++ b/drivers/staging/vt6655/IEEE11h.c
@@ -192,8 +192,6 @@ static bool s_bRxTPCReq(PSMgmtObject pMgmt,
 	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)); */
 }
 
 /*---------------------  Export Variables  --------------------------*/
@@ -274,8 +272,6 @@ IEEE11hbMgrRxAction(void *pMgmtHandle, void *pRxPacket)
 			pAction->byCategory);
 		pAction->byCategory |= 0x80;
 
-		/*return CARDbSendPacket(pMgmt->pAdapter, pAction, PKT_TYPE_802_11_MNG,
-		  uLength);*/
 		return true;
 	}
 	return true;
@@ -317,6 +313,4 @@ bool IEEE11hbMSRRepTx(void *pMgmtHandle)
 	if (csMgmt_xmit(pMgmt->pAdapter, pTxPacket) != CMD_STATUS_PENDING)
 		return false;
 	return true;
-/*    return CARDbSendPacket(pMgmt->pAdapter, pMSRRep, PKT_TYPE_802_11_MNG,
-      uLength); */
 }
diff --git a/drivers/staging/vt6655/baseband.c b/drivers/staging/vt6655/baseband.c
index fa14659ba43..6f95fb618dc 100644
--- a/drivers/staging/vt6655/baseband.c
+++ b/drivers/staging/vt6655/baseband.c
@@ -1095,7 +1095,6 @@ unsigned char byVT3253B0_AIROHA2230[CB_VT3253B0_INIT_FOR_AIROHA2230][2] = {
 	{0x69, 0x00},
 	{0x6a, 0x00},
 	{0x6b, 0x00},
-	//{0x6c, 0x80},
 	{0x6c, 0x00}, //RobertYu:20050125, request by JJSue
 	{0x6d, 0x03},
 	{0x6e, 0x01},
@@ -1357,7 +1356,6 @@ unsigned char byVT3253B0_UW2451[CB_VT3253B0_INIT_FOR_UW2451][2] = {
 	{0x69, 0x00},
 	{0x6a, 0x00},
 	{0x6b, 0x00},
-	//{0x6c, 0x80},
 	{0x6c, 0x00}, //RobertYu:20050125, request by JJSue
 	{0x6d, 0x03},
 	{0x6e, 0x01},
@@ -1786,29 +1784,27 @@ BBuGetFrameTime(
 	uRate = (unsigned int)awcFrameTime[uRateIdx];
 
 	if (uRateIdx <= 3) {          //CCK mode
-
-		if (byPreambleType == 1) {//Short
+		if (byPreambleType == 1) //Short
 			uPreamble = 96;
-		} else {
+		else
 			uPreamble = 192;
-		}
+
 		uFrameTime = (cbFrameLength * 80) / uRate;  //?????
 		uTmp = (uFrameTime * uRate) / 80;
-		if (cbFrameLength != uTmp) {
+		if (cbFrameLength != uTmp)
 			uFrameTime++;
-		}
 
 		return uPreamble + uFrameTime;
 	} else {
 		uFrameTime = (cbFrameLength * 8 + 22) / uRate;   //????????
 		uTmp = ((uFrameTime * uRate) - 22) / 8;
-		if (cbFrameLength != uTmp) {
+		if (cbFrameLength != uTmp)
 			uFrameTime++;
-		}
+
 		uFrameTime = uFrameTime * 4;    //???????
-		if (byPktType != PK_TYPE_11A) {
+		if (byPktType != PK_TYPE_11A)
 			uFrameTime += 6;     //??????
-		}
+
 		return 20 + uFrameTime; //??????
 	}
 }
@@ -2129,16 +2125,16 @@ bool BBbVT3253Init(PSDevice pDevice)
 
 	if (byRFType == RF_RFMD2959) {
 		if (byLocalID <= REV_ID_VT3253_A1) {
-			for (ii = 0; ii < CB_VT3253_INIT_FOR_RFMD; ii++) {
+			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++) {
+			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++) {
+
+			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);
 		}
@@ -2151,12 +2147,12 @@ bool BBbVT3253Init(PSDevice pDevice)
 		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++) {
+		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++) {
+
+		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;
@@ -2166,12 +2162,12 @@ bool BBbVT3253Init(PSDevice pDevice)
 		pDevice->ldBmThreshold[2] = 0;
 		pDevice->ldBmThreshold[3] = 0;
 	} else if (byRFType == RF_UW2451) {
-		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_UW2451; ii++) {
+		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++) {
+
+		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);
 
@@ -2184,9 +2180,9 @@ bool BBbVT3253Init(PSDevice pDevice)
 		pDevice->ldBmThreshold[2] = 0;
 		pDevice->ldBmThreshold[3] = 0;
 	} else if (byRFType == RF_UW2452) {
-		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_UW2451; ii++) {
+		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)
@@ -2205,11 +2201,8 @@ bool BBbVT3253Init(PSDevice pDevice)
 		//}}
 		bResult &= BBbWriteEmbedded(dwIoBase, 0xb0, 0x58);
 
-		for (ii = 0; ii < CB_VT3253B0_AGC; ii++) {
+		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;
@@ -2222,12 +2215,12 @@ bool BBbVT3253Init(PSDevice pDevice)
 		//}} RobertYu
 
 	} else if (byRFType == RF_VT3226) {
-		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_AIROHA2230; ii++) {
+		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++) {
+
+		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;
@@ -2240,9 +2233,9 @@ bool BBbVT3253Init(PSDevice pDevice)
 		MACvSetRFLE_LatchBase(dwIoBase);
 		//{{ RobertYu: 20050104
 	} else if (byRFType == RF_AIROHA7230) {
-		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_AIROHA2230; ii++) {
+		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)
@@ -2253,9 +2246,9 @@ bool BBbVT3253Init(PSDevice pDevice)
 		bResult &= BBbWriteEmbedded(dwIoBase, 0xd7, 0x06);
 		//}}
 
-		for (ii = 0; ii < CB_VT3253B0_AGC; ii++) {
+		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;
@@ -2411,17 +2404,15 @@ BBvSetShortSlotTime(PSDevice pDevice)
 
 	BBbReadEmbedded(pDevice->PortOffset, 0x0A, &byBBRxConf);//CR10
 
-	if (pDevice->bShortSlotTime) {
+	if (pDevice->bShortSlotTime)
 		byBBRxConf &= 0xDF;//1101 1111
-	} else {
+	else
 		byBBRxConf |= 0x20;//0010 0000
-	}
 
 	// patch for 3253B0 Baseband with Cardbus module
 	BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byBBVGA);
-	if (byBBVGA == pDevice->abyBBVGA[0]) {
+	if (byBBVGA == pDevice->abyBBVGA[0])
 		byBBRxConf |= 0x20;//0010 0000
-	}
 
 	BBbWriteEmbedded(pDevice->PortOffset, 0x0A, byBBRxConf);//CR10
 }
@@ -2613,13 +2604,11 @@ s_ulGetRatio(PSDevice pDevice)
 	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];
 	}
@@ -2627,7 +2616,6 @@ s_ulGetRatio(PSDevice pDevice)
 		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];
 	}
@@ -2635,7 +2623,6 @@ s_ulGetRatio(PSDevice pDevice)
 		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];
 	}
@@ -2644,7 +2631,6 @@ s_ulGetRatio(PSDevice pDevice)
 			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];
 	}
@@ -2653,7 +2639,6 @@ s_ulGetRatio(PSDevice pDevice)
 			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];
 	}
@@ -2662,7 +2647,6 @@ s_ulGetRatio(PSDevice pDevice)
 			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];
 	}
@@ -2671,7 +2655,6 @@ s_ulGetRatio(PSDevice pDevice)
 			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];
 	}
@@ -2679,7 +2662,6 @@ s_ulGetRatio(PSDevice pDevice)
 		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];
 	}
@@ -2687,21 +2669,18 @@ s_ulGetRatio(PSDevice pDevice)
 		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;
 	}
 
@@ -2714,9 +2693,8 @@ BBvClearAntDivSQ3Value(PSDevice pDevice)
 	unsigned int ii;
 
 	pDevice->uDiversityCnt = 0;
-	for (ii = 0; ii < MAX_RATE; ii++) {
+	for (ii = 0; ii < MAX_RATE; ii++)
 		pDevice->uNumSQ3[ii] = 0;
-	}
 }
 
 /*
@@ -2737,11 +2715,10 @@ BBvClearAntDivSQ3Value(PSDevice pDevice)
 void
 BBvAntennaDiversity(PSDevice pDevice, unsigned char byRxRate, unsigned char bySQ3)
 {
-	if ((byRxRate >= MAX_RATE) || (pDevice->wAntDiversityMaxRate >= MAX_RATE)) {
+	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]++;
 
diff --git a/drivers/staging/vt6655/bssdb.c b/drivers/staging/vt6655/bssdb.c
index d7efd0173a9..59679cd4681 100644
--- a/drivers/staging/vt6655/bssdb.c
+++ b/drivers/staging/vt6655/bssdb.c
@@ -497,9 +497,8 @@ BSSbInsertToBSSList(
 		}
 	}
 
-	if (bParsingQuiet && (pQuiet != NULL)) {
+	if (bParsingQuiet && (pQuiet != NULL))
 		CARDbStartQuiet(pMgmt->pAdapter);
-	}
 
 	pBSSList->uIELength = uIELength;
 	if (pBSSList->uIELength > WLAN_BEACON_FR_MAXLEN)
@@ -662,9 +661,8 @@ BSSbUpdateToBSSList(
 		}
 	}
 
-	if (bParsingQuiet && (pQuiet != NULL)) {
+	if (bParsingQuiet && (pQuiet != NULL))
 		CARDbStartQuiet(pMgmt->pAdapter);
-	}
 
 	pBSSList->uIELength = uIELength;
 	if (pBSSList->uIELength > WLAN_BEACON_FR_MAXLEN)
diff --git a/drivers/staging/vt6655/bssdb.h b/drivers/staging/vt6655/bssdb.h
index 5c77677251e..a0938b7dd08 100644
--- a/drivers/staging/vt6655/bssdb.h
+++ b/drivers/staging/vt6655/bssdb.h
@@ -112,7 +112,6 @@ typedef struct tagKnownBSS {
 	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];
@@ -147,7 +146,6 @@ typedef struct tagKnownBSS {
 
 	// Clear count
 	unsigned int	uClearCount;
-//    unsigned char abyIEs[WLAN_BEACON_FR_MAXLEN];
 	unsigned int	uIELength;
 	QWORD           qwBSSTimestamp;
 	QWORD           qwLocalTSF;     // local TSF timer
diff --git a/drivers/staging/vt6655/card.c b/drivers/staging/vt6655/card.c
index db38ca05113..05bf48a24f4 100644
--- a/drivers/staging/vt6655/card.c
+++ b/drivers/staging/vt6655/card.c
@@ -24,7 +24,6 @@
  *      vUpdateIFS - Update slotTime,SIFS,DIFS, and EIFS
  *      CARDvUpdateBasicTopRate - Update BasicTopRate
  *      CARDbAddBasicRate - Add to BasicRateSet
- *      CARDbSetBasicRate - Set Basic Tx Rate
  *      CARDbIsOFDMinBasicRate - Check if any OFDM rate is in BasicRateSet
  *      CARDvSetLoopbackMode - Set Loopback mode
  *      CARDbSoftwareReset - Sortware reset NIC
@@ -60,7 +59,6 @@
 
 /*---------------------  Static Definitions -------------------------*/
 
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 static int msglevel = MSG_LEVEL_INFO;
 
 #define C_SIFS_A        16      // micro sec.
@@ -339,36 +337,6 @@ s_vSetRSPINF(PSDevice pDevice, CARD_PHY_TYPE ePHYType, void *pvSupportRateIEs, v
 /*---------------------  Export Functions  --------------------------*/
 
 /*
- * Description: Card Send packet function
- *
- * Parameters:
- *  In:
- *      pDeviceHandler      - The adapter to be set
- *      pPacket             - Packet buffer pointer
- *      ePktType            - Packet type
- *      uLength             - Packet length
- *  Out:
- *      none
- *
- * Return Value: true if succeeded; false if failed.
- *
- */
-/*
-  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
  *
  * Parameters:
@@ -383,9 +351,9 @@ s_vSetRSPINF(PSDevice pDevice, CARD_PHY_TYPE ePHYType, void *pvSupportRateIEs, v
 bool CARDbIsShortPreamble(void *pDeviceHandler)
 {
 	PSDevice    pDevice = (PSDevice) pDeviceHandler;
-	if (pDevice->byPreambleType == 0) {
+	if (pDevice->byPreambleType == 0)
 		return false;
-	}
+
 	return true;
 }
 
@@ -427,15 +395,14 @@ bool CARDbSetPhyParameter(void *pDeviceHandler, CARD_PHY_TYPE ePHYType, unsigned
 	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) {
+		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);
@@ -443,9 +410,9 @@ bool CARDbSetPhyParameter(void *pDeviceHandler, CARD_PHY_TYPE ePHYType, unsigned
 			pDevice->abyBBVGA[2] = 0x10;
 			pDevice->abyBBVGA[3] = 0x10;
 			BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
-			if (byData == 0x1C) {
+			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;
@@ -463,18 +430,18 @@ bool CARDbSetPhyParameter(void *pDeviceHandler, CARD_PHY_TYPE ePHYType, unsigned
 		byDIFS = C_SIFS_A + 2*C_SLOT_SHORT;
 		byCWMaxMin = 0xA4;
 	} else if (ePHYType == PHY_TYPE_11B) {
-		if (pSupportRates == NULL) {
+		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) {
+			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);
@@ -499,9 +466,9 @@ bool CARDbSetPhyParameter(void *pDeviceHandler, CARD_PHY_TYPE ePHYType, unsigned
 			pDevice->abyBBVGA[2] = 0x00;
 			pDevice->abyBBVGA[3] = 0x00;
 			BBbReadEmbedded(pDevice->PortOffset, 0xE7, &byData);
-			if (byData == 0x20) {
+			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);
@@ -519,26 +486,25 @@ bool CARDbSetPhyParameter(void *pDeviceHandler, CARD_PHY_TYPE ePHYType, unsigned
 			bySlot = C_SLOT_LONG;
 			byDIFS = C_SIFS_BG + 2*C_SLOT_LONG;
 		}
-		if (VNTWIFIbyGetMaxSupportRate(pSupportRates, pExtSupportRates) > RATE_11M) {
+		if (VNTWIFIbyGetMaxSupportRate(pSupportRates, pExtSupportRates) > RATE_11M)
 			byCWMaxMin = 0xA4;
-		} else {
+		else
 			byCWMaxMin = 0xA5;
-		}
+
 		if (pDevice->bProtectMode != VNTWIFIbIsProtectMode(byERPField)) {
 			pDevice->bProtectMode = VNTWIFIbIsProtectMode(byERPField);
-			if (pDevice->bProtectMode) {
+			if (pDevice->bProtectMode)
 				MACvEnableProtectMD(pDevice->PortOffset);
-			} else {
+			else
 				MACvDisableProtectMD(pDevice->PortOffset);
-			}
+
 		}
 		if (pDevice->bBarkerPreambleMd != VNTWIFIbIsBarkerMode(byERPField)) {
 			pDevice->bBarkerPreambleMd = VNTWIFIbIsBarkerMode(byERPField);
-			if (pDevice->bBarkerPreambleMd) {
+			if (pDevice->bBarkerPreambleMd)
 				MACvEnableBarkerPreambleMd(pDevice->PortOffset);
-			} else {
+			else
 				MACvDisableBarkerPreambleMd(pDevice->PortOffset);
-			}
 		}
 	}
 
@@ -567,22 +533,22 @@ bool CARDbSetPhyParameter(void *pDeviceHandler, CARD_PHY_TYPE ePHYType, unsigned
 	if (pDevice->bySlot != bySlot) {
 		pDevice->bySlot = bySlot;
 		VNSvOutPortB(pDevice->PortOffset + MAC_REG_SLOT, pDevice->bySlot);
-		if (pDevice->bySlot == C_SLOT_SHORT) {
+		if (pDevice->bySlot == C_SLOT_SHORT)
 			pDevice->bShortSlotTime = true;
-		} else {
+		else
 			pDevice->bShortSlotTime = false;
-		}
+
 		BBvSetShortSlotTime(pDevice);
 	}
 	if (pDevice->byCWMaxMin != byCWMaxMin) {
 		pDevice->byCWMaxMin = byCWMaxMin;
 		VNSvOutPortB(pDevice->PortOffset + MAC_REG_CWMAXMIN0, pDevice->byCWMaxMin);
 	}
-	if (VNTWIFIbIsShortPreamble(wCapInfo)) {
+	if (VNTWIFIbIsShortPreamble(wCapInfo))
 		pDevice->byPreambleType = pDevice->byShortPreamble;
-	} else {
+	else
 		pDevice->byPreambleType = 0;
-	}
+
 	s_vSetRSPINF(pDevice, ePHYType, pSupportRates, pExtSupportRates);
 	pDevice->eCurrentPHYType = ePHYType;
 	// set for NDIS OID_802_11SUPPORTED_RATES
@@ -662,9 +628,9 @@ bool CARDbSetBeaconPeriod(void *pDeviceHandler, unsigned short wBeaconInterval)
 	uLowRemain = uBeaconInterval - uLowRemain;
 
 	// check if carry when add one beacon interval
-	if ((~uLowNextTBTT) < uLowRemain) {
+	if ((~uLowNextTBTT) < uLowRemain)
 		HIDWORD(qwNextTBTT)++;
-	}
+
 	LODWORD(qwNextTBTT) = uLowNextTBTT + uLowRemain;
 
 	// set HW beacon interval
@@ -720,15 +686,13 @@ bool CARDbStopTxPacket(void *pDeviceHandler, CARD_PKT_TYPE ePktType)
 	}
 	// wait all TD0 complete
 	if (pDevice->bStopTx0Pkt == true) {
-		if (pDevice->iTDUsed[TYPE_TXDMA0] != 0) {
+		if (pDevice->iTDUsed[TYPE_TXDMA0] != 0)
 			return false;
-		}
 	}
 	// wait all Data TD complete
 	if (pDevice->bStopDataPkt == true) {
-		if (pDevice->iTDUsed[TYPE_AC0DMA] != 0) {
+		if (pDevice->iTDUsed[TYPE_AC0DMA] != 0)
 			return false;
-		}
 	}
 
 	return true;
@@ -792,16 +756,16 @@ bool CARDbSetBSSID(void *pDeviceHandler, unsigned char *pbyBSSID, CARD_OP_MODE e
 
 	MACvWriteBSSIDAddress(pDevice->PortOffset, pbyBSSID);
 	memcpy(pDevice->abyBSSID, pbyBSSID, WLAN_BSSID_LEN);
-	if (eOPMode == OP_MODE_ADHOC) {
+	if (eOPMode == OP_MODE_ADHOC)
 		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_HOSTCR, HOSTCR_ADHOC);
-	} else {
+	else
 		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_HOSTCR, HOSTCR_ADHOC);
-	}
-	if (eOPMode == OP_MODE_AP) {
+
+	if (eOPMode == OP_MODE_AP)
 		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_HOSTCR, HOSTCR_AP);
-	} else {
+	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;
@@ -1044,31 +1008,31 @@ CARDbAdd_PMKID_Candidate(
 		memset(&pDevice->gsPMKIDCandidate, 0, sizeof(SPMKIDCandidateEvent));
 	}
 
-	for (ii = 0; ii < 6; ii++) {
+	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 && (wRSNCap & BIT0)) {
+			if (bRSNCapExist && (wRSNCap & BIT0))
 				pCandidateList->Flags |= NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED;
-			} else {
+			else
 				pCandidateList->Flags &= ~(NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED);
-			}
+
 			return true;
 		}
 	}
 
 	// New Candidate
 	pCandidateList = &pDevice->gsPMKIDCandidate.CandidateList[pDevice->gsPMKIDCandidate.NumCandidates];
-	if (bRSNCapExist && (wRSNCap & BIT0)) {
+	if (bRSNCapExist && (wRSNCap & BIT0))
 		pCandidateList->Flags |= NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED;
-	} else {
+	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);
@@ -1149,16 +1113,16 @@ CARDbStartMeasure(
 				// start immediately by setting start TSF == current TSF + 2 TU
 				LODWORD(qwStartTSF) = LODWORD(qwCurrTSF) + 2048;
 				HIDWORD(qwStartTSF) = HIDWORD(qwCurrTSF);
-				if (LODWORD(qwCurrTSF) > LODWORD(qwStartTSF)) {
+				if (LODWORD(qwCurrTSF) > LODWORD(qwStartTSF))
 					HIDWORD(qwStartTSF)++;
-				}
+
 				bExpired = false;
 				break;
 			} else {
 				// start at setting start TSF - 1TU(for channel switching)
-				if (LODWORD(qwStartTSF) < 1024) {
+				if (LODWORD(qwStartTSF) < 1024)
 					HIDWORD(qwStartTSF)--;
-				}
+
 				LODWORD(qwStartTSF) -= 1024;
 			}
 
@@ -1247,9 +1211,9 @@ CARDbChannelSwitch(
 	pDevice->byChannelSwitchCount = byCount;
 	pDevice->byNewChannel = byNewChannel;
 	pDevice->bChannelSwitch = true;
-	if (byMode == 1) {
+	if (byMode == 1)
 		bResult = CARDbStopTxPacket(pDevice, PKT_TYPE_802_11_ALL);
-	}
+
 	return bResult;
 }
 
@@ -1282,9 +1246,9 @@ CARDbSetQuiet(
 
 	if (bResetQuiet) {
 		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_MSRCTL, (MSRCTL_QUIETTXCHK | MSRCTL_QUIETEN));
-		for (ii = 0; ii < MAX_QUIET_COUNT; ii++) {
+		for (ii = 0; ii < MAX_QUIET_COUNT; ii++)
 			pDevice->sQuiet[ii].bEnable = false;
-		}
+
 		pDevice->uQuietEnqueue = 0;
 		pDevice->bEnableFirstQuiet = false;
 		pDevice->bQuietEnable = false;
@@ -1299,11 +1263,8 @@ CARDbSetQuiet(
 		pDevice->sQuiet[pDevice->uQuietEnqueue].dwStartTime += wQuietOffset;
 		pDevice->uQuietEnqueue++;
 		pDevice->uQuietEnqueue %= MAX_QUIET_COUNT;
-		if (pDevice->byQuietStartCount < byQuietCount) {
+		if (pDevice->byQuietStartCount < byQuietCount)
 			pDevice->byQuietStartCount = byQuietCount;
-		}
-	} else {
-		// we can not handle Quiet EID more
 	}
 	return true;
 }
@@ -1399,9 +1360,9 @@ CARDbStartQuiet(
 		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) {
+				if (pDevice->sQuiet[ii].bEnable == true)
 					pDevice->sQuiet[ii].dwStartTime -= 0x80000000;
-				}
+
 			}
 			pDevice->dwCurrentQuietEndTime -= 0x80000000;
 		}
@@ -1433,13 +1394,13 @@ CARDvSetPowerConstraint(
 	PSDevice    pDevice = (PSDevice) pDeviceHandler;
 
 	if (byChannel > CB_MAX_CHANNEL_24G) {
-		if (pDevice->bCountryInfo5G == true) {
+		if (pDevice->bCountryInfo5G == true)
 			pDevice->abyLocalPwr[byChannel] = pDevice->abyRegPwr[byChannel] - byPower;
-		}
+
 	} else {
-		if (pDevice->bCountryInfo24G == true) {
+		if (pDevice->bCountryInfo24G == true)
 			pDevice->abyLocalPwr[byChannel] = pDevice->abyRegPwr[byChannel] - byPower;
-		}
+
 	}
 }
 
@@ -1618,9 +1579,9 @@ unsigned short CARDwGetCCKControlRate(void *pDeviceHandler, unsigned short wRate
 	unsigned int ui = (unsigned int) wRateIdx;
 
 	while (ui > RATE_1M) {
-		if (pDevice->wBasicRate & ((unsigned short)1 << ui)) {
+		if (pDevice->wBasicRate & ((unsigned short)1 << ui))
 			return (unsigned short)ui;
-		}
+
 		ui--;
 	}
 	return (unsigned short)RATE_1M;
@@ -1820,11 +1781,11 @@ void vUpdateIFS(void *pDeviceHandler)
 		byMaxMin = 5;
 	} else { // PK_TYPE_11GA & PK_TYPE_11GB
 		pDevice->uSIFS = C_SIFS_BG;
-		if (pDevice->bShortSlotTime) {
+		if (pDevice->bShortSlotTime)
 			pDevice->uSlot = C_SLOT_SHORT;
-		} else {
+		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;
@@ -1877,19 +1838,6 @@ void CARDvUpdateBasicTopRate(void *pDeviceHandler)
 	pDevice->byTopCCKBasicRate = byTopCCK;
 }
 
-/*
- * Description: Set NIC Tx Basic Rate
- *
- * Parameters:
- *  In:
- *      pDevice         - The adapter to be set
- *      wBasicRate      - Basic Rate to be set
- *  Out:
- *      none
- *
- * Return Value: true if succeeded; false if failed.
- *
- */
 bool CARDbAddBasicRate(void *pDeviceHandler, unsigned short wRateIdx)
 {
 	PSDevice pDevice = (PSDevice) pDeviceHandler;
@@ -1919,13 +1867,12 @@ unsigned char CARDbyGetPktType(void *pDeviceHandler)
 {
 	PSDevice pDevice = (PSDevice) pDeviceHandler;
 
-	if (pDevice->byBBType == BB_TYPE_11A || pDevice->byBBType == BB_TYPE_11B) {
+	if (pDevice->byBBType == BB_TYPE_11A || pDevice->byBBType == BB_TYPE_11B)
 		return (unsigned char)pDevice->byBBType;
-	} else if (CARDbIsOFDMinBasicRate((void *)pDevice)) {
+	else if (CARDbIsOFDMinBasicRate((void *)pDevice))
 		return PK_TYPE_11GA;
-	} else {
+	else
 		return PK_TYPE_11GB;
-	}
 }
 
 /*
@@ -2004,9 +1951,9 @@ QWORD CARDqGetTSFOffset(unsigned char byRxRate, QWORD qwTSF1, QWORD qwTSF2)
 	LODWORD(qwTSFOffset) = 0;
 	wRxBcnTSFOffst = cwRXBCNTSFOff[byRxRate%MAX_RATE];
 	(qwTSF2).u.dwLowDword += (unsigned long)(wRxBcnTSFOffst);
-	if ((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
@@ -2074,8 +2021,6 @@ QWORD CARDqGetNextTBTT(QWORD qwTSF, unsigned short wBeaconInterval)
 	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;
@@ -2117,7 +2062,7 @@ void CARDvSetFirstNextTBTT(unsigned long dwIoBase, unsigned short wBeaconInterva
 	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;
 }
 
diff --git a/drivers/staging/vt6655/card.h b/drivers/staging/vt6655/card.h
index ac6e2b4d0fd..829be92838b 100644
--- a/drivers/staging/vt6655/card.h
+++ b/drivers/staging/vt6655/card.h
@@ -104,7 +104,6 @@ void CARDvSafeResetRx(void *pDeviceHandler);
 //xxx
 bool CARDbRadioPowerOff(void *pDeviceHandler);
 bool CARDbRadioPowerOn(void *pDeviceHandler);
-//bool CARDbSendPacket(void *pDeviceHandler, void *pPacket, CARD_PKT_TYPE ePktType, unsigned int uLength);
 bool CARDbIsShortPreamble(void *pDeviceHandler);
 bool CARDbIsShorSlotTime(void *pDeviceHandler);
 bool CARDbSetPhyParameter(void *pDeviceHandler, CARD_PHY_TYPE ePHYType, unsigned short wCapInfo, unsigned char byERPField, void *pvSupportRateIEs, void *pvExtSupportRateIEs);
diff --git a/drivers/staging/vt6655/channel.c b/drivers/staging/vt6655/channel.c
index 3198a31e2ed..d5b89b7aaf2 100644
--- a/drivers/staging/vt6655/channel.c
+++ b/drivers/staging/vt6655/channel.c
@@ -30,7 +30,6 @@
 
 #define CARD_MAX_CHANNEL_TBL    56
 
-//static int msglevel = MSG_LEVEL_DEBUG;
 static int msglevel = MSG_LEVEL_INFO;
 
 /*---------------------  Static Variables  --------------------------*/
@@ -421,9 +420,8 @@ 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:
@@ -509,7 +507,6 @@ 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;
 }
 
@@ -527,13 +524,11 @@ bool set_channel(void *pDeviceHandler, unsigned int uConnectionChannel)
 	PSDevice pDevice = (PSDevice) pDeviceHandler;
 	bool bResult = true;
 
-	if (pDevice->byCurrentCh == uConnectionChannel) {
+	if (pDevice->byCurrentCh == uConnectionChannel)
 		return bResult;
-	}
 
-	if (!sChannelTbl[uConnectionChannel].bValid) {
+	if (!sChannelTbl[uConnectionChannel].bValid)
 		return false;
-	}
 
 	if ((uConnectionChannel > CB_MAX_CHANNEL_24G) &&
 	    (pDevice->eCurrentPHYType != PHY_TYPE_11A)) {
@@ -548,9 +543,8 @@ 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;
@@ -560,7 +554,6 @@ bool set_channel(void *pDeviceHandler, unsigned int uConnectionChannel)
 	if (pDevice->bEnablePSMode)
 		RFvWriteWakeProgSyn(pDevice->PortOffset, pDevice->byRFType, uConnectionChannel);
 
-	//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "CARDbSetMediaChannel: %d\n", (unsigned char)uConnectionChannel);
 	BBvSoftwareReset(pDevice->PortOffset);
 
 	if (pDevice->byLocalID > REV_ID_VT3253_B1) {
@@ -573,11 +566,10 @@ bool set_channel(void *pDeviceHandler, unsigned int uConnectionChannel)
 		MACvSelectPage0(pDevice->PortOffset);
 	}
 
-	if (pDevice->eCurrentPHYType == PHY_TYPE_11B) {
+	if (pDevice->eCurrentPHYType == PHY_TYPE_11B)
 		RFbSetPower(pDevice, RATE_1M, pDevice->byCurrentCh);
-	} else {
+	else
 		RFbSetPower(pDevice, RATE_6M, pDevice->byCurrentCh);
-	}
 
 	return bResult;
 }
@@ -604,15 +596,15 @@ void set_country_info(void *pDeviceHandler, CARD_PHY_TYPE ePHYType, void *pIE)
 
 	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;
 	}
 	pDevice->abyCountryCode[0] = pIE_Country->abyCountryString[0];
@@ -655,19 +647,19 @@ unsigned char set_support_channels(void *pDeviceHandler, unsigned char *pbyIEs)
 	byCount = 0;
 	if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[28] == true) {
 		for (ii = 28; ii < 36; ii += 2) {
-			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true) {
+			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true)
 				byCount++;
-			}
 		}
+
 		*pbyChTupple++ = 34;
 		*pbyChTupple++ = byCount;
 		byLen += 2;
 	} else if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[29] == true) {
 		for (ii = 29; ii < 36; ii += 2) {
-			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true) {
+			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true)
 				byCount++;
-			}
 		}
+
 		*pbyChTupple++ = 36;
 		*pbyChTupple++ = byCount;
 		byLen += 2;
@@ -676,10 +668,10 @@ unsigned char set_support_channels(void *pDeviceHandler, unsigned char *pbyIEs)
 	byCount = 0;
 	if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[36] == true) {
 		for (ii = 36; ii < 40; ii++) {
-			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true) {
+			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true)
 				byCount++;
-			}
 		}
+
 		*pbyChTupple++ = 52;
 		*pbyChTupple++ = byCount;
 		byLen += 2;
@@ -688,19 +680,19 @@ unsigned char set_support_channels(void *pDeviceHandler, unsigned char *pbyIEs)
 	byCount = 0;
 	if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[40] == true) {
 		for (ii = 40; ii < 51; ii++) {
-			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true) {
+			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true)
 				byCount++;
-			}
 		}
+
 		*pbyChTupple++ = 100;
 		*pbyChTupple++ = byCount;
 		byLen += 2;
 	} else if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[51] == true) {
 		for (ii = 51; ii < 56; ii++) {
-			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true) {
+			if (ChannelRuleTab[pDevice->byZoneType].bChannelIdxList[ii] == true)
 				byCount++;
-			}
 		}
+
 		*pbyChTupple++ = 149;
 		*pbyChTupple++ = byCount;
 		byLen += 2;
@@ -767,14 +759,13 @@ unsigned char auto_channel_select(void *pDeviceHandler, CARD_PHY_TYPE ePHYType)
 	if (ePHYType == PHY_TYPE_11A) {
 		for (ii = CB_MAX_CHANNEL_24G + 1; ii <= CB_MAX_CHANNEL; ii++) {
 			if (sChannelTbl[ii].bValid) {
-				if (byOptionChannel == 0) {
+				if (byOptionChannel == 0)
 					byOptionChannel = (unsigned char) ii;
-				}
-				if (sChannelTbl[ii].byMAP == 0) {
+
+				if (sChannelTbl[ii].byMAP == 0)
 					return (unsigned char) ii;
-				} else if (!(sChannelTbl[ii].byMAP & 0x08)) {
+				else if (!(sChannelTbl[ii].byMAP & 0x08))
 					byOptionChannel = (unsigned char) ii;
-				}
 			}
 		}
 	} else {
@@ -784,25 +775,24 @@ unsigned char auto_channel_select(void *pDeviceHandler, CARD_PHY_TYPE ePHYType)
 				if (sChannelTbl[ii].byMAP == 0) {
 					aiWeight[ii] += 100;
 				} else if (sChannelTbl[ii].byMAP & 0x01) {
-					if (ii > 3) {
+					if (ii > 3)
 						aiWeight[ii - 3] -= 10;
-					}
-					if (ii > 2) {
+
+					if (ii > 2)
 						aiWeight[ii - 2] -= 20;
-					}
-					if (ii > 1) {
+
+					if (ii > 1)
 						aiWeight[ii - 1] -= 40;
-					}
+
 					aiWeight[ii] -= 80;
-					if (ii < CB_MAX_CHANNEL_24G) {
+					if (ii < CB_MAX_CHANNEL_24G)
 						aiWeight[ii + 1] -= 40;
-					}
-					if (ii < (CB_MAX_CHANNEL_24G - 1)) {
+
+					if (ii < (CB_MAX_CHANNEL_24G - 1))
 						aiWeight[ii+2] -= 20;
-					}
-					if (ii < (CB_MAX_CHANNEL_24G - 2)) {
+
+					if (ii < (CB_MAX_CHANNEL_24G - 2))
 						aiWeight[ii+3] -= 10;
-					}
 				}
 			}
 		}
diff --git a/drivers/staging/vt6655/datarate.c b/drivers/staging/vt6655/datarate.c
index c9a89cd7633..f8420d65cd9 100644
--- a/drivers/staging/vt6655/datarate.c
+++ b/drivers/staging/vt6655/datarate.c
@@ -49,9 +49,8 @@
 
 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;
-const unsigned char acbyIERate[MAX_RATE] =
+static const unsigned char acbyIERate[MAX_RATE] =
 {0x02, 0x04, 0x0B, 0x16, 0x0C, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C};
 
 #define AUTORATE_TXOK_CNT       0x0400
@@ -251,13 +250,11 @@ RATEvParseMaxRate(
 			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)) {
+	if ((pDevice->byPacketType == PK_TYPE_11GB) && CARDbIsOFDMinBasicRate((void *)pDevice))
 		pDevice->byPacketType = PK_TYPE_11GA;
-	}
 
 	*pbyTopCCKRate = pDevice->byTopCCKBasicRate;
 	*pbyTopOFDMRate = pDevice->byTopOFDMBasicRate;
@@ -299,7 +296,6 @@ RATEvTxRateFallBack(
 	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};
 	unsigned long dwThroughputTbl[MAX_RATE] = {10, 20, 55, 110, 60, 90, 120, 180, 240, 360, 480, 540};
 	unsigned long dwThroughput = 0;
@@ -322,15 +318,14 @@ RATEvTxRateFallBack(
 		return;
 	}
 
-	if (psNodeDBTable->uTimeCount >= AUTORATE_TIMEOUT) {
+	if (psNodeDBTable->uTimeCount >= AUTORATE_TIMEOUT)
 		psNodeDBTable->uTimeCount = 0;
-	}
 
 	for (ii = 0; ii < MAX_RATE; ii++) {
 		if (psNodeDBTable->wSuppRate & (0x0001<<ii)) {
-			if (bAutoRate[ii]) {
+			if (bAutoRate[ii])
 				wIdxUpRate = (unsigned short) ii;
-			}
+
 		} else {
 			bAutoRate[ii] = false;
 		}
@@ -340,13 +335,11 @@ RATEvTxRateFallBack(
 		if ((psNodeDBTable->uTxOk[ii] != 0) ||
 		    (psNodeDBTable->uTxFail[ii] != 0)) {
 			dwThroughputTbl[ii] *= psNodeDBTable->uTxOk[ii];
-			if (ii < RATE_11M) {
+			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];
 
@@ -371,7 +364,6 @@ RATEvTxRateFallBack(
 //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;
 }
diff --git a/drivers/staging/vt6655/desc.h b/drivers/staging/vt6655/desc.h
index f8e41487f85..c620cbfbaad 100644
--- a/drivers/staging/vt6655/desc.h
+++ b/drivers/staging/vt6655/desc.h
@@ -213,29 +213,6 @@ typedef struct tagDEVICE_RD_INFO {
 	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;
-  }
-  }
-*/
-
-/*
-  typedef struct tagRDES0 {
-  unsigned short wResCount;
-  unsigned short wf1Owner;
-//    unsigned short f15Reserved : 15;
-//    unsigned short f1Owner : 1;
-} __attribute__ ((__packed__))
-SRDES0;
-*/
-
 #ifdef __BIG_ENDIAN
 
 typedef struct tagRDES0 {
@@ -284,17 +261,6 @@ typedef const SRxDesc *PCSRxDesc;
 
 #ifdef __BIG_ENDIAN
 
-/*
-  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__))
-STDES0;
-*/
-
 typedef struct tagTDES0 {
 	volatile    unsigned char byTSR0;
 	volatile    unsigned char byTSR1;
@@ -339,19 +305,6 @@ typedef struct tagDEVICE_TD_INFO {
 	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;
-  }
-  }
-*/
-
 //
 // transmit descriptor
 //
diff --git a/drivers/staging/vt6655/device.h b/drivers/staging/vt6655/device.h
index 062c3a374b9..45fc8a0b9b5 100644
--- a/drivers/staging/vt6655/device.h
+++ b/drivers/staging/vt6655/device.h
@@ -148,8 +148,17 @@
 
 /*---------------------  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...)		\
+do {					\
+	if (l <= msglevel)		\
+		printk(p, ##args);	\
+} while (0)
+
+#define PRINT_K(p, args...)		\
+do {					\
+	if (PRIVATE_Message)		\
+		printk(p, ##args);	\
+} while (0)
 
 //0:11A 1:11B 2:11G
 typedef enum _VIA_BB_TYPE
@@ -741,12 +750,6 @@ typedef struct __device_info {
 	bool bWPADEVUp;
 	struct sk_buff          *skb;
 #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-/*
-  bool bwextstep0;
-  bool bwextstep1;
-  bool bwextstep2;
-  bool bwextstep3;
-*/
 	unsigned int	bwextcount;
 	bool bWPASuppWextEnabled;
 #endif
diff --git a/drivers/staging/vt6655/device_main.c b/drivers/staging/vt6655/device_main.c
index a952df1bf9d..1d3908d044d 100644
--- a/drivers/staging/vt6655/device_main.c
+++ b/drivers/staging/vt6655/device_main.c
@@ -89,7 +89,6 @@
 #include <linux/slab.h>
 
 /*---------------------  Static Definitions -------------------------*/
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 static int          msglevel                =   MSG_LEVEL_INFO;
 
 //
@@ -100,14 +99,8 @@ 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;
 
 #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);
-*/
 
 #define RX_DESC_MIN0     16
 #define RX_DESC_MAX0     128
@@ -346,38 +339,6 @@ static void vt6655_remove(struct pci_dev *pcid)
 	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_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)
 {
 	POPTIONS pOpts = &(pDevice->sOpts);
@@ -395,7 +356,6 @@ static void device_get_options(PSDevice pDevice, int index, char *devname)
 
 	pOpts->flags |= DEVICE_FLAGS_PREAMBLE_TYPE;
 	pOpts->flags |= DEVICE_FLAGS_OP_MODE;
-	//pOpts->flags|=DEVICE_FLAGS_PS_MODE;
 	pOpts->short_retry = SHORT_RETRY_DEF;
 	pOpts->long_retry = LONG_RETRY_DEF;
 	pOpts->bbp_type = BBP_TYPE_DEF;
@@ -431,7 +391,6 @@ device_set_options(PSDevice pDevice) {
 
 //PLICE_DEBUG->
 	pDevice->byAutoFBCtrl = AUTO_FB_0;
-	//pDevice->byAutoFBCtrl = AUTO_FB_1;
 //PLICE_DEBUG<-
 	pDevice->bUpdateBBVGA = true;
 	pDevice->byFOETuning = 0;
@@ -549,12 +508,12 @@ static void device_init_registers(PSDevice pDevice, DEVICE_INIT_TYPE InitType)
 		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);
+		pDevice->ulDiversityNValue = 100*260;
+		pDevice->ulDiversityMValue = 100*16;
+		pDevice->byTMax = 1;
+		pDevice->byTMax2 = 4;
+		pDevice->ulSQ3TH = 0;
+		pDevice->byTMax3 = 64;
 
 		if (byValue == (EEP_ANTENNA_AUX | EEP_ANTENNA_MAIN)) {
 			pDevice->byAntennaCount = 2;
@@ -568,7 +527,7 @@ static void device_init_registers(PSDevice pDevice, DEVICE_INIT_TYPE InitType)
 			// chester for antenna
 			byValue1 = SROMbyReadEmbedded(pDevice->PortOffset, EEP_OFS_ANTENNA);
 			if ((byValue1 & 0x08) == 0)
-				pDevice->bDiversityEnable = false;//SROMbyReadEmbedded(pDevice->PortOffset, 0x50);
+				pDevice->bDiversityEnable = false;
 			else
 				pDevice->bDiversityEnable = true;
 		} else  {
@@ -593,7 +552,6 @@ static void device_init_registers(PSDevice pDevice, DEVICE_INIT_TYPE InitType)
 		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];
@@ -635,9 +593,9 @@ static void device_init_registers(PSDevice pDevice, DEVICE_INIT_TYPE InitType)
 		pDevice->byRFType &= RF_MASK;
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pDevice->byRFType = %x\n", pDevice->byRFType);
 
-		if (!pDevice->bZoneRegExist) {
+		if (!pDevice->bZoneRegExist)
 			pDevice->byZoneType = pDevice->abyEEPROM[EEP_OFS_ZONETYPE];
-		}
+
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pDevice->byZoneType = %x\n", pDevice->byZoneType);
 
 		//Init RF module
@@ -647,21 +605,18 @@ static void device_init_registers(PSDevice pDevice, DEVICE_INIT_TYPE InitType)
 		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);
 
 		// Load power Table
 
 		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) {
+			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) {
+			if (pDevice->abyOFDMPwrTbl[ii + 1] == 0)
 				pDevice->abyOFDMPwrTbl[ii + 1] = pDevice->byOFDMPwrG;
-			}
+
 			pDevice->abyCCKDefaultPwr[ii + 1] = byCCKPwrdBm;
 			pDevice->abyOFDMDefaultPwr[ii + 1] = byOFDMPwrdBm;
 		}
@@ -715,14 +670,12 @@ static void device_init_registers(PSDevice pDevice, DEVICE_INIT_TYPE InitType)
 
 		pDevice->byCurrentCh = 0;
 
-		//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) {
+		if (pDevice->uConnectionRate == RATE_AUTO)
 			pDevice->wCurrentRate = RATE_54M;
-		} else {
+		else
 			pDevice->wCurrentRate = (unsigned short)pDevice->uConnectionRate;
-		}
 
 		// default G Mode
 		VNTWIFIbConfigPhyMode(pDevice->pMgmt, PHY_TYPE_11G);
@@ -738,22 +691,25 @@ static void device_init_registers(PSDevice pDevice, DEVICE_INIT_TYPE InitType)
 			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->byGPIO & GPIO0_DATA)
+				pDevice->bHWRadioOff = true;
 
+			if (!(pDevice->byGPIO & GPIO0_DATA))
+				pDevice->bHWRadioOff = false;
 		}
-		if (pDevice->bRadioControlOff) {
+
+		if (pDevice->bRadioControlOff)
 			CARDbRadioPowerOff(pDevice);
-		} else  CARDbRadioPowerOn(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 || pDevice->bRadioControlOff) {
+	if (pDevice->bHWRadioOff || pDevice->bRadioControlOff)
 		CARDbRadioPowerOff(pDevice);
-	}
 
 #endif
 }
@@ -768,9 +724,8 @@ CARDvSafeResetRx(pDevice);
 // reset Rx pointer
 CARDvSafeResetTx(pDevice);
 
-if (pDevice->byLocalID <= REV_ID_VT3253_A1) {
+if (pDevice->byLocalID <= REV_ID_VT3253_A1)
 	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_RCR, RCR_WPAERR);
-}
 
 pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
 
@@ -807,7 +762,7 @@ 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) {
 		wpahdr = (viawget_wpa_header *)pDevice->skb->data;
@@ -823,9 +778,6 @@ static bool device_release_WPADEV(PSDevice pDevice)
 		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) {
 			set_current_state(TASK_UNINTERRUPTIBLE);
 			schedule_timeout(HZ / 20);          //wait 50ms
@@ -869,7 +821,6 @@ vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent)
 	}
 
 	// Chain it all together
-	// SET_MODULE_OWNER(dev);
 	SET_NETDEV_DEV(dev, &pcid->dev);
 
 	if (bFirst) {
@@ -902,7 +853,6 @@ vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent)
 
 #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);
 	{
 		int i;
@@ -916,7 +866,6 @@ vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent)
 			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) {
@@ -942,11 +891,7 @@ vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent)
 
 #endif
 
-#ifdef	DEBUG
-	//return  0;
-#endif
 	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");
@@ -967,7 +912,6 @@ vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent)
 
 	VNSvInPortB(pDevice->PortOffset+0x4F, &value);
 	printk("Before write: value is %x\n", value);
-	//VNSvInPortB(pDevice->PortOffset+0x3F, 0x00);
 	VNSvOutPortB(pDevice->PortOffset, value);
 	VNSvInPortB(pDevice->PortOffset+0x4F, &value);
 	printk("After write: value is %x\n", value);
@@ -1075,11 +1019,6 @@ static bool device_get_pci_info(PSDevice pDevice, struct pci_dev *pcid) {
 	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);
 
 	pDevice->pcid = pcid;
@@ -1088,13 +1027,6 @@ static bool device_get_pci_info(PSDevice pDevice, struct pci_dev *pcid) {
 	pci_write_config_byte(pcid, PCI_COMMAND, (b|PCI_COMMAND_MASTER));
 
 #ifdef	PLICE_DEBUG
-	//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);
-
 	for (ii = 0; ii < 0xFF; ii++) {
 		pci_read_config_byte(pcid, ii, &value);
 		pci_config[ii] = value;
@@ -1468,7 +1400,6 @@ static int device_rx_srv(PSDevice pDevice, unsigned int uIdx) {
 	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)) {
@@ -1564,9 +1495,9 @@ static int device_tx_srv(PSDevice pDevice, unsigned int uIdx) {
 						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) {
+					if ((pTxBufHead->wFragCtl & FRAGCTL_ENDFRAG) != FRAGCTL_NONFRAG)
 						pDevice->s802_11Counter.TransmittedFragmentCount++;
-					}
+
 					pStats->tx_packets++;
 					pStats->tx_bytes += pTD->pTDInfo->skb->len;
 				} else {
@@ -1584,7 +1515,6 @@ static int device_tx_srv(PSDevice pDevice, unsigned int uIdx) {
 					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);
 				}
@@ -1596,8 +1526,6 @@ static int device_tx_srv(PSDevice pDevice, unsigned int uIdx) {
 						(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 ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) &&
 				    (pTD->pTDInfo->byFlags & TD_FLAGS_NETIF_SKB)) {
@@ -1633,9 +1561,9 @@ static int device_tx_srv(PSDevice pDevice, unsigned int uIdx) {
 			bFull = true;
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " AC0DMA is Full = %d\n", pDevice->iTDUsed[uIdx]);
 		}
-		if (netif_queue_stopped(pDevice->dev) && !bFull) {
+		if (netif_queue_stopped(pDevice->dev) && !bFull)
 			netif_wake_queue(pDevice->dev);
-		}
+
 	}
 
 	pDevice->apTailTD[uIdx] = pTD;
@@ -1689,33 +1617,22 @@ int MlmeThread(
 {
 	PSDevice	pDevice =  (PSDevice) Context;
 	PSRxMgmtPacket			pRxMgmtPacket;
-	// int i;
-	//complete(&pDevice->notify);
 
-	//i = 0;
-#if 1
 	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);
 			vMgrRxManagePacket(pDevice, pDevice->pMgmt, pRxMgmtPacket);
 		}
 		spin_unlock_irq(&pDevice->lock);
 		if (mlme_kill == 0)
 			break;
-		//udelay(200);
-#endif
+
 		schedule();
 		if (mlme_kill == 0)
 			break;
 	}
 
-#endif
 	return 0;
 }
 
@@ -1727,9 +1644,9 @@ static int  device_open(struct net_device *dev) {
 #endif
 
 	pDevice->rx_buf_sz = PKT_BUF_SZ;
-	if (!device_init_rings(pDevice)) {
+	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)
@@ -1749,11 +1666,10 @@ static int  device_open(struct net_device *dev) {
 	device_init_defrag_cb(pDevice);
 	device_init_td0_ring(pDevice);
 	device_init_td1_ring(pDevice);
-//    VNTWIFIvSet11h(pDevice->pMgmt, pDevice->b11hEnable);
 
-	if (pDevice->bDiversityRegCtlON) {
+	if (pDevice->bDiversityRegCtlON)
 		device_init_diversity_timer(pDevice);
-	}
+
 	vMgrObjectInit(pDevice);
 	vMgrTimerInit(pDevice);
 
@@ -1773,8 +1689,6 @@ static int  device_open(struct net_device *dev) {
 	mlme_kill = 1;
 #endif
 
-	//wait_for_completion(&pDevice->notify);
-
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "call device_init_registers\n");
 	device_init_registers(pDevice, DEVICE_INIT_COLD);
 	MACvReadEtherAddress(pDevice->PortOffset, pDevice->abyCurrentNetAddr);
@@ -1786,12 +1700,6 @@ static int  device_open(struct net_device *dev) {
 	add_timer(&(pDevice->pMgmt->sTimerSecondCallback));
 
 #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
-	/*
-	  pDevice->bwextstep0 = false;
-	  pDevice->bwextstep1 = false;
-	  pDevice->bwextstep2 = false;
-	  pDevice->bwextstep3 = false;
-	*/
 	pDevice->bwextcount = 0;
 	pDevice->bWPASuppWextEnabled = false;
 #endif
@@ -1873,9 +1781,7 @@ static int  device_close(struct net_device *dev) {
 	pDevice->flags &= (~DEVICE_FLAGS_OPENED);
 	//2008-0714-01<Add>by chester
 	device_release_WPADEV(pDevice);
-//PLICE_DEBUG->
-	//tasklet_kill(&pDevice->RxMngWorkItem);
-//PLICE_DEBUG<-
+
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "device_close.. \n");
 	return 0;
 }
@@ -1922,7 +1828,6 @@ bool device_dma0_xmit(PSDevice pDevice, struct sk_buff *skb, unsigned int uNodeI
 	unsigned int cbHeaderSize;
 	unsigned int ii;
 	SKeyItem        STempKey;
-//    unsigned char byKeyIndex = 0;
 
 	if (pDevice->bStopTx0Pkt) {
 		dev_kfree_skb_irq(skb);
@@ -1951,9 +1856,9 @@ bool device_dma0_xmit(PSDevice pDevice, struct sk_buff *skb, unsigned int uNodeI
 	cbFrameBodySize = skb->len - ETH_HLEN;
 
 	// 802.1H
-	if (ntohs(pDevice->sTxEthHeader.wType) > ETH_DATA_LEN) {
+	if (ntohs(pDevice->sTxEthHeader.wType) > ETH_DATA_LEN)
 		cbFrameBodySize += 8;
-	}
+
 	uMACfragNum = cbGetFragCount(pDevice, pTransmitKey, cbFrameBodySize, &pDevice->sTxEthHeader);
 
 	if (uMACfragNum > AVAIL_TD(pDevice, TYPE_TXDMA0)) {
@@ -1964,11 +1869,10 @@ bool device_dma0_xmit(PSDevice pDevice, struct sk_buff *skb, unsigned int uNodeI
 
 	if (pDevice->bFixRate) {
 		if (pDevice->eCurrentPHYType == PHY_TYPE_11B) {
-			if (pDevice->uConnectionRate >= RATE_11M) {
+			if (pDevice->uConnectionRate >= RATE_11M)
 				pDevice->wCurrentRate = RATE_11M;
-			} else {
+			else
 				pDevice->wCurrentRate = (unsigned short)pDevice->uConnectionRate;
-			}
 		} else {
 			if (pDevice->uConnectionRate >= RATE_54M)
 				pDevice->wCurrentRate = RATE_54M;
@@ -1980,11 +1884,10 @@ bool device_dma0_xmit(PSDevice pDevice, struct sk_buff *skb, unsigned int uNodeI
 	}
 
 	//preamble type
-	if (pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble) {
+	if (pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble)
 		pDevice->byPreambleType = pDevice->byShortPreamble;
-	} else {
+	else
 		pDevice->byPreambleType = PREAMBLE_LONG;
-	}
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dma0: pDevice->wCurrentRate = %d \n", pDevice->wCurrentRate);
 
@@ -1993,11 +1896,10 @@ bool device_dma0_xmit(PSDevice pDevice, struct sk_buff *skb, unsigned int uNodeI
 	} else if (pDevice->eCurrentPHYType == PHY_TYPE_11A) {
 		byPktType = PK_TYPE_11A;
 	} else {
-		if (pDevice->bProtectMode) {
+		if (pDevice->bProtectMode)
 			byPktType = PK_TYPE_11GB;
-		} else {
+		else
 			byPktType = PK_TYPE_11GA;
-		}
 	}
 
 	if (pDevice->bEncryptionEnable)
@@ -2119,12 +2021,11 @@ static int  device_xmit(struct sk_buff *skb, struct net_device *dev) {
 					return 0;
 				}
 
-				if (pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble) {
+				if (pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble)
 					pDevice->byPreambleType = pDevice->byShortPreamble;
-
-				} else {
+				else
 					pDevice->byPreambleType = PREAMBLE_LONG;
-				}
+
 				bNodeExist = true;
 
 			}
@@ -2145,9 +2046,8 @@ static int  device_xmit(struct sk_buff *skb, struct net_device *dev) {
 	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) {
+	if (ntohs(pDevice->sTxEthHeader.wType) > ETH_DATA_LEN)
 		cbFrameBodySize += 8;
-	}
 
 	if (pDevice->bEncryptionEnable) {
 		bNeedEncryption = true;
@@ -2183,9 +2083,9 @@ static int  device_xmit(struct sk_buff *skb, struct net_device *dev) {
 			pbyBSSID = pDevice->abyBroadcastAddr;
 			if (KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, GROUP_KEY, &pTransmitKey) == false) {
 				pTransmitKey = NULL;
-				if (pDevice->pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
+				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
+				else
 					DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG "NOT IBSS and KEY is NULL. [%d]\n", pDevice->pMgmt->eCurrMode);
 			} else {
 				bTKIP_UseGTK = true;
@@ -2234,11 +2134,10 @@ static int  device_xmit(struct sk_buff *skb, struct net_device *dev) {
 #endif
 
 		if (pDevice->eCurrentPHYType == PHY_TYPE_11B) {
-			if (pDevice->uConnectionRate >= RATE_11M) {
+			if (pDevice->uConnectionRate >= RATE_11M)
 				pDevice->wCurrentRate = RATE_11M;
-			} else {
+			else
 				pDevice->wCurrentRate = (unsigned short)pDevice->uConnectionRate;
-			}
 		} else {
 			if ((pDevice->eCurrentPHYType == PHY_TYPE_11A) &&
 			    (pDevice->uConnectionRate <= RATE_6M)) {
@@ -2279,24 +2178,18 @@ static int  device_xmit(struct sk_buff *skb, struct net_device *dev) {
 		}
 	}
 
-//    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) {
+		if (pDevice->bProtectMode)
 			byPktType = PK_TYPE_11GB;
-		} else {
+		else
 			byPktType = PK_TYPE_11GA;
-		}
 	}
 
-//#ifdef	PLICE_DEBUG
-//	printk("FIX RATE:CurrentRate is %d");
-//#endif
-
 	if (bNeedEncryption) {
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ntohs Pkt Type=%04x\n", ntohs(pDevice->sTxEthHeader.wType));
 		if ((pDevice->sTxEthHeader.wType) == TYPE_PKT_802_1x) {
@@ -2369,17 +2262,13 @@ static int  device_xmit(struct sk_buff *skb, struct net_device *dev) {
 #ifdef TxInSleep
 	pDevice->nTxDataTimeCout = 0; //2008-8-21 chester <add> for send null packet
 #endif
-	if (AVAIL_TD(pDevice, TYPE_AC0DMA) <= 1) {
+	if (AVAIL_TD(pDevice, TYPE_AC0DMA) <= 1)
 		netif_stop_queue(dev);
-	}
 
 	pDevice->apCurrTD[TYPE_AC0DMA] = pHeadTD;
-//#ifdef	PLICE_DEBUG
-	if (pDevice->bFixRate) {
+
+	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
@@ -2411,7 +2300,6 @@ static int  device_xmit(struct sk_buff *skb, struct net_device *dev) {
 	}
 
 	MACvTransmitAC0(pDevice->PortOffset);
-//    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "acdma0:pDevice->apCurrTD= %p\n", pHeadTD);
 
 	dev->trans_start = jiffies;
 
@@ -2430,7 +2318,6 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance) {
 	int             handled = 0;
 	unsigned char byData = 0;
 	int             ii = 0;
-//    unsigned char byRSSI;
 
 	MACvReadISR(pDevice->PortOffset, &pDevice->dwIsr);
 
@@ -2441,17 +2328,6 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance) {
 		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);
@@ -2459,9 +2335,9 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance) {
 
 	//Make sure current page is 0
 	VNSvInPortB(pDevice->PortOffset + MAC_REG_PAGE1SEL, &byOrgPageSel);
-	if (byOrgPageSel == 1) {
+	if (byOrgPageSel == 1)
 		MACvSelectPage0(pDevice->PortOffset);
-	} else
+	else
 		byOrgPageSel = 0;
 
 	MACvReadMIBCounter(pDevice->PortOffset, &dwMIBCounter);
@@ -2492,7 +2368,6 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance) {
 				VNSvInPortD(pDevice->PortOffset + MAC_REG_MAR4, &(pDevice->dwOrgMAR4));
 				MACvSelectPage0(pDevice->PortOffset);
 				//xxxx
-				// WCMDbFlushCommandQueue(pDevice->pMgmt, true);
 				if (set_channel(pDevice, pDevice->pCurrMeasureEID->sReq.byChannel)) {
 					pDevice->bMeasureInProgress = true;
 					MACvSelectPage1(pDevice->PortOffset);
@@ -2500,12 +2375,11 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance) {
 					MACvSelectPage0(pDevice->PortOffset);
 					pDevice->byBasicMap = 0;
 					pDevice->byCCAFraction = 0;
-					for (ii = 0; ii < 8; ii++) {
+					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);
@@ -2529,7 +2403,6 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance) {
 				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);
@@ -2572,9 +2445,7 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance) {
 
 				}
 			}
-			if (pDevice->eOPMode == OP_MODE_ADHOC) {
-				//pDevice->bBeaconSent = false;
-			} else {
+			if (pDevice->eOPMode != OP_MODE_ADHOC) {
 				if ((pDevice->bUpdateBBVGA) && pDevice->bLinkPass && (pDevice->uCurrRSSI != 0)) {
 					long            ldBm;
 
@@ -2605,9 +2476,8 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance) {
 			}
 
 			pDevice->bBeaconSent = false;
-			if (pDevice->bEnablePSMode) {
+			if (pDevice->bEnablePSMode)
 				PSbIsNextTBTTWakeUp((void *)pDevice);
-			}
 
 			if ((pDevice->eOPMode == OP_MODE_AP) ||
 			    (pDevice->eOPMode == OP_MODE_ADHOC)) {
@@ -2615,9 +2485,7 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance) {
 							  (pMgmt->wIBSSBeaconPeriod - MAKE_BEACON_RESERVED) << 10);
 			}
 
-			if (pDevice->eOPMode == OP_MODE_ADHOC && pDevice->pMgmt->wCurrATIMWindow > 0) {
-				// todo adhoc PS mode
-			}
+			/* TODO: adhoc PS mode */
 
 		}
 
@@ -2651,27 +2519,24 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance) {
 					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) {
+		if (pDevice->dwIsr & ISR_RXDMA0)
 			max_count += device_rx_srv(pDevice, TYPE_RXDMA0);
-		}
-		if (pDevice->dwIsr & ISR_RXDMA1) {
+
+		if (pDevice->dwIsr & ISR_RXDMA1)
 			max_count += device_rx_srv(pDevice, TYPE_RXDMA1);
-		}
-		if (pDevice->dwIsr & ISR_TXDMA0) {
+
+		if (pDevice->dwIsr & ISR_TXDMA0)
 			max_count += device_tx_srv(pDevice, TYPE_TXDMA0);
-		}
-		if (pDevice->dwIsr & ISR_AC0DMA) {
+
+		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)
@@ -2692,9 +2557,8 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance) {
 			break;
 	}
 
-	if (byOrgPageSel == 1) {
+	if (byOrgPageSel == 1)
 		MACvSelectPage1(pDevice->PortOffset);
-	}
 
 	spin_unlock_irq(&pDevice->lock);
 	MACvIntEnable(pDevice->PortOffset, IMR_MASK_VALUE);
@@ -3099,16 +2963,6 @@ static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd) {
 	case SIOCGIWPRIV:
 		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 (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
@@ -3195,9 +3049,9 @@ static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd) {
 				rc = 0;
 			}
 
-		if (test_and_set_bit(0, (void *)&(pMgmt->uCmdBusy))) {
+		if (test_and_set_bit(0, (void *)&(pMgmt->uCmdBusy)))
 			return -EBUSY;
-		}
+
 		rc = private_ioctl(pDevice, rq);
 		clear_bit(0, (void *)&(pMgmt->uCmdBusy));
 		break;
@@ -3290,8 +3144,6 @@ static int __init vt6655_init_module(void)
 {
 	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)
diff --git a/drivers/staging/vt6655/dpc.c b/drivers/staging/vt6655/dpc.c
index 771bf35ae04..7ddaf2603ba 100644
--- a/drivers/staging/vt6655/dpc.c
+++ b/drivers/staging/vt6655/dpc.c
@@ -60,7 +60,6 @@
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 static int msglevel = MSG_LEVEL_INFO;
 
 const unsigned char acbyRxRate[MAX_RATE] =
@@ -182,11 +181,11 @@ s_vProcessRxMACHeader(PSDevice pDevice, unsigned char *pbyRxBufferAddr,
 			cbHeaderSize -= 8;
 			pwType = (unsigned short *)(pbyRxBufferAddr + cbHeaderSize);
 			if (bIsWEP) {
-				if (bExtIV) {
+				if (bExtIV)
 					*pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN - 8);    // 8 is IV&ExtIV
-				} else {
+				else
 					*pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN - 4);    // 4 is IV
-				}
+
 			} else {
 				*pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN);
 			}
@@ -195,11 +194,11 @@ s_vProcessRxMACHeader(PSDevice pDevice, unsigned char *pbyRxBufferAddr,
 		cbHeaderSize -= 2;
 		pwType = (unsigned short *)(pbyRxBufferAddr + cbHeaderSize);
 		if (bIsWEP) {
-			if (bExtIV) {
+			if (bExtIV)
 				*pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN - 8);    // 8 is IV&ExtIV
-			} else {
+			else
 				*pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN - 4);    // 4 is IV
-			}
+
 		} else {
 			*pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN);
 		}
@@ -223,6 +222,7 @@ static unsigned char s_byGetRateIdx(unsigned char byRate)
 		if (acbyRxRate[byRateIdx % MAX_RATE] == byRate)
 			return byRateIdx;
 	}
+
 	return 0;
 }
 
@@ -327,15 +327,11 @@ device_receive_frame(
 	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);
-#endif
 //PLICE_DEBUG<-
 	pwFrameSize = (unsigned short *)(skb->data + 2);
 	FrameSize = cpu_to_le16(pCurrRD->m_rd1RD1.wReqCount) - cpu_to_le16(pCurrRD->m_rd0RD0.wResCount);
@@ -366,7 +362,6 @@ device_receive_frame(
 		return false;
 	}
 //PLICE_DEBUG->
-#if 1
 	// update receive statistic counter
 	STAvUpdateRDStatCounter(&pDevice->scStatistic,
 				*pbyRsr,
@@ -375,14 +370,12 @@ device_receive_frame(
 				pbyFrame,
 				FrameSize);
 
-#endif
-
 	pMACHeader = (PS802_11Header)((unsigned char *)(skb->data) + 8);
 //PLICE_DEBUG<-
 	if (pDevice->bMeasureInProgress) {
-		if ((*pbyRsr & RSR_CRCOK) != 0) {
+		if ((*pbyRsr & RSR_CRCOK) != 0)
 			pDevice->byBasicMap |= 0x01;
-		}
+
 		dwDuration = (FrameSize << 4);
 		dwDuration /= acbyRxRate[*pbyRxRate%MAX_RATE];
 		if (*pbyRxRate <= RATE_11M) {
@@ -399,9 +392,9 @@ device_receive_frame(
 		RFvRSSITodBm(pDevice, *pbyRSSI, &ldBm);
 		ldBmThreshold = -57;
 		for (ii = 7; ii > 0;) {
-			if (ldBm > ldBmThreshold) {
+			if (ldBm > ldBmThreshold)
 				break;
-			}
+
 			ldBmThreshold -= 5;
 			ii--;
 		}
@@ -436,9 +429,8 @@ device_receive_frame(
 	}
 
 	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
-		if (s_bAPModeRxCtl(pDevice, pbyFrame, iSANodeIndex)) {
+		if (s_bAPModeRxCtl(pDevice, pbyFrame, iSANodeIndex))
 			return false;
-		}
 	}
 
 	if (IS_FC_WEP(pbyFrame)) {
@@ -488,13 +480,10 @@ device_receive_frame(
 				    (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)) {
+					if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_TKIP))
 						pDevice->s802_11Counter.TKIPICVErrors++;
-					} else if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_CCMP)) {
+					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;
 			}
@@ -557,13 +546,9 @@ device_receive_frame(
 			pRxPacket->byRxRate = s_byGetRateIdx(*pbyRxRate);
 			pRxPacket->byRxChannel = (*pbyRxSts) >> 2;
 //PLICE_DEBUG->
-//EnQueue(pDevice,pRxPacket);
 
 #ifdef	THREAD
 			EnQueue(pDevice, pRxPacket);
-
-			//up(&pDevice->mlme_semaphore);
-			//Enque (pDevice->FirstRecvMngList,pDevice->LastRecvMngList,pMgmt);
 #else
 
 #ifdef	TASK_LET
@@ -571,12 +556,10 @@ device_receive_frame(
 			tasklet_schedule(&pDevice->RxMngWorkItem);
 #else
 			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;
@@ -590,9 +573,8 @@ device_receive_frame(
 				netif_rx(skb);
 				return true;
 			}
-		} else {
-			// Control Frame
 		}
+
 		return false;
 	} else {
 		if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
@@ -643,14 +625,9 @@ device_receive_frame(
 // 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) {
+		if (!IS_FC_MOREDATA((skb->data+4))) {
+			if (pDevice->pMgmt->bInTIMWake == true)
 				pDevice->pMgmt->bInTIMWake = false;
-			}
 		}
 	}
 
@@ -661,9 +638,9 @@ device_receive_frame(
 		BBvAntennaDiversity(pDevice, s_byGetRateIdx(*pbyRxRate), 0);
 	}
 
-	if (pDevice->byLocalID != REV_ID_VT3253_B1) {
+	if (pDevice->byLocalID != REV_ID_VT3253_B1)
 		pDevice->uCurrRSSI = *pbyRSSI;
-	}
+
 	pDevice->byCurrSQ = *pbySQ;
 
 	if ((*pbyRSSI != 0) &&
@@ -673,11 +650,10 @@ device_receive_frame(
 		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) {
+		for (ii = 0; ii < RSSI_STAT_COUNT; ii++)
+			if (pMgmt->pCurrBSS->ldBmAverage[ii] != 0)
 				pMgmt->pCurrBSS->ldBmMAX = max(pMgmt->pCurrBSS->ldBmAverage[ii], ldBm);
-			}
-		}
+
 	}
 
 	// -----------------------------------------------
@@ -720,9 +696,8 @@ device_receive_frame(
 	}
 
 	if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_TKIP)) {
-		if (bIsWEP) {
+		if (bIsWEP)
 			FrameSize -= 8;  //MIC
-		}
 	}
 
 	//--------------------------------------------------------------------------------
@@ -765,16 +740,12 @@ device_receive_frame(
 
 			pdwMIC_L = (__le32 *)(skb->data + 4 + FrameSize);
 			pdwMIC_R = (__le32 *)(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 ((le32_to_cpu(*pdwMIC_L) != dwLocalMIC_L) ||
 			    (le32_to_cpu(*pdwMIC_R) != dwLocalMIC_R) ||
 			    pDevice->bRxMICFail) {
 				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])) {
@@ -813,10 +784,8 @@ device_receive_frame(
 					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;
@@ -863,10 +832,8 @@ device_receive_frame(
 				    !((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) {
@@ -881,10 +848,6 @@ device_receive_frame(
 		}
 	} // ----- 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
@@ -917,22 +880,6 @@ device_receive_frame(
 	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;
-  }
-  }
-*/
 
 	skb->ip_summed = CHECKSUM_NONE;
 	pStats->rx_bytes += skb->len;
@@ -1121,19 +1068,11 @@ static bool s_bHandleRxEncryption(
 
 	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) {
-//            pDevice->s802_11Counter.DecryptFailureCount.QuadPart++;
-		}
+
 		return false;
 	}
 	if (byDecMode != pKey->byCipherSuite) {
-		if (byDecMode == KEY_CTL_WEP) {
-//            pDevice->s802_11Counter.WEPUndecryptableCount.QuadPart++;
-		} else if (pDevice->bLinkPass) {
-//            pDevice->s802_11Counter.DecryptFailureCount.QuadPart++;
-		}
+
 		*pKeyOut = NULL;
 		return false;
 	}
@@ -1151,9 +1090,9 @@ static bool s_bHandleRxEncryption(
 			rc4_init(&pDevice->SBox, pDevice->abyPRNG, pKey->uKeyLength + 3);
 			rc4_encrypt(&pDevice->SBox, pbyIV+4, pbyIV+4, PayloadLen);
 
-			if (ETHbIsBufferCrc32Ok(pbyIV+4, PayloadLen)) {
+			if (ETHbIsBufferCrc32Ok(pbyIV+4, PayloadLen))
 				*pbyNewRsr |= NEWRSR_DECRYPTOK;
-			}
+
 		}
 	} else if ((byDecMode == KEY_CTL_TKIP) ||
 		   (byDecMode == KEY_CTL_CCMP)) {
@@ -1162,11 +1101,11 @@ static bool s_bHandleRxEncryption(
 		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) {
+		if (byDecMode == KEY_CTL_TKIP)
 			*pwRxTSC15_0 = cpu_to_le16(MAKEWORD(*(pbyIV + 2), *pbyIV));
-		} else {
+		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) &&
@@ -1231,14 +1170,8 @@ static bool s_bHostWepRxEncryption(
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "AES:%d %d %d\n", pDevice->pMgmt->byCSSPK, pDevice->pMgmt->byCSSGK, byDecMode);
 
-	if (byDecMode != pKey->byCipherSuite) {
-		if (byDecMode == KEY_CTL_WEP) {
-//            pDevice->s802_11Counter.WEPUndecryptableCount.QuadPart++;
-		} else if (pDevice->bLinkPass) {
-//            pDevice->s802_11Counter.DecryptFailureCount.QuadPart++;
-		}
+	if (byDecMode != pKey->byCipherSuite)
 		return false;
-	}
 
 	if (byDecMode == KEY_CTL_WEP) {
 		// handle WEP
@@ -1257,9 +1190,9 @@ static bool s_bHostWepRxEncryption(
 			rc4_init(&pDevice->SBox, pDevice->abyPRNG, pKey->uKeyLength + 3);
 			rc4_encrypt(&pDevice->SBox, pbyIV+4, pbyIV+4, PayloadLen);
 
-			if (ETHbIsBufferCrc32Ok(pbyIV+4, PayloadLen)) {
+			if (ETHbIsBufferCrc32Ok(pbyIV+4, PayloadLen))
 				*pbyNewRsr |= NEWRSR_DECRYPTOK;
-			}
+
 		}
 	} else if ((byDecMode == KEY_CTL_TKIP) ||
 		   (byDecMode == KEY_CTL_CCMP)) {
@@ -1269,11 +1202,11 @@ static bool s_bHostWepRxEncryption(
 		*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) {
+		if (byDecMode == KEY_CTL_TKIP)
 			*pwRxTSC15_0 = cpu_to_le16(MAKEWORD(*(pbyIV+2), *pbyIV));
-		} else {
+		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) {
@@ -1386,9 +1319,8 @@ static bool s_bAPModeRxData(
 		if (bRelayAndForward)
 			iDANodeIndex = 0;
 
-		if ((pDevice->uAssocCount > 1) && (iDANodeIndex >= 0)) {
+		if ((pDevice->uAssocCount > 1) && (iDANodeIndex >= 0))
 			ROUTEbRelay(pDevice, (unsigned char *)(skb->data + cbHeaderOffset), FrameSize, (unsigned int)iDANodeIndex);
-		}
 
 		if (bRelayOnly)
 			return false;
diff --git a/drivers/staging/vt6655/hostap.c b/drivers/staging/vt6655/hostap.c
index 6eecd535891..317c2a8ee16 100644
--- a/drivers/staging/vt6655/hostap.c
+++ b/drivers/staging/vt6655/hostap.c
@@ -48,7 +48,6 @@
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 static int msglevel = MSG_LEVEL_INFO;
 
 /*---------------------  Static Functions  --------------------------*/
@@ -207,11 +206,11 @@ static int hostap_remove_sta(PSDevice pDevice,
 {
 	unsigned int uNodeIndex;
 
-	if (BSSDBbIsSTAInNodeDB(pDevice->pMgmt, param->sta_addr, &uNodeIndex)) {
+	if (BSSDBbIsSTAInNodeDB(pDevice->pMgmt, param->sta_addr, &uNodeIndex))
 		BSSvRemoveOneNode(pDevice, uNodeIndex);
-	} else {
+	else
 		return -ENOENT;
-	}
+
 	return 0;
 }
 
@@ -234,14 +233,13 @@ static int hostap_add_sta(PSDevice pDevice,
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	unsigned int uNodeIndex;
 
-	if (!BSSDBbIsSTAInNodeDB(pMgmt, param->sta_addr, &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;
 // TODO listenInterval
-//    pMgmt->sNodeDBTable[uNodeIndex].wListenInterval = 1;
 	pMgmt->sNodeDBTable[uNodeIndex].bPSEnable = false;
 	pMgmt->sNodeDBTable[uNodeIndex].bySuppRate = param->u.add_sta.tx_supp_rates;
 
@@ -296,8 +294,6 @@ static int hostap_get_info_sta(PSDevice pDevice,
 	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;
 	}
@@ -307,37 +303,6 @@ static int hostap_get_info_sta(PSDevice pDevice,
 
 /*
  * Description:
- *      reset txexec
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *      true, false
- *
- * Return Value:
- *
- */
-/*
-  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;
-  }
-*/
-
-/*
- * Description:
  *      set station flag
  *
  * Parameters:
@@ -456,19 +421,12 @@ static int hostap_set_encryption(PSDevice pDevice,
 	unsigned char abySeq[MAX_KEY_LEN];
 	unsigned long long 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->u.crypt.alg > WPA_ALG_CCMP)
 		return -EINVAL;
@@ -516,7 +474,7 @@ static int hostap_set_encryption(PSDevice pDevice,
 		       MAX_KEY_LEN
 );
 
-		return ret;
+		return 0;
 	}
 
 	memcpy(abyKey, param->u.crypt.key, param->u.crypt.key_len);
@@ -572,7 +530,7 @@ static int hostap_set_encryption(PSDevice pDevice,
 		pMgmt->byCSSGK = KEY_CTL_WEP;
 		pMgmt->sNodeDBTable[iNodeIndex].byCipherSuite = KEY_CTL_WEP;
 		pMgmt->sNodeDBTable[iNodeIndex].dwKeyIndex = dwKeyIndex;
-		return ret;
+		return 0;
 	}
 
 	if (param->u.crypt.seq) {
@@ -663,7 +621,7 @@ static int hostap_set_encryption(PSDevice pDevice,
 	pMgmt->sNodeDBTable[iNodeIndex].dwTSC47_16 = 0;
 	pMgmt->sNodeDBTable[iNodeIndex].wTSC15_0 = 0;
 
-	return ret;
+	return 0;
 }
 
 /*
@@ -684,7 +642,6 @@ static int hostap_get_encryption(PSDevice pDevice,
 				 int param_len)
 {
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	int     ret = 0;
 	int     ii;
 	int     iNodeIndex = 0;
 
@@ -701,11 +658,10 @@ static int hostap_get_encryption(PSDevice 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++) {
+	for (ii = 0; ii < 8; ii++)
 		param->u.crypt.seq[ii] = (unsigned char)pMgmt->sNodeDBTable[iNodeIndex].KeyRSC >> (ii * 8);
-	}
 
-	return ret;
+	return 0;
 }
 
 /*
@@ -780,12 +736,6 @@ int vt6655_hostap_ioctl(PSDevice pDevice, struct iw_point *p)
 		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);
-		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);
@@ -814,9 +764,8 @@ int vt6655_hostap_ioctl(PSDevice pDevice, struct iw_point *p)
 	}
 
 	if ((ret == 0) && ap_ioctl) {
-		if (copy_to_user(p->pointer, param, p->length)) {
+		if (copy_to_user(p->pointer, param, p->length))
 			ret = -EFAULT;
-		}
 	}
 
 out:
diff --git a/drivers/staging/vt6655/ioctl.c b/drivers/staging/vt6655/ioctl.c
index b5cd2e44e53..18d11d1f4fa 100644
--- a/drivers/staging/vt6655/ioctl.c
+++ b/drivers/staging/vt6655/ioctl.c
@@ -305,7 +305,8 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 			result = -EINVAL;
 			break;
 		}
-		pList = (PSBSSIDList)kmalloc(sizeof(SBSSIDList) + (sList.uItem * sizeof(SBSSIDItem)), (int)GFP_ATOMIC);
+		pList = (PSBSSIDList)kmalloc(sizeof(SBSSIDList) + (sList.uItem * sizeof(SBSSIDItem)),
+					     GFP_ATOMIC);
 		if (pList == NULL) {
 			result = -ENOMEM;
 			break;
@@ -318,7 +319,6 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 				pList->sBSSIDList[ii].uChannel = pBSS->uChannel;
 				pList->sBSSIDList[ii].wBeaconInterval = pBSS->wBeaconInterval;
 				pList->sBSSIDList[ii].wCapInfo = pBSS->wCapInfo;
-				/* pList->sBSSIDList[ii].uRSSI = pBSS->uRSSI; */
 				RFvRSSITodBm(pDevice, (unsigned char)(pBSS->uRSSI), &ldBm);
 				pList->sBSSIDList[ii].uRSSI = (unsigned int)ldBm;
 				memcpy(pList->sBSSIDList[ii].abyBSSID, pBSS->abyBSSID, WLAN_BSSID_LEN);
@@ -576,7 +576,8 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 			result = -EINVAL;
 			break;
 		}
-		pNodeList = (PSNodeList)kmalloc(sizeof(SNodeList) + (sNodeList.uItem * sizeof(SNodeItem)), (int)GFP_ATOMIC);
+		pNodeList = (PSNodeList)kmalloc(sizeof(SNodeList) + (sNodeList.uItem * sizeof(SNodeItem)),
+						GFP_ATOMIC);
 		if (pNodeList == NULL) {
 			result = -ENOMEM;
 			break;
diff --git a/drivers/staging/vt6655/ioctl.h b/drivers/staging/vt6655/ioctl.h
index ae240fd71a1..2f0db920e2c 100644
--- a/drivers/staging/vt6655/ioctl.h
+++ b/drivers/staging/vt6655/ioctl.h
@@ -41,13 +41,4 @@
 
 int private_ioctl(PSDevice pDevice, struct ifreq *rq);
 
-/*
-  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 bfea01f5e00..b7bd1909fa7 100644
--- a/drivers/staging/vt6655/iowpa.h
+++ b/drivers/staging/vt6655/iowpa.h
@@ -34,13 +34,6 @@
 #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;
-*/
 
 enum {
 	VIAWGET_SET_WPA = 1,
diff --git a/drivers/staging/vt6655/iwctl.c b/drivers/staging/vt6655/iwctl.c
index ac3fc16704c..ae2b87f177f 100644
--- a/drivers/staging/vt6655/iwctl.c
+++ b/drivers/staging/vt6655/iwctl.c
@@ -66,7 +66,6 @@ static const long frequency_list[] = {
 
 /*---------------------  Static Classes  ----------------------------*/
 
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 static int msglevel = MSG_LEVEL_INFO;
 
 /*---------------------  Static Variables  --------------------------*/
@@ -89,7 +88,6 @@ struct iw_statistics *iwctl_get_wireless_stats(struct net_device *dev)
 #endif
 	RFvRSSITodBm(pDevice, (unsigned char)(pDevice->uCurrRSSI), &ldBm);
 	pDevice->wstats.qual.level = ldBm;
-	//pDevice->wstats.qual.level = 0x100 - pDevice->uCurrRSSI;
 	pDevice->wstats.qual.noise = 0;
 	pDevice->wstats.qual.updated = 1;
 	pDevice->wstats.discard.nwid = 0;
@@ -237,11 +235,11 @@ int iwctl_giwscan(struct net_device *dev,
 			//ADD mode
 			memset(&iwe, 0, sizeof(iwe));
 			iwe.cmd = SIOCGIWMODE;
-			if (WLAN_GET_CAP_INFO_ESS(pBSS->wCapInfo)) {
+			if (WLAN_GET_CAP_INFO_ESS(pBSS->wCapInfo))
 				iwe.u.mode = IW_MODE_INFRA;
-			} else {
+			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
@@ -268,26 +266,25 @@ int iwctl_giwscan(struct net_device *dev,
 			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) {
+			else if (-ldBm > 90)
 				iwe.u.qual.qual = 0;
-			} else {
+			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)) {
+			if (WLAN_GET_CAP_INFO_PRIVACY(pBSS->wCapInfo))
 				iwe.u.data.flags = IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
-			} else {
+			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));
@@ -440,9 +437,9 @@ int iwctl_siwmode(struct net_device *dev,
 	case IW_MODE_ADHOC:
 		if (pMgmt->eConfigMode != WMAC_CONFIG_IBSS_STA) {
 			pMgmt->eConfigMode = WMAC_CONFIG_IBSS_STA;
-			if (pDevice->flags & DEVICE_FLAGS_OPENED) {
+			if (pDevice->flags & DEVICE_FLAGS_OPENED)
 				pDevice->bCommit = true;
-			}
+
 		}
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set mode to ad-hoc \n");
 		break;
@@ -450,9 +447,9 @@ int iwctl_siwmode(struct net_device *dev,
 	case IW_MODE_INFRA:
 		if (pMgmt->eConfigMode != WMAC_CONFIG_ESS_STA) {
 			pMgmt->eConfigMode = WMAC_CONFIG_ESS_STA;
-			if (pDevice->flags & DEVICE_FLAGS_OPENED) {
+			if (pDevice->flags & DEVICE_FLAGS_OPENED)
 				pDevice->bCommit = true;
-			}
+
 		}
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set mode to infrastructure \n");
 		break;
@@ -464,9 +461,9 @@ int iwctl_siwmode(struct net_device *dev,
 
 		if (pMgmt->eConfigMode != WMAC_CONFIG_AP) {
 			pMgmt->eConfigMode = WMAC_CONFIG_AP;
-			if (pDevice->flags & DEVICE_FLAGS_OPENED) {
+			if (pDevice->flags & DEVICE_FLAGS_OPENED)
 				pDevice->bCommit = true;
-			}
+
 		}
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set mode to Access Point \n");
 		break;
@@ -640,7 +637,6 @@ int iwctl_siwap(struct net_device *dev,
 	if (pMgmt->eScanState ==  WMAC_IS_SCANNING) {
 		// In scanning..
 		printk("SIOCSIWAP(??)-->In scanning...\n");
-		//  return -EAGAIN;
 	}
 	if (wrq->sa_family != ARPHRD_ETHER)
 		rc = -EINVAL;
@@ -674,9 +670,9 @@ int iwctl_siwap(struct net_device *dev,
 			}
 		}
 
-		if (pDevice->flags & DEVICE_FLAGS_OPENED) {
+		if (pDevice->flags & DEVICE_FLAGS_OPENED)
 			pDevice->bCommit = true;
-		}
+
 	}
 	return rc;
 }
@@ -700,9 +696,8 @@ int iwctl_giwap(struct net_device *dev,
 	if ((pDevice->bLinkPass == false) && (pMgmt->eCurrMode != WMAC_MODE_ESS_AP))
 		memset(wrq->sa_data, 0, 6);
 
-	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
+	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP)
 		memcpy(wrq->sa_data, pMgmt->abyCurrBSSID, 6);
-	}
 
 	wrq->sa_family = ARPHRD_ETHER;
 
@@ -719,42 +714,66 @@ int iwctl_giwaplist(struct net_device *dev,
 		    char *extra)
 {
 	int ii, jj, rc = 0;
-	struct sockaddr sock[IW_MAX_AP];
-	struct iw_quality qual[IW_MAX_AP];
+	struct sockaddr *sock	= NULL;
+	struct sockaddr *s	= NULL;
+	struct iw_quality *qual	= NULL;
+	struct iw_quality *q	= NULL;
+	PKnownBSS pBSS		= NULL;
+
 	PSDevice pDevice = (PSDevice)netdev_priv(dev);
 	PSMgmtObject pMgmt = &(pDevice->sMgmtObj);
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWAPLIST \n");
-	// Only super-user can see AP list
+	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWAPLIST\n");
 
 	if (!capable(CAP_NET_ADMIN)) {
 		rc = -EPERM;
-		return rc;
+		goto exit;
 	}
 
-	if (wrq->pointer) {
-		PKnownBSS pBSS = &(pMgmt->sBSSList[0]);
+	if (!wrq->pointer)
+		goto exit;
 
-		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;
-			qual[jj].qual = qual[jj].noise = 0;
-			qual[jj].updated = 2;
-			jj++;
-		}
+	sock = kmalloc_array(IW_MAX_AP, sizeof(struct sockaddr), GFP_KERNEL);
+	if (!sock) {
+		rc = -ENOMEM;
+		goto exit;
+	}
+
+	qual = kmalloc_array(IW_MAX_AP, sizeof(struct iw_quality), GFP_KERNEL);
+	if (!qual) {
+		rc = -ENOMEM;
+		goto exit;
+	}
+
+	for (ii = 0, jj = 0; ii < MAX_BSS_NUM; ii++) {
+		pBSS = &(pMgmt->sBSSList[ii]);
 
-		wrq->flags = 1; // Should be define'd
-		wrq->length = jj;
-		memcpy(extra, sock, sizeof(struct sockaddr)*jj);
-		memcpy(extra + sizeof(struct sockaddr)*jj, qual, sizeof(struct iw_quality)*jj);
+		if (!pBSS->bActive)
+			continue;
+		if (jj >= IW_MAX_AP)
+			break;
+
+		s = &sock[jj];
+		q = &qual[jj];
+
+		memcpy(s->sa_data, pBSS->abyBSSID, 6);
+		s->sa_family	= ARPHRD_ETHER;
+		q->level	= pBSS->uRSSI;
+		q->qual		= 0;
+		q->noise	= 0;
+		q->updated	= 2;
+		jj++;
 	}
 
+	wrq->flags = 1; /* Should be define'd */
+	wrq->length = jj;
+	memcpy(extra, sock, sizeof(struct sockaddr) * jj);
+	memcpy(extra + sizeof(struct sockaddr) * jj,
+		qual,
+		sizeof(struct iw_quality) * jj);
+exit:
+	kfree(sock);
+	kfree(qual);
 	return rc;
 }
 
@@ -778,7 +797,6 @@ int iwctl_siwessid(struct net_device *dev,
 	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) {
@@ -862,9 +880,8 @@ int iwctl_siwessid(struct net_device *dev,
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set essid = %s \n", pItemSSID->abySSID);
 	}
 
-	if (pDevice->flags & DEVICE_FLAGS_OPENED) {
+	if (pDevice->flags & DEVICE_FLAGS_OPENED)
 		pDevice->bCommit = true;
-	}
 
 	return 0;
 }
@@ -889,7 +906,6 @@ int iwctl_giwessid(struct net_device *dev,
 
 	// Get the current SSID
 	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;
@@ -1016,21 +1032,7 @@ int iwctl_giwrate(struct net_device *dev,
 				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 (pDevice->uConnectionRate == 13)
-//                brate = abySupportedRates[pDevice->wCurrentRate];
+
 		wrq->value = brate * 500000;
 		// If more than one rate, set auto
 		if (pDevice->bFixRate == true)
@@ -1058,11 +1060,11 @@ int iwctl_siwrts(struct net_device *dev,
 		int rthr = wrq->value;
 		if (wrq->disabled)
 			rthr = 2312;
-		if ((rthr < 0) || (rthr > 2312)) {
+
+		if ((rthr < 0) || (rthr > 2312))
 			rc = -EINVAL;
-		} else {
+		else
 			pDevice->wRTSThreshold = rthr;
-		}
 	}
 
 	return 0;
@@ -1162,9 +1164,8 @@ int iwctl_siwretry(struct net_device *dev,
 			pDevice->byLongRetryLimit = wrq->value;
 		}
 	}
-	if (wrq->flags & IW_RETRY_LIFETIME) {
+	if (wrq->flags & IW_RETRY_LIFETIME)
 		pDevice->wMaxTransmitMSDULifetime = wrq->value;
-	}
 
 	return rc;
 }
@@ -1234,10 +1235,13 @@ int iwctl_siwencode(struct net_device *dev,
 		}
 
 		if (dwKeyIndex < 1 && ((wrq->flags & IW_ENCODE_NOKEY) == 0)) {//set default key
-			if (pDevice->byKeyIndex < WLAN_WEP_NKEYS) {
+			if (pDevice->byKeyIndex < WLAN_WEP_NKEYS)
 				dwKeyIndex = pDevice->byKeyIndex;
-			} else dwKeyIndex = 0;
-		} else dwKeyIndex--;
+			else
+				dwKeyIndex = 0;
+		} else {
+			dwKeyIndex--;
+		}
 
 		// Check the size of the key
 		if (wrq->length > WLAN_WEP232_KEYLEN) {
@@ -1261,9 +1265,8 @@ int iwctl_siwencode(struct net_device *dev,
 			memcpy(pDevice->abyKey, extra, wrq->length);
 
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "abyKey: ");
-			for (ii = 0; ii < wrq->length; ii++) {
+			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);
@@ -1319,82 +1322,6 @@ int iwctl_siwencode(struct net_device *dev,
 	}
 //End Modify,Einsn
 
-/*
-  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;
-  }
-
-  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) {
-  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");
 		pMgmt->bShareKeyAlgorithm = true;
@@ -1406,77 +1333,6 @@ int iwctl_siwencode(struct net_device *dev,
 	return rc;
 }
 
-/*
- * 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");
-//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--;
-//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;
-
-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);
-//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;
-//  }
-
-//End Modify,Einsn
-
-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,
@@ -1491,16 +1347,17 @@ int iwctl_giwencode(struct net_device *dev,
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWENCODE\n");
 
-	if (index > WLAN_WEP_NKEYS) {
+	if (index > WLAN_WEP_NKEYS)
 		return	-EINVAL;
-	}
+
 	if (index < 1) {//get default key
-		if (pDevice->byKeyIndex < WLAN_WEP_NKEYS) {
+		if (pDevice->byKeyIndex < WLAN_WEP_NKEYS)
 			index = pDevice->byKeyIndex;
-		} else
+		else
 			index = 0;
-	} else
+	} else {
 		index--;
+	}
 
 	memset(abyKey, 0, WLAN_WEP232_KEYLEN);
 	// Check encryption mode
@@ -1657,38 +1514,36 @@ int iwctl_siwauth(struct net_device *dev,
 	switch (wrq->flags & IW_AUTH_INDEX) {
 	case IW_AUTH_WPA_VERSION:
 		wpa_version = wrq->value;
-		if (wrq->value == IW_AUTH_WPA_VERSION_DISABLED) {
+		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) {
+		else if (wrq->value == IW_AUTH_WPA_VERSION_WPA)
 			PRINT_K("iwctl_siwauth:set WPADEV to WPA1******\n");
-		} else {
+		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) {
-			//do nothing,einsn liu
-		} else pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
+		else if (pairwise == IW_AUTH_CIPHER_NONE)
+			; /* do nothing,einsn liu */
+		else
+			pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
 
 		break;
 	case IW_AUTH_CIPHER_GROUP:
 		if (wpa_version == IW_AUTH_WPA_VERSION_DISABLED)
 			break;
 		if (pairwise == IW_AUTH_CIPHER_NONE) {
-			if (wrq->value == IW_AUTH_CIPHER_CCMP) {
+			if (wrq->value == IW_AUTH_CIPHER_CCMP)
 				pDevice->eEncryptionStatus = Ndis802_11Encryption3Enabled;
-			} else {
+			else
 				pDevice->eEncryptionStatus = Ndis802_11Encryption2Enabled;
-			}
 		}
 		break;
 	case IW_AUTH_KEY_MGMT:
@@ -1696,13 +1551,15 @@ int iwctl_siwauth(struct net_device *dev,
 		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
+				pMgmt->eAuthenMode = WMAC_AUTH_WPA2;
 		} else if (wpa_version == IW_AUTH_WPA_VERSION_WPA) {
-			if (wrq->value == 0) {
+			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;
+			else
+				pMgmt->eAuthenMode = WMAC_AUTH_WPA;
 		}
 
 		break;
@@ -1711,14 +1568,13 @@ 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) {
+		if (wrq->value == IW_AUTH_ALG_OPEN_SYSTEM)
 			pMgmt->bShareKeyAlgorithm = false;
-		} else if (wrq->value == IW_AUTH_ALG_SHARED_KEY) {
+		else if (wrq->value == IW_AUTH_ALG_SHARED_KEY)
 			pMgmt->bShareKeyAlgorithm = true;
-		}
+
 		break;
 	case IW_AUTH_WPA_ENABLED:
-		//pDevice->bWPADevEnable = !! wrq->value;
 		break;
 	case IW_AUTH_RX_UNENCRYPTED_EAPOL:
 		break;
@@ -1733,7 +1589,6 @@ int iwctl_siwauth(struct net_device *dev,
 			pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
 			pMgmt->bShareKeyAlgorithm = false;
 			pMgmt->eAuthenMode = false;
-			//pDevice->bWPADevEnable = false;
 		}
 
 		break;
@@ -1741,15 +1596,7 @@ int iwctl_siwauth(struct net_device *dev,
 		ret = -EOPNOTSUPP;
 		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");
-*/
+
 	return ret;
 }
 
@@ -1808,11 +1655,12 @@ int iwctl_giwgenie(struct net_device *dev,
 	if (pMgmt->wWPAIELen > 0) {
 		wrq->length = pMgmt->wWPAIELen;
 		if (pMgmt->wWPAIELen <= space) {
-			if (copy_to_user(extra, pMgmt->abyWPAIE, pMgmt->wWPAIELen)) {
+			if (copy_to_user(extra, pMgmt->abyWPAIE, pMgmt->wWPAIELen))
 				ret = -EFAULT;
-			}
-		} else
+
+		} else {
 			ret = -E2BIG;
+		}
 	}
 
 	return ret;
@@ -1833,21 +1681,15 @@ int iwctl_siwencodeext(struct net_device *dev,
 	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;
 
 	PRINT_K("SIOCSIWENCODEEXT...... \n");
 
-	blen = sizeof(*param);
-	buf = kmalloc((int)blen, (int)GFP_KERNEL);
-	if (buf == NULL)
+	param = kzalloc(sizeof(*param), GFP_KERNEL);
+	if (param == NULL)
 		return -ENOMEM;
-	memset(buf, 0, blen);
-	param = (struct viawget_wpa_param *)buf;
 
 //recover alg_name
 	switch (ext->alg) {
@@ -1909,18 +1751,18 @@ int iwctl_siwencodeext(struct net_device *dev,
 //****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) {
+		if (param->u.wpa_key.key_index == 0)
 			pDevice->bwextcount++;
-		}
-		if ((pDevice->bwextcount == 1) && (param->u.wpa_key.key_index == 1)) {
+
+		if ((pDevice->bwextcount == 1) && (param->u.wpa_key.key_index == 1))
 			pDevice->bwextcount++;
-		}
-		if ((pDevice->bwextcount == 2) && (param->u.wpa_key.key_index == 2)) {
+
+		if ((pDevice->bwextcount == 2) && (param->u.wpa_key.key_index == 2))
 			pDevice->bwextcount++;
-		}
-		if ((pDevice->bwextcount == 3) && (param->u.wpa_key.key_index == 3)) {
+
+		if ((pDevice->bwextcount == 3) && (param->u.wpa_key.key_index == 3))
 			pDevice->bwextcount++;
-		}
+
 	}
 	if (pDevice->bwextcount == 4) {
 		printk("SIOCSIWENCODEEXT:Enable WPA WEXT SUPPORT!!!!!\n");
@@ -1954,7 +1796,6 @@ int iwctl_siwmlme(struct net_device *dev,
 	PSDevice			pDevice = (PSDevice)netdev_priv(dev);
 	PSMgmtObject	pMgmt = &(pDevice->sMgmtObj);
 	struct iw_mlme *mlme = (struct iw_mlme *)extra;
-	//u16 reason = cpu_to_le16(mlme->reason_code);
 	int ret = 0;
 
 	if (memcmp(pMgmt->abyCurrBSSID, mlme->addr.sa_data, ETH_ALEN)) {
@@ -1989,69 +1830,6 @@ int iwctl_siwmlme(struct net_device *dev,
  * 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
@@ -2129,13 +1907,9 @@ const struct iw_handler_def	iwctl_handler_def =
 {
 	.get_wireless_stats = &iwctl_get_wireless_stats,
 	.num_standard	= sizeof(iwctl_handler)/sizeof(iw_handler),
-//	.num_private	= sizeof(iwctl_private_handler)/sizeof(iw_handler),
-//	.num_private_args = sizeof(iwctl_private_args)/sizeof(struct iw_priv_args),
 	.num_private	= 0,
 	.num_private_args = 0,
 	.standard	= (iw_handler *)iwctl_handler,
-//	.private	= (iw_handler *) iwctl_private_handler,
-//	.private_args	= (struct iw_priv_args *)iwctl_private_args,
 	.private	= NULL,
 	.private_args	= NULL,
 };
diff --git a/drivers/staging/vt6655/key.c b/drivers/staging/vt6655/key.c
index 78b5809b830..09a8bf50527 100644
--- a/drivers/staging/vt6655/key.c
+++ b/drivers/staging/vt6655/key.c
@@ -46,7 +46,6 @@
 
 /*---------------------  Static Variables  --------------------------*/
 static int msglevel = MSG_LEVEL_INFO;
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 /*---------------------  Static Functions  --------------------------*/
 
 /*---------------------  Export Variables  --------------------------*/
@@ -255,11 +254,10 @@ bool KeybSetKey(
 
 			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++) {
+			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);
@@ -321,9 +319,9 @@ bool KeybSetKey(
 		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++) {
+		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);
@@ -361,9 +359,9 @@ bool KeybRemoveKey(
 	if (is_broadcast_ether_addr(pbyBSSID)) {
 		// delete all keys
 		if ((dwKeyIndex & PAIRWISE_KEY) != 0) {
-			for (i = 0; i < MAX_KEY_TABLE; i++) {
+			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) {
@@ -429,9 +427,9 @@ bool KeybRemoveAllKey(
 		if (pTable->KeyTable[i].bInUse &&
 		    ether_addr_equal(pTable->KeyTable[i].abyBSSID, pbyBSSID)) {
 			pTable->KeyTable[i].PairwiseKey.bKeyValid = false;
-			for (u = 0; u < MAX_GROUP_KEY; u++) {
+			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;
@@ -480,9 +478,8 @@ void KeyvRemoveAllWEPKey(
 {
 	int i;
 
-	for (i = 0; i < MAX_GROUP_KEY; i++) {
+	for (i = 0; i < MAX_GROUP_KEY; i++)
 		KeyvRemoveWEPKey(pTable, i, dwIoBase);
-	}
 }
 
 /*
@@ -517,9 +514,9 @@ bool KeybGetTransmitKey(
 
 					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++) {
+					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;
@@ -538,9 +535,9 @@ bool KeybGetTransmitKey(
 
 					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++) {
+					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);
 
@@ -553,9 +550,9 @@ bool KeybGetTransmitKey(
 		} // BSSID match
 	}
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ERROR: NO Match BSSID !!! ");
-	for (ii = 0; ii < 6; ii++) {
+	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;
 }
@@ -623,11 +620,10 @@ bool KeybSetDefaultKey(
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Enter KeybSetDefaultKey: %1x, %d \n", (int)dwKeyIndex, (int)uKeyLength);
 
-	if ((dwKeyIndex & PAIRWISE_KEY) != 0) {                  // Pairwise key
+	if ((dwKeyIndex & PAIRWISE_KEY) != 0) // Pairwise key
 		return false;
-	} else if ((dwKeyIndex & 0x000000FF) >= MAX_GROUP_KEY) {
+	else if ((dwKeyIndex & 0x000000FF) >= MAX_GROUP_KEY)
 		return false;
-	}
 
 	if (uKeyLength > MAX_KEY_LEN)
 		return false;
@@ -685,9 +681,9 @@ bool KeybSetDefaultKey(
 	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++) {
+	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);
@@ -731,11 +727,10 @@ bool KeybSetAllGroupKey(
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Enter KeybSetAllGroupKey: %lX\n", dwKeyIndex);
 
-	if ((dwKeyIndex & PAIRWISE_KEY) != 0) {                  // Pairwise key
+	if ((dwKeyIndex & PAIRWISE_KEY) != 0) // Pairwise key
 		return false;
-	} else if ((dwKeyIndex & 0x000000FF) >= MAX_GROUP_KEY) {
+	else if ((dwKeyIndex & 0x000000FF) >= MAX_GROUP_KEY)
 		return false;
-	}
 
 	for (i = 0; i < MAX_KEY_TABLE - 1; i++) {
 		if (pTable->KeyTable[i].bInUse) {
@@ -781,14 +776,10 @@ bool KeybSetAllGroupKey(
 			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++) {
+			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));
+			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
 
 		} // (pTable->KeyTable[i].bInUse == true)
 	}
diff --git a/drivers/staging/vt6655/key.h b/drivers/staging/vt6655/key.h
index 356e6de8f87..4b8b4b6bd6c 100644
--- a/drivers/staging/vt6655/key.h
+++ b/drivers/staging/vt6655/key.h
@@ -76,8 +76,6 @@ typedef struct tagSKeyTable
 	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
 
diff --git a/drivers/staging/vt6655/mac.c b/drivers/staging/vt6655/mac.c
index 0ec079fa039..af6876a01ee 100644
--- a/drivers/staging/vt6655/mac.c
+++ b/drivers/staging/vt6655/mac.c
@@ -74,7 +74,6 @@
 
 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 Classes  ----------------------------*/
 
@@ -642,16 +641,14 @@ void MACvSaveContext(unsigned long dwIoBase, unsigned char *pbyCxtBuf)
 	int         ii;
 
 	// read page0 register
-	for (ii = 0; ii < MAC_MAX_CONTEXT_SIZE_PAGE0; ii++) {
+	for (ii = 0; ii < MAC_MAX_CONTEXT_SIZE_PAGE0; ii++)
 		VNSvInPortB((dwIoBase + ii), (pbyCxtBuf + ii));
-	}
 
 	MACvSelectPage1(dwIoBase);
 
 	// read page1 register
-	for (ii = 0; ii < MAC_MAX_CONTEXT_SIZE_PAGE1; ii++) {
+	for (ii = 0; ii < MAC_MAX_CONTEXT_SIZE_PAGE1; ii++)
 		VNSvInPortB((dwIoBase + ii), (pbyCxtBuf + MAC_MAX_CONTEXT_SIZE_PAGE0 + ii));
-	}
 
 	MACvSelectPage0(dwIoBase);
 }
@@ -676,25 +673,24 @@ void MACvRestoreContext(unsigned long dwIoBase, unsigned char *pbyCxtBuf)
 
 	MACvSelectPage1(dwIoBase);
 	// restore page1
-	for (ii = 0; ii < MAC_MAX_CONTEXT_SIZE_PAGE1; ii++) {
+	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++) {
+	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++) {
+	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++) {
+	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));
@@ -729,24 +725,20 @@ bool MACbCompareContext(unsigned long dwIoBase, unsigned char *pbyCxtBuf)
 
 	// compare CURR_RX_DESC_ADDR, CURR_TX_DESC_ADDR
 	VNSvInPortD(dwIoBase + MAC_REG_TXDMAPTR0, &dwData);
-	if (dwData != *(unsigned long *)(pbyCxtBuf + MAC_REG_TXDMAPTR0)) {
+	if (dwData != *(unsigned long *)(pbyCxtBuf + MAC_REG_TXDMAPTR0))
 		return false;
-	}
 
 	VNSvInPortD(dwIoBase + MAC_REG_AC0DMAPTR, &dwData);
-	if (dwData != *(unsigned long *)(pbyCxtBuf + MAC_REG_AC0DMAPTR)) {
+	if (dwData != *(unsigned long *)(pbyCxtBuf + MAC_REG_AC0DMAPTR))
 		return false;
-	}
 
 	VNSvInPortD(dwIoBase + MAC_REG_RXDMAPTR0, &dwData);
-	if (dwData != *(unsigned long *)(pbyCxtBuf + MAC_REG_RXDMAPTR0)) {
+	if (dwData != *(unsigned long *)(pbyCxtBuf + MAC_REG_RXDMAPTR0))
 		return false;
-	}
 
 	VNSvInPortD(dwIoBase + MAC_REG_RXDMAPTR1, &dwData);
-	if (dwData != *(unsigned long *)(pbyCxtBuf + MAC_REG_RXDMAPTR1)) {
+	if (dwData != *(unsigned long *)(pbyCxtBuf + MAC_REG_RXDMAPTR1))
 		return false;
-	}
 
 	return true;
 }
@@ -770,7 +762,6 @@ bool MACbSoftwareReset(unsigned long dwIoBase)
 	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++) {
@@ -809,7 +800,6 @@ bool MACbSafeSoftwareReset(unsigned long dwIoBase)
 	MACvSaveContext(dwIoBase, abyTmpRegData);
 	// do reset
 	bRetVal = MACbSoftwareReset(dwIoBase);
-	//BBvSoftwareReset(pDevice->PortOffset);
 	// restore MAC context, except CR0
 	MACvRestoreContext(dwIoBase, abyTmpRegData);
 
@@ -1022,11 +1012,6 @@ void MACvInitialize(unsigned long 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);
@@ -1063,21 +1048,21 @@ void MACvSetCurrRx0DescAddr(unsigned long dwIoBase, unsigned long dwCurrDescAddr
 	unsigned char byOrgDMACtl;
 
 	VNSvInPortB(dwIoBase + MAC_REG_RXDMACTL0, &byOrgDMACtl);
-	if (byOrgDMACtl & DMACTL_RUN) {
+	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) {
+
+	if (ww == W_MAX_TIMEOUT)
 		DBG_PORT80(0x13);
-	}
+
 	VNSvOutPortD(dwIoBase + MAC_REG_RXDMAPTR0, dwCurrDescAddr);
-	if (byOrgDMACtl & DMACTL_RUN) {
+	if (byOrgDMACtl & DMACTL_RUN)
 		VNSvOutPortB(dwIoBase + MAC_REG_RXDMACTL0, DMACTL_RUN);
-	}
 }
 
 /*
@@ -1101,21 +1086,21 @@ void MACvSetCurrRx1DescAddr(unsigned long dwIoBase, unsigned long dwCurrDescAddr
 	unsigned char byOrgDMACtl;
 
 	VNSvInPortB(dwIoBase + MAC_REG_RXDMACTL1, &byOrgDMACtl);
-	if (byOrgDMACtl & DMACTL_RUN) {
+	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) {
+	if (ww == W_MAX_TIMEOUT)
 		DBG_PORT80(0x14);
-	}
+
 	VNSvOutPortD(dwIoBase + MAC_REG_RXDMAPTR1, dwCurrDescAddr);
-	if (byOrgDMACtl & DMACTL_RUN) {
+	if (byOrgDMACtl & DMACTL_RUN)
 		VNSvOutPortB(dwIoBase + MAC_REG_RXDMACTL1, DMACTL_RUN);
-	}
+
 }
 
 /*
@@ -1139,21 +1124,20 @@ void MACvSetCurrTx0DescAddrEx(unsigned long dwIoBase, unsigned long dwCurrDescAd
 	unsigned char byOrgDMACtl;
 
 	VNSvInPortB(dwIoBase + MAC_REG_TXDMACTL0, &byOrgDMACtl);
-	if (byOrgDMACtl & DMACTL_RUN) {
+	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) {
+	if (ww == W_MAX_TIMEOUT)
 		DBG_PORT80(0x25);
-	}
+
 	VNSvOutPortD(dwIoBase + MAC_REG_TXDMAPTR0, dwCurrDescAddr);
-	if (byOrgDMACtl & DMACTL_RUN) {
+	if (byOrgDMACtl & DMACTL_RUN)
 		VNSvOutPortB(dwIoBase + MAC_REG_TXDMACTL0, DMACTL_RUN);
-	}
 }
 
 /*
@@ -1178,9 +1162,9 @@ void MACvSetCurrAC0DescAddrEx(unsigned long dwIoBase, unsigned long dwCurrDescAd
 	unsigned char byOrgDMACtl;
 
 	VNSvInPortB(dwIoBase + MAC_REG_AC0DMACTL, &byOrgDMACtl);
-	if (byOrgDMACtl & DMACTL_RUN) {
+	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))
@@ -1191,18 +1175,16 @@ void MACvSetCurrAC0DescAddrEx(unsigned long dwIoBase, unsigned long dwCurrDescAd
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x26)\n");
 	}
 	VNSvOutPortD(dwIoBase + MAC_REG_AC0DMAPTR, dwCurrDescAddr);
-	if (byOrgDMACtl & DMACTL_RUN) {
+	if (byOrgDMACtl & DMACTL_RUN)
 		VNSvOutPortB(dwIoBase + MAC_REG_AC0DMACTL, DMACTL_RUN);
-	}
 }
 
 void MACvSetCurrTXDescAddr(int iTxType, unsigned long dwIoBase, unsigned long dwCurrDescAddr)
 {
-	if (iTxType == TYPE_AC0DMA) {
+	if (iTxType == TYPE_AC0DMA)
 		MACvSetCurrAC0DescAddrEx(dwIoBase, dwCurrDescAddr);
-	} else if (iTxType == TYPE_TXDMA0) {
+	else if (iTxType == TYPE_TXDMA0)
 		MACvSetCurrTx0DescAddrEx(dwIoBase, dwCurrDescAddr);
-	}
 }
 
 /*
@@ -1392,9 +1374,9 @@ bool MACbPSWakeup(unsigned long dwIoBase)
 	unsigned char byOrgValue;
 	unsigned int ww;
 	// Read PSCTL
-	if (MACbIsRegBitsOff(dwIoBase, MAC_REG_PSCTL, PSCTL_PS)) {
+	if (MACbIsRegBitsOff(dwIoBase, MAC_REG_PSCTL, PSCTL_PS))
 		return true;
-	}
+
 	// Disable PS
 	MACvRegBitsOff(dwIoBase, MAC_REG_PSCTL, PSCTL_PSEN);
 
@@ -1543,9 +1525,9 @@ void MACvSetDefaultKeyEntry(unsigned long dwIoBase, unsigned int uKeyLen,
 		VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
 	}
 	dwData = *pdwKey;
-	if (uKeyLen == WLAN_WEP104_KEYLEN) {
+	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);
diff --git a/drivers/staging/vt6655/mac.h b/drivers/staging/vt6655/mac.h
index 4615db05549..7333b8b526f 100644
--- a/drivers/staging/vt6655/mac.h
+++ b/drivers/staging/vt6655/mac.h
@@ -1042,7 +1042,6 @@ void MACvSetKeyEntry(unsigned long dwIoBase, unsigned short wKeyCtl, unsigned in
 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);
-//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);
diff --git a/drivers/staging/vt6655/mib.c b/drivers/staging/vt6655/mib.c
index 6a59652052b..36895970a89 100644
--- a/drivers/staging/vt6655/mib.c
+++ b/drivers/staging/vt6655/mib.c
@@ -187,75 +187,75 @@ void STAvUpdateRDStatCounter(PSStatCounter pStatistic,
 
 	if (byRxRate == 22) {
 		pStatistic->CustomStat.ullRsr11M++;
-		if (byRSR & RSR_CRCOK) {
+		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) {
+		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) {
+		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) {
+		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) {
+		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) {
+		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) {
+		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) {
+		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) {
+		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) {
+		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) {
+		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) {
+		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);
@@ -288,13 +288,12 @@ void STAvUpdateRDStatCounter(PSStatCounter pStatistic,
 	pStatistic->dwRsrRxPacket++;
 	pStatistic->dwRsrRxOctet += cbFrameLength;
 
-	if (IS_TYPE_DATA(pbyBuffer)) {
+	if (IS_TYPE_DATA(pbyBuffer))
 		pStatistic->dwRsrRxData++;
-	} else if (IS_TYPE_MGMT(pbyBuffer)) {
+	else if (IS_TYPE_MGMT(pbyBuffer))
 		pStatistic->dwRsrRxManage++;
-	} else if (IS_TYPE_CONTROL(pbyBuffer)) {
+	else if (IS_TYPE_CONTROL(pbyBuffer))
 		pStatistic->dwRsrRxControl++;
-	}
 
 	if (byRSR & RSR_ADDRBROAD)
 		pStatistic->dwRsrBroadcast++;
@@ -306,23 +305,22 @@ void STAvUpdateRDStatCounter(PSStatCounter pStatistic,
 	if (WLAN_GET_FC_MOREFRAG(pHeader->wFrameCtl))
 		pStatistic->dwRsrRxFragment++;
 
-	if (cbFrameLength < ETH_ZLEN + 4) {
+	if (cbFrameLength < ETH_ZLEN + 4)
 		pStatistic->dwRsrRunt++;
-	} else if (cbFrameLength == ETH_ZLEN + 4) {
+	else if (cbFrameLength == ETH_ZLEN + 4)
 		pStatistic->dwRsrRxFrmLen64++;
-	} else if ((65 <= cbFrameLength) && (cbFrameLength <= 127)) {
+	else if ((65 <= cbFrameLength) && (cbFrameLength <= 127))
 		pStatistic->dwRsrRxFrmLen65_127++;
-	} else if ((128 <= cbFrameLength) && (cbFrameLength <= 255)) {
+	else if ((128 <= cbFrameLength) && (cbFrameLength <= 255))
 		pStatistic->dwRsrRxFrmLen128_255++;
-	} else if ((256 <= cbFrameLength) && (cbFrameLength <= 511)) {
+	else if ((256 <= cbFrameLength) && (cbFrameLength <= 511))
 		pStatistic->dwRsrRxFrmLen256_511++;
-	} else if ((512 <= cbFrameLength) && (cbFrameLength <= 1023)) {
+	else if ((512 <= cbFrameLength) && (cbFrameLength <= 1023))
 		pStatistic->dwRsrRxFrmLen512_1023++;
-	} else if ((1024 <= cbFrameLength) && (cbFrameLength <= ETH_FRAME_LEN + 4)) {
+	else if ((1024 <= cbFrameLength) && (cbFrameLength <= ETH_FRAME_LEN + 4))
 		pStatistic->dwRsrRxFrmLen1024_1518++;
-	} else if (cbFrameLength > ETH_FRAME_LEN + 4) {
+	else if (cbFrameLength > ETH_FRAME_LEN + 4)
 		pStatistic->dwRsrLong++;
-	}
 }
 
 /*
@@ -399,11 +397,11 @@ STAvUpdateTDStatCounter(
 	unsigned char byTSR0_NCR = byTSR0 & TSR0_NCR;
 
 	pHeader = (PWLAN_80211HDR_A4) pbyBuffer;
-	if (WLAN_GET_FC_TODS(pHeader->wFrameCtl) == 0) {
+	if (WLAN_GET_FC_TODS(pHeader->wFrameCtl) == 0)
 		pbyDestAddr = &(pHeader->abyAddr1[0]);
-	} else {
+	else
 		pbyDestAddr = &(pHeader->abyAddr3[0]);
-	}
+
 	// increase tx packet count
 	pStatistic->dwTsrTxPacket[uIdx]++;
 	pStatistic->dwTsrTxOctet[uIdx] += cbFrameLength;
@@ -504,7 +502,6 @@ STAvUpdate802_11Counter(
 	unsigned long dwCounter
 )
 {
-	//p802_11Counter->TransmittedFragmentCount
 	p802_11Counter->MulticastTransmittedFrameCount = (unsigned long long) (pStatistic->dwTsrBroadcast[TYPE_AC0DMA] +
 									       pStatistic->dwTsrBroadcast[TYPE_TXDMA0] +
 									       pStatistic->dwTsrMulticast[TYPE_AC0DMA] +
@@ -513,12 +510,10 @@ STAvUpdate802_11Counter(
 	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);
 }
diff --git a/drivers/staging/vt6655/mib.h b/drivers/staging/vt6655/mib.h
index 6b99c119b78..c0a59489184 100644
--- a/drivers/staging/vt6655/mib.h
+++ b/drivers/staging/vt6655/mib.h
@@ -61,10 +61,6 @@ typedef struct tagSDot11Counters {
 	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;
 
 //
@@ -304,10 +300,6 @@ typedef struct tagSStatCounter {
 	unsigned long long   ullTxMulticastBytes[TYPE_MAXTD];
 	unsigned long long   ullTxDirectedBytes[TYPE_MAXTD];
 
-//    unsigned long dwTxRetryCount[8];
-	//
-	// ISR status count
-	//
 	SISRCounters ISRStat;
 
 	SCustomCounters CustomStat;
diff --git a/drivers/staging/vt6655/michael.c b/drivers/staging/vt6655/michael.c
index ade4c855f20..edee48777aa 100644
--- a/drivers/staging/vt6655/michael.c
+++ b/drivers/staging/vt6655/michael.c
@@ -47,10 +47,7 @@
 /*---------------------  Static Variables  --------------------------*/
 
 /*---------------------  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 void s_vClear(void);                       // Clear the internal message,
 // resets the object to the state just after construction.
 static void s_vSetKey(u32  dwK0, u32  dwK1);
@@ -65,30 +62,6 @@ static unsigned int nBytesInM;      // # bytes in M
 
 /*---------------------  Export Functions  --------------------------*/
 
-/*
-  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;
-}
-
-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;
-	}
-}
-*/
-
 static void s_vClear(void)
 {
 	// Reset the state to the empty message.
@@ -164,9 +137,9 @@ void MIC_vGetMIC(u32 *pdwL, u32 *pdwR)
 	s_vAppendByte(0);
 	s_vAppendByte(0);
 	// and then zeroes until the length is a multiple of 4
-	while (nBytesInM != 0) {
+	while (nBytesInM != 0)
 		s_vAppendByte(0);
-	}
+
 	// The s_vAppendByte function has already computed the result.
 	*pdwL = L;
 	*pdwR = R;
diff --git a/drivers/staging/vt6655/power.c b/drivers/staging/vt6655/power.c
index 4bd1ccb7951..5dfa911c6f4 100644
--- a/drivers/staging/vt6655/power.c
+++ b/drivers/staging/vt6655/power.c
@@ -95,14 +95,12 @@ PSvEnablePowerSaving(
 	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;
 	}
 
@@ -110,13 +108,10 @@ PSvEnablePowerSaving(
 	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) {
+	/* We don't send null pkt in ad hoc mode since beacon will handle this. */
+	if (pDevice->eOPMode != OP_MODE_ADHOC && pDevice->eOPMode == OP_MODE_INFRASTRUCTURE)
 		PSbSendNullPacket(pDevice);
-	}
+
 	pDevice->bPWBitOn = true;
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "PS:Power Saving Mode Enable... \n");
 	return;
@@ -138,7 +133,6 @@ PSvDisablePowerSaving(
 )
 {
 	PSDevice        pDevice = (PSDevice)hDeviceContext;
-//    PSMgmtObject    pMgmt = pDevice->pMgmt;
 
 	// disable power saving hw function
 	MACbPSWakeup(pDevice->PortOffset);
@@ -151,9 +145,9 @@ PSvDisablePowerSaving(
 
 	pDevice->bEnablePSMode = false;
 
-	if (pDevice->eOPMode == OP_MODE_INFRASTRUCTURE) {
+	if (pDevice->eOPMode == OP_MODE_INFRASTRUCTURE)
 		PSbSendNullPacket(pDevice);
-	}
+
 	pDevice->bPWBitOn = false;
 	return;
 }
@@ -258,8 +252,6 @@ PSvSendPSPOLL(
 	// 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;
@@ -284,17 +276,15 @@ PSbSendNullPacket(
 	PSMgmtObject        pMgmt = pDevice->pMgmt;
 	unsigned int uIdx;
 
-	if (!pDevice->bLinkPass) {
+	if (!pDevice->bLinkPass)
 		return false;
-	}
+
 #ifdef TxInSleep
-	if (!pDevice->bEnablePSMode && !pDevice->fTxDataInSleep) {
+	if (!pDevice->bEnablePSMode && !pDevice->fTxDataInSleep)
 		return false;
-	}
 #else
-	if (!pDevice->bEnablePSMode) {
+	if (!pDevice->bEnablePSMode)
 		return false;
-	}
 #endif
 	if (pDevice->bEnablePSMode) {
 		for (uIdx = 0; uIdx < TYPE_MAXTD; uIdx++) {
@@ -323,9 +313,8 @@ PSbSendNullPacket(
 ));
 	}
 
-	if (pMgmt->eCurrMode != WMAC_MODE_IBSS_STA) {
+	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);
@@ -336,8 +325,6 @@ PSbSendNullPacket(
 	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;
@@ -363,9 +350,8 @@ PSbIsNextTBTTWakeUp(
 	bool bWakeUp = false;
 
 	if (pMgmt->wListenInterval >= 2) {
-		if (pMgmt->wCountToWakeUp == 0) {
+		if (pMgmt->wCountToWakeUp == 0)
 			pMgmt->wCountToWakeUp = pMgmt->wListenInterval;
-		}
 
 		pMgmt->wCountToWakeUp--;
 
diff --git a/drivers/staging/vt6655/power.h b/drivers/staging/vt6655/power.h
index 337dd659f12..ce56124acf1 100644
--- a/drivers/staging/vt6655/power.h
+++ b/drivers/staging/vt6655/power.h
@@ -42,9 +42,6 @@
 
 /*---------------------  Export Functions  --------------------------*/
 
-// PSDevice pDevice
-// PSDevice hDeviceContext
-
 bool
 PSbConsiderPowerDown(
 	void *hDeviceContext,
diff --git a/drivers/staging/vt6655/rf.c b/drivers/staging/vt6655/rf.c
index edb1b2768b1..99c89a14d89 100644
--- a/drivers/staging/vt6655/rf.c
+++ b/drivers/staging/vt6655/rf.c
@@ -39,8 +39,6 @@
 
 /*---------------------  Static Definitions -------------------------*/
 
-//static int          msglevel                =MSG_LEVEL_INFO;
-
 #define BY_AL2230_REG_LEN     23 //24bit
 #define CB_AL2230_INIT_SEQ    15
 #define SWITCH_CHANNEL_DELAY_AL2230 200 //us
@@ -183,7 +181,6 @@ static const unsigned long dwAL7230InitTable[CB_AL7230_INIT_SEQ] = {
 	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,
@@ -192,7 +189,6 @@ static const unsigned long dwAL7230InitTable[CB_AL7230_INIT_SEQ] = {
 	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,
@@ -585,10 +581,9 @@ bool IFRFbWriteEmbedded(unsigned long dwIoBase, unsigned long dwData)
 			break;
 	}
 
-	if (ww == W_MAX_TIMEOUT) {
-//        DBG_PORT80_ALWAYS(0x32);
+	if (ww == W_MAX_TIMEOUT)
 		return false;
-	}
+
 	return true;
 }
 
@@ -839,13 +834,12 @@ bool RFvWriteWakeProgSyn(unsigned long dwIoBase, unsigned char byRFType, unsigne
 
 		byInitCount = CB_AL2230_INIT_SEQ + 2; // Init Reg + Channel Reg (2)
 		bySleepCount = 0;
-		if (byInitCount > (MISCFIFO_SYNDATASIZE - bySleepCount)) {
+		if (byInitCount > (MISCFIFO_SYNDATASIZE - bySleepCount))
 			return false;
-		}
 
-		for (ii = 0; ii < CB_AL2230_INIT_SEQ; ii++) {
+		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]);
@@ -856,18 +850,15 @@ bool RFvWriteWakeProgSyn(unsigned long dwIoBase, unsigned char byRFType, unsigne
 	case RF_AIROHA7230:
 		byInitCount = CB_AL7230_INIT_SEQ + 3; // Init Reg + Channel Reg (3)
 		bySleepCount = 0;
-		if (byInitCount > (MISCFIFO_SYNDATASIZE - bySleepCount)) {
+		if (byInitCount > (MISCFIFO_SYNDATASIZE - bySleepCount))
 			return false;
-		}
 
 		if (uChannel <= CB_MAX_CHANNEL_24G) {
-			for (ii = 0; ii < CB_AL7230_INIT_SEQ; ii++) {
+			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++) {
+			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]);
@@ -916,12 +907,11 @@ bool RFbSetPower(
 	unsigned char byDec = 0;
 	unsigned char byPwrdBm = 0;
 
-	if (pDevice->dwDiagRefCount != 0) {
+	if (pDevice->dwDiagRefCount != 0)
 		return true;
-	}
-	if ((uCH < 1) || (uCH > CB_MAX_CHANNEL)) {
+
+	if ((uCH < 1) || (uCH > CB_MAX_CHANNEL))
 		return false;
-	}
 
 	switch (uRATE) {
 	case RATE_1M:
@@ -930,22 +920,19 @@ bool RFbSetPower(
 	case RATE_11M:
 		byPwr = pDevice->abyCCKPwrTbl[uCH];
 		byPwrdBm = pDevice->abyCCKDefaultPwr[uCH];
-//PLICE_DEBUG->
-		//byPwr+=5;
-//PLICE_DEBUG <-
 		break;
 	case RATE_6M:
 	case RATE_9M:
 	case RATE_18M:
 		byPwr = pDevice->abyOFDMPwrTbl[uCH];
-		if (pDevice->byRFType == RF_UW2452) {
+		if (pDevice->byRFType == RF_UW2452)
 			byDec = byPwr + 14;
-		} else {
+		else
 			byDec = byPwr + 10;
-		}
-		if (byDec >= pDevice->byMaxPwrLevel) {
+
+		if (byDec >= pDevice->byMaxPwrLevel)
 			byDec = pDevice->byMaxPwrLevel-1;
-		}
+
 		if (pDevice->byRFType == RF_UW2452) {
 			byPwrdBm = byDec - byPwr;
 			byPwrdBm /= 3;
@@ -953,11 +940,9 @@ bool RFbSetPower(
 			byPwrdBm = byDec - byPwr;
 			byPwrdBm >>= 1;
 		}
+
 		byPwrdBm += pDevice->abyOFDMDefaultPwr[uCH];
 		byPwr = byDec;
-//PLICE_DEBUG->
-		//byPwr+=5;
-//PLICE_DEBUG<-
 		break;
 	case RATE_24M:
 	case RATE_36M:
@@ -965,20 +950,16 @@ bool RFbSetPower(
 	case RATE_54M:
 		byPwr = pDevice->abyOFDMPwrTbl[uCH];
 		byPwrdBm = pDevice->abyOFDMDefaultPwr[uCH];
-//PLICE_DEBUG->
-		//byPwr+=5;
-//PLICE_DEBUG<-
 		break;
 	}
 
-	if (pDevice->byCurPwr == byPwr) {
+	if (pDevice->byCurPwr == byPwr)
 		return true;
-	}
 
 	bResult = RFbRawSetPower(pDevice, byPwr, uRATE);
-	if (bResult) {
+	if (bResult)
 		pDevice->byCurPwr = byPwr;
-	}
+
 	return bResult;
 }
 
@@ -1005,17 +986,17 @@ bool RFbRawSetPower(
 	bool bResult = true;
 	unsigned long dwMax7230Pwr = 0;
 
-	if (byPwr >=  pDevice->byMaxPwrLevel) {
+	if (byPwr >=  pDevice->byMaxPwrLevel)
 		return false;
-	}
+
 	switch (pDevice->byRFType) {
 	case RF_AIROHA:
 		bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, dwAL2230PowerTable[byPwr]);
-		if (uRATE <= RATE_11M) {
+		if (uRATE <= RATE_11M)
 			bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, 0x0001B400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
-		} else {
+		else
 			bResult &= IFRFbWriteEmbedded(pDevice->PortOffset, 0x0005A400+(BY_AL2230_REG_LEN<<3)+IFREGCTL_REGW);
-		}
+
 		break;
 
 	case RF_AL2230S:
diff --git a/drivers/staging/vt6655/rxtx.c b/drivers/staging/vt6655/rxtx.c
index c2653eb4f76..2219d71885a 100644
--- a/drivers/staging/vt6655/rxtx.c
+++ b/drivers/staging/vt6655/rxtx.c
@@ -68,7 +68,6 @@
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 static int msglevel = MSG_LEVEL_INFO;
 
 #define	PLICE_DEBUG
@@ -230,14 +229,14 @@ s_vFillTxKey(
 		*pdwIV |= (unsigned long)byKeyIndex << 30;
 		*pdwIV = cpu_to_le32(*pdwIV);
 		pDevice->dwIVCounter++;
-		if (pDevice->dwIVCounter > WEP_IV_MASK) {
+		if (pDevice->dwIVCounter > WEP_IV_MASK)
 			pDevice->dwIVCounter = 0;
-		}
+
 	} else if (pTransmitKey->byCipherSuite == KEY_CTL_TKIP) {
 		pTransmitKey->wTSC15_0++;
-		if (pTransmitKey->wTSC15_0 == 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);
@@ -251,9 +250,9 @@ s_vFillTxKey(
 
 	} else if (pTransmitKey->byCipherSuite == KEY_CTL_CCMP) {
 		pTransmitKey->wTSC15_0++;
-		if (pTransmitKey->wTSC15_0 == 0) {
+		if (pTransmitKey->wTSC15_0 == 0)
 			pTransmitKey->dwTSC47_16++;
-		}
+
 		memcpy(pbyBuf, pTransmitKey->abyKey, 16);
 
 		// Make IV
@@ -278,11 +277,11 @@ s_vFillTxKey(
 
 		//Fill MICHDR1
 		*((unsigned char *)(pMICHDR+16)) = 0; // HLEN[15:8]
-		if (pDevice->bLongHeader) {
+		if (pDevice->bLongHeader)
 			*((unsigned char *)(pMICHDR+17)) = 28; // HLEN[7:0]
-		} else {
+		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);
@@ -294,9 +293,9 @@ s_vFillTxKey(
 		wValue &= 0x000F;
 		wValue = cpu_to_le16(wValue);
 		memcpy(pMICHDR+38, (unsigned char *)&wValue, 2); // MSKSEQCTL
-		if (pDevice->bLongHeader) {
+		if (pDevice->bLongHeader)
 			memcpy(pMICHDR+40, &(pMACHeader->abyAddr4[0]), 6);
-		}
+
 	}
 }
 
@@ -359,17 +358,15 @@ s_uGetTxRsvTime(
 	unsigned int uDataTime, uAckTime;
 
 	uDataTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, cbFrameLength, wRate);
-	if (byPktType == PK_TYPE_11B) {//llb,CCK mode
+	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
+	else //11g 2.4G OFDM mode & 11a 5G OFDM mode
 		uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, (unsigned short)pDevice->byTopOFDMBasicRate);
-	}
 
-	if (bNeedAck) {
+	if (bNeedAck)
 		return uDataTime + pDevice->uSIFS + uAckTime;
-	} else {
+	else
 		return uDataTime;
-	}
 }
 
 //byFreqType: 0=>5GHZ 1=>2.4GHZ
@@ -429,9 +426,8 @@ s_uGetDataDuration(
 	bool bLastFrag = 0;
 	unsigned int uAckTime = 0, uNextPktTime = 0;
 
-	if (uFragIdx == (uMACfragNum-1)) {
+	if (uFragIdx == (uMACfragNum-1))
 		bLastFrag = 1;
-	}
 
 	switch (byDurType) {
 	case DATADUR_B:    //DATADUR_B
@@ -443,11 +439,11 @@ s_uGetDataDuration(
 				return 0;
 			}
 		} else {//First Frag or Mid Frag
-			if (uFragIdx == (uMACfragNum-2)) {
+			if (uFragIdx == (uMACfragNum-2))
 				uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wRate, bNeedAck);
-			} else {
+			else
 				uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wRate, bNeedAck);
-			}
+
 			if (bNeedAck) {
 				uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
 				return pDevice->uSIFS + uAckTime + uNextPktTime;
@@ -466,11 +462,11 @@ s_uGetDataDuration(
 				return 0;
 			}
 		} else {//First Frag or Mid Frag
-			if (uFragIdx == (uMACfragNum-2)) {
+			if (uFragIdx == (uMACfragNum-2))
 				uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wRate, bNeedAck);
-			} else {
+			else
 				uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wRate, bNeedAck);
-			}
+
 			if (bNeedAck) {
 				uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
 				return pDevice->uSIFS + uAckTime + uNextPktTime;
@@ -495,22 +491,22 @@ s_uGetDataDuration(
 				else if (wRate > RATE_54M)
 					wRate = RATE_54M;
 
-				if (uFragIdx == (uMACfragNum-2)) {
+				if (uFragIdx == (uMACfragNum-2))
 					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wFB_Opt0[FB_RATE0][wRate-RATE_18M], bNeedAck);
-				} else {
+				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)) {
+				if (uFragIdx == (uMACfragNum-2))
 					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wFB_Opt1[FB_RATE0][wRate-RATE_18M], bNeedAck);
-				} else {
+				else
 					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE0][wRate-RATE_18M], bNeedAck);
-				}
+
 			}
 
 			if (bNeedAck) {
@@ -537,11 +533,10 @@ s_uGetDataDuration(
 				else if (wRate > RATE_54M)
 					wRate = RATE_54M;
 
-				if (uFragIdx == (uMACfragNum-2)) {
+				if (uFragIdx == (uMACfragNum-2))
 					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wFB_Opt0[FB_RATE1][wRate-RATE_18M], bNeedAck);
-				} else {
+				else
 					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE1][wRate-RATE_18M], bNeedAck);
-				}
 
 			} else { // (byFBOption == AUTO_FB_1)
 				if (wRate < RATE_18M)
@@ -549,11 +544,10 @@ s_uGetDataDuration(
 				else if (wRate > RATE_54M)
 					wRate = RATE_54M;
 
-				if (uFragIdx == (uMACfragNum-2)) {
+				if (uFragIdx == (uMACfragNum-2))
 					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wFB_Opt1[FB_RATE1][wRate-RATE_18M], bNeedAck);
-				} else {
+				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);
@@ -609,54 +603,54 @@ s_uGetRTSCTSDuration(
 
 	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)) {
+		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)) {
+		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)) {
+		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)) {
+		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)) {
+		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)) {
+		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)) {
+		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)) {
+		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)) {
+		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)) {
+		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)) {
+		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)) {
+		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:
@@ -684,9 +678,9 @@ s_uFillDataHead(
 {
 	unsigned short wLen = 0x0000;
 
-	if (pTxDataHead == NULL) {
+	if (pTxDataHead == NULL)
 		return 0;
-	}
+
 
 	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
 		if (byFBOption == AUTO_FB_NONE) {
@@ -849,11 +843,11 @@ s_vFillRTSHead(
 			} else {
 				memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
 			}
-			if (pDevice->eOPMode == OP_MODE_AP) {
+			if (pDevice->eOPMode == OP_MODE_AP)
 				memcpy(&(pBuf->Data.abyTA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
-			} else {
+			else
 				memcpy(&(pBuf->Data.abyTA[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
-			}
+
 		} else {
 			PSRTS_g_FB pBuf = (PSRTS_g_FB)pvRTS;
 			//Get SignalField,ServiceField,Length
@@ -885,11 +879,10 @@ s_vFillRTSHead(
 				memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
 			}
 
-			if (pDevice->eOPMode == OP_MODE_AP) {
+			if (pDevice->eOPMode == OP_MODE_AP)
 				memcpy(&(pBuf->Data.abyTA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
-			} else {
+			else
 				memcpy(&(pBuf->Data.abyTA[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
-			}
 
 		} // if (byFBOption == AUTO_FB_NONE)
 	} else if (byPktType == PK_TYPE_11A) {
@@ -913,11 +906,10 @@ s_vFillRTSHead(
 				memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
 			}
 
-			if (pDevice->eOPMode == OP_MODE_AP) {
+			if (pDevice->eOPMode == OP_MODE_AP)
 				memcpy(&(pBuf->Data.abyTA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
-			} else {
+			else
 				memcpy(&(pBuf->Data.abyTA[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
-			}
 
 		} else {
 			PSRTS_a_FB pBuf = (PSRTS_a_FB)pvRTS;
@@ -940,11 +932,10 @@ s_vFillRTSHead(
 			} else {
 				memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
 			}
-			if (pDevice->eOPMode == OP_MODE_AP) {
+			if (pDevice->eOPMode == OP_MODE_AP)
 				memcpy(&(pBuf->Data.abyTA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
-			} else {
+			else
 				memcpy(&(pBuf->Data.abyTA[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
-			}
 		}
 	} else if (byPktType == PK_TYPE_11B) {
 		PSRTS_ab pBuf = (PSRTS_ab)pvRTS;
@@ -966,11 +957,10 @@ s_vFillRTSHead(
 			memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
 		}
 
-		if (pDevice->eOPMode == OP_MODE_AP) {
+		if (pDevice->eOPMode == OP_MODE_AP)
 			memcpy(&(pBuf->Data.abyTA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
-		} else {
+		else
 			memcpy(&(pBuf->Data.abyTA[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
-		}
 	}
 }
 
@@ -991,9 +981,8 @@ s_vFillCTSHead(
 	unsigned int uCTSFrameLen = 14;
 	unsigned short wLen = 0x0000;
 
-	if (pvCTS == NULL) {
+	if (pvCTS == NULL)
 		return;
-	}
 
 	if (bDisCRC) {
 		// When CRCDIS bit is on, H/W forgot to generate FCS for CTS frame,
@@ -1093,22 +1082,18 @@ s_vGenerateTxParameter(
 	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) {
+	if (wFifoCtl & FIFOCTL_CRCDIS)
 		bDisCRC = true;
-	}
 
-	if (wFifoCtl & FIFOCTL_AUTO_FB_0) {
+	if (wFifoCtl & FIFOCTL_AUTO_FB_0)
 		byFBOption = AUTO_FB_0;
-	} else if (wFifoCtl & FIFOCTL_AUTO_FB_1) {
+	else if (wFifoCtl & FIFOCTL_AUTO_FB_1)
 		byFBOption = AUTO_FB_1;
-	}
 
 	if (pDevice->bLongHeader)
 		cbMACHdLen = WLAN_HDR_ADDR3_LEN + 6;
@@ -1174,13 +1159,8 @@ s_vGenerateTxParameter(
 			}
 		}
 	}
-	//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
-*/
+
 static
 void
 s_vFillFragParameter(
@@ -1193,10 +1173,8 @@ s_vFillFragParameter(
 )
 {
 	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
@@ -1204,26 +1182,21 @@ s_vFillFragParameter(
 		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
+		if (wFragType == FRAGCTL_ENDFRAG) //Last Fragmentation
 			ptdCurr->m_td1TD1.byTCR |= (TCR_STP | TCR_EDP | EDMSDU);
-		} else {
+		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
+		if (wFragType == FRAGCTL_ENDFRAG) //Last Fragmentation
 			ptdCurr->m_td1TD1.byTCR |= (TCR_STP | TCR_EDP | EDMSDU);
-		} else {
+		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
@@ -1245,8 +1218,6 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 	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;
@@ -1254,8 +1225,6 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 	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;
 	u32 dwMICKey0, dwMICKey1;
 	u32 dwMIC_Priority;
@@ -1274,7 +1243,6 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 	unsigned char *pbyType;
 	PSTxDesc       ptdCurr;
 	PSTxBufHead    psTxBufHd = (PSTxBufHead) pbyTxBufferAddr;
-//    unsigned int tmpDescIdx;
 	unsigned int cbHeaderLength = 0;
 	void *pvRrvTime;
 	PSMICHDRHead   pMICHDR;
@@ -1289,7 +1257,6 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 
 	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])))
@@ -1312,9 +1279,8 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 		if (pTransmitKey->byCipherSuite == KEY_CTL_WEP) {
 			cbIVlen = 4;
 			cbICVlen = 4;
-			if (pTransmitKey->uKeyLength == WLAN_WEP232_KEYLEN) {
+			if (pTransmitKey->uKeyLength == WLAN_WEP232_KEYLEN)
 				bIsWEP256 = true;
-			}
 		}
 		if (pTransmitKey->byCipherSuite == KEY_CTL_TKIP) {
 			cbIVlen = 8;//IV+ExtIV
@@ -1347,11 +1313,10 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 	//
 	// Use for AUTO FALL BACK
 	//
-	if (psTxBufHd->wFIFOCtl & FIFOCTL_AUTO_FB_0) {
+	if (psTxBufHd->wFIFOCtl & FIFOCTL_AUTO_FB_0)
 		byFBOption = AUTO_FB_0;
-	} else if (psTxBufHd->wFIFOCtl & FIFOCTL_AUTO_FB_1) {
+	else if (psTxBufHd->wFIFOCtl & FIFOCTL_AUTO_FB_1)
 		byFBOption = AUTO_FB_1;
-	}
 
 	//////////////////////////////////////////////////////
 	//Set RrvTime/RTS/CTS Buffer
@@ -1465,11 +1430,11 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 		//FragNum = (FrameSize-(Hdr+FCS))/(Fragment Size -(Hrd+FCS)))
 		uMACfragNum = (unsigned short) ((cbFrameBodySize + cbMIClen) / cbFragPayloadSize);
 		cbLastFragPayloadSize = (cbFrameBodySize + cbMIClen) % cbFragPayloadSize;
-		if (cbLastFragPayloadSize == 0) {
+		if (cbLastFragPayloadSize == 0)
 			cbLastFragPayloadSize = cbFragPayloadSize;
-		} else {
+		else
 			uMACfragNum++;
-		}
+
 		//[Hdr+(IV)+last fragment payload+(MIC)+(ICV)+FCS]
 		cbLastFragmentSize = cbMACHdLen + cbLastFragPayloadSize + cbIVlen + cbICVlen + cbFCSlen;
 
@@ -1567,7 +1532,6 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 				//    Last Fragmentation
 				//=========================
 				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Last Fragmentation...\n");
-				//tmpDescIdx = (uDescIdx + uFragIdx) % pDevice->cbTD[uDMAIdx];
 
 				wFragType = FRAGCTL_ENDFRAG;
 
@@ -1600,7 +1564,6 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 				//---------------------------
 
 				pbyBuffer = (unsigned char *)pHeadTD->pTDInfo->buf;
-				//pbyBuffer = (unsigned char *)pDevice->aamTxBuf[uDMAIdx][tmpDescIdx].pbyVAddr;
 
 				uLength = cbHeaderLength + cbMACHdLen + uPadding + cbIVlen;
 
@@ -1644,12 +1607,6 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 								*(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 {
@@ -1690,7 +1647,6 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 				//    Middle Fragmentation
 				//=========================
 				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Middle Fragmentation...\n");
-				//tmpDescIdx = (uDescIdx + uFragIdx) % pDevice->cbTD[uDMAIdx];
 
 				wFragType = FRAGCTL_MIDFRAG;
 
@@ -1753,21 +1709,9 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 						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:%X, %X\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));
@@ -1804,8 +1748,6 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 		//=========================
 		//    No Fragmentation
 		//=========================
-		//DBG_PRTGRP03(("No Fragmentation...\n"));
-		//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "No Fragmentation...\n");
 		wFragType = FRAGCTL_NONFRAG;
 
 		//Set FragCtl in TxBufferHead
@@ -1864,12 +1806,6 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 
 		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);
 
@@ -1888,12 +1824,6 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 			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:%x, %x\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");
-*/
 
 		}
 
@@ -1917,12 +1847,9 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 
 		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);
-
 	}
 	*puMACfragNum = uMACfragNum;
-	//DBG_PRTGRP03(("s_cbFillTxBufHead END\n"));
+
 	return cbHeaderLength;
 }
 
@@ -1973,40 +1900,37 @@ vGenerateFIFOHeader(PSDevice pDevice, unsigned char byPktType, unsigned char *pb
 	pTxBufHead->wFIFOCtl |= FIFOCTL_GENINT;
 
 	//Set FIFOCTL_ISDMA0
-	if (TYPE_TXDMA0 == uDMAIdx) {
+	if (TYPE_TXDMA0 == uDMAIdx)
 		pTxBufHead->wFIFOCtl |= FIFOCTL_ISDMA0;
-	}
 
 	//Set FRAGCTL_MACHDCNT
-	if (pDevice->bLongHeader) {
+	if (pDevice->bLongHeader)
 		cbMacHdLen = WLAN_HDR_ADDR3_LEN + 6;
-	} else {
+	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
+	if (byPktType == PK_TYPE_11A) //0000 0000 0000 0000
 		;
-	} else if (byPktType == PK_TYPE_11B) {//0000 0001 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
+	else if (byPktType == PK_TYPE_11GB) //0000 0010 0000 0000
 		pTxBufHead->wFIFOCtl |= FIFOCTL_11GB;
-	} else if (byPktType == PK_TYPE_11GA) {//0000 0011 0000 0000
+	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
+	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) {
+		if (pDevice->byAutoFBCtrl == AUTO_FB_0)
 			pTxBufHead->wFIFOCtl |= FIFOCTL_AUTO_FB_0;
-		} else if (pDevice->byAutoFBCtrl == AUTO_FB_1) {
+		else if (pDevice->byAutoFBCtrl == AUTO_FB_1)
 			pTxBufHead->wFIFOCtl |= FIFOCTL_AUTO_FB_1;
-		}
 	}
 
 	//Set FRAGCTL_WEPTYP
@@ -2031,10 +1955,6 @@ vGenerateFIFOHeader(PSDevice pDevice, unsigned char byPktType, unsigned char *pb
 #endif
 	pTxBufHead->byTxPower = pDevice->byCurPwr;
 
-/*
-  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);
@@ -2075,13 +1995,12 @@ vGenerateMACHeader(
 {
 	PS802_11Header  pMACHeader = (PS802_11Header)pbyBufferAddr;
 
-	memset(pMACHeader, 0, (sizeof(S802_11Header)));  //- sizeof(pMACHeader->dwIV)));
+	memset(pMACHeader, 0, (sizeof(S802_11Header)));
 
-	if (uDMAIdx == TYPE_ATIMDMA) {
+	if (uDMAIdx == TYPE_ATIMDMA)
 		pMACHeader->wFrameCtl = TYPE_802_11_ATIM;
-	} else {
+	else
 		pMACHeader->wFrameCtl = TYPE_802_11_DATA;
-	}
 
 	if (pDevice->eOPMode == OP_MODE_AP) {
 		memcpy(&(pMACHeader->abyAddr1[0]), &(psEthHeader->abyDstAddr[0]), ETH_ALEN);
@@ -2122,9 +2041,8 @@ vGenerateMACHeader(
 			pDevice->wSeqCounter = 0;
 	}
 
-	if ((wFragType == FRAGCTL_STAFRAG) || (wFragType == FRAGCTL_MIDFRAG)) { //StartFrag or MidFrag
+	if ((wFragType == FRAGCTL_STAFRAG) || (wFragType == FRAGCTL_MIDFRAG)) //StartFrag or MidFrag
 		pMACHeader->wFrameCtl |= FC_MOREFRAG;
-	}
 }
 
 CMD_STATUS csMgmt_xmit(PSDevice pDevice, PSTxMgmtPacket pPacket) {
@@ -2156,9 +2074,8 @@ CMD_STATUS csMgmt_xmit(PSDevice pDevice, PSTxMgmtPacket pPacket) {
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	unsigned short wCurrentRate = RATE_1M;
 
-	if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 0) {
+	if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 0)
 		return CMD_STATUS_RESOURCES;
-	}
 
 	pFrstTD = pDevice->apCurrTD[TYPE_TXDMA0];
 	pbyTxBufferAddr = (unsigned char *)pFrstTD->pTDInfo->buf;
@@ -2179,11 +2096,11 @@ CMD_STATUS csMgmt_xmit(PSDevice pDevice, PSTxMgmtPacket pPacket) {
 	// 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) {
+	if (pDevice->pMgmt->eScanState != WMAC_NO_SCANNING)
 		RFbSetPower(pDevice, wCurrentRate, pDevice->byCurrentCh);
-	} else {
+	else
 		RFbSetPower(pDevice, wCurrentRate, pMgmt->uCurrChannel);
-	}
+
 	pTxBufHead->byTxPower = pDevice->byCurPwr;
 	//+++++++++++++++++++++ Patch VT3253 A1 performance +++++++++++++++++++++++++++
 	if (pDevice->byFOETuning) {
@@ -2247,8 +2164,6 @@ CMD_STATUS csMgmt_xmit(PSDevice pDevice, PSTxMgmtPacket pPacket) {
 			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
@@ -2263,9 +2178,9 @@ CMD_STATUS csMgmt_xmit(PSDevice pDevice, PSTxMgmtPacket pPacket) {
 	cbFrameSize = cbMacHdLen + cbFrameBodySize + cbIVlen + cbMIClen + cbICVlen + cbFCSlen;
 
 	//Set FIFOCTL_GrpAckPolicy
-	if (pDevice->bGrpAckPolicy == true) {//0000 0100 0000 0000
+	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
@@ -2401,9 +2316,8 @@ CMD_STATUS csMgmt_xmit(PSDevice pDevice, PSTxMgmtPacket pPacket) {
 
 	pDevice->iTDUsed[TYPE_TXDMA0]++;
 
-	if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 1) {
+	if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 1)
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " available td0 <= 1\n");
-	}
 
 	pDevice->apCurrTD[TYPE_TXDMA0] = pFrstTD->next;
 
@@ -2559,11 +2473,10 @@ cbGetFragCount(
 		cbFragPayloadSize = cbFragmentSize - cbMACHdLen - cbIVlen - cbICVlen - cbFCSlen;
 		uMACfragNum = (unsigned short) ((cbFrameBodySize + cbMIClen) / cbFragPayloadSize);
 		cbLastFragPayloadSize = (cbFrameBodySize + cbMIClen) % cbFragPayloadSize;
-		if (cbLastFragPayloadSize == 0) {
+		if (cbLastFragPayloadSize == 0)
 			cbLastFragPayloadSize = cbFragPayloadSize;
-		} else {
+		else
 			uMACfragNum++;
-		}
 	}
 	return uMACfragNum;
 }
@@ -2613,15 +2526,14 @@ vDMA0_tx_80211(PSDevice  pDevice, struct sk_buff *skb, unsigned char *pbMPDU, un
 	unsigned char *pbyMacHdr;
 
 	unsigned int cbExtSuppRate = 0;
-//    PWLAN_IE        pItem;
 
 	pvRrvTime = pMICHDR = pvRTS = pvCTS = pvTxDataHd = NULL;
 
-	if (cbMPDULen <= WLAN_HDR_ADDR3_LEN) {
+	if (cbMPDULen <= WLAN_HDR_ADDR3_LEN)
 		cbFrameBodySize = 0;
-	} else {
+	else
 		cbFrameBodySize = cbMPDULen - WLAN_HDR_ADDR3_LEN;
-	}
+
 	p80211Header = (PUWLAN_80211HDR)pbMPDU;
 
 	pFrstTD = pDevice->apCurrTD[TYPE_TXDMA0];
@@ -2642,11 +2554,11 @@ vDMA0_tx_80211(PSDevice  pDevice, struct sk_buff *skb, unsigned char *pbMPDU, un
 	// 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) {
+	if (pDevice->pMgmt->eScanState != WMAC_NO_SCANNING)
 		RFbSetPower(pDevice, wCurrentRate, pDevice->byCurrentCh);
-	} else {
+	else
 		RFbSetPower(pDevice, wCurrentRate, pMgmt->uCurrChannel);
-	}
+
 	pTxBufHead->byTxPower = pDevice->byCurPwr;
 
 	//+++++++++++++++++++++ Patch VT3253 A1 performance +++++++++++++++++++++++++++
@@ -2704,17 +2616,14 @@ vDMA0_tx_80211(PSDevice  pDevice, struct sk_buff *skb, unsigned char *pbMPDU, un
 
 	// 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) {
+		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) {
+		if (((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates)->len != 0)
 			cbExtSuppRate += ((PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates)->len + WLAN_IEHDR_LEN;
-		}
 
-		if (cbExtSuppRate > 0) {
+		if (cbExtSuppRate > 0)
 			cbFrameBodySize = WLAN_ASSOCRESP_OFF_SUPP_RATES;
-		}
 	}
 
 	//Set FRAGCTL_MACHDCNT
@@ -2736,8 +2645,6 @@ vDMA0_tx_80211(PSDevice  pDevice, struct sk_buff *skb, unsigned char *pbMPDU, un
 			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
@@ -2753,9 +2660,9 @@ vDMA0_tx_80211(PSDevice  pDevice, struct sk_buff *skb, unsigned char *pbMPDU, un
 	cbFrameSize = cbMacHdLen + cbFrameBodySize + cbIVlen + cbMIClen + cbICVlen + cbFCSlen + cbExtSuppRate;
 
 	//Set FIFOCTL_GrpAckPolicy
-	if (pDevice->bGrpAckPolicy == true) {//0000 0100 0000 0000
+	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
@@ -2881,9 +2788,8 @@ vDMA0_tx_80211(PSDevice  pDevice, struct sk_buff *skb, unsigned char *pbMPDU, un
 			pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0 = pTransmitKey->wTSC15_0;
 		}
 
-		if ((pDevice->byLocalID <= REV_ID_VT3253_A1)) {
+		if ((pDevice->byLocalID <= REV_ID_VT3253_A1))
 			s_vSWencryption(pDevice, pTransmitKey, pbyPayloadHead, (unsigned short)(cbFrameBodySize + cbMIClen));
-		}
 	}
 
 	pMACHeader->wSeqCtl = cpu_to_le16(pDevice->wSeqCounter << 4);
@@ -2927,9 +2833,8 @@ vDMA0_tx_80211(PSDevice  pDevice, struct sk_buff *skb, unsigned char *pbMPDU, un
 
 	pDevice->iTDUsed[TYPE_TXDMA0]++;
 
-	if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 1) {
+	if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 1)
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " available td0 <= 1\n");
-	}
 
 	pDevice->apCurrTD[TYPE_TXDMA0] = pFrstTD->next;
 
diff --git a/drivers/staging/vt6655/rxtx.h b/drivers/staging/vt6655/rxtx.h
index 4f2cf34dc39..601bedb211d 100644
--- a/drivers/staging/vt6655/rxtx.h
+++ b/drivers/staging/vt6655/rxtx.h
@@ -39,17 +39,6 @@
 
 /*---------------------  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,
diff --git a/drivers/staging/vt6655/tether.c b/drivers/staging/vt6655/tether.c
index a5dc3c09cfd..1e7d3e2115a 100644
--- a/drivers/staging/vt6655/tether.c
+++ b/drivers/staging/vt6655/tether.c
@@ -98,8 +98,8 @@ bool ETHbIsBufferCrc32Ok(unsigned char *pbyBuffer, unsigned int cbFrameLength)
 	unsigned long dwCRC;
 
 	dwCRC = CRCdwGetCrc32(pbyBuffer, cbFrameLength - 4);
-	if (cpu_to_le32(*((unsigned long *)(pbyBuffer + cbFrameLength - 4))) != dwCRC) {
+	if (cpu_to_le32(*((unsigned long *)(pbyBuffer + cbFrameLength - 4))) != dwCRC)
 		return false;
-	}
+
 	return true;
 }
diff --git a/drivers/staging/vt6655/tkip.c b/drivers/staging/vt6655/tkip.c
index e7c17c6f7ea..f758d021c60 100644
--- a/drivers/staging/vt6655/tkip.c
+++ b/drivers/staging/vt6655/tkip.c
@@ -157,11 +157,11 @@ unsigned int rotr1(unsigned int a)
 {
 	unsigned int b;
 
-	if ((a & 0x01) == 0x01) {
+	if ((a & 0x01) == 0x01)
 		b = (a >> 1) | 0x8000;
-	} else {
+	else
 		b = (a >> 1) & 0x7fff;
-	}
+
 	b = b % 65536;
 	return b;
 }
@@ -189,7 +189,6 @@ void TKIPvMixKey(
 )
 {
 	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;
diff --git a/drivers/staging/vt6655/vntwifi.c b/drivers/staging/vt6655/vntwifi.c
index e78aedf9907..7d61598563c 100644
--- a/drivers/staging/vt6655/vntwifi.c
+++ b/drivers/staging/vt6655/vntwifi.c
@@ -39,8 +39,6 @@
 #include "datarate.h"
 
 /*---------------------  Static Definitions -------------------------*/
-//static int          msglevel                =MSG_LEVEL_DEBUG;
-//static int          msglevel                =MSG_LEVEL_INFO;
 
 /*---------------------  Static Classes  ----------------------------*/
 
@@ -153,9 +151,9 @@ VNTWIFIpGetCurrentChannel(
 )
 {
 	PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
-	if (pMgmtHandle != NULL) {
+	if (pMgmtHandle != NULL)
 		return pMgmt->uCurrChannel;
-	}
+
 	return 0;
 }
 
@@ -210,17 +208,17 @@ VNTWIFIbyGetMaxSupportRate(
 	if (pSupportRateIEs) {
 		for (ii = 0; ii < pSupportRateIEs->len; ii++) {
 			bySupportRate = DATARATEbyGetRateIdx(pSupportRateIEs->abyRates[ii]);
-			if (bySupportRate > byMaxSupportRate) {
+			if (bySupportRate > byMaxSupportRate)
 				byMaxSupportRate = bySupportRate;
-			}
+
 		}
 	}
 	if (pExtSupportRateIEs) {
 		for (ii = 0; ii < pExtSupportRateIEs->len; ii++) {
 			bySupportRate = DATARATEbyGetRateIdx(pExtSupportRateIEs->abyRates[ii]);
-			if (bySupportRate > byMaxSupportRate) {
+			if (bySupportRate > byMaxSupportRate)
 				byMaxSupportRate = bySupportRate;
-			}
+
 		}
 	}
 
@@ -361,11 +359,10 @@ VNTWIFIbConfigPhyMode(
 
 	if ((ePhyType != PHY_TYPE_AUTO) &&
 	    (ePhyType != pMgmt->eCurrentPHYMode)) {
-		if (CARDbSetPhyParameter(pMgmt->pAdapter, ePhyType, 0, 0, NULL, NULL) == true) {
+		if (CARDbSetPhyParameter(pMgmt->pAdapter, ePhyType, 0, 0, NULL, NULL) == true)
 			pMgmt->eCurrentPHYMode = ePhyType;
-		} else {
+		else
 			return false;
-		}
 	}
 	pMgmt->eConfigPHYMode = ePhyType;
 	return true;
@@ -379,9 +376,8 @@ VNTWIFIbGetConfigPhyMode(
 {
 	PSMgmtObject        pMgmt = (PSMgmtObject)pMgmtHandle;
 
-	if ((pMgmt != NULL) && (pePhyType != NULL)) {
+	if ((pMgmt != NULL) && (pePhyType != NULL))
 		*(PCARD_PHY_TYPE)pePhyType = pMgmt->eConfigPHYMode;
-	}
 }
 
 /*+
@@ -427,12 +423,12 @@ VNTWIFIvQueryBSSList(void *pMgmtHandle, unsigned int *puBSSCount, void **pvFirst
 
 	for (ii = 0; ii < MAX_BSS_NUM; ii++) {
 		pBSS = &(pMgmt->sBSSList[ii]);
-		if (!pBSS->bActive) {
+		if (!pBSS->bActive)
 			continue;
-		}
-		if (*pvFirstBSS == NULL) {
+
+		if (*pvFirstBSS == NULL)
 			*pvFirstBSS = &(pMgmt->sBSSList[ii]);
-		}
+
 		uCount++;
 	}
 	*puBSSCount = uCount;
@@ -452,9 +448,9 @@ VNTWIFIvGetNextBSS(
 
 	while (*pvNextBSS == NULL) {
 		pBSS++;
-		if (pBSS > &(pMgmt->sBSSList[MAX_BSS_NUM])) {
+		if (pBSS > &(pMgmt->sBSSList[MAX_BSS_NUM]))
 			return;
-		}
+
 		if (pBSS->bActive == true) {
 			*pvNextBSS = pBSS;
 			return;
@@ -489,10 +485,10 @@ VNTWIFIvUpdateNodeTxCounter(
 
 	if ((pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) ||
 	    (pMgmt->eCurrMode == WMAC_MODE_ESS_AP)) {
-		if (BSSDBbIsSTAInNodeDB(pMgmt, pbyDestAddress, &uNodeIndex) == false) {
+		if (BSSDBbIsSTAInNodeDB(pMgmt, pbyDestAddress, &uNodeIndex) == false)
 			return;
-		}
 	}
+
 	pMgmt->sNodeDBTable[uNodeIndex].uTxAttempts++;
 	if (bTxOk) {
 		// transmit success, TxAttempts at least plus one
@@ -502,9 +498,9 @@ VNTWIFIvUpdateNodeTxCounter(
 		pMgmt->sNodeDBTable[uNodeIndex].uTxFailures++;
 	}
 	pMgmt->sNodeDBTable[uNodeIndex].uTxRetry += pbyTxFailCount[MAX_RATE];
-	for (ii = 0; ii < MAX_RATE; ii++) {
+	for (ii = 0; ii < MAX_RATE; ii++)
 		pMgmt->sNodeDBTable[uNodeIndex].uTxFail[ii] += pbyTxFailCount[ii];
-	}
+
 	return;
 }
 
@@ -535,11 +531,11 @@ VNTWIFIvGetTxRate(
 			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) {
+			if (pMgmt->eCurrentPHYMode != PHY_TYPE_11A)
 				wTxDataRate = RATE_2M;
-			} else {
+			else
 				wTxDataRate = RATE_24M;
-			}
+
 			pSupportRateIEs = (PWLAN_IE_SUPP_RATES) pMgmt->abyCurrSuppRates;
 			pExtSupportRateIEs = (PWLAN_IE_SUPP_RATES) pMgmt->abyCurrExtSuppRates;
 		}
@@ -558,9 +554,9 @@ VNTWIFIvGetTxRate(
 					    pSupportRateIEs,
 					    pExtSupportRateIEs
 );
-	if (byACKRate > (unsigned char) wTxDataRate) {
+	if (byACKRate > (unsigned char) wTxDataRate)
 		byACKRate = (unsigned char) wTxDataRate;
-	}
+
 	byCCKBasicRate = VNTWIFIbyGetACKTxRate(RATE_11M,
 						pSupportRateIEs,
 						pExtSupportRateIEs
@@ -584,53 +580,12 @@ VNTWIFIbyGetKeyCypher(
 {
 	PSMgmtObject    pMgmt = (PSMgmtObject)pMgmtHandle;
 
-	if (bGroupKey) {
+	if (bGroupKey)
 		return pMgmt->byCSSGK;
-	} else {
+	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
 VNTWIFIbSetPMKIDCache(
 	void *pMgmtObject,
@@ -640,9 +595,9 @@ VNTWIFIbSetPMKIDCache(
 {
 	PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtObject;
 
-	if (ulCount > MAX_PMKID_CACHE) {
+	if (ulCount > MAX_PMKID_CACHE)
 		return false;
-	}
+
 	pMgmt->gsPMKIDCache.BSSIDInfoCount = ulCount;
 	memcpy(pMgmt->gsPMKIDCache.BSSIDInfo, pPMKIDInfo, (ulCount*sizeof(PMKIDInfo)));
 	return true;
@@ -657,15 +612,14 @@ VNTWIFIwGetMaxSupportRate(
 	PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtObject;
 
 	for (wRate = RATE_54M; wRate > RATE_1M; wRate--) {
-		if (pMgmt->sNodeDBTable[0].wSuppRate & (1<<wRate)) {
+		if (pMgmt->sNodeDBTable[0].wSuppRate & (1<<wRate))
 			return wRate;
-		}
 	}
-	if (pMgmt->eCurrentPHYMode == PHY_TYPE_11A) {
+
+	if (pMgmt->eCurrentPHYMode == PHY_TYPE_11A)
 		return RATE_6M;
-	} else {
+	else
 		return RATE_1M;
-	}
 }
 
 void
@@ -693,7 +647,6 @@ VNTWIFIbMeasureReport(
 	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))
 ) {
@@ -734,7 +687,7 @@ VNTWIFIbMeasureReport(
 	if (bEndOfReport) {
 		IEEE11hbMSRRepTx(pMgmt);
 	}
-	//spin_unlock_irq(&pDevice->lock);
+
 	return true;
 }
 
@@ -746,9 +699,7 @@ VNTWIFIbChannelSwitch(
 {
 	PSMgmtObject    pMgmt = (PSMgmtObject) pMgmtObject;
 
-	//spin_lock_irq(&pDevice->lock);
 	pMgmt->uCurrChannel = byNewChannel;
 	pMgmt->bSwitchChannel = false;
-	//spin_unlock_irq(&pDevice->lock);
 	return true;
 }
diff --git a/drivers/staging/vt6655/vntwifi.h b/drivers/staging/vt6655/vntwifi.h
index 4b01ebd7299..880b8ab109b 100644
--- a/drivers/staging/vt6655/vntwifi.h
+++ b/drivers/staging/vt6655/vntwifi.h
@@ -222,13 +222,6 @@ VNTWIFIvGetTxRate(
 	unsigned char *pbyCCKBasicRate,
 	unsigned char *pbyOFDMBasicRate
 );
-/*
-  bool
-  VNTWIFIbInit(
-  void *pAdapterHandler,
-  void **pMgmtHandler
-);
-*/
 
 unsigned char
 VNTWIFIbyGetKeyCypher(
@@ -276,12 +269,5 @@ VNTWIFIbChannelSwitch(
 	void *pMgmtObject,
 	unsigned char byNewChannel
 );
-/*
-  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 72caaa203dd..a689645fa01 100644
--- a/drivers/staging/vt6655/wcmd.c
+++ b/drivers/staging/vt6655/wcmd.c
@@ -60,7 +60,6 @@
 
 /*---------------------  Static Variables  --------------------------*/
 static int msglevel = MSG_LEVEL_INFO;
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 /*---------------------  Static Functions  --------------------------*/
 
 static
@@ -128,14 +127,13 @@ vAdHocBeaconStop(PSDevice  pDevice)
 		    (pMgmt->uScanChannel > CB_MAX_CHANNEL_24G)) {
 			bStop = true;
 		}
-		if (pMgmt->uIBSSChannel >  CB_MAX_CHANNEL_24G) {
+		if (pMgmt->uIBSSChannel >  CB_MAX_CHANNEL_24G)
 			bStop = true;
-		}
+
 	}
 
-	if (bStop) {
+	if (bStop)
 		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_TCR, TCR_AUTOBCNTX);
-	}
 } /* vAdHocBeaconStop */
 
 /*
@@ -197,13 +195,13 @@ s_vProbeChannel(
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	unsigned int ii;
 
-	if (pDevice->eCurrentPHYType == PHY_TYPE_11A) {
+	if (pDevice->eCurrentPHYType == PHY_TYPE_11A)
 		pbyRate = &abyCurrSuppRatesA[0];
-	} else if (pDevice->eCurrentPHYType == PHY_TYPE_11B) {
+	else if (pDevice->eCurrentPHYType == PHY_TYPE_11B)
 		pbyRate = &abyCurrSuppRatesB[0];
-	} else {
+	else
 		pbyRate = &abyCurrSuppRatesG[0];
-	}
+
 	// build an assocreq frame and send it
 	pTxPacket = s_MgrMakeProbeRequest
 		(
@@ -217,11 +215,10 @@ s_vProbeChannel(
 
 	if (pTxPacket != NULL) {
 		for (ii = 0; ii < 2; ii++) {
-			if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING) {
+			if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING)
 				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Probe request sending fail.. \n");
-			} else {
+			else
 				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Probe request is sending.. \n");
-			}
 		}
 	}
 }
@@ -360,11 +357,11 @@ vCommandTimer(
 			// 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) {
+			if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA)
 				CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_ADHOC);
-			} else {
+			else
 				CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_INFRASTRUCTURE);
-			}
+
 			vAdHocBeaconRestart(pDevice);
 			s_bCommandComplete(pDevice);
 
@@ -377,7 +374,6 @@ vCommandTimer(
 				return;
 			}
 			if (pMgmt->uScanChannel == pDevice->byMinChannel) {
-				//pMgmt->eScanType = WMAC_SCAN_ACTIVE;
 				pMgmt->abyScanBSSID[0] = 0xFF;
 				pMgmt->abyScanBSSID[1] = 0xFF;
 				pMgmt->abyScanBSSID[2] = 0xFF;
@@ -385,19 +381,17 @@ vCommandTimer(
 				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)) {
+			if (set_channel(pMgmt->pAdapter, pMgmt->uScanChannel))
 				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "SCAN Channel: %d\n", pMgmt->uScanChannel);
-			} else {
+			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
@@ -430,11 +424,10 @@ vCommandTimer(
 		// 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) {
+		if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA)
 			CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_ADHOC);
-		} else {
+		else
 			CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_INFRASTRUCTURE);
-		}
 
 		pMgmt->eScanState = WMAC_NO_SCANNING;
 		vAdHocBeaconRestart(pDevice);
@@ -468,7 +461,6 @@ vCommandTimer(
 			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;
@@ -480,7 +472,6 @@ vCommandTimer(
 		}
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " CARDbRadioPowerOff\n");
 		//2008-09-02  <mark>	by chester
-		// CARDbRadioPowerOff(pDevice);
 		s_bCommandComplete(pDevice);
 		break;
 
@@ -492,7 +483,6 @@ vCommandTimer(
 			return;
 		}
 //2008-09-02  <mark> by chester
-		// CARDbRadioPowerOff(pDevice);
 		s_bCommandComplete(pDevice);
 		break;
 
@@ -504,8 +494,6 @@ vCommandTimer(
 			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);
@@ -543,9 +531,9 @@ vCommandTimer(
 		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) {
+			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) {
@@ -560,9 +548,9 @@ vCommandTimer(
 		// if Adhoc mode
 		else if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
 			if (pMgmt->eCurrState == WMAC_STATE_JOINTED) {
-				if (netif_queue_stopped(pDevice->dev)) {
+				if (netif_queue_stopped(pDevice->dev))
 					netif_wake_queue(pDevice->dev);
-				}
+
 				pDevice->bLinkPass = true;
 
 				pMgmt->sNodeDBTable[0].bActive = true;
@@ -571,9 +559,9 @@ vCommandTimer(
 			} else {
 				// start own IBSS
 				vMgrCreateOwnIBSS((void *)pDevice, &Status);
-				if (Status != CMD_STATUS_SUCCESS) {
+				if (Status != CMD_STATUS_SUCCESS)
 					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " WLAN_CMD_IBSS_CREATE fail ! \n");
-				}
+
 				BSSvAddMulticastNode(pDevice);
 			}
 		}
@@ -583,13 +571,13 @@ vCommandTimer(
 			    pMgmt->eConfigMode == WMAC_CONFIG_AUTO) {
 				// start own IBSS
 				vMgrCreateOwnIBSS((void *)pDevice, &Status);
-				if (Status != CMD_STATUS_SUCCESS) {
+				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)) {
+				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");
@@ -641,12 +629,12 @@ vCommandTimer(
 	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) {
+			if (pDevice->ePSMode != WMAC_POWER_CAM)
 				PSvEnablePowerSaving((void *)pDevice, pMgmt->wListenInterval);
-			}
-			if (pMgmt->eAuthenMode >= WMAC_AUTH_WPA) {
+
+			if (pMgmt->eAuthenMode >= WMAC_AUTH_WPA)
 				KeybRemoveAllKey(&(pDevice->sKey), pDevice->abyBSSID, pDevice->PortOffset);
-			}
+
 			pDevice->bLinkPass = true;
 			pDevice->byLinkWaitCount = 0;
 			pDevice->byReAssocCount = 0;
@@ -655,9 +643,9 @@ vCommandTimer(
 				BBvSetFOE(pDevice->PortOffset);
 				PSbSendNullPacket(pDevice);
 			}
-			if (netif_queue_stopped(pDevice->dev)) {
+			if (netif_queue_stopped(pDevice->dev))
 				netif_wake_queue(pDevice->dev);
-			}
+
 #ifdef TxInSleep
 			if (pDevice->IsTxDataTrigger) {    //TxDataTimer is not triggered at the first time
 				del_timer(&pDevice->sTimerTxData);
@@ -667,8 +655,8 @@ vCommandTimer(
 				pDevice->sTimerTxData.expires = RUN_AT(10*HZ);      //10s callback
 				pDevice->fTxDataInSleep = false;
 				pDevice->nTxDataTimeCout = 0;
-			} else {
 			}
+
 			pDevice->IsTxDataTrigger = true;
 			add_timer(&pDevice->sTimerTxData);
 #endif
@@ -703,17 +691,17 @@ vCommandTimer(
 			pDevice->bFixRate = false;
 
 			vMgrCreateOwnIBSS((void *)pDevice, &Status);
-			if (Status != CMD_STATUS_SUCCESS) {
+			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)) {
+			if (netif_queue_stopped(pDevice->dev))
 				netif_wake_queue(pDevice->dev);
-			}
+
 			pDevice->bLinkPass = true;
 			add_timer(&pMgmt->sTimerSecondCallback);
 		}
@@ -730,9 +718,9 @@ vCommandTimer(
 				} else {
 					pDevice->bMoreData = true;
 				}
-				if (!device_dma0_xmit(pDevice, skb, 0)) {
+				if (!device_dma0_xmit(pDevice, skb, 0))
 					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Multicast ps tx fail \n");
-				}
+
 				pMgmt->sNodeDBTable[0].wEnQueueCnt--;
 			}
 		}
@@ -752,9 +740,9 @@ vCommandTimer(
 					} else {
 						pDevice->bMoreData = true;
 					}
-					if (!device_dma0_xmit(pDevice, skb, ii)) {
+					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.
@@ -785,7 +773,6 @@ vCommandTimer(
 		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);
@@ -820,7 +807,6 @@ s_bCommandComplete(
 {
 	PWLAN_IE_SSID pSSID;
 	bool bRadioCmd = false;
-	//unsigned short wDeAuthenReason = 0;
 	bool bForceSCAN = true;
 	PSMgmtObject  pMgmt = pDevice->pMgmt;
 
@@ -842,19 +828,11 @@ s_bCommandComplete(
 			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) {
+			if (pSSID->len != 0)
 				memcpy(pMgmt->abyScanSSID, pSSID, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-			} else {
+			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;
-  }
-  }
-*/
+
 			break;
 		case WLAN_CMD_SSID:
 			pDevice->eCommandState = WLAN_CMD_SSID_START;
@@ -900,9 +878,9 @@ bool bScheduleCommand(
 {
 	PSDevice        pDevice = (PSDevice)hDeviceContext;
 
-	if (pDevice->cbFreeCmdQueue == 0) {
+	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);
@@ -923,11 +901,6 @@ bool bScheduleCommand(
 		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_RX_PSPOLL:
 			break;
@@ -948,10 +921,9 @@ bool bScheduleCommand(
 	ADD_ONE_WITH_WRAP_AROUND(pDevice->uCmdEnqueueIdx, CMD_Q_SIZE);
 	pDevice->cbFreeCmdQueue--;
 
-	if (!pDevice->bCmdRunning) {
+	if (!pDevice->bCmdRunning)
 		s_bCommandComplete(pDevice);
-	} else {
-	}
+
 	return true;
 }
 
diff --git a/drivers/staging/vt6655/wctl.c b/drivers/staging/vt6655/wctl.c
index 950039f6d12..fddea9fc9cf 100644
--- a/drivers/staging/vt6655/wctl.c
+++ b/drivers/staging/vt6655/wctl.c
@@ -43,7 +43,7 @@
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
-// static int          msglevel                =MSG_LEVEL_INFO;
+
 /*---------------------  Static Functions  --------------------------*/
 
 /*---------------------  Export Variables  --------------------------*/
@@ -113,7 +113,6 @@ unsigned int WCTLuSearchDFCB(PSDevice pDevice, PS802_11Header pMACHeader)
 		if (pDevice->sRxDFCB[ii].bInUse &&
 		    ether_addr_equal(pDevice->sRxDFCB[ii].abyAddr2,
 				     pMACHeader->abyAddr2)) {
-			//
 			return ii;
 		}
 	}
@@ -192,9 +191,8 @@ bool WCTLbHandleFragment(PSDevice pDevice, PS802_11Header pMACHeader, unsigned i
 			pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].wFragNum = (pMACHeader->wSeqCtl & 0x000F);
 		} else {
 			pDevice->uCurrentDFCBIdx = WCTLuInsertDFCB(pDevice, pMACHeader);
-			if (pDevice->uCurrentDFCBIdx == pDevice->cbDFCB) {
+			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);
@@ -202,7 +200,6 @@ bool WCTLbHandleFragment(PSDevice pDevice, PS802_11Header pMACHeader, unsigned i
 		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);
@@ -214,7 +211,6 @@ bool WCTLbHandleFragment(PSDevice pDevice, PS802_11Header pMACHeader, unsigned i
 				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++;
@@ -228,7 +224,6 @@ bool WCTLbHandleFragment(PSDevice pDevice, PS802_11Header pMACHeader, unsigned i
 			//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/wmgr.c b/drivers/staging/vt6655/wmgr.c
index b673bc9b213..67384782b70 100644
--- a/drivers/staging/vt6655/wmgr.c
+++ b/drivers/staging/vt6655/wmgr.c
@@ -88,7 +88,6 @@
 
 /*---------------------  Static Variables  --------------------------*/
 static int msglevel = MSG_LEVEL_INFO;
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 
 /*---------------------  Static Functions  --------------------------*/
 //2008-8-4 <add> by chester
@@ -351,11 +350,10 @@ vMgrObjectInit(
 	pMgmt->pbyPSPacketPool = &pMgmt->byPSPacketPool[0];
 	pMgmt->pbyMgmtPacketPool = &pMgmt->byMgmtPacketPool[0];
 	pMgmt->uCurrChannel = pDevice->uChannel;
-	for (ii = 0; ii < WLAN_BSSID_LEN; ii++) {
+	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;
@@ -458,9 +456,9 @@ vMgrAssocBeginSta(
 
 	pMgmt->wCurrCapInfo = 0;
 	pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_ESS(1);
-	if (pDevice->bEncryptionEnable) {
+	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.
@@ -468,13 +466,11 @@ vMgrAssocBeginSta(
 	// 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)) {
+		if (CARDbIsShorSlotTime(pMgmt->pAdapter))
 			pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTSLOTTIME(1);
-		}
 	} else if (pMgmt->eCurrentPHYMode == PHY_TYPE_11B) {
-		if (CARDbIsShortPreamble(pMgmt->pAdapter)) {
+		if (CARDbIsShortPreamble(pMgmt->pAdapter))
 			pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
-		}
 	}
 	if (pMgmt->b11hEnable)
 		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SPECTRUMMNG(1);
@@ -499,8 +495,9 @@ vMgrAssocBeginSta(
 			pMgmt->eCurrState = WMAC_STATE_ASSOCPENDING;
 			*pStatus = CMD_STATUS_SUCCESS;
 		}
-	} else
+	} else {
 		*pStatus = CMD_STATUS_RESOURCES;
+	}
 
 	return;
 }
@@ -527,9 +524,8 @@ vMgrReAssocBeginSta(
 
 	pMgmt->wCurrCapInfo = 0;
 	pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_ESS(1);
-	if (pDevice->bEncryptionEnable) {
+	if (pDevice->bEncryptionEnable)
 		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_PRIVACY(1);
-	}
 
 	pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
 
@@ -539,14 +535,13 @@ vMgrReAssocBeginSta(
 	// 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)) {
+		if (CARDbIsShorSlotTime(pMgmt->pAdapter))
 			pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTSLOTTIME(1);
-		}
 	} else if (pMgmt->eCurrentPHYMode == PHY_TYPE_11B) {
-		if (CARDbIsShortPreamble(pMgmt->pAdapter)) {
+		if (CARDbIsShortPreamble(pMgmt->pAdapter))
 			pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
-		}
 	}
+
 	if (pMgmt->b11hEnable)
 		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SPECTRUMMNG(1);
 
@@ -565,11 +560,10 @@ vMgrReAssocBeginSta(
 	if (pTxPacket != NULL) {
 		/* send the frame */
 		*pStatus = csMgmt_xmit(pDevice, pTxPacket);
-		if (*pStatus != CMD_STATUS_PENDING) {
+		if (*pStatus != CMD_STATUS_PENDING)
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Reassociation tx failed.\n");
-		} else {
+		else
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Reassociation tx sending.\n");
-		}
 	}
 
 	return;
@@ -686,21 +680,20 @@ s_vMgrRxAssocRequest(
 		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) {
+		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) {
+		if (pDevice->eCurrentPHYType == PHY_TYPE_11G)
 			abyCurrExtSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)sFrame.pExtSuppRates,
 							    (PWLAN_IE_SUPP_RATES)abyCurrExtSuppRates,
 							    uRateLen);
-		} else {
+		else
 			abyCurrExtSuppRates[1] = 0;
-		}
 
 		RATEvParseMaxRate((void *)pDevice,
 				  (PWLAN_IE_SUPP_RATES)abyCurrSuppRates,
@@ -736,9 +729,8 @@ s_vMgrRxAssocRequest(
 			pDevice->bProtectMode = true;
 			pDevice->bNonERPPresent = true;
 		}
-		if (!pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble) {
+		if (!pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble)
 			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",
@@ -751,8 +743,8 @@ s_vMgrRxAssocRequest(
 			);
 		DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "Max Support rate = %d \n",
 			pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate);
-	}//else { TODO: received STA under state1 handle }
-	else {
+	} else {
+		/* TODO: received STA under state1 handle */
 		return;
 	}
 
@@ -769,17 +761,15 @@ s_vMgrRxAssocRequest(
 			(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrExtSuppRates
 );
 	if (pTxPacket != NULL) {
-		if (pDevice->bEnableHostapd) {
+		if (pDevice->bEnableHostapd)
 			return;
-		}
+
 		/* send the frame */
 		Status = csMgmt_xmit(pDevice, pTxPacket);
-		if (Status != CMD_STATUS_PENDING) {
+		if (Status != CMD_STATUS_PENDING)
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Assoc response tx failed\n");
-		} else {
+		else
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Assoc response tx sending..\n");
-		}
-
 	}
 
 	return;
@@ -839,9 +829,8 @@ s_vMgrRxReAssocRequest(
 			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) {
+		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,
@@ -891,9 +880,8 @@ s_vMgrRxReAssocRequest(
 			pDevice->bProtectMode = true;
 			pDevice->bNonERPPresent = true;
 		}
-		if (!pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble) {
+		if (!pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble)
 			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",
@@ -924,15 +912,14 @@ s_vMgrRxReAssocRequest(
 
 	if (pTxPacket != NULL) {
 		/* send the frame */
-		if (pDevice->bEnableHostapd) {
+		if (pDevice->bEnableHostapd)
 			return;
-		}
+
 		Status = csMgmt_xmit(pDevice, pTxPacket);
-		if (Status != CMD_STATUS_PENDING) {
+		if (Status != CMD_STATUS_PENDING)
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:ReAssoc response tx failed\n");
-		} else {
+		else
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:ReAssoc response tx sending..\n");
-		}
 	}
 	return;
 }
@@ -990,9 +977,9 @@ s_vMgrRxAssocResponse(
 		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)) {
+			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);
@@ -1189,9 +1176,8 @@ vMgrDeAuthenBeginSta(
 	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
 
 	*pStatus = csMgmt_xmit(pDevice, pTxPacket);
-	if (*pStatus == CMD_STATUS_PENDING) {
+	if (*pStatus == CMD_STATUS_PENDING)
 		*pStatus = CMD_STATUS_SUCCESS;
-	}
 
 	return;
 }
@@ -1340,13 +1326,13 @@ s_vMgrRxAuthenSequence_1(
 	pTxPacket->cbMPDULen = sFrame.len;
 	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
 	// send the frame
-	if (pDevice->bEnableHostapd) {
+	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) {
+	if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING)
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Authreq_reply sequence_1 tx failed.\n");
-	}
+
 	return;
 }
 
@@ -1384,11 +1370,6 @@ s_vMgrRxAuthenSequence_2(
 			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;
 
@@ -1424,17 +1405,12 @@ s_vMgrRxAuthenSequence_2(
 			pTxPacket->cbMPDULen = sFrame.len;
 			pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
 			// send the frame
-			if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING) {
+			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;
@@ -1523,12 +1499,12 @@ reply:
 	pTxPacket->cbMPDULen = sFrame.len;
 	pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
 	// send the frame
-	if (pDevice->bEnableHostapd) {
+	if (pDevice->bEnableHostapd)
 		return;
-	}
-	if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING) {
+
+	if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING)
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Authreq_reply sequence_4 tx failed.\n");
-	}
+
 	return;
 }
 
@@ -1559,12 +1535,6 @@ s_vMgrRxAuthenSequence_4(
 		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);
-	}
 }
 
 /*+
@@ -1588,7 +1558,6 @@ s_vMgrRxDisassociation(
 {
 	WLAN_FR_DISASSOC    sFrame;
 	unsigned int uNodeIndex = 0;
-//    CMD_STATUS          CmdStatus;
 	viawget_wpa_header *wpahdr;
 
 	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
@@ -1596,11 +1565,11 @@ s_vMgrRxDisassociation(
 		// a STA is leaving this BSS..
 		sFrame.len = pRxPacket->cbMPDULen;
 		sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
-		if (BSSDBbIsSTAInNodeDB(pMgmt, pRxPacket->p80211Header->sA3.abyAddr2, &uNodeIndex)) {
+		if (BSSDBbIsSTAInNodeDB(pMgmt, pRxPacket->p80211Header->sA3.abyAddr2, &uNodeIndex))
 			BSSvRemoveOneNode(pDevice, uNodeIndex);
-		} else {
+		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;
@@ -1668,11 +1637,10 @@ s_vMgrRxDeauthentication(
 		// a STA is leaving this BSS..
 		sFrame.len = pRxPacket->cbMPDULen;
 		sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
-		if (BSSDBbIsSTAInNodeDB(pMgmt, pRxPacket->p80211Header->sA3.abyAddr2, &uNodeIndex)) {
+		if (BSSDBbIsSTAInNodeDB(pMgmt, pRxPacket->p80211Header->sA3.abyAddr2, &uNodeIndex))
 			BSSvRemoveOneNode(pDevice, uNodeIndex);
-		} else {
+		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;
@@ -1871,7 +1839,6 @@ s_vMgrRxBeacon(
 				    (void *)pRxPacket
 );
 	} else {
-//        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "update bcn: RxChannel = : %d\n", byCurrChannel);
 		BSSbUpdateToBSSList((void *)pDevice,
 				    *sFrame.pqwTimestamp,
 				    *sFrame.pwBeaconInterval,
@@ -1894,9 +1861,8 @@ s_vMgrRxBeacon(
 
 	}
 
-	if (bInScan) {
+	if (bInScan)
 		return;
-	}
 
 	if (byCurrChannel == (unsigned char)pMgmt->uCurrChannel)
 		bIsChannelEqual = true;
@@ -1942,10 +1908,8 @@ s_vMgrRxBeacon(
 		pDevice->uCurrRSSI = pRxPacket->uRSSI;
 		pDevice->byCurrSQ = pRxPacket->bySQ;
 
-		if (pMgmt->sNodeDBTable[0].uInActiveCount != 0) {
+		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) {
@@ -1982,9 +1946,9 @@ s_vMgrRxBeacon(
 			//
 			// Basic Rate Set may change dynamically
 			//
-			if (pBSSList->eNetworkTypeInUse == PHY_TYPE_11B) {
+			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);
@@ -2044,22 +2008,20 @@ s_vMgrRxBeacon(
 
 	// check if beacon TSF larger or small than our local TSF
 	if (HIDWORD(qwTimestamp) == HIDWORD(qwLocalTSF)) {
-		if (LODWORD(qwTimestamp) >= LODWORD(qwLocalTSF)) {
+		if (LODWORD(qwTimestamp) >= LODWORD(qwLocalTSF))
 			bTSFOffsetPostive = true;
-		} else {
+		else
 			bTSFOffsetPostive = false;
-		}
 	} else if (HIDWORD(qwTimestamp) > HIDWORD(qwLocalTSF)) {
 		bTSFOffsetPostive = true;
 	} else if (HIDWORD(qwTimestamp) < HIDWORD(qwLocalTSF)) {
 		bTSFOffsetPostive = false;
 	}
 
-	if (bTSFOffsetPostive) {
+	if (bTSFOffsetPostive)
 		qwTSFOffset = CARDqGetTSFOffset(pRxPacket->byRxRate, (qwTimestamp), (qwLocalTSF));
-	} else {
+	else
 		qwTSFOffset = CARDqGetTSFOffset(pRxPacket->byRxRate, (qwLocalTSF), (qwTimestamp));
-	}
 
 	if (HIDWORD(qwTSFOffset) != 0 ||
 	    (LODWORD(qwTSFOffset) > TRIVIAL_SYNC_DIFFERENCE)) {
@@ -2101,11 +2063,9 @@ s_vMgrRxBeacon(
 			    (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) {
+
+				if (pMgmt->bInTIM)
 					PSvSendPSPOLL((PSDevice)pDevice);
-//                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BCN:PS-POLL sent..\n");
-				}
 
 			} else {
 				pMgmt->bInTIMWake = false;
@@ -2115,9 +2075,8 @@ s_vMgrRxBeacon(
 					if (PSbSendNullPacket(pDevice))
 						pDevice->bPWBitOn = true;
 				}
-				if (PSbConsiderPowerDown(pDevice, false, false)) {
+				if (PSbConsiderPowerDown(pDevice, false, false))
 					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BCN: Power down now...\n");
-				}
 			}
 
 		}
@@ -2181,11 +2140,6 @@ s_vMgrRxBeacon(
 					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;
-*/
 			}
 
 			// if other stations joined, indicate connection to upper layer..
@@ -2193,9 +2147,9 @@ s_vMgrRxBeacon(
 				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)) {
+				if (netif_queue_stopped(pDevice->dev))
 					netif_wake_queue(pDevice->dev);
-				}
+
 				pMgmt->sNodeDBTable[0].bActive = true;
 				pMgmt->sNodeDBTable[0].uInActiveCount = 0;
 
@@ -2230,12 +2184,8 @@ s_vMgrRxBeacon(
 						     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);
-				//  }
 			}
 		}
 	}
@@ -2365,9 +2315,8 @@ vMgrCreateOwnIBSS(
 			  &wMaxBasicRate, &wMaxSuppRate, &wSuppRate,
 			  &byTopCCKBasicRate, &byTopOFDMBasicRate);
 
-	if (pMgmt->eConfigMode == WMAC_CONFIG_AP) {
+	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);
@@ -2383,8 +2332,6 @@ vMgrCreateOwnIBSS(
 	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,
@@ -2428,9 +2375,8 @@ vMgrCreateOwnIBSS(
 		pMgmt->byDTIMCount = pMgmt->byDTIMPeriod - 1;
 	}
 
-	if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
+	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);
@@ -2453,8 +2399,6 @@ vMgrCreateOwnIBSS(
 
 	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 {
@@ -2474,11 +2418,10 @@ vMgrCreateOwnIBSS(
 	set_channel(pMgmt->pAdapter, pMgmt->uIBSSChannel);
 	pMgmt->uCurrChannel = pMgmt->uIBSSChannel;
 
-	if (CARDbIsShortPreamble(pMgmt->pAdapter)) {
+	if (CARDbIsShortPreamble(pMgmt->pAdapter))
 		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTPREAMBLE(1);
-	} else {
+	else
 		pMgmt->wCurrCapInfo &= (~WLAN_SET_CAP_INFO_SHORTPREAMBLE(1));
-	}
 
 	if (pMgmt->b11hEnable &&
 	    (pMgmt->eCurrentPHYMode == PHY_TYPE_11A)) {
@@ -2489,9 +2432,8 @@ vMgrCreateOwnIBSS(
 
 	pMgmt->eCurrState = WMAC_STATE_STARTED;
 	// Prepare beacon to send
-	if (bMgrPrepareBeaconToSend((void *)pDevice, pMgmt)) {
+	if (bMgrPrepareBeaconToSend((void *)pDevice, pMgmt))
 		*pStatus = CMD_STATUS_SUCCESS;
-	}
 
 	return;
 }
@@ -2540,7 +2482,6 @@ vMgrJoinBSSBegin(
 		return;
 	}
 
-	// memset(pMgmt->abyDesireBSSID, 0,  WLAN_BSSID_LEN);
 	// Search known BSS list for prefer BSSID or SSID
 
 	pCurr = BSSpSearchBSSList(pDevice,
@@ -2560,23 +2501,6 @@ vMgrJoinBSSBegin(
 	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;
-  }
-  }
-*/
 		}
 
 #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
@@ -2596,9 +2520,8 @@ vMgrJoinBSSBegin(
 			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) {
+			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;
@@ -2625,9 +2548,8 @@ vMgrJoinBSSBegin(
 				pItemRates->len += (unsigned char)ii;
 				if (pItemExtRates->len - ii > 0) {
 					pItemExtRates->len -= (unsigned char)ii;
-					for (uu = 0; uu < pItemExtRates->len; uu++) {
+					for (uu = 0; uu < pItemExtRates->len; uu++)
 						pItemExtRates->abyRates[uu] = pItemExtRates->abyRates[uu + ii];
-					}
 				} else {
 					pItemExtRates->len = 0;
 				}
@@ -2647,9 +2569,6 @@ vMgrJoinBSSBegin(
 			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.
@@ -2721,15 +2640,10 @@ vMgrJoinBSSBegin(
 			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;
 
 			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);
@@ -2766,7 +2680,7 @@ s_vMgrSynchBSS(
 {
 	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};
@@ -2862,9 +2776,8 @@ s_vMgrSynchBSS(
 		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) {
+		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);
 	}
@@ -2884,24 +2797,6 @@ s_vMgrSynchBSS(
 		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;
@@ -2943,7 +2838,7 @@ static void  Encyption_Rebuild(
 			}
 		}
 	}
-	//  }
+
 	return;
 }
 
@@ -2979,9 +2874,9 @@ s_vMgrFormatTIM(
 		if (!ii) {
 			// Mask out the broadcast bit which is indicated separately.
 			bMulticast = (byMap & byMask[0]) != 0;
-			if (bMulticast) {
+			if (bMulticast)
 				pMgmt->sNodeDBTable[0].bRxPSPoll = true;
-			}
+
 			byMap = 0;
 		}
 		if (byMap) {
@@ -3011,9 +2906,8 @@ s_vMgrFormatTIM(
 
 	// Append variable part of TIM
 
-	for (ii = wStartIndex, jj = 0; ii <= wEndIndex; ii++, jj++) {
+	for (ii = wStartIndex, jj = 0; ii <= wEndIndex; ii++, jj++)
 		pTIM->byVirtBitMap[jj] = pMgmt->abyPSTxMap[ii];
-	}
 
 	// Aid = 0 don't used.
 	pTIM->byVirtBitMap[0]  &= ~BIT0;
@@ -3068,9 +2962,8 @@ s_MgrMakeBeacon(
 			WLAN_SET_FC_FSTYPE(WLAN_FSTYPE_BEACON)
 ));
 
-	if (pDevice->bEnablePSMode) {
+	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);
@@ -3293,9 +3186,8 @@ s_MgrMakeProbeResponse(
 	*sFrame.pwBeaconInterval = cpu_to_le16(wCurrBeaconPeriod);
 	*sFrame.pwCapInfo = cpu_to_le16(wCurrCapInfo);
 
-	if (byPHYType == BB_TYPE_11B) {
+	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);
@@ -3538,27 +3430,27 @@ s_MgrMakeAssocRequest(
 		sFrame.pRSNWPA->abyMulticast[0] = 0x00;
 		sFrame.pRSNWPA->abyMulticast[1] = 0x50;
 		sFrame.pRSNWPA->abyMulticast[2] = 0xf2;
-		if (pMgmt->byCSSGK == KEY_CTL_WEP) {
+		if (pMgmt->byCSSGK == KEY_CTL_WEP)
 			sFrame.pRSNWPA->abyMulticast[3] = pMgmt->pCurrBSS->byGKType;
-		} else if (pMgmt->byCSSGK == KEY_CTL_TKIP) {
+		else if (pMgmt->byCSSGK == KEY_CTL_TKIP)
 			sFrame.pRSNWPA->abyMulticast[3] = WPA_TKIP;
-		} else if (pMgmt->byCSSGK == KEY_CTL_CCMP) {
+		else if (pMgmt->byCSSGK == KEY_CTL_CCMP)
 			sFrame.pRSNWPA->abyMulticast[3] = WPA_AESCCMP;
-		} else {
+		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) {
+		if (pMgmt->byCSSPK == KEY_CTL_TKIP)
 			sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_TKIP;
-		} else if (pMgmt->byCSSPK == KEY_CTL_CCMP) {
+		else if (pMgmt->byCSSPK == KEY_CTL_CCMP)
 			sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_AESCCMP;
-		} else {
+		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;
@@ -3567,13 +3459,12 @@ s_MgrMakeAssocRequest(
 
 		*pbyRSN++ = 0x50;
 		*pbyRSN++ = 0xf2;
-		if (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) {
+		if (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK)
 			*pbyRSN++ = WPA_AUTH_PSK;
-		} else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA) {
+		else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA)
 			*pbyRSN++ = WPA_AUTH_IEEE802_1X;
-		} else {
+		else
 			*pbyRSN++ = WPA_NONE;
-		}
 
 		sFrame.pRSNWPA->len += 6;
 
@@ -3604,15 +3495,14 @@ s_MgrMakeAssocRequest(
 		sFrame.pRSN->abyRSN[0] = 0x00;
 		sFrame.pRSN->abyRSN[1] = 0x0F;
 		sFrame.pRSN->abyRSN[2] = 0xAC;
-		if (pMgmt->byCSSGK == KEY_CTL_WEP) {
+		if (pMgmt->byCSSGK == KEY_CTL_WEP)
 			sFrame.pRSN->abyRSN[3] = pMgmt->pCurrBSS->byCSSGK;
-		} else if (pMgmt->byCSSGK == KEY_CTL_TKIP) {
+		else if (pMgmt->byCSSGK == KEY_CTL_TKIP)
 			sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_TKIP;
-		} else if (pMgmt->byCSSGK == KEY_CTL_CCMP) {
+		else if (pMgmt->byCSSGK == KEY_CTL_CCMP)
 			sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_CCMP;
-		} else {
+		else
 			sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_UNKNOWN;
-		}
 
 		// Pairwise Key Cipher Suite
 		sFrame.pRSN->abyRSN[4] = 1;
@@ -3620,15 +3510,15 @@ s_MgrMakeAssocRequest(
 		sFrame.pRSN->abyRSN[6] = 0x00;
 		sFrame.pRSN->abyRSN[7] = 0x0F;
 		sFrame.pRSN->abyRSN[8] = 0xAC;
-		if (pMgmt->byCSSPK == KEY_CTL_TKIP) {
+		if (pMgmt->byCSSPK == KEY_CTL_TKIP)
 			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_TKIP;
-		} else if (pMgmt->byCSSPK == KEY_CTL_CCMP) {
+		else if (pMgmt->byCSSPK == KEY_CTL_CCMP)
 			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_CCMP;
-		} else if (pMgmt->byCSSPK == KEY_CTL_NONE) {
+		else if (pMgmt->byCSSPK == KEY_CTL_NONE)
 			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_USE_GROUP;
-		} else {
+		else
 			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_UNKNOWN;
-		}
+
 		sFrame.pRSN->len += 6;
 
 		// Auth Key Management Suite
@@ -3637,13 +3527,13 @@ s_MgrMakeAssocRequest(
 		sFrame.pRSN->abyRSN[12] = 0x00;
 		sFrame.pRSN->abyRSN[13] = 0x0F;
 		sFrame.pRSN->abyRSN[14] = 0xAC;
-		if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK) {
+		if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)
 			sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_PSK;
-		} else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2) {
+		else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2)
 			sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_802_1X;
-		} else {
+		else
 			sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_UNKNOWN;
-		}
+
 		sFrame.pRSN->len += 6;
 
 		// RSN Capabilities
@@ -3668,9 +3558,8 @@ s_MgrMakeAssocRequest(
 					pbyRSN += 16;
 				}
 			}
-			if (*pwPMKID != 0) {
+			if (*pwPMKID != 0)
 				sFrame.pRSN->len += (2 + (*pwPMKID)*16);
-			}
 		}
 
 		sFrame.len += sFrame.pRSN->len + WLAN_IEHDR_LEN;
@@ -3785,27 +3674,27 @@ s_MgrMakeReAssocRequest(
 		sFrame.pRSNWPA->abyMulticast[0] = 0x00;
 		sFrame.pRSNWPA->abyMulticast[1] = 0x50;
 		sFrame.pRSNWPA->abyMulticast[2] = 0xf2;
-		if (pMgmt->byCSSGK == KEY_CTL_WEP) {
+		if (pMgmt->byCSSGK == KEY_CTL_WEP)
 			sFrame.pRSNWPA->abyMulticast[3] = pMgmt->pCurrBSS->byGKType;
-		} else if (pMgmt->byCSSGK == KEY_CTL_TKIP) {
+		else if (pMgmt->byCSSGK == KEY_CTL_TKIP)
 			sFrame.pRSNWPA->abyMulticast[3] = WPA_TKIP;
-		} else if (pMgmt->byCSSGK == KEY_CTL_CCMP) {
+		else if (pMgmt->byCSSGK == KEY_CTL_CCMP)
 			sFrame.pRSNWPA->abyMulticast[3] = WPA_AESCCMP;
-		} else {
+		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) {
+		if (pMgmt->byCSSPK == KEY_CTL_TKIP)
 			sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_TKIP;
-		} else if (pMgmt->byCSSPK == KEY_CTL_CCMP) {
+		else if (pMgmt->byCSSPK == KEY_CTL_CCMP)
 			sFrame.pRSNWPA->PKSList[0].abyOUI[3] = WPA_AESCCMP;
-		} else {
+		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;
@@ -3814,13 +3703,12 @@ s_MgrMakeReAssocRequest(
 
 		*pbyRSN++ = 0x50;
 		*pbyRSN++ = 0xf2;
-		if (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) {
+		if (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK)
 			*pbyRSN++ = WPA_AUTH_PSK;
-		} else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA) {
+		else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA)
 			*pbyRSN++ = WPA_AUTH_IEEE802_1X;
-		} else {
+		else
 			*pbyRSN++ = WPA_NONE;
-		}
 
 		sFrame.pRSNWPA->len += 6;
 
@@ -3850,15 +3738,14 @@ s_MgrMakeReAssocRequest(
 		sFrame.pRSN->abyRSN[0] = 0x00;
 		sFrame.pRSN->abyRSN[1] = 0x0F;
 		sFrame.pRSN->abyRSN[2] = 0xAC;
-		if (pMgmt->byCSSGK == KEY_CTL_WEP) {
+		if (pMgmt->byCSSGK == KEY_CTL_WEP)
 			sFrame.pRSN->abyRSN[3] = pMgmt->pCurrBSS->byCSSGK;
-		} else if (pMgmt->byCSSGK == KEY_CTL_TKIP) {
+		else if (pMgmt->byCSSGK == KEY_CTL_TKIP)
 			sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_TKIP;
-		} else if (pMgmt->byCSSGK == KEY_CTL_CCMP) {
+		else if (pMgmt->byCSSGK == KEY_CTL_CCMP)
 			sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_CCMP;
-		} else {
+		else
 			sFrame.pRSN->abyRSN[3] = WLAN_11i_CSS_UNKNOWN;
-		}
 
 		// Pairwise Key Cipher Suite
 		sFrame.pRSN->abyRSN[4] = 1;
@@ -3866,15 +3753,15 @@ s_MgrMakeReAssocRequest(
 		sFrame.pRSN->abyRSN[6] = 0x00;
 		sFrame.pRSN->abyRSN[7] = 0x0F;
 		sFrame.pRSN->abyRSN[8] = 0xAC;
-		if (pMgmt->byCSSPK == KEY_CTL_TKIP) {
+		if (pMgmt->byCSSPK == KEY_CTL_TKIP)
 			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_TKIP;
-		} else if (pMgmt->byCSSPK == KEY_CTL_CCMP) {
+		else if (pMgmt->byCSSPK == KEY_CTL_CCMP)
 			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_CCMP;
-		} else if (pMgmt->byCSSPK == KEY_CTL_NONE) {
+		else if (pMgmt->byCSSPK == KEY_CTL_NONE)
 			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_USE_GROUP;
-		} else {
+		else
 			sFrame.pRSN->abyRSN[9] = WLAN_11i_CSS_UNKNOWN;
-		}
+
 		sFrame.pRSN->len += 6;
 
 		// Auth Key Management Suite
@@ -3883,13 +3770,13 @@ s_MgrMakeReAssocRequest(
 		sFrame.pRSN->abyRSN[12] = 0x00;
 		sFrame.pRSN->abyRSN[13] = 0x0F;
 		sFrame.pRSN->abyRSN[14] = 0xAC;
-		if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK) {
+		if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)
 			sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_PSK;
-		} else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2) {
+		else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2)
 			sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_802_1X;
-		} else {
+		else
 			sFrame.pRSN->abyRSN[15] = WLAN_11i_AKMSS_UNKNOWN;
-		}
+
 		sFrame.pRSN->len += 6;
 
 		// RSN Capabilities
@@ -3914,9 +3801,9 @@ s_MgrMakeReAssocRequest(
 					pbyRSN += 16;
 				}
 			}
-			if (*pwPMKID != 0) {
+
+			if (*pwPMKID != 0)
 				sFrame.pRSN->len += (2 + (*pwPMKID) * 16);
-			}
 		}
 
 		sFrame.len += sFrame.pRSN->len + WLAN_IEHDR_LEN;
@@ -4228,10 +4115,7 @@ s_vMgrRxProbeRequest(
 		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);
-*/
+
 		if (sFrame.pSSID->len != 0) {
 			if (sFrame.pSSID->len != ((PWLAN_IE_SSID)pMgmt->abyCurrSSID)->len)
 				return;
@@ -4242,9 +4126,8 @@ s_vMgrRxProbeRequest(
 			}
 		}
 
-		if ((sFrame.pSuppRates->len > 4) || (sFrame.pExtSuppRates != NULL)) {
+		if ((sFrame.pSuppRates->len > 4) || (sFrame.pExtSuppRates != NULL))
 			byPHYType = BB_TYPE_11G;
-		}
 
 		// Probe response reply..
 		pTxPacket = s_MgrMakeProbeResponse
@@ -4267,8 +4150,6 @@ s_vMgrRxProbeRequest(
 			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");
 			}
 		}
 	}
@@ -4361,7 +4242,6 @@ vMgrRxManagePacket(
 
 	case WLAN_FSTYPE_PROBEREQ:
 		// Frame Clase = 0
-		//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx probereq\n");
 		s_vMgrRxProbeRequest(pDevice, pMgmt, pRxPacket);
 		break;
 
@@ -4374,10 +4254,9 @@ vMgrRxManagePacket(
 
 	case WLAN_FSTYPE_BEACON:
 		// Frame Clase = 0
-		//DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx beacon\n");
-		if (pMgmt->eScanState != WMAC_NO_SCANNING) {
+		if (pMgmt->eScanState != WMAC_NO_SCANNING)
 			bInScan = true;
-		}
+
 		s_vMgrRxBeacon(pDevice, pMgmt, pRxPacket, bInScan);
 		break;
 
@@ -4442,12 +4321,11 @@ bMgrPrepareBeaconToSend(
 	PSDevice            pDevice = (PSDevice)hDeviceContext;
 	PSTxMgmtPacket      pTxPacket;
 
-//    pDevice->bBeaconBufReady = false;
-	if (pDevice->bEncryptionEnable || pDevice->bEnable8021x) {
+	if (pDevice->bEncryptionEnable || pDevice->bEnable8021x)
 		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_PRIVACY(1);
-	} else {
+	else
 		pMgmt->wCurrCapInfo &= ~WLAN_SET_CAP_INFO_PRIVACY(1);
-	}
+
 	pTxPacket = s_MgrMakeBeacon
 		(
 			pDevice,
@@ -4455,7 +4333,7 @@ bMgrPrepareBeaconToSend(
 			pMgmt->wCurrCapInfo,
 			pMgmt->wCurrBeaconPeriod,
 			pMgmt->uCurrChannel,
-			pMgmt->wCurrATIMWindow, //0,
+			pMgmt->wCurrATIMWindow,
 			(PWLAN_IE_SSID)pMgmt->abyCurrSSID,
 			(unsigned char *)pMgmt->abyCurrBSSID,
 			(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
@@ -4575,22 +4453,22 @@ bAdd_PMKID_Candidate(
 	for (ii = 0; ii < pDevice->gsPMKIDCandidate.NumCandidates; ii++) {
 		pCandidateList = &pDevice->gsPMKIDCandidate.CandidateList[ii];
 		if (!memcmp(pCandidateList->BSSID, pbyBSSID, ETH_ALEN)) {
-			if (psRSNCapObj->bRSNCapExist && (psRSNCapObj->wRSNCap & BIT0)) {
+			if (psRSNCapObj->bRSNCapExist && (psRSNCapObj->wRSNCap & BIT0))
 				pCandidateList->Flags |= NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED;
-			} else {
+			else
 				pCandidateList->Flags &= ~(NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED);
-			}
+
 			return true;
 		}
 	}
 
 	// New Candidate
 	pCandidateList = &pDevice->gsPMKIDCandidate.CandidateList[pDevice->gsPMKIDCandidate.NumCandidates];
-	if (psRSNCapObj->bRSNCapExist && (psRSNCapObj->wRSNCap & BIT0)) {
+	if (psRSNCapObj->bRSNCapExist && (psRSNCapObj->wRSNCap & BIT0))
 		pCandidateList->Flags |= NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED;
-	} else {
+	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);
diff --git a/drivers/staging/vt6655/wmgr.h b/drivers/staging/vt6655/wmgr.h
index b91f1f89ed0..2312d71bac4 100644
--- a/drivers/staging/vt6655/wmgr.h
+++ b/drivers/staging/vt6655/wmgr.h
@@ -119,32 +119,6 @@ typedef struct tagSAssocInfo {
 } 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;
-*/
-
-// Pre-configured Mode (from XP)
-/*
-  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;
-*/
-
 typedef enum tagWMAC_SCAN_TYPE {
 	WMAC_SCAN_ACTIVE,
 	WMAC_SCAN_PASSIVE,
@@ -232,9 +206,6 @@ typedef struct tagSMgmtObject
 	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];
@@ -256,7 +227,6 @@ typedef struct tagSMgmtObject
 	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;
diff --git a/drivers/staging/vt6655/wpa.c b/drivers/staging/vt6655/wpa.c
index 990ea0f9e9f..9be59c23d7e 100644
--- a/drivers/staging/vt6655/wpa.c
+++ b/drivers/staging/vt6655/wpa.c
@@ -160,8 +160,7 @@ WPA_ParseRSN(
 						;
 				} 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);
 		}
@@ -187,7 +186,7 @@ WPA_ParseRSN(
 						;
 				} else
 					break;
-				//DBG_PRN_GRP14(("abyAuthType[%d]: %X\n", j-1, pBSSList->abyAuthType[j-1]));
+
 			}
 			if (j > 0)
 				pBSSList->wAuthCount = (unsigned short)j;
@@ -206,9 +205,6 @@ WPA_ParseRSN(
 				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;
diff --git a/drivers/staging/vt6655/wpa2.c b/drivers/staging/vt6655/wpa2.c
index 2013122e92b..4e1b63be380 100644
--- a/drivers/staging/vt6655/wpa2.c
+++ b/drivers/staging/vt6655/wpa2.c
@@ -37,7 +37,6 @@
 
 /*---------------------  Static Definitions -------------------------*/
 static int msglevel = MSG_LEVEL_INFO;
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
@@ -122,9 +121,9 @@ WPA2vParseRSN(
 	WPA2_ClearRSN(pBSSNode);
 
 	if (pRSN->len == 2) { // ver(2)
-		if ((pRSN->byElementID == WLAN_EID_RSN) && (pRSN->wVersion == 1)) {
+		if ((pRSN->byElementID == WLAN_EID_RSN) && (pRSN->wVersion == 1))
 			pBSSNode->bWPA2Valid = true;
-		}
+
 		return;
 	}
 
@@ -268,9 +267,9 @@ WPA2uSetIEs(
 	unsigned int ii = 0;
 	unsigned short *pwPMKID = NULL;
 
-	if (pRSNIEs == NULL) {
+	if (pRSNIEs == NULL)
 		return 0;
-	}
+
 	if (((pMgmt->eAuthenMode == WMAC_AUTH_WPA2) ||
 	     (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) &&
 	    (pMgmt->pCurrBSS != NULL)) {
@@ -283,15 +282,14 @@ WPA2uSetIEs(
 		pRSNIEs->abyRSN[0] = 0x00;
 		pRSNIEs->abyRSN[1] = 0x0F;
 		pRSNIEs->abyRSN[2] = 0xAC;
-		if (pMgmt->byCSSGK == KEY_CTL_WEP) {
+		if (pMgmt->byCSSGK == KEY_CTL_WEP)
 			pRSNIEs->abyRSN[3] = pMgmt->pCurrBSS->byCSSGK;
-		} else if (pMgmt->byCSSGK == KEY_CTL_TKIP) {
+		else if (pMgmt->byCSSGK == KEY_CTL_TKIP)
 			pRSNIEs->abyRSN[3] = WLAN_11i_CSS_TKIP;
-		} else if (pMgmt->byCSSGK == KEY_CTL_CCMP) {
+		else if (pMgmt->byCSSGK == KEY_CTL_CCMP)
 			pRSNIEs->abyRSN[3] = WLAN_11i_CSS_CCMP;
-		} else {
+		else
 			pRSNIEs->abyRSN[3] = WLAN_11i_CSS_UNKNOWN;
-		}
 
 		// Pairwise Key Cipher Suite
 		pRSNIEs->abyRSN[4] = 1;
@@ -299,15 +297,15 @@ WPA2uSetIEs(
 		pRSNIEs->abyRSN[6] = 0x00;
 		pRSNIEs->abyRSN[7] = 0x0F;
 		pRSNIEs->abyRSN[8] = 0xAC;
-		if (pMgmt->byCSSPK == KEY_CTL_TKIP) {
+		if (pMgmt->byCSSPK == KEY_CTL_TKIP)
 			pRSNIEs->abyRSN[9] = WLAN_11i_CSS_TKIP;
-		} else if (pMgmt->byCSSPK == KEY_CTL_CCMP) {
+		else if (pMgmt->byCSSPK == KEY_CTL_CCMP)
 			pRSNIEs->abyRSN[9] = WLAN_11i_CSS_CCMP;
-		} else if (pMgmt->byCSSPK == KEY_CTL_NONE) {
+		else if (pMgmt->byCSSPK == KEY_CTL_NONE)
 			pRSNIEs->abyRSN[9] = WLAN_11i_CSS_USE_GROUP;
-		} else {
+		else
 			pRSNIEs->abyRSN[9] = WLAN_11i_CSS_UNKNOWN;
-		}
+
 		pRSNIEs->len += 6;
 
 		// Auth Key Management Suite
@@ -316,13 +314,13 @@ WPA2uSetIEs(
 		pRSNIEs->abyRSN[12] = 0x00;
 		pRSNIEs->abyRSN[13] = 0x0F;
 		pRSNIEs->abyRSN[14] = 0xAC;
-		if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK) {
+		if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)
 			pRSNIEs->abyRSN[15] = WLAN_11i_AKMSS_PSK;
-		} else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2) {
+		else if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2)
 			pRSNIEs->abyRSN[15] = WLAN_11i_AKMSS_802_1X;
-		} else {
+		else
 			pRSNIEs->abyRSN[15] = WLAN_11i_AKMSS_UNKNOWN;
-		}
+
 		pRSNIEs->len += 6;
 
 		// RSN Capabilities
@@ -348,11 +346,10 @@ WPA2uSetIEs(
 					pbyBuffer += 16;
 				}
 			}
-			if (*pwPMKID != 0) {
+			if (*pwPMKID != 0)
 				pRSNIEs->len += (2 + (*pwPMKID)*16);
-			} else {
+			else
 				pbyBuffer = &pRSNIEs->abyRSN[18];
-			}
 		}
 		return pRSNIEs->len + WLAN_IEHDR_LEN;
 	}
diff --git a/drivers/staging/vt6655/wpactl.c b/drivers/staging/vt6655/wpactl.c
index d17224f39b4..8392d4d1d5e 100644
--- a/drivers/staging/vt6655/wpactl.c
+++ b/drivers/staging/vt6655/wpactl.c
@@ -51,7 +51,6 @@ static const int frequency_list[] = {
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 static int msglevel = MSG_LEVEL_INFO;
 
 /*---------------------  Static Functions  --------------------------*/
@@ -188,7 +187,8 @@ 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) __must_hold(&pDevice->lock)
 {
 	struct viawget_wpa_param *param = ctx;
 	PSMgmtObject pMgmt = pDevice->pMgmt;
@@ -196,7 +196,6 @@ int wpa_set_keys(PSDevice pDevice, void *ctx, bool fcpfkernel)
 	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;
 	int ret = 0;
 	int uu, ii;
@@ -213,13 +212,12 @@ int wpa_set_keys(PSDevice pDevice, void *ctx, bool fcpfkernel)
 		pDevice->byKeyIndex = 0;
 		pDevice->bTransmitKey = false;
 		KeyvRemoveAllWEPKey(&(pDevice->sKey), pDevice->PortOffset);
-		for (uu = 0; uu < MAX_KEY_TABLE; uu++) {
+		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 {
@@ -258,7 +256,6 @@ int wpa_set_keys(PSDevice pDevice, void *ctx, bool fcpfkernel)
 		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 {
@@ -277,7 +274,6 @@ int wpa_set_keys(PSDevice pDevice, void *ctx, bool fcpfkernel)
 				LODWORD(KeyRSC) |= (abySeq[ii] << (ii * 8));
 			else
 				HIDWORD(KeyRSC) |= (abySeq[ii] << ((ii-4) * 8));
-			//KeyRSC |= (abySeq[ii] << (ii * 8));
 		}
 		dwKeyIndex |= 1 << 29;
 	}
@@ -287,13 +283,11 @@ int wpa_set_keys(PSDevice pDevice, void *ctx, bool fcpfkernel)
 		return -EINVAL;
 	}
 
-	if (param->u.wpa_key.alg_name == WPA_ALG_TKIP) {
+	if (param->u.wpa_key.alg_name == WPA_ALG_TKIP)
 		pDevice->eEncryptionStatus = Ndis802_11Encryption2Enabled;
-	}
 
-	if (param->u.wpa_key.alg_name == WPA_ALG_CCMP) {
+	if (param->u.wpa_key.alg_name == WPA_ALG_CCMP)
 		pDevice->eEncryptionStatus = Ndis802_11Encryption3Enabled;
-	}
 
 	if (param->u.wpa_key.set_tx)
 		dwKeyIndex |= (1 << 31);
@@ -324,7 +318,6 @@ int wpa_set_keys(PSDevice pDevice, void *ctx, bool fcpfkernel)
 	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;
 	}
@@ -332,7 +325,6 @@ int wpa_set_keys(PSDevice pDevice, void *ctx, bool fcpfkernel)
 	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;
 	}
 
@@ -360,8 +352,6 @@ int wpa_set_keys(PSDevice pDevice, void *ctx, bool fcpfkernel)
 			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;
 		}
 
@@ -371,16 +361,13 @@ int wpa_set_keys(PSDevice pDevice, void *ctx, bool fcpfkernel)
 		// 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);
+		if (pMgmt->eConfigMode == WMAC_CONFIG_IBSS_STA)
 			return -EINVAL;
-		}
+
 		if (KeybSetKey(&(pDevice->sKey),
 			       &param->addr[0],
 			       dwKeyIndex,
@@ -394,17 +381,7 @@ int wpa_set_keys(PSDevice pDevice, void *ctx, bool fcpfkernel)
 
 		} else {
 			// Key Table Full
-			if (ether_addr_equal(param->addr, 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;
-			}
+			return -EINVAL;
 		}
 	} // BSSID not 0xffffffffffff
 	if ((ret == 0) && ((param->u.wpa_key.set_tx) != 0)) {
@@ -412,17 +389,6 @@ int wpa_set_keys(PSDevice pDevice, void *ctx, bool fcpfkernel)
 		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]
-);
-*/
 
 	return ret;
 }
@@ -445,12 +411,11 @@ static int wpa_set_wpa(PSDevice pDevice,
 		       struct viawget_wpa_param *param)
 {
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	int ret = 0;
 
 	pMgmt->eAuthenMode = WMAC_AUTH_OPEN;
 	pMgmt->bShareKeyAlgorithm = false;
 
-	return ret;
+	return 0;
 }
 
 /*
@@ -471,7 +436,6 @@ static int wpa_set_disassociate(PSDevice pDevice,
 				struct viawget_wpa_param *param)
 {
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
-	int ret = 0;
 
 	spin_lock_irq(&pDevice->lock);
 	if (pDevice->bLinkPass) {
@@ -480,7 +444,7 @@ static int wpa_set_disassociate(PSDevice pDevice,
 	}
 	spin_unlock_irq(&pDevice->lock);
 
-	return ret;
+	return 0;
 }
 
 /*
@@ -500,14 +464,12 @@ static int wpa_set_disassociate(PSDevice pDevice,
 static int wpa_set_scan(PSDevice pDevice,
 			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);
 
-	return ret;
+	return 0;
 }
 
 /*
@@ -528,11 +490,10 @@ static int wpa_get_bssid(PSDevice pDevice,
 			 struct viawget_wpa_param *param)
 {
 	PSMgmtObject        pMgmt = pDevice->pMgmt;
-	int ret = 0;
 
 	memcpy(param->u.wpa_associate.bssid, pMgmt->abyCurrBSSID , 6);
 
-	return ret;
+	return 0;
 }
 
 /*
@@ -554,14 +515,13 @@ static int wpa_get_ssid(PSDevice pDevice,
 {
 	PSMgmtObject        pMgmt = pDevice->pMgmt;
 	PWLAN_IE_SSID       pItemSSID;
-	int ret = 0;
 
 	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 0;
 }
 
 /*
@@ -593,7 +553,7 @@ static int wpa_get_scan(PSDevice pDevice,
 
 	unsigned char *ptempBSS;
 
-	ptempBSS = kmalloc(sizeof(KnownBSS), (int)GFP_ATOMIC);
+	ptempBSS = kmalloc(sizeof(KnownBSS), GFP_ATOMIC);
 
 	if (ptempBSS == NULL) {
 		printk(KERN_ERR "bubble sort kmalloc memory fail@@@\n");
@@ -635,7 +595,7 @@ static int wpa_get_scan(PSDevice pDevice,
 		count++;
 	}
 
-	pBuf = kcalloc(count, sizeof(struct viawget_scan_result), (int)GFP_ATOMIC);
+	pBuf = kcalloc(count, sizeof(struct viawget_scan_result), GFP_ATOMIC);
 
 	if (pBuf == NULL) {
 		ret = -ENOMEM;
@@ -654,11 +614,7 @@ static int wpa_get_scan(PSDevice pDevice,
 			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);
@@ -675,11 +631,11 @@ static int wpa_get_scan(PSDevice pDevice,
 	if (jj < count)
 		count = jj;
 
-	if (copy_to_user(param->u.scan_results.buf, pBuf, sizeof(struct viawget_scan_result) * count)) {
+	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;
@@ -706,7 +662,6 @@ static int wpa_set_associate(PSDevice pDevice,
 	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
@@ -739,9 +694,8 @@ static int wpa_set_associate(PSDevice pDevice,
 	// set bssid
 	if (memcmp(param->u.wpa_associate.bssid, &abyNullAddr[0], 6) != 0)
 		memcpy(pMgmt->abyDesireBSSID, param->u.wpa_associate.bssid, 6);
-	else {
+	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)
@@ -788,13 +742,10 @@ static int wpa_set_associate(PSDevice pDevice,
 
 	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;
@@ -830,7 +781,7 @@ static int wpa_set_associate(PSDevice pDevice,
 	bScheduleCommand((void *)pDevice, WLAN_CMD_SSID, NULL);
 	spin_unlock_irq(&pDevice->lock);
 
-	return ret;
+	return 0;
 }
 
 /*
@@ -857,7 +808,7 @@ int wpa_ioctl(PSDevice pDevice, struct iw_point *p)
 	    p->length > VIAWGET_WPA_MAX_BUF_SIZE || !p->pointer)
 		return -EINVAL;
 
-	param = kmalloc((int)p->length, (int)GFP_KERNEL);
+	param = kmalloc((int)p->length, GFP_KERNEL);
 	if (param == NULL)
 		return -ENOMEM;
 
diff --git a/drivers/staging/vt6655/wroute.c b/drivers/staging/vt6655/wroute.c
index c39d5ed59de..4da3fef139d 100644
--- a/drivers/staging/vt6655/wroute.c
+++ b/drivers/staging/vt6655/wroute.c
@@ -44,7 +44,6 @@
 
 /*---------------------  Static Variables  --------------------------*/
 static int msglevel = MSG_LEVEL_INFO;
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 /*---------------------  Static Functions  --------------------------*/
 
 /*---------------------  Export Variables  --------------------------*/
diff --git a/drivers/staging/vt6656/Makefile b/drivers/staging/vt6656/Makefile
index 1d829b46c2e..b5ec483f3eb 100644
--- a/drivers/staging/vt6656/Makefile
+++ b/drivers/staging/vt6656/Makefile
@@ -18,8 +18,6 @@ vt6656_stage-y +=	main_usb.o \
 			datarate.o \
 			rc4.o \
 			tether.o \
-			tcrc.o \
-			hostap.o \
 			wpa.o \
 			key.o \
 			tkip.o \
@@ -27,10 +25,8 @@ vt6656_stage-y +=	main_usb.o \
 			rf.o \
 			iwctl.o \
 			wpactl.o \
-			aes_ccmp.o \
 			usbpipe.o \
 			channel.o \
-			control.o \
 			firmware.o \
 			int.o
 
diff --git a/drivers/staging/vt6656/aes_ccmp.c b/drivers/staging/vt6656/aes_ccmp.c
deleted file mode 100644
index e2bfa8d266c..00000000000
--- a/drivers/staging/vt6656/aes_ccmp.c
+++ /dev/null
@@ -1,373 +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: aes_ccmp.c
- *
- * Purpose: AES_CCMP decryption
- *
- * Author: Warren Hsu
- *
- * Date: Feb 15, 2005
- *
- * Functions:
- *      AESbGenCCMP - Parsing RX-packet
- *
- * Revision History:
- */
-
-#include "device.h"
-#include "80211hdr.h"
-
-/*
- * SBOX Table
- */
-
-static 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,
-	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
-};
-
-static 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,
-	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
-};
-
-static 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,
-	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 void xor_128(u8 *a, u8 *b, u8 *out)
-{
-	u32 *dwPtrA = (u32 *) a;
-	u32 *dwPtrB = (u32 *) b;
-	u32 *dwPtrOut = (u32 *) out;
-
-	(*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
-	(*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
-	(*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
-	(*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
-}
-
-static void xor_32(u8 *a, u8 *b, u8 *out)
-{
-	u32 *dwPtrA = (u32 *) a;
-	u32 *dwPtrB = (u32 *) b;
-	u32 *dwPtrOut = (u32 *) out;
-
-	(*dwPtrOut++) = (*dwPtrA++) ^ (*dwPtrB++);
-}
-
-static void AddRoundKey(u8 *key, int round)
-{
-	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]];
-	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]);
-
-	xor_32(&key[4], &key[0], &key[4]);
-	xor_32(&key[8], &key[4], &key[8]);
-	xor_32(&key[12], &key[8], &key[12]);
-}
-
-static void SubBytes(u8 *in, u8 *out)
-{
-	int i;
-
-	for (i = 0; i < 16; i++)
-		out[i] = sbox_table[in[i]];
-}
-
-static void ShiftRows(u8 *in, u8 *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];
-}
-
-static void MixColumns(u8 *in, u8 *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]];
-}
-
-static void AESv128(u8 *key, u8 *data, u8 *ciphertext)
-{
-	int  i;
-	int  round;
-	u8 TmpdataA[16];
-	u8 TmpdataB[16];
-	u8 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);
-		}
-	}
-
-}
-
-/*
- * Description: AES decryption
- *
- * Parameters:
- *  In:
- *      pbyRxKey            - The key used to decrypt
- *      pbyFrame            - Starting address of packet header
- *      wFrameSize          - Total packet size including CRC
- *  Out:
- *      none
- *
- * Return Value: MIC compare result
- *
- */
-
-bool AESbGenCCMP(u8 *pbyRxKey, u8 *pbyFrame, u16 wFrameSize)
-{
-	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 */
-	u16            wPayloadSize = wFrameSize - 8 - 8 - 4 - WLAN_HDR_ADDR3_LEN;
-	bool            bA4 = false;
-	u8            byTmp;
-	u16            wCnt;
-	int             ii, jj, kk;
-
-	pbyIV = pbyFrame + WLAN_HDR_ADDR3_LEN;
-	if (WLAN_GET_FC_TODS(*(u16 *) pbyFrame) &&
-	    WLAN_GET_FC_FROMDS(*(u16 *) 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->addr2, 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] = (u8)(wPayloadSize >> 8);
-	MIC_IV[15] = (u8)(wPayloadSize & 0xff);
-
-	/* MIC_HDR1 */
-	MIC_HDR1[0] = (u8)(wHLen >> 8);
-	MIC_HDR1[1] = (u8)(wHLen & 0xff);
-	byTmp = (u8)(le16_to_cpu(pMACHeader->frame_control) >> 8);
-	MIC_HDR1[2] = byTmp & 0x8f;
-	byTmp = (u8)(le16_to_cpu(pMACHeader->frame_control) & 0xff);
-	byTmp &= 0x87;
-	MIC_HDR1[3] = byTmp | 0x40;
-	memcpy(&(MIC_HDR1[4]), pMACHeader->addr1, ETH_ALEN);
-	memcpy(&(MIC_HDR1[10]), pMACHeader->addr2, ETH_ALEN);
-
-	/* MIC_HDR2 */
-	memcpy(&(MIC_HDR2[0]), pMACHeader->addr3, ETH_ALEN);
-	byTmp = (u8)(le16_to_cpu(pMACHeader->seq_ctrl) >> 8);
-	MIC_HDR2[6] = byTmp & 0x0f;
-	MIC_HDR2[7] = 0;
-
-	if (bA4) {
-		memcpy(&(MIC_HDR2[8]), pMACHeader->addr4, 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] = (u8) (wCnt >> 8);
-		abyCTRPLD[15] = (u8) (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] = (u8) (wCnt >> 8);
-	abyCTRPLD[15] = (u8) (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 calculated MIC */
-
-	wCnt = 0;
-	abyCTRPLD[14] = (u8) (wCnt >> 8);
-	abyCTRPLD[15] = (u8) (wCnt & 0xff);
-	AESv128(pbyRxKey, abyCTRPLD, abyTmp);
-
-	for (kk = 0; kk < 8; kk++)
-		abyTmp[kk] = abyTmp[kk] ^ pbyPayload[kk];
-
-	/* => above is the packet dec-MIC */
-
-	if (!memcmp(abyMIC, abyTmp, 8))
-		return true;
-	else
-		return false;
-}
diff --git a/drivers/staging/vt6656/aes_ccmp.h b/drivers/staging/vt6656/aes_ccmp.h
deleted file mode 100644
index ed6a9ae315d..00000000000
--- a/drivers/staging/vt6656/aes_ccmp.h
+++ /dev/null
@@ -1,35 +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: aes_ccmp.h
- *
- * Purpose: AES_CCMP Decryption
- *
- * Author: Warren Hsu
- *
- * Date: Feb 15, 2005
- *
- */
-
-#ifndef __AES_H__
-#define __AES_H__
-
-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 3d4610e25fc..694e34a5ff9 100644
--- a/drivers/staging/vt6656/baseband.c
+++ b/drivers/staging/vt6656/baseband.c
@@ -40,13 +40,8 @@
 #include "mac.h"
 #include "baseband.h"
 #include "rf.h"
-#include "srom.h"
-#include "control.h"
+#include "usbpipe.h"
 #include "datarate.h"
-#include "rndis.h"
-
-static int          msglevel                =MSG_LEVEL_INFO;
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 
 static u8 abyVT3184_AGC[] = {
     0x00,   //0
@@ -638,80 +633,59 @@ static const u16 awcFrameTime[MAX_RATE] =
 {10, 20, 55, 110, 24, 36, 48, 72, 96, 144, 192, 216};
 
 /*
-static
-unsigned long
-s_ulGetLowSQ3(PSDevice pDevice);
-
-static
-unsigned long
-s_ulGetRatio(PSDevice pDevice);
-
-static
-void
-s_vClearSQ3Value(PSDevice pDevice);
-*/
-
-/*
  * Description: Calculate data frame transmitting time
  *
  * Parameters:
  *  In:
- *      byPreambleType  - Preamble Type
- *      byPktType        - PK_TYPE_11A, PK_TYPE_11B, PK_TYPE_11GB, PK_TYPE_11GA
- *      cbFrameLength   - Baseband Type
- *      wRate           - Tx Rate
+ *	preamble_type	- Preamble Type
+ *	pkt_type	- PK_TYPE_11A, PK_TYPE_11B, PK_TYPE_11GB, PK_TYPE_11GA
+ *	frame_length	- Baseband Type
+ *	tx_rate		- Tx Rate
  *  Out:
  *
  * Return Value: FrameTime
  *
  */
-unsigned int
-BBuGetFrameTime(
-     u8 byPreambleType,
-     u8 byPktType,
-     unsigned int cbFrameLength,
-     u16 wRate
-    )
+unsigned int BBuGetFrameTime(u8 preamble_type, u8 pkt_type,
+	unsigned int frame_length, u16 tx_rate)
 {
-    unsigned int uFrameTime;
-    unsigned int uPreamble;
-    unsigned int uTmp;
-    unsigned int uRateIdx = (unsigned int)wRate;
-    unsigned int uRate = 0;
-
-    if (uRateIdx > RATE_54M) {
-        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 frame_time;
+	unsigned int preamble;
+	unsigned int tmp;
+	unsigned int rate = 0;
+
+	if (tx_rate > RATE_54M)
+		return 0;
+
+	rate = (unsigned int)awcFrameTime[tx_rate];
+
+	if (tx_rate <= 3) {
+		if (preamble_type == 1)
+			preamble = 96;
+		else
+			preamble = 192;
+
+		frame_time = (frame_length * 80) / rate;
+		tmp = (frame_time * rate) / 80;
+
+		if (frame_length != tmp)
+			frame_time++;
+
+		return preamble + frame_time;
+	} else {
+		frame_time = (frame_length * 8 + 22) / rate;
+		tmp = ((frame_time * rate) - 22) / 8;
+
+		if (frame_length != tmp)
+			frame_time++;
+
+		frame_time = frame_time * 4;
+
+		if (pkt_type != PK_TYPE_11A)
+			frame_time += 6;
+
+		return 20 + frame_time;
+	}
 }
 
 /*
@@ -719,9 +693,9 @@ BBuGetFrameTime(
  *
  * Parameters:
  *  In:
- *      pDevice         - Device Structure
- *      cbFrameLength   - Tx Frame Length
- *      wRate           - Tx Rate
+ *      priv         - Device Structure
+ *      frame_length   - Tx Frame Length
+ *      tx_rate           - Tx Rate
  *  Out:
  *	struct vnt_phy_field *phy
  * 			- pointer to Phy Length field
@@ -731,153 +705,135 @@ BBuGetFrameTime(
  * Return Value: none
  *
  */
-void BBvCalculateParameter(struct vnt_private *pDevice, u32 cbFrameLength,
-	u16 wRate, u8 byPacketType, struct vnt_phy_field *phy)
+void BBvCalculateParameter(struct vnt_private *priv, u32 frame_length,
+	u16 tx_rate, u8 pkt_type, struct vnt_phy_field *phy)
 {
-	u32 cbBitCount;
-	u32 cbUsCount = 0;
-	u32 cbTmp;
-	int bExtBit;
-	u8 byPreambleType = pDevice->byPreambleType;
-	int bCCK = pDevice->bCCK;
-
-    cbBitCount = cbFrameLength * 8;
-    bExtBit = false;
-
-    switch (wRate) {
-    case RATE_1M :
-        cbUsCount = cbBitCount;
-	phy->signal = 0x00;
-        break;
-
-    case RATE_2M :
-        cbUsCount = cbBitCount / 2;
-        if (byPreambleType == 1)
-		phy->signal = 0x09;
-        else // long preamble
-		phy->signal = 0x01;
-        break;
-
-    case RATE_5M :
-        if (bCCK == false)
-            cbBitCount ++;
-        cbUsCount = (cbBitCount * 10) / 55;
-        cbTmp = (cbUsCount * 55) / 10;
-        if (cbTmp != cbBitCount)
-            cbUsCount ++;
-        if (byPreambleType == 1)
-		phy->signal = 0x0a;
-        else // long preamble
-		phy->signal = 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)
-		phy->signal = 0x0b;
-        else // long preamble
-		phy->signal = 0x03;
-        break;
-
-    case RATE_6M :
-        if(byPacketType == PK_TYPE_11A) {//11a, 5GHZ
-		phy->signal = 0x9b;
-        }
-        else {//11g, 2.4GHZ
-		phy->signal = 0x8b;
-        }
-        break;
-
-    case RATE_9M :
-        if(byPacketType == PK_TYPE_11A) {//11a, 5GHZ
-		phy->signal = 0x9f;
-        }
-        else {//11g, 2.4GHZ
-		phy->signal = 0x8f;
-        }
-        break;
-
-    case RATE_12M :
-        if(byPacketType == PK_TYPE_11A) {//11a, 5GHZ
-		phy->signal = 0x9a;
-        }
-        else {//11g, 2.4GHZ
-		phy->signal = 0x8a;
-        }
-        break;
-
-    case RATE_18M :
-        if(byPacketType == PK_TYPE_11A) {//11a, 5GHZ
-		phy->signal = 0x9e;
-        }
-        else {//11g, 2.4GHZ
-		phy->signal = 0x8e;
-        }
-        break;
-
-    case RATE_24M :
-        if(byPacketType == PK_TYPE_11A) {//11a, 5GHZ
-		phy->signal = 0x99;
-        }
-        else {//11g, 2.4GHZ
-		phy->signal = 0x89;
-        }
-        break;
-
-    case RATE_36M :
-        if(byPacketType == PK_TYPE_11A) {//11a, 5GHZ
-		phy->signal = 0x9d;
-        }
-        else {//11g, 2.4GHZ
-		phy->signal = 0x8d;
-        }
-        break;
-
-    case RATE_48M :
-        if(byPacketType == PK_TYPE_11A) {//11a, 5GHZ
-		phy->signal = 0x98;
-        }
-        else {//11g, 2.4GHZ
-		phy->signal = 0x88;
-        }
-        break;
-
-    case RATE_54M :
-        if (byPacketType == PK_TYPE_11A) {//11a, 5GHZ
-		phy->signal = 0x9c;
-        }
-        else {//11g, 2.4GHZ
-		phy->signal = 0x8c;
-        }
-        break;
-
-    default :
-        if (byPacketType == PK_TYPE_11A) {//11a, 5GHZ
-		phy->signal = 0x9c;
-        }
-        else {//11g, 2.4GHZ
-		phy->signal = 0x8c;
-        }
-        break;
-    }
-
-	if (byPacketType == PK_TYPE_11B) {
+	u32 bit_count;
+	u32 count = 0;
+	u32 tmp;
+	int ext_bit;
+	u8 preamble_type = priv->byPreambleType;
+
+	bit_count = frame_length * 8;
+	ext_bit = false;
+
+	switch (tx_rate) {
+	case RATE_1M:
+		count = bit_count;
+
+		phy->signal = 0x00;
+
+		break;
+	case RATE_2M:
+		count = bit_count / 2;
+
+		if (preamble_type == 1)
+			phy->signal = 0x09;
+		else
+			phy->signal = 0x01;
+
+		break;
+	case RATE_5M:
+		count = (bit_count * 10) / 55;
+		tmp = (count * 55) / 10;
+
+		if (tmp != bit_count)
+			count++;
+
+		if (preamble_type == 1)
+			phy->signal = 0x0a;
+		else
+			phy->signal = 0x02;
+
+		break;
+	case RATE_11M:
+		count = bit_count / 11;
+		tmp = count * 11;
+
+		if (tmp != bit_count) {
+			count++;
+
+			if ((bit_count - tmp) <= 3)
+				ext_bit = true;
+		}
+
+		if (preamble_type == 1)
+			phy->signal = 0x0b;
+		else
+			phy->signal = 0x03;
+
+		break;
+	case RATE_6M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x9b;
+		else
+			phy->signal = 0x8b;
+
+		break;
+	case RATE_9M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x9f;
+		else
+			phy->signal = 0x8f;
+
+		break;
+	case RATE_12M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x9a;
+		else
+			phy->signal = 0x8a;
+
+		break;
+	case RATE_18M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x9e;
+		else
+			phy->signal = 0x8e;
+
+		break;
+	case RATE_24M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x99;
+		else
+			phy->signal = 0x89;
+
+		break;
+	case RATE_36M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x9d;
+		else
+			phy->signal = 0x8d;
+
+		break;
+	case RATE_48M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x98;
+		else
+			phy->signal = 0x88;
+
+		break;
+	case RATE_54M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x9c;
+		else
+			phy->signal = 0x8c;
+		break;
+	default:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x9c;
+		else
+			phy->signal = 0x8c;
+		break;
+	}
+
+	if (pkt_type == PK_TYPE_11B) {
 		phy->service = 0x00;
-		if (bExtBit)
+		if (ext_bit)
 			phy->service |= 0x80;
-		phy->len = cpu_to_le16((u16)cbUsCount);
+		phy->len = cpu_to_le16((u16)count);
 	} else {
 		phy->service = 0x00;
-		phy->len = cpu_to_le16((u16)cbFrameLength);
+		phy->len = cpu_to_le16((u16)frame_length);
 	}
 }
 
@@ -886,35 +842,31 @@ void BBvCalculateParameter(struct vnt_private *pDevice, u32 cbFrameLength,
  *
  * Parameters:
  *  In:
- *      pDevice          - Device Structure
- *      byAntennaMode    - Antenna Mode
+ *	priv		- Device Structure
+ *	antenna_mode	- Antenna Mode
  *  Out:
  *      none
  *
  * Return Value: none
  *
  */
-void BBvSetAntennaMode(struct vnt_private *pDevice, u8 byAntennaMode)
+void BBvSetAntennaMode(struct vnt_private *priv, u8 antenna_mode)
 {
-    switch (byAntennaMode) {
-        case ANT_TXA:
-        case ANT_TXB:
-            break;
-        case ANT_RXA:
-            pDevice->byBBRxConf &= 0xFC;
-            break;
-        case ANT_RXB:
-            pDevice->byBBRxConf &= 0xFE;
-            pDevice->byBBRxConf |= 0x02;
-            break;
-    }
-
-    CONTROLnsRequestOut(pDevice,
-                    MESSAGE_TYPE_SET_ANTMD,
-                    (u16) byAntennaMode,
-                    0,
-                    0,
-                    NULL);
+	switch (antenna_mode) {
+	case ANT_TXA:
+	case ANT_TXB:
+		break;
+	case ANT_RXA:
+		priv->byBBRxConf &= 0xFC;
+		break;
+	case ANT_RXB:
+		priv->byBBRxConf &= 0xFE;
+		priv->byBBRxConf |= 0x02;
+		break;
+	}
+
+	vnt_control_out(priv, MESSAGE_TYPE_SET_ANTMD,
+		(u16)antenna_mode, 0, 0, NULL);
 }
 
 /*
@@ -934,14 +886,14 @@ void BBvSetAntennaMode(struct vnt_private *pDevice, u8 byAntennaMode)
 int BBbVT3184Init(struct vnt_private *priv)
 {
 	int status;
-	u16 lenght;
+	u16 length;
 	u8 *addr;
 	u8 *agc;
-	u16 lenght_agc;
+	u16 length_agc;
 	u8 array[256];
 	u8 data;
 
-	status = CONTROLnsRequestIn(priv, MESSAGE_TYPE_READ, 0,
+	status = vnt_control_in(priv, MESSAGE_TYPE_READ, 0,
 		MESSAGE_REQUEST_EEPROM, EEP_MAX_CONTEXT_SIZE,
 						priv->abyEEPROM);
 	if (status != STATUS_SUCCESS)
@@ -955,20 +907,20 @@ int BBbVT3184Init(struct vnt_private *priv)
 			(priv->abyEEPROM[EEP_OFS_ZONETYPE] != 0x00)) {
 			priv->abyEEPROM[EEP_OFS_ZONETYPE] = 0;
 			priv->abyEEPROM[EEP_OFS_MAXCHANNEL] = 0x0B;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-						"Init Zone Type :USA\n");
+
+			dev_dbg(&priv->usb->dev, "Init Zone Type :USA\n");
 		} else if ((priv->config_file.ZoneType == 1) &&
 			(priv->abyEEPROM[EEP_OFS_ZONETYPE] != 0x01)) {
 			priv->abyEEPROM[EEP_OFS_ZONETYPE] = 0x01;
 			priv->abyEEPROM[EEP_OFS_MAXCHANNEL] = 0x0D;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-						"Init Zone Type :Japan\n");
+
+			dev_dbg(&priv->usb->dev, "Init Zone Type :Japan\n");
 		} else if ((priv->config_file.ZoneType == 2) &&
 			(priv->abyEEPROM[EEP_OFS_ZONETYPE] != 0x02)) {
 			priv->abyEEPROM[EEP_OFS_ZONETYPE] = 0x02;
 			priv->abyEEPROM[EEP_OFS_MAXCHANNEL] = 0x0D;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-						"Init Zone Type :Europe\n");
+
+			dev_dbg(&priv->usb->dev, "Init Zone Type :Europe\n");
 		} else {
 			if (priv->config_file.ZoneType !=
 					priv->abyEEPROM[EEP_OFS_ZONETYPE])
@@ -988,18 +940,17 @@ int BBbVT3184Init(struct vnt_private *priv)
 
 	priv->byRFType = priv->abyEEPROM[EEP_OFS_RFTYPE];
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Zone Type %x\n",
-							priv->byZoneType);
+	dev_dbg(&priv->usb->dev, "Zone Type %x\n", priv->byZoneType);
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"RF Type %d\n", priv->byRFType);
+	dev_dbg(&priv->usb->dev, "RF Type %d\n", priv->byRFType);
 
 	if ((priv->byRFType == RF_AL2230) ||
 				(priv->byRFType == RF_AL2230S)) {
 		priv->byBBRxConf = abyVT3184_AL2230[10];
-		lenght = sizeof(abyVT3184_AL2230);
+		length = sizeof(abyVT3184_AL2230);
 		addr = abyVT3184_AL2230;
 		agc = abyVT3184_AGC;
-		lenght_agc = sizeof(abyVT3184_AGC);
+		length_agc = sizeof(abyVT3184_AGC);
 
 		priv->abyBBVGA[0] = 0x1C;
 		priv->abyBBVGA[1] = 0x10;
@@ -1011,10 +962,10 @@ int BBbVT3184Init(struct vnt_private *priv)
 		priv->ldBmThreshold[3] = 0;
 	} else if (priv->byRFType == RF_AIROHA7230) {
 		priv->byBBRxConf = abyVT3184_AL2230[10];
-		lenght = sizeof(abyVT3184_AL2230);
+		length = sizeof(abyVT3184_AL2230);
 		addr = abyVT3184_AL2230;
 		agc = abyVT3184_AGC;
-		lenght_agc = sizeof(abyVT3184_AGC);
+		length_agc = sizeof(abyVT3184_AGC);
 
 		addr[0xd7] = 0x06;
 
@@ -1029,10 +980,10 @@ int BBbVT3184Init(struct vnt_private *priv)
 	} else if ((priv->byRFType == RF_VT3226) ||
 			(priv->byRFType == RF_VT3226D0)) {
 		priv->byBBRxConf = abyVT3184_VT3226D0[10];
-		lenght = sizeof(abyVT3184_VT3226D0);
+		length = sizeof(abyVT3184_VT3226D0);
 		addr = abyVT3184_VT3226D0;
 		agc = abyVT3184_AGC;
-		lenght_agc = sizeof(abyVT3184_AGC);
+		length_agc = sizeof(abyVT3184_AGC);
 
 		priv->abyBBVGA[0] = 0x20;
 		priv->abyBBVGA[1] = 0x10;
@@ -1046,10 +997,10 @@ int BBbVT3184Init(struct vnt_private *priv)
 		MACvRegBitsOn(priv, MAC_REG_SOFTPWRCTL2, SOFTPWRCTL_RFLEOPT);
 	} else if ((priv->byRFType == RF_VT3342A0)) {
 		priv->byBBRxConf = abyVT3184_VT3226D0[10];
-		lenght = sizeof(abyVT3184_VT3226D0);
+		length = sizeof(abyVT3184_VT3226D0);
 		addr = abyVT3184_VT3226D0;
 		agc = abyVT3184_AGC;
-		lenght_agc = sizeof(abyVT3184_AGC);
+		length_agc = sizeof(abyVT3184_AGC);
 
 		priv->abyBBVGA[0] = 0x20;
 		priv->abyBBVGA[1] = 0x10;
@@ -1065,40 +1016,39 @@ int BBbVT3184Init(struct vnt_private *priv)
 		return true;
 	}
 
-	memcpy(array, addr, lenght);
+	memcpy(array, addr, length);
 
-	CONTROLnsRequestOut(priv, MESSAGE_TYPE_WRITE, 0,
-		MESSAGE_REQUEST_BBREG, lenght, array);
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE, 0,
+		MESSAGE_REQUEST_BBREG, length, array);
 
-	memcpy(array, agc, lenght_agc);
+	memcpy(array, agc, length_agc);
 
-	CONTROLnsRequestOut(priv, MESSAGE_TYPE_WRITE, 0,
-		MESSAGE_REQUEST_BBAGC, lenght_agc, array);
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE, 0,
+		MESSAGE_REQUEST_BBAGC, length_agc, array);
 
 	if ((priv->byRFType == RF_VT3226) ||
 		(priv->byRFType == RF_VT3342A0)) {
-		ControlvWriteByte(priv, MESSAGE_REQUEST_MACREG,
+		vnt_control_out_u8(priv, MESSAGE_REQUEST_MACREG,
 						MAC_REG_ITRTMSET, 0x23);
 		MACvRegBitsOn(priv, MAC_REG_PAPEDELAY, 0x01);
 	} else if (priv->byRFType == RF_VT3226D0) {
-		ControlvWriteByte(priv, MESSAGE_REQUEST_MACREG,
+		vnt_control_out_u8(priv, MESSAGE_REQUEST_MACREG,
 						MAC_REG_ITRTMSET, 0x11);
 		MACvRegBitsOn(priv, MAC_REG_PAPEDELAY, 0x01);
 	}
 
-	ControlvWriteByte(priv, MESSAGE_REQUEST_BBREG, 0x04, 0x7f);
-	ControlvWriteByte(priv, MESSAGE_REQUEST_BBREG, 0x0d, 0x01);
-
-	RFbRFTableDownload(priv);
+	vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x04, 0x7f);
+	vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x0d, 0x01);
 
+	vnt_rf_table_download(priv);
 
 	/* Fix for TX USB resets from vendors driver */
-	CONTROLnsRequestIn(priv, MESSAGE_TYPE_READ, USB_REG4,
+	vnt_control_in(priv, MESSAGE_TYPE_READ, USB_REG4,
 		MESSAGE_REQUEST_MEM, sizeof(data), &data);
 
 	data |= 0x2;
 
-	CONTROLnsRequestOut(priv, MESSAGE_TYPE_WRITE, USB_REG4,
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE, USB_REG4,
 		MESSAGE_REQUEST_MEM, sizeof(data), &data);
 
 	return true;
@@ -1109,41 +1059,42 @@ int BBbVT3184Init(struct vnt_private *priv)
  *
  * Parameters:
  *  In:
- *      pDevice     - Device Structure
+ *	priv	- Device Structure
  *  Out:
  *      none
  *
  * Return Value: none
  *
  */
-void BBvSetShortSlotTime(struct vnt_private *pDevice)
+void BBvSetShortSlotTime(struct vnt_private *priv)
 {
-    u8 byBBVGA=0;
+	u8 bb_vga = 0;
 
-	if (pDevice->bShortSlotTime)
-        pDevice->byBBRxConf &= 0xDF;//1101 1111
+	if (priv->bShortSlotTime)
+		priv->byBBRxConf &= 0xdf;
 	else
-        pDevice->byBBRxConf |= 0x20;//0010 0000
+		priv->byBBRxConf |= 0x20;
+
+	vnt_control_in_u8(priv, MESSAGE_REQUEST_BBREG, 0xe7, &bb_vga);
 
-    ControlvReadByte (pDevice, MESSAGE_REQUEST_BBREG, 0xE7, &byBBVGA);
-	if (byBBVGA == pDevice->abyBBVGA[0])
-        pDevice->byBBRxConf |= 0x20;//0010 0000
+	if (bb_vga == priv->abyBBVGA[0])
+		priv->byBBRxConf |= 0x20;
 
-    ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x0A, pDevice->byBBRxConf);
+	vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x0a, priv->byBBRxConf);
 }
 
-void BBvSetVGAGainOffset(struct vnt_private *pDevice, u8 byData)
+void BBvSetVGAGainOffset(struct vnt_private *priv, u8 data)
 {
 
-    ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xE7, byData);
+	vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0xE7, data);
 
-    // patch for 3253B0 Baseband with Cardbus module
-	if (pDevice->bShortSlotTime)
-		pDevice->byBBRxConf &= 0xDF; /* 1101 1111 */
+	/* patch for 3253B0 Baseband with Cardbus module */
+	if (priv->bShortSlotTime)
+		priv->byBBRxConf &= 0xdf; /* 1101 1111 */
 	else
-		pDevice->byBBRxConf |= 0x20; /* 0010 0000 */
+		priv->byBBRxConf |= 0x20; /* 0010 0000 */
 
-    ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x0A, pDevice->byBBRxConf);//CR10
+	vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x0a, priv->byBBRxConf);
 }
 
 /*
@@ -1151,735 +1102,288 @@ void BBvSetVGAGainOffset(struct vnt_private *pDevice, u8 byData)
  *
  * Parameters:
  *  In:
- *      pDevice          - Device Structure
+ *	priv	- Device Structure
  *  Out:
  *      none
  *
  * Return Value: none
  *
  */
-void BBvSetDeepSleep(struct vnt_private *pDevice)
-{
-    ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x0c, 0x17);//CR12
-    ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x0D, 0xB9);//CR13
-}
-
-void BBvExitDeepSleep(struct vnt_private *pDevice)
+void BBvSetDeepSleep(struct vnt_private *priv)
 {
-    ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x0C, 0x00);//CR12
-    ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x0D, 0x01);//CR13
+	vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x0c, 0x17);/* CR12 */
+	vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x0d, 0xB9);/* CR13 */
 }
 
-static unsigned long s_ulGetLowSQ3(struct vnt_private *pDevice)
+void BBvExitDeepSleep(struct vnt_private *priv)
 {
-	int ii;
-	unsigned long ulSQ3 = 0;
-	unsigned long ulMaxPacket;
-
-    ulMaxPacket = pDevice->aulPktNum[RATE_54M];
-	if (pDevice->aulPktNum[RATE_54M] != 0)
-        ulSQ3 = pDevice->aulSQ3Val[RATE_54M] / pDevice->aulPktNum[RATE_54M];
-
-	for (ii = RATE_48M; ii >= RATE_6M; ii--)
-		if (pDevice->aulPktNum[ii] > ulMaxPacket) {
-            ulMaxPacket = pDevice->aulPktNum[ii];
-            ulSQ3 = pDevice->aulSQ3Val[ii] / pDevice->aulPktNum[ii];
-        }
-
-    return ulSQ3;
+	vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x0c, 0x00);/* CR12 */
+	vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x0d, 0x01);/* CR13 */
 }
 
-static unsigned long s_ulGetRatio(struct vnt_private *pDevice)
+void BBvUpdatePreEDThreshold(struct vnt_private *priv, int scanning)
 {
-	int ii, jj;
-	unsigned long ulRatio = 0;
-	unsigned long ulMaxPacket;
-	unsigned long ulPacketNum;
-
-    //This is a thousand-ratio
-    ulMaxPacket = pDevice->aulPktNum[RATE_54M];
-    if ( pDevice->aulPktNum[RATE_54M] != 0 ) {
-        ulPacketNum = pDevice->aulPktNum[RATE_54M];
-        ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
-        ulRatio += TOP_RATE_54M;
-    }
-	for (ii = RATE_48M; ii >= RATE_1M; ii--)
-        if ( pDevice->aulPktNum[ii] > ulMaxPacket ) {
-            ulPacketNum = 0;
-            for ( jj=RATE_54M;jj>=ii;jj--)
-                ulPacketNum += pDevice->aulPktNum[jj];
-            ulRatio = (ulPacketNum * 1000 / pDevice->uDiversityCnt);
-            ulRatio += TOP_RATE_48M;
-            ulMaxPacket = pDevice->aulPktNum[ii];
-        }
-
-    return ulRatio;
-}
-
-static void s_vClearSQ3Value(struct vnt_private *pDevice)
-{
-    int ii;
-    pDevice->uDiversityCnt = 0;
-
-    for ( ii=RATE_1M;ii<MAX_RATE;ii++) {
-        pDevice->aulPktNum[ii] = 0;
-        pDevice->aulSQ3Val[ii] = 0;
-    }
-}
-
-/*
- * Description: Antenna Diversity
- *
- * Parameters:
- *  In:
- *      pDevice          - Device Structure
- *      byRSR            - RSR from received packet
- *      bySQ3            - SQ3 value from received packet
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-
-void BBvAntennaDiversity(struct vnt_private *pDevice,
-	u8 byRxRate, u8 bySQ3)
-{
-
-    pDevice->uDiversityCnt++;
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pDevice->uDiversityCnt = %d\n", (int)pDevice->uDiversityCnt);
-
-    if (byRxRate == 2) {
-        pDevice->aulPktNum[RATE_1M]++;
-    }
-    else if (byRxRate==4) {
-        pDevice->aulPktNum[RATE_2M]++;
-    }
-    else if (byRxRate==11) {
-        pDevice->aulPktNum[RATE_5M]++;
-    }
-    else if (byRxRate==22) {
-        pDevice->aulPktNum[RATE_11M]++;
-    }
-    else if(byRxRate==12){
-        pDevice->aulPktNum[RATE_6M]++;
-        pDevice->aulSQ3Val[RATE_6M] += bySQ3;
-    }
-    else if(byRxRate==18){
-        pDevice->aulPktNum[RATE_9M]++;
-        pDevice->aulSQ3Val[RATE_9M] += bySQ3;
-    }
-    else if(byRxRate==24){
-        pDevice->aulPktNum[RATE_12M]++;
-        pDevice->aulSQ3Val[RATE_12M] += bySQ3;
-    }
-    else if(byRxRate==36){
-        pDevice->aulPktNum[RATE_18M]++;
-        pDevice->aulSQ3Val[RATE_18M] += bySQ3;
-    }
-    else if(byRxRate==48){
-        pDevice->aulPktNum[RATE_24M]++;
-        pDevice->aulSQ3Val[RATE_24M] += bySQ3;
-    }
-    else if(byRxRate==72){
-        pDevice->aulPktNum[RATE_36M]++;
-        pDevice->aulSQ3Val[RATE_36M] += bySQ3;
-    }
-    else if(byRxRate==96){
-        pDevice->aulPktNum[RATE_48M]++;
-        pDevice->aulSQ3Val[RATE_48M] += bySQ3;
-    }
-    else if(byRxRate==108){
-        pDevice->aulPktNum[RATE_54M]++;
-        pDevice->aulSQ3Val[RATE_54M] += bySQ3;
-    }
-
-    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->aulPktNum[RATE_54M]);
-
-            pDevice->ulSQ3_State0 = s_ulGetLowSQ3(pDevice);
-            pDevice->ulRatio_State0 = s_ulGetRatio(pDevice);
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"SQ3_State0, SQ3= [%08x] rate = [%08x]\n",(int)pDevice->ulSQ3_State0,(int)pDevice->ulRatio_State0);
-
-            if ( ((pDevice->aulPktNum[RATE_54M] < pDevice->ulDiversityNValue/2) &&
-                  (pDevice->ulSQ3_State0 > pDevice->ulSQ3TH) ) ||
-                 (pDevice->ulSQ3_State0 == 0 ) )  {
-
-                if ( pDevice->byTMax == 0 )
-                    return;
-
-		bScheduleCommand((void *) pDevice,
-				 WLAN_CMD_CHANGE_ANTENNA,
-				 NULL);
-
-                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);
-            }
-            s_vClearSQ3Value(pDevice);
-
-        }
-    } else { //byAntennaState == 1
-
-        if (pDevice->uDiversityCnt > pDevice->ulDiversityMValue) {
-
-            del_timer(&pDevice->TimerSQ3Tmax1);
-            pDevice->ulSQ3_State1 = s_ulGetLowSQ3(pDevice);
-            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->ulSQ3_State1 == 0) && (pDevice->ulSQ3_State0 != 0)) ||
-                 ((pDevice->ulSQ3_State1 == 0) && (pDevice->ulSQ3_State0 == 0) && (pDevice->ulRatio_State1 < pDevice->ulRatio_State0)) ||
-                 ((pDevice->ulSQ3_State1 != 0) && (pDevice->ulSQ3_State0 != 0) && (pDevice->ulSQ3_State0 < pDevice->ulSQ3_State1))
-               ) {
-
-		bScheduleCommand((void *) pDevice,
-				 WLAN_CMD_CHANGE_ANTENNA,
-				 NULL);
-
-                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;
-            s_vClearSQ3Value(pDevice);
-        }
-    } //byAntennaState
-}
-
-/*+
- *
- * Description:
- *  Timer for SQ3 antenna diversity
- *
- * Parameters:
- *  In:
- *      pvSysSpec1
- *      hDeviceContext - Pointer to the adapter
- *      pvSysSpec2
- *      pvSysSpec3
- *  Out:
- *      none
- *
- * Return Value: none
- *
--*/
-
-void TimerSQ3CallBack(struct vnt_private *pDevice)
-{
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"TimerSQ3CallBack...");
-    spin_lock_irq(&pDevice->lock);
-
-    bScheduleCommand((void *) pDevice, WLAN_CMD_CHANGE_ANTENNA, NULL);
-    pDevice->byAntennaState = 0;
-    s_vClearSQ3Value(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);
-
-    spin_unlock_irq(&pDevice->lock);
-}
+	u8 cr_201 = 0x0, cr_206 = 0x0;
+	u8 ed_inx = priv->byBBPreEDIndex;
+
+	switch (priv->byRFType) {
+	case RF_AL2230:
+	case RF_AL2230S:
+	case RF_AIROHA7230:
+		if (scanning) { /* Max sensitivity */
+			ed_inx = 0;
+			cr_206 = 0x30;
+			break;
+		}
 
-/*+
- *
- * Description:
- *  Timer for SQ3 antenna diversity
- *
- * Parameters:
- *  In:
- *      pvSysSpec1
- *      hDeviceContext - Pointer to the adapter
- *      pvSysSpec2
- *      pvSysSpec3
- *  Out:
- *      none
- *
- * Return Value: none
- *
--*/
+		if (priv->byBBPreEDRSSI <= 45) {
+			ed_inx = 20;
+			cr_201 = 0xff;
+		} else if (priv->byBBPreEDRSSI <= 46) {
+			ed_inx = 19;
+			cr_201 = 0x1a;
+		} else if (priv->byBBPreEDRSSI <= 47) {
+			ed_inx = 18;
+			cr_201 = 0x15;
+		} else if (priv->byBBPreEDRSSI <= 49) {
+			ed_inx = 17;
+			cr_201 = 0xe;
+		} else if (priv->byBBPreEDRSSI <= 51) {
+			ed_inx = 16;
+			cr_201 = 0x9;
+		} else if (priv->byBBPreEDRSSI <= 53) {
+			ed_inx = 15;
+			cr_201 = 0x6;
+		} else if (priv->byBBPreEDRSSI <= 55) {
+			ed_inx = 14;
+			cr_201 = 0x3;
+		} else if (priv->byBBPreEDRSSI <= 56) {
+			ed_inx = 13;
+			cr_201 = 0x2;
+			cr_206 = 0xa0;
+		} else if (priv->byBBPreEDRSSI <= 57) {
+			ed_inx = 12;
+			cr_201 = 0x2;
+			cr_206 = 0x20;
+		} else if (priv->byBBPreEDRSSI <= 58) {
+			ed_inx = 11;
+			cr_201 = 0x1;
+			cr_206 = 0xa0;
+		} else if (priv->byBBPreEDRSSI <= 59) {
+			ed_inx = 10;
+			cr_201 = 0x1;
+			cr_206 = 0x54;
+		} else if (priv->byBBPreEDRSSI <= 60) {
+			ed_inx = 9;
+			cr_201 = 0x1;
+			cr_206 = 0x18;
+		} else if (priv->byBBPreEDRSSI <= 61) {
+			ed_inx = 8;
+			cr_206 = 0xe3;
+		} else if (priv->byBBPreEDRSSI <= 62) {
+			ed_inx = 7;
+			cr_206 = 0xb9;
+		} else if (priv->byBBPreEDRSSI <= 63) {
+			ed_inx = 6;
+			cr_206 = 0x93;
+		} else if (priv->byBBPreEDRSSI <= 64) {
+			ed_inx = 5;
+			cr_206 = 0x79;
+		} else if (priv->byBBPreEDRSSI <= 65) {
+			ed_inx = 4;
+			cr_206 = 0x62;
+		} else if (priv->byBBPreEDRSSI <= 66) {
+			ed_inx = 3;
+			cr_206 = 0x51;
+		} else if (priv->byBBPreEDRSSI <= 67) {
+			ed_inx = 2;
+			cr_206 = 0x43;
+		} else if (priv->byBBPreEDRSSI <= 68) {
+			ed_inx = 1;
+			cr_206 = 0x36;
+		} else {
+			ed_inx = 0;
+			cr_206 = 0x30;
+		}
+		break;
+
+	case RF_VT3226:
+	case RF_VT3226D0:
+		if (scanning)	{ /* Max sensitivity */
+			ed_inx = 0;
+			cr_206 = 0x24;
+			break;
+		}
 
-void TimerSQ3Tmax3CallBack(struct vnt_private *pDevice)
-{
+		if (priv->byBBPreEDRSSI <= 41) {
+			ed_inx = 22;
+			cr_201 = 0xff;
+		} else if (priv->byBBPreEDRSSI <= 42) {
+			ed_inx = 21;
+			cr_201 = 0x36;
+		} else if (priv->byBBPreEDRSSI <= 43) {
+			ed_inx = 20;
+			cr_201 = 0x26;
+		} else if (priv->byBBPreEDRSSI <= 45) {
+			ed_inx = 19;
+			cr_201 = 0x18;
+		} else if (priv->byBBPreEDRSSI <= 47) {
+			ed_inx = 18;
+			cr_201 = 0x11;
+		} else if (priv->byBBPreEDRSSI <= 49) {
+			ed_inx = 17;
+			cr_201 = 0xa;
+		} else if (priv->byBBPreEDRSSI <= 51) {
+			ed_inx = 16;
+			cr_201 = 0x7;
+		} else if (priv->byBBPreEDRSSI <= 53) {
+			ed_inx = 15;
+			cr_201 = 0x4;
+		} else if (priv->byBBPreEDRSSI <= 55) {
+			ed_inx = 14;
+			cr_201 = 0x2;
+			cr_206 = 0xc0;
+		} else if (priv->byBBPreEDRSSI <= 56) {
+			ed_inx = 13;
+			cr_201 = 0x2;
+			cr_206 = 0x30;
+		} else if (priv->byBBPreEDRSSI <= 57) {
+			ed_inx = 12;
+			cr_201 = 0x1;
+			cr_206 = 0xb0;
+		} else if (priv->byBBPreEDRSSI <= 58) {
+			ed_inx = 11;
+			cr_201 = 0x1;
+			cr_206 = 0x70;
+		} else if (priv->byBBPreEDRSSI <= 59) {
+			ed_inx = 10;
+			cr_201 = 0x1;
+			cr_206 = 0x30;
+		} else if (priv->byBBPreEDRSSI <= 60) {
+			ed_inx = 9;
+			cr_206 = 0xea;
+		} else if (priv->byBBPreEDRSSI <= 61) {
+			ed_inx = 8;
+			cr_206 = 0xc0;
+		} else if (priv->byBBPreEDRSSI <= 62) {
+			ed_inx = 7;
+			cr_206 = 0x9c;
+		} else if (priv->byBBPreEDRSSI <= 63) {
+			ed_inx = 6;
+			cr_206 = 0x80;
+		} else if (priv->byBBPreEDRSSI <= 64) {
+			ed_inx = 5;
+			cr_206 = 0x68;
+		} else if (priv->byBBPreEDRSSI <= 65) {
+			ed_inx = 4;
+			cr_206 = 0x52;
+		} else if (priv->byBBPreEDRSSI <= 66) {
+			ed_inx = 3;
+			cr_206 = 0x43;
+		} else if (priv->byBBPreEDRSSI <= 67) {
+			ed_inx = 2;
+			cr_206 = 0x36;
+		} else if (priv->byBBPreEDRSSI <= 68) {
+			ed_inx = 1;
+			cr_206 = 0x2d;
+		} else {
+			ed_inx = 0;
+			cr_206 = 0x24;
+		}
+		break;
 
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"TimerSQ3Tmax3CallBack...");
-    spin_lock_irq(&pDevice->lock);
+	case RF_VT3342A0:
+		if (scanning) { /* need Max sensitivity */
+			ed_inx = 0;
+			cr_206 = 0x38;
+			break;
+		}
 
-    pDevice->ulRatio_State0 = s_ulGetRatio(pDevice);
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"SQ3_State0 = [%08x]\n",(int)pDevice->ulRatio_State0);
+		if (priv->byBBPreEDRSSI <= 41) {
+			ed_inx = 20;
+			cr_201 = 0xff;
+		} else if (priv->byBBPreEDRSSI <= 42) {
+			ed_inx = 19;
+			cr_201 = 0x36;
+		} else if (priv->byBBPreEDRSSI <= 43) {
+			ed_inx = 18;
+			cr_201 = 0x26;
+		} else if (priv->byBBPreEDRSSI <= 45) {
+			ed_inx = 17;
+			cr_201 = 0x18;
+		} else if (priv->byBBPreEDRSSI <= 47) {
+			ed_inx = 16;
+			cr_201 = 0x11;
+		} else if (priv->byBBPreEDRSSI <= 49) {
+			ed_inx = 15;
+			cr_201 = 0xa;
+		} else if (priv->byBBPreEDRSSI <= 51) {
+			ed_inx = 14;
+			cr_201 = 0x7;
+		} else if (priv->byBBPreEDRSSI <= 53) {
+			ed_inx = 13;
+			cr_201 = 0x4;
+		} else if (priv->byBBPreEDRSSI <= 55) {
+			ed_inx = 12;
+			cr_201 = 0x2;
+			cr_206 = 0xc0;
+		} else if (priv->byBBPreEDRSSI <= 56) {
+			ed_inx = 11;
+			cr_201 = 0x2;
+			cr_206 = 0x30;
+		} else if (priv->byBBPreEDRSSI <= 57) {
+			ed_inx = 10;
+			cr_201 = 0x1;
+			cr_206 = 0xb0;
+		} else if (priv->byBBPreEDRSSI <= 58) {
+			ed_inx = 9;
+			cr_201 = 0x1;
+			cr_206 = 0x70;
+		} else if (priv->byBBPreEDRSSI <= 59) {
+			ed_inx = 8;
+			cr_201 = 0x1;
+			cr_206 = 0x30;
+		} else if (priv->byBBPreEDRSSI <= 60) {
+			ed_inx = 7;
+			cr_206 = 0xea;
+		} else if (priv->byBBPreEDRSSI <= 61) {
+			ed_inx = 6;
+			cr_206 = 0xc0;
+		} else if (priv->byBBPreEDRSSI <= 62) {
+			ed_inx = 5;
+			cr_206 = 0x9c;
+		} else if (priv->byBBPreEDRSSI <= 63) {
+			ed_inx = 4;
+			cr_206 = 0x80;
+		} else if (priv->byBBPreEDRSSI <= 64) {
+			ed_inx = 3;
+			cr_206 = 0x68;
+		} else if (priv->byBBPreEDRSSI <= 65) {
+			ed_inx = 2;
+			cr_206 = 0x52;
+		} else if (priv->byBBPreEDRSSI <= 66) {
+			ed_inx = 1;
+			cr_206 = 0x43;
+		} else {
+			ed_inx = 0;
+			cr_206 = 0x38;
+		}
+		break;
 
-    s_vClearSQ3Value(pDevice);
-    if ( pDevice->byTMax == 0 ) {
-        pDevice->TimerSQ3Tmax3.expires =  RUN_AT(pDevice->byTMax3 * HZ);
-        add_timer(&pDevice->TimerSQ3Tmax3);
-        spin_unlock_irq(&pDevice->lock);
-        return;
-    }
+	}
 
-    bScheduleCommand((void *) pDevice, WLAN_CMD_CHANGE_ANTENNA, NULL);
-    pDevice->byAntennaState = 1;
-    del_timer(&pDevice->TimerSQ3Tmax3);
-    del_timer(&pDevice->TimerSQ3Tmax2);
-    pDevice->TimerSQ3Tmax1.expires =  RUN_AT(pDevice->byTMax * HZ);
-    add_timer(&pDevice->TimerSQ3Tmax1);
+	if (ed_inx == priv->byBBPreEDIndex && !scanning)
+		return;
 
-    spin_unlock_irq(&pDevice->lock);
-}
+	priv->byBBPreEDIndex = ed_inx;
 
-void BBvUpdatePreEDThreshold(struct vnt_private *pDevice, int bScanning)
-{
+	dev_dbg(&priv->usb->dev, "%s byBBPreEDRSSI %d\n",
+					__func__, priv->byBBPreEDRSSI);
 
-    switch(pDevice->byRFType)
-    {
-        case RF_AL2230:
-        case RF_AL2230S:
-        case RF_AIROHA7230:
-            //RobertYu:20060627, update new table
-
-            if( bScanning )
-            {   // need Max sensitivity //RSSI -69, -70,....
-                pDevice->byBBPreEDIndex = 0;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x30); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -69, -70, -71,...\n");
-                break;
-            }
-
-            if(pDevice->byBBPreEDRSSI <= 45) { // RSSI 0, -1,-2,....-45
-                if(pDevice->byBBPreEDIndex == 20) break;
-                pDevice->byBBPreEDIndex = 20;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0xFF); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI 0, -1,-2,..-45\n");
-            } else if(pDevice->byBBPreEDRSSI <= 46)  { //RSSI -46
-                if(pDevice->byBBPreEDIndex == 19) break;
-                pDevice->byBBPreEDIndex = 19;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x1A); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -46\n");
-            } else if(pDevice->byBBPreEDRSSI <= 47)  { //RSSI -47
-                if(pDevice->byBBPreEDIndex == 18) break;
-                pDevice->byBBPreEDIndex = 18;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x15); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -47\n");
-            } else if(pDevice->byBBPreEDRSSI <= 49)  { //RSSI -48, -49
-                if(pDevice->byBBPreEDIndex == 17) break;
-                pDevice->byBBPreEDIndex = 17;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x0E); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -48,-49\n");
-            } else if(pDevice->byBBPreEDRSSI <= 51)  { //RSSI -50, -51
-                if(pDevice->byBBPreEDIndex == 16) break;
-                pDevice->byBBPreEDIndex = 16;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x09); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -50,-51\n");
-            } else if(pDevice->byBBPreEDRSSI <= 53)  { //RSSI -52, -53
-                if(pDevice->byBBPreEDIndex == 15) break;
-                pDevice->byBBPreEDIndex = 15;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x06); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -52,-53\n");
-            } else if(pDevice->byBBPreEDRSSI <= 55)  { //RSSI -54, -55
-                if(pDevice->byBBPreEDIndex == 14) break;
-                pDevice->byBBPreEDIndex = 14;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x03); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -54,-55\n");
-            } else if(pDevice->byBBPreEDRSSI <= 56)  { //RSSI -56
-                if(pDevice->byBBPreEDIndex == 13) break;
-                pDevice->byBBPreEDIndex = 13;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x02); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0xA0); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -56\n");
-            } else if(pDevice->byBBPreEDRSSI <= 57)  { //RSSI -57
-                if(pDevice->byBBPreEDIndex == 12) break;
-                pDevice->byBBPreEDIndex = 12;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x02); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x20); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -57\n");
-            } else if(pDevice->byBBPreEDRSSI <= 58)  { //RSSI -58
-                if(pDevice->byBBPreEDIndex == 11) break;
-                pDevice->byBBPreEDIndex = 11;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x01); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0xA0); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -58\n");
-            } else if(pDevice->byBBPreEDRSSI <= 59)  { //RSSI -59
-                if(pDevice->byBBPreEDIndex == 10) break;
-                pDevice->byBBPreEDIndex = 10;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x01); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x54); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -59\n");
-            } else if(pDevice->byBBPreEDRSSI <= 60)  { //RSSI -60
-                if(pDevice->byBBPreEDIndex == 9) break;
-                pDevice->byBBPreEDIndex = 9;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x01); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x18); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -60\n");
-            } else if(pDevice->byBBPreEDRSSI <= 61)  { //RSSI -61
-                if(pDevice->byBBPreEDIndex == 8) break;
-                pDevice->byBBPreEDIndex = 8;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0xE3); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -61\n");
-            } else if(pDevice->byBBPreEDRSSI <= 62)  { //RSSI -62
-                if(pDevice->byBBPreEDIndex == 7) break;
-                pDevice->byBBPreEDIndex = 7;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0xB9); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -62\n");
-            } else if(pDevice->byBBPreEDRSSI <= 63)  { //RSSI -63
-                if(pDevice->byBBPreEDIndex == 6) break;
-                pDevice->byBBPreEDIndex = 6;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x93); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -63\n");
-            } else if(pDevice->byBBPreEDRSSI <= 64)  { //RSSI -64
-                if(pDevice->byBBPreEDIndex == 5) break;
-                pDevice->byBBPreEDIndex = 5;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x79); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -64\n");
-            } else if(pDevice->byBBPreEDRSSI <= 65)  { //RSSI -65
-                if(pDevice->byBBPreEDIndex == 4) break;
-                pDevice->byBBPreEDIndex = 4;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x62); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -65\n");
-            } else if(pDevice->byBBPreEDRSSI <= 66)  { //RSSI -66
-                if(pDevice->byBBPreEDIndex == 3) break;
-                pDevice->byBBPreEDIndex = 3;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x51); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -66\n");
-            } else if(pDevice->byBBPreEDRSSI <= 67)  { //RSSI -67
-                if(pDevice->byBBPreEDIndex == 2) break;
-                pDevice->byBBPreEDIndex = 2;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x43); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -67\n");
-            } else if(pDevice->byBBPreEDRSSI <= 68)  { //RSSI -68
-                if(pDevice->byBBPreEDIndex == 1) break;
-                pDevice->byBBPreEDIndex = 1;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x36); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -68\n");
-            } else { //RSSI -69, -70,....
-                if(pDevice->byBBPreEDIndex == 0) break;
-                pDevice->byBBPreEDIndex = 0;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x30); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -69, -70,...\n");
-            }
-            break;
-
-        case RF_VT3226:
-        case RF_VT3226D0:
-            //RobertYu:20060627, update new table
-
-            if( bScanning )
-            {   // need Max sensitivity  //RSSI -69, -70, ...
-                pDevice->byBBPreEDIndex = 0;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x24); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -69, -70,..\n");
-                break;
-            }
-
-            if(pDevice->byBBPreEDRSSI <= 41) { // RSSI 0, -1,-2,....-41
-                if(pDevice->byBBPreEDIndex == 22) break;
-                pDevice->byBBPreEDIndex = 22;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0xFF); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI 0, -1,-2,..-41\n");
-            } else if(pDevice->byBBPreEDRSSI <= 42)  { //RSSI -42
-                if(pDevice->byBBPreEDIndex == 21) break;
-                pDevice->byBBPreEDIndex = 21;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x36); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -42\n");
-            } else if(pDevice->byBBPreEDRSSI <= 43)  { //RSSI -43
-                if(pDevice->byBBPreEDIndex == 20) break;
-                pDevice->byBBPreEDIndex = 20;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x26); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -43\n");
-            } else if(pDevice->byBBPreEDRSSI <= 45)  { //RSSI -44, -45
-                if(pDevice->byBBPreEDIndex == 19) break;
-                pDevice->byBBPreEDIndex = 19;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x18); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -44,-45\n");
-            } else if(pDevice->byBBPreEDRSSI <= 47)  { //RSSI -46, -47
-                if(pDevice->byBBPreEDIndex == 18) break;
-                pDevice->byBBPreEDIndex = 18;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x11); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -46,-47\n");
-            } else if(pDevice->byBBPreEDRSSI <= 49)  { //RSSI -48, -49
-                if(pDevice->byBBPreEDIndex == 17) break;
-                pDevice->byBBPreEDIndex = 17;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x0a); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -48,-49\n");
-            } else if(pDevice->byBBPreEDRSSI <= 51)  { //RSSI -50, -51
-                if(pDevice->byBBPreEDIndex == 16) break;
-                pDevice->byBBPreEDIndex = 16;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x07); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -50,-51\n");
-            } else if(pDevice->byBBPreEDRSSI <= 53)  { //RSSI -52, -53
-                if(pDevice->byBBPreEDIndex == 15) break;
-                pDevice->byBBPreEDIndex = 15;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x04); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -52,-53\n");
-            } else if(pDevice->byBBPreEDRSSI <= 55)  { //RSSI -54, -55
-                if(pDevice->byBBPreEDIndex == 14) break;
-                pDevice->byBBPreEDIndex = 14;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x02); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0xC0); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -54,-55\n");
-            } else if(pDevice->byBBPreEDRSSI <= 56)  { //RSSI -56
-                if(pDevice->byBBPreEDIndex == 13) break;
-                pDevice->byBBPreEDIndex = 13;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x02); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x30); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -56\n");
-            } else if(pDevice->byBBPreEDRSSI <= 57)  { //RSSI -57
-                if(pDevice->byBBPreEDIndex == 12) break;
-                pDevice->byBBPreEDIndex = 12;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x01); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0xB0); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -57\n");
-            } else if(pDevice->byBBPreEDRSSI <= 58)  { //RSSI -58
-                if(pDevice->byBBPreEDIndex == 11) break;
-                pDevice->byBBPreEDIndex = 11;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x01); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x70); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -58\n");
-            } else if(pDevice->byBBPreEDRSSI <= 59)  { //RSSI -59
-                if(pDevice->byBBPreEDIndex == 10) break;
-                pDevice->byBBPreEDIndex = 10;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x01); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x30); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -59\n");
-            } else if(pDevice->byBBPreEDRSSI <= 60)  { //RSSI -60
-                if(pDevice->byBBPreEDIndex == 9) break;
-                pDevice->byBBPreEDIndex = 9;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0xEA); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -60\n");
-            } else if(pDevice->byBBPreEDRSSI <= 61)  { //RSSI -61
-                if(pDevice->byBBPreEDIndex == 8) break;
-                pDevice->byBBPreEDIndex = 8;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0xC0); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -61\n");
-            } else if(pDevice->byBBPreEDRSSI <= 62)  { //RSSI -62
-                if(pDevice->byBBPreEDIndex == 7) break;
-                pDevice->byBBPreEDIndex = 7;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x9C); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -62\n");
-            } else if(pDevice->byBBPreEDRSSI <= 63)  { //RSSI -63
-                if(pDevice->byBBPreEDIndex == 6) break;
-                pDevice->byBBPreEDIndex = 6;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x80); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -63\n");
-            } else if(pDevice->byBBPreEDRSSI <= 64)  { //RSSI -64
-                if(pDevice->byBBPreEDIndex == 5) break;
-                pDevice->byBBPreEDIndex = 5;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x68); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -64\n");
-            } else if(pDevice->byBBPreEDRSSI <= 65)  { //RSSI -65
-                if(pDevice->byBBPreEDIndex == 4) break;
-                pDevice->byBBPreEDIndex = 4;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x52); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -65\n");
-            } else if(pDevice->byBBPreEDRSSI <= 66)  { //RSSI -66
-                if(pDevice->byBBPreEDIndex == 3) break;
-                pDevice->byBBPreEDIndex = 3;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x43); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -66\n");
-            } else if(pDevice->byBBPreEDRSSI <= 67)  { //RSSI -67
-                if(pDevice->byBBPreEDIndex == 2) break;
-                pDevice->byBBPreEDIndex = 2;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x36); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -67\n");
-            } else if(pDevice->byBBPreEDRSSI <= 68)  { //RSSI -68
-                if(pDevice->byBBPreEDIndex == 1) break;
-                pDevice->byBBPreEDIndex = 1;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x2D); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -68\n");
-            } else { //RSSI -69, -70, ...
-                if(pDevice->byBBPreEDIndex == 0) break;
-                pDevice->byBBPreEDIndex = 0;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x24); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -69, -70,..\n");
-            }
-            break;
-
-        case RF_VT3342A0: //RobertYu:20060627, testing table
-            if( bScanning )
-            {   // need Max sensitivity  //RSSI -67, -68, ...
-                pDevice->byBBPreEDIndex = 0;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x38); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -67, -68,..\n");
-                break;
-            }
-
-            if(pDevice->byBBPreEDRSSI <= 41) { // RSSI 0, -1,-2,....-41
-                if(pDevice->byBBPreEDIndex == 20) break;
-                pDevice->byBBPreEDIndex = 20;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0xFF); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI 0, -1,-2,..-41\n");
-            } else if(pDevice->byBBPreEDRSSI <= 42)  { //RSSI -42
-                if(pDevice->byBBPreEDIndex == 19) break;
-                pDevice->byBBPreEDIndex = 19;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x36); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -42\n");
-            } else if(pDevice->byBBPreEDRSSI <= 43)  { //RSSI -43
-                if(pDevice->byBBPreEDIndex == 18) break;
-                pDevice->byBBPreEDIndex = 18;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x26); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -43\n");
-            } else if(pDevice->byBBPreEDRSSI <= 45)  { //RSSI -44, -45
-                if(pDevice->byBBPreEDIndex == 17) break;
-                pDevice->byBBPreEDIndex = 17;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x18); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -44,-45\n");
-            } else if(pDevice->byBBPreEDRSSI <= 47)  { //RSSI -46, -47
-                if(pDevice->byBBPreEDIndex == 16) break;
-                pDevice->byBBPreEDIndex = 16;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x11); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -46,-47\n");
-            } else if(pDevice->byBBPreEDRSSI <= 49)  { //RSSI -48, -49
-                if(pDevice->byBBPreEDIndex == 15) break;
-                pDevice->byBBPreEDIndex = 15;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x0a); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -48,-49\n");
-            } else if(pDevice->byBBPreEDRSSI <= 51)  { //RSSI -50, -51
-                if(pDevice->byBBPreEDIndex == 14) break;
-                pDevice->byBBPreEDIndex = 14;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x07); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -50,-51\n");
-            } else if(pDevice->byBBPreEDRSSI <= 53)  { //RSSI -52, -53
-                if(pDevice->byBBPreEDIndex == 13) break;
-                pDevice->byBBPreEDIndex = 13;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x04); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x00); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -52,-53\n");
-            } else if(pDevice->byBBPreEDRSSI <= 55)  { //RSSI -54, -55
-                if(pDevice->byBBPreEDIndex == 12) break;
-                pDevice->byBBPreEDIndex = 12;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x02); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0xC0); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -54,-55\n");
-            } else if(pDevice->byBBPreEDRSSI <= 56)  { //RSSI -56
-                if(pDevice->byBBPreEDIndex == 11) break;
-                pDevice->byBBPreEDIndex = 11;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x02); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x30); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -56\n");
-            } else if(pDevice->byBBPreEDRSSI <= 57)  { //RSSI -57
-                if(pDevice->byBBPreEDIndex == 10) break;
-                pDevice->byBBPreEDIndex = 10;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x01); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0xB0); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -57\n");
-            } else if(pDevice->byBBPreEDRSSI <= 58)  { //RSSI -58
-                if(pDevice->byBBPreEDIndex == 9) break;
-                pDevice->byBBPreEDIndex = 9;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x01); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x70); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -58\n");
-            } else if(pDevice->byBBPreEDRSSI <= 59)  { //RSSI -59
-                if(pDevice->byBBPreEDIndex == 8) break;
-                pDevice->byBBPreEDIndex = 8;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x01); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x30); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -59\n");
-            } else if(pDevice->byBBPreEDRSSI <= 60)  { //RSSI -60
-                if(pDevice->byBBPreEDIndex == 7) break;
-                pDevice->byBBPreEDIndex = 7;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0xEA); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -60\n");
-            } else if(pDevice->byBBPreEDRSSI <= 61)  { //RSSI -61
-                if(pDevice->byBBPreEDIndex == 6) break;
-                pDevice->byBBPreEDIndex = 6;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0xC0); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -61\n");
-            } else if(pDevice->byBBPreEDRSSI <= 62)  { //RSSI -62
-                if(pDevice->byBBPreEDIndex == 5) break;
-                pDevice->byBBPreEDIndex = 5;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x9C); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -62\n");
-            } else if(pDevice->byBBPreEDRSSI <= 63)  { //RSSI -63
-                if(pDevice->byBBPreEDIndex == 4) break;
-                pDevice->byBBPreEDIndex = 4;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x80); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -63\n");
-            } else if(pDevice->byBBPreEDRSSI <= 64)  { //RSSI -64
-                if(pDevice->byBBPreEDIndex == 3) break;
-                pDevice->byBBPreEDIndex = 3;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x68); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -64\n");
-            } else if(pDevice->byBBPreEDRSSI <= 65)  { //RSSI -65
-                if(pDevice->byBBPreEDIndex == 2) break;
-                pDevice->byBBPreEDIndex = 2;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x52); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -65\n");
-            } else if(pDevice->byBBPreEDRSSI <= 66)  { //RSSI -66
-                if(pDevice->byBBPreEDIndex == 1) break;
-                pDevice->byBBPreEDIndex = 1;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x43); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -66\n");
-            } else { //RSSI -67, -68, ...
-                if(pDevice->byBBPreEDIndex == 0) break;
-                pDevice->byBBPreEDIndex = 0;
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xC9, 0x00); //CR201(0xC9)
-                ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xCE, 0x38); //CR206(0xCE)
-                DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"            pDevice->byBBPreEDRSSI -67, -68,..\n");
-            }
-            break;
-
-    }
+	if (!cr_201 && !cr_206)
+		return;
 
+	vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0xc9, cr_201);
+	vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0xce, cr_206);
 }
 
diff --git a/drivers/staging/vt6656/baseband.h b/drivers/staging/vt6656/baseband.h
index 79faedf4a5e..3044d6c4205 100644
--- a/drivers/staging/vt6656/baseband.h
+++ b/drivers/staging/vt6656/baseband.h
@@ -88,23 +88,11 @@ struct vnt_phy_field {
 	__le16 len;
 } __packed;
 
-unsigned int
-BBuGetFrameTime(
-     u8 byPreambleType,
-     u8 byFreqType,
-     unsigned int cbFrameLength,
-     u16 wRate
-    );
+unsigned int BBuGetFrameTime(u8 preamble_type, u8 pkt_type,
+	unsigned int frame_length, u16 tx_rate);
 
-void BBvCalculateParameter(struct vnt_private *, u32 cbFrameLength,
-	u16 wRate, u8 byPacketType, struct vnt_phy_field *);
-
-/* timer for antenna diversity */
-
-void TimerSQ3CallBack(struct vnt_private *);
-void TimerSQ3Tmax3CallBack(struct vnt_private *);
-
-void BBvAntennaDiversity(struct vnt_private *, u8 byRxRate, u8 bySQ3);
+void BBvCalculateParameter(struct vnt_private *, u32 frame_length,
+	u16 tx_rate, u8 pkt_type, struct vnt_phy_field *);
 
 void BBvSetShortSlotTime(struct vnt_private *);
 void BBvSetVGAGainOffset(struct vnt_private *, u8 byData);
@@ -112,6 +100,6 @@ void BBvSetAntennaMode(struct vnt_private *, u8 byAntennaMode);
 int BBbVT3184Init(struct vnt_private *);
 void BBvSetDeepSleep(struct vnt_private *);
 void BBvExitDeepSleep(struct vnt_private *);
-void BBvUpdatePreEDThreshold(struct vnt_private *, int bScanning);
+void BBvUpdatePreEDThreshold(struct vnt_private *, int scanning);
 
 #endif /* __BASEBAND_H__ */
diff --git a/drivers/staging/vt6656/bssdb.c b/drivers/staging/vt6656/bssdb.c
index 9c78dab95d3..8e9ce96442a 100644
--- a/drivers/staging/vt6656/bssdb.c
+++ b/drivers/staging/vt6656/bssdb.c
@@ -53,8 +53,7 @@
 #include "card.h"
 #include "mac.h"
 #include "wpa2.h"
-#include "control.h"
-#include "rndis.h"
+#include "usbpipe.h"
 #include "iowpa.h"
 #include "power.h"
 
@@ -456,16 +455,15 @@ int BSSbInsertToBSSList(struct vnt_private *pDevice,
 		}
 	}
 
-	if (pDevice->bUpdateBBVGA) {
-		/* Monitor if RSSI is too strong. */
-		pBSSList->byRSSIStatCnt = 0;
-		RFvRSSITodBm(pDevice, (u8) (pRxPacket->uRSSI),
-			     &pBSSList->ldBmMAX);
-		pBSSList->ldBmAverage[0] = pBSSList->ldBmMAX;
-		pBSSList->ldBmAverRange = pBSSList->ldBmMAX;
-		for (ii = 1; ii < RSSI_STAT_COUNT; ii++)
-			pBSSList->ldBmAverage[ii] = 0;
-	}
+	/* Monitor if RSSI is too strong. */
+	pBSSList->byRSSIStatCnt = 0;
+
+	vnt_rf_rssi_to_dbm(pDevice, (u8)pRxPacket->uRSSI, &pBSSList->ldBmMAX);
+
+	pBSSList->ldBmAverage[0] = pBSSList->ldBmMAX;
+	pBSSList->ldBmAverRange = pBSSList->ldBmMAX;
+	for (ii = 1; ii < RSSI_STAT_COUNT; ii++)
+		pBSSList->ldBmAverage[ii] = 0;
 
 	pBSSList->uIELength = uIELength;
 	if (pBSSList->uIELength > WLAN_BEACON_FR_MAXLEN)
@@ -581,7 +579,7 @@ int BSSbUpdateToBSSList(struct vnt_private *pDevice,
 	}
 
 	if (pRxPacket->uRSSI != 0) {
-		RFvRSSITodBm(pDevice, (u8) (pRxPacket->uRSSI), &ldBm);
+		vnt_rf_rssi_to_dbm(pDevice, (u8)pRxPacket->uRSSI, &ldBm);
 		/* Monitor if RSSI is too strong. */
 		pBSSList->byRSSIStatCnt++;
 		pBSSList->byRSSIStatCnt %= RSSI_STAT_COUNT;
@@ -774,8 +772,8 @@ void BSSvAddMulticastNode(struct vnt_private *pDevice)
 {
 	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
 
-	if (!pDevice->bEnableHostWEP)
-		memset(&pMgmt->sNodeDBTable[0], 0, sizeof(KnownNodeDB));
+	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;
@@ -818,8 +816,6 @@ void BSSvSecondCallBack(struct work_struct *work)
 	if (pDevice->Flags & fMP_DISCONNECTED)
 		return;
 
-	spin_lock_irq(&pDevice->lock);
-
 	pDevice->uAssocCount = 0;
 
 	/* Power Saving Mode Tx Burst */
@@ -1001,10 +997,8 @@ void BSSvSecondCallBack(struct work_struct *work)
 
 		if (pMgmt->sNodeDBTable[0].bActive) { /* Assoc with BSS */
 
-			if (pDevice->bUpdateBBVGA) {
-				s_vCheckSensitivity(pDevice);
-				s_vCheckPreEDThreshold(pDevice);
-			}
+			s_vCheckSensitivity(pDevice);
+			s_vCheckPreEDThreshold(pDevice);
 
 			if (pMgmt->sNodeDBTable[0].uInActiveCount >=
 							(LOST_BEACON_COUNT/2) &&
@@ -1022,10 +1016,10 @@ void BSSvSecondCallBack(struct work_struct *work)
 				pMgmt->eCurrState = WMAC_STATE_IDLE;
 				netif_stop_queue(pDevice->dev);
 				pDevice->bLinkPass = false;
-				ControlvMaskByte(pDevice,
-						 MESSAGE_REQUEST_MACREG,
-						 MAC_REG_PAPEDELAY, LEDSTS_STS,
-						 LEDSTS_SLOW);
+
+				vnt_mac_set_led(pDevice, LEDSTS_STS,
+								LEDSTS_SLOW);
+
 				pDevice->bRoaming = true;
 				pDevice->bIsRoaming = false;
 
@@ -1121,10 +1115,9 @@ void BSSvSecondCallBack(struct work_struct *work)
 		}
 		if (pMgmt->eCurrState == WMAC_STATE_JOINTED) {
 
-			if (pDevice->bUpdateBBVGA) {
-				s_vCheckSensitivity(pDevice);
-				s_vCheckPreEDThreshold(pDevice);
-			}
+			s_vCheckSensitivity(pDevice);
+			s_vCheckPreEDThreshold(pDevice);
+
 			if (pMgmt->sNodeDBTable[0].uInActiveCount >=
 						ADHOC_LOST_BEACON_COUNT) {
 				DBG_PRT(MSG_LEVEL_NOTICE,
@@ -1134,10 +1127,8 @@ void BSSvSecondCallBack(struct work_struct *work)
 				pMgmt->eCurrState = WMAC_STATE_STARTED;
 				netif_stop_queue(pDevice->dev);
 				pDevice->bLinkPass = false;
-				ControlvMaskByte(pDevice,
-						 MESSAGE_REQUEST_MACREG,
-						 MAC_REG_PAPEDELAY, LEDSTS_STS,
-						 LEDSTS_SLOW);
+				vnt_mac_set_led(pDevice, LEDSTS_STS,
+								LEDSTS_SLOW);
 			}
 		}
 	}
@@ -1158,8 +1149,6 @@ void BSSvSecondCallBack(struct work_struct *work)
 			netif_wake_queue(pDevice->dev);
 	}
 
-	spin_unlock_irq(&pDevice->lock);
-
 	schedule_delayed_work(&pDevice->second_callback_work, HZ);
 }
 
@@ -1429,7 +1418,7 @@ static void s_uCalculateLinkQual(struct vnt_private *pDevice)
 	if (pDevice->bLinkPass != true) {
 		pDevice->wstats.qual.qual = 0;
 	} else {
-		RFvRSSITodBm(pDevice, (u8) (pDevice->uCurrRSSI), &ldBm);
+		vnt_rf_rssi_to_dbm(pDevice, (u8) (pDevice->uCurrRSSI), &ldBm);
 		if (-ldBm < 50)
 			RssiRatio = 4000;
 		else if (-ldBm > 90)
diff --git a/drivers/staging/vt6656/card.c b/drivers/staging/vt6656/card.c
index 0d8772858f0..d662e5431da 100644
--- a/drivers/staging/vt6656/card.c
+++ b/drivers/staging/vt6656/card.c
@@ -57,11 +57,7 @@
 #include "rc4.h"
 #include "country.h"
 #include "datarate.h"
-#include "rndis.h"
-#include "control.h"
-
-//static int          msglevel                =MSG_LEVEL_DEBUG;
-static int          msglevel                =MSG_LEVEL_INFO;
+#include "usbpipe.h"
 
 //const u16 cwRXBCNTSFOff[MAX_RATE] =
 //{17, 34, 96, 192, 34, 23, 17, 11, 8, 5, 4, 3};
@@ -98,24 +94,24 @@ void CARDbSetMediaChannel(struct vnt_private *priv, u32 connection_channel)
 	/* Set Channel[7] = 0 to tell H/W channel is changing now. */
 	MACvRegBitsOff(priv, MAC_REG_CHANNEL, 0xb0);
 
-	CONTROLnsRequestOut(priv, MESSAGE_TYPE_SELECT_CHANNLE,
+	vnt_control_out(priv, MESSAGE_TYPE_SELECT_CHANNLE,
 					connection_channel, 0, 0, NULL);
 
 	if (priv->byBBType == BB_TYPE_11A) {
 		priv->byCurPwr = 0xff;
-		RFbRawSetPower(priv,
+		vnt_rf_set_txpower(priv,
 			priv->abyOFDMAPwrTbl[connection_channel-15], RATE_54M);
 	} else if (priv->byBBType == BB_TYPE_11G) {
 		priv->byCurPwr = 0xff;
-		RFbRawSetPower(priv,
+		vnt_rf_set_txpower(priv,
 			priv->abyOFDMPwrTbl[connection_channel-1], RATE_54M);
 	} else {
 		priv->byCurPwr = 0xff;
-		RFbRawSetPower(priv,
+		vnt_rf_set_txpower(priv,
 			priv->abyCCKPwrTbl[connection_channel-1], RATE_1M);
 	}
 
-	ControlvWriteByte(priv, MESSAGE_REQUEST_MACREG, MAC_REG_CHANNEL,
+	vnt_control_out_u8(priv, MESSAGE_REQUEST_MACREG, MAC_REG_CHANNEL,
 		(u8)(connection_channel|0x80));
 }
 
@@ -124,20 +120,20 @@ void CARDbSetMediaChannel(struct vnt_private *priv, u32 connection_channel)
  *
  * Parameters:
  *  In:
- *      pDevice             - The adapter to be set
- *      wRateIdx            - Receiving data rate
+ *      priv		- The adapter to be set
+ *      rate_idx	- Receiving data rate
  *  Out:
  *      none
  *
  * Return Value: response Control frame rate
  *
  */
-static u16 swGetCCKControlRate(struct vnt_private *pDevice, u16 wRateIdx)
+static u16 swGetCCKControlRate(struct vnt_private *priv, u16 rate_idx)
 {
-	u16 ui = wRateIdx;
+	u16 ui = rate_idx;
 
 	while (ui > RATE_1M) {
-		if (pDevice->wBasicRate & (1 << ui))
+		if (priv->wBasicRate & (1 << ui))
 			return ui;
 		ui--;
 	}
@@ -150,39 +146,39 @@ static u16 swGetCCKControlRate(struct vnt_private *pDevice, u16 wRateIdx)
  *
  * Parameters:
  *  In:
- *      pDevice             - The adapter to be set
- *      wRateIdx            - Receiving data rate
+ *      priv		- The adapter to be set
+ *      rate_idx	- Receiving data rate
  *  Out:
  *      none
  *
  * Return Value: response Control frame rate
  *
  */
-static u16 swGetOFDMControlRate(struct vnt_private *pDevice, u16 wRateIdx)
+static u16 swGetOFDMControlRate(struct vnt_private *priv, u16 rate_idx)
 {
-	u16 ui = wRateIdx;
+	u16 ui = rate_idx;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"BASIC RATE: %X\n",
-		pDevice->wBasicRate);
+	dev_dbg(&priv->usb->dev, "%s basic rate: %d\n",
+					__func__,  priv->wBasicRate);
 
-	if (!CARDbIsOFDMinBasicRate(pDevice)) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-			"swGetOFDMControlRate:(NO OFDM) %d\n", wRateIdx);
-		if (wRateIdx > RATE_24M)
-			wRateIdx = RATE_24M;
-		return wRateIdx;
+	if (!CARDbIsOFDMinBasicRate(priv)) {
+		dev_dbg(&priv->usb->dev, "%s (NO OFDM) %d\n",
+						__func__, rate_idx);
+		if (rate_idx > RATE_24M)
+			rate_idx = RATE_24M;
+		return rate_idx;
 	}
 
 	while (ui > RATE_11M) {
-		if (pDevice->wBasicRate & (1 << ui)) {
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-				"swGetOFDMControlRate: %d\n", ui);
+		if (priv->wBasicRate & (1 << ui)) {
+			dev_dbg(&priv->usb->dev, "%s rate: %d\n",
+							__func__, ui);
 			return ui;
 		}
 		ui--;
 	}
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"swGetOFDMControlRate: 6M\n");
+	dev_dbg(&priv->usb->dev, "%s basic rate: 24M\n", __func__);
 
 	return RATE_24M;
 }
@@ -191,114 +187,95 @@ static u16 swGetOFDMControlRate(struct vnt_private *pDevice, u16 wRateIdx)
  * Description: Calculate TxRate and RsvTime fields for RSPINF in OFDM mode.
  *
  * Parameters:
- *  In:
- *      wRate           - Tx Rate
- *      byPktType       - Tx Packet type
- *  Out:
- *      pbyTxRate       - pointer to RSPINF TxRate field
- *      pbyRsvTime      - pointer to RSPINF RsvTime field
+ * In:
+ *	rate	- Tx Rate
+ *	bb_type	- Tx Packet type
+ * Out:
+ *	tx_rate	- pointer to RSPINF TxRate field
+ *	rsv_time- pointer to RSPINF RsvTime field
  *
  * Return Value: none
  *
  */
-static void
-CARDvCalculateOFDMRParameter (
-      u16 wRate,
-      u8 byBBType,
-     u8 * pbyTxRate,
-     u8 * pbyRsvTime
-    )
+static void CARDvCalculateOFDMRParameter(u16 rate, u8 bb_type,
+					u8 *tx_rate, u8 *rsv_time)
 {
-    switch (wRate) {
-    case RATE_6M :
-        if (byBBType == BB_TYPE_11A) {//5GHZ
-            *pbyTxRate = 0x9B;
-            *pbyRsvTime = 24;
-        }
-        else {
-            *pbyTxRate = 0x8B;
-            *pbyRsvTime = 30;
-        }
-        break;
-
-    case RATE_9M :
-        if (byBBType == BB_TYPE_11A) {//5GHZ
-            *pbyTxRate = 0x9F;
-            *pbyRsvTime = 16;
-        }
-        else {
-            *pbyTxRate = 0x8F;
-            *pbyRsvTime = 22;
-        }
-        break;
-
-   case RATE_12M :
-        if (byBBType == BB_TYPE_11A) {//5GHZ
-            *pbyTxRate = 0x9A;
-            *pbyRsvTime = 12;
-        }
-        else {
-            *pbyTxRate = 0x8A;
-            *pbyRsvTime = 18;
-        }
-        break;
-
-   case RATE_18M :
-        if (byBBType == BB_TYPE_11A) {//5GHZ
-            *pbyTxRate = 0x9E;
-            *pbyRsvTime = 8;
-        }
-        else {
-            *pbyTxRate = 0x8E;
-            *pbyRsvTime = 14;
-        }
-        break;
-
-    case RATE_36M :
-        if (byBBType == BB_TYPE_11A) {//5GHZ
-            *pbyTxRate = 0x9D;
-            *pbyRsvTime = 4;
-        }
-        else {
-            *pbyTxRate = 0x8D;
-            *pbyRsvTime = 10;
-        }
-        break;
-
-    case RATE_48M :
-        if (byBBType == BB_TYPE_11A) {//5GHZ
-            *pbyTxRate = 0x98;
-            *pbyRsvTime = 4;
-        }
-        else {
-            *pbyTxRate = 0x88;
-            *pbyRsvTime = 10;
-        }
-        break;
-
-    case RATE_54M :
-        if (byBBType == BB_TYPE_11A) {//5GHZ
-            *pbyTxRate = 0x9C;
-            *pbyRsvTime = 4;
-        }
-        else {
-            *pbyTxRate = 0x8C;
-            *pbyRsvTime = 10;
-        }
-        break;
-
-    case RATE_24M :
-    default :
-        if (byBBType == BB_TYPE_11A) {//5GHZ
-            *pbyTxRate = 0x99;
-            *pbyRsvTime = 8;
-        }
-        else {
-            *pbyTxRate = 0x89;
-            *pbyRsvTime = 14;
-        }
-        break;
-    }
+
+	switch (rate) {
+	case RATE_6M:
+		if (bb_type == BB_TYPE_11A) {
+			*tx_rate = 0x9b;
+			*rsv_time = 24;
+		} else {
+			*tx_rate = 0x8b;
+			*rsv_time = 30;
+		}
+			break;
+	case RATE_9M:
+		if (bb_type == BB_TYPE_11A) {
+			*tx_rate = 0x9f;
+			*rsv_time = 16;
+		} else {
+			*tx_rate = 0x8f;
+			*rsv_time = 22;
+		}
+		break;
+	case RATE_12M:
+		if (bb_type == BB_TYPE_11A) {
+			*tx_rate = 0x9a;
+			*rsv_time = 12;
+		} else {
+			*tx_rate = 0x8a;
+			*rsv_time = 18;
+		}
+		break;
+	case RATE_18M:
+		if (bb_type == BB_TYPE_11A) {
+			*tx_rate = 0x9e;
+			*rsv_time = 8;
+		} else {
+			*tx_rate = 0x8e;
+			*rsv_time = 14;
+		}
+		break;
+	case RATE_36M:
+		if (bb_type == BB_TYPE_11A) {
+			*tx_rate = 0x9d;
+			*rsv_time = 4;
+		} else {
+			*tx_rate = 0x8d;
+			*rsv_time = 10;
+		}
+		break;
+	case RATE_48M:
+		if (bb_type == BB_TYPE_11A) {
+			*tx_rate = 0x98;
+			*rsv_time = 4;
+		} else {
+			*tx_rate = 0x88;
+			*rsv_time = 10;
+		}
+		break;
+	case RATE_54M:
+		if (bb_type == BB_TYPE_11A) {
+			*tx_rate = 0x9c;
+			*rsv_time = 4;
+		} else {
+			*tx_rate = 0x8c;
+			*rsv_time = 10;
+		}
+		break;
+	case RATE_24M:
+	default:
+		if (bb_type == BB_TYPE_11A) {
+			*tx_rate = 0x99;
+			*rsv_time = 8;
+		} else {
+			*tx_rate = 0x89;
+			*rsv_time = 14;
+		}
+		break;
+	}
 }
 
 /*
@@ -313,112 +290,91 @@ CARDvCalculateOFDMRParameter (
  * Return Value: None.
  *
  */
-void CARDvSetRSPINF(struct vnt_private *pDevice, u8 byBBType)
+
+void CARDvSetRSPINF(struct vnt_private *priv, u8 bb_type)
 {
 	struct vnt_phy_field phy[4];
-	u8 abyTxRate[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; /* For OFDM */
-	u8 abyRsvTime[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
-	u8 abyData[34];
+	u8 tx_rate[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; /* For OFDM */
+	u8 rsv_time[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
+	u8 data[34];
 	int i;
 
-    //RSPINF_b_1
-	BBvCalculateParameter(pDevice, 14,
-		swGetCCKControlRate(pDevice, RATE_1M), PK_TYPE_11B, &phy[0]);
-
-    ///RSPINF_b_2
-	BBvCalculateParameter(pDevice, 14,
-		swGetCCKControlRate(pDevice, RATE_2M), PK_TYPE_11B, &phy[1]);
-
-    //RSPINF_b_5
-	BBvCalculateParameter(pDevice, 14,
-		swGetCCKControlRate(pDevice, RATE_5M), PK_TYPE_11B, &phy[2]);
-
-    //RSPINF_b_11
-	BBvCalculateParameter(pDevice, 14,
-		swGetCCKControlRate(pDevice, RATE_11M), PK_TYPE_11B, &phy[3]);
-
-    //RSPINF_a_6
-    CARDvCalculateOFDMRParameter (RATE_6M,
-                                 byBBType,
-                                 &abyTxRate[0],
-                                 &abyRsvTime[0]);
-
-    //RSPINF_a_9
-    CARDvCalculateOFDMRParameter (RATE_9M,
-                                 byBBType,
-                                 &abyTxRate[1],
-                                 &abyRsvTime[1]);
-
-    //RSPINF_a_12
-    CARDvCalculateOFDMRParameter (RATE_12M,
-                                 byBBType,
-                                 &abyTxRate[2],
-                                 &abyRsvTime[2]);
-
-    //RSPINF_a_18
-    CARDvCalculateOFDMRParameter (RATE_18M,
-                                 byBBType,
-                                 &abyTxRate[3],
-                                 &abyRsvTime[3]);
-
-    //RSPINF_a_24
-    CARDvCalculateOFDMRParameter (RATE_24M,
-                                 byBBType,
-                                 &abyTxRate[4],
-                                 &abyRsvTime[4]);
-
-    //RSPINF_a_36
-    CARDvCalculateOFDMRParameter (swGetOFDMControlRate(pDevice, RATE_36M),
-                                 byBBType,
-                                 &abyTxRate[5],
-                                 &abyRsvTime[5]);
-
-    //RSPINF_a_48
-    CARDvCalculateOFDMRParameter (swGetOFDMControlRate(pDevice, RATE_48M),
-                                 byBBType,
-                                 &abyTxRate[6],
-                                 &abyRsvTime[6]);
-
-    //RSPINF_a_54
-    CARDvCalculateOFDMRParameter (swGetOFDMControlRate(pDevice, RATE_54M),
-                                 byBBType,
-                                 &abyTxRate[7],
-                                 &abyRsvTime[7]);
-
-    //RSPINF_a_72
-    CARDvCalculateOFDMRParameter (swGetOFDMControlRate(pDevice, RATE_54M),
-                                 byBBType,
-                                 &abyTxRate[8],
-                                 &abyRsvTime[8]);
-
-	put_unaligned(phy[0].len, (u16 *)&abyData[0]);
-	abyData[2] = phy[0].signal;
-	abyData[3] = phy[0].service;
-
-	put_unaligned(phy[1].len, (u16 *)&abyData[4]);
-	abyData[6] = phy[1].signal;
-	abyData[7] = phy[1].service;
-
-	put_unaligned(phy[2].len, (u16 *)&abyData[8]);
-	abyData[10] = phy[2].signal;
-	abyData[11] = phy[2].service;
-
-	put_unaligned(phy[3].len, (u16 *)&abyData[12]);
-	abyData[14] = phy[3].signal;
-	abyData[15] = phy[3].service;
-
-    for (i = 0; i < 9; i++) {
-	abyData[16+i*2] = abyTxRate[i];
-	abyData[16+i*2+1] = abyRsvTime[i];
-    }
-
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_WRITE,
-                        MAC_REG_RSPINF_B_1,
-                        MESSAGE_REQUEST_MACREG,
-                        34,
-                        &abyData[0]);
+	/*RSPINF_b_1*/
+	BBvCalculateParameter(priv, 14,
+		swGetCCKControlRate(priv, RATE_1M), PK_TYPE_11B, &phy[0]);
+
+	/*RSPINF_b_2*/
+	BBvCalculateParameter(priv, 14,
+		swGetCCKControlRate(priv, RATE_2M), PK_TYPE_11B, &phy[1]);
+
+	/*RSPINF_b_5*/
+	BBvCalculateParameter(priv, 14,
+		swGetCCKControlRate(priv, RATE_5M), PK_TYPE_11B, &phy[2]);
+
+	/*RSPINF_b_11*/
+	BBvCalculateParameter(priv, 14,
+		swGetCCKControlRate(priv, RATE_11M), PK_TYPE_11B, &phy[3]);
+
+
+	/*RSPINF_a_6*/
+	CARDvCalculateOFDMRParameter(RATE_6M, bb_type,
+						&tx_rate[0], &rsv_time[0]);
 
+	/*RSPINF_a_9*/
+	CARDvCalculateOFDMRParameter(RATE_9M, bb_type,
+						&tx_rate[1], &rsv_time[1]);
+
+	/*RSPINF_a_12*/
+	CARDvCalculateOFDMRParameter(RATE_12M, bb_type,
+						&tx_rate[2], &rsv_time[2]);
+
+	/*RSPINF_a_18*/
+	CARDvCalculateOFDMRParameter(RATE_18M, bb_type,
+						&tx_rate[3], &rsv_time[3]);
+
+	/*RSPINF_a_24*/
+	CARDvCalculateOFDMRParameter(RATE_24M, bb_type,
+						&tx_rate[4], &rsv_time[4]);
+
+	/*RSPINF_a_36*/
+	CARDvCalculateOFDMRParameter(swGetOFDMControlRate(priv, RATE_36M),
+					bb_type, &tx_rate[5], &rsv_time[5]);
+
+	/*RSPINF_a_48*/
+	CARDvCalculateOFDMRParameter(swGetOFDMControlRate(priv, RATE_48M),
+					bb_type, &tx_rate[6], &rsv_time[6]);
+
+	/*RSPINF_a_54*/
+	CARDvCalculateOFDMRParameter(swGetOFDMControlRate(priv, RATE_54M),
+					bb_type, &tx_rate[7], &rsv_time[7]);
+
+	/*RSPINF_a_72*/
+	CARDvCalculateOFDMRParameter(swGetOFDMControlRate(priv, RATE_54M),
+					bb_type, &tx_rate[8], &rsv_time[8]);
+
+	put_unaligned(phy[0].len, (u16 *)&data[0]);
+	data[2] = phy[0].signal;
+	data[3] = phy[0].service;
+
+	put_unaligned(phy[1].len, (u16 *)&data[4]);
+	data[6] = phy[1].signal;
+	data[7] = phy[1].service;
+
+	put_unaligned(phy[2].len, (u16 *)&data[8]);
+	data[10] = phy[2].signal;
+	data[11] = phy[2].service;
+
+	put_unaligned(phy[3].len, (u16 *)&data[12]);
+	data[14] = phy[3].signal;
+	data[15] = phy[3].service;
+
+	for (i = 0; i < 9; i++) {
+		data[16 + i * 2] = tx_rate[i];
+		data[16 + i * 2 + 1] = rsv_time[i];
+	}
+
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE,
+		MAC_REG_RSPINF_B_1, MESSAGE_REQUEST_MACREG, 34, &data[0]);
 }
 
 /*
@@ -426,120 +382,119 @@ void CARDvSetRSPINF(struct vnt_private *pDevice, u8 byBBType)
  *
  * Parameters:
  *  In:
- *      pDevice             - The adapter to be set
- *  Out:
- *      none
+ *	priv - The adapter to be set
+ * Out:
+ *	none
  *
  * Return Value: None.
  *
  */
-void vUpdateIFS(struct vnt_private *pDevice)
+void vUpdateIFS(struct vnt_private *priv)
 {
-	u8 byMaxMin = 0;
-	u8 byData[4];
-
-    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
-        u8 byRate = 0;
-        bool bOFDMRate = false;
-	unsigned int ii = 0;
-        PWLAN_IE_SUPP_RATES pItemRates = NULL;
-
-        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;
-
-	pItemRates = (PWLAN_IE_SUPP_RATES)pDevice->vnt_mgmt.abyCurrSuppRates;
-        for (ii = 0; ii < pItemRates->len; ii++) {
-            byRate = (u8)(pItemRates->abyRates[ii]&0x7F);
-            if (RATEwGetRateIdx(byRate) > RATE_11M) {
-                bOFDMRate = true;
-                break;
-            }
-        }
-        if (bOFDMRate == false) {
-		pItemRates = (PWLAN_IE_SUPP_RATES)pDevice->vnt_mgmt
-			.abyCurrExtSuppRates;
-            for (ii = 0; ii < pItemRates->len; ii++) {
-                byRate = (u8)(pItemRates->abyRates[ii]&0x7F);
-                if (RATEwGetRateIdx(byRate) > RATE_11M) {
-                    bOFDMRate = true;
-                    break;
-                }
-            }
-        }
-        if (bOFDMRate == true) {
-            pDevice->uCwMin = C_CWMIN_A;
-            byMaxMin = 4;
-        } else {
-            pDevice->uCwMin = C_CWMIN_B;
-            byMaxMin = 5;
-        }
-    }
-
-    pDevice->uCwMax = C_CWMAX;
-    pDevice->uEIFS = C_EIFS;
-
-    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,
-                        MESSAGE_REQUEST_MACREG,
-                        4,
-                        &byData[0]);
-
-    byMaxMin |= 0xA0;//1010 1111,C_CWMAX = 1023
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_WRITE,
-                        MAC_REG_CWMAXMIN0,
-                        MESSAGE_REQUEST_MACREG,
-                        1,
-                        &byMaxMin);
+	u8 max_min = 0;
+	u8 data[4];
+
+	if (priv->byPacketType == PK_TYPE_11A) {
+		priv->uSlot = C_SLOT_SHORT;
+		priv->uSIFS = C_SIFS_A;
+		priv->uDIFS = C_SIFS_A + 2 * C_SLOT_SHORT;
+		priv->uCwMin = C_CWMIN_A;
+		max_min = 4;
+	} else if (priv->byPacketType == PK_TYPE_11B) {
+		priv->uSlot = C_SLOT_LONG;
+		priv->uSIFS = C_SIFS_BG;
+		priv->uDIFS = C_SIFS_BG + 2 * C_SLOT_LONG;
+		priv->uCwMin = C_CWMIN_B;
+		max_min = 5;
+	} else {/* PK_TYPE_11GA & PK_TYPE_11GB */
+		u8 rate = 0;
+		bool ofdm_rate = false;
+		unsigned int ii = 0;
+		PWLAN_IE_SUPP_RATES item_rates = NULL;
+
+		priv->uSIFS = C_SIFS_BG;
+
+		if (priv->bShortSlotTime)
+			priv->uSlot = C_SLOT_SHORT;
+		else
+			priv->uSlot = C_SLOT_LONG;
+
+		priv->uDIFS = C_SIFS_BG + 2 * priv->uSlot;
+
+		item_rates =
+			(PWLAN_IE_SUPP_RATES)priv->vnt_mgmt.abyCurrSuppRates;
+
+		for (ii = 0; ii < item_rates->len; ii++) {
+			rate = (u8)(item_rates->abyRates[ii] & 0x7f);
+			if (RATEwGetRateIdx(rate) > RATE_11M) {
+				ofdm_rate = true;
+				break;
+			}
+		}
+
+		if (ofdm_rate == false) {
+			item_rates = (PWLAN_IE_SUPP_RATES)priv->vnt_mgmt
+				.abyCurrExtSuppRates;
+			for (ii = 0; ii < item_rates->len; ii++) {
+				rate = (u8)(item_rates->abyRates[ii] & 0x7f);
+				if (RATEwGetRateIdx(rate) > RATE_11M) {
+					ofdm_rate = true;
+					break;
+				}
+			}
+		}
+
+		if (ofdm_rate == true) {
+			priv->uCwMin = C_CWMIN_A;
+			max_min = 4;
+		} else {
+			priv->uCwMin = C_CWMIN_B;
+			max_min = 5;
+			}
+	}
+
+	priv->uCwMax = C_CWMAX;
+	priv->uEIFS = C_EIFS;
+
+	data[0] = (u8)priv->uSIFS;
+	data[1] = (u8)priv->uDIFS;
+	data[2] = (u8)priv->uEIFS;
+	data[3] = (u8)priv->uSlot;
+
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE, MAC_REG_SIFS,
+		MESSAGE_REQUEST_MACREG, 4, &data[0]);
+
+	max_min |= 0xa0;
+
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE, MAC_REG_CWMAXMIN0,
+		MESSAGE_REQUEST_MACREG, 1, &max_min);
 }
 
-void CARDvUpdateBasicTopRate(struct vnt_private *pDevice)
+void CARDvUpdateBasicTopRate(struct vnt_private *priv)
 {
-	u8 byTopOFDM = RATE_24M, byTopCCK = RATE_1M;
-	u8 ii;
-
-     //Determines the highest basic rate.
-     for (ii = RATE_54M; ii >= RATE_6M; ii --) {
-         if ( (pDevice->wBasicRate) & ((u16)(1<<ii)) ) {
-             byTopOFDM = ii;
-             break;
-         }
-     }
-     pDevice->byTopOFDMBasicRate = byTopOFDM;
-
-     for (ii = RATE_11M;; ii --) {
-         if ( (pDevice->wBasicRate) & ((u16)(1<<ii)) ) {
-             byTopCCK = ii;
-             break;
-         }
-         if (ii == RATE_1M)
-            break;
-     }
-     pDevice->byTopCCKBasicRate = byTopCCK;
+	u8 top_ofdm = RATE_24M, top_cck = RATE_1M;
+	u8 i;
+
+	/*Determines the highest basic rate.*/
+	for (i = RATE_54M; i >= RATE_6M; i--) {
+		if (priv->wBasicRate & (u16)(1 << i)) {
+			top_ofdm = i;
+			break;
+		}
+	}
+
+	priv->byTopOFDMBasicRate = top_ofdm;
+
+	for (i = RATE_11M;; i--) {
+		if (priv->wBasicRate & (u16)(1 << i)) {
+			top_cck = i;
+			break;
+		}
+		if (i == RATE_1M)
+			break;
+	}
+
+	priv->byTopCCKBasicRate = top_cck;
  }
 
 /*
@@ -555,39 +510,36 @@ void CARDvUpdateBasicTopRate(struct vnt_private *pDevice)
  * Return Value: true if succeeded; false if failed.
  *
  */
-void CARDbAddBasicRate(struct vnt_private *pDevice, u16 wRateIdx)
+void CARDbAddBasicRate(struct vnt_private *priv, u16 rate_idx)
 {
-	u16 wRate = (1 << wRateIdx);
 
-    pDevice->wBasicRate |= wRate;
+	priv->wBasicRate |= (1 << rate_idx);
 
-    //Determines the highest basic rate.
-    CARDvUpdateBasicTopRate(pDevice);
+	/*Determines the highest basic rate.*/
+	CARDvUpdateBasicTopRate(priv);
 }
 
-int CARDbIsOFDMinBasicRate(struct vnt_private *pDevice)
+int CARDbIsOFDMinBasicRate(struct vnt_private *priv)
 {
 	int ii;
 
-    for (ii = RATE_54M; ii >= RATE_6M; ii --) {
-        if ((pDevice->wBasicRate) & ((u16)(1<<ii)))
-            return true;
-    }
-    return false;
+	for (ii = RATE_54M; ii >= RATE_6M; ii--) {
+		if ((priv->wBasicRate) & ((u16)(1 << ii)))
+			return true;
+	}
+
+	return false;
 }
 
-u8 CARDbyGetPktType(struct vnt_private *pDevice)
+u8 CARDbyGetPktType(struct vnt_private *priv)
 {
 
-    if (pDevice->byBBType == BB_TYPE_11A || pDevice->byBBType == BB_TYPE_11B) {
-        return (u8)pDevice->byBBType;
-    }
-    else if (CARDbIsOFDMinBasicRate(pDevice)) {
-        return PK_TYPE_11GA;
-    }
-    else {
-        return PK_TYPE_11GB;
-    }
+	if (priv->byBBType == BB_TYPE_11A || priv->byBBType == BB_TYPE_11B)
+		return (u8)priv->byBBType;
+	else if (CARDbIsOFDMinBasicRate(priv))
+		return PK_TYPE_11GA;
+	else
+		return PK_TYPE_11GB;
 }
 
 /*
@@ -596,27 +548,27 @@ u8 CARDbyGetPktType(struct vnt_private *pDevice)
  *
  * Parameters:
  *  In:
- *      pDevice         - The adapter to be sync.
- *      qwTSF1          - Rx BCN's TSF
- *      qwTSF2          - Local TSF
+ *      rx_rate	- rx rate.
+ *      tsf1	- Rx BCN's TSF
+ *      tsf2	- Local TSF
  *  Out:
  *      none
  *
  * Return Value: TSF Offset value
  *
  */
-u64 CARDqGetTSFOffset(u8 byRxRate, u64 qwTSF1, u64 qwTSF2)
+u64 CARDqGetTSFOffset(u8 rx_rate, u64 tsf1, u64 tsf2)
 {
-	u64 qwTSFOffset = 0;
-	u16 wRxBcnTSFOffst = 0;
+	u64 tsf_offset = 0;
+	u16 rx_bcn_offset = 0;
 
-	wRxBcnTSFOffst = cwRXBCNTSFOff[byRxRate % MAX_RATE];
+	rx_bcn_offset = cwRXBCNTSFOff[rx_rate % MAX_RATE];
 
-	qwTSF2 += (u64)wRxBcnTSFOffst;
+	tsf2 += (u64)rx_bcn_offset;
 
-	qwTSFOffset = qwTSF1 - qwTSF2;
+	tsf_offset = tsf1 - tsf2;
 
-	return qwTSFOffset;
+	return tsf_offset;
 }
 
 /*
@@ -625,42 +577,34 @@ u64 CARDqGetTSFOffset(u8 byRxRate, u64 qwTSF1, u64 qwTSF2)
  *
  * Parameters:
  *  In:
- *      pDevice         - The adapter to be sync.
- *      qwBSSTimestamp  - Rx BCN's TSF
- *      qwLocalTSF      - Local TSF
+ *      priv		- The adapter to be sync.
+ *      time_stamp	- Rx BCN's TSF
+ *      local_tsf	- Local TSF
  *  Out:
  *      none
  *
  * Return Value: none
  *
  */
-void CARDvAdjustTSF(struct vnt_private *pDevice, u8 byRxRate,
-		u64 qwBSSTimestamp, u64 qwLocalTSF)
+void CARDvAdjustTSF(struct vnt_private *priv, u8 rx_rate,
+		u64 time_stamp, u64 local_tsf)
 {
-	u64 qwTSFOffset = 0;
-	u8 pbyData[8];
-
-    qwTSFOffset = CARDqGetTSFOffset(byRxRate, qwBSSTimestamp, qwLocalTSF);
-    // adjust TSF
-    // HW's TSF add TSF Offset reg
-
-	pbyData[0] = (u8)qwTSFOffset;
-	pbyData[1] = (u8)(qwTSFOffset >> 8);
-	pbyData[2] = (u8)(qwTSFOffset >> 16);
-	pbyData[3] = (u8)(qwTSFOffset >> 24);
-	pbyData[4] = (u8)(qwTSFOffset >> 32);
-	pbyData[5] = (u8)(qwTSFOffset >> 40);
-	pbyData[6] = (u8)(qwTSFOffset >> 48);
-	pbyData[7] = (u8)(qwTSFOffset >> 56);
-
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_SET_TSFTBTT,
-                        MESSAGE_REQUEST_TSF,
-                        0,
-                        8,
-                        pbyData
-                        );
-
+	u64 tsf_offset = 0;
+	u8 data[8];
+
+	tsf_offset = CARDqGetTSFOffset(rx_rate, time_stamp, local_tsf);
+
+	data[0] = (u8)tsf_offset;
+	data[1] = (u8)(tsf_offset >> 8);
+	data[2] = (u8)(tsf_offset >> 16);
+	data[3] = (u8)(tsf_offset >> 24);
+	data[4] = (u8)(tsf_offset >> 32);
+	data[5] = (u8)(tsf_offset >> 40);
+	data[6] = (u8)(tsf_offset >> 48);
+	data[7] = (u8)(tsf_offset >> 56);
+
+	vnt_control_out(priv, MESSAGE_TYPE_SET_TSFTBTT,
+		MESSAGE_REQUEST_TSF, 0, 8, data);
 }
 /*
  * Description: Read NIC TSF counter
@@ -668,17 +612,17 @@ void CARDvAdjustTSF(struct vnt_private *pDevice, u8 byRxRate,
  *
  * Parameters:
  *  In:
- *      pDevice         - The adapter to be read
+ *	priv		- The adapter to be read
  *  Out:
- *      qwCurrTSF       - Current TSF counter
+ *	current_tsf	- Current TSF counter
  *
  * Return Value: true if success; otherwise false
  *
  */
-bool CARDbGetCurrentTSF(struct vnt_private *pDevice, u64 *pqwCurrTSF)
+bool CARDbGetCurrentTSF(struct vnt_private *priv, u64 *current_tsf)
 {
 
-	*pqwCurrTSF = pDevice->qwCurrTSF;
+	*current_tsf = priv->qwCurrTSF;
 
 	return true;
 }
@@ -689,17 +633,17 @@ bool CARDbGetCurrentTSF(struct vnt_private *pDevice, u64 *pqwCurrTSF)
  *
  * Parameters:
  *  In:
- *      pDevice         - The adapter to be read
+ *      priv	- The adapter to be read
  *
  * Return Value: true if success; otherwise false
  *
  */
-bool CARDbClearCurrentTSF(struct vnt_private *pDevice)
+bool CARDbClearCurrentTSF(struct vnt_private *priv)
 {
 
-	MACvRegBitsOn(pDevice, MAC_REG_TFTCTL, TFTCTL_TSFCNTRST);
+	MACvRegBitsOn(priv, MAC_REG_TFTCTL, TFTCTL_TSFCNTRST);
 
-	pDevice->qwCurrTSF = 0;
+	priv->qwCurrTSF = 0;
 
 	return true;
 }
@@ -710,30 +654,30 @@ bool CARDbClearCurrentTSF(struct vnt_private *pDevice)
  *
  * Parameters:
  *  In:
- *      qwTSF           - Current TSF counter
- *      wbeaconInterval - Beacon Interval
+ *      tsf		- Current TSF counter
+ *      beacon_interval - Beacon Interval
  *  Out:
- *      qwCurrTSF       - Current TSF counter
+ *      tsf		- Current TSF counter
  *
  * Return Value: TSF value of next Beacon
  *
  */
-u64 CARDqGetNextTBTT(u64 qwTSF, u16 wBeaconInterval)
+u64 CARDqGetNextTBTT(u64 tsf, u16 beacon_interval)
 {
-	u32 uBeaconInterval;
+	u32 beacon_int;
 
-	uBeaconInterval = wBeaconInterval * 1024;
+	beacon_int = beacon_interval * 1024;
 
 	/* Next TBTT =
 	*	((local_current_TSF / beacon_interval) + 1) * beacon_interval
 	*/
-	if (uBeaconInterval) {
-		do_div(qwTSF, uBeaconInterval);
-		qwTSF += 1;
-		qwTSF *= uBeaconInterval;
+	if (beacon_int) {
+		do_div(tsf, beacon_int);
+		tsf += 1;
+		tsf *= beacon_int;
 	}
 
-	return qwTSF;
+	return tsf;
 }
 
 /*
@@ -743,41 +687,35 @@ u64 CARDqGetNextTBTT(u64 qwTSF, u16 wBeaconInterval)
  * Parameters:
  *  In:
  *      dwIoBase        - IO Base
- *      wBeaconInterval - Beacon Interval
+ *	beacon_interval - Beacon Interval
  *  Out:
  *      none
  *
  * Return Value: none
  *
  */
-void CARDvSetFirstNextTBTT(struct vnt_private *pDevice, u16 wBeaconInterval)
+void CARDvSetFirstNextTBTT(struct vnt_private *priv, u16 beacon_interval)
 {
-	u64 qwNextTBTT = 0;
-	u8 pbyData[8];
-
-	CARDbClearCurrentTSF(pDevice);
-    //CARDbGetCurrentTSF(pDevice, &qwNextTBTT); //Get Local TSF counter
-	qwNextTBTT = CARDqGetNextTBTT(qwNextTBTT, wBeaconInterval);
-    // Set NextTBTT
-
-	pbyData[0] = (u8)qwNextTBTT;
-	pbyData[1] = (u8)(qwNextTBTT >> 8);
-	pbyData[2] = (u8)(qwNextTBTT >> 16);
-	pbyData[3] = (u8)(qwNextTBTT >> 24);
-	pbyData[4] = (u8)(qwNextTBTT >> 32);
-	pbyData[5] = (u8)(qwNextTBTT >> 40);
-	pbyData[6] = (u8)(qwNextTBTT >> 48);
-	pbyData[7] = (u8)(qwNextTBTT >> 56);
-
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_SET_TSFTBTT,
-                        MESSAGE_REQUEST_TBTT,
-                        0,
-                        8,
-                        pbyData
-                        );
-
-    return;
+	u64 next_tbtt = 0;
+	u8 data[8];
+
+	CARDbClearCurrentTSF(priv);
+
+	next_tbtt = CARDqGetNextTBTT(next_tbtt, beacon_interval);
+
+	data[0] = (u8)next_tbtt;
+	data[1] = (u8)(next_tbtt >> 8);
+	data[2] = (u8)(next_tbtt >> 16);
+	data[3] = (u8)(next_tbtt >> 24);
+	data[4] = (u8)(next_tbtt >> 32);
+	data[5] = (u8)(next_tbtt >> 40);
+	data[6] = (u8)(next_tbtt >> 48);
+	data[7] = (u8)(next_tbtt >> 56);
+
+	vnt_control_out(priv, MESSAGE_TYPE_SET_TSFTBTT,
+		MESSAGE_REQUEST_TBTT, 0, 8, data);
+
+	return;
 }
 
 /*
@@ -786,45 +724,37 @@ void CARDvSetFirstNextTBTT(struct vnt_private *pDevice, u16 wBeaconInterval)
  *
  * Parameters:
  *  In:
- *      pDevice         - The adapter to be set
- *      qwTSF           - Current TSF counter
- *      wBeaconInterval - Beacon Interval
+ *	priv		- The adapter to be set
+ *      tsf		- Current TSF counter
+ *      beacon_interval - Beacon Interval
  *  Out:
  *      none
  *
  * Return Value: none
  *
  */
-void CARDvUpdateNextTBTT(struct vnt_private *pDevice, u64 qwTSF,
-			u16 wBeaconInterval)
+void CARDvUpdateNextTBTT(struct vnt_private *priv, u64 tsf,
+			u16 beacon_interval)
 {
-	u8 pbyData[8];
-
-    qwTSF = CARDqGetNextTBTT(qwTSF, wBeaconInterval);
+	u8 data[8];
 
-    // Set NextTBTT
+	tsf = CARDqGetNextTBTT(tsf, beacon_interval);
 
-	pbyData[0] = (u8)qwTSF;
-	pbyData[1] = (u8)(qwTSF >> 8);
-	pbyData[2] = (u8)(qwTSF >> 16);
-	pbyData[3] = (u8)(qwTSF >> 24);
-	pbyData[4] = (u8)(qwTSF >> 32);
-	pbyData[5] = (u8)(qwTSF >> 40);
-	pbyData[6] = (u8)(qwTSF >> 48);
-	pbyData[7] = (u8)(qwTSF >> 56);
+	data[0] = (u8)tsf;
+	data[1] = (u8)(tsf >> 8);
+	data[2] = (u8)(tsf >> 16);
+	data[3] = (u8)(tsf >> 24);
+	data[4] = (u8)(tsf >> 32);
+	data[5] = (u8)(tsf >> 40);
+	data[6] = (u8)(tsf >> 48);
+	data[7] = (u8)(tsf >> 56);
 
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_SET_TSFTBTT,
-                        MESSAGE_REQUEST_TBTT,
-                        0,
-                        8,
-                        pbyData
-                        );
+	vnt_control_out(priv, MESSAGE_TYPE_SET_TSFTBTT,
+		MESSAGE_REQUEST_TBTT, 0, 8, data);
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-		"Card:Update Next TBTT[%8lx]\n", (unsigned long)qwTSF);
+	dev_dbg(&priv->usb->dev, "%s TBTT: %8llx\n", __func__, tsf);
 
-    return;
+	return;
 }
 
 /*
@@ -832,38 +762,36 @@ void CARDvUpdateNextTBTT(struct vnt_private *pDevice, u64 qwTSF,
  *
  * Parameters:
  *  In:
- *      pDevice         - The adapter to be turned off
+ *      priv         - The adapter to be turned off
  *  Out:
  *      none
  *
  * Return Value: true if success; otherwise false
  *
  */
-int CARDbRadioPowerOff(struct vnt_private *pDevice)
+int CARDbRadioPowerOff(struct vnt_private *priv)
 {
-	int bResult = true;
-
-    //if (pDevice->bRadioOff == true)
-    //    return true;
-
-    pDevice->bRadioOff = true;
-
-    switch (pDevice->byRFType) {
-        case RF_AL2230:
-        case RF_AL2230S:
-        case RF_AIROHA7230:
-        case RF_VT3226:     //RobertYu:20051111
-        case RF_VT3226D0:
-        case RF_VT3342A0:   //RobertYu:20060609
-            MACvRegBitsOff(pDevice, MAC_REG_SOFTPWRCTL, (SOFTPWRCTL_SWPE2 | SOFTPWRCTL_SWPE3));
-            break;
-    }
+	int ret = true;
+
+	priv->bRadioOff = true;
+
+	switch (priv->byRFType) {
+	case RF_AL2230:
+	case RF_AL2230S:
+	case RF_AIROHA7230:
+	case RF_VT3226:
+	case RF_VT3226D0:
+	case RF_VT3342A0:
+		MACvRegBitsOff(priv, MAC_REG_SOFTPWRCTL,
+				(SOFTPWRCTL_SWPE2 | SOFTPWRCTL_SWPE3));
+		break;
+	}
 
-    MACvRegBitsOff(pDevice, MAC_REG_HOSTCR, HOSTCR_RXON);
+	MACvRegBitsOff(priv, MAC_REG_HOSTCR, HOSTCR_RXON);
 
-    BBvSetDeepSleep(pDevice);
+	BBvSetDeepSleep(priv);
 
-    return bResult;
+	return ret;
 }
 
 /*
@@ -871,84 +799,79 @@ int CARDbRadioPowerOff(struct vnt_private *pDevice)
  *
  * Parameters:
  *  In:
- *      pDevice         - The adapter to be turned on
+ *      priv         - The adapter to be turned on
  *  Out:
  *      none
  *
  * Return Value: true if success; otherwise false
  *
  */
-int CARDbRadioPowerOn(struct vnt_private *pDevice)
+int CARDbRadioPowerOn(struct vnt_private *priv)
 {
-	int bResult = true;
+	int ret = true;
 
-    if ((pDevice->bHWRadioOff == true) || (pDevice->bRadioControlOff == true)) {
-        return false;
-    }
+	if (priv->bHWRadioOff == true || priv->bRadioControlOff == true)
+		return false;
 
-    //if (pDevice->bRadioOff == false)
-    //    return true;
+	priv->bRadioOff = false;
 
-    pDevice->bRadioOff = false;
+	BBvExitDeepSleep(priv);
 
-    BBvExitDeepSleep(pDevice);
+	MACvRegBitsOn(priv, MAC_REG_HOSTCR, HOSTCR_RXON);
 
-    MACvRegBitsOn(pDevice, MAC_REG_HOSTCR, HOSTCR_RXON);
-
-    switch (pDevice->byRFType) {
-        case RF_AL2230:
-        case RF_AL2230S:
-        case RF_AIROHA7230:
-        case RF_VT3226:     //RobertYu:20051111
-        case RF_VT3226D0:
-        case RF_VT3342A0:   //RobertYu:20060609
-            MACvRegBitsOn(pDevice, MAC_REG_SOFTPWRCTL, (SOFTPWRCTL_SWPE2 | SOFTPWRCTL_SWPE3));
-            break;
-    }
+	switch (priv->byRFType) {
+	case RF_AL2230:
+	case RF_AL2230S:
+	case RF_AIROHA7230:
+	case RF_VT3226:
+	case RF_VT3226D0:
+	case RF_VT3342A0:
+		MACvRegBitsOn(priv, MAC_REG_SOFTPWRCTL,
+			(SOFTPWRCTL_SWPE2 | SOFTPWRCTL_SWPE3));
+		break;
+	}
 
-    return bResult;
+	return ret;
 }
 
-void CARDvSetBSSMode(struct vnt_private *pDevice)
+void CARDvSetBSSMode(struct vnt_private *priv)
 {
-    // Set BB and packet type at the same time.//{{RobertYu:20050222, AL7230 have two TX PA output, only connet to b/g now
-    // so in 11a mode need to set the MAC Reg0x4C to 11b/g mode to turn on PA
-    if( (pDevice->byRFType == RF_AIROHA7230 ) && (pDevice->byBBType == BB_TYPE_11A) )
-    {
-        MACvSetBBType(pDevice, BB_TYPE_11G);
-    }
-    else
-    {
-        MACvSetBBType(pDevice, pDevice->byBBType);
-    }
-    pDevice->byPacketType = CARDbyGetPktType(pDevice);
-
-    if (pDevice->byBBType == BB_TYPE_11A) {
-        ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x88, 0x03);
-    } else if (pDevice->byBBType == BB_TYPE_11B) {
-        ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x88, 0x02);
-    } else if (pDevice->byBBType == BB_TYPE_11G) {
-        ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0x88, 0x08);
-    }
-
-    vUpdateIFS(pDevice);
-    CARDvSetRSPINF(pDevice, (u8)pDevice->byBBType);
-
-    if ( pDevice->byBBType == BB_TYPE_11A ) {
-        //request by Jack 2005-04-26
-        if (pDevice->byRFType == RF_AIROHA7230) {
-            pDevice->abyBBVGA[0] = 0x20;
-            ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xE7, pDevice->abyBBVGA[0]);
-        }
-        pDevice->abyBBVGA[2] = 0x10;
-        pDevice->abyBBVGA[3] = 0x10;
-    } else {
-        //request by Jack 2005-04-26
-        if (pDevice->byRFType == RF_AIROHA7230) {
-            pDevice->abyBBVGA[0] = 0x1C;
-            ControlvWriteByte(pDevice, MESSAGE_REQUEST_BBREG, 0xE7, pDevice->abyBBVGA[0]);
-        }
-        pDevice->abyBBVGA[2] = 0x0;
-        pDevice->abyBBVGA[3] = 0x0;
-    }
+	if (priv->byRFType == RF_AIROHA7230 && priv->byBBType == BB_TYPE_11A)
+		MACvSetBBType(priv, BB_TYPE_11G);
+	else
+		MACvSetBBType(priv, priv->byBBType);
+
+	priv->byPacketType = CARDbyGetPktType(priv);
+
+	if (priv->byBBType == BB_TYPE_11A)
+		vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x88, 0x03);
+	else if (priv->byBBType == BB_TYPE_11B)
+		vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x88, 0x02);
+	else if (priv->byBBType == BB_TYPE_11G)
+		vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x88, 0x08);
+
+	vUpdateIFS(priv);
+	CARDvSetRSPINF(priv, (u8)priv->byBBType);
+
+	if (priv->byBBType == BB_TYPE_11A) {
+		if (priv->byRFType == RF_AIROHA7230) {
+			priv->abyBBVGA[0] = 0x20;
+
+			vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG,
+						0xe7, priv->abyBBVGA[0]);
+		}
+
+		priv->abyBBVGA[2] = 0x10;
+		priv->abyBBVGA[3] = 0x10;
+	} else {
+		if (priv->byRFType == RF_AIROHA7230) {
+			priv->abyBBVGA[0] = 0x1c;
+
+			vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG,
+						0xe7, priv->abyBBVGA[0]);
+		}
+
+		priv->abyBBVGA[2] = 0x0;
+		priv->abyBBVGA[3] = 0x0;
+	}
 }
diff --git a/drivers/staging/vt6656/card.h b/drivers/staging/vt6656/card.h
index f843e50875f..ac734714c7d 100644
--- a/drivers/staging/vt6656/card.h
+++ b/drivers/staging/vt6656/card.h
@@ -46,10 +46,10 @@ typedef enum _CARD_PHY_TYPE {
 struct vnt_private;
 
 void CARDbSetMediaChannel(struct vnt_private *pDevice, u32 uConnectionChannel);
-void CARDvSetRSPINF(struct vnt_private *pDevice, u8 byBBType);
-void vUpdateIFS(struct vnt_private *pDevice);
-void CARDvUpdateBasicTopRate(struct vnt_private *pDevice);
-void CARDbAddBasicRate(struct vnt_private *pDevice, u16 wRateIdx);
+void CARDvSetRSPINF(struct vnt_private *, u8);
+void vUpdateIFS(struct vnt_private *);
+void CARDvUpdateBasicTopRate(struct vnt_private *);
+void CARDbAddBasicRate(struct vnt_private *, u16);
 int CARDbIsOFDMinBasicRate(struct vnt_private *pDevice);
 void CARDvAdjustTSF(struct vnt_private *pDevice, u8 byRxRate,
 		u64 qwBSSTimestamp, u64 qwLocalTSF);
diff --git a/drivers/staging/vt6656/control.c b/drivers/staging/vt6656/control.c
deleted file mode 100644
index 026784fcde8..00000000000
--- a/drivers/staging/vt6656/control.c
+++ /dev/null
@@ -1,79 +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: control.c
- *
- * Purpose: Handle USB control endpoint
- *
- * Author: Jerry Chen
- *
- * Date: Apr. 5, 2004
- *
- * Functions:
- *      CONTROLnsRequestOut - Write variable length bytes to MEM/BB/MAC/EEPROM
- *      CONTROLnsRequestIn - Read variable length bytes from MEM/BB/MAC/EEPROM
- *      ControlvWriteByte - Write one byte to MEM/BB/MAC/EEPROM
- *      ControlvReadByte - Read one byte from MEM/BB/MAC/EEPROM
- *      ControlvMaskByte - Read one byte from MEM/BB/MAC/EEPROM and clear/set
- *				some bits in the same address
- *
- * Revision History:
- *      04-05-2004 Jerry Chen:  Initial release
- *      11-24-2004 Warren Hsu: Add ControlvWriteByte, ControlvReadByte,
- *					ControlvMaskByte
- *
- */
-
-#include "control.h"
-#include "rndis.h"
-
-/* static int          msglevel                =MSG_LEVEL_INFO;  */
-/* static int          msglevel                =MSG_LEVEL_DEBUG; */
-
-void ControlvWriteByte(struct vnt_private *pDevice, u8 reg, u8 reg_off,
-			u8 data)
-{
-
-	CONTROLnsRequestOut(pDevice, MESSAGE_TYPE_WRITE, reg_off, reg,
-		sizeof(u8), &data);
-
-	return;
-}
-
-void ControlvReadByte(struct vnt_private *pDevice, u8 reg, u8 reg_off,
-			u8 *data)
-{
-	CONTROLnsRequestIn(pDevice, MESSAGE_TYPE_READ,
-			reg_off, reg, sizeof(u8), data);
-	return;
-}
-
-void ControlvMaskByte(struct vnt_private *pDevice, u8 reg_type, u8 reg_off,
-			u8 reg_mask, u8 data)
-{
-	u8 reg_data[2];
-
-	reg_data[0] = data;
-	reg_data[1] = reg_mask;
-
-	CONTROLnsRequestOut(pDevice, MESSAGE_TYPE_WRITE_MASK, reg_off,
-			reg_type, ARRAY_SIZE(reg_data), reg_data);
-
-	return;
-}
diff --git a/drivers/staging/vt6656/control.h b/drivers/staging/vt6656/control.h
deleted file mode 100644
index 9da9b96add7..00000000000
--- a/drivers/staging/vt6656/control.h
+++ /dev/null
@@ -1,54 +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: control.h
- *
- * Purpose:
- *
- * Author: Jerry Chen
- *
- * Date: Apr. 5, 2004
- *
- */
-
-#ifndef __CONTROL_H__
-#define __CONTROL_H__
-
-#include "device.h"
-#include "usbpipe.h"
-
-#define CONTROLnsRequestOut(Device, Request, Value, Index, Length, Buffer) \
-	PIPEnsControlOut(Device, Request, Value, Index, Length, Buffer)
-
-#define CONTROLnsRequestOutAsyn(Device, Request, Value, Index, Length, Buffer) \
-	PIPEnsControlOutAsyn(Device, Request, Value, Index, Length, Buffer)
-
-#define CONTROLnsRequestIn(Device, Request, Value, Index, Length, Buffer) \
-	PIPEnsControlIn(Device, Request, Value, Index, Length, Buffer)
-
-void ControlvWriteByte(struct vnt_private *pDevice, u8 reg, u8 reg_off,
-			u8 data);
-
-void ControlvReadByte(struct vnt_private *pDevice, u8 reg, u8 reg_off,
-			u8 *data);
-
-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/datarate.c b/drivers/staging/vt6656/datarate.c
index 547db6f0c53..8032d6b5b38 100644
--- a/drivers/staging/vt6656/datarate.c
+++ b/drivers/staging/vt6656/datarate.c
@@ -40,7 +40,6 @@
 #include "datarate.h"
 #include "card.h"
 #include "baseband.h"
-#include "srom.h"
 #include "rf.h"
 
 /* static int msglevel = MSG_LEVEL_DEBUG; */
diff --git a/drivers/staging/vt6656/desc.h b/drivers/staging/vt6656/desc.h
index 7c6dd5f5229..617d479b85c 100644
--- a/drivers/staging/vt6656/desc.h
+++ b/drivers/staging/vt6656/desc.h
@@ -39,28 +39,15 @@
 /* max transmit or receive buffer size */
 #define CB_MAX_BUF_SIZE     2900U       /* NOTE: must be multiple of 4 */
 
-/* max TX buffer size */
-#define CB_MAX_TX_BUF_SIZE        CB_MAX_BUF_SIZE
-/* max RX buffer size when not use Multi-RD */
-#define CB_MAX_RX_BUF_SIZE_NORMAL CB_MAX_BUF_SIZE
-
-#define CB_BEACON_BUF_SIZE  512U        /* default beacon buffer size */
-
 #define MAX_TOTAL_SIZE_WITH_ALL_HEADERS CB_MAX_BUF_SIZE
 
 #define MAX_INTERRUPT_SIZE              32
 
-#define RX_BLOCKS           64          /* from 0x60 to 0xA0 */
-#define TX_BLOCKS           32          /* from 0xA0 to 0xC0 */
-
 #define CB_MAX_RX_DESC      128         /* max # of descriptors */
 #define CB_MIN_RX_DESC      16          /* min # of RX descriptors */
 #define CB_MAX_TX_DESC      128         /* max # of descriptors */
 #define CB_MIN_TX_DESC      16          /* min # of TX descriptors */
 
-#define CB_RD_NUM           64          /* default # of RD */
-#define CB_TD_NUM           64          /* default # of TD */
-
 /*
  * bits in the RSR register
  */
@@ -91,15 +78,6 @@
 #define TSR_ACKDATA         0x02
 #define TSR_VALID           0x01
 
-#define CB_PROTOCOL_RESERVED_SECTION    16
-
-/*
- * if retries exceed 15 times, TX will abort, and
- * if TX fifo underflow, TX will fail
- * we should try to resend it
- */
-#define CB_MAX_TX_ABORT_RETRY   3
-
 #define FIFOCTL_AUTO_FB_1   0x1000
 #define FIFOCTL_AUTO_FB_0   0x0800
 #define FIFOCTL_GRPACK      0x0400
@@ -126,50 +104,4 @@
 #define FRAGCTL_STAFRAG     0x0001
 #define FRAGCTL_NONFRAG     0x0000
 
-#define TYPE_TXDMA0     0
-#define TYPE_AC0DMA     1
-#define TYPE_ATIMDMA    2
-#define TYPE_SYNCDMA    3
-#define TYPE_MAXTD      2
-
-#define TYPE_BEACONDMA  4
-
-#define TYPE_RXDMA0     0
-#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) */
-#define TD_FLAGS_PS_RETRY  0x04 /* check if PS STA frame re-transmit */
-
-/*
- * TX FIFO header
- */
-typedef struct tagSBEACONCtl {
-	u32 BufReady:1;
-	u32 TSF:15;
-	u32 BufLen:11;
-	u32 Reserved:5;
-} __attribute__ ((__packed__))
-SBEACONCtl;
-
-typedef struct tagSSecretKey {
-	u32 dwLowDword;
-    u8    byHighByte;
-} __attribute__ ((__packed__))
-SSecretKey;
-
-typedef struct tagSKeyEntry {
-    u8  abyAddrHi[2];
-    u16  wKCTL;
-    u8  abyAddrLo[4];
-	u32 dwKey0[4];
-	u32 dwKey1[4];
-	u32 dwKey2[4];
-	u32 dwKey3[4];
-	u32 dwKey4[4];
-} __attribute__ ((__packed__))
-SKeyEntry;
-
 #endif /* __DESC_H__ */
diff --git a/drivers/staging/vt6656/device.h b/drivers/staging/vt6656/device.h
index e2abe3ddd24..5b64ca7b62f 100644
--- a/drivers/staging/vt6656/device.h
+++ b/drivers/staging/vt6656/device.h
@@ -52,53 +52,35 @@
 #undef DEVICE_ETHTOOL_IOCTL_SUPPORT
 #endif
 
-/* please copy below macro to driver_event.c for API */
-#define RT_INSMOD_EVENT_FLAG                             0x0101
-#define RT_UPDEV_EVENT_FLAG                               0x0102
-#define RT_DISCONNECTED_EVENT_FLAG               0x0103
-#define RT_WPACONNECTED_EVENT_FLAG             0x0104
-#define RT_DOWNDEV_EVENT_FLAG                        0x0105
-#define RT_RMMOD_EVENT_FLAG                              0x0106
+#define MAX_RATE			12
 
 /*
  * device specific
  */
 
-#include "device_cfg.h"
 #include "80211hdr.h"
 #include "tether.h"
 #include "wmgr.h"
 #include "wcmd.h"
-#include "srom.h"
 #include "rc4.h"
 #include "desc.h"
 #include "key.h"
 #include "card.h"
-#include "rndis.h"
 
 #define VNT_USB_VENDOR_ID                     0x160a
 #define VNT_USB_PRODUCT_ID                    0x3184
 
-#define MAC_MAX_CONTEXT_REG     (256+128)
+#define DEVICE_NAME			"vt6656"
+#define DEVICE_FULL_DRV_NAM		"VIA Networking Wireless LAN USB Driver"
 
-#define MAX_MULTICAST_ADDRESS_NUM       32
-#define MULTICAST_ADDRESS_LIST_SIZE     (MAX_MULTICAST_ADDRESS_NUM * ETH_ALEN)
+#define DEVICE_VERSION			"1.19_12"
 
-#define DUPLICATE_RX_CACHE_LENGTH       5
-
-#define NUM_KEY_ENTRY                   11
+#define CONFIG_PATH			"/etc/vntconfiguration.dat"
 
-#define TX_WEP_NONE                     0
-#define TX_WEP_OTF                      1
-#define TX_WEP_SW                       2
-#define TX_WEP_SWOTP                    3
-#define TX_WEP_OTPSW                    4
-#define TX_WEP_SW232                    5
+#define MAX_UINTS			8
+#define OPTION_DEFAULT			{ [0 ... MAX_UINTS-1] = -1}
 
-#define KEYSEL_WEP40                    0
-#define KEYSEL_WEP104                   1
-#define KEYSEL_TKIP                     2
-#define KEYSEL_CCMP                     3
+#define DUPLICATE_RX_CACHE_LENGTH       5
 
 #define AUTO_FB_NONE            0
 #define AUTO_FB_0               1
@@ -119,23 +101,86 @@
 #define ANT_RXA                 2
 #define ANT_RXB                 3
 
-#define MAXCHECKHANGCNT         4
-
-/* Packet type */
-#define TX_PKT_UNI              0x00
-#define TX_PKT_MULTI            0x01
-#define TX_PKT_BROAD            0x02
-
 #define BB_VGA_LEVEL            4
 #define BB_VGA_CHANGE_THRESHOLD 3
 
+#define EEP_MAX_CONTEXT_SIZE    256
+
+/* Contents in the EEPROM */
+#define EEP_OFS_PAR		0x0
+#define EEP_OFS_ANTENNA		0x17
+#define EEP_OFS_RADIOCTL	0x18
+#define EEP_OFS_RFTYPE		0x1b
+#define EEP_OFS_MINCHANNEL	0x1c
+#define EEP_OFS_MAXCHANNEL	0x1d
+#define EEP_OFS_SIGNATURE	0x1e
+#define EEP_OFS_ZONETYPE	0x1f
+#define EEP_OFS_RFTABLE		0x20
+#define EEP_OFS_PWR_CCK		0x20
+#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
+
+#define EEP_OFS_MAJOR_VER	0x2e
+#define EEP_OFS_MINOR_VER	0x2f
+
+#define EEP_OFS_CCK_PWR_TBL	0x30
+#define EEP_OFS_OFDM_PWR_TBL	0x40
+#define EEP_OFS_OFDMA_PWR_TBL	0x50
+
+/* Bits in EEP_OFS_ANTENNA */
+#define EEP_ANTENNA_MAIN	0x1
+#define EEP_ANTENNA_AUX		0x2
+#define EEP_ANTINV		0x4
+
+/* Bits in EEP_OFS_RADIOCTL */
+#define EEP_RADIOCTL_ENABLE	0x80
+
+/* control commands */
+#define MESSAGE_TYPE_READ		0x1
+#define MESSAGE_TYPE_WRITE		0x0
+#define MESSAGE_TYPE_LOCK_OR		0x2
+#define MESSAGE_TYPE_LOCK_AND		0x3
+#define MESSAGE_TYPE_WRITE_MASK		0x4
+#define MESSAGE_TYPE_CARDINIT		0x5
+#define MESSAGE_TYPE_INIT_RSP		0x6
+#define MESSAGE_TYPE_MACSHUTDOWN	0x7
+#define MESSAGE_TYPE_SETKEY		0x8
+#define MESSAGE_TYPE_CLRKEYENTRY	0x9
+#define MESSAGE_TYPE_WRITE_MISCFF	0xa
+#define MESSAGE_TYPE_SET_ANTMD		0xb
+#define MESSAGE_TYPE_SELECT_CHANNLE	0xc
+#define MESSAGE_TYPE_SET_TSFTBTT	0xd
+#define MESSAGE_TYPE_SET_SSTIFS		0xe
+#define MESSAGE_TYPE_CHANGE_BBTYPE	0xf
+#define MESSAGE_TYPE_DISABLE_PS		0x10
+#define MESSAGE_TYPE_WRITE_IFRF		0x11
+
+/* command read/write(index) */
+#define MESSAGE_REQUEST_MEM		0x1
+#define MESSAGE_REQUEST_BBREG		0x2
+#define MESSAGE_REQUEST_MACREG		0x3
+#define MESSAGE_REQUEST_EEPROM		0x4
+#define MESSAGE_REQUEST_TSF		0x5
+#define MESSAGE_REQUEST_TBTT		0x6
+#define MESSAGE_REQUEST_BBAGC		0x7
+#define MESSAGE_REQUEST_VERSION		0x8
+#define MESSAGE_REQUEST_RF_INIT		0x9
+#define MESSAGE_REQUEST_RF_INIT2	0xa
+#define MESSAGE_REQUEST_RF_CH0		0xb
+#define MESSAGE_REQUEST_RF_CH1		0xc
+#define MESSAGE_REQUEST_RF_CH2		0xd
+
+/* USB registers */
+#define USB_REG4			0x604
+
 #ifndef RUN_AT
 #define RUN_AT(x)                       (jiffies+(x))
 #endif
 
-/* DMA related */
-#define RESERV_AC0DMA                   4
-
 #define PRIVATE_Message                 0
 
 #define DBG_PRT(l, p, args...) { if (l <= msglevel) printk(p, ##args); }
@@ -153,6 +198,23 @@ typedef enum __device_msg_level {
 #define DEVICE_INIT_RESET	0x1 /* reset init or Dx to D0 power remain */
 #define DEVICE_INIT_DXPL	0x2 /* Dx to D0 power lost init */
 
+/* Device init */
+struct vnt_cmd_card_init {
+	u8 init_class;
+	u8 exist_sw_net_addr;
+	u8 sw_net_addr[6];
+	u8 short_retry_limit;
+	u8 long_retry_limit;
+};
+
+struct vnt_rsp_card_init {
+	u8 status;
+	u8 net_addr[6];
+	u8 rf_type;
+	u8 min_channel;
+	u8 max_channel;
+};
+
 /* USB */
 
 /*
@@ -176,13 +238,13 @@ struct vnt_rcb {
 
 /* used to track bulk out irps */
 struct vnt_usb_send_context {
-	void *pDevice;
-	struct sk_buff *pPacket;
-	struct urb *pUrb;
-	unsigned int uBufLen;
+	void *priv;
+	struct sk_buff *skb;
+	struct urb *urb;
+	unsigned int buf_len;
 	u8 type;
-	bool bBoolInUse;
-	unsigned char Data[MAX_TOTAL_SIZE_WITH_ALL_HEADERS];
+	bool in_use;
+	unsigned char data[MAX_TOTAL_SIZE_WITH_ALL_HEADERS];
 };
 
 /* tx packet info for rxtx */
@@ -278,9 +340,9 @@ typedef struct tagSPMKIDCandidateEvent {
 
 /* The receive duplicate detection cache entry */
 typedef struct tagSCacheEntry{
-    u16        wFmSequence;
-    u8        abyAddr2[ETH_ALEN];
-    u16        wFrameCtl;
+	__le16 wFmSequence;
+	u8 abyAddr2[ETH_ALEN];
+	__le16 wFrameCtl;
 } SCacheEntry, *PSCacheEntry;
 
 typedef struct tagSCache{
@@ -309,29 +371,9 @@ typedef struct tagSDeFragControlBlock
 
 /* flags for options */
 #define     DEVICE_FLAGS_UNPLUG          0x00000001UL
-#define     DEVICE_FLAGS_PREAMBLE_TYPE   0x00000002UL
-#define     DEVICE_FLAGS_OP_MODE         0x00000004UL
-#define     DEVICE_FLAGS_PS_MODE         0x00000008UL
-#define		DEVICE_FLAGS_80211h_MODE	 0x00000010UL
 
 /* flags for driver status */
 #define     DEVICE_FLAGS_OPENED          0x00010000UL
-#define     DEVICE_FLAGS_WOL_ENABLED     0x00080000UL
-/* flags for capabilities */
-#define     DEVICE_FLAGS_TX_ALIGN        0x01000000UL
-#define     DEVICE_FLAGS_HAVE_CAM        0x02000000UL
-#define     DEVICE_FLAGS_FLOW_CTRL       0x04000000UL
-
-/* flags for MII status */
-#define     DEVICE_LINK_FAIL             0x00000001UL
-#define     DEVICE_SPEED_10              0x00000002UL
-#define     DEVICE_SPEED_100             0x00000004UL
-#define     DEVICE_SPEED_1000            0x00000008UL
-#define     DEVICE_DUPLEX_FULL           0x00000010UL
-#define     DEVICE_AUTONEG_ENABLE        0x00000020UL
-#define     DEVICE_FORCED_BY_EEPROM      0x00000040UL
-/* for device_set_media_duplex */
-#define     DEVICE_LINK_CHANGE           0x00000001UL
 
 typedef struct __device_opt {
 	int nRxDescs0;  /* number of RX descriptors 0 */
@@ -363,11 +405,10 @@ struct vnt_private {
 	u8 byRxMode;
 
 	spinlock_t lock;
+	struct mutex usb_lock;
 
 	u32 rx_bytes;
 
-	u8 byRevId;
-
 	u32 flags;
 	unsigned long Flags;
 
@@ -379,9 +420,7 @@ struct vnt_private {
 	u32 uCurrentDFCBIdx;
 
 	/* USB */
-	struct urb *pControlURB;
 	struct urb *pInterruptURB;
-	struct usb_ctrlrequest sUsbCtlRequest;
 	u32 int_interval;
 
 	/* Variables to track resources for the BULK In Pipe */
@@ -453,30 +492,6 @@ struct vnt_private {
 	u8 byRadioCtl;
 	u8 bHWRadioOff;
 
-	/* SQ3 functions for antenna diversity */
-	struct timer_list TimerSQ3Tmax1;
-	struct timer_list TimerSQ3Tmax2;
-	struct timer_list TimerSQ3Tmax3;
-
-	int bDiversityRegCtlON;
-	int bDiversityEnable;
-	unsigned long ulDiversityNValue;
-	unsigned long ulDiversityMValue;
-	u8 byTMax;
-	u8 byTMax2;
-	u8 byTMax3;
-	unsigned long ulSQ3TH;
-
-	unsigned long uDiversityCnt;
-	u8 byAntennaState;
-	unsigned long ulRatio_State0;
-	unsigned long ulRatio_State1;
-	unsigned long ulSQ3_State0;
-	unsigned long ulSQ3_State1;
-
-	unsigned long aulSQ3Val[MAX_RATE];
-	unsigned long aulPktNum[MAX_RATE];
-
 	/* IFS & Cw */
 	u32 uSIFS;  /* Current SIFS */
 	u32 uDIFS;  /* Current DIFS */
@@ -496,14 +511,9 @@ struct vnt_private {
 	u8 byBBType; /* 0: 11A, 1:11B, 2:11G */
 	u8 byPacketType; /* 0:11a 1:11b 2:11gb 3:11ga */
 	u16 wBasicRate;
-	u8 byACKRate;
 	u8 byTopOFDMBasicRate;
 	u8 byTopCCKBasicRate;
 
-	u32 dwAotoRateTxOkCnt;
-	u32 dwAotoRateTxFailCnt;
-	u32 dwErrorRateThreshold[13];
-	u32 dwTPTable[MAX_RATE];
 	u8 abyEEPROM[EEP_MAX_CONTEXT_SIZE];  /*u32 alignment */
 
 	u8 byMinChannel;
@@ -542,7 +552,6 @@ struct vnt_private {
 
 	u32 dwMaxReceiveLifetime;  /* dot11MaxReceiveLifetime */
 
-	int bCCK;
 	int bEncryptionEnable;
 	int bShortSlotTime;
 	int bProtectMode;
@@ -569,7 +578,6 @@ struct vnt_private {
 	int bBeaconSent;
 	int bFixRate;
 	u8 byCurrentCh;
-	u32 uScanTime;
 
 	CMD_STATE eCommandState;
 
@@ -600,7 +608,6 @@ struct vnt_private {
 	u8 bSameBSSCurNum;
 	int bRoaming;
 	int b11hEable;
-	unsigned long ulTxPower;
 
 	/* Encryption */
 	NDIS_802_11_WEP_STATUS eEncryptionStatus;
@@ -629,7 +636,6 @@ struct vnt_private {
 	int bRxMICFail;
 
 	/* For Update BaseBand VGA Gain Offset */
-	int bUpdateBBVGA;
 	u32 uBBVGADiffCount;
 	u8 byBBVGANew;
 	u8 byBBVGACurrent;
@@ -641,18 +647,6 @@ struct vnt_private {
 
 	int bRadioCmd;
 
-	/* For FOE Tuning */
-	u8  byFOETuning;
-
-	/* For Auto Power Tunning */
-	u8  byAutoPwrTunning;
-
-	/* BaseBand Loopback Use */
-	u8 byBBCR4d;
-	u8 byBBCRc9;
-	u8 byBBCR88;
-	u8 byBBCR09;
-
 	/* command timer */
 	struct delayed_work run_command_work;
 	/* One second callback */
@@ -689,13 +683,6 @@ struct vnt_private {
 	int bwextstep3;
 	int bWPASuppWextEnabled;
 
-	/* user space daemon: hostapd, is used for HOSTAP */
-	int bEnableHostapd;
-	int bEnable8021x;
-	int bEnableHostWEP;
-	struct net_device *apdev;
-	int (*tx_80211)(struct sk_buff *skb, struct net_device *dev);
-
 	u32 uChannel;
 
 	struct iw_statistics wstats; /* wireless stats */
@@ -732,25 +719,15 @@ struct vnt_private {
         (uVar)++;                                   \
 }
 
-#define fMP_RESET_IN_PROGRESS               0x00000001
 #define fMP_DISCONNECTED                    0x00000002
-#define fMP_HALT_IN_PROGRESS                0x00000004
-#define fMP_SURPRISE_REMOVED                0x00000008
-#define fMP_RECV_LOOKASIDE                  0x00000010
-#define fMP_INIT_IN_PROGRESS                0x00000020
-#define fMP_SEND_SIDE_RESOURCE_ALLOCATED    0x00000040
-#define fMP_RECV_SIDE_RESOURCE_ALLOCATED    0x00000080
 #define fMP_POST_READS                      0x00000100
 #define fMP_POST_WRITES                     0x00000200
-#define fMP_CONTROL_READS                   0x00000400
-#define fMP_CONTROL_WRITES                  0x00000800
 
 #define MP_SET_FLAG(_M, _F)             ((_M)->Flags |= (_F))
 #define MP_CLEAR_FLAG(_M, _F)            ((_M)->Flags &= ~(_F))
 #define MP_TEST_FLAGS(_M, _F)            (((_M)->Flags & (_F)) == (_F))
 
-#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)
+#define MP_IS_READY(_M)        (((_M)->Flags & fMP_DISCONNECTED) == 0)
 
 int device_alloc_frag_buf(struct vnt_private *, PSDeFragControlBlock pDeF);
 void vnt_configure_filter(struct vnt_private *);
diff --git a/drivers/staging/vt6656/device_cfg.h b/drivers/staging/vt6656/device_cfg.h
deleted file mode 100644
index 0b9d8349c2e..00000000000
--- a/drivers/staging/vt6656/device_cfg.h
+++ /dev/null
@@ -1,87 +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: device_cfg.h
- *
- * Purpose: Driver configuration header
- * Author: Lyndon Chen
- *
- * Date: Dec 9, 2005
- *
- */
-#ifndef __DEVICE_CONFIG_H
-#define __DEVICE_CONFIG_H
-
-#include <linux/types.h>
-
-typedef
-struct _version {
-    unsigned char   major;
-    unsigned char   minor;
-    unsigned char   build;
-} version_t, *pversion_t;
-
-#ifndef false
-#define false   (0)
-#endif
-
-#ifndef true
-#define true    (!(false))
-#endif
-
-#define VID_TABLE_SIZE      64
-#define MCAST_TABLE_SIZE    64
-#define MCAM_SIZE           32
-#define VCAM_SIZE           32
-#define TX_QUEUE_NO         8
-
-#define DEVICE_NAME         "vt6656"
-#define DEVICE_FULL_DRV_NAM "VIA Networking Wireless LAN USB Driver"
-
-#ifndef MAJOR_VERSION
-#define MAJOR_VERSION       1
-#endif
-
-#ifndef MINOR_VERSION
-#define MINOR_VERSION       13
-#endif
-
-#ifndef DEVICE_VERSION
-#define DEVICE_VERSION       "1.19_12"
-#endif
-
-#define MAX_RATE	12
-
-/* config file */
-#include <linux/fs.h>
-#include <linux/fcntl.h>
-#ifndef CONFIG_PATH
-#define CONFIG_PATH            "/etc/vntconfiguration.dat"
-#endif
-
-/* Max: 2378 = 2312 Payload + 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 {
-    VT3184 = 1
-} CHIP_TYPE, *PCHIP_TYPE;
-
-#endif
diff --git a/drivers/staging/vt6656/dpc.c b/drivers/staging/vt6656/dpc.c
index 4ccaa7e29a1..c0ec5b37aa7 100644
--- a/drivers/staging/vt6656/dpc.c
+++ b/drivers/staging/vt6656/dpc.c
@@ -26,10 +26,7 @@
  *
  * Functions:
  *      device_receive_frame - Rcv 802.11 frame function
- *      s_bAPModeRxCtl- AP Rcv frame filer Ctl.
- *      s_bAPModeRxData- AP Rcv data frame handle
  *      s_bHandleRxEncryption- Rcv decrypted data via on-fly
- *      s_bHostWepRxEncryption- Rcv encrypted data via host
  *      s_byGetRateIdx- get rate index
  *      s_vGetDASA- get data offset
  *      s_vProcessRxMACHeader- Rcv 802.11 and translate to 802.3
@@ -48,12 +45,9 @@
 #include "baseband.h"
 #include "michael.h"
 #include "tkip.h"
-#include "tcrc.h"
 #include "wctl.h"
-#include "hostap.h"
 #include "rf.h"
 #include "iowpa.h"
-#include "aes_ccmp.h"
 #include "datarate.h"
 #include "usbpipe.h"
 
@@ -77,20 +71,10 @@ static void s_vProcessRxMACHeader(struct vnt_private *pDevice,
 	u8 *pbyRxBufferAddr, u32 cbPacketSize, int bIsWEP, int bExtIV,
 	u32 *pcbHeadSize);
 
-static int s_bAPModeRxCtl(struct vnt_private *pDevice, u8 *pbyFrame,
-	s32 iSANodeIndex);
-
-static int s_bAPModeRxData(struct vnt_private *pDevice, struct sk_buff *skb,
-	u32 FrameSize, u32 cbHeaderOffset, s32 iSANodeIndex, s32 iDANodeIndex);
-
 static int s_bHandleRxEncryption(struct vnt_private *pDevice, u8 *pbyFrame,
 	u32 FrameSize, u8 *pbyRsr, u8 *pbyNewRsr, PSKeyItem *pKeyOut,
 	s32 *pbExtIV, u16 *pwRxTSC15_0, u32 *pdwRxTSC47_16);
 
-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);
-
 /*+
  *
  * Description:
@@ -261,14 +245,13 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, struct vnt_rcb *pRCB,
 	u32 cbHeaderOffset, cbIVOffset;
 	u32 FrameSize;
 	u16 wEtherType = 0;
-	s32 iSANodeIndex = -1, iDANodeIndex = -1;
+	s32 iSANodeIndex = -1;
 	int ii;
 	u8 *pbyRxSts, *pbyRxRate, *pbySQ, *pby3SQ;
 	u32 cbHeaderSize;
 	PSKeyItem pKey = NULL;
 	u16 wRxTSC15_0 = 0;
 	u32 dwRxTSC47_16 = 0;
-	SKeyItem STempKey;
 	/* signed long ldBm = 0; */
 	int bIsWEP = false; int bExtIV = false;
 	u32 dwWbkStatus;
@@ -390,50 +373,14 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, struct vnt_rcb *pRCB,
         }
     }
 
-    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);
-        }
+
+	bRxDecryOK = s_bHandleRxEncryption(pDevice, pbyFrame, FrameSize,
+		pbyRsr, pbyNewRsr, &pKey, &bExtIV, &wRxTSC15_0, &dwRxTSC47_16);
 
         if (bRxDecryOK) {
             if ((*pbyNewRsr & NEWRSR_DECRYPTOK) == 0) {
@@ -517,22 +464,6 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, struct vnt_rcb *pRCB,
             }
             pRxPacket->byRxChannel = (*pbyRxSts) >> 2;
 
-            // hostap Deamon handle 802.11 management
-            if (pDevice->bEnableHostapd) {
-	            skb->dev = pDevice->apdev;
-	            //skb->data += 4;
-	            //skb->tail += 4;
-	            skb->data += 8;
-	            skb->tail += 8;
-                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;
-	        }
-
             //
             // Insert the RCB in the Recv Mng list
             //
@@ -625,13 +556,6 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, struct vnt_rcb *pRCB,
         }
     }
 
-    // Now it only supports 802.11g Infrastructure Mode, and support rate must up to 54 Mbps
-    if (pDevice->bDiversityEnable && (FrameSize>50) &&
-	pDevice->op_mode == NL80211_IFTYPE_STATION &&
-       (pDevice->bLinkPass == true)) {
-        BBvAntennaDiversity(pDevice, s_byGetRateIdx(*pbyRxRate), 0);
-    }
-
     // ++++++++ For BaseBand Algorithm +++++++++++++++
     pDevice->uCurrRSSI = *pbyRSSI;
     pDevice->byCurrSQ = *pbySQ;
@@ -654,45 +578,6 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, struct vnt_rcb *pRCB,
     }
 */
 
-    // -----------------------------------------------
-
-    if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) && (pDevice->bEnable8021x == true)){
-        u8    abyMacHdr[24];
-
-        // Only 802.1x packet incoming allowed
-        if (bIsWEP)
-            cbIVOffset = 8;
-        else
-            cbIVOffset = 0;
-        wEtherType = (skb->data[cbIVOffset + 8 + 24 + 6] << 8) |
-                    skb->data[cbIVOffset + 8 + 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 + 8), 24);
-                memcpy((skb->data + 8 + cbIVOffset), &abyMacHdr[0], 24);
-            }
-
-            skb->data +=  (cbIVOffset + 8);
-            skb->tail +=  (cbIVOffset + 8);
-            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
@@ -827,26 +712,6 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, struct vnt_rcb *pRCB,
     if (FrameSize < 12)
         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);
@@ -866,112 +731,6 @@ int RXbBulkInProcessData(struct vnt_private *pDevice, struct vnt_rcb *pRCB,
     return true;
 }
 
-static int s_bAPModeRxCtl(struct vnt_private *pDevice, u8 *pbyFrame,
-	s32 iSANodeIndex)
-{
-	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
-	struct ieee80211_hdr *p802_11Header;
-	CMD_STATUS Status;
-
-    if (IS_CTL_PSPOLL(pbyFrame) || !IS_TYPE_CONTROL(pbyFrame)) {
-
-        p802_11Header = (struct ieee80211_hdr *) (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,
-                                         (u8 *)(p802_11Header->addr2),
-                                         (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,
-                                         (u8 *)(p802_11Header->addr2),
-                                         (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,
-                                       (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->addr3);
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ADDR2:%pM\n",
-				p802_11Header->addr2);
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ADDR1:%pM\n",
-				p802_11Header->addr1);
-                    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: frame_control= %x\n", p802_11Header->frame_control);
-                    return true;
-            }
-        }
-    }
-    return false;
-
-}
-
 static int s_bHandleRxEncryption(struct vnt_private *pDevice, u8 *pbyFrame,
 	u32 FrameSize, u8 *pbyRsr, u8 *pbyNewRsr, PSKeyItem *pKeyOut,
 	s32 *pbExtIV, u16 *pwRxTSC15_0, u32 *pdwRxTSC47_16)
@@ -1098,213 +857,20 @@ static int s_bHandleRxEncryption(struct vnt_private *pDevice, u8 *pbyFrame,
     return true;
 }
 
-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)
-{
-	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
-	struct ieee80211_hdr *pMACHeader;
-	u32 PayloadLen = FrameSize;
-	u8 *pbyIV;
-	u8 byKeyIdx;
-	u8 byDecMode = KEY_CTL_WEP;
-
-	*pwRxTSC15_0 = 0;
-	*pdwRxTSC47_16 = 0;
-
-    pbyIV = pbyFrame + WLAN_HDR_ADDR3_LEN;
-    if ( WLAN_GET_FC_TODS(*(u16 *)pbyFrame) &&
-         WLAN_GET_FC_FROMDS(*(u16 *)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->byCSSGK == KEY_CTL_TKIP)
-        byDecMode = KEY_CTL_TKIP;
-    else if (pMgmt->byCSSGK == KEY_CTL_CCMP)
-        byDecMode = KEY_CTL_CCMP;
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"AES:%d %d %d\n", pMgmt->byCSSPK, pMgmt->byCSSGK, byDecMode);
-
-    if (byDecMode != pKey->byCipherSuite) {
-        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(*(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(*(u16 *)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 = (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)) {
-                    *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;
-}
-
-static int s_bAPModeRxData(struct vnt_private *pDevice, struct sk_buff *skb,
-	u32 FrameSize, u32 cbHeaderOffset, s32 iSANodeIndex, s32 iDANodeIndex)
-{
-	struct sk_buff *skbcpy;
-	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
-	int  bRelayAndForward = false;
-	int bRelayOnly = false;
-	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((u8 *)(skb->data+cbHeaderOffset))) {
-       if (pMgmt->sNodeDBTable[0].bPSEnable) {
-
-	    skbcpy = netdev_alloc_skb(pDevice->dev, 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->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 (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.
-
-	                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)) {
-		bRelayPacketSend(pDevice, (u8 *) (skb->data + cbHeaderOffset),
-				 FrameSize, (unsigned int) iDANodeIndex);
-        }
-
-        if (bRelayOnly)
-            return false;
-    }
-    // none associate, don't forward
-    if (pDevice->uAssocCount == 0)
-        return false;
-
-    return true;
-}
-
 void RXvWorkItem(struct work_struct *work)
 {
 	struct vnt_private *priv =
 		container_of(work, struct vnt_private, read_work_item);
 	int status;
 	struct vnt_rcb *rcb = NULL;
+	unsigned long flags;
 
 	if (priv->Flags & fMP_DISCONNECTED)
 		return;
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---->Rx Polling Thread\n");
 
-	spin_lock_irq(&priv->lock);
+	spin_lock_irqsave(&priv->lock, flags);
 
 	while ((priv->Flags & fMP_POST_READS) && MP_IS_READY(priv) &&
 			(priv->NumRecvFreeList != 0)) {
@@ -1319,7 +885,7 @@ void RXvWorkItem(struct work_struct *work)
 
 	priv->bIsRxWorkItemQueued = false;
 
-	spin_unlock_irq(&priv->lock);
+	spin_unlock_irqrestore(&priv->lock, flags);
 }
 
 void RXvFreeRCB(struct vnt_rcb *rcb, int re_alloc_skb)
@@ -1363,34 +929,43 @@ void RXvMngWorkItem(struct work_struct *work)
 	struct vnt_rcb *pRCB = NULL;
 	struct vnt_rx_mgmt *pRxPacket;
 	int bReAllocSkb = false;
+	unsigned long flags;
 
 	if (pDevice->Flags & fMP_DISCONNECTED)
 		return;
 
     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---->Rx Mng Thread\n");
 
-    spin_lock_irq(&pDevice->lock);
     while (pDevice->NumRecvMngList!=0)
     {
+	spin_lock_irqsave(&pDevice->lock, flags);
+
         pRCB = pDevice->FirstRecvMngList;
         pDevice->NumRecvMngList--;
         DequeueRCB(pDevice->FirstRecvMngList, pDevice->LastRecvMngList);
+
+	spin_unlock_irqrestore(&pDevice->lock, flags);
+
         if(!pRCB){
             break;
         }
         pRxPacket = &(pRCB->sMngPacket);
 	vMgrRxManagePacket(pDevice, &pDevice->vnt_mgmt, pRxPacket);
         pRCB->Ref--;
-        if(pRCB->Ref == 0) {
-            DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"RxvFreeMng %d %d\n",pDevice->NumRecvFreeList, pDevice->NumRecvMngList);
-            RXvFreeRCB(pRCB, bReAllocSkb);
-        } else {
+	if (pRCB->Ref == 0) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"RxvFreeMng %d %d\n",
+			pDevice->NumRecvFreeList, pDevice->NumRecvMngList);
+
+		spin_lock_irqsave(&pDevice->lock, flags);
+
+		RXvFreeRCB(pRCB, bReAllocSkb);
+
+		spin_unlock_irqrestore(&pDevice->lock, flags);
+	} else {
             DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Rx Mng Only we have the right to free RCB\n");
         }
     }
 
 	pDevice->bIsRxMngWorkItemQueued = false;
-	spin_unlock_irq(&pDevice->lock);
-
 }
 
diff --git a/drivers/staging/vt6656/firmware.c b/drivers/staging/vt6656/firmware.c
index cd2ea76c8b1..1159f0b3457 100644
--- a/drivers/staging/vt6656/firmware.c
+++ b/drivers/staging/vt6656/firmware.c
@@ -31,9 +31,9 @@
  *
  */
 
+#include <linux/compiler.h>
 #include "firmware.h"
-#include "control.h"
-#include "rndis.h"
+#include "usbpipe.h"
 
 static int msglevel = MSG_LEVEL_INFO;
 /* static int msglevel = MSG_LEVEL_DEBUG; */
@@ -54,7 +54,6 @@ int FIRMWAREbDownload(struct vnt_private *pDevice)
 	int ii, rc;
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---->Download firmware\n");
-	spin_unlock_irq(&pDevice->lock);
 
 	rc = request_firmware(&fw, FIRMWARE_NAME, dev);
 	if (rc) {
@@ -71,7 +70,7 @@ int FIRMWAREbDownload(struct vnt_private *pDevice)
 		wLength = min_t(int, fw->size - ii, FIRMWARE_CHUNK_SIZE);
 		memcpy(pBuffer, fw->data + ii, wLength);
 
-		NdisStatus = CONTROLnsRequestOutAsyn(pDevice,
+		NdisStatus = vnt_control_out(pDevice,
 						0,
 						0x1200+ii,
 						0x0000,
@@ -91,7 +90,6 @@ free_fw:
 out:
 	kfree(pBuffer);
 
-	spin_lock_irq(&pDevice->lock);
 	return result;
 }
 MODULE_FIRMWARE(FIRMWARE_NAME);
@@ -102,7 +100,7 @@ int FIRMWAREbBrach2Sram(struct vnt_private *pDevice)
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---->Branch to Sram\n");
 
-	NdisStatus = CONTROLnsRequestOut(pDevice,
+	NdisStatus = vnt_control_out(pDevice,
 					1,
 					0x1200,
 					0x0000,
@@ -118,7 +116,7 @@ int FIRMWAREbCheckVersion(struct vnt_private *pDevice)
 {
 	int ntStatus;
 
-	ntStatus = CONTROLnsRequestIn(pDevice,
+	ntStatus = vnt_control_in(pDevice,
 					MESSAGE_TYPE_READ,
 					0,
 					MESSAGE_REQUEST_VERSION,
diff --git a/drivers/staging/vt6656/hostap.c b/drivers/staging/vt6656/hostap.c
deleted file mode 100644
index 67ba48b9a8d..00000000000
--- a/drivers/staging/vt6656/hostap.c
+++ /dev/null
@@ -1,779 +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: hostap.c
- *
- * Purpose: handle hostap daemon ioctl input/out functions
- *
- * Author: Lyndon Chen
- *
- * Date: Oct. 20, 2003
- *
- * Functions:
- *
- * Revision History:
- *
- */
-
-#include "hostap.h"
-#include "iocmd.h"
-#include "mac.h"
-#include "card.h"
-#include "baseband.h"
-#include "wpactl.h"
-#include "key.h"
-#include "datarate.h"
-
-#define VIAWGET_HOSTAPD_MAX_BUF_SIZE 1024
-#define HOSTAP_CRYPT_FLAG_SET_TX_KEY BIT0
-#define HOSTAP_CRYPT_ERR_UNKNOWN_ADDR 3
-#define HOSTAP_CRYPT_ERR_KEY_SET_FAILED 5
-
-static int          msglevel                =MSG_LEVEL_INFO;
-
-/*
- * Description:
- *      register net_device (AP) for hostap daemon
- *
- * Parameters:
- *  In:
- *      pDevice             -
- *      rtnl_locked         -
- *  Out:
- *
- * Return Value:
- *
- */
-
-static int hostap_enable_hostapd(struct vnt_private *pDevice, int rtnl_locked)
-{
-	struct vnt_private *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);
-
-	pDevice->apdev = kzalloc(sizeof(struct net_device), GFP_KERNEL);
-	if (pDevice->apdev == NULL)
-		return -ENOMEM;
-
-    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;
-
-	pDevice->apdev->type = ARPHRD_IEEE80211;
-
-	pDevice->apdev->base_addr = dev->base_addr;
-	pDevice->apdev->irq = dev->irq;
-	pDevice->apdev->mem_start = dev->mem_start;
-	pDevice->apdev->mem_end = dev->mem_end;
-	sprintf(pDevice->apdev->name, "%sap", dev->name);
-	if (rtnl_locked)
-		ret = register_netdevice(pDevice->apdev);
-	else
-		ret = register_netdev(pDevice->apdev);
-	if (ret) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%s: register_netdevice(AP) failed!\n",
-		       dev->name);
-		return -1;
-	}
-
-    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%s: Registered netdevice %s for AP management\n",
-	       dev->name, pDevice->apdev->name);
-
-    KeyvInitTable(pDevice,&pDevice->sKey);
-
-	return 0;
-}
-
-/*
- * Description:
- *      unregister net_device(AP)
- *
- * Parameters:
- *  In:
- *      pDevice             -
- *      rtnl_locked         -
- *  Out:
- *
- * Return Value:
- *
- */
-
-static int hostap_disable_hostapd(struct vnt_private *pDevice, int rtnl_locked)
-{
-
-    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 (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);
-	}
-	if (pDevice->apdev)
-		free_netdev(pDevice->apdev);
-	pDevice->apdev = NULL;
-    pDevice->bEnable8021x = false;
-    pDevice->bEnableHostWEP = false;
-    pDevice->bEncryptionEnable = false;
-
-	return 0;
-}
-
-/*
- * Description:
- *      Set enable/disable hostapd mode
- *
- * Parameters:
- *  In:
- *      pDevice             -
- *      rtnl_locked         -
- *  Out:
- *
- * Return Value:
- *
- */
-
-int vt6656_hostap_set_hostapd(struct vnt_private *pDevice,
-	int val, int rtnl_locked)
-{
-	if (val < 0 || val > 1)
-		return -EINVAL;
-
-	if (pDevice->bEnableHostapd == val)
-		return 0;
-
-	pDevice->bEnableHostapd = val;
-
-	if (val)
-		return hostap_enable_hostapd(pDevice, rtnl_locked);
-	else
-		return hostap_disable_hostapd(pDevice, rtnl_locked);
-}
-
-/*
- * Description:
- *      remove station function supported for hostap daemon
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *
- * Return Value:
- *
- */
-static int hostap_remove_sta(struct vnt_private *pDevice,
-	struct viawget_hostapd_param *param)
-{
-	unsigned int uNodeIndex;
-
-    if (BSSbIsSTAInNodeDB(pDevice, param->sta_addr, &uNodeIndex)) {
-        BSSvRemoveOneNode(pDevice, uNodeIndex);
-    }
-    else {
-        return -ENOENT;
-    }
-	return 0;
-}
-
-/*
- * Description:
- *      add a station from hostap daemon
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *
- * Return Value:
- *
- */
-static int hostap_add_sta(struct vnt_private *pDevice,
-	struct viawget_hostapd_param *param)
-{
-	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
-	unsigned int uNodeIndex;
-
-	if (!BSSbIsSTAInNodeDB(pDevice, param->sta_addr, &uNodeIndex))
-		BSSvCreateOneNode(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 = (u16)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;
-}
-
-/*
- * Description:
- *      get station info
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *
- * Return Value:
- *
- */
-
-static int hostap_get_info_sta(struct vnt_private *pDevice,
-	struct viawget_hostapd_param *param)
-{
-	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
-	unsigned int uNodeIndex;
-
-    if (BSSbIsSTAInNodeDB(pDevice, 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;
-	}
-
-	return 0;
-}
-
-/*
- * Description:
- *      set station flag
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *
- * Return Value:
- *
- */
-static int hostap_set_flags_sta(struct vnt_private *pDevice,
-		struct viawget_hostapd_param *param)
-{
-	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
-	unsigned int uNodeIndex;
-
-    if (BSSbIsSTAInNodeDB(pDevice, 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;
-	}
-
-	return 0;
-}
-
-/*
- * Description:
- *      set generic element (wpa ie)
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *
- * Return Value:
- *
- */
-static int hostap_set_generic_element(struct vnt_private *pDevice,
-					struct viawget_hostapd_param *param)
-{
-	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
-
-    memcpy( pMgmt->abyWPAIE,
-            param->u.generic_elem.data,
-            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);
-
-    // 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;
-    }
-
-	return 0;
-}
-
-/*
- * Description:
- *      flush station nodes table.
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *  Out:
- *
- * Return Value:
- *
- */
-
-static void hostap_flush_sta(struct vnt_private *pDevice)
-{
-    // reserved node index =0 for multicast node.
-    BSSvClearNodeDBTable(pDevice, 1);
-    pDevice->uAssocCount = 0;
-
-    return;
-}
-
-/*
- * Description:
- *      set each stations encryption key
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *
- * Return Value:
- *
- */
-static int hostap_set_encryption(struct vnt_private *pDevice,
-	struct viawget_hostapd_param *param, int param_len)
-{
-	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
-	u32 dwKeyIndex = 0;
-	u8 abyKey[MAX_KEY_LEN];
-	u8 abySeq[MAX_KEY_LEN];
-	NDIS_802_11_KEY_RSC   KeyRSC;
-	u8 byKeyDecMode = KEY_CTL_WEP;
-	int ret = 0;
-	s32 iNodeIndex = -1;
-	int ii;
-	bool 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");
-		return -EINVAL;
-	}
-
-	if (is_broadcast_ether_addr(param->sta_addr)) {
-		if (param->u.crypt.idx >= MAX_GROUP_KEY)
-			return -EINVAL;
-        iNodeIndex = 0;
-
-	} else {
-	    if (BSSbIsSTAInNodeDB(pDevice, 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);
-
-	if (param->u.crypt.alg == WPA_ALG_NONE) {
-
-        if (pMgmt->sNodeDBTable[iNodeIndex].bOnFly == true) {
-            if (KeybRemoveKey( pDevice,
-                               &(pDevice->sKey),
-                               param->sta_addr,
-                               pMgmt->sNodeDBTable[iNodeIndex].dwKeyIndex
-                              ) == 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 = (u32)(param->u.crypt.idx);
-    if (param->u.crypt.flags & HOSTAP_CRYPT_FLAG_SET_TX_KEY) {
-        pDevice->byKeyIndex = (u8)dwKeyIndex;
-        pDevice->bTransmitKey = true;
-        dwKeyIndex |= (1 << 31);
-    }
-
-	if (param->u.crypt.alg == WPA_ALG_WEP) {
-
-        if ((pDevice->bEnable8021x == false) || (iNodeIndex == 0)) {
-            KeybSetDefaultKey(  pDevice,
-                                &(pDevice->sKey),
-                                dwKeyIndex & ~(BIT30 | USE_KEYRSC),
-                                param->u.crypt.key_len,
-                                NULL,
-                                abyKey,
-                                KEY_CTL_WEP
-                             );
-
-        } else {
-            // 8021x enable, individual key
-            dwKeyIndex |= (1 << 30); // set pairwise key
-		if (KeybSetKey(pDevice, &(pDevice->sKey),
-			&param->sta_addr[0],
-			dwKeyIndex & ~(USE_KEYRSC),
-			param->u.crypt.key_len,
-			&KeyRSC, (u8 *)abyKey,
-			KEY_CTL_WEP
-                           ) == 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++)
-			KeyRSC |= (unsigned long)abySeq[ii] << (ii * 8);
-
-		dwKeyIndex |= 1 << 29;
-		pMgmt->sNodeDBTable[iNodeIndex].KeyRSC = KeyRSC;
-	}
-
-	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.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,
-                           &(pDevice->sKey),
-                           dwKeyIndex,
-                           param->u.crypt.key_len,
-			&KeyRSC,
-                           abyKey,
-                           byKeyDecMode
-                          );
-       pMgmt->sNodeDBTable[iNodeIndex].bOnFly = true;
-
-    } else {
-        dwKeyIndex |= (1 << 30); // set pairwise key
-        if (KeybSetKey(pDevice,
-                       &(pDevice->sKey),
-                       &param->sta_addr[0],
-                       dwKeyIndex,
-                       param->u.crypt.key_len,
-			&KeyRSC,
-                       (u8 *)abyKey,
-                        byKeyDecMode
-                       ) == 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
-// Todo.. xxxxxx
-        //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;
-
-	return ret;
-}
-
-/*
- * Description:
- *      get each stations encryption key
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      param     -
- *  Out:
- *
- * Return Value:
- *
- */
-static int hostap_get_encryption(struct vnt_private *pDevice,
-				       struct viawget_hostapd_param *param,
-				       int param_len)
-{
-	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
-	int ret = 0;
-	int ii;
-	s32 iNodeIndex = 0;
-
-	param->u.crypt.err = 0;
-
-	if (is_broadcast_ether_addr(param->sta_addr)) {
-        iNodeIndex = 0;
-	} else {
-	    if (BSSbIsSTAInNodeDB(pDevice, 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] = (u8)pMgmt->sNodeDBTable[iNodeIndex].KeyRSC >> (ii * 8);
-    }
-
-	return ret;
-}
-
-/*
- * Description:
- *      vt6656_hostap_ioctl main function supported for hostap daemon.
- *
- * Parameters:
- *  In:
- *      pDevice   -
- *      iw_point  -
- *  Out:
- *
- * Return Value:
- *
- */
-
-int vt6656_hostap_ioctl(struct vnt_private *pDevice, struct iw_point *p)
-{
-	struct viawget_hostapd_param *param;
-	int ret = 0;
-	int ap_ioctl = 0;
-
-	if (p->length < sizeof(struct viawget_hostapd_param) ||
-	    p->length > VIAWGET_HOSTAPD_MAX_BUF_SIZE || !p->pointer)
-		return -EINVAL;
-
-	param = kmalloc((int)p->length, GFP_KERNEL);
-	if (param == NULL)
-		return -ENOMEM;
-
-	if (copy_from_user(param, p->pointer, p->length)) {
-		ret = -EFAULT;
-		goto out;
-	}
-
-	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);
-		ret = hostap_set_encryption(pDevice, param, p->length);
-        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);
-		ret = hostap_get_encryption(pDevice, param, p->length);
-        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");
-		ret = -EOPNOTSUPP;
-		goto out;
-	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);
-		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);
-		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);
-		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;
-		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);
-		break;
-
-	case VIAWGET_HOSTAPD_MLME:
-	    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");
-		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;
-
-	case VIAWGET_HOSTAPD_STA_CLEAR_STATS:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_STA_CLEAR_STATS \n");
-	    ret = -EOPNOTSUPP;
-	    goto out;
-	default:
-	    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "vt6656_hostap_ioctl: unknown cmd=%d\n",
-		       (int)param->cmd);
-		ret = -EOPNOTSUPP;
-		goto out;
-	}
-
-	if ((ret == 0) && ap_ioctl) {
-		if (copy_to_user(p->pointer, param, p->length)) {
-			ret = -EFAULT;
-			goto out;
-		}
-	}
-
- out:
-	kfree(param);
-
-	return ret;
-}
-
diff --git a/drivers/staging/vt6656/hostap.h b/drivers/staging/vt6656/hostap.h
deleted file mode 100644
index 6a68f7e69cf..00000000000
--- a/drivers/staging/vt6656/hostap.h
+++ /dev/null
@@ -1,58 +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: hostap.h
- *
- * Purpose:
- *
- * Author: Lyndon Chen
- *
- * Date: May 21, 2003
- *
- */
-
-#ifndef __HOSTAP_H__
-#define __HOSTAP_H__
-
-#include "device.h"
-
-#define WLAN_RATE_1M    BIT0
-#define WLAN_RATE_2M    BIT1
-#define WLAN_RATE_5M5   BIT2
-#define WLAN_RATE_11M   BIT3
-#define WLAN_RATE_6M    BIT4
-#define WLAN_RATE_9M    BIT5
-#define WLAN_RATE_12M   BIT6
-#define WLAN_RATE_18M   BIT7
-#define WLAN_RATE_24M   BIT8
-#define WLAN_RATE_36M   BIT9
-#define WLAN_RATE_48M   BIT10
-#define WLAN_RATE_54M   BIT11
-
-#ifndef ETH_P_PAE
-#define ETH_P_PAE 0x888E /* Port Access Entity (IEEE 802.1X) */
-#endif /* ETH_P_PAE */
-
-#ifndef ARPHRD_IEEE80211
-#define ARPHRD_IEEE80211 801
-#endif
-
-int vt6656_hostap_set_hostapd(struct vnt_private *, int val, int rtnl_locked);
-int vt6656_hostap_ioctl(struct vnt_private *, struct iw_point *p);
-
-#endif /* __HOSTAP_H__ */
diff --git a/drivers/staging/vt6656/int.c b/drivers/staging/vt6656/int.c
index cca56b2f243..f2a5225b50f 100644
--- a/drivers/staging/vt6656/int.c
+++ b/drivers/staging/vt6656/int.c
@@ -65,15 +65,16 @@ static int msglevel = MSG_LEVEL_INFO; /* MSG_LEVEL_DEBUG */
 -*/
 void INTvWorkItem(struct vnt_private *pDevice)
 {
+	unsigned long flags;
 	int ntStatus;
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---->Interrupt Polling Thread\n");
 
-	spin_lock_irq(&pDevice->lock);
+	spin_lock_irqsave(&pDevice->lock, flags);
 
 	ntStatus = PIPEnsInterruptRead(pDevice);
 
-	spin_unlock_irq(&pDevice->lock);
+	spin_unlock_irqrestore(&pDevice->lock, flags);
 }
 
 void INTnsProcessData(struct vnt_private *priv)
diff --git a/drivers/staging/vt6656/iocmd.h b/drivers/staging/vt6656/iocmd.h
index f3406da7336..b957e6d475a 100644
--- a/drivers/staging/vt6656/iocmd.h
+++ b/drivers/staging/vt6656/iocmd.h
@@ -29,397 +29,10 @@
 #ifndef __IOCMD_H__
 #define __IOCMD_H__
 
-// ioctl Command code
-#define MAGIC_CODE	                 0x3142
-#define IOCTL_CMD_TEST	            (SIOCDEVPRIVATE + 0)
-#define IOCTL_CMD_SET			    (SIOCDEVPRIVATE + 1)
-#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
-
-} WMAC_CMD, *PWMAC_CMD;
-
 typedef enum tagWZONETYPE {
   ZoneType_USA = 0,
   ZoneType_Japan = 1,
   ZoneType_Europe = 2
 } WZONETYPE;
 
-#define ADHOC	0
-#define INFRA	1
-#define BOTH	2
-#define AP	    3
-
-#define ADHOC_STARTED	   1
-#define ADHOC_JOINTED	   2
-
-#define PHY80211a 0
-#define PHY80211b 1
-#define PHY80211g 2
-
-#define SSID_ID                0
-#define SSID_MAXLEN            32
-#define BSSID_LEN              6
-#define WEP_NKEYS              4
-#define WEP_KEYMAXLEN          29
-#define WEP_40BIT_LEN          5
-#define WEP_104BIT_LEN         13
-#define WEP_232BIT_LEN         16
-
-// Ioctl interface structure
-// Command structure
-//
-typedef struct tagSCmdRequest {
-	u8 name[16];
-	void	*data;
-	u16	    wResult;
-	u16     wCmdCode;
-} __packed SCmdRequest, *PSCmdRequest;
-
-//
-// Scan
-//
-
-typedef struct tagSCmdScan {
-
-    u8	    ssid[SSID_MAXLEN + 2];
-
-} __packed SCmdScan, *PSCmdScan;
-
-//
-// BSS Join
-//
-
-typedef struct tagSCmdBSSJoin {
-
-    u16	    wBSSType;
-    u16     wBBPType;
-    u8	    ssid[SSID_MAXLEN + 2];
-    u32	    uChannel;
-    bool    bPSEnable;
-    bool    bShareKeyAuth;
-
-} __packed SCmdBSSJoin, *PSCmdBSSJoin;
-
-//
-// Zonetype Setting
-//
-
-typedef struct tagSCmdZoneTypeSet {
-
- bool       bWrite;
- WZONETYPE  ZoneType;
-
-} __packed SCmdZoneTypeSet, *PSCmdZoneTypeSet;
-
-typedef struct tagSWPAResult {
-         char	ifname[100];
-	u8 proto;
-	u8 key_mgmt;
-	u8 eap_type;
-         bool authenticated;
-} __packed SWPAResult, *PSWPAResult;
-
-typedef struct tagSCmdStartAP {
-
-    u16	    wBSSType;
-    u16     wBBPType;
-    u8	    ssid[SSID_MAXLEN + 2];
-	u32 uChannel;
-	u32 uBeaconInt;
-    bool    bShareKeyAuth;
-    u8      byBasicRate;
-
-} __packed SCmdStartAP, *PSCmdStartAP;
-
-typedef struct tagSCmdSetWEP {
-
-    bool    bEnableWep;
-    u8      byKeyIndex;
-    u8      abyWepKey[WEP_NKEYS][WEP_KEYMAXLEN];
-    bool    bWepKeyAvailable[WEP_NKEYS];
-    u32     auWepKeyLength[WEP_NKEYS];
-
-} __packed SCmdSetWEP, *PSCmdSetWEP;
-
-typedef struct tagSBSSIDItem {
-
-	u32	    uChannel;
-    u8      abyBSSID[BSSID_LEN];
-    u8      abySSID[SSID_MAXLEN + 1];
-    u16	    wBeaconInterval;
-    u16	    wCapInfo;
-    u8      byNetType;
-    bool    bWEPOn;
-    u32     uRSSI;
-
-} __packed SBSSIDItem;
-
-typedef struct tagSBSSIDList {
-
-	u32		    uItem;
-	SBSSIDItem	sBSSIDList[0];
-} __packed SBSSIDList, *PSBSSIDList;
-
-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;
-
-} __packed SNodeItem;
-
-typedef struct tagSNodeList {
-
-	u32		    uItem;
-	SNodeItem	sNodeList[0];
-
-} __packed SNodeList, *PSNodeList;
-
-typedef struct tagSCmdLinkStatus {
-
-    bool    bLink;
-	u16	    wBSSType;
-	u8      byState;
-    u8      abyBSSID[BSSID_LEN];
-    u8      abySSID[SSID_MAXLEN + 2];
-    u32     uChannel;
-    u32     uLinkRate;
-
-} __packed SCmdLinkStatus, *PSCmdLinkStatus;
-
-//
-// 802.11 counter
-//
-typedef struct tagSDot11MIBCount {
-    u32 TransmittedFragmentCount;
-    u32 MulticastTransmittedFrameCount;
-    u32 FailedCount;
-    u32 RetryCount;
-    u32 MultipleRetryCount;
-    u32 RTSSuccessCount;
-    u32 RTSFailureCount;
-    u32 ACKFailureCount;
-    u32 FrameDuplicateCount;
-    u32 ReceivedFragmentCount;
-    u32 MulticastReceivedFrameCount;
-    u32 FCSErrorCount;
-} __packed SDot11MIBCount, *PSDot11MIBCount;
-
-//
-// statistic counter
-//
-typedef struct tagSStatMIBCount {
-    //
-    // ISR status count
-    //
-    u32   dwIsrTx0OK;
-    u32   dwIsrTx1OK;
-    u32   dwIsrBeaconTxOK;
-    u32   dwIsrRxOK;
-    u32   dwIsrTBTTInt;
-    u32   dwIsrSTIMERInt;
-    u32   dwIsrUnrecoverableError;
-    u32   dwIsrSoftInterrupt;
-    u32   dwIsrRxNoBuf;
-    /////////////////////////////////////
-
-	u32 dwIsrUnknown; /* unknown interrupt count */
-
-    // RSR status count
-    //
-    u32   dwRsrFrmAlgnErr;
-    u32   dwRsrErr;
-    u32   dwRsrCRCErr;
-    u32   dwRsrCRCOk;
-    u32   dwRsrBSSIDOk;
-    u32   dwRsrADDROk;
-    u32   dwRsrICVOk;
-    u32   dwNewRsrShortPreamble;
-    u32   dwRsrLong;
-    u32   dwRsrRunt;
-
-    u32   dwRsrRxControl;
-    u32   dwRsrRxData;
-    u32   dwRsrRxManage;
-
-    u32   dwRsrRxPacket;
-    u32   dwRsrRxOctet;
-    u32   dwRsrBroadcast;
-    u32   dwRsrMulticast;
-    u32   dwRsrDirected;
-    // 64-bit OID
-    u32   ullRsrOK;
-
-    // for some optional OIDs (64 bits) and DMI support
-    u32   ullRxBroadcastBytes;
-    u32   ullRxMulticastBytes;
-    u32   ullRxDirectedBytes;
-    u32   ullRxBroadcastFrames;
-    u32   ullRxMulticastFrames;
-    u32   ullRxDirectedFrames;
-
-    u32   dwRsrRxFragment;
-    u32   dwRsrRxFrmLen64;
-    u32   dwRsrRxFrmLen65_127;
-    u32   dwRsrRxFrmLen128_255;
-    u32   dwRsrRxFrmLen256_511;
-    u32   dwRsrRxFrmLen512_1023;
-    u32   dwRsrRxFrmLen1024_1518;
-
-    // TSR0,1 status count
-    //
-	u32 dwTsrTotalRetry[2];        /* total collision retry count */
-	u32 dwTsrOnceRetry[2];         /* this packet had one collision */
-	u32 dwTsrMoreThanOnceRetry[2]; /* this packet had many collisions */
-	u32 dwTsrRetry[2];             /* this packet has ever occur collision,
-					* that is (dwTsrOnceCollision0 plus
-					* dwTsrMoreThanOnceCollision0) */
-    u32   dwTsrACKData[2];
-    u32   dwTsrErr[2];
-    u32   dwAllTsrOK[2];
-    u32   dwTsrRetryTimeout[2];
-    u32   dwTsrTransmitTimeout[2];
-
-    u32   dwTsrTxPacket[2];
-    u32   dwTsrTxOctet[2];
-    u32   dwTsrBroadcast[2];
-    u32   dwTsrMulticast[2];
-    u32   dwTsrDirected[2];
-
-    // RD/TD count
-    u32   dwCntRxFrmLength;
-    u32   dwCntTxBufLength;
-
-    u8    abyCntRxPattern[16];
-    u8    abyCntTxPattern[16];
-
-	/* Software check.... */
-	u32 dwCntRxDataErr;  /* rx buffer data CRC err count */
-	u32 dwCntDecryptErr; /* rx buffer data CRC err count */
-	u32 dwCntRxICVErr;   /* rx buffer data CRC err count */
-	u32 idxRxErrorDesc;  /* index for rx data error RD */
-
-	/* 64-bit OID */
-	u32   ullTsrOK[2];
-
-    // for some optional OIDs (64 bits) and DMI support
-    u32   ullTxBroadcastFrames[2];
-    u32   ullTxMulticastFrames[2];
-    u32   ullTxDirectedFrames[2];
-    u32   ullTxBroadcastBytes[2];
-    u32   ullTxMulticastBytes[2];
-    u32   ullTxDirectedBytes[2];
-} __packed SStatMIBCount, *PSStatMIBCount;
-
-typedef struct tagSCmdValue {
-
-    u32     dwValue;
-
-} __packed SCmdValue,  *PSCmdValue;
-
-//
-// hostapd & viawget ioctl related
-//
-
-// VIAGWET_IOCTL_HOSTAPD ioctl() cmd:
-enum {
-	VIAWGET_HOSTAPD_FLUSH = 1,
-	VIAWGET_HOSTAPD_ADD_STA = 2,
-	VIAWGET_HOSTAPD_REMOVE_STA = 3,
-	VIAWGET_HOSTAPD_GET_INFO_STA = 4,
-	VIAWGET_HOSTAPD_SET_ENCRYPTION = 5,
-	VIAWGET_HOSTAPD_GET_ENCRYPTION = 6,
-	VIAWGET_HOSTAPD_SET_FLAGS_STA = 7,
-	VIAWGET_HOSTAPD_SET_ASSOC_AP_ADDR = 8,
-	VIAWGET_HOSTAPD_SET_GENERIC_ELEMENT = 9,
-	VIAWGET_HOSTAPD_MLME = 10,
-	VIAWGET_HOSTAPD_SCAN_REQ = 11,
-	VIAWGET_HOSTAPD_STA_CLEAR_STATS = 12,
-};
-
-#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];
-	union {
-		struct {
-			u16 aid;
-			u16 capability;
-			u8 tx_supp_rates;
-		} add_sta;
-		struct {
-			u32 inactive_sec;
-		} get_info_sta;
-		struct {
-			u8 alg;
-			u32 flags;
-			u32 err;
-			u8 idx;
-			u8 seq[8];
-			u16 key_len;
-			u8 key[0];
-		} crypt;
-		struct {
-			u32 flags_and;
-			u32 flags_or;
-		} set_flags_sta;
-		struct {
-			u16 rid;
-			u16 len;
-			u8 data[0];
-		} rid;
-		struct {
-			u8 len;
-			u8 data[0];
-		} generic_elem;
-		struct {
-			u16 cmd;
-			u16 reason_code;
-		} mlme;
-		struct {
-			u8 ssid_len;
-			u8 ssid[32];
-		} scan_req;
-	} u;
-} __packed;
-
 #endif /* __IOCMD_H__ */
diff --git a/drivers/staging/vt6656/iowpa.h b/drivers/staging/vt6656/iowpa.h
index 97af32e1fa4..98f2b2195af 100644
--- a/drivers/staging/vt6656/iowpa.h
+++ b/drivers/staging/vt6656/iowpa.h
@@ -31,43 +31,6 @@
 
 #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;
-*/
-
-enum {
-	VIAWGET_SET_WPA = 1,
-	VIAWGET_SET_KEY = 2,
-	VIAWGET_SET_SCAN = 3,
-	VIAWGET_GET_SCAN = 4,
-	VIAWGET_GET_SSID = 5,
-	VIAWGET_GET_BSSID = 6,
-	VIAWGET_SET_DROP_UNENCRYPT = 7,
-	VIAWGET_SET_DEAUTHENTICATE = 8,
-	VIAWGET_SET_ASSOCIATE = 9,
-	VIAWGET_SET_DISASSOCIATE = 10
-};
-
-enum {
-	VIAWGET_ASSOC_MSG = 1,
-	VIAWGET_DISASSOC_MSG = 2,
-	VIAWGET_PTK_MIC_MSG = 3,
-	VIAWGET_GTK_MIC_MSG = 4,
-	VIAWGET_CCKM_ROAM_MSG = 5,
-	VIAWGET_DEVICECLOSE_MSG = 6
-};
-
-typedef struct viawget_wpa_header {
-	u8 type;
-	u16 req_ie_len;
-	u16 resp_ie_len;
-} __packed viawget_wpa_header;
-
 struct viawget_wpa_param {
 	u32 cmd;
 	u8 addr[6];
@@ -109,20 +72,4 @@ struct viawget_wpa_param {
 	} u;
 } __packed;
 
-struct viawget_scan_result {
-	u8 bssid[6];
-	u8 ssid[32];
-	u16 ssid_len;
-	u8 wpa_ie[WPA_IE_LEN];
-	u16 wpa_ie_len;
-	u8 rsn_ie[WPA_IE_LEN];
-	u16 rsn_ie_len;
-	int freq; // MHz
-	int caps; // e.g. privacy
-	int qual; // signal quality
-	int noise;
-	int level;
-	int maxrate;
-} __packed;
-
 #endif /* __IOWPA_H__ */
diff --git a/drivers/staging/vt6656/iwctl.c b/drivers/staging/vt6656/iwctl.c
index cf4c06a4288..c43718d788c 100644
--- a/drivers/staging/vt6656/iwctl.c
+++ b/drivers/staging/vt6656/iwctl.c
@@ -34,13 +34,11 @@
 #include "iwctl.h"
 #include "mac.h"
 #include "card.h"
-#include "hostap.h"
 #include "power.h"
 #include "rf.h"
 #include "iowpa.h"
 #include "wpactl.h"
-#include "control.h"
-#include "rndis.h"
+#include "usbpipe.h"
 #include "baseband.h"
 
 static const long frequency_list[] = {
@@ -59,7 +57,7 @@ struct iw_statistics *iwctl_get_wireless_stats(struct net_device *dev)
 	long ldBm;
 
 	pDevice->wstats.status = pDevice->op_mode;
-	RFvRSSITodBm(pDevice, (u8)(pDevice->uCurrRSSI), &ldBm);
+	vnt_rf_rssi_to_dbm(pDevice, (u8)(pDevice->uCurrRSSI), &ldBm);
 	pDevice->wstats.qual.level = ldBm;
 	pDevice->wstats.qual.noise = 0;
 	pDevice->wstats.qual.updated = 1;
@@ -93,6 +91,7 @@ int iwctl_siwscan(struct net_device *dev, struct iw_request_info *info,
 	struct iw_scan_req *req = (struct iw_scan_req *)extra;
 	u8 abyScanSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
 	PWLAN_IE_SSID pItemSSID = NULL;
+	unsigned long flags;
 
 	if (!(pDevice->flags & DEVICE_FLAGS_OPENED))
 		return -EINVAL;
@@ -117,7 +116,7 @@ int iwctl_siwscan(struct net_device *dev, struct iw_request_info *info,
 		return 0;
 	}
 
-	spin_lock_irq(&pDevice->lock);
+	spin_lock_irqsave(&pDevice->lock, flags);
 
 	BSSvClearBSSList((void *)pDevice, pDevice->bLinkPass);
 
@@ -138,7 +137,8 @@ int iwctl_siwscan(struct net_device *dev, struct iw_request_info *info,
 			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);
+
+			spin_unlock_irqrestore(&pDevice->lock, flags);
 
 			return 0;
 		} else if (req->scan_type == IW_SCAN_TYPE_PASSIVE) { // passive scan
@@ -150,7 +150,8 @@ int iwctl_siwscan(struct net_device *dev, struct iw_request_info *info,
 
 	pMgmt->eScanType = WMAC_SCAN_PASSIVE;
 	bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, NULL);
-	spin_unlock_irq(&pDevice->lock);
+
+	spin_unlock_irqrestore(&pDevice->lock, flags);
 
 	return 0;
 }
@@ -234,7 +235,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, (u8)(pBSS->uRSSI), &ldBm);
+			vnt_rf_rssi_to_dbm(pDevice, (u8)(pBSS->uRSSI), &ldBm);
 			iwe.u.qual.level = ldBm;
 			iwe.u.qual.noise = 0;
 
@@ -377,6 +378,7 @@ int iwctl_siwmode(struct net_device *dev, struct iw_request_info *info,
 	struct vnt_private *pDevice = netdev_priv(dev);
 	__u32 *wmode = &wrqu->mode;
 	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
+	unsigned long flags;
 	int rc = 0;
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWMODE\n");
@@ -384,12 +386,6 @@ int iwctl_siwmode(struct net_device *dev, struct iw_request_info *info,
 	if (pMgmt == NULL)
 		return -EFAULT;
 
-	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) {
@@ -409,19 +405,9 @@ int iwctl_siwmode(struct net_device *dev, struct iw_request_info *info,
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set mode to infrastructure\n");
 		break;
 	case IW_MODE_MASTER:
-
-		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;
-		}
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set mode to Access Point\n");
-		break;
-
 	case IW_MODE_REPEAT:
 		pMgmt->eConfigMode = WMAC_CONFIG_ESS_STA;
 		rc = -EOPNOTSUPP;
@@ -433,16 +419,17 @@ int iwctl_siwmode(struct net_device *dev, struct iw_request_info *info,
 	if (pDevice->bCommit) {
 		if (pMgmt->eConfigMode == WMAC_CONFIG_AP) {
 			netif_stop_queue(pDevice->dev);
-			spin_lock_irq(&pDevice->lock);
+
+			spin_lock_irqsave(&pDevice->lock, flags);
+
 			bScheduleCommand((void *) pDevice,
 				WLAN_CMD_RUN_AP, NULL);
-			spin_unlock_irq(&pDevice->lock);
+
+			spin_unlock_irqrestore(&pDevice->lock, flags);
 		} else {
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
 				"Commit the settings\n");
 
-			spin_lock_irq(&pDevice->lock);
-
 			if (pDevice->bLinkPass &&
 				memcmp(pMgmt->abyCurrSSID,
 					pMgmt->abyDesireSSID,
@@ -455,9 +442,7 @@ int iwctl_siwmode(struct net_device *dev, struct iw_request_info *info,
 				memset(pMgmt->abyCurrBSSID, 0, 6);
 			}
 
-			ControlvMaskByte(pDevice,
-				MESSAGE_REQUEST_MACREG,	MAC_REG_PAPEDELAY,
-					LEDSTS_STS, LEDSTS_SLOW);
+			vnt_mac_set_led(pDevice, LEDSTS_STS, LEDSTS_SLOW);
 
 			netif_stop_queue(pDevice->dev);
 
@@ -471,8 +456,6 @@ int iwctl_siwmode(struct net_device *dev, struct iw_request_info *info,
 			bScheduleCommand((void *) pDevice,
 				 WLAN_CMD_SSID,
 				 NULL);
-
-			spin_unlock_irq(&pDevice->lock);
 		}
 		pDevice->bCommit = false;
 	}
@@ -1172,7 +1155,7 @@ int iwctl_siwencode(struct net_device *dev, struct iw_request_info *info,
 	struct iw_point *wrq = &wrqu->encoding;
 	u32 dwKeyIndex = (u32)(wrq->flags & IW_ENCODE_INDEX);
 	int ii;
-	int uu;
+	u8 uu;
 	int rc = 0;
 	int index = (wrq->flags & IW_ENCODE_INDEX);
 
@@ -1212,14 +1195,12 @@ int iwctl_siwencode(struct net_device *dev, struct iw_request_info *info,
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", pDevice->abyKey[ii]);
 
 		if (pDevice->flags & DEVICE_FLAGS_OPENED) {
-			spin_lock_irq(&pDevice->lock);
 			KeybSetDefaultKey(pDevice,
 					&(pDevice->sKey),
 					dwKeyIndex | (1 << 31),
 					wrq->length, NULL,
 					pDevice->abyKey,
 					KEY_CTL_WEP);
-			spin_unlock_irq(&pDevice->lock);
 		}
 		pDevice->byKeyIndex = (u8)dwKeyIndex;
 		pDevice->uKeyLength = wrq->length;
@@ -1242,10 +1223,8 @@ int iwctl_siwencode(struct net_device *dev, struct iw_request_info *info,
 		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, uu);
-			spin_unlock_irq(&pDevice->lock);
 		}
 	}
 	if (wrq->flags & IW_ENCODE_RESTRICTED) {
@@ -1342,12 +1321,9 @@ int iwctl_siwpower(struct net_device *dev, struct iw_request_info *info,
 		return rc;
 	}
 
-	spin_lock_irq(&pDevice->lock);
-
 	if (wrq->disabled) {
 		pDevice->ePSMode = WMAC_POWER_CAM;
 		PSvDisablePowerSaving(pDevice);
-		spin_unlock_irq(&pDevice->lock);
 		return rc;
 	}
 	if ((wrq->flags & IW_POWER_TYPE) == IW_POWER_TIMEOUT) {
@@ -1359,8 +1335,6 @@ int iwctl_siwpower(struct net_device *dev, struct iw_request_info *info,
 		PSvEnablePowerSaving((void *)pDevice, pMgmt->wListenInterval);
 	}
 
-	spin_unlock_irq(&pDevice->lock);
-
 	switch (wrq->flags & IW_POWER_MODE) {
 	case IW_POWER_UNICAST_R:
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWPOWER: IW_POWER_UNICAST_R\n");
@@ -1425,7 +1399,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, (u8)(pDevice->uCurrRSSI), &ldBm);
+		vnt_rf_rssi_to_dbm(pDevice, (u8)(pDevice->uCurrRSSI), &ldBm);
 		wrq->value = ldBm;
 	} else {
 		wrq->value = 0;
@@ -1723,9 +1697,7 @@ int iwctl_siwencodeext(struct net_device *dev, struct iw_request_info *info,
 		KeyvInitTable(pDevice, &pDevice->sKey);
 	}
 /*******/
-	spin_lock_irq(&pDevice->lock);
 	ret = wpa_set_keys(pDevice, param);
-	spin_unlock_irq(&pDevice->lock);
 
 error:
 	kfree(buf);
diff --git a/drivers/staging/vt6656/key.c b/drivers/staging/vt6656/key.c
index be92c048a12..38ea67531d9 100644
--- a/drivers/staging/vt6656/key.c
+++ b/drivers/staging/vt6656/key.c
@@ -39,8 +39,7 @@
 #include "mac.h"
 #include "tmacro.h"
 #include "key.h"
-#include "rndis.h"
-#include "control.h"
+#include "usbpipe.h"
 
 static int          msglevel                =MSG_LEVEL_INFO;
 //static int          msglevel                =MSG_LEVEL_DEBUG;
@@ -68,15 +67,10 @@ static void s_vCheckKeyTableValid(struct vnt_private *pDevice,
             //MACvDisableKeyEntry(pDevice, i);
         }
     }
-    if ( wLength != 0 ) {
-        CONTROLnsRequestOut(pDevice,
-                            MESSAGE_TYPE_CLRKEYENTRY,
-                            0,
-                            0,
-                            wLength,
-                            pbyData
-                            );
-    }
+
+	if (wLength != 0)
+		vnt_control_out(pDevice, MESSAGE_TYPE_CLRKEYENTRY,
+			0, 0, wLength, pbyData);
 
 }
 
@@ -97,7 +91,6 @@ void KeyvInitTable(struct vnt_private *pDevice, PSKeyManagement pTable)
 	int i, jj;
 	u8 pbyData[MAX_KEY_TABLE+1];
 
-    spin_lock_irq(&pDevice->lock);
     for (i=0;i<MAX_KEY_TABLE;i++) {
         pTable->KeyTable[i].bInUse = false;
         pTable->KeyTable[i].PairwiseKey.bKeyValid = false;
@@ -114,15 +107,9 @@ void KeyvInitTable(struct vnt_private *pDevice, PSKeyManagement pTable)
         pbyData[i] = (u8) i;
     }
     pbyData[i] = (u8) i;
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_CLRKEYENTRY,
-                        0,
-                        0,
-                        11,
-                        pbyData
-                        );
 
-    spin_unlock_irq(&pDevice->lock);
+	vnt_control_out(pDevice, MESSAGE_TYPE_CLRKEYENTRY,
+			0, 0, 11, pbyData);
 
     return;
 }
@@ -251,7 +238,9 @@ 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, (u32 *)pKey->abyKey);
+
+	    MACvSetKeyEntry(pDevice, pTable->KeyTable[i].wKeyCtl, i, uKeyIdx,
+			pbyBSSID, pKey->abyKey);
 
 		if ((dwKeyIndex & USE_KEYRSC) == 0)
 			pKey->KeyRSC = 0; /* RSC set by NIC */
@@ -319,7 +308,9 @@ 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, (u32 *)pKey->abyKey);
+
+	MACvSetKeyEntry(pDevice, pTable->KeyTable[j].wKeyCtl, j, uKeyIdx,
+					pbyBSSID, pKey->abyKey);
 
 		if ((dwKeyIndex & USE_KEYRSC) == 0)
 			pKey->KeyRSC = 0; /* RSC set by NIC */
@@ -614,7 +605,9 @@ 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, (u32 *) pKey->abyKey);
+	MACvSetKeyEntry(pDevice, pTable->KeyTable[MAX_KEY_TABLE-1].wKeyCtl,
+		MAX_KEY_TABLE-1, uKeyIdx,
+		pTable->KeyTable[MAX_KEY_TABLE-1].abyBSSID, pKey->abyKey);
 
 		if ((dwKeyIndex & USE_KEYRSC) == 0)
 			pKey->KeyRSC = 0; /* RSC set by NIC */
@@ -708,7 +701,8 @@ int KeybSetAllGroupKey(struct vnt_private *pDevice, PSKeyManagement pTable,
                     pKey->abyKey[15] |= 0x80;
             }
 
-            MACvSetKeyEntry(pDevice, pTable->KeyTable[i].wKeyCtl, i, uKeyIdx, pTable->KeyTable[i].abyBSSID, (u32 *) pKey->abyKey);
+	    MACvSetKeyEntry(pDevice, pTable->KeyTable[i].wKeyCtl, i, uKeyIdx,
+			pTable->KeyTable[i].abyBSSID, pKey->abyKey);
 
 		if ((dwKeyIndex & USE_KEYRSC) == 0)
 			pKey->KeyRSC = 0; /* RSC set by NIC */
diff --git a/drivers/staging/vt6656/mac.c b/drivers/staging/vt6656/mac.c
index 3ce19ddbc56..cadf7cd280c 100644
--- a/drivers/staging/vt6656/mac.c
+++ b/drivers/staging/vt6656/mac.c
@@ -35,11 +35,7 @@
 #include "desc.h"
 #include "mac.h"
 #include "80211hdr.h"
-#include "rndis.h"
-#include "control.h"
-
-//static int          msglevel                =MSG_LEVEL_DEBUG;
-static int          msglevel                =MSG_LEVEL_INFO;
+#include "usbpipe.h"
 
 /*
  * Description:
@@ -54,11 +50,11 @@ static int          msglevel                =MSG_LEVEL_INFO;
  * Return Value: none
  *
  */
-void MACvWriteMultiAddr(struct vnt_private *pDevice, u64 mc_filter)
+void MACvWriteMultiAddr(struct vnt_private *priv, u64 mc_filter)
 {
 	__le64 le_mc = cpu_to_le64(mc_filter);
 
-	CONTROLnsRequestOut(pDevice, MESSAGE_TYPE_WRITE, MAC_REG_MAR0,
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE, MAC_REG_MAR0,
 		MESSAGE_REQUEST_MACREG, sizeof(le_mc), (u8 *)&le_mc);
 }
 
@@ -73,31 +69,20 @@ void MACvWriteMultiAddr(struct vnt_private *pDevice, u64 mc_filter)
  *
  *
  */
-void MACbShutdown(struct vnt_private *pDevice)
+void MACbShutdown(struct vnt_private *priv)
 {
-    CONTROLnsRequestOutAsyn(pDevice,
-                        MESSAGE_TYPE_MACSHUTDOWN,
-                        0,
-                        0,
-                        0,
-                        NULL
-                        );
+	vnt_control_out(priv, MESSAGE_TYPE_MACSHUTDOWN, 0, 0, 0, NULL);
 }
 
-void MACvSetBBType(struct vnt_private *pDevice, u8 byType)
+void MACvSetBBType(struct vnt_private *priv, u8 type)
 {
-	u8 pbyData[2];
-
-    pbyData[0] = byType;
-    pbyData[1] = EnCFG_BBType_MASK;
-
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_WRITE_MASK,
-                        MAC_REG_ENCFG0,
-                        MESSAGE_REQUEST_MACREG,
-			ARRAY_SIZE(pbyData),
-                        pbyData
-                        );
+	u8 data[2];
+
+	data[0] = type;
+	data[1] = EnCFG_BBType_MASK;
+
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK, MAC_REG_ENCFG0,
+		MESSAGE_REQUEST_MACREG,	ARRAY_SIZE(data), data);
 }
 
 /*
@@ -114,20 +99,10 @@ void MACvSetBBType(struct vnt_private *pDevice, u8 byType)
  * Return Value: none
  *
  */
-void MACvDisableKeyEntry(struct vnt_private *pDevice, u32 uEntryIdx)
+void MACvDisableKeyEntry(struct vnt_private *priv, u8 entry_idx)
 {
-	u8 byData;
-
-    byData = (u8) uEntryIdx;
-
-    //issue write misc fifo command to device
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_CLRKEYENTRY,
-                        0,
-                        0,
-                        1,
-                        &byData
-                        );
+	vnt_control_out(priv, MESSAGE_TYPE_CLRKEYENTRY, 0, 0,
+		sizeof(entry_idx), &entry_idx);
 }
 
 /*
@@ -144,210 +119,137 @@ void MACvDisableKeyEntry(struct vnt_private *pDevice, u32 uEntryIdx)
  * Return Value: none
  *
  */
-void MACvSetKeyEntry(struct vnt_private *pDevice, u16 wKeyCtl, u32 uEntryIdx,
-	u32 uKeyIdx, u8 *pbyAddr, u32 *pdwKey)
+void MACvSetKeyEntry(struct vnt_private *priv, u16 key_ctl, u32 entry_idx,
+	u32 key_idx, u8 *addr, u8 *key)
 {
-	u8 *pbyKey;
-	u16 wOffset;
-	u32 dwData1, dwData2;
-	int ii;
-	u8 pbyData[24];
-
-	if (pDevice->byLocalID <= MAC_REVISION_A1)
-		if (pDevice->vnt_mgmt.byCSSPK == KEY_CTL_CCMP)
+	struct vnt_mac_set_key set_key;
+	u16 offset;
+
+	if (priv->byLocalID <= MAC_REVISION_A1)
+		if (priv->vnt_mgmt.byCSSPK == KEY_CTL_CCMP)
 			return;
 
-    wOffset = MISCFIFO_KEYETRY0;
-    wOffset += (uEntryIdx * MISCFIFO_KEYENTRYSIZE);
-
-    dwData1 = 0;
-    dwData1 |= wKeyCtl;
-    dwData1 <<= 16;
-    dwData1 |= MAKEWORD(*(pbyAddr+4), *(pbyAddr+5));
-
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"1. wOffset: %d, Data: %X,"\
-		" KeyCtl:%X\n", wOffset, dwData1, wKeyCtl);
-
-    dwData2 = 0;
-    dwData2 |= *(pbyAddr+3);
-    dwData2 <<= 8;
-    dwData2 |= *(pbyAddr+2);
-    dwData2 <<= 8;
-    dwData2 |= *(pbyAddr+1);
-    dwData2 <<= 8;
-    dwData2 |= *(pbyAddr+0);
-
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"2. wOffset: %d, Data: %X\n",
-		wOffset, dwData2);
-
-    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,
-                        (u16)uKeyIdx,
-			ARRAY_SIZE(pbyData),
-                        pbyData
-                        );
+	offset = MISCFIFO_KEYETRY0;
+	offset += (entry_idx * MISCFIFO_KEYENTRYSIZE);
+
+	set_key.u.write.key_ctl = cpu_to_le16(key_ctl);
+	memcpy(set_key.u.write.addr, addr, ETH_ALEN);
+
+	/* swap over swap[0] and swap[1] to get correct write order */
+	swap(set_key.u.swap[0], set_key.u.swap[1]);
+
+	memcpy(set_key.key, key, WLAN_KEY_LEN_CCMP);
 
+	dev_dbg(&priv->usb->dev, "offset %d key ctl %d set key %24ph\n",
+				offset, key_ctl, (u8 *)&set_key);
+
+	vnt_control_out(priv, MESSAGE_TYPE_SETKEY, offset,
+		(u16)key_idx, sizeof(struct vnt_mac_set_key), (u8 *)&set_key);
 }
 
-void MACvRegBitsOff(struct vnt_private *pDevice, u8 byRegOfs, u8 byBits)
+void MACvRegBitsOff(struct vnt_private *priv, u8 reg_ofs, u8 bits)
 {
-	u8 pbyData[2];
-
-    pbyData[0] = 0;
-    pbyData[1] = byBits;
-
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_WRITE_MASK,
-                        byRegOfs,
-                        MESSAGE_REQUEST_MACREG,
-			ARRAY_SIZE(pbyData),
-                        pbyData
-                        );
+	u8 data[2];
+
+	data[0] = 0;
+	data[1] = bits;
+
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK,
+		reg_ofs, MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
 }
 
-void MACvRegBitsOn(struct vnt_private *pDevice, u8 byRegOfs, u8 byBits)
+void MACvRegBitsOn(struct vnt_private *priv, u8 reg_ofs, u8 bits)
 {
-	u8 pbyData[2];
-
-    pbyData[0] = byBits;
-    pbyData[1] = byBits;
-
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_WRITE_MASK,
-                        byRegOfs,
-                        MESSAGE_REQUEST_MACREG,
-			ARRAY_SIZE(pbyData),
-                        pbyData
-                        );
+	u8 data[2];
+
+	data[0] = bits;
+	data[1] = bits;
+
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK,
+		reg_ofs, MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
 }
 
-void MACvWriteWord(struct vnt_private *pDevice, u8 byRegOfs, u16 wData)
+void MACvWriteWord(struct vnt_private *priv, u8 reg_ofs, u16 word)
 {
-	u8 pbyData[2];
+	u8 data[2];
 
-    pbyData[0] = (u8)(wData & 0xff);
-    pbyData[1] = (u8)(wData >> 8);
+	data[0] = (u8)(word & 0xff);
+	data[1] = (u8)(word >> 8);
 
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_WRITE,
-                        byRegOfs,
-                        MESSAGE_REQUEST_MACREG,
-			ARRAY_SIZE(pbyData),
-                        pbyData
-                        );
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE,
+		reg_ofs, MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
+}
 
+void MACvWriteBSSIDAddress(struct vnt_private *priv, u8 *addr)
+{
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE, MAC_REG_BSSID0,
+		MESSAGE_REQUEST_MACREG, ETH_ALEN, addr);
 }
 
-void MACvWriteBSSIDAddress(struct vnt_private *pDevice, u8 *pbyEtherAddr)
+void MACvEnableProtectMD(struct vnt_private *priv)
 {
-	u8 pbyData[6];
-
-    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,
-                        MAC_REG_BSSID0,
-                        MESSAGE_REQUEST_MACREG,
-			ARRAY_SIZE(pbyData),
-                        pbyData
-                        );
+	u8 data[2];
+
+	data[0] = EnCFG_ProtectMd;
+	data[1] = EnCFG_ProtectMd;
+
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK,
+		MAC_REG_ENCFG0, MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
 }
 
-void MACvEnableProtectMD(struct vnt_private *pDevice)
+void MACvDisableProtectMD(struct vnt_private *priv)
 {
-	u8 pbyData[2];
-
-    pbyData[0] = EnCFG_ProtectMd;
-    pbyData[1] = EnCFG_ProtectMd;
-
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_WRITE_MASK,
-                        MAC_REG_ENCFG0,
-                        MESSAGE_REQUEST_MACREG,
-			ARRAY_SIZE(pbyData),
-                        pbyData
-                        );
+	u8 data[2];
+
+	data[0] = 0;
+	data[1] = EnCFG_ProtectMd;
+
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK,
+		MAC_REG_ENCFG0, MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
 }
 
-void MACvDisableProtectMD(struct vnt_private *pDevice)
+void MACvEnableBarkerPreambleMd(struct vnt_private *priv)
 {
-	u8 pbyData[2];
-
-    pbyData[0] = 0;
-    pbyData[1] = EnCFG_ProtectMd;
-
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_WRITE_MASK,
-                        MAC_REG_ENCFG0,
-                        MESSAGE_REQUEST_MACREG,
-			ARRAY_SIZE(pbyData),
-                        pbyData
-                        );
+	u8 data[2];
+
+	data[0] = EnCFG_BarkerPream;
+	data[1] = EnCFG_BarkerPream;
+
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK,
+		MAC_REG_ENCFG2, MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
 }
 
-void MACvEnableBarkerPreambleMd(struct vnt_private *pDevice)
+void MACvDisableBarkerPreambleMd(struct vnt_private *priv)
 {
-	u8 pbyData[2];
-
-    pbyData[0] = EnCFG_BarkerPream;
-    pbyData[1] = EnCFG_BarkerPream;
-
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_WRITE_MASK,
-                        MAC_REG_ENCFG2,
-                        MESSAGE_REQUEST_MACREG,
-			ARRAY_SIZE(pbyData),
-                        pbyData
-                        );
+	u8 data[2];
+
+	data[0] = 0;
+	data[1] = EnCFG_BarkerPream;
+
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK,
+		MAC_REG_ENCFG2, MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
 }
 
-void MACvDisableBarkerPreambleMd(struct vnt_private *pDevice)
+void MACvWriteBeaconInterval(struct vnt_private *priv, u16 interval)
 {
-	u8 pbyData[2];
-
-    pbyData[0] = 0;
-    pbyData[1] = EnCFG_BarkerPream;
-
-    CONTROLnsRequestOut(pDevice,
-                        MESSAGE_TYPE_WRITE_MASK,
-                        MAC_REG_ENCFG2,
-                        MESSAGE_REQUEST_MACREG,
-			ARRAY_SIZE(pbyData),
-                        pbyData
-                        );
+	u8 data[2];
+
+	data[0] = (u8)(interval & 0xff);
+	data[1] = (u8)(interval >> 8);
+
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE,
+		MAC_REG_BI, MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
 }
 
-void MACvWriteBeaconInterval(struct vnt_private *pDevice, u16 wInterval)
+void vnt_mac_set_led(struct vnt_private *priv, u8 state, u8 led)
 {
-	u8 pbyData[2];
-
-	pbyData[0] = (u8)(wInterval & 0xff);
-	pbyData[1] = (u8)(wInterval >> 8);
-
-    CONTROLnsRequestOut(pDevice,
-			MESSAGE_TYPE_WRITE,
-			MAC_REG_BI,
-			MESSAGE_REQUEST_MACREG,
-			ARRAY_SIZE(pbyData),
-			pbyData
-			);
+	u8 data[2];
+
+	data[0] = led;
+	data[1] = state;
+
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK, MAC_REG_PAPEDELAY,
+			MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
+
+	return;
 }
diff --git a/drivers/staging/vt6656/mac.h b/drivers/staging/vt6656/mac.h
index 4053e431ef9..986ca95f720 100644
--- a/drivers/staging/vt6656/mac.h
+++ b/drivers/staging/vt6656/mac.h
@@ -403,11 +403,22 @@
 #define MAC_REVISION_A0     0x00
 #define MAC_REVISION_A1     0x01
 
+struct vnt_mac_set_key {
+	union {
+		struct {
+			u8 addr[ETH_ALEN];
+			__le16 key_ctl;
+		} write __packed;
+		u32 swap[2];
+	} u;
+	u8 key[WLAN_KEY_LEN_CCMP];
+} __packed;
+
 void MACvWriteMultiAddr(struct vnt_private *, u64);
 void MACbShutdown(struct vnt_private *);
 void MACvSetBBType(struct vnt_private *, u8);
-void MACvDisableKeyEntry(struct vnt_private *, u32);
-void MACvSetKeyEntry(struct vnt_private *, u16, u32, u32, u8 *, u32 *);
+void MACvDisableKeyEntry(struct vnt_private *, u8);
+void MACvSetKeyEntry(struct vnt_private *, u16, u32, u32, u8 *, u8 *);
 void MACvRegBitsOff(struct vnt_private *, u8, u8);
 void MACvRegBitsOn(struct vnt_private *, u8, u8);
 void MACvWriteWord(struct vnt_private *, u8, u16);
@@ -417,5 +428,6 @@ void MACvDisableProtectMD(struct vnt_private *);
 void MACvEnableBarkerPreambleMd(struct vnt_private *);
 void MACvDisableBarkerPreambleMd(struct vnt_private *);
 void MACvWriteBeaconInterval(struct vnt_private *, u16);
+void vnt_mac_set_led(struct vnt_private *priv, u8, u8);
 
 #endif /* __MAC_H__ */
diff --git a/drivers/staging/vt6656/main_usb.c b/drivers/staging/vt6656/main_usb.c
index 3c9323069e0..e18071f121c 100644
--- a/drivers/staging/vt6656/main_usb.c
+++ b/drivers/staging/vt6656/main_usb.c
@@ -57,17 +57,15 @@
 #include "power.h"
 #include "wcmd.h"
 #include "iocmd.h"
-#include "tcrc.h"
 #include "rxtx.h"
 #include "bssdb.h"
-#include "hostap.h"
 #include "wpactl.h"
 #include "iwctl.h"
 #include "dpc.h"
 #include "datarate.h"
 #include "rf.h"
 #include "firmware.h"
-#include "control.h"
+#include "usbpipe.h"
 #include "channel.h"
 #include "int.h"
 #include "iowpa.h"
@@ -216,8 +214,6 @@ static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd);
 
 static int device_init_registers(struct vnt_private *pDevice);
 static bool device_init_defrag_cb(struct vnt_private *pDevice);
-static void device_init_diversity_timer(struct vnt_private *pDevice);
-static int  device_dma0_tx_80211(struct sk_buff *skb, struct net_device *dev);
 
 static int  ethtool_ioctl(struct net_device *dev, struct ifreq *);
 static void device_free_tx_bufs(struct vnt_private *pDevice);
@@ -262,33 +258,8 @@ device_set_options(struct vnt_private *pDevice) {
     pDevice->byBBType = BBP_TYPE_DEF;
     pDevice->byPacketType = pDevice->byBBType;
     pDevice->byAutoFBCtrl = AUTO_FB_0;
-    pDevice->bUpdateBBVGA = true;
-    pDevice->byFOETuning = 0;
-    pDevice->byAutoPwrTunning = 0;
     pDevice->byPreambleType = 0;
     pDevice->bExistSWNetAddr = false;
-    /* pDevice->bDiversityRegCtlON = true; */
-    pDevice->bDiversityRegCtlON = false;
-}
-
-static void device_init_diversity_timer(struct vnt_private *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)TimerSQ3Tmax3CallBack;
-    pDevice->TimerSQ3Tmax3.expires = RUN_AT(HZ);
-
-    return;
 }
 
 /*
@@ -312,8 +283,6 @@ static int device_init_registers(struct vnt_private *pDevice)
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "---->INIbInitAdapter. [%d][%d]\n",
 				DEVICE_INIT_COLD, pDevice->byPacketType);
 
-	spin_lock_irq(&pDevice->lock);
-
 	memcpy(pDevice->abyBroadcastAddr, abyBroadcastAddr, ETH_ALEN);
 	memcpy(pDevice->abySNAP_RFC1042, abySNAP_RFC1042, ETH_ALEN);
 	memcpy(pDevice->abySNAP_Bridgetunnel, abySNAP_Bridgetunnel, ETH_ALEN);
@@ -323,20 +292,17 @@ static int device_init_registers(struct vnt_private *pDevice)
 			if (FIRMWAREbBrach2Sram(pDevice) == false) {
 				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
 					" FIRMWAREbBrach2Sram fail\n");
-				spin_unlock_irq(&pDevice->lock);
 				return false;
 			}
 		} else {
 			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
 				" FIRMWAREbDownload fail\n");
-			spin_unlock_irq(&pDevice->lock);
 			return false;
 		}
 	}
 
 	if (!BBbVT3184Init(pDevice)) {
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" BBbVT3184Init fail\n");
-		spin_unlock_irq(&pDevice->lock);
 		return false;
 	}
 
@@ -348,37 +314,31 @@ static int device_init_registers(struct vnt_private *pDevice)
 	init_cmd->long_retry_limit = pDevice->byLongRetryLimit;
 
 	/* issue card_init command to device */
-	ntStatus = CONTROLnsRequestOut(pDevice,
+	ntStatus = vnt_control_out(pDevice,
 		MESSAGE_TYPE_CARDINIT, 0, 0,
 		sizeof(struct vnt_cmd_card_init), (u8 *)init_cmd);
 	if (ntStatus != STATUS_SUCCESS) {
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" Issue Card init fail\n");
-		spin_unlock_irq(&pDevice->lock);
 		return false;
 	}
 
-	ntStatus = CONTROLnsRequestIn(pDevice, MESSAGE_TYPE_INIT_RSP, 0, 0,
+	ntStatus = vnt_control_in(pDevice, MESSAGE_TYPE_INIT_RSP, 0, 0,
 		sizeof(struct vnt_rsp_card_init), (u8 *)init_rsp);
 	if (ntStatus != STATUS_SUCCESS) {
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
 			"Cardinit request in status fail!\n");
-		spin_unlock_irq(&pDevice->lock);
 		return false;
 	}
 
 	/* local ID for AES functions */
-	ntStatus = CONTROLnsRequestIn(pDevice, MESSAGE_TYPE_READ,
+	ntStatus = vnt_control_in(pDevice, MESSAGE_TYPE_READ,
 		MAC_REG_LOCALID, MESSAGE_REQUEST_MACREG, 1,
 			&pDevice->byLocalID);
-	if (ntStatus != STATUS_SUCCESS) {
-		spin_unlock_irq(&pDevice->lock);
+	if (ntStatus != STATUS_SUCCESS)
 		return false;
-	}
 
 	/* do MACbSoftwareReset in MACvInitialize */
 
-	/* force CCK */
-	pDevice->bCCK = true;
 	pDevice->bProtectMode = false;
 	/* only used in 11g type, sync with ERP IE */
 	pDevice->bNonERPPresent = false;
@@ -396,7 +356,7 @@ static int device_init_registers(struct vnt_private *pDevice)
 
 	pDevice->byTopOFDMBasicRate = RATE_24M;
 	pDevice->byTopCCKBasicRate = RATE_1M;
-	pDevice->byRevId = 0;
+
 	/* target to IF pin while programming to RF chip */
 	pDevice->byCurPwr = 0xFF;
 
@@ -461,13 +421,7 @@ static int device_init_registers(struct vnt_private *pDevice)
 			pDevice->byRxAntennaMode = ANT_A;
 		else
 			pDevice->byRxAntennaMode = ANT_B;
-
-		if (pDevice->bDiversityRegCtlON)
-			pDevice->bDiversityEnable = true;
-		else
-			pDevice->bDiversityEnable = false;
 	} else  {
-		pDevice->bDiversityEnable = false;
 		pDevice->byAntennaCount = 1;
 		pDevice->dwTxAntennaSel = 0;
 		pDevice->dwRxAntennaSel = 0;
@@ -489,27 +443,14 @@ static int device_init_registers(struct vnt_private *pDevice)
 		}
 	}
 
-	pDevice->ulDiversityNValue = 100 * 255;
-	pDevice->ulDiversityMValue = 100 * 16;
-	pDevice->byTMax = 1;
-	pDevice->byTMax2 = 4;
-	pDevice->ulSQ3TH = 0;
-	pDevice->byTMax3 = 64;
-
 	/* get Auto Fall Back type */
 	pDevice->byAutoFBCtrl = AUTO_FB_0;
 
-	/* set SCAN Time */
-	pDevice->uScanTime = WLAN_SCAN_MINITIME;
-
 	/* default Auto Mode */
 	/* pDevice->NetworkType = Ndis802_11Automode; */
 	pDevice->eConfigPHYMode = PHY_TYPE_AUTO;
 	pDevice->byBBType = BB_TYPE_11G;
 
-	/* initialize BBP registers */
-	pDevice->ulTxPower = 25;
-
 	/* get channel range */
 	pDevice->byMinChannel = 1;
 	pDevice->byMaxChannel = CB_MAX_CHANNEL;
@@ -517,11 +458,6 @@ static int device_init_registers(struct vnt_private *pDevice)
 	/* get RFType */
 	pDevice->byRFType = init_rsp->rf_type;
 
-	if ((pDevice->byRFType & RF_EMU) != 0) {
-		/* force change RevID for VT3253 emu */
-		pDevice->byRevId = 0x80;
-	}
-
 	/* load vt3266 calibration parameters in EEPROM */
 	if (pDevice->byRFType == RF_VT3226D0) {
 		if ((pDevice->abyEEPROM[EEP_OFS_MAJOR_VER] == 0x1) &&
@@ -532,28 +468,28 @@ static int device_init_registers(struct vnt_private *pDevice)
 			byCalibRXIQ = pDevice->abyEEPROM[EEP_OFS_CALIB_RX_IQ];
 			if (byCalibTXIQ || byCalibTXDC || byCalibRXIQ) {
 			/* CR255, enable TX/RX IQ and DC compensation mode */
-				ControlvWriteByte(pDevice,
+				vnt_control_out_u8(pDevice,
 					MESSAGE_REQUEST_BBREG,
 					0xff,
 					0x03);
 			/* CR251, TX I/Q Imbalance Calibration */
-				ControlvWriteByte(pDevice,
+				vnt_control_out_u8(pDevice,
 					MESSAGE_REQUEST_BBREG,
 					0xfb,
 					byCalibTXIQ);
 			/* CR252, TX DC-Offset Calibration */
-				ControlvWriteByte(pDevice,
+				vnt_control_out_u8(pDevice,
 					MESSAGE_REQUEST_BBREG,
 					0xfC,
 					byCalibTXDC);
 			/* CR253, RX I/Q Imbalance Calibration */
-				ControlvWriteByte(pDevice,
+				vnt_control_out_u8(pDevice,
 					MESSAGE_REQUEST_BBREG,
 					0xfd,
 					byCalibRXIQ);
 			} else {
 			/* CR255, turn off BB Calibration compensation */
-				ControlvWriteByte(pDevice,
+				vnt_control_out_u8(pDevice,
 					MESSAGE_REQUEST_BBREG,
 					0xff,
 					0x0);
@@ -590,24 +526,20 @@ static int device_init_registers(struct vnt_private *pDevice)
 	BBvSetShortSlotTime(pDevice);
 	CARDvSetBSSMode(pDevice);
 
-	if (pDevice->bUpdateBBVGA) {
-		pDevice->byBBVGACurrent = pDevice->abyBBVGA[0];
-		pDevice->byBBVGANew = pDevice->byBBVGACurrent;
+	pDevice->byBBVGACurrent = pDevice->abyBBVGA[0];
+	pDevice->byBBVGANew = pDevice->byBBVGACurrent;
 
-		BBvSetVGAGainOffset(pDevice, pDevice->abyBBVGA[0]);
-	}
+	BBvSetVGAGainOffset(pDevice, pDevice->abyBBVGA[0]);
 
 	pDevice->byRadioCtl = pDevice->abyEEPROM[EEP_OFS_RADIOCTL];
 	pDevice->bHWRadioOff = false;
 
 	if ((pDevice->byRadioCtl & EEP_RADIOCTL_ENABLE) != 0) {
-		ntStatus = CONTROLnsRequestIn(pDevice, MESSAGE_TYPE_READ,
+		ntStatus = vnt_control_in(pDevice, MESSAGE_TYPE_READ,
 			MAC_REG_GPIOCTL1, MESSAGE_REQUEST_MACREG, 1, &byTmp);
 
-		if (ntStatus != STATUS_SUCCESS) {
-			spin_unlock_irq(&pDevice->lock);
+		if (ntStatus != STATUS_SUCCESS)
 			return false;
-		}
 
 		if ((byTmp & GPIO3_DATA) == 0) {
 			pDevice->bHWRadioOff = true;
@@ -619,11 +551,9 @@ static int device_init_registers(struct vnt_private *pDevice)
 
 	}
 
-	ControlvMaskByte(pDevice, MESSAGE_REQUEST_MACREG,
-				MAC_REG_PAPEDELAY, LEDSTS_TMLEN, 0x38);
+	vnt_mac_set_led(pDevice, LEDSTS_TMLEN, 0x38);
 
-	ControlvMaskByte(pDevice, MESSAGE_REQUEST_MACREG,
-				MAC_REG_PAPEDELAY, LEDSTS_STS, LEDSTS_SLOW);
+	vnt_mac_set_led(pDevice, LEDSTS_STS, LEDSTS_SLOW);
 
 	MACvRegBitsOn(pDevice, MAC_REG_GPIOCTL0, 0x01);
 
@@ -634,9 +564,6 @@ static int device_init_registers(struct vnt_private *pDevice)
 		CARDbRadioPowerOn(pDevice);
 	}
 
-
-	spin_unlock_irq(&pDevice->lock);
-
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"<----INIbInitAdapter Exit\n");
 
 	return true;
@@ -709,18 +636,13 @@ vt6656_probe(struct usb_interface *intf, const struct usb_device_id *id)
 
 	device_set_options(pDevice);
 	spin_lock_init(&pDevice->lock);
+	mutex_init(&pDevice->usb_lock);
+
 	INIT_DELAYED_WORK(&pDevice->run_command_work, vRunCommand);
 	INIT_DELAYED_WORK(&pDevice->second_callback_work, BSSvSecondCallBack);
 	INIT_WORK(&pDevice->read_work_item, RXvWorkItem);
 	INIT_WORK(&pDevice->rx_mng_work_item, RXvMngWorkItem);
 
-	pDevice->pControlURB = usb_alloc_urb(0, GFP_ATOMIC);
-	if (!pDevice->pControlURB) {
-		DBG_PRT(MSG_LEVEL_ERR, KERN_ERR"Failed to alloc control urb\n");
-		goto err_netdev;
-	}
-
-	pDevice->tx_80211 = device_dma0_tx_80211;
 	pDevice->vnt_mgmt.pAdapter = (void *) pDevice;
 
 	netdev->netdev_ops = &device_netdev_ops;
@@ -757,9 +679,9 @@ static void device_free_tx_bufs(struct vnt_private *priv)
 	for (ii = 0; ii < priv->cbTD; ii++) {
 		tx_context = priv->apTD[ii];
 		/* deallocate URBs */
-		if (tx_context->pUrb) {
-			usb_kill_urb(tx_context->pUrb);
-			usb_free_urb(tx_context->pUrb);
+		if (tx_context->urb) {
+			usb_kill_urb(tx_context->urb);
+			usb_free_urb(tx_context->urb);
 		}
 
 		kfree(tx_context);
@@ -825,17 +747,17 @@ static bool device_alloc_bufs(struct vnt_private *priv)
 		}
 
 		priv->apTD[ii] = tx_context;
-		tx_context->pDevice = priv;
+		tx_context->priv = priv;
 
 		/* allocate URBs */
-		tx_context->pUrb = usb_alloc_urb(0, GFP_ATOMIC);
-		if (tx_context->pUrb == NULL) {
+		tx_context->urb = usb_alloc_urb(0, GFP_ATOMIC);
+		if (!tx_context->urb) {
 			DBG_PRT(MSG_LEVEL_ERR,
 				KERN_ERR "alloc tx urb failed\n");
 			goto free_tx;
 		}
 
-		tx_context->bBoolInUse = false;
+		tx_context->in_use = false;
 	}
 
 	/* allocate RCB mem */
@@ -976,8 +898,6 @@ static int  device_open(struct net_device *dev)
     }
 
     MP_CLEAR_FLAG(pDevice, fMP_DISCONNECTED);
-    MP_CLEAR_FLAG(pDevice, fMP_CONTROL_READS);
-    MP_CLEAR_FLAG(pDevice, fMP_CONTROL_WRITES);
     MP_SET_FLAG(pDevice, fMP_POST_READS);
     MP_SET_FLAG(pDevice, fMP_POST_WRITES);
 
@@ -999,9 +919,6 @@ static int  device_open(struct net_device *dev)
     pDevice->bRoaming = false;
     pDevice->bIsRoaming = false;
     pDevice->bEnableRoaming = false;
-    if (pDevice->bDiversityRegCtlON) {
-        device_init_diversity_timer(pDevice);
-    }
 
     vMgrObjectInit(pDevice);
 
@@ -1025,7 +942,6 @@ static int  device_open(struct net_device *dev)
 
     /* if WEP key already set by iwconfig but device not yet open */
     if ((pDevice->bEncryptionEnable == true) && (pDevice->bTransmitKey == true)) {
-         spin_lock_irq(&pDevice->lock);
          KeybSetDefaultKey( pDevice,
                             &(pDevice->sKey),
                             pDevice->byKeyIndex | (1 << 31),
@@ -1034,7 +950,7 @@ static int  device_open(struct net_device *dev)
                             pDevice->abyKey,
                             KEY_CTL_WEP
                           );
-         spin_unlock_irq(&pDevice->lock);
+
          pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
     }
 
@@ -1066,7 +982,7 @@ static int device_close(struct net_device *dev)
 {
 	struct vnt_private *pDevice = netdev_priv(dev);
 	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
-	int uu;
+	u8 uu;
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "device_close1\n");
     if (pDevice == NULL)
@@ -1081,10 +997,9 @@ static int device_close(struct net_device *dev)
         pMgmt->bShareKeyAlgorithm = false;
         pDevice->bEncryptionEnable = false;
         pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
-	spin_lock_irq(&pDevice->lock);
+
 	for (uu = 0; uu < MAX_KEY_TABLE; uu++)
                 MACvDisableKeyEntry(pDevice,uu);
-	spin_unlock_irq(&pDevice->lock);
 
     if ((pDevice->flags & DEVICE_FLAGS_UNPLUG) == false) {
         MACbShutdown(pDevice);
@@ -1097,12 +1012,6 @@ static int device_close(struct net_device *dev)
 	cancel_delayed_work_sync(&pDevice->run_command_work);
 	cancel_delayed_work_sync(&pDevice->second_callback_work);
 
-    if (pDevice->bDiversityRegCtlON) {
-        del_timer(&pDevice->TimerSQ3Tmax1);
-        del_timer(&pDevice->TimerSQ3Tmax2);
-        del_timer(&pDevice->TimerSQ3Tmax3);
-    }
-
 	cancel_work_sync(&pDevice->rx_mng_work_item);
 	cancel_work_sync(&pDevice->read_work_item);
 
@@ -1145,36 +1054,17 @@ static void vt6656_disconnect(struct usb_interface *intf)
 
 	if (device->dev) {
 		unregister_netdev(device->dev);
-
-		usb_kill_urb(device->pControlURB);
-		usb_free_urb(device->pControlURB);
-
 		free_netdev(device->dev);
 	}
 }
 
-static int device_dma0_tx_80211(struct sk_buff *skb, struct net_device *dev)
-{
-	struct vnt_private *pDevice = netdev_priv(dev);
-
-	spin_lock_irq(&pDevice->lock);
-
-	if (unlikely(pDevice->bStopTx0Pkt))
-		dev_kfree_skb_irq(skb);
-	else
-		vDMA0_tx_80211(pDevice, skb);
-
-	spin_unlock_irq(&pDevice->lock);
-
-	return NETDEV_TX_OK;
-}
-
 static int device_xmit(struct sk_buff *skb, struct net_device *dev)
 {
 	struct vnt_private *pDevice = netdev_priv(dev);
 	struct net_device_stats *stats = &pDevice->stats;
+	unsigned long flags;
 
-	spin_lock_irq(&pDevice->lock);
+	spin_lock_irqsave(&pDevice->lock, flags);
 
 	netif_stop_queue(dev);
 
@@ -1189,13 +1079,13 @@ static int device_xmit(struct sk_buff *skb, struct net_device *dev)
 		goto out;
 	}
 
-	if (nsDMA_tx_packet(pDevice, TYPE_AC0DMA, skb)) {
+	if (nsDMA_tx_packet(pDevice, skb)) {
 		if (netif_queue_stopped(dev))
 			netif_wake_queue(dev);
 	}
 
 out:
-	spin_unlock_irq(&pDevice->lock);
+	spin_unlock_irqrestore(&pDevice->lock, flags);
 
 	return NETDEV_TX_OK;
 }
@@ -1377,7 +1267,7 @@ void vnt_configure_filter(struct vnt_private *priv)
 	u8 tmp = 0;
 	int rc;
 
-	rc = CONTROLnsRequestIn(priv, MESSAGE_TYPE_READ,
+	rc = vnt_control_in(priv, MESSAGE_TYPE_READ,
 		MAC_REG_RCR, MESSAGE_REQUEST_MACREG, 1, &tmp);
 	if (rc == 0)
 		priv->byRxMode = tmp;
@@ -1418,7 +1308,7 @@ void vnt_configure_filter(struct vnt_private *priv)
 		priv->byRxMode &= ~(RCR_UNICAST);
 	}
 
-	ControlvWriteByte(priv, MESSAGE_REQUEST_MACREG,
+	vnt_control_out_u8(priv, MESSAGE_REQUEST_MACREG,
 					MAC_REG_RCR, priv->byRxMode);
 
 	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
@@ -1434,20 +1324,9 @@ static struct net_device_stats *device_get_stats(struct net_device *dev)
 
 static int device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 {
-	struct vnt_private *pDevice = netdev_priv(dev);
-	struct iwreq *wrq = (struct iwreq *) rq;
 	int rc = 0;
 
 	switch (cmd) {
-
-	case IOCTL_CMD_HOSTAPD:
-
-		if (!(pDevice->flags & DEVICE_FLAGS_OPENED))
-			rc = -EFAULT;
-
-		rc = vt6656_hostap_ioctl(pDevice, &wrq->u.data);
-		break;
-
 	case SIOCETHTOOL:
 		return ethtool_ioctl(dev, rq);
 
diff --git a/drivers/staging/vt6656/power.c b/drivers/staging/vt6656/power.c
index 43da58927cd..ddbd04695c9 100644
--- a/drivers/staging/vt6656/power.c
+++ b/drivers/staging/vt6656/power.c
@@ -44,8 +44,7 @@
 #include "wcmd.h"
 #include "rxtx.h"
 #include "card.h"
-#include "control.h"
-#include "rndis.h"
+#include "usbpipe.h"
 
 static int msglevel = MSG_LEVEL_INFO;
 
@@ -130,7 +129,7 @@ void PSvDisablePowerSaving(struct vnt_private *pDevice)
 {
 
 	/* disable power saving hw function */
-	CONTROLnsRequestOut(pDevice, MESSAGE_TYPE_DISABLE_PS, 0,
+	vnt_control_out(pDevice, MESSAGE_TYPE_DISABLE_PS, 0,
 						0, 0, NULL);
 
 	/* clear AutoSleep */
@@ -163,7 +162,7 @@ int PSbConsiderPowerDown(struct vnt_private *pDevice, int bCheckRxDMA,
 	u8 byData;
 
 	/* check if already in Doze mode */
-	ControlvReadByte(pDevice, MESSAGE_REQUEST_MACREG,
+	vnt_control_in_u8(pDevice, MESSAGE_REQUEST_MACREG,
 					MAC_REG_PSCTL, &byData);
 
 	if ((byData & PSCTL_PS) != 0)
diff --git a/drivers/staging/vt6656/rf.c b/drivers/staging/vt6656/rf.c
index 1e8f64bff03..3f54ae3cfb4 100644
--- a/drivers/staging/vt6656/rf.c
+++ b/drivers/staging/vt6656/rf.c
@@ -26,7 +26,7 @@
  * Date: Feb. 19, 2004
  *
  * Functions:
- *      IFRFbWriteEmbedded      - Embedded write RF register via MAC
+ *	vnt_rf_write_embedded	- Embedded write RF register via MAC
  *
  * Revision History:
  *	RF_VT3226: RobertYu:20051111, VT3226C0 and before
@@ -38,12 +38,9 @@
 #include "mac.h"
 #include "rf.h"
 #include "baseband.h"
-#include "control.h"
-#include "rndis.h"
+#include "usbpipe.h"
 #include "datarate.h"
 
-static int          msglevel                =MSG_LEVEL_INFO;
-//static int          msglevel                =MSG_LEVEL_DEBUG;
 #define BY_AL2230_REG_LEN     23 //24bit
 #define CB_AL2230_INIT_SEQ    15
 #define AL2230_PWR_IDX_LEN    64
@@ -355,7 +352,7 @@ static u8 al7230_channel_table2[CB_MAX_CHANNEL][3] = {
     };
 
 ///{{RobertYu:20051111
-static u8 at3226_init_table[CB_VT3226_INIT_SEQ][3] = {
+static u8 vt3226_init_table[CB_VT3226_INIT_SEQ][3] = {
     {0x03, 0xFF, 0x80},
     {0x02, 0x82, 0xA1},
     {0x03, 0xC6, 0xA2},
@@ -369,7 +366,7 @@ static u8 at3226_init_table[CB_VT3226_INIT_SEQ][3] = {
     {0x02, 0x00, 0x2A}
     };
 
-static u8 at3226d0_init_table[CB_VT3226_INIT_SEQ][3] = {
+static u8 vt3226d0_init_table[CB_VT3226_INIT_SEQ][3] = {
     {0x03, 0xFF, 0x80},
     {0x03, 0x02, 0x21}, //RobertYu:20060327
     {0x03, 0xC6, 0xA2},
@@ -707,17 +704,17 @@ const u8 RFaby11aChannelIndex[200] = {
  * Return Value: true if succeeded; false if failed.
  *
  */
-int IFRFbWriteEmbedded(struct vnt_private *pDevice, u32 dwData)
+int vnt_rf_write_embedded(struct vnt_private *priv, u32 data)
 {
-	u8 pbyData[4];
+	u8 reg_data[4];
 
-	pbyData[0] = (u8)dwData;
-	pbyData[1] = (u8)(dwData >> 8);
-	pbyData[2] = (u8)(dwData >> 16);
-	pbyData[3] = (u8)(dwData >> 24);
+	reg_data[0] = (u8)data;
+	reg_data[1] = (u8)(data >> 8);
+	reg_data[2] = (u8)(data >> 16);
+	reg_data[3] = (u8)(data >> 24);
 
-	CONTROLnsRequestOut(pDevice,
-		MESSAGE_TYPE_WRITE_IFRF, 0, 0, 4, pbyData);
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE_IFRF,
+				0, 0, ARRAY_SIZE(reg_data), reg_data);
 
 	return true;
 }
@@ -735,7 +732,7 @@ int IFRFbWriteEmbedded(struct vnt_private *pDevice, u32 dwData)
  * Return Value: true if succeeded; false if failed.
  *
  */
-int RFbSetPower(struct vnt_private *priv, u32 rate, u32 channel)
+int vnt_rf_setpower(struct vnt_private *priv, u32 rate, u32 channel)
 {
 	int ret = true;
 	u8 power = priv->byCCKPwr;
@@ -764,11 +761,37 @@ int RFbSetPower(struct vnt_private *priv, u32 rate, u32 channel)
 		break;
 	}
 
-	ret = RFbRawSetPower(priv, power, rate);
+	ret = vnt_rf_set_txpower(priv, power, rate);
 
 	return ret;
 }
 
+static u8 vnt_rf_addpower(struct vnt_private *priv)
+{
+	s32 rssi = -priv->uCurrRSSI;
+
+	if (!rssi)
+		return 7;
+
+	if (priv->byRFType == RF_VT3226D0) {
+		if (rssi < -70)
+			return 9;
+		else if (rssi < -65)
+			return 7;
+		else if (rssi < -60)
+			return 5;
+	} else {
+		if (rssi < -80)
+			return 9;
+		else if (rssi < -75)
+			return 7;
+		else if (rssi < -70)
+			return 5;
+	}
+
+	return 0;
+}
+
 /*
  * Description: Set Tx power
  *
@@ -783,11 +806,15 @@ int RFbSetPower(struct vnt_private *priv, u32 rate, u32 channel)
  *
  */
 
-int RFbRawSetPower(struct vnt_private *priv, u8 power, u32 rate)
+int vnt_rf_set_txpower(struct vnt_private *priv, u8 power, u32 rate)
 {
 	u32 power_setting = 0;
 	int ret = true;
 
+	power += vnt_rf_addpower(priv);
+	if (power > VNT_RF_MAX_POWER)
+		power = VNT_RF_MAX_POWER;
+
 	if (priv->byCurPwr == power)
 		return true;
 
@@ -798,42 +825,42 @@ int RFbRawSetPower(struct vnt_private *priv, u8 power, u32 rate)
 		if (priv->byCurPwr >= AL2230_PWR_IDX_LEN)
 			return false;
 
-		ret &= IFRFbWriteEmbedded(priv,
+		ret &= vnt_rf_write_embedded(priv,
 			al2230_power_table[priv->byCurPwr]);
 
 		if (rate <= RATE_11M)
-			ret &= IFRFbWriteEmbedded(priv, 0x0001b400 +
+			ret &= vnt_rf_write_embedded(priv, 0x0001b400 +
 				(BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW);
 		else
-			ret &= IFRFbWriteEmbedded(priv, 0x0005a400 +
+			ret &= vnt_rf_write_embedded(priv, 0x0005a400 +
 				(BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW);
 		break;
 	case RF_AL2230S:
 		if (priv->byCurPwr >= AL2230_PWR_IDX_LEN)
 			return false;
 
-		ret &= IFRFbWriteEmbedded(priv,
+		ret &= vnt_rf_write_embedded(priv,
 			al2230_power_table[priv->byCurPwr]);
 
 		if (rate <= RATE_11M) {
-			ret &= IFRFbWriteEmbedded(priv, 0x040c1400 +
+			ret &= vnt_rf_write_embedded(priv, 0x040c1400 +
 				(BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW);
-			ret &= IFRFbWriteEmbedded(priv, 0x00299b00 +
+			ret &= vnt_rf_write_embedded(priv, 0x00299b00 +
 				(BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW);
 		} else {
-			ret &= IFRFbWriteEmbedded(priv, 0x0005a400 +
+			ret &= vnt_rf_write_embedded(priv, 0x0005a400 +
 				(BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW);
-			ret &= IFRFbWriteEmbedded(priv, 0x00099b00 +
+			ret &= vnt_rf_write_embedded(priv, 0x00099b00 +
 				(BY_AL2230_REG_LEN << 3) + IFREGCTL_REGW);
 		}
 		break;
 
 	case RF_AIROHA7230:
 		if (rate <= RATE_11M)
-			ret &= IFRFbWriteEmbedded(priv, 0x111bb900 +
+			ret &= vnt_rf_write_embedded(priv, 0x111bb900 +
 				(BY_AL7230_REG_LEN << 3)+IFREGCTL_REGW);
 		else
-			ret &= IFRFbWriteEmbedded(priv, 0x221bb900 +
+			ret &= vnt_rf_write_embedded(priv, 0x221bb900 +
 				(BY_AL7230_REG_LEN << 3)+IFREGCTL_REGW);
 
 		if (priv->byCurPwr > AL7230_PWR_IDX_LEN)
@@ -846,7 +873,7 @@ int RFbRawSetPower(struct vnt_private *priv, u8 power, u32 rate)
 		power_setting = 0x080c0b00 | ((priv->byCurPwr) << 12) |
 				(BY_AL7230_REG_LEN << 3) | IFREGCTL_REGW;
 
-		ret &= IFRFbWriteEmbedded(priv, power_setting);
+		ret &= vnt_rf_write_embedded(priv, power_setting);
 
 		break;
 
@@ -856,7 +883,7 @@ int RFbRawSetPower(struct vnt_private *priv, u8 power, u32 rate)
 		power_setting = ((0x3f - priv->byCurPwr) << 20) | (0x17 << 8) |
 				(BY_VT3226_REG_LEN << 3) | IFREGCTL_REGW;
 
-		ret &= IFRFbWriteEmbedded(priv, power_setting);
+		ret &= vnt_rf_write_embedded(priv, power_setting);
 
 		break;
 	case RF_VT3226D0:
@@ -868,42 +895,42 @@ int RFbRawSetPower(struct vnt_private *priv, u8 power, u32 rate)
 				(0xe07 << 8) | (BY_VT3226_REG_LEN << 3) |
 						IFREGCTL_REGW;
 
-			ret &= IFRFbWriteEmbedded(priv, power_setting);
-			ret &= IFRFbWriteEmbedded(priv, 0x03c6a200 +
+			ret &= vnt_rf_write_embedded(priv, power_setting);
+			ret &= vnt_rf_write_embedded(priv, 0x03c6a200 +
 					(BY_VT3226_REG_LEN<<3)+IFREGCTL_REGW);
 
 			if (priv->vnt_mgmt.eScanState != WMAC_NO_SCANNING) {
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-				"RFbRawSetPower> 11B mode uCurrChannel[%d]\n",
+				dev_dbg(&priv->usb->dev,
+				"vnt_rf_set_txpower> 11B mode uCurrChannel[%d]\n",
 						priv->vnt_mgmt.uScanChannel);
-				ret &= IFRFbWriteEmbedded(priv,
+				ret &= vnt_rf_write_embedded(priv,
 					vt3226d0_lo_current_table[priv->
 						vnt_mgmt.uScanChannel - 1]);
 			} else {
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-				"RFbRawSetPower> 11B mode uCurrChannel[%d]\n",
+				dev_dbg(&priv->usb->dev,
+				"vnt_rf_set_txpower> 11B mode uCurrChannel[%d]\n",
 						priv->vnt_mgmt.uCurrChannel);
-				ret &= IFRFbWriteEmbedded(priv,
+				ret &= vnt_rf_write_embedded(priv,
 					vt3226d0_lo_current_table[priv->
 						vnt_mgmt.uCurrChannel - 1]);
 			}
 
-			ret &= IFRFbWriteEmbedded(priv, 0x015C0800 +
+			ret &= vnt_rf_write_embedded(priv, 0x015C0800 +
 				(BY_VT3226_REG_LEN<<3)+IFREGCTL_REGW);
 		} else {
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-					"@@@@ RFbRawSetPower> 11G mode\n");
+			dev_dbg(&priv->usb->dev,
+					"@@@@ vnt_rf_set_txpower> 11G mode\n");
 
 			power_setting = ((0x3f-priv->byCurPwr) << 20) |
 				(0x7 << 8) | (BY_VT3226_REG_LEN << 3) |
 					IFREGCTL_REGW;
 
-			ret &= IFRFbWriteEmbedded(priv, power_setting);
-			ret &= IFRFbWriteEmbedded(priv, 0x00C6A200 +
+			ret &= vnt_rf_write_embedded(priv, power_setting);
+			ret &= vnt_rf_write_embedded(priv, 0x00C6A200 +
 				(BY_VT3226_REG_LEN << 3) + IFREGCTL_REGW);
-			ret &= IFRFbWriteEmbedded(priv, 0x016BC600 +
+			ret &= vnt_rf_write_embedded(priv, 0x016BC600 +
 					(BY_VT3226_REG_LEN<<3)+IFREGCTL_REGW);
-			ret &= IFRFbWriteEmbedded(priv, 0x00900800 +
+			ret &= vnt_rf_write_embedded(priv, 0x00900800 +
 					(BY_VT3226_REG_LEN<<3)+IFREGCTL_REGW);
 		}
 		break;
@@ -916,7 +943,7 @@ int RFbRawSetPower(struct vnt_private *priv, u8 power, u32 rate)
 			(0x27 << 8) | (BY_VT3342_REG_LEN << 3) |
 					IFREGCTL_REGW;
 
-		ret &= IFRFbWriteEmbedded(priv, power_setting);
+		ret &= vnt_rf_write_embedded(priv, power_setting);
 
 		break;
 	default:
@@ -940,7 +967,7 @@ int RFbRawSetPower(struct vnt_private *priv, u8 power, u32 rate)
  * Return Value: none
  *
 -*/
-void RFvRSSITodBm(struct vnt_private *priv, u8 rssi, long *dbm)
+void vnt_rf_rssi_to_dbm(struct vnt_private *priv, u8 rssi, long *dbm)
 {
 	u8 idx = (((rssi & 0xc0) >> 6) & 0x03);
 	long b = (rssi & 0x3f);
@@ -963,7 +990,7 @@ void RFvRSSITodBm(struct vnt_private *priv, u8 rssi, long *dbm)
 	*dbm = -1 * (a + b * 2);
 }
 
-void RFbRFTableDownload(struct vnt_private *priv)
+void vnt_rf_table_download(struct vnt_private *priv)
 {
 	u16 length1 = 0, length2 = 0, length3 = 0;
 	u8 *addr1 = NULL, *addr2 = NULL, *addr3 = NULL;
@@ -992,7 +1019,7 @@ void RFbRFTableDownload(struct vnt_private *priv)
 		length1 = CB_VT3226_INIT_SEQ * 3;
 		length2 = CB_MAX_CHANNEL_24G * 3;
 		length3 = CB_MAX_CHANNEL_24G * 3;
-		addr1 = &at3226_init_table[0][0];
+		addr1 = &vt3226_init_table[0][0];
 		addr2 = &vt3226_channel_table0[0][0];
 		addr3 = &vt3226_channel_table1[0][0];
 		break;
@@ -1000,7 +1027,7 @@ void RFbRFTableDownload(struct vnt_private *priv)
 		length1 = CB_VT3226_INIT_SEQ * 3;
 		length2 = CB_MAX_CHANNEL_24G * 3;
 		length3 = CB_MAX_CHANNEL_24G * 3;
-		addr1 = &at3226d0_init_table[0][0];
+		addr1 = &vt3226d0_init_table[0][0];
 		addr2 = &vt3226_channel_table0[0][0];
 		addr3 = &vt3226_channel_table1[0][0];
 		break;
@@ -1017,7 +1044,7 @@ void RFbRFTableDownload(struct vnt_private *priv)
 	/* Init Table */
 	memcpy(array, addr1, length1);
 
-	CONTROLnsRequestOut(priv, MESSAGE_TYPE_WRITE, 0,
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE, 0,
 		MESSAGE_REQUEST_RF_INIT, length1, array);
 
 	/* Channel Table 0 */
@@ -1030,7 +1057,7 @@ void RFbRFTableDownload(struct vnt_private *priv)
 
 		memcpy(array, addr2, length);
 
-		CONTROLnsRequestOut(priv, MESSAGE_TYPE_WRITE,
+		vnt_control_out(priv, MESSAGE_TYPE_WRITE,
 			value, MESSAGE_REQUEST_RF_CH0, length, array);
 
 		length2 -= length;
@@ -1048,7 +1075,7 @@ void RFbRFTableDownload(struct vnt_private *priv)
 
 		memcpy(array, addr3, length);
 
-		CONTROLnsRequestOut(priv, MESSAGE_TYPE_WRITE,
+		vnt_control_out(priv, MESSAGE_TYPE_WRITE,
 			value, MESSAGE_REQUEST_RF_CH1, length, array);
 
 		length3 -= length;
@@ -1065,7 +1092,7 @@ void RFbRFTableDownload(struct vnt_private *priv)
 		memcpy(array, addr1, length1);
 
 		/* Init Table 2 */
-		CONTROLnsRequestOut(priv, MESSAGE_TYPE_WRITE,
+		vnt_control_out(priv, MESSAGE_TYPE_WRITE,
 			0, MESSAGE_REQUEST_RF_INIT2, length1, array);
 
 		/* Channel Table 0 */
@@ -1078,7 +1105,7 @@ void RFbRFTableDownload(struct vnt_private *priv)
 
 			memcpy(array, addr2, length);
 
-			CONTROLnsRequestOut(priv, MESSAGE_TYPE_WRITE,
+			vnt_control_out(priv, MESSAGE_TYPE_WRITE,
 				value, MESSAGE_REQUEST_RF_CH2, length, array);
 
 			length2 -= length;
diff --git a/drivers/staging/vt6656/rf.h b/drivers/staging/vt6656/rf.h
index de5c613b4ef..cb331518568 100644
--- a/drivers/staging/vt6656/rf.h
+++ b/drivers/staging/vt6656/rf.h
@@ -53,12 +53,14 @@
 #define RF_EMU              0x80
 #define RF_MASK             0x7F
 
+#define VNT_RF_MAX_POWER    0x3f
+
 extern const u8 RFaby11aChannelIndex[200];
 
-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 vnt_rf_write_embedded(struct vnt_private *, u32);
+int vnt_rf_setpower(struct vnt_private *, u32, u32);
+int vnt_rf_set_txpower(struct vnt_private *, u8, u32);
+void vnt_rf_rssi_to_dbm(struct vnt_private *, u8, long *);
+void vnt_rf_table_download(struct vnt_private *);
 
 #endif /* __RF_H__ */
diff --git a/drivers/staging/vt6656/rndis.h b/drivers/staging/vt6656/rndis.h
deleted file mode 100644
index 3661f82766e..00000000000
--- a/drivers/staging/vt6656/rndis.h
+++ /dev/null
@@ -1,151 +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: rndis.h
- *
- * Purpose: Interface between firmware and driver
- *
- * Author: Warren Hsu
- *
- * Date: Nov 24, 2004
- *
- */
-
-#ifndef __RNDIS_H__
-#define __RNDIS_H__
-
-#define MESSAGE_TYPE_READ               0x01
-#define MESSAGE_TYPE_WRITE              0x00
-#define MESSAGE_TYPE_LOCK_OR            0x02
-#define MESSAGE_TYPE_LOCK_AND           0x03
-#define MESSAGE_TYPE_WRITE_MASK         0x04
-#define MESSAGE_TYPE_CARDINIT           0x05
-#define MESSAGE_TYPE_INIT_RSP           0x06
-#define MESSAGE_TYPE_MACSHUTDOWN        0x07
-#define MESSAGE_TYPE_SETKEY             0x08
-#define MESSAGE_TYPE_CLRKEYENTRY        0x09
-#define MESSAGE_TYPE_WRITE_MISCFF       0x0A
-#define MESSAGE_TYPE_SET_ANTMD          0x0B
-#define MESSAGE_TYPE_SELECT_CHANNLE     0x0C
-#define MESSAGE_TYPE_SET_TSFTBTT        0x0D
-#define MESSAGE_TYPE_SET_SSTIFS         0x0E
-#define MESSAGE_TYPE_CHANGE_BBTYPE      0x0F
-#define MESSAGE_TYPE_DISABLE_PS         0x10
-#define MESSAGE_TYPE_WRITE_IFRF         0x11
-
-//used for read/write(index)
-#define MESSAGE_REQUEST_MEM             0x01
-#define MESSAGE_REQUEST_BBREG           0x02
-#define MESSAGE_REQUEST_MACREG          0x03
-#define MESSAGE_REQUEST_EEPROM          0x04
-#define MESSAGE_REQUEST_TSF             0x05
-#define MESSAGE_REQUEST_TBTT            0x06
-#define MESSAGE_REQUEST_BBAGC           0x07
-#define MESSAGE_REQUEST_VERSION         0x08
-#define MESSAGE_REQUEST_RF_INIT         0x09
-#define MESSAGE_REQUEST_RF_INIT2        0x0A
-#define MESSAGE_REQUEST_RF_CH0          0x0B
-#define MESSAGE_REQUEST_RF_CH1          0x0C
-#define MESSAGE_REQUEST_RF_CH2          0x0D
-
-#define VIAUSB20_PACKET_HEADER          0x04
-
-#define USB_REG4	0x604
-
-typedef struct _CMD_MESSAGE
-{
-    u8        byData[256];
-} CMD_MESSAGE, *PCMD_MESSAGE;
-
-typedef struct _CMD_WRITE_MASK
-{
-    u8        byData;
-    u8        byMask;
-} CMD_WRITE_MASK, *PCMD_WRITE_MASK;
-
-struct vnt_cmd_card_init
-{
-	u8 init_class;
-	u8 exist_sw_net_addr;
-	u8 sw_net_addr[6];
-	u8 short_retry_limit;
-	u8 long_retry_limit;
-};
-
-struct vnt_rsp_card_init
-{
-	u8 status;
-	u8 net_addr[6];
-	u8 rf_type;
-	u8 min_channel;
-	u8 max_channel;
-};
-
-typedef struct _CMD_SET_KEY
-{
-    u16        wKCTL;
-    u8        abyMacAddr[6];
-    u8        abyKey[16];
-} CMD_SET_KEY, *PCMD_SET_KEY;
-
-typedef struct _CMD_CLRKEY_ENTRY
-{
-    u8        abyKeyEntry[11];
-} CMD_CLRKEY_ENTRY, *PCMD_CLRKEY_ENTRY;
-
-typedef struct _CMD_WRITE_MISCFF
-{
-    u32       adwMiscFFData[22][4];  //a key entry has only 22 dwords
-} CMD_WRITE_MISCFF, *PCMD_WRITE_MISCFF;
-
-typedef struct _CMD_SET_TSFTBTT
-{
-    u8        abyTSF_TBTT[8];
-} CMD_SET_TSFTBTT, *PCMD_SET_TSFTBTT;
-
-typedef struct _CMD_SET_SSTIFS
-{
-    u8        bySIFS;
-    u8        byDIFS;
-    u8        byEIFS;
-    u8        bySlotTime;
-    u8        byCwMax_Min;
-    u8        byBBCR10;
-} CMD_SET_SSTIFS, *PCMD_SET_SSTIFS;
-
-typedef struct _CMD_CHANGE_BBTYPE
-{
-    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;
-
-#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 3840323858f..704f4d3639b 100644
--- a/drivers/staging/vt6656/rxtx.c
+++ b/drivers/staging/vt6656/rxtx.c
@@ -54,9 +54,7 @@
 #include "mac.h"
 #include "michael.h"
 #include "tkip.h"
-#include "tcrc.h"
 #include "wctl.h"
-#include "hostap.h"
 #include "rf.h"
 #include "datarate.h"
 #include "usbpipe.h"
@@ -101,11 +99,11 @@ static struct vnt_usb_send_context *s_vGetFreeContext(struct vnt_private *);
 static u16 s_vGenerateTxParameter(struct vnt_private *pDevice,
 	u8 byPktType, u16 wCurrentRate,	struct vnt_tx_buffer *tx_buffer,
 	struct vnt_mic_hdr **mic_hdr, u32 need_mic, u32 cbFrameSize,
-	int bNeedACK, u32 uDMAIdx, struct ethhdr *psEthHeader, bool need_rts);
+	int bNeedACK, struct ethhdr *psEthHeader, bool need_rts);
 
 static void s_vGenerateMACHeader(struct vnt_private *pDevice,
 	u8 *pbyBufferAddr, u16 wDuration, struct ethhdr *psEthHeader,
-	int bNeedEncrypt, u16 wFragType, u32 uDMAIdx, u32 uFragIdx);
+	int bNeedEncrypt, u16 wFragType, u32 uFragIdx);
 
 static void s_vFillTxKey(struct vnt_private *pDevice,
 	struct vnt_tx_fifo_head *fifo_head, u8 *pbyIVHead,
@@ -119,9 +117,9 @@ static unsigned int s_uGetTxRsvTime(struct vnt_private *pDevice, u8 byPktType,
 	u32 cbFrameLength, u16 wRate, int bNeedAck);
 
 static __le16 s_uGetRTSCTSRsvTime(struct vnt_private *priv,
-	u8 rsv_type, u8 pkt_type, u32 frame_lenght, u16 current_rate);
+	u8 rsv_type, u8 pkt_type, u32 frame_length, u16 current_rate);
 
-static u16 s_vFillCTSHead(struct vnt_private *pDevice, u32 uDMAIdx,
+static u16 s_vFillCTSHead(struct vnt_private *pDevice,
 	u8 byPktType, union vnt_tx_data_head *head, u32 cbFrameLength,
 	int bNeedAck, u16 wCurrentRate, u8 byFBOption);
 
@@ -149,9 +147,9 @@ static struct vnt_usb_send_context
 			return NULL;
 
 		context = priv->apTD[ii];
-		if (context->bBoolInUse == false) {
-			context->bBoolInUse = true;
-			memset(context->Data, 0,
+		if (context->in_use == false) {
+			context->in_use = true;
+			memset(context->data, 0,
 					MAX_TOTAL_SIZE_WITH_ALL_HEADERS);
 			return context;
 		}
@@ -181,41 +179,40 @@ static void s_vFillTxKey(struct vnt_private *pDevice,
 	struct vnt_mic_hdr *mic_hdr)
 {
 	u8 *pbyBuf = (u8 *)&fifo_head->adwTxKey[0];
-	u32 *pdwIV = (u32 *)pbyIVHead;
-	u32 *pdwExtIV = (u32 *)((u8 *)pbyIVHead + 4);
+	__le32 *pdwIV = (__le32 *)pbyIVHead;
+	__le32 *pdwExtIV = (__le32 *)((u8 *)pbyIVHead + 4);
 	struct ieee80211_hdr *pMACHeader = (struct ieee80211_hdr *)pbyHdrBuf;
-	u32 dwRevIVCounter;
+	__le32 rev_iv_counter;
 
 	/* Fill TXKEY */
 	if (pTransmitKey == NULL)
 		return;
 
-	dwRevIVCounter = cpu_to_le32(pDevice->dwIVCounter);
-	*pdwIV = pDevice->dwIVCounter;
+	rev_iv_counter = cpu_to_le32(pDevice->dwIVCounter);
+	*pdwIV = cpu_to_le32(pDevice->dwIVCounter);
 	pDevice->byKeyIndex = pTransmitKey->dwKeyIndex & 0xf;
 
 	switch (pTransmitKey->byCipherSuite) {
 	case KEY_CTL_WEP:
 		if (pTransmitKey->uKeyLength == WLAN_WEP232_KEYLEN) {
-			memcpy(pDevice->abyPRNG, (u8 *)&dwRevIVCounter, 3);
+			memcpy(pDevice->abyPRNG, (u8 *)&rev_iv_counter, 3);
 			memcpy(pDevice->abyPRNG + 3, pTransmitKey->abyKey,
 						pTransmitKey->uKeyLength);
 		} else {
-			memcpy(pbyBuf, (u8 *)&dwRevIVCounter, 3);
+			memcpy(pbyBuf, (u8 *)&rev_iv_counter, 3);
 			memcpy(pbyBuf + 3, pTransmitKey->abyKey,
 						pTransmitKey->uKeyLength);
 			if (pTransmitKey->uKeyLength == WLAN_WEP40_KEYLEN) {
-				memcpy(pbyBuf+8, (u8 *)&dwRevIVCounter, 3);
-			memcpy(pbyBuf+11, pTransmitKey->abyKey,
+				memcpy(pbyBuf+8, (u8 *)&rev_iv_counter, 3);
+				memcpy(pbyBuf+11, pTransmitKey->abyKey,
 						pTransmitKey->uKeyLength);
 			}
 
 			memcpy(pDevice->abyPRNG, pbyBuf, 16);
 		}
 		/* Append IV after Mac Header */
-		*pdwIV &= WEP_IV_MASK;
-		*pdwIV |= (u32)pDevice->byKeyIndex << 30;
-		*pdwIV = cpu_to_le32(*pdwIV);
+		*pdwIV &= cpu_to_le32(WEP_IV_MASK);
+		*pdwIV |= cpu_to_le32((u32)pDevice->byKeyIndex << 30);
 
 		pDevice->dwIVCounter++;
 		if (pDevice->dwIVCounter > WEP_IV_MASK)
@@ -256,7 +253,7 @@ static void s_vFillTxKey(struct vnt_private *pDevice,
 		*(pbyIVHead+3) = (u8)(((pDevice->byKeyIndex << 6) &
 							0xc0) | 0x20);
 
-		*pdwIV |= cpu_to_le16((u16)(pTransmitKey->wTSC15_0));
+		*pdwIV |= cpu_to_le32((u32)(pTransmitKey->wTSC15_0));
 
 		/* Append IV&ExtIV after Mac Header */
 		*pdwExtIV = cpu_to_le32(pTransmitKey->dwTSC47_16);
@@ -283,9 +280,10 @@ static void s_vFillTxKey(struct vnt_private *pDevice,
 
 		/* MICHDR2 */
 		memcpy(mic_hdr->addr3, pMACHeader->addr3, ETH_ALEN);
-		mic_hdr->frame_control = cpu_to_le16(pMACHeader->frame_control
-								& 0xc78f);
-		mic_hdr->seq_ctrl = cpu_to_le16(pMACHeader->seq_ctrl & 0xf);
+		mic_hdr->frame_control = cpu_to_le16(
+			le16_to_cpu(pMACHeader->frame_control) & 0xc78f);
+		mic_hdr->seq_ctrl = cpu_to_le16(
+				le16_to_cpu(pMACHeader->seq_ctrl) & 0xf);
 
 		if (ieee80211_has_a4(pMACHeader->frame_control))
 			memcpy(mic_hdr->addr4, pMACHeader->addr4, ETH_ALEN);
@@ -305,7 +303,7 @@ static void s_vSWencryption(struct vnt_private *pDevice,
     if (pTransmitKey->byCipherSuite == KEY_CTL_WEP) {
         //=======================================================================
         // Append ICV after payload
-        dwICV = CRCdwGetCrc32Ex(pbyPayloadHead, wPayloadSize, dwICV);//ICV(Payload)
+	dwICV = ether_crc_le(wPayloadSize, pbyPayloadHead);
         pdwICV = (u32 *)(pbyPayloadHead + wPayloadSize);
         // finally, we must invert dwCRC to get the correct answer
         *pdwICV = cpu_to_le32(~dwICV);
@@ -316,7 +314,7 @@ static void s_vSWencryption(struct vnt_private *pDevice,
     } else if (pTransmitKey->byCipherSuite == KEY_CTL_TKIP) {
         //=======================================================================
         //Append ICV after payload
-        dwICV = CRCdwGetCrc32Ex(pbyPayloadHead, wPayloadSize, dwICV);//ICV(Payload)
+	dwICV = ether_crc_le(wPayloadSize, pbyPayloadHead);
         pdwICV = (u32 *)(pbyPayloadHead + wPayloadSize);
         // finally, we must invert dwCRC to get the correct answer
         *pdwICV = cpu_to_le32(~dwICV);
@@ -368,14 +366,14 @@ static __le16 vnt_rxtx_rsvtime_le16(struct vnt_private *priv, u8 pkt_type,
 
 //byFreqType: 0=>5GHZ 1=>2.4GHZ
 static __le16 s_uGetRTSCTSRsvTime(struct vnt_private *priv,
-	u8 rsv_type, u8 pkt_type, u32 frame_lenght, u16 current_rate)
+	u8 rsv_type, u8 pkt_type, u32 frame_length, u16 current_rate)
 {
 	u32 rrv_time, rts_time, cts_time, ack_time, data_time;
 
 	rrv_time = rts_time = cts_time = ack_time = data_time = 0;
 
 	data_time = BBuGetFrameTime(priv->byPreambleType, pkt_type,
-						frame_lenght, current_rate);
+						frame_length, current_rate);
 
 	if (rsv_type == 0) {
 		rts_time = BBuGetFrameTime(priv->byPreambleType,
@@ -551,7 +549,8 @@ static int vnt_fill_ieee80211_rts(struct vnt_private *priv,
 		__le16 duration)
 {
 	rts->duration = duration;
-	rts->frame_control = TYPE_CTL_RTS;
+	rts->frame_control =
+		cpu_to_le16(IEEE80211_FTYPE_CTL | IEEE80211_STYPE_RTS);
 
 	if (priv->op_mode == NL80211_IFTYPE_ADHOC ||
 				priv->op_mode == NL80211_IFTYPE_AP)
@@ -713,7 +712,7 @@ static u16 s_vFillRTSHead(struct vnt_private *pDevice, u8 byPktType,
 	return 0;
 }
 
-static u16 s_vFillCTSHead(struct vnt_private *pDevice, u32 uDMAIdx,
+static u16 s_vFillCTSHead(struct vnt_private *pDevice,
 	u8 byPktType, union vnt_tx_data_head *head, u32 cbFrameLength,
 	int bNeedAck, u16 wCurrentRate, u8 byFBOption)
 {
@@ -741,7 +740,9 @@ static u16 s_vFillCTSHead(struct vnt_private *pDevice, u32 uDMAIdx,
 			pDevice->tx_rate_fb1, bNeedAck, byFBOption);
 		/* Get CTS Frame body */
 		pBuf->data.duration = pBuf->duration_ba;
-		pBuf->data.frame_control = TYPE_CTL_CTS;
+		pBuf->data.frame_control =
+			cpu_to_le16(IEEE80211_FTYPE_CTL | IEEE80211_STYPE_CTS);
+
 		memcpy(pBuf->data.ra, pDevice->abyCurrentNetAddr, ETH_ALEN);
 
 		return vnt_rxtx_datahead_g_fb(pDevice, byPktType, wCurrentRate,
@@ -757,7 +758,9 @@ static u16 s_vFillCTSHead(struct vnt_private *pDevice, u32 uDMAIdx,
 			wCurrentRate, bNeedAck, byFBOption);
 		/*Get CTS Frame body*/
 		pBuf->data.duration = pBuf->duration_ba;
-		pBuf->data.frame_control = TYPE_CTL_CTS;
+		pBuf->data.frame_control =
+			cpu_to_le16(IEEE80211_FTYPE_CTL | IEEE80211_STYPE_CTS);
+
 		memcpy(pBuf->data.ra, pDevice->abyCurrentNetAddr, ETH_ALEN);
 
 		return vnt_rxtx_datahead_g(pDevice, byPktType, wCurrentRate,
@@ -782,7 +785,6 @@ static u16 s_vFillCTSHead(struct vnt_private *pDevice, u32 uDMAIdx,
  *      pCTS            - CTS Buffer
  *      cbFrameSize     - Transmit Data Length (Hdr+Payload+FCS)
  *      bNeedACK        - If need ACK
- *      uDMAIdx         - DMA Index
  *  Out:
  *      none
  *
@@ -793,14 +795,14 @@ static u16 s_vFillCTSHead(struct vnt_private *pDevice, u32 uDMAIdx,
 static u16 s_vGenerateTxParameter(struct vnt_private *pDevice,
 	u8 byPktType, u16 wCurrentRate,	struct vnt_tx_buffer *tx_buffer,
 	struct vnt_mic_hdr **mic_hdr, u32 need_mic, u32 cbFrameSize,
-	int bNeedACK, u32 uDMAIdx, struct ethhdr *psEthHeader, bool need_rts)
+	int bNeedACK, struct ethhdr *psEthHeader, bool need_rts)
 {
 	struct vnt_tx_fifo_head *pFifoHead = &tx_buffer->fifo_head;
 	union vnt_tx_data_head *head = NULL;
 	u16 wFifoCtl;
 	u8 byFBOption = AUTO_FB_NONE;
 
-	pFifoHead->wReserved = wCurrentRate;
+	pFifoHead->current_rate = cpu_to_le16(wCurrentRate);
 	wFifoCtl = pFifoHead->wFIFOCtl;
 
 	if (wFifoCtl & FIFOCTL_AUTO_FB_0)
@@ -808,9 +810,6 @@ static u16 s_vGenerateTxParameter(struct vnt_private *pDevice,
 	else if (wFifoCtl & FIFOCTL_AUTO_FB_1)
 		byFBOption = AUTO_FB_1;
 
-	if (!pFifoHead)
-		return 0;
-
 	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
 		if (need_rts) {
 			struct vnt_rrv_time_rts *pBuf =
@@ -864,7 +863,7 @@ static u16 s_vGenerateTxParameter(struct vnt_private *pDevice,
 			}
 
 			/* Fill CTS */
-			return s_vFillCTSHead(pDevice, uDMAIdx, byPktType,
+			return s_vFillCTSHead(pDevice, byPktType,
 				head, cbFrameSize, bNeedACK, wCurrentRate,
 					byFBOption);
 		}
@@ -949,12 +948,11 @@ static u16 s_vGenerateTxParameter(struct vnt_private *pDevice,
 
 static int s_bPacketToWirelessUsb(struct vnt_private *pDevice, u8 byPktType,
 	struct vnt_tx_buffer *tx_buffer, int bNeedEncryption,
-	u32 uSkbPacketLen, u32 uDMAIdx,	struct ethhdr *psEthHeader,
+	u32 uSkbPacketLen, struct ethhdr *psEthHeader,
 	u8 *pPacket, PSKeyItem pTransmitKey, u32 uNodeIndex, u16 wCurrentRate,
 	u32 *pcbHeaderLen, u32 *pcbTotalLen)
 {
 	struct vnt_tx_fifo_head *pTxBufHead = &tx_buffer->fifo_head;
-	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
 	u32 cbFrameSize, cbFrameBodySize;
 	u32 cb802_1_H_len;
 	u32 cbIVlen = 0, cbICVlen = 0, cbMIClen = 0, cbMACHdLen = 0;
@@ -1008,7 +1006,7 @@ static int s_bPacketToWirelessUsb(struct vnt_private *pDevice, u8 byPktType,
 		pTxBufHead->wFIFOCtl |= FIFOCTL_NEEDACK;
 	}
 
-    pTxBufHead->wTimeStamp = DEFAULT_MSDU_LIFETIME_RES_64us;
+    pTxBufHead->time_stamp = cpu_to_le16(DEFAULT_MSDU_LIFETIME_RES_64us);
 
     //Set FRAGCTL_MACHDCNT
 	cbMACHdLen = WLAN_HDR_ADDR3_LEN;
@@ -1145,27 +1143,21 @@ static int s_bPacketToWirelessUsb(struct vnt_private *pDevice, u8 byPktType,
     //=========================
     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"No Fragmentation...\n");
     byFragType = FRAGCTL_NONFRAG;
-    //uDMAIdx = TYPE_AC0DMA;
     //pTxBufHead = (PSTxBufHead) &(pTxBufHead->adwTxKey[0]);
 
 	/* Fill FIFO, RrvTime, RTS and CTS */
 	uDuration = s_vGenerateTxParameter(pDevice, byPktType, wCurrentRate,
 			tx_buffer, &pMICHDR, cbMICHDR,
-			cbFrameSize, bNeedACK, uDMAIdx, psEthHeader, bRTS);
+			cbFrameSize, bNeedACK, psEthHeader, bRTS);
 
     // Generate TX MAC Header
     s_vGenerateMACHeader(pDevice, pbyMacHdr, (u16)uDuration, psEthHeader, bNeedEncryption,
-                           byFragType, uDMAIdx, 0);
+		byFragType, 0);
 
     if (bNeedEncryption == true) {
         //Fill TXKEY
 	s_vFillTxKey(pDevice, pTxBufHead, pbyIVHead, pTransmitKey,
 		pbyMacHdr, (u16)cbFrameBodySize, pMICHDR);
-
-        if (pDevice->bEnableHostWEP) {
-            pMgmt->sNodeDBTable[uNodeIndex].dwTSC47_16 = pTransmitKey->dwTSC47_16;
-            pMgmt->sNodeDBTable[uNodeIndex].wTSC15_0 = pTransmitKey->wTSC15_0;
-        }
     }
 
 	/* 802.1H */
@@ -1287,7 +1279,7 @@ static int s_bPacketToWirelessUsb(struct vnt_private *pDevice, u8 byPktType,
 
 static void s_vGenerateMACHeader(struct vnt_private *pDevice,
 	u8 *pbyBufferAddr, u16 wDuration, struct ethhdr *psEthHeader,
-	int bNeedEncrypt, u16 wFragType, u32 uDMAIdx, u32 uFragIdx)
+	int bNeedEncrypt, u16 wFragType, u32 uFragIdx)
 {
 	struct ieee80211_hdr *pMACHeader = (struct ieee80211_hdr *)pbyBufferAddr;
 
@@ -1383,38 +1375,39 @@ CMD_STATUS csMgmt_xmit(struct vnt_private *pDevice,
 	u16 wTxBufSize;
 	u32 cbMacHdLen;
 	u16 wCurrentRate = RATE_1M;
+	unsigned long flags;
 
-	pContext = s_vGetFreeContext(pDevice);
+	if (pDevice->byBBType == BB_TYPE_11A) {
+		wCurrentRate = RATE_6M;
+		byPktType = PK_TYPE_11A;
+	} else {
+		wCurrentRate = RATE_1M;
+		byPktType = PK_TYPE_11B;
+	}
 
-    if (NULL == pContext) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ManagementSend TX...NO CONTEXT!\n");
-        return CMD_STATUS_RESOURCES;
-    }
+	if (pMgmt->eScanState != WMAC_NO_SCANNING)
+		vnt_rf_setpower(pDevice, wCurrentRate, pDevice->byCurrentCh);
+	else
+		vnt_rf_setpower(pDevice, wCurrentRate, pMgmt->uCurrChannel);
+
+	pDevice->wCurrentRate = wCurrentRate;
+
+	spin_lock_irqsave(&pDevice->lock, flags);
+
+	pContext = s_vGetFreeContext(pDevice);
+	if (!pContext) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
+			"ManagementSend TX...NO CONTEXT!\n");
+		spin_unlock_irqrestore(&pDevice->lock, flags);
+		return CMD_STATUS_RESOURCES;
+	}
 
-	pTX_Buffer = (struct vnt_tx_buffer *)&pContext->Data[0];
+	pTX_Buffer = (struct vnt_tx_buffer *)&pContext->data[0];
     cbFrameBodySize = pPacket->cbPayloadLen;
 	pTxBufHead = &pTX_Buffer->fifo_head;
 	pbyTxBufferAddr = (u8 *)&pTxBufHead->adwTxKey[0];
 	wTxBufSize = sizeof(struct vnt_tx_fifo_head);
 
-    if (pDevice->byBBType == BB_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 (pMgmt->eScanState != WMAC_NO_SCANNING) {
-        RFbSetPower(pDevice, wCurrentRate, pDevice->byCurrentCh);
-    } else {
-        RFbSetPower(pDevice, wCurrentRate, pMgmt->uCurrChannel);
-    }
-    pDevice->wCurrentRate = wCurrentRate;
 
     //Set packet type
     if (byPktType == PK_TYPE_11A) {//0000 0000 0000 0000
@@ -1431,7 +1424,7 @@ CMD_STATUS csMgmt_xmit(struct vnt_private *pDevice,
     }
 
     pTxBufHead->wFIFOCtl |= FIFOCTL_TMOEN;
-    pTxBufHead->wTimeStamp = cpu_to_le16(DEFAULT_MGN_LIFETIME_RES_64us);
+    pTxBufHead->time_stamp = cpu_to_le16(DEFAULT_MGN_LIFETIME_RES_64us);
 
     if (is_multicast_ether_addr(pPacket->p80211Header->sA3.abyAddr1)) {
         bNeedACK = false;
@@ -1527,7 +1520,7 @@ CMD_STATUS csMgmt_xmit(struct vnt_private *pDevice,
 	/* Fill FIFO,RrvTime,RTS,and CTS */
 	uDuration = s_vGenerateTxParameter(pDevice, byPktType, wCurrentRate,
 		pTX_Buffer, &pMICHDR, 0,
-		cbFrameSize, bNeedACK, TYPE_TXDMA0, &sEthHeader, false);
+		cbFrameSize, bNeedACK, &sEthHeader, false);
 
     pMACHeader = (struct ieee80211_hdr *) (pbyTxBufferAddr + cbHeaderSize);
 
@@ -1605,13 +1598,13 @@ CMD_STATUS csMgmt_xmit(struct vnt_private *pDevice,
 	}
     }
 
-    pTX_Buffer->wTxByteCount = cpu_to_le16((u16)(cbReqCount));
+    pTX_Buffer->tx_byte_count = 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 = (u16)cbReqCount + 4;  //USB header
+	pContext->skb = NULL;
+	pContext->type = CONTEXT_MGMT_PACKET;
+	pContext->buf_len = (u16)cbReqCount + 4; /* USB header */
 
     if (WLAN_GET_FC_TODS(pMACHeader->frame_control) == 0) {
 	s_vSaveTxPktInfo(pDevice, (u8)(pTX_Buffer->byPKTNO & 0x0F),
@@ -1625,6 +1618,9 @@ CMD_STATUS csMgmt_xmit(struct vnt_private *pDevice,
     }
 
     PIPEnsSendBulkOut(pDevice,pContext);
+
+	spin_unlock_irqrestore(&pDevice->lock, flags);
+
     return CMD_STATUS_PENDING;
 }
 
@@ -1649,7 +1645,7 @@ CMD_STATUS csBeacon_xmit(struct vnt_private *pDevice,
         return status ;
     }
 
-	pTX_Buffer = (struct vnt_beacon_buffer *)&pContext->Data[0];
+	pTX_Buffer = (struct vnt_beacon_buffer *)&pContext->data[0];
 	short_head = &pTX_Buffer->short_head;
 
     cbFrameBodySize = pPacket->cbPayloadLen;
@@ -1697,377 +1693,19 @@ CMD_STATUS csBeacon_xmit(struct vnt_private *pDevice,
 
     cbReqCount = cbHeaderSize + WLAN_HDR_ADDR3_LEN + cbFrameBodySize;
 
-    pTX_Buffer->wTxByteCount = (u16)cbReqCount;
+    pTX_Buffer->tx_byte_count = cpu_to_le16((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 = (u16)cbReqCount + 4;  //USB header
+	pContext->skb = NULL;
+	pContext->type = CONTEXT_MGMT_PACKET;
+	pContext->buf_len = (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;
-	struct vnt_tx_buffer *pTX_Buffer;
-	struct vnt_tx_fifo_head *pTxBufHead;
-	u8 byPktType;
-	u8 *pbyTxBufferAddr;
-	u32 uDuration, cbReqCount;
-	struct ieee80211_hdr *pMACHeader;
-	u32 cbHeaderSize, cbFrameBodySize;
-	int bNeedACK, bIsPSPOLL = false;
-	u32 cbFrameSize;
-	u32 cbIVlen = 0, cbICVlen = 0, cbMIClen = 0, cbFCSlen = 4;
-	u32 uPadding = 0;
-	u32 cbMICHDR = 0, uLength = 0;
-	u32 dwMICKey0, dwMICKey1;
-	u32 dwMIC_Priority;
-	u32 *pdwMIC_L, *pdwMIC_R;
-	u16 wTxBufSize;
-	u32 cbMacHdLen;
-	struct ethhdr sEthHeader;
-	struct vnt_mic_hdr *pMICHDR;
-	u32 wCurrentRate = RATE_1M;
-	PUWLAN_80211HDR  p80211Header;
-	u32 uNodeIndex = 0;
-	int bNodeExist = false;
-	SKeyItem STempKey;
-	PSKeyItem pTransmitKey = NULL;
-	u8 *pbyIVHead, *pbyPayloadHead, *pbyMacHdr;
-	u32 cbExtSuppRate = 0;
-	struct vnt_usb_send_context *pContext;
-
-	pMICHDR = NULL;
-
-    if(skb->len <= WLAN_HDR_ADDR3_LEN) {
-       cbFrameBodySize = 0;
-    }
-    else {
-       cbFrameBodySize = skb->len - WLAN_HDR_ADDR3_LEN;
-    }
-    p80211Header = (PUWLAN_80211HDR)skb->data;
-
-	pContext = s_vGetFreeContext(pDevice);
-
-    if (NULL == pContext) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"DMA0 TX...NO CONTEXT!\n");
-        dev_kfree_skb_irq(skb);
-        return ;
-    }
-
-	pTX_Buffer = (struct vnt_tx_buffer *)&pContext->Data[0];
-	pTxBufHead = &pTX_Buffer->fifo_head;
-	pbyTxBufferAddr = (u8 *)&pTxBufHead->adwTxKey[0];
-	wTxBufSize = sizeof(struct vnt_tx_fifo_head);
-
-    if (pDevice->byBBType == BB_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 (pMgmt->eScanState != WMAC_NO_SCANNING) {
-        RFbSetPower(pDevice, wCurrentRate, pDevice->byCurrentCh);
-    } else {
-        RFbSetPower(pDevice, wCurrentRate, pMgmt->uCurrChannel);
-    }
-
-    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)) {
-        bNeedACK = false;
-        if (pDevice->bEnableHostWEP) {
-            uNodeIndex = 0;
-            bNodeExist = true;
-        }
-    }
-    else {
-        if (pDevice->bEnableHostWEP) {
-            if (BSSbIsSTAInNodeDB(pDevice, (u8 *)(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 daemon 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((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.
-
-    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(struct vnt_mic_hdr);
-            pTxBufHead->wFragCtl |= FRAGCTL_AES;
-        }
-        //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
-	cbHeaderSize = wTxBufSize + sizeof(struct vnt_rrv_time_cts) + cbMICHDR +
-		sizeof(struct vnt_cts);
-
-    }
-    else {//802.11a/b packet
-	cbHeaderSize = wTxBufSize + sizeof(struct vnt_rrv_time_ab) + cbMICHDR +
-					sizeof(struct vnt_tx_datahead_ab);
-    }
-    memcpy(&(sEthHeader.h_dest[0]),
-	   &(p80211Header->sA3.abyAddr1[0]),
-	   ETH_ALEN);
-    memcpy(&(sEthHeader.h_source[0]),
-	   &(p80211Header->sA3.abyAddr2[0]),
-	   ETH_ALEN);
-    //=========================
-    //    No Fragmentation
-    //=========================
-    pTxBufHead->wFragCtl |= (u16)FRAGCTL_NONFRAG;
-
-	/* Fill FIFO,RrvTime,RTS,and CTS */
-	uDuration = s_vGenerateTxParameter(pDevice, byPktType, wCurrentRate,
-		pTX_Buffer, &pMICHDR, cbMICHDR,
-		cbFrameSize, bNeedACK, TYPE_TXDMA0, &sEthHeader, false);
-
-	pMACHeader = (struct ieee80211_hdr *) (pbyTxBufferAddr + cbHeaderSize);
-
-    cbReqCount = cbHeaderSize + cbMacHdLen + uPadding + cbIVlen + (cbFrameBodySize + cbMIClen) + cbExtSuppRate;
-
-    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->frame_control &= cpu_to_le16(0xfffc);
-    memcpy(pbyPayloadHead, (skb->data + cbMacHdLen), cbFrameBodySize);
-
-    // replace support rate, patch for hostapd daemon( 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 = *(u32 *)(&pTransmitKey->abyKey[16]);
-            dwMICKey1 = *(u32 *)(&pTransmitKey->abyKey[20]);
-
-            // DO Software Michael
-            MIC_vInit(dwMICKey0, dwMICKey1);
-            MIC_vAppend((u8 *)&(sEthHeader.h_dest[0]), 12);
-            dwMIC_Priority = 0;
-            MIC_vAppend((u8 *)&dwMIC_Priority, 4);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"DMA0_tx_8021:MIC KEY:"\
-			" %X, %X\n", dwMICKey0, dwMICKey1);
-
-            uLength = cbHeaderSize + cbMacHdLen + uPadding + cbIVlen;
-
-            MIC_vAppend((pbyTxBufferAddr + uLength), cbFrameBodySize);
-
-            pdwMIC_L = (u32 *)(pbyTxBufferAddr + uLength + cbFrameBodySize);
-            pdwMIC_R = (u32 *)(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:%x, %x\n",
-			*pdwMIC_L, *pdwMIC_R);
-
-        }
-
-	s_vFillTxKey(pDevice, pTxBufHead, pbyIVHead, pTransmitKey,
-		pbyMacHdr, (u16)cbFrameBodySize, 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, (u16)(cbFrameBodySize + cbMIClen));
-        }
-    }
-
-    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.
-        // 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) {
-		struct vnt_tx_datahead_g *data_head = &pTX_Buffer->tx_head.
-						tx_cts.tx.head.cts_g.data_head;
-		data_head->duration_a =
-			cpu_to_le16(p80211Header->sA2.wDurationID);
-		data_head->duration_b =
-			cpu_to_le16(p80211Header->sA2.wDurationID);
-	} else {
-		struct vnt_tx_datahead_ab *data_head = &pTX_Buffer->tx_head.
-					tx_ab.tx.head.data_head_ab;
-		data_head->duration =
-			cpu_to_le16(p80211Header->sA2.wDurationID);
-	}
-    }
-
-    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 = (u16)cbReqCount + 4;  //USB header
-
-    if (WLAN_GET_FC_TODS(pMACHeader->frame_control) == 0) {
-	s_vSaveTxPktInfo(pDevice, (u8)(pTX_Buffer->byPKTNO & 0x0F),
-			&pMACHeader->addr1[0], (u16)cbFrameSize,
-			pTxBufHead->wFIFOCtl);
-    }
-    else {
-	s_vSaveTxPktInfo(pDevice, (u8)(pTX_Buffer->byPKTNO & 0x0F),
-			&pMACHeader->addr3[0], (u16)cbFrameSize,
-			pTxBufHead->wFIFOCtl);
-    }
-    PIPEnsSendBulkOut(pDevice,pContext);
-    return ;
-
-}
-
 //TYPE_AC0DMA data tx
 /*
  * Description:
@@ -2083,8 +1721,7 @@ void vDMA0_tx_80211(struct vnt_private *pDevice, struct sk_buff *skb)
  * Return Value: NULL
  */
 
-int nsDMA_tx_packet(struct vnt_private *pDevice,
-	u32 uDMAIdx, struct sk_buff *skb)
+int nsDMA_tx_packet(struct vnt_private *pDevice, struct sk_buff *skb)
 {
 	struct net_device_stats *pStats = &pDevice->stats;
 	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
@@ -2096,7 +1733,6 @@ int nsDMA_tx_packet(struct vnt_private *pDevice,
 	u8 byPktType;
 	int bNeedEncryption = false;
 	PSKeyItem pTransmitKey = NULL;
-	SKeyItem STempKey;
 	int ii;
 	int bTKIP_UseGTK = false;
 	int bNeedDeAuth = false;
@@ -2173,15 +1809,7 @@ int nsDMA_tx_packet(struct vnt_private *pDevice,
         }
     }
 
-	pContext = s_vGetFreeContext(pDevice);
-
-    if (pContext == NULL) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG" pContext == NULL\n");
-        dev_kfree_skb_irq(skb);
-        return STATUS_RESOURCES;
-    }
-
-    memcpy(pDevice->sTxEthHeader.h_dest, (u8 *)(skb->data), ETH_HLEN);
+	memcpy(&pDevice->sTxEthHeader, skb->data, ETH_HLEN);
 
 //mike add:station mode check eapol-key challenge--->
 {
@@ -2268,22 +1896,6 @@ int nsDMA_tx_packet(struct vnt_private *pDevice,
         } 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
-                );
-         }
-    }
-
     byPktType = (u8)pDevice->byPacketType;
 
     if (pDevice->bFixRate) {
@@ -2346,12 +1958,10 @@ int nsDMA_tx_packet(struct vnt_private *pDevice,
 	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;
 			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;
 		}
@@ -2389,21 +1999,11 @@ int nsDMA_tx_packet(struct vnt_private *pDevice,
                     }
                 }
             }
-
-            if (pDevice->bEnableHostWEP) {
-                if ((uNodeIndex != 0) &&
-                    (pMgmt->sNodeDBTable[uNodeIndex].dwKeyIndex & PAIRWISE_KEY)) {
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Find PTK [%X]\n",
-				pTransmitKey->dwKeyIndex);
-                    bNeedEncryption = true;
-                 }
-             }
         }
         else {
 
             if (pTransmitKey == NULL) {
                 DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"return no tx key\n");
-		pContext->bBoolInUse = false;
                 dev_kfree_skb_irq(skb);
                 pStats->tx_dropped++;
                 return STATUS_FAILURE;
@@ -2411,21 +2011,28 @@ int nsDMA_tx_packet(struct vnt_private *pDevice,
         }
     }
 
-	pTX_Buffer = (struct vnt_tx_buffer *)&pContext->Data[0];
+	pContext = s_vGetFreeContext(pDevice);
+	if (!pContext) {
+		DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG" pContext == NULL\n");
+		dev_kfree_skb_irq(skb);
+		return STATUS_RESOURCES;
+	}
+
+	pTX_Buffer = (struct vnt_tx_buffer *)&pContext->data[0];
 
     fConvertedPacket = s_bPacketToWirelessUsb(pDevice, byPktType,
 			pTX_Buffer, bNeedEncryption,
-                        skb->len, uDMAIdx, &pDevice->sTxEthHeader,
+			skb->len, &pDevice->sTxEthHeader,
                         (u8 *)skb->data, pTransmitKey, uNodeIndex,
                         pDevice->wCurrentRate,
                         &uHeaderLen, &BytesToWrite
                        );
 
-    if (fConvertedPacket == false) {
-        pContext->bBoolInUse = false;
-        dev_kfree_skb_irq(skb);
-        return STATUS_FAILURE;
-    }
+	if (fConvertedPacket == false) {
+		pContext->in_use = false;
+		dev_kfree_skb_irq(skb);
+		return STATUS_FAILURE;
+	}
 
     if ( pDevice->bEnablePSMode == true ) {
         if ( !pDevice->bPSModeTxBurst ) {
@@ -2437,11 +2044,11 @@ int nsDMA_tx_packet(struct vnt_private *pDevice,
     }
 
     pTX_Buffer->byPKTNO = (u8) (((pDevice->wCurrentRate<<4) &0x00F0) | ((pDevice->wSeqCounter - 1) & 0x000F));
-    pTX_Buffer->wTxByteCount = (u16)BytesToWrite;
+    pTX_Buffer->tx_byte_count = cpu_to_le16((u16)BytesToWrite);
 
-    pContext->pPacket = skb;
-    pContext->type = CONTEXT_DATA_PACKET;
-    pContext->uBufLen = (u16)BytesToWrite + 4 ; //USB header
+	pContext->skb = skb;
+	pContext->type = CONTEXT_DATA_PACKET;
+	pContext->buf_len = (u16)BytesToWrite + 4 ; /* USB header */
 
     s_vSaveTxPktInfo(pDevice, (u8)(pTX_Buffer->byPKTNO & 0x0F),
 			&pDevice->sTxEthHeader.h_dest[0],
@@ -2456,154 +2063,12 @@ int nsDMA_tx_packet(struct vnt_private *pDevice,
 	bScheduleCommand((void *) pDevice, WLAN_CMD_DEAUTH, (u8 *) &wReason);
     }
 
-  if(status!=STATUS_PENDING) {
-     pContext->bBoolInUse = false;
-    dev_kfree_skb_irq(skb);
-    return STATUS_FAILURE;
-  }
-  else
-    return 0;
-
-}
-
-/*
- * Description:
- *      Relay packet send (AC1DMA) from rx dpc.
- *
- * Parameters:
- *  In:
- *      pDevice         - Pointer to the adapter
- *      pPacket         - Pointer to rx packet
- *      cbPacketSize    - rx ethernet frame size
- *  Out:
- *      TURE, false
- *
- * Return Value: Return true if packet is copy to dma1; otherwise false
- */
-
-int bRelayPacketSend(struct vnt_private *pDevice, u8 *pbySkbData, u32 uDataLen,
-	u32 uNodeIndex)
-{
-	struct vnt_manager *pMgmt = &pDevice->vnt_mgmt;
-	struct vnt_tx_buffer *pTX_Buffer;
-	u32 BytesToWrite = 0, uHeaderLen = 0;
-	u8 byPktType = PK_TYPE_11B;
-	int bNeedEncryption = false;
-	SKeyItem STempKey;
-	PSKeyItem pTransmitKey = NULL;
-	u8 *pbyBSSID;
-	struct vnt_usb_send_context *pContext;
-	u8 byPktTyp;
-	int fConvertedPacket;
-	u32 status;
-	u16 wKeepRate = pDevice->wCurrentRate;
-
-	pContext = s_vGetFreeContext(pDevice);
-
-    if (NULL == pContext) {
-        return false;
-    }
-
-    memcpy(pDevice->sTxEthHeader.h_dest, (u8 *)pbySkbData, ETH_HLEN);
-
-    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", 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
-                  );
-        }
-    }
-
-    if ( bNeedEncryption && (pTransmitKey == NULL) ) {
-        pContext->bBoolInUse = false;
-        return false;
-    }
-
-    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 = (u16)pDevice->uConnectionRate;
-            }
-        } else {
-            if ((pDevice->byBBType == BB_TYPE_11A) &&
-                (pDevice->uConnectionRate <= RATE_6M)) {
-                pDevice->wCurrentRate = RATE_6M;
-            } else {
-                if (pDevice->uConnectionRate >= RATE_54M)
-                    pDevice->wCurrentRate = RATE_54M;
-                else
-                    pDevice->wCurrentRate = (u16)pDevice->uConnectionRate;
-            }
-        }
-    }
-    else {
-        pDevice->wCurrentRate = pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate;
-    }
-
-    if (wKeepRate != pDevice->wCurrentRate) {
-	bScheduleCommand((void *) pDevice, WLAN_CMD_SETPOWER, NULL);
-    }
-
-    if (pDevice->wCurrentRate <= RATE_11M)
-        byPktType = PK_TYPE_11B;
-
-    BytesToWrite = uDataLen + ETH_FCS_LEN;
-
-    // Convert the packet to an usb frame and copy into our buffer
-    // and send the irp.
-
-	pTX_Buffer = (struct vnt_tx_buffer *)&pContext->Data[0];
-
-    fConvertedPacket = s_bPacketToWirelessUsb(pDevice, byPktType,
-			pTX_Buffer, bNeedEncryption,
-                         uDataLen, TYPE_AC0DMA, &pDevice->sTxEthHeader,
-                         pbySkbData, pTransmitKey, uNodeIndex,
-                         pDevice->wCurrentRate,
-                         &uHeaderLen, &BytesToWrite
-                        );
-
-    if (fConvertedPacket == false) {
-        pContext->bBoolInUse = false;
-        return false;
-    }
-
-    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 = (u16)BytesToWrite + 4 ; //USB header
-
-    s_vSaveTxPktInfo(pDevice, (u8)(pTX_Buffer->byPKTNO & 0x0F),
-		&pDevice->sTxEthHeader.h_dest[0],
-		(u16)(BytesToWrite - uHeaderLen),
-		pTX_Buffer->fifo_head.wFIFOCtl);
+	if (status != STATUS_PENDING) {
+		pContext->in_use = false;
+		dev_kfree_skb_irq(skb);
+		return STATUS_FAILURE;
+	}
 
-    status = PIPEnsSendBulkOut(pDevice,pContext);
 
-    return true;
+	return 0;
 }
-
diff --git a/drivers/staging/vt6656/rxtx.h b/drivers/staging/vt6656/rxtx.h
index 6d6539d29d0..6db3337f1d1 100644
--- a/drivers/staging/vt6656/rxtx.h
+++ b/drivers/staging/vt6656/rxtx.h
@@ -217,15 +217,15 @@ union vnt_tx_head {
 struct vnt_tx_fifo_head {
 	u32 adwTxKey[4];
 	u16 wFIFOCtl;
-	u16 wTimeStamp;
+	__le16 time_stamp;
 	u16 wFragCtl;
-	u16 wReserved;
+	__le16 current_rate;
 } __packed;
 
 struct vnt_tx_buffer {
 	u8 byType;
 	u8 byPKTNO;
-	u16 wTxByteCount;
+	__le16 tx_byte_count;
 	struct vnt_tx_fifo_head fifo_head;
 	union vnt_tx_head tx_head;
 } __packed;
@@ -241,16 +241,14 @@ struct vnt_tx_short_buf_head {
 struct vnt_beacon_buffer {
 	u8 byType;
 	u8 byPKTNO;
-	u16 wTxByteCount;
+	__le16 tx_byte_count;
 	struct vnt_tx_short_buf_head short_head;
 	struct ieee80211_hdr hdr;
 } __packed;
 
 void vDMA0_tx_80211(struct vnt_private *, struct sk_buff *skb);
-int nsDMA_tx_packet(struct vnt_private *, u32 uDMAIdx, struct sk_buff *skb);
+int nsDMA_tx_packet(struct vnt_private *, struct sk_buff *skb);
 CMD_STATUS csMgmt_xmit(struct vnt_private *, struct vnt_tx_mgmt *);
 CMD_STATUS csBeacon_xmit(struct vnt_private *, struct vnt_tx_mgmt *);
-int bRelayPacketSend(struct vnt_private *, u8 *pbySkbData, u32 uDataLen,
-	u32 uNodeIndex);
 
 #endif /* __RXTX_H__ */
diff --git a/drivers/staging/vt6656/srom.h b/drivers/staging/vt6656/srom.h
deleted file mode 100644
index 488192d79ac..00000000000
--- a/drivers/staging/vt6656/srom.h
+++ /dev/null
@@ -1,112 +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: srom.h
- *
- * Purpose: Implement functions to access eeprom
- *
- * Author: Jerry Chen
- *
- * Date: Jan 29, 2003
- *
- */
-
-#ifndef __SROM_H__
-#define __SROM_H__
-
-#define EEP_MAX_CONTEXT_SIZE    256
-
-#define CB_EEPROM_READBYTE_WAIT 900     //us
-
-#define W_MAX_I2CRETRY          0x0fff
-
-//
-// Contents in the EEPROM
-//
-#define EEP_OFS_PAR         0x00        // physical address
-#define EEP_OFS_ANTENNA     0x17
-#define EEP_OFS_RADIOCTL    0x18
-#define EEP_OFS_RFTYPE      0x1B        // for select RF
-#define EEP_OFS_MINCHANNEL  0x1C        // Min Channel #
-#define EEP_OFS_MAXCHANNEL  0x1D        // Max Channel #
-#define EEP_OFS_SIGNATURE   0x1E        //
-#define EEP_OFS_ZONETYPE    0x1F        //
-#define EEP_OFS_RFTABLE     0x20        // RF POWER TABLE
-#define EEP_OFS_PWR_CCK     0x20
-#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
-
-#define EEP_OFS_MAJOR_VER 0x2E
-#define EEP_OFS_MINOR_VER 0x2F
-
-#define EEP_OFS_CCK_PWR_TBL     0x30
-#define EEP_OFS_OFDM_PWR_TBL    0x40
-#define EEP_OFS_OFDMA_PWR_TBL   0x50
-
-//
-// Bits in EEP_OFS_ANTENNA
-//
-#define EEP_ANTENNA_MAIN    0x01
-#define EEP_ANTENNA_AUX     0x02
-#define EEP_ANTINV          0x04
-
-//
-// Bits in EEP_OFS_RADIOCTL
-//
-#define EEP_RADIOCTL_ENABLE 0x80
-
-// AT24C02 eeprom contents
-//      2048 bits = 256 bytes = 128 words
-//
-typedef struct tagSSromReg {
-    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;
-
-#endif /* __EEPROM_H__ */
diff --git a/drivers/staging/vt6656/tcrc.c b/drivers/staging/vt6656/tcrc.c
deleted file mode 100644
index 7229f26c052..00000000000
--- a/drivers/staging/vt6656/tcrc.c
+++ /dev/null
@@ -1,183 +0,0 @@
-/*
- * Copyright (c) 2003 VIA Networking, 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: tcrc.c
- *
- * Purpose: Implement functions to calculate CRC
- *
- * Author: Tevin Chen
- *
- * Date: May 21, 1996
- *
- * Functions:
- *      CRCdwCrc32 -
- *      CRCdwGetCrc32 -
- *      CRCdwGetCrc32Ex -
- *
- * Revision History:
- *
- */
-
-#include "tcrc.h"
-
-/* 32-bit CRC table */
-static const u32 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
-};
-
-/*+
- *
- * Description:
- *    Generate a CRC-32 from the data stream
- *
- * Parameters:
- *  In:
- *      pbyData     - the data stream
- *      cbByte      - the length of the stream
- *      dwCrcSeed   - Seed for CRC32
- *  Out:
- *      none
- *
- * Return Value: CRC-32
- *
--*/
-u32 CRCdwCrc32(u8 * pbyData, unsigned int cbByte, u32 dwCrcSeed)
-{
-	u32 dwCrc;
-
-	dwCrc = dwCrcSeed;
-	while (cbByte--) {
-		dwCrc = s_adwCrc32Table[(u8)((dwCrc ^ (*pbyData)) & 0xFF)] ^
-			(dwCrc >> 8);
-		pbyData++;
-	}
-
-	return dwCrc;
-}
-
-/*+
- *
- * Description:
- * To test CRC generator, input 8 bytes packet
- *      -- 0xff 0xff 0xff 0xff 0x00 0x00 0x00 0x00
- * the generated CRC should be
- *      -- 0xff 0xff 0xff 0xff
- *
- * Parameters:
- *  In:
- *      pbyData     - the data stream
- *      cbByte      - the length of the stream
- *  Out:
- *      none
- *
- * Return Value: CRC-32
- *
--*/
-u32 CRCdwGetCrc32(u8 * pbyData, unsigned int cbByte)
-{
-    return ~CRCdwCrc32(pbyData, cbByte, 0xFFFFFFFFL);
-}
-
-/*+
- *
- * Description:
- *
- * NOTE.... Because CRCdwGetCrc32Ex() is an iteration function,
- *          this means we will use the output of CRCdwGetCrc32Ex()
- *          to be a new argument to do next CRCdwGetCrc32Ex() calculation.
- *          Thus, the final result must be inverted to be the
- *          correct answer.
- *
- * Parameters:
- *  In:
- *      pbyData     - the data stream
- *      cbByte      - the length of the stream
- *  Out:
- *      none
- *
- * Return Value: CRC-32
- *
--*/
-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
deleted file mode 100644
index 5b1f368964e..00000000000
--- a/drivers/staging/vt6656/tcrc.h
+++ /dev/null
@@ -1,38 +0,0 @@
-/*
- * Copyright (c) 2003 VIA Networking, 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: tcrc.h
- *
- * Purpose: Implement functions to calculate CRC
- *
- * Author: Tevin Chen
- *
- * Date: Jan. 28, 1997
- *
- */
-
-#ifndef __TCRC_H__
-#define __TCRC_H__
-
-#include <linux/types.h>
-
-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 1db1e8419a9..2ef54f608cb 100644
--- a/drivers/staging/vt6656/tether.c
+++ b/drivers/staging/vt6656/tether.c
@@ -33,7 +33,6 @@
 
 #include "device.h"
 #include "tmacro.h"
-#include "tcrc.h"
 #include "tether.h"
 
 /*
@@ -51,11 +50,11 @@
  */
 bool ETHbIsBufferCrc32Ok(u8 * pbyBuffer, unsigned int cbFrameLength)
 {
-	u32 dwCRC;
+	u32 n_crc = ~ether_crc_le(cbFrameLength - 4, pbyBuffer);
 
-	dwCRC = CRCdwGetCrc32(pbyBuffer, cbFrameLength - 4);
-	if (cpu_to_le32(*((u32 *)(pbyBuffer + cbFrameLength - 4))) != dwCRC)
+	if (le32_to_cpu(*((__le32 *)(pbyBuffer + cbFrameLength - 4))) != n_crc)
 		return false;
+
 	return true;
 }
 
diff --git a/drivers/staging/vt6656/tether.h b/drivers/staging/vt6656/tether.h
index aec6b568a4a..f57fcfdc24d 100644
--- a/drivers/staging/vt6656/tether.h
+++ b/drivers/staging/vt6656/tether.h
@@ -60,8 +60,6 @@
 #define TYPE_DATE_NULL      0x4800
 
 #define TYPE_CTL_PSPOLL     0xa400
-#define TYPE_CTL_RTS        0xb400
-#define TYPE_CTL_CTS        0xc400
 #define TYPE_CTL_ACK        0xd400
 
 #else //if LITTLE_ENDIAN
@@ -91,8 +89,6 @@
 #define TYPE_DATE_NULL      0x0048
 
 #define TYPE_CTL_PSPOLL     0x00a4
-#define TYPE_CTL_RTS        0x00b4
-#define TYPE_CTL_CTS        0x00c4
 #define TYPE_CTL_ACK        0x00d4
 
 #endif //#ifdef __BIG_ENDIAN
diff --git a/drivers/staging/vt6656/usbpipe.c b/drivers/staging/vt6656/usbpipe.c
index c5838d99f89..e4751b71e4d 100644
--- a/drivers/staging/vt6656/usbpipe.c
+++ b/drivers/staging/vt6656/usbpipe.c
@@ -26,10 +26,10 @@
  * Date: Mar. 29, 2005
  *
  * Functions:
- *      CONTROLnsRequestOut - Write variable length bytes to MEM/BB/MAC/EEPROM
- *      CONTROLnsRequestIn - Read variable length bytes from MEM/BB/MAC/EEPROM
- *      ControlvWriteByte - Write one byte to MEM/BB/MAC/EEPROM
- *      ControlvReadByte - Read one byte from MEM/BB/MAC/EEPROM
+ *	vnt_control_out - Write variable length bytes to MEM/BB/MAC/EEPROM
+ *	vnt_control_in - Read variable length bytes from MEM/BB/MAC/EEPROM
+ *	vnt_control_out_u8 - Write one byte to MEM/BB/MAC/EEPROM
+ *	vnt_control_in_u8 - Read one byte from MEM/BB/MAC/EEPROM
  *      ControlvMaskByte - Read one byte from MEM/BB/MAC/EEPROM and clear/set some bits in the same address
  *
  * Revision History:
@@ -41,9 +41,9 @@
 #include "int.h"
 #include "rxtx.h"
 #include "dpc.h"
-#include "control.h"
 #include "desc.h"
 #include "device.h"
+#include "usbpipe.h"
 
 //endpoint def
 //endpoint 0: control
@@ -51,9 +51,6 @@
 //endpoint 2: read bulk
 //endpoint 3: write bulk
 
-//static int          msglevel                =MSG_LEVEL_DEBUG;
-static int          msglevel                =MSG_LEVEL_INFO;
-
 #define USB_CTL_WAIT   500 //ms
 
 #ifndef URB_ASYNC_UNLINK
@@ -63,226 +60,61 @@ static int          msglevel                =MSG_LEVEL_INFO;
 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);
 
-int PIPEnsControlOutAsyn(struct vnt_private *pDevice, u8 byRequest,
-	u16 wValue, u16 wIndex, u16 wLength, u8 *pbyBuffer)
+int vnt_control_out(struct vnt_private *priv, u8 request, u16 value,
+		u16 index, u16 length, u8 *buffer)
 {
-	int ntStatus;
-
-    if (pDevice->Flags & fMP_DISCONNECTED)
-        return STATUS_FAILURE;
-
-    if (pDevice->Flags & fMP_CONTROL_WRITES)
-        return STATUS_FAILURE;
-
-    if (in_interrupt()) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"in_interrupt return ..byRequest %x\n", byRequest);
-        return STATUS_FAILURE;
-    }
-
-    ntStatus = usb_control_msg(
-                            pDevice->usb,
-                            usb_sndctrlpipe(pDevice->usb , 0),
-                            byRequest,
-                            0x40, // RequestType
-                            wValue,
-                            wIndex,
-			    (void *) pbyBuffer,
-                            wLength,
-                            HZ
-                          );
-    if (ntStatus >= 0) {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"usb_sndctrlpipe ntStatus= %d\n", ntStatus);
-        ntStatus = 0;
-    } else {
-        DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"usb_sndctrlpipe fail, ntStatus= %d\n", ntStatus);
-    }
-
-    return ntStatus;
-}
-
-int PIPEnsControlOut(struct vnt_private *pDevice, u8 byRequest, u16 wValue,
-		u16 wIndex, u16 wLength, u8 *pbyBuffer)
-		__releases(&pDevice->lock)
-		__acquires(&pDevice->lock)
-{
-	int ntStatus = 0;
-	int ii;
-
-    if (pDevice->Flags & fMP_DISCONNECTED)
-        return STATUS_FAILURE;
-
-    if (pDevice->Flags & fMP_CONTROL_WRITES)
-        return STATUS_FAILURE;
+	int status = 0;
 
-	if (pDevice->Flags & fMP_CONTROL_READS)
+	if (priv->Flags & fMP_DISCONNECTED)
 		return STATUS_FAILURE;
 
-	if (pDevice->pControlURB->hcpriv)
-		return STATUS_FAILURE;
+	mutex_lock(&priv->usb_lock);
 
-	MP_SET_FLAG(pDevice, fMP_CONTROL_WRITES);
-
-	pDevice->sUsbCtlRequest.bRequestType = 0x40;
-	pDevice->sUsbCtlRequest.bRequest = byRequest;
-	pDevice->sUsbCtlRequest.wValue = cpu_to_le16p(&wValue);
-	pDevice->sUsbCtlRequest.wIndex = cpu_to_le16p(&wIndex);
-	pDevice->sUsbCtlRequest.wLength = cpu_to_le16p(&wLength);
-	pDevice->pControlURB->transfer_flags |= URB_ASYNC_UNLINK;
-    pDevice->pControlURB->actual_length = 0;
-    // Notice, pbyBuffer limited point to variable buffer, can't be constant.
-  	usb_fill_control_urb(pDevice->pControlURB, pDevice->usb,
-			 usb_sndctrlpipe(pDevice->usb , 0), (char *) &pDevice->sUsbCtlRequest,
-			 pbyBuffer, wLength, s_nsControlInUsbIoCompleteWrite, pDevice);
-
-	ntStatus = usb_submit_urb(pDevice->pControlURB, GFP_ATOMIC);
-	if (ntStatus != 0) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-			"control send request submission failed: %d\n",
-				ntStatus);
-		MP_CLEAR_FLAG(pDevice, fMP_CONTROL_WRITES);
-		return STATUS_FAILURE;
-	}
+	status = usb_control_msg(priv->usb,
+		usb_sndctrlpipe(priv->usb, 0), request, 0x40, value,
+			index, buffer, length, USB_CTL_WAIT);
 
-	spin_unlock_irq(&pDevice->lock);
-    for (ii = 0; ii <= USB_CTL_WAIT; ii ++) {
+	mutex_unlock(&priv->usb_lock);
 
-	if (pDevice->Flags & fMP_CONTROL_WRITES)
-		mdelay(1);
-        else
-		break;
+	if (status < (int)length)
+		return STATUS_FAILURE;
 
-        if (ii >= USB_CTL_WAIT) {
-		DBG_PRT(MSG_LEVEL_DEBUG,
-			KERN_INFO "control send request submission timeout\n");
-            spin_lock_irq(&pDevice->lock);
-            MP_CLEAR_FLAG(pDevice, fMP_CONTROL_WRITES);
-            return STATUS_FAILURE;
-        }
-    }
-	spin_lock_irq(&pDevice->lock);
-
-    return STATUS_SUCCESS;
+	return STATUS_SUCCESS;
 }
 
-int PIPEnsControlIn(struct vnt_private *pDevice, u8 byRequest, u16 wValue,
-	u16 wIndex, u16 wLength,  u8 *pbyBuffer)
-	__releases(&pDevice->lock)
-	__acquires(&pDevice->lock)
+void vnt_control_out_u8(struct vnt_private *priv, u8 reg, u8 reg_off, u8 data)
 {
-	int ntStatus = 0;
-	int ii;
-
-    if (pDevice->Flags & fMP_DISCONNECTED)
-        return STATUS_FAILURE;
-
-    if (pDevice->Flags & fMP_CONTROL_READS)
-	return STATUS_FAILURE;
-
-	if (pDevice->Flags & fMP_CONTROL_WRITES)
-		return STATUS_FAILURE;
+	vnt_control_out(priv, MESSAGE_TYPE_WRITE,
+					reg_off, reg, sizeof(u8), &data);
+}
 
-	if (pDevice->pControlURB->hcpriv)
-		return STATUS_FAILURE;
+int vnt_control_in(struct vnt_private *priv, u8 request, u16 value,
+		u16 index, u16 length, u8 *buffer)
+{
+	int status;
 
-	MP_SET_FLAG(pDevice, fMP_CONTROL_READS);
-
-	pDevice->sUsbCtlRequest.bRequestType = 0xC0;
-	pDevice->sUsbCtlRequest.bRequest = byRequest;
-	pDevice->sUsbCtlRequest.wValue = cpu_to_le16p(&wValue);
-	pDevice->sUsbCtlRequest.wIndex = cpu_to_le16p(&wIndex);
-	pDevice->sUsbCtlRequest.wLength = cpu_to_le16p(&wLength);
-	pDevice->pControlURB->transfer_flags |= URB_ASYNC_UNLINK;
-    pDevice->pControlURB->actual_length = 0;
-	usb_fill_control_urb(pDevice->pControlURB, pDevice->usb,
-			 usb_rcvctrlpipe(pDevice->usb , 0), (char *) &pDevice->sUsbCtlRequest,
-			 pbyBuffer, wLength, s_nsControlInUsbIoCompleteRead, pDevice);
-
-	ntStatus = usb_submit_urb(pDevice->pControlURB, GFP_ATOMIC);
-	if (ntStatus != 0) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-			"control request submission failed: %d\n", ntStatus);
-		MP_CLEAR_FLAG(pDevice, fMP_CONTROL_READS);
+	if (priv->Flags & fMP_DISCONNECTED)
 		return STATUS_FAILURE;
-	}
 
-	spin_unlock_irq(&pDevice->lock);
-    for (ii = 0; ii <= USB_CTL_WAIT; ii ++) {
+	mutex_lock(&priv->usb_lock);
 
-	if (pDevice->Flags & fMP_CONTROL_READS)
-		mdelay(1);
-	else
-		break;
-
-	if (ii >= USB_CTL_WAIT) {
-		DBG_PRT(MSG_LEVEL_DEBUG,
-			KERN_INFO "control rcv request submission timeout\n");
-            spin_lock_irq(&pDevice->lock);
-            MP_CLEAR_FLAG(pDevice, fMP_CONTROL_READS);
-            return STATUS_FAILURE;
-        }
-    }
-	spin_lock_irq(&pDevice->lock);
-
-    return ntStatus;
-}
+	status = usb_control_msg(priv->usb,
+		usb_rcvctrlpipe(priv->usb, 0), request, 0xc0, value,
+			index, buffer, length, USB_CTL_WAIT);
 
-static void s_nsControlInUsbIoCompleteWrite(struct urb *urb)
-{
-	struct vnt_private *pDevice = (struct vnt_private *)urb->context;
+	mutex_unlock(&priv->usb_lock);
 
-	pDevice = urb->context;
-	switch (urb->status) {
-	case 0:
-		break;
-	case -EINPROGRESS:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ctrl write urb status EINPROGRESS%d\n", urb->status);
-		break;
-	case -ENOENT:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ctrl write urb status ENOENT %d\n", urb->status);
-		break;
-	default:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ctrl write urb status %d\n", urb->status);
-	}
+	if (status < (int)length)
+		return STATUS_FAILURE;
 
-    MP_CLEAR_FLAG(pDevice, fMP_CONTROL_WRITES);
+	return STATUS_SUCCESS;
 }
 
-/*
- * Description:
- *      Complete function of usb Control callback
- *
- * Parameters:
- *  In:
- *      pDevice     - Pointer to the adapter
- *
- *  Out:
- *      none
- *
- * Return Value: STATUS_INSUFFICIENT_RESOURCES or result of IoCallDriver
- *
- */
-
-static void s_nsControlInUsbIoCompleteRead(struct urb *urb)
+void vnt_control_in_u8(struct vnt_private *priv, u8 reg, u8 reg_off, u8 *data)
 {
-	struct vnt_private *pDevice = (struct vnt_private *)urb->context;
-
-	switch (urb->status) {
-	case 0:
-		break;
-	case -EINPROGRESS:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ctrl read urb status EINPROGRESS%d\n", urb->status);
-		break;
-	case -ENOENT:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ctrl read urb status = ENOENT %d\n", urb->status);
-		break;
-	default:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ctrl read urb status %d\n", urb->status);
-	}
-
-    MP_CLEAR_FLAG(pDevice, fMP_CONTROL_READS);
+	vnt_control_in(priv, MESSAGE_TYPE_READ,
+			reg_off, reg, sizeof(u8), data);
 }
 
 /*
@@ -303,9 +135,6 @@ int PIPEnsInterruptRead(struct vnt_private *priv)
 {
 	int status = STATUS_FAILURE;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-			"---->s_nsStartInterruptUsbRead()\n");
-
 	if (priv->int_buf.in_use == true)
 		return STATUS_FAILURE;
 
@@ -322,14 +151,10 @@ int PIPEnsInterruptRead(struct vnt_private *priv)
 
 	status = usb_submit_urb(priv->pInterruptURB, GFP_ATOMIC);
 	if (status) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-			"Submit int URB failed %d\n", status);
+		dev_dbg(&priv->usb->dev, "Submit int URB failed %d\n", status);
 		priv->int_buf.in_use = false;
 	}
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-		"<----s_nsStartInterruptUsbRead Return(%x)\n", status);
-
 	return status;
 }
 
@@ -353,9 +178,6 @@ static void s_nsInterruptUsbIoCompleteRead(struct urb *urb)
 	struct vnt_private *priv = urb->context;
 	int status;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-			"---->s_nsInterruptUsbIoCompleteRead\n");
-
 	switch (urb->status) {
 	case 0:
 	case -ETIMEDOUT:
@@ -371,22 +193,17 @@ static void s_nsInterruptUsbIoCompleteRead(struct urb *urb)
 
 	status = urb->status;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-		"s_nsInterruptUsbIoCompleteRead Status %d\n", status);
-
 	if (status != STATUS_SUCCESS) {
 		priv->int_buf.in_use = false;
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-			"IntUSBIoCompleteControl STATUS = %d\n", status);
+		dev_dbg(&priv->usb->dev, "%s status = %d\n", __func__, status);
 	} else {
 		INTnsProcessData(priv);
 	}
 
 	status = usb_submit_urb(priv->pInterruptURB, GFP_ATOMIC);
 	if (status) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-			"Submit int URB failed %d\n", status);
+		dev_dbg(&priv->usb->dev, "Submit int URB failed %d\n", status);
 	} else {
 		priv->int_buf.in_use = true;
 	}
@@ -413,14 +230,12 @@ int PIPEnsBulkInUsbRead(struct vnt_private *priv, struct vnt_rcb *rcb)
 	int status = 0;
 	struct urb *urb;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---->s_nsStartBulkInUsbRead\n");
-
 	if (priv->Flags & fMP_DISCONNECTED)
 		return STATUS_FAILURE;
 
 	urb = rcb->pUrb;
 	if (rcb->skb == NULL) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"rcb->skb is null\n");
+		dev_dbg(&priv->usb->dev, "rcb->skb is null\n");
 		return status;
 	}
 
@@ -434,8 +249,7 @@ int PIPEnsBulkInUsbRead(struct vnt_private *priv, struct vnt_rcb *rcb)
 
 	status = usb_submit_urb(urb, GFP_ATOMIC);
 	if (status != 0) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-				"Submit Rx URB failed %d\n", status);
+		dev_dbg(&priv->usb->dev, "Submit Rx URB failed %d\n", status);
 		return STATUS_FAILURE ;
 	}
 
@@ -464,10 +278,9 @@ static void s_nsBulkInUsbIoCompleteRead(struct urb *urb)
 {
 	struct vnt_rcb *rcb = urb->context;
 	struct vnt_private *priv = rcb->pDevice;
+	unsigned long flags;
 	int re_alloc_skb = false;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---->s_nsBulkInUsbIoCompleteRead\n");
-
 	switch (urb->status) {
 	case 0:
 		break;
@@ -477,29 +290,29 @@ static void s_nsBulkInUsbIoCompleteRead(struct urb *urb)
 		return;
 	case -ETIMEDOUT:
 	default:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-				"BULK In failed %d\n", urb->status);
+		dev_dbg(&priv->usb->dev, "BULK In failed %d\n", urb->status);
 		break;
 	}
 
 	if (urb->actual_length) {
-		spin_lock(&priv->lock);
+		spin_lock_irqsave(&priv->lock, flags);
 
 		if (RXbBulkInProcessData(priv, rcb, urb->actual_length) == true)
 			re_alloc_skb = true;
 
-		spin_unlock(&priv->lock);
+		spin_unlock_irqrestore(&priv->lock, flags);
 	}
 
 	rcb->Ref--;
 	if (rcb->Ref == 0) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"RxvFreeNormal %d\n",
-							priv->NumRecvFreeList);
-		spin_lock(&priv->lock);
+		dev_dbg(&priv->usb->dev,
+				"RxvFreeNormal %d\n", priv->NumRecvFreeList);
+
+		spin_lock_irqsave(&priv->lock, flags);
 
 		RXvFreeRCB(rcb, re_alloc_skb);
 
-		spin_unlock(&priv->lock);
+		spin_unlock_irqrestore(&priv->lock, flags);
 	}
 
 	return;
@@ -527,28 +340,26 @@ int PIPEnsSendBulkOut(struct vnt_private *priv,
 
 	priv->bPWBitOn = false;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"s_nsSendBulkOut\n");
-
 	if (!(MP_IS_READY(priv) && priv->Flags & fMP_POST_WRITES)) {
-		context->bBoolInUse = false;
+		context->in_use = false;
 		return STATUS_RESOURCES;
 	}
 
-	urb = context->pUrb;
+	urb = context->urb;
 
 	usb_fill_bulk_urb(urb,
 			priv->usb,
 			usb_sndbulkpipe(priv->usb, 3),
-			context->Data,
-			context->uBufLen,
+			context->data,
+			context->buf_len,
 			s_nsBulkOutIoCompleteWrite,
 			context);
 
 	status = usb_submit_urb(urb, GFP_ATOMIC);
 	if (status != 0) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-				"Submit Tx URB failed %d\n", status);
-		context->bBoolInUse = false;
+		dev_dbg(&priv->usb->dev, "Submit Tx URB failed %d\n", status);
+
+		context->in_use = false;
 		return STATUS_FAILURE;
 	}
 
@@ -586,25 +397,21 @@ int PIPEnsSendBulkOut(struct vnt_private *priv,
 static void s_nsBulkOutIoCompleteWrite(struct urb *urb)
 {
 	struct vnt_usb_send_context *context = urb->context;
-	struct vnt_private *priv = context->pDevice;
+	struct vnt_private *priv = context->priv;
 	u8 context_type = context->type;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---->s_nsBulkOutIoCompleteWrite\n");
-
 	switch (urb->status) {
 	case 0:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-			"Write %d bytes\n", context->uBufLen);
+		dev_dbg(&priv->usb->dev, "Write %d bytes\n", context->buf_len);
 		break;
 	case -ECONNRESET:
 	case -ENOENT:
 	case -ESHUTDOWN:
-		context->bBoolInUse = false;
+		context->in_use = false;
 		return;
 	case -ETIMEDOUT:
 	default:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-				"BULK Out failed %d\n", urb->status);
+		dev_dbg(&priv->usb->dev, "BULK Out failed %d\n", urb->status);
 		break;
 	}
 
@@ -612,11 +419,11 @@ static void s_nsBulkOutIoCompleteWrite(struct urb *urb)
 		return;
 
 	if (CONTEXT_DATA_PACKET == context_type) {
-		if (context->pPacket != NULL) {
-			dev_kfree_skb_irq(context->pPacket);
-			context->pPacket = NULL;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-				"tx  %d bytes\n", context->uBufLen);
+		if (context->skb != NULL) {
+			dev_kfree_skb_irq(context->skb);
+			context->skb = NULL;
+			dev_dbg(&priv->usb->dev,
+					"tx  %d bytes\n", context->buf_len);
 		}
 
 		priv->dev->trans_start = jiffies;
@@ -627,7 +434,7 @@ static void s_nsBulkOutIoCompleteWrite(struct urb *urb)
 			netif_wake_queue(priv->dev);
 	}
 
-	context->bBoolInUse = false;
+	context->in_use = false;
 
 	return;
 }
diff --git a/drivers/staging/vt6656/usbpipe.h b/drivers/staging/vt6656/usbpipe.h
index f53770329e7..ea71782d890 100644
--- a/drivers/staging/vt6656/usbpipe.h
+++ b/drivers/staging/vt6656/usbpipe.h
@@ -32,12 +32,11 @@
 
 #include "device.h"
 
-int PIPEnsControlOut(struct vnt_private *, u8 byRequest, u16 wValue,
-		u16 wIndex, u16 wLength, u8 *pbyBuffer);
-int PIPEnsControlOutAsyn(struct vnt_private *, u8 byRequest,
-	u16 wValue, u16 wIndex, u16 wLength, u8 *pbyBuffer);
-int PIPEnsControlIn(struct vnt_private *, u8 byRequest, u16 wValue,
-	u16 wIndex, u16 wLength,  u8 *pbyBuffer);
+int vnt_control_out(struct vnt_private *, u8, u16, u16, u16, u8 *);
+int vnt_control_in(struct vnt_private *, u8, u16, u16, u16,  u8 *);
+
+void vnt_control_out_u8(struct vnt_private *, u8, u8, u8);
+void vnt_control_in_u8(struct vnt_private *, u8, u8, u8 *);
 
 int PIPEnsInterruptRead(struct vnt_private *);
 int PIPEnsBulkInUsbRead(struct vnt_private *, struct vnt_rcb *pRCB);
diff --git a/drivers/staging/vt6656/wcmd.c b/drivers/staging/vt6656/wcmd.c
index 3cf3f24247a..da72d4df6fc 100644
--- a/drivers/staging/vt6656/wcmd.c
+++ b/drivers/staging/vt6656/wcmd.c
@@ -48,10 +48,9 @@
 #include "power.h"
 #include "wctl.h"
 #include "baseband.h"
-#include "control.h"
+#include "usbpipe.h"
 #include "rxtx.h"
 #include "rf.h"
-#include "rndis.h"
 #include "channel.h"
 #include "iowpa.h"
 
@@ -279,6 +278,7 @@ void vRunCommand(struct work_struct *work)
 	int ii;
 	u8 byMask[8] = {1, 2, 4, 8, 0x10, 0x20, 0x40, 0x80};
 	u8 byData;
+	unsigned long flags;
 
 	if (pDevice->Flags & fMP_DISCONNECTED)
 		return;
@@ -286,8 +286,6 @@ void vRunCommand(struct work_struct *work)
 	if (pDevice->bCmdRunning != true)
 		return;
 
-	spin_lock_irq(&pDevice->lock);
-
 	switch (pDevice->eCommandState) {
 
 	case WLAN_CMD_SCAN_START:
@@ -346,11 +344,10 @@ void vRunCommand(struct work_struct *work)
 			CARDbSetMediaChannel(pDevice, pMgmt->uScanChannel);
 			// Set Baseband to be more sensitive.
 
-			if (pDevice->bUpdateBBVGA) {
-				BBvSetShortSlotTime(pDevice);
-				BBvSetVGAGainOffset(pDevice, pDevice->abyBBVGA[0]);
-				BBvUpdatePreEDThreshold(pDevice, true);
-			}
+			BBvSetShortSlotTime(pDevice);
+			BBvSetVGAGainOffset(pDevice, pDevice->abyBBVGA[0]);
+			BBvUpdatePreEDThreshold(pDevice, true);
+
 			pMgmt->uScanChannel++;
 
 			while (!ChannelValid(pDevice->byZoneType, pMgmt->uScanChannel) &&
@@ -365,11 +362,9 @@ void vRunCommand(struct work_struct *work)
 			if ((pMgmt->b11hEnable == false) ||
 			    (pMgmt->uScanChannel < CB_MAX_CHANNEL_24G)) {
 				s_vProbeChannel(pDevice);
-				spin_unlock_irq(&pDevice->lock);
 				vCommandTimerWait((void *) pDevice, 100);
 				return;
 			} else {
-				spin_unlock_irq(&pDevice->lock);
 				vCommandTimerWait((void *) pDevice, WCMD_PASSIVE_SCAN_TIME);
 				return;
 			}
@@ -385,11 +380,9 @@ void vRunCommand(struct work_struct *work)
 			CARDvSetBSSMode(pDevice);
 		}
 
-		if (pDevice->bUpdateBBVGA) {
-			BBvSetShortSlotTime(pDevice);
-			BBvSetVGAGainOffset(pDevice, pDevice->byBBVGACurrent);
-			BBvUpdatePreEDThreshold(pDevice, false);
-		}
+		BBvSetShortSlotTime(pDevice);
+		BBvSetVGAGainOffset(pDevice, pDevice->byBBVGACurrent);
+		BBvUpdatePreEDThreshold(pDevice, false);
 
 		// Set channel back
 		vAdHocBeaconRestart(pDevice);
@@ -433,7 +426,9 @@ void vRunCommand(struct work_struct *work)
 					     (8),
 					     &Status);
 			pDevice->bLinkPass = false;
-			ControlvMaskByte(pDevice, MESSAGE_REQUEST_MACREG, MAC_REG_PAPEDELAY, LEDSTS_STS, LEDSTS_SLOW);
+
+			vnt_mac_set_led(pDevice, LEDSTS_STS, LEDSTS_SLOW);
+
 			// unlock command busy
 			pItemSSID = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
 			pItemSSID->len = 0;
@@ -479,7 +474,8 @@ void vRunCommand(struct work_struct *work)
 			}
 			netif_stop_queue(pDevice->dev);
 			pDevice->bLinkPass = false;
-			ControlvMaskByte(pDevice, MESSAGE_REQUEST_MACREG, MAC_REG_PAPEDELAY, LEDSTS_STS, LEDSTS_SLOW);
+
+			vnt_mac_set_led(pDevice, LEDSTS_STS, LEDSTS_SLOW);
 		}
 		// set initial state
 		pMgmt->eCurrState = WMAC_STATE_IDLE;
@@ -504,7 +500,6 @@ void vRunCommand(struct work_struct *work)
 				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;
 			}
@@ -515,7 +510,9 @@ void vRunCommand(struct work_struct *work)
 				if (netif_queue_stopped(pDevice->dev))
 					netif_wake_queue(pDevice->dev);
 				pDevice->bLinkPass = true;
-				ControlvMaskByte(pDevice, MESSAGE_REQUEST_MACREG, MAC_REG_PAPEDELAY, LEDSTS_STS, LEDSTS_INTER);
+
+				vnt_mac_set_led(pDevice, LEDSTS_STS, LEDSTS_INTER);
+
 				pMgmt->sNodeDBTable[0].bActive = true;
 				pMgmt->sNodeDBTable[0].uInActiveCount = 0;
 			} else {
@@ -546,7 +543,6 @@ void vRunCommand(struct work_struct *work)
 				s_bClearBSSID_SCAN(pDevice);
 /*
 				pDevice->bLinkPass = true;
-				ControlvMaskByte(pDevice,MESSAGE_REQUEST_MACREG,MAC_REG_PAPEDELAY,LEDSTS_STS,LEDSTS_INTER);
 				if (netif_queue_stopped(pDevice->dev)){
 					netif_wake_queue(pDevice->dev);
 				}
@@ -578,7 +574,6 @@ void vRunCommand(struct work_struct *work)
 				pDevice->byLinkWaitCount = 0;
 				pDevice->eCommandState = WLAN_ASSOCIATE_WAIT;
 				vCommandTimerWait((void *) pDevice, ASSOCIATE_TIMEOUT);
-				spin_unlock_irq(&pDevice->lock);
 				return;
 			}
 		} else if (pMgmt->eCurrState < WMAC_STATE_AUTHPENDING) {
@@ -587,7 +582,6 @@ void vRunCommand(struct work_struct *work)
 			//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;
 		}
@@ -610,7 +604,9 @@ void vRunCommand(struct work_struct *work)
 			pDevice->byLinkWaitCount = 0;
 			pDevice->byReAssocCount = 0;
 			pDevice->bLinkPass = true;
-			ControlvMaskByte(pDevice, MESSAGE_REQUEST_MACREG, MAC_REG_PAPEDELAY, LEDSTS_STS, LEDSTS_INTER);
+
+			vnt_mac_set_led(pDevice, LEDSTS_STS, LEDSTS_INTER);
+
 			s_bClearBSSID_SCAN(pDevice);
 
 			if (netif_queue_stopped(pDevice->dev))
@@ -622,7 +618,6 @@ void vRunCommand(struct work_struct *work)
 			//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;
 		}
@@ -637,11 +632,11 @@ void vRunCommand(struct work_struct *work)
 			pMgmt->eCurrState = WMAC_STATE_IDLE;
 			pMgmt->eCurrMode = WMAC_MODE_STANDBY;
 			pDevice->bLinkPass = false;
-			ControlvMaskByte(pDevice, MESSAGE_REQUEST_MACREG, MAC_REG_PAPEDELAY, LEDSTS_STS, LEDSTS_SLOW);
-			if (pDevice->bEnableHostWEP == true)
-				BSSvClearNodeDBTable(pDevice, 1);
-			else
-				BSSvClearNodeDBTable(pDevice, 0);
+
+			vnt_mac_set_led(pDevice, LEDSTS_STS, LEDSTS_SLOW);
+
+			BSSvClearNodeDBTable(pDevice, 0);
+
 			pDevice->uAssocCount = 0;
 			pMgmt->eCurrState = WMAC_STATE_IDLE;
 			pDevice->bFixRate = false;
@@ -659,7 +654,9 @@ void vRunCommand(struct work_struct *work)
 			if (netif_queue_stopped(pDevice->dev))
 				netif_wake_queue(pDevice->dev);
 			pDevice->bLinkPass = true;
-			ControlvMaskByte(pDevice, MESSAGE_REQUEST_MACREG, MAC_REG_PAPEDELAY, LEDSTS_STS, LEDSTS_INTER);
+
+			vnt_mac_set_led(pDevice, LEDSTS_STS, LEDSTS_INTER);
+
 			schedule_delayed_work(&pDevice->second_callback_work, HZ);
 		}
 		break;
@@ -675,9 +672,13 @@ void vRunCommand(struct work_struct *work)
 					pDevice->bMoreData = true;
 				}
 
-				if (nsDMA_tx_packet(pDevice, TYPE_AC0DMA, skb) != 0)
+				spin_lock_irqsave(&pDevice->lock, flags);
+
+				if (nsDMA_tx_packet(pDevice, skb) != 0)
 					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Multicast ps tx fail\n");
 
+				spin_unlock_irqrestore(&pDevice->lock, flags);
+
 				pMgmt->sNodeDBTable[0].wEnQueueCnt--;
 			}
 		}
@@ -698,9 +699,13 @@ void vRunCommand(struct work_struct *work)
 						pDevice->bMoreData = true;
 					}
 
-					if (nsDMA_tx_packet(pDevice, TYPE_AC0DMA, skb) != 0)
+					spin_lock_irqsave(&pDevice->lock, flags);
+
+					if (nsDMA_tx_packet(pDevice, skb) != 0)
 						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "sta ps tx fail\n");
 
+					spin_unlock_irqrestore(&pDevice->lock, flags);
+
 					pMgmt->sNodeDBTable[ii].wEnQueueCnt--;
 					// check if sta ps enable, wait next pspoll
 					// if sta ps disable, send all pending buffers.
@@ -729,7 +734,7 @@ void vRunCommand(struct work_struct *work)
 			int ntStatus = STATUS_SUCCESS;
 			u8            byTmp;
 
-			ntStatus = CONTROLnsRequestIn(pDevice,
+			ntStatus = vnt_control_in(pDevice,
 					MESSAGE_TYPE_READ,
 					MAC_REG_GPIOCTL1,
 					MESSAGE_REQUEST_MACREG,
@@ -752,9 +757,9 @@ void vRunCommand(struct work_struct *work)
 
 				pDevice->byKeyIndex = 0;
 				pDevice->bTransmitKey = false;
-				spin_unlock_irq(&pDevice->lock);
+
 				KeyvInitTable(pDevice, &pDevice->sKey);
-				spin_lock_irq(&pDevice->lock);
+
 				pMgmt->byCSSPK = KEY_CTL_NONE;
 				pMgmt->byCSSGK = KEY_CTL_NONE;
 
@@ -795,14 +800,17 @@ void vRunCommand(struct work_struct *work)
 				netif_stop_queue(pDevice->dev);
 				CARDbRadioPowerOff(pDevice);
 				MACvRegBitsOn(pDevice, MAC_REG_GPIOCTL1, GPIO3_INTMD);
-				ControlvMaskByte(pDevice, MESSAGE_REQUEST_MACREG, MAC_REG_PAPEDELAY, LEDSTS_STS, LEDSTS_OFF);
+
+				vnt_mac_set_led(pDevice, LEDSTS_STS, LEDSTS_OFF);
+
 				pDevice->bHWRadioOff = true;
 			} else {
 				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO" WLAN_CMD_RADIO_START_ON........................\n");
 				pDevice->bHWRadioOff = false;
 				CARDbRadioPowerOn(pDevice);
 				MACvRegBitsOff(pDevice, MAC_REG_GPIOCTL1, GPIO3_INTMD);
-				ControlvMaskByte(pDevice, MESSAGE_REQUEST_MACREG, MAC_REG_PAPEDELAY, LEDSTS_STS, LEDSTS_ON);
+
+				vnt_mac_set_led(pDevice, LEDSTS_STS, LEDSTS_ON);
 			}
 		}
 
@@ -827,7 +835,8 @@ void vRunCommand(struct work_struct *work)
 
 	case WLAN_CMD_SETPOWER_START:
 
-		RFbSetPower(pDevice, pDevice->wCurrentRate, pMgmt->uCurrChannel);
+		vnt_rf_setpower(pDevice, pDevice->wCurrentRate,
+							pMgmt->uCurrChannel);
 
 		break;
 
@@ -853,10 +862,10 @@ void vRunCommand(struct work_struct *work)
 		break;
 
 	case WLAN_CMD_MAC_DISPOWERSAVING_START:
-		ControlvReadByte(pDevice, MESSAGE_REQUEST_MACREG, MAC_REG_PSCTL, &byData);
+		vnt_control_in_u8(pDevice, MESSAGE_REQUEST_MACREG, MAC_REG_PSCTL, &byData);
 		if ((byData & PSCTL_PS) != 0) {
 			// disable power saving hw function
-			CONTROLnsRequestOut(pDevice,
+			vnt_control_out(pDevice,
 					MESSAGE_TYPE_DISABLE_PS,
 					0,
 					0,
@@ -882,7 +891,6 @@ void vRunCommand(struct work_struct *work)
 
 	s_bCommandComplete(pDevice);
 
-	spin_unlock_irq(&pDevice->lock);
 	return;
 }
 
diff --git a/drivers/staging/vt6656/wctl.c b/drivers/staging/vt6656/wctl.c
index 814342cd948..efdc5d5d38e 100644
--- a/drivers/staging/vt6656/wctl.c
+++ b/drivers/staging/vt6656/wctl.c
@@ -70,7 +70,7 @@ bool WCTLbIsDuplicate (PSCache pCache, struct ieee80211_hdr *pMACHeader)
             pCacheEntry = &(pCache->asCacheEntry[uIndex]);
             if ((pCacheEntry->wFmSequence == pMACHeader->seq_ctrl) &&
 		ether_addr_equal(pCacheEntry->abyAddr2, pMACHeader->addr2) &&
-                (LOBYTE(pCacheEntry->wFrameCtl) == LOBYTE(pMACHeader->frame_control))
+		(pCacheEntry->wFrameCtl == pMACHeader->frame_control)
                 ) {
                 /* Duplicate match */
                 return true;
diff --git a/drivers/staging/vt6656/wmgr.c b/drivers/staging/vt6656/wmgr.c
index 0d69719a742..18723eab93d 100644
--- a/drivers/staging/vt6656/wmgr.c
+++ b/drivers/staging/vt6656/wmgr.c
@@ -78,8 +78,7 @@
 #include "wpa.h"
 #include "rf.h"
 #include "iowpa.h"
-#include "control.h"
-#include "rndis.h"
+#include "usbpipe.h"
 
 static int msglevel = MSG_LEVEL_INFO;
 //static int          msglevel                =MSG_LEVEL_DEBUG;
@@ -541,10 +540,6 @@ static void s_vMgrRxAssocRequest(struct vnt_private *pDevice,
                   (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) {
@@ -690,9 +685,6 @@ static void s_vMgrRxReAssocRequest(struct vnt_private *pDevice,
 
     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");
@@ -764,7 +756,8 @@ static void s_vMgrRxAssocResponse(struct vnt_private *pDevice,
             pItemSSID = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
             DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "Link with AP(SSID): %s\n", pItemSSID->abySSID);
             pDevice->bLinkPass = true;
-            ControlvMaskByte(pDevice,MESSAGE_REQUEST_MACREG,MAC_REG_PAPEDELAY,LEDSTS_STS,LEDSTS_INTER);
+
+	    vnt_mac_set_led(pDevice, LEDSTS_STS, LEDSTS_INTER);
 
 	//if(pDevice->bWPASuppWextEnabled == true)
 	   {
@@ -1075,9 +1068,6 @@ static void s_vMgrRxAuthenSequence_1(struct vnt_private *pDevice,
     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");
@@ -1257,9 +1247,6 @@ reply:
     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");
     }
@@ -1412,7 +1399,8 @@ static void s_vMgrRxDeauthentication(struct vnt_private *pDevice,
                     pMgmt->eCurrState = WMAC_STATE_IDLE;
                     netif_stop_queue(pDevice->dev);
                     pDevice->bLinkPass = false;
-                    ControlvMaskByte(pDevice,MESSAGE_REQUEST_MACREG,MAC_REG_PAPEDELAY,LEDSTS_STS,LEDSTS_SLOW);
+
+		    vnt_mac_set_led(pDevice, LEDSTS_STS, LEDSTS_SLOW);
                 }
             }
 
@@ -1897,7 +1885,9 @@ if(ChannelExceedZoneType(pDevice,byCurrChannel)==true)
                 DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Current IBSS State: [Started]........to: [Jointed] \n");
                 pMgmt->eCurrState = WMAC_STATE_JOINTED;
                 pDevice->bLinkPass = true;
-                ControlvMaskByte(pDevice,MESSAGE_REQUEST_MACREG,MAC_REG_PAPEDELAY,LEDSTS_STS,LEDSTS_INTER);
+
+		vnt_mac_set_led(pDevice, LEDSTS_STS, LEDSTS_INTER);
+
                 if (netif_queue_stopped(pDevice->dev)){
                     netif_wake_queue(pDevice->dev);
                 }
@@ -2502,7 +2492,9 @@ void vMgrJoinBSSBegin(struct vnt_private *pDevice, PCMD_STATUS pStatus)
             // Adopt BSS state in Adapter Device Object
 	    pDevice->op_mode = NL80211_IFTYPE_ADHOC;
             pDevice->bLinkPass = true;
-            ControlvMaskByte(pDevice,MESSAGE_REQUEST_MACREG,MAC_REG_PAPEDELAY,LEDSTS_STS,LEDSTS_INTER);
+
+	    vnt_mac_set_led(pDevice, LEDSTS_STS, LEDSTS_INTER);
+
             memcpy(pDevice->abyBSSID, pCurr->abyBSSID, WLAN_BSSID_LEN);
 
 		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Join IBSS ok:%pM\n",
@@ -2572,7 +2564,6 @@ static void s_vMgrSynchBSS(struct vnt_private *pDevice, u32 uBSSMode,
     }
 
     // Init the BSS informations
-    pDevice->bCCK = true;
     pDevice->bProtectMode = false;
     MACvDisableProtectMD(pDevice);
     pDevice->bBarkerPreambleMd = false;
@@ -2655,8 +2646,7 @@ static void s_vMgrSynchBSS(struct vnt_private *pDevice, u32 uBSSMode,
     pMgmt->uCurrChannel = pCurr->uChannel;
     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "<----s_bSynchBSS Set Channel [%d]\n", pCurr->uChannel);
 
-    if ((pDevice->bUpdateBBVGA) &&
-        (pDevice->byBBVGACurrent != pDevice->abyBBVGA[0])) {
+    if (pDevice->byBBVGACurrent != pDevice->abyBBVGA[0]) {
         pDevice->byBBVGACurrent = pDevice->abyBBVGA[0];
         BBvSetVGAGainOffset(pDevice, pDevice->byBBVGACurrent);
         BBvSetShortSlotTime(pDevice);
@@ -2932,16 +2922,6 @@ static struct vnt_tx_mgmt *s_MgrMakeBeacon(struct vnt_private *pDevice,
              ((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;
@@ -3052,17 +3032,6 @@ static struct vnt_tx_mgmt *s_MgrMakeProbeResponse(struct vnt_private *pDevice,
              );
     }
 
-    // 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;
@@ -4058,14 +4027,14 @@ int bMgrPrepareBeaconToSend(struct vnt_private *pDevice,
 	struct vnt_manager *pMgmt)
 {
 	struct vnt_tx_mgmt *pTxPacket;
+	unsigned long flags;
 
 //    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);
-    }
+	if (pDevice->bEncryptionEnable)
+		pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_PRIVACY(1);
+	else
+		pMgmt->wCurrCapInfo &= ~WLAN_SET_CAP_INFO_PRIVACY(1);
+
     pTxPacket = s_MgrMakeBeacon
                 (
                   pDevice,
@@ -4084,8 +4053,13 @@ int bMgrPrepareBeaconToSend(struct vnt_private *pDevice,
         (pMgmt->abyCurrBSSID[0] == 0))
         return false;
 
-    csBeacon_xmit(pDevice, pTxPacket);
-    MACvRegBitsOn(pDevice, MAC_REG_TCR, TCR_AUTOBCNTX);
+	spin_lock_irqsave(&pDevice->lock, flags);
+
+	csBeacon_xmit(pDevice, pTxPacket);
+
+	spin_unlock_irqrestore(&pDevice->lock, flags);
+
+	MACvRegBitsOn(pDevice, MAC_REG_TCR, TCR_AUTOBCNTX);
 
     return true;
 }
diff --git a/drivers/staging/vt6656/wpactl.c b/drivers/staging/vt6656/wpactl.c
index f4a8a5cb9fe..0a067151eca 100644
--- a/drivers/staging/vt6656/wpactl.c
+++ b/drivers/staging/vt6656/wpactl.c
@@ -38,8 +38,7 @@
 #include "wmgr.h"
 #include "iocmd.h"
 #include "iowpa.h"
-#include "control.h"
-#include "rndis.h"
+#include "usbpipe.h"
 #include "rf.h"
 
 static int msglevel = MSG_LEVEL_INFO;
@@ -67,7 +66,7 @@ int wpa_set_keys(struct vnt_private *pDevice, void *ctx)
 	u64 KeyRSC;
 	u8 byKeyDecMode = KEY_CTL_WEP;
 	int ret = 0;
-	int uu;
+	u8 uu;
 	int ii;
 
 	if (param->u.wpa_key.alg_name > WPA_ALG_CCMP)
diff --git a/drivers/staging/winbond/wb35tx.c b/drivers/staging/winbond/wb35tx.c
index 708c5b05f86..870cff39a22 100644
--- a/drivers/staging/winbond/wb35tx.c
+++ b/drivers/staging/winbond/wb35tx.c
@@ -49,7 +49,7 @@ static void Wb35Tx_complete(struct urb *pUrb)
 
 	/* The URB is completed, check the result */
 	if (pWb35Tx->EP4VM_status != 0) {
-		printk("URB submission failed\n");
+		dev_err(&pUrb->dev->dev, "URB submission failed\n");
 		pWb35Tx->EP4vm_state = VM_STOP;
 		goto error;
 	}
@@ -96,7 +96,7 @@ static void Wb35Tx(struct wbsoft_priv *adapter)
 	pWb35Tx->EP4vm_state = VM_RUNNING;
 	retv = usb_submit_urb(pUrb, GFP_ATOMIC);
 	if (retv < 0) {
-		printk("EP4 Tx Irp sending error\n");
+		dev_err(&pUrb->dev->dev, "EP4 Tx Irp sending error\n");
 		goto cleanup;
 	}
 
@@ -218,7 +218,7 @@ static void Wb35Tx_EP2VM_complete(struct urb *pUrb)
 
 	/* The Urb is completed, check the result */
 	if (pWb35Tx->EP2VM_status != 0) {
-		printk("EP2 IoCompleteRoutine return error\n");
+		dev_err(&pUrb->dev->dev, "EP2 IoCompleteRoutine return error\n");
 		pWb35Tx->EP2vm_state = VM_STOP;
 		goto error;
 	}
diff --git a/drivers/staging/winbond/wb35tx_s.h b/drivers/staging/winbond/wb35tx_s.h
index 715f87d6ac5..dc120085d52 100644
--- a/drivers/staging/winbond/wb35tx_s.h
+++ b/drivers/staging/winbond/wb35tx_s.h
@@ -12,7 +12,7 @@
 /* Internal variable for module */
 struct wb35_tx {
 	/* For Tx buffer */
-	u8	TxBuffer[ MAX_USB_TX_BUFFER_NUMBER ][ MAX_USB_TX_BUFFER ];
+	u8	TxBuffer[MAX_USB_TX_BUFFER_NUMBER][MAX_USB_TX_BUFFER];
 
 	/* For Interrupt pipe */
 	u8	EP2_buf[MAX_INTERRUPT_LENGTH];
diff --git a/drivers/staging/wlags49_h2/wl_priv.c b/drivers/staging/wlags49_h2/wl_priv.c
index 41f332499d4..aff92735022 100644
--- a/drivers/staging/wlags49_h2/wl_priv.c
+++ b/drivers/staging/wlags49_h2/wl_priv.c
@@ -179,8 +179,6 @@ int wvlan_uil(struct uilreq *urq, struct wl_private *lp)
  ******************************************************************************/
 int wvlan_uil_connect(struct uilreq *urq, struct wl_private *lp)
 {
-	int result = 0;
-
 	if (!(lp->flags & WVLAN2_UIL_CONNECTED)) {
 		lp->flags |= WVLAN2_UIL_CONNECTED;
 		urq->hcfCtx = &(lp->hcfCtx);
@@ -190,7 +188,7 @@ int wvlan_uil_connect(struct uilreq *urq, struct wl_private *lp)
 		urq->result = UIL_ERR_IN_USE;
 	}
 
-	return result;
+	return 0;
 } /* wvlan_uil_connect */
 /*============================================================================*/
 
@@ -218,8 +216,6 @@ int wvlan_uil_connect(struct uilreq *urq, struct wl_private *lp)
  ******************************************************************************/
 int wvlan_uil_disconnect(struct uilreq *urq, struct wl_private *lp)
 {
-	int result = 0;
-
 	if (urq->hcfCtx == &(lp->hcfCtx)) {
 		if (lp->flags & WVLAN2_UIL_CONNECTED) {
 			lp->flags &= ~WVLAN2_UIL_CONNECTED;
@@ -238,7 +234,7 @@ int wvlan_uil_disconnect(struct uilreq *urq, struct wl_private *lp)
 		urq->result = UIL_ERR_WRONG_IFB;
 	}
 
-	return result;
+	return 0;
 } /* wvlan_uil_disconnect */
 /*============================================================================*/
 
@@ -1580,7 +1576,6 @@ int wvlan_set_netname(struct net_device *dev,
 {
 	struct wl_private *lp = wl_priv(dev);
 	unsigned long flags;
-	int ret = 0;
 
 	wl_lock(lp, &flags);
 
@@ -1591,7 +1586,7 @@ int wvlan_set_netname(struct net_device *dev,
 	wl_apply(lp);
 	wl_unlock(lp, &flags);
 
-	return ret;
+	return 0;
 } /* wvlan_set_netname */
 /*============================================================================*/
 
@@ -1683,7 +1678,6 @@ int wvlan_set_station_nickname(struct net_device *dev,
 	struct wl_private *lp = wl_priv(dev);
 	unsigned long flags;
 	size_t len;
-	int         ret = 0;
 
 	wl_lock(lp, &flags);
 
@@ -1695,7 +1689,7 @@ int wvlan_set_station_nickname(struct net_device *dev,
 	wl_apply(lp);
 	wl_unlock(lp, &flags);
 
-	return ret;
+	return 0;
 } /* wvlan_set_station_nickname */
 /*============================================================================*/
 
diff --git a/drivers/staging/wlags49_h2/wl_wext.c b/drivers/staging/wlags49_h2/wl_wext.c
index 49eeeaee664..3aeff818afc 100644
--- a/drivers/staging/wlags49_h2/wl_wext.c
+++ b/drivers/staging/wlags49_h2/wl_wext.c
@@ -159,15 +159,12 @@ static int hermes_set_tkip_keys(ltv_t *ltv, u16 key_idx, u8 *addr,
 /* Set up the LTV to clear the appropriate key */
 static int hermes_clear_tkip_keys(ltv_t *ltv, u16 key_idx, u8 *addr)
 {
-	int ret;
-
 	switch (key_idx) {
 	case 0:
 		if (!is_broadcast_ether_addr(addr)) {
 			ltv->len = 7;
 			ltv->typ = CFG_REMOVE_TKIP_MAPPED_KEY;
 			memcpy(&ltv->u.u8[0], addr, ETH_ALEN);
-			ret = 0;
 		}
 		break;
 	case 1:
@@ -178,13 +175,12 @@ static int hermes_clear_tkip_keys(ltv_t *ltv, u16 key_idx, u8 *addr)
 		ltv->typ = CFG_REMOVE_TKIP_DEFAULT_KEY;
 		ltv->u.u16[0] = cpu_to_le16(key_idx);
 
-		ret = 0;
 		break;
 	default:
 		break;
 	}
 
-	return ret;
+	return 0;
 }
 
 /* Set the WEP keys in the wl_private structure */
@@ -3027,13 +3023,10 @@ static int wireless_set_genie(struct net_device *dev,
 			      struct iw_point *data, char *extra)
 
 {
-	int   ret = 0;
-
 	/* We can't write this to the card, but apparently this
 	 * operation needs to succeed */
-	ret = 0;
 
-	return ret;
+	return 0;
 }
 /*============================================================================*/
 
diff --git a/drivers/staging/wlan-ng/hfa384x_usb.c b/drivers/staging/wlan-ng/hfa384x_usb.c
index 5b8b094c872..98343ff7061 100644
--- a/drivers/staging/wlan-ng/hfa384x_usb.c
+++ b/drivers/staging/wlan-ng/hfa384x_usb.c
@@ -203,7 +203,7 @@ static int hfa384x_usbctlx_submit(hfa384x_t *hw, hfa384x_usbctlx_t *ctlx);
 static void unlocked_usbctlx_complete(hfa384x_t *hw, hfa384x_usbctlx_t *ctlx);
 
 struct usbctlx_completor {
-	int (*complete) (struct usbctlx_completor *);
+	int (*complete)(struct usbctlx_completor *);
 };
 
 static int
@@ -272,7 +272,7 @@ static int hfa384x_isgood_pdrcode(u16 pdrcode);
 
 static inline const char *ctlxstr(CTLX_STATE s)
 {
-	static const char *ctlx_str[] = {
+	static const char * const ctlx_str[] = {
 		"Initial state",
 		"Complete",
 		"Request failed",
@@ -350,14 +350,14 @@ static int submit_rx_urb(hfa384x_t *hw, gfp_t memflags)
 
 	result = -ENOLINK;
 	if (!hw->wlandev->hwremoved &&
-			!test_bit(WORK_RX_HALT, &hw->usb_flags)) {
+	    !test_bit(WORK_RX_HALT, &hw->usb_flags)) {
 		result = SUBMIT_URB(&hw->rx_urb, memflags);
 
 		/* Check whether we need to reset the RX pipe */
 		if (result == -EPIPE) {
 			netdev_warn(hw->wlandev->netdev,
-			       "%s rx pipe stalled: requesting reset\n",
-			       hw->wlandev->netdev->name);
+				    "%s rx pipe stalled: requesting reset\n",
+				    hw->wlandev->netdev->name);
 			if (!test_and_set_bit(WORK_RX_HALT, &hw->usb_flags))
 				schedule_work(&hw->usb_work);
 		}
@@ -398,16 +398,15 @@ static int submit_tx_urb(hfa384x_t *hw, struct urb *tx_urb, gfp_t memflags)
 
 	result = -ENOLINK;
 	if (netif_running(netdev)) {
-
-		if (!hw->wlandev->hwremoved
-		    && !test_bit(WORK_TX_HALT, &hw->usb_flags)) {
+		if (!hw->wlandev->hwremoved &&
+		    !test_bit(WORK_TX_HALT, &hw->usb_flags)) {
 			result = SUBMIT_URB(tx_urb, memflags);
 
 			/* Test whether we need to reset the TX pipe */
 			if (result == -EPIPE) {
 				netdev_warn(hw->wlandev->netdev,
-				       "%s tx pipe stalled: requesting reset\n",
-				       netdev->name);
+					    "%s tx pipe stalled: requesting reset\n",
+					    netdev->name);
 				set_bit(WORK_TX_HALT, &hw->usb_flags);
 				schedule_work(&hw->usb_work);
 			} else if (result == 0) {
@@ -455,11 +454,11 @@ static void hfa384x_usb_defer(struct work_struct *data)
 		ret = usb_clear_halt(hw->usb, hw->endp_in);
 		if (ret != 0) {
 			netdev_err(hw->wlandev->netdev,
-			       "Failed to clear rx pipe for %s: err=%d\n",
-			       netdev->name, ret);
+				   "Failed to clear rx pipe for %s: err=%d\n",
+				   netdev->name, ret);
 		} else {
 			netdev_info(hw->wlandev->netdev, "%s rx pipe reset complete.\n",
-			       netdev->name);
+				    netdev->name);
 			clear_bit(WORK_RX_HALT, &hw->usb_flags);
 			set_bit(WORK_RX_RESUME, &hw->usb_flags);
 		}
@@ -472,7 +471,8 @@ static void hfa384x_usb_defer(struct work_struct *data)
 		ret = submit_rx_urb(hw, GFP_KERNEL);
 		if (ret != 0) {
 			netdev_err(hw->wlandev->netdev,
-			       "Failed to resume %s rx pipe.\n", netdev->name);
+				   "Failed to resume %s rx pipe.\n",
+				   netdev->name);
 		} else {
 			clear_bit(WORK_RX_RESUME, &hw->usb_flags);
 		}
@@ -486,11 +486,11 @@ static void hfa384x_usb_defer(struct work_struct *data)
 		ret = usb_clear_halt(hw->usb, hw->endp_out);
 		if (ret != 0) {
 			netdev_err(hw->wlandev->netdev,
-			       "Failed to clear tx pipe for %s: err=%d\n",
-			       netdev->name, ret);
+				   "Failed to clear tx pipe for %s: err=%d\n",
+				   netdev->name, ret);
 		} else {
 			netdev_info(hw->wlandev->netdev, "%s tx pipe reset complete.\n",
-			       netdev->name);
+				    netdev->name);
 			clear_bit(WORK_TX_HALT, &hw->usb_flags);
 			set_bit(WORK_TX_RESUME, &hw->usb_flags);
 
@@ -642,8 +642,7 @@ usbctlx_get_status(const hfa384x_usb_cmdresp_t *cmdresp,
 	result->resp1 = le16_to_cpu(cmdresp->resp1);
 	result->resp2 = le16_to_cpu(cmdresp->resp2);
 
-	pr_debug("cmdresult:status=0x%04x "
-		 "resp0=0x%04x resp1=0x%04x resp2=0x%04x\n",
+	pr_debug("cmdresult:status=0x%04x resp0=0x%04x resp1=0x%04x resp2=0x%04x\n",
 		 result->status, result->resp0, result->resp1, result->resp2);
 
 	return result->status & HFA384x_STATUS_RESULT;
@@ -656,7 +655,6 @@ usbctlx_get_rridresult(const hfa384x_usb_rridresp_t *rridresp,
 	result->rid = le16_to_cpu(rridresp->rid);
 	result->riddata = rridresp->data;
 	result->riddata_len = ((le16_to_cpu(rridresp->frmlen) - 1) * 2);
-
 }
 
 /*----------------------------------------------------------------
@@ -675,7 +673,7 @@ static inline int usbctlx_cmd_completor_fn(struct usbctlx_completor *head)
 {
 	struct usbctlx_cmd_completor *complete;
 
-	complete = (struct usbctlx_cmd_completor *) head;
+	complete = (struct usbctlx_cmd_completor *)head;
 	return usbctlx_get_status(complete->cmdresp, complete->result);
 }
 
@@ -710,15 +708,14 @@ static int usbctlx_rrid_completor_fn(struct usbctlx_completor *head)
 	struct usbctlx_rrid_completor *complete;
 	hfa384x_rridresult_t rridresult;
 
-	complete = (struct usbctlx_rrid_completor *) head;
+	complete = (struct usbctlx_rrid_completor *)head;
 	usbctlx_get_rridresult(complete->rridresp, &rridresult);
 
 	/* Validate the length, note body len calculation in bytes */
 	if (rridresult.riddata_len != complete->riddatalen) {
-		printk(KERN_WARNING
-		       "RID len mismatch, rid=0x%04x hlen=%d fwlen=%d\n",
-		       rridresult.rid,
-		       complete->riddatalen, rridresult.riddata_len);
+		pr_warn("RID len mismatch, rid=0x%04x hlen=%d fwlen=%d\n",
+			rridresult.rid,
+			complete->riddatalen, rridresult.riddata_len);
 		return -ENODATA;
 	}
 
@@ -745,14 +742,12 @@ static inline struct usbctlx_completor *init_rrid_completor(
 * Completor object:
 * Interprets the results of a synchronous RID-write
 ----------------------------------------------------------------*/
-typedef struct usbctlx_cmd_completor usbctlx_wrid_completor_t;
 #define init_wrid_completor  init_cmd_completor
 
 /*----------------------------------------------------------------
 * Completor object:
 * Interprets the results of a synchronous memory-write
 ----------------------------------------------------------------*/
-typedef struct usbctlx_cmd_completor usbctlx_wmem_completor_t;
 #define init_wmem_completor  init_cmd_completor
 
 /*----------------------------------------------------------------
@@ -766,11 +761,11 @@ struct usbctlx_rmem_completor {
 	void *data;
 	unsigned int len;
 };
-typedef struct usbctlx_rmem_completor usbctlx_rmem_completor_t;
 
 static int usbctlx_rmem_completor_fn(struct usbctlx_completor *head)
 {
-	usbctlx_rmem_completor_t *complete = (usbctlx_rmem_completor_t *) head;
+	struct usbctlx_rmem_completor *complete =
+		(struct usbctlx_rmem_completor *)head;
 
 	pr_debug("rmemresp:len=%d\n", complete->rmemresp->frmlen);
 	memcpy(complete->data, complete->rmemresp->data, complete->len);
@@ -778,7 +773,7 @@ static int usbctlx_rmem_completor_fn(struct usbctlx_completor *head)
 }
 
 static inline struct usbctlx_completor *init_rmem_completor(
-						usbctlx_rmem_completor_t
+						struct usbctlx_rmem_completor
 							*completor,
 						hfa384x_usb_rmemresp_t
 							*rmemresp,
@@ -991,9 +986,7 @@ int hfa384x_cmd_initialize(hfa384x_t *hw)
 
 	result = hfa384x_docmd_wait(hw, &cmd);
 
-	pr_debug("cmdresp.init: "
-		 "status=0x%04x, resp0=0x%04x, "
-		 "resp1=0x%04x, resp2=0x%04x\n",
+	pr_debug("cmdresp.init: status=0x%04x, resp0=0x%04x, resp1=0x%04x, resp2=0x%04x\n",
 		 cmd.result.status,
 		 cmd.result.resp0, cmd.result.resp1, cmd.result.resp2);
 	if (result == 0) {
@@ -1212,7 +1205,7 @@ int hfa384x_corereset(hfa384x_t *hw, int holdtime, int settletime, int genesis)
 	result = usb_reset_device(hw->usb);
 	if (result < 0) {
 		netdev_err(hw->wlandev->netdev, "usb_reset_device() failed, result=%d.\n",
-		       result);
+			   result);
 	}
 
 	return result;
@@ -1312,8 +1305,8 @@ cleanup:
 			result = completor->complete(completor);
 		} else {
 			netdev_warn(hw->wlandev->netdev, "CTLX[%d] error: state(%s)\n",
-			       le16_to_cpu(ctlx->outbuf.type),
-			       ctlxstr(ctlx->state));
+				    le16_to_cpu(ctlx->outbuf.type),
+				    ctlxstr(ctlx->state));
 			result = -EIO;
 		}
 
@@ -1381,8 +1374,7 @@ hfa384x_docmd(hfa384x_t *hw,
 
 	ctlx->outbufsize = sizeof(ctlx->outbuf.cmdreq);
 
-	pr_debug("cmdreq: cmd=0x%04x "
-		 "parm0=0x%04x parm1=0x%04x parm2=0x%04x\n",
+	pr_debug("cmdreq: cmd=0x%04x parm0=0x%04x parm1=0x%04x parm2=0x%04x\n",
 		 cmd->cmd, cmd->parm0, cmd->parm1, cmd->parm2);
 
 	ctlx->reapable = mode;
@@ -1566,7 +1558,7 @@ hfa384x_dowrid(hfa384x_t *hw,
 	if (result != 0) {
 		kfree(ctlx);
 	} else if (mode == DOWAIT) {
-		usbctlx_wrid_completor_t completor;
+		struct usbctlx_cmd_completor completor;
 		hfa384x_cmdresult_t wridresult;
 
 		result = hfa384x_usbctlx_complete_sync(hw,
@@ -1658,7 +1650,7 @@ hfa384x_dormem(hfa384x_t *hw,
 	if (result != 0) {
 		kfree(ctlx);
 	} else if (mode == DOWAIT) {
-		usbctlx_rmem_completor_t completor;
+		struct usbctlx_rmem_completor completor;
 
 		result =
 		    hfa384x_usbctlx_complete_sync(hw, ctlx,
@@ -1748,7 +1740,7 @@ hfa384x_dowmem(hfa384x_t *hw,
 	if (result != 0) {
 		kfree(ctlx);
 	} else if (mode == DOWAIT) {
-		usbctlx_wmem_completor_t completor;
+		struct usbctlx_cmd_completor completor;
 		hfa384x_cmdresult_t wmemresult;
 
 		result = hfa384x_usbctlx_complete_sync(hw,
@@ -2018,7 +2010,8 @@ int hfa384x_drvr_flashdl_write(hfa384x_t *hw, u32 daddr, void *buf, u32 len)
 	if (hw->dlstate != HFA384x_DLSTATE_FLASHENABLED)
 		return -EINVAL;
 
-	netdev_info(hw->wlandev->netdev, "Download %d bytes to flash @0x%06x\n", len, daddr);
+	netdev_info(hw->wlandev->netdev,
+		    "Download %d bytes to flash @0x%06x\n", len, daddr);
 
 	/* Convert to flat address for arithmetic */
 	/* NOTE: dlbuffer RID stores the address in AUX format */
@@ -2026,11 +2019,6 @@ int hfa384x_drvr_flashdl_write(hfa384x_t *hw, u32 daddr, void *buf, u32 len)
 	    HFA384x_ADDR_AUX_MKFLAT(hw->bufinfo.page, hw->bufinfo.offset);
 	pr_debug("dlbuf.page=0x%04x dlbuf.offset=0x%04x dlbufaddr=0x%08x\n",
 		 hw->bufinfo.page, hw->bufinfo.offset, dlbufaddr);
-
-#if 0
-	netdev_warn(hw->wlandev->netdev, "dlbuf@0x%06lx len=%d to=%d\n", dlbufaddr,
-	       hw->bufinfo.len, hw->dltimeout);
-#endif
 	/* Calculations to determine how many fills of the dlbuffer to do
 	 * and how many USB wmemreq's to do for each fill.  At this point
 	 * in time, the dlbuffer size and the wmemreq size are the same.
@@ -2056,15 +2044,15 @@ int hfa384x_drvr_flashdl_write(hfa384x_t *hw, u32 daddr, void *buf, u32 len)
 		burnhi = HFA384x_ADDR_CMD_MKPAGE(burndaddr);
 
 		netdev_info(hw->wlandev->netdev, "Writing %d bytes to flash @0x%06x\n",
-		       burnlen, burndaddr);
+			    burnlen, burndaddr);
 
 		/* Set the download mode */
 		result = hfa384x_cmd_download(hw, HFA384x_PROGMODE_NV,
 					      burnlo, burnhi, burnlen);
 		if (result) {
-			netdev_err(hw->wlandev->netdev, "download(NV,lo=%x,hi=%x,len=%x) "
-			       "cmd failed, result=%d. Aborting d/l\n",
-			       burnlo, burnhi, burnlen, result);
+			netdev_err(hw->wlandev->netdev,
+				   "download(NV,lo=%x,hi=%x,len=%x) cmd failed, result=%d. Aborting d/l\n",
+				   burnlo, burnhi, burnlen, result);
 			goto exit_proc;
 		}
 
@@ -2095,9 +2083,8 @@ int hfa384x_drvr_flashdl_write(hfa384x_t *hw, u32 daddr, void *buf, u32 len)
 					      0, 0, 0);
 		if (result) {
 			netdev_err(hw->wlandev->netdev,
-			       "download(NVWRITE,lo=%x,hi=%x,len=%x) "
-			       "cmd failed, result=%d. Aborting d/l\n",
-			       burnlo, burnhi, burnlen, result);
+				   "download(NVWRITE,lo=%x,hi=%x,len=%x) cmd failed, result=%d. Aborting d/l\n",
+				   burnlo, burnhi, burnlen, result);
 			goto exit_proc;
 		}
 
@@ -2280,7 +2267,7 @@ int hfa384x_drvr_ramdl_enable(hfa384x_t *hw, u32 exeaddr)
 	for (i = 0; i < HFA384x_PORTID_MAX; i++) {
 		if (hw->port_enabled[i]) {
 			netdev_err(hw->wlandev->netdev,
-			       "Can't download with a macport enabled.\n");
+				   "Can't download with a macport enabled.\n");
 			return -EINVAL;
 		}
 	}
@@ -2352,7 +2339,8 @@ int hfa384x_drvr_ramdl_write(hfa384x_t *hw, u32 daddr, void *buf, u32 len)
 	if (hw->dlstate != HFA384x_DLSTATE_RAMENABLED)
 		return -EINVAL;
 
-	netdev_info(hw->wlandev->netdev, "Writing %d bytes to ram @0x%06x\n", len, daddr);
+	netdev_info(hw->wlandev->netdev, "Writing %d bytes to ram @0x%06x\n",
+		    len, daddr);
 
 	/* How many dowmem calls?  */
 	nwrites = len / HFA384x_USB_RWMEM_MAXLEN;
@@ -2450,7 +2438,8 @@ int hfa384x_drvr_readpda(hfa384x_t *hw, void *buf, unsigned int len)
 
 		if (result) {
 			netdev_warn(hw->wlandev->netdev,
-			       "Read from index %zd failed, continuing\n", i);
+				    "Read from index %zd failed, continuing\n",
+				    i);
 			continue;
 		}
 
@@ -2462,14 +2451,15 @@ int hfa384x_drvr_readpda(hfa384x_t *hw, void *buf, unsigned int len)
 			pdrcode = le16_to_cpu(pda[currpdr + 1]);
 			/* Test the record length */
 			if (pdrlen > HFA384x_PDR_LEN_MAX || pdrlen == 0) {
-				netdev_err(hw->wlandev->netdev, "pdrlen invalid=%d\n", pdrlen);
+				netdev_err(hw->wlandev->netdev,
+					   "pdrlen invalid=%d\n", pdrlen);
 				pdaok = 0;
 				break;
 			}
 			/* Test the code */
 			if (!hfa384x_isgood_pdrcode(pdrcode)) {
 				netdev_err(hw->wlandev->netdev, "pdrcode invalid=%d\n",
-				       pdrcode);
+					   pdrcode);
 				pdaok = 0;
 				break;
 			}
@@ -2485,13 +2475,13 @@ int hfa384x_drvr_readpda(hfa384x_t *hw, void *buf, unsigned int len)
 		}
 		if (pdaok) {
 			netdev_info(hw->wlandev->netdev,
-			       "PDA Read from 0x%08x in %s space.\n",
-			       pdaloc[i].cardaddr,
-			       pdaloc[i].auxctl == 0 ? "EXTDS" :
-			       pdaloc[i].auxctl == 1 ? "NV" :
-			       pdaloc[i].auxctl == 2 ? "PHY" :
-			       pdaloc[i].auxctl == 3 ? "ICSRAM" :
-			       "<bogus auxctl>");
+				    "PDA Read from 0x%08x in %s space.\n",
+				    pdaloc[i].cardaddr,
+				    pdaloc[i].auxctl == 0 ? "EXTDS" :
+				    pdaloc[i].auxctl == 1 ? "NV" :
+				    pdaloc[i].auxctl == 2 ? "PHY" :
+				    pdaloc[i].auxctl == 3 ? "ICSRAM" :
+				    "<bogus auxctl>");
 			break;
 		}
 	}
@@ -2586,7 +2576,8 @@ int hfa384x_drvr_start(hfa384x_t *hw)
 	result = submit_rx_urb(hw, GFP_KERNEL);
 	if (result != 0) {
 		netdev_err(hw->wlandev->netdev,
-		       "Fatal, failed to submit RX URB, result=%d\n", result);
+			   "Fatal, failed to submit RX URB, result=%d\n",
+			   result);
 		goto done;
 	}
 
@@ -2602,12 +2593,13 @@ int hfa384x_drvr_start(hfa384x_t *hw)
 	 */
 	result1 = hfa384x_cmd_initialize(hw);
 	msleep(1000);
-	result = result2 = hfa384x_cmd_initialize(hw);
+	result = hfa384x_cmd_initialize(hw);
+	result2 = result;
 	if (result1 != 0) {
 		if (result2 != 0) {
 			netdev_err(hw->wlandev->netdev,
-				"cmd_initialize() failed on two attempts, results %d and %d\n",
-				result1, result2);
+				   "cmd_initialize() failed on two attempts, results %d and %d\n",
+				   result1, result2);
 			usb_kill_urb(&hw->rx_urb);
 			goto done;
 		} else {
@@ -2617,9 +2609,9 @@ int hfa384x_drvr_start(hfa384x_t *hw)
 		}
 	} else if (result2 != 0) {
 		netdev_warn(hw->wlandev->netdev, "First cmd_initialize() succeeded, but second attempt failed (result=%d)\n",
-			result2);
+			    result2);
 		netdev_warn(hw->wlandev->netdev,
-		       "Most likely the card will be functional\n");
+			    "Most likely the card will be functional\n");
 		goto done;
 	}
 
@@ -2650,7 +2642,6 @@ done:
 ----------------------------------------------------------------*/
 int hfa384x_drvr_stop(hfa384x_t *hw)
 {
-	int result = 0;
 	int i;
 
 	might_sleep();
@@ -2675,7 +2666,7 @@ int hfa384x_drvr_stop(hfa384x_t *hw)
 	for (i = 0; i < HFA384x_NUMPORTS_MAX; i++)
 		hw->port_enabled[i] = 0;
 
-	return result;
+	return 0;
 }
 
 /*----------------------------------------------------------------
@@ -2784,7 +2775,8 @@ int hfa384x_drvr_txframe(hfa384x_t *hw, struct sk_buff *skb,
 	result = 1;
 	ret = submit_tx_urb(hw, &hw->tx_urb, GFP_ATOMIC);
 	if (ret != 0) {
-		netdev_err(hw->wlandev->netdev, "submit_tx_urb() failed, error=%d\n", ret);
+		netdev_err(hw->wlandev->netdev,
+			   "submit_tx_urb() failed, error=%d\n", ret);
 		result = 3;
 	}
 
@@ -2826,7 +2818,7 @@ void hfa384x_tx_timeout(wlandevice_t *wlandev)
 ----------------------------------------------------------------*/
 static void hfa384x_usbctlx_reaper_task(unsigned long data)
 {
-	hfa384x_t *hw = (hfa384x_t *) data;
+	hfa384x_t *hw = (hfa384x_t *)data;
 	struct list_head *entry;
 	struct list_head *temp;
 	unsigned long flags;
@@ -2845,7 +2837,6 @@ static void hfa384x_usbctlx_reaper_task(unsigned long data)
 	}
 
 	spin_unlock_irqrestore(&hw->ctlxq.lock, flags);
-
 }
 
 /*----------------------------------------------------------------
@@ -2864,7 +2855,7 @@ static void hfa384x_usbctlx_reaper_task(unsigned long data)
 ----------------------------------------------------------------*/
 static void hfa384x_usbctlx_completion_task(unsigned long data)
 {
-	hfa384x_t *hw = (hfa384x_t *) data;
+	hfa384x_t *hw = (hfa384x_t *)data;
 	struct list_head *entry;
 	struct list_head *temp;
 	unsigned long flags;
@@ -3010,7 +3001,8 @@ static void unlocked_usbctlx_complete(hfa384x_t *hw, hfa384x_usbctlx_t *ctlx)
 
 	default:
 		netdev_err(hw->wlandev->netdev, "CTLX[%d] not in a terminating state(%s)\n",
-		       le16_to_cpu(ctlx->outbuf.type), ctlxstr(ctlx->state));
+			   le16_to_cpu(ctlx->outbuf.type),
+			   ctlxstr(ctlx->state));
 		break;
 	}			/* switch */
 }
@@ -3092,8 +3084,8 @@ static void hfa384x_usbctlxq_run(hfa384x_t *hw)
 			 * and schedule a reset ...
 			 */
 			netdev_warn(hw->wlandev->netdev,
-			       "%s tx pipe stalled: requesting reset\n",
-			       hw->wlandev->netdev->name);
+				    "%s tx pipe stalled: requesting reset\n",
+				    hw->wlandev->netdev->name);
 			list_move(&head->list, &hw->ctlxq.pending);
 			set_bit(WORK_TX_HALT, &hw->usb_flags);
 			schedule_work(&hw->usb_work);
@@ -3102,12 +3094,12 @@ static void hfa384x_usbctlxq_run(hfa384x_t *hw)
 
 		if (result == -ESHUTDOWN) {
 			netdev_warn(hw->wlandev->netdev, "%s urb shutdown!\n",
-			       hw->wlandev->netdev->name);
+				    hw->wlandev->netdev->name);
 			break;
 		}
 
 		netdev_err(hw->wlandev->netdev, "Failed to submit CTLX[%d]: error=%d\n",
-		       le16_to_cpu(head->outbuf.type), result);
+			   le16_to_cpu(head->outbuf.type), result);
 		unlocked_usbctlx_complete(hw, head);
 	}			/* while */
 
@@ -3135,7 +3127,7 @@ static void hfa384x_usbin_callback(struct urb *urb)
 {
 	wlandevice_t *wlandev = urb->context;
 	hfa384x_t *hw;
-	hfa384x_usbin_t *usbin = (hfa384x_usbin_t *) urb->transfer_buffer;
+	hfa384x_usbin_t *usbin = (hfa384x_usbin_t *)urb->transfer_buffer;
 	struct sk_buff *skb = NULL;
 	int result;
 	int urb_status;
@@ -3174,7 +3166,7 @@ static void hfa384x_usbin_callback(struct urb *urb)
 
 	case -EPIPE:
 		netdev_warn(hw->wlandev->netdev, "%s rx pipe stalled: requesting reset\n",
-		       wlandev->netdev->name);
+			    wlandev->netdev->name);
 		if (!test_and_set_bit(WORK_RX_HALT, &hw->usb_flags))
 			schedule_work(&hw->usb_work);
 		++(wlandev->linux_stats.rx_errors);
@@ -3225,8 +3217,8 @@ static void hfa384x_usbin_callback(struct urb *urb)
 
 		if (result != 0) {
 			netdev_err(hw->wlandev->netdev,
-			       "Fatal, failed to resubmit rx_urb. error=%d\n",
-			       result);
+				   "Fatal, failed to resubmit rx_urb. error=%d\n",
+				   result);
 		}
 	}
 
@@ -3361,9 +3353,9 @@ retry:
 		 */
 		if (ctlx->outbuf.type != intype) {
 			netdev_warn(hw->wlandev->netdev,
-			       "Expected IN[%d], received IN[%d] - ignored.\n",
-			       le16_to_cpu(ctlx->outbuf.type),
-			       le16_to_cpu(intype));
+				    "Expected IN[%d], received IN[%d] - ignored.\n",
+				    le16_to_cpu(ctlx->outbuf.type),
+				    le16_to_cpu(intype));
 			goto unlock;
 		}
 
@@ -3397,10 +3389,9 @@ retry:
 			 * Throw this CTLX away ...
 			 */
 			netdev_err(hw->wlandev->netdev,
-			       "Matched IN URB, CTLX[%d] in invalid state(%s)."
-			       " Discarded.\n",
-			       le16_to_cpu(ctlx->outbuf.type),
-			       ctlxstr(ctlx->state));
+				   "Matched IN URB, CTLX[%d] in invalid state(%s). Discarded.\n",
+				   le16_to_cpu(ctlx->outbuf.type),
+				   ctlxstr(ctlx->state));
 			if (unlocked_usbctlx_cancel_async(hw, ctlx) == 0)
 				run_queue = 1;
 			break;
@@ -3464,7 +3455,7 @@ static void hfa384x_usbin_txcompl(wlandevice_t *wlandev,
 ----------------------------------------------------------------*/
 static void hfa384x_usbin_rx(wlandevice_t *wlandev, struct sk_buff *skb)
 {
-	hfa384x_usbin_t *usbin = (hfa384x_usbin_t *) skb->data;
+	hfa384x_usbin_t *usbin = (hfa384x_usbin_t *)skb->data;
 	hfa384x_t *hw = wlandev->priv;
 	int hdrlen;
 	struct p80211_rxmeta *rxmeta;
@@ -3535,7 +3526,8 @@ static void hfa384x_usbin_rx(wlandevice_t *wlandev, struct sk_buff *skb)
 
 	default:
 		netdev_warn(hw->wlandev->netdev, "Received frame on unsupported port=%d\n",
-		       HFA384x_RXSTATUS_MACPORT_GET(usbin->rxfrm.desc.status));
+			    HFA384x_RXSTATUS_MACPORT_GET(
+				    usbin->rxfrm.desc.status));
 		goto done;
 		break;
 	}
@@ -3597,8 +3589,8 @@ static void hfa384x_int_rxmonitor(wlandevice_t *wlandev,
 	skb = dev_alloc_skb(skblen);
 	if (skb == NULL) {
 		netdev_err(hw->wlandev->netdev,
-		       "alloc_skb failed trying to allocate %d bytes\n",
-		       skblen);
+			   "alloc_skb failed trying to allocate %d bytes\n",
+			   skblen);
 		return;
 	}
 
@@ -3608,7 +3600,7 @@ static void hfa384x_int_rxmonitor(wlandevice_t *wlandev,
 		struct p80211_caphdr *caphdr;
 		/* The NEW header format! */
 		datap = skb_put(skb, sizeof(struct p80211_caphdr));
-		caphdr = (struct p80211_caphdr *) datap;
+		caphdr = (struct p80211_caphdr *)datap;
 
 		caphdr->version = htonl(P80211CAPTURE_VERSION);
 		caphdr->length = htonl(sizeof(struct p80211_caphdr));
@@ -3705,7 +3697,6 @@ static void hfa384x_usbout_callback(struct urb *urb)
 #endif
 
 	if (wlandev && wlandev->netdev) {
-
 		switch (urb->status) {
 		case 0:
 			hfa384x_usbout_tx(wlandev, usbout);
@@ -3715,8 +3706,8 @@ static void hfa384x_usbout_callback(struct urb *urb)
 			{
 				hfa384x_t *hw = wlandev->priv;
 				netdev_warn(hw->wlandev->netdev,
-				       "%s tx pipe stalled: requesting reset\n",
-				       wlandev->netdev->name);
+					    "%s tx pipe stalled: requesting reset\n",
+					    wlandev->netdev->name);
 				if (!test_and_set_bit
 				    (WORK_TX_HALT, &hw->usb_flags))
 					schedule_work(&hw->usb_work);
@@ -3731,8 +3722,8 @@ static void hfa384x_usbout_callback(struct urb *urb)
 				hfa384x_t *hw = wlandev->priv;
 
 				if (!test_and_set_bit
-				    (THROTTLE_TX, &hw->usb_flags)
-				    && !timer_pending(&hw->throttle)) {
+				    (THROTTLE_TX, &hw->usb_flags) &&
+				    !timer_pending(&hw->throttle)) {
 					mod_timer(&hw->throttle,
 						  jiffies + THROTTLE_JIFFIES);
 				}
@@ -3748,7 +3739,7 @@ static void hfa384x_usbout_callback(struct urb *urb)
 
 		default:
 			netdev_info(wlandev->netdev, "unknown urb->status=%d\n",
-			       urb->status);
+				    urb->status);
 			++(wlandev->linux_stats.tx_errors);
 			break;
 		}		/* switch */
@@ -3842,9 +3833,9 @@ retry:
 		default:
 			/* This is NOT a valid CTLX "success" state! */
 			netdev_err(hw->wlandev->netdev,
-				"Illegal CTLX[%d] success state(%s, %d) in OUT URB\n",
-				le16_to_cpu(ctlx->outbuf.type),
-				ctlxstr(ctlx->state), urb->status);
+				   "Illegal CTLX[%d] success state(%s, %d) in OUT URB\n",
+				   le16_to_cpu(ctlx->outbuf.type),
+				   ctlxstr(ctlx->state), urb->status);
 			break;
 		}		/* switch */
 	} else {
@@ -3852,8 +3843,8 @@ retry:
 		if ((urb->status == -EPIPE) &&
 		    !test_and_set_bit(WORK_TX_HALT, &hw->usb_flags)) {
 			netdev_warn(hw->wlandev->netdev,
-			       "%s tx pipe stalled: requesting reset\n",
-			       hw->wlandev->netdev->name);
+				    "%s tx pipe stalled: requesting reset\n",
+				    hw->wlandev->netdev->name);
 			schedule_work(&hw->usb_work);
 		}
 
@@ -3904,7 +3895,7 @@ delresp:
 ----------------------------------------------------------------*/
 static void hfa384x_usbctlx_reqtimerfn(unsigned long data)
 {
-	hfa384x_t *hw = (hfa384x_t *) data;
+	hfa384x_t *hw = (hfa384x_t *)data;
 	unsigned long flags;
 
 	spin_lock_irqsave(&hw->ctlxq.lock, flags);
@@ -3962,7 +3953,7 @@ static void hfa384x_usbctlx_reqtimerfn(unsigned long data)
 ----------------------------------------------------------------*/
 static void hfa384x_usbctlx_resptimerfn(unsigned long data)
 {
-	hfa384x_t *hw = (hfa384x_t *) data;
+	hfa384x_t *hw = (hfa384x_t *)data;
 	unsigned long flags;
 
 	spin_lock_irqsave(&hw->ctlxq.lock, flags);
@@ -4001,7 +3992,7 @@ static void hfa384x_usbctlx_resptimerfn(unsigned long data)
 ----------------------------------------------------------------*/
 static void hfa384x_usb_throttlefn(unsigned long data)
 {
-	hfa384x_t *hw = (hfa384x_t *) data;
+	hfa384x_t *hw = (hfa384x_t *)data;
 	unsigned long flags;
 
 	spin_lock_irqsave(&hw->ctlxq.lock, flags);
@@ -4139,13 +4130,13 @@ static int hfa384x_isgood_pdrcode(u16 pdrcode)
 	default:
 		if (pdrcode < 0x1000) {
 			/* code is OK, but we don't know exactly what it is */
-			pr_debug("Encountered unknown PDR#=0x%04x, "
-				 "assuming it's ok.\n", pdrcode);
+			pr_debug("Encountered unknown PDR#=0x%04x, assuming it's ok.\n",
+				 pdrcode);
 			return 1;
 		} else {
 			/* bad code */
-			pr_debug("Encountered unknown PDR#=0x%04x, "
-				 "(>=0x1000), assuming it's bad.\n", pdrcode);
+			pr_debug("Encountered unknown PDR#=0x%04x, (>=0x1000), assuming it's bad.\n",
+				 pdrcode);
 			return 0;
 		}
 		break;
diff --git a/drivers/staging/wlan-ng/p80211conv.c b/drivers/staging/wlan-ng/p80211conv.c
index 3df753b51e8..913676e1797 100644
--- a/drivers/staging/wlan-ng/p80211conv.c
+++ b/drivers/staging/wlan-ng/p80211conv.c
@@ -195,8 +195,8 @@ int skb_ether_to_p80211(wlandevice_t *wlandev, u32 ethconv,
 		memcpy(p80211_hdr->a3.a3, &e_hdr.saddr, ETH_ALEN);
 		break;
 	default:
-		printk(KERN_ERR
-		       "Error: Converting eth to wlan in unknown mode.\n");
+		netdev_err(wlandev->netdev,
+			   "Error: Converting eth to wlan in unknown mode.\n");
 		return 1;
 		break;
 	}
@@ -213,7 +213,7 @@ int skb_ether_to_p80211(wlandevice_t *wlandev, u32 ethconv,
 				  (wlandev->hostwep & HOSTWEP_DEFAULTKEY_MASK),
 				  p80211_wep->iv, p80211_wep->icv);
 		if (foo) {
-			printk(KERN_WARNING
+			netdev_warn(wlandev->netdev,
 			       "Host en-WEP failed, dropping frame (%d).\n",
 			       foo);
 			return 2;
@@ -310,7 +310,7 @@ int skb_p80211_to_ether(wlandevice_t *wlandev, u32 ethconv,
 	} else {
 		payload_offset = WLAN_HDR_A4_LEN;
 		if (payload_length < WLAN_HDR_A4_LEN - WLAN_HDR_A3_LEN) {
-			printk(KERN_ERR "A4 frame too short!\n");
+			netdev_err(netdev, "A4 frame too short!\n");
 			return 1;
 		}
 		payload_length -= (WLAN_HDR_A4_LEN - WLAN_HDR_A3_LEN);
@@ -322,8 +322,8 @@ int skb_p80211_to_ether(wlandevice_t *wlandev, u32 ethconv,
 	if ((wlandev->hostwep & HOSTWEP_PRIVACYINVOKED) && WLAN_GET_FC_ISWEP(fc)
 	    && (wlandev->hostwep & HOSTWEP_DECRYPT)) {
 		if (payload_length <= 8) {
-			printk(KERN_ERR "WEP frame too short (%u).\n",
-			       skb->len);
+			netdev_err(netdev,
+				   "WEP frame too short (%u).\n", skb->len);
 			return 1;
 		}
 		foo = wep_decrypt(wlandev, skb->data + payload_offset + 4,
@@ -367,7 +367,7 @@ int skb_p80211_to_ether(wlandevice_t *wlandev, u32 ethconv,
 		if (payload_length > (netdev->mtu + WLAN_ETHHDR_LEN)) {
 			/* A bogus length ethfrm has been encap'd. */
 			/* Is someone trying an oflow attack? */
-			printk(KERN_ERR "ENCAP frame too large (%d > %d)\n",
+			netdev_err(netdev, "ENCAP frame too large (%d > %d)\n",
 			       payload_length, netdev->mtu + WLAN_ETHHDR_LEN);
 			return 1;
 		}
@@ -396,7 +396,7 @@ int skb_p80211_to_ether(wlandevice_t *wlandev, u32 ethconv,
 		if (payload_length > netdev->mtu) {
 			/* A bogus length ethfrm has been sent. */
 			/* Is someone trying an oflow attack? */
-			printk(KERN_ERR "SNAP frame too large (%d > %d)\n",
+			netdev_err(netdev, "SNAP frame too large (%d > %d)\n",
 			       payload_length, netdev->mtu);
 			return 1;
 		}
@@ -428,7 +428,7 @@ int skb_p80211_to_ether(wlandevice_t *wlandev, u32 ethconv,
 			> netdev->mtu) {
 			/* A bogus length ethfrm has been sent. */
 			/* Is someone trying an oflow attack? */
-			printk(KERN_ERR "DIXII frame too large (%ld > %d)\n",
+			netdev_err(netdev, "DIXII frame too large (%ld > %d)\n",
 			       (long int)(payload_length -
 					sizeof(struct wlan_llc) -
 					sizeof(struct wlan_snap)), netdev->mtu);
@@ -463,7 +463,7 @@ int skb_p80211_to_ether(wlandevice_t *wlandev, u32 ethconv,
 		if (payload_length > netdev->mtu) {
 			/* A bogus length ethfrm has been sent. */
 			/* Is someone trying an oflow attack? */
-			printk(KERN_ERR "OTHER frame too large (%d > %d)\n",
+			netdev_err(netdev, "OTHER frame too large (%d > %d)\n",
 			       payload_length, netdev->mtu);
 			return 1;
 		}
@@ -603,8 +603,8 @@ int p80211skb_rxmeta_attach(struct wlandevice *wlandev, struct sk_buff *skb)
 
 	/* If these already have metadata, we error out! */
 	if (P80211SKB_RXMETA(skb) != NULL) {
-		printk(KERN_ERR "%s: RXmeta already attached!\n",
-		       wlandev->name);
+		netdev_err(wlandev->netdev,
+			   "%s: RXmeta already attached!\n", wlandev->name);
 		result = 0;
 		goto exit;
 	}
@@ -613,8 +613,8 @@ int p80211skb_rxmeta_attach(struct wlandevice *wlandev, struct sk_buff *skb)
 	rxmeta = kzalloc(sizeof(struct p80211_rxmeta), GFP_ATOMIC);
 
 	if (rxmeta == NULL) {
-		printk(KERN_ERR "%s: Failed to allocate rxmeta.\n",
-		       wlandev->name);
+		netdev_err(wlandev->netdev,
+			   "%s: Failed to allocate rxmeta.\n", wlandev->name);
 		result = 1;
 		goto exit;
 	}
@@ -656,6 +656,7 @@ void p80211skb_free(struct wlandevice *wlandev, struct sk_buff *skb)
 	if (meta && meta->rx)
 		p80211skb_rxmeta_detach(skb);
 	else
-		printk(KERN_ERR "Freeing an skb (%p) w/ no frmmeta.\n", skb);
+		netdev_err(wlandev->netdev,
+			   "Freeing an skb (%p) w/ no frmmeta.\n", skb);
 	dev_kfree_skb(skb);
 }
diff --git a/drivers/staging/wlan-ng/p80211netdev.c b/drivers/staging/wlan-ng/p80211netdev.c
index e3ae8024d3a..00b186c5972 100644
--- a/drivers/staging/wlan-ng/p80211netdev.c
+++ b/drivers/staging/wlan-ng/p80211netdev.c
@@ -90,9 +90,6 @@
 
 #include "cfg80211.c"
 
-/* Support functions */
-static void p80211netdev_rx_bh(unsigned long arg);
-
 /* netdevice method functions */
 static int p80211knetdev_init(netdevice_t *netdev);
 static struct net_device_stats *p80211knetdev_get_stats(netdevice_t *netdev);
@@ -243,25 +240,59 @@ void p80211netdev_rx(wlandevice_t *wlandev, struct sk_buff *skb)
 	tasklet_schedule(&wlandev->rx_bh);
 }
 
-/*----------------------------------------------------------------
-* p80211netdev_rx_bh
-*
-* Deferred processing of all received frames.
-*
-* Arguments:
-*	wlandev		WLAN network device structure
-*	skb		skbuff containing a full 802.11 frame.
-* Returns:
-*	nothing
-* Side effects:
-*
-----------------------------------------------------------------*/
+#define CONV_TO_ETHER_SKIPPED	0x01
+#define CONV_TO_ETHER_FAILED	0x02
+
+/**
+ * p80211_convert_to_ether - conversion from 802.11 frame to ethernet frame
+ * @wlandev: pointer to WLAN device
+ * @skb: pointer to socket buffer
+ *
+ * Returns: 0 if conversion succeeded
+ *	    CONV_TO_ETHER_FAILED if conversion failed
+ *	    CONV_TO_ETHER_SKIPPED if frame is ignored
+ */
+static int p80211_convert_to_ether(wlandevice_t *wlandev, struct sk_buff *skb)
+{
+	struct p80211_hdr_a3 *hdr;
+
+	hdr = (struct p80211_hdr_a3 *) skb->data;
+	if (p80211_rx_typedrop(wlandev, hdr->fc))
+		return CONV_TO_ETHER_SKIPPED;
+
+	/* perform mcast filtering: allow my local address through but reject
+	 * anything else that isn't multicast
+	 */
+	if (wlandev->netdev->flags & IFF_ALLMULTI) {
+		if (!ether_addr_equal_unaligned(wlandev->netdev->dev_addr,
+						hdr->a1)) {
+			if (!is_multicast_ether_addr(hdr->a1))
+				return CONV_TO_ETHER_SKIPPED;
+		}
+	}
+
+	if (skb_p80211_to_ether(wlandev, wlandev->ethconv, skb) == 0) {
+		skb->dev->last_rx = jiffies;
+		wlandev->linux_stats.rx_packets++;
+		wlandev->linux_stats.rx_bytes += skb->len;
+		netif_rx_ni(skb);
+		return 0;
+	}
+
+	netdev_dbg(wlandev->netdev, "p80211_convert_to_ether failed.\n");
+	return CONV_TO_ETHER_FAILED;
+}
+
+/**
+ * p80211netdev_rx_bh - deferred processing of all received frames
+ *
+ * @arg: pointer to WLAN network device structure (cast to unsigned long)
+ */
 static void p80211netdev_rx_bh(unsigned long arg)
 {
 	wlandevice_t *wlandev = (wlandevice_t *) arg;
 	struct sk_buff *skb = NULL;
 	netdevice_t *dev = wlandev->netdev;
-	struct p80211_hdr_a3 *hdr;
 
 	/* Let's empty our our queue */
 	while ((skb = skb_dequeue(&wlandev->nsd_rxq))) {
@@ -284,37 +315,8 @@ static void p80211netdev_rx_bh(unsigned long arg)
 				netif_rx_ni(skb);
 				continue;
 			} else {
-				hdr = (struct p80211_hdr_a3 *) skb->data;
-				if (p80211_rx_typedrop(wlandev, hdr->fc)) {
-					dev_kfree_skb(skb);
-					continue;
-				}
-
-				/* perform mcast filtering */
-				if (wlandev->netdev->flags & IFF_ALLMULTI) {
-					/* allow my local address through */
-					if (memcmp
-					    (hdr->a1, wlandev->netdev->dev_addr,
-					     ETH_ALEN) != 0) {
-						/* but reject anything else that
-						   isn't multicast */
-						if (!(hdr->a1[0] & 0x01)) {
-							dev_kfree_skb(skb);
-							continue;
-						}
-					}
-				}
-
-				if (skb_p80211_to_ether
-				    (wlandev, wlandev->ethconv, skb) == 0) {
-					skb->dev->last_rx = jiffies;
-					wlandev->linux_stats.rx_packets++;
-					wlandev->linux_stats.rx_bytes +=
-					    skb->len;
-					netif_rx_ni(skb);
+				if (!p80211_convert_to_ether(wlandev, skb))
 					continue;
-				}
-				pr_debug("p80211_to_ether failed.\n");
 			}
 		}
 		dev_kfree_skb(skb);
@@ -363,7 +365,7 @@ static int p80211knetdev_hard_start_xmit(struct sk_buff *skb,
 	memset(&p80211_wep, 0, sizeof(struct p80211_metawep));
 
 	if (netif_queue_stopped(netdev)) {
-		pr_debug("called when queue stopped.\n");
+		netdev_dbg(netdev, "called when queue stopped.\n");
 		result = 1;
 		goto failed;
 	}
@@ -383,8 +385,7 @@ static int p80211knetdev_hard_start_xmit(struct sk_buff *skb,
 		 */
 		if (skb->protocol != ETH_P_80211_RAW) {
 			netif_start_queue(wlandev->netdev);
-			printk(KERN_NOTICE
-			       "Tx attempt prior to association, frame dropped.\n");
+			netdev_notice(netdev, "Tx attempt prior to association, frame dropped.\n");
 			wlandev->linux_stats.tx_dropped++;
 			result = 0;
 			goto failed;
@@ -406,8 +407,8 @@ static int p80211knetdev_hard_start_xmit(struct sk_buff *skb,
 		    (wlandev, wlandev->ethconv, skb, &p80211_hdr,
 		     &p80211_wep) != 0) {
 			/* convert failed */
-			pr_debug("ether_to_80211(%d) failed.\n",
-				 wlandev->ethconv);
+			netdev_dbg(netdev, "ether_to_80211(%d) failed.\n",
+				   wlandev->ethconv);
 			result = 1;
 			goto failed;
 		}
@@ -432,17 +433,17 @@ static int p80211knetdev_hard_start_xmit(struct sk_buff *skb,
 		result = NETDEV_TX_OK;
 	} else if (txresult == 1) {
 		/* success, no more avail */
-		pr_debug("txframe success, no more bufs\n");
+		netdev_dbg(netdev, "txframe success, no more bufs\n");
 		/* netdev->tbusy = 1;  don't set here, irqhdlr */
 		/*   may have already cleared it */
 		result = NETDEV_TX_OK;
 	} else if (txresult == 2) {
 		/* alloc failure, drop frame */
-		pr_debug("txframe returned alloc_fail\n");
+		netdev_dbg(netdev, "txframe returned alloc_fail\n");
 		result = NETDEV_TX_BUSY;
 	} else {
 		/* buffer full or queue busy, drop frame. */
-		pr_debug("txframe returned full or busy\n");
+		netdev_dbg(netdev, "txframe returned full or busy\n");
 		result = NETDEV_TX_BUSY;
 	}
 
@@ -562,7 +563,7 @@ static int p80211knetdev_do_ioctl(netdevice_t *dev, struct ifreq *ifr, int cmd)
 	wlandevice_t *wlandev = dev->ml_priv;
 	u8 *msgbuf;
 
-	pr_debug("rx'd ioctl, cmd=%d, len=%d\n", cmd, req->len);
+	netdev_dbg(dev, "rx'd ioctl, cmd=%d, len=%d\n", cmd, req->len);
 
 #ifdef SIOCETHTOOL
 	if (cmd == SIOCETHTOOL) {
@@ -683,8 +684,7 @@ static int p80211knetdev_set_mac_address(netdevice_t *dev, void *addr)
 	 * change the netdev address
 	 */
 	if (result != 0 || resultcode->data != P80211ENUM_resultcode_success) {
-		printk(KERN_ERR
-		       "Low-level driver failed dot11req_mibset(dot11MACAddress).\n");
+		netdev_err(dev, "Low-level driver failed dot11req_mibset(dot11MACAddress).\n");
 		result = -EADDRNOTAVAIL;
 	} else {
 		/* everything's ok, change the addr in netdev */
@@ -763,7 +763,7 @@ int wlan_setup(wlandevice_t *wlandev, struct device *physdev)
 	/* Allocate and initialize the wiphy struct */
 	wiphy = wlan_create_wiphy(physdev, wlandev);
 	if (wiphy == NULL) {
-		printk(KERN_ERR "Failed to alloc wiphy.\n");
+		dev_err(physdev, "Failed to alloc wiphy.\n");
 		return 1;
 	}
 
@@ -771,7 +771,7 @@ int wlan_setup(wlandevice_t *wlandev, struct device *physdev)
 	netdev = alloc_netdev(sizeof(struct wireless_dev), "wlan%d",
 				ether_setup);
 	if (netdev == NULL) {
-		printk(KERN_ERR "Failed to alloc netdev.\n");
+		dev_err(physdev, "Failed to alloc netdev.\n");
 		wlan_free_wiphy(wiphy);
 		result = 1;
 	} else {
@@ -947,7 +947,8 @@ static int p80211_rx_typedrop(wlandevice_t *wlandev, u16 fc)
 	ftype = WLAN_GET_FC_FTYPE(fc);
 	fstype = WLAN_GET_FC_FSTYPE(fc);
 #if 0
-	pr_debug("rx_typedrop : ftype=%d fstype=%d.\n", ftype, fstype);
+	netdev_dbg(wlandev->netdev, "rx_typedrop : ftype=%d fstype=%d.\n",
+		   ftype, fstype);
 #endif
 	switch (ftype) {
 	case WLAN_FTYPE_MGMT:
@@ -956,7 +957,7 @@ static int p80211_rx_typedrop(wlandevice_t *wlandev, u16 fc)
 			drop = 1;
 			break;
 		}
-		pr_debug("rx'd mgmt:\n");
+		netdev_dbg(wlandev->netdev, "rx'd mgmt:\n");
 		wlandev->rx.mgmt++;
 		switch (fstype) {
 		case WLAN_FSTYPE_ASSOCREQ:
@@ -1018,7 +1019,7 @@ static int p80211_rx_typedrop(wlandevice_t *wlandev, u16 fc)
 			drop = 1;
 			break;
 		}
-		pr_debug("rx'd ctl:\n");
+		netdev_dbg(wlandev->netdev, "rx'd ctl:\n");
 		wlandev->rx.ctl++;
 		switch (fstype) {
 		case WLAN_FSTYPE_PSPOLL:
@@ -1070,19 +1071,19 @@ static int p80211_rx_typedrop(wlandevice_t *wlandev, u16 fc)
 			wlandev->rx.data__cfack_cfpoll++;
 			break;
 		case WLAN_FSTYPE_NULL:
-			pr_debug("rx'd data:null\n");
+			netdev_dbg(wlandev->netdev, "rx'd data:null\n");
 			wlandev->rx.null++;
 			break;
 		case WLAN_FSTYPE_CFACK:
-			pr_debug("rx'd data:cfack\n");
+			netdev_dbg(wlandev->netdev, "rx'd data:cfack\n");
 			wlandev->rx.cfack++;
 			break;
 		case WLAN_FSTYPE_CFPOLL:
-			pr_debug("rx'd data:cfpoll\n");
+			netdev_dbg(wlandev->netdev, "rx'd data:cfpoll\n");
 			wlandev->rx.cfpoll++;
 			break;
 		case WLAN_FSTYPE_CFACK_CFPOLL:
-			pr_debug("rx'd data:cfack_cfpoll\n");
+			netdev_dbg(wlandev->netdev, "rx'd data:cfack_cfpoll\n");
 			wlandev->rx.cfack_cfpoll++;
 			break;
 		default:
@@ -1103,8 +1104,8 @@ static void p80211knetdev_tx_timeout(netdevice_t *netdev)
 	if (wlandev->tx_timeout) {
 		wlandev->tx_timeout(wlandev);
 	} else {
-		printk(KERN_WARNING "Implement tx_timeout for %s\n",
-		       wlandev->nsdname);
+		netdev_warn(netdev, "Implement tx_timeout for %s\n",
+			    wlandev->nsdname);
 		netif_wake_queue(wlandev->netdev);
 	}
 }
diff --git a/drivers/staging/wlan-ng/p80211req.c b/drivers/staging/wlan-ng/p80211req.c
index cdfd808d685..7221379c974 100644
--- a/drivers/staging/wlan-ng/p80211req.c
+++ b/drivers/staging/wlan-ng/p80211req.c
@@ -95,7 +95,6 @@ static void p80211req_mibset_mibget(wlandevice_t *wlandev,
 ----------------------------------------------------------------*/
 int p80211req_dorequest(wlandevice_t *wlandev, u8 *msgbuf)
 {
-	int result = 0;
 	struct p80211msg *msg = (struct p80211msg *) msgbuf;
 
 	/* Check to make sure the MSD is running */
@@ -109,9 +108,9 @@ int p80211req_dorequest(wlandevice_t *wlandev, u8 *msgbuf)
 	/* Check Permissions */
 	if (!capable(CAP_NET_ADMIN) &&
 	(msg->msgcode != DIDmsg_dot11req_mibget)) {
-		printk(KERN_ERR
-		       "%s: only dot11req_mibget allowed for non-root.\n",
-		       wlandev->name);
+		netdev_err(wlandev->netdev,
+			   "%s: only dot11req_mibget allowed for non-root.\n",
+			   wlandev->name);
 		return -EPERM;
 	}
 
@@ -129,7 +128,7 @@ int p80211req_dorequest(wlandevice_t *wlandev, u8 *msgbuf)
 		wlandev->mlmerequest(wlandev, msg);
 
 	clear_bit(1, &(wlandev->request_pending));
-	return result;	/* if result==0, msg->status still may contain an err */
+	return 0;	/* if result==0, msg->status still may contain an err */
 }
 
 /*----------------------------------------------------------------
diff --git a/drivers/staging/wlan-ng/prism2fw.c b/drivers/staging/wlan-ng/prism2fw.c
index 2b0c23587df..f7870355c69 100644
--- a/drivers/staging/wlan-ng/prism2fw.c
+++ b/drivers/staging/wlan-ng/prism2fw.c
@@ -205,19 +205,20 @@ static int prism2_fwtry(struct usb_device *udev, wlandevice_t *wlandev)
 {
 	const struct firmware *fw_entry = NULL;
 
-	printk(KERN_INFO "prism2_usb: Checking for firmware %s\n",
+	netdev_info(wlandev->netdev, "prism2_usb: Checking for firmware %s\n",
 	       PRISM2_USB_FWFILE);
 	if (request_ihex_firmware(&fw_entry,
 				  PRISM2_USB_FWFILE, &udev->dev) != 0) {
-		printk(KERN_INFO
+		netdev_info(wlandev->netdev,
 		       "prism2_usb: Firmware not available, but not essential\n");
-		printk(KERN_INFO
+		netdev_info(wlandev->netdev,
 		       "prism2_usb: can continue to use card anyway.\n");
 		return 1;
 	}
 
-	printk(KERN_INFO "prism2_usb: %s will be processed, size %zu\n",
-	       PRISM2_USB_FWFILE, fw_entry->size);
+	netdev_info(wlandev->netdev,
+		    "prism2_usb: %s will be processed, size %zu\n",
+		    PRISM2_USB_FWFILE, fw_entry->size);
 	prism2_fwapply((const struct ihex_binrec *)fw_entry->data, wlandev);
 
 	release_firmware(fw_entry);
@@ -275,7 +276,7 @@ static int prism2_fwapply(const struct ihex_binrec *rfptr, wlandevice_t *wlandev
 
 	/* Build the PDA we're going to use. */
 	if (read_cardpda(&pda, wlandev)) {
-		printk(KERN_ERR "load_cardpda failed, exiting.\n");
+		netdev_err(wlandev->netdev, "load_cardpda failed, exiting.\n");
 		return 1;
 	}
 
@@ -299,7 +300,7 @@ static int prism2_fwapply(const struct ihex_binrec *rfptr, wlandevice_t *wlandev
 	/* DIDmsg_dot11req_mibget */
 	prism2mgmt_mibset_mibget(wlandev, &getmsg);
 	if (getmsg.resultcode.data != P80211ENUM_resultcode_success)
-		printk(KERN_ERR "Couldn't fetch PRI-SUP info\n");
+		netdev_err(wlandev->netdev, "Couldn't fetch PRI-SUP info\n");
 
 	/* Already in host order */
 	priid.role = *data++;
@@ -311,19 +312,21 @@ static int prism2_fwapply(const struct ihex_binrec *rfptr, wlandevice_t *wlandev
 	/* Read the S3 file */
 	result = read_fwfile(rfptr);
 	if (result) {
-		printk(KERN_ERR "Failed to read the data exiting.\n");
+		netdev_err(wlandev->netdev,
+			   "Failed to read the data exiting.\n");
 		return 1;
 	}
 
 	result = validate_identity();
 
 	if (result) {
-		printk(KERN_ERR "Incompatible firmware image.\n");
+		netdev_err(wlandev->netdev, "Incompatible firmware image.\n");
 		return 1;
 	}
 
 	if (startaddr == 0x00000000) {
-		printk(KERN_ERR "Can't RAM download a Flash image!\n");
+		netdev_err(wlandev->netdev,
+			   "Can't RAM download a Flash image!\n");
 		return 1;
 	}
 
@@ -333,20 +336,20 @@ static int prism2_fwapply(const struct ihex_binrec *rfptr, wlandevice_t *wlandev
 	/* Do any plugging */
 	result = plugimage(fchunk, nfchunks, s3plug, ns3plug, &pda);
 	if (result) {
-		printk(KERN_ERR "Failed to plug data.\n");
+		netdev_err(wlandev->netdev, "Failed to plug data.\n");
 		return 1;
 	}
 
 	/* Insert any CRCs */
 	if (crcimage(fchunk, nfchunks, s3crc, ns3crc)) {
-		printk(KERN_ERR "Failed to insert all CRCs\n");
+		netdev_err(wlandev->netdev, "Failed to insert all CRCs\n");
 		return 1;
 	}
 
 	/* Write the image */
 	result = writeimage(wlandev, fchunk, nfchunks);
 	if (result) {
-		printk(KERN_ERR "Failed to ramwrite image data.\n");
+		netdev_err(wlandev->netdev, "Failed to ramwrite image data.\n");
 		return 1;
 	}
 
@@ -354,7 +357,7 @@ static int prism2_fwapply(const struct ihex_binrec *rfptr, wlandevice_t *wlandev
 	free_chunks(fchunk, &nfchunks);
 	free_srecs();
 
-	printk(KERN_INFO "prism2_usb: firmware loading finished.\n");
+	netdev_info(wlandev->netdev, "prism2_usb: firmware loading finished.\n");
 
 	return result;
 }
@@ -410,8 +413,7 @@ static int crcimage(struct imgchunk *fchunk, unsigned int nfchunks,
 				break;
 		}
 		if (c >= nfchunks) {
-			printk(KERN_ERR
-			       "Failed to find chunk for "
+			pr_err("Failed to find chunk for "
 			       "crcrec[%d], addr=0x%06x len=%d , "
 			       "aborting crc.\n",
 			       i, s3crc[i].addr, s3crc[i].len);
@@ -537,8 +539,7 @@ static int mkimage(struct imgchunk *clist, unsigned int *ccnt)
 	for (i = 0; i < *ccnt; i++) {
 		clist[i].data = kzalloc(clist[i].len, GFP_KERNEL);
 		if (clist[i].data == NULL) {
-			printk(KERN_ERR
-			       "failed to allocate image space, exitting.\n");
+			pr_err("failed to allocate image space, exitting.\n");
 			return 1;
 		}
 		pr_debug("chunk[%d]: addr=0x%06x len=%d\n",
@@ -556,8 +557,7 @@ static int mkimage(struct imgchunk *clist, unsigned int *ccnt)
 				break;
 		}
 		if (((unsigned int)j) >= (*ccnt)) {
-			printk(KERN_ERR
-			       "s3rec(a=0x%06x,l=%d), no chunk match, exiting.\n",
+			pr_err("s3rec(a=0x%06x,l=%d), no chunk match, exiting.\n",
 			       s3start, s3data[i].len);
 			return 1;
 		}
@@ -627,8 +627,7 @@ static int mkpdrlist(struct pda *pda)
 
 	}
 	if (curroff >= (HFA384x_PDA_LEN_MAX / 2)) {
-		printk(KERN_ERR
-		       "no end record found or invalid lengths in "
+		pr_err("no end record found or invalid lengths in "
 		       "PDR data, exiting. %x %d\n", curroff, pda->nrec);
 		return 1;
 	}
@@ -685,8 +684,7 @@ static int plugimage(struct imgchunk *fchunk, unsigned int nfchunks,
 			j = -1;
 		}
 		if (j >= pda->nrec && j != -1) { /*  if no matching PDR, fail */
-			printk(KERN_WARNING
-			       "warning: Failed to find PDR for "
+			pr_warn("warning: Failed to find PDR for "
 			       "plugrec 0x%04x.\n", s3plug[i].itemcode);
 			continue;	/* and move on to the next PDR */
 #if 0
@@ -704,8 +702,7 @@ static int plugimage(struct imgchunk *fchunk, unsigned int nfchunks,
 
 		/* Validate plug len against PDR len */
 		if (j != -1 && s3plug[i].len < le16_to_cpu(pda->rec[j]->len)) {
-			printk(KERN_ERR
-			       "error: Plug vs. PDR len mismatch for "
+			pr_err("error: Plug vs. PDR len mismatch for "
 			       "plugrec 0x%04x, abort plugging.\n",
 			       s3plug[i].itemcode);
 			result = 1;
@@ -720,8 +717,7 @@ static int plugimage(struct imgchunk *fchunk, unsigned int nfchunks,
 				break;
 		}
 		if (c >= nfchunks) {
-			printk(KERN_ERR
-			       "error: Failed to find image chunk for "
+			pr_err("error: Failed to find image chunk for "
 			       "plugrec 0x%04x.\n", s3plug[i].itemcode);
 			result = 1;
 			continue;
@@ -898,7 +894,7 @@ static int read_fwfile(const struct ihex_binrec *record)
 
 			ns3plug++;
 			if (ns3plug == S3PLUG_MAX) {
-				printk(KERN_ERR "S3 plugrec limit reached - aborting\n");
+				pr_err("S3 plugrec limit reached - aborting\n");
 				return 1;
 			}
 			break;
@@ -915,7 +911,7 @@ static int read_fwfile(const struct ihex_binrec *record)
 				      s3crc[ns3crc].dowrite);
 			ns3crc++;
 			if (ns3crc == S3CRC_MAX) {
-				printk(KERN_ERR "S3 crcrec limit reached - aborting\n");
+				pr_err("S3 crcrec limit reached - aborting\n");
 				return 1;
 			}
 			break;
@@ -929,7 +925,7 @@ static int read_fwfile(const struct ihex_binrec *record)
 				      s3info[ns3info].len,
 				      s3info[ns3info].type);
 			if (((s3info[ns3info].len - 1) * sizeof(u16)) > sizeof(s3info[ns3info].info)) {
-				printk(KERN_ERR " S3 inforec length too long - aborting\n");
+				pr_err("S3 inforec length too long - aborting\n");
 				return 1;
 			}
 
@@ -943,7 +939,7 @@ static int read_fwfile(const struct ihex_binrec *record)
 
 			ns3info++;
 			if (ns3info == S3INFO_MAX) {
-				printk(KERN_ERR "S3 inforec limit reached - aborting\n");
+				pr_err("S3 inforec limit reached - aborting\n");
 				return 1;
 			}
 			break;
@@ -953,7 +949,7 @@ static int read_fwfile(const struct ihex_binrec *record)
 			s3data[ns3data].data = (uint8_t *) record->data;
 			ns3data++;
 			if (ns3data == S3DATA_MAX) {
-				printk(KERN_ERR "S3 datarec limit reached - aborting\n");
+				pr_err("S3 datarec limit reached - aborting\n");
 				return 1;
 			}
 			break;
@@ -997,9 +993,9 @@ static int writeimage(wlandevice_t *wlandev, struct imgchunk *fchunk,
 	if (!rstmsg || !rwrmsg) {
 		kfree(rstmsg);
 		kfree(rwrmsg);
-		printk(KERN_ERR
-		       "writeimage: no memory for firmware download, "
-		       "aborting download\n");
+		netdev_err(wlandev->netdev,
+			   "writeimage: no memory for firmware download, "
+			   "aborting download\n");
 		return -ENOMEM;
 	}
 
@@ -1042,16 +1038,16 @@ static int writeimage(wlandevice_t *wlandev, struct imgchunk *fchunk,
 
 	result = prism2mgmt_ramdl_state(wlandev, rstmsg);
 	if (result) {
-		printk(KERN_ERR
-		       "writeimage state enable failed w/ result=%d, "
-		       "aborting download\n", result);
+		netdev_err(wlandev->netdev,
+			   "writeimage state enable failed w/ result=%d, "
+			   "aborting download\n", result);
 		goto free_result;
 	}
 	resultcode = rstmsg->resultcode.data;
 	if (resultcode != P80211ENUM_resultcode_success) {
-		printk(KERN_ERR
-		       "writeimage()->xxxdl_state msg indicates failure, "
-		       "w/ resultcode=%d, aborting download.\n", resultcode);
+		netdev_err(wlandev->netdev,
+			   "writeimage()->xxxdl_state msg indicates failure, "
+			   "w/ resultcode=%d, aborting download.\n", resultcode);
 		result = 1;
 		goto free_result;
 	}
@@ -1085,15 +1081,14 @@ static int writeimage(wlandevice_t *wlandev, struct imgchunk *fchunk,
 
 			/* Check the results */
 			if (result) {
-				printk(KERN_ERR
-				       "writeimage chunk write failed w/ result=%d, "
-				       "aborting download\n", result);
+				netdev_err(wlandev->netdev,
+					   "writeimage chunk write failed w/ "
+					   "result=%d, aborting download\n", result);
 				goto free_result;
 			}
 			resultcode = rstmsg->resultcode.data;
 			if (resultcode != P80211ENUM_resultcode_success) {
-				printk(KERN_ERR
-				       "writeimage()->xxxdl_write msg indicates failure, "
+				pr_err("writeimage()->xxxdl_write msg indicates failure, "
 				       "w/ resultcode=%d, aborting download.\n",
 				       resultcode);
 				result = 1;
@@ -1110,16 +1105,16 @@ static int writeimage(wlandevice_t *wlandev, struct imgchunk *fchunk,
 
 	result = prism2mgmt_ramdl_state(wlandev, rstmsg);
 	if (result) {
-		printk(KERN_ERR
-		       "writeimage state disable failed w/ result=%d, "
-		       "aborting download\n", result);
+		netdev_err(wlandev->netdev,
+			   "writeimage state disable failed w/ result=%d, "
+			   "aborting download\n", result);
 		goto free_result;
 	}
 	resultcode = rstmsg->resultcode.data;
 	if (resultcode != P80211ENUM_resultcode_success) {
-		printk(KERN_ERR
-		       "writeimage()->xxxdl_state msg indicates failure, "
-		       "w/ resultcode=%d, aborting download.\n", resultcode);
+		netdev_err(wlandev->netdev,
+			   "writeimage()->xxxdl_state msg indicates failure, "
+			   "w/ resultcode=%d, aborting download.\n", resultcode);
 		result = 1;
 		goto free_result;
 	}
diff --git a/drivers/staging/wlan-ng/prism2mgmt.c b/drivers/staging/wlan-ng/prism2mgmt.c
index a9909f6b000..d110b362c3b 100644
--- a/drivers/staging/wlan-ng/prism2mgmt.c
+++ b/drivers/staging/wlan-ng/prism2mgmt.c
@@ -129,8 +129,8 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 				     hw->ident_sta_fw.minor,
 				     hw->ident_sta_fw.variant) <
 	    HFA384x_FIRMWARE_VERSION(1, 3, 2)) {
-		printk(KERN_ERR
-		       "HostScan not supported with current firmware (<1.3.2).\n");
+		netdev_err(wlandev->netdev,
+			   "HostScan not supported with current firmware (<1.3.2).\n");
 		result = 1;
 		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
 		goto exit;
@@ -143,8 +143,8 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 					  HFA384x_RID_CNFROAMINGMODE,
 					  &roamingmode);
 	if (result) {
-		printk(KERN_ERR "getconfig(ROAMMODE) failed. result=%d\n",
-		       result);
+		netdev_err(wlandev->netdev,
+			   "getconfig(ROAMMODE) failed. result=%d\n", result);
 		msg->resultcode.data =
 		    P80211ENUM_resultcode_implementation_failure;
 		goto exit;
@@ -155,8 +155,8 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 					  HFA384x_RID_CNFROAMINGMODE,
 					  HFA384x_ROAMMODE_HOSTSCAN_HOSTROAM);
 	if (result) {
-		printk(KERN_ERR "setconfig(ROAMINGMODE) failed. result=%d\n",
-		       result);
+		netdev_err(wlandev->netdev,
+			   "setconfig(ROAMINGMODE) failed. result=%d\n", result);
 		msg->resultcode.data =
 		    P80211ENUM_resultcode_implementation_failure;
 		goto exit;
@@ -176,8 +176,9 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 		    hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFPASSIVESCANCTRL,
 					     word);
 		if (result) {
-			printk(KERN_WARNING "Passive scan not supported with "
-			       "current firmware.  (<1.5.1)\n");
+			netdev_warn(wlandev->netdev,
+				    "Passive scan not supported with "
+				    "current firmware.  (<1.5.1)\n");
 		}
 	}
 
@@ -203,8 +204,8 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 	/* Enable the MAC port if it's not already enabled  */
 	result = hfa384x_drvr_getconfig16(hw, HFA384x_RID_PORTSTATUS, &word);
 	if (result) {
-		printk(KERN_ERR "getconfig(PORTSTATUS) failed. "
-		       "result=%d\n", result);
+		netdev_err(wlandev->netdev,
+			   "getconfig(PORTSTATUS) failed. result=%d\n", result);
 		msg->resultcode.data =
 		    P80211ENUM_resultcode_implementation_failure;
 		goto exit;
@@ -216,9 +217,9 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 					HFA384x_RID_CNFROAMINGMODE,
 					HFA384x_ROAMMODE_HOSTSCAN_HOSTROAM);
 		if (result) {
-			printk(KERN_ERR
-			       "setconfig(ROAMINGMODE) failed. result=%d\n",
-			       result);
+			netdev_err(wlandev->netdev,
+				   "setconfig(ROAMINGMODE) failed. result=%d\n",
+				   result);
 			msg->resultcode.data =
 			    P80211ENUM_resultcode_implementation_failure;
 			goto exit;
@@ -232,7 +233,7 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 						wordbuf,
 						HFA384x_RID_CNFOWNSSID_LEN);
 		if (result) {
-			printk(KERN_ERR "Failed to set OwnSSID.\n");
+			netdev_err(wlandev->netdev, "Failed to set OwnSSID.\n");
 			msg->resultcode.data =
 			    P80211ENUM_resultcode_implementation_failure;
 			goto exit;
@@ -241,7 +242,8 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 						wordbuf,
 						HFA384x_RID_CNFDESIREDSSID_LEN);
 		if (result) {
-			printk(KERN_ERR "Failed to set DesiredSSID.\n");
+			netdev_err(wlandev->netdev,
+				   "Failed to set DesiredSSID.\n");
 			msg->resultcode.data =
 			    P80211ENUM_resultcode_implementation_failure;
 			goto exit;
@@ -251,7 +253,8 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 						  HFA384x_RID_CNFPORTTYPE,
 						  HFA384x_PORTTYPE_IBSS);
 		if (result) {
-			printk(KERN_ERR "Failed to set CNFPORTTYPE.\n");
+			netdev_err(wlandev->netdev,
+				   "Failed to set CNFPORTTYPE.\n");
 			msg->resultcode.data =
 			    P80211ENUM_resultcode_implementation_failure;
 			goto exit;
@@ -261,15 +264,16 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 					HFA384x_RID_CREATEIBSS,
 					HFA384x_CREATEIBSS_JOINCREATEIBSS);
 		if (result) {
-			printk(KERN_ERR "Failed to set CREATEIBSS.\n");
+			netdev_err(wlandev->netdev,
+				   "Failed to set CREATEIBSS.\n");
 			msg->resultcode.data =
 			    P80211ENUM_resultcode_implementation_failure;
 			goto exit;
 		}
 		result = hfa384x_drvr_enable(hw, 0);
 		if (result) {
-			printk(KERN_ERR "drvr_enable(0) failed. "
-			       "result=%d\n", result);
+			netdev_err(wlandev->netdev,
+				   "drvr_enable(0) failed. result=%d\n", result);
 			msg->resultcode.data =
 			    P80211ENUM_resultcode_implementation_failure;
 			goto exit;
@@ -288,8 +292,8 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 					HFA384x_RID_HOSTSCAN, &scanreq,
 					sizeof(hfa384x_HostScanRequest_data_t));
 	if (result) {
-		printk(KERN_ERR "setconfig(SCANREQUEST) failed. result=%d\n",
-		       result);
+		netdev_err(wlandev->netdev,
+			   "setconfig(SCANREQUEST) failed. result=%d\n", result);
 		msg->resultcode.data =
 		    P80211ENUM_resultcode_implementation_failure;
 		goto exit;
@@ -310,8 +314,8 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 	if (istmpenable) {
 		result = hfa384x_drvr_disable(hw, 0);
 		if (result) {
-			printk(KERN_ERR "drvr_disable(0) failed. "
-			       "result=%d\n", result);
+			netdev_err(wlandev->netdev,
+				   "drvr_disable(0) failed. result=%d\n", result);
 			msg->resultcode.data =
 			    P80211ENUM_resultcode_implementation_failure;
 			goto exit;
@@ -322,8 +326,8 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 	result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFROAMINGMODE,
 					  roamingmode);
 	if (result) {
-		printk(KERN_ERR "setconfig(ROAMMODE) failed. result=%d\n",
-		       result);
+		netdev_err(wlandev->netdev,
+			   "setconfig(ROAMMODE) failed. result=%d\n", result);
 		msg->resultcode.data =
 		    P80211ENUM_resultcode_implementation_failure;
 		goto exit;
@@ -372,8 +376,9 @@ int prism2mgmt_scan_results(wlandevice_t *wlandev, void *msgp)
 	req->resultcode.status = P80211ENUM_msgitem_status_data_ok;
 
 	if (!hw->scanresults) {
-		printk(KERN_ERR
-		       "dot11req_scan_results can only be used after a successful dot11req_scan.\n");
+		netdev_err(wlandev->netdev,
+			   "dot11req_scan_results can only be used after "
+			   "a successful dot11req_scan.\n");
 		result = 2;
 		req->resultcode.data = P80211ENUM_resultcode_invalid_parameters;
 		goto exit;
@@ -555,14 +560,14 @@ int prism2mgmt_start(wlandevice_t *wlandev, void *msgp)
 	result = hfa384x_drvr_setconfig(hw, HFA384x_RID_CNFOWNSSID,
 					bytebuf, HFA384x_RID_CNFOWNSSID_LEN);
 	if (result) {
-		printk(KERN_ERR "Failed to set CnfOwnSSID\n");
+		netdev_err(wlandev->netdev, "Failed to set CnfOwnSSID\n");
 		goto failed;
 	}
 	result = hfa384x_drvr_setconfig(hw, HFA384x_RID_CNFDESIREDSSID,
 					bytebuf,
 					HFA384x_RID_CNFDESIREDSSID_LEN);
 	if (result) {
-		printk(KERN_ERR "Failed to set CnfDesiredSSID\n");
+		netdev_err(wlandev->netdev, "Failed to set CnfDesiredSSID\n");
 		goto failed;
 	}
 
@@ -574,7 +579,8 @@ int prism2mgmt_start(wlandevice_t *wlandev, void *msgp)
 	word = msg->beaconperiod.data;
 	result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFAPBCNint, word);
 	if (result) {
-		printk(KERN_ERR "Failed to set beacon period=%d.\n", word);
+		netdev_err(wlandev->netdev,
+			   "Failed to set beacon period=%d.\n", word);
 		goto failed;
 	}
 
@@ -582,7 +588,8 @@ int prism2mgmt_start(wlandevice_t *wlandev, void *msgp)
 	word = msg->dschannel.data;
 	result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFOWNCHANNEL, word);
 	if (result) {
-		printk(KERN_ERR "Failed to set channel=%d.\n", word);
+		netdev_err(wlandev->netdev,
+			   "Failed to set channel=%d.\n", word);
 		goto failed;
 	}
 	/* Basic rates */
@@ -610,7 +617,8 @@ int prism2mgmt_start(wlandevice_t *wlandev, void *msgp)
 
 	result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFBASICRATES, word);
 	if (result) {
-		printk(KERN_ERR "Failed to set basicrates=%d.\n", word);
+		netdev_err(wlandev->netdev,
+			   "Failed to set basicrates=%d.\n", word);
 		goto failed;
 	}
 
@@ -639,13 +647,14 @@ int prism2mgmt_start(wlandevice_t *wlandev, void *msgp)
 
 	result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFSUPPRATES, word);
 	if (result) {
-		printk(KERN_ERR "Failed to set supprates=%d.\n", word);
+		netdev_err(wlandev->netdev,
+			   "Failed to set supprates=%d.\n", word);
 		goto failed;
 	}
 
 	result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_TXRATECNTL, word);
 	if (result) {
-		printk(KERN_ERR "Failed to set txrates=%d.\n", word);
+		netdev_err(wlandev->netdev, "Failed to set txrates=%d.\n", word);
 		goto failed;
 	}
 
@@ -659,7 +668,8 @@ int prism2mgmt_start(wlandevice_t *wlandev, void *msgp)
 	/* Enable the Port */
 	result = hfa384x_drvr_enable(hw, 0);
 	if (result) {
-		printk(KERN_ERR "Enable macport failed, result=%d.\n", result);
+		netdev_err(wlandev->netdev,
+			   "Enable macport failed, result=%d.\n", result);
 		goto failed;
 	}
 
@@ -704,8 +714,8 @@ int prism2mgmt_readpda(wlandevice_t *wlandev, void *msgp)
 	 * state.
 	 */
 	if (wlandev->msdstate != WLAN_MSD_FWLOAD) {
-		printk(KERN_ERR
-		       "PDA may only be read " "in the fwload state.\n");
+		netdev_err(wlandev->netdev,
+			   "PDA may only be read in the fwload state.\n");
 		msg->resultcode.data =
 		    P80211ENUM_resultcode_implementation_failure;
 		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
@@ -717,9 +727,9 @@ int prism2mgmt_readpda(wlandevice_t *wlandev, void *msgp)
 					      msg->pda.data,
 					      HFA384x_PDA_LEN_MAX);
 		if (result) {
-			printk(KERN_ERR
-			       "hfa384x_drvr_readpda() failed, "
-			       "result=%d\n", result);
+			netdev_err(wlandev->netdev,
+				   "hfa384x_drvr_readpda() failed, "
+				   "result=%d\n", result);
 
 			msg->resultcode.data =
 			    P80211ENUM_resultcode_implementation_failure;
@@ -766,9 +776,9 @@ int prism2mgmt_ramdl_state(wlandevice_t *wlandev, void *msgp)
 	struct p80211msg_p2req_ramdl_state *msg = msgp;
 
 	if (wlandev->msdstate != WLAN_MSD_FWLOAD) {
-		printk(KERN_ERR
-		       "ramdl_state(): may only be called "
-		       "in the fwload state.\n");
+		netdev_err(wlandev->netdev,
+			   "ramdl_state(): may only be called "
+			   "in the fwload state.\n");
 		msg->resultcode.data =
 		    P80211ENUM_resultcode_implementation_failure;
 		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
@@ -825,9 +835,9 @@ int prism2mgmt_ramdl_write(wlandevice_t *wlandev, void *msgp)
 	u8 *buf;
 
 	if (wlandev->msdstate != WLAN_MSD_FWLOAD) {
-		printk(KERN_ERR
-		       "ramdl_write(): may only be called "
-		       "in the fwload state.\n");
+		netdev_err(wlandev->netdev,
+			   "ramdl_write(): may only be called "
+			   "in the fwload state.\n");
 		msg->resultcode.data =
 		    P80211ENUM_resultcode_implementation_failure;
 		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
@@ -885,9 +895,9 @@ int prism2mgmt_flashdl_state(wlandevice_t *wlandev, void *msgp)
 	struct p80211msg_p2req_flashdl_state *msg = msgp;
 
 	if (wlandev->msdstate != WLAN_MSD_FWLOAD) {
-		printk(KERN_ERR
-		       "flashdl_state(): may only be called "
-		       "in the fwload state.\n");
+		netdev_err(wlandev->netdev,
+			   "flashdl_state(): may only be called "
+			   "in the fwload state.\n");
 		msg->resultcode.data =
 		    P80211ENUM_resultcode_implementation_failure;
 		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
@@ -920,8 +930,9 @@ int prism2mgmt_flashdl_state(wlandevice_t *wlandev, void *msgp)
 		wlandev->msdstate = WLAN_MSD_HWPRESENT;
 		result = prism2sta_ifstate(wlandev, P80211ENUM_ifstate_fwload);
 		if (result != P80211ENUM_resultcode_success) {
-			printk(KERN_ERR "prism2sta_ifstate(fwload) failed,"
-			       "P80211ENUM_resultcode=%d\n", result);
+			netdev_err(wlandev->netdev,
+				   "prism2sta_ifstate(fwload) failed,"
+				   "P80211ENUM_resultcode=%d\n", result);
 			msg->resultcode.data =
 			    P80211ENUM_resultcode_implementation_failure;
 			result = -1;
@@ -958,9 +969,9 @@ int prism2mgmt_flashdl_write(wlandevice_t *wlandev, void *msgp)
 	u8 *buf;
 
 	if (wlandev->msdstate != WLAN_MSD_FWLOAD) {
-		printk(KERN_ERR
-		       "flashdl_write(): may only be called "
-		       "in the fwload state.\n");
+		netdev_err(wlandev->netdev,
+			   "flashdl_write(): may only be called "
+			   "in the fwload state.\n");
 		msg->resultcode.data =
 		    P80211ENUM_resultcode_implementation_failure;
 		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
@@ -1153,7 +1164,7 @@ int prism2mgmt_wlansniff(wlandevice_t *wlandev, void *msgp)
 
 		}
 
-		printk(KERN_INFO "monitor mode disabled\n");
+		netdev_info(wlandev->netdev, "monitor mode disabled\n");
 		msg->resultcode.data = P80211ENUM_resultcode_success;
 		result = 0;
 		goto exit;
@@ -1282,7 +1293,7 @@ int prism2mgmt_wlansniff(wlandevice_t *wlandev, void *msgp)
 		}
 
 		if (wlandev->netdev->type == ARPHRD_ETHER)
-			printk(KERN_INFO "monitor mode enabled\n");
+			netdev_info(wlandev->netdev, "monitor mode enabled\n");
 
 		/* Set the driver state */
 		/* Do we want the prism2 header? */
diff --git a/drivers/staging/wlan-ng/prism2mgmt.h b/drivers/staging/wlan-ng/prism2mgmt.h
index 190d390c849..b62fdcba94e 100644
--- a/drivers/staging/wlan-ng/prism2mgmt.h
+++ b/drivers/staging/wlan-ng/prism2mgmt.h
@@ -109,4 +109,9 @@ void prism2sta_processing_defer(struct work_struct *data);
 void prism2sta_commsqual_defer(struct work_struct *data);
 void prism2sta_commsqual_timer(unsigned long data);
 
+/* Interface callback functions, passing data back up to the cfg80211 layer */
+void prism2_connect_result(wlandevice_t *wlandev, u8 failed);
+void prism2_disconnected(wlandevice_t *wlandev);
+void prism2_roamed(wlandevice_t *wlandev);
+
 #endif
diff --git a/drivers/staging/wlan-ng/prism2mib.c b/drivers/staging/wlan-ng/prism2mib.c
index 9b5f3b72d3c..0fb42dfca2a 100644
--- a/drivers/staging/wlan-ng/prism2mib.c
+++ b/drivers/staging/wlan-ng/prism2mib.c
@@ -672,7 +672,7 @@ static int prism2mib_fragmentationthreshold(struct mibrec *mib,
 
 	if (!isget)
 		if ((*uint32) % 2) {
-			printk(KERN_WARNING "Attempt to set odd number "
+			netdev_warn(wlandev->netdev, "Attempt to set odd number "
 			       "FragmentationThreshold\n");
 			msg->resultcode.data =
 			    P80211ENUM_resultcode_not_supported;
@@ -742,7 +742,7 @@ static int prism2mib_priv(struct mibrec *mib,
 			break;
 		}
 	default:
-		printk(KERN_ERR "Unhandled DID 0x%08x\n", mib->did);
+		netdev_err(wlandev->netdev, "Unhandled DID 0x%08x\n", mib->did);
 	}
 
 	return 0;
diff --git a/drivers/staging/wlan-ng/prism2sta.c b/drivers/staging/wlan-ng/prism2sta.c
index f9ccf2371db..278b6a1ef31 100644
--- a/drivers/staging/wlan-ng/prism2sta.c
+++ b/drivers/staging/wlan-ng/prism2sta.c
@@ -120,10 +120,6 @@ MODULE_PARM_DESC(prism2_reset_settletime, "reset settle time in ms");
 
 MODULE_LICENSE("Dual MPL/GPL");
 
-void prism2_connect_result(wlandevice_t *wlandev, u8 failed);
-void prism2_disconnected(wlandevice_t *wlandev);
-void prism2_roamed(wlandevice_t *wlandev);
-
 static int prism2sta_open(wlandevice_t *wlandev);
 static int prism2sta_close(wlandevice_t *wlandev);
 static void prism2sta_reset(wlandevice_t *wlandev);
@@ -405,8 +401,9 @@ static int prism2sta_mlmerequest(wlandevice_t *wlandev, struct p80211msg *msg)
 			break;
 		}
 	default:
-		printk(KERN_WARNING "Unknown mgmt request message 0x%08x",
-		       msg->msgcode);
+		netdev_warn(wlandev->netdev,
+			    "Unknown mgmt request message 0x%08x",
+			    msg->msgcode);
 		break;
 	}
 
@@ -469,7 +466,7 @@ u32 prism2sta_ifstate(wlandevice_t *wlandev, u32 ifstate)
 			result = P80211ENUM_resultcode_success;
 			break;
 		case WLAN_MSD_RUNNING:
-			printk(KERN_WARNING
+			netdev_warn(wlandev->netdev,
 			       "Cannot enter fwload state from enable state,"
 			       "you must disable first.\n");
 			result = P80211ENUM_resultcode_invalid_parameters;
@@ -1431,7 +1428,7 @@ void prism2sta_processing_defer(struct work_struct *data)
 
 	default:
 		/* This is bad, IO port problems? */
-		printk(KERN_WARNING
+		netdev_warn(wlandev->netdev,
 		       "unknown linkstatus=0x%02x\n", hw->link_status);
 		return;
 	}
@@ -1513,7 +1510,7 @@ static void prism2sta_inf_assocstatus(wlandevice_t *wlandev,
 
 	if (i >= hw->authlist.cnt) {
 		if (rec.assocstatus != HFA384x_ASSOCSTATUS_AUTHFAIL)
-			printk(KERN_WARNING
+			netdev_warn(wlandev->netdev,
 	"assocstatus info frame received for non-authenticated station.\n");
 	} else {
 		hw->authlist.assoc[i] =
@@ -1521,7 +1518,7 @@ static void prism2sta_inf_assocstatus(wlandevice_t *wlandev,
 		     rec.assocstatus == HFA384x_ASSOCSTATUS_REASSOC);
 
 		if (rec.assocstatus == HFA384x_ASSOCSTATUS_AUTHFAIL)
-			printk(KERN_WARNING
+			netdev_warn(wlandev->netdev,
 "authfail assocstatus info frame received for authenticated station.\n");
 	}
 }
@@ -1791,16 +1788,16 @@ void prism2sta_ev_info(wlandevice_t *wlandev, hfa384x_InfFrame_t *inf)
 		prism2sta_inf_psusercnt(wlandev, inf);
 		break;
 	case HFA384x_IT_KEYIDCHANGED:
-		printk(KERN_WARNING "Unhandled IT_KEYIDCHANGED\n");
+		netdev_warn(wlandev->netdev, "Unhandled IT_KEYIDCHANGED\n");
 		break;
 	case HFA384x_IT_ASSOCREQ:
-		printk(KERN_WARNING "Unhandled IT_ASSOCREQ\n");
+		netdev_warn(wlandev->netdev, "Unhandled IT_ASSOCREQ\n");
 		break;
 	case HFA384x_IT_MICFAILURE:
-		printk(KERN_WARNING "Unhandled IT_MICFAILURE\n");
+		netdev_warn(wlandev->netdev, "Unhandled IT_MICFAILURE\n");
 		break;
 	default:
-		printk(KERN_WARNING
+		netdev_warn(wlandev->netdev,
 		       "Unknown info type=0x%02x\n", inf->infotype);
 		break;
 	}
diff --git a/drivers/staging/wlan-ng/prism2usb.c b/drivers/staging/wlan-ng/prism2usb.c
index 4739c14d835..e92bbc12bb0 100644
--- a/drivers/staging/wlan-ng/prism2usb.c
+++ b/drivers/staging/wlan-ng/prism2usb.c
@@ -4,103 +4,58 @@
 #include "prism2sta.c"
 #include "prism2fw.c"
 
-#define PRISM_USB_DEVICE(vid, pid, name)	\
-	USB_DEVICE(vid, pid),			\
-	.driver_info = (unsigned long) name
+#define PRISM_DEV(vid, pid, name)		\
+	{ USB_DEVICE(vid, pid),			\
+	.driver_info = (unsigned long) name }
 
 static struct usb_device_id usb_prism_tbl[] = {
-	{PRISM_USB_DEVICE(0x04bb, 0x0922, "IOData AirPort WN-B11/USBS")},
-	{PRISM_USB_DEVICE(0x07aa, 0x0012, "Corega Wireless LAN USB Stick-11")},
-	{PRISM_USB_DEVICE(0x09aa, 0x3642, "Prism2.x 11Mbps WLAN USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x1668, 0x0408, "Actiontec Prism2.5 11Mbps WLAN USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x1668, 0x0421, "Actiontec Prism2.5 11Mbps WLAN USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x1915, 0x2236, "Linksys WUSB11v3.0 11Mbps WLAN USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x066b, 0x2212, "Linksys WUSB11v2.5 11Mbps WLAN USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x066b, 0x2213, "Linksys WUSB12v1.1 11Mbps WLAN USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x067c, 0x1022, "Siemens SpeedStream 1022 11Mbps WLAN USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x049f, 0x0033,
-	 "Compaq/Intel W100 PRO/Wireless 11Mbps multiport WLAN Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x0411, 0x0016, "Melco WLI-USB-S11 11Mbps WLAN Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x08de, 0x7a01, "PRISM25 IEEE 802.11 Mini USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x8086, 0x1111, "Intel PRO/Wireless 2011B LAN USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x0d8e, 0x7a01, "PRISM25 IEEE 802.11 Mini USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x045e, 0x006e, "Microsoft MN510 Wireless USB Adapter")},
-	{PRISM_USB_DEVICE(0x0967, 0x0204, "Acer Warplink USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x0cde, 0x0002, "Z-Com 725/726 Prism2.5 USB/USB Integrated")},
-	{PRISM_USB_DEVICE
-	 (0x0cde, 0x0005, "Z-Com Xl735 Wireless 802.11b USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x413c, 0x8100, "Dell TrueMobile 1180 Wireless USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x0b3b, 0x1601, "ALLNET 0193 11Mbps WLAN USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x0b3b, 0x1602, "ZyXEL ZyAIR B200 Wireless USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x0baf, 0x00eb, "USRobotics USR1120 Wireless USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x0411, 0x0027, "Melco WLI-USB-KS11G 11Mbps WLAN Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x04f1, 0x3009, "JVC MP-XP7250 Builtin USB WLAN Adapter")},
-	{PRISM_USB_DEVICE(0x0846, 0x4110, "NetGear MA111")},
-	{PRISM_USB_DEVICE(0x03f3, 0x0020, "Adaptec AWN-8020 USB WLAN Adapter")},
-	{PRISM_USB_DEVICE(0x2821, 0x3300, "ASUS-WL140 Wireless USB Adapter")},
-	{PRISM_USB_DEVICE(0x2001, 0x3700, "DWL-122 Wireless USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x2001, 0x3702, "DWL-120 Rev F Wireless USB Adapter")},
-	{PRISM_USB_DEVICE(0x50c2, 0x4013, "Averatec USB WLAN Adapter")},
-	{PRISM_USB_DEVICE(0x2c02, 0x14ea, "Planex GW-US11H WLAN USB Adapter")},
-	{PRISM_USB_DEVICE(0x124a, 0x168b, "Airvast PRISM3 WLAN USB Adapter")},
-	{PRISM_USB_DEVICE(0x083a, 0x3503, "T-Sinus 111 USB WLAN Adapter")},
-	{PRISM_USB_DEVICE(0x2821, 0x3300, "Hawking HighDB USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x0411, 0x0044, "Melco WLI-USB-KB11 11Mbps WLAN Adapter")},
-	{PRISM_USB_DEVICE(0x1668, 0x6106, "ROPEX FreeLan 802.11b USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x124a, 0x4017, "Pheenet WL-503IA 802.11b USB Adapter")},
-	{PRISM_USB_DEVICE(0x0bb2, 0x0302, "Ambit Microsystems Corp.")},
-	{PRISM_USB_DEVICE
-	 (0x9016, 0x182d, "Sitecom WL-022 802.11b USB Adapter")},
-	{PRISM_USB_DEVICE
-	 (0x0543, 0x0f01, "ViewSonic Airsync USB Adapter 11Mbps (Prism2.5)")},
-	{ /* terminator */ }
+	PRISM_DEV(0x04bb, 0x0922, "IOData AirPort WN-B11/USBS"),
+	PRISM_DEV(0x07aa, 0x0012, "Corega Wireless LAN USB Stick-11"),
+	PRISM_DEV(0x09aa, 0x3642, "Prism2.x 11Mbps WLAN USB Adapter"),
+	PRISM_DEV(0x1668, 0x0408, "Actiontec Prism2.5 11Mbps WLAN USB Adapter"),
+	PRISM_DEV(0x1668, 0x0421, "Actiontec Prism2.5 11Mbps WLAN USB Adapter"),
+	PRISM_DEV(0x1915, 0x2236, "Linksys WUSB11v3.0 11Mbps WLAN USB Adapter"),
+	PRISM_DEV(0x066b, 0x2212, "Linksys WUSB11v2.5 11Mbps WLAN USB Adapter"),
+	PRISM_DEV(0x066b, 0x2213, "Linksys WUSB12v1.1 11Mbps WLAN USB Adapter"),
+	PRISM_DEV(0x0411, 0x0016, "Melco WLI-USB-S11 11Mbps WLAN Adapter"),
+	PRISM_DEV(0x08de, 0x7a01, "PRISM25 IEEE 802.11 Mini USB Adapter"),
+	PRISM_DEV(0x8086, 0x1111, "Intel PRO/Wireless 2011B LAN USB Adapter"),
+	PRISM_DEV(0x0d8e, 0x7a01, "PRISM25 IEEE 802.11 Mini USB Adapter"),
+	PRISM_DEV(0x045e, 0x006e, "Microsoft MN510 Wireless USB Adapter"),
+	PRISM_DEV(0x0967, 0x0204, "Acer Warplink USB Adapter"),
+	PRISM_DEV(0x0cde, 0x0002, "Z-Com 725/726 Prism2.5 USB/USB Integrated"),
+	PRISM_DEV(0x0cde, 0x0005, "Z-Com Xl735 Wireless 802.11b USB Adapter"),
+	PRISM_DEV(0x413c, 0x8100, "Dell TrueMobile 1180 Wireless USB Adapter"),
+	PRISM_DEV(0x0b3b, 0x1601, "ALLNET 0193 11Mbps WLAN USB Adapter"),
+	PRISM_DEV(0x0b3b, 0x1602, "ZyXEL ZyAIR B200 Wireless USB Adapter"),
+	PRISM_DEV(0x0baf, 0x00eb, "USRobotics USR1120 Wireless USB Adapter"),
+	PRISM_DEV(0x0411, 0x0027, "Melco WLI-USB-KS11G 11Mbps WLAN Adapter"),
+	PRISM_DEV(0x04f1, 0x3009, "JVC MP-XP7250 Builtin USB WLAN Adapter"),
+	PRISM_DEV(0x0846, 0x4110, "NetGear MA111"),
+	PRISM_DEV(0x03f3, 0x0020, "Adaptec AWN-8020 USB WLAN Adapter"),
+	PRISM_DEV(0x2821, 0x3300, "ASUS-WL140 Wireless USB Adapter"),
+	PRISM_DEV(0x2001, 0x3700, "DWL-122 Wireless USB Adapter"),
+	PRISM_DEV(0x2001, 0x3702, "DWL-120 Rev F Wireless USB Adapter"),
+	PRISM_DEV(0x50c2, 0x4013, "Averatec USB WLAN Adapter"),
+	PRISM_DEV(0x2c02, 0x14ea, "Planex GW-US11H WLAN USB Adapter"),
+	PRISM_DEV(0x124a, 0x168b, "Airvast PRISM3 WLAN USB Adapter"),
+	PRISM_DEV(0x083a, 0x3503, "T-Sinus 111 USB WLAN Adapter"),
+	PRISM_DEV(0x2821, 0x3300, "Hawking HighDB USB Adapter"),
+	PRISM_DEV(0x0411, 0x0044, "Melco WLI-USB-KB11 11Mbps WLAN Adapter"),
+	PRISM_DEV(0x1668, 0x6106, "ROPEX FreeLan 802.11b USB Adapter"),
+	PRISM_DEV(0x124a, 0x4017, "Pheenet WL-503IA 802.11b USB Adapter"),
+	PRISM_DEV(0x0bb2, 0x0302, "Ambit Microsystems Corp."),
+	PRISM_DEV(0x9016, 0x182d, "Sitecom WL-022 802.11b USB Adapter"),
+	PRISM_DEV(0x0543, 0x0f01,
+		"ViewSonic Airsync USB Adapter 11Mbps (Prism2.5)"),
+	PRISM_DEV(0x067c, 0x1022,
+		"Siemens SpeedStream 1022 11Mbps WLAN USB Adapter"),
+	PRISM_DEV(0x049f, 0x0033,
+		"Compaq/Intel W100 PRO/Wireless 11Mbps multiport WLAN Adapter"),
+	{ } /* terminator */
 };
-
 MODULE_DEVICE_TABLE(usb, usb_prism_tbl);
 
-/*----------------------------------------------------------------
-* prism2sta_probe_usb
-*
-* Probe routine called by the USB subsystem.
-*
-* Arguments:
-*	dev		ptr to the usb_device struct
-*	ifnum		interface number being offered
-*
-* Returns:
-*	NULL		- we're not claiming the device+interface
-*	non-NULL	- we are claiming the device+interface and
-*			  this is a ptr to the data we want back
-*			  when disconnect is called.
-*
-* Side effects:
-*
-* Call context:
-*	I'm not sure, assume it's interrupt.
-*
-----------------------------------------------------------------*/
 static int prism2sta_probe_usb(struct usb_interface *interface,
 			       const struct usb_device_id *id)
 {
@@ -141,7 +96,8 @@ static int prism2sta_probe_usb(struct usb_interface *interface,
 					   prism2_reset_settletime, 0);
 		if (result != 0) {
 			result = -EIO;
-			dev_err(&interface->dev, "hfa384x_corereset() failed.\n");
+			dev_err(&interface->dev,
+				"hfa384x_corereset() failed.\n");
 			goto failed_reset;
 		}
 	}
@@ -176,31 +132,11 @@ done:
 	return result;
 }
 
-/*----------------------------------------------------------------
-* prism2sta_disconnect_usb
-*
-* Called when a device previously claimed by probe is removed
-* from the USB.
-*
-* Arguments:
-*	dev		ptr to the usb_device struct
-*	ptr		ptr returned by probe() when the device
-*                       was claimed.
-*
-* Returns:
-*	Nothing
-*
-* Side effects:
-*
-* Call context:
-*	process
-----------------------------------------------------------------*/
 static void prism2sta_disconnect_usb(struct usb_interface *interface)
 {
 	wlandevice_t *wlandev;
 
 	wlandev = (wlandevice_t *) usb_get_intfdata(interface);
-
 	if (wlandev != NULL) {
 		LIST_HEAD(cleanlist);
 		struct list_head *entry;
@@ -292,6 +228,7 @@ static int prism2sta_suspend(struct usb_interface *interface,
 {
 	hfa384x_t *hw = NULL;
 	wlandevice_t *wlandev;
+
 	wlandev = (wlandevice_t *) usb_get_intfdata(interface);
 	if (!wlandev)
 		return -ENODEV;
@@ -314,6 +251,7 @@ static int prism2sta_resume(struct usb_interface *interface)
 	int result = 0;
 	hfa384x_t *hw = NULL;
 	wlandevice_t *wlandev;
+
 	wlandev = (wlandevice_t *) usb_get_intfdata(interface);
 	if (!wlandev)
 		return -ENODEV;
diff --git a/drivers/staging/xgifb/TODO b/drivers/staging/xgifb/TODO
index 392b29d8f13..7eb99140a39 100644
--- a/drivers/staging/xgifb/TODO
+++ b/drivers/staging/xgifb/TODO
@@ -6,8 +6,7 @@ Arnaud
 TODO:
 - clean ups
 - sort out dup ids with SiS driver
-- remove useless/wrong/unused #ifdef/code/...
-- fix printk usages
+- remove useless/wrong/unused code...
 - get rid of non-linux related stuff
 
 Please send patches to:
diff --git a/drivers/staging/xgifb/XGI_main.h b/drivers/staging/xgifb/XGI_main.h
index 95ce9708cec..85079fea715 100644
--- a/drivers/staging/xgifb/XGI_main.h
+++ b/drivers/staging/xgifb/XGI_main.h
@@ -4,12 +4,8 @@
 #include "XGIfb.h"
 #include "vb_def.h"
 
-#ifndef PCI_DEVICE_ID_XGI_42
 #define PCI_DEVICE_ID_XGI_42      0x042
-#endif
-#ifndef PCI_DEVICE_ID_XGI_27
 #define PCI_DEVICE_ID_XGI_27      0x027
-#endif
 
 static const struct pci_device_id xgifb_pci_table[] = {
 	{PCI_DEVICE(PCI_VENDOR_ID_XGI, PCI_DEVICE_ID_XGI_20)},
diff --git a/drivers/staging/xgifb/XGI_main_26.c b/drivers/staging/xgifb/XGI_main_26.c
index 466804687fc..f82f0574bbd 100644
--- a/drivers/staging/xgifb/XGI_main_26.c
+++ b/drivers/staging/xgifb/XGI_main_26.c
@@ -66,6 +66,7 @@ static int XGIfb_mode_rate_to_dclock(struct vb_device_info *XGI_Pr,
 	unsigned short ModeIdIndex = 0, ClockIndex = 0;
 	unsigned short RefreshRateTableIndex = 0;
 	int Clock;
+
 	InitTo330Pointer(HwDeviceExtension->jChipType, XGI_Pr);
 
 	XGI_SearchModeID(ModeNo, &ModeIdIndex);
@@ -95,6 +96,7 @@ static int XGIfb_mode_rate_to_ddata(struct vb_device_info *XGI_Pr,
 	unsigned short HRE, HBE, HRS, HDE;
 	unsigned char sr_data, cr_data, cr_data2;
 	int B, C, D, F, temp, j;
+
 	InitTo330Pointer(HwDeviceExtension->jChipType, XGI_Pr);
 	if (!XGI_SearchModeID(ModeNo, &ModeIdIndex))
 		return 0;
@@ -216,6 +218,7 @@ void XGIRegInit(struct vb_device_info *XGI_Pr, unsigned long BaseAddr)
 	XGI_Pr->P3c8 = BaseAddr + 0x18;
 	XGI_Pr->P3c9 = BaseAddr + 0x19;
 	XGI_Pr->P3da = BaseAddr + 0x2A;
+	XGI_Pr->Part0Port = BaseAddr + XGI_CRT2_PORT_00;
 	/* Digital video interface registers (LCD) */
 	XGI_Pr->Part1Port = BaseAddr + SIS_CRT2_PORT_04;
 	/* 301 TV Encoder registers */
diff --git a/drivers/staging/xgifb/vb_def.h b/drivers/staging/xgifb/vb_def.h
index 949f0e5eed8..481eb174fdf 100644
--- a/drivers/staging/xgifb/vb_def.h
+++ b/drivers/staging/xgifb/vb_def.h
@@ -44,12 +44,10 @@
 #define SetLCDtoNonExpanding 0x0010
 #define SetLCDDualLink       0x0100
 #define SetLCDLowResolution  0x0200
-#define SetLCDStdMode        0x0400
 
 /* LCD Capability shampoo */
 #define DefaultLCDCap        0x80ea
 #define EnableLCD24bpp       0x0004 /* default */
-#define DisableLCD24bpp      0x0000
 #define LCDPolarity          0x00c0 /* default: SyncNN */
 #define XGI_LCDDualLink      0x0100
 #define EnableSpectrum       0x0200
diff --git a/drivers/staging/xgifb/vb_init.c b/drivers/staging/xgifb/vb_init.c
index 21541720e05..ff210ddcbf9 100644
--- a/drivers/staging/xgifb/vb_init.c
+++ b/drivers/staging/xgifb/vb_init.c
@@ -130,6 +130,7 @@ static void XGINew_DDRII_Bootup_XG27(
 			unsigned long P3c4, struct vb_device_info *pVBInfo)
 {
 	unsigned long P3d4 = P3c4 + 0x10;
+
 	pVBInfo->ram_type = XGINew_GetXG20DRAMType(HwDeviceExtension, pVBInfo);
 	XGINew_SetMemoryClock(pVBInfo);
 
@@ -389,6 +390,7 @@ static void XGI_SetDRAM_Helper(unsigned long P3d4, u8 seed, u8 temp2, u8 reg,
 	u8 shift_factor, u8 mask1, u8 mask2)
 {
 	u8 j;
+
 	for (j = 0; j < 4; j++) {
 		temp2 |= (((seed >> (2 * j)) & 0x03) << shift_factor);
 		xgifb_reg_set(P3d4, reg, temp2);
diff --git a/drivers/staging/xgifb/vb_setmode.c b/drivers/staging/xgifb/vb_setmode.c
index 400c726753f..8795e173cb5 100644
--- a/drivers/staging/xgifb/vb_setmode.c
+++ b/drivers/staging/xgifb/vb_setmode.c
@@ -49,6 +49,7 @@ void InitTo330Pointer(unsigned char ChipType, struct vb_device_info *pVBInfo)
 
 	if (ChipType == XG27) {
 		unsigned char temp;
+
 		pVBInfo->MCLKData = XGI27New_MCLKData;
 		pVBInfo->CR40 = XGI27_cr41;
 		pVBInfo->XGINew_CR97 = 0xc1;
@@ -5222,6 +5223,7 @@ void XGI_SenseCRT1(struct vb_device_info *pVBInfo)
 	unsigned short temp;
 
 	int i;
+
 	xgifb_reg_set(pVBInfo->P3c4, 0x05, 0x86);
 
 	/* to fix XG42 single LCD sense to CRT+LCD */
@@ -5460,6 +5462,7 @@ unsigned char XGISetModeNew(struct xgifb_video_info *xgifb_info,
 	unsigned short ModeIdIndex;
 	struct vb_device_info VBINF;
 	struct vb_device_info *pVBInfo = &VBINF;
+
 	pVBInfo->IF_DEF_LVDS = 0;
 
 	if (HwDeviceExtension->jChipType >= XG20)
diff --git a/drivers/staging/xgifb/vgatypes.h b/drivers/staging/xgifb/vgatypes.h
index 264351441f9..61fa10fd470 100644
--- a/drivers/staging/xgifb/vgatypes.h
+++ b/drivers/staging/xgifb/vgatypes.h
@@ -5,7 +5,6 @@
 #include "../../video/fbdev/sis/vgatypes.h"
 #include "../../video/fbdev/sis/sis.h"		/* for LCD_TYPE */
 
-#ifndef XGI_VB_CHIP_TYPE
 enum XGI_VB_CHIP_TYPE {
 	VB_CHIP_Legacy = 0,
 	VB_CHIP_301,
@@ -19,7 +18,6 @@ enum XGI_VB_CHIP_TYPE {
 	VB_CHIP_UNKNOWN, /* other video bridge or no video bridge */
 	MAX_VB_CHIP
 };
-#endif
 
 struct xgi_hw_device_info {
 	unsigned long ulExternalChip; /* NO VB or other video bridge*/
diff --git a/drivers/staging/xillybus/xillybus.h b/drivers/staging/xillybus/xillybus.h
index e5e91d61288..78a749a7a1c 100644
--- a/drivers/staging/xillybus/xillybus.h
+++ b/drivers/staging/xillybus/xillybus.h
@@ -116,7 +116,6 @@ struct xilly_endpoint {
 	 */
 	struct pci_dev *pdev;
 	struct device *dev;
-	struct resource res; /* OF devices only */
 	struct xilly_endpoint_hardware *ephw;
 
 	struct list_head ep_list;
diff --git a/drivers/staging/xillybus/xillybus_core.c b/drivers/staging/xillybus/xillybus_core.c
index b0a6696f2da..fe8f9d28b03 100644
--- a/drivers/staging/xillybus/xillybus_core.c
+++ b/drivers/staging/xillybus/xillybus_core.c
@@ -2094,7 +2094,7 @@ struct xilly_endpoint *xillybus_init_endpoint(struct pci_dev *pdev,
 {
 	struct xilly_endpoint *endpoint;
 
-	endpoint = kzalloc(sizeof(*endpoint), GFP_KERNEL);
+	endpoint = devm_kzalloc(dev, sizeof(*endpoint), GFP_KERNEL);
 	if (!endpoint) {
 		dev_err(dev, "Failed to allocate memory. Aborting.\n");
 		return NULL;
diff --git a/drivers/staging/xillybus/xillybus_of.c b/drivers/staging/xillybus/xillybus_of.c
index 23a609b0ab1..46ea010b4ac 100644
--- a/drivers/staging/xillybus/xillybus_of.c
+++ b/drivers/staging/xillybus/xillybus_of.c
@@ -19,6 +19,7 @@
 #include <linux/of_address.h>
 #include <linux/of_device.h>
 #include <linux/of_platform.h>
+#include <linux/err.h>
 #include "xillybus.h"
 
 MODULE_DESCRIPTION("Xillybus driver for Open Firmware");
@@ -123,6 +124,7 @@ static int xilly_drv_probe(struct platform_device *op)
 	struct xilly_endpoint *endpoint;
 	int rc = 0;
 	int irq;
+	struct resource res;
 	struct xilly_endpoint_hardware *ephw = &of_hw;
 
 	if (of_property_read_bool(dev->of_node, "dma-coherent"))
@@ -135,38 +137,26 @@ static int xilly_drv_probe(struct platform_device *op)
 
 	dev_set_drvdata(dev, endpoint);
 
-	rc = of_address_to_resource(dev->of_node, 0, &endpoint->res);
+	rc = of_address_to_resource(dev->of_node, 0, &res);
 	if (rc) {
 		dev_warn(endpoint->dev,
 			 "Failed to obtain device tree resource\n");
-		goto failed_request_regions;
+		return rc;
 	}
 
-	if  (!request_mem_region(endpoint->res.start,
-				 resource_size(&endpoint->res), xillyname)) {
-		dev_err(endpoint->dev,
-			"request_mem_region failed. Aborting.\n");
-		rc = -EBUSY;
-		goto failed_request_regions;
-	}
+	endpoint->registers = devm_ioremap_resource(dev, &res);
 
-	endpoint->registers = of_iomap(dev->of_node, 0);
-	if (!endpoint->registers) {
-		dev_err(endpoint->dev,
-			"Failed to map I/O memory. Aborting.\n");
-		rc = -EIO;
-		goto failed_iomap0;
-	}
+	if (IS_ERR(endpoint->registers))
+		return PTR_ERR(endpoint->registers);
 
 	irq = irq_of_parse_and_map(dev->of_node, 0);
 
-	rc = request_irq(irq, xillybus_isr, 0, xillyname, endpoint);
+	rc = devm_request_irq(dev, irq, xillybus_isr, 0, xillyname, endpoint);
 
 	if (rc) {
 		dev_err(endpoint->dev,
 			"Failed to register IRQ handler. Aborting.\n");
-		rc = -ENODEV;
-		goto failed_register_irq;
+		return -ENODEV;
 	}
 
 	rc = xillybus_endpoint_discovery(endpoint);
@@ -174,18 +164,8 @@ static int xilly_drv_probe(struct platform_device *op)
 	if (!rc)
 		return 0;
 
-	free_irq(irq, endpoint);
-
-failed_register_irq:
-	iounmap(endpoint->registers);
-failed_iomap0:
-	release_mem_region(endpoint->res.start,
-			   resource_size(&endpoint->res));
-
-failed_request_regions:
 	xillybus_do_cleanup(&endpoint->cleanup, endpoint);
 
-	kfree(endpoint);
 	return rc;
 }
 
@@ -193,20 +173,11 @@ static int xilly_drv_remove(struct platform_device *op)
 {
 	struct device *dev = &op->dev;
 	struct xilly_endpoint *endpoint = dev_get_drvdata(dev);
-	int irq = irq_of_parse_and_map(dev->of_node, 0);
 
 	xillybus_endpoint_remove(endpoint);
 
-	free_irq(irq, endpoint);
-
-	iounmap(endpoint->registers);
-	release_mem_region(endpoint->res.start,
-			   resource_size(&endpoint->res));
-
 	xillybus_do_cleanup(&endpoint->cleanup, endpoint);
 
-	kfree(endpoint);
-
 	return 0;
 }
 
diff --git a/drivers/staging/xillybus/xillybus_pcie.c b/drivers/staging/xillybus/xillybus_pcie.c
index 51426d80ca7..a4fe51c90e9 100644
--- a/drivers/staging/xillybus/xillybus_pcie.c
+++ b/drivers/staging/xillybus/xillybus_pcie.c
@@ -141,38 +141,32 @@ static int xilly_probe(struct pci_dev *pdev,
 
 	pci_set_drvdata(pdev, endpoint);
 
-	rc = pci_enable_device(pdev);
-
-	/* L0s has caused packet drops. No power saving, thank you. */
-
-	pci_disable_link_state(pdev, PCIE_LINK_STATE_L0S);
+	rc = pcim_enable_device(pdev);
 
 	if (rc) {
 		dev_err(endpoint->dev,
-			"pci_enable_device() failed. Aborting.\n");
-		goto no_enable;
+			"pcim_enable_device() failed. Aborting.\n");
+		return rc;
 	}
 
+	/* L0s has caused packet drops. No power saving, thank you. */
+
+	pci_disable_link_state(pdev, PCIE_LINK_STATE_L0S);
+
 	if (!(pci_resource_flags(pdev, 0) & IORESOURCE_MEM)) {
 		dev_err(endpoint->dev,
 			"Incorrect BAR configuration. Aborting.\n");
-		rc = -ENODEV;
-		goto bad_bar;
+		return -ENODEV;
 	}
 
-	rc = pci_request_regions(pdev, xillyname);
+	rc = pcim_iomap_regions(pdev, 0x01, xillyname);
 	if (rc) {
 		dev_err(endpoint->dev,
-			"pci_request_regions() failed. Aborting.\n");
-		goto failed_request_regions;
+			"pcim_iomap_regions() failed. Aborting.\n");
+		return rc;
 	}
 
-	endpoint->registers = pci_iomap(pdev, 0, 128);
-	if (!endpoint->registers) {
-		dev_err(endpoint->dev, "Failed to map BAR 0. Aborting.\n");
-		rc = -EIO;
-		goto failed_iomap0;
-	}
+	endpoint->registers = pcim_iomap_table(pdev)[0];
 
 	pci_set_master(pdev);
 
@@ -180,16 +174,15 @@ static int xilly_probe(struct pci_dev *pdev,
 	if (pci_enable_msi(pdev)) {
 		dev_err(endpoint->dev,
 			"Failed to enable MSI interrupts. Aborting.\n");
-		rc = -ENODEV;
-		goto failed_enable_msi;
+		return -ENODEV;
 	}
-	rc = request_irq(pdev->irq, xillybus_isr, 0, xillyname, endpoint);
+	rc = devm_request_irq(&pdev->dev, pdev->irq, xillybus_isr, 0,
+			      xillyname, endpoint);
 
 	if (rc) {
 		dev_err(endpoint->dev,
 			"Failed to register MSI handler. Aborting.\n");
-		rc = -ENODEV;
-		goto failed_register_msi;
+		return -ENODEV;
 	}
 
 	/*
@@ -203,8 +196,7 @@ static int xilly_probe(struct pci_dev *pdev,
 		endpoint->dma_using_dac = 0;
 	else {
 		dev_err(endpoint->dev, "Failed to set DMA mask. Aborting.\n");
-		rc = -ENODEV;
-		goto failed_dmamask;
+		return -ENODEV;
 	}
 
 	rc = xillybus_endpoint_discovery(endpoint);
@@ -212,22 +204,8 @@ static int xilly_probe(struct pci_dev *pdev,
 	if (!rc)
 		return 0;
 
-failed_dmamask:
-	free_irq(pdev->irq, endpoint);
-failed_register_msi:
-	pci_disable_msi(pdev);
-failed_enable_msi:
-	/* pci_clear_master(pdev); Nobody else seems to do this */
-	pci_iounmap(pdev, endpoint->registers);
-failed_iomap0:
-	pci_release_regions(pdev);
-failed_request_regions:
-bad_bar:
-	pci_disable_device(pdev);
-no_enable:
 	xillybus_do_cleanup(&endpoint->cleanup, endpoint);
 
-	kfree(endpoint);
 	return rc;
 }
 
@@ -237,16 +215,7 @@ static void xilly_remove(struct pci_dev *pdev)
 
 	xillybus_endpoint_remove(endpoint);
 
-	free_irq(pdev->irq, endpoint);
-
-	pci_disable_msi(pdev);
-	pci_iounmap(pdev, endpoint->registers);
-	pci_release_regions(pdev);
-	pci_disable_device(pdev);
-
 	xillybus_do_cleanup(&endpoint->cleanup, endpoint);
-
-	kfree(endpoint);
 }
 
 MODULE_DEVICE_TABLE(pci, xillyids);
diff --git a/drivers/target/iscsi/iscsi_target.c b/drivers/target/iscsi/iscsi_target.c
index 78cab13bbb1..46588c85d39 100644
--- a/drivers/target/iscsi/iscsi_target.c
+++ b/drivers/target/iscsi/iscsi_target.c
@@ -1593,7 +1593,9 @@ int iscsit_process_nop_out(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
 	 * Initiator is expecting a NopIN ping reply..
 	 */
 	if (hdr->itt != RESERVED_ITT) {
-		BUG_ON(!cmd);
+		if (!cmd)
+			return iscsit_add_reject(conn, ISCSI_REASON_PROTOCOL_ERROR,
+						(unsigned char *)hdr);
 
 		spin_lock_bh(&conn->cmd_lock);
 		list_add_tail(&cmd->i_conn_node, &conn->conn_cmd_list);
diff --git a/drivers/target/iscsi/iscsi_target_core.h b/drivers/target/iscsi/iscsi_target_core.h
index 6960f22909a..302eb3b7871 100644
--- a/drivers/target/iscsi/iscsi_target_core.h
+++ b/drivers/target/iscsi/iscsi_target_core.h
@@ -775,6 +775,7 @@ struct iscsi_np {
 	int			np_ip_proto;
 	int			np_sock_type;
 	enum np_thread_state_table np_thread_state;
+	bool                    enabled;
 	enum iscsi_timer_flags_table np_login_timer_flags;
 	u32			np_exports;
 	enum np_flags_table	np_flags;
diff --git a/drivers/target/iscsi/iscsi_target_login.c b/drivers/target/iscsi/iscsi_target_login.c
index 8739b98f6f9..ca31fa1b8a4 100644
--- a/drivers/target/iscsi/iscsi_target_login.c
+++ b/drivers/target/iscsi/iscsi_target_login.c
@@ -436,7 +436,7 @@ static int iscsi_login_zero_tsih_s2(
 		}
 		off = mrdsl % PAGE_SIZE;
 		if (!off)
-			return 0;
+			goto check_prot;
 
 		if (mrdsl < PAGE_SIZE)
 			mrdsl = PAGE_SIZE;
@@ -452,6 +452,31 @@ static int iscsi_login_zero_tsih_s2(
 				ISCSI_LOGIN_STATUS_NO_RESOURCES);
 			return -1;
 		}
+		/*
+		 * ISER currently requires that ImmediateData + Unsolicited
+		 * Data be disabled when protection / signature MRs are enabled.
+		 */
+check_prot:
+		if (sess->se_sess->sup_prot_ops &
+		   (TARGET_PROT_DOUT_STRIP | TARGET_PROT_DOUT_PASS |
+		    TARGET_PROT_DOUT_INSERT)) {
+
+			sprintf(buf, "ImmediateData=No");
+			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;
+			}
+
+			sprintf(buf, "InitialR2T=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;
+			}
+			pr_debug("Forcing ImmediateData=No + InitialR2T=Yes for"
+				 " T10-PI enabled ISER session\n");
+		}
 	}
 
 	return 0;
@@ -984,6 +1009,7 @@ int iscsi_target_setup_login_socket(
 	}
 
 	np->np_transport = t;
+	np->enabled = true;
 	return 0;
 }
 
diff --git a/drivers/target/iscsi/iscsi_target_tpg.c b/drivers/target/iscsi/iscsi_target_tpg.c
index eb96b20dc09..ca1811858af 100644
--- a/drivers/target/iscsi/iscsi_target_tpg.c
+++ b/drivers/target/iscsi/iscsi_target_tpg.c
@@ -184,6 +184,7 @@ static void iscsit_clear_tpg_np_login_thread(
 		return;
 	}
 
+	tpg_np->tpg_np->enabled = false;
 	iscsit_reset_np_thread(tpg_np->tpg_np, tpg_np, tpg, shutdown);
 }
 
diff --git a/drivers/target/target_core_device.c b/drivers/target/target_core_device.c
index 72618776ede..11d26fe65bf 100644
--- a/drivers/target/target_core_device.c
+++ b/drivers/target/target_core_device.c
@@ -798,10 +798,10 @@ int se_dev_set_emulate_write_cache(struct se_device *dev, int flag)
 		pr_err("emulate_write_cache not supported for pSCSI\n");
 		return -EINVAL;
 	}
-	if (dev->transport->get_write_cache) {
-		pr_warn("emulate_write_cache cannot be changed when underlying"
-			" HW reports WriteCacheEnabled, ignoring request\n");
-		return 0;
+	if (flag &&
+	    dev->transport->get_write_cache) {
+		pr_err("emulate_write_cache not supported for this device\n");
+		return -EINVAL;
 	}
 
 	dev->dev_attrib.emulate_write_cache = flag;
@@ -936,6 +936,10 @@ int se_dev_set_pi_prot_type(struct se_device *dev, int flag)
 		return 0;
 	}
 	if (!dev->transport->init_prot || !dev->transport->free_prot) {
+		/* 0 is only allowed value for non-supporting backends */
+		if (flag == 0)
+			return 0;
+
 		pr_err("DIF protection not supported by backend: %s\n",
 		       dev->transport->name);
 		return -ENOSYS;
diff --git a/drivers/target/target_core_transport.c b/drivers/target/target_core_transport.c
index 4badca1cd62..2179feed0d6 100644
--- a/drivers/target/target_core_transport.c
+++ b/drivers/target/target_core_transport.c
@@ -1113,6 +1113,7 @@ void transport_init_se_cmd(
 	init_completion(&cmd->cmd_wait_comp);
 	init_completion(&cmd->task_stop_comp);
 	spin_lock_init(&cmd->t_state_lock);
+	kref_init(&cmd->cmd_kref);
 	cmd->transport_state = CMD_T_DEV_ACTIVE;
 
 	cmd->se_tfo = tfo;
@@ -2357,7 +2358,6 @@ int target_get_sess_cmd(struct se_session *se_sess, struct se_cmd *se_cmd,
 	unsigned long flags;
 	int ret = 0;
 
-	kref_init(&se_cmd->cmd_kref);
 	/*
 	 * Add a second kref if the fabric caller is expecting to handle
 	 * fabric acknowledgement that requires two target_put_sess_cmd()
diff --git a/drivers/target/tcm_fc/tfc_cmd.c b/drivers/target/tcm_fc/tfc_cmd.c
index 01cf37f212c..f5fd515b2be 100644
--- a/drivers/target/tcm_fc/tfc_cmd.c
+++ b/drivers/target/tcm_fc/tfc_cmd.c
@@ -90,18 +90,18 @@ static void ft_free_cmd(struct ft_cmd *cmd)
 {
 	struct fc_frame *fp;
 	struct fc_lport *lport;
-	struct se_session *se_sess;
+	struct ft_sess *sess;
 
 	if (!cmd)
 		return;
-	se_sess = cmd->sess->se_sess;
+	sess = cmd->sess;
 	fp = cmd->req_frame;
 	lport = fr_dev(fp);
 	if (fr_seq(fp))
 		lport->tt.seq_release(fr_seq(fp));
 	fc_frame_free(fp);
-	percpu_ida_free(&se_sess->sess_tag_pool, cmd->se_cmd.map_tag);
-	ft_sess_put(cmd->sess);	/* undo get from lookup at recv */
+	percpu_ida_free(&sess->se_sess->sess_tag_pool, cmd->se_cmd.map_tag);
+	ft_sess_put(sess);	/* undo get from lookup at recv */
 }
 
 void ft_release_cmd(struct se_cmd *se_cmd)
diff --git a/drivers/tty/goldfish.c b/drivers/tty/goldfish.c
index 75dc9d25f32..09495f515fa 100644
--- a/drivers/tty/goldfish.c
+++ b/drivers/tty/goldfish.c
@@ -21,6 +21,7 @@
 #include <linux/slab.h>
 #include <linux/io.h>
 #include <linux/module.h>
+#include <linux/goldfish.h>
 
 enum {
 	GOLDFISH_TTY_PUT_CHAR       = 0x00,
@@ -29,6 +30,7 @@ enum {
 
 	GOLDFISH_TTY_DATA_PTR       = 0x10,
 	GOLDFISH_TTY_DATA_LEN       = 0x14,
+	GOLDFISH_TTY_DATA_PTR_HIGH  = 0x18,
 
 	GOLDFISH_TTY_CMD_INT_DISABLE    = 0,
 	GOLDFISH_TTY_CMD_INT_ENABLE     = 1,
@@ -57,7 +59,8 @@ static void goldfish_tty_do_write(int line, const char *buf, unsigned count)
 	struct goldfish_tty *qtty = &goldfish_ttys[line];
 	void __iomem *base = qtty->base;
 	spin_lock_irqsave(&qtty->lock, irq_flags);
-	writel((u32)buf, base + GOLDFISH_TTY_DATA_PTR);
+	gf_write64((u64)buf, base + GOLDFISH_TTY_DATA_PTR,
+				base + GOLDFISH_TTY_DATA_PTR_HIGH);
 	writel(count, base + GOLDFISH_TTY_DATA_LEN);
 	writel(GOLDFISH_TTY_CMD_WRITE_BUFFER, base + GOLDFISH_TTY_CMD);
 	spin_unlock_irqrestore(&qtty->lock, irq_flags);
@@ -73,12 +76,13 @@ static irqreturn_t goldfish_tty_interrupt(int irq, void *dev_id)
 	u32 count;
 
 	count = readl(base + GOLDFISH_TTY_BYTES_READY);
-	if(count == 0)
+	if (count == 0)
 		return IRQ_NONE;
 
 	count = tty_prepare_flip_string(&qtty->port, &buf, count);
 	spin_lock_irqsave(&qtty->lock, irq_flags);
-	writel((u32)buf, base + GOLDFISH_TTY_DATA_PTR);
+	gf_write64((u64)buf, base + GOLDFISH_TTY_DATA_PTR,
+				base + GOLDFISH_TTY_DATA_PTR_HIGH);
 	writel(count, base + GOLDFISH_TTY_DATA_LEN);
 	writel(GOLDFISH_TTY_CMD_READ_BUFFER, base + GOLDFISH_TTY_CMD);
 	spin_unlock_irqrestore(&qtty->lock, irq_flags);
@@ -88,24 +92,26 @@ static irqreturn_t goldfish_tty_interrupt(int irq, void *dev_id)
 
 static int goldfish_tty_activate(struct tty_port *port, struct tty_struct *tty)
 {
-	struct goldfish_tty *qtty = container_of(port, struct goldfish_tty, port);
+	struct goldfish_tty *qtty = container_of(port, struct goldfish_tty,
+									port);
 	writel(GOLDFISH_TTY_CMD_INT_ENABLE, qtty->base + GOLDFISH_TTY_CMD);
 	return 0;
 }
 
 static void goldfish_tty_shutdown(struct tty_port *port)
 {
-	struct goldfish_tty *qtty = container_of(port, struct goldfish_tty, port);
+	struct goldfish_tty *qtty = container_of(port, struct goldfish_tty,
+									port);
 	writel(GOLDFISH_TTY_CMD_INT_DISABLE, qtty->base + GOLDFISH_TTY_CMD);
 }
 
-static int goldfish_tty_open(struct tty_struct * tty, struct file * filp)
+static int goldfish_tty_open(struct tty_struct *tty, struct file *filp)
 {
 	struct goldfish_tty *qtty = &goldfish_ttys[tty->index];
 	return tty_port_open(&qtty->port, tty, filp);
 }
 
-static void goldfish_tty_close(struct tty_struct * tty, struct file * filp)
+static void goldfish_tty_close(struct tty_struct *tty, struct file *filp)
 {
 	tty_port_close(tty->port, tty, filp);
 }
@@ -115,7 +121,8 @@ static void goldfish_tty_hangup(struct tty_struct *tty)
 	tty_port_hangup(tty->port);
 }
 
-static int goldfish_tty_write(struct tty_struct * tty, const unsigned char *buf, int count)
+static int goldfish_tty_write(struct tty_struct *tty, const unsigned char *buf,
+								int count)
 {
 	goldfish_tty_do_write(tty->index, buf, count);
 	return count;
@@ -133,12 +140,14 @@ static int goldfish_tty_chars_in_buffer(struct tty_struct *tty)
 	return readl(base + GOLDFISH_TTY_BYTES_READY);
 }
 
-static void goldfish_tty_console_write(struct console *co, const char *b, unsigned count)
+static void goldfish_tty_console_write(struct console *co, const char *b,
+								unsigned count)
 {
 	goldfish_tty_do_write(co->index, b, count);
 }
 
-static struct tty_driver *goldfish_tty_console_device(struct console *c, int *index)
+static struct tty_driver *goldfish_tty_console_device(struct console *c,
+								int *index)
 {
 	*index = c->index;
 	return goldfish_tty_driver;
@@ -146,9 +155,9 @@ static struct tty_driver *goldfish_tty_console_device(struct console *c, int *in
 
 static int goldfish_tty_console_setup(struct console *co, char *options)
 {
-	if((unsigned)co->index > goldfish_tty_line_count)
+	if ((unsigned)co->index > goldfish_tty_line_count)
 		return -ENODEV;
-	if(goldfish_ttys[co->index].base == 0)
+	if (goldfish_ttys[co->index].base == 0)
 		return -ENODEV;
 	return 0;
 }
@@ -158,7 +167,7 @@ static struct tty_port_operations goldfish_port_ops = {
 	.shutdown = goldfish_tty_shutdown
 };
 
-static struct tty_operations goldfish_tty_ops = {
+static const struct tty_operations goldfish_tty_ops = {
 	.open = goldfish_tty_open,
 	.close = goldfish_tty_close,
 	.hangup = goldfish_tty_hangup,
@@ -172,13 +181,14 @@ static int goldfish_tty_create_driver(void)
 	int ret;
 	struct tty_driver *tty;
 
-	goldfish_ttys = kzalloc(sizeof(*goldfish_ttys) * goldfish_tty_line_count, GFP_KERNEL);
-	if(goldfish_ttys == NULL) {
+	goldfish_ttys = kzalloc(sizeof(*goldfish_ttys) *
+				goldfish_tty_line_count, GFP_KERNEL);
+	if (goldfish_ttys == NULL) {
 		ret = -ENOMEM;
 		goto err_alloc_goldfish_ttys_failed;
 	}
 	tty = alloc_tty_driver(goldfish_tty_line_count);
-	if(tty == NULL) {
+	if (tty == NULL) {
 		ret = -ENOMEM;
 		goto err_alloc_tty_driver_failed;
 	}
@@ -187,10 +197,11 @@ static int goldfish_tty_create_driver(void)
 	tty->type = TTY_DRIVER_TYPE_SERIAL;
 	tty->subtype = SERIAL_TYPE_NORMAL;
 	tty->init_termios = tty_std_termios;
-	tty->flags = TTY_DRIVER_RESET_TERMIOS | TTY_DRIVER_REAL_RAW | TTY_DRIVER_DYNAMIC_DEV;
+	tty->flags = TTY_DRIVER_RESET_TERMIOS | TTY_DRIVER_REAL_RAW |
+						TTY_DRIVER_DYNAMIC_DEV;
 	tty_set_operations(tty, &goldfish_tty_ops);
 	ret = tty_register_driver(tty);
-	if(ret)
+	if (ret)
 		goto err_tty_register_driver_failed;
 
 	goldfish_tty_driver = tty;
@@ -225,7 +236,7 @@ static int goldfish_tty_probe(struct platform_device *pdev)
 	u32 irq;
 
 	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if(r == NULL)
+	if (r == NULL)
 		return -EINVAL;
 
 	base = ioremap(r->start, 0x1000);
@@ -233,18 +244,18 @@ static int goldfish_tty_probe(struct platform_device *pdev)
 		pr_err("goldfish_tty: unable to remap base\n");
 
 	r = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-	if(r == NULL)
+	if (r == NULL)
 		goto err_unmap;
 
 	irq = r->start;
 
-	if(pdev->id >= goldfish_tty_line_count)
+	if (pdev->id >= goldfish_tty_line_count)
 		goto err_unmap;
 
 	mutex_lock(&goldfish_tty_lock);
-	if(goldfish_tty_current_line_count == 0) {
+	if (goldfish_tty_current_line_count == 0) {
 		ret = goldfish_tty_create_driver();
-		if(ret)
+		if (ret)
 			goto err_create_driver_failed;
 	}
 	goldfish_tty_current_line_count++;
@@ -258,14 +269,15 @@ static int goldfish_tty_probe(struct platform_device *pdev)
 
 	writel(GOLDFISH_TTY_CMD_INT_DISABLE, base + GOLDFISH_TTY_CMD);
 
-	ret = request_irq(irq, goldfish_tty_interrupt, IRQF_SHARED, "goldfish_tty", pdev);
-	if(ret)
+	ret = request_irq(irq, goldfish_tty_interrupt, IRQF_SHARED,
+						"goldfish_tty", pdev);
+	if (ret)
 		goto err_request_irq_failed;
 
 
 	ttydev = tty_port_register_device(&qtty->port, goldfish_tty_driver,
 							pdev->id, &pdev->dev);
-	if(IS_ERR(ttydev)) {
+	if (IS_ERR(ttydev)) {
 		ret = PTR_ERR(ttydev);
 		goto err_tty_register_device_failed;
 	}
@@ -286,7 +298,7 @@ err_tty_register_device_failed:
 	free_irq(irq, pdev);
 err_request_irq_failed:
 	goldfish_tty_current_line_count--;
-	if(goldfish_tty_current_line_count == 0)
+	if (goldfish_tty_current_line_count == 0)
 		goldfish_tty_delete_driver();
 err_create_driver_failed:
 	mutex_unlock(&goldfish_tty_lock);
@@ -308,7 +320,7 @@ static int goldfish_tty_remove(struct platform_device *pdev)
 	qtty->base = 0;
 	free_irq(qtty->irq, pdev);
 	goldfish_tty_current_line_count--;
-	if(goldfish_tty_current_line_count == 0)
+	if (goldfish_tty_current_line_count == 0)
 		goldfish_tty_delete_driver();
 	mutex_unlock(&goldfish_tty_lock);
 	return 0;
diff --git a/drivers/tty/hvc/hvc_console.c b/drivers/tty/hvc/hvc_console.c
index 94f9e3a3841..4fcec1d793a 100644
--- a/drivers/tty/hvc/hvc_console.c
+++ b/drivers/tty/hvc/hvc_console.c
@@ -190,7 +190,7 @@ static struct tty_driver *hvc_console_device(struct console *c, int *index)
 	return hvc_driver;
 }
 
-static int __init hvc_console_setup(struct console *co, char *options)
+static int hvc_console_setup(struct console *co, char *options)
 {	
 	if (co->index < 0 || co->index >= MAX_NR_HVC_CONSOLES)
 		return -ENODEV;
@@ -760,10 +760,17 @@ static int khvcd(void *unused)
 			if (poll_mask == 0)
 				schedule();
 			else {
+				unsigned long j_timeout;
+
 				if (timeout < MAX_TIMEOUT)
 					timeout += (timeout >> 6) + 1;
 
-				msleep_interruptible(timeout);
+				/*
+				 * We don't use msleep_interruptible otherwise
+				 * "kick" will fail to wake us up
+				 */
+				j_timeout = msecs_to_jiffies(timeout) + 1;
+				schedule_timeout_interruptible(j_timeout);
 			}
 		}
 		__set_current_state(TASK_RUNNING);
diff --git a/drivers/tty/hvc/hvc_dcc.c b/drivers/tty/hvc/hvc_dcc.c
index 3502a7bbb69..809920d80a6 100644
--- a/drivers/tty/hvc/hvc_dcc.c
+++ b/drivers/tty/hvc/hvc_dcc.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2010, Code Aurora Forum. All rights reserved.
+/* Copyright (c) 2010, 2014 The Linux Foundation. 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 version 2 and
@@ -8,20 +8,11 @@
  * 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/console.h>
-#include <linux/delay.h>
-#include <linux/err.h>
 #include <linux/init.h>
-#include <linux/moduleparam.h>
-#include <linux/types.h>
 
+#include <asm/dcc.h>
 #include <asm/processor.h>
 
 #include "hvc_console.h"
@@ -30,35 +21,6 @@
 #define DCC_STATUS_RX		(1 << 30)
 #define DCC_STATUS_TX		(1 << 29)
 
-static inline u32 __dcc_getstatus(void)
-{
-	u32 __ret;
-	asm volatile("mrc p14, 0, %0, c0, c1, 0	@ read comms ctrl reg"
-		: "=r" (__ret) : : "cc");
-
-	return __ret;
-}
-
-
-static inline char __dcc_getchar(void)
-{
-	char __c;
-
-	asm volatile("mrc p14, 0, %0, c0, c5, 0	@ read comms data reg"
-		: "=r" (__c));
-	isb();
-
-	return __c;
-}
-
-static inline void __dcc_putchar(char c)
-{
-	asm volatile("mcr p14, 0, %0, c0, c5, 0	@ write a char"
-		: /* no output register */
-		: "r" (c));
-	isb();
-}
-
 static int hvc_dcc_put_chars(uint32_t vt, const char *buf, int count)
 {
 	int i;
diff --git a/drivers/tty/n_hdlc.c b/drivers/tty/n_hdlc.c
index 1b2db9a3038..644ddb841d9 100644
--- a/drivers/tty/n_hdlc.c
+++ b/drivers/tty/n_hdlc.c
@@ -848,13 +848,11 @@ static struct n_hdlc *n_hdlc_alloc(void)
 {
 	struct n_hdlc_buf *buf;
 	int i;
-	struct n_hdlc *n_hdlc = kmalloc(sizeof(*n_hdlc), GFP_KERNEL);
+	struct n_hdlc *n_hdlc = kzalloc(sizeof(*n_hdlc), GFP_KERNEL);
 
 	if (!n_hdlc)
 		return NULL;
 
-	memset(n_hdlc, 0, sizeof(*n_hdlc));
-
 	n_hdlc_buf_list_init(&n_hdlc->rx_free_buf_list);
 	n_hdlc_buf_list_init(&n_hdlc->tx_free_buf_list);
 	n_hdlc_buf_list_init(&n_hdlc->rx_buf_list);
@@ -952,8 +950,6 @@ static char hdlc_register_ok[] __initdata =
 	KERN_INFO "N_HDLC line discipline registered.\n";
 static char hdlc_register_fail[] __initdata =
 	KERN_ERR "error registering line discipline: %d\n";
-static char hdlc_init_fail[] __initdata =
-	KERN_INFO "N_HDLC: init failure %d\n";
 
 static int __init n_hdlc_init(void)
 {
@@ -973,8 +969,6 @@ static int __init n_hdlc_init(void)
 	else
 		printk(hdlc_register_fail, status);
 
-	if (status)
-		printk(hdlc_init_fail, status);
 	return status;
 	
 }	/* end of init_module() */
diff --git a/drivers/tty/n_tty.c b/drivers/tty/n_tty.c
index 746ae80b972..f95569dedc8 100644
--- a/drivers/tty/n_tty.c
+++ b/drivers/tty/n_tty.c
@@ -2353,8 +2353,12 @@ static ssize_t n_tty_write(struct tty_struct *tty, struct file *file,
 			if (tty->ops->flush_chars)
 				tty->ops->flush_chars(tty);
 		} else {
+			struct n_tty_data *ldata = tty->disc_data;
+
 			while (nr > 0) {
+				mutex_lock(&ldata->output_lock);
 				c = tty->ops->write(tty, b, nr);
+				mutex_unlock(&ldata->output_lock);
 				if (c < 0) {
 					retval = c;
 					goto break_out;
diff --git a/drivers/tty/serial/8250/8250_core.c b/drivers/tty/serial/8250/8250_core.c
index 81f909c2101..27f7ad6b74c 100644
--- a/drivers/tty/serial/8250/8250_core.c
+++ b/drivers/tty/serial/8250/8250_core.c
@@ -555,7 +555,7 @@ static void serial8250_set_sleep(struct uart_8250_port *p, int sleep)
 	 */
 	if ((p->port.type == PORT_XR17V35X) ||
 	   (p->port.type == PORT_XR17D15X)) {
-		serial_out(p, UART_EXAR_SLEEP, 0xff);
+		serial_out(p, UART_EXAR_SLEEP, sleep ? 0xff : 0);
 		return;
 	}
 
@@ -1520,7 +1520,7 @@ int serial8250_handle_irq(struct uart_port *port, unsigned int iir)
 			status = serial8250_rx_chars(up, status);
 	}
 	serial8250_modem_status(up);
-	if (status & UART_LSR_THRE)
+	if (!up->dma && (status & UART_LSR_THRE))
 		serial8250_tx_chars(up);
 
 	spin_unlock_irqrestore(&port->lock, flags);
@@ -1926,13 +1926,8 @@ static void serial8250_put_poll_char(struct uart_port *port,
 	wait_for_xmitr(up, BOTH_EMPTY);
 	/*
 	 *	Send the character out.
-	 *	If a LF, also do CR...
 	 */
 	serial_port_out(port, UART_TX, c);
-	if (c == 10) {
-		wait_for_xmitr(up, BOTH_EMPTY);
-		serial_port_out(port, UART_TX, 13);
-	}
 
 	/*
 	 *	Finally, wait for transmitter to become empty
@@ -2338,9 +2333,11 @@ serial8250_do_set_termios(struct uart_port *port, struct ktermios *termios,
 	 * the trigger, or the MCR RTS bit is cleared.  In the case where
 	 * the remote UART is not using CTS auto flow control, we must
 	 * have sufficient FIFO entries for the latency of the remote
-	 * UART to respond.  IOW, at least 32 bytes of FIFO.
+	 * UART to respond.  IOW, at least 32 bytes of FIFO. Also enable
+	 * AFE if hw flow control is supported
 	 */
-	if (up->capabilities & UART_CAP_AFE && port->fifosize >= 32) {
+	if ((up->capabilities & UART_CAP_AFE && (port->fifosize >= 32)) ||
+	    (port->flags & UPF_HARD_FLOW)) {
 		up->mcr &= ~UART_MCR_AFE;
 		if (termios->c_cflag & CRTSCTS)
 			up->mcr |= UART_MCR_AFE;
diff --git a/drivers/tty/serial/8250/8250_dma.c b/drivers/tty/serial/8250/8250_dma.c
index 7046769608d..148ffe4c232 100644
--- a/drivers/tty/serial/8250/8250_dma.c
+++ b/drivers/tty/serial/8250/8250_dma.c
@@ -20,12 +20,15 @@ static void __dma_tx_complete(void *param)
 	struct uart_8250_port	*p = param;
 	struct uart_8250_dma	*dma = p->dma;
 	struct circ_buf		*xmit = &p->port.state->xmit;
-
-	dma->tx_running = 0;
+	unsigned long	flags;
 
 	dma_sync_single_for_cpu(dma->txchan->device->dev, dma->tx_addr,
 				UART_XMIT_SIZE, DMA_TO_DEVICE);
 
+	spin_lock_irqsave(&p->port.lock, flags);
+
+	dma->tx_running = 0;
+
 	xmit->tail += dma->tx_size;
 	xmit->tail &= UART_XMIT_SIZE - 1;
 	p->port.icount.tx += dma->tx_size;
@@ -35,6 +38,8 @@ static void __dma_tx_complete(void *param)
 
 	if (!uart_circ_empty(xmit) && !uart_tx_stopped(&p->port))
 		serial8250_tx_dma(p);
+
+	spin_unlock_irqrestore(&p->port.lock, flags);
 }
 
 static void __dma_rx_complete(void *param)
@@ -187,21 +192,28 @@ int serial8250_request_dma(struct uart_8250_port *p)
 
 	dma->rx_buf = dma_alloc_coherent(dma->rxchan->device->dev, dma->rx_size,
 					&dma->rx_addr, GFP_KERNEL);
-	if (!dma->rx_buf) {
-		dma_release_channel(dma->rxchan);
-		dma_release_channel(dma->txchan);
-		return -ENOMEM;
-	}
+	if (!dma->rx_buf)
+		goto err;
 
 	/* TX buffer */
 	dma->tx_addr = dma_map_single(dma->txchan->device->dev,
 					p->port.state->xmit.buf,
 					UART_XMIT_SIZE,
 					DMA_TO_DEVICE);
+	if (dma_mapping_error(dma->txchan->device->dev, dma->tx_addr)) {
+		dma_free_coherent(dma->rxchan->device->dev, dma->rx_size,
+				  dma->rx_buf, dma->rx_addr);
+		goto err;
+	}
 
 	dev_dbg_ratelimited(p->port.dev, "got both dma channels\n");
 
 	return 0;
+err:
+	dma_release_channel(dma->rxchan);
+	dma_release_channel(dma->txchan);
+
+	return -ENOMEM;
 }
 EXPORT_SYMBOL_GPL(serial8250_request_dma);
 
diff --git a/drivers/tty/serial/8250/8250_dw.c b/drivers/tty/serial/8250/8250_dw.c
index ed311357674..51b307aab75 100644
--- a/drivers/tty/serial/8250/8250_dw.c
+++ b/drivers/tty/serial/8250/8250_dw.c
@@ -62,6 +62,70 @@ struct dw8250_data {
 	struct uart_8250_dma	dma;
 };
 
+struct dw8250_acpi_desc {
+	void (*set_termios)(struct uart_port *p, struct ktermios *termios,
+			    struct ktermios *old);
+};
+
+#define BYT_PRV_CLK			0x800
+#define BYT_PRV_CLK_EN			(1 << 0)
+#define BYT_PRV_CLK_M_VAL_SHIFT		1
+#define BYT_PRV_CLK_N_VAL_SHIFT		16
+#define BYT_PRV_CLK_UPDATE		(1 << 31)
+
+static void byt_set_termios(struct uart_port *p, struct ktermios *termios,
+			    struct ktermios *old)
+{
+	unsigned int baud = tty_termios_baud_rate(termios);
+	unsigned int m, n;
+	u32 reg;
+
+	/*
+	* For baud rates 0.5M, 1M, 1.5M, 2M, 2.5M, 3M, 3.5M and 4M the
+	* dividers must be adjusted.
+	*
+	* uartclk = (m / n) * 100 MHz, where m <= n
+	*/
+	switch (baud) {
+	case 500000:
+	case 1000000:
+	case 2000000:
+	case 4000000:
+		m = 64;
+		n = 100;
+		p->uartclk = 64000000;
+		break;
+	case 3500000:
+		m = 56;
+		n = 100;
+		p->uartclk = 56000000;
+		break;
+	case 1500000:
+	case 3000000:
+		m = 48;
+		n = 100;
+		p->uartclk = 48000000;
+		break;
+	case 2500000:
+		m = 40;
+		n = 100;
+		p->uartclk = 40000000;
+		break;
+	default:
+		m = 2304;
+		n = 3125;
+		p->uartclk = 73728000;
+	}
+
+	/* Reset the clock */
+	reg = (m << BYT_PRV_CLK_M_VAL_SHIFT) | (n << BYT_PRV_CLK_N_VAL_SHIFT);
+	writel(reg, p->membase + BYT_PRV_CLK);
+	reg |= BYT_PRV_CLK_EN | BYT_PRV_CLK_UPDATE;
+	writel(reg, p->membase + BYT_PRV_CLK);
+
+	serial8250_do_set_termios(p, termios, old);
+}
+
 static inline int dw8250_modify_msr(struct uart_port *p, int offset, int value)
 {
 	struct dw8250_data *d = p->private_data;
@@ -278,6 +342,7 @@ static int dw8250_probe_acpi(struct uart_8250_port *up,
 {
 	const struct acpi_device_id *id;
 	struct uart_port *p = &up->port;
+	struct dw8250_acpi_desc *acpi_desc;
 
 	dw8250_setup_port(up);
 
@@ -290,14 +355,18 @@ static int dw8250_probe_acpi(struct uart_8250_port *up,
 	p->serial_out = dw8250_serial_out32;
 	p->regshift = 2;
 
-	if (!p->uartclk)
-		p->uartclk = (unsigned int)id->driver_data;
-
 	up->dma = &data->dma;
 
 	up->dma->rxconf.src_maxburst = p->fifosize / 4;
 	up->dma->txconf.dst_maxburst = p->fifosize / 4;
 
+	acpi_desc = (struct dw8250_acpi_desc *)id->driver_data;
+	if (!acpi_desc)
+		return 0;
+
+	if (acpi_desc->set_termios)
+		p->set_termios = acpi_desc->set_termios;
+
 	return 0;
 }
 
@@ -445,12 +514,16 @@ static const struct of_device_id dw8250_of_match[] = {
 };
 MODULE_DEVICE_TABLE(of, dw8250_of_match);
 
+static struct dw8250_acpi_desc byt_8250_desc = {
+	.set_termios = byt_set_termios,
+};
+
 static const struct acpi_device_id dw8250_acpi_match[] = {
 	{ "INT33C4", 0 },
 	{ "INT33C5", 0 },
 	{ "INT3434", 0 },
 	{ "INT3435", 0 },
-	{ "80860F0A", 0 },
+	{ "80860F0A", (kernel_ulong_t)&byt_8250_desc},
 	{ },
 };
 MODULE_DEVICE_TABLE(acpi, dw8250_acpi_match);
diff --git a/drivers/tty/serial/8250/8250_early.c b/drivers/tty/serial/8250/8250_early.c
index c100d6343d5..cfef801a49d 100644
--- a/drivers/tty/serial/8250/8250_early.c
+++ b/drivers/tty/serial/8250/8250_early.c
@@ -35,18 +35,8 @@
 #include <linux/serial_8250.h>
 #include <asm/io.h>
 #include <asm/serial.h>
-#ifdef CONFIG_FIX_EARLYCON_MEM
-#include <asm/pgtable.h>
-#include <asm/fixmap.h>
-#endif
 
-struct early_serial8250_device {
-	struct uart_port port;
-	char options[16];		/* e.g., 115200n8 */
-	unsigned int baud;
-};
-
-static struct early_serial8250_device early_device;
+static struct earlycon_device *early_device;
 
 unsigned int __weak __init serial8250_early_in(struct uart_port *port, int offset)
 {
@@ -100,7 +90,7 @@ static void __init serial_putc(struct uart_port *port, int c)
 static void __init early_serial8250_write(struct console *console,
 					const char *s, unsigned int count)
 {
-	struct uart_port *port = &early_device.port;
+	struct uart_port *port = &early_device->port;
 	unsigned int ier;
 
 	/* Save the IER and disable interrupts */
@@ -129,7 +119,7 @@ static unsigned int __init probe_baud(struct uart_port *port)
 	return (port->uartclk / 16) / quot;
 }
 
-static void __init init_port(struct early_serial8250_device *device)
+static void __init init_port(struct earlycon_device *device)
 {
 	struct uart_port *port = &device->port;
 	unsigned int divisor;
@@ -148,128 +138,42 @@ static void __init init_port(struct early_serial8250_device *device)
 	serial8250_early_out(port, UART_LCR, c & ~UART_LCR_DLAB);
 }
 
-static int __init parse_options(struct early_serial8250_device *device,
-								char *options)
+static int __init early_serial8250_setup(struct earlycon_device *device,
+					 const char *options)
 {
-	struct uart_port *port = &device->port;
-	int mmio, mmio32, length;
-
-	if (!options)
-		return -ENODEV;
-
-	port->uartclk = BASE_BAUD * 16;
-
-	mmio = !strncmp(options, "mmio,", 5);
-	mmio32 = !strncmp(options, "mmio32,", 7);
-	if (mmio || mmio32) {
-		port->iotype = (mmio ? UPIO_MEM : UPIO_MEM32);
-		port->mapbase = simple_strtoul(options + (mmio ? 5 : 7),
-					       &options, 0);
-		if (mmio32)
-			port->regshift = 2;
-#ifdef CONFIG_FIX_EARLYCON_MEM
-		set_fixmap_nocache(FIX_EARLYCON_MEM_BASE,
-					port->mapbase & PAGE_MASK);
-		port->membase =
-			(void __iomem *)__fix_to_virt(FIX_EARLYCON_MEM_BASE);
-		port->membase += port->mapbase & ~PAGE_MASK;
-#else
-		port->membase = ioremap_nocache(port->mapbase, 64);
-		if (!port->membase) {
-			printk(KERN_ERR "%s: Couldn't ioremap 0x%llx\n",
-				__func__,
-			       (unsigned long long) port->mapbase);
-			return -ENOMEM;
-		}
-#endif
-	} else if (!strncmp(options, "io,", 3)) {
-		port->iotype = UPIO_PORT;
-		port->iobase = simple_strtoul(options + 3, &options, 0);
-		mmio = 0;
-	} else
-		return -EINVAL;
-
-	options = strchr(options, ',');
-	if (options) {
-		options++;
-		device->baud = simple_strtoul(options, NULL, 0);
-		length = min(strcspn(options, " ") + 1,
-			     (size_t)(sizeof(device->options)));
-		strlcpy(device->options, options, length);
-	} else {
-		device->baud = probe_baud(port);
-		snprintf(device->options, sizeof(device->options), "%u",
-			device->baud);
-	}
-
-	if (mmio || mmio32)
-		printk(KERN_INFO
-		       "Early serial console at MMIO%s 0x%llx (options '%s')\n",
-			mmio32 ? "32" : "",
-			(unsigned long long)port->mapbase,
-			device->options);
-	else
-		printk(KERN_INFO
-		      "Early serial console at I/O port 0x%lx (options '%s')\n",
-			port->iobase,
-			device->options);
-
-	return 0;
-}
-
-static struct console early_serial8250_console __initdata = {
-	.name	= "uart",
-	.write	= early_serial8250_write,
-	.flags	= CON_PRINTBUFFER | CON_BOOT,
-	.index	= -1,
-};
-
-static int __init early_serial8250_setup(char *options)
-{
-	struct early_serial8250_device *device = &early_device;
-	int err;
-
-	if (device->port.membase || device->port.iobase)
+	if (!(device->port.membase || device->port.iobase))
 		return 0;
 
-	err = parse_options(device, options);
-	if (err < 0)
-		return err;
+	if (!device->baud)
+		device->baud = probe_baud(&device->port);
 
 	init_port(device);
+
+	early_device = device;
+	device->con->write = early_serial8250_write;
 	return 0;
 }
+EARLYCON_DECLARE(uart8250, early_serial8250_setup);
+EARLYCON_DECLARE(uart, early_serial8250_setup);
 
 int __init setup_early_serial8250_console(char *cmdline)
 {
-	char *options;
-	int err;
+	char match[] = "uart8250";
 
-	options = strstr(cmdline, "uart8250,");
-	if (!options) {
-		options = strstr(cmdline, "uart,");
-		if (!options)
-			return 0;
-	}
+	if (cmdline && cmdline[4] == ',')
+		match[4] = '\0';
 
-	options = strchr(cmdline, ',') + 1;
-	err = early_serial8250_setup(options);
-	if (err < 0)
-		return err;
-
-	register_console(&early_serial8250_console);
-
-	return 0;
+	return setup_earlycon(cmdline, match, early_serial8250_setup);
 }
 
 int serial8250_find_port_for_earlycon(void)
 {
-	struct early_serial8250_device *device = &early_device;
-	struct uart_port *port = &device->port;
+	struct earlycon_device *device = early_device;
+	struct uart_port *port = device ? &device->port : NULL;
 	int line;
 	int ret;
 
-	if (!device->port.membase && !device->port.iobase)
+	if (!port || (!port->membase && !port->iobase))
 		return -ENODEV;
 
 	line = serial8250_find_port(port);
@@ -284,5 +188,3 @@ int serial8250_find_port_for_earlycon(void)
 
 	return ret;
 }
-
-early_param("earlycon", setup_early_serial8250_console);
diff --git a/drivers/tty/serial/8250/8250_pci.c b/drivers/tty/serial/8250/8250_pci.c
index b14bcba96c2..33137b3ba94 100644
--- a/drivers/tty/serial/8250/8250_pci.c
+++ b/drivers/tty/serial/8250/8250_pci.c
@@ -1753,6 +1753,8 @@ pci_wch_ch353_setup(struct serial_private *priv,
 #define PCI_VENDOR_ID_ADVANTECH		0x13fe
 #define PCI_DEVICE_ID_INTEL_CE4100_UART 0x2e66
 #define PCI_DEVICE_ID_ADVANTECH_PCI3620	0x3620
+#define PCI_DEVICE_ID_ADVANTECH_PCI3618	0x3618
+#define PCI_DEVICE_ID_ADVANTECH_PCIf618	0xf618
 #define PCI_DEVICE_ID_TITAN_200I	0x8028
 #define PCI_DEVICE_ID_TITAN_400I	0x8048
 #define PCI_DEVICE_ID_TITAN_800I	0x8088
@@ -1778,6 +1780,7 @@ pci_wch_ch353_setup(struct serial_private *priv,
 #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_1S1P	0x5053
 #define PCI_DEVICE_ID_WCH_CH353_2S1P	0x7053
 #define PCI_VENDOR_ID_AGESTAR		0x5372
 #define PCI_DEVICE_ID_AGESTAR_9375	0x6872
@@ -2410,6 +2413,14 @@ static struct pci_serial_quirk pci_serial_quirks[] __refdata = {
 		.subdevice	= PCI_ANY_ID,
 		.setup		= pci_omegapci_setup,
 	},
+	/* WCH CH353 1S1P card (16550 clone) */
+	{
+		.vendor         = PCI_VENDOR_ID_WCH,
+		.device         = PCI_DEVICE_ID_WCH_CH353_1S1P,
+		.subvendor      = PCI_ANY_ID,
+		.subdevice      = PCI_ANY_ID,
+		.setup          = pci_wch_ch353_setup,
+	},
 	/* WCH CH353 2S1P card (16550 clone) */
 	{
 		.vendor         = PCI_VENDOR_ID_WCH,
@@ -3526,6 +3537,7 @@ static const struct pci_device_id blacklist[] = {
 
 	/* multi-io cards handled by parport_serial */
 	{ PCI_DEVICE(0x4348, 0x7053), }, /* WCH CH353 2S1P */
+	{ PCI_DEVICE(0x4348, 0x5053), }, /* WCH CH353 1S1P */
 };
 
 /*
@@ -3880,6 +3892,13 @@ static struct pci_device_id serial_pci_tbl[] = {
 	{	PCI_VENDOR_ID_ADVANTECH, PCI_DEVICE_ID_ADVANTECH_PCI3620,
 		PCI_DEVICE_ID_ADVANTECH_PCI3620, 0x0001, 0, 0,
 		pbn_b2_8_921600 },
+	/* Advantech also use 0x3618 and 0xf618 */
+	{	PCI_VENDOR_ID_ADVANTECH, PCI_DEVICE_ID_ADVANTECH_PCI3618,
+		PCI_DEVICE_ID_ADVANTECH_PCI3618, PCI_ANY_ID, 0, 0,
+		pbn_b0_4_921600 },
+	{	PCI_VENDOR_ID_ADVANTECH, PCI_DEVICE_ID_ADVANTECH_PCIf618,
+		PCI_DEVICE_ID_ADVANTECH_PCI3618, PCI_ANY_ID, 0, 0,
+		pbn_b0_4_921600 },
 	{	PCI_VENDOR_ID_V3, PCI_DEVICE_ID_V3_V960,
 		PCI_SUBVENDOR_ID_CONNECT_TECH,
 		PCI_SUBDEVICE_ID_CONNECT_TECH_BH8_232, 0, 0,
diff --git a/drivers/tty/serial/8250/Kconfig b/drivers/tty/serial/8250/Kconfig
index 23329918f22..349ee598b34 100644
--- a/drivers/tty/serial/8250/Kconfig
+++ b/drivers/tty/serial/8250/Kconfig
@@ -61,6 +61,7 @@ config SERIAL_8250_CONSOLE
 	bool "Console on 8250/16550 and compatible serial port"
 	depends on SERIAL_8250=y
 	select SERIAL_CORE_CONSOLE
+	select SERIAL_EARLYCON
 	---help---
 	  If you say Y here, it will be possible to use a serial port as the
 	  system console (the system console is the device which receives all
@@ -90,11 +91,6 @@ config SERIAL_8250_CONSOLE
 
 	  If unsure, say N.
 
-config FIX_EARLYCON_MEM
-	bool
-	depends on X86
-	default y
-
 config SERIAL_8250_GSC
 	tristate
 	depends on SERIAL_8250 && GSC
diff --git a/drivers/tty/serial/Kconfig b/drivers/tty/serial/Kconfig
index 2e6d8ddc442..fb57159bad3 100644
--- a/drivers/tty/serial/Kconfig
+++ b/drivers/tty/serial/Kconfig
@@ -7,6 +7,13 @@ if TTY
 menu "Serial drivers"
 	depends on HAS_IOMEM
 
+config SERIAL_EARLYCON
+	bool
+	help
+	  Support for early consoles with the earlycon parameter. This enables
+	  the console before standard serial driver is probed. The console is
+	  enabled when early_param is processed.
+
 source "drivers/tty/serial/8250/Kconfig"
 
 comment "Non-8250 serial port support"
@@ -53,6 +60,7 @@ config SERIAL_AMBA_PL011_CONSOLE
 	bool "Support for console on AMBA serial port"
 	depends on SERIAL_AMBA_PL011=y
 	select SERIAL_CORE_CONSOLE
+	select SERIAL_EARLYCON
 	---help---
 	  Say Y here if you wish to use an AMBA PrimeCell UART as the system
 	  console (the system console is the device which receives all kernel
@@ -65,6 +73,18 @@ config SERIAL_AMBA_PL011_CONSOLE
 	  your boot loader (lilo or loadlin) about how to pass options to the
 	  kernel at boot time.)
 
+config SERIAL_EARLYCON_ARM_SEMIHOST
+	bool "Early console using ARM semihosting"
+	depends on ARM64 || ARM
+	select SERIAL_CORE
+	select SERIAL_CORE_CONSOLE
+	select SERIAL_EARLYCON
+	help
+	  Support for early debug console using ARM semihosting. This enables
+	  the console before standard serial driver is probed. This is enabled
+	  with "earlycon=smh" on the kernel command line. The console is
+	  enabled when early_param is processed.
+
 config SERIAL_SB1250_DUART
 	tristate "BCM1xxx on-chip DUART serial support"
 	depends on SIBYTE_SB1xxx_SOC=y
@@ -97,6 +117,7 @@ config SERIAL_ATMEL
 	bool "AT91 / AT32 on-chip serial port support"
 	depends on ARCH_AT91 || AVR32
 	select SERIAL_CORE
+	select SERIAL_MCTRL_GPIO
 	help
 	  This enables the driver for the on-chip UARTs of the Atmel
 	  AT91 and AT32 processors.
@@ -1160,6 +1181,16 @@ config SERIAL_SCCNXP_CONSOLE
 	help
 	  Support for console on SCCNXP serial ports.
 
+config SERIAL_SC16IS7XX
+	tristate "SC16IS7xx serial support"
+	depends on I2C
+	select SERIAL_CORE
+	select REGMAP_I2C if I2C
+	help
+	  This selects support for SC16IS7xx serial ports.
+	  Supported ICs are SC16IS740, SC16IS741, SC16IS750, SC16IS752,
+	  SC16IS760 and SC16IS762.
+
 config SERIAL_BFIN_SPORT
 	tristate "Blackfin SPORT emulate UART"
 	depends on BLACKFIN
@@ -1226,6 +1257,7 @@ config SERIAL_BFIN_SPORT3_UART_CTSRTS
 config SERIAL_TIMBERDALE
 	tristate "Support for timberdale UART"
 	select SERIAL_CORE
+	depends on X86_32 || COMPILE_TEST
 	---help---
 	Add support for UART controller on timberdale.
 
@@ -1322,7 +1354,7 @@ config SERIAL_IFX6X60
 
 config SERIAL_PCH_UART
 	tristate "Intel EG20T PCH/LAPIS Semicon IOH(ML7213/ML7223/ML7831) UART"
-	depends on PCI
+	depends on PCI && (X86_32 || COMPILE_TEST)
 	select SERIAL_CORE
 	help
 	  This driver is for PCH(Platform controller Hub) UART of Intel EG20T
@@ -1368,18 +1400,19 @@ config SERIAL_MXS_AUART_CONSOLE
 	  Enable a MXS AUART port to be the system console.
 
 config SERIAL_XILINX_PS_UART
-	tristate "Xilinx PS UART support"
+	tristate "Cadence (Xilinx Zynq) UART support"
 	depends on OF
 	select SERIAL_CORE
 	help
-	  This driver supports the Xilinx PS UART port.
+	  This driver supports the Cadence UART. It is found e.g. in Xilinx
+	  Zynq.
 
 config SERIAL_XILINX_PS_UART_CONSOLE
-	bool "Xilinx PS UART console support"
+	bool "Cadence UART console support"
 	depends on SERIAL_XILINX_PS_UART=y
 	select SERIAL_CORE_CONSOLE
 	help
-	  Enable a Xilinx PS UART port to be the system console.
+	  Enable a Cadence UART port to be the system console.
 
 config SERIAL_AR933X
 	tristate "AR933X serial port support"
@@ -1478,6 +1511,7 @@ config SERIAL_RP2_NR_UARTS
 
 config SERIAL_FSL_LPUART
 	tristate "Freescale lpuart serial port support"
+	depends on HAS_DMA
 	select SERIAL_CORE
 	help
 	  Support for the on-chip lpuart on some Freescale SOCs.
@@ -1507,6 +1541,20 @@ config SERIAL_ST_ASC_CONSOLE
 	depends on SERIAL_ST_ASC=y
 	select SERIAL_CORE_CONSOLE
 
+config SERIAL_MEN_Z135
+	tristate "MEN 16z135 Support"
+	select SERIAL_CORE
+	depends on MCB
+	help
+	  Say yes here to enable support for the MEN 16z135 High Speed UART IP-Core
+	  on a MCB carrier.
+
+	  This driver can also be build as a module. If so, the module will be called
+	  men_z135_uart.ko
+
 endmenu
 
+config SERIAL_MCTRL_GPIO
+	tristate
+
 endif # TTY
diff --git a/drivers/tty/serial/Makefile b/drivers/tty/serial/Makefile
index 3680854fef4..0080cc362e0 100644
--- a/drivers/tty/serial/Makefile
+++ b/drivers/tty/serial/Makefile
@@ -5,6 +5,9 @@
 obj-$(CONFIG_SERIAL_CORE) += serial_core.o
 obj-$(CONFIG_SERIAL_21285) += 21285.o
 
+obj-$(CONFIG_SERIAL_EARLYCON) += earlycon.o
+obj-$(CONFIG_SERIAL_EARLYCON_ARM_SEMIHOST) += earlycon-arm-semihost.o
+
 # These Sparc drivers have to appear before others such as 8250
 # which share ttySx minor node space.  Otherwise console device
 # names change and other unplesantries.
@@ -48,6 +51,7 @@ obj-$(CONFIG_SERIAL_MPSC) += mpsc.o
 obj-$(CONFIG_SERIAL_SB1250_DUART) += sb1250-duart.o
 obj-$(CONFIG_ETRAX_SERIAL) += crisv10.o
 obj-$(CONFIG_SERIAL_SCCNXP) += sccnxp.o
+obj-$(CONFIG_SERIAL_SC16IS7XX) += sc16is7xx.o
 obj-$(CONFIG_SERIAL_JSM) += jsm/
 obj-$(CONFIG_SERIAL_TXX9) += serial_txx9.o
 obj-$(CONFIG_SERIAL_VR41XX) += vr41xx_siu.o
@@ -87,3 +91,7 @@ obj-$(CONFIG_SERIAL_EFM32_UART) += efm32-uart.o
 obj-$(CONFIG_SERIAL_ARC)	+= arc_uart.o
 obj-$(CONFIG_SERIAL_RP2)	+= rp2.o
 obj-$(CONFIG_SERIAL_FSL_LPUART)	+= fsl_lpuart.o
+obj-$(CONFIG_SERIAL_MEN_Z135)	+= men_z135_uart.o
+
+# GPIOLIB helpers for modem control lines
+obj-$(CONFIG_SERIAL_MCTRL_GPIO)	+= serial_mctrl_gpio.o
diff --git a/drivers/tty/serial/amba-pl011.c b/drivers/tty/serial/amba-pl011.c
index d4eda24aa68..ee3d8034678 100644
--- a/drivers/tty/serial/amba-pl011.c
+++ b/drivers/tty/serial/amba-pl011.c
@@ -303,7 +303,7 @@ static void pl011_dma_probe_initcall(struct device *dev, struct uart_amba_port *
 
 	/* Optionally make use of an RX channel as well */
 	chan = dma_request_slave_channel(dev, "rx");
-	
+
 	if (!chan && plat->dma_rx_param) {
 		chan = dma_request_channel(mask, plat->dma_filter, plat->dma_rx_param);
 
@@ -318,7 +318,7 @@ static void pl011_dma_probe_initcall(struct device *dev, struct uart_amba_port *
 			.src_addr = uap->port.mapbase + UART01x_DR,
 			.src_addr_width = DMA_SLAVE_BUSWIDTH_1_BYTE,
 			.direction = DMA_DEV_TO_MEM,
-			.src_maxburst = uap->fifosize >> 1,
+			.src_maxburst = uap->fifosize >> 2,
 			.device_fc = false,
 		};
 
@@ -2045,6 +2045,34 @@ static struct console amba_console = {
 };
 
 #define AMBA_CONSOLE	(&amba_console)
+
+static void pl011_putc(struct uart_port *port, int c)
+{
+	while (readl(port->membase + UART01x_FR) & UART01x_FR_TXFF)
+		;
+	writeb(c, port->membase + UART01x_DR);
+	while (readl(port->membase + UART01x_FR) & UART01x_FR_BUSY)
+		;
+}
+
+static void pl011_early_write(struct console *con, const char *s, unsigned n)
+{
+	struct earlycon_device *dev = con->data;
+
+	uart_console_write(&dev->port, s, n, pl011_putc);
+}
+
+static int __init pl011_early_console_setup(struct earlycon_device *device,
+					    const char *opt)
+{
+	if (!device->port.membase)
+		return -ENODEV;
+
+	device->con->write = pl011_early_write;
+	return 0;
+}
+EARLYCON_DECLARE(pl011, pl011_early_console_setup);
+
 #else
 #define AMBA_CONSOLE	NULL
 #endif
@@ -2176,6 +2204,7 @@ static int pl011_probe(struct amba_device *dev, const struct amba_id *id)
 static int pl011_remove(struct amba_device *dev)
 {
 	struct uart_amba_port *uap = amba_get_drvdata(dev);
+	bool busy = false;
 	int i;
 
 	uart_remove_one_port(&amba_reg, &uap->port);
@@ -2183,9 +2212,12 @@ static int pl011_remove(struct amba_device *dev)
 	for (i = 0; i < ARRAY_SIZE(amba_ports); i++)
 		if (amba_ports[i] == uap)
 			amba_ports[i] = NULL;
+		else if (amba_ports[i])
+			busy = true;
 
 	pl011_dma_remove(uap);
-	uart_unregister_driver(&amba_reg);
+	if (!busy)
+		uart_unregister_driver(&amba_reg);
 	return 0;
 }
 
diff --git a/drivers/tty/serial/atmel_serial.c b/drivers/tty/serial/atmel_serial.c
index 53eeea13ff1..3fceae099c4 100644
--- a/drivers/tty/serial/atmel_serial.c
+++ b/drivers/tty/serial/atmel_serial.c
@@ -43,6 +43,9 @@
 #include <linux/platform_data/atmel.h>
 #include <linux/timer.h>
 #include <linux/gpio.h>
+#include <linux/gpio/consumer.h>
+#include <linux/err.h>
+#include <linux/irq.h>
 
 #include <asm/io.h>
 #include <asm/ioctls.h>
@@ -57,6 +60,8 @@
 
 #include <linux/serial_core.h>
 
+#include "serial_mctrl_gpio.h"
+
 static void atmel_start_rx(struct uart_port *port);
 static void atmel_stop_rx(struct uart_port *port);
 
@@ -162,8 +167,10 @@ struct atmel_uart_port {
 	struct circ_buf		rx_ring;
 
 	struct serial_rs485	rs485;		/* rs485 settings */
-	int			rts_gpio;	/* optional RTS GPIO */
+	struct mctrl_gpios	*gpios;
+	int			gpio_irq[UART_GPIO_MAX];
 	unsigned int		tx_done_mask;
+	bool			ms_irq_enabled;
 	bool			is_usart;	/* usart or uart */
 	struct timer_list	uart_timer;	/* uart timer */
 	int (*prepare_rx)(struct uart_port *port);
@@ -237,6 +244,50 @@ static bool atmel_use_dma_rx(struct uart_port *port)
 	return atmel_port->use_dma_rx;
 }
 
+static unsigned int atmel_get_lines_status(struct uart_port *port)
+{
+	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
+	unsigned int status, ret = 0;
+
+	status = UART_GET_CSR(port);
+
+	mctrl_gpio_get(atmel_port->gpios, &ret);
+
+	if (!IS_ERR_OR_NULL(mctrl_gpio_to_gpiod(atmel_port->gpios,
+						UART_GPIO_CTS))) {
+		if (ret & TIOCM_CTS)
+			status &= ~ATMEL_US_CTS;
+		else
+			status |= ATMEL_US_CTS;
+	}
+
+	if (!IS_ERR_OR_NULL(mctrl_gpio_to_gpiod(atmel_port->gpios,
+						UART_GPIO_DSR))) {
+		if (ret & TIOCM_DSR)
+			status &= ~ATMEL_US_DSR;
+		else
+			status |= ATMEL_US_DSR;
+	}
+
+	if (!IS_ERR_OR_NULL(mctrl_gpio_to_gpiod(atmel_port->gpios,
+						UART_GPIO_RI))) {
+		if (ret & TIOCM_RI)
+			status &= ~ATMEL_US_RI;
+		else
+			status |= ATMEL_US_RI;
+	}
+
+	if (!IS_ERR_OR_NULL(mctrl_gpio_to_gpiod(atmel_port->gpios,
+						UART_GPIO_DCD))) {
+		if (ret & TIOCM_CD)
+			status &= ~ATMEL_US_DCD;
+		else
+			status |= ATMEL_US_DCD;
+	}
+
+	return status;
+}
+
 /* Enable or disable the rs485 support */
 void atmel_config_rs485(struct uart_port *port, struct serial_rs485 *rs485conf)
 {
@@ -296,17 +347,6 @@ static void atmel_set_mctrl(struct uart_port *port, u_int mctrl)
 	unsigned int mode;
 	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
 
-	/*
-	 * AT91RM9200 Errata #39: RTS0 is not internally connected
-	 * to PA21. We need to drive the pin as a GPIO.
-	 */
-	if (gpio_is_valid(atmel_port->rts_gpio)) {
-		if (mctrl & TIOCM_RTS)
-			gpio_set_value(atmel_port->rts_gpio, 0);
-		else
-			gpio_set_value(atmel_port->rts_gpio, 1);
-	}
-
 	if (mctrl & TIOCM_RTS)
 		control |= ATMEL_US_RTSEN;
 	else
@@ -319,6 +359,8 @@ static void atmel_set_mctrl(struct uart_port *port, u_int mctrl)
 
 	UART_PUT_CR(port, control);
 
+	mctrl_gpio_set(atmel_port->gpios, mctrl);
+
 	/* Local loopback mode? */
 	mode = UART_GET_MR(port) & ~ATMEL_US_CHMODE;
 	if (mctrl & TIOCM_LOOP)
@@ -346,7 +388,8 @@ static void atmel_set_mctrl(struct uart_port *port, u_int mctrl)
  */
 static u_int atmel_get_mctrl(struct uart_port *port)
 {
-	unsigned int status, ret = 0;
+	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
+	unsigned int ret = 0, status;
 
 	status = UART_GET_CSR(port);
 
@@ -362,7 +405,7 @@ static u_int atmel_get_mctrl(struct uart_port *port)
 	if (!(status & ATMEL_US_RI))
 		ret |= TIOCM_RI;
 
-	return ret;
+	return mctrl_gpio_get(atmel_port->gpios, &ret);
 }
 
 /*
@@ -449,8 +492,38 @@ static void atmel_stop_rx(struct uart_port *port)
  */
 static void atmel_enable_ms(struct uart_port *port)
 {
-	UART_PUT_IER(port, ATMEL_US_RIIC | ATMEL_US_DSRIC
-			| ATMEL_US_DCDIC | ATMEL_US_CTSIC);
+	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
+	uint32_t ier = 0;
+
+	/*
+	 * Interrupt should not be enabled twice
+	 */
+	if (atmel_port->ms_irq_enabled)
+		return;
+
+	atmel_port->ms_irq_enabled = true;
+
+	if (atmel_port->gpio_irq[UART_GPIO_CTS] >= 0)
+		enable_irq(atmel_port->gpio_irq[UART_GPIO_CTS]);
+	else
+		ier |= ATMEL_US_CTSIC;
+
+	if (atmel_port->gpio_irq[UART_GPIO_DSR] >= 0)
+		enable_irq(atmel_port->gpio_irq[UART_GPIO_DSR]);
+	else
+		ier |= ATMEL_US_DSRIC;
+
+	if (atmel_port->gpio_irq[UART_GPIO_RI] >= 0)
+		enable_irq(atmel_port->gpio_irq[UART_GPIO_RI]);
+	else
+		ier |= ATMEL_US_RIIC;
+
+	if (atmel_port->gpio_irq[UART_GPIO_DCD] >= 0)
+		enable_irq(atmel_port->gpio_irq[UART_GPIO_DCD]);
+	else
+		ier |= ATMEL_US_DCDIC;
+
+	UART_PUT_IER(port, ier);
 }
 
 /*
@@ -1039,11 +1112,31 @@ atmel_handle_status(struct uart_port *port, unsigned int pending,
 static irqreturn_t atmel_interrupt(int irq, void *dev_id)
 {
 	struct uart_port *port = dev_id;
+	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
 	unsigned int status, pending, pass_counter = 0;
+	bool gpio_handled = false;
 
 	do {
-		status = UART_GET_CSR(port);
+		status = atmel_get_lines_status(port);
 		pending = status & UART_GET_IMR(port);
+		if (!gpio_handled) {
+			/*
+			 * Dealing with GPIO interrupt
+			 */
+			if (irq == atmel_port->gpio_irq[UART_GPIO_CTS])
+				pending |= ATMEL_US_CTSIC;
+
+			if (irq == atmel_port->gpio_irq[UART_GPIO_DSR])
+				pending |= ATMEL_US_DSRIC;
+
+			if (irq == atmel_port->gpio_irq[UART_GPIO_RI])
+				pending |= ATMEL_US_RIIC;
+
+			if (irq == atmel_port->gpio_irq[UART_GPIO_DCD])
+				pending |= ATMEL_US_DCDIC;
+
+			gpio_handled = true;
+		}
 		if (!pending)
 			break;
 
@@ -1523,6 +1616,45 @@ static void atmel_get_ip_name(struct uart_port *port)
 	}
 }
 
+static void atmel_free_gpio_irq(struct uart_port *port)
+{
+	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
+	enum mctrl_gpio_idx i;
+
+	for (i = 0; i < UART_GPIO_MAX; i++)
+		if (atmel_port->gpio_irq[i] >= 0)
+			free_irq(atmel_port->gpio_irq[i], port);
+}
+
+static int atmel_request_gpio_irq(struct uart_port *port)
+{
+	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
+	int *irq = atmel_port->gpio_irq;
+	enum mctrl_gpio_idx i;
+	int err = 0;
+
+	for (i = 0; (i < UART_GPIO_MAX) && !err; i++) {
+		if (irq[i] < 0)
+			continue;
+
+		irq_set_status_flags(irq[i], IRQ_NOAUTOEN);
+		err = request_irq(irq[i], atmel_interrupt, IRQ_TYPE_EDGE_BOTH,
+				  "atmel_serial", port);
+		if (err)
+			dev_err(port->dev, "atmel_startup - Can't get %d irq\n",
+				irq[i]);
+	}
+
+	/*
+	 * If something went wrong, rollback.
+	 */
+	while (err && (--i >= 0))
+		if (irq[i] >= 0)
+			free_irq(irq[i], port);
+
+	return err;
+}
+
 /*
  * Perform initialization and enable port for reception
  */
@@ -1539,6 +1671,7 @@ static int atmel_startup(struct uart_port *port)
 	 * handle an unexpected interrupt
 	 */
 	UART_PUT_IDR(port, -1);
+	atmel_port->ms_irq_enabled = false;
 
 	/*
 	 * Allocate the IRQ
@@ -1551,6 +1684,13 @@ static int atmel_startup(struct uart_port *port)
 	}
 
 	/*
+	 * Get the GPIO lines IRQ
+	 */
+	retval = atmel_request_gpio_irq(port);
+	if (retval)
+		goto free_irq;
+
+	/*
 	 * Initialize DMA (if necessary)
 	 */
 	atmel_init_property(atmel_port, pdev);
@@ -1568,7 +1708,7 @@ static int atmel_startup(struct uart_port *port)
 	}
 
 	/* Save current CSR for comparison in atmel_tasklet_func() */
-	atmel_port->irq_status_prev = UART_GET_CSR(port);
+	atmel_port->irq_status_prev = atmel_get_lines_status(port);
 	atmel_port->irq_status = atmel_port->irq_status_prev;
 
 	/*
@@ -1614,6 +1754,11 @@ static int atmel_startup(struct uart_port *port)
 	}
 
 	return 0;
+
+free_irq:
+	free_irq(port->irq, port);
+
+	return retval;
 }
 
 /*
@@ -1661,9 +1806,12 @@ static void atmel_shutdown(struct uart_port *port)
 	atmel_port->rx_ring.tail = 0;
 
 	/*
-	 * Free the interrupt
+	 * Free the interrupts
 	 */
 	free_irq(port->irq, port);
+	atmel_free_gpio_irq(port);
+
+	atmel_port->ms_irq_enabled = false;
 }
 
 /*
@@ -2324,6 +2472,26 @@ static int atmel_serial_resume(struct platform_device *pdev)
 #define atmel_serial_resume NULL
 #endif
 
+static int atmel_init_gpios(struct atmel_uart_port *p, struct device *dev)
+{
+	enum mctrl_gpio_idx i;
+	struct gpio_desc *gpiod;
+
+	p->gpios = mctrl_gpio_init(dev, 0);
+	if (IS_ERR_OR_NULL(p->gpios))
+		return -1;
+
+	for (i = 0; i < UART_GPIO_MAX; i++) {
+		gpiod = mctrl_gpio_to_gpiod(p->gpios, i);
+		if (gpiod && (gpiod_get_direction(gpiod) == GPIOF_DIR_IN))
+			p->gpio_irq[i] = gpiod_to_irq(gpiod);
+		else
+			p->gpio_irq[i] = -EINVAL;
+	}
+
+	return 0;
+}
+
 static int atmel_serial_probe(struct platform_device *pdev)
 {
 	struct atmel_uart_port *port;
@@ -2359,25 +2527,11 @@ static int atmel_serial_probe(struct platform_device *pdev)
 	port = &atmel_ports[ret];
 	port->backup_imr = 0;
 	port->uart.line = ret;
-	port->rts_gpio = -EINVAL; /* Invalid, zero could be valid */
-	if (pdata)
-		port->rts_gpio = pdata->rts_gpio;
-	else if (np)
-		port->rts_gpio = of_get_named_gpio(np, "rts-gpios", 0);
-
-	if (gpio_is_valid(port->rts_gpio)) {
-		ret = devm_gpio_request(&pdev->dev, port->rts_gpio, "RTS");
-		if (ret) {
-			dev_err(&pdev->dev, "error requesting RTS GPIO\n");
-			goto err;
-		}
-		/* Default to 1 as RTS is active low */
-		ret = gpio_direction_output(port->rts_gpio, 1);
-		if (ret) {
-			dev_err(&pdev->dev, "error setting up RTS GPIO\n");
-			goto err;
-		}
-	}
+
+	ret = atmel_init_gpios(port, &pdev->dev);
+	if (ret < 0)
+		dev_err(&pdev->dev, "%s",
+			"Failed to initialize GPIOs. The serial port may not work as expected");
 
 	ret = atmel_init_port(port, pdev);
 	if (ret)
diff --git a/drivers/tty/serial/clps711x.c b/drivers/tty/serial/clps711x.c
index 5e6fdb1ea73..14aaea0d413 100644
--- a/drivers/tty/serial/clps711x.c
+++ b/drivers/tty/serial/clps711x.c
@@ -368,16 +368,12 @@ static const struct uart_ops uart_clps711x_ops = {
 static void uart_clps711x_console_putchar(struct uart_port *port, int ch)
 {
 	struct clps711x_port *s = dev_get_drvdata(port->dev);
+	u32 sysflg = 0;
 
 	/* Wait for FIFO is not full */
-	while (1) {
-		u32 sysflg = 0;
-
+	do {
 		regmap_read(s->syscon, SYSFLG_OFFSET, &sysflg);
-		if (!(sysflg & SYSFLG_UTXFF))
-			break;
-		cond_resched();
-	}
+	} while (sysflg & SYSFLG_UTXFF);
 
 	writew(ch, port->membase + UARTDR_OFFSET);
 }
@@ -387,18 +383,14 @@ static void uart_clps711x_console_write(struct console *co, const char *c,
 {
 	struct uart_port *port = clps711x_uart.state[co->index].uart_port;
 	struct clps711x_port *s = dev_get_drvdata(port->dev);
+	u32 sysflg = 0;
 
 	uart_console_write(port, c, n, uart_clps711x_console_putchar);
 
 	/* Wait for transmitter to become empty */
-	while (1) {
-		u32 sysflg = 0;
-
+	do {
 		regmap_read(s->syscon, SYSFLG_OFFSET, &sysflg);
-		if (!(sysflg & SYSFLG_UBUSY))
-			break;
-		cond_resched();
-	}
+	} while (sysflg & SYSFLG_UBUSY);
 }
 
 static int uart_clps711x_console_setup(struct console *co, char *options)
diff --git a/drivers/tty/serial/cpm_uart/cpm_uart_core.c b/drivers/tty/serial/cpm_uart/cpm_uart_core.c
index 7d76214612c..aa60e6d13ec 100644
--- a/drivers/tty/serial/cpm_uart/cpm_uart_core.c
+++ b/drivers/tty/serial/cpm_uart/cpm_uart_core.c
@@ -971,7 +971,7 @@ static void cpm_uart_config_port(struct uart_port *port, int flags)
  * Note that this is called with interrupts already disabled
  */
 static void cpm_uart_early_write(struct uart_cpm_port *pinfo,
-		const char *string, u_int count)
+		const char *string, u_int count, bool handle_linefeed)
 {
 	unsigned int i;
 	cbd_t __iomem *bdp, *bdbase;
@@ -1013,7 +1013,7 @@ static void cpm_uart_early_write(struct uart_cpm_port *pinfo,
 			bdp++;
 
 		/* if a LF, also do CR... */
-		if (*string == 10) {
+		if (handle_linefeed && *string == 10) {
 			while ((in_be16(&bdp->cbd_sc) & BD_SC_READY) != 0)
 				;
 
@@ -1111,7 +1111,7 @@ static void cpm_put_poll_char(struct uart_port *port,
 	static char ch[2];
 
 	ch[0] = (char)c;
-	cpm_uart_early_write(pinfo, ch, 1);
+	cpm_uart_early_write(pinfo, ch, 1, false);
 }
 #endif /* CONFIG_CONSOLE_POLL */
 
@@ -1275,7 +1275,7 @@ static void cpm_uart_console_write(struct console *co, const char *s,
 		spin_lock_irqsave(&pinfo->port.lock, flags);
 	}
 
-	cpm_uart_early_write(pinfo, s, count);
+	cpm_uart_early_write(pinfo, s, count, true);
 
 	if (unlikely(nolock)) {
 		local_irq_restore(flags);
diff --git a/drivers/tty/serial/earlycon-arm-semihost.c b/drivers/tty/serial/earlycon-arm-semihost.c
new file mode 100644
index 00000000000..383db10fbb4
--- /dev/null
+++ b/drivers/tty/serial/earlycon-arm-semihost.c
@@ -0,0 +1,61 @@
+/*
+ * Copyright (C) 2012 ARM Ltd.
+ * Author: Marc Zyngier <marc.zyngier@arm.com>
+ *
+ * Adapted for ARM and earlycon:
+ * Copyright (C) 2014 Linaro Ltd.
+ * Author: Rob Herring <robh@kernel.org>
+ *
+ * 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/kernel.h>
+#include <linux/console.h>
+#include <linux/init.h>
+#include <linux/serial_core.h>
+
+#ifdef CONFIG_THUMB2_KERNEL
+#define SEMIHOST_SWI	"0xab"
+#else
+#define SEMIHOST_SWI	"0x123456"
+#endif
+
+/*
+ * Semihosting-based debug console
+ */
+static void smh_putc(struct uart_port *port, int c)
+{
+#ifdef CONFIG_ARM64
+	asm volatile("mov  x1, %0\n"
+		     "mov  x0, #3\n"
+		     "hlt  0xf000\n"
+		     : : "r" (&c) : "x0", "x1", "memory");
+#else
+	asm volatile("mov  r1, %0\n"
+		     "mov  r0, #3\n"
+		     "svc  " SEMIHOST_SWI "\n"
+		     : : "r" (&c) : "r0", "r1", "memory");
+#endif
+}
+
+static void smh_write(struct console *con, const char *s, unsigned n)
+{
+	struct earlycon_device *dev = con->data;
+	uart_console_write(&dev->port, s, n, smh_putc);
+}
+
+int __init early_smh_setup(struct earlycon_device *device, const char *opt)
+{
+	device->con->write = smh_write;
+	return 0;
+}
+EARLYCON_DECLARE(smh, early_smh_setup);
diff --git a/drivers/tty/serial/earlycon.c b/drivers/tty/serial/earlycon.c
new file mode 100644
index 00000000000..c92e83088ad
--- /dev/null
+++ b/drivers/tty/serial/earlycon.c
@@ -0,0 +1,144 @@
+/*
+ * Copyright (C) 2014 Linaro Ltd.
+ * Author: Rob Herring <robh@kernel.org>
+ *
+ * Based on 8250 earlycon:
+ * (c) Copyright 2004 Hewlett-Packard Development Company, L.P.
+ *	Bjorn Helgaas <bjorn.helgaas@hp.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/console.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/serial_core.h>
+
+#ifdef CONFIG_FIX_EARLYCON_MEM
+#include <asm/fixmap.h>
+#endif
+
+#include <asm/serial.h>
+
+static struct console early_con = {
+	.name =		"earlycon",
+	.flags =	CON_PRINTBUFFER | CON_BOOT,
+	.index =	-1,
+};
+
+static struct earlycon_device early_console_dev = {
+	.con = &early_con,
+};
+
+static void __iomem * __init earlycon_map(unsigned long paddr, size_t size)
+{
+	void __iomem *base;
+#ifdef CONFIG_FIX_EARLYCON_MEM
+	set_fixmap_io(FIX_EARLYCON_MEM_BASE, paddr & PAGE_MASK);
+	base = (void __iomem *)__fix_to_virt(FIX_EARLYCON_MEM_BASE);
+	base += paddr & ~PAGE_MASK;
+#else
+	base = ioremap(paddr, size);
+#endif
+	if (!base)
+		pr_err("%s: Couldn't map 0x%llx\n", __func__,
+		       (unsigned long long)paddr);
+
+	return base;
+}
+
+static int __init parse_options(struct earlycon_device *device,
+				char *options)
+{
+	struct uart_port *port = &device->port;
+	int mmio, mmio32, length;
+	unsigned long addr;
+
+	if (!options)
+		return -ENODEV;
+
+	mmio = !strncmp(options, "mmio,", 5);
+	mmio32 = !strncmp(options, "mmio32,", 7);
+	if (mmio || mmio32) {
+		port->iotype = (mmio ? UPIO_MEM : UPIO_MEM32);
+		options += mmio ? 5 : 7;
+		addr = simple_strtoul(options, NULL, 0);
+		port->mapbase = addr;
+		if (mmio32)
+			port->regshift = 2;
+	} else if (!strncmp(options, "io,", 3)) {
+		port->iotype = UPIO_PORT;
+		options += 3;
+		addr = simple_strtoul(options, NULL, 0);
+		port->iobase = addr;
+		mmio = 0;
+	} else if (!strncmp(options, "0x", 2)) {
+		port->iotype = UPIO_MEM;
+		addr = simple_strtoul(options, NULL, 0);
+		port->mapbase = addr;
+	} else {
+		return -EINVAL;
+	}
+
+	port->uartclk = BASE_BAUD * 16;
+
+	options = strchr(options, ',');
+	if (options) {
+		options++;
+		device->baud = simple_strtoul(options, NULL, 0);
+		length = min(strcspn(options, " ") + 1,
+			     (size_t)(sizeof(device->options)));
+		strlcpy(device->options, options, length);
+	}
+
+	if (mmio || mmio32)
+		pr_info("Early serial console at MMIO%s 0x%llx (options '%s')\n",
+			mmio32 ? "32" : "",
+			(unsigned long long)port->mapbase,
+			device->options);
+	else
+		pr_info("Early serial console at I/O port 0x%lx (options '%s')\n",
+			port->iobase,
+			device->options);
+
+	return 0;
+}
+
+int __init setup_earlycon(char *buf, const char *match,
+			  int (*setup)(struct earlycon_device *, const char *))
+{
+	int err;
+	size_t len;
+	struct uart_port *port = &early_console_dev.port;
+
+	if (!buf || !match || !setup)
+		return 0;
+
+	len = strlen(match);
+	if (strncmp(buf, match, len))
+		return 0;
+	if (buf[len] && (buf[len] != ','))
+		return 0;
+
+	buf += len + 1;
+
+	err = parse_options(&early_console_dev, buf);
+	/* On parsing error, pass the options buf to the setup function */
+	if (!err)
+		buf = NULL;
+
+	if (port->mapbase)
+		port->membase = earlycon_map(port->mapbase, 64);
+
+	early_console_dev.con->data = &early_console_dev;
+	err = setup(&early_console_dev, buf);
+	if (err < 0)
+		return err;
+	if (!early_console_dev.con->write)
+		return -ENODEV;
+
+	register_console(early_console_dev.con);
+	return 0;
+}
diff --git a/drivers/tty/serial/efm32-uart.c b/drivers/tty/serial/efm32-uart.c
index 028582e924a..b373f6416e8 100644
--- a/drivers/tty/serial/efm32-uart.c
+++ b/drivers/tty/serial/efm32-uart.c
@@ -798,6 +798,9 @@ static int efm32_uart_remove(struct platform_device *pdev)
 
 static const struct of_device_id efm32_uart_dt_ids[] = {
 	{
+		.compatible = "energymicro,efm32-uart",
+	}, {
+		/* doesn't follow the "vendor,device" scheme, don't use */
 		.compatible = "efm32,uart",
 	}, {
 		/* sentinel */
@@ -839,6 +842,7 @@ static void __exit efm32_uart_exit(void)
 	platform_driver_unregister(&efm32_uart_driver);
 	uart_unregister_driver(&efm32_uart_reg);
 }
+module_exit(efm32_uart_exit);
 
 MODULE_AUTHOR("Uwe Kleine-Koenig <u.kleine-koenig@pengutronix.de>");
 MODULE_DESCRIPTION("EFM32 UART/USART driver");
diff --git a/drivers/tty/serial/imx.c b/drivers/tty/serial/imx.c
index 3b6c1a2e25d..e2f93874989 100644
--- a/drivers/tty/serial/imx.c
+++ b/drivers/tty/serial/imx.c
@@ -116,7 +116,7 @@
 #define UCR3_DSR	(1<<10) /* Data set ready */
 #define UCR3_DCD	(1<<9)	/* Data carrier detect */
 #define UCR3_RI		(1<<8)	/* Ring indicator */
-#define UCR3_TIMEOUTEN	(1<<7)	/* Timeout interrupt enable */
+#define UCR3_ADNIMP	(1<<7)	/* Autobaud Detection Not Improved */
 #define UCR3_RXDSEN	(1<<6)	/* Receive status interrupt enable */
 #define UCR3_AIRINTEN	(1<<5)	/* Async IR wake interrupt enable */
 #define UCR3_AWAKEN	(1<<4)	/* Async wake interrupt enable */
@@ -444,6 +444,10 @@ static void imx_stop_rx(struct uart_port *port)
 
 	temp = readl(sport->port.membase + UCR2);
 	writel(temp & ~UCR2_RXEN, sport->port.membase + UCR2);
+
+	/* disable the `Receiver Ready Interrrupt` */
+	temp = readl(sport->port.membase + UCR1);
+	writel(temp & ~UCR1_RRDYEN, sport->port.membase + UCR1);
 }
 
 /*
@@ -1070,7 +1074,7 @@ static void imx_disable_dma(struct imx_port *sport)
 static int imx_startup(struct uart_port *port)
 {
 	struct imx_port *sport = (struct imx_port *)port;
-	int retval;
+	int retval, i;
 	unsigned long flags, temp;
 
 	retval = clk_prepare_enable(sport->clk_per);
@@ -1098,17 +1102,15 @@ static int imx_startup(struct uart_port *port)
 
 	writel(temp & ~UCR4_DREN, sport->port.membase + UCR4);
 
-	if (USE_IRDA(sport)) {
-		/* reset fifo's and state machines */
-		int i = 100;
-		temp = readl(sport->port.membase + UCR2);
-		temp &= ~UCR2_SRST;
-		writel(temp, sport->port.membase + UCR2);
-		while (!(readl(sport->port.membase + UCR2) & UCR2_SRST) &&
-		    (--i > 0)) {
-			udelay(1);
-		}
-	}
+	/* Reset fifo's and state machines */
+	i = 100;
+
+	temp = readl(sport->port.membase + UCR2);
+	temp &= ~UCR2_SRST;
+	writel(temp, sport->port.membase + UCR2);
+
+	while (!(readl(sport->port.membase + UCR2) & UCR2_SRST) && (--i > 0))
+		udelay(1);
 
 	/*
 	 * Allocate the IRQ(s) i.MX1 has three interrupts whereas later
@@ -1163,18 +1165,9 @@ static int imx_startup(struct uart_port *port)
 		temp |= UCR2_IRTS;
 	writel(temp, sport->port.membase + UCR2);
 
-	if (USE_IRDA(sport)) {
-		/* clear RX-FIFO */
-		int i = 64;
-		while ((--i > 0) &&
-			(readl(sport->port.membase + URXD0) & URXD_CHARRDY)) {
-			barrier();
-		}
-	}
-
 	if (!is_imx1_uart(sport)) {
 		temp = readl(sport->port.membase + UCR3);
-		temp |= IMX21_UCR3_RXDMUXSEL;
+		temp |= IMX21_UCR3_RXDMUXSEL | UCR3_ADNIMP;
 		writel(temp, sport->port.membase + UCR3);
 	}
 
diff --git a/drivers/tty/serial/kgdb_nmi.c b/drivers/tty/serial/kgdb_nmi.c
index 5f673b7ca50..cfadf2971b1 100644
--- a/drivers/tty/serial/kgdb_nmi.c
+++ b/drivers/tty/serial/kgdb_nmi.c
@@ -44,13 +44,22 @@ MODULE_PARM_DESC(magic, "magic sequence to enter NMI debugger (default $3#33)");
 
 static bool kgdb_nmi_tty_enabled;
 
+static int kgdb_nmi_console_setup(struct console *co, char *options)
+{
+	/* The NMI console uses the dbg_io_ops to issue console messages. To
+	 * avoid duplicate messages during kdb sessions we must inform kdb's
+	 * I/O utilities that messages sent to the console will automatically
+	 * be displayed on the dbg_io.
+	 */
+	dbg_io_ops->is_console = true;
+
+	return 0;
+}
+
 static void kgdb_nmi_console_write(struct console *co, const char *s, uint c)
 {
 	int i;
 
-	if (!kgdb_nmi_tty_enabled || atomic_read(&kgdb_active) >= 0)
-		return;
-
 	for (i = 0; i < c; i++)
 		dbg_io_ops->write_char(s[i]);
 }
@@ -65,6 +74,7 @@ static struct tty_driver *kgdb_nmi_console_device(struct console *co, int *idx)
 
 static struct console kgdb_nmi_console = {
 	.name	= "ttyNMI",
+	.setup  = kgdb_nmi_console_setup,
 	.write	= kgdb_nmi_console_write,
 	.device	= kgdb_nmi_console_device,
 	.flags	= CON_PRINTBUFFER | CON_ANYTIME | CON_ENABLED,
@@ -80,29 +90,10 @@ static struct console kgdb_nmi_console = {
 
 struct kgdb_nmi_tty_priv {
 	struct tty_port port;
-	struct tasklet_struct tlet;
+	struct timer_list timer;
 	STRUCT_KFIFO(char, KGDB_NMI_FIFO_SIZE) fifo;
 };
 
-static struct kgdb_nmi_tty_priv *kgdb_nmi_port_to_priv(struct tty_port *port)
-{
-	return container_of(port, struct kgdb_nmi_tty_priv, port);
-}
-
-/*
- * Our debugging console is polled in a tasklet, so we'll check for input
- * every tick. In HZ-less mode, we should program the next tick.  We have
- * to use the lowlevel stuff as no locks should be grabbed.
- */
-#ifdef CONFIG_HIGH_RES_TIMERS
-static void kgdb_tty_poke(void)
-{
-	tick_program_event(ktime_get(), 0);
-}
-#else
-static inline void kgdb_tty_poke(void) {}
-#endif
-
 static struct tty_port *kgdb_nmi_port;
 
 static void kgdb_tty_recv(int ch)
@@ -113,14 +104,13 @@ static void kgdb_tty_recv(int ch)
 	if (!kgdb_nmi_port || ch < 0)
 		return;
 	/*
-	 * Can't use port->tty->driver_data as tty might be not there. Tasklet
+	 * Can't use port->tty->driver_data as tty might be not there. Timer
 	 * will check for tty and will get the ref, but here we don't have to
 	 * do that, and actually, we can't: we're in NMI context, no locks are
 	 * possible.
 	 */
-	priv = kgdb_nmi_port_to_priv(kgdb_nmi_port);
+	priv = container_of(kgdb_nmi_port, struct kgdb_nmi_tty_priv, port);
 	kfifo_in(&priv->fifo, &c, 1);
-	kgdb_tty_poke();
 }
 
 static int kgdb_nmi_poll_one_knock(void)
@@ -204,7 +194,8 @@ static void kgdb_nmi_tty_receiver(unsigned long data)
 	struct kgdb_nmi_tty_priv *priv = (void *)data;
 	char ch;
 
-	tasklet_schedule(&priv->tlet);
+	priv->timer.expires = jiffies + (HZ/100);
+	add_timer(&priv->timer);
 
 	if (likely(!kgdb_nmi_tty_enabled || !kfifo_len(&priv->fifo)))
 		return;
@@ -216,18 +207,22 @@ static void kgdb_nmi_tty_receiver(unsigned long data)
 
 static int kgdb_nmi_tty_activate(struct tty_port *port, struct tty_struct *tty)
 {
-	struct kgdb_nmi_tty_priv *priv = tty->driver_data;
+	struct kgdb_nmi_tty_priv *priv =
+	    container_of(port, struct kgdb_nmi_tty_priv, port);
 
 	kgdb_nmi_port = port;
-	tasklet_schedule(&priv->tlet);
+	priv->timer.expires = jiffies + (HZ/100);
+	add_timer(&priv->timer);
+
 	return 0;
 }
 
 static void kgdb_nmi_tty_shutdown(struct tty_port *port)
 {
-	struct kgdb_nmi_tty_priv *priv = port->tty->driver_data;
+	struct kgdb_nmi_tty_priv *priv =
+	    container_of(port, struct kgdb_nmi_tty_priv, port);
 
-	tasklet_kill(&priv->tlet);
+	del_timer(&priv->timer);
 	kgdb_nmi_port = NULL;
 }
 
@@ -246,7 +241,7 @@ static int kgdb_nmi_tty_install(struct tty_driver *drv, struct tty_struct *tty)
 		return -ENOMEM;
 
 	INIT_KFIFO(priv->fifo);
-	tasklet_init(&priv->tlet, kgdb_nmi_tty_receiver, (unsigned long)priv);
+	setup_timer(&priv->timer, kgdb_nmi_tty_receiver, (unsigned long)priv);
 	tty_port_init(&priv->port);
 	priv->port.ops = &kgdb_nmi_tty_port_ops;
 	tty->driver_data = priv;
diff --git a/drivers/tty/serial/men_z135_uart.c b/drivers/tty/serial/men_z135_uart.c
new file mode 100644
index 00000000000..c9d18548783
--- /dev/null
+++ b/drivers/tty/serial/men_z135_uart.c
@@ -0,0 +1,867 @@
+/*
+ * MEN 16z135 High Speed UART
+ *
+ * Copyright (C) 2014 MEN Mikroelektronik GmbH (www.men.de)
+ * Author: Johannes Thumshirn <johannes.thumshirn@men.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; version 2 of the License.
+ */
+#define pr_fmt(fmt) KBUILD_MODNAME ":" fmt
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/serial_core.h>
+#include <linux/ioport.h>
+#include <linux/io.h>
+#include <linux/tty_flip.h>
+#include <linux/bitops.h>
+#include <linux/mcb.h>
+
+#define MEN_Z135_MAX_PORTS		12
+#define MEN_Z135_BASECLK		29491200
+#define MEN_Z135_FIFO_SIZE		1024
+#define MEN_Z135_NUM_MSI_VECTORS	2
+#define MEN_Z135_FIFO_WATERMARK		1020
+
+#define MEN_Z135_STAT_REG		0x0
+#define MEN_Z135_RX_RAM			0x4
+#define MEN_Z135_TX_RAM			0x400
+#define MEN_Z135_RX_CTRL		0x800
+#define MEN_Z135_TX_CTRL		0x804
+#define MEN_Z135_CONF_REG		0x808
+#define MEN_Z135_UART_FREQ		0x80c
+#define MEN_Z135_BAUD_REG		0x810
+#define MENZ135_TIMEOUT			0x814
+
+#define MEN_Z135_MEM_SIZE		0x818
+
+#define IS_IRQ(x) ((x) & 1)
+#define IRQ_ID(x) (((x) >> 1) & 7)
+
+#define MEN_Z135_IER_RXCIEN BIT(0)		/* RX Space IRQ */
+#define MEN_Z135_IER_TXCIEN BIT(1)		/* TX Space IRQ */
+#define MEN_Z135_IER_RLSIEN BIT(2)		/* Receiver Line Status IRQ */
+#define MEN_Z135_IER_MSIEN  BIT(3)		/* Modem Status IRQ */
+#define MEN_Z135_ALL_IRQS (MEN_Z135_IER_RXCIEN		\
+				| MEN_Z135_IER_RLSIEN	\
+				| MEN_Z135_IER_MSIEN	\
+				| MEN_Z135_IER_TXCIEN)
+
+#define MEN_Z135_MCR_DTR	BIT(24)
+#define MEN_Z135_MCR_RTS	BIT(25)
+#define MEN_Z135_MCR_OUT1	BIT(26)
+#define MEN_Z135_MCR_OUT2	BIT(27)
+#define MEN_Z135_MCR_LOOP	BIT(28)
+#define MEN_Z135_MCR_RCFC	BIT(29)
+
+#define MEN_Z135_MSR_DCTS	BIT(0)
+#define MEN_Z135_MSR_DDSR	BIT(1)
+#define MEN_Z135_MSR_DRI	BIT(2)
+#define MEN_Z135_MSR_DDCD	BIT(3)
+#define MEN_Z135_MSR_CTS	BIT(4)
+#define MEN_Z135_MSR_DSR	BIT(5)
+#define MEN_Z135_MSR_RI		BIT(6)
+#define MEN_Z135_MSR_DCD	BIT(7)
+
+#define MEN_Z135_LCR_SHIFT 8	/* LCR shift mask */
+
+#define MEN_Z135_WL5 0		/* CS5 */
+#define MEN_Z135_WL6 1		/* CS6 */
+#define MEN_Z135_WL7 2		/* CS7 */
+#define MEN_Z135_WL8 3		/* CS8 */
+
+#define MEN_Z135_STB_SHIFT 2	/* Stopbits */
+#define MEN_Z135_NSTB1 0
+#define MEN_Z135_NSTB2 1
+
+#define MEN_Z135_PEN_SHIFT 3	/* Parity enable */
+#define MEN_Z135_PAR_DIS 0
+#define MEN_Z135_PAR_ENA 1
+
+#define MEN_Z135_PTY_SHIFT 4	/* Parity type */
+#define MEN_Z135_PTY_ODD 0
+#define MEN_Z135_PTY_EVN 1
+
+#define MEN_Z135_LSR_DR BIT(0)
+#define MEN_Z135_LSR_OE BIT(1)
+#define MEN_Z135_LSR_PE BIT(2)
+#define MEN_Z135_LSR_FE BIT(3)
+#define MEN_Z135_LSR_BI BIT(4)
+#define MEN_Z135_LSR_THEP BIT(5)
+#define MEN_Z135_LSR_TEXP BIT(6)
+#define MEN_Z135_LSR_RXFIFOERR BIT(7)
+
+#define MEN_Z135_IRQ_ID_MST 0
+#define MEN_Z135_IRQ_ID_TSA 1
+#define MEN_Z135_IRQ_ID_RDA 2
+#define MEN_Z135_IRQ_ID_RLS 3
+#define MEN_Z135_IRQ_ID_CTI 6
+
+#define LCR(x) (((x) >> MEN_Z135_LCR_SHIFT) & 0xff)
+
+#define BYTES_TO_ALIGN(x) ((x) & 0x3)
+
+static int line;
+
+static int txlvl = 5;
+module_param(txlvl, int, S_IRUGO);
+MODULE_PARM_DESC(txlvl, "TX IRQ trigger level 0-7, default 5 (128 byte)");
+
+static int rxlvl = 6;
+module_param(rxlvl, int, S_IRUGO);
+MODULE_PARM_DESC(rxlvl, "RX IRQ trigger level 0-7, default 6 (256 byte)");
+
+static int align;
+module_param(align, int, S_IRUGO);
+MODULE_PARM_DESC(align, "Keep hardware FIFO write pointer aligned, default 0");
+
+struct men_z135_port {
+	struct uart_port port;
+	struct mcb_device *mdev;
+	unsigned char *rxbuf;
+	u32 stat_reg;
+	spinlock_t lock;
+};
+#define to_men_z135(port) container_of((port), struct men_z135_port, port)
+
+/**
+ * men_z135_reg_set() - Set value in register
+ * @uart: The UART port
+ * @addr: Register address
+ * @val: value to set
+ */
+static inline void men_z135_reg_set(struct men_z135_port *uart,
+				u32 addr, u32 val)
+{
+	struct uart_port *port = &uart->port;
+	unsigned long flags;
+	u32 reg;
+
+	spin_lock_irqsave(&uart->lock, flags);
+
+	reg = ioread32(port->membase + addr);
+	reg |= val;
+	iowrite32(reg, port->membase + addr);
+
+	spin_unlock_irqrestore(&uart->lock, flags);
+}
+
+/**
+ * men_z135_reg_clr() - Unset value in register
+ * @uart: The UART port
+ * @addr: Register address
+ * @val: value to clear
+ */
+static inline void men_z135_reg_clr(struct men_z135_port *uart,
+				u32 addr, u32 val)
+{
+	struct uart_port *port = &uart->port;
+	unsigned long flags;
+	u32 reg;
+
+	spin_lock_irqsave(&uart->lock, flags);
+
+	reg = ioread32(port->membase + addr);
+	reg &= ~val;
+	iowrite32(reg, port->membase + addr);
+
+	spin_unlock_irqrestore(&uart->lock, flags);
+}
+
+/**
+ * men_z135_handle_modem_status() - Handle change of modem status
+ * @port: The UART port
+ *
+ * Handle change of modem status register. This is done by reading the "delta"
+ * versions of DCD (Data Carrier Detect) and CTS (Clear To Send).
+ */
+static void men_z135_handle_modem_status(struct men_z135_port *uart)
+{
+	if (uart->stat_reg & MEN_Z135_MSR_DDCD)
+		uart_handle_dcd_change(&uart->port,
+				uart->stat_reg & ~MEN_Z135_MSR_DCD);
+	if (uart->stat_reg & MEN_Z135_MSR_DCTS)
+		uart_handle_cts_change(&uart->port,
+				uart->stat_reg & ~MEN_Z135_MSR_CTS);
+}
+
+static void men_z135_handle_lsr(struct men_z135_port *uart)
+{
+	struct uart_port *port = &uart->port;
+	u8 lsr;
+
+	lsr = (uart->stat_reg >> 16) & 0xff;
+
+	if (lsr & MEN_Z135_LSR_OE)
+		port->icount.overrun++;
+	if (lsr & MEN_Z135_LSR_PE)
+		port->icount.parity++;
+	if (lsr & MEN_Z135_LSR_FE)
+		port->icount.frame++;
+	if (lsr & MEN_Z135_LSR_BI) {
+		port->icount.brk++;
+		uart_handle_break(port);
+	}
+}
+
+/**
+ * get_rx_fifo_content() - Get the number of bytes in RX FIFO
+ * @uart: The UART port
+ *
+ * Read RXC register from hardware and return current FIFO fill size.
+ */
+static u16 get_rx_fifo_content(struct men_z135_port *uart)
+{
+	struct uart_port *port = &uart->port;
+	u32 stat_reg;
+	u16 rxc;
+	u8 rxc_lo;
+	u8 rxc_hi;
+
+	stat_reg = ioread32(port->membase + MEN_Z135_STAT_REG);
+	rxc_lo = stat_reg >> 24;
+	rxc_hi = (stat_reg & 0xC0) >> 6;
+
+	rxc = rxc_lo | (rxc_hi << 8);
+
+	return rxc;
+}
+
+/**
+ * men_z135_handle_rx() - RX tasklet routine
+ * @arg: Pointer to struct men_z135_port
+ *
+ * Copy from RX FIFO and acknowledge number of bytes copied.
+ */
+static void men_z135_handle_rx(struct men_z135_port *uart)
+{
+	struct uart_port *port = &uart->port;
+	struct tty_port *tport = &port->state->port;
+	int copied;
+	u16 size;
+	int room;
+
+	size = get_rx_fifo_content(uart);
+
+	if (size == 0)
+		return;
+
+	/* Avoid accidently accessing TX FIFO instead of RX FIFO. Last
+	 * longword in RX FIFO cannot be read.(0x004-0x3FF)
+	 */
+	if (size > MEN_Z135_FIFO_WATERMARK)
+		size = MEN_Z135_FIFO_WATERMARK;
+
+	room = tty_buffer_request_room(tport, size);
+	if (room != size)
+		dev_warn(&uart->mdev->dev,
+			"Not enough room in flip buffer, truncating to %d\n",
+			room);
+
+	if (room == 0)
+		return;
+
+	memcpy_fromio(uart->rxbuf, port->membase + MEN_Z135_RX_RAM, room);
+	/* Be sure to first copy all data and then acknowledge it */
+	mb();
+	iowrite32(room, port->membase +  MEN_Z135_RX_CTRL);
+
+	copied = tty_insert_flip_string(tport, uart->rxbuf, room);
+	if (copied != room)
+		dev_warn(&uart->mdev->dev,
+			"Only copied %d instead of %d bytes\n",
+			copied, room);
+
+	port->icount.rx += copied;
+
+	tty_flip_buffer_push(tport);
+
+}
+
+/**
+ * men_z135_handle_tx() - TX tasklet routine
+ * @arg: Pointer to struct men_z135_port
+ *
+ */
+static void men_z135_handle_tx(struct men_z135_port *uart)
+{
+	struct uart_port *port = &uart->port;
+	struct circ_buf *xmit = &port->state->xmit;
+	u32 txc;
+	u32 wptr;
+	int qlen;
+	int n;
+	int txfree;
+	int head;
+	int tail;
+	int s;
+
+	if (uart_circ_empty(xmit))
+		goto out;
+
+	if (uart_tx_stopped(port))
+		goto out;
+
+	if (port->x_char)
+		goto out;
+
+	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
+		uart_write_wakeup(port);
+
+	/* calculate bytes to copy */
+	qlen = uart_circ_chars_pending(xmit);
+	if (qlen <= 0)
+		goto out;
+
+	wptr = ioread32(port->membase + MEN_Z135_TX_CTRL);
+	txc = (wptr >> 16) & 0x3ff;
+	wptr &= 0x3ff;
+
+	if (txc > MEN_Z135_FIFO_WATERMARK)
+		txc = MEN_Z135_FIFO_WATERMARK;
+
+	txfree = MEN_Z135_FIFO_WATERMARK - txc;
+	if (txfree <= 0) {
+		pr_err("Not enough room in TX FIFO have %d, need %d\n",
+			txfree, qlen);
+		goto irq_en;
+	}
+
+	/* if we're not aligned, it's better to copy only 1 or 2 bytes and
+	 * then the rest.
+	 */
+	if (align && qlen >= 3 && BYTES_TO_ALIGN(wptr))
+		n = 4 - BYTES_TO_ALIGN(wptr);
+	else if (qlen > txfree)
+		n = txfree;
+	else
+		n = qlen;
+
+	if (n <= 0)
+		goto irq_en;
+
+	head = xmit->head & (UART_XMIT_SIZE - 1);
+	tail = xmit->tail & (UART_XMIT_SIZE - 1);
+
+	s = ((head >= tail) ? head : UART_XMIT_SIZE) - tail;
+	n = min(n, s);
+
+	memcpy_toio(port->membase + MEN_Z135_TX_RAM, &xmit->buf[xmit->tail], n);
+	xmit->tail = (xmit->tail + n) & (UART_XMIT_SIZE - 1);
+	mmiowb();
+
+	iowrite32(n & 0x3ff, port->membase + MEN_Z135_TX_CTRL);
+
+	port->icount.tx += n;
+
+irq_en:
+	if (!uart_circ_empty(xmit))
+		men_z135_reg_set(uart, MEN_Z135_CONF_REG, MEN_Z135_IER_TXCIEN);
+	else
+		men_z135_reg_clr(uart, MEN_Z135_CONF_REG, MEN_Z135_IER_TXCIEN);
+
+out:
+	return;
+
+}
+
+/**
+ * men_z135_intr() - Handle legacy IRQs
+ * @irq: The IRQ number
+ * @data: Pointer to UART port
+ *
+ * Check IIR register to see which tasklet to start.
+ */
+static irqreturn_t men_z135_intr(int irq, void *data)
+{
+	struct men_z135_port *uart = (struct men_z135_port *)data;
+	struct uart_port *port = &uart->port;
+	int irq_id;
+
+	uart->stat_reg = ioread32(port->membase + MEN_Z135_STAT_REG);
+	/* IRQ pending is low active */
+	if (IS_IRQ(uart->stat_reg))
+		return IRQ_NONE;
+
+	irq_id = IRQ_ID(uart->stat_reg);
+	switch (irq_id) {
+	case MEN_Z135_IRQ_ID_MST:
+		men_z135_handle_modem_status(uart);
+		break;
+	case MEN_Z135_IRQ_ID_TSA:
+		men_z135_handle_tx(uart);
+		break;
+	case MEN_Z135_IRQ_ID_CTI:
+		dev_dbg(&uart->mdev->dev, "Character Timeout Indication\n");
+		/* Fallthrough */
+	case MEN_Z135_IRQ_ID_RDA:
+		/* Reading data clears RX IRQ */
+		men_z135_handle_rx(uart);
+		break;
+	case MEN_Z135_IRQ_ID_RLS:
+		men_z135_handle_lsr(uart);
+		break;
+	default:
+		dev_warn(&uart->mdev->dev, "Unknown IRQ id %d\n", irq_id);
+		return IRQ_NONE;
+	}
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * men_z135_request_irq() - Request IRQ for 16z135 core
+ * @uart: z135 private uart port structure
+ *
+ * Request an IRQ for 16z135 to use. First try using MSI, if it fails
+ * fall back to using legacy interrupts.
+ */
+static int men_z135_request_irq(struct men_z135_port *uart)
+{
+	struct device *dev = &uart->mdev->dev;
+	struct uart_port *port = &uart->port;
+	int err = 0;
+
+	err = request_irq(port->irq, men_z135_intr, IRQF_SHARED,
+			"men_z135_intr", uart);
+	if (err)
+		dev_err(dev, "Error %d getting interrupt\n", err);
+
+	return err;
+}
+
+/**
+ * men_z135_tx_empty() - Handle tx_empty call
+ * @port: The UART port
+ *
+ * This function tests whether the TX FIFO and shifter for the port
+ * described by @port is empty.
+ */
+static unsigned int men_z135_tx_empty(struct uart_port *port)
+{
+	u32 wptr;
+	u16 txc;
+
+	wptr = ioread32(port->membase + MEN_Z135_TX_CTRL);
+	txc = (wptr >> 16) & 0x3ff;
+
+	if (txc == 0)
+		return TIOCSER_TEMT;
+	else
+		return 0;
+}
+
+/**
+ * men_z135_set_mctrl() - Set modem control lines
+ * @port: The UART port
+ * @mctrl: The modem control lines
+ *
+ * This function sets the modem control lines for a port described by @port
+ * to the state described by @mctrl
+ */
+static void men_z135_set_mctrl(struct uart_port *port, unsigned int mctrl)
+{
+	struct men_z135_port *uart = to_men_z135(port);
+	u32 conf_reg = 0;
+
+	if (mctrl & TIOCM_RTS)
+		conf_reg |= MEN_Z135_MCR_RTS;
+	if (mctrl & TIOCM_DTR)
+		conf_reg |= MEN_Z135_MCR_DTR;
+	if (mctrl & TIOCM_OUT1)
+		conf_reg |= MEN_Z135_MCR_OUT1;
+	if (mctrl & TIOCM_OUT2)
+		conf_reg |= MEN_Z135_MCR_OUT2;
+	if (mctrl & TIOCM_LOOP)
+		conf_reg |= MEN_Z135_MCR_LOOP;
+
+	men_z135_reg_set(uart, MEN_Z135_CONF_REG, conf_reg);
+}
+
+/**
+ * men_z135_get_mctrl() - Get modem control lines
+ * @port: The UART port
+ *
+ * Retruns the current state of modem control inputs.
+ */
+static unsigned int men_z135_get_mctrl(struct uart_port *port)
+{
+	unsigned int mctrl = 0;
+	u32 stat_reg;
+	u8 msr;
+
+	stat_reg = ioread32(port->membase + MEN_Z135_STAT_REG);
+
+	msr = ~((stat_reg >> 8) & 0xff);
+
+	if (msr & MEN_Z135_MSR_CTS)
+		mctrl |= TIOCM_CTS;
+	if (msr & MEN_Z135_MSR_DSR)
+		mctrl |= TIOCM_DSR;
+	if (msr & MEN_Z135_MSR_RI)
+		mctrl |= TIOCM_RI;
+	if (msr & MEN_Z135_MSR_DCD)
+		mctrl |= TIOCM_CAR;
+
+	return mctrl;
+}
+
+/**
+ * men_z135_stop_tx() - Stop transmitting characters
+ * @port: The UART port
+ *
+ * Stop transmitting characters. This might be due to CTS line becomming
+ * inactive or the tty layer indicating we want to stop transmission due to
+ * an XOFF character.
+ */
+static void men_z135_stop_tx(struct uart_port *port)
+{
+	struct men_z135_port *uart = to_men_z135(port);
+
+	men_z135_reg_clr(uart, MEN_Z135_CONF_REG, MEN_Z135_IER_TXCIEN);
+}
+
+/**
+ * men_z135_start_tx() - Start transmitting characters
+ * @port: The UART port
+ *
+ * Start transmitting character. This actually doesn't transmit anything, but
+ * fires off the TX tasklet.
+ */
+static void men_z135_start_tx(struct uart_port *port)
+{
+	struct men_z135_port *uart = to_men_z135(port);
+
+	men_z135_handle_tx(uart);
+}
+
+/**
+ * men_z135_stop_rx() - Stop receiving characters
+ * @port: The UART port
+ *
+ * Stop receiving characters; the port is in the process of being closed.
+ */
+static void men_z135_stop_rx(struct uart_port *port)
+{
+	struct men_z135_port *uart = to_men_z135(port);
+
+	men_z135_reg_clr(uart, MEN_Z135_CONF_REG, MEN_Z135_IER_RXCIEN);
+}
+
+/**
+ * men_z135_enable_ms() - Enable Modem Status
+ * port:
+ *
+ * Enable Modem Status IRQ.
+ */
+static void men_z135_enable_ms(struct uart_port *port)
+{
+	struct men_z135_port *uart = to_men_z135(port);
+
+	men_z135_reg_set(uart, MEN_Z135_CONF_REG, MEN_Z135_IER_MSIEN);
+}
+
+static int men_z135_startup(struct uart_port *port)
+{
+	struct men_z135_port *uart = to_men_z135(port);
+	int err;
+	u32 conf_reg = 0;
+
+	err = men_z135_request_irq(uart);
+	if (err)
+		return -ENODEV;
+
+	conf_reg = ioread32(port->membase + MEN_Z135_CONF_REG);
+
+	/* Activate all but TX space available IRQ */
+	conf_reg |= MEN_Z135_ALL_IRQS & ~MEN_Z135_IER_TXCIEN;
+	conf_reg &= ~(0xff << 16);
+	conf_reg |= (txlvl << 16);
+	conf_reg |= (rxlvl << 20);
+
+	iowrite32(conf_reg, port->membase + MEN_Z135_CONF_REG);
+
+	return 0;
+}
+
+static void men_z135_shutdown(struct uart_port *port)
+{
+	struct men_z135_port *uart = to_men_z135(port);
+	u32 conf_reg = 0;
+
+	conf_reg |= MEN_Z135_ALL_IRQS;
+
+	men_z135_reg_clr(uart, MEN_Z135_CONF_REG, conf_reg);
+
+	free_irq(uart->port.irq, uart);
+}
+
+static void men_z135_set_termios(struct uart_port *port,
+				struct ktermios *termios,
+				struct ktermios *old)
+{
+	unsigned int baud;
+	u32 conf_reg;
+	u32 bd_reg;
+	u32 uart_freq;
+	u8 lcr;
+
+	conf_reg = ioread32(port->membase + MEN_Z135_CONF_REG);
+	lcr = LCR(conf_reg);
+
+	/* byte size */
+	switch (termios->c_cflag & CSIZE) {
+	case CS5:
+		lcr |= MEN_Z135_WL5;
+		break;
+	case CS6:
+		lcr |= MEN_Z135_WL6;
+		break;
+	case CS7:
+		lcr |= MEN_Z135_WL7;
+		break;
+	case CS8:
+		lcr |= MEN_Z135_WL8;
+		break;
+	}
+
+	/* stop bits */
+	if (termios->c_cflag & CSTOPB)
+		lcr |= MEN_Z135_NSTB2 << MEN_Z135_STB_SHIFT;
+
+	/* parity */
+	if (termios->c_cflag & PARENB) {
+		lcr |= MEN_Z135_PAR_ENA << MEN_Z135_PEN_SHIFT;
+
+		if (termios->c_cflag & PARODD)
+			lcr |= MEN_Z135_PTY_ODD << MEN_Z135_PTY_SHIFT;
+		else
+			lcr |= MEN_Z135_PTY_EVN << MEN_Z135_PTY_SHIFT;
+	} else
+		lcr |= MEN_Z135_PAR_DIS << MEN_Z135_PEN_SHIFT;
+
+	termios->c_cflag &= ~CMSPAR; /* Mark/Space parity is not supported */
+
+	conf_reg |= lcr << MEN_Z135_LCR_SHIFT;
+	iowrite32(conf_reg, port->membase + MEN_Z135_CONF_REG);
+
+	uart_freq = ioread32(port->membase + MEN_Z135_UART_FREQ);
+	if (uart_freq == 0)
+		uart_freq = MEN_Z135_BASECLK;
+
+	baud = uart_get_baud_rate(port, termios, old, 0, uart_freq / 16);
+
+	spin_lock(&port->lock);
+	if (tty_termios_baud_rate(termios))
+		tty_termios_encode_baud_rate(termios, baud, baud);
+
+	bd_reg = uart_freq / (4 * baud);
+	iowrite32(bd_reg, port->membase + MEN_Z135_BAUD_REG);
+
+	uart_update_timeout(port, termios->c_cflag, baud);
+	spin_unlock(&port->lock);
+}
+
+static const char *men_z135_type(struct uart_port *port)
+{
+	return KBUILD_MODNAME;
+}
+
+static void men_z135_release_port(struct uart_port *port)
+{
+	iounmap(port->membase);
+	port->membase = NULL;
+
+	release_mem_region(port->mapbase, MEN_Z135_MEM_SIZE);
+}
+
+static int men_z135_request_port(struct uart_port *port)
+{
+	int size = MEN_Z135_MEM_SIZE;
+
+	if (!request_mem_region(port->mapbase, size, "men_z135_port"))
+		return -EBUSY;
+
+	port->membase = ioremap(port->mapbase, MEN_Z135_MEM_SIZE);
+	if (port->membase == NULL) {
+		release_mem_region(port->mapbase, MEN_Z135_MEM_SIZE);
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static void men_z135_config_port(struct uart_port *port, int type)
+{
+	port->type = PORT_MEN_Z135;
+	men_z135_request_port(port);
+}
+
+static int men_z135_verify_port(struct uart_port *port,
+				struct serial_struct *serinfo)
+{
+	return -EINVAL;
+}
+
+static struct uart_ops men_z135_ops = {
+	.tx_empty = men_z135_tx_empty,
+	.set_mctrl = men_z135_set_mctrl,
+	.get_mctrl = men_z135_get_mctrl,
+	.stop_tx = men_z135_stop_tx,
+	.start_tx = men_z135_start_tx,
+	.stop_rx = men_z135_stop_rx,
+	.enable_ms = men_z135_enable_ms,
+	.startup = men_z135_startup,
+	.shutdown = men_z135_shutdown,
+	.set_termios = men_z135_set_termios,
+	.type = men_z135_type,
+	.release_port = men_z135_release_port,
+	.request_port = men_z135_request_port,
+	.config_port = men_z135_config_port,
+	.verify_port = men_z135_verify_port,
+};
+
+static struct uart_driver men_z135_driver = {
+	.owner = THIS_MODULE,
+	.driver_name = KBUILD_MODNAME,
+	.dev_name = "ttyHSU",
+	.major = 0,
+	.minor = 0,
+	.nr = MEN_Z135_MAX_PORTS,
+};
+
+/**
+ * men_z135_probe() - Probe a z135 instance
+ * @mdev: The MCB device
+ * @id: The MCB device ID
+ *
+ * men_z135_probe does the basic setup of hardware resources and registers the
+ * new uart port to the tty layer.
+ */
+static int men_z135_probe(struct mcb_device *mdev,
+			const struct mcb_device_id *id)
+{
+	struct men_z135_port *uart;
+	struct resource *mem;
+	struct device *dev;
+	int err;
+
+	dev = &mdev->dev;
+
+	uart = devm_kzalloc(dev, sizeof(struct men_z135_port), GFP_KERNEL);
+	if (!uart)
+		return -ENOMEM;
+
+	uart->rxbuf = (unsigned char *)__get_free_page(GFP_KERNEL);
+	if (!uart->rxbuf)
+		return -ENOMEM;
+
+	mem = &mdev->mem;
+
+	mcb_set_drvdata(mdev, uart);
+
+	uart->port.uartclk = MEN_Z135_BASECLK * 16;
+	uart->port.fifosize = MEN_Z135_FIFO_SIZE;
+	uart->port.iotype = UPIO_MEM;
+	uart->port.ops = &men_z135_ops;
+	uart->port.irq = mcb_get_irq(mdev);
+	uart->port.iotype = UPIO_MEM;
+	uart->port.flags = UPF_BOOT_AUTOCONF | UPF_IOREMAP;
+	uart->port.line = line++;
+	uart->port.dev = dev;
+	uart->port.type = PORT_MEN_Z135;
+	uart->port.mapbase = mem->start;
+	uart->port.membase = NULL;
+	uart->mdev = mdev;
+
+	spin_lock_init(&uart->port.lock);
+	spin_lock_init(&uart->lock);
+
+	err = uart_add_one_port(&men_z135_driver, &uart->port);
+	if (err)
+		goto err;
+
+	return 0;
+
+err:
+	free_page((unsigned long) uart->rxbuf);
+	dev_err(dev, "Failed to add UART: %d\n", err);
+
+	return err;
+}
+
+/**
+ * men_z135_remove() - Remove a z135 instance from the system
+ *
+ * @mdev: The MCB device
+ */
+static void men_z135_remove(struct mcb_device *mdev)
+{
+	struct men_z135_port *uart = mcb_get_drvdata(mdev);
+
+	line--;
+	uart_remove_one_port(&men_z135_driver, &uart->port);
+	free_page((unsigned long) uart->rxbuf);
+}
+
+static const struct mcb_device_id men_z135_ids[] = {
+	{ .device = 0x87 },
+};
+MODULE_DEVICE_TABLE(mcb, men_z135_ids);
+
+static struct mcb_driver mcb_driver = {
+	.driver = {
+		.name = "z135-uart",
+		.owner = THIS_MODULE,
+	},
+	.probe = men_z135_probe,
+	.remove = men_z135_remove,
+	.id_table = men_z135_ids,
+};
+
+/**
+ * men_z135_init() - Driver Registration Routine
+ *
+ * men_z135_init is the first routine called when the driver is loaded. All it
+ * does is register with the legacy MEN Chameleon subsystem.
+ */
+static int __init men_z135_init(void)
+{
+	int err;
+
+	err = uart_register_driver(&men_z135_driver);
+	if (err) {
+		pr_err("Failed to register UART: %d\n", err);
+		return err;
+	}
+
+	err = mcb_register_driver(&mcb_driver);
+	if  (err) {
+		pr_err("Failed to register MCB driver: %d\n", err);
+		uart_unregister_driver(&men_z135_driver);
+		return err;
+	}
+
+	return 0;
+}
+module_init(men_z135_init);
+
+/**
+ * men_z135_exit() - Driver Exit Routine
+ *
+ * men_z135_exit is called just before the driver is removed from memory.
+ */
+static void __exit men_z135_exit(void)
+{
+	mcb_unregister_driver(&mcb_driver);
+	uart_unregister_driver(&men_z135_driver);
+}
+module_exit(men_z135_exit);
+
+MODULE_AUTHOR("Johannes Thumshirn <johannes.thumshirn@men.de>");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("MEN 16z135 High Speed UART");
+MODULE_ALIAS("mcb:16z135");
diff --git a/drivers/tty/serial/msm_serial.c b/drivers/tty/serial/msm_serial.c
index 053b98eb46c..778e376f197 100644
--- a/drivers/tty/serial/msm_serial.c
+++ b/drivers/tty/serial/msm_serial.c
@@ -52,7 +52,6 @@ struct msm_port {
 	struct clk		*clk;
 	struct clk		*pclk;
 	unsigned int		imr;
-	void __iomem		*gsbi_base;
 	int			is_uartdm;
 	unsigned int		old_snap_state;
 };
@@ -599,9 +598,7 @@ static const char *msm_type(struct uart_port *port)
 static void msm_release_port(struct uart_port *port)
 {
 	struct platform_device *pdev = to_platform_device(port->dev);
-	struct msm_port *msm_port = UART_TO_MSM(port);
 	struct resource *uart_resource;
-	struct resource *gsbi_resource;
 	resource_size_t size;
 
 	uart_resource = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -612,28 +609,12 @@ static void msm_release_port(struct uart_port *port)
 	release_mem_region(port->mapbase, size);
 	iounmap(port->membase);
 	port->membase = NULL;
-
-	if (msm_port->gsbi_base) {
-		writel_relaxed(GSBI_PROTOCOL_IDLE,
-				msm_port->gsbi_base + GSBI_CONTROL);
-
-		gsbi_resource = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-		if (unlikely(!gsbi_resource))
-			return;
-
-		size = resource_size(gsbi_resource);
-		release_mem_region(gsbi_resource->start, size);
-		iounmap(msm_port->gsbi_base);
-		msm_port->gsbi_base = NULL;
-	}
 }
 
 static int msm_request_port(struct uart_port *port)
 {
-	struct msm_port *msm_port = UART_TO_MSM(port);
 	struct platform_device *pdev = to_platform_device(port->dev);
 	struct resource *uart_resource;
-	struct resource *gsbi_resource;
 	resource_size_t size;
 	int ret;
 
@@ -652,30 +633,8 @@ static int msm_request_port(struct uart_port *port)
 		goto fail_release_port;
 	}
 
-	gsbi_resource = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-	/* Is this a GSBI-based port? */
-	if (gsbi_resource) {
-		size = resource_size(gsbi_resource);
-
-		if (!request_mem_region(gsbi_resource->start, size,
-						 "msm_serial")) {
-			ret = -EBUSY;
-			goto fail_release_port_membase;
-		}
-
-		msm_port->gsbi_base = ioremap(gsbi_resource->start, size);
-		if (!msm_port->gsbi_base) {
-			ret = -EBUSY;
-			goto fail_release_gsbi;
-		}
-	}
-
 	return 0;
 
-fail_release_gsbi:
-	release_mem_region(gsbi_resource->start, size);
-fail_release_port_membase:
-	iounmap(port->membase);
 fail_release_port:
 	release_mem_region(port->mapbase, size);
 	return ret;
@@ -683,7 +642,6 @@ fail_release_port:
 
 static void msm_config_port(struct uart_port *port, int flags)
 {
-	struct msm_port *msm_port = UART_TO_MSM(port);
 	int ret;
 	if (flags & UART_CONFIG_TYPE) {
 		port->type = PORT_MSM;
@@ -691,9 +649,6 @@ static void msm_config_port(struct uart_port *port, int flags)
 		if (ret)
 			return;
 	}
-	if (msm_port->gsbi_base)
-		writel_relaxed(GSBI_PROTOCOL_UART,
-				msm_port->gsbi_base + GSBI_CONTROL);
 }
 
 static int msm_verify_port(struct uart_port *port, struct serial_struct *ser)
@@ -1110,6 +1065,7 @@ static struct of_device_id msm_match_table[] = {
 
 static struct platform_driver msm_platform_driver = {
 	.remove = msm_serial_remove,
+	.probe = msm_serial_probe,
 	.driver = {
 		.name = "msm_serial",
 		.owner = THIS_MODULE,
@@ -1125,7 +1081,7 @@ static int __init msm_serial_init(void)
 	if (unlikely(ret))
 		return ret;
 
-	ret = platform_driver_probe(&msm_platform_driver, msm_serial_probe);
+	ret = platform_driver_register(&msm_platform_driver);
 	if (unlikely(ret))
 		uart_unregister_driver(&msm_uart_driver);
 
diff --git a/drivers/tty/serial/msm_serial.h b/drivers/tty/serial/msm_serial.h
index 1e9b68b6f9e..d98d45efdf8 100644
--- a/drivers/tty/serial/msm_serial.h
+++ b/drivers/tty/serial/msm_serial.h
@@ -109,11 +109,6 @@
 #define UART_ISR		0x0014
 #define UART_ISR_TX_READY	(1 << 7)
 
-#define GSBI_CONTROL		0x0
-#define GSBI_PROTOCOL_CODE	0x30
-#define GSBI_PROTOCOL_UART	0x40
-#define GSBI_PROTOCOL_IDLE	0x0
-
 #define UARTDM_RXFS		0x50
 #define UARTDM_RXFS_BUF_SHIFT	0x7
 #define UARTDM_RXFS_BUF_MASK	0x7
diff --git a/drivers/tty/serial/mux.c b/drivers/tty/serial/mux.c
index 7fd6aaaacd8..be127d0da32 100644
--- a/drivers/tty/serial/mux.c
+++ b/drivers/tty/serial/mux.c
@@ -29,7 +29,7 @@
 #include <asm/irq.h>
 #include <asm/parisc-device.h>
 
-#ifdef CONFIG_MAGIC_SYSRQ
+#if defined(CONFIG_SERIAL_MUX_CONSOLE) && defined(CONFIG_MAGIC_SYSRQ)
 #include <linux/sysrq.h>
 #define SUPPORT_SYSRQ
 #endif
@@ -613,7 +613,7 @@ static void __exit mux_exit(void)
 {
 	/* Delete the Mux timer. */
 	if(port_cnt > 0) {
-		del_timer(&mux_timer);
+		del_timer_sync(&mux_timer);
 #ifdef CONFIG_SERIAL_MUX_CONSOLE
 		unregister_console(&mux_console);
 #endif
diff --git a/drivers/tty/serial/of_serial.c b/drivers/tty/serial/of_serial.c
index 99246606a25..68d4455f3cf 100644
--- a/drivers/tty/serial/of_serial.c
+++ b/drivers/tty/serial/of_serial.c
@@ -173,6 +173,7 @@ static int of_platform_serial_probe(struct platform_device *ofdev)
 	{
 		struct uart_8250_port port8250;
 		memset(&port8250, 0, sizeof(port8250));
+		port.type = port_type;
 		port8250.port = port;
 
 		if (port.fifosize)
@@ -182,6 +183,10 @@ static int of_platform_serial_probe(struct platform_device *ofdev)
 					  "auto-flow-control"))
 			port8250.capabilities |= UART_CAP_AFE;
 
+		if (of_property_read_bool(ofdev->dev.of_node,
+					  "has-hw-flow-control"))
+			port8250.port.flags |= UPF_HARD_FLOW;
+
 		ret = serial8250_register_8250_port(&port8250);
 		break;
 	}
diff --git a/drivers/tty/serial/omap-serial.c b/drivers/tty/serial/omap-serial.c
index dd8b1a5458f..d017cec8a34 100644
--- a/drivers/tty/serial/omap-serial.c
+++ b/drivers/tty/serial/omap-serial.c
@@ -163,10 +163,6 @@ struct uart_omap_port {
 	u8			wakeups_enabled;
 	u32			features;
 
-	int			DTR_gpio;
-	int			DTR_inverted;
-	int			DTR_active;
-
 	struct serial_rs485	rs485;
 	int			rts_gpio;
 
@@ -184,8 +180,6 @@ static struct uart_omap_port *ui[OMAP_MAX_HSUART_PORTS];
 /* Forward declaration of functions */
 static void serial_omap_mdr1_errataset(struct uart_omap_port *up, u8 mdr1);
 
-static struct workqueue_struct *serial_omap_uart_wq;
-
 static inline unsigned int serial_in(struct uart_omap_port *up, int offset)
 {
 	offset <<= up->port.regshift;
@@ -225,14 +219,19 @@ static inline void serial_omap_enable_wakeirq(struct uart_omap_port *up,
 	if (enable)
 		enable_irq(up->wakeirq);
 	else
-		disable_irq(up->wakeirq);
+		disable_irq_nosync(up->wakeirq);
 }
 
 static void serial_omap_enable_wakeup(struct uart_omap_port *up, bool enable)
 {
 	struct omap_uart_port_info *pdata = dev_get_platdata(up->dev);
 
+	if (enable == up->wakeups_enabled)
+		return;
+
 	serial_omap_enable_wakeirq(up, enable);
+	up->wakeups_enabled = enable;
+
 	if (!pdata || !pdata->enable_wakeup)
 		return;
 
@@ -393,11 +392,8 @@ static void transmit_chars(struct uart_omap_port *up, unsigned int lsr)
 			break;
 	} while (--count > 0);
 
-	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS) {
-		spin_unlock(&up->port.lock);
+	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
 		uart_write_wakeup(&up->port);
-		spin_lock(&up->port.lock);
-	}
 
 	if (uart_circ_empty(xmit))
 		serial_omap_stop_tx(&up->port);
@@ -692,16 +688,6 @@ static void serial_omap_set_mctrl(struct uart_port *port, unsigned int mctrl)
 	serial_out(up, UART_MCR, up->mcr);
 	pm_runtime_mark_last_busy(up->dev);
 	pm_runtime_put_autosuspend(up->dev);
-
-	if (gpio_is_valid(up->DTR_gpio) &&
-	    !!(mctrl & TIOCM_DTR) != up->DTR_active) {
-		up->DTR_active = !up->DTR_active;
-		if (gpio_cansleep(up->DTR_gpio))
-			schedule_work(&up->qos_work);
-		else
-			gpio_set_value(up->DTR_gpio,
-				       up->DTR_active != up->DTR_inverted);
-	}
 }
 
 static void serial_omap_break_ctl(struct uart_port *port, int break_state)
@@ -845,9 +831,6 @@ static void serial_omap_uart_qos_work(struct work_struct *work)
 						qos_work);
 
 	pm_qos_update_request(&up->pm_qos_request, up->latency);
-	if (gpio_is_valid(up->DTR_gpio))
-		gpio_set_value_cansleep(up->DTR_gpio,
-					up->DTR_active != up->DTR_inverted);
 }
 
 static void
@@ -1415,7 +1398,7 @@ serial_omap_ioctl(struct uart_port *port, unsigned int cmd, unsigned long arg)
 
 	switch (cmd) {
 	case TIOCSRS485:
-		if (copy_from_user(&rs485conf, (struct serial_rs485 *) arg,
+		if (copy_from_user(&rs485conf, (void __user *) arg,
 					sizeof(rs485conf)))
 			return -EFAULT;
 
@@ -1423,7 +1406,7 @@ serial_omap_ioctl(struct uart_port *port, unsigned int cmd, unsigned long arg)
 		break;
 
 	case TIOCGRS485:
-		if (copy_to_user((struct serial_rs485 *) arg,
+		if (copy_to_user((void __user *) arg,
 					&(to_uart_omap_port(port)->rs485),
 					sizeof(rs485conf)))
 			return -EFAULT;
@@ -1495,6 +1478,11 @@ static int serial_omap_suspend(struct device *dev)
 	uart_suspend_port(&serial_omap_reg, &up->port);
 	flush_work(&up->qos_work);
 
+	if (device_may_wakeup(dev))
+		serial_omap_enable_wakeup(up, true);
+	else
+		serial_omap_enable_wakeup(up, false);
+
 	return 0;
 }
 
@@ -1502,6 +1490,9 @@ static int serial_omap_resume(struct device *dev)
 {
 	struct uart_omap_port *up = dev_get_drvdata(dev);
 
+	if (device_may_wakeup(dev))
+		serial_omap_enable_wakeup(up, false);
+
 	uart_resume_port(&serial_omap_reg, &up->port);
 
 	return 0;
@@ -1601,7 +1592,7 @@ static int serial_omap_probe_rs485(struct uart_omap_port *up,
 	/* check for tx enable gpio */
 	up->rts_gpio = of_get_named_gpio_flags(np, "rts-gpio", 0, &flags);
 	if (gpio_is_valid(up->rts_gpio)) {
-		ret = gpio_request(up->rts_gpio, "omap-serial");
+		ret = devm_gpio_request(up->dev, up->rts_gpio, "omap-serial");
 		if (ret < 0)
 			return ret;
 		ret = gpio_direction_output(up->rts_gpio,
@@ -1631,10 +1622,13 @@ static int serial_omap_probe_rs485(struct uart_omap_port *up,
 
 static int serial_omap_probe(struct platform_device *pdev)
 {
-	struct uart_omap_port	*up;
-	struct resource		*mem, *irq;
 	struct omap_uart_port_info *omap_up_info = dev_get_platdata(&pdev->dev);
-	int ret, uartirq = 0, wakeirq = 0;
+	struct uart_omap_port *up;
+	struct resource *mem;
+	void __iomem *base;
+	int uartirq = 0;
+	int wakeirq = 0;
+	int ret;
 
 	/* The optional wakeirq may be specified in the board dts file */
 	if (pdev->dev.of_node) {
@@ -1645,48 +1639,19 @@ static int serial_omap_probe(struct platform_device *pdev)
 		omap_up_info = of_get_uart_port_info(&pdev->dev);
 		pdev->dev.platform_data = omap_up_info;
 	} else {
-		irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-		if (!irq) {
-			dev_err(&pdev->dev, "no irq resource?\n");
-			return -ENODEV;
-		}
-		uartirq = irq->start;
-	}
-
-	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!mem) {
-		dev_err(&pdev->dev, "no mem resource?\n");
-		return -ENODEV;
-	}
-
-	if (!devm_request_mem_region(&pdev->dev, mem->start, resource_size(mem),
-				pdev->dev.driver->name)) {
-		dev_err(&pdev->dev, "memory region already claimed\n");
-		return -EBUSY;
-	}
-
-	if (gpio_is_valid(omap_up_info->DTR_gpio) &&
-	    omap_up_info->DTR_present) {
-		ret = gpio_request(omap_up_info->DTR_gpio, "omap-serial");
-		if (ret < 0)
-			return ret;
-		ret = gpio_direction_output(omap_up_info->DTR_gpio,
-					    omap_up_info->DTR_inverted);
-		if (ret < 0)
-			return ret;
+		uartirq = platform_get_irq(pdev, 0);
+		if (uartirq < 0)
+			return -EPROBE_DEFER;
 	}
 
 	up = devm_kzalloc(&pdev->dev, sizeof(*up), GFP_KERNEL);
 	if (!up)
 		return -ENOMEM;
 
-	if (gpio_is_valid(omap_up_info->DTR_gpio) &&
-	    omap_up_info->DTR_present) {
-		up->DTR_gpio = omap_up_info->DTR_gpio;
-		up->DTR_inverted = omap_up_info->DTR_inverted;
-	} else
-		up->DTR_gpio = -EINVAL;
-	up->DTR_active = 0;
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	base = devm_ioremap_resource(&pdev->dev, mem);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
 
 	up->dev = &pdev->dev;
 	up->port.dev = &pdev->dev;
@@ -1720,14 +1685,7 @@ static int serial_omap_probe(struct platform_device *pdev)
 
 	sprintf(up->name, "OMAP UART%d", up->port.line);
 	up->port.mapbase = mem->start;
-	up->port.membase = devm_ioremap(&pdev->dev, mem->start,
-						resource_size(mem));
-	if (!up->port.membase) {
-		dev_err(&pdev->dev, "can't ioremap UART\n");
-		ret = -ENOMEM;
-		goto err_ioremap;
-	}
-
+	up->port.membase = base;
 	up->port.flags = omap_up_info->flags;
 	up->port.uartclk = omap_up_info->uartclk;
 	if (!up->port.uartclk) {
@@ -1741,12 +1699,12 @@ static int serial_omap_probe(struct platform_device *pdev)
 	up->calc_latency = PM_QOS_CPU_DMA_LAT_DEFAULT_VALUE;
 	pm_qos_add_request(&up->pm_qos_request,
 		PM_QOS_CPU_DMA_LATENCY, up->latency);
-	serial_omap_uart_wq = create_singlethread_workqueue(up->name);
 	INIT_WORK(&up->qos_work, serial_omap_uart_qos_work);
 
 	platform_set_drvdata(pdev, up);
 	if (omap_up_info->autosuspend_timeout == 0)
 		omap_up_info->autosuspend_timeout = -1;
+
 	device_init_wakeup(up->dev, true);
 	pm_runtime_use_autosuspend(&pdev->dev);
 	pm_runtime_set_autosuspend_delay(&pdev->dev,
@@ -1773,7 +1731,6 @@ static int serial_omap_probe(struct platform_device *pdev)
 err_add_port:
 	pm_runtime_put(&pdev->dev);
 	pm_runtime_disable(&pdev->dev);
-err_ioremap:
 err_rs485:
 err_port_line:
 	dev_err(&pdev->dev, "[UART%d]: failure [%s]: %d\n",
@@ -1789,6 +1746,7 @@ static int serial_omap_remove(struct platform_device *dev)
 	pm_runtime_disable(up->dev);
 	uart_remove_one_port(&serial_omap_reg, &up->port);
 	pm_qos_remove_request(&up->pm_qos_request);
+	device_init_wakeup(&dev->dev, false);
 
 	return 0;
 }
@@ -1877,17 +1835,7 @@ static int serial_omap_runtime_suspend(struct device *dev)
 
 	up->context_loss_cnt = serial_omap_get_context_loss_count(up);
 
-	if (device_may_wakeup(dev)) {
-		if (!up->wakeups_enabled) {
-			serial_omap_enable_wakeup(up, true);
-			up->wakeups_enabled = true;
-		}
-	} else {
-		if (up->wakeups_enabled) {
-			serial_omap_enable_wakeup(up, false);
-			up->wakeups_enabled = false;
-		}
-	}
+	serial_omap_enable_wakeup(up, true);
 
 	up->latency = PM_QOS_CPU_DMA_LAT_DEFAULT_VALUE;
 	schedule_work(&up->qos_work);
@@ -1901,6 +1849,8 @@ static int serial_omap_runtime_resume(struct device *dev)
 
 	int loss_cnt = serial_omap_get_context_loss_count(up);
 
+	serial_omap_enable_wakeup(up, false);
+
 	if (loss_cnt < 0) {
 		dev_dbg(dev, "serial_omap_get_context_loss_count failed : %d\n",
 			loss_cnt);
diff --git a/drivers/tty/serial/pch_uart.c b/drivers/tty/serial/pch_uart.c
index 0931b3fe9ed..0cb6a8e52bd 100644
--- a/drivers/tty/serial/pch_uart.c
+++ b/drivers/tty/serial/pch_uart.c
@@ -257,6 +257,8 @@ struct eg20t_port {
 	dma_addr_t			rx_buf_dma;
 
 	struct dentry	*debugfs;
+#define IRQ_NAME_SIZE 17
+	char				irq_name[IRQ_NAME_SIZE];
 
 	/* protect the eg20t_port private structure and io access to membase */
 	spinlock_t lock;
@@ -1343,7 +1345,7 @@ static int pch_uart_startup(struct uart_port *port)
 		return ret;
 
 	ret = request_irq(priv->port.irq, pch_uart_interrupt, IRQF_SHARED,
-			KBUILD_MODNAME, priv);
+			priv->irq_name, priv);
 	if (ret < 0)
 		return ret;
 
@@ -1588,13 +1590,8 @@ static void pch_uart_put_poll_char(struct uart_port *port,
 	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
@@ -1818,6 +1815,10 @@ static struct eg20t_port *pch_uart_init_port(struct pci_dev *pdev,
 	priv->port.line = board->line_no;
 	priv->trigger = PCH_UART_HAL_TRIGGER_M;
 
+	snprintf(priv->irq_name, IRQ_NAME_SIZE,
+		 KBUILD_MODNAME ":" PCH_UART_DRIVER_DEVICE "%d",
+		 priv->port.line);
+
 	spin_lock_init(&priv->port.lock);
 
 	pci_set_drvdata(pdev, priv);
diff --git a/drivers/tty/serial/pxa.c b/drivers/tty/serial/pxa.c
index f9f20f38376..9e7ee39f8b2 100644
--- a/drivers/tty/serial/pxa.c
+++ b/drivers/tty/serial/pxa.c
@@ -711,13 +711,8 @@ static void serial_pxa_put_poll_char(struct uart_port *port,
 	wait_for_xmitr(up);
 	/*
 	 *	Send the character out.
-	 *	If a LF, also do CR...
 	 */
 	serial_out(up, UART_TX, c);
-	if (c == 10) {
-		wait_for_xmitr(up);
-		serial_out(up, UART_TX, 13);
-	}
 
 	/*
 	 *	Finally, wait for transmitter to become empty
diff --git a/drivers/tty/serial/samsung.c b/drivers/tty/serial/samsung.c
index 23f45960073..329337711bb 100644
--- a/drivers/tty/serial/samsung.c
+++ b/drivers/tty/serial/samsung.c
@@ -53,6 +53,29 @@
 
 #include "samsung.h"
 
+#if	defined(CONFIG_SERIAL_SAMSUNG_DEBUG) &&	\
+	defined(CONFIG_DEBUG_LL) &&		\
+	!defined(MODULE)
+
+extern void printascii(const char *);
+
+__printf(1, 2)
+static void dbg(const char *fmt, ...)
+{
+	va_list va;
+	char buff[256];
+
+	va_start(va, fmt);
+	vscnprintf(buff, sizeof(buf), fmt, va);
+	va_end(va);
+
+	printascii(buff);
+}
+
+#else
+#define dbg(fmt, ...) do { if (0) no_printk(fmt, ##__VA_ARGS__); } while (0)
+#endif
+
 /* UART name and device definitions */
 
 #define S3C24XX_SERIAL_NAME	"ttySAC"
@@ -468,8 +491,8 @@ static int s3c24xx_serial_startup(struct uart_port *port)
 	struct s3c24xx_uart_port *ourport = to_ourport(port);
 	int ret;
 
-	dbg("s3c24xx_serial_startup: port=%p (%08lx,%p)\n",
-	    port->mapbase, port->membase);
+	dbg("s3c24xx_serial_startup: port=%p (%08llx,%p)\n",
+	    port, (unsigned long long)port->mapbase, port->membase);
 
 	rx_enabled(port) = 1;
 
@@ -514,8 +537,8 @@ static int s3c64xx_serial_startup(struct uart_port *port)
 	struct s3c24xx_uart_port *ourport = to_ourport(port);
 	int ret;
 
-	dbg("s3c64xx_serial_startup: port=%p (%08lx,%p)\n",
-	    port->mapbase, port->membase);
+	dbg("s3c64xx_serial_startup: port=%p (%08llx,%p)\n",
+	    port, (unsigned long long)port->mapbase, port->membase);
 
 	wr_regl(port, S3C64XX_UINTM, 0xf);
 
@@ -1160,7 +1183,7 @@ static int s3c24xx_serial_init_port(struct s3c24xx_uart_port *ourport,
 		return -EINVAL;
 	}
 
-	dbg("resource %p (%lx..%lx)\n", res, res->start, res->end);
+	dbg("resource %pR)\n", res);
 
 	port->membase = devm_ioremap(port->dev, res->start, resource_size(res));
 	if (!port->membase) {
@@ -1203,7 +1226,7 @@ static int s3c24xx_serial_init_port(struct s3c24xx_uart_port *ourport,
 		wr_regl(port, S3C64XX_UINTSP, 0xf);
 	}
 
-	dbg("port: map=%08x, mem=%08x, irq=%d (%d,%d), clock=%ld\n",
+	dbg("port: map=%08x, mem=%p, irq=%d (%d,%d), clock=%u\n",
 	    port->mapbase, port->membase, port->irq,
 	    ourport->rx_irq, ourport->tx_irq, port->uartclk);
 
@@ -1446,8 +1469,8 @@ 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)
 {
-	unsigned int ufcon = rd_regl(cons_uart, S3C2410_UFCON);
-	unsigned int ucon = rd_regl(cons_uart, S3C2410_UCON);
+	unsigned int ufcon = rd_regl(port, S3C2410_UFCON);
+	unsigned int ucon = rd_regl(port, S3C2410_UCON);
 
 	/* not possible to xmit on unconfigured port */
 	if (!s3c24xx_port_configured(ucon))
@@ -1455,7 +1478,7 @@ static void s3c24xx_serial_put_poll_char(struct uart_port *port,
 
 	while (!s3c24xx_serial_console_txrdy(port, ufcon))
 		cpu_relax();
-	wr_regb(cons_uart, S3C2410_UTXH, c);
+	wr_regb(port, S3C2410_UTXH, c);
 }
 
 #endif /* CONFIG_CONSOLE_POLL */
@@ -1463,22 +1486,23 @@ static void s3c24xx_serial_put_poll_char(struct uart_port *port,
 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;
+	unsigned int ufcon = rd_regl(port, S3C2410_UFCON);
 
 	while (!s3c24xx_serial_console_txrdy(port, ufcon))
-		barrier();
-	wr_regb(cons_uart, S3C2410_UTXH, ch);
+		cpu_relax();
+	wr_regb(port, S3C2410_UTXH, ch);
 }
 
 static void
 s3c24xx_serial_console_write(struct console *co, const char *s,
 			     unsigned int count)
 {
+	unsigned int ucon = rd_regl(cons_uart, S3C2410_UCON);
+
+	/* not possible to xmit on unconfigured port */
+	if (!s3c24xx_port_configured(ucon))
+		return;
+
 	uart_console_write(cons_uart, s, count, s3c24xx_serial_console_putchar);
 }
 
diff --git a/drivers/tty/serial/samsung.h b/drivers/tty/serial/samsung.h
index 8827e5424ce..eb071dd19b2 100644
--- a/drivers/tty/serial/samsung.h
+++ b/drivers/tty/serial/samsung.h
@@ -1,3 +1,6 @@
+#ifndef __SAMSUNG_H
+#define __SAMSUNG_H
+
 /*
  * Driver for Samsung SoC onboard UARTs.
  *
@@ -77,24 +80,4 @@ 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))
 
-#if defined(CONFIG_SERIAL_SAMSUNG_DEBUG) && \
-    defined(CONFIG_DEBUG_LL) && \
-    !defined(MODULE)
-
-extern void printascii(const char *);
-
-static void dbg(const char *fmt, ...)
-{
-	va_list va;
-	char buff[256];
-
-	va_start(va, fmt);
-	vsprintf(buff, fmt, va);
-	va_end(va);
-
-	printascii(buff);
-}
-
-#else
-#define dbg(x...) do { } while (0)
 #endif
diff --git a/drivers/tty/serial/sc16is7xx.c b/drivers/tty/serial/sc16is7xx.c
new file mode 100644
index 00000000000..1b6a77c4b2c
--- /dev/null
+++ b/drivers/tty/serial/sc16is7xx.c
@@ -0,0 +1,1277 @@
+/*
+ * SC16IS7xx tty serial driver - Copyright (C) 2014 GridPoint
+ * Author: Jon Ringle <jringle@gridpoint.com>
+ *
+ *  Based on max310x.c, by Alexander Shiyan <shc_work@mail.ru>
+ *
+ * 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/bitops.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/regmap.h>
+#include <linux/serial_core.h>
+#include <linux/serial.h>
+#include <linux/tty.h>
+#include <linux/tty_flip.h>
+#include <linux/uaccess.h>
+
+#define SC16IS7XX_NAME			"sc16is7xx"
+
+/* SC16IS7XX register definitions */
+#define SC16IS7XX_RHR_REG		(0x00) /* RX FIFO */
+#define SC16IS7XX_THR_REG		(0x00) /* TX FIFO */
+#define SC16IS7XX_IER_REG		(0x01) /* Interrupt enable */
+#define SC16IS7XX_IIR_REG		(0x02) /* Interrupt Identification */
+#define SC16IS7XX_FCR_REG		(0x02) /* FIFO control */
+#define SC16IS7XX_LCR_REG		(0x03) /* Line Control */
+#define SC16IS7XX_MCR_REG		(0x04) /* Modem Control */
+#define SC16IS7XX_LSR_REG		(0x05) /* Line Status */
+#define SC16IS7XX_MSR_REG		(0x06) /* Modem Status */
+#define SC16IS7XX_SPR_REG		(0x07) /* Scratch Pad */
+#define SC16IS7XX_TXLVL_REG		(0x08) /* TX FIFO level */
+#define SC16IS7XX_RXLVL_REG		(0x09) /* RX FIFO level */
+#define SC16IS7XX_IODIR_REG		(0x0a) /* I/O Direction
+						* - only on 75x/76x
+						*/
+#define SC16IS7XX_IOSTATE_REG		(0x0b) /* I/O State
+						* - only on 75x/76x
+						*/
+#define SC16IS7XX_IOINTENA_REG		(0x0c) /* I/O Interrupt Enable
+						* - only on 75x/76x
+						*/
+#define SC16IS7XX_IOCONTROL_REG		(0x0e) /* I/O Control
+						* - only on 75x/76x
+						*/
+#define SC16IS7XX_EFCR_REG		(0x0f) /* Extra Features Control */
+
+/* TCR/TLR Register set: Only if ((MCR[2] == 1) && (EFR[4] == 1)) */
+#define SC16IS7XX_TCR_REG		(0x06) /* Transmit control */
+#define SC16IS7XX_TLR_REG		(0x07) /* Trigger level */
+
+/* Special Register set: Only if ((LCR[7] == 1) && (LCR != 0xBF)) */
+#define SC16IS7XX_DLL_REG		(0x00) /* Divisor Latch Low */
+#define SC16IS7XX_DLH_REG		(0x01) /* Divisor Latch High */
+
+/* Enhanced Register set: Only if (LCR == 0xBF) */
+#define SC16IS7XX_EFR_REG		(0x02) /* Enhanced Features */
+#define SC16IS7XX_XON1_REG		(0x04) /* Xon1 word */
+#define SC16IS7XX_XON2_REG		(0x05) /* Xon2 word */
+#define SC16IS7XX_XOFF1_REG		(0x06) /* Xoff1 word */
+#define SC16IS7XX_XOFF2_REG		(0x07) /* Xoff2 word */
+
+/* IER register bits */
+#define SC16IS7XX_IER_RDI_BIT		(1 << 0) /* Enable RX data interrupt */
+#define SC16IS7XX_IER_THRI_BIT		(1 << 1) /* Enable TX holding register
+						  * interrupt */
+#define SC16IS7XX_IER_RLSI_BIT		(1 << 2) /* Enable RX line status
+						  * interrupt */
+#define SC16IS7XX_IER_MSI_BIT		(1 << 3) /* Enable Modem status
+						  * interrupt */
+
+/* IER register bits - write only if (EFR[4] == 1) */
+#define SC16IS7XX_IER_SLEEP_BIT		(1 << 4) /* Enable Sleep mode */
+#define SC16IS7XX_IER_XOFFI_BIT		(1 << 5) /* Enable Xoff interrupt */
+#define SC16IS7XX_IER_RTSI_BIT		(1 << 6) /* Enable nRTS interrupt */
+#define SC16IS7XX_IER_CTSI_BIT		(1 << 7) /* Enable nCTS interrupt */
+
+/* FCR register bits */
+#define SC16IS7XX_FCR_FIFO_BIT		(1 << 0) /* Enable FIFO */
+#define SC16IS7XX_FCR_RXRESET_BIT	(1 << 1) /* Reset RX FIFO */
+#define SC16IS7XX_FCR_TXRESET_BIT	(1 << 2) /* Reset TX FIFO */
+#define SC16IS7XX_FCR_RXLVLL_BIT	(1 << 6) /* RX Trigger level LSB */
+#define SC16IS7XX_FCR_RXLVLH_BIT	(1 << 7) /* RX Trigger level MSB */
+
+/* FCR register bits - write only if (EFR[4] == 1) */
+#define SC16IS7XX_FCR_TXLVLL_BIT	(1 << 4) /* TX Trigger level LSB */
+#define SC16IS7XX_FCR_TXLVLH_BIT	(1 << 5) /* TX Trigger level MSB */
+
+/* IIR register bits */
+#define SC16IS7XX_IIR_NO_INT_BIT	(1 << 0) /* No interrupts pending */
+#define SC16IS7XX_IIR_ID_MASK		0x3e     /* Mask for the interrupt ID */
+#define SC16IS7XX_IIR_THRI_SRC		0x02     /* TX holding register empty */
+#define SC16IS7XX_IIR_RDI_SRC		0x04     /* RX data interrupt */
+#define SC16IS7XX_IIR_RLSE_SRC		0x06     /* RX line status error */
+#define SC16IS7XX_IIR_RTOI_SRC		0x0c     /* RX time-out interrupt */
+#define SC16IS7XX_IIR_MSI_SRC		0x00     /* Modem status interrupt
+						  * - only on 75x/76x
+						  */
+#define SC16IS7XX_IIR_INPIN_SRC		0x30     /* Input pin change of state
+						  * - only on 75x/76x
+						  */
+#define SC16IS7XX_IIR_XOFFI_SRC		0x10     /* Received Xoff */
+#define SC16IS7XX_IIR_CTSRTS_SRC	0x20     /* nCTS,nRTS change of state
+						  * from active (LOW)
+						  * to inactive (HIGH)
+						  */
+/* LCR register bits */
+#define SC16IS7XX_LCR_LENGTH0_BIT	(1 << 0) /* Word length bit 0 */
+#define SC16IS7XX_LCR_LENGTH1_BIT	(1 << 1) /* Word length bit 1
+						  *
+						  * Word length bits table:
+						  * 00 -> 5 bit words
+						  * 01 -> 6 bit words
+						  * 10 -> 7 bit words
+						  * 11 -> 8 bit words
+						  */
+#define SC16IS7XX_LCR_STOPLEN_BIT	(1 << 2) /* STOP length bit
+						  *
+						  * STOP length bit table:
+						  * 0 -> 1 stop bit
+						  * 1 -> 1-1.5 stop bits if
+						  *      word length is 5,
+						  *      2 stop bits otherwise
+						  */
+#define SC16IS7XX_LCR_PARITY_BIT	(1 << 3) /* Parity bit enable */
+#define SC16IS7XX_LCR_EVENPARITY_BIT	(1 << 4) /* Even parity bit enable */
+#define SC16IS7XX_LCR_FORCEPARITY_BIT	(1 << 5) /* 9-bit multidrop parity */
+#define SC16IS7XX_LCR_TXBREAK_BIT	(1 << 6) /* TX break enable */
+#define SC16IS7XX_LCR_DLAB_BIT		(1 << 7) /* Divisor Latch enable */
+#define SC16IS7XX_LCR_WORD_LEN_5	(0x00)
+#define SC16IS7XX_LCR_WORD_LEN_6	(0x01)
+#define SC16IS7XX_LCR_WORD_LEN_7	(0x02)
+#define SC16IS7XX_LCR_WORD_LEN_8	(0x03)
+#define SC16IS7XX_LCR_CONF_MODE_A	SC16IS7XX_LCR_DLAB_BIT /* Special
+								* reg set */
+#define SC16IS7XX_LCR_CONF_MODE_B	0xBF                   /* Enhanced
+								* reg set */
+
+/* MCR register bits */
+#define SC16IS7XX_MCR_DTR_BIT		(1 << 0) /* DTR complement
+						  * - only on 75x/76x
+						  */
+#define SC16IS7XX_MCR_RTS_BIT		(1 << 1) /* RTS complement */
+#define SC16IS7XX_MCR_TCRTLR_BIT	(1 << 2) /* TCR/TLR register enable */
+#define SC16IS7XX_MCR_LOOP_BIT		(1 << 4) /* Enable loopback test mode */
+#define SC16IS7XX_MCR_XONANY_BIT	(1 << 5) /* Enable Xon Any
+						  * - write enabled
+						  * if (EFR[4] == 1)
+						  */
+#define SC16IS7XX_MCR_IRDA_BIT		(1 << 6) /* Enable IrDA mode
+						  * - write enabled
+						  * if (EFR[4] == 1)
+						  */
+#define SC16IS7XX_MCR_CLKSEL_BIT	(1 << 7) /* Divide clock by 4
+						  * - write enabled
+						  * if (EFR[4] == 1)
+						  */
+
+/* LSR register bits */
+#define SC16IS7XX_LSR_DR_BIT		(1 << 0) /* Receiver data ready */
+#define SC16IS7XX_LSR_OE_BIT		(1 << 1) /* Overrun Error */
+#define SC16IS7XX_LSR_PE_BIT		(1 << 2) /* Parity Error */
+#define SC16IS7XX_LSR_FE_BIT		(1 << 3) /* Frame Error */
+#define SC16IS7XX_LSR_BI_BIT		(1 << 4) /* Break Interrupt */
+#define SC16IS7XX_LSR_BRK_ERROR_MASK	0x1E     /* BI, FE, PE, OE bits */
+#define SC16IS7XX_LSR_THRE_BIT		(1 << 5) /* TX holding register empty */
+#define SC16IS7XX_LSR_TEMT_BIT		(1 << 6) /* Transmitter empty */
+#define SC16IS7XX_LSR_FIFOE_BIT		(1 << 7) /* Fifo Error */
+
+/* MSR register bits */
+#define SC16IS7XX_MSR_DCTS_BIT		(1 << 0) /* Delta CTS Clear To Send */
+#define SC16IS7XX_MSR_DDSR_BIT		(1 << 1) /* Delta DSR Data Set Ready
+						  * or (IO4)
+						  * - only on 75x/76x
+						  */
+#define SC16IS7XX_MSR_DRI_BIT		(1 << 2) /* Delta RI Ring Indicator
+						  * or (IO7)
+						  * - only on 75x/76x
+						  */
+#define SC16IS7XX_MSR_DCD_BIT		(1 << 3) /* Delta CD Carrier Detect
+						  * or (IO6)
+						  * - only on 75x/76x
+						  */
+#define SC16IS7XX_MSR_CTS_BIT		(1 << 0) /* CTS */
+#define SC16IS7XX_MSR_DSR_BIT		(1 << 1) /* DSR (IO4)
+						  * - only on 75x/76x
+						  */
+#define SC16IS7XX_MSR_RI_BIT		(1 << 2) /* RI (IO7)
+						  * - only on 75x/76x
+						  */
+#define SC16IS7XX_MSR_CD_BIT		(1 << 3) /* CD (IO6)
+						  * - only on 75x/76x
+						  */
+#define SC16IS7XX_MSR_DELTA_MASK	0x0F     /* Any of the delta bits! */
+
+/*
+ * TCR register bits
+ * TCR trigger levels are available from 0 to 60 characters with a granularity
+ * of four.
+ * The programmer must program the TCR such that TCR[3:0] > TCR[7:4]. There is
+ * no built-in hardware check to make sure this condition is met. Also, the TCR
+ * must be programmed with this condition before auto RTS or software flow
+ * control is enabled to avoid spurious operation of the device.
+ */
+#define SC16IS7XX_TCR_RX_HALT(words)	((((words) / 4) & 0x0f) << 0)
+#define SC16IS7XX_TCR_RX_RESUME(words)	((((words) / 4) & 0x0f) << 4)
+
+/*
+ * TLR register bits
+ * If TLR[3:0] or TLR[7:4] are logical 0, the selectable trigger levels via the
+ * FIFO Control Register (FCR) are used for the transmit and receive FIFO
+ * trigger levels. Trigger levels from 4 characters to 60 characters are
+ * available with a granularity of four.
+ *
+ * When the trigger level setting in TLR is zero, the SC16IS740/750/760 uses the
+ * trigger level setting defined in FCR. If TLR has non-zero trigger level value
+ * the trigger level defined in FCR is discarded. This applies to both transmit
+ * FIFO and receive FIFO trigger level setting.
+ *
+ * When TLR is used for RX trigger level control, FCR[7:6] should be left at the
+ * default state, that is, '00'.
+ */
+#define SC16IS7XX_TLR_TX_TRIGGER(words)	((((words) / 4) & 0x0f) << 0)
+#define SC16IS7XX_TLR_RX_TRIGGER(words)	((((words) / 4) & 0x0f) << 4)
+
+/* IOControl register bits (Only 750/760) */
+#define SC16IS7XX_IOCONTROL_LATCH_BIT	(1 << 0) /* Enable input latching */
+#define SC16IS7XX_IOCONTROL_GPIO_BIT	(1 << 1) /* Enable GPIO[7:4] */
+#define SC16IS7XX_IOCONTROL_SRESET_BIT	(1 << 3) /* Software Reset */
+
+/* EFCR register bits */
+#define SC16IS7XX_EFCR_9BIT_MODE_BIT	(1 << 0) /* Enable 9-bit or Multidrop
+						  * mode (RS485) */
+#define SC16IS7XX_EFCR_RXDISABLE_BIT	(1 << 1) /* Disable receiver */
+#define SC16IS7XX_EFCR_TXDISABLE_BIT	(1 << 2) /* Disable transmitter */
+#define SC16IS7XX_EFCR_AUTO_RS485_BIT	(1 << 4) /* Auto RS485 RTS direction */
+#define SC16IS7XX_EFCR_RTS_INVERT_BIT	(1 << 5) /* RTS output inversion */
+#define SC16IS7XX_EFCR_IRDA_MODE_BIT	(1 << 7) /* IrDA mode
+						  * 0 = rate upto 115.2 kbit/s
+						  *   - Only 750/760
+						  * 1 = rate upto 1.152 Mbit/s
+						  *   - Only 760
+						  */
+
+/* EFR register bits */
+#define SC16IS7XX_EFR_AUTORTS_BIT	(1 << 6) /* Auto RTS flow ctrl enable */
+#define SC16IS7XX_EFR_AUTOCTS_BIT	(1 << 7) /* Auto CTS flow ctrl enable */
+#define SC16IS7XX_EFR_XOFF2_DETECT_BIT	(1 << 5) /* Enable Xoff2 detection */
+#define SC16IS7XX_EFR_ENABLE_BIT	(1 << 4) /* Enable enhanced functions
+						  * and writing to IER[7:4],
+						  * FCR[5:4], MCR[7:5]
+						  */
+#define SC16IS7XX_EFR_SWFLOW3_BIT	(1 << 3) /* SWFLOW bit 3 */
+#define SC16IS7XX_EFR_SWFLOW2_BIT	(1 << 2) /* SWFLOW bit 2
+						  *
+						  * SWFLOW bits 3 & 2 table:
+						  * 00 -> no transmitter flow
+						  *       control
+						  * 01 -> transmitter generates
+						  *       XON2 and XOFF2
+						  * 10 -> transmitter generates
+						  *       XON1 and XOFF1
+						  * 11 -> transmitter generates
+						  *       XON1, XON2, XOFF1 and
+						  *       XOFF2
+						  */
+#define SC16IS7XX_EFR_SWFLOW1_BIT	(1 << 1) /* SWFLOW bit 2 */
+#define SC16IS7XX_EFR_SWFLOW0_BIT	(1 << 0) /* SWFLOW bit 3
+						  *
+						  * SWFLOW bits 3 & 2 table:
+						  * 00 -> no received flow
+						  *       control
+						  * 01 -> receiver compares
+						  *       XON2 and XOFF2
+						  * 10 -> receiver compares
+						  *       XON1 and XOFF1
+						  * 11 -> receiver compares
+						  *       XON1, XON2, XOFF1 and
+						  *       XOFF2
+						  */
+
+/* Misc definitions */
+#define SC16IS7XX_FIFO_SIZE		(64)
+#define SC16IS7XX_REG_SHIFT		2
+
+struct sc16is7xx_devtype {
+	char	name[10];
+	int	nr_gpio;
+	int	nr_uart;
+};
+
+struct sc16is7xx_one {
+	struct uart_port		port;
+	struct work_struct		tx_work;
+	struct work_struct		md_work;
+
+	struct serial_rs485		rs485;
+};
+
+struct sc16is7xx_port {
+	struct uart_driver		uart;
+	struct sc16is7xx_devtype	*devtype;
+	struct regmap			*regmap;
+	struct mutex			mutex;
+	struct clk			*clk;
+#ifdef CONFIG_GPIOLIB
+	struct gpio_chip		gpio;
+#endif
+	unsigned char			buf[SC16IS7XX_FIFO_SIZE];
+	struct sc16is7xx_one		p[0];
+};
+
+#define to_sc16is7xx_one(p,e)	((container_of((p), struct sc16is7xx_one, e)))
+
+static u8 sc16is7xx_port_read(struct uart_port *port, u8 reg)
+{
+	struct sc16is7xx_port *s = dev_get_drvdata(port->dev);
+	unsigned int val = 0;
+
+	regmap_read(s->regmap,
+		    (reg << SC16IS7XX_REG_SHIFT) | port->line, &val);
+
+	return val;
+}
+
+static void sc16is7xx_port_write(struct uart_port *port, u8 reg, u8 val)
+{
+	struct sc16is7xx_port *s = dev_get_drvdata(port->dev);
+
+	regmap_write(s->regmap,
+		     (reg << SC16IS7XX_REG_SHIFT) | port->line, val);
+}
+
+static void sc16is7xx_port_update(struct uart_port *port, u8 reg,
+				  u8 mask, u8 val)
+{
+	struct sc16is7xx_port *s = dev_get_drvdata(port->dev);
+
+	regmap_update_bits(s->regmap,
+			   (reg << SC16IS7XX_REG_SHIFT) | port->line,
+			   mask, val);
+}
+
+
+static void sc16is7xx_power(struct uart_port *port, int on)
+{
+	sc16is7xx_port_update(port, SC16IS7XX_IER_REG,
+			      SC16IS7XX_IER_SLEEP_BIT,
+			      on ? 0 : SC16IS7XX_IER_SLEEP_BIT);
+}
+
+static const struct sc16is7xx_devtype sc16is74x_devtype = {
+	.name		= "SC16IS74X",
+	.nr_gpio	= 0,
+	.nr_uart	= 1,
+};
+
+static const struct sc16is7xx_devtype sc16is750_devtype = {
+	.name		= "SC16IS750",
+	.nr_gpio	= 8,
+	.nr_uart	= 1,
+};
+
+static const struct sc16is7xx_devtype sc16is752_devtype = {
+	.name		= "SC16IS752",
+	.nr_gpio	= 8,
+	.nr_uart	= 2,
+};
+
+static const struct sc16is7xx_devtype sc16is760_devtype = {
+	.name		= "SC16IS760",
+	.nr_gpio	= 8,
+	.nr_uart	= 1,
+};
+
+static const struct sc16is7xx_devtype sc16is762_devtype = {
+	.name		= "SC16IS762",
+	.nr_gpio	= 8,
+	.nr_uart	= 2,
+};
+
+static bool sc16is7xx_regmap_volatile(struct device *dev, unsigned int reg)
+{
+	switch (reg >> SC16IS7XX_REG_SHIFT) {
+	case SC16IS7XX_RHR_REG:
+	case SC16IS7XX_IIR_REG:
+	case SC16IS7XX_LSR_REG:
+	case SC16IS7XX_MSR_REG:
+	case SC16IS7XX_TXLVL_REG:
+	case SC16IS7XX_RXLVL_REG:
+	case SC16IS7XX_IOSTATE_REG:
+		return true;
+	default:
+		break;
+	}
+
+	return false;
+}
+
+static bool sc16is7xx_regmap_precious(struct device *dev, unsigned int reg)
+{
+	switch (reg >> SC16IS7XX_REG_SHIFT) {
+	case SC16IS7XX_RHR_REG:
+		return true;
+	default:
+		break;
+	}
+
+	return false;
+}
+
+static int sc16is7xx_set_baud(struct uart_port *port, int baud)
+{
+	struct sc16is7xx_port *s = dev_get_drvdata(port->dev);
+	u8 lcr;
+	u8 prescaler = 0;
+	unsigned long clk = port->uartclk, div = clk / 16 / baud;
+
+	if (div > 0xffff) {
+		prescaler = SC16IS7XX_MCR_CLKSEL_BIT;
+		div /= 4;
+	}
+
+	lcr = sc16is7xx_port_read(port, SC16IS7XX_LCR_REG);
+
+	/* Open the LCR divisors for configuration */
+	sc16is7xx_port_write(port, SC16IS7XX_LCR_REG,
+			     SC16IS7XX_LCR_CONF_MODE_B);
+
+	/* Enable enhanced features */
+	regcache_cache_bypass(s->regmap, true);
+	sc16is7xx_port_write(port, SC16IS7XX_EFR_REG,
+			     SC16IS7XX_EFR_ENABLE_BIT);
+	regcache_cache_bypass(s->regmap, false);
+
+	/* Put LCR back to the normal mode */
+	sc16is7xx_port_write(port, SC16IS7XX_LCR_REG, lcr);
+
+	sc16is7xx_port_update(port, SC16IS7XX_MCR_REG,
+			      SC16IS7XX_MCR_CLKSEL_BIT,
+			      prescaler);
+
+	/* Open the LCR divisors for configuration */
+	sc16is7xx_port_write(port, SC16IS7XX_LCR_REG,
+			     SC16IS7XX_LCR_CONF_MODE_A);
+
+	/* Write the new divisor */
+	regcache_cache_bypass(s->regmap, true);
+	sc16is7xx_port_write(port, SC16IS7XX_DLH_REG, div / 256);
+	sc16is7xx_port_write(port, SC16IS7XX_DLL_REG, div % 256);
+	regcache_cache_bypass(s->regmap, false);
+
+	/* Put LCR back to the normal mode */
+	sc16is7xx_port_write(port, SC16IS7XX_LCR_REG, lcr);
+
+	return DIV_ROUND_CLOSEST(clk / 16, div);
+}
+
+static void sc16is7xx_handle_rx(struct uart_port *port, unsigned int rxlen,
+				unsigned int iir)
+{
+	struct sc16is7xx_port *s = dev_get_drvdata(port->dev);
+	unsigned int lsr = 0, ch, flag, bytes_read, i;
+	bool read_lsr = (iir == SC16IS7XX_IIR_RLSE_SRC) ? true : false;
+
+	if (unlikely(rxlen >= sizeof(s->buf))) {
+		dev_warn_ratelimited(port->dev,
+				     "Port %i: Possible RX FIFO overrun: %d\n",
+				     port->line, rxlen);
+		port->icount.buf_overrun++;
+		/* Ensure sanity of RX level */
+		rxlen = sizeof(s->buf);
+	}
+
+	while (rxlen) {
+		/* Only read lsr if there are possible errors in FIFO */
+		if (read_lsr) {
+			lsr = sc16is7xx_port_read(port, SC16IS7XX_LSR_REG);
+			if (!(lsr & SC16IS7XX_LSR_FIFOE_BIT))
+				read_lsr = false; /* No errors left in FIFO */
+		} else
+			lsr = 0;
+
+		if (read_lsr) {
+			s->buf[0] = sc16is7xx_port_read(port, SC16IS7XX_RHR_REG);
+			bytes_read = 1;
+		} else {
+			regcache_cache_bypass(s->regmap, true);
+			regmap_raw_read(s->regmap, SC16IS7XX_RHR_REG,
+					s->buf, rxlen);
+			regcache_cache_bypass(s->regmap, false);
+			bytes_read = rxlen;
+		}
+
+		lsr &= SC16IS7XX_LSR_BRK_ERROR_MASK;
+
+		port->icount.rx++;
+		flag = TTY_NORMAL;
+
+		if (unlikely(lsr)) {
+			if (lsr & SC16IS7XX_LSR_BI_BIT) {
+				port->icount.brk++;
+				if (uart_handle_break(port))
+					continue;
+			} else if (lsr & SC16IS7XX_LSR_PE_BIT)
+				port->icount.parity++;
+			else if (lsr & SC16IS7XX_LSR_FE_BIT)
+				port->icount.frame++;
+			else if (lsr & SC16IS7XX_LSR_OE_BIT)
+				port->icount.overrun++;
+
+			lsr &= port->read_status_mask;
+			if (lsr & SC16IS7XX_LSR_BI_BIT)
+				flag = TTY_BREAK;
+			else if (lsr & SC16IS7XX_LSR_PE_BIT)
+				flag = TTY_PARITY;
+			else if (lsr & SC16IS7XX_LSR_FE_BIT)
+				flag = TTY_FRAME;
+			else if (lsr & SC16IS7XX_LSR_OE_BIT)
+				flag = TTY_OVERRUN;
+		}
+
+		for (i = 0; i < bytes_read; ++i) {
+			ch = s->buf[i];
+			if (uart_handle_sysrq_char(port, ch))
+				continue;
+
+			if (lsr & port->ignore_status_mask)
+				continue;
+
+			uart_insert_char(port, lsr, SC16IS7XX_LSR_OE_BIT, ch,
+					 flag);
+		}
+		rxlen -= bytes_read;
+	}
+
+	tty_flip_buffer_push(&port->state->port);
+}
+
+static void sc16is7xx_handle_tx(struct uart_port *port)
+{
+	struct sc16is7xx_port *s = dev_get_drvdata(port->dev);
+	struct circ_buf *xmit = &port->state->xmit;
+	unsigned int txlen, to_send, i;
+
+	if (unlikely(port->x_char)) {
+		sc16is7xx_port_write(port, SC16IS7XX_THR_REG, port->x_char);
+		port->icount.tx++;
+		port->x_char = 0;
+		return;
+	}
+
+	if (uart_circ_empty(xmit) || uart_tx_stopped(port))
+		return;
+
+	/* Get length of data pending in circular buffer */
+	to_send = uart_circ_chars_pending(xmit);
+	if (likely(to_send)) {
+		/* Limit to size of TX FIFO */
+		txlen = sc16is7xx_port_read(port, SC16IS7XX_TXLVL_REG);
+		to_send = (to_send > txlen) ? txlen : to_send;
+
+		/* Add data to send */
+		port->icount.tx += to_send;
+
+		/* Convert to linear buffer */
+		for (i = 0; i < to_send; ++i) {
+			s->buf[i] = xmit->buf[xmit->tail];
+			xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1);
+		}
+		regcache_cache_bypass(s->regmap, true);
+		regmap_raw_write(s->regmap, SC16IS7XX_THR_REG, s->buf, to_send);
+		regcache_cache_bypass(s->regmap, false);
+	}
+
+	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
+		uart_write_wakeup(port);
+}
+
+static void sc16is7xx_port_irq(struct sc16is7xx_port *s, int portno)
+{
+	struct uart_port *port = &s->p[portno].port;
+
+	do {
+		unsigned int iir, msr, rxlen;
+
+		iir = sc16is7xx_port_read(port, SC16IS7XX_IIR_REG);
+		if (iir & SC16IS7XX_IIR_NO_INT_BIT)
+			break;
+
+		iir &= SC16IS7XX_IIR_ID_MASK;
+
+		switch (iir) {
+		case SC16IS7XX_IIR_RDI_SRC:
+		case SC16IS7XX_IIR_RLSE_SRC:
+		case SC16IS7XX_IIR_RTOI_SRC:
+		case SC16IS7XX_IIR_XOFFI_SRC:
+			rxlen = sc16is7xx_port_read(port, SC16IS7XX_RXLVL_REG);
+			if (rxlen)
+				sc16is7xx_handle_rx(port, rxlen, iir);
+			break;
+
+		case SC16IS7XX_IIR_CTSRTS_SRC:
+			msr = sc16is7xx_port_read(port, SC16IS7XX_MSR_REG);
+			uart_handle_cts_change(port,
+					       !!(msr & SC16IS7XX_MSR_CTS_BIT));
+			break;
+		case SC16IS7XX_IIR_THRI_SRC:
+			mutex_lock(&s->mutex);
+			sc16is7xx_handle_tx(port);
+			mutex_unlock(&s->mutex);
+			break;
+		default:
+			dev_err_ratelimited(port->dev,
+					    "Port %i: Unexpected interrupt: %x",
+					    port->line, iir);
+			break;
+		}
+	} while (1);
+}
+
+static irqreturn_t sc16is7xx_ist(int irq, void *dev_id)
+{
+	struct sc16is7xx_port *s = (struct sc16is7xx_port *)dev_id;
+	int i;
+
+	for (i = 0; i < s->uart.nr; ++i)
+		sc16is7xx_port_irq(s, i);
+
+	return IRQ_HANDLED;
+}
+
+static void sc16is7xx_wq_proc(struct work_struct *ws)
+{
+	struct sc16is7xx_one *one = to_sc16is7xx_one(ws, tx_work);
+	struct sc16is7xx_port *s = dev_get_drvdata(one->port.dev);
+
+	mutex_lock(&s->mutex);
+	sc16is7xx_handle_tx(&one->port);
+	mutex_unlock(&s->mutex);
+}
+
+static void sc16is7xx_stop_tx(struct uart_port* port)
+{
+	struct sc16is7xx_one *one = to_sc16is7xx_one(port, port);
+	struct circ_buf *xmit = &one->port.state->xmit;
+
+	/* handle rs485 */
+	if (one->rs485.flags & SER_RS485_ENABLED) {
+		/* do nothing if current tx not yet completed */
+		int lsr = sc16is7xx_port_read(port, SC16IS7XX_LSR_REG);
+		if (!(lsr & SC16IS7XX_LSR_TEMT_BIT))
+			return;
+
+		if (uart_circ_empty(xmit) &&
+		    (one->rs485.delay_rts_after_send > 0))
+			mdelay(one->rs485.delay_rts_after_send);
+	}
+
+	sc16is7xx_port_update(port, SC16IS7XX_IER_REG,
+			      SC16IS7XX_IER_THRI_BIT,
+			      0);
+}
+
+static void sc16is7xx_stop_rx(struct uart_port* port)
+{
+	struct sc16is7xx_one *one = to_sc16is7xx_one(port, port);
+
+	one->port.read_status_mask &= ~SC16IS7XX_LSR_DR_BIT;
+	sc16is7xx_port_update(port, SC16IS7XX_IER_REG,
+			      SC16IS7XX_LSR_DR_BIT,
+			      0);
+}
+
+static void sc16is7xx_start_tx(struct uart_port *port)
+{
+	struct sc16is7xx_one *one = to_sc16is7xx_one(port, port);
+
+	/* handle rs485 */
+	if ((one->rs485.flags & SER_RS485_ENABLED) &&
+	    (one->rs485.delay_rts_before_send > 0)) {
+		mdelay(one->rs485.delay_rts_before_send);
+	}
+
+	if (!work_pending(&one->tx_work))
+		schedule_work(&one->tx_work);
+}
+
+static unsigned int sc16is7xx_tx_empty(struct uart_port *port)
+{
+	unsigned int lvl, lsr;
+
+	lvl = sc16is7xx_port_read(port, SC16IS7XX_TXLVL_REG);
+	lsr = sc16is7xx_port_read(port, SC16IS7XX_LSR_REG);
+
+	return ((lsr & SC16IS7XX_LSR_THRE_BIT) && !lvl) ? TIOCSER_TEMT : 0;
+}
+
+static unsigned int sc16is7xx_get_mctrl(struct uart_port *port)
+{
+	/* DCD and DSR are not wired and CTS/RTS is handled automatically
+	 * so just indicate DSR and CAR asserted
+	 */
+	return TIOCM_DSR | TIOCM_CAR;
+}
+
+static void sc16is7xx_md_proc(struct work_struct *ws)
+{
+	struct sc16is7xx_one *one = to_sc16is7xx_one(ws, md_work);
+
+	sc16is7xx_port_update(&one->port, SC16IS7XX_MCR_REG,
+			      SC16IS7XX_MCR_LOOP_BIT,
+			      (one->port.mctrl & TIOCM_LOOP) ?
+				      SC16IS7XX_MCR_LOOP_BIT : 0);
+}
+
+static void sc16is7xx_set_mctrl(struct uart_port *port, unsigned int mctrl)
+{
+	struct sc16is7xx_one *one = to_sc16is7xx_one(port, port);
+
+	schedule_work(&one->md_work);
+}
+
+static void sc16is7xx_break_ctl(struct uart_port *port, int break_state)
+{
+	sc16is7xx_port_update(port, SC16IS7XX_LCR_REG,
+			      SC16IS7XX_LCR_TXBREAK_BIT,
+			      break_state ? SC16IS7XX_LCR_TXBREAK_BIT : 0);
+}
+
+static void sc16is7xx_set_termios(struct uart_port *port,
+				  struct ktermios *termios,
+				  struct ktermios *old)
+{
+	struct sc16is7xx_port *s = dev_get_drvdata(port->dev);
+	unsigned int lcr, flow = 0;
+	int baud;
+
+	/* Mask termios capabilities we don't support */
+	termios->c_cflag &= ~CMSPAR;
+
+	/* Word size */
+	switch (termios->c_cflag & CSIZE) {
+	case CS5:
+		lcr = SC16IS7XX_LCR_WORD_LEN_5;
+		break;
+	case CS6:
+		lcr = SC16IS7XX_LCR_WORD_LEN_6;
+		break;
+	case CS7:
+		lcr = SC16IS7XX_LCR_WORD_LEN_7;
+		break;
+	case CS8:
+		lcr = SC16IS7XX_LCR_WORD_LEN_8;
+		break;
+	default:
+		lcr = SC16IS7XX_LCR_WORD_LEN_8;
+		termios->c_cflag &= ~CSIZE;
+		termios->c_cflag |= CS8;
+		break;
+	}
+
+	/* Parity */
+	if (termios->c_cflag & PARENB) {
+		lcr |= SC16IS7XX_LCR_PARITY_BIT;
+		if (!(termios->c_cflag & PARODD))
+			lcr |= SC16IS7XX_LCR_EVENPARITY_BIT;
+	}
+
+	/* Stop bits */
+	if (termios->c_cflag & CSTOPB)
+		lcr |= SC16IS7XX_LCR_STOPLEN_BIT; /* 2 stops */
+
+	/* Set read status mask */
+	port->read_status_mask = SC16IS7XX_LSR_OE_BIT;
+	if (termios->c_iflag & INPCK)
+		port->read_status_mask |= SC16IS7XX_LSR_PE_BIT |
+					  SC16IS7XX_LSR_FE_BIT;
+	if (termios->c_iflag & (BRKINT | PARMRK))
+		port->read_status_mask |= SC16IS7XX_LSR_BI_BIT;
+
+	/* Set status ignore mask */
+	port->ignore_status_mask = 0;
+	if (termios->c_iflag & IGNBRK)
+		port->ignore_status_mask |= SC16IS7XX_LSR_BI_BIT;
+	if (!(termios->c_cflag & CREAD))
+		port->ignore_status_mask |= SC16IS7XX_LSR_BRK_ERROR_MASK;
+
+	sc16is7xx_port_write(port, SC16IS7XX_LCR_REG,
+			     SC16IS7XX_LCR_CONF_MODE_B);
+
+	/* Configure flow control */
+	regcache_cache_bypass(s->regmap, true);
+	sc16is7xx_port_write(port, SC16IS7XX_XON1_REG, termios->c_cc[VSTART]);
+	sc16is7xx_port_write(port, SC16IS7XX_XOFF1_REG, termios->c_cc[VSTOP]);
+	if (termios->c_cflag & CRTSCTS)
+		flow |= SC16IS7XX_EFR_AUTOCTS_BIT |
+			SC16IS7XX_EFR_AUTORTS_BIT;
+	if (termios->c_iflag & IXON)
+		flow |= SC16IS7XX_EFR_SWFLOW3_BIT;
+	if (termios->c_iflag & IXOFF)
+		flow |= SC16IS7XX_EFR_SWFLOW1_BIT;
+
+	sc16is7xx_port_write(port, SC16IS7XX_EFR_REG, flow);
+	regcache_cache_bypass(s->regmap, false);
+
+	/* Update LCR register */
+	sc16is7xx_port_write(port, SC16IS7XX_LCR_REG, lcr);
+
+	/* Get baud rate generator configuration */
+	baud = uart_get_baud_rate(port, termios, old,
+				  port->uartclk / 16 / 4 / 0xffff,
+				  port->uartclk / 16);
+
+	/* Setup baudrate generator */
+	baud = sc16is7xx_set_baud(port, baud);
+
+	/* Update timeout according to new baud rate */
+	uart_update_timeout(port, termios->c_cflag, baud);
+}
+
+#if defined(TIOCSRS485) && defined(TIOCGRS485)
+static void sc16is7xx_config_rs485(struct uart_port *port,
+				   struct serial_rs485 *rs485)
+{
+	struct sc16is7xx_one *one = to_sc16is7xx_one(port, port);
+
+	one->rs485 = *rs485;
+
+	if (one->rs485.flags & SER_RS485_ENABLED) {
+		sc16is7xx_port_update(port, SC16IS7XX_EFCR_REG,
+				      SC16IS7XX_EFCR_AUTO_RS485_BIT,
+				      SC16IS7XX_EFCR_AUTO_RS485_BIT);
+	} else {
+		sc16is7xx_port_update(port, SC16IS7XX_EFCR_REG,
+				      SC16IS7XX_EFCR_AUTO_RS485_BIT,
+				      0);
+	}
+}
+#endif
+
+static int sc16is7xx_ioctl(struct uart_port *port, unsigned int cmd,
+			   unsigned long arg)
+{
+#if defined(TIOCSRS485) && defined(TIOCGRS485)
+	struct serial_rs485 rs485;
+
+	switch (cmd) {
+	case TIOCSRS485:
+		if (copy_from_user(&rs485, (void __user *)arg, sizeof(rs485)))
+			return -EFAULT;
+
+		sc16is7xx_config_rs485(port, &rs485);
+		return 0;
+	case TIOCGRS485:
+		if (copy_to_user((void __user *)arg,
+				 &(to_sc16is7xx_one(port, port)->rs485),
+				 sizeof(rs485)))
+			return -EFAULT;
+		return 0;
+	default:
+		break;
+	}
+#endif
+
+	return -ENOIOCTLCMD;
+}
+
+static int sc16is7xx_startup(struct uart_port *port)
+{
+	struct sc16is7xx_port *s = dev_get_drvdata(port->dev);
+	unsigned int val;
+
+	sc16is7xx_power(port, 1);
+
+	/* Reset FIFOs*/
+	val = SC16IS7XX_FCR_RXRESET_BIT | SC16IS7XX_FCR_TXRESET_BIT;
+	sc16is7xx_port_write(port, SC16IS7XX_FCR_REG, val);
+	udelay(5);
+	sc16is7xx_port_write(port, SC16IS7XX_FCR_REG,
+			     SC16IS7XX_FCR_FIFO_BIT);
+
+	/* Enable EFR */
+	sc16is7xx_port_write(port, SC16IS7XX_LCR_REG,
+			     SC16IS7XX_LCR_CONF_MODE_B);
+
+	regcache_cache_bypass(s->regmap, true);
+
+	/* Enable write access to enhanced features and internal clock div */
+	sc16is7xx_port_write(port, SC16IS7XX_EFR_REG,
+			     SC16IS7XX_EFR_ENABLE_BIT);
+
+	/* Enable TCR/TLR */
+	sc16is7xx_port_update(port, SC16IS7XX_MCR_REG,
+			      SC16IS7XX_MCR_TCRTLR_BIT,
+			      SC16IS7XX_MCR_TCRTLR_BIT);
+
+	/* Configure flow control levels */
+	/* Flow control halt level 48, resume level 24 */
+	sc16is7xx_port_write(port, SC16IS7XX_TCR_REG,
+			     SC16IS7XX_TCR_RX_RESUME(24) |
+			     SC16IS7XX_TCR_RX_HALT(48));
+
+	regcache_cache_bypass(s->regmap, false);
+
+	/* Now, initialize the UART */
+	sc16is7xx_port_write(port, SC16IS7XX_LCR_REG, SC16IS7XX_LCR_WORD_LEN_8);
+
+	/* Enable the Rx and Tx FIFO */
+	sc16is7xx_port_update(port, SC16IS7XX_EFCR_REG,
+			      SC16IS7XX_EFCR_RXDISABLE_BIT |
+			      SC16IS7XX_EFCR_TXDISABLE_BIT,
+			      0);
+
+	/* Enable RX, TX, CTS change interrupts */
+	val = SC16IS7XX_IER_RDI_BIT | SC16IS7XX_IER_THRI_BIT |
+	      SC16IS7XX_IER_CTSI_BIT;
+	sc16is7xx_port_write(port, SC16IS7XX_IER_REG, val);
+
+	return 0;
+}
+
+static void sc16is7xx_shutdown(struct uart_port *port)
+{
+	/* Disable all interrupts */
+	sc16is7xx_port_write(port, SC16IS7XX_IER_REG, 0);
+	/* Disable TX/RX */
+	sc16is7xx_port_write(port, SC16IS7XX_EFCR_REG,
+			     SC16IS7XX_EFCR_RXDISABLE_BIT |
+			     SC16IS7XX_EFCR_TXDISABLE_BIT);
+
+	sc16is7xx_power(port, 0);
+}
+
+static const char *sc16is7xx_type(struct uart_port *port)
+{
+	struct sc16is7xx_port *s = dev_get_drvdata(port->dev);
+
+	return (port->type == PORT_SC16IS7XX) ? s->devtype->name : NULL;
+}
+
+static int sc16is7xx_request_port(struct uart_port *port)
+{
+	/* Do nothing */
+	return 0;
+}
+
+static void sc16is7xx_config_port(struct uart_port *port, int flags)
+{
+	if (flags & UART_CONFIG_TYPE)
+		port->type = PORT_SC16IS7XX;
+}
+
+static int sc16is7xx_verify_port(struct uart_port *port,
+				 struct serial_struct *s)
+{
+	if ((s->type != PORT_UNKNOWN) && (s->type != PORT_SC16IS7XX))
+		return -EINVAL;
+	if (s->irq != port->irq)
+		return -EINVAL;
+
+	return 0;
+}
+
+static void sc16is7xx_pm(struct uart_port *port, unsigned int state,
+			 unsigned int oldstate)
+{
+	sc16is7xx_power(port, (state == UART_PM_STATE_ON) ? 1 : 0);
+}
+
+static void sc16is7xx_null_void(struct uart_port *port)
+{
+	/* Do nothing */
+}
+
+static const struct uart_ops sc16is7xx_ops = {
+	.tx_empty	= sc16is7xx_tx_empty,
+	.set_mctrl	= sc16is7xx_set_mctrl,
+	.get_mctrl	= sc16is7xx_get_mctrl,
+	.stop_tx	= sc16is7xx_stop_tx,
+	.start_tx	= sc16is7xx_start_tx,
+	.stop_rx	= sc16is7xx_stop_rx,
+	.enable_ms	= sc16is7xx_null_void,
+	.break_ctl	= sc16is7xx_break_ctl,
+	.startup	= sc16is7xx_startup,
+	.shutdown	= sc16is7xx_shutdown,
+	.set_termios	= sc16is7xx_set_termios,
+	.type		= sc16is7xx_type,
+	.request_port	= sc16is7xx_request_port,
+	.release_port	= sc16is7xx_null_void,
+	.config_port	= sc16is7xx_config_port,
+	.verify_port	= sc16is7xx_verify_port,
+	.ioctl		= sc16is7xx_ioctl,
+	.pm		= sc16is7xx_pm,
+};
+
+#ifdef CONFIG_GPIOLIB
+static int sc16is7xx_gpio_get(struct gpio_chip *chip, unsigned offset)
+{
+	unsigned int val;
+	struct sc16is7xx_port *s = container_of(chip, struct sc16is7xx_port,
+						gpio);
+	struct uart_port *port = &s->p[0].port;
+
+	val = sc16is7xx_port_read(port, SC16IS7XX_IOSTATE_REG);
+
+	return !!(val & BIT(offset));
+}
+
+static void sc16is7xx_gpio_set(struct gpio_chip *chip, unsigned offset, int val)
+{
+	struct sc16is7xx_port *s = container_of(chip, struct sc16is7xx_port,
+						gpio);
+	struct uart_port *port = &s->p[0].port;
+
+	sc16is7xx_port_update(port, SC16IS7XX_IOSTATE_REG, BIT(offset),
+			      val ? BIT(offset) : 0);
+}
+
+static int sc16is7xx_gpio_direction_input(struct gpio_chip *chip,
+					  unsigned offset)
+{
+	struct sc16is7xx_port *s = container_of(chip, struct sc16is7xx_port,
+						gpio);
+	struct uart_port *port = &s->p[0].port;
+
+	sc16is7xx_port_update(port, SC16IS7XX_IODIR_REG, BIT(offset), 0);
+
+	return 0;
+}
+
+static int sc16is7xx_gpio_direction_output(struct gpio_chip *chip,
+					   unsigned offset, int val)
+{
+	struct sc16is7xx_port *s = container_of(chip, struct sc16is7xx_port,
+						gpio);
+	struct uart_port *port = &s->p[0].port;
+
+	sc16is7xx_port_update(port, SC16IS7XX_IOSTATE_REG, BIT(offset),
+			      val ? BIT(offset) : 0);
+	sc16is7xx_port_update(port, SC16IS7XX_IODIR_REG, BIT(offset),
+			      BIT(offset));
+
+	return 0;
+}
+#endif
+
+static int sc16is7xx_probe(struct device *dev,
+			   struct sc16is7xx_devtype *devtype,
+			   struct regmap *regmap, int irq, unsigned long flags)
+{
+	unsigned long freq, *pfreq = dev_get_platdata(dev);
+	struct clk *clk;
+	int i, ret;
+	struct sc16is7xx_port *s;
+
+	if (IS_ERR(regmap))
+		return PTR_ERR(regmap);
+
+	/* Alloc port structure */
+	s = devm_kzalloc(dev, sizeof(*s) +
+			 sizeof(struct sc16is7xx_one) * devtype->nr_uart,
+			 GFP_KERNEL);
+	if (!s) {
+		dev_err(dev, "Error allocating port structure\n");
+		return -ENOMEM;
+	}
+
+	clk = devm_clk_get(dev, NULL);
+	if (IS_ERR(clk)) {
+		if (pfreq)
+			freq = *pfreq;
+		else
+			return PTR_ERR(clk);
+	} else {
+		freq = clk_get_rate(clk);
+	}
+
+	s->regmap = regmap;
+	s->devtype = devtype;
+	dev_set_drvdata(dev, s);
+
+	/* Register UART driver */
+	s->uart.owner		= THIS_MODULE;
+	s->uart.dev_name	= "ttySC";
+	s->uart.nr		= devtype->nr_uart;
+	ret = uart_register_driver(&s->uart);
+	if (ret) {
+		dev_err(dev, "Registering UART driver failed\n");
+		goto out_clk;
+	}
+
+#ifdef CONFIG_GPIOLIB
+	if (devtype->nr_gpio) {
+		/* Setup GPIO cotroller */
+		s->gpio.owner		 = THIS_MODULE;
+		s->gpio.dev		 = dev;
+		s->gpio.label		 = dev_name(dev);
+		s->gpio.direction_input	 = sc16is7xx_gpio_direction_input;
+		s->gpio.get		 = sc16is7xx_gpio_get;
+		s->gpio.direction_output = sc16is7xx_gpio_direction_output;
+		s->gpio.set		 = sc16is7xx_gpio_set;
+		s->gpio.base		 = -1;
+		s->gpio.ngpio		 = devtype->nr_gpio;
+		s->gpio.can_sleep	 = 1;
+		ret = gpiochip_add(&s->gpio);
+		if (ret)
+			goto out_uart;
+	}
+#endif
+
+	mutex_init(&s->mutex);
+
+	for (i = 0; i < devtype->nr_uart; ++i) {
+		/* Initialize port data */
+		s->p[i].port.line	= i;
+		s->p[i].port.dev	= dev;
+		s->p[i].port.irq	= irq;
+		s->p[i].port.type	= PORT_SC16IS7XX;
+		s->p[i].port.fifosize	= SC16IS7XX_FIFO_SIZE;
+		s->p[i].port.flags	= UPF_FIXED_TYPE | UPF_LOW_LATENCY;
+		s->p[i].port.iotype	= UPIO_PORT;
+		s->p[i].port.uartclk	= freq;
+		s->p[i].port.ops	= &sc16is7xx_ops;
+		/* Disable all interrupts */
+		sc16is7xx_port_write(&s->p[i].port, SC16IS7XX_IER_REG, 0);
+		/* Disable TX/RX */
+		sc16is7xx_port_write(&s->p[i].port, SC16IS7XX_EFCR_REG,
+				     SC16IS7XX_EFCR_RXDISABLE_BIT |
+				     SC16IS7XX_EFCR_TXDISABLE_BIT);
+		/* Initialize queue for start TX */
+		INIT_WORK(&s->p[i].tx_work, sc16is7xx_wq_proc);
+		/* Initialize queue for changing mode */
+		INIT_WORK(&s->p[i].md_work, sc16is7xx_md_proc);
+		/* Register port */
+		uart_add_one_port(&s->uart, &s->p[i].port);
+		/* Go to suspend mode */
+		sc16is7xx_power(&s->p[i].port, 0);
+	}
+
+	/* Setup interrupt */
+	ret = devm_request_threaded_irq(dev, irq, NULL, sc16is7xx_ist,
+					IRQF_ONESHOT | flags, dev_name(dev), s);
+	if (!ret)
+		return 0;
+
+	mutex_destroy(&s->mutex);
+
+#ifdef CONFIG_GPIOLIB
+	if (devtype->nr_gpio)
+		WARN_ON(gpiochip_remove(&s->gpio));
+
+out_uart:
+#endif
+	uart_unregister_driver(&s->uart);
+
+out_clk:
+	if (!IS_ERR(s->clk))
+		clk_disable_unprepare(s->clk);
+
+	return ret;
+}
+
+static int sc16is7xx_remove(struct device *dev)
+{
+	struct sc16is7xx_port *s = dev_get_drvdata(dev);
+	int i, ret = 0;
+
+#ifdef CONFIG_GPIOLIB
+	if (s->devtype->nr_gpio) {
+		ret = gpiochip_remove(&s->gpio);
+		if (ret)
+			return ret;
+	}
+#endif
+
+	for (i = 0; i < s->uart.nr; i++) {
+		cancel_work_sync(&s->p[i].tx_work);
+		cancel_work_sync(&s->p[i].md_work);
+		uart_remove_one_port(&s->uart, &s->p[i].port);
+		sc16is7xx_power(&s->p[i].port, 0);
+	}
+
+	mutex_destroy(&s->mutex);
+	uart_unregister_driver(&s->uart);
+	if (!IS_ERR(s->clk))
+		clk_disable_unprepare(s->clk);
+
+	return ret;
+}
+
+static const struct of_device_id __maybe_unused sc16is7xx_dt_ids[] = {
+	{ .compatible = "nxp,sc16is740",	.data = &sc16is74x_devtype, },
+	{ .compatible = "nxp,sc16is741",	.data = &sc16is74x_devtype, },
+	{ .compatible = "nxp,sc16is750",	.data = &sc16is750_devtype, },
+	{ .compatible = "nxp,sc16is752",	.data = &sc16is752_devtype, },
+	{ .compatible = "nxp,sc16is760",	.data = &sc16is760_devtype, },
+	{ .compatible = "nxp,sc16is762",	.data = &sc16is762_devtype, },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, sc16is7xx_dt_ids);
+
+static struct regmap_config regcfg = {
+	.reg_bits = 7,
+	.pad_bits = 1,
+	.val_bits = 8,
+	.cache_type = REGCACHE_RBTREE,
+	.volatile_reg = sc16is7xx_regmap_volatile,
+	.precious_reg = sc16is7xx_regmap_precious,
+};
+
+static int sc16is7xx_i2c_probe(struct i2c_client *i2c,
+			       const struct i2c_device_id *id)
+{
+	struct sc16is7xx_devtype *devtype;
+	unsigned long flags = 0;
+	struct regmap *regmap;
+
+	if (i2c->dev.of_node) {
+		const struct of_device_id *of_id =
+				of_match_device(sc16is7xx_dt_ids, &i2c->dev);
+
+		devtype = (struct sc16is7xx_devtype *)of_id->data;
+	} else {
+		devtype = (struct sc16is7xx_devtype *)id->driver_data;
+		flags = IRQF_TRIGGER_FALLING;
+	}
+
+	regcfg.max_register = (0xf << SC16IS7XX_REG_SHIFT) |
+			      (devtype->nr_uart - 1);
+	regmap = devm_regmap_init_i2c(i2c, &regcfg);
+
+	return sc16is7xx_probe(&i2c->dev, devtype, regmap, i2c->irq, flags);
+}
+
+static int sc16is7xx_i2c_remove(struct i2c_client *client)
+{
+	return sc16is7xx_remove(&client->dev);
+}
+
+static const struct i2c_device_id sc16is7xx_i2c_id_table[] = {
+	{ "sc16is74x",	(kernel_ulong_t)&sc16is74x_devtype, },
+	{ "sc16is750",	(kernel_ulong_t)&sc16is750_devtype, },
+	{ "sc16is752",	(kernel_ulong_t)&sc16is752_devtype, },
+	{ "sc16is760",	(kernel_ulong_t)&sc16is760_devtype, },
+	{ "sc16is762",	(kernel_ulong_t)&sc16is762_devtype, },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, sc16is7xx_i2c_id_table);
+
+static struct i2c_driver sc16is7xx_i2c_uart_driver = {
+	.driver = {
+		.name		= SC16IS7XX_NAME,
+		.owner		= THIS_MODULE,
+		.of_match_table	= of_match_ptr(sc16is7xx_dt_ids),
+	},
+	.probe		= sc16is7xx_i2c_probe,
+	.remove		= sc16is7xx_i2c_remove,
+	.id_table	= sc16is7xx_i2c_id_table,
+};
+module_i2c_driver(sc16is7xx_i2c_uart_driver);
+MODULE_ALIAS("i2c:sc16is7xx");
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Jon Ringle <jringle@gridpoint.com>");
+MODULE_DESCRIPTION("SC16IS7XX serial driver");
diff --git a/drivers/tty/serial/sccnxp.c b/drivers/tty/serial/sccnxp.c
index a447f71538e..5443b46345e 100644
--- a/drivers/tty/serial/sccnxp.c
+++ b/drivers/tty/serial/sccnxp.c
@@ -474,9 +474,7 @@ static void sccnxp_timer(unsigned long data)
 	sccnxp_handle_events(s);
 	spin_unlock_irqrestore(&s->lock, flags);
 
-	if (!timer_pending(&s->timer))
-		mod_timer(&s->timer, jiffies +
-			  usecs_to_jiffies(s->pdata.poll_time_us));
+	mod_timer(&s->timer, jiffies + usecs_to_jiffies(s->pdata.poll_time_us));
 }
 
 static irqreturn_t sccnxp_ist(int irq, void *dev_id)
@@ -674,6 +672,8 @@ static void sccnxp_set_termios(struct uart_port *port,
 	port->ignore_status_mask = 0;
 	if (termios->c_iflag & IGNBRK)
 		port->ignore_status_mask |= SR_BRK;
+	if (termios->c_iflag & IGNPAR)
+		port->ignore_status_mask |= SR_PE;
 	if (!(termios->c_cflag & CREAD))
 		port->ignore_status_mask |= SR_PE | SR_OVR | SR_FE | SR_BRK;
 
diff --git a/drivers/tty/serial/serial_core.c b/drivers/tty/serial/serial_core.c
index 2cf5649a6dc..fbf6c5ad222 100644
--- a/drivers/tty/serial/serial_core.c
+++ b/drivers/tty/serial/serial_core.c
@@ -89,8 +89,7 @@ static void __uart_start(struct tty_struct *tty)
 	struct uart_state *state = tty->driver_data;
 	struct uart_port *port = state->uart_port;
 
-	if (!uart_circ_empty(&state->xmit) && state->xmit.buf &&
-	    !tty->stopped && !tty->hw_stopped)
+	if (!tty->stopped && !tty->hw_stopped)
 		port->ops->start_tx(port);
 }
 
@@ -138,6 +137,11 @@ static int uart_port_startup(struct tty_struct *tty, struct uart_state *state,
 		return 1;
 
 	/*
+	 * Make sure the device is in D0 state.
+	 */
+	uart_change_pm(state, UART_PM_STATE_ON);
+
+	/*
 	 * Initialise and allocate the transmit and temporary
 	 * buffer.
 	 */
@@ -170,8 +174,12 @@ static int uart_port_startup(struct tty_struct *tty, struct uart_state *state,
 			if (tty->termios.c_cflag & CBAUD)
 				uart_set_mctrl(uport, TIOCM_RTS | TIOCM_DTR);
 		}
-
-		if (tty_port_cts_enabled(port)) {
+		/*
+		 * if hw support flow control without software intervention,
+		 * then skip the below check
+		 */
+		if (tty_port_cts_enabled(port) &&
+		    !(uport->flags & UPF_HARD_FLOW)) {
 			spin_lock_irq(&uport->lock);
 			if (!(uport->ops->get_mctrl(uport) & TIOCM_CTS))
 				tty->hw_stopped = 1;
@@ -826,25 +834,29 @@ static int uart_set_info(struct tty_struct *tty, struct tty_port *port,
 		 * If we fail to request resources for the
 		 * new port, try to restore the old settings.
 		 */
-		if (retval && old_type != PORT_UNKNOWN) {
+		if (retval) {
 			uport->iobase = old_iobase;
 			uport->type = old_type;
 			uport->hub6 = old_hub6;
 			uport->iotype = old_iotype;
 			uport->regshift = old_shift;
 			uport->mapbase = old_mapbase;
-			retval = uport->ops->request_port(uport);
-			/*
-			 * If we failed to restore the old settings,
-			 * we fail like this.
-			 */
-			if (retval)
-				uport->type = PORT_UNKNOWN;
 
-			/*
-			 * We failed anyway.
-			 */
-			retval = -EBUSY;
+			if (old_type != PORT_UNKNOWN) {
+				retval = uport->ops->request_port(uport);
+				/*
+				 * If we failed to restore the old settings,
+				 * we fail like this.
+				 */
+				if (retval)
+					uport->type = PORT_UNKNOWN;
+
+				/*
+				 * We failed anyway.
+				 */
+				retval = -EBUSY;
+			}
+
 			/* Added to return the correct error -Ram Gupta */
 			goto exit;
 		}
@@ -1452,6 +1464,8 @@ static void uart_hangup(struct tty_struct *tty)
 		clear_bit(ASYNCB_NORMAL_ACTIVE, &port->flags);
 		spin_unlock_irqrestore(&port->lock, flags);
 		tty_port_tty_set(port, NULL);
+		if (!uart_console(state->uart_port))
+			uart_change_pm(state, UART_PM_STATE_OFF);
 		wake_up_interruptible(&port->open_wait);
 		wake_up_interruptible(&port->delta_msr_wait);
 	}
@@ -1570,12 +1584,6 @@ static int uart_open(struct tty_struct *tty, struct file *filp)
 	}
 
 	/*
-	 * Make sure the device is in D0 state.
-	 */
-	if (port->count == 1)
-		uart_change_pm(state, UART_PM_STATE_ON);
-
-	/*
 	 * Start up the serial port.
 	 */
 	retval = uart_startup(tty, state, 0);
@@ -2235,6 +2243,9 @@ static void uart_poll_put_char(struct tty_driver *driver, int line, char ch)
 		return;
 
 	port = state->uart_port;
+
+	if (ch == '\n')
+		port->ops->poll_put_char(port, '\r');
 	port->ops->poll_put_char(port, ch);
 }
 #endif
@@ -2768,7 +2779,9 @@ void uart_handle_cts_change(struct uart_port *uport, unsigned int status)
 
 	uport->icount.cts++;
 
-	if (tty_port_cts_enabled(port)) {
+	/* skip below code if the hw flow control is supported */
+	if (tty_port_cts_enabled(port) &&
+	    !(uport->flags & UPF_HARD_FLOW)) {
 		if (tty->hw_stopped) {
 			if (status) {
 				tty->hw_stopped = 0;
diff --git a/drivers/tty/serial/serial_mctrl_gpio.c b/drivers/tty/serial/serial_mctrl_gpio.c
new file mode 100644
index 00000000000..bf9560ffe3f
--- /dev/null
+++ b/drivers/tty/serial/serial_mctrl_gpio.c
@@ -0,0 +1,143 @@
+/*
+ * Helpers for controlling modem lines via GPIO
+ *
+ * Copyright (C) 2014 Paratronic S.A.
+ *
+ * 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/err.h>
+#include <linux/device.h>
+#include <linux/gpio/consumer.h>
+#include <uapi/asm-generic/termios.h>
+
+#include "serial_mctrl_gpio.h"
+
+struct mctrl_gpios {
+	struct gpio_desc *gpio[UART_GPIO_MAX];
+};
+
+static const struct {
+	const char *name;
+	unsigned int mctrl;
+	bool dir_out;
+} mctrl_gpios_desc[UART_GPIO_MAX] = {
+	{ "cts", TIOCM_CTS, false, },
+	{ "dsr", TIOCM_DSR, false, },
+	{ "dcd", TIOCM_CD, false, },
+	{ "rng", TIOCM_RNG, false, },
+	{ "rts", TIOCM_RTS, true, },
+	{ "dtr", TIOCM_DTR, true, },
+	{ "out1", TIOCM_OUT1, true, },
+	{ "out2", TIOCM_OUT2, true, },
+};
+
+void mctrl_gpio_set(struct mctrl_gpios *gpios, unsigned int mctrl)
+{
+	enum mctrl_gpio_idx i;
+
+	if (IS_ERR_OR_NULL(gpios))
+		return;
+
+	for (i = 0; i < UART_GPIO_MAX; i++)
+		if (!IS_ERR_OR_NULL(gpios->gpio[i]) &&
+		    mctrl_gpios_desc[i].dir_out)
+			gpiod_set_value(gpios->gpio[i],
+					!!(mctrl & mctrl_gpios_desc[i].mctrl));
+}
+EXPORT_SYMBOL_GPL(mctrl_gpio_set);
+
+struct gpio_desc *mctrl_gpio_to_gpiod(struct mctrl_gpios *gpios,
+				      enum mctrl_gpio_idx gidx)
+{
+	if (!IS_ERR_OR_NULL(gpios) && !IS_ERR_OR_NULL(gpios->gpio[gidx]))
+		return gpios->gpio[gidx];
+	else
+		return NULL;
+}
+EXPORT_SYMBOL_GPL(mctrl_gpio_to_gpiod);
+
+unsigned int mctrl_gpio_get(struct mctrl_gpios *gpios, unsigned int *mctrl)
+{
+	enum mctrl_gpio_idx i;
+
+	/*
+	 * return it unchanged if the structure is not allocated
+	 */
+	if (IS_ERR_OR_NULL(gpios))
+		return *mctrl;
+
+	for (i = 0; i < UART_GPIO_MAX; i++) {
+		if (!IS_ERR_OR_NULL(gpios->gpio[i]) &&
+		    !mctrl_gpios_desc[i].dir_out) {
+			if (gpiod_get_value(gpios->gpio[i]))
+				*mctrl |= mctrl_gpios_desc[i].mctrl;
+			else
+				*mctrl &= ~mctrl_gpios_desc[i].mctrl;
+		}
+	}
+
+	return *mctrl;
+}
+EXPORT_SYMBOL_GPL(mctrl_gpio_get);
+
+struct mctrl_gpios *mctrl_gpio_init(struct device *dev, unsigned int idx)
+{
+	struct mctrl_gpios *gpios;
+	enum mctrl_gpio_idx i;
+	int err;
+
+	gpios = devm_kzalloc(dev, sizeof(*gpios), GFP_KERNEL);
+	if (!gpios)
+		return ERR_PTR(-ENOMEM);
+
+	for (i = 0; i < UART_GPIO_MAX; i++) {
+		gpios->gpio[i] = devm_gpiod_get_index(dev,
+						      mctrl_gpios_desc[i].name,
+						      idx);
+
+		/*
+		 * The GPIOs are maybe not all filled,
+		 * this is not an error.
+		 */
+		if (IS_ERR_OR_NULL(gpios->gpio[i]))
+			continue;
+
+		if (mctrl_gpios_desc[i].dir_out)
+			err = gpiod_direction_output(gpios->gpio[i], 0);
+		else
+			err = gpiod_direction_input(gpios->gpio[i]);
+		if (err) {
+			dev_dbg(dev, "Unable to set direction for %s GPIO",
+				mctrl_gpios_desc[i].name);
+			devm_gpiod_put(dev, gpios->gpio[i]);
+			gpios->gpio[i] = NULL;
+		}
+	}
+
+	return gpios;
+}
+EXPORT_SYMBOL_GPL(mctrl_gpio_init);
+
+void mctrl_gpio_free(struct device *dev, struct mctrl_gpios *gpios)
+{
+	enum mctrl_gpio_idx i;
+
+	if (IS_ERR_OR_NULL(gpios))
+		return;
+
+	for (i = 0; i < UART_GPIO_MAX; i++)
+		if (!IS_ERR_OR_NULL(gpios->gpio[i]))
+			devm_gpiod_put(dev, gpios->gpio[i]);
+	devm_kfree(dev, gpios);
+}
+EXPORT_SYMBOL_GPL(mctrl_gpio_free);
diff --git a/drivers/tty/serial/serial_mctrl_gpio.h b/drivers/tty/serial/serial_mctrl_gpio.h
new file mode 100644
index 00000000000..400ba0494a1
--- /dev/null
+++ b/drivers/tty/serial/serial_mctrl_gpio.h
@@ -0,0 +1,110 @@
+/*
+ * Helpers for controlling modem lines via GPIO
+ *
+ * Copyright (C) 2014 Paratronic S.A.
+ *
+ * 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 __SERIAL_MCTRL_GPIO__
+#define __SERIAL_MCTRL_GPIO__
+
+#include <linux/err.h>
+#include <linux/device.h>
+#include <linux/gpio/consumer.h>
+
+enum mctrl_gpio_idx {
+	UART_GPIO_CTS,
+	UART_GPIO_DSR,
+	UART_GPIO_DCD,
+	UART_GPIO_RNG,
+	UART_GPIO_RI = UART_GPIO_RNG,
+	UART_GPIO_RTS,
+	UART_GPIO_DTR,
+	UART_GPIO_OUT1,
+	UART_GPIO_OUT2,
+	UART_GPIO_MAX,
+};
+
+/*
+ * Opaque descriptor for modem lines controlled by GPIOs
+ */
+struct mctrl_gpios;
+
+#ifdef CONFIG_GPIOLIB
+
+/*
+ * Set state of the modem control output lines via GPIOs.
+ */
+void mctrl_gpio_set(struct mctrl_gpios *gpios, unsigned int mctrl);
+
+/*
+ * Get state of the modem control output lines from GPIOs.
+ * The mctrl flags are updated and returned.
+ */
+unsigned int mctrl_gpio_get(struct mctrl_gpios *gpios, unsigned int *mctrl);
+
+/*
+ * Returns the associated struct gpio_desc to the modem line gidx
+ */
+struct gpio_desc *mctrl_gpio_to_gpiod(struct mctrl_gpios *gpios,
+				      enum mctrl_gpio_idx gidx);
+
+/*
+ * Request and set direction of modem control lines GPIOs.
+ * devm_* functions are used, so there's no need to call mctrl_gpio_free().
+ * Returns a pointer to the allocated mctrl structure if ok, -ENOMEM on
+ * allocation error.
+ */
+struct mctrl_gpios *mctrl_gpio_init(struct device *dev, unsigned int idx);
+
+/*
+ * Free the mctrl_gpios structure.
+ * Normally, this function will not be called, as the GPIOs will
+ * be disposed of by the resource management code.
+ */
+void mctrl_gpio_free(struct device *dev, struct mctrl_gpios *gpios);
+
+#else /* GPIOLIB */
+
+static inline
+void mctrl_gpio_set(struct mctrl_gpios *gpios, unsigned int mctrl)
+{
+}
+
+static inline
+unsigned int mctrl_gpio_get(struct mctrl_gpios *gpios, unsigned int *mctrl)
+{
+	return *mctrl;
+}
+
+static inline
+struct gpio_desc *mctrl_gpio_to_gpiod(struct mctrl_gpios *gpios,
+				      enum mctrl_gpio_idx gidx)
+{
+	return ERR_PTR(-ENOSYS);
+}
+
+static inline
+struct mctrl_gpios *mctrl_gpio_init(struct device *dev, unsigned int idx)
+{
+	return ERR_PTR(-ENOSYS);
+}
+
+static inline
+void mctrl_gpio_free(struct device *dev, struct mctrl_gpios *gpios)
+{
+}
+
+#endif /* GPIOLIB */
+
+#endif
diff --git a/drivers/tty/serial/serial_txx9.c b/drivers/tty/serial/serial_txx9.c
index 90a080b1f9e..60f49b9d7e3 100644
--- a/drivers/tty/serial/serial_txx9.c
+++ b/drivers/tty/serial/serial_txx9.c
@@ -535,13 +535,8 @@ static void serial_txx9_put_poll_char(struct uart_port *port, unsigned char c)
 	wait_for_xmitr(up);
 	/*
 	 *	Send the character out.
-	 *	If a LF, also do CR...
 	 */
 	sio_out(up, TXX9_SITFIFO, c);
-	if (c == 10) {
-		wait_for_xmitr(up);
-		sio_out(up, TXX9_SITFIFO, 13);
-	}
 
 	/*
 	 *	Finally, wait for transmitter to become empty
diff --git a/drivers/tty/serial/sirfsoc_uart.c b/drivers/tty/serial/sirfsoc_uart.c
index 68b0fd4b9a6..1f2be48c92c 100644
--- a/drivers/tty/serial/sirfsoc_uart.c
+++ b/drivers/tty/serial/sirfsoc_uart.c
@@ -358,9 +358,11 @@ static irqreturn_t sirfsoc_uart_usp_cts_handler(int irq, void *dev_id)
 {
 	struct sirfsoc_uart_port *sirfport = (struct sirfsoc_uart_port *)dev_id;
 	struct uart_port *port = &sirfport->port;
+	spin_lock(&port->lock);
 	if (gpio_is_valid(sirfport->cts_gpio) && sirfport->ms_enabled)
 		uart_handle_cts_change(port,
 				!gpio_get_value(sirfport->cts_gpio));
+	spin_unlock(&port->lock);
 	return IRQ_HANDLED;
 }
 
@@ -428,10 +430,6 @@ sirfsoc_uart_pio_rx_chars(struct uart_port *port, unsigned int max_rx_count)
 	sirfport->rx_io_count += rx_count;
 	port->icount.rx += rx_count;
 
-	spin_unlock(&port->lock);
-	tty_flip_buffer_push(&port->state->port);
-	spin_lock(&port->lock);
-
 	return rx_count;
 }
 
@@ -465,6 +463,7 @@ static void sirfsoc_uart_tx_dma_complete_callback(void *param)
 	struct circ_buf *xmit = &port->state->xmit;
 	unsigned long flags;
 
+	spin_lock_irqsave(&port->lock, flags);
 	xmit->tail = (xmit->tail + sirfport->transfer_size) &
 				(UART_XMIT_SIZE - 1);
 	port->icount.tx += sirfport->transfer_size;
@@ -473,10 +472,9 @@ static void sirfsoc_uart_tx_dma_complete_callback(void *param)
 	if (sirfport->tx_dma_addr)
 		dma_unmap_single(port->dev, sirfport->tx_dma_addr,
 				sirfport->transfer_size, DMA_TO_DEVICE);
-	spin_lock_irqsave(&sirfport->tx_lock, flags);
 	sirfport->tx_dma_state = TX_DMA_IDLE;
 	sirfsoc_uart_tx_with_dma(sirfport);
-	spin_unlock_irqrestore(&sirfport->tx_lock, flags);
+	spin_unlock_irqrestore(&port->lock, flags);
 }
 
 static void sirfsoc_uart_insert_rx_buf_to_tty(
@@ -489,7 +487,6 @@ static void sirfsoc_uart_insert_rx_buf_to_tty(
 	inserted = tty_insert_flip_string(tport,
 		sirfport->rx_dma_items[sirfport->rx_completed].xmit.buf, count);
 	port->icount.rx += inserted;
-	tty_flip_buffer_push(tport);
 }
 
 static void sirfsoc_rx_submit_one_dma_desc(struct uart_port *port, int index)
@@ -525,7 +522,7 @@ static void sirfsoc_rx_tmo_process_tl(unsigned long param)
 	unsigned long flags;
 	struct dma_tx_state tx_state;
 
-	spin_lock_irqsave(&sirfport->rx_lock, flags);
+	spin_lock_irqsave(&port->lock, flags);
 	while (DMA_COMPLETE == dmaengine_tx_status(sirfport->rx_dma_chan,
 		sirfport->rx_dma_items[sirfport->rx_completed].cookie, &tx_state)) {
 		sirfsoc_uart_insert_rx_buf_to_tty(sirfport,
@@ -541,12 +538,8 @@ static void sirfsoc_rx_tmo_process_tl(unsigned long param)
 	wr_regl(port, ureg->sirfsoc_rx_dma_io_ctrl,
 			rd_regl(port, ureg->sirfsoc_rx_dma_io_ctrl) |
 			SIRFUART_IO_MODE);
-	spin_unlock_irqrestore(&sirfport->rx_lock, flags);
-	spin_lock(&port->lock);
 	sirfsoc_uart_pio_rx_chars(port, 4 - sirfport->rx_io_count);
-	spin_unlock(&port->lock);
 	if (sirfport->rx_io_count == 4) {
-		spin_lock_irqsave(&sirfport->rx_lock, flags);
 		sirfport->rx_io_count = 0;
 		wr_regl(port, ureg->sirfsoc_int_st_reg,
 				uint_st->sirfsoc_rx_done);
@@ -557,11 +550,8 @@ static void sirfsoc_rx_tmo_process_tl(unsigned long param)
 		else
 			wr_regl(port, SIRFUART_INT_EN_CLR,
 					uint_en->sirfsoc_rx_done_en);
-		spin_unlock_irqrestore(&sirfport->rx_lock, flags);
-
 		sirfsoc_uart_start_next_rx_dma(port);
 	} else {
-		spin_lock_irqsave(&sirfport->rx_lock, flags);
 		wr_regl(port, ureg->sirfsoc_int_st_reg,
 				uint_st->sirfsoc_rx_done);
 		if (!sirfport->is_marco)
@@ -571,8 +561,9 @@ static void sirfsoc_rx_tmo_process_tl(unsigned long param)
 		else
 			wr_regl(port, ureg->sirfsoc_int_en_reg,
 					uint_en->sirfsoc_rx_done_en);
-		spin_unlock_irqrestore(&sirfport->rx_lock, flags);
 	}
+	spin_unlock_irqrestore(&port->lock, flags);
+	tty_flip_buffer_push(&port->state->port);
 }
 
 static void sirfsoc_uart_handle_rx_tmo(struct sirfsoc_uart_port *sirfport)
@@ -581,8 +572,6 @@ static void sirfsoc_uart_handle_rx_tmo(struct sirfsoc_uart_port *sirfport)
 	struct sirfsoc_register *ureg = &sirfport->uart_reg->uart_reg;
 	struct sirfsoc_int_en *uint_en = &sirfport->uart_reg->uart_int_en;
 	struct dma_tx_state tx_state;
-	spin_lock(&sirfport->rx_lock);
-
 	dmaengine_tx_status(sirfport->rx_dma_chan,
 		sirfport->rx_dma_items[sirfport->rx_issued].cookie, &tx_state);
 	dmaengine_terminate_all(sirfport->rx_dma_chan);
@@ -595,7 +584,6 @@ static void sirfsoc_uart_handle_rx_tmo(struct sirfsoc_uart_port *sirfport)
 	else
 		wr_regl(port, SIRFUART_INT_EN_CLR,
 				uint_en->sirfsoc_rx_timeout_en);
-	spin_unlock(&sirfport->rx_lock);
 	tasklet_schedule(&sirfport->rx_tmo_process_tasklet);
 }
 
@@ -659,7 +647,6 @@ static irqreturn_t sirfsoc_uart_isr(int irq, void *dev_id)
 		intr_status &= port->read_status_mask;
 		uart_insert_char(port, intr_status,
 					uint_en->sirfsoc_rx_oflow_en, 0, flag);
-		tty_flip_buffer_push(&state->port);
 	}
 recv_char:
 	if ((sirfport->uart_reg->uart_type == SIRF_REAL_UART) &&
@@ -684,6 +671,9 @@ recv_char:
 			sirfsoc_uart_pio_rx_chars(port,
 					SIRFSOC_UART_IO_RX_MAX_CNT);
 	}
+	spin_unlock(&port->lock);
+	tty_flip_buffer_push(&state->port);
+	spin_lock(&port->lock);
 	if (intr_status & uint_st->sirfsoc_txfifo_empty) {
 		if (sirfport->tx_dma_chan)
 			sirfsoc_uart_tx_with_dma(sirfport);
@@ -702,6 +692,7 @@ recv_char:
 		}
 	}
 	spin_unlock(&port->lock);
+
 	return IRQ_HANDLED;
 }
 
@@ -713,7 +704,7 @@ static void sirfsoc_uart_rx_dma_complete_tl(unsigned long param)
 	struct sirfsoc_int_en *uint_en = &sirfport->uart_reg->uart_int_en;
 	unsigned long flags;
 	struct dma_tx_state tx_state;
-	spin_lock_irqsave(&sirfport->rx_lock, flags);
+	spin_lock_irqsave(&port->lock, flags);
 	while (DMA_COMPLETE == dmaengine_tx_status(sirfport->rx_dma_chan,
 			sirfport->rx_dma_items[sirfport->rx_completed].cookie, &tx_state)) {
 		sirfsoc_uart_insert_rx_buf_to_tty(sirfport,
@@ -726,17 +717,20 @@ static void sirfsoc_uart_rx_dma_complete_tl(unsigned long param)
 			sirfport->rx_completed++;
 		sirfport->rx_completed %= SIRFSOC_RX_LOOP_BUF_CNT;
 	}
-	spin_unlock_irqrestore(&sirfport->rx_lock, flags);
+	spin_unlock_irqrestore(&port->lock, flags);
+	tty_flip_buffer_push(&port->state->port);
 }
 
 static void sirfsoc_uart_rx_dma_complete_callback(void *param)
 {
 	struct sirfsoc_uart_port *sirfport = (struct sirfsoc_uart_port *)param;
-	spin_lock(&sirfport->rx_lock);
+	unsigned long flags;
+
+	spin_lock_irqsave(&sirfport->port.lock, flags);
 	sirfport->rx_issued++;
 	sirfport->rx_issued %= SIRFSOC_RX_LOOP_BUF_CNT;
-	spin_unlock(&sirfport->rx_lock);
 	tasklet_schedule(&sirfport->rx_dma_complete_tasklet);
+	spin_unlock_irqrestore(&sirfport->port.lock, flags);
 }
 
 /* submit rx dma task into dmaengine */
@@ -745,18 +739,14 @@ static void sirfsoc_uart_start_next_rx_dma(struct uart_port *port)
 	struct sirfsoc_uart_port *sirfport = to_sirfport(port);
 	struct sirfsoc_register *ureg = &sirfport->uart_reg->uart_reg;
 	struct sirfsoc_int_en *uint_en = &sirfport->uart_reg->uart_int_en;
-	unsigned long flags;
 	int i;
-	spin_lock_irqsave(&sirfport->rx_lock, flags);
 	sirfport->rx_io_count = 0;
 	wr_regl(port, ureg->sirfsoc_rx_dma_io_ctrl,
 		rd_regl(port, ureg->sirfsoc_rx_dma_io_ctrl) &
 		~SIRFUART_IO_MODE);
-	spin_unlock_irqrestore(&sirfport->rx_lock, flags);
 	for (i = 0; i < SIRFSOC_RX_LOOP_BUF_CNT; i++)
 		sirfsoc_rx_submit_one_dma_desc(port, i);
 	sirfport->rx_completed = sirfport->rx_issued = 0;
-	spin_lock_irqsave(&sirfport->rx_lock, flags);
 	if (!sirfport->is_marco)
 		wr_regl(port, ureg->sirfsoc_int_en_reg,
 				rd_regl(port, ureg->sirfsoc_int_en_reg) |
@@ -764,7 +754,6 @@ static void sirfsoc_uart_start_next_rx_dma(struct uart_port *port)
 	else
 		wr_regl(port, ureg->sirfsoc_int_en_reg,
 			SIRFUART_RX_DMA_INT_EN(port, uint_en));
-	spin_unlock_irqrestore(&sirfport->rx_lock, flags);
 }
 
 static void sirfsoc_uart_start_rx(struct uart_port *port)
@@ -1228,7 +1217,7 @@ static void sirfsoc_uart_console_putchar(struct uart_port *port, int ch)
 	while (rd_regl(port,
 		ureg->sirfsoc_tx_fifo_status) & ufifo_st->ff_full(port->line))
 		cpu_relax();
-	wr_regb(port, ureg->sirfsoc_tx_fifo_data, ch);
+	wr_regl(port, ureg->sirfsoc_tx_fifo_data, ch);
 }
 
 static void sirfsoc_uart_console_write(struct console *co, const char *s,
@@ -1369,8 +1358,6 @@ usp_no_flow_control:
 		ret = -EFAULT;
 		goto err;
 	}
-	spin_lock_init(&sirfport->rx_lock);
-	spin_lock_init(&sirfport->tx_lock);
 	tasklet_init(&sirfport->rx_dma_complete_tasklet,
 			sirfsoc_uart_rx_dma_complete_tl, (unsigned long)sirfport);
 	tasklet_init(&sirfport->rx_tmo_process_tasklet,
diff --git a/drivers/tty/serial/sirfsoc_uart.h b/drivers/tty/serial/sirfsoc_uart.h
index 8a6eddad2f3..69a62ebd3af 100644
--- a/drivers/tty/serial/sirfsoc_uart.h
+++ b/drivers/tty/serial/sirfsoc_uart.h
@@ -424,8 +424,6 @@ struct sirfsoc_uart_port {
 	struct dma_chan			*tx_dma_chan;
 	dma_addr_t			tx_dma_addr;
 	struct dma_async_tx_descriptor	*tx_dma_desc;
-	spinlock_t			rx_lock;
-	spinlock_t			tx_lock;
 	struct tasklet_struct		rx_dma_complete_tasklet;
 	struct tasklet_struct		rx_tmo_process_tasklet;
 	unsigned int			rx_io_count;
@@ -441,9 +439,7 @@ struct sirfsoc_uart_port {
 
 /* Register Access Control */
 #define portaddr(port, reg)		((port)->membase + (reg))
-#define rd_regb(port, reg)		(__raw_readb(portaddr(port, reg)))
 #define rd_regl(port, reg)		(__raw_readl(portaddr(port, reg)))
-#define wr_regb(port, reg, val)		__raw_writeb(val, portaddr(port, reg))
 #define wr_regl(port, reg, val)		__raw_writel(val, portaddr(port, reg))
 
 /* UART Port Mask */
diff --git a/drivers/tty/serial/st-asc.c b/drivers/tty/serial/st-asc.c
index 21e6e84c0df..c7f61ac2713 100644
--- a/drivers/tty/serial/st-asc.c
+++ b/drivers/tty/serial/st-asc.c
@@ -194,9 +194,9 @@ static inline u32 asc_txfifo_is_empty(struct uart_port *port)
 	return asc_in(port, ASC_STA) & ASC_STA_TE;
 }
 
-static inline int asc_txfifo_is_full(struct uart_port *port)
+static inline u32 asc_txfifo_is_half_empty(struct uart_port *port)
 {
-	return asc_in(port, ASC_STA) & ASC_STA_TF;
+	return asc_in(port, ASC_STA) & ASC_STA_THE;
 }
 
 static inline const char *asc_port_name(struct uart_port *port)
@@ -295,7 +295,7 @@ static void asc_receive_chars(struct uart_port *port)
 			status & ASC_STA_OE) {
 
 			if (c & ASC_RXBUF_FE) {
-				if (c == ASC_RXBUF_FE) {
+				if (c == (ASC_RXBUF_FE | ASC_RXBUF_DUMMY_RX)) {
 					port->icount.brk++;
 					if (uart_handle_break(port))
 						continue;
@@ -325,7 +325,7 @@ static void asc_receive_chars(struct uart_port *port)
 				flag = TTY_FRAME;
 		}
 
-		if (uart_handle_sysrq_char(port, c))
+		if (uart_handle_sysrq_char(port, c & 0xff))
 			continue;
 
 		uart_insert_char(port, c, ASC_RXBUF_DUMMY_OE, c & 0xff, flag);
@@ -628,7 +628,7 @@ static int asc_get_poll_char(struct uart_port *port)
 
 static void asc_put_poll_char(struct uart_port *port, unsigned char c)
 {
-	while (asc_txfifo_is_full(port))
+	while (!asc_txfifo_is_half_empty(port))
 		cpu_relax();
 	asc_out(port, ASC_TXBUF, c);
 }
@@ -783,7 +783,7 @@ static void asc_console_putchar(struct uart_port *port, int ch)
 	unsigned int timeout = 1000000;
 
 	/* Wait for upto 1 second in case flow control is stopping us. */
-	while (--timeout && asc_txfifo_is_full(port))
+	while (--timeout && !asc_txfifo_is_half_empty(port))
 		udelay(1);
 
 	asc_out(port, ASC_TXBUF, ch);
diff --git a/drivers/tty/serial/uartlite.c b/drivers/tty/serial/uartlite.c
index 5f90ef24d47..dce27f34937 100644
--- a/drivers/tty/serial/uartlite.c
+++ b/drivers/tty/serial/uartlite.c
@@ -418,14 +418,23 @@ static struct uart_ops ulite_ops = {
 #ifdef CONFIG_SERIAL_UARTLITE_CONSOLE
 static void ulite_console_wait_tx(struct uart_port *port)
 {
-	int i;
 	u8 val;
-
-	/* Spin waiting for TX fifo to have space available */
-	for (i = 0; i < 100000; i++) {
+	unsigned long timeout;
+
+	/*
+	 * Spin waiting for TX fifo to have space available.
+	 * When using the Microblaze Debug Module this can take up to 1s
+	 */
+	timeout = jiffies + msecs_to_jiffies(1000);
+	while (1) {
 		val = uart_in32(ULITE_STATUS, port);
 		if ((val & ULITE_STATUS_TXFULL) == 0)
 			break;
+		if (time_after(jiffies, timeout)) {
+			dev_warn(port->dev,
+				 "timeout waiting for TX buffer empty\n");
+			break;
+		}
 		cpu_relax();
 	}
 }
diff --git a/drivers/tty/serial/xilinx_uartps.c b/drivers/tty/serial/xilinx_uartps.c
index f619ad5b5ea..8809775e2ba 100644
--- a/drivers/tty/serial/xilinx_uartps.c
+++ b/drivers/tty/serial/xilinx_uartps.c
@@ -1,7 +1,7 @@
 /*
- * Xilinx PS UART driver
+ * Cadence UART driver (found in Xilinx Zynq)
  *
- * 2011 - 2013 (C) Xilinx Inc.
+ * 2011 - 2014 (C) Xilinx Inc.
  *
  * This program is free software; you can redistribute it
  * and/or modify it under the terms of the GNU General Public
@@ -9,6 +9,9 @@
  * either version 2 of the License, or (at your option) any
  * later version.
  *
+ * This driver has originally been pushed by Xilinx using a Zynq-branding. This
+ * still shows in the naming of this file, the kconfig symbols and some symbols
+ * in the code.
  */
 
 #if defined(CONFIG_SERIAL_XILINX_PS_UART_CONSOLE) && defined(CONFIG_MAGIC_SYSRQ)
@@ -28,16 +31,16 @@
 #include <linux/of.h>
 #include <linux/module.h>
 
-#define XUARTPS_TTY_NAME	"ttyPS"
-#define XUARTPS_NAME		"xuartps"
-#define XUARTPS_MAJOR		0	/* use dynamic node allocation */
-#define XUARTPS_MINOR		0	/* works best with devtmpfs */
-#define XUARTPS_NR_PORTS	2
-#define XUARTPS_FIFO_SIZE	64	/* FIFO size */
-#define XUARTPS_REGISTER_SPACE	0xFFF
+#define CDNS_UART_TTY_NAME	"ttyPS"
+#define CDNS_UART_NAME		"xuartps"
+#define CDNS_UART_MAJOR		0	/* use dynamic node allocation */
+#define CDNS_UART_MINOR		0	/* works best with devtmpfs */
+#define CDNS_UART_NR_PORTS	2
+#define CDNS_UART_FIFO_SIZE	64	/* FIFO size */
+#define CDNS_UART_REGISTER_SPACE	0xFFF
 
-#define xuartps_readl(offset)		ioread32(port->membase + offset)
-#define xuartps_writel(val, offset)	iowrite32(val, port->membase + offset)
+#define cdns_uart_readl(offset)		ioread32(port->membase + offset)
+#define cdns_uart_writel(val, offset)	iowrite32(val, port->membase + offset)
 
 /* Rx Trigger level */
 static int rx_trigger_level = 56;
@@ -49,77 +52,62 @@ static int rx_timeout = 10;
 module_param(rx_timeout, uint, S_IRUGO);
 MODULE_PARM_DESC(rx_timeout, "Rx timeout, 1-255");
 
-/********************************Register Map********************************/
-/** UART
- *
- * Register offsets for the UART.
- *
- */
-#define XUARTPS_CR_OFFSET	0x00  /* Control Register [8:0] */
-#define XUARTPS_MR_OFFSET	0x04  /* Mode Register [10:0] */
-#define XUARTPS_IER_OFFSET	0x08  /* Interrupt Enable [10:0] */
-#define XUARTPS_IDR_OFFSET	0x0C  /* Interrupt Disable [10:0] */
-#define XUARTPS_IMR_OFFSET	0x10  /* Interrupt Mask [10:0] */
-#define XUARTPS_ISR_OFFSET	0x14  /* Interrupt Status [10:0]*/
-#define XUARTPS_BAUDGEN_OFFSET	0x18  /* Baud Rate Generator [15:0] */
-#define XUARTPS_RXTOUT_OFFSET	0x1C  /* RX Timeout [7:0] */
-#define XUARTPS_RXWM_OFFSET	0x20  /* RX FIFO Trigger Level [5:0] */
-#define XUARTPS_MODEMCR_OFFSET	0x24  /* Modem Control [5:0] */
-#define XUARTPS_MODEMSR_OFFSET	0x28  /* Modem Status [8:0] */
-#define XUARTPS_SR_OFFSET	0x2C  /* Channel Status [11:0] */
-#define XUARTPS_FIFO_OFFSET	0x30  /* FIFO [15:0] or [7:0] */
-#define XUARTPS_BAUDDIV_OFFSET	0x34  /* Baud Rate Divider [7:0] */
-#define XUARTPS_FLOWDEL_OFFSET	0x38  /* Flow Delay [15:0] */
-#define XUARTPS_IRRX_PWIDTH_OFFSET 0x3C /* IR Minimum Received Pulse
-						Width [15:0] */
-#define XUARTPS_IRTX_PWIDTH_OFFSET 0x40 /* IR Transmitted pulse
-						Width [7:0] */
-#define XUARTPS_TXWM_OFFSET	0x44  /* TX FIFO Trigger Level [5:0] */
-
-/** Control Register
- *
- * The Control register (CR) controls the major functions of the device.
- *
- * Control Register Bit Definitions
- */
-#define XUARTPS_CR_STOPBRK	0x00000100  /* Stop TX break */
-#define XUARTPS_CR_STARTBRK	0x00000080  /* Set TX break */
-#define XUARTPS_CR_TX_DIS	0x00000020  /* TX disabled. */
-#define XUARTPS_CR_TX_EN	0x00000010  /* TX enabled */
-#define XUARTPS_CR_RX_DIS	0x00000008  /* RX disabled. */
-#define XUARTPS_CR_RX_EN	0x00000004  /* RX enabled */
-#define XUARTPS_CR_TXRST	0x00000002  /* TX logic reset */
-#define XUARTPS_CR_RXRST	0x00000001  /* RX logic reset */
-#define XUARTPS_CR_RST_TO	0x00000040  /* Restart Timeout Counter */
-
-/** Mode Register
- *
+/* Register offsets for the UART. */
+#define CDNS_UART_CR_OFFSET		0x00  /* Control Register */
+#define CDNS_UART_MR_OFFSET		0x04  /* Mode Register */
+#define CDNS_UART_IER_OFFSET		0x08  /* Interrupt Enable */
+#define CDNS_UART_IDR_OFFSET		0x0C  /* Interrupt Disable */
+#define CDNS_UART_IMR_OFFSET		0x10  /* Interrupt Mask */
+#define CDNS_UART_ISR_OFFSET		0x14  /* Interrupt Status */
+#define CDNS_UART_BAUDGEN_OFFSET	0x18  /* Baud Rate Generator */
+#define CDNS_UART_RXTOUT_OFFSET		0x1C  /* RX Timeout */
+#define CDNS_UART_RXWM_OFFSET		0x20  /* RX FIFO Trigger Level */
+#define CDNS_UART_MODEMCR_OFFSET	0x24  /* Modem Control */
+#define CDNS_UART_MODEMSR_OFFSET	0x28  /* Modem Status */
+#define CDNS_UART_SR_OFFSET		0x2C  /* Channel Status */
+#define CDNS_UART_FIFO_OFFSET		0x30  /* FIFO */
+#define CDNS_UART_BAUDDIV_OFFSET	0x34  /* Baud Rate Divider */
+#define CDNS_UART_FLOWDEL_OFFSET	0x38  /* Flow Delay */
+#define CDNS_UART_IRRX_PWIDTH_OFFSET	0x3C  /* IR Min Received Pulse Width */
+#define CDNS_UART_IRTX_PWIDTH_OFFSET	0x40  /* IR Transmitted pulse Width */
+#define CDNS_UART_TXWM_OFFSET		0x44  /* TX FIFO Trigger Level */
+
+/* Control Register Bit Definitions */
+#define CDNS_UART_CR_STOPBRK	0x00000100  /* Stop TX break */
+#define CDNS_UART_CR_STARTBRK	0x00000080  /* Set TX break */
+#define CDNS_UART_CR_TX_DIS	0x00000020  /* TX disabled. */
+#define CDNS_UART_CR_TX_EN	0x00000010  /* TX enabled */
+#define CDNS_UART_CR_RX_DIS	0x00000008  /* RX disabled. */
+#define CDNS_UART_CR_RX_EN	0x00000004  /* RX enabled */
+#define CDNS_UART_CR_TXRST	0x00000002  /* TX logic reset */
+#define CDNS_UART_CR_RXRST	0x00000001  /* RX logic reset */
+#define CDNS_UART_CR_RST_TO	0x00000040  /* Restart Timeout Counter */
+
+/*
+ * Mode Register:
  * The mode register (MR) defines the mode of transfer as well as the data
  * format. If this register is modified during transmission or reception,
  * data validity cannot be guaranteed.
- *
- * Mode Register Bit Definitions
- *
  */
-#define XUARTPS_MR_CLKSEL		0x00000001  /* Pre-scalar selection */
-#define XUARTPS_MR_CHMODE_L_LOOP	0x00000200  /* Local loop back mode */
-#define XUARTPS_MR_CHMODE_NORM		0x00000000  /* Normal mode */
+#define CDNS_UART_MR_CLKSEL		0x00000001  /* Pre-scalar selection */
+#define CDNS_UART_MR_CHMODE_L_LOOP	0x00000200  /* Local loop back mode */
+#define CDNS_UART_MR_CHMODE_NORM	0x00000000  /* Normal mode */
 
-#define XUARTPS_MR_STOPMODE_2_BIT	0x00000080  /* 2 stop bits */
-#define XUARTPS_MR_STOPMODE_1_BIT	0x00000000  /* 1 stop bit */
+#define CDNS_UART_MR_STOPMODE_2_BIT	0x00000080  /* 2 stop bits */
+#define CDNS_UART_MR_STOPMODE_1_BIT	0x00000000  /* 1 stop bit */
 
-#define XUARTPS_MR_PARITY_NONE		0x00000020  /* No parity mode */
-#define XUARTPS_MR_PARITY_MARK		0x00000018  /* Mark parity mode */
-#define XUARTPS_MR_PARITY_SPACE		0x00000010  /* Space parity mode */
-#define XUARTPS_MR_PARITY_ODD		0x00000008  /* Odd parity mode */
-#define XUARTPS_MR_PARITY_EVEN		0x00000000  /* Even parity mode */
+#define CDNS_UART_MR_PARITY_NONE	0x00000020  /* No parity mode */
+#define CDNS_UART_MR_PARITY_MARK	0x00000018  /* Mark parity mode */
+#define CDNS_UART_MR_PARITY_SPACE	0x00000010  /* Space parity mode */
+#define CDNS_UART_MR_PARITY_ODD		0x00000008  /* Odd parity mode */
+#define CDNS_UART_MR_PARITY_EVEN	0x00000000  /* Even parity mode */
 
-#define XUARTPS_MR_CHARLEN_6_BIT	0x00000006  /* 6 bits data */
-#define XUARTPS_MR_CHARLEN_7_BIT	0x00000004  /* 7 bits data */
-#define XUARTPS_MR_CHARLEN_8_BIT	0x00000000  /* 8 bits data */
+#define CDNS_UART_MR_CHARLEN_6_BIT	0x00000006  /* 6 bits data */
+#define CDNS_UART_MR_CHARLEN_7_BIT	0x00000004  /* 7 bits data */
+#define CDNS_UART_MR_CHARLEN_8_BIT	0x00000000  /* 8 bits data */
 
-/** Interrupt Registers
- *
+/*
+ * Interrupt Registers:
  * Interrupt control logic uses the interrupt enable register (IER) and the
  * interrupt disable register (IDR) to set the value of the bits in the
  * interrupt mask register (IMR). The IMR determines whether to pass an
@@ -127,65 +115,65 @@ MODULE_PARM_DESC(rx_timeout, "Rx timeout, 1-255");
  * Writing a 1 to IER Enables an interrupt, writing a 1 to IDR disables an
  * interrupt. IMR and ISR are read only, and IER and IDR are write only.
  * Reading either IER or IDR returns 0x00.
- *
  * All four registers have the same bit definitions.
  */
-#define XUARTPS_IXR_TOUT	0x00000100 /* RX Timeout error interrupt */
-#define XUARTPS_IXR_PARITY	0x00000080 /* Parity error interrupt */
-#define XUARTPS_IXR_FRAMING	0x00000040 /* Framing error interrupt */
-#define XUARTPS_IXR_OVERRUN	0x00000020 /* Overrun error interrupt */
-#define XUARTPS_IXR_TXFULL	0x00000010 /* TX FIFO Full interrupt */
-#define XUARTPS_IXR_TXEMPTY	0x00000008 /* TX FIFO empty interrupt */
-#define XUARTPS_ISR_RXEMPTY	0x00000002 /* RX FIFO empty interrupt */
-#define XUARTPS_IXR_RXTRIG	0x00000001 /* RX FIFO trigger interrupt */
-#define XUARTPS_IXR_RXFULL	0x00000004 /* RX FIFO full interrupt. */
-#define XUARTPS_IXR_RXEMPTY	0x00000002 /* RX FIFO empty interrupt. */
-#define XUARTPS_IXR_MASK	0x00001FFF /* Valid bit mask */
+#define CDNS_UART_IXR_TOUT	0x00000100 /* RX Timeout error interrupt */
+#define CDNS_UART_IXR_PARITY	0x00000080 /* Parity error interrupt */
+#define CDNS_UART_IXR_FRAMING	0x00000040 /* Framing error interrupt */
+#define CDNS_UART_IXR_OVERRUN	0x00000020 /* Overrun error interrupt */
+#define CDNS_UART_IXR_TXFULL	0x00000010 /* TX FIFO Full interrupt */
+#define CDNS_UART_IXR_TXEMPTY	0x00000008 /* TX FIFO empty interrupt */
+#define CDNS_UART_ISR_RXEMPTY	0x00000002 /* RX FIFO empty interrupt */
+#define CDNS_UART_IXR_RXTRIG	0x00000001 /* RX FIFO trigger interrupt */
+#define CDNS_UART_IXR_RXFULL	0x00000004 /* RX FIFO full interrupt. */
+#define CDNS_UART_IXR_RXEMPTY	0x00000002 /* RX FIFO empty interrupt. */
+#define CDNS_UART_IXR_MASK	0x00001FFF /* Valid bit mask */
 
 /* Goes in read_status_mask for break detection as the HW doesn't do it*/
-#define XUARTPS_IXR_BRK		0x80000000
+#define CDNS_UART_IXR_BRK	0x80000000
 
-/** Channel Status Register
- *
+/*
+ * Channel Status Register:
  * The channel status register (CSR) is provided to enable the control logic
  * to monitor the status of bits in the channel interrupt status register,
  * even if these are masked out by the interrupt mask register.
  */
-#define XUARTPS_SR_RXEMPTY	0x00000002 /* RX FIFO empty */
-#define XUARTPS_SR_TXEMPTY	0x00000008 /* TX FIFO empty */
-#define XUARTPS_SR_TXFULL	0x00000010 /* TX FIFO full */
-#define XUARTPS_SR_RXTRIG	0x00000001 /* Rx Trigger */
+#define CDNS_UART_SR_RXEMPTY	0x00000002 /* RX FIFO empty */
+#define CDNS_UART_SR_TXEMPTY	0x00000008 /* TX FIFO empty */
+#define CDNS_UART_SR_TXFULL	0x00000010 /* TX FIFO full */
+#define CDNS_UART_SR_RXTRIG	0x00000001 /* Rx Trigger */
 
 /* baud dividers min/max values */
-#define XUARTPS_BDIV_MIN	4
-#define XUARTPS_BDIV_MAX	255
-#define XUARTPS_CD_MAX		65535
+#define CDNS_UART_BDIV_MIN	4
+#define CDNS_UART_BDIV_MAX	255
+#define CDNS_UART_CD_MAX	65535
 
 /**
- * struct xuartps - device data
- * @port		Pointer to the UART port
- * @refclk		Reference clock
- * @aperclk		APB clock
- * @baud		Current baud rate
- * @clk_rate_change_nb	Notifier block for clock changes
+ * struct cdns_uart - device data
+ * @port:		Pointer to the UART port
+ * @uartclk:		Reference clock
+ * @pclk:		APB clock
+ * @baud:		Current baud rate
+ * @clk_rate_change_nb:	Notifier block for clock changes
  */
-struct xuartps {
+struct cdns_uart {
 	struct uart_port	*port;
-	struct clk		*refclk;
-	struct clk		*aperclk;
+	struct clk		*uartclk;
+	struct clk		*pclk;
 	unsigned int		baud;
 	struct notifier_block	clk_rate_change_nb;
 };
-#define to_xuartps(_nb) container_of(_nb, struct xuartps, clk_rate_change_nb);
+#define to_cdns_uart(_nb) container_of(_nb, struct cdns_uart, \
+		clk_rate_change_nb);
 
 /**
- * xuartps_isr - Interrupt handler
+ * cdns_uart_isr - Interrupt handler
  * @irq: Irq number
  * @dev_id: Id of the port
  *
- * Returns IRQHANDLED
- **/
-static irqreturn_t xuartps_isr(int irq, void *dev_id)
+ * Return: IRQHANDLED
+ */
+static irqreturn_t cdns_uart_isr(int irq, void *dev_id)
 {
 	struct uart_port *port = (struct uart_port *)dev_id;
 	unsigned long flags;
@@ -198,43 +186,42 @@ static irqreturn_t xuartps_isr(int irq, void *dev_id)
 	/* Read the interrupt status register to determine which
 	 * interrupt(s) is/are active.
 	 */
-	isrstatus = xuartps_readl(XUARTPS_ISR_OFFSET);
+	isrstatus = cdns_uart_readl(CDNS_UART_ISR_OFFSET);
 
 	/*
 	 * There is no hardware break detection, so we interpret framing
 	 * error with all-zeros data as a break sequence. Most of the time,
 	 * there's another non-zero byte at the end of the sequence.
 	 */
-
-	if (isrstatus & XUARTPS_IXR_FRAMING) {
-		while (!(xuartps_readl(XUARTPS_SR_OFFSET) &
-					XUARTPS_SR_RXEMPTY)) {
-			if (!xuartps_readl(XUARTPS_FIFO_OFFSET)) {
-				port->read_status_mask |= XUARTPS_IXR_BRK;
-				isrstatus &= ~XUARTPS_IXR_FRAMING;
+	if (isrstatus & CDNS_UART_IXR_FRAMING) {
+		while (!(cdns_uart_readl(CDNS_UART_SR_OFFSET) &
+					CDNS_UART_SR_RXEMPTY)) {
+			if (!cdns_uart_readl(CDNS_UART_FIFO_OFFSET)) {
+				port->read_status_mask |= CDNS_UART_IXR_BRK;
+				isrstatus &= ~CDNS_UART_IXR_FRAMING;
 			}
 		}
-		xuartps_writel(XUARTPS_IXR_FRAMING, XUARTPS_ISR_OFFSET);
+		cdns_uart_writel(CDNS_UART_IXR_FRAMING, CDNS_UART_ISR_OFFSET);
 	}
 
 	/* drop byte with parity error if IGNPAR specified */
-	if (isrstatus & port->ignore_status_mask & XUARTPS_IXR_PARITY)
-		isrstatus &= ~(XUARTPS_IXR_RXTRIG | XUARTPS_IXR_TOUT);
+	if (isrstatus & port->ignore_status_mask & CDNS_UART_IXR_PARITY)
+		isrstatus &= ~(CDNS_UART_IXR_RXTRIG | CDNS_UART_IXR_TOUT);
 
 	isrstatus &= port->read_status_mask;
 	isrstatus &= ~port->ignore_status_mask;
 
-	if ((isrstatus & XUARTPS_IXR_TOUT) ||
-		(isrstatus & XUARTPS_IXR_RXTRIG)) {
+	if ((isrstatus & CDNS_UART_IXR_TOUT) ||
+		(isrstatus & CDNS_UART_IXR_RXTRIG)) {
 		/* Receive Timeout Interrupt */
-		while ((xuartps_readl(XUARTPS_SR_OFFSET) &
-			XUARTPS_SR_RXEMPTY) != XUARTPS_SR_RXEMPTY) {
-			data = xuartps_readl(XUARTPS_FIFO_OFFSET);
+		while ((cdns_uart_readl(CDNS_UART_SR_OFFSET) &
+			CDNS_UART_SR_RXEMPTY) != CDNS_UART_SR_RXEMPTY) {
+			data = cdns_uart_readl(CDNS_UART_FIFO_OFFSET);
 
 			/* Non-NULL byte after BREAK is garbage (99%) */
 			if (data && (port->read_status_mask &
-						XUARTPS_IXR_BRK)) {
-				port->read_status_mask &= ~XUARTPS_IXR_BRK;
+						CDNS_UART_IXR_BRK)) {
+				port->read_status_mask &= ~CDNS_UART_IXR_BRK;
 				port->icount.brk++;
 				if (uart_handle_break(port))
 					continue;
@@ -258,16 +245,17 @@ static irqreturn_t xuartps_isr(int irq, void *dev_id)
 
 			port->icount.rx++;
 
-			if (isrstatus & XUARTPS_IXR_PARITY) {
+			if (isrstatus & CDNS_UART_IXR_PARITY) {
 				port->icount.parity++;
 				status = TTY_PARITY;
-			} else if (isrstatus & XUARTPS_IXR_FRAMING) {
+			} else if (isrstatus & CDNS_UART_IXR_FRAMING) {
 				port->icount.frame++;
 				status = TTY_FRAME;
-			} else if (isrstatus & XUARTPS_IXR_OVERRUN)
+			} else if (isrstatus & CDNS_UART_IXR_OVERRUN) {
 				port->icount.overrun++;
+			}
 
-			uart_insert_char(port, isrstatus, XUARTPS_IXR_OVERRUN,
+			uart_insert_char(port, isrstatus, CDNS_UART_IXR_OVERRUN,
 					data, status);
 		}
 		spin_unlock(&port->lock);
@@ -276,10 +264,10 @@ static irqreturn_t xuartps_isr(int irq, void *dev_id)
 	}
 
 	/* Dispatch an appropriate handler */
-	if ((isrstatus & XUARTPS_IXR_TXEMPTY) == XUARTPS_IXR_TXEMPTY) {
+	if ((isrstatus & CDNS_UART_IXR_TXEMPTY) == CDNS_UART_IXR_TXEMPTY) {
 		if (uart_circ_empty(&port->state->xmit)) {
-			xuartps_writel(XUARTPS_IXR_TXEMPTY,
-						XUARTPS_IDR_OFFSET);
+			cdns_uart_writel(CDNS_UART_IXR_TXEMPTY,
+						CDNS_UART_IDR_OFFSET);
 		} else {
 			numbytes = port->fifosize;
 			/* Break if no more data available in the UART buffer */
@@ -287,12 +275,12 @@ static irqreturn_t xuartps_isr(int irq, void *dev_id)
 				if (uart_circ_empty(&port->state->xmit))
 					break;
 				/* Get the data from the UART circular buffer
-				 * and write it to the xuartps's TX_FIFO
+				 * and write it to the cdns_uart's TX_FIFO
 				 * register.
 				 */
-				xuartps_writel(
+				cdns_uart_writel(
 					port->state->xmit.buf[port->state->xmit.
-					tail], XUARTPS_FIFO_OFFSET);
+					tail], CDNS_UART_FIFO_OFFSET);
 
 				port->icount.tx++;
 
@@ -300,7 +288,7 @@ static irqreturn_t xuartps_isr(int irq, void *dev_id)
 				 * the buffer if it reaches limit.
 				 */
 				port->state->xmit.tail =
-					(port->state->xmit.tail + 1) & \
+					(port->state->xmit.tail + 1) &
 						(UART_XMIT_SIZE - 1);
 			}
 
@@ -310,7 +298,7 @@ static irqreturn_t xuartps_isr(int irq, void *dev_id)
 		}
 	}
 
-	xuartps_writel(isrstatus, XUARTPS_ISR_OFFSET);
+	cdns_uart_writel(isrstatus, CDNS_UART_ISR_OFFSET);
 
 	/* be sure to release the lock and tty before leaving */
 	spin_unlock_irqrestore(&port->lock, flags);
@@ -319,13 +307,13 @@ static irqreturn_t xuartps_isr(int irq, void *dev_id)
 }
 
 /**
- * xuartps_calc_baud_divs - Calculate baud rate divisors
+ * cdns_uart_calc_baud_divs - Calculate baud rate divisors
  * @clk: UART module input clock
  * @baud: Desired baud rate
  * @rbdiv: BDIV value (return value)
  * @rcd: CD value (return value)
  * @div8: Value for clk_sel bit in mod (return value)
- * Returns baud rate, requested baud when possible, or actual baud when there
+ * Return: baud rate, requested baud when possible, or actual baud when there
  *	was too much error, zero if no valid divisors are found.
  *
  * Formula to obtain baud rate is
@@ -338,8 +326,8 @@ static irqreturn_t xuartps_isr(int irq, void *dev_id)
  *			baud rate generate register
  *			baud rate clock divisor register
  */
-static unsigned int xuartps_calc_baud_divs(unsigned int clk, unsigned int baud,
-		u32 *rbdiv, u32 *rcd, int *div8)
+static unsigned int cdns_uart_calc_baud_divs(unsigned int clk,
+		unsigned int baud, u32 *rbdiv, u32 *rcd, int *div8)
 {
 	u32 cd, bdiv;
 	unsigned int calc_baud;
@@ -347,16 +335,16 @@ static unsigned int xuartps_calc_baud_divs(unsigned int clk, unsigned int baud,
 	unsigned int bauderror;
 	unsigned int besterror = ~0;
 
-	if (baud < clk / ((XUARTPS_BDIV_MAX + 1) * XUARTPS_CD_MAX)) {
+	if (baud < clk / ((CDNS_UART_BDIV_MAX + 1) * CDNS_UART_CD_MAX)) {
 		*div8 = 1;
 		clk /= 8;
 	} else {
 		*div8 = 0;
 	}
 
-	for (bdiv = XUARTPS_BDIV_MIN; bdiv <= XUARTPS_BDIV_MAX; bdiv++) {
+	for (bdiv = CDNS_UART_BDIV_MIN; bdiv <= CDNS_UART_BDIV_MAX; bdiv++) {
 		cd = DIV_ROUND_CLOSEST(clk, baud * (bdiv + 1));
-		if (cd < 1 || cd > XUARTPS_CD_MAX)
+		if (cd < 1 || cd > CDNS_UART_CD_MAX)
 			continue;
 
 		calc_baud = clk / (cd * (bdiv + 1));
@@ -381,47 +369,47 @@ static unsigned int xuartps_calc_baud_divs(unsigned int clk, unsigned int baud,
 }
 
 /**
- * xuartps_set_baud_rate - Calculate and set the baud rate
+ * cdns_uart_set_baud_rate - Calculate and set the baud rate
  * @port: Handle to the uart port structure
  * @baud: Baud rate to set
- * Returns baud rate, requested baud when possible, or actual baud when there
+ * Return: baud rate, requested baud when possible, or actual baud when there
  *	   was too much error, zero if no valid divisors are found.
  */
-static unsigned int xuartps_set_baud_rate(struct uart_port *port,
+static unsigned int cdns_uart_set_baud_rate(struct uart_port *port,
 		unsigned int baud)
 {
 	unsigned int calc_baud;
 	u32 cd = 0, bdiv = 0;
 	u32 mreg;
 	int div8;
-	struct xuartps *xuartps = port->private_data;
+	struct cdns_uart *cdns_uart = port->private_data;
 
-	calc_baud = xuartps_calc_baud_divs(port->uartclk, baud, &bdiv, &cd,
+	calc_baud = cdns_uart_calc_baud_divs(port->uartclk, baud, &bdiv, &cd,
 			&div8);
 
 	/* Write new divisors to hardware */
-	mreg = xuartps_readl(XUARTPS_MR_OFFSET);
+	mreg = cdns_uart_readl(CDNS_UART_MR_OFFSET);
 	if (div8)
-		mreg |= XUARTPS_MR_CLKSEL;
+		mreg |= CDNS_UART_MR_CLKSEL;
 	else
-		mreg &= ~XUARTPS_MR_CLKSEL;
-	xuartps_writel(mreg, XUARTPS_MR_OFFSET);
-	xuartps_writel(cd, XUARTPS_BAUDGEN_OFFSET);
-	xuartps_writel(bdiv, XUARTPS_BAUDDIV_OFFSET);
-	xuartps->baud = baud;
+		mreg &= ~CDNS_UART_MR_CLKSEL;
+	cdns_uart_writel(mreg, CDNS_UART_MR_OFFSET);
+	cdns_uart_writel(cd, CDNS_UART_BAUDGEN_OFFSET);
+	cdns_uart_writel(bdiv, CDNS_UART_BAUDDIV_OFFSET);
+	cdns_uart->baud = baud;
 
 	return calc_baud;
 }
 
 #ifdef CONFIG_COMMON_CLK
 /**
- * xuartps_clk_notitifer_cb - Clock notifier callback
+ * cdns_uart_clk_notitifer_cb - Clock notifier callback
  * @nb:		Notifier block
  * @event:	Notify event
  * @data:	Notifier data
- * Returns NOTIFY_OK on success, NOTIFY_BAD on error.
+ * Return:	NOTIFY_OK or NOTIFY_DONE on success, NOTIFY_BAD on error.
  */
-static int xuartps_clk_notifier_cb(struct notifier_block *nb,
+static int cdns_uart_clk_notifier_cb(struct notifier_block *nb,
 		unsigned long event, void *data)
 {
 	u32 ctrl_reg;
@@ -429,35 +417,36 @@ static int xuartps_clk_notifier_cb(struct notifier_block *nb,
 	int locked = 0;
 	struct clk_notifier_data *ndata = data;
 	unsigned long flags = 0;
-	struct xuartps *xuartps = to_xuartps(nb);
+	struct cdns_uart *cdns_uart = to_cdns_uart(nb);
 
-	port = xuartps->port;
+	port = cdns_uart->port;
 	if (port->suspended)
 		return NOTIFY_OK;
 
 	switch (event) {
 	case PRE_RATE_CHANGE:
 	{
-		u32 bdiv;
-		u32 cd;
+		u32 bdiv, cd;
 		int div8;
 
 		/*
 		 * Find out if current baud-rate can be achieved with new clock
 		 * frequency.
 		 */
-		if (!xuartps_calc_baud_divs(ndata->new_rate, xuartps->baud,
-					&bdiv, &cd, &div8))
+		if (!cdns_uart_calc_baud_divs(ndata->new_rate, cdns_uart->baud,
+					&bdiv, &cd, &div8)) {
+			dev_warn(port->dev, "clock rate change rejected\n");
 			return NOTIFY_BAD;
+		}
 
-		spin_lock_irqsave(&xuartps->port->lock, flags);
+		spin_lock_irqsave(&cdns_uart->port->lock, flags);
 
 		/* Disable the TX and RX to set baud rate */
-		xuartps_writel(xuartps_readl(XUARTPS_CR_OFFSET) |
-				(XUARTPS_CR_TX_DIS | XUARTPS_CR_RX_DIS),
-				XUARTPS_CR_OFFSET);
+		ctrl_reg = cdns_uart_readl(CDNS_UART_CR_OFFSET);
+		ctrl_reg |= CDNS_UART_CR_TX_DIS | CDNS_UART_CR_RX_DIS;
+		cdns_uart_writel(ctrl_reg, CDNS_UART_CR_OFFSET);
 
-		spin_unlock_irqrestore(&xuartps->port->lock, flags);
+		spin_unlock_irqrestore(&cdns_uart->port->lock, flags);
 
 		return NOTIFY_OK;
 	}
@@ -467,25 +456,25 @@ static int xuartps_clk_notifier_cb(struct notifier_block *nb,
 		 * frequency.
 		 */
 
-		spin_lock_irqsave(&xuartps->port->lock, flags);
+		spin_lock_irqsave(&cdns_uart->port->lock, flags);
 
 		locked = 1;
 		port->uartclk = ndata->new_rate;
 
-		xuartps->baud = xuartps_set_baud_rate(xuartps->port,
-				xuartps->baud);
+		cdns_uart->baud = cdns_uart_set_baud_rate(cdns_uart->port,
+				cdns_uart->baud);
 		/* fall through */
 	case ABORT_RATE_CHANGE:
 		if (!locked)
-			spin_lock_irqsave(&xuartps->port->lock, flags);
+			spin_lock_irqsave(&cdns_uart->port->lock, flags);
 
 		/* Set TX/RX Reset */
-		xuartps_writel(xuartps_readl(XUARTPS_CR_OFFSET) |
-				(XUARTPS_CR_TXRST | XUARTPS_CR_RXRST),
-				XUARTPS_CR_OFFSET);
+		ctrl_reg = cdns_uart_readl(CDNS_UART_CR_OFFSET);
+		ctrl_reg |= CDNS_UART_CR_TXRST | CDNS_UART_CR_RXRST;
+		cdns_uart_writel(ctrl_reg, CDNS_UART_CR_OFFSET);
 
-		while (xuartps_readl(XUARTPS_CR_OFFSET) &
-				(XUARTPS_CR_TXRST | XUARTPS_CR_RXRST))
+		while (cdns_uart_readl(CDNS_UART_CR_OFFSET) &
+				(CDNS_UART_CR_TXRST | CDNS_UART_CR_RXRST))
 			cpu_relax();
 
 		/*
@@ -493,14 +482,13 @@ static int xuartps_clk_notifier_cb(struct notifier_block *nb,
 		 * enable bit and RX enable bit to enable the transmitter and
 		 * receiver.
 		 */
-		xuartps_writel(rx_timeout, XUARTPS_RXTOUT_OFFSET);
-		ctrl_reg = xuartps_readl(XUARTPS_CR_OFFSET);
-		xuartps_writel(
-			(ctrl_reg & ~(XUARTPS_CR_TX_DIS | XUARTPS_CR_RX_DIS)) |
-			(XUARTPS_CR_TX_EN | XUARTPS_CR_RX_EN),
-			XUARTPS_CR_OFFSET);
+		cdns_uart_writel(rx_timeout, CDNS_UART_RXTOUT_OFFSET);
+		ctrl_reg = cdns_uart_readl(CDNS_UART_CR_OFFSET);
+		ctrl_reg &= ~(CDNS_UART_CR_TX_DIS | CDNS_UART_CR_RX_DIS);
+		ctrl_reg |= CDNS_UART_CR_TX_EN | CDNS_UART_CR_RX_EN;
+		cdns_uart_writel(ctrl_reg, CDNS_UART_CR_OFFSET);
 
-		spin_unlock_irqrestore(&xuartps->port->lock, flags);
+		spin_unlock_irqrestore(&cdns_uart->port->lock, flags);
 
 		return NOTIFY_OK;
 	default:
@@ -509,40 +497,36 @@ static int xuartps_clk_notifier_cb(struct notifier_block *nb,
 }
 #endif
 
-/*----------------------Uart Operations---------------------------*/
-
 /**
- * xuartps_start_tx -  Start transmitting bytes
+ * cdns_uart_start_tx -  Start transmitting bytes
  * @port: Handle to the uart port structure
- *
- **/
-static void xuartps_start_tx(struct uart_port *port)
+ */
+static void cdns_uart_start_tx(struct uart_port *port)
 {
 	unsigned int status, numbytes = port->fifosize;
 
 	if (uart_circ_empty(&port->state->xmit) || uart_tx_stopped(port))
 		return;
 
-	status = xuartps_readl(XUARTPS_CR_OFFSET);
+	status = cdns_uart_readl(CDNS_UART_CR_OFFSET);
 	/* Set the TX enable bit and clear the TX disable bit to enable the
 	 * transmitter.
 	 */
-	xuartps_writel((status & ~XUARTPS_CR_TX_DIS) | XUARTPS_CR_TX_EN,
-		XUARTPS_CR_OFFSET);
-
-	while (numbytes-- && ((xuartps_readl(XUARTPS_SR_OFFSET)
-		& XUARTPS_SR_TXFULL)) != XUARTPS_SR_TXFULL) {
+	cdns_uart_writel((status & ~CDNS_UART_CR_TX_DIS) | CDNS_UART_CR_TX_EN,
+		CDNS_UART_CR_OFFSET);
 
+	while (numbytes-- && ((cdns_uart_readl(CDNS_UART_SR_OFFSET) &
+				CDNS_UART_SR_TXFULL)) != CDNS_UART_SR_TXFULL) {
 		/* Break if no more data available in the UART buffer */
 		if (uart_circ_empty(&port->state->xmit))
 			break;
 
 		/* Get the data from the UART circular buffer and
-		 * write it to the xuartps's TX_FIFO register.
+		 * write it to the cdns_uart's TX_FIFO register.
 		 */
-		xuartps_writel(
+		cdns_uart_writel(
 			port->state->xmit.buf[port->state->xmit.tail],
-			XUARTPS_FIFO_OFFSET);
+			CDNS_UART_FIFO_OFFSET);
 		port->icount.tx++;
 
 		/* Adjust the tail of the UART buffer and wrap
@@ -551,94 +535,90 @@ static void xuartps_start_tx(struct uart_port *port)
 		port->state->xmit.tail = (port->state->xmit.tail + 1) &
 					(UART_XMIT_SIZE - 1);
 	}
-	xuartps_writel(XUARTPS_IXR_TXEMPTY, XUARTPS_ISR_OFFSET);
+	cdns_uart_writel(CDNS_UART_IXR_TXEMPTY, CDNS_UART_ISR_OFFSET);
 	/* Enable the TX Empty interrupt */
-	xuartps_writel(XUARTPS_IXR_TXEMPTY, XUARTPS_IER_OFFSET);
+	cdns_uart_writel(CDNS_UART_IXR_TXEMPTY, CDNS_UART_IER_OFFSET);
 
 	if (uart_circ_chars_pending(&port->state->xmit) < WAKEUP_CHARS)
 		uart_write_wakeup(port);
 }
 
 /**
- * xuartps_stop_tx - Stop TX
+ * cdns_uart_stop_tx - Stop TX
  * @port: Handle to the uart port structure
- *
- **/
-static void xuartps_stop_tx(struct uart_port *port)
+ */
+static void cdns_uart_stop_tx(struct uart_port *port)
 {
 	unsigned int regval;
 
-	regval = xuartps_readl(XUARTPS_CR_OFFSET);
-	regval |= XUARTPS_CR_TX_DIS;
+	regval = cdns_uart_readl(CDNS_UART_CR_OFFSET);
+	regval |= CDNS_UART_CR_TX_DIS;
 	/* Disable the transmitter */
-	xuartps_writel(regval, XUARTPS_CR_OFFSET);
+	cdns_uart_writel(regval, CDNS_UART_CR_OFFSET);
 }
 
 /**
- * xuartps_stop_rx - Stop RX
+ * cdns_uart_stop_rx - Stop RX
  * @port: Handle to the uart port structure
- *
- **/
-static void xuartps_stop_rx(struct uart_port *port)
+ */
+static void cdns_uart_stop_rx(struct uart_port *port)
 {
 	unsigned int regval;
 
-	regval = xuartps_readl(XUARTPS_CR_OFFSET);
-	regval |= XUARTPS_CR_RX_DIS;
+	regval = cdns_uart_readl(CDNS_UART_CR_OFFSET);
+	regval |= CDNS_UART_CR_RX_DIS;
 	/* Disable the receiver */
-	xuartps_writel(regval, XUARTPS_CR_OFFSET);
+	cdns_uart_writel(regval, CDNS_UART_CR_OFFSET);
 }
 
 /**
- * xuartps_tx_empty -  Check whether TX is empty
+ * cdns_uart_tx_empty -  Check whether TX is empty
  * @port: Handle to the uart port structure
  *
- * Returns TIOCSER_TEMT on success, 0 otherwise
- **/
-static unsigned int xuartps_tx_empty(struct uart_port *port)
+ * Return: TIOCSER_TEMT on success, 0 otherwise
+ */
+static unsigned int cdns_uart_tx_empty(struct uart_port *port)
 {
 	unsigned int status;
 
-	status = xuartps_readl(XUARTPS_ISR_OFFSET) & XUARTPS_IXR_TXEMPTY;
+	status = cdns_uart_readl(CDNS_UART_ISR_OFFSET) & CDNS_UART_IXR_TXEMPTY;
 	return status ? TIOCSER_TEMT : 0;
 }
 
 /**
- * xuartps_break_ctl - Based on the input ctl we have to start or stop
+ * cdns_uart_break_ctl - Based on the input ctl we have to start or stop
  *			transmitting char breaks
  * @port: Handle to the uart port structure
  * @ctl: Value based on which start or stop decision is taken
- *
- **/
-static void xuartps_break_ctl(struct uart_port *port, int ctl)
+ */
+static void cdns_uart_break_ctl(struct uart_port *port, int ctl)
 {
 	unsigned int status;
 	unsigned long flags;
 
 	spin_lock_irqsave(&port->lock, flags);
 
-	status = xuartps_readl(XUARTPS_CR_OFFSET);
+	status = cdns_uart_readl(CDNS_UART_CR_OFFSET);
 
 	if (ctl == -1)
-		xuartps_writel(XUARTPS_CR_STARTBRK | status,
-					XUARTPS_CR_OFFSET);
+		cdns_uart_writel(CDNS_UART_CR_STARTBRK | status,
+					CDNS_UART_CR_OFFSET);
 	else {
-		if ((status & XUARTPS_CR_STOPBRK) == 0)
-			xuartps_writel(XUARTPS_CR_STOPBRK | status,
-					 XUARTPS_CR_OFFSET);
+		if ((status & CDNS_UART_CR_STOPBRK) == 0)
+			cdns_uart_writel(CDNS_UART_CR_STOPBRK | status,
+					 CDNS_UART_CR_OFFSET);
 	}
 	spin_unlock_irqrestore(&port->lock, flags);
 }
 
 /**
- * xuartps_set_termios - termios operations, handling data length, parity,
+ * cdns_uart_set_termios - termios operations, handling data length, parity,
  *				stop bits, flow control, baud rate
  * @port: Handle to the uart port structure
  * @termios: Handle to the input termios structure
  * @old: Values of the previously saved termios structure
- *
- **/
-static void xuartps_set_termios(struct uart_port *port,
+ */
+static void cdns_uart_set_termios(struct uart_port *port,
 				struct ktermios *termios, struct ktermios *old)
 {
 	unsigned int cval = 0;
@@ -649,81 +629,79 @@ static void xuartps_set_termios(struct uart_port *port,
 	spin_lock_irqsave(&port->lock, flags);
 
 	/* Empty the receive FIFO 1st before making changes */
-	while ((xuartps_readl(XUARTPS_SR_OFFSET) &
-		 XUARTPS_SR_RXEMPTY) != XUARTPS_SR_RXEMPTY) {
-		xuartps_readl(XUARTPS_FIFO_OFFSET);
+	while ((cdns_uart_readl(CDNS_UART_SR_OFFSET) &
+		 CDNS_UART_SR_RXEMPTY) != CDNS_UART_SR_RXEMPTY) {
+		cdns_uart_readl(CDNS_UART_FIFO_OFFSET);
 	}
 
 	/* Disable the TX and RX to set baud rate */
-	xuartps_writel(xuartps_readl(XUARTPS_CR_OFFSET) |
-			(XUARTPS_CR_TX_DIS | XUARTPS_CR_RX_DIS),
-			XUARTPS_CR_OFFSET);
+	ctrl_reg = cdns_uart_readl(CDNS_UART_CR_OFFSET);
+	ctrl_reg |= CDNS_UART_CR_TX_DIS | CDNS_UART_CR_RX_DIS;
+	cdns_uart_writel(ctrl_reg, CDNS_UART_CR_OFFSET);
 
 	/*
 	 * Min baud rate = 6bps and Max Baud Rate is 10Mbps for 100Mhz clk
 	 * min and max baud should be calculated here based on port->uartclk.
 	 * this way we get a valid baud and can safely call set_baud()
 	 */
-	minbaud = port->uartclk / ((XUARTPS_BDIV_MAX + 1) * XUARTPS_CD_MAX * 8);
-	maxbaud = port->uartclk / (XUARTPS_BDIV_MIN + 1);
+	minbaud = port->uartclk /
+			((CDNS_UART_BDIV_MAX + 1) * CDNS_UART_CD_MAX * 8);
+	maxbaud = port->uartclk / (CDNS_UART_BDIV_MIN + 1);
 	baud = uart_get_baud_rate(port, termios, old, minbaud, maxbaud);
-	baud = xuartps_set_baud_rate(port, baud);
+	baud = cdns_uart_set_baud_rate(port, baud);
 	if (tty_termios_baud_rate(termios))
 		tty_termios_encode_baud_rate(termios, baud, baud);
 
-	/*
-	 * Update the per-port timeout.
-	 */
+	/* Update the per-port timeout. */
 	uart_update_timeout(port, termios->c_cflag, baud);
 
 	/* Set TX/RX Reset */
-	xuartps_writel(xuartps_readl(XUARTPS_CR_OFFSET) |
-			(XUARTPS_CR_TXRST | XUARTPS_CR_RXRST),
-			XUARTPS_CR_OFFSET);
+	ctrl_reg = cdns_uart_readl(CDNS_UART_CR_OFFSET);
+	ctrl_reg |= CDNS_UART_CR_TXRST | CDNS_UART_CR_RXRST;
+	cdns_uart_writel(ctrl_reg, CDNS_UART_CR_OFFSET);
 
-	ctrl_reg = xuartps_readl(XUARTPS_CR_OFFSET);
-
-	/* Clear the RX disable and TX disable bits and then set the TX enable
+	/*
+	 * Clear the RX disable and TX disable bits and then set the TX enable
 	 * bit and RX enable bit to enable the transmitter and receiver.
 	 */
-	xuartps_writel(
-		(ctrl_reg & ~(XUARTPS_CR_TX_DIS | XUARTPS_CR_RX_DIS))
-			| (XUARTPS_CR_TX_EN | XUARTPS_CR_RX_EN),
-			XUARTPS_CR_OFFSET);
+	ctrl_reg = cdns_uart_readl(CDNS_UART_CR_OFFSET);
+	ctrl_reg &= ~(CDNS_UART_CR_TX_DIS | CDNS_UART_CR_RX_DIS);
+	ctrl_reg |= CDNS_UART_CR_TX_EN | CDNS_UART_CR_RX_EN;
+	cdns_uart_writel(ctrl_reg, CDNS_UART_CR_OFFSET);
 
-	xuartps_writel(rx_timeout, XUARTPS_RXTOUT_OFFSET);
+	cdns_uart_writel(rx_timeout, CDNS_UART_RXTOUT_OFFSET);
 
-	port->read_status_mask = XUARTPS_IXR_TXEMPTY | XUARTPS_IXR_RXTRIG |
-			XUARTPS_IXR_OVERRUN | XUARTPS_IXR_TOUT;
+	port->read_status_mask = CDNS_UART_IXR_TXEMPTY | CDNS_UART_IXR_RXTRIG |
+			CDNS_UART_IXR_OVERRUN | CDNS_UART_IXR_TOUT;
 	port->ignore_status_mask = 0;
 
 	if (termios->c_iflag & INPCK)
-		port->read_status_mask |= XUARTPS_IXR_PARITY |
-		XUARTPS_IXR_FRAMING;
+		port->read_status_mask |= CDNS_UART_IXR_PARITY |
+		CDNS_UART_IXR_FRAMING;
 
 	if (termios->c_iflag & IGNPAR)
-		port->ignore_status_mask |= XUARTPS_IXR_PARITY |
-			XUARTPS_IXR_FRAMING | XUARTPS_IXR_OVERRUN;
+		port->ignore_status_mask |= CDNS_UART_IXR_PARITY |
+			CDNS_UART_IXR_FRAMING | CDNS_UART_IXR_OVERRUN;
 
 	/* ignore all characters if CREAD is not set */
 	if ((termios->c_cflag & CREAD) == 0)
-		port->ignore_status_mask |= XUARTPS_IXR_RXTRIG |
-			XUARTPS_IXR_TOUT | XUARTPS_IXR_PARITY |
-			XUARTPS_IXR_FRAMING | XUARTPS_IXR_OVERRUN;
+		port->ignore_status_mask |= CDNS_UART_IXR_RXTRIG |
+			CDNS_UART_IXR_TOUT | CDNS_UART_IXR_PARITY |
+			CDNS_UART_IXR_FRAMING | CDNS_UART_IXR_OVERRUN;
 
-	mode_reg = xuartps_readl(XUARTPS_MR_OFFSET);
+	mode_reg = cdns_uart_readl(CDNS_UART_MR_OFFSET);
 
 	/* Handling Data Size */
 	switch (termios->c_cflag & CSIZE) {
 	case CS6:
-		cval |= XUARTPS_MR_CHARLEN_6_BIT;
+		cval |= CDNS_UART_MR_CHARLEN_6_BIT;
 		break;
 	case CS7:
-		cval |= XUARTPS_MR_CHARLEN_7_BIT;
+		cval |= CDNS_UART_MR_CHARLEN_7_BIT;
 		break;
 	default:
 	case CS8:
-		cval |= XUARTPS_MR_CHARLEN_8_BIT;
+		cval |= CDNS_UART_MR_CHARLEN_8_BIT;
 		termios->c_cflag &= ~CSIZE;
 		termios->c_cflag |= CS8;
 		break;
@@ -731,134 +709,135 @@ static void xuartps_set_termios(struct uart_port *port,
 
 	/* Handling Parity and Stop Bits length */
 	if (termios->c_cflag & CSTOPB)
-		cval |= XUARTPS_MR_STOPMODE_2_BIT; /* 2 STOP bits */
+		cval |= CDNS_UART_MR_STOPMODE_2_BIT; /* 2 STOP bits */
 	else
-		cval |= XUARTPS_MR_STOPMODE_1_BIT; /* 1 STOP bit */
+		cval |= CDNS_UART_MR_STOPMODE_1_BIT; /* 1 STOP bit */
 
 	if (termios->c_cflag & PARENB) {
 		/* Mark or Space parity */
 		if (termios->c_cflag & CMSPAR) {
 			if (termios->c_cflag & PARODD)
-				cval |= XUARTPS_MR_PARITY_MARK;
+				cval |= CDNS_UART_MR_PARITY_MARK;
 			else
-				cval |= XUARTPS_MR_PARITY_SPACE;
+				cval |= CDNS_UART_MR_PARITY_SPACE;
 		} else {
 			if (termios->c_cflag & PARODD)
-				cval |= XUARTPS_MR_PARITY_ODD;
+				cval |= CDNS_UART_MR_PARITY_ODD;
 			else
-				cval |= XUARTPS_MR_PARITY_EVEN;
+				cval |= CDNS_UART_MR_PARITY_EVEN;
 		}
 	} else {
-		cval |= XUARTPS_MR_PARITY_NONE;
+		cval |= CDNS_UART_MR_PARITY_NONE;
 	}
 	cval |= mode_reg & 1;
-	xuartps_writel(cval, XUARTPS_MR_OFFSET);
+	cdns_uart_writel(cval, CDNS_UART_MR_OFFSET);
 
 	spin_unlock_irqrestore(&port->lock, flags);
 }
 
 /**
- * xuartps_startup - Called when an application opens a xuartps port
+ * cdns_uart_startup - Called when an application opens a cdns_uart port
  * @port: Handle to the uart port structure
  *
- * Returns 0 on success, negative error otherwise
- **/
-static int xuartps_startup(struct uart_port *port)
+ * Return: 0 on success, negative errno otherwise
+ */
+static int cdns_uart_startup(struct uart_port *port)
 {
 	unsigned int retval = 0, status = 0;
 
-	retval = request_irq(port->irq, xuartps_isr, 0, XUARTPS_NAME,
+	retval = request_irq(port->irq, cdns_uart_isr, 0, CDNS_UART_NAME,
 								(void *)port);
 	if (retval)
 		return retval;
 
 	/* Disable the TX and RX */
-	xuartps_writel(XUARTPS_CR_TX_DIS | XUARTPS_CR_RX_DIS,
-						XUARTPS_CR_OFFSET);
+	cdns_uart_writel(CDNS_UART_CR_TX_DIS | CDNS_UART_CR_RX_DIS,
+						CDNS_UART_CR_OFFSET);
 
 	/* Set the Control Register with TX/RX Enable, TX/RX Reset,
 	 * no break chars.
 	 */
-	xuartps_writel(XUARTPS_CR_TXRST | XUARTPS_CR_RXRST,
-				XUARTPS_CR_OFFSET);
+	cdns_uart_writel(CDNS_UART_CR_TXRST | CDNS_UART_CR_RXRST,
+				CDNS_UART_CR_OFFSET);
 
-	status = xuartps_readl(XUARTPS_CR_OFFSET);
+	status = cdns_uart_readl(CDNS_UART_CR_OFFSET);
 
 	/* Clear the RX disable and TX disable bits and then set the TX enable
 	 * bit and RX enable bit to enable the transmitter and receiver.
 	 */
-	xuartps_writel((status & ~(XUARTPS_CR_TX_DIS | XUARTPS_CR_RX_DIS))
-			| (XUARTPS_CR_TX_EN | XUARTPS_CR_RX_EN |
-			XUARTPS_CR_STOPBRK), XUARTPS_CR_OFFSET);
+	cdns_uart_writel((status & ~(CDNS_UART_CR_TX_DIS | CDNS_UART_CR_RX_DIS))
+			| (CDNS_UART_CR_TX_EN | CDNS_UART_CR_RX_EN |
+			CDNS_UART_CR_STOPBRK), CDNS_UART_CR_OFFSET);
 
 	/* Set the Mode Register with normal mode,8 data bits,1 stop bit,
 	 * no parity.
 	 */
-	xuartps_writel(XUARTPS_MR_CHMODE_NORM | XUARTPS_MR_STOPMODE_1_BIT
-		| XUARTPS_MR_PARITY_NONE | XUARTPS_MR_CHARLEN_8_BIT,
-		 XUARTPS_MR_OFFSET);
+	cdns_uart_writel(CDNS_UART_MR_CHMODE_NORM | CDNS_UART_MR_STOPMODE_1_BIT
+		| CDNS_UART_MR_PARITY_NONE | CDNS_UART_MR_CHARLEN_8_BIT,
+		 CDNS_UART_MR_OFFSET);
 
 	/*
 	 * Set the RX FIFO Trigger level to use most of the FIFO, but it
 	 * can be tuned with a module parameter
 	 */
-	xuartps_writel(rx_trigger_level, XUARTPS_RXWM_OFFSET);
+	cdns_uart_writel(rx_trigger_level, CDNS_UART_RXWM_OFFSET);
 
 	/*
 	 * Receive Timeout register is enabled but it
 	 * can be tuned with a module parameter
 	 */
-	xuartps_writel(rx_timeout, XUARTPS_RXTOUT_OFFSET);
+	cdns_uart_writel(rx_timeout, CDNS_UART_RXTOUT_OFFSET);
 
 	/* Clear out any pending interrupts before enabling them */
-	xuartps_writel(xuartps_readl(XUARTPS_ISR_OFFSET), XUARTPS_ISR_OFFSET);
+	cdns_uart_writel(cdns_uart_readl(CDNS_UART_ISR_OFFSET),
+			CDNS_UART_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);
+	cdns_uart_writel(CDNS_UART_IXR_TXEMPTY | CDNS_UART_IXR_PARITY |
+		CDNS_UART_IXR_FRAMING | CDNS_UART_IXR_OVERRUN |
+		CDNS_UART_IXR_RXTRIG | CDNS_UART_IXR_TOUT,
+		CDNS_UART_IER_OFFSET);
 
 	return retval;
 }
 
 /**
- * xuartps_shutdown - Called when an application closes a xuartps port
+ * cdns_uart_shutdown - Called when an application closes a cdns_uart port
  * @port: Handle to the uart port structure
- *
- **/
-static void xuartps_shutdown(struct uart_port *port)
+ */
+static void cdns_uart_shutdown(struct uart_port *port)
 {
 	int status;
 
 	/* Disable interrupts */
-	status = xuartps_readl(XUARTPS_IMR_OFFSET);
-	xuartps_writel(status, XUARTPS_IDR_OFFSET);
+	status = cdns_uart_readl(CDNS_UART_IMR_OFFSET);
+	cdns_uart_writel(status, CDNS_UART_IDR_OFFSET);
 
 	/* Disable the TX and RX */
-	xuartps_writel(XUARTPS_CR_TX_DIS | XUARTPS_CR_RX_DIS,
-				 XUARTPS_CR_OFFSET);
+	cdns_uart_writel(CDNS_UART_CR_TX_DIS | CDNS_UART_CR_RX_DIS,
+				 CDNS_UART_CR_OFFSET);
 	free_irq(port->irq, port);
 }
 
 /**
- * xuartps_type - Set UART type to xuartps port
+ * cdns_uart_type - Set UART type to cdns_uart port
  * @port: Handle to the uart port structure
  *
- * Returns string on success, NULL otherwise
- **/
-static const char *xuartps_type(struct uart_port *port)
+ * Return: string on success, NULL otherwise
+ */
+static const char *cdns_uart_type(struct uart_port *port)
 {
-	return port->type == PORT_XUARTPS ? XUARTPS_NAME : NULL;
+	return port->type == PORT_XUARTPS ? CDNS_UART_NAME : NULL;
 }
 
 /**
- * xuartps_verify_port - Verify the port params
+ * cdns_uart_verify_port - Verify the port params
  * @port: Handle to the uart port structure
  * @ser: Handle to the structure whose members are compared
  *
- * Returns 0 if success otherwise -EINVAL
- **/
-static int xuartps_verify_port(struct uart_port *port,
+ * Return: 0 on success, negative errno otherwise.
+ */
+static int cdns_uart_verify_port(struct uart_port *port,
 					struct serial_struct *ser)
 {
 	if (ser->type != PORT_UNKNOWN && ser->type != PORT_XUARTPS)
@@ -875,187 +854,170 @@ static int xuartps_verify_port(struct uart_port *port,
 }
 
 /**
- * xuartps_request_port - Claim the memory region attached to xuartps port,
- *				called when the driver adds a xuartps port via
+ * cdns_uart_request_port - Claim the memory region attached to cdns_uart port,
+ *				called when the driver adds a cdns_uart port via
  *				uart_add_one_port()
  * @port: Handle to the uart port structure
  *
- * Returns 0, -ENOMEM if request fails
- **/
-static int xuartps_request_port(struct uart_port *port)
+ * Return: 0 on success, negative errno otherwise.
+ */
+static int cdns_uart_request_port(struct uart_port *port)
 {
-	if (!request_mem_region(port->mapbase, XUARTPS_REGISTER_SPACE,
-					 XUARTPS_NAME)) {
+	if (!request_mem_region(port->mapbase, CDNS_UART_REGISTER_SPACE,
+					 CDNS_UART_NAME)) {
 		return -ENOMEM;
 	}
 
-	port->membase = ioremap(port->mapbase, XUARTPS_REGISTER_SPACE);
+	port->membase = ioremap(port->mapbase, CDNS_UART_REGISTER_SPACE);
 	if (!port->membase) {
 		dev_err(port->dev, "Unable to map registers\n");
-		release_mem_region(port->mapbase, XUARTPS_REGISTER_SPACE);
+		release_mem_region(port->mapbase, CDNS_UART_REGISTER_SPACE);
 		return -ENOMEM;
 	}
 	return 0;
 }
 
 /**
- * xuartps_release_port - Release the memory region attached to a xuartps
- *				port, called when the driver removes a xuartps
- *				port via uart_remove_one_port().
+ * cdns_uart_release_port - Release UART port
  * @port: Handle to the uart port structure
  *
- **/
-static void xuartps_release_port(struct uart_port *port)
+ * Release the memory region attached to a cdns_uart port. Called when the
+ * driver removes a cdns_uart port via uart_remove_one_port().
+ */
+static void cdns_uart_release_port(struct uart_port *port)
 {
-	release_mem_region(port->mapbase, XUARTPS_REGISTER_SPACE);
+	release_mem_region(port->mapbase, CDNS_UART_REGISTER_SPACE);
 	iounmap(port->membase);
 	port->membase = NULL;
 }
 
 /**
- * xuartps_config_port - Configure xuartps, called when the driver adds a
- *				xuartps port
+ * cdns_uart_config_port - Configure UART port
  * @port: Handle to the uart port structure
  * @flags: If any
- *
- **/
-static void xuartps_config_port(struct uart_port *port, int flags)
+ */
+static void cdns_uart_config_port(struct uart_port *port, int flags)
 {
-	if (flags & UART_CONFIG_TYPE && xuartps_request_port(port) == 0)
+	if (flags & UART_CONFIG_TYPE && cdns_uart_request_port(port) == 0)
 		port->type = PORT_XUARTPS;
 }
 
 /**
- * xuartps_get_mctrl - Get the modem control state
- *
+ * cdns_uart_get_mctrl - Get the modem control state
  * @port: Handle to the uart port structure
  *
- * Returns the modem control state
- *
- **/
-static unsigned int xuartps_get_mctrl(struct uart_port *port)
+ * Return: the modem control state
+ */
+static unsigned int cdns_uart_get_mctrl(struct uart_port *port)
 {
 	return TIOCM_CTS | TIOCM_DSR | TIOCM_CAR;
 }
 
-static void xuartps_set_mctrl(struct uart_port *port, unsigned int mctrl)
+static void cdns_uart_set_mctrl(struct uart_port *port, unsigned int mctrl)
 {
 	/* N/A */
 }
 
-static void xuartps_enable_ms(struct uart_port *port)
+static void cdns_uart_enable_ms(struct uart_port *port)
 {
 	/* N/A */
 }
 
 #ifdef CONFIG_CONSOLE_POLL
-static int xuartps_poll_get_char(struct uart_port *port)
+static int cdns_uart_poll_get_char(struct uart_port *port)
 {
 	u32 imr;
 	int c;
 
 	/* Disable all interrupts */
-	imr = xuartps_readl(XUARTPS_IMR_OFFSET);
-	xuartps_writel(imr, XUARTPS_IDR_OFFSET);
+	imr = cdns_uart_readl(CDNS_UART_IMR_OFFSET);
+	cdns_uart_writel(imr, CDNS_UART_IDR_OFFSET);
 
 	/* Check if FIFO is empty */
-	if (xuartps_readl(XUARTPS_SR_OFFSET) & XUARTPS_SR_RXEMPTY)
+	if (cdns_uart_readl(CDNS_UART_SR_OFFSET) & CDNS_UART_SR_RXEMPTY)
 		c = NO_POLL_CHAR;
 	else /* Read a character */
-		c = (unsigned char) xuartps_readl(XUARTPS_FIFO_OFFSET);
+		c = (unsigned char) cdns_uart_readl(CDNS_UART_FIFO_OFFSET);
 
 	/* Enable interrupts */
-	xuartps_writel(imr, XUARTPS_IER_OFFSET);
+	cdns_uart_writel(imr, CDNS_UART_IER_OFFSET);
 
 	return c;
 }
 
-static void xuartps_poll_put_char(struct uart_port *port, unsigned char c)
+static void cdns_uart_poll_put_char(struct uart_port *port, unsigned char c)
 {
 	u32 imr;
 
 	/* Disable all interrupts */
-	imr = xuartps_readl(XUARTPS_IMR_OFFSET);
-	xuartps_writel(imr, XUARTPS_IDR_OFFSET);
+	imr = cdns_uart_readl(CDNS_UART_IMR_OFFSET);
+	cdns_uart_writel(imr, CDNS_UART_IDR_OFFSET);
 
 	/* Wait until FIFO is empty */
-	while (!(xuartps_readl(XUARTPS_SR_OFFSET) & XUARTPS_SR_TXEMPTY))
+	while (!(cdns_uart_readl(CDNS_UART_SR_OFFSET) & CDNS_UART_SR_TXEMPTY))
 		cpu_relax();
 
 	/* Write a character */
-	xuartps_writel(c, XUARTPS_FIFO_OFFSET);
+	cdns_uart_writel(c, CDNS_UART_FIFO_OFFSET);
 
 	/* Wait until FIFO is empty */
-	while (!(xuartps_readl(XUARTPS_SR_OFFSET) & XUARTPS_SR_TXEMPTY))
+	while (!(cdns_uart_readl(CDNS_UART_SR_OFFSET) & CDNS_UART_SR_TXEMPTY))
 		cpu_relax();
 
 	/* Enable interrupts */
-	xuartps_writel(imr, XUARTPS_IER_OFFSET);
+	cdns_uart_writel(imr, CDNS_UART_IER_OFFSET);
 
 	return;
 }
 #endif
 
-/** The UART operations structure
- */
-static struct uart_ops xuartps_ops = {
-	.set_mctrl	= xuartps_set_mctrl,
-	.get_mctrl	= xuartps_get_mctrl,
-	.enable_ms	= xuartps_enable_ms,
-
-	.start_tx	= xuartps_start_tx,	/* Start transmitting */
-	.stop_tx	= xuartps_stop_tx,	/* Stop transmission */
-	.stop_rx	= xuartps_stop_rx,	/* Stop reception */
-	.tx_empty	= xuartps_tx_empty,	/* Transmitter busy? */
-	.break_ctl	= xuartps_break_ctl,	/* Start/stop
-						 * transmitting break
-						 */
-	.set_termios	= xuartps_set_termios,	/* Set termios */
-	.startup	= xuartps_startup,	/* App opens xuartps */
-	.shutdown	= xuartps_shutdown,	/* App closes xuartps */
-	.type		= xuartps_type,		/* Set UART type */
-	.verify_port	= xuartps_verify_port,	/* Verification of port
-						 * params
-						 */
-	.request_port	= xuartps_request_port,	/* Claim resources
-						 * associated with a
-						 * xuartps port
-						 */
-	.release_port	= xuartps_release_port,	/* Release resources
-						 * associated with a
-						 * xuartps port
-						 */
-	.config_port	= xuartps_config_port,	/* Configure when driver
-						 * adds a xuartps port
-						 */
+static struct uart_ops cdns_uart_ops = {
+	.set_mctrl	= cdns_uart_set_mctrl,
+	.get_mctrl	= cdns_uart_get_mctrl,
+	.enable_ms	= cdns_uart_enable_ms,
+	.start_tx	= cdns_uart_start_tx,
+	.stop_tx	= cdns_uart_stop_tx,
+	.stop_rx	= cdns_uart_stop_rx,
+	.tx_empty	= cdns_uart_tx_empty,
+	.break_ctl	= cdns_uart_break_ctl,
+	.set_termios	= cdns_uart_set_termios,
+	.startup	= cdns_uart_startup,
+	.shutdown	= cdns_uart_shutdown,
+	.type		= cdns_uart_type,
+	.verify_port	= cdns_uart_verify_port,
+	.request_port	= cdns_uart_request_port,
+	.release_port	= cdns_uart_release_port,
+	.config_port	= cdns_uart_config_port,
 #ifdef CONFIG_CONSOLE_POLL
-	.poll_get_char	= xuartps_poll_get_char,
-	.poll_put_char	= xuartps_poll_put_char,
+	.poll_get_char	= cdns_uart_poll_get_char,
+	.poll_put_char	= cdns_uart_poll_put_char,
 #endif
 };
 
-static struct uart_port xuartps_port[2];
+static struct uart_port cdns_uart_port[2];
 
 /**
- * xuartps_get_port - Configure the port from the platform device resource
- *			info
+ * cdns_uart_get_port - Configure the port from platform device resource info
+ * @id: Port id
  *
- * Returns a pointer to a uart_port or NULL for failure
- **/
-static struct uart_port *xuartps_get_port(void)
+ * Return: a pointer to a uart_port or NULL for failure
+ */
+static struct uart_port *cdns_uart_get_port(int id)
 {
 	struct uart_port *port;
-	int id;
 
-	/* Find the next unused port */
-	for (id = 0; id < XUARTPS_NR_PORTS; id++)
-		if (xuartps_port[id].mapbase == 0)
-			break;
+	/* Try the given port id if failed use default method */
+	if (cdns_uart_port[id].mapbase != 0) {
+		/* Find the next unused port */
+		for (id = 0; id < CDNS_UART_NR_PORTS; id++)
+			if (cdns_uart_port[id].mapbase == 0)
+				break;
+	}
 
-	if (id >= XUARTPS_NR_PORTS)
+	if (id >= CDNS_UART_NR_PORTS)
 		return NULL;
 
-	port = &xuartps_port[id];
+	port = &cdns_uart_port[id];
 
 	/* At this point, we've got an empty uart_port struct, initialize it */
 	spin_lock_init(&port->lock);
@@ -1065,50 +1027,46 @@ static struct uart_port *xuartps_get_port(void)
 	port->type	= PORT_UNKNOWN;
 	port->iotype	= UPIO_MEM32;
 	port->flags	= UPF_BOOT_AUTOCONF;
-	port->ops	= &xuartps_ops;
-	port->fifosize	= XUARTPS_FIFO_SIZE;
+	port->ops	= &cdns_uart_ops;
+	port->fifosize	= CDNS_UART_FIFO_SIZE;
 	port->line	= id;
 	port->dev	= NULL;
 	return port;
 }
 
-/*-----------------------Console driver operations--------------------------*/
-
 #ifdef CONFIG_SERIAL_XILINX_PS_UART_CONSOLE
 /**
- * xuartps_console_wait_tx - Wait for the TX to be full
+ * cdns_uart_console_wait_tx - Wait for the TX to be full
  * @port: Handle to the uart port structure
- *
- **/
-static void xuartps_console_wait_tx(struct uart_port *port)
+ */
+static void cdns_uart_console_wait_tx(struct uart_port *port)
 {
-	while ((xuartps_readl(XUARTPS_SR_OFFSET) & XUARTPS_SR_TXEMPTY)
-				!= XUARTPS_SR_TXEMPTY)
+	while ((cdns_uart_readl(CDNS_UART_SR_OFFSET) & CDNS_UART_SR_TXEMPTY)
+				!= CDNS_UART_SR_TXEMPTY)
 		barrier();
 }
 
 /**
- * xuartps_console_putchar - write the character to the FIFO buffer
+ * cdns_uart_console_putchar - write the character to the FIFO buffer
  * @port: Handle to the uart port structure
  * @ch: Character to be written
- *
- **/
-static void xuartps_console_putchar(struct uart_port *port, int ch)
+ */
+static void cdns_uart_console_putchar(struct uart_port *port, int ch)
 {
-	xuartps_console_wait_tx(port);
-	xuartps_writel(ch, XUARTPS_FIFO_OFFSET);
+	cdns_uart_console_wait_tx(port);
+	cdns_uart_writel(ch, CDNS_UART_FIFO_OFFSET);
 }
 
 /**
- * xuartps_console_write - perform write operation
- * @port: Handle to the uart port structure
+ * cdns_uart_console_write - perform write operation
+ * @co: Console handle
  * @s: Pointer to character array
  * @count: No of characters
- **/
-static void xuartps_console_write(struct console *co, const char *s,
+ */
+static void cdns_uart_console_write(struct console *co, const char *s,
 				unsigned int count)
 {
-	struct uart_port *port = &xuartps_port[co->index];
+	struct uart_port *port = &cdns_uart_port[co->index];
 	unsigned long flags;
 	unsigned int imr, ctrl;
 	int locked = 1;
@@ -1119,49 +1077,45 @@ static void xuartps_console_write(struct console *co, const char *s,
 		spin_lock_irqsave(&port->lock, flags);
 
 	/* save and disable interrupt */
-	imr = xuartps_readl(XUARTPS_IMR_OFFSET);
-	xuartps_writel(imr, XUARTPS_IDR_OFFSET);
+	imr = cdns_uart_readl(CDNS_UART_IMR_OFFSET);
+	cdns_uart_writel(imr, CDNS_UART_IDR_OFFSET);
 
 	/*
 	 * Make sure that the tx part is enabled. Set the TX enable bit and
 	 * clear the TX disable bit to enable the transmitter.
 	 */
-	ctrl = xuartps_readl(XUARTPS_CR_OFFSET);
-	xuartps_writel((ctrl & ~XUARTPS_CR_TX_DIS) | XUARTPS_CR_TX_EN,
-		XUARTPS_CR_OFFSET);
+	ctrl = cdns_uart_readl(CDNS_UART_CR_OFFSET);
+	cdns_uart_writel((ctrl & ~CDNS_UART_CR_TX_DIS) | CDNS_UART_CR_TX_EN,
+		CDNS_UART_CR_OFFSET);
 
-	uart_console_write(port, s, count, xuartps_console_putchar);
-	xuartps_console_wait_tx(port);
+	uart_console_write(port, s, count, cdns_uart_console_putchar);
+	cdns_uart_console_wait_tx(port);
 
-	xuartps_writel(ctrl, XUARTPS_CR_OFFSET);
+	cdns_uart_writel(ctrl, CDNS_UART_CR_OFFSET);
 
-	/* restore interrupt state, it seems like there may be a h/w bug
-	 * in that the interrupt enable register should not need to be
-	 * written based on the data sheet
-	 */
-	xuartps_writel(~imr, XUARTPS_IDR_OFFSET);
-	xuartps_writel(imr, XUARTPS_IER_OFFSET);
+	/* restore interrupt state */
+	cdns_uart_writel(imr, CDNS_UART_IER_OFFSET);
 
 	if (locked)
 		spin_unlock_irqrestore(&port->lock, flags);
 }
 
 /**
- * xuartps_console_setup - Initialize the uart to default config
+ * cdns_uart_console_setup - Initialize the uart to default config
  * @co: Console handle
  * @options: Initial settings of uart
  *
- * Returns 0, -ENODEV if no device
- **/
-static int __init xuartps_console_setup(struct console *co, char *options)
+ * Return: 0 on success, negative errno otherwise.
+ */
+static int __init cdns_uart_console_setup(struct console *co, char *options)
 {
-	struct uart_port *port = &xuartps_port[co->index];
+	struct uart_port *port = &cdns_uart_port[co->index];
 	int baud = 9600;
 	int bits = 8;
 	int parity = 'n';
 	int flow = 'n';
 
-	if (co->index < 0 || co->index >= XUARTPS_NR_PORTS)
+	if (co->index < 0 || co->index >= CDNS_UART_NR_PORTS)
 		return -EINVAL;
 
 	if (!port->mapbase) {
@@ -1175,55 +1129,53 @@ static int __init xuartps_console_setup(struct console *co, char *options)
 	return uart_set_options(port, co, baud, parity, bits, flow);
 }
 
-static struct uart_driver xuartps_uart_driver;
+static struct uart_driver cdns_uart_uart_driver;
 
-static struct console xuartps_console = {
-	.name	= XUARTPS_TTY_NAME,
-	.write	= xuartps_console_write,
+static struct console cdns_uart_console = {
+	.name	= CDNS_UART_TTY_NAME,
+	.write	= cdns_uart_console_write,
 	.device	= uart_console_device,
-	.setup	= xuartps_console_setup,
+	.setup	= cdns_uart_console_setup,
 	.flags	= CON_PRINTBUFFER,
 	.index	= -1, /* Specified on the cmdline (e.g. console=ttyPS ) */
-	.data	= &xuartps_uart_driver,
+	.data	= &cdns_uart_uart_driver,
 };
 
 /**
- * xuartps_console_init - Initialization call
+ * cdns_uart_console_init - Initialization call
  *
- * Returns 0 on success, negative error otherwise
- **/
-static int __init xuartps_console_init(void)
+ * Return: 0 on success, negative errno otherwise
+ */
+static int __init cdns_uart_console_init(void)
 {
-	register_console(&xuartps_console);
+	register_console(&cdns_uart_console);
 	return 0;
 }
 
-console_initcall(xuartps_console_init);
+console_initcall(cdns_uart_console_init);
 
 #endif /* CONFIG_SERIAL_XILINX_PS_UART_CONSOLE */
 
-/** Structure Definitions
- */
-static struct uart_driver xuartps_uart_driver = {
-	.owner		= THIS_MODULE,		/* Owner */
-	.driver_name	= XUARTPS_NAME,		/* Driver name */
-	.dev_name	= XUARTPS_TTY_NAME,	/* Node name */
-	.major		= XUARTPS_MAJOR,	/* Major number */
-	.minor		= XUARTPS_MINOR,	/* Minor number */
-	.nr		= XUARTPS_NR_PORTS,	/* Number of UART ports */
+static struct uart_driver cdns_uart_uart_driver = {
+	.owner		= THIS_MODULE,
+	.driver_name	= CDNS_UART_NAME,
+	.dev_name	= CDNS_UART_TTY_NAME,
+	.major		= CDNS_UART_MAJOR,
+	.minor		= CDNS_UART_MINOR,
+	.nr		= CDNS_UART_NR_PORTS,
 #ifdef CONFIG_SERIAL_XILINX_PS_UART_CONSOLE
-	.cons		= &xuartps_console,	/* Console */
+	.cons		= &cdns_uart_console,
 #endif
 };
 
 #ifdef CONFIG_PM_SLEEP
 /**
- * xuartps_suspend - suspend event
+ * cdns_uart_suspend - suspend event
  * @device: Pointer to the device structure
  *
- * Returns 0
+ * Return: 0
  */
-static int xuartps_suspend(struct device *device)
+static int cdns_uart_suspend(struct device *device)
 {
 	struct uart_port *port = dev_get_drvdata(device);
 	struct tty_struct *tty;
@@ -1242,23 +1194,24 @@ static int xuartps_suspend(struct device *device)
 	 * Call the API provided in serial_core.c file which handles
 	 * the suspend.
 	 */
-	uart_suspend_port(&xuartps_uart_driver, port);
+	uart_suspend_port(&cdns_uart_uart_driver, port);
 	if (console_suspend_enabled && !may_wake) {
-		struct xuartps *xuartps = port->private_data;
+		struct cdns_uart *cdns_uart = port->private_data;
 
-		clk_disable(xuartps->refclk);
-		clk_disable(xuartps->aperclk);
+		clk_disable(cdns_uart->uartclk);
+		clk_disable(cdns_uart->pclk);
 	} else {
 		unsigned long flags = 0;
 
 		spin_lock_irqsave(&port->lock, flags);
 		/* Empty the receive FIFO 1st before making changes */
-		while (!(xuartps_readl(XUARTPS_SR_OFFSET) & XUARTPS_SR_RXEMPTY))
-			xuartps_readl(XUARTPS_FIFO_OFFSET);
+		while (!(cdns_uart_readl(CDNS_UART_SR_OFFSET) &
+					CDNS_UART_SR_RXEMPTY))
+			cdns_uart_readl(CDNS_UART_FIFO_OFFSET);
 		/* set RX trigger level to 1 */
-		xuartps_writel(1, XUARTPS_RXWM_OFFSET);
+		cdns_uart_writel(1, CDNS_UART_RXWM_OFFSET);
 		/* disable RX timeout interrups */
-		xuartps_writel(XUARTPS_IXR_TOUT, XUARTPS_IDR_OFFSET);
+		cdns_uart_writel(CDNS_UART_IXR_TOUT, CDNS_UART_IDR_OFFSET);
 		spin_unlock_irqrestore(&port->lock, flags);
 	}
 
@@ -1266,12 +1219,12 @@ static int xuartps_suspend(struct device *device)
 }
 
 /**
- * xuartps_resume - Resume after a previous suspend
+ * cdns_uart_resume - Resume after a previous suspend
  * @device: Pointer to the device structure
  *
- * Returns 0
+ * Return: 0
  */
-static int xuartps_resume(struct device *device)
+static int cdns_uart_resume(struct device *device)
 {
 	struct uart_port *port = dev_get_drvdata(device);
 	unsigned long flags = 0;
@@ -1289,87 +1242,95 @@ static int xuartps_resume(struct device *device)
 	}
 
 	if (console_suspend_enabled && !may_wake) {
-		struct xuartps *xuartps = port->private_data;
+		struct cdns_uart *cdns_uart = port->private_data;
 
-		clk_enable(xuartps->aperclk);
-		clk_enable(xuartps->refclk);
+		clk_enable(cdns_uart->pclk);
+		clk_enable(cdns_uart->uartclk);
 
 		spin_lock_irqsave(&port->lock, flags);
 
 		/* Set TX/RX Reset */
-		xuartps_writel(xuartps_readl(XUARTPS_CR_OFFSET) |
-				(XUARTPS_CR_TXRST | XUARTPS_CR_RXRST),
-				XUARTPS_CR_OFFSET);
-		while (xuartps_readl(XUARTPS_CR_OFFSET) &
-				(XUARTPS_CR_TXRST | XUARTPS_CR_RXRST))
+		ctrl_reg = cdns_uart_readl(CDNS_UART_CR_OFFSET);
+		ctrl_reg |= CDNS_UART_CR_TXRST | CDNS_UART_CR_RXRST;
+		cdns_uart_writel(ctrl_reg, CDNS_UART_CR_OFFSET);
+		while (cdns_uart_readl(CDNS_UART_CR_OFFSET) &
+				(CDNS_UART_CR_TXRST | CDNS_UART_CR_RXRST))
 			cpu_relax();
 
 		/* restore rx timeout value */
-		xuartps_writel(rx_timeout, XUARTPS_RXTOUT_OFFSET);
+		cdns_uart_writel(rx_timeout, CDNS_UART_RXTOUT_OFFSET);
 		/* Enable Tx/Rx */
-		ctrl_reg = xuartps_readl(XUARTPS_CR_OFFSET);
-		xuartps_writel(
-			(ctrl_reg & ~(XUARTPS_CR_TX_DIS | XUARTPS_CR_RX_DIS)) |
-			(XUARTPS_CR_TX_EN | XUARTPS_CR_RX_EN),
-			XUARTPS_CR_OFFSET);
+		ctrl_reg = cdns_uart_readl(CDNS_UART_CR_OFFSET);
+		ctrl_reg &= ~(CDNS_UART_CR_TX_DIS | CDNS_UART_CR_RX_DIS);
+		ctrl_reg |= CDNS_UART_CR_TX_EN | CDNS_UART_CR_RX_EN;
+		cdns_uart_writel(ctrl_reg, CDNS_UART_CR_OFFSET);
 
 		spin_unlock_irqrestore(&port->lock, flags);
 	} else {
 		spin_lock_irqsave(&port->lock, flags);
 		/* restore original rx trigger level */
-		xuartps_writel(rx_trigger_level, XUARTPS_RXWM_OFFSET);
+		cdns_uart_writel(rx_trigger_level, CDNS_UART_RXWM_OFFSET);
 		/* enable RX timeout interrupt */
-		xuartps_writel(XUARTPS_IXR_TOUT, XUARTPS_IER_OFFSET);
+		cdns_uart_writel(CDNS_UART_IXR_TOUT, CDNS_UART_IER_OFFSET);
 		spin_unlock_irqrestore(&port->lock, flags);
 	}
 
-	return uart_resume_port(&xuartps_uart_driver, port);
+	return uart_resume_port(&cdns_uart_uart_driver, port);
 }
 #endif /* ! CONFIG_PM_SLEEP */
 
-static SIMPLE_DEV_PM_OPS(xuartps_dev_pm_ops, xuartps_suspend, xuartps_resume);
+static SIMPLE_DEV_PM_OPS(cdns_uart_dev_pm_ops, cdns_uart_suspend,
+		cdns_uart_resume);
 
-/* ---------------------------------------------------------------------
- * Platform bus binding
- */
 /**
- * xuartps_probe - Platform driver probe
+ * cdns_uart_probe - Platform driver probe
  * @pdev: Pointer to the platform device structure
  *
- * Returns 0 on success, negative error otherwise
- **/
-static int xuartps_probe(struct platform_device *pdev)
+ * Return: 0 on success, negative errno otherwise
+ */
+static int cdns_uart_probe(struct platform_device *pdev)
 {
-	int rc;
+	int rc, id;
 	struct uart_port *port;
 	struct resource *res, *res2;
-	struct xuartps *xuartps_data;
+	struct cdns_uart *cdns_uart_data;
 
-	xuartps_data = devm_kzalloc(&pdev->dev, sizeof(*xuartps_data),
+	cdns_uart_data = devm_kzalloc(&pdev->dev, sizeof(*cdns_uart_data),
 			GFP_KERNEL);
-	if (!xuartps_data)
+	if (!cdns_uart_data)
 		return -ENOMEM;
 
-	xuartps_data->aperclk = devm_clk_get(&pdev->dev, "aper_clk");
-	if (IS_ERR(xuartps_data->aperclk)) {
-		dev_err(&pdev->dev, "aper_clk clock not found.\n");
-		return PTR_ERR(xuartps_data->aperclk);
+	cdns_uart_data->pclk = devm_clk_get(&pdev->dev, "pclk");
+	if (IS_ERR(cdns_uart_data->pclk)) {
+		cdns_uart_data->pclk = devm_clk_get(&pdev->dev, "aper_clk");
+		if (!IS_ERR(cdns_uart_data->pclk))
+			dev_err(&pdev->dev, "clock name 'aper_clk' is deprecated.\n");
 	}
-	xuartps_data->refclk = devm_clk_get(&pdev->dev, "ref_clk");
-	if (IS_ERR(xuartps_data->refclk)) {
-		dev_err(&pdev->dev, "ref_clk clock not found.\n");
-		return PTR_ERR(xuartps_data->refclk);
+	if (IS_ERR(cdns_uart_data->pclk)) {
+		dev_err(&pdev->dev, "pclk clock not found.\n");
+		return PTR_ERR(cdns_uart_data->pclk);
 	}
 
-	rc = clk_prepare_enable(xuartps_data->aperclk);
+	cdns_uart_data->uartclk = devm_clk_get(&pdev->dev, "uart_clk");
+	if (IS_ERR(cdns_uart_data->uartclk)) {
+		cdns_uart_data->uartclk = devm_clk_get(&pdev->dev, "ref_clk");
+		if (!IS_ERR(cdns_uart_data->uartclk))
+			dev_err(&pdev->dev, "clock name 'ref_clk' is deprecated.\n");
+	}
+	if (IS_ERR(cdns_uart_data->uartclk)) {
+		dev_err(&pdev->dev, "uart_clk clock not found.\n");
+		return PTR_ERR(cdns_uart_data->uartclk);
+	}
+
+	rc = clk_prepare_enable(cdns_uart_data->pclk);
 	if (rc) {
-		dev_err(&pdev->dev, "Unable to enable APER clock.\n");
+		dev_err(&pdev->dev, "Unable to enable pclk clock.\n");
 		return rc;
 	}
-	rc = clk_prepare_enable(xuartps_data->refclk);
+	rc = clk_prepare_enable(cdns_uart_data->uartclk);
 	if (rc) {
 		dev_err(&pdev->dev, "Unable to enable device clock.\n");
-		goto err_out_clk_dis_aper;
+		goto err_out_clk_dis_pclk;
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -1385,15 +1346,19 @@ static int xuartps_probe(struct platform_device *pdev)
 	}
 
 #ifdef CONFIG_COMMON_CLK
-	xuartps_data->clk_rate_change_nb.notifier_call =
-			xuartps_clk_notifier_cb;
-	if (clk_notifier_register(xuartps_data->refclk,
-				&xuartps_data->clk_rate_change_nb))
+	cdns_uart_data->clk_rate_change_nb.notifier_call =
+			cdns_uart_clk_notifier_cb;
+	if (clk_notifier_register(cdns_uart_data->uartclk,
+				&cdns_uart_data->clk_rate_change_nb))
 		dev_warn(&pdev->dev, "Unable to register clock notifier.\n");
 #endif
+	/* Look for a serialN alias */
+	id = of_alias_get_id(pdev->dev.of_node, "serial");
+	if (id < 0)
+		id = 0;
 
 	/* Initialize the port structure */
-	port = xuartps_get_port();
+	port = cdns_uart_get_port(id);
 
 	if (!port) {
 		dev_err(&pdev->dev, "Cannot get uart_port structure\n");
@@ -1407,11 +1372,11 @@ static int xuartps_probe(struct platform_device *pdev)
 		port->mapbase = res->start;
 		port->irq = res2->start;
 		port->dev = &pdev->dev;
-		port->uartclk = clk_get_rate(xuartps_data->refclk);
-		port->private_data = xuartps_data;
-		xuartps_data->port = port;
+		port->uartclk = clk_get_rate(cdns_uart_data->uartclk);
+		port->private_data = cdns_uart_data;
+		cdns_uart_data->port = port;
 		platform_set_drvdata(pdev, port);
-		rc = uart_add_one_port(&xuartps_uart_driver, port);
+		rc = uart_add_one_port(&cdns_uart_uart_driver, port);
 		if (rc) {
 			dev_err(&pdev->dev,
 				"uart_add_one_port() failed; err=%i\n", rc);
@@ -1422,103 +1387,89 @@ static int xuartps_probe(struct platform_device *pdev)
 
 err_out_notif_unreg:
 #ifdef CONFIG_COMMON_CLK
-	clk_notifier_unregister(xuartps_data->refclk,
-			&xuartps_data->clk_rate_change_nb);
+	clk_notifier_unregister(cdns_uart_data->uartclk,
+			&cdns_uart_data->clk_rate_change_nb);
 #endif
 err_out_clk_disable:
-	clk_disable_unprepare(xuartps_data->refclk);
-err_out_clk_dis_aper:
-	clk_disable_unprepare(xuartps_data->aperclk);
+	clk_disable_unprepare(cdns_uart_data->uartclk);
+err_out_clk_dis_pclk:
+	clk_disable_unprepare(cdns_uart_data->pclk);
 
 	return rc;
 }
 
 /**
- * xuartps_remove - called when the platform driver is unregistered
+ * cdns_uart_remove - called when the platform driver is unregistered
  * @pdev: Pointer to the platform device structure
  *
- * Returns 0 on success, negative error otherwise
- **/
-static int xuartps_remove(struct platform_device *pdev)
+ * Return: 0 on success, negative errno otherwise
+ */
+static int cdns_uart_remove(struct platform_device *pdev)
 {
 	struct uart_port *port = platform_get_drvdata(pdev);
-	struct xuartps *xuartps_data = port->private_data;
+	struct cdns_uart *cdns_uart_data = port->private_data;
 	int rc;
 
-	/* Remove the xuartps port from the serial core */
+	/* Remove the cdns_uart port from the serial core */
 #ifdef CONFIG_COMMON_CLK
-	clk_notifier_unregister(xuartps_data->refclk,
-			&xuartps_data->clk_rate_change_nb);
+	clk_notifier_unregister(cdns_uart_data->uartclk,
+			&cdns_uart_data->clk_rate_change_nb);
 #endif
-	rc = uart_remove_one_port(&xuartps_uart_driver, port);
+	rc = uart_remove_one_port(&cdns_uart_uart_driver, port);
 	port->mapbase = 0;
-	clk_disable_unprepare(xuartps_data->refclk);
-	clk_disable_unprepare(xuartps_data->aperclk);
+	clk_disable_unprepare(cdns_uart_data->uartclk);
+	clk_disable_unprepare(cdns_uart_data->pclk);
 	return rc;
 }
 
 /* Match table for of_platform binding */
-static struct of_device_id xuartps_of_match[] = {
+static struct of_device_id cdns_uart_of_match[] = {
 	{ .compatible = "xlnx,xuartps", },
+	{ .compatible = "cdns,uart-r1p8", },
 	{}
 };
-MODULE_DEVICE_TABLE(of, xuartps_of_match);
+MODULE_DEVICE_TABLE(of, cdns_uart_of_match);
 
-static struct platform_driver xuartps_platform_driver = {
-	.probe   = xuartps_probe,		/* Probe method */
-	.remove  = xuartps_remove,		/* Detach method */
+static struct platform_driver cdns_uart_platform_driver = {
+	.probe   = cdns_uart_probe,
+	.remove  = cdns_uart_remove,
 	.driver  = {
 		.owner = THIS_MODULE,
-		.name = XUARTPS_NAME,		/* Driver name */
-		.of_match_table = xuartps_of_match,
-		.pm = &xuartps_dev_pm_ops,
+		.name = CDNS_UART_NAME,
+		.of_match_table = cdns_uart_of_match,
+		.pm = &cdns_uart_dev_pm_ops,
 		},
 };
 
-/* ---------------------------------------------------------------------
- * Module Init and Exit
- */
-/**
- * xuartps_init - Initial driver registration call
- *
- * Returns whether the registration was successful or not
- **/
-static int __init xuartps_init(void)
+static int __init cdns_uart_init(void)
 {
 	int retval = 0;
 
-	/* Register the xuartps driver with the serial core */
-	retval = uart_register_driver(&xuartps_uart_driver);
+	/* Register the cdns_uart driver with the serial core */
+	retval = uart_register_driver(&cdns_uart_uart_driver);
 	if (retval)
 		return retval;
 
 	/* Register the platform driver */
-	retval = platform_driver_register(&xuartps_platform_driver);
+	retval = platform_driver_register(&cdns_uart_platform_driver);
 	if (retval)
-		uart_unregister_driver(&xuartps_uart_driver);
+		uart_unregister_driver(&cdns_uart_uart_driver);
 
 	return retval;
 }
 
-/**
- * xuartps_exit - Driver unregistration call
- **/
-static void __exit xuartps_exit(void)
+static void __exit cdns_uart_exit(void)
 {
-	/* The order of unregistration is important. Unregister the
-	 * UART driver before the platform driver crashes the system.
-	 */
-
 	/* Unregister the platform driver */
-	platform_driver_unregister(&xuartps_platform_driver);
+	platform_driver_unregister(&cdns_uart_platform_driver);
 
-	/* Unregister the xuartps driver */
-	uart_unregister_driver(&xuartps_uart_driver);
+	/* Unregister the cdns_uart driver */
+	uart_unregister_driver(&cdns_uart_uart_driver);
 }
 
-module_init(xuartps_init);
-module_exit(xuartps_exit);
+module_init(cdns_uart_init);
+module_exit(cdns_uart_exit);
 
-MODULE_DESCRIPTION("Driver for PS UART");
+MODULE_DESCRIPTION("Driver for Cadence UART");
 MODULE_AUTHOR("Xilinx Inc.");
 MODULE_LICENSE("GPL");
diff --git a/drivers/tty/tty_buffer.c b/drivers/tty/tty_buffer.c
index 8ebd9f88a6f..cf78d1985cd 100644
--- a/drivers/tty/tty_buffer.c
+++ b/drivers/tty/tty_buffer.c
@@ -258,7 +258,11 @@ static int __tty_buffer_request_room(struct tty_port *port, size_t size,
 			n->flags = flags;
 			buf->tail = n;
 			b->commit = b->used;
-			smp_mb();
+			/* paired w/ barrier in flush_to_ldisc(); ensures the
+			 * latest commit value can be read before the head is
+			 * advanced to the next buffer
+			 */
+			smp_wmb();
 			b->next = n;
 		} else if (change)
 			size = 0;
@@ -444,17 +448,24 @@ static void flush_to_ldisc(struct work_struct *work)
 
 	while (1) {
 		struct tty_buffer *head = buf->head;
+		struct tty_buffer *next;
 		int count;
 
 		/* Ldisc or user is trying to gain exclusive access */
 		if (atomic_read(&buf->priority))
 			break;
 
+		next = head->next;
+		/* paired w/ barrier in __tty_buffer_request_room();
+		 * ensures commit value read is not stale if the head
+		 * is advancing to the next buffer
+		 */
+		smp_rmb();
 		count = head->commit - head->read;
 		if (!count) {
-			if (head->next == NULL)
+			if (next == NULL)
 				break;
-			buf->head = head->next;
+			buf->head = next;
 			tty_buffer_free(port, head);
 			continue;
 		}
diff --git a/drivers/tty/tty_io.c b/drivers/tty/tty_io.c
index d3448a90f0f..34110719fe0 100644
--- a/drivers/tty/tty_io.c
+++ b/drivers/tty/tty_io.c
@@ -878,9 +878,8 @@ void disassociate_ctty(int on_exit)
 	spin_lock_irq(&current->sighand->siglock);
 	put_pid(current->signal->tty_old_pgrp);
 	current->signal->tty_old_pgrp = NULL;
-	spin_unlock_irq(&current->sighand->siglock);
 
-	tty = get_current_tty();
+	tty = tty_kref_get(current->signal->tty);
 	if (tty) {
 		unsigned long flags;
 		spin_lock_irqsave(&tty->ctrl_lock, flags);
@@ -897,6 +896,7 @@ void disassociate_ctty(int on_exit)
 #endif
 	}
 
+	spin_unlock_irq(&current->sighand->siglock);
 	/* Now clear signal->tty under the lock */
 	read_lock(&tasklist_lock);
 	session_clear_tty(task_session(current));
diff --git a/drivers/tty/vt/consolemap.c b/drivers/tty/vt/consolemap.c
index 2978ca596a7..610b720d3b9 100644
--- a/drivers/tty/vt/consolemap.c
+++ b/drivers/tty/vt/consolemap.c
@@ -179,7 +179,6 @@ struct uni_pagedir {
 	unsigned long	sum;
 	unsigned char	*inverse_translations[4];
 	u16		*inverse_trans_unicode;
-	int		readonly;
 };
 
 static struct uni_pagedir *dflt;
@@ -262,7 +261,7 @@ u16 inverse_translate(struct vc_data *conp, int glyph, int use_unicode)
 	int m;
 	if (glyph < 0 || glyph >= MAX_GLYPH)
 		return 0;
-	else if (!(p = (struct uni_pagedir *)*conp->vc_uni_pagedir_loc))
+	else if (!(p = *conp->vc_uni_pagedir_loc))
 		return glyph;
 	else if (use_unicode) {
 		if (!p->inverse_trans_unicode)
@@ -287,7 +286,7 @@ static void update_user_maps(void)
 	for (i = 0; i < MAX_NR_CONSOLES; i++) {
 		if (!vc_cons_allocated(i))
 			continue;
-		p = (struct uni_pagedir *)*vc_cons[i].d->vc_uni_pagedir_loc;
+		p = *vc_cons[i].d->vc_uni_pagedir_loc;
 		if (p && p != q) {
 			set_inverse_transl(vc_cons[i].d, p, USER_MAP);
 			set_inverse_trans_unicode(vc_cons[i].d, p);
@@ -418,10 +417,10 @@ void con_free_unimap(struct vc_data *vc)
 {
 	struct uni_pagedir *p;
 
-	p = (struct uni_pagedir *)*vc->vc_uni_pagedir_loc;
+	p = *vc->vc_uni_pagedir_loc;
 	if (!p)
 		return;
-	*vc->vc_uni_pagedir_loc = 0;
+	*vc->vc_uni_pagedir_loc = NULL;
 	if (--p->refcount)
 		return;
 	con_release_unimap(p);
@@ -436,7 +435,7 @@ static int con_unify_unimap(struct vc_data *conp, struct uni_pagedir *p)
 	for (i = 0; i < MAX_NR_CONSOLES; i++) {
 		if (!vc_cons_allocated(i))
 			continue;
-		q = (struct uni_pagedir *)*vc_cons[i].d->vc_uni_pagedir_loc;
+		q = *vc_cons[i].d->vc_uni_pagedir_loc;
 		if (!q || q == p || q->sum != p->sum)
 			continue;
 		for (j = 0; j < 32; j++) {
@@ -459,7 +458,7 @@ static int con_unify_unimap(struct vc_data *conp, struct uni_pagedir *p)
 		}
 		if (j == 32) {
 			q->refcount++;
-			*conp->vc_uni_pagedir_loc = (unsigned long)q;
+			*conp->vc_uni_pagedir_loc = q;
 			con_release_unimap(p);
 			kfree(p);
 			return 1;
@@ -500,10 +499,7 @@ static int con_do_clear_unimap(struct vc_data *vc, struct unimapinit *ui)
 {
 	struct uni_pagedir *p, *q;
 
-	p = (struct uni_pagedir *)*vc->vc_uni_pagedir_loc;
-	if (p && p->readonly)
-		return -EIO;
-
+	p = *vc->vc_uni_pagedir_loc;
 	if (!p || --p->refcount) {
 		q = kzalloc(sizeof(*p), GFP_KERNEL);
 		if (!q) {
@@ -512,7 +508,7 @@ static int con_do_clear_unimap(struct vc_data *vc, struct unimapinit *ui)
 			return -ENOMEM;
 		}
 		q->refcount=1;
-		*vc->vc_uni_pagedir_loc = (unsigned long)q;
+		*vc->vc_uni_pagedir_loc = q;
 	} else {
 		if (p == dflt) dflt = NULL;
 		p->refcount++;
@@ -536,19 +532,13 @@ int con_set_unimap(struct vc_data *vc, ushort ct, struct unipair __user *list)
 	int err = 0, err1, i;
 	struct uni_pagedir *p, *q;
 
+	if (!ct)
+		return 0;
+
 	console_lock();
 
 	/* Save original vc_unipagdir_loc in case we allocate a new one */
-	p = (struct uni_pagedir *)*vc->vc_uni_pagedir_loc;
-	if (p->readonly) {
-		console_unlock();
-		return -EIO;
-	}
-	
-	if (!ct) {
-		console_unlock();
-		return 0;
-	}
+	p = *vc->vc_uni_pagedir_loc;
 	
 	if (p->refcount > 1) {
 		int j, k;
@@ -564,7 +554,7 @@ int con_set_unimap(struct vc_data *vc, ushort ct, struct unipair __user *list)
 		 * Since refcount was > 1, con_clear_unimap() allocated a
 		 * a new uni_pagedir for this vc.  Re: p != q
 		 */
-		q = (struct uni_pagedir *)*vc->vc_uni_pagedir_loc;
+		q = *vc->vc_uni_pagedir_loc;
 
 		/*
 		 * uni_pgdir is a 32*32*64 table with rows allocated
@@ -586,7 +576,7 @@ int con_set_unimap(struct vc_data *vc, ushort ct, struct unipair __user *list)
 					err1 = con_insert_unipair(q, l, p2[k]);
 					if (err1) {
 						p->refcount++;
-						*vc->vc_uni_pagedir_loc = (unsigned long)p;
+						*vc->vc_uni_pagedir_loc = p;
 						con_release_unimap(q);
 						kfree(q);
 						console_unlock();
@@ -655,12 +645,12 @@ int con_set_default_unimap(struct vc_data *vc)
 	struct uni_pagedir *p;
 
 	if (dflt) {
-		p = (struct uni_pagedir *)*vc->vc_uni_pagedir_loc;
+		p = *vc->vc_uni_pagedir_loc;
 		if (p == dflt)
 			return 0;
 
 		dflt->refcount++;
-		*vc->vc_uni_pagedir_loc = (unsigned long)dflt;
+		*vc->vc_uni_pagedir_loc = dflt;
 		if (p && !--p->refcount) {
 			con_release_unimap(p);
 			kfree(p);
@@ -674,7 +664,7 @@ int con_set_default_unimap(struct vc_data *vc)
 	if (err)
 		return err;
     
-	p = (struct uni_pagedir *)*vc->vc_uni_pagedir_loc;
+	p = *vc->vc_uni_pagedir_loc;
 	q = dfont_unitable;
 	
 	for (i = 0; i < 256; i++)
@@ -685,7 +675,7 @@ int con_set_default_unimap(struct vc_data *vc)
 		}
 			
 	if (con_unify_unimap(vc, p)) {
-		dflt = (struct uni_pagedir *)*vc->vc_uni_pagedir_loc;
+		dflt = *vc->vc_uni_pagedir_loc;
 		return err;
 	}
 
@@ -713,9 +703,9 @@ int con_copy_unimap(struct vc_data *dst_vc, struct vc_data *src_vc)
 	if (*dst_vc->vc_uni_pagedir_loc == *src_vc->vc_uni_pagedir_loc)
 		return 0;
 	con_free_unimap(dst_vc);
-	q = (struct uni_pagedir *)*src_vc->vc_uni_pagedir_loc;
+	q = *src_vc->vc_uni_pagedir_loc;
 	q->refcount++;
-	*dst_vc->vc_uni_pagedir_loc = (long)q;
+	*dst_vc->vc_uni_pagedir_loc = q;
 	return 0;
 }
 EXPORT_SYMBOL(con_copy_unimap);
@@ -737,7 +727,7 @@ int con_get_unimap(struct vc_data *vc, ushort ct, ushort __user *uct, struct uni
 
 	ect = 0;
 	if (*vc->vc_uni_pagedir_loc) {
-		p = (struct uni_pagedir *)*vc->vc_uni_pagedir_loc;
+		p = *vc->vc_uni_pagedir_loc;
 		for (i = 0; i < 32; i++)
 		if ((p1 = p->uni_pgdir[i]))
 			for (j = 0; j < 32; j++)
@@ -810,7 +800,7 @@ conv_uni_to_pc(struct vc_data *conp, long ucs)
 	if (!*conp->vc_uni_pagedir_loc)
 		return -3;
 
-	p = (struct uni_pagedir *)*conp->vc_uni_pagedir_loc;  
+	p = *conp->vc_uni_pagedir_loc;
 	if ((p1 = p->uni_pgdir[ucs >> 11]) &&
 	    (p2 = p1[(ucs >> 6) & 0x1f]) &&
 	    (h = p2[ucs & 0x3f]) < MAX_GLYPH)
diff --git a/drivers/tty/vt/vt.c b/drivers/tty/vt/vt.c
index 3ad0b61e35b..5e0f6ff2e2f 100644
--- a/drivers/tty/vt/vt.c
+++ b/drivers/tty/vt/vt.c
@@ -735,7 +735,7 @@ static void visual_init(struct vc_data *vc, int num, int init)
 	vc->vc_num = num;
 	vc->vc_display_fg = &master_display_fg;
 	vc->vc_uni_pagedir_loc = &vc->vc_uni_pagedir;
-	vc->vc_uni_pagedir = 0;
+	vc->vc_uni_pagedir = NULL;
 	vc->vc_hi_font_mask = 0;
 	vc->vc_complement_mask = 0;
 	vc->vc_can_do_color = 0;
@@ -1231,6 +1231,52 @@ static void default_attr(struct vc_data *vc)
 	vc->vc_color = vc->vc_def_color;
 }
 
+struct rgb { u8 r; u8 g; u8 b; };
+
+struct rgb rgb_from_256(int i)
+{
+	struct rgb c;
+	if (i < 8) {            /* Standard colours. */
+		c.r = i&1 ? 0xaa : 0x00;
+		c.g = i&2 ? 0xaa : 0x00;
+		c.b = i&4 ? 0xaa : 0x00;
+	} else if (i < 16) {
+		c.r = i&1 ? 0xff : 0x55;
+		c.g = i&2 ? 0xff : 0x55;
+		c.b = i&4 ? 0xff : 0x55;
+	} else if (i < 232) {   /* 6x6x6 colour cube. */
+		c.r = (i - 16) / 36 * 85 / 2;
+		c.g = (i - 16) / 6 % 6 * 85 / 2;
+		c.b = (i - 16) % 6 * 85 / 2;
+	} else                  /* Grayscale ramp. */
+		c.r = c.g = c.b = i * 10 - 2312;
+	return c;
+}
+
+static void rgb_foreground(struct vc_data *vc, struct rgb c)
+{
+	u8 hue, max = c.r;
+	if (c.g > max)
+		max = c.g;
+	if (c.b > max)
+		max = c.b;
+	hue = (c.r > max/2 ? 4 : 0)
+	    | (c.g > max/2 ? 2 : 0)
+	    | (c.b > max/2 ? 1 : 0);
+	if (hue == 7 && max <= 0x55)
+		hue = 0, vc->vc_intensity = 2;
+	else
+		vc->vc_intensity = (max > 0xaa) + 1;
+	vc->vc_color = (vc->vc_color & 0xf0) | hue;
+}
+
+static void rgb_background(struct vc_data *vc, struct rgb c)
+{
+	/* For backgrounds, err on the dark side. */
+	vc->vc_color = (vc->vc_color & 0x0f)
+		| (c.r&0x80) >> 1 | (c.g&0x80) >> 2 | (c.b&0x80) >> 3;
+}
+
 /* console_lock is held */
 static void csi_m(struct vc_data *vc)
 {
@@ -1302,8 +1348,7 @@ static void csi_m(struct vc_data *vc)
 			case 27:
 				vc->vc_reverse = 0;
 				break;
-			case 38:
-			case 48: /* ITU T.416
+			case 38: /* ITU T.416
 				  * Higher colour modes.
 				  * They break the usual properties of SGR codes
 				  * and thus need to be detected and ignored by
@@ -1315,15 +1360,41 @@ static void csi_m(struct vc_data *vc)
 				i++;
 				if (i > vc->vc_npar)
 					break;
-				if (vc->vc_par[i] == 5)      /* 256 colours */
-					i++;                 /* ubiquitous */
-				else if (vc->vc_par[i] == 2) /* 24 bit colours */
-					i += 3;              /* extremely rare */
+				if (vc->vc_par[i] == 5 &&  /* 256 colours */
+				    i < vc->vc_npar) {     /* ubiquitous */
+					i++;
+					rgb_foreground(vc,
+						rgb_from_256(vc->vc_par[i]));
+				} else if (vc->vc_par[i] == 2 &&  /* 24 bit */
+				           i <= vc->vc_npar + 3) {/* extremely rare */
+					struct rgb c = {r:vc->vc_par[i+1],
+							g:vc->vc_par[i+2],
+							b:vc->vc_par[i+3]};
+					rgb_foreground(vc, c);
+					i += 3;
+				}
 				/* Subcommands 3 (CMY) and 4 (CMYK) are so insane
-				 * that detecting them is not worth the few extra
-				 * bytes of kernel's size.
+				 * there's no point in supporting them.
 				 */
 				break;
+			case 48:
+				i++;
+				if (i > vc->vc_npar)
+					break;
+				if (vc->vc_par[i] == 5 &&  /* 256 colours */
+				    i < vc->vc_npar) {
+					i++;
+					rgb_background(vc,
+						rgb_from_256(vc->vc_par[i]));
+				} else if (vc->vc_par[i] == 2 && /* 24 bit */
+				           i <= vc->vc_npar + 3) {
+					struct rgb c = {r:vc->vc_par[i+1],
+							g:vc->vc_par[i+2],
+							b:vc->vc_par[i+3]};
+					rgb_background(vc, c);
+					i += 3;
+				}
+				break;
 			case 39:
 				vc->vc_color = (vc->vc_def_color & 0x0f) | (vc->vc_color & 0xf0);
 				break;
diff --git a/drivers/uio/uio.c b/drivers/uio/uio.c
index a673e5b6a2e..e371f5af11f 100644
--- a/drivers/uio/uio.c
+++ b/drivers/uio/uio.c
@@ -655,7 +655,7 @@ static int uio_mmap_physical(struct vm_area_struct *vma)
 
 	if (mem->addr & ~PAGE_MASK)
 		return -ENODEV;
-	if (vma->vm_end - vma->vm_start > mem->size)
+	if (vma->vm_end - vma->vm_start > PAGE_ALIGN(mem->size))
 		return -EINVAL;
 
 	vma->vm_ops = &uio_physical_vm_ops;
diff --git a/drivers/uio/uio_dmem_genirq.c b/drivers/uio/uio_dmem_genirq.c
index 1270f3b2613..8d0bba46956 100644
--- a/drivers/uio/uio_dmem_genirq.c
+++ b/drivers/uio/uio_dmem_genirq.c
@@ -204,7 +204,7 @@ static int uio_dmem_genirq_probe(struct platform_device *pdev)
 		ret = platform_get_irq(pdev, 0);
 		if (ret < 0) {
 			dev_err(&pdev->dev, "failed to get IRQ\n");
-			goto bad0;
+			goto bad1;
 		}
 		uioinfo->irq = ret;
 	}
@@ -275,6 +275,7 @@ static int uio_dmem_genirq_probe(struct platform_device *pdev)
 	ret = uio_register_device(&pdev->dev, priv->uioinfo);
 	if (ret) {
 		dev_err(&pdev->dev, "unable to register uio device\n");
+		pm_runtime_disable(&pdev->dev);
 		goto bad1;
 	}
 
@@ -282,7 +283,6 @@ static int uio_dmem_genirq_probe(struct platform_device *pdev)
 	return 0;
  bad1:
 	kfree(priv);
-	pm_runtime_disable(&pdev->dev);
  bad0:
 	/* kfree uioinfo for OF */
 	if (pdev->dev.of_node)
diff --git a/drivers/usb/Makefile b/drivers/usb/Makefile
index 1ae2bf39d84..3cba892b83a 100644
--- a/drivers/usb/Makefile
+++ b/drivers/usb/Makefile
@@ -28,6 +28,7 @@ obj-$(CONFIG_USB_IMX21_HCD)	+= host/
 obj-$(CONFIG_USB_FSL_MPH_DR_OF)	+= host/
 obj-$(CONFIG_USB_FUSBH200_HCD)	+= host/
 obj-$(CONFIG_USB_FOTG210_HCD)	+= host/
+obj-$(CONFIG_USB_MAX3421_HCD)	+= host/
 
 obj-$(CONFIG_USB_C67X00_HCD)	+= c67x00/
 
@@ -58,4 +59,4 @@ obj-$(CONFIG_USB_CHIPIDEA)	+= chipidea/
 obj-$(CONFIG_USB_RENESAS_USBHS)	+= renesas_usbhs/
 obj-$(CONFIG_USB_GADGET)	+= gadget/
 
-obj-$(CONFIG_USB_COMMON)	+= usb-common.o
+obj-$(CONFIG_USB_COMMON)	+= common/
diff --git a/drivers/usb/chipidea/Makefile b/drivers/usb/chipidea/Makefile
index 480bd4d5710..2f099c7df7b 100644
--- a/drivers/usb/chipidea/Makefile
+++ b/drivers/usb/chipidea/Makefile
@@ -6,6 +6,7 @@ ci_hdrc-y				:= core.o otg.o
 ci_hdrc-$(CONFIG_USB_CHIPIDEA_UDC)	+= udc.o
 ci_hdrc-$(CONFIG_USB_CHIPIDEA_HOST)	+= host.o
 ci_hdrc-$(CONFIG_USB_CHIPIDEA_DEBUG)	+= debug.o
+ci_hdrc-$(CONFIG_USB_OTG_FSM)		+= otg_fsm.o
 
 # Glue/Bridge layers go here
 
diff --git a/drivers/usb/chipidea/bits.h b/drivers/usb/chipidea/bits.h
index 83d06c1455b..ca57e3dcd3d 100644
--- a/drivers/usb/chipidea/bits.h
+++ b/drivers/usb/chipidea/bits.h
@@ -44,9 +44,14 @@
 #define DEVICEADDR_USBADR     (0x7FUL << 25)
 
 /* PORTSC */
+#define PORTSC_CCS            BIT(0)
+#define PORTSC_CSC            BIT(1)
+#define PORTSC_PEC            BIT(3)
+#define PORTSC_OCC            BIT(5)
 #define PORTSC_FPR            BIT(6)
 #define PORTSC_SUSP           BIT(7)
 #define PORTSC_HSP            BIT(9)
+#define PORTSC_PP             BIT(12)
 #define PORTSC_PTC            (0x0FUL << 16)
 #define PORTSC_PHCD(d)	      ((d) ? BIT(22) : BIT(23))
 /* PTS and PTW for non lpm version only */
@@ -56,6 +61,9 @@
 #define PORTSC_PTW            BIT(28)
 #define PORTSC_STS            BIT(29)
 
+#define PORTSC_W1C_BITS						\
+	(PORTSC_CSC | PORTSC_PEC | PORTSC_OCC)
+
 /* DEVLC */
 #define DEVLC_PFSC            BIT(23)
 #define DEVLC_PSPD            (0x03UL << 25)
@@ -72,6 +80,8 @@
 
 /* OTGSC */
 #define OTGSC_IDPU	      BIT(5)
+#define OTGSC_HADP	      BIT(6)
+#define OTGSC_HABA	      BIT(7)
 #define OTGSC_ID	      BIT(8)
 #define OTGSC_AVV	      BIT(9)
 #define OTGSC_ASV	      BIT(10)
diff --git a/drivers/usb/chipidea/ci.h b/drivers/usb/chipidea/ci.h
index e206406ae1d..9563cb56d56 100644
--- a/drivers/usb/chipidea/ci.h
+++ b/drivers/usb/chipidea/ci.h
@@ -17,6 +17,7 @@
 #include <linux/irqreturn.h>
 #include <linux/usb.h>
 #include <linux/usb/gadget.h>
+#include <linux/usb/otg-fsm.h>
 
 /******************************************************************************
  * DEFINE
@@ -139,6 +140,8 @@ struct hw_bank {
  * @roles: array of supported roles for this controller
  * @role: current role
  * @is_otg: if the device is otg-capable
+ * @fsm: otg finite state machine
+ * @fsm_timer: pointer to timer list of otg fsm
  * @work: work for role changing
  * @wq: workqueue thread
  * @qh_pool: allocation pool for queue heads
@@ -174,6 +177,8 @@ struct ci_hdrc {
 	struct ci_role_driver		*roles[CI_ROLE_END];
 	enum ci_role			role;
 	bool				is_otg;
+	struct otg_fsm			fsm;
+	struct ci_otg_fsm_timer_list	*fsm_timer;
 	struct work_struct		work;
 	struct workqueue_struct		*wq;
 
@@ -319,6 +324,24 @@ static inline u32 hw_test_and_write(struct ci_hdrc *ci, enum ci_hw_regs reg,
 	return (val & mask) >> __ffs(mask);
 }
 
+/**
+ * ci_otg_is_fsm_mode: runtime check if otg controller
+ * is in otg fsm mode.
+ */
+static inline bool ci_otg_is_fsm_mode(struct ci_hdrc *ci)
+{
+#ifdef CONFIG_USB_OTG_FSM
+	return ci->is_otg && ci->roles[CI_ROLE_HOST] &&
+					ci->roles[CI_ROLE_GADGET];
+#else
+	return false;
+#endif
+}
+
+u32 hw_read_intr_enable(struct ci_hdrc *ci);
+
+u32 hw_read_intr_status(struct ci_hdrc *ci);
+
 int hw_device_reset(struct ci_hdrc *ci, u32 mode);
 
 int hw_port_test_set(struct ci_hdrc *ci, u8 mode);
diff --git a/drivers/usb/chipidea/ci_hdrc_msm.c b/drivers/usb/chipidea/ci_hdrc_msm.c
index 2d51d852b47..d72b9d2de2c 100644
--- a/drivers/usb/chipidea/ci_hdrc_msm.c
+++ b/drivers/usb/chipidea/ci_hdrc_msm.c
@@ -47,6 +47,7 @@ static void ci_hdrc_msm_notify_event(struct ci_hdrc *ci, unsigned event)
 
 static struct ci_hdrc_platform_data ci_hdrc_msm_platdata = {
 	.name			= "ci_hdrc_msm",
+	.capoffset		= DEF_CAPOFFSET,
 	.flags			= CI_HDRC_REGS_SHARED |
 				  CI_HDRC_REQUIRE_TRANSCEIVER |
 				  CI_HDRC_DISABLE_STREAMING,
@@ -57,9 +58,21 @@ static struct ci_hdrc_platform_data ci_hdrc_msm_platdata = {
 static int ci_hdrc_msm_probe(struct platform_device *pdev)
 {
 	struct platform_device *plat_ci;
+	struct usb_phy *phy;
 
 	dev_dbg(&pdev->dev, "ci_hdrc_msm_probe\n");
 
+	/*
+	 * OTG(PHY) driver takes care of PHY initialization, clock management,
+	 * powering up VBUS, mapping of registers address space and power
+	 * management.
+	 */
+	phy = devm_usb_get_phy_by_phandle(&pdev->dev, "usb-phy", 0);
+	if (IS_ERR(phy))
+		return PTR_ERR(phy);
+
+	ci_hdrc_msm_platdata.phy = phy;
+
 	plat_ci = ci_hdrc_add_device(&pdev->dev,
 				pdev->resource, pdev->num_resources,
 				&ci_hdrc_msm_platdata);
@@ -86,10 +99,19 @@ static int ci_hdrc_msm_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static const struct of_device_id msm_ci_dt_match[] = {
+	{ .compatible = "qcom,ci-hdrc", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, msm_ci_dt_match);
+
 static struct platform_driver ci_hdrc_msm_driver = {
 	.probe = ci_hdrc_msm_probe,
 	.remove = ci_hdrc_msm_remove,
-	.driver = { .name = "msm_hsusb", },
+	.driver = {
+		.name = "msm_hsusb",
+		.of_match_table = msm_ci_dt_match,
+	},
 };
 
 module_platform_driver(ci_hdrc_msm_driver);
diff --git a/drivers/usb/chipidea/core.c b/drivers/usb/chipidea/core.c
index ca6831c5b76..619d13e2999 100644
--- a/drivers/usb/chipidea/core.c
+++ b/drivers/usb/chipidea/core.c
@@ -23,7 +23,7 @@
  * - BUS:    bus glue code, bus abstraction layer
  *
  * Compile Options
- * - CONFIG_USB_GADGET_DEBUG_FILES: enable debug facilities
+ * - CONFIG_USB_CHIPIDEA_DEBUG: enable debug facilities
  * - STALL_IN:  non-empty bulk-in pipes cannot be halted
  *              if defined mass storage compliance succeeds but with warnings
  *              => case 4: Hi >  Dn
@@ -42,10 +42,6 @@
  * - Not Supported: 15 & 16 (ISO)
  *
  * TODO List
- * - OTG
- * - Interrupt Traffic
- * - GET_STATUS(device) - always reports 0
- * - Gadget API (majority of optional features)
  * - Suspend & Remote Wakeup
  */
 #include <linux/delay.h>
@@ -74,6 +70,7 @@
 #include "host.h"
 #include "debug.h"
 #include "otg.h"
+#include "otg_fsm.h"
 
 /* Controller register map */
 static const u8 ci_regs_nolpm[] = {
@@ -140,6 +137,26 @@ static int hw_alloc_regmap(struct ci_hdrc *ci, bool is_lpm)
 }
 
 /**
+ * hw_read_intr_enable: returns interrupt enable register
+ *
+ * This function returns register data
+ */
+u32 hw_read_intr_enable(struct ci_hdrc *ci)
+{
+	return hw_read(ci, OP_USBINTR, ~0);
+}
+
+/**
+ * hw_read_intr_status: returns interrupt status register
+ *
+ * This function returns register data
+ */
+u32 hw_read_intr_status(struct ci_hdrc *ci)
+{
+	return hw_read(ci, OP_USBSTS, ~0);
+}
+
+/**
  * hw_port_test_set: writes port test mode (execute without interruption)
  * @mode: new value
  *
@@ -179,11 +196,10 @@ static void ci_hdrc_enter_lpm(struct ci_hdrc *ci, bool enable)
 		hw_write(ci, reg, PORTSC_PHCD(ci->hw_bank.lpm),
 				0);
 		/* 
-		 * The controller needs at least 1ms to reflect
-		 * PHY's status, the PHY also needs some time (less
+		 * the PHY needs some time (less
 		 * than 1ms) to leave low power mode.
 		 */
-		usleep_range(1500, 2000);
+		usleep_range(1000, 1100);
 	}
 }
 
@@ -277,6 +293,39 @@ static void hw_phymode_configure(struct ci_hdrc *ci)
 }
 
 /**
+ * ci_usb_phy_init: initialize phy according to different phy type
+ * @ci: the controller
+  *
+ * This function returns an error code if usb_phy_init has failed
+ */
+static int ci_usb_phy_init(struct ci_hdrc *ci)
+{
+	int ret;
+
+	switch (ci->platdata->phy_mode) {
+	case USBPHY_INTERFACE_MODE_UTMI:
+	case USBPHY_INTERFACE_MODE_UTMIW:
+	case USBPHY_INTERFACE_MODE_HSIC:
+		ret = usb_phy_init(ci->transceiver);
+		if (ret)
+			return ret;
+		hw_phymode_configure(ci);
+		break;
+	case USBPHY_INTERFACE_MODE_ULPI:
+	case USBPHY_INTERFACE_MODE_SERIAL:
+		hw_phymode_configure(ci);
+		ret = usb_phy_init(ci->transceiver);
+		if (ret)
+			return ret;
+		break;
+	default:
+		ret = usb_phy_init(ci->transceiver);
+	}
+
+	return ret;
+}
+
+/**
  * hw_device_reset: resets chip (execute without interruption)
  * @ci: the controller
   *
@@ -359,8 +408,14 @@ static irqreturn_t ci_irq(int irq, void *data)
 	irqreturn_t ret = IRQ_NONE;
 	u32 otgsc = 0;
 
-	if (ci->is_otg)
-		otgsc = hw_read(ci, OP_OTGSC, ~0);
+	if (ci->is_otg) {
+		otgsc = hw_read_otgsc(ci, ~0);
+		if (ci_otg_is_fsm_mode(ci)) {
+			ret = ci_otg_fsm_irq(ci);
+			if (ret == IRQ_HANDLED)
+				return ret;
+		}
+	}
 
 	/*
 	 * Handle id change interrupt, it indicates device/host function
@@ -368,9 +423,9 @@ static irqreturn_t ci_irq(int irq, void *data)
 	 */
 	if (ci->is_otg && (otgsc & OTGSC_IDIE) && (otgsc & OTGSC_IDIS)) {
 		ci->id_event = true;
-		ci_clear_otg_interrupt(ci, OTGSC_IDIS);
-		disable_irq_nosync(ci->irq);
-		queue_work(ci->wq, &ci->work);
+		/* Clear ID change irq status */
+		hw_write_otgsc(ci, OTGSC_IDIS, OTGSC_IDIS);
+		ci_otg_queue_work(ci);
 		return IRQ_HANDLED;
 	}
 
@@ -380,9 +435,9 @@ static irqreturn_t ci_irq(int irq, void *data)
 	 */
 	if (ci->is_otg && (otgsc & OTGSC_BSVIE) && (otgsc & OTGSC_BSVIS)) {
 		ci->b_sess_valid_event = true;
-		ci_clear_otg_interrupt(ci, OTGSC_BSVIS);
-		disable_irq_nosync(ci->irq);
-		queue_work(ci->wq, &ci->work);
+		/* Clear BSV irq */
+		hw_write_otgsc(ci, OTGSC_BSVIS, OTGSC_BSVIS);
+		ci_otg_queue_work(ci);
 		return IRQ_HANDLED;
 	}
 
@@ -500,11 +555,8 @@ static void ci_get_otg_capable(struct ci_hdrc *ci)
 		ci->is_otg = (hw_read(ci, CAP_DCCPARAMS,
 				DCCPARAMS_DC | DCCPARAMS_HC)
 					== (DCCPARAMS_DC | DCCPARAMS_HC));
-	if (ci->is_otg) {
+	if (ci->is_otg)
 		dev_dbg(ci->dev, "It is OTG capable controller\n");
-		ci_disable_otg_interrupt(ci, OTGSC_INT_EN_BITS);
-		ci_clear_otg_interrupt(ci, OTGSC_INT_STATUS_BITS);
-	}
 }
 
 static int ci_hdrc_probe(struct platform_device *pdev)
@@ -543,8 +595,6 @@ static int ci_hdrc_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	hw_phymode_configure(ci);
-
 	if (ci->platdata->phy)
 		ci->transceiver = ci->platdata->phy;
 	else
@@ -564,10 +614,17 @@ static int ci_hdrc_probe(struct platform_device *pdev)
 		return -EPROBE_DEFER;
 	}
 
-	ret = usb_phy_init(ci->transceiver);
+	ret = ci_usb_phy_init(ci);
 	if (ret) {
 		dev_err(dev, "unable to init phy: %d\n", ret);
 		return ret;
+	} else {
+		/* 
+		 * The delay to sync PHY's status, the maximum delay is
+		 * 2ms since the otgsc uses 1ms timer to debounce the
+		 * PHY's input
+		 */
+		usleep_range(2000, 2500);
 	}
 
 	ci->hw_bank.phys = res->start;
@@ -602,6 +659,9 @@ static int ci_hdrc_probe(struct platform_device *pdev)
 	}
 
 	if (ci->is_otg) {
+		/* Disable and clear all OTG irq */
+		hw_write_otgsc(ci, OTGSC_INT_EN_BITS | OTGSC_INT_STATUS_BITS,
+							OTGSC_INT_STATUS_BITS);
 		ret = ci_hdrc_otg_init(ci);
 		if (ret) {
 			dev_err(dev, "init otg fails, ret = %d\n", ret);
@@ -611,13 +671,9 @@ static int ci_hdrc_probe(struct platform_device *pdev)
 
 	if (ci->roles[CI_ROLE_HOST] && ci->roles[CI_ROLE_GADGET]) {
 		if (ci->is_otg) {
-			/*
-			 * ID pin needs 1ms debouce time,
-			 * we delay 2ms for safe.
-			 */
-			mdelay(2);
 			ci->role = ci_otg_role(ci);
-			ci_enable_otg_interrupt(ci, OTGSC_IDIE);
+			/* Enable ID change irq */
+			hw_write_otgsc(ci, OTGSC_IDIE, OTGSC_IDIE);
 		} else {
 			/*
 			 * If the controller is not OTG capable, but support
@@ -636,10 +692,13 @@ static int ci_hdrc_probe(struct platform_device *pdev)
 	if (ci->role == CI_ROLE_GADGET)
 		ci_handle_vbus_change(ci);
 
-	ret = ci_role_start(ci, ci->role);
-	if (ret) {
-		dev_err(dev, "can't start %s role\n", ci_role(ci)->name);
-		goto stop;
+	if (!ci_otg_is_fsm_mode(ci)) {
+		ret = ci_role_start(ci, ci->role);
+		if (ret) {
+			dev_err(dev, "can't start %s role\n",
+						ci_role(ci)->name);
+			goto stop;
+		}
 	}
 
 	platform_set_drvdata(pdev, ci);
@@ -648,6 +707,9 @@ static int ci_hdrc_probe(struct platform_device *pdev)
 	if (ret)
 		goto stop;
 
+	if (ci_otg_is_fsm_mode(ci))
+		ci_hdrc_otg_fsm_start(ci);
+
 	ret = dbg_create_files(ci);
 	if (!ret)
 		return 0;
@@ -680,6 +742,7 @@ static struct platform_driver ci_hdrc_driver = {
 	.remove	= ci_hdrc_remove,
 	.driver	= {
 		.name	= "ci_hdrc",
+		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/usb/chipidea/debug.c b/drivers/usb/chipidea/debug.c
index 96d899aee47..7cccab6ff30 100644
--- a/drivers/usb/chipidea/debug.c
+++ b/drivers/usb/chipidea/debug.c
@@ -7,11 +7,15 @@
 #include <linux/uaccess.h>
 #include <linux/usb/ch9.h>
 #include <linux/usb/gadget.h>
+#include <linux/usb/phy.h>
+#include <linux/usb/otg.h>
+#include <linux/usb/otg-fsm.h>
 
 #include "ci.h"
 #include "udc.h"
 #include "bits.h"
 #include "debug.h"
+#include "otg.h"
 
 /**
  * ci_device_show: prints information about device capabilities and status
@@ -204,6 +208,80 @@ static const struct file_operations ci_requests_fops = {
 	.release	= single_release,
 };
 
+int ci_otg_show(struct seq_file *s, void *unused)
+{
+	struct ci_hdrc *ci = s->private;
+	struct otg_fsm *fsm;
+
+	if (!ci || !ci_otg_is_fsm_mode(ci))
+		return 0;
+
+	fsm = &ci->fsm;
+
+	/* ------ State ----- */
+	seq_printf(s, "OTG state: %s\n\n",
+		usb_otg_state_string(ci->transceiver->state));
+
+	/* ------ State Machine Variables ----- */
+	seq_printf(s, "a_bus_drop: %d\n", fsm->a_bus_drop);
+
+	seq_printf(s, "a_bus_req: %d\n", fsm->a_bus_req);
+
+	seq_printf(s, "a_srp_det: %d\n", fsm->a_srp_det);
+
+	seq_printf(s, "a_vbus_vld: %d\n", fsm->a_vbus_vld);
+
+	seq_printf(s, "b_conn: %d\n", fsm->b_conn);
+
+	seq_printf(s, "adp_change: %d\n", fsm->adp_change);
+
+	seq_printf(s, "power_up: %d\n", fsm->power_up);
+
+	seq_printf(s, "a_bus_resume: %d\n", fsm->a_bus_resume);
+
+	seq_printf(s, "a_bus_suspend: %d\n", fsm->a_bus_suspend);
+
+	seq_printf(s, "a_conn: %d\n", fsm->a_conn);
+
+	seq_printf(s, "b_bus_req: %d\n", fsm->b_bus_req);
+
+	seq_printf(s, "b_bus_suspend: %d\n", fsm->b_bus_suspend);
+
+	seq_printf(s, "b_se0_srp: %d\n", fsm->b_se0_srp);
+
+	seq_printf(s, "b_ssend_srp: %d\n", fsm->b_ssend_srp);
+
+	seq_printf(s, "b_sess_vld: %d\n", fsm->b_sess_vld);
+
+	seq_printf(s, "b_srp_done: %d\n", fsm->b_srp_done);
+
+	seq_printf(s, "drv_vbus: %d\n", fsm->drv_vbus);
+
+	seq_printf(s, "loc_conn: %d\n", fsm->loc_conn);
+
+	seq_printf(s, "loc_sof: %d\n", fsm->loc_sof);
+
+	seq_printf(s, "adp_prb: %d\n", fsm->adp_prb);
+
+	seq_printf(s, "id: %d\n", fsm->id);
+
+	seq_printf(s, "protocol: %d\n", fsm->protocol);
+
+	return 0;
+}
+
+static int ci_otg_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ci_otg_show, inode->i_private);
+}
+
+static const struct file_operations ci_otg_fops = {
+	.open			= ci_otg_open,
+	.read			= seq_read,
+	.llseek			= seq_lseek,
+	.release		= single_release,
+};
+
 static int ci_role_show(struct seq_file *s, void *data)
 {
 	struct ci_hdrc *ci = s->private;
@@ -253,6 +331,50 @@ static const struct file_operations ci_role_fops = {
 	.release	= single_release,
 };
 
+int ci_registers_show(struct seq_file *s, void *unused)
+{
+	struct ci_hdrc *ci = s->private;
+	u32 tmp_reg;
+
+	if (!ci)
+		return 0;
+
+	/* ------ Registers ----- */
+	tmp_reg = hw_read_intr_enable(ci);
+	seq_printf(s, "USBINTR reg: %08x\n", tmp_reg);
+
+	tmp_reg = hw_read_intr_status(ci);
+	seq_printf(s, "USBSTS reg: %08x\n", tmp_reg);
+
+	tmp_reg = hw_read(ci, OP_USBMODE, ~0);
+	seq_printf(s, "USBMODE reg: %08x\n", tmp_reg);
+
+	tmp_reg = hw_read(ci, OP_USBCMD, ~0);
+	seq_printf(s, "USBCMD reg: %08x\n", tmp_reg);
+
+	tmp_reg = hw_read(ci, OP_PORTSC, ~0);
+	seq_printf(s, "PORTSC reg: %08x\n", tmp_reg);
+
+	if (ci->is_otg) {
+		tmp_reg = hw_read_otgsc(ci, ~0);
+		seq_printf(s, "OTGSC reg: %08x\n", tmp_reg);
+	}
+
+	return 0;
+}
+
+static int ci_registers_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ci_registers_show, inode->i_private);
+}
+
+static const struct file_operations ci_registers_fops = {
+	.open			= ci_registers_open,
+	.read			= seq_read,
+	.llseek			= seq_lseek,
+	.release		= single_release,
+};
+
 /**
  * dbg_create_files: initializes the attribute interface
  * @ci: device
@@ -287,8 +409,21 @@ int dbg_create_files(struct ci_hdrc *ci)
 	if (!dent)
 		goto err;
 
+	if (ci_otg_is_fsm_mode(ci)) {
+		dent = debugfs_create_file("otg", S_IRUGO, ci->debugfs, ci,
+					&ci_otg_fops);
+		if (!dent)
+			goto err;
+	}
+
 	dent = debugfs_create_file("role", S_IRUGO | S_IWUSR, ci->debugfs, ci,
 				   &ci_role_fops);
+	if (!dent)
+		goto err;
+
+	dent = debugfs_create_file("registers", S_IRUGO, ci->debugfs, ci,
+				&ci_registers_fops);
+
 	if (dent)
 		return 0;
 err:
diff --git a/drivers/usb/chipidea/host.c b/drivers/usb/chipidea/host.c
index a8ac6c16dac..a93d950e946 100644
--- a/drivers/usb/chipidea/host.c
+++ b/drivers/usb/chipidea/host.c
@@ -67,7 +67,11 @@ static int host_start(struct ci_hdrc *ci)
 	ehci->has_tdi_phy_lpm = ci->hw_bank.lpm;
 	ehci->imx28_write_fix = ci->imx28_write_fix;
 
-	if (ci->platdata->reg_vbus) {
+	/*
+	 * vbus is always on if host is not in OTG FSM mode,
+	 * otherwise should be controlled by OTG FSM
+	 */
+	if (ci->platdata->reg_vbus && !ci_otg_is_fsm_mode(ci)) {
 		ret = regulator_enable(ci->platdata->reg_vbus);
 		if (ret) {
 			dev_err(ci->dev,
@@ -78,10 +82,17 @@ static int host_start(struct ci_hdrc *ci)
 	}
 
 	ret = usb_add_hcd(hcd, 0, 0);
-	if (ret)
+	if (ret) {
 		goto disable_reg;
-	else
+	} else {
+		struct usb_otg *otg = ci->transceiver->otg;
+
 		ci->hcd = hcd;
+		if (otg) {
+			otg->host = &hcd->self;
+			hcd->self.otg_port = 1;
+		}
+	}
 
 	if (ci->platdata->flags & CI_HDRC_DISABLE_STREAMING)
 		hw_write(ci, OP_USBMODE, USBMODE_CI_SDIS, USBMODE_CI_SDIS);
@@ -89,7 +100,7 @@ static int host_start(struct ci_hdrc *ci)
 	return ret;
 
 disable_reg:
-	if (ci->platdata->reg_vbus)
+	if (ci->platdata->reg_vbus && !ci_otg_is_fsm_mode(ci))
 		regulator_disable(ci->platdata->reg_vbus);
 
 put_hcd:
@@ -105,7 +116,7 @@ static void host_stop(struct ci_hdrc *ci)
 	if (hcd) {
 		usb_remove_hcd(hcd);
 		usb_put_hcd(hcd);
-		if (ci->platdata->reg_vbus)
+		if (ci->platdata->reg_vbus && !ci_otg_is_fsm_mode(ci))
 			regulator_disable(ci->platdata->reg_vbus);
 	}
 }
diff --git a/drivers/usb/chipidea/otg.c b/drivers/usb/chipidea/otg.c
index 39bd7ec8bf7..a048b08b9d4 100644
--- a/drivers/usb/chipidea/otg.c
+++ b/drivers/usb/chipidea/otg.c
@@ -11,8 +11,8 @@
  */
 
 /*
- * This file mainly handles otgsc register, it may include OTG operation
- * in the future.
+ * This file mainly handles otgsc register, OTG fsm operations for HNP and SRP
+ * are also included.
  */
 
 #include <linux/usb/otg.h>
@@ -22,6 +22,26 @@
 #include "ci.h"
 #include "bits.h"
 #include "otg.h"
+#include "otg_fsm.h"
+
+/**
+ * hw_read_otgsc returns otgsc register bits value.
+ * @mask: bitfield mask
+ */
+u32 hw_read_otgsc(struct ci_hdrc *ci, u32 mask)
+{
+	return hw_read(ci, OP_OTGSC, mask);
+}
+
+/**
+ * hw_write_otgsc updates target bits of OTGSC register.
+ * @mask: bitfield mask
+ * @data: to be written
+ */
+void hw_write_otgsc(struct ci_hdrc *ci, u32 mask, u32 data)
+{
+	hw_write(ci, OP_OTGSC, mask | OTGSC_INT_STATUS_BITS, data);
+}
 
 /**
  * ci_otg_role - pick role based on ID pin state
@@ -29,8 +49,7 @@
  */
 enum ci_role ci_otg_role(struct ci_hdrc *ci)
 {
-	u32 sts = hw_read(ci, OP_OTGSC, ~0);
-	enum ci_role role = sts & OTGSC_ID
+	enum ci_role role = hw_read_otgsc(ci, OTGSC_ID)
 		? CI_ROLE_GADGET
 		: CI_ROLE_HOST;
 
@@ -39,14 +58,10 @@ enum ci_role ci_otg_role(struct ci_hdrc *ci)
 
 void ci_handle_vbus_change(struct ci_hdrc *ci)
 {
-	u32 otgsc;
-
 	if (!ci->is_otg)
 		return;
 
-	otgsc = hw_read(ci, OP_OTGSC, ~0);
-
-	if (otgsc & OTGSC_BSV)
+	if (hw_read_otgsc(ci, OTGSC_BSV))
 		usb_gadget_vbus_connect(&ci->gadget);
 	else
 		usb_gadget_vbus_disconnect(&ci->gadget);
@@ -76,6 +91,11 @@ static void ci_otg_work(struct work_struct *work)
 {
 	struct ci_hdrc *ci = container_of(work, struct ci_hdrc, work);
 
+	if (ci_otg_is_fsm_mode(ci) && !ci_otg_fsm_work(ci)) {
+		enable_irq(ci->irq);
+		return;
+	}
+
 	if (ci->id_event) {
 		ci->id_event = false;
 		ci_handle_id_switch(ci);
@@ -102,6 +122,9 @@ int ci_hdrc_otg_init(struct ci_hdrc *ci)
 		return -ENODEV;
 	}
 
+	if (ci_otg_is_fsm_mode(ci))
+		return ci_hdrc_otg_fsm_init(ci);
+
 	return 0;
 }
 
@@ -115,6 +138,9 @@ void ci_hdrc_otg_destroy(struct ci_hdrc *ci)
 		flush_workqueue(ci->wq);
 		destroy_workqueue(ci->wq);
 	}
-	ci_disable_otg_interrupt(ci, OTGSC_INT_EN_BITS);
-	ci_clear_otg_interrupt(ci, OTGSC_INT_STATUS_BITS);
+	/* Disable all OTG irq and clear status */
+	hw_write_otgsc(ci, OTGSC_INT_EN_BITS | OTGSC_INT_STATUS_BITS,
+						OTGSC_INT_STATUS_BITS);
+	if (ci_otg_is_fsm_mode(ci))
+		ci_hdrc_otg_fsm_remove(ci);
 }
diff --git a/drivers/usb/chipidea/otg.h b/drivers/usb/chipidea/otg.h
index 449bee07f4f..9ecb598e48f 100644
--- a/drivers/usb/chipidea/otg.h
+++ b/drivers/usb/chipidea/otg.h
@@ -11,25 +11,16 @@
 #ifndef __DRIVERS_USB_CHIPIDEA_OTG_H
 #define __DRIVERS_USB_CHIPIDEA_OTG_H
 
-static inline void ci_clear_otg_interrupt(struct ci_hdrc *ci, u32 bits)
-{
-	/* Only clear request bits */
-	hw_write(ci, OP_OTGSC, OTGSC_INT_STATUS_BITS, bits);
-}
-
-static inline void ci_enable_otg_interrupt(struct ci_hdrc *ci, u32 bits)
-{
-	hw_write(ci, OP_OTGSC, bits | OTGSC_INT_STATUS_BITS, bits);
-}
-
-static inline void ci_disable_otg_interrupt(struct ci_hdrc *ci, u32 bits)
-{
-	hw_write(ci, OP_OTGSC, bits | OTGSC_INT_STATUS_BITS, 0);
-}
-
+u32 hw_read_otgsc(struct ci_hdrc *ci, u32 mask);
+void hw_write_otgsc(struct ci_hdrc *ci, u32 mask, u32 data);
 int ci_hdrc_otg_init(struct ci_hdrc *ci);
 void ci_hdrc_otg_destroy(struct ci_hdrc *ci);
 enum ci_role ci_otg_role(struct ci_hdrc *ci);
 void ci_handle_vbus_change(struct ci_hdrc *ci);
+static inline void ci_otg_queue_work(struct ci_hdrc *ci)
+{
+	disable_irq_nosync(ci->irq);
+	queue_work(ci->wq, &ci->work);
+}
 
 #endif /* __DRIVERS_USB_CHIPIDEA_OTG_H */
diff --git a/drivers/usb/chipidea/otg_fsm.c b/drivers/usb/chipidea/otg_fsm.c
new file mode 100644
index 00000000000..caaabc58021
--- /dev/null
+++ b/drivers/usb/chipidea/otg_fsm.c
@@ -0,0 +1,842 @@
+/*
+ * otg_fsm.c - ChipIdea USB IP core OTG FSM driver
+ *
+ * Copyright (C) 2014 Freescale Semiconductor, Inc.
+ *
+ * Author: Jun Li
+ *
+ * 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 file mainly handles OTG fsm, it includes OTG fsm operations
+ * for HNP and SRP.
+ *
+ * TODO List
+ * - ADP
+ * - OTG test device
+ */
+
+#include <linux/usb/otg.h>
+#include <linux/usb/gadget.h>
+#include <linux/usb/hcd.h>
+#include <linux/usb/chipidea.h>
+#include <linux/regulator/consumer.h>
+
+#include "ci.h"
+#include "bits.h"
+#include "otg.h"
+#include "otg_fsm.h"
+
+static struct ci_otg_fsm_timer *otg_timer_initializer
+(struct ci_hdrc *ci, void (*function)(void *, unsigned long),
+			unsigned long expires, unsigned long data)
+{
+	struct ci_otg_fsm_timer *timer;
+
+	timer = devm_kzalloc(ci->dev, sizeof(struct ci_otg_fsm_timer),
+								GFP_KERNEL);
+	if (!timer)
+		return NULL;
+	timer->function = function;
+	timer->expires = expires;
+	timer->data = data;
+	return timer;
+}
+
+/* Add for otg: interact with user space app */
+static ssize_t
+get_a_bus_req(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	char		*next;
+	unsigned	size, t;
+	struct ci_hdrc	*ci = dev_get_drvdata(dev);
+
+	next = buf;
+	size = PAGE_SIZE;
+	t = scnprintf(next, size, "%d\n", ci->fsm.a_bus_req);
+	size -= t;
+	next += t;
+
+	return PAGE_SIZE - size;
+}
+
+static ssize_t
+set_a_bus_req(struct device *dev, struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct ci_hdrc *ci = dev_get_drvdata(dev);
+
+	if (count > 2)
+		return -1;
+
+	mutex_lock(&ci->fsm.lock);
+	if (buf[0] == '0') {
+		ci->fsm.a_bus_req = 0;
+	} else if (buf[0] == '1') {
+		/* If a_bus_drop is TRUE, a_bus_req can't be set */
+		if (ci->fsm.a_bus_drop) {
+			mutex_unlock(&ci->fsm.lock);
+			return count;
+		}
+		ci->fsm.a_bus_req = 1;
+	}
+
+	ci_otg_queue_work(ci);
+	mutex_unlock(&ci->fsm.lock);
+
+	return count;
+}
+static DEVICE_ATTR(a_bus_req, S_IRUGO | S_IWUSR, get_a_bus_req, set_a_bus_req);
+
+static ssize_t
+get_a_bus_drop(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	char		*next;
+	unsigned	size, t;
+	struct ci_hdrc	*ci = dev_get_drvdata(dev);
+
+	next = buf;
+	size = PAGE_SIZE;
+	t = scnprintf(next, size, "%d\n", ci->fsm.a_bus_drop);
+	size -= t;
+	next += t;
+
+	return PAGE_SIZE - size;
+}
+
+static ssize_t
+set_a_bus_drop(struct device *dev, struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct ci_hdrc	*ci = dev_get_drvdata(dev);
+
+	if (count > 2)
+		return -1;
+
+	mutex_lock(&ci->fsm.lock);
+	if (buf[0] == '0') {
+		ci->fsm.a_bus_drop = 0;
+	} else if (buf[0] == '1') {
+		ci->fsm.a_bus_drop = 1;
+		ci->fsm.a_bus_req = 0;
+	}
+
+	ci_otg_queue_work(ci);
+	mutex_unlock(&ci->fsm.lock);
+
+	return count;
+}
+static DEVICE_ATTR(a_bus_drop, S_IRUGO | S_IWUSR, get_a_bus_drop,
+						set_a_bus_drop);
+
+static ssize_t
+get_b_bus_req(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	char		*next;
+	unsigned	size, t;
+	struct ci_hdrc	*ci = dev_get_drvdata(dev);
+
+	next = buf;
+	size = PAGE_SIZE;
+	t = scnprintf(next, size, "%d\n", ci->fsm.b_bus_req);
+	size -= t;
+	next += t;
+
+	return PAGE_SIZE - size;
+}
+
+static ssize_t
+set_b_bus_req(struct device *dev, struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct ci_hdrc	*ci = dev_get_drvdata(dev);
+
+	if (count > 2)
+		return -1;
+
+	mutex_lock(&ci->fsm.lock);
+	if (buf[0] == '0')
+		ci->fsm.b_bus_req = 0;
+	else if (buf[0] == '1')
+		ci->fsm.b_bus_req = 1;
+
+	ci_otg_queue_work(ci);
+	mutex_unlock(&ci->fsm.lock);
+
+	return count;
+}
+static DEVICE_ATTR(b_bus_req, S_IRUGO | S_IWUSR, get_b_bus_req, set_b_bus_req);
+
+static ssize_t
+set_a_clr_err(struct device *dev, struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct ci_hdrc	*ci = dev_get_drvdata(dev);
+
+	if (count > 2)
+		return -1;
+
+	mutex_lock(&ci->fsm.lock);
+	if (buf[0] == '1')
+		ci->fsm.a_clr_err = 1;
+
+	ci_otg_queue_work(ci);
+	mutex_unlock(&ci->fsm.lock);
+
+	return count;
+}
+static DEVICE_ATTR(a_clr_err, S_IWUSR, NULL, set_a_clr_err);
+
+static struct attribute *inputs_attrs[] = {
+	&dev_attr_a_bus_req.attr,
+	&dev_attr_a_bus_drop.attr,
+	&dev_attr_b_bus_req.attr,
+	&dev_attr_a_clr_err.attr,
+	NULL,
+};
+
+static struct attribute_group inputs_attr_group = {
+	.name = "inputs",
+	.attrs = inputs_attrs,
+};
+
+/*
+ * Add timer to active timer list
+ */
+static void ci_otg_add_timer(struct ci_hdrc *ci, enum ci_otg_fsm_timer_index t)
+{
+	struct ci_otg_fsm_timer *tmp_timer;
+	struct ci_otg_fsm_timer *timer = ci->fsm_timer->timer_list[t];
+	struct list_head *active_timers = &ci->fsm_timer->active_timers;
+
+	if (t >= NUM_CI_OTG_FSM_TIMERS)
+		return;
+
+	/*
+	 * Check if the timer is already in the active list,
+	 * if so update timer count
+	 */
+	list_for_each_entry(tmp_timer, active_timers, list)
+		if (tmp_timer == timer) {
+			timer->count = timer->expires;
+			return;
+		}
+
+	timer->count = timer->expires;
+	list_add_tail(&timer->list, active_timers);
+
+	/* Enable 1ms irq */
+	if (!(hw_read_otgsc(ci, OTGSC_1MSIE)))
+		hw_write_otgsc(ci, OTGSC_1MSIE, OTGSC_1MSIE);
+}
+
+/*
+ * Remove timer from active timer list
+ */
+static void ci_otg_del_timer(struct ci_hdrc *ci, enum ci_otg_fsm_timer_index t)
+{
+	struct ci_otg_fsm_timer *tmp_timer, *del_tmp;
+	struct ci_otg_fsm_timer *timer = ci->fsm_timer->timer_list[t];
+	struct list_head *active_timers = &ci->fsm_timer->active_timers;
+
+	if (t >= NUM_CI_OTG_FSM_TIMERS)
+		return;
+
+	list_for_each_entry_safe(tmp_timer, del_tmp, active_timers, list)
+		if (tmp_timer == timer)
+			list_del(&timer->list);
+
+	/* Disable 1ms irq if there is no any active timer */
+	if (list_empty(active_timers))
+		hw_write_otgsc(ci, OTGSC_1MSIE, 0);
+}
+
+/*
+ * Reduce timer count by 1, and find timeout conditions.
+ * Called by otg 1ms timer interrupt
+ */
+static inline int ci_otg_tick_timer(struct ci_hdrc *ci)
+{
+	struct ci_otg_fsm_timer *tmp_timer, *del_tmp;
+	struct list_head *active_timers = &ci->fsm_timer->active_timers;
+	int expired = 0;
+
+	list_for_each_entry_safe(tmp_timer, del_tmp, active_timers, list) {
+		tmp_timer->count--;
+		/* check if timer expires */
+		if (!tmp_timer->count) {
+			list_del(&tmp_timer->list);
+			tmp_timer->function(ci, tmp_timer->data);
+			expired = 1;
+		}
+	}
+
+	/* disable 1ms irq if there is no any timer active */
+	if ((expired == 1) && list_empty(active_timers))
+		hw_write_otgsc(ci, OTGSC_1MSIE, 0);
+
+	return expired;
+}
+
+/* The timeout callback function to set time out bit */
+static void set_tmout(void *ptr, unsigned long indicator)
+{
+	*(int *)indicator = 1;
+}
+
+static void set_tmout_and_fsm(void *ptr, unsigned long indicator)
+{
+	struct ci_hdrc *ci = (struct ci_hdrc *)ptr;
+
+	set_tmout(ci, indicator);
+
+	ci_otg_queue_work(ci);
+}
+
+static void a_wait_vfall_tmout_func(void *ptr, unsigned long indicator)
+{
+	struct ci_hdrc *ci = (struct ci_hdrc *)ptr;
+
+	set_tmout(ci, indicator);
+	/* Disable port power */
+	hw_write(ci, OP_PORTSC, PORTSC_W1C_BITS | PORTSC_PP, 0);
+	/* Clear exsiting DP irq */
+	hw_write_otgsc(ci, OTGSC_DPIS, OTGSC_DPIS);
+	/* Enable data pulse irq */
+	hw_write_otgsc(ci, OTGSC_DPIE, OTGSC_DPIE);
+	ci_otg_queue_work(ci);
+}
+
+static void b_ase0_brst_tmout_func(void *ptr, unsigned long indicator)
+{
+	struct ci_hdrc *ci = (struct ci_hdrc *)ptr;
+
+	set_tmout(ci, indicator);
+	if (!hw_read_otgsc(ci, OTGSC_BSV))
+		ci->fsm.b_sess_vld = 0;
+
+	ci_otg_queue_work(ci);
+}
+
+static void b_ssend_srp_tmout_func(void *ptr, unsigned long indicator)
+{
+	struct ci_hdrc *ci = (struct ci_hdrc *)ptr;
+
+	set_tmout(ci, indicator);
+
+	/* only vbus fall below B_sess_vld in b_idle state */
+	if (ci->transceiver->state == OTG_STATE_B_IDLE)
+		ci_otg_queue_work(ci);
+}
+
+static void b_sess_vld_tmout_func(void *ptr, unsigned long indicator)
+{
+	struct ci_hdrc *ci = (struct ci_hdrc *)ptr;
+
+	/* Check if A detached */
+	if (!(hw_read_otgsc(ci, OTGSC_BSV))) {
+		ci->fsm.b_sess_vld = 0;
+		ci_otg_add_timer(ci, B_SSEND_SRP);
+		ci_otg_queue_work(ci);
+	}
+}
+
+static void b_data_pulse_end(void *ptr, unsigned long indicator)
+{
+	struct ci_hdrc *ci = (struct ci_hdrc *)ptr;
+
+	ci->fsm.b_srp_done = 1;
+	ci->fsm.b_bus_req = 0;
+	if (ci->fsm.power_up)
+		ci->fsm.power_up = 0;
+
+	hw_write_otgsc(ci, OTGSC_HABA, 0);
+
+	ci_otg_queue_work(ci);
+}
+
+/* Initialize timers */
+static int ci_otg_init_timers(struct ci_hdrc *ci)
+{
+	struct otg_fsm *fsm = &ci->fsm;
+
+	/* FSM used timers */
+	ci->fsm_timer->timer_list[A_WAIT_VRISE] =
+		otg_timer_initializer(ci, &set_tmout_and_fsm, TA_WAIT_VRISE,
+			(unsigned long)&fsm->a_wait_vrise_tmout);
+	if (ci->fsm_timer->timer_list[A_WAIT_VRISE] == NULL)
+		return -ENOMEM;
+
+	ci->fsm_timer->timer_list[A_WAIT_VFALL] =
+		otg_timer_initializer(ci, &a_wait_vfall_tmout_func,
+		TA_WAIT_VFALL, (unsigned long)&fsm->a_wait_vfall_tmout);
+	if (ci->fsm_timer->timer_list[A_WAIT_VFALL] == NULL)
+		return -ENOMEM;
+
+	ci->fsm_timer->timer_list[A_WAIT_BCON] =
+		otg_timer_initializer(ci, &set_tmout_and_fsm, TA_WAIT_BCON,
+				(unsigned long)&fsm->a_wait_bcon_tmout);
+	if (ci->fsm_timer->timer_list[A_WAIT_BCON] == NULL)
+		return -ENOMEM;
+
+	ci->fsm_timer->timer_list[A_AIDL_BDIS] =
+		otg_timer_initializer(ci, &set_tmout_and_fsm, TA_AIDL_BDIS,
+				(unsigned long)&fsm->a_aidl_bdis_tmout);
+	if (ci->fsm_timer->timer_list[A_AIDL_BDIS] == NULL)
+		return -ENOMEM;
+
+	ci->fsm_timer->timer_list[A_BIDL_ADIS] =
+		otg_timer_initializer(ci, &set_tmout_and_fsm, TA_BIDL_ADIS,
+				(unsigned long)&fsm->a_bidl_adis_tmout);
+	if (ci->fsm_timer->timer_list[A_BIDL_ADIS] == NULL)
+		return -ENOMEM;
+
+	ci->fsm_timer->timer_list[B_ASE0_BRST] =
+		otg_timer_initializer(ci, &b_ase0_brst_tmout_func, TB_ASE0_BRST,
+					(unsigned long)&fsm->b_ase0_brst_tmout);
+	if (ci->fsm_timer->timer_list[B_ASE0_BRST] == NULL)
+		return -ENOMEM;
+
+	ci->fsm_timer->timer_list[B_SE0_SRP] =
+		otg_timer_initializer(ci, &set_tmout_and_fsm, TB_SE0_SRP,
+					(unsigned long)&fsm->b_se0_srp);
+	if (ci->fsm_timer->timer_list[B_SE0_SRP] == NULL)
+		return -ENOMEM;
+
+	ci->fsm_timer->timer_list[B_SSEND_SRP] =
+		otg_timer_initializer(ci, &b_ssend_srp_tmout_func, TB_SSEND_SRP,
+					(unsigned long)&fsm->b_ssend_srp);
+	if (ci->fsm_timer->timer_list[B_SSEND_SRP] == NULL)
+		return -ENOMEM;
+
+	ci->fsm_timer->timer_list[B_SRP_FAIL] =
+		otg_timer_initializer(ci, &set_tmout, TB_SRP_FAIL,
+				(unsigned long)&fsm->b_srp_done);
+	if (ci->fsm_timer->timer_list[B_SRP_FAIL] == NULL)
+		return -ENOMEM;
+
+	ci->fsm_timer->timer_list[B_DATA_PLS] =
+		otg_timer_initializer(ci, &b_data_pulse_end, TB_DATA_PLS, 0);
+	if (ci->fsm_timer->timer_list[B_DATA_PLS] == NULL)
+		return -ENOMEM;
+
+	ci->fsm_timer->timer_list[B_SESS_VLD] =	otg_timer_initializer(ci,
+					&b_sess_vld_tmout_func, TB_SESS_VLD, 0);
+	if (ci->fsm_timer->timer_list[B_SESS_VLD] == NULL)
+		return -ENOMEM;
+
+	return 0;
+}
+
+/* -------------------------------------------------------------*/
+/* Operations that will be called from OTG Finite State Machine */
+/* -------------------------------------------------------------*/
+static void ci_otg_fsm_add_timer(struct otg_fsm *fsm, enum otg_fsm_timer t)
+{
+	struct ci_hdrc	*ci = container_of(fsm, struct ci_hdrc, fsm);
+
+	if (t < NUM_OTG_FSM_TIMERS)
+		ci_otg_add_timer(ci, t);
+	return;
+}
+
+static void ci_otg_fsm_del_timer(struct otg_fsm *fsm, enum otg_fsm_timer t)
+{
+	struct ci_hdrc	*ci = container_of(fsm, struct ci_hdrc, fsm);
+
+	if (t < NUM_OTG_FSM_TIMERS)
+		ci_otg_del_timer(ci, t);
+	return;
+}
+
+/*
+ * A-device drive vbus: turn on vbus regulator and enable port power
+ * Data pulse irq should be disabled while vbus is on.
+ */
+static void ci_otg_drv_vbus(struct otg_fsm *fsm, int on)
+{
+	int ret;
+	struct ci_hdrc	*ci = container_of(fsm, struct ci_hdrc, fsm);
+
+	if (on) {
+		/* Enable power power */
+		hw_write(ci, OP_PORTSC, PORTSC_W1C_BITS | PORTSC_PP,
+							PORTSC_PP);
+		if (ci->platdata->reg_vbus) {
+			ret = regulator_enable(ci->platdata->reg_vbus);
+			if (ret) {
+				dev_err(ci->dev,
+				"Failed to enable vbus regulator, ret=%d\n",
+				ret);
+				return;
+			}
+		}
+		/* Disable data pulse irq */
+		hw_write_otgsc(ci, OTGSC_DPIE, 0);
+
+		fsm->a_srp_det = 0;
+		fsm->power_up = 0;
+	} else {
+		if (ci->platdata->reg_vbus)
+			regulator_disable(ci->platdata->reg_vbus);
+
+		fsm->a_bus_drop = 1;
+		fsm->a_bus_req = 0;
+	}
+}
+
+/*
+ * Control data line by Run Stop bit.
+ */
+static void ci_otg_loc_conn(struct otg_fsm *fsm, int on)
+{
+	struct ci_hdrc	*ci = container_of(fsm, struct ci_hdrc, fsm);
+
+	if (on)
+		hw_write(ci, OP_USBCMD, USBCMD_RS, USBCMD_RS);
+	else
+		hw_write(ci, OP_USBCMD, USBCMD_RS, 0);
+}
+
+/*
+ * Generate SOF by host.
+ * This is controlled through suspend/resume the port.
+ * In host mode, controller will automatically send SOF.
+ * Suspend will block the data on the port.
+ */
+static void ci_otg_loc_sof(struct otg_fsm *fsm, int on)
+{
+	struct ci_hdrc	*ci = container_of(fsm, struct ci_hdrc, fsm);
+
+	if (on)
+		hw_write(ci, OP_PORTSC, PORTSC_W1C_BITS | PORTSC_FPR,
+							PORTSC_FPR);
+	else
+		hw_write(ci, OP_PORTSC, PORTSC_W1C_BITS | PORTSC_SUSP,
+							PORTSC_SUSP);
+}
+
+/*
+ * Start SRP pulsing by data-line pulsing,
+ * no v-bus pulsing followed
+ */
+static void ci_otg_start_pulse(struct otg_fsm *fsm)
+{
+	struct ci_hdrc	*ci = container_of(fsm, struct ci_hdrc, fsm);
+
+	/* Hardware Assistant Data pulse */
+	hw_write_otgsc(ci, OTGSC_HADP, OTGSC_HADP);
+
+	ci_otg_add_timer(ci, B_DATA_PLS);
+}
+
+static int ci_otg_start_host(struct otg_fsm *fsm, int on)
+{
+	struct ci_hdrc	*ci = container_of(fsm, struct ci_hdrc, fsm);
+
+	mutex_unlock(&fsm->lock);
+	if (on) {
+		ci_role_stop(ci);
+		ci_role_start(ci, CI_ROLE_HOST);
+	} else {
+		ci_role_stop(ci);
+		hw_device_reset(ci, USBMODE_CM_DC);
+		ci_role_start(ci, CI_ROLE_GADGET);
+	}
+	mutex_lock(&fsm->lock);
+	return 0;
+}
+
+static int ci_otg_start_gadget(struct otg_fsm *fsm, int on)
+{
+	struct ci_hdrc	*ci = container_of(fsm, struct ci_hdrc, fsm);
+
+	mutex_unlock(&fsm->lock);
+	if (on)
+		usb_gadget_vbus_connect(&ci->gadget);
+	else
+		usb_gadget_vbus_disconnect(&ci->gadget);
+	mutex_lock(&fsm->lock);
+
+	return 0;
+}
+
+static struct otg_fsm_ops ci_otg_ops = {
+	.drv_vbus = ci_otg_drv_vbus,
+	.loc_conn = ci_otg_loc_conn,
+	.loc_sof = ci_otg_loc_sof,
+	.start_pulse = ci_otg_start_pulse,
+	.add_timer = ci_otg_fsm_add_timer,
+	.del_timer = ci_otg_fsm_del_timer,
+	.start_host = ci_otg_start_host,
+	.start_gadget = ci_otg_start_gadget,
+};
+
+int ci_otg_fsm_work(struct ci_hdrc *ci)
+{
+	/*
+	 * Don't do fsm transition for B device
+	 * when there is no gadget class driver
+	 */
+	if (ci->fsm.id && !(ci->driver) &&
+		ci->transceiver->state < OTG_STATE_A_IDLE)
+		return 0;
+
+	if (otg_statemachine(&ci->fsm)) {
+		if (ci->transceiver->state == OTG_STATE_A_IDLE) {
+			/*
+			 * Further state change for cases:
+			 * a_idle to b_idle; or
+			 * a_idle to a_wait_vrise due to ID change(1->0), so
+			 * B-dev becomes A-dev can try to start new session
+			 * consequently; or
+			 * a_idle to a_wait_vrise when power up
+			 */
+			if ((ci->fsm.id) || (ci->id_event) ||
+						(ci->fsm.power_up))
+				ci_otg_queue_work(ci);
+			if (ci->id_event)
+				ci->id_event = false;
+		} else if (ci->transceiver->state == OTG_STATE_B_IDLE) {
+			if (ci->fsm.b_sess_vld) {
+				ci->fsm.power_up = 0;
+				/*
+				 * Further transite to b_periphearl state
+				 * when register gadget driver with vbus on
+				 */
+				ci_otg_queue_work(ci);
+			}
+		}
+	}
+	return 0;
+}
+
+/*
+ * Update fsm variables in each state if catching expected interrupts,
+ * called by otg fsm isr.
+ */
+static void ci_otg_fsm_event(struct ci_hdrc *ci)
+{
+	u32 intr_sts, otg_bsess_vld, port_conn;
+	struct otg_fsm *fsm = &ci->fsm;
+
+	intr_sts = hw_read_intr_status(ci);
+	otg_bsess_vld = hw_read_otgsc(ci, OTGSC_BSV);
+	port_conn = hw_read(ci, OP_PORTSC, PORTSC_CCS);
+
+	switch (ci->transceiver->state) {
+	case OTG_STATE_A_WAIT_BCON:
+		if (port_conn) {
+			fsm->b_conn = 1;
+			fsm->a_bus_req = 1;
+			ci_otg_queue_work(ci);
+		}
+		break;
+	case OTG_STATE_B_IDLE:
+		if (otg_bsess_vld && (intr_sts & USBi_PCI) && port_conn) {
+			fsm->b_sess_vld = 1;
+			ci_otg_queue_work(ci);
+		}
+		break;
+	case OTG_STATE_B_PERIPHERAL:
+		if ((intr_sts & USBi_SLI) && port_conn && otg_bsess_vld) {
+			fsm->a_bus_suspend = 1;
+			ci_otg_queue_work(ci);
+		} else if (intr_sts & USBi_PCI) {
+			if (fsm->a_bus_suspend == 1)
+				fsm->a_bus_suspend = 0;
+		}
+		break;
+	case OTG_STATE_B_HOST:
+		if ((intr_sts & USBi_PCI) && !port_conn) {
+			fsm->a_conn = 0;
+			fsm->b_bus_req = 0;
+			ci_otg_queue_work(ci);
+			ci_otg_add_timer(ci, B_SESS_VLD);
+		}
+		break;
+	case OTG_STATE_A_PERIPHERAL:
+		if (intr_sts & USBi_SLI) {
+			 fsm->b_bus_suspend = 1;
+			/*
+			 * Init a timer to know how long this suspend
+			 * will contine, if time out, indicates B no longer
+			 * wants to be host role
+			 */
+			 ci_otg_add_timer(ci, A_BIDL_ADIS);
+		}
+
+		if (intr_sts & USBi_URI)
+			ci_otg_del_timer(ci, A_BIDL_ADIS);
+
+		if (intr_sts & USBi_PCI) {
+			if (fsm->b_bus_suspend == 1) {
+				ci_otg_del_timer(ci, A_BIDL_ADIS);
+				fsm->b_bus_suspend = 0;
+			}
+		}
+		break;
+	case OTG_STATE_A_SUSPEND:
+		if ((intr_sts & USBi_PCI) && !port_conn) {
+			fsm->b_conn = 0;
+
+			/* if gadget driver is binded */
+			if (ci->driver) {
+				/* A device to be peripheral mode */
+				ci->gadget.is_a_peripheral = 1;
+			}
+			ci_otg_queue_work(ci);
+		}
+		break;
+	case OTG_STATE_A_HOST:
+		if ((intr_sts & USBi_PCI) && !port_conn) {
+			fsm->b_conn = 0;
+			ci_otg_queue_work(ci);
+		}
+		break;
+	case OTG_STATE_B_WAIT_ACON:
+		if ((intr_sts & USBi_PCI) && port_conn) {
+			fsm->a_conn = 1;
+			ci_otg_queue_work(ci);
+		}
+		break;
+	default:
+		break;
+	}
+}
+
+/*
+ * ci_otg_irq - otg fsm related irq handling
+ * and also update otg fsm variable by monitoring usb host and udc
+ * state change interrupts.
+ * @ci: ci_hdrc
+ */
+irqreturn_t ci_otg_fsm_irq(struct ci_hdrc *ci)
+{
+	irqreturn_t retval =  IRQ_NONE;
+	u32 otgsc, otg_int_src = 0;
+	struct otg_fsm *fsm = &ci->fsm;
+
+	otgsc = hw_read_otgsc(ci, ~0);
+	otg_int_src = otgsc & OTGSC_INT_STATUS_BITS & (otgsc >> 8);
+	fsm->id = (otgsc & OTGSC_ID) ? 1 : 0;
+
+	if (otg_int_src) {
+		if (otg_int_src & OTGSC_1MSIS) {
+			hw_write_otgsc(ci, OTGSC_1MSIS, OTGSC_1MSIS);
+			retval = ci_otg_tick_timer(ci);
+			return IRQ_HANDLED;
+		} else if (otg_int_src & OTGSC_DPIS) {
+			hw_write_otgsc(ci, OTGSC_DPIS, OTGSC_DPIS);
+			fsm->a_srp_det = 1;
+			fsm->a_bus_drop = 0;
+		} else if (otg_int_src & OTGSC_IDIS) {
+			hw_write_otgsc(ci, OTGSC_IDIS, OTGSC_IDIS);
+			if (fsm->id == 0) {
+				fsm->a_bus_drop = 0;
+				fsm->a_bus_req = 1;
+				ci->id_event = true;
+			}
+		} else if (otg_int_src & OTGSC_BSVIS) {
+			hw_write_otgsc(ci, OTGSC_BSVIS, OTGSC_BSVIS);
+			if (otgsc & OTGSC_BSV) {
+				fsm->b_sess_vld = 1;
+				ci_otg_del_timer(ci, B_SSEND_SRP);
+				ci_otg_del_timer(ci, B_SRP_FAIL);
+				fsm->b_ssend_srp = 0;
+			} else {
+				fsm->b_sess_vld = 0;
+				if (fsm->id)
+					ci_otg_add_timer(ci, B_SSEND_SRP);
+			}
+		} else if (otg_int_src & OTGSC_AVVIS) {
+			hw_write_otgsc(ci, OTGSC_AVVIS, OTGSC_AVVIS);
+			if (otgsc & OTGSC_AVV) {
+				fsm->a_vbus_vld = 1;
+			} else {
+				fsm->a_vbus_vld = 0;
+				fsm->b_conn = 0;
+			}
+		}
+		ci_otg_queue_work(ci);
+		return IRQ_HANDLED;
+	}
+
+	ci_otg_fsm_event(ci);
+
+	return retval;
+}
+
+void ci_hdrc_otg_fsm_start(struct ci_hdrc *ci)
+{
+	ci_otg_queue_work(ci);
+}
+
+int ci_hdrc_otg_fsm_init(struct ci_hdrc *ci)
+{
+	int retval = 0;
+	struct usb_otg *otg;
+
+	otg = devm_kzalloc(ci->dev,
+			sizeof(struct usb_otg), GFP_KERNEL);
+	if (!otg) {
+		dev_err(ci->dev,
+		"Failed to allocate usb_otg structure for ci hdrc otg!\n");
+		return -ENOMEM;
+	}
+
+	otg->phy = ci->transceiver;
+	otg->gadget = &ci->gadget;
+	ci->fsm.otg = otg;
+	ci->transceiver->otg = ci->fsm.otg;
+	ci->fsm.power_up = 1;
+	ci->fsm.id = hw_read_otgsc(ci, OTGSC_ID) ? 1 : 0;
+	ci->transceiver->state = OTG_STATE_UNDEFINED;
+	ci->fsm.ops = &ci_otg_ops;
+
+	mutex_init(&ci->fsm.lock);
+
+	ci->fsm_timer = devm_kzalloc(ci->dev,
+			sizeof(struct ci_otg_fsm_timer_list), GFP_KERNEL);
+	if (!ci->fsm_timer) {
+		dev_err(ci->dev,
+		"Failed to allocate timer structure for ci hdrc otg!\n");
+		return -ENOMEM;
+	}
+
+	INIT_LIST_HEAD(&ci->fsm_timer->active_timers);
+	retval = ci_otg_init_timers(ci);
+	if (retval) {
+		dev_err(ci->dev, "Couldn't init OTG timers\n");
+		return retval;
+	}
+
+	retval = sysfs_create_group(&ci->dev->kobj, &inputs_attr_group);
+	if (retval < 0) {
+		dev_dbg(ci->dev,
+			"Can't register sysfs attr group: %d\n", retval);
+		return retval;
+	}
+
+	/* Enable A vbus valid irq */
+	hw_write_otgsc(ci, OTGSC_AVVIE, OTGSC_AVVIE);
+
+	if (ci->fsm.id) {
+		ci->fsm.b_ssend_srp =
+			hw_read_otgsc(ci, OTGSC_BSV) ? 0 : 1;
+		ci->fsm.b_sess_vld =
+			hw_read_otgsc(ci, OTGSC_BSV) ? 1 : 0;
+		/* Enable BSV irq */
+		hw_write_otgsc(ci, OTGSC_BSVIE, OTGSC_BSVIE);
+	}
+
+	return 0;
+}
+
+void ci_hdrc_otg_fsm_remove(struct ci_hdrc *ci)
+{
+	sysfs_remove_group(&ci->dev->kobj, &inputs_attr_group);
+}
diff --git a/drivers/usb/chipidea/otg_fsm.h b/drivers/usb/chipidea/otg_fsm.h
new file mode 100644
index 00000000000..94c085f456a
--- /dev/null
+++ b/drivers/usb/chipidea/otg_fsm.h
@@ -0,0 +1,129 @@
+/*
+ * Copyright (C) 2014 Freescale Semiconductor, Inc.
+ *
+ * Author: Jun Li
+ *
+ * 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 __DRIVERS_USB_CHIPIDEA_OTG_FSM_H
+#define __DRIVERS_USB_CHIPIDEA_OTG_FSM_H
+
+#include <linux/usb/otg-fsm.h>
+
+/*
+ *  A-DEVICE timing  constants
+ */
+
+/* Wait for VBUS Rise  */
+#define TA_WAIT_VRISE        (100)	/* a_wait_vrise: section 7.1.2
+					 * a_wait_vrise_tmr: section 7.4.5.1
+					 * TA_VBUS_RISE <= 100ms, section 4.4
+					 * Table 4-1: Electrical Characteristics
+					 * ->DC Electrical Timing
+					 */
+/* Wait for VBUS Fall  */
+#define TA_WAIT_VFALL        (1000)	/* a_wait_vfall: section 7.1.7
+					 * a_wait_vfall_tmr: section: 7.4.5.2
+					 */
+/* Wait for B-Connect */
+#define TA_WAIT_BCON         (10000)	/* a_wait_bcon: section 7.1.3
+					 * TA_WAIT_BCON: should be between 1100
+					 * and 30000 ms, section 5.5, Table 5-1
+					 */
+/* A-Idle to B-Disconnect */
+#define TA_AIDL_BDIS         (5000)	/* a_suspend min 200 ms, section 5.2.1
+					 * TA_AIDL_BDIS: section 5.5, Table 5-1
+					 */
+/* B-Idle to A-Disconnect */
+#define TA_BIDL_ADIS         (500)	/* TA_BIDL_ADIS: section 5.2.1
+					 * 500ms is used for B switch to host
+					 * for safe
+					 */
+
+/*
+ * B-device timing constants
+ */
+
+/* Data-Line Pulse Time*/
+#define TB_DATA_PLS          (10)	/* b_srp_init,continue 5~10ms
+					 * section:5.1.3
+					 */
+/* SRP Fail Time  */
+#define TB_SRP_FAIL          (6000)	/* b_srp_init,fail time 5~6s
+					 * section:5.1.6
+					 */
+/* A-SE0 to B-Reset  */
+#define TB_ASE0_BRST         (155)	/* minimum 155 ms, section:5.3.1 */
+/* SE0 Time Before SRP */
+#define TB_SE0_SRP           (1000)	/* b_idle,minimum 1s, section:5.1.2 */
+/* SSEND time before SRP */
+#define TB_SSEND_SRP         (1500)	/* minimum 1.5 sec, section:5.1.2 */
+
+#define TB_SESS_VLD          (1000)
+
+enum ci_otg_fsm_timer_index {
+	/*
+	 * CI specific timers, start from the end
+	 * of standard and auxiliary OTG timers
+	 */
+	B_DATA_PLS = NUM_OTG_FSM_TIMERS,
+	B_SSEND_SRP,
+	B_SESS_VLD,
+
+	NUM_CI_OTG_FSM_TIMERS,
+};
+
+struct ci_otg_fsm_timer {
+	unsigned long expires;  /* Number of count increase to timeout */
+	unsigned long count;    /* Tick counter */
+	void (*function)(void *, unsigned long);        /* Timeout function */
+	unsigned long data;     /* Data passed to function */
+	struct list_head list;
+};
+
+struct ci_otg_fsm_timer_list {
+	struct ci_otg_fsm_timer *timer_list[NUM_CI_OTG_FSM_TIMERS];
+	struct list_head active_timers;
+};
+
+#ifdef CONFIG_USB_OTG_FSM
+
+int ci_hdrc_otg_fsm_init(struct ci_hdrc *ci);
+int ci_otg_fsm_work(struct ci_hdrc *ci);
+irqreturn_t ci_otg_fsm_irq(struct ci_hdrc *ci);
+void ci_hdrc_otg_fsm_start(struct ci_hdrc *ci);
+void ci_hdrc_otg_fsm_remove(struct ci_hdrc *ci);
+
+#else
+
+static inline int ci_hdrc_otg_fsm_init(struct ci_hdrc *ci)
+{
+	return 0;
+}
+
+static inline int ci_otg_fsm_work(struct ci_hdrc *ci)
+{
+	return -ENXIO;
+}
+
+static inline irqreturn_t ci_otg_fsm_irq(struct ci_hdrc *ci)
+{
+	return IRQ_NONE;
+}
+
+static inline void ci_hdrc_otg_fsm_start(struct ci_hdrc *ci)
+{
+
+}
+
+static inline void ci_hdrc_otg_fsm_remove(struct ci_hdrc *ci)
+{
+
+}
+
+#endif
+
+#endif /* __DRIVERS_USB_CHIPIDEA_OTG_FSM_H */
diff --git a/drivers/usb/chipidea/udc.c b/drivers/usb/chipidea/udc.c
index 7739c64ef25..69425b3cb6b 100644
--- a/drivers/usb/chipidea/udc.c
+++ b/drivers/usb/chipidea/udc.c
@@ -20,6 +20,7 @@
 #include <linux/pm_runtime.h>
 #include <linux/usb/ch9.h>
 #include <linux/usb/gadget.h>
+#include <linux/usb/otg-fsm.h>
 #include <linux/usb/chipidea.h>
 
 #include "ci.h"
@@ -27,6 +28,7 @@
 #include "bits.h"
 #include "debug.h"
 #include "otg.h"
+#include "otg_fsm.h"
 
 /* control endpoint description */
 static const struct usb_endpoint_descriptor
@@ -242,26 +244,6 @@ static int hw_port_is_high_speed(struct ci_hdrc *ci)
 }
 
 /**
- * hw_read_intr_enable: returns interrupt enable register
- *
- * This function returns register data
- */
-static u32 hw_read_intr_enable(struct ci_hdrc *ci)
-{
-	return hw_read(ci, OP_USBINTR, ~0);
-}
-
-/**
- * hw_read_intr_status: returns interrupt status register
- *
- * This function returns register data
- */
-static u32 hw_read_intr_status(struct ci_hdrc *ci)
-{
-	return hw_read(ci, OP_USBSTS, ~0);
-}
-
-/**
  * hw_test_and_clear_complete: test & clear complete status (execute without
  *                             interruption)
  * @n: endpoint number
@@ -727,6 +709,8 @@ __acquires(ci->lock)
 	if (ci->status == NULL)
 		retval = -ENOMEM;
 
+	usb_gadget_set_state(&ci->gadget, USB_STATE_DEFAULT);
+
 done:
 	spin_lock(&ci->lock);
 
@@ -841,7 +825,6 @@ __acquires(hwep->lock)
 	if ((setup->bRequestType & USB_RECIP_MASK) == USB_RECIP_DEVICE) {
 		/* Assume that device is bus powered for now. */
 		*(u16 *)req->buf = ci->remote_wakeup << 1;
-		retval = 0;
 	} else if ((setup->bRequestType & USB_RECIP_MASK) \
 		   == USB_RECIP_ENDPOINT) {
 		dir = (le16_to_cpu(setup->wIndex) & USB_ENDPOINT_DIR_MASK) ?
@@ -883,6 +866,8 @@ isr_setup_status_complete(struct usb_ep *ep, struct usb_request *req)
 	if (ci->setaddr) {
 		hw_usb_set_address(ci, ci->address);
 		ci->setaddr = false;
+		if (ci->address)
+			usb_gadget_set_state(&ci->gadget, USB_STATE_ADDRESS);
 	}
 
 	spin_lock_irqsave(&ci->lock, flags);
@@ -1072,6 +1057,14 @@ __acquires(ci->lock)
 				default:
 					break;
 				}
+				break;
+			case USB_DEVICE_B_HNP_ENABLE:
+				if (ci_otg_is_fsm_mode(ci)) {
+					ci->gadget.b_hnp_enable = 1;
+					err = isr_setup_status_phase(
+							ci);
+				}
+				break;
 			default:
 				goto delegate;
 			}
@@ -1477,7 +1470,7 @@ static int ci_udc_vbus_session(struct usb_gadget *_gadget, int is_active)
 			pm_runtime_get_sync(&_gadget->dev);
 			hw_device_reset(ci, USBMODE_CM_DC);
 			hw_device_state(ci, ci->ep0out->qh.dma);
-			dev_dbg(ci->dev, "Connected to host\n");
+			usb_gadget_set_state(_gadget, USB_STATE_POWERED);
 		} else {
 			if (ci->driver)
 				ci->driver->disconnect(&ci->gadget);
@@ -1487,7 +1480,7 @@ static int ci_udc_vbus_session(struct usb_gadget *_gadget, int is_active)
 				CI_HDRC_CONTROLLER_STOPPED_EVENT);
 			_gadget_stop_activity(&ci->gadget);
 			pm_runtime_put_sync(&_gadget->dev);
-			dev_dbg(ci->dev, "Disconnected from host\n");
+			usb_gadget_set_state(_gadget, USB_STATE_NOTATTACHED);
 		}
 	}
 
@@ -1655,6 +1648,13 @@ static int ci_udc_start(struct usb_gadget *gadget,
 		return retval;
 
 	ci->driver = driver;
+
+	/* Start otg fsm for B-device */
+	if (ci_otg_is_fsm_mode(ci) && ci->fsm.id) {
+		ci_hdrc_otg_fsm_start(ci);
+		return retval;
+	}
+
 	pm_runtime_get_sync(&ci->gadget.dev);
 	if (ci->vbus_active) {
 		spin_lock_irqsave(&ci->lock, flags);
@@ -1753,6 +1753,8 @@ static irqreturn_t udc_irq(struct ci_hdrc *ci)
 				ci->suspended = 1;
 				spin_unlock(&ci->lock);
 				ci->driver->suspend(&ci->gadget);
+				usb_gadget_set_state(&ci->gadget,
+						USB_STATE_SUSPENDED);
 				spin_lock(&ci->lock);
 			}
 		}
@@ -1779,7 +1781,7 @@ static int udc_start(struct ci_hdrc *ci)
 	ci->gadget.ops          = &usb_gadget_ops;
 	ci->gadget.speed        = USB_SPEED_UNKNOWN;
 	ci->gadget.max_speed    = USB_SPEED_HIGH;
-	ci->gadget.is_otg       = 0;
+	ci->gadget.is_otg       = ci->is_otg ? 1 : 0;
 	ci->gadget.name         = ci->platdata->name;
 
 	INIT_LIST_HEAD(&ci->gadget.ep_list);
@@ -1843,21 +1845,22 @@ void ci_hdrc_gadget_destroy(struct ci_hdrc *ci)
 
 static int udc_id_switch_for_device(struct ci_hdrc *ci)
 {
-	if (ci->is_otg) {
-		ci_clear_otg_interrupt(ci, OTGSC_BSVIS);
-		ci_enable_otg_interrupt(ci, OTGSC_BSVIE);
-	}
+	if (ci->is_otg)
+		/* Clear and enable BSV irq */
+		hw_write_otgsc(ci, OTGSC_BSVIS | OTGSC_BSVIE,
+					OTGSC_BSVIS | OTGSC_BSVIE);
 
 	return 0;
 }
 
 static void udc_id_switch_for_host(struct ci_hdrc *ci)
 {
-	if (ci->is_otg) {
-		/* host doesn't care B_SESSION_VALID event */
-		ci_clear_otg_interrupt(ci, OTGSC_BSVIS);
-		ci_disable_otg_interrupt(ci, OTGSC_BSVIE);
-	}
+	/*
+	 * host doesn't care B_SESSION_VALID event
+	 * so clear and disbale BSV irq
+	 */
+	if (ci->is_otg)
+		hw_write_otgsc(ci, OTGSC_BSVIE | OTGSC_BSVIS, OTGSC_BSVIS);
 }
 
 /**
diff --git a/drivers/usb/chipidea/usbmisc_imx.c b/drivers/usb/chipidea/usbmisc_imx.c
index cd061abe350..85293b8b1df 100644
--- a/drivers/usb/chipidea/usbmisc_imx.c
+++ b/drivers/usb/chipidea/usbmisc_imx.c
@@ -21,16 +21,39 @@
 #define MX25_USB_PHY_CTRL_OFFSET	0x08
 #define MX25_BM_EXTERNAL_VBUS_DIVIDER	BIT(23)
 
+#define MX25_EHCI_INTERFACE_SINGLE_UNI	(2 << 0)
+#define MX25_EHCI_INTERFACE_DIFF_UNI	(0 << 0)
+#define MX25_EHCI_INTERFACE_MASK	(0xf)
+
+#define MX25_OTG_SIC_SHIFT		29
+#define MX25_OTG_SIC_MASK		(0x3 << MX25_OTG_SIC_SHIFT)
+#define MX25_OTG_PM_BIT			BIT(24)
+#define MX25_OTG_PP_BIT			BIT(11)
+#define MX25_OTG_OCPOL_BIT		BIT(3)
+
+#define MX25_H1_SIC_SHIFT		21
+#define MX25_H1_SIC_MASK		(0x3 << MX25_H1_SIC_SHIFT)
+#define MX25_H1_PP_BIT			BIT(18)
+#define MX25_H1_PM_BIT			BIT(16)
+#define MX25_H1_IPPUE_UP_BIT		BIT(7)
+#define MX25_H1_IPPUE_DOWN_BIT		BIT(6)
+#define MX25_H1_TLL_BIT			BIT(5)
+#define MX25_H1_USBTE_BIT		BIT(4)
+#define MX25_H1_OCPOL_BIT		BIT(2)
+
 #define MX27_H1_PM_BIT			BIT(8)
 #define MX27_H2_PM_BIT			BIT(16)
 #define MX27_OTG_PM_BIT			BIT(24)
 
 #define MX53_USB_OTG_PHY_CTRL_0_OFFSET	0x08
+#define MX53_USB_OTG_PHY_CTRL_1_OFFSET	0x0c
 #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 MX53_USB_PHYCTRL1_PLLDIV_MASK	0x3
+#define MX53_USB_PLL_DIV_24_MHZ		0x01
 
 #define MX6_BM_OVER_CUR_DIS		BIT(7)
 
@@ -50,6 +73,39 @@ struct imx_usbmisc {
 
 static struct imx_usbmisc *usbmisc;
 
+static int usbmisc_imx25_init(struct imx_usbmisc_data *data)
+{
+	unsigned long flags;
+	u32 val = 0;
+
+	if (data->index > 1)
+		return -EINVAL;
+
+	spin_lock_irqsave(&usbmisc->lock, flags);
+	switch (data->index) {
+	case 0:
+		val = readl(usbmisc->base);
+		val &= ~(MX25_OTG_SIC_MASK | MX25_OTG_PP_BIT);
+		val |= (MX25_EHCI_INTERFACE_DIFF_UNI & MX25_EHCI_INTERFACE_MASK) << MX25_OTG_SIC_SHIFT;
+		val |= (MX25_OTG_PM_BIT | MX25_OTG_OCPOL_BIT);
+		writel(val, usbmisc->base);
+		break;
+	case 1:
+		val = readl(usbmisc->base);
+		val &= ~(MX25_H1_SIC_MASK | MX25_H1_PP_BIT |  MX25_H1_IPPUE_UP_BIT);
+		val |= (MX25_EHCI_INTERFACE_SINGLE_UNI & MX25_EHCI_INTERFACE_MASK) << MX25_H1_SIC_SHIFT;
+		val |= (MX25_H1_PM_BIT | MX25_H1_OCPOL_BIT | MX25_H1_TLL_BIT |
+			MX25_H1_USBTE_BIT | MX25_H1_IPPUE_DOWN_BIT);
+
+		writel(val, usbmisc->base);
+
+		break;
+	}
+	spin_unlock_irqrestore(&usbmisc->lock, flags);
+
+	return 0;
+}
+
 static int usbmisc_imx25_post(struct imx_usbmisc_data *data)
 {
 	void __iomem *reg;
@@ -111,6 +167,13 @@ static int usbmisc_imx53_init(struct imx_usbmisc_data *data)
 	if (data->index > 3)
 		return -EINVAL;
 
+	/* Select a 24 MHz reference clock for the PHY  */
+	reg = usbmisc->base + MX53_USB_OTG_PHY_CTRL_1_OFFSET;
+	val = readl(reg);
+	val &= ~MX53_USB_PHYCTRL1_PLLDIV_MASK;
+	val |= MX53_USB_PLL_DIV_24_MHZ;
+	writel(val, usbmisc->base + MX53_USB_OTG_PHY_CTRL_1_OFFSET);
+
 	if (data->disable_oc) {
 		spin_lock_irqsave(&usbmisc->lock, flags);
 		switch (data->index) {
@@ -159,6 +222,7 @@ static int usbmisc_imx6q_init(struct imx_usbmisc_data *data)
 }
 
 static const struct usbmisc_ops imx25_usbmisc_ops = {
+	.init = usbmisc_imx25_init,
 	.post = usbmisc_imx25_post,
 };
 
@@ -200,6 +264,10 @@ static const struct of_device_id usbmisc_imx_dt_ids[] = {
 		.data = &imx25_usbmisc_ops,
 	},
 	{
+		.compatible = "fsl,imx35-usbmisc",
+		.data = &imx25_usbmisc_ops,
+	},
+	{
 		.compatible = "fsl,imx27-usbmisc",
 		.data = &imx27_usbmisc_ops,
 	},
diff --git a/drivers/usb/class/cdc-acm.c b/drivers/usb/class/cdc-acm.c
index 900f7ff805e..e934e19f49f 100644
--- a/drivers/usb/class/cdc-acm.c
+++ b/drivers/usb/class/cdc-acm.c
@@ -122,13 +122,23 @@ static void acm_release_minor(struct acm *acm)
 static int acm_ctrl_msg(struct acm *acm, int request, int value,
 							void *buf, int len)
 {
-	int retval = usb_control_msg(acm->dev, usb_sndctrlpipe(acm->dev, 0),
+	int retval;
+
+	retval = usb_autopm_get_interface(acm->control);
+	if (retval)
+		return retval;
+
+	retval = usb_control_msg(acm->dev, usb_sndctrlpipe(acm->dev, 0),
 		request, USB_RT_ACM, value,
 		acm->control->altsetting[0].desc.bInterfaceNumber,
 		buf, len, 5000);
+
 	dev_dbg(&acm->control->dev,
 			"%s - rq 0x%02x, val %#x, len %#x, result %d\n",
 			__func__, request, value, len, retval);
+
+	usb_autopm_put_interface(acm->control);
+
 	return retval < 0 ? retval : 0;
 }
 
@@ -406,19 +416,21 @@ static void acm_read_bulk_callback(struct urb *urb)
 		dev_dbg(&acm->data->dev, "%s - disconnected\n", __func__);
 		return;
 	}
-	usb_mark_last_busy(acm->dev);
 
 	if (urb->status) {
 		dev_dbg(&acm->data->dev, "%s - non-zero urb status: %d\n",
 							__func__, urb->status);
 		return;
 	}
+
+	usb_mark_last_busy(acm->dev);
+
 	acm_process_read_urb(acm, urb);
 
 	/* throttle device if requested by tty */
 	spin_lock_irqsave(&acm->read_lock, flags);
 	acm->throttled = acm->throttle_req;
-	if (!acm->throttled && !acm->susp_count) {
+	if (!acm->throttled) {
 		spin_unlock_irqrestore(&acm->read_lock, flags);
 		acm_submit_read_urb(acm, rb->index, GFP_ATOMIC);
 	} else {
@@ -492,10 +504,30 @@ static int acm_tty_open(struct tty_struct *tty, struct file *filp)
 	return tty_port_open(&acm->port, tty, filp);
 }
 
+static void acm_port_dtr_rts(struct tty_port *port, int raise)
+{
+	struct acm *acm = container_of(port, struct acm, port);
+	int val;
+	int res;
+
+	if (raise)
+		val = ACM_CTRL_DTR | ACM_CTRL_RTS;
+	else
+		val = 0;
+
+	/* FIXME: add missing ctrlout locking throughout driver */
+	acm->ctrlout = val;
+
+	res = acm_set_control(acm, val);
+	if (res && (acm->ctrl_caps & USB_CDC_CAP_LINE))
+		dev_err(&acm->control->dev, "failed to set dtr/rts\n");
+}
+
 static int acm_port_activate(struct tty_port *port, struct tty_struct *tty)
 {
 	struct acm *acm = container_of(port, struct acm, port);
 	int retval = -ENODEV;
+	int i;
 
 	dev_dbg(&acm->control->dev, "%s\n", __func__);
 
@@ -515,19 +547,13 @@ static int acm_port_activate(struct tty_port *port, struct tty_struct *tty)
 	acm->control->needs_remote_wakeup = 1;
 
 	acm->ctrlurb->dev = acm->dev;
-	if (usb_submit_urb(acm->ctrlurb, GFP_KERNEL)) {
+	retval = usb_submit_urb(acm->ctrlurb, GFP_KERNEL);
+	if (retval) {
 		dev_err(&acm->control->dev,
 			"%s - usb_submit_urb(ctrl irq) failed\n", __func__);
 		goto error_submit_urb;
 	}
 
-	acm->ctrlout = ACM_CTRL_DTR | ACM_CTRL_RTS;
-	if (acm_set_control(acm, acm->ctrlout) < 0 &&
-	    (acm->ctrl_caps & USB_CDC_CAP_LINE))
-		goto error_set_control;
-
-	usb_autopm_put_interface(acm->control);
-
 	/*
 	 * Unthrottle device in case the TTY was closed while throttled.
 	 */
@@ -536,24 +562,27 @@ static int acm_port_activate(struct tty_port *port, struct tty_struct *tty)
 	acm->throttle_req = 0;
 	spin_unlock_irq(&acm->read_lock);
 
-	if (acm_submit_read_urbs(acm, GFP_KERNEL))
+	retval = acm_submit_read_urbs(acm, GFP_KERNEL);
+	if (retval)
 		goto error_submit_read_urbs;
 
+	usb_autopm_put_interface(acm->control);
+
 	mutex_unlock(&acm->mutex);
 
 	return 0;
 
 error_submit_read_urbs:
-	acm->ctrlout = 0;
-	acm_set_control(acm, acm->ctrlout);
-error_set_control:
+	for (i = 0; i < acm->rx_buflimit; i++)
+		usb_kill_urb(acm->read_urbs[i]);
 	usb_kill_urb(acm->ctrlurb);
 error_submit_urb:
 	usb_autopm_put_interface(acm->control);
 error_get_interface:
 disconnected:
 	mutex_unlock(&acm->mutex);
-	return retval;
+
+	return usb_translate_errors(retval);
 }
 
 static void acm_port_destruct(struct tty_port *port)
@@ -571,23 +600,37 @@ static void acm_port_destruct(struct tty_port *port)
 static void acm_port_shutdown(struct tty_port *port)
 {
 	struct acm *acm = container_of(port, struct acm, port);
+	struct urb *urb;
+	struct acm_wb *wb;
 	int i;
 
 	dev_dbg(&acm->control->dev, "%s\n", __func__);
 
-	mutex_lock(&acm->mutex);
-	if (!acm->disconnected) {
-		usb_autopm_get_interface(acm->control);
-		acm_set_control(acm, acm->ctrlout = 0);
-		usb_kill_urb(acm->ctrlurb);
-		for (i = 0; i < ACM_NW; i++)
-			usb_kill_urb(acm->wb[i].urb);
-		for (i = 0; i < acm->rx_buflimit; i++)
-			usb_kill_urb(acm->read_urbs[i]);
-		acm->control->needs_remote_wakeup = 0;
-		usb_autopm_put_interface(acm->control);
+	/*
+	 * Need to grab write_lock to prevent race with resume, but no need to
+	 * hold it due to the tty-port initialised flag.
+	 */
+	spin_lock_irq(&acm->write_lock);
+	spin_unlock_irq(&acm->write_lock);
+
+	usb_autopm_get_interface_no_resume(acm->control);
+	acm->control->needs_remote_wakeup = 0;
+	usb_autopm_put_interface(acm->control);
+
+	for (;;) {
+		urb = usb_get_from_anchor(&acm->delayed);
+		if (!urb)
+			break;
+		wb = urb->context;
+		wb->use = 0;
+		usb_autopm_put_interface_async(acm->control);
 	}
-	mutex_unlock(&acm->mutex);
+
+	usb_kill_urb(acm->ctrlurb);
+	for (i = 0; i < ACM_NW; i++)
+		usb_kill_urb(acm->wb[i].urb);
+	for (i = 0; i < acm->rx_buflimit; i++)
+		usb_kill_urb(acm->read_urbs[i]);
 }
 
 static void acm_tty_cleanup(struct tty_struct *tty)
@@ -644,16 +687,18 @@ static int acm_tty_write(struct tty_struct *tty,
 	memcpy(wb->buf, buf, count);
 	wb->len = count;
 
-	usb_autopm_get_interface_async(acm->control);
+	stat = usb_autopm_get_interface_async(acm->control);
+	if (stat) {
+		wb->use = 0;
+		spin_unlock_irqrestore(&acm->write_lock, flags);
+		return stat;
+	}
+
 	if (acm->susp_count) {
-		if (!acm->delayed_wb)
-			acm->delayed_wb = wb;
-		else
-			usb_autopm_put_interface_async(acm->control);
+		usb_anchor_urb(wb->urb, &acm->delayed);
 		spin_unlock_irqrestore(&acm->write_lock, flags);
-		return count;	/* A white lie */
+		return count;
 	}
-	usb_mark_last_busy(acm->dev);
 
 	stat = acm_start_wb(acm, wb);
 	spin_unlock_irqrestore(&acm->write_lock, flags);
@@ -956,6 +1001,7 @@ static void acm_tty_set_termios(struct tty_struct *tty,
 }
 
 static const struct tty_port_operations acm_port_ops = {
+	.dtr_rts = acm_port_dtr_rts,
 	.shutdown = acm_port_shutdown,
 	.activate = acm_port_activate,
 	.destruct = acm_port_destruct,
@@ -1267,6 +1313,7 @@ made_compressed_probe:
 		acm->bInterval = epread->bInterval;
 	tty_port_init(&acm->port);
 	acm->port.ops = &acm_port_ops;
+	init_usb_anchor(&acm->delayed);
 
 	buf = usb_alloc_coherent(usb_dev, ctrlsize, GFP_KERNEL, &acm->ctrl_dma);
 	if (!buf) {
@@ -1392,8 +1439,6 @@ skip_countries:
 
 	dev_info(&intf->dev, "ttyACM%d: USB ACM device\n", minor);
 
-	acm_set_control(acm, acm->ctrlout);
-
 	acm->line.dwDTERate = cpu_to_le32(9600);
 	acm->line.bDataBits = 8;
 	acm_set_line(acm, &acm->line);
@@ -1512,27 +1557,20 @@ static int acm_suspend(struct usb_interface *intf, pm_message_t message)
 	struct acm *acm = usb_get_intfdata(intf);
 	int cnt;
 
+	spin_lock_irq(&acm->write_lock);
 	if (PMSG_IS_AUTO(message)) {
-		int b;
-
-		spin_lock_irq(&acm->write_lock);
-		b = acm->transmitting;
-		spin_unlock_irq(&acm->write_lock);
-		if (b)
+		if (acm->transmitting) {
+			spin_unlock_irq(&acm->write_lock);
 			return -EBUSY;
+		}
 	}
-
-	spin_lock_irq(&acm->read_lock);
-	spin_lock(&acm->write_lock);
 	cnt = acm->susp_count++;
-	spin_unlock(&acm->write_lock);
-	spin_unlock_irq(&acm->read_lock);
+	spin_unlock_irq(&acm->write_lock);
 
 	if (cnt)
 		return 0;
 
-	if (test_bit(ASYNCB_INITIALIZED, &acm->port.flags))
-		stop_data_traffic(acm);
+	stop_data_traffic(acm);
 
 	return 0;
 }
@@ -1540,29 +1578,23 @@ static int acm_suspend(struct usb_interface *intf, pm_message_t message)
 static int acm_resume(struct usb_interface *intf)
 {
 	struct acm *acm = usb_get_intfdata(intf);
-	struct acm_wb *wb;
+	struct urb *urb;
 	int rv = 0;
-	int cnt;
 
-	spin_lock_irq(&acm->read_lock);
-	acm->susp_count -= 1;
-	cnt = acm->susp_count;
-	spin_unlock_irq(&acm->read_lock);
+	spin_lock_irq(&acm->write_lock);
 
-	if (cnt)
-		return 0;
+	if (--acm->susp_count)
+		goto out;
 
 	if (test_bit(ASYNCB_INITIALIZED, &acm->port.flags)) {
-		rv = usb_submit_urb(acm->ctrlurb, GFP_NOIO);
+		rv = usb_submit_urb(acm->ctrlurb, GFP_ATOMIC);
 
-		spin_lock_irq(&acm->write_lock);
-		if (acm->delayed_wb) {
-			wb = acm->delayed_wb;
-			acm->delayed_wb = NULL;
-			spin_unlock_irq(&acm->write_lock);
-			acm_start_wb(acm, wb);
-		} else {
-			spin_unlock_irq(&acm->write_lock);
+		for (;;) {
+			urb = usb_get_from_anchor(&acm->delayed);
+			if (!urb)
+				break;
+
+			acm_start_wb(acm, urb->context);
 		}
 
 		/*
@@ -1570,12 +1602,13 @@ static int acm_resume(struct usb_interface *intf)
 		 * do the write path at all cost
 		 */
 		if (rv < 0)
-			goto err_out;
+			goto out;
 
-		rv = acm_submit_read_urbs(acm, GFP_NOIO);
+		rv = acm_submit_read_urbs(acm, GFP_ATOMIC);
 	}
+out:
+	spin_unlock_irq(&acm->write_lock);
 
-err_out:
 	return rv;
 }
 
@@ -1652,13 +1685,27 @@ static const struct usb_device_id acm_ids[] = {
 	},
 	/* Motorola H24 HSPA module: */
 	{ USB_DEVICE(0x22b8, 0x2d91) }, /* modem                                */
-	{ USB_DEVICE(0x22b8, 0x2d92) }, /* modem           + diagnostics        */
-	{ USB_DEVICE(0x22b8, 0x2d93) }, /* modem + AT port                      */
-	{ USB_DEVICE(0x22b8, 0x2d95) }, /* modem + AT port + diagnostics        */
-	{ USB_DEVICE(0x22b8, 0x2d96) }, /* modem                         + NMEA */
-	{ USB_DEVICE(0x22b8, 0x2d97) }, /* modem           + diagnostics + NMEA */
-	{ USB_DEVICE(0x22b8, 0x2d99) }, /* modem + AT port               + NMEA */
-	{ USB_DEVICE(0x22b8, 0x2d9a) }, /* modem + AT port + diagnostics + NMEA */
+	{ USB_DEVICE(0x22b8, 0x2d92),   /* modem           + diagnostics        */
+	.driver_info = NO_UNION_NORMAL, /* handle only modem interface          */
+	},
+	{ USB_DEVICE(0x22b8, 0x2d93),   /* modem + AT port                      */
+	.driver_info = NO_UNION_NORMAL, /* handle only modem interface          */
+	},
+	{ USB_DEVICE(0x22b8, 0x2d95),   /* modem + AT port + diagnostics        */
+	.driver_info = NO_UNION_NORMAL, /* handle only modem interface          */
+	},
+	{ USB_DEVICE(0x22b8, 0x2d96),   /* modem                         + NMEA */
+	.driver_info = NO_UNION_NORMAL, /* handle only modem interface          */
+	},
+	{ USB_DEVICE(0x22b8, 0x2d97),   /* modem           + diagnostics + NMEA */
+	.driver_info = NO_UNION_NORMAL, /* handle only modem interface          */
+	},
+	{ USB_DEVICE(0x22b8, 0x2d99),   /* modem + AT port               + NMEA */
+	.driver_info = NO_UNION_NORMAL, /* handle only modem interface          */
+	},
+	{ USB_DEVICE(0x22b8, 0x2d9a),   /* modem + AT port + diagnostics + NMEA */
+	.driver_info = NO_UNION_NORMAL, /* handle only modem interface          */
+	},
 
 	{ USB_DEVICE(0x0572, 0x1329), /* Hummingbird huc56s (Conexant) */
 	.driver_info = NO_UNION_NORMAL, /* union descriptor misplaced on
diff --git a/drivers/usb/class/cdc-acm.h b/drivers/usb/class/cdc-acm.h
index e38dc785808..fc75651afe1 100644
--- a/drivers/usb/class/cdc-acm.h
+++ b/drivers/usb/class/cdc-acm.h
@@ -120,15 +120,15 @@ struct acm {
 	unsigned int throttled:1;			/* actually throttled */
 	unsigned int throttle_req:1;			/* throttle requested */
 	u8 bInterval;
-	struct acm_wb *delayed_wb;			/* write queued for a device about to be woken */
+	struct usb_anchor delayed;			/* writes queued for a device about to be woken */
 };
 
 #define CDC_DATA_INTERFACE_TYPE	0x0a
 
 /* constants describing various quirks and errors */
-#define NO_UNION_NORMAL			1
-#define SINGLE_RX_URB			2
-#define NO_CAP_LINE			4
-#define NOT_A_MODEM			8
-#define NO_DATA_INTERFACE		16
-#define IGNORE_DEVICE			32
+#define NO_UNION_NORMAL			BIT(0)
+#define SINGLE_RX_URB			BIT(1)
+#define NO_CAP_LINE			BIT(2)
+#define NOT_A_MODEM			BIT(3)
+#define NO_DATA_INTERFACE		BIT(4)
+#define IGNORE_DEVICE			BIT(5)
diff --git a/drivers/usb/class/usbtmc.c b/drivers/usb/class/usbtmc.c
index cfbec9c7e09..103a6e9ee49 100644
--- a/drivers/usb/class/usbtmc.c
+++ b/drivers/usb/class/usbtmc.c
@@ -383,9 +383,12 @@ exit:
 static int send_request_dev_dep_msg_in(struct usbtmc_device_data *data, size_t transfer_size)
 {
 	int retval;
-	u8 buffer[USBTMC_HEADER_SIZE];
+	u8 *buffer;
 	int actual;
 
+	buffer = kmalloc(USBTMC_HEADER_SIZE, GFP_KERNEL);
+	if (!buffer)
+		return -ENOMEM;
 	/* Setup IO buffer for REQUEST_DEV_DEP_MSG_IN message
 	 * Refer to class specs for details
 	 */
@@ -417,6 +420,7 @@ static int send_request_dev_dep_msg_in(struct usbtmc_device_data *data, size_t t
 	if (!data->bTag)
 		data->bTag++;
 
+	kfree(buffer);
 	if (retval < 0) {
 		dev_err(&data->intf->dev, "usb_bulk_msg in send_request_dev_dep_msg_in() returned %d\n", retval);
 		return retval;
diff --git a/drivers/usb/common/Makefile b/drivers/usb/common/Makefile
new file mode 100644
index 00000000000..752646167e1
--- /dev/null
+++ b/drivers/usb/common/Makefile
@@ -0,0 +1,6 @@
+#
+# Makefile for the usb common parts.
+#
+
+obj-$(CONFIG_USB_COMMON) += usb-common.o
+obj-$(CONFIG_USB_OTG_FSM) += usb-otg-fsm.o
diff --git a/drivers/usb/usb-common.c b/drivers/usb/common/usb-common.c
index d771870a819..6dfd30a863c 100644
--- a/drivers/usb/usb-common.c
+++ b/drivers/usb/common/usb-common.c
@@ -69,7 +69,7 @@ const char *usb_state_string(enum usb_device_state state)
 		[USB_STATE_RECONNECTING] = "reconnecting",
 		[USB_STATE_UNAUTHENTICATED] = "unauthenticated",
 		[USB_STATE_DEFAULT] = "default",
-		[USB_STATE_ADDRESS] = "addresssed",
+		[USB_STATE_ADDRESS] = "addressed",
 		[USB_STATE_CONFIGURED] = "configured",
 		[USB_STATE_SUSPENDED] = "suspended",
 	};
diff --git a/drivers/usb/phy/phy-fsm-usb.c b/drivers/usb/common/usb-otg-fsm.c
index c47e5a6edde..98e8340a5bb 100644
--- a/drivers/usb/phy/phy-fsm-usb.c
+++ b/drivers/usb/common/usb-otg-fsm.c
@@ -303,17 +303,18 @@ int otg_statemachine(struct otg_fsm *fsm)
 			otg_set_state(fsm, OTG_STATE_A_WAIT_VRISE);
 		break;
 	case OTG_STATE_A_WAIT_VRISE:
-		if (fsm->id || fsm->a_bus_drop || fsm->a_vbus_vld ||
-				fsm->a_wait_vrise_tmout) {
+		if (fsm->a_vbus_vld)
 			otg_set_state(fsm, OTG_STATE_A_WAIT_BCON);
-		}
+		else if (fsm->id || fsm->a_bus_drop ||
+				fsm->a_wait_vrise_tmout)
+			otg_set_state(fsm, OTG_STATE_A_WAIT_VFALL);
 		break;
 	case OTG_STATE_A_WAIT_BCON:
 		if (!fsm->a_vbus_vld)
 			otg_set_state(fsm, OTG_STATE_A_VBUS_ERR);
 		else if (fsm->b_conn)
 			otg_set_state(fsm, OTG_STATE_A_HOST);
-		else if (fsm->id | fsm->a_bus_drop | fsm->a_wait_bcon_tmout)
+		else if (fsm->id || fsm->a_bus_drop || fsm->a_wait_bcon_tmout)
 			otg_set_state(fsm, OTG_STATE_A_WAIT_VFALL);
 		break;
 	case OTG_STATE_A_HOST:
@@ -363,3 +364,4 @@ int otg_statemachine(struct otg_fsm *fsm)
 	VDBG("quit statemachine, changed = %d\n", state_changed);
 	return state_changed;
 }
+EXPORT_SYMBOL_GPL(otg_statemachine);
diff --git a/drivers/usb/core/Kconfig b/drivers/usb/core/Kconfig
index cb8e99156f5..1060657ca1b 100644
--- a/drivers/usb/core/Kconfig
+++ b/drivers/usb/core/Kconfig
@@ -1,13 +1,6 @@
 #
 # USB Core configuration
 #
-config USB_DEBUG
-	bool "USB verbose debug messages"
-	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
-	  problem with USB support and want to see more of what is going on.
-
 config USB_ANNOUNCE_NEW_DEVICES
 	bool "USB announce new devices"
 	help
@@ -88,3 +81,12 @@ config USB_OTG_BLACKLIST_HUB
 	  and software costs by not supporting external hubs.  So
 	  are "Embedded Hosts" that don't offer OTG support.
 
+config USB_OTG_FSM
+	tristate "USB 2.0 OTG FSM implementation"
+	depends on USB
+	select USB_OTG
+	select USB_PHY
+	help
+	  Implements OTG Finite State Machine as specified in On-The-Go
+	  and Embedded Host Supplement to the USB Revision 2.0 Specification.
+
diff --git a/drivers/usb/core/hcd-pci.c b/drivers/usb/core/hcd-pci.c
index d59d99347d5..82044b5d611 100644
--- a/drivers/usb/core/hcd-pci.c
+++ b/drivers/usb/core/hcd-pci.c
@@ -75,7 +75,7 @@ static void for_each_companion(struct pci_dev *pdev, struct usb_hcd *hcd,
 				PCI_SLOT(companion->devfn) != slot)
 			continue;
 		companion_hcd = pci_get_drvdata(companion);
-		if (!companion_hcd)
+		if (!companion_hcd || !companion_hcd->self.root_hub)
 			continue;
 		fn(pdev, hcd, companion, companion_hcd);
 	}
@@ -192,7 +192,6 @@ int usb_hcd_pci_probe(struct pci_dev *dev, const struct pci_device_id *id)
 
 	if (pci_enable_device(dev) < 0)
 		return -ENODEV;
-	dev->current_state = PCI_D0;
 
 	/*
 	 * The xHCI driver has its own irq management
diff --git a/drivers/usb/core/hcd.c b/drivers/usb/core/hcd.c
index 9c4e2922b04..bec31e2efb8 100644
--- a/drivers/usb/core/hcd.c
+++ b/drivers/usb/core/hcd.c
@@ -918,6 +918,7 @@ static void usb_bus_init (struct usb_bus *bus)
 	bus->bandwidth_allocated = 0;
 	bus->bandwidth_int_reqs  = 0;
 	bus->bandwidth_isoc_reqs = 0;
+	mutex_init(&bus->usb_address0_mutex);
 
 	INIT_LIST_HEAD (&bus->bus_list);
 }
@@ -1502,6 +1503,9 @@ int usb_hcd_map_urb_for_dma(struct usb_hcd *hcd, struct urb *urb,
 					ret = -EAGAIN;
 				else
 					urb->transfer_flags |= URB_DMA_MAP_PAGE;
+			} else if (is_vmalloc_addr(urb->transfer_buffer)) {
+				WARN_ONCE(1, "transfer buffer not dma capable\n");
+				ret = -EAGAIN;
 			} else {
 				urb->transfer_dma = dma_map_single(
 						hcd->self.controller,
@@ -2263,9 +2267,7 @@ static void hcd_resume_work(struct work_struct *work)
 	struct usb_hcd *hcd = container_of(work, struct usb_hcd, wakeup_work);
 	struct usb_device *udev = hcd->self.root_hub;
 
-	usb_lock_device(udev);
 	usb_remote_wakeup(udev);
-	usb_unlock_device(udev);
 }
 
 /**
@@ -2454,11 +2456,13 @@ struct usb_hcd *usb_create_shared_hcd(const struct hc_driver *driver,
 		mutex_init(hcd->bandwidth_mutex);
 		dev_set_drvdata(dev, hcd);
 	} else {
+		mutex_lock(&usb_port_peer_mutex);
 		hcd->bandwidth_mutex = primary_hcd->bandwidth_mutex;
 		hcd->primary_hcd = primary_hcd;
 		primary_hcd->primary_hcd = primary_hcd;
 		hcd->shared_hcd = primary_hcd;
 		primary_hcd->shared_hcd = hcd;
+		mutex_unlock(&usb_port_peer_mutex);
 	}
 
 	kref_init(&hcd->kref);
@@ -2510,18 +2514,25 @@ EXPORT_SYMBOL_GPL(usb_create_hcd);
  * deallocated.
  *
  * Make sure to only deallocate the bandwidth_mutex when the primary HCD is
- * freed.  When hcd_release() is called for the non-primary HCD, set the
- * primary_hcd's shared_hcd pointer to null (since the non-primary HCD will be
- * freed shortly).
+ * freed.  When hcd_release() is called for either hcd in a peer set
+ * invalidate the peer's ->shared_hcd and ->primary_hcd pointers to
+ * block new peering attempts
  */
-static void hcd_release (struct kref *kref)
+static void hcd_release(struct kref *kref)
 {
 	struct usb_hcd *hcd = container_of (kref, struct usb_hcd, kref);
 
+	mutex_lock(&usb_port_peer_mutex);
 	if (usb_hcd_is_primary_hcd(hcd))
 		kfree(hcd->bandwidth_mutex);
-	else
-		hcd->shared_hcd->shared_hcd = NULL;
+	if (hcd->shared_hcd) {
+		struct usb_hcd *peer = hcd->shared_hcd;
+
+		peer->shared_hcd = NULL;
+		if (peer->primary_hcd == hcd)
+			peer->primary_hcd = NULL;
+	}
+	mutex_unlock(&usb_port_peer_mutex);
 	kfree(hcd);
 }
 
@@ -2589,6 +2600,21 @@ static int usb_hcd_request_irqs(struct usb_hcd *hcd,
 	return 0;
 }
 
+/*
+ * Before we free this root hub, flush in-flight peering attempts
+ * and disable peer lookups
+ */
+static void usb_put_invalidate_rhdev(struct usb_hcd *hcd)
+{
+	struct usb_device *rhdev;
+
+	mutex_lock(&usb_port_peer_mutex);
+	rhdev = hcd->self.root_hub;
+	hcd->self.root_hub = NULL;
+	mutex_unlock(&usb_port_peer_mutex);
+	usb_put_dev(rhdev);
+}
+
 /**
  * usb_add_hcd - finish generic HCD structure initialization and register
  * @hcd: the usb_hcd structure to initialize
@@ -2649,7 +2675,9 @@ int usb_add_hcd(struct usb_hcd *hcd,
 		retval = -ENOMEM;
 		goto err_allocate_root_hub;
 	}
+	mutex_lock(&usb_port_peer_mutex);
 	hcd->self.root_hub = rhdev;
+	mutex_unlock(&usb_port_peer_mutex);
 
 	switch (hcd->speed) {
 	case HCD_USB11:
@@ -2758,7 +2786,7 @@ err_hcd_driver_start:
 err_request_irq:
 err_hcd_driver_setup:
 err_set_rh_speed:
-	usb_put_dev(hcd->self.root_hub);
+	usb_put_invalidate_rhdev(hcd);
 err_allocate_root_hub:
 	usb_deregister_bus(&hcd->self);
 err_register_bus:
@@ -2838,7 +2866,6 @@ void usb_remove_hcd(struct usb_hcd *hcd)
 			free_irq(hcd->irq, hcd);
 	}
 
-	usb_put_dev(hcd->self.root_hub);
 	usb_deregister_bus(&hcd->self);
 	hcd_buffer_destroy(hcd);
 	if (hcd->remove_phy && hcd->phy) {
@@ -2846,6 +2873,8 @@ void usb_remove_hcd(struct usb_hcd *hcd)
 		usb_put_phy(hcd->phy);
 		hcd->phy = NULL;
 	}
+
+	usb_put_invalidate_rhdev(hcd);
 }
 EXPORT_SYMBOL_GPL(usb_remove_hcd);
 
diff --git a/drivers/usb/core/hub.c b/drivers/usb/core/hub.c
index 090469ebfcf..db6287025c0 100644
--- a/drivers/usb/core/hub.c
+++ b/drivers/usb/core/hub.c
@@ -36,11 +36,6 @@
 #define USB_VENDOR_GENESYS_LOGIC		0x05e3
 #define HUB_QUIRK_CHECK_PORT_AUTOSUSPEND	0x01
 
-static inline int hub_is_superspeed(struct usb_device *hdev)
-{
-	return (hdev->descriptor.bDeviceProtocol == USB_HUB_PR_SS);
-}
-
 /* Protect struct usb_device->state and ->children members
  * Note: Both are also protected by ->dev.sem, except that ->state can
  * change to USB_STATE_NOTATTACHED even when the semaphore isn't held. */
@@ -55,6 +50,9 @@ static DECLARE_WAIT_QUEUE_HEAD(khubd_wait);
 
 static struct task_struct *khubd_task;
 
+/* synchronize hub-port add/remove and peering operations */
+DEFINE_MUTEX(usb_port_peer_mutex);
+
 /* cycle leds on hubs that aren't blinking for attention */
 static bool blinkenlights = 0;
 module_param (blinkenlights, bool, S_IRUGO);
@@ -412,30 +410,35 @@ static int set_port_feature(struct usb_device *hdev, int port1, int feature)
 		NULL, 0, 1000);
 }
 
+static char *to_led_name(int selector)
+{
+	switch (selector) {
+	case HUB_LED_AMBER:
+		return "amber";
+	case HUB_LED_GREEN:
+		return "green";
+	case HUB_LED_OFF:
+		return "off";
+	case HUB_LED_AUTO:
+		return "auto";
+	default:
+		return "??";
+	}
+}
+
 /*
  * USB 2.0 spec Section 11.24.2.7.1.10 and table 11-7
  * for info about using port indicators
  */
-static void set_port_led(
-	struct usb_hub *hub,
-	int port1,
-	int selector
-)
+static void set_port_led(struct usb_hub *hub, int port1, int selector)
 {
-	int status = set_port_feature(hub->hdev, (selector << 8) | port1,
+	struct usb_port *port_dev = hub->ports[port1 - 1];
+	int status;
+
+	status = set_port_feature(hub->hdev, (selector << 8) | port1,
 			USB_PORT_FEAT_INDICATOR);
-	if (status < 0)
-		dev_dbg (hub->intfdev,
-			"port %d indicator %s status %d\n",
-			port1,
-			({ char *s; switch (selector) {
-			case HUB_LED_AMBER: s = "amber"; break;
-			case HUB_LED_GREEN: s = "green"; break;
-			case HUB_LED_OFF: s = "off"; break;
-			case HUB_LED_AUTO: s = "auto"; break;
-			default: s = "??"; break;
-			} s; }),
-			status);
+	dev_dbg(&port_dev->dev, "indicator %s status %d\n",
+		to_led_name(selector), status);
 }
 
 #define	LED_CYCLE_PERIOD	((2*HZ)/3)
@@ -743,16 +746,20 @@ int usb_hub_set_port_power(struct usb_device *hdev, struct usb_hub *hub,
 			   int port1, bool set)
 {
 	int ret;
-	struct usb_port *port_dev = hub->ports[port1 - 1];
 
 	if (set)
 		ret = set_port_feature(hdev, port1, USB_PORT_FEAT_POWER);
 	else
 		ret = usb_clear_port_feature(hdev, port1, USB_PORT_FEAT_POWER);
 
-	if (!ret)
-		port_dev->power_is_on = set;
-	return ret;
+	if (ret)
+		return ret;
+
+	if (set)
+		set_bit(port1, hub->power_bits);
+	else
+		clear_bit(port1, hub->power_bits);
+	return 0;
 }
 
 /**
@@ -810,16 +817,9 @@ int usb_hub_clear_tt_buffer(struct urb *urb)
 }
 EXPORT_SYMBOL_GPL(usb_hub_clear_tt_buffer);
 
-/* If do_delay is false, return the number of milliseconds the caller
- * needs to delay.
- */
-static unsigned hub_power_on(struct usb_hub *hub, bool do_delay)
+static void hub_power_on(struct usb_hub *hub, bool do_delay)
 {
 	int port1;
-	unsigned pgood_delay = hub->descriptor->bPwrOn2PwrGood * 2;
-	unsigned delay;
-	u16 wHubCharacteristics =
-			le16_to_cpu(hub->descriptor->wHubCharacteristics);
 
 	/* Enable power on each port.  Some hubs have reserved values
 	 * of LPSM (> 2) in their descriptors, even though they are
@@ -827,23 +827,19 @@ static unsigned hub_power_on(struct usb_hub *hub, bool do_delay)
 	 * but only emulate it.  In all cases, the ports won't work
 	 * unless we send these messages to the hub.
 	 */
-	if ((wHubCharacteristics & HUB_CHAR_LPSM) < 2)
+	if (hub_is_port_power_switchable(hub))
 		dev_dbg(hub->intfdev, "enabling power on all ports\n");
 	else
 		dev_dbg(hub->intfdev, "trying to enable port power on "
 				"non-switchable hub\n");
 	for (port1 = 1; port1 <= hub->hdev->maxchild; port1++)
-		if (hub->ports[port1 - 1]->power_is_on)
+		if (test_bit(port1, hub->power_bits))
 			set_port_feature(hub->hdev, port1, USB_PORT_FEAT_POWER);
 		else
 			usb_clear_port_feature(hub->hdev, port1,
 						USB_PORT_FEAT_POWER);
-
-	/* Wait at least 100 msec for power to become stable */
-	delay = max(pgood_delay, (unsigned) 100);
 	if (do_delay)
-		msleep(delay);
-	return delay;
+		msleep(hub_power_on_good_delay(hub));
 }
 
 static int hub_hub_status(struct usb_hub *hub,
@@ -911,20 +907,20 @@ static int hub_usb3_port_disable(struct usb_hub *hub, int port1)
 		msleep(HUB_DEBOUNCE_STEP);
 	}
 	if (total_time >= HUB_DEBOUNCE_TIMEOUT)
-		dev_warn(hub->intfdev, "Could not disable port %d after %d ms\n",
-				port1, total_time);
+		dev_warn(&hub->ports[port1 - 1]->dev,
+				"Could not disable after %d ms\n", total_time);
 
 	return hub_set_port_link_state(hub, port1, USB_SS_PORT_LS_RX_DETECT);
 }
 
 static int hub_port_disable(struct usb_hub *hub, int port1, int set_state)
 {
+	struct usb_port *port_dev = hub->ports[port1 - 1];
 	struct usb_device *hdev = hub->hdev;
 	int ret = 0;
 
-	if (hub->ports[port1 - 1]->child && set_state)
-		usb_set_device_state(hub->ports[port1 - 1]->child,
-				USB_STATE_NOTATTACHED);
+	if (port_dev->child && set_state)
+		usb_set_device_state(port_dev->child, USB_STATE_NOTATTACHED);
 	if (!hub->error) {
 		if (hub_is_superspeed(hub->hdev))
 			ret = hub_usb3_port_disable(hub, port1);
@@ -933,8 +929,7 @@ static int hub_port_disable(struct usb_hub *hub, int port1, int set_state)
 					USB_PORT_FEAT_ENABLE);
 	}
 	if (ret && ret != -ENODEV)
-		dev_err(hub->intfdev, "cannot disable port %d (err = %d)\n",
-				port1, ret);
+		dev_err(&port_dev->dev, "cannot disable (err = %d)\n", ret);
 	return ret;
 }
 
@@ -945,7 +940,7 @@ static int hub_port_disable(struct usb_hub *hub, int port1, int set_state)
  */
 static void hub_port_logical_disconnect(struct usb_hub *hub, int port1)
 {
-	dev_dbg(hub->intfdev, "logical disconnect on port %d\n", port1);
+	dev_dbg(&hub->ports[port1 - 1]->dev, "logical disconnect\n");
 	hub_port_disable(hub, port1, 1);
 
 	/* FIXME let caller ask to power down the port:
@@ -1048,7 +1043,9 @@ static void hub_activate(struct usb_hub *hub, enum hub_activation_type type)
 		 * for HUB_POST_RESET, but it's easier not to.
 		 */
 		if (type == HUB_INIT) {
-			delay = hub_power_on(hub, false);
+			unsigned delay = hub_power_on_good_delay(hub);
+
+			hub_power_on(hub, false);
 			INIT_DELAYED_WORK(&hub->init_work, hub_init_func2);
 			queue_delayed_work(system_power_efficient_wq,
 					&hub->init_work,
@@ -1083,21 +1080,23 @@ static void hub_activate(struct usb_hub *hub, enum hub_activation_type type)
 	}
  init2:
 
-	/* Check each port and set hub->change_bits to let khubd know
+	/*
+	 * Check each port and set hub->change_bits to let khubd know
 	 * which ports need attention.
 	 */
 	for (port1 = 1; port1 <= hdev->maxchild; ++port1) {
-		struct usb_device *udev = hub->ports[port1 - 1]->child;
+		struct usb_port *port_dev = hub->ports[port1 - 1];
+		struct usb_device *udev = port_dev->child;
 		u16 portstatus, portchange;
 
 		portstatus = portchange = 0;
 		status = hub_port_status(hub, port1, &portstatus, &portchange);
 		if (udev || (portstatus & USB_PORT_STAT_CONNECTION))
-			dev_dbg(hub->intfdev,
-					"port %d: status %04x change %04x\n",
-					port1, portstatus, portchange);
+			dev_dbg(&port_dev->dev, "status %04x change %04x\n",
+					portstatus, portchange);
 
-		/* After anything other than HUB_RESUME (i.e., initialization
+		/*
+		 * After anything other than HUB_RESUME (i.e., initialization
 		 * or any sort of reset), every port should be disabled.
 		 * Unconnected ports should likewise be disabled (paranoia),
 		 * and so should ports for which we have no usb_device.
@@ -1173,15 +1172,13 @@ static void hub_activate(struct usb_hub *hub, enum hub_activation_type type)
 				set_bit(port1, hub->change_bits);
 
 		} else if (udev->persist_enabled) {
-			struct usb_port *port_dev = hub->ports[port1 - 1];
-
 #ifdef CONFIG_PM
 			udev->reset_resume = 1;
 #endif
 			/* Don't set the change_bits when the device
 			 * was powered off.
 			 */
-			if (port_dev->power_is_on)
+			if (test_bit(port1, hub->power_bits))
 				set_bit(port1, hub->change_bits);
 
 		} else {
@@ -1276,12 +1273,22 @@ static void hub_quiesce(struct usb_hub *hub, enum hub_quiescing_type type)
 		flush_work(&hub->tt.clear_work);
 }
 
+static void hub_pm_barrier_for_all_ports(struct usb_hub *hub)
+{
+	int i;
+
+	for (i = 0; i < hub->hdev->maxchild; ++i)
+		pm_runtime_barrier(&hub->ports[i]->dev);
+}
+
 /* caller has locked the hub device */
 static int hub_pre_reset(struct usb_interface *intf)
 {
 	struct usb_hub *hub = usb_get_intfdata(intf);
 
 	hub_quiesce(hub, HUB_PRE_RESET);
+	hub->in_reset = 1;
+	hub_pm_barrier_for_all_ports(hub);
 	return 0;
 }
 
@@ -1290,6 +1297,8 @@ static int hub_post_reset(struct usb_interface *intf)
 {
 	struct usb_hub *hub = usb_get_intfdata(intf);
 
+	hub->in_reset = 0;
+	hub_pm_barrier_for_all_ports(hub);
 	hub_activate(hub, HUB_POST_RESET);
 	return 0;
 }
@@ -1307,6 +1316,7 @@ static int hub_configure(struct usb_hub *hub,
 	char *message = "out of memory";
 	unsigned unit_load;
 	unsigned full_load;
+	unsigned maxchild;
 
 	hub->buffer = kmalloc(sizeof(*hub->buffer), GFP_KERNEL);
 	if (!hub->buffer) {
@@ -1345,12 +1355,11 @@ static int hub_configure(struct usb_hub *hub,
 		goto fail;
 	}
 
-	hdev->maxchild = hub->descriptor->bNbrPorts;
-	dev_info (hub_dev, "%d port%s detected\n", hdev->maxchild,
-		(hdev->maxchild == 1) ? "" : "s");
+	maxchild = hub->descriptor->bNbrPorts;
+	dev_info(hub_dev, "%d port%s detected\n", maxchild,
+			(maxchild == 1) ? "" : "s");
 
-	hub->ports = kzalloc(hdev->maxchild * sizeof(struct usb_port *),
-			     GFP_KERNEL);
+	hub->ports = kzalloc(maxchild * sizeof(struct usb_port *), GFP_KERNEL);
 	if (!hub->ports) {
 		ret = -ENOMEM;
 		goto fail;
@@ -1371,11 +1380,11 @@ static int hub_configure(struct usb_hub *hub,
 		int	i;
 		char	portstr[USB_MAXCHILDREN + 1];
 
-		for (i = 0; i < hdev->maxchild; i++)
+		for (i = 0; i < maxchild; i++)
 			portstr[i] = hub->descriptor->u.hs.DeviceRemovable
 				    [((i + 1) / 8)] & (1 << ((i + 1) % 8))
 				? 'F' : 'R';
-		portstr[hdev->maxchild] = 0;
+		portstr[maxchild] = 0;
 		dev_dbg(hub_dev, "compound device; port removable status: %s\n", portstr);
 	} else
 		dev_dbg(hub_dev, "standalone hub\n");
@@ -1487,7 +1496,7 @@ static int hub_configure(struct usb_hub *hub,
 		if (hcd->power_budget > 0)
 			hdev->bus_mA = hcd->power_budget;
 		else
-			hdev->bus_mA = full_load * hdev->maxchild;
+			hdev->bus_mA = full_load * maxchild;
 		if (hdev->bus_mA >= full_load)
 			hub->mA_per_port = full_load;
 		else {
@@ -1502,7 +1511,7 @@ static int hub_configure(struct usb_hub *hub,
 			hub->descriptor->bHubContrCurrent);
 		hub->limited_power = 1;
 
-		if (remaining < hdev->maxchild * unit_load)
+		if (remaining < maxchild * unit_load)
 			dev_warn(hub_dev,
 					"insufficient power available "
 					"to use all downstream ports\n");
@@ -1570,15 +1579,19 @@ static int hub_configure(struct usb_hub *hub,
 	if (hub->has_indicators && blinkenlights)
 		hub->indicator[0] = INDICATOR_CYCLE;
 
-	for (i = 0; i < hdev->maxchild; i++) {
+	mutex_lock(&usb_port_peer_mutex);
+	for (i = 0; i < maxchild; i++) {
 		ret = usb_hub_create_port_device(hub, i + 1);
 		if (ret < 0) {
 			dev_err(hub->intfdev,
 				"couldn't create port%d device.\n", i + 1);
-			hdev->maxchild = i;
-			goto fail_keep_maxchild;
+			break;
 		}
 	}
+	hdev->maxchild = i;
+	mutex_unlock(&usb_port_peer_mutex);
+	if (ret < 0)
+		goto fail;
 
 	usb_hub_adjust_deviceremovable(hdev, hub->descriptor);
 
@@ -1586,8 +1599,6 @@ static int hub_configure(struct usb_hub *hub,
 	return 0;
 
 fail:
-	hdev->maxchild = 0;
-fail_keep_maxchild:
 	dev_err (hub_dev, "config failed, %s (err %d)\n",
 			message, ret);
 	/* hub_disconnect() frees urb and descriptor */
@@ -1623,6 +1634,8 @@ static void hub_disconnect(struct usb_interface *intf)
 	hub->error = 0;
 	hub_quiesce(hub, HUB_DISCONNECT);
 
+	mutex_lock(&usb_port_peer_mutex);
+
 	/* Avoid races with recursively_mark_NOTATTACHED() */
 	spin_lock_irq(&device_state_lock);
 	port1 = hdev->maxchild;
@@ -1633,6 +1646,8 @@ static void hub_disconnect(struct usb_interface *intf)
 	for (; port1 > 0; --port1)
 		usb_hub_remove_port_device(hub, port1);
 
+	mutex_unlock(&usb_port_peer_mutex);
+
 	if (hub->hdev->speed == USB_SPEED_HIGH)
 		highspeed_hubs--;
 
@@ -2024,6 +2039,18 @@ static void hub_free_dev(struct usb_device *udev)
 		hcd->driver->free_dev(hcd, udev);
 }
 
+static void hub_disconnect_children(struct usb_device *udev)
+{
+	struct usb_hub *hub = usb_hub_to_struct_hub(udev);
+	int i;
+
+	/* Free up all the children before we remove this device */
+	for (i = 0; i < udev->maxchild; i++) {
+		if (hub->ports[i]->child)
+			usb_disconnect(&hub->ports[i]->child);
+	}
+}
+
 /**
  * usb_disconnect - disconnect a device (usbcore-internal)
  * @pdev: pointer to device being disconnected
@@ -2042,9 +2069,10 @@ static void hub_free_dev(struct usb_device *udev)
  */
 void usb_disconnect(struct usb_device **pdev)
 {
-	struct usb_device	*udev = *pdev;
-	struct usb_hub		*hub = usb_hub_to_struct_hub(udev);
-	int			i;
+	struct usb_port *port_dev = NULL;
+	struct usb_device *udev = *pdev;
+	struct usb_hub *hub;
+	int port1;
 
 	/* mark the device as inactive, so any further urb submissions for
 	 * this device (and any of its children) will fail immediately.
@@ -2056,11 +2084,7 @@ void usb_disconnect(struct usb_device **pdev)
 
 	usb_lock_device(udev);
 
-	/* Free up all the children before we remove this device */
-	for (i = 0; i < udev->maxchild; i++) {
-		if (hub->ports[i]->child)
-			usb_disconnect(&hub->ports[i]->child);
-	}
+	hub_disconnect_children(udev);
 
 	/* deallocate hcd/hardware state ... nuking all pending urbs and
 	 * cleaning up all state associated with the current configuration
@@ -2071,16 +2095,19 @@ void usb_disconnect(struct usb_device **pdev)
 	usb_hcd_synchronize_unlinks(udev);
 
 	if (udev->parent) {
-		struct usb_hub *hub = usb_hub_to_struct_hub(udev->parent);
-		struct usb_port	*port_dev = hub->ports[udev->portnum - 1];
+		port1 = udev->portnum;
+		hub = usb_hub_to_struct_hub(udev->parent);
+		port_dev = hub->ports[port1 - 1];
 
 		sysfs_remove_link(&udev->dev.kobj, "port");
 		sysfs_remove_link(&port_dev->dev.kobj, "device");
 
-		if (!port_dev->did_runtime_put)
-			pm_runtime_put(&port_dev->dev);
-		else
-			port_dev->did_runtime_put = false;
+		/*
+		 * As usb_port_runtime_resume() de-references udev, make
+		 * sure no resumes occur during removal
+		 */
+		if (!test_and_set_bit(port1, hub->child_usage_bits))
+			pm_runtime_get_sync(&port_dev->dev);
 	}
 
 	usb_remove_ep_devs(&udev->ep0);
@@ -2102,6 +2129,9 @@ void usb_disconnect(struct usb_device **pdev)
 	*pdev = NULL;
 	spin_unlock_irq(&device_state_lock);
 
+	if (port_dev && test_and_clear_bit(port1, hub->child_usage_bits))
+		pm_runtime_put(&port_dev->dev);
+
 	hub_free_dev(udev);
 
 	put_device(&udev->dev);
@@ -2289,6 +2319,22 @@ static void set_usb_port_removable(struct usb_device *udev)
 		udev->removable = USB_DEVICE_REMOVABLE;
 	else
 		udev->removable = USB_DEVICE_FIXED;
+
+	/*
+	 * Platform firmware may have populated an alternative value for
+	 * removable.  If the parent port has a known connect_type use
+	 * that instead.
+	 */
+	switch (hub->ports[udev->portnum - 1]->connect_type) {
+	case USB_PORT_CONNECT_TYPE_HOT_PLUG:
+		udev->removable = USB_DEVICE_REMOVABLE;
+		break;
+	case USB_PORT_CONNECT_TYPE_HARD_WIRED:
+		udev->removable = USB_DEVICE_FIXED;
+		break;
+	default: /* use what was set above */
+		break;
+	}
 }
 
 /**
@@ -2358,11 +2404,7 @@ int usb_new_device(struct usb_device *udev)
 
 	device_enable_async_suspend(&udev->dev);
 
-	/*
-	 * check whether the hub marks this port as non-removable. Do it
-	 * now so that platform-specific data can override it in
-	 * device_add()
-	 */
+	/* check whether the hub or firmware marks this port as non-removable */
 	if (udev->parent)
 		set_usb_port_removable(udev);
 
@@ -2379,7 +2421,8 @@ int usb_new_device(struct usb_device *udev)
 	/* Create link files between child device and usb port device. */
 	if (udev->parent) {
 		struct usb_hub *hub = usb_hub_to_struct_hub(udev->parent);
-		struct usb_port	*port_dev = hub->ports[udev->portnum - 1];
+		int port1 = udev->portnum;
+		struct usb_port	*port_dev = hub->ports[port1 - 1];
 
 		err = sysfs_create_link(&udev->dev.kobj,
 				&port_dev->dev.kobj, "port");
@@ -2393,7 +2436,8 @@ int usb_new_device(struct usb_device *udev)
 			goto fail;
 		}
 
-		pm_runtime_get_sync(&port_dev->dev);
+		if (!test_and_set_bit(port1, hub->child_usage_bits))
+			pm_runtime_get_sync(&port_dev->dev);
 	}
 
 	(void) usb_create_ep_devs(&udev->dev, &udev->ep0, udev);
@@ -2561,9 +2605,9 @@ static int hub_port_wait_reset(struct usb_hub *hub, int port1,
 		if (delay_time >= 2 * HUB_SHORT_RESET_TIME)
 			delay = HUB_LONG_RESET_TIME;
 
-		dev_dbg (hub->intfdev,
-			"port %d not %sreset yet, waiting %dms\n",
-			port1, warm ? "warm " : "", delay);
+		dev_dbg(&hub->ports[port1 - 1]->dev,
+				"not %sreset yet, waiting %dms\n",
+				warm ? "warm " : "", delay);
 	}
 
 	if ((portstatus & USB_PORT_STAT_RESET))
@@ -2647,6 +2691,7 @@ static int hub_port_reset(struct usb_hub *hub, int port1,
 {
 	int i, status;
 	u16 portchange, portstatus;
+	struct usb_port *port_dev = hub->ports[port1 - 1];
 
 	if (!hub_is_superspeed(hub->hdev)) {
 		if (warm) {
@@ -2680,9 +2725,9 @@ static int hub_port_reset(struct usb_hub *hub, int port1,
 		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);
+			dev_err(&port_dev->dev,
+					"cannot %sreset (err = %d)\n",
+					warm ? "warm " : "", status);
 		} else {
 			status = hub_port_wait_reset(hub, port1, udev, delay,
 								warm);
@@ -2715,21 +2760,19 @@ static int hub_port_reset(struct usb_hub *hub, int port1,
 			 * hot or warm reset failed.  Try another warm reset.
 			 */
 			if (!warm) {
-				dev_dbg(hub->intfdev, "hot reset failed, warm reset port %d\n",
-						port1);
+				dev_dbg(&port_dev->dev,
+						"hot reset failed, warm reset\n");
 				warm = true;
 			}
 		}
 
-		dev_dbg (hub->intfdev,
-			"port %d not enabled, trying %sreset again...\n",
-			port1, warm ? "warm " : "");
+		dev_dbg(&port_dev->dev,
+				"not enabled, trying %sreset again...\n",
+				warm ? "warm " : "");
 		delay = HUB_LONG_RESET_TIME;
 	}
 
-	dev_err (hub->intfdev,
-		"Cannot enable port %i.  Maybe the USB cable is bad?\n",
-		port1);
+	dev_err(&port_dev->dev, "Cannot enable. Maybe the USB cable is bad?\n");
 
 done:
 	if (!hub_is_superspeed(hub->hdev))
@@ -2754,6 +2797,20 @@ static int port_is_power_on(struct usb_hub *hub, unsigned portstatus)
 	return ret;
 }
 
+static void usb_lock_port(struct usb_port *port_dev)
+		__acquires(&port_dev->status_lock)
+{
+	mutex_lock(&port_dev->status_lock);
+	__acquire(&port_dev->status_lock);
+}
+
+static void usb_unlock_port(struct usb_port *port_dev)
+		__releases(&port_dev->status_lock)
+{
+	mutex_unlock(&port_dev->status_lock);
+	__release(&port_dev->status_lock);
+}
+
 #ifdef	CONFIG_PM
 
 /* Check if a port is suspended(USB2.0 port) or in U3 state(USB3.0 port) */
@@ -2780,6 +2837,8 @@ static int check_port_resume_type(struct usb_device *udev,
 		struct usb_hub *hub, int port1,
 		int status, unsigned portchange, unsigned portstatus)
 {
+	struct usb_port *port_dev = hub->ports[port1 - 1];
+
 	/* Is the device still present? */
 	if (status || port_is_suspended(hub, portstatus) ||
 			!port_is_power_on(hub, portstatus) ||
@@ -2799,9 +2858,8 @@ static int check_port_resume_type(struct usb_device *udev,
 	}
 
 	if (status) {
-		dev_dbg(hub->intfdev,
-				"port %d status %04x.%04x after resume, %d\n",
-				port1, portchange, portstatus, status);
+		dev_dbg(&port_dev->dev, "status %04x.%04x after resume, %d\n",
+				portchange, portstatus, status);
 	} else if (udev->reset_resume) {
 
 		/* Late port handoff can set status-change bits */
@@ -2975,6 +3033,8 @@ int usb_port_suspend(struct usb_device *udev, pm_message_t msg)
 	int		status;
 	bool		really_suspend = true;
 
+	usb_lock_port(port_dev);
+
 	/* enable remote wakeup when appropriate; this lets the device
 	 * wake up the upstream hub (including maybe the root hub).
 	 *
@@ -3032,8 +3092,7 @@ int usb_port_suspend(struct usb_device *udev, pm_message_t msg)
 		status = 0;
 	}
 	if (status) {
-		dev_dbg(hub->intfdev, "can't suspend port %d, status %d\n",
-				port1, status);
+		dev_dbg(&port_dev->dev, "can't suspend, status %d\n", status);
 
 		/* Try to enable USB3 LPM and LTM again */
 		usb_unlocked_enable_lpm(udev);
@@ -3064,12 +3123,13 @@ int usb_port_suspend(struct usb_device *udev, pm_message_t msg)
 		usb_set_device_state(udev, USB_STATE_SUSPENDED);
 	}
 
-	if (status == 0 && !udev->do_remote_wakeup && udev->persist_enabled) {
+	if (status == 0 && !udev->do_remote_wakeup && udev->persist_enabled
+			&& test_and_clear_bit(port1, hub->child_usage_bits))
 		pm_runtime_put_sync(&port_dev->dev);
-		port_dev->did_runtime_put = true;
-	}
 
 	usb_mark_last_busy(hub->hdev);
+
+	usb_unlock_port(port_dev);
 	return status;
 }
 
@@ -3209,9 +3269,8 @@ int usb_port_resume(struct usb_device *udev, pm_message_t msg)
 	int		status;
 	u16		portchange, portstatus;
 
-	if (port_dev->did_runtime_put) {
+	if (!test_and_set_bit(port1, hub->child_usage_bits)) {
 		status = pm_runtime_get_sync(&port_dev->dev);
-		port_dev->did_runtime_put = false;
 		if (status < 0) {
 			dev_dbg(&udev->dev, "can't resume usb port, status %d\n",
 					status);
@@ -3219,15 +3278,13 @@ int usb_port_resume(struct usb_device *udev, pm_message_t msg)
 		}
 	}
 
+	usb_lock_port(port_dev);
+
 	/* Skip the initial Clear-Suspend step for a remote wakeup */
 	status = hub_port_status(hub, port1, &portstatus, &portchange);
 	if (status == 0 && !port_is_suspended(hub, portstatus))
 		goto SuspendCleared;
 
-	/* dev_dbg(hub->intfdev, "resume port %d\n", port1); */
-
-	set_bit(port1, hub->busy_bits);
-
 	/* see 7.1.7.7; affects power usage, but not budgeting */
 	if (hub_is_superspeed(hub->hdev))
 		status = hub_set_port_link_state(hub, port1, USB_SS_PORT_LS_U0);
@@ -3235,8 +3292,7 @@ int usb_port_resume(struct usb_device *udev, pm_message_t msg)
 		status = usb_clear_port_feature(hub->hdev,
 				port1, USB_PORT_FEAT_SUSPEND);
 	if (status) {
-		dev_dbg(hub->intfdev, "can't resume port %d, status %d\n",
-				port1, status);
+		dev_dbg(&port_dev->dev, "can't resume, status %d\n", status);
 	} else {
 		/* drive resume for at least 20 msec */
 		dev_dbg(&udev->dev, "usb %sresume\n",
@@ -3267,8 +3323,6 @@ int usb_port_resume(struct usb_device *udev, pm_message_t msg)
 		}
 	}
 
-	clear_bit(port1, hub->busy_bits);
-
 	status = check_port_resume_type(udev,
 			hub, port1, status, portchange, portstatus);
 	if (status == 0)
@@ -3286,16 +3340,18 @@ int usb_port_resume(struct usb_device *udev, pm_message_t msg)
 		usb_unlocked_enable_lpm(udev);
 	}
 
+	usb_unlock_port(port_dev);
+
 	return status;
 }
 
 #ifdef	CONFIG_PM_RUNTIME
 
-/* caller has locked udev */
 int usb_remote_wakeup(struct usb_device *udev)
 {
 	int	status = 0;
 
+	usb_lock_device(udev);
 	if (udev->state == USB_STATE_SUSPENDED) {
 		dev_dbg(&udev->dev, "usb %sresume\n", "wakeup-");
 		status = usb_autoresume_device(udev);
@@ -3304,9 +3360,59 @@ int usb_remote_wakeup(struct usb_device *udev)
 			usb_autosuspend_device(udev);
 		}
 	}
+	usb_unlock_device(udev);
 	return status;
 }
 
+/* Returns 1 if there was a remote wakeup and a connect status change. */
+static int hub_handle_remote_wakeup(struct usb_hub *hub, unsigned int port,
+		u16 portstatus, u16 portchange)
+		__must_hold(&port_dev->status_lock)
+{
+	struct usb_port *port_dev = hub->ports[port - 1];
+	struct usb_device *hdev;
+	struct usb_device *udev;
+	int connect_change = 0;
+	int ret;
+
+	hdev = hub->hdev;
+	udev = port_dev->child;
+	if (!hub_is_superspeed(hdev)) {
+		if (!(portchange & USB_PORT_STAT_C_SUSPEND))
+			return 0;
+		usb_clear_port_feature(hdev, port, USB_PORT_FEAT_C_SUSPEND);
+	} else {
+		if (!udev || udev->state != USB_STATE_SUSPENDED ||
+				 (portstatus & USB_PORT_STAT_LINK_STATE) !=
+				 USB_SS_PORT_LS_U0)
+			return 0;
+	}
+
+	if (udev) {
+		/* TRSMRCY = 10 msec */
+		msleep(10);
+
+		usb_unlock_port(port_dev);
+		ret = usb_remote_wakeup(udev);
+		usb_lock_port(port_dev);
+		if (ret < 0)
+			connect_change = 1;
+	} else {
+		ret = -ENODEV;
+		hub_port_disable(hub, port, 1);
+	}
+	dev_dbg(&port_dev->dev, "resume, status %d\n", ret);
+	return connect_change;
+}
+
+#else
+
+static int hub_handle_remote_wakeup(struct usb_hub *hub, unsigned int port,
+		u16 portstatus, u16 portchange)
+{
+	return 0;
+}
+
 #endif
 
 static int check_ports_changed(struct usb_hub *hub)
@@ -3337,12 +3443,11 @@ static int hub_suspend(struct usb_interface *intf, pm_message_t msg)
 	 */
 	hub->wakeup_enabled_descendants = 0;
 	for (port1 = 1; port1 <= hdev->maxchild; port1++) {
-		struct usb_device	*udev;
+		struct usb_port *port_dev = hub->ports[port1 - 1];
+		struct usb_device *udev = port_dev->child;
 
-		udev = hub->ports[port1 - 1]->child;
 		if (udev && udev->can_submit) {
-			dev_warn(&intf->dev, "port %d not suspended yet\n",
-					port1);
+			dev_warn(&port_dev->dev, "not suspended yet\n");
 			if (PMSG_IS_AUTO(msg))
 				return -EBUSY;
 		}
@@ -3861,6 +3966,12 @@ EXPORT_SYMBOL_GPL(usb_disable_ltm);
 void usb_enable_ltm(struct usb_device *udev) { }
 EXPORT_SYMBOL_GPL(usb_enable_ltm);
 
+static int hub_handle_remote_wakeup(struct usb_hub *hub, unsigned int port,
+		u16 portstatus, u16 portchange)
+{
+	return 0;
+}
+
 #endif	/* CONFIG_PM */
 
 
@@ -3882,9 +3993,10 @@ EXPORT_SYMBOL_GPL(usb_enable_ltm);
 int hub_port_debounce(struct usb_hub *hub, int port1, bool must_be_connected)
 {
 	int ret;
-	int total_time, stable_time = 0;
 	u16 portchange, portstatus;
 	unsigned connection = 0xffff;
+	int total_time, stable_time = 0;
+	struct usb_port *port_dev = hub->ports[port1 - 1];
 
 	for (total_time = 0; ; total_time += HUB_DEBOUNCE_STEP) {
 		ret = hub_port_status(hub, port1, &portstatus, &portchange);
@@ -3913,9 +4025,8 @@ int hub_port_debounce(struct usb_hub *hub, int port1, bool must_be_connected)
 		msleep(HUB_DEBOUNCE_STEP);
 	}
 
-	dev_dbg (hub->intfdev,
-		"debounce: port %d: total %dms stable %dms status 0x%x\n",
-		port1, total_time, stable_time, portstatus);
+	dev_dbg(&port_dev->dev, "debounce total %dms stable %dms status 0x%x\n",
+			total_time, stable_time, portstatus);
 
 	if (stable_time < HUB_DEBOUNCE_STABLE)
 		return -ETIMEDOUT;
@@ -3974,13 +4085,14 @@ static int hub_set_address(struct usb_device *udev, int devnum)
  */
 static void hub_set_initial_usb2_lpm_policy(struct usb_device *udev)
 {
-	int connect_type;
+	struct usb_hub *hub = usb_hub_to_struct_hub(udev->parent);
+	int connect_type = USB_PORT_CONNECT_TYPE_UNKNOWN;
 
 	if (!udev->usb2_hw_lpm_capable)
 		return;
 
-	connect_type = usb_get_hub_port_connect_type(udev->parent,
-			udev->portnum);
+	if (hub)
+		connect_type = hub->ports[udev->portnum - 1]->connect_type;
 
 	if ((udev->bos->ext_cap->bmAttributes & cpu_to_le32(USB_BESL_SUPPORT)) ||
 			connect_type == USB_PORT_CONNECT_TYPE_HARD_WIRED) {
@@ -4008,16 +4120,15 @@ static int hub_enable_device(struct usb_device *udev)
  * Returns device in USB_STATE_ADDRESS, except on error.
  *
  * If this is called for an already-existing device (as part of
- * usb_reset_and_verify_device), the caller must own the device lock.  For a
- * newly detected device that is not accessible through any global
- * pointers, it's not necessary to lock the device.
+ * usb_reset_and_verify_device), the caller must own the device lock and
+ * the port lock.  For a newly detected device that is not accessible
+ * through any global pointers, it's not necessary to lock the device,
+ * but it is still necessary to lock the port.
  */
 static int
 hub_port_init (struct usb_hub *hub, struct usb_device *udev, int port1,
 		int retry_counter)
 {
-	static DEFINE_MUTEX(usb_address0_mutex);
-
 	struct usb_device	*hdev = hub->hdev;
 	struct usb_hcd		*hcd = bus_to_hcd(hdev->bus);
 	int			i, j, retval;
@@ -4040,7 +4151,7 @@ hub_port_init (struct usb_hub *hub, struct usb_device *udev, int port1,
 	if (oldspeed == USB_SPEED_LOW)
 		delay = HUB_LONG_RESET_TIME;
 
-	mutex_lock(&usb_address0_mutex);
+	mutex_lock(&hdev->bus->usb_address0_mutex);
 
 	/* Reset the device; full speed may morph to high speed */
 	/* FIXME a USB 2.0 device may morph into SuperSpeed on reset. */
@@ -4317,7 +4428,7 @@ fail:
 		hub_port_disable(hub, port1, 0);
 		update_devnum(udev, devnum);	/* for disconnect processing */
 	}
-	mutex_unlock(&usb_address0_mutex);
+	mutex_unlock(&hdev->bus->usb_address0_mutex);
 	return retval;
 }
 
@@ -4358,9 +4469,10 @@ hub_power_remaining (struct usb_hub *hub)
 
 	remaining = hdev->bus_mA - hub->descriptor->bHubContrCurrent;
 	for (port1 = 1; port1 <= hdev->maxchild; ++port1) {
-		struct usb_device	*udev = hub->ports[port1 - 1]->child;
-		int			delta;
-		unsigned		unit_load;
+		struct usb_port *port_dev = hub->ports[port1 - 1];
+		struct usb_device *udev = port_dev->child;
+		unsigned unit_load;
+		int delta;
 
 		if (!udev)
 			continue;
@@ -4380,9 +4492,8 @@ hub_power_remaining (struct usb_hub *hub)
 		else
 			delta = 8;
 		if (delta > hub->mA_per_port)
-			dev_warn(&udev->dev,
-				 "%dmA is over %umA budget for port %d!\n",
-				 delta, hub->mA_per_port, port1);
+			dev_warn(&port_dev->dev, "%dmA is over %umA budget!\n",
+					delta, hub->mA_per_port);
 		remaining -= delta;
 	}
 	if (remaining < 0) {
@@ -4393,78 +4504,23 @@ hub_power_remaining (struct usb_hub *hub)
 	return remaining;
 }
 
-/* Handle physical or logical connection change events.
- * This routine is called when:
- * 	a port connection-change occurs;
- *	a port enable-change occurs (often caused by EMI);
- *	usb_reset_and_verify_device() encounters changed descriptors (as from
- *		a firmware download)
- * caller already locked the hub
- */
-static void hub_port_connect_change(struct usb_hub *hub, int port1,
-					u16 portstatus, u16 portchange)
+static void hub_port_connect(struct usb_hub *hub, int port1, u16 portstatus,
+		u16 portchange)
 {
-	struct usb_device *hdev = hub->hdev;
-	struct device *hub_dev = hub->intfdev;
-	struct usb_hcd *hcd = bus_to_hcd(hdev->bus);
-	unsigned wHubCharacteristics =
-			le16_to_cpu(hub->descriptor->wHubCharacteristics);
-	struct usb_device *udev;
 	int status, i;
 	unsigned unit_load;
-
-	dev_dbg (hub_dev,
-		"port %d, status %04x, change %04x, %s\n",
-		port1, portstatus, portchange, portspeed(hub, portstatus));
-
-	if (hub->has_indicators) {
-		set_port_led(hub, port1, HUB_LED_AUTO);
-		hub->indicator[port1-1] = INDICATOR_AUTO;
-	}
-
-#ifdef	CONFIG_USB_OTG
-	/* during HNP, don't repeat the debounce */
-	if (hdev->bus->is_b_host)
-		portchange &= ~(USB_PORT_STAT_C_CONNECTION |
-				USB_PORT_STAT_C_ENABLE);
-#endif
-
-	/* Try to resuscitate an existing device */
-	udev = hub->ports[port1 - 1]->child;
-	if ((portstatus & USB_PORT_STAT_CONNECTION) && udev &&
-			udev->state != USB_STATE_NOTATTACHED) {
-		usb_lock_device(udev);
-		if (portstatus & USB_PORT_STAT_ENABLE) {
-			status = 0;		/* Nothing to do */
-
-#ifdef CONFIG_PM_RUNTIME
-		} else if (udev->state == USB_STATE_SUSPENDED &&
-				udev->persist_enabled) {
-			/* For a suspended device, treat this as a
-			 * remote wakeup event.
-			 */
-			status = usb_remote_wakeup(udev);
-#endif
-
-		} else {
-			status = -ENODEV;	/* Don't resuscitate */
-		}
-		usb_unlock_device(udev);
-
-		if (status == 0) {
-			clear_bit(port1, hub->change_bits);
-			return;
-		}
-	}
+	struct usb_device *hdev = hub->hdev;
+	struct usb_hcd *hcd = bus_to_hcd(hdev->bus);
+	struct usb_port *port_dev = hub->ports[port1 - 1];
+	struct usb_device *udev = port_dev->child;
 
 	/* Disconnect any existing devices under this port */
 	if (udev) {
 		if (hcd->phy && !hdev->parent &&
 				!(portstatus & USB_PORT_STAT_CONNECTION))
 			usb_phy_notify_disconnect(hcd->phy, udev->speed);
-		usb_disconnect(&hub->ports[port1 - 1]->child);
+		usb_disconnect(&port_dev->child);
 	}
-	clear_bit(port1, hub->change_bits);
 
 	/* We can forget about a "removed" device when there's a physical
 	 * disconnect or the connect status changes.
@@ -4478,8 +4534,8 @@ static void hub_port_connect_change(struct usb_hub *hub, int port1,
 		status = hub_port_debounce_be_stable(hub, port1);
 		if (status < 0) {
 			if (status != -ENODEV && printk_ratelimit())
-				dev_err(hub_dev, "connect-debounce failed, "
-						"port %d disabled\n", port1);
+				dev_err(&port_dev->dev,
+						"connect-debounce failed\n");
 			portstatus &= ~USB_PORT_STAT_CONNECTION;
 		} else {
 			portstatus = status;
@@ -4493,7 +4549,7 @@ static void hub_port_connect_change(struct usb_hub *hub, int port1,
 			test_bit(port1, hub->removed_bits)) {
 
 		/* maybe switch power back on (e.g. root hub was reset) */
-		if ((wHubCharacteristics & HUB_CHAR_LPSM) < 2
+		if (hub_is_port_power_switchable(hub)
 				&& !port_is_power_on(hub, portstatus))
 			set_port_feature(hdev, port1, USB_PORT_FEAT_POWER);
 
@@ -4514,9 +4570,8 @@ static void hub_port_connect_change(struct usb_hub *hub, int port1,
 		 */
 		udev = usb_alloc_dev(hdev, hdev->bus, port1);
 		if (!udev) {
-			dev_err (hub_dev,
-				"couldn't allocate port %d usb_device\n",
-				port1);
+			dev_err(&port_dev->dev,
+					"couldn't allocate usb_device\n");
 			goto done;
 		}
 
@@ -4538,7 +4593,9 @@ static void hub_port_connect_change(struct usb_hub *hub, int port1,
 		}
 
 		/* reset (non-USB 3.0 devices) and get descriptor */
+		usb_lock_port(port_dev);
 		status = hub_port_init(hub, udev, port1, i);
+		usb_unlock_port(port_dev);
 		if (status < 0)
 			goto loop;
 
@@ -4590,6 +4647,8 @@ static void hub_port_connect_change(struct usb_hub *hub, int port1,
 		 */
 		status = 0;
 
+		mutex_lock(&usb_port_peer_mutex);
+
 		/* We mustn't add new devices if the parent hub has
 		 * been disconnected; we would race with the
 		 * recursively_mark_NOTATTACHED() routine.
@@ -4598,16 +4657,19 @@ static void hub_port_connect_change(struct usb_hub *hub, int port1,
 		if (hdev->state == USB_STATE_NOTATTACHED)
 			status = -ENOTCONN;
 		else
-			hub->ports[port1 - 1]->child = udev;
+			port_dev->child = udev;
 		spin_unlock_irq(&device_state_lock);
+		mutex_unlock(&usb_port_peer_mutex);
 
 		/* Run it through the hoops (find a driver, etc) */
 		if (!status) {
 			status = usb_new_device(udev);
 			if (status) {
+				mutex_lock(&usb_port_peer_mutex);
 				spin_lock_irq(&device_state_lock);
-				hub->ports[port1 - 1]->child = NULL;
+				port_dev->child = NULL;
 				spin_unlock_irq(&device_state_lock);
+				mutex_unlock(&usb_port_peer_mutex);
 			}
 		}
 
@@ -4616,7 +4678,7 @@ static void hub_port_connect_change(struct usb_hub *hub, int port1,
 
 		status = hub_power_remaining(hub);
 		if (status)
-			dev_dbg(hub_dev, "%dmA power budget left\n", status);
+			dev_dbg(hub->intfdev, "%dmA power budget left\n", status);
 
 		return;
 
@@ -4634,56 +4696,200 @@ loop:
 			!hcd->driver->port_handed_over ||
 			!(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);
+			dev_err(&port_dev->dev,
+					"unable to enumerate USB device\n");
 	}
 
 done:
 	hub_port_disable(hub, port1, 1);
 	if (hcd->driver->relinquish_port && !hub->hdev->parent)
 		hcd->driver->relinquish_port(hcd, port1);
+
 }
 
-/* Returns 1 if there was a remote wakeup and a connect status change. */
-static int hub_handle_remote_wakeup(struct usb_hub *hub, unsigned int port,
-		u16 portstatus, u16 portchange)
+/* Handle physical or logical connection change events.
+ * This routine is called when:
+ *	a port connection-change occurs;
+ *	a port enable-change occurs (often caused by EMI);
+ *	usb_reset_and_verify_device() encounters changed descriptors (as from
+ *		a firmware download)
+ * caller already locked the hub
+ */
+static void hub_port_connect_change(struct usb_hub *hub, int port1,
+					u16 portstatus, u16 portchange)
+		__must_hold(&port_dev->status_lock)
 {
-	struct usb_device *hdev;
-	struct usb_device *udev;
-	int connect_change = 0;
-	int ret;
+	struct usb_port *port_dev = hub->ports[port1 - 1];
+	struct usb_device *udev = port_dev->child;
+	int status = -ENODEV;
 
-	hdev = hub->hdev;
-	udev = hub->ports[port - 1]->child;
-	if (!hub_is_superspeed(hdev)) {
-		if (!(portchange & USB_PORT_STAT_C_SUSPEND))
-			return 0;
-		usb_clear_port_feature(hdev, port, USB_PORT_FEAT_C_SUSPEND);
-	} else {
-		if (!udev || udev->state != USB_STATE_SUSPENDED ||
-				 (portstatus & USB_PORT_STAT_LINK_STATE) !=
-				 USB_SS_PORT_LS_U0)
-			return 0;
+	dev_dbg(&port_dev->dev, "status %04x, change %04x, %s\n", portstatus,
+			portchange, portspeed(hub, portstatus));
+
+	if (hub->has_indicators) {
+		set_port_led(hub, port1, HUB_LED_AUTO);
+		hub->indicator[port1-1] = INDICATOR_AUTO;
 	}
 
-	if (udev) {
-		/* TRSMRCY = 10 msec */
-		msleep(10);
+#ifdef	CONFIG_USB_OTG
+	/* during HNP, don't repeat the debounce */
+	if (hub->hdev->bus->is_b_host)
+		portchange &= ~(USB_PORT_STAT_C_CONNECTION |
+				USB_PORT_STAT_C_ENABLE);
+#endif
+
+	/* Try to resuscitate an existing device */
+	if ((portstatus & USB_PORT_STAT_CONNECTION) && udev &&
+			udev->state != USB_STATE_NOTATTACHED) {
+		if (portstatus & USB_PORT_STAT_ENABLE) {
+			status = 0;		/* Nothing to do */
+#ifdef CONFIG_PM_RUNTIME
+		} else if (udev->state == USB_STATE_SUSPENDED &&
+				udev->persist_enabled) {
+			/* For a suspended device, treat this as a
+			 * remote wakeup event.
+			 */
+			usb_unlock_port(port_dev);
+			status = usb_remote_wakeup(udev);
+			usb_lock_port(port_dev);
+#endif
+		} else {
+			/* Don't resuscitate */;
+		}
+	}
+	clear_bit(port1, hub->change_bits);
+
+	/* successfully revalidated the connection */
+	if (status == 0)
+		return;
+
+	usb_unlock_port(port_dev);
+	hub_port_connect(hub, port1, portstatus, portchange);
+	usb_lock_port(port_dev);
+}
+
+static void port_event(struct usb_hub *hub, int port1)
+		__must_hold(&port_dev->status_lock)
+{
+	int connect_change, reset_device = 0;
+	struct usb_port *port_dev = hub->ports[port1 - 1];
+	struct usb_device *udev = port_dev->child;
+	struct usb_device *hdev = hub->hdev;
+	u16 portstatus, portchange;
+
+	connect_change = test_bit(port1, hub->change_bits);
+	clear_bit(port1, hub->event_bits);
+	clear_bit(port1, hub->wakeup_bits);
+
+	if (hub_port_status(hub, port1, &portstatus, &portchange) < 0)
+		return;
+
+	if (portchange & USB_PORT_STAT_C_CONNECTION) {
+		usb_clear_port_feature(hdev, port1, USB_PORT_FEAT_C_CONNECTION);
+		connect_change = 1;
+	}
+
+	if (portchange & USB_PORT_STAT_C_ENABLE) {
+		if (!connect_change)
+			dev_dbg(&port_dev->dev, "enable change, status %08x\n",
+					portstatus);
+		usb_clear_port_feature(hdev, port1, USB_PORT_FEAT_C_ENABLE);
+
+		/*
+		 * EM interference sometimes causes badly shielded USB devices
+		 * to be shutdown by the hub, this hack enables them again.
+		 * Works at least with mouse driver.
+		 */
+		if (!(portstatus & USB_PORT_STAT_ENABLE)
+		    && !connect_change && udev) {
+			dev_err(&port_dev->dev, "disabled by hub (EMI?), re-enabling...\n");
+			connect_change = 1;
+		}
+	}
+
+	if (portchange & USB_PORT_STAT_C_OVERCURRENT) {
+		u16 status = 0, unused;
+
+		dev_dbg(&port_dev->dev, "over-current change\n");
+		usb_clear_port_feature(hdev, port1,
+				USB_PORT_FEAT_C_OVER_CURRENT);
+		msleep(100);	/* Cool down */
+		hub_power_on(hub, true);
+		hub_port_status(hub, port1, &status, &unused);
+		if (status & USB_PORT_STAT_OVERCURRENT)
+			dev_err(&port_dev->dev, "over-current condition\n");
+	}
+
+	if (portchange & USB_PORT_STAT_C_RESET) {
+		dev_dbg(&port_dev->dev, "reset change\n");
+		usb_clear_port_feature(hdev, port1, USB_PORT_FEAT_C_RESET);
+	}
+	if ((portchange & USB_PORT_STAT_C_BH_RESET)
+	    && hub_is_superspeed(hdev)) {
+		dev_dbg(&port_dev->dev, "warm reset change\n");
+		usb_clear_port_feature(hdev, port1,
+				USB_PORT_FEAT_C_BH_PORT_RESET);
+	}
+	if (portchange & USB_PORT_STAT_C_LINK_STATE) {
+		dev_dbg(&port_dev->dev, "link state change\n");
+		usb_clear_port_feature(hdev, port1,
+				USB_PORT_FEAT_C_PORT_LINK_STATE);
+	}
+	if (portchange & USB_PORT_STAT_C_CONFIG_ERROR) {
+		dev_warn(&port_dev->dev, "config error\n");
+		usb_clear_port_feature(hdev, port1,
+				USB_PORT_FEAT_C_PORT_CONFIG_ERROR);
+	}
+
+	/* skip port actions that require the port to be powered on */
+	if (!pm_runtime_active(&port_dev->dev))
+		return;
 
+	if (hub_handle_remote_wakeup(hub, port1, portstatus, portchange))
+		connect_change = 1;
+
+	/*
+	 * Warm reset a USB3 protocol port if it's in
+	 * SS.Inactive state.
+	 */
+	if (hub_port_warm_reset_required(hub, portstatus)) {
+		dev_dbg(&port_dev->dev, "do warm reset\n");
+		if (!udev || !(portstatus & USB_PORT_STAT_CONNECTION)
+				|| udev->state == USB_STATE_NOTATTACHED) {
+			if (hub_port_reset(hub, port1, NULL,
+					HUB_BH_RESET_TIME, true) < 0)
+				hub_port_disable(hub, port1, 1);
+		} else
+			reset_device = 1;
+	}
+
+	/*
+	 * On disconnect USB3 protocol ports transit from U0 to
+	 * SS.Inactive to Rx.Detect. If this happens a warm-
+	 * reset is not needed, but a (re)connect may happen
+	 * before khubd runs and sees the disconnect, and the
+	 * device may be an unknown state.
+	 *
+	 * If the port went through SS.Inactive without khubd
+	 * seeing it the C_LINK_STATE change flag will be set,
+	 * and we reset the dev to put it in a known state.
+	 */
+	if (reset_device || (udev && hub_is_superspeed(hub->hdev)
+				&& (portchange & USB_PORT_STAT_C_LINK_STATE)
+				&& (portstatus & USB_PORT_STAT_CONNECTION))) {
+		usb_unlock_port(port_dev);
 		usb_lock_device(udev);
-		ret = usb_remote_wakeup(udev);
+		usb_reset_device(udev);
 		usb_unlock_device(udev);
-		if (ret < 0)
-			connect_change = 1;
-	} else {
-		ret = -ENODEV;
-		hub_port_disable(hub, port, 1);
+		usb_lock_port(port_dev);
+		connect_change = 0;
 	}
-	dev_dbg(hub->intfdev, "resume on port %d, status %d\n",
-			port, ret);
-	return connect_change;
+
+	if (connect_change)
+		hub_port_connect_change(hub, port1, portstatus, portchange);
 }
 
+
 static void hub_events(void)
 {
 	struct list_head *tmp;
@@ -4693,10 +4899,7 @@ static void hub_events(void)
 	struct device *hub_dev;
 	u16 hubstatus;
 	u16 hubchange;
-	u16 portstatus;
-	u16 portchange;
 	int i, ret;
-	int connect_change, wakeup_change;
 
 	/*
 	 *  We restart the list every time to avoid a deadlock with
@@ -4770,146 +4973,28 @@ static void hub_events(void)
 
 		/* deal with port status changes */
 		for (i = 1; i <= hdev->maxchild; i++) {
-			struct usb_device *udev = hub->ports[i - 1]->child;
-
-			if (test_bit(i, hub->busy_bits))
-				continue;
-			connect_change = test_bit(i, hub->change_bits);
-			wakeup_change = test_and_clear_bit(i, hub->wakeup_bits);
-			if (!test_and_clear_bit(i, hub->event_bits) &&
-					!connect_change && !wakeup_change)
-				continue;
-
-			ret = hub_port_status(hub, i,
-					&portstatus, &portchange);
-			if (ret < 0)
-				continue;
-
-			if (portchange & USB_PORT_STAT_C_CONNECTION) {
-				usb_clear_port_feature(hdev, i,
-					USB_PORT_FEAT_C_CONNECTION);
-				connect_change = 1;
-			}
-
-			if (portchange & USB_PORT_STAT_C_ENABLE) {
-				if (!connect_change)
-					dev_dbg (hub_dev,
-						"port %d enable change, "
-						"status %08x\n",
-						i, portstatus);
-				usb_clear_port_feature(hdev, i,
-					USB_PORT_FEAT_C_ENABLE);
+			struct usb_port *port_dev = hub->ports[i - 1];
 
+			if (test_bit(i, hub->event_bits)
+					|| test_bit(i, hub->change_bits)
+					|| test_bit(i, hub->wakeup_bits)) {
 				/*
-				 * EM interference sometimes causes badly
-				 * shielded USB devices to be shutdown by
-				 * the hub, this hack enables them again.
-				 * Works at least with mouse driver.
+				 * The get_noresume and barrier ensure that if
+				 * the port was in the process of resuming, we
+				 * flush that work and keep the port active for
+				 * the duration of the port_event().  However,
+				 * if the port is runtime pm suspended
+				 * (powered-off), we leave it in that state, run
+				 * an abbreviated port_event(), and move on.
 				 */
-				if (!(portstatus & USB_PORT_STAT_ENABLE)
-				    && !connect_change
-				    && hub->ports[i - 1]->child) {
-					dev_err (hub_dev,
-					    "port %i "
-					    "disabled by hub (EMI?), "
-					    "re-enabling...\n",
-						i);
-					connect_change = 1;
-				}
-			}
-
-			if (hub_handle_remote_wakeup(hub, i,
-						portstatus, portchange))
-				connect_change = 1;
-
-			if (portchange & USB_PORT_STAT_C_OVERCURRENT) {
-				u16 status = 0;
-				u16 unused;
-
-				dev_dbg(hub_dev, "over-current change on port "
-					"%d\n", i);
-				usb_clear_port_feature(hdev, i,
-					USB_PORT_FEAT_C_OVER_CURRENT);
-				msleep(100);	/* Cool down */
-				hub_power_on(hub, true);
-				hub_port_status(hub, i, &status, &unused);
-				if (status & USB_PORT_STAT_OVERCURRENT)
-					dev_err(hub_dev, "over-current "
-						"condition on port %d\n", i);
-			}
-
-			if (portchange & USB_PORT_STAT_C_RESET) {
-				dev_dbg (hub_dev,
-					"reset change on port %d\n",
-					i);
-				usb_clear_port_feature(hdev, i,
-					USB_PORT_FEAT_C_RESET);
-			}
-			if ((portchange & USB_PORT_STAT_C_BH_RESET) &&
-					hub_is_superspeed(hub->hdev)) {
-				dev_dbg(hub_dev,
-					"warm reset change on port %d\n",
-					i);
-				usb_clear_port_feature(hdev, i,
-					USB_PORT_FEAT_C_BH_PORT_RESET);
-			}
-			if (portchange & USB_PORT_STAT_C_LINK_STATE) {
-				usb_clear_port_feature(hub->hdev, i,
-						USB_PORT_FEAT_C_PORT_LINK_STATE);
-			}
-			if (portchange & USB_PORT_STAT_C_CONFIG_ERROR) {
-				dev_warn(hub_dev,
-					"config error on port %d\n",
-					i);
-				usb_clear_port_feature(hub->hdev, i,
-						USB_PORT_FEAT_C_PORT_CONFIG_ERROR);
+				pm_runtime_get_noresume(&port_dev->dev);
+				pm_runtime_barrier(&port_dev->dev);
+				usb_lock_port(port_dev);
+				port_event(hub, i);
+				usb_unlock_port(port_dev);
+				pm_runtime_put_sync(&port_dev->dev);
 			}
-
-			/* Warm reset a USB3 protocol port if it's in
-			 * SS.Inactive state.
-			 */
-			if (hub_port_warm_reset_required(hub, portstatus)) {
-				int status;
-
-				dev_dbg(hub_dev, "warm reset port %d\n", i);
-				if (!udev ||
-				    !(portstatus & USB_PORT_STAT_CONNECTION) ||
-				    udev->state == USB_STATE_NOTATTACHED) {
-					status = hub_port_reset(hub, i,
-							NULL, HUB_BH_RESET_TIME,
-							true);
-					if (status < 0)
-						hub_port_disable(hub, i, 1);
-				} else {
-					usb_lock_device(udev);
-					status = usb_reset_device(udev);
-					usb_unlock_device(udev);
-					connect_change = 0;
-				}
-			/*
-			 * On disconnect USB3 protocol ports transit from U0 to
-			 * SS.Inactive to Rx.Detect. If this happens a warm-
-			 * reset is not needed, but a (re)connect may happen
-			 * before khubd runs and sees the disconnect, and the
-			 * device may be an unknown state.
-			 *
-			 * If the port went through SS.Inactive without khubd
-			 * seeing it the C_LINK_STATE change flag will be set,
-			 * and we reset the dev to put it in a known state.
-			 */
-			} else if (udev && hub_is_superspeed(hub->hdev) &&
-				   (portchange & USB_PORT_STAT_C_LINK_STATE) &&
-				   (portstatus & USB_PORT_STAT_CONNECTION)) {
-				usb_lock_device(udev);
-				usb_reset_device(udev);
-				usb_unlock_device(udev);
-				connect_change = 0;
-			}
-
-			if (connect_change)
-				hub_port_connect_change(hub, i,
-						portstatus, portchange);
-		} /* end for i */
+		}
 
 		/* deal with hub status changes */
 		if (test_and_clear_bit(0, hub->event_bits) == 0)
@@ -5144,15 +5229,18 @@ static int descriptors_changed(struct usb_device *udev,
  * if the reset wasn't even attempted.
  *
  * Note:
- * The caller must own the device lock.  For example, it's safe to use
- * this from a driver probe() routine after downloading new firmware.
- * For calls that might not occur during probe(), drivers should lock
- * the device using usb_lock_device_for_reset().
+ * The caller must own the device lock and the port lock, the latter is
+ * taken by usb_reset_device().  For example, it's safe to use
+ * usb_reset_device() from a driver probe() routine after downloading
+ * new firmware.  For calls that might not occur during probe(), drivers
+ * should lock the device using usb_lock_device_for_reset().
  *
  * Locking exception: This routine may also be called from within an
  * autoresume handler.  Such usage won't conflict with other tasks
  * holding the device lock because these tasks should always call
- * usb_autopm_resume_device(), thereby preventing any unwanted autoresume.
+ * usb_autopm_resume_device(), thereby preventing any unwanted
+ * autoresume.  The autoresume handler is expected to have already
+ * acquired the port lock before calling this routine.
  */
 static int usb_reset_and_verify_device(struct usb_device *udev)
 {
@@ -5171,11 +5259,9 @@ static int usb_reset_and_verify_device(struct usb_device *udev)
 		return -EINVAL;
 	}
 
-	if (!parent_hdev) {
-		/* this requires hcd-specific logic; see ohci_restart() */
-		dev_dbg(&udev->dev, "%s for root hub!\n", __func__);
+	if (!parent_hdev)
 		return -EISDIR;
-	}
+
 	parent_hub = usb_hub_to_struct_hub(parent_hdev);
 
 	/* Disable USB2 hardware LPM.
@@ -5204,7 +5290,6 @@ static int usb_reset_and_verify_device(struct usb_device *udev)
 		goto re_enumerate;
 	}
 
-	set_bit(port1, parent_hub->busy_bits);
 	for (i = 0; i < SET_CONFIG_TRIES; ++i) {
 
 		/* ep0 maxpacket size may change; let the HCD know about it.
@@ -5214,7 +5299,6 @@ static int usb_reset_and_verify_device(struct usb_device *udev)
 		if (ret >= 0 || ret == -ENOTCONN || ret == -ENODEV)
 			break;
 	}
-	clear_bit(port1, parent_hub->busy_bits);
 
 	if (ret < 0)
 		goto re_enumerate;
@@ -5335,7 +5419,9 @@ int usb_reset_device(struct usb_device *udev)
 	int ret;
 	int i;
 	unsigned int noio_flag;
+	struct usb_port *port_dev;
 	struct usb_host_config *config = udev->actconfig;
+	struct usb_hub *hub = usb_hub_to_struct_hub(udev->parent);
 
 	if (udev->state == USB_STATE_NOTATTACHED ||
 			udev->state == USB_STATE_SUSPENDED) {
@@ -5344,6 +5430,14 @@ int usb_reset_device(struct usb_device *udev)
 		return -EINVAL;
 	}
 
+	if (!udev->parent) {
+		/* this requires hcd-specific logic; see ohci_restart() */
+		dev_dbg(&udev->dev, "%s for root hub!\n", __func__);
+		return -EISDIR;
+	}
+
+	port_dev = hub->ports[udev->portnum - 1];
+
 	/*
 	 * Don't allocate memory with GFP_KERNEL in current
 	 * context to avoid possible deadlock if usb mass
@@ -5377,7 +5471,9 @@ int usb_reset_device(struct usb_device *udev)
 		}
 	}
 
+	usb_lock_port(port_dev);
 	ret = usb_reset_and_verify_device(udev);
+	usb_unlock_port(port_dev);
 
 	if (config) {
 		for (i = config->desc.bNumInterfaces - 1; i >= 0; --i) {
@@ -5472,56 +5568,26 @@ struct usb_device *usb_hub_find_child(struct usb_device *hdev,
 }
 EXPORT_SYMBOL_GPL(usb_hub_find_child);
 
-/**
- * usb_set_hub_port_connect_type - set hub port connect type.
- * @hdev: USB device belonging to the usb hub
- * @port1: port num of the port
- * @type: connect type of the port
- */
-void usb_set_hub_port_connect_type(struct usb_device *hdev, int port1,
-	enum usb_port_connect_type type)
-{
-	struct usb_hub *hub = usb_hub_to_struct_hub(hdev);
-
-	if (hub)
-		hub->ports[port1 - 1]->connect_type = type;
-}
-
-/**
- * usb_get_hub_port_connect_type - Get the port's connect type
- * @hdev: USB device belonging to the usb hub
- * @port1: port num of the port
- *
- * Return: The connect type of the port if successful. Or
- * USB_PORT_CONNECT_TYPE_UNKNOWN if input params are invalid.
- */
-enum usb_port_connect_type
-usb_get_hub_port_connect_type(struct usb_device *hdev, int port1)
-{
-	struct usb_hub *hub = usb_hub_to_struct_hub(hdev);
-
-	if (!hub)
-		return USB_PORT_CONNECT_TYPE_UNKNOWN;
-
-	return hub->ports[port1 - 1]->connect_type;
-}
-
 void usb_hub_adjust_deviceremovable(struct usb_device *hdev,
 		struct usb_hub_descriptor *desc)
 {
+	struct usb_hub *hub = usb_hub_to_struct_hub(hdev);
 	enum usb_port_connect_type connect_type;
 	int i;
 
+	if (!hub)
+		return;
+
 	if (!hub_is_superspeed(hdev)) {
 		for (i = 1; i <= hdev->maxchild; i++) {
-			connect_type = usb_get_hub_port_connect_type(hdev, i);
+			struct usb_port *port_dev = hub->ports[i - 1];
 
+			connect_type = port_dev->connect_type;
 			if (connect_type == USB_PORT_CONNECT_TYPE_HARD_WIRED) {
 				u8 mask = 1 << (i%8);
 
 				if (!(desc->u.hs.DeviceRemovable[i/8] & mask)) {
-					dev_dbg(&hdev->dev, "usb port%d's DeviceRemovable is changed to 1 according to platform information.\n",
-						i);
+					dev_dbg(&port_dev->dev, "DeviceRemovable is changed to 1 according to platform information.\n");
 					desc->u.hs.DeviceRemovable[i/8]	|= mask;
 				}
 			}
@@ -5530,14 +5596,14 @@ void usb_hub_adjust_deviceremovable(struct usb_device *hdev,
 		u16 port_removable = le16_to_cpu(desc->u.ss.DeviceRemovable);
 
 		for (i = 1; i <= hdev->maxchild; i++) {
-			connect_type = usb_get_hub_port_connect_type(hdev, i);
+			struct usb_port *port_dev = hub->ports[i - 1];
 
+			connect_type = port_dev->connect_type;
 			if (connect_type == USB_PORT_CONNECT_TYPE_HARD_WIRED) {
 				u16 mask = 1 << i;
 
 				if (!(port_removable & mask)) {
-					dev_dbg(&hdev->dev, "usb port%d's DeviceRemovable is changed to 1 according to platform information.\n",
-						i);
+					dev_dbg(&port_dev->dev, "DeviceRemovable is changed to 1 according to platform information.\n");
 					port_removable |= mask;
 				}
 			}
diff --git a/drivers/usb/core/hub.h b/drivers/usb/core/hub.h
index 33bcb2c6f90..0a7cdc0ef0a 100644
--- a/drivers/usb/core/hub.h
+++ b/drivers/usb/core/hub.h
@@ -45,12 +45,13 @@ struct usb_hub {
 	unsigned long		event_bits[1];	/* status change bitmask */
 	unsigned long		change_bits[1];	/* ports with logical connect
 							status change */
-	unsigned long		busy_bits[1];	/* ports being reset or
-							resumed */
 	unsigned long		removed_bits[1]; /* ports with a "removed"
 							device present */
 	unsigned long		wakeup_bits[1];	/* ports that have signaled
 							remote wakeup */
+	unsigned long		power_bits[1]; /* ports that are powered */
+	unsigned long		child_usage_bits[1]; /* ports powered on for
+							children */
 #if USB_MAXCHILDREN > 31 /* 8*sizeof(unsigned long) - 1 */
 #error event_bits[] is too short!
 #endif
@@ -66,6 +67,7 @@ struct usb_hub {
 	unsigned		limited_power:1;
 	unsigned		quiescing:1;
 	unsigned		disconnected:1;
+	unsigned		in_reset:1;
 
 	unsigned		quirk_check_port_auto_suspend:1;
 
@@ -81,19 +83,23 @@ struct usb_hub {
  * @child: usb device attached to the port
  * @dev: generic device interface
  * @port_owner: port's owner
+ * @peer: related usb2 and usb3 ports (share the same connector)
  * @connect_type: port's connect type
+ * @location: opaque representation of platform connector location
+ * @status_lock: synchronize port_event() vs usb_port_{suspend|resume}
  * @portnum: port index num based one
- * @power_is_on: port's power state
- * @did_runtime_put: port has done pm_runtime_put().
+ * @is_superspeed cache super-speed status
  */
 struct usb_port {
 	struct usb_device *child;
 	struct device dev;
 	struct usb_dev_state *port_owner;
+	struct usb_port *peer;
 	enum usb_port_connect_type connect_type;
+	usb_port_location_t location;
+	struct mutex status_lock;
 	u8 portnum;
-	unsigned power_is_on:1;
-	unsigned did_runtime_put:1;
+	unsigned int is_superspeed:1;
 };
 
 #define to_usb_port(_dev) \
@@ -111,6 +117,29 @@ extern int hub_port_debounce(struct usb_hub *hub, int port1,
 extern int usb_clear_port_feature(struct usb_device *hdev,
 		int port1, int feature);
 
+static inline bool hub_is_port_power_switchable(struct usb_hub *hub)
+{
+	__le16 hcs;
+
+	if (!hub)
+		return false;
+	hcs = hub->descriptor->wHubCharacteristics;
+	return (le16_to_cpu(hcs) & HUB_CHAR_LPSM) < HUB_CHAR_NO_LPSM;
+}
+
+static inline int hub_is_superspeed(struct usb_device *hdev)
+{
+	return hdev->descriptor.bDeviceProtocol == USB_HUB_PR_SS;
+}
+
+static inline unsigned hub_power_on_good_delay(struct usb_hub *hub)
+{
+	unsigned delay = hub->descriptor->bPwrOn2PwrGood * 2;
+
+	/* Wait at least 100 msec for power to become stable */
+	return max(delay, 100U);
+}
+
 static inline int hub_port_debounce_be_connected(struct usb_hub *hub,
 		int port1)
 {
diff --git a/drivers/usb/core/port.c b/drivers/usb/core/port.c
index 51542f85239..62036faf56c 100644
--- a/drivers/usb/core/port.c
+++ b/drivers/usb/core/port.c
@@ -76,17 +76,29 @@ static int usb_port_runtime_resume(struct device *dev)
 	struct usb_device *hdev = to_usb_device(dev->parent->parent);
 	struct usb_interface *intf = to_usb_interface(dev->parent);
 	struct usb_hub *hub = usb_hub_to_struct_hub(hdev);
+	struct usb_device *udev = port_dev->child;
+	struct usb_port *peer = port_dev->peer;
 	int port1 = port_dev->portnum;
 	int retval;
 
 	if (!hub)
 		return -EINVAL;
+	if (hub->in_reset) {
+		set_bit(port1, hub->power_bits);
+		return 0;
+	}
 
-	usb_autopm_get_interface(intf);
-	set_bit(port1, hub->busy_bits);
+	/*
+	 * Power on our usb3 peer before this usb2 port to prevent a usb3
+	 * device from degrading to its usb2 connection
+	 */
+	if (!port_dev->is_superspeed && peer)
+		pm_runtime_get_sync(&peer->dev);
 
+	usb_autopm_get_interface(intf);
 	retval = usb_hub_set_port_power(hdev, hub, port1, true);
-	if (port_dev->child && !retval) {
+	msleep(hub_power_on_good_delay(hub));
+	if (udev && !retval) {
 		/*
 		 * Attempt to wait for usb hub port to be reconnected in order
 		 * to make the resume procedure successful.  The device may have
@@ -97,12 +109,17 @@ static int usb_port_runtime_resume(struct device *dev)
 		if (retval < 0)
 			dev_dbg(&port_dev->dev, "can't get reconnection after setting port  power on, status %d\n",
 					retval);
-		usb_clear_port_feature(hdev, port1, USB_PORT_FEAT_C_ENABLE);
 		retval = 0;
+
+		/* Force the child awake to revalidate after the power loss. */
+		if (!test_and_set_bit(port1, hub->child_usage_bits)) {
+			pm_runtime_get_noresume(&port_dev->dev);
+			pm_request_resume(&udev->dev);
+		}
 	}
 
-	clear_bit(port1, hub->busy_bits);
 	usb_autopm_put_interface(intf);
+
 	return retval;
 }
 
@@ -112,23 +129,34 @@ static int usb_port_runtime_suspend(struct device *dev)
 	struct usb_device *hdev = to_usb_device(dev->parent->parent);
 	struct usb_interface *intf = to_usb_interface(dev->parent);
 	struct usb_hub *hub = usb_hub_to_struct_hub(hdev);
+	struct usb_port *peer = port_dev->peer;
 	int port1 = port_dev->portnum;
 	int retval;
 
 	if (!hub)
 		return -EINVAL;
+	if (hub->in_reset)
+		return -EBUSY;
 
 	if (dev_pm_qos_flags(&port_dev->dev, PM_QOS_FLAG_NO_POWER_OFF)
 			== PM_QOS_FLAGS_ALL)
 		return -EAGAIN;
 
 	usb_autopm_get_interface(intf);
-	set_bit(port1, hub->busy_bits);
 	retval = usb_hub_set_port_power(hdev, hub, port1, false);
 	usb_clear_port_feature(hdev, port1, USB_PORT_FEAT_C_CONNECTION);
-	usb_clear_port_feature(hdev, port1,	USB_PORT_FEAT_C_ENABLE);
-	clear_bit(port1, hub->busy_bits);
+	if (!port_dev->is_superspeed)
+		usb_clear_port_feature(hdev, port1, USB_PORT_FEAT_C_ENABLE);
 	usb_autopm_put_interface(intf);
+
+	/*
+	 * Our peer usb3 port may now be able to suspend, so
+	 * asynchronously queue a suspend request to observe that this
+	 * usb2 port is now off.
+	 */
+	if (!port_dev->is_superspeed && peer)
+		pm_runtime_put(&peer->dev);
+
 	return retval;
 }
 #endif
@@ -146,9 +174,215 @@ struct device_type usb_port_device_type = {
 	.pm =		&usb_port_pm_ops,
 };
 
+static struct device_driver usb_port_driver = {
+	.name = "usb",
+	.owner = THIS_MODULE,
+};
+
+static int link_peers(struct usb_port *left, struct usb_port *right)
+{
+	struct usb_port *ss_port, *hs_port;
+	int rc;
+
+	if (left->peer == right && right->peer == left)
+		return 0;
+
+	if (left->peer || right->peer) {
+		struct usb_port *lpeer = left->peer;
+		struct usb_port *rpeer = right->peer;
+
+		WARN(1, "failed to peer %s and %s (%s -> %p) (%s -> %p)\n",
+			dev_name(&left->dev), dev_name(&right->dev),
+			dev_name(&left->dev), lpeer,
+			dev_name(&right->dev), rpeer);
+		return -EBUSY;
+	}
+
+	rc = sysfs_create_link(&left->dev.kobj, &right->dev.kobj, "peer");
+	if (rc)
+		return rc;
+	rc = sysfs_create_link(&right->dev.kobj, &left->dev.kobj, "peer");
+	if (rc) {
+		sysfs_remove_link(&left->dev.kobj, "peer");
+		return rc;
+	}
+
+	/*
+	 * We need to wake the HiSpeed port to make sure we don't race
+	 * setting ->peer with usb_port_runtime_suspend().  Otherwise we
+	 * may miss a suspend event for the SuperSpeed port.
+	 */
+	if (left->is_superspeed) {
+		ss_port = left;
+		WARN_ON(right->is_superspeed);
+		hs_port = right;
+	} else {
+		ss_port = right;
+		WARN_ON(!right->is_superspeed);
+		hs_port = left;
+	}
+	pm_runtime_get_sync(&hs_port->dev);
+
+	left->peer = right;
+	right->peer = left;
+
+	/*
+	 * The SuperSpeed reference is dropped when the HiSpeed port in
+	 * this relationship suspends, i.e. when it is safe to allow a
+	 * SuperSpeed connection to drop since there is no risk of a
+	 * device degrading to its powered-off HiSpeed connection.
+	 *
+	 * Also, drop the HiSpeed ref taken above.
+	 */
+	pm_runtime_get_sync(&ss_port->dev);
+	pm_runtime_put(&hs_port->dev);
+
+	return 0;
+}
+
+static void link_peers_report(struct usb_port *left, struct usb_port *right)
+{
+	int rc;
+
+	rc = link_peers(left, right);
+	if (rc == 0) {
+		dev_dbg(&left->dev, "peered to %s\n", dev_name(&right->dev));
+	} else {
+		dev_warn(&left->dev, "failed to peer to %s (%d)\n",
+				dev_name(&right->dev), rc);
+		pr_warn_once("usb: port power management may be unreliable\n");
+	}
+}
+
+static void unlink_peers(struct usb_port *left, struct usb_port *right)
+{
+	struct usb_port *ss_port, *hs_port;
+
+	WARN(right->peer != left || left->peer != right,
+			"%s and %s are not peers?\n",
+			dev_name(&left->dev), dev_name(&right->dev));
+
+	/*
+	 * We wake the HiSpeed port to make sure we don't race its
+	 * usb_port_runtime_resume() event which takes a SuperSpeed ref
+	 * when ->peer is !NULL.
+	 */
+	if (left->is_superspeed) {
+		ss_port = left;
+		hs_port = right;
+	} else {
+		ss_port = right;
+		hs_port = left;
+	}
+
+	pm_runtime_get_sync(&hs_port->dev);
+
+	sysfs_remove_link(&left->dev.kobj, "peer");
+	right->peer = NULL;
+	sysfs_remove_link(&right->dev.kobj, "peer");
+	left->peer = NULL;
+
+	/* Drop the SuperSpeed ref held on behalf of the active HiSpeed port */
+	pm_runtime_put(&ss_port->dev);
+
+	/* Drop the ref taken above */
+	pm_runtime_put(&hs_port->dev);
+}
+
+/*
+ * For each usb hub device in the system check to see if it is in the
+ * peer domain of the given port_dev, and if it is check to see if it
+ * has a port that matches the given port by location
+ */
+static int match_location(struct usb_device *peer_hdev, void *p)
+{
+	int port1;
+	struct usb_hcd *hcd, *peer_hcd;
+	struct usb_port *port_dev = p, *peer;
+	struct usb_hub *peer_hub = usb_hub_to_struct_hub(peer_hdev);
+	struct usb_device *hdev = to_usb_device(port_dev->dev.parent->parent);
+
+	if (!peer_hub)
+		return 0;
+
+	hcd = bus_to_hcd(hdev->bus);
+	peer_hcd = bus_to_hcd(peer_hdev->bus);
+	/* peer_hcd is provisional until we verify it against the known peer */
+	if (peer_hcd != hcd->shared_hcd)
+		return 0;
+
+	for (port1 = 1; port1 <= peer_hdev->maxchild; port1++) {
+		peer = peer_hub->ports[port1 - 1];
+		if (peer && peer->location == port_dev->location) {
+			link_peers_report(port_dev, peer);
+			return 1; /* done */
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * Find the peer port either via explicit platform firmware "location"
+ * data, the peer hcd for root hubs, or the upstream peer relationship
+ * for all other hubs.
+ */
+static void find_and_link_peer(struct usb_hub *hub, int port1)
+{
+	struct usb_port *port_dev = hub->ports[port1 - 1], *peer;
+	struct usb_device *hdev = hub->hdev;
+	struct usb_device *peer_hdev;
+	struct usb_hub *peer_hub;
+
+	/*
+	 * If location data is available then we can only peer this port
+	 * by a location match, not the default peer (lest we create a
+	 * situation where we need to go back and undo a default peering
+	 * when the port is later peered by location data)
+	 */
+	if (port_dev->location) {
+		/* we link the peer in match_location() if found */
+		usb_for_each_dev(port_dev, match_location);
+		return;
+	} else if (!hdev->parent) {
+		struct usb_hcd *hcd = bus_to_hcd(hdev->bus);
+		struct usb_hcd *peer_hcd = hcd->shared_hcd;
+
+		if (!peer_hcd)
+			return;
+
+		peer_hdev = peer_hcd->self.root_hub;
+	} else {
+		struct usb_port *upstream;
+		struct usb_device *parent = hdev->parent;
+		struct usb_hub *parent_hub = usb_hub_to_struct_hub(parent);
+
+		if (!parent_hub)
+			return;
+
+		upstream = parent_hub->ports[hdev->portnum - 1];
+		if (!upstream || !upstream->peer)
+			return;
+
+		peer_hdev = upstream->peer->child;
+	}
+
+	peer_hub = usb_hub_to_struct_hub(peer_hdev);
+	if (!peer_hub || port1 > peer_hdev->maxchild)
+		return;
+
+	/*
+	 * we found a valid default peer, last check is to make sure it
+	 * does not have location data
+	 */
+	peer = peer_hub->ports[port1 - 1];
+	if (peer && peer->location == 0)
+		link_peers_report(port_dev, peer);
+}
+
 int usb_hub_create_port_device(struct usb_hub *hub, int port1)
 {
-	struct usb_port *port_dev = NULL;
+	struct usb_port *port_dev;
 	int retval;
 
 	port_dev = kzalloc(sizeof(*port_dev), GFP_KERNEL);
@@ -159,24 +393,33 @@ int usb_hub_create_port_device(struct usb_hub *hub, int port1)
 
 	hub->ports[port1 - 1] = port_dev;
 	port_dev->portnum = port1;
-	port_dev->power_is_on = true;
+	set_bit(port1, hub->power_bits);
 	port_dev->dev.parent = hub->intfdev;
 	port_dev->dev.groups = port_dev_group;
 	port_dev->dev.type = &usb_port_device_type;
-	dev_set_name(&port_dev->dev, "port%d", port1);
-
+	port_dev->dev.driver = &usb_port_driver;
+	if (hub_is_superspeed(hub->hdev))
+		port_dev->is_superspeed = 1;
+	dev_set_name(&port_dev->dev, "%s-port%d", dev_name(&hub->hdev->dev),
+			port1);
+	mutex_init(&port_dev->status_lock);
 	retval = device_register(&port_dev->dev);
 	if (retval)
 		goto error_register;
 
+	find_and_link_peer(hub, port1);
+
 	pm_runtime_set_active(&port_dev->dev);
 
-	/* It would be dangerous if user space couldn't
-	 * prevent usb device from being powered off. So don't
-	 * enable port runtime pm if failed to expose port's pm qos.
+	/*
+	 * Do not enable port runtime pm if the hub does not support
+	 * power switching.  Also, userspace must have final say of
+	 * whether a port is permitted to power-off.  Do not enable
+	 * runtime pm if we fail to expose pm_qos_no_power_off.
 	 */
-	if (!dev_pm_qos_expose_flags(&port_dev->dev,
-			PM_QOS_FLAG_NO_POWER_OFF))
+	if (hub_is_port_power_switchable(hub)
+			&& dev_pm_qos_expose_flags(&port_dev->dev,
+			PM_QOS_FLAG_NO_POWER_OFF) == 0)
 		pm_runtime_enable(&port_dev->dev);
 
 	device_enable_async_suspend(&port_dev->dev);
@@ -188,9 +431,13 @@ exit:
 	return retval;
 }
 
-void usb_hub_remove_port_device(struct usb_hub *hub,
-				       int port1)
+void usb_hub_remove_port_device(struct usb_hub *hub, int port1)
 {
-	device_unregister(&hub->ports[port1 - 1]->dev);
-}
+	struct usb_port *port_dev = hub->ports[port1 - 1];
+	struct usb_port *peer;
 
+	peer = port_dev->peer;
+	if (peer)
+		unlink_peers(port_dev, peer);
+	device_unregister(&port_dev->dev);
+}
diff --git a/drivers/usb/core/usb-acpi.c b/drivers/usb/core/usb-acpi.c
index 5ca4070b1f3..2776cfe64c0 100644
--- a/drivers/usb/core/usb-acpi.c
+++ b/drivers/usb/core/usb-acpi.c
@@ -17,7 +17,7 @@
 #include <linux/pci.h>
 #include <linux/usb/hcd.h>
 
-#include "usb.h"
+#include "hub.h"
 
 /**
  * usb_acpi_power_manageable - check whether usb port has
@@ -55,13 +55,18 @@ EXPORT_SYMBOL_GPL(usb_acpi_power_manageable);
  */
 int usb_acpi_set_power_state(struct usb_device *hdev, int index, bool enable)
 {
+	struct usb_hub *hub = usb_hub_to_struct_hub(hdev);
+	struct usb_port *port_dev;
 	acpi_handle port_handle;
 	unsigned char state;
 	int port1 = index + 1;
 	int error = -EINVAL;
 
-	port_handle = (acpi_handle)usb_get_hub_port_acpi_handle(hdev,
-		port1);
+	if (!hub)
+		return -ENODEV;
+	port_dev = hub->ports[port1 - 1];
+
+	port_handle = (acpi_handle) usb_get_hub_port_acpi_handle(hdev, port1);
 	if (!port_handle)
 		return error;
 
@@ -72,23 +77,21 @@ int usb_acpi_set_power_state(struct usb_device *hdev, int index, bool enable)
 
 	error = acpi_bus_set_power(port_handle, state);
 	if (!error)
-		dev_dbg(&hdev->dev, "The power of hub port %d was set to %d\n",
-			port1, enable);
+		dev_dbg(&port_dev->dev, "acpi: power was set to %d\n", enable);
 	else
-		dev_dbg(&hdev->dev, "The power of hub port failed to be set\n");
+		dev_dbg(&port_dev->dev, "acpi: power failed to be set\n");
 
 	return error;
 }
 EXPORT_SYMBOL_GPL(usb_acpi_set_power_state);
 
-static int usb_acpi_check_port_connect_type(struct usb_device *hdev,
-	acpi_handle handle, int port1)
+static enum usb_port_connect_type usb_acpi_get_connect_type(acpi_handle handle,
+		struct acpi_pld_info *pld)
 {
-	acpi_status status;
+	enum usb_port_connect_type connect_type = USB_PORT_CONNECT_TYPE_UNKNOWN;
 	struct acpi_buffer buffer = { ACPI_ALLOCATE_BUFFER, NULL };
 	union acpi_object *upc;
-	struct acpi_pld_info *pld;
-	int ret = 0;
+	acpi_status status;
 
 	/*
 	 * According to ACPI Spec 9.13. PLD indicates whether usb port is
@@ -98,39 +101,37 @@ static int usb_acpi_check_port_connect_type(struct usb_device *hdev,
 	 * a usb device is directly hard-wired to the port. If no visible and
 	 * no connectable, the port would be not used.
 	 */
-	status = acpi_get_physical_device_location(handle, &pld);
-	if (ACPI_FAILURE(status))
-		return -ENODEV;
-
 	status = acpi_evaluate_object(handle, "_UPC", NULL, &buffer);
 	upc = buffer.pointer;
 	if (!upc || (upc->type != ACPI_TYPE_PACKAGE)
 		|| upc->package.count != 4) {
-		ret = -EINVAL;
 		goto out;
 	}
 
 	if (upc->package.elements[0].integer.value)
 		if (pld->user_visible)
-			usb_set_hub_port_connect_type(hdev, port1,
-				USB_PORT_CONNECT_TYPE_HOT_PLUG);
+			connect_type = USB_PORT_CONNECT_TYPE_HOT_PLUG;
 		else
-			usb_set_hub_port_connect_type(hdev, port1,
-				USB_PORT_CONNECT_TYPE_HARD_WIRED);
+			connect_type = USB_PORT_CONNECT_TYPE_HARD_WIRED;
 	else if (!pld->user_visible)
-		usb_set_hub_port_connect_type(hdev, port1, USB_PORT_NOT_USED);
-
+		connect_type = USB_PORT_NOT_USED;
 out:
-	ACPI_FREE(pld);
 	kfree(upc);
-	return ret;
+	return connect_type;
 }
 
+
+/*
+ * Private to usb-acpi, all the core needs to know is that
+ * port_dev->location is non-zero when it has been set by the firmware.
+ */
+#define USB_ACPI_LOCATION_VALID (1 << 31)
+
 static struct acpi_device *usb_acpi_find_companion(struct device *dev)
 {
 	struct usb_device *udev;
+	struct acpi_device *adev;
 	acpi_handle *parent_handle;
-	int port_num;
 
 	/*
 	 * In the ACPI DSDT table, only usb root hub and usb ports are
@@ -147,37 +148,19 @@ static struct acpi_device *usb_acpi_find_companion(struct device *dev)
 	 */
 	if (is_usb_device(dev)) {
 		udev = to_usb_device(dev);
-		if (udev->parent) {
-			enum usb_port_connect_type type;
-
-			/*
-			 * According usb port's connect type to set usb device's
-			 * removability.
-			 */
-			type = usb_get_hub_port_connect_type(udev->parent,
-				udev->portnum);
-			switch (type) {
-			case USB_PORT_CONNECT_TYPE_HOT_PLUG:
-				udev->removable = USB_DEVICE_REMOVABLE;
-				break;
-			case USB_PORT_CONNECT_TYPE_HARD_WIRED:
-				udev->removable = USB_DEVICE_FIXED;
-				break;
-			default:
-				udev->removable = USB_DEVICE_REMOVABLE_UNKNOWN;
-				break;
-			}
-
+		if (udev->parent)
 			return NULL;
-		}
 
-		/* root hub's parent is the usb hcd. */
-		return acpi_find_child_device(ACPI_COMPANION(dev->parent),
-					      udev->portnum, false);
+		/* root hub is only child (_ADR=0) under its parent, the HC */
+		adev = ACPI_COMPANION(dev->parent);
+		return acpi_find_child_device(adev, 0, false);
 	} else if (is_usb_port(dev)) {
-		struct acpi_device *adev = NULL;
+		struct usb_port *port_dev = to_usb_port(dev);
+		int port1 = port_dev->portnum;
+		struct acpi_pld_info *pld;
+		acpi_handle *handle;
+		acpi_status status;
 
-		sscanf(dev_name(dev), "port%d", &port_num);
 		/* Get the struct usb_device point of port's hub */
 		udev = to_usb_device(dev->parent->parent);
 
@@ -188,12 +171,11 @@ static struct acpi_device *usb_acpi_find_companion(struct device *dev)
 		 */
 		if (!udev->parent) {
 			struct usb_hcd *hcd = bus_to_hcd(udev->bus);
-			int raw_port_num;
+			int raw;
 
-			raw_port_num = usb_hcd_find_raw_port_number(hcd,
-				port_num);
+			raw = usb_hcd_find_raw_port_number(hcd, port1);
 			adev = acpi_find_child_device(ACPI_COMPANION(&udev->dev),
-						      raw_port_num, false);
+					raw, false);
 			if (!adev)
 				return NULL;
 		} else {
@@ -204,11 +186,20 @@ static struct acpi_device *usb_acpi_find_companion(struct device *dev)
 				return NULL;
 
 			acpi_bus_get_device(parent_handle, &adev);
-			adev = acpi_find_child_device(adev, port_num, false);
+			adev = acpi_find_child_device(adev, port1, false);
 			if (!adev)
 				return NULL;
 		}
-		usb_acpi_check_port_connect_type(udev, adev->handle, port_num);
+		handle = adev->handle;
+		status = acpi_get_physical_device_location(handle, &pld);
+		if (ACPI_FAILURE(status) || !pld)
+			return adev;
+
+		port_dev->location = USB_ACPI_LOCATION_VALID
+			| pld->group_token << 8 | pld->group_position;
+		port_dev->connect_type = usb_acpi_get_connect_type(handle, pld);
+		ACPI_FREE(pld);
+
 		return adev;
 	}
 
diff --git a/drivers/usb/core/usb.h b/drivers/usb/core/usb.h
index 75bf649da82..d9d08720c38 100644
--- a/drivers/usb/core/usb.h
+++ b/drivers/usb/core/usb.h
@@ -107,11 +107,6 @@ static inline int usb_autoresume_device(struct usb_device *udev)
 	return 0;
 }
 
-static inline int usb_remote_wakeup(struct usb_device *udev)
-{
-	return 0;
-}
-
 static inline int usb_set_usb2_hardware_lpm(struct usb_device *udev, int enable)
 {
 	return 0;
@@ -119,6 +114,7 @@ static inline int usb_set_usb2_hardware_lpm(struct usb_device *udev, int enable)
 #endif
 
 extern struct bus_type usb_bus_type;
+extern struct mutex usb_port_peer_mutex;
 extern struct device_type usb_device_type;
 extern struct device_type usb_if_device_type;
 extern struct device_type usb_ep_device_type;
@@ -170,15 +166,17 @@ extern void usbfs_conn_disc_event(void);
 extern int usb_devio_init(void);
 extern void usb_devio_cleanup(void);
 
+/*
+ * Firmware specific cookie identifying a port's location. '0' == no location
+ * data available
+ */
+typedef u32 usb_port_location_t;
+
 /* internal notify stuff */
 extern void usb_notify_add_device(struct usb_device *udev);
 extern void usb_notify_remove_device(struct usb_device *udev);
 extern void usb_notify_add_bus(struct usb_bus *ubus);
 extern void usb_notify_remove_bus(struct usb_bus *ubus);
-extern enum usb_port_connect_type
-	usb_get_hub_port_connect_type(struct usb_device *hdev, int port1);
-extern void usb_set_hub_port_connect_type(struct usb_device *hdev, int port1,
-	enum usb_port_connect_type type);
 extern void usb_hub_adjust_deviceremovable(struct usb_device *hdev,
 		struct usb_hub_descriptor *desc);
 
diff --git a/drivers/usb/dwc2/Kconfig b/drivers/usb/dwc2/Kconfig
index be947d67384..f93807b3631 100644
--- a/drivers/usb/dwc2/Kconfig
+++ b/drivers/usb/dwc2/Kconfig
@@ -1,25 +1,58 @@
 config USB_DWC2
-	tristate "DesignWare USB2 DRD Core Support"
+	bool "DesignWare USB2 DRD Core Support"
 	depends on USB
 	help
-	  Say Y or M here if your system has a Dual Role HighSpeed
-	  USB controller based on the DesignWare HSOTG IP Core.
+	  Say Y here if your system has a Dual Role Hi-Speed 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.
+	  For host mode, if you choose to build the 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. For gadget mode, there will be a single
+	  module called dwc2_gadget.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.
+	  NOTE: The s3c-hsotg driver is now renamed to dwc2_gadget. The
+	  host and gadget drivers are still currently separate drivers.
+	  There are plans to merge the dwc2_gadget driver with the dwc2
+	  host driver in the near future to create a dual-role driver.
 
 if USB_DWC2
 
+config USB_DWC2_HOST
+	tristate "Host only mode"
+	depends on USB
+	help
+	  The Designware USB2.0 high-speed host controller
+	  integrated into many SoCs.
+
+config USB_DWC2_PLATFORM
+	bool "DWC2 Platform"
+	depends on USB_DWC2_HOST
+	default USB_DWC2_HOST
+	help
+	  The Designware USB2.0 platform interface module for
+	  controllers directly connected to the CPU. This is only
+	  used for host mode.
+
+config USB_DWC2_PCI
+	bool "DWC2 PCI"
+	depends on USB_DWC2_HOST && PCI
+	default USB_DWC2_HOST
+	help
+	  The Designware USB2.0 PCI interface module for controllers
+	  connected to a PCI bus. This is only used for host mode.
+
+comment "Gadget mode requires USB Gadget support to be enabled"
+
+config USB_DWC2_PERIPHERAL
+	tristate "Gadget only mode"
+	depends on USB_GADGET
+	help
+	  The Designware USB2.0 high-speed gadget controller
+	  integrated into many SoCs.
+
 config USB_DWC2_DEBUG
 	bool "Enable Debugging Messages"
 	help
diff --git a/drivers/usb/dwc2/Makefile b/drivers/usb/dwc2/Makefile
index 11529d3439b..b73d2a52797 100644
--- a/drivers/usb/dwc2/Makefile
+++ b/drivers/usb/dwc2/Makefile
@@ -1,25 +1,28 @@
 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.
-
+obj-$(CONFIG_USB_DWC2_HOST)		+= dwc2.o
+dwc2-y					:= core.o core_intr.o
 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
+# NOTE: The previous s3c-hsotg peripheral mode only driver has been moved to
+# this location and renamed gadget.c. When building for dynamically linked
+# modules, dwc2_gadget.ko will get built for peripheral mode. For host mode,
+# the core module will be dwc2.ko, the PCI bus interface module will called
+# dwc2_pci.ko and the platform interface module will be called dwc2_platform.ko.
+# At present the host and gadget driver will be separate drivers, but there
+# are plans in the near future to create a dual-role driver.
+
+ifneq ($(CONFIG_USB_DWC2_PCI),)
+	obj-$(CONFIG_USB_DWC2_HOST)	+= dwc2_pci.o
+	dwc2_pci-y			:= pci.o
+endif
+
+ifneq ($(CONFIG_USB_DWC2_PLATFORM),)
+	obj-$(CONFIG_USB_DWC2_HOST)	+= dwc2_platform.o
+	dwc2_platform-y			:= platform.o
 endif
-obj-$(CONFIG_USB_DWC2)			+= dwc2_platform.o
 
-dwc2_pci-y				+= pci.o
-dwc2_platform-y				+= platform.o
+obj-$(CONFIG_USB_DWC2_PERIPHERAL)	+= dwc2_gadget.o
+dwc2_gadget-y				:= gadget.o
diff --git a/drivers/usb/dwc2/core.c b/drivers/usb/dwc2/core.c
index 1d129884cc3..27d2c9b8a03 100644
--- a/drivers/usb/dwc2/core.c
+++ b/drivers/usb/dwc2/core.c
@@ -507,6 +507,72 @@ void dwc2_disable_host_interrupts(struct dwc2_hsotg *hsotg)
 	writel(intmsk, hsotg->regs + GINTMSK);
 }
 
+/*
+ * dwc2_calculate_dynamic_fifo() - Calculates the default fifo size
+ * For system that have a total fifo depth that is smaller than the default
+ * RX + TX fifo size.
+ *
+ * @hsotg: Programming view of DWC_otg controller
+ */
+static void dwc2_calculate_dynamic_fifo(struct dwc2_hsotg *hsotg)
+{
+	struct dwc2_core_params *params = hsotg->core_params;
+	struct dwc2_hw_params *hw = &hsotg->hw_params;
+	u32 rxfsiz, nptxfsiz, ptxfsiz, total_fifo_size;
+
+	total_fifo_size = hw->total_fifo_size;
+	rxfsiz = params->host_rx_fifo_size;
+	nptxfsiz = params->host_nperio_tx_fifo_size;
+	ptxfsiz = params->host_perio_tx_fifo_size;
+
+	/*
+	 * Will use Method 2 defined in the DWC2 spec: minimum FIFO depth
+	 * allocation with support for high bandwidth endpoints. Synopsys
+	 * defines MPS(Max Packet size) for a periodic EP=1024, and for
+	 * non-periodic as 512.
+	 */
+	if (total_fifo_size < (rxfsiz + nptxfsiz + ptxfsiz)) {
+		/*
+		 * For Buffer DMA mode/Scatter Gather DMA mode
+		 * 2 * ((Largest Packet size / 4) + 1 + 1) + n
+		 * with n = number of host channel.
+		 * 2 * ((1024/4) + 2) = 516
+		 */
+		rxfsiz = 516 + hw->host_channels;
+
+		/*
+		 * min non-periodic tx fifo depth
+		 * 2 * (largest non-periodic USB packet used / 4)
+		 * 2 * (512/4) = 256
+		 */
+		nptxfsiz = 256;
+
+		/*
+		 * min periodic tx fifo depth
+		 * (largest packet size*MC)/4
+		 * (1024 * 3)/4 = 768
+		 */
+		ptxfsiz = 768;
+
+		params->host_rx_fifo_size = rxfsiz;
+		params->host_nperio_tx_fifo_size = nptxfsiz;
+		params->host_perio_tx_fifo_size = ptxfsiz;
+	}
+
+	/*
+	 * If the summation of RX, NPTX and PTX fifo sizes is still
+	 * bigger than the total_fifo_size, then we have a problem.
+	 *
+	 * We won't be able to allocate as many endpoints. Right now,
+	 * we're just printing an error message, but ideally this FIFO
+	 * allocation algorithm would be improved in the future.
+	 *
+	 * FIXME improve this FIFO allocation algorithm.
+	 */
+	if (unlikely(total_fifo_size < (rxfsiz + nptxfsiz + ptxfsiz)))
+		dev_err(hsotg->dev, "invalid fifo sizes\n");
+}
+
 static void dwc2_config_fifos(struct dwc2_hsotg *hsotg)
 {
 	struct dwc2_core_params *params = hsotg->core_params;
@@ -515,6 +581,8 @@ static void dwc2_config_fifos(struct dwc2_hsotg *hsotg)
 	if (!params->enable_dynamic_fifo)
 		return;
 
+	dwc2_calculate_dynamic_fifo(hsotg);
+
 	/* Rx FIFO */
 	grxfsiz = readl(hsotg->regs + GRXFSIZ);
 	dev_dbg(hsotg->dev, "initial grxfsiz=%08x\n", grxfsiz);
diff --git a/drivers/usb/dwc2/core.h b/drivers/usb/dwc2/core.h
index 648519c024b..1efd10cc962 100644
--- a/drivers/usb/dwc2/core.h
+++ b/drivers/usb/dwc2/core.h
@@ -37,6 +37,10 @@
 #ifndef __DWC2_CORE_H__
 #define __DWC2_CORE_H__
 
+#include <linux/phy/phy.h>
+#include <linux/regulator/consumer.h>
+#include <linux/usb/gadget.h>
+#include <linux/usb/otg.h>
 #include <linux/usb/phy.h>
 #include "hw.h"
 
@@ -54,6 +58,184 @@ static inline void do_write(u32 value, void *addr)
 /* Maximum number of Endpoints/HostChannels */
 #define MAX_EPS_CHANNELS	16
 
+/* s3c-hsotg declarations */
+static const char * const s3c_hsotg_supply_names[] = {
+	"vusb_d",               /* digital USB supply, 1.2V */
+	"vusb_a",               /* analog USB supply, 1.1V */
+};
+
+/*
+ * EP0_MPS_LIMIT
+ *
+ * Unfortunately there seems to be a limit of the amount of data that can
+ * be transferred by IN transactions on EP0. This is either 127 bytes or 3
+ * packets (which practically means 1 packet and 63 bytes of data) when the
+ * MPS is set to 64.
+ *
+ * This means if we are wanting to move >127 bytes of data, we need to
+ * split the transactions up, but just doing one packet at a time does
+ * not work (this may be an implicit DATA0 PID on first packet of the
+ * transaction) and doing 2 packets is outside the controller's limits.
+ *
+ * If we try to lower the MPS size for EP0, then no transfers work properly
+ * for EP0, and the system will fail basic enumeration. As no cause for this
+ * has currently been found, we cannot support any large IN transfers for
+ * EP0.
+ */
+#define EP0_MPS_LIMIT   64
+
+struct s3c_hsotg;
+struct s3c_hsotg_req;
+
+/**
+ * struct s3c_hsotg_ep - driver endpoint definition.
+ * @ep: The gadget layer representation of the endpoint.
+ * @name: The driver generated name for the endpoint.
+ * @queue: Queue of requests for this endpoint.
+ * @parent: Reference back to the parent device structure.
+ * @req: The current request that the endpoint is processing. This is
+ *       used to indicate an request has been loaded onto the endpoint
+ *       and has yet to be completed (maybe due to data move, or simply
+ *       awaiting an ack from the core all the data has been completed).
+ * @debugfs: File entry for debugfs file for this endpoint.
+ * @lock: State lock to protect contents of endpoint.
+ * @dir_in: Set to true if this endpoint is of the IN direction, which
+ *          means that it is sending data to the Host.
+ * @index: The index for the endpoint registers.
+ * @mc: Multi Count - number of transactions per microframe
+ * @interval - Interval for periodic endpoints
+ * @name: The name array passed to the USB core.
+ * @halted: Set if the endpoint has been halted.
+ * @periodic: Set if this is a periodic ep, such as Interrupt
+ * @isochronous: Set if this is a isochronous ep
+ * @sent_zlp: Set if we've sent a zero-length packet.
+ * @total_data: The total number of data bytes done.
+ * @fifo_size: The size of the FIFO (for periodic IN endpoints)
+ * @fifo_load: The amount of data loaded into the FIFO (periodic IN)
+ * @last_load: The offset of data for the last start of request.
+ * @size_loaded: The last loaded size for DxEPTSIZE for periodic IN
+ *
+ * This is the driver's state for each registered enpoint, allowing it
+ * to keep track of transactions that need doing. Each endpoint has a
+ * lock to protect the state, to try and avoid using an overall lock
+ * for the host controller as much as possible.
+ *
+ * For periodic IN endpoints, we have fifo_size and fifo_load to try
+ * and keep track of the amount of data in the periodic FIFO for each
+ * of these as we don't have a status register that tells us how much
+ * is in each of them. (note, this may actually be useless information
+ * as in shared-fifo mode periodic in acts like a single-frame packet
+ * buffer than a fifo)
+ */
+struct s3c_hsotg_ep {
+	struct usb_ep           ep;
+	struct list_head        queue;
+	struct s3c_hsotg        *parent;
+	struct s3c_hsotg_req    *req;
+	struct dentry           *debugfs;
+
+	unsigned long           total_data;
+	unsigned int            size_loaded;
+	unsigned int            last_load;
+	unsigned int            fifo_load;
+	unsigned short          fifo_size;
+
+	unsigned char           dir_in;
+	unsigned char           index;
+	unsigned char           mc;
+	unsigned char           interval;
+
+	unsigned int            halted:1;
+	unsigned int            periodic:1;
+	unsigned int            isochronous:1;
+	unsigned int            sent_zlp:1;
+
+	char                    name[10];
+};
+
+/**
+ * struct s3c_hsotg - driver state.
+ * @dev: The parent device supplied to the probe function
+ * @driver: USB gadget driver
+ * @phy: The otg phy transceiver structure for phy control.
+ * @uphy: The otg phy transceiver structure for old USB phy control.
+ * @plat: The platform specific configuration data. This can be removed once
+ * all SoCs support usb transceiver.
+ * @regs: The memory area mapped for accessing registers.
+ * @irq: The IRQ number we are using
+ * @supplies: Definition of USB power supplies
+ * @phyif: PHY interface width
+ * @dedicated_fifos: Set if the hardware has dedicated IN-EP fifos.
+ * @num_of_eps: Number of available EPs (excluding EP0)
+ * @debug_root: root directrory for debugfs.
+ * @debug_file: main status file for debugfs.
+ * @debug_fifo: FIFO status file for debugfs.
+ * @ep0_reply: Request used for ep0 reply.
+ * @ep0_buff: Buffer for EP0 reply data, if needed.
+ * @ctrl_buff: Buffer for EP0 control requests.
+ * @ctrl_req: Request for EP0 control packets.
+ * @setup: NAK management for EP0 SETUP
+ * @last_rst: Time of last reset
+ * @eps: The endpoints being supplied to the gadget framework
+ */
+struct s3c_hsotg {
+	struct device            *dev;
+	struct usb_gadget_driver *driver;
+	struct phy               *phy;
+	struct usb_phy           *uphy;
+	struct s3c_hsotg_plat    *plat;
+
+	spinlock_t              lock;
+
+	void __iomem            *regs;
+	int                     irq;
+	struct clk              *clk;
+
+	struct regulator_bulk_data supplies[ARRAY_SIZE(s3c_hsotg_supply_names)];
+
+	u32                     phyif;
+	unsigned int            dedicated_fifos:1;
+	unsigned char           num_of_eps;
+
+	struct dentry           *debug_root;
+	struct dentry           *debug_file;
+	struct dentry           *debug_fifo;
+
+	struct usb_request      *ep0_reply;
+	struct usb_request      *ctrl_req;
+	u8                      ep0_buff[8];
+	u8                      ctrl_buff[8];
+
+	struct usb_gadget       gadget;
+	unsigned int            setup;
+	unsigned long           last_rst;
+	struct s3c_hsotg_ep     *eps;
+};
+
+/**
+ * struct s3c_hsotg_req - data transfer request
+ * @req: The USB gadget request
+ * @queue: The list of requests for the endpoint this is queued for.
+ * @in_progress: Has already had size/packets written to core
+ * @mapped: DMA buffer for this request has been mapped via dma_map_single().
+ */
+struct s3c_hsotg_req {
+	struct usb_request      req;
+	struct list_head        queue;
+	unsigned char           in_progress;
+	unsigned char           mapped;
+};
+
+#define call_gadget(_hs, _entry) \
+do { \
+	if ((_hs)->gadget.speed != USB_SPEED_UNKNOWN && \
+		(_hs)->driver && (_hs)->driver->_entry) { \
+		spin_unlock(&_hs->lock); \
+		(_hs)->driver->_entry(&(_hs)->gadget); \
+		spin_lock(&_hs->lock); \
+	} \
+} while (0)
+
 struct dwc2_hsotg;
 struct dwc2_host_chan;
 
diff --git a/drivers/usb/gadget/s3c-hsotg.c b/drivers/usb/dwc2/gadget.c
index 2a9cb674926..f3c56a2fed5 100644
--- a/drivers/usb/gadget/s3c-hsotg.c
+++ b/drivers/usb/dwc2/gadget.c
@@ -37,175 +37,7 @@
 #include <linux/usb/phy.h>
 #include <linux/platform_data/s3c-hsotg.h>
 
-#include "s3c-hsotg.h"
-
-static const char * const s3c_hsotg_supply_names[] = {
-	"vusb_d",		/* digital USB supply, 1.2V */
-	"vusb_a",		/* analog USB supply, 1.1V */
-};
-
-/*
- * EP0_MPS_LIMIT
- *
- * Unfortunately there seems to be a limit of the amount of data that can
- * be transferred by IN transactions on EP0. This is either 127 bytes or 3
- * packets (which practically means 1 packet and 63 bytes of data) when the
- * MPS is set to 64.
- *
- * This means if we are wanting to move >127 bytes of data, we need to
- * split the transactions up, but just doing one packet at a time does
- * not work (this may be an implicit DATA0 PID on first packet of the
- * transaction) and doing 2 packets is outside the controller's limits.
- *
- * If we try to lower the MPS size for EP0, then no transfers work properly
- * for EP0, and the system will fail basic enumeration. As no cause for this
- * has currently been found, we cannot support any large IN transfers for
- * EP0.
- */
-#define EP0_MPS_LIMIT	64
-
-struct s3c_hsotg;
-struct s3c_hsotg_req;
-
-/**
- * struct s3c_hsotg_ep - driver endpoint definition.
- * @ep: The gadget layer representation of the endpoint.
- * @name: The driver generated name for the endpoint.
- * @queue: Queue of requests for this endpoint.
- * @parent: Reference back to the parent device structure.
- * @req: The current request that the endpoint is processing. This is
- *       used to indicate an request has been loaded onto the endpoint
- *       and has yet to be completed (maybe due to data move, or simply
- *	 awaiting an ack from the core all the data has been completed).
- * @debugfs: File entry for debugfs file for this endpoint.
- * @lock: State lock to protect contents of endpoint.
- * @dir_in: Set to true if this endpoint is of the IN direction, which
- *	    means that it is sending data to the Host.
- * @index: The index for the endpoint registers.
- * @mc: Multi Count - number of transactions per microframe
- * @interval - Interval for periodic endpoints
- * @name: The name array passed to the USB core.
- * @halted: Set if the endpoint has been halted.
- * @periodic: Set if this is a periodic ep, such as Interrupt
- * @isochronous: Set if this is a isochronous ep
- * @sent_zlp: Set if we've sent a zero-length packet.
- * @total_data: The total number of data bytes done.
- * @fifo_size: The size of the FIFO (for periodic IN endpoints)
- * @fifo_load: The amount of data loaded into the FIFO (periodic IN)
- * @last_load: The offset of data for the last start of request.
- * @size_loaded: The last loaded size for DxEPTSIZE for periodic IN
- *
- * This is the driver's state for each registered enpoint, allowing it
- * to keep track of transactions that need doing. Each endpoint has a
- * lock to protect the state, to try and avoid using an overall lock
- * for the host controller as much as possible.
- *
- * For periodic IN endpoints, we have fifo_size and fifo_load to try
- * and keep track of the amount of data in the periodic FIFO for each
- * of these as we don't have a status register that tells us how much
- * is in each of them. (note, this may actually be useless information
- * as in shared-fifo mode periodic in acts like a single-frame packet
- * buffer than a fifo)
- */
-struct s3c_hsotg_ep {
-	struct usb_ep		ep;
-	struct list_head	queue;
-	struct s3c_hsotg	*parent;
-	struct s3c_hsotg_req	*req;
-	struct dentry		*debugfs;
-
-
-	unsigned long		total_data;
-	unsigned int		size_loaded;
-	unsigned int		last_load;
-	unsigned int		fifo_load;
-	unsigned short		fifo_size;
-
-	unsigned char		dir_in;
-	unsigned char		index;
-	unsigned char		mc;
-	unsigned char		interval;
-
-	unsigned int		halted:1;
-	unsigned int		periodic:1;
-	unsigned int		isochronous:1;
-	unsigned int		sent_zlp:1;
-
-	char			name[10];
-};
-
-/**
- * struct s3c_hsotg - driver state.
- * @dev: The parent device supplied to the probe function
- * @driver: USB gadget driver
- * @phy: The otg phy transceiver structure for phy control.
- * @uphy: The otg phy transceiver structure for old USB phy control.
- * @plat: The platform specific configuration data. This can be removed once
- * all SoCs support usb transceiver.
- * @regs: The memory area mapped for accessing registers.
- * @irq: The IRQ number we are using
- * @supplies: Definition of USB power supplies
- * @phyif: PHY interface width
- * @dedicated_fifos: Set if the hardware has dedicated IN-EP fifos.
- * @num_of_eps: Number of available EPs (excluding EP0)
- * @debug_root: root directrory for debugfs.
- * @debug_file: main status file for debugfs.
- * @debug_fifo: FIFO status file for debugfs.
- * @ep0_reply: Request used for ep0 reply.
- * @ep0_buff: Buffer for EP0 reply data, if needed.
- * @ctrl_buff: Buffer for EP0 control requests.
- * @ctrl_req: Request for EP0 control packets.
- * @setup: NAK management for EP0 SETUP
- * @last_rst: Time of last reset
- * @eps: The endpoints being supplied to the gadget framework
- */
-struct s3c_hsotg {
-	struct device		 *dev;
-	struct usb_gadget_driver *driver;
-	struct phy		 *phy;
-	struct usb_phy		 *uphy;
-	struct s3c_hsotg_plat	 *plat;
-
-	spinlock_t              lock;
-
-	void __iomem		*regs;
-	int			irq;
-	struct clk		*clk;
-
-	struct regulator_bulk_data supplies[ARRAY_SIZE(s3c_hsotg_supply_names)];
-
-	u32			phyif;
-	unsigned int		dedicated_fifos:1;
-	unsigned char           num_of_eps;
-
-	struct dentry		*debug_root;
-	struct dentry		*debug_file;
-	struct dentry		*debug_fifo;
-
-	struct usb_request	*ep0_reply;
-	struct usb_request	*ctrl_req;
-	u8			ep0_buff[8];
-	u8			ctrl_buff[8];
-
-	struct usb_gadget	gadget;
-	unsigned int		setup;
-	unsigned long           last_rst;
-	struct s3c_hsotg_ep	*eps;
-};
-
-/**
- * struct s3c_hsotg_req - data transfer request
- * @req: The USB gadget request
- * @queue: The list of requests for the endpoint this is queued for.
- * @in_progress: Has already had size/packets written to core
- * @mapped: DMA buffer for this request has been mapped via dma_map_single().
- */
-struct s3c_hsotg_req {
-	struct usb_request	req;
-	struct list_head	queue;
-	unsigned char		in_progress;
-	unsigned char		mapped;
-};
+#include "core.h"
 
 /* conversion functions */
 static inline struct s3c_hsotg_req *our_req(struct usb_request *req)
@@ -340,9 +172,8 @@ static void s3c_hsotg_init_fifo(struct s3c_hsotg *hsotg)
 	/* set FIFO sizes to 2048/1024 */
 
 	writel(2048, hsotg->regs + GRXFSIZ);
-	writel(GNPTXFSIZ_NPTxFStAddr(2048) |
-	       GNPTXFSIZ_NPTxFDep(1024),
-	       hsotg->regs + GNPTXFSIZ);
+	writel((2048 << FIFOSIZE_STARTADDR_SHIFT) |
+		(1024 << FIFOSIZE_DEPTH_SHIFT), hsotg->regs + GNPTXFSIZ);
 
 	/*
 	 * arange all the rest of the TX FIFOs, as some versions of this
@@ -362,10 +193,10 @@ static void s3c_hsotg_init_fifo(struct s3c_hsotg *hsotg)
 
 	for (ep = 1; ep <= 15; ep++) {
 		val = addr;
-		val |= size << DPTXFSIZn_DPTxFSize_SHIFT;
+		val |= size << FIFOSIZE_DEPTH_SHIFT;
 		addr += size;
 
-		writel(val, hsotg->regs + DPTXFSIZn(ep));
+		writel(val, hsotg->regs + DPTXFSIZN(ep));
 	}
 
 	/*
@@ -373,15 +204,15 @@ static void s3c_hsotg_init_fifo(struct s3c_hsotg *hsotg)
 	 * all fifos are flushed before continuing
 	 */
 
-	writel(GRSTCTL_TxFNum(0x10) | GRSTCTL_TxFFlsh |
-	       GRSTCTL_RxFFlsh, hsotg->regs + GRSTCTL);
+	writel(GRSTCTL_TXFNUM(0x10) | GRSTCTL_TXFFLSH |
+	       GRSTCTL_RXFFLSH, hsotg->regs + GRSTCTL);
 
 	/* wait until the fifos are both flushed */
 	timeout = 100;
 	while (1) {
 		val = readl(hsotg->regs + GRSTCTL);
 
-		if ((val & (GRSTCTL_TxFFlsh | GRSTCTL_RxFFlsh)) == 0)
+		if ((val & (GRSTCTL_TXFFLSH | GRSTCTL_RXFFLSH)) == 0)
 			break;
 
 		if (--timeout == 0) {
@@ -495,14 +326,14 @@ static int s3c_hsotg_write_fifo(struct s3c_hsotg *hsotg,
 		 * how much data is left in the fifo.
 		 */
 
-		size_left = DxEPTSIZ_XferSize_GET(epsize);
+		size_left = DXEPTSIZ_XFERSIZE_GET(epsize);
 
 		/*
 		 * if shared fifo, we cannot write anything until the
 		 * previous data has been completely sent.
 		 */
 		if (hs_ep->fifo_load != 0) {
-			s3c_hsotg_en_gsint(hsotg, GINTSTS_PTxFEmp);
+			s3c_hsotg_en_gsint(hsotg, GINTSTS_PTXFEMP);
 			return -ENOSPC;
 		}
 
@@ -523,7 +354,7 @@ static int s3c_hsotg_write_fifo(struct s3c_hsotg *hsotg,
 			__func__, can_write);
 
 		if (can_write <= 0) {
-			s3c_hsotg_en_gsint(hsotg, GINTSTS_PTxFEmp);
+			s3c_hsotg_en_gsint(hsotg, GINTSTS_PTXFEMP);
 			return -ENOSPC;
 		}
 	} else if (hsotg->dedicated_fifos && hs_ep->index != 0) {
@@ -532,16 +363,16 @@ static int s3c_hsotg_write_fifo(struct s3c_hsotg *hsotg,
 		can_write &= 0xffff;
 		can_write *= 4;
 	} else {
-		if (GNPTXSTS_NPTxQSpcAvail_GET(gnptxsts) == 0) {
+		if (GNPTXSTS_NP_TXQ_SPC_AVAIL_GET(gnptxsts) == 0) {
 			dev_dbg(hsotg->dev,
 				"%s: no queue slots available (0x%08x)\n",
 				__func__, gnptxsts);
 
-			s3c_hsotg_en_gsint(hsotg, GINTSTS_NPTxFEmp);
+			s3c_hsotg_en_gsint(hsotg, GINTSTS_NPTXFEMP);
 			return -ENOSPC;
 		}
 
-		can_write = GNPTXSTS_NPTxFSpcAvail_GET(gnptxsts);
+		can_write = GNPTXSTS_NP_TXF_SPC_AVAIL_GET(gnptxsts);
 		can_write *= 4;	/* fifo size is in 32bit quantities. */
 	}
 
@@ -569,8 +400,8 @@ static int s3c_hsotg_write_fifo(struct s3c_hsotg *hsotg,
 		/* it's needed only when we do not use dedicated fifos */
 		if (!hsotg->dedicated_fifos)
 			s3c_hsotg_en_gsint(hsotg,
-					   periodic ? GINTSTS_PTxFEmp :
-					   GINTSTS_NPTxFEmp);
+					   periodic ? GINTSTS_PTXFEMP :
+					   GINTSTS_NPTXFEMP);
 	}
 
 	/* see if we can write data */
@@ -598,8 +429,8 @@ static int s3c_hsotg_write_fifo(struct s3c_hsotg *hsotg,
 		/* it's needed only when we do not use dedicated fifos */
 		if (!hsotg->dedicated_fifos)
 			s3c_hsotg_en_gsint(hsotg,
-					   periodic ? GINTSTS_PTxFEmp :
-					   GINTSTS_NPTxFEmp);
+					   periodic ? GINTSTS_PTXFEMP :
+					   GINTSTS_NPTXFEMP);
 	}
 
 	dev_dbg(hsotg->dev, "write %d/%d, can_write %d, done %d\n",
@@ -636,12 +467,12 @@ static unsigned get_ep_limit(struct s3c_hsotg_ep *hs_ep)
 	unsigned maxpkt;
 
 	if (index != 0) {
-		maxsize = DxEPTSIZ_XferSize_LIMIT + 1;
-		maxpkt = DxEPTSIZ_PktCnt_LIMIT + 1;
+		maxsize = DXEPTSIZ_XFERSIZE_LIMIT + 1;
+		maxpkt = DXEPTSIZ_PKTCNT_LIMIT + 1;
 	} else {
 		maxsize = 64+64;
 		if (hs_ep->dir_in)
-			maxpkt = DIEPTSIZ0_PktCnt_LIMIT + 1;
+			maxpkt = DIEPTSIZ0_PKTCNT_LIMIT + 1;
 		else
 			maxpkt = 2;
 	}
@@ -710,7 +541,7 @@ static void s3c_hsotg_start_req(struct s3c_hsotg *hsotg,
 	/* If endpoint is stalled, we will restart request later */
 	ctrl = readl(hsotg->regs + epctrl_reg);
 
-	if (ctrl & DxEPCTL_Stall) {
+	if (ctrl & DXEPCTL_STALL) {
 		dev_warn(hsotg->dev, "%s: ep%d is stalled\n", __func__, index);
 		return;
 	}
@@ -720,7 +551,7 @@ static void s3c_hsotg_start_req(struct s3c_hsotg *hsotg,
 		ureq->length, ureq->actual);
 	if (0)
 		dev_dbg(hsotg->dev,
-			"REQ buf %p len %d dma 0x%pad noi=%d zp=%d snok=%d\n",
+			"REQ buf %p len %d dma %pad noi=%d zp=%d snok=%d\n",
 			ureq->buf, length, &ureq->dma,
 			ureq->no_interrupt, ureq->zero, ureq->short_not_ok);
 
@@ -750,9 +581,9 @@ static void s3c_hsotg_start_req(struct s3c_hsotg *hsotg,
 
 	if (dir_in && index != 0)
 		if (hs_ep->isochronous)
-			epsize = DxEPTSIZ_MC(packets);
+			epsize = DXEPTSIZ_MC(packets);
 		else
-			epsize = DxEPTSIZ_MC(1);
+			epsize = DXEPTSIZ_MC(1);
 	else
 		epsize = 0;
 
@@ -766,8 +597,8 @@ static void s3c_hsotg_start_req(struct s3c_hsotg *hsotg,
 			packets++;
 	}
 
-	epsize |= DxEPTSIZ_PktCnt(packets);
-	epsize |= DxEPTSIZ_XferSize(length);
+	epsize |= DXEPTSIZ_PKTCNT(packets);
+	epsize |= DXEPTSIZ_XFERSIZE(length);
 
 	dev_dbg(hsotg->dev, "%s: %d@%d/%d, 0x%08x => 0x%08x\n",
 		__func__, packets, length, ureq->length, epsize, epsize_reg);
@@ -789,12 +620,12 @@ static void s3c_hsotg_start_req(struct s3c_hsotg *hsotg,
 		dma_reg = dir_in ? DIEPDMA(index) : DOEPDMA(index);
 		writel(ureq->dma, hsotg->regs + dma_reg);
 
-		dev_dbg(hsotg->dev, "%s: 0x%pad => 0x%08x\n",
+		dev_dbg(hsotg->dev, "%s: %pad => 0x%08x\n",
 			__func__, &ureq->dma, dma_reg);
 	}
 
-	ctrl |= DxEPCTL_EPEna;	/* ensure ep enabled */
-	ctrl |= DxEPCTL_USBActEp;
+	ctrl |= DXEPCTL_EPENA;	/* ensure ep enabled */
+	ctrl |= DXEPCTL_USBACTEP;
 
 	dev_dbg(hsotg->dev, "setup req:%d\n", hsotg->setup);
 
@@ -802,7 +633,7 @@ static void s3c_hsotg_start_req(struct s3c_hsotg *hsotg,
 	if (hsotg->setup && index == 0)
 		hsotg->setup = 0;
 	else
-		ctrl |= DxEPCTL_CNAK;	/* clear NAK set by core */
+		ctrl |= DXEPCTL_CNAK;	/* clear NAK set by core */
 
 
 	dev_dbg(hsotg->dev, "%s: DxEPCTL=0x%08x\n", __func__, ctrl);
@@ -828,7 +659,7 @@ static void s3c_hsotg_start_req(struct s3c_hsotg *hsotg,
 	 * to debugging to see what is going on.
 	 */
 	if (dir_in)
-		writel(DIEPMSK_INTknTXFEmpMsk,
+		writel(DIEPMSK_INTKNTXFEMPMSK,
 		       hsotg->regs + DIEPINT(index));
 
 	/*
@@ -837,12 +668,12 @@ static void s3c_hsotg_start_req(struct s3c_hsotg *hsotg,
 	 */
 
 	/* check ep is enabled */
-	if (!(readl(hsotg->regs + epctrl_reg) & DxEPCTL_EPEna))
+	if (!(readl(hsotg->regs + epctrl_reg) & DXEPCTL_EPENA))
 		dev_warn(hsotg->dev,
-			 "ep%d: failed to become enabled (DxEPCTL=0x%08x)?\n",
+			 "ep%d: failed to become enabled (DXEPCTL=0x%08x)?\n",
 			 index, readl(hsotg->regs + epctrl_reg));
 
-	dev_dbg(hsotg->dev, "%s: DxEPCTL=0x%08x\n",
+	dev_dbg(hsotg->dev, "%s: DXEPCTL=0x%08x\n",
 		__func__, readl(hsotg->regs + epctrl_reg));
 
 	/* enable ep interrupts */
@@ -1205,12 +1036,12 @@ static void s3c_hsotg_stall_ep0(struct s3c_hsotg *hsotg) {
 	 */
 
 	ctrl = readl(hsotg->regs + reg);
-	ctrl |= DxEPCTL_Stall;
-	ctrl |= DxEPCTL_CNAK;
+	ctrl |= DXEPCTL_STALL;
+	ctrl |= DXEPCTL_CNAK;
 	writel(ctrl, hsotg->regs + reg);
 
 	dev_dbg(hsotg->dev,
-		"written DxEPCTL=0x%08x to %08x (DxEPCTL=0x%08x)\n",
+		"written DXEPCTL=0x%08x to %08x (DXEPCTL=0x%08x)\n",
 		ctrl, reg, readl(hsotg->regs + reg));
 
 	 /*
@@ -1262,8 +1093,9 @@ static void s3c_hsotg_process_control(struct s3c_hsotg *hsotg,
 		case USB_REQ_SET_ADDRESS:
 			s3c_hsotg_disconnect(hsotg);
 			dcfg = readl(hsotg->regs + DCFG);
-			dcfg &= ~DCFG_DevAddr_MASK;
-			dcfg |= ctrl->wValue << DCFG_DevAddr_SHIFT;
+			dcfg &= ~DCFG_DEVADDR_MASK;
+			dcfg |= (le16_to_cpu(ctrl->wValue) <<
+				 DCFG_DEVADDR_SHIFT) & DCFG_DEVADDR_MASK;
 			writel(dcfg, hsotg->regs + DCFG);
 
 			dev_info(hsotg->dev, "new address %d\n", ctrl->wValue);
@@ -1458,7 +1290,7 @@ static void s3c_hsotg_rx_data(struct s3c_hsotg *hsotg, int ep_idx, int size)
 		int ptr;
 
 		dev_warn(hsotg->dev,
-			 "%s: FIFO %d bytes on ep%d but no req (DxEPCTl=0x%08x)\n",
+			 "%s: FIFO %d bytes on ep%d but no req (DXEPCTl=0x%08x)\n",
 			 __func__, size, ep_idx, epctl);
 
 		/* dump the data from the FIFO, we've nothing we can do */
@@ -1530,13 +1362,13 @@ static void s3c_hsotg_send_zlp(struct s3c_hsotg *hsotg,
 	dev_dbg(hsotg->dev, "sending zero-length packet\n");
 
 	/* issue a zero-sized packet to terminate this */
-	writel(DxEPTSIZ_MC(1) | DxEPTSIZ_PktCnt(1) |
-	       DxEPTSIZ_XferSize(0), hsotg->regs + DIEPTSIZ(0));
+	writel(DXEPTSIZ_MC(1) | DXEPTSIZ_PKTCNT(1) |
+	       DXEPTSIZ_XFERSIZE(0), hsotg->regs + DIEPTSIZ(0));
 
 	ctrl = readl(hsotg->regs + DIEPCTL0);
-	ctrl |= DxEPCTL_CNAK;  /* clear NAK set by core */
-	ctrl |= DxEPCTL_EPEna; /* ensure ep enabled */
-	ctrl |= DxEPCTL_USBActEp;
+	ctrl |= DXEPCTL_CNAK;  /* clear NAK set by core */
+	ctrl |= DXEPCTL_EPENA; /* ensure ep enabled */
+	ctrl |= DXEPCTL_USBACTEP;
 	writel(ctrl, hsotg->regs + DIEPCTL0);
 }
 
@@ -1557,7 +1389,7 @@ static void s3c_hsotg_handle_outdone(struct s3c_hsotg *hsotg,
 	struct s3c_hsotg_ep *hs_ep = &hsotg->eps[epnum];
 	struct s3c_hsotg_req *hs_req = hs_ep->req;
 	struct usb_request *req = &hs_req->req;
-	unsigned size_left = DxEPTSIZ_XferSize_GET(epsize);
+	unsigned size_left = DXEPTSIZ_XFERSIZE_GET(epsize);
 	int result = 0;
 
 	if (!hs_req) {
@@ -1657,24 +1489,22 @@ static void s3c_hsotg_handle_rx(struct s3c_hsotg *hsotg)
 
 	WARN_ON(using_dma(hsotg));
 
-	epnum = grxstsr & GRXSTS_EPNum_MASK;
-	status = grxstsr & GRXSTS_PktSts_MASK;
+	epnum = grxstsr & GRXSTS_EPNUM_MASK;
+	status = grxstsr & GRXSTS_PKTSTS_MASK;
 
-	size = grxstsr & GRXSTS_ByteCnt_MASK;
-	size >>= GRXSTS_ByteCnt_SHIFT;
+	size = grxstsr & GRXSTS_BYTECNT_MASK;
+	size >>= GRXSTS_BYTECNT_SHIFT;
 
 	if (1)
 		dev_dbg(hsotg->dev, "%s: GRXSTSP=0x%08x (%d@%d)\n",
 			__func__, grxstsr, size, epnum);
 
-#define __status(x) ((x) >> GRXSTS_PktSts_SHIFT)
-
-	switch (status >> GRXSTS_PktSts_SHIFT) {
-	case __status(GRXSTS_PktSts_GlobalOutNAK):
-		dev_dbg(hsotg->dev, "GlobalOutNAK\n");
+	switch ((status & GRXSTS_PKTSTS_MASK) >> GRXSTS_PKTSTS_SHIFT) {
+	case GRXSTS_PKTSTS_GLOBALOUTNAK:
+		dev_dbg(hsotg->dev, "GLOBALOUTNAK\n");
 		break;
 
-	case __status(GRXSTS_PktSts_OutDone):
+	case GRXSTS_PKTSTS_OUTDONE:
 		dev_dbg(hsotg->dev, "OutDone (Frame=0x%08x)\n",
 			s3c_hsotg_read_frameno(hsotg));
 
@@ -1682,7 +1512,7 @@ static void s3c_hsotg_handle_rx(struct s3c_hsotg *hsotg)
 			s3c_hsotg_handle_outdone(hsotg, epnum, false);
 		break;
 
-	case __status(GRXSTS_PktSts_SetupDone):
+	case GRXSTS_PKTSTS_SETUPDONE:
 		dev_dbg(hsotg->dev,
 			"SetupDone (Frame=0x%08x, DOPEPCTL=0x%08x)\n",
 			s3c_hsotg_read_frameno(hsotg),
@@ -1691,11 +1521,11 @@ static void s3c_hsotg_handle_rx(struct s3c_hsotg *hsotg)
 		s3c_hsotg_handle_outdone(hsotg, epnum, true);
 		break;
 
-	case __status(GRXSTS_PktSts_OutRX):
+	case GRXSTS_PKTSTS_OUTRX:
 		s3c_hsotg_rx_data(hsotg, epnum, size);
 		break;
 
-	case __status(GRXSTS_PktSts_SetupRX):
+	case GRXSTS_PKTSTS_SETUPRX:
 		dev_dbg(hsotg->dev,
 			"SetupRX (Frame=0x%08x, DOPEPCTL=0x%08x)\n",
 			s3c_hsotg_read_frameno(hsotg),
@@ -1761,7 +1591,7 @@ static void s3c_hsotg_set_ep_maxpacket(struct s3c_hsotg *hsotg,
 		hs_ep->ep.maxpacket = mps;
 		hs_ep->mc = 1;
 	} else {
-		mpsval = mps & DxEPCTL_MPS_MASK;
+		mpsval = mps & DXEPCTL_MPS_MASK;
 		if (mpsval > 1024)
 			goto bad_mps;
 		mcval = ((mps >> 11) & 0x3) + 1;
@@ -1777,13 +1607,13 @@ static void s3c_hsotg_set_ep_maxpacket(struct s3c_hsotg *hsotg,
 	 */
 
 	reg = readl(regs + DIEPCTL(ep));
-	reg &= ~DxEPCTL_MPS_MASK;
+	reg &= ~DXEPCTL_MPS_MASK;
 	reg |= mpsval;
 	writel(reg, regs + DIEPCTL(ep));
 
 	if (ep) {
 		reg = readl(regs + DOEPCTL(ep));
-		reg &= ~DxEPCTL_MPS_MASK;
+		reg &= ~DXEPCTL_MPS_MASK;
 		reg |= mpsval;
 		writel(reg, regs + DOEPCTL(ep));
 	}
@@ -1804,7 +1634,7 @@ static void s3c_hsotg_txfifo_flush(struct s3c_hsotg *hsotg, unsigned int idx)
 	int timeout;
 	int val;
 
-	writel(GRSTCTL_TxFNum(idx) | GRSTCTL_TxFFlsh,
+	writel(GRSTCTL_TXFNUM(idx) | GRSTCTL_TXFFLSH,
 		hsotg->regs + GRSTCTL);
 
 	/* wait until the fifo is flushed */
@@ -1813,7 +1643,7 @@ static void s3c_hsotg_txfifo_flush(struct s3c_hsotg *hsotg, unsigned int idx)
 	while (1) {
 		val = readl(hsotg->regs + GRSTCTL);
 
-		if ((val & (GRSTCTL_TxFFlsh)) == 0)
+		if ((val & (GRSTCTL_TXFFLSH)) == 0)
 			break;
 
 		if (--timeout == 0) {
@@ -1896,7 +1726,7 @@ static void s3c_hsotg_complete_in(struct s3c_hsotg *hsotg,
 	 * aligned).
 	 */
 
-	size_left = DxEPTSIZ_XferSize_GET(epsize);
+	size_left = DXEPTSIZ_XFERSIZE_GET(epsize);
 
 	size_done = hs_ep->size_loaded - size_left;
 	size_done += hs_ep->last_load;
@@ -1963,17 +1793,17 @@ static void s3c_hsotg_epint(struct s3c_hsotg *hsotg, unsigned int idx,
 	dev_dbg(hsotg->dev, "%s: ep%d(%s) DxEPINT=0x%08x\n",
 		__func__, idx, dir_in ? "in" : "out", ints);
 
-	if (ints & DxEPINT_XferCompl) {
+	if (ints & DXEPINT_XFERCOMPL) {
 		if (hs_ep->isochronous && hs_ep->interval == 1) {
-			if (ctrl & DxEPCTL_EOFrNum)
-				ctrl |= DxEPCTL_SetEvenFr;
+			if (ctrl & DXEPCTL_EOFRNUM)
+				ctrl |= DXEPCTL_SETEVENFR;
 			else
-				ctrl |= DxEPCTL_SetOddFr;
+				ctrl |= DXEPCTL_SETODDFR;
 			writel(ctrl, hsotg->regs + epctl_reg);
 		}
 
 		dev_dbg(hsotg->dev,
-			"%s: XferCompl: DxEPCTL=0x%08x, DxEPTSIZ=%08x\n",
+			"%s: XferCompl: DxEPCTL=0x%08x, DXEPTSIZ=%08x\n",
 			__func__, readl(hsotg->regs + epctl_reg),
 			readl(hsotg->regs + epsiz_reg));
 
@@ -1996,7 +1826,7 @@ static void s3c_hsotg_epint(struct s3c_hsotg *hsotg, unsigned int idx,
 		}
 	}
 
-	if (ints & DxEPINT_EPDisbld) {
+	if (ints & DXEPINT_EPDISBLD) {
 		dev_dbg(hsotg->dev, "%s: EPDisbld\n", __func__);
 
 		if (dir_in) {
@@ -2004,20 +1834,20 @@ static void s3c_hsotg_epint(struct s3c_hsotg *hsotg, unsigned int idx,
 
 			s3c_hsotg_txfifo_flush(hsotg, idx);
 
-			if ((epctl & DxEPCTL_Stall) &&
-				(epctl & DxEPCTL_EPType_Bulk)) {
+			if ((epctl & DXEPCTL_STALL) &&
+				(epctl & DXEPCTL_EPTYPE_BULK)) {
 				int dctl = readl(hsotg->regs + DCTL);
 
-				dctl |= DCTL_CGNPInNAK;
+				dctl |= DCTL_CGNPINNAK;
 				writel(dctl, hsotg->regs + DCTL);
 			}
 		}
 	}
 
-	if (ints & DxEPINT_AHBErr)
+	if (ints & DXEPINT_AHBERR)
 		dev_dbg(hsotg->dev, "%s: AHBErr\n", __func__);
 
-	if (ints & DxEPINT_Setup) {  /* Setup or Timeout */
+	if (ints & DXEPINT_SETUP) {  /* Setup or Timeout */
 		dev_dbg(hsotg->dev, "%s: Setup/Timeout\n",  __func__);
 
 		if (using_dma(hsotg) && idx == 0) {
@@ -2035,25 +1865,25 @@ static void s3c_hsotg_epint(struct s3c_hsotg *hsotg, unsigned int idx,
 		}
 	}
 
-	if (ints & DxEPINT_Back2BackSetup)
+	if (ints & DXEPINT_BACK2BACKSETUP)
 		dev_dbg(hsotg->dev, "%s: B2BSetup/INEPNakEff\n", __func__);
 
 	if (dir_in && !hs_ep->isochronous) {
 		/* not sure if this is important, but we'll clear it anyway */
-		if (ints & DIEPMSK_INTknTXFEmpMsk) {
+		if (ints & DIEPMSK_INTKNTXFEMPMSK) {
 			dev_dbg(hsotg->dev, "%s: ep%d: INTknTXFEmpMsk\n",
 				__func__, idx);
 		}
 
 		/* this probably means something bad is happening */
-		if (ints & DIEPMSK_INTknEPMisMsk) {
+		if (ints & DIEPMSK_INTKNEPMISMSK) {
 			dev_warn(hsotg->dev, "%s: ep%d: INTknEP\n",
 				 __func__, idx);
 		}
 
 		/* FIFO has space or is empty (see GAHBCFG) */
 		if (hsotg->dedicated_fifos &&
-		    ints & DIEPMSK_TxFIFOEmpty) {
+		    ints & DIEPMSK_TXFIFOEMPTY) {
 			dev_dbg(hsotg->dev, "%s: ep%d: TxFIFOEmpty\n",
 				__func__, idx);
 			if (!using_dma(hsotg))
@@ -2089,21 +1919,21 @@ static void s3c_hsotg_irq_enumdone(struct s3c_hsotg *hsotg)
 	 */
 
 	/* catch both EnumSpd_FS and EnumSpd_FS48 */
-	switch (dsts & DSTS_EnumSpd_MASK) {
-	case DSTS_EnumSpd_FS:
-	case DSTS_EnumSpd_FS48:
+	switch (dsts & DSTS_ENUMSPD_MASK) {
+	case DSTS_ENUMSPD_FS:
+	case DSTS_ENUMSPD_FS48:
 		hsotg->gadget.speed = USB_SPEED_FULL;
 		ep0_mps = EP0_MPS_LIMIT;
 		ep_mps = 1023;
 		break;
 
-	case DSTS_EnumSpd_HS:
+	case DSTS_ENUMSPD_HS:
 		hsotg->gadget.speed = USB_SPEED_HIGH;
 		ep0_mps = EP0_MPS_LIMIT;
 		ep_mps = 1024;
 		break;
 
-	case DSTS_EnumSpd_LS:
+	case DSTS_ENUMSPD_LS:
 		hsotg->gadget.speed = USB_SPEED_LOW;
 		/*
 		 * note, we don't actually support LS in this driver at the
@@ -2169,16 +1999,6 @@ static void kill_all_requests(struct s3c_hsotg *hsotg,
 			s3c_hsotg_txfifo_flush(hsotg, ep->index);
 }
 
-#define call_gadget(_hs, _entry) \
-do { \
-	if ((_hs)->gadget.speed != USB_SPEED_UNKNOWN &&	\
-	    (_hs)->driver && (_hs)->driver->_entry) { \
-		spin_unlock(&_hs->lock); \
-		(_hs)->driver->_entry(&(_hs)->gadget); \
-		spin_lock(&_hs->lock); \
-	} \
-} while (0)
-
 /**
  * s3c_hsotg_disconnect - disconnect service
  * @hsotg: The device state.
@@ -2226,9 +2046,9 @@ static void s3c_hsotg_irq_fifoempty(struct s3c_hsotg *hsotg, bool periodic)
 }
 
 /* IRQ flags which will trigger a retry around the IRQ loop */
-#define IRQ_RETRY_MASK (GINTSTS_NPTxFEmp | \
-			GINTSTS_PTxFEmp |  \
-			GINTSTS_RxFLvl)
+#define IRQ_RETRY_MASK (GINTSTS_NPTXFEMP | \
+			GINTSTS_PTXFEMP |  \
+			GINTSTS_RXFLVL)
 
 /**
  * s3c_hsotg_corereset - issue softreset to the core
@@ -2244,14 +2064,14 @@ static int s3c_hsotg_corereset(struct s3c_hsotg *hsotg)
 	dev_dbg(hsotg->dev, "resetting core\n");
 
 	/* issue soft reset */
-	writel(GRSTCTL_CSftRst, hsotg->regs + GRSTCTL);
+	writel(GRSTCTL_CSFTRST, hsotg->regs + GRSTCTL);
 
 	timeout = 10000;
 	do {
 		grstctl = readl(hsotg->regs + GRSTCTL);
-	} while ((grstctl & GRSTCTL_CSftRst) && timeout-- > 0);
+	} while ((grstctl & GRSTCTL_CSFTRST) && timeout-- > 0);
 
-	if (grstctl & GRSTCTL_CSftRst) {
+	if (grstctl & GRSTCTL_CSFTRST) {
 		dev_err(hsotg->dev, "Failed to get CSftRst asserted\n");
 		return -EINVAL;
 	}
@@ -2268,7 +2088,7 @@ static int s3c_hsotg_corereset(struct s3c_hsotg *hsotg)
 			return -ETIMEDOUT;
 		}
 
-		if (!(grstctl & GRSTCTL_AHBIdle))
+		if (!(grstctl & GRSTCTL_AHBIDLE))
 			continue;
 
 		break;		/* reset done */
@@ -2294,14 +2114,14 @@ static void s3c_hsotg_core_init(struct s3c_hsotg *hsotg)
 	 */
 
 	/* set the PLL on, remove the HNP/SRP and set the PHY */
-	writel(hsotg->phyif | GUSBCFG_TOutCal(7) |
+	writel(hsotg->phyif | GUSBCFG_TOUTCAL(7) |
 	       (0x5 << 10), hsotg->regs + GUSBCFG);
 
 	s3c_hsotg_init_fifo(hsotg);
 
-	__orr32(hsotg->regs + DCTL, DCTL_SftDiscon);
+	__orr32(hsotg->regs + DCTL, DCTL_SFTDISCON);
 
-	writel(1 << 18 | DCFG_DevSpd_HS,  hsotg->regs + DCFG);
+	writel(1 << 18 | DCFG_DEVSPD_HS,  hsotg->regs + DCFG);
 
 	/* Clear any pending OTG interrupts */
 	writel(0xffffffff, hsotg->regs + GOTGINT);
@@ -2309,21 +2129,21 @@ static void s3c_hsotg_core_init(struct s3c_hsotg *hsotg)
 	/* Clear any pending interrupts */
 	writel(0xffffffff, hsotg->regs + GINTSTS);
 
-	writel(GINTSTS_ErlySusp | GINTSTS_SessReqInt |
-	       GINTSTS_GOUTNakEff | GINTSTS_GINNakEff |
-	       GINTSTS_ConIDStsChng | GINTSTS_USBRst |
-	       GINTSTS_EnumDone | GINTSTS_OTGInt |
-	       GINTSTS_USBSusp | GINTSTS_WkUpInt,
-	       hsotg->regs + GINTMSK);
+	writel(GINTSTS_ERLYSUSP | GINTSTS_SESSREQINT |
+		GINTSTS_GOUTNAKEFF | GINTSTS_GINNAKEFF |
+		GINTSTS_CONIDSTSCHNG | GINTSTS_USBRST |
+		GINTSTS_ENUMDONE | GINTSTS_OTGINT |
+		GINTSTS_USBSUSP | GINTSTS_WKUPINT,
+		hsotg->regs + GINTMSK);
 
 	if (using_dma(hsotg))
-		writel(GAHBCFG_GlblIntrEn | GAHBCFG_DMAEn |
-		       GAHBCFG_HBstLen_Incr4,
+		writel(GAHBCFG_GLBL_INTR_EN | GAHBCFG_DMA_EN |
+		       GAHBCFG_HBSTLEN_INCR4,
 		       hsotg->regs + GAHBCFG);
 	else
-		writel(((hsotg->dedicated_fifos) ? (GAHBCFG_NPTxFEmpLvl |
-						    GAHBCFG_PTxFEmpLvl) : 0) |
-		       GAHBCFG_GlblIntrEn,
+		writel(((hsotg->dedicated_fifos) ? (GAHBCFG_NP_TXF_EMP_LVL |
+						    GAHBCFG_P_TXF_EMP_LVL) : 0) |
+		       GAHBCFG_GLBL_INTR_EN,
 		       hsotg->regs + GAHBCFG);
 
 	/*
@@ -2332,22 +2152,22 @@ static void s3c_hsotg_core_init(struct s3c_hsotg *hsotg)
 	 * interrupts.
 	 */
 
-	writel(((hsotg->dedicated_fifos) ? DIEPMSK_TxFIFOEmpty |
-	       DIEPMSK_INTknTXFEmpMsk : 0) |
-	       DIEPMSK_EPDisbldMsk | DIEPMSK_XferComplMsk |
-	       DIEPMSK_TimeOUTMsk | DIEPMSK_AHBErrMsk |
-	       DIEPMSK_INTknEPMisMsk,
-	       hsotg->regs + DIEPMSK);
+	writel(((hsotg->dedicated_fifos) ? DIEPMSK_TXFIFOEMPTY |
+		DIEPMSK_INTKNTXFEMPMSK : 0) |
+		DIEPMSK_EPDISBLDMSK | DIEPMSK_XFERCOMPLMSK |
+		DIEPMSK_TIMEOUTMSK | DIEPMSK_AHBERRMSK |
+		DIEPMSK_INTKNEPMISMSK,
+		hsotg->regs + DIEPMSK);
 
 	/*
 	 * don't need XferCompl, we get that from RXFIFO in slave mode. In
 	 * DMA mode we may need this.
 	 */
-	writel((using_dma(hsotg) ? (DIEPMSK_XferComplMsk |
-				    DIEPMSK_TimeOUTMsk) : 0) |
-	       DOEPMSK_EPDisbldMsk | DOEPMSK_AHBErrMsk |
-	       DOEPMSK_SetupMsk,
-	       hsotg->regs + DOEPMSK);
+	writel((using_dma(hsotg) ? (DIEPMSK_XFERCOMPLMSK |
+				    DIEPMSK_TIMEOUTMSK) : 0) |
+		DOEPMSK_EPDISBLDMSK | DOEPMSK_AHBERRMSK |
+		DOEPMSK_SETUPMSK,
+		hsotg->regs + DOEPMSK);
 
 	writel(0, hsotg->regs + DAINTMSK);
 
@@ -2356,7 +2176,7 @@ static void s3c_hsotg_core_init(struct s3c_hsotg *hsotg)
 		readl(hsotg->regs + DOEPCTL0));
 
 	/* enable in and out endpoint interrupts */
-	s3c_hsotg_en_gsint(hsotg, GINTSTS_OEPInt | GINTSTS_IEPInt);
+	s3c_hsotg_en_gsint(hsotg, GINTSTS_OEPINT | GINTSTS_IEPINT);
 
 	/*
 	 * Enable the RXFIFO when in slave mode, as this is how we collect
@@ -2364,15 +2184,15 @@ static void s3c_hsotg_core_init(struct s3c_hsotg *hsotg)
 	 * things we cannot process, so do not use it.
 	 */
 	if (!using_dma(hsotg))
-		s3c_hsotg_en_gsint(hsotg, GINTSTS_RxFLvl);
+		s3c_hsotg_en_gsint(hsotg, GINTSTS_RXFLVL);
 
 	/* Enable interrupts for EP0 in and out */
 	s3c_hsotg_ctrl_epint(hsotg, 0, 0, 1);
 	s3c_hsotg_ctrl_epint(hsotg, 0, 1, 1);
 
-	__orr32(hsotg->regs + DCTL, DCTL_PWROnPrgDone);
+	__orr32(hsotg->regs + DCTL, DCTL_PWRONPRGDONE);
 	udelay(10);  /* see openiboot */
-	__bic32(hsotg->regs + DCTL, DCTL_PWROnPrgDone);
+	__bic32(hsotg->regs + DCTL, DCTL_PWRONPRGDONE);
 
 	dev_dbg(hsotg->dev, "DCTL=0x%08x\n", readl(hsotg->regs + DCTL));
 
@@ -2382,17 +2202,17 @@ static void s3c_hsotg_core_init(struct s3c_hsotg *hsotg)
 	 */
 
 	/* set to read 1 8byte packet */
-	writel(DxEPTSIZ_MC(1) | DxEPTSIZ_PktCnt(1) |
-	       DxEPTSIZ_XferSize(8), hsotg->regs + DOEPTSIZ0);
+	writel(DXEPTSIZ_MC(1) | DXEPTSIZ_PKTCNT(1) |
+	       DXEPTSIZ_XFERSIZE(8), hsotg->regs + DOEPTSIZ0);
 
 	writel(s3c_hsotg_ep0_mps(hsotg->eps[0].ep.maxpacket) |
-	       DxEPCTL_CNAK | DxEPCTL_EPEna |
-	       DxEPCTL_USBActEp,
+	       DXEPCTL_CNAK | DXEPCTL_EPENA |
+	       DXEPCTL_USBACTEP,
 	       hsotg->regs + DOEPCTL0);
 
 	/* enable, but don't activate EP0in */
 	writel(s3c_hsotg_ep0_mps(hsotg->eps[0].ep.maxpacket) |
-	       DxEPCTL_USBActEp, hsotg->regs + DIEPCTL0);
+	       DXEPCTL_USBACTEP, hsotg->regs + DIEPCTL0);
 
 	s3c_hsotg_enqueue_setup(hsotg);
 
@@ -2401,14 +2221,14 @@ static void s3c_hsotg_core_init(struct s3c_hsotg *hsotg)
 		readl(hsotg->regs + DOEPCTL0));
 
 	/* clear global NAKs */
-	writel(DCTL_CGOUTNak | DCTL_CGNPInNAK,
+	writel(DCTL_CGOUTNAK | DCTL_CGNPINNAK,
 	       hsotg->regs + DCTL);
 
 	/* must be at-least 3ms to allow bus to see disconnect */
 	mdelay(3);
 
 	/* remove the soft-disconnect and let's go */
-	__bic32(hsotg->regs + DCTL, DCTL_SftDiscon);
+	__bic32(hsotg->regs + DCTL, DCTL_SFTDISCON);
 }
 
 /**
@@ -2433,7 +2253,7 @@ irq_retry:
 
 	gintsts &= gintmsk;
 
-	if (gintsts & GINTSTS_OTGInt) {
+	if (gintsts & GINTSTS_OTGINT) {
 		u32 otgint = readl(hsotg->regs + GOTGINT);
 
 		dev_info(hsotg->dev, "OTGInt: %08x\n", otgint);
@@ -2441,34 +2261,34 @@ irq_retry:
 		writel(otgint, hsotg->regs + GOTGINT);
 	}
 
-	if (gintsts & GINTSTS_SessReqInt) {
+	if (gintsts & GINTSTS_SESSREQINT) {
 		dev_dbg(hsotg->dev, "%s: SessReqInt\n", __func__);
-		writel(GINTSTS_SessReqInt, hsotg->regs + GINTSTS);
+		writel(GINTSTS_SESSREQINT, hsotg->regs + GINTSTS);
 	}
 
-	if (gintsts & GINTSTS_EnumDone) {
-		writel(GINTSTS_EnumDone, hsotg->regs + GINTSTS);
+	if (gintsts & GINTSTS_ENUMDONE) {
+		writel(GINTSTS_ENUMDONE, hsotg->regs + GINTSTS);
 
 		s3c_hsotg_irq_enumdone(hsotg);
 	}
 
-	if (gintsts & GINTSTS_ConIDStsChng) {
+	if (gintsts & GINTSTS_CONIDSTSCHNG) {
 		dev_dbg(hsotg->dev, "ConIDStsChg (DSTS=0x%08x, GOTCTL=%08x)\n",
 			readl(hsotg->regs + DSTS),
 			readl(hsotg->regs + GOTGCTL));
 
-		writel(GINTSTS_ConIDStsChng, hsotg->regs + GINTSTS);
+		writel(GINTSTS_CONIDSTSCHNG, hsotg->regs + GINTSTS);
 	}
 
-	if (gintsts & (GINTSTS_OEPInt | GINTSTS_IEPInt)) {
+	if (gintsts & (GINTSTS_OEPINT | GINTSTS_IEPINT)) {
 		u32 daint = readl(hsotg->regs + DAINT);
 		u32 daintmsk = readl(hsotg->regs + DAINTMSK);
 		u32 daint_out, daint_in;
 		int ep;
 
 		daint &= daintmsk;
-		daint_out = daint >> DAINT_OutEP_SHIFT;
-		daint_in = daint & ~(daint_out << DAINT_OutEP_SHIFT);
+		daint_out = daint >> DAINT_OUTEP_SHIFT;
+		daint_in = daint & ~(daint_out << DAINT_OUTEP_SHIFT);
 
 		dev_dbg(hsotg->dev, "%s: daint=%08x\n", __func__, daint);
 
@@ -2483,7 +2303,7 @@ irq_retry:
 		}
 	}
 
-	if (gintsts & GINTSTS_USBRst) {
+	if (gintsts & GINTSTS_USBRST) {
 
 		u32 usb_status = readl(hsotg->regs + GOTGCTL);
 
@@ -2491,7 +2311,7 @@ irq_retry:
 		dev_dbg(hsotg->dev, "GNPTXSTS=%08x\n",
 			readl(hsotg->regs + GNPTXSTS));
 
-		writel(GINTSTS_USBRst, hsotg->regs + GINTSTS);
+		writel(GINTSTS_USBRST, hsotg->regs + GINTSTS);
 
 		if (usb_status & GOTGCTL_BSESVLD) {
 			if (time_after(jiffies, hsotg->last_rst +
@@ -2508,7 +2328,7 @@ irq_retry:
 
 	/* check both FIFOs */
 
-	if (gintsts & GINTSTS_NPTxFEmp) {
+	if (gintsts & GINTSTS_NPTXFEMP) {
 		dev_dbg(hsotg->dev, "NPTxFEmp\n");
 
 		/*
@@ -2517,20 +2337,20 @@ irq_retry:
 		 * it needs re-enabling
 		 */
 
-		s3c_hsotg_disable_gsint(hsotg, GINTSTS_NPTxFEmp);
+		s3c_hsotg_disable_gsint(hsotg, GINTSTS_NPTXFEMP);
 		s3c_hsotg_irq_fifoempty(hsotg, false);
 	}
 
-	if (gintsts & GINTSTS_PTxFEmp) {
+	if (gintsts & GINTSTS_PTXFEMP) {
 		dev_dbg(hsotg->dev, "PTxFEmp\n");
 
 		/* See note in GINTSTS_NPTxFEmp */
 
-		s3c_hsotg_disable_gsint(hsotg, GINTSTS_PTxFEmp);
+		s3c_hsotg_disable_gsint(hsotg, GINTSTS_PTXFEMP);
 		s3c_hsotg_irq_fifoempty(hsotg, true);
 	}
 
-	if (gintsts & GINTSTS_RxFLvl) {
+	if (gintsts & GINTSTS_RXFLVL) {
 		/*
 		 * note, since GINTSTS_RxFLvl doubles as FIFO-not-empty,
 		 * we need to retry s3c_hsotg_handle_rx if this is still
@@ -2540,28 +2360,28 @@ irq_retry:
 		s3c_hsotg_handle_rx(hsotg);
 	}
 
-	if (gintsts & GINTSTS_ModeMis) {
+	if (gintsts & GINTSTS_MODEMIS) {
 		dev_warn(hsotg->dev, "warning, mode mismatch triggered\n");
-		writel(GINTSTS_ModeMis, hsotg->regs + GINTSTS);
+		writel(GINTSTS_MODEMIS, hsotg->regs + GINTSTS);
 	}
 
-	if (gintsts & GINTSTS_USBSusp) {
+	if (gintsts & GINTSTS_USBSUSP) {
 		dev_info(hsotg->dev, "GINTSTS_USBSusp\n");
-		writel(GINTSTS_USBSusp, hsotg->regs + GINTSTS);
+		writel(GINTSTS_USBSUSP, hsotg->regs + GINTSTS);
 
 		call_gadget(hsotg, suspend);
 	}
 
-	if (gintsts & GINTSTS_WkUpInt) {
+	if (gintsts & GINTSTS_WKUPINT) {
 		dev_info(hsotg->dev, "GINTSTS_WkUpIn\n");
-		writel(GINTSTS_WkUpInt, hsotg->regs + GINTSTS);
+		writel(GINTSTS_WKUPINT, hsotg->regs + GINTSTS);
 
 		call_gadget(hsotg, resume);
 	}
 
-	if (gintsts & GINTSTS_ErlySusp) {
+	if (gintsts & GINTSTS_ERLYSUSP) {
 		dev_dbg(hsotg->dev, "GINTSTS_ErlySusp\n");
-		writel(GINTSTS_ErlySusp, hsotg->regs + GINTSTS);
+		writel(GINTSTS_ERLYSUSP, hsotg->regs + GINTSTS);
 	}
 
 	/*
@@ -2570,18 +2390,18 @@ irq_retry:
 	 * the occurrence.
 	 */
 
-	if (gintsts & GINTSTS_GOUTNakEff) {
+	if (gintsts & GINTSTS_GOUTNAKEFF) {
 		dev_info(hsotg->dev, "GOUTNakEff triggered\n");
 
-		writel(DCTL_CGOUTNak, hsotg->regs + DCTL);
+		writel(DCTL_CGOUTNAK, hsotg->regs + DCTL);
 
 		s3c_hsotg_dump(hsotg);
 	}
 
-	if (gintsts & GINTSTS_GINNakEff) {
+	if (gintsts & GINTSTS_GINNAKEFF) {
 		dev_info(hsotg->dev, "GINNakEff triggered\n");
 
-		writel(DCTL_CGNPInNAK, hsotg->regs + DCTL);
+		writel(DCTL_CGNPINNAK, hsotg->regs + DCTL);
 
 		s3c_hsotg_dump(hsotg);
 	}
@@ -2645,14 +2465,14 @@ static int s3c_hsotg_ep_enable(struct usb_ep *ep,
 
 	spin_lock_irqsave(&hsotg->lock, flags);
 
-	epctrl &= ~(DxEPCTL_EPType_MASK | DxEPCTL_MPS_MASK);
-	epctrl |= DxEPCTL_MPS(mps);
+	epctrl &= ~(DXEPCTL_EPTYPE_MASK | DXEPCTL_MPS_MASK);
+	epctrl |= DXEPCTL_MPS(mps);
 
 	/*
 	 * mark the endpoint as active, otherwise the core may ignore
 	 * transactions entirely for this endpoint
 	 */
-	epctrl |= DxEPCTL_USBActEp;
+	epctrl |= DXEPCTL_USBACTEP;
 
 	/*
 	 * set the NAK status on the endpoint, otherwise we might try and
@@ -2661,7 +2481,7 @@ static int s3c_hsotg_ep_enable(struct usb_ep *ep,
 	 * size register hasn't been set.
 	 */
 
-	epctrl |= DxEPCTL_SNAK;
+	epctrl |= DXEPCTL_SNAK;
 
 	/* update the endpoint state */
 	s3c_hsotg_set_ep_maxpacket(hsotg, hs_ep->index, mps);
@@ -2677,15 +2497,15 @@ static int s3c_hsotg_ep_enable(struct usb_ep *ep,
 
 	switch (desc->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK) {
 	case USB_ENDPOINT_XFER_ISOC:
-		epctrl |= DxEPCTL_EPType_Iso;
-		epctrl |= DxEPCTL_SetEvenFr;
+		epctrl |= DXEPCTL_EPTYPE_ISO;
+		epctrl |= DXEPCTL_SETEVENFR;
 		hs_ep->isochronous = 1;
 		if (dir_in)
 			hs_ep->periodic = 1;
 		break;
 
 	case USB_ENDPOINT_XFER_BULK:
-		epctrl |= DxEPCTL_EPType_Bulk;
+		epctrl |= DXEPCTL_EPTYPE_BULK;
 		break;
 
 	case USB_ENDPOINT_XFER_INT:
@@ -2698,14 +2518,14 @@ static int s3c_hsotg_ep_enable(struct usb_ep *ep,
 			 */
 
 			hs_ep->periodic = 1;
-			epctrl |= DxEPCTL_TxFNum(index);
+			epctrl |= DXEPCTL_TXFNUM(index);
 		}
 
-		epctrl |= DxEPCTL_EPType_Intterupt;
+		epctrl |= DXEPCTL_EPTYPE_INTERRUPT;
 		break;
 
 	case USB_ENDPOINT_XFER_CONTROL:
-		epctrl |= DxEPCTL_EPType_Control;
+		epctrl |= DXEPCTL_EPTYPE_CONTROL;
 		break;
 	}
 
@@ -2714,11 +2534,11 @@ static int s3c_hsotg_ep_enable(struct usb_ep *ep,
 	 * a unique tx-fifo even if it is non-periodic.
 	 */
 	if (dir_in && hsotg->dedicated_fifos)
-		epctrl |= DxEPCTL_TxFNum(index);
+		epctrl |= DXEPCTL_TXFNUM(index);
 
 	/* for non control endpoints, set PID to D0 */
 	if (index)
-		epctrl |= DxEPCTL_SetD0PID;
+		epctrl |= DXEPCTL_SETD0PID;
 
 	dev_dbg(hsotg->dev, "%s: write DxEPCTL=0x%08x\n",
 		__func__, epctrl);
@@ -2763,9 +2583,9 @@ static int s3c_hsotg_ep_disable(struct usb_ep *ep)
 
 
 	ctrl = readl(hsotg->regs + epctrl_reg);
-	ctrl &= ~DxEPCTL_EPEna;
-	ctrl &= ~DxEPCTL_USBActEp;
-	ctrl |= DxEPCTL_SNAK;
+	ctrl &= ~DXEPCTL_EPENA;
+	ctrl &= ~DXEPCTL_USBACTEP;
+	ctrl |= DXEPCTL_SNAK;
 
 	dev_dbg(hsotg->dev, "%s: DxEPCTL=0x%08x\n", __func__, ctrl);
 	writel(ctrl, hsotg->regs + epctrl_reg);
@@ -2852,15 +2672,15 @@ static int s3c_hsotg_ep_sethalt(struct usb_ep *ep, int value)
 	epctl = readl(hs->regs + epreg);
 
 	if (value) {
-		epctl |= DxEPCTL_Stall + DxEPCTL_SNAK;
-		if (epctl & DxEPCTL_EPEna)
-			epctl |= DxEPCTL_EPDis;
+		epctl |= DXEPCTL_STALL + DXEPCTL_SNAK;
+		if (epctl & DXEPCTL_EPENA)
+			epctl |= DXEPCTL_EPDIS;
 	} else {
-		epctl &= ~DxEPCTL_Stall;
-		xfertype = epctl & DxEPCTL_EPType_MASK;
-		if (xfertype == DxEPCTL_EPType_Bulk ||
-			xfertype == DxEPCTL_EPType_Intterupt)
-				epctl |= DxEPCTL_SetD0PID;
+		epctl &= ~DXEPCTL_STALL;
+		xfertype = epctl & DXEPCTL_EPTYPE_MASK;
+		if (xfertype == DXEPCTL_EPTYPE_BULK ||
+			xfertype == DXEPCTL_EPTYPE_INTERRUPT)
+				epctl |= DXEPCTL_SETD0PID;
 	}
 
 	writel(epctl, hs->regs + epreg);
@@ -2869,13 +2689,13 @@ static int s3c_hsotg_ep_sethalt(struct usb_ep *ep, int value)
 	epctl = readl(hs->regs + epreg);
 
 	if (value)
-		epctl |= DxEPCTL_Stall;
+		epctl |= DXEPCTL_STALL;
 	else {
-		epctl &= ~DxEPCTL_Stall;
-		xfertype = epctl & DxEPCTL_EPType_MASK;
-		if (xfertype == DxEPCTL_EPType_Bulk ||
-			xfertype == DxEPCTL_EPType_Intterupt)
-				epctl |= DxEPCTL_SetD0PID;
+		epctl &= ~DXEPCTL_STALL;
+		xfertype = epctl & DXEPCTL_EPTYPE_MASK;
+		if (xfertype == DXEPCTL_EPTYPE_BULK ||
+			xfertype == DXEPCTL_EPTYPE_INTERRUPT)
+				epctl |= DXEPCTL_SETD0PID;
 	}
 
 	writel(epctl, hs->regs + epreg);
@@ -2965,22 +2785,22 @@ static void s3c_hsotg_init(struct s3c_hsotg *hsotg)
 {
 	/* unmask subset of endpoint interrupts */
 
-	writel(DIEPMSK_TimeOUTMsk | DIEPMSK_AHBErrMsk |
-	       DIEPMSK_EPDisbldMsk | DIEPMSK_XferComplMsk,
-	       hsotg->regs + DIEPMSK);
+	writel(DIEPMSK_TIMEOUTMSK | DIEPMSK_AHBERRMSK |
+		DIEPMSK_EPDISBLDMSK | DIEPMSK_XFERCOMPLMSK,
+		hsotg->regs + DIEPMSK);
 
-	writel(DOEPMSK_SetupMsk | DOEPMSK_AHBErrMsk |
-	       DOEPMSK_EPDisbldMsk | DOEPMSK_XferComplMsk,
-	       hsotg->regs + DOEPMSK);
+	writel(DOEPMSK_SETUPMSK | DOEPMSK_AHBERRMSK |
+		DOEPMSK_EPDISBLDMSK | DOEPMSK_XFERCOMPLMSK,
+		hsotg->regs + DOEPMSK);
 
 	writel(0, hsotg->regs + DAINTMSK);
 
 	/* Be in disconnected state until gadget is registered */
-	__orr32(hsotg->regs + DCTL, DCTL_SftDiscon);
+	__orr32(hsotg->regs + DCTL, DCTL_SFTDISCON);
 
 	if (0) {
 		/* post global nak until we're ready */
-		writel(DCTL_SGNPInNAK | DCTL_SGOUTNak,
+		writel(DCTL_SGNPINNAK | DCTL_SGOUTNAK,
 		       hsotg->regs + DCTL);
 	}
 
@@ -2993,10 +2813,10 @@ static void s3c_hsotg_init(struct s3c_hsotg *hsotg)
 	s3c_hsotg_init_fifo(hsotg);
 
 	/* set the PLL on, remove the HNP/SRP and set the PHY */
-	writel(GUSBCFG_PHYIf16 | GUSBCFG_TOutCal(7) | (0x5 << 10),
+	writel(GUSBCFG_PHYIF16 | GUSBCFG_TOUTCAL(7) | (0x5 << 10),
 	       hsotg->regs + GUSBCFG);
 
-	writel(using_dma(hsotg) ? GAHBCFG_DMAEn : 0x0,
+	writel(using_dma(hsotg) ? GAHBCFG_DMA_EN : 0x0,
 	       hsotg->regs + GAHBCFG);
 }
 
@@ -3187,8 +3007,8 @@ static void s3c_hsotg_initep(struct s3c_hsotg *hsotg,
 	 * code is changed to make each endpoint's direction changeable.
 	 */
 
-	ptxfifo = readl(hsotg->regs + DPTXFSIZn(epnum));
-	hs_ep->fifo_size = DPTXFSIZn_DPTxFSize_GET(ptxfifo) * 4;
+	ptxfifo = readl(hsotg->regs + DPTXFSIZN(epnum));
+	hs_ep->fifo_size = FIFOSIZE_DEPTH_GET(ptxfifo) * 4;
 
 	/*
 	 * if we're using dma, we need to set the next-endpoint pointer
@@ -3196,7 +3016,7 @@ static void s3c_hsotg_initep(struct s3c_hsotg *hsotg,
 	 */
 
 	if (using_dma(hsotg)) {
-		u32 next = DxEPCTL_NextEp((epnum + 1) % 15);
+		u32 next = DXEPCTL_NEXTEP((epnum + 1) % 15);
 		writel(next, hsotg->regs + DIEPCTL(epnum));
 		writel(next, hsotg->regs + DOEPCTL(epnum));
 	}
@@ -3250,10 +3070,10 @@ static void s3c_hsotg_dump(struct s3c_hsotg *hsotg)
 	/* show periodic fifo settings */
 
 	for (idx = 1; idx <= 15; idx++) {
-		val = readl(regs + DPTXFSIZn(idx));
+		val = readl(regs + DPTXFSIZN(idx));
 		dev_info(dev, "DPTx[%d] FSize=%d, StAddr=0x%08x\n", idx,
-			 val >> DPTXFSIZn_DPTxFSize_SHIFT,
-			 val & DPTXFSIZn_DPTxFStAddr_MASK);
+			 val >> FIFOSIZE_DEPTH_SHIFT,
+			 val & FIFOSIZE_STARTADDR_MASK);
 	}
 
 	for (idx = 0; idx < 15; idx++) {
@@ -3368,17 +3188,17 @@ static int fifo_show(struct seq_file *seq, void *v)
 
 	val = readl(regs + GNPTXFSIZ);
 	seq_printf(seq, "NPTXFIFO: Size %d, Start 0x%08x\n",
-		   val >> GNPTXFSIZ_NPTxFDep_SHIFT,
-		   val & GNPTXFSIZ_NPTxFStAddr_MASK);
+		   val >> FIFOSIZE_DEPTH_SHIFT,
+		   val & FIFOSIZE_DEPTH_MASK);
 
 	seq_puts(seq, "\nPeriodic TXFIFOs:\n");
 
 	for (idx = 1; idx <= 15; idx++) {
-		val = readl(regs + DPTXFSIZn(idx));
+		val = readl(regs + DPTXFSIZN(idx));
 
 		seq_printf(seq, "\tDPTXFIFO%2d: Size %d, Start 0x%08x\n", idx,
-			   val >> DPTXFSIZn_DPTxFSize_SHIFT,
-			   val & DPTXFSIZn_DPTxFStAddr_MASK);
+			   val >> FIFOSIZE_DEPTH_SHIFT,
+			   val & FIFOSIZE_STARTADDR_MASK);
 	}
 
 	return 0;
@@ -3662,14 +3482,14 @@ static int s3c_hsotg_probe(struct platform_device *pdev)
 	}
 
 	/* Set default UTMI width */
-	hsotg->phyif = GUSBCFG_PHYIf16;
+	hsotg->phyif = GUSBCFG_PHYIF16;
 
 	/*
 	 * If using the generic PHY framework, check if the PHY bus
 	 * width is 8-bit and set the phyif appropriately.
 	 */
 	if (hsotg->phy && (phy_get_bus_width(phy) == 8))
-		hsotg->phyif = GUSBCFG_PHYIf8;
+		hsotg->phyif = GUSBCFG_PHYIF8;
 
 	if (hsotg->phy)
 		phy_init(hsotg->phy);
diff --git a/drivers/usb/dwc2/hw.h b/drivers/usb/dwc2/hw.h
index 9c92a3c7588..51248b93549 100644
--- a/drivers/usb/dwc2/hw.h
+++ b/drivers/usb/dwc2/hw.h
@@ -109,6 +109,7 @@
 #define GUSBCFG_FSINTF			(1 << 5)
 #define GUSBCFG_ULPI_UTMI_SEL		(1 << 4)
 #define GUSBCFG_PHYIF16			(1 << 3)
+#define GUSBCFG_PHYIF8			(0 << 3)
 #define GUSBCFG_TOUTCAL_MASK		(0x7 << 0)
 #define GUSBCFG_TOUTCAL_SHIFT		0
 #define GUSBCFG_TOUTCAL_LIMIT		0x7
@@ -403,6 +404,7 @@
 #define FIFOSIZE_DEPTH_SHIFT		16
 #define FIFOSIZE_STARTADDR_MASK		(0xffff << 0)
 #define FIFOSIZE_STARTADDR_SHIFT	0
+#define FIFOSIZE_DEPTH_GET(_x)		(((_x) >> 16) & 0xffff)
 
 /* Device mode registers */
 
@@ -519,11 +521,11 @@
 #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
-#define DXEPCTL_EPTYPE_ISO		1
-#define DXEPCTL_EPTYPE_BULK		2
-#define DXEPCTL_EPTYPE_INTTERUPT	3
+#define DXEPCTL_EPTYPE_CONTROL		(0x0 << 18)
+#define DXEPCTL_EPTYPE_ISO		(0x1 << 18)
+#define DXEPCTL_EPTYPE_BULK		(0x2 << 18)
+#define DXEPCTL_EPTYPE_INTERRUPT	(0x3 << 18)
+
 #define DXEPCTL_NAKSTS			(1 << 17)
 #define DXEPCTL_DPID			(1 << 16)
 #define DXEPCTL_EOFRNUM			(1 << 16)
diff --git a/drivers/usb/dwc2/platform.c b/drivers/usb/dwc2/platform.c
index eaba547ce26..a10e7a35357 100644
--- a/drivers/usb/dwc2/platform.c
+++ b/drivers/usb/dwc2/platform.c
@@ -134,6 +134,12 @@ static int dwc2_driver_probe(struct platform_device *dev)
 		/* Default all params to autodetect */
 		dwc2_set_all_params(&defparams, -1);
 		params = &defparams;
+
+		/*
+		 * Disable descriptor dma mode by default as the HW can support
+		 * it, but does not support it for SPLIT transactions.
+		 */
+		defparams.dma_desc_enable = 0;
 	}
 
 	hsotg = devm_kzalloc(&dev->dev, sizeof(*hsotg), GFP_KERNEL);
diff --git a/drivers/usb/dwc3/Kconfig b/drivers/usb/dwc3/Kconfig
index e2c730fc9a9..8eb996e4f05 100644
--- a/drivers/usb/dwc3/Kconfig
+++ b/drivers/usb/dwc3/Kconfig
@@ -44,7 +44,7 @@ comment "Platform Glue Driver Support"
 
 config USB_DWC3_OMAP
 	tristate "Texas Instruments OMAP5 and similar Platforms"
-	depends on EXTCON
+	depends on EXTCON && (ARCH_OMAP2PLUS || COMPILE_TEST)
 	default USB_DWC3
 	help
 	  Some platforms from Texas Instruments like OMAP5, DRA7xxx and
@@ -54,6 +54,7 @@ config USB_DWC3_OMAP
 
 config USB_DWC3_EXYNOS
 	tristate "Samsung Exynos Platform"
+	depends on ARCH_EXYNOS || COMPILE_TEST
 	default USB_DWC3
 	help
 	  Recent Exynos5 SoCs ship with one DesignWare Core USB3 IP inside,
@@ -72,6 +73,7 @@ config USB_DWC3_PCI
 
 config USB_DWC3_KEYSTONE
 	tristate "Texas Instruments Keystone2 Platforms"
+	depends on ARCH_KEYSTONE || COMPILE_TEST
 	default USB_DWC3
 	help
 	  Support of USB2/3 functionality in TI Keystone2 platforms.
diff --git a/drivers/usb/dwc3/core.c b/drivers/usb/dwc3/core.c
index d001417e8e3..eb69eb9f06c 100644
--- a/drivers/usb/dwc3/core.c
+++ b/drivers/usb/dwc3/core.c
@@ -486,70 +486,20 @@ static void dwc3_core_exit(struct dwc3 *dwc)
 	phy_exit(dwc->usb3_generic_phy);
 }
 
-#define DWC3_ALIGN_MASK		(16 - 1)
-
-static int dwc3_probe(struct platform_device *pdev)
+static int dwc3_core_get_phy(struct dwc3 *dwc)
 {
-	struct device		*dev = &pdev->dev;
-	struct dwc3_platform_data *pdata = dev_get_platdata(dev);
+	struct device		*dev = dwc->dev;
 	struct device_node	*node = dev->of_node;
-	struct resource		*res;
-	struct dwc3		*dwc;
-
-	int			ret = -ENOMEM;
-
-	void __iomem		*regs;
-	void			*mem;
-
-	mem = devm_kzalloc(dev, sizeof(*dwc) + DWC3_ALIGN_MASK, GFP_KERNEL);
-	if (!mem) {
-		dev_err(dev, "not enough memory\n");
-		return -ENOMEM;
-	}
-	dwc = PTR_ALIGN(mem, DWC3_ALIGN_MASK + 1);
-	dwc->mem = mem;
-
-	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-	if (!res) {
-		dev_err(dev, "missing IRQ\n");
-		return -ENODEV;
-	}
-	dwc->xhci_resources[1].start = res->start;
-	dwc->xhci_resources[1].end = res->end;
-	dwc->xhci_resources[1].flags = res->flags;
-	dwc->xhci_resources[1].name = res->name;
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(dev, "missing memory resource\n");
-		return -ENODEV;
-	}
+	int ret;
 
 	if (node) {
-		dwc->maximum_speed = of_usb_get_maximum_speed(node);
-
 		dwc->usb2_phy = devm_usb_get_phy_by_phandle(dev, "usb-phy", 0);
 		dwc->usb3_phy = devm_usb_get_phy_by_phandle(dev, "usb-phy", 1);
-
-		dwc->needs_fifo_resize = of_property_read_bool(node, "tx-fifo-resize");
-		dwc->dr_mode = of_usb_get_dr_mode(node);
-	} else if (pdata) {
-		dwc->maximum_speed = pdata->maximum_speed;
-
-		dwc->usb2_phy = devm_usb_get_phy(dev, USB_PHY_TYPE_USB2);
-		dwc->usb3_phy = devm_usb_get_phy(dev, USB_PHY_TYPE_USB3);
-
-		dwc->needs_fifo_resize = pdata->tx_fifo_resize;
-		dwc->dr_mode = pdata->dr_mode;
 	} else {
 		dwc->usb2_phy = devm_usb_get_phy(dev, USB_PHY_TYPE_USB2);
 		dwc->usb3_phy = devm_usb_get_phy(dev, USB_PHY_TYPE_USB3);
 	}
 
-	/* default to superspeed if no maximum_speed passed */
-	if (dwc->maximum_speed == USB_SPEED_UNKNOWN)
-		dwc->maximum_speed = USB_SPEED_SUPER;
-
 	if (IS_ERR(dwc->usb2_phy)) {
 		ret = PTR_ERR(dwc->usb2_phy);
 		if (ret == -ENXIO || ret == -ENODEV) {
@@ -600,6 +550,132 @@ static int dwc3_probe(struct platform_device *pdev)
 		}
 	}
 
+	return 0;
+}
+
+static int dwc3_core_init_mode(struct dwc3 *dwc)
+{
+	struct device *dev = dwc->dev;
+	int ret;
+
+	switch (dwc->dr_mode) {
+	case USB_DR_MODE_PERIPHERAL:
+		dwc3_set_mode(dwc, DWC3_GCTL_PRTCAP_DEVICE);
+		ret = dwc3_gadget_init(dwc);
+		if (ret) {
+			dev_err(dev, "failed to initialize gadget\n");
+			return ret;
+		}
+		break;
+	case USB_DR_MODE_HOST:
+		dwc3_set_mode(dwc, DWC3_GCTL_PRTCAP_HOST);
+		ret = dwc3_host_init(dwc);
+		if (ret) {
+			dev_err(dev, "failed to initialize host\n");
+			return ret;
+		}
+		break;
+	case USB_DR_MODE_OTG:
+		dwc3_set_mode(dwc, DWC3_GCTL_PRTCAP_OTG);
+		ret = dwc3_host_init(dwc);
+		if (ret) {
+			dev_err(dev, "failed to initialize host\n");
+			return ret;
+		}
+
+		ret = dwc3_gadget_init(dwc);
+		if (ret) {
+			dev_err(dev, "failed to initialize gadget\n");
+			return ret;
+		}
+		break;
+	default:
+		dev_err(dev, "Unsupported mode of operation %d\n", dwc->dr_mode);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void dwc3_core_exit_mode(struct dwc3 *dwc)
+{
+	switch (dwc->dr_mode) {
+	case USB_DR_MODE_PERIPHERAL:
+		dwc3_gadget_exit(dwc);
+		break;
+	case USB_DR_MODE_HOST:
+		dwc3_host_exit(dwc);
+		break;
+	case USB_DR_MODE_OTG:
+		dwc3_host_exit(dwc);
+		dwc3_gadget_exit(dwc);
+		break;
+	default:
+		/* do nothing */
+		break;
+	}
+}
+
+#define DWC3_ALIGN_MASK		(16 - 1)
+
+static int dwc3_probe(struct platform_device *pdev)
+{
+	struct device		*dev = &pdev->dev;
+	struct dwc3_platform_data *pdata = dev_get_platdata(dev);
+	struct device_node	*node = dev->of_node;
+	struct resource		*res;
+	struct dwc3		*dwc;
+
+	int			ret;
+
+	void __iomem		*regs;
+	void			*mem;
+
+	mem = devm_kzalloc(dev, sizeof(*dwc) + DWC3_ALIGN_MASK, GFP_KERNEL);
+	if (!mem) {
+		dev_err(dev, "not enough memory\n");
+		return -ENOMEM;
+	}
+	dwc = PTR_ALIGN(mem, DWC3_ALIGN_MASK + 1);
+	dwc->mem = mem;
+	dwc->dev = dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res) {
+		dev_err(dev, "missing IRQ\n");
+		return -ENODEV;
+	}
+	dwc->xhci_resources[1].start = res->start;
+	dwc->xhci_resources[1].end = res->end;
+	dwc->xhci_resources[1].flags = res->flags;
+	dwc->xhci_resources[1].name = res->name;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(dev, "missing memory resource\n");
+		return -ENODEV;
+	}
+
+	if (node) {
+		dwc->maximum_speed = of_usb_get_maximum_speed(node);
+
+		dwc->needs_fifo_resize = of_property_read_bool(node, "tx-fifo-resize");
+		dwc->dr_mode = of_usb_get_dr_mode(node);
+	} else if (pdata) {
+		dwc->maximum_speed = pdata->maximum_speed;
+
+		dwc->needs_fifo_resize = pdata->tx_fifo_resize;
+		dwc->dr_mode = pdata->dr_mode;
+	}
+
+	/* default to superspeed if no maximum_speed passed */
+	if (dwc->maximum_speed == USB_SPEED_UNKNOWN)
+		dwc->maximum_speed = USB_SPEED_SUPER;
+
+	ret = dwc3_core_get_phy(dwc);
+	if (ret)
+		return ret;
+
 	dwc->xhci_resources[0].start = res->start;
 	dwc->xhci_resources[0].end = dwc->xhci_resources[0].start +
 					DWC3_XHCI_REGS_END;
@@ -621,7 +697,6 @@ static int dwc3_probe(struct platform_device *pdev)
 
 	dwc->regs	= regs;
 	dwc->regs_size	= resource_size(res);
-	dwc->dev	= dev;
 
 	dev->dma_mask	= dev->parent->dma_mask;
 	dev->dma_parms	= dev->parent->dma_parms;
@@ -670,41 +745,9 @@ static int dwc3_probe(struct platform_device *pdev)
 		goto err_usb3phy_power;
 	}
 
-	switch (dwc->dr_mode) {
-	case USB_DR_MODE_PERIPHERAL:
-		dwc3_set_mode(dwc, DWC3_GCTL_PRTCAP_DEVICE);
-		ret = dwc3_gadget_init(dwc);
-		if (ret) {
-			dev_err(dev, "failed to initialize gadget\n");
-			goto err2;
-		}
-		break;
-	case USB_DR_MODE_HOST:
-		dwc3_set_mode(dwc, DWC3_GCTL_PRTCAP_HOST);
-		ret = dwc3_host_init(dwc);
-		if (ret) {
-			dev_err(dev, "failed to initialize host\n");
-			goto err2;
-		}
-		break;
-	case USB_DR_MODE_OTG:
-		dwc3_set_mode(dwc, DWC3_GCTL_PRTCAP_OTG);
-		ret = dwc3_host_init(dwc);
-		if (ret) {
-			dev_err(dev, "failed to initialize host\n");
-			goto err2;
-		}
-
-		ret = dwc3_gadget_init(dwc);
-		if (ret) {
-			dev_err(dev, "failed to initialize gadget\n");
-			goto err2;
-		}
-		break;
-	default:
-		dev_err(dev, "Unsupported mode of operation %d\n", dwc->dr_mode);
+	ret = dwc3_core_init_mode(dwc);
+	if (ret)
 		goto err2;
-	}
 
 	ret = dwc3_debugfs_init(dwc);
 	if (ret) {
@@ -717,21 +760,7 @@ static int dwc3_probe(struct platform_device *pdev)
 	return 0;
 
 err3:
-	switch (dwc->dr_mode) {
-	case USB_DR_MODE_PERIPHERAL:
-		dwc3_gadget_exit(dwc);
-		break;
-	case USB_DR_MODE_HOST:
-		dwc3_host_exit(dwc);
-		break;
-	case USB_DR_MODE_OTG:
-		dwc3_host_exit(dwc);
-		dwc3_gadget_exit(dwc);
-		break;
-	default:
-		/* do nothing */
-		break;
-	}
+	dwc3_core_exit_mode(dwc);
 
 err2:
 	dwc3_event_buffers_cleanup(dwc);
@@ -766,23 +795,7 @@ static int dwc3_remove(struct platform_device *pdev)
 	pm_runtime_disable(&pdev->dev);
 
 	dwc3_debugfs_exit(dwc);
-
-	switch (dwc->dr_mode) {
-	case USB_DR_MODE_PERIPHERAL:
-		dwc3_gadget_exit(dwc);
-		break;
-	case USB_DR_MODE_HOST:
-		dwc3_host_exit(dwc);
-		break;
-	case USB_DR_MODE_OTG:
-		dwc3_host_exit(dwc);
-		dwc3_gadget_exit(dwc);
-		break;
-	default:
-		/* do nothing */
-		break;
-	}
-
+	dwc3_core_exit_mode(dwc);
 	dwc3_event_buffers_cleanup(dwc);
 	dwc3_free_event_buffers(dwc);
 	dwc3_core_exit(dwc);
@@ -821,6 +834,7 @@ static void dwc3_complete(struct device *dev)
 
 	spin_lock_irqsave(&dwc->lock, flags);
 
+	dwc3_event_buffers_setup(dwc);
 	switch (dwc->dr_mode) {
 	case USB_DR_MODE_PERIPHERAL:
 	case USB_DR_MODE_OTG:
@@ -828,7 +842,6 @@ static void dwc3_complete(struct device *dev)
 		/* FALLTHROUGH */
 	case USB_DR_MODE_HOST:
 	default:
-		dwc3_event_buffers_setup(dwc);
 		break;
 	}
 
diff --git a/drivers/usb/dwc3/dwc3-exynos.c b/drivers/usb/dwc3/dwc3-exynos.c
index 28c8ad79f5e..f9fb8adb785 100644
--- a/drivers/usb/dwc3/dwc3-exynos.c
+++ b/drivers/usb/dwc3/dwc3-exynos.c
@@ -24,9 +24,10 @@
 #include <linux/dma-mapping.h>
 #include <linux/clk.h>
 #include <linux/usb/otg.h>
-#include <linux/usb/usb_phy_gen_xceiv.h>
+#include <linux/usb/usb_phy_generic.h>
 #include <linux/of.h>
 #include <linux/of_platform.h>
+#include <linux/regulator/consumer.h>
 
 struct dwc3_exynos {
 	struct platform_device	*usb2_phy;
@@ -34,17 +35,19 @@ struct dwc3_exynos {
 	struct device		*dev;
 
 	struct clk		*clk;
+	struct regulator	*vdd33;
+	struct regulator	*vdd10;
 };
 
 static int dwc3_exynos_register_phys(struct dwc3_exynos *exynos)
 {
-	struct usb_phy_gen_xceiv_platform_data pdata;
+	struct usb_phy_generic_platform_data pdata;
 	struct platform_device	*pdev;
 	int			ret;
 
 	memset(&pdata, 0x00, sizeof(pdata));
 
-	pdev = platform_device_alloc("usb_phy_gen_xceiv", PLATFORM_DEVID_AUTO);
+	pdev = platform_device_alloc("usb_phy_generic", PLATFORM_DEVID_AUTO);
 	if (!pdev)
 		return -ENOMEM;
 
@@ -56,7 +59,7 @@ static int dwc3_exynos_register_phys(struct dwc3_exynos *exynos)
 	if (ret)
 		goto err1;
 
-	pdev = platform_device_alloc("usb_phy_gen_xceiv", PLATFORM_DEVID_AUTO);
+	pdev = platform_device_alloc("usb_phy_generic", PLATFORM_DEVID_AUTO);
 	if (!pdev) {
 		ret = -ENOMEM;
 		goto err1;
@@ -107,12 +110,12 @@ static int dwc3_exynos_probe(struct platform_device *pdev)
 	struct device		*dev = &pdev->dev;
 	struct device_node	*node = dev->of_node;
 
-	int			ret = -ENOMEM;
+	int			ret;
 
 	exynos = devm_kzalloc(dev, sizeof(*exynos), GFP_KERNEL);
 	if (!exynos) {
 		dev_err(dev, "not enough memory\n");
-		goto err1;
+		return -ENOMEM;
 	}
 
 	/*
@@ -122,21 +125,20 @@ static int dwc3_exynos_probe(struct platform_device *pdev)
 	 */
 	ret = dma_coerce_mask_and_coherent(dev, DMA_BIT_MASK(32));
 	if (ret)
-		goto err1;
+		return ret;
 
 	platform_set_drvdata(pdev, exynos);
 
 	ret = dwc3_exynos_register_phys(exynos);
 	if (ret) {
 		dev_err(dev, "couldn't register PHYs\n");
-		goto err1;
+		return ret;
 	}
 
 	clk = devm_clk_get(dev, "usbdrd30");
 	if (IS_ERR(clk)) {
 		dev_err(dev, "couldn't get clock\n");
-		ret = -EINVAL;
-		goto err1;
+		return -EINVAL;
 	}
 
 	exynos->dev	= dev;
@@ -144,23 +146,48 @@ static int dwc3_exynos_probe(struct platform_device *pdev)
 
 	clk_prepare_enable(exynos->clk);
 
+	exynos->vdd33 = devm_regulator_get(dev, "vdd33");
+	if (IS_ERR(exynos->vdd33)) {
+		ret = PTR_ERR(exynos->vdd33);
+		goto err2;
+	}
+	ret = regulator_enable(exynos->vdd33);
+	if (ret) {
+		dev_err(dev, "Failed to enable VDD33 supply\n");
+		goto err2;
+	}
+
+	exynos->vdd10 = devm_regulator_get(dev, "vdd10");
+	if (IS_ERR(exynos->vdd10)) {
+		ret = PTR_ERR(exynos->vdd10);
+		goto err3;
+	}
+	ret = regulator_enable(exynos->vdd10);
+	if (ret) {
+		dev_err(dev, "Failed to enable VDD10 supply\n");
+		goto err3;
+	}
+
 	if (node) {
 		ret = of_platform_populate(node, NULL, NULL, dev);
 		if (ret) {
 			dev_err(dev, "failed to add dwc3 core\n");
-			goto err2;
+			goto err4;
 		}
 	} else {
 		dev_err(dev, "no device node, failed to add dwc3 core\n");
 		ret = -ENODEV;
-		goto err2;
+		goto err4;
 	}
 
 	return 0;
 
+err4:
+	regulator_disable(exynos->vdd10);
+err3:
+	regulator_disable(exynos->vdd33);
 err2:
 	clk_disable_unprepare(clk);
-err1:
 	return ret;
 }
 
@@ -174,6 +201,9 @@ static int dwc3_exynos_remove(struct platform_device *pdev)
 
 	clk_disable_unprepare(exynos->clk);
 
+	regulator_disable(exynos->vdd33);
+	regulator_disable(exynos->vdd10);
+
 	return 0;
 }
 
@@ -192,12 +222,27 @@ static int dwc3_exynos_suspend(struct device *dev)
 
 	clk_disable(exynos->clk);
 
+	regulator_disable(exynos->vdd33);
+	regulator_disable(exynos->vdd10);
+
 	return 0;
 }
 
 static int dwc3_exynos_resume(struct device *dev)
 {
 	struct dwc3_exynos *exynos = dev_get_drvdata(dev);
+	int ret;
+
+	ret = regulator_enable(exynos->vdd33);
+	if (ret) {
+		dev_err(dev, "Failed to enable VDD33 supply\n");
+		return ret;
+	}
+	ret = regulator_enable(exynos->vdd10);
+	if (ret) {
+		dev_err(dev, "Failed to enable VDD10 supply\n");
+		return ret;
+	}
 
 	clk_enable(exynos->clk);
 
diff --git a/drivers/usb/dwc3/dwc3-omap.c b/drivers/usb/dwc3/dwc3-omap.c
index 1160ff41bed..4af4c356765 100644
--- a/drivers/usb/dwc3/dwc3-omap.c
+++ b/drivers/usb/dwc3/dwc3-omap.c
@@ -393,7 +393,7 @@ static int dwc3_omap_probe(struct platform_device *pdev)
 	struct extcon_dev	*edev;
 	struct regulator	*vbus_reg = NULL;
 
-	int			ret = -ENOMEM;
+	int			ret;
 	int			irq;
 
 	int			utmi_mode = 0;
diff --git a/drivers/usb/dwc3/dwc3-pci.c b/drivers/usb/dwc3/dwc3-pci.c
index f393c183cc6..a60bab7dfa0 100644
--- a/drivers/usb/dwc3/dwc3-pci.c
+++ b/drivers/usb/dwc3/dwc3-pci.c
@@ -23,7 +23,7 @@
 #include <linux/platform_device.h>
 
 #include <linux/usb/otg.h>
-#include <linux/usb/usb_phy_gen_xceiv.h>
+#include <linux/usb/usb_phy_generic.h>
 
 /* FIXME define these in <linux/pci_ids.h> */
 #define PCI_VENDOR_ID_SYNOPSYS		0x16c3
@@ -40,13 +40,13 @@ struct dwc3_pci {
 
 static int dwc3_pci_register_phys(struct dwc3_pci *glue)
 {
-	struct usb_phy_gen_xceiv_platform_data pdata;
+	struct usb_phy_generic_platform_data pdata;
 	struct platform_device	*pdev;
 	int			ret;
 
 	memset(&pdata, 0x00, sizeof(pdata));
 
-	pdev = platform_device_alloc("usb_phy_gen_xceiv", 0);
+	pdev = platform_device_alloc("usb_phy_generic", 0);
 	if (!pdev)
 		return -ENOMEM;
 
@@ -58,7 +58,7 @@ static int dwc3_pci_register_phys(struct dwc3_pci *glue)
 	if (ret)
 		goto err1;
 
-	pdev = platform_device_alloc("usb_phy_gen_xceiv", 1);
+	pdev = platform_device_alloc("usb_phy_generic", 1);
 	if (!pdev) {
 		ret = -ENOMEM;
 		goto err1;
@@ -99,7 +99,7 @@ static int dwc3_pci_probe(struct pci_dev *pci,
 	struct resource		res[2];
 	struct platform_device	*dwc3;
 	struct dwc3_pci		*glue;
-	int			ret = -ENOMEM;
+	int			ret;
 	struct device		*dev = &pci->dev;
 
 	glue = devm_kzalloc(dev, sizeof(*glue), GFP_KERNEL);
@@ -110,7 +110,7 @@ static int dwc3_pci_probe(struct pci_dev *pci,
 
 	glue->dev = dev;
 
-	ret = pci_enable_device(pci);
+	ret = pcim_enable_device(pci);
 	if (ret) {
 		dev_err(dev, "failed to enable pci device\n");
 		return -ENODEV;
@@ -127,8 +127,7 @@ static int dwc3_pci_probe(struct pci_dev *pci,
 	dwc3 = platform_device_alloc("dwc3", PLATFORM_DEVID_AUTO);
 	if (!dwc3) {
 		dev_err(dev, "couldn't allocate dwc3 device\n");
-		ret = -ENOMEM;
-		goto err1;
+		return -ENOMEM;
 	}
 
 	memset(res, 0x00, sizeof(struct resource) * ARRAY_SIZE(res));
@@ -145,7 +144,7 @@ static int dwc3_pci_probe(struct pci_dev *pci,
 	ret = platform_device_add_resources(dwc3, res, ARRAY_SIZE(res));
 	if (ret) {
 		dev_err(dev, "couldn't add resources to dwc3 device\n");
-		goto err1;
+		return ret;
 	}
 
 	pci_set_drvdata(pci, glue);
@@ -167,9 +166,6 @@ static int dwc3_pci_probe(struct pci_dev *pci,
 
 err3:
 	platform_device_put(dwc3);
-err1:
-	pci_disable_device(pci);
-
 	return ret;
 }
 
@@ -180,7 +176,6 @@ static void dwc3_pci_remove(struct pci_dev *pci)
 	platform_device_unregister(glue->dwc3);
 	platform_device_unregister(glue->usb2_phy);
 	platform_device_unregister(glue->usb3_phy);
-	pci_disable_device(pci);
 }
 
 static const struct pci_device_id dwc3_pci_id_table[] = {
diff --git a/drivers/usb/dwc3/gadget.c b/drivers/usb/dwc3/gadget.c
index a740eac74d5..9d64dd02c57 100644
--- a/drivers/usb/dwc3/gadget.c
+++ b/drivers/usb/dwc3/gadget.c
@@ -187,15 +187,12 @@ int dwc3_gadget_resize_tx_fifos(struct dwc3 *dwc)
 	 * improve this algorithm so that we better use the internal
 	 * FIFO space
 	 */
-	for (num = 0; num < DWC3_ENDPOINTS_NUM; num++) {
-		struct dwc3_ep	*dep = dwc->eps[num];
-		int		fifo_number = dep->number >> 1;
+	for (num = 0; num < dwc->num_in_eps; num++) {
+		/* bit0 indicates direction; 1 means IN ep */
+		struct dwc3_ep	*dep = dwc->eps[(num << 1) | 1];
 		int		mult = 1;
 		int		tmp;
 
-		if (!(dep->number & 1))
-			continue;
-
 		if (!(dep->flags & DWC3_EP_ENABLED))
 			continue;
 
@@ -224,8 +221,7 @@ int dwc3_gadget_resize_tx_fifos(struct dwc3 *dwc)
 		dev_vdbg(dwc->dev, "%s: Fifo Addr %04x Size %d\n",
 				dep->name, last_fifo_depth, fifo_size & 0xffff);
 
-		dwc3_writel(dwc->regs, DWC3_GTXFIFOSIZ(fifo_number),
-				fifo_size);
+		dwc3_writel(dwc->regs, DWC3_GTXFIFOSIZ(num), fifo_size);
 
 		last_fifo_depth += (fifo_size & 0xffff);
 	}
@@ -302,11 +298,76 @@ static const char *dwc3_gadget_ep_cmd_string(u8 cmd)
 	}
 }
 
+static const char *dwc3_gadget_generic_cmd_string(u8 cmd)
+{
+	switch (cmd) {
+	case DWC3_DGCMD_SET_LMP:
+		return "Set LMP";
+	case DWC3_DGCMD_SET_PERIODIC_PAR:
+		return "Set Periodic Parameters";
+	case DWC3_DGCMD_XMIT_FUNCTION:
+		return "Transmit Function Wake Device Notification";
+	case DWC3_DGCMD_SET_SCRATCHPAD_ADDR_LO:
+		return "Set Scratchpad Buffer Array Address Lo";
+	case DWC3_DGCMD_SET_SCRATCHPAD_ADDR_HI:
+		return "Set Scratchpad Buffer Array Address Hi";
+	case DWC3_DGCMD_SELECTED_FIFO_FLUSH:
+		return "Selected FIFO Flush";
+	case DWC3_DGCMD_ALL_FIFO_FLUSH:
+		return "All FIFO Flush";
+	case DWC3_DGCMD_SET_ENDPOINT_NRDY:
+		return "Set Endpoint NRDY";
+	case DWC3_DGCMD_RUN_SOC_BUS_LOOPBACK:
+		return "Run SoC Bus Loopback Test";
+	default:
+		return "UNKNOWN";
+	}
+}
+
+static const char *dwc3_gadget_link_string(enum dwc3_link_state link_state)
+{
+	switch (link_state) {
+	case DWC3_LINK_STATE_U0:
+		return "U0";
+	case DWC3_LINK_STATE_U1:
+		return "U1";
+	case DWC3_LINK_STATE_U2:
+		return "U2";
+	case DWC3_LINK_STATE_U3:
+		return "U3";
+	case DWC3_LINK_STATE_SS_DIS:
+		return "SS.Disabled";
+	case DWC3_LINK_STATE_RX_DET:
+		return "RX.Detect";
+	case DWC3_LINK_STATE_SS_INACT:
+		return "SS.Inactive";
+	case DWC3_LINK_STATE_POLL:
+		return "Polling";
+	case DWC3_LINK_STATE_RECOV:
+		return "Recovery";
+	case DWC3_LINK_STATE_HRESET:
+		return "Hot Reset";
+	case DWC3_LINK_STATE_CMPLY:
+		return "Compliance";
+	case DWC3_LINK_STATE_LPBK:
+		return "Loopback";
+	case DWC3_LINK_STATE_RESET:
+		return "Reset";
+	case DWC3_LINK_STATE_RESUME:
+		return "Resume";
+	default:
+		return "UNKNOWN link state\n";
+	}
+}
+
 int dwc3_send_gadget_generic_command(struct dwc3 *dwc, int cmd, u32 param)
 {
 	u32		timeout = 500;
 	u32		reg;
 
+	dev_vdbg(dwc->dev, "generic cmd '%s' [%d] param %08x\n",
+			dwc3_gadget_generic_cmd_string(cmd), cmd, param);
+
 	dwc3_writel(dwc->regs, DWC3_DGCMDPAR, param);
 	dwc3_writel(dwc->regs, DWC3_DGCMD, cmd | DWC3_DGCMD_CMDACT);
 
@@ -336,9 +397,9 @@ int dwc3_send_gadget_ep_cmd(struct dwc3 *dwc, unsigned ep,
 	u32			timeout = 500;
 	u32			reg;
 
-	dev_vdbg(dwc->dev, "%s: cmd '%s' params %08x %08x %08x\n",
+	dev_vdbg(dwc->dev, "%s: cmd '%s' [%d] params %08x %08x %08x\n",
 			dep->name,
-			dwc3_gadget_ep_cmd_string(cmd), params->param0,
+			dwc3_gadget_ep_cmd_string(cmd), cmd, params->param0,
 			params->param1, params->param2);
 
 	dwc3_writel(dwc->regs, DWC3_DEPCMDPAR0(ep), params->param0);
@@ -519,7 +580,7 @@ static int __dwc3_gadget_ep_enable(struct dwc3_ep *dep,
 {
 	struct dwc3		*dwc = dep->dwc;
 	u32			reg;
-	int			ret = -ENOMEM;
+	int			ret;
 
 	dev_vdbg(dwc->dev, "Enabling %s\n", dep->name);
 
@@ -608,6 +669,10 @@ static int __dwc3_gadget_ep_disable(struct dwc3_ep *dep)
 
 	dwc3_remove_requests(dwc, dep);
 
+	/* make sure HW endpoint isn't stalled */
+	if (dep->flags & DWC3_EP_STALL)
+		__dwc3_gadget_ep_set_halt(dep, 0);
+
 	reg = dwc3_readl(dwc->regs, DWC3_DALEPENA);
 	reg &= ~DWC3_DALEPENA_EP(dep->number);
 	dwc3_writel(dwc->regs, DWC3_DALEPENA, reg);
@@ -2445,8 +2510,6 @@ static void dwc3_gadget_linksts_change_interrupt(struct dwc3 *dwc,
 		}
 	}
 
-	dwc->link_state = next;
-
 	switch (next) {
 	case DWC3_LINK_STATE_U1:
 		if (dwc->speed == USB_SPEED_SUPER)
@@ -2464,7 +2527,11 @@ static void dwc3_gadget_linksts_change_interrupt(struct dwc3 *dwc,
 		break;
 	}
 
-	dev_vdbg(dwc->dev, "%s link %d\n", __func__, dwc->link_state);
+	dev_vdbg(dwc->dev, "link change: %s [%d] -> %s [%d]\n",
+			dwc3_gadget_link_string(dwc->link_state),
+			dwc->link_state, dwc3_gadget_link_string(next), next);
+
+	dwc->link_state = next;
 }
 
 static void dwc3_gadget_hibernation_interrupt(struct dwc3 *dwc,
diff --git a/drivers/usb/gadget/Kconfig b/drivers/usb/gadget/Kconfig
index 3557c7e5040..ba18e9c110c 100644
--- a/drivers/usb/gadget/Kconfig
+++ b/drivers/usb/gadget/Kconfig
@@ -157,7 +157,7 @@ config USB_AT91
 
 config USB_LPC32XX
 	tristate "LPC32XX USB Peripheral Controller"
-	depends on ARCH_LPC32XX
+	depends on ARCH_LPC32XX && I2C
 	select USB_ISP1301
 	help
 	   This option selects the USB device controller in the LPC32xx SoC.
@@ -226,7 +226,7 @@ config USB_GR_UDC
 config USB_OMAP
 	tristate "OMAP USB Device Controller"
 	depends on ARCH_OMAP1
-	select ISP1301_OMAP if MACH_OMAP_H2 || MACH_OMAP_H3
+	depends on ISP1301_OMAP || !(MACH_OMAP_H2 || MACH_OMAP_H3)
 	help
 	   Many Texas Instruments OMAP processors have flexible full
 	   speed USB device controllers, with support for up to 30
@@ -300,12 +300,6 @@ config USB_PXA27X
 	   dynamically linked module called "pxa27x_udc" and force all
 	   gadget drivers to also be dynamically linked.
 
-config USB_S3C_HSOTG
-	tristate "Designware/S3C HS/OtG USB Device controller"
-	help
-	  The Designware USB2.0 high-speed gadget controller
-	  integrated into many SoCs.
-
 config USB_S3C2410
 	tristate "S3C2410 USB Device Controller"
 	depends on ARCH_S3C24XX
diff --git a/drivers/usb/gadget/Makefile b/drivers/usb/gadget/Makefile
index 5f150bc1b4b..49514ea60a9 100644
--- a/drivers/usb/gadget/Makefile
+++ b/drivers/usb/gadget/Makefile
@@ -26,7 +26,6 @@ fsl_usb2_udc-$(CONFIG_ARCH_MXC)	+= fsl_mxc_udc.o
 obj-$(CONFIG_USB_M66592)	+= m66592-udc.o
 obj-$(CONFIG_USB_R8A66597)	+= r8a66597-udc.o
 obj-$(CONFIG_USB_FSL_QE)	+= fsl_qe_udc.o
-obj-$(CONFIG_USB_S3C_HSOTG)	+= s3c-hsotg.o
 obj-$(CONFIG_USB_S3C_HSUDC)	+= s3c-hsudc.o
 obj-$(CONFIG_USB_LPC32XX)	+= lpc32xx_udc.o
 obj-$(CONFIG_USB_EG20T)		+= pch_udc.o
diff --git a/drivers/usb/gadget/at91_udc.c b/drivers/usb/gadget/at91_udc.c
index f605ad8c190..cfd18bcca72 100644
--- a/drivers/usb/gadget/at91_udc.c
+++ b/drivers/usb/gadget/at91_udc.c
@@ -1709,16 +1709,6 @@ static int at91udc_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	if (pdev->num_resources != 2) {
-		DBG("invalid num_resources\n");
-		return -ENODEV;
-	}
-	if ((pdev->resource[0].flags != IORESOURCE_MEM)
-			|| (pdev->resource[1].flags != IORESOURCE_IRQ)) {
-		DBG("invalid resource type\n");
-		return -ENODEV;
-	}
-
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res)
 		return -ENXIO;
diff --git a/drivers/usb/gadget/atmel_usba_udc.c b/drivers/usb/gadget/atmel_usba_udc.c
index 9f65324f9ae..76023ce449a 100644
--- a/drivers/usb/gadget/atmel_usba_udc.c
+++ b/drivers/usb/gadget/atmel_usba_udc.c
@@ -1686,7 +1686,7 @@ static irqreturn_t usba_udc_irq(int irq, void *devid)
 		reset_all_endpoints(udc);
 
 		if (udc->gadget.speed != USB_SPEED_UNKNOWN
-				&& udc->driver->disconnect) {
+				&& udc->driver && udc->driver->disconnect) {
 			udc->gadget.speed = USB_SPEED_UNKNOWN;
 			spin_unlock(&udc->lock);
 			udc->driver->disconnect(&udc->gadget);
diff --git a/drivers/usb/gadget/composite.c b/drivers/usb/gadget/composite.c
index fab906429b8..f8015193205 100644
--- a/drivers/usb/gadget/composite.c
+++ b/drivers/usb/gadget/composite.c
@@ -21,6 +21,24 @@
 #include <linux/usb/composite.h>
 #include <asm/unaligned.h>
 
+#include "u_os_desc.h"
+
+/**
+ * struct usb_os_string - represents OS String to be reported by a gadget
+ * @bLength: total length of the entire descritor, always 0x12
+ * @bDescriptorType: USB_DT_STRING
+ * @qwSignature: the OS String proper
+ * @bMS_VendorCode: code used by the host for subsequent requests
+ * @bPad: not used, must be zero
+ */
+struct usb_os_string {
+	__u8	bLength;
+	__u8	bDescriptorType;
+	__u8	qwSignature[OS_STRING_QW_SIGN_LEN];
+	__u8	bMS_VendorCode;
+	__u8	bPad;
+} __packed;
+
 /*
  * The code in this file is utility code, used to build a gadget driver
  * from one or more "function" drivers, one or more "configuration"
@@ -422,6 +440,7 @@ static int config_desc(struct usb_composite_dev *cdev, unsigned w_value)
 {
 	struct usb_gadget		*gadget = cdev->gadget;
 	struct usb_configuration	*c;
+	struct list_head		*pos;
 	u8				type = w_value >> 8;
 	enum usb_device_speed		speed = USB_SPEED_UNKNOWN;
 
@@ -440,7 +459,20 @@ static int config_desc(struct usb_composite_dev *cdev, unsigned w_value)
 
 	/* This is a lookup by config *INDEX* */
 	w_value &= 0xff;
-	list_for_each_entry(c, &cdev->configs, list) {
+
+	pos = &cdev->configs;
+	c = cdev->os_desc_config;
+	if (c)
+		goto check_config;
+
+	while ((pos = pos->next) !=  &cdev->configs) {
+		c = list_entry(pos, typeof(*c), list);
+
+		/* skip OS Descriptors config which is handled separately */
+		if (c == cdev->os_desc_config)
+			continue;
+
+check_config:
 		/* ignore configs that won't work at this speed */
 		switch (speed) {
 		case USB_SPEED_SUPER:
@@ -634,6 +666,7 @@ static int set_config(struct usb_composite_dev *cdev,
 	if (!c)
 		goto done;
 
+	usb_gadget_set_state(gadget, USB_STATE_CONFIGURED);
 	cdev->config = c;
 
 	/* Initialize all interfaces by setting them to altsetting zero. */
@@ -960,6 +993,19 @@ static int get_string(struct usb_composite_dev *cdev,
 		return s->bLength;
 	}
 
+	if (cdev->use_os_string && language == 0 && id == OS_STRING_IDX) {
+		struct usb_os_string *b = buf;
+		b->bLength = sizeof(*b);
+		b->bDescriptorType = USB_DT_STRING;
+		compiletime_assert(
+			sizeof(b->qwSignature) == sizeof(cdev->qw_sign),
+			"qwSignature size must be equal to qw_sign");
+		memcpy(&b->qwSignature, cdev->qw_sign, sizeof(b->qwSignature));
+		b->bMS_VendorCode = cdev->b_vendor_code;
+		b->bPad = 0;
+		return sizeof(*b);
+	}
+
 	list_for_each_entry(uc, &cdev->gstrings, list) {
 		struct usb_gadget_strings **sp;
 
@@ -1206,6 +1252,156 @@ static void composite_setup_complete(struct usb_ep *ep, struct usb_request *req)
 				req->status, req->actual, req->length);
 }
 
+static int count_ext_compat(struct usb_configuration *c)
+{
+	int i, res;
+
+	res = 0;
+	for (i = 0; i < c->next_interface_id; ++i) {
+		struct usb_function *f;
+		int j;
+
+		f = c->interface[i];
+		for (j = 0; j < f->os_desc_n; ++j) {
+			struct usb_os_desc *d;
+
+			if (i != f->os_desc_table[j].if_id)
+				continue;
+			d = f->os_desc_table[j].os_desc;
+			if (d && d->ext_compat_id)
+				++res;
+		}
+	}
+	BUG_ON(res > 255);
+	return res;
+}
+
+static void fill_ext_compat(struct usb_configuration *c, u8 *buf)
+{
+	int i, count;
+
+	count = 16;
+	for (i = 0; i < c->next_interface_id; ++i) {
+		struct usb_function *f;
+		int j;
+
+		f = c->interface[i];
+		for (j = 0; j < f->os_desc_n; ++j) {
+			struct usb_os_desc *d;
+
+			if (i != f->os_desc_table[j].if_id)
+				continue;
+			d = f->os_desc_table[j].os_desc;
+			if (d && d->ext_compat_id) {
+				*buf++ = i;
+				*buf++ = 0x01;
+				memcpy(buf, d->ext_compat_id, 16);
+				buf += 22;
+			} else {
+				++buf;
+				*buf = 0x01;
+				buf += 23;
+			}
+			count += 24;
+			if (count >= 4096)
+				return;
+		}
+	}
+}
+
+static int count_ext_prop(struct usb_configuration *c, int interface)
+{
+	struct usb_function *f;
+	int j;
+
+	f = c->interface[interface];
+	for (j = 0; j < f->os_desc_n; ++j) {
+		struct usb_os_desc *d;
+
+		if (interface != f->os_desc_table[j].if_id)
+			continue;
+		d = f->os_desc_table[j].os_desc;
+		if (d && d->ext_compat_id)
+			return d->ext_prop_count;
+	}
+	return 0;
+}
+
+static int len_ext_prop(struct usb_configuration *c, int interface)
+{
+	struct usb_function *f;
+	struct usb_os_desc *d;
+	int j, res;
+
+	res = 10; /* header length */
+	f = c->interface[interface];
+	for (j = 0; j < f->os_desc_n; ++j) {
+		if (interface != f->os_desc_table[j].if_id)
+			continue;
+		d = f->os_desc_table[j].os_desc;
+		if (d)
+			return min(res + d->ext_prop_len, 4096);
+	}
+	return res;
+}
+
+static int fill_ext_prop(struct usb_configuration *c, int interface, u8 *buf)
+{
+	struct usb_function *f;
+	struct usb_os_desc *d;
+	struct usb_os_desc_ext_prop *ext_prop;
+	int j, count, n, ret;
+	u8 *start = buf;
+
+	f = c->interface[interface];
+	for (j = 0; j < f->os_desc_n; ++j) {
+		if (interface != f->os_desc_table[j].if_id)
+			continue;
+		d = f->os_desc_table[j].os_desc;
+		if (d)
+			list_for_each_entry(ext_prop, &d->ext_prop, entry) {
+				/* 4kB minus header length */
+				n = buf - start;
+				if (n >= 4086)
+					return 0;
+
+				count = ext_prop->data_len +
+					ext_prop->name_len + 14;
+				if (count > 4086 - n)
+					return -EINVAL;
+				usb_ext_prop_put_size(buf, count);
+				usb_ext_prop_put_type(buf, ext_prop->type);
+				ret = usb_ext_prop_put_name(buf, ext_prop->name,
+							    ext_prop->name_len);
+				if (ret < 0)
+					return ret;
+				switch (ext_prop->type) {
+				case USB_EXT_PROP_UNICODE:
+				case USB_EXT_PROP_UNICODE_ENV:
+				case USB_EXT_PROP_UNICODE_LINK:
+					usb_ext_prop_put_unicode(buf, ret,
+							 ext_prop->data,
+							 ext_prop->data_len);
+					break;
+				case USB_EXT_PROP_BINARY:
+					usb_ext_prop_put_binary(buf, ret,
+							ext_prop->data,
+							ext_prop->data_len);
+					break;
+				case USB_EXT_PROP_LE32:
+					/* not implemented */
+				case USB_EXT_PROP_BE32:
+					/* not implemented */
+				default:
+					return -EINVAL;
+				}
+				buf += count;
+			}
+	}
+
+	return 0;
+}
+
 /*
  * The setup() callback implements all the ep0 functionality that's
  * not handled lower down, in hardware or the hardware driver(like
@@ -1415,6 +1611,91 @@ composite_setup(struct usb_gadget *gadget, const struct usb_ctrlrequest *ctrl)
 		break;
 	default:
 unknown:
+		/*
+		 * OS descriptors handling
+		 */
+		if (cdev->use_os_string && cdev->os_desc_config &&
+		    (ctrl->bRequest & USB_TYPE_VENDOR) &&
+		    ctrl->bRequest == cdev->b_vendor_code) {
+			struct usb_request		*req;
+			struct usb_configuration	*os_desc_cfg;
+			u8				*buf;
+			int				interface;
+			int				count = 0;
+
+			req = cdev->os_desc_req;
+			req->complete = composite_setup_complete;
+			buf = req->buf;
+			os_desc_cfg = cdev->os_desc_config;
+			memset(buf, 0, w_length);
+			buf[5] = 0x01;
+			switch (ctrl->bRequestType & USB_RECIP_MASK) {
+			case USB_RECIP_DEVICE:
+				if (w_index != 0x4 || (w_value >> 8))
+					break;
+				buf[6] = w_index;
+				if (w_length == 0x10) {
+					/* Number of ext compat interfaces */
+					count = count_ext_compat(os_desc_cfg);
+					buf[8] = count;
+					count *= 24; /* 24 B/ext compat desc */
+					count += 16; /* header */
+					put_unaligned_le32(count, buf);
+					value = w_length;
+				} else {
+					/* "extended compatibility ID"s */
+					count = count_ext_compat(os_desc_cfg);
+					buf[8] = count;
+					count *= 24; /* 24 B/ext compat desc */
+					count += 16; /* header */
+					put_unaligned_le32(count, buf);
+					buf += 16;
+					fill_ext_compat(os_desc_cfg, buf);
+					value = w_length;
+				}
+				break;
+			case USB_RECIP_INTERFACE:
+				if (w_index != 0x5 || (w_value >> 8))
+					break;
+				interface = w_value & 0xFF;
+				buf[6] = w_index;
+				if (w_length == 0x0A) {
+					count = count_ext_prop(os_desc_cfg,
+						interface);
+					put_unaligned_le16(count, buf + 8);
+					count = len_ext_prop(os_desc_cfg,
+						interface);
+					put_unaligned_le32(count, buf);
+
+					value = w_length;
+				} else {
+					count = count_ext_prop(os_desc_cfg,
+						interface);
+					put_unaligned_le16(count, buf + 8);
+					count = len_ext_prop(os_desc_cfg,
+						interface);
+					put_unaligned_le32(count, buf);
+					buf += 10;
+					value = fill_ext_prop(os_desc_cfg,
+							      interface, buf);
+					if (value < 0)
+						return value;
+
+					value = w_length;
+				}
+				break;
+			}
+			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);
+			}
+			return value;
+		}
+
 		VDBG(cdev,
 			"non-core control req%02x.%02x v%04x i%04x l%d\n",
 			ctrl->bRequestType, ctrl->bRequest,
@@ -1638,6 +1919,29 @@ fail:
 	return ret;
 }
 
+int composite_os_desc_req_prepare(struct usb_composite_dev *cdev,
+				  struct usb_ep *ep0)
+{
+	int ret = 0;
+
+	cdev->os_desc_req = usb_ep_alloc_request(ep0, GFP_KERNEL);
+	if (!cdev->os_desc_req) {
+		ret = PTR_ERR(cdev->os_desc_req);
+		goto end;
+	}
+
+	/* OS feature descriptor length <= 4kB */
+	cdev->os_desc_req->buf = kmalloc(4096, GFP_KERNEL);
+	if (!cdev->os_desc_req->buf) {
+		ret = PTR_ERR(cdev->os_desc_req->buf);
+		kfree(cdev->os_desc_req);
+		goto end;
+	}
+	cdev->os_desc_req->complete = composite_setup_complete;
+end:
+	return ret;
+}
+
 void composite_dev_cleanup(struct usb_composite_dev *cdev)
 {
 	struct usb_gadget_string_container *uc, *tmp;
@@ -1646,6 +1950,10 @@ void composite_dev_cleanup(struct usb_composite_dev *cdev)
 		list_del(&uc->list);
 		kfree(uc);
 	}
+	if (cdev->os_desc_req) {
+		kfree(cdev->os_desc_req->buf);
+		usb_ep_free_request(cdev->gadget->ep0, cdev->os_desc_req);
+	}
 	if (cdev->req) {
 		kfree(cdev->req->buf);
 		usb_ep_free_request(cdev->gadget->ep0, cdev->req);
@@ -1683,6 +1991,12 @@ static int composite_bind(struct usb_gadget *gadget,
 	if (status < 0)
 		goto fail;
 
+	if (cdev->use_os_string) {
+		status = composite_os_desc_req_prepare(cdev, gadget->ep0);
+		if (status)
+			goto fail;
+	}
+
 	update_unchanged_dev_desc(&cdev->desc, composite->dev);
 
 	/* has userspace failed to provide a serial number? */
diff --git a/drivers/usb/gadget/configfs.c b/drivers/usb/gadget/configfs.c
index 7d1cc01796b..2ddcd635ca2 100644
--- a/drivers/usb/gadget/configfs.c
+++ b/drivers/usb/gadget/configfs.c
@@ -2,9 +2,12 @@
 #include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/device.h>
+#include <linux/nls.h>
 #include <linux/usb/composite.h>
 #include <linux/usb/gadget_configfs.h>
 #include "configfs.h"
+#include "u_f.h"
+#include "u_os_desc.h"
 
 int check_user_usb_string(const char *name,
 		struct usb_gadget_strings *stringtab_dev)
@@ -43,7 +46,8 @@ struct gadget_info {
 	struct config_group functions_group;
 	struct config_group configs_group;
 	struct config_group strings_group;
-	struct config_group *default_groups[4];
+	struct config_group os_desc_group;
+	struct config_group *default_groups[5];
 
 	struct mutex lock;
 	struct usb_gadget_strings *gstrings[MAX_USB_STRING_LANGS + 1];
@@ -56,6 +60,9 @@ struct gadget_info {
 #endif
 	struct usb_composite_driver composite;
 	struct usb_composite_dev cdev;
+	bool use_os_desc;
+	char b_vendor_code;
+	char qw_sign[OS_STRING_QW_SIGN_LEN];
 };
 
 struct config_usb_cfg {
@@ -79,6 +86,10 @@ struct gadget_strings {
 	struct list_head list;
 };
 
+struct os_desc {
+	struct config_group group;
+};
+
 struct gadget_config_name {
 	struct usb_gadget_strings stringtab_dev;
 	struct usb_string strings;
@@ -736,6 +747,525 @@ static void gadget_strings_attr_release(struct config_item *item)
 USB_CONFIG_STRING_RW_OPS(gadget_strings);
 USB_CONFIG_STRINGS_LANG(gadget_strings, gadget_info);
 
+static inline struct os_desc *to_os_desc(struct config_item *item)
+{
+	return container_of(to_config_group(item), struct os_desc, group);
+}
+
+CONFIGFS_ATTR_STRUCT(os_desc);
+CONFIGFS_ATTR_OPS(os_desc);
+
+static ssize_t os_desc_use_show(struct os_desc *os_desc, char *page)
+{
+	struct gadget_info *gi;
+
+	gi = to_gadget_info(os_desc->group.cg_item.ci_parent);
+
+	return sprintf(page, "%d", gi->use_os_desc);
+}
+
+static ssize_t os_desc_use_store(struct os_desc *os_desc, const char *page,
+				 size_t len)
+{
+	struct gadget_info *gi;
+	int ret;
+	bool use;
+
+	gi = to_gadget_info(os_desc->group.cg_item.ci_parent);
+
+	mutex_lock(&gi->lock);
+	ret = strtobool(page, &use);
+	if (!ret) {
+		gi->use_os_desc = use;
+		ret = len;
+	}
+	mutex_unlock(&gi->lock);
+
+	return ret;
+}
+
+static struct os_desc_attribute os_desc_use =
+	__CONFIGFS_ATTR(use, S_IRUGO | S_IWUSR,
+			os_desc_use_show,
+			os_desc_use_store);
+
+static ssize_t os_desc_b_vendor_code_show(struct os_desc *os_desc, char *page)
+{
+	struct gadget_info *gi;
+
+	gi = to_gadget_info(os_desc->group.cg_item.ci_parent);
+
+	return sprintf(page, "%d", gi->b_vendor_code);
+}
+
+static ssize_t os_desc_b_vendor_code_store(struct os_desc *os_desc,
+					   const char *page, size_t len)
+{
+	struct gadget_info *gi;
+	int ret;
+	u8 b_vendor_code;
+
+	gi = to_gadget_info(os_desc->group.cg_item.ci_parent);
+
+	mutex_lock(&gi->lock);
+	ret = kstrtou8(page, 0, &b_vendor_code);
+	if (!ret) {
+		gi->b_vendor_code = b_vendor_code;
+		ret = len;
+	}
+	mutex_unlock(&gi->lock);
+
+	return ret;
+}
+
+static struct os_desc_attribute os_desc_b_vendor_code =
+	__CONFIGFS_ATTR(b_vendor_code, S_IRUGO | S_IWUSR,
+			os_desc_b_vendor_code_show,
+			os_desc_b_vendor_code_store);
+
+static ssize_t os_desc_qw_sign_show(struct os_desc *os_desc, char *page)
+{
+	struct gadget_info *gi;
+
+	gi = to_gadget_info(os_desc->group.cg_item.ci_parent);
+
+	memcpy(page, gi->qw_sign, OS_STRING_QW_SIGN_LEN);
+
+	return OS_STRING_QW_SIGN_LEN;
+}
+
+static ssize_t os_desc_qw_sign_store(struct os_desc *os_desc, const char *page,
+				     size_t len)
+{
+	struct gadget_info *gi;
+	int res, l;
+
+	gi = to_gadget_info(os_desc->group.cg_item.ci_parent);
+	l = min((int)len, OS_STRING_QW_SIGN_LEN >> 1);
+	if (page[l - 1] == '\n')
+		--l;
+
+	mutex_lock(&gi->lock);
+	res = utf8s_to_utf16s(page, l,
+			      UTF16_LITTLE_ENDIAN, (wchar_t *) gi->qw_sign,
+			      OS_STRING_QW_SIGN_LEN);
+	if (res > 0)
+		res = len;
+	mutex_unlock(&gi->lock);
+
+	return res;
+}
+
+static struct os_desc_attribute os_desc_qw_sign =
+	__CONFIGFS_ATTR(qw_sign, S_IRUGO | S_IWUSR,
+			os_desc_qw_sign_show,
+			os_desc_qw_sign_store);
+
+static struct configfs_attribute *os_desc_attrs[] = {
+	&os_desc_use.attr,
+	&os_desc_b_vendor_code.attr,
+	&os_desc_qw_sign.attr,
+	NULL,
+};
+
+static void os_desc_attr_release(struct config_item *item)
+{
+	struct os_desc *os_desc = to_os_desc(item);
+	kfree(os_desc);
+}
+
+static int os_desc_link(struct config_item *os_desc_ci,
+			struct config_item *usb_cfg_ci)
+{
+	struct gadget_info *gi = container_of(to_config_group(os_desc_ci),
+					struct gadget_info, os_desc_group);
+	struct usb_composite_dev *cdev = &gi->cdev;
+	struct config_usb_cfg *c_target =
+		container_of(to_config_group(usb_cfg_ci),
+			     struct config_usb_cfg, group);
+	struct usb_configuration *c;
+	int ret;
+
+	mutex_lock(&gi->lock);
+	list_for_each_entry(c, &cdev->configs, list) {
+		if (c == &c_target->c)
+			break;
+	}
+	if (c != &c_target->c) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (cdev->os_desc_config) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	cdev->os_desc_config = &c_target->c;
+	ret = 0;
+
+out:
+	mutex_unlock(&gi->lock);
+	return ret;
+}
+
+static int os_desc_unlink(struct config_item *os_desc_ci,
+			  struct config_item *usb_cfg_ci)
+{
+	struct gadget_info *gi = container_of(to_config_group(os_desc_ci),
+					struct gadget_info, os_desc_group);
+	struct usb_composite_dev *cdev = &gi->cdev;
+
+	mutex_lock(&gi->lock);
+	if (gi->udc_name)
+		unregister_gadget(gi);
+	cdev->os_desc_config = NULL;
+	WARN_ON(gi->udc_name);
+	mutex_unlock(&gi->lock);
+	return 0;
+}
+
+static struct configfs_item_operations os_desc_ops = {
+	.release                = os_desc_attr_release,
+	.show_attribute         = os_desc_attr_show,
+	.store_attribute        = os_desc_attr_store,
+	.allow_link		= os_desc_link,
+	.drop_link		= os_desc_unlink,
+};
+
+static struct config_item_type os_desc_type = {
+	.ct_item_ops	= &os_desc_ops,
+	.ct_attrs	= os_desc_attrs,
+	.ct_owner	= THIS_MODULE,
+};
+
+CONFIGFS_ATTR_STRUCT(usb_os_desc);
+CONFIGFS_ATTR_OPS(usb_os_desc);
+
+
+static inline struct usb_os_desc_ext_prop
+*to_usb_os_desc_ext_prop(struct config_item *item)
+{
+	return container_of(item, struct usb_os_desc_ext_prop, item);
+}
+
+CONFIGFS_ATTR_STRUCT(usb_os_desc_ext_prop);
+CONFIGFS_ATTR_OPS(usb_os_desc_ext_prop);
+
+static ssize_t ext_prop_type_show(struct usb_os_desc_ext_prop *ext_prop,
+				  char *page)
+{
+	return sprintf(page, "%d", ext_prop->type);
+}
+
+static ssize_t ext_prop_type_store(struct usb_os_desc_ext_prop *ext_prop,
+				   const char *page, size_t len)
+{
+	struct usb_os_desc *desc = to_usb_os_desc(ext_prop->item.ci_parent);
+	u8 type;
+	int ret;
+
+	if (desc->opts_mutex)
+		mutex_lock(desc->opts_mutex);
+	ret = kstrtou8(page, 0, &type);
+	if (ret)
+		goto end;
+	if (type < USB_EXT_PROP_UNICODE || type > USB_EXT_PROP_UNICODE_MULTI) {
+		ret = -EINVAL;
+		goto end;
+	}
+
+	if ((ext_prop->type == USB_EXT_PROP_BINARY ||
+	    ext_prop->type == USB_EXT_PROP_LE32 ||
+	    ext_prop->type == USB_EXT_PROP_BE32) &&
+	    (type == USB_EXT_PROP_UNICODE ||
+	    type == USB_EXT_PROP_UNICODE_ENV ||
+	    type == USB_EXT_PROP_UNICODE_LINK))
+		ext_prop->data_len <<= 1;
+	else if ((ext_prop->type == USB_EXT_PROP_UNICODE ||
+		   ext_prop->type == USB_EXT_PROP_UNICODE_ENV ||
+		   ext_prop->type == USB_EXT_PROP_UNICODE_LINK) &&
+		   (type == USB_EXT_PROP_BINARY ||
+		   type == USB_EXT_PROP_LE32 ||
+		   type == USB_EXT_PROP_BE32))
+		ext_prop->data_len >>= 1;
+	ext_prop->type = type;
+	ret = len;
+
+end:
+	if (desc->opts_mutex)
+		mutex_unlock(desc->opts_mutex);
+	return ret;
+}
+
+static ssize_t ext_prop_data_show(struct usb_os_desc_ext_prop *ext_prop,
+				  char *page)
+{
+	int len = ext_prop->data_len;
+
+	if (ext_prop->type == USB_EXT_PROP_UNICODE ||
+	    ext_prop->type == USB_EXT_PROP_UNICODE_ENV ||
+	    ext_prop->type == USB_EXT_PROP_UNICODE_LINK)
+		len >>= 1;
+	memcpy(page, ext_prop->data, len);
+
+	return len;
+}
+
+static ssize_t ext_prop_data_store(struct usb_os_desc_ext_prop *ext_prop,
+				   const char *page, size_t len)
+{
+	struct usb_os_desc *desc = to_usb_os_desc(ext_prop->item.ci_parent);
+	char *new_data;
+	size_t ret_len = len;
+
+	if (page[len - 1] == '\n' || page[len - 1] == '\0')
+		--len;
+	new_data = kzalloc(len, GFP_KERNEL);
+	if (!new_data)
+		return -ENOMEM;
+
+	memcpy(new_data, page, len);
+
+	if (desc->opts_mutex)
+		mutex_lock(desc->opts_mutex);
+	kfree(ext_prop->data);
+	ext_prop->data = new_data;
+	desc->ext_prop_len -= ext_prop->data_len;
+	ext_prop->data_len = len;
+	desc->ext_prop_len += ext_prop->data_len;
+	if (ext_prop->type == USB_EXT_PROP_UNICODE ||
+	    ext_prop->type == USB_EXT_PROP_UNICODE_ENV ||
+	    ext_prop->type == USB_EXT_PROP_UNICODE_LINK) {
+		desc->ext_prop_len -= ext_prop->data_len;
+		ext_prop->data_len <<= 1;
+		ext_prop->data_len += 2;
+		desc->ext_prop_len += ext_prop->data_len;
+	}
+	if (desc->opts_mutex)
+		mutex_unlock(desc->opts_mutex);
+	return ret_len;
+}
+
+static struct usb_os_desc_ext_prop_attribute ext_prop_type =
+	__CONFIGFS_ATTR(type, S_IRUGO | S_IWUSR,
+			ext_prop_type_show, ext_prop_type_store);
+
+static struct usb_os_desc_ext_prop_attribute ext_prop_data =
+	__CONFIGFS_ATTR(data, S_IRUGO | S_IWUSR,
+			ext_prop_data_show, ext_prop_data_store);
+
+static struct configfs_attribute *ext_prop_attrs[] = {
+	&ext_prop_type.attr,
+	&ext_prop_data.attr,
+	NULL,
+};
+
+static void usb_os_desc_ext_prop_release(struct config_item *item)
+{
+	struct usb_os_desc_ext_prop *ext_prop = to_usb_os_desc_ext_prop(item);
+
+	kfree(ext_prop); /* frees a whole chunk */
+}
+
+static struct configfs_item_operations ext_prop_ops = {
+	.release		= usb_os_desc_ext_prop_release,
+	.show_attribute		= usb_os_desc_ext_prop_attr_show,
+	.store_attribute	= usb_os_desc_ext_prop_attr_store,
+};
+
+static struct config_item *ext_prop_make(
+		struct config_group *group,
+		const char *name)
+{
+	struct usb_os_desc_ext_prop *ext_prop;
+	struct config_item_type *ext_prop_type;
+	struct usb_os_desc *desc;
+	char *vlabuf;
+
+	vla_group(data_chunk);
+	vla_item(data_chunk, struct usb_os_desc_ext_prop, ext_prop, 1);
+	vla_item(data_chunk, struct config_item_type, ext_prop_type, 1);
+
+	vlabuf = kzalloc(vla_group_size(data_chunk), GFP_KERNEL);
+	if (!vlabuf)
+		return ERR_PTR(-ENOMEM);
+
+	ext_prop = vla_ptr(vlabuf, data_chunk, ext_prop);
+	ext_prop_type = vla_ptr(vlabuf, data_chunk, ext_prop_type);
+
+	desc = container_of(group, struct usb_os_desc, group);
+	ext_prop_type->ct_item_ops = &ext_prop_ops;
+	ext_prop_type->ct_attrs = ext_prop_attrs;
+	ext_prop_type->ct_owner = desc->owner;
+
+	config_item_init_type_name(&ext_prop->item, name, ext_prop_type);
+
+	ext_prop->name = kstrdup(name, GFP_KERNEL);
+	if (!ext_prop->name) {
+		kfree(vlabuf);
+		return ERR_PTR(-ENOMEM);
+	}
+	desc->ext_prop_len += 14;
+	ext_prop->name_len = 2 * strlen(ext_prop->name) + 2;
+	if (desc->opts_mutex)
+		mutex_lock(desc->opts_mutex);
+	desc->ext_prop_len += ext_prop->name_len;
+	list_add_tail(&ext_prop->entry, &desc->ext_prop);
+	++desc->ext_prop_count;
+	if (desc->opts_mutex)
+		mutex_unlock(desc->opts_mutex);
+
+	return &ext_prop->item;
+}
+
+static void ext_prop_drop(struct config_group *group, struct config_item *item)
+{
+	struct usb_os_desc_ext_prop *ext_prop = to_usb_os_desc_ext_prop(item);
+	struct usb_os_desc *desc = to_usb_os_desc(&group->cg_item);
+
+	if (desc->opts_mutex)
+		mutex_lock(desc->opts_mutex);
+	list_del(&ext_prop->entry);
+	--desc->ext_prop_count;
+	kfree(ext_prop->name);
+	desc->ext_prop_len -= (ext_prop->name_len + ext_prop->data_len + 14);
+	if (desc->opts_mutex)
+		mutex_unlock(desc->opts_mutex);
+	config_item_put(item);
+}
+
+static struct configfs_group_operations interf_grp_ops = {
+	.make_item	= &ext_prop_make,
+	.drop_item	= &ext_prop_drop,
+};
+
+static struct configfs_item_operations interf_item_ops = {
+	.show_attribute		= usb_os_desc_attr_show,
+	.store_attribute	= usb_os_desc_attr_store,
+};
+
+static ssize_t rndis_grp_compatible_id_show(struct usb_os_desc *desc,
+					    char *page)
+{
+	memcpy(page, desc->ext_compat_id, 8);
+	return 8;
+}
+
+static ssize_t rndis_grp_compatible_id_store(struct usb_os_desc *desc,
+					     const char *page, size_t len)
+{
+	int l;
+
+	l = min_t(int, 8, len);
+	if (page[l - 1] == '\n')
+		--l;
+	if (desc->opts_mutex)
+		mutex_lock(desc->opts_mutex);
+	memcpy(desc->ext_compat_id, page, l);
+	desc->ext_compat_id[l] = '\0';
+
+	if (desc->opts_mutex)
+		mutex_unlock(desc->opts_mutex);
+
+	return len;
+}
+
+static struct usb_os_desc_attribute rndis_grp_attr_compatible_id =
+	__CONFIGFS_ATTR(compatible_id, S_IRUGO | S_IWUSR,
+			rndis_grp_compatible_id_show,
+			rndis_grp_compatible_id_store);
+
+static ssize_t rndis_grp_sub_compatible_id_show(struct usb_os_desc *desc,
+						char *page)
+{
+	memcpy(page, desc->ext_compat_id + 8, 8);
+	return 8;
+}
+
+static ssize_t rndis_grp_sub_compatible_id_store(struct usb_os_desc *desc,
+						 const char *page, size_t len)
+{
+	int l;
+
+	l = min_t(int, 8, len);
+	if (page[l - 1] == '\n')
+		--l;
+	if (desc->opts_mutex)
+		mutex_lock(desc->opts_mutex);
+	memcpy(desc->ext_compat_id + 8, page, l);
+	desc->ext_compat_id[l + 8] = '\0';
+
+	if (desc->opts_mutex)
+		mutex_unlock(desc->opts_mutex);
+
+	return len;
+}
+
+static struct usb_os_desc_attribute rndis_grp_attr_sub_compatible_id =
+	__CONFIGFS_ATTR(sub_compatible_id, S_IRUGO | S_IWUSR,
+			rndis_grp_sub_compatible_id_show,
+			rndis_grp_sub_compatible_id_store);
+
+static struct configfs_attribute *interf_grp_attrs[] = {
+	&rndis_grp_attr_compatible_id.attr,
+	&rndis_grp_attr_sub_compatible_id.attr,
+	NULL
+};
+
+int usb_os_desc_prepare_interf_dir(struct config_group *parent,
+				   int n_interf,
+				   struct usb_os_desc **desc,
+				   struct module *owner)
+{
+	struct config_group **f_default_groups, *os_desc_group,
+				**interface_groups;
+	struct config_item_type *os_desc_type, *interface_type;
+
+	vla_group(data_chunk);
+	vla_item(data_chunk, struct config_group *, f_default_groups, 2);
+	vla_item(data_chunk, struct config_group, os_desc_group, 1);
+	vla_item(data_chunk, struct config_group *, interface_groups,
+		 n_interf + 1);
+	vla_item(data_chunk, struct config_item_type, os_desc_type, 1);
+	vla_item(data_chunk, struct config_item_type, interface_type, 1);
+
+	char *vlabuf = kzalloc(vla_group_size(data_chunk), GFP_KERNEL);
+	if (!vlabuf)
+		return -ENOMEM;
+
+	f_default_groups = vla_ptr(vlabuf, data_chunk, f_default_groups);
+	os_desc_group = vla_ptr(vlabuf, data_chunk, os_desc_group);
+	os_desc_type = vla_ptr(vlabuf, data_chunk, os_desc_type);
+	interface_groups = vla_ptr(vlabuf, data_chunk, interface_groups);
+	interface_type = vla_ptr(vlabuf, data_chunk, interface_type);
+
+	parent->default_groups = f_default_groups;
+	os_desc_type->ct_owner = owner;
+	config_group_init_type_name(os_desc_group, "os_desc", os_desc_type);
+	f_default_groups[0] = os_desc_group;
+
+	os_desc_group->default_groups = interface_groups;
+	interface_type->ct_item_ops = &interf_item_ops;
+	interface_type->ct_group_ops = &interf_grp_ops;
+	interface_type->ct_attrs = interf_grp_attrs;
+	interface_type->ct_owner = owner;
+
+	while (n_interf--) {
+		struct usb_os_desc *d;
+
+		d = desc[n_interf];
+		d->owner = owner;
+		config_group_init_type_name(&d->group, "", interface_type);
+		config_item_set_name(&d->group.cg_item, "interface.%d",
+				     n_interf);
+		interface_groups[n_interf] = &d->group;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(usb_os_desc_prepare_interf_dir);
+
 static int configfs_do_nothing(struct usb_composite_dev *cdev)
 {
 	WARN_ON(1);
@@ -745,6 +1275,9 @@ static int configfs_do_nothing(struct usb_composite_dev *cdev)
 int composite_dev_prepare(struct usb_composite_driver *composite,
 		struct usb_composite_dev *dev);
 
+int composite_os_desc_req_prepare(struct usb_composite_dev *cdev,
+				  struct usb_ep *ep0);
+
 static void purge_configs_funcs(struct gadget_info *gi)
 {
 	struct usb_configuration	*c;
@@ -793,7 +1326,7 @@ static int configfs_composite_bind(struct usb_gadget *gadget,
 	ret = -EINVAL;
 
 	if (list_empty(&gi->cdev.configs)) {
-		pr_err("Need atleast one configuration in %s.\n",
+		pr_err("Need at least one configuration in %s.\n",
 				gi->composite.name);
 		goto err_comp_cleanup;
 	}
@@ -804,7 +1337,7 @@ static int configfs_composite_bind(struct usb_gadget *gadget,
 
 		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",
+			pr_err("Config %s/%d of %s needs at least one function.\n",
 			      c->label, c->bConfigurationValue,
 			      gi->composite.name);
 			goto err_comp_cleanup;
@@ -839,6 +1372,12 @@ static int configfs_composite_bind(struct usb_gadget *gadget,
 		gi->cdev.desc.iSerialNumber = s[USB_GADGET_SERIAL_IDX].id;
 	}
 
+	if (gi->use_os_desc) {
+		cdev->use_os_string = true;
+		cdev->b_vendor_code = gi->b_vendor_code;
+		memcpy(cdev->qw_sign, gi->qw_sign, OS_STRING_QW_SIGN_LEN);
+	}
+
 	/* Go through all configs, attach all functions */
 	list_for_each_entry(c, &gi->cdev.configs, list) {
 		struct config_usb_cfg *cfg;
@@ -874,6 +1413,12 @@ static int configfs_composite_bind(struct usb_gadget *gadget,
 		}
 		usb_ep_autoconfig_reset(cdev->gadget);
 	}
+	if (cdev->use_os_string) {
+		ret = composite_os_desc_req_prepare(cdev, gadget->ep0);
+		if (ret)
+			goto err_purge_funcs;
+	}
+
 	usb_ep_autoconfig_reset(cdev->gadget);
 	return 0;
 
@@ -929,6 +1474,7 @@ static struct config_group *gadgets_make(
 	gi->group.default_groups[0] = &gi->functions_group;
 	gi->group.default_groups[1] = &gi->configs_group;
 	gi->group.default_groups[2] = &gi->strings_group;
+	gi->group.default_groups[3] = &gi->os_desc_group;
 
 	config_group_init_type_name(&gi->functions_group, "functions",
 			&functions_type);
@@ -936,6 +1482,8 @@ static struct config_group *gadgets_make(
 			&config_desc_type);
 	config_group_init_type_name(&gi->strings_group, "strings",
 			&gadget_strings_strings_type);
+	config_group_init_type_name(&gi->os_desc_group, "os_desc",
+			&os_desc_type);
 
 	gi->composite.bind = configfs_do_nothing;
 	gi->composite.unbind = configfs_do_nothing;
@@ -1005,7 +1553,7 @@ void unregister_gadget_item(struct config_item *item)
 
 	unregister_gadget(gi);
 }
-EXPORT_SYMBOL(unregister_gadget_item);
+EXPORT_SYMBOL_GPL(unregister_gadget_item);
 
 static int __init gadget_cfs_init(void)
 {
diff --git a/drivers/usb/gadget/configfs.h b/drivers/usb/gadget/configfs.h
index a7b564a913d..a14ac792c69 100644
--- a/drivers/usb/gadget/configfs.h
+++ b/drivers/usb/gadget/configfs.h
@@ -1,6 +1,18 @@
 #ifndef USB__GADGET__CONFIGFS__H
 #define USB__GADGET__CONFIGFS__H
 
+#include <linux/configfs.h>
+
 void unregister_gadget_item(struct config_item *item);
 
+int usb_os_desc_prepare_interf_dir(struct config_group *parent,
+				   int n_interf,
+				   struct usb_os_desc **desc,
+				   struct module *owner);
+
+static inline struct usb_os_desc *to_usb_os_desc(struct config_item *item)
+{
+	return container_of(to_config_group(item), struct usb_os_desc, group);
+}
+
 #endif /*  USB__GADGET__CONFIGFS__H */
diff --git a/drivers/usb/gadget/dummy_hcd.c b/drivers/usb/gadget/dummy_hcd.c
index 8c06430dcc4..2b54955d316 100644
--- a/drivers/usb/gadget/dummy_hcd.c
+++ b/drivers/usb/gadget/dummy_hcd.c
@@ -561,7 +561,6 @@ static int dummy_disable(struct usb_ep *_ep)
 	struct dummy_ep		*ep;
 	struct dummy		*dum;
 	unsigned long		flags;
-	int			retval;
 
 	ep = usb_ep_to_dummy_ep(_ep);
 	if (!_ep || !ep->desc || _ep->name == ep0name)
@@ -571,12 +570,11 @@ static int dummy_disable(struct usb_ep *_ep)
 	spin_lock_irqsave(&dum->lock, flags);
 	ep->desc = NULL;
 	ep->stream_en = 0;
-	retval = 0;
 	nuke(dum, ep);
 	spin_unlock_irqrestore(&dum->lock, flags);
 
 	dev_dbg(udc_dev(dum), "disabled %s\n", _ep->name);
-	return retval;
+	return 0;
 }
 
 static struct usb_request *dummy_alloc_request(struct usb_ep *_ep,
diff --git a/drivers/usb/gadget/f_fs.c b/drivers/usb/gadget/f_fs.c
index 2e164dca08e..74202d67f91 100644
--- a/drivers/usb/gadget/f_fs.c
+++ b/drivers/usb/gadget/f_fs.c
@@ -33,36 +33,11 @@
 #include <linux/poll.h>
 
 #include "u_fs.h"
+#include "u_f.h"
 #include "configfs.h"
 
 #define FUNCTIONFS_MAGIC	0xa647361 /* Chosen by a honest dice roll ;) */
 
-/* Variable Length Array Macros **********************************************/
-#define vla_group(groupname) size_t groupname##__next = 0
-#define vla_group_size(groupname) groupname##__next
-
-#define vla_item(groupname, type, name, n) \
-	size_t groupname##_##name##__offset = ({			       \
-		size_t align_mask = __alignof__(type) - 1;		       \
-		size_t offset = (groupname##__next + align_mask) & ~align_mask;\
-		size_t size = (n) * sizeof(type);			       \
-		groupname##__next = offset + size;			       \
-		offset;							       \
-	})
-
-#define vla_item_with_sz(groupname, type, name, n) \
-	size_t groupname##_##name##__sz = (n) * sizeof(type);		       \
-	size_t groupname##_##name##__offset = ({			       \
-		size_t align_mask = __alignof__(type) - 1;		       \
-		size_t offset = (groupname##__next + align_mask) & ~align_mask;\
-		size_t size = groupname##_##name##__sz;			       \
-		groupname##__next = offset + size;			       \
-		offset;							       \
-	})
-
-#define vla_ptr(ptr, groupname, name) \
-	((void *) ((char *)ptr + groupname##_##name##__offset))
-
 /* Reference counter handling */
 static void ffs_data_get(struct ffs_data *ffs);
 static void ffs_data_put(struct ffs_data *ffs);
@@ -190,7 +165,7 @@ ffs_sb_create_file(struct super_block *sb, const char *name, void *data,
 /* Devices management *******************************************************/
 
 DEFINE_MUTEX(ffs_lock);
-EXPORT_SYMBOL(ffs_lock);
+EXPORT_SYMBOL_GPL(ffs_lock);
 
 static struct ffs_dev *_ffs_find_dev(const char *name);
 static struct ffs_dev *_ffs_alloc_dev(void);
@@ -745,6 +720,12 @@ static ssize_t ffs_epfile_io(struct file *file, struct ffs_io_data *io_data)
 		 */
 		struct usb_gadget *gadget = epfile->ffs->gadget;
 
+		spin_lock_irq(&epfile->ffs->eps_lock);
+		/* In the meantime, endpoint got disabled or changed. */
+		if (epfile->ep != ep) {
+			spin_unlock_irq(&epfile->ffs->eps_lock);
+			return -ESHUTDOWN;
+		}
 		/*
 		 * Controller may require buffer size to be aligned to
 		 * maxpacketsize of an out endpoint.
@@ -752,6 +733,7 @@ static ssize_t ffs_epfile_io(struct file *file, struct ffs_io_data *io_data)
 		data_len = io_data->read ?
 			   usb_ep_align_maybe(gadget, ep->ep, io_data->len) :
 			   io_data->len;
+		spin_unlock_irq(&epfile->ffs->eps_lock);
 
 		data = kmalloc(data_len, GFP_KERNEL);
 		if (unlikely(!data))
@@ -2876,7 +2858,7 @@ int ffs_name_dev(struct ffs_dev *dev, const char *name)
 
 	return ret;
 }
-EXPORT_SYMBOL(ffs_name_dev);
+EXPORT_SYMBOL_GPL(ffs_name_dev);
 
 int ffs_single_dev(struct ffs_dev *dev)
 {
@@ -2893,7 +2875,7 @@ int ffs_single_dev(struct ffs_dev *dev)
 	ffs_dev_unlock();
 	return ret;
 }
-EXPORT_SYMBOL(ffs_single_dev);
+EXPORT_SYMBOL_GPL(ffs_single_dev);
 
 /*
  * ffs_lock must be taken by the caller of this function
diff --git a/drivers/usb/gadget/f_rndis.c b/drivers/usb/gadget/f_rndis.c
index c11761ce511..eed3ad87804 100644
--- a/drivers/usb/gadget/f_rndis.c
+++ b/drivers/usb/gadget/f_rndis.c
@@ -27,6 +27,7 @@
 #include "u_ether_configfs.h"
 #include "u_rndis.h"
 #include "rndis.h"
+#include "configfs.h"
 
 /*
  * This function is an RNDIS Ethernet port -- a Microsoft protocol that's
@@ -377,7 +378,7 @@ static struct sk_buff *rndis_add_header(struct gether *port,
 	if (skb2)
 		rndis_add_hdr(skb2);
 
-	dev_kfree_skb_any(skb);
+	dev_kfree_skb(skb);
 	return skb2;
 }
 
@@ -682,6 +683,15 @@ rndis_bind(struct usb_configuration *c, struct usb_function *f)
 
 	rndis_opts = container_of(f->fi, struct f_rndis_opts, func_inst);
 
+	if (cdev->use_os_string) {
+		f->os_desc_table = kzalloc(sizeof(*f->os_desc_table),
+					   GFP_KERNEL);
+		if (!f->os_desc_table)
+			return PTR_ERR(f->os_desc_table);
+		f->os_desc_n = 1;
+		f->os_desc_table[0].os_desc = &rndis_opts->rndis_os_desc;
+	}
+
 	/*
 	 * in drivers/usb/gadget/configfs.c:configfs_composite_bind()
 	 * configurations are bound in sequence with list_for_each_entry,
@@ -693,14 +703,16 @@ rndis_bind(struct usb_configuration *c, struct usb_function *f)
 		gether_set_gadget(rndis_opts->net, cdev->gadget);
 		status = gether_register_netdev(rndis_opts->net);
 		if (status)
-			return status;
+			goto fail;
 		rndis_opts->bound = true;
 	}
 
 	us = usb_gstrings_attach(cdev, rndis_strings,
 				 ARRAY_SIZE(rndis_string_defs));
-	if (IS_ERR(us))
-		return PTR_ERR(us);
+	if (IS_ERR(us)) {
+		status = PTR_ERR(us);
+		goto fail;
+	}
 	rndis_control_intf.iInterface = us[0].id;
 	rndis_data_intf.iInterface = us[1].id;
 	rndis_iad_descriptor.iFunction = us[2].id;
@@ -802,6 +814,8 @@ rndis_bind(struct usb_configuration *c, struct usb_function *f)
 	return 0;
 
 fail:
+	kfree(f->os_desc_table);
+	f->os_desc_n = 0;
 	usb_free_all_descriptors(f);
 
 	if (rndis->notify_req) {
@@ -834,7 +848,7 @@ void rndis_borrow_net(struct usb_function_instance *f, struct net_device *net)
 	opts->borrowed_net = opts->bound = true;
 	opts->net = net;
 }
-EXPORT_SYMBOL(rndis_borrow_net);
+EXPORT_SYMBOL_GPL(rndis_borrow_net);
 
 static inline struct f_rndis_opts *to_f_rndis_opts(struct config_item *item)
 {
@@ -882,16 +896,21 @@ static void rndis_free_inst(struct usb_function_instance *f)
 		else
 			free_netdev(opts->net);
 	}
+
+	kfree(opts->rndis_os_desc.group.default_groups); /* single VLA chunk */
 	kfree(opts);
 }
 
 static struct usb_function_instance *rndis_alloc_inst(void)
 {
 	struct f_rndis_opts *opts;
+	struct usb_os_desc *descs[1];
 
 	opts = kzalloc(sizeof(*opts), GFP_KERNEL);
 	if (!opts)
 		return ERR_PTR(-ENOMEM);
+	opts->rndis_os_desc.ext_compat_id = opts->rndis_ext_compat_id;
+
 	mutex_init(&opts->lock);
 	opts->func_inst.free_func_inst = rndis_free_inst;
 	opts->net = gether_setup_default();
@@ -900,7 +919,11 @@ static struct usb_function_instance *rndis_alloc_inst(void)
 		kfree(opts);
 		return ERR_CAST(net);
 	}
+	INIT_LIST_HEAD(&opts->rndis_os_desc.ext_prop);
 
+	descs[0] = &opts->rndis_os_desc;
+	usb_os_desc_prepare_interf_dir(&opts->func_inst.group, 1, descs,
+				       THIS_MODULE);
 	config_group_init_type_name(&opts->func_inst.group, "",
 				    &rndis_func_type);
 
@@ -925,6 +948,8 @@ static void rndis_unbind(struct usb_configuration *c, struct usb_function *f)
 {
 	struct f_rndis		*rndis = func_to_rndis(f);
 
+	kfree(f->os_desc_table);
+	f->os_desc_n = 0;
 	usb_free_all_descriptors(f);
 
 	kfree(rndis->notify_req->buf);
diff --git a/drivers/usb/gadget/f_subset.c b/drivers/usb/gadget/f_subset.c
index df4a0dcbc99..1ea8baf3333 100644
--- a/drivers/usb/gadget/f_subset.c
+++ b/drivers/usb/gadget/f_subset.c
@@ -276,7 +276,7 @@ static int geth_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
 	}
 
 	net = gether_connect(&geth->port);
-	return PTR_RET(net);
+	return PTR_ERR_OR_ZERO(net);
 }
 
 static void geth_disable(struct usb_function *f)
diff --git a/drivers/usb/gadget/f_uac2.c b/drivers/usb/gadget/f_uac2.c
index bc23040c779..6261db4a991 100644
--- a/drivers/usb/gadget/f_uac2.c
+++ b/drivers/usb/gadget/f_uac2.c
@@ -196,7 +196,7 @@ agdev_iso_complete(struct usb_ep *ep, struct usb_request *req)
 	struct snd_uac2_chip *uac2 = prm->uac2;
 
 	/* i/f shutting down */
-	if (!prm->ep_enabled)
+	if (!prm->ep_enabled || req->status == -ESHUTDOWN)
 		return;
 
 	/*
@@ -974,8 +974,6 @@ afunc_bind(struct usb_configuration *cfg, struct usb_function *fn)
 	prm->rbuf = kzalloc(prm->max_psize * USB_XFERS, GFP_KERNEL);
 	if (!prm->rbuf) {
 		prm->max_psize = 0;
-		dev_err(&uac2->pdev.dev,
-			"%s:%d Error!\n", __func__, __LINE__);
 		goto err;
 	}
 
@@ -984,8 +982,6 @@ afunc_bind(struct usb_configuration *cfg, struct usb_function *fn)
 	prm->rbuf = kzalloc(prm->max_psize * USB_XFERS, GFP_KERNEL);
 	if (!prm->rbuf) {
 		prm->max_psize = 0;
-		dev_err(&uac2->pdev.dev,
-			"%s:%d Error!\n", __func__, __LINE__);
 		goto err;
 	}
 
@@ -1298,10 +1294,8 @@ static int audio_bind_config(struct usb_configuration *cfg)
 	int res;
 
 	agdev_g = kzalloc(sizeof *agdev_g, GFP_KERNEL);
-	if (agdev_g == NULL) {
-		printk(KERN_ERR "Unable to allocate audio gadget\n");
+	if (agdev_g == NULL)
 		return -ENOMEM;
-	}
 
 	res = usb_string_ids_tab(cfg->cdev, strings_fn);
 	if (res)
diff --git a/drivers/usb/gadget/fotg210-udc.c b/drivers/usb/gadget/fotg210-udc.c
index 2d0305280e8..e143d69f601 100644
--- a/drivers/usb/gadget/fotg210-udc.c
+++ b/drivers/usb/gadget/fotg210-udc.c
@@ -1112,17 +1112,13 @@ static int fotg210_udc_probe(struct platform_device *pdev)
 
 	/* initialize udc */
 	fotg210 = kzalloc(sizeof(struct fotg210_udc), GFP_KERNEL);
-	if (fotg210 == NULL) {
-		pr_err("kzalloc error\n");
+	if (fotg210 == NULL)
 		goto err_alloc;
-	}
 
 	for (i = 0; i < FOTG210_MAX_NUM_EP; i++) {
 		_ep[i] = kzalloc(sizeof(struct fotg210_ep), GFP_KERNEL);
-		if (_ep[i] == NULL) {
-			pr_err("_ep kzalloc error\n");
+		if (_ep[i] == NULL)
 			goto err_alloc;
-		}
 		fotg210->ep[i] = _ep[i];
 	}
 
diff --git a/drivers/usb/gadget/fsl_udc_core.c b/drivers/usb/gadget/fsl_udc_core.c
index 15960af0f67..28e4fc95702 100644
--- a/drivers/usb/gadget/fsl_udc_core.c
+++ b/drivers/usb/gadget/fsl_udc_core.c
@@ -1219,6 +1219,10 @@ static int fsl_pullup(struct usb_gadget *gadget, int is_on)
 	struct fsl_udc *udc;
 
 	udc = container_of(gadget, struct fsl_udc, gadget);
+
+	if (!udc->vbus_active)
+		return -EOPNOTSUPP;
+
 	udc->softconnect = (is_on != 0);
 	if (can_pullup(udc))
 		fsl_writel((fsl_readl(&dr_regs->usbcmd) | USB_CMD_RUN_STOP),
@@ -2252,10 +2256,8 @@ static int __init struct_udc_setup(struct fsl_udc *udc,
 	udc->phy_mode = pdata->phy_mode;
 
 	udc->eps = kzalloc(sizeof(struct fsl_ep) * udc->max_ep, GFP_KERNEL);
-	if (!udc->eps) {
-		ERR("malloc fsl_ep failed\n");
+	if (!udc->eps)
 		return -1;
-	}
 
 	/* initialized QHs, take care of alignment */
 	size = udc->max_ep * sizeof(struct ep_queue_head);
@@ -2338,10 +2340,8 @@ static int __init fsl_udc_probe(struct platform_device *pdev)
 	u32 dccparams;
 
 	udc_controller = kzalloc(sizeof(struct fsl_udc), GFP_KERNEL);
-	if (udc_controller == NULL) {
-		ERR("malloc udc failed\n");
+	if (udc_controller == NULL)
 		return -ENOMEM;
-	}
 
 	pdata = dev_get_platdata(&pdev->dev);
 	udc_controller->pdata = pdata;
@@ -2532,8 +2532,8 @@ static int __exit fsl_udc_remove(struct platform_device *pdev)
 	if (!udc_controller)
 		return -ENODEV;
 
-	usb_del_gadget_udc(&udc_controller->gadget);
 	udc_controller->done = &done;
+	usb_del_gadget_udc(&udc_controller->gadget);
 
 	fsl_udc_clk_release();
 
diff --git a/drivers/usb/gadget/fusb300_udc.c b/drivers/usb/gadget/fusb300_udc.c
index 6423f1840ed..3deb4e93807 100644
--- a/drivers/usb/gadget/fusb300_udc.c
+++ b/drivers/usb/gadget/fusb300_udc.c
@@ -1400,17 +1400,13 @@ static int __init fusb300_probe(struct platform_device *pdev)
 
 	/* initialize udc */
 	fusb300 = kzalloc(sizeof(struct fusb300), GFP_KERNEL);
-	if (fusb300 == NULL) {
-		pr_err("kzalloc error\n");
+	if (fusb300 == NULL)
 		goto clean_up;
-	}
 
 	for (i = 0; i < FUSB300_MAX_NUM_EP; i++) {
 		_ep[i] = kzalloc(sizeof(struct fusb300_ep), GFP_KERNEL);
-		if (_ep[i] == NULL) {
-			pr_err("_ep kzalloc error\n");
+		if (_ep[i] == NULL)
 			goto clean_up;
-		}
 		fusb300->ep[i] = _ep[i];
 	}
 
diff --git a/drivers/usb/gadget/gr_udc.c b/drivers/usb/gadget/gr_udc.c
index f984ee75324..99a37ed03e2 100644
--- a/drivers/usb/gadget/gr_udc.c
+++ b/drivers/usb/gadget/gr_udc.c
@@ -143,6 +143,7 @@ static void gr_seq_ep_show(struct seq_file *seq, struct gr_ep *ep)
 	seq_printf(seq, "  wedged = %d\n", ep->wedged);
 	seq_printf(seq, "  callback = %d\n", ep->callback);
 	seq_printf(seq, "  maxpacket = %d\n", ep->ep.maxpacket);
+	seq_printf(seq, "  maxpacket_limit = %d\n", ep->ep.maxpacket_limit);
 	seq_printf(seq, "  bytes_per_buffer = %d\n", ep->bytes_per_buffer);
 	if (mode == 1 || mode == 3)
 		seq_printf(seq, "  nt = %d\n",
@@ -1541,6 +1542,10 @@ static int gr_ep_enable(struct usb_ep *_ep,
 	} else if (max == 0) {
 		dev_err(dev->dev, "Max payload cannot be set to 0\n");
 		return -EINVAL;
+	} else if (max > ep->ep.maxpacket_limit) {
+		dev_err(dev->dev, "Requested max payload %d > limit %d\n",
+			max, ep->ep.maxpacket_limit);
+		return -EINVAL;
 	}
 
 	spin_lock(&ep->dev->lock);
@@ -1679,7 +1684,7 @@ static int gr_queue_ext(struct usb_ep *_ep, struct usb_request *_req,
 	if (ep->is_in)
 		gr_dbgprint_request("EXTERN", ep, req);
 
-	ret = gr_queue(ep, req, gfp_flags);
+	ret = gr_queue(ep, req, GFP_ATOMIC);
 
 	spin_unlock(&ep->dev->lock);
 
@@ -1985,8 +1990,8 @@ static int gr_ep_init(struct gr_udc *dev, int num, int is_in, u32 maxplimit)
 	INIT_LIST_HEAD(&ep->queue);
 
 	if (num == 0) {
-		_req = gr_alloc_request(&ep->ep, GFP_KERNEL);
-		buf = devm_kzalloc(dev->dev, PAGE_SIZE, GFP_DMA | GFP_KERNEL);
+		_req = gr_alloc_request(&ep->ep, GFP_ATOMIC);
+		buf = devm_kzalloc(dev->dev, PAGE_SIZE, GFP_DMA | GFP_ATOMIC);
 		if (!_req || !buf) {
 			/* possible _req freed by gr_probe via gr_remove */
 			return -ENOMEM;
@@ -2020,9 +2025,7 @@ static int gr_udc_init(struct gr_udc *dev)
 	u32 dmactrl_val;
 	int i;
 	int ret = 0;
-	u32 *bufsizes;
 	u32 bufsize;
-	int len;
 
 	gr_set_address(dev, 0);
 
@@ -2033,19 +2036,17 @@ static int gr_udc_init(struct gr_udc *dev)
 	INIT_LIST_HEAD(&dev->ep_list);
 	gr_set_ep0state(dev, GR_EP0_DISCONNECT);
 
-	bufsizes = (u32 *)of_get_property(np, "epobufsizes", &len);
-	len /= sizeof(u32);
 	for (i = 0; i < dev->nepo; i++) {
-		bufsize = (bufsizes && i < len) ? bufsizes[i] : 1024;
+		if (of_property_read_u32_index(np, "epobufsizes", i, &bufsize))
+			bufsize = 1024;
 		ret = gr_ep_init(dev, i, 0, bufsize);
 		if (ret)
 			return ret;
 	}
 
-	bufsizes = (u32 *)of_get_property(np, "epibufsizes", &len);
-	len /= sizeof(u32);
 	for (i = 0; i < dev->nepi; i++) {
-		bufsize = (bufsizes && i < len) ? bufsizes[i] : 1024;
+		if (of_property_read_u32_index(np, "epibufsizes", i, &bufsize))
+			bufsize = 1024;
 		ret = gr_ep_init(dev, i, 1, bufsize);
 		if (ret)
 			return ret;
@@ -2065,9 +2066,9 @@ static int gr_udc_init(struct gr_udc *dev)
 	return 0;
 }
 
-static int gr_remove(struct platform_device *ofdev)
+static int gr_remove(struct platform_device *pdev)
 {
-	struct gr_udc *dev = dev_get_drvdata(&ofdev->dev);
+	struct gr_udc *dev = platform_get_drvdata(pdev);
 
 	if (dev->added)
 		usb_del_gadget_udc(&dev->gadget); /* Shuts everything down */
@@ -2077,7 +2078,7 @@ static int gr_remove(struct platform_device *ofdev)
 	gr_dfs_delete(dev);
 	if (dev->desc_pool)
 		dma_pool_destroy(dev->desc_pool);
-	dev_set_drvdata(&ofdev->dev, NULL);
+	platform_set_drvdata(pdev, NULL);
 
 	gr_free_request(&dev->epi[0].ep, &dev->ep0reqi->req);
 	gr_free_request(&dev->epo[0].ep, &dev->ep0reqo->req);
@@ -2090,7 +2091,7 @@ static int gr_request_irq(struct gr_udc *dev, int irq)
 					 IRQF_SHARED, driver_name, dev);
 }
 
-static int gr_probe(struct platform_device *ofdev)
+static int gr_probe(struct platform_device *pdev)
 {
 	struct gr_udc *dev;
 	struct resource *res;
@@ -2098,30 +2099,32 @@ static int gr_probe(struct platform_device *ofdev)
 	int retval;
 	u32 status;
 
-	dev = devm_kzalloc(&ofdev->dev, sizeof(*dev), GFP_KERNEL);
+	dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
 	if (!dev)
 		return -ENOMEM;
-	dev->dev = &ofdev->dev;
+	dev->dev = &pdev->dev;
 
-	res = platform_get_resource(ofdev, IORESOURCE_MEM, 0);
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	regs = devm_ioremap_resource(dev->dev, res);
 	if (IS_ERR(regs))
 		return PTR_ERR(regs);
 
-	dev->irq = irq_of_parse_and_map(dev->dev->of_node, 0);
-	if (!dev->irq) {
+	dev->irq = platform_get_irq(pdev, 0);
+	if (dev->irq <= 0) {
 		dev_err(dev->dev, "No irq found\n");
 		return -ENODEV;
 	}
 
 	/* Some core configurations has separate irqs for IN and OUT events */
-	dev->irqi = irq_of_parse_and_map(dev->dev->of_node, 1);
-	if (dev->irqi) {
-		dev->irqo = irq_of_parse_and_map(dev->dev->of_node, 2);
-		if (!dev->irqo) {
+	dev->irqi = platform_get_irq(pdev, 1);
+	if (dev->irqi > 0) {
+		dev->irqo = platform_get_irq(pdev, 2);
+		if (dev->irqo <= 0) {
 			dev_err(dev->dev, "Found irqi but not irqo\n");
 			return -ENODEV;
 		}
+	} else {
+		dev->irqi = 0;
 	}
 
 	dev->gadget.name = driver_name;
@@ -2132,7 +2135,7 @@ static int gr_probe(struct platform_device *ofdev)
 	spin_lock_init(&dev->lock);
 	dev->regs = regs;
 
-	dev_set_drvdata(&ofdev->dev, dev);
+	platform_set_drvdata(pdev, dev);
 
 	/* Determine number of endpoints and data interface mode */
 	status = gr_read32(&dev->regs->status);
@@ -2204,7 +2207,7 @@ out:
 	spin_unlock(&dev->lock);
 
 	if (retval)
-		gr_remove(ofdev);
+		gr_remove(pdev);
 
 	return retval;
 }
diff --git a/drivers/usb/gadget/inode.c b/drivers/usb/gadget/inode.c
index b5be6f0308c..ee6c16416c3 100644
--- a/drivers/usb/gadget/inode.c
+++ b/drivers/usb/gadget/inode.c
@@ -1494,6 +1494,7 @@ gadgetfs_setup (struct usb_gadget *gadget, const struct usb_ctrlrequest *ctrl)
 		 */
 		if (value == 0) {
 			INFO (dev, "configuration #%d\n", dev->current_config);
+			usb_gadget_set_state(gadget, USB_STATE_CONFIGURED);
 			if (dev->usermode_setup) {
 				dev->setup_can_stall = 0;
 				goto delegate;
@@ -1501,7 +1502,7 @@ gadgetfs_setup (struct usb_gadget *gadget, const struct usb_ctrlrequest *ctrl)
 		}
 		break;
 
-#ifndef	CONFIG_USB_GADGET_PXA25X
+#ifndef	CONFIG_USB_PXA25X
 	/* PXA automagically handles this request too */
 	case USB_REQ_GET_CONFIGURATION:
 		if (ctrl->bRequestType != 0x80)
@@ -2043,6 +2044,7 @@ gadgetfs_fill_super (struct super_block *sb, void *opts, int silent)
 		return -ESRCH;
 
 	/* fake probe to determine $CHIP */
+	CHIP = NULL;
 	usb_gadget_probe_driver(&probe_driver);
 	if (!CHIP)
 		return -ENODEV;
diff --git a/drivers/usb/gadget/m66592-udc.c b/drivers/usb/gadget/m66592-udc.c
index 8cae01d8859..0d17174b86f 100644
--- a/drivers/usb/gadget/m66592-udc.c
+++ b/drivers/usb/gadget/m66592-udc.c
@@ -1594,7 +1594,6 @@ static int __init m66592_probe(struct platform_device *pdev)
 	m66592 = kzalloc(sizeof(struct m66592), GFP_KERNEL);
 	if (m66592 == NULL) {
 		ret = -ENOMEM;
-		pr_err("kzalloc error\n");
 		goto clean_up;
 	}
 
diff --git a/drivers/usb/gadget/mv_u3d_core.c b/drivers/usb/gadget/mv_u3d_core.c
index d2ca59e7b47..16248711c15 100644
--- a/drivers/usb/gadget/mv_u3d_core.c
+++ b/drivers/usb/gadget/mv_u3d_core.c
@@ -297,10 +297,8 @@ static struct mv_u3d_trb *mv_u3d_build_trb_one(struct mv_u3d_req *req,
 	u3d = req->ep->u3d;
 
 	trb = kzalloc(sizeof(*trb), GFP_ATOMIC);
-	if (!trb) {
-		dev_err(u3d->dev, "%s, trb alloc fail\n", __func__);
+	if (!trb)
 		return NULL;
-	}
 
 	/*
 	 * Be careful that no _GFP_HIGHMEM is set,
@@ -446,17 +444,12 @@ static int mv_u3d_req_to_trb(struct mv_u3d_req *req)
 			trb_num++;
 
 		trb = kcalloc(trb_num, sizeof(*trb), GFP_ATOMIC);
-		if (!trb) {
-			dev_err(u3d->dev,
-					"%s, trb alloc fail\n", __func__);
+		if (!trb)
 			return -ENOMEM;
-		}
 
 		trb_hw = kcalloc(trb_num, sizeof(*trb_hw), GFP_ATOMIC);
 		if (!trb_hw) {
 			kfree(trb);
-			dev_err(u3d->dev,
-					"%s, trb_hw alloc fail\n", __func__);
 			return -ENOMEM;
 		}
 
@@ -1811,7 +1804,6 @@ static int mv_u3d_probe(struct platform_device *dev)
 
 	u3d = kzalloc(sizeof(*u3d), GFP_KERNEL);
 	if (!u3d) {
-		dev_err(&dev->dev, "failed to allocate memory for u3d\n");
 		retval = -ENOMEM;
 		goto err_alloc_private;
 	}
@@ -1905,7 +1897,6 @@ static int mv_u3d_probe(struct platform_device *dev)
 	size = u3d->max_eps * sizeof(struct mv_u3d_ep) * 2;
 	u3d->eps = kzalloc(size, GFP_KERNEL);
 	if (!u3d->eps) {
-		dev_err(&dev->dev, "allocate ep memory failed\n");
 		retval = -ENOMEM;
 		goto err_alloc_eps;
 	}
@@ -1913,7 +1904,6 @@ static int mv_u3d_probe(struct platform_device *dev)
 	/* initialize ep0 status request structure */
 	u3d->status_req = kzalloc(sizeof(struct mv_u3d_req) + 8, GFP_KERNEL);
 	if (!u3d->status_req) {
-		dev_err(&dev->dev, "allocate status_req memory failed\n");
 		retval = -ENOMEM;
 		goto err_alloc_status_req;
 	}
diff --git a/drivers/usb/gadget/net2280.c b/drivers/usb/gadget/net2280.c
index 43e5e2f9888..300b3a71383 100644
--- a/drivers/usb/gadget/net2280.c
+++ b/drivers/usb/gadget/net2280.c
@@ -1972,7 +1972,9 @@ static int net2280_stop(struct usb_gadget *_gadget,
 	device_remove_file (&dev->pdev->dev, &dev_attr_function);
 	device_remove_file (&dev->pdev->dev, &dev_attr_queues);
 
-	DEBUG (dev, "unregistered driver '%s'\n", driver->driver.name);
+	DEBUG(dev, "unregistered driver '%s'\n",
+			driver ? driver->driver.name : "");
+
 	return 0;
 }
 
diff --git a/drivers/usb/gadget/r8a66597-udc.c b/drivers/usb/gadget/r8a66597-udc.c
index aff0a6718bc..b698a490cc7 100644
--- a/drivers/usb/gadget/r8a66597-udc.c
+++ b/drivers/usb/gadget/r8a66597-udc.c
@@ -1904,7 +1904,6 @@ static int __init r8a66597_probe(struct platform_device *pdev)
 	r8a66597 = kzalloc(sizeof(struct r8a66597), GFP_KERNEL);
 	if (r8a66597 == NULL) {
 		ret = -ENOMEM;
-		dev_err(&pdev->dev, "kzalloc error\n");
 		goto clean_up;
 	}
 
diff --git a/drivers/usb/gadget/rndis.c b/drivers/usb/gadget/rndis.c
index d822d822efb..95d2324f697 100644
--- a/drivers/usb/gadget/rndis.c
+++ b/drivers/usb/gadget/rndis.c
@@ -35,6 +35,7 @@
 #include <asm/byteorder.h>
 #include <asm/unaligned.h>
 
+#include "u_rndis.h"
 
 #undef	VERBOSE_DEBUG
 
@@ -760,7 +761,7 @@ int rndis_signal_connect(int configNr)
 	return rndis_indicate_status_msg(configNr,
 					  RNDIS_STATUS_MEDIA_CONNECT);
 }
-EXPORT_SYMBOL(rndis_signal_connect);
+EXPORT_SYMBOL_GPL(rndis_signal_connect);
 
 int rndis_signal_disconnect(int configNr)
 {
@@ -769,7 +770,7 @@ int rndis_signal_disconnect(int configNr)
 	return rndis_indicate_status_msg(configNr,
 					  RNDIS_STATUS_MEDIA_DISCONNECT);
 }
-EXPORT_SYMBOL(rndis_signal_disconnect);
+EXPORT_SYMBOL_GPL(rndis_signal_disconnect);
 
 void rndis_uninit(int configNr)
 {
@@ -784,13 +785,13 @@ void rndis_uninit(int configNr)
 	while ((buf = rndis_get_next_response(configNr, &length)))
 		rndis_free_response(configNr, buf);
 }
-EXPORT_SYMBOL(rndis_uninit);
+EXPORT_SYMBOL_GPL(rndis_uninit);
 
 void rndis_set_host_mac(int configNr, const u8 *addr)
 {
 	rndis_per_dev_params[configNr].host_mac = addr;
 }
-EXPORT_SYMBOL(rndis_set_host_mac);
+EXPORT_SYMBOL_GPL(rndis_set_host_mac);
 
 /*
  * Message Parser
@@ -873,7 +874,7 @@ int rndis_msg_parser(u8 configNr, u8 *buf)
 
 	return -ENOTSUPP;
 }
-EXPORT_SYMBOL(rndis_msg_parser);
+EXPORT_SYMBOL_GPL(rndis_msg_parser);
 
 int rndis_register(void (*resp_avail)(void *v), void *v)
 {
@@ -895,7 +896,7 @@ int rndis_register(void (*resp_avail)(void *v), void *v)
 
 	return -ENODEV;
 }
-EXPORT_SYMBOL(rndis_register);
+EXPORT_SYMBOL_GPL(rndis_register);
 
 void rndis_deregister(int configNr)
 {
@@ -904,7 +905,7 @@ void rndis_deregister(int configNr)
 	if (configNr >= RNDIS_MAX_CONFIGS) return;
 	rndis_per_dev_params[configNr].used = 0;
 }
-EXPORT_SYMBOL(rndis_deregister);
+EXPORT_SYMBOL_GPL(rndis_deregister);
 
 int rndis_set_param_dev(u8 configNr, struct net_device *dev, u16 *cdc_filter)
 {
@@ -918,7 +919,7 @@ int rndis_set_param_dev(u8 configNr, struct net_device *dev, u16 *cdc_filter)
 
 	return 0;
 }
-EXPORT_SYMBOL(rndis_set_param_dev);
+EXPORT_SYMBOL_GPL(rndis_set_param_dev);
 
 int rndis_set_param_vendor(u8 configNr, u32 vendorID, const char *vendorDescr)
 {
@@ -931,7 +932,7 @@ int rndis_set_param_vendor(u8 configNr, u32 vendorID, const char *vendorDescr)
 
 	return 0;
 }
-EXPORT_SYMBOL(rndis_set_param_vendor);
+EXPORT_SYMBOL_GPL(rndis_set_param_vendor);
 
 int rndis_set_param_medium(u8 configNr, u32 medium, u32 speed)
 {
@@ -943,7 +944,7 @@ int rndis_set_param_medium(u8 configNr, u32 medium, u32 speed)
 
 	return 0;
 }
-EXPORT_SYMBOL(rndis_set_param_medium);
+EXPORT_SYMBOL_GPL(rndis_set_param_medium);
 
 void rndis_add_hdr(struct sk_buff *skb)
 {
@@ -958,7 +959,7 @@ void rndis_add_hdr(struct sk_buff *skb)
 	header->DataOffset = cpu_to_le32(36);
 	header->DataLength = cpu_to_le32(skb->len - sizeof(*header));
 }
-EXPORT_SYMBOL(rndis_add_hdr);
+EXPORT_SYMBOL_GPL(rndis_add_hdr);
 
 void rndis_free_response(int configNr, u8 *buf)
 {
@@ -975,7 +976,7 @@ void rndis_free_response(int configNr, u8 *buf)
 		}
 	}
 }
-EXPORT_SYMBOL(rndis_free_response);
+EXPORT_SYMBOL_GPL(rndis_free_response);
 
 u8 *rndis_get_next_response(int configNr, u32 *length)
 {
@@ -997,7 +998,7 @@ u8 *rndis_get_next_response(int configNr, u32 *length)
 
 	return NULL;
 }
-EXPORT_SYMBOL(rndis_get_next_response);
+EXPORT_SYMBOL_GPL(rndis_get_next_response);
 
 static rndis_resp_t *rndis_add_response(int configNr, u32 length)
 {
@@ -1041,7 +1042,7 @@ int rndis_rm_hdr(struct gether *port,
 	skb_queue_tail(list, skb);
 	return 0;
 }
-EXPORT_SYMBOL(rndis_rm_hdr);
+EXPORT_SYMBOL_GPL(rndis_rm_hdr);
 
 #ifdef CONFIG_USB_GADGET_DEBUG_FILES
 
diff --git a/drivers/usb/gadget/s3c-hsotg.h b/drivers/usb/gadget/s3c-hsotg.h
deleted file mode 100644
index 85f549ff8c1..00000000000
--- a/drivers/usb/gadget/s3c-hsotg.h
+++ /dev/null
@@ -1,378 +0,0 @@
-/* drivers/usb/gadget/s3c-hsotg.h
- *
- * Copyright 2008 Openmoko, Inc.
- * Copyright 2008 Simtec Electronics
- *      http://armlinux.simtec.co.uk/
- *      Ben Dooks <ben@simtec.co.uk>
- *
- * USB2.0 Highspeed/OtG Synopsis DWC2 device block registers
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
-*/
-
-#ifndef __REGS_USB_HSOTG_H
-#define __REGS_USB_HSOTG_H __FILE__
-
-#define HSOTG_REG(x) (x)
-
-#define GOTGCTL				HSOTG_REG(0x000)
-#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_HSSETHNPEN			(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_DbnceDone			(1 << 19)
-#define GOTGINT_ADevTOUTChg			(1 << 18)
-#define GOTGINT_HstNegDet			(1 << 17)
-#define GOTGINT_HstnegSucStsChng		(1 << 9)
-#define GOTGINT_SesReqSucStsChng		(1 << 8)
-#define GOTGINT_SesEndDet			(1 << 2)
-
-#define GAHBCFG				HSOTG_REG(0x008)
-#define GAHBCFG_PTxFEmpLvl			(1 << 8)
-#define GAHBCFG_NPTxFEmpLvl			(1 << 7)
-#define GAHBCFG_DMAEn				(1 << 5)
-#define GAHBCFG_HBstLen_MASK			(0xf << 1)
-#define GAHBCFG_HBstLen_SHIFT			(1)
-#define GAHBCFG_HBstLen_Single			(0x0 << 1)
-#define GAHBCFG_HBstLen_Incr			(0x1 << 1)
-#define GAHBCFG_HBstLen_Incr4			(0x3 << 1)
-#define GAHBCFG_HBstLen_Incr8			(0x5 << 1)
-#define GAHBCFG_HBstLen_Incr16			(0x7 << 1)
-#define GAHBCFG_GlblIntrEn			(1 << 0)
-
-#define GUSBCFG				HSOTG_REG(0x00C)
-#define GUSBCFG_PHYLPClkSel			(1 << 15)
-#define GUSBCFG_HNPCap				(1 << 9)
-#define GUSBCFG_SRPCap				(1 << 8)
-#define GUSBCFG_PHYIf16			(1 << 3)
-#define GUSBCFG_PHYIf8				(0 << 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_INTknQFlsh			(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_PTxFEmp			(1 << 26)
-#define GINTSTS_HChInt				(1 << 25)
-#define GINTSTS_PrtInt				(1 << 24)
-#define GINTSTS_FetSusp			(1 << 22)
-#define GINTSTS_incompIP			(1 << 21)
-#define GINTSTS_IncomplSOIN			(1 << 20)
-#define GINTSTS_OEPInt				(1 << 19)
-#define GINTSTS_IEPInt				(1 << 18)
-#define GINTSTS_EPMis				(1 << 17)
-#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_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_CurMod_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		(0x1 << 17)
-#define GRXSTS_PktSts_OutRX			(0x2 << 17)
-#define GRXSTS_PktSts_OutDone			(0x3 << 17)
-#define GRXSTS_PktSts_SetupDone		(0x4 << 17)
-#define GRXSTS_PktSts_SetupRX			(0x6 << 17)
-
-#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_NPTxFDep_MASK		(0xffff << 16)
-#define GNPTXFSIZ_NPTxFDep_SHIFT		(16)
-#define GNPTXFSIZ_NPTxFDep_LIMIT		(0xffff)
-#define GNPTXFSIZ_NPTxFDep(_x)			((_x) << 16)
-#define GNPTXFSIZ_NPTxFStAddr_MASK		(0xffff << 0)
-#define GNPTXFSIZ_NPTxFStAddr_SHIFT		(0)
-#define GNPTXFSIZ_NPTxFStAddr_LIMIT		(0xffff)
-#define GNPTXFSIZ_NPTxFStAddr(_x)		((_x) << 0)
-
-#define GNPTXSTS				HSOTG_REG(0x02C)
-
-#define GNPTXSTS_NPtxQTop_MASK			(0x7f << 24)
-#define GNPTXSTS_NPtxQTop_SHIFT		(24)
-
-#define GNPTXSTS_NPTxQSpcAvail_MASK		(0xff << 16)
-#define GNPTXSTS_NPTxQSpcAvail_SHIFT		(16)
-#define GNPTXSTS_NPTxQSpcAvail_GET(_v)		(((_v) >> 16) & 0xff)
-
-#define GNPTXSTS_NPTxFSpcAvail_MASK		(0xffff << 0)
-#define GNPTXSTS_NPTxFSpcAvail_SHIFT		(0)
-#define GNPTXSTS_NPTxFSpcAvail_GET(_v)		(((_v) >> 0) & 0xffff)
-
-
-#define HPTXFSIZ				HSOTG_REG(0x100)
-
-#define DPTXFSIZn(_a)		HSOTG_REG(0x104 + (((_a) - 1) * 4))
-
-#define DPTXFSIZn_DPTxFSize_MASK		(0xffff << 16)
-#define DPTXFSIZn_DPTxFSize_SHIFT		(16)
-#define DPTXFSIZn_DPTxFSize_GET(_v)		(((_v) >> 16) & 0xffff)
-#define DPTXFSIZn_DPTxFSize_LIMIT		(0xffff)
-#define DPTXFSIZn_DPTxFSize(_x)		((_x) << 16)
-
-#define DPTXFSIZn_DPTxFStAddr_MASK		(0xffff << 0)
-#define DPTXFSIZn_DPTxFStAddr_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_NZStsOUTHShk			(1 << 2)
-
-#define DCFG_DevSpd_MASK			(0x3 << 0)
-#define DCFG_DevSpd_SHIFT			(0)
-#define DCFG_DevSpd_HS				(0x0 << 0)
-#define DCFG_DevSpd_FS				(0x1 << 0)
-#define DCFG_DevSpd_LS				(0x2 << 0)
-#define DCFG_DevSpd_FS48			(0x3 << 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			(0x0 << 1)
-#define DSTS_EnumSpd_FS			(0x1 << 1)
-#define DSTS_EnumSpd_LS			(0x2 << 1)
-#define DSTS_EnumSpd_FS48			(0x3 << 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 DOEPCTL0				HSOTG_REG(0xB00)
-#define DIEPCTL(_a)			HSOTG_REG(0x900 + ((_a) * 0x20))
-#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 differenct for EP0
- */
-#define D0EPCTL_MPS_MASK			(0x3 << 0)
-#define D0EPCTL_MPS_SHIFT			(0)
-#define D0EPCTL_MPS_64				(0x0 << 0)
-#define D0EPCTL_MPS_32				(0x1 << 0)
-#define D0EPCTL_MPS_16				(0x2 << 0)
-#define D0EPCTL_MPS_8				(0x3 << 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			(0x0 << 18)
-#define DxEPCTL_EPType_Iso			(0x1 << 18)
-#define DxEPCTL_EPType_Bulk			(0x2 << 18)
-#define DxEPCTL_EPType_Intterupt		(0x3 << 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_GET(_v)		(((_v) >> 19) & 0x3ff)
-#define DxEPTSIZ_PktCnt_LIMIT			(0x3ff)
-#define DxEPTSIZ_PktCnt(_x)			((_x) << 19)
-
-#define DxEPTSIZ_XferSize_MASK			(0x7ffff << 0)
-#define DxEPTSIZ_XferSize_SHIFT		(0)
-#define DxEPTSIZ_XferSize_GET(_v)		(((_v) >> 0) & 0x7ffff)
-#define DxEPTSIZ_XferSize_LIMIT		(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 EPFIFO(_a)			HSOTG_REG(0x1000 + ((_a) * 0x1000))
-
-#endif /* __REGS_USB_HSOTG_H */
diff --git a/drivers/usb/gadget/s3c2410_udc.c b/drivers/usb/gadget/s3c2410_udc.c
index dd9678f85c5..7987aa049fa 100644
--- a/drivers/usb/gadget/s3c2410_udc.c
+++ b/drivers/usb/gadget/s3c2410_udc.c
@@ -117,7 +117,8 @@ static int dprintk(int level, const char *fmt, ...)
 			sizeof(printk_buf)-len, fmt, args);
 	va_end(args);
 
-	return pr_debug("%s", printk_buf);
+	pr_debug("%s", printk_buf);
+	return len;
 }
 #else
 static int dprintk(int level, const char *fmt, ...)
diff --git a/drivers/usb/gadget/storage_common.c b/drivers/usb/gadget/storage_common.c
index ec20a1f50c2..ff205a7bc55 100644
--- a/drivers/usb/gadget/storage_common.c
+++ b/drivers/usb/gadget/storage_common.c
@@ -43,7 +43,7 @@ struct usb_interface_descriptor fsg_intf_desc = {
 	.bInterfaceProtocol =	USB_PR_BULK,	/* Adjusted during fsg_bind() */
 	.iInterface =		FSG_STRING_INTERFACE,
 };
-EXPORT_SYMBOL(fsg_intf_desc);
+EXPORT_SYMBOL_GPL(fsg_intf_desc);
 
 /*
  * Three full-speed endpoint descriptors: bulk-in, bulk-out, and
@@ -58,7 +58,7 @@ struct usb_endpoint_descriptor fsg_fs_bulk_in_desc = {
 	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
 	/* wMaxPacketSize set by autoconfiguration */
 };
-EXPORT_SYMBOL(fsg_fs_bulk_in_desc);
+EXPORT_SYMBOL_GPL(fsg_fs_bulk_in_desc);
 
 struct usb_endpoint_descriptor fsg_fs_bulk_out_desc = {
 	.bLength =		USB_DT_ENDPOINT_SIZE,
@@ -68,7 +68,7 @@ struct usb_endpoint_descriptor fsg_fs_bulk_out_desc = {
 	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
 	/* wMaxPacketSize set by autoconfiguration */
 };
-EXPORT_SYMBOL(fsg_fs_bulk_out_desc);
+EXPORT_SYMBOL_GPL(fsg_fs_bulk_out_desc);
 
 struct usb_descriptor_header *fsg_fs_function[] = {
 	(struct usb_descriptor_header *) &fsg_intf_desc,
@@ -76,7 +76,7 @@ struct usb_descriptor_header *fsg_fs_function[] = {
 	(struct usb_descriptor_header *) &fsg_fs_bulk_out_desc,
 	NULL,
 };
-EXPORT_SYMBOL(fsg_fs_function);
+EXPORT_SYMBOL_GPL(fsg_fs_function);
 
 
 /*
@@ -95,7 +95,7 @@ struct usb_endpoint_descriptor fsg_hs_bulk_in_desc = {
 	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
 	.wMaxPacketSize =	cpu_to_le16(512),
 };
-EXPORT_SYMBOL(fsg_hs_bulk_in_desc);
+EXPORT_SYMBOL_GPL(fsg_hs_bulk_in_desc);
 
 struct usb_endpoint_descriptor fsg_hs_bulk_out_desc = {
 	.bLength =		USB_DT_ENDPOINT_SIZE,
@@ -106,7 +106,7 @@ struct usb_endpoint_descriptor fsg_hs_bulk_out_desc = {
 	.wMaxPacketSize =	cpu_to_le16(512),
 	.bInterval =		1,	/* NAK every 1 uframe */
 };
-EXPORT_SYMBOL(fsg_hs_bulk_out_desc);
+EXPORT_SYMBOL_GPL(fsg_hs_bulk_out_desc);
 
 
 struct usb_descriptor_header *fsg_hs_function[] = {
@@ -115,7 +115,7 @@ struct usb_descriptor_header *fsg_hs_function[] = {
 	(struct usb_descriptor_header *) &fsg_hs_bulk_out_desc,
 	NULL,
 };
-EXPORT_SYMBOL(fsg_hs_function);
+EXPORT_SYMBOL_GPL(fsg_hs_function);
 
 struct usb_endpoint_descriptor fsg_ss_bulk_in_desc = {
 	.bLength =		USB_DT_ENDPOINT_SIZE,
@@ -125,7 +125,7 @@ struct usb_endpoint_descriptor fsg_ss_bulk_in_desc = {
 	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
 	.wMaxPacketSize =	cpu_to_le16(1024),
 };
-EXPORT_SYMBOL(fsg_ss_bulk_in_desc);
+EXPORT_SYMBOL_GPL(fsg_ss_bulk_in_desc);
 
 struct usb_ss_ep_comp_descriptor fsg_ss_bulk_in_comp_desc = {
 	.bLength =		sizeof(fsg_ss_bulk_in_comp_desc),
@@ -133,7 +133,7 @@ struct usb_ss_ep_comp_descriptor fsg_ss_bulk_in_comp_desc = {
 
 	/*.bMaxBurst =		DYNAMIC, */
 };
-EXPORT_SYMBOL(fsg_ss_bulk_in_comp_desc);
+EXPORT_SYMBOL_GPL(fsg_ss_bulk_in_comp_desc);
 
 struct usb_endpoint_descriptor fsg_ss_bulk_out_desc = {
 	.bLength =		USB_DT_ENDPOINT_SIZE,
@@ -143,7 +143,7 @@ struct usb_endpoint_descriptor fsg_ss_bulk_out_desc = {
 	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
 	.wMaxPacketSize =	cpu_to_le16(1024),
 };
-EXPORT_SYMBOL(fsg_ss_bulk_out_desc);
+EXPORT_SYMBOL_GPL(fsg_ss_bulk_out_desc);
 
 struct usb_ss_ep_comp_descriptor fsg_ss_bulk_out_comp_desc = {
 	.bLength =		sizeof(fsg_ss_bulk_in_comp_desc),
@@ -151,7 +151,7 @@ struct usb_ss_ep_comp_descriptor fsg_ss_bulk_out_comp_desc = {
 
 	/*.bMaxBurst =		DYNAMIC, */
 };
-EXPORT_SYMBOL(fsg_ss_bulk_out_comp_desc);
+EXPORT_SYMBOL_GPL(fsg_ss_bulk_out_comp_desc);
 
 struct usb_descriptor_header *fsg_ss_function[] = {
 	(struct usb_descriptor_header *) &fsg_intf_desc,
@@ -161,7 +161,7 @@ struct usb_descriptor_header *fsg_ss_function[] = {
 	(struct usb_descriptor_header *) &fsg_ss_bulk_out_comp_desc,
 	NULL,
 };
-EXPORT_SYMBOL(fsg_ss_function);
+EXPORT_SYMBOL_GPL(fsg_ss_function);
 
 
  /*-------------------------------------------------------------------------*/
@@ -179,7 +179,7 @@ void fsg_lun_close(struct fsg_lun *curlun)
 		curlun->filp = NULL;
 	}
 }
-EXPORT_SYMBOL(fsg_lun_close);
+EXPORT_SYMBOL_GPL(fsg_lun_close);
 
 int fsg_lun_open(struct fsg_lun *curlun, const char *filename)
 {
@@ -278,7 +278,7 @@ out:
 	fput(filp);
 	return rc;
 }
-EXPORT_SYMBOL(fsg_lun_open);
+EXPORT_SYMBOL_GPL(fsg_lun_open);
 
 
 /*-------------------------------------------------------------------------*/
@@ -295,7 +295,7 @@ int fsg_lun_fsync_sub(struct fsg_lun *curlun)
 		return 0;
 	return vfs_fsync(filp, 1);
 }
-EXPORT_SYMBOL(fsg_lun_fsync_sub);
+EXPORT_SYMBOL_GPL(fsg_lun_fsync_sub);
 
 void store_cdrom_address(u8 *dest, int msf, u32 addr)
 {
@@ -314,7 +314,7 @@ void store_cdrom_address(u8 *dest, int msf, u32 addr)
 		put_unaligned_be32(addr, dest);
 	}
 }
-EXPORT_SYMBOL(store_cdrom_address);
+EXPORT_SYMBOL_GPL(store_cdrom_address);
 
 /*-------------------------------------------------------------------------*/
 
@@ -325,13 +325,13 @@ ssize_t fsg_show_ro(struct fsg_lun *curlun, char *buf)
 				  ? curlun->ro
 				  : curlun->initially_ro);
 }
-EXPORT_SYMBOL(fsg_show_ro);
+EXPORT_SYMBOL_GPL(fsg_show_ro);
 
 ssize_t fsg_show_nofua(struct fsg_lun *curlun, char *buf)
 {
 	return sprintf(buf, "%u\n", curlun->nofua);
 }
-EXPORT_SYMBOL(fsg_show_nofua);
+EXPORT_SYMBOL_GPL(fsg_show_nofua);
 
 ssize_t fsg_show_file(struct fsg_lun *curlun, struct rw_semaphore *filesem,
 		      char *buf)
@@ -357,19 +357,19 @@ ssize_t fsg_show_file(struct fsg_lun *curlun, struct rw_semaphore *filesem,
 	up_read(filesem);
 	return rc;
 }
-EXPORT_SYMBOL(fsg_show_file);
+EXPORT_SYMBOL_GPL(fsg_show_file);
 
 ssize_t fsg_show_cdrom(struct fsg_lun *curlun, char *buf)
 {
 	return sprintf(buf, "%u\n", curlun->cdrom);
 }
-EXPORT_SYMBOL(fsg_show_cdrom);
+EXPORT_SYMBOL_GPL(fsg_show_cdrom);
 
 ssize_t fsg_show_removable(struct fsg_lun *curlun, char *buf)
 {
 	return sprintf(buf, "%u\n", curlun->removable);
 }
-EXPORT_SYMBOL(fsg_show_removable);
+EXPORT_SYMBOL_GPL(fsg_show_removable);
 
 /*
  * The caller must hold fsg->filesem for reading when calling this function.
@@ -410,7 +410,7 @@ ssize_t fsg_store_ro(struct fsg_lun *curlun, struct rw_semaphore *filesem,
 
 	return rc;
 }
-EXPORT_SYMBOL(fsg_store_ro);
+EXPORT_SYMBOL_GPL(fsg_store_ro);
 
 ssize_t fsg_store_nofua(struct fsg_lun *curlun, const char *buf, size_t count)
 {
@@ -429,7 +429,7 @@ ssize_t fsg_store_nofua(struct fsg_lun *curlun, const char *buf, size_t count)
 
 	return count;
 }
-EXPORT_SYMBOL(fsg_store_nofua);
+EXPORT_SYMBOL_GPL(fsg_store_nofua);
 
 ssize_t fsg_store_file(struct fsg_lun *curlun, struct rw_semaphore *filesem,
 		       const char *buf, size_t count)
@@ -460,7 +460,7 @@ ssize_t fsg_store_file(struct fsg_lun *curlun, struct rw_semaphore *filesem,
 	up_write(filesem);
 	return (rc < 0 ? rc : count);
 }
-EXPORT_SYMBOL(fsg_store_file);
+EXPORT_SYMBOL_GPL(fsg_store_file);
 
 ssize_t fsg_store_cdrom(struct fsg_lun *curlun, struct rw_semaphore *filesem,
 			const char *buf, size_t count)
@@ -483,7 +483,7 @@ ssize_t fsg_store_cdrom(struct fsg_lun *curlun, struct rw_semaphore *filesem,
 
 	return ret;
 }
-EXPORT_SYMBOL(fsg_store_cdrom);
+EXPORT_SYMBOL_GPL(fsg_store_cdrom);
 
 ssize_t fsg_store_removable(struct fsg_lun *curlun, const char *buf,
 			    size_t count)
@@ -499,6 +499,6 @@ ssize_t fsg_store_removable(struct fsg_lun *curlun, const char *buf,
 
 	return count;
 }
-EXPORT_SYMBOL(fsg_store_removable);
+EXPORT_SYMBOL_GPL(fsg_store_removable);
 
 MODULE_LICENSE("GPL");
diff --git a/drivers/usb/gadget/tcm_usb_gadget.c b/drivers/usb/gadget/tcm_usb_gadget.c
index 819875c7e39..6cdb7a534f2 100644
--- a/drivers/usb/gadget/tcm_usb_gadget.c
+++ b/drivers/usb/gadget/tcm_usb_gadget.c
@@ -1383,10 +1383,8 @@ static struct se_node_acl *usbg_alloc_fabric_acl(struct se_portal_group *se_tpg)
 	struct usbg_nacl *nacl;
 
 	nacl = kzalloc(sizeof(struct usbg_nacl), GFP_KERNEL);
-	if (!nacl) {
-		printk(KERN_ERR "Unable to allocate struct usbg_nacl\n");
+	if (!nacl)
 		return NULL;
-	}
 
 	return &nacl->se_node_acl;
 }
@@ -1561,10 +1559,8 @@ static struct se_portal_group *usbg_make_tpg(
 	}
 
 	tpg = kzalloc(sizeof(struct usbg_tpg), GFP_KERNEL);
-	if (!tpg) {
-		printk(KERN_ERR "Unable to allocate struct usbg_tpg");
+	if (!tpg)
 		return ERR_PTR(-ENOMEM);
-	}
 	mutex_init(&tpg->tpg_mutex);
 	atomic_set(&tpg->tpg_port_count, 0);
 	tpg->workqueue = alloc_workqueue("tcm_usb_gadget", 0, 1);
@@ -1613,10 +1609,8 @@ static struct se_wwn *usbg_make_tport(
 		return ERR_PTR(-EINVAL);
 
 	tport = kzalloc(sizeof(struct usbg_tport), GFP_KERNEL);
-	if (!(tport)) {
-		printk(KERN_ERR "Unable to allocate struct usbg_tport");
+	if (!(tport))
 		return ERR_PTR(-ENOMEM);
-	}
 	tport->tport_wwpn = wwpn;
 	snprintf(tport->tport_name, sizeof(tport->tport_name), "%s", wnn_name);
 	return &tport->tport_wwn;
@@ -1727,10 +1721,8 @@ static int tcm_usbg_make_nexus(struct usbg_tpg *tpg, char *name)
 
 	ret = -ENOMEM;
 	tv_nexus = kzalloc(sizeof(*tv_nexus), GFP_KERNEL);
-	if (!tv_nexus) {
-		pr_err("Unable to allocate struct tcm_vhost_nexus\n");
+	if (!tv_nexus)
 		goto err_unlock;
-	}
 	tv_nexus->tvn_se_sess = transport_init_session(TARGET_PROT_NORMAL);
 	if (IS_ERR(tv_nexus->tvn_se_sess))
 		goto err_free;
diff --git a/drivers/usb/gadget/u_ether.c b/drivers/usb/gadget/u_ether.c
index 50d09c28913..fe0880d0873 100644
--- a/drivers/usb/gadget/u_ether.c
+++ b/drivers/usb/gadget/u_ether.c
@@ -48,8 +48,6 @@
 
 #define UETH__VERSION	"29-May-2008"
 
-#define GETHER_NAPI_WEIGHT	32
-
 struct eth_dev {
 	/* lock is held while accessing port_usb
 	 */
@@ -74,7 +72,6 @@ struct eth_dev {
 						struct sk_buff_head *list);
 
 	struct work_struct	work;
-	struct napi_struct	rx_napi;
 
 	unsigned long		todo;
 #define	WORK_RX_MEMORY		0
@@ -256,16 +253,18 @@ enomem:
 		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;
+	struct sk_buff	*skb = req->context, *skb2;
 	struct eth_dev	*dev = ep->driver_data;
 	int		status = req->status;
-	bool		rx_queue = 0;
 
 	switch (status) {
 
@@ -289,8 +288,30 @@ static void rx_complete(struct usb_ep *ep, struct usb_request *req)
 		} else {
 			skb_queue_tail(&dev->rx_frames, skb);
 		}
-		if (!status)
-			rx_queue = 1;
+		skb = NULL;
+
+		skb2 = skb_dequeue(&dev->rx_frames);
+		while (skb2) {
+			if (status < 0
+					|| ETH_HLEN > skb2->len
+					|| skb2->len > VLAN_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 */
@@ -313,20 +334,22 @@ quiesce:
 		/* FALLTHROUGH */
 
 	default:
-		rx_queue = 1;
-		dev_kfree_skb_any(skb);
 		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);
-
-	if (rx_queue && likely(napi_schedule_prep(&dev->rx_napi)))
-		__napi_schedule(&dev->rx_napi);
+		req = NULL;
+	}
+	if (req)
+		rx_submit(dev, req, GFP_ATOMIC);
 }
 
 static int prealloc(struct list_head *list, struct usb_ep *ep, unsigned n)
@@ -391,24 +414,16 @@ static void rx_fill(struct eth_dev *dev, gfp_t gfp_flags)
 {
 	struct usb_request	*req;
 	unsigned long		flags;
-	int			rx_counts = 0;
 
 	/* fill unused rxq slots with some skb */
 	spin_lock_irqsave(&dev->req_lock, flags);
 	while (!list_empty(&dev->rx_reqs)) {
-
-		if (++rx_counts > qlen(dev->gadget, dev->qmult))
-			break;
-
 		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) {
-			spin_lock_irqsave(&dev->req_lock, flags);
-			list_add(&req->list, &dev->rx_reqs);
-			spin_unlock_irqrestore(&dev->req_lock, flags);
 			defer_kevent(dev, WORK_RX_MEMORY);
 			return;
 		}
@@ -418,41 +433,6 @@ static void rx_fill(struct eth_dev *dev, gfp_t gfp_flags)
 	spin_unlock_irqrestore(&dev->req_lock, flags);
 }
 
-static int gether_poll(struct napi_struct *napi, int budget)
-{
-	struct eth_dev  *dev = container_of(napi, struct eth_dev, rx_napi);
-	struct sk_buff	*skb;
-	unsigned int	work_done = 0;
-	int		status = 0;
-
-	while ((skb = skb_dequeue(&dev->rx_frames))) {
-		if (status < 0
-				|| ETH_HLEN > skb->len
-				|| skb->len > VLAN_ETH_FRAME_LEN) {
-			dev->net->stats.rx_errors++;
-			dev->net->stats.rx_length_errors++;
-			DBG(dev, "rx length %d\n", skb->len);
-			dev_kfree_skb_any(skb);
-			continue;
-		}
-		skb->protocol = eth_type_trans(skb, dev->net);
-		dev->net->stats.rx_packets++;
-		dev->net->stats.rx_bytes += skb->len;
-
-		status = netif_rx_ni(skb);
-	}
-
-	if (netif_running(dev->net)) {
-		rx_fill(dev, GFP_KERNEL);
-		work_done++;
-	}
-
-	if (work_done < budget)
-		napi_complete(&dev->rx_napi);
-
-	return work_done;
-}
-
 static void eth_work(struct work_struct *work)
 {
 	struct eth_dev	*dev = container_of(work, struct eth_dev, work);
@@ -645,7 +625,6 @@ static void eth_start(struct eth_dev *dev, gfp_t gfp_flags)
 	/* and open the tx floodgates */
 	atomic_set(&dev->tx_qlen, 0);
 	netif_wake_queue(dev->net);
-	napi_enable(&dev->rx_napi);
 }
 
 static int eth_open(struct net_device *net)
@@ -672,7 +651,6 @@ static int eth_stop(struct net_device *net)
 	unsigned long	flags;
 
 	VDBG(dev, "%s\n", __func__);
-	napi_disable(&dev->rx_napi);
 	netif_stop_queue(net);
 
 	DBG(dev, "stop stats: rx/tx %ld/%ld, errs %ld/%ld\n",
@@ -790,7 +768,6 @@ struct eth_dev *gether_setup_name(struct usb_gadget *g,
 		return ERR_PTR(-ENOMEM);
 
 	dev = netdev_priv(net);
-	netif_napi_add(net, &dev->rx_napi, gether_poll, GETHER_NAPI_WEIGHT);
 	spin_lock_init(&dev->lock);
 	spin_lock_init(&dev->req_lock);
 	INIT_WORK(&dev->work, eth_work);
@@ -841,7 +818,7 @@ struct eth_dev *gether_setup_name(struct usb_gadget *g,
 
 	return dev;
 }
-EXPORT_SYMBOL(gether_setup_name);
+EXPORT_SYMBOL_GPL(gether_setup_name);
 
 struct net_device *gether_setup_name_default(const char *netname)
 {
@@ -853,7 +830,6 @@ struct net_device *gether_setup_name_default(const char *netname)
 		return ERR_PTR(-ENOMEM);
 
 	dev = netdev_priv(net);
-	netif_napi_add(net, &dev->rx_napi, gether_poll, GETHER_NAPI_WEIGHT);
 	spin_lock_init(&dev->lock);
 	spin_lock_init(&dev->req_lock);
 	INIT_WORK(&dev->work, eth_work);
@@ -879,7 +855,7 @@ struct net_device *gether_setup_name_default(const char *netname)
 
 	return net;
 }
-EXPORT_SYMBOL(gether_setup_name_default);
+EXPORT_SYMBOL_GPL(gether_setup_name_default);
 
 int gether_register_netdev(struct net_device *net)
 {
@@ -917,7 +893,7 @@ int gether_register_netdev(struct net_device *net)
 
 	return status;
 }
-EXPORT_SYMBOL(gether_register_netdev);
+EXPORT_SYMBOL_GPL(gether_register_netdev);
 
 void gether_set_gadget(struct net_device *net, struct usb_gadget *g)
 {
@@ -927,7 +903,7 @@ void gether_set_gadget(struct net_device *net, struct usb_gadget *g)
 	dev->gadget = g;
 	SET_NETDEV_DEV(net, &g->dev);
 }
-EXPORT_SYMBOL(gether_set_gadget);
+EXPORT_SYMBOL_GPL(gether_set_gadget);
 
 int gether_set_dev_addr(struct net_device *net, const char *dev_addr)
 {
@@ -940,7 +916,7 @@ int gether_set_dev_addr(struct net_device *net, const char *dev_addr)
 	memcpy(dev->dev_mac, new_addr, ETH_ALEN);
 	return 0;
 }
-EXPORT_SYMBOL(gether_set_dev_addr);
+EXPORT_SYMBOL_GPL(gether_set_dev_addr);
 
 int gether_get_dev_addr(struct net_device *net, char *dev_addr, int len)
 {
@@ -949,7 +925,7 @@ int gether_get_dev_addr(struct net_device *net, char *dev_addr, int len)
 	dev = netdev_priv(net);
 	return get_ether_addr_str(dev->dev_mac, dev_addr, len);
 }
-EXPORT_SYMBOL(gether_get_dev_addr);
+EXPORT_SYMBOL_GPL(gether_get_dev_addr);
 
 int gether_set_host_addr(struct net_device *net, const char *host_addr)
 {
@@ -962,7 +938,7 @@ int gether_set_host_addr(struct net_device *net, const char *host_addr)
 	memcpy(dev->host_mac, new_addr, ETH_ALEN);
 	return 0;
 }
-EXPORT_SYMBOL(gether_set_host_addr);
+EXPORT_SYMBOL_GPL(gether_set_host_addr);
 
 int gether_get_host_addr(struct net_device *net, char *host_addr, int len)
 {
@@ -971,7 +947,7 @@ int gether_get_host_addr(struct net_device *net, char *host_addr, int len)
 	dev = netdev_priv(net);
 	return get_ether_addr_str(dev->host_mac, host_addr, len);
 }
-EXPORT_SYMBOL(gether_get_host_addr);
+EXPORT_SYMBOL_GPL(gether_get_host_addr);
 
 int gether_get_host_addr_cdc(struct net_device *net, char *host_addr, int len)
 {
@@ -985,7 +961,7 @@ int gether_get_host_addr_cdc(struct net_device *net, char *host_addr, int len)
 
 	return strlen(host_addr);
 }
-EXPORT_SYMBOL(gether_get_host_addr_cdc);
+EXPORT_SYMBOL_GPL(gether_get_host_addr_cdc);
 
 void gether_get_host_addr_u8(struct net_device *net, u8 host_mac[ETH_ALEN])
 {
@@ -994,7 +970,7 @@ void gether_get_host_addr_u8(struct net_device *net, u8 host_mac[ETH_ALEN])
 	dev = netdev_priv(net);
 	memcpy(host_mac, dev->host_mac, ETH_ALEN);
 }
-EXPORT_SYMBOL(gether_get_host_addr_u8);
+EXPORT_SYMBOL_GPL(gether_get_host_addr_u8);
 
 void gether_set_qmult(struct net_device *net, unsigned qmult)
 {
@@ -1003,7 +979,7 @@ void gether_set_qmult(struct net_device *net, unsigned qmult)
 	dev = netdev_priv(net);
 	dev->qmult = qmult;
 }
-EXPORT_SYMBOL(gether_set_qmult);
+EXPORT_SYMBOL_GPL(gether_set_qmult);
 
 unsigned gether_get_qmult(struct net_device *net)
 {
@@ -1012,7 +988,7 @@ unsigned gether_get_qmult(struct net_device *net)
 	dev = netdev_priv(net);
 	return dev->qmult;
 }
-EXPORT_SYMBOL(gether_get_qmult);
+EXPORT_SYMBOL_GPL(gether_get_qmult);
 
 int gether_get_ifname(struct net_device *net, char *name, int len)
 {
@@ -1021,7 +997,7 @@ int gether_get_ifname(struct net_device *net, char *name, int len)
 	rtnl_unlock();
 	return strlen(name);
 }
-EXPORT_SYMBOL(gether_get_ifname);
+EXPORT_SYMBOL_GPL(gether_get_ifname);
 
 /**
  * gether_cleanup - remove Ethernet-over-USB device
@@ -1038,7 +1014,7 @@ void gether_cleanup(struct eth_dev *dev)
 	flush_work(&dev->work);
 	free_netdev(dev->net);
 }
-EXPORT_SYMBOL(gether_cleanup);
+EXPORT_SYMBOL_GPL(gether_cleanup);
 
 /**
  * gether_connect - notify network layer that USB link is active
@@ -1119,7 +1095,7 @@ fail0:
 		return ERR_PTR(result);
 	return dev->net;
 }
-EXPORT_SYMBOL(gether_connect);
+EXPORT_SYMBOL_GPL(gether_connect);
 
 /**
  * gether_disconnect - notify network layer that USB link is inactive
@@ -1137,7 +1113,6 @@ void gether_disconnect(struct gether *link)
 {
 	struct eth_dev		*dev = link->ioport;
 	struct usb_request	*req;
-	struct sk_buff		*skb;
 
 	WARN_ON(!dev);
 	if (!dev)
@@ -1164,12 +1139,6 @@ void gether_disconnect(struct gether *link)
 		spin_lock(&dev->req_lock);
 	}
 	spin_unlock(&dev->req_lock);
-
-	spin_lock(&dev->rx_frames.lock);
-	while ((skb = __skb_dequeue(&dev->rx_frames)))
-		dev_kfree_skb_any(skb);
-	spin_unlock(&dev->rx_frames.lock);
-
 	link->in_ep->driver_data = NULL;
 	link->in_ep->desc = NULL;
 
@@ -1197,7 +1166,7 @@ void gether_disconnect(struct gether *link)
 	dev->port_usb = NULL;
 	spin_unlock(&dev->lock);
 }
-EXPORT_SYMBOL(gether_disconnect);
+EXPORT_SYMBOL_GPL(gether_disconnect);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("David Brownell");
diff --git a/drivers/usb/gadget/u_f.c b/drivers/usb/gadget/u_f.c
index 63b6642c162..c6276f0268a 100644
--- a/drivers/usb/gadget/u_f.c
+++ b/drivers/usb/gadget/u_f.c
@@ -29,4 +29,4 @@ struct usb_request *alloc_ep_req(struct usb_ep *ep, int len, int default_len)
 	}
 	return req;
 }
-EXPORT_SYMBOL(alloc_ep_req);
+EXPORT_SYMBOL_GPL(alloc_ep_req);
diff --git a/drivers/usb/gadget/u_f.h b/drivers/usb/gadget/u_f.h
index 71034c061fc..1d5f0eb6855 100644
--- a/drivers/usb/gadget/u_f.h
+++ b/drivers/usb/gadget/u_f.h
@@ -16,6 +16,32 @@
 #ifndef __U_F_H__
 #define __U_F_H__
 
+/* Variable Length Array Macros **********************************************/
+#define vla_group(groupname) size_t groupname##__next = 0
+#define vla_group_size(groupname) groupname##__next
+
+#define vla_item(groupname, type, name, n) \
+	size_t groupname##_##name##__offset = ({			       \
+		size_t align_mask = __alignof__(type) - 1;		       \
+		size_t offset = (groupname##__next + align_mask) & ~align_mask;\
+		size_t size = (n) * sizeof(type);			       \
+		groupname##__next = offset + size;			       \
+		offset;							       \
+	})
+
+#define vla_item_with_sz(groupname, type, name, n) \
+	size_t groupname##_##name##__sz = (n) * sizeof(type);		       \
+	size_t groupname##_##name##__offset = ({			       \
+		size_t align_mask = __alignof__(type) - 1;		       \
+		size_t offset = (groupname##__next + align_mask) & ~align_mask;\
+		size_t size = groupname##_##name##__sz;			       \
+		groupname##__next = offset + size;			       \
+		offset;							       \
+	})
+
+#define vla_ptr(ptr, groupname, name) \
+	((void *) ((char *)ptr + groupname##_##name##__offset))
+
 struct usb_ep;
 struct usb_request;
 
diff --git a/drivers/usb/gadget/u_os_desc.h b/drivers/usb/gadget/u_os_desc.h
new file mode 100644
index 00000000000..ea5cf8c2da2
--- /dev/null
+++ b/drivers/usb/gadget/u_os_desc.h
@@ -0,0 +1,90 @@
+/*
+ * u_os_desc.h
+ *
+ * Utility definitions for "OS Descriptors" support
+ *
+ * Copyright (c) 2014 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * Author: Andrzej Pietrasiewicz <andrzej.p@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 __U_OS_DESC_H__
+#define __U_OS_DESC_H__
+
+#include <asm/unaligned.h>
+#include <linux/nls.h>
+
+#define USB_EXT_PROP_DW_SIZE			0
+#define USB_EXT_PROP_DW_PROPERTY_DATA_TYPE	4
+#define USB_EXT_PROP_W_PROPERTY_NAME_LENGTH	8
+#define USB_EXT_PROP_B_PROPERTY_NAME		10
+#define USB_EXT_PROP_DW_PROPERTY_DATA_LENGTH	10
+#define USB_EXT_PROP_B_PROPERTY_DATA		14
+
+#define USB_EXT_PROP_RESERVED			0
+#define USB_EXT_PROP_UNICODE			1
+#define USB_EXT_PROP_UNICODE_ENV		2
+#define USB_EXT_PROP_BINARY			3
+#define USB_EXT_PROP_LE32			4
+#define USB_EXT_PROP_BE32			5
+#define USB_EXT_PROP_UNICODE_LINK		6
+#define USB_EXT_PROP_UNICODE_MULTI		7
+
+static inline void usb_ext_prop_put_size(u8 *buf, int dw_size)
+{
+	put_unaligned_le32(dw_size, &buf[USB_EXT_PROP_DW_SIZE]);
+}
+
+static inline void usb_ext_prop_put_type(u8 *buf, int type)
+{
+	put_unaligned_le32(type, &buf[USB_EXT_PROP_DW_PROPERTY_DATA_TYPE]);
+}
+
+static inline int usb_ext_prop_put_name(u8 *buf, const char *name, int pnl)
+{
+	int result;
+
+	put_unaligned_le16(pnl, &buf[USB_EXT_PROP_W_PROPERTY_NAME_LENGTH]);
+	result = utf8s_to_utf16s(name, strlen(name), UTF16_LITTLE_ENDIAN,
+		(wchar_t *) &buf[USB_EXT_PROP_B_PROPERTY_NAME], pnl - 2);
+	if (result < 0)
+		return result;
+
+	put_unaligned_le16(0, &buf[USB_EXT_PROP_B_PROPERTY_NAME + pnl]);
+
+	return pnl;
+}
+
+static inline void usb_ext_prop_put_binary(u8 *buf, int pnl, const u8 *data,
+					   int data_len)
+{
+	put_unaligned_le32(data_len,
+			   &buf[USB_EXT_PROP_DW_PROPERTY_DATA_LENGTH + pnl]);
+	memcpy(&buf[USB_EXT_PROP_B_PROPERTY_DATA + pnl], data, data_len);
+}
+
+static inline int usb_ext_prop_put_unicode(u8 *buf, int pnl, const char *string,
+					   int data_len)
+{
+	int result;
+	put_unaligned_le32(data_len,
+			&buf[USB_EXT_PROP_DW_PROPERTY_DATA_LENGTH + pnl]);
+
+	result = utf8s_to_utf16s(string, data_len >> 1, UTF16_LITTLE_ENDIAN,
+			(wchar_t *) &buf[USB_EXT_PROP_B_PROPERTY_DATA + pnl],
+			data_len - 2);
+	if (result < 0)
+		return result;
+
+	put_unaligned_le16(0,
+			&buf[USB_EXT_PROP_B_PROPERTY_DATA + pnl + data_len]);
+
+	return data_len;
+}
+
+#endif /* __U_OS_DESC_H__ */
diff --git a/drivers/usb/gadget/u_rndis.h b/drivers/usb/gadget/u_rndis.h
index 7291b15c9dc..e902aa42a29 100644
--- a/drivers/usb/gadget/u_rndis.h
+++ b/drivers/usb/gadget/u_rndis.h
@@ -26,6 +26,9 @@ struct f_rndis_opts {
 	bool				bound;
 	bool				borrowed_net;
 
+	struct usb_os_desc		rndis_os_desc;
+	char				rndis_ext_compat_id[16];
+
 	/*
 	 * Read/write access to configfs attributes is handled by configfs.
 	 *
diff --git a/drivers/usb/gadget/udc-core.c b/drivers/usb/gadget/udc-core.c
index 27768a7d986..b0d98172bc0 100644
--- a/drivers/usb/gadget/udc-core.c
+++ b/drivers/usb/gadget/udc-core.c
@@ -428,6 +428,8 @@ int usb_gadget_unregister_driver(struct usb_gadget_driver *driver)
 	list_for_each_entry(udc, &udc_list, list)
 		if (udc->driver == driver) {
 			usb_gadget_remove_driver(udc);
+			usb_gadget_set_state(udc->gadget,
+					USB_STATE_NOTATTACHED);
 			ret = 0;
 			break;
 		}
diff --git a/drivers/usb/gadget/uvc_queue.c b/drivers/usb/gadget/uvc_queue.c
index 0bb5d50075d..1c29bc954db 100644
--- a/drivers/usb/gadget/uvc_queue.c
+++ b/drivers/usb/gadget/uvc_queue.c
@@ -20,6 +20,7 @@
 #include <linux/vmalloc.h>
 #include <linux/wait.h>
 
+#include <media/v4l2-common.h>
 #include <media/videobuf2-vmalloc.h>
 
 #include "uvc.h"
@@ -136,6 +137,8 @@ static int uvc_queue_init(struct uvc_video_queue *queue,
 	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_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC
+				     | V4L2_BUF_FLAG_TSTAMP_SRC_EOF;
 	ret = vb2_queue_init(&queue->queue);
 	if (ret)
 		return ret;
@@ -379,14 +382,9 @@ static struct uvc_buffer *uvc_queue_next_buffer(struct uvc_video_queue *queue,
 	else
 		nextbuf = NULL;
 
-	/*
-	 * 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.field = V4L2_FIELD_NONE;
 	buf->buf.v4l2_buf.sequence = queue->sequence++;
-	do_gettimeofday(&buf->buf.v4l2_buf.timestamp);
+	v4l2_get_timestamp(&buf->buf.v4l2_buf.timestamp);
 
 	vb2_set_plane_payload(&buf->buf, 0, buf->bytesused);
 	vb2_buffer_done(&buf->buf, VB2_BUF_STATE_DONE);
diff --git a/drivers/usb/gadget/zero.c b/drivers/usb/gadget/zero.c
index 9f170c53e3d..134f354ede6 100644
--- a/drivers/usb/gadget/zero.c
+++ b/drivers/usb/gadget/zero.c
@@ -300,7 +300,7 @@ static int __init zero_bind(struct usb_composite_dev *cdev)
 	ss_opts->isoc_interval = gzero_options.isoc_interval;
 	ss_opts->isoc_maxpacket = gzero_options.isoc_maxpacket;
 	ss_opts->isoc_mult = gzero_options.isoc_mult;
-	ss_opts->isoc_maxburst = gzero_options.isoc_maxpacket;
+	ss_opts->isoc_maxburst = gzero_options.isoc_maxburst;
 	ss_opts->bulk_buflen = gzero_options.bulk_buflen;
 
 	func_ss = usb_get_function(func_inst_ss);
diff --git a/drivers/usb/host/Kconfig b/drivers/usb/host/Kconfig
index 3d9e54062d6..61b7817bd66 100644
--- a/drivers/usb/host/Kconfig
+++ b/drivers/usb/host/Kconfig
@@ -29,6 +29,14 @@ if USB_XHCI_HCD
 config USB_XHCI_PLATFORM
 	tristate
 
+config USB_XHCI_MVEBU
+	tristate "xHCI support for Marvell Armada 375/38x"
+	select USB_XHCI_PLATFORM
+	depends on ARCH_MVEBU || COMPILE_TEST
+	---help---
+	  Say 'Y' to enable the support for the xHCI host controller
+	  found in Marvell Armada 375/38x ARM SOCs.
+
 endif # USB_XHCI_HCD
 
 config USB_EHCI_HCD
@@ -170,7 +178,6 @@ config USB_EHCI_MSM
 	tristate "Support for Qualcomm QSD/MSM on-chip EHCI USB controller"
 	depends on ARCH_MSM
 	select USB_EHCI_ROOT_HUB_TT
-	select USB_MSM_OTG
 	---help---
 	  Enables support for the USB Host controller present on the
 	  Qualcomm chipsets. Root Hub has inbuilt TT.
@@ -343,10 +350,19 @@ config USB_FOTG210_HCD
 	  To compile this driver as a module, choose M here: the
 	  module will be called fotg210-hcd.
 
+config USB_MAX3421_HCD
+	tristate "MAX3421 HCD (USB-over-SPI) support"
+	depends on USB && SPI
+	---help---
+	  The Maxim MAX3421E chip supports standard USB 2.0-compliant
+	  full-speed devices either in host or peripheral mode.  This
+	  driver supports the host-mode of the MAX3421E only.
+
+	  To compile this driver as a module, choose M here: the module will
+	  be called max3421-hcd.
+
 config USB_OHCI_HCD
 	tristate "OHCI HCD (USB 1.1) support"
-	select ISP1301_OMAP if MACH_OMAP_H2 || MACH_OMAP_H3
-	depends on USB_ISP1301 || !ARCH_LPC32XX
 	---help---
 	  The Open Host Controller Interface (OHCI) is a standard for accessing
 	  USB 1.1 host controller hardware.  It does more in hardware than Intel's
@@ -365,6 +381,7 @@ if USB_OHCI_HCD
 config USB_OHCI_HCD_OMAP1
 	tristate "OHCI support for OMAP1/2 chips"
 	depends on ARCH_OMAP1
+	depends on ISP1301_OMAP || !(MACH_OMAP_H2 || MACH_OMAP_H3)
 	default y
 	---help---
 	  Enables support for the OHCI controller on OMAP1/2 chips.
@@ -388,6 +405,7 @@ config USB_OHCI_HCD_S3C2410
 config USB_OHCI_HCD_LPC32XX
 	tristate "Support for LPC on-chip OHCI USB controller"
 	depends on USB_OHCI_HCD && ARCH_LPC32XX
+	depends on USB_ISP1301
 	default y
 	---help---
           Enables support for the on-chip OHCI controller on
@@ -417,6 +435,16 @@ config USB_OHCI_HCD_OMAP3
 	  Enables support for the on-chip OHCI controller on
 	  OMAP3 and later chips.
 
+config USB_OHCI_HCD_DAVINCI
+	bool "OHCI support for TI DaVinci DA8xx"
+	depends on ARCH_DAVINCI_DA8XX
+	depends on USB_OHCI_HCD=y
+	default y
+	help
+	  Enables support for the DaVinci DA8xx integrated OHCI
+	  controller. This driver cannot currently be a loadable
+	  module because it lacks a proper PHY abstraction.
+
 config USB_OHCI_ATH79
 	bool "USB OHCI support for the Atheros AR71XX/AR7240 SoCs (DEPRECATED)"
 	depends on (SOC_AR71XX || SOC_AR724X)
diff --git a/drivers/usb/host/Makefile b/drivers/usb/host/Makefile
index 7530468c9a4..af89a903d97 100644
--- a/drivers/usb/host/Makefile
+++ b/drivers/usb/host/Makefile
@@ -19,6 +19,9 @@ xhci-hcd-$(CONFIG_PCI)	+= xhci-pci.o
 
 ifneq ($(CONFIG_USB_XHCI_PLATFORM), )
 	xhci-hcd-y		+= xhci-plat.o
+ifneq ($(CONFIG_USB_XHCI_MVEBU), )
+	xhci-hcd-y		+= xhci-mvebu.o
+endif
 endif
 
 obj-$(CONFIG_USB_WHCI_HCD)	+= whci/
@@ -70,3 +73,4 @@ obj-$(CONFIG_USB_HCD_BCMA)	+= bcma-hcd.o
 obj-$(CONFIG_USB_HCD_SSB)	+= ssb-hcd.o
 obj-$(CONFIG_USB_FUSBH200_HCD)	+= fusbh200-hcd.o
 obj-$(CONFIG_USB_FOTG210_HCD)	+= fotg210-hcd.o
+obj-$(CONFIG_USB_MAX3421_HCD)	+= max3421-hcd.o
diff --git a/drivers/usb/host/ehci-exynos.c b/drivers/usb/host/ehci-exynos.c
index d1d8c47777c..d1c76216350 100644
--- a/drivers/usb/host/ehci-exynos.c
+++ b/drivers/usb/host/ehci-exynos.c
@@ -19,6 +19,7 @@
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/of_gpio.h>
+#include <linux/phy/phy.h>
 #include <linux/platform_device.h>
 #include <linux/usb/phy.h>
 #include <linux/usb/samsung_usb_phy.h>
@@ -42,17 +43,106 @@
 static const char hcd_name[] = "ehci-exynos";
 static struct hc_driver __read_mostly exynos_ehci_hc_driver;
 
+#define PHY_NUMBER 3
+
 struct exynos_ehci_hcd {
 	struct clk *clk;
 	struct usb_phy *phy;
 	struct usb_otg *otg;
+	struct phy *phy_g[PHY_NUMBER];
 };
 
 #define to_exynos_ehci(hcd) (struct exynos_ehci_hcd *)(hcd_to_ehci(hcd)->priv)
 
-static void exynos_setup_vbus_gpio(struct platform_device *pdev)
+static int exynos_ehci_get_phy(struct device *dev,
+				struct exynos_ehci_hcd *exynos_ehci)
+{
+	struct device_node *child;
+	struct phy *phy;
+	int phy_number;
+	int ret = 0;
+
+	exynos_ehci->phy = devm_usb_get_phy(dev, USB_PHY_TYPE_USB2);
+	if (IS_ERR(exynos_ehci->phy)) {
+		ret = PTR_ERR(exynos_ehci->phy);
+		if (ret != -ENXIO && ret != -ENODEV) {
+			dev_err(dev, "no usb2 phy configured\n");
+			return ret;
+		}
+		dev_dbg(dev, "Failed to get usb2 phy\n");
+	} else {
+		exynos_ehci->otg = exynos_ehci->phy->otg;
+	}
+
+	for_each_available_child_of_node(dev->of_node, child) {
+		ret = of_property_read_u32(child, "reg", &phy_number);
+		if (ret) {
+			dev_err(dev, "Failed to parse device tree\n");
+			of_node_put(child);
+			return ret;
+		}
+
+		if (phy_number >= PHY_NUMBER) {
+			dev_err(dev, "Invalid number of PHYs\n");
+			of_node_put(child);
+			return -EINVAL;
+		}
+
+		phy = devm_of_phy_get(dev, child, 0);
+		of_node_put(child);
+		if (IS_ERR(phy)) {
+			ret = PTR_ERR(phy);
+			if (ret != -ENOSYS && ret != -ENODEV) {
+				dev_err(dev, "no usb2 phy configured\n");
+				return ret;
+			}
+			dev_dbg(dev, "Failed to get usb2 phy\n");
+		}
+		exynos_ehci->phy_g[phy_number] = phy;
+	}
+
+	return ret;
+}
+
+static int exynos_ehci_phy_enable(struct device *dev)
+{
+	struct usb_hcd *hcd = dev_get_drvdata(dev);
+	struct exynos_ehci_hcd *exynos_ehci = to_exynos_ehci(hcd);
+	int i;
+	int ret = 0;
+
+	if (!IS_ERR(exynos_ehci->phy))
+		return usb_phy_init(exynos_ehci->phy);
+
+	for (i = 0; ret == 0 && i < PHY_NUMBER; i++)
+		if (!IS_ERR(exynos_ehci->phy_g[i]))
+			ret = phy_power_on(exynos_ehci->phy_g[i]);
+	if (ret)
+		for (i--; i >= 0; i--)
+			if (!IS_ERR(exynos_ehci->phy_g[i]))
+				phy_power_off(exynos_ehci->phy_g[i]);
+
+	return ret;
+}
+
+static void exynos_ehci_phy_disable(struct device *dev)
+{
+	struct usb_hcd *hcd = dev_get_drvdata(dev);
+	struct exynos_ehci_hcd *exynos_ehci = to_exynos_ehci(hcd);
+	int i;
+
+	if (!IS_ERR(exynos_ehci->phy)) {
+		usb_phy_shutdown(exynos_ehci->phy);
+		return;
+	}
+
+	for (i = 0; i < PHY_NUMBER; i++)
+		if (!IS_ERR(exynos_ehci->phy_g[i]))
+			phy_power_off(exynos_ehci->phy_g[i]);
+}
+
+static void exynos_setup_vbus_gpio(struct device *dev)
 {
-	struct device *dev = &pdev->dev;
 	int err;
 	int gpio;
 
@@ -75,7 +165,6 @@ static int exynos_ehci_probe(struct platform_device *pdev)
 	struct usb_hcd *hcd;
 	struct ehci_hcd *ehci;
 	struct resource *res;
-	struct usb_phy *phy;
 	int irq;
 	int err;
 
@@ -88,7 +177,7 @@ static int exynos_ehci_probe(struct platform_device *pdev)
 	if (err)
 		return err;
 
-	exynos_setup_vbus_gpio(pdev);
+	exynos_setup_vbus_gpio(&pdev->dev);
 
 	hcd = usb_create_hcd(&exynos_ehci_hc_driver,
 			     &pdev->dev, dev_name(&pdev->dev));
@@ -102,15 +191,9 @@ static int exynos_ehci_probe(struct platform_device *pdev)
 					"samsung,exynos5440-ehci"))
 		goto skip_phy;
 
-	phy = devm_usb_get_phy(&pdev->dev, USB_PHY_TYPE_USB2);
-	if (IS_ERR(phy)) {
-		usb_put_hcd(hcd);
-		dev_warn(&pdev->dev, "no platform data or transceiver defined\n");
-		return -EPROBE_DEFER;
-	} else {
-		exynos_ehci->phy = phy;
-		exynos_ehci->otg = phy->otg;
-	}
+	err = exynos_ehci_get_phy(&pdev->dev, exynos_ehci);
+	if (err)
+		goto fail_clk;
 
 skip_phy:
 
@@ -135,10 +218,9 @@ skip_phy:
 
 	hcd->rsrc_start = res->start;
 	hcd->rsrc_len = resource_size(res);
-	hcd->regs = devm_ioremap(&pdev->dev, res->start, hcd->rsrc_len);
-	if (!hcd->regs) {
-		dev_err(&pdev->dev, "Failed to remap I/O memory\n");
-		err = -ENOMEM;
+	hcd->regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(hcd->regs)) {
+		err = PTR_ERR(hcd->regs);
 		goto fail_io;
 	}
 
@@ -152,8 +234,11 @@ skip_phy:
 	if (exynos_ehci->otg)
 		exynos_ehci->otg->set_host(exynos_ehci->otg, &hcd->self);
 
-	if (exynos_ehci->phy)
-		usb_phy_init(exynos_ehci->phy);
+	err = exynos_ehci_phy_enable(&pdev->dev);
+	if (err) {
+		dev_err(&pdev->dev, "Failed to enable USB phy\n");
+		goto fail_io;
+	}
 
 	ehci = hcd_to_ehci(hcd);
 	ehci->caps = hcd->regs;
@@ -173,8 +258,7 @@ skip_phy:
 	return 0;
 
 fail_add_hcd:
-	if (exynos_ehci->phy)
-		usb_phy_shutdown(exynos_ehci->phy);
+	exynos_ehci_phy_disable(&pdev->dev);
 fail_io:
 	clk_disable_unprepare(exynos_ehci->clk);
 fail_clk:
@@ -192,8 +276,7 @@ static int exynos_ehci_remove(struct platform_device *pdev)
 	if (exynos_ehci->otg)
 		exynos_ehci->otg->set_host(exynos_ehci->otg, &hcd->self);
 
-	if (exynos_ehci->phy)
-		usb_phy_shutdown(exynos_ehci->phy);
+	exynos_ehci_phy_disable(&pdev->dev);
 
 	clk_disable_unprepare(exynos_ehci->clk);
 
@@ -212,12 +295,13 @@ static int exynos_ehci_suspend(struct device *dev)
 	int rc;
 
 	rc = ehci_suspend(hcd, do_wakeup);
+	if (rc)
+		return rc;
 
 	if (exynos_ehci->otg)
 		exynos_ehci->otg->set_host(exynos_ehci->otg, &hcd->self);
 
-	if (exynos_ehci->phy)
-		usb_phy_shutdown(exynos_ehci->phy);
+	exynos_ehci_phy_disable(dev);
 
 	clk_disable_unprepare(exynos_ehci->clk);
 
@@ -228,14 +312,19 @@ static int exynos_ehci_resume(struct device *dev)
 {
 	struct usb_hcd *hcd = dev_get_drvdata(dev);
 	struct exynos_ehci_hcd *exynos_ehci = to_exynos_ehci(hcd);
+	int ret;
 
 	clk_prepare_enable(exynos_ehci->clk);
 
 	if (exynos_ehci->otg)
 		exynos_ehci->otg->set_host(exynos_ehci->otg, &hcd->self);
 
-	if (exynos_ehci->phy)
-		usb_phy_init(exynos_ehci->phy);
+	ret = exynos_ehci_phy_enable(dev);
+	if (ret) {
+		dev_err(dev, "Failed to enable USB phy\n");
+		clk_disable_unprepare(exynos_ehci->clk);
+		return ret;
+	}
 
 	/* DMA burst Enable */
 	writel(EHCI_INSNREG00_ENABLE_DMA_BURST, EHCI_INSNREG00(hcd->regs));
diff --git a/drivers/usb/host/ehci-fsl.c b/drivers/usb/host/ehci-fsl.c
index 6f2c8d3899d..cf2734b532a 100644
--- a/drivers/usb/host/ehci-fsl.c
+++ b/drivers/usb/host/ehci-fsl.c
@@ -248,7 +248,8 @@ static int ehci_fsl_setup_phy(struct usb_hcd *hcd,
 		break;
 	}
 
-	if (pdata->have_sysif_regs && pdata->controller_ver &&
+	if (pdata->have_sysif_regs &&
+	    pdata->controller_ver > FSL_USB_VER_1_6 &&
 	    (phy_mode == FSL_USB2_PHY_ULPI)) {
 		/* check PHY_CLK_VALID to get phy clk valid */
 		if (!(spin_event_timeout(in_be32(non_ehci + FSL_SOC_USB_CTRL) &
diff --git a/drivers/usb/host/ehci-hub.c b/drivers/usb/host/ehci-hub.c
index 7ae0c4d5174..cc305c71ac3 100644
--- a/drivers/usb/host/ehci-hub.c
+++ b/drivers/usb/host/ehci-hub.c
@@ -33,15 +33,6 @@
 
 #ifdef	CONFIG_PM
 
-static int ehci_hub_control(
-	struct usb_hcd	*hcd,
-	u16		typeReq,
-	u16		wValue,
-	u16		wIndex,
-	char		*buf,
-	u16		wLength
-);
-
 static int persist_enabled_on_companion(struct usb_device *udev, void *unused)
 {
 	return !udev->maxchild && udev->persist_enabled &&
@@ -865,7 +856,7 @@ cleanup:
 #endif /* CONFIG_USB_HCD_TEST_MODE */
 /*-------------------------------------------------------------------------*/
 
-static int ehci_hub_control (
+int ehci_hub_control(
 	struct usb_hcd	*hcd,
 	u16		typeReq,
 	u16		wValue,
@@ -1285,6 +1276,7 @@ error_exit:
 	spin_unlock_irqrestore (&ehci->lock, flags);
 	return retval;
 }
+EXPORT_SYMBOL_GPL(ehci_hub_control);
 
 static void ehci_relinquish_port(struct usb_hcd *hcd, int portnum)
 {
diff --git a/drivers/usb/host/ehci-msm.c b/drivers/usb/host/ehci-msm.c
index f341651d6f6..982c09bebe0 100644
--- a/drivers/usb/host/ehci-msm.c
+++ b/drivers/usb/host/ehci-msm.c
@@ -96,10 +96,9 @@ static int ehci_msm_probe(struct platform_device *pdev)
 
 	hcd->rsrc_start = res->start;
 	hcd->rsrc_len = resource_size(res);
-	hcd->regs = devm_ioremap(&pdev->dev, hcd->rsrc_start, hcd->rsrc_len);
-	if (!hcd->regs) {
-		dev_err(&pdev->dev, "ioremap failed\n");
-		ret = -ENOMEM;
+	hcd->regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(hcd->regs)) {
+		ret = PTR_ERR(hcd->regs);
 		goto put_hcd;
 	}
 
diff --git a/drivers/usb/host/ehci-mv.c b/drivers/usb/host/ehci-mv.c
index bd61612a725..08147c35f83 100644
--- a/drivers/usb/host/ehci-mv.c
+++ b/drivers/usb/host/ehci-mv.c
@@ -176,11 +176,9 @@ static int mv_ehci_probe(struct platform_device *pdev)
 		goto err_put_hcd;
 	}
 
-	ehci_mv->phy_regs = devm_ioremap(&pdev->dev, r->start,
-					 resource_size(r));
-	if (!ehci_mv->phy_regs) {
-		dev_err(&pdev->dev, "failed to map phy I/O memory\n");
-		retval = -EFAULT;
+	ehci_mv->phy_regs = devm_ioremap_resource(&pdev->dev, r);
+	if (IS_ERR(ehci_mv->phy_regs)) {
+		retval = PTR_ERR(ehci_mv->phy_regs);
 		goto err_put_hcd;
 	}
 
@@ -191,11 +189,9 @@ static int mv_ehci_probe(struct platform_device *pdev)
 		goto err_put_hcd;
 	}
 
-	ehci_mv->cap_regs = devm_ioremap(&pdev->dev, r->start,
-					 resource_size(r));
-	if (ehci_mv->cap_regs == NULL) {
-		dev_err(&pdev->dev, "failed to map I/O memory\n");
-		retval = -EFAULT;
+	ehci_mv->cap_regs = devm_ioremap_resource(&pdev->dev, r);
+	if (IS_ERR(ehci_mv->cap_regs)) {
+		retval = PTR_ERR(ehci_mv->cap_regs);
 		goto err_put_hcd;
 	}
 
diff --git a/drivers/usb/host/ehci-orion.c b/drivers/usb/host/ehci-orion.c
index 30d35e5e503..22e15cab8ea 100644
--- a/drivers/usb/host/ehci-orion.c
+++ b/drivers/usb/host/ehci-orion.c
@@ -15,6 +15,7 @@
 #include <linux/clk.h>
 #include <linux/platform_data/usb-ehci-orion.h>
 #include <linux/of.h>
+#include <linux/phy/phy.h>
 #include <linux/of_device.h>
 #include <linux/of_irq.h>
 #include <linux/usb.h>
@@ -42,6 +43,13 @@
 
 #define DRIVER_DESC "EHCI orion driver"
 
+#define hcd_to_orion_priv(h) ((struct orion_ehci_hcd *)hcd_to_ehci(h)->priv)
+
+struct orion_ehci_hcd {
+	struct clk *clk;
+	struct phy *phy;
+};
+
 static const char hcd_name[] = "ehci-orion";
 
 static struct hc_driver __read_mostly ehci_orion_hc_driver;
@@ -137,6 +145,10 @@ ehci_orion_conf_mbus_windows(struct usb_hcd *hcd,
 	}
 }
 
+static const struct ehci_driver_overrides orion_overrides __initconst = {
+	.extra_priv_size =	sizeof(struct orion_ehci_hcd),
+};
+
 static int ehci_orion_drv_probe(struct platform_device *pdev)
 {
 	struct orion_ehci_data *pd = dev_get_platdata(&pdev->dev);
@@ -144,26 +156,23 @@ static int ehci_orion_drv_probe(struct platform_device *pdev)
 	struct resource *res;
 	struct usb_hcd *hcd;
 	struct ehci_hcd *ehci;
-	struct clk *clk;
 	void __iomem *regs;
 	int irq, err;
 	enum orion_ehci_phy_ver phy_version;
+	struct orion_ehci_hcd *priv;
 
 	if (usb_disabled())
 		return -ENODEV;
 
 	pr_debug("Initializing Orion-SoC USB Host Controller\n");
 
-	if (pdev->dev.of_node)
-		irq = irq_of_parse_and_map(pdev->dev.of_node, 0);
-	else
-		irq = platform_get_irq(pdev, 0);
+	irq = platform_get_irq(pdev, 0);
 	if (irq <= 0) {
 		dev_err(&pdev->dev,
 			"Found HC with no IRQ. Check %s setup!\n",
 			dev_name(&pdev->dev));
 		err = -ENODEV;
-		goto err1;
+		goto err;
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -172,7 +181,7 @@ static int ehci_orion_drv_probe(struct platform_device *pdev)
 			"Found HC with no register addr. Check %s setup!\n",
 			dev_name(&pdev->dev));
 		err = -ENODEV;
-		goto err1;
+		goto err;
 	}
 
 	/*
@@ -182,25 +191,19 @@ static int ehci_orion_drv_probe(struct platform_device *pdev)
 	 */
 	err = dma_coerce_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(32));
 	if (err)
-		goto err1;
+		goto err;
 
 	regs = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(regs)) {
 		err = PTR_ERR(regs);
-		goto err1;
+		goto err;
 	}
 
-	/* Not all platforms can gate the clock, so it is not
-	   an error if the clock does not exists. */
-	clk = devm_clk_get(&pdev->dev, NULL);
-	if (!IS_ERR(clk))
-		clk_prepare_enable(clk);
-
 	hcd = usb_create_hcd(&ehci_orion_hc_driver,
 			&pdev->dev, dev_name(&pdev->dev));
 	if (!hcd) {
 		err = -ENOMEM;
-		goto err2;
+		goto err;
 	}
 
 	hcd->rsrc_start = res->start;
@@ -211,6 +214,29 @@ static int ehci_orion_drv_probe(struct platform_device *pdev)
 	ehci->caps = hcd->regs + 0x100;
 	hcd->has_tt = 1;
 
+	priv = hcd_to_orion_priv(hcd);
+	/*
+	 * Not all platforms can gate the clock, so it is not an error if
+	 * the clock does not exists.
+	 */
+	priv->clk = devm_clk_get(&pdev->dev, NULL);
+	if (!IS_ERR(priv->clk))
+		clk_prepare_enable(priv->clk);
+
+	priv->phy = devm_phy_optional_get(&pdev->dev, "usb");
+	if (IS_ERR(priv->phy)) {
+		err = PTR_ERR(priv->phy);
+		goto err_phy_get;
+	} else {
+		err = phy_init(priv->phy);
+		if (err)
+			goto err_phy_init;
+
+		err = phy_power_on(priv->phy);
+		if (err)
+			goto err_phy_power_on;
+	}
+
 	/*
 	 * (Re-)program MBUS remapping windows if we are asked to.
 	 */
@@ -240,17 +266,23 @@ static int ehci_orion_drv_probe(struct platform_device *pdev)
 
 	err = usb_add_hcd(hcd, irq, IRQF_SHARED);
 	if (err)
-		goto err3;
+		goto err_add_hcd;
 
 	device_wakeup_enable(hcd->self.controller);
 	return 0;
 
-err3:
+err_add_hcd:
+	if (!IS_ERR(priv->phy))
+		phy_power_off(priv->phy);
+err_phy_power_on:
+	if (!IS_ERR(priv->phy))
+		phy_exit(priv->phy);
+err_phy_init:
+err_phy_get:
+	if (!IS_ERR(priv->clk))
+		clk_disable_unprepare(priv->clk);
 	usb_put_hcd(hcd);
-err2:
-	if (!IS_ERR(clk))
-		clk_disable_unprepare(clk);
-err1:
+err:
 	dev_err(&pdev->dev, "init %s fail, %d\n",
 		dev_name(&pdev->dev), err);
 
@@ -260,14 +292,20 @@ err1:
 static int ehci_orion_drv_remove(struct platform_device *pdev)
 {
 	struct usb_hcd *hcd = platform_get_drvdata(pdev);
-	struct clk *clk;
+	struct orion_ehci_hcd *priv = hcd_to_orion_priv(hcd);
 
 	usb_remove_hcd(hcd);
+
+	if (!IS_ERR(priv->phy)) {
+		phy_power_off(priv->phy);
+		phy_exit(priv->phy);
+	}
+
+	if (!IS_ERR(priv->clk))
+		clk_disable_unprepare(priv->clk);
+
 	usb_put_hcd(hcd);
 
-	clk = devm_clk_get(&pdev->dev, NULL);
-	if (!IS_ERR(clk))
-		clk_disable_unprepare(clk);
 	return 0;
 }
 
@@ -295,7 +333,7 @@ static int __init ehci_orion_init(void)
 
 	pr_info("%s: " DRIVER_DESC "\n", hcd_name);
 
-	ehci_init_driver(&ehci_orion_hc_driver, NULL);
+	ehci_init_driver(&ehci_orion_hc_driver, &orion_overrides);
 	return platform_driver_register(&ehci_orion_driver);
 }
 module_init(ehci_orion_init);
diff --git a/drivers/usb/host/ehci-platform.c b/drivers/usb/host/ehci-platform.c
index b3a0e11073a..2f5b9ce3e04 100644
--- a/drivers/usb/host/ehci-platform.c
+++ b/drivers/usb/host/ehci-platform.c
@@ -29,6 +29,7 @@
 #include <linux/of.h>
 #include <linux/phy/phy.h>
 #include <linux/platform_device.h>
+#include <linux/reset.h>
 #include <linux/usb.h>
 #include <linux/usb/hcd.h>
 #include <linux/usb/ehci_pdriver.h>
@@ -41,6 +42,7 @@
 
 struct ehci_platform_priv {
 	struct clk *clks[EHCI_MAX_CLKS];
+	struct reset_control *rst;
 	struct phy *phy;
 };
 
@@ -208,6 +210,18 @@ static int ehci_platform_probe(struct platform_device *dev)
 		}
 	}
 
+	priv->rst = devm_reset_control_get_optional(&dev->dev, NULL);
+	if (IS_ERR(priv->rst)) {
+		err = PTR_ERR(priv->rst);
+		if (err == -EPROBE_DEFER)
+			goto err_put_clks;
+		priv->rst = NULL;
+	} else {
+		err = reset_control_deassert(priv->rst);
+		if (err)
+			goto err_put_clks;
+	}
+
 	if (pdata->big_endian_desc)
 		ehci->big_endian_desc = 1;
 	if (pdata->big_endian_mmio)
@@ -218,7 +232,7 @@ static int ehci_platform_probe(struct platform_device *dev)
 		dev_err(&dev->dev,
 			"Error: CONFIG_USB_EHCI_BIG_ENDIAN_MMIO not set\n");
 		err = -EINVAL;
-		goto err_put_clks;
+		goto err_reset;
 	}
 #endif
 #ifndef CONFIG_USB_EHCI_BIG_ENDIAN_DESC
@@ -226,14 +240,14 @@ static int ehci_platform_probe(struct platform_device *dev)
 		dev_err(&dev->dev,
 			"Error: CONFIG_USB_EHCI_BIG_ENDIAN_DESC not set\n");
 		err = -EINVAL;
-		goto err_put_clks;
+		goto err_reset;
 	}
 #endif
 
 	if (pdata->power_on) {
 		err = pdata->power_on(dev);
 		if (err < 0)
-			goto err_put_clks;
+			goto err_reset;
 	}
 
 	hcd->rsrc_start = res_mem->start;
@@ -256,6 +270,9 @@ static int ehci_platform_probe(struct platform_device *dev)
 err_power:
 	if (pdata->power_off)
 		pdata->power_off(dev);
+err_reset:
+	if (priv->rst)
+		reset_control_assert(priv->rst);
 err_put_clks:
 	while (--clk >= 0)
 		clk_put(priv->clks[clk]);
@@ -280,6 +297,9 @@ static int ehci_platform_remove(struct platform_device *dev)
 	if (pdata->power_off)
 		pdata->power_off(dev);
 
+	if (priv->rst)
+		reset_control_assert(priv->rst);
+
 	for (clk = 0; clk < EHCI_MAX_CLKS && priv->clks[clk]; clk++)
 		clk_put(priv->clks[clk]);
 
@@ -303,6 +323,8 @@ static int ehci_platform_suspend(struct device *dev)
 	int ret;
 
 	ret = ehci_suspend(hcd, do_wakeup);
+	if (ret)
+		return ret;
 
 	if (pdata->power_suspend)
 		pdata->power_suspend(pdev);
diff --git a/drivers/usb/host/ehci-spear.c b/drivers/usb/host/ehci-spear.c
index 8bd915b2ae8..1d59958ad0c 100644
--- a/drivers/usb/host/ehci-spear.c
+++ b/drivers/usb/host/ehci-spear.c
@@ -106,16 +106,9 @@ static int spear_ehci_hcd_drv_probe(struct platform_device *pdev)
 
 	hcd->rsrc_start = res->start;
 	hcd->rsrc_len = resource_size(res);
-	if (!devm_request_mem_region(&pdev->dev, hcd->rsrc_start, hcd->rsrc_len,
-				driver->description)) {
-		retval = -EBUSY;
-		goto err_put_hcd;
-	}
-
-	hcd->regs = devm_ioremap(&pdev->dev, hcd->rsrc_start, hcd->rsrc_len);
-	if (hcd->regs == NULL) {
-		dev_dbg(&pdev->dev, "error mapping memory\n");
-		retval = -ENOMEM;
+	hcd->regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(hcd->regs)) {
+		retval = PTR_ERR(hcd->regs);
 		goto err_put_hcd;
 	}
 
diff --git a/drivers/usb/host/ehci-tegra.c b/drivers/usb/host/ehci-tegra.c
index 27ac6ad53c3..6fdcb8ad229 100644
--- a/drivers/usb/host/ehci-tegra.c
+++ b/drivers/usb/host/ehci-tegra.c
@@ -51,10 +51,6 @@ struct tegra_ehci_soc_config {
 	bool has_hostpc;
 };
 
-static int (*orig_hub_control)(struct usb_hcd *hcd,
-				u16 typeReq, u16 wValue, u16 wIndex,
-				char *buf, u16 wLength);
-
 struct tegra_ehci_hcd {
 	struct tegra_usb_phy *phy;
 	struct clk *clk;
@@ -236,7 +232,7 @@ static int tegra_ehci_hub_control(
 	spin_unlock_irqrestore(&ehci->lock, flags);
 
 	/* Handle the hub control events here */
-	return orig_hub_control(hcd, typeReq, wValue, wIndex, buf, wLength);
+	return ehci_hub_control(hcd, typeReq, wValue, wIndex, buf, wLength);
 
 done:
 	spin_unlock_irqrestore(&ehci->lock, flags);
@@ -415,10 +411,9 @@ static int tegra_ehci_probe(struct platform_device *pdev)
 	}
 	hcd->rsrc_start = res->start;
 	hcd->rsrc_len = resource_size(res);
-	hcd->regs = devm_ioremap(&pdev->dev, res->start, resource_size(res));
-	if (!hcd->regs) {
-		dev_err(&pdev->dev, "Failed to remap I/O memory\n");
-		err = -ENOMEM;
+	hcd->regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(hcd->regs)) {
+		err = PTR_ERR(hcd->regs);
 		goto cleanup_clk_en;
 	}
 	ehci->caps = hcd->regs + 0x100;
@@ -509,8 +504,31 @@ static struct platform_driver tegra_ehci_driver = {
 	}
 };
 
+static int tegra_ehci_reset(struct usb_hcd *hcd)
+{
+	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
+	int retval;
+	int txfifothresh;
+
+	retval = ehci_setup(hcd);
+	if (retval)
+		return retval;
+
+	/*
+	 * We should really pull this value out of tegra_ehci_soc_config, but
+	 * to avoid needing access to it, make use of the fact that Tegra20 is
+	 * the only one so far that needs a value of 10, and Tegra20 is the
+	 * only one which doesn't set has_hostpc.
+	 */
+	txfifothresh = ehci->has_hostpc ? 0x10 : 10;
+	ehci_writel(ehci, txfifothresh << 16, &ehci->regs->txfill_tuning);
+
+	return 0;
+}
+
 static const struct ehci_driver_overrides tegra_overrides __initconst = {
 	.extra_priv_size	= sizeof(struct tegra_ehci_hcd),
+	.reset			= tegra_ehci_reset,
 };
 
 static int __init ehci_tegra_init(void)
@@ -531,8 +549,6 @@ static int __init ehci_tegra_init(void)
 	 * too easy.
 	 */
 
-	orig_hub_control = tegra_ehci_hc_driver.hub_control;
-
 	tegra_ehci_hc_driver.map_urb_for_dma = tegra_ehci_map_urb_for_dma;
 	tegra_ehci_hc_driver.unmap_urb_for_dma = tegra_ehci_unmap_urb_for_dma;
 	tegra_ehci_hc_driver.hub_control = tegra_ehci_hub_control;
diff --git a/drivers/usb/host/ehci.h b/drivers/usb/host/ehci.h
index 9dfc6c1394d..eee228a26a0 100644
--- a/drivers/usb/host/ehci.h
+++ b/drivers/usb/host/ehci.h
@@ -872,4 +872,7 @@ extern int	ehci_suspend(struct usb_hcd *hcd, bool do_wakeup);
 extern int	ehci_resume(struct usb_hcd *hcd, bool hibernated);
 #endif	/* CONFIG_PM */
 
+extern int	ehci_hub_control(struct usb_hcd	*hcd, u16 typeReq, u16 wValue,
+				 u16 wIndex, char *buf, u16 wLength);
+
 #endif /* __LINUX_EHCI_HCD_H */
diff --git a/drivers/usb/host/max3421-hcd.c b/drivers/usb/host/max3421-hcd.c
new file mode 100644
index 00000000000..858efcfda50
--- /dev/null
+++ b/drivers/usb/host/max3421-hcd.c
@@ -0,0 +1,1957 @@
+/*
+ * MAX3421 Host Controller driver for USB.
+ *
+ * Author: David Mosberger-Tang <davidm@egauge.net>
+ *
+ * (C) Copyright 2014 David Mosberger-Tang <davidm@egauge.net>
+ *
+ * MAX3421 is a chip implementing a USB 2.0 Full-/Low-Speed host
+ * controller on a SPI bus.
+ *
+ * Based on:
+ *	o MAX3421E datasheet
+ *		http://datasheets.maximintegrated.com/en/ds/MAX3421E.pdf
+ *	o MAX3421E Programming Guide
+ *		http://www.hdl.co.jp/ftpdata/utl-001/AN3785.pdf
+ *	o gadget/dummy_hcd.c
+ *		For USB HCD implementation.
+ *	o Arduino MAX3421 driver
+ *	     https://github.com/felis/USB_Host_Shield_2.0/blob/master/Usb.cpp
+ *
+ * This file is licenced under the GPL v2.
+ *
+ * Important note on worst-case (full-speed) packet size constraints
+ * (See USB 2.0 Section 5.6.3 and following):
+ *
+ *	- control:	  64 bytes
+ *	- isochronous:	1023 bytes
+ *	- interrupt:	  64 bytes
+ *	- bulk:		  64 bytes
+ *
+ * Since the MAX3421 FIFO size is 64 bytes, we do not have to work about
+ * multi-FIFO writes/reads for a single USB packet *except* for isochronous
+ * transfers.  We don't support isochronous transfers at this time, so we
+ * just assume that a USB packet always fits into a single FIFO buffer.
+ *
+ * NOTE: The June 2006 version of "MAX3421E Programming Guide"
+ * (AN3785) has conflicting info for the RCVDAVIRQ bit:
+ *
+ *	The description of RCVDAVIRQ says "The CPU *must* clear
+ *	this IRQ bit (by writing a 1 to it) before reading the
+ *	RCVFIFO data.
+ *
+ * However, the earlier section on "Programming BULK-IN
+ * Transfers" says * that:
+ *
+ *	After the CPU retrieves the data, it clears the
+ *	RCVDAVIRQ bit.
+ *
+ * The December 2006 version has been corrected and it consistently
+ * states the second behavior is the correct one.
+ *
+ * Synchronous SPI transactions sleep so we can't perform any such
+ * transactions while holding a spin-lock (and/or while interrupts are
+ * masked).  To achieve this, all SPI transactions are issued from a
+ * single thread (max3421_spi_thread).
+ */
+
+#include <linux/module.h>
+#include <linux/spi/spi.h>
+#include <linux/usb.h>
+#include <linux/usb/hcd.h>
+
+#include <linux/platform_data/max3421-hcd.h>
+
+#define DRIVER_DESC	"MAX3421 USB Host-Controller Driver"
+#define DRIVER_VERSION	"1.0"
+
+/* 11-bit counter that wraps around (USB 2.0 Section 8.3.3): */
+#define USB_MAX_FRAME_NUMBER	0x7ff
+#define USB_MAX_RETRIES		3 /* # of retries before error is reported */
+
+/*
+ * Max. # of times we're willing to retransmit a request immediately in
+ * resposne to a NAK.  Afterwards, we fall back on trying once a frame.
+ */
+#define NAK_MAX_FAST_RETRANSMITS	2
+
+#define POWER_BUDGET	500	/* in mA; use 8 for low-power port testing */
+
+/* Port-change mask: */
+#define PORT_C_MASK	((USB_PORT_STAT_C_CONNECTION |	\
+			  USB_PORT_STAT_C_ENABLE |	\
+			  USB_PORT_STAT_C_SUSPEND |	\
+			  USB_PORT_STAT_C_OVERCURRENT | \
+			  USB_PORT_STAT_C_RESET) << 16)
+
+enum max3421_rh_state {
+	MAX3421_RH_RESET,
+	MAX3421_RH_SUSPENDED,
+	MAX3421_RH_RUNNING
+};
+
+enum pkt_state {
+	PKT_STATE_SETUP,	/* waiting to send setup packet to ctrl pipe */
+	PKT_STATE_TRANSFER,	/* waiting to xfer transfer_buffer */
+	PKT_STATE_TERMINATE	/* waiting to terminate control transfer */
+};
+
+enum scheduling_pass {
+	SCHED_PASS_PERIODIC,
+	SCHED_PASS_NON_PERIODIC,
+	SCHED_PASS_DONE
+};
+
+struct max3421_dma_buf {
+	u8 data[2];
+};
+
+struct max3421_hcd {
+	spinlock_t lock;
+
+	struct task_struct *spi_thread;
+
+	struct max3421_hcd *next;
+
+	enum max3421_rh_state rh_state;
+	/* lower 16 bits contain port status, upper 16 bits the change mask: */
+	u32 port_status;
+
+	unsigned active:1;
+
+	struct list_head ep_list;	/* list of EP's with work */
+
+	/*
+	 * The following are owned by spi_thread (may be accessed by
+	 * SPI-thread without acquiring the HCD lock:
+	 */
+	u8 rev;				/* chip revision */
+	u16 frame_number;
+	/*
+	 * kmalloc'd buffers guaranteed to be in separate (DMA)
+	 * cache-lines:
+	 */
+	struct max3421_dma_buf *tx;
+	struct max3421_dma_buf *rx;
+	/*
+	 * URB we're currently processing.  Must not be reset to NULL
+	 * unless MAX3421E chip is idle:
+	 */
+	struct urb *curr_urb;
+	enum scheduling_pass sched_pass;
+	struct usb_device *loaded_dev;	/* dev that's loaded into the chip */
+	int loaded_epnum;		/* epnum whose toggles are loaded */
+	int urb_done;			/* > 0 -> no errors, < 0: errno */
+	size_t curr_len;
+	u8 hien;
+	u8 mode;
+	u8 iopins[2];
+	unsigned int do_enable_irq:1;
+	unsigned int do_reset_hcd:1;
+	unsigned int do_reset_port:1;
+	unsigned int do_check_unlink:1;
+	unsigned int do_iopin_update:1;
+#ifdef DEBUG
+	unsigned long err_stat[16];
+#endif
+};
+
+struct max3421_ep {
+	struct usb_host_endpoint *ep;
+	struct list_head ep_list;
+	u32 naks;
+	u16 last_active;		/* frame # this ep was last active */
+	enum pkt_state pkt_state;
+	u8 retries;
+	u8 retransmit;			/* packet needs retransmission */
+};
+
+static struct max3421_hcd *max3421_hcd_list;
+
+#define MAX3421_FIFO_SIZE	64
+
+#define MAX3421_SPI_DIR_RD	0	/* read register from MAX3421 */
+#define MAX3421_SPI_DIR_WR	1	/* write register to MAX3421 */
+
+/* SPI commands: */
+#define MAX3421_SPI_DIR_SHIFT	1
+#define MAX3421_SPI_REG_SHIFT	3
+
+#define MAX3421_REG_RCVFIFO	1
+#define MAX3421_REG_SNDFIFO	2
+#define MAX3421_REG_SUDFIFO	4
+#define MAX3421_REG_RCVBC	6
+#define MAX3421_REG_SNDBC	7
+#define MAX3421_REG_USBIRQ	13
+#define MAX3421_REG_USBIEN	14
+#define MAX3421_REG_USBCTL	15
+#define MAX3421_REG_CPUCTL	16
+#define MAX3421_REG_PINCTL	17
+#define MAX3421_REG_REVISION	18
+#define MAX3421_REG_IOPINS1	20
+#define MAX3421_REG_IOPINS2	21
+#define MAX3421_REG_GPINIRQ	22
+#define MAX3421_REG_GPINIEN	23
+#define MAX3421_REG_GPINPOL	24
+#define MAX3421_REG_HIRQ	25
+#define MAX3421_REG_HIEN	26
+#define MAX3421_REG_MODE	27
+#define MAX3421_REG_PERADDR	28
+#define MAX3421_REG_HCTL	29
+#define MAX3421_REG_HXFR	30
+#define MAX3421_REG_HRSL	31
+
+enum {
+	MAX3421_USBIRQ_OSCOKIRQ_BIT = 0,
+	MAX3421_USBIRQ_NOVBUSIRQ_BIT = 5,
+	MAX3421_USBIRQ_VBUSIRQ_BIT
+};
+
+enum {
+	MAX3421_CPUCTL_IE_BIT = 0,
+	MAX3421_CPUCTL_PULSEWID0_BIT = 6,
+	MAX3421_CPUCTL_PULSEWID1_BIT
+};
+
+enum {
+	MAX3421_USBCTL_PWRDOWN_BIT = 4,
+	MAX3421_USBCTL_CHIPRES_BIT
+};
+
+enum {
+	MAX3421_PINCTL_GPXA_BIT	= 0,
+	MAX3421_PINCTL_GPXB_BIT,
+	MAX3421_PINCTL_POSINT_BIT,
+	MAX3421_PINCTL_INTLEVEL_BIT,
+	MAX3421_PINCTL_FDUPSPI_BIT,
+	MAX3421_PINCTL_EP0INAK_BIT,
+	MAX3421_PINCTL_EP2INAK_BIT,
+	MAX3421_PINCTL_EP3INAK_BIT,
+};
+
+enum {
+	MAX3421_HI_BUSEVENT_BIT = 0,	/* bus-reset/-resume */
+	MAX3421_HI_RWU_BIT,		/* remote wakeup */
+	MAX3421_HI_RCVDAV_BIT,		/* receive FIFO data available */
+	MAX3421_HI_SNDBAV_BIT,		/* send buffer available */
+	MAX3421_HI_SUSDN_BIT,		/* suspend operation done */
+	MAX3421_HI_CONDET_BIT,		/* peripheral connect/disconnect */
+	MAX3421_HI_FRAME_BIT,		/* frame generator */
+	MAX3421_HI_HXFRDN_BIT,		/* host transfer done */
+};
+
+enum {
+	MAX3421_HCTL_BUSRST_BIT = 0,
+	MAX3421_HCTL_FRMRST_BIT,
+	MAX3421_HCTL_SAMPLEBUS_BIT,
+	MAX3421_HCTL_SIGRSM_BIT,
+	MAX3421_HCTL_RCVTOG0_BIT,
+	MAX3421_HCTL_RCVTOG1_BIT,
+	MAX3421_HCTL_SNDTOG0_BIT,
+	MAX3421_HCTL_SNDTOG1_BIT
+};
+
+enum {
+	MAX3421_MODE_HOST_BIT = 0,
+	MAX3421_MODE_LOWSPEED_BIT,
+	MAX3421_MODE_HUBPRE_BIT,
+	MAX3421_MODE_SOFKAENAB_BIT,
+	MAX3421_MODE_SEPIRQ_BIT,
+	MAX3421_MODE_DELAYISO_BIT,
+	MAX3421_MODE_DMPULLDN_BIT,
+	MAX3421_MODE_DPPULLDN_BIT
+};
+
+enum {
+	MAX3421_HRSL_OK = 0,
+	MAX3421_HRSL_BUSY,
+	MAX3421_HRSL_BADREQ,
+	MAX3421_HRSL_UNDEF,
+	MAX3421_HRSL_NAK,
+	MAX3421_HRSL_STALL,
+	MAX3421_HRSL_TOGERR,
+	MAX3421_HRSL_WRONGPID,
+	MAX3421_HRSL_BADBC,
+	MAX3421_HRSL_PIDERR,
+	MAX3421_HRSL_PKTERR,
+	MAX3421_HRSL_CRCERR,
+	MAX3421_HRSL_KERR,
+	MAX3421_HRSL_JERR,
+	MAX3421_HRSL_TIMEOUT,
+	MAX3421_HRSL_BABBLE,
+	MAX3421_HRSL_RESULT_MASK = 0xf,
+	MAX3421_HRSL_RCVTOGRD_BIT = 4,
+	MAX3421_HRSL_SNDTOGRD_BIT,
+	MAX3421_HRSL_KSTATUS_BIT,
+	MAX3421_HRSL_JSTATUS_BIT
+};
+
+/* Return same error-codes as ohci.h:cc_to_error: */
+static const int hrsl_to_error[] = {
+	[MAX3421_HRSL_OK] =		0,
+	[MAX3421_HRSL_BUSY] =		-EINVAL,
+	[MAX3421_HRSL_BADREQ] =		-EINVAL,
+	[MAX3421_HRSL_UNDEF] =		-EINVAL,
+	[MAX3421_HRSL_NAK] =		-EAGAIN,
+	[MAX3421_HRSL_STALL] =		-EPIPE,
+	[MAX3421_HRSL_TOGERR] =		-EILSEQ,
+	[MAX3421_HRSL_WRONGPID] =	-EPROTO,
+	[MAX3421_HRSL_BADBC] =		-EREMOTEIO,
+	[MAX3421_HRSL_PIDERR] =		-EPROTO,
+	[MAX3421_HRSL_PKTERR] =		-EPROTO,
+	[MAX3421_HRSL_CRCERR] =		-EILSEQ,
+	[MAX3421_HRSL_KERR] =		-EIO,
+	[MAX3421_HRSL_JERR] =		-EIO,
+	[MAX3421_HRSL_TIMEOUT] =	-ETIME,
+	[MAX3421_HRSL_BABBLE] =		-EOVERFLOW
+};
+
+/*
+ * See http://www.beyondlogic.org/usbnutshell/usb4.shtml#Control for a
+ * reasonable overview of how control transfers use the the IN/OUT
+ * tokens.
+ */
+#define MAX3421_HXFR_BULK_IN(ep)	(0x00 | (ep))	/* bulk or interrupt */
+#define MAX3421_HXFR_SETUP		 0x10
+#define MAX3421_HXFR_BULK_OUT(ep)	(0x20 | (ep))	/* bulk or interrupt */
+#define MAX3421_HXFR_ISO_IN(ep)		(0x40 | (ep))
+#define MAX3421_HXFR_ISO_OUT(ep)	(0x60 | (ep))
+#define MAX3421_HXFR_HS_IN		 0x80		/* handshake in */
+#define MAX3421_HXFR_HS_OUT		 0xa0		/* handshake out */
+
+#define field(val, bit)	((val) << (bit))
+
+static inline s16
+frame_diff(u16 left, u16 right)
+{
+	return ((unsigned) (left - right)) % (USB_MAX_FRAME_NUMBER + 1);
+}
+
+static inline struct max3421_hcd *
+hcd_to_max3421(struct usb_hcd *hcd)
+{
+	return (struct max3421_hcd *) hcd->hcd_priv;
+}
+
+static inline struct usb_hcd *
+max3421_to_hcd(struct max3421_hcd *max3421_hcd)
+{
+	return container_of((void *) max3421_hcd, struct usb_hcd, hcd_priv);
+}
+
+static u8
+spi_rd8(struct usb_hcd *hcd, unsigned int reg)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	struct spi_device *spi = to_spi_device(hcd->self.controller);
+	struct spi_transfer transfer;
+	struct spi_message msg;
+
+	memset(&transfer, 0, sizeof(transfer));
+
+	spi_message_init(&msg);
+
+	max3421_hcd->tx->data[0] =
+		(field(reg, MAX3421_SPI_REG_SHIFT) |
+		 field(MAX3421_SPI_DIR_RD, MAX3421_SPI_DIR_SHIFT));
+
+	transfer.tx_buf = max3421_hcd->tx->data;
+	transfer.rx_buf = max3421_hcd->rx->data;
+	transfer.len = 2;
+
+	spi_message_add_tail(&transfer, &msg);
+	spi_sync(spi, &msg);
+
+	return max3421_hcd->rx->data[1];
+}
+
+static void
+spi_wr8(struct usb_hcd *hcd, unsigned int reg, u8 val)
+{
+	struct spi_device *spi = to_spi_device(hcd->self.controller);
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	struct spi_transfer transfer;
+	struct spi_message msg;
+
+	memset(&transfer, 0, sizeof(transfer));
+
+	spi_message_init(&msg);
+
+	max3421_hcd->tx->data[0] =
+		(field(reg, MAX3421_SPI_REG_SHIFT) |
+		 field(MAX3421_SPI_DIR_WR, MAX3421_SPI_DIR_SHIFT));
+	max3421_hcd->tx->data[1] = val;
+
+	transfer.tx_buf = max3421_hcd->tx->data;
+	transfer.len = 2;
+
+	spi_message_add_tail(&transfer, &msg);
+	spi_sync(spi, &msg);
+}
+
+static void
+spi_rd_buf(struct usb_hcd *hcd, unsigned int reg, void *buf, size_t len)
+{
+	struct spi_device *spi = to_spi_device(hcd->self.controller);
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	struct spi_transfer transfer[2];
+	struct spi_message msg;
+
+	memset(transfer, 0, sizeof(transfer));
+
+	spi_message_init(&msg);
+
+	max3421_hcd->tx->data[0] =
+		(field(reg, MAX3421_SPI_REG_SHIFT) |
+		 field(MAX3421_SPI_DIR_RD, MAX3421_SPI_DIR_SHIFT));
+	transfer[0].tx_buf = max3421_hcd->tx->data;
+	transfer[0].len = 1;
+
+	transfer[1].rx_buf = buf;
+	transfer[1].len = len;
+
+	spi_message_add_tail(&transfer[0], &msg);
+	spi_message_add_tail(&transfer[1], &msg);
+	spi_sync(spi, &msg);
+}
+
+static void
+spi_wr_buf(struct usb_hcd *hcd, unsigned int reg, void *buf, size_t len)
+{
+	struct spi_device *spi = to_spi_device(hcd->self.controller);
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	struct spi_transfer transfer[2];
+	struct spi_message msg;
+
+	memset(transfer, 0, sizeof(transfer));
+
+	spi_message_init(&msg);
+
+	max3421_hcd->tx->data[0] =
+		(field(reg, MAX3421_SPI_REG_SHIFT) |
+		 field(MAX3421_SPI_DIR_WR, MAX3421_SPI_DIR_SHIFT));
+
+	transfer[0].tx_buf = max3421_hcd->tx->data;
+	transfer[0].len = 1;
+
+	transfer[1].tx_buf = buf;
+	transfer[1].len = len;
+
+	spi_message_add_tail(&transfer[0], &msg);
+	spi_message_add_tail(&transfer[1], &msg);
+	spi_sync(spi, &msg);
+}
+
+/*
+ * Figure out the correct setting for the LOWSPEED and HUBPRE mode
+ * bits.  The HUBPRE bit needs to be set when MAX3421E operates at
+ * full speed, but it's talking to a low-speed device (i.e., through a
+ * hub).  Setting that bit ensures that every low-speed packet is
+ * preceded by a full-speed PRE PID.  Possible configurations:
+ *
+ * Hub speed:	Device speed:	=>	LOWSPEED bit:	HUBPRE bit:
+ *	FULL	FULL		=>	0		0
+ *	FULL	LOW		=>	1		1
+ *	LOW	LOW		=>	1		0
+ *	LOW	FULL		=>	1		0
+ */
+static void
+max3421_set_speed(struct usb_hcd *hcd, struct usb_device *dev)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	u8 mode_lowspeed, mode_hubpre, mode = max3421_hcd->mode;
+
+	mode_lowspeed = BIT(MAX3421_MODE_LOWSPEED_BIT);
+	mode_hubpre   = BIT(MAX3421_MODE_HUBPRE_BIT);
+	if (max3421_hcd->port_status & USB_PORT_STAT_LOW_SPEED) {
+		mode |=  mode_lowspeed;
+		mode &= ~mode_hubpre;
+	} else if (dev->speed == USB_SPEED_LOW) {
+		mode |= mode_lowspeed | mode_hubpre;
+	} else {
+		mode &= ~(mode_lowspeed | mode_hubpre);
+	}
+	if (mode != max3421_hcd->mode) {
+		max3421_hcd->mode = mode;
+		spi_wr8(hcd, MAX3421_REG_MODE, max3421_hcd->mode);
+	}
+
+}
+
+/*
+ * Caller must NOT hold HCD spinlock.
+ */
+static void
+max3421_set_address(struct usb_hcd *hcd, struct usb_device *dev, int epnum,
+		    int force_toggles)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	int old_epnum, same_ep, rcvtog, sndtog;
+	struct usb_device *old_dev;
+	u8 hctl;
+
+	old_dev = max3421_hcd->loaded_dev;
+	old_epnum = max3421_hcd->loaded_epnum;
+
+	same_ep = (dev == old_dev && epnum == old_epnum);
+	if (same_ep && !force_toggles)
+		return;
+
+	if (old_dev && !same_ep) {
+		/* save the old end-points toggles: */
+		u8 hrsl = spi_rd8(hcd, MAX3421_REG_HRSL);
+
+		rcvtog = (hrsl >> MAX3421_HRSL_RCVTOGRD_BIT) & 1;
+		sndtog = (hrsl >> MAX3421_HRSL_SNDTOGRD_BIT) & 1;
+
+		/* no locking: HCD (i.e., we) own toggles, don't we? */
+		usb_settoggle(old_dev, old_epnum, 0, rcvtog);
+		usb_settoggle(old_dev, old_epnum, 1, sndtog);
+	}
+	/* setup new endpoint's toggle bits: */
+	rcvtog = usb_gettoggle(dev, epnum, 0);
+	sndtog = usb_gettoggle(dev, epnum, 1);
+	hctl = (BIT(rcvtog + MAX3421_HCTL_RCVTOG0_BIT) |
+		BIT(sndtog + MAX3421_HCTL_SNDTOG0_BIT));
+
+	max3421_hcd->loaded_epnum = epnum;
+	spi_wr8(hcd, MAX3421_REG_HCTL, hctl);
+
+	/*
+	 * Note: devnum for one and the same device can change during
+	 * address-assignment so it's best to just always load the
+	 * address whenever the end-point changed/was forced.
+	 */
+	max3421_hcd->loaded_dev = dev;
+	spi_wr8(hcd, MAX3421_REG_PERADDR, dev->devnum);
+}
+
+static int
+max3421_ctrl_setup(struct usb_hcd *hcd, struct urb *urb)
+{
+	spi_wr_buf(hcd, MAX3421_REG_SUDFIFO, urb->setup_packet, 8);
+	return MAX3421_HXFR_SETUP;
+}
+
+static int
+max3421_transfer_in(struct usb_hcd *hcd, struct urb *urb)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	int epnum = usb_pipeendpoint(urb->pipe);
+
+	max3421_hcd->curr_len = 0;
+	max3421_hcd->hien |= BIT(MAX3421_HI_RCVDAV_BIT);
+	return MAX3421_HXFR_BULK_IN(epnum);
+}
+
+static int
+max3421_transfer_out(struct usb_hcd *hcd, struct urb *urb, int fast_retransmit)
+{
+	struct spi_device *spi = to_spi_device(hcd->self.controller);
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	int epnum = usb_pipeendpoint(urb->pipe);
+	u32 max_packet;
+	void *src;
+
+	src = urb->transfer_buffer + urb->actual_length;
+
+	if (fast_retransmit) {
+		if (max3421_hcd->rev == 0x12) {
+			/* work around rev 0x12 bug: */
+			spi_wr8(hcd, MAX3421_REG_SNDBC, 0);
+			spi_wr8(hcd, MAX3421_REG_SNDFIFO, ((u8 *) src)[0]);
+			spi_wr8(hcd, MAX3421_REG_SNDBC, max3421_hcd->curr_len);
+		}
+		return MAX3421_HXFR_BULK_OUT(epnum);
+	}
+
+	max_packet = usb_maxpacket(urb->dev, urb->pipe, 1);
+
+	if (max_packet > MAX3421_FIFO_SIZE) {
+		/*
+		 * We do not support isochronous transfers at this
+		 * time.
+		 */
+		dev_err(&spi->dev,
+			"%s: packet-size of %u too big (limit is %u bytes)",
+			__func__, max_packet, MAX3421_FIFO_SIZE);
+		max3421_hcd->urb_done = -EMSGSIZE;
+		return -EMSGSIZE;
+	}
+	max3421_hcd->curr_len = min((urb->transfer_buffer_length -
+				     urb->actual_length), max_packet);
+
+	spi_wr_buf(hcd, MAX3421_REG_SNDFIFO, src, max3421_hcd->curr_len);
+	spi_wr8(hcd, MAX3421_REG_SNDBC, max3421_hcd->curr_len);
+	return MAX3421_HXFR_BULK_OUT(epnum);
+}
+
+/*
+ * Issue the next host-transfer command.
+ * Caller must NOT hold HCD spinlock.
+ */
+static void
+max3421_next_transfer(struct usb_hcd *hcd, int fast_retransmit)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	struct urb *urb = max3421_hcd->curr_urb;
+	struct max3421_ep *max3421_ep;
+	int cmd = -EINVAL;
+
+	if (!urb)
+		return;	/* nothing to do */
+
+	max3421_ep = urb->ep->hcpriv;
+
+	switch (max3421_ep->pkt_state) {
+	case PKT_STATE_SETUP:
+		cmd = max3421_ctrl_setup(hcd, urb);
+		break;
+
+	case PKT_STATE_TRANSFER:
+		if (usb_urb_dir_in(urb))
+			cmd = max3421_transfer_in(hcd, urb);
+		else
+			cmd = max3421_transfer_out(hcd, urb, fast_retransmit);
+		break;
+
+	case PKT_STATE_TERMINATE:
+		/*
+		 * IN transfers are terminated with HS_OUT token,
+		 * OUT transfers with HS_IN:
+		 */
+		if (usb_urb_dir_in(urb))
+			cmd = MAX3421_HXFR_HS_OUT;
+		else
+			cmd = MAX3421_HXFR_HS_IN;
+		break;
+	}
+
+	if (cmd < 0)
+		return;
+
+	/* issue the command and wait for host-xfer-done interrupt: */
+
+	spi_wr8(hcd, MAX3421_REG_HXFR, cmd);
+	max3421_hcd->hien |= BIT(MAX3421_HI_HXFRDN_BIT);
+}
+
+/*
+ * Find the next URB to process and start its execution.
+ *
+ * At this time, we do not anticipate ever connecting a USB hub to the
+ * MAX3421 chip, so at most USB device can be connected and we can use
+ * a simplistic scheduler: at the start of a frame, schedule all
+ * periodic transfers.  Once that is done, use the remainder of the
+ * frame to process non-periodic (bulk & control) transfers.
+ *
+ * Preconditions:
+ * o Caller must NOT hold HCD spinlock.
+ * o max3421_hcd->curr_urb MUST BE NULL.
+ * o MAX3421E chip must be idle.
+ */
+static int
+max3421_select_and_start_urb(struct usb_hcd *hcd)
+{
+	struct spi_device *spi = to_spi_device(hcd->self.controller);
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	struct urb *urb, *curr_urb = NULL;
+	struct max3421_ep *max3421_ep;
+	int epnum, force_toggles = 0;
+	struct usb_host_endpoint *ep;
+	struct list_head *pos;
+	unsigned long flags;
+
+	spin_lock_irqsave(&max3421_hcd->lock, flags);
+
+	for (;
+	     max3421_hcd->sched_pass < SCHED_PASS_DONE;
+	     ++max3421_hcd->sched_pass)
+		list_for_each(pos, &max3421_hcd->ep_list) {
+			urb = NULL;
+			max3421_ep = container_of(pos, struct max3421_ep,
+						  ep_list);
+			ep = max3421_ep->ep;
+
+			switch (usb_endpoint_type(&ep->desc)) {
+			case USB_ENDPOINT_XFER_ISOC:
+			case USB_ENDPOINT_XFER_INT:
+				if (max3421_hcd->sched_pass !=
+				    SCHED_PASS_PERIODIC)
+					continue;
+				break;
+
+			case USB_ENDPOINT_XFER_CONTROL:
+			case USB_ENDPOINT_XFER_BULK:
+				if (max3421_hcd->sched_pass !=
+				    SCHED_PASS_NON_PERIODIC)
+					continue;
+				break;
+			}
+
+			if (list_empty(&ep->urb_list))
+				continue;	/* nothing to do */
+			urb = list_first_entry(&ep->urb_list, struct urb,
+					       urb_list);
+			if (urb->unlinked) {
+				dev_dbg(&spi->dev, "%s: URB %p unlinked=%d",
+					__func__, urb, urb->unlinked);
+				max3421_hcd->curr_urb = urb;
+				max3421_hcd->urb_done = 1;
+				spin_unlock_irqrestore(&max3421_hcd->lock,
+						       flags);
+				return 1;
+			}
+
+			switch (usb_endpoint_type(&ep->desc)) {
+			case USB_ENDPOINT_XFER_CONTROL:
+				/*
+				 * Allow one control transaction per
+				 * frame per endpoint:
+				 */
+				if (frame_diff(max3421_ep->last_active,
+					       max3421_hcd->frame_number) == 0)
+					continue;
+				break;
+
+			case USB_ENDPOINT_XFER_BULK:
+				if (max3421_ep->retransmit
+				    && (frame_diff(max3421_ep->last_active,
+						   max3421_hcd->frame_number)
+					== 0))
+					/*
+					 * We already tried this EP
+					 * during this frame and got a
+					 * NAK or error; wait for next frame
+					 */
+					continue;
+				break;
+
+			case USB_ENDPOINT_XFER_ISOC:
+			case USB_ENDPOINT_XFER_INT:
+				if (frame_diff(max3421_hcd->frame_number,
+					       max3421_ep->last_active)
+				    < urb->interval)
+					/*
+					 * We already processed this
+					 * end-point in the current
+					 * frame
+					 */
+					continue;
+				break;
+			}
+
+			/* move current ep to tail: */
+			list_move_tail(pos, &max3421_hcd->ep_list);
+			curr_urb = urb;
+			goto done;
+		}
+done:
+	if (!curr_urb) {
+		spin_unlock_irqrestore(&max3421_hcd->lock, flags);
+		return 0;
+	}
+
+	urb = max3421_hcd->curr_urb = curr_urb;
+	epnum = usb_endpoint_num(&urb->ep->desc);
+	if (max3421_ep->retransmit)
+		/* restart (part of) a USB transaction: */
+		max3421_ep->retransmit = 0;
+	else {
+		/* start USB transaction: */
+		if (usb_endpoint_xfer_control(&ep->desc)) {
+			/*
+			 * See USB 2.0 spec section 8.6.1
+			 * Initialization via SETUP Token:
+			 */
+			usb_settoggle(urb->dev, epnum, 0, 1);
+			usb_settoggle(urb->dev, epnum, 1, 1);
+			max3421_ep->pkt_state = PKT_STATE_SETUP;
+			force_toggles = 1;
+		} else
+			max3421_ep->pkt_state = PKT_STATE_TRANSFER;
+	}
+
+	spin_unlock_irqrestore(&max3421_hcd->lock, flags);
+
+	max3421_ep->last_active = max3421_hcd->frame_number;
+	max3421_set_address(hcd, urb->dev, epnum, force_toggles);
+	max3421_set_speed(hcd, urb->dev);
+	max3421_next_transfer(hcd, 0);
+	return 1;
+}
+
+/*
+ * Check all endpoints for URBs that got unlinked.
+ *
+ * Caller must NOT hold HCD spinlock.
+ */
+static int
+max3421_check_unlink(struct usb_hcd *hcd)
+{
+	struct spi_device *spi = to_spi_device(hcd->self.controller);
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	struct list_head *pos, *upos, *next_upos;
+	struct max3421_ep *max3421_ep;
+	struct usb_host_endpoint *ep;
+	struct urb *urb;
+	unsigned long flags;
+	int retval = 0;
+
+	spin_lock_irqsave(&max3421_hcd->lock, flags);
+	list_for_each(pos, &max3421_hcd->ep_list) {
+		max3421_ep = container_of(pos, struct max3421_ep, ep_list);
+		ep = max3421_ep->ep;
+		list_for_each_safe(upos, next_upos, &ep->urb_list) {
+			urb = container_of(upos, struct urb, urb_list);
+			if (urb->unlinked) {
+				retval = 1;
+				dev_dbg(&spi->dev, "%s: URB %p unlinked=%d",
+					__func__, urb, urb->unlinked);
+				usb_hcd_unlink_urb_from_ep(hcd, urb);
+				spin_unlock_irqrestore(&max3421_hcd->lock,
+						       flags);
+				usb_hcd_giveback_urb(hcd, urb, 0);
+				spin_lock_irqsave(&max3421_hcd->lock, flags);
+			}
+		}
+	}
+	spin_unlock_irqrestore(&max3421_hcd->lock, flags);
+	return retval;
+}
+
+/*
+ * Caller must NOT hold HCD spinlock.
+ */
+static void
+max3421_slow_retransmit(struct usb_hcd *hcd)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	struct urb *urb = max3421_hcd->curr_urb;
+	struct max3421_ep *max3421_ep;
+
+	max3421_ep = urb->ep->hcpriv;
+	max3421_ep->retransmit = 1;
+	max3421_hcd->curr_urb = NULL;
+}
+
+/*
+ * Caller must NOT hold HCD spinlock.
+ */
+static void
+max3421_recv_data_available(struct usb_hcd *hcd)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	struct urb *urb = max3421_hcd->curr_urb;
+	size_t remaining, transfer_size;
+	u8 rcvbc;
+
+	rcvbc = spi_rd8(hcd, MAX3421_REG_RCVBC);
+
+	if (rcvbc > MAX3421_FIFO_SIZE)
+		rcvbc = MAX3421_FIFO_SIZE;
+	if (urb->actual_length >= urb->transfer_buffer_length)
+		remaining = 0;
+	else
+		remaining = urb->transfer_buffer_length - urb->actual_length;
+	transfer_size = rcvbc;
+	if (transfer_size > remaining)
+		transfer_size = remaining;
+	if (transfer_size > 0) {
+		void *dst = urb->transfer_buffer + urb->actual_length;
+
+		spi_rd_buf(hcd, MAX3421_REG_RCVFIFO, dst, transfer_size);
+		urb->actual_length += transfer_size;
+		max3421_hcd->curr_len = transfer_size;
+	}
+
+	/* ack the RCVDAV irq now that the FIFO has been read: */
+	spi_wr8(hcd, MAX3421_REG_HIRQ, BIT(MAX3421_HI_RCVDAV_BIT));
+}
+
+static void
+max3421_handle_error(struct usb_hcd *hcd, u8 hrsl)
+{
+	struct spi_device *spi = to_spi_device(hcd->self.controller);
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	u8 result_code = hrsl & MAX3421_HRSL_RESULT_MASK;
+	struct urb *urb = max3421_hcd->curr_urb;
+	struct max3421_ep *max3421_ep = urb->ep->hcpriv;
+	int switch_sndfifo;
+
+	/*
+	 * If an OUT command results in any response other than OK
+	 * (i.e., error or NAK), we have to perform a dummy-write to
+	 * SNDBC so the FIFO gets switched back to us.  Otherwise, we
+	 * get out of sync with the SNDFIFO double buffer.
+	 */
+	switch_sndfifo = (max3421_ep->pkt_state == PKT_STATE_TRANSFER &&
+			  usb_urb_dir_out(urb));
+
+	switch (result_code) {
+	case MAX3421_HRSL_OK:
+		return;			/* this shouldn't happen */
+
+	case MAX3421_HRSL_WRONGPID:	/* received wrong PID */
+	case MAX3421_HRSL_BUSY:		/* SIE busy */
+	case MAX3421_HRSL_BADREQ:	/* bad val in HXFR */
+	case MAX3421_HRSL_UNDEF:	/* reserved */
+	case MAX3421_HRSL_KERR:		/* K-state instead of response */
+	case MAX3421_HRSL_JERR:		/* J-state instead of response */
+		/*
+		 * packet experienced an error that we cannot recover
+		 * from; report error
+		 */
+		max3421_hcd->urb_done = hrsl_to_error[result_code];
+		dev_dbg(&spi->dev, "%s: unexpected error HRSL=0x%02x",
+			__func__, hrsl);
+		break;
+
+	case MAX3421_HRSL_TOGERR:
+		if (usb_urb_dir_in(urb))
+			; /* don't do anything (device will switch toggle) */
+		else {
+			/* flip the send toggle bit: */
+			int sndtog = (hrsl >> MAX3421_HRSL_SNDTOGRD_BIT) & 1;
+
+			sndtog ^= 1;
+			spi_wr8(hcd, MAX3421_REG_HCTL,
+				BIT(sndtog + MAX3421_HCTL_SNDTOG0_BIT));
+		}
+		/* FALL THROUGH */
+	case MAX3421_HRSL_BADBC:	/* bad byte count */
+	case MAX3421_HRSL_PIDERR:	/* received PID is corrupted */
+	case MAX3421_HRSL_PKTERR:	/* packet error (stuff, EOP) */
+	case MAX3421_HRSL_CRCERR:	/* CRC error */
+	case MAX3421_HRSL_BABBLE:	/* device talked too long */
+	case MAX3421_HRSL_TIMEOUT:
+		if (max3421_ep->retries++ < USB_MAX_RETRIES)
+			/* retry the packet again in the next frame */
+			max3421_slow_retransmit(hcd);
+		else {
+			/* Based on ohci.h cc_to_err[]: */
+			max3421_hcd->urb_done = hrsl_to_error[result_code];
+			dev_dbg(&spi->dev, "%s: unexpected error HRSL=0x%02x",
+				__func__, hrsl);
+		}
+		break;
+
+	case MAX3421_HRSL_STALL:
+		dev_dbg(&spi->dev, "%s: unexpected error HRSL=0x%02x",
+			__func__, hrsl);
+		max3421_hcd->urb_done = hrsl_to_error[result_code];
+		break;
+
+	case MAX3421_HRSL_NAK:
+		/*
+		 * Device wasn't ready for data or has no data
+		 * available: retry the packet again.
+		 */
+		if (max3421_ep->naks++ < NAK_MAX_FAST_RETRANSMITS) {
+			max3421_next_transfer(hcd, 1);
+			switch_sndfifo = 0;
+		} else
+			max3421_slow_retransmit(hcd);
+		break;
+	}
+	if (switch_sndfifo)
+		spi_wr8(hcd, MAX3421_REG_SNDBC, 0);
+}
+
+/*
+ * Caller must NOT hold HCD spinlock.
+ */
+static int
+max3421_transfer_in_done(struct usb_hcd *hcd, struct urb *urb)
+{
+	struct spi_device *spi = to_spi_device(hcd->self.controller);
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	u32 max_packet;
+
+	if (urb->actual_length >= urb->transfer_buffer_length)
+		return 1;	/* read is complete, so we're done */
+
+	/*
+	 * USB 2.0 Section 5.3.2 Pipes: packets must be full size
+	 * except for last one.
+	 */
+	max_packet = usb_maxpacket(urb->dev, urb->pipe, 0);
+	if (max_packet > MAX3421_FIFO_SIZE) {
+		/*
+		 * We do not support isochronous transfers at this
+		 * time...
+		 */
+		dev_err(&spi->dev,
+			"%s: packet-size of %u too big (limit is %u bytes)",
+			__func__, max_packet, MAX3421_FIFO_SIZE);
+		return -EINVAL;
+	}
+
+	if (max3421_hcd->curr_len < max_packet) {
+		if (urb->transfer_flags & URB_SHORT_NOT_OK) {
+			/*
+			 * remaining > 0 and received an
+			 * unexpected partial packet ->
+			 * error
+			 */
+			return -EREMOTEIO;
+		} else
+			/* short read, but it's OK */
+			return 1;
+	}
+	return 0;	/* not done */
+}
+
+/*
+ * Caller must NOT hold HCD spinlock.
+ */
+static int
+max3421_transfer_out_done(struct usb_hcd *hcd, struct urb *urb)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+
+	urb->actual_length += max3421_hcd->curr_len;
+	if (urb->actual_length < urb->transfer_buffer_length)
+		return 0;
+	if (urb->transfer_flags & URB_ZERO_PACKET) {
+		/*
+		 * Some hardware needs a zero-size packet at the end
+		 * of a bulk-out transfer if the last transfer was a
+		 * full-sized packet (i.e., such hardware use <
+		 * max_packet as an indicator that the end of the
+		 * packet has been reached).
+		 */
+		u32 max_packet = usb_maxpacket(urb->dev, urb->pipe, 1);
+
+		if (max3421_hcd->curr_len == max_packet)
+			return 0;
+	}
+	return 1;
+}
+
+/*
+ * Caller must NOT hold HCD spinlock.
+ */
+static void
+max3421_host_transfer_done(struct usb_hcd *hcd)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	struct urb *urb = max3421_hcd->curr_urb;
+	struct max3421_ep *max3421_ep;
+	u8 result_code, hrsl;
+	int urb_done = 0;
+
+	max3421_hcd->hien &= ~(BIT(MAX3421_HI_HXFRDN_BIT) |
+			       BIT(MAX3421_HI_RCVDAV_BIT));
+
+	hrsl = spi_rd8(hcd, MAX3421_REG_HRSL);
+	result_code = hrsl & MAX3421_HRSL_RESULT_MASK;
+
+#ifdef DEBUG
+	++max3421_hcd->err_stat[result_code];
+#endif
+
+	max3421_ep = urb->ep->hcpriv;
+
+	if (unlikely(result_code != MAX3421_HRSL_OK)) {
+		max3421_handle_error(hcd, hrsl);
+		return;
+	}
+
+	max3421_ep->naks = 0;
+	max3421_ep->retries = 0;
+	switch (max3421_ep->pkt_state) {
+
+	case PKT_STATE_SETUP:
+		if (urb->transfer_buffer_length > 0)
+			max3421_ep->pkt_state = PKT_STATE_TRANSFER;
+		else
+			max3421_ep->pkt_state = PKT_STATE_TERMINATE;
+		break;
+
+	case PKT_STATE_TRANSFER:
+		if (usb_urb_dir_in(urb))
+			urb_done = max3421_transfer_in_done(hcd, urb);
+		else
+			urb_done = max3421_transfer_out_done(hcd, urb);
+		if (urb_done > 0 && usb_pipetype(urb->pipe) == PIPE_CONTROL) {
+			/*
+			 * We aren't really done - we still need to
+			 * terminate the control transfer:
+			 */
+			max3421_hcd->urb_done = urb_done = 0;
+			max3421_ep->pkt_state = PKT_STATE_TERMINATE;
+		}
+		break;
+
+	case PKT_STATE_TERMINATE:
+		urb_done = 1;
+		break;
+	}
+
+	if (urb_done)
+		max3421_hcd->urb_done = urb_done;
+	else
+		max3421_next_transfer(hcd, 0);
+}
+
+/*
+ * Caller must NOT hold HCD spinlock.
+ */
+static void
+max3421_detect_conn(struct usb_hcd *hcd)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	unsigned int jk, have_conn = 0;
+	u32 old_port_status, chg;
+	unsigned long flags;
+	u8 hrsl, mode;
+
+	hrsl = spi_rd8(hcd, MAX3421_REG_HRSL);
+
+	jk = ((((hrsl >> MAX3421_HRSL_JSTATUS_BIT) & 1) << 0) |
+	      (((hrsl >> MAX3421_HRSL_KSTATUS_BIT) & 1) << 1));
+
+	mode = max3421_hcd->mode;
+
+	switch (jk) {
+	case 0x0: /* SE0: disconnect */
+		/*
+		 * Turn off SOFKAENAB bit to avoid getting interrupt
+		 * every milli-second:
+		 */
+		mode &= ~BIT(MAX3421_MODE_SOFKAENAB_BIT);
+		break;
+
+	case 0x1: /* J=0,K=1: low-speed (in full-speed or vice versa) */
+	case 0x2: /* J=1,K=0: full-speed (in full-speed or vice versa) */
+		if (jk == 0x2)
+			/* need to switch to the other speed: */
+			mode ^= BIT(MAX3421_MODE_LOWSPEED_BIT);
+		/* turn on SOFKAENAB bit: */
+		mode |= BIT(MAX3421_MODE_SOFKAENAB_BIT);
+		have_conn = 1;
+		break;
+
+	case 0x3: /* illegal */
+		break;
+	}
+
+	max3421_hcd->mode = mode;
+	spi_wr8(hcd, MAX3421_REG_MODE, max3421_hcd->mode);
+
+	spin_lock_irqsave(&max3421_hcd->lock, flags);
+	old_port_status = max3421_hcd->port_status;
+	if (have_conn)
+		max3421_hcd->port_status |=  USB_PORT_STAT_CONNECTION;
+	else
+		max3421_hcd->port_status &= ~USB_PORT_STAT_CONNECTION;
+	if (mode & BIT(MAX3421_MODE_LOWSPEED_BIT))
+		max3421_hcd->port_status |=  USB_PORT_STAT_LOW_SPEED;
+	else
+		max3421_hcd->port_status &= ~USB_PORT_STAT_LOW_SPEED;
+	chg = (old_port_status ^ max3421_hcd->port_status);
+	max3421_hcd->port_status |= chg << 16;
+	spin_unlock_irqrestore(&max3421_hcd->lock, flags);
+}
+
+static irqreturn_t
+max3421_irq_handler(int irq, void *dev_id)
+{
+	struct usb_hcd *hcd = dev_id;
+	struct spi_device *spi = to_spi_device(hcd->self.controller);
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+
+	if (max3421_hcd->spi_thread &&
+	    max3421_hcd->spi_thread->state != TASK_RUNNING)
+		wake_up_process(max3421_hcd->spi_thread);
+	if (!max3421_hcd->do_enable_irq) {
+		max3421_hcd->do_enable_irq = 1;
+		disable_irq_nosync(spi->irq);
+	}
+	return IRQ_HANDLED;
+}
+
+#ifdef DEBUG
+
+static void
+dump_eps(struct usb_hcd *hcd)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	struct max3421_ep *max3421_ep;
+	struct usb_host_endpoint *ep;
+	struct list_head *pos, *upos;
+	char ubuf[512], *dp, *end;
+	unsigned long flags;
+	struct urb *urb;
+	int epnum, ret;
+
+	spin_lock_irqsave(&max3421_hcd->lock, flags);
+	list_for_each(pos, &max3421_hcd->ep_list) {
+		max3421_ep = container_of(pos, struct max3421_ep, ep_list);
+		ep = max3421_ep->ep;
+
+		dp = ubuf;
+		end = dp + sizeof(ubuf);
+		*dp = '\0';
+		list_for_each(upos, &ep->urb_list) {
+			urb = container_of(upos, struct urb, urb_list);
+			ret = snprintf(dp, end - dp, " %p(%d.%s %d/%d)", urb,
+				       usb_pipetype(urb->pipe),
+				       usb_urb_dir_in(urb) ? "IN" : "OUT",
+				       urb->actual_length,
+				       urb->transfer_buffer_length);
+			if (ret < 0 || ret >= end - dp)
+				break;	/* error or buffer full */
+			dp += ret;
+		}
+
+		epnum = usb_endpoint_num(&ep->desc);
+		pr_info("EP%0u %u lst %04u rtr %u nak %6u rxmt %u: %s\n",
+			epnum, max3421_ep->pkt_state, max3421_ep->last_active,
+			max3421_ep->retries, max3421_ep->naks,
+			max3421_ep->retransmit, ubuf);
+	}
+	spin_unlock_irqrestore(&max3421_hcd->lock, flags);
+}
+
+#endif /* DEBUG */
+
+/* Return zero if no work was performed, 1 otherwise.  */
+static int
+max3421_handle_irqs(struct usb_hcd *hcd)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	u32 chg, old_port_status;
+	unsigned long flags;
+	u8 hirq;
+
+	/*
+	 * Read and ack pending interrupts (CPU must never
+	 * clear SNDBAV directly and RCVDAV must be cleared by
+	 * max3421_recv_data_available()!):
+	 */
+	hirq = spi_rd8(hcd, MAX3421_REG_HIRQ);
+	hirq &= max3421_hcd->hien;
+	if (!hirq)
+		return 0;
+
+	spi_wr8(hcd, MAX3421_REG_HIRQ,
+		hirq & ~(BIT(MAX3421_HI_SNDBAV_BIT) |
+			 BIT(MAX3421_HI_RCVDAV_BIT)));
+
+	if (hirq & BIT(MAX3421_HI_FRAME_BIT)) {
+		max3421_hcd->frame_number = ((max3421_hcd->frame_number + 1)
+					     & USB_MAX_FRAME_NUMBER);
+		max3421_hcd->sched_pass = SCHED_PASS_PERIODIC;
+	}
+
+	if (hirq & BIT(MAX3421_HI_RCVDAV_BIT))
+		max3421_recv_data_available(hcd);
+
+	if (hirq & BIT(MAX3421_HI_HXFRDN_BIT))
+		max3421_host_transfer_done(hcd);
+
+	if (hirq & BIT(MAX3421_HI_CONDET_BIT))
+		max3421_detect_conn(hcd);
+
+	/*
+	 * Now process interrupts that may affect HCD state
+	 * other than the end-points:
+	 */
+	spin_lock_irqsave(&max3421_hcd->lock, flags);
+
+	old_port_status = max3421_hcd->port_status;
+	if (hirq & BIT(MAX3421_HI_BUSEVENT_BIT)) {
+		if (max3421_hcd->port_status & USB_PORT_STAT_RESET) {
+			/* BUSEVENT due to completion of Bus Reset */
+			max3421_hcd->port_status &= ~USB_PORT_STAT_RESET;
+			max3421_hcd->port_status |=  USB_PORT_STAT_ENABLE;
+		} else {
+			/* BUSEVENT due to completion of Bus Resume */
+			pr_info("%s: BUSEVENT Bus Resume Done\n", __func__);
+		}
+	}
+	if (hirq & BIT(MAX3421_HI_RWU_BIT))
+		pr_info("%s: RWU\n", __func__);
+	if (hirq & BIT(MAX3421_HI_SUSDN_BIT))
+		pr_info("%s: SUSDN\n", __func__);
+
+	chg = (old_port_status ^ max3421_hcd->port_status);
+	max3421_hcd->port_status |= chg << 16;
+
+	spin_unlock_irqrestore(&max3421_hcd->lock, flags);
+
+#ifdef DEBUG
+	{
+		static unsigned long last_time;
+		char sbuf[16 * 16], *dp, *end;
+		int i;
+
+		if (jiffies - last_time > 5*HZ) {
+			dp = sbuf;
+			end = sbuf + sizeof(sbuf);
+			*dp = '\0';
+			for (i = 0; i < 16; ++i) {
+				int ret = snprintf(dp, end - dp, " %lu",
+						   max3421_hcd->err_stat[i]);
+				if (ret < 0 || ret >= end - dp)
+					break;	/* error or buffer full */
+				dp += ret;
+			}
+			pr_info("%s: hrsl_stats %s\n", __func__, sbuf);
+			memset(max3421_hcd->err_stat, 0,
+			       sizeof(max3421_hcd->err_stat));
+			last_time = jiffies;
+
+			dump_eps(hcd);
+		}
+	}
+#endif
+	return 1;
+}
+
+static int
+max3421_reset_hcd(struct usb_hcd *hcd)
+{
+	struct spi_device *spi = to_spi_device(hcd->self.controller);
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	int timeout;
+
+	/* perform a chip reset and wait for OSCIRQ signal to appear: */
+	spi_wr8(hcd, MAX3421_REG_USBCTL, BIT(MAX3421_USBCTL_CHIPRES_BIT));
+	/* clear reset: */
+	spi_wr8(hcd, MAX3421_REG_USBCTL, 0);
+	timeout = 1000;
+	while (1) {
+		if (spi_rd8(hcd, MAX3421_REG_USBIRQ)
+		    & BIT(MAX3421_USBIRQ_OSCOKIRQ_BIT))
+			break;
+		if (--timeout < 0) {
+			dev_err(&spi->dev,
+				"timed out waiting for oscillator OK signal");
+			return 1;
+		}
+		cond_resched();
+	}
+
+	/*
+	 * Turn on host mode, automatic generation of SOF packets, and
+	 * enable pull-down registers on DM/DP:
+	 */
+	max3421_hcd->mode = (BIT(MAX3421_MODE_HOST_BIT) |
+			     BIT(MAX3421_MODE_SOFKAENAB_BIT) |
+			     BIT(MAX3421_MODE_DMPULLDN_BIT) |
+			     BIT(MAX3421_MODE_DPPULLDN_BIT));
+	spi_wr8(hcd, MAX3421_REG_MODE, max3421_hcd->mode);
+
+	/* reset frame-number: */
+	max3421_hcd->frame_number = USB_MAX_FRAME_NUMBER;
+	spi_wr8(hcd, MAX3421_REG_HCTL, BIT(MAX3421_HCTL_FRMRST_BIT));
+
+	/* sample the state of the D+ and D- lines */
+	spi_wr8(hcd, MAX3421_REG_HCTL, BIT(MAX3421_HCTL_SAMPLEBUS_BIT));
+	max3421_detect_conn(hcd);
+
+	/* enable frame, connection-detected, and bus-event interrupts: */
+	max3421_hcd->hien = (BIT(MAX3421_HI_FRAME_BIT) |
+			     BIT(MAX3421_HI_CONDET_BIT) |
+			     BIT(MAX3421_HI_BUSEVENT_BIT));
+	spi_wr8(hcd, MAX3421_REG_HIEN, max3421_hcd->hien);
+
+	/* enable interrupts: */
+	spi_wr8(hcd, MAX3421_REG_CPUCTL, BIT(MAX3421_CPUCTL_IE_BIT));
+	return 1;
+}
+
+static int
+max3421_urb_done(struct usb_hcd *hcd)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	unsigned long flags;
+	struct urb *urb;
+	int status;
+
+	status = max3421_hcd->urb_done;
+	max3421_hcd->urb_done = 0;
+	if (status > 0)
+		status = 0;
+	urb = max3421_hcd->curr_urb;
+	if (urb) {
+		max3421_hcd->curr_urb = NULL;
+		spin_lock_irqsave(&max3421_hcd->lock, flags);
+		usb_hcd_unlink_urb_from_ep(hcd, urb);
+		spin_unlock_irqrestore(&max3421_hcd->lock, flags);
+
+		/* must be called without the HCD spinlock: */
+		usb_hcd_giveback_urb(hcd, urb, status);
+	}
+	return 1;
+}
+
+static int
+max3421_spi_thread(void *dev_id)
+{
+	struct usb_hcd *hcd = dev_id;
+	struct spi_device *spi = to_spi_device(hcd->self.controller);
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	int i, i_worked = 1;
+
+	/* set full-duplex SPI mode, low-active interrupt pin: */
+	spi_wr8(hcd, MAX3421_REG_PINCTL,
+		(BIT(MAX3421_PINCTL_FDUPSPI_BIT) |	/* full-duplex */
+		 BIT(MAX3421_PINCTL_INTLEVEL_BIT)));	/* low-active irq */
+
+	while (!kthread_should_stop()) {
+		max3421_hcd->rev = spi_rd8(hcd, MAX3421_REG_REVISION);
+		if (max3421_hcd->rev == 0x12 || max3421_hcd->rev == 0x13)
+			break;
+		dev_err(&spi->dev, "bad rev 0x%02x", max3421_hcd->rev);
+		msleep(10000);
+	}
+	dev_info(&spi->dev, "rev 0x%x, SPI clk %dHz, bpw %u, irq %d\n",
+		 max3421_hcd->rev, spi->max_speed_hz, spi->bits_per_word,
+		 spi->irq);
+
+	while (!kthread_should_stop()) {
+		if (!i_worked) {
+			/*
+			 * We'll be waiting for wakeups from the hard
+			 * interrupt handler, so now is a good time to
+			 * sync our hien with the chip:
+			 */
+			spi_wr8(hcd, MAX3421_REG_HIEN, max3421_hcd->hien);
+
+			set_current_state(TASK_INTERRUPTIBLE);
+			if (max3421_hcd->do_enable_irq) {
+				max3421_hcd->do_enable_irq = 0;
+				enable_irq(spi->irq);
+			}
+			schedule();
+			__set_current_state(TASK_RUNNING);
+		}
+
+		i_worked = 0;
+
+		if (max3421_hcd->urb_done)
+			i_worked |= max3421_urb_done(hcd);
+		else if (max3421_handle_irqs(hcd))
+			i_worked = 1;
+		else if (!max3421_hcd->curr_urb)
+			i_worked |= max3421_select_and_start_urb(hcd);
+
+		if (max3421_hcd->do_reset_hcd) {
+			/* reset the HCD: */
+			max3421_hcd->do_reset_hcd = 0;
+			i_worked |= max3421_reset_hcd(hcd);
+		}
+		if (max3421_hcd->do_reset_port) {
+			/* perform a USB bus reset: */
+			max3421_hcd->do_reset_port = 0;
+			spi_wr8(hcd, MAX3421_REG_HCTL,
+				BIT(MAX3421_HCTL_BUSRST_BIT));
+			i_worked = 1;
+		}
+		if (max3421_hcd->do_check_unlink) {
+			max3421_hcd->do_check_unlink = 0;
+			i_worked |= max3421_check_unlink(hcd);
+		}
+		if (max3421_hcd->do_iopin_update) {
+			/*
+			 * IOPINS1/IOPINS2 do not auto-increment, so we can't
+			 * use spi_wr_buf().
+			 */
+			for (i = 0; i < ARRAY_SIZE(max3421_hcd->iopins); ++i) {
+				u8 val = spi_rd8(hcd, MAX3421_REG_IOPINS1);
+
+				val = ((val & 0xf0) |
+				       (max3421_hcd->iopins[i] & 0x0f));
+				spi_wr8(hcd, MAX3421_REG_IOPINS1 + i, val);
+				max3421_hcd->iopins[i] = val;
+			}
+			max3421_hcd->do_iopin_update = 0;
+			i_worked = 1;
+		}
+	}
+	set_current_state(TASK_RUNNING);
+	dev_info(&spi->dev, "SPI thread exiting");
+	return 0;
+}
+
+static int
+max3421_reset_port(struct usb_hcd *hcd)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+
+	max3421_hcd->port_status &= ~(USB_PORT_STAT_ENABLE |
+				      USB_PORT_STAT_LOW_SPEED);
+	max3421_hcd->do_reset_port = 1;
+	wake_up_process(max3421_hcd->spi_thread);
+	return 0;
+}
+
+static int
+max3421_reset(struct usb_hcd *hcd)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+
+	hcd->self.sg_tablesize = 0;
+	hcd->speed = HCD_USB2;
+	hcd->self.root_hub->speed = USB_SPEED_FULL;
+	max3421_hcd->do_reset_hcd = 1;
+	wake_up_process(max3421_hcd->spi_thread);
+	return 0;
+}
+
+static int
+max3421_start(struct usb_hcd *hcd)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+
+	spin_lock_init(&max3421_hcd->lock);
+	max3421_hcd->rh_state = MAX3421_RH_RUNNING;
+
+	INIT_LIST_HEAD(&max3421_hcd->ep_list);
+
+	hcd->power_budget = POWER_BUDGET;
+	hcd->state = HC_STATE_RUNNING;
+	hcd->uses_new_polling = 1;
+	return 0;
+}
+
+static void
+max3421_stop(struct usb_hcd *hcd)
+{
+}
+
+static int
+max3421_urb_enqueue(struct usb_hcd *hcd, struct urb *urb, gfp_t mem_flags)
+{
+	struct spi_device *spi = to_spi_device(hcd->self.controller);
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	struct max3421_ep *max3421_ep;
+	unsigned long flags;
+	int retval;
+
+	switch (usb_pipetype(urb->pipe)) {
+	case PIPE_INTERRUPT:
+	case PIPE_ISOCHRONOUS:
+		if (urb->interval < 0) {
+			dev_err(&spi->dev,
+			  "%s: interval=%d for intr-/iso-pipe; expected > 0\n",
+				__func__, urb->interval);
+			return -EINVAL;
+		}
+	default:
+		break;
+	}
+
+	spin_lock_irqsave(&max3421_hcd->lock, flags);
+
+	max3421_ep = urb->ep->hcpriv;
+	if (!max3421_ep) {
+		/* gets freed in max3421_endpoint_disable: */
+		max3421_ep = kzalloc(sizeof(struct max3421_ep), mem_flags);
+		if (!max3421_ep) {
+			retval = -ENOMEM;
+			goto out;
+		}
+		max3421_ep->ep = urb->ep;
+		max3421_ep->last_active = max3421_hcd->frame_number;
+		urb->ep->hcpriv = max3421_ep;
+
+		list_add_tail(&max3421_ep->ep_list, &max3421_hcd->ep_list);
+	}
+
+	retval = usb_hcd_link_urb_to_ep(hcd, urb);
+	if (retval == 0) {
+		/* Since we added to the queue, restart scheduling: */
+		max3421_hcd->sched_pass = SCHED_PASS_PERIODIC;
+		wake_up_process(max3421_hcd->spi_thread);
+	}
+
+out:
+	spin_unlock_irqrestore(&max3421_hcd->lock, flags);
+	return retval;
+}
+
+static int
+max3421_urb_dequeue(struct usb_hcd *hcd, struct urb *urb, int status)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	unsigned long flags;
+	int retval;
+
+	spin_lock_irqsave(&max3421_hcd->lock, flags);
+
+	/*
+	 * This will set urb->unlinked which in turn causes the entry
+	 * to be dropped at the next opportunity.
+	 */
+	retval = usb_hcd_check_unlink_urb(hcd, urb, status);
+	if (retval == 0) {
+		max3421_hcd->do_check_unlink = 1;
+		wake_up_process(max3421_hcd->spi_thread);
+	}
+	spin_unlock_irqrestore(&max3421_hcd->lock, flags);
+	return retval;
+}
+
+static void
+max3421_endpoint_disable(struct usb_hcd *hcd, struct usb_host_endpoint *ep)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	unsigned long flags;
+
+	spin_lock_irqsave(&max3421_hcd->lock, flags);
+
+	if (ep->hcpriv) {
+		struct max3421_ep *max3421_ep = ep->hcpriv;
+
+		/* remove myself from the ep_list: */
+		if (!list_empty(&max3421_ep->ep_list))
+			list_del(&max3421_ep->ep_list);
+		kfree(max3421_ep);
+		ep->hcpriv = NULL;
+	}
+
+	spin_unlock_irqrestore(&max3421_hcd->lock, flags);
+}
+
+static int
+max3421_get_frame_number(struct usb_hcd *hcd)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	return max3421_hcd->frame_number;
+}
+
+/*
+ * Should return a non-zero value when any port is undergoing a resume
+ * transition while the root hub is suspended.
+ */
+static int
+max3421_hub_status_data(struct usb_hcd *hcd, char *buf)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	unsigned long flags;
+	int retval = 0;
+
+	spin_lock_irqsave(&max3421_hcd->lock, flags);
+	if (!HCD_HW_ACCESSIBLE(hcd))
+		goto done;
+
+	*buf = 0;
+	if ((max3421_hcd->port_status & PORT_C_MASK) != 0) {
+		*buf = (1 << 1); /* a hub over-current condition exists */
+		dev_dbg(hcd->self.controller,
+			"port status 0x%08x has changes\n",
+			max3421_hcd->port_status);
+		retval = 1;
+		if (max3421_hcd->rh_state == MAX3421_RH_SUSPENDED)
+			usb_hcd_resume_root_hub(hcd);
+	}
+done:
+	spin_unlock_irqrestore(&max3421_hcd->lock, flags);
+	return retval;
+}
+
+static inline void
+hub_descriptor(struct usb_hub_descriptor *desc)
+{
+	memset(desc, 0, sizeof(*desc));
+	/*
+	 * See Table 11-13: Hub Descriptor in USB 2.0 spec.
+	 */
+	desc->bDescriptorType = 0x29;	/* hub descriptor */
+	desc->bDescLength = 9;
+	desc->wHubCharacteristics = cpu_to_le16(0x0001);
+	desc->bNbrPorts = 1;
+}
+
+/*
+ * Set the MAX3421E general-purpose output with number PIN_NUMBER to
+ * VALUE (0 or 1).  PIN_NUMBER may be in the range from 1-8.  For
+ * any other value, this function acts as a no-op.
+ */
+static void
+max3421_gpout_set_value(struct usb_hcd *hcd, u8 pin_number, u8 value)
+{
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	u8 mask, idx;
+
+	--pin_number;
+	if (pin_number > 7)
+		return;
+
+	mask = 1u << pin_number;
+	idx = pin_number / 4;
+
+	if (value)
+		max3421_hcd->iopins[idx] |=  mask;
+	else
+		max3421_hcd->iopins[idx] &= ~mask;
+	max3421_hcd->do_iopin_update = 1;
+	wake_up_process(max3421_hcd->spi_thread);
+}
+
+static int
+max3421_hub_control(struct usb_hcd *hcd, u16 type_req, u16 value, u16 index,
+		    char *buf, u16 length)
+{
+	struct spi_device *spi = to_spi_device(hcd->self.controller);
+	struct max3421_hcd *max3421_hcd = hcd_to_max3421(hcd);
+	struct max3421_hcd_platform_data *pdata;
+	unsigned long flags;
+	int retval = 0;
+
+	spin_lock_irqsave(&max3421_hcd->lock, flags);
+
+	pdata = spi->dev.platform_data;
+
+	switch (type_req) {
+	case ClearHubFeature:
+		break;
+	case ClearPortFeature:
+		switch (value) {
+		case USB_PORT_FEAT_SUSPEND:
+			break;
+		case USB_PORT_FEAT_POWER:
+			dev_dbg(hcd->self.controller, "power-off\n");
+			max3421_gpout_set_value(hcd, pdata->vbus_gpout,
+						!pdata->vbus_active_level);
+			/* FALLS THROUGH */
+		default:
+			max3421_hcd->port_status &= ~(1 << value);
+		}
+		break;
+	case GetHubDescriptor:
+		hub_descriptor((struct usb_hub_descriptor *) buf);
+		break;
+
+	case DeviceRequest | USB_REQ_GET_DESCRIPTOR:
+	case GetPortErrorCount:
+	case SetHubDepth:
+		/* USB3 only */
+		goto error;
+
+	case GetHubStatus:
+		*(__le32 *) buf = cpu_to_le32(0);
+		break;
+
+	case GetPortStatus:
+		if (index != 1) {
+			retval = -EPIPE;
+			goto error;
+		}
+		((__le16 *) buf)[0] = cpu_to_le16(max3421_hcd->port_status);
+		((__le16 *) buf)[1] =
+			cpu_to_le16(max3421_hcd->port_status >> 16);
+		break;
+
+	case SetHubFeature:
+		retval = -EPIPE;
+		break;
+
+	case SetPortFeature:
+		switch (value) {
+		case USB_PORT_FEAT_LINK_STATE:
+		case USB_PORT_FEAT_U1_TIMEOUT:
+		case USB_PORT_FEAT_U2_TIMEOUT:
+		case USB_PORT_FEAT_BH_PORT_RESET:
+			goto error;
+		case USB_PORT_FEAT_SUSPEND:
+			if (max3421_hcd->active)
+				max3421_hcd->port_status |=
+					USB_PORT_STAT_SUSPEND;
+			break;
+		case USB_PORT_FEAT_POWER:
+			dev_dbg(hcd->self.controller, "power-on\n");
+			max3421_hcd->port_status |= USB_PORT_STAT_POWER;
+			max3421_gpout_set_value(hcd, pdata->vbus_gpout,
+						pdata->vbus_active_level);
+			break;
+		case USB_PORT_FEAT_RESET:
+			max3421_reset_port(hcd);
+			/* FALLS THROUGH */
+		default:
+			if ((max3421_hcd->port_status & USB_PORT_STAT_POWER)
+			    != 0)
+				max3421_hcd->port_status |= (1 << value);
+		}
+		break;
+
+	default:
+		dev_dbg(hcd->self.controller,
+			"hub control req%04x v%04x i%04x l%d\n",
+			type_req, value, index, length);
+error:		/* "protocol stall" on error */
+		retval = -EPIPE;
+	}
+
+	spin_unlock_irqrestore(&max3421_hcd->lock, flags);
+	return retval;
+}
+
+static int
+max3421_bus_suspend(struct usb_hcd *hcd)
+{
+	return -1;
+}
+
+static int
+max3421_bus_resume(struct usb_hcd *hcd)
+{
+	return -1;
+}
+
+/*
+ * The SPI driver already takes care of DMA-mapping/unmapping, so no
+ * reason to do it twice.
+ */
+static int
+max3421_map_urb_for_dma(struct usb_hcd *hcd, struct urb *urb, gfp_t mem_flags)
+{
+	return 0;
+}
+
+static void
+max3421_unmap_urb_for_dma(struct usb_hcd *hcd, struct urb *urb)
+{
+}
+
+static struct hc_driver max3421_hcd_desc = {
+	.description =		"max3421",
+	.product_desc =		DRIVER_DESC,
+	.hcd_priv_size =	sizeof(struct max3421_hcd),
+	.flags =		HCD_USB11,
+	.reset =		max3421_reset,
+	.start =		max3421_start,
+	.stop =			max3421_stop,
+	.get_frame_number =	max3421_get_frame_number,
+	.urb_enqueue =		max3421_urb_enqueue,
+	.urb_dequeue =		max3421_urb_dequeue,
+	.map_urb_for_dma =	max3421_map_urb_for_dma,
+	.unmap_urb_for_dma =	max3421_unmap_urb_for_dma,
+	.endpoint_disable =	max3421_endpoint_disable,
+	.hub_status_data =	max3421_hub_status_data,
+	.hub_control =		max3421_hub_control,
+	.bus_suspend =		max3421_bus_suspend,
+	.bus_resume =		max3421_bus_resume,
+};
+
+static int
+max3421_probe(struct spi_device *spi)
+{
+	struct max3421_hcd *max3421_hcd;
+	struct usb_hcd *hcd = NULL;
+	int retval = -ENOMEM;
+
+	if (spi_setup(spi) < 0) {
+		dev_err(&spi->dev, "Unable to setup SPI bus");
+		return -EFAULT;
+	}
+
+	hcd = usb_create_hcd(&max3421_hcd_desc, &spi->dev,
+			     dev_name(&spi->dev));
+	if (!hcd) {
+		dev_err(&spi->dev, "failed to create HCD structure\n");
+		goto error;
+	}
+	set_bit(HCD_FLAG_POLL_RH, &hcd->flags);
+	max3421_hcd = hcd_to_max3421(hcd);
+	max3421_hcd->next = max3421_hcd_list;
+	max3421_hcd_list = max3421_hcd;
+	INIT_LIST_HEAD(&max3421_hcd->ep_list);
+
+	max3421_hcd->tx = kmalloc(sizeof(*max3421_hcd->tx), GFP_KERNEL);
+	if (!max3421_hcd->tx) {
+		dev_err(&spi->dev, "failed to kmalloc tx buffer\n");
+		goto error;
+	}
+	max3421_hcd->rx = kmalloc(sizeof(*max3421_hcd->rx), GFP_KERNEL);
+	if (!max3421_hcd->rx) {
+		dev_err(&spi->dev, "failed to kmalloc rx buffer\n");
+		goto error;
+	}
+
+	max3421_hcd->spi_thread = kthread_run(max3421_spi_thread, hcd,
+					      "max3421_spi_thread");
+	if (max3421_hcd->spi_thread == ERR_PTR(-ENOMEM)) {
+		dev_err(&spi->dev,
+			"failed to create SPI thread (out of memory)\n");
+		goto error;
+	}
+
+	retval = usb_add_hcd(hcd, 0, 0);
+	if (retval) {
+		dev_err(&spi->dev, "failed to add HCD\n");
+		goto error;
+	}
+
+	retval = request_irq(spi->irq, max3421_irq_handler,
+			     IRQF_TRIGGER_LOW, "max3421", hcd);
+	if (retval < 0) {
+		dev_err(&spi->dev, "failed to request irq %d\n", spi->irq);
+		goto error;
+	}
+	return 0;
+
+error:
+	if (hcd) {
+		kfree(max3421_hcd->tx);
+		kfree(max3421_hcd->rx);
+		if (max3421_hcd->spi_thread)
+			kthread_stop(max3421_hcd->spi_thread);
+		usb_put_hcd(hcd);
+	}
+	return retval;
+}
+
+static int
+max3421_remove(struct spi_device *spi)
+{
+	struct max3421_hcd *max3421_hcd = NULL, **prev;
+	struct usb_hcd *hcd = NULL;
+	unsigned long flags;
+
+	for (prev = &max3421_hcd_list; *prev; prev = &(*prev)->next) {
+		max3421_hcd = *prev;
+		hcd = max3421_to_hcd(max3421_hcd);
+		if (hcd->self.controller == &spi->dev)
+			break;
+	}
+	if (!max3421_hcd) {
+		dev_err(&spi->dev, "no MAX3421 HCD found for SPI device %p\n",
+			spi);
+		return -ENODEV;
+	}
+
+	usb_remove_hcd(hcd);
+
+	spin_lock_irqsave(&max3421_hcd->lock, flags);
+
+	kthread_stop(max3421_hcd->spi_thread);
+	*prev = max3421_hcd->next;
+
+	spin_unlock_irqrestore(&max3421_hcd->lock, flags);
+
+	free_irq(spi->irq, hcd);
+
+	usb_put_hcd(hcd);
+	return 0;
+}
+
+static struct spi_driver max3421_driver = {
+	.probe		= max3421_probe,
+	.remove		= max3421_remove,
+	.driver		= {
+		.name	= "max3421-hcd",
+		.owner	= THIS_MODULE,
+	},
+};
+
+module_spi_driver(max3421_driver);
+
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_AUTHOR("David Mosberger <davidm@egauge.net>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/usb/host/ohci-at91.c b/drivers/usb/host/ohci-at91.c
index 091ae4905cf..e49eb4f90f5 100644
--- a/drivers/usb/host/ohci-at91.c
+++ b/drivers/usb/host/ohci-at91.c
@@ -46,9 +46,6 @@ static const char hcd_name[] = "ohci-atmel";
 
 static struct hc_driver __read_mostly ohci_at91_hc_driver;
 static int clocked;
-static int (*orig_ohci_hub_control)(struct usb_hcd  *hcd, u16 typeReq,
-			u16 wValue, u16 wIndex, char *buf, u16 wLength);
-static int (*orig_ohci_hub_status_data)(struct usb_hcd *hcd, char *buf);
 
 extern int usb_disabled(void);
 
@@ -262,7 +259,7 @@ static int ohci_at91_usb_get_power(struct at91_usbh_data *pdata, int port)
 static int ohci_at91_hub_status_data(struct usb_hcd *hcd, char *buf)
 {
 	struct at91_usbh_data *pdata = hcd->self.controller->platform_data;
-	int length = orig_ohci_hub_status_data(hcd, buf);
+	int length = ohci_hub_status_data(hcd, buf);
 	int port;
 
 	at91_for_each_port(port) {
@@ -340,8 +337,7 @@ static int ohci_at91_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 		break;
 	}
 
-	ret = orig_ohci_hub_control(hcd, typeReq, wValue, wIndex + 1,
-				buf, wLength);
+	ret = ohci_hub_control(hcd, typeReq, wValue, wIndex + 1, buf, wLength);
 	if (ret)
 		goto out;
 
@@ -690,9 +686,6 @@ static int __init ohci_at91_init(void)
 	 * too easy.
 	 */
 
-	orig_ohci_hub_control = ohci_at91_hc_driver.hub_control;
-	orig_ohci_hub_status_data = ohci_at91_hc_driver.hub_status_data;
-
 	ohci_at91_hc_driver.hub_status_data	= ohci_at91_hub_status_data;
 	ohci_at91_hc_driver.hub_control		= ohci_at91_hub_control;
 
diff --git a/drivers/usb/host/ohci-exynos.c b/drivers/usb/host/ohci-exynos.c
index 68588d8a09b..060a6a41475 100644
--- a/drivers/usb/host/ohci-exynos.c
+++ b/drivers/usb/host/ohci-exynos.c
@@ -18,6 +18,7 @@
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
+#include <linux/phy/phy.h>
 #include <linux/usb/phy.h>
 #include <linux/usb/samsung_usb_phy.h>
 #include <linux/usb.h>
@@ -33,28 +34,110 @@ static struct hc_driver __read_mostly exynos_ohci_hc_driver;
 
 #define to_exynos_ohci(hcd) (struct exynos_ohci_hcd *)(hcd_to_ohci(hcd)->priv)
 
+#define PHY_NUMBER 3
+
 struct exynos_ohci_hcd {
 	struct clk *clk;
 	struct usb_phy *phy;
 	struct usb_otg *otg;
+	struct phy *phy_g[PHY_NUMBER];
 };
 
-static void exynos_ohci_phy_enable(struct platform_device *pdev)
+static int exynos_ohci_get_phy(struct device *dev,
+				struct exynos_ohci_hcd *exynos_ohci)
 {
-	struct usb_hcd *hcd = platform_get_drvdata(pdev);
+	struct device_node *child;
+	struct phy *phy;
+	int phy_number;
+	int ret = 0;
+
+	exynos_ohci->phy = devm_usb_get_phy(dev, USB_PHY_TYPE_USB2);
+	if (IS_ERR(exynos_ohci->phy)) {
+		ret = PTR_ERR(exynos_ohci->phy);
+		if (ret != -ENXIO && ret != -ENODEV) {
+			dev_err(dev, "no usb2 phy configured\n");
+			return ret;
+		}
+		dev_dbg(dev, "Failed to get usb2 phy\n");
+	} else {
+		exynos_ohci->otg = exynos_ohci->phy->otg;
+	}
+
+	/*
+	 * Getting generic phy:
+	 * We are keeping both types of phys as a part of transiting OHCI
+	 * to generic phy framework, so as to maintain backward compatibilty
+	 * with old DTB.
+	 * If there are existing devices using DTB files built from them,
+	 * to remove the support for old bindings in this driver,
+	 * we need to make sure that such devices have their DTBs
+	 * updated to ones built from new DTS.
+	 */
+	for_each_available_child_of_node(dev->of_node, child) {
+		ret = of_property_read_u32(child, "reg", &phy_number);
+		if (ret) {
+			dev_err(dev, "Failed to parse device tree\n");
+			of_node_put(child);
+			return ret;
+		}
+
+		if (phy_number >= PHY_NUMBER) {
+			dev_err(dev, "Invalid number of PHYs\n");
+			of_node_put(child);
+			return -EINVAL;
+		}
+
+		phy = devm_of_phy_get(dev, child, 0);
+		of_node_put(child);
+		if (IS_ERR(phy)) {
+			ret = PTR_ERR(phy);
+			if (ret != -ENOSYS && ret != -ENODEV) {
+				dev_err(dev, "no usb2 phy configured\n");
+				return ret;
+			}
+			dev_dbg(dev, "Failed to get usb2 phy\n");
+		}
+		exynos_ohci->phy_g[phy_number] = phy;
+	}
+
+	return ret;
+}
+
+static int exynos_ohci_phy_enable(struct device *dev)
+{
+	struct usb_hcd *hcd = dev_get_drvdata(dev);
 	struct exynos_ohci_hcd *exynos_ohci = to_exynos_ohci(hcd);
+	int i;
+	int ret = 0;
+
+	if (!IS_ERR(exynos_ohci->phy))
+		return usb_phy_init(exynos_ohci->phy);
+
+	for (i = 0; ret == 0 && i < PHY_NUMBER; i++)
+		if (!IS_ERR(exynos_ohci->phy_g[i]))
+			ret = phy_power_on(exynos_ohci->phy_g[i]);
+	if (ret)
+		for (i--; i >= 0; i--)
+			if (!IS_ERR(exynos_ohci->phy_g[i]))
+				phy_power_off(exynos_ohci->phy_g[i]);
 
-	if (exynos_ohci->phy)
-		usb_phy_init(exynos_ohci->phy);
+	return ret;
 }
 
-static void exynos_ohci_phy_disable(struct platform_device *pdev)
+static void exynos_ohci_phy_disable(struct device *dev)
 {
-	struct usb_hcd *hcd = platform_get_drvdata(pdev);
+	struct usb_hcd *hcd = dev_get_drvdata(dev);
 	struct exynos_ohci_hcd *exynos_ohci = to_exynos_ohci(hcd);
+	int i;
 
-	if (exynos_ohci->phy)
+	if (!IS_ERR(exynos_ohci->phy)) {
 		usb_phy_shutdown(exynos_ohci->phy);
+		return;
+	}
+
+	for (i = 0; i < PHY_NUMBER; i++)
+		if (!IS_ERR(exynos_ohci->phy_g[i]))
+			phy_power_off(exynos_ohci->phy_g[i]);
 }
 
 static int exynos_ohci_probe(struct platform_device *pdev)
@@ -62,7 +145,6 @@ static int exynos_ohci_probe(struct platform_device *pdev)
 	struct exynos_ohci_hcd *exynos_ohci;
 	struct usb_hcd *hcd;
 	struct resource *res;
-	struct usb_phy *phy;
 	int irq;
 	int err;
 
@@ -88,15 +170,9 @@ static int exynos_ohci_probe(struct platform_device *pdev)
 					"samsung,exynos5440-ohci"))
 		goto skip_phy;
 
-	phy = devm_usb_get_phy(&pdev->dev, USB_PHY_TYPE_USB2);
-	if (IS_ERR(phy)) {
-		usb_put_hcd(hcd);
-		dev_warn(&pdev->dev, "no platform data or transceiver defined\n");
-		return -EPROBE_DEFER;
-	} else {
-		exynos_ohci->phy = phy;
-		exynos_ohci->otg = phy->otg;
-	}
+	err = exynos_ohci_get_phy(&pdev->dev, exynos_ohci);
+	if (err)
+		goto fail_clk;
 
 skip_phy:
 	exynos_ohci->clk = devm_clk_get(&pdev->dev, "usbhost");
@@ -120,10 +196,9 @@ skip_phy:
 
 	hcd->rsrc_start = res->start;
 	hcd->rsrc_len = resource_size(res);
-	hcd->regs = devm_ioremap(&pdev->dev, res->start, hcd->rsrc_len);
-	if (!hcd->regs) {
-		dev_err(&pdev->dev, "Failed to remap I/O memory\n");
-		err = -ENOMEM;
+	hcd->regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(hcd->regs)) {
+		err = PTR_ERR(hcd->regs);
 		goto fail_io;
 	}
 
@@ -139,7 +214,11 @@ skip_phy:
 
 	platform_set_drvdata(pdev, hcd);
 
-	exynos_ohci_phy_enable(pdev);
+	err = exynos_ohci_phy_enable(&pdev->dev);
+	if (err) {
+		dev_err(&pdev->dev, "Failed to enable USB phy\n");
+		goto fail_io;
+	}
 
 	err = usb_add_hcd(hcd, irq, IRQF_SHARED);
 	if (err) {
@@ -150,7 +229,7 @@ skip_phy:
 	return 0;
 
 fail_add_hcd:
-	exynos_ohci_phy_disable(pdev);
+	exynos_ohci_phy_disable(&pdev->dev);
 fail_io:
 	clk_disable_unprepare(exynos_ohci->clk);
 fail_clk:
@@ -168,7 +247,7 @@ static int exynos_ohci_remove(struct platform_device *pdev)
 	if (exynos_ohci->otg)
 		exynos_ohci->otg->set_host(exynos_ohci->otg, &hcd->self);
 
-	exynos_ohci_phy_disable(pdev);
+	exynos_ohci_phy_disable(&pdev->dev);
 
 	clk_disable_unprepare(exynos_ohci->clk);
 
@@ -190,26 +269,19 @@ static int exynos_ohci_suspend(struct device *dev)
 {
 	struct usb_hcd *hcd = dev_get_drvdata(dev);
 	struct exynos_ohci_hcd *exynos_ohci = to_exynos_ohci(hcd);
-	struct ohci_hcd *ohci = hcd_to_ohci(hcd);
-	struct platform_device *pdev = to_platform_device(dev);
 	bool do_wakeup = device_may_wakeup(dev);
-	unsigned long flags;
 	int rc = ohci_suspend(hcd, do_wakeup);
 
 	if (rc)
 		return rc;
 
-	spin_lock_irqsave(&ohci->lock, flags);
-
 	if (exynos_ohci->otg)
 		exynos_ohci->otg->set_host(exynos_ohci->otg, &hcd->self);
 
-	exynos_ohci_phy_disable(pdev);
+	exynos_ohci_phy_disable(dev);
 
 	clk_disable_unprepare(exynos_ohci->clk);
 
-	spin_unlock_irqrestore(&ohci->lock, flags);
-
 	return 0;
 }
 
@@ -217,14 +289,19 @@ static int exynos_ohci_resume(struct device *dev)
 {
 	struct usb_hcd *hcd			= dev_get_drvdata(dev);
 	struct exynos_ohci_hcd *exynos_ohci	= to_exynos_ohci(hcd);
-	struct platform_device *pdev		= to_platform_device(dev);
+	int ret;
 
 	clk_prepare_enable(exynos_ohci->clk);
 
 	if (exynos_ohci->otg)
 		exynos_ohci->otg->set_host(exynos_ohci->otg, &hcd->self);
 
-	exynos_ohci_phy_enable(pdev);
+	ret = exynos_ohci_phy_enable(dev);
+	if (ret) {
+		dev_err(dev, "Failed to enable USB phy\n");
+		clk_disable_unprepare(exynos_ohci->clk);
+		return ret;
+	}
 
 	ohci_resume(hcd, false);
 
diff --git a/drivers/usb/host/ohci-hcd.c b/drivers/usb/host/ohci-hcd.c
index 3586460fb2a..f98d03f3144 100644
--- a/drivers/usb/host/ohci-hcd.c
+++ b/drivers/usb/host/ohci-hcd.c
@@ -1178,7 +1178,7 @@ MODULE_LICENSE ("GPL");
 #define SA1111_DRIVER		ohci_hcd_sa1111_driver
 #endif
 
-#ifdef CONFIG_ARCH_DAVINCI_DA8XX
+#ifdef CONFIG_USB_OHCI_HCD_DAVINCI
 #include "ohci-da8xx.c"
 #define DAVINCI_PLATFORM_DRIVER	ohci_hcd_da8xx_driver
 #endif
diff --git a/drivers/usb/host/ohci-hub.c b/drivers/usb/host/ohci-hub.c
index c81c8721cc5..b4940de1eba 100644
--- a/drivers/usb/host/ohci-hub.c
+++ b/drivers/usb/host/ohci-hub.c
@@ -90,6 +90,24 @@ __acquires(ohci->lock)
 	dl_done_list (ohci);
 	finish_unlinks (ohci, ohci_frame_no(ohci));
 
+	/*
+	 * Some controllers don't handle "global" suspend properly if
+	 * there are unsuspended ports.  For these controllers, put all
+	 * the enabled ports into suspend before suspending the root hub.
+	 */
+	if (ohci->flags & OHCI_QUIRK_GLOBAL_SUSPEND) {
+		__hc32 __iomem	*portstat = ohci->regs->roothub.portstatus;
+		int		i;
+		unsigned	temp;
+
+		for (i = 0; i < ohci->num_ports; (++i, ++portstat)) {
+			temp = ohci_readl(ohci, portstat);
+			if ((temp & (RH_PS_PES | RH_PS_PSS)) ==
+					RH_PS_PES)
+				ohci_writel(ohci, RH_PS_PSS, portstat);
+		}
+	}
+
 	/* maybe resume can wake root hub */
 	if (ohci_to_hcd(ohci)->self.root_hub->do_remote_wakeup || autostop) {
 		ohci->hc_control |= OHCI_CTRL_RWE;
@@ -438,8 +456,7 @@ static int ohci_root_hub_state_changes(struct ohci_hcd *ohci, int changed,
 
 /* build "status change" packet (one or two bytes) from HC registers */
 
-static int
-ohci_hub_status_data (struct usb_hcd *hcd, char *buf)
+int ohci_hub_status_data(struct usb_hcd *hcd, char *buf)
 {
 	struct ohci_hcd	*ohci = hcd_to_ohci (hcd);
 	int		i, changed = 0, length = 1;
@@ -504,6 +521,7 @@ done:
 
 	return changed ? length : 0;
 }
+EXPORT_SYMBOL_GPL(ohci_hub_status_data);
 
 /*-------------------------------------------------------------------------*/
 
@@ -646,7 +664,7 @@ static inline int root_port_reset (struct ohci_hcd *ohci, unsigned port)
 	return 0;
 }
 
-static int ohci_hub_control (
+int ohci_hub_control(
 	struct usb_hcd	*hcd,
 	u16		typeReq,
 	u16		wValue,
@@ -772,4 +790,4 @@ error:
 	}
 	return retval;
 }
-
+EXPORT_SYMBOL_GPL(ohci_hub_control);
diff --git a/drivers/usb/host/ohci-jz4740.c b/drivers/usb/host/ohci-jz4740.c
index af8dc1b92d7..c2c221a332e 100644
--- a/drivers/usb/host/ohci-jz4740.c
+++ b/drivers/usb/host/ohci-jz4740.c
@@ -82,14 +82,14 @@ static int ohci_jz4740_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 	u16 wIndex, char *buf, u16 wLength)
 {
 	struct jz4740_ohci_hcd *jz4740_ohci = hcd_to_jz4740_hcd(hcd);
-	int ret;
+	int ret = 0;
 
 	switch (typeReq) {
-	case SetHubFeature:
+	case SetPortFeature:
 		if (wValue == USB_PORT_FEAT_POWER)
 			ret = ohci_jz4740_set_vbus_power(jz4740_ohci, true);
 		break;
-	case ClearHubFeature:
+	case ClearPortFeature:
 		if (wValue == USB_PORT_FEAT_POWER)
 			ret = ohci_jz4740_set_vbus_power(jz4740_ohci, false);
 		break;
diff --git a/drivers/usb/host/ohci-pci.c b/drivers/usb/host/ohci-pci.c
index 90879e9ccbe..bb150967572 100644
--- a/drivers/usb/host/ohci-pci.c
+++ b/drivers/usb/host/ohci-pci.c
@@ -160,6 +160,7 @@ static int ohci_quirk_amd700(struct usb_hcd *hcd)
 		ohci_dbg(ohci, "enabled AMD prefetch quirk\n");
 	}
 
+	ohci->flags |= OHCI_QUIRK_GLOBAL_SUSPEND;
 	return 0;
 }
 
diff --git a/drivers/usb/host/ohci-platform.c b/drivers/usb/host/ohci-platform.c
index b6002c951c5..4369299064c 100644
--- a/drivers/usb/host/ohci-platform.c
+++ b/drivers/usb/host/ohci-platform.c
@@ -24,6 +24,7 @@
 #include <linux/err.h>
 #include <linux/phy/phy.h>
 #include <linux/platform_device.h>
+#include <linux/reset.h>
 #include <linux/usb/ohci_pdriver.h>
 #include <linux/usb.h>
 #include <linux/usb/hcd.h>
@@ -36,6 +37,7 @@
 
 struct ohci_platform_priv {
 	struct clk *clks[OHCI_MAX_CLKS];
+	struct reset_control *rst;
 	struct phy *phy;
 };
 
@@ -191,6 +193,19 @@ static int ohci_platform_probe(struct platform_device *dev)
 				break;
 			}
 		}
+
+	}
+
+	priv->rst = devm_reset_control_get_optional(&dev->dev, NULL);
+	if (IS_ERR(priv->rst)) {
+		err = PTR_ERR(priv->rst);
+		if (err == -EPROBE_DEFER)
+			goto err_put_clks;
+		priv->rst = NULL;
+	} else {
+		err = reset_control_deassert(priv->rst);
+		if (err)
+			goto err_put_clks;
 	}
 
 	if (pdata->big_endian_desc)
@@ -203,7 +218,7 @@ static int ohci_platform_probe(struct platform_device *dev)
 		dev_err(&dev->dev,
 			"Error: CONFIG_USB_OHCI_BIG_ENDIAN_MMIO not set\n");
 		err = -EINVAL;
-		goto err_put_clks;
+		goto err_reset;
 	}
 #endif
 #ifndef CONFIG_USB_OHCI_BIG_ENDIAN_DESC
@@ -211,14 +226,14 @@ static int ohci_platform_probe(struct platform_device *dev)
 		dev_err(&dev->dev,
 			"Error: CONFIG_USB_OHCI_BIG_ENDIAN_DESC not set\n");
 		err = -EINVAL;
-		goto err_put_clks;
+		goto err_reset;
 	}
 #endif
 
 	if (pdata->power_on) {
 		err = pdata->power_on(dev);
 		if (err < 0)
-			goto err_put_clks;
+			goto err_reset;
 	}
 
 	hcd->rsrc_start = res_mem->start;
@@ -242,6 +257,9 @@ static int ohci_platform_probe(struct platform_device *dev)
 err_power:
 	if (pdata->power_off)
 		pdata->power_off(dev);
+err_reset:
+	if (priv->rst)
+		reset_control_assert(priv->rst);
 err_put_clks:
 	while (--clk >= 0)
 		clk_put(priv->clks[clk]);
@@ -266,6 +284,9 @@ static int ohci_platform_remove(struct platform_device *dev)
 	if (pdata->power_off)
 		pdata->power_off(dev);
 
+	if (priv->rst)
+		reset_control_assert(priv->rst);
+
 	for (clk = 0; clk < OHCI_MAX_CLKS && priv->clks[clk]; clk++)
 		clk_put(priv->clks[clk]);
 
diff --git a/drivers/usb/host/ohci-pxa27x.c b/drivers/usb/host/ohci-pxa27x.c
index d21d5fefa76..e68f3d02cd1 100644
--- a/drivers/usb/host/ohci-pxa27x.c
+++ b/drivers/usb/host/ohci-pxa27x.c
@@ -30,6 +30,7 @@
 #include <linux/platform_data/usb-ohci-pxa27x.h>
 #include <linux/platform_data/usb-pxa3xx-ulpi.h>
 #include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
 #include <linux/signal.h>
 #include <linux/usb.h>
 #include <linux/usb/hcd.h>
@@ -120,6 +121,8 @@ static struct hc_driver __read_mostly ohci_pxa27x_hc_driver;
 struct pxa27x_ohci {
 	struct clk	*clk;
 	void __iomem	*mmio_base;
+	struct regulator *vbus[3];
+	bool		vbus_enabled[3];
 };
 
 #define to_pxa27x_ohci(hcd)	(struct pxa27x_ohci *)(hcd_to_ohci(hcd)->priv)
@@ -166,6 +169,52 @@ static int pxa27x_ohci_select_pmm(struct pxa27x_ohci *pxa_ohci, int mode)
 	return 0;
 }
 
+static int pxa27x_ohci_set_vbus_power(struct pxa27x_ohci *pxa_ohci,
+				      unsigned int port, bool enable)
+{
+	struct regulator *vbus = pxa_ohci->vbus[port];
+	int ret = 0;
+
+	if (IS_ERR_OR_NULL(vbus))
+		return 0;
+
+	if (enable && !pxa_ohci->vbus_enabled[port])
+		ret = regulator_enable(vbus);
+	else if (!enable && pxa_ohci->vbus_enabled[port])
+		ret = regulator_disable(vbus);
+
+	if (ret < 0)
+		return ret;
+
+	pxa_ohci->vbus_enabled[port] = enable;
+
+	return 0;
+}
+
+static int pxa27x_ohci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
+				   u16 wIndex, char *buf, u16 wLength)
+{
+	struct pxa27x_ohci *pxa_ohci = to_pxa27x_ohci(hcd);
+	int ret;
+
+	switch (typeReq) {
+	case SetPortFeature:
+	case ClearPortFeature:
+		if (!wIndex || wIndex > 3)
+			return -EPIPE;
+
+		if (wValue != USB_PORT_FEAT_POWER)
+			break;
+
+		ret = pxa27x_ohci_set_vbus_power(pxa_ohci, wIndex - 1,
+						 typeReq == SetPortFeature);
+		if (ret)
+			return ret;
+		break;
+	}
+
+	return ohci_hub_control(hcd, typeReq, wValue, wIndex, buf, wLength);
+}
 /*-------------------------------------------------------------------------*/
 
 static inline void pxa27x_setup_hc(struct pxa27x_ohci *pxa_ohci,
@@ -372,6 +421,7 @@ int usb_hcd_pxa27x_probe (const struct hc_driver *driver, struct platform_device
 	struct ohci_hcd *ohci;
 	struct resource *r;
 	struct clk *usb_clk;
+	unsigned int i;
 
 	retval = ohci_pxa_of_init(pdev);
 	if (retval)
@@ -417,6 +467,16 @@ int usb_hcd_pxa27x_probe (const struct hc_driver *driver, struct platform_device
 	pxa_ohci->clk = usb_clk;
 	pxa_ohci->mmio_base = (void __iomem *)hcd->regs;
 
+	for (i = 0; i < 3; ++i) {
+		char name[6];
+
+		if (!(inf->flags & (ENABLE_PORT1 << i)))
+			continue;
+
+		sprintf(name, "vbus%u", i + 1);
+		pxa_ohci->vbus[i] = devm_regulator_get(&pdev->dev, name);
+	}
+
 	retval = pxa27x_start_hc(pxa_ohci, &pdev->dev);
 	if (retval < 0) {
 		pr_debug("pxa27x_start_hc failed");
@@ -462,9 +522,14 @@ int usb_hcd_pxa27x_probe (const struct hc_driver *driver, struct platform_device
 void usb_hcd_pxa27x_remove (struct usb_hcd *hcd, struct platform_device *pdev)
 {
 	struct pxa27x_ohci *pxa_ohci = to_pxa27x_ohci(hcd);
+	unsigned int i;
 
 	usb_remove_hcd(hcd);
 	pxa27x_stop_hc(pxa_ohci, &pdev->dev);
+
+	for (i = 0; i < 3; ++i)
+		pxa27x_ohci_set_vbus_power(pxa_ohci, i, false);
+
 	usb_put_hcd(hcd);
 }
 
@@ -563,7 +628,10 @@ static int __init ohci_pxa27x_init(void)
 		return -ENODEV;
 
 	pr_info("%s: " DRIVER_DESC "\n", hcd_name);
+
 	ohci_init_driver(&ohci_pxa27x_hc_driver, &pxa27x_overrides);
+	ohci_pxa27x_hc_driver.hub_control = pxa27x_ohci_hub_control;
+
 	return platform_driver_register(&ohci_hcd_pxa27x_driver);
 }
 module_init(ohci_pxa27x_init);
diff --git a/drivers/usb/host/ohci-s3c2410.c b/drivers/usb/host/ohci-s3c2410.c
index ff7c8f1c48f..3d753a9d314 100644
--- a/drivers/usb/host/ohci-s3c2410.c
+++ b/drivers/usb/host/ohci-s3c2410.c
@@ -45,10 +45,6 @@ static struct clk *usb_clk;
 
 /* forward definitions */
 
-static int (*orig_ohci_hub_control)(struct usb_hcd  *hcd, u16 typeReq,
-			u16 wValue, u16 wIndex, char *buf, u16 wLength);
-static int (*orig_ohci_hub_status_data)(struct usb_hcd *hcd, char *buf);
-
 static void s3c2410_hcd_oc(struct s3c2410_hcd_info *info, int port_oc);
 
 /* conversion functions */
@@ -110,7 +106,7 @@ ohci_s3c2410_hub_status_data(struct usb_hcd *hcd, char *buf)
 	int orig;
 	int portno;
 
-	orig = orig_ohci_hub_status_data(hcd, buf);
+	orig = ohci_hub_status_data(hcd, buf);
 
 	if (info == NULL)
 		return orig;
@@ -181,7 +177,7 @@ static int ohci_s3c2410_hub_control(
 	 * process the request straight away and exit */
 
 	if (info == NULL) {
-		ret = orig_ohci_hub_control(hcd, typeReq, wValue,
+		ret = ohci_hub_control(hcd, typeReq, wValue,
 				       wIndex, buf, wLength);
 		goto out;
 	}
@@ -231,7 +227,7 @@ static int ohci_s3c2410_hub_control(
 		break;
 	}
 
-	ret = orig_ohci_hub_control(hcd, typeReq, wValue, wIndex, buf, wLength);
+	ret = ohci_hub_control(hcd, typeReq, wValue, wIndex, buf, wLength);
 	if (ret)
 		goto out;
 
@@ -489,9 +485,6 @@ static int __init ohci_s3c2410_init(void)
 	 * override these functions by making it too easy.
 	 */
 
-	orig_ohci_hub_control = ohci_s3c2410_hc_driver.hub_control;
-	orig_ohci_hub_status_data = ohci_s3c2410_hc_driver.hub_status_data;
-
 	ohci_s3c2410_hc_driver.hub_status_data	= ohci_s3c2410_hub_status_data;
 	ohci_s3c2410_hc_driver.hub_control	= ohci_s3c2410_hub_control;
 
diff --git a/drivers/usb/host/ohci.h b/drivers/usb/host/ohci.h
index 9250cada13f..05e02a709d4 100644
--- a/drivers/usb/host/ohci.h
+++ b/drivers/usb/host/ohci.h
@@ -405,6 +405,8 @@ struct ohci_hcd {
 #define	OHCI_QUIRK_HUB_POWER	0x100			/* distrust firmware power/oc setup */
 #define	OHCI_QUIRK_AMD_PLL	0x200			/* AMD PLL quirk*/
 #define	OHCI_QUIRK_AMD_PREFETCH	0x400			/* pre-fetch for ISO transfer */
+#define	OHCI_QUIRK_GLOBAL_SUSPEND	0x800		/* must suspend ports */
+
 	// there are also chip quirks/bugs in init logic
 
 	struct work_struct	nec_work;	/* Worker for NEC quirk */
@@ -727,3 +729,6 @@ extern int	ohci_setup(struct usb_hcd *hcd);
 extern int	ohci_suspend(struct usb_hcd *hcd, bool do_wakeup);
 extern int	ohci_resume(struct usb_hcd *hcd, bool hibernated);
 #endif
+extern int	ohci_hub_control(struct usb_hcd	*hcd, u16 typeReq, u16 wValue,
+				 u16 wIndex, char *buf, u16 wLength);
+extern int	ohci_hub_status_data(struct usb_hcd *hcd, char *buf);
diff --git a/drivers/usb/host/pci-quirks.h b/drivers/usb/host/pci-quirks.h
index 638e88f7a28..c622ddf21c9 100644
--- a/drivers/usb/host/pci-quirks.h
+++ b/drivers/usb/host/pci-quirks.h
@@ -5,6 +5,7 @@
 void uhci_reset_hc(struct pci_dev *pdev, unsigned long base);
 int uhci_check_and_reset_hc(struct pci_dev *pdev, unsigned long base);
 int usb_amd_find_chipset_info(void);
+int usb_hcd_amd_remote_wakeup_quirk(struct pci_dev *pdev);
 bool usb_amd_hang_symptom_quirk(void);
 bool usb_amd_prefetch_quirk(void);
 void usb_amd_dev_put(void);
diff --git a/drivers/usb/host/xhci-hub.c b/drivers/usb/host/xhci-hub.c
index 1ad6bc1951c..6231ce6aa0c 100644
--- a/drivers/usb/host/xhci-hub.c
+++ b/drivers/usb/host/xhci-hub.c
@@ -20,7 +20,8 @@
  * Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
-#include <linux/gfp.h>
+
+#include <linux/slab.h>
 #include <asm/unaligned.h>
 
 #include "xhci.h"
@@ -270,7 +271,6 @@ static int xhci_stop_device(struct xhci_hcd *xhci, int slot_id, int suspend)
 	struct xhci_virt_device *virt_dev;
 	struct xhci_command *cmd;
 	unsigned long flags;
-	int timeleft;
 	int ret;
 	int i;
 
@@ -284,34 +284,31 @@ static int xhci_stop_device(struct xhci_hcd *xhci, int slot_id, int suspend)
 
 	spin_lock_irqsave(&xhci->lock, flags);
 	for (i = LAST_EP_INDEX; i > 0; i--) {
-		if (virt_dev->eps[i].ring && virt_dev->eps[i].ring->dequeue)
-			xhci_queue_stop_endpoint(xhci, slot_id, i, suspend);
+		if (virt_dev->eps[i].ring && virt_dev->eps[i].ring->dequeue) {
+			struct xhci_command *command;
+			command = xhci_alloc_command(xhci, false, false,
+						     GFP_NOIO);
+			if (!command) {
+				spin_unlock_irqrestore(&xhci->lock, flags);
+				xhci_free_command(xhci, cmd);
+				return -ENOMEM;
+
+			}
+			xhci_queue_stop_endpoint(xhci, command, slot_id, i,
+						 suspend);
+		}
 	}
-	cmd->command_trb = xhci_find_next_enqueue(xhci->cmd_ring);
-	list_add_tail(&cmd->cmd_list, &virt_dev->cmd_list);
-	xhci_queue_stop_endpoint(xhci, slot_id, 0, suspend);
+	xhci_queue_stop_endpoint(xhci, cmd, slot_id, 0, suspend);
 	xhci_ring_cmd_db(xhci);
 	spin_unlock_irqrestore(&xhci->lock, flags);
 
 	/* Wait for last stop endpoint command to finish */
-	timeleft = wait_for_completion_interruptible_timeout(
-			cmd->completion,
-			XHCI_CMD_DEFAULT_TIMEOUT);
-	if (timeleft <= 0) {
-		xhci_warn(xhci, "%s while waiting for stop endpoint command\n",
-				timeleft == 0 ? "Timeout" : "Signal");
-		spin_lock_irqsave(&xhci->lock, flags);
-		/* The timeout might have raced with the event ring handler, so
-		 * only delete from the list if the item isn't poisoned.
-		 */
-		if (cmd->cmd_list.next != LIST_POISON1)
-			list_del(&cmd->cmd_list);
-		spin_unlock_irqrestore(&xhci->lock, flags);
+	wait_for_completion(cmd->completion);
+
+	if (cmd->status == COMP_CMD_ABORT || cmd->status == COMP_CMD_STOP) {
+		xhci_warn(xhci, "Timeout while waiting for stop endpoint command\n");
 		ret = -ETIME;
-		goto command_cleanup;
 	}
-
-command_cleanup:
 	xhci_free_command(xhci, cmd);
 	return ret;
 }
diff --git a/drivers/usb/host/xhci-mem.c b/drivers/usb/host/xhci-mem.c
index c089668308a..6a57e81c2a7 100644
--- a/drivers/usb/host/xhci-mem.c
+++ b/drivers/usb/host/xhci-mem.c
@@ -1020,7 +1020,6 @@ int xhci_alloc_virt_device(struct xhci_hcd *xhci, int slot_id,
 	dev->num_rings_cached = 0;
 
 	init_completion(&dev->cmd_completion);
-	INIT_LIST_HEAD(&dev->cmd_list);
 	dev->udev = udev;
 
 	/* Point to output device context in dcbaa. */
@@ -1794,10 +1793,11 @@ void xhci_free_command(struct xhci_hcd *xhci,
 void xhci_mem_cleanup(struct xhci_hcd *xhci)
 {
 	struct device	*dev = xhci_to_hcd(xhci)->self.controller;
-	struct xhci_cd  *cur_cd, *next_cd;
 	int size;
 	int i, j, num_ports;
 
+	del_timer_sync(&xhci->cmd_timer);
+
 	/* Free the Event Ring Segment Table and the actual Event Ring */
 	size = sizeof(struct xhci_erst_entry)*(xhci->erst.num_entries);
 	if (xhci->erst.entries)
@@ -1816,11 +1816,7 @@ void xhci_mem_cleanup(struct xhci_hcd *xhci)
 		xhci_ring_free(xhci, xhci->cmd_ring);
 	xhci->cmd_ring = NULL;
 	xhci_dbg_trace(xhci, trace_xhci_dbg_init, "Freed command ring");
-	list_for_each_entry_safe(cur_cd, next_cd,
-			&xhci->cancel_cmd_list, cancel_cmd_list) {
-		list_del(&cur_cd->cancel_cmd_list);
-		kfree(cur_cd);
-	}
+	xhci_cleanup_command_queue(xhci);
 
 	for (i = 1; i < MAX_HC_SLOTS; ++i)
 		xhci_free_virt_device(xhci, i);
@@ -2323,7 +2319,7 @@ int xhci_mem_init(struct xhci_hcd *xhci, gfp_t flags)
 	u32 page_size, temp;
 	int i;
 
-	INIT_LIST_HEAD(&xhci->cancel_cmd_list);
+	INIT_LIST_HEAD(&xhci->cmd_list);
 
 	page_size = readl(&xhci->op_regs->page_size);
 	xhci_dbg_trace(xhci, trace_xhci_dbg_init,
@@ -2509,6 +2505,11 @@ int xhci_mem_init(struct xhci_hcd *xhci, gfp_t flags)
 			"Wrote ERST address to ir_set 0.");
 	xhci_print_ir_set(xhci, 0);
 
+	/* init command timeout timer */
+	init_timer(&xhci->cmd_timer);
+	xhci->cmd_timer.data = (unsigned long) xhci;
+	xhci->cmd_timer.function = xhci_handle_command_timeout;
+
 	/*
 	 * XXX: Might need to set the Interrupter Moderation Register to
 	 * something other than the default (~1ms minimum between interrupts).
diff --git a/drivers/usb/host/xhci-mvebu.c b/drivers/usb/host/xhci-mvebu.c
new file mode 100644
index 00000000000..1eefc988192
--- /dev/null
+++ b/drivers/usb/host/xhci-mvebu.c
@@ -0,0 +1,72 @@
+/*
+ * Copyright (C) 2014 Marvell
+ * Author: Gregory CLEMENT <gregory.clement@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
+ * version 2 as published by the Free Software Foundation.
+ */
+
+#include <linux/io.h>
+#include <linux/mbus.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+
+#include "xhci-mvebu.h"
+
+#define USB3_MAX_WINDOWS	4
+#define USB3_WIN_CTRL(w)	(0x0 + ((w) * 8))
+#define USB3_WIN_BASE(w)	(0x4 + ((w) * 8))
+
+static void xhci_mvebu_mbus_config(void __iomem *base,
+			const struct mbus_dram_target_info *dram)
+{
+	int win;
+
+	/* Clear all existing windows */
+	for (win = 0; win < USB3_MAX_WINDOWS; win++) {
+		writel(0, base + USB3_WIN_CTRL(win));
+		writel(0, base + USB3_WIN_BASE(win));
+	}
+
+	/* Program each DRAM CS in a seperate window */
+	for (win = 0; win < dram->num_cs; win++) {
+		const struct mbus_dram_window *cs = dram->cs + win;
+
+		writel(((cs->size - 1) & 0xffff0000) | (cs->mbus_attr << 8) |
+		       (dram->mbus_dram_target_id << 4) | 1,
+		       base + USB3_WIN_CTRL(win));
+
+		writel((cs->base & 0xffff0000), base + USB3_WIN_BASE(win));
+	}
+}
+
+int xhci_mvebu_mbus_init_quirk(struct platform_device *pdev)
+{
+	struct resource	*res;
+	void __iomem *base;
+	const struct mbus_dram_target_info *dram;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	if (!res)
+		return -ENODEV;
+
+	/*
+	 * We don't use devm_ioremap() because this mapping should
+	 * only exists for the duration of this probe function.
+	 */
+	base = ioremap(res->start, resource_size(res));
+	if (!base)
+		return -ENODEV;
+
+	dram = mv_mbus_dram_info();
+	xhci_mvebu_mbus_config(base, dram);
+
+	/*
+	 * This memory area was only needed to configure the MBus
+	 * windows, and is therefore no longer useful.
+	 */
+	iounmap(base);
+
+	return 0;
+}
diff --git a/drivers/usb/host/xhci-mvebu.h b/drivers/usb/host/xhci-mvebu.h
new file mode 100644
index 00000000000..7ede92aa41f
--- /dev/null
+++ b/drivers/usb/host/xhci-mvebu.h
@@ -0,0 +1,21 @@
+/*
+ * Copyright (C) 2014 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 __LINUX_XHCI_MVEBU_H
+#define __LINUX_XHCI_MVEBU_H
+#if IS_ENABLED(CONFIG_USB_XHCI_MVEBU)
+int xhci_mvebu_mbus_init_quirk(struct platform_device *pdev);
+#else
+static inline int xhci_mvebu_mbus_init_quirk(struct platform_device *pdev)
+{
+	return 0;
+}
+#endif
+#endif /* __LINUX_XHCI_MVEBU_H */
diff --git a/drivers/usb/host/xhci-pci.c b/drivers/usb/host/xhci-pci.c
index 47390e369cd..e20520f4275 100644
--- a/drivers/usb/host/xhci-pci.c
+++ b/drivers/usb/host/xhci-pci.c
@@ -135,20 +135,24 @@ static void xhci_pci_quirks(struct device *dev, struct xhci_hcd *xhci)
 		if (pdev->subsystem_vendor == PCI_VENDOR_ID_HP)
 			xhci->quirks |= XHCI_SPURIOUS_WAKEUP;
 	}
+	if (pdev->vendor == PCI_VENDOR_ID_INTEL &&
+		pdev->device == PCI_DEVICE_ID_INTEL_LYNXPOINT_LP_XHCI) {
+		xhci->quirks |= XHCI_SPURIOUS_REBOOT;
+	}
 	if (pdev->vendor == PCI_VENDOR_ID_ETRON &&
 			pdev->device == PCI_DEVICE_ID_ASROCK_P67) {
 		xhci->quirks |= XHCI_RESET_ON_RESUME;
-		xhci_dbg_trace(xhci, trace_xhci_dbg_quirks,
-				"QUIRK: Resetting on resume");
 		xhci->quirks |= XHCI_TRUST_TX_LENGTH;
 	}
 	if (pdev->vendor == PCI_VENDOR_ID_RENESAS &&
-			pdev->device == 0x0015 &&
-			pdev->subsystem_vendor == PCI_VENDOR_ID_SAMSUNG &&
-			pdev->subsystem_device == 0xc0cd)
+			pdev->device == 0x0015)
 		xhci->quirks |= XHCI_RESET_ON_RESUME;
 	if (pdev->vendor == PCI_VENDOR_ID_VIA)
 		xhci->quirks |= XHCI_RESET_ON_RESUME;
+
+	if (xhci->quirks & XHCI_RESET_ON_RESUME)
+		xhci_dbg_trace(xhci, trace_xhci_dbg_quirks,
+				"QUIRK: Resetting on resume");
 }
 
 /* called during probe() after chip reset completes */
diff --git a/drivers/usb/host/xhci-plat.c b/drivers/usb/host/xhci-plat.c
index 151901ce1ba..29d8adb5c8d 100644
--- a/drivers/usb/host/xhci-plat.c
+++ b/drivers/usb/host/xhci-plat.c
@@ -11,13 +11,15 @@
  * version 2 as published by the Free Software Foundation.
  */
 
-#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/dma-mapping.h>
 #include <linux/module.h>
-#include <linux/slab.h>
 #include <linux/of.h>
-#include <linux/dma-mapping.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
 
 #include "xhci.h"
+#include "xhci-mvebu.h"
 
 static void xhci_plat_quirks(struct device *dev, struct xhci_hcd *xhci)
 {
@@ -35,6 +37,11 @@ static int xhci_plat_setup(struct usb_hcd *hcd)
 	return xhci_gen_setup(hcd, xhci_plat_quirks);
 }
 
+static int xhci_plat_start(struct usb_hcd *hcd)
+{
+	return xhci_run(hcd);
+}
+
 static const struct hc_driver xhci_plat_xhci_driver = {
 	.description =		"xhci-hcd",
 	.product_desc =		"xHCI Host Controller",
@@ -50,7 +57,7 @@ static const struct hc_driver xhci_plat_xhci_driver = {
 	 * basic lifecycle operations
 	 */
 	.reset =		xhci_plat_setup,
-	.start =		xhci_run,
+	.start =		xhci_plat_start,
 	.stop =			xhci_stop,
 	.shutdown =		xhci_shutdown,
 
@@ -91,6 +98,7 @@ static int xhci_plat_probe(struct platform_device *pdev)
 	struct xhci_hcd		*xhci;
 	struct resource         *res;
 	struct usb_hcd		*hcd;
+	struct clk              *clk;
 	int			ret;
 	int			irq;
 
@@ -107,6 +115,15 @@ static int xhci_plat_probe(struct platform_device *pdev)
 	if (!res)
 		return -ENODEV;
 
+	if (of_device_is_compatible(pdev->dev.of_node,
+				    "marvell,armada-375-xhci") ||
+	    of_device_is_compatible(pdev->dev.of_node,
+				    "marvell,armada-380-xhci")) {
+		ret = xhci_mvebu_mbus_init_quirk(pdev);
+		if (ret)
+			return ret;
+	}
+
 	/* Initialize dma_mask and coherent_dma_mask to 32-bits */
 	ret = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32));
 	if (ret)
@@ -137,14 +154,27 @@ static int xhci_plat_probe(struct platform_device *pdev)
 		goto release_mem_region;
 	}
 
+	/*
+	 * Not all platforms have a clk so it is not an error if the
+	 * clock does not exists.
+	 */
+	clk = devm_clk_get(&pdev->dev, NULL);
+	if (!IS_ERR(clk)) {
+		ret = clk_prepare_enable(clk);
+		if (ret)
+			goto unmap_registers;
+	}
+
 	ret = usb_add_hcd(hcd, irq, IRQF_SHARED);
 	if (ret)
-		goto unmap_registers;
+		goto disable_clk;
+
 	device_wakeup_enable(hcd->self.controller);
 
 	/* USB 2.0 roothub is stored in the platform_device now. */
 	hcd = platform_get_drvdata(pdev);
 	xhci = hcd_to_xhci(hcd);
+	xhci->clk = clk;
 	xhci->shared_hcd = usb_create_shared_hcd(driver, &pdev->dev,
 			dev_name(&pdev->dev), hcd);
 	if (!xhci->shared_hcd) {
@@ -173,6 +203,10 @@ put_usb3_hcd:
 dealloc_usb2_hcd:
 	usb_remove_hcd(hcd);
 
+disable_clk:
+	if (!IS_ERR(clk))
+		clk_disable_unprepare(clk);
+
 unmap_registers:
 	iounmap(hcd->regs);
 
@@ -189,11 +223,14 @@ static int xhci_plat_remove(struct platform_device *dev)
 {
 	struct usb_hcd	*hcd = platform_get_drvdata(dev);
 	struct xhci_hcd	*xhci = hcd_to_xhci(hcd);
+	struct clk *clk = xhci->clk;
 
 	usb_remove_hcd(xhci->shared_hcd);
 	usb_put_hcd(xhci->shared_hcd);
 
 	usb_remove_hcd(hcd);
+	if (!IS_ERR(clk))
+		clk_disable_unprepare(clk);
 	iounmap(hcd->regs);
 	release_mem_region(hcd->rsrc_start, hcd->rsrc_len);
 	usb_put_hcd(hcd);
@@ -202,7 +239,7 @@ static int xhci_plat_remove(struct platform_device *dev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int xhci_plat_suspend(struct device *dev)
 {
 	struct usb_hcd	*hcd = dev_get_drvdata(dev);
@@ -231,6 +268,8 @@ static const struct dev_pm_ops xhci_plat_pm_ops = {
 static const struct of_device_id usb_xhci_of_match[] = {
 	{ .compatible = "generic-xhci" },
 	{ .compatible = "xhci-platform" },
+	{ .compatible = "marvell,armada-375-xhci"},
+	{ .compatible = "marvell,armada-380-xhci"},
 	{ },
 };
 MODULE_DEVICE_TABLE(of, usb_xhci_of_match);
diff --git a/drivers/usb/host/xhci-ring.c b/drivers/usb/host/xhci-ring.c
index 5f926bea5ab..d67ff71209f 100644
--- a/drivers/usb/host/xhci-ring.c
+++ b/drivers/usb/host/xhci-ring.c
@@ -69,10 +69,6 @@
 #include "xhci.h"
 #include "xhci-trace.h"
 
-static int handle_cmd_in_cmd_wait_list(struct xhci_hcd *xhci,
-		struct xhci_virt_device *virt_dev,
-		struct xhci_event_cmd *event);
-
 /*
  * Returns zero if the TRB isn't in this segment, otherwise it returns the DMA
  * address of the TRB.
@@ -123,16 +119,6 @@ static int enqueue_is_link_trb(struct xhci_ring *ring)
 	return TRB_TYPE_LINK_LE32(link->control);
 }
 
-union xhci_trb *xhci_find_next_enqueue(struct xhci_ring *ring)
-{
-	/* Enqueue pointer can be left pointing to the link TRB,
-	 * we must handle that
-	 */
-	if (TRB_TYPE_LINK_LE32(ring->enqueue->link.control))
-		return ring->enq_seg->next->trbs;
-	return ring->enqueue;
-}
-
 /* Updates trb to point to the next TRB in the ring, and updates seg if the next
  * TRB is in a new segment.  This does not skip over link TRBs, and it does not
  * effect the ring dequeue or enqueue pointers.
@@ -301,17 +287,7 @@ static int xhci_abort_cmd_ring(struct xhci_hcd *xhci)
 
 	xhci_dbg(xhci, "Abort command ring\n");
 
-	if (!(xhci->cmd_ring_state & CMD_RING_STATE_RUNNING)) {
-		xhci_dbg(xhci, "The command ring isn't running, "
-				"Have the command ring been stopped?\n");
-		return 0;
-	}
-
 	temp_64 = xhci_read_64(xhci, &xhci->op_regs->cmd_ring);
-	if (!(temp_64 & CMD_RING_RUNNING)) {
-		xhci_dbg(xhci, "Command ring had been stopped\n");
-		return 0;
-	}
 	xhci->cmd_ring_state = CMD_RING_STATE_ABORTED;
 	xhci_write_64(xhci, temp_64 | CMD_RING_ABORT,
 			&xhci->op_regs->cmd_ring);
@@ -337,71 +313,6 @@ static int xhci_abort_cmd_ring(struct xhci_hcd *xhci)
 	return 0;
 }
 
-static int xhci_queue_cd(struct xhci_hcd *xhci,
-		struct xhci_command *command,
-		union xhci_trb *cmd_trb)
-{
-	struct xhci_cd *cd;
-	cd = kzalloc(sizeof(struct xhci_cd), GFP_ATOMIC);
-	if (!cd)
-		return -ENOMEM;
-	INIT_LIST_HEAD(&cd->cancel_cmd_list);
-
-	cd->command = command;
-	cd->cmd_trb = cmd_trb;
-	list_add_tail(&cd->cancel_cmd_list, &xhci->cancel_cmd_list);
-
-	return 0;
-}
-
-/*
- * Cancel the command which has issue.
- *
- * Some commands may hang due to waiting for acknowledgement from
- * usb device. It is outside of the xHC's ability to control and
- * will cause the command ring is blocked. When it occurs software
- * should intervene to recover the command ring.
- * See Section 4.6.1.1 and 4.6.1.2
- */
-int xhci_cancel_cmd(struct xhci_hcd *xhci, struct xhci_command *command,
-		union xhci_trb *cmd_trb)
-{
-	int retval = 0;
-	unsigned long flags;
-
-	spin_lock_irqsave(&xhci->lock, flags);
-
-	if (xhci->xhc_state & XHCI_STATE_DYING) {
-		xhci_warn(xhci, "Abort the command ring,"
-				" but the xHCI is dead.\n");
-		retval = -ESHUTDOWN;
-		goto fail;
-	}
-
-	/* queue the cmd desriptor to cancel_cmd_list */
-	retval = xhci_queue_cd(xhci, command, cmd_trb);
-	if (retval) {
-		xhci_warn(xhci, "Queuing command descriptor failed.\n");
-		goto fail;
-	}
-
-	/* abort command ring */
-	retval = xhci_abort_cmd_ring(xhci);
-	if (retval) {
-		xhci_err(xhci, "Abort command ring failed\n");
-		if (unlikely(retval == -ESHUTDOWN)) {
-			spin_unlock_irqrestore(&xhci->lock, flags);
-			usb_hc_died(xhci_to_hcd(xhci)->primary_hcd);
-			xhci_dbg(xhci, "xHCI host controller is dead.\n");
-			return retval;
-		}
-	}
-
-fail:
-	spin_unlock_irqrestore(&xhci->lock, flags);
-	return retval;
-}
-
 void xhci_ring_ep_doorbell(struct xhci_hcd *xhci,
 		unsigned int slot_id,
 		unsigned int ep_index,
@@ -550,6 +461,7 @@ void xhci_find_new_dequeue_state(struct xhci_hcd *xhci,
 	struct xhci_ring *ep_ring;
 	struct xhci_generic_trb *trb;
 	dma_addr_t addr;
+	u64 hw_dequeue;
 
 	ep_ring = xhci_triad_to_transfer_ring(xhci, slot_id,
 			ep_index, stream_id);
@@ -559,16 +471,6 @@ void xhci_find_new_dequeue_state(struct xhci_hcd *xhci,
 				stream_id);
 		return;
 	}
-	state->new_cycle_state = 0;
-	xhci_dbg_trace(xhci, trace_xhci_dbg_cancel_urb,
-			"Finding segment containing stopped TRB.");
-	state->new_deq_seg = find_trb_seg(cur_td->start_seg,
-			dev->eps[ep_index].stopped_trb,
-			&state->new_cycle_state);
-	if (!state->new_deq_seg) {
-		WARN_ON(1);
-		return;
-	}
 
 	/* Dig out the cycle state saved by the xHC during the stop ep cmd */
 	xhci_dbg_trace(xhci, trace_xhci_dbg_cancel_urb,
@@ -577,46 +479,57 @@ void xhci_find_new_dequeue_state(struct xhci_hcd *xhci,
 	if (ep->ep_state & EP_HAS_STREAMS) {
 		struct xhci_stream_ctx *ctx =
 			&ep->stream_info->stream_ctx_array[stream_id];
-		state->new_cycle_state = 0x1 & le64_to_cpu(ctx->stream_ring);
+		hw_dequeue = le64_to_cpu(ctx->stream_ring);
 	} else {
 		struct xhci_ep_ctx *ep_ctx
 			= xhci_get_ep_ctx(xhci, dev->out_ctx, ep_index);
-		state->new_cycle_state = 0x1 & le64_to_cpu(ep_ctx->deq);
+		hw_dequeue = le64_to_cpu(ep_ctx->deq);
+	}
+
+	/* Find virtual address and segment of hardware dequeue pointer */
+	state->new_deq_seg = ep_ring->deq_seg;
+	state->new_deq_ptr = ep_ring->dequeue;
+	while (xhci_trb_virt_to_dma(state->new_deq_seg, state->new_deq_ptr)
+			!= (dma_addr_t)(hw_dequeue & ~0xf)) {
+		next_trb(xhci, ep_ring, &state->new_deq_seg,
+					&state->new_deq_ptr);
+		if (state->new_deq_ptr == ep_ring->dequeue) {
+			WARN_ON(1);
+			return;
+		}
 	}
+	/*
+	 * Find cycle state for last_trb, starting at old cycle state of
+	 * hw_dequeue. If there is only one segment ring, find_trb_seg() will
+	 * return immediately and cannot toggle the cycle state if this search
+	 * wraps around, so add one more toggle manually in that case.
+	 */
+	state->new_cycle_state = hw_dequeue & 0x1;
+	if (ep_ring->first_seg == ep_ring->first_seg->next &&
+			cur_td->last_trb < state->new_deq_ptr)
+		state->new_cycle_state ^= 0x1;
 
 	state->new_deq_ptr = cur_td->last_trb;
 	xhci_dbg_trace(xhci, trace_xhci_dbg_cancel_urb,
 			"Finding segment containing last TRB in TD.");
 	state->new_deq_seg = find_trb_seg(state->new_deq_seg,
-			state->new_deq_ptr,
-			&state->new_cycle_state);
+			state->new_deq_ptr, &state->new_cycle_state);
 	if (!state->new_deq_seg) {
 		WARN_ON(1);
 		return;
 	}
 
+	/* Increment to find next TRB after last_trb. Cycle if appropriate. */
 	trb = &state->new_deq_ptr->generic;
 	if (TRB_TYPE_LINK_LE32(trb->field[3]) &&
 	    (trb->field[3] & cpu_to_le32(LINK_TOGGLE)))
 		state->new_cycle_state ^= 0x1;
 	next_trb(xhci, ep_ring, &state->new_deq_seg, &state->new_deq_ptr);
 
-	/*
-	 * If there is only one segment in a ring, find_trb_seg()'s while loop
-	 * will not run, and it will return before it has a chance to see if it
-	 * needs to toggle the cycle bit.  It can't tell if the stalled transfer
-	 * ended just before the link TRB on a one-segment ring, or if the TD
-	 * wrapped around the top of the ring, because it doesn't have the TD in
-	 * question.  Look for the one-segment case where stalled TRB's address
-	 * is greater than the new dequeue pointer address.
-	 */
-	if (ep_ring->first_seg == ep_ring->first_seg->next &&
-			state->new_deq_ptr < dev->eps[ep_index].stopped_trb)
-		state->new_cycle_state ^= 0x1;
+	/* Don't update the ring cycle state for the producer (us). */
 	xhci_dbg_trace(xhci, trace_xhci_dbg_cancel_urb,
 			"Cycle state = 0x%x", state->new_cycle_state);
 
-	/* Don't update the ring cycle state for the producer (us). */
 	xhci_dbg_trace(xhci, trace_xhci_dbg_cancel_urb,
 			"New dequeue segment = %p (virtual)",
 			state->new_deq_seg);
@@ -682,12 +595,14 @@ static void td_to_noop(struct xhci_hcd *xhci, struct xhci_ring *ep_ring,
 	}
 }
 
-static int queue_set_tr_deq(struct xhci_hcd *xhci, int slot_id,
+static int queue_set_tr_deq(struct xhci_hcd *xhci,
+		struct xhci_command *cmd, int slot_id,
 		unsigned int ep_index, unsigned int stream_id,
 		struct xhci_segment *deq_seg,
 		union xhci_trb *deq_ptr, u32 cycle_state);
 
 void xhci_queue_new_dequeue_state(struct xhci_hcd *xhci,
+		struct xhci_command *cmd,
 		unsigned int slot_id, unsigned int ep_index,
 		unsigned int stream_id,
 		struct xhci_dequeue_state *deq_state)
@@ -702,7 +617,7 @@ void xhci_queue_new_dequeue_state(struct xhci_hcd *xhci,
 			deq_state->new_deq_ptr,
 			(unsigned long long)xhci_trb_virt_to_dma(deq_state->new_deq_seg, deq_state->new_deq_ptr),
 			deq_state->new_cycle_state);
-	queue_set_tr_deq(xhci, slot_id, ep_index, stream_id,
+	queue_set_tr_deq(xhci, cmd, slot_id, ep_index, stream_id,
 			deq_state->new_deq_seg,
 			deq_state->new_deq_ptr,
 			(u32) deq_state->new_cycle_state);
@@ -771,7 +686,6 @@ static void xhci_handle_cmd_stop_ep(struct xhci_hcd *xhci, int slot_id,
 		union xhci_trb *trb, struct xhci_event_cmd *event)
 {
 	unsigned int ep_index;
-	struct xhci_virt_device *virt_dev;
 	struct xhci_ring *ep_ring;
 	struct xhci_virt_ep *ep;
 	struct list_head *entry;
@@ -781,11 +695,7 @@ static void xhci_handle_cmd_stop_ep(struct xhci_hcd *xhci, int slot_id,
 	struct xhci_dequeue_state deq_state;
 
 	if (unlikely(TRB_TO_SUSPEND_PORT(le32_to_cpu(trb->generic.field[3])))) {
-		virt_dev = xhci->devs[slot_id];
-		if (virt_dev)
-			handle_cmd_in_cmd_wait_list(xhci, virt_dev,
-				event);
-		else
+		if (!xhci->devs[slot_id])
 			xhci_warn(xhci, "Stop endpoint command "
 				"completion for disabled slot %u\n",
 				slot_id);
@@ -799,7 +709,6 @@ static void xhci_handle_cmd_stop_ep(struct xhci_hcd *xhci, int slot_id,
 	if (list_empty(&ep->cancelled_td_list)) {
 		xhci_stop_watchdog_timer_in_irq(xhci, ep);
 		ep->stopped_td = NULL;
-		ep->stopped_trb = NULL;
 		ring_doorbell_for_active_rings(xhci, slot_id, ep_index);
 		return;
 	}
@@ -857,7 +766,9 @@ remove_finished_td:
 
 	/* If necessary, queue a Set Transfer Ring Dequeue Pointer command */
 	if (deq_state.new_deq_ptr && deq_state.new_deq_seg) {
-		xhci_queue_new_dequeue_state(xhci,
+		struct xhci_command *command;
+		command = xhci_alloc_command(xhci, false, false, GFP_ATOMIC);
+		xhci_queue_new_dequeue_state(xhci, command,
 				slot_id, ep_index,
 				ep->stopped_td->urb->stream_id,
 				&deq_state);
@@ -867,11 +778,9 @@ remove_finished_td:
 		ring_doorbell_for_active_rings(xhci, slot_id, ep_index);
 	}
 
-	/* Clear stopped_td and stopped_trb if endpoint is not halted */
-	if (!(ep->ep_state & EP_HALTED)) {
+	/* Clear stopped_td if endpoint is not halted */
+	if (!(ep->ep_state & EP_HALTED))
 		ep->stopped_td = NULL;
-		ep->stopped_trb = NULL;
-	}
 
 	/*
 	 * Drop the lock and complete the URBs in the cancelled TD list.
@@ -1207,9 +1116,11 @@ static void xhci_handle_cmd_reset_ep(struct xhci_hcd *xhci, int slot_id,
 	 * because the HW can't handle two commands being queued in a row.
 	 */
 	if (xhci->quirks & XHCI_RESET_EP_QUIRK) {
+		struct xhci_command *command;
+		command = xhci_alloc_command(xhci, false, false, GFP_ATOMIC);
 		xhci_dbg_trace(xhci, trace_xhci_dbg_quirks,
 				"Queueing configure endpoint command");
-		xhci_queue_configure_endpoint(xhci,
+		xhci_queue_configure_endpoint(xhci, command,
 				xhci->devs[slot_id]->in_ctx->dma, slot_id,
 				false);
 		xhci_ring_cmd_db(xhci);
@@ -1220,187 +1131,6 @@ static void xhci_handle_cmd_reset_ep(struct xhci_hcd *xhci, int slot_id,
 	}
 }
 
-/* Complete the command and detele it from the devcie's command queue.
- */
-static void xhci_complete_cmd_in_cmd_wait_list(struct xhci_hcd *xhci,
-		struct xhci_command *command, u32 status)
-{
-	command->status = status;
-	list_del(&command->cmd_list);
-	if (command->completion)
-		complete(command->completion);
-	else
-		xhci_free_command(xhci, command);
-}
-
-
-/* Check to see if a command in the device's command queue matches this one.
- * Signal the completion or free the command, and return 1.  Return 0 if the
- * completed command isn't at the head of the command list.
- */
-static int handle_cmd_in_cmd_wait_list(struct xhci_hcd *xhci,
-		struct xhci_virt_device *virt_dev,
-		struct xhci_event_cmd *event)
-{
-	struct xhci_command *command;
-
-	if (list_empty(&virt_dev->cmd_list))
-		return 0;
-
-	command = list_entry(virt_dev->cmd_list.next,
-			struct xhci_command, cmd_list);
-	if (xhci->cmd_ring->dequeue != command->command_trb)
-		return 0;
-
-	xhci_complete_cmd_in_cmd_wait_list(xhci, command,
-			GET_COMP_CODE(le32_to_cpu(event->status)));
-	return 1;
-}
-
-/*
- * Finding the command trb need to be cancelled and modifying it to
- * NO OP command. And if the command is in device's command wait
- * list, finishing and freeing it.
- *
- * If we can't find the command trb, we think it had already been
- * executed.
- */
-static void xhci_cmd_to_noop(struct xhci_hcd *xhci, struct xhci_cd *cur_cd)
-{
-	struct xhci_segment *cur_seg;
-	union xhci_trb *cmd_trb;
-	u32 cycle_state;
-
-	if (xhci->cmd_ring->dequeue == xhci->cmd_ring->enqueue)
-		return;
-
-	/* find the current segment of command ring */
-	cur_seg = find_trb_seg(xhci->cmd_ring->first_seg,
-			xhci->cmd_ring->dequeue, &cycle_state);
-
-	if (!cur_seg) {
-		xhci_warn(xhci, "Command ring mismatch, dequeue = %p %llx (dma)\n",
-				xhci->cmd_ring->dequeue,
-				(unsigned long long)
-				xhci_trb_virt_to_dma(xhci->cmd_ring->deq_seg,
-					xhci->cmd_ring->dequeue));
-		xhci_debug_ring(xhci, xhci->cmd_ring);
-		xhci_dbg_ring_ptrs(xhci, xhci->cmd_ring);
-		return;
-	}
-
-	/* find the command trb matched by cd from command ring */
-	for (cmd_trb = xhci->cmd_ring->dequeue;
-			cmd_trb != xhci->cmd_ring->enqueue;
-			next_trb(xhci, xhci->cmd_ring, &cur_seg, &cmd_trb)) {
-		/* If the trb is link trb, continue */
-		if (TRB_TYPE_LINK_LE32(cmd_trb->generic.field[3]))
-			continue;
-
-		if (cur_cd->cmd_trb == cmd_trb) {
-
-			/* If the command in device's command list, we should
-			 * finish it and free the command structure.
-			 */
-			if (cur_cd->command)
-				xhci_complete_cmd_in_cmd_wait_list(xhci,
-					cur_cd->command, COMP_CMD_STOP);
-
-			/* get cycle state from the origin command trb */
-			cycle_state = le32_to_cpu(cmd_trb->generic.field[3])
-				& TRB_CYCLE;
-
-			/* modify the command trb to NO OP command */
-			cmd_trb->generic.field[0] = 0;
-			cmd_trb->generic.field[1] = 0;
-			cmd_trb->generic.field[2] = 0;
-			cmd_trb->generic.field[3] = cpu_to_le32(
-					TRB_TYPE(TRB_CMD_NOOP) | cycle_state);
-			break;
-		}
-	}
-}
-
-static void xhci_cancel_cmd_in_cd_list(struct xhci_hcd *xhci)
-{
-	struct xhci_cd *cur_cd, *next_cd;
-
-	if (list_empty(&xhci->cancel_cmd_list))
-		return;
-
-	list_for_each_entry_safe(cur_cd, next_cd,
-			&xhci->cancel_cmd_list, cancel_cmd_list) {
-		xhci_cmd_to_noop(xhci, cur_cd);
-		list_del(&cur_cd->cancel_cmd_list);
-		kfree(cur_cd);
-	}
-}
-
-/*
- * traversing the cancel_cmd_list. If the command descriptor according
- * to cmd_trb is found, the function free it and return 1, otherwise
- * return 0.
- */
-static int xhci_search_cmd_trb_in_cd_list(struct xhci_hcd *xhci,
-		union xhci_trb *cmd_trb)
-{
-	struct xhci_cd *cur_cd, *next_cd;
-
-	if (list_empty(&xhci->cancel_cmd_list))
-		return 0;
-
-	list_for_each_entry_safe(cur_cd, next_cd,
-			&xhci->cancel_cmd_list, cancel_cmd_list) {
-		if (cur_cd->cmd_trb == cmd_trb) {
-			if (cur_cd->command)
-				xhci_complete_cmd_in_cmd_wait_list(xhci,
-					cur_cd->command, COMP_CMD_STOP);
-			list_del(&cur_cd->cancel_cmd_list);
-			kfree(cur_cd);
-			return 1;
-		}
-	}
-
-	return 0;
-}
-
-/*
- * If the cmd_trb_comp_code is COMP_CMD_ABORT, we just check whether the
- * trb pointed by the command ring dequeue pointer is the trb we want to
- * cancel or not. And if the cmd_trb_comp_code is COMP_CMD_STOP, we will
- * traverse the cancel_cmd_list to trun the all of the commands according
- * to command descriptor to NO-OP trb.
- */
-static int handle_stopped_cmd_ring(struct xhci_hcd *xhci,
-		int cmd_trb_comp_code)
-{
-	int cur_trb_is_good = 0;
-
-	/* Searching the cmd trb pointed by the command ring dequeue
-	 * pointer in command descriptor list. If it is found, free it.
-	 */
-	cur_trb_is_good = xhci_search_cmd_trb_in_cd_list(xhci,
-			xhci->cmd_ring->dequeue);
-
-	if (cmd_trb_comp_code == COMP_CMD_ABORT)
-		xhci->cmd_ring_state = CMD_RING_STATE_STOPPED;
-	else if (cmd_trb_comp_code == COMP_CMD_STOP) {
-		/* traversing the cancel_cmd_list and canceling
-		 * the command according to command descriptor
-		 */
-		xhci_cancel_cmd_in_cd_list(xhci);
-
-		xhci->cmd_ring_state = CMD_RING_STATE_RUNNING;
-		/*
-		 * ring command ring doorbell again to restart the
-		 * command ring
-		 */
-		if (xhci->cmd_ring->dequeue != xhci->cmd_ring->enqueue)
-			xhci_ring_cmd_db(xhci);
-	}
-	return cur_trb_is_good;
-}
-
 static void xhci_handle_cmd_enable_slot(struct xhci_hcd *xhci, int slot_id,
 		u32 cmd_comp_code)
 {
@@ -1408,7 +1138,6 @@ static void xhci_handle_cmd_enable_slot(struct xhci_hcd *xhci, int slot_id,
 		xhci->slot_id = slot_id;
 	else
 		xhci->slot_id = 0;
-	complete(&xhci->addr_dev);
 }
 
 static void xhci_handle_cmd_disable_slot(struct xhci_hcd *xhci, int slot_id)
@@ -1433,9 +1162,6 @@ static void xhci_handle_cmd_config_ep(struct xhci_hcd *xhci, int slot_id,
 	unsigned int ep_state;
 	u32 add_flags, drop_flags;
 
-	virt_dev = xhci->devs[slot_id];
-	if (handle_cmd_in_cmd_wait_list(xhci, virt_dev, event))
-		return;
 	/*
 	 * Configure endpoint commands can come from the USB core
 	 * configuration or alt setting changes, or because the HW
@@ -1444,6 +1170,7 @@ static void xhci_handle_cmd_config_ep(struct xhci_hcd *xhci, int slot_id,
 	 * If the command was for a halted endpoint, the xHCI driver
 	 * is not waiting on the configure endpoint command.
 	 */
+	virt_dev = xhci->devs[slot_id];
 	ctrl_ctx = xhci_get_input_control_ctx(xhci, virt_dev->in_ctx);
 	if (!ctrl_ctx) {
 		xhci_warn(xhci, "Could not get input context, bad type.\n");
@@ -1466,7 +1193,7 @@ static void xhci_handle_cmd_config_ep(struct xhci_hcd *xhci, int slot_id,
 			add_flags - SLOT_FLAG == drop_flags) {
 		ep_state = virt_dev->eps[ep_index].ep_state;
 		if (!(ep_state & EP_HALTED))
-			goto bandwidth_change;
+			return;
 		xhci_dbg_trace(xhci, trace_xhci_dbg_quirks,
 				"Completed config ep cmd - "
 				"last ep index = %d, state = %d",
@@ -1476,43 +1203,14 @@ static void xhci_handle_cmd_config_ep(struct xhci_hcd *xhci, int slot_id,
 		ring_doorbell_for_active_rings(xhci, slot_id, ep_index);
 		return;
 	}
-bandwidth_change:
-	xhci_dbg_trace(xhci,  trace_xhci_dbg_context_change,
-			"Completed config ep cmd");
-	virt_dev->cmd_status = cmd_comp_code;
-	complete(&virt_dev->cmd_completion);
 	return;
 }
 
-static void xhci_handle_cmd_eval_ctx(struct xhci_hcd *xhci, int slot_id,
-		struct xhci_event_cmd *event, u32 cmd_comp_code)
-{
-	struct xhci_virt_device *virt_dev;
-
-	virt_dev = xhci->devs[slot_id];
-	if (handle_cmd_in_cmd_wait_list(xhci, virt_dev, event))
-		return;
-	virt_dev->cmd_status = cmd_comp_code;
-	complete(&virt_dev->cmd_completion);
-}
-
-static void xhci_handle_cmd_addr_dev(struct xhci_hcd *xhci, int slot_id,
-		u32 cmd_comp_code)
-{
-	xhci->devs[slot_id]->cmd_status = cmd_comp_code;
-	complete(&xhci->addr_dev);
-}
-
 static void xhci_handle_cmd_reset_dev(struct xhci_hcd *xhci, int slot_id,
 		struct xhci_event_cmd *event)
 {
-	struct xhci_virt_device *virt_dev;
-
 	xhci_dbg(xhci, "Completed reset device command.\n");
-	virt_dev = xhci->devs[slot_id];
-	if (virt_dev)
-		handle_cmd_in_cmd_wait_list(xhci, virt_dev, event);
-	else
+	if (!xhci->devs[slot_id])
 		xhci_warn(xhci, "Reset device command completion "
 				"for disabled slot %u\n", slot_id);
 }
@@ -1530,6 +1228,116 @@ static void xhci_handle_cmd_nec_get_fw(struct xhci_hcd *xhci,
 			NEC_FW_MINOR(le32_to_cpu(event->status)));
 }
 
+static void xhci_complete_del_and_free_cmd(struct xhci_command *cmd, u32 status)
+{
+	list_del(&cmd->cmd_list);
+
+	if (cmd->completion) {
+		cmd->status = status;
+		complete(cmd->completion);
+	} else {
+		kfree(cmd);
+	}
+}
+
+void xhci_cleanup_command_queue(struct xhci_hcd *xhci)
+{
+	struct xhci_command *cur_cmd, *tmp_cmd;
+	list_for_each_entry_safe(cur_cmd, tmp_cmd, &xhci->cmd_list, cmd_list)
+		xhci_complete_del_and_free_cmd(cur_cmd, COMP_CMD_ABORT);
+}
+
+/*
+ * Turn all commands on command ring with status set to "aborted" to no-op trbs.
+ * If there are other commands waiting then restart the ring and kick the timer.
+ * This must be called with command ring stopped and xhci->lock held.
+ */
+static void xhci_handle_stopped_cmd_ring(struct xhci_hcd *xhci,
+					 struct xhci_command *cur_cmd)
+{
+	struct xhci_command *i_cmd, *tmp_cmd;
+	u32 cycle_state;
+
+	/* Turn all aborted commands in list to no-ops, then restart */
+	list_for_each_entry_safe(i_cmd, tmp_cmd, &xhci->cmd_list,
+				 cmd_list) {
+
+		if (i_cmd->status != COMP_CMD_ABORT)
+			continue;
+
+		i_cmd->status = COMP_CMD_STOP;
+
+		xhci_dbg(xhci, "Turn aborted command %p to no-op\n",
+			 i_cmd->command_trb);
+		/* get cycle state from the original cmd trb */
+		cycle_state = le32_to_cpu(
+			i_cmd->command_trb->generic.field[3]) &	TRB_CYCLE;
+		/* modify the command trb to no-op command */
+		i_cmd->command_trb->generic.field[0] = 0;
+		i_cmd->command_trb->generic.field[1] = 0;
+		i_cmd->command_trb->generic.field[2] = 0;
+		i_cmd->command_trb->generic.field[3] = cpu_to_le32(
+			TRB_TYPE(TRB_CMD_NOOP) | cycle_state);
+
+		/*
+		 * caller waiting for completion is called when command
+		 *  completion event is received for these no-op commands
+		 */
+	}
+
+	xhci->cmd_ring_state = CMD_RING_STATE_RUNNING;
+
+	/* ring command ring doorbell to restart the command ring */
+	if ((xhci->cmd_ring->dequeue != xhci->cmd_ring->enqueue) &&
+	    !(xhci->xhc_state & XHCI_STATE_DYING)) {
+		xhci->current_cmd = cur_cmd;
+		mod_timer(&xhci->cmd_timer, jiffies + XHCI_CMD_DEFAULT_TIMEOUT);
+		xhci_ring_cmd_db(xhci);
+	}
+	return;
+}
+
+
+void xhci_handle_command_timeout(unsigned long data)
+{
+	struct xhci_hcd *xhci;
+	int ret;
+	unsigned long flags;
+	u64 hw_ring_state;
+	struct xhci_command *cur_cmd = NULL;
+	xhci = (struct xhci_hcd *) data;
+
+	/* mark this command to be cancelled */
+	spin_lock_irqsave(&xhci->lock, flags);
+	if (xhci->current_cmd) {
+		cur_cmd = xhci->current_cmd;
+		cur_cmd->status = COMP_CMD_ABORT;
+	}
+
+
+	/* Make sure command ring is running before aborting it */
+	hw_ring_state = xhci_read_64(xhci, &xhci->op_regs->cmd_ring);
+	if ((xhci->cmd_ring_state & CMD_RING_STATE_RUNNING) &&
+	    (hw_ring_state & CMD_RING_RUNNING))  {
+
+		spin_unlock_irqrestore(&xhci->lock, flags);
+		xhci_dbg(xhci, "Command timeout\n");
+		ret = xhci_abort_cmd_ring(xhci);
+		if (unlikely(ret == -ESHUTDOWN)) {
+			xhci_err(xhci, "Abort command ring failed\n");
+			xhci_cleanup_command_queue(xhci);
+			usb_hc_died(xhci_to_hcd(xhci)->primary_hcd);
+			xhci_dbg(xhci, "xHCI host controller is dead.\n");
+		}
+		return;
+	}
+	/* command timeout on stopped ring, ring can't be aborted */
+	xhci_dbg(xhci, "Command timeout on stopped ring\n");
+	xhci_handle_stopped_cmd_ring(xhci, xhci->current_cmd);
+	spin_unlock_irqrestore(&xhci->lock, flags);
+	return;
+}
+
 static void handle_cmd_completion(struct xhci_hcd *xhci,
 		struct xhci_event_cmd *event)
 {
@@ -1538,6 +1346,7 @@ static void handle_cmd_completion(struct xhci_hcd *xhci,
 	dma_addr_t cmd_dequeue_dma;
 	u32 cmd_comp_code;
 	union xhci_trb *cmd_trb;
+	struct xhci_command *cmd;
 	u32 cmd_type;
 
 	cmd_dma = le64_to_cpu(event->cmd_trb);
@@ -1555,26 +1364,35 @@ static void handle_cmd_completion(struct xhci_hcd *xhci,
 		return;
 	}
 
+	cmd = list_entry(xhci->cmd_list.next, struct xhci_command, cmd_list);
+
+	if (cmd->command_trb != xhci->cmd_ring->dequeue) {
+		xhci_err(xhci,
+			 "Command completion event does not match command\n");
+		return;
+	}
+
+	del_timer(&xhci->cmd_timer);
+
 	trace_xhci_cmd_completion(cmd_trb, (struct xhci_generic_trb *) event);
 
 	cmd_comp_code = GET_COMP_CODE(le32_to_cpu(event->status));
-	if (cmd_comp_code == COMP_CMD_ABORT || cmd_comp_code == COMP_CMD_STOP) {
-		/* If the return value is 0, we think the trb pointed by
-		 * command ring dequeue pointer is a good trb. The good
-		 * trb means we don't want to cancel the trb, but it have
-		 * been stopped by host. So we should handle it normally.
-		 * Otherwise, driver should invoke inc_deq() and return.
-		 */
-		if (handle_stopped_cmd_ring(xhci, cmd_comp_code)) {
-			inc_deq(xhci, xhci->cmd_ring);
-			return;
-		}
-		/* There is no command to handle if we get a stop event when the
-		 * command ring is empty, event->cmd_trb points to the next
-		 * unset command
-		 */
-		if (xhci->cmd_ring->dequeue == xhci->cmd_ring->enqueue)
-			return;
+
+	/* If CMD ring stopped we own the trbs between enqueue and dequeue */
+	if (cmd_comp_code == COMP_CMD_STOP) {
+		xhci_handle_stopped_cmd_ring(xhci, cmd);
+		return;
+	}
+	/*
+	 * Host aborted the command ring, check if the current command was
+	 * supposed to be aborted, otherwise continue normally.
+	 * The command ring is stopped now, but the xHC will issue a Command
+	 * Ring Stopped event which will cause us to restart it.
+	 */
+	if (cmd_comp_code == COMP_CMD_ABORT) {
+		xhci->cmd_ring_state = CMD_RING_STATE_STOPPED;
+		if (cmd->status == COMP_CMD_ABORT)
+			goto event_handled;
 	}
 
 	cmd_type = TRB_FIELD_TO_TYPE(le32_to_cpu(cmd_trb->generic.field[3]));
@@ -1586,13 +1404,13 @@ static void handle_cmd_completion(struct xhci_hcd *xhci,
 		xhci_handle_cmd_disable_slot(xhci, slot_id);
 		break;
 	case TRB_CONFIG_EP:
-		xhci_handle_cmd_config_ep(xhci, slot_id, event, cmd_comp_code);
+		if (!cmd->completion)
+			xhci_handle_cmd_config_ep(xhci, slot_id, event,
+						  cmd_comp_code);
 		break;
 	case TRB_EVAL_CONTEXT:
-		xhci_handle_cmd_eval_ctx(xhci, slot_id, event, cmd_comp_code);
 		break;
 	case TRB_ADDR_DEV:
-		xhci_handle_cmd_addr_dev(xhci, slot_id, cmd_comp_code);
 		break;
 	case TRB_STOP_RING:
 		WARN_ON(slot_id != TRB_TO_SLOT_ID(
@@ -1605,6 +1423,9 @@ static void handle_cmd_completion(struct xhci_hcd *xhci,
 		xhci_handle_cmd_set_deq(xhci, slot_id, cmd_trb, cmd_comp_code);
 		break;
 	case TRB_CMD_NOOP:
+		/* Is this an aborted command turned to NO-OP? */
+		if (cmd->status == COMP_CMD_STOP)
+			cmd_comp_code = COMP_CMD_STOP;
 		break;
 	case TRB_RESET_EP:
 		WARN_ON(slot_id != TRB_TO_SLOT_ID(
@@ -1624,6 +1445,17 @@ static void handle_cmd_completion(struct xhci_hcd *xhci,
 		xhci->error_bitmask |= 1 << 6;
 		break;
 	}
+
+	/* restart timer if this wasn't the last command */
+	if (cmd->cmd_list.next != &xhci->cmd_list) {
+		xhci->current_cmd = list_entry(cmd->cmd_list.next,
+					       struct xhci_command, cmd_list);
+		mod_timer(&xhci->cmd_timer, jiffies + XHCI_CMD_DEFAULT_TIMEOUT);
+	}
+
+event_handled:
+	xhci_complete_del_and_free_cmd(cmd, cmd_comp_code);
+
 	inc_deq(xhci, xhci->cmd_ring);
 }
 
@@ -1939,16 +1771,19 @@ static void xhci_cleanup_halted_endpoint(struct xhci_hcd *xhci,
 		struct xhci_td *td, union xhci_trb *event_trb)
 {
 	struct xhci_virt_ep *ep = &xhci->devs[slot_id]->eps[ep_index];
+	struct xhci_command *command;
+	command = xhci_alloc_command(xhci, false, false, GFP_ATOMIC);
+	if (!command)
+		return;
+
 	ep->ep_state |= EP_HALTED;
 	ep->stopped_td = td;
-	ep->stopped_trb = event_trb;
 	ep->stopped_stream = stream_id;
 
-	xhci_queue_reset_ep(xhci, slot_id, ep_index);
+	xhci_queue_reset_ep(xhci, command, slot_id, ep_index);
 	xhci_cleanup_stalled_ring(xhci, td->urb->dev, ep_index);
 
 	ep->stopped_td = NULL;
-	ep->stopped_trb = NULL;
 	ep->stopped_stream = 0;
 
 	xhci_ring_cmd_db(xhci);
@@ -2030,7 +1865,6 @@ static int finish_td(struct xhci_hcd *xhci, struct xhci_td *td,
 		 * the ring dequeue pointer or take this TD off any lists yet.
 		 */
 		ep->stopped_td = td;
-		ep->stopped_trb = event_trb;
 		return 0;
 	} else {
 		if (trb_comp_code == COMP_STALL) {
@@ -2042,7 +1876,6 @@ static int finish_td(struct xhci_hcd *xhci, struct xhci_td *td,
 			 * USB class driver clear the stall later.
 			 */
 			ep->stopped_td = td;
-			ep->stopped_trb = event_trb;
 			ep->stopped_stream = ep_ring->stream_id;
 		} else if (xhci_requires_manual_halt_cleanup(xhci,
 					ep_ctx, trb_comp_code)) {
@@ -2659,7 +2492,7 @@ static int handle_tx_event(struct xhci_hcd *xhci,
 				 * successful event after a short transfer.
 				 * Ignore it.
 				 */
-				if ((xhci->quirks & XHCI_SPURIOUS_SUCCESS) && 
+				if ((xhci->quirks & XHCI_SPURIOUS_SUCCESS) &&
 						ep_ring->last_td_was_short) {
 					ep_ring->last_td_was_short = false;
 					ret = 0;
@@ -4001,11 +3834,14 @@ int xhci_queue_isoc_tx_prepare(struct xhci_hcd *xhci, gfp_t mem_flags,
  * Don't decrement xhci->cmd_ring_reserved_trbs after we've queued the TRB
  * because the command event handler may want to resubmit a failed command.
  */
-static int queue_command(struct xhci_hcd *xhci, u32 field1, u32 field2,
-		u32 field3, u32 field4, bool command_must_succeed)
+static int queue_command(struct xhci_hcd *xhci, struct xhci_command *cmd,
+			 u32 field1, u32 field2,
+			 u32 field3, u32 field4, bool command_must_succeed)
 {
 	int reserved_trbs = xhci->cmd_ring_reserved_trbs;
 	int ret;
+	if (xhci->xhc_state & XHCI_STATE_DYING)
+		return -ESHUTDOWN;
 
 	if (!command_must_succeed)
 		reserved_trbs++;
@@ -4019,57 +3855,71 @@ static int queue_command(struct xhci_hcd *xhci, u32 field1, u32 field2,
 					"unfailable commands failed.\n");
 		return ret;
 	}
+
+	cmd->command_trb = xhci->cmd_ring->enqueue;
+	list_add_tail(&cmd->cmd_list, &xhci->cmd_list);
+
+	/* if there are no other commands queued we start the timeout timer */
+	if (xhci->cmd_list.next == &cmd->cmd_list &&
+	    !timer_pending(&xhci->cmd_timer)) {
+		xhci->current_cmd = cmd;
+		mod_timer(&xhci->cmd_timer, jiffies + XHCI_CMD_DEFAULT_TIMEOUT);
+	}
+
 	queue_trb(xhci, xhci->cmd_ring, false, field1, field2, field3,
 			field4 | xhci->cmd_ring->cycle_state);
 	return 0;
 }
 
 /* Queue a slot enable or disable request on the command ring */
-int xhci_queue_slot_control(struct xhci_hcd *xhci, u32 trb_type, u32 slot_id)
+int xhci_queue_slot_control(struct xhci_hcd *xhci, struct xhci_command *cmd,
+		u32 trb_type, u32 slot_id)
 {
-	return queue_command(xhci, 0, 0, 0,
+	return queue_command(xhci, cmd, 0, 0, 0,
 			TRB_TYPE(trb_type) | SLOT_ID_FOR_TRB(slot_id), false);
 }
 
 /* Queue an address device command TRB */
-int xhci_queue_address_device(struct xhci_hcd *xhci, dma_addr_t in_ctx_ptr,
-			      u32 slot_id, enum xhci_setup_dev setup)
+int xhci_queue_address_device(struct xhci_hcd *xhci, struct xhci_command *cmd,
+		dma_addr_t in_ctx_ptr, u32 slot_id, enum xhci_setup_dev setup)
 {
-	return queue_command(xhci, lower_32_bits(in_ctx_ptr),
+	return queue_command(xhci, cmd, lower_32_bits(in_ctx_ptr),
 			upper_32_bits(in_ctx_ptr), 0,
 			TRB_TYPE(TRB_ADDR_DEV) | SLOT_ID_FOR_TRB(slot_id)
 			| (setup == SETUP_CONTEXT_ONLY ? TRB_BSR : 0), false);
 }
 
-int xhci_queue_vendor_command(struct xhci_hcd *xhci,
+int xhci_queue_vendor_command(struct xhci_hcd *xhci, struct xhci_command *cmd,
 		u32 field1, u32 field2, u32 field3, u32 field4)
 {
-	return queue_command(xhci, field1, field2, field3, field4, false);
+	return queue_command(xhci, cmd, field1, field2, field3, field4, false);
 }
 
 /* Queue a reset device command TRB */
-int xhci_queue_reset_device(struct xhci_hcd *xhci, u32 slot_id)
+int xhci_queue_reset_device(struct xhci_hcd *xhci, struct xhci_command *cmd,
+		u32 slot_id)
 {
-	return queue_command(xhci, 0, 0, 0,
+	return queue_command(xhci, cmd, 0, 0, 0,
 			TRB_TYPE(TRB_RESET_DEV) | SLOT_ID_FOR_TRB(slot_id),
 			false);
 }
 
 /* Queue a configure endpoint command TRB */
-int xhci_queue_configure_endpoint(struct xhci_hcd *xhci, dma_addr_t in_ctx_ptr,
+int xhci_queue_configure_endpoint(struct xhci_hcd *xhci,
+		struct xhci_command *cmd, dma_addr_t in_ctx_ptr,
 		u32 slot_id, bool command_must_succeed)
 {
-	return queue_command(xhci, lower_32_bits(in_ctx_ptr),
+	return queue_command(xhci, cmd, lower_32_bits(in_ctx_ptr),
 			upper_32_bits(in_ctx_ptr), 0,
 			TRB_TYPE(TRB_CONFIG_EP) | SLOT_ID_FOR_TRB(slot_id),
 			command_must_succeed);
 }
 
 /* Queue an evaluate context command TRB */
-int xhci_queue_evaluate_context(struct xhci_hcd *xhci, dma_addr_t in_ctx_ptr,
-		u32 slot_id, bool command_must_succeed)
+int xhci_queue_evaluate_context(struct xhci_hcd *xhci, struct xhci_command *cmd,
+		dma_addr_t in_ctx_ptr, u32 slot_id, bool command_must_succeed)
 {
-	return queue_command(xhci, lower_32_bits(in_ctx_ptr),
+	return queue_command(xhci, cmd, lower_32_bits(in_ctx_ptr),
 			upper_32_bits(in_ctx_ptr), 0,
 			TRB_TYPE(TRB_EVAL_CONTEXT) | SLOT_ID_FOR_TRB(slot_id),
 			command_must_succeed);
@@ -4079,25 +3929,26 @@ int xhci_queue_evaluate_context(struct xhci_hcd *xhci, dma_addr_t in_ctx_ptr,
  * Suspend is set to indicate "Stop Endpoint Command" is being issued to stop
  * activity on an endpoint that is about to be suspended.
  */
-int xhci_queue_stop_endpoint(struct xhci_hcd *xhci, int slot_id,
-		unsigned int ep_index, int suspend)
+int xhci_queue_stop_endpoint(struct xhci_hcd *xhci, struct xhci_command *cmd,
+			     int slot_id, unsigned int ep_index, int suspend)
 {
 	u32 trb_slot_id = SLOT_ID_FOR_TRB(slot_id);
 	u32 trb_ep_index = EP_ID_FOR_TRB(ep_index);
 	u32 type = TRB_TYPE(TRB_STOP_RING);
 	u32 trb_suspend = SUSPEND_PORT_FOR_TRB(suspend);
 
-	return queue_command(xhci, 0, 0, 0,
+	return queue_command(xhci, cmd, 0, 0, 0,
 			trb_slot_id | trb_ep_index | type | trb_suspend, false);
 }
 
 /* Set Transfer Ring Dequeue Pointer command.
  * This should not be used for endpoints that have streams enabled.
  */
-static int queue_set_tr_deq(struct xhci_hcd *xhci, int slot_id,
-		unsigned int ep_index, unsigned int stream_id,
-		struct xhci_segment *deq_seg,
-		union xhci_trb *deq_ptr, u32 cycle_state)
+static int queue_set_tr_deq(struct xhci_hcd *xhci, struct xhci_command *cmd,
+			int slot_id,
+			unsigned int ep_index, unsigned int stream_id,
+			struct xhci_segment *deq_seg,
+			union xhci_trb *deq_ptr, u32 cycle_state)
 {
 	dma_addr_t addr;
 	u32 trb_slot_id = SLOT_ID_FOR_TRB(slot_id);
@@ -4124,18 +3975,19 @@ static int queue_set_tr_deq(struct xhci_hcd *xhci, int slot_id,
 	ep->queued_deq_ptr = deq_ptr;
 	if (stream_id)
 		trb_sct = SCT_FOR_TRB(SCT_PRI_TR);
-	return queue_command(xhci, lower_32_bits(addr) | trb_sct | cycle_state,
+	return queue_command(xhci, cmd,
+			lower_32_bits(addr) | trb_sct | cycle_state,
 			upper_32_bits(addr), trb_stream_id,
 			trb_slot_id | trb_ep_index | type, false);
 }
 
-int xhci_queue_reset_ep(struct xhci_hcd *xhci, int slot_id,
-		unsigned int ep_index)
+int xhci_queue_reset_ep(struct xhci_hcd *xhci, struct xhci_command *cmd,
+			int slot_id, unsigned int ep_index)
 {
 	u32 trb_slot_id = SLOT_ID_FOR_TRB(slot_id);
 	u32 trb_ep_index = EP_ID_FOR_TRB(ep_index);
 	u32 type = TRB_TYPE(TRB_RESET_EP);
 
-	return queue_command(xhci, 0, 0, 0, trb_slot_id | trb_ep_index | type,
-			false);
+	return queue_command(xhci, cmd, 0, 0, 0,
+			trb_slot_id | trb_ep_index | type, false);
 }
diff --git a/drivers/usb/host/xhci.c b/drivers/usb/host/xhci.c
index 8fe4e124ddd..2b8d9a24af0 100644
--- a/drivers/usb/host/xhci.c
+++ b/drivers/usb/host/xhci.c
@@ -291,7 +291,7 @@ static int xhci_setup_msix(struct xhci_hcd *xhci)
 		xhci->msix_entries[i].vector = 0;
 	}
 
-	ret = pci_enable_msix(pdev, xhci->msix_entries, xhci->msix_count);
+	ret = pci_enable_msix_exact(pdev, xhci->msix_entries, xhci->msix_count);
 	if (ret) {
 		xhci_dbg_trace(xhci, trace_xhci_dbg_init,
 				"Failed to enable MSI-X");
@@ -408,16 +408,16 @@ static int xhci_try_enable_msi(struct usb_hcd *hcd)
 
 #else
 
-static int xhci_try_enable_msi(struct usb_hcd *hcd)
+static inline int xhci_try_enable_msi(struct usb_hcd *hcd)
 {
 	return 0;
 }
 
-static void xhci_cleanup_msix(struct xhci_hcd *xhci)
+static inline void xhci_cleanup_msix(struct xhci_hcd *xhci)
 {
 }
 
-static void xhci_msix_sync_irqs(struct xhci_hcd *xhci)
+static inline void xhci_msix_sync_irqs(struct xhci_hcd *xhci)
 {
 }
 
@@ -641,10 +641,14 @@ int xhci_run(struct usb_hcd *hcd)
 	writel(ER_IRQ_ENABLE(temp), &xhci->ir_set->irq_pending);
 	xhci_print_ir_set(xhci, 0);
 
-	if (xhci->quirks & XHCI_NEC_HOST)
-		xhci_queue_vendor_command(xhci, 0, 0, 0,
+	if (xhci->quirks & XHCI_NEC_HOST) {
+		struct xhci_command *command;
+		command = xhci_alloc_command(xhci, false, false, GFP_KERNEL);
+		if (!command)
+			return -ENOMEM;
+		xhci_queue_vendor_command(xhci, command, 0, 0, 0,
 				TRB_TYPE(TRB_NEC_GET_FW));
-
+	}
 	xhci_dbg_trace(xhci, trace_xhci_dbg_init,
 			"Finished xhci_run for USB2 roothub");
 	return 0;
@@ -1187,10 +1191,10 @@ static int xhci_configure_endpoint(struct xhci_hcd *xhci,
 static int xhci_check_maxpacket(struct xhci_hcd *xhci, unsigned int slot_id,
 		unsigned int ep_index, struct urb *urb)
 {
-	struct xhci_container_ctx *in_ctx;
 	struct xhci_container_ctx *out_ctx;
 	struct xhci_input_control_ctx *ctrl_ctx;
 	struct xhci_ep_ctx *ep_ctx;
+	struct xhci_command *command;
 	int max_packet_size;
 	int hw_max_packet_size;
 	int ret = 0;
@@ -1215,18 +1219,24 @@ static int xhci_check_maxpacket(struct xhci_hcd *xhci, unsigned int slot_id,
 		/* FIXME: This won't work if a non-default control endpoint
 		 * changes max packet sizes.
 		 */
-		in_ctx = xhci->devs[slot_id]->in_ctx;
-		ctrl_ctx = xhci_get_input_control_ctx(xhci, in_ctx);
+
+		command = xhci_alloc_command(xhci, false, true, GFP_KERNEL);
+		if (!command)
+			return -ENOMEM;
+
+		command->in_ctx = xhci->devs[slot_id]->in_ctx;
+		ctrl_ctx = xhci_get_input_control_ctx(xhci, command->in_ctx);
 		if (!ctrl_ctx) {
 			xhci_warn(xhci, "%s: Could not get input context, bad type.\n",
 					__func__);
-			return -ENOMEM;
+			ret = -ENOMEM;
+			goto command_cleanup;
 		}
 		/* Set up the modified control endpoint 0 */
 		xhci_endpoint_copy(xhci, xhci->devs[slot_id]->in_ctx,
 				xhci->devs[slot_id]->out_ctx, ep_index);
 
-		ep_ctx = xhci_get_ep_ctx(xhci, in_ctx, ep_index);
+		ep_ctx = xhci_get_ep_ctx(xhci, command->in_ctx, ep_index);
 		ep_ctx->ep_info2 &= cpu_to_le32(~MAX_PACKET_MASK);
 		ep_ctx->ep_info2 |= cpu_to_le32(MAX_PACKET(max_packet_size));
 
@@ -1234,17 +1244,20 @@ static int xhci_check_maxpacket(struct xhci_hcd *xhci, unsigned int slot_id,
 		ctrl_ctx->drop_flags = 0;
 
 		xhci_dbg(xhci, "Slot %d input context\n", slot_id);
-		xhci_dbg_ctx(xhci, in_ctx, ep_index);
+		xhci_dbg_ctx(xhci, command->in_ctx, ep_index);
 		xhci_dbg(xhci, "Slot %d output context\n", slot_id);
 		xhci_dbg_ctx(xhci, out_ctx, ep_index);
 
-		ret = xhci_configure_endpoint(xhci, urb->dev, NULL,
+		ret = xhci_configure_endpoint(xhci, urb->dev, command,
 				true, false);
 
 		/* Clean up the input context for later use by bandwidth
 		 * functions.
 		 */
 		ctrl_ctx->add_flags = cpu_to_le32(SLOT_FLAG);
+command_cleanup:
+		kfree(command->completion);
+		kfree(command);
 	}
 	return ret;
 }
@@ -1465,6 +1478,7 @@ int xhci_urb_dequeue(struct usb_hcd *hcd, struct urb *urb, int status)
 	unsigned int ep_index;
 	struct xhci_ring *ep_ring;
 	struct xhci_virt_ep *ep;
+	struct xhci_command *command;
 
 	xhci = hcd_to_xhci(hcd);
 	spin_lock_irqsave(&xhci->lock, flags);
@@ -1534,12 +1548,14 @@ int xhci_urb_dequeue(struct usb_hcd *hcd, struct urb *urb, int status)
 	 * the first cancellation to be handled.
 	 */
 	if (!(ep->ep_state & EP_HALT_PENDING)) {
+		command = xhci_alloc_command(xhci, false, false, GFP_ATOMIC);
 		ep->ep_state |= EP_HALT_PENDING;
 		ep->stop_cmds_pending++;
 		ep->stop_cmd_timer.expires = jiffies +
 			XHCI_STOP_EP_CMD_TIMEOUT * HZ;
 		add_timer(&ep->stop_cmd_timer);
-		xhci_queue_stop_endpoint(xhci, urb->dev->slot_id, ep_index, 0);
+		xhci_queue_stop_endpoint(xhci, command, urb->dev->slot_id,
+					 ep_index, 0);
 		xhci_ring_cmd_db(xhci);
 	}
 done:
@@ -1804,6 +1820,11 @@ static int xhci_configure_endpoint_result(struct xhci_hcd *xhci,
 	int ret;
 
 	switch (*cmd_status) {
+	case COMP_CMD_ABORT:
+	case COMP_CMD_STOP:
+		xhci_warn(xhci, "Timeout while waiting for configure endpoint command\n");
+		ret = -ETIME;
+		break;
 	case COMP_ENOMEM:
 		dev_warn(&udev->dev, "Not enough host controller resources "
 				"for new device state.\n");
@@ -1850,6 +1871,11 @@ static int xhci_evaluate_context_result(struct xhci_hcd *xhci,
 	struct xhci_virt_device *virt_dev = xhci->devs[udev->slot_id];
 
 	switch (*cmd_status) {
+	case COMP_CMD_ABORT:
+	case COMP_CMD_STOP:
+		xhci_warn(xhci, "Timeout while waiting for evaluate context command\n");
+		ret = -ETIME;
+		break;
 	case COMP_EINVAL:
 		dev_warn(&udev->dev, "WARN: xHCI driver setup invalid evaluate "
 				"context command.\n");
@@ -2574,23 +2600,17 @@ static int xhci_configure_endpoint(struct xhci_hcd *xhci,
 		bool ctx_change, bool must_succeed)
 {
 	int ret;
-	int timeleft;
 	unsigned long flags;
-	struct xhci_container_ctx *in_ctx;
 	struct xhci_input_control_ctx *ctrl_ctx;
-	struct completion *cmd_completion;
-	u32 *cmd_status;
 	struct xhci_virt_device *virt_dev;
-	union xhci_trb *cmd_trb;
+
+	if (!command)
+		return -EINVAL;
 
 	spin_lock_irqsave(&xhci->lock, flags);
 	virt_dev = xhci->devs[udev->slot_id];
 
-	if (command)
-		in_ctx = command->in_ctx;
-	else
-		in_ctx = virt_dev->in_ctx;
-	ctrl_ctx = xhci_get_input_control_ctx(xhci, in_ctx);
+	ctrl_ctx = xhci_get_input_control_ctx(xhci, command->in_ctx);
 	if (!ctrl_ctx) {
 		spin_unlock_irqrestore(&xhci->lock, flags);
 		xhci_warn(xhci, "%s: Could not get input context, bad type.\n",
@@ -2607,7 +2627,7 @@ static int xhci_configure_endpoint(struct xhci_hcd *xhci,
 		return -ENOMEM;
 	}
 	if ((xhci->quirks & XHCI_SW_BW_CHECKING) &&
-			xhci_reserve_bandwidth(xhci, virt_dev, in_ctx)) {
+	    xhci_reserve_bandwidth(xhci, virt_dev, command->in_ctx)) {
 		if ((xhci->quirks & XHCI_EP_LIMIT_QUIRK))
 			xhci_free_host_resources(xhci, ctrl_ctx);
 		spin_unlock_irqrestore(&xhci->lock, flags);
@@ -2615,27 +2635,15 @@ static int xhci_configure_endpoint(struct xhci_hcd *xhci,
 		return -ENOMEM;
 	}
 
-	if (command) {
-		cmd_completion = command->completion;
-		cmd_status = &command->status;
-		command->command_trb = xhci_find_next_enqueue(xhci->cmd_ring);
-		list_add_tail(&command->cmd_list, &virt_dev->cmd_list);
-	} else {
-		cmd_completion = &virt_dev->cmd_completion;
-		cmd_status = &virt_dev->cmd_status;
-	}
-	init_completion(cmd_completion);
-
-	cmd_trb = xhci_find_next_enqueue(xhci->cmd_ring);
 	if (!ctx_change)
-		ret = xhci_queue_configure_endpoint(xhci, in_ctx->dma,
+		ret = xhci_queue_configure_endpoint(xhci, command,
+				command->in_ctx->dma,
 				udev->slot_id, must_succeed);
 	else
-		ret = xhci_queue_evaluate_context(xhci, in_ctx->dma,
+		ret = xhci_queue_evaluate_context(xhci, command,
+				command->in_ctx->dma,
 				udev->slot_id, must_succeed);
 	if (ret < 0) {
-		if (command)
-			list_del(&command->cmd_list);
 		if ((xhci->quirks & XHCI_EP_LIMIT_QUIRK))
 			xhci_free_host_resources(xhci, ctrl_ctx);
 		spin_unlock_irqrestore(&xhci->lock, flags);
@@ -2647,26 +2655,14 @@ static int xhci_configure_endpoint(struct xhci_hcd *xhci,
 	spin_unlock_irqrestore(&xhci->lock, flags);
 
 	/* Wait for the configure endpoint command to complete */
-	timeleft = wait_for_completion_interruptible_timeout(
-			cmd_completion,
-			XHCI_CMD_DEFAULT_TIMEOUT);
-	if (timeleft <= 0) {
-		xhci_warn(xhci, "%s while waiting for %s command\n",
-				timeleft == 0 ? "Timeout" : "Signal",
-				ctx_change == 0 ?
-					"configure endpoint" :
-					"evaluate context");
-		/* cancel the configure endpoint command */
-		ret = xhci_cancel_cmd(xhci, command, cmd_trb);
-		if (ret < 0)
-			return ret;
-		return -ETIME;
-	}
+	wait_for_completion(command->completion);
 
 	if (!ctx_change)
-		ret = xhci_configure_endpoint_result(xhci, udev, cmd_status);
+		ret = xhci_configure_endpoint_result(xhci, udev,
+						     &command->status);
 	else
-		ret = xhci_evaluate_context_result(xhci, udev, cmd_status);
+		ret = xhci_evaluate_context_result(xhci, udev,
+						   &command->status);
 
 	if ((xhci->quirks & XHCI_EP_LIMIT_QUIRK)) {
 		spin_lock_irqsave(&xhci->lock, flags);
@@ -2714,6 +2710,7 @@ int xhci_check_bandwidth(struct usb_hcd *hcd, struct usb_device *udev)
 	struct xhci_virt_device	*virt_dev;
 	struct xhci_input_control_ctx *ctrl_ctx;
 	struct xhci_slot_ctx *slot_ctx;
+	struct xhci_command *command;
 
 	ret = xhci_check_args(hcd, udev, NULL, 0, true, __func__);
 	if (ret <= 0)
@@ -2725,12 +2722,19 @@ int xhci_check_bandwidth(struct usb_hcd *hcd, struct usb_device *udev)
 	xhci_dbg(xhci, "%s called for udev %p\n", __func__, udev);
 	virt_dev = xhci->devs[udev->slot_id];
 
+	command = xhci_alloc_command(xhci, false, true, GFP_KERNEL);
+	if (!command)
+		return -ENOMEM;
+
+	command->in_ctx = virt_dev->in_ctx;
+
 	/* See section 4.6.6 - A0 = 1; A1 = D0 = D1 = 0 */
-	ctrl_ctx = xhci_get_input_control_ctx(xhci, virt_dev->in_ctx);
+	ctrl_ctx = xhci_get_input_control_ctx(xhci, command->in_ctx);
 	if (!ctrl_ctx) {
 		xhci_warn(xhci, "%s: Could not get input context, bad type.\n",
 				__func__);
-		return -ENOMEM;
+		ret = -ENOMEM;
+		goto command_cleanup;
 	}
 	ctrl_ctx->add_flags |= cpu_to_le32(SLOT_FLAG);
 	ctrl_ctx->add_flags &= cpu_to_le32(~EP0_FLAG);
@@ -2738,20 +2742,20 @@ int xhci_check_bandwidth(struct usb_hcd *hcd, struct usb_device *udev)
 
 	/* Don't issue the command if there's no endpoints to update. */
 	if (ctrl_ctx->add_flags == cpu_to_le32(SLOT_FLAG) &&
-			ctrl_ctx->drop_flags == 0)
-		return 0;
-
+	    ctrl_ctx->drop_flags == 0) {
+		ret = 0;
+		goto command_cleanup;
+	}
 	xhci_dbg(xhci, "New Input Control Context:\n");
 	slot_ctx = xhci_get_slot_ctx(xhci, virt_dev->in_ctx);
 	xhci_dbg_ctx(xhci, virt_dev->in_ctx,
 		     LAST_CTX_TO_EP_NUM(le32_to_cpu(slot_ctx->dev_info)));
 
-	ret = xhci_configure_endpoint(xhci, udev, NULL,
+	ret = xhci_configure_endpoint(xhci, udev, command,
 			false, false);
-	if (ret) {
+	if (ret)
 		/* Callee should call reset_bandwidth() */
-		return ret;
-	}
+		goto command_cleanup;
 
 	xhci_dbg(xhci, "Output context after successful config ep cmd:\n");
 	xhci_dbg_ctx(xhci, virt_dev->out_ctx,
@@ -2783,6 +2787,9 @@ int xhci_check_bandwidth(struct usb_hcd *hcd, struct usb_device *udev)
 		virt_dev->eps[i].ring = virt_dev->eps[i].new_ring;
 		virt_dev->eps[i].new_ring = NULL;
 	}
+command_cleanup:
+	kfree(command->completion);
+	kfree(command);
 
 	return ret;
 }
@@ -2884,9 +2891,14 @@ void xhci_cleanup_stalled_ring(struct xhci_hcd *xhci,
 	 * issue a configure endpoint command later.
 	 */
 	if (!(xhci->quirks & XHCI_RESET_EP_QUIRK)) {
+		struct xhci_command *command;
+		/* Can't sleep if we're called from cleanup_halted_endpoint() */
+		command = xhci_alloc_command(xhci, false, false, GFP_ATOMIC);
+		if (!command)
+			return;
 		xhci_dbg_trace(xhci, trace_xhci_dbg_reset_ep,
 				"Queueing new dequeue state");
-		xhci_queue_new_dequeue_state(xhci, udev->slot_id,
+		xhci_queue_new_dequeue_state(xhci, command, udev->slot_id,
 				ep_index, ep->stopped_stream, &deq_state);
 	} else {
 		/* Better hope no one uses the input context between now and the
@@ -2917,6 +2929,7 @@ void xhci_endpoint_reset(struct usb_hcd *hcd,
 	unsigned long flags;
 	int ret;
 	struct xhci_virt_ep *virt_ep;
+	struct xhci_command *command;
 
 	xhci = hcd_to_xhci(hcd);
 	udev = (struct usb_device *) ep->hcpriv;
@@ -2939,10 +2952,14 @@ void xhci_endpoint_reset(struct usb_hcd *hcd,
 		return;
 	}
 
+	command = xhci_alloc_command(xhci, false, false, GFP_ATOMIC);
+	if (!command)
+		return;
+
 	xhci_dbg_trace(xhci, trace_xhci_dbg_reset_ep,
 			"Queueing reset endpoint command");
 	spin_lock_irqsave(&xhci->lock, flags);
-	ret = xhci_queue_reset_ep(xhci, udev->slot_id, ep_index);
+	ret = xhci_queue_reset_ep(xhci, command, udev->slot_id, ep_index);
 	/*
 	 * Can't change the ring dequeue pointer until it's transitioned to the
 	 * stopped state, which is only upon a successful reset endpoint
@@ -2954,7 +2971,6 @@ void xhci_endpoint_reset(struct usb_hcd *hcd,
 		xhci_ring_cmd_db(xhci);
 	}
 	virt_ep->stopped_td = NULL;
-	virt_ep->stopped_trb = NULL;
 	virt_ep->stopped_stream = 0;
 	spin_unlock_irqrestore(&xhci->lock, flags);
 
@@ -3417,7 +3433,6 @@ int xhci_discover_or_reset_device(struct usb_hcd *hcd, struct usb_device *udev)
 	unsigned int slot_id;
 	struct xhci_virt_device *virt_dev;
 	struct xhci_command *reset_device_cmd;
-	int timeleft;
 	int last_freed_endpoint;
 	struct xhci_slot_ctx *slot_ctx;
 	int old_active_eps = 0;
@@ -3474,13 +3489,10 @@ int xhci_discover_or_reset_device(struct usb_hcd *hcd, struct usb_device *udev)
 
 	/* Attempt to submit the Reset Device command to the command ring */
 	spin_lock_irqsave(&xhci->lock, flags);
-	reset_device_cmd->command_trb = xhci_find_next_enqueue(xhci->cmd_ring);
 
-	list_add_tail(&reset_device_cmd->cmd_list, &virt_dev->cmd_list);
-	ret = xhci_queue_reset_device(xhci, slot_id);
+	ret = xhci_queue_reset_device(xhci, reset_device_cmd, slot_id);
 	if (ret) {
 		xhci_dbg(xhci, "FIXME: allocate a command ring segment\n");
-		list_del(&reset_device_cmd->cmd_list);
 		spin_unlock_irqrestore(&xhci->lock, flags);
 		goto command_cleanup;
 	}
@@ -3488,22 +3500,7 @@ int xhci_discover_or_reset_device(struct usb_hcd *hcd, struct usb_device *udev)
 	spin_unlock_irqrestore(&xhci->lock, flags);
 
 	/* Wait for the Reset Device command to finish */
-	timeleft = wait_for_completion_interruptible_timeout(
-			reset_device_cmd->completion,
-			XHCI_CMD_DEFAULT_TIMEOUT);
-	if (timeleft <= 0) {
-		xhci_warn(xhci, "%s while waiting for reset device command\n",
-				timeleft == 0 ? "Timeout" : "Signal");
-		spin_lock_irqsave(&xhci->lock, flags);
-		/* The timeout might have raced with the event ring handler, so
-		 * only delete from the list if the item isn't poisoned.
-		 */
-		if (reset_device_cmd->cmd_list.next != LIST_POISON1)
-			list_del(&reset_device_cmd->cmd_list);
-		spin_unlock_irqrestore(&xhci->lock, flags);
-		ret = -ETIME;
-		goto command_cleanup;
-	}
+	wait_for_completion(reset_device_cmd->completion);
 
 	/* The Reset Device command can't fail, according to the 0.95/0.96 spec,
 	 * unless we tried to reset a slot ID that wasn't enabled,
@@ -3511,6 +3508,11 @@ int xhci_discover_or_reset_device(struct usb_hcd *hcd, struct usb_device *udev)
 	 */
 	ret = reset_device_cmd->status;
 	switch (ret) {
+	case COMP_CMD_ABORT:
+	case COMP_CMD_STOP:
+		xhci_warn(xhci, "Timeout waiting for reset device command\n");
+		ret = -ETIME;
+		goto command_cleanup;
 	case COMP_EBADSLT: /* 0.95 completion code for bad slot ID */
 	case COMP_CTX_STATE: /* 0.96 completion code for same thing */
 		xhci_dbg(xhci, "Can't reset device (slot ID %u) in %s state\n",
@@ -3590,6 +3592,11 @@ void xhci_free_dev(struct usb_hcd *hcd, struct usb_device *udev)
 	unsigned long flags;
 	u32 state;
 	int i, ret;
+	struct xhci_command *command;
+
+	command = xhci_alloc_command(xhci, false, false, GFP_KERNEL);
+	if (!command)
+		return;
 
 #ifndef CONFIG_USB_DEFAULT_PERSIST
 	/*
@@ -3605,8 +3612,10 @@ void xhci_free_dev(struct usb_hcd *hcd, struct usb_device *udev)
 	/* If the host is halted due to driver unload, we still need to free the
 	 * device.
 	 */
-	if (ret <= 0 && ret != -ENODEV)
+	if (ret <= 0 && ret != -ENODEV) {
+		kfree(command);
 		return;
+	}
 
 	virt_dev = xhci->devs[udev->slot_id];
 
@@ -3623,16 +3632,19 @@ void xhci_free_dev(struct usb_hcd *hcd, struct usb_device *udev)
 			(xhci->xhc_state & XHCI_STATE_HALTED)) {
 		xhci_free_virt_device(xhci, udev->slot_id);
 		spin_unlock_irqrestore(&xhci->lock, flags);
+		kfree(command);
 		return;
 	}
 
-	if (xhci_queue_slot_control(xhci, TRB_DISABLE_SLOT, udev->slot_id)) {
+	if (xhci_queue_slot_control(xhci, command, TRB_DISABLE_SLOT,
+				    udev->slot_id)) {
 		spin_unlock_irqrestore(&xhci->lock, flags);
 		xhci_dbg(xhci, "FIXME: allocate a command ring segment\n");
 		return;
 	}
 	xhci_ring_cmd_db(xhci);
 	spin_unlock_irqrestore(&xhci->lock, flags);
+
 	/*
 	 * Event command completion handler will free any data structures
 	 * associated with the slot.  XXX Can free sleep?
@@ -3670,33 +3682,33 @@ int xhci_alloc_dev(struct usb_hcd *hcd, struct usb_device *udev)
 {
 	struct xhci_hcd *xhci = hcd_to_xhci(hcd);
 	unsigned long flags;
-	int timeleft;
 	int ret;
-	union xhci_trb *cmd_trb;
+	struct xhci_command *command;
+
+	command = xhci_alloc_command(xhci, false, false, GFP_KERNEL);
+	if (!command)
+		return 0;
 
 	spin_lock_irqsave(&xhci->lock, flags);
-	cmd_trb = xhci_find_next_enqueue(xhci->cmd_ring);
-	ret = xhci_queue_slot_control(xhci, TRB_ENABLE_SLOT, 0);
+	command->completion = &xhci->addr_dev;
+	ret = xhci_queue_slot_control(xhci, command, TRB_ENABLE_SLOT, 0);
 	if (ret) {
 		spin_unlock_irqrestore(&xhci->lock, flags);
 		xhci_dbg(xhci, "FIXME: allocate a command ring segment\n");
+		kfree(command);
 		return 0;
 	}
 	xhci_ring_cmd_db(xhci);
 	spin_unlock_irqrestore(&xhci->lock, flags);
 
-	/* XXX: how much time for xHC slot assignment? */
-	timeleft = wait_for_completion_interruptible_timeout(&xhci->addr_dev,
-			XHCI_CMD_DEFAULT_TIMEOUT);
-	if (timeleft <= 0) {
-		xhci_warn(xhci, "%s while waiting for a slot\n",
-				timeleft == 0 ? "Timeout" : "Signal");
-		/* cancel the enable slot request */
-		return xhci_cancel_cmd(xhci, NULL, cmd_trb);
-	}
+	wait_for_completion(command->completion);
 
-	if (!xhci->slot_id) {
+	if (!xhci->slot_id || command->status != COMP_SUCCESS) {
 		xhci_err(xhci, "Error while assigning device slot ID\n");
+		xhci_err(xhci, "Max number of devices this xHCI host supports is %u.\n",
+				HCS_MAX_SLOTS(
+					readl(&xhci->cap_regs->hcs_params1)));
+		kfree(command);
 		return 0;
 	}
 
@@ -3731,6 +3743,8 @@ int xhci_alloc_dev(struct usb_hcd *hcd, struct usb_device *udev)
 		pm_runtime_get_noresume(hcd->self.controller);
 #endif
 
+
+	kfree(command);
 	/* Is this a LS or FS device under a HS hub? */
 	/* Hub or peripherial? */
 	return 1;
@@ -3738,7 +3752,10 @@ int xhci_alloc_dev(struct usb_hcd *hcd, struct usb_device *udev)
 disable_slot:
 	/* Disable slot, if we can do it without mem alloc */
 	spin_lock_irqsave(&xhci->lock, flags);
-	if (!xhci_queue_slot_control(xhci, TRB_DISABLE_SLOT, udev->slot_id))
+	command->completion = NULL;
+	command->status = 0;
+	if (!xhci_queue_slot_control(xhci, command, TRB_DISABLE_SLOT,
+				     udev->slot_id))
 		xhci_ring_cmd_db(xhci);
 	spin_unlock_irqrestore(&xhci->lock, flags);
 	return 0;
@@ -3755,14 +3772,13 @@ static int xhci_setup_device(struct usb_hcd *hcd, struct usb_device *udev,
 {
 	const char *act = setup == SETUP_CONTEXT_ONLY ? "context" : "address";
 	unsigned long flags;
-	int timeleft;
 	struct xhci_virt_device *virt_dev;
 	int ret = 0;
 	struct xhci_hcd *xhci = hcd_to_xhci(hcd);
 	struct xhci_slot_ctx *slot_ctx;
 	struct xhci_input_control_ctx *ctrl_ctx;
 	u64 temp_64;
-	union xhci_trb *cmd_trb;
+	struct xhci_command *command;
 
 	if (!udev->slot_id) {
 		xhci_dbg_trace(xhci, trace_xhci_dbg_address,
@@ -3783,11 +3799,19 @@ static int xhci_setup_device(struct usb_hcd *hcd, struct usb_device *udev,
 		return -EINVAL;
 	}
 
+	command = xhci_alloc_command(xhci, false, false, GFP_KERNEL);
+	if (!command)
+		return -ENOMEM;
+
+	command->in_ctx = virt_dev->in_ctx;
+	command->completion = &xhci->addr_dev;
+
 	slot_ctx = xhci_get_slot_ctx(xhci, virt_dev->in_ctx);
 	ctrl_ctx = xhci_get_input_control_ctx(xhci, virt_dev->in_ctx);
 	if (!ctrl_ctx) {
 		xhci_warn(xhci, "%s: Could not get input context, bad type.\n",
 				__func__);
+		kfree(command);
 		return -EINVAL;
 	}
 	/*
@@ -3809,36 +3833,31 @@ static int xhci_setup_device(struct usb_hcd *hcd, struct usb_device *udev,
 				le32_to_cpu(slot_ctx->dev_info) >> 27);
 
 	spin_lock_irqsave(&xhci->lock, flags);
-	cmd_trb = xhci_find_next_enqueue(xhci->cmd_ring);
-	ret = xhci_queue_address_device(xhci, virt_dev->in_ctx->dma,
+	ret = xhci_queue_address_device(xhci, command, virt_dev->in_ctx->dma,
 					udev->slot_id, setup);
 	if (ret) {
 		spin_unlock_irqrestore(&xhci->lock, flags);
 		xhci_dbg_trace(xhci, trace_xhci_dbg_address,
 				"FIXME: allocate a command ring segment");
+		kfree(command);
 		return ret;
 	}
 	xhci_ring_cmd_db(xhci);
 	spin_unlock_irqrestore(&xhci->lock, flags);
 
 	/* ctrl tx can take up to 5 sec; XXX: need more time for xHC? */
-	timeleft = wait_for_completion_interruptible_timeout(&xhci->addr_dev,
-			XHCI_CMD_DEFAULT_TIMEOUT);
+	wait_for_completion(command->completion);
+
 	/* FIXME: From section 4.3.4: "Software shall be responsible for timing
 	 * the SetAddress() "recovery interval" required by USB and aborting the
 	 * command on a timeout.
 	 */
-	if (timeleft <= 0) {
-		xhci_warn(xhci, "%s while waiting for setup %s command\n",
-			  timeleft == 0 ? "Timeout" : "Signal", act);
-		/* cancel the address device command */
-		ret = xhci_cancel_cmd(xhci, NULL, cmd_trb);
-		if (ret < 0)
-			return ret;
-		return -ETIME;
-	}
-
-	switch (virt_dev->cmd_status) {
+	switch (command->status) {
+	case COMP_CMD_ABORT:
+	case COMP_CMD_STOP:
+		xhci_warn(xhci, "Timeout while waiting for setup device command\n");
+		ret = -ETIME;
+		break;
 	case COMP_CTX_STATE:
 	case COMP_EBADSLT:
 		xhci_err(xhci, "Setup ERROR: setup %s command for slot %d.\n",
@@ -3861,7 +3880,7 @@ static int xhci_setup_device(struct usb_hcd *hcd, struct usb_device *udev,
 	default:
 		xhci_err(xhci,
 			 "ERROR: unexpected setup %s command completion code 0x%x.\n",
-			 act, virt_dev->cmd_status);
+			 act, command->status);
 		xhci_dbg(xhci, "Slot ID %d Output Context:\n", udev->slot_id);
 		xhci_dbg_ctx(xhci, virt_dev->out_ctx, 2);
 		trace_xhci_address_ctx(xhci, virt_dev->out_ctx, 1);
@@ -3869,6 +3888,7 @@ static int xhci_setup_device(struct usb_hcd *hcd, struct usb_device *udev,
 		break;
 	}
 	if (ret) {
+		kfree(command);
 		return ret;
 	}
 	temp_64 = xhci_read_64(xhci, &xhci->op_regs->dcbaa_ptr);
@@ -3903,7 +3923,7 @@ static int xhci_setup_device(struct usb_hcd *hcd, struct usb_device *udev,
 	xhci_dbg_trace(xhci, trace_xhci_dbg_address,
 		       "Internal device address = %d",
 		       le32_to_cpu(slot_ctx->dev_state) & DEV_ADDR_MASK);
-
+	kfree(command);
 	return 0;
 }
 
@@ -4093,7 +4113,7 @@ int xhci_set_usb2_hardware_lpm(struct usb_hcd *hcd,
 	field = le32_to_cpu(udev->bos->ext_cap->bmAttributes);
 
 	xhci_dbg(xhci, "%s port %d USB2 hardware LPM\n",
-			enable ? "enable" : "disable", port_num);
+			enable ? "enable" : "disable", port_num + 1);
 
 	if (enable) {
 		/* Host supports BESL timeout instead of HIRD */
diff --git a/drivers/usb/host/xhci.h b/drivers/usb/host/xhci.h
index d280e9213d0..9ffecd56600 100644
--- a/drivers/usb/host/xhci.h
+++ b/drivers/usb/host/xhci.h
@@ -865,8 +865,6 @@ struct xhci_virt_ep {
 #define EP_GETTING_NO_STREAMS	(1 << 5)
 	/* ----  Related to URB cancellation ---- */
 	struct list_head	cancelled_td_list;
-	/* The TRB that was last reported in a stopped endpoint ring */
-	union xhci_trb		*stopped_trb;
 	struct xhci_td		*stopped_td;
 	unsigned int		stopped_stream;
 	/* Watchdog timer for stop endpoint command to cancel URBs */
@@ -939,9 +937,6 @@ struct xhci_virt_device {
 #define	XHCI_MAX_RINGS_CACHED	31
 	struct xhci_virt_ep		eps[31];
 	struct completion		cmd_completion;
-	/* Status of the last command issued for this device */
-	u32				cmd_status;
-	struct list_head		cmd_list;
 	u8				fake_port;
 	u8				real_port;
 	struct xhci_interval_bw_table	*bw_table;
@@ -1300,7 +1295,6 @@ struct xhci_td {
 
 /* command descriptor */
 struct xhci_cd {
-	struct list_head	cancel_cmd_list;
 	struct xhci_command	*command;
 	union xhci_trb		*cmd_trb;
 };
@@ -1478,6 +1472,8 @@ struct xhci_hcd {
 	/* msi-x vectors */
 	int		msix_count;
 	struct msix_entry	*msix_entries;
+	/* optional clock */
+	struct clk		*clk;
 	/* data structures */
 	struct xhci_device_context_array *dcbaa;
 	struct xhci_ring	*cmd_ring;
@@ -1485,8 +1481,10 @@ struct xhci_hcd {
 #define CMD_RING_STATE_RUNNING         (1 << 0)
 #define CMD_RING_STATE_ABORTED         (1 << 1)
 #define CMD_RING_STATE_STOPPED         (1 << 2)
-	struct list_head        cancel_cmd_list;
+	struct list_head        cmd_list;
 	unsigned int		cmd_ring_reserved_trbs;
+	struct timer_list	cmd_timer;
+	struct xhci_command	*current_cmd;
 	struct xhci_ring	*event_ring;
 	struct xhci_erst	erst;
 	/* Scratchpad */
@@ -1740,8 +1738,7 @@ static inline int xhci_register_pci(void) { return 0; }
 static inline void xhci_unregister_pci(void) {}
 #endif
 
-#if defined(CONFIG_USB_XHCI_PLATFORM) \
-	|| defined(CONFIG_USB_XHCI_PLATFORM_MODULE)
+#if IS_ENABLED(CONFIG_USB_XHCI_PLATFORM)
 int xhci_register_plat(void);
 void xhci_unregister_plat(void);
 #else
@@ -1810,13 +1807,14 @@ struct xhci_segment *trb_in_td(struct xhci_segment *start_seg,
 		dma_addr_t suspect_dma);
 int xhci_is_vendor_info_code(struct xhci_hcd *xhci, unsigned int trb_comp_code);
 void xhci_ring_cmd_db(struct xhci_hcd *xhci);
-int xhci_queue_slot_control(struct xhci_hcd *xhci, u32 trb_type, u32 slot_id);
-int xhci_queue_address_device(struct xhci_hcd *xhci, dma_addr_t in_ctx_ptr,
-		u32 slot_id, enum xhci_setup_dev);
-int xhci_queue_vendor_command(struct xhci_hcd *xhci,
+int xhci_queue_slot_control(struct xhci_hcd *xhci, struct xhci_command *cmd,
+		u32 trb_type, u32 slot_id);
+int xhci_queue_address_device(struct xhci_hcd *xhci, struct xhci_command *cmd,
+		dma_addr_t in_ctx_ptr, u32 slot_id, enum xhci_setup_dev);
+int xhci_queue_vendor_command(struct xhci_hcd *xhci, struct xhci_command *cmd,
 		u32 field1, u32 field2, u32 field3, u32 field4);
-int xhci_queue_stop_endpoint(struct xhci_hcd *xhci, int slot_id,
-		unsigned int ep_index, int suspend);
+int xhci_queue_stop_endpoint(struct xhci_hcd *xhci, struct xhci_command *cmd,
+		int slot_id, unsigned int ep_index, int suspend);
 int xhci_queue_ctrl_tx(struct xhci_hcd *xhci, gfp_t mem_flags, struct urb *urb,
 		int slot_id, unsigned int ep_index);
 int xhci_queue_bulk_tx(struct xhci_hcd *xhci, gfp_t mem_flags, struct urb *urb,
@@ -1825,18 +1823,21 @@ int xhci_queue_intr_tx(struct xhci_hcd *xhci, gfp_t mem_flags, struct urb *urb,
 		int slot_id, unsigned int ep_index);
 int xhci_queue_isoc_tx_prepare(struct xhci_hcd *xhci, gfp_t mem_flags,
 		struct urb *urb, int slot_id, unsigned int ep_index);
-int xhci_queue_configure_endpoint(struct xhci_hcd *xhci, dma_addr_t in_ctx_ptr,
-		u32 slot_id, bool command_must_succeed);
-int xhci_queue_evaluate_context(struct xhci_hcd *xhci, dma_addr_t in_ctx_ptr,
-		u32 slot_id, bool command_must_succeed);
-int xhci_queue_reset_ep(struct xhci_hcd *xhci, int slot_id,
-		unsigned int ep_index);
-int xhci_queue_reset_device(struct xhci_hcd *xhci, u32 slot_id);
+int xhci_queue_configure_endpoint(struct xhci_hcd *xhci,
+		struct xhci_command *cmd, dma_addr_t in_ctx_ptr, u32 slot_id,
+		bool command_must_succeed);
+int xhci_queue_evaluate_context(struct xhci_hcd *xhci, struct xhci_command *cmd,
+		dma_addr_t in_ctx_ptr, u32 slot_id, bool command_must_succeed);
+int xhci_queue_reset_ep(struct xhci_hcd *xhci, struct xhci_command *cmd,
+		int slot_id, unsigned int ep_index);
+int xhci_queue_reset_device(struct xhci_hcd *xhci, struct xhci_command *cmd,
+		u32 slot_id);
 void xhci_find_new_dequeue_state(struct xhci_hcd *xhci,
 		unsigned int slot_id, unsigned int ep_index,
 		unsigned int stream_id, struct xhci_td *cur_td,
 		struct xhci_dequeue_state *state);
 void xhci_queue_new_dequeue_state(struct xhci_hcd *xhci,
+		struct xhci_command *cmd,
 		unsigned int slot_id, unsigned int ep_index,
 		unsigned int stream_id,
 		struct xhci_dequeue_state *deq_state);
@@ -1846,11 +1847,11 @@ void xhci_queue_config_ep_quirk(struct xhci_hcd *xhci,
 		unsigned int slot_id, unsigned int ep_index,
 		struct xhci_dequeue_state *deq_state);
 void xhci_stop_endpoint_command_watchdog(unsigned long arg);
-int xhci_cancel_cmd(struct xhci_hcd *xhci, struct xhci_command *command,
-		union xhci_trb *cmd_trb);
+void xhci_handle_command_timeout(unsigned long data);
+
 void xhci_ring_ep_doorbell(struct xhci_hcd *xhci, unsigned int slot_id,
 		unsigned int ep_index, unsigned int stream_id);
-union xhci_trb *xhci_find_next_enqueue(struct xhci_ring *ring);
+void xhci_cleanup_command_queue(struct xhci_hcd *xhci);
 
 /* xHCI roothub code */
 void xhci_set_link_state(struct xhci_hcd *xhci, __le32 __iomem **port_array,
diff --git a/drivers/usb/misc/appledisplay.c b/drivers/usb/misc/appledisplay.c
index ba6a5d6e618..b3d245ef46e 100644
--- a/drivers/usb/misc/appledisplay.c
+++ b/drivers/usb/misc/appledisplay.c
@@ -81,6 +81,7 @@ struct appledisplay {
 	struct delayed_work work;
 	int button_pressed;
 	spinlock_t lock;
+	struct mutex sysfslock;		/* concurrent read and write */
 };
 
 static atomic_t count_displays = ATOMIC_INIT(0);
@@ -110,7 +111,7 @@ static void appledisplay_complete(struct urb *urb)
 			__func__, status);
 		return;
 	default:
-		dev_dbg(dev, "%s - nonzero urb status received: %d/n",
+		dev_dbg(dev, "%s - nonzero urb status received: %d\n",
 			__func__, status);
 		goto exit;
 	}
@@ -144,6 +145,7 @@ static int appledisplay_bl_update_status(struct backlight_device *bd)
 	struct appledisplay *pdata = bl_get_data(bd);
 	int retval;
 
+	mutex_lock(&pdata->sysfslock);
 	pdata->msgdata[0] = 0x10;
 	pdata->msgdata[1] = bd->props.brightness;
 
@@ -156,15 +158,17 @@ static int appledisplay_bl_update_status(struct backlight_device *bd)
 		0,
 		pdata->msgdata, 2,
 		ACD_USB_TIMEOUT);
-
+	mutex_unlock(&pdata->sysfslock);
+	
 	return retval;
 }
 
 static int appledisplay_bl_get_brightness(struct backlight_device *bd)
 {
 	struct appledisplay *pdata = bl_get_data(bd);
-	int retval;
+	int retval, brightness;
 
+	mutex_lock(&pdata->sysfslock);
 	retval = usb_control_msg(
 		pdata->udev,
 		usb_rcvctrlpipe(pdata->udev, 0),
@@ -174,11 +178,13 @@ static int appledisplay_bl_get_brightness(struct backlight_device *bd)
 		0,
 		pdata->msgdata, 2,
 		ACD_USB_TIMEOUT);
+	brightness = pdata->msgdata[1];
+	mutex_unlock(&pdata->sysfslock);
 
 	if (retval < 0)
 		return retval;
 	else
-		return pdata->msgdata[1];
+		return brightness;
 }
 
 static const struct backlight_ops appledisplay_bl_data = {
@@ -241,6 +247,7 @@ static int appledisplay_probe(struct usb_interface *iface,
 
 	spin_lock_init(&pdata->lock);
 	INIT_DELAYED_WORK(&pdata->work, appledisplay_work);
+	mutex_init(&pdata->sysfslock);
 
 	/* Allocate buffer for control messages */
 	pdata->msgdata = kmalloc(ACD_MSG_BUFFER_LEN, GFP_KERNEL);
diff --git a/drivers/usb/misc/ftdi-elan.c b/drivers/usb/misc/ftdi-elan.c
index a4a3c7cd4a1..8ab1f8f3c26 100644
--- a/drivers/usb/misc/ftdi-elan.c
+++ b/drivers/usb/misc/ftdi-elan.c
@@ -1,40 +1,43 @@
 /*
-* USB FTDI client driver for Elan Digital Systems's Uxxx adapters
-*
-* Copyright(C) 2006 Elan Digital Systems Limited
-* http://www.elandigitalsystems.com
-*
-* Author and Maintainer - Tony Olech - Elan Digital Systems
-* tony.olech@elandigitalsystems.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 driver was written by Tony Olech(tony.olech@elandigitalsystems.com)
-* based on various USB client drivers in the 2.6.15 linux kernel
-* with constant reference to the 3rd Edition of Linux Device Drivers
-* published by O'Reilly
-*
-* The U132 adapter is a USB to CardBus adapter specifically designed
-* for PC cards that contain an OHCI host controller. Typical PC cards
-* are the Orange Mobile 3G Option GlobeTrotter Fusion card.
-*
-* The U132 adapter will *NOT *work with PC cards that do not contain
-* an OHCI controller. A simple way to test whether a PC card has an
-* OHCI controller as an interface is to insert the PC card directly
-* into a laptop(or desktop) with a CardBus slot and if "lspci" shows
-* a new USB controller and "lsusb -v" shows a new OHCI Host Controller
-* then there is a good chance that the U132 adapter will support the
-* PC card.(you also need the specific client driver for the PC card)
-*
-* Please inform the Author and Maintainer about any PC cards that
-* contain OHCI Host Controller and work when directly connected to
-* an embedded CardBus slot but do not work when they are connected
-* via an ELAN U132 adapter.
-*
-*/
+ * USB FTDI client driver for Elan Digital Systems's Uxxx adapters
+ *
+ * Copyright(C) 2006 Elan Digital Systems Limited
+ * http://www.elandigitalsystems.com
+ *
+ * Author and Maintainer - Tony Olech - Elan Digital Systems
+ * tony.olech@elandigitalsystems.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 driver was written by Tony Olech(tony.olech@elandigitalsystems.com)
+ * based on various USB client drivers in the 2.6.15 linux kernel
+ * with constant reference to the 3rd Edition of Linux Device Drivers
+ * published by O'Reilly
+ *
+ * The U132 adapter is a USB to CardBus adapter specifically designed
+ * for PC cards that contain an OHCI host controller. Typical PC cards
+ * are the Orange Mobile 3G Option GlobeTrotter Fusion card.
+ *
+ * The U132 adapter will *NOT *work with PC cards that do not contain
+ * an OHCI controller. A simple way to test whether a PC card has an
+ * OHCI controller as an interface is to insert the PC card directly
+ * into a laptop(or desktop) with a CardBus slot and if "lspci" shows
+ * a new USB controller and "lsusb -v" shows a new OHCI Host Controller
+ * then there is a good chance that the U132 adapter will support the
+ * PC card.(you also need the specific client driver for the PC card)
+ *
+ * Please inform the Author and Maintainer about any PC cards that
+ * contain OHCI Host Controller and work when directly connected to
+ * an embedded CardBus slot but do not work when they are connected
+ * via an ELAN U132 adapter.
+ *
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/kernel.h>
 #include <linux/errno.h>
 #include <linux/init.h>
@@ -55,31 +58,31 @@ MODULE_LICENSE("GPL");
 #define INT_MODULE_PARM(n, v) static int n = v;module_param(n, int, 0444)
 static bool distrust_firmware = 1;
 module_param(distrust_firmware, bool, 0);
-MODULE_PARM_DESC(distrust_firmware, "true to distrust firmware power/overcurren"
-        "t setup");
+MODULE_PARM_DESC(distrust_firmware,
+		 "true to distrust firmware power/overcurrent setup");
 extern struct platform_driver u132_platform_driver;
 static struct workqueue_struct *status_queue;
 static struct workqueue_struct *command_queue;
 static struct workqueue_struct *respond_queue;
 /*
-* ftdi_module_lock exists to protect access to global variables
-*
-*/
+ * ftdi_module_lock exists to protect access to global variables
+ *
+ */
 static struct mutex ftdi_module_lock;
 static int ftdi_instances = 0;
 static struct list_head ftdi_static_list;
 /*
-* end of the global variables protected by ftdi_module_lock
-*/
+ * end of the global variables protected by ftdi_module_lock
+ */
 #include "usb_u132.h"
 #include <asm/io.h>
 #include <linux/usb/hcd.h>
 
-	/* FIXME ohci.h is ONLY for internal use by the OHCI driver.
-	 * If you're going to try stuff like this, you need to split
-	 * out shareable stuff (register declarations?) into its own
-	 * file, maybe name <linux/usb/ohci.h>
-	 */
+/* FIXME ohci.h is ONLY for internal use by the OHCI driver.
+ * If you're going to try stuff like this, you need to split
+ * out shareable stuff (register declarations?) into its own
+ * file, maybe name <linux/usb/ohci.h>
+ */
 
 #include "../host/ohci.h"
 /* Define these values to match your devices*/
@@ -87,140 +90,140 @@ static struct list_head ftdi_static_list;
 #define USB_FTDI_ELAN_PRODUCT_ID 0xd6ea
 /* table of devices that work with this driver*/
 static const struct usb_device_id ftdi_elan_table[] = {
-        {USB_DEVICE(USB_FTDI_ELAN_VENDOR_ID, USB_FTDI_ELAN_PRODUCT_ID)},
-        { /* Terminating entry */ }
+	{USB_DEVICE(USB_FTDI_ELAN_VENDOR_ID, USB_FTDI_ELAN_PRODUCT_ID)},
+	{ /* Terminating entry */ }
 };
 
 MODULE_DEVICE_TABLE(usb, ftdi_elan_table);
 /* only the jtag(firmware upgrade device) interface requires
-* a device file and corresponding minor number, but the
-* interface is created unconditionally - I suppose it could
-* be configured or not according to a module parameter.
-* But since we(now) require one interface per device,
-* and since it unlikely that a normal installation would
-* require more than a couple of elan-ftdi devices, 8 seems
-* like a reasonable limit to have here, and if someone
-* really requires more than 8 devices, then they can frig the
-* code and recompile
-*/
+ * a device file and corresponding minor number, but the
+ * interface is created unconditionally - I suppose it could
+ * be configured or not according to a module parameter.
+ * But since we(now) require one interface per device,
+ * and since it unlikely that a normal installation would
+ * require more than a couple of elan-ftdi devices, 8 seems
+ * like a reasonable limit to have here, and if someone
+ * really requires more than 8 devices, then they can frig the
+ * code and recompile
+ */
 #define USB_FTDI_ELAN_MINOR_BASE 192
 #define COMMAND_BITS 5
 #define COMMAND_SIZE (1<<COMMAND_BITS)
 #define COMMAND_MASK (COMMAND_SIZE-1)
 struct u132_command {
-        u8 header;
-        u16 length;
-        u8 address;
-        u8 width;
-        u32 value;
-        int follows;
-        void *buffer;
+	u8 header;
+	u16 length;
+	u8 address;
+	u8 width;
+	u32 value;
+	int follows;
+	void *buffer;
 };
 #define RESPOND_BITS 5
 #define RESPOND_SIZE (1<<RESPOND_BITS)
 #define RESPOND_MASK (RESPOND_SIZE-1)
 struct u132_respond {
-        u8 header;
-        u8 address;
-        u32 *value;
-        int *result;
-        struct completion wait_completion;
+	u8 header;
+	u8 address;
+	u32 *value;
+	int *result;
+	struct completion wait_completion;
 };
 struct u132_target {
-        void *endp;
-        struct urb *urb;
-        int toggle_bits;
-        int error_count;
-        int condition_code;
-        int repeat_number;
-        int halted;
-        int skipped;
-        int actual;
-        int non_null;
-        int active;
-        int abandoning;
-        void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
-                int toggle_bits, int error_count, int condition_code,
-                int repeat_number, int halted, int skipped, int actual,
-                int non_null);
+	void *endp;
+	struct urb *urb;
+	int toggle_bits;
+	int error_count;
+	int condition_code;
+	int repeat_number;
+	int halted;
+	int skipped;
+	int actual;
+	int non_null;
+	int active;
+	int abandoning;
+	void (*callback)(void *endp, struct urb *urb, u8 *buf, int len,
+			 int toggle_bits, int error_count, int condition_code,
+			 int repeat_number, int halted, int skipped, int actual,
+			 int non_null);
 };
 /* Structure to hold all of our device specific stuff*/
 struct usb_ftdi {
-        struct list_head ftdi_list;
-        struct mutex u132_lock;
-        int command_next;
-        int command_head;
-        struct u132_command command[COMMAND_SIZE];
-        int respond_next;
-        int respond_head;
-        struct u132_respond respond[RESPOND_SIZE];
-        struct u132_target target[4];
-        char device_name[16];
-        unsigned synchronized:1;
-        unsigned enumerated:1;
-        unsigned registered:1;
-        unsigned initialized:1;
-        unsigned card_ejected:1;
-        int function;
-        int sequence_num;
-        int disconnected;
-        int gone_away;
-        int stuck_status;
-        int status_queue_delay;
-        struct semaphore sw_lock;
-        struct usb_device *udev;
-        struct usb_interface *interface;
-        struct usb_class_driver *class;
-        struct delayed_work status_work;
-        struct delayed_work command_work;
-        struct delayed_work respond_work;
-        struct u132_platform_data platform_data;
-        struct resource resources[0];
-        struct platform_device platform_dev;
-        unsigned char *bulk_in_buffer;
-        size_t bulk_in_size;
-        size_t bulk_in_last;
-        size_t bulk_in_left;
-        __u8 bulk_in_endpointAddr;
-        __u8 bulk_out_endpointAddr;
-        struct kref kref;
-        u32 controlreg;
-        u8 response[4 + 1024];
-        int expected;
-        int received;
-        int ed_found;
+	struct list_head ftdi_list;
+	struct mutex u132_lock;
+	int command_next;
+	int command_head;
+	struct u132_command command[COMMAND_SIZE];
+	int respond_next;
+	int respond_head;
+	struct u132_respond respond[RESPOND_SIZE];
+	struct u132_target target[4];
+	char device_name[16];
+	unsigned synchronized:1;
+	unsigned enumerated:1;
+	unsigned registered:1;
+	unsigned initialized:1;
+	unsigned card_ejected:1;
+	int function;
+	int sequence_num;
+	int disconnected;
+	int gone_away;
+	int stuck_status;
+	int status_queue_delay;
+	struct semaphore sw_lock;
+	struct usb_device *udev;
+	struct usb_interface *interface;
+	struct usb_class_driver *class;
+	struct delayed_work status_work;
+	struct delayed_work command_work;
+	struct delayed_work respond_work;
+	struct u132_platform_data platform_data;
+	struct resource resources[0];
+	struct platform_device platform_dev;
+	unsigned char *bulk_in_buffer;
+	size_t bulk_in_size;
+	size_t bulk_in_last;
+	size_t bulk_in_left;
+	__u8 bulk_in_endpointAddr;
+	__u8 bulk_out_endpointAddr;
+	struct kref kref;
+	u32 controlreg;
+	u8 response[4 + 1024];
+	int expected;
+	int received;
+	int ed_found;
 };
 #define kref_to_usb_ftdi(d) container_of(d, struct usb_ftdi, kref)
 #define platform_device_to_usb_ftdi(d) container_of(d, struct usb_ftdi, \
-        platform_dev)
+						    platform_dev)
 static struct usb_driver ftdi_elan_driver;
 static void ftdi_elan_delete(struct kref *kref)
 {
-        struct usb_ftdi *ftdi = kref_to_usb_ftdi(kref);
-        dev_warn(&ftdi->udev->dev, "FREEING ftdi=%p\n", ftdi);
-        usb_put_dev(ftdi->udev);
-        ftdi->disconnected += 1;
-        mutex_lock(&ftdi_module_lock);
-        list_del_init(&ftdi->ftdi_list);
-        ftdi_instances -= 1;
-        mutex_unlock(&ftdi_module_lock);
-        kfree(ftdi->bulk_in_buffer);
-        ftdi->bulk_in_buffer = NULL;
+	struct usb_ftdi *ftdi = kref_to_usb_ftdi(kref);
+	dev_warn(&ftdi->udev->dev, "FREEING ftdi=%p\n", ftdi);
+	usb_put_dev(ftdi->udev);
+	ftdi->disconnected += 1;
+	mutex_lock(&ftdi_module_lock);
+	list_del_init(&ftdi->ftdi_list);
+	ftdi_instances -= 1;
+	mutex_unlock(&ftdi_module_lock);
+	kfree(ftdi->bulk_in_buffer);
+	ftdi->bulk_in_buffer = NULL;
 }
 
 static void ftdi_elan_put_kref(struct usb_ftdi *ftdi)
 {
-        kref_put(&ftdi->kref, ftdi_elan_delete);
+	kref_put(&ftdi->kref, ftdi_elan_delete);
 }
 
 static void ftdi_elan_get_kref(struct usb_ftdi *ftdi)
 {
-        kref_get(&ftdi->kref);
+	kref_get(&ftdi->kref);
 }
 
 static void ftdi_elan_init_kref(struct usb_ftdi *ftdi)
 {
-        kref_init(&ftdi->kref);
+	kref_init(&ftdi->kref);
 }
 
 static void ftdi_status_requeue_work(struct usb_ftdi *ftdi, unsigned int delta)
@@ -237,8 +240,8 @@ static void ftdi_status_queue_work(struct usb_ftdi *ftdi, unsigned int delta)
 
 static void ftdi_status_cancel_work(struct usb_ftdi *ftdi)
 {
-        if (cancel_delayed_work(&ftdi->status_work))
-                kref_put(&ftdi->kref, ftdi_elan_delete);
+	if (cancel_delayed_work(&ftdi->status_work))
+		kref_put(&ftdi->kref, ftdi_elan_delete);
 }
 
 static void ftdi_command_requeue_work(struct usb_ftdi *ftdi, unsigned int delta)
@@ -255,12 +258,12 @@ static void ftdi_command_queue_work(struct usb_ftdi *ftdi, unsigned int delta)
 
 static void ftdi_command_cancel_work(struct usb_ftdi *ftdi)
 {
-        if (cancel_delayed_work(&ftdi->command_work))
-                kref_put(&ftdi->kref, ftdi_elan_delete);
+	if (cancel_delayed_work(&ftdi->command_work))
+		kref_put(&ftdi->kref, ftdi_elan_delete);
 }
 
 static void ftdi_response_requeue_work(struct usb_ftdi *ftdi,
-        unsigned int delta)
+				       unsigned int delta)
 {
 	if (!queue_delayed_work(respond_queue, &ftdi->respond_work, delta))
 		kref_put(&ftdi->kref, ftdi_elan_delete);
@@ -274,26 +277,26 @@ static void ftdi_respond_queue_work(struct usb_ftdi *ftdi, unsigned int delta)
 
 static void ftdi_response_cancel_work(struct usb_ftdi *ftdi)
 {
-        if (cancel_delayed_work(&ftdi->respond_work))
-                kref_put(&ftdi->kref, ftdi_elan_delete);
+	if (cancel_delayed_work(&ftdi->respond_work))
+		kref_put(&ftdi->kref, ftdi_elan_delete);
 }
 
 void ftdi_elan_gone_away(struct platform_device *pdev)
 {
-        struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
-        ftdi->gone_away += 1;
-        ftdi_elan_put_kref(ftdi);
+	struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
+	ftdi->gone_away += 1;
+	ftdi_elan_put_kref(ftdi);
 }
 
 
 EXPORT_SYMBOL_GPL(ftdi_elan_gone_away);
 static void ftdi_release_platform_dev(struct device *dev)
 {
-        dev->parent = NULL;
+	dev->parent = NULL;
 }
 
 static void ftdi_elan_do_callback(struct usb_ftdi *ftdi,
-        struct u132_target *target, u8 *buffer, int length);
+				  struct u132_target *target, u8 *buffer, int length);
 static void ftdi_elan_kick_command_queue(struct usb_ftdi *ftdi);
 static void ftdi_elan_kick_respond_queue(struct usb_ftdi *ftdi);
 static int ftdi_elan_setupOHCI(struct usb_ftdi *ftdi);
@@ -305,421 +308,416 @@ static int ftdi_elan_command_engine(struct usb_ftdi *ftdi);
 static int ftdi_elan_respond_engine(struct usb_ftdi *ftdi);
 static int ftdi_elan_hcd_init(struct usb_ftdi *ftdi)
 {
-        int result;
-        if (ftdi->platform_dev.dev.parent)
-                return -EBUSY;
-        ftdi_elan_get_kref(ftdi);
-        ftdi->platform_data.potpg = 100;
-        ftdi->platform_data.reset = NULL;
-        ftdi->platform_dev.id = ftdi->sequence_num;
-        ftdi->platform_dev.resource = ftdi->resources;
-        ftdi->platform_dev.num_resources = ARRAY_SIZE(ftdi->resources);
-        ftdi->platform_dev.dev.platform_data = &ftdi->platform_data;
-        ftdi->platform_dev.dev.parent = NULL;
-        ftdi->platform_dev.dev.release = ftdi_release_platform_dev;
-        ftdi->platform_dev.dev.dma_mask = NULL;
-        snprintf(ftdi->device_name, sizeof(ftdi->device_name), "u132_hcd");
-        ftdi->platform_dev.name = ftdi->device_name;
-        dev_info(&ftdi->udev->dev, "requesting module '%s'\n", "u132_hcd");
-        request_module("u132_hcd");
-        dev_info(&ftdi->udev->dev, "registering '%s'\n",
-                ftdi->platform_dev.name);
-        result = platform_device_register(&ftdi->platform_dev);
-        return result;
+	int result;
+	if (ftdi->platform_dev.dev.parent)
+		return -EBUSY;
+	ftdi_elan_get_kref(ftdi);
+	ftdi->platform_data.potpg = 100;
+	ftdi->platform_data.reset = NULL;
+	ftdi->platform_dev.id = ftdi->sequence_num;
+	ftdi->platform_dev.resource = ftdi->resources;
+	ftdi->platform_dev.num_resources = ARRAY_SIZE(ftdi->resources);
+	ftdi->platform_dev.dev.platform_data = &ftdi->platform_data;
+	ftdi->platform_dev.dev.parent = NULL;
+	ftdi->platform_dev.dev.release = ftdi_release_platform_dev;
+	ftdi->platform_dev.dev.dma_mask = NULL;
+	snprintf(ftdi->device_name, sizeof(ftdi->device_name), "u132_hcd");
+	ftdi->platform_dev.name = ftdi->device_name;
+	dev_info(&ftdi->udev->dev, "requesting module '%s'\n", "u132_hcd");
+	request_module("u132_hcd");
+	dev_info(&ftdi->udev->dev, "registering '%s'\n",
+		 ftdi->platform_dev.name);
+	result = platform_device_register(&ftdi->platform_dev);
+	return result;
 }
 
 static void ftdi_elan_abandon_completions(struct usb_ftdi *ftdi)
 {
-        mutex_lock(&ftdi->u132_lock);
-        while (ftdi->respond_next > ftdi->respond_head) {
-                struct u132_respond *respond = &ftdi->respond[RESPOND_MASK &
-                        ftdi->respond_head++];
-                *respond->result = -ESHUTDOWN;
-                *respond->value = 0;
-                complete(&respond->wait_completion);
-        } mutex_unlock(&ftdi->u132_lock);
+	mutex_lock(&ftdi->u132_lock);
+	while (ftdi->respond_next > ftdi->respond_head) {
+		struct u132_respond *respond = &ftdi->respond[RESPOND_MASK &
+							      ftdi->respond_head++];
+		*respond->result = -ESHUTDOWN;
+		*respond->value = 0;
+		complete(&respond->wait_completion);
+	} mutex_unlock(&ftdi->u132_lock);
 }
 
 static void ftdi_elan_abandon_targets(struct usb_ftdi *ftdi)
 {
-        int ed_number = 4;
-        mutex_lock(&ftdi->u132_lock);
-        while (ed_number-- > 0) {
-                struct u132_target *target = &ftdi->target[ed_number];
-                if (target->active == 1) {
-                        target->condition_code = TD_DEVNOTRESP;
-                        mutex_unlock(&ftdi->u132_lock);
-                        ftdi_elan_do_callback(ftdi, target, NULL, 0);
-                        mutex_lock(&ftdi->u132_lock);
-                }
-        }
-        ftdi->received = 0;
-        ftdi->expected = 4;
-        ftdi->ed_found = 0;
-        mutex_unlock(&ftdi->u132_lock);
+	int ed_number = 4;
+	mutex_lock(&ftdi->u132_lock);
+	while (ed_number-- > 0) {
+		struct u132_target *target = &ftdi->target[ed_number];
+		if (target->active == 1) {
+			target->condition_code = TD_DEVNOTRESP;
+			mutex_unlock(&ftdi->u132_lock);
+			ftdi_elan_do_callback(ftdi, target, NULL, 0);
+			mutex_lock(&ftdi->u132_lock);
+		}
+	}
+	ftdi->received = 0;
+	ftdi->expected = 4;
+	ftdi->ed_found = 0;
+	mutex_unlock(&ftdi->u132_lock);
 }
 
 static void ftdi_elan_flush_targets(struct usb_ftdi *ftdi)
 {
-        int ed_number = 4;
-        mutex_lock(&ftdi->u132_lock);
-        while (ed_number-- > 0) {
-                struct u132_target *target = &ftdi->target[ed_number];
-                target->abandoning = 1;
-              wait_1:if (target->active == 1) {
-                        int command_size = ftdi->command_next -
-                                ftdi->command_head;
-                        if (command_size < COMMAND_SIZE) {
-                                struct u132_command *command = &ftdi->command[
-                                        COMMAND_MASK & ftdi->command_next];
-                                command->header = 0x80 | (ed_number << 5) | 0x4;
-                                command->length = 0x00;
-                                command->address = 0x00;
-                                command->width = 0x00;
-                                command->follows = 0;
-                                command->value = 0;
-                                command->buffer = &command->value;
-                                ftdi->command_next += 1;
-                                ftdi_elan_kick_command_queue(ftdi);
-                        } else {
-                                mutex_unlock(&ftdi->u132_lock);
-                                msleep(100);
-                                mutex_lock(&ftdi->u132_lock);
-                                goto wait_1;
-                        }
-                }
-              wait_2:if (target->active == 1) {
-                        int command_size = ftdi->command_next -
-                                ftdi->command_head;
-                        if (command_size < COMMAND_SIZE) {
-                                struct u132_command *command = &ftdi->command[
-                                        COMMAND_MASK & ftdi->command_next];
-                                command->header = 0x90 | (ed_number << 5);
-                                command->length = 0x00;
-                                command->address = 0x00;
-                                command->width = 0x00;
-                                command->follows = 0;
-                                command->value = 0;
-                                command->buffer = &command->value;
-                                ftdi->command_next += 1;
-                                ftdi_elan_kick_command_queue(ftdi);
-                        } else {
-                                mutex_unlock(&ftdi->u132_lock);
-                                msleep(100);
-                                mutex_lock(&ftdi->u132_lock);
-                                goto wait_2;
-                        }
-                }
-        }
-        ftdi->received = 0;
-        ftdi->expected = 4;
-        ftdi->ed_found = 0;
-        mutex_unlock(&ftdi->u132_lock);
+	int ed_number = 4;
+	mutex_lock(&ftdi->u132_lock);
+	while (ed_number-- > 0) {
+		struct u132_target *target = &ftdi->target[ed_number];
+		target->abandoning = 1;
+	wait_1:if (target->active == 1) {
+			int command_size = ftdi->command_next -
+				ftdi->command_head;
+			if (command_size < COMMAND_SIZE) {
+				struct u132_command *command = &ftdi->command[
+					COMMAND_MASK & ftdi->command_next];
+				command->header = 0x80 | (ed_number << 5) | 0x4;
+				command->length = 0x00;
+				command->address = 0x00;
+				command->width = 0x00;
+				command->follows = 0;
+				command->value = 0;
+				command->buffer = &command->value;
+				ftdi->command_next += 1;
+				ftdi_elan_kick_command_queue(ftdi);
+			} else {
+				mutex_unlock(&ftdi->u132_lock);
+				msleep(100);
+				mutex_lock(&ftdi->u132_lock);
+				goto wait_1;
+			}
+		}
+	wait_2:if (target->active == 1) {
+			int command_size = ftdi->command_next -
+				ftdi->command_head;
+			if (command_size < COMMAND_SIZE) {
+				struct u132_command *command = &ftdi->command[
+					COMMAND_MASK & ftdi->command_next];
+				command->header = 0x90 | (ed_number << 5);
+				command->length = 0x00;
+				command->address = 0x00;
+				command->width = 0x00;
+				command->follows = 0;
+				command->value = 0;
+				command->buffer = &command->value;
+				ftdi->command_next += 1;
+				ftdi_elan_kick_command_queue(ftdi);
+			} else {
+				mutex_unlock(&ftdi->u132_lock);
+				msleep(100);
+				mutex_lock(&ftdi->u132_lock);
+				goto wait_2;
+			}
+		}
+	}
+	ftdi->received = 0;
+	ftdi->expected = 4;
+	ftdi->ed_found = 0;
+	mutex_unlock(&ftdi->u132_lock);
 }
 
 static void ftdi_elan_cancel_targets(struct usb_ftdi *ftdi)
 {
-        int ed_number = 4;
-        mutex_lock(&ftdi->u132_lock);
-        while (ed_number-- > 0) {
-                struct u132_target *target = &ftdi->target[ed_number];
-                target->abandoning = 1;
-              wait:if (target->active == 1) {
-                        int command_size = ftdi->command_next -
-                                ftdi->command_head;
-                        if (command_size < COMMAND_SIZE) {
-                                struct u132_command *command = &ftdi->command[
-                                        COMMAND_MASK & ftdi->command_next];
-                                command->header = 0x80 | (ed_number << 5) | 0x4;
-                                command->length = 0x00;
-                                command->address = 0x00;
-                                command->width = 0x00;
-                                command->follows = 0;
-                                command->value = 0;
-                                command->buffer = &command->value;
-                                ftdi->command_next += 1;
-                                ftdi_elan_kick_command_queue(ftdi);
-                        } else {
-                                mutex_unlock(&ftdi->u132_lock);
-                                msleep(100);
-                                mutex_lock(&ftdi->u132_lock);
-                                goto wait;
-                        }
-                }
-        }
-        ftdi->received = 0;
-        ftdi->expected = 4;
-        ftdi->ed_found = 0;
-        mutex_unlock(&ftdi->u132_lock);
+	int ed_number = 4;
+	mutex_lock(&ftdi->u132_lock);
+	while (ed_number-- > 0) {
+		struct u132_target *target = &ftdi->target[ed_number];
+		target->abandoning = 1;
+	wait:if (target->active == 1) {
+			int command_size = ftdi->command_next -
+				ftdi->command_head;
+			if (command_size < COMMAND_SIZE) {
+				struct u132_command *command = &ftdi->command[
+					COMMAND_MASK & ftdi->command_next];
+				command->header = 0x80 | (ed_number << 5) | 0x4;
+				command->length = 0x00;
+				command->address = 0x00;
+				command->width = 0x00;
+				command->follows = 0;
+				command->value = 0;
+				command->buffer = &command->value;
+				ftdi->command_next += 1;
+				ftdi_elan_kick_command_queue(ftdi);
+			} else {
+				mutex_unlock(&ftdi->u132_lock);
+				msleep(100);
+				mutex_lock(&ftdi->u132_lock);
+				goto wait;
+			}
+		}
+	}
+	ftdi->received = 0;
+	ftdi->expected = 4;
+	ftdi->ed_found = 0;
+	mutex_unlock(&ftdi->u132_lock);
 }
 
 static void ftdi_elan_kick_command_queue(struct usb_ftdi *ftdi)
 {
-        ftdi_command_queue_work(ftdi, 0);
+	ftdi_command_queue_work(ftdi, 0);
 }
 
 static void ftdi_elan_command_work(struct work_struct *work)
 {
-        struct usb_ftdi *ftdi =
+	struct usb_ftdi *ftdi =
 		container_of(work, struct usb_ftdi, command_work.work);
 
-        if (ftdi->disconnected > 0) {
-                ftdi_elan_put_kref(ftdi);
-                return;
-        } else {
-                int retval = ftdi_elan_command_engine(ftdi);
-                if (retval == -ESHUTDOWN) {
-                        ftdi->disconnected += 1;
-                } else if (retval == -ENODEV) {
-                        ftdi->disconnected += 1;
-                } else if (retval)
-                        dev_err(&ftdi->udev->dev, "command error %d\n", retval);
-                ftdi_command_requeue_work(ftdi, msecs_to_jiffies(10));
-                return;
-        }
+	if (ftdi->disconnected > 0) {
+		ftdi_elan_put_kref(ftdi);
+		return;
+	} else {
+		int retval = ftdi_elan_command_engine(ftdi);
+		if (retval == -ESHUTDOWN) {
+			ftdi->disconnected += 1;
+		} else if (retval == -ENODEV) {
+			ftdi->disconnected += 1;
+		} else if (retval)
+			dev_err(&ftdi->udev->dev, "command error %d\n", retval);
+		ftdi_command_requeue_work(ftdi, msecs_to_jiffies(10));
+		return;
+	}
 }
 
 static void ftdi_elan_kick_respond_queue(struct usb_ftdi *ftdi)
 {
-        ftdi_respond_queue_work(ftdi, 0);
+	ftdi_respond_queue_work(ftdi, 0);
 }
 
 static void ftdi_elan_respond_work(struct work_struct *work)
 {
-        struct usb_ftdi *ftdi =
+	struct usb_ftdi *ftdi =
 		container_of(work, struct usb_ftdi, respond_work.work);
-        if (ftdi->disconnected > 0) {
-                ftdi_elan_put_kref(ftdi);
-                return;
-        } else {
-                int retval = ftdi_elan_respond_engine(ftdi);
-                if (retval == 0) {
-                } else if (retval == -ESHUTDOWN) {
-                        ftdi->disconnected += 1;
-                } else if (retval == -ENODEV) {
-                        ftdi->disconnected += 1;
-                } else if (retval == -EILSEQ) {
-                        ftdi->disconnected += 1;
-                } else {
-                        ftdi->disconnected += 1;
-                        dev_err(&ftdi->udev->dev, "respond error %d\n", retval);
-                }
-                if (ftdi->disconnected > 0) {
-                        ftdi_elan_abandon_completions(ftdi);
-                        ftdi_elan_abandon_targets(ftdi);
-                }
-                ftdi_response_requeue_work(ftdi, msecs_to_jiffies(10));
-                return;
-        }
+	if (ftdi->disconnected > 0) {
+		ftdi_elan_put_kref(ftdi);
+		return;
+	} else {
+		int retval = ftdi_elan_respond_engine(ftdi);
+		if (retval == 0) {
+		} else if (retval == -ESHUTDOWN) {
+			ftdi->disconnected += 1;
+		} else if (retval == -ENODEV) {
+			ftdi->disconnected += 1;
+		} else if (retval == -EILSEQ) {
+			ftdi->disconnected += 1;
+		} else {
+			ftdi->disconnected += 1;
+			dev_err(&ftdi->udev->dev, "respond error %d\n", retval);
+		}
+		if (ftdi->disconnected > 0) {
+			ftdi_elan_abandon_completions(ftdi);
+			ftdi_elan_abandon_targets(ftdi);
+		}
+		ftdi_response_requeue_work(ftdi, msecs_to_jiffies(10));
+		return;
+	}
 }
 
 
 /*
-* the sw_lock is initially held and will be freed
-* after the FTDI has been synchronized
-*
-*/
+ * the sw_lock is initially held and will be freed
+ * after the FTDI has been synchronized
+ *
+ */
 static void ftdi_elan_status_work(struct work_struct *work)
 {
-        struct usb_ftdi *ftdi =
+	struct usb_ftdi *ftdi =
 		container_of(work, struct usb_ftdi, status_work.work);
-        int work_delay_in_msec = 0;
-        if (ftdi->disconnected > 0) {
-                ftdi_elan_put_kref(ftdi);
-                return;
-        } else if (ftdi->synchronized == 0) {
-                down(&ftdi->sw_lock);
-                if (ftdi_elan_synchronize(ftdi) == 0) {
-                        ftdi->synchronized = 1;
-                        ftdi_command_queue_work(ftdi, 1);
-                        ftdi_respond_queue_work(ftdi, 1);
-                        up(&ftdi->sw_lock);
-                        work_delay_in_msec = 100;
-                } else {
-                        dev_err(&ftdi->udev->dev, "synchronize failed\n");
-                        up(&ftdi->sw_lock);
-                        work_delay_in_msec = 10 *1000;
-                }
-        } else if (ftdi->stuck_status > 0) {
-                if (ftdi_elan_stuck_waiting(ftdi) == 0) {
-                        ftdi->stuck_status = 0;
-                        ftdi->synchronized = 0;
-                } else if ((ftdi->stuck_status++ % 60) == 1) {
-                        dev_err(&ftdi->udev->dev, "WRONG type of card inserted "
-                                "- please remove\n");
-                } else
-                        dev_err(&ftdi->udev->dev, "WRONG type of card inserted "
-                                "- checked %d times\n", ftdi->stuck_status);
-                work_delay_in_msec = 100;
-        } else if (ftdi->enumerated == 0) {
-                if (ftdi_elan_enumeratePCI(ftdi) == 0) {
-                        ftdi->enumerated = 1;
-                        work_delay_in_msec = 250;
-                } else
-                        work_delay_in_msec = 1000;
-        } else if (ftdi->initialized == 0) {
-                if (ftdi_elan_setupOHCI(ftdi) == 0) {
-                        ftdi->initialized = 1;
-                        work_delay_in_msec = 500;
-                } else {
-                        dev_err(&ftdi->udev->dev, "initialized failed - trying "
-                                "again in 10 seconds\n");
-                        work_delay_in_msec = 1 *1000;
-                }
-        } else if (ftdi->registered == 0) {
-                work_delay_in_msec = 10;
-                if (ftdi_elan_hcd_init(ftdi) == 0) {
-                        ftdi->registered = 1;
-                } else
-                        dev_err(&ftdi->udev->dev, "register failed\n");
-                work_delay_in_msec = 250;
-        } else {
-                if (ftdi_elan_checkingPCI(ftdi) == 0) {
-                        work_delay_in_msec = 250;
-                } else if (ftdi->controlreg & 0x00400000) {
-                        if (ftdi->gone_away > 0) {
-                                dev_err(&ftdi->udev->dev, "PCI device eject con"
-                                        "firmed platform_dev.dev.parent=%p plat"
-                                        "form_dev.dev=%p\n",
-                                        ftdi->platform_dev.dev.parent,
-                                        &ftdi->platform_dev.dev);
-                                platform_device_unregister(&ftdi->platform_dev);
-                                ftdi->platform_dev.dev.parent = NULL;
-                                ftdi->registered = 0;
-                                ftdi->enumerated = 0;
-                                ftdi->card_ejected = 0;
-                                ftdi->initialized = 0;
-                                ftdi->gone_away = 0;
-                        } else
-                                ftdi_elan_flush_targets(ftdi);
-                        work_delay_in_msec = 250;
-                } else {
-                        dev_err(&ftdi->udev->dev, "PCI device has disappeared\n"
-                                );
-                        ftdi_elan_cancel_targets(ftdi);
-                        work_delay_in_msec = 500;
-                        ftdi->enumerated = 0;
-                        ftdi->initialized = 0;
-                }
-        }
-        if (ftdi->disconnected > 0) {
-                ftdi_elan_put_kref(ftdi);
-                return;
-        } else {
-                ftdi_status_requeue_work(ftdi,
-                        msecs_to_jiffies(work_delay_in_msec));
-                return;
-        }
+	int work_delay_in_msec = 0;
+	if (ftdi->disconnected > 0) {
+		ftdi_elan_put_kref(ftdi);
+		return;
+	} else if (ftdi->synchronized == 0) {
+		down(&ftdi->sw_lock);
+		if (ftdi_elan_synchronize(ftdi) == 0) {
+			ftdi->synchronized = 1;
+			ftdi_command_queue_work(ftdi, 1);
+			ftdi_respond_queue_work(ftdi, 1);
+			up(&ftdi->sw_lock);
+			work_delay_in_msec = 100;
+		} else {
+			dev_err(&ftdi->udev->dev, "synchronize failed\n");
+			up(&ftdi->sw_lock);
+			work_delay_in_msec = 10 *1000;
+		}
+	} else if (ftdi->stuck_status > 0) {
+		if (ftdi_elan_stuck_waiting(ftdi) == 0) {
+			ftdi->stuck_status = 0;
+			ftdi->synchronized = 0;
+		} else if ((ftdi->stuck_status++ % 60) == 1) {
+			dev_err(&ftdi->udev->dev, "WRONG type of card inserted - please remove\n");
+		} else
+			dev_err(&ftdi->udev->dev, "WRONG type of card inserted - checked %d times\n",
+				ftdi->stuck_status);
+		work_delay_in_msec = 100;
+	} else if (ftdi->enumerated == 0) {
+		if (ftdi_elan_enumeratePCI(ftdi) == 0) {
+			ftdi->enumerated = 1;
+			work_delay_in_msec = 250;
+		} else
+			work_delay_in_msec = 1000;
+	} else if (ftdi->initialized == 0) {
+		if (ftdi_elan_setupOHCI(ftdi) == 0) {
+			ftdi->initialized = 1;
+			work_delay_in_msec = 500;
+		} else {
+			dev_err(&ftdi->udev->dev, "initialized failed - trying again in 10 seconds\n");
+			work_delay_in_msec = 1 *1000;
+		}
+	} else if (ftdi->registered == 0) {
+		work_delay_in_msec = 10;
+		if (ftdi_elan_hcd_init(ftdi) == 0) {
+			ftdi->registered = 1;
+		} else
+			dev_err(&ftdi->udev->dev, "register failed\n");
+		work_delay_in_msec = 250;
+	} else {
+		if (ftdi_elan_checkingPCI(ftdi) == 0) {
+			work_delay_in_msec = 250;
+		} else if (ftdi->controlreg & 0x00400000) {
+			if (ftdi->gone_away > 0) {
+				dev_err(&ftdi->udev->dev, "PCI device eject confirmed platform_dev.dev.parent=%p platform_dev.dev=%p\n",
+					ftdi->platform_dev.dev.parent,
+					&ftdi->platform_dev.dev);
+				platform_device_unregister(&ftdi->platform_dev);
+				ftdi->platform_dev.dev.parent = NULL;
+				ftdi->registered = 0;
+				ftdi->enumerated = 0;
+				ftdi->card_ejected = 0;
+				ftdi->initialized = 0;
+				ftdi->gone_away = 0;
+			} else
+				ftdi_elan_flush_targets(ftdi);
+			work_delay_in_msec = 250;
+		} else {
+			dev_err(&ftdi->udev->dev, "PCI device has disappeared\n");
+			ftdi_elan_cancel_targets(ftdi);
+			work_delay_in_msec = 500;
+			ftdi->enumerated = 0;
+			ftdi->initialized = 0;
+		}
+	}
+	if (ftdi->disconnected > 0) {
+		ftdi_elan_put_kref(ftdi);
+		return;
+	} else {
+		ftdi_status_requeue_work(ftdi,
+					 msecs_to_jiffies(work_delay_in_msec));
+		return;
+	}
 }
 
 
 /*
-* file_operations for the jtag interface
-*
-* the usage count for the device is incremented on open()
-* and decremented on release()
-*/
+ * file_operations for the jtag interface
+ *
+ * the usage count for the device is incremented on open()
+ * and decremented on release()
+ */
 static int ftdi_elan_open(struct inode *inode, struct file *file)
 {
 	int subminor;
 	struct usb_interface *interface;
 
-        subminor = iminor(inode);
-        interface = usb_find_interface(&ftdi_elan_driver, subminor);
-
-        if (!interface) {
-                printk(KERN_ERR "can't find device for minor %d\n", subminor);
-                return -ENODEV;
-        } else {
-                struct usb_ftdi *ftdi = usb_get_intfdata(interface);
-                if (!ftdi) {
-                        return -ENODEV;
-                } else {
-                        if (down_interruptible(&ftdi->sw_lock)) {
-                                return -EINTR;
-                        } else {
-                                ftdi_elan_get_kref(ftdi);
-                                file->private_data = ftdi;
-                                return 0;
-                        }
-                }
-        }
+	subminor = iminor(inode);
+	interface = usb_find_interface(&ftdi_elan_driver, subminor);
+
+	if (!interface) {
+		pr_err("can't find device for minor %d\n", subminor);
+		return -ENODEV;
+	} else {
+		struct usb_ftdi *ftdi = usb_get_intfdata(interface);
+		if (!ftdi) {
+			return -ENODEV;
+		} else {
+			if (down_interruptible(&ftdi->sw_lock)) {
+				return -EINTR;
+			} else {
+				ftdi_elan_get_kref(ftdi);
+				file->private_data = ftdi;
+				return 0;
+			}
+		}
+	}
 }
 
 static int ftdi_elan_release(struct inode *inode, struct file *file)
 {
-        struct usb_ftdi *ftdi = file->private_data;
-        if (ftdi == NULL)
-                return -ENODEV;
-        up(&ftdi->sw_lock);        /* decrement the count on our device */
-        ftdi_elan_put_kref(ftdi);
-        return 0;
+	struct usb_ftdi *ftdi = file->private_data;
+	if (ftdi == NULL)
+		return -ENODEV;
+	up(&ftdi->sw_lock);        /* decrement the count on our device */
+	ftdi_elan_put_kref(ftdi);
+	return 0;
 }
 
 
 /*
-*
-* blocking bulk reads are used to get data from the device
-*
-*/
+ *
+ * blocking bulk reads are used to get data from the device
+ *
+ */
 static ssize_t ftdi_elan_read(struct file *file, char __user *buffer,
 			      size_t count, loff_t *ppos)
 {
-        char data[30 *3 + 4];
-        char *d = data;
-        int m = (sizeof(data) - 1) / 3;
-        int bytes_read = 0;
-        int retry_on_empty = 10;
-        int retry_on_timeout = 5;
-        struct usb_ftdi *ftdi = file->private_data;
-        if (ftdi->disconnected > 0) {
-                return -ENODEV;
-        }
-        data[0] = 0;
-      have:if (ftdi->bulk_in_left > 0) {
-                if (count-- > 0) {
-                        char *p = ++ftdi->bulk_in_last + ftdi->bulk_in_buffer;
-                        ftdi->bulk_in_left -= 1;
-                        if (bytes_read < m) {
-                                d += sprintf(d, " %02X", 0x000000FF & *p);
-                        } else if (bytes_read > m) {
-                        } else
-                                d += sprintf(d, " ..");
-                        if (copy_to_user(buffer++, p, 1)) {
-                                return -EFAULT;
-                        } else {
-                                bytes_read += 1;
-                                goto have;
-                        }
-                } else
-                        return bytes_read;
-        }
-      more:if (count > 0) {
-                int packet_bytes = 0;
-                int retval = usb_bulk_msg(ftdi->udev,
-                        usb_rcvbulkpipe(ftdi->udev, ftdi->bulk_in_endpointAddr),
-                         ftdi->bulk_in_buffer, ftdi->bulk_in_size,
-                        &packet_bytes, 50);
-                if (packet_bytes > 2) {
-                        ftdi->bulk_in_left = packet_bytes - 2;
-                        ftdi->bulk_in_last = 1;
-                        goto have;
-                } else if (retval == -ETIMEDOUT) {
-                        if (retry_on_timeout-- > 0) {
-                                goto more;
-                        } else if (bytes_read > 0) {
-                                return bytes_read;
-                        } else
-                                return retval;
-                } else if (retval == 0) {
-                        if (retry_on_empty-- > 0) {
-                                goto more;
-                        } else
-                                return bytes_read;
-                } else
-                        return retval;
-        } else
-                return bytes_read;
+	char data[30 *3 + 4];
+	char *d = data;
+	int m = (sizeof(data) - 1) / 3;
+	int bytes_read = 0;
+	int retry_on_empty = 10;
+	int retry_on_timeout = 5;
+	struct usb_ftdi *ftdi = file->private_data;
+	if (ftdi->disconnected > 0) {
+		return -ENODEV;
+	}
+	data[0] = 0;
+have:if (ftdi->bulk_in_left > 0) {
+		if (count-- > 0) {
+			char *p = ++ftdi->bulk_in_last + ftdi->bulk_in_buffer;
+			ftdi->bulk_in_left -= 1;
+			if (bytes_read < m) {
+				d += sprintf(d, " %02X", 0x000000FF & *p);
+			} else if (bytes_read > m) {
+			} else
+				d += sprintf(d, " ..");
+			if (copy_to_user(buffer++, p, 1)) {
+				return -EFAULT;
+			} else {
+				bytes_read += 1;
+				goto have;
+			}
+		} else
+			return bytes_read;
+	}
+more:if (count > 0) {
+		int packet_bytes = 0;
+		int retval = usb_bulk_msg(ftdi->udev,
+					  usb_rcvbulkpipe(ftdi->udev, ftdi->bulk_in_endpointAddr),
+					  ftdi->bulk_in_buffer, ftdi->bulk_in_size,
+					  &packet_bytes, 50);
+		if (packet_bytes > 2) {
+			ftdi->bulk_in_left = packet_bytes - 2;
+			ftdi->bulk_in_last = 1;
+			goto have;
+		} else if (retval == -ETIMEDOUT) {
+			if (retry_on_timeout-- > 0) {
+				goto more;
+			} else if (bytes_read > 0) {
+				return bytes_read;
+			} else
+				return retval;
+		} else if (retval == 0) {
+			if (retry_on_empty-- > 0) {
+				goto more;
+			} else
+				return bytes_read;
+		} else
+			return retval;
+	} else
+		return bytes_read;
 }
 
 static void ftdi_elan_write_bulk_callback(struct urb *urb)
@@ -728,467 +726,460 @@ static void ftdi_elan_write_bulk_callback(struct urb *urb)
 	int status = urb->status;
 
 	if (status && !(status == -ENOENT || status == -ECONNRESET ||
-	    status == -ESHUTDOWN)) {
-                dev_err(&ftdi->udev->dev, "urb=%p write bulk status received: %"
-                        "d\n", urb, status);
-        }
-        usb_free_coherent(urb->dev, urb->transfer_buffer_length,
-                urb->transfer_buffer, urb->transfer_dma);
+			status == -ESHUTDOWN)) {
+		dev_err(&ftdi->udev->dev,
+			"urb=%p write bulk status received: %d\n", urb, status);
+	}
+	usb_free_coherent(urb->dev, urb->transfer_buffer_length,
+			  urb->transfer_buffer, urb->transfer_dma);
 }
 
 static int fill_buffer_with_all_queued_commands(struct usb_ftdi *ftdi,
-        char *buf, int command_size, int total_size)
-{
-        int ed_commands = 0;
-        int b = 0;
-        int I = command_size;
-        int i = ftdi->command_head;
-        while (I-- > 0) {
-                struct u132_command *command = &ftdi->command[COMMAND_MASK &
-                        i++];
-                int F = command->follows;
-                u8 *f = command->buffer;
-                if (command->header & 0x80) {
-                        ed_commands |= 1 << (0x3 & (command->header >> 5));
-                }
-                buf[b++] = command->header;
-                buf[b++] = (command->length >> 0) & 0x00FF;
-                buf[b++] = (command->length >> 8) & 0x00FF;
-                buf[b++] = command->address;
-                buf[b++] = command->width;
-                while (F-- > 0) {
-                        buf[b++] = *f++;
-                }
-        }
-        return ed_commands;
+						char *buf, int command_size, int total_size)
+{
+	int ed_commands = 0;
+	int b = 0;
+	int I = command_size;
+	int i = ftdi->command_head;
+	while (I-- > 0) {
+		struct u132_command *command = &ftdi->command[COMMAND_MASK &
+							      i++];
+		int F = command->follows;
+		u8 *f = command->buffer;
+		if (command->header & 0x80) {
+			ed_commands |= 1 << (0x3 & (command->header >> 5));
+		}
+		buf[b++] = command->header;
+		buf[b++] = (command->length >> 0) & 0x00FF;
+		buf[b++] = (command->length >> 8) & 0x00FF;
+		buf[b++] = command->address;
+		buf[b++] = command->width;
+		while (F-- > 0) {
+			buf[b++] = *f++;
+		}
+	}
+	return ed_commands;
 }
 
 static int ftdi_elan_total_command_size(struct usb_ftdi *ftdi, int command_size)
 {
-        int total_size = 0;
-        int I = command_size;
-        int i = ftdi->command_head;
-        while (I-- > 0) {
-                struct u132_command *command = &ftdi->command[COMMAND_MASK &
-                        i++];
-                total_size += 5 + command->follows;
-        } return total_size;
+	int total_size = 0;
+	int I = command_size;
+	int i = ftdi->command_head;
+	while (I-- > 0) {
+		struct u132_command *command = &ftdi->command[COMMAND_MASK &
+							      i++];
+		total_size += 5 + command->follows;
+	} return total_size;
 }
 
 static int ftdi_elan_command_engine(struct usb_ftdi *ftdi)
 {
-        int retval;
-        char *buf;
-        int ed_commands;
-        int total_size;
-        struct urb *urb;
-        int command_size = ftdi->command_next - ftdi->command_head;
-        if (command_size == 0)
-                return 0;
-        total_size = ftdi_elan_total_command_size(ftdi, command_size);
-        urb = usb_alloc_urb(0, GFP_KERNEL);
-        if (!urb) {
-                dev_err(&ftdi->udev->dev, "could not get a urb to write %d comm"
-                        "ands totaling %d bytes to the Uxxx\n", command_size,
-                        total_size);
-                return -ENOMEM;
-        }
-        buf = usb_alloc_coherent(ftdi->udev, total_size, GFP_KERNEL,
-                &urb->transfer_dma);
-        if (!buf) {
-                dev_err(&ftdi->udev->dev, "could not get a buffer to write %d c"
-                        "ommands totaling %d bytes to the Uxxx\n", command_size,
-                         total_size);
-                usb_free_urb(urb);
-                return -ENOMEM;
-        }
-        ed_commands = fill_buffer_with_all_queued_commands(ftdi, buf,
-                command_size, total_size);
-        usb_fill_bulk_urb(urb, ftdi->udev, usb_sndbulkpipe(ftdi->udev,
-                ftdi->bulk_out_endpointAddr), buf, total_size,
-                ftdi_elan_write_bulk_callback, ftdi);
-        urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
-        if (ed_commands) {
-                char diag[40 *3 + 4];
-                char *d = diag;
-                int m = total_size;
-                u8 *c = buf;
-                int s = (sizeof(diag) - 1) / 3;
-                diag[0] = 0;
-                while (s-- > 0 && m-- > 0) {
-                        if (s > 0 || m == 0) {
-                                d += sprintf(d, " %02X", *c++);
-                        } else
-                                d += sprintf(d, " ..");
-                }
-        }
-        retval = usb_submit_urb(urb, GFP_KERNEL);
-        if (retval) {
-                dev_err(&ftdi->udev->dev, "failed %d to submit urb %p to write "
-                        "%d commands totaling %d bytes to the Uxxx\n", retval,
-                        urb, command_size, total_size);
-                usb_free_coherent(ftdi->udev, total_size, buf, urb->transfer_dma);
-                usb_free_urb(urb);
-                return retval;
-        }
-        usb_free_urb(urb);        /* release our reference to this urb,
-                the USB core will eventually free it entirely */
-        ftdi->command_head += command_size;
-        ftdi_elan_kick_respond_queue(ftdi);
-        return 0;
+	int retval;
+	char *buf;
+	int ed_commands;
+	int total_size;
+	struct urb *urb;
+	int command_size = ftdi->command_next - ftdi->command_head;
+	if (command_size == 0)
+		return 0;
+	total_size = ftdi_elan_total_command_size(ftdi, command_size);
+	urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!urb) {
+		dev_err(&ftdi->udev->dev, "could not get a urb to write %d commands totaling %d bytes to the Uxxx\n",
+			command_size, total_size);
+		return -ENOMEM;
+	}
+	buf = usb_alloc_coherent(ftdi->udev, total_size, GFP_KERNEL,
+				 &urb->transfer_dma);
+	if (!buf) {
+		dev_err(&ftdi->udev->dev, "could not get a buffer to write %d commands totaling %d bytes to the Uxxx\n",
+			command_size, total_size);
+		usb_free_urb(urb);
+		return -ENOMEM;
+	}
+	ed_commands = fill_buffer_with_all_queued_commands(ftdi, buf,
+							   command_size, total_size);
+	usb_fill_bulk_urb(urb, ftdi->udev, usb_sndbulkpipe(ftdi->udev,
+							   ftdi->bulk_out_endpointAddr), buf, total_size,
+			  ftdi_elan_write_bulk_callback, ftdi);
+	urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+	if (ed_commands) {
+		char diag[40 *3 + 4];
+		char *d = diag;
+		int m = total_size;
+		u8 *c = buf;
+		int s = (sizeof(diag) - 1) / 3;
+		diag[0] = 0;
+		while (s-- > 0 && m-- > 0) {
+			if (s > 0 || m == 0) {
+				d += sprintf(d, " %02X", *c++);
+			} else
+				d += sprintf(d, " ..");
+		}
+	}
+	retval = usb_submit_urb(urb, GFP_KERNEL);
+	if (retval) {
+		dev_err(&ftdi->udev->dev, "failed %d to submit urb %p to write %d commands totaling %d bytes to the Uxxx\n",
+			retval, urb, command_size, total_size);
+		usb_free_coherent(ftdi->udev, total_size, buf, urb->transfer_dma);
+		usb_free_urb(urb);
+		return retval;
+	}
+	usb_free_urb(urb);        /* release our reference to this urb,
+				     the USB core will eventually free it entirely */
+	ftdi->command_head += command_size;
+	ftdi_elan_kick_respond_queue(ftdi);
+	return 0;
 }
 
 static void ftdi_elan_do_callback(struct usb_ftdi *ftdi,
-        struct u132_target *target, u8 *buffer, int length)
-{
-        struct urb *urb = target->urb;
-        int halted = target->halted;
-        int skipped = target->skipped;
-        int actual = target->actual;
-        int non_null = target->non_null;
-        int toggle_bits = target->toggle_bits;
-        int error_count = target->error_count;
-        int condition_code = target->condition_code;
-        int repeat_number = target->repeat_number;
-        void (*callback) (void *, struct urb *, u8 *, int, int, int, int, int,
-                int, int, int, int) = target->callback;
-        target->active -= 1;
-        target->callback = NULL;
-        (*callback) (target->endp, urb, buffer, length, toggle_bits,
-                error_count, condition_code, repeat_number, halted, skipped,
-                actual, non_null);
+				  struct u132_target *target, u8 *buffer, int length)
+{
+	struct urb *urb = target->urb;
+	int halted = target->halted;
+	int skipped = target->skipped;
+	int actual = target->actual;
+	int non_null = target->non_null;
+	int toggle_bits = target->toggle_bits;
+	int error_count = target->error_count;
+	int condition_code = target->condition_code;
+	int repeat_number = target->repeat_number;
+	void (*callback) (void *, struct urb *, u8 *, int, int, int, int, int,
+			  int, int, int, int) = target->callback;
+	target->active -= 1;
+	target->callback = NULL;
+	(*callback) (target->endp, urb, buffer, length, toggle_bits,
+		     error_count, condition_code, repeat_number, halted, skipped,
+		     actual, non_null);
 }
 
 static char *have_ed_set_response(struct usb_ftdi *ftdi,
-        struct u132_target *target, u16 ed_length, int ed_number, int ed_type,
-        char *b)
-{
-        int payload = (ed_length >> 0) & 0x07FF;
-        mutex_lock(&ftdi->u132_lock);
-        target->actual = 0;
-        target->non_null = (ed_length >> 15) & 0x0001;
-        target->repeat_number = (ed_length >> 11) & 0x000F;
-        if (ed_type == 0x02) {
-                if (payload == 0 || target->abandoning > 0) {
-                        target->abandoning = 0;
-                        mutex_unlock(&ftdi->u132_lock);
-                        ftdi_elan_do_callback(ftdi, target, 4 + ftdi->response,
-                                payload);
-                        ftdi->received = 0;
-                        ftdi->expected = 4;
-                        ftdi->ed_found = 0;
-                        return ftdi->response;
-                } else {
-                        ftdi->expected = 4 + payload;
-                        ftdi->ed_found = 1;
-                        mutex_unlock(&ftdi->u132_lock);
-                        return b;
-                }
-        } else if (ed_type == 0x03) {
-                if (payload == 0 || target->abandoning > 0) {
-                        target->abandoning = 0;
-                        mutex_unlock(&ftdi->u132_lock);
-                        ftdi_elan_do_callback(ftdi, target, 4 + ftdi->response,
-                                payload);
-                        ftdi->received = 0;
-                        ftdi->expected = 4;
-                        ftdi->ed_found = 0;
-                        return ftdi->response;
-                } else {
-                        ftdi->expected = 4 + payload;
-                        ftdi->ed_found = 1;
-                        mutex_unlock(&ftdi->u132_lock);
-                        return b;
-                }
-        } else if (ed_type == 0x01) {
-                target->abandoning = 0;
-                mutex_unlock(&ftdi->u132_lock);
-                ftdi_elan_do_callback(ftdi, target, 4 + ftdi->response,
-                        payload);
-                ftdi->received = 0;
-                ftdi->expected = 4;
-                ftdi->ed_found = 0;
-                return ftdi->response;
-        } else {
-                target->abandoning = 0;
-                mutex_unlock(&ftdi->u132_lock);
-                ftdi_elan_do_callback(ftdi, target, 4 + ftdi->response,
-                        payload);
-                ftdi->received = 0;
-                ftdi->expected = 4;
-                ftdi->ed_found = 0;
-                return ftdi->response;
-        }
+				  struct u132_target *target, u16 ed_length, int ed_number, int ed_type,
+				  char *b)
+{
+	int payload = (ed_length >> 0) & 0x07FF;
+	mutex_lock(&ftdi->u132_lock);
+	target->actual = 0;
+	target->non_null = (ed_length >> 15) & 0x0001;
+	target->repeat_number = (ed_length >> 11) & 0x000F;
+	if (ed_type == 0x02) {
+		if (payload == 0 || target->abandoning > 0) {
+			target->abandoning = 0;
+			mutex_unlock(&ftdi->u132_lock);
+			ftdi_elan_do_callback(ftdi, target, 4 + ftdi->response,
+					      payload);
+			ftdi->received = 0;
+			ftdi->expected = 4;
+			ftdi->ed_found = 0;
+			return ftdi->response;
+		} else {
+			ftdi->expected = 4 + payload;
+			ftdi->ed_found = 1;
+			mutex_unlock(&ftdi->u132_lock);
+			return b;
+		}
+	} else if (ed_type == 0x03) {
+		if (payload == 0 || target->abandoning > 0) {
+			target->abandoning = 0;
+			mutex_unlock(&ftdi->u132_lock);
+			ftdi_elan_do_callback(ftdi, target, 4 + ftdi->response,
+					      payload);
+			ftdi->received = 0;
+			ftdi->expected = 4;
+			ftdi->ed_found = 0;
+			return ftdi->response;
+		} else {
+			ftdi->expected = 4 + payload;
+			ftdi->ed_found = 1;
+			mutex_unlock(&ftdi->u132_lock);
+			return b;
+		}
+	} else if (ed_type == 0x01) {
+		target->abandoning = 0;
+		mutex_unlock(&ftdi->u132_lock);
+		ftdi_elan_do_callback(ftdi, target, 4 + ftdi->response,
+				      payload);
+		ftdi->received = 0;
+		ftdi->expected = 4;
+		ftdi->ed_found = 0;
+		return ftdi->response;
+	} else {
+		target->abandoning = 0;
+		mutex_unlock(&ftdi->u132_lock);
+		ftdi_elan_do_callback(ftdi, target, 4 + ftdi->response,
+				      payload);
+		ftdi->received = 0;
+		ftdi->expected = 4;
+		ftdi->ed_found = 0;
+		return ftdi->response;
+	}
 }
 
 static char *have_ed_get_response(struct usb_ftdi *ftdi,
-        struct u132_target *target, u16 ed_length, int ed_number, int ed_type,
-        char *b)
+				  struct u132_target *target, u16 ed_length, int ed_number, int ed_type,
+				  char *b)
 {
-        mutex_lock(&ftdi->u132_lock);
-        target->condition_code = TD_DEVNOTRESP;
-        target->actual = (ed_length >> 0) & 0x01FF;
-        target->non_null = (ed_length >> 15) & 0x0001;
-        target->repeat_number = (ed_length >> 11) & 0x000F;
-        mutex_unlock(&ftdi->u132_lock);
-        if (target->active)
-                ftdi_elan_do_callback(ftdi, target, NULL, 0);
-        target->abandoning = 0;
-        ftdi->received = 0;
-        ftdi->expected = 4;
-        ftdi->ed_found = 0;
-        return ftdi->response;
+	mutex_lock(&ftdi->u132_lock);
+	target->condition_code = TD_DEVNOTRESP;
+	target->actual = (ed_length >> 0) & 0x01FF;
+	target->non_null = (ed_length >> 15) & 0x0001;
+	target->repeat_number = (ed_length >> 11) & 0x000F;
+	mutex_unlock(&ftdi->u132_lock);
+	if (target->active)
+		ftdi_elan_do_callback(ftdi, target, NULL, 0);
+	target->abandoning = 0;
+	ftdi->received = 0;
+	ftdi->expected = 4;
+	ftdi->ed_found = 0;
+	return ftdi->response;
 }
 
 
 /*
-* The engine tries to empty the FTDI fifo
-*
-* all responses found in the fifo data are dispatched thus
-* the response buffer can only ever hold a maximum sized
-* response from the Uxxx.
-*
-*/
+ * The engine tries to empty the FTDI fifo
+ *
+ * all responses found in the fifo data are dispatched thus
+ * the response buffer can only ever hold a maximum sized
+ * response from the Uxxx.
+ *
+ */
 static int ftdi_elan_respond_engine(struct usb_ftdi *ftdi)
 {
-        u8 *b = ftdi->response + ftdi->received;
-        int bytes_read = 0;
-        int retry_on_empty = 1;
-        int retry_on_timeout = 3;
-        int empty_packets = 0;
-      read:{
-                int packet_bytes = 0;
-                int retval = usb_bulk_msg(ftdi->udev,
-                        usb_rcvbulkpipe(ftdi->udev, ftdi->bulk_in_endpointAddr),
-                         ftdi->bulk_in_buffer, ftdi->bulk_in_size,
-                        &packet_bytes, 500);
-                char diag[30 *3 + 4];
-                char *d = diag;
-                int m = packet_bytes;
-                u8 *c = ftdi->bulk_in_buffer;
-                int s = (sizeof(diag) - 1) / 3;
-                diag[0] = 0;
-                while (s-- > 0 && m-- > 0) {
-                        if (s > 0 || m == 0) {
-                                d += sprintf(d, " %02X", *c++);
-                        } else
-                                d += sprintf(d, " ..");
-                }
-                if (packet_bytes > 2) {
-                        ftdi->bulk_in_left = packet_bytes - 2;
-                        ftdi->bulk_in_last = 1;
-                        goto have;
-                } else if (retval == -ETIMEDOUT) {
-                        if (retry_on_timeout-- > 0) {
-                                dev_err(&ftdi->udev->dev, "TIMED OUT with packe"
-                                        "t_bytes = %d with total %d bytes%s\n",
-                                        packet_bytes, bytes_read, diag);
-                                goto more;
-                        } else if (bytes_read > 0) {
-                                dev_err(&ftdi->udev->dev, "ONLY %d bytes%s\n",
-                                        bytes_read, diag);
-                                return -ENOMEM;
-                        } else {
-                                dev_err(&ftdi->udev->dev, "TIMED OUT with packe"
-                                        "t_bytes = %d with total %d bytes%s\n",
-                                        packet_bytes, bytes_read, diag);
-                                return -ENOMEM;
-                        }
-                } else if (retval == -EILSEQ) {
-                        dev_err(&ftdi->udev->dev, "error = %d with packet_bytes"
-                                " = %d with total %d bytes%s\n", retval,
-                                packet_bytes, bytes_read, diag);
-                        return retval;
-                } else if (retval) {
-                        dev_err(&ftdi->udev->dev, "error = %d with packet_bytes"
-                                " = %d with total %d bytes%s\n", retval,
-                                packet_bytes, bytes_read, diag);
-                        return retval;
-                } else if (packet_bytes == 2) {
-                        unsigned char s0 = ftdi->bulk_in_buffer[0];
-                        unsigned char s1 = ftdi->bulk_in_buffer[1];
-                        empty_packets += 1;
-                        if (s0 == 0x31 && s1 == 0x60) {
-                                if (retry_on_empty-- > 0) {
-                                        goto more;
-                                } else
-                                        return 0;
-                        } else if (s0 == 0x31 && s1 == 0x00) {
-                                if (retry_on_empty-- > 0) {
-                                        goto more;
-                                } else
-                                        return 0;
-                        } else {
-                                if (retry_on_empty-- > 0) {
-                                        goto more;
-                                } else
-                                        return 0;
-                        }
-                } else if (packet_bytes == 1) {
-                        if (retry_on_empty-- > 0) {
-                                goto more;
-                        } else
-                                return 0;
-                } else {
-                        if (retry_on_empty-- > 0) {
-                                goto more;
-                        } else
-                                return 0;
-                }
-        }
-      more:{
-                goto read;
-        }
-      have:if (ftdi->bulk_in_left > 0) {
-                u8 c = ftdi->bulk_in_buffer[++ftdi->bulk_in_last];
-                bytes_read += 1;
-                ftdi->bulk_in_left -= 1;
-                if (ftdi->received == 0 && c == 0xFF) {
-                        goto have;
-                } else
-                        *b++ = c;
-                if (++ftdi->received < ftdi->expected) {
-                        goto have;
-                } else if (ftdi->ed_found) {
-                        int ed_number = (ftdi->response[0] >> 5) & 0x03;
-                        u16 ed_length = (ftdi->response[2] << 8) |
-                                ftdi->response[1];
-                        struct u132_target *target = &ftdi->target[ed_number];
-                        int payload = (ed_length >> 0) & 0x07FF;
-                        char diag[30 *3 + 4];
-                        char *d = diag;
-                        int m = payload;
-                        u8 *c = 4 + ftdi->response;
-                        int s = (sizeof(diag) - 1) / 3;
-                        diag[0] = 0;
-                        while (s-- > 0 && m-- > 0) {
-                                if (s > 0 || m == 0) {
-                                        d += sprintf(d, " %02X", *c++);
-                                } else
-                                        d += sprintf(d, " ..");
-                        }
-                        ftdi_elan_do_callback(ftdi, target, 4 + ftdi->response,
-                                payload);
-                        ftdi->received = 0;
-                        ftdi->expected = 4;
-                        ftdi->ed_found = 0;
-                        b = ftdi->response;
-                        goto have;
-                } else if (ftdi->expected == 8) {
-                        u8 buscmd;
-                        int respond_head = ftdi->respond_head++;
-                        struct u132_respond *respond = &ftdi->respond[
-                                RESPOND_MASK & respond_head];
-                        u32 data = ftdi->response[7];
-                        data <<= 8;
-                        data |= ftdi->response[6];
-                        data <<= 8;
-                        data |= ftdi->response[5];
-                        data <<= 8;
-                        data |= ftdi->response[4];
-                        *respond->value = data;
-                        *respond->result = 0;
-                        complete(&respond->wait_completion);
-                        ftdi->received = 0;
-                        ftdi->expected = 4;
-                        ftdi->ed_found = 0;
-                        b = ftdi->response;
-                        buscmd = (ftdi->response[0] >> 0) & 0x0F;
-                        if (buscmd == 0x00) {
-                        } else if (buscmd == 0x02) {
-                        } else if (buscmd == 0x06) {
-                        } else if (buscmd == 0x0A) {
-                        } else
-                                dev_err(&ftdi->udev->dev, "Uxxx unknown(%0X) va"
-                                        "lue = %08X\n", buscmd, data);
-                        goto have;
-                } else {
-                        if ((ftdi->response[0] & 0x80) == 0x00) {
-                                ftdi->expected = 8;
-                                goto have;
-                        } else {
-                                int ed_number = (ftdi->response[0] >> 5) & 0x03;
-                                int ed_type = (ftdi->response[0] >> 0) & 0x03;
-                                u16 ed_length = (ftdi->response[2] << 8) |
-                                        ftdi->response[1];
-                                struct u132_target *target = &ftdi->target[
-                                        ed_number];
-                                target->halted = (ftdi->response[0] >> 3) &
-                                        0x01;
-                                target->skipped = (ftdi->response[0] >> 2) &
-                                        0x01;
-                                target->toggle_bits = (ftdi->response[3] >> 6)
-                                        & 0x03;
-                                target->error_count = (ftdi->response[3] >> 4)
-                                        & 0x03;
-                                target->condition_code = (ftdi->response[
-                                        3] >> 0) & 0x0F;
-                                if ((ftdi->response[0] & 0x10) == 0x00) {
-                                        b = have_ed_set_response(ftdi, target,
-                                                ed_length, ed_number, ed_type,
-                                                b);
-                                        goto have;
-                                } else {
-                                        b = have_ed_get_response(ftdi, target,
-                                                ed_length, ed_number, ed_type,
-                                                b);
-                                        goto have;
-                                }
-                        }
-                }
-        } else
-                goto more;
+	u8 *b = ftdi->response + ftdi->received;
+	int bytes_read = 0;
+	int retry_on_empty = 1;
+	int retry_on_timeout = 3;
+	int empty_packets = 0;
+read:{
+		int packet_bytes = 0;
+		int retval = usb_bulk_msg(ftdi->udev,
+					  usb_rcvbulkpipe(ftdi->udev, ftdi->bulk_in_endpointAddr),
+					  ftdi->bulk_in_buffer, ftdi->bulk_in_size,
+					  &packet_bytes, 500);
+		char diag[30 *3 + 4];
+		char *d = diag;
+		int m = packet_bytes;
+		u8 *c = ftdi->bulk_in_buffer;
+		int s = (sizeof(diag) - 1) / 3;
+		diag[0] = 0;
+		while (s-- > 0 && m-- > 0) {
+			if (s > 0 || m == 0) {
+				d += sprintf(d, " %02X", *c++);
+			} else
+				d += sprintf(d, " ..");
+		}
+		if (packet_bytes > 2) {
+			ftdi->bulk_in_left = packet_bytes - 2;
+			ftdi->bulk_in_last = 1;
+			goto have;
+		} else if (retval == -ETIMEDOUT) {
+			if (retry_on_timeout-- > 0) {
+				dev_err(&ftdi->udev->dev, "TIMED OUT with packet_bytes = %d with total %d bytes%s\n",
+					packet_bytes, bytes_read, diag);
+				goto more;
+			} else if (bytes_read > 0) {
+				dev_err(&ftdi->udev->dev, "ONLY %d bytes%s\n",
+					bytes_read, diag);
+				return -ENOMEM;
+			} else {
+				dev_err(&ftdi->udev->dev, "TIMED OUT with packet_bytes = %d with total %d bytes%s\n",
+					packet_bytes, bytes_read, diag);
+				return -ENOMEM;
+			}
+		} else if (retval == -EILSEQ) {
+			dev_err(&ftdi->udev->dev, "error = %d with packet_bytes = %d with total %d bytes%s\n",
+				retval, packet_bytes, bytes_read, diag);
+			return retval;
+		} else if (retval) {
+			dev_err(&ftdi->udev->dev, "error = %d with packet_bytes = %d with total %d bytes%s\n",
+				retval, packet_bytes, bytes_read, diag);
+			return retval;
+		} else if (packet_bytes == 2) {
+			unsigned char s0 = ftdi->bulk_in_buffer[0];
+			unsigned char s1 = ftdi->bulk_in_buffer[1];
+			empty_packets += 1;
+			if (s0 == 0x31 && s1 == 0x60) {
+				if (retry_on_empty-- > 0) {
+					goto more;
+				} else
+					return 0;
+			} else if (s0 == 0x31 && s1 == 0x00) {
+				if (retry_on_empty-- > 0) {
+					goto more;
+				} else
+					return 0;
+			} else {
+				if (retry_on_empty-- > 0) {
+					goto more;
+				} else
+					return 0;
+			}
+		} else if (packet_bytes == 1) {
+			if (retry_on_empty-- > 0) {
+				goto more;
+			} else
+				return 0;
+		} else {
+			if (retry_on_empty-- > 0) {
+				goto more;
+			} else
+				return 0;
+		}
+	}
+more:{
+		goto read;
+	}
+have:if (ftdi->bulk_in_left > 0) {
+		u8 c = ftdi->bulk_in_buffer[++ftdi->bulk_in_last];
+		bytes_read += 1;
+		ftdi->bulk_in_left -= 1;
+		if (ftdi->received == 0 && c == 0xFF) {
+			goto have;
+		} else
+			*b++ = c;
+		if (++ftdi->received < ftdi->expected) {
+			goto have;
+		} else if (ftdi->ed_found) {
+			int ed_number = (ftdi->response[0] >> 5) & 0x03;
+			u16 ed_length = (ftdi->response[2] << 8) |
+				ftdi->response[1];
+			struct u132_target *target = &ftdi->target[ed_number];
+			int payload = (ed_length >> 0) & 0x07FF;
+			char diag[30 *3 + 4];
+			char *d = diag;
+			int m = payload;
+			u8 *c = 4 + ftdi->response;
+			int s = (sizeof(diag) - 1) / 3;
+			diag[0] = 0;
+			while (s-- > 0 && m-- > 0) {
+				if (s > 0 || m == 0) {
+					d += sprintf(d, " %02X", *c++);
+				} else
+					d += sprintf(d, " ..");
+			}
+			ftdi_elan_do_callback(ftdi, target, 4 + ftdi->response,
+					      payload);
+			ftdi->received = 0;
+			ftdi->expected = 4;
+			ftdi->ed_found = 0;
+			b = ftdi->response;
+			goto have;
+		} else if (ftdi->expected == 8) {
+			u8 buscmd;
+			int respond_head = ftdi->respond_head++;
+			struct u132_respond *respond = &ftdi->respond[
+				RESPOND_MASK & respond_head];
+			u32 data = ftdi->response[7];
+			data <<= 8;
+			data |= ftdi->response[6];
+			data <<= 8;
+			data |= ftdi->response[5];
+			data <<= 8;
+			data |= ftdi->response[4];
+			*respond->value = data;
+			*respond->result = 0;
+			complete(&respond->wait_completion);
+			ftdi->received = 0;
+			ftdi->expected = 4;
+			ftdi->ed_found = 0;
+			b = ftdi->response;
+			buscmd = (ftdi->response[0] >> 0) & 0x0F;
+			if (buscmd == 0x00) {
+			} else if (buscmd == 0x02) {
+			} else if (buscmd == 0x06) {
+			} else if (buscmd == 0x0A) {
+			} else
+				dev_err(&ftdi->udev->dev, "Uxxx unknown(%0X) value = %08X\n",
+					buscmd, data);
+			goto have;
+		} else {
+			if ((ftdi->response[0] & 0x80) == 0x00) {
+				ftdi->expected = 8;
+				goto have;
+			} else {
+				int ed_number = (ftdi->response[0] >> 5) & 0x03;
+				int ed_type = (ftdi->response[0] >> 0) & 0x03;
+				u16 ed_length = (ftdi->response[2] << 8) |
+					ftdi->response[1];
+				struct u132_target *target = &ftdi->target[
+					ed_number];
+				target->halted = (ftdi->response[0] >> 3) &
+					0x01;
+				target->skipped = (ftdi->response[0] >> 2) &
+					0x01;
+				target->toggle_bits = (ftdi->response[3] >> 6)
+					& 0x03;
+				target->error_count = (ftdi->response[3] >> 4)
+					& 0x03;
+				target->condition_code = (ftdi->response[
+								  3] >> 0) & 0x0F;
+				if ((ftdi->response[0] & 0x10) == 0x00) {
+					b = have_ed_set_response(ftdi, target,
+								 ed_length, ed_number, ed_type,
+								 b);
+					goto have;
+				} else {
+					b = have_ed_get_response(ftdi, target,
+								 ed_length, ed_number, ed_type,
+								 b);
+					goto have;
+				}
+			}
+		}
+	} else
+		goto more;
 }
 
 
 /*
-* create a urb, and a buffer for it, and copy the data to the urb
-*
-*/
+ * create a urb, and a buffer for it, and copy the data to the urb
+ *
+ */
 static ssize_t ftdi_elan_write(struct file *file,
 			       const char __user *user_buffer, size_t count,
 			       loff_t *ppos)
 {
-        int retval = 0;
-        struct urb *urb;
-        char *buf;
-        struct usb_ftdi *ftdi = file->private_data;
-
-        if (ftdi->disconnected > 0) {
-                return -ENODEV;
-        }
-        if (count == 0) {
-                goto exit;
-        }
-        urb = usb_alloc_urb(0, GFP_KERNEL);
-        if (!urb) {
-                retval = -ENOMEM;
-                goto error_1;
-        }
-        buf = usb_alloc_coherent(ftdi->udev, count, GFP_KERNEL,
-                &urb->transfer_dma);
-        if (!buf) {
-                retval = -ENOMEM;
-                goto error_2;
-        }
-        if (copy_from_user(buf, user_buffer, count)) {
-                retval = -EFAULT;
-                goto error_3;
-        }
-        usb_fill_bulk_urb(urb, ftdi->udev, usb_sndbulkpipe(ftdi->udev,
-                ftdi->bulk_out_endpointAddr), buf, count,
-                ftdi_elan_write_bulk_callback, ftdi);
-        urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
-        retval = usb_submit_urb(urb, GFP_KERNEL);
-        if (retval) {
-                dev_err(&ftdi->udev->dev, "failed submitting write urb, error %"
-                        "d\n", retval);
-                goto error_3;
-        }
-        usb_free_urb(urb);
+	int retval = 0;
+	struct urb *urb;
+	char *buf;
+	struct usb_ftdi *ftdi = file->private_data;
+
+	if (ftdi->disconnected > 0) {
+		return -ENODEV;
+	}
+	if (count == 0) {
+		goto exit;
+	}
+	urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!urb) {
+		retval = -ENOMEM;
+		goto error_1;
+	}
+	buf = usb_alloc_coherent(ftdi->udev, count, GFP_KERNEL,
+				 &urb->transfer_dma);
+	if (!buf) {
+		retval = -ENOMEM;
+		goto error_2;
+	}
+	if (copy_from_user(buf, user_buffer, count)) {
+		retval = -EFAULT;
+		goto error_3;
+	}
+	usb_fill_bulk_urb(urb, ftdi->udev, usb_sndbulkpipe(ftdi->udev,
+							   ftdi->bulk_out_endpointAddr), buf, count,
+			  ftdi_elan_write_bulk_callback, ftdi);
+	urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+	retval = usb_submit_urb(urb, GFP_KERNEL);
+	if (retval) {
+		dev_err(&ftdi->udev->dev,
+			"failed submitting write urb, error %d\n", retval);
+		goto error_3;
+	}
+	usb_free_urb(urb);
 
 exit:
-        return count;
+	return count;
 error_3:
 	usb_free_coherent(ftdi->udev, count, buf, urb->transfer_dma);
 error_2:
@@ -1198,29 +1189,29 @@ error_1:
 }
 
 static const struct file_operations ftdi_elan_fops = {
-        .owner = THIS_MODULE,
-        .llseek = no_llseek,
-        .read = ftdi_elan_read,
-        .write = ftdi_elan_write,
-        .open = ftdi_elan_open,
-        .release = ftdi_elan_release,
+	.owner = THIS_MODULE,
+	.llseek = no_llseek,
+	.read = ftdi_elan_read,
+	.write = ftdi_elan_write,
+	.open = ftdi_elan_open,
+	.release = ftdi_elan_release,
 };
 
 /*
-* usb class driver info in order to get a minor number from the usb core,
-* and to have the device registered with the driver core
-*/
+ * usb class driver info in order to get a minor number from the usb core,
+ * and to have the device registered with the driver core
+ */
 static struct usb_class_driver ftdi_elan_jtag_class = {
-        .name = "ftdi-%d-jtag",
-        .fops = &ftdi_elan_fops,
-        .minor_base = USB_FTDI_ELAN_MINOR_BASE,
+	.name = "ftdi-%d-jtag",
+	.fops = &ftdi_elan_fops,
+	.minor_base = USB_FTDI_ELAN_MINOR_BASE,
 };
 
 /*
-* the following definitions are for the
-* ELAN FPGA state machgine processor that
-* lies on the other side of the FTDI chip
-*/
+ * the following definitions are for the
+ * ELAN FPGA state machgine processor that
+ * lies on the other side of the FTDI chip
+ */
 #define cPCIu132rd 0x0
 #define cPCIu132wr 0x1
 #define cPCIiord 0x2
@@ -1251,1694 +1242,1663 @@ static struct usb_class_driver ftdi_elan_jtag_class = {
 #define cCCnotaccessed 0xF
 static int ftdi_elan_write_reg(struct usb_ftdi *ftdi, u32 data)
 {
-      wait:if (ftdi->disconnected > 0) {
-                return -ENODEV;
-        } else {
-                int command_size;
-                mutex_lock(&ftdi->u132_lock);
-                command_size = ftdi->command_next - ftdi->command_head;
-                if (command_size < COMMAND_SIZE) {
-                        struct u132_command *command = &ftdi->command[
-                                COMMAND_MASK & ftdi->command_next];
-                        command->header = 0x00 | cPCIu132wr;
-                        command->length = 0x04;
-                        command->address = 0x00;
-                        command->width = 0x00;
-                        command->follows = 4;
-                        command->value = data;
-                        command->buffer = &command->value;
-                        ftdi->command_next += 1;
-                        ftdi_elan_kick_command_queue(ftdi);
-                        mutex_unlock(&ftdi->u132_lock);
-                        return 0;
-                } else {
-                        mutex_unlock(&ftdi->u132_lock);
-                        msleep(100);
-                        goto wait;
-                }
-        }
+wait:if (ftdi->disconnected > 0) {
+		return -ENODEV;
+	} else {
+		int command_size;
+		mutex_lock(&ftdi->u132_lock);
+		command_size = ftdi->command_next - ftdi->command_head;
+		if (command_size < COMMAND_SIZE) {
+			struct u132_command *command = &ftdi->command[
+				COMMAND_MASK & ftdi->command_next];
+			command->header = 0x00 | cPCIu132wr;
+			command->length = 0x04;
+			command->address = 0x00;
+			command->width = 0x00;
+			command->follows = 4;
+			command->value = data;
+			command->buffer = &command->value;
+			ftdi->command_next += 1;
+			ftdi_elan_kick_command_queue(ftdi);
+			mutex_unlock(&ftdi->u132_lock);
+			return 0;
+		} else {
+			mutex_unlock(&ftdi->u132_lock);
+			msleep(100);
+			goto wait;
+		}
+	}
 }
 
 static int ftdi_elan_write_config(struct usb_ftdi *ftdi, int config_offset,
-        u8 width, u32 data)
-{
-        u8 addressofs = config_offset / 4;
-      wait:if (ftdi->disconnected > 0) {
-                return -ENODEV;
-        } else {
-                int command_size;
-                mutex_lock(&ftdi->u132_lock);
-                command_size = ftdi->command_next - ftdi->command_head;
-                if (command_size < COMMAND_SIZE) {
-                        struct u132_command *command = &ftdi->command[
-                                COMMAND_MASK & ftdi->command_next];
-                        command->header = 0x00 | (cPCIcfgwr & 0x0F);
-                        command->length = 0x04;
-                        command->address = addressofs;
-                        command->width = 0x00 | (width & 0x0F);
-                        command->follows = 4;
-                        command->value = data;
-                        command->buffer = &command->value;
-                        ftdi->command_next += 1;
-                        ftdi_elan_kick_command_queue(ftdi);
-                        mutex_unlock(&ftdi->u132_lock);
-                        return 0;
-                } else {
-                        mutex_unlock(&ftdi->u132_lock);
-                        msleep(100);
-                        goto wait;
-                }
-        }
+				  u8 width, u32 data)
+{
+	u8 addressofs = config_offset / 4;
+wait:if (ftdi->disconnected > 0) {
+		return -ENODEV;
+	} else {
+		int command_size;
+		mutex_lock(&ftdi->u132_lock);
+		command_size = ftdi->command_next - ftdi->command_head;
+		if (command_size < COMMAND_SIZE) {
+			struct u132_command *command = &ftdi->command[
+				COMMAND_MASK & ftdi->command_next];
+			command->header = 0x00 | (cPCIcfgwr & 0x0F);
+			command->length = 0x04;
+			command->address = addressofs;
+			command->width = 0x00 | (width & 0x0F);
+			command->follows = 4;
+			command->value = data;
+			command->buffer = &command->value;
+			ftdi->command_next += 1;
+			ftdi_elan_kick_command_queue(ftdi);
+			mutex_unlock(&ftdi->u132_lock);
+			return 0;
+		} else {
+			mutex_unlock(&ftdi->u132_lock);
+			msleep(100);
+			goto wait;
+		}
+	}
 }
 
 static int ftdi_elan_write_pcimem(struct usb_ftdi *ftdi, int mem_offset,
-        u8 width, u32 data)
-{
-        u8 addressofs = mem_offset / 4;
-      wait:if (ftdi->disconnected > 0) {
-                return -ENODEV;
-        } else {
-                int command_size;
-                mutex_lock(&ftdi->u132_lock);
-                command_size = ftdi->command_next - ftdi->command_head;
-                if (command_size < COMMAND_SIZE) {
-                        struct u132_command *command = &ftdi->command[
-                                COMMAND_MASK & ftdi->command_next];
-                        command->header = 0x00 | (cPCImemwr & 0x0F);
-                        command->length = 0x04;
-                        command->address = addressofs;
-                        command->width = 0x00 | (width & 0x0F);
-                        command->follows = 4;
-                        command->value = data;
-                        command->buffer = &command->value;
-                        ftdi->command_next += 1;
-                        ftdi_elan_kick_command_queue(ftdi);
-                        mutex_unlock(&ftdi->u132_lock);
-                        return 0;
-                } else {
-                        mutex_unlock(&ftdi->u132_lock);
-                        msleep(100);
-                        goto wait;
-                }
-        }
+				  u8 width, u32 data)
+{
+	u8 addressofs = mem_offset / 4;
+wait:if (ftdi->disconnected > 0) {
+		return -ENODEV;
+	} else {
+		int command_size;
+		mutex_lock(&ftdi->u132_lock);
+		command_size = ftdi->command_next - ftdi->command_head;
+		if (command_size < COMMAND_SIZE) {
+			struct u132_command *command = &ftdi->command[
+				COMMAND_MASK & ftdi->command_next];
+			command->header = 0x00 | (cPCImemwr & 0x0F);
+			command->length = 0x04;
+			command->address = addressofs;
+			command->width = 0x00 | (width & 0x0F);
+			command->follows = 4;
+			command->value = data;
+			command->buffer = &command->value;
+			ftdi->command_next += 1;
+			ftdi_elan_kick_command_queue(ftdi);
+			mutex_unlock(&ftdi->u132_lock);
+			return 0;
+		} else {
+			mutex_unlock(&ftdi->u132_lock);
+			msleep(100);
+			goto wait;
+		}
+	}
 }
 
 int usb_ftdi_elan_write_pcimem(struct platform_device *pdev, int mem_offset,
-        u8 width, u32 data)
+			       u8 width, u32 data)
 {
-        struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
-        return ftdi_elan_write_pcimem(ftdi, mem_offset, width, data);
+	struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
+	return ftdi_elan_write_pcimem(ftdi, mem_offset, width, data);
 }
 
 
 EXPORT_SYMBOL_GPL(usb_ftdi_elan_write_pcimem);
 static int ftdi_elan_read_reg(struct usb_ftdi *ftdi, u32 *data)
 {
-      wait:if (ftdi->disconnected > 0) {
-                return -ENODEV;
-        } else {
-                int command_size;
-                int respond_size;
-                mutex_lock(&ftdi->u132_lock);
-                command_size = ftdi->command_next - ftdi->command_head;
-                respond_size = ftdi->respond_next - ftdi->respond_head;
-                if (command_size < COMMAND_SIZE && respond_size < RESPOND_SIZE)
-                        {
-                        struct u132_command *command = &ftdi->command[
-                                COMMAND_MASK & ftdi->command_next];
-                        struct u132_respond *respond = &ftdi->respond[
-                                RESPOND_MASK & ftdi->respond_next];
-                        int result = -ENODEV;
-                        respond->result = &result;
-                        respond->header = command->header = 0x00 | cPCIu132rd;
-                        command->length = 0x04;
-                        respond->address = command->address = cU132cmd_status;
-                        command->width = 0x00;
-                        command->follows = 0;
-                        command->value = 0;
-                        command->buffer = NULL;
-                        respond->value = data;
-                        init_completion(&respond->wait_completion);
-                        ftdi->command_next += 1;
-                        ftdi->respond_next += 1;
-                        ftdi_elan_kick_command_queue(ftdi);
-                        mutex_unlock(&ftdi->u132_lock);
-                        wait_for_completion(&respond->wait_completion);
-                        return result;
-                } else {
-                        mutex_unlock(&ftdi->u132_lock);
-                        msleep(100);
-                        goto wait;
-                }
-        }
+wait:if (ftdi->disconnected > 0) {
+		return -ENODEV;
+	} else {
+		int command_size;
+		int respond_size;
+		mutex_lock(&ftdi->u132_lock);
+		command_size = ftdi->command_next - ftdi->command_head;
+		respond_size = ftdi->respond_next - ftdi->respond_head;
+		if (command_size < COMMAND_SIZE && respond_size < RESPOND_SIZE)
+		{
+			struct u132_command *command = &ftdi->command[
+				COMMAND_MASK & ftdi->command_next];
+			struct u132_respond *respond = &ftdi->respond[
+				RESPOND_MASK & ftdi->respond_next];
+			int result = -ENODEV;
+			respond->result = &result;
+			respond->header = command->header = 0x00 | cPCIu132rd;
+			command->length = 0x04;
+			respond->address = command->address = cU132cmd_status;
+			command->width = 0x00;
+			command->follows = 0;
+			command->value = 0;
+			command->buffer = NULL;
+			respond->value = data;
+			init_completion(&respond->wait_completion);
+			ftdi->command_next += 1;
+			ftdi->respond_next += 1;
+			ftdi_elan_kick_command_queue(ftdi);
+			mutex_unlock(&ftdi->u132_lock);
+			wait_for_completion(&respond->wait_completion);
+			return result;
+		} else {
+			mutex_unlock(&ftdi->u132_lock);
+			msleep(100);
+			goto wait;
+		}
+	}
 }
 
 static int ftdi_elan_read_config(struct usb_ftdi *ftdi, int config_offset,
-        u8 width, u32 *data)
-{
-        u8 addressofs = config_offset / 4;
-      wait:if (ftdi->disconnected > 0) {
-                return -ENODEV;
-        } else {
-                int command_size;
-                int respond_size;
-                mutex_lock(&ftdi->u132_lock);
-                command_size = ftdi->command_next - ftdi->command_head;
-                respond_size = ftdi->respond_next - ftdi->respond_head;
-                if (command_size < COMMAND_SIZE && respond_size < RESPOND_SIZE)
-                        {
-                        struct u132_command *command = &ftdi->command[
-                                COMMAND_MASK & ftdi->command_next];
-                        struct u132_respond *respond = &ftdi->respond[
-                                RESPOND_MASK & ftdi->respond_next];
-                        int result = -ENODEV;
-                        respond->result = &result;
-                        respond->header = command->header = 0x00 | (cPCIcfgrd &
-                                0x0F);
-                        command->length = 0x04;
-                        respond->address = command->address = addressofs;
-                        command->width = 0x00 | (width & 0x0F);
-                        command->follows = 0;
-                        command->value = 0;
-                        command->buffer = NULL;
-                        respond->value = data;
-                        init_completion(&respond->wait_completion);
-                        ftdi->command_next += 1;
-                        ftdi->respond_next += 1;
-                        ftdi_elan_kick_command_queue(ftdi);
-                        mutex_unlock(&ftdi->u132_lock);
-                        wait_for_completion(&respond->wait_completion);
-                        return result;
-                } else {
-                        mutex_unlock(&ftdi->u132_lock);
-                        msleep(100);
-                        goto wait;
-                }
-        }
+				 u8 width, u32 *data)
+{
+	u8 addressofs = config_offset / 4;
+wait:if (ftdi->disconnected > 0) {
+		return -ENODEV;
+	} else {
+		int command_size;
+		int respond_size;
+		mutex_lock(&ftdi->u132_lock);
+		command_size = ftdi->command_next - ftdi->command_head;
+		respond_size = ftdi->respond_next - ftdi->respond_head;
+		if (command_size < COMMAND_SIZE && respond_size < RESPOND_SIZE)
+		{
+			struct u132_command *command = &ftdi->command[
+				COMMAND_MASK & ftdi->command_next];
+			struct u132_respond *respond = &ftdi->respond[
+				RESPOND_MASK & ftdi->respond_next];
+			int result = -ENODEV;
+			respond->result = &result;
+			respond->header = command->header = 0x00 | (cPCIcfgrd &
+								    0x0F);
+			command->length = 0x04;
+			respond->address = command->address = addressofs;
+			command->width = 0x00 | (width & 0x0F);
+			command->follows = 0;
+			command->value = 0;
+			command->buffer = NULL;
+			respond->value = data;
+			init_completion(&respond->wait_completion);
+			ftdi->command_next += 1;
+			ftdi->respond_next += 1;
+			ftdi_elan_kick_command_queue(ftdi);
+			mutex_unlock(&ftdi->u132_lock);
+			wait_for_completion(&respond->wait_completion);
+			return result;
+		} else {
+			mutex_unlock(&ftdi->u132_lock);
+			msleep(100);
+			goto wait;
+		}
+	}
 }
 
 static int ftdi_elan_read_pcimem(struct usb_ftdi *ftdi, int mem_offset,
-        u8 width, u32 *data)
-{
-        u8 addressofs = mem_offset / 4;
-      wait:if (ftdi->disconnected > 0) {
-                return -ENODEV;
-        } else {
-                int command_size;
-                int respond_size;
-                mutex_lock(&ftdi->u132_lock);
-                command_size = ftdi->command_next - ftdi->command_head;
-                respond_size = ftdi->respond_next - ftdi->respond_head;
-                if (command_size < COMMAND_SIZE && respond_size < RESPOND_SIZE)
-                        {
-                        struct u132_command *command = &ftdi->command[
-                                COMMAND_MASK & ftdi->command_next];
-                        struct u132_respond *respond = &ftdi->respond[
-                                RESPOND_MASK & ftdi->respond_next];
-                        int result = -ENODEV;
-                        respond->result = &result;
-                        respond->header = command->header = 0x00 | (cPCImemrd &
-                                0x0F);
-                        command->length = 0x04;
-                        respond->address = command->address = addressofs;
-                        command->width = 0x00 | (width & 0x0F);
-                        command->follows = 0;
-                        command->value = 0;
-                        command->buffer = NULL;
-                        respond->value = data;
-                        init_completion(&respond->wait_completion);
-                        ftdi->command_next += 1;
-                        ftdi->respond_next += 1;
-                        ftdi_elan_kick_command_queue(ftdi);
-                        mutex_unlock(&ftdi->u132_lock);
-                        wait_for_completion(&respond->wait_completion);
-                        return result;
-                } else {
-                        mutex_unlock(&ftdi->u132_lock);
-                        msleep(100);
-                        goto wait;
-                }
-        }
+				 u8 width, u32 *data)
+{
+	u8 addressofs = mem_offset / 4;
+wait:if (ftdi->disconnected > 0) {
+		return -ENODEV;
+	} else {
+		int command_size;
+		int respond_size;
+		mutex_lock(&ftdi->u132_lock);
+		command_size = ftdi->command_next - ftdi->command_head;
+		respond_size = ftdi->respond_next - ftdi->respond_head;
+		if (command_size < COMMAND_SIZE && respond_size < RESPOND_SIZE)
+		{
+			struct u132_command *command = &ftdi->command[
+				COMMAND_MASK & ftdi->command_next];
+			struct u132_respond *respond = &ftdi->respond[
+				RESPOND_MASK & ftdi->respond_next];
+			int result = -ENODEV;
+			respond->result = &result;
+			respond->header = command->header = 0x00 | (cPCImemrd &
+								    0x0F);
+			command->length = 0x04;
+			respond->address = command->address = addressofs;
+			command->width = 0x00 | (width & 0x0F);
+			command->follows = 0;
+			command->value = 0;
+			command->buffer = NULL;
+			respond->value = data;
+			init_completion(&respond->wait_completion);
+			ftdi->command_next += 1;
+			ftdi->respond_next += 1;
+			ftdi_elan_kick_command_queue(ftdi);
+			mutex_unlock(&ftdi->u132_lock);
+			wait_for_completion(&respond->wait_completion);
+			return result;
+		} else {
+			mutex_unlock(&ftdi->u132_lock);
+			msleep(100);
+			goto wait;
+		}
+	}
 }
 
 int usb_ftdi_elan_read_pcimem(struct platform_device *pdev, int mem_offset,
-        u8 width, u32 *data)
+			      u8 width, u32 *data)
 {
-        struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
-        if (ftdi->initialized == 0) {
-                return -ENODEV;
-        } else
-                return ftdi_elan_read_pcimem(ftdi, mem_offset, width, data);
+	struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
+	if (ftdi->initialized == 0) {
+		return -ENODEV;
+	} else
+		return ftdi_elan_read_pcimem(ftdi, mem_offset, width, data);
 }
 
 
 EXPORT_SYMBOL_GPL(usb_ftdi_elan_read_pcimem);
 static int ftdi_elan_edset_setup(struct usb_ftdi *ftdi, u8 ed_number,
-        void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
-        void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
-        int toggle_bits, int error_count, int condition_code, int repeat_number,
-         int halted, int skipped, int actual, int non_null))
-{
-        u8 ed = ed_number - 1;
-      wait:if (ftdi->disconnected > 0) {
-                return -ENODEV;
-        } else if (ftdi->initialized == 0) {
-                return -ENODEV;
-        } else {
-                int command_size;
-                mutex_lock(&ftdi->u132_lock);
-                command_size = ftdi->command_next - ftdi->command_head;
-                if (command_size < COMMAND_SIZE) {
-                        struct u132_target *target = &ftdi->target[ed];
-                        struct u132_command *command = &ftdi->command[
-                                COMMAND_MASK & ftdi->command_next];
-                        command->header = 0x80 | (ed << 5);
-                        command->length = 0x8007;
-                        command->address = (toggle_bits << 6) | (ep_number << 2)
-                                | (address << 0);
-                        command->width = usb_maxpacket(urb->dev, urb->pipe,
-                                usb_pipeout(urb->pipe));
-                        command->follows = 8;
-                        command->value = 0;
-                        command->buffer = urb->setup_packet;
-                        target->callback = callback;
-                        target->endp = endp;
-                        target->urb = urb;
-                        target->active = 1;
-                        ftdi->command_next += 1;
-                        ftdi_elan_kick_command_queue(ftdi);
-                        mutex_unlock(&ftdi->u132_lock);
-                        return 0;
-                } else {
-                        mutex_unlock(&ftdi->u132_lock);
-                        msleep(100);
-                        goto wait;
-                }
-        }
+				 void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
+				 void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
+						   int toggle_bits, int error_count, int condition_code, int repeat_number,
+						   int halted, int skipped, int actual, int non_null))
+{
+	u8 ed = ed_number - 1;
+wait:if (ftdi->disconnected > 0) {
+		return -ENODEV;
+	} else if (ftdi->initialized == 0) {
+		return -ENODEV;
+	} else {
+		int command_size;
+		mutex_lock(&ftdi->u132_lock);
+		command_size = ftdi->command_next - ftdi->command_head;
+		if (command_size < COMMAND_SIZE) {
+			struct u132_target *target = &ftdi->target[ed];
+			struct u132_command *command = &ftdi->command[
+				COMMAND_MASK & ftdi->command_next];
+			command->header = 0x80 | (ed << 5);
+			command->length = 0x8007;
+			command->address = (toggle_bits << 6) | (ep_number << 2)
+				| (address << 0);
+			command->width = usb_maxpacket(urb->dev, urb->pipe,
+						       usb_pipeout(urb->pipe));
+			command->follows = 8;
+			command->value = 0;
+			command->buffer = urb->setup_packet;
+			target->callback = callback;
+			target->endp = endp;
+			target->urb = urb;
+			target->active = 1;
+			ftdi->command_next += 1;
+			ftdi_elan_kick_command_queue(ftdi);
+			mutex_unlock(&ftdi->u132_lock);
+			return 0;
+		} else {
+			mutex_unlock(&ftdi->u132_lock);
+			msleep(100);
+			goto wait;
+		}
+	}
 }
 
 int usb_ftdi_elan_edset_setup(struct platform_device *pdev, u8 ed_number,
-        void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
-        void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
-        int toggle_bits, int error_count, int condition_code, int repeat_number,
-         int halted, int skipped, int actual, int non_null))
+			      void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
+			      void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
+						int toggle_bits, int error_count, int condition_code, int repeat_number,
+						int halted, int skipped, int actual, int non_null))
 {
-        struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
-        return ftdi_elan_edset_setup(ftdi, ed_number, endp, urb, address,
-                ep_number, toggle_bits, callback);
+	struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
+	return ftdi_elan_edset_setup(ftdi, ed_number, endp, urb, address,
+				     ep_number, toggle_bits, callback);
 }
 
 
 EXPORT_SYMBOL_GPL(usb_ftdi_elan_edset_setup);
 static int ftdi_elan_edset_input(struct usb_ftdi *ftdi, u8 ed_number,
-        void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
-        void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
-        int toggle_bits, int error_count, int condition_code, int repeat_number,
-         int halted, int skipped, int actual, int non_null))
-{
-        u8 ed = ed_number - 1;
-      wait:if (ftdi->disconnected > 0) {
-                return -ENODEV;
-        } else if (ftdi->initialized == 0) {
-                return -ENODEV;
-        } else {
-                int command_size;
-                mutex_lock(&ftdi->u132_lock);
-                command_size = ftdi->command_next - ftdi->command_head;
-                if (command_size < COMMAND_SIZE) {
-                        struct u132_target *target = &ftdi->target[ed];
-                        struct u132_command *command = &ftdi->command[
-                                COMMAND_MASK & ftdi->command_next];
-                        u32 remaining_length = urb->transfer_buffer_length -
-                                urb->actual_length;
-                        command->header = 0x82 | (ed << 5);
-                        if (remaining_length == 0) {
-                                command->length = 0x0000;
-                        } else if (remaining_length > 1024) {
-                                command->length = 0x8000 | 1023;
-                        } else
-                                command->length = 0x8000 | (remaining_length -
-                                        1);
-                        command->address = (toggle_bits << 6) | (ep_number << 2)
-                                | (address << 0);
-                        command->width = usb_maxpacket(urb->dev, urb->pipe,
-                                usb_pipeout(urb->pipe));
-                        command->follows = 0;
-                        command->value = 0;
-                        command->buffer = NULL;
-                        target->callback = callback;
-                        target->endp = endp;
-                        target->urb = urb;
-                        target->active = 1;
-                        ftdi->command_next += 1;
-                        ftdi_elan_kick_command_queue(ftdi);
-                        mutex_unlock(&ftdi->u132_lock);
-                        return 0;
-                } else {
-                        mutex_unlock(&ftdi->u132_lock);
-                        msleep(100);
-                        goto wait;
-                }
-        }
+				 void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
+				 void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
+						   int toggle_bits, int error_count, int condition_code, int repeat_number,
+						   int halted, int skipped, int actual, int non_null))
+{
+	u8 ed = ed_number - 1;
+wait:if (ftdi->disconnected > 0) {
+		return -ENODEV;
+	} else if (ftdi->initialized == 0) {
+		return -ENODEV;
+	} else {
+		int command_size;
+		mutex_lock(&ftdi->u132_lock);
+		command_size = ftdi->command_next - ftdi->command_head;
+		if (command_size < COMMAND_SIZE) {
+			struct u132_target *target = &ftdi->target[ed];
+			struct u132_command *command = &ftdi->command[
+				COMMAND_MASK & ftdi->command_next];
+			u32 remaining_length = urb->transfer_buffer_length -
+				urb->actual_length;
+			command->header = 0x82 | (ed << 5);
+			if (remaining_length == 0) {
+				command->length = 0x0000;
+			} else if (remaining_length > 1024) {
+				command->length = 0x8000 | 1023;
+			} else
+				command->length = 0x8000 | (remaining_length -
+							    1);
+			command->address = (toggle_bits << 6) | (ep_number << 2)
+				| (address << 0);
+			command->width = usb_maxpacket(urb->dev, urb->pipe,
+						       usb_pipeout(urb->pipe));
+			command->follows = 0;
+			command->value = 0;
+			command->buffer = NULL;
+			target->callback = callback;
+			target->endp = endp;
+			target->urb = urb;
+			target->active = 1;
+			ftdi->command_next += 1;
+			ftdi_elan_kick_command_queue(ftdi);
+			mutex_unlock(&ftdi->u132_lock);
+			return 0;
+		} else {
+			mutex_unlock(&ftdi->u132_lock);
+			msleep(100);
+			goto wait;
+		}
+	}
 }
 
 int usb_ftdi_elan_edset_input(struct platform_device *pdev, u8 ed_number,
-        void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
-        void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
-        int toggle_bits, int error_count, int condition_code, int repeat_number,
-         int halted, int skipped, int actual, int non_null))
+			      void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
+			      void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
+						int toggle_bits, int error_count, int condition_code, int repeat_number,
+						int halted, int skipped, int actual, int non_null))
 {
-        struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
-        return ftdi_elan_edset_input(ftdi, ed_number, endp, urb, address,
-                ep_number, toggle_bits, callback);
+	struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
+	return ftdi_elan_edset_input(ftdi, ed_number, endp, urb, address,
+				     ep_number, toggle_bits, callback);
 }
 
 
 EXPORT_SYMBOL_GPL(usb_ftdi_elan_edset_input);
 static int ftdi_elan_edset_empty(struct usb_ftdi *ftdi, u8 ed_number,
-        void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
-        void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
-        int toggle_bits, int error_count, int condition_code, int repeat_number,
-         int halted, int skipped, int actual, int non_null))
-{
-        u8 ed = ed_number - 1;
-      wait:if (ftdi->disconnected > 0) {
-                return -ENODEV;
-        } else if (ftdi->initialized == 0) {
-                return -ENODEV;
-        } else {
-                int command_size;
-                mutex_lock(&ftdi->u132_lock);
-                command_size = ftdi->command_next - ftdi->command_head;
-                if (command_size < COMMAND_SIZE) {
-                        struct u132_target *target = &ftdi->target[ed];
-                        struct u132_command *command = &ftdi->command[
-                                COMMAND_MASK & ftdi->command_next];
-                        command->header = 0x81 | (ed << 5);
-                        command->length = 0x0000;
-                        command->address = (toggle_bits << 6) | (ep_number << 2)
-                                | (address << 0);
-                        command->width = usb_maxpacket(urb->dev, urb->pipe,
-                                usb_pipeout(urb->pipe));
-                        command->follows = 0;
-                        command->value = 0;
-                        command->buffer = NULL;
-                        target->callback = callback;
-                        target->endp = endp;
-                        target->urb = urb;
-                        target->active = 1;
-                        ftdi->command_next += 1;
-                        ftdi_elan_kick_command_queue(ftdi);
-                        mutex_unlock(&ftdi->u132_lock);
-                        return 0;
-                } else {
-                        mutex_unlock(&ftdi->u132_lock);
-                        msleep(100);
-                        goto wait;
-                }
-        }
+				 void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
+				 void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
+						   int toggle_bits, int error_count, int condition_code, int repeat_number,
+						   int halted, int skipped, int actual, int non_null))
+{
+	u8 ed = ed_number - 1;
+wait:if (ftdi->disconnected > 0) {
+		return -ENODEV;
+	} else if (ftdi->initialized == 0) {
+		return -ENODEV;
+	} else {
+		int command_size;
+		mutex_lock(&ftdi->u132_lock);
+		command_size = ftdi->command_next - ftdi->command_head;
+		if (command_size < COMMAND_SIZE) {
+			struct u132_target *target = &ftdi->target[ed];
+			struct u132_command *command = &ftdi->command[
+				COMMAND_MASK & ftdi->command_next];
+			command->header = 0x81 | (ed << 5);
+			command->length = 0x0000;
+			command->address = (toggle_bits << 6) | (ep_number << 2)
+				| (address << 0);
+			command->width = usb_maxpacket(urb->dev, urb->pipe,
+						       usb_pipeout(urb->pipe));
+			command->follows = 0;
+			command->value = 0;
+			command->buffer = NULL;
+			target->callback = callback;
+			target->endp = endp;
+			target->urb = urb;
+			target->active = 1;
+			ftdi->command_next += 1;
+			ftdi_elan_kick_command_queue(ftdi);
+			mutex_unlock(&ftdi->u132_lock);
+			return 0;
+		} else {
+			mutex_unlock(&ftdi->u132_lock);
+			msleep(100);
+			goto wait;
+		}
+	}
 }
 
 int usb_ftdi_elan_edset_empty(struct platform_device *pdev, u8 ed_number,
-        void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
-        void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
-        int toggle_bits, int error_count, int condition_code, int repeat_number,
-         int halted, int skipped, int actual, int non_null))
+			      void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
+			      void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
+						int toggle_bits, int error_count, int condition_code, int repeat_number,
+						int halted, int skipped, int actual, int non_null))
 {
-        struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
-        return ftdi_elan_edset_empty(ftdi, ed_number, endp, urb, address,
-                ep_number, toggle_bits, callback);
+	struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
+	return ftdi_elan_edset_empty(ftdi, ed_number, endp, urb, address,
+				     ep_number, toggle_bits, callback);
 }
 
 
 EXPORT_SYMBOL_GPL(usb_ftdi_elan_edset_empty);
 static int ftdi_elan_edset_output(struct usb_ftdi *ftdi, u8 ed_number,
-        void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
-        void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
-        int toggle_bits, int error_count, int condition_code, int repeat_number,
-         int halted, int skipped, int actual, int non_null))
-{
-        u8 ed = ed_number - 1;
-      wait:if (ftdi->disconnected > 0) {
-                return -ENODEV;
-        } else if (ftdi->initialized == 0) {
-                return -ENODEV;
-        } else {
-                int command_size;
-                mutex_lock(&ftdi->u132_lock);
-                command_size = ftdi->command_next - ftdi->command_head;
-                if (command_size < COMMAND_SIZE) {
-                        u8 *b;
-                        u16 urb_size;
-                        int i = 0;
-                        char data[30 *3 + 4];
-                        char *d = data;
-                        int m = (sizeof(data) - 1) / 3;
-                        int l = 0;
-                        struct u132_target *target = &ftdi->target[ed];
-                        struct u132_command *command = &ftdi->command[
-                                COMMAND_MASK & ftdi->command_next];
-                        command->header = 0x81 | (ed << 5);
-                        command->address = (toggle_bits << 6) | (ep_number << 2)
-                                | (address << 0);
-                        command->width = usb_maxpacket(urb->dev, urb->pipe,
-                                usb_pipeout(urb->pipe));
-                        command->follows = min_t(u32, 1024,
-                                urb->transfer_buffer_length -
-                                urb->actual_length);
-                        command->value = 0;
-                        command->buffer = urb->transfer_buffer +
-                                urb->actual_length;
-                        command->length = 0x8000 | (command->follows - 1);
-                        b = command->buffer;
-                        urb_size = command->follows;
-                        data[0] = 0;
-                        while (urb_size-- > 0) {
-                                if (i > m) {
-                                } else if (i++ < m) {
-                                        int w = sprintf(d, " %02X", *b++);
-                                        d += w;
-                                        l += w;
-                                } else
-                                        d += sprintf(d, " ..");
-                        }
-                        target->callback = callback;
-                        target->endp = endp;
-                        target->urb = urb;
-                        target->active = 1;
-                        ftdi->command_next += 1;
-                        ftdi_elan_kick_command_queue(ftdi);
-                        mutex_unlock(&ftdi->u132_lock);
-                        return 0;
-                } else {
-                        mutex_unlock(&ftdi->u132_lock);
-                        msleep(100);
-                        goto wait;
-                }
-        }
+				  void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
+				  void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
+						    int toggle_bits, int error_count, int condition_code, int repeat_number,
+						    int halted, int skipped, int actual, int non_null))
+{
+	u8 ed = ed_number - 1;
+wait:if (ftdi->disconnected > 0) {
+		return -ENODEV;
+	} else if (ftdi->initialized == 0) {
+		return -ENODEV;
+	} else {
+		int command_size;
+		mutex_lock(&ftdi->u132_lock);
+		command_size = ftdi->command_next - ftdi->command_head;
+		if (command_size < COMMAND_SIZE) {
+			u8 *b;
+			u16 urb_size;
+			int i = 0;
+			char data[30 *3 + 4];
+			char *d = data;
+			int m = (sizeof(data) - 1) / 3;
+			int l = 0;
+			struct u132_target *target = &ftdi->target[ed];
+			struct u132_command *command = &ftdi->command[
+				COMMAND_MASK & ftdi->command_next];
+			command->header = 0x81 | (ed << 5);
+			command->address = (toggle_bits << 6) | (ep_number << 2)
+				| (address << 0);
+			command->width = usb_maxpacket(urb->dev, urb->pipe,
+						       usb_pipeout(urb->pipe));
+			command->follows = min_t(u32, 1024,
+						 urb->transfer_buffer_length -
+						 urb->actual_length);
+			command->value = 0;
+			command->buffer = urb->transfer_buffer +
+				urb->actual_length;
+			command->length = 0x8000 | (command->follows - 1);
+			b = command->buffer;
+			urb_size = command->follows;
+			data[0] = 0;
+			while (urb_size-- > 0) {
+				if (i > m) {
+				} else if (i++ < m) {
+					int w = sprintf(d, " %02X", *b++);
+					d += w;
+					l += w;
+				} else
+					d += sprintf(d, " ..");
+			}
+			target->callback = callback;
+			target->endp = endp;
+			target->urb = urb;
+			target->active = 1;
+			ftdi->command_next += 1;
+			ftdi_elan_kick_command_queue(ftdi);
+			mutex_unlock(&ftdi->u132_lock);
+			return 0;
+		} else {
+			mutex_unlock(&ftdi->u132_lock);
+			msleep(100);
+			goto wait;
+		}
+	}
 }
 
 int usb_ftdi_elan_edset_output(struct platform_device *pdev, u8 ed_number,
-        void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
-        void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
-        int toggle_bits, int error_count, int condition_code, int repeat_number,
-         int halted, int skipped, int actual, int non_null))
+			       void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
+			       void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
+						 int toggle_bits, int error_count, int condition_code, int repeat_number,
+						 int halted, int skipped, int actual, int non_null))
 {
-        struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
-        return ftdi_elan_edset_output(ftdi, ed_number, endp, urb, address,
-                ep_number, toggle_bits, callback);
+	struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
+	return ftdi_elan_edset_output(ftdi, ed_number, endp, urb, address,
+				      ep_number, toggle_bits, callback);
 }
 
 
 EXPORT_SYMBOL_GPL(usb_ftdi_elan_edset_output);
 static int ftdi_elan_edset_single(struct usb_ftdi *ftdi, u8 ed_number,
-        void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
-        void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
-        int toggle_bits, int error_count, int condition_code, int repeat_number,
-         int halted, int skipped, int actual, int non_null))
-{
-        u8 ed = ed_number - 1;
-      wait:if (ftdi->disconnected > 0) {
-                return -ENODEV;
-        } else if (ftdi->initialized == 0) {
-                return -ENODEV;
-        } else {
-                int command_size;
-                mutex_lock(&ftdi->u132_lock);
-                command_size = ftdi->command_next - ftdi->command_head;
-                if (command_size < COMMAND_SIZE) {
-                        u32 remaining_length = urb->transfer_buffer_length -
-                                urb->actual_length;
-                        struct u132_target *target = &ftdi->target[ed];
-                        struct u132_command *command = &ftdi->command[
-                                COMMAND_MASK & ftdi->command_next];
-                        command->header = 0x83 | (ed << 5);
-                        if (remaining_length == 0) {
-                                command->length = 0x0000;
-                        } else if (remaining_length > 1024) {
-                                command->length = 0x8000 | 1023;
-                        } else
-                                command->length = 0x8000 | (remaining_length -
-                                        1);
-                        command->address = (toggle_bits << 6) | (ep_number << 2)
-                                | (address << 0);
-                        command->width = usb_maxpacket(urb->dev, urb->pipe,
-                                usb_pipeout(urb->pipe));
-                        command->follows = 0;
-                        command->value = 0;
-                        command->buffer = NULL;
-                        target->callback = callback;
-                        target->endp = endp;
-                        target->urb = urb;
-                        target->active = 1;
-                        ftdi->command_next += 1;
-                        ftdi_elan_kick_command_queue(ftdi);
-                        mutex_unlock(&ftdi->u132_lock);
-                        return 0;
-                } else {
-                        mutex_unlock(&ftdi->u132_lock);
-                        msleep(100);
-                        goto wait;
-                }
-        }
+				  void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
+				  void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
+						    int toggle_bits, int error_count, int condition_code, int repeat_number,
+						    int halted, int skipped, int actual, int non_null))
+{
+	u8 ed = ed_number - 1;
+wait:if (ftdi->disconnected > 0) {
+		return -ENODEV;
+	} else if (ftdi->initialized == 0) {
+		return -ENODEV;
+	} else {
+		int command_size;
+		mutex_lock(&ftdi->u132_lock);
+		command_size = ftdi->command_next - ftdi->command_head;
+		if (command_size < COMMAND_SIZE) {
+			u32 remaining_length = urb->transfer_buffer_length -
+				urb->actual_length;
+			struct u132_target *target = &ftdi->target[ed];
+			struct u132_command *command = &ftdi->command[
+				COMMAND_MASK & ftdi->command_next];
+			command->header = 0x83 | (ed << 5);
+			if (remaining_length == 0) {
+				command->length = 0x0000;
+			} else if (remaining_length > 1024) {
+				command->length = 0x8000 | 1023;
+			} else
+				command->length = 0x8000 | (remaining_length -
+							    1);
+			command->address = (toggle_bits << 6) | (ep_number << 2)
+				| (address << 0);
+			command->width = usb_maxpacket(urb->dev, urb->pipe,
+						       usb_pipeout(urb->pipe));
+			command->follows = 0;
+			command->value = 0;
+			command->buffer = NULL;
+			target->callback = callback;
+			target->endp = endp;
+			target->urb = urb;
+			target->active = 1;
+			ftdi->command_next += 1;
+			ftdi_elan_kick_command_queue(ftdi);
+			mutex_unlock(&ftdi->u132_lock);
+			return 0;
+		} else {
+			mutex_unlock(&ftdi->u132_lock);
+			msleep(100);
+			goto wait;
+		}
+	}
 }
 
 int usb_ftdi_elan_edset_single(struct platform_device *pdev, u8 ed_number,
-        void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
-        void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
-        int toggle_bits, int error_count, int condition_code, int repeat_number,
-         int halted, int skipped, int actual, int non_null))
+			       void *endp, struct urb *urb, u8 address, u8 ep_number, u8 toggle_bits,
+			       void (*callback) (void *endp, struct urb *urb, u8 *buf, int len,
+						 int toggle_bits, int error_count, int condition_code, int repeat_number,
+						 int halted, int skipped, int actual, int non_null))
 {
-        struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
-        return ftdi_elan_edset_single(ftdi, ed_number, endp, urb, address,
-                ep_number, toggle_bits, callback);
+	struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
+	return ftdi_elan_edset_single(ftdi, ed_number, endp, urb, address,
+				      ep_number, toggle_bits, callback);
 }
 
 
 EXPORT_SYMBOL_GPL(usb_ftdi_elan_edset_single);
 static int ftdi_elan_edset_flush(struct usb_ftdi *ftdi, u8 ed_number,
-        void *endp)
-{
-        u8 ed = ed_number - 1;
-        if (ftdi->disconnected > 0) {
-                return -ENODEV;
-        } else if (ftdi->initialized == 0) {
-                return -ENODEV;
-        } else {
-                struct u132_target *target = &ftdi->target[ed];
-                mutex_lock(&ftdi->u132_lock);
-                if (target->abandoning > 0) {
-                        mutex_unlock(&ftdi->u132_lock);
-                        return 0;
-                } else {
-                        target->abandoning = 1;
-                      wait_1:if (target->active == 1) {
-                                int command_size = ftdi->command_next -
-                                        ftdi->command_head;
-                                if (command_size < COMMAND_SIZE) {
-                                        struct u132_command *command =
-                                                &ftdi->command[COMMAND_MASK &
-                                                ftdi->command_next];
-                                        command->header = 0x80 | (ed << 5) |
-                                                0x4;
-                                        command->length = 0x00;
-                                        command->address = 0x00;
-                                        command->width = 0x00;
-                                        command->follows = 0;
-                                        command->value = 0;
-                                        command->buffer = &command->value;
-                                        ftdi->command_next += 1;
-                                        ftdi_elan_kick_command_queue(ftdi);
-                                } else {
-                                        mutex_unlock(&ftdi->u132_lock);
-                                        msleep(100);
-                                        mutex_lock(&ftdi->u132_lock);
-                                        goto wait_1;
-                                }
-                        }
-                        mutex_unlock(&ftdi->u132_lock);
-                        return 0;
-                }
-        }
+				 void *endp)
+{
+	u8 ed = ed_number - 1;
+	if (ftdi->disconnected > 0) {
+		return -ENODEV;
+	} else if (ftdi->initialized == 0) {
+		return -ENODEV;
+	} else {
+		struct u132_target *target = &ftdi->target[ed];
+		mutex_lock(&ftdi->u132_lock);
+		if (target->abandoning > 0) {
+			mutex_unlock(&ftdi->u132_lock);
+			return 0;
+		} else {
+			target->abandoning = 1;
+		wait_1:if (target->active == 1) {
+				int command_size = ftdi->command_next -
+					ftdi->command_head;
+				if (command_size < COMMAND_SIZE) {
+					struct u132_command *command =
+						&ftdi->command[COMMAND_MASK &
+							       ftdi->command_next];
+					command->header = 0x80 | (ed << 5) |
+						0x4;
+					command->length = 0x00;
+					command->address = 0x00;
+					command->width = 0x00;
+					command->follows = 0;
+					command->value = 0;
+					command->buffer = &command->value;
+					ftdi->command_next += 1;
+					ftdi_elan_kick_command_queue(ftdi);
+				} else {
+					mutex_unlock(&ftdi->u132_lock);
+					msleep(100);
+					mutex_lock(&ftdi->u132_lock);
+					goto wait_1;
+				}
+			}
+			mutex_unlock(&ftdi->u132_lock);
+			return 0;
+		}
+	}
 }
 
 int usb_ftdi_elan_edset_flush(struct platform_device *pdev, u8 ed_number,
-        void *endp)
+			      void *endp)
 {
-        struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
-        return ftdi_elan_edset_flush(ftdi, ed_number, endp);
+	struct usb_ftdi *ftdi = platform_device_to_usb_ftdi(pdev);
+	return ftdi_elan_edset_flush(ftdi, ed_number, endp);
 }
 
 
 EXPORT_SYMBOL_GPL(usb_ftdi_elan_edset_flush);
 static int ftdi_elan_flush_input_fifo(struct usb_ftdi *ftdi)
 {
-        int retry_on_empty = 10;
-        int retry_on_timeout = 5;
-        int retry_on_status = 20;
-      more:{
-                int packet_bytes = 0;
-                int retval = usb_bulk_msg(ftdi->udev,
-                        usb_rcvbulkpipe(ftdi->udev, ftdi->bulk_in_endpointAddr),
-                         ftdi->bulk_in_buffer, ftdi->bulk_in_size,
-                        &packet_bytes, 100);
-                if (packet_bytes > 2) {
-                        char diag[30 *3 + 4];
-                        char *d = diag;
-                        int m = (sizeof(diag) - 1) / 3;
-                        char *b = ftdi->bulk_in_buffer;
-                        int bytes_read = 0;
-                        diag[0] = 0;
-                        while (packet_bytes-- > 0) {
-                                char c = *b++;
-                                if (bytes_read < m) {
-                                        d += sprintf(d, " %02X",
-                                                0x000000FF & c);
-                                } else if (bytes_read > m) {
-                                } else
-                                        d += sprintf(d, " ..");
-                                bytes_read += 1;
-                                continue;
-                        }
-                        goto more;
-                } else if (packet_bytes > 1) {
-                        char s1 = ftdi->bulk_in_buffer[0];
-                        char s2 = ftdi->bulk_in_buffer[1];
-                        if (s1 == 0x31 && s2 == 0x60) {
-                                return 0;
-                        } else if (retry_on_status-- > 0) {
-                                goto more;
-                        } else {
-                                dev_err(&ftdi->udev->dev, "STATUS ERROR retry l"
-                                        "imit reached\n");
-                                return -EFAULT;
-                        }
-                } else if (packet_bytes > 0) {
-                        char b1 = ftdi->bulk_in_buffer[0];
-                        dev_err(&ftdi->udev->dev, "only one byte flushed from F"
-                                "TDI = %02X\n", b1);
-                        if (retry_on_status-- > 0) {
-                                goto more;
-                        } else {
-                                dev_err(&ftdi->udev->dev, "STATUS ERROR retry l"
-                                        "imit reached\n");
-                                return -EFAULT;
-                        }
-                } else if (retval == -ETIMEDOUT) {
-                        if (retry_on_timeout-- > 0) {
-                                goto more;
-                        } else {
-                                dev_err(&ftdi->udev->dev, "TIMED OUT retry limi"
-                                        "t reached\n");
-                                return -ENOMEM;
-                        }
-                } else if (retval == 0) {
-                        if (retry_on_empty-- > 0) {
-                                goto more;
-                        } else {
-                                dev_err(&ftdi->udev->dev, "empty packet retry l"
-                                        "imit reached\n");
-                                return -ENOMEM;
-                        }
-                } else {
-                        dev_err(&ftdi->udev->dev, "error = %d\n", retval);
-                        return retval;
-                }
-        }
-        return -1;
+	int retry_on_empty = 10;
+	int retry_on_timeout = 5;
+	int retry_on_status = 20;
+more:{
+		int packet_bytes = 0;
+		int retval = usb_bulk_msg(ftdi->udev,
+					  usb_rcvbulkpipe(ftdi->udev, ftdi->bulk_in_endpointAddr),
+					  ftdi->bulk_in_buffer, ftdi->bulk_in_size,
+					  &packet_bytes, 100);
+		if (packet_bytes > 2) {
+			char diag[30 *3 + 4];
+			char *d = diag;
+			int m = (sizeof(diag) - 1) / 3;
+			char *b = ftdi->bulk_in_buffer;
+			int bytes_read = 0;
+			diag[0] = 0;
+			while (packet_bytes-- > 0) {
+				char c = *b++;
+				if (bytes_read < m) {
+					d += sprintf(d, " %02X",
+						     0x000000FF & c);
+				} else if (bytes_read > m) {
+				} else
+					d += sprintf(d, " ..");
+				bytes_read += 1;
+				continue;
+			}
+			goto more;
+		} else if (packet_bytes > 1) {
+			char s1 = ftdi->bulk_in_buffer[0];
+			char s2 = ftdi->bulk_in_buffer[1];
+			if (s1 == 0x31 && s2 == 0x60) {
+				return 0;
+			} else if (retry_on_status-- > 0) {
+				goto more;
+			} else {
+				dev_err(&ftdi->udev->dev, "STATUS ERROR retry limit reached\n");
+				return -EFAULT;
+			}
+		} else if (packet_bytes > 0) {
+			char b1 = ftdi->bulk_in_buffer[0];
+			dev_err(&ftdi->udev->dev, "only one byte flushed from FTDI = %02X\n",
+				b1);
+			if (retry_on_status-- > 0) {
+				goto more;
+			} else {
+				dev_err(&ftdi->udev->dev, "STATUS ERROR retry limit reached\n");
+				return -EFAULT;
+			}
+		} else if (retval == -ETIMEDOUT) {
+			if (retry_on_timeout-- > 0) {
+				goto more;
+			} else {
+				dev_err(&ftdi->udev->dev, "TIMED OUT retry limit reached\n");
+				return -ENOMEM;
+			}
+		} else if (retval == 0) {
+			if (retry_on_empty-- > 0) {
+				goto more;
+			} else {
+				dev_err(&ftdi->udev->dev, "empty packet retry limit reached\n");
+				return -ENOMEM;
+			}
+		} else {
+			dev_err(&ftdi->udev->dev, "error = %d\n", retval);
+			return retval;
+		}
+	}
+	return -1;
 }
 
 
 /*
-* send the long flush sequence
-*
-*/
+ * send the long flush sequence
+ *
+ */
 static int ftdi_elan_synchronize_flush(struct usb_ftdi *ftdi)
 {
-        int retval;
-        struct urb *urb;
-        char *buf;
-        int I = 257;
-        int i = 0;
-        urb = usb_alloc_urb(0, GFP_KERNEL);
-        if (!urb) {
-                dev_err(&ftdi->udev->dev, "could not alloc a urb for flush sequ"
-                        "ence\n");
-                return -ENOMEM;
-        }
-        buf = usb_alloc_coherent(ftdi->udev, I, GFP_KERNEL, &urb->transfer_dma);
-        if (!buf) {
-                dev_err(&ftdi->udev->dev, "could not get a buffer for flush seq"
-                        "uence\n");
-                usb_free_urb(urb);
-                return -ENOMEM;
-        }
-        while (I-- > 0)
-                buf[i++] = 0x55;
-        usb_fill_bulk_urb(urb, ftdi->udev, usb_sndbulkpipe(ftdi->udev,
-                ftdi->bulk_out_endpointAddr), buf, i,
-                ftdi_elan_write_bulk_callback, ftdi);
-        urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
-        retval = usb_submit_urb(urb, GFP_KERNEL);
-        if (retval) {
-                dev_err(&ftdi->udev->dev, "failed to submit urb containing the "
-                        "flush sequence\n");
-                usb_free_coherent(ftdi->udev, i, buf, urb->transfer_dma);
-                usb_free_urb(urb);
-                return -ENOMEM;
-        }
-        usb_free_urb(urb);
-        return 0;
+	int retval;
+	struct urb *urb;
+	char *buf;
+	int I = 257;
+	int i = 0;
+	urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!urb) {
+		dev_err(&ftdi->udev->dev, "could not alloc a urb for flush sequence\n");
+		return -ENOMEM;
+	}
+	buf = usb_alloc_coherent(ftdi->udev, I, GFP_KERNEL, &urb->transfer_dma);
+	if (!buf) {
+		dev_err(&ftdi->udev->dev, "could not get a buffer for flush sequence\n");
+		usb_free_urb(urb);
+		return -ENOMEM;
+	}
+	while (I-- > 0)
+		buf[i++] = 0x55;
+	usb_fill_bulk_urb(urb, ftdi->udev, usb_sndbulkpipe(ftdi->udev,
+							   ftdi->bulk_out_endpointAddr), buf, i,
+			  ftdi_elan_write_bulk_callback, ftdi);
+	urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+	retval = usb_submit_urb(urb, GFP_KERNEL);
+	if (retval) {
+		dev_err(&ftdi->udev->dev, "failed to submit urb containing the flush sequence\n");
+		usb_free_coherent(ftdi->udev, i, buf, urb->transfer_dma);
+		usb_free_urb(urb);
+		return -ENOMEM;
+	}
+	usb_free_urb(urb);
+	return 0;
 }
 
 
 /*
-* send the reset sequence
-*
-*/
+ * send the reset sequence
+ *
+ */
 static int ftdi_elan_synchronize_reset(struct usb_ftdi *ftdi)
 {
-        int retval;
-        struct urb *urb;
-        char *buf;
-        int I = 4;
-        int i = 0;
-        urb = usb_alloc_urb(0, GFP_KERNEL);
-        if (!urb) {
-                dev_err(&ftdi->udev->dev, "could not get a urb for the reset se"
-                        "quence\n");
-                return -ENOMEM;
-        }
-        buf = usb_alloc_coherent(ftdi->udev, I, GFP_KERNEL, &urb->transfer_dma);
-        if (!buf) {
-                dev_err(&ftdi->udev->dev, "could not get a buffer for the reset"
-                        " sequence\n");
-                usb_free_urb(urb);
-                return -ENOMEM;
-        }
-        buf[i++] = 0x55;
-        buf[i++] = 0xAA;
-        buf[i++] = 0x5A;
-        buf[i++] = 0xA5;
-        usb_fill_bulk_urb(urb, ftdi->udev, usb_sndbulkpipe(ftdi->udev,
-                ftdi->bulk_out_endpointAddr), buf, i,
-                ftdi_elan_write_bulk_callback, ftdi);
-        urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
-        retval = usb_submit_urb(urb, GFP_KERNEL);
-        if (retval) {
-                dev_err(&ftdi->udev->dev, "failed to submit urb containing the "
-                        "reset sequence\n");
-                usb_free_coherent(ftdi->udev, i, buf, urb->transfer_dma);
-                usb_free_urb(urb);
-                return -ENOMEM;
-        }
-        usb_free_urb(urb);
-        return 0;
+	int retval;
+	struct urb *urb;
+	char *buf;
+	int I = 4;
+	int i = 0;
+	urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!urb) {
+		dev_err(&ftdi->udev->dev, "could not get a urb for the reset sequence\n");
+		return -ENOMEM;
+	}
+	buf = usb_alloc_coherent(ftdi->udev, I, GFP_KERNEL, &urb->transfer_dma);
+	if (!buf) {
+		dev_err(&ftdi->udev->dev, "could not get a buffer for the reset sequence\n");
+		usb_free_urb(urb);
+		return -ENOMEM;
+	}
+	buf[i++] = 0x55;
+	buf[i++] = 0xAA;
+	buf[i++] = 0x5A;
+	buf[i++] = 0xA5;
+	usb_fill_bulk_urb(urb, ftdi->udev, usb_sndbulkpipe(ftdi->udev,
+							   ftdi->bulk_out_endpointAddr), buf, i,
+			  ftdi_elan_write_bulk_callback, ftdi);
+	urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+	retval = usb_submit_urb(urb, GFP_KERNEL);
+	if (retval) {
+		dev_err(&ftdi->udev->dev, "failed to submit urb containing the reset sequence\n");
+		usb_free_coherent(ftdi->udev, i, buf, urb->transfer_dma);
+		usb_free_urb(urb);
+		return -ENOMEM;
+	}
+	usb_free_urb(urb);
+	return 0;
 }
 
 static int ftdi_elan_synchronize(struct usb_ftdi *ftdi)
 {
-        int retval;
-        int long_stop = 10;
-        int retry_on_timeout = 5;
-        int retry_on_empty = 10;
-        int err_count = 0;
-        retval = ftdi_elan_flush_input_fifo(ftdi);
-        if (retval)
-                return retval;
-        ftdi->bulk_in_left = 0;
-        ftdi->bulk_in_last = -1;
-        while (long_stop-- > 0) {
-                int read_stop;
-                int read_stuck;
-                retval = ftdi_elan_synchronize_flush(ftdi);
-                if (retval)
-                        return retval;
-                retval = ftdi_elan_flush_input_fifo(ftdi);
-                if (retval)
-                        return retval;
-              reset:retval = ftdi_elan_synchronize_reset(ftdi);
-                if (retval)
-                        return retval;
-                read_stop = 100;
-                read_stuck = 10;
-              read:{
-                        int packet_bytes = 0;
-                        retval = usb_bulk_msg(ftdi->udev,
-                                usb_rcvbulkpipe(ftdi->udev,
-                                ftdi->bulk_in_endpointAddr),
-                                ftdi->bulk_in_buffer, ftdi->bulk_in_size,
-                                &packet_bytes, 500);
-                        if (packet_bytes > 2) {
-                                char diag[30 *3 + 4];
-                                char *d = diag;
-                                int m = (sizeof(diag) - 1) / 3;
-                                char *b = ftdi->bulk_in_buffer;
-                                int bytes_read = 0;
-                                unsigned char c = 0;
-                                diag[0] = 0;
-                                while (packet_bytes-- > 0) {
-                                        c = *b++;
-                                        if (bytes_read < m) {
-                                                d += sprintf(d, " %02X", c);
-                                        } else if (bytes_read > m) {
-                                        } else
-                                                d += sprintf(d, " ..");
-                                        bytes_read += 1;
-                                        continue;
-                                }
-                                if (c == 0x7E) {
-                                        return 0;
-                                } else {
-                                        if (c == 0x55) {
-                                                goto read;
-                                        } else if (read_stop-- > 0) {
-                                                goto read;
-                                        } else {
-                                                dev_err(&ftdi->udev->dev, "retr"
-                                                        "y limit reached\n");
-                                                continue;
-                                        }
-                                }
-                        } else if (packet_bytes > 1) {
-                                unsigned char s1 = ftdi->bulk_in_buffer[0];
-                                unsigned char s2 = ftdi->bulk_in_buffer[1];
-                                if (s1 == 0x31 && s2 == 0x00) {
-                                        if (read_stuck-- > 0) {
-                                                goto read;
-                                        } else
-                                                goto reset;
-                                } else if (s1 == 0x31 && s2 == 0x60) {
-                                        if (read_stop-- > 0) {
-                                                goto read;
-                                        } else {
-                                                dev_err(&ftdi->udev->dev, "retr"
-                                                        "y limit reached\n");
-                                                continue;
-                                        }
-                                } else {
-                                        if (read_stop-- > 0) {
-                                                goto read;
-                                        } else {
-                                                dev_err(&ftdi->udev->dev, "retr"
-                                                        "y limit reached\n");
-                                                continue;
-                                        }
-                                }
-                        } else if (packet_bytes > 0) {
-                                if (read_stop-- > 0) {
-                                        goto read;
-                                } else {
-                                        dev_err(&ftdi->udev->dev, "retry limit "
-                                                "reached\n");
-                                        continue;
-                                }
-                        } else if (retval == -ETIMEDOUT) {
-                                if (retry_on_timeout-- > 0) {
-                                        goto read;
-                                } else {
-                                        dev_err(&ftdi->udev->dev, "TIMED OUT re"
-                                                "try limit reached\n");
-                                        continue;
-                                }
-                        } else if (retval == 0) {
-                                if (retry_on_empty-- > 0) {
-                                        goto read;
-                                } else {
-                                        dev_err(&ftdi->udev->dev, "empty packet"
-                                                " retry limit reached\n");
-                                        continue;
-                                }
-                        } else {
-                                err_count += 1;
-                                dev_err(&ftdi->udev->dev, "error = %d\n",
-                                        retval);
-                                if (read_stop-- > 0) {
-                                        goto read;
-                                } else {
-                                        dev_err(&ftdi->udev->dev, "retry limit "
-                                                "reached\n");
-                                        continue;
-                                }
-                        }
-                }
-        }
-        dev_err(&ftdi->udev->dev, "failed to synchronize\n");
-        return -EFAULT;
+	int retval;
+	int long_stop = 10;
+	int retry_on_timeout = 5;
+	int retry_on_empty = 10;
+	int err_count = 0;
+	retval = ftdi_elan_flush_input_fifo(ftdi);
+	if (retval)
+		return retval;
+	ftdi->bulk_in_left = 0;
+	ftdi->bulk_in_last = -1;
+	while (long_stop-- > 0) {
+		int read_stop;
+		int read_stuck;
+		retval = ftdi_elan_synchronize_flush(ftdi);
+		if (retval)
+			return retval;
+		retval = ftdi_elan_flush_input_fifo(ftdi);
+		if (retval)
+			return retval;
+	reset:retval = ftdi_elan_synchronize_reset(ftdi);
+		if (retval)
+			return retval;
+		read_stop = 100;
+		read_stuck = 10;
+	read:{
+			int packet_bytes = 0;
+			retval = usb_bulk_msg(ftdi->udev,
+					      usb_rcvbulkpipe(ftdi->udev,
+							      ftdi->bulk_in_endpointAddr),
+					      ftdi->bulk_in_buffer, ftdi->bulk_in_size,
+					      &packet_bytes, 500);
+			if (packet_bytes > 2) {
+				char diag[30 *3 + 4];
+				char *d = diag;
+				int m = (sizeof(diag) - 1) / 3;
+				char *b = ftdi->bulk_in_buffer;
+				int bytes_read = 0;
+				unsigned char c = 0;
+				diag[0] = 0;
+				while (packet_bytes-- > 0) {
+					c = *b++;
+					if (bytes_read < m) {
+						d += sprintf(d, " %02X", c);
+					} else if (bytes_read > m) {
+					} else
+						d += sprintf(d, " ..");
+					bytes_read += 1;
+					continue;
+				}
+				if (c == 0x7E) {
+					return 0;
+				} else {
+					if (c == 0x55) {
+						goto read;
+					} else if (read_stop-- > 0) {
+						goto read;
+					} else {
+						dev_err(&ftdi->udev->dev, "retry limit reached\n");
+						continue;
+					}
+				}
+			} else if (packet_bytes > 1) {
+				unsigned char s1 = ftdi->bulk_in_buffer[0];
+				unsigned char s2 = ftdi->bulk_in_buffer[1];
+				if (s1 == 0x31 && s2 == 0x00) {
+					if (read_stuck-- > 0) {
+						goto read;
+					} else
+						goto reset;
+				} else if (s1 == 0x31 && s2 == 0x60) {
+					if (read_stop-- > 0) {
+						goto read;
+					} else {
+						dev_err(&ftdi->udev->dev, "retry limit reached\n");
+						continue;
+					}
+				} else {
+					if (read_stop-- > 0) {
+						goto read;
+					} else {
+						dev_err(&ftdi->udev->dev, "retry limit reached\n");
+						continue;
+					}
+				}
+			} else if (packet_bytes > 0) {
+				if (read_stop-- > 0) {
+					goto read;
+				} else {
+					dev_err(&ftdi->udev->dev, "retry limit reached\n");
+					continue;
+				}
+			} else if (retval == -ETIMEDOUT) {
+				if (retry_on_timeout-- > 0) {
+					goto read;
+				} else {
+					dev_err(&ftdi->udev->dev, "TIMED OUT retry limit reached\n");
+					continue;
+				}
+			} else if (retval == 0) {
+				if (retry_on_empty-- > 0) {
+					goto read;
+				} else {
+					dev_err(&ftdi->udev->dev, "empty packet retry limit reached\n");
+					continue;
+				}
+			} else {
+				err_count += 1;
+				dev_err(&ftdi->udev->dev, "error = %d\n",
+					retval);
+				if (read_stop-- > 0) {
+					goto read;
+				} else {
+					dev_err(&ftdi->udev->dev, "retry limit reached\n");
+					continue;
+				}
+			}
+		}
+	}
+	dev_err(&ftdi->udev->dev, "failed to synchronize\n");
+	return -EFAULT;
 }
 
 static int ftdi_elan_stuck_waiting(struct usb_ftdi *ftdi)
 {
-        int retry_on_empty = 10;
-        int retry_on_timeout = 5;
-        int retry_on_status = 50;
-      more:{
-                int packet_bytes = 0;
-                int retval = usb_bulk_msg(ftdi->udev,
-                        usb_rcvbulkpipe(ftdi->udev, ftdi->bulk_in_endpointAddr),
-                         ftdi->bulk_in_buffer, ftdi->bulk_in_size,
-                        &packet_bytes, 1000);
-                if (packet_bytes > 2) {
-                        char diag[30 *3 + 4];
-                        char *d = diag;
-                        int m = (sizeof(diag) - 1) / 3;
-                        char *b = ftdi->bulk_in_buffer;
-                        int bytes_read = 0;
-                        diag[0] = 0;
-                        while (packet_bytes-- > 0) {
-                                char c = *b++;
-                                if (bytes_read < m) {
-                                        d += sprintf(d, " %02X",
-                                                0x000000FF & c);
-                                } else if (bytes_read > m) {
-                                } else
-                                        d += sprintf(d, " ..");
-                                bytes_read += 1;
-                                continue;
-                        }
-                        goto more;
-                } else if (packet_bytes > 1) {
-                        char s1 = ftdi->bulk_in_buffer[0];
-                        char s2 = ftdi->bulk_in_buffer[1];
-                        if (s1 == 0x31 && s2 == 0x60) {
-                                return 0;
-                        } else if (retry_on_status-- > 0) {
-                                msleep(5);
-                                goto more;
-                        } else
-                                return -EFAULT;
-                } else if (packet_bytes > 0) {
-                        char b1 = ftdi->bulk_in_buffer[0];
-                        dev_err(&ftdi->udev->dev, "only one byte flushed from F"
-                                "TDI = %02X\n", b1);
-                        if (retry_on_status-- > 0) {
-                                msleep(5);
-                                goto more;
-                        } else {
-                                dev_err(&ftdi->udev->dev, "STATUS ERROR retry l"
-                                        "imit reached\n");
-                                return -EFAULT;
-                        }
-                } else if (retval == -ETIMEDOUT) {
-                        if (retry_on_timeout-- > 0) {
-                                goto more;
-                        } else {
-                                dev_err(&ftdi->udev->dev, "TIMED OUT retry limi"
-                                        "t reached\n");
-                                return -ENOMEM;
-                        }
-                } else if (retval == 0) {
-                        if (retry_on_empty-- > 0) {
-                                goto more;
-                        } else {
-                                dev_err(&ftdi->udev->dev, "empty packet retry l"
-                                        "imit reached\n");
-                                return -ENOMEM;
-                        }
-                } else {
-                        dev_err(&ftdi->udev->dev, "error = %d\n", retval);
-                        return -ENOMEM;
-                }
-        }
-        return -1;
+	int retry_on_empty = 10;
+	int retry_on_timeout = 5;
+	int retry_on_status = 50;
+more:{
+		int packet_bytes = 0;
+		int retval = usb_bulk_msg(ftdi->udev,
+					  usb_rcvbulkpipe(ftdi->udev, ftdi->bulk_in_endpointAddr),
+					  ftdi->bulk_in_buffer, ftdi->bulk_in_size,
+					  &packet_bytes, 1000);
+		if (packet_bytes > 2) {
+			char diag[30 *3 + 4];
+			char *d = diag;
+			int m = (sizeof(diag) - 1) / 3;
+			char *b = ftdi->bulk_in_buffer;
+			int bytes_read = 0;
+			diag[0] = 0;
+			while (packet_bytes-- > 0) {
+				char c = *b++;
+				if (bytes_read < m) {
+					d += sprintf(d, " %02X",
+						     0x000000FF & c);
+				} else if (bytes_read > m) {
+				} else
+					d += sprintf(d, " ..");
+				bytes_read += 1;
+				continue;
+			}
+			goto more;
+		} else if (packet_bytes > 1) {
+			char s1 = ftdi->bulk_in_buffer[0];
+			char s2 = ftdi->bulk_in_buffer[1];
+			if (s1 == 0x31 && s2 == 0x60) {
+				return 0;
+			} else if (retry_on_status-- > 0) {
+				msleep(5);
+				goto more;
+			} else
+				return -EFAULT;
+		} else if (packet_bytes > 0) {
+			char b1 = ftdi->bulk_in_buffer[0];
+			dev_err(&ftdi->udev->dev, "only one byte flushed from FTDI = %02X\n", b1);
+			if (retry_on_status-- > 0) {
+				msleep(5);
+				goto more;
+			} else {
+				dev_err(&ftdi->udev->dev, "STATUS ERROR retry limit reached\n");
+				return -EFAULT;
+			}
+		} else if (retval == -ETIMEDOUT) {
+			if (retry_on_timeout-- > 0) {
+				goto more;
+			} else {
+				dev_err(&ftdi->udev->dev, "TIMED OUT retry limit reached\n");
+				return -ENOMEM;
+			}
+		} else if (retval == 0) {
+			if (retry_on_empty-- > 0) {
+				goto more;
+			} else {
+				dev_err(&ftdi->udev->dev, "empty packet retry limit reached\n");
+				return -ENOMEM;
+			}
+		} else {
+			dev_err(&ftdi->udev->dev, "error = %d\n", retval);
+			return -ENOMEM;
+		}
+	}
+	return -1;
 }
 
 static int ftdi_elan_checkingPCI(struct usb_ftdi *ftdi)
 {
-        int UxxxStatus = ftdi_elan_read_reg(ftdi, &ftdi->controlreg);
-        if (UxxxStatus)
-                return UxxxStatus;
-        if (ftdi->controlreg & 0x00400000) {
-                if (ftdi->card_ejected) {
-                } else {
-                        ftdi->card_ejected = 1;
-                        dev_err(&ftdi->udev->dev, "CARD EJECTED - controlreg = "
-                                "%08X\n", ftdi->controlreg);
-                }
-                return -ENODEV;
-        } else {
-                u8 fn = ftdi->function - 1;
-                int activePCIfn = fn << 8;
-                u32 pcidata;
-                u32 pciVID;
-                u32 pciPID;
-                int reg = 0;
-                UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
-                        &pcidata);
-                if (UxxxStatus)
-                        return UxxxStatus;
-                pciVID = pcidata & 0xFFFF;
-                pciPID = (pcidata >> 16) & 0xFFFF;
-                if (pciVID == ftdi->platform_data.vendor && pciPID ==
-                        ftdi->platform_data.device) {
-                        return 0;
-                } else {
-                        dev_err(&ftdi->udev->dev, "vendor=%04X pciVID=%04X devi"
-                                "ce=%04X pciPID=%04X\n",
-                                ftdi->platform_data.vendor, pciVID,
-                                ftdi->platform_data.device, pciPID);
-                        return -ENODEV;
-                }
-        }
+	int UxxxStatus = ftdi_elan_read_reg(ftdi, &ftdi->controlreg);
+	if (UxxxStatus)
+		return UxxxStatus;
+	if (ftdi->controlreg & 0x00400000) {
+		if (ftdi->card_ejected) {
+		} else {
+			ftdi->card_ejected = 1;
+			dev_err(&ftdi->udev->dev, "CARD EJECTED - controlreg = %08X\n",
+				ftdi->controlreg);
+		}
+		return -ENODEV;
+	} else {
+		u8 fn = ftdi->function - 1;
+		int activePCIfn = fn << 8;
+		u32 pcidata;
+		u32 pciVID;
+		u32 pciPID;
+		int reg = 0;
+		UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
+						   &pcidata);
+		if (UxxxStatus)
+			return UxxxStatus;
+		pciVID = pcidata & 0xFFFF;
+		pciPID = (pcidata >> 16) & 0xFFFF;
+		if (pciVID == ftdi->platform_data.vendor && pciPID ==
+		    ftdi->platform_data.device) {
+			return 0;
+		} else {
+			dev_err(&ftdi->udev->dev, "vendor=%04X pciVID=%04X device=%04X pciPID=%04X\n",
+				ftdi->platform_data.vendor, pciVID,
+				ftdi->platform_data.device, pciPID);
+			return -ENODEV;
+		}
+	}
 }
 
 
 #define ftdi_read_pcimem(ftdi, member, data) ftdi_elan_read_pcimem(ftdi, \
-        offsetof(struct ohci_regs, member), 0, data);
+								   offsetof(struct ohci_regs, member), 0, data);
 #define ftdi_write_pcimem(ftdi, member, data) ftdi_elan_write_pcimem(ftdi, \
-        offsetof(struct ohci_regs, member), 0, data);
+								     offsetof(struct ohci_regs, member), 0, data);
 
 #define OHCI_CONTROL_INIT OHCI_CTRL_CBSR
-#define OHCI_INTR_INIT (OHCI_INTR_MIE | OHCI_INTR_UE | OHCI_INTR_RD | \
-        OHCI_INTR_WDH)
+#define OHCI_INTR_INIT (OHCI_INTR_MIE | OHCI_INTR_UE | OHCI_INTR_RD |	\
+			OHCI_INTR_WDH)
 static int ftdi_elan_check_controller(struct usb_ftdi *ftdi, int quirk)
 {
-        int devices = 0;
-        int retval;
-        u32 hc_control;
-        int num_ports;
-        u32 control;
-        u32 rh_a = -1;
-        u32 status;
-        u32 fminterval;
-        u32 hc_fminterval;
-        u32 periodicstart;
-        u32 cmdstatus;
-        u32 roothub_a;
-        int mask = OHCI_INTR_INIT;
-        int sleep_time = 0;
-        int reset_timeout = 30;        /* ... allow extra time */
-        int temp;
-        retval = ftdi_write_pcimem(ftdi, intrdisable, OHCI_INTR_MIE);
-        if (retval)
-                return retval;
-        retval = ftdi_read_pcimem(ftdi, control, &control);
-        if (retval)
-                return retval;
-        retval = ftdi_read_pcimem(ftdi, roothub.a, &rh_a);
-        if (retval)
-                return retval;
-        num_ports = rh_a & RH_A_NDP;
-        retval = ftdi_read_pcimem(ftdi, fminterval, &hc_fminterval);
-        if (retval)
-                return retval;
-        hc_fminterval &= 0x3fff;
-        if (hc_fminterval != FI) {
-        }
-        hc_fminterval |= FSMP(hc_fminterval) << 16;
-        retval = ftdi_read_pcimem(ftdi, control, &hc_control);
-        if (retval)
-                return retval;
-        switch (hc_control & OHCI_CTRL_HCFS) {
-        case OHCI_USB_OPER:
-                sleep_time = 0;
-                break;
-        case OHCI_USB_SUSPEND:
-        case OHCI_USB_RESUME:
-                hc_control &= OHCI_CTRL_RWC;
-                hc_control |= OHCI_USB_RESUME;
-                sleep_time = 10;
-                break;
-        default:
-                hc_control &= OHCI_CTRL_RWC;
-                hc_control |= OHCI_USB_RESET;
-                sleep_time = 50;
-                break;
-        }
-        retval = ftdi_write_pcimem(ftdi, control, hc_control);
-        if (retval)
-                return retval;
-        retval = ftdi_read_pcimem(ftdi, control, &control);
-        if (retval)
-                return retval;
-        msleep(sleep_time);
-        retval = ftdi_read_pcimem(ftdi, roothub.a, &roothub_a);
-        if (retval)
-                return retval;
-        if (!(roothub_a & RH_A_NPS)) {        /* power down each port */
-                for (temp = 0; temp < num_ports; temp++) {
-                        retval = ftdi_write_pcimem(ftdi,
-                                roothub.portstatus[temp], RH_PS_LSDA);
-                        if (retval)
-                                return retval;
-                }
-        }
-        retval = ftdi_read_pcimem(ftdi, control, &control);
-        if (retval)
-                return retval;
-      retry:retval = ftdi_read_pcimem(ftdi, cmdstatus, &status);
-        if (retval)
-                return retval;
-        retval = ftdi_write_pcimem(ftdi, cmdstatus, OHCI_HCR);
-        if (retval)
-                return retval;
-      extra:{
-                retval = ftdi_read_pcimem(ftdi, cmdstatus, &status);
-                if (retval)
-                        return retval;
-                if (0 != (status & OHCI_HCR)) {
-                        if (--reset_timeout == 0) {
-                                dev_err(&ftdi->udev->dev, "USB HC reset timed o"
-                                        "ut!\n");
-                                return -ENODEV;
-                        } else {
-                                msleep(5);
-                                goto extra;
-                        }
-                }
-        }
-        if (quirk & OHCI_QUIRK_INITRESET) {
-                retval = ftdi_write_pcimem(ftdi, control, hc_control);
-                if (retval)
-                        return retval;
-                retval = ftdi_read_pcimem(ftdi, control, &control);
-                if (retval)
-                        return retval;
-        }
-        retval = ftdi_write_pcimem(ftdi, ed_controlhead, 0x00000000);
-        if (retval)
-                return retval;
-        retval = ftdi_write_pcimem(ftdi, ed_bulkhead, 0x11000000);
-        if (retval)
-                return retval;
-        retval = ftdi_write_pcimem(ftdi, hcca, 0x00000000);
-        if (retval)
-                return retval;
-        retval = ftdi_read_pcimem(ftdi, fminterval, &fminterval);
-        if (retval)
-                return retval;
-        retval = ftdi_write_pcimem(ftdi, fminterval,
-                ((fminterval & FIT) ^ FIT) | hc_fminterval);
-        if (retval)
-                return retval;
-        retval = ftdi_write_pcimem(ftdi, periodicstart,
-                ((9 *hc_fminterval) / 10) & 0x3fff);
-        if (retval)
-                return retval;
-        retval = ftdi_read_pcimem(ftdi, fminterval, &fminterval);
-        if (retval)
-                return retval;
-        retval = ftdi_read_pcimem(ftdi, periodicstart, &periodicstart);
-        if (retval)
-                return retval;
-        if (0 == (fminterval & 0x3fff0000) || 0 == periodicstart) {
-                if (!(quirk & OHCI_QUIRK_INITRESET)) {
-                        quirk |= OHCI_QUIRK_INITRESET;
-                        goto retry;
-                } else
-                        dev_err(&ftdi->udev->dev, "init err(%08x %04x)\n",
-                                fminterval, periodicstart);
-        }                        /* start controller operations */
-        hc_control &= OHCI_CTRL_RWC;
-        hc_control |= OHCI_CONTROL_INIT | OHCI_CTRL_BLE | OHCI_USB_OPER;
-        retval = ftdi_write_pcimem(ftdi, control, hc_control);
-        if (retval)
-                return retval;
-        retval = ftdi_write_pcimem(ftdi, cmdstatus, OHCI_BLF);
-        if (retval)
-                return retval;
-        retval = ftdi_read_pcimem(ftdi, cmdstatus, &cmdstatus);
-        if (retval)
-                return retval;
-        retval = ftdi_read_pcimem(ftdi, control, &control);
-        if (retval)
-                return retval;
-        retval = ftdi_write_pcimem(ftdi, roothub.status, RH_HS_DRWE);
-        if (retval)
-                return retval;
-        retval = ftdi_write_pcimem(ftdi, intrstatus, mask);
-        if (retval)
-                return retval;
-        retval = ftdi_write_pcimem(ftdi, intrdisable,
-                OHCI_INTR_MIE | OHCI_INTR_OC | OHCI_INTR_RHSC | OHCI_INTR_FNO |
-                OHCI_INTR_UE | OHCI_INTR_RD | OHCI_INTR_SF | OHCI_INTR_WDH |
-                OHCI_INTR_SO);
-        if (retval)
-                return retval;        /* handle root hub init quirks ... */
-        retval = ftdi_read_pcimem(ftdi, roothub.a, &roothub_a);
-        if (retval)
-                return retval;
-        roothub_a &= ~(RH_A_PSM | RH_A_OCPM);
-        if (quirk & OHCI_QUIRK_SUPERIO) {
-                roothub_a |= RH_A_NOCP;
-                roothub_a &= ~(RH_A_POTPGT | RH_A_NPS);
-                retval = ftdi_write_pcimem(ftdi, roothub.a, roothub_a);
-                if (retval)
-                        return retval;
-        } else if ((quirk & OHCI_QUIRK_AMD756) || distrust_firmware) {
-                roothub_a |= RH_A_NPS;
-                retval = ftdi_write_pcimem(ftdi, roothub.a, roothub_a);
-                if (retval)
-                        return retval;
-        }
-        retval = ftdi_write_pcimem(ftdi, roothub.status, RH_HS_LPSC);
-        if (retval)
-                return retval;
-        retval = ftdi_write_pcimem(ftdi, roothub.b,
-                (roothub_a & RH_A_NPS) ? 0 : RH_B_PPCM);
-        if (retval)
-                return retval;
-        retval = ftdi_read_pcimem(ftdi, control, &control);
-        if (retval)
-                return retval;
-        mdelay((roothub_a >> 23) & 0x1fe);
-        for (temp = 0; temp < num_ports; temp++) {
-                u32 portstatus;
-                retval = ftdi_read_pcimem(ftdi, roothub.portstatus[temp],
-                        &portstatus);
-                if (retval)
-                        return retval;
-                if (1 & portstatus)
-                        devices += 1;
-        }
-        return devices;
+	int devices = 0;
+	int retval;
+	u32 hc_control;
+	int num_ports;
+	u32 control;
+	u32 rh_a = -1;
+	u32 status;
+	u32 fminterval;
+	u32 hc_fminterval;
+	u32 periodicstart;
+	u32 cmdstatus;
+	u32 roothub_a;
+	int mask = OHCI_INTR_INIT;
+	int sleep_time = 0;
+	int reset_timeout = 30;        /* ... allow extra time */
+	int temp;
+	retval = ftdi_write_pcimem(ftdi, intrdisable, OHCI_INTR_MIE);
+	if (retval)
+		return retval;
+	retval = ftdi_read_pcimem(ftdi, control, &control);
+	if (retval)
+		return retval;
+	retval = ftdi_read_pcimem(ftdi, roothub.a, &rh_a);
+	if (retval)
+		return retval;
+	num_ports = rh_a & RH_A_NDP;
+	retval = ftdi_read_pcimem(ftdi, fminterval, &hc_fminterval);
+	if (retval)
+		return retval;
+	hc_fminterval &= 0x3fff;
+	if (hc_fminterval != FI) {
+	}
+	hc_fminterval |= FSMP(hc_fminterval) << 16;
+	retval = ftdi_read_pcimem(ftdi, control, &hc_control);
+	if (retval)
+		return retval;
+	switch (hc_control & OHCI_CTRL_HCFS) {
+	case OHCI_USB_OPER:
+		sleep_time = 0;
+		break;
+	case OHCI_USB_SUSPEND:
+	case OHCI_USB_RESUME:
+		hc_control &= OHCI_CTRL_RWC;
+		hc_control |= OHCI_USB_RESUME;
+		sleep_time = 10;
+		break;
+	default:
+		hc_control &= OHCI_CTRL_RWC;
+		hc_control |= OHCI_USB_RESET;
+		sleep_time = 50;
+		break;
+	}
+	retval = ftdi_write_pcimem(ftdi, control, hc_control);
+	if (retval)
+		return retval;
+	retval = ftdi_read_pcimem(ftdi, control, &control);
+	if (retval)
+		return retval;
+	msleep(sleep_time);
+	retval = ftdi_read_pcimem(ftdi, roothub.a, &roothub_a);
+	if (retval)
+		return retval;
+	if (!(roothub_a & RH_A_NPS)) {        /* power down each port */
+		for (temp = 0; temp < num_ports; temp++) {
+			retval = ftdi_write_pcimem(ftdi,
+						   roothub.portstatus[temp], RH_PS_LSDA);
+			if (retval)
+				return retval;
+		}
+	}
+	retval = ftdi_read_pcimem(ftdi, control, &control);
+	if (retval)
+		return retval;
+retry:retval = ftdi_read_pcimem(ftdi, cmdstatus, &status);
+	if (retval)
+		return retval;
+	retval = ftdi_write_pcimem(ftdi, cmdstatus, OHCI_HCR);
+	if (retval)
+		return retval;
+extra:{
+		retval = ftdi_read_pcimem(ftdi, cmdstatus, &status);
+		if (retval)
+			return retval;
+		if (0 != (status & OHCI_HCR)) {
+			if (--reset_timeout == 0) {
+				dev_err(&ftdi->udev->dev, "USB HC reset timed out!\n");
+				return -ENODEV;
+			} else {
+				msleep(5);
+				goto extra;
+			}
+		}
+	}
+	if (quirk & OHCI_QUIRK_INITRESET) {
+		retval = ftdi_write_pcimem(ftdi, control, hc_control);
+		if (retval)
+			return retval;
+		retval = ftdi_read_pcimem(ftdi, control, &control);
+		if (retval)
+			return retval;
+	}
+	retval = ftdi_write_pcimem(ftdi, ed_controlhead, 0x00000000);
+	if (retval)
+		return retval;
+	retval = ftdi_write_pcimem(ftdi, ed_bulkhead, 0x11000000);
+	if (retval)
+		return retval;
+	retval = ftdi_write_pcimem(ftdi, hcca, 0x00000000);
+	if (retval)
+		return retval;
+	retval = ftdi_read_pcimem(ftdi, fminterval, &fminterval);
+	if (retval)
+		return retval;
+	retval = ftdi_write_pcimem(ftdi, fminterval,
+				   ((fminterval & FIT) ^ FIT) | hc_fminterval);
+	if (retval)
+		return retval;
+	retval = ftdi_write_pcimem(ftdi, periodicstart,
+				   ((9 *hc_fminterval) / 10) & 0x3fff);
+	if (retval)
+		return retval;
+	retval = ftdi_read_pcimem(ftdi, fminterval, &fminterval);
+	if (retval)
+		return retval;
+	retval = ftdi_read_pcimem(ftdi, periodicstart, &periodicstart);
+	if (retval)
+		return retval;
+	if (0 == (fminterval & 0x3fff0000) || 0 == periodicstart) {
+		if (!(quirk & OHCI_QUIRK_INITRESET)) {
+			quirk |= OHCI_QUIRK_INITRESET;
+			goto retry;
+		} else
+			dev_err(&ftdi->udev->dev, "init err(%08x %04x)\n",
+				fminterval, periodicstart);
+	}                        /* start controller operations */
+	hc_control &= OHCI_CTRL_RWC;
+	hc_control |= OHCI_CONTROL_INIT | OHCI_CTRL_BLE | OHCI_USB_OPER;
+	retval = ftdi_write_pcimem(ftdi, control, hc_control);
+	if (retval)
+		return retval;
+	retval = ftdi_write_pcimem(ftdi, cmdstatus, OHCI_BLF);
+	if (retval)
+		return retval;
+	retval = ftdi_read_pcimem(ftdi, cmdstatus, &cmdstatus);
+	if (retval)
+		return retval;
+	retval = ftdi_read_pcimem(ftdi, control, &control);
+	if (retval)
+		return retval;
+	retval = ftdi_write_pcimem(ftdi, roothub.status, RH_HS_DRWE);
+	if (retval)
+		return retval;
+	retval = ftdi_write_pcimem(ftdi, intrstatus, mask);
+	if (retval)
+		return retval;
+	retval = ftdi_write_pcimem(ftdi, intrdisable,
+				   OHCI_INTR_MIE | OHCI_INTR_OC | OHCI_INTR_RHSC | OHCI_INTR_FNO |
+				   OHCI_INTR_UE | OHCI_INTR_RD | OHCI_INTR_SF | OHCI_INTR_WDH |
+				   OHCI_INTR_SO);
+	if (retval)
+		return retval;        /* handle root hub init quirks ... */
+	retval = ftdi_read_pcimem(ftdi, roothub.a, &roothub_a);
+	if (retval)
+		return retval;
+	roothub_a &= ~(RH_A_PSM | RH_A_OCPM);
+	if (quirk & OHCI_QUIRK_SUPERIO) {
+		roothub_a |= RH_A_NOCP;
+		roothub_a &= ~(RH_A_POTPGT | RH_A_NPS);
+		retval = ftdi_write_pcimem(ftdi, roothub.a, roothub_a);
+		if (retval)
+			return retval;
+	} else if ((quirk & OHCI_QUIRK_AMD756) || distrust_firmware) {
+		roothub_a |= RH_A_NPS;
+		retval = ftdi_write_pcimem(ftdi, roothub.a, roothub_a);
+		if (retval)
+			return retval;
+	}
+	retval = ftdi_write_pcimem(ftdi, roothub.status, RH_HS_LPSC);
+	if (retval)
+		return retval;
+	retval = ftdi_write_pcimem(ftdi, roothub.b,
+				   (roothub_a & RH_A_NPS) ? 0 : RH_B_PPCM);
+	if (retval)
+		return retval;
+	retval = ftdi_read_pcimem(ftdi, control, &control);
+	if (retval)
+		return retval;
+	mdelay((roothub_a >> 23) & 0x1fe);
+	for (temp = 0; temp < num_ports; temp++) {
+		u32 portstatus;
+		retval = ftdi_read_pcimem(ftdi, roothub.portstatus[temp],
+					  &portstatus);
+		if (retval)
+			return retval;
+		if (1 & portstatus)
+			devices += 1;
+	}
+	return devices;
 }
 
 static int ftdi_elan_setup_controller(struct usb_ftdi *ftdi, int fn)
 {
-        u32 latence_timer;
-        int UxxxStatus;
-        u32 pcidata;
-        int reg = 0;
-        int activePCIfn = fn << 8;
-        UxxxStatus = ftdi_elan_write_reg(ftdi, 0x0000025FL | 0x2800);
-        if (UxxxStatus)
-                return UxxxStatus;
-        reg = 16;
-        UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0,
-                0xFFFFFFFF);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
-                &pcidata);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0,
-                0xF0000000);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
-                &pcidata);
-        if (UxxxStatus)
-                return UxxxStatus;
-        reg = 12;
-        UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
-                &latence_timer);
-        if (UxxxStatus)
-                return UxxxStatus;
-        latence_timer &= 0xFFFF00FF;
-        latence_timer |= 0x00001600;
-        UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0x00,
-                latence_timer);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
-                &pcidata);
-        if (UxxxStatus)
-                return UxxxStatus;
-        reg = 4;
-        UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0x00,
-                0x06);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
-                &pcidata);
-        if (UxxxStatus)
-                return UxxxStatus;
-        for (reg = 0; reg <= 0x54; reg += 4) {
-                UxxxStatus = ftdi_elan_read_pcimem(ftdi, reg, 0, &pcidata);
-                if (UxxxStatus)
-                        return UxxxStatus;
-        }
-        return 0;
+	u32 latence_timer;
+	int UxxxStatus;
+	u32 pcidata;
+	int reg = 0;
+	int activePCIfn = fn << 8;
+	UxxxStatus = ftdi_elan_write_reg(ftdi, 0x0000025FL | 0x2800);
+	if (UxxxStatus)
+		return UxxxStatus;
+	reg = 16;
+	UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0,
+					    0xFFFFFFFF);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
+					   &pcidata);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0,
+					    0xF0000000);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
+					   &pcidata);
+	if (UxxxStatus)
+		return UxxxStatus;
+	reg = 12;
+	UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
+					   &latence_timer);
+	if (UxxxStatus)
+		return UxxxStatus;
+	latence_timer &= 0xFFFF00FF;
+	latence_timer |= 0x00001600;
+	UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0x00,
+					    latence_timer);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
+					   &pcidata);
+	if (UxxxStatus)
+		return UxxxStatus;
+	reg = 4;
+	UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0x00,
+					    0x06);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
+					   &pcidata);
+	if (UxxxStatus)
+		return UxxxStatus;
+	for (reg = 0; reg <= 0x54; reg += 4) {
+		UxxxStatus = ftdi_elan_read_pcimem(ftdi, reg, 0, &pcidata);
+		if (UxxxStatus)
+			return UxxxStatus;
+	}
+	return 0;
 }
 
 static int ftdi_elan_close_controller(struct usb_ftdi *ftdi, int fn)
 {
-        u32 latence_timer;
-        int UxxxStatus;
-        u32 pcidata;
-        int reg = 0;
-        int activePCIfn = fn << 8;
-        UxxxStatus = ftdi_elan_write_reg(ftdi, 0x0000025FL | 0x2800);
-        if (UxxxStatus)
-                return UxxxStatus;
-        reg = 16;
-        UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0,
-                0xFFFFFFFF);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
-                &pcidata);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0,
-                0x00000000);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
-                &pcidata);
-        if (UxxxStatus)
-                return UxxxStatus;
-        reg = 12;
-        UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
-                &latence_timer);
-        if (UxxxStatus)
-                return UxxxStatus;
-        latence_timer &= 0xFFFF00FF;
-        latence_timer |= 0x00001600;
-        UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0x00,
-                latence_timer);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
-                &pcidata);
-        if (UxxxStatus)
-                return UxxxStatus;
-        reg = 4;
-        UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0x00,
-                0x00);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
-                &pcidata);
-        if (UxxxStatus)
-                return UxxxStatus;
-        return 0;
+	u32 latence_timer;
+	int UxxxStatus;
+	u32 pcidata;
+	int reg = 0;
+	int activePCIfn = fn << 8;
+	UxxxStatus = ftdi_elan_write_reg(ftdi, 0x0000025FL | 0x2800);
+	if (UxxxStatus)
+		return UxxxStatus;
+	reg = 16;
+	UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0,
+					    0xFFFFFFFF);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
+					   &pcidata);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0,
+					    0x00000000);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
+					   &pcidata);
+	if (UxxxStatus)
+		return UxxxStatus;
+	reg = 12;
+	UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
+					   &latence_timer);
+	if (UxxxStatus)
+		return UxxxStatus;
+	latence_timer &= 0xFFFF00FF;
+	latence_timer |= 0x00001600;
+	UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0x00,
+					    latence_timer);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
+					   &pcidata);
+	if (UxxxStatus)
+		return UxxxStatus;
+	reg = 4;
+	UxxxStatus = ftdi_elan_write_config(ftdi, activePCIfn | reg, 0x00,
+					    0x00);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
+					   &pcidata);
+	if (UxxxStatus)
+		return UxxxStatus;
+	return 0;
 }
 
 static int ftdi_elan_found_controller(struct usb_ftdi *ftdi, int fn, int quirk)
 {
-        int result;
-        int UxxxStatus;
-        UxxxStatus = ftdi_elan_setup_controller(ftdi, fn);
-        if (UxxxStatus)
-                return UxxxStatus;
-        result = ftdi_elan_check_controller(ftdi, quirk);
-        UxxxStatus = ftdi_elan_close_controller(ftdi, fn);
-        if (UxxxStatus)
-                return UxxxStatus;
-        return result;
+	int result;
+	int UxxxStatus;
+	UxxxStatus = ftdi_elan_setup_controller(ftdi, fn);
+	if (UxxxStatus)
+		return UxxxStatus;
+	result = ftdi_elan_check_controller(ftdi, quirk);
+	UxxxStatus = ftdi_elan_close_controller(ftdi, fn);
+	if (UxxxStatus)
+		return UxxxStatus;
+	return result;
 }
 
 static int ftdi_elan_enumeratePCI(struct usb_ftdi *ftdi)
 {
-        u32 controlreg;
-        u8 sensebits;
-        int UxxxStatus;
-        UxxxStatus = ftdi_elan_read_reg(ftdi, &controlreg);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_write_reg(ftdi, 0x00000000L);
-        if (UxxxStatus)
-                return UxxxStatus;
-        msleep(750);
-        UxxxStatus = ftdi_elan_write_reg(ftdi, 0x00000200L | 0x100);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_write_reg(ftdi, 0x00000200L | 0x500);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_read_reg(ftdi, &controlreg);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_write_reg(ftdi, 0x0000020CL | 0x000);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_write_reg(ftdi, 0x0000020DL | 0x000);
-        if (UxxxStatus)
-                return UxxxStatus;
-        msleep(250);
-        UxxxStatus = ftdi_elan_write_reg(ftdi, 0x0000020FL | 0x000);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_read_reg(ftdi, &controlreg);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_write_reg(ftdi, 0x0000025FL | 0x800);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_read_reg(ftdi, &controlreg);
-        if (UxxxStatus)
-                return UxxxStatus;
-        UxxxStatus = ftdi_elan_read_reg(ftdi, &controlreg);
-        if (UxxxStatus)
-                return UxxxStatus;
-        msleep(1000);
-        sensebits = (controlreg >> 16) & 0x000F;
-        if (0x0D == sensebits)
-                return 0;
-        else
+	u32 controlreg;
+	u8 sensebits;
+	int UxxxStatus;
+	UxxxStatus = ftdi_elan_read_reg(ftdi, &controlreg);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_write_reg(ftdi, 0x00000000L);
+	if (UxxxStatus)
+		return UxxxStatus;
+	msleep(750);
+	UxxxStatus = ftdi_elan_write_reg(ftdi, 0x00000200L | 0x100);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_write_reg(ftdi, 0x00000200L | 0x500);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_read_reg(ftdi, &controlreg);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_write_reg(ftdi, 0x0000020CL | 0x000);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_write_reg(ftdi, 0x0000020DL | 0x000);
+	if (UxxxStatus)
+		return UxxxStatus;
+	msleep(250);
+	UxxxStatus = ftdi_elan_write_reg(ftdi, 0x0000020FL | 0x000);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_read_reg(ftdi, &controlreg);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_write_reg(ftdi, 0x0000025FL | 0x800);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_read_reg(ftdi, &controlreg);
+	if (UxxxStatus)
+		return UxxxStatus;
+	UxxxStatus = ftdi_elan_read_reg(ftdi, &controlreg);
+	if (UxxxStatus)
+		return UxxxStatus;
+	msleep(1000);
+	sensebits = (controlreg >> 16) & 0x000F;
+	if (0x0D == sensebits)
+		return 0;
+	else
 		return - ENXIO;
 }
 
 static int ftdi_elan_setupOHCI(struct usb_ftdi *ftdi)
 {
-        int UxxxStatus;
-        u32 pcidata;
-        int reg = 0;
-        u8 fn;
-        int activePCIfn = 0;
-        int max_devices = 0;
-        int controllers = 0;
-        int unrecognized = 0;
-        ftdi->function = 0;
-        for (fn = 0; (fn < 4); fn++) {
-                u32 pciVID = 0;
-                u32 pciPID = 0;
-                int devices = 0;
-                activePCIfn = fn << 8;
-                UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
-                        &pcidata);
-                if (UxxxStatus)
-                        return UxxxStatus;
-                pciVID = pcidata & 0xFFFF;
-                pciPID = (pcidata >> 16) & 0xFFFF;
-                if ((pciVID == PCI_VENDOR_ID_OPTI) && (pciPID == 0xc861)) {
-                        devices = ftdi_elan_found_controller(ftdi, fn, 0);
-                        controllers += 1;
-                } else if ((pciVID == PCI_VENDOR_ID_NEC) && (pciPID == 0x0035))
-                        {
-                        devices = ftdi_elan_found_controller(ftdi, fn, 0);
-                        controllers += 1;
-                } else if ((pciVID == PCI_VENDOR_ID_AL) && (pciPID == 0x5237)) {
-                        devices = ftdi_elan_found_controller(ftdi, fn, 0);
-                        controllers += 1;
-                } else if ((pciVID == PCI_VENDOR_ID_ATT) && (pciPID == 0x5802))
-                        {
-                        devices = ftdi_elan_found_controller(ftdi, fn, 0);
-                        controllers += 1;
-                } else if (pciVID == PCI_VENDOR_ID_AMD && pciPID == 0x740c) {
-                        devices = ftdi_elan_found_controller(ftdi, fn,
-                                OHCI_QUIRK_AMD756);
-                        controllers += 1;
-                } else if (pciVID == PCI_VENDOR_ID_COMPAQ && pciPID == 0xa0f8) {
-                        devices = ftdi_elan_found_controller(ftdi, fn,
-                                OHCI_QUIRK_ZFMICRO);
-                        controllers += 1;
-                } else if (0 == pcidata) {
-                } else
-                        unrecognized += 1;
-                if (devices > max_devices) {
-                        max_devices = devices;
-                        ftdi->function = fn + 1;
-                        ftdi->platform_data.vendor = pciVID;
-                        ftdi->platform_data.device = pciPID;
-                }
-        }
-        if (ftdi->function > 0) {
-                UxxxStatus = ftdi_elan_setup_controller(ftdi,
-                        ftdi->function - 1);
-                if (UxxxStatus)
-                        return UxxxStatus;
-                return 0;
-        } else if (controllers > 0) {
-                return -ENXIO;
-        } else if (unrecognized > 0) {
-                return -ENXIO;
-        } else {
-                ftdi->enumerated = 0;
-                return -ENXIO;
-        }
+	int UxxxStatus;
+	u32 pcidata;
+	int reg = 0;
+	u8 fn;
+	int activePCIfn = 0;
+	int max_devices = 0;
+	int controllers = 0;
+	int unrecognized = 0;
+	ftdi->function = 0;
+	for (fn = 0; (fn < 4); fn++) {
+		u32 pciVID = 0;
+		u32 pciPID = 0;
+		int devices = 0;
+		activePCIfn = fn << 8;
+		UxxxStatus = ftdi_elan_read_config(ftdi, activePCIfn | reg, 0,
+						   &pcidata);
+		if (UxxxStatus)
+			return UxxxStatus;
+		pciVID = pcidata & 0xFFFF;
+		pciPID = (pcidata >> 16) & 0xFFFF;
+		if ((pciVID == PCI_VENDOR_ID_OPTI) && (pciPID == 0xc861)) {
+			devices = ftdi_elan_found_controller(ftdi, fn, 0);
+			controllers += 1;
+		} else if ((pciVID == PCI_VENDOR_ID_NEC) && (pciPID == 0x0035))
+		{
+			devices = ftdi_elan_found_controller(ftdi, fn, 0);
+			controllers += 1;
+		} else if ((pciVID == PCI_VENDOR_ID_AL) && (pciPID == 0x5237)) {
+			devices = ftdi_elan_found_controller(ftdi, fn, 0);
+			controllers += 1;
+		} else if ((pciVID == PCI_VENDOR_ID_ATT) && (pciPID == 0x5802))
+		{
+			devices = ftdi_elan_found_controller(ftdi, fn, 0);
+			controllers += 1;
+		} else if (pciVID == PCI_VENDOR_ID_AMD && pciPID == 0x740c) {
+			devices = ftdi_elan_found_controller(ftdi, fn,
+							     OHCI_QUIRK_AMD756);
+			controllers += 1;
+		} else if (pciVID == PCI_VENDOR_ID_COMPAQ && pciPID == 0xa0f8) {
+			devices = ftdi_elan_found_controller(ftdi, fn,
+							     OHCI_QUIRK_ZFMICRO);
+			controllers += 1;
+		} else if (0 == pcidata) {
+		} else
+			unrecognized += 1;
+		if (devices > max_devices) {
+			max_devices = devices;
+			ftdi->function = fn + 1;
+			ftdi->platform_data.vendor = pciVID;
+			ftdi->platform_data.device = pciPID;
+		}
+	}
+	if (ftdi->function > 0) {
+		UxxxStatus = ftdi_elan_setup_controller(ftdi,
+							ftdi->function - 1);
+		if (UxxxStatus)
+			return UxxxStatus;
+		return 0;
+	} else if (controllers > 0) {
+		return -ENXIO;
+	} else if (unrecognized > 0) {
+		return -ENXIO;
+	} else {
+		ftdi->enumerated = 0;
+		return -ENXIO;
+	}
 }
 
 
 /*
-* we use only the first bulk-in and bulk-out endpoints
-*/
+ * we use only the first bulk-in and bulk-out endpoints
+ */
 static int ftdi_elan_probe(struct usb_interface *interface,
-        const struct usb_device_id *id)
+			   const struct usb_device_id *id)
 {
-        struct usb_host_interface *iface_desc;
-        struct usb_endpoint_descriptor *endpoint;
-        size_t buffer_size;
-        int i;
-        int retval = -ENOMEM;
-        struct usb_ftdi *ftdi;
+	struct usb_host_interface *iface_desc;
+	struct usb_endpoint_descriptor *endpoint;
+	size_t buffer_size;
+	int i;
+	int retval = -ENOMEM;
+	struct usb_ftdi *ftdi;
 
 	ftdi = kzalloc(sizeof(struct usb_ftdi), GFP_KERNEL);
-	if (!ftdi) {
-                printk(KERN_ERR "Out of memory\n");
-                return -ENOMEM;
-        }
-
-        mutex_lock(&ftdi_module_lock);
-        list_add_tail(&ftdi->ftdi_list, &ftdi_static_list);
-        ftdi->sequence_num = ++ftdi_instances;
-        mutex_unlock(&ftdi_module_lock);
-        ftdi_elan_init_kref(ftdi);
+	if (!ftdi)
+		return -ENOMEM;
+
+	mutex_lock(&ftdi_module_lock);
+	list_add_tail(&ftdi->ftdi_list, &ftdi_static_list);
+	ftdi->sequence_num = ++ftdi_instances;
+	mutex_unlock(&ftdi_module_lock);
+	ftdi_elan_init_kref(ftdi);
 	sema_init(&ftdi->sw_lock, 1);
-        ftdi->udev = usb_get_dev(interface_to_usbdev(interface));
-        ftdi->interface = interface;
-        mutex_init(&ftdi->u132_lock);
-        ftdi->expected = 4;
-        iface_desc = interface->cur_altsetting;
-        for (i = 0; i < iface_desc->desc.bNumEndpoints; ++i) {
-                endpoint = &iface_desc->endpoint[i].desc;
-                if (!ftdi->bulk_in_endpointAddr &&
+	ftdi->udev = usb_get_dev(interface_to_usbdev(interface));
+	ftdi->interface = interface;
+	mutex_init(&ftdi->u132_lock);
+	ftdi->expected = 4;
+	iface_desc = interface->cur_altsetting;
+	for (i = 0; i < iface_desc->desc.bNumEndpoints; ++i) {
+		endpoint = &iface_desc->endpoint[i].desc;
+		if (!ftdi->bulk_in_endpointAddr &&
 		    usb_endpoint_is_bulk_in(endpoint)) {
-                        buffer_size = usb_endpoint_maxp(endpoint);
-                        ftdi->bulk_in_size = buffer_size;
-                        ftdi->bulk_in_endpointAddr = endpoint->bEndpointAddress;
-                        ftdi->bulk_in_buffer = kmalloc(buffer_size, GFP_KERNEL);
-                        if (!ftdi->bulk_in_buffer) {
-                                dev_err(&ftdi->udev->dev, "Could not allocate b"
-                                        "ulk_in_buffer\n");
-                                retval = -ENOMEM;
-                                goto error;
-                        }
-                }
-                if (!ftdi->bulk_out_endpointAddr &&
+			buffer_size = usb_endpoint_maxp(endpoint);
+			ftdi->bulk_in_size = buffer_size;
+			ftdi->bulk_in_endpointAddr = endpoint->bEndpointAddress;
+			ftdi->bulk_in_buffer = kmalloc(buffer_size, GFP_KERNEL);
+			if (!ftdi->bulk_in_buffer) {
+				dev_err(&ftdi->udev->dev, "Could not allocate bulk_in_buffer\n");
+				retval = -ENOMEM;
+				goto error;
+			}
+		}
+		if (!ftdi->bulk_out_endpointAddr &&
 		    usb_endpoint_is_bulk_out(endpoint)) {
-                        ftdi->bulk_out_endpointAddr =
-                                endpoint->bEndpointAddress;
-                }
-        }
-        if (!(ftdi->bulk_in_endpointAddr && ftdi->bulk_out_endpointAddr)) {
-                dev_err(&ftdi->udev->dev, "Could not find both bulk-in and bulk"
-                        "-out endpoints\n");
-                retval = -ENODEV;
-                goto error;
-        }
-        dev_info(&ftdi->udev->dev, "interface %d has I=%02X O=%02X\n",
-                iface_desc->desc.bInterfaceNumber, ftdi->bulk_in_endpointAddr,
-                ftdi->bulk_out_endpointAddr);
-        usb_set_intfdata(interface, ftdi);
-        if (iface_desc->desc.bInterfaceNumber == 0 &&
-                ftdi->bulk_in_endpointAddr == 0x81 &&
-                ftdi->bulk_out_endpointAddr == 0x02) {
-                retval = usb_register_dev(interface, &ftdi_elan_jtag_class);
-                if (retval) {
-                        dev_err(&ftdi->udev->dev, "Not able to get a minor for "
-                                "this device.\n");
-                        usb_set_intfdata(interface, NULL);
-                        retval = -ENOMEM;
-                        goto error;
-                } else {
-                        ftdi->class = &ftdi_elan_jtag_class;
-                        dev_info(&ftdi->udev->dev, "USB FDTI=%p JTAG interface "
-                                "%d now attached to ftdi%d\n", ftdi,
-                                iface_desc->desc.bInterfaceNumber,
-                                interface->minor);
-                        return 0;
-                }
-        } else if (iface_desc->desc.bInterfaceNumber == 1 &&
-                ftdi->bulk_in_endpointAddr == 0x83 &&
-                ftdi->bulk_out_endpointAddr == 0x04) {
-                ftdi->class = NULL;
-                dev_info(&ftdi->udev->dev, "USB FDTI=%p ELAN interface %d now a"
-                        "ctivated\n", ftdi, iface_desc->desc.bInterfaceNumber);
-                INIT_DELAYED_WORK(&ftdi->status_work, ftdi_elan_status_work);
-                INIT_DELAYED_WORK(&ftdi->command_work, ftdi_elan_command_work);
-                INIT_DELAYED_WORK(&ftdi->respond_work, ftdi_elan_respond_work);
-                ftdi_status_queue_work(ftdi, msecs_to_jiffies(3 *1000));
-                return 0;
-        } else {
-                dev_err(&ftdi->udev->dev,
-                        "Could not find ELAN's U132 device\n");
-                retval = -ENODEV;
-                goto error;
-        }
-      error:if (ftdi) {
-                ftdi_elan_put_kref(ftdi);
-        }
-        return retval;
+			ftdi->bulk_out_endpointAddr =
+				endpoint->bEndpointAddress;
+		}
+	}
+	if (!(ftdi->bulk_in_endpointAddr && ftdi->bulk_out_endpointAddr)) {
+		dev_err(&ftdi->udev->dev, "Could not find both bulk-in and bulk-out endpoints\n");
+		retval = -ENODEV;
+		goto error;
+	}
+	dev_info(&ftdi->udev->dev, "interface %d has I=%02X O=%02X\n",
+		 iface_desc->desc.bInterfaceNumber, ftdi->bulk_in_endpointAddr,
+		 ftdi->bulk_out_endpointAddr);
+	usb_set_intfdata(interface, ftdi);
+	if (iface_desc->desc.bInterfaceNumber == 0 &&
+	    ftdi->bulk_in_endpointAddr == 0x81 &&
+	    ftdi->bulk_out_endpointAddr == 0x02) {
+		retval = usb_register_dev(interface, &ftdi_elan_jtag_class);
+		if (retval) {
+			dev_err(&ftdi->udev->dev, "Not able to get a minor for this device\n");
+			usb_set_intfdata(interface, NULL);
+			retval = -ENOMEM;
+			goto error;
+		} else {
+			ftdi->class = &ftdi_elan_jtag_class;
+			dev_info(&ftdi->udev->dev, "USB FDTI=%p JTAG interface %d now attached to ftdi%d\n",
+				 ftdi, iface_desc->desc.bInterfaceNumber,
+				 interface->minor);
+			return 0;
+		}
+	} else if (iface_desc->desc.bInterfaceNumber == 1 &&
+		   ftdi->bulk_in_endpointAddr == 0x83 &&
+		   ftdi->bulk_out_endpointAddr == 0x04) {
+		ftdi->class = NULL;
+		dev_info(&ftdi->udev->dev, "USB FDTI=%p ELAN interface %d now activated\n",
+			 ftdi, iface_desc->desc.bInterfaceNumber);
+		INIT_DELAYED_WORK(&ftdi->status_work, ftdi_elan_status_work);
+		INIT_DELAYED_WORK(&ftdi->command_work, ftdi_elan_command_work);
+		INIT_DELAYED_WORK(&ftdi->respond_work, ftdi_elan_respond_work);
+		ftdi_status_queue_work(ftdi, msecs_to_jiffies(3 *1000));
+		return 0;
+	} else {
+		dev_err(&ftdi->udev->dev,
+			"Could not find ELAN's U132 device\n");
+		retval = -ENODEV;
+		goto error;
+	}
+error:if (ftdi) {
+		ftdi_elan_put_kref(ftdi);
+	}
+	return retval;
 }
 
 static void ftdi_elan_disconnect(struct usb_interface *interface)
 {
-        struct usb_ftdi *ftdi = usb_get_intfdata(interface);
-        ftdi->disconnected += 1;
-        if (ftdi->class) {
-                int minor = interface->minor;
-                struct usb_class_driver *class = ftdi->class;
-                usb_set_intfdata(interface, NULL);
-                usb_deregister_dev(interface, class);
-                dev_info(&ftdi->udev->dev, "USB FTDI U132 jtag interface on min"
-                        "or %d now disconnected\n", minor);
-        } else {
-                ftdi_status_cancel_work(ftdi);
-                ftdi_command_cancel_work(ftdi);
-                ftdi_response_cancel_work(ftdi);
-                ftdi_elan_abandon_completions(ftdi);
-                ftdi_elan_abandon_targets(ftdi);
-                if (ftdi->registered) {
-                        platform_device_unregister(&ftdi->platform_dev);
-                        ftdi->synchronized = 0;
-                        ftdi->enumerated = 0;
-                        ftdi->initialized = 0;
-                        ftdi->registered = 0;
-                }
-                flush_workqueue(status_queue);
-                flush_workqueue(command_queue);
-                flush_workqueue(respond_queue);
-                ftdi->disconnected += 1;
-                usb_set_intfdata(interface, NULL);
-                dev_info(&ftdi->udev->dev, "USB FTDI U132 host controller inter"
-                        "face now disconnected\n");
-        }
-        ftdi_elan_put_kref(ftdi);
+	struct usb_ftdi *ftdi = usb_get_intfdata(interface);
+	ftdi->disconnected += 1;
+	if (ftdi->class) {
+		int minor = interface->minor;
+		struct usb_class_driver *class = ftdi->class;
+		usb_set_intfdata(interface, NULL);
+		usb_deregister_dev(interface, class);
+		dev_info(&ftdi->udev->dev, "USB FTDI U132 jtag interface on minor %d now disconnected\n",
+			 minor);
+	} else {
+		ftdi_status_cancel_work(ftdi);
+		ftdi_command_cancel_work(ftdi);
+		ftdi_response_cancel_work(ftdi);
+		ftdi_elan_abandon_completions(ftdi);
+		ftdi_elan_abandon_targets(ftdi);
+		if (ftdi->registered) {
+			platform_device_unregister(&ftdi->platform_dev);
+			ftdi->synchronized = 0;
+			ftdi->enumerated = 0;
+			ftdi->initialized = 0;
+			ftdi->registered = 0;
+		}
+		flush_workqueue(status_queue);
+		flush_workqueue(command_queue);
+		flush_workqueue(respond_queue);
+		ftdi->disconnected += 1;
+		usb_set_intfdata(interface, NULL);
+		dev_info(&ftdi->udev->dev, "USB FTDI U132 host controller interface now disconnected\n");
+	}
+	ftdi_elan_put_kref(ftdi);
 }
 
 static struct usb_driver ftdi_elan_driver = {
-        .name = "ftdi-elan",
-        .probe = ftdi_elan_probe,
-        .disconnect = ftdi_elan_disconnect,
-        .id_table = ftdi_elan_table,
+	.name = "ftdi-elan",
+	.probe = ftdi_elan_probe,
+	.disconnect = ftdi_elan_disconnect,
+	.id_table = ftdi_elan_table,
 };
 static int __init ftdi_elan_init(void)
 {
-        int result;
-        printk(KERN_INFO "driver %s\n", ftdi_elan_driver.name);
-        mutex_init(&ftdi_module_lock);
-        INIT_LIST_HEAD(&ftdi_static_list);
-        status_queue = create_singlethread_workqueue("ftdi-status-control");
+	int result;
+	pr_info("driver %s\n", ftdi_elan_driver.name);
+	mutex_init(&ftdi_module_lock);
+	INIT_LIST_HEAD(&ftdi_static_list);
+	status_queue = create_singlethread_workqueue("ftdi-status-control");
 	if (!status_queue)
 		goto err_status_queue;
-        command_queue = create_singlethread_workqueue("ftdi-command-engine");
+	command_queue = create_singlethread_workqueue("ftdi-command-engine");
 	if (!command_queue)
 		goto err_command_queue;
-        respond_queue = create_singlethread_workqueue("ftdi-respond-engine");
+	respond_queue = create_singlethread_workqueue("ftdi-respond-engine");
 	if (!respond_queue)
 		goto err_respond_queue;
-        result = usb_register(&ftdi_elan_driver);
-        if (result) {
+	result = usb_register(&ftdi_elan_driver);
+	if (result) {
 		destroy_workqueue(status_queue);
 		destroy_workqueue(command_queue);
 		destroy_workqueue(respond_queue);
-                printk(KERN_ERR "usb_register failed. Error number %d\n",
-		       result);
+		pr_err("usb_register failed. Error number %d\n", result);
 	}
-        return result;
+	return result;
 
- err_respond_queue:
+err_respond_queue:
 	destroy_workqueue(command_queue);
- err_command_queue:
+err_command_queue:
 	destroy_workqueue(status_queue);
- err_status_queue:
-	printk(KERN_ERR "%s couldn't create workqueue\n", ftdi_elan_driver.name);
+err_status_queue:
+	pr_err("%s couldn't create workqueue\n", ftdi_elan_driver.name);
 	return -ENOMEM;
 }
 
 static void __exit ftdi_elan_exit(void)
 {
-        struct usb_ftdi *ftdi;
-        struct usb_ftdi *temp;
-        usb_deregister(&ftdi_elan_driver);
-        printk(KERN_INFO "ftdi_u132 driver deregistered\n");
-        list_for_each_entry_safe(ftdi, temp, &ftdi_static_list, ftdi_list) {
-                ftdi_status_cancel_work(ftdi);
-                ftdi_command_cancel_work(ftdi);
-                ftdi_response_cancel_work(ftdi);
-        } flush_workqueue(status_queue);
-        destroy_workqueue(status_queue);
-        status_queue = NULL;
-        flush_workqueue(command_queue);
-        destroy_workqueue(command_queue);
-        command_queue = NULL;
-        flush_workqueue(respond_queue);
-        destroy_workqueue(respond_queue);
-        respond_queue = NULL;
+	struct usb_ftdi *ftdi;
+	struct usb_ftdi *temp;
+	usb_deregister(&ftdi_elan_driver);
+	pr_info("ftdi_u132 driver deregistered\n");
+	list_for_each_entry_safe(ftdi, temp, &ftdi_static_list, ftdi_list) {
+		ftdi_status_cancel_work(ftdi);
+		ftdi_command_cancel_work(ftdi);
+		ftdi_response_cancel_work(ftdi);
+	} flush_workqueue(status_queue);
+	destroy_workqueue(status_queue);
+	status_queue = NULL;
+	flush_workqueue(command_queue);
+	destroy_workqueue(command_queue);
+	command_queue = NULL;
+	flush_workqueue(respond_queue);
+	destroy_workqueue(respond_queue);
+	respond_queue = NULL;
 }
 
 
diff --git a/drivers/usb/misc/iowarrior.c b/drivers/usb/misc/iowarrior.c
index 20bcfdd7eac..c6bfd13f6c9 100644
--- a/drivers/usb/misc/iowarrior.c
+++ b/drivers/usb/misc/iowarrior.c
@@ -51,19 +51,12 @@
 */
 #define MAX_WRITES_IN_FLIGHT 4
 
-/* Use our own dbg macro */
-#undef dbg
-#define dbg( format, arg... ) do { if( debug ) printk( KERN_DEBUG __FILE__ ": " format "\n" , ## arg ); } while ( 0 )
-
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_LICENSE("GPL");
 
 /* Module parameters */
 static DEFINE_MUTEX(iowarrior_mutex);
-static bool debug = 0;
-module_param(debug, bool, 0644);
-MODULE_PARM_DESC(debug, "debug=1 enables debugging messages");
 
 static struct usb_driver iowarrior_driver;
 static DEFINE_MUTEX(iowarrior_open_disc_lock);
@@ -235,8 +228,8 @@ static void iowarrior_write_callback(struct urb *urb)
 	if (status &&
 	    !(status == -ENOENT ||
 	      status == -ECONNRESET || status == -ESHUTDOWN)) {
-		dbg("%s - nonzero write bulk status received: %d",
-		    __func__, status);
+		dev_dbg(&dev->interface->dev,
+			"nonzero write bulk status received: %d\n", status);
 	}
 	/* free up our allocated buffer */
 	usb_free_coherent(urb->dev, urb->transfer_buffer_length,
@@ -251,7 +244,7 @@ static void iowarrior_write_callback(struct urb *urb)
  */
 static inline void iowarrior_delete(struct iowarrior *dev)
 {
-	dbg("%s - minor %d", __func__, dev->minor);
+	dev_dbg(&dev->interface->dev, "minor %d\n", dev->minor);
 	kfree(dev->int_in_buffer);
 	usb_free_urb(dev->int_in_urb);
 	kfree(dev->read_queue);
@@ -288,7 +281,8 @@ static ssize_t iowarrior_read(struct file *file, char __user *buffer,
 	if (dev == NULL || !dev->present)
 		return -ENODEV;
 
-	dbg("%s - minor %d, count = %zd", __func__, dev->minor, count);
+	dev_dbg(&dev->interface->dev, "minor %d, count = %zd\n",
+		dev->minor, count);
 
 	/* read count must be packet size (+ time stamp) */
 	if ((count != dev->report_size)
@@ -356,7 +350,8 @@ static ssize_t iowarrior_write(struct file *file,
 		retval = -ENODEV;
 		goto exit;
 	}
-	dbg("%s - minor %d, count = %zd", __func__, dev->minor, count);
+	dev_dbg(&dev->interface->dev, "minor %d, count = %zd\n",
+		dev->minor, count);
 	/* if count is 0 we're already done */
 	if (count == 0) {
 		retval = 0;
@@ -418,14 +413,16 @@ static ssize_t iowarrior_write(struct file *file,
 		int_out_urb = usb_alloc_urb(0, GFP_KERNEL);
 		if (!int_out_urb) {
 			retval = -ENOMEM;
-			dbg("%s Unable to allocate urb ", __func__);
+			dev_dbg(&dev->interface->dev,
+				"Unable to allocate urb\n");
 			goto error_no_urb;
 		}
 		buf = usb_alloc_coherent(dev->udev, dev->report_size,
 					 GFP_KERNEL, &int_out_urb->transfer_dma);
 		if (!buf) {
 			retval = -ENOMEM;
-			dbg("%s Unable to allocate buffer ", __func__);
+			dev_dbg(&dev->interface->dev,
+				"Unable to allocate buffer\n");
 			goto error_no_buffer;
 		}
 		usb_fill_int_urb(int_out_urb, dev->udev,
@@ -441,8 +438,9 @@ static ssize_t iowarrior_write(struct file *file,
 		}
 		retval = usb_submit_urb(int_out_urb, GFP_KERNEL);
 		if (retval) {
-			dbg("%s submit error %d for urb nr.%d", __func__,
-			    retval, atomic_read(&dev->write_busy));
+			dev_dbg(&dev->interface->dev,
+				"submit error %d for urb nr.%d\n",
+				retval, atomic_read(&dev->write_busy));
 			goto error;
 		}
 		/* submit was ok */
@@ -502,8 +500,8 @@ static long iowarrior_ioctl(struct file *file, unsigned int cmd,
 		goto error_out;
 	}
 
-	dbg("%s - minor %d, cmd 0x%.4x, arg %ld", __func__, dev->minor, cmd,
-	    arg);
+	dev_dbg(&dev->interface->dev, "minor %d, cmd 0x%.4x, arg %ld\n",
+		dev->minor, cmd, arg);
 
 	retval = 0;
 	io_res = 0;
@@ -601,8 +599,6 @@ static int iowarrior_open(struct inode *inode, struct file *file)
 	int subminor;
 	int retval = 0;
 
-	dbg("%s", __func__);
-
 	mutex_lock(&iowarrior_mutex);
 	subminor = iminor(inode);
 
@@ -662,7 +658,7 @@ static int iowarrior_release(struct inode *inode, struct file *file)
 		return -ENODEV;
 	}
 
-	dbg("%s - minor %d", __func__, dev->minor);
+	dev_dbg(&dev->interface->dev, "minor %d\n", dev->minor);
 
 	/* lock our device */
 	mutex_lock(&dev->mutex);
diff --git a/drivers/usb/misc/usb3503.c b/drivers/usb/misc/usb3503.c
index a31641e18d1..f43c61989ce 100644
--- a/drivers/usb/misc/usb3503.c
+++ b/drivers/usb/misc/usb3503.c
@@ -18,6 +18,7 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  */
 
+#include <linux/clk.h>
 #include <linux/i2c.h>
 #include <linux/gpio.h>
 #include <linux/delay.h>
@@ -57,10 +58,12 @@ struct usb3503 {
 	enum usb3503_mode	mode;
 	struct regmap		*regmap;
 	struct device		*dev;
+	struct clk		*clk;
 	u8	port_off_mask;
 	int	gpio_intn;
 	int	gpio_reset;
 	int	gpio_connect;
+	bool	secondary_ref_clk;
 };
 
 static int usb3503_reset(struct usb3503 *hub, int state)
@@ -184,8 +187,58 @@ static int usb3503_probe(struct usb3503 *hub)
 		hub->gpio_reset		= pdata->gpio_reset;
 		hub->mode		= pdata->initial_mode;
 	} else if (np) {
+		struct clk *clk;
 		hub->port_off_mask = 0;
 
+		clk = devm_clk_get(dev, "refclk");
+		if (IS_ERR(clk) && PTR_ERR(clk) != -ENOENT) {
+			dev_err(dev, "unable to request refclk (%d)\n", err);
+			return PTR_ERR(clk);
+		}
+
+		if (!IS_ERR(clk)) {
+			u32 rate = 0;
+			hub->clk = clk;
+
+			if (!of_property_read_u32(np, "refclk-frequency",
+						 &rate)) {
+
+				switch (rate) {
+				case 38400000:
+				case 26000000:
+				case 19200000:
+				case 12000000:
+					hub->secondary_ref_clk = 0;
+					break;
+				case 24000000:
+				case 27000000:
+				case 25000000:
+				case 50000000:
+					hub->secondary_ref_clk = 1;
+					break;
+				default:
+					dev_err(dev,
+						"unsupported reference clock rate (%d)\n",
+						(int) rate);
+					return -EINVAL;
+				}
+				err = clk_set_rate(hub->clk, rate);
+				if (err) {
+					dev_err(dev,
+						"unable to set reference clock rate to %d\n",
+						(int) rate);
+					return err;
+				}
+			}
+
+			err = clk_prepare_enable(hub->clk);
+			if (err) {
+				dev_err(dev,
+					"unable to enable reference clock\n");
+				return err;
+			}
+		}
+
 		property = of_get_property(np, "disabled-ports", &len);
 		if (property && (len / sizeof(u32)) > 0) {
 			int i;
@@ -213,8 +266,10 @@ static int usb3503_probe(struct usb3503 *hub)
 		dev_err(dev, "Ports disabled with no control interface\n");
 
 	if (gpio_is_valid(hub->gpio_intn)) {
-		err = devm_gpio_request_one(dev, hub->gpio_intn,
-				GPIOF_OUT_INIT_HIGH, "usb3503 intn");
+		int val = hub->secondary_ref_clk ? GPIOF_OUT_INIT_LOW :
+						   GPIOF_OUT_INIT_HIGH;
+		err = devm_gpio_request_one(dev, hub->gpio_intn, val,
+					    "usb3503 intn");
 		if (err) {
 			dev_err(dev,
 				"unable to request GPIO %d as connect pin (%d)\n",
diff --git a/drivers/usb/misc/usbtest.c b/drivers/usb/misc/usbtest.c
index f6568b5e9b0..51a6da25677 100644
--- a/drivers/usb/misc/usbtest.c
+++ b/drivers/usb/misc/usbtest.c
@@ -1320,6 +1320,11 @@ static int unlink1(struct usbtest_dev *dev, int pipe, int size, int async)
 	urb->context = &completion;
 	urb->complete = unlink1_callback;
 
+	if (usb_pipeout(urb->pipe)) {
+		simple_fill_buf(urb);
+		urb->transfer_flags |= URB_ZERO_PACKET;
+	}
+
 	/* keep the endpoint busy.  there are lots of hc/hcd-internal
 	 * states, and testing should get to all of them over time.
 	 *
@@ -1340,6 +1345,9 @@ static int unlink1(struct usbtest_dev *dev, int pipe, int size, int async)
 		while (!completion_done(&completion)) {
 			retval = usb_unlink_urb(urb);
 
+			if (retval == 0 && usb_pipein(urb->pipe))
+				retval = simple_check_buf(dev, urb);
+
 			switch (retval) {
 			case -EBUSY:
 			case -EIDRM:
@@ -1450,6 +1458,11 @@ static int unlink_queued(struct usbtest_dev *dev, int pipe, unsigned num,
 				unlink_queued_callback, &ctx);
 		ctx.urbs[i]->transfer_dma = buf_dma;
 		ctx.urbs[i]->transfer_flags = URB_NO_TRANSFER_DMA_MAP;
+
+		if (usb_pipeout(ctx.urbs[i]->pipe)) {
+			simple_fill_buf(ctx.urbs[i]);
+			ctx.urbs[i]->transfer_flags |= URB_ZERO_PACKET;
+		}
 	}
 
 	/* Submit all the URBs and then unlink URBs num - 4 and num - 2. */
diff --git a/drivers/usb/misc/yurex.c b/drivers/usb/misc/yurex.c
index 24278208bf7..1472805083d 100644
--- a/drivers/usb/misc/yurex.c
+++ b/drivers/usb/misc/yurex.c
@@ -296,6 +296,7 @@ static int yurex_probe(struct usb_interface *interface, const struct usb_device_
 
 	/* save our data pointer in this interface device */
 	usb_set_intfdata(interface, dev);
+	dev->bbu = -1;
 
 	/* we can register the device now, as it is ready */
 	retval = usb_register_dev(interface, &yurex_class);
@@ -306,8 +307,6 @@ static int yurex_probe(struct usb_interface *interface, const struct usb_device_
 		goto error;
 	}
 
-	dev->bbu = -1;
-
 	dev_info(&interface->dev,
 		 "USB YUREX device now attached to Yurex #%d\n",
 		 interface->minor);
diff --git a/drivers/usb/musb/Kconfig b/drivers/usb/musb/Kconfig
index 8b789792f6f..06cc5d6ea68 100644
--- a/drivers/usb/musb/Kconfig
+++ b/drivers/usb/musb/Kconfig
@@ -76,7 +76,7 @@ config USB_MUSB_TUSB6010
 
 config USB_MUSB_OMAP2PLUS
 	tristate "OMAP2430 and onwards"
-	depends on ARCH_OMAP2PLUS
+	depends on ARCH_OMAP2PLUS && USB
 	select GENERIC_PHY
 
 config USB_MUSB_AM35X
@@ -141,10 +141,11 @@ config USB_TI_CPPI_DMA
 config USB_TI_CPPI41_DMA
 	bool 'TI CPPI 4.1 (AM335x)'
 	depends on ARCH_OMAP
+	select TI_CPPI41
 
 config USB_TUSB_OMAP_DMA
 	bool 'TUSB 6010'
-	depends on USB_MUSB_TUSB6010
+	depends on USB_MUSB_TUSB6010 = USB_MUSB_HDRC # both built-in or both modules
 	depends on ARCH_OMAP
 	help
 	  Enable DMA transfers on TUSB 6010 when OMAP DMA is available.
diff --git a/drivers/usb/musb/am35x.c b/drivers/usb/musb/am35x.c
index b3aa0184af9..0a34dd85955 100644
--- a/drivers/usb/musb/am35x.c
+++ b/drivers/usb/musb/am35x.c
@@ -32,7 +32,7 @@
 #include <linux/io.h>
 #include <linux/platform_device.h>
 #include <linux/dma-mapping.h>
-#include <linux/usb/usb_phy_gen_xceiv.h>
+#include <linux/usb/usb_phy_generic.h>
 #include <linux/platform_data/usb-omap.h>
 
 #include "musb_core.h"
@@ -85,6 +85,7 @@
 struct am35x_glue {
 	struct device		*dev;
 	struct platform_device	*musb;
+	struct platform_device	*phy;
 	struct clk		*phy_clk;
 	struct clk		*clk;
 };
@@ -360,7 +361,6 @@ static int am35x_musb_init(struct musb *musb)
 	if (!rev)
 		return -ENODEV;
 
-	usb_nop_xceiv_register();
 	musb->xceiv = usb_get_phy(USB_PHY_TYPE_USB2);
 	if (IS_ERR_OR_NULL(musb->xceiv))
 		return -EPROBE_DEFER;
@@ -402,7 +402,6 @@ static int am35x_musb_exit(struct musb *musb)
 		data->set_phy_power(0);
 
 	usb_put_phy(musb->xceiv);
-	usb_nop_xceiv_unregister();
 
 	return 0;
 }
@@ -505,6 +504,9 @@ static int am35x_probe(struct platform_device *pdev)
 
 	pdata->platform_ops		= &am35x_ops;
 
+	glue->phy = usb_phy_generic_register();
+	if (IS_ERR(glue->phy))
+		goto err7;
 	platform_set_drvdata(pdev, glue);
 
 	pinfo = am35x_dev_info;
@@ -518,11 +520,14 @@ static int am35x_probe(struct platform_device *pdev)
 	if (IS_ERR(musb)) {
 		ret = PTR_ERR(musb);
 		dev_err(&pdev->dev, "failed to register musb device: %d\n", ret);
-		goto err7;
+		goto err8;
 	}
 
 	return 0;
 
+err8:
+	usb_phy_generic_unregister(glue->phy);
+
 err7:
 	clk_disable(clk);
 
@@ -547,6 +552,7 @@ static int am35x_remove(struct platform_device *pdev)
 	struct am35x_glue	*glue = platform_get_drvdata(pdev);
 
 	platform_device_unregister(glue->musb);
+	usb_phy_generic_unregister(glue->phy);
 	clk_disable(glue->clk);
 	clk_disable(glue->phy_clk);
 	clk_put(glue->clk);
diff --git a/drivers/usb/musb/blackfin.c b/drivers/usb/musb/blackfin.c
index 796677fa9a1..d40d5f0b552 100644
--- a/drivers/usb/musb/blackfin.c
+++ b/drivers/usb/musb/blackfin.c
@@ -18,7 +18,7 @@
 #include <linux/platform_device.h>
 #include <linux/dma-mapping.h>
 #include <linux/prefetch.h>
-#include <linux/usb/usb_phy_gen_xceiv.h>
+#include <linux/usb/usb_phy_generic.h>
 
 #include <asm/cacheflush.h>
 
@@ -29,6 +29,7 @@
 struct bfin_glue {
 	struct device		*dev;
 	struct platform_device	*musb;
+	struct platform_device	*phy;
 };
 #define glue_to_musb(g)		platform_get_drvdata(g->musb)
 
@@ -401,7 +402,6 @@ static int bfin_musb_init(struct musb *musb)
 	}
 	gpio_direction_output(musb->config->gpio_vrsel, 0);
 
-	usb_nop_xceiv_register();
 	musb->xceiv = usb_get_phy(USB_PHY_TYPE_USB2);
 	if (IS_ERR_OR_NULL(musb->xceiv)) {
 		gpio_free(musb->config->gpio_vrsel);
@@ -424,9 +424,8 @@ static int bfin_musb_init(struct musb *musb)
 static int bfin_musb_exit(struct musb *musb)
 {
 	gpio_free(musb->config->gpio_vrsel);
-
 	usb_put_phy(musb->xceiv);
-	usb_nop_xceiv_unregister();
+
 	return 0;
 }
 
@@ -477,6 +476,9 @@ static int bfin_probe(struct platform_device *pdev)
 
 	pdata->platform_ops		= &bfin_ops;
 
+	glue->phy = usb_phy_generic_register();
+	if (IS_ERR(glue->phy))
+		goto err2;
 	platform_set_drvdata(pdev, glue);
 
 	memset(musb_resources, 0x00, sizeof(*musb_resources) *
@@ -514,6 +516,9 @@ static int bfin_probe(struct platform_device *pdev)
 	return 0;
 
 err3:
+	usb_phy_generic_unregister(glue->phy);
+
+err2:
 	platform_device_put(musb);
 
 err1:
@@ -528,6 +533,7 @@ static int bfin_remove(struct platform_device *pdev)
 	struct bfin_glue		*glue = platform_get_drvdata(pdev);
 
 	platform_device_unregister(glue->musb);
+	usb_phy_generic_unregister(glue->phy);
 	kfree(glue);
 
 	return 0;
diff --git a/drivers/usb/musb/da8xx.c b/drivers/usb/musb/da8xx.c
index e3486de7199..058775e647a 100644
--- a/drivers/usb/musb/da8xx.c
+++ b/drivers/usb/musb/da8xx.c
@@ -32,7 +32,7 @@
 #include <linux/io.h>
 #include <linux/platform_device.h>
 #include <linux/dma-mapping.h>
-#include <linux/usb/usb_phy_gen_xceiv.h>
+#include <linux/usb/usb_phy_generic.h>
 
 #include <mach/da8xx.h>
 #include <linux/platform_data/usb-davinci.h>
@@ -85,6 +85,7 @@
 struct da8xx_glue {
 	struct device		*dev;
 	struct platform_device	*musb;
+	struct platform_device	*phy;
 	struct clk		*clk;
 };
 
@@ -418,7 +419,6 @@ static int da8xx_musb_init(struct musb *musb)
 	if (!rev)
 		goto fail;
 
-	usb_nop_xceiv_register();
 	musb->xceiv = usb_get_phy(USB_PHY_TYPE_USB2);
 	if (IS_ERR_OR_NULL(musb->xceiv)) {
 		ret = -EPROBE_DEFER;
@@ -453,7 +453,6 @@ static int da8xx_musb_exit(struct musb *musb)
 	phy_off();
 
 	usb_put_phy(musb->xceiv);
-	usb_nop_xceiv_unregister();
 
 	return 0;
 }
@@ -512,6 +511,11 @@ static int da8xx_probe(struct platform_device *pdev)
 
 	pdata->platform_ops		= &da8xx_ops;
 
+	glue->phy = usb_phy_generic_register();
+	if (IS_ERR(glue->phy)) {
+		ret = PTR_ERR(glue->phy);
+		goto err5;
+	}
 	platform_set_drvdata(pdev, glue);
 
 	memset(musb_resources, 0x00, sizeof(*musb_resources) *
@@ -538,11 +542,14 @@ static int da8xx_probe(struct platform_device *pdev)
 	if (IS_ERR(musb)) {
 		ret = PTR_ERR(musb);
 		dev_err(&pdev->dev, "failed to register musb device: %d\n", ret);
-		goto err5;
+		goto err6;
 	}
 
 	return 0;
 
+err6:
+	usb_phy_generic_unregister(glue->phy);
+
 err5:
 	clk_disable(clk);
 
@@ -561,6 +568,7 @@ static int da8xx_remove(struct platform_device *pdev)
 	struct da8xx_glue		*glue = platform_get_drvdata(pdev);
 
 	platform_device_unregister(glue->musb);
+	usb_phy_generic_unregister(glue->phy);
 	clk_disable(glue->clk);
 	clk_put(glue->clk);
 	kfree(glue);
diff --git a/drivers/usb/musb/davinci.c b/drivers/usb/musb/davinci.c
index c259dac9d05..de8492b06e4 100644
--- a/drivers/usb/musb/davinci.c
+++ b/drivers/usb/musb/davinci.c
@@ -32,7 +32,7 @@
 #include <linux/gpio.h>
 #include <linux/platform_device.h>
 #include <linux/dma-mapping.h>
-#include <linux/usb/usb_phy_gen_xceiv.h>
+#include <linux/usb/usb_phy_generic.h>
 
 #include <mach/cputype.h>
 #include <mach/hardware.h>
@@ -381,7 +381,6 @@ static int davinci_musb_init(struct musb *musb)
 	u32		revision;
 	int 		ret = -ENODEV;
 
-	usb_nop_xceiv_register();
 	musb->xceiv = usb_get_phy(USB_PHY_TYPE_USB2);
 	if (IS_ERR_OR_NULL(musb->xceiv)) {
 		ret = -EPROBE_DEFER;
@@ -439,7 +438,7 @@ static int davinci_musb_init(struct musb *musb)
 fail:
 	usb_put_phy(musb->xceiv);
 unregister:
-	usb_nop_xceiv_unregister();
+	usb_phy_generic_unregister();
 	return ret;
 }
 
@@ -487,7 +486,6 @@ static int davinci_musb_exit(struct musb *musb)
 	phy_off();
 
 	usb_put_phy(musb->xceiv);
-	usb_nop_xceiv_unregister();
 
 	return 0;
 }
@@ -545,6 +543,7 @@ static int davinci_probe(struct platform_device *pdev)
 
 	pdata->platform_ops		= &davinci_ops;
 
+	usb_phy_generic_register();
 	platform_set_drvdata(pdev, glue);
 
 	memset(musb_resources, 0x00, sizeof(*musb_resources) *
@@ -603,6 +602,7 @@ static int davinci_remove(struct platform_device *pdev)
 	struct davinci_glue		*glue = platform_get_drvdata(pdev);
 
 	platform_device_unregister(glue->musb);
+	usb_phy_generic_unregister();
 	clk_disable(glue->clk);
 	clk_put(glue->clk);
 	kfree(glue);
diff --git a/drivers/usb/musb/musb_core.c b/drivers/usb/musb/musb_core.c
index 07576907e2c..61da471b7ae 100644
--- a/drivers/usb/musb/musb_core.c
+++ b/drivers/usb/musb/musb_core.c
@@ -848,6 +848,10 @@ b_host:
 		}
 	}
 
+	/* handle babble condition */
+	if (int_usb & MUSB_INTR_BABBLE)
+		schedule_work(&musb->recover_work);
+
 #if 0
 /* REVISIT ... this would be for multiplexing periodic endpoints, or
  * supporting transfer phasing to prevent exceeding ISO bandwidth
@@ -1746,6 +1750,34 @@ static void musb_irq_work(struct work_struct *data)
 	}
 }
 
+/* Recover from babble interrupt conditions */
+static void musb_recover_work(struct work_struct *data)
+{
+	struct musb *musb = container_of(data, struct musb, recover_work);
+	int status;
+
+	musb_platform_reset(musb);
+
+	usb_phy_vbus_off(musb->xceiv);
+	udelay(100);
+
+	usb_phy_vbus_on(musb->xceiv);
+	udelay(100);
+
+	/*
+	 * When a babble condition occurs, the musb controller removes the
+	 * session bit and the endpoint config is lost.
+	 */
+	if (musb->dyn_fifo)
+		status = ep_config_from_table(musb);
+	else
+		status = ep_config_from_hw(musb);
+
+	/* start the session again */
+	if (status == 0)
+		musb_start(musb);
+}
+
 /* --------------------------------------------------------------------------
  * Init support
  */
@@ -1913,6 +1945,7 @@ musb_init_controller(struct device *dev, int nIrq, void __iomem *ctrl)
 
 	/* Init IRQ workqueue before request_irq */
 	INIT_WORK(&musb->irq_work, musb_irq_work);
+	INIT_WORK(&musb->recover_work, musb_recover_work);
 	INIT_DELAYED_WORK(&musb->deassert_reset_work, musb_deassert_reset);
 	INIT_DELAYED_WORK(&musb->finish_resume_work, musb_host_finish_resume);
 
@@ -2008,6 +2041,7 @@ fail4:
 
 fail3:
 	cancel_work_sync(&musb->irq_work);
+	cancel_work_sync(&musb->recover_work);
 	cancel_delayed_work_sync(&musb->finish_resume_work);
 	cancel_delayed_work_sync(&musb->deassert_reset_work);
 	if (musb->dma_controller)
@@ -2073,6 +2107,7 @@ static int musb_remove(struct platform_device *pdev)
 		dma_controller_destroy(musb->dma_controller);
 
 	cancel_work_sync(&musb->irq_work);
+	cancel_work_sync(&musb->recover_work);
 	cancel_delayed_work_sync(&musb->finish_resume_work);
 	cancel_delayed_work_sync(&musb->deassert_reset_work);
 	musb_free(musb);
diff --git a/drivers/usb/musb/musb_core.h b/drivers/usb/musb/musb_core.h
index 7083e82776f..d155a156f24 100644
--- a/drivers/usb/musb/musb_core.h
+++ b/drivers/usb/musb/musb_core.h
@@ -192,6 +192,7 @@ struct musb_platform_ops {
 
 	int	(*set_mode)(struct musb *musb, u8 mode);
 	void	(*try_idle)(struct musb *musb, unsigned long timeout);
+	void	(*reset)(struct musb *musb);
 
 	int	(*vbus_status)(struct musb *musb);
 	void	(*set_vbus)(struct musb *musb, int on);
@@ -296,6 +297,7 @@ struct musb {
 
 	irqreturn_t		(*isr)(int, void *);
 	struct work_struct	irq_work;
+	struct work_struct	recover_work;
 	struct delayed_work	deassert_reset_work;
 	struct delayed_work	finish_resume_work;
 	u16			hwvers;
@@ -337,6 +339,7 @@ struct musb {
 	dma_addr_t		async;
 	dma_addr_t		sync;
 	void __iomem		*sync_va;
+	u8			tusb_revision;
 #endif
 
 	/* passed down from chip/board specific irq handlers */
@@ -552,6 +555,12 @@ static inline void musb_platform_try_idle(struct musb *musb,
 		musb->ops->try_idle(musb, timeout);
 }
 
+static inline void musb_platform_reset(struct musb *musb)
+{
+	if (musb->ops->reset)
+		musb->ops->reset(musb);
+}
+
 static inline int musb_platform_get_vbus_status(struct musb *musb)
 {
 	if (!musb->ops->vbus_status)
diff --git a/drivers/usb/musb/musb_dsps.c b/drivers/usb/musb/musb_dsps.c
index 3372ded5def..51beb13c7e1 100644
--- a/drivers/usb/musb/musb_dsps.c
+++ b/drivers/usb/musb/musb_dsps.c
@@ -35,7 +35,7 @@
 #include <linux/dma-mapping.h>
 #include <linux/pm_runtime.h>
 #include <linux/module.h>
-#include <linux/usb/usb_phy_gen_xceiv.h>
+#include <linux/usb/usb_phy_generic.h>
 #include <linux/platform_data/usb-omap.h>
 #include <linux/sizes.h>
 
@@ -329,9 +329,21 @@ 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 (is_host_active(musb) && usbintr & MUSB_INTR_BABBLE)
+	if (is_host_active(musb) && usbintr & MUSB_INTR_BABBLE) {
 		pr_info("CAUTION: musb: Babble Interrupt Occurred\n");
 
+		/*
+		 * When a babble condition occurs, the musb controller removes
+		 * the session and is no longer in host mode. Hence, all
+		 * devices connected to its root hub get disconnected.
+		 *
+		 * Hand this error down to the musb core isr, so it can
+		 * recover.
+		 */
+		musb->int_usb = MUSB_INTR_BABBLE | MUSB_INTR_DISCONNECT;
+		musb->int_tx = musb->int_rx = 0;
+	}
+
 	if (usbintr & ((1 << wrp->drvvbus) << wrp->usb_shift)) {
 		int drvvbus = dsps_readl(reg_base, wrp->status);
 		void __iomem *mregs = musb->mregs;
@@ -470,8 +482,9 @@ static int dsps_musb_exit(struct musb *musb)
 	struct dsps_glue *glue = dev_get_drvdata(dev->parent);
 
 	del_timer_sync(&glue->timer);
-
 	usb_phy_shutdown(musb->xceiv);
+	debugfs_remove_recursive(glue->dbgfs_root);
+
 	return 0;
 }
 
@@ -523,6 +536,16 @@ static int dsps_musb_set_mode(struct musb *musb, u8 mode)
 	return 0;
 }
 
+static void dsps_musb_reset(struct musb *musb)
+{
+	struct device *dev = musb->controller;
+	struct dsps_glue *glue = dev_get_drvdata(dev->parent);
+	const struct dsps_musb_wrapper *wrp = glue->wrp;
+
+	dsps_writel(musb->ctrl_base, wrp->control, (1 << wrp->reset));
+	udelay(100);
+}
+
 static struct musb_platform_ops dsps_ops = {
 	.init		= dsps_musb_init,
 	.exit		= dsps_musb_exit,
@@ -532,6 +555,7 @@ static struct musb_platform_ops dsps_ops = {
 
 	.try_idle	= dsps_musb_try_idle,
 	.set_mode	= dsps_musb_set_mode,
+	.reset		= dsps_musb_reset,
 };
 
 static u64 musb_dmamask = DMA_BIT_MASK(32);
@@ -708,8 +732,6 @@ static int dsps_remove(struct platform_device *pdev)
 	pm_runtime_put(&pdev->dev);
 	pm_runtime_disable(&pdev->dev);
 
-	debugfs_remove_recursive(glue->dbgfs_root);
-
 	return 0;
 }
 
@@ -751,7 +773,7 @@ static const struct of_device_id musb_dsps_of_match[] = {
 };
 MODULE_DEVICE_TABLE(of, musb_dsps_of_match);
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int dsps_suspend(struct device *dev)
 {
 	struct dsps_glue *glue = dev_get_drvdata(dev);
diff --git a/drivers/usb/musb/omap2430.c b/drivers/usb/musb/omap2430.c
index d341c149a2f..d369bf1f393 100644
--- a/drivers/usb/musb/omap2430.c
+++ b/drivers/usb/musb/omap2430.c
@@ -316,7 +316,13 @@ static void omap_musb_mailbox_work(struct work_struct *mailbox_work)
 {
 	struct omap2430_glue *glue = container_of(mailbox_work,
 				struct omap2430_glue, omap_musb_mailbox_work);
+	struct musb *musb = glue_to_musb(glue);
+	struct device *dev = musb->controller;
+
+	pm_runtime_get_sync(dev);
 	omap_musb_set_mailbox(glue);
+	pm_runtime_mark_last_busy(dev);
+	pm_runtime_put_autosuspend(dev);
 }
 
 static irqreturn_t omap2430_musb_interrupt(int irq, void *__hci)
@@ -416,6 +422,7 @@ static int omap2430_musb_init(struct musb *musb)
 		omap_musb_set_mailbox(glue);
 
 	phy_init(musb->phy);
+	phy_power_on(musb->phy);
 
 	pm_runtime_put_noidle(musb->controller);
 	return 0;
@@ -478,6 +485,7 @@ static int omap2430_musb_exit(struct musb *musb)
 	del_timer_sync(&musb_idle_timer);
 
 	omap2430_low_level_exit(musb);
+	phy_power_off(musb->phy);
 	phy_exit(musb->phy);
 
 	return 0;
diff --git a/drivers/usb/musb/tusb6010.c b/drivers/usb/musb/tusb6010.c
index 4e9fb1d0869..159ef4be1ef 100644
--- a/drivers/usb/musb/tusb6010.c
+++ b/drivers/usb/musb/tusb6010.c
@@ -24,13 +24,14 @@
 #include <linux/io.h>
 #include <linux/platform_device.h>
 #include <linux/dma-mapping.h>
-#include <linux/usb/usb_phy_gen_xceiv.h>
+#include <linux/usb/usb_phy_generic.h>
 
 #include "musb_core.h"
 
 struct tusb6010_glue {
 	struct device		*dev;
 	struct platform_device	*musb;
+	struct platform_device	*phy;
 };
 
 static void tusb_musb_set_vbus(struct musb *musb, int is_on);
@@ -42,7 +43,7 @@ static void tusb_musb_set_vbus(struct musb *musb, int is_on);
  * Checks the revision. We need to use the DMA register as 3.0 does not
  * have correct versions for TUSB_PRCM_REV or TUSB_INT_CTRL_REV.
  */
-u8 tusb_get_revision(struct musb *musb)
+static u8 tusb_get_revision(struct musb *musb)
 {
 	void __iomem	*tbase = musb->ctrl_base;
 	u32		die_id;
@@ -58,14 +59,13 @@ u8 tusb_get_revision(struct musb *musb)
 
 	return rev;
 }
-EXPORT_SYMBOL_GPL(tusb_get_revision);
 
-static int tusb_print_revision(struct musb *musb)
+static void tusb_print_revision(struct musb *musb)
 {
 	void __iomem	*tbase = musb->ctrl_base;
 	u8		rev;
 
-	rev = tusb_get_revision(musb);
+	rev = musb->tusb_revision;
 
 	pr_info("tusb: %s%i.%i %s%i.%i %s%i.%i %s%i.%i %s%i %s%i.%i\n",
 		"prcm",
@@ -84,8 +84,6 @@ static int tusb_print_revision(struct musb *musb)
 		TUSB_DIDR1_HI_CHIP_REV(musb_readl(tbase, TUSB_DIDR1_HI)),
 		"rev",
 		TUSB_REV_MAJOR(rev), TUSB_REV_MINOR(rev));
-
-	return tusb_get_revision(musb);
 }
 
 #define WBUS_QUIRK_MASK	(TUSB_PHY_OTG_CTRL_TESTM2 | TUSB_PHY_OTG_CTRL_TESTM1 \
@@ -349,7 +347,7 @@ static void tusb_allow_idle(struct musb *musb, u32 wakeup_enables)
 	u32		reg;
 
 	if ((wakeup_enables & TUSB_PRCM_WBUS)
-			&& (tusb_get_revision(musb) == TUSB_REV_30))
+			&& (musb->tusb_revision == TUSB_REV_30))
 		tusb_wbus_quirk(musb, 1);
 
 	tusb_set_clock_source(musb, 0);
@@ -797,7 +795,7 @@ static irqreturn_t tusb_musb_interrupt(int irq, void *__hci)
 		u32	reg;
 		u32	i;
 
-		if (tusb_get_revision(musb) == TUSB_REV_30)
+		if (musb->tusb_revision == TUSB_REV_30)
 			tusb_wbus_quirk(musb, 0);
 
 		/* there are issues re-locking the PLL on wakeup ... */
@@ -1011,10 +1009,11 @@ static int tusb_musb_start(struct musb *musb)
 		goto err;
 	}
 
-	ret = tusb_print_revision(musb);
-	if (ret < 2) {
+	musb->tusb_revision = tusb_get_revision(musb);
+	tusb_print_revision(musb);
+	if (musb->tusb_revision < 2) {
 		printk(KERN_ERR "tusb: Unsupported TUSB6010 revision %i\n",
-				ret);
+				musb->tusb_revision);
 		goto err;
 	}
 
@@ -1065,7 +1064,6 @@ static int tusb_musb_init(struct musb *musb)
 	void __iomem		*sync = NULL;
 	int			ret;
 
-	usb_nop_xceiv_register();
 	musb->xceiv = usb_get_phy(USB_PHY_TYPE_USB2);
 	if (IS_ERR_OR_NULL(musb->xceiv))
 		return -EPROBE_DEFER;
@@ -1117,7 +1115,6 @@ done:
 			iounmap(sync);
 
 		usb_put_phy(musb->xceiv);
-		usb_nop_xceiv_unregister();
 	}
 	return ret;
 }
@@ -1133,7 +1130,6 @@ static int tusb_musb_exit(struct musb *musb)
 	iounmap(musb->sync_va);
 
 	usb_put_phy(musb->xceiv);
-	usb_nop_xceiv_unregister();
 	return 0;
 }
 
@@ -1176,6 +1172,7 @@ static int tusb_probe(struct platform_device *pdev)
 
 	pdata->platform_ops		= &tusb_ops;
 
+	usb_phy_generic_register();
 	platform_set_drvdata(pdev, glue);
 
 	memset(musb_resources, 0x00, sizeof(*musb_resources) *
@@ -1224,6 +1221,7 @@ static int tusb_remove(struct platform_device *pdev)
 	struct tusb6010_glue		*glue = platform_get_drvdata(pdev);
 
 	platform_device_unregister(glue->musb);
+	usb_phy_generic_unregister(glue->phy);
 	kfree(glue);
 
 	return 0;
diff --git a/drivers/usb/musb/tusb6010.h b/drivers/usb/musb/tusb6010.h
index 35c933a5d99..aec86c86ce3 100644
--- a/drivers/usb/musb/tusb6010.h
+++ b/drivers/usb/musb/tusb6010.h
@@ -12,14 +12,6 @@
 #ifndef __TUSB6010_H__
 #define __TUSB6010_H__
 
-extern u8 tusb_get_revision(struct musb *musb);
-
-#ifdef CONFIG_USB_TUSB6010
-#define musb_in_tusb()			1
-#else
-#define musb_in_tusb()			0
-#endif
-
 #ifdef CONFIG_USB_TUSB_OMAP_DMA
 #define tusb_dma_omap()			1
 #else
diff --git a/drivers/usb/musb/tusb6010_omap.c b/drivers/usb/musb/tusb6010_omap.c
index e33b6b2c44c..3ce152c0408 100644
--- a/drivers/usb/musb/tusb6010_omap.c
+++ b/drivers/usb/musb/tusb6010_omap.c
@@ -677,7 +677,7 @@ struct dma_controller *dma_controller_create(struct musb *musb, void __iomem *ba
 	tusb_dma->controller.channel_program = tusb_omap_dma_program;
 	tusb_dma->controller.channel_abort = tusb_omap_dma_abort;
 
-	if (tusb_get_revision(musb) >= TUSB_REV_30)
+	if (musb->tusb_revision >= TUSB_REV_30)
 		tusb_dma->multichannel = 1;
 
 	for (i = 0; i < MAX_DMAREQ; i++) {
diff --git a/drivers/usb/phy/Kconfig b/drivers/usb/phy/Kconfig
index 416e0c8cf6f..e253fa05be6 100644
--- a/drivers/usb/phy/Kconfig
+++ b/drivers/usb/phy/Kconfig
@@ -6,15 +6,6 @@ menu "USB Physical Layer drivers"
 config USB_PHY
 	def_bool n
 
-config USB_OTG_FSM
-	tristate "USB 2.0 OTG FSM implementation"
-	depends on USB
-	select USB_OTG
-	select USB_PHY
-	help
-	  Implements OTG Final State Machine as specified in On-The-Go
-	  and Embedded Host Supplement to the USB Revision 2.0 Specification.
-
 #
 # USB Transceiver Drivers
 #
@@ -59,14 +50,6 @@ config KEYSTONE_USB_PHY
 	  interface to interact with USB 2.0 and USB 3.0 PHY that is part
 	  of the Keystone SOC.
 
-config MV_U3D_PHY
-	bool "Marvell USB 3.0 PHY controller Driver"
-	depends on CPU_MMP3
-	select USB_PHY
-	help
-	  Enable this to support Marvell USB 3.0 phy controller for Marvell
-	  SoC.
-
 config NOP_USB_XCEIV
 	tristate "NOP USB Transceiver Driver"
 	select USB_PHY
@@ -171,11 +154,12 @@ config USB_ISP1301
 	  module will be called phy-isp1301.
 
 config USB_MSM_OTG
-	tristate "OTG support for Qualcomm on-chip USB controller"
-	depends on (USB || USB_GADGET) && ARCH_MSM
+	tristate "Qualcomm on-chip USB OTG controller support"
+	depends on (USB || USB_GADGET) && (ARCH_MSM || ARCH_QCOM || COMPILE_TEST)
+	depends on RESET_CONTROLLER
 	select USB_PHY
 	help
-	  Enable this to support the USB OTG transceiver on MSM chips. It
+	  Enable this to support the USB OTG transceiver on Qualcomm 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
@@ -208,6 +192,7 @@ config USB_MXS_PHY
 config USB_RCAR_PHY
 	tristate "Renesas R-Car USB PHY support"
 	depends on USB || USB_GADGET
+	depends on ARCH_R8A7778 || ARCH_R8A7779 || COMPILE_TEST
 	select USB_PHY
 	help
 	  Say Y here to add support for the Renesas R-Car USB common PHY driver.
@@ -232,7 +217,7 @@ config USB_RCAR_GEN2_PHY
 
 config USB_ULPI
 	bool "Generic ULPI Transceiver Driver"
-	depends on ARM
+	depends on ARM || ARM64
 	help
 	  Enable this to support ULPI connected USB OTG transceivers which
 	  are likely found on embedded boards.
diff --git a/drivers/usb/phy/Makefile b/drivers/usb/phy/Makefile
index f8fa719a31b..24a91332d4a 100644
--- a/drivers/usb/phy/Makefile
+++ b/drivers/usb/phy/Makefile
@@ -3,14 +3,12 @@
 #
 obj-$(CONFIG_USB_PHY)			+= phy.o
 obj-$(CONFIG_OF)			+= of.o
-obj-$(CONFIG_USB_OTG_FSM)		+= phy-fsm-usb.o
 
 # transceiver drivers, keep the list sorted
 
 obj-$(CONFIG_AB8500_USB)		+= phy-ab8500-usb.o
 obj-$(CONFIG_FSL_USB2_OTG)		+= phy-fsl-usb.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-generic.o
 obj-$(CONFIG_TAHVO_USB)			+= phy-tahvo.o
 obj-$(CONFIG_AM335X_CONTROL_USB)	+= phy-am335x-control.o
diff --git a/drivers/usb/phy/phy-am335x-control.c b/drivers/usb/phy/phy-am335x-control.c
index d75196ad5f2..35b6083b799 100644
--- a/drivers/usb/phy/phy-am335x-control.c
+++ b/drivers/usb/phy/phy-am335x-control.c
@@ -3,6 +3,7 @@
 #include <linux/err.h>
 #include <linux/of.h>
 #include <linux/io.h>
+#include <linux/delay.h>
 #include "am35x-phy-control.h"
 
 struct am335x_control_usb {
@@ -86,6 +87,14 @@ static void am335x_phy_power(struct phy_control *phy_ctrl, u32 id, bool on)
 	}
 
 	writel(val, usb_ctrl->phy_reg + reg);
+
+	/*
+	 * Give the PHY ~1ms to complete the power up operation.
+	 * Tests have shown unstable behaviour if other USB PHY related
+	 * registers are written too shortly after such a transition.
+	 */
+	if (on)
+		mdelay(1);
 }
 
 static const struct phy_control ctrl_am335x = {
diff --git a/drivers/usb/phy/phy-am335x.c b/drivers/usb/phy/phy-am335x.c
index 12fc3468a01..585e50cb198 100644
--- a/drivers/usb/phy/phy-am335x.c
+++ b/drivers/usb/phy/phy-am335x.c
@@ -2,7 +2,7 @@
 #include <linux/platform_device.h>
 #include <linux/dma-mapping.h>
 #include <linux/usb/otg.h>
-#include <linux/usb/usb_phy_gen_xceiv.h>
+#include <linux/usb/usb_phy_generic.h>
 #include <linux/slab.h>
 #include <linux/clk.h>
 #include <linux/regulator/consumer.h>
@@ -13,7 +13,7 @@
 #include "phy-generic.h"
 
 struct am335x_phy {
-	struct usb_phy_gen_xceiv usb_phy_gen;
+	struct usb_phy_generic usb_phy_gen;
 	struct phy_control *phy_ctrl;
 	int id;
 };
diff --git a/drivers/usb/phy/phy-generic.c b/drivers/usb/phy/phy-generic.c
index bb394980532..7594e5069ae 100644
--- a/drivers/usb/phy/phy-generic.c
+++ b/drivers/usb/phy/phy-generic.c
@@ -30,7 +30,7 @@
 #include <linux/platform_device.h>
 #include <linux/dma-mapping.h>
 #include <linux/usb/otg.h>
-#include <linux/usb/usb_phy_gen_xceiv.h>
+#include <linux/usb/usb_phy_generic.h>
 #include <linux/slab.h>
 #include <linux/clk.h>
 #include <linux/regulator/consumer.h>
@@ -41,34 +41,25 @@
 
 #include "phy-generic.h"
 
-static struct platform_device *pd;
-
-void usb_nop_xceiv_register(void)
+struct platform_device *usb_phy_generic_register(void)
 {
-	if (pd)
-		return;
-	pd = platform_device_register_simple("usb_phy_gen_xceiv", -1, NULL, 0);
-	if (IS_ERR(pd)) {
-		pr_err("Unable to register generic usb transceiver\n");
-		pd = NULL;
-		return;
-	}
+	return platform_device_register_simple("usb_phy_generic",
+			PLATFORM_DEVID_AUTO, NULL, 0);
 }
-EXPORT_SYMBOL(usb_nop_xceiv_register);
+EXPORT_SYMBOL_GPL(usb_phy_generic_register);
 
-void usb_nop_xceiv_unregister(void)
+void usb_phy_generic_unregister(struct platform_device *pdev)
 {
-	platform_device_unregister(pd);
-	pd = NULL;
+	platform_device_unregister(pdev);
 }
-EXPORT_SYMBOL(usb_nop_xceiv_unregister);
+EXPORT_SYMBOL_GPL(usb_phy_generic_unregister);
 
 static int nop_set_suspend(struct usb_phy *x, int suspend)
 {
 	return 0;
 }
 
-static void nop_reset_set(struct usb_phy_gen_xceiv *nop, int asserted)
+static void nop_reset_set(struct usb_phy_generic *nop, int asserted)
 {
 	int value;
 
@@ -87,7 +78,7 @@ static void nop_reset_set(struct usb_phy_gen_xceiv *nop, int asserted)
 
 int usb_gen_phy_init(struct usb_phy *phy)
 {
-	struct usb_phy_gen_xceiv *nop = dev_get_drvdata(phy->dev);
+	struct usb_phy_generic *nop = dev_get_drvdata(phy->dev);
 
 	if (!IS_ERR(nop->vcc)) {
 		if (regulator_enable(nop->vcc))
@@ -106,7 +97,7 @@ EXPORT_SYMBOL_GPL(usb_gen_phy_init);
 
 void usb_gen_phy_shutdown(struct usb_phy *phy)
 {
-	struct usb_phy_gen_xceiv *nop = dev_get_drvdata(phy->dev);
+	struct usb_phy_generic *nop = dev_get_drvdata(phy->dev);
 
 	/* Assert RESET */
 	nop_reset_set(nop, 1);
@@ -150,8 +141,8 @@ static int nop_set_host(struct usb_otg *otg, struct usb_bus *host)
 	return 0;
 }
 
-int usb_phy_gen_create_phy(struct device *dev, struct usb_phy_gen_xceiv *nop,
-		struct usb_phy_gen_xceiv_platform_data *pdata)
+int usb_phy_gen_create_phy(struct device *dev, struct usb_phy_generic *nop,
+		struct usb_phy_generic_platform_data *pdata)
 {
 	enum usb_phy_type type = USB_PHY_TYPE_USB2;
 	int err;
@@ -245,10 +236,10 @@ int usb_phy_gen_create_phy(struct device *dev, struct usb_phy_gen_xceiv *nop,
 }
 EXPORT_SYMBOL_GPL(usb_phy_gen_create_phy);
 
-static int usb_phy_gen_xceiv_probe(struct platform_device *pdev)
+static int usb_phy_generic_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
-	struct usb_phy_gen_xceiv	*nop;
+	struct usb_phy_generic	*nop;
 	int err;
 
 	nop = devm_kzalloc(dev, sizeof(*nop), GFP_KERNEL);
@@ -274,9 +265,9 @@ static int usb_phy_gen_xceiv_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static int usb_phy_gen_xceiv_remove(struct platform_device *pdev)
+static int usb_phy_generic_remove(struct platform_device *pdev)
 {
-	struct usb_phy_gen_xceiv *nop = platform_get_drvdata(pdev);
+	struct usb_phy_generic *nop = platform_get_drvdata(pdev);
 
 	usb_remove_phy(&nop->phy);
 
@@ -290,29 +281,29 @@ static const struct of_device_id nop_xceiv_dt_ids[] = {
 
 MODULE_DEVICE_TABLE(of, nop_xceiv_dt_ids);
 
-static struct platform_driver usb_phy_gen_xceiv_driver = {
-	.probe		= usb_phy_gen_xceiv_probe,
-	.remove		= usb_phy_gen_xceiv_remove,
+static struct platform_driver usb_phy_generic_driver = {
+	.probe		= usb_phy_generic_probe,
+	.remove		= usb_phy_generic_remove,
 	.driver		= {
-		.name	= "usb_phy_gen_xceiv",
+		.name	= "usb_phy_generic",
 		.owner	= THIS_MODULE,
 		.of_match_table = nop_xceiv_dt_ids,
 	},
 };
 
-static int __init usb_phy_gen_xceiv_init(void)
+static int __init usb_phy_generic_init(void)
 {
-	return platform_driver_register(&usb_phy_gen_xceiv_driver);
+	return platform_driver_register(&usb_phy_generic_driver);
 }
-subsys_initcall(usb_phy_gen_xceiv_init);
+subsys_initcall(usb_phy_generic_init);
 
-static void __exit usb_phy_gen_xceiv_exit(void)
+static void __exit usb_phy_generic_exit(void)
 {
-	platform_driver_unregister(&usb_phy_gen_xceiv_driver);
+	platform_driver_unregister(&usb_phy_generic_driver);
 }
-module_exit(usb_phy_gen_xceiv_exit);
+module_exit(usb_phy_generic_exit);
 
-MODULE_ALIAS("platform:usb_phy_gen_xceiv");
+MODULE_ALIAS("platform:usb_phy_generic");
 MODULE_AUTHOR("Texas Instruments Inc");
 MODULE_DESCRIPTION("NOP USB Transceiver driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/usb/phy/phy-generic.h b/drivers/usb/phy/phy-generic.h
index 38a81f307b8..d8feacc0b7f 100644
--- a/drivers/usb/phy/phy-generic.h
+++ b/drivers/usb/phy/phy-generic.h
@@ -1,9 +1,9 @@
 #ifndef _PHY_GENERIC_H_
 #define _PHY_GENERIC_H_
 
-#include <linux/usb/usb_phy_gen_xceiv.h>
+#include <linux/usb/usb_phy_generic.h>
 
-struct usb_phy_gen_xceiv {
+struct usb_phy_generic {
 	struct usb_phy phy;
 	struct device *dev;
 	struct clk *clk;
@@ -15,7 +15,7 @@ struct usb_phy_gen_xceiv {
 int usb_gen_phy_init(struct usb_phy *phy);
 void usb_gen_phy_shutdown(struct usb_phy *phy);
 
-int usb_phy_gen_create_phy(struct device *dev, struct usb_phy_gen_xceiv *nop,
-		struct usb_phy_gen_xceiv_platform_data *pdata);
+int usb_phy_gen_create_phy(struct device *dev, struct usb_phy_generic *nop,
+		struct usb_phy_generic_platform_data *pdata);
 
 #endif
diff --git a/drivers/usb/phy/phy-isp1301-omap.c b/drivers/usb/phy/phy-isp1301-omap.c
index 6e146d723b3..69e49be8866 100644
--- a/drivers/usb/phy/phy-isp1301-omap.c
+++ b/drivers/usb/phy/phy-isp1301-omap.c
@@ -1295,7 +1295,7 @@ isp1301_set_host(struct usb_otg *otg, struct usb_bus *host)
 		return isp1301_otg_enable(isp);
 	return 0;
 
-#elif	!defined(CONFIG_USB_GADGET_OMAP)
+#elif !IS_ENABLED(CONFIG_USB_OMAP)
 	// FIXME update its refcount
 	otg->host = host;
 
diff --git a/drivers/usb/phy/phy-keystone.c b/drivers/usb/phy/phy-keystone.c
index d762003896c..f4d722de912 100644
--- a/drivers/usb/phy/phy-keystone.c
+++ b/drivers/usb/phy/phy-keystone.c
@@ -18,7 +18,7 @@
 
 #include <linux/module.h>
 #include <linux/platform_device.h>
-#include <linux/usb/usb_phy_gen_xceiv.h>
+#include <linux/usb/usb_phy_generic.h>
 #include <linux/io.h>
 #include <linux/of.h>
 
@@ -35,7 +35,7 @@
 #define PHY_REF_SSP_EN			BIT(29)
 
 struct keystone_usbphy {
-	struct usb_phy_gen_xceiv	usb_phy_gen;
+	struct usb_phy_generic	usb_phy_gen;
 	void __iomem			*phy_ctrl;
 };
 
diff --git a/drivers/usb/phy/phy-msm-usb.c b/drivers/usb/phy/phy-msm-usb.c
index 5b37b81f2ef..ced34f39bdd 100644
--- a/drivers/usb/phy/phy-msm-usb.c
+++ b/drivers/usb/phy/phy-msm-usb.c
@@ -30,9 +30,13 @@
 #include <linux/debugfs.h>
 #include <linux/seq_file.h>
 #include <linux/pm_runtime.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/reset.h>
 
 #include <linux/usb.h>
 #include <linux/usb/otg.h>
+#include <linux/usb/of.h>
 #include <linux/usb/ulpi.h>
 #include <linux/usb/gadget.h>
 #include <linux/usb/hcd.h>
@@ -44,6 +48,7 @@
 #define DRIVER_NAME	"msm_otg"
 
 #define ULPI_IO_TIMEOUT_USEC	(10 * 1000)
+#define LINK_RESET_TIMEOUT_USEC	(250 * 1000)
 
 #define USB_PHY_3P3_VOL_MIN	3050000 /* uV */
 #define USB_PHY_3P3_VOL_MAX	3300000 /* uV */
@@ -57,48 +62,38 @@
 
 #define USB_PHY_VDD_DIG_VOL_MIN	1000000 /* uV */
 #define USB_PHY_VDD_DIG_VOL_MAX	1320000 /* uV */
+#define USB_PHY_SUSP_DIG_VOL	500000  /* uV */
 
-static struct regulator *hsusb_3p3;
-static struct regulator *hsusb_1p8;
-static struct regulator *hsusb_vddcx;
+enum vdd_levels {
+	VDD_LEVEL_NONE = 0,
+	VDD_LEVEL_MIN,
+	VDD_LEVEL_MAX,
+};
 
 static int msm_hsusb_init_vddcx(struct msm_otg *motg, int init)
 {
 	int ret = 0;
 
 	if (init) {
-		hsusb_vddcx = regulator_get(motg->phy.dev, "HSUSB_VDDCX");
-		if (IS_ERR(hsusb_vddcx)) {
-			dev_err(motg->phy.dev, "unable to get hsusb vddcx\n");
-			return PTR_ERR(hsusb_vddcx);
-		}
-
-		ret = regulator_set_voltage(hsusb_vddcx,
-				USB_PHY_VDD_DIG_VOL_MIN,
-				USB_PHY_VDD_DIG_VOL_MAX);
+		ret = regulator_set_voltage(motg->vddcx,
+				motg->vdd_levels[VDD_LEVEL_MIN],
+				motg->vdd_levels[VDD_LEVEL_MAX]);
 		if (ret) {
-			dev_err(motg->phy.dev, "unable to set the voltage "
-					"for hsusb vddcx\n");
-			regulator_put(hsusb_vddcx);
+			dev_err(motg->phy.dev, "Cannot set vddcx voltage\n");
 			return ret;
 		}
 
-		ret = regulator_enable(hsusb_vddcx);
-		if (ret) {
+		ret = regulator_enable(motg->vddcx);
+		if (ret)
 			dev_err(motg->phy.dev, "unable to enable hsusb vddcx\n");
-			regulator_put(hsusb_vddcx);
-		}
 	} else {
-		ret = regulator_set_voltage(hsusb_vddcx, 0,
-			USB_PHY_VDD_DIG_VOL_MAX);
+		ret = regulator_set_voltage(motg->vddcx, 0,
+				motg->vdd_levels[VDD_LEVEL_MAX]);
 		if (ret)
-			dev_err(motg->phy.dev, "unable to set the voltage "
-					"for hsusb vddcx\n");
-		ret = regulator_disable(hsusb_vddcx);
+			dev_err(motg->phy.dev, "Cannot set vddcx voltage\n");
+		ret = regulator_disable(motg->vddcx);
 		if (ret)
 			dev_err(motg->phy.dev, "unable to disable hsusb vddcx\n");
-
-		regulator_put(hsusb_vddcx);
 	}
 
 	return ret;
@@ -109,98 +104,67 @@ static int msm_hsusb_ldo_init(struct msm_otg *motg, int init)
 	int rc = 0;
 
 	if (init) {
-		hsusb_3p3 = regulator_get(motg->phy.dev, "HSUSB_3p3");
-		if (IS_ERR(hsusb_3p3)) {
-			dev_err(motg->phy.dev, "unable to get hsusb 3p3\n");
-			return PTR_ERR(hsusb_3p3);
-		}
-
-		rc = regulator_set_voltage(hsusb_3p3, USB_PHY_3P3_VOL_MIN,
+		rc = regulator_set_voltage(motg->v3p3, USB_PHY_3P3_VOL_MIN,
 				USB_PHY_3P3_VOL_MAX);
 		if (rc) {
-			dev_err(motg->phy.dev, "unable to set voltage level "
-					"for hsusb 3p3\n");
-			goto put_3p3;
+			dev_err(motg->phy.dev, "Cannot set v3p3 voltage\n");
+			goto exit;
 		}
-		rc = regulator_enable(hsusb_3p3);
+		rc = regulator_enable(motg->v3p3);
 		if (rc) {
 			dev_err(motg->phy.dev, "unable to enable the hsusb 3p3\n");
-			goto put_3p3;
+			goto exit;
 		}
-		hsusb_1p8 = regulator_get(motg->phy.dev, "HSUSB_1p8");
-		if (IS_ERR(hsusb_1p8)) {
-			dev_err(motg->phy.dev, "unable to get hsusb 1p8\n");
-			rc = PTR_ERR(hsusb_1p8);
-			goto disable_3p3;
-		}
-		rc = regulator_set_voltage(hsusb_1p8, USB_PHY_1P8_VOL_MIN,
+		rc = regulator_set_voltage(motg->v1p8, USB_PHY_1P8_VOL_MIN,
 				USB_PHY_1P8_VOL_MAX);
 		if (rc) {
-			dev_err(motg->phy.dev, "unable to set voltage level "
-					"for hsusb 1p8\n");
-			goto put_1p8;
+			dev_err(motg->phy.dev, "Cannot set v1p8 voltage\n");
+			goto disable_3p3;
 		}
-		rc = regulator_enable(hsusb_1p8);
+		rc = regulator_enable(motg->v1p8);
 		if (rc) {
 			dev_err(motg->phy.dev, "unable to enable the hsusb 1p8\n");
-			goto put_1p8;
+			goto disable_3p3;
 		}
 
 		return 0;
 	}
 
-	regulator_disable(hsusb_1p8);
-put_1p8:
-	regulator_put(hsusb_1p8);
+	regulator_disable(motg->v1p8);
 disable_3p3:
-	regulator_disable(hsusb_3p3);
-put_3p3:
-	regulator_put(hsusb_3p3);
+	regulator_disable(motg->v3p3);
+exit:
 	return rc;
 }
 
-static int msm_hsusb_ldo_set_mode(int on)
+static int msm_hsusb_ldo_set_mode(struct msm_otg *motg, int on)
 {
 	int ret = 0;
 
-	if (!hsusb_1p8 || IS_ERR(hsusb_1p8)) {
-		pr_err("%s: HSUSB_1p8 is not initialized\n", __func__);
-		return -ENODEV;
-	}
-
-	if (!hsusb_3p3 || IS_ERR(hsusb_3p3)) {
-		pr_err("%s: HSUSB_3p3 is not initialized\n", __func__);
-		return -ENODEV;
-	}
-
 	if (on) {
-		ret = regulator_set_optimum_mode(hsusb_1p8,
+		ret = regulator_set_optimum_mode(motg->v1p8,
 				USB_PHY_1P8_HPM_LOAD);
 		if (ret < 0) {
-			pr_err("%s: Unable to set HPM of the regulator "
-				"HSUSB_1p8\n", __func__);
+			pr_err("Could not set HPM for v1p8\n");
 			return ret;
 		}
-		ret = regulator_set_optimum_mode(hsusb_3p3,
+		ret = regulator_set_optimum_mode(motg->v3p3,
 				USB_PHY_3P3_HPM_LOAD);
 		if (ret < 0) {
-			pr_err("%s: Unable to set HPM of the regulator "
-				"HSUSB_3p3\n", __func__);
-			regulator_set_optimum_mode(hsusb_1p8,
+			pr_err("Could not set HPM for v3p3\n");
+			regulator_set_optimum_mode(motg->v1p8,
 				USB_PHY_1P8_LPM_LOAD);
 			return ret;
 		}
 	} else {
-		ret = regulator_set_optimum_mode(hsusb_1p8,
+		ret = regulator_set_optimum_mode(motg->v1p8,
 				USB_PHY_1P8_LPM_LOAD);
 		if (ret < 0)
-			pr_err("%s: Unable to set LPM of the regulator "
-				"HSUSB_1p8\n", __func__);
-		ret = regulator_set_optimum_mode(hsusb_3p3,
+			pr_err("Could not set LPM for v1p8\n");
+		ret = regulator_set_optimum_mode(motg->v3p3,
 				USB_PHY_3P3_LPM_LOAD);
 		if (ret < 0)
-			pr_err("%s: Unable to set LPM of the regulator "
-				"HSUSB_3p3\n", __func__);
+			pr_err("Could not set LPM for v3p3\n");
 	}
 
 	pr_debug("reg (%s)\n", on ? "HPM" : "LPM");
@@ -265,27 +229,47 @@ static struct usb_phy_io_ops msm_otg_io_ops = {
 static void ulpi_init(struct msm_otg *motg)
 {
 	struct msm_otg_platform_data *pdata = motg->pdata;
-	int *seq = pdata->phy_init_seq;
+	int *seq = pdata->phy_init_seq, idx;
+	u32 addr = ULPI_EXT_VENDOR_SPECIFIC;
 
-	if (!seq)
-		return;
+	for (idx = 0; idx < pdata->phy_init_sz; idx++) {
+		if (seq[idx] == -1)
+			continue;
 
-	while (seq[0] >= 0) {
 		dev_vdbg(motg->phy.dev, "ulpi: write 0x%02x to 0x%02x\n",
-				seq[0], seq[1]);
-		ulpi_write(&motg->phy, seq[0], seq[1]);
-		seq += 2;
+				seq[idx], addr + idx);
+		ulpi_write(&motg->phy, seq[idx], addr + idx);
 	}
 }
 
+static int msm_phy_notify_disconnect(struct usb_phy *phy,
+				   enum usb_device_speed speed)
+{
+	int val;
+
+	/*
+	 * Put the transceiver in non-driving mode. Otherwise host
+	 * may not detect soft-disconnection.
+	 */
+	val = ulpi_read(phy, ULPI_FUNC_CTRL);
+	val &= ~ULPI_FUNC_CTRL_OPMODE_MASK;
+	val |= ULPI_FUNC_CTRL_OPMODE_NONDRIVING;
+	ulpi_write(phy, val, ULPI_FUNC_CTRL);
+
+	return 0;
+}
+
 static int msm_otg_link_clk_reset(struct msm_otg *motg, bool assert)
 {
-	int ret = 0;
+	int ret;
 
-	if (!motg->pdata->link_clk_reset)
-		return ret;
+	if (motg->pdata->link_clk_reset)
+		ret = motg->pdata->link_clk_reset(motg->clk, assert);
+	else if (assert)
+		ret = reset_control_assert(motg->link_rst);
+	else
+		ret = reset_control_deassert(motg->link_rst);
 
-	ret = motg->pdata->link_clk_reset(motg->clk, assert);
 	if (ret)
 		dev_err(motg->phy.dev, "usb link clk reset %s failed\n",
 			assert ? "assert" : "deassert");
@@ -295,111 +279,150 @@ static int msm_otg_link_clk_reset(struct msm_otg *motg, bool assert)
 
 static int msm_otg_phy_clk_reset(struct msm_otg *motg)
 {
-	int ret = 0;
+	int ret;
 
-	if (!motg->pdata->phy_clk_reset)
-		return ret;
+	if (motg->pdata->phy_clk_reset)
+		ret = motg->pdata->phy_clk_reset(motg->phy_reset_clk);
+	else
+		ret = reset_control_reset(motg->phy_rst);
 
-	ret = motg->pdata->phy_clk_reset(motg->phy_reset_clk);
 	if (ret)
 		dev_err(motg->phy.dev, "usb phy clk reset failed\n");
 
 	return ret;
 }
 
-static int msm_otg_phy_reset(struct msm_otg *motg)
+static int msm_link_reset(struct msm_otg *motg)
 {
 	u32 val;
 	int ret;
-	int retries;
 
 	ret = msm_otg_link_clk_reset(motg, 1);
 	if (ret)
 		return ret;
-	ret = msm_otg_phy_clk_reset(motg);
-	if (ret)
-		return ret;
+
+	/* wait for 1ms delay as suggested in HPG. */
+	usleep_range(1000, 1200);
+
 	ret = msm_otg_link_clk_reset(motg, 0);
 	if (ret)
 		return ret;
 
+	if (motg->phy_number)
+		writel(readl(USB_PHY_CTRL2) | BIT(16), USB_PHY_CTRL2);
+
+	/* put transceiver in serial mode as part of reset */
 	val = readl(USB_PORTSC) & ~PORTSC_PTS_MASK;
-	writel(val | PORTSC_PTS_ULPI, USB_PORTSC);
+	writel(val | PORTSC_PTS_SERIAL, USB_PORTSC);
 
-	for (retries = 3; retries > 0; retries--) {
-		ret = ulpi_write(&motg->phy, ULPI_FUNC_CTRL_SUSPENDM,
-				ULPI_CLR(ULPI_FUNC_CTRL));
-		if (!ret)
-			break;
-		ret = msm_otg_phy_clk_reset(motg);
-		if (ret)
-			return ret;
-	}
-	if (!retries)
-		return -ETIMEDOUT;
+	return 0;
+}
 
-	/* This reset calibrates the phy, if the above write succeeded */
-	ret = msm_otg_phy_clk_reset(motg);
-	if (ret)
-		return ret;
+static int msm_otg_reset(struct usb_phy *phy)
+{
+	struct msm_otg *motg = container_of(phy, struct msm_otg, phy);
+	int cnt = 0;
 
-	for (retries = 3; retries > 0; retries--) {
-		ret = ulpi_read(&motg->phy, ULPI_DEBUG);
-		if (ret != -ETIMEDOUT)
+	writel(USBCMD_RESET, USB_USBCMD);
+	while (cnt < LINK_RESET_TIMEOUT_USEC) {
+		if (!(readl(USB_USBCMD) & USBCMD_RESET))
 			break;
-		ret = msm_otg_phy_clk_reset(motg);
-		if (ret)
-			return ret;
+		udelay(1);
+		cnt++;
 	}
-	if (!retries)
+	if (cnt >= LINK_RESET_TIMEOUT_USEC)
 		return -ETIMEDOUT;
 
-	dev_info(motg->phy.dev, "phy_reset: success\n");
+	/* select ULPI phy and clear other status/control bits in PORTSC */
+	writel(PORTSC_PTS_ULPI, USB_PORTSC);
+
+	writel(0x0, USB_AHBBURST);
+	writel(0x08, USB_AHBMODE);
+
+	if (motg->phy_number)
+		writel(readl(USB_PHY_CTRL2) | BIT(16), USB_PHY_CTRL2);
 	return 0;
 }
 
-#define LINK_RESET_TIMEOUT_USEC		(250 * 1000)
-static int msm_otg_reset(struct usb_phy *phy)
+static void msm_phy_reset(struct msm_otg *motg)
+{
+	void __iomem *addr;
+
+	if (motg->pdata->phy_type != SNPS_28NM_INTEGRATED_PHY) {
+		msm_otg_phy_clk_reset(motg);
+		return;
+	}
+
+	addr = USB_PHY_CTRL;
+	if (motg->phy_number)
+		addr = USB_PHY_CTRL2;
+
+	/* Assert USB PHY_POR */
+	writel(readl(addr) | PHY_POR_ASSERT, addr);
+
+	/*
+	 * wait for minimum 10 microseconds as suggested in HPG.
+	 * Use a slightly larger value since the exact value didn't
+	 * work 100% of the time.
+	 */
+	udelay(12);
+
+	/* Deassert USB PHY_POR */
+	writel(readl(addr) & ~PHY_POR_ASSERT, addr);
+}
+
+static int msm_usb_reset(struct usb_phy *phy)
 {
 	struct msm_otg *motg = container_of(phy, struct msm_otg, phy);
-	struct msm_otg_platform_data *pdata = motg->pdata;
-	int cnt = 0;
 	int ret;
-	u32 val = 0;
-	u32 ulpi_val = 0;
 
-	ret = msm_otg_phy_reset(motg);
+	if (!IS_ERR(motg->core_clk))
+		clk_prepare_enable(motg->core_clk);
+
+	ret = msm_link_reset(motg);
 	if (ret) {
 		dev_err(phy->dev, "phy_reset failed\n");
 		return ret;
 	}
 
-	ulpi_init(motg);
-
-	writel(USBCMD_RESET, USB_USBCMD);
-	while (cnt < LINK_RESET_TIMEOUT_USEC) {
-		if (!(readl(USB_USBCMD) & USBCMD_RESET))
-			break;
-		udelay(1);
-		cnt++;
+	ret = msm_otg_reset(&motg->phy);
+	if (ret) {
+		dev_err(phy->dev, "link reset failed\n");
+		return ret;
 	}
-	if (cnt >= LINK_RESET_TIMEOUT_USEC)
-		return -ETIMEDOUT;
-
-	/* select ULPI phy */
-	writel(0x80000000, USB_PORTSC);
 
 	msleep(100);
 
-	writel(0x0, USB_AHBBURST);
-	writel(0x00, USB_AHBMODE);
+	/* Reset USB PHY after performing USB Link RESET */
+	msm_phy_reset(motg);
+
+	if (!IS_ERR(motg->core_clk))
+		clk_disable_unprepare(motg->core_clk);
+
+	return 0;
+}
+
+static int msm_phy_init(struct usb_phy *phy)
+{
+	struct msm_otg *motg = container_of(phy, struct msm_otg, phy);
+	struct msm_otg_platform_data *pdata = motg->pdata;
+	u32 val, ulpi_val = 0;
+
+	/* Program USB PHY Override registers. */
+	ulpi_init(motg);
+
+	/*
+	 * It is recommended in HPG to reset USB PHY after programming
+	 * USB PHY Override registers.
+	 */
+	msm_phy_reset(motg);
 
 	if (pdata->otg_control == OTG_PHY_CONTROL) {
 		val = readl(USB_OTGSC);
-		if (pdata->mode == USB_OTG) {
+		if (pdata->mode == USB_DR_MODE_OTG) {
 			ulpi_val = ULPI_INT_IDGRD | ULPI_INT_SESS_VALID;
 			val |= OTGSC_IDIE | OTGSC_BSVIE;
-		} else if (pdata->mode == USB_PERIPHERAL) {
+		} else if (pdata->mode == USB_DR_MODE_PERIPHERAL) {
 			ulpi_val = ULPI_INT_SESS_VALID;
 			val |= OTGSC_BSVIE;
 		}
@@ -408,6 +431,9 @@ static int msm_otg_reset(struct usb_phy *phy)
 		ulpi_write(phy, ulpi_val, ULPI_USB_INT_EN_FALL);
 	}
 
+	if (motg->phy_number)
+		writel(readl(USB_PHY_CTRL2) | BIT(16), USB_PHY_CTRL2);
+
 	return 0;
 }
 
@@ -416,22 +442,20 @@ static int msm_otg_reset(struct usb_phy *phy)
 
 #ifdef CONFIG_PM
 
-#define USB_PHY_SUSP_DIG_VOL  500000
-static int msm_hsusb_config_vddcx(int high)
+static int msm_hsusb_config_vddcx(struct msm_otg *motg, int high)
 {
-	int max_vol = USB_PHY_VDD_DIG_VOL_MAX;
+	int max_vol = motg->vdd_levels[VDD_LEVEL_MAX];
 	int min_vol;
 	int ret;
 
 	if (high)
-		min_vol = USB_PHY_VDD_DIG_VOL_MIN;
+		min_vol = motg->vdd_levels[VDD_LEVEL_MIN];
 	else
-		min_vol = USB_PHY_SUSP_DIG_VOL;
+		min_vol = motg->vdd_levels[VDD_LEVEL_NONE];
 
-	ret = regulator_set_voltage(hsusb_vddcx, min_vol, max_vol);
+	ret = regulator_set_voltage(motg->vddcx, min_vol, max_vol);
 	if (ret) {
-		pr_err("%s: unable to set the voltage for regulator "
-			"HSUSB_VDDCX\n", __func__);
+		pr_err("Cannot set vddcx voltage\n");
 		return ret;
 	}
 
@@ -445,6 +469,7 @@ static int msm_otg_suspend(struct msm_otg *motg)
 	struct usb_phy *phy = &motg->phy;
 	struct usb_bus *bus = phy->otg->host;
 	struct msm_otg_platform_data *pdata = motg->pdata;
+	void __iomem *addr;
 	int cnt = 0;
 
 	if (atomic_read(&motg->in_lpm))
@@ -504,22 +529,23 @@ static int msm_otg_suspend(struct msm_otg *motg)
 	 */
 	writel(readl(USB_USBCMD) | ASYNC_INTR_CTRL | ULPI_STP_CTRL, USB_USBCMD);
 
+	addr = USB_PHY_CTRL;
+	if (motg->phy_number)
+		addr = USB_PHY_CTRL2;
+
 	if (motg->pdata->phy_type == SNPS_28NM_INTEGRATED_PHY &&
 			motg->pdata->otg_control == OTG_PMIC_CONTROL)
-		writel(readl(USB_PHY_CTRL) | PHY_RETEN, USB_PHY_CTRL);
+		writel(readl(addr) | PHY_RETEN, addr);
 
 	clk_disable_unprepare(motg->pclk);
 	clk_disable_unprepare(motg->clk);
-	if (motg->core_clk)
+	if (!IS_ERR(motg->core_clk))
 		clk_disable_unprepare(motg->core_clk);
 
-	if (!IS_ERR(motg->pclk_src))
-		clk_disable_unprepare(motg->pclk_src);
-
 	if (motg->pdata->phy_type == SNPS_28NM_INTEGRATED_PHY &&
 			motg->pdata->otg_control == OTG_PMIC_CONTROL) {
-		msm_hsusb_ldo_set_mode(0);
-		msm_hsusb_config_vddcx(0);
+		msm_hsusb_ldo_set_mode(motg, 0);
+		msm_hsusb_config_vddcx(motg, 0);
 	}
 
 	if (device_may_wakeup(phy->dev))
@@ -539,25 +565,28 @@ static int msm_otg_resume(struct msm_otg *motg)
 {
 	struct usb_phy *phy = &motg->phy;
 	struct usb_bus *bus = phy->otg->host;
+	void __iomem *addr;
 	int cnt = 0;
 	unsigned temp;
 
 	if (!atomic_read(&motg->in_lpm))
 		return 0;
 
-	if (!IS_ERR(motg->pclk_src))
-		clk_prepare_enable(motg->pclk_src);
-
 	clk_prepare_enable(motg->pclk);
 	clk_prepare_enable(motg->clk);
-	if (motg->core_clk)
+	if (!IS_ERR(motg->core_clk))
 		clk_prepare_enable(motg->core_clk);
 
 	if (motg->pdata->phy_type == SNPS_28NM_INTEGRATED_PHY &&
 			motg->pdata->otg_control == OTG_PMIC_CONTROL) {
-		msm_hsusb_ldo_set_mode(1);
-		msm_hsusb_config_vddcx(1);
-		writel(readl(USB_PHY_CTRL) & ~PHY_RETEN, USB_PHY_CTRL);
+
+		addr = USB_PHY_CTRL;
+		if (motg->phy_number)
+			addr = USB_PHY_CTRL2;
+
+		msm_hsusb_ldo_set_mode(motg, 1);
+		msm_hsusb_config_vddcx(motg, 1);
+		writel(readl(addr) & ~PHY_RETEN, addr);
 	}
 
 	temp = readl(USB_USBCMD);
@@ -586,8 +615,7 @@ static int msm_otg_resume(struct msm_otg *motg)
 		 * PHY. USB state can not be restored. Re-insertion
 		 * of USB cable is the only way to get USB working.
 		 */
-		dev_err(phy->dev, "Unable to resume USB."
-				"Re-plugin the cable\n");
+		dev_err(phy->dev, "Unable to resume USB. Re-plugin the cable\n");
 		msm_otg_reset(phy);
 	}
 
@@ -687,7 +715,7 @@ static int msm_otg_set_host(struct usb_otg *otg, struct usb_bus *host)
 	 * Fail host registration if this board can support
 	 * only peripheral configuration.
 	 */
-	if (motg->pdata->mode == USB_PERIPHERAL) {
+	if (motg->pdata->mode == USB_DR_MODE_PERIPHERAL) {
 		dev_info(otg->phy->dev, "Host mode is not supported\n");
 		return -ENODEV;
 	}
@@ -716,7 +744,7 @@ static int msm_otg_set_host(struct usb_otg *otg, struct usb_bus *host)
 	 * Kick the state machine work, if peripheral is not supported
 	 * or peripheral is already registered with us.
 	 */
-	if (motg->pdata->mode == USB_HOST || otg->gadget) {
+	if (motg->pdata->mode == USB_DR_MODE_HOST || otg->gadget) {
 		pm_runtime_get_sync(otg->phy->dev);
 		schedule_work(&motg->sm_work);
 	}
@@ -760,7 +788,7 @@ static int msm_otg_set_peripheral(struct usb_otg *otg,
 	 * Fail peripheral registration if this board can support
 	 * only host configuration.
 	 */
-	if (motg->pdata->mode == USB_HOST) {
+	if (motg->pdata->mode == USB_DR_MODE_HOST) {
 		dev_info(otg->phy->dev, "Peripheral mode is not supported\n");
 		return -ENODEV;
 	}
@@ -785,7 +813,7 @@ static int msm_otg_set_peripheral(struct usb_otg *otg,
 	 * Kick the state machine work, if host is not supported
 	 * or host is already registered with us.
 	 */
-	if (motg->pdata->mode == USB_PERIPHERAL || otg->host) {
+	if (motg->pdata->mode == USB_DR_MODE_PERIPHERAL || otg->host) {
 		pm_runtime_get_sync(otg->phy->dev);
 		schedule_work(&motg->sm_work);
 	}
@@ -1106,7 +1134,7 @@ static void msm_otg_init_sm(struct msm_otg *motg)
 	u32 otgsc = readl(USB_OTGSC);
 
 	switch (pdata->mode) {
-	case USB_OTG:
+	case USB_DR_MODE_OTG:
 		if (pdata->otg_control == OTG_PHY_CONTROL) {
 			if (otgsc & OTGSC_ID)
 				set_bit(ID, &motg->inputs);
@@ -1118,21 +1146,14 @@ static void msm_otg_init_sm(struct msm_otg *motg)
 			else
 				clear_bit(B_SESS_VLD, &motg->inputs);
 		} else if (pdata->otg_control == OTG_USER_CONTROL) {
-			if (pdata->default_mode == USB_HOST) {
-				clear_bit(ID, &motg->inputs);
-			} else if (pdata->default_mode == USB_PERIPHERAL) {
-				set_bit(ID, &motg->inputs);
-				set_bit(B_SESS_VLD, &motg->inputs);
-			} else {
 				set_bit(ID, &motg->inputs);
 				clear_bit(B_SESS_VLD, &motg->inputs);
-			}
 		}
 		break;
-	case USB_HOST:
+	case USB_DR_MODE_HOST:
 		clear_bit(ID, &motg->inputs);
 		break;
-	case USB_PERIPHERAL:
+	case USB_DR_MODE_PERIPHERAL:
 		set_bit(ID, &motg->inputs);
 		if (otgsc & OTGSC_BSV)
 			set_bit(B_SESS_VLD, &motg->inputs);
@@ -1282,13 +1303,13 @@ static int msm_otg_mode_show(struct seq_file *s, void *unused)
 
 	switch (otg->phy->state) {
 	case OTG_STATE_A_HOST:
-		seq_printf(s, "host\n");
+		seq_puts(s, "host\n");
 		break;
 	case OTG_STATE_B_PERIPHERAL:
-		seq_printf(s, "peripheral\n");
+		seq_puts(s, "peripheral\n");
 		break;
 	default:
-		seq_printf(s, "none\n");
+		seq_puts(s, "none\n");
 		break;
 	}
 
@@ -1308,7 +1329,7 @@ static ssize_t msm_otg_mode_write(struct file *file, const char __user *ubuf,
 	char buf[16];
 	struct usb_otg *otg = motg->phy.otg;
 	int status = count;
-	enum usb_mode_type req_mode;
+	enum usb_dr_mode req_mode;
 
 	memset(buf, 0x00, sizeof(buf));
 
@@ -1318,18 +1339,18 @@ static ssize_t msm_otg_mode_write(struct file *file, const char __user *ubuf,
 	}
 
 	if (!strncmp(buf, "host", 4)) {
-		req_mode = USB_HOST;
+		req_mode = USB_DR_MODE_HOST;
 	} else if (!strncmp(buf, "peripheral", 10)) {
-		req_mode = USB_PERIPHERAL;
+		req_mode = USB_DR_MODE_PERIPHERAL;
 	} else if (!strncmp(buf, "none", 4)) {
-		req_mode = USB_NONE;
+		req_mode = USB_DR_MODE_UNKNOWN;
 	} else {
 		status = -EINVAL;
 		goto out;
 	}
 
 	switch (req_mode) {
-	case USB_NONE:
+	case USB_DR_MODE_UNKNOWN:
 		switch (otg->phy->state) {
 		case OTG_STATE_A_HOST:
 		case OTG_STATE_B_PERIPHERAL:
@@ -1340,7 +1361,7 @@ static ssize_t msm_otg_mode_write(struct file *file, const char __user *ubuf,
 			goto out;
 		}
 		break;
-	case USB_PERIPHERAL:
+	case USB_DR_MODE_PERIPHERAL:
 		switch (otg->phy->state) {
 		case OTG_STATE_B_IDLE:
 		case OTG_STATE_A_HOST:
@@ -1351,7 +1372,7 @@ static ssize_t msm_otg_mode_write(struct file *file, const char __user *ubuf,
 			goto out;
 		}
 		break;
-	case USB_HOST:
+	case USB_DR_MODE_HOST:
 		switch (otg->phy->state) {
 		case OTG_STATE_B_IDLE:
 		case OTG_STATE_B_PERIPHERAL:
@@ -1406,74 +1427,154 @@ static void msm_otg_debugfs_cleanup(void)
 	debugfs_remove(msm_otg_dbg_root);
 }
 
-static int __init msm_otg_probe(struct platform_device *pdev)
+static struct of_device_id msm_otg_dt_match[] = {
+	{
+		.compatible = "qcom,usb-otg-ci",
+		.data = (void *) CI_45NM_INTEGRATED_PHY
+	},
+	{
+		.compatible = "qcom,usb-otg-snps",
+		.data = (void *) SNPS_28NM_INTEGRATED_PHY
+	},
+	{ }
+};
+MODULE_DEVICE_TABLE(of, msm_otg_dt_match);
+
+static int msm_otg_read_dt(struct platform_device *pdev, struct msm_otg *motg)
+{
+	struct msm_otg_platform_data *pdata;
+	const struct of_device_id *id;
+	struct device_node *node = pdev->dev.of_node;
+	struct property *prop;
+	int len, ret, words;
+	u32 val, tmp[3];
+
+	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return -ENOMEM;
+
+	motg->pdata = pdata;
+
+	id = of_match_device(msm_otg_dt_match, &pdev->dev);
+	pdata->phy_type = (enum msm_usb_phy_type) id->data;
+
+	motg->link_rst = devm_reset_control_get(&pdev->dev, "link");
+	if (IS_ERR(motg->link_rst))
+		return PTR_ERR(motg->link_rst);
+
+	motg->phy_rst = devm_reset_control_get(&pdev->dev, "phy");
+	if (IS_ERR(motg->phy_rst))
+		return PTR_ERR(motg->phy_rst);
+
+	pdata->mode = of_usb_get_dr_mode(node);
+	if (pdata->mode == USB_DR_MODE_UNKNOWN)
+		pdata->mode = USB_DR_MODE_OTG;
+
+	pdata->otg_control = OTG_PHY_CONTROL;
+	if (!of_property_read_u32(node, "qcom,otg-control", &val))
+		if (val == OTG_PMIC_CONTROL)
+			pdata->otg_control = val;
+
+	if (!of_property_read_u32(node, "qcom,phy-num", &val) && val < 2)
+		motg->phy_number = val;
+
+	motg->vdd_levels[VDD_LEVEL_NONE] = USB_PHY_SUSP_DIG_VOL;
+	motg->vdd_levels[VDD_LEVEL_MIN] = USB_PHY_VDD_DIG_VOL_MIN;
+	motg->vdd_levels[VDD_LEVEL_MAX] = USB_PHY_VDD_DIG_VOL_MAX;
+
+	if (of_get_property(node, "qcom,vdd-levels", &len) &&
+	    len == sizeof(tmp)) {
+		of_property_read_u32_array(node, "qcom,vdd-levels",
+					   tmp, len / sizeof(*tmp));
+		motg->vdd_levels[VDD_LEVEL_NONE] = tmp[VDD_LEVEL_NONE];
+		motg->vdd_levels[VDD_LEVEL_MIN] = tmp[VDD_LEVEL_MIN];
+		motg->vdd_levels[VDD_LEVEL_MAX] = tmp[VDD_LEVEL_MAX];
+	}
+
+	prop = of_find_property(node, "qcom,phy-init-sequence", &len);
+	if (!prop || !len)
+		return 0;
+
+	words = len / sizeof(u32);
+
+	if (words >= ULPI_EXT_VENDOR_SPECIFIC) {
+		dev_warn(&pdev->dev, "Too big PHY init sequence %d\n", words);
+		return 0;
+	}
+
+	pdata->phy_init_seq = devm_kzalloc(&pdev->dev, len, GFP_KERNEL);
+	if (!pdata->phy_init_seq) {
+		dev_warn(&pdev->dev, "No space for PHY init sequence\n");
+		return 0;
+	}
+
+	ret = of_property_read_u32_array(node, "qcom,phy-init-sequence",
+					 pdata->phy_init_seq, words);
+	if (!ret)
+		pdata->phy_init_sz = words;
+
+	return 0;
+}
+
+static int msm_otg_probe(struct platform_device *pdev)
 {
+	struct regulator_bulk_data regs[3];
 	int ret = 0;
+	struct device_node *np = pdev->dev.of_node;
+	struct msm_otg_platform_data *pdata;
 	struct resource *res;
 	struct msm_otg *motg;
 	struct usb_phy *phy;
+	void __iomem *phy_select;
 
-	dev_info(&pdev->dev, "msm_otg probe\n");
-	if (!dev_get_platdata(&pdev->dev)) {
-		dev_err(&pdev->dev, "No platform data given. Bailing out\n");
-		return -ENODEV;
-	}
-
-	motg = kzalloc(sizeof(struct msm_otg), GFP_KERNEL);
+	motg = devm_kzalloc(&pdev->dev, sizeof(struct msm_otg), GFP_KERNEL);
 	if (!motg) {
 		dev_err(&pdev->dev, "unable to allocate msm_otg\n");
 		return -ENOMEM;
 	}
 
-	motg->phy.otg = kzalloc(sizeof(struct usb_otg), GFP_KERNEL);
+	pdata = dev_get_platdata(&pdev->dev);
+	if (!pdata) {
+		if (!np)
+			return -ENXIO;
+		ret = msm_otg_read_dt(pdev, motg);
+		if (ret)
+			return ret;
+	}
+
+	motg->phy.otg = devm_kzalloc(&pdev->dev, sizeof(struct usb_otg),
+				     GFP_KERNEL);
 	if (!motg->phy.otg) {
 		dev_err(&pdev->dev, "unable to allocate msm_otg\n");
-		ret = -ENOMEM;
-		goto free_motg;
+		return -ENOMEM;
 	}
 
-	motg->pdata = dev_get_platdata(&pdev->dev);
 	phy = &motg->phy;
 	phy->dev = &pdev->dev;
 
-	motg->phy_reset_clk = clk_get(&pdev->dev, "usb_phy_clk");
+	motg->phy_reset_clk = devm_clk_get(&pdev->dev,
+					   np ? "phy" : "usb_phy_clk");
 	if (IS_ERR(motg->phy_reset_clk)) {
 		dev_err(&pdev->dev, "failed to get usb_phy_clk\n");
-		ret = PTR_ERR(motg->phy_reset_clk);
-		goto free_motg;
+		return PTR_ERR(motg->phy_reset_clk);
 	}
 
-	motg->clk = clk_get(&pdev->dev, "usb_hs_clk");
+	motg->clk = devm_clk_get(&pdev->dev, np ? "core" : "usb_hs_clk");
 	if (IS_ERR(motg->clk)) {
 		dev_err(&pdev->dev, "failed to get usb_hs_clk\n");
-		ret = PTR_ERR(motg->clk);
-		goto put_phy_reset_clk;
+		return PTR_ERR(motg->clk);
 	}
-	clk_set_rate(motg->clk, 60000000);
 
 	/*
 	 * If USB Core is running its protocol engine based on CORE CLK,
 	 * CORE CLK  must be running at >55Mhz for correct HSUSB
 	 * operation and USB core cannot tolerate frequency changes on
-	 * CORE CLK. For such USB cores, vote for maximum clk frequency
-	 * on pclk source
+	 * CORE CLK.
 	 */
-	 if (motg->pdata->pclk_src_name) {
-		motg->pclk_src = clk_get(&pdev->dev,
-			motg->pdata->pclk_src_name);
-		if (IS_ERR(motg->pclk_src))
-			goto put_clk;
-		clk_set_rate(motg->pclk_src, INT_MAX);
-		clk_prepare_enable(motg->pclk_src);
-	} else
-		motg->pclk_src = ERR_PTR(-ENOENT);
-
-
-	motg->pclk = clk_get(&pdev->dev, "usb_hs_pclk");
+	motg->pclk = devm_clk_get(&pdev->dev, np ? "iface" : "usb_hs_pclk");
 	if (IS_ERR(motg->pclk)) {
 		dev_err(&pdev->dev, "failed to get usb_hs_pclk\n");
-		ret = PTR_ERR(motg->pclk);
-		goto put_pclk_src;
+		return PTR_ERR(motg->pclk);
 	}
 
 	/*
@@ -1481,69 +1582,90 @@ static int __init msm_otg_probe(struct platform_device *pdev)
 	 * clock is introduced to remove the dependency on AXI
 	 * bus frequency.
 	 */
-	motg->core_clk = clk_get(&pdev->dev, "usb_hs_core_clk");
-	if (IS_ERR(motg->core_clk))
-		motg->core_clk = NULL;
+	motg->core_clk = devm_clk_get(&pdev->dev,
+				      np ? "alt_core" : "usb_hs_core_clk");
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&pdev->dev, "failed to get platform resource mem\n");
-		ret = -ENODEV;
-		goto put_core_clk;
-	}
+	if (!res)
+		return -EINVAL;
+	motg->regs = devm_ioremap(&pdev->dev, res->start, resource_size(res));
+	if (!motg->regs)
+		return -ENOMEM;
 
-	motg->regs = ioremap(res->start, resource_size(res));
-	if (!motg->regs) {
-		dev_err(&pdev->dev, "ioremap failed\n");
-		ret = -ENOMEM;
-		goto put_core_clk;
+	/*
+	 * NOTE: The PHYs can be multiplexed between the chipidea controller
+	 * and the dwc3 controller, using a single bit. It is important that
+	 * the dwc3 driver does not set this bit in an incompatible way.
+	 */
+	if (motg->phy_number) {
+		phy_select = devm_ioremap_nocache(&pdev->dev, USB2_PHY_SEL, 4);
+		if (IS_ERR(phy_select))
+			return PTR_ERR(phy_select);
+		/* Enable second PHY with the OTG port */
+		writel(0x1, phy_select);
 	}
+
 	dev_info(&pdev->dev, "OTG regs = %p\n", motg->regs);
 
 	motg->irq = platform_get_irq(pdev, 0);
-	if (!motg->irq) {
+	if (motg->irq < 0) {
 		dev_err(&pdev->dev, "platform_get_irq failed\n");
-		ret = -ENODEV;
-		goto free_regs;
+		return motg->irq;
 	}
 
+	regs[0].supply = "vddcx";
+	regs[1].supply = "v3p3";
+	regs[2].supply = "v1p8";
+
+	ret = devm_regulator_bulk_get(motg->phy.dev, ARRAY_SIZE(regs), regs);
+	if (ret)
+		return ret;
+
+	motg->vddcx = regs[0].consumer;
+	motg->v3p3  = regs[1].consumer;
+	motg->v1p8  = regs[2].consumer;
+
+	clk_set_rate(motg->clk, 60000000);
+
 	clk_prepare_enable(motg->clk);
 	clk_prepare_enable(motg->pclk);
 
+	if (!IS_ERR(motg->core_clk))
+		clk_prepare_enable(motg->core_clk);
+
 	ret = msm_hsusb_init_vddcx(motg, 1);
 	if (ret) {
 		dev_err(&pdev->dev, "hsusb vddcx configuration failed\n");
-		goto free_regs;
+		goto disable_clks;
 	}
 
 	ret = msm_hsusb_ldo_init(motg, 1);
 	if (ret) {
 		dev_err(&pdev->dev, "hsusb vreg configuration failed\n");
-		goto vddcx_exit;
+		goto disable_vddcx;
 	}
-	ret = msm_hsusb_ldo_set_mode(1);
+	ret = msm_hsusb_ldo_set_mode(motg, 1);
 	if (ret) {
 		dev_err(&pdev->dev, "hsusb vreg enable failed\n");
-		goto ldo_exit;
+		goto disable_ldo;
 	}
 
-	if (motg->core_clk)
-		clk_prepare_enable(motg->core_clk);
-
 	writel(0, USB_USBINTR);
 	writel(0, USB_OTGSC);
 
 	INIT_WORK(&motg->sm_work, msm_otg_sm_work);
 	INIT_DELAYED_WORK(&motg->chg_work, msm_chg_detect_work);
-	ret = request_irq(motg->irq, msm_otg_irq, IRQF_SHARED,
+	ret = devm_request_irq(&pdev->dev, motg->irq, msm_otg_irq, IRQF_SHARED,
 					"msm_otg", motg);
 	if (ret) {
 		dev_err(&pdev->dev, "request irq failed\n");
-		goto disable_clks;
+		goto disable_ldo;
 	}
 
-	phy->init = msm_otg_reset;
+	phy->init = msm_phy_init;
 	phy->set_power = msm_otg_set_power;
+	phy->notify_disconnect = msm_phy_notify_disconnect;
+	phy->type = USB_PHY_TYPE_USB2;
 
 	phy->io_ops = &msm_otg_io_ops;
 
@@ -1551,54 +1673,38 @@ static int __init msm_otg_probe(struct platform_device *pdev)
 	phy->otg->set_host = msm_otg_set_host;
 	phy->otg->set_peripheral = msm_otg_set_peripheral;
 
-	ret = usb_add_phy(&motg->phy, USB_PHY_TYPE_USB2);
+	msm_usb_reset(phy);
+
+	ret = usb_add_phy_dev(&motg->phy);
 	if (ret) {
 		dev_err(&pdev->dev, "usb_add_phy failed\n");
-		goto free_irq;
+		goto disable_ldo;
 	}
 
 	platform_set_drvdata(pdev, motg);
 	device_init_wakeup(&pdev->dev, 1);
 
-	if (motg->pdata->mode == USB_OTG &&
-			motg->pdata->otg_control == OTG_USER_CONTROL) {
+	if (motg->pdata->mode == USB_DR_MODE_OTG &&
+		motg->pdata->otg_control == OTG_USER_CONTROL) {
 		ret = msm_otg_debugfs_init(motg);
 		if (ret)
-			dev_dbg(&pdev->dev, "mode debugfs file is"
-					"not available\n");
+			dev_dbg(&pdev->dev, "Can not create mode change file\n");
 	}
 
 	pm_runtime_set_active(&pdev->dev);
 	pm_runtime_enable(&pdev->dev);
 
 	return 0;
-free_irq:
-	free_irq(motg->irq, motg);
+
+disable_ldo:
+	msm_hsusb_ldo_init(motg, 0);
+disable_vddcx:
+	msm_hsusb_init_vddcx(motg, 0);
 disable_clks:
 	clk_disable_unprepare(motg->pclk);
 	clk_disable_unprepare(motg->clk);
-ldo_exit:
-	msm_hsusb_ldo_init(motg, 0);
-vddcx_exit:
-	msm_hsusb_init_vddcx(motg, 0);
-free_regs:
-	iounmap(motg->regs);
-put_core_clk:
-	if (motg->core_clk)
-		clk_put(motg->core_clk);
-	clk_put(motg->pclk);
-put_pclk_src:
-	if (!IS_ERR(motg->pclk_src)) {
-		clk_disable_unprepare(motg->pclk_src);
-		clk_put(motg->pclk_src);
-	}
-put_clk:
-	clk_put(motg->clk);
-put_phy_reset_clk:
-	clk_put(motg->phy_reset_clk);
-free_motg:
-	kfree(motg->phy.otg);
-	kfree(motg);
+	if (!IS_ERR(motg->core_clk))
+		clk_disable_unprepare(motg->core_clk);
 	return ret;
 }
 
@@ -1621,7 +1727,7 @@ static int msm_otg_remove(struct platform_device *pdev)
 	pm_runtime_disable(&pdev->dev);
 
 	usb_remove_phy(phy);
-	free_irq(motg->irq, motg);
+	disable_irq(motg->irq);
 
 	/*
 	 * Put PHY in low power mode.
@@ -1641,26 +1747,12 @@ static int msm_otg_remove(struct platform_device *pdev)
 
 	clk_disable_unprepare(motg->pclk);
 	clk_disable_unprepare(motg->clk);
-	if (motg->core_clk)
+	if (!IS_ERR(motg->core_clk))
 		clk_disable_unprepare(motg->core_clk);
-	if (!IS_ERR(motg->pclk_src)) {
-		clk_disable_unprepare(motg->pclk_src);
-		clk_put(motg->pclk_src);
-	}
 	msm_hsusb_ldo_init(motg, 0);
 
-	iounmap(motg->regs);
 	pm_runtime_set_suspended(&pdev->dev);
 
-	clk_put(motg->phy_reset_clk);
-	clk_put(motg->pclk);
-	clk_put(motg->clk);
-	if (motg->core_clk)
-		clk_put(motg->core_clk);
-
-	kfree(motg->phy.otg);
-	kfree(motg);
-
 	return 0;
 }
 
@@ -1740,15 +1832,17 @@ static const struct dev_pm_ops msm_otg_dev_pm_ops = {
 };
 
 static struct platform_driver msm_otg_driver = {
+	.probe = msm_otg_probe,
 	.remove = msm_otg_remove,
 	.driver = {
 		.name = DRIVER_NAME,
 		.owner = THIS_MODULE,
 		.pm = &msm_otg_dev_pm_ops,
+		.of_match_table = msm_otg_dt_match,
 	},
 };
 
-module_platform_driver_probe(msm_otg_driver, msm_otg_probe);
+module_platform_driver(msm_otg_driver);
 
 MODULE_LICENSE("GPL v2");
 MODULE_DESCRIPTION("MSM USB transceiver driver");
diff --git a/drivers/usb/phy/phy-mv-u3d-usb.c b/drivers/usb/phy/phy-mv-u3d-usb.c
deleted file mode 100644
index d317903022b..00000000000
--- a/drivers/usb/phy/phy-mv-u3d-usb.c
+++ /dev/null
@@ -1,338 +0,0 @@
-/*
- * Copyright (C) 2011 Marvell International Ltd. 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.
- */
-
-#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/usb/otg.h>
-#include <linux/platform_data/mv_usb.h>
-
-#include "phy-mv-u3d-usb.h"
-
-/*
- * struct mv_u3d_phy - transceiver driver state
- * @phy: transceiver structure
- * @dev: The parent device supplied to the probe function
- * @clk: usb phy clock
- * @base: usb phy register memory base
- */
-struct mv_u3d_phy {
-	struct usb_phy	phy;
-	struct mv_usb_platform_data *plat;
-	struct device	*dev;
-	struct clk	*clk;
-	void __iomem	*base;
-};
-
-static u32 mv_u3d_phy_read(void __iomem *base, u32 reg)
-{
-	void __iomem *addr, *data;
-
-	addr = base;
-	data = base + 0x4;
-
-	writel_relaxed(reg, addr);
-	return readl_relaxed(data);
-}
-
-static void mv_u3d_phy_set(void __iomem *base, u32 reg, u32 value)
-{
-	void __iomem *addr, *data;
-	u32 tmp;
-
-	addr = base;
-	data = base + 0x4;
-
-	writel_relaxed(reg, addr);
-	tmp = readl_relaxed(data);
-	tmp |= value;
-	writel_relaxed(tmp, data);
-}
-
-static void mv_u3d_phy_clear(void __iomem *base, u32 reg, u32 value)
-{
-	void __iomem *addr, *data;
-	u32 tmp;
-
-	addr = base;
-	data = base + 0x4;
-
-	writel_relaxed(reg, addr);
-	tmp = readl_relaxed(data);
-	tmp &= ~value;
-	writel_relaxed(tmp, data);
-}
-
-static void mv_u3d_phy_write(void __iomem *base, u32 reg, u32 value)
-{
-	void __iomem *addr, *data;
-
-	addr = base;
-	data = base + 0x4;
-
-	writel_relaxed(reg, addr);
-	writel_relaxed(value, data);
-}
-
-static void mv_u3d_phy_shutdown(struct usb_phy *phy)
-{
-	struct mv_u3d_phy *mv_u3d_phy;
-	void __iomem *base;
-	u32 val;
-
-	mv_u3d_phy = container_of(phy, struct mv_u3d_phy, phy);
-	base = mv_u3d_phy->base;
-
-	/* Power down Reference Analog current, bit 15
-	 * Power down PLL, bit 14
-	 * Power down Receiver, bit 13
-	 * Power down Transmitter, bit 12
-	 * of USB3_POWER_PLL_CONTROL register
-	 */
-	val = mv_u3d_phy_read(base, USB3_POWER_PLL_CONTROL);
-	val &= ~(USB3_POWER_PLL_CONTROL_PU);
-	mv_u3d_phy_write(base, USB3_POWER_PLL_CONTROL, val);
-
-	if (mv_u3d_phy->clk)
-		clk_disable(mv_u3d_phy->clk);
-}
-
-static int mv_u3d_phy_init(struct usb_phy *phy)
-{
-	struct mv_u3d_phy *mv_u3d_phy;
-	void __iomem *base;
-	u32 val, count;
-
-	/* enable usb3 phy */
-	mv_u3d_phy = container_of(phy, struct mv_u3d_phy, phy);
-
-	if (mv_u3d_phy->clk)
-		clk_enable(mv_u3d_phy->clk);
-
-	base = mv_u3d_phy->base;
-
-	val = mv_u3d_phy_read(base, USB3_POWER_PLL_CONTROL);
-	val &= ~(USB3_POWER_PLL_CONTROL_PU_MASK);
-	val |= 0xF << USB3_POWER_PLL_CONTROL_PU_SHIFT;
-	mv_u3d_phy_write(base, USB3_POWER_PLL_CONTROL, val);
-	udelay(100);
-
-	mv_u3d_phy_write(base, USB3_RESET_CONTROL,
-			USB3_RESET_CONTROL_RESET_PIPE);
-	udelay(100);
-
-	mv_u3d_phy_write(base, USB3_RESET_CONTROL,
-			USB3_RESET_CONTROL_RESET_PIPE
-			| USB3_RESET_CONTROL_RESET_PHY);
-	udelay(100);
-
-	val = mv_u3d_phy_read(base, USB3_POWER_PLL_CONTROL);
-	val &= ~(USB3_POWER_PLL_CONTROL_REF_FREF_SEL_MASK
-		| USB3_POWER_PLL_CONTROL_PHY_MODE_MASK);
-	val |=  (USB3_PLL_25MHZ << USB3_POWER_PLL_CONTROL_REF_FREF_SEL_SHIFT)
-		| (0x5 << USB3_POWER_PLL_CONTROL_PHY_MODE_SHIFT);
-	mv_u3d_phy_write(base, USB3_POWER_PLL_CONTROL, val);
-	udelay(100);
-
-	mv_u3d_phy_clear(base, USB3_KVCO_CALI_CONTROL,
-		USB3_KVCO_CALI_CONTROL_USE_MAX_PLL_RATE_MASK);
-	udelay(100);
-
-	val = mv_u3d_phy_read(base, USB3_SQUELCH_FFE);
-	val &= ~(USB3_SQUELCH_FFE_FFE_CAP_SEL_MASK
-		| USB3_SQUELCH_FFE_FFE_RES_SEL_MASK
-		| USB3_SQUELCH_FFE_SQ_THRESH_IN_MASK);
-	val |= ((0xD << USB3_SQUELCH_FFE_FFE_CAP_SEL_SHIFT)
-		| (0x7 << USB3_SQUELCH_FFE_FFE_RES_SEL_SHIFT)
-		| (0x8 << USB3_SQUELCH_FFE_SQ_THRESH_IN_SHIFT));
-	mv_u3d_phy_write(base, USB3_SQUELCH_FFE, val);
-	udelay(100);
-
-	val = mv_u3d_phy_read(base, USB3_GEN1_SET0);
-	val &= ~USB3_GEN1_SET0_G1_TX_SLEW_CTRL_EN_MASK;
-	val |= 1 << USB3_GEN1_SET0_G1_TX_EMPH_EN_SHIFT;
-	mv_u3d_phy_write(base, USB3_GEN1_SET0, val);
-	udelay(100);
-
-	val = mv_u3d_phy_read(base, USB3_GEN2_SET0);
-	val &= ~(USB3_GEN2_SET0_G2_TX_AMP_MASK
-		| USB3_GEN2_SET0_G2_TX_EMPH_AMP_MASK
-		| USB3_GEN2_SET0_G2_TX_SLEW_CTRL_EN_MASK);
-	val |= ((0x14 << USB3_GEN2_SET0_G2_TX_AMP_SHIFT)
-		| (1 << USB3_GEN2_SET0_G2_TX_AMP_ADJ_SHIFT)
-		| (0xA << USB3_GEN2_SET0_G2_TX_EMPH_AMP_SHIFT)
-		| (1 << USB3_GEN2_SET0_G2_TX_EMPH_EN_SHIFT));
-	mv_u3d_phy_write(base, USB3_GEN2_SET0, val);
-	udelay(100);
-
-	mv_u3d_phy_read(base, USB3_TX_EMPPH);
-	val &= ~(USB3_TX_EMPPH_AMP_MASK
-		| USB3_TX_EMPPH_EN_MASK
-		| USB3_TX_EMPPH_AMP_FORCE_MASK
-		| USB3_TX_EMPPH_PAR1_MASK
-		| USB3_TX_EMPPH_PAR2_MASK);
-	val |= ((0xB << USB3_TX_EMPPH_AMP_SHIFT)
-		| (1 << USB3_TX_EMPPH_EN_SHIFT)
-		| (1 << USB3_TX_EMPPH_AMP_FORCE_SHIFT)
-		| (0x1C << USB3_TX_EMPPH_PAR1_SHIFT)
-		| (1 << USB3_TX_EMPPH_PAR2_SHIFT));
-
-	mv_u3d_phy_write(base, USB3_TX_EMPPH, val);
-	udelay(100);
-
-	val = mv_u3d_phy_read(base, USB3_GEN2_SET1);
-	val &= ~(USB3_GEN2_SET1_G2_RX_SELMUPI_MASK
-		| USB3_GEN2_SET1_G2_RX_SELMUPF_MASK
-		| USB3_GEN2_SET1_G2_RX_SELMUFI_MASK
-		| USB3_GEN2_SET1_G2_RX_SELMUFF_MASK);
-	val |= ((1 << USB3_GEN2_SET1_G2_RX_SELMUPI_SHIFT)
-		| (1 << USB3_GEN2_SET1_G2_RX_SELMUPF_SHIFT)
-		| (1 << USB3_GEN2_SET1_G2_RX_SELMUFI_SHIFT)
-		| (1 << USB3_GEN2_SET1_G2_RX_SELMUFF_SHIFT));
-	mv_u3d_phy_write(base, USB3_GEN2_SET1, val);
-	udelay(100);
-
-	val = mv_u3d_phy_read(base, USB3_DIGITAL_LOOPBACK_EN);
-	val &= ~USB3_DIGITAL_LOOPBACK_EN_SEL_BITS_MASK;
-	val |= 1 << USB3_DIGITAL_LOOPBACK_EN_SEL_BITS_SHIFT;
-	mv_u3d_phy_write(base, USB3_DIGITAL_LOOPBACK_EN, val);
-	udelay(100);
-
-	val = mv_u3d_phy_read(base, USB3_IMPEDANCE_TX_SSC);
-	val &= ~USB3_IMPEDANCE_TX_SSC_SSC_AMP_MASK;
-	val |= 0xC << USB3_IMPEDANCE_TX_SSC_SSC_AMP_SHIFT;
-	mv_u3d_phy_write(base, USB3_IMPEDANCE_TX_SSC, val);
-	udelay(100);
-
-	val = mv_u3d_phy_read(base, USB3_IMPEDANCE_CALI_CTRL);
-	val &= ~USB3_IMPEDANCE_CALI_CTRL_IMP_CAL_THR_MASK;
-	val |= 0x4 << USB3_IMPEDANCE_CALI_CTRL_IMP_CAL_THR_SHIFT;
-	mv_u3d_phy_write(base, USB3_IMPEDANCE_CALI_CTRL, val);
-	udelay(100);
-
-	val = mv_u3d_phy_read(base, USB3_PHY_ISOLATION_MODE);
-	val &= ~(USB3_PHY_ISOLATION_MODE_PHY_GEN_RX_MASK
-		| USB3_PHY_ISOLATION_MODE_PHY_GEN_TX_MASK
-		| USB3_PHY_ISOLATION_MODE_TX_DRV_IDLE_MASK);
-	val |= ((1 << USB3_PHY_ISOLATION_MODE_PHY_GEN_RX_SHIFT)
-		| (1 << USB3_PHY_ISOLATION_MODE_PHY_GEN_TX_SHIFT));
-	mv_u3d_phy_write(base, USB3_PHY_ISOLATION_MODE, val);
-	udelay(100);
-
-	val = mv_u3d_phy_read(base, USB3_TXDETRX);
-	val &= ~(USB3_TXDETRX_VTHSEL_MASK);
-	val |= 0x1 << USB3_TXDETRX_VTHSEL_SHIFT;
-	mv_u3d_phy_write(base, USB3_TXDETRX, val);
-	udelay(100);
-
-	dev_dbg(mv_u3d_phy->dev, "start calibration\n");
-
-calstart:
-	/* Perform Manual Calibration */
-	mv_u3d_phy_set(base, USB3_KVCO_CALI_CONTROL,
-		1 << USB3_KVCO_CALI_CONTROL_CAL_START_SHIFT);
-
-	mdelay(1);
-
-	count = 0;
-	while (1) {
-		val = mv_u3d_phy_read(base, USB3_KVCO_CALI_CONTROL);
-		if (val & (1 << USB3_KVCO_CALI_CONTROL_CAL_DONE_SHIFT))
-			break;
-		else if (count > 50) {
-			dev_dbg(mv_u3d_phy->dev, "calibration failure, retry...\n");
-			goto calstart;
-		}
-		count++;
-		mdelay(1);
-	}
-
-	/* active PIPE interface */
-	mv_u3d_phy_write(base, USB3_PIPE_SM_CTRL,
-		1 << USB3_PIPE_SM_CTRL_PHY_INIT_DONE);
-
-	return 0;
-}
-
-static int mv_u3d_phy_probe(struct platform_device *pdev)
-{
-	struct mv_u3d_phy *mv_u3d_phy;
-	struct mv_usb_platform_data *pdata;
-	struct device *dev = &pdev->dev;
-	struct resource *res;
-	void __iomem	*phy_base;
-	int	ret;
-
-	pdata = dev_get_platdata(&pdev->dev);
-	if (!pdata) {
-		dev_err(&pdev->dev, "%s: no platform data defined\n", __func__);
-		return -EINVAL;
-	}
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	phy_base = devm_ioremap_resource(dev, res);
-	if (IS_ERR(phy_base))
-		return PTR_ERR(phy_base);
-
-	mv_u3d_phy = devm_kzalloc(dev, sizeof(*mv_u3d_phy), GFP_KERNEL);
-	if (!mv_u3d_phy)
-		return -ENOMEM;
-
-	mv_u3d_phy->dev			= &pdev->dev;
-	mv_u3d_phy->plat		= pdata;
-	mv_u3d_phy->base		= phy_base;
-	mv_u3d_phy->phy.dev		= mv_u3d_phy->dev;
-	mv_u3d_phy->phy.label		= "mv-u3d-phy";
-	mv_u3d_phy->phy.init		= mv_u3d_phy_init;
-	mv_u3d_phy->phy.shutdown	= mv_u3d_phy_shutdown;
-
-	ret = usb_add_phy(&mv_u3d_phy->phy, USB_PHY_TYPE_USB3);
-	if (ret)
-		goto err;
-
-	if (!mv_u3d_phy->clk)
-		mv_u3d_phy->clk = clk_get(mv_u3d_phy->dev, "u3dphy");
-
-	platform_set_drvdata(pdev, mv_u3d_phy);
-
-	dev_info(&pdev->dev, "Initialized Marvell USB 3.0 PHY\n");
-err:
-	return ret;
-}
-
-static int mv_u3d_phy_remove(struct platform_device *pdev)
-{
-	struct mv_u3d_phy *mv_u3d_phy = platform_get_drvdata(pdev);
-
-	usb_remove_phy(&mv_u3d_phy->phy);
-
-	if (mv_u3d_phy->clk) {
-		clk_put(mv_u3d_phy->clk);
-		mv_u3d_phy->clk = NULL;
-	}
-
-	return 0;
-}
-
-static struct platform_driver mv_u3d_phy_driver = {
-	.probe		= mv_u3d_phy_probe,
-	.remove		= mv_u3d_phy_remove,
-	.driver		= {
-		.name	= "mv-u3d-phy",
-		.owner	= THIS_MODULE,
-	},
-};
-
-module_platform_driver(mv_u3d_phy_driver);
-MODULE_DESCRIPTION("Marvell USB 3.0 PHY controller");
-MODULE_AUTHOR("Yu Xu <yuxu@marvell.com>");
-MODULE_LICENSE("GPL");
-MODULE_ALIAS("platform:mv-u3d-phy");
diff --git a/drivers/usb/phy/phy-mv-u3d-usb.h b/drivers/usb/phy/phy-mv-u3d-usb.h
deleted file mode 100644
index 2a658cb9a52..00000000000
--- a/drivers/usb/phy/phy-mv-u3d-usb.h
+++ /dev/null
@@ -1,105 +0,0 @@
-/*
- * Copyright (C) 2011 Marvell International Ltd. 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.
- */
-
-#ifndef __MV_U3D_PHY_H
-#define __MV_U3D_PHY_H
-
-#define USB3_POWER_PLL_CONTROL		0x1
-#define USB3_KVCO_CALI_CONTROL		0x2
-#define USB3_IMPEDANCE_CALI_CTRL	0x3
-#define USB3_IMPEDANCE_TX_SSC		0x4
-#define USB3_SQUELCH_FFE		0x6
-#define USB3_GEN1_SET0			0xD
-#define USB3_GEN2_SET0			0xF
-#define USB3_GEN2_SET1			0x10
-#define USB3_DIGITAL_LOOPBACK_EN	0x23
-#define USB3_PHY_ISOLATION_MODE		0x26
-#define USB3_TXDETRX			0x48
-#define USB3_TX_EMPPH			0x5E
-#define USB3_RESET_CONTROL		0x90
-#define USB3_PIPE_SM_CTRL		0x91
-
-#define USB3_RESET_CONTROL_RESET_PIPE			0x1
-#define USB3_RESET_CONTROL_RESET_PHY			0x2
-
-#define USB3_POWER_PLL_CONTROL_REF_FREF_SEL_MASK	(0x1F << 0)
-#define USB3_POWER_PLL_CONTROL_REF_FREF_SEL_SHIFT	0
-#define USB3_PLL_25MHZ					0x2
-#define USB3_PLL_26MHZ					0x5
-#define USB3_POWER_PLL_CONTROL_PHY_MODE_MASK		(0x7 << 5)
-#define USB3_POWER_PLL_CONTROL_PHY_MODE_SHIFT		5
-#define USB3_POWER_PLL_CONTROL_PU_MASK			(0xF << 12)
-#define USB3_POWER_PLL_CONTROL_PU_SHIFT			12
-#define USB3_POWER_PLL_CONTROL_PU			(0xF << 12)
-
-#define USB3_KVCO_CALI_CONTROL_USE_MAX_PLL_RATE_MASK	(0x1 << 12)
-#define USB3_KVCO_CALI_CONTROL_USE_MAX_PLL_RATE_SHIFT	12
-#define USB3_KVCO_CALI_CONTROL_CAL_DONE_SHIFT		14
-#define USB3_KVCO_CALI_CONTROL_CAL_START_SHIFT		15
-
-#define USB3_SQUELCH_FFE_FFE_CAP_SEL_MASK		0xF
-#define USB3_SQUELCH_FFE_FFE_CAP_SEL_SHIFT		0
-#define USB3_SQUELCH_FFE_FFE_RES_SEL_MASK		(0x7 << 4)
-#define USB3_SQUELCH_FFE_FFE_RES_SEL_SHIFT		4
-#define USB3_SQUELCH_FFE_SQ_THRESH_IN_MASK		(0x1F << 8)
-#define USB3_SQUELCH_FFE_SQ_THRESH_IN_SHIFT		8
-
-#define USB3_GEN1_SET0_G1_TX_SLEW_CTRL_EN_MASK		(0x1 << 15)
-#define USB3_GEN1_SET0_G1_TX_EMPH_EN_SHIFT		11
-
-#define USB3_GEN2_SET0_G2_TX_AMP_MASK			(0x1F << 1)
-#define USB3_GEN2_SET0_G2_TX_AMP_SHIFT			1
-#define USB3_GEN2_SET0_G2_TX_AMP_ADJ_SHIFT		6
-#define USB3_GEN2_SET0_G2_TX_EMPH_AMP_MASK		(0xF << 7)
-#define USB3_GEN2_SET0_G2_TX_EMPH_AMP_SHIFT		7
-#define USB3_GEN2_SET0_G2_TX_EMPH_EN_MASK		(0x1 << 11)
-#define USB3_GEN2_SET0_G2_TX_EMPH_EN_SHIFT		11
-#define USB3_GEN2_SET0_G2_TX_SLEW_CTRL_EN_MASK		(0x1 << 15)
-#define USB3_GEN2_SET0_G2_TX_SLEW_CTRL_EN_SHIFT		15
-
-#define USB3_GEN2_SET1_G2_RX_SELMUPI_MASK		(0x7 << 0)
-#define USB3_GEN2_SET1_G2_RX_SELMUPI_SHIFT		0
-#define USB3_GEN2_SET1_G2_RX_SELMUPF_MASK		(0x7 << 3)
-#define USB3_GEN2_SET1_G2_RX_SELMUPF_SHIFT		3
-#define USB3_GEN2_SET1_G2_RX_SELMUFI_MASK		(0x3 << 6)
-#define USB3_GEN2_SET1_G2_RX_SELMUFI_SHIFT		6
-#define USB3_GEN2_SET1_G2_RX_SELMUFF_MASK		(0x3 << 8)
-#define USB3_GEN2_SET1_G2_RX_SELMUFF_SHIFT		8
-
-#define USB3_DIGITAL_LOOPBACK_EN_SEL_BITS_MASK		(0x3 << 10)
-#define USB3_DIGITAL_LOOPBACK_EN_SEL_BITS_SHIFT		10
-
-#define USB3_IMPEDANCE_CALI_CTRL_IMP_CAL_THR_MASK	(0x7 << 12)
-#define USB3_IMPEDANCE_CALI_CTRL_IMP_CAL_THR_SHIFT	12
-
-#define USB3_IMPEDANCE_TX_SSC_SSC_AMP_MASK		(0x3F << 0)
-#define USB3_IMPEDANCE_TX_SSC_SSC_AMP_SHIFT		0
-
-#define USB3_PHY_ISOLATION_MODE_PHY_GEN_RX_MASK		0xF
-#define USB3_PHY_ISOLATION_MODE_PHY_GEN_RX_SHIFT	0
-#define USB3_PHY_ISOLATION_MODE_PHY_GEN_TX_MASK		(0xF << 4)
-#define USB3_PHY_ISOLATION_MODE_PHY_GEN_TX_SHIFT	4
-#define USB3_PHY_ISOLATION_MODE_TX_DRV_IDLE_MASK	(0x1 << 8)
-
-#define USB3_TXDETRX_VTHSEL_MASK			(0x3 << 4)
-#define USB3_TXDETRX_VTHSEL_SHIFT			4
-
-#define USB3_TX_EMPPH_AMP_MASK				(0xF << 0)
-#define USB3_TX_EMPPH_AMP_SHIFT				0
-#define USB3_TX_EMPPH_EN_MASK				(0x1 << 6)
-#define USB3_TX_EMPPH_EN_SHIFT				6
-#define USB3_TX_EMPPH_AMP_FORCE_MASK			(0x1 << 7)
-#define USB3_TX_EMPPH_AMP_FORCE_SHIFT			7
-#define USB3_TX_EMPPH_PAR1_MASK				(0x1F << 8)
-#define USB3_TX_EMPPH_PAR1_SHIFT			8
-#define USB3_TX_EMPPH_PAR2_MASK				(0x1 << 13)
-#define USB3_TX_EMPPH_PAR2_SHIFT			13
-
-#define USB3_PIPE_SM_CTRL_PHY_INIT_DONE			15
-
-#endif /* __MV_U3D_PHY_H */
diff --git a/drivers/usb/phy/phy-ulpi.c b/drivers/usb/phy/phy-ulpi.c
index 17ea3f271bd..4e3877c329f 100644
--- a/drivers/usb/phy/phy-ulpi.c
+++ b/drivers/usb/phy/phy-ulpi.c
@@ -48,6 +48,7 @@ static struct ulpi_info ulpi_ids[] = {
 	ULPI_INFO(ULPI_ID(0x04cc, 0x1504), "NXP ISP1504"),
 	ULPI_INFO(ULPI_ID(0x0424, 0x0006), "SMSC USB331x"),
 	ULPI_INFO(ULPI_ID(0x0424, 0x0007), "SMSC USB3320"),
+	ULPI_INFO(ULPI_ID(0x0424, 0x0009), "SMSC USB334x"),
 	ULPI_INFO(ULPI_ID(0x0451, 0x1507), "TI TUSB1210"),
 };
 
diff --git a/drivers/usb/phy/phy.c b/drivers/usb/phy/phy.c
index 8afa813d690..36b6bce33b2 100644
--- a/drivers/usb/phy/phy.c
+++ b/drivers/usb/phy/phy.c
@@ -132,6 +132,9 @@ struct usb_phy *usb_get_phy(enum usb_phy_type type)
 	if (IS_ERR(phy) || !try_module_get(phy->dev->driver->owner)) {
 		pr_debug("PHY: unable to find transceiver of type %s\n",
 			usb_phy_type_string(type));
+		if (!IS_ERR(phy))
+			phy = ERR_PTR(-ENODEV);
+
 		goto err0;
 	}
 
diff --git a/drivers/usb/serial/bus.c b/drivers/usb/serial/bus.c
index 35a2373cde6..9374bd2aba2 100644
--- a/drivers/usb/serial/bus.c
+++ b/drivers/usb/serial/bus.c
@@ -97,13 +97,19 @@ static int usb_serial_device_remove(struct device *dev)
 	struct usb_serial_port *port;
 	int retval = 0;
 	int minor;
+	int autopm_err;
 
 	port = to_usb_serial_port(dev);
 	if (!port)
 		return -ENODEV;
 
-	/* make sure suspend/resume doesn't race against port_remove */
-	usb_autopm_get_interface(port->serial->interface);
+	/*
+	 * Make sure suspend/resume doesn't race against port_remove.
+	 *
+	 * Note that no further runtime PM callbacks will be made if
+	 * autopm_get fails.
+	 */
+	autopm_err = usb_autopm_get_interface(port->serial->interface);
 
 	minor = port->minor;
 	tty_unregister_device(usb_serial_tty_driver, minor);
@@ -117,7 +123,9 @@ static int usb_serial_device_remove(struct device *dev)
 	dev_info(dev, "%s converter now disconnected from ttyUSB%d\n",
 		 driver->description, minor);
 
-	usb_autopm_put_interface(port->serial->interface);
+	if (!autopm_err)
+		usb_autopm_put_interface(port->serial->interface);
+
 	return retval;
 }
 
diff --git a/drivers/usb/serial/cp210x.c b/drivers/usb/serial/cp210x.c
index 95fa1217afd..762e4a5f5ae 100644
--- a/drivers/usb/serial/cp210x.c
+++ b/drivers/usb/serial/cp210x.c
@@ -104,6 +104,7 @@ static const struct usb_device_id id_table[] = {
 	{ USB_DEVICE(0x10C4, 0x8218) }, /* Lipowsky Industrie Elektronik GmbH, HARP-1 */
 	{ USB_DEVICE(0x10C4, 0x822B) }, /* Modem EDGE(GSM) Comander 2 */
 	{ USB_DEVICE(0x10C4, 0x826B) }, /* Cygnal Integrated Products, Inc., Fasttrax GPS demonstration module */
+	{ USB_DEVICE(0x10C4, 0x8281) }, /* Nanotec Plug & Drive */
 	{ USB_DEVICE(0x10C4, 0x8293) }, /* Telegesis ETRX2USB */
 	{ USB_DEVICE(0x10C4, 0x82F9) }, /* Procyon AVS */
 	{ USB_DEVICE(0x10C4, 0x8341) }, /* Siemens MC35PU GPRS Modem */
diff --git a/drivers/usb/serial/ftdi_sio.c b/drivers/usb/serial/ftdi_sio.c
index 44ab1298680..7c6e1dedeb0 100644
--- a/drivers/usb/serial/ftdi_sio.c
+++ b/drivers/usb/serial/ftdi_sio.c
@@ -909,6 +909,39 @@ static const struct usb_device_id id_table_combined[] = {
 	{ USB_DEVICE(FTDI_VID, FTDI_Z3X_PID) },
 	/* Cressi Devices */
 	{ USB_DEVICE(FTDI_VID, FTDI_CRESSI_PID) },
+	/* Brainboxes Devices */
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_VX_001_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_VX_012_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_VX_023_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_VX_034_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_101_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_160_1_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_160_2_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_160_3_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_160_4_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_160_5_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_160_6_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_160_7_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_160_8_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_257_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_279_1_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_279_2_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_279_3_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_279_4_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_313_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_324_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_346_1_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_346_2_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_357_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_606_1_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_606_2_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_606_3_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_701_1_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_701_2_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_842_1_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_842_2_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_842_3_PID) },
+	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_842_4_PID) },
 	{ }					/* Terminating entry */
 };
 
diff --git a/drivers/usb/serial/ftdi_sio_ids.h b/drivers/usb/serial/ftdi_sio_ids.h
index e599fbfcde5..993c93df687 100644
--- a/drivers/usb/serial/ftdi_sio_ids.h
+++ b/drivers/usb/serial/ftdi_sio_ids.h
@@ -1326,3 +1326,40 @@
  * Manufacturer: Cressi
  */
 #define FTDI_CRESSI_PID		0x87d0
+
+/*
+ * Brainboxes devices
+ */
+#define BRAINBOXES_VID			0x05d1
+#define BRAINBOXES_VX_001_PID		0x1001 /* VX-001 ExpressCard 1 Port RS232 */
+#define BRAINBOXES_VX_012_PID		0x1002 /* VX-012 ExpressCard 2 Port RS232 */
+#define BRAINBOXES_VX_023_PID		0x1003 /* VX-023 ExpressCard 1 Port RS422/485 */
+#define BRAINBOXES_VX_034_PID		0x1004 /* VX-034 ExpressCard 2 Port RS422/485 */
+#define BRAINBOXES_US_101_PID		0x1011 /* US-101 1xRS232 */
+#define BRAINBOXES_US_324_PID		0x1013 /* US-324 1xRS422/485 1Mbaud */
+#define BRAINBOXES_US_606_1_PID		0x2001 /* US-606 6 Port RS232 Serial Port 1 and 2 */
+#define BRAINBOXES_US_606_2_PID		0x2002 /* US-606 6 Port RS232 Serial Port 3 and 4 */
+#define BRAINBOXES_US_606_3_PID		0x2003 /* US-606 6 Port RS232 Serial Port 4 and 6 */
+#define BRAINBOXES_US_701_1_PID		0x2011 /* US-701 4xRS232 1Mbaud Port 1 and 2 */
+#define BRAINBOXES_US_701_2_PID		0x2012 /* US-701 4xRS422 1Mbaud Port 3 and 4 */
+#define BRAINBOXES_US_279_1_PID		0x2021 /* US-279 8xRS422 1Mbaud Port 1 and 2 */
+#define BRAINBOXES_US_279_2_PID		0x2022 /* US-279 8xRS422 1Mbaud Port 3 and 4 */
+#define BRAINBOXES_US_279_3_PID		0x2023 /* US-279 8xRS422 1Mbaud Port 5 and 6 */
+#define BRAINBOXES_US_279_4_PID		0x2024 /* US-279 8xRS422 1Mbaud Port 7 and 8 */
+#define BRAINBOXES_US_346_1_PID		0x3011 /* US-346 4xRS422/485 1Mbaud Port 1 and 2 */
+#define BRAINBOXES_US_346_2_PID		0x3012 /* US-346 4xRS422/485 1Mbaud Port 3 and 4 */
+#define BRAINBOXES_US_257_PID		0x5001 /* US-257 2xRS232 1Mbaud */
+#define BRAINBOXES_US_313_PID		0x6001 /* US-313 2xRS422/485 1Mbaud */
+#define BRAINBOXES_US_357_PID		0x7001 /* US_357 1xRS232/422/485 */
+#define BRAINBOXES_US_842_1_PID		0x8001 /* US-842 8xRS422/485 1Mbaud Port 1 and 2 */
+#define BRAINBOXES_US_842_2_PID		0x8002 /* US-842 8xRS422/485 1Mbaud Port 3 and 4 */
+#define BRAINBOXES_US_842_3_PID		0x8003 /* US-842 8xRS422/485 1Mbaud Port 5 and 6 */
+#define BRAINBOXES_US_842_4_PID		0x8004 /* US-842 8xRS422/485 1Mbaud Port 7 and 8 */
+#define BRAINBOXES_US_160_1_PID		0x9001 /* US-160 16xRS232 1Mbaud Port 1 and 2 */
+#define BRAINBOXES_US_160_2_PID		0x9002 /* US-160 16xRS232 1Mbaud Port 3 and 4 */
+#define BRAINBOXES_US_160_3_PID		0x9003 /* US-160 16xRS232 1Mbaud Port 5 and 6 */
+#define BRAINBOXES_US_160_4_PID		0x9004 /* US-160 16xRS232 1Mbaud Port 7 and 8 */
+#define BRAINBOXES_US_160_5_PID		0x9005 /* US-160 16xRS232 1Mbaud Port 9 and 10 */
+#define BRAINBOXES_US_160_6_PID		0x9006 /* US-160 16xRS232 1Mbaud Port 11 and 12 */
+#define BRAINBOXES_US_160_7_PID		0x9007 /* US-160 16xRS232 1Mbaud Port 13 and 14 */
+#define BRAINBOXES_US_160_8_PID		0x9008 /* US-160 16xRS232 1Mbaud Port 15 and 16 */
diff --git a/drivers/usb/serial/io_ti.c b/drivers/usb/serial/io_ti.c
index a2db5be9c30..df90dae53eb 100644
--- a/drivers/usb/serial/io_ti.c
+++ b/drivers/usb/serial/io_ti.c
@@ -28,6 +28,7 @@
 #include <linux/spinlock.h>
 #include <linux/mutex.h>
 #include <linux/serial.h>
+#include <linux/swab.h>
 #include <linux/kfifo.h>
 #include <linux/ioctl.h>
 #include <linux/firmware.h>
@@ -280,7 +281,7 @@ static int read_download_mem(struct usb_device *dev, int start_address,
 {
 	int status = 0;
 	__u8 read_length;
-	__be16 be_start_address;
+	u16 be_start_address;
 
 	dev_dbg(&dev->dev, "%s - @ %x for %d\n", __func__, start_address, length);
 
@@ -296,10 +297,14 @@ static int read_download_mem(struct usb_device *dev, int start_address,
 		if (read_length > 1) {
 			dev_dbg(&dev->dev, "%s - @ %x for %d\n", __func__, start_address, read_length);
 		}
-		be_start_address = cpu_to_be16(start_address);
+		/*
+		 * NOTE: Must use swab as wIndex is sent in little-endian
+		 *       byte order regardless of host byte order.
+		 */
+		be_start_address = swab16((u16)start_address);
 		status = ti_vread_sync(dev, UMPC_MEMORY_READ,
 					(__u16)address_type,
-					(__force __u16)be_start_address,
+					be_start_address,
 					buffer, read_length);
 
 		if (status) {
@@ -394,7 +399,7 @@ static int write_i2c_mem(struct edgeport_serial *serial,
 	struct device *dev = &serial->serial->dev->dev;
 	int status = 0;
 	int write_length;
-	__be16 be_start_address;
+	u16 be_start_address;
 
 	/* We can only send a maximum of 1 aligned byte page at a time */
 
@@ -409,11 +414,16 @@ static int write_i2c_mem(struct edgeport_serial *serial,
 		__func__, start_address, write_length);
 	usb_serial_debug_data(dev, __func__, write_length, buffer);
 
-	/* Write first page */
-	be_start_address = cpu_to_be16(start_address);
+	/*
+	 * Write first page.
+	 *
+	 * NOTE: Must use swab as wIndex is sent in little-endian byte order
+	 *       regardless of host byte order.
+	 */
+	be_start_address = swab16((u16)start_address);
 	status = ti_vsend_sync(serial->serial->dev,
 				UMPC_MEMORY_WRITE, (__u16)address_type,
-				(__force __u16)be_start_address,
+				be_start_address,
 				buffer,	write_length);
 	if (status) {
 		dev_dbg(dev, "%s - ERROR %d\n", __func__, status);
@@ -436,11 +446,16 @@ static int write_i2c_mem(struct edgeport_serial *serial,
 			__func__, start_address, write_length);
 		usb_serial_debug_data(dev, __func__, write_length, buffer);
 
-		/* Write next page */
-		be_start_address = cpu_to_be16(start_address);
+		/*
+		 * Write next page.
+		 *
+		 * NOTE: Must use swab as wIndex is sent in little-endian byte
+		 *       order regardless of host byte order.
+		 */
+		be_start_address = swab16((u16)start_address);
 		status = ti_vsend_sync(serial->serial->dev, UMPC_MEMORY_WRITE,
 				(__u16)address_type,
-				(__force __u16)be_start_address,
+				be_start_address,
 				buffer, write_length);
 		if (status) {
 			dev_err(dev, "%s - ERROR %d\n", __func__, status);
@@ -585,8 +600,8 @@ static int get_descriptor_addr(struct edgeport_serial *serial,
 		if (rom_desc->Type == desc_type)
 			return start_address;
 
-		start_address = start_address + sizeof(struct ti_i2c_desc)
-							+ rom_desc->Size;
+		start_address = start_address + sizeof(struct ti_i2c_desc) +
+						le16_to_cpu(rom_desc->Size);
 
 	} while ((start_address < TI_MAX_I2C_SIZE) && rom_desc->Type);
 
@@ -599,7 +614,7 @@ static int valid_csum(struct ti_i2c_desc *rom_desc, __u8 *buffer)
 	__u16 i;
 	__u8 cs = 0;
 
-	for (i = 0; i < rom_desc->Size; i++)
+	for (i = 0; i < le16_to_cpu(rom_desc->Size); i++)
 		cs = (__u8)(cs + buffer[i]);
 
 	if (cs != rom_desc->CheckSum) {
@@ -650,7 +665,7 @@ static int check_i2c_image(struct edgeport_serial *serial)
 			break;
 
 		if ((start_address + sizeof(struct ti_i2c_desc) +
-					rom_desc->Size) > TI_MAX_I2C_SIZE) {
+			le16_to_cpu(rom_desc->Size)) > TI_MAX_I2C_SIZE) {
 			status = -ENODEV;
 			dev_dbg(dev, "%s - structure too big, erroring out.\n", __func__);
 			break;
@@ -665,7 +680,8 @@ static int check_i2c_image(struct edgeport_serial *serial)
 			/* Read the descriptor data */
 			status = read_rom(serial, start_address +
 						sizeof(struct ti_i2c_desc),
-						rom_desc->Size, buffer);
+						le16_to_cpu(rom_desc->Size),
+						buffer);
 			if (status)
 				break;
 
@@ -674,7 +690,7 @@ static int check_i2c_image(struct edgeport_serial *serial)
 				break;
 		}
 		start_address = start_address + sizeof(struct ti_i2c_desc) +
-								rom_desc->Size;
+						le16_to_cpu(rom_desc->Size);
 
 	} while ((rom_desc->Type != I2C_DESC_TYPE_ION) &&
 				(start_address < TI_MAX_I2C_SIZE));
@@ -712,7 +728,7 @@ static int get_manuf_info(struct edgeport_serial *serial, __u8 *buffer)
 
 	/* Read the descriptor data */
 	status = read_rom(serial, start_address+sizeof(struct ti_i2c_desc),
-						rom_desc->Size, buffer);
+					le16_to_cpu(rom_desc->Size), buffer);
 	if (status)
 		goto exit;
 
diff --git a/drivers/usb/serial/keyspan.c b/drivers/usb/serial/keyspan.c
index d3acaead5a8..93cb7cebda6 100644
--- a/drivers/usb/serial/keyspan.c
+++ b/drivers/usb/serial/keyspan.c
@@ -1535,14 +1535,14 @@ static int keyspan_usa26_send_setup(struct usb_serial *serial,
 
 	this_urb = p_priv->outcont_urb;
 
-	dev_dbg(&port->dev, "%s - endpoint %d\n", __func__, usb_pipeendpoint(this_urb->pipe));
-
 		/* Make sure we have an urb then send the message */
 	if (this_urb == NULL) {
 		dev_dbg(&port->dev, "%s - oops no urb.\n", __func__);
 		return -1;
 	}
 
+	dev_dbg(&port->dev, "%s - endpoint %d\n", __func__, usb_pipeendpoint(this_urb->pipe));
+
 	/* Save reset port val for resend.
 	   Don't overwrite resend for open/close condition. */
 	if ((reset_port + 1) > p_priv->resend_cont)
diff --git a/drivers/usb/serial/kobil_sct.c b/drivers/usb/serial/kobil_sct.c
index fee242387f5..078f9ed419c 100644
--- a/drivers/usb/serial/kobil_sct.c
+++ b/drivers/usb/serial/kobil_sct.c
@@ -215,13 +215,13 @@ static int kobil_open(struct tty_struct *tty, struct usb_serial_port *port)
 			priv->device_type == KOBIL_ADAPTER_K_PRODUCT_ID) {
 		/* Setting Baudrate, Parity and Stopbits */
 		result = usb_control_msg(port->serial->dev,
-			  usb_rcvctrlpipe(port->serial->dev, 0),
+			  usb_sndctrlpipe(port->serial->dev, 0),
 			  SUSBCRequest_SetBaudRateParityAndStopBits,
 			  USB_TYPE_VENDOR | USB_RECIP_ENDPOINT | USB_DIR_OUT,
 			  SUSBCR_SBR_9600 | SUSBCR_SPASB_EvenParity |
 							SUSBCR_SPASB_1StopBit,
 			  0,
-			  transfer_buffer,
+			  NULL,
 			  0,
 			  KOBIL_TIMEOUT
 		);
@@ -229,12 +229,12 @@ static int kobil_open(struct tty_struct *tty, struct usb_serial_port *port)
 
 		/* reset all queues */
 		result = usb_control_msg(port->serial->dev,
-			  usb_rcvctrlpipe(port->serial->dev, 0),
+			  usb_sndctrlpipe(port->serial->dev, 0),
 			  SUSBCRequest_Misc,
 			  USB_TYPE_VENDOR | USB_RECIP_ENDPOINT | USB_DIR_OUT,
 			  SUSBCR_MSC_ResetAllQueues,
 			  0,
-			  transfer_buffer,
+			  NULL,
 			  0,
 			  KOBIL_TIMEOUT
 		);
@@ -445,12 +445,12 @@ static int kobil_tiocmset(struct tty_struct *tty,
 		else
 			dev_dbg(dev, "%s - Clearing DTR\n", __func__);
 		result = usb_control_msg(port->serial->dev,
-			  usb_rcvctrlpipe(port->serial->dev, 0),
+			  usb_sndctrlpipe(port->serial->dev, 0),
 			  SUSBCRequest_SetStatusLinesOrQueues,
 			  USB_TYPE_VENDOR | USB_RECIP_ENDPOINT | USB_DIR_OUT,
 			  ((dtr != 0) ? SUSBCR_SSL_SETDTR : SUSBCR_SSL_CLRDTR),
 			  0,
-			  transfer_buffer,
+			  NULL,
 			  0,
 			  KOBIL_TIMEOUT);
 	} else {
@@ -459,12 +459,12 @@ static int kobil_tiocmset(struct tty_struct *tty,
 		else
 			dev_dbg(dev, "%s - Clearing RTS\n", __func__);
 		result = usb_control_msg(port->serial->dev,
-			usb_rcvctrlpipe(port->serial->dev, 0),
+			usb_sndctrlpipe(port->serial->dev, 0),
 			SUSBCRequest_SetStatusLinesOrQueues,
 			USB_TYPE_VENDOR | USB_RECIP_ENDPOINT | USB_DIR_OUT,
 			((rts != 0) ? SUSBCR_SSL_SETRTS : SUSBCR_SSL_CLRRTS),
 			0,
-			transfer_buffer,
+			NULL,
 			0,
 			KOBIL_TIMEOUT);
 	}
@@ -514,7 +514,7 @@ static void kobil_set_termios(struct tty_struct *tty,
 	tty_encode_baud_rate(tty, speed, speed);
 
 	result = usb_control_msg(port->serial->dev,
-		  usb_rcvctrlpipe(port->serial->dev, 0),
+		  usb_sndctrlpipe(port->serial->dev, 0),
 		  SUSBCRequest_SetBaudRateParityAndStopBits,
 		  USB_TYPE_VENDOR | USB_RECIP_ENDPOINT | USB_DIR_OUT,
 		  urb_val,
@@ -546,12 +546,12 @@ static int kobil_ioctl(struct tty_struct *tty,
 			return -ENOBUFS;
 
 		result = usb_control_msg(port->serial->dev,
-			  usb_rcvctrlpipe(port->serial->dev, 0),
+			  usb_sndctrlpipe(port->serial->dev, 0),
 			  SUSBCRequest_Misc,
 			  USB_TYPE_VENDOR | USB_RECIP_ENDPOINT | USB_DIR_OUT,
 			  SUSBCR_MSC_ResetAllQueues,
 			  0,
-			  NULL, /* transfer_buffer, */
+			  NULL,
 			  0,
 			  KOBIL_TIMEOUT
 			);
diff --git a/drivers/usb/serial/option.c b/drivers/usb/serial/option.c
index 68fc9fe6593..51e30740b2f 100644
--- a/drivers/usb/serial/option.c
+++ b/drivers/usb/serial/option.c
@@ -234,8 +234,31 @@ static void option_instat_callback(struct urb *urb);
 #define QUALCOMM_VENDOR_ID			0x05C6
 
 #define CMOTECH_VENDOR_ID			0x16d8
-#define CMOTECH_PRODUCT_6008			0x6008
-#define CMOTECH_PRODUCT_6280			0x6280
+#define CMOTECH_PRODUCT_6001			0x6001
+#define CMOTECH_PRODUCT_CMU_300			0x6002
+#define CMOTECH_PRODUCT_6003			0x6003
+#define CMOTECH_PRODUCT_6004			0x6004
+#define CMOTECH_PRODUCT_6005			0x6005
+#define CMOTECH_PRODUCT_CGU_628A		0x6006
+#define CMOTECH_PRODUCT_CHE_628S		0x6007
+#define CMOTECH_PRODUCT_CMU_301			0x6008
+#define CMOTECH_PRODUCT_CHU_628			0x6280
+#define CMOTECH_PRODUCT_CHU_628S		0x6281
+#define CMOTECH_PRODUCT_CDU_680			0x6803
+#define CMOTECH_PRODUCT_CDU_685A		0x6804
+#define CMOTECH_PRODUCT_CHU_720S		0x7001
+#define CMOTECH_PRODUCT_7002			0x7002
+#define CMOTECH_PRODUCT_CHU_629K		0x7003
+#define CMOTECH_PRODUCT_7004			0x7004
+#define CMOTECH_PRODUCT_7005			0x7005
+#define CMOTECH_PRODUCT_CGU_629			0x7006
+#define CMOTECH_PRODUCT_CHU_629S		0x700a
+#define CMOTECH_PRODUCT_CHU_720I		0x7211
+#define CMOTECH_PRODUCT_7212			0x7212
+#define CMOTECH_PRODUCT_7213			0x7213
+#define CMOTECH_PRODUCT_7251			0x7251
+#define CMOTECH_PRODUCT_7252			0x7252
+#define CMOTECH_PRODUCT_7253			0x7253
 
 #define TELIT_VENDOR_ID				0x1bc7
 #define TELIT_PRODUCT_UC864E			0x1003
@@ -243,6 +266,7 @@ static void option_instat_callback(struct urb *urb);
 #define TELIT_PRODUCT_CC864_DUAL		0x1005
 #define TELIT_PRODUCT_CC864_SINGLE		0x1006
 #define TELIT_PRODUCT_DE910_DUAL		0x1010
+#define TELIT_PRODUCT_UE910_V2			0x1012
 #define TELIT_PRODUCT_LE920			0x1200
 
 /* ZTE PRODUCTS */
@@ -286,6 +310,7 @@ static void option_instat_callback(struct urb *urb);
 #define ALCATEL_PRODUCT_X060S_X200		0x0000
 #define ALCATEL_PRODUCT_X220_X500D		0x0017
 #define ALCATEL_PRODUCT_L100V			0x011e
+#define ALCATEL_PRODUCT_L800MA			0x0203
 
 #define PIRELLI_VENDOR_ID			0x1266
 #define PIRELLI_PRODUCT_C100_1			0x1002
@@ -348,6 +373,7 @@ static void option_instat_callback(struct urb *urb);
 #define OLIVETTI_PRODUCT_OLICARD100		0xc000
 #define OLIVETTI_PRODUCT_OLICARD145		0xc003
 #define OLIVETTI_PRODUCT_OLICARD200		0xc005
+#define OLIVETTI_PRODUCT_OLICARD500		0xc00b
 
 /* Celot products */
 #define CELOT_VENDOR_ID				0x211f
@@ -501,6 +527,10 @@ static const struct option_blacklist_info huawei_cdc12_blacklist = {
 	.reserved = BIT(1) | BIT(2),
 };
 
+static const struct option_blacklist_info net_intf0_blacklist = {
+	.reserved = BIT(0),
+};
+
 static const struct option_blacklist_info net_intf1_blacklist = {
 	.reserved = BIT(1),
 };
@@ -1034,13 +1064,53 @@ static const struct usb_device_id option_ids[] = {
 	{ USB_DEVICE(QUALCOMM_VENDOR_ID, 0x6613)}, /* Onda H600/ZTE MF330 */
 	{ USB_DEVICE(QUALCOMM_VENDOR_ID, 0x0023)}, /* ONYX 3G device */
 	{ USB_DEVICE(QUALCOMM_VENDOR_ID, 0x9000)}, /* SIMCom SIM5218 */
-	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_6280) }, /* BP3-USB & BP3-EXT HSDPA */
-	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_6008) },
+	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_6001) },
+	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_CMU_300) },
+	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_6003),
+	  .driver_info = (kernel_ulong_t)&net_intf0_blacklist },
+	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_6004) },
+	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_6005) },
+	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_CGU_628A) },
+	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_CHE_628S),
+	  .driver_info = (kernel_ulong_t)&net_intf0_blacklist },
+	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_CMU_301),
+	  .driver_info = (kernel_ulong_t)&net_intf0_blacklist },
+	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_CHU_628),
+	  .driver_info = (kernel_ulong_t)&net_intf0_blacklist },
+	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_CHU_628S) },
+	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_CDU_680) },
+	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_CDU_685A) },
+	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_CHU_720S),
+	  .driver_info = (kernel_ulong_t)&net_intf0_blacklist },
+	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_7002),
+	  .driver_info = (kernel_ulong_t)&net_intf0_blacklist },
+	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_CHU_629K),
+	  .driver_info = (kernel_ulong_t)&net_intf4_blacklist },
+	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_7004),
+	  .driver_info = (kernel_ulong_t)&net_intf3_blacklist },
+	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_7005) },
+	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_CGU_629),
+	  .driver_info = (kernel_ulong_t)&net_intf5_blacklist },
+	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_CHU_629S),
+	  .driver_info = (kernel_ulong_t)&net_intf4_blacklist },
+	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_CHU_720I),
+	  .driver_info = (kernel_ulong_t)&net_intf0_blacklist },
+	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_7212),
+	  .driver_info = (kernel_ulong_t)&net_intf0_blacklist },
+	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_7213),
+	  .driver_info = (kernel_ulong_t)&net_intf0_blacklist },
+	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_7251),
+	  .driver_info = (kernel_ulong_t)&net_intf1_blacklist },
+	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_7252),
+	  .driver_info = (kernel_ulong_t)&net_intf1_blacklist },
+	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_7253),
+	  .driver_info = (kernel_ulong_t)&net_intf1_blacklist },
 	{ USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_UC864E) },
 	{ USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_UC864G) },
 	{ USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_CC864_DUAL) },
 	{ USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_CC864_SINGLE) },
 	{ USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_DE910_DUAL) },
+	{ USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_UE910_V2) },
 	{ USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_LE920),
 		.driver_info = (kernel_ulong_t)&telit_le920_blacklist },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_MF622, 0xff, 0xff, 0xff) }, /* ZTE WCDMA products */
@@ -1498,6 +1568,8 @@ static const struct usb_device_id option_ids[] = {
 	  .driver_info = (kernel_ulong_t)&net_intf5_blacklist },
 	{ USB_DEVICE(ALCATEL_VENDOR_ID, ALCATEL_PRODUCT_L100V),
 	  .driver_info = (kernel_ulong_t)&net_intf4_blacklist },
+	{ USB_DEVICE(ALCATEL_VENDOR_ID, ALCATEL_PRODUCT_L800MA),
+	  .driver_info = (kernel_ulong_t)&net_intf2_blacklist },
 	{ USB_DEVICE(AIRPLUS_VENDOR_ID, AIRPLUS_PRODUCT_MCD650) },
 	{ USB_DEVICE(TLAYTECH_VENDOR_ID, TLAYTECH_PRODUCT_TEU800) },
 	{ USB_DEVICE(LONGCHEER_VENDOR_ID, FOUR_G_SYSTEMS_PRODUCT_W14),
@@ -1543,6 +1615,9 @@ static const struct usb_device_id option_ids[] = {
 	{ USB_DEVICE(OLIVETTI_VENDOR_ID, OLIVETTI_PRODUCT_OLICARD200),
 		.driver_info = (kernel_ulong_t)&net_intf6_blacklist
 	},
+	{ USB_DEVICE(OLIVETTI_VENDOR_ID, OLIVETTI_PRODUCT_OLICARD500),
+		.driver_info = (kernel_ulong_t)&net_intf4_blacklist
+	},
 	{ USB_DEVICE(CELOT_VENDOR_ID, CELOT_PRODUCT_CT680M) }, /* CT-650 CDMA 450 1xEVDO 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.*/
 	{ USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CEM600) },
@@ -1654,7 +1729,6 @@ static struct usb_serial_driver option_1port_device = {
 	.write             = usb_wwan_write,
 	.write_room        = usb_wwan_write_room,
 	.chars_in_buffer   = usb_wwan_chars_in_buffer,
-	.set_termios       = usb_wwan_set_termios,
 	.tiocmget          = usb_wwan_tiocmget,
 	.tiocmset          = usb_wwan_tiocmset,
 	.ioctl             = usb_wwan_ioctl,
@@ -1829,6 +1903,7 @@ static void option_instat_callback(struct urb *urb)
 
 	/* Resubmit urb so we continue receiving IRQ data */
 	if (status != -ESHUTDOWN && status != -ENOENT) {
+		usb_mark_last_busy(port->serial->dev);
 		err = usb_submit_urb(urb, GFP_ATOMIC);
 		if (err)
 			dev_dbg(dev, "%s: resubmit intr urb failed. (%d)\n",
@@ -1848,6 +1923,7 @@ static int option_send_setup(struct usb_serial_port *port)
 	struct option_private *priv = intfdata->private;
 	struct usb_wwan_port_private *portdata;
 	int val = 0;
+	int res;
 
 	portdata = usb_get_serial_port_data(port);
 
@@ -1856,9 +1932,17 @@ static int option_send_setup(struct usb_serial_port *port)
 	if (portdata->rts_state)
 		val |= 0x02;
 
-	return usb_control_msg(serial->dev, usb_rcvctrlpipe(serial->dev, 0),
+	res = usb_autopm_get_interface(serial->interface);
+	if (res)
+		return res;
+
+	res = usb_control_msg(serial->dev, usb_sndctrlpipe(serial->dev, 0),
 				0x22, 0x21, val, priv->bInterfaceNumber, NULL,
 				0, USB_CTRL_SET_TIMEOUT);
+
+	usb_autopm_put_interface(serial->interface);
+
+	return res;
 }
 
 MODULE_AUTHOR(DRIVER_AUTHOR);
diff --git a/drivers/usb/serial/pl2303.c b/drivers/usb/serial/pl2303.c
index 2e22fc22c38..b3d5a35c0d4 100644
--- a/drivers/usb/serial/pl2303.c
+++ b/drivers/usb/serial/pl2303.c
@@ -83,6 +83,9 @@ static const struct usb_device_id id_table[] = {
 	{ USB_DEVICE(YCCABLE_VENDOR_ID, YCCABLE_PRODUCT_ID) },
 	{ USB_DEVICE(SUPERIAL_VENDOR_ID, SUPERIAL_PRODUCT_ID) },
 	{ USB_DEVICE(HP_VENDOR_ID, HP_LD220_PRODUCT_ID) },
+	{ USB_DEVICE(HP_VENDOR_ID, HP_LD960_PRODUCT_ID) },
+	{ USB_DEVICE(HP_VENDOR_ID, HP_LCM220_PRODUCT_ID) },
+	{ USB_DEVICE(HP_VENDOR_ID, HP_LCM960_PRODUCT_ID) },
 	{ USB_DEVICE(CRESSI_VENDOR_ID, CRESSI_EDY_PRODUCT_ID) },
 	{ USB_DEVICE(ZEAGLE_VENDOR_ID, ZEAGLE_N2ITION3_PRODUCT_ID) },
 	{ USB_DEVICE(SONY_VENDOR_ID, SONY_QN3USB_PRODUCT_ID) },
diff --git a/drivers/usb/serial/pl2303.h b/drivers/usb/serial/pl2303.h
index c38b8c00c06..42bc082896a 100644
--- a/drivers/usb/serial/pl2303.h
+++ b/drivers/usb/serial/pl2303.h
@@ -121,8 +121,11 @@
 #define SUPERIAL_VENDOR_ID	0x5372
 #define SUPERIAL_PRODUCT_ID	0x2303
 
-/* Hewlett-Packard LD220-HP POS Pole Display */
+/* Hewlett-Packard POS Pole Displays */
 #define HP_VENDOR_ID		0x03f0
+#define HP_LD960_PRODUCT_ID	0x0b39
+#define HP_LCM220_PRODUCT_ID	0x3139
+#define HP_LCM960_PRODUCT_ID	0x3239
 #define HP_LD220_PRODUCT_ID	0x3524
 
 /* Cressi Edy (diving computer) PC interface */
diff --git a/drivers/usb/serial/qcserial.c b/drivers/usb/serial/qcserial.c
index 968a40201e5..b2aa003bf41 100644
--- a/drivers/usb/serial/qcserial.c
+++ b/drivers/usb/serial/qcserial.c
@@ -22,8 +22,17 @@
 #define DRIVER_AUTHOR "Qualcomm Inc"
 #define DRIVER_DESC "Qualcomm USB Serial driver"
 
+/* standard device layouts supported by this driver */
+enum qcserial_layouts {
+	QCSERIAL_G2K = 0,	/* Gobi 2000 */
+	QCSERIAL_G1K = 1,	/* Gobi 1000 */
+	QCSERIAL_SWI = 2,	/* Sierra Wireless */
+};
+
 #define DEVICE_G1K(v, p) \
-	USB_DEVICE(v, p), .driver_info = 1
+	USB_DEVICE(v, p), .driver_info = QCSERIAL_G1K
+#define DEVICE_SWI(v, p) \
+	USB_DEVICE(v, p), .driver_info = QCSERIAL_SWI
 
 static const struct usb_device_id id_table[] = {
 	/* Gobi 1000 devices */
@@ -126,22 +135,27 @@ static const struct usb_device_id id_table[] = {
 	{USB_DEVICE(0x12D1, 0x14F1)},	/* Sony Gobi 3000 Composite */
 	{USB_DEVICE(0x0AF0, 0x8120)},	/* Option GTM681W */
 
-	/* non Gobi Qualcomm serial devices */
-	{USB_DEVICE_INTERFACE_NUMBER(0x0f3d, 0x68a2, 0)},	/* Sierra Wireless MC7700 Device Management */
-	{USB_DEVICE_INTERFACE_NUMBER(0x0f3d, 0x68a2, 2)},	/* Sierra Wireless MC7700 NMEA */
-	{USB_DEVICE_INTERFACE_NUMBER(0x0f3d, 0x68a2, 3)},	/* Sierra Wireless MC7700 Modem */
-	{USB_DEVICE_INTERFACE_NUMBER(0x114f, 0x68a2, 0)},	/* Sierra Wireless MC7750 Device Management */
-	{USB_DEVICE_INTERFACE_NUMBER(0x114f, 0x68a2, 2)},	/* Sierra Wireless MC7750 NMEA */
-	{USB_DEVICE_INTERFACE_NUMBER(0x114f, 0x68a2, 3)},	/* Sierra Wireless MC7750 Modem */
-	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x68a2, 0)},	/* Sierra Wireless MC7710 Device Management */
-	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x68a2, 2)},	/* Sierra Wireless MC7710 NMEA */
-	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x68a2, 3)},	/* Sierra Wireless MC7710 Modem */
-	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x901c, 0)},	/* Sierra Wireless EM7700 Device Management */
-	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x901c, 2)},	/* Sierra Wireless EM7700 NMEA */
-	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x901c, 3)},	/* Sierra Wireless EM7700 Modem */
-	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x9051, 0)},	/* Netgear AirCard 340U Device Management */
-	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x9051, 2)},	/* Netgear AirCard 340U NMEA */
-	{USB_DEVICE_INTERFACE_NUMBER(0x1199, 0x9051, 3)},	/* Netgear AirCard 340U Modem */
+	/* non-Gobi Sierra Wireless devices */
+	{DEVICE_SWI(0x0f3d, 0x68a2)},	/* Sierra Wireless MC7700 */
+	{DEVICE_SWI(0x114f, 0x68a2)},	/* Sierra Wireless MC7750 */
+	{DEVICE_SWI(0x1199, 0x68a2)},	/* Sierra Wireless MC7710 */
+	{DEVICE_SWI(0x1199, 0x68c0)},	/* Sierra Wireless MC73xx */
+	{DEVICE_SWI(0x1199, 0x901c)},	/* Sierra Wireless EM7700 */
+	{DEVICE_SWI(0x1199, 0x901f)},	/* Sierra Wireless EM7355 */
+	{DEVICE_SWI(0x1199, 0x9040)},	/* Sierra Wireless Modem */
+	{DEVICE_SWI(0x1199, 0x9041)},	/* Sierra Wireless MC7305/MC7355 */
+	{DEVICE_SWI(0x1199, 0x9051)},	/* Netgear AirCard 340U */
+	{DEVICE_SWI(0x1199, 0x9053)},	/* Sierra Wireless Modem */
+	{DEVICE_SWI(0x1199, 0x9054)},	/* Sierra Wireless Modem */
+	{DEVICE_SWI(0x1199, 0x9055)},	/* Netgear AirCard 341U */
+	{DEVICE_SWI(0x1199, 0x9056)},	/* Sierra Wireless Modem */
+	{DEVICE_SWI(0x1199, 0x9060)},	/* Sierra Wireless Modem */
+	{DEVICE_SWI(0x1199, 0x9061)},	/* Sierra Wireless Modem */
+	{DEVICE_SWI(0x413c, 0x81a2)},	/* Dell Wireless 5806 Gobi(TM) 4G LTE Mobile Broadband Card */
+	{DEVICE_SWI(0x413c, 0x81a3)},	/* Dell Wireless 5570 HSPA+ (42Mbps) Mobile Broadband Card */
+	{DEVICE_SWI(0x413c, 0x81a4)},	/* Dell Wireless 5570e HSPA+ (42Mbps) Mobile Broadband Card */
+	{DEVICE_SWI(0x413c, 0x81a8)},	/* Dell Wireless 5808 Gobi(TM) 4G LTE Mobile Broadband Card */
+	{DEVICE_SWI(0x413c, 0x81a9)},	/* Dell Wireless 5808e Gobi(TM) 4G LTE Mobile Broadband Card */
 
 	{ }				/* Terminating entry */
 };
@@ -154,11 +168,8 @@ static int qcprobe(struct usb_serial *serial, const struct usb_device_id *id)
 	int retval = -ENODEV;
 	__u8 nintf;
 	__u8 ifnum;
-	bool is_gobi1k = id->driver_info ? true : false;
 	int altsetting = -1;
 
-	dev_dbg(dev, "Is Gobi 1000 = %d\n", is_gobi1k);
-
 	nintf = serial->dev->actconfig->desc.bNumInterfaces;
 	dev_dbg(dev, "Num Interfaces = %d\n", nintf);
 	ifnum = intf->desc.bInterfaceNumber;
@@ -186,32 +197,29 @@ static int qcprobe(struct usb_serial *serial, const struct usb_device_id *id)
 
 	}
 
-	/* allow any number of interfaces when doing direct interface match */
-	if (id->match_flags & USB_DEVICE_ID_MATCH_INT_NUMBER) {
-		dev_dbg(dev, "Generic Qualcomm serial interface found\n");
-		altsetting = 0;
-		goto done;
-	}
-
-	if (nintf < 3 || nintf > 4) {
-		dev_err(dev, "unknown number of interfaces: %d\n", nintf);
-		goto done;
-	}
-
 	/* default to enabling interface */
 	altsetting = 0;
 
-	/* Composite mode; don't bind to the QMI/net interface as that
+	/*
+	 * Composite mode; don't bind to the QMI/net interface as that
 	 * gets handled by other drivers.
 	 */
 
-	if (is_gobi1k) {
-		/* Gobi 1K USB layout:
+	switch (id->driver_info) {
+	case QCSERIAL_G1K:
+		/*
+		 * Gobi 1K USB layout:
 		 * 0: DM/DIAG (use libqcdm from ModemManager for communication)
 		 * 1: serial port (doesn't respond)
 		 * 2: AT-capable modem port
 		 * 3: QMI/net
 		 */
+		if (nintf < 3 || nintf > 4) {
+			dev_err(dev, "unknown number of interfaces: %d\n", nintf);
+			altsetting = -1;
+			goto done;
+		}
+
 		if (ifnum == 0) {
 			dev_dbg(dev, "Gobi 1K DM/DIAG interface found\n");
 			altsetting = 1;
@@ -219,13 +227,21 @@ static int qcprobe(struct usb_serial *serial, const struct usb_device_id *id)
 			dev_dbg(dev, "Modem port found\n");
 		else
 			altsetting = -1;
-	} else {
-		/* Gobi 2K+ USB layout:
+		break;
+	case QCSERIAL_G2K:
+		/*
+		 * Gobi 2K+ USB layout:
 		 * 0: QMI/net
 		 * 1: DM/DIAG (use libqcdm from ModemManager for communication)
 		 * 2: AT-capable modem port
 		 * 3: NMEA
 		 */
+		if (nintf < 3 || nintf > 4) {
+			dev_err(dev, "unknown number of interfaces: %d\n", nintf);
+			altsetting = -1;
+			goto done;
+		}
+
 		switch (ifnum) {
 		case 0:
 			/* Don't claim the QMI/net interface */
@@ -246,6 +262,35 @@ static int qcprobe(struct usb_serial *serial, const struct usb_device_id *id)
 			dev_dbg(dev, "Gobi 2K+ NMEA GPS interface found\n");
 			break;
 		}
+		break;
+	case QCSERIAL_SWI:
+		/*
+		 * Sierra Wireless layout:
+		 * 0: DM/DIAG (use libqcdm from ModemManager for communication)
+		 * 2: NMEA
+		 * 3: AT-capable modem port
+		 * 8: QMI/net
+		 */
+		switch (ifnum) {
+		case 0:
+			dev_dbg(dev, "DM/DIAG interface found\n");
+			break;
+		case 2:
+			dev_dbg(dev, "NMEA GPS interface found\n");
+			break;
+		case 3:
+			dev_dbg(dev, "Modem port found\n");
+			break;
+		default:
+			/* don't claim any unsupported interface */
+			altsetting = -1;
+			break;
+		}
+		break;
+	default:
+		dev_err(dev, "unsupported device layout type: %lu\n",
+			id->driver_info);
+		break;
 	}
 
 done:
diff --git a/drivers/usb/serial/sierra.c b/drivers/usb/serial/sierra.c
index a9eb6221a81..6f7f01eb556 100644
--- a/drivers/usb/serial/sierra.c
+++ b/drivers/usb/serial/sierra.c
@@ -58,6 +58,7 @@ struct sierra_intf_private {
 	spinlock_t susp_lock;
 	unsigned int suspended:1;
 	int in_flight;
+	unsigned int open_ports;
 };
 
 static int sierra_set_power_state(struct usb_device *udev, __u16 swiState)
@@ -291,7 +292,6 @@ static const struct usb_device_id id_table[] = {
 	{ USB_DEVICE(0x0f3d, 0x68A3), 	/* Airprime/Sierra Wireless Direct IP modems */
 	  .driver_info = (kernel_ulong_t)&direct_ip_interface_blacklist
 	},
-       { USB_DEVICE(0x413C, 0x08133) }, /* Dell Computer Corp. Wireless 5720 VZW Mobile Broadband (EVDO Rev-A) Minicard GPS Port */
 
 	{ }
 };
@@ -316,7 +316,6 @@ struct sierra_port_private {
 	int dsr_state;
 	int dcd_state;
 	int ri_state;
-	unsigned int opened:1;
 };
 
 static int sierra_send_setup(struct usb_serial_port *port)
@@ -365,20 +364,13 @@ static int sierra_send_setup(struct usb_serial_port *port)
 	if (retval < 0)
 		return retval;
 
-	retval = usb_control_msg(serial->dev, usb_rcvctrlpipe(serial->dev, 0),
+	retval = usb_control_msg(serial->dev, usb_sndctrlpipe(serial->dev, 0),
 		0x22, 0x21, val, interface, NULL, 0, USB_CTRL_SET_TIMEOUT);
 	usb_autopm_put_interface(serial->interface);
 
 	return retval;
 }
 
-static void sierra_set_termios(struct tty_struct *tty,
-		struct usb_serial_port *port, struct ktermios *old_termios)
-{
-	tty_termios_copy_hw(&tty->termios, old_termios);
-	sierra_send_setup(port);
-}
-
 static int sierra_tiocmget(struct tty_struct *tty)
 {
 	struct usb_serial_port *port = tty->driver_data;
@@ -419,9 +411,7 @@ static int sierra_tiocmset(struct tty_struct *tty,
 
 static void sierra_release_urb(struct urb *urb)
 {
-	struct usb_serial_port *port;
 	if (urb) {
-		port = urb->context;
 		kfree(urb->transfer_buffer);
 		usb_free_urb(urb);
 	}
@@ -434,7 +424,7 @@ static void sierra_outdat_callback(struct urb *urb)
 	struct sierra_intf_private *intfdata;
 	int status = urb->status;
 
-	intfdata = port->serial->private;
+	intfdata = usb_get_serial_data(port->serial);
 
 	/* free up the transfer buffer, as usb_free_urb() does not do this */
 	kfree(urb->transfer_buffer);
@@ -471,7 +461,7 @@ static int sierra_write(struct tty_struct *tty, struct usb_serial_port *port,
 		return 0;
 
 	portdata = usb_get_serial_port_data(port);
-	intfdata = serial->private;
+	intfdata = usb_get_serial_data(serial);
 
 	dev_dbg(&port->dev, "%s: write (%zd bytes)\n", __func__, writesize);
 	spin_lock_irqsave(&portdata->lock, flags);
@@ -675,6 +665,23 @@ static int sierra_write_room(struct tty_struct *tty)
 	return 2048;
 }
 
+static int sierra_chars_in_buffer(struct tty_struct *tty)
+{
+	struct usb_serial_port *port = tty->driver_data;
+	struct sierra_port_private *portdata = usb_get_serial_port_data(port);
+	unsigned long flags;
+	int chars;
+
+	/* NOTE: This overcounts somewhat. */
+	spin_lock_irqsave(&portdata->lock, flags);
+	chars = portdata->outstanding_urbs * MAX_TRANSFER;
+	spin_unlock_irqrestore(&portdata->lock, flags);
+
+	dev_dbg(&port->dev, "%s - %d\n", __func__, chars);
+
+	return chars;
+}
+
 static void sierra_stop_rx_urbs(struct usb_serial_port *port)
 {
 	int i;
@@ -730,9 +737,6 @@ static struct urb *sierra_setup_urb(struct usb_serial *serial, int endpoint,
 	struct urb	*urb;
 	u8		*buf;
 
-	if (endpoint == -1)
-		return NULL;
-
 	urb = usb_alloc_urb(0, mem_flags);
 	if (!urb)
 		return NULL;
@@ -759,40 +763,48 @@ static void sierra_close(struct usb_serial_port *port)
 	int i;
 	struct usb_serial *serial = port->serial;
 	struct sierra_port_private *portdata;
-	struct sierra_intf_private *intfdata = port->serial->private;
+	struct sierra_intf_private *intfdata = usb_get_serial_data(serial);
+	struct urb *urb;
 
 	portdata = usb_get_serial_port_data(port);
 
-	portdata->rts_state = 0;
-	portdata->dtr_state = 0;
-
-	mutex_lock(&serial->disc_mutex);
-	if (!serial->disconnected) {
+	/*
+	 * Need to take susp_lock to make sure port is not already being
+	 * resumed, but no need to hold it due to ASYNC_INITIALIZED.
+	 */
+	spin_lock_irq(&intfdata->susp_lock);
+	if (--intfdata->open_ports == 0)
 		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);
+	spin_unlock_irq(&intfdata->susp_lock);
 
+	for (;;) {
+		urb = usb_get_from_anchor(&portdata->delayed);
+		if (!urb)
+			break;
+		kfree(urb->transfer_buffer);
+		usb_free_urb(urb);
+		usb_autopm_put_interface_async(serial->interface);
+		spin_lock(&portdata->lock);
+		portdata->outstanding_urbs--;
+		spin_unlock(&portdata->lock);
 	}
-	mutex_unlock(&serial->disc_mutex);
-	spin_lock_irq(&intfdata->susp_lock);
-	portdata->opened = 0;
-	spin_unlock_irq(&intfdata->susp_lock);
 
 	sierra_stop_rx_urbs(port);
+	usb_kill_anchored_urbs(&portdata->active);
+
 	for (i = 0; i < portdata->num_in_urbs; i++) {
 		sierra_release_urb(portdata->in_urbs[i]);
 		portdata->in_urbs[i] = NULL;
 	}
+
+	usb_autopm_get_interface_no_resume(serial->interface);
 }
 
 static int sierra_open(struct tty_struct *tty, struct usb_serial_port *port)
 {
 	struct sierra_port_private *portdata;
 	struct usb_serial *serial = port->serial;
-	struct sierra_intf_private *intfdata = serial->private;
+	struct sierra_intf_private *intfdata = usb_get_serial_data(serial);
 	int i;
 	int err;
 	int endpoint;
@@ -800,11 +812,6 @@ static int sierra_open(struct tty_struct *tty, struct usb_serial_port *port)
 
 	portdata = usb_get_serial_port_data(port);
 
-	/* Set some sane defaults */
-	portdata->rts_state = 1;
-	portdata->dtr_state = 1;
-
-
 	endpoint = port->bulk_in_endpointAddress;
 	for (i = 0; i < portdata->num_in_urbs; i++) {
 		urb = sierra_setup_urb(serial, endpoint, USB_DIR_IN, port,
@@ -817,23 +824,26 @@ static int sierra_open(struct tty_struct *tty, struct usb_serial_port *port)
 			usb_sndbulkpipe(serial->dev, endpoint) | USB_DIR_IN);
 
 	err = sierra_submit_rx_urbs(port, GFP_KERNEL);
-	if (err) {
-		/* get rid of everything as in close */
-		sierra_close(port);
-		/* restore balance for autopm */
-		if (!serial->disconnected)
-			usb_autopm_put_interface(serial->interface);
-		return err;
-	}
-	sierra_send_setup(port);
+	if (err)
+		goto err_submit;
 
-	serial->interface->needs_remote_wakeup = 1;
 	spin_lock_irq(&intfdata->susp_lock);
-	portdata->opened = 1;
+	if (++intfdata->open_ports == 1)
+		serial->interface->needs_remote_wakeup = 1;
 	spin_unlock_irq(&intfdata->susp_lock);
 	usb_autopm_put_interface(serial->interface);
 
 	return 0;
+
+err_submit:
+	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;
+	}
+
+	return err;
 }
 
 
@@ -929,6 +939,7 @@ static int sierra_port_remove(struct usb_serial_port *port)
 	struct sierra_port_private *portdata;
 
 	portdata = usb_get_serial_port_data(port);
+	usb_set_serial_port_data(port, NULL);
 	kfree(portdata);
 
 	return 0;
@@ -945,6 +956,8 @@ static void stop_read_write_urbs(struct usb_serial *serial)
 	for (i = 0; i < serial->num_ports; ++i) {
 		port = serial->port[i];
 		portdata = usb_get_serial_port_data(port);
+		if (!portdata)
+			continue;
 		sierra_stop_rx_urbs(port);
 		usb_kill_anchored_urbs(&portdata->active);
 	}
@@ -952,58 +965,84 @@ static void stop_read_write_urbs(struct usb_serial *serial)
 
 static int sierra_suspend(struct usb_serial *serial, pm_message_t message)
 {
-	struct sierra_intf_private *intfdata;
-	int b;
+	struct sierra_intf_private *intfdata = usb_get_serial_data(serial);
 
+	spin_lock_irq(&intfdata->susp_lock);
 	if (PMSG_IS_AUTO(message)) {
-		intfdata = serial->private;
-		spin_lock_irq(&intfdata->susp_lock);
-		b = intfdata->in_flight;
-
-		if (b) {
+		if (intfdata->in_flight) {
 			spin_unlock_irq(&intfdata->susp_lock);
 			return -EBUSY;
-		} else {
-			intfdata->suspended = 1;
-			spin_unlock_irq(&intfdata->susp_lock);
 		}
 	}
+	intfdata->suspended = 1;
+	spin_unlock_irq(&intfdata->susp_lock);
+
 	stop_read_write_urbs(serial);
 
 	return 0;
 }
 
+/* Caller must hold susp_lock. */
+static int sierra_submit_delayed_urbs(struct usb_serial_port *port)
+{
+	struct sierra_port_private *portdata = usb_get_serial_port_data(port);
+	struct sierra_intf_private *intfdata;
+	struct urb *urb;
+	int ec = 0;
+	int err;
+
+	intfdata = usb_get_serial_data(port->serial);
+
+	for (;;) {
+		urb = usb_get_from_anchor(&portdata->delayed);
+		if (!urb)
+			break;
+
+		usb_anchor_urb(urb, &portdata->active);
+		intfdata->in_flight++;
+		err = usb_submit_urb(urb, GFP_ATOMIC);
+		if (err) {
+			dev_err(&port->dev, "%s - submit urb failed: %d",
+					__func__, err);
+			ec++;
+			intfdata->in_flight--;
+			usb_unanchor_urb(urb);
+			kfree(urb->transfer_buffer);
+			usb_free_urb(urb);
+
+			spin_lock(&portdata->lock);
+			portdata->outstanding_urbs--;
+			spin_unlock(&portdata->lock);
+		}
+	}
+
+	if (ec)
+		return -EIO;
+
+	return 0;
+}
+
 static int sierra_resume(struct usb_serial *serial)
 {
 	struct usb_serial_port *port;
-	struct sierra_intf_private *intfdata = serial->private;
-	struct sierra_port_private *portdata;
-	struct urb *urb;
+	struct sierra_intf_private *intfdata = usb_get_serial_data(serial);
 	int ec = 0;
 	int i, err;
 
 	spin_lock_irq(&intfdata->susp_lock);
 	for (i = 0; i < serial->num_ports; i++) {
 		port = serial->port[i];
-		portdata = usb_get_serial_port_data(port);
 
-		while ((urb = usb_get_from_anchor(&portdata->delayed))) {
-			usb_anchor_urb(urb, &portdata->active);
-			intfdata->in_flight++;
-			err = usb_submit_urb(urb, GFP_ATOMIC);
-			if (err < 0) {
-				intfdata->in_flight--;
-				usb_unanchor_urb(urb);
-				usb_scuttle_anchored_urbs(&portdata->delayed);
-				break;
-			}
-		}
+		if (!test_bit(ASYNCB_INITIALIZED, &port->port.flags))
+			continue;
 
-		if (portdata->opened) {
-			err = sierra_submit_rx_urbs(port, GFP_ATOMIC);
-			if (err)
-				ec++;
-		}
+		err = sierra_submit_delayed_urbs(port);
+		if (err)
+			ec++;
+
+		err = sierra_submit_rx_urbs(port, GFP_ATOMIC);
+		if (err)
+			ec++;
 	}
 	intfdata->suspended = 0;
 	spin_unlock_irq(&intfdata->susp_lock);
@@ -1030,7 +1069,7 @@ static struct usb_serial_driver sierra_device = {
 	.dtr_rts	   = sierra_dtr_rts,
 	.write             = sierra_write,
 	.write_room        = sierra_write_room,
-	.set_termios       = sierra_set_termios,
+	.chars_in_buffer   = sierra_chars_in_buffer,
 	.tiocmget          = sierra_tiocmget,
 	.tiocmset          = sierra_tiocmset,
 	.attach            = sierra_startup,
diff --git a/drivers/usb/serial/usb-serial.c b/drivers/usb/serial/usb-serial.c
index 81fc0dfcfdc..02de3110fe9 100644
--- a/drivers/usb/serial/usb-serial.c
+++ b/drivers/usb/serial/usb-serial.c
@@ -1060,6 +1060,7 @@ static void usb_serial_disconnect(struct usb_interface *interface)
 	struct usb_serial *serial = usb_get_intfdata(interface);
 	struct device *dev = &interface->dev;
 	struct usb_serial_port *port;
+	struct tty_struct *tty;
 
 	usb_serial_console_disconnect(serial);
 
@@ -1070,18 +1071,16 @@ static void usb_serial_disconnect(struct usb_interface *interface)
 
 	for (i = 0; i < serial->num_ports; ++i) {
 		port = serial->port[i];
-		if (port) {
-			struct tty_struct *tty = tty_port_tty_get(&port->port);
-			if (tty) {
-				tty_vhangup(tty);
-				tty_kref_put(tty);
-			}
-			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);
+		tty = tty_port_tty_get(&port->port);
+		if (tty) {
+			tty_vhangup(tty);
+			tty_kref_put(tty);
 		}
+		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);
 	}
 	if (serial->type->disconnect)
 		serial->type->disconnect(serial);
@@ -1094,7 +1093,6 @@ static void usb_serial_disconnect(struct usb_interface *interface)
 int usb_serial_suspend(struct usb_interface *intf, pm_message_t message)
 {
 	struct usb_serial *serial = usb_get_intfdata(intf);
-	struct usb_serial_port *port;
 	int i, r = 0;
 
 	serial->suspending = 1;
@@ -1112,12 +1110,8 @@ 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)
-			usb_serial_port_poison_urbs(port);
-	}
-
+	for (i = 0; i < serial->num_ports; ++i)
+		usb_serial_port_poison_urbs(serial->port[i]);
 err_out:
 	return r;
 }
@@ -1125,14 +1119,10 @@ 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);
-	}
+	for (i = 0; i < serial->num_ports; ++i)
+		usb_serial_port_unpoison_urbs(serial->port[i]);
 }
 
 int usb_serial_resume(struct usb_interface *intf)
@@ -1347,10 +1337,12 @@ static int usb_serial_register(struct usb_serial_driver *driver)
 static void usb_serial_deregister(struct usb_serial_driver *device)
 {
 	pr_info("USB Serial deregistering driver %s\n", device->description);
+
 	mutex_lock(&table_lock);
 	list_del(&device->driver_list);
-	usb_serial_bus_deregister(device);
 	mutex_unlock(&table_lock);
+
+	usb_serial_bus_deregister(device);
 }
 
 /**
diff --git a/drivers/usb/serial/usb-wwan.h b/drivers/usb/serial/usb-wwan.h
index 684739b8efd..f22dff58b58 100644
--- a/drivers/usb/serial/usb-wwan.h
+++ b/drivers/usb/serial/usb-wwan.h
@@ -11,15 +11,11 @@ extern void usb_wwan_close(struct usb_serial_port *port);
 extern int usb_wwan_port_probe(struct usb_serial_port *port);
 extern int usb_wwan_port_remove(struct usb_serial_port *port);
 extern int usb_wwan_write_room(struct tty_struct *tty);
-extern void usb_wwan_set_termios(struct tty_struct *tty,
-				 struct usb_serial_port *port,
-				 struct ktermios *old);
 extern int usb_wwan_tiocmget(struct tty_struct *tty);
 extern int usb_wwan_tiocmset(struct tty_struct *tty,
 			     unsigned int set, unsigned int clear);
 extern int usb_wwan_ioctl(struct tty_struct *tty,
 			  unsigned int cmd, unsigned long arg);
-extern int usb_wwan_send_setup(struct usb_serial_port *port);
 extern int usb_wwan_write(struct tty_struct *tty, struct usb_serial_port *port,
 			  const unsigned char *buf, int count);
 extern int usb_wwan_chars_in_buffer(struct tty_struct *tty);
@@ -39,6 +35,7 @@ struct usb_wwan_intf_private {
 	spinlock_t susp_lock;
 	unsigned int suspended:1;
 	int in_flight;
+	unsigned int open_ports;
 	int (*send_setup) (struct usb_serial_port *port);
 	void *private;
 };
@@ -51,7 +48,6 @@ struct usb_wwan_port_private {
 	struct urb *out_urbs[N_OUT_URB];
 	u8 *out_buffer[N_OUT_URB];
 	unsigned long out_busy;	/* Bit vector of URBs in use */
-	int opened;
 	struct usb_anchor delayed;
 
 	/* Settings for the port */
diff --git a/drivers/usb/serial/usb_wwan.c b/drivers/usb/serial/usb_wwan.c
index f1ec1680e82..2f805cb386a 100644
--- a/drivers/usb/serial/usb_wwan.c
+++ b/drivers/usb/serial/usb_wwan.c
@@ -41,7 +41,7 @@ void usb_wwan_dtr_rts(struct usb_serial_port *port, int on)
 	struct usb_wwan_port_private *portdata;
 	struct usb_wwan_intf_private *intfdata;
 
-	intfdata = port->serial->private;
+	intfdata = usb_get_serial_data(port->serial);
 
 	if (!intfdata->send_setup)
 		return;
@@ -55,20 +55,6 @@ void usb_wwan_dtr_rts(struct usb_serial_port *port, int on)
 }
 EXPORT_SYMBOL(usb_wwan_dtr_rts);
 
-void usb_wwan_set_termios(struct tty_struct *tty,
-			  struct usb_serial_port *port,
-			  struct ktermios *old_termios)
-{
-	struct usb_wwan_intf_private *intfdata = port->serial->private;
-
-	/* Doesn't support option setting */
-	tty_termios_copy_hw(&tty->termios, old_termios);
-
-	if (intfdata->send_setup)
-		intfdata->send_setup(port);
-}
-EXPORT_SYMBOL(usb_wwan_set_termios);
-
 int usb_wwan_tiocmget(struct tty_struct *tty)
 {
 	struct usb_serial_port *port = tty->driver_data;
@@ -96,7 +82,7 @@ int usb_wwan_tiocmset(struct tty_struct *tty,
 	struct usb_wwan_intf_private *intfdata;
 
 	portdata = usb_get_serial_port_data(port);
-	intfdata = port->serial->private;
+	intfdata = usb_get_serial_data(port->serial);
 
 	if (!intfdata->send_setup)
 		return -EINVAL;
@@ -192,7 +178,6 @@ int usb_wwan_ioctl(struct tty_struct *tty,
 }
 EXPORT_SYMBOL(usb_wwan_ioctl);
 
-/* Write */
 int usb_wwan_write(struct tty_struct *tty, struct usb_serial_port *port,
 		   const unsigned char *buf, int count)
 {
@@ -205,7 +190,7 @@ int usb_wwan_write(struct tty_struct *tty, struct usb_serial_port *port,
 	unsigned long flags;
 
 	portdata = usb_get_serial_port_data(port);
-	intfdata = port->serial->private;
+	intfdata = usb_get_serial_data(port->serial);
 
 	dev_dbg(&port->dev, "%s: write (%d chars)\n", __func__, count);
 
@@ -228,8 +213,10 @@ int usb_wwan_write(struct tty_struct *tty, struct usb_serial_port *port,
 			usb_pipeendpoint(this_urb->pipe), i);
 
 		err = usb_autopm_get_interface_async(port->serial->interface);
-		if (err < 0)
+		if (err < 0) {
+			clear_bit(i, &portdata->out_busy);
 			break;
+		}
 
 		/* send the data */
 		memcpy(this_urb->transfer_buffer, buf, todo);
@@ -244,9 +231,9 @@ int usb_wwan_write(struct tty_struct *tty, struct usb_serial_port *port,
 			spin_unlock_irqrestore(&intfdata->susp_lock, flags);
 			err = usb_submit_urb(this_urb, GFP_ATOMIC);
 			if (err) {
-				dev_dbg(&port->dev,
-					"usb_submit_urb %p (write bulk) failed (%d)\n",
-					this_urb, err);
+				dev_err(&port->dev,
+					"%s: submit urb %d failed: %d\n",
+					__func__, i, err);
 				clear_bit(i, &portdata->out_busy);
 				spin_lock_irqsave(&intfdata->susp_lock, flags);
 				intfdata->in_flight--;
@@ -314,7 +301,7 @@ static void usb_wwan_outdat_callback(struct urb *urb)
 	int i;
 
 	port = urb->context;
-	intfdata = port->serial->private;
+	intfdata = usb_get_serial_data(port->serial);
 
 	usb_serial_port_softint(port);
 	usb_autopm_put_interface_async(port->serial->interface);
@@ -384,7 +371,15 @@ int usb_wwan_open(struct tty_struct *tty, struct usb_serial_port *port)
 	struct urb *urb;
 
 	portdata = usb_get_serial_port_data(port);
-	intfdata = serial->private;
+	intfdata = usb_get_serial_data(serial);
+
+	if (port->interrupt_in_urb) {
+		err = usb_submit_urb(port->interrupt_in_urb, GFP_KERNEL);
+		if (err) {
+			dev_err(&port->dev, "%s: submit int urb failed: %d\n",
+				__func__, err);
+		}
+	}
 
 	/* Start reading from the IN endpoint */
 	for (i = 0; i < N_IN_URB; i++) {
@@ -393,17 +388,15 @@ int usb_wwan_open(struct tty_struct *tty, struct usb_serial_port *port)
 			continue;
 		err = usb_submit_urb(urb, GFP_KERNEL);
 		if (err) {
-			dev_dbg(&port->dev, "%s: submit urb %d failed (%d) %d\n",
-				__func__, i, err, urb->transfer_buffer_length);
+			dev_err(&port->dev,
+				"%s: submit read urb %d failed: %d\n",
+				__func__, i, err);
 		}
 	}
 
-	if (intfdata->send_setup)
-		intfdata->send_setup(port);
-
-	serial->interface->needs_remote_wakeup = 1;
 	spin_lock_irq(&intfdata->susp_lock);
-	portdata->opened = 1;
+	if (++intfdata->open_ports == 1)
+		serial->interface->needs_remote_wakeup = 1;
 	spin_unlock_irq(&intfdata->susp_lock);
 	/* this balances a get in the generic USB serial code */
 	usb_autopm_put_interface(serial->interface);
@@ -412,32 +405,56 @@ int usb_wwan_open(struct tty_struct *tty, struct usb_serial_port *port)
 }
 EXPORT_SYMBOL(usb_wwan_open);
 
+static void unbusy_queued_urb(struct urb *urb,
+					struct usb_wwan_port_private *portdata)
+{
+	int i;
+
+	for (i = 0; i < N_OUT_URB; i++) {
+		if (urb == portdata->out_urbs[i]) {
+			clear_bit(i, &portdata->out_busy);
+			break;
+		}
+	}
+}
+
 void usb_wwan_close(struct usb_serial_port *port)
 {
 	int i;
 	struct usb_serial *serial = port->serial;
 	struct usb_wwan_port_private *portdata;
-	struct usb_wwan_intf_private *intfdata = port->serial->private;
+	struct usb_wwan_intf_private *intfdata = usb_get_serial_data(serial);
+	struct urb *urb;
 
 	portdata = usb_get_serial_port_data(port);
 
-	/* Stop reading/writing urbs */
+	/*
+	 * Need to take susp_lock to make sure port is not already being
+	 * resumed, but no need to hold it due to ASYNC_INITIALIZED.
+	 */
 	spin_lock_irq(&intfdata->susp_lock);
-	portdata->opened = 0;
+	if (--intfdata->open_ports == 0)
+		serial->interface->needs_remote_wakeup = 0;
 	spin_unlock_irq(&intfdata->susp_lock);
 
+	for (;;) {
+		urb = usb_get_from_anchor(&portdata->delayed);
+		if (!urb)
+			break;
+		unbusy_queued_urb(urb, portdata);
+		usb_autopm_put_interface_async(serial->interface);
+	}
+
 	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]);
+	usb_kill_urb(port->interrupt_in_urb);
 
-	/* 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);
 
-/* Helper functions used by usb_wwan_setup_urbs */
 static struct urb *usb_wwan_setup_urb(struct usb_serial_port *port,
 				      int endpoint,
 				      int dir, void *ctx, char *buf, int len,
@@ -450,7 +467,6 @@ static struct urb *usb_wwan_setup_urb(struct usb_serial_port *port,
 	if (!urb)
 		return NULL;
 
-	/* Fill URB using supplied data. */
 	usb_fill_bulk_urb(urb, serial->dev,
 			  usb_sndbulkpipe(serial->dev, endpoint) | dir,
 			  buf, len, callback, ctx);
@@ -463,9 +479,11 @@ int usb_wwan_port_probe(struct usb_serial_port *port)
 	struct usb_wwan_port_private *portdata;
 	struct urb *urb;
 	u8 *buffer;
-	int err;
 	int i;
 
+	if (!port->bulk_in_size || !port->bulk_out_size)
+		return -ENODEV;
+
 	portdata = kzalloc(sizeof(*portdata), GFP_KERNEL);
 	if (!portdata)
 		return -ENOMEM;
@@ -473,9 +491,6 @@ int usb_wwan_port_probe(struct usb_serial_port *port)
 	init_usb_anchor(&portdata->delayed);
 
 	for (i = 0; i < N_IN_URB; i++) {
-		if (!port->bulk_in_size)
-			break;
-
 		buffer = (u8 *)__get_free_page(GFP_KERNEL);
 		if (!buffer)
 			goto bail_out_error;
@@ -489,9 +504,6 @@ int usb_wwan_port_probe(struct usb_serial_port *port)
 	}
 
 	for (i = 0; i < N_OUT_URB; i++) {
-		if (!port->bulk_out_size)
-			break;
-
 		buffer = kmalloc(OUT_BUFLEN, GFP_KERNEL);
 		if (!buffer)
 			goto bail_out_error2;
@@ -506,13 +518,6 @@ int usb_wwan_port_probe(struct usb_serial_port *port)
 
 	usb_set_serial_port_data(port, portdata);
 
-	if (port->interrupt_in_urb) {
-		err = usb_submit_urb(port->interrupt_in_urb, GFP_KERNEL);
-		if (err)
-			dev_dbg(&port->dev, "%s: submit irq_in urb failed %d\n",
-				__func__, err);
-	}
-
 	return 0;
 
 bail_out_error2:
@@ -539,32 +544,28 @@ int usb_wwan_port_remove(struct usb_serial_port *port)
 	portdata = usb_get_serial_port_data(port);
 	usb_set_serial_port_data(port, NULL);
 
-	/* Stop reading/writing urbs and free them */
 	for (i = 0; i < N_IN_URB; i++) {
-		usb_kill_urb(portdata->in_urbs[i]);
 		usb_free_urb(portdata->in_urbs[i]);
 		free_page((unsigned long)portdata->in_buffer[i]);
 	}
 	for (i = 0; i < N_OUT_URB; i++) {
-		usb_kill_urb(portdata->out_urbs[i]);
 		usb_free_urb(portdata->out_urbs[i]);
 		kfree(portdata->out_buffer[i]);
 	}
 
-	/* Now free port private data */
 	kfree(portdata);
+
 	return 0;
 }
 EXPORT_SYMBOL(usb_wwan_port_remove);
 
 #ifdef CONFIG_PM
-static void stop_read_write_urbs(struct usb_serial *serial)
+static void stop_urbs(struct usb_serial *serial)
 {
 	int i, j;
 	struct usb_serial_port *port;
 	struct usb_wwan_port_private *portdata;
 
-	/* Stop reading/writing urbs */
 	for (i = 0; i < serial->num_ports; ++i) {
 		port = serial->port[i];
 		portdata = usb_get_serial_port_data(port);
@@ -574,123 +575,117 @@ static void stop_read_write_urbs(struct usb_serial *serial)
 			usb_kill_urb(portdata->in_urbs[j]);
 		for (j = 0; j < N_OUT_URB; j++)
 			usb_kill_urb(portdata->out_urbs[j]);
+		usb_kill_urb(port->interrupt_in_urb);
 	}
 }
 
 int usb_wwan_suspend(struct usb_serial *serial, pm_message_t message)
 {
-	struct usb_wwan_intf_private *intfdata = serial->private;
-	int b;
+	struct usb_wwan_intf_private *intfdata = usb_get_serial_data(serial);
 
+	spin_lock_irq(&intfdata->susp_lock);
 	if (PMSG_IS_AUTO(message)) {
-		spin_lock_irq(&intfdata->susp_lock);
-		b = intfdata->in_flight;
-		spin_unlock_irq(&intfdata->susp_lock);
-
-		if (b)
+		if (intfdata->in_flight) {
+			spin_unlock_irq(&intfdata->susp_lock);
 			return -EBUSY;
+		}
 	}
-
-	spin_lock_irq(&intfdata->susp_lock);
 	intfdata->suspended = 1;
 	spin_unlock_irq(&intfdata->susp_lock);
-	stop_read_write_urbs(serial);
+
+	stop_urbs(serial);
 
 	return 0;
 }
 EXPORT_SYMBOL(usb_wwan_suspend);
 
-static void unbusy_queued_urb(struct urb *urb, struct usb_wwan_port_private *portdata)
-{
-	int i;
-
-	for (i = 0; i < N_OUT_URB; i++) {
-		if (urb == portdata->out_urbs[i]) {
-			clear_bit(i, &portdata->out_busy);
-			break;
-		}
-	}
-}
-
-static void play_delayed(struct usb_serial_port *port)
+/* Caller must hold susp_lock. */
+static int usb_wwan_submit_delayed_urbs(struct usb_serial_port *port)
 {
-	struct usb_wwan_intf_private *data;
+	struct usb_serial *serial = port->serial;
+	struct usb_wwan_intf_private *data = usb_get_serial_data(serial);
 	struct usb_wwan_port_private *portdata;
 	struct urb *urb;
+	int err_count = 0;
 	int err;
 
 	portdata = usb_get_serial_port_data(port);
-	data = port->serial->private;
-	while ((urb = usb_get_from_anchor(&portdata->delayed))) {
-		err = usb_submit_urb(urb, GFP_ATOMIC);
-		if (!err) {
-			data->in_flight++;
-		} else {
-			/* we have to throw away the rest */
-			do {
-				unbusy_queued_urb(urb, portdata);
-				usb_autopm_put_interface_no_suspend(port->serial->interface);
-			} while ((urb = usb_get_from_anchor(&portdata->delayed)));
+
+	for (;;) {
+		urb = usb_get_from_anchor(&portdata->delayed);
+		if (!urb)
 			break;
+
+		err = usb_submit_urb(urb, GFP_ATOMIC);
+		if (err) {
+			dev_err(&port->dev, "%s: submit urb failed: %d\n",
+					__func__, err);
+			err_count++;
+			unbusy_queued_urb(urb, portdata);
+			usb_autopm_put_interface_async(serial->interface);
+			continue;
 		}
+		data->in_flight++;
 	}
+
+	if (err_count)
+		return -EIO;
+
+	return 0;
 }
 
 int usb_wwan_resume(struct usb_serial *serial)
 {
 	int i, j;
 	struct usb_serial_port *port;
-	struct usb_wwan_intf_private *intfdata = serial->private;
+	struct usb_wwan_intf_private *intfdata = usb_get_serial_data(serial);
 	struct usb_wwan_port_private *portdata;
 	struct urb *urb;
-	int err = 0;
+	int err;
+	int err_count = 0;
 
-	/* get the interrupt URBs resubmitted unconditionally */
+	spin_lock_irq(&intfdata->susp_lock);
 	for (i = 0; i < serial->num_ports; i++) {
 		port = serial->port[i];
-		if (!port->interrupt_in_urb) {
-			dev_dbg(&port->dev, "%s: No interrupt URB for port\n", __func__);
+
+		if (!test_bit(ASYNCB_INITIALIZED, &port->port.flags))
 			continue;
-		}
-		err = usb_submit_urb(port->interrupt_in_urb, GFP_NOIO);
-		dev_dbg(&port->dev, "Submitted interrupt URB for port (result %d)\n", err);
-		if (err < 0) {
-			dev_err(&port->dev, "%s: Error %d for interrupt URB\n",
-				__func__, err);
-			goto err_out;
-		}
-	}
 
-	for (i = 0; i < serial->num_ports; i++) {
-		/* walk all ports */
-		port = serial->port[i];
 		portdata = usb_get_serial_port_data(port);
 
-		/* skip closed ports */
-		spin_lock_irq(&intfdata->susp_lock);
-		if (!portdata || !portdata->opened) {
-			spin_unlock_irq(&intfdata->susp_lock);
-			continue;
+		if (port->interrupt_in_urb) {
+			err = usb_submit_urb(port->interrupt_in_urb,
+					GFP_ATOMIC);
+			if (err) {
+				dev_err(&port->dev,
+					"%s: submit int urb failed: %d\n",
+					__func__, err);
+				err_count++;
+			}
 		}
 
+		err = usb_wwan_submit_delayed_urbs(port);
+		if (err)
+			err_count++;
+
 		for (j = 0; j < N_IN_URB; j++) {
 			urb = portdata->in_urbs[j];
 			err = usb_submit_urb(urb, GFP_ATOMIC);
 			if (err < 0) {
-				dev_err(&port->dev, "%s: Error %d for bulk URB %d\n",
-					__func__, err, i);
-				spin_unlock_irq(&intfdata->susp_lock);
-				goto err_out;
+				dev_err(&port->dev,
+					"%s: submit read urb %d failed: %d\n",
+					__func__, i, err);
+				err_count++;
 			}
 		}
-		play_delayed(port);
-		spin_unlock_irq(&intfdata->susp_lock);
 	}
-	spin_lock_irq(&intfdata->susp_lock);
 	intfdata->suspended = 0;
 	spin_unlock_irq(&intfdata->susp_lock);
-err_out:
-	return err;
+
+	if (err_count)
+		return -EIO;
+
+	return 0;
 }
 EXPORT_SYMBOL(usb_wwan_resume);
 #endif
diff --git a/drivers/usb/storage/ene_ub6250.c b/drivers/usb/storage/ene_ub6250.c
index 1bfc9a6cab5..ef6efb55dc3 100644
--- a/drivers/usb/storage/ene_ub6250.c
+++ b/drivers/usb/storage/ene_ub6250.c
@@ -1928,11 +1928,10 @@ static int ene_load_bincode(struct us_data *us, unsigned char flag)
 		usb_stor_dbg(us, "load firmware %s failed\n", fw_name);
 		goto nofw;
 	}
-	buf = kmalloc(sd_fw->size, GFP_KERNEL);
+	buf = kmemdup(sd_fw->data, sd_fw->size, GFP_KERNEL);
 	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);
 	bcb->DataTransferLength = sd_fw->size;
diff --git a/drivers/usb/storage/shuttle_usbat.c b/drivers/usb/storage/shuttle_usbat.c
index 4ef2a80728f..008d805c3d2 100644
--- a/drivers/usb/storage/shuttle_usbat.c
+++ b/drivers/usb/storage/shuttle_usbat.c
@@ -1851,7 +1851,7 @@ static int usbat_probe(struct usb_interface *intf,
 	us->transport_name = "Shuttle USBAT";
 	us->transport = usbat_flash_transport;
 	us->transport_reset = usb_stor_CB_reset;
-	us->max_lun = 1;
+	us->max_lun = 0;
 
 	result = usb_stor_probe2(us);
 	return result;
diff --git a/drivers/usb/storage/uas.c b/drivers/usb/storage/uas.c
index a7ac97cc594..511b2295316 100644
--- a/drivers/usb/storage/uas.c
+++ b/drivers/usb/storage/uas.c
@@ -137,7 +137,7 @@ static void uas_do_work(struct work_struct *work)
 		if (!(cmdinfo->state & IS_IN_WORK_LIST))
 			continue;
 
-		err = uas_submit_urbs(cmnd, cmnd->device->hostdata, GFP_NOIO);
+		err = uas_submit_urbs(cmnd, cmnd->device->hostdata, GFP_ATOMIC);
 		if (!err)
 			cmdinfo->state &= ~IS_IN_WORK_LIST;
 		else
@@ -803,7 +803,7 @@ static int uas_eh_task_mgmt(struct scsi_cmnd *cmnd,
 
 	devinfo->running_task = 1;
 	memset(&devinfo->response, 0, sizeof(devinfo->response));
-	sense_urb = uas_submit_sense_urb(cmnd, GFP_NOIO,
+	sense_urb = uas_submit_sense_urb(cmnd, GFP_ATOMIC,
 					 devinfo->use_streams ? tag : 0);
 	if (!sense_urb) {
 		shost_printk(KERN_INFO, shost,
@@ -813,7 +813,7 @@ static int uas_eh_task_mgmt(struct scsi_cmnd *cmnd,
 		spin_unlock_irqrestore(&devinfo->lock, flags);
 		return FAILED;
 	}
-	if (uas_submit_task_urb(cmnd, GFP_NOIO, function, tag)) {
+	if (uas_submit_task_urb(cmnd, GFP_ATOMIC, function, tag)) {
 		shost_printk(KERN_INFO, shost,
 			     "%s: %s: submit task mgmt urb failed\n",
 			     __func__, fname);
@@ -1030,7 +1030,7 @@ static int uas_configure_endpoints(struct uas_dev_info *devinfo)
 		devinfo->use_streams = 0;
 	} else {
 		devinfo->qdepth = usb_alloc_streams(devinfo->intf, eps + 1,
-						    3, 256, GFP_KERNEL);
+						    3, 256, GFP_NOIO);
 		if (devinfo->qdepth < 0)
 			return devinfo->qdepth;
 		devinfo->use_streams = 1;
@@ -1047,7 +1047,7 @@ static void uas_free_streams(struct uas_dev_info *devinfo)
 	eps[0] = usb_pipe_endpoint(udev, devinfo->status_pipe);
 	eps[1] = usb_pipe_endpoint(udev, devinfo->data_in_pipe);
 	eps[2] = usb_pipe_endpoint(udev, devinfo->data_out_pipe);
-	usb_free_streams(devinfo->intf, eps, 3, GFP_KERNEL);
+	usb_free_streams(devinfo->intf, eps, 3, GFP_NOIO);
 }
 
 static int uas_probe(struct usb_interface *intf, const struct usb_device_id *id)
@@ -1096,16 +1096,17 @@ static int uas_probe(struct usb_interface *intf, const struct usb_device_id *id)
 	if (result)
 		goto free_streams;
 
+	usb_set_intfdata(intf, shost);
 	result = scsi_add_host(shost, &intf->dev);
 	if (result)
 		goto free_streams;
 
 	scsi_scan_host(shost);
-	usb_set_intfdata(intf, shost);
 	return result;
 
 free_streams:
 	uas_free_streams(devinfo);
+	usb_set_intfdata(intf, NULL);
 set_alt0:
 	usb_set_interface(udev, intf->altsetting[0].desc.bInterfaceNumber, 0);
 	if (shost)
diff --git a/drivers/usb/storage/unusual_devs.h b/drivers/usb/storage/unusual_devs.h
index f4a82291894..174a447868c 100644
--- a/drivers/usb/storage/unusual_devs.h
+++ b/drivers/usb/storage/unusual_devs.h
@@ -234,6 +234,20 @@ UNUSUAL_DEV(  0x0421, 0x0495, 0x0370, 0x0370,
 		USB_SC_DEVICE, USB_PR_DEVICE, NULL,
 		US_FL_MAX_SECTORS_64 ),
 
+/* Reported by Daniele Forsi <dforsi@gmail.com> */
+UNUSUAL_DEV(  0x0421, 0x04b9, 0x0350, 0x0350,
+		"Nokia",
+		"5300",
+		USB_SC_DEVICE, USB_PR_DEVICE, NULL,
+		US_FL_MAX_SECTORS_64 ),
+
+/* Patch submitted by Victor A. Santos <victoraur.santos@gmail.com> */
+UNUSUAL_DEV(  0x0421, 0x05af, 0x0742, 0x0742,
+		"Nokia",
+		"305",
+		USB_SC_DEVICE, USB_PR_DEVICE, NULL,
+		US_FL_MAX_SECTORS_64),
+
 /* Patch submitted by Mikhail Zolotaryov <lebon@lebon.org.ua> */
 UNUSUAL_DEV(  0x0421, 0x06aa, 0x1110, 0x1110,
 		"Nokia",
diff --git a/drivers/usb/wusbcore/mmc.c b/drivers/usb/wusbcore/mmc.c
index 44741267c91..3f485df9622 100644
--- a/drivers/usb/wusbcore/mmc.c
+++ b/drivers/usb/wusbcore/mmc.c
@@ -301,7 +301,7 @@ int wusbhc_chid_set(struct wusbhc *wusbhc, const struct wusb_ckhdid *chid)
 
 	if (chid)
 		result = uwb_radio_start(&wusbhc->pal);
-	else
+	else if (wusbhc->uwb_rc)
 		uwb_radio_stop(&wusbhc->pal);
 
 	return result;
diff --git a/drivers/usb/wusbcore/wa-rpipe.c b/drivers/usb/wusbcore/wa-rpipe.c
index 6d6da127f6d..a80c5d284b5 100644
--- a/drivers/usb/wusbcore/wa-rpipe.c
+++ b/drivers/usb/wusbcore/wa-rpipe.c
@@ -524,7 +524,7 @@ void rpipe_ep_disable(struct wahc *wa, struct usb_host_endpoint *ep)
 		u16 index = le16_to_cpu(rpipe->descr.wRPipeIndex);
 
 		usb_control_msg(
-			wa->usb_dev, usb_rcvctrlpipe(wa->usb_dev, 0),
+			wa->usb_dev, usb_sndctrlpipe(wa->usb_dev, 0),
 			USB_REQ_RPIPE_ABORT,
 			USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_RPIPE,
 			0, index, NULL, 0, USB_CTRL_SET_TIMEOUT);
@@ -545,7 +545,7 @@ void rpipe_clear_feature_stalled(struct wahc *wa, struct usb_host_endpoint *ep)
 		u16 index = le16_to_cpu(rpipe->descr.wRPipeIndex);
 
 		usb_control_msg(
-			wa->usb_dev, usb_rcvctrlpipe(wa->usb_dev, 0),
+			wa->usb_dev, usb_sndctrlpipe(wa->usb_dev, 0),
 			USB_REQ_CLEAR_FEATURE,
 			USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_RPIPE,
 			RPIPE_STALL, index, NULL, 0, USB_CTRL_SET_TIMEOUT);
diff --git a/drivers/usb/wusbcore/wa-xfer.c b/drivers/usb/wusbcore/wa-xfer.c
index c8e2a47d62a..3e2e4ed2015 100644
--- a/drivers/usb/wusbcore/wa-xfer.c
+++ b/drivers/usb/wusbcore/wa-xfer.c
@@ -2390,10 +2390,10 @@ error_complete:
 		done) {
 
 		dev_info(dev, "Control EP stall.  Queue delayed work.\n");
-		spin_lock_irq(&wa->xfer_list_lock);
+		spin_lock(&wa->xfer_list_lock);
 		/* move xfer from xfer_list to xfer_errored_list. */
 		list_move_tail(&xfer->list_node, &wa->xfer_errored_list);
-		spin_unlock_irq(&wa->xfer_list_lock);
+		spin_unlock(&wa->xfer_list_lock);
 		spin_unlock_irqrestore(&xfer->lock, flags);
 		queue_work(wusbd, &wa->xfer_error_work);
 	} else {
diff --git a/drivers/uwb/beacon.c b/drivers/uwb/beacon.c
index 57b5ff61020..f40745fcbd3 100644
--- a/drivers/uwb/beacon.c
+++ b/drivers/uwb/beacon.c
@@ -125,8 +125,10 @@ int uwb_rc_beacon(struct uwb_rc *rc, int channel, unsigned bpst_offset)
 	else {
 		/* channel >= 0...dah */
 		result = uwb_rc_start_beacon(rc, bpst_offset, channel);
-		if (result < 0)
+		if (result < 0) {
+			dev_err(dev, "Cannot start beaconing: %d\n", result);
 			return result;
+		}
 		if (le16_to_cpu(rc->ies->wIELength) > 0) {
 			result = uwb_rc_set_ie(rc, rc->ies);
 			if (result < 0) {
@@ -395,7 +397,6 @@ int uwbd_evt_handle_rc_beacon(struct uwb_event *evt)
 	struct uwb_rc_evt_beacon *be;
 	struct uwb_beacon_frame *bf;
 	struct uwb_beca_e *bce;
-	unsigned long last_ts;
 
 	rc = evt->rc;
 	be = container_of(evt->notif.rceb, struct uwb_rc_evt_beacon, rceb);
@@ -439,8 +440,6 @@ int uwbd_evt_handle_rc_beacon(struct uwb_event *evt)
 	/* purge old beacon data */
 	kfree(bce->be);
 
-	last_ts = bce->ts_jiffies;
-
 	/* Update commonly used fields */
 	bce->ts_jiffies = evt->ts_jiffies;
 	bce->be = be;
diff --git a/drivers/uwb/drp.c b/drivers/uwb/drp.c
index 16ada8341c4..05b7bd76225 100644
--- a/drivers/uwb/drp.c
+++ b/drivers/uwb/drp.c
@@ -29,19 +29,19 @@
 enum uwb_drp_conflict_action {
 	/* Reservation is maintained, no action needed */
 	UWB_DRP_CONFLICT_MANTAIN = 0,
-	
+
 	/* the device shall not transmit frames in conflicting MASs in
 	 * the following superframe. If the device is the reservation
 	 * target, it shall also set the Reason Code in its DRP IE to
 	 * Conflict in its beacon in the following superframe.
 	 */
 	UWB_DRP_CONFLICT_ACT1,
-	
+
 	/* the device shall not set the Reservation Status bit to ONE
 	 * and shall not transmit frames in conflicting MASs. If the
 	 * device is the reservation target, it shall also set the
 	 * Reason Code in its DRP IE to Conflict.
-	 */	
+	 */
 	UWB_DRP_CONFLICT_ACT2,
 
 	/* the device shall not transmit frames in conflicting MASs in
@@ -59,6 +59,7 @@ static void uwb_rc_set_drp_cmd_done(struct uwb_rc *rc, void *arg,
 				    struct uwb_rceb *reply, ssize_t reply_size)
 {
 	struct uwb_rc_evt_set_drp_ie *r = (struct uwb_rc_evt_set_drp_ie *)reply;
+	unsigned long flags;
 
 	if (r != NULL) {
 		if (r->bResultCode != UWB_RC_RES_SUCCESS)
@@ -67,14 +68,14 @@ static void uwb_rc_set_drp_cmd_done(struct uwb_rc *rc, void *arg,
 	} else
 		dev_err(&rc->uwb_dev.dev, "SET-DRP-IE: timeout\n");
 
-	spin_lock_irq(&rc->rsvs_lock);
+	spin_lock_irqsave(&rc->rsvs_lock, flags);
 	if (rc->set_drp_ie_pending > 1) {
 		rc->set_drp_ie_pending = 0;
-		uwb_rsv_queue_update(rc);	
+		uwb_rsv_queue_update(rc);
 	} else {
-		rc->set_drp_ie_pending = 0;	
+		rc->set_drp_ie_pending = 0;
 	}
-	spin_unlock_irq(&rc->rsvs_lock);
+	spin_unlock_irqrestore(&rc->rsvs_lock, flags);
 }
 
 /**
@@ -114,7 +115,8 @@ int uwb_rc_send_all_drp_ie(struct uwb_rc *rc)
 			if (uwb_rsv_has_two_drp_ies(rsv) &&
 				(rsv->mv.companion_drp_ie != NULL)) {
 				mv = &rsv->mv;
-				num_bytes += mv->companion_drp_ie->hdr.length + 2;	
+				num_bytes +=
+					mv->companion_drp_ie->hdr.length + 2;
 			}
 		}
 	}
@@ -138,21 +140,23 @@ int uwb_rc_send_all_drp_ie(struct uwb_rc *rc)
 			memcpy(IEDataptr, rsv->drp_ie,
 			       rsv->drp_ie->hdr.length + 2);
 			IEDataptr += rsv->drp_ie->hdr.length + 2;
-			
+
 			if (uwb_rsv_has_two_drp_ies(rsv) &&
 				(rsv->mv.companion_drp_ie != NULL)) {
 				mv = &rsv->mv;
 				memcpy(IEDataptr, mv->companion_drp_ie,
 				       mv->companion_drp_ie->hdr.length + 2);
-				IEDataptr += mv->companion_drp_ie->hdr.length + 2;	
+				IEDataptr +=
+					mv->companion_drp_ie->hdr.length + 2;
 			}
 		}
 	}
 
-	result = uwb_rc_cmd_async(rc, "SET-DRP-IE", &cmd->rccb, sizeof(*cmd) + num_bytes,
-				  UWB_RC_CET_GENERAL, UWB_RC_CMD_SET_DRP_IE,
-				  uwb_rc_set_drp_cmd_done, NULL);
-	
+	result = uwb_rc_cmd_async(rc, "SET-DRP-IE",
+				&cmd->rccb, sizeof(*cmd) + num_bytes,
+				UWB_RC_CET_GENERAL, UWB_RC_CMD_SET_DRP_IE,
+				uwb_rc_set_drp_cmd_done, NULL);
+
 	rc->set_drp_ie_pending = 1;
 
 	kfree(cmd);
@@ -175,8 +179,8 @@ static int evaluate_conflict_action(struct uwb_ie_drp *ext_drp_ie, int ext_beaco
 	int ext_tie_breaker = uwb_ie_drp_tiebreaker(ext_drp_ie);
 	int ext_status      = uwb_ie_drp_status(ext_drp_ie);
 	int ext_type        = uwb_ie_drp_type(ext_drp_ie);
-	
-	
+
+
 	/* [ECMA-368 2nd Edition] 17.4.6 */
 	if (ext_type == UWB_DRP_TYPE_PCA && our_type == UWB_DRP_TYPE_PCA) {
 		return UWB_DRP_CONFLICT_MANTAIN;
@@ -186,7 +190,7 @@ static int evaluate_conflict_action(struct uwb_ie_drp *ext_drp_ie, int ext_beaco
 	if (our_type == UWB_DRP_TYPE_ALIEN_BP) {
 		return UWB_DRP_CONFLICT_MANTAIN;
 	}
-	
+
 	/* [ECMA-368 2nd Edition] 17.4.6-2 */
 	if (ext_type == UWB_DRP_TYPE_ALIEN_BP) {
 		/* here we know our_type != UWB_DRP_TYPE_ALIEN_BP */
@@ -214,7 +218,7 @@ static int evaluate_conflict_action(struct uwb_ie_drp *ext_drp_ie, int ext_beaco
 	    our_beacon_slot >  ext_beacon_slot) {
 		return UWB_DRP_CONFLICT_MANTAIN;
 	}
-	
+
 	if (our_status == 0) {
 		if (our_tie_breaker == ext_tie_breaker) {
 			/* [ECMA-368 2nd Edition] 17.4.6-6a */
@@ -243,9 +247,9 @@ static int evaluate_conflict_action(struct uwb_ie_drp *ext_drp_ie, int ext_beaco
 	return UWB_DRP_CONFLICT_MANTAIN;
 }
 
-static void handle_conflict_normal(struct uwb_ie_drp *drp_ie, 
-				   int ext_beacon_slot, 
-				   struct uwb_rsv *rsv, 
+static void handle_conflict_normal(struct uwb_ie_drp *drp_ie,
+				   int ext_beacon_slot,
+				   struct uwb_rsv *rsv,
 				   struct uwb_mas_bm *conflicting_mas)
 {
 	struct uwb_rc *rc = rsv->rc;
@@ -262,7 +266,7 @@ static void handle_conflict_normal(struct uwb_ie_drp *drp_ie,
 			uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_TO_BE_MOVED);
 			if (bow->can_reserve_extra_mases == false)
 				uwb_rsv_backoff_win_increment(rc);
-			
+
 			break;
 		case UWB_DRP_CONFLICT_ACT3:
 			uwb_rsv_backoff_win_increment(rc);
@@ -277,13 +281,13 @@ static void handle_conflict_normal(struct uwb_ie_drp *drp_ie,
 		switch(action) {
 		case UWB_DRP_CONFLICT_ACT2:
 		case UWB_DRP_CONFLICT_ACT3:
-			uwb_rsv_set_state(rsv, UWB_RSV_STATE_T_CONFLICT);	
+			uwb_rsv_set_state(rsv, UWB_RSV_STATE_T_CONFLICT);
 		default:
 			break;
 		}
 
 	}
-	
+
 }
 
 static void handle_conflict_expanding(struct uwb_ie_drp *drp_ie, int ext_beacon_slot,
@@ -294,7 +298,7 @@ static void handle_conflict_expanding(struct uwb_ie_drp *drp_ie, int ext_beacon_
 	struct uwb_drp_backoff_win *bow = &rc->bow;
 	struct uwb_rsv_move *mv = &rsv->mv;
 	int action;
-	
+
 	if (companion_only) {
 		/* status of companion is 0 at this point */
 		action = evaluate_conflict_action(drp_ie, ext_beacon_slot, rsv, 0);
@@ -302,21 +306,24 @@ static void handle_conflict_expanding(struct uwb_ie_drp *drp_ie, int ext_beacon_
 			switch(action) {
 			case UWB_DRP_CONFLICT_ACT2:
 			case UWB_DRP_CONFLICT_ACT3:
-				uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_ESTABLISHED);
+				uwb_rsv_set_state(rsv,
+						UWB_RSV_STATE_O_ESTABLISHED);
 				rsv->needs_release_companion_mas = false;
 				if (bow->can_reserve_extra_mases == false)
 					uwb_rsv_backoff_win_increment(rc);
-				uwb_drp_avail_release(rsv->rc, &rsv->mv.companion_mas);
+				uwb_drp_avail_release(rsv->rc,
+						&rsv->mv.companion_mas);
 			}
-		} else { /* rsv is target */			
+		} else { /* rsv is target */
 			switch(action) {
 			case UWB_DRP_CONFLICT_ACT2:
 			case UWB_DRP_CONFLICT_ACT3:
-				uwb_rsv_set_state(rsv, UWB_RSV_STATE_T_EXPANDING_CONFLICT);
+				uwb_rsv_set_state(rsv,
+					UWB_RSV_STATE_T_EXPANDING_CONFLICT);
                                 /* send_drp_avail_ie = true; */
 			}
 		}
-	} else { /* also base part of the reservation is conflicting */		
+	} else { /* also base part of the reservation is conflicting */
 		if (uwb_rsv_is_owner(rsv)) {
 			uwb_rsv_backoff_win_increment(rc);
 			/* remove companion part */
@@ -325,7 +332,8 @@ static void handle_conflict_expanding(struct uwb_ie_drp *drp_ie, int ext_beacon_
 			/* drop some mases with reason modified */
 
 			/* put in the companion the mases to be dropped */
-			bitmap_andnot(mv->companion_mas.bm, rsv->mas.bm, conflicting_mas->bm, UWB_NUM_MAS);
+			bitmap_andnot(mv->companion_mas.bm, rsv->mas.bm,
+					conflicting_mas->bm, UWB_NUM_MAS);
 			uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MODIFIED);
 		} else { /* it is a target rsv */
 			uwb_rsv_set_state(rsv, UWB_RSV_STATE_T_CONFLICT);
@@ -335,7 +343,7 @@ static void handle_conflict_expanding(struct uwb_ie_drp *drp_ie, int ext_beacon_
 }
 
 static void uwb_drp_handle_conflict_rsv(struct uwb_rc *rc, struct uwb_rsv *rsv,
-					struct uwb_rc_evt_drp *drp_evt, 
+					struct uwb_rc_evt_drp *drp_evt,
 					struct uwb_ie_drp *drp_ie,
 					struct uwb_mas_bm *conflicting_mas)
 {
@@ -344,83 +352,107 @@ static void uwb_drp_handle_conflict_rsv(struct uwb_rc *rc, struct uwb_rsv *rsv,
 	/* check if the conflicting reservation has two drp_ies */
 	if (uwb_rsv_has_two_drp_ies(rsv)) {
 		mv = &rsv->mv;
-		if (bitmap_intersects(rsv->mas.bm, conflicting_mas->bm, UWB_NUM_MAS)) {
-			handle_conflict_expanding(drp_ie, drp_evt->beacon_slot_number,
-						  rsv, false, conflicting_mas);
+		if (bitmap_intersects(rsv->mas.bm, conflicting_mas->bm,
+								UWB_NUM_MAS)) {
+			handle_conflict_expanding(drp_ie,
+						drp_evt->beacon_slot_number,
+						rsv, false, conflicting_mas);
 		} else {
-			if (bitmap_intersects(mv->companion_mas.bm, conflicting_mas->bm, UWB_NUM_MAS)) {
-				handle_conflict_expanding(drp_ie, drp_evt->beacon_slot_number,
-							  rsv, true, conflicting_mas);	
+			if (bitmap_intersects(mv->companion_mas.bm,
+					conflicting_mas->bm, UWB_NUM_MAS)) {
+				handle_conflict_expanding(
+					drp_ie, drp_evt->beacon_slot_number,
+					rsv, true, conflicting_mas);
 			}
 		}
-	} else if (bitmap_intersects(rsv->mas.bm, conflicting_mas->bm, UWB_NUM_MAS)) {
-		handle_conflict_normal(drp_ie, drp_evt->beacon_slot_number, rsv, conflicting_mas);
+	} else if (bitmap_intersects(rsv->mas.bm, conflicting_mas->bm,
+							UWB_NUM_MAS)) {
+		handle_conflict_normal(drp_ie, drp_evt->beacon_slot_number,
+					rsv, conflicting_mas);
 	}
 }
 
 static void uwb_drp_handle_all_conflict_rsv(struct uwb_rc *rc,
-					    struct uwb_rc_evt_drp *drp_evt, 
+					    struct uwb_rc_evt_drp *drp_evt,
 					    struct uwb_ie_drp *drp_ie,
 					    struct uwb_mas_bm *conflicting_mas)
 {
 	struct uwb_rsv *rsv;
-	
+
 	list_for_each_entry(rsv, &rc->reservations, rc_node) {
-		uwb_drp_handle_conflict_rsv(rc, rsv, drp_evt, drp_ie, conflicting_mas);	
+		uwb_drp_handle_conflict_rsv(rc, rsv, drp_evt, drp_ie,
+							conflicting_mas);
+	}
+}
+
+static void uwb_drp_process_target_accepted(struct uwb_rc *rc,
+	struct uwb_rsv *rsv, struct uwb_rc_evt_drp *drp_evt,
+	struct uwb_ie_drp *drp_ie, struct uwb_mas_bm *mas)
+{
+	struct uwb_rsv_move *mv = &rsv->mv;
+	int status;
+
+	status = uwb_ie_drp_status(drp_ie);
+
+	if (rsv->state == UWB_RSV_STATE_T_CONFLICT) {
+		uwb_rsv_set_state(rsv, UWB_RSV_STATE_T_CONFLICT);
+		return;
+	}
+
+	if (rsv->state == UWB_RSV_STATE_T_EXPANDING_ACCEPTED) {
+		/* drp_ie is companion */
+		if (!bitmap_equal(rsv->mas.bm, mas->bm, UWB_NUM_MAS)) {
+			/* stroke companion */
+			uwb_rsv_set_state(rsv,
+				UWB_RSV_STATE_T_EXPANDING_ACCEPTED);
+		}
+	} else {
+		if (!bitmap_equal(rsv->mas.bm, mas->bm, UWB_NUM_MAS)) {
+			if (uwb_drp_avail_reserve_pending(rc, mas) == -EBUSY) {
+				/* FIXME: there is a conflict, find
+				 * the conflicting reservations and
+				 * take a sensible action. Consider
+				 * that in drp_ie there is the
+				 * "neighbour" */
+				uwb_drp_handle_all_conflict_rsv(rc, drp_evt,
+						drp_ie, mas);
+			} else {
+				/* accept the extra reservation */
+				bitmap_copy(mv->companion_mas.bm, mas->bm,
+								UWB_NUM_MAS);
+				uwb_rsv_set_state(rsv,
+					UWB_RSV_STATE_T_EXPANDING_ACCEPTED);
+			}
+		} else {
+			if (status) {
+				uwb_rsv_set_state(rsv,
+						UWB_RSV_STATE_T_ACCEPTED);
+			}
+		}
+
 	}
 }
-	
+
 /*
  * Based on the DRP IE, transition a target reservation to a new
  * state.
  */
 static void uwb_drp_process_target(struct uwb_rc *rc, struct uwb_rsv *rsv,
-				   struct uwb_ie_drp *drp_ie, struct uwb_rc_evt_drp *drp_evt)
+		   struct uwb_ie_drp *drp_ie, struct uwb_rc_evt_drp *drp_evt)
 {
 	struct device *dev = &rc->uwb_dev.dev;
 	struct uwb_rsv_move *mv = &rsv->mv;
 	int status;
 	enum uwb_drp_reason reason_code;
 	struct uwb_mas_bm mas;
-	
+
 	status = uwb_ie_drp_status(drp_ie);
 	reason_code = uwb_ie_drp_reason_code(drp_ie);
 	uwb_drp_ie_to_bm(&mas, drp_ie);
 
 	switch (reason_code) {
 	case UWB_DRP_REASON_ACCEPTED:
-
-		if (rsv->state == UWB_RSV_STATE_T_CONFLICT) {
-			uwb_rsv_set_state(rsv, UWB_RSV_STATE_T_CONFLICT);
-			break;
-		}
-
-		if (rsv->state == UWB_RSV_STATE_T_EXPANDING_ACCEPTED) {
-			/* drp_ie is companion */
-			if (!bitmap_equal(rsv->mas.bm, mas.bm, UWB_NUM_MAS))
-				/* stroke companion */
-				uwb_rsv_set_state(rsv, UWB_RSV_STATE_T_EXPANDING_ACCEPTED);	
-		} else {
-			if (!bitmap_equal(rsv->mas.bm, mas.bm, UWB_NUM_MAS)) {
-				if (uwb_drp_avail_reserve_pending(rc, &mas) == -EBUSY) {
-					/* FIXME: there is a conflict, find
-					 * the conflicting reservations and
-					 * take a sensible action. Consider
-					 * that in drp_ie there is the
-					 * "neighbour" */
-					uwb_drp_handle_all_conflict_rsv(rc, drp_evt, drp_ie, &mas);
-				} else {
-					/* accept the extra reservation */
-					bitmap_copy(mv->companion_mas.bm, mas.bm, UWB_NUM_MAS);
-					uwb_rsv_set_state(rsv, UWB_RSV_STATE_T_EXPANDING_ACCEPTED);
-				}
-			} else {
-				if (status) {
-					uwb_rsv_set_state(rsv, UWB_RSV_STATE_T_ACCEPTED);
-				}
-			}
-			
-		}
+		uwb_drp_process_target_accepted(rc, rsv, drp_evt, drp_ie, &mas);
 		break;
 
 	case UWB_DRP_REASON_MODIFIED:
@@ -433,7 +465,8 @@ static void uwb_drp_process_target(struct uwb_rc *rc, struct uwb_rsv *rsv,
 		/* find if the owner wants to expand or reduce */
 		if (bitmap_subset(mas.bm, rsv->mas.bm, UWB_NUM_MAS)) {
 			/* owner is reducing */
-			bitmap_andnot(mv->companion_mas.bm, rsv->mas.bm, mas.bm, UWB_NUM_MAS);
+			bitmap_andnot(mv->companion_mas.bm, rsv->mas.bm, mas.bm,
+				UWB_NUM_MAS);
 			uwb_drp_avail_release(rsv->rc, &mv->companion_mas);
 		}
 
@@ -446,6 +479,48 @@ static void uwb_drp_process_target(struct uwb_rc *rc, struct uwb_rsv *rsv,
 	}
 }
 
+static void uwb_drp_process_owner_accepted(struct uwb_rsv *rsv,
+						struct uwb_mas_bm *mas)
+{
+	struct uwb_rsv_move *mv = &rsv->mv;
+
+	switch (rsv->state) {
+	case UWB_RSV_STATE_O_PENDING:
+	case UWB_RSV_STATE_O_INITIATED:
+	case UWB_RSV_STATE_O_ESTABLISHED:
+		uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_ESTABLISHED);
+		break;
+	case UWB_RSV_STATE_O_MODIFIED:
+		if (bitmap_equal(mas->bm, rsv->mas.bm, UWB_NUM_MAS))
+			uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_ESTABLISHED);
+		else
+			uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MODIFIED);
+		break;
+
+	case UWB_RSV_STATE_O_MOVE_REDUCING: /* shouldn' t be a problem */
+		if (bitmap_equal(mas->bm, rsv->mas.bm, UWB_NUM_MAS))
+			uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_ESTABLISHED);
+		else
+			uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_REDUCING);
+		break;
+	case UWB_RSV_STATE_O_MOVE_EXPANDING:
+		if (bitmap_equal(mas->bm, mv->companion_mas.bm, UWB_NUM_MAS)) {
+			/* Companion reservation accepted */
+			uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_COMBINING);
+		} else {
+			uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_EXPANDING);
+		}
+		break;
+	case UWB_RSV_STATE_O_MOVE_COMBINING:
+		if (bitmap_equal(mas->bm, rsv->mas.bm, UWB_NUM_MAS))
+			uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_REDUCING);
+		else
+			uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_COMBINING);
+		break;
+	default:
+		break;
+	}
+}
 /*
  * Based on the DRP IE, transition an owner reservation to a new
  * state.
@@ -455,7 +530,6 @@ static void uwb_drp_process_owner(struct uwb_rc *rc, struct uwb_rsv *rsv,
 				  struct uwb_rc_evt_drp *drp_evt)
 {
 	struct device *dev = &rc->uwb_dev.dev;
-	struct uwb_rsv_move *mv = &rsv->mv;
 	int status;
 	enum uwb_drp_reason reason_code;
 	struct uwb_mas_bm mas;
@@ -467,44 +541,7 @@ static void uwb_drp_process_owner(struct uwb_rc *rc, struct uwb_rsv *rsv,
 	if (status) {
 		switch (reason_code) {
 		case UWB_DRP_REASON_ACCEPTED:
-			switch (rsv->state) {
-			case UWB_RSV_STATE_O_PENDING:
-			case UWB_RSV_STATE_O_INITIATED:
-			case UWB_RSV_STATE_O_ESTABLISHED:
-				uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_ESTABLISHED);
-				break;
-			case UWB_RSV_STATE_O_MODIFIED:
-				if (bitmap_equal(mas.bm, rsv->mas.bm, UWB_NUM_MAS)) {
-					uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_ESTABLISHED);
-				} else {
-					uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MODIFIED);	
-				}
-				break;
-				
-			case UWB_RSV_STATE_O_MOVE_REDUCING: /* shouldn' t be a problem */
-				if (bitmap_equal(mas.bm, rsv->mas.bm, UWB_NUM_MAS)) {
-					uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_ESTABLISHED);
-				} else {
-					uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_REDUCING);	
-				}
-				break;
-			case UWB_RSV_STATE_O_MOVE_EXPANDING:
-				if (bitmap_equal(mas.bm, mv->companion_mas.bm, UWB_NUM_MAS)) {
-					/* Companion reservation accepted */
-					uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_COMBINING);
-				} else {
-					uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_EXPANDING);
-				}
-				break;
-			case UWB_RSV_STATE_O_MOVE_COMBINING:
-				if (bitmap_equal(mas.bm, rsv->mas.bm, UWB_NUM_MAS))
-					uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_REDUCING);
-				else
-					uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_COMBINING);
-				break;
-			default:
-				break;	
-			}
+			uwb_drp_process_owner_accepted(rsv, &mas);
 			break;
 		default:
 			dev_warn(dev, "ignoring invalid DRP IE state (%d/%d)\n",
@@ -544,9 +581,9 @@ static void uwb_cnflt_update_work(struct work_struct *work)
 						     cnflt_update_work);
 	struct uwb_cnflt_alien *c;
 	struct uwb_rc *rc = cnflt->rc;
-	
+
 	unsigned long delay_us = UWB_MAS_LENGTH_US * UWB_MAS_PER_ZONE;
-	
+
 	mutex_lock(&rc->rsvs_mutex);
 
 	list_del(&cnflt->rc_node);
@@ -555,10 +592,12 @@ static void uwb_cnflt_update_work(struct work_struct *work)
 	bitmap_zero(rc->cnflt_alien_bitmap.bm, UWB_NUM_MAS);
 
 	list_for_each_entry(c, &rc->cnflt_alien_list, rc_node) {
-		bitmap_or(rc->cnflt_alien_bitmap.bm, rc->cnflt_alien_bitmap.bm, c->mas.bm, UWB_NUM_MAS);			
+		bitmap_or(rc->cnflt_alien_bitmap.bm, rc->cnflt_alien_bitmap.bm,
+						c->mas.bm, UWB_NUM_MAS);
 	}
-	
-	queue_delayed_work(rc->rsv_workq, &rc->rsv_alien_bp_work, usecs_to_jiffies(delay_us));
+
+	queue_delayed_work(rc->rsv_workq, &rc->rsv_alien_bp_work,
+					usecs_to_jiffies(delay_us));
 
 	kfree(cnflt);
 	mutex_unlock(&rc->rsvs_mutex);
@@ -582,10 +621,10 @@ static void uwb_drp_handle_alien_drp(struct uwb_rc *rc, struct uwb_ie_drp *drp_i
 	struct uwb_cnflt_alien *cnflt;
 	char buf[72];
 	unsigned long delay_us = UWB_MAS_LENGTH_US * UWB_MAS_PER_ZONE;
-	
+
 	uwb_drp_ie_to_bm(&mas, drp_ie);
 	bitmap_scnprintf(buf, sizeof(buf), mas.bm, UWB_NUM_MAS);
-	
+
 	list_for_each_entry(cnflt, &rc->cnflt_alien_list, rc_node) {
 		if (bitmap_equal(cnflt->mas.bm, mas.bm, UWB_NUM_MAS)) {
 			/* Existing alien BP reservation conflicting
@@ -599,8 +638,11 @@ static void uwb_drp_handle_alien_drp(struct uwb_rc *rc, struct uwb_ie_drp *drp_i
 
 	/* alloc and initialize new uwb_cnflt_alien */
 	cnflt = kzalloc(sizeof(struct uwb_cnflt_alien), GFP_KERNEL);
-	if (!cnflt)
+	if (!cnflt) {
 		dev_err(dev, "failed to alloc uwb_cnflt_alien struct\n");
+		return;
+	}
+
 	INIT_LIST_HEAD(&cnflt->rc_node);
 	init_timer(&cnflt->timer);
 	cnflt->timer.function = uwb_cnflt_timer;
@@ -608,7 +650,7 @@ static void uwb_drp_handle_alien_drp(struct uwb_rc *rc, struct uwb_ie_drp *drp_i
 
 	cnflt->rc = rc;
 	INIT_WORK(&cnflt->cnflt_update_work, uwb_cnflt_update_work);
-	
+
 	bitmap_copy(cnflt->mas.bm, mas.bm, UWB_NUM_MAS);
 
 	list_add_tail(&cnflt->rc_node, &rc->cnflt_alien_list);
@@ -617,17 +659,17 @@ static void uwb_drp_handle_alien_drp(struct uwb_rc *rc, struct uwb_ie_drp *drp_i
 	bitmap_or(rc->cnflt_alien_bitmap.bm, rc->cnflt_alien_bitmap.bm, mas.bm, UWB_NUM_MAS);
 
 	queue_delayed_work(rc->rsv_workq, &rc->rsv_alien_bp_work, usecs_to_jiffies(delay_us));
-	
+
 	/* start the timer */
 	uwb_cnflt_alien_stroke_timer(cnflt);
 }
 
 static void uwb_drp_process_not_involved(struct uwb_rc *rc,
-					 struct uwb_rc_evt_drp *drp_evt, 
+					 struct uwb_rc_evt_drp *drp_evt,
 					 struct uwb_ie_drp *drp_ie)
 {
 	struct uwb_mas_bm mas;
-	
+
 	uwb_drp_ie_to_bm(&mas, drp_ie);
 	uwb_drp_handle_all_conflict_rsv(rc, drp_evt, drp_ie, &mas);
 }
@@ -647,7 +689,7 @@ static void uwb_drp_process_involved(struct uwb_rc *rc, struct uwb_dev *src,
 		 */
 		return;
 	}
-	
+
 	/*
 	 * Do nothing with DRP IEs for reservations that have been
 	 * terminated.
@@ -656,12 +698,12 @@ static void uwb_drp_process_involved(struct uwb_rc *rc, struct uwb_dev *src,
 		uwb_rsv_set_state(rsv, UWB_RSV_STATE_NONE);
 		return;
 	}
-			
+
 	if (uwb_ie_drp_owner(drp_ie))
 		uwb_drp_process_target(rc, rsv, drp_ie, drp_evt);
 	else
 		uwb_drp_process_owner(rc, rsv, src, drp_ie, drp_evt);
-	
+
 }
 
 
diff --git a/drivers/uwb/est.c b/drivers/uwb/est.c
index 457f31d99bf..f3e23258428 100644
--- a/drivers/uwb/est.c
+++ b/drivers/uwb/est.c
@@ -258,7 +258,6 @@ int uwb_est_register(u8 type, u8 event_high, u16 vendor, u16 product,
 {
 	unsigned long flags;
 	unsigned itr;
-	u16 type_event_high;
 	int result = 0;
 
 	write_lock_irqsave(&uwb_est_lock, flags);
@@ -268,7 +267,6 @@ int uwb_est_register(u8 type, u8 event_high, u16 vendor, u16 product,
 			goto out;
 	}
 	/* Find the right spot to insert it in */
-	type_event_high = type << 8 | event_high;
 	for (itr = 0; itr < uwb_est_used; itr++)
 		if (uwb_est[itr].type_event_high < type
 		    && uwb_est[itr].vendor < vendor
diff --git a/drivers/uwb/ie-rcv.c b/drivers/uwb/ie-rcv.c
index 917e6d78a79..5fac5744a69 100644
--- a/drivers/uwb/ie-rcv.c
+++ b/drivers/uwb/ie-rcv.c
@@ -31,7 +31,6 @@ int uwbd_evt_handle_rc_ie_rcv(struct uwb_event *evt)
 	int result = -EINVAL;
 	struct device *dev = &evt->rc->uwb_dev.dev;
 	struct uwb_rc_evt_ie_rcv *iercv;
-	size_t iesize;
 
 	/* Is there enough data to decode it? */
 	if (evt->notif.size < sizeof(*iercv)) {
@@ -41,7 +40,6 @@ int uwbd_evt_handle_rc_ie_rcv(struct uwb_event *evt)
 		goto error;
 	}
 	iercv = container_of(evt->notif.rceb, struct uwb_rc_evt_ie_rcv, rceb);
-	iesize = le16_to_cpu(iercv->wIELength);
 
 	dev_dbg(dev, "IE received, element ID=%d\n", iercv->IEData[0]);
 
diff --git a/drivers/uwb/radio.c b/drivers/uwb/radio.c
index fd23d985ea7..2427e944883 100644
--- a/drivers/uwb/radio.c
+++ b/drivers/uwb/radio.c
@@ -115,7 +115,7 @@ int uwb_radio_start(struct uwb_pal *pal)
 EXPORT_SYMBOL_GPL(uwb_radio_start);
 
 /**
- * uwb_radio_stop - request tha the radio be stopped.
+ * uwb_radio_stop - request that the radio be stopped.
  * @pal: the PAL making the request.
  *
  * Stops the radio if no other PAL is making use of it.
diff --git a/drivers/uwb/rsv.c b/drivers/uwb/rsv.c
index 3fe61194104..0887ae98278 100644
--- a/drivers/uwb/rsv.c
+++ b/drivers/uwb/rsv.c
@@ -163,8 +163,10 @@ static int uwb_rsv_get_stream(struct uwb_rsv *rsv)
 	}
 
 	stream = find_first_zero_bit(streams_bm, UWB_NUM_STREAMS);
-	if (stream >= UWB_NUM_STREAMS)
+	if (stream >= UWB_NUM_STREAMS) {
+		dev_err(dev, "%s: no available stream found\n", __func__);
 		return -EBUSY;
+	}
 
 	rsv->stream = stream;
 	set_bit(stream, streams_bm);
@@ -249,7 +251,9 @@ static void uwb_rsv_stroke_timer(struct uwb_rsv *rsv)
 	 * super frame and should not be terminated if no response is
 	 * received.
 	 */
-	if (rsv->is_multicast) {
+	if (rsv->state == UWB_RSV_STATE_NONE) {
+		sframes = 0;
+	} else if (rsv->is_multicast) {
 		if (rsv->state == UWB_RSV_STATE_O_INITIATED
 		    || rsv->state == UWB_RSV_STATE_O_MOVE_EXPANDING
 		    || rsv->state == UWB_RSV_STATE_O_MOVE_COMBINING
@@ -322,6 +326,7 @@ void uwb_rsv_set_state(struct uwb_rsv *rsv, enum uwb_rsv_state new_state)
 	switch (new_state) {
 	case UWB_RSV_STATE_NONE:
 		uwb_rsv_state_update(rsv, UWB_RSV_STATE_NONE);
+		uwb_rsv_remove(rsv);
 		uwb_rsv_callback(rsv);
 		break;
 	case UWB_RSV_STATE_O_INITIATED:
@@ -442,6 +447,8 @@ static void uwb_rsv_handle_timeout_work(struct work_struct *work)
 		uwb_rsv_set_state(rsv, UWB_RSV_STATE_T_ACCEPTED);
 		uwb_drp_avail_release(rsv->rc, &rsv->mv.companion_mas);
 		goto unlock;
+	case UWB_RSV_STATE_NONE:
+		goto unlock;
 	default:
 		break;
 	}
@@ -550,12 +557,16 @@ int uwb_rsv_establish(struct uwb_rsv *rsv)
 {
 	struct uwb_rc *rc = rsv->rc;
 	struct uwb_mas_bm available;
+	struct device *dev = &rc->uwb_dev.dev;
 	int ret;
 
 	mutex_lock(&rc->rsvs_mutex);
 	ret = uwb_rsv_get_stream(rsv);
-	if (ret)
+	if (ret) {
+		dev_err(dev, "%s: uwb_rsv_get_stream failed: %d\n",
+			__func__, ret);
 		goto out;
+	}
 
 	rsv->tiebreaker = prandom_u32() & 1;
 	/* get available mas bitmap */
@@ -565,12 +576,16 @@ int uwb_rsv_establish(struct uwb_rsv *rsv)
 	if (ret == UWB_RSV_ALLOC_NOT_FOUND) {
 		ret = -EBUSY;
 		uwb_rsv_put_stream(rsv);
+		dev_err(dev, "%s: uwb_rsv_find_best_allocation failed: %d\n",
+			__func__, ret);
 		goto out;
 	}
 
 	ret = uwb_drp_avail_reserve_pending(rc, &rsv->mas);
 	if (ret != 0) {
 		uwb_rsv_put_stream(rsv);
+		dev_err(dev, "%s: uwb_drp_avail_reserve_pending failed: %d\n",
+			__func__, ret);
 		goto out;
 	}
 
diff --git a/drivers/vfio/vfio.c b/drivers/vfio/vfio.c
index 512f479d8a5..f018d8d0f97 100644
--- a/drivers/vfio/vfio.c
+++ b/drivers/vfio/vfio.c
@@ -349,7 +349,6 @@ struct vfio_device *vfio_group_create_device(struct vfio_group *group,
 					     void *device_data)
 {
 	struct vfio_device *device;
-	int ret;
 
 	device = kzalloc(sizeof(*device), GFP_KERNEL);
 	if (!device)
@@ -360,12 +359,7 @@ struct vfio_device *vfio_group_create_device(struct vfio_group *group,
 	device->group = group;
 	device->ops = ops;
 	device->device_data = device_data;
-
-	ret = dev_set_drvdata(dev, device);
-	if (ret) {
-		kfree(device);
-		return ERR_PTR(ret);
-	}
+	dev_set_drvdata(dev, device);
 
 	/* No need to get group_lock, caller has group reference */
 	vfio_group_get(group);
diff --git a/drivers/video/console/vgacon.c b/drivers/video/console/vgacon.c
index 9d8feac6763..f267284b423 100644
--- a/drivers/video/console/vgacon.c
+++ b/drivers/video/console/vgacon.c
@@ -87,7 +87,8 @@ static void vgacon_save_screen(struct vc_data *c);
 static int vgacon_scroll(struct vc_data *c, int t, int b, int dir,
 			 int lines);
 static void vgacon_invert_region(struct vc_data *c, u16 * p, int count);
-static unsigned long vgacon_uni_pagedir[2];
+static struct uni_pagedir *vgacon_uni_pagedir;
+static int vgacon_refcount;
 
 /* Description of the hardware situation */
 static int		vga_init_done		__read_mostly;
@@ -553,7 +554,7 @@ static const char *vgacon_startup(void)
 
 static void vgacon_init(struct vc_data *c, int init)
 {
-	unsigned long p;
+	struct uni_pagedir *p;
 
 	/*
 	 * We cannot be loaded as a module, therefore init is always 1,
@@ -575,12 +576,12 @@ static void vgacon_init(struct vc_data *c, int init)
 	if (vga_512_chars)
 		c->vc_hi_font_mask = 0x0800;
 	p = *c->vc_uni_pagedir_loc;
-	if (c->vc_uni_pagedir_loc == &c->vc_uni_pagedir ||
-	    !--c->vc_uni_pagedir_loc[1])
+	if (c->vc_uni_pagedir_loc != &vgacon_uni_pagedir) {
 		con_free_unimap(c);
-	c->vc_uni_pagedir_loc = vgacon_uni_pagedir;
-	vgacon_uni_pagedir[1]++;
-	if (!vgacon_uni_pagedir[0] && p)
+		c->vc_uni_pagedir_loc = &vgacon_uni_pagedir;
+		vgacon_refcount++;
+	}
+	if (!vgacon_uni_pagedir && p)
 		con_set_default_unimap(c);
 
 	/* Only set the default if the user didn't deliberately override it */
@@ -597,7 +598,7 @@ static void vgacon_deinit(struct vc_data *c)
 		vga_set_mem_top(c);
 	}
 
-	if (!--vgacon_uni_pagedir[1])
+	if (!--vgacon_refcount)
 		con_free_unimap(c);
 	c->vc_uni_pagedir_loc = &c->vc_uni_pagedir;
 	con_set_default_unimap(c);
diff --git a/drivers/video/fbdev/atafb.c b/drivers/video/fbdev/atafb.c
index e21d1f58554..4953b657635 100644
--- a/drivers/video/fbdev/atafb.c
+++ b/drivers/video/fbdev/atafb.c
@@ -191,7 +191,7 @@ static struct fb_info fb_info = {
 };
 
 static void *screen_base;	/* base address of screen */
-static void *real_screen_base;	/* (only for Overscan) */
+static unsigned long phys_screen_base;	/* (only for Overscan) */
 
 static int screen_len;
 
@@ -213,7 +213,8 @@ static unsigned int external_yres;
  */
 static unsigned int external_depth;
 static int external_pmode;
-static void *external_addr;
+static void *external_screen_base;
+static unsigned long external_addr;
 static unsigned long external_len;
 static unsigned long external_vgaiobase;
 static unsigned int external_bitspercol = 6;
@@ -592,7 +593,7 @@ static int tt_encode_fix(struct fb_fix_screeninfo *fix, struct atafb_par *par)
 	int mode;
 
 	strcpy(fix->id, "Atari Builtin");
-	fix->smem_start = (unsigned long)real_screen_base;
+	fix->smem_start = phys_screen_base;
 	fix->smem_len = screen_len;
 	fix->type = FB_TYPE_INTERLEAVED_PLANES;
 	fix->type_aux = 2;
@@ -790,7 +791,7 @@ static void tt_get_par(struct atafb_par *par)
 	addr = ((shifter.bas_hi & 0xff) << 16) |
 	       ((shifter.bas_md & 0xff) << 8)  |
 	       ((shifter.bas_lo & 0xff));
-	par->screen_base = phys_to_virt(addr);
+	par->screen_base = atari_stram_to_virt(addr);
 }
 
 static void tt_set_par(struct atafb_par *par)
@@ -888,7 +889,7 @@ static int falcon_encode_fix(struct fb_fix_screeninfo *fix,
 			     struct atafb_par *par)
 {
 	strcpy(fix->id, "Atari Builtin");
-	fix->smem_start = (unsigned long)real_screen_base;
+	fix->smem_start = phys_screen_base;
 	fix->smem_len = screen_len;
 	fix->type = FB_TYPE_INTERLEAVED_PLANES;
 	fix->type_aux = 2;
@@ -1584,7 +1585,7 @@ static void falcon_get_par(struct atafb_par *par)
 	addr = (shifter.bas_hi & 0xff) << 16 |
 	       (shifter.bas_md & 0xff) << 8  |
 	       (shifter.bas_lo & 0xff);
-	par->screen_base = phys_to_virt(addr);
+	par->screen_base = atari_stram_to_virt(addr);
 
 	/* derived parameters */
 	hw->ste_mode = (hw->f_shift & 0x510) == 0 && hw->st_shift == 0x100;
@@ -1814,7 +1815,7 @@ static int stste_encode_fix(struct fb_fix_screeninfo *fix,
 	int mode;
 
 	strcpy(fix->id, "Atari Builtin");
-	fix->smem_start = (unsigned long)real_screen_base;
+	fix->smem_start = phys_screen_base;
 	fix->smem_len = screen_len;
 	fix->type = FB_TYPE_INTERLEAVED_PLANES;
 	fix->type_aux = 2;
@@ -1980,7 +1981,7 @@ static void stste_get_par(struct atafb_par *par)
 	       ((shifter.bas_md & 0xff) << 8);
 	if (ATARIHW_PRESENT(EXTD_SHIFTER))
 		addr |= (shifter.bas_lo & 0xff);
-	par->screen_base = phys_to_virt(addr);
+	par->screen_base = atari_stram_to_virt(addr);
 }
 
 static void stste_set_par(struct atafb_par *par)
@@ -2039,7 +2040,7 @@ static int stste_detect(void)
 static void stste_set_screen_base(void *s_base)
 {
 	unsigned long addr;
-	addr = virt_to_phys(s_base);
+	addr = atari_stram_to_phys(s_base);
 	/* Setup Screen Memory */
 	shifter.bas_hi = (unsigned char)((addr & 0xff0000) >> 16);
 	shifter.bas_md = (unsigned char)((addr & 0x00ff00) >> 8);
@@ -2113,7 +2114,7 @@ static void st_ovsc_switch(void)
 static int ext_encode_fix(struct fb_fix_screeninfo *fix, struct atafb_par *par)
 {
 	strcpy(fix->id, "Unknown Extern");
-	fix->smem_start = (unsigned long)external_addr;
+	fix->smem_start = external_addr;
 	fix->smem_len = PAGE_ALIGN(external_len);
 	if (external_depth == 1) {
 		fix->type = FB_TYPE_PACKED_PIXELS;
@@ -2213,7 +2214,7 @@ static int ext_encode_var(struct fb_var_screeninfo *var, struct atafb_par *par)
 
 static void ext_get_par(struct atafb_par *par)
 {
-	par->screen_base = external_addr;
+	par->screen_base = external_screen_base;
 }
 
 static void ext_set_par(struct atafb_par *par)
@@ -2286,7 +2287,7 @@ static void set_screen_base(void *s_base)
 {
 	unsigned long addr;
 
-	addr = virt_to_phys(s_base);
+	addr = atari_stram_to_phys(s_base);
 	/* Setup Screen Memory */
 	shifter.bas_hi = (unsigned char)((addr & 0xff0000) >> 16);
 	shifter.bas_md = (unsigned char)((addr & 0x00ff00) >> 8);
@@ -2433,7 +2434,9 @@ static void atafb_set_disp(struct fb_info *info)
 	atafb_get_var(&info->var, info);
 	atafb_get_fix(&info->fix, info);
 
-	info->screen_base = (void *)info->fix.smem_start;
+	/* Note: smem_start derives from phys_screen_base, not screen_base! */
+	info->screen_base = (external_addr ? external_screen_base :
+				atari_stram_to_virt(info->fix.smem_start));
 }
 
 static int atafb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
@@ -2904,7 +2907,7 @@ static void __init atafb_setup_ext(char *spec)
 	external_yres = yres;
 	external_depth = depth;
 	external_pmode = planes;
-	external_addr = (void *)addr;
+	external_addr = addr;
 	external_len = len;
 
 	if (external_card_type == IS_MV300) {
@@ -3166,30 +3169,30 @@ int __init atafb_init(void)
 		memset(screen_base, 0, mem_req);
 		pad = -(unsigned long)screen_base & (PAGE_SIZE - 1);
 		screen_base += pad;
-		real_screen_base = screen_base + ovsc_offset;
+		phys_screen_base = atari_stram_to_phys(screen_base + ovsc_offset);
 		screen_len = (mem_req - pad - ovsc_offset) & PAGE_MASK;
 		st_ovsc_switch();
 		if (CPU_IS_040_OR_060) {
 			/* On a '040+, the cache mode of video RAM must be set to
 			 * write-through also for internal video hardware! */
-			cache_push(virt_to_phys(screen_base), screen_len);
+			cache_push(atari_stram_to_phys(screen_base), screen_len);
 			kernel_set_cachemode(screen_base, screen_len,
 					     IOMAP_WRITETHROUGH);
 		}
-		printk("atafb: screen_base %p real_screen_base %p screen_len %d\n",
-			screen_base, real_screen_base, screen_len);
+		printk("atafb: screen_base %p phys_screen_base %lx screen_len %d\n",
+			screen_base, phys_screen_base, screen_len);
 #ifdef ATAFB_EXT
 	} else {
 		/* Map the video memory (physical address given) to somewhere
 		 * in the kernel address space.
 		 */
-		external_addr = ioremap_writethrough((unsigned long)external_addr,
+		external_screen_base = ioremap_writethrough(external_addr,
 						     external_len);
 		if (external_vgaiobase)
 			external_vgaiobase =
 			  (unsigned long)ioremap(external_vgaiobase, 0x10000);
-		screen_base =
-		real_screen_base = external_addr;
+		screen_base = external_screen_base;
+		phys_screen_base = external_addr;
 		screen_len = external_len & PAGE_MASK;
 		memset (screen_base, 0, external_len);
 	}
@@ -3235,8 +3238,8 @@ int __init atafb_init(void)
 	if (register_framebuffer(&fb_info) < 0) {
 #ifdef ATAFB_EXT
 		if (external_addr) {
-			iounmap(external_addr);
-			external_addr = NULL;
+			iounmap(external_screen_base);
+			external_addr = 0;
 		}
 		if (external_vgaiobase) {
 			iounmap((void*)external_vgaiobase);
diff --git a/drivers/vme/bridges/vme_tsi148.c b/drivers/vme/bridges/vme_tsi148.c
index 06990c6a1a6..61e706c0e00 100644
--- a/drivers/vme/bridges/vme_tsi148.c
+++ b/drivers/vme/bridges/vme_tsi148.c
@@ -320,7 +320,7 @@ static int tsi148_irq_init(struct vme_bridge *tsi148_bridge)
 	struct pci_dev *pdev;
 	struct tsi148_driver *bridge;
 
-	pdev = container_of(tsi148_bridge->parent, struct pci_dev, dev);
+	pdev = to_pci_dev(tsi148_bridge->parent);
 
 	bridge = tsi148_bridge->driver_priv;
 
@@ -433,9 +433,7 @@ static void tsi148_irq_set(struct vme_bridge *tsi148_bridge, int level,
 		iowrite32be(tmp, bridge->base + TSI148_LCSR_INTEO);
 
 		if (sync != 0) {
-			pdev = container_of(tsi148_bridge->parent,
-				struct pci_dev, dev);
-
+			pdev = to_pci_dev(tsi148_bridge->parent);
 			synchronize_irq(pdev->irq);
 		}
 	} else {
@@ -741,7 +739,7 @@ static int tsi148_slave_get(struct vme_slave_resource *image, int *enabled,
 	reg_join(vme_bound_high, vme_bound_low, &vme_bound);
 	reg_join(pci_offset_high, pci_offset_low, &pci_offset);
 
-	*pci_base = (dma_addr_t)vme_base + pci_offset;
+	*pci_base = (dma_addr_t)(*vme_base + pci_offset);
 
 	*enabled = 0;
 	*aspace = 0;
@@ -814,7 +812,7 @@ static int tsi148_alloc_resource(struct vme_master_resource *image,
 
 	tsi148_bridge = image->parent;
 
-	pdev = container_of(tsi148_bridge->parent, struct pci_dev, dev);
+	pdev = to_pci_dev(tsi148_bridge->parent);
 
 	existing_size = (unsigned long long)(image->bus_resource.end -
 		image->bus_resource.start);
@@ -910,11 +908,15 @@ static int tsi148_master_set(struct vme_master_resource *image, int enabled,
 	unsigned long long pci_bound, vme_offset, pci_base;
 	struct vme_bridge *tsi148_bridge;
 	struct tsi148_driver *bridge;
+	struct pci_bus_region region;
+	struct pci_dev *pdev;
 
 	tsi148_bridge = image->parent;
 
 	bridge = tsi148_bridge->driver_priv;
 
+	pdev = to_pci_dev(tsi148_bridge->parent);
+
 	/* Verify input data */
 	if (vme_base & 0xFFFF) {
 		dev_err(tsi148_bridge->parent, "Invalid VME Window "
@@ -949,7 +951,9 @@ static int tsi148_master_set(struct vme_master_resource *image, int enabled,
 		pci_bound = 0;
 		vme_offset = 0;
 	} else {
-		pci_base = (unsigned long long)image->bus_resource.start;
+		pcibios_resource_to_bus(pdev->bus, &region,
+					&image->bus_resource);
+		pci_base = region.start;
 
 		/*
 		 * Bound address is a valid address for the window, adjust
@@ -2232,7 +2236,7 @@ static void *tsi148_alloc_consistent(struct device *parent, size_t size,
 	struct pci_dev *pdev;
 
 	/* Find pci_dev container of dev */
-	pdev = container_of(parent, struct pci_dev, dev);
+	pdev = to_pci_dev(parent);
 
 	return pci_alloc_consistent(pdev, size, dma);
 }
@@ -2243,7 +2247,7 @@ static void tsi148_free_consistent(struct device *parent, size_t size,
 	struct pci_dev *pdev;
 
 	/* Find pci_dev container of dev */
-	pdev = container_of(parent, struct pci_dev, dev);
+	pdev = to_pci_dev(parent);
 
 	pci_free_consistent(pdev, size, vaddr, dma);
 }
diff --git a/drivers/w1/w1.c b/drivers/w1/w1.c
index b96f61b15dc..5d734152054 100644
--- a/drivers/w1/w1.c
+++ b/drivers/w1/w1.c
@@ -614,27 +614,11 @@ end:
 	return err;
 }
 
-/*
- * Handle sysfs file creation and removal here, before userspace is told that
- * the device is added / removed from the system
- */
-static int w1_bus_notify(struct notifier_block *nb, unsigned long action,
-			 void *data)
+static int w1_family_notify(unsigned long action, struct w1_slave *sl)
 {
-	struct device *dev = data;
-	struct w1_slave *sl;
 	struct w1_family_ops *fops;
 	int err;
 
-	/*
-	 * Only care about slave devices at the moment.  Yes, we should use a
-	 * separate "type" for this, but for now, look at the release function
-	 * to know which type it is...
-	 */
-	if (dev->release != w1_slave_release)
-		return 0;
-
-	sl = dev_to_w1_slave(dev);
 	fops = sl->family->fops;
 
 	if (!fops)
@@ -673,10 +657,6 @@ static int w1_bus_notify(struct notifier_block *nb, unsigned long action,
 	return 0;
 }
 
-static struct notifier_block w1_bus_nb = {
-	.notifier_call = w1_bus_notify,
-};
-
 static int __w1_attach_slave_device(struct w1_slave *sl)
 {
 	int err;
@@ -698,6 +678,9 @@ static int __w1_attach_slave_device(struct w1_slave *sl)
 	dev_dbg(&sl->dev, "%s: registering %s as %p.\n", __func__,
 		dev_name(&sl->dev), sl);
 
+	/* suppress for w1_family_notify before sending KOBJ_ADD */
+	dev_set_uevent_suppress(&sl->dev, true);
+
 	err = device_register(&sl->dev);
 	if (err < 0) {
 		dev_err(&sl->dev,
@@ -705,7 +688,7 @@ static int __w1_attach_slave_device(struct w1_slave *sl)
 			dev_name(&sl->dev), err);
 		return err;
 	}
-
+	w1_family_notify(BUS_NOTIFY_ADD_DEVICE, sl);
 
 	dev_set_uevent_suppress(&sl->dev, false);
 	kobject_uevent(&sl->dev.kobj, KOBJ_ADD);
@@ -799,6 +782,7 @@ int w1_unref_slave(struct w1_slave *sl)
 		msg.type = W1_SLAVE_REMOVE;
 		w1_netlink_send(sl->master, &msg);
 
+		w1_family_notify(BUS_NOTIFY_DEL_DEVICE, sl);
 		device_unregister(&sl->dev);
 		#ifdef DEBUG
 		memset(sl, 0, sizeof(*sl));
@@ -1094,6 +1078,8 @@ static void w1_search_process(struct w1_master *dev, u8 search_type)
  * w1_process_callbacks() - execute each dev->async_list callback entry
  * @dev: w1_master device
  *
+ * The w1 master list_mutex must be held.
+ *
  * Return: 1 if there were commands to executed 0 otherwise
  */
 int w1_process_callbacks(struct w1_master *dev)
@@ -1186,10 +1172,6 @@ static int __init w1_init(void)
 		goto err_out_exit_init;
 	}
 
-	retval = bus_register_notifier(&w1_bus_type, &w1_bus_nb);
-	if (retval)
-		goto err_out_bus_unregister;
-
 	retval = driver_register(&w1_master_driver);
 	if (retval) {
 		printk(KERN_ERR
diff --git a/drivers/w1/w1.h b/drivers/w1/w1.h
index 734dab7fc68..56a49ba41d8 100644
--- a/drivers/w1/w1.h
+++ b/drivers/w1/w1.h
@@ -203,7 +203,6 @@ enum w1_master_flags {
  * @search_id:		allows continuing a search
  * @refcnt:		reference count
  * @priv:		private data storage
- * @priv_size:		size allocated
  * @enable_pullup:	allows a strong pullup
  * @pullup_duration:	time for the next strong pullup
  * @flags:		one of w1_master_flags
@@ -214,7 +213,6 @@ enum w1_master_flags {
  * @dev:		sysfs device
  * @bus_master:		io operations available
  * @seq:		sequence number used for netlink broadcasts
- * @portid:		destination for the current netlink command
  */
 struct w1_master
 {
@@ -241,7 +239,6 @@ struct w1_master
 	atomic_t		refcnt;
 
 	void			*priv;
-	int			priv_size;
 
 	/** 5V strong pullup enabled flag, 1 enabled, zero disabled. */
 	int			enable_pullup;
@@ -260,11 +257,6 @@ struct w1_master
 	struct w1_bus_master	*bus_master;
 
 	u32			seq;
-	/* port id to send netlink responses to.  The value is temporarily
-	 * stored here while processing a message, set after locking the
-	 * mutex, zero before unlocking the mutex.
-	 */
-	u32			portid;
 };
 
 /**
diff --git a/drivers/w1/w1_int.c b/drivers/w1/w1_int.c
index 9b084db739c..728039d2efe 100644
--- a/drivers/w1/w1_int.c
+++ b/drivers/w1/w1_int.c
@@ -219,9 +219,13 @@ void __w1_remove_master_device(struct w1_master *dev)
 
 		if (msleep_interruptible(1000))
 			flush_signals(current);
+		mutex_lock(&dev->list_mutex);
 		w1_process_callbacks(dev);
+		mutex_unlock(&dev->list_mutex);
 	}
+	mutex_lock(&dev->list_mutex);
 	w1_process_callbacks(dev);
+	mutex_unlock(&dev->list_mutex);
 
 	memset(&msg, 0, sizeof(msg));
 	msg.id.mst.id = dev->id;
diff --git a/drivers/w1/w1_netlink.c b/drivers/w1/w1_netlink.c
index 5234964fe00..351a2978ba7 100644
--- a/drivers/w1/w1_netlink.c
+++ b/drivers/w1/w1_netlink.c
@@ -29,51 +29,247 @@
 #include "w1_netlink.h"
 
 #if defined(CONFIG_W1_CON) && (defined(CONFIG_CONNECTOR) || (defined(CONFIG_CONNECTOR_MODULE) && defined(CONFIG_W1_MODULE)))
-void w1_netlink_send(struct w1_master *dev, struct w1_netlink_msg *msg)
+
+#define MIN(a, b)                   (((a) < (b)) ? (a) : (b))
+
+/* Bundle together everything required to process a request in one memory
+ * allocation.
+ */
+struct w1_cb_block {
+	atomic_t refcnt;
+	u32 portid; /* Sending process port ID */
+	/* maximum value for first_cn->len */
+	u16 maxlen;
+	/* pointers to building up the reply message */
+	struct cn_msg *first_cn; /* fixed once the structure is populated */
+	struct cn_msg *cn; /* advances as cn_msg is appeneded */
+	struct w1_netlink_msg *msg; /* advances as w1_netlink_msg is appened */
+	struct w1_netlink_cmd *cmd; /* advances as cmds are appened */
+	struct w1_netlink_msg *cur_msg; /* currently message being processed */
+	/* copy of the original request follows */
+	struct cn_msg request_cn;
+	/* followed by variable length:
+	 * cn_msg, data (w1_netlink_msg and w1_netlink_cmd)
+	 * one or more struct w1_cb_node
+	 * reply first_cn, data (w1_netlink_msg and w1_netlink_cmd)
+	 */
+};
+struct w1_cb_node {
+	struct w1_async_cmd async;
+	/* pointers within w1_cb_block and cn data */
+	struct w1_cb_block *block;
+	struct w1_netlink_msg *msg;
+	struct w1_slave *sl;
+	struct w1_master *dev;
+};
+
+/**
+ * w1_reply_len() - calculate current reply length, compare to maxlen
+ * @block: block to calculate
+ *
+ * Calculates the current message length including possible multiple
+ * cn_msg and data, excludes the first sizeof(struct cn_msg).  Direclty
+ * compariable to maxlen and usable to send the message.
+ */
+static u16 w1_reply_len(struct w1_cb_block *block)
+{
+	if (!block->cn)
+		return 0;
+	return (u8 *)block->cn - (u8 *)block->first_cn + block->cn->len;
+}
+
+static void w1_unref_block(struct w1_cb_block *block)
+{
+	if (atomic_sub_return(1, &block->refcnt) == 0) {
+		u16 len = w1_reply_len(block);
+		if (len) {
+			cn_netlink_send_mult(block->first_cn, len,
+				block->portid, 0, GFP_KERNEL);
+		}
+		kfree(block);
+	}
+}
+
+/**
+ * w1_reply_make_space() - send message if needed to make space
+ * @block: block to make space on
+ * @space: how many bytes requested
+ *
+ * Verify there is enough room left for the caller to add "space" bytes to the
+ * message, if there isn't send the message and reset.
+ */
+static void w1_reply_make_space(struct w1_cb_block *block, u16 space)
+{
+	u16 len = w1_reply_len(block);
+	if (len + space >= block->maxlen) {
+		cn_netlink_send_mult(block->first_cn, len, block->portid, 0, GFP_KERNEL);
+		block->first_cn->len = 0;
+		block->cn = NULL;
+		block->msg = NULL;
+		block->cmd = NULL;
+	}
+}
+
+/* Early send when replies aren't bundled. */
+static void w1_netlink_check_send(struct w1_cb_block *block)
+{
+	if (!(block->request_cn.flags & W1_CN_BUNDLE) && block->cn)
+		w1_reply_make_space(block, block->maxlen);
+}
+
+/**
+ * w1_netlink_setup_msg() - prepare to write block->msg
+ * @block: block to operate on
+ * @ack: determines if cn can be reused
+ *
+ * block->cn will be setup with the correct ack, advancing if needed
+ * block->cn->len does not include space for block->msg
+ * block->msg advances but remains uninitialized
+ */
+static void w1_netlink_setup_msg(struct w1_cb_block *block, u32 ack)
 {
-	char buf[sizeof(struct cn_msg) + sizeof(struct w1_netlink_msg)];
-	struct cn_msg *m = (struct cn_msg *)buf;
-	struct w1_netlink_msg *w = (struct w1_netlink_msg *)(m+1);
+	if (block->cn && block->cn->ack == ack) {
+		block->msg = (struct w1_netlink_msg *)(block->cn->data + block->cn->len);
+	} else {
+		/* advance or set to data */
+		if (block->cn)
+			block->cn = (struct cn_msg *)(block->cn->data +
+				block->cn->len);
+		else
+			block->cn = block->first_cn;
 
-	memset(buf, 0, sizeof(buf));
+		memcpy(block->cn, &block->request_cn, sizeof(*block->cn));
+		block->cn->len = 0;
+		block->cn->ack = ack;
+		block->msg = (struct w1_netlink_msg *)block->cn->data;
+	}
+}
 
-	m->id.idx = CN_W1_IDX;
-	m->id.val = CN_W1_VAL;
+/* Append cmd to msg, include cmd->data as well.  This is because
+ * any following data goes with the command and in the case of a read is
+ * the results.
+ */
+static void w1_netlink_queue_cmd(struct w1_cb_block *block,
+	struct w1_netlink_cmd *cmd)
+{
+	u32 space;
+	w1_reply_make_space(block, sizeof(struct cn_msg) +
+		sizeof(struct w1_netlink_msg) + sizeof(*cmd) + cmd->len);
 
-	m->seq = dev->seq++;
-	m->len = sizeof(struct w1_netlink_msg);
+	/* There's a status message sent after each command, so no point
+	 * in trying to bundle this cmd after an existing one, because
+	 * there won't be one.  Allocate and copy over a new cn_msg.
+	 */
+	w1_netlink_setup_msg(block, block->request_cn.seq + 1);
+	memcpy(block->msg, block->cur_msg, sizeof(*block->msg));
+	block->cn->len += sizeof(*block->msg);
+	block->msg->len = 0;
+	block->cmd = (struct w1_netlink_cmd *)(block->msg->data);
+
+	space = sizeof(*cmd) + cmd->len;
+	if (block->cmd != cmd)
+		memcpy(block->cmd, cmd, space);
+	block->cn->len += space;
+	block->msg->len += space;
+}
 
-	memcpy(w, msg, sizeof(struct w1_netlink_msg));
+/* Append req_msg and req_cmd, no other commands and no data from req_cmd are
+ * copied.
+ */
+static void w1_netlink_queue_status(struct w1_cb_block *block,
+	struct w1_netlink_msg *req_msg, struct w1_netlink_cmd *req_cmd,
+	int error)
+{
+	u16 space = sizeof(struct cn_msg) + sizeof(*req_msg) + sizeof(*req_cmd);
+	w1_reply_make_space(block, space);
+	w1_netlink_setup_msg(block, block->request_cn.ack);
+
+	memcpy(block->msg, req_msg, sizeof(*req_msg));
+	block->cn->len += sizeof(*req_msg);
+	block->msg->len = 0;
+	block->msg->status = (u8)-error;
+	if (req_cmd) {
+		struct w1_netlink_cmd *cmd = (struct w1_netlink_cmd *)block->msg->data;
+		memcpy(cmd, req_cmd, sizeof(*cmd));
+		block->cn->len += sizeof(*cmd);
+		block->msg->len += sizeof(*cmd);
+		cmd->len = 0;
+	}
+	w1_netlink_check_send(block);
+}
 
-	cn_netlink_send(m, dev->portid, 0, GFP_KERNEL);
+/**
+ * w1_netlink_send_error() - sends the error message now
+ * @cn: original cn_msg
+ * @msg: original w1_netlink_msg
+ * @portid: where to send it
+ * @error: error status
+ *
+ * Use when a block isn't available to queue the message to and cn, msg
+ * might not be contiguous.
+ */
+static void w1_netlink_send_error(struct cn_msg *cn, struct w1_netlink_msg *msg,
+	int portid, int error)
+{
+	struct {
+		struct cn_msg cn;
+		struct w1_netlink_msg msg;
+	} packet;
+	memcpy(&packet.cn, cn, sizeof(packet.cn));
+	memcpy(&packet.msg, msg, sizeof(packet.msg));
+	packet.cn.len = sizeof(packet.msg);
+	packet.msg.len = 0;
+	packet.msg.status = (u8)-error;
+	cn_netlink_send(&packet.cn, portid, 0, GFP_KERNEL);
+}
+
+/**
+ * w1_netlink_send() - sends w1 netlink notifications
+ * @dev: w1_master the even is associated with or for
+ * @msg: w1_netlink_msg message to be sent
+ *
+ * This are notifications generated from the kernel.
+ */
+void w1_netlink_send(struct w1_master *dev, struct w1_netlink_msg *msg)
+{
+	struct {
+		struct cn_msg cn;
+		struct w1_netlink_msg msg;
+	} packet;
+	memset(&packet, 0, sizeof(packet));
+
+	packet.cn.id.idx = CN_W1_IDX;
+	packet.cn.id.val = CN_W1_VAL;
+
+	packet.cn.seq = dev->seq++;
+	packet.cn.len = sizeof(*msg);
+
+	memcpy(&packet.msg, msg, sizeof(*msg));
+	packet.msg.len = 0;
+
+	cn_netlink_send(&packet.cn, 0, 0, GFP_KERNEL);
 }
 
 static void w1_send_slave(struct w1_master *dev, u64 rn)
 {
-	struct cn_msg *msg = dev->priv;
-	struct w1_netlink_msg *hdr = (struct w1_netlink_msg *)(msg + 1);
-	struct w1_netlink_cmd *cmd = (struct w1_netlink_cmd *)(hdr + 1);
-	int avail;
+	struct w1_cb_block *block = dev->priv;
+	struct w1_netlink_cmd *cache_cmd = block->cmd;
 	u64 *data;
 
-	avail = dev->priv_size - cmd->len;
-
-	if (avail < 8) {
-		msg->ack++;
-		cn_netlink_send(msg, dev->portid, 0, GFP_KERNEL);
+	w1_reply_make_space(block, sizeof(*data));
 
-		msg->len = sizeof(struct w1_netlink_msg) +
-			sizeof(struct w1_netlink_cmd);
-		hdr->len = sizeof(struct w1_netlink_cmd);
-		cmd->len = 0;
+	/* Add cmd back if the packet was sent */
+	if (!block->cmd) {
+		cache_cmd->len = 0;
+		w1_netlink_queue_cmd(block, cache_cmd);
 	}
 
-	data = (void *)(cmd + 1) + cmd->len;
+	data = (u64 *)(block->cmd->data + block->cmd->len);
 
 	*data = rn;
-	cmd->len += 8;
-	hdr->len += 8;
-	msg->len += 8;
+	block->cn->len += sizeof(*data);
+	block->msg->len += sizeof(*data);
+	block->cmd->len += sizeof(*data);
 }
 
 static void w1_found_send_slave(struct w1_master *dev, u64 rn)
@@ -85,40 +281,15 @@ static void w1_found_send_slave(struct w1_master *dev, u64 rn)
 }
 
 /* Get the current slave list, or search (with or without alarm) */
-static int w1_get_slaves(struct w1_master *dev,
-		struct cn_msg *req_msg, struct w1_netlink_msg *req_hdr,
-		struct w1_netlink_cmd *req_cmd)
+static int w1_get_slaves(struct w1_master *dev, struct w1_netlink_cmd *req_cmd)
 {
-	struct cn_msg *msg;
-	struct w1_netlink_msg *hdr;
-	struct w1_netlink_cmd *cmd;
 	struct w1_slave *sl;
 
-	msg = kzalloc(PAGE_SIZE, GFP_KERNEL);
-	if (!msg)
-		return -ENOMEM;
-
-	msg->id = req_msg->id;
-	msg->seq = req_msg->seq;
-	msg->ack = 0;
-	msg->len = sizeof(struct w1_netlink_msg) +
-		sizeof(struct w1_netlink_cmd);
-
-	hdr = (struct w1_netlink_msg *)(msg + 1);
-	cmd = (struct w1_netlink_cmd *)(hdr + 1);
-
-	hdr->type = W1_MASTER_CMD;
-	hdr->id = req_hdr->id;
-	hdr->len = sizeof(struct w1_netlink_cmd);
-
-	cmd->cmd = req_cmd->cmd;
-	cmd->len = 0;
-
-	dev->priv = msg;
-	dev->priv_size = PAGE_SIZE - msg->len - sizeof(struct cn_msg);
+	req_cmd->len = 0;
+	w1_netlink_queue_cmd(dev->priv, req_cmd);
 
 	if (req_cmd->cmd == W1_CMD_LIST_SLAVES) {
-		__u64 rn;
+		u64 rn;
 		mutex_lock(&dev->list_mutex);
 		list_for_each_entry(sl, &dev->slist, w1_slave_entry) {
 			memcpy(&rn, &sl->reg_num, sizeof(rn));
@@ -126,73 +297,26 @@ static int w1_get_slaves(struct w1_master *dev,
 		}
 		mutex_unlock(&dev->list_mutex);
 	} else {
-		w1_search_process_cb(dev, cmd->cmd == W1_CMD_ALARM_SEARCH ?
+		w1_search_process_cb(dev, req_cmd->cmd == W1_CMD_ALARM_SEARCH ?
 			W1_ALARM_SEARCH : W1_SEARCH, w1_found_send_slave);
 	}
 
-	msg->ack = 0;
-	cn_netlink_send(msg, dev->portid, 0, GFP_KERNEL);
-
-	dev->priv = NULL;
-	dev->priv_size = 0;
-
-	kfree(msg);
-
 	return 0;
 }
 
-static int w1_send_read_reply(struct cn_msg *msg, struct w1_netlink_msg *hdr,
-		struct w1_netlink_cmd *cmd, u32 portid)
-{
-	void *data;
-	struct w1_netlink_msg *h;
-	struct w1_netlink_cmd *c;
-	struct cn_msg *cm;
-	int err;
-
-	data = kzalloc(sizeof(struct cn_msg) +
-			sizeof(struct w1_netlink_msg) +
-			sizeof(struct w1_netlink_cmd) +
-			cmd->len, GFP_KERNEL);
-	if (!data)
-		return -ENOMEM;
-
-	cm = (struct cn_msg *)(data);
-	h = (struct w1_netlink_msg *)(cm + 1);
-	c = (struct w1_netlink_cmd *)(h + 1);
-
-	memcpy(cm, msg, sizeof(struct cn_msg));
-	memcpy(h, hdr, sizeof(struct w1_netlink_msg));
-	memcpy(c, cmd, sizeof(struct w1_netlink_cmd));
-
-	cm->ack = msg->seq+1;
-	cm->len = sizeof(struct w1_netlink_msg) +
-		sizeof(struct w1_netlink_cmd) + cmd->len;
-
-	h->len = sizeof(struct w1_netlink_cmd) + cmd->len;
-
-	memcpy(c->data, cmd->data, c->len);
-
-	err = cn_netlink_send(cm, portid, 0, GFP_KERNEL);
-
-	kfree(data);
-
-	return err;
-}
-
-static int w1_process_command_io(struct w1_master *dev, struct cn_msg *msg,
-		struct w1_netlink_msg *hdr, struct w1_netlink_cmd *cmd)
+static int w1_process_command_io(struct w1_master *dev,
+	struct w1_netlink_cmd *cmd)
 {
 	int err = 0;
 
 	switch (cmd->cmd) {
 	case W1_CMD_TOUCH:
 		w1_touch_block(dev, cmd->data, cmd->len);
-		w1_send_read_reply(msg, hdr, cmd, dev->portid);
+		w1_netlink_queue_cmd(dev->priv, cmd);
 		break;
 	case W1_CMD_READ:
 		w1_read_block(dev, cmd->data, cmd->len);
-		w1_send_read_reply(msg, hdr, cmd, dev->portid);
+		w1_netlink_queue_cmd(dev->priv, cmd);
 		break;
 	case W1_CMD_WRITE:
 		w1_write_block(dev, cmd->data, cmd->len);
@@ -206,14 +330,13 @@ static int w1_process_command_io(struct w1_master *dev, struct cn_msg *msg,
 }
 
 static int w1_process_command_addremove(struct w1_master *dev,
-	struct cn_msg *msg, struct w1_netlink_msg *hdr,
 	struct w1_netlink_cmd *cmd)
 {
 	struct w1_slave *sl;
 	int err = 0;
 	struct w1_reg_num *id;
 
-	if (cmd->len != 8)
+	if (cmd->len != sizeof(*id))
 		return -EINVAL;
 
 	id = (struct w1_reg_num *)cmd->data;
@@ -241,7 +364,6 @@ static int w1_process_command_addremove(struct w1_master *dev,
 }
 
 static int w1_process_command_master(struct w1_master *dev,
-	struct cn_msg *req_msg, struct w1_netlink_msg *req_hdr,
 	struct w1_netlink_cmd *req_cmd)
 {
 	int err = -EINVAL;
@@ -254,13 +376,13 @@ static int w1_process_command_master(struct w1_master *dev,
 	case W1_CMD_ALARM_SEARCH:
 	case W1_CMD_LIST_SLAVES:
 		mutex_unlock(&dev->bus_mutex);
-		err = w1_get_slaves(dev, req_msg, req_hdr, req_cmd);
+		err = w1_get_slaves(dev, req_cmd);
 		mutex_lock(&dev->bus_mutex);
 		break;
 	case W1_CMD_READ:
 	case W1_CMD_WRITE:
 	case W1_CMD_TOUCH:
-		err = w1_process_command_io(dev, req_msg, req_hdr, req_cmd);
+		err = w1_process_command_io(dev, req_cmd);
 		break;
 	case W1_CMD_RESET:
 		err = w1_reset_bus(dev);
@@ -269,8 +391,7 @@ static int w1_process_command_master(struct w1_master *dev,
 	case W1_CMD_SLAVE_REMOVE:
 		mutex_unlock(&dev->bus_mutex);
 		mutex_lock(&dev->mutex);
-		err = w1_process_command_addremove(dev, req_msg, req_hdr,
-			req_cmd);
+		err = w1_process_command_addremove(dev, req_cmd);
 		mutex_unlock(&dev->mutex);
 		mutex_lock(&dev->bus_mutex);
 		break;
@@ -282,30 +403,23 @@ static int w1_process_command_master(struct w1_master *dev,
 	return err;
 }
 
-static int w1_process_command_slave(struct w1_slave *sl, struct cn_msg *msg,
-		struct w1_netlink_msg *hdr, struct w1_netlink_cmd *cmd)
+static int w1_process_command_slave(struct w1_slave *sl,
+		struct w1_netlink_cmd *cmd)
 {
 	dev_dbg(&sl->master->dev, "%s: %02x.%012llx.%02x: cmd=%02x, len=%u.\n",
 		__func__, sl->reg_num.family, (unsigned long long)sl->reg_num.id,
 		sl->reg_num.crc, cmd->cmd, cmd->len);
 
-	return w1_process_command_io(sl->master, msg, hdr, cmd);
+	return w1_process_command_io(sl->master, cmd);
 }
 
-static int w1_process_command_root(struct cn_msg *msg,
-	struct w1_netlink_msg *mcmd, u32 portid)
+static int w1_process_command_root(struct cn_msg *req_cn, u32 portid)
 {
-	struct w1_master *m;
+	struct w1_master *dev;
 	struct cn_msg *cn;
-	struct w1_netlink_msg *w;
+	struct w1_netlink_msg *msg;
 	u32 *id;
 
-	if (mcmd->type != W1_LIST_MASTERS) {
-		printk(KERN_NOTICE "%s: msg: %x.%x, wrong type: %u, len: %u.\n",
-			__func__, msg->id.idx, msg->id.val, mcmd->type, mcmd->len);
-		return -EPROTO;
-	}
-
 	cn = kmalloc(PAGE_SIZE, GFP_KERNEL);
 	if (!cn)
 		return -ENOMEM;
@@ -313,32 +427,30 @@ static int w1_process_command_root(struct cn_msg *msg,
 	cn->id.idx = CN_W1_IDX;
 	cn->id.val = CN_W1_VAL;
 
-	cn->seq = msg->seq;
-	cn->ack = 1;
+	cn->seq = req_cn->seq;
+	cn->ack = req_cn->seq + 1;
 	cn->len = sizeof(struct w1_netlink_msg);
-	w = (struct w1_netlink_msg *)(cn + 1);
+	msg = (struct w1_netlink_msg *)cn->data;
 
-	w->type = W1_LIST_MASTERS;
-	w->status = 0;
-	w->len = 0;
-	id = (u32 *)(w + 1);
+	msg->type = W1_LIST_MASTERS;
+	msg->status = 0;
+	msg->len = 0;
+	id = (u32 *)msg->data;
 
 	mutex_lock(&w1_mlock);
-	list_for_each_entry(m, &w1_masters, w1_master_entry) {
+	list_for_each_entry(dev, &w1_masters, w1_master_entry) {
 		if (cn->len + sizeof(*id) > PAGE_SIZE - sizeof(struct cn_msg)) {
 			cn_netlink_send(cn, portid, 0, GFP_KERNEL);
-			cn->ack++;
 			cn->len = sizeof(struct w1_netlink_msg);
-			w->len = 0;
-			id = (u32 *)(w + 1);
+			msg->len = 0;
+			id = (u32 *)msg->data;
 		}
 
-		*id = m->id;
-		w->len += sizeof(*id);
+		*id = dev->id;
+		msg->len += sizeof(*id);
 		cn->len += sizeof(*id);
 		id++;
 	}
-	cn->ack = 0;
 	cn_netlink_send(cn, portid, 0, GFP_KERNEL);
 	mutex_unlock(&w1_mlock);
 
@@ -346,187 +458,233 @@ static int w1_process_command_root(struct cn_msg *msg,
 	return 0;
 }
 
-static int w1_netlink_send_error(struct cn_msg *rcmsg, struct w1_netlink_msg *rmsg,
-		struct w1_netlink_cmd *rcmd, int portid, int error)
-{
-	struct cn_msg *cmsg;
-	struct w1_netlink_msg *msg;
-	struct w1_netlink_cmd *cmd;
-
-	cmsg = kzalloc(sizeof(*msg) + sizeof(*cmd) + sizeof(*cmsg), GFP_KERNEL);
-	if (!cmsg)
-		return -ENOMEM;
-
-	msg = (struct w1_netlink_msg *)(cmsg + 1);
-	cmd = (struct w1_netlink_cmd *)(msg + 1);
-
-	memcpy(cmsg, rcmsg, sizeof(*cmsg));
-	cmsg->len = sizeof(*msg);
-
-	memcpy(msg, rmsg, sizeof(*msg));
-	msg->len = 0;
-	msg->status = (short)-error;
-
-	if (rcmd) {
-		memcpy(cmd, rcmd, sizeof(*cmd));
-		cmd->len = 0;
-		msg->len += sizeof(*cmd);
-		cmsg->len += sizeof(*cmd);
-	}
-
-	error = cn_netlink_send(cmsg, portid, 0, GFP_KERNEL);
-	kfree(cmsg);
-
-	return error;
-}
-
-/* Bundle together a reference count, the full message, and broken out
- * commands to be executed on each w1 master kthread in one memory allocation.
- */
-struct w1_cb_block {
-	atomic_t refcnt;
-	u32 portid; /* Sending process port ID */
-	struct cn_msg msg;
-	/* cn_msg data */
-	/* one or more variable length struct w1_cb_node */
-};
-struct w1_cb_node {
-	struct w1_async_cmd async;
-	/* pointers within w1_cb_block and msg data */
-	struct w1_cb_block *block;
-	struct w1_netlink_msg *m;
-	struct w1_slave *sl;
-	struct w1_master *dev;
-};
-
 static void w1_process_cb(struct w1_master *dev, struct w1_async_cmd *async_cmd)
 {
 	struct w1_cb_node *node = container_of(async_cmd, struct w1_cb_node,
 		async);
-	u16 mlen = node->m->len;
-	u8 *cmd_data = node->m->data;
+	u16 mlen = node->msg->len;
+	u16 len;
 	int err = 0;
 	struct w1_slave *sl = node->sl;
-	struct w1_netlink_cmd *cmd = NULL;
+	struct w1_netlink_cmd *cmd = (struct w1_netlink_cmd *)node->msg->data;
 
 	mutex_lock(&dev->bus_mutex);
-	dev->portid = node->block->portid;
+	dev->priv = node->block;
 	if (sl && w1_reset_select_slave(sl))
 		err = -ENODEV;
+	node->block->cur_msg = node->msg;
 
 	while (mlen && !err) {
-		cmd = (struct w1_netlink_cmd *)cmd_data;
-
 		if (cmd->len + sizeof(struct w1_netlink_cmd) > mlen) {
 			err = -E2BIG;
 			break;
 		}
 
 		if (sl)
-			err = w1_process_command_slave(sl, &node->block->msg,
-				node->m, cmd);
+			err = w1_process_command_slave(sl, cmd);
 		else
-			err = w1_process_command_master(dev, &node->block->msg,
-				node->m, cmd);
+			err = w1_process_command_master(dev, cmd);
+		w1_netlink_check_send(node->block);
 
-		w1_netlink_send_error(&node->block->msg, node->m, cmd,
-			node->block->portid, err);
+		w1_netlink_queue_status(node->block, node->msg, cmd, err);
 		err = 0;
 
-		cmd_data += cmd->len + sizeof(struct w1_netlink_cmd);
-		mlen -= cmd->len + sizeof(struct w1_netlink_cmd);
+		len = sizeof(*cmd) + cmd->len;
+		cmd = (struct w1_netlink_cmd *)((u8 *)cmd + len);
+		mlen -= len;
 	}
 
 	if (!cmd || err)
-		w1_netlink_send_error(&node->block->msg, node->m, cmd,
-			node->block->portid, err);
+		w1_netlink_queue_status(node->block, node->msg, cmd, err);
 
+	/* ref taken in w1_search_slave or w1_search_master_id when building
+	 * the block
+	 */
 	if (sl)
 		w1_unref_slave(sl);
 	else
 		atomic_dec(&dev->refcnt);
-	dev->portid = 0;
+	dev->priv = NULL;
 	mutex_unlock(&dev->bus_mutex);
 
 	mutex_lock(&dev->list_mutex);
 	list_del(&async_cmd->async_entry);
 	mutex_unlock(&dev->list_mutex);
 
-	if (atomic_sub_return(1, &node->block->refcnt) == 0)
-		kfree(node->block);
+	w1_unref_block(node->block);
+}
+
+static void w1_list_count_cmds(struct w1_netlink_msg *msg, int *cmd_count,
+	u16 *slave_len)
+{
+	struct w1_netlink_cmd *cmd = (struct w1_netlink_cmd *)msg->data;
+	u16 mlen = msg->len;
+	u16 len;
+	int slave_list = 0;
+	while (mlen) {
+		if (cmd->len + sizeof(struct w1_netlink_cmd) > mlen)
+			break;
+
+		switch (cmd->cmd) {
+		case W1_CMD_SEARCH:
+		case W1_CMD_ALARM_SEARCH:
+		case W1_CMD_LIST_SLAVES:
+			++slave_list;
+		}
+		++*cmd_count;
+		len = sizeof(*cmd) + cmd->len;
+		cmd = (struct w1_netlink_cmd *)((u8 *)cmd + len);
+		mlen -= len;
+	}
+
+	if (slave_list) {
+		struct w1_master *dev = w1_search_master_id(msg->id.mst.id);
+		if (dev) {
+			/* Bytes, and likely an overstimate, and if it isn't
+			 * the results can still be split between packets.
+			 */
+			*slave_len += sizeof(struct w1_reg_num) * slave_list *
+				(dev->slave_count + dev->max_slave_count);
+			/* search incremented it */
+			atomic_dec(&dev->refcnt);
+		}
+	}
 }
 
-static void w1_cn_callback(struct cn_msg *msg, struct netlink_skb_parms *nsp)
+static void w1_cn_callback(struct cn_msg *cn, struct netlink_skb_parms *nsp)
 {
-	struct w1_netlink_msg *m = (struct w1_netlink_msg *)(msg + 1);
+	struct w1_netlink_msg *msg = (struct w1_netlink_msg *)(cn + 1);
 	struct w1_slave *sl;
 	struct w1_master *dev;
 	u16 msg_len;
+	u16 slave_len = 0;
 	int err = 0;
 	struct w1_cb_block *block = NULL;
 	struct w1_cb_node *node = NULL;
 	int node_count = 0;
+	int cmd_count = 0;
+
+	/* If any unknown flag is set let the application know, that way
+	 * applications can detect the absence of features in kernels that
+	 * don't know about them.  http://lwn.net/Articles/587527/
+	 */
+	if (cn->flags & ~(W1_CN_BUNDLE)) {
+		w1_netlink_send_error(cn, msg, nsp->portid, -EINVAL);
+		return;
+	}
 
 	/* Count the number of master or slave commands there are to allocate
 	 * space for one cb_node each.
 	 */
-	msg_len = msg->len;
+	msg_len = cn->len;
 	while (msg_len && !err) {
-		if (m->len + sizeof(struct w1_netlink_msg) > msg_len) {
+		if (msg->len + sizeof(struct w1_netlink_msg) > msg_len) {
 			err = -E2BIG;
 			break;
 		}
 
-		if (m->type == W1_MASTER_CMD || m->type == W1_SLAVE_CMD)
+		/* count messages for nodes and allocate any additional space
+		 * required for slave lists
+		 */
+		if (msg->type == W1_MASTER_CMD || msg->type == W1_SLAVE_CMD) {
 			++node_count;
+			w1_list_count_cmds(msg, &cmd_count, &slave_len);
+		}
 
-		msg_len -= sizeof(struct w1_netlink_msg) + m->len;
-		m = (struct w1_netlink_msg *)(((u8 *)m) +
-			sizeof(struct w1_netlink_msg) + m->len);
+		msg_len -= sizeof(struct w1_netlink_msg) + msg->len;
+		msg = (struct w1_netlink_msg *)(((u8 *)msg) +
+			sizeof(struct w1_netlink_msg) + msg->len);
 	}
-	m = (struct w1_netlink_msg *)(msg + 1);
+	msg = (struct w1_netlink_msg *)(cn + 1);
 	if (node_count) {
-		/* msg->len doesn't include itself */
-		long size = sizeof(struct w1_cb_block) + msg->len +
-			node_count*sizeof(struct w1_cb_node);
-		block = kmalloc(size, GFP_KERNEL);
+		int size;
+		u16 reply_size = sizeof(*cn) + cn->len + slave_len;
+		if (cn->flags & W1_CN_BUNDLE) {
+			/* bundling duplicats some of the messages */
+			reply_size += 2 * cmd_count * (sizeof(struct cn_msg) +
+				sizeof(struct w1_netlink_msg) +
+				sizeof(struct w1_netlink_cmd));
+		}
+		reply_size = MIN(CONNECTOR_MAX_MSG_SIZE, reply_size);
+
+		/* allocate space for the block, a copy of the original message,
+		 * one node per cmd to point into the original message,
+		 * space for replies which is the original message size plus
+		 * space for any list slave data and status messages
+		 * cn->len doesn't include itself which is part of the block
+		 * */
+		size =  /* block + original message */
+			sizeof(struct w1_cb_block) + sizeof(*cn) + cn->len +
+			/* space for nodes */
+			node_count * sizeof(struct w1_cb_node) +
+			/* replies */
+			sizeof(struct cn_msg) + reply_size;
+		block = kzalloc(size, GFP_KERNEL);
 		if (!block) {
-			w1_netlink_send_error(msg, m, NULL, nsp->portid,
-				-ENOMEM);
+			/* if the system is already out of memory,
+			 * (A) will this work, and (B) would it be better
+			 * to not try?
+			 */
+			w1_netlink_send_error(cn, msg, nsp->portid, -ENOMEM);
 			return;
 		}
 		atomic_set(&block->refcnt, 1);
 		block->portid = nsp->portid;
-		memcpy(&block->msg, msg, sizeof(*msg) + msg->len);
-		node = (struct w1_cb_node *)((u8 *)block->msg.data + msg->len);
+		memcpy(&block->request_cn, cn, sizeof(*cn) + cn->len);
+		node = (struct w1_cb_node *)(block->request_cn.data + cn->len);
+
+		/* Sneeky, when not bundling, reply_size is the allocated space
+		 * required for the reply, cn_msg isn't part of maxlen so
+		 * it should be reply_size - sizeof(struct cn_msg), however
+		 * when checking if there is enough space, w1_reply_make_space
+		 * is called with the full message size including cn_msg,
+		 * because it isn't known at that time if an additional cn_msg
+		 * will need to be allocated.  So an extra cn_msg is added
+		 * above in "size".
+		 */
+		block->maxlen = reply_size;
+		block->first_cn = (struct cn_msg *)(node + node_count);
+		memset(block->first_cn, 0, sizeof(*block->first_cn));
 	}
 
-	msg_len = msg->len;
+	msg_len = cn->len;
 	while (msg_len && !err) {
-		struct w1_reg_num id;
-		u16 mlen = m->len;
 
 		dev = NULL;
 		sl = NULL;
 
-		memcpy(&id, m->id.id, sizeof(id));
-#if 0
-		printk("%s: %02x.%012llx.%02x: type=%02x, len=%u.\n",
-				__func__, id.family, (unsigned long long)id.id, id.crc, m->type, m->len);
-#endif
-		if (m->len + sizeof(struct w1_netlink_msg) > msg_len) {
+		if (msg->len + sizeof(struct w1_netlink_msg) > msg_len) {
 			err = -E2BIG;
 			break;
 		}
 
-		if (m->type == W1_MASTER_CMD) {
-			dev = w1_search_master_id(m->id.mst.id);
-		} else if (m->type == W1_SLAVE_CMD) {
-			sl = w1_search_slave(&id);
+		/* execute on this thread, no need to process later */
+		if (msg->type == W1_LIST_MASTERS) {
+			err = w1_process_command_root(cn, nsp->portid);
+			goto out_cont;
+		}
+
+		/* All following message types require additional data,
+		 * check here before references are taken.
+		 */
+		if (!msg->len) {
+			err = -EPROTO;
+			goto out_cont;
+		}
+
+		/* both search calls take references */
+		if (msg->type == W1_MASTER_CMD) {
+			dev = w1_search_master_id(msg->id.mst.id);
+		} else if (msg->type == W1_SLAVE_CMD) {
+			sl = w1_search_slave((struct w1_reg_num *)msg->id.id);
 			if (sl)
 				dev = sl->master;
 		} else {
-			err = w1_process_command_root(msg, m, nsp->portid);
+			printk(KERN_NOTICE
+				"%s: cn: %x.%x, wrong type: %u, len: %u.\n",
+				__func__, cn->id.idx, cn->id.val,
+				msg->type, msg->len);
+			err = -EPROTO;
 			goto out_cont;
 		}
 
@@ -536,14 +694,12 @@ static void w1_cn_callback(struct cn_msg *msg, struct netlink_skb_parms *nsp)
 		}
 
 		err = 0;
-		if (!mlen)
-			goto out_cont;
 
 		atomic_inc(&block->refcnt);
 		node->async.cb = w1_process_cb;
 		node->block = block;
-		node->m = (struct w1_netlink_msg *)((u8 *)&block->msg +
-			(size_t)((u8 *)m - (u8 *)msg));
+		node->msg = (struct w1_netlink_msg *)((u8 *)&block->request_cn +
+			(size_t)((u8 *)msg - (u8 *)cn));
 		node->sl = sl;
 		node->dev = dev;
 
@@ -554,10 +710,15 @@ static void w1_cn_callback(struct cn_msg *msg, struct netlink_skb_parms *nsp)
 		++node;
 
 out_cont:
+		/* Can't queue because that modifies block and another
+		 * thread could be processing the messages by now and
+		 * there isn't a lock, send directly.
+		 */
 		if (err)
-			w1_netlink_send_error(msg, m, NULL, nsp->portid, err);
-		msg_len -= sizeof(struct w1_netlink_msg) + m->len;
-		m = (struct w1_netlink_msg *)(((u8 *)m) + sizeof(struct w1_netlink_msg) + m->len);
+			w1_netlink_send_error(cn, msg, nsp->portid, err);
+		msg_len -= sizeof(struct w1_netlink_msg) + msg->len;
+		msg = (struct w1_netlink_msg *)(((u8 *)msg) +
+			sizeof(struct w1_netlink_msg) + msg->len);
 
 		/*
 		 * Let's allow requests for nonexisting devices.
@@ -565,8 +726,8 @@ out_cont:
 		if (err == -ENODEV)
 			err = 0;
 	}
-	if (block && atomic_sub_return(1, &block->refcnt) == 0)
-		kfree(block);
+	if (block)
+		w1_unref_block(block);
 }
 
 int w1_init_netlink(void)
@@ -583,7 +744,7 @@ void w1_fini_netlink(void)
 	cn_del_callback(&w1_id);
 }
 #else
-void w1_netlink_send(struct w1_master *dev, struct w1_netlink_msg *msg)
+void w1_netlink_send(struct w1_master *dev, struct w1_netlink_msg *cn)
 {
 }
 
diff --git a/drivers/w1/w1_netlink.h b/drivers/w1/w1_netlink.h
index 1e9504e6765..c99a9ce05e6 100644
--- a/drivers/w1/w1_netlink.h
+++ b/drivers/w1/w1_netlink.h
@@ -28,6 +28,17 @@
 #include "w1.h"
 
 /**
+ * enum w1_cn_msg_flags - bitfield flags for struct cn_msg.flags
+ *
+ * @W1_CN_BUNDLE: Request bundling replies into fewer messagse.  Be prepared
+ * to handle multiple struct cn_msg, struct w1_netlink_msg, and
+ * struct w1_netlink_cmd in one packet.
+ */
+enum w1_cn_msg_flags {
+	W1_CN_BUNDLE = 1,
+};
+
+/**
  * enum w1_netlink_message_types - message type
  *
  * @W1_SLAVE_ADD: notification that a slave device was added
@@ -49,6 +60,19 @@ enum w1_netlink_message_types {
 	W1_LIST_MASTERS,
 };
 
+/**
+ * struct w1_netlink_msg - holds w1 message type, id, and result
+ *
+ * @type: one of enum w1_netlink_message_types
+ * @status: kernel feedback for success 0 or errno failure value
+ * @len: length of data following w1_netlink_msg
+ * @id: union holding master bus id (msg.id) and slave device id (id[8]).
+ * @data: start address of any following data
+ *
+ * The base message structure for w1 messages over netlink.
+ * The netlink connector data sequence is, struct nlmsghdr, struct cn_msg,
+ * then one or more struct w1_netlink_msg (each with optional data).
+ */
 struct w1_netlink_msg
 {
 	__u8				type;
@@ -66,6 +90,7 @@ struct w1_netlink_msg
 
 /**
  * enum w1_commands - commands available for master or slave operations
+ *
  * @W1_CMD_READ: read len bytes
  * @W1_CMD_WRITE: write len bytes
  * @W1_CMD_SEARCH: initiate a standard search, returns only the slave
@@ -93,6 +118,17 @@ enum w1_commands {
 	W1_CMD_MAX
 };
 
+/**
+ * struct w1_netlink_cmd - holds the command and data
+ *
+ * @cmd: one of enum w1_commands
+ * @res: reserved
+ * @len: length of data following w1_netlink_cmd
+ * @data: start address of any following data
+ *
+ * One or more struct w1_netlink_cmd is placed starting at w1_netlink_msg.data
+ * each with optional data.
+ */
 struct w1_netlink_cmd
 {
 	__u8				cmd;
diff --git a/drivers/xen/events/events_fifo.c b/drivers/xen/events/events_fifo.c
index 96109a9972b..84b4bfb8434 100644
--- a/drivers/xen/events/events_fifo.c
+++ b/drivers/xen/events/events_fifo.c
@@ -66,7 +66,22 @@ static DEFINE_PER_CPU(struct evtchn_fifo_queue, cpu_queue);
 static event_word_t *event_array[MAX_EVENT_ARRAY_PAGES] __read_mostly;
 static unsigned event_array_pages __read_mostly;
 
+/*
+ * sync_set_bit() and friends must be unsigned long aligned on non-x86
+ * platforms.
+ */
+#if !defined(CONFIG_X86) && BITS_PER_LONG > 32
+
+#define BM(w) (unsigned long *)((unsigned long)w & ~0x7UL)
+#define EVTCHN_FIFO_BIT(b, w) \
+    (((unsigned long)w & 0x4UL) ? (EVTCHN_FIFO_ ##b + 32) : EVTCHN_FIFO_ ##b)
+
+#else
+
 #define BM(w) ((unsigned long *)(w))
+#define EVTCHN_FIFO_BIT(b, w) EVTCHN_FIFO_ ##b
+
+#endif
 
 static inline event_word_t *event_word_from_port(unsigned port)
 {
@@ -161,33 +176,38 @@ static void evtchn_fifo_bind_to_cpu(struct irq_info *info, unsigned cpu)
 static void evtchn_fifo_clear_pending(unsigned port)
 {
 	event_word_t *word = event_word_from_port(port);
-	sync_clear_bit(EVTCHN_FIFO_PENDING, BM(word));
+	sync_clear_bit(EVTCHN_FIFO_BIT(PENDING, word), BM(word));
 }
 
 static void evtchn_fifo_set_pending(unsigned port)
 {
 	event_word_t *word = event_word_from_port(port);
-	sync_set_bit(EVTCHN_FIFO_PENDING, BM(word));
+	sync_set_bit(EVTCHN_FIFO_BIT(PENDING, word), BM(word));
 }
 
 static bool evtchn_fifo_is_pending(unsigned port)
 {
 	event_word_t *word = event_word_from_port(port);
-	return sync_test_bit(EVTCHN_FIFO_PENDING, BM(word));
+	return sync_test_bit(EVTCHN_FIFO_BIT(PENDING, word), BM(word));
 }
 
 static bool evtchn_fifo_test_and_set_mask(unsigned port)
 {
 	event_word_t *word = event_word_from_port(port);
-	return sync_test_and_set_bit(EVTCHN_FIFO_MASKED, BM(word));
+	return sync_test_and_set_bit(EVTCHN_FIFO_BIT(MASKED, word), BM(word));
 }
 
 static void evtchn_fifo_mask(unsigned port)
 {
 	event_word_t *word = event_word_from_port(port);
-	sync_set_bit(EVTCHN_FIFO_MASKED, BM(word));
+	sync_set_bit(EVTCHN_FIFO_BIT(MASKED, word), BM(word));
 }
 
+static bool evtchn_fifo_is_masked(unsigned port)
+{
+	event_word_t *word = event_word_from_port(port);
+	return sync_test_bit(EVTCHN_FIFO_BIT(MASKED, word), BM(word));
+}
 /*
  * Clear MASKED, spinning if BUSY is set.
  */
@@ -211,7 +231,7 @@ static void evtchn_fifo_unmask(unsigned port)
 	BUG_ON(!irqs_disabled());
 
 	clear_masked(word);
-	if (sync_test_bit(EVTCHN_FIFO_PENDING, BM(word))) {
+	if (evtchn_fifo_is_pending(port)) {
 		struct evtchn_unmask unmask = { .port = port };
 		(void)HYPERVISOR_event_channel_op(EVTCHNOP_unmask, &unmask);
 	}
@@ -243,7 +263,7 @@ static void handle_irq_for_port(unsigned port)
 
 static void consume_one_event(unsigned cpu,
 			      struct evtchn_fifo_control_block *control_block,
-			      unsigned priority, uint32_t *ready)
+			      unsigned priority, unsigned long *ready)
 {
 	struct evtchn_fifo_queue *q = &per_cpu(cpu_queue, cpu);
 	uint32_t head;
@@ -273,10 +293,9 @@ static void consume_one_event(unsigned cpu,
 	 * copy of the ready word.
 	 */
 	if (head == 0)
-		clear_bit(priority, BM(ready));
+		clear_bit(priority, ready);
 
-	if (sync_test_bit(EVTCHN_FIFO_PENDING, BM(word))
-	    && !sync_test_bit(EVTCHN_FIFO_MASKED, BM(word)))
+	if (evtchn_fifo_is_pending(port) && !evtchn_fifo_is_masked(port))
 		handle_irq_for_port(port);
 
 	q->head[priority] = head;
@@ -285,7 +304,7 @@ static void consume_one_event(unsigned cpu,
 static void evtchn_fifo_handle_events(unsigned cpu)
 {
 	struct evtchn_fifo_control_block *control_block;
-	uint32_t ready;
+	unsigned long ready;
 	unsigned q;
 
 	control_block = per_cpu(cpu_control_block, cpu);
diff --git a/drivers/xen/manage.c b/drivers/xen/manage.c
index 32f9236c959..c3667b202f2 100644
--- a/drivers/xen/manage.c
+++ b/drivers/xen/manage.c
@@ -41,9 +41,6 @@ static enum shutdown_state shutting_down = SHUTDOWN_INVALID;
 
 struct suspend_info {
 	int cancelled;
-	unsigned long arg; /* extra hypercall argument */
-	void (*pre)(void);
-	void (*post)(int cancelled);
 };
 
 static RAW_NOTIFIER_HEAD(xen_resume_notifier);
@@ -61,26 +58,6 @@ void xen_resume_notifier_unregister(struct notifier_block *nb)
 EXPORT_SYMBOL_GPL(xen_resume_notifier_unregister);
 
 #ifdef CONFIG_HIBERNATE_CALLBACKS
-static void xen_hvm_post_suspend(int cancelled)
-{
-	xen_arch_hvm_post_suspend(cancelled);
-	gnttab_resume();
-}
-
-static void xen_pre_suspend(void)
-{
-	xen_mm_pin_all();
-	gnttab_suspend();
-	xen_arch_pre_suspend();
-}
-
-static void xen_post_suspend(int cancelled)
-{
-	xen_arch_post_suspend(cancelled);
-	gnttab_resume();
-	xen_mm_unpin_all();
-}
-
 static int xen_suspend(void *data)
 {
 	struct suspend_info *si = data;
@@ -94,18 +71,20 @@ static int xen_suspend(void *data)
 		return err;
 	}
 
-	if (si->pre)
-		si->pre();
+	gnttab_suspend();
+	xen_arch_pre_suspend();
 
 	/*
 	 * This hypercall returns 1 if suspend was cancelled
 	 * or the domain was merely checkpointed, and 0 if it
 	 * is resuming in a new domain.
 	 */
-	si->cancelled = HYPERVISOR_suspend(si->arg);
+	si->cancelled = HYPERVISOR_suspend(xen_pv_domain()
+                                           ? virt_to_mfn(xen_start_info)
+                                           : 0);
 
-	if (si->post)
-		si->post(si->cancelled);
+	xen_arch_post_suspend(si->cancelled);
+	gnttab_resume();
 
 	if (!si->cancelled) {
 		xen_irq_resume();
@@ -154,16 +133,6 @@ static void do_suspend(void)
 
 	si.cancelled = 1;
 
-	if (xen_hvm_domain()) {
-		si.arg = 0UL;
-		si.pre = NULL;
-		si.post = &xen_hvm_post_suspend;
-	} else {
-		si.arg = virt_to_mfn(xen_start_info);
-		si.pre = &xen_pre_suspend;
-		si.post = &xen_post_suspend;
-	}
-
 	err = stop_machine(xen_suspend, &si, cpumask_of(0));
 
 	raw_notifier_call_chain(&xen_resume_notifier, 0, NULL);
diff --git a/drivers/xen/xen-acpi-processor.c b/drivers/xen/xen-acpi-processor.c
index 82358d14ecf..59fc190f1e9 100644
--- a/drivers/xen/xen-acpi-processor.c
+++ b/drivers/xen/xen-acpi-processor.c
@@ -127,7 +127,7 @@ static int push_cxx_to_hypervisor(struct acpi_processor *_pr)
 			pr_debug("     C%d: %s %d uS\n",
 				 cx->type, cx->desc, (u32)cx->latency);
 		}
-	} else if (ret != -EINVAL)
+	} else if ((ret != -EINVAL) && (ret != -ENOSYS))
 		/* EINVAL means the ACPI ID is incorrect - meaning the ACPI
 		 * table is referencing a non-existing CPU - which can happen
 		 * with broken ACPI tables. */
@@ -259,7 +259,7 @@ static int push_pxx_to_hypervisor(struct acpi_processor *_pr)
 			(u32) perf->states[i].power,
 			(u32) perf->states[i].transition_latency);
 		}
-	} else if (ret != -EINVAL)
+	} else if ((ret != -EINVAL) && (ret != -ENOSYS))
 		/* EINVAL means the ACPI ID is incorrect - meaning the ACPI
 		 * table is referencing a non-existing CPU - which can happen
 		 * with broken ACPI tables. */
diff --git a/drivers/xen/xen-pciback/pci_stub.c b/drivers/xen/xen-pciback/pci_stub.c
index 62fcd485f0a..d57a173685f 100644
--- a/drivers/xen/xen-pciback/pci_stub.c
+++ b/drivers/xen/xen-pciback/pci_stub.c
@@ -242,6 +242,15 @@ struct pci_dev *pcistub_get_pci_dev(struct xen_pcibk_device *pdev,
 	return found_dev;
 }
 
+/*
+ * Called when:
+ *  - XenBus state has been reconfigure (pci unplug). See xen_pcibk_remove_device
+ *  - XenBus state has been disconnected (guest shutdown). See xen_pcibk_xenbus_remove
+ *  - 'echo BDF > unbind' on pciback module with no guest attached. See pcistub_remove
+ *  - 'echo BDF > unbind' with a guest still using it. See pcistub_remove
+ *
+ *  As such we have to be careful.
+ */
 void pcistub_put_pci_dev(struct pci_dev *dev)
 {
 	struct pcistub_device *psdev, *found_psdev = NULL;
@@ -272,16 +281,16 @@ void pcistub_put_pci_dev(struct pci_dev *dev)
 	 * and want to inhibit the user from fiddling with 'reset'
 	 */
 	pci_reset_function(dev);
-	pci_restore_state(psdev->dev);
+	pci_restore_state(dev);
 
 	/* This disables the device. */
-	xen_pcibk_reset_device(found_psdev->dev);
+	xen_pcibk_reset_device(dev);
 
 	/* And cleanup up our emulated fields. */
-	xen_pcibk_config_free_dyn_fields(found_psdev->dev);
-	xen_pcibk_config_reset_dev(found_psdev->dev);
+	xen_pcibk_config_reset_dev(dev);
+	xen_pcibk_config_free_dyn_fields(dev);
 
-	xen_unregister_device_domain_owner(found_psdev->dev);
+	xen_unregister_device_domain_owner(dev);
 
 	spin_lock_irqsave(&found_psdev->lock, flags);
 	found_psdev->pdev = NULL;
@@ -493,6 +502,8 @@ static int pcistub_seize(struct pci_dev *dev)
 	return err;
 }
 
+/* Called when 'bind'. This means we must _NOT_ call pci_reset_function or
+ * other functions that take the sysfs lock. */
 static int pcistub_probe(struct pci_dev *dev, const struct pci_device_id *id)
 {
 	int err = 0;
@@ -520,6 +531,8 @@ out:
 	return err;
 }
 
+/* Called when 'unbind'. This means we must _NOT_ call pci_reset_function or
+ * other functions that take the sysfs lock. */
 static void pcistub_remove(struct pci_dev *dev)
 {
 	struct pcistub_device *psdev, *found_psdev = NULL;
@@ -551,6 +564,8 @@ static void pcistub_remove(struct pci_dev *dev)
 			pr_warn("****** shutdown driver domain before binding device\n");
 			pr_warn("****** to other drivers or domains\n");
 
+			/* N.B. This ends up calling pcistub_put_pci_dev which ends up
+			 * doing the FLR. */
 			xen_pcibk_release_pci_dev(found_psdev->pdev,
 						found_psdev->dev);
 		}
diff --git a/drivers/xen/xen-pciback/xenbus.c b/drivers/xen/xen-pciback/xenbus.c
index a9ed867afab..4a7e6e0a5f4 100644
--- a/drivers/xen/xen-pciback/xenbus.c
+++ b/drivers/xen/xen-pciback/xenbus.c
@@ -93,6 +93,8 @@ static void free_pdev(struct xen_pcibk_device *pdev)
 
 	xen_pcibk_disconnect(pdev);
 
+	/* N.B. This calls pcistub_put_pci_dev which does the FLR on all
+	 * of the PCIe devices. */
 	xen_pcibk_release_devices(pdev);
 
 	dev_set_drvdata(&pdev->xdev->dev, NULL);
@@ -286,6 +288,8 @@ static int xen_pcibk_remove_device(struct xen_pcibk_device *pdev,
 	dev_dbg(&dev->dev, "unregistering for %d\n", pdev->xdev->otherend_id);
 	xen_unregister_device_domain_owner(dev);
 
+	/* N.B. This ends up calling pcistub_put_pci_dev which ends up
+	 * doing the FLR. */
 	xen_pcibk_release_pci_dev(pdev, dev);
 
 out:
diff --git a/firmware/WHENCE b/firmware/WHENCE
index 8388f02de2b..0c4d96dee9b 100644
--- a/firmware/WHENCE
+++ b/firmware/WHENCE
@@ -273,16 +273,6 @@ Converted from Intel HEX files, used in our binary representation of ihex.
 
 --------------------------------------------------------------------------
 
-Driver: ip2 -- Computone IntelliPort Plus serial device
-
-File: intelliport2.bin
-
-Licence: Unknown
-
-Found in hex form in kernel source.
-
---------------------------------------------------------------------------
-
 Driver: CPiA2 -- cameras based on Vision's CPiA2
 
 File: cpia2/stv0672_vp4.bin
diff --git a/fs/Makefile b/fs/Makefile
index f9cb9876e46..4030cbfbc9a 100644
--- a/fs/Makefile
+++ b/fs/Makefile
@@ -14,14 +14,13 @@ obj-y :=	open.o read_write.o file_table.o super.o \
 		stack.o fs_struct.o statfs.o
 
 ifeq ($(CONFIG_BLOCK),y)
-obj-y +=	buffer.o bio.o block_dev.o direct-io.o mpage.o ioprio.o
+obj-y +=	buffer.o block_dev.o direct-io.o mpage.o
 else
 obj-y +=	no-block.o
 endif
 
 obj-$(CONFIG_PROC_FS) += proc_namespace.o
 
-obj-$(CONFIG_BLK_DEV_INTEGRITY) += bio-integrity.o
 obj-y				+= notify/
 obj-$(CONFIG_EPOLL)		+= eventpoll.o
 obj-$(CONFIG_ANON_INODES)	+= anon_inodes.o
diff --git a/fs/affs/super.c b/fs/affs/super.c
index 6d589f28bf9..895ac7dc9db 100644
--- a/fs/affs/super.c
+++ b/fs/affs/super.c
@@ -340,8 +340,6 @@ static int affs_fill_super(struct super_block *sb, void *data, int silent)
 				&blocksize,&sbi->s_prefix,
 				sbi->s_volume, &mount_flags)) {
 		printk(KERN_ERR "AFFS: Error parsing options\n");
-		kfree(sbi->s_prefix);
-		kfree(sbi);
 		return -EINVAL;
 	}
 	/* N.B. after this point s_prefix must be released */
diff --git a/fs/afs/cmservice.c b/fs/afs/cmservice.c
index 1c8c6cc6de3..4b0eff6da67 100644
--- a/fs/afs/cmservice.c
+++ b/fs/afs/cmservice.c
@@ -130,6 +130,15 @@ static void afs_cm_destructor(struct afs_call *call)
 {
 	_enter("");
 
+	/* Break the callbacks here so that we do it after the final ACK is
+	 * received.  The step number here must match the final number in
+	 * afs_deliver_cb_callback().
+	 */
+	if (call->unmarshall == 6) {
+		ASSERT(call->server && call->count && call->request);
+		afs_break_callbacks(call->server, call->count, call->request);
+	}
+
 	afs_put_server(call->server);
 	call->server = NULL;
 	kfree(call->buffer);
@@ -272,6 +281,16 @@ static int afs_deliver_cb_callback(struct afs_call *call, struct sk_buff *skb,
 		_debug("trailer");
 		if (skb->len != 0)
 			return -EBADMSG;
+
+		/* Record that the message was unmarshalled successfully so
+		 * that the call destructor can know do the callback breaking
+		 * work, even if the final ACK isn't received.
+		 *
+		 * If the step number changes, then afs_cm_destructor() must be
+		 * updated also.
+		 */
+		call->unmarshall++;
+	case 6:
 		break;
 	}
 
diff --git a/fs/afs/internal.h b/fs/afs/internal.h
index be75b500005..590b55f46d6 100644
--- a/fs/afs/internal.h
+++ b/fs/afs/internal.h
@@ -75,7 +75,7 @@ struct afs_call {
 	const struct afs_call_type *type;	/* type of call */
 	const struct afs_wait_mode *wait_mode;	/* completion wait mode */
 	wait_queue_head_t	waitq;		/* processes awaiting completion */
-	work_func_t		async_workfn;
+	void (*async_workfn)(struct afs_call *call); /* asynchronous work function */
 	struct work_struct	async_work;	/* asynchronous work processor */
 	struct work_struct	work;		/* actual work processor */
 	struct sk_buff_head	rx_queue;	/* received packets */
diff --git a/fs/afs/rxrpc.c b/fs/afs/rxrpc.c
index ef943df73b8..03a3beb1700 100644
--- a/fs/afs/rxrpc.c
+++ b/fs/afs/rxrpc.c
@@ -25,7 +25,7 @@ static void afs_wake_up_call_waiter(struct afs_call *);
 static int afs_wait_for_call_to_complete(struct afs_call *);
 static void afs_wake_up_async_call(struct afs_call *);
 static int afs_dont_wait_for_call_to_complete(struct afs_call *);
-static void afs_process_async_call(struct work_struct *);
+static void afs_process_async_call(struct afs_call *);
 static void afs_rx_interceptor(struct sock *, unsigned long, struct sk_buff *);
 static int afs_deliver_cm_op_id(struct afs_call *, struct sk_buff *, bool);
 
@@ -58,6 +58,13 @@ static void afs_collect_incoming_call(struct work_struct *);
 static struct sk_buff_head afs_incoming_calls;
 static DECLARE_WORK(afs_collect_incoming_call_work, afs_collect_incoming_call);
 
+static void afs_async_workfn(struct work_struct *work)
+{
+	struct afs_call *call = container_of(work, struct afs_call, async_work);
+
+	call->async_workfn(call);
+}
+
 /*
  * open an RxRPC socket and bind it to be a server for callback notifications
  * - the socket is left in blocking mode and non-blocking ops use MSG_DONTWAIT
@@ -184,6 +191,28 @@ static void afs_free_call(struct afs_call *call)
 }
 
 /*
+ * End a call but do not free it
+ */
+static void afs_end_call_nofree(struct afs_call *call)
+{
+	if (call->rxcall) {
+		rxrpc_kernel_end_call(call->rxcall);
+		call->rxcall = NULL;
+	}
+	if (call->type->destructor)
+		call->type->destructor(call);
+}
+
+/*
+ * End a call and free it
+ */
+static void afs_end_call(struct afs_call *call)
+{
+	afs_end_call_nofree(call);
+	afs_free_call(call);
+}
+
+/*
  * allocate a call with flat request and reply buffers
  */
 struct afs_call *afs_alloc_flat_call(const struct afs_call_type *type,
@@ -326,7 +355,8 @@ int afs_make_call(struct in_addr *addr, struct afs_call *call, gfp_t gfp,
 	       atomic_read(&afs_outstanding_calls));
 
 	call->wait_mode = wait_mode;
-	INIT_WORK(&call->async_work, afs_process_async_call);
+	call->async_workfn = afs_process_async_call;
+	INIT_WORK(&call->async_work, afs_async_workfn);
 
 	memset(&srx, 0, sizeof(srx));
 	srx.srx_family = AF_RXRPC;
@@ -383,11 +413,8 @@ error_do_abort:
 	rxrpc_kernel_abort_call(rxcall, RX_USER_ABORT);
 	while ((skb = skb_dequeue(&call->rx_queue)))
 		afs_free_skb(skb);
-	rxrpc_kernel_end_call(rxcall);
-	call->rxcall = NULL;
 error_kill_call:
-	call->type->destructor(call);
-	afs_free_call(call);
+	afs_end_call(call);
 	_leave(" = %d", ret);
 	return ret;
 }
@@ -509,12 +536,8 @@ static void afs_deliver_to_call(struct afs_call *call)
 	if (call->state >= AFS_CALL_COMPLETE) {
 		while ((skb = skb_dequeue(&call->rx_queue)))
 			afs_free_skb(skb);
-		if (call->incoming) {
-			rxrpc_kernel_end_call(call->rxcall);
-			call->rxcall = NULL;
-			call->type->destructor(call);
-			afs_free_call(call);
-		}
+		if (call->incoming)
+			afs_end_call(call);
 	}
 
 	_leave("");
@@ -564,10 +587,7 @@ static int afs_wait_for_call_to_complete(struct afs_call *call)
 	}
 
 	_debug("call complete");
-	rxrpc_kernel_end_call(call->rxcall);
-	call->rxcall = NULL;
-	call->type->destructor(call);
-	afs_free_call(call);
+	afs_end_call(call);
 	_leave(" = %d", ret);
 	return ret;
 }
@@ -603,11 +623,8 @@ static int afs_dont_wait_for_call_to_complete(struct afs_call *call)
 /*
  * delete an asynchronous call
  */
-static void afs_delete_async_call(struct work_struct *work)
+static void afs_delete_async_call(struct afs_call *call)
 {
-	struct afs_call *call =
-		container_of(work, struct afs_call, async_work);
-
 	_enter("");
 
 	afs_free_call(call);
@@ -620,11 +637,8 @@ static void afs_delete_async_call(struct work_struct *work)
  * - on a multiple-thread workqueue this work item may try to run on several
  *   CPUs at the same time
  */
-static void afs_process_async_call(struct work_struct *work)
+static void afs_process_async_call(struct afs_call *call)
 {
-	struct afs_call *call =
-		container_of(work, struct afs_call, async_work);
-
 	_enter("");
 
 	if (!skb_queue_empty(&call->rx_queue))
@@ -637,10 +651,7 @@ static void afs_process_async_call(struct work_struct *work)
 		call->reply = NULL;
 
 		/* kill the call */
-		rxrpc_kernel_end_call(call->rxcall);
-		call->rxcall = NULL;
-		if (call->type->destructor)
-			call->type->destructor(call);
+		afs_end_call_nofree(call);
 
 		/* we can't just delete the call because the work item may be
 		 * queued */
@@ -663,13 +674,6 @@ void afs_transfer_reply(struct afs_call *call, struct sk_buff *skb)
 	call->reply_size += len;
 }
 
-static void afs_async_workfn(struct work_struct *work)
-{
-	struct afs_call *call = container_of(work, struct afs_call, async_work);
-
-	call->async_workfn(work);
-}
-
 /*
  * accept the backlog of incoming calls
  */
@@ -790,10 +794,7 @@ void afs_send_empty_reply(struct afs_call *call)
 		_debug("oom");
 		rxrpc_kernel_abort_call(call->rxcall, RX_USER_ABORT);
 	default:
-		rxrpc_kernel_end_call(call->rxcall);
-		call->rxcall = NULL;
-		call->type->destructor(call);
-		afs_free_call(call);
+		afs_end_call(call);
 		_leave(" [error]");
 		return;
 	}
@@ -823,17 +824,16 @@ void afs_send_simple_reply(struct afs_call *call, const void *buf, size_t len)
 	call->state = AFS_CALL_AWAIT_ACK;
 	n = rxrpc_kernel_send_data(call->rxcall, &msg, len);
 	if (n >= 0) {
+		/* Success */
 		_leave(" [replied]");
 		return;
 	}
+
 	if (n == -ENOMEM) {
 		_debug("oom");
 		rxrpc_kernel_abort_call(call->rxcall, RX_USER_ABORT);
 	}
-	rxrpc_kernel_end_call(call->rxcall);
-	call->rxcall = NULL;
-	call->type->destructor(call);
-	afs_free_call(call);
+	afs_end_call(call);
 	_leave(" [error]");
 }
 
diff --git a/fs/aio.c b/fs/aio.c
index 12a3de0ee6d..a0ed6c7d2cd 100644
--- a/fs/aio.c
+++ b/fs/aio.c
@@ -112,6 +112,11 @@ struct kioctx {
 
 	struct work_struct	free_work;
 
+	/*
+	 * signals when all in-flight requests are done
+	 */
+	struct completion *requests_done;
+
 	struct {
 		/*
 		 * This counts the number of available slots in the ringbuffer,
@@ -508,6 +513,10 @@ static void free_ioctx_reqs(struct percpu_ref *ref)
 {
 	struct kioctx *ctx = container_of(ref, struct kioctx, reqs);
 
+	/* At this point we know that there are no any in-flight requests */
+	if (ctx->requests_done)
+		complete(ctx->requests_done);
+
 	INIT_WORK(&ctx->free_work, free_ioctx);
 	schedule_work(&ctx->free_work);
 }
@@ -718,7 +727,8 @@ err:
  *	when the processes owning a context have all exited to encourage
  *	the rapid destruction of the kioctx.
  */
-static void kill_ioctx(struct mm_struct *mm, struct kioctx *ctx)
+static void kill_ioctx(struct mm_struct *mm, struct kioctx *ctx,
+		struct completion *requests_done)
 {
 	if (!atomic_xchg(&ctx->dead, 1)) {
 		struct kioctx_table *table;
@@ -747,7 +757,11 @@ static void kill_ioctx(struct mm_struct *mm, struct kioctx *ctx)
 		if (ctx->mmap_size)
 			vm_munmap(ctx->mmap_base, ctx->mmap_size);
 
+		ctx->requests_done = requests_done;
 		percpu_ref_kill(&ctx->users);
+	} else {
+		if (requests_done)
+			complete(requests_done);
 	}
 }
 
@@ -809,7 +823,7 @@ void exit_aio(struct mm_struct *mm)
 		 */
 		ctx->mmap_size = 0;
 
-		kill_ioctx(mm, ctx);
+		kill_ioctx(mm, ctx, NULL);
 	}
 }
 
@@ -1185,7 +1199,7 @@ SYSCALL_DEFINE2(io_setup, unsigned, nr_events, aio_context_t __user *, ctxp)
 	if (!IS_ERR(ioctx)) {
 		ret = put_user(ioctx->user_id, ctxp);
 		if (ret)
-			kill_ioctx(current->mm, ioctx);
+			kill_ioctx(current->mm, ioctx, NULL);
 		percpu_ref_put(&ioctx->users);
 	}
 
@@ -1203,8 +1217,22 @@ SYSCALL_DEFINE1(io_destroy, aio_context_t, ctx)
 {
 	struct kioctx *ioctx = lookup_ioctx(ctx);
 	if (likely(NULL != ioctx)) {
-		kill_ioctx(current->mm, ioctx);
+		struct completion requests_done =
+			COMPLETION_INITIALIZER_ONSTACK(requests_done);
+
+		/* Pass requests_done to kill_ioctx() where it can be set
+		 * in a thread-safe way. If we try to set it here then we have
+		 * a race condition if two io_destroy() called simultaneously.
+		 */
+		kill_ioctx(current->mm, ioctx, &requests_done);
 		percpu_ref_put(&ioctx->users);
+
+		/* Wait until all IO for the context are done. Otherwise kernel
+		 * keep using user-space buffers even if user thinks the context
+		 * is destroyed.
+		 */
+		wait_for_completion(&requests_done);
+
 		return 0;
 	}
 	pr_debug("EINVAL: io_destroy: invalid context id\n");
@@ -1299,10 +1327,8 @@ rw_common:
 						&iovec, compat)
 			: aio_setup_single_vector(req, rw, buf, &nr_segs,
 						  iovec);
-		if (ret)
-			return ret;
-
-		ret = rw_verify_area(rw, file, &req->ki_pos, req->ki_nbytes);
+		if (!ret)
+			ret = rw_verify_area(rw, file, &req->ki_pos, req->ki_nbytes);
 		if (ret < 0) {
 			if (iovec != &inline_vec)
 				kfree(iovec);
diff --git a/fs/autofs4/root.c b/fs/autofs4/root.c
index 2caf36ac3e9..cc87c1abac9 100644
--- a/fs/autofs4/root.c
+++ b/fs/autofs4/root.c
@@ -179,7 +179,7 @@ static struct dentry *autofs4_lookup_active(struct dentry *dentry)
 		spin_lock(&active->d_lock);
 
 		/* Already gone? */
-		if (!d_count(active))
+		if ((int) d_count(active) <= 0)
 			goto next;
 
 		qstr = &active->d_name;
@@ -230,7 +230,7 @@ static struct dentry *autofs4_lookup_expiring(struct dentry *dentry)
 
 		spin_lock(&expiring->d_lock);
 
-		/* Bad luck, we've already been dentry_iput */
+		/* We've already been dentry_iput or unlinked */
 		if (!expiring->d_inode)
 			goto next;
 
diff --git a/fs/btrfs/ctree.h b/fs/btrfs/ctree.h
index 4c48df572bd..ba6b88528dc 100644
--- a/fs/btrfs/ctree.h
+++ b/fs/btrfs/ctree.h
@@ -2058,6 +2058,20 @@ struct btrfs_ioctl_defrag_range_args {
 #define btrfs_raw_test_opt(o, opt)	((o) & BTRFS_MOUNT_##opt)
 #define btrfs_test_opt(root, opt)	((root)->fs_info->mount_opt & \
 					 BTRFS_MOUNT_##opt)
+#define btrfs_set_and_info(root, opt, fmt, args...)			\
+{									\
+	if (!btrfs_test_opt(root, opt))					\
+		btrfs_info(root->fs_info, fmt, ##args);			\
+	btrfs_set_opt(root->fs_info->mount_opt, opt);			\
+}
+
+#define btrfs_clear_and_info(root, opt, fmt, args...)			\
+{									\
+	if (btrfs_test_opt(root, opt))					\
+		btrfs_info(root->fs_info, fmt, ##args);			\
+	btrfs_clear_opt(root->fs_info->mount_opt, opt);			\
+}
+
 /*
  * Inode flags
  */
diff --git a/fs/btrfs/disk-io.c b/fs/btrfs/disk-io.c
index 029d46c2e17..983314932af 100644
--- a/fs/btrfs/disk-io.c
+++ b/fs/btrfs/disk-io.c
@@ -2861,7 +2861,7 @@ retry_root_backup:
 			printk(KERN_ERR "BTRFS: failed to read log tree\n");
 			free_extent_buffer(log_tree_root->node);
 			kfree(log_tree_root);
-			goto fail_trans_kthread;
+			goto fail_qgroup;
 		}
 		/* returns with log_tree_root freed on success */
 		ret = btrfs_recover_log_trees(log_tree_root);
@@ -2870,24 +2870,24 @@ retry_root_backup:
 				    "Failed to recover log tree");
 			free_extent_buffer(log_tree_root->node);
 			kfree(log_tree_root);
-			goto fail_trans_kthread;
+			goto fail_qgroup;
 		}
 
 		if (sb->s_flags & MS_RDONLY) {
 			ret = btrfs_commit_super(tree_root);
 			if (ret)
-				goto fail_trans_kthread;
+				goto fail_qgroup;
 		}
 	}
 
 	ret = btrfs_find_orphan_roots(tree_root);
 	if (ret)
-		goto fail_trans_kthread;
+		goto fail_qgroup;
 
 	if (!(sb->s_flags & MS_RDONLY)) {
 		ret = btrfs_cleanup_fs_roots(fs_info);
 		if (ret)
-			goto fail_trans_kthread;
+			goto fail_qgroup;
 
 		ret = btrfs_recover_relocation(tree_root);
 		if (ret < 0) {
diff --git a/fs/btrfs/extent-tree.c b/fs/btrfs/extent-tree.c
index 1306487c82c..5590af92094 100644
--- a/fs/btrfs/extent-tree.c
+++ b/fs/btrfs/extent-tree.c
@@ -1542,6 +1542,7 @@ again:
 				ret = 0;
 		}
 		if (ret) {
+			key.objectid = bytenr;
 			key.type = BTRFS_EXTENT_ITEM_KEY;
 			key.offset = num_bytes;
 			btrfs_release_path(path);
@@ -3542,11 +3543,13 @@ static u64 btrfs_reduce_alloc_profile(struct btrfs_root *root, u64 flags)
 	return extended_to_chunk(flags | tmp);
 }
 
-static u64 get_alloc_profile(struct btrfs_root *root, u64 flags)
+static u64 get_alloc_profile(struct btrfs_root *root, u64 orig_flags)
 {
 	unsigned seq;
+	u64 flags;
 
 	do {
+		flags = orig_flags;
 		seq = read_seqbegin(&root->fs_info->profiles_lock);
 
 		if (flags & BTRFS_BLOCK_GROUP_DATA)
@@ -5719,6 +5722,7 @@ static int __btrfs_free_extent(struct btrfs_trans_handle *trans,
 
 			if (ret > 0 && skinny_metadata) {
 				skinny_metadata = false;
+				key.objectid = bytenr;
 				key.type = BTRFS_EXTENT_ITEM_KEY;
 				key.offset = num_bytes;
 				btrfs_release_path(path);
diff --git a/fs/btrfs/file.c b/fs/btrfs/file.c
index eb742c07e7a..ae6af072b63 100644
--- a/fs/btrfs/file.c
+++ b/fs/btrfs/file.c
@@ -800,7 +800,7 @@ next_slot:
 		if (start > key.offset && end < extent_end) {
 			BUG_ON(del_nr > 0);
 			if (extent_type == BTRFS_FILE_EXTENT_INLINE) {
-				ret = -EINVAL;
+				ret = -EOPNOTSUPP;
 				break;
 			}
 
@@ -846,7 +846,7 @@ next_slot:
 		 */
 		if (start <= key.offset && end < extent_end) {
 			if (extent_type == BTRFS_FILE_EXTENT_INLINE) {
-				ret = -EINVAL;
+				ret = -EOPNOTSUPP;
 				break;
 			}
 
@@ -872,7 +872,7 @@ next_slot:
 		if (start > key.offset && end >= extent_end) {
 			BUG_ON(del_nr > 0);
 			if (extent_type == BTRFS_FILE_EXTENT_INLINE) {
-				ret = -EINVAL;
+				ret = -EOPNOTSUPP;
 				break;
 			}
 
@@ -1777,7 +1777,7 @@ static ssize_t btrfs_file_aio_write(struct kiocb *iocb,
 	start_pos = round_down(pos, root->sectorsize);
 	if (start_pos > i_size_read(inode)) {
 		/* Expand hole size to cover write data, preventing empty gap */
-		end_pos = round_up(pos + iov->iov_len, root->sectorsize);
+		end_pos = round_up(pos + count, root->sectorsize);
 		err = btrfs_cont_expand(inode, i_size_read(inode), end_pos);
 		if (err) {
 			mutex_unlock(&inode->i_mutex);
diff --git a/fs/btrfs/inode-map.c b/fs/btrfs/inode-map.c
index cc8ca193d83..86935f5ae29 100644
--- a/fs/btrfs/inode-map.c
+++ b/fs/btrfs/inode-map.c
@@ -176,7 +176,11 @@ static void start_caching(struct btrfs_root *root)
 
 	tsk = kthread_run(caching_kthread, root, "btrfs-ino-cache-%llu\n",
 			  root->root_key.objectid);
-	BUG_ON(IS_ERR(tsk)); /* -ENOMEM */
+	if (IS_ERR(tsk)) {
+		btrfs_warn(root->fs_info, "failed to start inode caching task");
+		btrfs_clear_and_info(root, CHANGE_INODE_CACHE,
+				"disabling inode map caching");
+	}
 }
 
 int btrfs_find_free_ino(struct btrfs_root *root, u64 *objectid)
@@ -205,24 +209,14 @@ again:
 
 void btrfs_return_ino(struct btrfs_root *root, u64 objectid)
 {
-	struct btrfs_free_space_ctl *ctl = root->free_ino_ctl;
 	struct btrfs_free_space_ctl *pinned = root->free_ino_pinned;
 
 	if (!btrfs_test_opt(root, INODE_MAP_CACHE))
 		return;
-
 again:
 	if (root->cached == BTRFS_CACHE_FINISHED) {
-		__btrfs_add_free_space(ctl, objectid, 1);
+		__btrfs_add_free_space(pinned, objectid, 1);
 	} else {
-		/*
-		 * If we are in the process of caching free ino chunks,
-		 * to avoid adding the same inode number to the free_ino
-		 * tree twice due to cross transaction, we'll leave it
-		 * in the pinned tree until a transaction is committed
-		 * or the caching work is done.
-		 */
-
 		down_write(&root->fs_info->commit_root_sem);
 		spin_lock(&root->cache_lock);
 		if (root->cached == BTRFS_CACHE_FINISHED) {
@@ -234,11 +228,7 @@ again:
 
 		start_caching(root);
 
-		if (objectid <= root->cache_progress ||
-		    objectid >= root->highest_objectid)
-			__btrfs_add_free_space(ctl, objectid, 1);
-		else
-			__btrfs_add_free_space(pinned, objectid, 1);
+		__btrfs_add_free_space(pinned, objectid, 1);
 
 		up_write(&root->fs_info->commit_root_sem);
 	}
diff --git a/fs/btrfs/ioctl.c b/fs/btrfs/ioctl.c
index f45040a4bb7..3f52bb7a58d 100644
--- a/fs/btrfs/ioctl.c
+++ b/fs/btrfs/ioctl.c
@@ -3066,7 +3066,7 @@ process_slot:
 							 new_key.offset + datal,
 							 1);
 				if (ret) {
-					if (ret != -EINVAL)
+					if (ret != -EOPNOTSUPP)
 						btrfs_abort_transaction(trans,
 								root, ret);
 					btrfs_end_transaction(trans, root);
@@ -3120,6 +3120,8 @@ process_slot:
 			} else if (type == BTRFS_FILE_EXTENT_INLINE) {
 				u64 skip = 0;
 				u64 trim = 0;
+				u64 aligned_end = 0;
+
 				if (off > key.offset) {
 					skip = off - key.offset;
 					new_key.offset += skip;
@@ -3136,12 +3138,14 @@ process_slot:
 				size -= skip + trim;
 				datal -= skip + trim;
 
+				aligned_end = ALIGN(new_key.offset + datal,
+						    root->sectorsize);
 				ret = btrfs_drop_extents(trans, root, inode,
 							 new_key.offset,
-							 new_key.offset + datal,
+							 aligned_end,
 							 1);
 				if (ret) {
-					if (ret != -EINVAL)
+					if (ret != -EOPNOTSUPP)
 						btrfs_abort_transaction(trans,
 							root, ret);
 					btrfs_end_transaction(trans, root);
diff --git a/fs/btrfs/send.c b/fs/btrfs/send.c
index 1ac3ca98c42..fd38b505347 100644
--- a/fs/btrfs/send.c
+++ b/fs/btrfs/send.c
@@ -349,6 +349,11 @@ static int fs_path_ensure_buf(struct fs_path *p, int len)
 	if (p->buf_len >= len)
 		return 0;
 
+	if (len > PATH_MAX) {
+		WARN_ON(1);
+		return -ENOMEM;
+	}
+
 	path_len = p->end - p->start;
 	old_buf_len = p->buf_len;
 
@@ -1663,7 +1668,7 @@ static int get_first_ref(struct btrfs_root *root, u64 ino,
 		goto out;
 	}
 
-	if (key.type == BTRFS_INODE_REF_KEY) {
+	if (found_key.type == BTRFS_INODE_REF_KEY) {
 		struct btrfs_inode_ref *iref;
 		iref = btrfs_item_ptr(path->nodes[0], path->slots[0],
 				      struct btrfs_inode_ref);
diff --git a/fs/btrfs/super.c b/fs/btrfs/super.c
index 5011aadacab..9601d25a460 100644
--- a/fs/btrfs/super.c
+++ b/fs/btrfs/super.c
@@ -385,20 +385,6 @@ static match_table_t tokens = {
 	{Opt_err, NULL},
 };
 
-#define btrfs_set_and_info(root, opt, fmt, args...)			\
-{									\
-	if (!btrfs_test_opt(root, opt))					\
-		btrfs_info(root->fs_info, fmt, ##args);			\
-	btrfs_set_opt(root->fs_info->mount_opt, opt);			\
-}
-
-#define btrfs_clear_and_info(root, opt, fmt, args...)			\
-{									\
-	if (btrfs_test_opt(root, opt))					\
-		btrfs_info(root->fs_info, fmt, ##args);			\
-	btrfs_clear_opt(root->fs_info->mount_opt, opt);			\
-}
-
 /*
  * Regular mount options parser.  Everything that is needed only when
  * reading in a new superblock is parsed here.
@@ -1186,7 +1172,6 @@ static struct dentry *mount_subvol(const char *subvol_name, int flags,
 		return ERR_PTR(-ENOMEM);
 	mnt = vfs_kern_mount(&btrfs_fs_type, flags, device_name,
 			     newargs);
-	kfree(newargs);
 
 	if (PTR_RET(mnt) == -EBUSY) {
 		if (flags & MS_RDONLY) {
@@ -1196,17 +1181,22 @@ static struct dentry *mount_subvol(const char *subvol_name, int flags,
 			int r;
 			mnt = vfs_kern_mount(&btrfs_fs_type, flags | MS_RDONLY, device_name,
 					     newargs);
-			if (IS_ERR(mnt))
+			if (IS_ERR(mnt)) {
+				kfree(newargs);
 				return ERR_CAST(mnt);
+			}
 
 			r = btrfs_remount(mnt->mnt_sb, &flags, NULL);
 			if (r < 0) {
 				/* FIXME: release vfsmount mnt ??*/
+				kfree(newargs);
 				return ERR_PTR(r);
 			}
 		}
 	}
 
+	kfree(newargs);
+
 	if (IS_ERR(mnt))
 		return ERR_CAST(mnt);
 
diff --git a/fs/ceph/caps.c b/fs/ceph/caps.c
index 2e5e648eb5c..c561b628ebc 100644
--- a/fs/ceph/caps.c
+++ b/fs/ceph/caps.c
@@ -3261,7 +3261,7 @@ int ceph_encode_inode_release(void **p, struct inode *inode,
 			rel->seq = cpu_to_le32(cap->seq);
 			rel->issue_seq = cpu_to_le32(cap->issue_seq),
 			rel->mseq = cpu_to_le32(cap->mseq);
-			rel->caps = cpu_to_le32(cap->issued);
+			rel->caps = cpu_to_le32(cap->implemented);
 			rel->wanted = cpu_to_le32(cap->mds_wanted);
 			rel->dname_len = 0;
 			rel->dname_seq = 0;
diff --git a/fs/ceph/dir.c b/fs/ceph/dir.c
index 766410a12c2..c29d6ae6887 100644
--- a/fs/ceph/dir.c
+++ b/fs/ceph/dir.c
@@ -141,7 +141,7 @@ static int __dcache_readdir(struct file *file,  struct dir_context *ctx,
 
 	/* start at beginning? */
 	if (ctx->pos == 2 || last == NULL ||
-	    ctx->pos < ceph_dentry(last)->offset) {
+	    fpos_cmp(ctx->pos, ceph_dentry(last)->offset) < 0) {
 		if (list_empty(&parent->d_subdirs))
 			goto out_unlock;
 		p = parent->d_subdirs.prev;
@@ -182,9 +182,16 @@ more:
 	spin_unlock(&dentry->d_lock);
 	spin_unlock(&parent->d_lock);
 
+	/* make sure a dentry wasn't dropped while we didn't have parent lock */
+	if (!ceph_dir_is_complete(dir)) {
+		dout(" lost dir complete on %p; falling back to mds\n", dir);
+		dput(dentry);
+		err = -EAGAIN;
+		goto out;
+	}
+
 	dout(" %llu (%llu) dentry %p %.*s %p\n", di->offset, ctx->pos,
 	     dentry, dentry->d_name.len, dentry->d_name.name, dentry->d_inode);
-	ctx->pos = di->offset;
 	if (!dir_emit(ctx, dentry->d_name.name,
 		      dentry->d_name.len,
 		      ceph_translate_ino(dentry->d_sb, dentry->d_inode->i_ino),
@@ -198,19 +205,12 @@ more:
 		return 0;
 	}
 
+	ctx->pos = di->offset + 1;
+
 	if (last)
 		dput(last);
 	last = dentry;
 
-	ctx->pos++;
-
-	/* make sure a dentry wasn't dropped while we didn't have parent lock */
-	if (!ceph_dir_is_complete(dir)) {
-		dout(" lost dir complete on %p; falling back to mds\n", dir);
-		err = -EAGAIN;
-		goto out;
-	}
-
 	spin_lock(&parent->d_lock);
 	p = p->prev;	/* advance to next dentry */
 	goto more;
@@ -296,6 +296,8 @@ static int ceph_readdir(struct file *file, struct dir_context *ctx)
 		err = __dcache_readdir(file, ctx, shared_gen);
 		if (err != -EAGAIN)
 			return err;
+		frag = fpos_frag(ctx->pos);
+		off = fpos_off(ctx->pos);
 	} else {
 		spin_unlock(&ci->i_ceph_lock);
 	}
@@ -446,7 +448,6 @@ more:
 	if (atomic_read(&ci->i_release_count) == fi->dir_release_count) {
 		dout(" marking %p complete\n", inode);
 		__ceph_dir_set_complete(ci, fi->dir_release_count);
-		ci->i_max_offset = ctx->pos;
 	}
 	spin_unlock(&ci->i_ceph_lock);
 
@@ -935,14 +936,16 @@ static int ceph_rename(struct inode *old_dir, struct dentry *old_dentry,
 		 * to do it here.
 		 */
 
-		/* d_move screws up d_subdirs order */
-		ceph_dir_clear_complete(new_dir);
-
 		d_move(old_dentry, new_dentry);
 
 		/* ensure target dentry is invalidated, despite
 		   rehashing bug in vfs_rename_dir */
 		ceph_invalidate_dentry_lease(new_dentry);
+
+		/* d_move screws up sibling dentries' offsets */
+		ceph_dir_clear_complete(old_dir);
+		ceph_dir_clear_complete(new_dir);
+
 	}
 	ceph_mdsc_put_request(req);
 	return err;
diff --git a/fs/ceph/file.c b/fs/ceph/file.c
index 39da1c2efa5..88a6df4cbe6 100644
--- a/fs/ceph/file.c
+++ b/fs/ceph/file.c
@@ -1221,9 +1221,6 @@ static long ceph_fallocate(struct file *file, int mode,
 	if (!S_ISREG(inode->i_mode))
 		return -EOPNOTSUPP;
 
-	if (IS_SWAPFILE(inode))
-		return -ETXTBSY;
-
 	mutex_lock(&inode->i_mutex);
 
 	if (ceph_snap(inode) != CEPH_NOSNAP) {
diff --git a/fs/ceph/inode.c b/fs/ceph/inode.c
index 0b0728e5be2..233c6f96910 100644
--- a/fs/ceph/inode.c
+++ b/fs/ceph/inode.c
@@ -744,7 +744,6 @@ static int fill_inode(struct inode *inode,
 	    !__ceph_dir_is_complete(ci)) {
 		dout(" marking %p complete (empty)\n", inode);
 		__ceph_dir_set_complete(ci, atomic_read(&ci->i_release_count));
-		ci->i_max_offset = 2;
 	}
 no_change:
 	/* only update max_size on auth cap */
@@ -890,41 +889,6 @@ out_unlock:
 }
 
 /*
- * Set dentry's directory position based on the current dir's max, and
- * order it in d_subdirs, so that dcache_readdir behaves.
- *
- * Always called under directory's i_mutex.
- */
-static void ceph_set_dentry_offset(struct dentry *dn)
-{
-	struct dentry *dir = dn->d_parent;
-	struct inode *inode = dir->d_inode;
-	struct ceph_inode_info *ci;
-	struct ceph_dentry_info *di;
-
-	BUG_ON(!inode);
-
-	ci = ceph_inode(inode);
-	di = ceph_dentry(dn);
-
-	spin_lock(&ci->i_ceph_lock);
-	if (!__ceph_dir_is_complete(ci)) {
-		spin_unlock(&ci->i_ceph_lock);
-		return;
-	}
-	di->offset = ceph_inode(inode)->i_max_offset++;
-	spin_unlock(&ci->i_ceph_lock);
-
-	spin_lock(&dir->d_lock);
-	spin_lock_nested(&dn->d_lock, DENTRY_D_LOCK_NESTED);
-	list_move(&dn->d_u.d_child, &dir->d_subdirs);
-	dout("set_dentry_offset %p %lld (%p %p)\n", dn, di->offset,
-	     dn->d_u.d_child.prev, dn->d_u.d_child.next);
-	spin_unlock(&dn->d_lock);
-	spin_unlock(&dir->d_lock);
-}
-
-/*
  * splice a dentry to an inode.
  * caller must hold directory i_mutex for this to be safe.
  *
@@ -933,7 +897,7 @@ static void ceph_set_dentry_offset(struct dentry *dn)
  * the caller) if we fail.
  */
 static struct dentry *splice_dentry(struct dentry *dn, struct inode *in,
-				    bool *prehash, bool set_offset)
+				    bool *prehash)
 {
 	struct dentry *realdn;
 
@@ -965,8 +929,6 @@ static struct dentry *splice_dentry(struct dentry *dn, struct inode *in,
 	}
 	if ((!prehash || *prehash) && d_unhashed(dn))
 		d_rehash(dn);
-	if (set_offset)
-		ceph_set_dentry_offset(dn);
 out:
 	return dn;
 }
@@ -987,7 +949,6 @@ int ceph_fill_trace(struct super_block *sb, struct ceph_mds_request *req,
 {
 	struct ceph_mds_reply_info_parsed *rinfo = &req->r_reply_info;
 	struct inode *in = NULL;
-	struct ceph_mds_reply_inode *ininfo;
 	struct ceph_vino vino;
 	struct ceph_fs_client *fsc = ceph_sb_to_client(sb);
 	int err = 0;
@@ -1161,6 +1122,9 @@ retry_lookup:
 
 		/* rename? */
 		if (req->r_old_dentry && req->r_op == CEPH_MDS_OP_RENAME) {
+			struct inode *olddir = req->r_old_dentry_dir;
+			BUG_ON(!olddir);
+
 			dout(" src %p '%.*s' dst %p '%.*s'\n",
 			     req->r_old_dentry,
 			     req->r_old_dentry->d_name.len,
@@ -1180,13 +1144,10 @@ retry_lookup:
 			   rehashing bug in vfs_rename_dir */
 			ceph_invalidate_dentry_lease(dn);
 
-			/*
-			 * d_move() puts the renamed dentry at the end of
-			 * d_subdirs.  We need to assign it an appropriate
-			 * directory offset so we can behave when dir is
-			 * complete.
-			 */
-			ceph_set_dentry_offset(req->r_old_dentry);
+			/* d_move screws up sibling dentries' offsets */
+			ceph_dir_clear_complete(dir);
+			ceph_dir_clear_complete(olddir);
+
 			dout("dn %p gets new offset %lld\n", req->r_old_dentry,
 			     ceph_dentry(req->r_old_dentry)->offset);
 
@@ -1213,8 +1174,9 @@ retry_lookup:
 
 		/* attach proper inode */
 		if (!dn->d_inode) {
+			ceph_dir_clear_complete(dir);
 			ihold(in);
-			dn = splice_dentry(dn, in, &have_lease, true);
+			dn = splice_dentry(dn, in, &have_lease);
 			if (IS_ERR(dn)) {
 				err = PTR_ERR(dn);
 				goto done;
@@ -1235,17 +1197,16 @@ retry_lookup:
 		   (req->r_op == CEPH_MDS_OP_LOOKUPSNAP ||
 		    req->r_op == CEPH_MDS_OP_MKSNAP)) {
 		struct dentry *dn = req->r_dentry;
+		struct inode *dir = req->r_locked_dir;
 
 		/* fill out a snapdir LOOKUPSNAP dentry */
 		BUG_ON(!dn);
-		BUG_ON(!req->r_locked_dir);
-		BUG_ON(ceph_snap(req->r_locked_dir) != CEPH_SNAPDIR);
-		ininfo = rinfo->targeti.in;
-		vino.ino = le64_to_cpu(ininfo->ino);
-		vino.snap = le64_to_cpu(ininfo->snapid);
+		BUG_ON(!dir);
+		BUG_ON(ceph_snap(dir) != CEPH_SNAPDIR);
 		dout(" linking snapped dir %p to dn %p\n", in, dn);
+		ceph_dir_clear_complete(dir);
 		ihold(in);
-		dn = splice_dentry(dn, in, NULL, true);
+		dn = splice_dentry(dn, in, NULL);
 		if (IS_ERR(dn)) {
 			err = PTR_ERR(dn);
 			goto done;
@@ -1407,7 +1368,7 @@ retry_lookup:
 		}
 
 		if (!dn->d_inode) {
-			dn = splice_dentry(dn, in, NULL, false);
+			dn = splice_dentry(dn, in, NULL);
 			if (IS_ERR(dn)) {
 				err = PTR_ERR(dn);
 				dn = NULL;
diff --git a/fs/ceph/ioctl.c b/fs/ceph/ioctl.c
index fdf941b44ff..a822a6e5829 100644
--- a/fs/ceph/ioctl.c
+++ b/fs/ceph/ioctl.c
@@ -109,6 +109,8 @@ static long ceph_ioctl_set_layout(struct file *file, void __user *arg)
 		return PTR_ERR(req);
 	req->r_inode = inode;
 	ihold(inode);
+	req->r_num_caps = 1;
+
 	req->r_inode_drop = CEPH_CAP_FILE_SHARED | CEPH_CAP_FILE_EXCL;
 
 	req->r_args.setlayout.layout.fl_stripe_unit =
@@ -153,6 +155,7 @@ static long ceph_ioctl_set_layout_policy (struct file *file, void __user *arg)
 		return PTR_ERR(req);
 	req->r_inode = inode;
 	ihold(inode);
+	req->r_num_caps = 1;
 
 	req->r_args.setlayout.layout.fl_stripe_unit =
 			cpu_to_le32(l.stripe_unit);
diff --git a/fs/ceph/locks.c b/fs/ceph/locks.c
index d94ba0df9f4..191398852a2 100644
--- a/fs/ceph/locks.c
+++ b/fs/ceph/locks.c
@@ -45,6 +45,7 @@ static int ceph_lock_message(u8 lock_type, u16 operation, struct file *file,
 		return PTR_ERR(req);
 	req->r_inode = inode;
 	ihold(inode);
+	req->r_num_caps = 1;
 
 	/* mds requires start and length rather than start and end */
 	if (LLONG_MAX == fl->fl_end)
diff --git a/fs/ceph/super.h b/fs/ceph/super.h
index 7866cd05a6b..ead05cc1f44 100644
--- a/fs/ceph/super.h
+++ b/fs/ceph/super.h
@@ -266,7 +266,6 @@ struct ceph_inode_info {
 	struct timespec i_rctime;
 	u64 i_rbytes, i_rfiles, i_rsubdirs;
 	u64 i_files, i_subdirs;
-	u64 i_max_offset;  /* largest readdir offset, set with complete dir */
 
 	struct rb_root i_fragtree;
 	struct mutex i_fragtree_mutex;
diff --git a/fs/cifs/cifsfs.c b/fs/cifs/cifsfs.c
index df9c9141c09..5be1f997ecd 100644
--- a/fs/cifs/cifsfs.c
+++ b/fs/cifs/cifsfs.c
@@ -253,6 +253,11 @@ cifs_alloc_inode(struct super_block *sb)
 	cifs_set_oplock_level(cifs_inode, 0);
 	cifs_inode->delete_pending = false;
 	cifs_inode->invalid_mapping = false;
+	clear_bit(CIFS_INODE_PENDING_OPLOCK_BREAK, &cifs_inode->flags);
+	clear_bit(CIFS_INODE_PENDING_WRITERS, &cifs_inode->flags);
+	clear_bit(CIFS_INODE_DOWNGRADE_OPLOCK_TO_L2, &cifs_inode->flags);
+	spin_lock_init(&cifs_inode->writers_lock);
+	cifs_inode->writers = 0;
 	cifs_inode->vfs_inode.i_blkbits = 14;  /* 2**14 = CIFS_MAX_MSGSIZE */
 	cifs_inode->server_eof = 0;
 	cifs_inode->uniqueid = 0;
@@ -732,19 +737,26 @@ static ssize_t cifs_file_aio_write(struct kiocb *iocb, const struct iovec *iov,
 				   unsigned long nr_segs, loff_t pos)
 {
 	struct inode *inode = file_inode(iocb->ki_filp);
+	struct cifsInodeInfo *cinode = CIFS_I(inode);
 	ssize_t written;
 	int rc;
 
+	written = cifs_get_writer(cinode);
+	if (written)
+		return written;
+
 	written = generic_file_aio_write(iocb, iov, nr_segs, pos);
 
 	if (CIFS_CACHE_WRITE(CIFS_I(inode)))
-		return written;
+		goto out;
 
 	rc = filemap_fdatawrite(inode->i_mapping);
 	if (rc)
 		cifs_dbg(FYI, "cifs_file_aio_write: %d rc on %p inode\n",
 			 rc, inode);
 
+out:
+	cifs_put_writer(cinode);
 	return written;
 }
 
diff --git a/fs/cifs/cifsglob.h b/fs/cifs/cifsglob.h
index c0f3718b77a..30f6e9251a4 100644
--- a/fs/cifs/cifsglob.h
+++ b/fs/cifs/cifsglob.h
@@ -228,6 +228,8 @@ struct smb_version_operations {
 	/* verify the message */
 	int (*check_message)(char *, unsigned int);
 	bool (*is_oplock_break)(char *, struct TCP_Server_Info *);
+	void (*downgrade_oplock)(struct TCP_Server_Info *,
+					struct cifsInodeInfo *, bool);
 	/* process transaction2 response */
 	bool (*check_trans2)(struct mid_q_entry *, struct TCP_Server_Info *,
 			     char *, int);
@@ -1113,6 +1115,12 @@ struct cifsInodeInfo {
 	unsigned int epoch;		/* used to track lease state changes */
 	bool delete_pending;		/* DELETE_ON_CLOSE is set */
 	bool invalid_mapping;		/* pagecache is invalid */
+	unsigned long flags;
+#define CIFS_INODE_PENDING_OPLOCK_BREAK   (0) /* oplock break in progress */
+#define CIFS_INODE_PENDING_WRITERS	  (1) /* Writes in progress */
+#define CIFS_INODE_DOWNGRADE_OPLOCK_TO_L2 (2) /* Downgrade oplock to L2 */
+	spinlock_t writers_lock;
+	unsigned int writers;		/* Number of writers on this inode */
 	unsigned long time;		/* jiffies of last update of inode */
 	u64  server_eof;		/* current file size on server -- protected by i_lock */
 	u64  uniqueid;			/* server inode number */
diff --git a/fs/cifs/cifsproto.h b/fs/cifs/cifsproto.h
index acc4ee8ed07..ca7980a1e30 100644
--- a/fs/cifs/cifsproto.h
+++ b/fs/cifs/cifsproto.h
@@ -127,6 +127,9 @@ extern u64 cifs_UnixTimeToNT(struct timespec);
 extern struct timespec cnvrtDosUnixTm(__le16 le_date, __le16 le_time,
 				      int offset);
 extern void cifs_set_oplock_level(struct cifsInodeInfo *cinode, __u32 oplock);
+extern int cifs_get_writer(struct cifsInodeInfo *cinode);
+extern void cifs_put_writer(struct cifsInodeInfo *cinode);
+extern void cifs_done_oplock_break(struct cifsInodeInfo *cinode);
 extern int cifs_unlock_range(struct cifsFileInfo *cfile,
 			     struct file_lock *flock, const unsigned int xid);
 extern int cifs_push_mandatory_locks(struct cifsFileInfo *cfile);
diff --git a/fs/cifs/cifssmb.c b/fs/cifs/cifssmb.c
index f3264bd7a83..6ce4e0954b9 100644
--- a/fs/cifs/cifssmb.c
+++ b/fs/cifs/cifssmb.c
@@ -6197,6 +6197,9 @@ QAllEAsRetry:
 	cifs_dbg(FYI, "ea length %d\n", list_len);
 	if (list_len <= 8) {
 		cifs_dbg(FYI, "empty EA list returned from server\n");
+		/* didn't find the named attribute */
+		if (ea_name)
+			rc = -ENODATA;
 		goto QAllEAsOut;
 	}
 
diff --git a/fs/cifs/file.c b/fs/cifs/file.c
index 8add25538a3..5ed03e0b8b4 100644
--- a/fs/cifs/file.c
+++ b/fs/cifs/file.c
@@ -2599,7 +2599,7 @@ cifs_writev(struct kiocb *iocb, const struct iovec *iov,
 			ssize_t err;
 
 			err = generic_write_sync(file, iocb->ki_pos - rc, rc);
-			if (rc < 0)
+			if (err < 0)
 				rc = err;
 		}
 	} else {
@@ -2621,12 +2621,20 @@ cifs_strict_writev(struct kiocb *iocb, const struct iovec *iov,
 	struct cifs_tcon *tcon = tlink_tcon(cfile->tlink);
 	ssize_t written;
 
+	written = cifs_get_writer(cinode);
+	if (written)
+		return written;
+
 	if (CIFS_CACHE_WRITE(cinode)) {
 		if (cap_unix(tcon->ses) &&
 		(CIFS_UNIX_FCNTL_CAP & le64_to_cpu(tcon->fsUnixInfo.Capability))
-		    && ((cifs_sb->mnt_cifs_flags & CIFS_MOUNT_NOPOSIXBRL) == 0))
-			return generic_file_aio_write(iocb, iov, nr_segs, pos);
-		return cifs_writev(iocb, iov, nr_segs, pos);
+		  && ((cifs_sb->mnt_cifs_flags & CIFS_MOUNT_NOPOSIXBRL) == 0)) {
+			written = generic_file_aio_write(
+					iocb, iov, nr_segs, pos);
+			goto out;
+		}
+		written = cifs_writev(iocb, iov, nr_segs, pos);
+		goto out;
 	}
 	/*
 	 * For non-oplocked files in strict cache mode we need to write the data
@@ -2646,6 +2654,8 @@ cifs_strict_writev(struct kiocb *iocb, const struct iovec *iov,
 			 inode);
 		cinode->oplock = 0;
 	}
+out:
+	cifs_put_writer(cinode);
 	return written;
 }
 
@@ -2872,7 +2882,7 @@ ssize_t cifs_user_readv(struct kiocb *iocb, const struct iovec *iov,
 					    cifs_uncached_readv_complete);
 		if (!rdata) {
 			rc = -ENOMEM;
-			goto error;
+			break;
 		}
 
 		rc = cifs_read_allocate_pages(rdata, npages);
@@ -3621,6 +3631,13 @@ static int cifs_launder_page(struct page *page)
 	return rc;
 }
 
+static int
+cifs_pending_writers_wait(void *unused)
+{
+	schedule();
+	return 0;
+}
+
 void cifs_oplock_break(struct work_struct *work)
 {
 	struct cifsFileInfo *cfile = container_of(work, struct cifsFileInfo,
@@ -3628,8 +3645,15 @@ void cifs_oplock_break(struct work_struct *work)
 	struct inode *inode = cfile->dentry->d_inode;
 	struct cifsInodeInfo *cinode = CIFS_I(inode);
 	struct cifs_tcon *tcon = tlink_tcon(cfile->tlink);
+	struct TCP_Server_Info *server = tcon->ses->server;
 	int rc = 0;
 
+	wait_on_bit(&cinode->flags, CIFS_INODE_PENDING_WRITERS,
+			cifs_pending_writers_wait, TASK_UNINTERRUPTIBLE);
+
+	server->ops->downgrade_oplock(server, cinode,
+		test_bit(CIFS_INODE_DOWNGRADE_OPLOCK_TO_L2, &cinode->flags));
+
 	if (!CIFS_CACHE_WRITE(cinode) && CIFS_CACHE_READ(cinode) &&
 						cifs_has_mand_locks(cinode)) {
 		cifs_dbg(FYI, "Reset oplock to None for inode=%p due to mand locks\n",
@@ -3666,6 +3690,7 @@ void cifs_oplock_break(struct work_struct *work)
 							     cinode);
 		cifs_dbg(FYI, "Oplock release rc = %d\n", rc);
 	}
+	cifs_done_oplock_break(cinode);
 }
 
 /*
diff --git a/fs/cifs/inode.c b/fs/cifs/inode.c
index aadc2b68678..a22d667f106 100644
--- a/fs/cifs/inode.c
+++ b/fs/cifs/inode.c
@@ -1737,6 +1737,9 @@ cifs_inode_needs_reval(struct inode *inode)
 	if (cifs_i->time == 0)
 		return true;
 
+	if (!cifs_sb->actimeo)
+		return true;
+
 	if (!time_in_range(jiffies, cifs_i->time,
 				cifs_i->time + cifs_sb->actimeo))
 		return true;
diff --git a/fs/cifs/misc.c b/fs/cifs/misc.c
index 2f9f3790679..3b0c62e622d 100644
--- a/fs/cifs/misc.c
+++ b/fs/cifs/misc.c
@@ -466,8 +466,22 @@ is_valid_oplock_break(char *buffer, struct TCP_Server_Info *srv)
 				cifs_dbg(FYI, "file id match, oplock break\n");
 				pCifsInode = CIFS_I(netfile->dentry->d_inode);
 
-				cifs_set_oplock_level(pCifsInode,
-					pSMB->OplockLevel ? OPLOCK_READ : 0);
+				set_bit(CIFS_INODE_PENDING_OPLOCK_BREAK,
+					&pCifsInode->flags);
+
+				/*
+				 * Set flag if the server downgrades the oplock
+				 * to L2 else clear.
+				 */
+				if (pSMB->OplockLevel)
+					set_bit(
+					   CIFS_INODE_DOWNGRADE_OPLOCK_TO_L2,
+					   &pCifsInode->flags);
+				else
+					clear_bit(
+					   CIFS_INODE_DOWNGRADE_OPLOCK_TO_L2,
+					   &pCifsInode->flags);
+
 				queue_work(cifsiod_wq,
 					   &netfile->oplock_break);
 				netfile->oplock_break_cancelled = false;
@@ -551,6 +565,62 @@ void cifs_set_oplock_level(struct cifsInodeInfo *cinode, __u32 oplock)
 		cinode->oplock = 0;
 }
 
+static int
+cifs_oplock_break_wait(void *unused)
+{
+	schedule();
+	return signal_pending(current) ? -ERESTARTSYS : 0;
+}
+
+/*
+ * We wait for oplock breaks to be processed before we attempt to perform
+ * writes.
+ */
+int cifs_get_writer(struct cifsInodeInfo *cinode)
+{
+	int rc;
+
+start:
+	rc = wait_on_bit(&cinode->flags, CIFS_INODE_PENDING_OPLOCK_BREAK,
+				   cifs_oplock_break_wait, TASK_KILLABLE);
+	if (rc)
+		return rc;
+
+	spin_lock(&cinode->writers_lock);
+	if (!cinode->writers)
+		set_bit(CIFS_INODE_PENDING_WRITERS, &cinode->flags);
+	cinode->writers++;
+	/* Check to see if we have started servicing an oplock break */
+	if (test_bit(CIFS_INODE_PENDING_OPLOCK_BREAK, &cinode->flags)) {
+		cinode->writers--;
+		if (cinode->writers == 0) {
+			clear_bit(CIFS_INODE_PENDING_WRITERS, &cinode->flags);
+			wake_up_bit(&cinode->flags, CIFS_INODE_PENDING_WRITERS);
+		}
+		spin_unlock(&cinode->writers_lock);
+		goto start;
+	}
+	spin_unlock(&cinode->writers_lock);
+	return 0;
+}
+
+void cifs_put_writer(struct cifsInodeInfo *cinode)
+{
+	spin_lock(&cinode->writers_lock);
+	cinode->writers--;
+	if (cinode->writers == 0) {
+		clear_bit(CIFS_INODE_PENDING_WRITERS, &cinode->flags);
+		wake_up_bit(&cinode->flags, CIFS_INODE_PENDING_WRITERS);
+	}
+	spin_unlock(&cinode->writers_lock);
+}
+
+void cifs_done_oplock_break(struct cifsInodeInfo *cinode)
+{
+	clear_bit(CIFS_INODE_PENDING_OPLOCK_BREAK, &cinode->flags);
+	wake_up_bit(&cinode->flags, CIFS_INODE_PENDING_OPLOCK_BREAK);
+}
+
 bool
 backup_cred(struct cifs_sb_info *cifs_sb)
 {
diff --git a/fs/cifs/smb1ops.c b/fs/cifs/smb1ops.c
index 526fb89f923..d1fdfa84870 100644
--- a/fs/cifs/smb1ops.c
+++ b/fs/cifs/smb1ops.c
@@ -372,6 +372,16 @@ coalesce_t2(char *second_buf, struct smb_hdr *target_hdr)
 	return 0;
 }
 
+static void
+cifs_downgrade_oplock(struct TCP_Server_Info *server,
+			struct cifsInodeInfo *cinode, bool set_level2)
+{
+	if (set_level2)
+		cifs_set_oplock_level(cinode, OPLOCK_READ);
+	else
+		cifs_set_oplock_level(cinode, 0);
+}
+
 static bool
 cifs_check_trans2(struct mid_q_entry *mid, struct TCP_Server_Info *server,
 		  char *buf, int malformed)
@@ -1019,6 +1029,7 @@ struct smb_version_operations smb1_operations = {
 	.clear_stats = cifs_clear_stats,
 	.print_stats = cifs_print_stats,
 	.is_oplock_break = is_valid_oplock_break,
+	.downgrade_oplock = cifs_downgrade_oplock,
 	.check_trans2 = cifs_check_trans2,
 	.need_neg = cifs_need_neg,
 	.negotiate = cifs_negotiate,
diff --git a/fs/cifs/smb2misc.c b/fs/cifs/smb2misc.c
index fb3966265b6..b8021fde987 100644
--- a/fs/cifs/smb2misc.c
+++ b/fs/cifs/smb2misc.c
@@ -575,9 +575,21 @@ smb2_is_valid_oplock_break(char *buffer, struct TCP_Server_Info *server)
 				else
 					cfile->oplock_break_cancelled = false;
 
-				server->ops->set_oplock_level(cinode,
-				  rsp->OplockLevel ? SMB2_OPLOCK_LEVEL_II : 0,
-				  0, NULL);
+				set_bit(CIFS_INODE_PENDING_OPLOCK_BREAK,
+					&cinode->flags);
+
+				/*
+				 * Set flag if the server downgrades the oplock
+				 * to L2 else clear.
+				 */
+				if (rsp->OplockLevel)
+					set_bit(
+					   CIFS_INODE_DOWNGRADE_OPLOCK_TO_L2,
+					   &cinode->flags);
+				else
+					clear_bit(
+					   CIFS_INODE_DOWNGRADE_OPLOCK_TO_L2,
+					   &cinode->flags);
 
 				queue_work(cifsiod_wq, &cfile->oplock_break);
 
diff --git a/fs/cifs/smb2ops.c b/fs/cifs/smb2ops.c
index 192f51a12cf..35ddc3ed119 100644
--- a/fs/cifs/smb2ops.c
+++ b/fs/cifs/smb2ops.c
@@ -905,6 +905,17 @@ smb2_query_symlink(const unsigned int xid, struct cifs_tcon *tcon,
 }
 
 static void
+smb2_downgrade_oplock(struct TCP_Server_Info *server,
+			struct cifsInodeInfo *cinode, bool set_level2)
+{
+	if (set_level2)
+		server->ops->set_oplock_level(cinode, SMB2_OPLOCK_LEVEL_II,
+						0, NULL);
+	else
+		server->ops->set_oplock_level(cinode, 0, 0, NULL);
+}
+
+static void
 smb2_set_oplock_level(struct cifsInodeInfo *cinode, __u32 oplock,
 		      unsigned int epoch, bool *purge_cache)
 {
@@ -1110,6 +1121,7 @@ struct smb_version_operations smb20_operations = {
 	.clear_stats = smb2_clear_stats,
 	.print_stats = smb2_print_stats,
 	.is_oplock_break = smb2_is_valid_oplock_break,
+	.downgrade_oplock = smb2_downgrade_oplock,
 	.need_neg = smb2_need_neg,
 	.negotiate = smb2_negotiate,
 	.negotiate_wsize = smb2_negotiate_wsize,
@@ -1184,6 +1196,7 @@ struct smb_version_operations smb21_operations = {
 	.clear_stats = smb2_clear_stats,
 	.print_stats = smb2_print_stats,
 	.is_oplock_break = smb2_is_valid_oplock_break,
+	.downgrade_oplock = smb2_downgrade_oplock,
 	.need_neg = smb2_need_neg,
 	.negotiate = smb2_negotiate,
 	.negotiate_wsize = smb2_negotiate_wsize,
@@ -1259,6 +1272,7 @@ struct smb_version_operations smb30_operations = {
 	.print_stats = smb2_print_stats,
 	.dump_share_caps = smb2_dump_share_caps,
 	.is_oplock_break = smb2_is_valid_oplock_break,
+	.downgrade_oplock = smb2_downgrade_oplock,
 	.need_neg = smb2_need_neg,
 	.negotiate = smb2_negotiate,
 	.negotiate_wsize = smb2_negotiate_wsize,
diff --git a/fs/cifs/smb2pdu.c b/fs/cifs/smb2pdu.c
index 86034470106..3802f8c94ac 100644
--- a/fs/cifs/smb2pdu.c
+++ b/fs/cifs/smb2pdu.c
@@ -1352,7 +1352,6 @@ SMB2_set_compression(const unsigned int xid, struct cifs_tcon *tcon,
 		     u64 persistent_fid, u64 volatile_fid)
 {
 	int rc;
-	char *res_key = NULL;
 	struct  compress_ioctl fsctl_input;
 	char *ret_data = NULL;
 
@@ -1365,7 +1364,6 @@ SMB2_set_compression(const unsigned int xid, struct cifs_tcon *tcon,
 			2 /* in data len */, &ret_data /* out data */, NULL);
 
 	cifs_dbg(FYI, "set compression rc %d\n", rc);
-	kfree(res_key);
 
 	return rc;
 }
diff --git a/fs/compat.c b/fs/compat.c
index ca926ad0430..66d3d3c6b4b 100644
--- a/fs/compat.c
+++ b/fs/compat.c
@@ -457,9 +457,9 @@ COMPAT_SYSCALL_DEFINE3(fcntl64, unsigned int, fd, unsigned int, cmd,
 	case F_GETLK64:
 	case F_SETLK64:
 	case F_SETLKW64:
-	case F_GETLKP:
-	case F_SETLKP:
-	case F_SETLKPW:
+	case F_OFD_GETLK:
+	case F_OFD_SETLK:
+	case F_OFD_SETLKW:
 		ret = get_compat_flock64(&f, compat_ptr(arg));
 		if (ret != 0)
 			break;
@@ -468,7 +468,7 @@ COMPAT_SYSCALL_DEFINE3(fcntl64, unsigned int, fd, unsigned int, cmd,
 		conv_cmd = convert_fcntl_cmd(cmd);
 		ret = sys_fcntl(fd, conv_cmd, (unsigned long)&f);
 		set_fs(old_fs);
-		if ((conv_cmd == F_GETLK || conv_cmd == F_GETLKP) && ret == 0) {
+		if ((conv_cmd == F_GETLK || conv_cmd == F_OFD_GETLK) && ret == 0) {
 			/* need to return lock information - see above for commentary */
 			if (f.l_start > COMPAT_LOFF_T_MAX)
 				ret = -EOVERFLOW;
@@ -493,9 +493,9 @@ COMPAT_SYSCALL_DEFINE3(fcntl, unsigned int, fd, unsigned int, cmd,
 	case F_GETLK64:
 	case F_SETLK64:
 	case F_SETLKW64:
-	case F_GETLKP:
-	case F_SETLKP:
-	case F_SETLKPW:
+	case F_OFD_GETLK:
+	case F_OFD_SETLK:
+	case F_OFD_SETLKW:
 		return -EINVAL;
 	}
 	return compat_sys_fcntl64(fd, cmd, arg);
diff --git a/fs/coredump.c b/fs/coredump.c
index e3ad709a423..0b2528fb640 100644
--- a/fs/coredump.c
+++ b/fs/coredump.c
@@ -73,10 +73,15 @@ static int expand_corename(struct core_name *cn, int size)
 static int cn_vprintf(struct core_name *cn, const char *fmt, va_list arg)
 {
 	int free, need;
+	va_list arg_copy;
 
 again:
 	free = cn->size - cn->used;
-	need = vsnprintf(cn->corename + cn->used, free, fmt, arg);
+
+	va_copy(arg_copy, arg);
+	need = vsnprintf(cn->corename + cn->used, free, fmt, arg_copy);
+	va_end(arg_copy);
+
 	if (need < free) {
 		cn->used += need;
 		return 0;
diff --git a/fs/dcache.c b/fs/dcache.c
index 40707d88a94..be2bea834bf 100644
--- a/fs/dcache.c
+++ b/fs/dcache.c
@@ -246,16 +246,8 @@ static void __d_free(struct rcu_head *head)
 	kmem_cache_free(dentry_cache, dentry); 
 }
 
-/*
- * no locks, please.
- */
-static void d_free(struct dentry *dentry)
+static void dentry_free(struct dentry *dentry)
 {
-	BUG_ON((int)dentry->d_lockref.count > 0);
-	this_cpu_dec(nr_dentry);
-	if (dentry->d_op && dentry->d_op->d_release)
-		dentry->d_op->d_release(dentry);
-
 	/* if dentry was never visible to RCU, immediate free is OK */
 	if (!(dentry->d_flags & DCACHE_RCUACCESS))
 		__d_free(&dentry->d_u.d_rcu);
@@ -403,56 +395,6 @@ static void dentry_lru_add(struct dentry *dentry)
 		d_lru_add(dentry);
 }
 
-/*
- * Remove a dentry with references from the LRU.
- *
- * If we are on the shrink list, then we can get to try_prune_one_dentry() and
- * lose our last reference through the parent walk. In this case, we need to
- * remove ourselves from the shrink list, not the LRU.
- */
-static void dentry_lru_del(struct dentry *dentry)
-{
-	if (dentry->d_flags & DCACHE_LRU_LIST) {
-		if (dentry->d_flags & DCACHE_SHRINK_LIST)
-			return d_shrink_del(dentry);
-		d_lru_del(dentry);
-	}
-}
-
-/**
- * d_kill - kill dentry and return parent
- * @dentry: dentry to kill
- * @parent: parent dentry
- *
- * The dentry must already be unhashed and removed from the LRU.
- *
- * If this is the root of the dentry tree, return NULL.
- *
- * dentry->d_lock and parent->d_lock must be held by caller, and are dropped by
- * d_kill.
- */
-static struct dentry *d_kill(struct dentry *dentry, struct dentry *parent)
-	__releases(dentry->d_lock)
-	__releases(parent->d_lock)
-	__releases(dentry->d_inode->i_lock)
-{
-	list_del(&dentry->d_u.d_child);
-	/*
-	 * Inform d_walk() that we are no longer attached to the
-	 * dentry tree
-	 */
-	dentry->d_flags |= DCACHE_DENTRY_KILLED;
-	if (parent)
-		spin_unlock(&parent->d_lock);
-	dentry_iput(dentry);
-	/*
-	 * dentry_iput drops the locks, at which point nobody (except
-	 * transient RCU lookups) can reach this dentry.
-	 */
-	d_free(dentry);
-	return parent;
-}
-
 /**
  * d_drop - drop a dentry
  * @dentry: dentry to drop
@@ -499,37 +441,12 @@ void d_drop(struct dentry *dentry)
 }
 EXPORT_SYMBOL(d_drop);
 
-/*
- * Finish off a dentry we've decided to kill.
- * dentry->d_lock must be held, returns with it unlocked.
- * If ref is non-zero, then decrement the refcount too.
- * Returns dentry requiring refcount drop, or NULL if we're done.
- */
-static struct dentry *
-dentry_kill(struct dentry *dentry, int unlock_on_failure)
-	__releases(dentry->d_lock)
+static void __dentry_kill(struct dentry *dentry)
 {
-	struct inode *inode;
-	struct dentry *parent;
-
-	inode = dentry->d_inode;
-	if (inode && !spin_trylock(&inode->i_lock)) {
-relock:
-		if (unlock_on_failure) {
-			spin_unlock(&dentry->d_lock);
-			cpu_relax();
-		}
-		return dentry; /* try again with same dentry */
-	}
-	if (IS_ROOT(dentry))
-		parent = NULL;
-	else
+	struct dentry *parent = NULL;
+	bool can_free = true;
+	if (!IS_ROOT(dentry))
 		parent = dentry->d_parent;
-	if (parent && !spin_trylock(&parent->d_lock)) {
-		if (inode)
-			spin_unlock(&inode->i_lock);
-		goto relock;
-	}
 
 	/*
 	 * The dentry is now unrecoverably dead to the world.
@@ -543,10 +460,103 @@ relock:
 	if ((dentry->d_flags & DCACHE_OP_PRUNE) && !d_unhashed(dentry))
 		dentry->d_op->d_prune(dentry);
 
-	dentry_lru_del(dentry);
+	if (dentry->d_flags & DCACHE_LRU_LIST) {
+		if (!(dentry->d_flags & DCACHE_SHRINK_LIST))
+			d_lru_del(dentry);
+	}
 	/* if it was on the hash then remove it */
 	__d_drop(dentry);
-	return d_kill(dentry, parent);
+	list_del(&dentry->d_u.d_child);
+	/*
+	 * Inform d_walk() that we are no longer attached to the
+	 * dentry tree
+	 */
+	dentry->d_flags |= DCACHE_DENTRY_KILLED;
+	if (parent)
+		spin_unlock(&parent->d_lock);
+	dentry_iput(dentry);
+	/*
+	 * dentry_iput drops the locks, at which point nobody (except
+	 * transient RCU lookups) can reach this dentry.
+	 */
+	BUG_ON((int)dentry->d_lockref.count > 0);
+	this_cpu_dec(nr_dentry);
+	if (dentry->d_op && dentry->d_op->d_release)
+		dentry->d_op->d_release(dentry);
+
+	spin_lock(&dentry->d_lock);
+	if (dentry->d_flags & DCACHE_SHRINK_LIST) {
+		dentry->d_flags |= DCACHE_MAY_FREE;
+		can_free = false;
+	}
+	spin_unlock(&dentry->d_lock);
+	if (likely(can_free))
+		dentry_free(dentry);
+}
+
+/*
+ * Finish off a dentry we've decided to kill.
+ * dentry->d_lock must be held, returns with it unlocked.
+ * If ref is non-zero, then decrement the refcount too.
+ * Returns dentry requiring refcount drop, or NULL if we're done.
+ */
+static struct dentry *dentry_kill(struct dentry *dentry)
+	__releases(dentry->d_lock)
+{
+	struct inode *inode = dentry->d_inode;
+	struct dentry *parent = NULL;
+
+	if (inode && unlikely(!spin_trylock(&inode->i_lock)))
+		goto failed;
+
+	if (!IS_ROOT(dentry)) {
+		parent = dentry->d_parent;
+		if (unlikely(!spin_trylock(&parent->d_lock))) {
+			if (inode)
+				spin_unlock(&inode->i_lock);
+			goto failed;
+		}
+	}
+
+	__dentry_kill(dentry);
+	return parent;
+
+failed:
+	spin_unlock(&dentry->d_lock);
+	cpu_relax();
+	return dentry; /* try again with same dentry */
+}
+
+static inline struct dentry *lock_parent(struct dentry *dentry)
+{
+	struct dentry *parent = dentry->d_parent;
+	if (IS_ROOT(dentry))
+		return NULL;
+	if (likely(spin_trylock(&parent->d_lock)))
+		return parent;
+	spin_unlock(&dentry->d_lock);
+	rcu_read_lock();
+again:
+	parent = ACCESS_ONCE(dentry->d_parent);
+	spin_lock(&parent->d_lock);
+	/*
+	 * We can't blindly lock dentry until we are sure
+	 * that we won't violate the locking order.
+	 * Any changes of dentry->d_parent must have
+	 * been done with parent->d_lock held, so
+	 * spin_lock() above is enough of a barrier
+	 * for checking if it's still our child.
+	 */
+	if (unlikely(parent != dentry->d_parent)) {
+		spin_unlock(&parent->d_lock);
+		goto again;
+	}
+	rcu_read_unlock();
+	if (parent != dentry)
+		spin_lock_nested(&dentry->d_lock, DENTRY_D_LOCK_NESTED);
+	else
+		parent = NULL;
+	return parent;
 }
 
 /* 
@@ -602,7 +612,7 @@ repeat:
 	return;
 
 kill_it:
-	dentry = dentry_kill(dentry, 1);
+	dentry = dentry_kill(dentry);
 	if (dentry)
 		goto repeat;
 }
@@ -815,64 +825,15 @@ restart:
 }
 EXPORT_SYMBOL(d_prune_aliases);
 
-/*
- * Try to throw away a dentry - free the inode, dput the parent.
- * Requires dentry->d_lock is held, and dentry->d_count == 0.
- * Releases dentry->d_lock.
- *
- * This may fail if locks cannot be acquired no problem, just try again.
- */
-static struct dentry * try_prune_one_dentry(struct dentry *dentry)
-	__releases(dentry->d_lock)
-{
-	struct dentry *parent;
-
-	parent = dentry_kill(dentry, 0);
-	/*
-	 * If dentry_kill returns NULL, we have nothing more to do.
-	 * if it returns the same dentry, trylocks failed. In either
-	 * case, just loop again.
-	 *
-	 * Otherwise, we need to prune ancestors too. This is necessary
-	 * to prevent quadratic behavior of shrink_dcache_parent(), but
-	 * is also expected to be beneficial in reducing dentry cache
-	 * fragmentation.
-	 */
-	if (!parent)
-		return NULL;
-	if (parent == dentry)
-		return dentry;
-
-	/* Prune ancestors. */
-	dentry = parent;
-	while (dentry) {
-		if (lockref_put_or_lock(&dentry->d_lockref))
-			return NULL;
-		dentry = dentry_kill(dentry, 1);
-	}
-	return NULL;
-}
-
 static void shrink_dentry_list(struct list_head *list)
 {
-	struct dentry *dentry;
-
-	rcu_read_lock();
-	for (;;) {
-		dentry = list_entry_rcu(list->prev, struct dentry, d_lru);
-		if (&dentry->d_lru == list)
-			break; /* empty */
+	struct dentry *dentry, *parent;
 
-		/*
-		 * Get the dentry lock, and re-verify that the dentry is
-		 * this on the shrinking list. If it is, we know that
-		 * DCACHE_SHRINK_LIST and DCACHE_LRU_LIST are set.
-		 */
+	while (!list_empty(list)) {
+		struct inode *inode;
+		dentry = list_entry(list->prev, struct dentry, d_lru);
 		spin_lock(&dentry->d_lock);
-		if (dentry != list_entry(list->prev, struct dentry, d_lru)) {
-			spin_unlock(&dentry->d_lock);
-			continue;
-		}
+		parent = lock_parent(dentry);
 
 		/*
 		 * The dispose list is isolated and dentries are not accounted
@@ -885,30 +846,63 @@ static void shrink_dentry_list(struct list_head *list)
 		 * We found an inuse dentry which was not removed from
 		 * the LRU because of laziness during lookup. Do not free it.
 		 */
-		if (dentry->d_lockref.count) {
+		if ((int)dentry->d_lockref.count > 0) {
 			spin_unlock(&dentry->d_lock);
+			if (parent)
+				spin_unlock(&parent->d_lock);
 			continue;
 		}
-		rcu_read_unlock();
 
-		/*
-		 * If 'try_to_prune()' returns a dentry, it will
-		 * be the same one we passed in, and d_lock will
-		 * have been held the whole time, so it will not
-		 * have been added to any other lists. We failed
-		 * to get the inode lock.
-		 *
-		 * We just add it back to the shrink list.
-		 */
-		dentry = try_prune_one_dentry(dentry);
 
-		rcu_read_lock();
-		if (dentry) {
+		if (unlikely(dentry->d_flags & DCACHE_DENTRY_KILLED)) {
+			bool can_free = dentry->d_flags & DCACHE_MAY_FREE;
+			spin_unlock(&dentry->d_lock);
+			if (parent)
+				spin_unlock(&parent->d_lock);
+			if (can_free)
+				dentry_free(dentry);
+			continue;
+		}
+
+		inode = dentry->d_inode;
+		if (inode && unlikely(!spin_trylock(&inode->i_lock))) {
 			d_shrink_add(dentry, list);
 			spin_unlock(&dentry->d_lock);
+			if (parent)
+				spin_unlock(&parent->d_lock);
+			continue;
+		}
+
+		__dentry_kill(dentry);
+
+		/*
+		 * We need to prune ancestors too. This is necessary to prevent
+		 * quadratic behavior of shrink_dcache_parent(), but is also
+		 * expected to be beneficial in reducing dentry cache
+		 * fragmentation.
+		 */
+		dentry = parent;
+		while (dentry && !lockref_put_or_lock(&dentry->d_lockref)) {
+			parent = lock_parent(dentry);
+			if (dentry->d_lockref.count != 1) {
+				dentry->d_lockref.count--;
+				spin_unlock(&dentry->d_lock);
+				if (parent)
+					spin_unlock(&parent->d_lock);
+				break;
+			}
+			inode = dentry->d_inode;	/* can't be NULL */
+			if (unlikely(!spin_trylock(&inode->i_lock))) {
+				spin_unlock(&dentry->d_lock);
+				if (parent)
+					spin_unlock(&parent->d_lock);
+				cpu_relax();
+				continue;
+			}
+			__dentry_kill(dentry);
+			dentry = parent;
 		}
 	}
-	rcu_read_unlock();
 }
 
 static enum lru_status
@@ -1261,34 +1255,23 @@ static enum d_walk_ret select_collect(void *_data, struct dentry *dentry)
 	if (data->start == dentry)
 		goto out;
 
-	/*
-	 * move only zero ref count dentries to the dispose list.
-	 *
-	 * Those which are presently on the shrink list, being processed
-	 * by shrink_dentry_list(), shouldn't be moved.  Otherwise the
-	 * loop in shrink_dcache_parent() might not make any progress
-	 * and loop forever.
-	 */
-	if (dentry->d_lockref.count) {
-		dentry_lru_del(dentry);
-	} else if (!(dentry->d_flags & DCACHE_SHRINK_LIST)) {
-		/*
-		 * We can't use d_lru_shrink_move() because we
-		 * need to get the global LRU lock and do the
-		 * LRU accounting.
-		 */
-		d_lru_del(dentry);
-		d_shrink_add(dentry, &data->dispose);
+	if (dentry->d_flags & DCACHE_SHRINK_LIST) {
 		data->found++;
-		ret = D_WALK_NORETRY;
+	} else {
+		if (dentry->d_flags & DCACHE_LRU_LIST)
+			d_lru_del(dentry);
+		if (!dentry->d_lockref.count) {
+			d_shrink_add(dentry, &data->dispose);
+			data->found++;
+		}
 	}
 	/*
 	 * We can return to the caller if we have found some (this
 	 * ensures forward progress). We'll be coming back to find
 	 * the rest.
 	 */
-	if (data->found && need_resched())
-		ret = D_WALK_QUIT;
+	if (!list_empty(&data->dispose))
+		ret = need_resched() ? D_WALK_QUIT : D_WALK_NORETRY;
 out:
 	return ret;
 }
@@ -1318,45 +1301,35 @@ void shrink_dcache_parent(struct dentry *parent)
 }
 EXPORT_SYMBOL(shrink_dcache_parent);
 
-static enum d_walk_ret umount_collect(void *_data, struct dentry *dentry)
+static enum d_walk_ret umount_check(void *_data, struct dentry *dentry)
 {
-	struct select_data *data = _data;
-	enum d_walk_ret ret = D_WALK_CONTINUE;
+	/* it has busy descendents; complain about those instead */
+	if (!list_empty(&dentry->d_subdirs))
+		return D_WALK_CONTINUE;
 
-	if (dentry->d_lockref.count) {
-		dentry_lru_del(dentry);
-		if (likely(!list_empty(&dentry->d_subdirs)))
-			goto out;
-		if (dentry == data->start && dentry->d_lockref.count == 1)
-			goto out;
-		printk(KERN_ERR
-		       "BUG: Dentry %p{i=%lx,n=%s}"
-		       " still in use (%d)"
-		       " [unmount of %s %s]\n",
+	/* root with refcount 1 is fine */
+	if (dentry == _data && dentry->d_lockref.count == 1)
+		return D_WALK_CONTINUE;
+
+	printk(KERN_ERR "BUG: Dentry %p{i=%lx,n=%pd} "
+			" still in use (%d) [unmount of %s %s]\n",
 		       dentry,
 		       dentry->d_inode ?
 		       dentry->d_inode->i_ino : 0UL,
-		       dentry->d_name.name,
+		       dentry,
 		       dentry->d_lockref.count,
 		       dentry->d_sb->s_type->name,
 		       dentry->d_sb->s_id);
-		BUG();
-	} else if (!(dentry->d_flags & DCACHE_SHRINK_LIST)) {
-		/*
-		 * We can't use d_lru_shrink_move() because we
-		 * need to get the global LRU lock and do the
-		 * LRU accounting.
-		 */
-		if (dentry->d_flags & DCACHE_LRU_LIST)
-			d_lru_del(dentry);
-		d_shrink_add(dentry, &data->dispose);
-		data->found++;
-		ret = D_WALK_NORETRY;
-	}
-out:
-	if (data->found && need_resched())
-		ret = D_WALK_QUIT;
-	return ret;
+	WARN_ON(1);
+	return D_WALK_CONTINUE;
+}
+
+static void do_one_tree(struct dentry *dentry)
+{
+	shrink_dcache_parent(dentry);
+	d_walk(dentry, dentry, umount_check, NULL);
+	d_drop(dentry);
+	dput(dentry);
 }
 
 /*
@@ -1366,40 +1339,15 @@ void shrink_dcache_for_umount(struct super_block *sb)
 {
 	struct dentry *dentry;
 
-	if (down_read_trylock(&sb->s_umount))
-		BUG();
+	WARN(down_read_trylock(&sb->s_umount), "s_umount should've been locked");
 
 	dentry = sb->s_root;
 	sb->s_root = NULL;
-	for (;;) {
-		struct select_data data;
-
-		INIT_LIST_HEAD(&data.dispose);
-		data.start = dentry;
-		data.found = 0;
-
-		d_walk(dentry, &data, umount_collect, NULL);
-		if (!data.found)
-			break;
-
-		shrink_dentry_list(&data.dispose);
-		cond_resched();
-	}
-	d_drop(dentry);
-	dput(dentry);
+	do_one_tree(dentry);
 
 	while (!hlist_bl_empty(&sb->s_anon)) {
-		struct select_data data;
-		dentry = hlist_bl_entry(hlist_bl_first(&sb->s_anon), struct dentry, d_hash);
-
-		INIT_LIST_HEAD(&data.dispose);
-		data.start = NULL;
-		data.found = 0;
-
-		d_walk(dentry, &data, umount_collect, NULL);
-		if (data.found)
-			shrink_dentry_list(&data.dispose);
-		cond_resched();
+		dentry = dget(hlist_bl_entry(hlist_bl_first(&sb->s_anon), struct dentry, d_hash));
+		do_one_tree(dentry);
 	}
 }
 
@@ -1647,8 +1595,7 @@ static void __d_instantiate(struct dentry *dentry, struct inode *inode)
 	unsigned add_flags = d_flags_for_inode(inode);
 
 	spin_lock(&dentry->d_lock);
-	dentry->d_flags &= ~DCACHE_ENTRY_TYPE;
-	dentry->d_flags |= add_flags;
+	__d_set_type(dentry, add_flags);
 	if (inode)
 		hlist_add_head(&dentry->d_alias, &inode->i_dentry);
 	dentry->d_inode = inode;
diff --git a/fs/exec.c b/fs/exec.c
index 476f3ebf437..238b7aa26f6 100644
--- a/fs/exec.c
+++ b/fs/exec.c
@@ -657,10 +657,10 @@ int setup_arg_pages(struct linux_binprm *bprm,
 	unsigned long rlim_stack;
 
 #ifdef CONFIG_STACK_GROWSUP
-	/* Limit stack size to 1GB */
+	/* Limit stack size */
 	stack_base = rlimit_max(RLIMIT_STACK);
-	if (stack_base > (1 << 30))
-		stack_base = 1 << 30;
+	if (stack_base > STACK_SIZE_MAX)
+		stack_base = STACK_SIZE_MAX;
 
 	/* Make sure we didn't let the argument array grow too large. */
 	if (vma->vm_end - vma->vm_start > stack_base)
diff --git a/fs/ext4/balloc.c b/fs/ext4/balloc.c
index 6ea7b1436bb..5c56785007e 100644
--- a/fs/ext4/balloc.c
+++ b/fs/ext4/balloc.c
@@ -667,7 +667,7 @@ ext4_fsblk_t ext4_count_free_clusters(struct super_block *sb)
 			continue;
 
 		x = ext4_count_free(bitmap_bh->b_data,
-				    EXT4_BLOCKS_PER_GROUP(sb) / 8);
+				    EXT4_CLUSTERS_PER_GROUP(sb) / 8);
 		printk(KERN_DEBUG "group %u: stored = %d, counted = %u\n",
 			i, ext4_free_group_clusters(sb, gdp), x);
 		bitmap_count += x;
diff --git a/fs/ext4/ext4.h b/fs/ext4/ext4.h
index f1c65dc7cc0..66946aa6212 100644
--- a/fs/ext4/ext4.h
+++ b/fs/ext4/ext4.h
@@ -2466,23 +2466,6 @@ static inline void ext4_update_i_disksize(struct inode *inode, loff_t newsize)
 	up_write(&EXT4_I(inode)->i_data_sem);
 }
 
-/*
- * Update i_disksize after writeback has been started. Races with truncate
- * are avoided by checking i_size under i_data_sem.
- */
-static inline void ext4_wb_update_i_disksize(struct inode *inode, loff_t newsize)
-{
-	loff_t i_size;
-
-	down_write(&EXT4_I(inode)->i_data_sem);
-	i_size = i_size_read(inode);
-	if (newsize > i_size)
-		newsize = i_size;
-	if (newsize > EXT4_I(inode)->i_disksize)
-		EXT4_I(inode)->i_disksize = newsize;
-	up_write(&EXT4_I(inode)->i_data_sem);
-}
-
 struct ext4_group_info {
 	unsigned long   bb_state;
 	struct rb_root  bb_free_root;
diff --git a/fs/ext4/extents.c b/fs/ext4/extents.c
index 82df3ce9874..01b0c208f62 100644
--- a/fs/ext4/extents.c
+++ b/fs/ext4/extents.c
@@ -3313,6 +3313,11 @@ static int ext4_split_extent(handle_t *handle,
 		return PTR_ERR(path);
 	depth = ext_depth(inode);
 	ex = path[depth].p_ext;
+	if (!ex) {
+		EXT4_ERROR_INODE(inode, "unexpected hole at %lu",
+				 (unsigned long) map->m_lblk);
+		return -EIO;
+	}
 	uninitialized = ext4_ext_is_uninitialized(ex);
 	split_flag1 = 0;
 
@@ -3694,6 +3699,12 @@ static int ext4_convert_initialized_extents(handle_t *handle,
 		}
 		depth = ext_depth(inode);
 		ex = path[depth].p_ext;
+		if (!ex) {
+			EXT4_ERROR_INODE(inode, "unexpected hole at %lu",
+					 (unsigned long) map->m_lblk);
+			err = -EIO;
+			goto out;
+		}
 	}
 
 	err = ext4_ext_get_access(handle, inode, path + depth);
@@ -4730,6 +4741,9 @@ static long ext4_zero_range(struct file *file, loff_t offset,
 
 	trace_ext4_zero_range(inode, offset, len, mode);
 
+	if (!S_ISREG(inode->i_mode))
+		return -EINVAL;
+
 	/*
 	 * Write out all dirty pages to avoid race conditions
 	 * Then release them.
@@ -4878,9 +4892,6 @@ long ext4_fallocate(struct file *file, int mode, loff_t offset, loff_t len)
 	if (mode & FALLOC_FL_PUNCH_HOLE)
 		return ext4_punch_hole(inode, offset, len);
 
-	if (mode & FALLOC_FL_COLLAPSE_RANGE)
-		return ext4_collapse_range(inode, offset, len);
-
 	ret = ext4_convert_inline_data(inode);
 	if (ret)
 		return ret;
@@ -4892,6 +4903,9 @@ long ext4_fallocate(struct file *file, int mode, loff_t offset, loff_t len)
 	if (!(ext4_test_inode_flag(inode, EXT4_INODE_EXTENTS)))
 		return -EOPNOTSUPP;
 
+	if (mode & FALLOC_FL_COLLAPSE_RANGE)
+		return ext4_collapse_range(inode, offset, len);
+
 	if (mode & FALLOC_FL_ZERO_RANGE)
 		return ext4_zero_range(file, offset, len, mode);
 
@@ -5229,18 +5243,19 @@ ext4_ext_shift_path_extents(struct ext4_ext_path *path, ext4_lblk_t shift,
 			if (ex_start == EXT_FIRST_EXTENT(path[depth].p_hdr))
 				update = 1;
 
-			*start = ex_last->ee_block +
+			*start = le32_to_cpu(ex_last->ee_block) +
 				ext4_ext_get_actual_len(ex_last);
 
 			while (ex_start <= ex_last) {
-				ex_start->ee_block -= shift;
-				if (ex_start >
-					EXT_FIRST_EXTENT(path[depth].p_hdr)) {
-					if (ext4_ext_try_to_merge_right(inode,
-						path, ex_start - 1))
-						ex_last--;
-				}
-				ex_start++;
+				le32_add_cpu(&ex_start->ee_block, -shift);
+				/* Try to merge to the left. */
+				if ((ex_start >
+				     EXT_FIRST_EXTENT(path[depth].p_hdr)) &&
+				    ext4_ext_try_to_merge_right(inode,
+							path, ex_start - 1))
+					ex_last--;
+				else
+					ex_start++;
 			}
 			err = ext4_ext_dirty(handle, inode, path + depth);
 			if (err)
@@ -5255,7 +5270,7 @@ ext4_ext_shift_path_extents(struct ext4_ext_path *path, ext4_lblk_t shift,
 		if (err)
 			goto out;
 
-		path[depth].p_idx->ei_block -= shift;
+		le32_add_cpu(&path[depth].p_idx->ei_block, -shift);
 		err = ext4_ext_dirty(handle, inode, path + depth);
 		if (err)
 			goto out;
@@ -5300,7 +5315,8 @@ ext4_ext_shift_extents(struct inode *inode, handle_t *handle,
 		return ret;
 	}
 
-	stop_block = extent->ee_block + ext4_ext_get_actual_len(extent);
+	stop_block = le32_to_cpu(extent->ee_block) +
+			ext4_ext_get_actual_len(extent);
 	ext4_ext_drop_refs(path);
 	kfree(path);
 
@@ -5313,10 +5329,18 @@ ext4_ext_shift_extents(struct inode *inode, handle_t *handle,
 	 * enough to accomodate the shift.
 	 */
 	path = ext4_ext_find_extent(inode, start - 1, NULL, 0);
+	if (IS_ERR(path))
+		return PTR_ERR(path);
 	depth = path->p_depth;
 	extent =  path[depth].p_ext;
-	ex_start = extent->ee_block;
-	ex_end = extent->ee_block + ext4_ext_get_actual_len(extent);
+	if (extent) {
+		ex_start = le32_to_cpu(extent->ee_block);
+		ex_end = le32_to_cpu(extent->ee_block) +
+			ext4_ext_get_actual_len(extent);
+	} else {
+		ex_start = 0;
+		ex_end = 0;
+	}
 	ext4_ext_drop_refs(path);
 	kfree(path);
 
@@ -5331,7 +5355,13 @@ ext4_ext_shift_extents(struct inode *inode, handle_t *handle,
 			return PTR_ERR(path);
 		depth = path->p_depth;
 		extent = path[depth].p_ext;
-		current_block = extent->ee_block;
+		if (!extent) {
+			EXT4_ERROR_INODE(inode, "unexpected hole at %lu",
+					 (unsigned long) start);
+			return -EIO;
+		}
+
+		current_block = le32_to_cpu(extent->ee_block);
 		if (start > current_block) {
 			/* Hole, move to the next extent */
 			ret = mext_next_extent(inode, path, &extent);
@@ -5365,17 +5395,18 @@ int ext4_collapse_range(struct inode *inode, loff_t offset, loff_t len)
 	ext4_lblk_t punch_start, punch_stop;
 	handle_t *handle;
 	unsigned int credits;
-	loff_t new_size;
+	loff_t new_size, ioffset;
 	int ret;
 
-	BUG_ON(offset + len > i_size_read(inode));
-
 	/* Collapse range works only on fs block size aligned offsets. */
 	if (offset & (EXT4_BLOCK_SIZE(sb) - 1) ||
 	    len & (EXT4_BLOCK_SIZE(sb) - 1))
 		return -EINVAL;
 
 	if (!S_ISREG(inode->i_mode))
+		return -EINVAL;
+
+	if (EXT4_SB(inode->i_sb)->s_cluster_ratio > 1)
 		return -EOPNOTSUPP;
 
 	trace_ext4_collapse_range(inode, offset, len);
@@ -5383,22 +5414,34 @@ int ext4_collapse_range(struct inode *inode, loff_t offset, loff_t len)
 	punch_start = offset >> EXT4_BLOCK_SIZE_BITS(sb);
 	punch_stop = (offset + len) >> EXT4_BLOCK_SIZE_BITS(sb);
 
+	/* Call ext4_force_commit to flush all data in case of data=journal. */
+	if (ext4_should_journal_data(inode)) {
+		ret = ext4_force_commit(inode->i_sb);
+		if (ret)
+			return ret;
+	}
+
+	/*
+	 * Need to round down offset to be aligned with page size boundary
+	 * for page size > block size.
+	 */
+	ioffset = round_down(offset, PAGE_SIZE);
+
 	/* Write out all dirty pages */
-	ret = filemap_write_and_wait_range(inode->i_mapping, offset, -1);
+	ret = filemap_write_and_wait_range(inode->i_mapping, ioffset,
+					   LLONG_MAX);
 	if (ret)
 		return ret;
 
 	/* Take mutex lock */
 	mutex_lock(&inode->i_mutex);
 
-	/* It's not possible punch hole on append only file */
-	if (IS_APPEND(inode) || IS_IMMUTABLE(inode)) {
-		ret = -EPERM;
-		goto out_mutex;
-	}
-
-	if (IS_SWAPFILE(inode)) {
-		ret = -ETXTBSY;
+	/*
+	 * There is no need to overlap collapse range with EOF, in which case
+	 * it is effectively a truncate operation
+	 */
+	if (offset + len >= i_size_read(inode)) {
+		ret = -EINVAL;
 		goto out_mutex;
 	}
 
@@ -5408,7 +5451,7 @@ int ext4_collapse_range(struct inode *inode, loff_t offset, loff_t len)
 		goto out_mutex;
 	}
 
-	truncate_pagecache_range(inode, offset, -1);
+	truncate_pagecache(inode, ioffset);
 
 	/* Wait for existing dio to complete */
 	ext4_inode_block_unlocked_dio(inode);
@@ -5425,7 +5468,7 @@ int ext4_collapse_range(struct inode *inode, loff_t offset, loff_t len)
 	ext4_discard_preallocations(inode);
 
 	ret = ext4_es_remove_extent(inode, punch_start,
-				    EXT_MAX_BLOCKS - punch_start - 1);
+				    EXT_MAX_BLOCKS - punch_start);
 	if (ret) {
 		up_write(&EXT4_I(inode)->i_data_sem);
 		goto out_stop;
@@ -5436,6 +5479,7 @@ int ext4_collapse_range(struct inode *inode, loff_t offset, loff_t len)
 		up_write(&EXT4_I(inode)->i_data_sem);
 		goto out_stop;
 	}
+	ext4_discard_preallocations(inode);
 
 	ret = ext4_ext_shift_extents(inode, handle, punch_stop,
 				     punch_stop - punch_start);
@@ -5445,10 +5489,9 @@ int ext4_collapse_range(struct inode *inode, loff_t offset, loff_t len)
 	}
 
 	new_size = i_size_read(inode) - len;
-	truncate_setsize(inode, new_size);
+	i_size_write(inode, new_size);
 	EXT4_I(inode)->i_disksize = new_size;
 
-	ext4_discard_preallocations(inode);
 	up_write(&EXT4_I(inode)->i_data_sem);
 	if (IS_SYNC(inode))
 		ext4_handle_sync(handle);
diff --git a/fs/ext4/extents_status.c b/fs/ext4/extents_status.c
index 0a014a7194b..0ebc21204b5 100644
--- a/fs/ext4/extents_status.c
+++ b/fs/ext4/extents_status.c
@@ -810,7 +810,7 @@ retry:
 
 			newes.es_lblk = end + 1;
 			newes.es_len = len2;
-			block = 0x7FDEADBEEF;
+			block = 0x7FDEADBEEFULL;
 			if (ext4_es_is_written(&orig_es) ||
 			    ext4_es_is_unwritten(&orig_es))
 				block = ext4_es_pblock(&orig_es) +
diff --git a/fs/ext4/file.c b/fs/ext4/file.c
index ca7502d89fd..063fc153835 100644
--- a/fs/ext4/file.c
+++ b/fs/ext4/file.c
@@ -82,7 +82,7 @@ ext4_unaligned_aio(struct inode *inode, const struct iovec *iov,
 	size_t count = iov_length(iov, nr_segs);
 	loff_t final_size = pos + count;
 
-	if (pos >= inode->i_size)
+	if (pos >= i_size_read(inode))
 		return 0;
 
 	if ((pos & blockmask) || (final_size & blockmask))
diff --git a/fs/ext4/inode.c b/fs/ext4/inode.c
index 5b0d2c7d540..d7b7462a0e1 100644
--- a/fs/ext4/inode.c
+++ b/fs/ext4/inode.c
@@ -522,6 +522,10 @@ int ext4_map_blocks(handle_t *handle, struct inode *inode,
 	if (unlikely(map->m_len > INT_MAX))
 		map->m_len = INT_MAX;
 
+	/* We can handle the block number less than EXT_MAX_BLOCKS */
+	if (unlikely(map->m_lblk >= EXT_MAX_BLOCKS))
+		return -EIO;
+
 	/* Lookup extent status tree firstly */
 	if (ext4_es_lookup_extent(inode, map->m_lblk, &es)) {
 		ext4_es_lru_add(inode);
@@ -2243,13 +2247,23 @@ static int mpage_map_and_submit_extent(handle_t *handle,
 			return err;
 	} while (map->m_len);
 
-	/* Update on-disk size after IO is submitted */
+	/*
+	 * Update on-disk size after IO is submitted.  Races with
+	 * truncate are avoided by checking i_size under i_data_sem.
+	 */
 	disksize = ((loff_t)mpd->first_page) << PAGE_CACHE_SHIFT;
 	if (disksize > EXT4_I(inode)->i_disksize) {
 		int err2;
-
-		ext4_wb_update_i_disksize(inode, disksize);
+		loff_t i_size;
+
+		down_write(&EXT4_I(inode)->i_data_sem);
+		i_size = i_size_read(inode);
+		if (disksize > i_size)
+			disksize = i_size;
+		if (disksize > EXT4_I(inode)->i_disksize)
+			EXT4_I(inode)->i_disksize = disksize;
 		err2 = ext4_mark_inode_dirty(handle, inode);
+		up_write(&EXT4_I(inode)->i_data_sem);
 		if (err2)
 			ext4_error(inode->i_sb,
 				   "Failed to mark inode %lu dirty",
@@ -3527,15 +3541,6 @@ int ext4_punch_hole(struct inode *inode, loff_t offset, loff_t length)
 	}
 
 	mutex_lock(&inode->i_mutex);
-	/* It's not possible punch hole on append only file */
-	if (IS_APPEND(inode) || IS_IMMUTABLE(inode)) {
-		ret = -EPERM;
-		goto out_mutex;
-	}
-	if (IS_SWAPFILE(inode)) {
-		ret = -ETXTBSY;
-		goto out_mutex;
-	}
 
 	/* No need to punch hole beyond i_size */
 	if (offset >= inode->i_size)
@@ -3616,7 +3621,6 @@ int ext4_punch_hole(struct inode *inode, loff_t offset, loff_t length)
 		ret = ext4_free_hole_blocks(handle, inode, first_block,
 					    stop_block);
 
-	ext4_discard_preallocations(inode);
 	up_write(&EXT4_I(inode)->i_data_sem);
 	if (IS_SYNC(inode))
 		ext4_handle_sync(handle);
@@ -4423,21 +4427,20 @@ out_brelse:
  *
  * We are called from a few places:
  *
- * - Within generic_file_write() for O_SYNC files.
+ * - Within generic_file_aio_write() -> generic_write_sync() for O_SYNC files.
  *   Here, there will be no transaction running. We wait for any running
  *   transaction to commit.
  *
- * - Within sys_sync(), kupdate and such.
- *   We wait on commit, if tol to.
+ * - Within flush work (sys_sync(), kupdate and such).
+ *   We wait on commit, if told to.
  *
- * - Within prune_icache() (PF_MEMALLOC == true)
- *   Here we simply return.  We can't afford to block kswapd on the
- *   journal commit.
+ * - Within iput_final() -> write_inode_now()
+ *   We wait on commit, if told to.
  *
  * In all cases it is actually safe for us to return without doing anything,
  * because the inode has been copied into a raw inode buffer in
- * ext4_mark_inode_dirty().  This is a correctness thing for O_SYNC and for
- * knfsd.
+ * ext4_mark_inode_dirty().  This is a correctness thing for WB_SYNC_ALL
+ * writeback.
  *
  * Note that we are absolutely dependent upon all inode dirtiers doing the
  * right thing: they *must* call mark_inode_dirty() after dirtying info in
@@ -4449,15 +4452,15 @@ out_brelse:
  *	stuff();
  *	inode->i_size = expr;
  *
- * is in error because a kswapd-driven write_inode() could occur while
- * `stuff()' is running, and the new i_size will be lost.  Plus the inode
- * will no longer be on the superblock's dirty inode list.
+ * is in error because write_inode() could occur while `stuff()' is running,
+ * and the new i_size will be lost.  Plus the inode will no longer be on the
+ * superblock's dirty inode list.
  */
 int ext4_write_inode(struct inode *inode, struct writeback_control *wbc)
 {
 	int err;
 
-	if (current->flags & PF_MEMALLOC)
+	if (WARN_ON_ONCE(current->flags & PF_MEMALLOC))
 		return 0;
 
 	if (EXT4_SB(inode->i_sb)->s_journal) {
diff --git a/fs/ext4/mballoc.c b/fs/ext4/mballoc.c
index a888cac76e9..c8238a26818 100644
--- a/fs/ext4/mballoc.c
+++ b/fs/ext4/mballoc.c
@@ -989,7 +989,7 @@ static int ext4_mb_get_buddy_page_lock(struct super_block *sb,
 	poff = block % blocks_per_page;
 	page = find_or_create_page(inode->i_mapping, pnum, GFP_NOFS);
 	if (!page)
-		return -EIO;
+		return -ENOMEM;
 	BUG_ON(page->mapping != inode->i_mapping);
 	e4b->bd_bitmap_page = page;
 	e4b->bd_bitmap = page_address(page) + (poff * sb->s_blocksize);
@@ -1003,7 +1003,7 @@ static int ext4_mb_get_buddy_page_lock(struct super_block *sb,
 	pnum = block / blocks_per_page;
 	page = find_or_create_page(inode->i_mapping, pnum, GFP_NOFS);
 	if (!page)
-		return -EIO;
+		return -ENOMEM;
 	BUG_ON(page->mapping != inode->i_mapping);
 	e4b->bd_buddy_page = page;
 	return 0;
@@ -1168,7 +1168,11 @@ ext4_mb_load_buddy(struct super_block *sb, ext4_group_t group,
 			unlock_page(page);
 		}
 	}
-	if (page == NULL || !PageUptodate(page)) {
+	if (page == NULL) {
+		ret = -ENOMEM;
+		goto err;
+	}
+	if (!PageUptodate(page)) {
 		ret = -EIO;
 		goto err;
 	}
@@ -1197,7 +1201,11 @@ ext4_mb_load_buddy(struct super_block *sb, ext4_group_t group,
 			unlock_page(page);
 		}
 	}
-	if (page == NULL || !PageUptodate(page)) {
+	if (page == NULL) {
+		ret = -ENOMEM;
+		goto err;
+	}
+	if (!PageUptodate(page)) {
 		ret = -EIO;
 		goto err;
 	}
@@ -5008,6 +5016,8 @@ error_return:
  */
 static int ext4_trim_extent(struct super_block *sb, int start, int count,
 			     ext4_group_t group, struct ext4_buddy *e4b)
+__releases(bitlock)
+__acquires(bitlock)
 {
 	struct ext4_free_extent ex;
 	int ret = 0;
diff --git a/fs/ext4/page-io.c b/fs/ext4/page-io.c
index ab95508e3d4..c18d95b5054 100644
--- a/fs/ext4/page-io.c
+++ b/fs/ext4/page-io.c
@@ -308,13 +308,14 @@ static void ext4_end_bio(struct bio *bio, int error)
 	if (error) {
 		struct inode *inode = io_end->inode;
 
-		ext4_warning(inode->i_sb, "I/O error writing to inode %lu "
+		ext4_warning(inode->i_sb, "I/O error %d writing to inode %lu "
 			     "(offset %llu size %ld starting block %llu)",
-			     inode->i_ino,
+			     error, inode->i_ino,
 			     (unsigned long long) io_end->offset,
 			     (long) io_end->size,
 			     (unsigned long long)
 			     bi_sector >> (inode->i_blkbits - 9));
+		mapping_set_error(inode->i_mapping, error);
 	}
 
 	if (io_end->flag & EXT4_IO_END_UNWRITTEN) {
diff --git a/fs/ext4/super.c b/fs/ext4/super.c
index f3c66709161..6f9e6fadac0 100644
--- a/fs/ext4/super.c
+++ b/fs/ext4/super.c
@@ -3869,19 +3869,38 @@ static int ext4_fill_super(struct super_block *sb, void *data, int silent)
 			goto failed_mount2;
 		}
 	}
+
+	/*
+	 * set up enough so that it can read an inode,
+	 * and create new inode for buddy allocator
+	 */
+	sbi->s_gdb_count = db_count;
+	if (!test_opt(sb, NOLOAD) &&
+	    EXT4_HAS_COMPAT_FEATURE(sb, EXT4_FEATURE_COMPAT_HAS_JOURNAL))
+		sb->s_op = &ext4_sops;
+	else
+		sb->s_op = &ext4_nojournal_sops;
+
+	ext4_ext_init(sb);
+	err = ext4_mb_init(sb);
+	if (err) {
+		ext4_msg(sb, KERN_ERR, "failed to initialize mballoc (%d)",
+			 err);
+		goto failed_mount2;
+	}
+
 	if (!ext4_check_descriptors(sb, &first_not_zeroed)) {
 		ext4_msg(sb, KERN_ERR, "group descriptors corrupted!");
-		goto failed_mount2;
+		goto failed_mount2a;
 	}
 	if (EXT4_HAS_INCOMPAT_FEATURE(sb, EXT4_FEATURE_INCOMPAT_FLEX_BG))
 		if (!ext4_fill_flex_info(sb)) {
 			ext4_msg(sb, KERN_ERR,
 			       "unable to initialize "
 			       "flex_bg meta info!");
-			goto failed_mount2;
+			goto failed_mount2a;
 		}
 
-	sbi->s_gdb_count = db_count;
 	get_random_bytes(&sbi->s_next_generation, sizeof(u32));
 	spin_lock_init(&sbi->s_next_gen_lock);
 
@@ -3916,14 +3935,6 @@ static int ext4_fill_super(struct super_block *sb, void *data, int silent)
 	sbi->s_stripe = ext4_get_stripe_size(sbi);
 	sbi->s_extent_max_zeroout_kb = 32;
 
-	/*
-	 * set up enough so that it can read an inode
-	 */
-	if (!test_opt(sb, NOLOAD) &&
-	    EXT4_HAS_COMPAT_FEATURE(sb, EXT4_FEATURE_COMPAT_HAS_JOURNAL))
-		sb->s_op = &ext4_sops;
-	else
-		sb->s_op = &ext4_nojournal_sops;
 	sb->s_export_op = &ext4_export_ops;
 	sb->s_xattr = ext4_xattr_handlers;
 #ifdef CONFIG_QUOTA
@@ -4113,21 +4124,13 @@ no_journal:
 	if (err) {
 		ext4_msg(sb, KERN_ERR, "failed to reserve %llu clusters for "
 			 "reserved pool", ext4_calculate_resv_clusters(sb));
-		goto failed_mount4a;
+		goto failed_mount5;
 	}
 
 	err = ext4_setup_system_zone(sb);
 	if (err) {
 		ext4_msg(sb, KERN_ERR, "failed to initialize system "
 			 "zone (%d)", err);
-		goto failed_mount4a;
-	}
-
-	ext4_ext_init(sb);
-	err = ext4_mb_init(sb);
-	if (err) {
-		ext4_msg(sb, KERN_ERR, "failed to initialize mballoc (%d)",
-			 err);
 		goto failed_mount5;
 	}
 
@@ -4204,11 +4207,8 @@ failed_mount8:
 failed_mount7:
 	ext4_unregister_li_request(sb);
 failed_mount6:
-	ext4_mb_release(sb);
-failed_mount5:
-	ext4_ext_release(sb);
 	ext4_release_system_zone(sb);
-failed_mount4a:
+failed_mount5:
 	dput(sb->s_root);
 	sb->s_root = NULL;
 failed_mount4:
@@ -4232,11 +4232,14 @@ failed_mount3:
 	percpu_counter_destroy(&sbi->s_extent_cache_cnt);
 	if (sbi->s_mmp_tsk)
 		kthread_stop(sbi->s_mmp_tsk);
+failed_mount2a:
+	ext4_mb_release(sb);
 failed_mount2:
 	for (i = 0; i < db_count; i++)
 		brelse(sbi->s_group_desc[i]);
 	ext4_kvfree(sbi->s_group_desc);
 failed_mount:
+	ext4_ext_release(sb);
 	if (sbi->s_chksum_driver)
 		crypto_free_shash(sbi->s_chksum_driver);
 	if (sbi->s_proc) {
diff --git a/fs/ext4/xattr.c b/fs/ext4/xattr.c
index 1f5cf588071..4eec399ec80 100644
--- a/fs/ext4/xattr.c
+++ b/fs/ext4/xattr.c
@@ -520,8 +520,8 @@ static void ext4_xattr_update_super_block(handle_t *handle,
 }
 
 /*
- * Release the xattr block BH: If the reference count is > 1, decrement
- * it; otherwise free the block.
+ * Release the xattr block BH: If the reference count is > 1, decrement it;
+ * otherwise free the block.
  */
 static void
 ext4_xattr_release_block(handle_t *handle, struct inode *inode,
@@ -542,16 +542,31 @@ ext4_xattr_release_block(handle_t *handle, struct inode *inode,
 		if (ce)
 			mb_cache_entry_free(ce);
 		get_bh(bh);
+		unlock_buffer(bh);
 		ext4_free_blocks(handle, inode, bh, 0, 1,
 				 EXT4_FREE_BLOCKS_METADATA |
 				 EXT4_FREE_BLOCKS_FORGET);
-		unlock_buffer(bh);
 	} else {
 		le32_add_cpu(&BHDR(bh)->h_refcount, -1);
 		if (ce)
 			mb_cache_entry_release(ce);
+		/*
+		 * Beware of this ugliness: Releasing of xattr block references
+		 * from different inodes can race and so we have to protect
+		 * from a race where someone else frees the block (and releases
+		 * its journal_head) before we are done dirtying the buffer. In
+		 * nojournal mode this race is harmless and we actually cannot
+		 * call ext4_handle_dirty_xattr_block() with locked buffer as
+		 * that function can call sync_dirty_buffer() so for that case
+		 * we handle the dirtying after unlocking the buffer.
+		 */
+		if (ext4_handle_valid(handle))
+			error = ext4_handle_dirty_xattr_block(handle, inode,
+							      bh);
 		unlock_buffer(bh);
-		error = ext4_handle_dirty_xattr_block(handle, inode, bh);
+		if (!ext4_handle_valid(handle))
+			error = ext4_handle_dirty_xattr_block(handle, inode,
+							      bh);
 		if (IS_SYNC(inode))
 			ext4_handle_sync(handle);
 		dquot_free_block(inode, EXT4_C2B(EXT4_SB(inode->i_sb), 1));
diff --git a/fs/fcntl.c b/fs/fcntl.c
index 9ead1596399..72c82f69b01 100644
--- a/fs/fcntl.c
+++ b/fs/fcntl.c
@@ -274,15 +274,15 @@ static long do_fcntl(int fd, unsigned int cmd, unsigned long arg,
 		break;
 #if BITS_PER_LONG != 32
 	/* 32-bit arches must use fcntl64() */
-	case F_GETLKP:
+	case F_OFD_GETLK:
 #endif
 	case F_GETLK:
 		err = fcntl_getlk(filp, cmd, (struct flock __user *) arg);
 		break;
 #if BITS_PER_LONG != 32
 	/* 32-bit arches must use fcntl64() */
-	case F_SETLKP:
-	case F_SETLKPW:
+	case F_OFD_SETLK:
+	case F_OFD_SETLKW:
 #endif
 		/* Fallthrough */
 	case F_SETLK:
@@ -399,13 +399,13 @@ SYSCALL_DEFINE3(fcntl64, unsigned int, fd, unsigned int, cmd,
 	
 	switch (cmd) {
 	case F_GETLK64:
-	case F_GETLKP:
+	case F_OFD_GETLK:
 		err = fcntl_getlk64(f.file, cmd, (struct flock64 __user *) arg);
 		break;
 	case F_SETLK64:
 	case F_SETLKW64:
-	case F_SETLKP:
-	case F_SETLKPW:
+	case F_OFD_SETLK:
+	case F_OFD_SETLKW:
 		err = fcntl_setlk64(fd, f.file, cmd,
 				(struct flock64 __user *) arg);
 		break;
diff --git a/fs/fuse/control.c b/fs/fuse/control.c
index a0b0855d00a..205e0d5d530 100644
--- a/fs/fuse/control.c
+++ b/fs/fuse/control.c
@@ -348,7 +348,7 @@ int __init fuse_ctl_init(void)
 	return register_filesystem(&fuse_ctl_fs_type);
 }
 
-void fuse_ctl_cleanup(void)
+void __exit fuse_ctl_cleanup(void)
 {
 	unregister_filesystem(&fuse_ctl_fs_type);
 }
diff --git a/fs/fuse/dir.c b/fs/fuse/dir.c
index 5b4e035b364..42198359fa1 100644
--- a/fs/fuse/dir.c
+++ b/fs/fuse/dir.c
@@ -679,6 +679,14 @@ static int fuse_symlink(struct inode *dir, struct dentry *entry,
 	return create_new_entry(fc, req, dir, entry, S_IFLNK);
 }
 
+static inline void fuse_update_ctime(struct inode *inode)
+{
+	if (!IS_NOCMTIME(inode)) {
+		inode->i_ctime = current_fs_time(inode->i_sb);
+		mark_inode_dirty_sync(inode);
+	}
+}
+
 static int fuse_unlink(struct inode *dir, struct dentry *entry)
 {
 	int err;
@@ -713,6 +721,7 @@ static int fuse_unlink(struct inode *dir, struct dentry *entry)
 		fuse_invalidate_attr(inode);
 		fuse_invalidate_attr(dir);
 		fuse_invalidate_entry_cache(entry);
+		fuse_update_ctime(inode);
 	} else if (err == -EINTR)
 		fuse_invalidate_entry(entry);
 	return err;
@@ -743,23 +752,26 @@ static int fuse_rmdir(struct inode *dir, struct dentry *entry)
 	return err;
 }
 
-static int fuse_rename(struct inode *olddir, struct dentry *oldent,
-		       struct inode *newdir, struct dentry *newent)
+static int fuse_rename_common(struct inode *olddir, struct dentry *oldent,
+			      struct inode *newdir, struct dentry *newent,
+			      unsigned int flags, int opcode, size_t argsize)
 {
 	int err;
-	struct fuse_rename_in inarg;
+	struct fuse_rename2_in inarg;
 	struct fuse_conn *fc = get_fuse_conn(olddir);
-	struct fuse_req *req = fuse_get_req_nopages(fc);
+	struct fuse_req *req;
 
+	req = fuse_get_req_nopages(fc);
 	if (IS_ERR(req))
 		return PTR_ERR(req);
 
-	memset(&inarg, 0, sizeof(inarg));
+	memset(&inarg, 0, argsize);
 	inarg.newdir = get_node_id(newdir);
-	req->in.h.opcode = FUSE_RENAME;
+	inarg.flags = flags;
+	req->in.h.opcode = opcode;
 	req->in.h.nodeid = get_node_id(olddir);
 	req->in.numargs = 3;
-	req->in.args[0].size = sizeof(inarg);
+	req->in.args[0].size = argsize;
 	req->in.args[0].value = &inarg;
 	req->in.args[1].size = oldent->d_name.len + 1;
 	req->in.args[1].value = oldent->d_name.name;
@@ -771,15 +783,22 @@ static int fuse_rename(struct inode *olddir, struct dentry *oldent,
 	if (!err) {
 		/* ctime changes */
 		fuse_invalidate_attr(oldent->d_inode);
+		fuse_update_ctime(oldent->d_inode);
+
+		if (flags & RENAME_EXCHANGE) {
+			fuse_invalidate_attr(newent->d_inode);
+			fuse_update_ctime(newent->d_inode);
+		}
 
 		fuse_invalidate_attr(olddir);
 		if (olddir != newdir)
 			fuse_invalidate_attr(newdir);
 
 		/* newent will end up negative */
-		if (newent->d_inode) {
+		if (!(flags & RENAME_EXCHANGE) && newent->d_inode) {
 			fuse_invalidate_attr(newent->d_inode);
 			fuse_invalidate_entry_cache(newent);
+			fuse_update_ctime(newent->d_inode);
 		}
 	} else if (err == -EINTR) {
 		/* If request was interrupted, DEITY only knows if the
@@ -795,6 +814,36 @@ static int fuse_rename(struct inode *olddir, struct dentry *oldent,
 	return err;
 }
 
+static int fuse_rename(struct inode *olddir, struct dentry *oldent,
+		       struct inode *newdir, struct dentry *newent)
+{
+	return fuse_rename_common(olddir, oldent, newdir, newent, 0,
+				  FUSE_RENAME, sizeof(struct fuse_rename_in));
+}
+
+static int fuse_rename2(struct inode *olddir, struct dentry *oldent,
+			struct inode *newdir, struct dentry *newent,
+			unsigned int flags)
+{
+	struct fuse_conn *fc = get_fuse_conn(olddir);
+	int err;
+
+	if (flags & ~(RENAME_NOREPLACE | RENAME_EXCHANGE))
+		return -EINVAL;
+
+	if (fc->no_rename2 || fc->minor < 23)
+		return -EINVAL;
+
+	err = fuse_rename_common(olddir, oldent, newdir, newent, flags,
+				 FUSE_RENAME2, sizeof(struct fuse_rename2_in));
+	if (err == -ENOSYS) {
+		fc->no_rename2 = 1;
+		err = -EINVAL;
+	}
+	return err;
+
+}
+
 static int fuse_link(struct dentry *entry, struct inode *newdir,
 		     struct dentry *newent)
 {
@@ -829,6 +878,7 @@ static int fuse_link(struct dentry *entry, struct inode *newdir,
 		inc_nlink(inode);
 		spin_unlock(&fc->lock);
 		fuse_invalidate_attr(inode);
+		fuse_update_ctime(inode);
 	} else if (err == -EINTR) {
 		fuse_invalidate_attr(inode);
 	}
@@ -846,6 +896,8 @@ static void fuse_fillattr(struct inode *inode, struct fuse_attr *attr,
 		attr->size = i_size_read(inode);
 		attr->mtime = inode->i_mtime.tv_sec;
 		attr->mtimensec = inode->i_mtime.tv_nsec;
+		attr->ctime = inode->i_ctime.tv_sec;
+		attr->ctimensec = inode->i_ctime.tv_nsec;
 	}
 
 	stat->dev = inode->i_sb->s_dev;
@@ -1504,7 +1556,7 @@ static bool update_mtime(unsigned ivalid, bool trust_local_mtime)
 }
 
 static void iattr_to_fattr(struct iattr *iattr, struct fuse_setattr_in *arg,
-			   bool trust_local_mtime)
+			   bool trust_local_cmtime)
 {
 	unsigned ivalid = iattr->ia_valid;
 
@@ -1523,13 +1575,18 @@ static void iattr_to_fattr(struct iattr *iattr, struct fuse_setattr_in *arg,
 		if (!(ivalid & ATTR_ATIME_SET))
 			arg->valid |= FATTR_ATIME_NOW;
 	}
-	if ((ivalid & ATTR_MTIME) && update_mtime(ivalid, trust_local_mtime)) {
+	if ((ivalid & ATTR_MTIME) && update_mtime(ivalid, trust_local_cmtime)) {
 		arg->valid |= FATTR_MTIME;
 		arg->mtime = iattr->ia_mtime.tv_sec;
 		arg->mtimensec = iattr->ia_mtime.tv_nsec;
-		if (!(ivalid & ATTR_MTIME_SET) && !trust_local_mtime)
+		if (!(ivalid & ATTR_MTIME_SET) && !trust_local_cmtime)
 			arg->valid |= FATTR_MTIME_NOW;
 	}
+	if ((ivalid & ATTR_CTIME) && trust_local_cmtime) {
+		arg->valid |= FATTR_CTIME;
+		arg->ctime = iattr->ia_ctime.tv_sec;
+		arg->ctimensec = iattr->ia_ctime.tv_nsec;
+	}
 }
 
 /*
@@ -1597,39 +1654,38 @@ static void fuse_setattr_fill(struct fuse_conn *fc, struct fuse_req *req,
 /*
  * Flush inode->i_mtime to the server
  */
-int fuse_flush_mtime(struct file *file, bool nofail)
+int fuse_flush_times(struct inode *inode, struct fuse_file *ff)
 {
-	struct inode *inode = file->f_mapping->host;
-	struct fuse_inode *fi = get_fuse_inode(inode);
 	struct fuse_conn *fc = get_fuse_conn(inode);
-	struct fuse_req *req = NULL;
+	struct fuse_req *req;
 	struct fuse_setattr_in inarg;
 	struct fuse_attr_out outarg;
 	int err;
 
-	if (nofail) {
-		req = fuse_get_req_nofail_nopages(fc, file);
-	} else {
-		req = fuse_get_req_nopages(fc);
-		if (IS_ERR(req))
-			return PTR_ERR(req);
-	}
+	req = fuse_get_req_nopages(fc);
+	if (IS_ERR(req))
+		return PTR_ERR(req);
 
 	memset(&inarg, 0, sizeof(inarg));
 	memset(&outarg, 0, sizeof(outarg));
 
-	inarg.valid |= FATTR_MTIME;
+	inarg.valid = FATTR_MTIME;
 	inarg.mtime = inode->i_mtime.tv_sec;
 	inarg.mtimensec = inode->i_mtime.tv_nsec;
-
+	if (fc->minor >= 23) {
+		inarg.valid |= FATTR_CTIME;
+		inarg.ctime = inode->i_ctime.tv_sec;
+		inarg.ctimensec = inode->i_ctime.tv_nsec;
+	}
+	if (ff) {
+		inarg.valid |= FATTR_FH;
+		inarg.fh = ff->fh;
+	}
 	fuse_setattr_fill(fc, req, inode, &inarg, &outarg);
 	fuse_request_send(fc, req);
 	err = req->out.h.error;
 	fuse_put_request(fc, req);
 
-	if (!err)
-		clear_bit(FUSE_I_MTIME_DIRTY, &fi->state);
-
 	return err;
 }
 
@@ -1653,7 +1709,7 @@ int fuse_do_setattr(struct inode *inode, struct iattr *attr,
 	bool is_wb = fc->writeback_cache;
 	loff_t oldsize;
 	int err;
-	bool trust_local_mtime = is_wb && S_ISREG(inode->i_mode);
+	bool trust_local_cmtime = is_wb && S_ISREG(inode->i_mode);
 
 	if (!(fc->flags & FUSE_DEFAULT_PERMISSIONS))
 		attr->ia_valid |= ATTR_FORCE;
@@ -1678,11 +1734,13 @@ int fuse_do_setattr(struct inode *inode, struct iattr *attr,
 	if (is_truncate) {
 		fuse_set_nowrite(inode);
 		set_bit(FUSE_I_SIZE_UNSTABLE, &fi->state);
+		if (trust_local_cmtime && attr->ia_size != inode->i_size)
+			attr->ia_valid |= ATTR_MTIME | ATTR_CTIME;
 	}
 
 	memset(&inarg, 0, sizeof(inarg));
 	memset(&outarg, 0, sizeof(outarg));
-	iattr_to_fattr(attr, &inarg, trust_local_mtime);
+	iattr_to_fattr(attr, &inarg, trust_local_cmtime);
 	if (file) {
 		struct fuse_file *ff = file->private_data;
 		inarg.valid |= FATTR_FH;
@@ -1711,9 +1769,12 @@ int fuse_do_setattr(struct inode *inode, struct iattr *attr,
 
 	spin_lock(&fc->lock);
 	/* the kernel maintains i_mtime locally */
-	if (trust_local_mtime && (attr->ia_valid & ATTR_MTIME)) {
-		inode->i_mtime = attr->ia_mtime;
-		clear_bit(FUSE_I_MTIME_DIRTY, &fi->state);
+	if (trust_local_cmtime) {
+		if (attr->ia_valid & ATTR_MTIME)
+			inode->i_mtime = attr->ia_mtime;
+		if (attr->ia_valid & ATTR_CTIME)
+			inode->i_ctime = attr->ia_ctime;
+		/* FIXME: clear I_DIRTY_SYNC? */
 	}
 
 	fuse_change_attributes_common(inode, &outarg.attr,
@@ -1810,8 +1871,10 @@ static int fuse_setxattr(struct dentry *entry, const char *name,
 		fc->no_setxattr = 1;
 		err = -EOPNOTSUPP;
 	}
-	if (!err)
+	if (!err) {
 		fuse_invalidate_attr(inode);
+		fuse_update_ctime(inode);
+	}
 	return err;
 }
 
@@ -1941,20 +2004,11 @@ static int fuse_removexattr(struct dentry *entry, const char *name)
 		fc->no_removexattr = 1;
 		err = -EOPNOTSUPP;
 	}
-	if (!err)
+	if (!err) {
 		fuse_invalidate_attr(inode);
-	return err;
-}
-
-static int fuse_update_time(struct inode *inode, struct timespec *now,
-			    int flags)
-{
-	if (flags & S_MTIME) {
-		inode->i_mtime = *now;
-		set_bit(FUSE_I_MTIME_DIRTY, &get_fuse_inode(inode)->state);
-		BUG_ON(!S_ISREG(inode->i_mode));
+		fuse_update_ctime(inode);
 	}
-	return 0;
+	return err;
 }
 
 static const struct inode_operations fuse_dir_inode_operations = {
@@ -1964,6 +2018,7 @@ static const struct inode_operations fuse_dir_inode_operations = {
 	.unlink		= fuse_unlink,
 	.rmdir		= fuse_rmdir,
 	.rename		= fuse_rename,
+	.rename2	= fuse_rename2,
 	.link		= fuse_link,
 	.setattr	= fuse_setattr,
 	.create		= fuse_create,
@@ -1996,7 +2051,6 @@ static const struct inode_operations fuse_common_inode_operations = {
 	.getxattr	= fuse_getxattr,
 	.listxattr	= fuse_listxattr,
 	.removexattr	= fuse_removexattr,
-	.update_time	= fuse_update_time,
 };
 
 static const struct inode_operations fuse_symlink_inode_operations = {
diff --git a/fs/fuse/file.c b/fs/fuse/file.c
index 13f8bdec511..96d513e01a5 100644
--- a/fs/fuse/file.c
+++ b/fs/fuse/file.c
@@ -223,6 +223,8 @@ void fuse_finish_open(struct inode *inode, struct file *file)
 		i_size_write(inode, 0);
 		spin_unlock(&fc->lock);
 		fuse_invalidate_attr(inode);
+		if (fc->writeback_cache)
+			file_update_time(file);
 	}
 	if ((file->f_mode & FMODE_WRITE) && fc->writeback_cache)
 		fuse_link_write_file(file);
@@ -232,18 +234,26 @@ int fuse_open_common(struct inode *inode, struct file *file, bool isdir)
 {
 	struct fuse_conn *fc = get_fuse_conn(inode);
 	int err;
+	bool lock_inode = (file->f_flags & O_TRUNC) &&
+			  fc->atomic_o_trunc &&
+			  fc->writeback_cache;
 
 	err = generic_file_open(inode, file);
 	if (err)
 		return err;
 
+	if (lock_inode)
+		mutex_lock(&inode->i_mutex);
+
 	err = fuse_do_open(fc, get_node_id(inode), file, isdir);
-	if (err)
-		return err;
 
-	fuse_finish_open(inode, file);
+	if (!err)
+		fuse_finish_open(inode, file);
 
-	return 0;
+	if (lock_inode)
+		mutex_unlock(&inode->i_mutex);
+
+	return err;
 }
 
 static void fuse_prepare_release(struct fuse_file *ff, int flags, int opcode)
@@ -314,10 +324,7 @@ static int fuse_release(struct inode *inode, struct file *file)
 
 	/* see fuse_vma_close() for !writeback_cache case */
 	if (fc->writeback_cache)
-		filemap_write_and_wait(file->f_mapping);
-
-	if (test_bit(FUSE_I_MTIME_DIRTY, &get_fuse_inode(inode)->state))
-		fuse_flush_mtime(file, true);
+		write_inode_now(inode, 1);
 
 	fuse_release_common(file, FUSE_RELEASE);
 
@@ -439,7 +446,7 @@ static int fuse_flush(struct file *file, fl_owner_t id)
 	if (fc->no_flush)
 		return 0;
 
-	err = filemap_write_and_wait(file->f_mapping);
+	err = write_inode_now(inode, 1);
 	if (err)
 		return err;
 
@@ -480,13 +487,6 @@ int fuse_fsync_common(struct file *file, loff_t start, loff_t end,
 	if (is_bad_inode(inode))
 		return -EIO;
 
-	err = filemap_write_and_wait_range(inode->i_mapping, start, end);
-	if (err)
-		return err;
-
-	if ((!isdir && fc->no_fsync) || (isdir && fc->no_fsyncdir))
-		return 0;
-
 	mutex_lock(&inode->i_mutex);
 
 	/*
@@ -494,17 +494,17 @@ int fuse_fsync_common(struct file *file, loff_t start, loff_t end,
 	 * wait for all outstanding writes, before sending the FSYNC
 	 * request.
 	 */
-	err = write_inode_now(inode, 0);
+	err = filemap_write_and_wait_range(inode->i_mapping, start, end);
 	if (err)
 		goto out;
 
 	fuse_sync_writes(inode);
+	err = sync_inode_metadata(inode, 1);
+	if (err)
+		goto out;
 
-	if (test_bit(FUSE_I_MTIME_DIRTY, &get_fuse_inode(inode)->state)) {
-		int err = fuse_flush_mtime(file, false);
-		if (err)
-			goto out;
-	}
+	if ((!isdir && fc->no_fsync) || (isdir && fc->no_fsyncdir))
+		goto out;
 
 	req = fuse_get_req_nopages(fc);
 	if (IS_ERR(req)) {
@@ -1659,13 +1659,13 @@ static void fuse_writepage_end(struct fuse_conn *fc, struct fuse_req *req)
 	fuse_writepage_free(fc, req);
 }
 
-static struct fuse_file *fuse_write_file_get(struct fuse_conn *fc,
-					     struct fuse_inode *fi)
+static struct fuse_file *__fuse_write_file_get(struct fuse_conn *fc,
+					       struct fuse_inode *fi)
 {
 	struct fuse_file *ff = NULL;
 
 	spin_lock(&fc->lock);
-	if (!WARN_ON(list_empty(&fi->write_files))) {
+	if (!list_empty(&fi->write_files)) {
 		ff = list_entry(fi->write_files.next, struct fuse_file,
 				write_entry);
 		fuse_file_get(ff);
@@ -1675,6 +1675,29 @@ static struct fuse_file *fuse_write_file_get(struct fuse_conn *fc,
 	return ff;
 }
 
+static struct fuse_file *fuse_write_file_get(struct fuse_conn *fc,
+					     struct fuse_inode *fi)
+{
+	struct fuse_file *ff = __fuse_write_file_get(fc, fi);
+	WARN_ON(!ff);
+	return ff;
+}
+
+int fuse_write_inode(struct inode *inode, struct writeback_control *wbc)
+{
+	struct fuse_conn *fc = get_fuse_conn(inode);
+	struct fuse_inode *fi = get_fuse_inode(inode);
+	struct fuse_file *ff;
+	int err;
+
+	ff = __fuse_write_file_get(fc, fi);
+	err = fuse_flush_times(inode, ff);
+	if (ff)
+		fuse_file_put(ff, 0);
+
+	return err;
+}
+
 static int fuse_writepage_locked(struct page *page)
 {
 	struct address_space *mapping = page->mapping;
@@ -2972,6 +2995,9 @@ static long fuse_file_fallocate(struct file *file, int mode, loff_t offset,
 	bool lock_inode = !(mode & FALLOC_FL_KEEP_SIZE) ||
 			   (mode & FALLOC_FL_PUNCH_HOLE);
 
+	if (mode & ~(FALLOC_FL_KEEP_SIZE | FALLOC_FL_PUNCH_HOLE))
+		return -EOPNOTSUPP;
+
 	if (fc->no_fallocate)
 		return -EOPNOTSUPP;
 
@@ -3017,12 +3043,8 @@ static long fuse_file_fallocate(struct file *file, int mode, loff_t offset,
 	if (!(mode & FALLOC_FL_KEEP_SIZE)) {
 		bool changed = fuse_write_update_size(inode, offset + length);
 
-		if (changed && fc->writeback_cache) {
-			struct fuse_inode *fi = get_fuse_inode(inode);
-
-			inode->i_mtime = current_fs_time(inode->i_sb);
-			set_bit(FUSE_I_MTIME_DIRTY, &fi->state);
-		}
+		if (changed && fc->writeback_cache)
+			file_update_time(file);
 	}
 
 	if (mode & FALLOC_FL_PUNCH_HOLE)
diff --git a/fs/fuse/fuse_i.h b/fs/fuse/fuse_i.h
index a257ed8ebee..7aa5c75e0de 100644
--- a/fs/fuse/fuse_i.h
+++ b/fs/fuse/fuse_i.h
@@ -119,8 +119,6 @@ enum {
 	FUSE_I_INIT_RDPLUS,
 	/** An operation changing file size is in progress  */
 	FUSE_I_SIZE_UNSTABLE,
-	/** i_mtime has been updated locally; a flush to userspace needed */
-	FUSE_I_MTIME_DIRTY,
 };
 
 struct fuse_conn;
@@ -544,6 +542,9 @@ struct fuse_conn {
 	/** Is fallocate not implemented by fs? */
 	unsigned no_fallocate:1;
 
+	/** Is rename with flags implemented by fs? */
+	unsigned no_rename2:1;
+
 	/** Use enhanced/automatic page cache invalidation. */
 	unsigned auto_inval_data:1;
 
@@ -725,7 +726,7 @@ int fuse_dev_init(void);
 void fuse_dev_cleanup(void);
 
 int fuse_ctl_init(void);
-void fuse_ctl_cleanup(void);
+void __exit fuse_ctl_cleanup(void);
 
 /**
  * Allocate a request
@@ -891,7 +892,8 @@ int fuse_dev_release(struct inode *inode, struct file *file);
 
 bool fuse_write_update_size(struct inode *inode, loff_t pos);
 
-int fuse_flush_mtime(struct file *file, bool nofail);
+int fuse_flush_times(struct inode *inode, struct fuse_file *ff);
+int fuse_write_inode(struct inode *inode, struct writeback_control *wbc);
 
 int fuse_do_setattr(struct inode *inode, struct iattr *attr,
 		    struct file *file);
diff --git a/fs/fuse/inode.c b/fs/fuse/inode.c
index 8d611696fca..754dcf23de8 100644
--- a/fs/fuse/inode.c
+++ b/fs/fuse/inode.c
@@ -175,9 +175,9 @@ void fuse_change_attributes_common(struct inode *inode, struct fuse_attr *attr,
 	if (!fc->writeback_cache || !S_ISREG(inode->i_mode)) {
 		inode->i_mtime.tv_sec   = attr->mtime;
 		inode->i_mtime.tv_nsec  = attr->mtimensec;
+		inode->i_ctime.tv_sec   = attr->ctime;
+		inode->i_ctime.tv_nsec  = attr->ctimensec;
 	}
-	inode->i_ctime.tv_sec   = attr->ctime;
-	inode->i_ctime.tv_nsec  = attr->ctimensec;
 
 	if (attr->blksize != 0)
 		inode->i_blkbits = ilog2(attr->blksize);
@@ -256,6 +256,8 @@ static void fuse_init_inode(struct inode *inode, struct fuse_attr *attr)
 	inode->i_size = attr->size;
 	inode->i_mtime.tv_sec  = attr->mtime;
 	inode->i_mtime.tv_nsec = attr->mtimensec;
+	inode->i_ctime.tv_sec  = attr->ctime;
+	inode->i_ctime.tv_nsec = attr->ctimensec;
 	if (S_ISREG(inode->i_mode)) {
 		fuse_init_common(inode);
 		fuse_init_file_inode(inode);
@@ -303,7 +305,7 @@ struct inode *fuse_iget(struct super_block *sb, u64 nodeid,
 
 	if ((inode->i_state & I_NEW)) {
 		inode->i_flags |= S_NOATIME;
-		if (!fc->writeback_cache || !S_ISREG(inode->i_mode))
+		if (!fc->writeback_cache || !S_ISREG(attr->mode))
 			inode->i_flags |= S_NOCMTIME;
 		inode->i_generation = generation;
 		inode->i_data.backing_dev_info = &fc->bdi;
@@ -788,6 +790,7 @@ static const struct super_operations fuse_super_operations = {
 	.alloc_inode    = fuse_alloc_inode,
 	.destroy_inode  = fuse_destroy_inode,
 	.evict_inode	= fuse_evict_inode,
+	.write_inode	= fuse_write_inode,
 	.drop_inode	= generic_delete_inode,
 	.remount_fs	= fuse_remount_fs,
 	.put_super	= fuse_put_super,
@@ -890,6 +893,11 @@ static void process_init_reply(struct fuse_conn *fc, struct fuse_req *req)
 				fc->async_dio = 1;
 			if (arg->flags & FUSE_WRITEBACK_CACHE)
 				fc->writeback_cache = 1;
+			if (arg->time_gran && arg->time_gran <= 1000000000)
+				fc->sb->s_time_gran = arg->time_gran;
+			else
+				fc->sb->s_time_gran = 1000000000;
+
 		} else {
 			ra_pages = fc->max_read / PAGE_CACHE_SIZE;
 			fc->no_lock = 1;
@@ -996,7 +1004,7 @@ static int fuse_fill_super(struct super_block *sb, void *data, int silent)
 	if (sb->s_flags & MS_MANDLOCK)
 		goto err;
 
-	sb->s_flags &= ~MS_NOSEC;
+	sb->s_flags &= ~(MS_NOSEC | MS_I_VERSION);
 
 	if (!parse_fuse_opt((char *) data, &d, is_bdev))
 		goto err;
diff --git a/fs/hugetlbfs/inode.c b/fs/hugetlbfs/inode.c
index 20402752093..e19d4c0caca 100644
--- a/fs/hugetlbfs/inode.c
+++ b/fs/hugetlbfs/inode.c
@@ -1030,6 +1030,11 @@ static int __init init_hugetlbfs_fs(void)
 	int error;
 	int i;
 
+	if (!hugepages_supported()) {
+		pr_info("hugetlbfs: disabling because there are no supported hugepage sizes\n");
+		return -ENOTSUPP;
+	}
+
 	error = bdi_init(&hugetlbfs_backing_dev_info);
 	if (error)
 		return error;
diff --git a/fs/kernfs/dir.c b/fs/kernfs/dir.c
index 78f3403300a..a693f5b01ae 100644
--- a/fs/kernfs/dir.c
+++ b/fs/kernfs/dir.c
@@ -232,9 +232,6 @@ static int kernfs_link_sibling(struct kernfs_node *kn)
 	struct rb_node **node = &kn->parent->dir.children.rb_node;
 	struct rb_node *parent = NULL;
 
-	if (kernfs_type(kn) == KERNFS_DIR)
-		kn->parent->dir.subdirs++;
-
 	while (*node) {
 		struct kernfs_node *pos;
 		int result;
@@ -249,9 +246,15 @@ static int kernfs_link_sibling(struct kernfs_node *kn)
 		else
 			return -EEXIST;
 	}
+
 	/* add new node and rebalance the tree */
 	rb_link_node(&kn->rb, parent, node);
 	rb_insert_color(&kn->rb, &kn->parent->dir.children);
+
+	/* successfully added, account subdir number */
+	if (kernfs_type(kn) == KERNFS_DIR)
+		kn->parent->dir.subdirs++;
+
 	return 0;
 }
 
@@ -711,6 +714,7 @@ struct kernfs_root *kernfs_create_root(struct kernfs_syscall_ops *scops,
 		return ERR_PTR(-ENOMEM);
 
 	ida_init(&root->ino_ida);
+	INIT_LIST_HEAD(&root->supers);
 
 	kn = __kernfs_new_node(root, "", S_IFDIR | S_IRUGO | S_IXUGO,
 			       KERNFS_DIR);
diff --git a/fs/kernfs/file.c b/fs/kernfs/file.c
index 8034706a7af..e3d37f607f9 100644
--- a/fs/kernfs/file.c
+++ b/fs/kernfs/file.c
@@ -14,6 +14,7 @@
 #include <linux/poll.h>
 #include <linux/pagemap.h>
 #include <linux/sched.h>
+#include <linux/fsnotify.h>
 
 #include "kernfs-internal.h"
 
@@ -484,6 +485,8 @@ static int kernfs_fop_mmap(struct file *file, struct vm_area_struct *vma)
 
 	ops = kernfs_ops(of->kn);
 	rc = ops->mmap(of, vma);
+	if (rc)
+		goto out_put;
 
 	/*
 	 * PowerPC's pci_mmap of legacy_mem uses shmem_zero_setup()
@@ -608,6 +611,7 @@ static void kernfs_put_open_node(struct kernfs_node *kn,
 static int kernfs_fop_open(struct inode *inode, struct file *file)
 {
 	struct kernfs_node *kn = file->f_path.dentry->d_fsdata;
+	struct kernfs_root *root = kernfs_root(kn);
 	const struct kernfs_ops *ops;
 	struct kernfs_open_file *of;
 	bool has_read, has_write, has_mmap;
@@ -622,14 +626,16 @@ static int kernfs_fop_open(struct inode *inode, struct file *file)
 	has_write = ops->write || ops->mmap;
 	has_mmap = ops->mmap;
 
-	/* check perms and supported operations */
-	if ((file->f_mode & FMODE_WRITE) &&
-	    (!(inode->i_mode & S_IWUGO) || !has_write))
-		goto err_out;
+	/* see the flag definition for details */
+	if (root->flags & KERNFS_ROOT_EXTRA_OPEN_PERM_CHECK) {
+		if ((file->f_mode & FMODE_WRITE) &&
+		    (!(inode->i_mode & S_IWUGO) || !has_write))
+			goto err_out;
 
-	if ((file->f_mode & FMODE_READ) &&
-	    (!(inode->i_mode & S_IRUGO) || !has_read))
-		goto err_out;
+		if ((file->f_mode & FMODE_READ) &&
+		    (!(inode->i_mode & S_IRUGO) || !has_read))
+			goto err_out;
+	}
 
 	/* allocate a kernfs_open_file for the file */
 	error = -ENOMEM;
@@ -785,20 +791,48 @@ static unsigned int kernfs_fop_poll(struct file *filp, poll_table *wait)
  */
 void kernfs_notify(struct kernfs_node *kn)
 {
+	struct kernfs_root *root = kernfs_root(kn);
 	struct kernfs_open_node *on;
+	struct kernfs_super_info *info;
 	unsigned long flags;
 
+	if (WARN_ON(kernfs_type(kn) != KERNFS_FILE))
+		return;
+
+	/* kick poll */
 	spin_lock_irqsave(&kernfs_open_node_lock, flags);
 
-	if (!WARN_ON(kernfs_type(kn) != KERNFS_FILE)) {
-		on = kn->attr.open;
-		if (on) {
-			atomic_inc(&on->event);
-			wake_up_interruptible(&on->poll);
-		}
+	on = kn->attr.open;
+	if (on) {
+		atomic_inc(&on->event);
+		wake_up_interruptible(&on->poll);
 	}
 
 	spin_unlock_irqrestore(&kernfs_open_node_lock, flags);
+
+	/* kick fsnotify */
+	mutex_lock(&kernfs_mutex);
+
+	list_for_each_entry(info, &root->supers, node) {
+		struct inode *inode;
+		struct dentry *dentry;
+
+		inode = ilookup(info->sb, kn->ino);
+		if (!inode)
+			continue;
+
+		dentry = d_find_any_alias(inode);
+		if (dentry) {
+			fsnotify_parent(NULL, dentry, FS_MODIFY);
+			fsnotify(inode, FS_MODIFY, inode, FSNOTIFY_EVENT_INODE,
+				 NULL, 0);
+			dput(dentry);
+		}
+
+		iput(inode);
+	}
+
+	mutex_unlock(&kernfs_mutex);
 }
 EXPORT_SYMBOL_GPL(kernfs_notify);
 
diff --git a/fs/kernfs/inode.c b/fs/kernfs/inode.c
index abb0f1f53d9..985217626e6 100644
--- a/fs/kernfs/inode.c
+++ b/fs/kernfs/inode.c
@@ -48,14 +48,18 @@ void __init kernfs_inode_init(void)
 
 static struct kernfs_iattrs *kernfs_iattrs(struct kernfs_node *kn)
 {
+	static DEFINE_MUTEX(iattr_mutex);
+	struct kernfs_iattrs *ret;
 	struct iattr *iattrs;
 
+	mutex_lock(&iattr_mutex);
+
 	if (kn->iattr)
-		return kn->iattr;
+		goto out_unlock;
 
 	kn->iattr = kzalloc(sizeof(struct kernfs_iattrs), GFP_KERNEL);
 	if (!kn->iattr)
-		return NULL;
+		goto out_unlock;
 	iattrs = &kn->iattr->ia_iattr;
 
 	/* assign default attributes */
@@ -65,8 +69,10 @@ static struct kernfs_iattrs *kernfs_iattrs(struct kernfs_node *kn)
 	iattrs->ia_atime = iattrs->ia_mtime = iattrs->ia_ctime = CURRENT_TIME;
 
 	simple_xattrs_init(&kn->iattr->xattrs);
-
-	return kn->iattr;
+out_unlock:
+	ret = kn->iattr;
+	mutex_unlock(&iattr_mutex);
+	return ret;
 }
 
 static int __kernfs_setattr(struct kernfs_node *kn, const struct iattr *iattr)
diff --git a/fs/kernfs/kernfs-internal.h b/fs/kernfs/kernfs-internal.h
index 8be13b2a079..dc84a3ef9ca 100644
--- a/fs/kernfs/kernfs-internal.h
+++ b/fs/kernfs/kernfs-internal.h
@@ -49,6 +49,8 @@ static inline struct kernfs_root *kernfs_root(struct kernfs_node *kn)
  * mount.c
  */
 struct kernfs_super_info {
+	struct super_block	*sb;
+
 	/*
 	 * The root associated with this super_block.  Each super_block is
 	 * identified by the root and ns it's associated with.
@@ -62,6 +64,9 @@ struct kernfs_super_info {
 	 * an array and compare kernfs_node tag against every entry.
 	 */
 	const void		*ns;
+
+	/* anchored at kernfs_root->supers, protected by kernfs_mutex */
+	struct list_head	node;
 };
 #define kernfs_info(SB) ((struct kernfs_super_info *)(SB->s_fs_info))
 
diff --git a/fs/kernfs/mount.c b/fs/kernfs/mount.c
index 6a5f04ac870..d171b98a6cd 100644
--- a/fs/kernfs/mount.c
+++ b/fs/kernfs/mount.c
@@ -62,15 +62,16 @@ struct kernfs_root *kernfs_root_from_sb(struct super_block *sb)
 	return NULL;
 }
 
-static int kernfs_fill_super(struct super_block *sb)
+static int kernfs_fill_super(struct super_block *sb, unsigned long magic)
 {
 	struct kernfs_super_info *info = kernfs_info(sb);
 	struct inode *inode;
 	struct dentry *root;
 
+	info->sb = sb;
 	sb->s_blocksize = PAGE_CACHE_SIZE;
 	sb->s_blocksize_bits = PAGE_CACHE_SHIFT;
-	sb->s_magic = SYSFS_MAGIC;
+	sb->s_magic = magic;
 	sb->s_op = &kernfs_sops;
 	sb->s_time_gran = 1;
 
@@ -131,6 +132,7 @@ const void *kernfs_super_ns(struct super_block *sb)
  * @fs_type: file_system_type of the fs being mounted
  * @flags: mount flags specified for the mount
  * @root: kernfs_root of the hierarchy being mounted
+ * @magic: file system specific magic number
  * @new_sb_created: tell the caller if we allocated a new superblock
  * @ns: optional namespace tag of the mount
  *
@@ -142,8 +144,8 @@ const void *kernfs_super_ns(struct super_block *sb)
  * The return value can be passed to the vfs layer verbatim.
  */
 struct dentry *kernfs_mount_ns(struct file_system_type *fs_type, int flags,
-			       struct kernfs_root *root, bool *new_sb_created,
-			       const void *ns)
+				struct kernfs_root *root, unsigned long magic,
+				bool *new_sb_created, const void *ns)
 {
 	struct super_block *sb;
 	struct kernfs_super_info *info;
@@ -166,12 +168,18 @@ struct dentry *kernfs_mount_ns(struct file_system_type *fs_type, int flags,
 		*new_sb_created = !sb->s_root;
 
 	if (!sb->s_root) {
-		error = kernfs_fill_super(sb);
+		struct kernfs_super_info *info = kernfs_info(sb);
+
+		error = kernfs_fill_super(sb, magic);
 		if (error) {
 			deactivate_locked_super(sb);
 			return ERR_PTR(error);
 		}
 		sb->s_flags |= MS_ACTIVE;
+
+		mutex_lock(&kernfs_mutex);
+		list_add(&info->node, &root->supers);
+		mutex_unlock(&kernfs_mutex);
 	}
 
 	return dget(sb->s_root);
@@ -190,6 +198,10 @@ void kernfs_kill_sb(struct super_block *sb)
 	struct kernfs_super_info *info = kernfs_info(sb);
 	struct kernfs_node *root_kn = sb->s_root->d_fsdata;
 
+	mutex_lock(&kernfs_mutex);
+	list_del(&info->node);
+	mutex_unlock(&kernfs_mutex);
+
 	/*
 	 * Remove the superblock from fs_supers/s_instances
 	 * so we can't find it, before freeing kernfs_super_info.
diff --git a/fs/locks.c b/fs/locks.c
index 13fc7a6d380..e390bd9ae06 100644
--- a/fs/locks.c
+++ b/fs/locks.c
@@ -135,7 +135,7 @@
 #define IS_POSIX(fl)	(fl->fl_flags & FL_POSIX)
 #define IS_FLOCK(fl)	(fl->fl_flags & FL_FLOCK)
 #define IS_LEASE(fl)	(fl->fl_flags & (FL_LEASE|FL_DELEG))
-#define IS_FILE_PVT(fl)	(fl->fl_flags & FL_FILE_PVT)
+#define IS_OFDLCK(fl)	(fl->fl_flags & FL_OFDLCK)
 
 static bool lease_breaking(struct file_lock *fl)
 {
@@ -389,18 +389,6 @@ static int flock64_to_posix_lock(struct file *filp, struct file_lock *fl,
 	fl->fl_ops = NULL;
 	fl->fl_lmops = NULL;
 
-	/* Ensure that fl->fl_filp has compatible f_mode */
-	switch (l->l_type) {
-	case F_RDLCK:
-		if (!(filp->f_mode & FMODE_READ))
-			return -EBADF;
-		break;
-	case F_WRLCK:
-		if (!(filp->f_mode & FMODE_WRITE))
-			return -EBADF;
-		break;
-	}
-
 	return assign_type(fl, l->l_type);
 }
 
@@ -564,7 +552,7 @@ static void __locks_insert_block(struct file_lock *blocker,
 	BUG_ON(!list_empty(&waiter->fl_block));
 	waiter->fl_next = blocker;
 	list_add_tail(&waiter->fl_block, &blocker->fl_block);
-	if (IS_POSIX(blocker) && !IS_FILE_PVT(blocker))
+	if (IS_POSIX(blocker) && !IS_OFDLCK(blocker))
 		locks_insert_global_blocked(waiter);
 }
 
@@ -759,12 +747,12 @@ EXPORT_SYMBOL(posix_test_lock);
  * of tasks (such as posix threads) sharing the same open file table.
  * To handle those cases, we just bail out after a few iterations.
  *
- * For FL_FILE_PVT locks, the owner is the filp, not the files_struct.
+ * For FL_OFDLCK locks, the owner is the filp, not the files_struct.
  * Because the owner is not even nominally tied to a thread of
  * execution, the deadlock detection below can't reasonably work well. Just
  * skip it for those.
  *
- * In principle, we could do a more limited deadlock detection on FL_FILE_PVT
+ * In principle, we could do a more limited deadlock detection on FL_OFDLCK
  * locks that just checks for the case where two tasks are attempting to
  * upgrade from read to write locks on the same inode.
  */
@@ -791,9 +779,9 @@ static int posix_locks_deadlock(struct file_lock *caller_fl,
 
 	/*
 	 * This deadlock detector can't reasonably detect deadlocks with
-	 * FL_FILE_PVT locks, since they aren't owned by a process, per-se.
+	 * FL_OFDLCK locks, since they aren't owned by a process, per-se.
 	 */
-	if (IS_FILE_PVT(caller_fl))
+	if (IS_OFDLCK(caller_fl))
 		return 0;
 
 	while ((block_fl = what_owner_is_waiting_for(block_fl))) {
@@ -1391,11 +1379,10 @@ int __break_lease(struct inode *inode, unsigned int mode, unsigned int type)
 
 restart:
 	break_time = flock->fl_break_time;
-	if (break_time != 0) {
+	if (break_time != 0)
 		break_time -= jiffies;
-		if (break_time == 0)
-			break_time++;
-	}
+	if (break_time == 0)
+		break_time++;
 	locks_insert_block(flock, new_fl);
 	spin_unlock(&inode->i_lock);
 	error = wait_event_interruptible_timeout(new_fl->fl_wait,
@@ -1891,7 +1878,7 @@ EXPORT_SYMBOL_GPL(vfs_test_lock);
 
 static int posix_lock_to_flock(struct flock *flock, struct file_lock *fl)
 {
-	flock->l_pid = IS_FILE_PVT(fl) ? -1 : fl->fl_pid;
+	flock->l_pid = IS_OFDLCK(fl) ? -1 : fl->fl_pid;
 #if BITS_PER_LONG == 32
 	/*
 	 * Make sure we can represent the posix lock via
@@ -1913,7 +1900,7 @@ static int posix_lock_to_flock(struct flock *flock, struct file_lock *fl)
 #if BITS_PER_LONG == 32
 static void posix_lock_to_flock64(struct flock64 *flock, struct file_lock *fl)
 {
-	flock->l_pid = IS_FILE_PVT(fl) ? -1 : fl->fl_pid;
+	flock->l_pid = IS_OFDLCK(fl) ? -1 : fl->fl_pid;
 	flock->l_start = fl->fl_start;
 	flock->l_len = fl->fl_end == OFFSET_MAX ? 0 :
 		fl->fl_end - fl->fl_start + 1;
@@ -1942,13 +1929,13 @@ int fcntl_getlk(struct file *filp, unsigned int cmd, struct flock __user *l)
 	if (error)
 		goto out;
 
-	if (cmd == F_GETLKP) {
+	if (cmd == F_OFD_GETLK) {
 		error = -EINVAL;
 		if (flock.l_pid != 0)
 			goto out;
 
 		cmd = F_GETLK;
-		file_lock.fl_flags |= FL_FILE_PVT;
+		file_lock.fl_flags |= FL_OFDLCK;
 		file_lock.fl_owner = (fl_owner_t)filp;
 	}
 
@@ -2035,6 +2022,22 @@ static int do_lock_file_wait(struct file *filp, unsigned int cmd,
 	return error;
 }
 
+/* Ensure that fl->fl_filp has compatible f_mode for F_SETLK calls */
+static int
+check_fmode_for_setlk(struct file_lock *fl)
+{
+	switch (fl->fl_type) {
+	case F_RDLCK:
+		if (!(fl->fl_file->f_mode & FMODE_READ))
+			return -EBADF;
+		break;
+	case F_WRLCK:
+		if (!(fl->fl_file->f_mode & FMODE_WRITE))
+			return -EBADF;
+	}
+	return 0;
+}
+
 /* Apply the lock described by l to an open file descriptor.
  * This implements both the F_SETLK and F_SETLKW commands of fcntl().
  */
@@ -2072,27 +2075,31 @@ again:
 	if (error)
 		goto out;
 
+	error = check_fmode_for_setlk(file_lock);
+	if (error)
+		goto out;
+
 	/*
 	 * If the cmd is requesting file-private locks, then set the
-	 * FL_FILE_PVT flag and override the owner.
+	 * FL_OFDLCK flag and override the owner.
 	 */
 	switch (cmd) {
-	case F_SETLKP:
+	case F_OFD_SETLK:
 		error = -EINVAL;
 		if (flock.l_pid != 0)
 			goto out;
 
 		cmd = F_SETLK;
-		file_lock->fl_flags |= FL_FILE_PVT;
+		file_lock->fl_flags |= FL_OFDLCK;
 		file_lock->fl_owner = (fl_owner_t)filp;
 		break;
-	case F_SETLKPW:
+	case F_OFD_SETLKW:
 		error = -EINVAL;
 		if (flock.l_pid != 0)
 			goto out;
 
 		cmd = F_SETLKW;
-		file_lock->fl_flags |= FL_FILE_PVT;
+		file_lock->fl_flags |= FL_OFDLCK;
 		file_lock->fl_owner = (fl_owner_t)filp;
 		/* Fallthrough */
 	case F_SETLKW:
@@ -2144,13 +2151,13 @@ int fcntl_getlk64(struct file *filp, unsigned int cmd, struct flock64 __user *l)
 	if (error)
 		goto out;
 
-	if (cmd == F_GETLKP) {
+	if (cmd == F_OFD_GETLK) {
 		error = -EINVAL;
 		if (flock.l_pid != 0)
 			goto out;
 
 		cmd = F_GETLK64;
-		file_lock.fl_flags |= FL_FILE_PVT;
+		file_lock.fl_flags |= FL_OFDLCK;
 		file_lock.fl_owner = (fl_owner_t)filp;
 	}
 
@@ -2207,27 +2214,31 @@ again:
 	if (error)
 		goto out;
 
+	error = check_fmode_for_setlk(file_lock);
+	if (error)
+		goto out;
+
 	/*
 	 * If the cmd is requesting file-private locks, then set the
-	 * FL_FILE_PVT flag and override the owner.
+	 * FL_OFDLCK flag and override the owner.
 	 */
 	switch (cmd) {
-	case F_SETLKP:
+	case F_OFD_SETLK:
 		error = -EINVAL;
 		if (flock.l_pid != 0)
 			goto out;
 
 		cmd = F_SETLK64;
-		file_lock->fl_flags |= FL_FILE_PVT;
+		file_lock->fl_flags |= FL_OFDLCK;
 		file_lock->fl_owner = (fl_owner_t)filp;
 		break;
-	case F_SETLKPW:
+	case F_OFD_SETLKW:
 		error = -EINVAL;
 		if (flock.l_pid != 0)
 			goto out;
 
 		cmd = F_SETLKW64;
-		file_lock->fl_flags |= FL_FILE_PVT;
+		file_lock->fl_flags |= FL_OFDLCK;
 		file_lock->fl_owner = (fl_owner_t)filp;
 		/* Fallthrough */
 	case F_SETLKW64:
@@ -2413,8 +2424,8 @@ static void lock_get_status(struct seq_file *f, struct file_lock *fl,
 	if (IS_POSIX(fl)) {
 		if (fl->fl_flags & FL_ACCESS)
 			seq_printf(f, "ACCESS");
-		else if (IS_FILE_PVT(fl))
-			seq_printf(f, "FLPVT ");
+		else if (IS_OFDLCK(fl))
+			seq_printf(f, "OFDLCK");
 		else
 			seq_printf(f, "POSIX ");
 
diff --git a/fs/namei.c b/fs/namei.c
index c6157c894fc..80168273396 100644
--- a/fs/namei.c
+++ b/fs/namei.c
@@ -1542,7 +1542,7 @@ static inline int walk_component(struct nameidata *nd, struct path *path,
 		inode = path->dentry->d_inode;
 	}
 	err = -ENOENT;
-	if (!inode)
+	if (!inode || d_is_negative(path->dentry))
 		goto out_path_put;
 
 	if (should_follow_link(path->dentry, follow)) {
@@ -2249,7 +2249,7 @@ mountpoint_last(struct nameidata *nd, struct path *path)
 	mutex_unlock(&dir->d_inode->i_mutex);
 
 done:
-	if (!dentry->d_inode) {
+	if (!dentry->d_inode || d_is_negative(dentry)) {
 		error = -ENOENT;
 		dput(dentry);
 		goto out;
@@ -2994,7 +2994,7 @@ retry_lookup:
 finish_lookup:
 	/* we _can_ be in RCU mode here */
 	error = -ENOENT;
-	if (d_is_negative(path->dentry)) {
+	if (!inode || d_is_negative(path->dentry)) {
 		path_to_nameidata(path, nd);
 		goto out;
 	}
diff --git a/fs/nfsd/nfs4acl.c b/fs/nfsd/nfs4acl.c
index 6f3f392d48a..f66c66b9f18 100644
--- a/fs/nfsd/nfs4acl.c
+++ b/fs/nfsd/nfs4acl.c
@@ -402,8 +402,10 @@ sort_pacl(struct posix_acl *pacl)
 	 * by uid/gid. */
 	int i, j;
 
-	if (pacl->a_count <= 4)
-		return; /* no users or groups */
+	/* no users or groups */
+	if (!pacl || pacl->a_count <= 4)
+		return;
+
 	i = 1;
 	while (pacl->a_entries[i].e_tag == ACL_USER)
 		i++;
@@ -530,13 +532,12 @@ posix_state_to_acl(struct posix_acl_state *state, unsigned int flags)
 
 	/*
 	 * ACLs with no ACEs are treated differently in the inheritable
-	 * and effective cases: when there are no inheritable ACEs, we
-	 * set a zero-length default posix acl:
+	 * and effective cases: when there are no inheritable ACEs,
+	 * calls ->set_acl with a NULL ACL structure.
 	 */
-	if (state->empty && (flags & NFS4_ACL_TYPE_DEFAULT)) {
-		pacl = posix_acl_alloc(0, GFP_KERNEL);
-		return pacl ? pacl : ERR_PTR(-ENOMEM);
-	}
+	if (state->empty && (flags & NFS4_ACL_TYPE_DEFAULT))
+		return NULL;
+
 	/*
 	 * When there are no effective ACEs, the following will end
 	 * up setting a 3-element effective posix ACL with all
@@ -589,7 +590,7 @@ posix_state_to_acl(struct posix_acl_state *state, unsigned int flags)
 		add_to_mask(state, &state->groups->aces[i].perms);
 	}
 
-	if (!state->users->n && !state->groups->n) {
+	if (state->users->n || state->groups->n) {
 		pace++;
 		pace->e_tag = ACL_MASK;
 		low_mode_from_nfs4(state->mask.allow, &pace->e_perm, flags);
diff --git a/fs/nfsd/nfs4callback.c b/fs/nfsd/nfs4callback.c
index 39c8ef875f9..2c73cae9899 100644
--- a/fs/nfsd/nfs4callback.c
+++ b/fs/nfsd/nfs4callback.c
@@ -654,9 +654,11 @@ static struct rpc_clnt *create_backchannel_client(struct rpc_create_args *args)
 
 static int setup_callback_client(struct nfs4_client *clp, struct nfs4_cb_conn *conn, struct nfsd4_session *ses)
 {
+	int maxtime = max_cb_time(clp->net);
 	struct rpc_timeout	timeparms = {
-		.to_initval	= max_cb_time(clp->net),
+		.to_initval	= maxtime,
 		.to_retries	= 0,
+		.to_maxval	= maxtime,
 	};
 	struct rpc_create_args args = {
 		.net		= clp->net,
diff --git a/fs/nfsd/nfs4state.c b/fs/nfsd/nfs4state.c
index 3ba65979a3c..9a77a5a2155 100644
--- a/fs/nfsd/nfs4state.c
+++ b/fs/nfsd/nfs4state.c
@@ -1078,6 +1078,18 @@ static struct nfs4_client *alloc_client(struct xdr_netobj name)
 		return NULL;
 	}
 	clp->cl_name.len = name.len;
+	INIT_LIST_HEAD(&clp->cl_sessions);
+	idr_init(&clp->cl_stateids);
+	atomic_set(&clp->cl_refcount, 0);
+	clp->cl_cb_state = NFSD4_CB_UNKNOWN;
+	INIT_LIST_HEAD(&clp->cl_idhash);
+	INIT_LIST_HEAD(&clp->cl_openowners);
+	INIT_LIST_HEAD(&clp->cl_delegations);
+	INIT_LIST_HEAD(&clp->cl_lru);
+	INIT_LIST_HEAD(&clp->cl_callbacks);
+	INIT_LIST_HEAD(&clp->cl_revoked);
+	spin_lock_init(&clp->cl_lock);
+	rpc_init_wait_queue(&clp->cl_cb_waitq, "Backchannel slot table");
 	return clp;
 }
 
@@ -1095,6 +1107,7 @@ free_client(struct nfs4_client *clp)
 		WARN_ON_ONCE(atomic_read(&ses->se_ref));
 		free_session(ses);
 	}
+	rpc_destroy_wait_queue(&clp->cl_cb_waitq);
 	free_svc_cred(&clp->cl_cred);
 	kfree(clp->cl_name.data);
 	idr_destroy(&clp->cl_stateids);
@@ -1347,7 +1360,6 @@ static struct nfs4_client *create_client(struct xdr_netobj name,
 	if (clp == NULL)
 		return NULL;
 
-	INIT_LIST_HEAD(&clp->cl_sessions);
 	ret = copy_cred(&clp->cl_cred, &rqstp->rq_cred);
 	if (ret) {
 		spin_lock(&nn->client_lock);
@@ -1355,20 +1367,9 @@ static struct nfs4_client *create_client(struct xdr_netobj name,
 		spin_unlock(&nn->client_lock);
 		return NULL;
 	}
-	idr_init(&clp->cl_stateids);
-	atomic_set(&clp->cl_refcount, 0);
-	clp->cl_cb_state = NFSD4_CB_UNKNOWN;
-	INIT_LIST_HEAD(&clp->cl_idhash);
-	INIT_LIST_HEAD(&clp->cl_openowners);
-	INIT_LIST_HEAD(&clp->cl_delegations);
-	INIT_LIST_HEAD(&clp->cl_lru);
-	INIT_LIST_HEAD(&clp->cl_callbacks);
-	INIT_LIST_HEAD(&clp->cl_revoked);
-	spin_lock_init(&clp->cl_lock);
 	nfsd4_init_callback(&clp->cl_cb_null);
 	clp->cl_time = get_seconds();
 	clear_bit(0, &clp->cl_cb_slot_busy);
-	rpc_init_wait_queue(&clp->cl_cb_waitq, "Backchannel slot table");
 	copy_verf(clp, verf);
 	rpc_copy_addr((struct sockaddr *) &clp->cl_addr, sa);
 	gen_confirm(clp);
@@ -3716,9 +3717,16 @@ out:
 static __be32
 nfsd4_free_lock_stateid(struct nfs4_ol_stateid *stp)
 {
-	if (check_for_locks(stp->st_file, lockowner(stp->st_stateowner)))
+	struct nfs4_lockowner *lo = lockowner(stp->st_stateowner);
+
+	if (check_for_locks(stp->st_file, lo))
 		return nfserr_locks_held;
-	release_lock_stateid(stp);
+	/*
+	 * Currently there's a 1-1 lock stateid<->lockowner
+	 * correspondance, and we have to delete the lockowner when we
+	 * delete the lock stateid:
+	 */
+	unhash_lockowner(lo);
 	return nfs_ok;
 }
 
@@ -4158,6 +4166,10 @@ static bool same_lockowner_ino(struct nfs4_lockowner *lo, struct inode *inode, c
 
 	if (!same_owner_str(&lo->lo_owner, owner, clid))
 		return false;
+	if (list_empty(&lo->lo_owner.so_stateids)) {
+		WARN_ON_ONCE(1);
+		return false;
+	}
 	lst = list_first_entry(&lo->lo_owner.so_stateids,
 			       struct nfs4_ol_stateid, st_perstateowner);
 	return lst->st_file->fi_inode == inode;
diff --git a/fs/nfsd/nfs4xdr.c b/fs/nfsd/nfs4xdr.c
index 2723c1badd0..18881f34737 100644
--- a/fs/nfsd/nfs4xdr.c
+++ b/fs/nfsd/nfs4xdr.c
@@ -3627,14 +3627,6 @@ nfsd4_encode_operation(struct nfsd4_compoundres *resp, struct nfsd4_op *op)
 	/* nfsd4_check_resp_size guarantees enough room for error status */
 	if (!op->status)
 		op->status = nfsd4_check_resp_size(resp, 0);
-	if (op->status == nfserr_resource && nfsd4_has_session(&resp->cstate)) {
-		struct nfsd4_slot *slot = resp->cstate.slot;
-
-		if (slot->sl_flags & NFSD4_SLOT_CACHETHIS)
-			op->status = nfserr_rep_too_big_to_cache;
-		else
-			op->status = nfserr_rep_too_big;
-	}
 	if (so) {
 		so->so_replay.rp_status = op->status;
 		so->so_replay.rp_buflen = (char *)resp->p - (char *)(statp+1);
diff --git a/fs/notify/fanotify/fanotify_user.c b/fs/notify/fanotify/fanotify_user.c
index 4e565c81430..732648b270d 100644
--- a/fs/notify/fanotify/fanotify_user.c
+++ b/fs/notify/fanotify/fanotify_user.c
@@ -698,6 +698,8 @@ SYSCALL_DEFINE2(fanotify_init, unsigned int, flags, unsigned int, event_f_flags)
 	}
 	group->overflow_event = &oevent->fse;
 
+	if (force_o_largefile())
+		event_f_flags |= O_LARGEFILE;
 	group->fanotify_data.f_flags = event_f_flags;
 #ifdef CONFIG_FANOTIFY_ACCESS_PERMISSIONS
 	spin_lock_init(&group->fanotify_data.access_lock);
diff --git a/fs/ocfs2/dlm/dlmmaster.c b/fs/ocfs2/dlm/dlmmaster.c
index af3f7aa73e1..ee1f88419cb 100644
--- a/fs/ocfs2/dlm/dlmmaster.c
+++ b/fs/ocfs2/dlm/dlmmaster.c
@@ -472,11 +472,15 @@ bail:
 
 void dlm_destroy_master_caches(void)
 {
-	if (dlm_lockname_cache)
+	if (dlm_lockname_cache) {
 		kmem_cache_destroy(dlm_lockname_cache);
+		dlm_lockname_cache = NULL;
+	}
 
-	if (dlm_lockres_cache)
+	if (dlm_lockres_cache) {
 		kmem_cache_destroy(dlm_lockres_cache);
+		dlm_lockres_cache = NULL;
+	}
 }
 
 static void dlm_lockres_release(struct kref *kref)
diff --git a/fs/open.c b/fs/open.c
index 3d30eb1fc95..9d64679cec7 100644
--- a/fs/open.c
+++ b/fs/open.c
@@ -254,17 +254,22 @@ int do_fallocate(struct file *file, int mode, loff_t offset, loff_t len)
 		return -EBADF;
 
 	/*
-	 * It's not possible to punch hole or perform collapse range
-	 * on append only file
+	 * We can only allow pure fallocate on append only files
 	 */
-	if (mode & (FALLOC_FL_PUNCH_HOLE | FALLOC_FL_COLLAPSE_RANGE)
-	    && IS_APPEND(inode))
+	if ((mode & ~FALLOC_FL_KEEP_SIZE) && IS_APPEND(inode))
 		return -EPERM;
 
 	if (IS_IMMUTABLE(inode))
 		return -EPERM;
 
 	/*
+	 * We can not allow to do any fallocate operation on an active
+	 * swapfile
+	 */
+	if (IS_SWAPFILE(inode))
+		ret = -ETXTBSY;
+
+	/*
 	 * Revalidate the write permissions, in case security policy has
 	 * changed since the files were opened.
 	 */
@@ -286,14 +291,6 @@ int do_fallocate(struct file *file, int mode, loff_t offset, loff_t len)
 	if (((offset + len) > inode->i_sb->s_maxbytes) || ((offset + len) < 0))
 		return -EFBIG;
 
-	/*
-	 * There is no need to overlap collapse range with EOF, in which case
-	 * it is effectively a truncate operation
-	 */
-	if ((mode & FALLOC_FL_COLLAPSE_RANGE) &&
-	    (offset + len >= i_size_read(inode)))
-		return -EINVAL;
-
 	if (!file->f_op->fallocate)
 		return -EOPNOTSUPP;
 
diff --git a/fs/posix_acl.c b/fs/posix_acl.c
index 9e363e41dac..0855f772cd4 100644
--- a/fs/posix_acl.c
+++ b/fs/posix_acl.c
@@ -246,6 +246,12 @@ posix_acl_equiv_mode(const struct posix_acl *acl, umode_t *mode_p)
 	umode_t mode = 0;
 	int not_equiv = 0;
 
+	/*
+	 * A null ACL can always be presented as mode bits.
+	 */
+	if (!acl)
+		return 0;
+
 	FOREACH_ACL_ENTRY(pa, acl, pe) {
 		switch (pa->e_tag) {
 			case ACL_USER_OBJ:
diff --git a/fs/splice.c b/fs/splice.c
index 9bc07d2b53c..e246954ea48 100644
--- a/fs/splice.c
+++ b/fs/splice.c
@@ -1537,7 +1537,7 @@ static long vmsplice_to_user(struct file *file, const struct iovec __user *uiov,
 	struct iovec iovstack[UIO_FASTIOV];
 	struct iovec *iov = iovstack;
 	struct iov_iter iter;
-	ssize_t count = 0;
+	ssize_t count;
 
 	pipe = get_pipe_info(file);
 	if (!pipe)
@@ -1546,8 +1546,9 @@ static long vmsplice_to_user(struct file *file, const struct iovec __user *uiov,
 	ret = rw_copy_check_uvector(READ, uiov, nr_segs,
 				    ARRAY_SIZE(iovstack), iovstack, &iov);
 	if (ret <= 0)
-		return ret;
+		goto out;
 
+	count = ret;
 	iov_iter_init(&iter, iov, nr_segs, count, 0);
 
 	sd.len = 0;
@@ -1560,6 +1561,7 @@ static long vmsplice_to_user(struct file *file, const struct iovec __user *uiov,
 	ret = __splice_from_pipe(pipe, &sd, pipe_to_user);
 	pipe_unlock(pipe);
 
+out:
 	if (iov != iovstack)
 		kfree(iov);
 
diff --git a/fs/super.c b/fs/super.c
index e9dc3c3fe15..48377f7463c 100644
--- a/fs/super.c
+++ b/fs/super.c
@@ -800,7 +800,10 @@ void emergency_remount(void)
 
 static DEFINE_IDA(unnamed_dev_ida);
 static DEFINE_SPINLOCK(unnamed_dev_lock);/* protects the above */
-static int unnamed_dev_start = 0; /* don't bother trying below it */
+/* Many userspace utilities consider an FSID of 0 invalid.
+ * Always return at least 1 from get_anon_bdev.
+ */
+static int unnamed_dev_start = 1;
 
 int get_anon_bdev(dev_t *p)
 {
diff --git a/fs/sysfs/file.c b/fs/sysfs/file.c
index 1b8b91b67fd..e9ef59b3abb 100644
--- a/fs/sysfs/file.c
+++ b/fs/sysfs/file.c
@@ -47,12 +47,13 @@ static int sysfs_kf_seq_show(struct seq_file *sf, void *v)
 	ssize_t count;
 	char *buf;
 
-	/* acquire buffer and ensure that it's >= PAGE_SIZE */
+	/* acquire buffer and ensure that it's >= PAGE_SIZE and clear */
 	count = seq_get_buf(sf, &buf);
 	if (count < PAGE_SIZE) {
 		seq_commit(sf, -1);
 		return 0;
 	}
+	memset(buf, 0, PAGE_SIZE);
 
 	/*
 	 * Invoke show().  Control may reach here via seq file lseek even
@@ -453,95 +454,3 @@ void sysfs_remove_bin_file(struct kobject *kobj,
 	kernfs_remove_by_name(kobj->sd, attr->attr.name);
 }
 EXPORT_SYMBOL_GPL(sysfs_remove_bin_file);
-
-struct sysfs_schedule_callback_struct {
-	struct list_head	workq_list;
-	struct kobject		*kobj;
-	void			(*func)(void *);
-	void			*data;
-	struct module		*owner;
-	struct work_struct	work;
-};
-
-static struct workqueue_struct *sysfs_workqueue;
-static DEFINE_MUTEX(sysfs_workq_mutex);
-static LIST_HEAD(sysfs_workq);
-static void sysfs_schedule_callback_work(struct work_struct *work)
-{
-	struct sysfs_schedule_callback_struct *ss = container_of(work,
-			struct sysfs_schedule_callback_struct, work);
-
-	(ss->func)(ss->data);
-	kobject_put(ss->kobj);
-	module_put(ss->owner);
-	mutex_lock(&sysfs_workq_mutex);
-	list_del(&ss->workq_list);
-	mutex_unlock(&sysfs_workq_mutex);
-	kfree(ss);
-}
-
-/**
- * sysfs_schedule_callback - helper to schedule a callback for a kobject
- * @kobj: object we're acting for.
- * @func: callback function to invoke later.
- * @data: argument to pass to @func.
- * @owner: module owning the callback code
- *
- * sysfs attribute methods must not unregister themselves or their parent
- * kobject (which would amount to the same thing).  Attempts to do so will
- * deadlock, since unregistration is mutually exclusive with driver
- * callbacks.
- *
- * Instead methods can call this routine, which will attempt to allocate
- * and schedule a workqueue request to call back @func with @data as its
- * argument in the workqueue's process context.  @kobj will be pinned
- * until @func returns.
- *
- * Returns 0 if the request was submitted, -ENOMEM if storage could not
- * be allocated, -ENODEV if a reference to @owner isn't available,
- * -EAGAIN if a callback has already been scheduled for @kobj.
- */
-int sysfs_schedule_callback(struct kobject *kobj, void (*func)(void *),
-		void *data, struct module *owner)
-{
-	struct sysfs_schedule_callback_struct *ss, *tmp;
-
-	if (!try_module_get(owner))
-		return -ENODEV;
-
-	mutex_lock(&sysfs_workq_mutex);
-	list_for_each_entry_safe(ss, tmp, &sysfs_workq, workq_list)
-		if (ss->kobj == kobj) {
-			module_put(owner);
-			mutex_unlock(&sysfs_workq_mutex);
-			return -EAGAIN;
-		}
-	mutex_unlock(&sysfs_workq_mutex);
-
-	if (sysfs_workqueue == NULL) {
-		sysfs_workqueue = create_singlethread_workqueue("sysfsd");
-		if (sysfs_workqueue == NULL) {
-			module_put(owner);
-			return -ENOMEM;
-		}
-	}
-
-	ss = kmalloc(sizeof(*ss), GFP_KERNEL);
-	if (!ss) {
-		module_put(owner);
-		return -ENOMEM;
-	}
-	kobject_get(kobj);
-	ss->kobj = kobj;
-	ss->func = func;
-	ss->data = data;
-	ss->owner = owner;
-	INIT_WORK(&ss->work, sysfs_schedule_callback_work);
-	INIT_LIST_HEAD(&ss->workq_list);
-	mutex_lock(&sysfs_workq_mutex);
-	list_add_tail(&ss->workq_list, &sysfs_workq);
-	mutex_unlock(&sysfs_workq_mutex);
-	queue_work(sysfs_workqueue, &ss->work);
-	return 0;
-}
-EXPORT_SYMBOL_GPL(sysfs_schedule_callback);
diff --git a/fs/sysfs/group.c b/fs/sysfs/group.c
index aa0406895b5..7d2a860ba78 100644
--- a/fs/sysfs/group.c
+++ b/fs/sysfs/group.c
@@ -18,7 +18,7 @@
 #include "sysfs.h"
 
 
-static void remove_files(struct kernfs_node *parent, struct kobject *kobj,
+static void remove_files(struct kernfs_node *parent,
 			 const struct attribute_group *grp)
 {
 	struct attribute *const *attr;
@@ -29,7 +29,7 @@ static void remove_files(struct kernfs_node *parent, struct kobject *kobj,
 			kernfs_remove_by_name(parent, (*attr)->name);
 	if (grp->bin_attrs)
 		for (bin_attr = grp->bin_attrs; *bin_attr; bin_attr++)
-			sysfs_remove_bin_file(kobj, *bin_attr);
+			kernfs_remove_by_name(parent, (*bin_attr)->attr.name);
 }
 
 static int create_files(struct kernfs_node *parent, struct kobject *kobj,
@@ -62,7 +62,7 @@ static int create_files(struct kernfs_node *parent, struct kobject *kobj,
 				break;
 		}
 		if (error) {
-			remove_files(parent, kobj, grp);
+			remove_files(parent, grp);
 			goto exit;
 		}
 	}
@@ -79,7 +79,7 @@ static int create_files(struct kernfs_node *parent, struct kobject *kobj,
 				break;
 		}
 		if (error)
-			remove_files(parent, kobj, grp);
+			remove_files(parent, grp);
 	}
 exit:
 	return error;
@@ -224,7 +224,7 @@ void sysfs_remove_group(struct kobject *kobj,
 		kernfs_get(kn);
 	}
 
-	remove_files(kn, kobj, grp);
+	remove_files(kn, grp);
 	if (grp->name)
 		kernfs_remove(kn);
 
diff --git a/fs/sysfs/mount.c b/fs/sysfs/mount.c
index a66ad6196f5..8a49486bf30 100644
--- a/fs/sysfs/mount.c
+++ b/fs/sysfs/mount.c
@@ -13,6 +13,7 @@
 #define DEBUG
 
 #include <linux/fs.h>
+#include <linux/magic.h>
 #include <linux/mount.h>
 #include <linux/init.h>
 #include <linux/user_namespace.h>
@@ -38,7 +39,8 @@ static struct dentry *sysfs_mount(struct file_system_type *fs_type,
 	}
 
 	ns = kobj_ns_grab_current(KOBJ_NS_TYPE_NET);
-	root = kernfs_mount_ns(fs_type, flags, sysfs_root, &new_sb, ns);
+	root = kernfs_mount_ns(fs_type, flags, sysfs_root,
+				SYSFS_MAGIC, &new_sb, ns);
 	if (IS_ERR(root) || !new_sb)
 		kobj_ns_drop(KOBJ_NS_TYPE_NET, ns);
 	return root;
@@ -63,7 +65,8 @@ int __init sysfs_init(void)
 {
 	int err;
 
-	sysfs_root = kernfs_create_root(NULL, 0, NULL);
+	sysfs_root = kernfs_create_root(NULL, KERNFS_ROOT_EXTRA_OPEN_PERM_CHECK,
+					NULL);
 	if (IS_ERR(sysfs_root))
 		return PTR_ERR(sysfs_root);
 
diff --git a/fs/ubifs/super.c b/fs/ubifs/super.c
index a1266089eca..a81c7b55689 100644
--- a/fs/ubifs/super.c
+++ b/fs/ubifs/super.c
@@ -1556,7 +1556,7 @@ static int ubifs_remount_rw(struct ubifs_info *c)
 	if (c->space_fixup) {
 		err = ubifs_fixup_free_space(c);
 		if (err)
-			return err;
+			goto out;
 	}
 
 	err = check_free_space(c);
diff --git a/fs/xfs/xfs_aops.c b/fs/xfs/xfs_aops.c
index 75df77d09f7..0479c32c5eb 100644
--- a/fs/xfs/xfs_aops.c
+++ b/fs/xfs/xfs_aops.c
@@ -1344,6 +1344,14 @@ __xfs_get_blocks(
 	/*
 	 * If this is O_DIRECT or the mpage code calling tell them how large
 	 * the mapping is, so that we can avoid repeated get_blocks calls.
+	 *
+	 * If the mapping spans EOF, then we have to break the mapping up as the
+	 * mapping for blocks beyond EOF must be marked new so that sub block
+	 * regions can be correctly zeroed. We can't do this for mappings within
+	 * EOF unless the mapping was just allocated or is unwritten, otherwise
+	 * the callers would overwrite existing data with zeros. Hence we have
+	 * to split the mapping into a range up to and including EOF, and a
+	 * second mapping for beyond EOF.
 	 */
 	if (direct || size > (1 << inode->i_blkbits)) {
 		xfs_off_t		mapping_size;
@@ -1354,6 +1362,12 @@ __xfs_get_blocks(
 		ASSERT(mapping_size > 0);
 		if (mapping_size > size)
 			mapping_size = size;
+		if (offset < i_size_read(inode) &&
+		    offset + mapping_size >= i_size_read(inode)) {
+			/* limit mapping to block that spans EOF */
+			mapping_size = roundup_64(i_size_read(inode) - offset,
+						  1 << inode->i_blkbits);
+		}
 		if (mapping_size > LONG_MAX)
 			mapping_size = LONG_MAX;
 
@@ -1566,6 +1580,16 @@ xfs_vm_write_failed(
 
 		xfs_vm_kill_delalloc_range(inode, block_offset,
 					   block_offset + bh->b_size);
+
+		/*
+		 * This buffer does not contain data anymore. make sure anyone
+		 * who finds it knows that for certain.
+		 */
+		clear_buffer_delay(bh);
+		clear_buffer_uptodate(bh);
+		clear_buffer_mapped(bh);
+		clear_buffer_new(bh);
+		clear_buffer_dirty(bh);
 	}
 
 }
@@ -1599,12 +1623,21 @@ xfs_vm_write_begin(
 	status = __block_write_begin(page, pos, len, xfs_get_blocks);
 	if (unlikely(status)) {
 		struct inode	*inode = mapping->host;
+		size_t		isize = i_size_read(inode);
 
 		xfs_vm_write_failed(inode, page, pos, len);
 		unlock_page(page);
 
-		if (pos + len > i_size_read(inode))
-			truncate_pagecache(inode, i_size_read(inode));
+		/*
+		 * If the write is beyond EOF, we only want to kill blocks
+		 * allocated in this write, not blocks that were previously
+		 * written successfully.
+		 */
+		if (pos + len > isize) {
+			ssize_t start = max_t(ssize_t, pos, isize);
+
+			truncate_pagecache_range(inode, start, pos + len);
+		}
 
 		page_cache_release(page);
 		page = NULL;
@@ -1615,9 +1648,12 @@ xfs_vm_write_begin(
 }
 
 /*
- * On failure, we only need to kill delalloc blocks beyond EOF because they
- * will never be written. For blocks within EOF, generic_write_end() zeros them
- * so they are safe to leave alone and be written with all the other valid data.
+ * On failure, we only need to kill delalloc blocks beyond EOF in the range of
+ * this specific write because they will never be written. Previous writes
+ * beyond EOF where block allocation succeeded do not need to be trashed, so
+ * only new blocks from this write should be trashed. For blocks within
+ * EOF, generic_write_end() zeros them so they are safe to leave alone and be
+ * written with all the other valid data.
  */
 STATIC int
 xfs_vm_write_end(
@@ -1640,8 +1676,11 @@ xfs_vm_write_end(
 		loff_t		to = pos + len;
 
 		if (to > isize) {
-			truncate_pagecache(inode, isize);
+			/* only kill blocks in this write beyond EOF */
+			if (pos > isize)
+				isize = pos;
 			xfs_vm_kill_delalloc_range(inode, isize, to);
+			truncate_pagecache_range(inode, isize, to);
 		}
 	}
 	return ret;
diff --git a/fs/xfs/xfs_attr.c b/fs/xfs/xfs_attr.c
index 01b6a0102fb..abda1124a70 100644
--- a/fs/xfs/xfs_attr.c
+++ b/fs/xfs/xfs_attr.c
@@ -213,7 +213,7 @@ xfs_attr_calc_size(
 		 * Out of line attribute, cannot double split, but
 		 * make room for the attribute value itself.
 		 */
-		uint	dblocks = XFS_B_TO_FSB(mp, valuelen);
+		uint	dblocks = xfs_attr3_rmt_blocks(mp, valuelen);
 		nblks += dblocks;
 		nblks += XFS_NEXTENTADD_SPACE_RES(mp, dblocks, XFS_ATTR_FORK);
 	}
@@ -698,11 +698,22 @@ xfs_attr_leaf_addname(xfs_da_args_t *args)
 
 		trace_xfs_attr_leaf_replace(args);
 
+		/* save the attribute state for later removal*/
 		args->op_flags |= XFS_DA_OP_RENAME;	/* an atomic rename */
 		args->blkno2 = args->blkno;		/* set 2nd entry info*/
 		args->index2 = args->index;
 		args->rmtblkno2 = args->rmtblkno;
 		args->rmtblkcnt2 = args->rmtblkcnt;
+		args->rmtvaluelen2 = args->rmtvaluelen;
+
+		/*
+		 * clear the remote attr state now that it is saved so that the
+		 * values reflect the state of the attribute we are about to
+		 * add, not the attribute we just found and will remove later.
+		 */
+		args->rmtblkno = 0;
+		args->rmtblkcnt = 0;
+		args->rmtvaluelen = 0;
 	}
 
 	/*
@@ -794,6 +805,7 @@ xfs_attr_leaf_addname(xfs_da_args_t *args)
 		args->blkno = args->blkno2;
 		args->rmtblkno = args->rmtblkno2;
 		args->rmtblkcnt = args->rmtblkcnt2;
+		args->rmtvaluelen = args->rmtvaluelen2;
 		if (args->rmtblkno) {
 			error = xfs_attr_rmtval_remove(args);
 			if (error)
@@ -999,13 +1011,22 @@ restart:
 
 		trace_xfs_attr_node_replace(args);
 
+		/* save the attribute state for later removal*/
 		args->op_flags |= XFS_DA_OP_RENAME;	/* atomic rename op */
 		args->blkno2 = args->blkno;		/* set 2nd entry info*/
 		args->index2 = args->index;
 		args->rmtblkno2 = args->rmtblkno;
 		args->rmtblkcnt2 = args->rmtblkcnt;
+		args->rmtvaluelen2 = args->rmtvaluelen;
+
+		/*
+		 * clear the remote attr state now that it is saved so that the
+		 * values reflect the state of the attribute we are about to
+		 * add, not the attribute we just found and will remove later.
+		 */
 		args->rmtblkno = 0;
 		args->rmtblkcnt = 0;
+		args->rmtvaluelen = 0;
 	}
 
 	retval = xfs_attr3_leaf_add(blk->bp, state->args);
@@ -1133,6 +1154,7 @@ restart:
 		args->blkno = args->blkno2;
 		args->rmtblkno = args->rmtblkno2;
 		args->rmtblkcnt = args->rmtblkcnt2;
+		args->rmtvaluelen = args->rmtvaluelen2;
 		if (args->rmtblkno) {
 			error = xfs_attr_rmtval_remove(args);
 			if (error)
diff --git a/fs/xfs/xfs_attr_leaf.c b/fs/xfs/xfs_attr_leaf.c
index fe9587fab17..511c283459b 100644
--- a/fs/xfs/xfs_attr_leaf.c
+++ b/fs/xfs/xfs_attr_leaf.c
@@ -1229,6 +1229,7 @@ xfs_attr3_leaf_add_work(
 		name_rmt->valueblk = 0;
 		args->rmtblkno = 1;
 		args->rmtblkcnt = xfs_attr3_rmt_blocks(mp, args->valuelen);
+		args->rmtvaluelen = args->valuelen;
 	}
 	xfs_trans_log_buf(args->trans, bp,
 	     XFS_DA_LOGRANGE(leaf, xfs_attr3_leaf_name(leaf, args->index),
@@ -2167,11 +2168,11 @@ xfs_attr3_leaf_lookup_int(
 			if (!xfs_attr_namesp_match(args->flags, entry->flags))
 				continue;
 			args->index = probe;
-			args->valuelen = be32_to_cpu(name_rmt->valuelen);
+			args->rmtvaluelen = be32_to_cpu(name_rmt->valuelen);
 			args->rmtblkno = be32_to_cpu(name_rmt->valueblk);
 			args->rmtblkcnt = xfs_attr3_rmt_blocks(
 							args->dp->i_mount,
-							args->valuelen);
+							args->rmtvaluelen);
 			return XFS_ERROR(EEXIST);
 		}
 	}
@@ -2220,19 +2221,19 @@ xfs_attr3_leaf_getvalue(
 		name_rmt = xfs_attr3_leaf_name_remote(leaf, args->index);
 		ASSERT(name_rmt->namelen == args->namelen);
 		ASSERT(memcmp(args->name, name_rmt->name, args->namelen) == 0);
-		valuelen = be32_to_cpu(name_rmt->valuelen);
+		args->rmtvaluelen = be32_to_cpu(name_rmt->valuelen);
 		args->rmtblkno = be32_to_cpu(name_rmt->valueblk);
 		args->rmtblkcnt = xfs_attr3_rmt_blocks(args->dp->i_mount,
-						       valuelen);
+						       args->rmtvaluelen);
 		if (args->flags & ATTR_KERNOVAL) {
-			args->valuelen = valuelen;
+			args->valuelen = args->rmtvaluelen;
 			return 0;
 		}
-		if (args->valuelen < valuelen) {
-			args->valuelen = valuelen;
+		if (args->valuelen < args->rmtvaluelen) {
+			args->valuelen = args->rmtvaluelen;
 			return XFS_ERROR(ERANGE);
 		}
-		args->valuelen = valuelen;
+		args->valuelen = args->rmtvaluelen;
 	}
 	return 0;
 }
@@ -2519,7 +2520,7 @@ xfs_attr3_leaf_clearflag(
 		ASSERT((entry->flags & XFS_ATTR_LOCAL) == 0);
 		name_rmt = xfs_attr3_leaf_name_remote(leaf, args->index);
 		name_rmt->valueblk = cpu_to_be32(args->rmtblkno);
-		name_rmt->valuelen = cpu_to_be32(args->valuelen);
+		name_rmt->valuelen = cpu_to_be32(args->rmtvaluelen);
 		xfs_trans_log_buf(args->trans, bp,
 			 XFS_DA_LOGRANGE(leaf, name_rmt, sizeof(*name_rmt)));
 	}
@@ -2677,7 +2678,7 @@ xfs_attr3_leaf_flipflags(
 		ASSERT((entry1->flags & XFS_ATTR_LOCAL) == 0);
 		name_rmt = xfs_attr3_leaf_name_remote(leaf1, args->index);
 		name_rmt->valueblk = cpu_to_be32(args->rmtblkno);
-		name_rmt->valuelen = cpu_to_be32(args->valuelen);
+		name_rmt->valuelen = cpu_to_be32(args->rmtvaluelen);
 		xfs_trans_log_buf(args->trans, bp1,
 			 XFS_DA_LOGRANGE(leaf1, name_rmt, sizeof(*name_rmt)));
 	}
diff --git a/fs/xfs/xfs_attr_list.c b/fs/xfs/xfs_attr_list.c
index 01db96f60cf..833fe5d98d8 100644
--- a/fs/xfs/xfs_attr_list.c
+++ b/fs/xfs/xfs_attr_list.c
@@ -447,6 +447,7 @@ xfs_attr3_leaf_list_int(
 				args.dp = context->dp;
 				args.whichfork = XFS_ATTR_FORK;
 				args.valuelen = valuelen;
+				args.rmtvaluelen = valuelen;
 				args.value = kmem_alloc(valuelen, KM_SLEEP | KM_NOFS);
 				args.rmtblkno = be32_to_cpu(name_rmt->valueblk);
 				args.rmtblkcnt = xfs_attr3_rmt_blocks(
diff --git a/fs/xfs/xfs_attr_remote.c b/fs/xfs/xfs_attr_remote.c
index 6e37823e293..d2e6e948cec 100644
--- a/fs/xfs/xfs_attr_remote.c
+++ b/fs/xfs/xfs_attr_remote.c
@@ -337,7 +337,7 @@ xfs_attr_rmtval_get(
 	struct xfs_buf		*bp;
 	xfs_dablk_t		lblkno = args->rmtblkno;
 	__uint8_t		*dst = args->value;
-	int			valuelen = args->valuelen;
+	int			valuelen;
 	int			nmap;
 	int			error;
 	int			blkcnt = args->rmtblkcnt;
@@ -347,7 +347,9 @@ xfs_attr_rmtval_get(
 	trace_xfs_attr_rmtval_get(args);
 
 	ASSERT(!(args->flags & ATTR_KERNOVAL));
+	ASSERT(args->rmtvaluelen == args->valuelen);
 
+	valuelen = args->rmtvaluelen;
 	while (valuelen > 0) {
 		nmap = ATTR_RMTVALUE_MAPSIZE;
 		error = xfs_bmapi_read(args->dp, (xfs_fileoff_t)lblkno,
@@ -415,7 +417,7 @@ xfs_attr_rmtval_set(
 	 * attributes have headers, we can't just do a straight byte to FSB
 	 * conversion and have to take the header space into account.
 	 */
-	blkcnt = xfs_attr3_rmt_blocks(mp, args->valuelen);
+	blkcnt = xfs_attr3_rmt_blocks(mp, args->rmtvaluelen);
 	error = xfs_bmap_first_unused(args->trans, args->dp, blkcnt, &lfileoff,
 						   XFS_ATTR_FORK);
 	if (error)
@@ -480,7 +482,7 @@ xfs_attr_rmtval_set(
 	 */
 	lblkno = args->rmtblkno;
 	blkcnt = args->rmtblkcnt;
-	valuelen = args->valuelen;
+	valuelen = args->rmtvaluelen;
 	while (valuelen > 0) {
 		struct xfs_buf	*bp;
 		xfs_daddr_t	dblkno;
diff --git a/fs/xfs/xfs_bmap.c b/fs/xfs/xfs_bmap.c
index 5b6092ef51e..f0efc7e970e 100644
--- a/fs/xfs/xfs_bmap.c
+++ b/fs/xfs/xfs_bmap.c
@@ -5413,6 +5413,7 @@ xfs_bmap_shift_extents(
 	int				whichfork = XFS_DATA_FORK;
 	int				logflags;
 	xfs_filblks_t			blockcount = 0;
+	int				total_extents;
 
 	if (unlikely(XFS_TEST_ERROR(
 	    (XFS_IFORK_FORMAT(ip, whichfork) != XFS_DINODE_FMT_EXTENTS &&
@@ -5429,7 +5430,6 @@ xfs_bmap_shift_extents(
 	ASSERT(current_ext != NULL);
 
 	ifp = XFS_IFORK_PTR(ip, whichfork);
-
 	if (!(ifp->if_flags & XFS_IFEXTENTS)) {
 		/* Read in all the extents */
 		error = xfs_iread_extents(tp, ip, whichfork);
@@ -5456,7 +5456,6 @@ xfs_bmap_shift_extents(
 
 	/* We are going to change core inode */
 	logflags = XFS_ILOG_CORE;
-
 	if (ifp->if_flags & XFS_IFBROOT) {
 		cur = xfs_bmbt_init_cursor(mp, tp, ip, whichfork);
 		cur->bc_private.b.firstblock = *firstblock;
@@ -5467,8 +5466,14 @@ xfs_bmap_shift_extents(
 		logflags |= XFS_ILOG_DEXT;
 	}
 
-	while (nexts++ < num_exts &&
-	       *current_ext <  XFS_IFORK_NEXTENTS(ip, whichfork)) {
+	/*
+	 * There may be delalloc extents in the data fork before the range we
+	 * are collapsing out, so we cannot
+	 * use the count of real extents here. Instead we have to calculate it
+	 * from the incore fork.
+	 */
+	total_extents = ifp->if_bytes / sizeof(xfs_bmbt_rec_t);
+	while (nexts++ < num_exts && *current_ext < total_extents) {
 
 		gotp = xfs_iext_get_ext(ifp, *current_ext);
 		xfs_bmbt_get_all(gotp, &got);
@@ -5556,10 +5561,11 @@ xfs_bmap_shift_extents(
 		}
 
 		(*current_ext)++;
+		total_extents = ifp->if_bytes / sizeof(xfs_bmbt_rec_t);
 	}
 
 	/* Check if we are done */
-	if (*current_ext ==  XFS_IFORK_NEXTENTS(ip, whichfork))
+	if (*current_ext == total_extents)
 		*done = 1;
 
 del_cursor:
@@ -5568,6 +5574,5 @@ del_cursor:
 			error ? XFS_BTREE_ERROR : XFS_BTREE_NOERROR);
 
 	xfs_trans_log_inode(tp, ip, logflags);
-
 	return error;
 }
diff --git a/fs/xfs/xfs_bmap_util.c b/fs/xfs/xfs_bmap_util.c
index 01f6a646caa..296160b8e78 100644
--- a/fs/xfs/xfs_bmap_util.c
+++ b/fs/xfs/xfs_bmap_util.c
@@ -1418,6 +1418,8 @@ xfs_zero_file_space(
 	xfs_off_t		end_boundary;
 	int			error;
 
+	trace_xfs_zero_file_space(ip);
+
 	granularity = max_t(uint, 1 << mp->m_sb.sb_blocklog, PAGE_CACHE_SIZE);
 
 	/*
@@ -1432,9 +1434,18 @@ xfs_zero_file_space(
 	ASSERT(end_boundary <= offset + len);
 
 	if (start_boundary < end_boundary - 1) {
-		/* punch out the page cache over the conversion range */
+		/*
+		 * punch out delayed allocation blocks and the page cache over
+		 * the conversion range
+		 */
+		xfs_ilock(ip, XFS_ILOCK_EXCL);
+		error = xfs_bmap_punch_delalloc_range(ip,
+				XFS_B_TO_FSBT(mp, start_boundary),
+				XFS_B_TO_FSB(mp, end_boundary - start_boundary));
+		xfs_iunlock(ip, XFS_ILOCK_EXCL);
 		truncate_pagecache_range(VFS_I(ip), start_boundary,
 					 end_boundary - 1);
+
 		/* convert the blocks */
 		error = xfs_alloc_file_space(ip, start_boundary,
 					end_boundary - start_boundary - 1,
diff --git a/fs/xfs/xfs_buf.c b/fs/xfs/xfs_buf.c
index 107f2fdfe41..cb10a0aaab3 100644
--- a/fs/xfs/xfs_buf.c
+++ b/fs/xfs/xfs_buf.c
@@ -1372,21 +1372,29 @@ xfs_buf_iorequest(
 		xfs_buf_wait_unpin(bp);
 	xfs_buf_hold(bp);
 
-	/* Set the count to 1 initially, this will stop an I/O
+	/*
+	 * Set the count to 1 initially, this will stop an I/O
 	 * completion callout which happens before we have started
 	 * all the I/O from calling xfs_buf_ioend too early.
 	 */
 	atomic_set(&bp->b_io_remaining, 1);
 	_xfs_buf_ioapply(bp);
-	_xfs_buf_ioend(bp, 1);
+	/*
+	 * If _xfs_buf_ioapply failed, we'll get back here with
+	 * only the reference we took above.  _xfs_buf_ioend will
+	 * drop it to zero, so we'd better not queue it for later,
+	 * or we'll free it before it's done.
+	 */
+	_xfs_buf_ioend(bp, bp->b_error ? 0 : 1);
 
 	xfs_buf_rele(bp);
 }
 
 /*
  * Waits for I/O to complete on the buffer supplied.  It returns immediately if
- * no I/O is pending or there is already a pending error on the buffer.  It
- * returns the I/O error code, if any, or 0 if there was no error.
+ * no I/O is pending or there is already a pending error on the buffer, in which
+ * case nothing will ever complete.  It returns the I/O error code, if any, or
+ * 0 if there was no error.
  */
 int
 xfs_buf_iowait(
diff --git a/fs/xfs/xfs_da_btree.h b/fs/xfs/xfs_da_btree.h
index 6e95ea79f5d..201c6091d26 100644
--- a/fs/xfs/xfs_da_btree.h
+++ b/fs/xfs/xfs_da_btree.h
@@ -60,10 +60,12 @@ typedef struct xfs_da_args {
 	int		index;		/* index of attr of interest in blk */
 	xfs_dablk_t	rmtblkno;	/* remote attr value starting blkno */
 	int		rmtblkcnt;	/* remote attr value block count */
+	int		rmtvaluelen;	/* remote attr value length in bytes */
 	xfs_dablk_t	blkno2;		/* blkno of 2nd attr leaf of interest */
 	int		index2;		/* index of 2nd attr in blk */
 	xfs_dablk_t	rmtblkno2;	/* remote attr value starting blkno */
 	int		rmtblkcnt2;	/* remote attr value block count */
+	int		rmtvaluelen2;	/* remote attr value length in bytes */
 	int		op_flags;	/* operation flags */
 	enum xfs_dacmp	cmpresult;	/* name compare result for lookups */
 } xfs_da_args_t;
diff --git a/fs/xfs/xfs_export.c b/fs/xfs/xfs_export.c
index 1399e187d42..753e467aa1a 100644
--- a/fs/xfs/xfs_export.c
+++ b/fs/xfs/xfs_export.c
@@ -237,7 +237,7 @@ xfs_fs_nfs_commit_metadata(
 
 	if (!lsn)
 		return 0;
-	return _xfs_log_force_lsn(mp, lsn, XFS_LOG_SYNC, NULL);
+	return -_xfs_log_force_lsn(mp, lsn, XFS_LOG_SYNC, NULL);
 }
 
 const struct export_operations xfs_export_operations = {
diff --git a/fs/xfs/xfs_file.c b/fs/xfs/xfs_file.c
index 79e96ce9873..830c1c937b8 100644
--- a/fs/xfs/xfs_file.c
+++ b/fs/xfs/xfs_file.c
@@ -155,7 +155,7 @@ xfs_dir_fsync(
 
 	if (!lsn)
 		return 0;
-	return _xfs_log_force_lsn(mp, lsn, XFS_LOG_SYNC, NULL);
+	return -_xfs_log_force_lsn(mp, lsn, XFS_LOG_SYNC, NULL);
 }
 
 STATIC int
@@ -295,7 +295,7 @@ xfs_file_aio_read(
 		xfs_rw_ilock(ip, XFS_IOLOCK_EXCL);
 
 		if (inode->i_mapping->nrpages) {
-			ret = -filemap_write_and_wait_range(
+			ret = filemap_write_and_wait_range(
 							VFS_I(ip)->i_mapping,
 							pos, -1);
 			if (ret) {
@@ -679,7 +679,7 @@ xfs_file_dio_aio_write(
 		goto out;
 
 	if (mapping->nrpages) {
-		ret = -filemap_write_and_wait_range(VFS_I(ip)->i_mapping,
+		ret = filemap_write_and_wait_range(VFS_I(ip)->i_mapping,
 						    pos, -1);
 		if (ret)
 			goto out;
@@ -837,11 +837,19 @@ xfs_file_fallocate(
 		unsigned blksize_mask = (1 << inode->i_blkbits) - 1;
 
 		if (offset & blksize_mask || len & blksize_mask) {
-			error = -EINVAL;
+			error = EINVAL;
+			goto out_unlock;
+		}
+
+		/*
+		 * There is no need to overlap collapse range with EOF,
+		 * in which case it is effectively a truncate operation
+		 */
+		if (offset + len >= i_size_read(inode)) {
+			error = EINVAL;
 			goto out_unlock;
 		}
 
-		ASSERT(offset + len < i_size_read(inode));
 		new_size = i_size_read(inode) - len;
 
 		error = xfs_collapse_file_space(ip, offset, len);
diff --git a/fs/xfs/xfs_inode.c b/fs/xfs/xfs_inode.c
index 5e7a38fa6ee..768087bedba 100644
--- a/fs/xfs/xfs_inode.c
+++ b/fs/xfs/xfs_inode.c
@@ -1334,7 +1334,8 @@ int
 xfs_create_tmpfile(
 	struct xfs_inode	*dp,
 	struct dentry		*dentry,
-	umode_t			mode)
+	umode_t			mode,
+	struct xfs_inode	**ipp)
 {
 	struct xfs_mount	*mp = dp->i_mount;
 	struct xfs_inode	*ip = NULL;
@@ -1402,7 +1403,6 @@ xfs_create_tmpfile(
 	xfs_qm_vop_create_dqattach(tp, ip, udqp, gdqp, pdqp);
 
 	ip->i_d.di_nlink--;
-	d_tmpfile(dentry, VFS_I(ip));
 	error = xfs_iunlink(tp, ip);
 	if (error)
 		goto out_trans_abort;
@@ -1415,6 +1415,7 @@ xfs_create_tmpfile(
 	xfs_qm_dqrele(gdqp);
 	xfs_qm_dqrele(pdqp);
 
+	*ipp = ip;
 	return 0;
 
  out_trans_abort:
diff --git a/fs/xfs/xfs_inode.h b/fs/xfs/xfs_inode.h
index 396cc1fafd0..f2fcde52b66 100644
--- a/fs/xfs/xfs_inode.h
+++ b/fs/xfs/xfs_inode.h
@@ -334,7 +334,7 @@ int		xfs_lookup(struct xfs_inode *dp, struct xfs_name *name,
 int		xfs_create(struct xfs_inode *dp, struct xfs_name *name,
 			   umode_t mode, xfs_dev_t rdev, struct xfs_inode **ipp);
 int		xfs_create_tmpfile(struct xfs_inode *dp, struct dentry *dentry,
-			   umode_t mode);
+			   umode_t mode, struct xfs_inode **ipp);
 int		xfs_remove(struct xfs_inode *dp, struct xfs_name *name,
 			   struct xfs_inode *ip);
 int		xfs_link(struct xfs_inode *tdp, struct xfs_inode *sip,
diff --git a/fs/xfs/xfs_iops.c b/fs/xfs/xfs_iops.c
index 89b07e43ca2..36d630319a2 100644
--- a/fs/xfs/xfs_iops.c
+++ b/fs/xfs/xfs_iops.c
@@ -72,8 +72,8 @@ xfs_initxattrs(
 	int			error = 0;
 
 	for (xattr = xattr_array; xattr->name != NULL; xattr++) {
-		error = xfs_attr_set(ip, xattr->name, xattr->value,
-				     xattr->value_len, ATTR_SECURE);
+		error = -xfs_attr_set(ip, xattr->name, xattr->value,
+				      xattr->value_len, ATTR_SECURE);
 		if (error < 0)
 			break;
 	}
@@ -93,8 +93,8 @@ xfs_init_security(
 	struct inode	*dir,
 	const struct qstr *qstr)
 {
-	return security_inode_init_security(inode, dir, qstr,
-					    &xfs_initxattrs, NULL);
+	return -security_inode_init_security(inode, dir, qstr,
+					     &xfs_initxattrs, NULL);
 }
 
 static void
@@ -124,15 +124,15 @@ xfs_cleanup_inode(
 	xfs_dentry_to_name(&teardown, dentry, 0);
 
 	xfs_remove(XFS_I(dir), &teardown, XFS_I(inode));
-	iput(inode);
 }
 
 STATIC int
-xfs_vn_mknod(
+xfs_generic_create(
 	struct inode	*dir,
 	struct dentry	*dentry,
 	umode_t		mode,
-	dev_t		rdev)
+	dev_t		rdev,
+	bool		tmpfile)	/* unnamed file */
 {
 	struct inode	*inode;
 	struct xfs_inode *ip = NULL;
@@ -156,8 +156,12 @@ xfs_vn_mknod(
 	if (error)
 		return error;
 
-	xfs_dentry_to_name(&name, dentry, mode);
-	error = xfs_create(XFS_I(dir), &name, mode, rdev, &ip);
+	if (!tmpfile) {
+		xfs_dentry_to_name(&name, dentry, mode);
+		error = xfs_create(XFS_I(dir), &name, mode, rdev, &ip);
+	} else {
+		error = xfs_create_tmpfile(XFS_I(dir), dentry, mode, &ip);
+	}
 	if (unlikely(error))
 		goto out_free_acl;
 
@@ -169,18 +173,22 @@ xfs_vn_mknod(
 
 #ifdef CONFIG_XFS_POSIX_ACL
 	if (default_acl) {
-		error = xfs_set_acl(inode, default_acl, ACL_TYPE_DEFAULT);
+		error = -xfs_set_acl(inode, default_acl, ACL_TYPE_DEFAULT);
 		if (error)
 			goto out_cleanup_inode;
 	}
 	if (acl) {
-		error = xfs_set_acl(inode, acl, ACL_TYPE_ACCESS);
+		error = -xfs_set_acl(inode, acl, ACL_TYPE_ACCESS);
 		if (error)
 			goto out_cleanup_inode;
 	}
 #endif
 
-	d_instantiate(dentry, inode);
+	if (tmpfile)
+		d_tmpfile(dentry, inode);
+	else
+		d_instantiate(dentry, inode);
+
  out_free_acl:
 	if (default_acl)
 		posix_acl_release(default_acl);
@@ -189,11 +197,23 @@ xfs_vn_mknod(
 	return -error;
 
  out_cleanup_inode:
-	xfs_cleanup_inode(dir, inode, dentry);
+	if (!tmpfile)
+		xfs_cleanup_inode(dir, inode, dentry);
+	iput(inode);
 	goto out_free_acl;
 }
 
 STATIC int
+xfs_vn_mknod(
+	struct inode	*dir,
+	struct dentry	*dentry,
+	umode_t		mode,
+	dev_t		rdev)
+{
+	return xfs_generic_create(dir, dentry, mode, rdev, false);
+}
+
+STATIC int
 xfs_vn_create(
 	struct inode	*dir,
 	struct dentry	*dentry,
@@ -353,6 +373,7 @@ xfs_vn_symlink(
 
  out_cleanup_inode:
 	xfs_cleanup_inode(dir, inode, dentry);
+	iput(inode);
  out:
 	return -error;
 }
@@ -1053,11 +1074,7 @@ xfs_vn_tmpfile(
 	struct dentry	*dentry,
 	umode_t		mode)
 {
-	int		error;
-
-	error = xfs_create_tmpfile(XFS_I(dir), dentry, mode);
-
-	return -error;
+	return xfs_generic_create(dir, dentry, mode, 0, true);
 }
 
 static const struct inode_operations xfs_inode_operations = {
diff --git a/fs/xfs/xfs_log.c b/fs/xfs/xfs_log.c
index 8497a00e399..a5f8bd9899d 100644
--- a/fs/xfs/xfs_log.c
+++ b/fs/xfs/xfs_log.c
@@ -616,11 +616,13 @@ xfs_log_mount(
 	int		error = 0;
 	int		min_logfsbs;
 
-	if (!(mp->m_flags & XFS_MOUNT_NORECOVERY))
-		xfs_notice(mp, "Mounting Filesystem");
-	else {
+	if (!(mp->m_flags & XFS_MOUNT_NORECOVERY)) {
+		xfs_notice(mp, "Mounting V%d Filesystem",
+			   XFS_SB_VERSION_NUM(&mp->m_sb));
+	} else {
 		xfs_notice(mp,
-"Mounting filesystem in no-recovery mode.  Filesystem will be inconsistent.");
+"Mounting V%d filesystem in no-recovery mode. Filesystem will be inconsistent.",
+			   XFS_SB_VERSION_NUM(&mp->m_sb));
 		ASSERT(mp->m_flags & XFS_MOUNT_RDONLY);
 	}
 
@@ -1181,11 +1183,14 @@ xlog_iodone(xfs_buf_t *bp)
 	/* log I/O is always issued ASYNC */
 	ASSERT(XFS_BUF_ISASYNC(bp));
 	xlog_state_done_syncing(iclog, aborted);
+
 	/*
-	 * do not reference the buffer (bp) here as we could race
-	 * with it being freed after writing the unmount record to the
-	 * log.
+	 * drop the buffer lock now that we are done. Nothing references
+	 * the buffer after this, so an unmount waiting on this lock can now
+	 * tear it down safely. As such, it is unsafe to reference the buffer
+	 * (bp) after the unlock as we could race with it being freed.
 	 */
+	xfs_buf_unlock(bp);
 }
 
 /*
@@ -1368,8 +1373,16 @@ xlog_alloc_log(
 	bp = xfs_buf_alloc(mp->m_logdev_targp, 0, BTOBB(log->l_iclog_size), 0);
 	if (!bp)
 		goto out_free_log;
-	bp->b_iodone = xlog_iodone;
+
+	/*
+	 * The iclogbuf buffer locks are held over IO but we are not going to do
+	 * IO yet.  Hence unlock the buffer so that the log IO path can grab it
+	 * when appropriately.
+	 */
 	ASSERT(xfs_buf_islocked(bp));
+	xfs_buf_unlock(bp);
+
+	bp->b_iodone = xlog_iodone;
 	log->l_xbuf = bp;
 
 	spin_lock_init(&log->l_icloglock);
@@ -1398,6 +1411,9 @@ xlog_alloc_log(
 		if (!bp)
 			goto out_free_iclog;
 
+		ASSERT(xfs_buf_islocked(bp));
+		xfs_buf_unlock(bp);
+
 		bp->b_iodone = xlog_iodone;
 		iclog->ic_bp = bp;
 		iclog->ic_data = bp->b_addr;
@@ -1422,7 +1438,6 @@ xlog_alloc_log(
 		iclog->ic_callback_tail = &(iclog->ic_callback);
 		iclog->ic_datap = (char *)iclog->ic_data + log->l_iclog_hsize;
 
-		ASSERT(xfs_buf_islocked(iclog->ic_bp));
 		init_waitqueue_head(&iclog->ic_force_wait);
 		init_waitqueue_head(&iclog->ic_write_wait);
 
@@ -1631,6 +1646,12 @@ xlog_cksum(
  * we transition the iclogs to IOERROR state *after* flushing all existing
  * iclogs to disk. This is because we don't want anymore new transactions to be
  * started or completed afterwards.
+ *
+ * We lock the iclogbufs here so that we can serialise against IO completion
+ * during unmount. We might be processing a shutdown triggered during unmount,
+ * and that can occur asynchronously to the unmount thread, and hence we need to
+ * ensure that completes before tearing down the iclogbufs. Hence we need to
+ * hold the buffer lock across the log IO to acheive that.
  */
 STATIC int
 xlog_bdstrat(
@@ -1638,6 +1659,7 @@ xlog_bdstrat(
 {
 	struct xlog_in_core	*iclog = bp->b_fspriv;
 
+	xfs_buf_lock(bp);
 	if (iclog->ic_state & XLOG_STATE_IOERROR) {
 		xfs_buf_ioerror(bp, EIO);
 		xfs_buf_stale(bp);
@@ -1645,7 +1667,8 @@ xlog_bdstrat(
 		/*
 		 * It would seem logical to return EIO here, but we rely on
 		 * the log state machine to propagate I/O errors instead of
-		 * doing it here.
+		 * doing it here. Similarly, IO completion will unlock the
+		 * buffer, so we don't do it here.
 		 */
 		return 0;
 	}
@@ -1847,14 +1870,28 @@ xlog_dealloc_log(
 	xlog_cil_destroy(log);
 
 	/*
-	 * always need to ensure that the extra buffer does not point to memory
-	 * owned by another log buffer before we free it.
+	 * Cycle all the iclogbuf locks to make sure all log IO completion
+	 * is done before we tear down these buffers.
 	 */
+	iclog = log->l_iclog;
+	for (i = 0; i < log->l_iclog_bufs; i++) {
+		xfs_buf_lock(iclog->ic_bp);
+		xfs_buf_unlock(iclog->ic_bp);
+		iclog = iclog->ic_next;
+	}
+
+	/*
+	 * Always need to ensure that the extra buffer does not point to memory
+	 * owned by another log buffer before we free it. Also, cycle the lock
+	 * first to ensure we've completed IO on it.
+	 */
+	xfs_buf_lock(log->l_xbuf);
+	xfs_buf_unlock(log->l_xbuf);
 	xfs_buf_set_empty(log->l_xbuf, BTOBB(log->l_iclog_size));
 	xfs_buf_free(log->l_xbuf);
 
 	iclog = log->l_iclog;
-	for (i=0; i<log->l_iclog_bufs; i++) {
+	for (i = 0; i < log->l_iclog_bufs; i++) {
 		xfs_buf_free(iclog->ic_bp);
 		next_iclog = iclog->ic_next;
 		kmem_free(iclog);
diff --git a/fs/xfs/xfs_mount.c b/fs/xfs/xfs_mount.c
index 993cb19e7d3..944f3d9456a 100644
--- a/fs/xfs/xfs_mount.c
+++ b/fs/xfs/xfs_mount.c
@@ -743,8 +743,6 @@ xfs_mountfs(
 		new_size *= mp->m_sb.sb_inodesize / XFS_DINODE_MIN_SIZE;
 		if (mp->m_sb.sb_inoalignmt >= XFS_B_TO_FSBT(mp, new_size))
 			mp->m_inode_cluster_size = new_size;
-		xfs_info(mp, "Using inode cluster size of %d bytes",
-			 mp->m_inode_cluster_size);
 	}
 
 	/*
diff --git a/fs/xfs/xfs_qm.c b/fs/xfs/xfs_qm.c
index 348e4d2ed6e..dc977b6e6a3 100644
--- a/fs/xfs/xfs_qm.c
+++ b/fs/xfs/xfs_qm.c
@@ -843,22 +843,17 @@ xfs_qm_init_quotainfo(
 
 	qinf = mp->m_quotainfo = kmem_zalloc(sizeof(xfs_quotainfo_t), KM_SLEEP);
 
-	if ((error = list_lru_init(&qinf->qi_lru))) {
-		kmem_free(qinf);
-		mp->m_quotainfo = NULL;
-		return error;
-	}
+	error = -list_lru_init(&qinf->qi_lru);
+	if (error)
+		goto out_free_qinf;
 
 	/*
 	 * See if quotainodes are setup, and if not, allocate them,
 	 * and change the superblock accordingly.
 	 */
-	if ((error = xfs_qm_init_quotainos(mp))) {
-		list_lru_destroy(&qinf->qi_lru);
-		kmem_free(qinf);
-		mp->m_quotainfo = NULL;
-		return error;
-	}
+	error = xfs_qm_init_quotainos(mp);
+	if (error)
+		goto out_free_lru;
 
 	INIT_RADIX_TREE(&qinf->qi_uquota_tree, GFP_NOFS);
 	INIT_RADIX_TREE(&qinf->qi_gquota_tree, GFP_NOFS);
@@ -918,7 +913,7 @@ xfs_qm_init_quotainfo(
 		qinf->qi_isoftlimit = be64_to_cpu(ddqp->d_ino_softlimit);
 		qinf->qi_rtbhardlimit = be64_to_cpu(ddqp->d_rtb_hardlimit);
 		qinf->qi_rtbsoftlimit = be64_to_cpu(ddqp->d_rtb_softlimit);
- 
+
 		xfs_qm_dqdestroy(dqp);
 	} else {
 		qinf->qi_btimelimit = XFS_QM_BTIMELIMIT;
@@ -935,6 +930,13 @@ xfs_qm_init_quotainfo(
 	qinf->qi_shrinker.flags = SHRINKER_NUMA_AWARE;
 	register_shrinker(&qinf->qi_shrinker);
 	return 0;
+
+out_free_lru:
+	list_lru_destroy(&qinf->qi_lru);
+out_free_qinf:
+	kmem_free(qinf);
+	mp->m_quotainfo = NULL;
+	return error;
 }
 
 
diff --git a/fs/xfs/xfs_sb.c b/fs/xfs/xfs_sb.c
index 0c0e41bbe4e..8baf61afae1 100644
--- a/fs/xfs/xfs_sb.c
+++ b/fs/xfs/xfs_sb.c
@@ -201,10 +201,6 @@ xfs_mount_validate_sb(
 	 * write validation, we don't need to check feature masks.
 	 */
 	if (check_version && XFS_SB_VERSION_NUM(sbp) == XFS_SB_VERSION_5) {
-		xfs_alert(mp,
-"Version 5 superblock detected. This kernel has EXPERIMENTAL support enabled!\n"
-"Use of these features in this kernel is at your own risk!");
-
 		if (xfs_sb_has_compat_feature(sbp,
 					XFS_SB_FEAT_COMPAT_UNKNOWN)) {
 			xfs_warn(mp,
diff --git a/fs/xfs/xfs_super.c b/fs/xfs/xfs_super.c
index 20537677637..3494eff8e4e 100644
--- a/fs/xfs/xfs_super.c
+++ b/fs/xfs/xfs_super.c
@@ -1433,11 +1433,11 @@ xfs_fs_fill_super(
 	if (error)
 		goto out_free_fsname;
 
-	error = xfs_init_mount_workqueues(mp);
+	error = -xfs_init_mount_workqueues(mp);
 	if (error)
 		goto out_close_devices;
 
-	error = xfs_icsb_init_counters(mp);
+	error = -xfs_icsb_init_counters(mp);
 	if (error)
 		goto out_destroy_workqueues;
 
diff --git a/fs/xfs/xfs_trace.h b/fs/xfs/xfs_trace.h
index a4ae41c179a..65d8c793a25 100644
--- a/fs/xfs/xfs_trace.h
+++ b/fs/xfs/xfs_trace.h
@@ -603,6 +603,7 @@ DEFINE_INODE_EVENT(xfs_readlink);
 DEFINE_INODE_EVENT(xfs_inactive_symlink);
 DEFINE_INODE_EVENT(xfs_alloc_file_space);
 DEFINE_INODE_EVENT(xfs_free_file_space);
+DEFINE_INODE_EVENT(xfs_zero_file_space);
 DEFINE_INODE_EVENT(xfs_collapse_file_space);
 DEFINE_INODE_EVENT(xfs_readdir);
 #ifdef CONFIG_XFS_POSIX_ACL
diff --git a/include/asm-generic/dma-coherent.h b/include/asm-generic/dma-coherent.h
index 2be8a2dbc86..0297e587579 100644
--- a/include/asm-generic/dma-coherent.h
+++ b/include/asm-generic/dma-coherent.h
@@ -16,16 +16,13 @@ int dma_mmap_from_coherent(struct device *dev, struct vm_area_struct *vma,
  * Standard interface
  */
 #define ARCH_HAS_DMA_DECLARE_COHERENT_MEMORY
-extern int
-dma_declare_coherent_memory(struct device *dev, dma_addr_t bus_addr,
-			    dma_addr_t device_addr, size_t size, int flags);
+int dma_declare_coherent_memory(struct device *dev, phys_addr_t phys_addr,
+				dma_addr_t device_addr, size_t size, int flags);
 
-extern void
-dma_release_declared_memory(struct device *dev);
+void dma_release_declared_memory(struct device *dev);
 
-extern void *
-dma_mark_declared_memory_occupied(struct device *dev,
-				  dma_addr_t device_addr, size_t size);
+void *dma_mark_declared_memory_occupied(struct device *dev,
+					dma_addr_t device_addr, size_t size);
 #else
 #define dma_alloc_from_coherent(dev, size, handle, ret) (0)
 #define dma_release_from_coherent(dev, order, vaddr) (0)
diff --git a/include/asm-generic/fixmap.h b/include/asm-generic/fixmap.h
index 5a64ca4621f..f23174fb9ec 100644
--- a/include/asm-generic/fixmap.h
+++ b/include/asm-generic/fixmap.h
@@ -93,5 +93,8 @@ static inline unsigned long virt_to_fix(const unsigned long vaddr)
 #define set_fixmap_io(idx, phys) \
 	__set_fixmap(idx, phys, FIXMAP_PAGE_IO)
 
+#define set_fixmap_offset_io(idx, phys) \
+	__set_fixmap_offset(idx, phys, FIXMAP_PAGE_IO)
+
 #endif /* __ASSEMBLY__ */
 #endif /* __ASM_GENERIC_FIXMAP_H */
diff --git a/include/asm-generic/pgtable.h b/include/asm-generic/pgtable.h
index 1ec08c198b6..a8015a7a55b 100644
--- a/include/asm-generic/pgtable.h
+++ b/include/asm-generic/pgtable.h
@@ -693,24 +693,35 @@ static inline int pmd_numa(pmd_t pmd)
 #ifndef pte_mknonnuma
 static inline pte_t pte_mknonnuma(pte_t pte)
 {
-	pte = pte_clear_flags(pte, _PAGE_NUMA);
-	return pte_set_flags(pte, _PAGE_PRESENT|_PAGE_ACCESSED);
+	pteval_t val = pte_val(pte);
+
+	val &= ~_PAGE_NUMA;
+	val |= (_PAGE_PRESENT|_PAGE_ACCESSED);
+	return __pte(val);
 }
 #endif
 
 #ifndef pmd_mknonnuma
 static inline pmd_t pmd_mknonnuma(pmd_t pmd)
 {
-	pmd = pmd_clear_flags(pmd, _PAGE_NUMA);
-	return pmd_set_flags(pmd, _PAGE_PRESENT|_PAGE_ACCESSED);
+	pmdval_t val = pmd_val(pmd);
+
+	val &= ~_PAGE_NUMA;
+	val |= (_PAGE_PRESENT|_PAGE_ACCESSED);
+
+	return __pmd(val);
 }
 #endif
 
 #ifndef pte_mknuma
 static inline pte_t pte_mknuma(pte_t pte)
 {
-	pte = pte_set_flags(pte, _PAGE_NUMA);
-	return pte_clear_flags(pte, _PAGE_PRESENT);
+	pteval_t val = pte_val(pte);
+
+	val &= ~_PAGE_PRESENT;
+	val |= _PAGE_NUMA;
+
+	return __pte(val);
 }
 #endif
 
@@ -729,8 +740,12 @@ static inline void ptep_set_numa(struct mm_struct *mm, unsigned long addr,
 #ifndef pmd_mknuma
 static inline pmd_t pmd_mknuma(pmd_t pmd)
 {
-	pmd = pmd_set_flags(pmd, _PAGE_NUMA);
-	return pmd_clear_flags(pmd, _PAGE_PRESENT);
+	pmdval_t val = pmd_val(pmd);
+
+	val &= ~_PAGE_PRESENT;
+	val |= _PAGE_NUMA;
+
+	return __pmd(val);
 }
 #endif
 
diff --git a/include/asm-generic/resource.h b/include/asm-generic/resource.h
index b4ea8f50fc6..5e752b95905 100644
--- a/include/asm-generic/resource.h
+++ b/include/asm-generic/resource.h
@@ -12,7 +12,7 @@
 	[RLIMIT_CPU]		= {  RLIM_INFINITY,  RLIM_INFINITY },	\
 	[RLIMIT_FSIZE]		= {  RLIM_INFINITY,  RLIM_INFINITY },	\
 	[RLIMIT_DATA]		= {  RLIM_INFINITY,  RLIM_INFINITY },	\
-	[RLIMIT_STACK]		= {       _STK_LIM,   _STK_LIM_MAX },	\
+	[RLIMIT_STACK]		= {       _STK_LIM,  RLIM_INFINITY },	\
 	[RLIMIT_CORE]		= {              0,  RLIM_INFINITY },	\
 	[RLIMIT_RSS]		= {  RLIM_INFINITY,  RLIM_INFINITY },	\
 	[RLIMIT_NPROC]		= {              0,              0 },	\
diff --git a/include/asm-generic/word-at-a-time.h b/include/asm-generic/word-at-a-time.h
index d3909effd72..94f9ea8abca 100644
--- a/include/asm-generic/word-at-a-time.h
+++ b/include/asm-generic/word-at-a-time.h
@@ -50,11 +50,7 @@ static inline bool has_zero(unsigned long val, unsigned long *data, const struct
 }
 
 #ifndef zero_bytemask
-#ifdef CONFIG_64BIT
-#define zero_bytemask(mask)	(~0ul << fls64(mask))
-#else
-#define zero_bytemask(mask)	(~0ul << fls(mask))
-#endif /* CONFIG_64BIT */
-#endif /* zero_bytemask */
+#define zero_bytemask(mask) (~1ul << __fls(mask))
+#endif
 
 #endif /* _ASM_WORD_AT_A_TIME_H */
diff --git a/include/drm/drm_crtc_helper.h b/include/drm/drm_crtc_helper.h
index 0bb34ca2ad2..36a5febac2a 100644
--- a/include/drm/drm_crtc_helper.h
+++ b/include/drm/drm_crtc_helper.h
@@ -125,7 +125,6 @@ struct drm_connector_helper_funcs {
 	struct drm_encoder *(*best_encoder)(struct drm_connector *connector);
 };
 
-extern int drm_helper_probe_single_connector_modes(struct drm_connector *connector, uint32_t maxX, uint32_t maxY);
 extern void drm_helper_disable_unused_functions(struct drm_device *dev);
 extern int drm_crtc_helper_set_config(struct drm_mode_set *set);
 extern bool drm_crtc_helper_set_mode(struct drm_crtc *crtc,
@@ -161,6 +160,11 @@ static inline void drm_connector_helper_add(struct drm_connector *connector,
 }
 
 extern void drm_helper_resume_force_mode(struct drm_device *dev);
+
+/* drm_probe_helper.c */
+extern int drm_helper_probe_single_connector_modes(struct drm_connector
+						   *connector, uint32_t maxX,
+						   uint32_t maxY);
 extern void drm_kms_helper_poll_init(struct drm_device *dev);
 extern void drm_kms_helper_poll_fini(struct drm_device *dev);
 extern bool drm_helper_hpd_irq_event(struct drm_device *dev);
diff --git a/include/drm/drm_dp_helper.h b/include/drm/drm_dp_helper.h
index b4f58914bf7..cfcacec5b89 100644
--- a/include/drm/drm_dp_helper.h
+++ b/include/drm/drm_dp_helper.h
@@ -456,6 +456,10 @@ struct drm_dp_aux_msg {
  * transactions. The drm_dp_aux_register_i2c_bus() function registers an
  * I2C adapter that can be passed to drm_probe_ddc(). Upon removal, drivers
  * should call drm_dp_aux_unregister_i2c_bus() to remove the I2C adapter.
+ *
+ * Note that the aux helper code assumes that the .transfer() function
+ * only modifies the reply field of the drm_dp_aux_msg structure.  The
+ * retry logic and i2c helpers assume this is the case.
  */
 struct drm_dp_aux {
 	const char *name;
diff --git a/include/drm/drm_pciids.h b/include/drm/drm_pciids.h
index 49376aec2fb..6dfd64b3a60 100644
--- a/include/drm/drm_pciids.h
+++ b/include/drm/drm_pciids.h
@@ -637,6 +637,22 @@
 	{0x1002, 0x983d, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_KABINI|RADEON_NEW_MEMMAP|RADEON_IS_IGP}, \
 	{0x1002, 0x983e, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_KABINI|RADEON_NEW_MEMMAP|RADEON_IS_IGP}, \
 	{0x1002, 0x983f, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_KABINI|RADEON_NEW_MEMMAP|RADEON_IS_IGP}, \
+	{0x1002, 0x9850, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_MULLINS|RADEON_IS_MOBILITY|RADEON_NEW_MEMMAP|RADEON_IS_IGP}, \
+	{0x1002, 0x9851, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_MULLINS|RADEON_IS_MOBILITY|RADEON_NEW_MEMMAP|RADEON_IS_IGP}, \
+	{0x1002, 0x9852, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_MULLINS|RADEON_IS_MOBILITY|RADEON_NEW_MEMMAP|RADEON_IS_IGP}, \
+	{0x1002, 0x9853, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_MULLINS|RADEON_IS_MOBILITY|RADEON_NEW_MEMMAP|RADEON_IS_IGP}, \
+	{0x1002, 0x9854, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_MULLINS|RADEON_IS_MOBILITY|RADEON_NEW_MEMMAP|RADEON_IS_IGP}, \
+	{0x1002, 0x9855, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_MULLINS|RADEON_IS_MOBILITY|RADEON_NEW_MEMMAP|RADEON_IS_IGP}, \
+	{0x1002, 0x9856, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_MULLINS|RADEON_IS_MOBILITY|RADEON_NEW_MEMMAP|RADEON_IS_IGP}, \
+	{0x1002, 0x9857, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_MULLINS|RADEON_IS_MOBILITY|RADEON_NEW_MEMMAP|RADEON_IS_IGP}, \
+	{0x1002, 0x9858, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_MULLINS|RADEON_IS_MOBILITY|RADEON_NEW_MEMMAP|RADEON_IS_IGP}, \
+	{0x1002, 0x9859, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_MULLINS|RADEON_IS_MOBILITY|RADEON_NEW_MEMMAP|RADEON_IS_IGP}, \
+	{0x1002, 0x985A, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_MULLINS|RADEON_IS_MOBILITY|RADEON_NEW_MEMMAP|RADEON_IS_IGP}, \
+	{0x1002, 0x985B, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_MULLINS|RADEON_IS_MOBILITY|RADEON_NEW_MEMMAP|RADEON_IS_IGP}, \
+	{0x1002, 0x985C, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_MULLINS|RADEON_IS_MOBILITY|RADEON_NEW_MEMMAP|RADEON_IS_IGP}, \
+	{0x1002, 0x985D, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_MULLINS|RADEON_IS_MOBILITY|RADEON_NEW_MEMMAP|RADEON_IS_IGP}, \
+	{0x1002, 0x985E, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_MULLINS|RADEON_IS_MOBILITY|RADEON_NEW_MEMMAP|RADEON_IS_IGP}, \
+	{0x1002, 0x985F, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_MULLINS|RADEON_IS_MOBILITY|RADEON_NEW_MEMMAP|RADEON_IS_IGP}, \
 	{0x1002, 0x9900, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_ARUBA|RADEON_IS_MOBILITY|RADEON_NEW_MEMMAP|RADEON_IS_IGP}, \
 	{0x1002, 0x9901, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_ARUBA|RADEON_NEW_MEMMAP|RADEON_IS_IGP}, \
 	{0x1002, 0x9903, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_ARUBA|RADEON_IS_MOBILITY|RADEON_NEW_MEMMAP|RADEON_IS_IGP}, \
diff --git a/include/drm/i915_pciids.h b/include/drm/i915_pciids.h
index 940ece4934b..012d58fa8ff 100644
--- a/include/drm/i915_pciids.h
+++ b/include/drm/i915_pciids.h
@@ -191,8 +191,8 @@
 	INTEL_VGA_DEVICE(0x0A06, info), /* ULT GT1 mobile */ \
 	INTEL_VGA_DEVICE(0x0A16, info), /* ULT GT2 mobile */ \
 	INTEL_VGA_DEVICE(0x0A26, info), /* ULT GT3 mobile */ \
-	INTEL_VGA_DEVICE(0x0A0E, info), /* ULT GT1 reserved */ \
-	INTEL_VGA_DEVICE(0x0A1E, info), /* ULT GT2 reserved */ \
+	INTEL_VGA_DEVICE(0x0A0E, info), /* ULX GT1 mobile */ \
+	INTEL_VGA_DEVICE(0x0A1E, info), /* ULX GT2 mobile */ \
 	INTEL_VGA_DEVICE(0x0A2E, info), /* ULT GT3 reserved */ \
 	INTEL_VGA_DEVICE(0x0D06, info), /* CRW GT1 mobile */ \
 	INTEL_VGA_DEVICE(0x0D16, info), /* CRW GT2 mobile */ \
diff --git a/include/dt-bindings/clk/at91.h b/include/dt-bindings/clock/at91.h
index 0b4cb999a3f..0b4cb999a3f 100644
--- a/include/dt-bindings/clk/at91.h
+++ b/include/dt-bindings/clock/at91.h
diff --git a/include/dt-bindings/clock/berlin2.h b/include/dt-bindings/clock/berlin2.h
new file mode 100644
index 00000000000..0c30800175d
--- /dev/null
+++ b/include/dt-bindings/clock/berlin2.h
@@ -0,0 +1,45 @@
+/*
+ * Berlin2 BG2/BG2CD clock tree IDs
+ */
+
+#define CLKID_SYS		0
+#define CLKID_CPU		1
+#define CLKID_DRMFIGO		2
+#define CLKID_CFG		3
+#define CLKID_GFX		4
+#define CLKID_ZSP		5
+#define CLKID_PERIF		6
+#define CLKID_PCUBE		7
+#define CLKID_VSCOPE		8
+#define CLKID_NFC_ECC		9
+#define CLKID_VPP		10
+#define CLKID_APP		11
+#define CLKID_AUDIO0		12
+#define CLKID_AUDIO2		13
+#define CLKID_AUDIO3		14
+#define CLKID_AUDIO1		15
+#define CLKID_GFX3D_CORE	16
+#define CLKID_GFX3D_SYS		17
+#define CLKID_ARC		18
+#define CLKID_VIP		19
+#define CLKID_SDIO0XIN		20
+#define CLKID_SDIO1XIN		21
+#define CLKID_GFX3D_EXTRA	22
+#define CLKID_GC360		23
+#define CLKID_SDIO_DLLMST	24
+#define CLKID_GETH0		25
+#define CLKID_GETH1		26
+#define CLKID_SATA		27
+#define CLKID_AHBAPB		28
+#define CLKID_USB0		29
+#define CLKID_USB1		30
+#define CLKID_PBRIDGE		31
+#define CLKID_SDIO0		32
+#define CLKID_SDIO1		33
+#define CLKID_NFC		34
+#define CLKID_SMEMC		35
+#define CLKID_AUDIOHD		36
+#define CLKID_VIDEO0		37
+#define CLKID_VIDEO1		38
+#define CLKID_VIDEO2		39
+#define CLKID_TWD		40
diff --git a/include/dt-bindings/clock/berlin2q.h b/include/dt-bindings/clock/berlin2q.h
new file mode 100644
index 00000000000..287fc3b4afb
--- /dev/null
+++ b/include/dt-bindings/clock/berlin2q.h
@@ -0,0 +1,31 @@
+/*
+ * Berlin2 BG2Q clock tree IDs
+ */
+
+#define CLKID_SYS		0
+#define CLKID_DRMFIGO		1
+#define CLKID_CFG		2
+#define CLKID_GFX2D		3
+#define CLKID_ZSP		4
+#define CLKID_PERIF		5
+#define CLKID_PCUBE		6
+#define CLKID_VSCOPE		7
+#define CLKID_NFC_ECC		8
+#define CLKID_VPP		9
+#define CLKID_APP		10
+#define CLKID_SDIO0XIN		11
+#define CLKID_SDIO1XIN		12
+#define CLKID_GFX2DAXI		13
+#define CLKID_GETH0		14
+#define CLKID_SATA		15
+#define CLKID_AHBAPB		16
+#define CLKID_USB0		17
+#define CLKID_USB1		18
+#define CLKID_USB2		19
+#define CLKID_USB3		20
+#define CLKID_PBRIDGE		21
+#define CLKID_SDIO		22
+#define CLKID_NFC		23
+#define CLKID_SMEMC		24
+#define CLKID_PCIE		25
+#define CLKID_TWD		26
diff --git a/include/dt-bindings/clock/exynos3250.h b/include/dt-bindings/clock/exynos3250.h
new file mode 100644
index 00000000000..b535e9da7de
--- /dev/null
+++ b/include/dt-bindings/clock/exynos3250.h
@@ -0,0 +1,258 @@
+/*
+ * Copyright (c) 2014 Samsung Electronics Co., Ltd.
+ * 	Author: Tomasz Figa <t.figa@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.
+ *
+ * Device Tree binding constants for Samsung Exynos3250 clock controllers.
+ */
+
+#ifndef _DT_BINDINGS_CLOCK_SAMSUNG_EXYNOS3250_CLOCK_H
+#define _DT_BINDINGS_CLOCK_SAMSUNG_EXYNOS3250_CLOCK_H
+
+/*
+ * Let each exported clock get a unique index, which is used on DT-enabled
+ * platforms to lookup the clock from a clock specifier. These indices are
+ * therefore considered an ABI and so must not be changed. This implies
+ * that new clocks should be added either in free spaces between clock groups
+ * or at the end.
+ */
+
+
+/*
+ * Main CMU
+ */
+
+#define CLK_OSCSEL			1
+#define CLK_FIN_PLL			2
+#define CLK_FOUT_APLL			3
+#define CLK_FOUT_VPLL			4
+#define CLK_FOUT_UPLL			5
+#define CLK_FOUT_MPLL			6
+
+/* Muxes */
+#define CLK_MOUT_MPLL_USER_L		16
+#define CLK_MOUT_GDL			17
+#define CLK_MOUT_MPLL_USER_R		18
+#define CLK_MOUT_GDR			19
+#define CLK_MOUT_EBI			20
+#define CLK_MOUT_ACLK_200		21
+#define CLK_MOUT_ACLK_160		22
+#define CLK_MOUT_ACLK_100		23
+#define CLK_MOUT_ACLK_266_1		24
+#define CLK_MOUT_ACLK_266_0		25
+#define CLK_MOUT_ACLK_266		26
+#define CLK_MOUT_VPLL			27
+#define CLK_MOUT_EPLL_USER		28
+#define CLK_MOUT_EBI_1			29
+#define CLK_MOUT_UPLL			30
+#define CLK_MOUT_ACLK_400_MCUISP_SUB	31
+#define CLK_MOUT_MPLL			32
+#define CLK_MOUT_ACLK_400_MCUISP	33
+#define CLK_MOUT_VPLLSRC		34
+#define CLK_MOUT_CAM1			35
+#define CLK_MOUT_CAM_BLK		36
+#define CLK_MOUT_MFC			37
+#define CLK_MOUT_MFC_1			38
+#define CLK_MOUT_MFC_0			39
+#define CLK_MOUT_G3D			40
+#define CLK_MOUT_G3D_1			41
+#define CLK_MOUT_G3D_0			42
+#define CLK_MOUT_MIPI0			43
+#define CLK_MOUT_FIMD0			44
+#define CLK_MOUT_UART_ISP		45
+#define CLK_MOUT_SPI1_ISP		46
+#define CLK_MOUT_SPI0_ISP		47
+#define CLK_MOUT_TSADC			48
+#define CLK_MOUT_MMC1			49
+#define CLK_MOUT_MMC0			50
+#define CLK_MOUT_UART1			51
+#define CLK_MOUT_UART0			52
+#define CLK_MOUT_SPI1			53
+#define CLK_MOUT_SPI0			54
+#define CLK_MOUT_AUDIO			55
+#define CLK_MOUT_MPLL_USER_C		56
+#define CLK_MOUT_HPM			57
+#define CLK_MOUT_CORE			58
+#define CLK_MOUT_APLL			59
+#define CLK_MOUT_ACLK_266_SUB		60
+
+/* Dividers */
+#define CLK_DIV_GPL			64
+#define CLK_DIV_GDL			65
+#define CLK_DIV_GPR			66
+#define CLK_DIV_GDR			67
+#define CLK_DIV_MPLL_PRE		68
+#define CLK_DIV_ACLK_400_MCUISP		69
+#define CLK_DIV_EBI			70
+#define CLK_DIV_ACLK_200		71
+#define CLK_DIV_ACLK_160		72
+#define CLK_DIV_ACLK_100		73
+#define CLK_DIV_ACLK_266		74
+#define CLK_DIV_CAM1			75
+#define CLK_DIV_CAM_BLK			76
+#define CLK_DIV_MFC			77
+#define CLK_DIV_G3D			78
+#define CLK_DIV_MIPI0_PRE		79
+#define CLK_DIV_MIPI0			80
+#define CLK_DIV_FIMD0			81
+#define CLK_DIV_UART_ISP		82
+#define CLK_DIV_SPI1_ISP_PRE		83
+#define CLK_DIV_SPI1_ISP		84
+#define CLK_DIV_SPI0_ISP_PRE		85
+#define CLK_DIV_SPI0_ISP		86
+#define CLK_DIV_TSADC_PRE		87
+#define CLK_DIV_TSADC			88
+#define CLK_DIV_MMC1_PRE		89
+#define CLK_DIV_MMC1			90
+#define CLK_DIV_MMC0_PRE		91
+#define CLK_DIV_MMC0			92
+#define CLK_DIV_UART1			93
+#define CLK_DIV_UART0			94
+#define CLK_DIV_SPI1_PRE		95
+#define CLK_DIV_SPI1			96
+#define CLK_DIV_SPI0_PRE		97
+#define CLK_DIV_SPI0			98
+#define CLK_DIV_PCM			99
+#define CLK_DIV_AUDIO			100
+#define CLK_DIV_I2S			101
+#define CLK_DIV_CORE2			102
+#define CLK_DIV_APLL			103
+#define CLK_DIV_PCLK_DBG		104
+#define CLK_DIV_ATB			105
+#define CLK_DIV_COREM			106
+#define CLK_DIV_CORE			107
+#define CLK_DIV_HPM			108
+#define CLK_DIV_COPY			109
+
+/* Gates */
+#define CLK_ASYNC_G3D			128
+#define CLK_ASYNC_MFCL			129
+#define CLK_PPMULEFT			130
+#define CLK_GPIO_LEFT			131
+#define CLK_ASYNC_ISPMX			132
+#define CLK_ASYNC_FSYSD			133
+#define CLK_ASYNC_LCD0X			134
+#define CLK_ASYNC_CAMX			135
+#define CLK_PPMURIGHT			136
+#define CLK_GPIO_RIGHT			137
+#define CLK_MONOCNT			138
+#define CLK_TZPC6			139
+#define CLK_PROVISIONKEY1		140
+#define CLK_PROVISIONKEY0		141
+#define CLK_CMU_ISPPART			142
+#define CLK_TMU_APBIF			143
+#define CLK_KEYIF			144
+#define CLK_RTC				145
+#define CLK_WDT				146
+#define CLK_MCT				147
+#define CLK_SECKEY			148
+#define CLK_TZPC5			149
+#define CLK_TZPC4			150
+#define CLK_TZPC3			151
+#define CLK_TZPC2			152
+#define CLK_TZPC1			153
+#define CLK_TZPC0			154
+#define CLK_CMU_COREPART		155
+#define CLK_CMU_TOPPART			156
+#define CLK_PMU_APBIF			157
+#define CLK_SYSREG			158
+#define CLK_CHIP_ID			159
+#define CLK_QEJPEG			160
+#define CLK_PIXELASYNCM1		161
+#define CLK_PIXELASYNCM0		162
+#define CLK_PPMUCAMIF			163
+#define CLK_QEM2MSCALER			164
+#define CLK_QEGSCALER1			165
+#define CLK_QEGSCALER0			166
+#define CLK_SMMUJPEG			167
+#define CLK_SMMUM2M2SCALER		168
+#define CLK_SMMUGSCALER1		169
+#define CLK_SMMUGSCALER0		170
+#define CLK_JPEG			171
+#define CLK_M2MSCALER			172
+#define CLK_GSCALER1			173
+#define CLK_GSCALER0			174
+#define CLK_QEMFC			175
+#define CLK_PPMUMFC_L			176
+#define CLK_SMMUMFC_L			177
+#define CLK_MFC				178
+#define CLK_SMMUG3D			179
+#define CLK_QEG3D			180
+#define CLK_PPMUG3D			181
+#define CLK_G3D				182
+#define CLK_QE_CH1_LCD			183
+#define CLK_QE_CH0_LCD			184
+#define CLK_PPMULCD0			185
+#define CLK_SMMUFIMD0			186
+#define CLK_DSIM0			187
+#define CLK_FIMD0			188
+#define CLK_CAM1			189
+#define CLK_UART_ISP_TOP		190
+#define CLK_SPI1_ISP_TOP		191
+#define CLK_SPI0_ISP_TOP		192
+#define CLK_TSADC			193
+#define CLK_PPMUFILE			194
+#define CLK_USBOTG			195
+#define CLK_USBHOST			196
+#define CLK_SROMC			197
+#define CLK_SDMMC1			198
+#define CLK_SDMMC0			199
+#define CLK_PDMA1			200
+#define CLK_PDMA0			201
+#define CLK_PWM				202
+#define CLK_PCM				203
+#define CLK_I2S				204
+#define CLK_SPI1			205
+#define CLK_SPI0			206
+#define CLK_I2C7			207
+#define CLK_I2C6			208
+#define CLK_I2C5			209
+#define CLK_I2C4			210
+#define CLK_I2C3			211
+#define CLK_I2C2			212
+#define CLK_I2C1			213
+#define CLK_I2C0			214
+#define CLK_UART1			215
+#define CLK_UART0			216
+#define CLK_BLOCK_LCD			217
+#define CLK_BLOCK_G3D			218
+#define CLK_BLOCK_MFC			219
+#define CLK_BLOCK_CAM			220
+#define CLK_SMIES			221
+
+/* Special clocks */
+#define CLK_SCLK_JPEG			224
+#define CLK_SCLK_M2MSCALER		225
+#define CLK_SCLK_GSCALER1		226
+#define CLK_SCLK_GSCALER0		227
+#define CLK_SCLK_MFC			228
+#define CLK_SCLK_G3D			229
+#define CLK_SCLK_MIPIDPHY2L		230
+#define CLK_SCLK_MIPI0			231
+#define CLK_SCLK_FIMD0			232
+#define CLK_SCLK_CAM1			233
+#define CLK_SCLK_UART_ISP		234
+#define CLK_SCLK_SPI1_ISP		235
+#define CLK_SCLK_SPI0_ISP		236
+#define CLK_SCLK_UPLL			237
+#define CLK_SCLK_TSADC			238
+#define CLK_SCLK_EBI			239
+#define CLK_SCLK_MMC1			240
+#define CLK_SCLK_MMC0			241
+#define CLK_SCLK_I2S			242
+#define CLK_SCLK_PCM			243
+#define CLK_SCLK_SPI1			244
+#define CLK_SCLK_SPI0			245
+#define CLK_SCLK_UART1			246
+#define CLK_SCLK_UART0			247
+
+/*
+ * Total number of clocks of main CMU.
+ * NOTE: Must be equal to last clock ID increased by one.
+ */
+#define CLK_NR_CLKS			248
+
+#endif /* _DT_BINDINGS_CLOCK_SAMSUNG_EXYNOS3250_CLOCK_H */
diff --git a/include/dt-bindings/clock/exynos4.h b/include/dt-bindings/clock/exynos4.h
index 75aff336dfb..1106ca540a9 100644
--- a/include/dt-bindings/clock/exynos4.h
+++ b/include/dt-bindings/clock/exynos4.h
@@ -33,6 +33,7 @@
 #define CLK_MOUT_MPLL_USER_C	18 /* Exynos4x12 only */
 #define CLK_MOUT_CORE		19
 #define CLK_MOUT_APLL		20
+#define CLK_SCLK_HDMIPHY	22
 
 /* gate for special clocks (sclk) */
 #define CLK_SCLK_FIMC0		128
@@ -181,7 +182,6 @@
 #define CLK_KEYIF		347
 #define CLK_AUDSS		348
 #define CLK_MIPI_HSI		349 /* Exynos4210 only */
-#define CLK_MDMA2		350 /* Exynos4210 only */
 #define CLK_PIXELASYNCM0	351
 #define CLK_PIXELASYNCM1	352
 #define CLK_FIMC_LITE0		353 /* Exynos4x12 only */
diff --git a/include/dt-bindings/clock/exynos5250.h b/include/dt-bindings/clock/exynos5250.h
index 922f2dca9bf..be6e97c54f5 100644
--- a/include/dt-bindings/clock/exynos5250.h
+++ b/include/dt-bindings/clock/exynos5250.h
@@ -150,11 +150,30 @@
 #define CLK_G2D			345
 #define CLK_MDMA0		346
 #define CLK_SMMU_MDMA0		347
+#define CLK_SSS			348
+#define CLK_G3D			349
+#define CLK_SMMU_TV		350
+#define CLK_SMMU_FIMD1		351
+#define CLK_SMMU_2D		352
+#define CLK_SMMU_FIMC_ISP	353
+#define CLK_SMMU_FIMC_DRC	354
+#define CLK_SMMU_FIMC_SCC	355
+#define CLK_SMMU_FIMC_SCP	356
+#define CLK_SMMU_FIMC_FD	357
+#define CLK_SMMU_FIMC_MCU	358
+#define CLK_SMMU_FIMC_ODC	359
+#define CLK_SMMU_FIMC_DIS0	360
+#define CLK_SMMU_FIMC_DIS1	361
+#define CLK_SMMU_FIMC_3DNR	362
+#define CLK_SMMU_FIMC_LITE0	363
+#define CLK_SMMU_FIMC_LITE1	364
+#define CLK_CAMIF_TOP		365
 
 /* mux clocks */
 #define CLK_MOUT_HDMI		1024
+#define CLK_MOUT_GPLL		1025
 
 /* must be greater than maximal clock id */
-#define CLK_NR_CLKS		1025
+#define CLK_NR_CLKS		1026
 
 #endif /* _DT_BINDINGS_CLOCK_EXYNOS_5250_H */
diff --git a/include/dt-bindings/clock/exynos5260-clk.h b/include/dt-bindings/clock/exynos5260-clk.h
new file mode 100644
index 00000000000..a4bac9a1764
--- /dev/null
+++ b/include/dt-bindings/clock/exynos5260-clk.h
@@ -0,0 +1,469 @@
+/*
+ * Copyright (c) 2014 Samsung Electronics Co., Ltd.
+ * Author: Rahul Sharma <rahul.sharma@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.
+ *
+ * Provides Constants for Exynos5260 clocks.
+*/
+
+#ifndef _DT_BINDINGS_CLK_EXYNOS5260_H
+#define _DT_BINDINGS_CLK_EXYNOS5260_H
+
+/* Clock names: <cmu><type><IP> */
+
+/* List Of Clocks For CMU_TOP */
+
+#define TOP_FOUT_DISP_PLL				1
+#define TOP_FOUT_AUD_PLL				2
+#define TOP_MOUT_AUDTOP_PLL_USER			3
+#define TOP_MOUT_AUD_PLL				4
+#define TOP_MOUT_DISP_PLL				5
+#define TOP_MOUT_BUSTOP_PLL_USER			6
+#define TOP_MOUT_MEMTOP_PLL_USER			7
+#define TOP_MOUT_MEDIATOP_PLL_USER			8
+#define TOP_MOUT_DISP_DISP_333				9
+#define TOP_MOUT_ACLK_DISP_333				10
+#define TOP_MOUT_DISP_DISP_222				11
+#define TOP_MOUT_ACLK_DISP_222				12
+#define TOP_MOUT_DISP_MEDIA_PIXEL			13
+#define TOP_MOUT_FIMD1					14
+#define TOP_MOUT_SCLK_PERI_SPI0_CLK			15
+#define TOP_MOUT_SCLK_PERI_SPI1_CLK			16
+#define TOP_MOUT_SCLK_PERI_SPI2_CLK			17
+#define TOP_MOUT_SCLK_PERI_UART0_UCLK			18
+#define TOP_MOUT_SCLK_PERI_UART2_UCLK			19
+#define TOP_MOUT_SCLK_PERI_UART1_UCLK			20
+#define TOP_MOUT_BUS4_BUSTOP_100			21
+#define TOP_MOUT_BUS4_BUSTOP_400			22
+#define TOP_MOUT_BUS3_BUSTOP_100			23
+#define TOP_MOUT_BUS3_BUSTOP_400			24
+#define TOP_MOUT_BUS2_BUSTOP_400			25
+#define TOP_MOUT_BUS2_BUSTOP_100			26
+#define TOP_MOUT_BUS1_BUSTOP_100			27
+#define TOP_MOUT_BUS1_BUSTOP_400			28
+#define TOP_MOUT_SCLK_FSYS_USB				29
+#define TOP_MOUT_SCLK_FSYS_MMC0_SDCLKIN_A		30
+#define TOP_MOUT_SCLK_FSYS_MMC1_SDCLKIN_A		31
+#define TOP_MOUT_SCLK_FSYS_MMC2_SDCLKIN_A		32
+#define TOP_MOUT_SCLK_FSYS_MMC0_SDCLKIN_B		33
+#define TOP_MOUT_SCLK_FSYS_MMC1_SDCLKIN_B		34
+#define TOP_MOUT_SCLK_FSYS_MMC2_SDCLKIN_B		35
+#define TOP_MOUT_ACLK_ISP1_266				36
+#define TOP_MOUT_ISP1_MEDIA_266				37
+#define TOP_MOUT_ACLK_ISP1_400				38
+#define TOP_MOUT_ISP1_MEDIA_400				39
+#define TOP_MOUT_SCLK_ISP1_SPI0				40
+#define TOP_MOUT_SCLK_ISP1_SPI1				41
+#define TOP_MOUT_SCLK_ISP1_UART				42
+#define TOP_MOUT_SCLK_ISP1_SENSOR2			43
+#define TOP_MOUT_SCLK_ISP1_SENSOR1			44
+#define TOP_MOUT_SCLK_ISP1_SENSOR0			45
+#define TOP_MOUT_ACLK_MFC_333				46
+#define TOP_MOUT_MFC_BUSTOP_333				47
+#define TOP_MOUT_ACLK_G2D_333				48
+#define TOP_MOUT_G2D_BUSTOP_333				49
+#define TOP_MOUT_ACLK_GSCL_FIMC				50
+#define TOP_MOUT_GSCL_BUSTOP_FIMC			51
+#define TOP_MOUT_ACLK_GSCL_333				52
+#define TOP_MOUT_GSCL_BUSTOP_333			53
+#define TOP_MOUT_ACLK_GSCL_400				54
+#define TOP_MOUT_M2M_MEDIATOP_400			55
+#define TOP_DOUT_ACLK_MFC_333				56
+#define TOP_DOUT_ACLK_G2D_333				57
+#define TOP_DOUT_SCLK_ISP1_SENSOR2_A			58
+#define TOP_DOUT_SCLK_ISP1_SENSOR1_A			59
+#define TOP_DOUT_SCLK_ISP1_SENSOR0_A			60
+#define TOP_DOUT_ACLK_GSCL_FIMC				61
+#define TOP_DOUT_ACLK_GSCL_400				62
+#define TOP_DOUT_ACLK_GSCL_333				63
+#define TOP_DOUT_SCLK_ISP1_SPI0_B			64
+#define TOP_DOUT_SCLK_ISP1_SPI0_A			65
+#define TOP_DOUT_ACLK_ISP1_400				66
+#define TOP_DOUT_ACLK_ISP1_266				67
+#define TOP_DOUT_SCLK_ISP1_UART				68
+#define TOP_DOUT_SCLK_ISP1_SPI1_B			69
+#define TOP_DOUT_SCLK_ISP1_SPI1_A			70
+#define TOP_DOUT_SCLK_ISP1_SENSOR2_B			71
+#define TOP_DOUT_SCLK_ISP1_SENSOR1_B			72
+#define TOP_DOUT_SCLK_ISP1_SENSOR0_B			73
+#define TOP_DOUTTOP__SCLK_HPM_TARGETCLK			74
+#define TOP_DOUT_SCLK_DISP_PIXEL			75
+#define TOP_DOUT_ACLK_DISP_222				76
+#define TOP_DOUT_ACLK_DISP_333				77
+#define TOP_DOUT_ACLK_BUS4_100				78
+#define TOP_DOUT_ACLK_BUS4_400				79
+#define TOP_DOUT_ACLK_BUS3_100				80
+#define TOP_DOUT_ACLK_BUS3_400				81
+#define TOP_DOUT_ACLK_BUS2_100				82
+#define TOP_DOUT_ACLK_BUS2_400				83
+#define TOP_DOUT_ACLK_BUS1_100				84
+#define TOP_DOUT_ACLK_BUS1_400				85
+#define TOP_DOUT_SCLK_PERI_SPI1_B			86
+#define TOP_DOUT_SCLK_PERI_SPI1_A			87
+#define TOP_DOUT_SCLK_PERI_SPI0_B			88
+#define TOP_DOUT_SCLK_PERI_SPI0_A			89
+#define TOP_DOUT_SCLK_PERI_UART0			90
+#define TOP_DOUT_SCLK_PERI_UART2			91
+#define TOP_DOUT_SCLK_PERI_UART1			92
+#define TOP_DOUT_SCLK_PERI_SPI2_B			93
+#define TOP_DOUT_SCLK_PERI_SPI2_A			94
+#define TOP_DOUT_ACLK_PERI_AUD				95
+#define TOP_DOUT_ACLK_PERI_66				96
+#define TOP_DOUT_SCLK_FSYS_MMC0_SDCLKIN_B		97
+#define TOP_DOUT_SCLK_FSYS_MMC0_SDCLKIN_A		98
+#define TOP_DOUT_SCLK_FSYS_USBDRD30_SUSPEND_CLK		99
+#define TOP_DOUT_ACLK_FSYS_200				100
+#define TOP_DOUT_SCLK_FSYS_MMC2_SDCLKIN_B		101
+#define TOP_DOUT_SCLK_FSYS_MMC2_SDCLKIN_A		102
+#define TOP_DOUT_SCLK_FSYS_MMC1_SDCLKIN_B		103
+#define TOP_DOUT_SCLK_FSYS_MMC1_SDCLKIN_A		104
+#define TOP_SCLK_FIMD1					105
+#define TOP_SCLK_MMC2					106
+#define TOP_SCLK_MMC1					107
+#define TOP_SCLK_MMC0					108
+#define PHYCLK_DPTX_PHY_CH3_TXD_CLK			109
+#define PHYCLK_DPTX_PHY_CH2_TXD_CLK			110
+#define PHYCLK_DPTX_PHY_CH1_TXD_CLK			111
+#define PHYCLK_DPTX_PHY_CH0_TXD_CLK			112
+#define phyclk_hdmi_phy_tmds_clko			113
+#define PHYCLK_HDMI_PHY_PIXEL_CLKO			114
+#define PHYCLK_HDMI_LINK_O_TMDS_CLKHI			115
+#define PHYCLK_MIPI_DPHY_4L_M_TXBYTECLKHS		116
+#define PHYCLK_DPTX_PHY_O_REF_CLK_24M			117
+#define PHYCLK_DPTX_PHY_CLK_DIV2			118
+#define PHYCLK_MIPI_DPHY_4L_M_RXCLKESC0			119
+#define PHYCLK_USBHOST20_PHY_PHYCLOCK			120
+#define PHYCLK_USBHOST20_PHY_FREECLK			121
+#define PHYCLK_USBHOST20_PHY_CLK48MOHCI			122
+#define PHYCLK_USBDRD30_UDRD30_PIPE_PCLK		123
+#define PHYCLK_USBDRD30_UDRD30_PHYCLOCK			124
+#define TOP_NR_CLK					125
+
+
+/* List Of Clocks For CMU_EGL */
+
+#define EGL_FOUT_EGL_PLL				1
+#define EGL_FOUT_EGL_DPLL				2
+#define EGL_MOUT_EGL_B					3
+#define EGL_MOUT_EGL_PLL				4
+#define EGL_DOUT_EGL_PLL				5
+#define EGL_DOUT_EGL_PCLK_DBG				6
+#define EGL_DOUT_EGL_ATCLK				7
+#define EGL_DOUT_PCLK_EGL				8
+#define EGL_DOUT_ACLK_EGL				9
+#define EGL_DOUT_EGL2					10
+#define EGL_DOUT_EGL1					11
+#define EGL_NR_CLK					12
+
+
+/* List Of Clocks For CMU_KFC */
+
+#define KFC_FOUT_KFC_PLL				1
+#define KFC_MOUT_KFC_PLL				2
+#define KFC_MOUT_KFC					3
+#define KFC_DOUT_KFC_PLL				4
+#define KFC_DOUT_PCLK_KFC				5
+#define KFC_DOUT_ACLK_KFC				6
+#define KFC_DOUT_KFC_PCLK_DBG				7
+#define KFC_DOUT_KFC_ATCLK				8
+#define KFC_DOUT_KFC2					9
+#define KFC_DOUT_KFC1					10
+#define KFC_NR_CLK					11
+
+
+/* List Of Clocks For CMU_MIF */
+
+#define MIF_FOUT_MEM_PLL				1
+#define MIF_FOUT_MEDIA_PLL				2
+#define MIF_FOUT_BUS_PLL				3
+#define MIF_MOUT_CLK2X_PHY				4
+#define MIF_MOUT_MIF_DREX2X				5
+#define MIF_MOUT_CLKM_PHY				6
+#define MIF_MOUT_MIF_DREX				7
+#define MIF_MOUT_MEDIA_PLL				8
+#define MIF_MOUT_BUS_PLL				9
+#define MIF_MOUT_MEM_PLL				10
+#define MIF_DOUT_ACLK_BUS_100				11
+#define MIF_DOUT_ACLK_BUS_200				12
+#define MIF_DOUT_ACLK_MIF_466				13
+#define MIF_DOUT_CLK2X_PHY				14
+#define MIF_DOUT_CLKM_PHY				15
+#define MIF_DOUT_BUS_PLL				16
+#define MIF_DOUT_MEM_PLL				17
+#define MIF_DOUT_MEDIA_PLL				18
+#define MIF_CLK_LPDDR3PHY_WRAP1				19
+#define MIF_CLK_LPDDR3PHY_WRAP0				20
+#define MIF_CLK_MONOCNT					21
+#define MIF_CLK_MIF_RTC					22
+#define MIF_CLK_DREX1					23
+#define MIF_CLK_DREX0					24
+#define MIF_CLK_INTMEM					25
+#define MIF_SCLK_LPDDR3PHY_WRAP_U1			26
+#define MIF_SCLK_LPDDR3PHY_WRAP_U0			27
+#define MIF_NR_CLK					28
+
+
+/* List Of Clocks For CMU_G3D */
+
+#define G3D_FOUT_G3D_PLL				1
+#define G3D_MOUT_G3D_PLL				2
+#define G3D_DOUT_PCLK_G3D				3
+#define G3D_DOUT_ACLK_G3D				4
+#define G3D_CLK_G3D_HPM					5
+#define G3D_CLK_G3D					6
+#define G3D_NR_CLK					7
+
+
+/* List Of Clocks For CMU_AUD */
+
+#define AUD_MOUT_SCLK_AUD_PCM				1
+#define AUD_MOUT_SCLK_AUD_I2S				2
+#define AUD_MOUT_AUD_PLL_USER				3
+#define AUD_DOUT_ACLK_AUD_131				4
+#define AUD_DOUT_SCLK_AUD_UART				5
+#define AUD_DOUT_SCLK_AUD_PCM				6
+#define AUD_DOUT_SCLK_AUD_I2S				7
+#define AUD_CLK_AUD_UART				8
+#define AUD_CLK_PCM					9
+#define AUD_CLK_I2S					10
+#define AUD_CLK_DMAC					11
+#define AUD_CLK_SRAMC					12
+#define AUD_SCLK_AUD_UART				13
+#define AUD_SCLK_PCM					14
+#define AUD_SCLK_I2S					15
+#define AUD_NR_CLK					16
+
+
+/* List Of Clocks For CMU_MFC */
+
+#define MFC_MOUT_ACLK_MFC_333_USER			1
+#define MFC_DOUT_PCLK_MFC_83				2
+#define MFC_CLK_MFC					3
+#define MFC_CLK_SMMU2_MFCM1				4
+#define MFC_CLK_SMMU2_MFCM0				5
+#define MFC_NR_CLK					6
+
+
+/* List Of Clocks For CMU_GSCL */
+
+#define GSCL_MOUT_ACLK_CSIS				1
+#define GSCL_MOUT_ACLK_GSCL_FIMC_USER			2
+#define GSCL_MOUT_ACLK_M2M_400_USER			3
+#define GSCL_MOUT_ACLK_GSCL_333_USER			4
+#define GSCL_DOUT_ACLK_CSIS_200				5
+#define GSCL_DOUT_PCLK_M2M_100				6
+#define GSCL_CLK_PIXEL_GSCL1				7
+#define GSCL_CLK_PIXEL_GSCL0				8
+#define GSCL_CLK_MSCL1					9
+#define GSCL_CLK_MSCL0					10
+#define GSCL_CLK_GSCL1					11
+#define GSCL_CLK_GSCL0					12
+#define GSCL_CLK_FIMC_LITE_D				13
+#define GSCL_CLK_FIMC_LITE_B				14
+#define GSCL_CLK_FIMC_LITE_A				15
+#define GSCL_CLK_CSIS1					16
+#define GSCL_CLK_CSIS0					17
+#define GSCL_CLK_SMMU3_LITE_D				18
+#define GSCL_CLK_SMMU3_LITE_B				19
+#define GSCL_CLK_SMMU3_LITE_A				20
+#define GSCL_CLK_SMMU3_GSCL0				21
+#define GSCL_CLK_SMMU3_GSCL1				22
+#define GSCL_CLK_SMMU3_MSCL0				23
+#define GSCL_CLK_SMMU3_MSCL1				24
+#define GSCL_SCLK_CSIS1_WRAP				25
+#define GSCL_SCLK_CSIS0_WRAP				26
+#define GSCL_NR_CLK					27
+
+
+/* List Of Clocks For CMU_FSYS */
+
+#define FSYS_MOUT_PHYCLK_USBHOST20_PHYCLK_USER		1
+#define FSYS_MOUT_PHYCLK_USBHOST20_FREECLK_USER		2
+#define FSYS_MOUT_PHYCLK_USBHOST20_CLK48MOHCI_USER	3
+#define FSYS_MOUT_PHYCLK_USBDRD30_PIPE_PCLK_USER	4
+#define FSYS_MOUT_PHYCLK_USBDRD30_PHYCLOCK_USER		5
+#define FSYS_CLK_TSI					6
+#define FSYS_CLK_USBLINK				7
+#define FSYS_CLK_USBHOST20				8
+#define FSYS_CLK_USBDRD30				9
+#define FSYS_CLK_SROMC					10
+#define FSYS_CLK_PDMA					11
+#define FSYS_CLK_MMC2					12
+#define FSYS_CLK_MMC1					13
+#define FSYS_CLK_MMC0					14
+#define FSYS_CLK_RTIC					15
+#define FSYS_CLK_SMMU_RTIC				16
+#define FSYS_PHYCLK_USBDRD30				17
+#define FSYS_PHYCLK_USBHOST20				18
+#define FSYS_NR_CLK					19
+
+
+/* List Of Clocks For CMU_PERI */
+
+#define PERI_MOUT_SCLK_SPDIF				1
+#define PERI_MOUT_SCLK_I2SCOD				2
+#define PERI_MOUT_SCLK_PCM				3
+#define PERI_DOUT_I2S					4
+#define PERI_DOUT_PCM					5
+#define PERI_CLK_WDT_KFC				6
+#define PERI_CLK_WDT_EGL				7
+#define PERI_CLK_HSIC3					8
+#define PERI_CLK_HSIC2					9
+#define PERI_CLK_HSIC1					10
+#define PERI_CLK_HSIC0					11
+#define PERI_CLK_PCM					12
+#define PERI_CLK_MCT					13
+#define PERI_CLK_I2S					14
+#define PERI_CLK_I2CHDMI				15
+#define PERI_CLK_I2C7					16
+#define PERI_CLK_I2C6					17
+#define PERI_CLK_I2C5					18
+#define PERI_CLK_I2C4					19
+#define PERI_CLK_I2C9					20
+#define PERI_CLK_I2C8					21
+#define PERI_CLK_I2C11					22
+#define PERI_CLK_I2C10					23
+#define PERI_CLK_HDMICEC				24
+#define PERI_CLK_EFUSE_WRITER				25
+#define PERI_CLK_ABB					26
+#define PERI_CLK_UART2					27
+#define PERI_CLK_UART1					28
+#define PERI_CLK_UART0					29
+#define PERI_CLK_ADC					30
+#define PERI_CLK_TMU4					31
+#define PERI_CLK_TMU3					32
+#define PERI_CLK_TMU2					33
+#define PERI_CLK_TMU1					34
+#define PERI_CLK_TMU0					35
+#define PERI_CLK_SPI2					36
+#define PERI_CLK_SPI1					37
+#define PERI_CLK_SPI0					38
+#define PERI_CLK_SPDIF					39
+#define PERI_CLK_PWM					40
+#define PERI_CLK_UART4					41
+#define PERI_CLK_CHIPID					42
+#define PERI_CLK_PROVKEY0				43
+#define PERI_CLK_PROVKEY1				44
+#define PERI_CLK_SECKEY					45
+#define PERI_CLK_TOP_RTC				46
+#define PERI_CLK_TZPC10					47
+#define PERI_CLK_TZPC9					48
+#define PERI_CLK_TZPC8					49
+#define PERI_CLK_TZPC7					50
+#define PERI_CLK_TZPC6					51
+#define PERI_CLK_TZPC5					52
+#define PERI_CLK_TZPC4					53
+#define PERI_CLK_TZPC3					54
+#define PERI_CLK_TZPC2					55
+#define PERI_CLK_TZPC1					56
+#define PERI_CLK_TZPC0					57
+#define PERI_SCLK_UART2					58
+#define PERI_SCLK_UART1					59
+#define PERI_SCLK_UART0					60
+#define PERI_SCLK_SPI2					61
+#define PERI_SCLK_SPI1					62
+#define PERI_SCLK_SPI0					63
+#define PERI_SCLK_SPDIF					64
+#define PERI_SCLK_I2S					65
+#define PERI_SCLK_PCM1					66
+#define PERI_NR_CLK					67
+
+
+/* List Of Clocks For CMU_DISP */
+
+#define DISP_MOUT_SCLK_HDMI_SPDIF			1
+#define DISP_MOUT_SCLK_HDMI_PIXEL			2
+#define DISP_MOUT_PHYCLK_MIPI_DPHY_4LMRXCLK_ESC0_USER	3
+#define DISP_MOUT_PHYCLK_HDMI_PHY_TMDS_CLKO_USER	4
+#define DISP_MOUT_PHYCLK_HDMI_PHY_REF_CLKO_USER		5
+#define DISP_MOUT_HDMI_PHY_PIXEL			6
+#define DISP_MOUT_PHYCLK_HDMI_LINK_O_TMDS_CLKHI_USER	7
+#define DISP_MOUT_PHYCLK_MIPI_DPHY_4L_M_TXBYTE_CLKHS	8
+#define DISP_MOUT_PHYCLK_DPTX_PHY_O_REF_CLK_24M_USER	9
+#define DISP_MOUT_PHYCLK_DPTX_PHY_CLK_DIV2_USER		10
+#define DISP_MOUT_PHYCLK_DPTX_PHY_CH3_TXD_CLK_USER	11
+#define DISP_MOUT_PHYCLK_DPTX_PHY_CH2_TXD_CLK_USER	12
+#define DISP_MOUT_PHYCLK_DPTX_PHY_CH1_TXD_CLK_USER	13
+#define DISP_MOUT_PHYCLK_DPTX_PHY_CH0_TXD_CLK_USER	14
+#define DISP_MOUT_ACLK_DISP_222_USER			15
+#define DISP_MOUT_SCLK_DISP_PIXEL_USER			16
+#define DISP_MOUT_ACLK_DISP_333_USER			17
+#define DISP_DOUT_SCLK_HDMI_PHY_PIXEL_CLKI		18
+#define DISP_DOUT_SCLK_FIMD1_EXTCLKPLL			19
+#define DISP_DOUT_PCLK_DISP_111				20
+#define DISP_CLK_SMMU_TV				21
+#define DISP_CLK_SMMU_FIMD1M1				22
+#define DISP_CLK_SMMU_FIMD1M0				23
+#define DISP_CLK_PIXEL_MIXER				24
+#define DISP_CLK_PIXEL_DISP				25
+#define DISP_CLK_MIXER					26
+#define DISP_CLK_MIPIPHY				27
+#define DISP_CLK_HDMIPHY				28
+#define DISP_CLK_HDMI					29
+#define DISP_CLK_FIMD1					30
+#define DISP_CLK_DSIM1					31
+#define DISP_CLK_DPPHY					32
+#define DISP_CLK_DP					33
+#define DISP_SCLK_PIXEL					34
+#define DISP_MOUT_HDMI_PHY_PIXEL_USER			35
+#define DISP_NR_CLK					36
+
+
+/* List Of Clocks For CMU_G2D */
+
+#define G2D_MOUT_ACLK_G2D_333_USER			1
+#define G2D_DOUT_PCLK_G2D_83				2
+#define G2D_CLK_SMMU3_JPEG				3
+#define G2D_CLK_MDMA					4
+#define G2D_CLK_JPEG					5
+#define G2D_CLK_G2D					6
+#define G2D_CLK_SSS					7
+#define G2D_CLK_SLIM_SSS				8
+#define G2D_CLK_SMMU_SLIM_SSS				9
+#define G2D_CLK_SMMU_SSS				10
+#define G2D_CLK_SMMU_MDMA				11
+#define G2D_CLK_SMMU3_G2D				12
+#define G2D_NR_CLK					13
+
+
+/* List Of Clocks For CMU_ISP */
+
+#define ISP_MOUT_ISP_400_USER				1
+#define ISP_MOUT_ISP_266_USER				2
+#define ISP_DOUT_SCLK_MPWM				3
+#define ISP_DOUT_CA5_PCLKDBG				4
+#define ISP_DOUT_CA5_ATCLKIN				5
+#define ISP_DOUT_PCLK_ISP_133				6
+#define ISP_DOUT_PCLK_ISP_66				7
+#define ISP_CLK_GIC					8
+#define ISP_CLK_WDT					9
+#define ISP_CLK_UART					10
+#define ISP_CLK_SPI1					11
+#define ISP_CLK_SPI0					12
+#define ISP_CLK_SMMU_SCALERP				13
+#define ISP_CLK_SMMU_SCALERC				14
+#define ISP_CLK_SMMU_ISPCX				15
+#define ISP_CLK_SMMU_ISP				16
+#define ISP_CLK_SMMU_FD					17
+#define ISP_CLK_SMMU_DRC				18
+#define ISP_CLK_PWM					19
+#define ISP_CLK_MTCADC					20
+#define ISP_CLK_MPWM					21
+#define ISP_CLK_MCUCTL					22
+#define ISP_CLK_I2C1					23
+#define ISP_CLK_I2C0					24
+#define ISP_CLK_FIMC_SCALERP				25
+#define ISP_CLK_FIMC_SCALERC				26
+#define ISP_CLK_FIMC					27
+#define ISP_CLK_FIMC_FD					28
+#define ISP_CLK_FIMC_DRC				29
+#define ISP_CLK_CA5					30
+#define ISP_SCLK_SPI0_EXT				31
+#define ISP_SCLK_SPI1_EXT				32
+#define ISP_SCLK_UART_EXT				33
+#define ISP_NR_CLK					34
+
+#endif
diff --git a/include/dt-bindings/clock/exynos5410.h b/include/dt-bindings/clock/exynos5410.h
new file mode 100644
index 00000000000..9b180f032e2
--- /dev/null
+++ b/include/dt-bindings/clock/exynos5410.h
@@ -0,0 +1,33 @@
+#ifndef _DT_BINDINGS_CLOCK_EXYNOS_5410_H
+#define _DT_BINDINGS_CLOCK_EXYNOS_5410_H
+
+/* core clocks */
+#define CLK_FIN_PLL 1
+#define CLK_FOUT_APLL 2
+#define CLK_FOUT_CPLL 3
+#define CLK_FOUT_MPLL 4
+#define CLK_FOUT_BPLL 5
+#define CLK_FOUT_KPLL 6
+
+/* gate for special clocks (sclk) */
+#define CLK_SCLK_UART0 128
+#define CLK_SCLK_UART1 129
+#define CLK_SCLK_UART2 130
+#define CLK_SCLK_UART3 131
+#define CLK_SCLK_MMC0 132
+#define CLK_SCLK_MMC1 133
+#define CLK_SCLK_MMC2 134
+
+/* gate clocks */
+#define CLK_UART0 257
+#define CLK_UART1 258
+#define CLK_UART2 259
+#define CLK_UART3 260
+#define CLK_MCT 315
+#define CLK_MMC0 351
+#define CLK_MMC1 352
+#define CLK_MMC2 353
+
+#define CLK_NR_CLKS 512
+
+#endif /* _DT_BINDINGS_CLOCK_EXYNOS_5410_H */
diff --git a/include/dt-bindings/clock/exynos5420.h b/include/dt-bindings/clock/exynos5420.h
index 5eefd8813f0..97dcb89d37d 100644
--- a/include/dt-bindings/clock/exynos5420.h
+++ b/include/dt-bindings/clock/exynos5420.h
@@ -58,6 +58,9 @@
 #define CLK_SCLK_GSCL_WA	156
 #define CLK_SCLK_GSCL_WB	157
 #define CLK_SCLK_HDMIPHY	158
+#define CLK_MAU_EPLL		159
+#define CLK_SCLK_HSIC_12M	160
+#define CLK_SCLK_MPHY_IXTAL24	161
 
 /* gate clocks */
 #define CLK_ACLK66_PERIC	256
@@ -69,10 +72,10 @@
 #define CLK_I2C1		262
 #define CLK_I2C2		263
 #define CLK_I2C3		264
-#define CLK_I2C4		265
-#define CLK_I2C5		266
-#define CLK_I2C6		267
-#define CLK_I2C7		268
+#define CLK_USI0		265
+#define CLK_USI1		266
+#define CLK_USI2		267
+#define CLK_USI3		268
 #define CLK_I2C_HDMI		269
 #define CLK_TSADC		270
 #define CLK_SPI0		271
@@ -85,9 +88,9 @@
 #define CLK_PCM2		278
 #define CLK_PWM			279
 #define CLK_SPDIF		280
-#define CLK_I2C8		281
-#define CLK_I2C9		282
-#define CLK_I2C10		283
+#define CLK_USI4		281
+#define CLK_USI5		282
+#define CLK_USI6		283
 #define CLK_ACLK66_PSGEN	300
 #define CLK_CHIPID		301
 #define CLK_SYSREG		302
@@ -140,7 +143,8 @@
 #define CLK_HDMI		413
 #define CLK_ACLK300_DISP1	420
 #define CLK_FIMD1		421
-#define CLK_SMMU_FIMD1		422
+#define CLK_SMMU_FIMD1M0	422
+#define CLK_SMMU_FIMD1M1	423
 #define CLK_ACLK166		430
 #define CLK_MIXER		431
 #define CLK_ACLK266		440
@@ -152,6 +156,7 @@
 #define CLK_JPEG		451
 #define CLK_JPEG2		452
 #define CLK_SMMU_JPEG		453
+#define CLK_SMMU_JPEG2		454
 #define CLK_ACLK300_GSCL	460
 #define CLK_SMMU_GSCL0		461
 #define CLK_SMMU_GSCL1		462
@@ -159,7 +164,7 @@
 #define CLK_GSCL_WB		464
 #define CLK_GSCL0		465
 #define CLK_GSCL1		466
-#define CLK_CLK_3AA		467
+#define CLK_FIMC_3AA		467
 #define CLK_ACLK266_G2D		470
 #define CLK_SSS			471
 #define CLK_SLIM_SSS		472
@@ -172,12 +177,32 @@
 #define CLK_SMMU_FIMCL1		493
 #define CLK_SMMU_FIMCL3		494
 #define CLK_FIMC_LITE3		495
+#define CLK_FIMC_LITE0		496
+#define CLK_FIMC_LITE1		497
 #define CLK_ACLK_G3D		500
 #define CLK_G3D			501
 #define CLK_SMMU_MIXER		502
+#define CLK_SMMU_G2D		503
+#define CLK_SMMU_MDMA0		504
+#define CLK_MC			505
+#define CLK_TOP_RTC		506
+#define CLK_SCLK_UART_ISP	510
+#define CLK_SCLK_SPI0_ISP	511
+#define CLK_SCLK_SPI1_ISP	512
+#define CLK_SCLK_PWM_ISP	513
+#define CLK_SCLK_ISP_SENSOR0	514
+#define CLK_SCLK_ISP_SENSOR1	515
+#define CLK_SCLK_ISP_SENSOR2	516
+#define CLK_ACLK432_SCALER	517
+#define CLK_ACLK432_CAM		518
+#define CLK_ACLK_FL1550_CAM	519
+#define CLK_ACLK550_CAM		520
 
 /* mux clocks */
 #define CLK_MOUT_HDMI		640
+#define CLK_MOUT_G3D		641
+#define CLK_MOUT_VPLL		642
+#define CLK_MOUT_MAUDIO0	643
 
 /* divider clocks */
 #define CLK_DOUT_PIXEL		768
diff --git a/include/dt-bindings/clock/imx6sx-clock.h b/include/dt-bindings/clock/imx6sx-clock.h
new file mode 100644
index 00000000000..421d8bb76f2
--- /dev/null
+++ b/include/dt-bindings/clock/imx6sx-clock.h
@@ -0,0 +1,256 @@
+/*
+ * Copyright (C) 2014 Freescale Semiconductor, 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.
+ *
+ */
+
+#ifndef __DT_BINDINGS_CLOCK_IMX6SX_H
+#define __DT_BINDINGS_CLOCK_IMX6SX_H
+
+#define IMX6SX_CLK_DUMMY		0
+#define IMX6SX_CLK_CKIL			1
+#define IMX6SX_CLK_CKIH			2
+#define IMX6SX_CLK_OSC			3
+#define IMX6SX_CLK_PLL1_SYS		4
+#define IMX6SX_CLK_PLL2_BUS		5
+#define IMX6SX_CLK_PLL3_USB_OTG		6
+#define IMX6SX_CLK_PLL4_AUDIO		7
+#define IMX6SX_CLK_PLL5_VIDEO		8
+#define IMX6SX_CLK_PLL6_ENET		9
+#define IMX6SX_CLK_PLL7_USB_HOST	10
+#define IMX6SX_CLK_USBPHY1		11
+#define IMX6SX_CLK_USBPHY2		12
+#define IMX6SX_CLK_USBPHY1_GATE		13
+#define IMX6SX_CLK_USBPHY2_GATE		14
+#define IMX6SX_CLK_PCIE_REF		15
+#define IMX6SX_CLK_PCIE_REF_125M	16
+#define IMX6SX_CLK_ENET_REF		17
+#define IMX6SX_CLK_PLL2_PFD0		18
+#define IMX6SX_CLK_PLL2_PFD1		19
+#define IMX6SX_CLK_PLL2_PFD2		20
+#define IMX6SX_CLK_PLL2_PFD3		21
+#define IMX6SX_CLK_PLL3_PFD0		22
+#define IMX6SX_CLK_PLL3_PFD1		23
+#define IMX6SX_CLK_PLL3_PFD2		24
+#define IMX6SX_CLK_PLL3_PFD3		25
+#define IMX6SX_CLK_PLL2_198M		26
+#define IMX6SX_CLK_PLL3_120M		27
+#define IMX6SX_CLK_PLL3_80M		28
+#define IMX6SX_CLK_PLL3_60M		29
+#define IMX6SX_CLK_TWD			30
+#define IMX6SX_CLK_PLL4_POST_DIV	31
+#define IMX6SX_CLK_PLL4_AUDIO_DIV	32
+#define IMX6SX_CLK_PLL5_POST_DIV	33
+#define IMX6SX_CLK_PLL5_VIDEO_DIV	34
+#define IMX6SX_CLK_STEP			35
+#define IMX6SX_CLK_PLL1_SW		36
+#define IMX6SX_CLK_OCRAM_SEL		37
+#define IMX6SX_CLK_PERIPH_PRE		38
+#define IMX6SX_CLK_PERIPH2_PRE		39
+#define IMX6SX_CLK_PERIPH_CLK2_SEL	40
+#define IMX6SX_CLK_PERIPH2_CLK2_SEL	41
+#define IMX6SX_CLK_PCIE_AXI_SEL		42
+#define IMX6SX_CLK_GPU_AXI_SEL		43
+#define IMX6SX_CLK_GPU_CORE_SEL		44
+#define IMX6SX_CLK_EIM_SLOW_SEL		45
+#define IMX6SX_CLK_USDHC1_SEL		46
+#define IMX6SX_CLK_USDHC2_SEL		47
+#define IMX6SX_CLK_USDHC3_SEL		48
+#define IMX6SX_CLK_USDHC4_SEL		49
+#define IMX6SX_CLK_SSI1_SEL		50
+#define IMX6SX_CLK_SSI2_SEL		51
+#define IMX6SX_CLK_SSI3_SEL		52
+#define IMX6SX_CLK_QSPI1_SEL		53
+#define IMX6SX_CLK_PERCLK_SEL		54
+#define IMX6SX_CLK_VID_SEL		55
+#define IMX6SX_CLK_ESAI_SEL		56
+#define IMX6SX_CLK_LDB_DI0_DIV_SEL	57
+#define IMX6SX_CLK_LDB_DI1_DIV_SEL	58
+#define IMX6SX_CLK_CAN_SEL		59
+#define IMX6SX_CLK_UART_SEL		60
+#define IMX6SX_CLK_QSPI2_SEL		61
+#define IMX6SX_CLK_LDB_DI1_SEL		62
+#define IMX6SX_CLK_LDB_DI0_SEL		63
+#define IMX6SX_CLK_SPDIF_SEL		64
+#define IMX6SX_CLK_AUDIO_SEL		65
+#define IMX6SX_CLK_ENET_PRE_SEL		66
+#define IMX6SX_CLK_ENET_SEL		67
+#define IMX6SX_CLK_M4_PRE_SEL		68
+#define IMX6SX_CLK_M4_SEL		69
+#define IMX6SX_CLK_ECSPI_SEL		70
+#define IMX6SX_CLK_LCDIF1_PRE_SEL	71
+#define IMX6SX_CLK_LCDIF2_PRE_SEL	72
+#define IMX6SX_CLK_LCDIF1_SEL		73
+#define IMX6SX_CLK_LCDIF2_SEL		74
+#define IMX6SX_CLK_DISPLAY_SEL		75
+#define IMX6SX_CLK_CSI_SEL		76
+#define IMX6SX_CLK_CKO1_SEL		77
+#define IMX6SX_CLK_CKO2_SEL		78
+#define IMX6SX_CLK_CKO			79
+#define IMX6SX_CLK_PERIPH_CLK2		80
+#define IMX6SX_CLK_PERIPH2_CLK2		81
+#define IMX6SX_CLK_IPG			82
+#define IMX6SX_CLK_GPU_CORE_PODF	83
+#define IMX6SX_CLK_GPU_AXI_PODF		84
+#define IMX6SX_CLK_LCDIF1_PODF		85
+#define IMX6SX_CLK_QSPI1_PODF		86
+#define IMX6SX_CLK_EIM_SLOW_PODF	87
+#define IMX6SX_CLK_LCDIF2_PODF		88
+#define IMX6SX_CLK_PERCLK		89
+#define IMX6SX_CLK_VID_PODF		90
+#define IMX6SX_CLK_CAN_PODF		91
+#define IMX6SX_CLK_USDHC1_PODF		92
+#define IMX6SX_CLK_USDHC2_PODF		93
+#define IMX6SX_CLK_USDHC3_PODF		94
+#define IMX6SX_CLK_USDHC4_PODF		95
+#define IMX6SX_CLK_UART_PODF		96
+#define IMX6SX_CLK_ESAI_PRED		97
+#define IMX6SX_CLK_ESAI_PODF		98
+#define IMX6SX_CLK_SSI3_PRED		99
+#define IMX6SX_CLK_SSI3_PODF		100
+#define IMX6SX_CLK_SSI1_PRED		101
+#define IMX6SX_CLK_SSI1_PODF		102
+#define IMX6SX_CLK_QSPI2_PRED		103
+#define IMX6SX_CLK_QSPI2_PODF		104
+#define IMX6SX_CLK_SSI2_PRED		105
+#define IMX6SX_CLK_SSI2_PODF		106
+#define IMX6SX_CLK_SPDIF_PRED		107
+#define IMX6SX_CLK_SPDIF_PODF		108
+#define IMX6SX_CLK_AUDIO_PRED		109
+#define IMX6SX_CLK_AUDIO_PODF		110
+#define IMX6SX_CLK_ENET_PODF		111
+#define IMX6SX_CLK_M4_PODF		112
+#define IMX6SX_CLK_ECSPI_PODF		113
+#define IMX6SX_CLK_LCDIF1_PRED		114
+#define IMX6SX_CLK_LCDIF2_PRED		115
+#define IMX6SX_CLK_DISPLAY_PODF		116
+#define IMX6SX_CLK_CSI_PODF		117
+#define IMX6SX_CLK_LDB_DI0_DIV_3_5	118
+#define IMX6SX_CLK_LDB_DI0_DIV_7	119
+#define IMX6SX_CLK_LDB_DI1_DIV_3_5	120
+#define IMX6SX_CLK_LDB_DI1_DIV_7	121
+#define IMX6SX_CLK_CKO1_PODF		122
+#define IMX6SX_CLK_CKO2_PODF		123
+#define IMX6SX_CLK_PERIPH		124
+#define IMX6SX_CLK_PERIPH2		125
+#define IMX6SX_CLK_OCRAM		126
+#define IMX6SX_CLK_AHB			127
+#define IMX6SX_CLK_MMDC_PODF		128
+#define IMX6SX_CLK_ARM			129
+#define IMX6SX_CLK_AIPS_TZ1		130
+#define IMX6SX_CLK_AIPS_TZ2		131
+#define IMX6SX_CLK_APBH_DMA		132
+#define IMX6SX_CLK_ASRC_GATE		133
+#define IMX6SX_CLK_CAAM_MEM		134
+#define IMX6SX_CLK_CAAM_ACLK		135
+#define IMX6SX_CLK_CAAM_IPG		136
+#define IMX6SX_CLK_CAN1_IPG		137
+#define IMX6SX_CLK_CAN1_SERIAL		138
+#define IMX6SX_CLK_CAN2_IPG		139
+#define IMX6SX_CLK_CAN2_SERIAL		140
+#define IMX6SX_CLK_CPU_DEBUG		141
+#define IMX6SX_CLK_DCIC1		142
+#define IMX6SX_CLK_DCIC2		143
+#define IMX6SX_CLK_AIPS_TZ3		144
+#define IMX6SX_CLK_ECSPI1		145
+#define IMX6SX_CLK_ECSPI2		146
+#define IMX6SX_CLK_ECSPI3		147
+#define IMX6SX_CLK_ECSPI4		148
+#define IMX6SX_CLK_ECSPI5		149
+#define IMX6SX_CLK_EPIT1		150
+#define IMX6SX_CLK_EPIT2		151
+#define IMX6SX_CLK_ESAI_EXTAL		152
+#define IMX6SX_CLK_WAKEUP		153
+#define IMX6SX_CLK_GPT_BUS		154
+#define IMX6SX_CLK_GPT_SERIAL		155
+#define IMX6SX_CLK_GPU			156
+#define IMX6SX_CLK_OCRAM_S		157
+#define IMX6SX_CLK_CANFD		158
+#define IMX6SX_CLK_CSI			159
+#define IMX6SX_CLK_I2C1			160
+#define IMX6SX_CLK_I2C2			161
+#define IMX6SX_CLK_I2C3			162
+#define IMX6SX_CLK_OCOTP		163
+#define IMX6SX_CLK_IOMUXC		164
+#define IMX6SX_CLK_IPMUX1		165
+#define IMX6SX_CLK_IPMUX2		166
+#define IMX6SX_CLK_IPMUX3		167
+#define IMX6SX_CLK_TZASC1		168
+#define IMX6SX_CLK_LCDIF_APB		169
+#define IMX6SX_CLK_PXP_AXI		170
+#define IMX6SX_CLK_M4			171
+#define IMX6SX_CLK_ENET			172
+#define IMX6SX_CLK_DISPLAY_AXI		173
+#define IMX6SX_CLK_LCDIF2_PIX		174
+#define IMX6SX_CLK_LCDIF1_PIX		175
+#define IMX6SX_CLK_LDB_DI0		176
+#define IMX6SX_CLK_QSPI1		177
+#define IMX6SX_CLK_MLB			178
+#define IMX6SX_CLK_MMDC_P0_FAST		179
+#define IMX6SX_CLK_MMDC_P0_IPG		180
+#define IMX6SX_CLK_AXI			181
+#define IMX6SX_CLK_PCIE_AXI		182
+#define IMX6SX_CLK_QSPI2		183
+#define IMX6SX_CLK_PER1_BCH		184
+#define IMX6SX_CLK_PER2_MAIN		185
+#define IMX6SX_CLK_PWM1			186
+#define IMX6SX_CLK_PWM2			187
+#define IMX6SX_CLK_PWM3			188
+#define IMX6SX_CLK_PWM4			189
+#define IMX6SX_CLK_GPMI_BCH_APB		190
+#define IMX6SX_CLK_GPMI_BCH		191
+#define IMX6SX_CLK_GPMI_IO		192
+#define IMX6SX_CLK_GPMI_APB		193
+#define IMX6SX_CLK_ROM			194
+#define IMX6SX_CLK_SDMA			195
+#define IMX6SX_CLK_SPBA			196
+#define IMX6SX_CLK_SPDIF		197
+#define IMX6SX_CLK_SSI1_IPG		198
+#define IMX6SX_CLK_SSI2_IPG		199
+#define IMX6SX_CLK_SSI3_IPG		200
+#define IMX6SX_CLK_SSI1			201
+#define IMX6SX_CLK_SSI2			202
+#define IMX6SX_CLK_SSI3			203
+#define IMX6SX_CLK_UART_IPG		204
+#define IMX6SX_CLK_UART_SERIAL		205
+#define IMX6SX_CLK_SAI1			206
+#define IMX6SX_CLK_SAI2			207
+#define IMX6SX_CLK_USBOH3		208
+#define IMX6SX_CLK_USDHC1		209
+#define IMX6SX_CLK_USDHC2		210
+#define IMX6SX_CLK_USDHC3		211
+#define IMX6SX_CLK_USDHC4		212
+#define IMX6SX_CLK_EIM_SLOW		213
+#define IMX6SX_CLK_PWM8			214
+#define IMX6SX_CLK_VADC			215
+#define IMX6SX_CLK_GIS			216
+#define IMX6SX_CLK_I2C4			217
+#define IMX6SX_CLK_PWM5			218
+#define IMX6SX_CLK_PWM6			219
+#define IMX6SX_CLK_PWM7			220
+#define IMX6SX_CLK_CKO1			221
+#define IMX6SX_CLK_CKO2			222
+#define IMX6SX_CLK_IPP_DI0		223
+#define IMX6SX_CLK_IPP_DI1		224
+#define IMX6SX_CLK_ENET_AHB		225
+#define IMX6SX_CLK_OCRAM_PODF		226
+#define IMX6SX_CLK_GPT_3M		227
+#define IMX6SX_CLK_ENET_PTP		228
+#define IMX6SX_CLK_ENET_PTP_REF		229
+#define IMX6SX_CLK_ENET2_REF		230
+#define IMX6SX_CLK_ENET2_REF_125M	231
+#define IMX6SX_CLK_AUDIO		232
+#define IMX6SX_CLK_LVDS1_SEL		233
+#define IMX6SX_CLK_LVDS1_OUT		234
+#define IMX6SX_CLK_ASRC_IPG		235
+#define IMX6SX_CLK_ASRC_MEM		236
+#define IMX6SX_CLK_SAI1_IPG		237
+#define IMX6SX_CLK_SAI2_IPG		238
+#define IMX6SX_CLK_ESAI_IPG		239
+#define IMX6SX_CLK_ESAI_MEM		240
+#define IMX6SX_CLK_CLK_END		241
+
+#endif /* __DT_BINDINGS_CLOCK_IMX6SX_H */
diff --git a/include/dt-bindings/clock/lsi,axm5516-clks.h b/include/dt-bindings/clock/lsi,axm5516-clks.h
new file mode 100644
index 00000000000..beb41ace5dd
--- /dev/null
+++ b/include/dt-bindings/clock/lsi,axm5516-clks.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (c) 2014 LSI Corporation
+ *
+ * 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.
+ */
+
+#ifndef _DT_BINDINGS_CLK_AXM5516_H
+#define _DT_BINDINGS_CLK_AXM5516_H
+
+#define AXXIA_CLK_FAB_PLL	0
+#define AXXIA_CLK_CPU_PLL	1
+#define AXXIA_CLK_SYS_PLL	2
+#define AXXIA_CLK_SM0_PLL	3
+#define AXXIA_CLK_SM1_PLL	4
+#define AXXIA_CLK_FAB_DIV	5
+#define AXXIA_CLK_SYS_DIV	6
+#define AXXIA_CLK_NRCP_DIV	7
+#define AXXIA_CLK_CPU0_DIV	8
+#define AXXIA_CLK_CPU1_DIV	9
+#define AXXIA_CLK_CPU2_DIV	10
+#define AXXIA_CLK_CPU3_DIV	11
+#define AXXIA_CLK_PER_DIV	12
+#define AXXIA_CLK_MMC_DIV	13
+#define AXXIA_CLK_FAB		14
+#define AXXIA_CLK_SYS		15
+#define AXXIA_CLK_NRCP		16
+#define AXXIA_CLK_CPU0		17
+#define AXXIA_CLK_CPU1		18
+#define AXXIA_CLK_CPU2		19
+#define AXXIA_CLK_CPU3		20
+#define AXXIA_CLK_PER		21
+#define AXXIA_CLK_MMC		22
+
+#endif
diff --git a/include/dt-bindings/clock/r7s72100-clock.h b/include/dt-bindings/clock/r7s72100-clock.h
new file mode 100644
index 00000000000..5128f4d94f4
--- /dev/null
+++ b/include/dt-bindings/clock/r7s72100-clock.h
@@ -0,0 +1,41 @@
+/*
+ * Copyright (C) 2014 Renesas Solutions Corp.
+ * Copyright (C) 2014 Wolfram Sang, Sang Engineering <wsa@sang-engineering.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.
+ */
+
+#ifndef __DT_BINDINGS_CLOCK_R7S72100_H__
+#define __DT_BINDINGS_CLOCK_R7S72100_H__
+
+#define R7S72100_CLK_PLL	0
+
+/* MSTP3 */
+#define R7S72100_CLK_MTU2	3
+
+/* MSTP4 */
+#define R7S72100_CLK_SCIF0	7
+#define R7S72100_CLK_SCIF1	6
+#define R7S72100_CLK_SCIF2	5
+#define R7S72100_CLK_SCIF3	4
+#define R7S72100_CLK_SCIF4	3
+#define R7S72100_CLK_SCIF5	2
+#define R7S72100_CLK_SCIF6	1
+#define R7S72100_CLK_SCIF7	0
+
+/* MSTP9 */
+#define R7S72100_CLK_I2C0	7
+#define R7S72100_CLK_I2C1	6
+#define R7S72100_CLK_I2C2	5
+#define R7S72100_CLK_I2C3	4
+
+/* MSTP10 */
+#define R7S72100_CLK_SPI0	7
+#define R7S72100_CLK_SPI1	6
+#define R7S72100_CLK_SPI2	5
+#define R7S72100_CLK_SPI3	4
+#define R7S72100_CLK_SPI4	3
+
+#endif /* __DT_BINDINGS_CLOCK_R7S72100_H__ */
diff --git a/include/dt-bindings/clock/r8a7790-clock.h b/include/dt-bindings/clock/r8a7790-clock.h
index 6548a5fbcf4..1118f7a4bca 100644
--- a/include/dt-bindings/clock/r8a7790-clock.h
+++ b/include/dt-bindings/clock/r8a7790-clock.h
@@ -33,8 +33,8 @@
 #define R8A7790_CLK_TMU0		25
 #define R8A7790_CLK_VSP1_DU1		27
 #define R8A7790_CLK_VSP1_DU0		28
-#define R8A7790_CLK_VSP1_RT		30
-#define R8A7790_CLK_VSP1_SY		31
+#define R8A7790_CLK_VSP1_R		30
+#define R8A7790_CLK_VSP1_S		31
 
 /* MSTP2 */
 #define R8A7790_CLK_SCIFA2		2
@@ -50,6 +50,7 @@
 #define R8A7790_CLK_SYS_DMAC0		19
 
 /* MSTP3 */
+#define R8A7790_CLK_IIC2		0
 #define R8A7790_CLK_TPU0		4
 #define R8A7790_CLK_MMCIF1		5
 #define R8A7790_CLK_SDHI3		11
@@ -57,6 +58,8 @@
 #define R8A7790_CLK_SDHI1		13
 #define R8A7790_CLK_SDHI0		14
 #define R8A7790_CLK_MMCIF0		15
+#define R8A7790_CLK_IIC0		18
+#define R8A7790_CLK_IIC1		23
 #define R8A7790_CLK_SSUSB		28
 #define R8A7790_CLK_CMT1		29
 #define R8A7790_CLK_USBDMAC0		30
diff --git a/include/dt-bindings/clock/r8a7791-clock.h b/include/dt-bindings/clock/r8a7791-clock.h
index 30f82f286e2..b050d18437c 100644
--- a/include/dt-bindings/clock/r8a7791-clock.h
+++ b/include/dt-bindings/clock/r8a7791-clock.h
@@ -32,7 +32,7 @@
 #define R8A7791_CLK_TMU0		25
 #define R8A7791_CLK_VSP1_DU1		27
 #define R8A7791_CLK_VSP1_DU0		28
-#define R8A7791_CLK_VSP1_SY		31
+#define R8A7791_CLK_VSP1_S		31
 
 /* MSTP2 */
 #define R8A7791_CLK_SCIFA2		2
@@ -43,7 +43,8 @@
 #define R8A7791_CLK_SCIFB1		7
 #define R8A7791_CLK_MSIOF1		8
 #define R8A7791_CLK_SCIFB2		16
-#define R8A7791_CLK_DMAC		18
+#define R8A7791_CLK_SYS_DMAC1		18
+#define R8A7791_CLK_SYS_DMAC0		19
 
 /* MSTP3 */
 #define R8A7791_CLK_TPU0		4
@@ -51,6 +52,8 @@
 #define R8A7791_CLK_SDHI1		12
 #define R8A7791_CLK_SDHI0		14
 #define R8A7791_CLK_MMCIF0		15
+#define R8A7791_CLK_IIC0		18
+#define R8A7791_CLK_IIC1		23
 #define R8A7791_CLK_SSUSB		28
 #define R8A7791_CLK_CMT1		29
 #define R8A7791_CLK_USBDMAC0		30
@@ -61,6 +64,7 @@
 #define R8A7791_CLK_PWM			23
 
 /* MSTP7 */
+#define R8A7791_CLK_EHCI		3
 #define R8A7791_CLK_HSUSB		4
 #define R8A7791_CLK_HSCIF2		13
 #define R8A7791_CLK_SCIF5		14
diff --git a/include/dt-bindings/clock/s3c2410.h b/include/dt-bindings/clock/s3c2410.h
new file mode 100644
index 00000000000..352a7673fc6
--- /dev/null
+++ b/include/dt-bindings/clock/s3c2410.h
@@ -0,0 +1,62 @@
+/*
+ * Copyright (c) 2013 Heiko Stuebner <heiko@sntech.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.
+ *
+ * Device Tree binding constants clock controllers of Samsung S3C2410 and later.
+ */
+
+#ifndef _DT_BINDINGS_CLOCK_SAMSUNG_S3C2410_CLOCK_H
+#define _DT_BINDINGS_CLOCK_SAMSUNG_S3C2410_CLOCK_H
+
+/*
+ * Let each exported clock get a unique index, which is used on DT-enabled
+ * platforms to lookup the clock from a clock specifier. These indices are
+ * therefore considered an ABI and so must not be changed. This implies
+ * that new clocks should be added either in free spaces between clock groups
+ * or at the end.
+ */
+
+/* Core clocks. */
+
+/* id 1 is reserved */
+#define MPLL			2
+#define UPLL			3
+#define FCLK			4
+#define HCLK			5
+#define PCLK			6
+#define UCLK			7
+#define ARMCLK			8
+
+/* pclk-gates */
+#define PCLK_UART0		16
+#define PCLK_UART1		17
+#define PCLK_UART2		18
+#define PCLK_I2C		19
+#define PCLK_SDI		20
+#define PCLK_SPI		21
+#define PCLK_ADC		22
+#define PCLK_AC97		23
+#define PCLK_I2S		24
+#define PCLK_PWM		25
+#define PCLK_RTC		26
+#define PCLK_GPIO		27
+
+
+/* hclk-gates */
+#define HCLK_LCD		32
+#define HCLK_USBH		33
+#define HCLK_USBD		34
+#define HCLK_NAND		35
+#define HCLK_CAM		36
+
+
+#define CAMIF			40
+
+
+/* Total number of clocks. */
+#define NR_CLKS			(CAMIF + 1)
+
+#endif /* _DT_BINDINGS_CLOCK_SAMSUNG_S3C2443_CLOCK_H */
diff --git a/include/dt-bindings/clock/s3c2412.h b/include/dt-bindings/clock/s3c2412.h
new file mode 100644
index 00000000000..aac1dcfda81
--- /dev/null
+++ b/include/dt-bindings/clock/s3c2412.h
@@ -0,0 +1,73 @@
+/*
+ * Copyright (c) 2013 Heiko Stuebner <heiko@sntech.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.
+ *
+ * Device Tree binding constants clock controllers of Samsung S3C2412.
+ */
+
+#ifndef _DT_BINDINGS_CLOCK_SAMSUNG_S3C2412_CLOCK_H
+#define _DT_BINDINGS_CLOCK_SAMSUNG_S3C2412_CLOCK_H
+
+/*
+ * Let each exported clock get a unique index, which is used on DT-enabled
+ * platforms to lookup the clock from a clock specifier. These indices are
+ * therefore considered an ABI and so must not be changed. This implies
+ * that new clocks should be added either in free spaces between clock groups
+ * or at the end.
+ */
+
+/* Core clocks. */
+
+/* id 1 is reserved */
+#define MPLL			2
+#define UPLL			3
+#define MDIVCLK			4
+#define MSYSCLK			5
+#define USYSCLK			6
+#define HCLK			7
+#define PCLK			8
+#define ARMDIV			9
+#define ARMCLK			10
+
+
+/* Special clocks */
+#define SCLK_CAM		16
+#define SCLK_UART		17
+#define SCLK_I2S		18
+#define SCLK_USBD		19
+#define SCLK_USBH		20
+
+/* pclk-gates */
+#define PCLK_WDT		32
+#define PCLK_SPI		33
+#define PCLK_I2S		34
+#define PCLK_I2C		35
+#define PCLK_ADC		36
+#define PCLK_RTC		37
+#define PCLK_GPIO		38
+#define PCLK_UART2		39
+#define PCLK_UART1		40
+#define PCLK_UART0		41
+#define PCLK_SDI		42
+#define PCLK_PWM		43
+#define PCLK_USBD		44
+
+/* hclk-gates */
+#define HCLK_HALF		48
+#define HCLK_X2			49
+#define HCLK_SDRAM		50
+#define HCLK_USBH		51
+#define HCLK_LCD		52
+#define HCLK_NAND		53
+#define HCLK_DMA3		54
+#define HCLK_DMA2		55
+#define HCLK_DMA1		56
+#define HCLK_DMA0		57
+
+/* Total number of clocks. */
+#define NR_CLKS			(HCLK_DMA0 + 1)
+
+#endif /* _DT_BINDINGS_CLOCK_SAMSUNG_S3C2412_CLOCK_H */
diff --git a/include/dt-bindings/clock/s3c2443.h b/include/dt-bindings/clock/s3c2443.h
new file mode 100644
index 00000000000..37e66b054d6
--- /dev/null
+++ b/include/dt-bindings/clock/s3c2443.h
@@ -0,0 +1,92 @@
+/*
+ * Copyright (c) 2013 Heiko Stuebner <heiko@sntech.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.
+ *
+ * Device Tree binding constants clock controllers of Samsung S3C2443 and later.
+ */
+
+#ifndef _DT_BINDINGS_CLOCK_SAMSUNG_S3C2443_CLOCK_H
+#define _DT_BINDINGS_CLOCK_SAMSUNG_S3C2443_CLOCK_H
+
+/*
+ * Let each exported clock get a unique index, which is used on DT-enabled
+ * platforms to lookup the clock from a clock specifier. These indices are
+ * therefore considered an ABI and so must not be changed. This implies
+ * that new clocks should be added either in free spaces between clock groups
+ * or at the end.
+ */
+
+/* Core clocks. */
+#define MSYSCLK			1
+#define ESYSCLK			2
+#define ARMDIV			3
+#define ARMCLK			4
+#define HCLK			5
+#define PCLK			6
+
+/* Special clocks */
+#define SCLK_HSSPI0		16
+#define SCLK_FIMD		17
+#define SCLK_I2S0		18
+#define SCLK_I2S1		19
+#define SCLK_HSMMC1		20
+#define SCLK_HSMMC_EXT		21
+#define SCLK_CAM		22
+#define SCLK_UART		23
+#define SCLK_USBH		24
+
+/* Muxes */
+#define MUX_HSSPI0		32
+#define MUX_HSSPI1		33
+#define MUX_HSMMC0		34
+#define MUX_HSMMC1		35
+
+/* hclk-gates */
+#define HCLK_DMA0		48
+#define HCLK_DMA1		49
+#define HCLK_DMA2		50
+#define HCLK_DMA3		51
+#define HCLK_DMA4		52
+#define HCLK_DMA5		53
+#define HCLK_DMA6		54
+#define HCLK_DMA7		55
+#define HCLK_CAM		56
+#define HCLK_LCD		57
+#define HCLK_USBH		58
+#define HCLK_USBD		59
+#define HCLK_IROM		60
+#define HCLK_HSMMC0		61
+#define HCLK_HSMMC1		62
+#define HCLK_CFC		63
+#define HCLK_SSMC		64
+#define HCLK_DRAM		65
+#define HCLK_2D			66
+
+/* pclk-gates */
+#define PCLK_UART0		72
+#define PCLK_UART1		73
+#define PCLK_UART2		74
+#define PCLK_UART3		75
+#define PCLK_I2C0		76
+#define PCLK_SDI		77
+#define PCLK_SPI0		78
+#define PCLK_ADC		79
+#define PCLK_AC97		80
+#define PCLK_I2S0		81
+#define PCLK_PWM		82
+#define PCLK_WDT		83
+#define PCLK_RTC		84
+#define PCLK_GPIO		85
+#define PCLK_SPI1		86
+#define PCLK_CHIPID		87
+#define PCLK_I2C1		88
+#define PCLK_I2S1		89
+#define PCLK_PCM		90
+
+/* Total number of clocks. */
+#define NR_CLKS			(PCLK_PCM + 1)
+
+#endif /* _DT_BINDINGS_CLOCK_SAMSUNG_S3C2443_CLOCK_H */
diff --git a/include/dt-bindings/clock/stih415-clks.h b/include/dt-bindings/clock/stih415-clks.h
new file mode 100644
index 00000000000..0d2c7397e02
--- /dev/null
+++ b/include/dt-bindings/clock/stih415-clks.h
@@ -0,0 +1,15 @@
+/*
+ * This header provides constants clk index STMicroelectronics
+ * STiH415 SoC.
+ */
+#ifndef _CLK_STIH415
+#define _CLK_STIH415
+
+/* CLOCKGEN A0 */
+#define CLK_ICN_REG		0
+#define CLK_ETH1_PHY		4
+
+/* CLOCKGEN A1 */
+#define CLK_GMAC0_PHY		3
+
+#endif
diff --git a/include/dt-bindings/clock/stih416-clks.h b/include/dt-bindings/clock/stih416-clks.h
new file mode 100644
index 00000000000..552c779eb6a
--- /dev/null
+++ b/include/dt-bindings/clock/stih416-clks.h
@@ -0,0 +1,15 @@
+/*
+ * This header provides constants clk index STMicroelectronics
+ * STiH416 SoC.
+ */
+#ifndef _CLK_STIH416
+#define _CLK_STIH416
+
+/* CLOCKGEN A0 */
+#define CLK_ICN_REG		0
+#define CLK_ETH1_PHY		4
+
+/* CLOCKGEN A1 */
+#define CLK_GMAC0_PHY		3
+
+#endif
diff --git a/include/dt-bindings/clock/tegra124-car.h b/include/dt-bindings/clock/tegra124-car.h
index 8c1603b1066..433528ab516 100644
--- a/include/dt-bindings/clock/tegra124-car.h
+++ b/include/dt-bindings/clock/tegra124-car.h
@@ -29,7 +29,7 @@
 /* 10 (register bit affects spdif_in and spdif_out) */
 #define TEGRA124_CLK_I2S1 11
 #define TEGRA124_CLK_I2C1 12
-#define TEGRA124_CLK_NDFLASH 13
+/* 13 */
 #define TEGRA124_CLK_SDMMC1 14
 #define TEGRA124_CLK_SDMMC4 15
 /* 16 */
@@ -83,7 +83,7 @@
 
 /* 64 */
 #define TEGRA124_CLK_UARTD 65
-#define TEGRA124_CLK_UARTE 66
+/* 66 */
 #define TEGRA124_CLK_I2C3 67
 #define TEGRA124_CLK_SBC4 68
 #define TEGRA124_CLK_SDMMC3 69
@@ -97,7 +97,7 @@
 #define TEGRA124_CLK_TRACE 77
 #define TEGRA124_CLK_SOC_THERM 78
 #define TEGRA124_CLK_DTV 79
-#define TEGRA124_CLK_NDSPEED 80
+/* 80 */
 #define TEGRA124_CLK_I2CSLOW 81
 #define TEGRA124_CLK_DSIB 82
 #define TEGRA124_CLK_TSEC 83
diff --git a/include/dt-bindings/pinctrl/omap.h b/include/dt-bindings/pinctrl/omap.h
index b04528cd033..1c75b8ca522 100644
--- a/include/dt-bindings/pinctrl/omap.h
+++ b/include/dt-bindings/pinctrl/omap.h
@@ -62,12 +62,29 @@
 #define OMAP3630_CORE2_IOPAD(pa, val)	OMAP_IOPAD_OFFSET((pa), 0x25a0) (val)
 #define OMAP3_WKUP_IOPAD(pa, val)	OMAP_IOPAD_OFFSET((pa), 0x2a00) (val)
 #define AM33XX_IOPAD(pa, val)		OMAP_IOPAD_OFFSET((pa), 0x0800) (val)
-#define OMAP4_CORE_IOPAD(pa, val)	OMAP_IOPAD_OFFSET((pa), 0x0040) (val)
-#define OMAP4_WKUP_IOPAD(pa, val)	OMAP_IOPAD_OFFSET((pa), 0xe040) (val)
 #define AM4372_IOPAD(pa, val)		OMAP_IOPAD_OFFSET((pa), 0x0800) (val)
-#define OMAP5_CORE_IOPAD(pa, val)	OMAP_IOPAD_OFFSET((pa), 0x2840) (val)
-#define OMAP5_WKUP_IOPAD(pa, val)	OMAP_IOPAD_OFFSET((pa), 0xc840) (val)
 #define DRA7XX_CORE_IOPAD(pa, val)	OMAP_IOPAD_OFFSET((pa), 0x3400) (val)
 
+/*
+ * Macros to allow using the offset from the padconf physical address
+ * instead  of the offset from padconf base.
+ */
+#define OMAP_PADCONF_OFFSET(offset, base_offset)	((offset) - (base_offset))
+
+#define OMAP4_IOPAD(offset, val)	OMAP_PADCONF_OFFSET((offset), 0x0040) (val)
+#define OMAP5_IOPAD(offset, val)	OMAP_PADCONF_OFFSET((offset), 0x0040) (val)
+
+/*
+ * Define some commonly used pins configured by the boards.
+ * Note that some boards use alternative pins, so check
+ * the schematics before using these.
+ */
+#define OMAP3_UART1_RX		0x152
+#define OMAP3_UART2_RX		0x14a
+#define OMAP3_UART3_RX		0x16e
+#define OMAP4_UART2_RX		0xdc
+#define OMAP4_UART3_RX		0x104
+#define OMAP4_UART4_RX		0x11c
+
 #endif
 
diff --git a/include/dt-bindings/reset-controller/stih415-resets.h b/include/dt-bindings/reset-controller/stih415-resets.h
index c2f8a66913c..c2329fe29cf 100644
--- a/include/dt-bindings/reset-controller/stih415-resets.h
+++ b/include/dt-bindings/reset-controller/stih415-resets.h
@@ -22,5 +22,6 @@
 #define STIH415_USB0_SOFTRESET		3
 #define STIH415_USB1_SOFTRESET		4
 #define STIH415_USB2_SOFTRESET		5
+#define STIH415_KEYSCAN_SOFTRESET	6
 
 #endif /* _DT_BINDINGS_RESET_CONTROLLER_STIH415 */
diff --git a/include/dt-bindings/reset-controller/stih416-resets.h b/include/dt-bindings/reset-controller/stih416-resets.h
index 2127743f23e..fcf9af1ac0b 100644
--- a/include/dt-bindings/reset-controller/stih416-resets.h
+++ b/include/dt-bindings/reset-controller/stih416-resets.h
@@ -46,5 +46,6 @@
 #define STIH416_COMPO_A_SOFTRESET	25
 #define STIH416_VP8_DEC_SOFTRESET	26
 #define STIH416_VTG_MAIN_SOFTRESET	27
+#define STIH416_KEYSCAN_SOFTRESET	28
 
 #endif /* _DT_BINDINGS_RESET_CONTROLLER_STIH416 */
diff --git a/include/dt-bindings/reset/altr,rst-mgr.h b/include/dt-bindings/reset/altr,rst-mgr.h
new file mode 100644
index 00000000000..3f04908fb87
--- /dev/null
+++ b/include/dt-bindings/reset/altr,rst-mgr.h
@@ -0,0 +1,90 @@
+/*
+ * Copyright (c) 2014, Steffen Trumtrar <s.trumtrar@pengutronix.de>
+ *
+ * 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 _DT_BINDINGS_RESET_ALTR_RST_MGR_H
+#define _DT_BINDINGS_RESET_ALTR_RST_MGR_H
+
+/* MPUMODRST */
+#define CPU0_RESET		0
+#define CPU1_RESET		1
+#define WDS_RESET		2
+#define SCUPER_RESET		3
+#define L2_RESET		4
+
+/* PERMODRST */
+#define EMAC0_RESET		32
+#define EMAC1_RESET		33
+#define USB0_RESET		34
+#define USB1_RESET		35
+#define NAND_RESET		36
+#define QSPI_RESET		37
+#define L4WD0_RESET		38
+#define L4WD1_RESET		39
+#define OSC1TIMER0_RESET	40
+#define OSC1TIMER1_RESET	41
+#define SPTIMER0_RESET		42
+#define SPTIMER1_RESET		43
+#define I2C0_RESET		44
+#define I2C1_RESET		45
+#define I2C2_RESET		46
+#define I2C3_RESET		47
+#define UART0_RESET		48
+#define UART1_RESET		49
+#define SPIM0_RESET		50
+#define SPIM1_RESET		51
+#define SPIS0_RESET		52
+#define SPIS1_RESET		53
+#define SDMMC_RESET		54
+#define CAN0_RESET		55
+#define CAN1_RESET		56
+#define GPIO0_RESET		57
+#define GPIO1_RESET		58
+#define GPIO2_RESET		59
+#define DMA_RESET		60
+#define SDR_RESET		61
+
+/* PER2MODRST */
+#define DMAIF0_RESET		64
+#define DMAIF1_RESET		65
+#define DMAIF2_RESET		66
+#define DMAIF3_RESET		67
+#define DMAIF4_RESET		68
+#define DMAIF5_RESET		69
+#define DMAIF6_RESET		70
+#define DMAIF7_RESET		71
+
+/* BRGMODRST */
+#define HPS2FPGA_RESET		96
+#define LWHPS2FPGA_RESET	97
+#define FPGA2HPS_RESET		98
+
+/* MISCMODRST*/
+#define ROM_RESET		128
+#define OCRAM_RESET		129
+#define SYSMGR_RESET		130
+#define SYSMGRCOLD_RESET	131
+#define FPGAMGR_RESET		132
+#define ACPIDMAP_RESET		133
+#define S2F_RESET		134
+#define S2FCOLD_RESET		135
+#define NRSTPIN_RESET		136
+#define TIMESTAMPCOLD_RESET	137
+#define CLKMGRCOLD_RESET	138
+#define SCANMGR_RESET		139
+#define FRZCTRLCOLD_RESET	140
+#define SYSDBG_RESET		141
+#define DBG_RESET		142
+#define TAPCOLD_RESET		143
+#define SDRCOLD_RESET		144
+
+#endif
diff --git a/include/dt-bindings/soc/qcom,gsbi.h b/include/dt-bindings/soc/qcom,gsbi.h
new file mode 100644
index 00000000000..7ac4292333a
--- /dev/null
+++ b/include/dt-bindings/soc/qcom,gsbi.h
@@ -0,0 +1,26 @@
+/* Copyright (c) 2013, The Linux Foundation. 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 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 __DT_BINDINGS_QCOM_GSBI_H
+#define __DT_BINDINGS_QCOM_GSBI_H
+
+#define GSBI_PROT_IDLE		0
+#define GSBI_PROT_I2C_UIM	1
+#define GSBI_PROT_I2C		2
+#define GSBI_PROT_SPI		3
+#define GSBI_PROT_UART_W_FC	4
+#define GSBI_PROT_UIM		5
+#define GSBI_PROT_I2C_UART	6
+
+#define GSBI_CRCI_QUP		0
+#define GSBI_CRCI_UART		1
+
+#endif
diff --git a/include/linux/amba/bus.h b/include/linux/amba/bus.h
index 63b5eff0a80..fdd7e1b61f6 100644
--- a/include/linux/amba/bus.h
+++ b/include/linux/amba/bus.h
@@ -47,6 +47,7 @@ struct amba_driver {
 enum amba_vendor {
 	AMBA_VENDOR_ARM = 0x41,
 	AMBA_VENDOR_ST = 0x80,
+	AMBA_VENDOR_QCOM = 0x51,
 };
 
 extern struct bus_type amba_bustype;
diff --git a/include/linux/amba/mmci.h b/include/linux/amba/mmci.h
index 32a89cf5ec4..8c98113069c 100644
--- a/include/linux/amba/mmci.h
+++ b/include/linux/amba/mmci.h
@@ -6,28 +6,9 @@
 
 #include <linux/mmc/host.h>
 
-
-/*
- * These defines is places here due to access is needed from machine
- * configuration files. The ST Micro version does not have ROD and
- * reuse the voltage registers for direction settings.
- */
-#define MCI_ST_DATA2DIREN	(1 << 2)
-#define MCI_ST_CMDDIREN		(1 << 3)
-#define MCI_ST_DATA0DIREN	(1 << 4)
-#define MCI_ST_DATA31DIREN	(1 << 5)
-#define MCI_ST_FBCLKEN		(1 << 7)
-#define MCI_ST_DATA74DIREN	(1 << 8)
-
-/* Just some dummy forwarding */
-struct dma_chan;
-
 /**
  * struct mmci_platform_data - platform configuration for the MMCI
  * (also known as PL180) block.
- * @f_max: the maximum operational frequency for this host in this
- * platform configuration. When this is specified it takes precedence
- * over the module parameter for the same frequency.
  * @ocr_mask: available voltages on the 4 pins from the block, this
  * is ignored if a regulator is used, see the MMC_VDD_* masks in
  * mmc/host.h
@@ -42,37 +23,14 @@ struct dma_chan;
  * @gpio_wp: read this GPIO pin to see if the card is write protected
  * @gpio_cd: read this GPIO pin to detect card insertion
  * @cd_invert: true if the gpio_cd pin value is active low
- * @capabilities: the capabilities of the block as implemented in
- * this platform, signify anything MMC_CAP_* from mmc/host.h
- * @capabilities2: more capabilities, MMC_CAP2_* from mmc/host.h
- * @sigdir: a bit field indicating for what bits in the MMC bus the host
- * should enable signal direction indication.
- * @dma_filter: function used to select an appropriate RX and TX
- * DMA channel to be used for DMA, if and only if you're deploying the
- * generic DMA engine
- * @dma_rx_param: parameter passed to the DMA allocation
- * filter in order to select an appropriate RX channel. If
- * there is a bidirectional RX+TX channel, then just specify
- * this and leave dma_tx_param set to NULL
- * @dma_tx_param: parameter passed to the DMA allocation
- * filter in order to select an appropriate TX channel. If this
- * is NULL the driver will attempt to use the RX channel as a
- * bidirectional channel
  */
 struct mmci_platform_data {
-	unsigned int f_max;
 	unsigned int ocr_mask;
 	int (*ios_handler)(struct device *, struct mmc_ios *);
 	unsigned int (*status)(struct device *);
 	int	gpio_wp;
 	int	gpio_cd;
 	bool	cd_invert;
-	unsigned long capabilities;
-	unsigned long capabilities2;
-	u32 sigdir;
-	bool (*dma_filter)(struct dma_chan *chan, void *filter_param);
-	void *dma_rx_param;
-	void *dma_tx_param;
 };
 
 #endif
diff --git a/include/linux/bio.h b/include/linux/bio.h
index bba55082692..5a645769f02 100644
--- a/include/linux/bio.h
+++ b/include/linux/bio.h
@@ -333,7 +333,7 @@ static inline struct bio *bio_next_split(struct bio *bio, int sectors,
 
 extern struct bio_set *bioset_create(unsigned int, unsigned int);
 extern void bioset_free(struct bio_set *);
-extern mempool_t *biovec_create_pool(struct bio_set *bs, int pool_entries);
+extern mempool_t *biovec_create_pool(int pool_entries);
 
 extern struct bio *bio_alloc_bioset(gfp_t, int, struct bio_set *);
 extern void bio_put(struct bio *);
diff --git a/include/linux/blk-mq.h b/include/linux/blk-mq.h
index 0120451545d..c1512883310 100644
--- a/include/linux/blk-mq.h
+++ b/include/linux/blk-mq.h
@@ -8,7 +8,13 @@ struct blk_mq_tags;
 struct blk_mq_cpu_notifier {
 	struct list_head list;
 	void *data;
-	void (*notify)(void *data, unsigned long action, unsigned int cpu);
+	int (*notify)(void *data, unsigned long action, unsigned int cpu);
+};
+
+struct blk_mq_ctxmap {
+	unsigned int map_size;
+	unsigned int bits_per_word;
+	struct blk_align_bitmap *map;
 };
 
 struct blk_mq_hw_ctx {
@@ -18,7 +24,11 @@ struct blk_mq_hw_ctx {
 	} ____cacheline_aligned_in_smp;
 
 	unsigned long		state;		/* BLK_MQ_S_* flags */
-	struct delayed_work	delayed_work;
+	struct delayed_work	run_work;
+	struct delayed_work	delay_work;
+	cpumask_var_t		cpumask;
+	int			next_cpu;
+	int			next_cpu_batch;
 
 	unsigned long		flags;		/* BLK_MQ_F_* flags */
 
@@ -27,13 +37,13 @@ struct blk_mq_hw_ctx {
 
 	void			*driver_data;
 
+	struct blk_mq_ctxmap	ctx_map;
+
 	unsigned int		nr_ctx;
 	struct blk_mq_ctx	**ctxs;
-	unsigned int 		nr_ctx_map;
-	unsigned long		*ctx_map;
 
-	struct request		**rqs;
-	struct list_head	page_list;
+	unsigned int		wait_index;
+
 	struct blk_mq_tags	*tags;
 
 	unsigned long		queued;
@@ -41,31 +51,40 @@ struct blk_mq_hw_ctx {
 #define BLK_MQ_MAX_DISPATCH_ORDER	10
 	unsigned long		dispatched[BLK_MQ_MAX_DISPATCH_ORDER];
 
-	unsigned int		queue_depth;
 	unsigned int		numa_node;
 	unsigned int		cmd_size;	/* per-request extra data */
 
+	atomic_t		nr_active;
+
 	struct blk_mq_cpu_notifier	cpu_notifier;
 	struct kobject		kobj;
 };
 
-struct blk_mq_reg {
+struct blk_mq_tag_set {
 	struct blk_mq_ops	*ops;
 	unsigned int		nr_hw_queues;
-	unsigned int		queue_depth;
+	unsigned int		queue_depth;	/* max hw supported */
 	unsigned int		reserved_tags;
 	unsigned int		cmd_size;	/* per-request extra data */
 	int			numa_node;
 	unsigned int		timeout;
 	unsigned int		flags;		/* BLK_MQ_F_* */
+	void			*driver_data;
+
+	struct blk_mq_tags	**tags;
+
+	struct mutex		tag_list_lock;
+	struct list_head	tag_list;
 };
 
 typedef int (queue_rq_fn)(struct blk_mq_hw_ctx *, struct request *);
 typedef struct blk_mq_hw_ctx *(map_queue_fn)(struct request_queue *, const int);
-typedef struct blk_mq_hw_ctx *(alloc_hctx_fn)(struct blk_mq_reg *,unsigned int);
-typedef void (free_hctx_fn)(struct blk_mq_hw_ctx *, unsigned int);
 typedef int (init_hctx_fn)(struct blk_mq_hw_ctx *, void *, unsigned int);
 typedef void (exit_hctx_fn)(struct blk_mq_hw_ctx *, unsigned int);
+typedef int (init_request_fn)(void *, struct request *, unsigned int,
+		unsigned int, unsigned int);
+typedef void (exit_request_fn)(void *, struct request *, unsigned int,
+		unsigned int);
 
 struct blk_mq_ops {
 	/*
@@ -86,18 +105,20 @@ struct blk_mq_ops {
 	softirq_done_fn		*complete;
 
 	/*
-	 * Override for hctx allocations (should probably go)
-	 */
-	alloc_hctx_fn		*alloc_hctx;
-	free_hctx_fn		*free_hctx;
-
-	/*
 	 * Called when the block layer side of a hardware queue has been
 	 * set up, allowing the driver to allocate/init matching structures.
 	 * Ditto for exit/teardown.
 	 */
 	init_hctx_fn		*init_hctx;
 	exit_hctx_fn		*exit_hctx;
+
+	/*
+	 * Called for every command allocated by the block layer to allow
+	 * the driver to set up driver specific data.
+	 * Ditto for exit/teardown.
+	 */
+	init_request_fn		*init_request;
+	exit_request_fn		*exit_request;
 };
 
 enum {
@@ -107,18 +128,24 @@ enum {
 
 	BLK_MQ_F_SHOULD_MERGE	= 1 << 0,
 	BLK_MQ_F_SHOULD_SORT	= 1 << 1,
-	BLK_MQ_F_SHOULD_IPI	= 1 << 2,
+	BLK_MQ_F_TAG_SHARED	= 1 << 2,
+	BLK_MQ_F_SG_MERGE	= 1 << 3,
+	BLK_MQ_F_SYSFS_UP	= 1 << 4,
 
 	BLK_MQ_S_STOPPED	= 0,
+	BLK_MQ_S_TAG_ACTIVE	= 1,
 
 	BLK_MQ_MAX_DEPTH	= 2048,
+
+	BLK_MQ_CPU_WORK_BATCH	= 8,
 };
 
-struct request_queue *blk_mq_init_queue(struct blk_mq_reg *, void *);
+struct request_queue *blk_mq_init_queue(struct blk_mq_tag_set *);
 int blk_mq_register_disk(struct gendisk *);
 void blk_mq_unregister_disk(struct gendisk *);
-int blk_mq_init_commands(struct request_queue *, int (*init)(void *data, struct blk_mq_hw_ctx *, struct request *, unsigned int), void *data);
-void blk_mq_free_commands(struct request_queue *, void (*free)(void *data, struct blk_mq_hw_ctx *, struct request *, unsigned int), void *data);
+
+int blk_mq_alloc_tag_set(struct blk_mq_tag_set *set);
+void blk_mq_free_tag_set(struct blk_mq_tag_set *set);
 
 void blk_mq_flush_plug_list(struct blk_plug *plug, bool from_schedule);
 
@@ -126,28 +153,28 @@ void blk_mq_insert_request(struct request *, bool, bool, bool);
 void blk_mq_run_queues(struct request_queue *q, bool async);
 void blk_mq_free_request(struct request *rq);
 bool blk_mq_can_queue(struct blk_mq_hw_ctx *);
-struct request *blk_mq_alloc_request(struct request_queue *q, int rw, gfp_t gfp);
-struct request *blk_mq_alloc_reserved_request(struct request_queue *q, int rw, gfp_t gfp);
-struct request *blk_mq_rq_from_tag(struct request_queue *q, unsigned int tag);
+struct request *blk_mq_alloc_request(struct request_queue *q, int rw,
+		gfp_t gfp, bool reserved);
+struct request *blk_mq_tag_to_rq(struct blk_mq_hw_ctx *hctx, unsigned int tag);
 
 struct blk_mq_hw_ctx *blk_mq_map_queue(struct request_queue *, const int ctx_index);
-struct blk_mq_hw_ctx *blk_mq_alloc_single_hw_queue(struct blk_mq_reg *, unsigned int);
-void blk_mq_free_single_hw_queue(struct blk_mq_hw_ctx *, unsigned int);
+struct blk_mq_hw_ctx *blk_mq_alloc_single_hw_queue(struct blk_mq_tag_set *, unsigned int, int);
 
-bool blk_mq_end_io_partial(struct request *rq, int error,
-		unsigned int nr_bytes);
-static inline void blk_mq_end_io(struct request *rq, int error)
-{
-	bool done = !blk_mq_end_io_partial(rq, error, blk_rq_bytes(rq));
-	BUG_ON(!done);
-}
+void blk_mq_end_io(struct request *rq, int error);
+void __blk_mq_end_io(struct request *rq, int error);
 
+void blk_mq_requeue_request(struct request *rq);
+void blk_mq_add_to_requeue_list(struct request *rq, bool at_head);
+void blk_mq_kick_requeue_list(struct request_queue *q);
 void blk_mq_complete_request(struct request *rq);
 
 void blk_mq_stop_hw_queue(struct blk_mq_hw_ctx *hctx);
 void blk_mq_start_hw_queue(struct blk_mq_hw_ctx *hctx);
 void blk_mq_stop_hw_queues(struct request_queue *q);
-void blk_mq_start_stopped_hw_queues(struct request_queue *q);
+void blk_mq_start_hw_queues(struct request_queue *q);
+void blk_mq_start_stopped_hw_queues(struct request_queue *q, bool async);
+void blk_mq_delay_queue(struct blk_mq_hw_ctx *hctx, unsigned long msecs);
+void blk_mq_tag_busy_iter(struct blk_mq_tags *tags, void (*fn)(void *data, unsigned long *), void *data);
 
 /*
  * Driver command data is immediately after the request. So subtract request
@@ -162,12 +189,6 @@ static inline void *blk_mq_rq_to_pdu(struct request *rq)
 	return (void *) rq + sizeof(*rq);
 }
 
-static inline struct request *blk_mq_tag_to_rq(struct blk_mq_hw_ctx *hctx,
-					       unsigned int tag)
-{
-	return hctx->rqs[tag];
-}
-
 #define queue_for_each_hw_ctx(q, hctx, i)				\
 	for ((i) = 0; (i) < (q)->nr_hw_queues &&			\
 	     ({ hctx = (q)->queue_hw_ctx[i]; 1; }); (i)++)
diff --git a/include/linux/blk_types.h b/include/linux/blk_types.h
index aa0eaa2d0bd..d8e4cea23a2 100644
--- a/include/linux/blk_types.h
+++ b/include/linux/blk_types.h
@@ -190,6 +190,7 @@ enum rq_flag_bits {
 	__REQ_PM,		/* runtime pm request */
 	__REQ_END,		/* last of chain of requests */
 	__REQ_HASHED,		/* on IO scheduler merge hash */
+	__REQ_MQ_INFLIGHT,	/* track inflight for MQ */
 	__REQ_NR_BITS,		/* stops here */
 };
 
@@ -243,5 +244,6 @@ enum rq_flag_bits {
 #define REQ_PM			(1ULL << __REQ_PM)
 #define REQ_END			(1ULL << __REQ_END)
 #define REQ_HASHED		(1ULL << __REQ_HASHED)
+#define REQ_MQ_INFLIGHT		(1ULL << __REQ_MQ_INFLIGHT)
 
 #endif /* __LINUX_BLK_TYPES_H */
diff --git a/include/linux/blkdev.h b/include/linux/blkdev.h
index 0d84981ee03..8aba35f46f8 100644
--- a/include/linux/blkdev.h
+++ b/include/linux/blkdev.h
@@ -90,15 +90,15 @@ enum rq_cmd_type_bits {
 #define BLK_MAX_CDB	16
 
 /*
- * try to put the fields that are referenced together in the same cacheline.
- * if you modify this structure, be sure to check block/blk-core.c:blk_rq_init()
- * as well!
+ * Try to put the fields that are referenced together in the same cacheline.
+ *
+ * If you modify this structure, make sure to update blk_rq_init() and
+ * especially blk_mq_rq_ctx_init() to take care of the added fields.
  */
 struct request {
 	struct list_head queuelist;
 	union {
 		struct call_single_data csd;
-		struct work_struct mq_flush_work;
 		unsigned long fifo_time;
 	};
 
@@ -178,7 +178,6 @@ struct request {
 	unsigned short ioprio;
 
 	void *special;		/* opaque pointer available for LLD use */
-	char *buffer;		/* kaddr of the current segment if available */
 
 	int tag;
 	int errors;
@@ -463,6 +462,10 @@ struct request_queue {
 	struct request		*flush_rq;
 	spinlock_t		mq_flush_lock;
 
+	struct list_head	requeue_list;
+	spinlock_t		requeue_lock;
+	struct work_struct	requeue_work;
+
 	struct mutex		sysfs_lock;
 
 	int			bypass_depth;
@@ -481,6 +484,9 @@ struct request_queue {
 	wait_queue_head_t	mq_freeze_wq;
 	struct percpu_counter	mq_usage_counter;
 	struct list_head	all_q_node;
+
+	struct blk_mq_tag_set	*tag_set;
+	struct list_head	tag_set_list;
 };
 
 #define QUEUE_FLAG_QUEUED	1	/* uses generic tag queueing */
@@ -504,6 +510,7 @@ struct request_queue {
 #define QUEUE_FLAG_SAME_FORCE  18	/* force complete on same CPU */
 #define QUEUE_FLAG_DEAD        19	/* queue tear-down finished */
 #define QUEUE_FLAG_INIT_DONE   20	/* queue is initialized */
+#define QUEUE_FLAG_NO_SG_MERGE 21	/* don't attempt to merge SG segments*/
 
 #define QUEUE_FLAG_DEFAULT	((1 << QUEUE_FLAG_IO_STAT) |		\
 				 (1 << QUEUE_FLAG_STACKABLE)	|	\
@@ -613,6 +620,15 @@ static inline void queue_flag_clear(unsigned int flag, struct request_queue *q)
 
 #define rq_data_dir(rq)		(((rq)->cmd_flags & 1) != 0)
 
+/*
+ * Driver can handle struct request, if it either has an old style
+ * request_fn defined, or is blk-mq based.
+ */
+static inline bool queue_is_rq_based(struct request_queue *q)
+{
+	return q->request_fn || q->mq_ops;
+}
+
 static inline unsigned int blk_queue_cluster(struct request_queue *q)
 {
 	return q->limits.cluster;
@@ -937,6 +953,7 @@ extern struct request *blk_fetch_request(struct request_queue *q);
  */
 extern bool blk_update_request(struct request *rq, int error,
 			       unsigned int nr_bytes);
+extern void blk_finish_request(struct request *rq, int error);
 extern bool blk_end_request(struct request *rq, int error,
 			    unsigned int nr_bytes);
 extern void blk_end_request_all(struct request *rq, int error);
@@ -1053,7 +1070,6 @@ static inline void blk_post_runtime_resume(struct request_queue *q, int err) {}
  * schedule() where blk_schedule_flush_plug() is called.
  */
 struct blk_plug {
-	unsigned long magic; /* detect uninitialized use-cases */
 	struct list_head list; /* requests */
 	struct list_head mq_list; /* blk-mq requests */
 	struct list_head cb_list; /* md requires an unplug callback */
@@ -1102,7 +1118,8 @@ static inline bool blk_needs_flush_plug(struct task_struct *tsk)
 /*
  * tag stuff
  */
-#define blk_rq_tagged(rq)		((rq)->cmd_flags & REQ_QUEUED)
+#define blk_rq_tagged(rq) \
+	((rq)->mq_ctx || ((rq)->cmd_flags & REQ_QUEUED))
 extern int blk_queue_start_tag(struct request_queue *, struct request *);
 extern struct request *blk_queue_find_tag(struct request_queue *, int);
 extern void blk_queue_end_tag(struct request_queue *, struct request *);
@@ -1370,8 +1387,9 @@ static inline void put_dev_sector(Sector p)
 }
 
 struct work_struct;
-int kblockd_schedule_work(struct request_queue *q, struct work_struct *work);
-int kblockd_schedule_delayed_work(struct request_queue *q, struct delayed_work *dwork, unsigned long delay);
+int kblockd_schedule_work(struct work_struct *work);
+int kblockd_schedule_delayed_work(struct delayed_work *dwork, unsigned long delay);
+int kblockd_schedule_delayed_work_on(int cpu, struct delayed_work *dwork, unsigned long delay);
 
 #ifdef CONFIG_BLK_CGROUP
 /*
diff --git a/include/linux/cgroup.h b/include/linux/cgroup.h
index c2515851c1a..d60904b9e50 100644
--- a/include/linux/cgroup.h
+++ b/include/linux/cgroup.h
@@ -473,6 +473,7 @@ struct cftype {
 };
 
 extern struct cgroup_root cgrp_dfl_root;
+extern struct css_set init_css_set;
 
 static inline bool cgroup_on_dfl(const struct cgroup *cgrp)
 {
@@ -700,6 +701,20 @@ static inline struct cgroup_subsys_state *task_css(struct task_struct *task,
 	return task_css_check(task, subsys_id, false);
 }
 
+/**
+ * task_css_is_root - test whether a task belongs to the root css
+ * @task: the target task
+ * @subsys_id: the target subsystem ID
+ *
+ * Test whether @task belongs to the root css on the specified subsystem.
+ * May be invoked in any context.
+ */
+static inline bool task_css_is_root(struct task_struct *task, int subsys_id)
+{
+	return task_css_check(task, subsys_id, true) ==
+		init_css_set.subsys[subsys_id];
+}
+
 static inline struct cgroup *task_cgroup(struct task_struct *task,
 					 int subsys_id)
 {
diff --git a/include/linux/clk/at91_pmc.h b/include/linux/clk/at91_pmc.h
index a6911ebbd02..de4268d4987 100644
--- a/include/linux/clk/at91_pmc.h
+++ b/include/linux/clk/at91_pmc.h
@@ -155,6 +155,7 @@ extern void __iomem *at91_pmc_base;
 #define		AT91_PMC_LOCKB		(1 <<  2)		/* PLLB Lock */
 #define		AT91_PMC_MCKRDY		(1 <<  3)		/* Master Clock */
 #define		AT91_PMC_LOCKU		(1 <<  6)		/* UPLL Lock [some SAM9] */
+#define		AT91_PMC_OSCSEL		(1 <<  7)		/* Slow Oscillator Selection [some SAM9] */
 #define		AT91_PMC_PCK0RDY	(1 <<  8)		/* Programmable Clock 0 */
 #define		AT91_PMC_PCK1RDY	(1 <<  9)		/* Programmable Clock 1 */
 #define		AT91_PMC_PCK2RDY	(1 << 10)		/* Programmable Clock 2 */
diff --git a/include/linux/connector.h b/include/linux/connector.h
index be9c4747d51..f8fe8637d77 100644
--- a/include/linux/connector.h
+++ b/include/linux/connector.h
@@ -71,6 +71,7 @@ struct cn_dev {
 int cn_add_callback(struct cb_id *id, const char *name,
 		    void (*callback)(struct cn_msg *, struct netlink_skb_parms *));
 void cn_del_callback(struct cb_id *);
+int cn_netlink_send_mult(struct cn_msg *msg, u16 len, u32 portid, u32 group, gfp_t gfp_mask);
 int cn_netlink_send(struct cn_msg *msg, u32 portid, u32 group, gfp_t gfp_mask);
 
 int cn_queue_add_callback(struct cn_queue_dev *dev, const char *name,
diff --git a/include/linux/console_struct.h b/include/linux/console_struct.h
index 7f0c3290856..e859c98d176 100644
--- a/include/linux/console_struct.h
+++ b/include/linux/console_struct.h
@@ -17,6 +17,7 @@
 #include <linux/workqueue.h>
 
 struct vt_struct;
+struct uni_pagedir;
 
 #define NPAR 16
 
@@ -104,8 +105,8 @@ struct vc_data {
 	unsigned int	vc_bell_pitch;		/* Console bell pitch */
 	unsigned int	vc_bell_duration;	/* Console bell duration */
 	struct vc_data **vc_display_fg;		/* [!] Ptr to var holding fg console for this display */
-	unsigned long	vc_uni_pagedir;
-	unsigned long	*vc_uni_pagedir_loc;  /* [!] Location of uni_pagedir variable for this console */
+	struct uni_pagedir *vc_uni_pagedir;
+	struct uni_pagedir **vc_uni_pagedir_loc; /* [!] Location of uni_pagedir variable for this console */
 	bool vc_panic_force_write; /* when oops/panic this VC can accept forced output/blanking */
 	/* additional information is in vt_kern.h */
 };
diff --git a/include/linux/dcache.h b/include/linux/dcache.h
index 3b9bfdb83ba..3c7ec327ebd 100644
--- a/include/linux/dcache.h
+++ b/include/linux/dcache.h
@@ -221,6 +221,8 @@ struct dentry_operations {
 #define DCACHE_SYMLINK_TYPE		0x00300000 /* Symlink */
 #define DCACHE_FILE_TYPE		0x00400000 /* Other file type */
 
+#define DCACHE_MAY_FREE			0x00800000
+
 extern seqlock_t rename_lock;
 
 static inline int dname_external(const struct dentry *dentry)
diff --git a/include/linux/device.h b/include/linux/device.h
index 233bbbeb768..580e3eed4b7 100644
--- a/include/linux/device.h
+++ b/include/linux/device.h
@@ -566,12 +566,6 @@ extern int __must_check device_create_bin_file(struct device *dev,
 					const struct bin_attribute *attr);
 extern void device_remove_bin_file(struct device *dev,
 				   const struct bin_attribute *attr);
-extern int device_schedule_callback_owner(struct device *dev,
-		void (*func)(struct device *dev), struct module *owner);
-
-/* This is a macro to avoid include problems with THIS_MODULE */
-#define device_schedule_callback(dev, func)			\
-	device_schedule_callback_owner(dev, func, THIS_MODULE)
 
 /* device resource management */
 typedef void (*dr_release_t)(struct device *dev, void *res);
@@ -629,6 +623,12 @@ static inline void *devm_kcalloc(struct device *dev,
 }
 extern void devm_kfree(struct device *dev, void *p);
 extern char *devm_kstrdup(struct device *dev, const char *s, gfp_t gfp);
+extern void *devm_kmemdup(struct device *dev, const void *src, size_t len,
+			  gfp_t gfp);
+
+extern unsigned long devm_get_free_pages(struct device *dev,
+					 gfp_t gfp_mask, unsigned int order);
+extern void devm_free_pages(struct device *dev, unsigned long addr);
 
 void __iomem *devm_ioremap_resource(struct device *dev, struct resource *res);
 void __iomem *devm_request_and_ioremap(struct device *dev,
@@ -679,6 +679,7 @@ struct acpi_dev_node {
  * 		variants, which GPIO pins act in what additional roles, and so
  * 		on.  This shrinks the "Board Support Packages" (BSPs) and
  * 		minimizes board-specific #ifdefs in drivers.
+ * @driver_data: Private pointer for driver specific info.
  * @power:	For device power management.
  * 		See Documentation/power/devices.txt for details.
  * @pm_domain:	Provide callbacks that are executed during system suspend,
@@ -740,6 +741,8 @@ struct device {
 					   device */
 	void		*platform_data;	/* Platform specific data, device
 					   core doesn't touch it */
+	void		*driver_data;	/* Driver data, set and get with
+					   dev_set/get_drvdata */
 	struct dev_pm_info	power;
 	struct dev_pm_domain	*pm_domain;
 
@@ -829,6 +832,16 @@ static inline void set_dev_node(struct device *dev, int node)
 }
 #endif
 
+static inline void *dev_get_drvdata(const struct device *dev)
+{
+	return dev->driver_data;
+}
+
+static inline void dev_set_drvdata(struct device *dev, void *data)
+{
+	dev->driver_data = data;
+}
+
 static inline struct pm_subsys_data *dev_to_psd(struct device *dev)
 {
 	return dev ? dev->power.subsys_data : NULL;
@@ -913,8 +926,6 @@ extern int device_move(struct device *dev, struct device *new_parent,
 extern const char *device_get_devnode(struct device *dev,
 				      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);
 
 static inline bool device_supports_offline(struct device *dev)
 {
@@ -932,10 +943,7 @@ extern int device_online(struct device *dev);
 extern struct device *__root_device_register(const char *name,
 					     struct module *owner);
 
-/*
- * This is a macro to avoid include problems with THIS_MODULE,
- * just as per what is done for device_schedule_callback() above.
- */
+/* This is a macro to avoid include problems with THIS_MODULE */
 #define root_device_register(name) \
 	__root_device_register(name, THIS_MODULE)
 
diff --git a/include/linux/dma-mapping.h b/include/linux/dma-mapping.h
index fd4aee29ad1..0c3eab1e39a 100644
--- a/include/linux/dma-mapping.h
+++ b/include/linux/dma-mapping.h
@@ -8,6 +8,12 @@
 #include <linux/dma-direction.h>
 #include <linux/scatterlist.h>
 
+/*
+ * A dma_addr_t can hold any valid DMA or bus address for the platform.
+ * It can be given to a device to use as a DMA source or target.  A CPU cannot
+ * reference a dma_addr_t directly because there may be translation between
+ * its physical address space and the bus address space.
+ */
 struct dma_map_ops {
 	void* (*alloc)(struct device *dev, size_t size,
 				dma_addr_t *dma_handle, gfp_t gfp,
@@ -186,7 +192,7 @@ static inline int dma_get_cache_alignment(void)
 
 #ifndef ARCH_HAS_DMA_DECLARE_COHERENT_MEMORY
 static inline int
-dma_declare_coherent_memory(struct device *dev, dma_addr_t bus_addr,
+dma_declare_coherent_memory(struct device *dev, phys_addr_t phys_addr,
 			    dma_addr_t device_addr, size_t size, int flags)
 {
 	return 0;
@@ -217,13 +223,14 @@ extern void *dmam_alloc_noncoherent(struct device *dev, size_t size,
 extern void dmam_free_noncoherent(struct device *dev, size_t size, void *vaddr,
 				  dma_addr_t dma_handle);
 #ifdef ARCH_HAS_DMA_DECLARE_COHERENT_MEMORY
-extern int dmam_declare_coherent_memory(struct device *dev, dma_addr_t bus_addr,
+extern int dmam_declare_coherent_memory(struct device *dev,
+					phys_addr_t phys_addr,
 					dma_addr_t device_addr, size_t size,
 					int flags);
 extern void dmam_release_declared_memory(struct device *dev);
 #else /* ARCH_HAS_DMA_DECLARE_COHERENT_MEMORY */
 static inline int dmam_declare_coherent_memory(struct device *dev,
-				dma_addr_t bus_addr, dma_addr_t device_addr,
+				phys_addr_t phys_addr, dma_addr_t device_addr,
 				size_t size, gfp_t gfp)
 {
 	return 0;
diff --git a/include/linux/dmaengine.h b/include/linux/dmaengine.h
index 8300fb87b84..72cb0ddb967 100644
--- a/include/linux/dmaengine.h
+++ b/include/linux/dmaengine.h
@@ -429,6 +429,7 @@ typedef bool (*dma_filter_fn)(struct dma_chan *chan, void *filter_param);
 typedef void (*dma_async_tx_callback)(void *dma_async_param);
 
 struct dmaengine_unmap_data {
+	u8 map_cnt;
 	u8 to_cnt;
 	u8 from_cnt;
 	u8 bidi_cnt;
diff --git a/include/linux/extcon.h b/include/linux/extcon.h
index f488145bb2d..36f49c405df 100644
--- a/include/linux/extcon.h
+++ b/include/linux/extcon.h
@@ -185,9 +185,22 @@ struct extcon_specific_cable_nb {
  */
 extern int extcon_dev_register(struct extcon_dev *edev);
 extern void extcon_dev_unregister(struct extcon_dev *edev);
+extern int devm_extcon_dev_register(struct device *dev,
+				    struct extcon_dev *edev);
+extern void devm_extcon_dev_unregister(struct device *dev,
+				       struct extcon_dev *edev);
 extern struct extcon_dev *extcon_get_extcon_dev(const char *extcon_name);
 
 /*
+ * Following APIs control the memory of extcon device.
+ */
+extern struct extcon_dev *extcon_dev_allocate(const char **cables);
+extern void extcon_dev_free(struct extcon_dev *edev);
+extern struct extcon_dev *devm_extcon_dev_allocate(struct device *dev,
+						   const char **cables);
+extern void devm_extcon_dev_free(struct device *dev, struct extcon_dev *edev);
+
+/*
  * get/set/update_state access the 32b encoded state value, which represents
  * states of all possible cables of the multistate port. For example, if one
  * calls extcon_set_state(edev, 0x7), it may mean that all the three cables
@@ -254,6 +267,30 @@ static inline int extcon_dev_register(struct extcon_dev *edev)
 
 static inline void extcon_dev_unregister(struct extcon_dev *edev) { }
 
+static inline int devm_extcon_dev_register(struct device *dev,
+					   struct extcon_dev *edev)
+{
+	return -EINVAL;
+}
+
+static inline void devm_extcon_dev_unregister(struct device *dev,
+					      struct extcon_dev *edev) { }
+
+static inline struct extcon_dev *extcon_dev_allocate(const char **cables)
+{
+	return ERR_PTR(-ENOSYS);
+}
+
+static inline void extcon_dev_free(struct extcon_dev *edev) { }
+
+static inline struct extcon_dev *devm_extcon_dev_allocate(struct device *dev,
+							  const char **cables)
+{
+	return ERR_PTR(-ENOSYS);
+}
+
+static inline void devm_extcon_dev_free(struct extcon_dev *edev) { }
+
 static inline u32 extcon_get_state(struct extcon_dev *edev)
 {
 	return 0;
diff --git a/include/linux/fs.h b/include/linux/fs.h
index 7a9c5bca2b7..878031227c5 100644
--- a/include/linux/fs.h
+++ b/include/linux/fs.h
@@ -815,7 +815,7 @@ static inline struct file *get_file(struct file *f)
 #define FL_SLEEP	128	/* A blocking lock */
 #define FL_DOWNGRADE_PENDING	256 /* Lease is being downgraded */
 #define FL_UNLOCK_PENDING	512 /* Lease is being broken */
-#define FL_FILE_PVT	1024	/* lock is private to the file */
+#define FL_OFDLCK	1024	/* lock is "owned" by struct file */
 
 /*
  * Special return value from posix_lock_file() and vfs_lock_file() for
diff --git a/include/linux/ftrace.h b/include/linux/ftrace.h
index 9212b017bc7..ae9504b4b67 100644
--- a/include/linux/ftrace.h
+++ b/include/linux/ftrace.h
@@ -535,6 +535,7 @@ static inline int ftrace_modify_call(struct dyn_ftrace *rec, unsigned long old_a
 extern int ftrace_arch_read_dyn_info(char *buf, int size);
 
 extern int skip_trace(unsigned long ip);
+extern void ftrace_module_init(struct module *mod);
 
 extern void ftrace_disable_daemon(void);
 extern void ftrace_enable_daemon(void);
@@ -544,6 +545,7 @@ static inline int ftrace_force_update(void) { return 0; }
 static inline void ftrace_disable_daemon(void) { }
 static inline void ftrace_enable_daemon(void) { }
 static inline void ftrace_release_mod(struct module *mod) {}
+static inline void ftrace_module_init(struct module *mod) {}
 static inline __init int register_ftrace_command(struct ftrace_func_command *cmd)
 {
 	return -EINVAL;
diff --git a/include/linux/goldfish.h b/include/linux/goldfish.h
new file mode 100644
index 00000000000..569236e6b2b
--- /dev/null
+++ b/include/linux/goldfish.h
@@ -0,0 +1,15 @@
+#ifndef __LINUX_GOLDFISH_H
+#define __LINUX_GOLDFISH_H
+
+/* Helpers for Goldfish virtual platform */
+
+static inline void gf_write64(unsigned long data,
+		void __iomem *portl, void __iomem *porth)
+{
+	writel((u32)data, portl);
+#ifdef CONFIG_64BIT
+	writel(data>>32, porth);
+#endif
+}
+
+#endif /* __LINUX_GOLDFISH_H */
diff --git a/include/linux/gpio/consumer.h b/include/linux/gpio/consumer.h
index bed128e8f4b..05e53ccb708 100644
--- a/include/linux/gpio/consumer.h
+++ b/include/linux/gpio/consumer.h
@@ -1,6 +1,7 @@
 #ifndef __LINUX_GPIO_CONSUMER_H
 #define __LINUX_GPIO_CONSUMER_H
 
+#include <linux/bug.h>
 #include <linux/err.h>
 #include <linux/kernel.h>
 
@@ -23,6 +24,12 @@ struct gpio_desc *__must_check gpiod_get(struct device *dev,
 struct gpio_desc *__must_check gpiod_get_index(struct device *dev,
 					       const char *con_id,
 					       unsigned int idx);
+struct gpio_desc *__must_check gpiod_get_optional(struct device *dev,
+						  const char *con_id);
+struct gpio_desc *__must_check gpiod_get_index_optional(struct device *dev,
+							const char *con_id,
+							unsigned int index);
+
 void gpiod_put(struct gpio_desc *desc);
 
 struct gpio_desc *__must_check devm_gpiod_get(struct device *dev,
@@ -30,6 +37,12 @@ struct gpio_desc *__must_check devm_gpiod_get(struct device *dev,
 struct gpio_desc *__must_check devm_gpiod_get_index(struct device *dev,
 						    const char *con_id,
 						    unsigned int idx);
+struct gpio_desc *__must_check devm_gpiod_get_optional(struct device *dev,
+						       const char *con_id);
+struct gpio_desc *__must_check
+devm_gpiod_get_index_optional(struct device *dev, const char *con_id,
+			      unsigned int index);
+
 void devm_gpiod_put(struct device *dev, struct gpio_desc *desc);
 
 int gpiod_get_direction(const struct gpio_desc *desc);
@@ -73,6 +86,20 @@ static inline struct gpio_desc *__must_check gpiod_get_index(struct device *dev,
 {
 	return ERR_PTR(-ENOSYS);
 }
+
+static inline struct gpio_desc *__must_check
+gpiod_get_optional(struct device *dev, const char *con_id)
+{
+	return ERR_PTR(-ENOSYS);
+}
+
+static inline struct gpio_desc *__must_check
+gpiod_get_index_optional(struct device *dev, const char *con_id,
+			 unsigned int index)
+{
+	return ERR_PTR(-ENOSYS);
+}
+
 static inline void gpiod_put(struct gpio_desc *desc)
 {
 	might_sleep();
@@ -93,6 +120,20 @@ struct gpio_desc *__must_check devm_gpiod_get_index(struct device *dev,
 {
 	return ERR_PTR(-ENOSYS);
 }
+
+static inline struct gpio_desc *__must_check
+devm_gpiod_get_optional(struct device *dev, const char *con_id)
+{
+	return ERR_PTR(-ENOSYS);
+}
+
+static inline struct gpio_desc *__must_check
+devm_gpiod_get_index_optional(struct device *dev, const char *con_id,
+			      unsigned int index)
+{
+	return ERR_PTR(-ENOSYS);
+}
+
 static inline void devm_gpiod_put(struct device *dev, struct gpio_desc *desc)
 {
 	might_sleep();
diff --git a/include/linux/gpio/driver.h b/include/linux/gpio/driver.h
index 1827b43966d..573e4f3243d 100644
--- a/include/linux/gpio/driver.h
+++ b/include/linux/gpio/driver.h
@@ -51,7 +51,10 @@ struct seq_file;
  *      format specifier for an unsigned int.  It is substituted by the actual
  *      number of the gpio.
  * @can_sleep: flag must be set iff get()/set() methods sleep, as they
- *	must while accessing GPIO expander chips over I2C or SPI
+ *	must while accessing GPIO expander chips over I2C or SPI. This
+ *	implies that if the chip supports IRQs, these IRQs need to be threaded
+ *	as the chip access may sleep when e.g. reading out the IRQ status
+ *	registers.
  * @exported: flags if the gpiochip is exported for use from sysfs. Private.
  *
  * A gpio_chip can help platforms abstract various sources of GPIOs so
diff --git a/include/linux/hid-sensor-hub.h b/include/linux/hid-sensor-hub.h
index b70cfd7ff29..51f7ccadf92 100644
--- a/include/linux/hid-sensor-hub.h
+++ b/include/linux/hid-sensor-hub.h
@@ -189,7 +189,7 @@ struct hid_sensor_common {
 	struct hid_sensor_hub_device *hsdev;
 	struct platform_device *pdev;
 	unsigned usage_id;
-	bool data_ready;
+	atomic_t data_ready;
 	struct iio_trigger *trigger;
 	struct hid_sensor_hub_attribute_info poll;
 	struct hid_sensor_hub_attribute_info report_state;
@@ -223,4 +223,10 @@ int hid_sensor_read_samp_freq_value(struct hid_sensor_common *st,
 int hid_sensor_get_usage_index(struct hid_sensor_hub_device *hsdev,
 				u32 report_id, int field_index, u32 usage_id);
 
+int hid_sensor_format_scale(u32 usage_id,
+			    struct hid_sensor_hub_attribute_info *attr_info,
+			    int *val0, int *val1);
+
+s32 hid_sensor_read_poll_value(struct hid_sensor_common *st);
+
 #endif
diff --git a/include/linux/hid-sensor-ids.h b/include/linux/hid-sensor-ids.h
index 14ead9e8eda..109f0e633e0 100644
--- a/include/linux/hid-sensor-ids.h
+++ b/include/linux/hid-sensor-ids.h
@@ -76,6 +76,7 @@
 #define HID_USAGE_SENSOR_ORIENT_TILT_Y				0x200480
 #define HID_USAGE_SENSOR_ORIENT_TILT_Z				0x200481
 
+#define HID_USAGE_SENSOR_DEVICE_ORIENTATION			0x20008A
 #define HID_USAGE_SENSOR_ORIENT_ROTATION_MATRIX			0x200482
 #define HID_USAGE_SENSOR_ORIENT_QUATERNION			0x200483
 #define HID_USAGE_SENSOR_ORIENT_MAGN_FLUX			0x200484
diff --git a/include/linux/hsi/hsi.h b/include/linux/hsi/hsi.h
index 39bfd5b8907..3ec06300d53 100644
--- a/include/linux/hsi/hsi.h
+++ b/include/linux/hsi/hsi.h
@@ -68,17 +68,31 @@ enum {
 };
 
 /**
+ * struct hsi_channel - channel resource used by the hsi clients
+ * @id: Channel number
+ * @name: Channel name
+ */
+struct hsi_channel {
+	unsigned int	id;
+	const char	*name;
+};
+
+/**
  * struct hsi_config - Configuration for RX/TX HSI modules
  * @mode: Bit transmission mode (STREAM or FRAME)
- * @channels: Number of channels to use [1..16]
+ * @channels: Channel resources used by the client
+ * @num_channels: Number of channel resources
+ * @num_hw_channels: Number of channels the transceiver is configured for [1..16]
  * @speed: Max bit transmission speed (Kbit/s)
  * @flow: RX flow type (SYNCHRONIZED or PIPELINE)
  * @arb_mode: Arbitration mode for TX frame (Round robin, priority)
  */
 struct hsi_config {
-	unsigned int	mode;
-	unsigned int	channels;
-	unsigned int	speed;
+	unsigned int		mode;
+	struct hsi_channel	*channels;
+	unsigned int		num_channels;
+	unsigned int		num_hw_channels;
+	unsigned int		speed;
 	union {
 		unsigned int	flow;		/* RX only */
 		unsigned int	arb_mode;	/* TX only */
@@ -282,6 +296,21 @@ struct hsi_controller *hsi_alloc_controller(unsigned int n_ports, gfp_t flags);
 void hsi_put_controller(struct hsi_controller *hsi);
 int hsi_register_controller(struct hsi_controller *hsi);
 void hsi_unregister_controller(struct hsi_controller *hsi);
+struct hsi_client *hsi_new_client(struct hsi_port *port,
+						struct hsi_board_info *info);
+int hsi_remove_client(struct device *dev, void *data);
+void hsi_port_unregister_clients(struct hsi_port *port);
+
+#ifdef CONFIG_OF
+void hsi_add_clients_from_dt(struct hsi_port *port,
+			     struct device_node *clients);
+#else
+static inline void hsi_add_clients_from_dt(struct hsi_port *port,
+					   struct device_node *clients)
+{
+	return;
+}
+#endif
 
 static inline void hsi_controller_set_drvdata(struct hsi_controller *hsi,
 								void *data)
@@ -305,6 +334,8 @@ static inline struct hsi_port *hsi_find_port_num(struct hsi_controller *hsi,
  */
 int hsi_async(struct hsi_client *cl, struct hsi_msg *msg);
 
+int hsi_get_channel_id_by_name(struct hsi_client *cl, char *name);
+
 /**
  * hsi_id - Get HSI controller ID associated to a client
  * @cl: Pointer to a HSI client
diff --git a/include/linux/hsi/ssi_protocol.h b/include/linux/hsi/ssi_protocol.h
new file mode 100644
index 00000000000..1433651be0d
--- /dev/null
+++ b/include/linux/hsi/ssi_protocol.h
@@ -0,0 +1,42 @@
+/*
+ * ssip_slave.h
+ *
+ * SSIP slave support header file
+ *
+ * Copyright (C) 2010 Nokia Corporation. All rights reserved.
+ *
+ * Contact: Carlos Chinea <carlos.chinea@nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ */
+
+#ifndef __LINUX_SSIP_SLAVE_H__
+#define __LINUX_SSIP_SLAVE_H__
+
+#include <linux/hsi/hsi.h>
+
+static inline void ssip_slave_put_master(struct hsi_client *master)
+{
+}
+
+struct hsi_client *ssip_slave_get_master(struct hsi_client *slave);
+int ssip_slave_start_tx(struct hsi_client *master);
+int ssip_slave_stop_tx(struct hsi_client *master);
+void ssip_reset_event(struct hsi_client *master);
+
+int ssip_slave_running(struct hsi_client *master);
+
+#endif /* __LINUX_SSIP_SLAVE_H__ */
+
diff --git a/include/linux/hugetlb.h b/include/linux/hugetlb.h
index 5b337cf8fb8..b65166de1d9 100644
--- a/include/linux/hugetlb.h
+++ b/include/linux/hugetlb.h
@@ -412,6 +412,16 @@ static inline spinlock_t *huge_pte_lockptr(struct hstate *h,
 	return &mm->page_table_lock;
 }
 
+static inline bool hugepages_supported(void)
+{
+	/*
+	 * Some platform decide whether they support huge pages at boot
+	 * time. On these, such as powerpc, HPAGE_SHIFT is set to 0 when
+	 * there is no such support
+	 */
+	return HPAGE_SHIFT != 0;
+}
+
 #else	/* CONFIG_HUGETLB_PAGE */
 struct hstate {};
 #define alloc_huge_page_node(h, nid) NULL
diff --git a/include/linux/hyperv.h b/include/linux/hyperv.h
index ab7359fde98..08cfaff8a07 100644
--- a/include/linux/hyperv.h
+++ b/include/linux/hyperv.h
@@ -147,15 +147,17 @@ hv_get_ringbuffer_availbytes(struct hv_ring_buffer_info *rbi,
  * 0 . 13 (Windows Server 2008)
  * 1 . 1  (Windows 7)
  * 2 . 4  (Windows 8)
+ * 3 . 0  (Windows 8 R2)
  */
 
 #define VERSION_WS2008  ((0 << 16) | (13))
 #define VERSION_WIN7    ((1 << 16) | (1))
 #define VERSION_WIN8    ((2 << 16) | (4))
+#define VERSION_WIN8_1    ((3 << 16) | (0))
 
 #define VERSION_INVAL -1
 
-#define VERSION_CURRENT VERSION_WIN8
+#define VERSION_CURRENT VERSION_WIN8_1
 
 /* Make maximum size of pipe payload of 16K */
 #define MAX_PIPE_DATA_PAYLOAD		(sizeof(u8) * 16384)
@@ -694,6 +696,8 @@ struct vmbus_channel {
 	 * preserve the earlier behavior.
 	 */
 	u32 target_vp;
+	/* The corresponding CPUID in the guest */
+	u32 target_cpu;
 	/*
 	 * Support for sub-channels. For high performance devices,
 	 * it will be useful to have multiple sub-channels to support
@@ -730,6 +734,11 @@ struct vmbus_channel {
 	 * Support per-channel state for use by vmbus drivers.
 	 */
 	void *per_channel_state;
+	/*
+	 * To support per-cpu lookup mapping of relid to channel,
+	 * link up channels based on their CPU affinity.
+	 */
+	struct list_head percpu_list;
 };
 
 static inline void set_channel_read_state(struct vmbus_channel *c, bool state)
diff --git a/include/linux/if_macvlan.h b/include/linux/if_macvlan.h
index 7c8b20b120e..a9a53b12397 100644
--- a/include/linux/if_macvlan.h
+++ b/include/linux/if_macvlan.h
@@ -56,6 +56,7 @@ struct macvlan_dev {
 	int			numqueues;
 	netdev_features_t	tap_features;
 	int			minor;
+	int			nest_level;
 };
 
 static inline void macvlan_count_rx(const struct macvlan_dev *vlan,
diff --git a/include/linux/if_vlan.h b/include/linux/if_vlan.h
index 13bbbde00e6..b2acc4a1b13 100644
--- a/include/linux/if_vlan.h
+++ b/include/linux/if_vlan.h
@@ -73,7 +73,7 @@ static inline struct vlan_ethhdr *vlan_eth_hdr(const struct sk_buff *skb)
 /* found in socket.c */
 extern void vlan_ioctl_set(int (*hook)(struct net *, void __user *));
 
-static inline int is_vlan_dev(struct net_device *dev)
+static inline bool is_vlan_dev(struct net_device *dev)
 {
         return dev->priv_flags & IFF_802_1Q_VLAN;
 }
@@ -159,6 +159,7 @@ struct vlan_dev_priv {
 #ifdef CONFIG_NET_POLL_CONTROLLER
 	struct netpoll				*netpoll;
 #endif
+	unsigned int				nest_level;
 };
 
 static inline struct vlan_dev_priv *vlan_dev_priv(const struct net_device *dev)
@@ -197,6 +198,12 @@ extern void vlan_vids_del_by_dev(struct net_device *dev,
 				 const struct net_device *by_dev);
 
 extern bool vlan_uses_dev(const struct net_device *dev);
+
+static inline int vlan_get_encap_level(struct net_device *dev)
+{
+	BUG_ON(!is_vlan_dev(dev));
+	return vlan_dev_priv(dev)->nest_level;
+}
 #else
 static inline struct net_device *
 __vlan_find_dev_deep(struct net_device *real_dev,
@@ -263,6 +270,11 @@ static inline bool vlan_uses_dev(const struct net_device *dev)
 {
 	return false;
 }
+static inline int vlan_get_encap_level(struct net_device *dev)
+{
+	BUG();
+	return 0;
+}
 #endif
 
 static inline bool vlan_hw_offload_capable(netdev_features_t features,
@@ -483,4 +495,5 @@ static inline void vlan_set_encap_proto(struct sk_buff *skb,
 		 */
 		skb->protocol = htons(ETH_P_802_2);
 }
+
 #endif /* !(_LINUX_IF_VLAN_H_) */
diff --git a/include/linux/iio/common/st_sensors.h b/include/linux/iio/common/st_sensors.h
index 3c005eb3a0a..96f51f0e009 100644
--- a/include/linux/iio/common/st_sensors.h
+++ b/include/linux/iio/common/st_sensors.h
@@ -269,6 +269,10 @@ int st_sensors_set_enable(struct iio_dev *indio_dev, bool enable);
 
 int st_sensors_set_axis_enable(struct iio_dev *indio_dev, u8 axis_enable);
 
+void st_sensors_power_enable(struct iio_dev *indio_dev);
+
+void st_sensors_power_disable(struct iio_dev *indio_dev);
+
 int st_sensors_set_odr(struct iio_dev *indio_dev, unsigned int odr);
 
 int st_sensors_set_dataready_irq(struct iio_dev *indio_dev, bool enable);
diff --git a/include/linux/iio/consumer.h b/include/linux/iio/consumer.h
index 2752b1fd12b..651f9a0e276 100644
--- a/include/linux/iio/consumer.h
+++ b/include/linux/iio/consumer.h
@@ -123,6 +123,19 @@ int iio_read_channel_raw(struct iio_channel *chan,
 			 int *val);
 
 /**
+ * iio_read_channel_average_raw() - read from a given channel
+ * @chan:		The channel being queried.
+ * @val:		Value read back.
+ *
+ * Note raw reads from iio channels are in adc counts and hence
+ * scale will need to be applied if standard units required.
+ *
+ * In opposit to the normal iio_read_channel_raw this function
+ * returns the average of multiple reads.
+ */
+int iio_read_channel_average_raw(struct iio_channel *chan, int *val);
+
+/**
  * iio_read_channel_processed() - read processed value from a given channel
  * @chan:		The channel being queried.
  * @val:		Value read back.
diff --git a/include/linux/iio/iio.h b/include/linux/iio/iio.h
index 5f2d00e7e48..ccde91725f9 100644
--- a/include/linux/iio/iio.h
+++ b/include/linux/iio/iio.h
@@ -177,6 +177,12 @@ struct iio_event_spec {
  *			shift:		Shift right by this before masking out
  *					realbits.
  *			endianness:	little or big endian
+ *			repeat:		Number of times real/storage bits
+ *					repeats. When the repeat element is
+ *					more than 1, then the type element in
+ *					sysfs will show a repeat value.
+ *					Otherwise, the number of repetitions is
+ *					omitted.
  * @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
@@ -219,6 +225,7 @@ struct iio_chan_spec {
 		u8	realbits;
 		u8	storagebits;
 		u8	shift;
+		u8	repeat;
 		enum iio_endian endianness;
 	} scan_type;
 	long			info_mask_separate;
@@ -288,6 +295,8 @@ static inline s64 iio_get_time_ns(void)
 #define INDIO_ALL_BUFFER_MODES					\
 	(INDIO_BUFFER_TRIGGERED | INDIO_BUFFER_HARDWARE)
 
+#define INDIO_MAX_RAW_ELEMENTS		4
+
 struct iio_trigger; /* forward declaration */
 struct iio_dev;
 
@@ -302,6 +311,14 @@ struct iio_dev;
  *			the channel in question.  Return value will specify the
  *			type of value returned by the device. val and val2 will
  *			contain the elements making up the returned value.
+ * @read_raw_multi:	function to return values from the device.
+ *			mask specifies which value. Note 0 means a reading of
+ *			the channel in question.  Return value will specify the
+ *			type of value returned by the device. vals pointer
+ *			contain the elements making up the returned value.
+ *			max_len specifies maximum number of elements
+ *			vals pointer can contain. val_len is used to return
+ *			length of valid elements in vals.
  * @write_raw:		function to write a value to the device.
  *			Parameters are the same as for read_raw.
  * @write_raw_get_fmt:	callback function to query the expected
@@ -328,6 +345,13 @@ struct iio_info {
 			int *val2,
 			long mask);
 
+	int (*read_raw_multi)(struct iio_dev *indio_dev,
+			struct iio_chan_spec const *chan,
+			int max_len,
+			int *vals,
+			int *val_len,
+			long mask);
+
 	int (*write_raw)(struct iio_dev *indio_dev,
 			 struct iio_chan_spec const *chan,
 			 int val,
diff --git a/include/linux/iio/types.h b/include/linux/iio/types.h
index 084d882fe01..d480631eabc 100644
--- a/include/linux/iio/types.h
+++ b/include/linux/iio/types.h
@@ -53,6 +53,9 @@ enum iio_modifier {
 	IIO_MOD_LIGHT_RED,
 	IIO_MOD_LIGHT_GREEN,
 	IIO_MOD_LIGHT_BLUE,
+	IIO_MOD_QUATERNION,
+	IIO_MOD_TEMP_AMBIENT,
+	IIO_MOD_TEMP_OBJECT,
 };
 
 enum iio_event_type {
@@ -79,6 +82,7 @@ enum iio_event_direction {
 #define IIO_VAL_INT_PLUS_MICRO 2
 #define IIO_VAL_INT_PLUS_NANO 3
 #define IIO_VAL_INT_PLUS_MICRO_DB 4
+#define IIO_VAL_INT_MULTIPLE 5
 #define IIO_VAL_FRACTIONAL 10
 #define IIO_VAL_FRACTIONAL_LOG2 11
 
diff --git a/include/linux/init.h b/include/linux/init.h
index a3ba2707634..2df8e8dd10a 100644
--- a/include/linux/init.h
+++ b/include/linux/init.h
@@ -297,16 +297,28 @@ void __init parse_early_options(char *cmdline);
 
 #else /* MODULE */
 
-/* Don't use these in loadable modules, but some people do... */
+/*
+ * In most cases loadable modules do not need custom
+ * initcall levels. There are still some valid cases where
+ * a driver may be needed early if built in, and does not
+ * matter when built as a loadable module. Like bus
+ * snooping debug drivers.
+ */
 #define early_initcall(fn)		module_init(fn)
 #define core_initcall(fn)		module_init(fn)
+#define core_initcall_sync(fn)		module_init(fn)
 #define postcore_initcall(fn)		module_init(fn)
+#define postcore_initcall_sync(fn)	module_init(fn)
 #define arch_initcall(fn)		module_init(fn)
 #define subsys_initcall(fn)		module_init(fn)
+#define subsys_initcall_sync(fn)	module_init(fn)
 #define fs_initcall(fn)			module_init(fn)
+#define fs_initcall_sync(fn)		module_init(fn)
 #define rootfs_initcall(fn)		module_init(fn)
 #define device_initcall(fn)		module_init(fn)
+#define device_initcall_sync(fn)	module_init(fn)
 #define late_initcall(fn)		module_init(fn)
+#define late_initcall_sync(fn)		module_init(fn)
 
 #define console_initcall(fn)		module_init(fn)
 #define security_initcall(fn)		module_init(fn)
diff --git a/include/linux/interrupt.h b/include/linux/interrupt.h
index 15711104328..cb19f09d7e3 100644
--- a/include/linux/interrupt.h
+++ b/include/linux/interrupt.h
@@ -203,7 +203,40 @@ static inline int check_wakeup_irqs(void) { return 0; }
 
 extern cpumask_var_t irq_default_affinity;
 
-extern int irq_set_affinity(unsigned int irq, const struct cpumask *cpumask);
+/* Internal implementation. Use the helpers below */
+extern int __irq_set_affinity(unsigned int irq, const struct cpumask *cpumask,
+			      bool force);
+
+/**
+ * irq_set_affinity - Set the irq affinity of a given irq
+ * @irq:	Interrupt to set affinity
+ * @cpumask:	cpumask
+ *
+ * Fails if cpumask does not contain an online CPU
+ */
+static inline int
+irq_set_affinity(unsigned int irq, const struct cpumask *cpumask)
+{
+	return __irq_set_affinity(irq, cpumask, false);
+}
+
+/**
+ * irq_force_affinity - Force the irq affinity of a given irq
+ * @irq:	Interrupt to set affinity
+ * @cpumask:	cpumask
+ *
+ * Same as irq_set_affinity, but without checking the mask against
+ * online cpus.
+ *
+ * Solely for low level cpu hotplug code, where we need to make per
+ * cpu interrupts affine before the cpu becomes online.
+ */
+static inline int
+irq_force_affinity(unsigned int irq, const struct cpumask *cpumask)
+{
+	return __irq_set_affinity(irq, cpumask, true);
+}
+
 extern int irq_can_set_affinity(unsigned int irq);
 extern int irq_select_affinity(unsigned int irq);
 
@@ -239,6 +272,11 @@ static inline int irq_set_affinity(unsigned int irq, const struct cpumask *m)
 	return -EINVAL;
 }
 
+static inline int irq_force_affinity(unsigned int irq, const struct cpumask *cpumask)
+{
+	return 0;
+}
+
 static inline int irq_can_set_affinity(unsigned int irq)
 {
 	return 0;
diff --git a/include/linux/irq.h b/include/linux/irq.h
index d278838908c..5c57efb863d 100644
--- a/include/linux/irq.h
+++ b/include/linux/irq.h
@@ -394,7 +394,8 @@ extern void remove_percpu_irq(unsigned int irq, struct irqaction *act);
 
 extern void irq_cpu_online(void);
 extern void irq_cpu_offline(void);
-extern int __irq_set_affinity_locked(struct irq_data *data,  const struct cpumask *cpumask);
+extern int irq_set_affinity_locked(struct irq_data *data,
+				   const struct cpumask *cpumask, bool force);
 
 #if defined(CONFIG_SMP) && defined(CONFIG_GENERIC_PENDING_IRQ)
 void irq_move_irq(struct irq_data *data);
@@ -602,6 +603,8 @@ static inline u32 irq_get_trigger_type(unsigned int irq)
 	return d ? irqd_get_trigger_type(d) : 0;
 }
 
+unsigned int arch_dynirq_lower_bound(unsigned int from);
+
 int __irq_alloc_descs(int irq, unsigned int from, unsigned int cnt, int node,
 		struct module *owner);
 
diff --git a/include/linux/kernfs.h b/include/linux/kernfs.h
index b0122dc6f96..17aa1cce6f8 100644
--- a/include/linux/kernfs.h
+++ b/include/linux/kernfs.h
@@ -50,7 +50,24 @@ enum kernfs_node_flag {
 
 /* @flags for kernfs_create_root() */
 enum kernfs_root_flag {
-	KERNFS_ROOT_CREATE_DEACTIVATED = 0x0001,
+	/*
+	 * kernfs_nodes are created in the deactivated state and invisible.
+	 * They require explicit kernfs_activate() to become visible.  This
+	 * can be used to make related nodes become visible atomically
+	 * after all nodes are created successfully.
+	 */
+	KERNFS_ROOT_CREATE_DEACTIVATED		= 0x0001,
+
+	/*
+	 * For regular flies, if the opener has CAP_DAC_OVERRIDE, open(2)
+	 * succeeds regardless of the RW permissions.  sysfs had an extra
+	 * layer of enforcement where open(2) fails with -EACCES regardless
+	 * of CAP_DAC_OVERRIDE if the permission doesn't have the
+	 * respective read or write access at all (none of S_IRUGO or
+	 * S_IWUGO) or the respective operation isn't implemented.  The
+	 * following flag enables that behavior.
+	 */
+	KERNFS_ROOT_EXTRA_OPEN_PERM_CHECK	= 0x0002,
 };
 
 /* type-specific structures for kernfs_node union members */
@@ -144,6 +161,10 @@ struct kernfs_root {
 	/* private fields, do not use outside kernfs proper */
 	struct ida		ino_ida;
 	struct kernfs_syscall_ops *syscall_ops;
+
+	/* list of kernfs_super_info of this root, protected by kernfs_mutex */
+	struct list_head	supers;
+
 	wait_queue_head_t	deactivate_waitq;
 };
 
@@ -280,8 +301,8 @@ void kernfs_notify(struct kernfs_node *kn);
 
 const void *kernfs_super_ns(struct super_block *sb);
 struct dentry *kernfs_mount_ns(struct file_system_type *fs_type, int flags,
-			       struct kernfs_root *root, bool *new_sb_created,
-			       const void *ns);
+			       struct kernfs_root *root, unsigned long magic,
+			       bool *new_sb_created, const void *ns);
 void kernfs_kill_sb(struct super_block *sb);
 
 void kernfs_init(void);
@@ -374,7 +395,8 @@ static inline const void *kernfs_super_ns(struct super_block *sb)
 
 static inline struct dentry *
 kernfs_mount_ns(struct file_system_type *fs_type, int flags,
-		struct kernfs_root *root, bool *new_sb_created, const void *ns)
+		struct kernfs_root *root, unsigned long magic,
+		bool *new_sb_created, const void *ns)
 { return ERR_PTR(-ENOSYS); }
 
 static inline void kernfs_kill_sb(struct super_block *sb) { }
@@ -432,9 +454,11 @@ static inline int kernfs_rename(struct kernfs_node *kn,
 
 static inline struct dentry *
 kernfs_mount(struct file_system_type *fs_type, int flags,
-	     struct kernfs_root *root, bool *new_sb_created)
+		struct kernfs_root *root, unsigned long magic,
+		bool *new_sb_created)
 {
-	return kernfs_mount_ns(fs_type, flags, root, new_sb_created, NULL);
+	return kernfs_mount_ns(fs_type, flags, root,
+				magic, new_sb_created, NULL);
 }
 
 #endif	/* __LINUX_KERNFS_H */
diff --git a/include/linux/kobject.h b/include/linux/kobject.h
index f896a33e834..2d61b909f41 100644
--- a/include/linux/kobject.h
+++ b/include/linux/kobject.h
@@ -32,8 +32,10 @@
 #define UEVENT_NUM_ENVP			32	/* number of env pointers */
 #define UEVENT_BUFFER_SIZE		2048	/* buffer for the variables */
 
+#ifdef CONFIG_UEVENT_HELPER
 /* path to the userspace helper executed on an event */
 extern char uevent_helper[];
+#endif
 
 /* counter to tag the uevent, read only except for the kobject core */
 extern u64 uevent_seqnum;
diff --git a/include/linux/libata.h b/include/linux/libata.h
index 1de36be64df..5ab4e3a7672 100644
--- a/include/linux/libata.h
+++ b/include/linux/libata.h
@@ -822,6 +822,7 @@ struct ata_port {
 	unsigned long		qc_allocated;
 	unsigned int		qc_active;
 	int			nr_active_links; /* #links with active qcs */
+	unsigned int		last_tag;	/* track next tag hw expects */
 
 	struct ata_link		link;		/* host default link */
 	struct ata_link		*slave_link;	/* see ata_slave_link_init() */
diff --git a/include/linux/linkage.h b/include/linux/linkage.h
index 34a513a2727..a6a42dd0246 100644
--- a/include/linux/linkage.h
+++ b/include/linux/linkage.h
@@ -12,9 +12,9 @@
 #endif
 
 #ifdef __cplusplus
-#define CPP_ASMLINKAGE extern "C" __visible
+#define CPP_ASMLINKAGE extern "C"
 #else
-#define CPP_ASMLINKAGE __visible
+#define CPP_ASMLINKAGE
 #endif
 
 #ifndef asmlinkage
diff --git a/include/linux/mbus.h b/include/linux/mbus.h
index 345b8c53b89..550c88fb026 100644
--- a/include/linux/mbus.h
+++ b/include/linux/mbus.h
@@ -73,6 +73,6 @@ int mvebu_mbus_del_window(phys_addr_t base, size_t size);
 int mvebu_mbus_init(const char *soc, phys_addr_t mbus_phys_base,
 		    size_t mbus_size, phys_addr_t sdram_phys_base,
 		    size_t sdram_size);
-int mvebu_mbus_dt_init(void);
+int mvebu_mbus_dt_init(bool is_coherent);
 
 #endif /* __LINUX_MBUS_H */
diff --git a/include/linux/mcb.h b/include/linux/mcb.h
index 2db284d1406..ed06e15a36a 100644
--- a/include/linux/mcb.h
+++ b/include/linux/mcb.h
@@ -16,6 +16,7 @@
 #include <linux/irqreturn.h>
 
 struct mcb_driver;
+struct mcb_device;
 
 /**
  * struct mcb_bus - MEN Chameleon Bus
@@ -23,11 +24,14 @@ struct mcb_driver;
  * @dev: pointer to carrier device
  * @children: the child busses
  * @bus_nr: mcb bus number
+ * @get_irq: callback to get IRQ number
  */
 struct mcb_bus {
 	struct list_head children;
 	struct device dev;
+	struct device *carrier;
 	int bus_nr;
+	int (*get_irq)(struct mcb_device *dev);
 };
 #define to_mcb_bus(b) container_of((b), struct mcb_bus, dev)
 
@@ -105,7 +109,7 @@ extern void mcb_unregister_driver(struct mcb_driver *driver);
 	module_driver(__mcb_driver, mcb_register_driver, mcb_unregister_driver);
 extern void mcb_bus_add_devices(const struct mcb_bus *bus);
 extern int mcb_device_register(struct mcb_bus *bus, struct mcb_device *dev);
-extern struct mcb_bus *mcb_alloc_bus(void);
+extern struct mcb_bus *mcb_alloc_bus(struct device *carrier);
 extern struct mcb_bus *mcb_bus_get(struct mcb_bus *bus);
 extern void mcb_bus_put(struct mcb_bus *bus);
 extern struct mcb_device *mcb_alloc_dev(struct mcb_bus *bus);
diff --git a/include/linux/mdio-gpio.h b/include/linux/mdio-gpio.h
index 7c9fe3c2be7..66c30a763b1 100644
--- a/include/linux/mdio-gpio.h
+++ b/include/linux/mdio-gpio.h
@@ -17,6 +17,11 @@ struct mdio_gpio_platform_data {
 	/* GPIO numbers for bus pins */
 	unsigned int mdc;
 	unsigned int mdio;
+	unsigned int mdo;
+
+	bool mdc_active_low;
+	bool mdio_active_low;
+	bool mdo_active_low;
 
 	unsigned int phy_mask;
 	int irqs[PHY_MAX_ADDR];
diff --git a/include/linux/memblock.h b/include/linux/memblock.h
index 8a20a51ed42..73dc382e72d 100644
--- a/include/linux/memblock.h
+++ b/include/linux/memblock.h
@@ -18,6 +18,7 @@
 #include <linux/mm.h>
 
 #define INIT_MEMBLOCK_REGIONS	128
+#define INIT_PHYSMEM_REGIONS	4
 
 /* Definition of memblock flags. */
 #define MEMBLOCK_HOTPLUG	0x1	/* hotpluggable region */
@@ -43,6 +44,9 @@ struct memblock {
 	phys_addr_t current_limit;
 	struct memblock_type memory;
 	struct memblock_type reserved;
+#ifdef CONFIG_HAVE_MEMBLOCK_PHYS_MAP
+	struct memblock_type physmem;
+#endif
 };
 
 extern struct memblock memblock;
@@ -71,6 +75,63 @@ int memblock_reserve(phys_addr_t base, phys_addr_t size);
 void memblock_trim_memory(phys_addr_t align);
 int memblock_mark_hotplug(phys_addr_t base, phys_addr_t size);
 int memblock_clear_hotplug(phys_addr_t base, phys_addr_t size);
+
+/* Low level functions */
+int memblock_add_range(struct memblock_type *type,
+		       phys_addr_t base, phys_addr_t size,
+		       int nid, unsigned long flags);
+
+int memblock_remove_range(struct memblock_type *type,
+			  phys_addr_t base,
+			  phys_addr_t size);
+
+void __next_mem_range(u64 *idx, int nid, struct memblock_type *type_a,
+		      struct memblock_type *type_b, phys_addr_t *out_start,
+		      phys_addr_t *out_end, int *out_nid);
+
+void __next_mem_range_rev(u64 *idx, int nid, struct memblock_type *type_a,
+			  struct memblock_type *type_b, phys_addr_t *out_start,
+			  phys_addr_t *out_end, int *out_nid);
+
+/**
+ * for_each_mem_range - iterate through memblock areas from type_a and not
+ * included in type_b. Or just type_a if type_b is NULL.
+ * @i: u64 used as loop variable
+ * @type_a: ptr to memblock_type to iterate
+ * @type_b: ptr to memblock_type which excludes from the iteration
+ * @nid: node selector, %NUMA_NO_NODE for all nodes
+ * @p_start: ptr to phys_addr_t for start address of the range, can be %NULL
+ * @p_end: ptr to phys_addr_t for end address of the range, can be %NULL
+ * @p_nid: ptr to int for nid of the range, can be %NULL
+ */
+#define for_each_mem_range(i, type_a, type_b, nid,			\
+			   p_start, p_end, p_nid)			\
+	for (i = 0, __next_mem_range(&i, nid, type_a, type_b,		\
+				     p_start, p_end, p_nid);		\
+	     i != (u64)ULLONG_MAX;					\
+	     __next_mem_range(&i, nid, type_a, type_b,			\
+			      p_start, p_end, p_nid))
+
+/**
+ * for_each_mem_range_rev - reverse iterate through memblock areas from
+ * type_a and not included in type_b. Or just type_a if type_b is NULL.
+ * @i: u64 used as loop variable
+ * @type_a: ptr to memblock_type to iterate
+ * @type_b: ptr to memblock_type which excludes from the iteration
+ * @nid: node selector, %NUMA_NO_NODE for all nodes
+ * @p_start: ptr to phys_addr_t for start address of the range, can be %NULL
+ * @p_end: ptr to phys_addr_t for end address of the range, can be %NULL
+ * @p_nid: ptr to int for nid of the range, can be %NULL
+ */
+#define for_each_mem_range_rev(i, type_a, type_b, nid,			\
+			       p_start, p_end, p_nid)			\
+	for (i = (u64)ULLONG_MAX,					\
+		     __next_mem_range_rev(&i, nid, type_a, type_b,	\
+					 p_start, p_end, p_nid);	\
+	     i != (u64)ULLONG_MAX;					\
+	     __next_mem_range_rev(&i, nid, type_a, type_b,		\
+				  p_start, p_end, p_nid))
+
 #ifdef CONFIG_MOVABLE_NODE
 static inline bool memblock_is_hotpluggable(struct memblock_region *m)
 {
@@ -113,9 +174,6 @@ void __next_mem_pfn_range(int *idx, int nid, unsigned long *out_start_pfn,
 	     i >= 0; __next_mem_pfn_range(&i, nid, p_start, p_end, p_nid))
 #endif /* CONFIG_HAVE_MEMBLOCK_NODE_MAP */
 
-void __next_free_mem_range(u64 *idx, int nid, phys_addr_t *out_start,
-			   phys_addr_t *out_end, int *out_nid);
-
 /**
  * for_each_free_mem_range - iterate through free memblock areas
  * @i: u64 used as loop variable
@@ -128,13 +186,8 @@ void __next_free_mem_range(u64 *idx, int nid, phys_addr_t *out_start,
  * soon as memblock is initialized.
  */
 #define for_each_free_mem_range(i, nid, p_start, p_end, p_nid)		\
-	for (i = 0,							\
-	     __next_free_mem_range(&i, nid, p_start, p_end, p_nid);	\
-	     i != (u64)ULLONG_MAX;					\
-	     __next_free_mem_range(&i, nid, p_start, p_end, p_nid))
-
-void __next_free_mem_range_rev(u64 *idx, int nid, phys_addr_t *out_start,
-			       phys_addr_t *out_end, int *out_nid);
+	for_each_mem_range(i, &memblock.memory, &memblock.reserved,	\
+			   nid, p_start, p_end, p_nid)
 
 /**
  * for_each_free_mem_range_reverse - rev-iterate through free memblock areas
@@ -148,10 +201,8 @@ void __next_free_mem_range_rev(u64 *idx, int nid, phys_addr_t *out_start,
  * order.  Available as soon as memblock is initialized.
  */
 #define for_each_free_mem_range_reverse(i, nid, p_start, p_end, p_nid)	\
-	for (i = (u64)ULLONG_MAX,					\
-	     __next_free_mem_range_rev(&i, nid, p_start, p_end, p_nid);	\
-	     i != (u64)ULLONG_MAX;					\
-	     __next_free_mem_range_rev(&i, nid, p_start, p_end, p_nid))
+	for_each_mem_range_rev(i, &memblock.memory, &memblock.reserved,	\
+			       nid, p_start, p_end, p_nid)
 
 static inline void memblock_set_region_flags(struct memblock_region *r,
 					     unsigned long flags)
diff --git a/include/linux/mfd/arizona/core.h b/include/linux/mfd/arizona/core.h
index 5cf8b91ce99..6d9371f8887 100644
--- a/include/linux/mfd/arizona/core.h
+++ b/include/linux/mfd/arizona/core.h
@@ -124,4 +124,7 @@ int wm5102_patch(struct arizona *arizona);
 int wm5110_patch(struct arizona *arizona);
 int wm8997_patch(struct arizona *arizona);
 
+extern int arizona_of_get_named_gpio(struct arizona *arizona, const char *prop,
+				     bool mandatory);
+
 #endif
diff --git a/include/linux/mfd/bcm590xx.h b/include/linux/mfd/bcm590xx.h
index 434df2d4e58..267aedee1c7 100644
--- a/include/linux/mfd/bcm590xx.h
+++ b/include/linux/mfd/bcm590xx.h
@@ -19,12 +19,15 @@
 #include <linux/regmap.h>
 
 /* max register address */
-#define BCM590XX_MAX_REGISTER	0xe7
+#define BCM590XX_MAX_REGISTER_PRI	0xe7
+#define BCM590XX_MAX_REGISTER_SEC	0xf0
 
 struct bcm590xx {
 	struct device *dev;
-	struct i2c_client *i2c_client;
-	struct regmap *regmap;
+	struct i2c_client *i2c_pri;
+	struct i2c_client *i2c_sec;
+	struct regmap *regmap_pri;
+	struct regmap *regmap_sec;
 	unsigned int id;
 };
 
diff --git a/include/linux/mfd/core.h b/include/linux/mfd/core.h
index bdba8c61207..f543de91ce1 100644
--- a/include/linux/mfd/core.h
+++ b/include/linux/mfd/core.h
@@ -63,7 +63,7 @@ struct mfd_cell {
 	/* A list of regulator supplies that should be mapped to the MFD
 	 * device rather than the child device when requested
 	 */
-	const char		**parent_supplies;
+	const char * const	*parent_supplies;
 	int			num_parent_supplies;
 };
 
diff --git a/include/linux/mfd/max14577-private.h b/include/linux/mfd/max14577-private.h
index c9b332fb0d5..49925360402 100644
--- a/include/linux/mfd/max14577-private.h
+++ b/include/linux/mfd/max14577-private.h
@@ -1,7 +1,7 @@
 /*
- * max14577-private.h - Common API for the Maxim 14577 internal sub chip
+ * max14577-private.h - Common API for the Maxim 14577/77836 internal sub chip
  *
- * Copyright (C) 2013 Samsung Electrnoics
+ * Copyright (C) 2014 Samsung Electrnoics
  * Chanwoo Choi <cw00.choi@samsung.com>
  * Krzysztof Kozlowski <k.kozlowski@samsung.com>
  *
@@ -22,9 +22,19 @@
 #include <linux/i2c.h>
 #include <linux/regmap.h>
 
-#define MAX14577_REG_INVALID		(0xff)
+#define I2C_ADDR_PMIC	(0x46 >> 1)
+#define I2C_ADDR_MUIC	(0x4A >> 1)
+#define I2C_ADDR_FG	(0x6C >> 1)
 
-/* Slave addr = 0x4A: Interrupt */
+enum maxim_device_type {
+	MAXIM_DEVICE_TYPE_UNKNOWN	= 0,
+	MAXIM_DEVICE_TYPE_MAX14577,
+	MAXIM_DEVICE_TYPE_MAX77836,
+
+	MAXIM_DEVICE_TYPE_NUM,
+};
+
+/* Slave addr = 0x4A: MUIC and Charger */
 enum max14577_reg {
 	MAX14577_REG_DEVICEID		= 0x00,
 	MAX14577_REG_INT1		= 0x01,
@@ -74,20 +84,22 @@ enum max14577_muic_charger_type {
 };
 
 /* MAX14577 interrupts */
-#define INT1_ADC_MASK			(0x1 << 0)
-#define INT1_ADCLOW_MASK		(0x1 << 1)
-#define INT1_ADCERR_MASK		(0x1 << 2)
-
-#define INT2_CHGTYP_MASK		(0x1 << 0)
-#define INT2_CHGDETRUN_MASK		(0x1 << 1)
-#define INT2_DCDTMR_MASK		(0x1 << 2)
-#define INT2_DBCHG_MASK			(0x1 << 3)
-#define INT2_VBVOLT_MASK		(0x1 << 4)
-
-#define INT3_EOC_MASK			(0x1 << 0)
-#define INT3_CGMBC_MASK			(0x1 << 1)
-#define INT3_OVP_MASK			(0x1 << 2)
-#define INT3_MBCCHGERR_MASK		(0x1 << 3)
+#define MAX14577_INT1_ADC_MASK		BIT(0)
+#define MAX14577_INT1_ADCLOW_MASK	BIT(1)
+#define MAX14577_INT1_ADCERR_MASK	BIT(2)
+#define MAX77836_INT1_ADC1K_MASK	BIT(3)
+
+#define MAX14577_INT2_CHGTYP_MASK	BIT(0)
+#define MAX14577_INT2_CHGDETRUN_MASK	BIT(1)
+#define MAX14577_INT2_DCDTMR_MASK	BIT(2)
+#define MAX14577_INT2_DBCHG_MASK	BIT(3)
+#define MAX14577_INT2_VBVOLT_MASK	BIT(4)
+#define MAX77836_INT2_VIDRM_MASK	BIT(5)
+
+#define MAX14577_INT3_EOC_MASK		BIT(0)
+#define MAX14577_INT3_CGMBC_MASK	BIT(1)
+#define MAX14577_INT3_OVP_MASK		BIT(2)
+#define MAX14577_INT3_MBCCHGERR_MASK	BIT(3)
 
 /* MAX14577 DEVICE ID register */
 #define DEVID_VENDORID_SHIFT		0
@@ -99,9 +111,11 @@ enum max14577_muic_charger_type {
 #define STATUS1_ADC_SHIFT		0
 #define STATUS1_ADCLOW_SHIFT		5
 #define STATUS1_ADCERR_SHIFT		6
+#define MAX77836_STATUS1_ADC1K_SHIFT	7
 #define STATUS1_ADC_MASK		(0x1f << STATUS1_ADC_SHIFT)
-#define STATUS1_ADCLOW_MASK		(0x1 << STATUS1_ADCLOW_SHIFT)
-#define STATUS1_ADCERR_MASK		(0x1 << STATUS1_ADCERR_SHIFT)
+#define STATUS1_ADCLOW_MASK		BIT(STATUS1_ADCLOW_SHIFT)
+#define STATUS1_ADCERR_MASK		BIT(STATUS1_ADCERR_SHIFT)
+#define MAX77836_STATUS1_ADC1K_MASK	BIT(MAX77836_STATUS1_ADC1K_SHIFT)
 
 /* MAX14577 STATUS2 register */
 #define STATUS2_CHGTYP_SHIFT		0
@@ -109,11 +123,13 @@ enum max14577_muic_charger_type {
 #define STATUS2_DCDTMR_SHIFT		4
 #define STATUS2_DBCHG_SHIFT		5
 #define STATUS2_VBVOLT_SHIFT		6
+#define MAX77836_STATUS2_VIDRM_SHIFT	7
 #define STATUS2_CHGTYP_MASK		(0x7 << STATUS2_CHGTYP_SHIFT)
-#define STATUS2_CHGDETRUN_MASK		(0x1 << STATUS2_CHGDETRUN_SHIFT)
-#define STATUS2_DCDTMR_MASK		(0x1 << STATUS2_DCDTMR_SHIFT)
-#define STATUS2_DBCHG_MASK		(0x1 << STATUS2_DBCHG_SHIFT)
-#define STATUS2_VBVOLT_MASK		(0x1 << STATUS2_VBVOLT_SHIFT)
+#define STATUS2_CHGDETRUN_MASK		BIT(STATUS2_CHGDETRUN_SHIFT)
+#define STATUS2_DCDTMR_MASK		BIT(STATUS2_DCDTMR_SHIFT)
+#define STATUS2_DBCHG_MASK		BIT(STATUS2_DBCHG_SHIFT)
+#define STATUS2_VBVOLT_MASK		BIT(STATUS2_VBVOLT_SHIFT)
+#define MAX77836_STATUS2_VIDRM_MASK	BIT(MAX77836_STATUS2_VIDRM_SHIFT)
 
 /* MAX14577 CONTROL1 register */
 #define COMN1SW_SHIFT			0
@@ -122,8 +138,8 @@ enum max14577_muic_charger_type {
 #define IDBEN_SHIFT			7
 #define COMN1SW_MASK			(0x7 << COMN1SW_SHIFT)
 #define COMP2SW_MASK			(0x7 << COMP2SW_SHIFT)
-#define MICEN_MASK			(0x1 << MICEN_SHIFT)
-#define IDBEN_MASK			(0x1 << IDBEN_SHIFT)
+#define MICEN_MASK			BIT(MICEN_SHIFT)
+#define IDBEN_MASK			BIT(IDBEN_SHIFT)
 #define CLEAR_IDBEN_MICEN_MASK		(COMN1SW_MASK | COMP2SW_MASK)
 #define CTRL1_SW_USB			((1 << COMP2SW_SHIFT) \
 						| (1 << COMN1SW_SHIFT))
@@ -143,14 +159,14 @@ enum max14577_muic_charger_type {
 #define CTRL2_ACCDET_SHIFT		(5)
 #define CTRL2_USBCPINT_SHIFT		(6)
 #define CTRL2_RCPS_SHIFT		(7)
-#define CTRL2_LOWPWR_MASK		(0x1 << CTRL2_LOWPWR_SHIFT)
-#define CTRL2_ADCEN_MASK		(0x1 << CTRL2_ADCEN_SHIFT)
-#define CTRL2_CPEN_MASK			(0x1 << CTRL2_CPEN_SHIFT)
-#define CTRL2_SFOUTASRT_MASK		(0x1 << CTRL2_SFOUTASRT_SHIFT)
-#define CTRL2_SFOUTORD_MASK		(0x1 << CTRL2_SFOUTORD_SHIFT)
-#define CTRL2_ACCDET_MASK		(0x1 << CTRL2_ACCDET_SHIFT)
-#define CTRL2_USBCPINT_MASK		(0x1 << CTRL2_USBCPINT_SHIFT)
-#define CTRL2_RCPS_MASK			(0x1 << CTR2_RCPS_SHIFT)
+#define CTRL2_LOWPWR_MASK		BIT(CTRL2_LOWPWR_SHIFT)
+#define CTRL2_ADCEN_MASK		BIT(CTRL2_ADCEN_SHIFT)
+#define CTRL2_CPEN_MASK			BIT(CTRL2_CPEN_SHIFT)
+#define CTRL2_SFOUTASRT_MASK		BIT(CTRL2_SFOUTASRT_SHIFT)
+#define CTRL2_SFOUTORD_MASK		BIT(CTRL2_SFOUTORD_SHIFT)
+#define CTRL2_ACCDET_MASK		BIT(CTRL2_ACCDET_SHIFT)
+#define CTRL2_USBCPINT_MASK		BIT(CTRL2_USBCPINT_SHIFT)
+#define CTRL2_RCPS_MASK			BIT(CTRL2_RCPS_SHIFT)
 
 #define CTRL2_CPEN1_LOWPWR0 ((1 << CTRL2_CPEN_SHIFT) | \
 				(0 << CTRL2_LOWPWR_SHIFT))
@@ -198,14 +214,14 @@ enum max14577_charger_reg {
 #define CDETCTRL1_DBEXIT_SHIFT		5
 #define CDETCTRL1_DBIDLE_SHIFT		6
 #define CDETCTRL1_CDPDET_SHIFT		7
-#define CDETCTRL1_CHGDETEN_MASK		(0x1 << CDETCTRL1_CHGDETEN_SHIFT)
-#define CDETCTRL1_CHGTYPMAN_MASK	(0x1 << CDETCTRL1_CHGTYPMAN_SHIFT)
-#define CDETCTRL1_DCDEN_MASK		(0x1 << CDETCTRL1_DCDEN_SHIFT)
-#define CDETCTRL1_DCD2SCT_MASK		(0x1 << CDETCTRL1_DCD2SCT_SHIFT)
-#define CDETCTRL1_DCHKTM_MASK		(0x1 << CDETCTRL1_DCHKTM_SHIFT)
-#define CDETCTRL1_DBEXIT_MASK		(0x1 << CDETCTRL1_DBEXIT_SHIFT)
-#define CDETCTRL1_DBIDLE_MASK		(0x1 << CDETCTRL1_DBIDLE_SHIFT)
-#define CDETCTRL1_CDPDET_MASK		(0x1 << CDETCTRL1_CDPDET_SHIFT)
+#define CDETCTRL1_CHGDETEN_MASK		BIT(CDETCTRL1_CHGDETEN_SHIFT)
+#define CDETCTRL1_CHGTYPMAN_MASK	BIT(CDETCTRL1_CHGTYPMAN_SHIFT)
+#define CDETCTRL1_DCDEN_MASK		BIT(CDETCTRL1_DCDEN_SHIFT)
+#define CDETCTRL1_DCD2SCT_MASK		BIT(CDETCTRL1_DCD2SCT_SHIFT)
+#define CDETCTRL1_DCHKTM_MASK		BIT(CDETCTRL1_DCHKTM_SHIFT)
+#define CDETCTRL1_DBEXIT_MASK		BIT(CDETCTRL1_DBEXIT_SHIFT)
+#define CDETCTRL1_DBIDLE_MASK		BIT(CDETCTRL1_DBIDLE_SHIFT)
+#define CDETCTRL1_CDPDET_MASK		BIT(CDETCTRL1_CDPDET_SHIFT)
 
 /* MAX14577 CHGCTRL1 register */
 #define CHGCTRL1_TCHW_SHIFT		4
@@ -213,9 +229,9 @@ enum max14577_charger_reg {
 
 /* MAX14577 CHGCTRL2 register */
 #define CHGCTRL2_MBCHOSTEN_SHIFT	6
-#define CHGCTRL2_MBCHOSTEN_MASK		(0x1 << CHGCTRL2_MBCHOSTEN_SHIFT)
+#define CHGCTRL2_MBCHOSTEN_MASK		BIT(CHGCTRL2_MBCHOSTEN_SHIFT)
 #define CHGCTRL2_VCHGR_RC_SHIFT		7
-#define CHGCTRL2_VCHGR_RC_MASK		(0x1 << CHGCTRL2_VCHGR_RC_SHIFT)
+#define CHGCTRL2_VCHGR_RC_MASK		BIT(CHGCTRL2_VCHGR_RC_SHIFT)
 
 /* MAX14577 CHGCTRL3 register */
 #define CHGCTRL3_MBCCVWRC_SHIFT		0
@@ -225,7 +241,7 @@ enum max14577_charger_reg {
 #define CHGCTRL4_MBCICHWRCH_SHIFT	0
 #define CHGCTRL4_MBCICHWRCH_MASK	(0xf << CHGCTRL4_MBCICHWRCH_SHIFT)
 #define CHGCTRL4_MBCICHWRCL_SHIFT	4
-#define CHGCTRL4_MBCICHWRCL_MASK	(0x1 << CHGCTRL4_MBCICHWRCL_SHIFT)
+#define CHGCTRL4_MBCICHWRCL_MASK	BIT(CHGCTRL4_MBCICHWRCL_SHIFT)
 
 /* MAX14577 CHGCTRL5 register */
 #define CHGCTRL5_EOCS_SHIFT		0
@@ -233,7 +249,7 @@ enum max14577_charger_reg {
 
 /* MAX14577 CHGCTRL6 register */
 #define CHGCTRL6_AUTOSTOP_SHIFT		5
-#define CHGCTRL6_AUTOSTOP_MASK		(0x1 << CHGCTRL6_AUTOSTOP_SHIFT)
+#define CHGCTRL6_AUTOSTOP_MASK		BIT(CHGCTRL6_AUTOSTOP_SHIFT)
 
 /* MAX14577 CHGCTRL7 register */
 #define CHGCTRL7_OTPCGHCVS_SHIFT	0
@@ -245,14 +261,111 @@ enum max14577_charger_reg {
 #define MAX14577_REGULATOR_CURRENT_LIMIT_HIGH_STEP	 50000
 #define MAX14577_REGULATOR_CURRENT_LIMIT_MAX		950000
 
+/* MAX77836 regulator current limits (as in CHGCTRL4 register), uA */
+#define MAX77836_REGULATOR_CURRENT_LIMIT_MIN		 45000
+#define MAX77836_REGULATOR_CURRENT_LIMIT_HIGH_START	100000
+#define MAX77836_REGULATOR_CURRENT_LIMIT_HIGH_STEP	 25000
+#define MAX77836_REGULATOR_CURRENT_LIMIT_MAX		475000
+
 /* MAX14577 regulator SFOUT LDO voltage, fixed, uV */
 #define MAX14577_REGULATOR_SAFEOUT_VOLTAGE		4900000
 
+/* MAX77836 regulator LDOx voltage, uV */
+#define MAX77836_REGULATOR_LDO_VOLTAGE_MIN		800000
+#define MAX77836_REGULATOR_LDO_VOLTAGE_MAX		3950000
+#define MAX77836_REGULATOR_LDO_VOLTAGE_STEP		50000
+#define MAX77836_REGULATOR_LDO_VOLTAGE_STEPS_NUM	64
+
+/* Slave addr = 0x46: PMIC */
+enum max77836_pmic_reg {
+	MAX77836_PMIC_REG_PMIC_ID		= 0x20,
+	MAX77836_PMIC_REG_PMIC_REV		= 0x21,
+	MAX77836_PMIC_REG_INTSRC		= 0x22,
+	MAX77836_PMIC_REG_INTSRC_MASK		= 0x23,
+	MAX77836_PMIC_REG_TOPSYS_INT		= 0x24,
+	MAX77836_PMIC_REG_TOPSYS_INT_MASK	= 0x26,
+	MAX77836_PMIC_REG_TOPSYS_STAT		= 0x28,
+	MAX77836_PMIC_REG_MRSTB_CNTL		= 0x2A,
+	MAX77836_PMIC_REG_LSCNFG		= 0x2B,
+
+	MAX77836_LDO_REG_CNFG1_LDO1		= 0x51,
+	MAX77836_LDO_REG_CNFG2_LDO1		= 0x52,
+	MAX77836_LDO_REG_CNFG1_LDO2		= 0x53,
+	MAX77836_LDO_REG_CNFG2_LDO2		= 0x54,
+	MAX77836_LDO_REG_CNFG_LDO_BIAS		= 0x55,
+
+	MAX77836_COMP_REG_COMP1			= 0x60,
+
+	MAX77836_PMIC_REG_END,
+};
+
+#define MAX77836_INTSRC_MASK_TOP_INT_SHIFT	1
+#define MAX77836_INTSRC_MASK_MUIC_CHG_INT_SHIFT	3
+#define MAX77836_INTSRC_MASK_TOP_INT_MASK	BIT(MAX77836_INTSRC_MASK_TOP_INT_SHIFT)
+#define MAX77836_INTSRC_MASK_MUIC_CHG_INT_MASK	BIT(MAX77836_INTSRC_MASK_MUIC_CHG_INT_SHIFT)
+
+/* MAX77836 PMIC interrupts */
+#define MAX77836_TOPSYS_INT_T120C_SHIFT		0
+#define MAX77836_TOPSYS_INT_T140C_SHIFT		1
+#define MAX77836_TOPSYS_INT_T120C_MASK		BIT(MAX77836_TOPSYS_INT_T120C_SHIFT)
+#define MAX77836_TOPSYS_INT_T140C_MASK		BIT(MAX77836_TOPSYS_INT_T140C_SHIFT)
+
+/* LDO1/LDO2 CONFIG1 register */
+#define MAX77836_CNFG1_LDO_PWRMD_SHIFT		6
+#define MAX77836_CNFG1_LDO_TV_SHIFT		0
+#define MAX77836_CNFG1_LDO_PWRMD_MASK		(0x3 << MAX77836_CNFG1_LDO_PWRMD_SHIFT)
+#define MAX77836_CNFG1_LDO_TV_MASK		(0x3f << MAX77836_CNFG1_LDO_TV_SHIFT)
+
+/* LDO1/LDO2 CONFIG2 register */
+#define MAX77836_CNFG2_LDO_OVCLMPEN_SHIFT	7
+#define MAX77836_CNFG2_LDO_ALPMEN_SHIFT		6
+#define MAX77836_CNFG2_LDO_COMP_SHIFT		4
+#define MAX77836_CNFG2_LDO_POK_SHIFT		3
+#define MAX77836_CNFG2_LDO_ADE_SHIFT		1
+#define MAX77836_CNFG2_LDO_SS_SHIFT		0
+#define MAX77836_CNFG2_LDO_OVCLMPEN_MASK	BIT(MAX77836_CNFG2_LDO_OVCLMPEN_SHIFT)
+#define MAX77836_CNFG2_LDO_ALPMEN_MASK		BIT(MAX77836_CNFG2_LDO_ALPMEN_SHIFT)
+#define MAX77836_CNFG2_LDO_COMP_MASK		(0x3 << MAX77836_CNFG2_LDO_COMP_SHIFT)
+#define MAX77836_CNFG2_LDO_POK_MASK		BIT(MAX77836_CNFG2_LDO_POK_SHIFT)
+#define MAX77836_CNFG2_LDO_ADE_MASK		BIT(MAX77836_CNFG2_LDO_ADE_SHIFT)
+#define MAX77836_CNFG2_LDO_SS_MASK		BIT(MAX77836_CNFG2_LDO_SS_SHIFT)
+
+/* Slave addr = 0x6C: Fuel-Gauge/Battery */
+enum max77836_fg_reg {
+	MAX77836_FG_REG_VCELL_MSB	= 0x02,
+	MAX77836_FG_REG_VCELL_LSB	= 0x03,
+	MAX77836_FG_REG_SOC_MSB		= 0x04,
+	MAX77836_FG_REG_SOC_LSB		= 0x05,
+	MAX77836_FG_REG_MODE_H		= 0x06,
+	MAX77836_FG_REG_MODE_L		= 0x07,
+	MAX77836_FG_REG_VERSION_MSB	= 0x08,
+	MAX77836_FG_REG_VERSION_LSB	= 0x09,
+	MAX77836_FG_REG_HIBRT_H		= 0x0A,
+	MAX77836_FG_REG_HIBRT_L		= 0x0B,
+	MAX77836_FG_REG_CONFIG_H	= 0x0C,
+	MAX77836_FG_REG_CONFIG_L	= 0x0D,
+	MAX77836_FG_REG_VALRT_MIN	= 0x14,
+	MAX77836_FG_REG_VALRT_MAX	= 0x15,
+	MAX77836_FG_REG_CRATE_MSB	= 0x16,
+	MAX77836_FG_REG_CRATE_LSB	= 0x17,
+	MAX77836_FG_REG_VRESET		= 0x18,
+	MAX77836_FG_REG_FGID		= 0x19,
+	MAX77836_FG_REG_STATUS_H	= 0x1A,
+	MAX77836_FG_REG_STATUS_L	= 0x1B,
+	/*
+	 * TODO: TABLE registers
+	 * TODO: CMD register
+	 */
+
+	MAX77836_FG_REG_END,
+};
+
 enum max14577_irq {
 	/* INT1 */
 	MAX14577_IRQ_INT1_ADC,
 	MAX14577_IRQ_INT1_ADCLOW,
 	MAX14577_IRQ_INT1_ADCERR,
+	MAX77836_IRQ_INT1_ADC1K,
 
 	/* INT2 */
 	MAX14577_IRQ_INT2_CHGTYP,
@@ -260,6 +373,7 @@ enum max14577_irq {
 	MAX14577_IRQ_INT2_DCDTMR,
 	MAX14577_IRQ_INT2_DBCHG,
 	MAX14577_IRQ_INT2_VBVOLT,
+	MAX77836_IRQ_INT2_VIDRM,
 
 	/* INT3 */
 	MAX14577_IRQ_INT3_EOC,
@@ -267,21 +381,25 @@ enum max14577_irq {
 	MAX14577_IRQ_INT3_OVP,
 	MAX14577_IRQ_INT3_MBCCHGERR,
 
+	/* TOPSYS_INT, only MAX77836 */
+	MAX77836_IRQ_TOPSYS_T140C,
+	MAX77836_IRQ_TOPSYS_T120C,
+
 	MAX14577_IRQ_NUM,
 };
 
 struct max14577 {
 	struct device *dev;
 	struct i2c_client *i2c; /* Slave addr = 0x4A */
+	struct i2c_client *i2c_pmic; /* Slave addr = 0x46 */
+	enum maxim_device_type dev_type;
 
-	struct regmap *regmap;
+	struct regmap *regmap; /* For MUIC and Charger */
+	struct regmap *regmap_pmic;
 
-	struct regmap_irq_chip_data *irq_data;
+	struct regmap_irq_chip_data *irq_data; /* For MUIC and Charger */
+	struct regmap_irq_chip_data *irq_data_pmic;
 	int irq;
-
-	/* Device ID */
-	u8 vendor_id;	/* Vendor Identification */
-	u8 device_id;	/* Chip Version */
 };
 
 /* MAX14577 shared regmap API function */
diff --git a/include/linux/mfd/max14577.h b/include/linux/mfd/max14577.h
index 736d39c3ec0..c83fbed1c7b 100644
--- a/include/linux/mfd/max14577.h
+++ b/include/linux/mfd/max14577.h
@@ -1,7 +1,7 @@
 /*
- * max14577.h - Driver for the Maxim 14577
+ * max14577.h - Driver for the Maxim 14577/77836
  *
- * Copyright (C) 2013 Samsung Electrnoics
+ * Copyright (C) 2014 Samsung Electrnoics
  * Chanwoo Choi <cw00.choi@samsung.com>
  * Krzysztof Kozlowski <k.kozlowski@samsung.com>
  *
@@ -20,6 +20,9 @@
  * MAX14577 has MUIC, Charger devices.
  * The devices share the same I2C bus and interrupt line
  * included in this mfd driver.
+ *
+ * MAX77836 has additional PMIC and Fuel-Gauge on different I2C slave
+ * addresses.
  */
 
 #ifndef __MAX14577_H__
@@ -32,7 +35,17 @@ enum max14577_regulators {
 	MAX14577_SAFEOUT = 0,
 	MAX14577_CHARGER,
 
-	MAX14577_REG_MAX,
+	MAX14577_REGULATOR_NUM,
+};
+
+/* MAX77836 regulator IDs */
+enum max77836_regulators {
+	MAX77836_SAFEOUT = 0,
+	MAX77836_CHARGER,
+	MAX77836_LDO1,
+	MAX77836_LDO2,
+
+	MAX77836_REGULATOR_NUM,
 };
 
 struct max14577_regulator_platform_data {
diff --git a/include/linux/mfd/palmas.h b/include/linux/mfd/palmas.h
index 9974e387e48..b8f87b70440 100644
--- a/include/linux/mfd/palmas.h
+++ b/include/linux/mfd/palmas.h
@@ -415,7 +415,7 @@ struct palmas_usb {
 	struct palmas *palmas;
 	struct device *dev;
 
-	struct extcon_dev edev;
+	struct extcon_dev *edev;
 
 	int id_otg_irq;
 	int id_irq;
diff --git a/include/linux/mfd/rtsx_common.h b/include/linux/mfd/rtsx_common.h
index 7c36cc55d2c..443176ee1ab 100644
--- a/include/linux/mfd/rtsx_common.h
+++ b/include/linux/mfd/rtsx_common.h
@@ -45,7 +45,6 @@ struct platform_device;
 struct rtsx_slot {
 	struct platform_device	*p_dev;
 	void			(*card_event)(struct platform_device *p_dev);
-	void			(*done_transfer)(struct platform_device *p_dev);
 };
 
 #endif
diff --git a/include/linux/mfd/rtsx_pci.h b/include/linux/mfd/rtsx_pci.h
index 8d6bbd609ad..a3835976f7c 100644
--- a/include/linux/mfd/rtsx_pci.h
+++ b/include/linux/mfd/rtsx_pci.h
@@ -943,12 +943,6 @@ void rtsx_pci_send_cmd_no_wait(struct rtsx_pcr *pcr);
 int rtsx_pci_send_cmd(struct rtsx_pcr *pcr, int timeout);
 int rtsx_pci_transfer_data(struct rtsx_pcr *pcr, struct scatterlist *sglist,
 		int num_sg, bool read, int timeout);
-int rtsx_pci_dma_map_sg(struct rtsx_pcr *pcr, struct scatterlist *sglist,
-		int num_sg, bool read);
-int rtsx_pci_dma_unmap_sg(struct rtsx_pcr *pcr, struct scatterlist *sglist,
-		int num_sg, bool read);
-int rtsx_pci_dma_transfer(struct rtsx_pcr *pcr, struct scatterlist *sglist,
-		int sg_count, bool read);
 int rtsx_pci_read_ppbuf(struct rtsx_pcr *pcr, u8 *buf, int buf_len);
 int rtsx_pci_write_ppbuf(struct rtsx_pcr *pcr, u8 *buf, int buf_len);
 int rtsx_pci_card_pull_ctl_enable(struct rtsx_pcr *pcr, int card);
diff --git a/include/linux/mfd/samsung/s2mps14.h b/include/linux/mfd/samsung/s2mps14.h
index 4b449b8ac54..900cd7a0431 100644
--- a/include/linux/mfd/samsung/s2mps14.h
+++ b/include/linux/mfd/samsung/s2mps14.h
@@ -148,6 +148,8 @@ enum s2mps14_regulators {
 #define S2MPS14_ENABLE_SHIFT		6
 /* On/Off controlled by PWREN */
 #define S2MPS14_ENABLE_SUSPEND		(0x01 << S2MPS14_ENABLE_SHIFT)
+/* On/Off controlled by LDO10EN or EMMCEN */
+#define S2MPS14_ENABLE_EXT_CONTROL	(0x00 << S2MPS14_ENABLE_SHIFT)
 #define S2MPS14_LDO_N_VOLTAGES		(S2MPS14_LDO_VSEL_MASK + 1)
 #define S2MPS14_BUCK_N_VOLTAGES		(S2MPS14_BUCK_VSEL_MASK + 1)
 
diff --git a/include/linux/mfd/syscon/exynos5-pmu.h b/include/linux/mfd/syscon/exynos5-pmu.h
new file mode 100644
index 00000000000..00ef24bf6ed
--- /dev/null
+++ b/include/linux/mfd/syscon/exynos5-pmu.h
@@ -0,0 +1,44 @@
+/*
+ * Exynos5 SoC series Power Management Unit (PMU) register offsets
+ * and bit definitions.
+ *
+ * Copyright (C) 2014 Samsung Electronics Co., Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef _LINUX_MFD_SYSCON_PMU_EXYNOS5_H_
+#define _LINUX_MFD_SYSCON_PMU_EXYNOS5_H_
+
+/* Exynos5 PMU register definitions */
+#define EXYNOS5_HDMI_PHY_CONTROL		(0x700)
+#define EXYNOS5_USBDRD_PHY_CONTROL		(0x704)
+
+/* Exynos5250 specific register definitions */
+#define EXYNOS5_USBHOST_PHY_CONTROL		(0x708)
+#define EXYNOS5_EFNAND_PHY_CONTROL		(0x70c)
+#define EXYNOS5_MIPI_PHY0_CONTROL		(0x710)
+#define EXYNOS5_MIPI_PHY1_CONTROL		(0x714)
+#define EXYNOS5_ADC_PHY_CONTROL			(0x718)
+#define EXYNOS5_MTCADC_PHY_CONTROL		(0x71c)
+#define EXYNOS5_DPTX_PHY_CONTROL		(0x720)
+#define EXYNOS5_SATA_PHY_CONTROL		(0x724)
+
+/* Exynos5420 specific register definitions */
+#define EXYNOS5420_USBDRD1_PHY_CONTROL		(0x708)
+#define EXYNOS5420_USBHOST_PHY_CONTROL		(0x70c)
+#define EXYNOS5420_MIPI_PHY0_CONTROL		(0x714)
+#define EXYNOS5420_MIPI_PHY1_CONTROL		(0x718)
+#define EXYNOS5420_MIPI_PHY2_CONTROL		(0x71c)
+#define EXYNOS5420_ADC_PHY_CONTROL		(0x720)
+#define EXYNOS5420_MTCADC_PHY_CONTROL		(0x724)
+#define EXYNOS5420_DPTX_PHY_CONTROL		(0x728)
+
+#define EXYNOS5_PHY_ENABLE			BIT(0)
+
+#define EXYNOS5_MIPI_PHY_S_RESETN		BIT(1)
+#define EXYNOS5_MIPI_PHY_M_RESETN		BIT(2)
+
+#endif /* _LINUX_MFD_SYSCON_PMU_EXYNOS5_H_ */
diff --git a/include/linux/mfd/tc3589x.h b/include/linux/mfd/tc3589x.h
index 6b8e1ff4672..e6088c2e209 100644
--- a/include/linux/mfd/tc3589x.h
+++ b/include/linux/mfd/tc3589x.h
@@ -111,7 +111,6 @@ enum tx3589x_block {
 #define TC3589x_INT_PORIRQ	7
 
 #define TC3589x_NR_INTERNAL_IRQS	8
-#define TC3589x_INT_GPIO(x)	(TC3589x_NR_INTERNAL_IRQS + (x))
 
 struct tc3589x {
 	struct mutex lock;
diff --git a/include/linux/mfd/tps65090.h b/include/linux/mfd/tps65090.h
index 3f43069413e..0bf2708df15 100644
--- a/include/linux/mfd/tps65090.h
+++ b/include/linux/mfd/tps65090.h
@@ -64,6 +64,20 @@ enum {
 	TPS65090_REGULATOR_MAX,
 };
 
+/* Register addresses */
+#define TPS65090_REG_INTR_STS	0x00
+#define TPS65090_REG_INTR_STS2	0x01
+#define TPS65090_REG_INTR_MASK	0x02
+#define TPS65090_REG_INTR_MASK2	0x03
+#define TPS65090_REG_CG_CTRL0	0x04
+#define TPS65090_REG_CG_CTRL1	0x05
+#define TPS65090_REG_CG_CTRL2	0x06
+#define TPS65090_REG_CG_CTRL3	0x07
+#define TPS65090_REG_CG_CTRL4	0x08
+#define TPS65090_REG_CG_CTRL5	0x09
+#define TPS65090_REG_CG_STATUS1	0x0a
+#define TPS65090_REG_CG_STATUS2	0x0b
+
 struct tps65090 {
 	struct device		*dev;
 	struct regmap		*rmap;
@@ -78,11 +92,16 @@ struct tps65090 {
  *     DCDC1, DCDC2 and DCDC3.
  * @gpio: Gpio number if external control is enabled and controlled through
  *     gpio.
+ * @overcurrent_wait_valid: True if the overcurrent_wait should be applied.
+ * @overcurrent_wait: Value to set as the overcurrent wait time.  This is the
+ *     actual bitfield value, not a time in ms (valid value are 0 - 3).
  */
 struct tps65090_regulator_plat_data {
 	struct regulator_init_data *reg_init_data;
 	bool enable_ext_control;
 	int gpio;
+	bool overcurrent_wait_valid;
+	int overcurrent_wait;
 };
 
 struct tps65090_platform_data {
diff --git a/include/linux/mfd/tps65217.h b/include/linux/mfd/tps65217.h
index 54b5458ec08..95d6938737f 100644
--- a/include/linux/mfd/tps65217.h
+++ b/include/linux/mfd/tps65217.h
@@ -254,7 +254,6 @@ struct tps65217 {
 	struct tps65217_board *pdata;
 	unsigned long id;
 	struct regulator_desc desc[TPS65217_NUM_REGULATOR];
-	struct regulator_dev *rdev[TPS65217_NUM_REGULATOR];
 	struct regmap *regmap;
 };
 
diff --git a/include/linux/mfd/tps6586x.h b/include/linux/mfd/tps6586x.h
index cbecec2e353..96187ed9f9b 100644
--- a/include/linux/mfd/tps6586x.h
+++ b/include/linux/mfd/tps6586x.h
@@ -17,6 +17,8 @@
 #define TPS658621A	0x15
 #define TPS658621CD	0x2c
 #define TPS658623	0x1b
+#define TPS658640	0x01
+#define TPS658640v2	0x02
 #define TPS658643	0x03
 
 enum {
diff --git a/include/linux/miscdevice.h b/include/linux/miscdevice.h
index 51e26f3cd3b..ee80dd7d9f6 100644
--- a/include/linux/miscdevice.h
+++ b/include/linux/miscdevice.h
@@ -64,7 +64,7 @@ struct miscdevice  {
 	umode_t mode;
 };
 
-extern int misc_register(struct miscdevice * misc);
+extern int misc_register(struct miscdevice *misc);
 extern int misc_deregister(struct miscdevice *misc);
 
 #define MODULE_ALIAS_MISCDEV(minor)				\
diff --git a/include/linux/mlx4/qp.h b/include/linux/mlx4/qp.h
index b66e7610d4e..7040dc98ff8 100644
--- a/include/linux/mlx4/qp.h
+++ b/include/linux/mlx4/qp.h
@@ -421,6 +421,17 @@ struct mlx4_wqe_inline_seg {
 	__be32			byte_count;
 };
 
+enum mlx4_update_qp_attr {
+	MLX4_UPDATE_QP_SMAC		= 1 << 0,
+};
+
+struct mlx4_update_qp_params {
+	u8	smac_index;
+};
+
+int mlx4_update_qp(struct mlx4_dev *dev, struct mlx4_qp *qp,
+		   enum mlx4_update_qp_attr attr,
+		   struct mlx4_update_qp_params *params);
 int mlx4_qp_modify(struct mlx4_dev *dev, struct mlx4_mtt *mtt,
 		   enum mlx4_qp_state cur_state, enum mlx4_qp_state new_state,
 		   struct mlx4_qp_context *context, enum mlx4_qp_optpar optpar,
diff --git a/include/linux/mlx5/device.h b/include/linux/mlx5/device.h
index 407bdb67fd4..3406cfb1267 100644
--- a/include/linux/mlx5/device.h
+++ b/include/linux/mlx5/device.h
@@ -179,6 +179,7 @@ enum {
 	MLX5_DEV_CAP_FLAG_BAD_QKEY_CNTR	= 1LL <<  9,
 	MLX5_DEV_CAP_FLAG_APM		= 1LL << 17,
 	MLX5_DEV_CAP_FLAG_ATOMIC	= 1LL << 18,
+	MLX5_DEV_CAP_FLAG_BLOCK_MCAST	= 1LL << 23,
 	MLX5_DEV_CAP_FLAG_ON_DMND_PG	= 1LL << 24,
 	MLX5_DEV_CAP_FLAG_CQ_MODER	= 1LL << 29,
 	MLX5_DEV_CAP_FLAG_RESIZE_CQ	= 1LL << 30,
diff --git a/include/linux/mlx5/qp.h b/include/linux/mlx5/qp.h
index f829ad80ff2..9709b30e2d6 100644
--- a/include/linux/mlx5/qp.h
+++ b/include/linux/mlx5/qp.h
@@ -146,6 +146,7 @@ enum {
 
 enum {
 	MLX5_QP_LAT_SENSITIVE	= 1 << 28,
+	MLX5_QP_BLOCK_MCAST	= 1 << 30,
 	MLX5_QP_ENABLE_SIG	= 1 << 31,
 };
 
diff --git a/include/linux/mm.h b/include/linux/mm.h
index bf9811e1321..d6777060449 100644
--- a/include/linux/mm.h
+++ b/include/linux/mm.h
@@ -370,6 +370,8 @@ static inline int is_vmalloc_or_module_addr(const void *x)
 }
 #endif
 
+extern void kvfree(const void *addr);
+
 static inline void compound_lock(struct page *page)
 {
 #ifdef CONFIG_TRANSPARENT_HUGEPAGE
diff --git a/include/linux/mtd/spear_smi.h b/include/linux/mtd/spear_smi.h
index 8ae1726044c..581603ac127 100644
--- a/include/linux/mtd/spear_smi.h
+++ b/include/linux/mtd/spear_smi.h
@@ -1,6 +1,6 @@
 /*
  * Copyright © 2010 ST Microelectronics
- * Shiraz Hashim <shiraz.hashim@st.com>
+ * Shiraz Hashim <shiraz.linux.kernel@gmail.com>
  *
  * This file is licensed under the terms of the GNU General Public
  * License version 2. This program is licensed "as is" without any
diff --git a/include/linux/net.h b/include/linux/net.h
index 94734a6259a..17d83393afc 100644
--- a/include/linux/net.h
+++ b/include/linux/net.h
@@ -248,24 +248,17 @@ do {								\
 bool __net_get_random_once(void *buf, int nbytes, bool *done,
 			   struct static_key *done_key);
 
-#ifdef HAVE_JUMP_LABEL
-#define ___NET_RANDOM_STATIC_KEY_INIT ((struct static_key) \
-		{ .enabled = ATOMIC_INIT(0), .entries = (void *)1 })
-#else /* !HAVE_JUMP_LABEL */
-#define ___NET_RANDOM_STATIC_KEY_INIT STATIC_KEY_INIT_FALSE
-#endif /* HAVE_JUMP_LABEL */
-
 #define net_get_random_once(buf, nbytes)				\
 	({								\
 		bool ___ret = false;					\
 		static bool ___done = false;				\
-		static struct static_key ___done_key =			\
-			___NET_RANDOM_STATIC_KEY_INIT;			\
-		if (!static_key_true(&___done_key))			\
+		static struct static_key ___once_key =			\
+			STATIC_KEY_INIT_TRUE;				\
+		if (static_key_true(&___once_key))			\
 			___ret = __net_get_random_once(buf,		\
 						       nbytes,		\
 						       &___done,	\
-						       &___done_key);	\
+						       &___once_key);	\
 		___ret;							\
 	})
 
diff --git a/include/linux/netdevice.h b/include/linux/netdevice.h
index 616415a4fee..6c1ae9fd950 100644
--- a/include/linux/netdevice.h
+++ b/include/linux/netdevice.h
@@ -1144,6 +1144,7 @@ struct net_device_ops {
 	netdev_tx_t		(*ndo_dfwd_start_xmit) (struct sk_buff *skb,
 							struct net_device *dev,
 							void *priv);
+	int			(*ndo_get_lock_subclass)(struct net_device *dev);
 };
 
 /**
@@ -2950,7 +2951,12 @@ static inline void netif_addr_lock(struct net_device *dev)
 
 static inline void netif_addr_lock_nested(struct net_device *dev)
 {
-	spin_lock_nested(&dev->addr_list_lock, SINGLE_DEPTH_NESTING);
+	int subclass = SINGLE_DEPTH_NESTING;
+
+	if (dev->netdev_ops->ndo_get_lock_subclass)
+		subclass = dev->netdev_ops->ndo_get_lock_subclass(dev);
+
+	spin_lock_nested(&dev->addr_list_lock, subclass);
 }
 
 static inline void netif_addr_lock_bh(struct net_device *dev)
@@ -3050,10 +3056,19 @@ extern int		weight_p;
 extern int		bpf_jit_enable;
 
 bool netdev_has_upper_dev(struct net_device *dev, struct net_device *upper_dev);
+struct net_device *netdev_upper_get_next_dev_rcu(struct net_device *dev,
+						     struct list_head **iter);
 struct net_device *netdev_all_upper_get_next_dev_rcu(struct net_device *dev,
 						     struct list_head **iter);
 
 /* iterate through upper list, must be called under RCU read lock */
+#define netdev_for_each_upper_dev_rcu(dev, updev, iter) \
+	for (iter = &(dev)->adj_list.upper, \
+	     updev = netdev_upper_get_next_dev_rcu(dev, &(iter)); \
+	     updev; \
+	     updev = netdev_upper_get_next_dev_rcu(dev, &(iter)))
+
+/* iterate through upper list, must be called under RCU read lock */
 #define netdev_for_each_all_upper_dev_rcu(dev, updev, iter) \
 	for (iter = &(dev)->all_adj_list.upper, \
 	     updev = netdev_all_upper_get_next_dev_rcu(dev, &(iter)); \
@@ -3077,6 +3092,14 @@ void *netdev_lower_get_next_private_rcu(struct net_device *dev,
 	     priv; \
 	     priv = netdev_lower_get_next_private_rcu(dev, &(iter)))
 
+void *netdev_lower_get_next(struct net_device *dev,
+				struct list_head **iter);
+#define netdev_for_each_lower_dev(dev, ldev, iter) \
+	for (iter = &(dev)->adj_list.lower, \
+	     ldev = netdev_lower_get_next(dev, &(iter)); \
+	     ldev; \
+	     ldev = netdev_lower_get_next(dev, &(iter)))
+
 void *netdev_adjacent_get_private(struct list_head *adj_list);
 void *netdev_lower_get_first_private_rcu(struct net_device *dev);
 struct net_device *netdev_master_upper_dev_get(struct net_device *dev);
@@ -3092,6 +3115,8 @@ void netdev_upper_dev_unlink(struct net_device *dev,
 void netdev_adjacent_rename_links(struct net_device *dev, char *oldname);
 void *netdev_lower_dev_get_private(struct net_device *dev,
 				   struct net_device *lower_dev);
+int dev_get_nest_level(struct net_device *dev,
+		       bool (*type_check)(struct net_device *dev));
 int skb_checksum_help(struct sk_buff *skb);
 struct sk_buff *__skb_gso_segment(struct sk_buff *skb,
 				  netdev_features_t features, bool tx_path);
@@ -3180,12 +3205,7 @@ void netdev_change_features(struct net_device *dev);
 void netif_stacked_transfer_operstate(const struct net_device *rootdev,
 					struct net_device *dev);
 
-netdev_features_t netif_skb_dev_features(struct sk_buff *skb,
-					 const struct net_device *dev);
-static inline netdev_features_t netif_skb_features(struct sk_buff *skb)
-{
-	return netif_skb_dev_features(skb, skb->dev);
-}
+netdev_features_t netif_skb_features(struct sk_buff *skb);
 
 static inline bool net_gso_ok(netdev_features_t features, int gso_type)
 {
diff --git a/include/linux/netlink.h b/include/linux/netlink.h
index aad8eeaf416..f64b01787dd 100644
--- a/include/linux/netlink.h
+++ b/include/linux/netlink.h
@@ -169,4 +169,11 @@ struct netlink_tap {
 extern int netlink_add_tap(struct netlink_tap *nt);
 extern int netlink_remove_tap(struct netlink_tap *nt);
 
+bool __netlink_ns_capable(const struct netlink_skb_parms *nsp,
+			  struct user_namespace *ns, int cap);
+bool netlink_ns_capable(const struct sk_buff *skb,
+			struct user_namespace *ns, int cap);
+bool netlink_capable(const struct sk_buff *skb, int cap);
+bool netlink_net_capable(const struct sk_buff *skb, int cap);
+
 #endif	/* __LINUX_NETLINK_H */
diff --git a/include/linux/of.h b/include/linux/of.h
index 919bf211877..fa362867b45 100644
--- a/include/linux/of.h
+++ b/include/linux/of.h
@@ -130,6 +130,12 @@ static inline int of_node_check_flag(struct device_node *n, unsigned long flag)
 	return test_bit(flag, &n->_flags);
 }
 
+static inline int of_node_test_and_set_flag(struct device_node *n,
+					    unsigned long flag)
+{
+	return test_and_set_bit(flag, &n->_flags);
+}
+
 static inline void of_node_set_flag(struct device_node *n, unsigned long flag)
 {
 	set_bit(flag, &n->_flags);
@@ -197,6 +203,7 @@ static inline unsigned long of_read_ulong(const __be32 *cell, int size)
 /* flag descriptions */
 #define OF_DYNAMIC	1 /* node and properties were allocated via kmalloc */
 #define OF_DETACHED	2 /* node has been detached from the device tree */
+#define OF_POPULATED	3 /* device already created for the node */
 
 #define OF_IS_DYNAMIC(x) test_bit(OF_DYNAMIC, &x->_flags)
 #define OF_MARK_DYNAMIC(x) set_bit(OF_DYNAMIC, &x->_flags)
@@ -349,7 +356,7 @@ int of_device_is_stdout_path(struct device_node *dn);
 
 #else /* CONFIG_OF */
 
-static inline const char* of_node_full_name(struct device_node *np)
+static inline const char* of_node_full_name(const struct device_node *np)
 {
 	return "<no-node>";
 }
@@ -374,6 +381,11 @@ static inline struct device_node *of_find_matching_node_and_match(
 	return NULL;
 }
 
+static inline struct device_node *of_find_node_by_path(const char *path)
+{
+	return NULL;
+}
+
 static inline struct device_node *of_get_parent(const struct device_node *node)
 {
 	return NULL;
diff --git a/include/linux/of_gpio.h b/include/linux/of_gpio.h
index f14123a5a9d..38fc0503601 100644
--- a/include/linux/of_gpio.h
+++ b/include/linux/of_gpio.h
@@ -19,7 +19,6 @@
 #include <linux/errno.h>
 #include <linux/gpio.h>
 #include <linux/of.h>
-#include <linux/gpio/consumer.h>
 
 struct device_node;
 
@@ -48,7 +47,7 @@ static inline struct of_mm_gpio_chip *to_of_mm_gpio_chip(struct gpio_chip *gc)
 	return container_of(gc, struct of_mm_gpio_chip, gc);
 }
 
-extern struct gpio_desc *of_get_named_gpiod_flags(struct device_node *np,
+extern int of_get_named_gpio_flags(struct device_node *np,
 		const char *list_name, int index, enum of_gpio_flags *flags);
 
 extern int of_mm_gpiochip_add(struct device_node *np,
@@ -63,10 +62,10 @@ extern int of_gpio_simple_xlate(struct gpio_chip *gc,
 #else /* CONFIG_OF_GPIO */
 
 /* Drivers may not strictly depend on the GPIO support, so let them link. */
-static inline struct gpio_desc *of_get_named_gpiod_flags(struct device_node *np,
+static inline int of_get_named_gpio_flags(struct device_node *np,
 		const char *list_name, int index, enum of_gpio_flags *flags)
 {
-	return ERR_PTR(-ENOSYS);
+	return -ENOSYS;
 }
 
 static inline int of_gpio_simple_xlate(struct gpio_chip *gc,
@@ -81,18 +80,6 @@ static inline void of_gpiochip_remove(struct gpio_chip *gc) { }
 
 #endif /* CONFIG_OF_GPIO */
 
-static inline int of_get_named_gpio_flags(struct device_node *np,
-		const char *list_name, int index, enum of_gpio_flags *flags)
-{
-	struct gpio_desc *desc;
-	desc = of_get_named_gpiod_flags(np, list_name, index, flags);
-
-	if (IS_ERR(desc))
-		return PTR_ERR(desc);
-	else
-		return desc_to_gpio(desc);
-}
-
 /**
  * of_gpio_named_count() - Count GPIOs for a device
  * @np:		device node to count GPIOs for
@@ -129,22 +116,6 @@ static inline int of_gpio_count(struct device_node *np)
 	return of_gpio_named_count(np, "gpios");
 }
 
-/**
- * of_get_gpiod_flags() - Get a GPIO descriptor and flags to use with GPIO API
- * @np:		device node to get GPIO from
- * @index:	index of the GPIO
- * @flags:	a flags pointer to fill in
- *
- * Returns GPIO descriptor to use with Linux generic GPIO API, or a errno
- * value on the error condition. If @flags is not NULL the function also fills
- * in flags for the GPIO.
- */
-static inline struct gpio_desc *of_get_gpiod_flags(struct device_node *np,
-					int index, enum of_gpio_flags *flags)
-{
-	return of_get_named_gpiod_flags(np, "gpios", index, flags);
-}
-
 static inline int of_get_gpio_flags(struct device_node *np, int index,
 		      enum of_gpio_flags *flags)
 {
diff --git a/include/linux/of_irq.h b/include/linux/of_irq.h
index 3f23b4472c3..6404253d810 100644
--- a/include/linux/of_irq.h
+++ b/include/linux/of_irq.h
@@ -44,11 +44,16 @@ extern void of_irq_init(const struct of_device_id *matches);
 
 #ifdef CONFIG_OF_IRQ
 extern int of_irq_count(struct device_node *dev);
+extern int of_irq_get(struct device_node *dev, int index);
 #else
 static inline int of_irq_count(struct device_node *dev)
 {
 	return 0;
 }
+static inline int of_irq_get(struct device_node *dev, int index)
+{
+	return 0;
+}
 #endif
 
 #if defined(CONFIG_OF)
diff --git a/include/linux/of_mdio.h b/include/linux/of_mdio.h
index 6fe8464ed76..881a7c3571f 100644
--- a/include/linux/of_mdio.h
+++ b/include/linux/of_mdio.h
@@ -31,7 +31,12 @@ extern struct mii_bus *of_mdio_find_bus(struct device_node *mdio_np);
 #else /* CONFIG_OF */
 static inline int of_mdiobus_register(struct mii_bus *mdio, struct device_node *np)
 {
-	return -ENOSYS;
+	/*
+	 * Fall back to the non-DT function to register a bus.
+	 * This way, we don't have to keep compat bits around in drivers.
+	 */
+
+	return mdiobus_register(mdio);
 }
 
 static inline struct phy_device *of_phy_find_device(struct device_node *phy_np)
diff --git a/include/linux/of_platform.h b/include/linux/of_platform.h
index 05cb4a92825..b1010eeaac0 100644
--- a/include/linux/of_platform.h
+++ b/include/linux/of_platform.h
@@ -72,6 +72,7 @@ extern int of_platform_populate(struct device_node *root,
 				const struct of_device_id *matches,
 				const struct of_dev_auxdata *lookup,
 				struct device *parent);
+extern int of_platform_depopulate(struct device *parent);
 #else
 static inline int of_platform_populate(struct device_node *root,
 					const struct of_device_id *matches,
@@ -80,6 +81,10 @@ static inline int of_platform_populate(struct device_node *root,
 {
 	return -ENODEV;
 }
+static inline int of_platform_depopulate(struct device *parent)
+{
+	return -ENODEV;
+}
 #endif
 
 #endif	/* _LINUX_OF_PLATFORM_H */
diff --git a/include/linux/omap-dma.h b/include/linux/omap-dma.h
index 41a13e70f41..7944cdc27be 100644
--- a/include/linux/omap-dma.h
+++ b/include/linux/omap-dma.h
@@ -10,7 +10,7 @@
 
 struct dma_chan;
 
-#if defined(CONFIG_DMA_OMAP) || defined(CONFIG_DMA_OMAP_MODULE)
+#if defined(CONFIG_DMA_OMAP) || (defined(CONFIG_DMA_OMAP_MODULE) && defined(MODULE))
 bool omap_dma_filter_fn(struct dma_chan *, void *);
 #else
 static inline bool omap_dma_filter_fn(struct dma_chan *c, void *d)
diff --git a/include/linux/pci.h b/include/linux/pci.h
index aab57b4abe7..71d9673c1b2 100644
--- a/include/linux/pci.h
+++ b/include/linux/pci.h
@@ -365,6 +365,7 @@ struct pci_dev {
 #endif
 	phys_addr_t rom; /* Physical address of ROM if it's not from the BAR */
 	size_t romlen; /* Length of ROM if it's not from the BAR */
+	char *driver_override; /* Driver name to force a match */
 };
 
 static inline struct pci_dev *pci_physfn(struct pci_dev *dev)
@@ -477,6 +478,19 @@ static inline bool pci_is_root_bus(struct pci_bus *pbus)
 	return !(pbus->parent);
 }
 
+/**
+ * pci_is_bridge - check if the PCI device is a bridge
+ * @dev: PCI device
+ *
+ * Return true if the PCI device is bridge whether it has subordinate
+ * or not.
+ */
+static inline bool pci_is_bridge(struct pci_dev *dev)
+{
+	return dev->hdr_type == PCI_HEADER_TYPE_BRIDGE ||
+		dev->hdr_type == PCI_HEADER_TYPE_CARDBUS;
+}
+
 static inline struct pci_dev *pci_upstream_bridge(struct pci_dev *dev)
 {
 	dev = pci_physfn(dev);
@@ -518,7 +532,7 @@ static inline int pcibios_err_to_errno(int err)
 	case PCIBIOS_FUNC_NOT_SUPPORTED:
 		return -ENOENT;
 	case PCIBIOS_BAD_VENDOR_ID:
-		return -EINVAL;
+		return -ENOTTY;
 	case PCIBIOS_DEVICE_NOT_FOUND:
 		return -ENODEV;
 	case PCIBIOS_BAD_REGISTER_NUMBER:
@@ -529,7 +543,7 @@ static inline int pcibios_err_to_errno(int err)
 		return -ENOSPC;
 	}
 
-	return -ENOTTY;
+	return -ERANGE;
 }
 
 /* Low-level architecture-dependent routines */
@@ -603,6 +617,9 @@ struct pci_error_handlers {
 	/* PCI slot has been reset */
 	pci_ers_result_t (*slot_reset)(struct pci_dev *dev);
 
+	/* PCI function reset prepare or completed */
+	void (*reset_notify)(struct pci_dev *dev, bool prepare);
+
 	/* Device driver may resume normal operations */
 	void (*resume)(struct pci_dev *dev);
 };
@@ -680,8 +697,8 @@ struct pci_driver {
 
 /**
  * PCI_VDEVICE - macro used to describe a specific pci device in short form
- * @vendor: the vendor name
- * @device: the 16 bit PCI Device ID
+ * @vend: the vendor name
+ * @dev: the 16 bit PCI Device ID
  *
  * This macro is used to create a struct pci_device_id that matches a
  * specific PCI device.  The subvendor, and subdevice fields will be set
@@ -689,9 +706,9 @@ struct pci_driver {
  * private data.
  */
 
-#define PCI_VDEVICE(vendor, device)		\
-	PCI_VENDOR_ID_##vendor, (device),	\
-	PCI_ANY_ID, PCI_ANY_ID, 0, 0
+#define PCI_VDEVICE(vend, dev) \
+	.vendor = PCI_VENDOR_ID_##vend, .device = (dev), \
+	.subvendor = PCI_ANY_ID, .subdevice = PCI_ANY_ID, 0, 0
 
 /* these external functions are only available when PCI support is enabled */
 #ifdef CONFIG_PCI
@@ -764,7 +781,7 @@ int pci_scan_slot(struct pci_bus *bus, int devfn);
 struct pci_dev *pci_scan_single_device(struct pci_bus *bus, int devfn);
 void pci_device_add(struct pci_dev *dev, struct pci_bus *bus);
 unsigned int pci_scan_child_bus(struct pci_bus *bus);
-int __must_check pci_bus_add_device(struct pci_dev *dev);
+void pci_bus_add_device(struct pci_dev *dev);
 void pci_read_bridge_bases(struct pci_bus *child);
 struct resource *pci_find_parent_resource(const struct pci_dev *dev,
 					  struct resource *res);
@@ -1158,7 +1175,6 @@ struct msix_entry {
 
 #ifdef CONFIG_PCI_MSI
 int pci_msi_vec_count(struct pci_dev *dev);
-int pci_enable_msi_block(struct pci_dev *dev, int nvec);
 void pci_msi_shutdown(struct pci_dev *dev);
 void pci_disable_msi(struct pci_dev *dev);
 int pci_msix_vec_count(struct pci_dev *dev);
@@ -1188,8 +1204,6 @@ static inline int pci_enable_msix_exact(struct pci_dev *dev,
 }
 #else
 static inline int pci_msi_vec_count(struct pci_dev *dev) { return -ENOSYS; }
-static inline int pci_enable_msi_block(struct pci_dev *dev, int nvec)
-{ return -ENOSYS; }
 static inline void pci_msi_shutdown(struct pci_dev *dev) { }
 static inline void pci_disable_msi(struct pci_dev *dev) { }
 static inline int pci_msix_vec_count(struct pci_dev *dev) { return -ENOSYS; }
@@ -1244,7 +1258,7 @@ static inline void pcie_set_ecrc_checking(struct pci_dev *dev) { }
 static inline void pcie_ecrc_get_policy(char *str) { }
 #endif
 
-#define pci_enable_msi(pdev)	pci_enable_msi_block(pdev, 1)
+#define pci_enable_msi(pdev)	pci_enable_msi_exact(pdev, 1)
 
 #ifdef CONFIG_HT_IRQ
 /* The functions a driver should call */
@@ -1572,13 +1586,13 @@ extern unsigned long pci_hotplug_io_size;
 extern unsigned long pci_hotplug_mem_size;
 
 /* Architecture-specific versions may override these (weak) */
-int pcibios_add_platform_entries(struct pci_dev *dev);
 void pcibios_disable_device(struct pci_dev *dev);
 void pcibios_set_master(struct pci_dev *dev);
 int pcibios_set_pcie_reset_state(struct pci_dev *dev,
 				 enum pcie_reset_state state);
 int pcibios_add_device(struct pci_dev *dev);
 void pcibios_release_device(struct pci_dev *dev);
+void pcibios_penalize_isa_irq(int irq, int active);
 
 #ifdef CONFIG_HIBERNATE_CALLBACKS
 extern struct dev_pm_ops pcibios_pm_ops;
diff --git a/include/linux/pci_ids.h b/include/linux/pci_ids.h
index d4de24b4d4c..7fa31731c85 100644
--- a/include/linux/pci_ids.h
+++ b/include/linux/pci_ids.h
@@ -1631,8 +1631,6 @@
 #define PCI_DEVICE_ID_ATT_VENUS_MODEM	0x480
 
 #define PCI_VENDOR_ID_SPECIALIX		0x11cb
-#define PCI_DEVICE_ID_SPECIALIX_IO8	0x2000
-#define PCI_DEVICE_ID_SPECIALIX_RIO	0x8000
 #define PCI_SUBDEVICE_ID_SPECIALIX_SPEED4 0xa004
 
 #define PCI_VENDOR_ID_ANALOG_DEVICES	0x11d4
@@ -2874,7 +2872,6 @@
 #define PCI_DEVICE_ID_SCALEMP_VSMP_CTL	0x1010
 
 #define PCI_VENDOR_ID_COMPUTONE		0x8e0e
-#define PCI_DEVICE_ID_COMPUTONE_IP2EX	0x0291
 #define PCI_DEVICE_ID_COMPUTONE_PG	0x0302
 #define PCI_SUBVENDOR_ID_COMPUTONE	0x8e0e
 #define PCI_SUBDEVICE_ID_COMPUTONE_PG4	0x0001
diff --git a/include/linux/percpu.h b/include/linux/percpu.h
index e7a0b95ed52..495c6543a8f 100644
--- a/include/linux/percpu.h
+++ b/include/linux/percpu.h
@@ -639,7 +639,7 @@ do {									\
 #  define raw_cpu_add_return_8(pcp, val)	raw_cpu_generic_add_return(pcp, val)
 # endif
 # define raw_cpu_add_return(pcp, val)	\
-	__pcpu_size_call_return2(raw_add_return_, pcp, val)
+	__pcpu_size_call_return2(raw_cpu_add_return_, pcp, val)
 #endif
 
 #define raw_cpu_sub_return(pcp, val)	raw_cpu_add_return(pcp, -(typeof(pcp))(val))
diff --git a/include/linux/perf_event.h b/include/linux/perf_event.h
index 3356abcfff1..3ef6ea12806 100644
--- a/include/linux/perf_event.h
+++ b/include/linux/perf_event.h
@@ -402,6 +402,8 @@ struct perf_event {
 
 	struct ring_buffer		*rb;
 	struct list_head		rb_entry;
+	unsigned long			rcu_batches;
+	int				rcu_pending;
 
 	/* poll related */
 	wait_queue_head_t		waitq;
diff --git a/include/linux/phy.h b/include/linux/phy.h
index 24126c4b27b..4d0221fd068 100644
--- a/include/linux/phy.h
+++ b/include/linux/phy.h
@@ -75,6 +75,7 @@ typedef enum {
 	PHY_INTERFACE_MODE_SMII,
 	PHY_INTERFACE_MODE_XGMII,
 	PHY_INTERFACE_MODE_MOCA,
+	PHY_INTERFACE_MODE_QSGMII,
 	PHY_INTERFACE_MODE_MAX,
 } phy_interface_t;
 
@@ -116,6 +117,8 @@ static inline const char *phy_modes(phy_interface_t interface)
 		return "xgmii";
 	case PHY_INTERFACE_MODE_MOCA:
 		return "moca";
+	case PHY_INTERFACE_MODE_QSGMII:
+		return "qsgmii";
 	default:
 		return "unknown";
 	}
diff --git a/include/linux/phy/phy.h b/include/linux/phy/phy.h
index e2f5ca96cdd..2760744cb2a 100644
--- a/include/linux/phy/phy.h
+++ b/include/linux/phy/phy.h
@@ -174,21 +174,29 @@ void devm_of_phy_provider_unregister(struct device *dev,
 #else
 static inline int phy_pm_runtime_get(struct phy *phy)
 {
+	if (!phy)
+		return 0;
 	return -ENOSYS;
 }
 
 static inline int phy_pm_runtime_get_sync(struct phy *phy)
 {
+	if (!phy)
+		return 0;
 	return -ENOSYS;
 }
 
 static inline int phy_pm_runtime_put(struct phy *phy)
 {
+	if (!phy)
+		return 0;
 	return -ENOSYS;
 }
 
 static inline int phy_pm_runtime_put_sync(struct phy *phy)
 {
+	if (!phy)
+		return 0;
 	return -ENOSYS;
 }
 
@@ -204,21 +212,29 @@ static inline void phy_pm_runtime_forbid(struct phy *phy)
 
 static inline int phy_init(struct phy *phy)
 {
+	if (!phy)
+		return 0;
 	return -ENOSYS;
 }
 
 static inline int phy_exit(struct phy *phy)
 {
+	if (!phy)
+		return 0;
 	return -ENOSYS;
 }
 
 static inline int phy_power_on(struct phy *phy)
 {
+	if (!phy)
+		return 0;
 	return -ENOSYS;
 }
 
 static inline int phy_power_off(struct phy *phy)
 {
+	if (!phy)
+		return 0;
 	return -ENOSYS;
 }
 
diff --git a/include/linux/platform_data/at91_adc.h b/include/linux/platform_data/at91_adc.h
index b3ca1e94e0c..7819fc78773 100644
--- a/include/linux/platform_data/at91_adc.h
+++ b/include/linux/platform_data/at91_adc.h
@@ -7,23 +7,10 @@
 #ifndef _AT91_ADC_H_
 #define _AT91_ADC_H_
 
-/**
- * struct at91_adc_reg_desc - Various informations relative to registers
- * @channel_base:	Base offset for the channel data registers
- * @drdy_mask:		Mask of the DRDY field in the relevant registers
-			(Interruptions registers mostly)
- * @status_register:	Offset of the Interrupt Status Register
- * @trigger_register:	Offset of the Trigger setup register
- * @mr_prescal_mask:	Mask of the PRESCAL field in the adc MR register
- * @mr_startup_mask:	Mask of the STARTUP field in the adc MR register
- */
-struct at91_adc_reg_desc {
-	u8	channel_base;
-	u32	drdy_mask;
-	u8	status_register;
-	u8	trigger_register;
-	u32	mr_prescal_mask;
-	u32	mr_startup_mask;
+enum atmel_adc_ts_type {
+	ATMEL_ADC_TOUCHSCREEN_NONE = 0,
+	ATMEL_ADC_TOUCHSCREEN_4WIRE = 4,
+	ATMEL_ADC_TOUCHSCREEN_5WIRE = 5,
 };
 
 /**
@@ -42,23 +29,21 @@ struct at91_adc_trigger {
 /**
  * struct at91_adc_data - platform data for ADC driver
  * @channels_used:		channels in use on the board as a bitmask
- * @num_channels:		global number of channels available on the board
- * @registers:			Registers definition on the board
  * @startup_time:		startup time of the ADC in microseconds
  * @trigger_list:		Triggers available in the ADC
  * @trigger_number:		Number of triggers available in the ADC
  * @use_external_triggers:	does the board has external triggers availables
  * @vref:			Reference voltage for the ADC in millivolts
+ * @touchscreen_type:		If a touchscreen is connected, its type (4 or 5 wires)
  */
 struct at91_adc_data {
 	unsigned long			channels_used;
-	u8				num_channels;
-	struct at91_adc_reg_desc	*registers;
 	u8				startup_time;
 	struct at91_adc_trigger		*trigger_list;
 	u8				trigger_number;
 	bool				use_external_triggers;
 	u16				vref;
+	enum atmel_adc_ts_type		touchscreen_type;
 };
 
 extern void __init at91_add_device_adc(struct at91_adc_data *data);
diff --git a/include/linux/platform_data/atmel.h b/include/linux/platform_data/atmel.h
index e26b0c14ede..4b452c6a2f7 100644
--- a/include/linux/platform_data/atmel.h
+++ b/include/linux/platform_data/atmel.h
@@ -84,14 +84,6 @@ struct atmel_uart_data {
 	short			use_dma_rx;	/* use receive DMA? */
 	void __iomem		*regs;		/* virt. base address, if any */
 	struct serial_rs485	rs485;		/* rs485 settings */
-	int			rts_gpio;	/* optional RTS GPIO */
-};
-
- /* Touchscreen Controller */
-struct at91_tsadcc_data {
-	unsigned int    adc_clock;
-	u8		pendet_debounce;
-	u8		ts_sample_hold_time;
 };
 
 /* CAN */
diff --git a/include/linux/platform_data/edma.h b/include/linux/platform_data/edma.h
index f50821cb64b..eb8d5627d08 100644
--- a/include/linux/platform_data/edma.h
+++ b/include/linux/platform_data/edma.h
@@ -43,15 +43,15 @@
 
 /* PaRAM slots are laid out like this */
 struct edmacc_param {
-	unsigned int opt;
-	unsigned int src;
-	unsigned int a_b_cnt;
-	unsigned int dst;
-	unsigned int src_dst_bidx;
-	unsigned int link_bcntrld;
-	unsigned int src_dst_cidx;
-	unsigned int ccnt;
-};
+	u32 opt;
+	u32 src;
+	u32 a_b_cnt;
+	u32 dst;
+	u32 src_dst_bidx;
+	u32 link_bcntrld;
+	u32 src_dst_cidx;
+	u32 ccnt;
+} __packed;
 
 /* fields in edmacc_param.opt */
 #define SAM		BIT(0)
@@ -130,7 +130,7 @@ void edma_set_src(unsigned slot, dma_addr_t src_port,
 				enum address_mode mode, enum fifo_width);
 void edma_set_dest(unsigned slot, dma_addr_t dest_port,
 				 enum address_mode mode, enum fifo_width);
-void edma_get_position(unsigned slot, dma_addr_t *src, dma_addr_t *dst);
+dma_addr_t edma_get_position(unsigned slot, bool dst);
 void edma_set_src_index(unsigned slot, s16 src_bidx, s16 src_cidx);
 void edma_set_dest_index(unsigned slot, s16 dest_bidx, s16 dest_cidx);
 void edma_set_transfer_params(unsigned slot, u16 acnt, u16 bcnt, u16 ccnt,
@@ -158,13 +158,6 @@ struct edma_rsv_info {
 
 /* platform_data for EDMA driver */
 struct edma_soc_info {
-
-	/* how many dma resources of each type */
-	unsigned	n_channel;
-	unsigned	n_region;
-	unsigned	n_slot;
-	unsigned	n_tc;
-	unsigned	n_cc;
 	/*
 	 * Default queue is expected to be a low-priority queue.
 	 * This way, long transfers on the default queue started
@@ -175,7 +168,6 @@ struct edma_soc_info {
 	/* Resource reservation for other cores */
 	struct edma_rsv_info	*rsv;
 
-	s8	(*queue_tc_mapping)[2];
 	s8	(*queue_priority_mapping)[2];
 	const s16	(*xbar_chans)[2];
 };
diff --git a/include/linux/platform_data/max3421-hcd.h b/include/linux/platform_data/max3421-hcd.h
new file mode 100644
index 00000000000..0303d197008
--- /dev/null
+++ b/include/linux/platform_data/max3421-hcd.h
@@ -0,0 +1,24 @@
+/*
+ * Copyright (c) 2014 eGauge Systems LLC
+ *	Contributed by David Mosberger-Tang <davidm@egauge.net>
+ *
+ * Platform-data structure for MAX3421 USB HCD driver.
+ *
+ */
+#ifndef MAX3421_HCD_PLAT_H_INCLUDED
+#define MAX3421_HCD_PLAT_H_INCLUDED
+
+/*
+ * This structure defines the mapping of certain auxiliary functions to the
+ * MAX3421E GPIO pins.  The chip has eight GP inputs and eight GP outputs.
+ * A value of 0 indicates that the pin is not used/wired to anything.
+ *
+ * At this point, the only control the max3421-hcd driver cares about is
+ * to control Vbus (5V to the peripheral).
+ */
+struct max3421_hcd_platform_data {
+	u8 vbus_gpout;			/* pin controlling Vbus */
+	u8 vbus_active_level;		/* level that turns on power */
+};
+
+#endif /* MAX3421_HCD_PLAT_H_INCLUDED */
diff --git a/include/linux/platform_data/syscon.h b/include/linux/platform_data/syscon.h
new file mode 100644
index 00000000000..2354c6fa372
--- /dev/null
+++ b/include/linux/platform_data/syscon.h
@@ -0,0 +1,8 @@
+#ifndef PLATFORM_DATA_SYSCON_H
+#define PLATFORM_DATA_SYSCON_H
+
+struct syscon_platform_data {
+	const char *label;
+};
+
+#endif
diff --git a/include/linux/rcupdate.h b/include/linux/rcupdate.h
index 00a7fd61b3c..5a75d19aa66 100644
--- a/include/linux/rcupdate.h
+++ b/include/linux/rcupdate.h
@@ -44,6 +44,7 @@
 #include <linux/debugobjects.h>
 #include <linux/bug.h>
 #include <linux/compiler.h>
+#include <linux/percpu.h>
 #include <asm/barrier.h>
 
 extern int rcu_expedited; /* for sysctl */
@@ -51,7 +52,17 @@ extern int rcu_expedited; /* for sysctl */
 extern int rcutorture_runnable; /* for sysctl */
 #endif /* #ifdef CONFIG_RCU_TORTURE_TEST */
 
+enum rcutorture_type {
+	RCU_FLAVOR,
+	RCU_BH_FLAVOR,
+	RCU_SCHED_FLAVOR,
+	SRCU_FLAVOR,
+	INVALID_RCU_FLAVOR
+};
+
 #if defined(CONFIG_TREE_RCU) || defined(CONFIG_TREE_PREEMPT_RCU)
+void rcutorture_get_gp_data(enum rcutorture_type test_type, int *flags,
+			    unsigned long *gpnum, unsigned long *completed);
 void rcutorture_record_test_transition(void);
 void rcutorture_record_progress(unsigned long vernum);
 void do_trace_rcu_torture_read(const char *rcutorturename,
@@ -60,6 +71,15 @@ void do_trace_rcu_torture_read(const char *rcutorturename,
 			       unsigned long c_old,
 			       unsigned long c);
 #else
+static inline void rcutorture_get_gp_data(enum rcutorture_type test_type,
+					  int *flags,
+					  unsigned long *gpnum,
+					  unsigned long *completed)
+{
+	*flags = 0;
+	*gpnum = 0;
+	*completed = 0;
+}
 static inline void rcutorture_record_test_transition(void)
 {
 }
@@ -228,6 +248,18 @@ void rcu_idle_exit(void);
 void rcu_irq_enter(void);
 void rcu_irq_exit(void);
 
+#ifdef CONFIG_RCU_STALL_COMMON
+void rcu_sysrq_start(void);
+void rcu_sysrq_end(void);
+#else /* #ifdef CONFIG_RCU_STALL_COMMON */
+static inline void rcu_sysrq_start(void)
+{
+}
+static inline void rcu_sysrq_end(void)
+{
+}
+#endif /* #else #ifdef CONFIG_RCU_STALL_COMMON */
+
 #ifdef CONFIG_RCU_USER_QS
 void rcu_user_enter(void);
 void rcu_user_exit(void);
@@ -268,6 +300,41 @@ bool __rcu_is_watching(void);
 #endif /* #if defined(CONFIG_DEBUG_LOCK_ALLOC) || defined(CONFIG_RCU_TRACE) || defined(CONFIG_SMP) */
 
 /*
+ * Hooks for cond_resched() and friends to avoid RCU CPU stall warnings.
+ */
+
+#define RCU_COND_RESCHED_LIM 256	/* ms vs. 100s of ms. */
+DECLARE_PER_CPU(int, rcu_cond_resched_count);
+void rcu_resched(void);
+
+/*
+ * Is it time to report RCU quiescent states?
+ *
+ * Note unsynchronized access to rcu_cond_resched_count.  Yes, we might
+ * increment some random CPU's count, and possibly also load the result from
+ * yet another CPU's count.  We might even clobber some other CPU's attempt
+ * to zero its counter.  This is all OK because the goal is not precision,
+ * but rather reasonable amortization of rcu_note_context_switch() overhead
+ * and extremely high probability of avoiding RCU CPU stall warnings.
+ * Note that this function has to be preempted in just the wrong place,
+ * many thousands of times in a row, for anything bad to happen.
+ */
+static inline bool rcu_should_resched(void)
+{
+	return raw_cpu_inc_return(rcu_cond_resched_count) >=
+	       RCU_COND_RESCHED_LIM;
+}
+
+/*
+ * Report quiscent states to RCU if it is time to do so.
+ */
+static inline void rcu_cond_resched(void)
+{
+	if (unlikely(rcu_should_resched()))
+		rcu_resched();
+}
+
+/*
  * Infrastructure to implement the synchronize_() primitives in
  * TREE_RCU and rcu_barrier_() primitives in TINY_RCU.
  */
@@ -328,7 +395,7 @@ extern struct lockdep_map rcu_lock_map;
 extern struct lockdep_map rcu_bh_lock_map;
 extern struct lockdep_map rcu_sched_lock_map;
 extern struct lockdep_map rcu_callback_map;
-extern int debug_lockdep_rcu_enabled(void);
+int debug_lockdep_rcu_enabled(void);
 
 /**
  * rcu_read_lock_held() - might we be in RCU read-side critical section?
@@ -949,6 +1016,9 @@ static inline notrace void rcu_read_unlock_sched_notrace(void)
  * pointers, but you must use rcu_assign_pointer() to initialize the
  * external-to-structure pointer -after- you have completely initialized
  * the reader-accessible portions of the linked structure.
+ *
+ * Note that unlike rcu_assign_pointer(), RCU_INIT_POINTER() provides no
+ * ordering guarantees for either the CPU or the compiler.
  */
 #define RCU_INIT_POINTER(p, v) \
 	do { \
diff --git a/include/linux/rcutiny.h b/include/linux/rcutiny.h
index 425c659d54e..d40a6a45133 100644
--- a/include/linux/rcutiny.h
+++ b/include/linux/rcutiny.h
@@ -119,6 +119,10 @@ static inline void rcu_sched_force_quiescent_state(void)
 {
 }
 
+static inline void show_rcu_gp_kthreads(void)
+{
+}
+
 static inline void rcu_cpu_stall_reset(void)
 {
 }
diff --git a/include/linux/rcutree.h b/include/linux/rcutree.h
index a59ca05fd4e..3e2f5d43274 100644
--- a/include/linux/rcutree.h
+++ b/include/linux/rcutree.h
@@ -84,6 +84,7 @@ extern unsigned long rcutorture_vernum;
 long rcu_batches_completed(void);
 long rcu_batches_completed_bh(void);
 long rcu_batches_completed_sched(void);
+void show_rcu_gp_kthreads(void);
 
 void rcu_force_quiescent_state(void);
 void rcu_bh_force_quiescent_state(void);
diff --git a/include/linux/regmap.h b/include/linux/regmap.h
index 85691b9b4fa..7b0e4b425cd 100644
--- a/include/linux/regmap.h
+++ b/include/linux/regmap.h
@@ -276,6 +276,10 @@ typedef int (*regmap_hw_async_write)(void *context,
 typedef int (*regmap_hw_read)(void *context,
 			      const void *reg_buf, size_t reg_size,
 			      void *val_buf, size_t val_size);
+typedef int (*regmap_hw_reg_read)(void *context, unsigned int reg,
+				  unsigned int *val);
+typedef int (*regmap_hw_reg_write)(void *context, unsigned int reg,
+				   unsigned int val);
 typedef struct regmap_async *(*regmap_hw_async_alloc)(void);
 typedef void (*regmap_hw_free_context)(void *context);
 
@@ -309,7 +313,9 @@ struct regmap_bus {
 	regmap_hw_write write;
 	regmap_hw_gather_write gather_write;
 	regmap_hw_async_write async_write;
+	regmap_hw_reg_write reg_write;
 	regmap_hw_read read;
+	regmap_hw_reg_read reg_read;
 	regmap_hw_free_context free_context;
 	regmap_hw_async_alloc async_alloc;
 	u8 read_flag_mask;
diff --git a/include/linux/regulator/consumer.h b/include/linux/regulator/consumer.h
index e530681bea7..a2d9d81038d 100644
--- a/include/linux/regulator/consumer.h
+++ b/include/linux/regulator/consumer.h
@@ -151,11 +151,13 @@ int regulator_register_supply_alias(struct device *dev, const char *id,
 				    const char *alias_id);
 void regulator_unregister_supply_alias(struct device *dev, const char *id);
 
-int regulator_bulk_register_supply_alias(struct device *dev, const char **id,
+int regulator_bulk_register_supply_alias(struct device *dev,
+					 const char *const *id,
 					 struct device *alias_dev,
-					 const char **alias_id, int num_id);
+					 const char *const *alias_id,
+					 int num_id);
 void regulator_bulk_unregister_supply_alias(struct device *dev,
-					    const char **id, int num_id);
+					    const char * const *id, int num_id);
 
 int devm_regulator_register_supply_alias(struct device *dev, const char *id,
 					 struct device *alias_dev,
@@ -164,12 +166,12 @@ void devm_regulator_unregister_supply_alias(struct device *dev,
 					    const char *id);
 
 int devm_regulator_bulk_register_supply_alias(struct device *dev,
-					      const char **id,
+					      const char *const *id,
 					      struct device *alias_dev,
-					      const char **alias_id,
+					      const char *const *alias_id,
 					      int num_id);
 void devm_regulator_bulk_unregister_supply_alias(struct device *dev,
-						 const char **id,
+						 const char *const *id,
 						 int num_id);
 
 /* regulator output control and status */
@@ -258,14 +260,14 @@ regulator_get_exclusive(struct device *dev, const char *id)
 static inline struct regulator *__must_check
 regulator_get_optional(struct device *dev, const char *id)
 {
-	return NULL;
+	return ERR_PTR(-ENODEV);
 }
 
 
 static inline struct regulator *__must_check
 devm_regulator_get_optional(struct device *dev, const char *id)
 {
-	return NULL;
+	return ERR_PTR(-ENODEV);
 }
 
 static inline void regulator_put(struct regulator *regulator)
@@ -290,17 +292,17 @@ static inline void regulator_unregister_supply_alias(struct device *dev,
 }
 
 static inline int regulator_bulk_register_supply_alias(struct device *dev,
-						       const char **id,
-						       struct device *alias_dev,
-						       const char **alias_id,
-						       int num_id)
+						const char *const *id,
+						struct device *alias_dev,
+						const char * const *alias_id,
+						int num_id)
 {
 	return 0;
 }
 
 static inline void regulator_bulk_unregister_supply_alias(struct device *dev,
-							  const char **id,
-							  int num_id)
+						const char * const *id,
+						int num_id)
 {
 }
 
@@ -317,15 +319,17 @@ static inline void devm_regulator_unregister_supply_alias(struct device *dev,
 {
 }
 
-static inline int devm_regulator_bulk_register_supply_alias(
-		struct device *dev, const char **id, struct device *alias_dev,
-		const char **alias_id, int num_id)
+static inline int devm_regulator_bulk_register_supply_alias(struct device *dev,
+						const char *const *id,
+						struct device *alias_dev,
+						const char *const *alias_id,
+						int num_id)
 {
 	return 0;
 }
 
 static inline void devm_regulator_bulk_unregister_supply_alias(
-		struct device *dev, const char **id, int num_id)
+	struct device *dev, const char *const *id, int num_id)
 {
 }
 
@@ -397,6 +401,12 @@ static inline int regulator_set_voltage(struct regulator *regulator,
 	return 0;
 }
 
+static inline int regulator_set_voltage_time(struct regulator *regulator,
+					     int old_uV, int new_uV)
+{
+	return 0;
+}
+
 static inline int regulator_get_voltage(struct regulator *regulator)
 {
 	return -EINVAL;
diff --git a/include/linux/reset.h b/include/linux/reset.h
index c0eda5023d7..349f150ae12 100644
--- a/include/linux/reset.h
+++ b/include/linux/reset.h
@@ -2,6 +2,7 @@
 #define _LINUX_RESET_H_
 
 struct device;
+struct device_node;
 struct reset_control;
 
 #ifdef CONFIG_RESET_CONTROLLER
@@ -33,6 +34,9 @@ static inline struct reset_control *devm_reset_control_get_optional(
 	return devm_reset_control_get(dev, id);
 }
 
+struct reset_control *of_reset_control_get(struct device_node *node,
+					   const char *id);
+
 #else
 
 static inline int reset_control_reset(struct reset_control *rstc)
@@ -75,6 +79,12 @@ static inline struct reset_control *devm_reset_control_get_optional(
 	return ERR_PTR(-ENOSYS);
 }
 
+static inline struct reset_control *of_reset_control_get(
+				struct device_node *node, const char *id)
+{
+	return ERR_PTR(-ENOSYS);
+}
+
 #endif /* CONFIG_RESET_CONTROLLER */
 
 #endif
diff --git a/include/linux/rtnetlink.h b/include/linux/rtnetlink.h
index 8e3e66ac0a5..953937ea523 100644
--- a/include/linux/rtnetlink.h
+++ b/include/linux/rtnetlink.h
@@ -4,6 +4,7 @@
 
 #include <linux/mutex.h>
 #include <linux/netdevice.h>
+#include <linux/wait.h>
 #include <uapi/linux/rtnetlink.h>
 
 extern int rtnetlink_send(struct sk_buff *skb, struct net *net, u32 pid, u32 group, int echo);
@@ -22,6 +23,10 @@ extern void rtnl_lock(void);
 extern void rtnl_unlock(void);
 extern int rtnl_trylock(void);
 extern int rtnl_is_locked(void);
+
+extern wait_queue_head_t netdev_unregistering_wq;
+extern struct mutex net_mutex;
+
 #ifdef CONFIG_PROVE_LOCKING
 extern int lockdep_rtnl_is_held(void);
 #else
diff --git a/include/linux/sched.h b/include/linux/sched.h
index 010cde3b44c..4dce5d844b7 100644
--- a/include/linux/sched.h
+++ b/include/linux/sched.h
@@ -220,7 +220,7 @@ print_cfs_rq(struct seq_file *m, int cpu, struct cfs_rq *cfs_rq);
 #define TASK_PARKED		512
 #define TASK_STATE_MAX		1024
 
-#define TASK_STATE_TO_CHAR_STR "RSDTtZXxKWP"
+#define TASK_STATE_TO_CHAR_STR "RSDTtXZxKWP"
 
 extern char ___assert_task_state[1 - 2*!!(
 		sizeof(TASK_STATE_TO_CHAR_STR)-1 != ilog2(TASK_STATE_MAX)+1)];
@@ -1153,9 +1153,12 @@ struct sched_dl_entity {
 	 *
 	 * @dl_boosted tells if we are boosted due to DI. If so we are
 	 * outside bandwidth enforcement mechanism (but only until we
-	 * exit the critical section).
+	 * exit the critical section);
+	 *
+	 * @dl_yielded tells if task gave up the cpu before consuming
+	 * all its available runtime during the last job.
 	 */
-	int dl_throttled, dl_new, dl_boosted;
+	int dl_throttled, dl_new, dl_boosted, dl_yielded;
 
 	/*
 	 * Bandwidth enforcement timer. Each -deadline task has its
diff --git a/include/linux/serial_core.h b/include/linux/serial_core.h
index f729be981da..7a15b5b24c0 100644
--- a/include/linux/serial_core.h
+++ b/include/linux/serial_core.h
@@ -285,6 +285,22 @@ static inline int uart_poll_timeout(struct uart_port *port)
 /*
  * Console helpers.
  */
+struct earlycon_device {
+	struct console *con;
+	struct uart_port port;
+	char options[16];		/* e.g., 115200n8 */
+	unsigned int baud;
+};
+int setup_earlycon(char *buf, const char *match,
+		   int (*setup)(struct earlycon_device *, const char *));
+
+#define EARLYCON_DECLARE(name, func) \
+static int __init name ## _setup_earlycon(char *buf) \
+{ \
+	return setup_earlycon(buf, __stringify(name), func); \
+} \
+early_param("earlycon", name ## _setup_earlycon);
+
 struct uart_port *uart_get_console(struct uart_port *ports, int nr,
 				   struct console *c);
 void uart_parse_options(char *options, int *baud, int *parity, int *bits,
diff --git a/include/linux/serio.h b/include/linux/serio.h
index 36aac733840..9f779c7a2da 100644
--- a/include/linux/serio.h
+++ b/include/linux/serio.h
@@ -23,6 +23,7 @@ struct serio {
 
 	char name[32];
 	char phys[32];
+	char firmware_id[128];
 
 	bool manual_bind;
 
diff --git a/include/linux/sh_timer.h b/include/linux/sh_timer.h
index 4d9dcd13831..8e1e036d6d4 100644
--- a/include/linux/sh_timer.h
+++ b/include/linux/sh_timer.h
@@ -7,6 +7,7 @@ struct sh_timer_config {
 	int timer_bit;
 	unsigned long clockevent_rating;
 	unsigned long clocksource_rating;
+	unsigned int channels_mask;
 };
 
 #endif /* __SH_TIMER_H__ */
diff --git a/include/linux/slub_def.h b/include/linux/slub_def.h
index f2f7398848c..d82abd40a3c 100644
--- a/include/linux/slub_def.h
+++ b/include/linux/slub_def.h
@@ -101,4 +101,13 @@ struct kmem_cache {
 	struct kmem_cache_node *node[MAX_NUMNODES];
 };
 
+#ifdef CONFIG_SYSFS
+#define SLAB_SUPPORTS_SYSFS
+void sysfs_slab_remove(struct kmem_cache *);
+#else
+static inline void sysfs_slab_remove(struct kmem_cache *s)
+{
+}
+#endif
+
 #endif /* _LINUX_SLUB_DEF_H */
diff --git a/include/linux/sock_diag.h b/include/linux/sock_diag.h
index 54f91d35e5f..46cca4c0684 100644
--- a/include/linux/sock_diag.h
+++ b/include/linux/sock_diag.h
@@ -23,7 +23,7 @@ int sock_diag_check_cookie(void *sk, __u32 *cookie);
 void sock_diag_save_cookie(void *sk, __u32 *cookie);
 
 int sock_diag_put_meminfo(struct sock *sk, struct sk_buff *skb, int attr);
-int sock_diag_put_filterinfo(struct user_namespace *user_ns, struct sock *sk,
+int sock_diag_put_filterinfo(bool may_report_filterinfo, struct sock *sk,
 			     struct sk_buff *skb, int attrtype);
 
 #endif
diff --git a/arch/blackfin/include/asm/bfin_spi3.h b/include/linux/spi/adi_spi3.h
index 0957e65a54b..c84123aa1d0 100644
--- a/arch/blackfin/include/asm/bfin_spi3.h
+++ b/include/linux/spi/adi_spi3.h
@@ -1,7 +1,7 @@
 /*
  * Analog Devices SPI3 controller driver
  *
- * Copyright (c) 2011 Analog Devices Inc.
+ * Copyright (c) 2014 Analog Devices 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
@@ -11,14 +11,10 @@
  * 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 _SPI_CHANNEL_H_
-#define _SPI_CHANNEL_H_
+#ifndef _ADI_SPI3_H_
+#define _ADI_SPI3_H_
 
 #include <linux/types.h>
 
@@ -209,9 +205,9 @@
 #define SPI_ILAT_CLR_TFI            0x00000800    /* Transmit Finish Indication */
 
 /*
- * bfin spi3 registers layout
+ * adi spi3 registers layout
  */
-struct bfin_spi_regs {
+struct adi_spi_regs {
 	u32 revid;
 	u32 control;
 	u32 rx_control;
@@ -240,7 +236,7 @@ struct bfin_spi_regs {
 #define MAX_CTRL_CS          8  /* cs in spi controller */
 
 /* device.platform_data for SSP controller devices */
-struct bfin_spi3_master {
+struct adi_spi3_master {
 	u16 num_chipselect;
 	u16 pin_req[7];
 };
@@ -248,11 +244,11 @@ struct bfin_spi3_master {
 /* spi_board_info.controller_data for SPI slave devices,
  * copied to spi_device.platform_data ... mostly for dma tuning
  */
-struct bfin_spi3_chip {
+struct adi_spi3_chip {
 	u32 control;
 	u16 cs_chg_udelay; /* Some devices require 16-bit delays */
 	u32 tx_dummy_val; /* tx value for rx only transfer */
 	bool enable_dma;
 };
 
-#endif /* _SPI_CHANNEL_H_ */
+#endif /* _ADI_SPI3_H_ */
diff --git a/include/linux/spi/rspi.h b/include/linux/spi/rspi.h
index a25bd6f65e7..e546b2ceb62 100644
--- a/include/linux/spi/rspi.h
+++ b/include/linux/spi/rspi.h
@@ -25,8 +25,6 @@ struct rspi_plat_data {
 	unsigned int dma_tx_id;
 	unsigned int dma_rx_id;
 
-	unsigned dma_width_16bit:1;	/* DMAC read/write width = 16-bit */
-
 	u16 num_chipselect;
 };
 
diff --git a/include/linux/sysfs.h b/include/linux/sysfs.h
index 084354b0e81..f97d0dbb59f 100644
--- a/include/linux/sysfs.h
+++ b/include/linux/sysfs.h
@@ -179,9 +179,6 @@ struct sysfs_ops {
 
 #ifdef CONFIG_SYSFS
 
-int sysfs_schedule_callback(struct kobject *kobj, void (*func)(void *),
-			    void *data, struct module *owner);
-
 int __must_check sysfs_create_dir_ns(struct kobject *kobj, const void *ns);
 void sysfs_remove_dir(struct kobject *kobj);
 int __must_check sysfs_rename_dir_ns(struct kobject *kobj, const char *new_name,
@@ -255,12 +252,6 @@ static inline void sysfs_enable_ns(struct kernfs_node *kn)
 
 #else /* CONFIG_SYSFS */
 
-static inline int sysfs_schedule_callback(struct kobject *kobj,
-		void (*func)(void *), void *data, struct module *owner)
-{
-	return -ENOSYS;
-}
-
 static inline int sysfs_create_dir_ns(struct kobject *kobj, const void *ns)
 {
 	return 0;
@@ -446,7 +437,7 @@ static inline int __must_check sysfs_create_file(struct kobject *kobj,
 static inline void sysfs_remove_file(struct kobject *kobj,
 				     const struct attribute *attr)
 {
-	return sysfs_remove_file_ns(kobj, attr, NULL);
+	sysfs_remove_file_ns(kobj, attr, NULL);
 }
 
 static inline int sysfs_rename_link(struct kobject *kobj, struct kobject *target,
diff --git a/include/linux/torture.h b/include/linux/torture.h
index b2e2b468e51..5ca58fcbaf1 100644
--- a/include/linux/torture.h
+++ b/include/linux/torture.h
@@ -49,12 +49,6 @@
 #define VERBOSE_TOROUT_ERRSTRING(s) \
 	do { if (verbose) pr_alert("%s" TORTURE_FLAG "!!! %s\n", torture_type, s); } while (0)
 
-/* Definitions for a non-string torture-test module parameter. */
-#define torture_parm(type, name, init, msg) \
-	static type name = init; \
-	module_param(name, type, 0444); \
-	MODULE_PARM_DESC(name, msg);
-
 /* Definitions for online/offline exerciser. */
 int torture_onoff_init(long ooholdoff, long oointerval);
 char *torture_onoff_stats(char *page);
@@ -81,7 +75,7 @@ void stutter_wait(const char *title);
 int torture_stutter_init(int s);
 
 /* Initialization and cleanup. */
-void torture_init_begin(char *ttype, bool v, int *runnable);
+bool torture_init_begin(char *ttype, bool v, int *runnable);
 void torture_init_end(void);
 bool torture_cleanup(void);
 bool torture_must_stop(void);
diff --git a/include/linux/tty_ldisc.h b/include/linux/tty_ldisc.h
index add26da2fae..00c9d688d7b 100644
--- a/include/linux/tty_ldisc.h
+++ b/include/linux/tty_ldisc.h
@@ -92,7 +92,10 @@
  *	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.
+ *	not have any more data to send, it can just return. If the line
+ *	discipline does have some data to send, please arise a tasklet
+ *	or workqueue to do the real data transfer. Do not send data in
+ *	this hook, it may leads to a deadlock.
  *
  * int (*hangup)(struct tty_struct *)
  *
diff --git a/include/linux/types.h b/include/linux/types.h
index 4d118ba1134..a0bb7048687 100644
--- a/include/linux/types.h
+++ b/include/linux/types.h
@@ -142,6 +142,7 @@ typedef unsigned long blkcnt_t;
 #define pgoff_t unsigned long
 #endif
 
+/* A dma_addr_t can hold any valid DMA or bus address for the platform */
 #ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
 typedef u64 dma_addr_t;
 #else
diff --git a/include/linux/usb.h b/include/linux/usb.h
index 6b7ec376fb4..d2465bc0e73 100644
--- a/include/linux/usb.h
+++ b/include/linux/usb.h
@@ -352,6 +352,8 @@ struct usb_bus {
 	struct usb_bus *hs_companion;	/* Companion EHCI bus, if any */
 	struct list_head bus_list;	/* list of busses */
 
+	struct mutex usb_address0_mutex; /* unaddressed device mutex */
+
 	int bandwidth_allocated;	/* on this bus: how much of the time
 					 * reserved for periodic (intr/iso)
 					 * requests is used, on average?
diff --git a/include/linux/usb/composite.h b/include/linux/usb/composite.h
index d3ca3b53837..7373203140e 100644
--- a/include/linux/usb/composite.h
+++ b/include/linux/usb/composite.h
@@ -57,6 +57,61 @@
 struct usb_configuration;
 
 /**
+ * struct usb_os_desc_ext_prop - describes one "Extended Property"
+ * @entry: used to keep a list of extended properties
+ * @type: Extended Property type
+ * @name_len: Extended Property unicode name length, including terminating '\0'
+ * @name: Extended Property name
+ * @data_len: Length of Extended Property blob (for unicode store double len)
+ * @data: Extended Property blob
+ * @item: Represents this Extended Property in configfs
+ */
+struct usb_os_desc_ext_prop {
+	struct list_head	entry;
+	u8			type;
+	int			name_len;
+	char			*name;
+	int			data_len;
+	char			*data;
+	struct config_item	item;
+};
+
+/**
+ * struct usb_os_desc - describes OS descriptors associated with one interface
+ * @ext_compat_id: 16 bytes of "Compatible ID" and "Subcompatible ID"
+ * @ext_prop: Extended Properties list
+ * @ext_prop_len: Total length of Extended Properties blobs
+ * @ext_prop_count: Number of Extended Properties
+ * @opts_mutex: Optional mutex protecting config data of a usb_function_instance
+ * @group: Represents OS descriptors associated with an interface in configfs
+ * @owner: Module associated with this OS descriptor
+ */
+struct usb_os_desc {
+	char			*ext_compat_id;
+	struct list_head	ext_prop;
+	int			ext_prop_len;
+	int			ext_prop_count;
+	struct mutex		*opts_mutex;
+	struct config_group	group;
+	struct module		*owner;
+};
+
+/**
+ * struct usb_os_desc_table - describes OS descriptors associated with one
+ * interface of a usb_function
+ * @if_id: Interface id
+ * @os_desc: "Extended Compatibility ID" and "Extended Properties" of the
+ *	interface
+ *
+ * Each interface can have at most one "Extended Compatibility ID" and a
+ * number of "Extended Properties".
+ */
+struct usb_os_desc_table {
+	int			if_id;
+	struct usb_os_desc	*os_desc;
+};
+
+/**
  * 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()
@@ -73,6 +128,10 @@ struct usb_configuration;
  *	be available at super speed.
  * @config: assigned when @usb_add_function() is called; this is the
  *	configuration with which this function is associated.
+ * @os_desc_table: Table of (interface id, os descriptors) pairs. The function
+ *	can expose more than one interface. If an interface is a member of
+ *	an IAD, only the first interface of IAD has its entry in the table.
+ * @os_desc_n: Number of entries in os_desc_table
  * @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.
@@ -129,6 +188,9 @@ struct usb_function {
 
 	struct usb_configuration	*config;
 
+	struct usb_os_desc_table	*os_desc_table;
+	unsigned			os_desc_n;
+
 	/* REVISIT:  bind() functions can be marked __init, which
 	 * makes trouble for section mismatch analysis.  See if
 	 * we can't restructure things to avoid mismatching.
@@ -327,6 +389,8 @@ extern void usb_composite_unregister(struct usb_composite_driver *driver);
 extern void usb_composite_setup_continue(struct usb_composite_dev *cdev);
 extern int composite_dev_prepare(struct usb_composite_driver *composite,
 		struct usb_composite_dev *cdev);
+extern int composite_os_desc_req_prepare(struct usb_composite_dev *cdev,
+					 struct usb_ep *ep0);
 void composite_dev_cleanup(struct usb_composite_dev *cdev);
 
 static inline struct usb_composite_driver *to_cdriver(
@@ -335,11 +399,19 @@ static inline struct usb_composite_driver *to_cdriver(
 	return container_of(gdrv, struct usb_composite_driver, gadget_driver);
 }
 
+#define OS_STRING_QW_SIGN_LEN		14
+#define OS_STRING_IDX			0xEE
+
 /**
  * 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
+ * @os_desc_req: used for OS descriptors responses; buffer is pre-allocated
  * @config: the currently active configuration
+ * @qw_sign: qwSignature part of the OS string
+ * @b_vendor_code: bMS_VendorCode part of the OS string
+ * @use_os_string: false by default, interested gadgets set it
+ * @os_desc_config: the configuration to be used with OS descriptors
  *
  * One of these devices is allocated and initialized before the
  * associated device driver's bind() is called.
@@ -369,9 +441,16 @@ static inline struct usb_composite_driver *to_cdriver(
 struct usb_composite_dev {
 	struct usb_gadget		*gadget;
 	struct usb_request		*req;
+	struct usb_request		*os_desc_req;
 
 	struct usb_configuration	*config;
 
+	/* OS String is a custom (yet popular) extension to the USB standard. */
+	u8				qw_sign[OS_STRING_QW_SIGN_LEN];
+	u8				b_vendor_code;
+	struct usb_configuration	*os_desc_config;
+	unsigned int			use_os_string:1;
+
 	/* private: */
 	/* internals */
 	unsigned int			suspended:1;
diff --git a/include/linux/usb/msm_hsusb.h b/include/linux/usb/msm_hsusb.h
index 32754835a39..b0a39243295 100644
--- a/include/linux/usb/msm_hsusb.h
+++ b/include/linux/usb/msm_hsusb.h
@@ -23,21 +23,6 @@
 #include <linux/clk.h>
 
 /**
- * Supported USB modes
- *
- * USB_PERIPHERAL       Only peripheral mode is supported.
- * USB_HOST             Only host mode is supported.
- * USB_OTG              OTG mode is supported.
- *
- */
-enum usb_mode_type {
-	USB_NONE = 0,
-	USB_PERIPHERAL,
-	USB_HOST,
-	USB_OTG,
-};
-
-/**
  * OTG control
  *
  * OTG_NO_CONTROL	Id/VBUS notifications not required. Useful in host
@@ -115,27 +100,23 @@ enum usb_chg_type {
 /**
  * struct msm_otg_platform_data - platform device data
  *              for msm_otg driver.
- * @phy_init_seq: PHY configuration sequence. val, reg pairs
- *              terminated by -1.
+ * @phy_init_seq: PHY configuration sequence values. Value of -1 is reserved as
+ *              "do not overwrite default vaule at this address".
+ * @phy_init_sz: PHY configuration sequence size.
  * @vbus_power: VBUS power on/off routine.
  * @power_budget: VBUS power budget in mA (0 will be treated as 500mA).
  * @mode: Supported mode (OTG/peripheral/host).
  * @otg_control: OTG switch controlled by user/Id pin
- * @default_mode: Default operational mode. Applicable only if
- *              OTG switch is controller by user.
- * @pclk_src_name: pclk is derived from ebi1_usb_clk in case of 7x27 and 8k
- *              dfab_usb_hs_clk in case of 8660 and 8960.
  */
 struct msm_otg_platform_data {
 	int *phy_init_seq;
+	int phy_init_sz;
 	void (*vbus_power)(bool on);
 	unsigned power_budget;
-	enum usb_mode_type mode;
+	enum usb_dr_mode mode;
 	enum otg_control_type otg_control;
-	enum usb_mode_type default_mode;
 	enum msm_usb_phy_type phy_type;
 	void (*setup_gpio)(enum usb_otg_state state);
-	char *pclk_src_name;
 	int (*link_clk_reset)(struct clk *link_clk, bool assert);
 	int (*phy_clk_reset)(struct clk *phy_clk);
 };
@@ -147,7 +128,6 @@ struct msm_otg_platform_data {
  * @irq: IRQ number assigned for HSUSB controller.
  * @clk: clock struct of usb_hs_clk.
  * @pclk: clock struct of usb_hs_pclk.
- * @pclk_src: pclk source for voting.
  * @phy_reset_clk: clock struct of usb_phy_clk.
  * @core_clk: clock struct of usb_hs_core_clk.
  * @regs: ioremapped register base address.
@@ -168,7 +148,6 @@ struct msm_otg {
 	int irq;
 	struct clk *clk;
 	struct clk *pclk;
-	struct clk *pclk_src;
 	struct clk *phy_reset_clk;
 	struct clk *core_clk;
 	void __iomem *regs;
@@ -179,10 +158,18 @@ struct msm_otg {
 	atomic_t in_lpm;
 	int async_int;
 	unsigned cur_power;
+	int phy_number;
 	struct delayed_work chg_work;
 	enum usb_chg_state chg_state;
 	enum usb_chg_type chg_type;
 	u8 dcd_retries;
+	struct regulator *v3p3;
+	struct regulator *v1p8;
+	struct regulator *vddcx;
+
+	struct reset_control *phy_rst;
+	struct reset_control *link_rst;
+	int vdd_levels[3];
 };
 
 #endif
diff --git a/include/linux/usb/msm_hsusb_hw.h b/include/linux/usb/msm_hsusb_hw.h
index 6e97a2d3d39..a29f6030afb 100644
--- a/include/linux/usb/msm_hsusb_hw.h
+++ b/include/linux/usb/msm_hsusb_hw.h
@@ -16,6 +16,9 @@
 #ifndef __LINUX_USB_GADGET_MSM72K_UDC_H__
 #define __LINUX_USB_GADGET_MSM72K_UDC_H__
 
+/* USB phy selector - in TCSR address range */
+#define USB2_PHY_SEL         0xfd4ab000
+
 #define USB_AHBBURST         (MSM_USB_BASE + 0x0090)
 #define USB_AHBMODE          (MSM_USB_BASE + 0x0098)
 #define USB_CAPLENGTH        (MSM_USB_BASE + 0x0100) /* 8 bit */
@@ -25,13 +28,15 @@
 #define USB_OTGSC            (MSM_USB_BASE + 0x01A4)
 #define USB_USBMODE          (MSM_USB_BASE + 0x01A8)
 #define USB_PHY_CTRL         (MSM_USB_BASE + 0x0240)
+#define USB_PHY_CTRL2        (MSM_USB_BASE + 0x0278)
 
 #define USBCMD_RESET   2
 #define USB_USBINTR          (MSM_USB_BASE + 0x0148)
 
 #define PORTSC_PHCD            (1 << 23) /* phy suspend mode */
-#define PORTSC_PTS_MASK         (3 << 30)
-#define PORTSC_PTS_ULPI         (3 << 30)
+#define PORTSC_PTS_MASK        (3 << 30)
+#define PORTSC_PTS_ULPI        (2 << 30)
+#define PORTSC_PTS_SERIAL      (3 << 30)
 
 #define USB_ULPI_VIEWPORT    (MSM_USB_BASE + 0x0170)
 #define ULPI_RUN              (1 << 30)
@@ -41,9 +46,14 @@
 #define ULPI_DATA(n)          ((n) & 255)
 #define ULPI_DATA_READ(n)     (((n) >> 8) & 255)
 
+/* synopsys 28nm phy registers */
+#define ULPI_PWR_CLK_MNG_REG	0x88
+#define OTG_COMP_DISABLE	BIT(0)
+
 #define ASYNC_INTR_CTRL         (1 << 29) /* Enable async interrupt */
 #define ULPI_STP_CTRL           (1 << 30) /* Block communication with PHY */
 #define PHY_RETEN               (1 << 1) /* PHY retention enable/disable */
+#define PHY_POR_ASSERT		(1 << 0) /* USB2 28nm PHY POR ASSERT */
 
 /* OTG definitions */
 #define OTGSC_INTSTS_MASK	(0x7f << 16)
diff --git a/include/linux/usb/usb_phy_gen_xceiv.h b/include/linux/usb/usb_phy_generic.h
index cc8d818a83b..68adae83aff 100644
--- a/include/linux/usb/usb_phy_gen_xceiv.h
+++ b/include/linux/usb/usb_phy_generic.h
@@ -3,7 +3,7 @@
 
 #include <linux/usb/otg.h>
 
-struct usb_phy_gen_xceiv_platform_data {
+struct usb_phy_generic_platform_data {
 	enum usb_phy_type type;
 	unsigned long clk_rate;
 
@@ -13,16 +13,17 @@ struct usb_phy_gen_xceiv_platform_data {
 	int gpio_reset;
 };
 
-#if defined(CONFIG_NOP_USB_XCEIV) || (defined(CONFIG_NOP_USB_XCEIV_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_NOP_USB_XCEIV)
 /* sometimes transceivers are accessed only through e.g. ULPI */
-extern void usb_nop_xceiv_register(void);
-extern void usb_nop_xceiv_unregister(void);
+extern struct platform_device *usb_phy_generic_register(void);
+extern void usb_phy_generic_unregister(struct platform_device *);
 #else
-static inline void usb_nop_xceiv_register(void)
+static inline struct platform_device *usb_phy_generic_register(void)
 {
+	return NULL;
 }
 
-static inline void usb_nop_xceiv_unregister(void)
+static inline void usb_phy_generic_unregister(struct platform_device *pdev)
 {
 }
 #endif
diff --git a/include/linux/vexpress.h b/include/linux/vexpress.h
index 617c01b8f74..a4c9547aae6 100644
--- a/include/linux/vexpress.h
+++ b/include/linux/vexpress.h
@@ -15,28 +15,15 @@
 #define _LINUX_VEXPRESS_H
 
 #include <linux/device.h>
+#include <linux/platform_device.h>
 #include <linux/reboot.h>
+#include <linux/regmap.h>
 
 #define VEXPRESS_SITE_MB		0
 #define VEXPRESS_SITE_DB1		1
 #define VEXPRESS_SITE_DB2		2
 #define VEXPRESS_SITE_MASTER		0xf
 
-#define VEXPRESS_CONFIG_STATUS_DONE	0
-#define VEXPRESS_CONFIG_STATUS_WAIT	1
-
-#define VEXPRESS_GPIO_MMC_CARDIN	0
-#define VEXPRESS_GPIO_MMC_WPROT		1
-#define VEXPRESS_GPIO_FLASH_WPn		2
-#define VEXPRESS_GPIO_LED0		3
-#define VEXPRESS_GPIO_LED1		4
-#define VEXPRESS_GPIO_LED2		5
-#define VEXPRESS_GPIO_LED3		6
-#define VEXPRESS_GPIO_LED4		7
-#define VEXPRESS_GPIO_LED5		8
-#define VEXPRESS_GPIO_LED6		9
-#define VEXPRESS_GPIO_LED7		10
-
 #define VEXPRESS_RES_FUNC(_site, _func)	\
 {					\
 	.start = (_site),		\
@@ -44,84 +31,43 @@
 	.flags = IORESOURCE_BUS,	\
 }
 
-/* Config bridge API */
+/* Config infrastructure */
 
-/**
- * struct vexpress_config_bridge_info - description of the platform
- * configuration infrastructure bridge.
- *
- * @name:	Bridge name
- *
- * @func_get:	Obtains pointer to a configuration function for a given
- *		device or a Device Tree node, to be used with @func_put
- *		and @func_exec. The node pointer should take precedence
- *		over device pointer when both are passed.
- *
- * @func_put:	Tells the bridge that the function will not be used any
- *		more, so all allocated resources can be released.
- *
- * @func_exec:	Executes a configuration function read or write operation.
- *		The offset selects a 32 bit word of the value accessed.
- *		Must return VEXPRESS_CONFIG_STATUS_DONE when operation
- *		is finished immediately, VEXPRESS_CONFIG_STATUS_WAIT when
- *		will be completed in some time or negative value in case
- *		of error.
- */
-struct vexpress_config_bridge_info {
-	const char *name;
-	void *(*func_get)(struct device *dev, struct device_node *node);
-	void (*func_put)(void *func);
-	int (*func_exec)(void *func, int offset, bool write, u32 *data);
-};
+void vexpress_config_set_master(u32 site);
+u32 vexpress_config_get_master(void);
 
-struct vexpress_config_bridge;
+void vexpress_config_lock(void *arg);
+void vexpress_config_unlock(void *arg);
 
-struct vexpress_config_bridge *vexpress_config_bridge_register(
-		struct device_node *node,
-		struct vexpress_config_bridge_info *info);
-void vexpress_config_bridge_unregister(struct vexpress_config_bridge *bridge);
+int vexpress_config_get_topo(struct device_node *node, u32 *site,
+		u32 *position, u32 *dcc);
 
-void vexpress_config_complete(struct vexpress_config_bridge *bridge,
-		int status);
+/* Config bridge API */
 
-/* Config function API */
+struct vexpress_config_bridge_ops {
+	struct regmap * (*regmap_init)(struct device *dev, void *context);
+	void (*regmap_exit)(struct regmap *regmap, void *context);
+};
 
-struct vexpress_config_func;
+struct device *vexpress_config_bridge_register(struct device *parent,
+		struct vexpress_config_bridge_ops *ops, void *context);
 
-struct vexpress_config_func *__vexpress_config_func_get(struct device *dev,
-		struct device_node *node);
-#define vexpress_config_func_get_by_dev(dev) \
-		__vexpress_config_func_get(dev, NULL)
-#define vexpress_config_func_get_by_node(node) \
-		__vexpress_config_func_get(NULL, node)
-void vexpress_config_func_put(struct vexpress_config_func *func);
+/* Config regmap API */
 
-/* Both may sleep! */
-int vexpress_config_read(struct vexpress_config_func *func, int offset,
-		u32 *data);
-int vexpress_config_write(struct vexpress_config_func *func, int offset,
-		u32 data);
+struct regmap *devm_regmap_init_vexpress_config(struct device *dev);
 
 /* Platform control */
 
+unsigned int vexpress_get_mci_cardin(struct device *dev);
 u32 vexpress_get_procid(int site);
-u32 vexpress_get_hbi(int site);
 void *vexpress_get_24mhz_clock_base(void);
 void vexpress_flags_set(u32 data);
 
-#define vexpress_get_site_by_node(node) __vexpress_get_site(NULL, node)
-#define vexpress_get_site_by_dev(dev) __vexpress_get_site(dev, NULL)
-unsigned __vexpress_get_site(struct device *dev, struct device_node *node);
-
 void vexpress_sysreg_early_init(void __iomem *base);
-void vexpress_sysreg_of_early_init(void);
+int vexpress_syscfg_device_register(struct platform_device *pdev);
 
 /* Clocks */
 
-struct clk *vexpress_osc_setup(struct device *dev);
-void vexpress_osc_of_setup(struct device_node *node);
-
 void vexpress_clk_init(void __iomem *sp810_base);
-void vexpress_clk_of_init(void);
 
 #endif
diff --git a/include/linux/wait.h b/include/linux/wait.h
index e7d9d9ed14f..bd68819f081 100644
--- a/include/linux/wait.h
+++ b/include/linux/wait.h
@@ -191,11 +191,23 @@ wait_queue_head_t *bit_waitqueue(void *, int);
 	(!__builtin_constant_p(state) ||				\
 		state == TASK_INTERRUPTIBLE || state == TASK_KILLABLE)	\
 
+/*
+ * The below macro ___wait_event() has an explicit shadow of the __ret
+ * variable when used from the wait_event_*() macros.
+ *
+ * This is so that both can use the ___wait_cond_timeout() construct
+ * to wrap the condition.
+ *
+ * The type inconsistency of the wait_event_*() __ret variable is also
+ * on purpose; we use long where we can return timeout values and int
+ * otherwise.
+ */
+
 #define ___wait_event(wq, condition, state, exclusive, ret, cmd)	\
 ({									\
 	__label__ __out;						\
 	wait_queue_t __wait;						\
-	long __ret = ret;						\
+	long __ret = ret;	/* explicit shadow */			\
 									\
 	INIT_LIST_HEAD(&__wait.task_list);				\
 	if (exclusive)							\
diff --git a/include/net/af_vsock.h b/include/net/af_vsock.h
index 7d64d3609ec..42827786940 100644
--- a/include/net/af_vsock.h
+++ b/include/net/af_vsock.h
@@ -155,7 +155,11 @@ struct vsock_transport {
 
 /**** CORE ****/
 
-int vsock_core_init(const struct vsock_transport *t);
+int __vsock_core_init(const struct vsock_transport *t, struct module *owner);
+static inline int vsock_core_init(const struct vsock_transport *t)
+{
+	return __vsock_core_init(t, THIS_MODULE);
+}
 void vsock_core_exit(void);
 
 /**** UTILS ****/
diff --git a/include/net/cfg80211.h b/include/net/cfg80211.h
index f3539a15c41..f856e5a746f 100644
--- a/include/net/cfg80211.h
+++ b/include/net/cfg80211.h
@@ -3669,6 +3669,18 @@ void cfg80211_sched_scan_results(struct wiphy *wiphy);
 void cfg80211_sched_scan_stopped(struct wiphy *wiphy);
 
 /**
+ * cfg80211_sched_scan_stopped_rtnl - notify that the scheduled scan has stopped
+ *
+ * @wiphy: the wiphy on which the scheduled scan stopped
+ *
+ * The driver can call this function to inform cfg80211 that the
+ * scheduled scan had to be stopped, for whatever reason.  The driver
+ * is then called back via the sched_scan_stop operation when done.
+ * This function should be called with rtnl locked.
+ */
+void cfg80211_sched_scan_stopped_rtnl(struct wiphy *wiphy);
+
+/**
  * cfg80211_inform_bss_width_frame - inform cfg80211 of a received BSS frame
  *
  * @wiphy: the wiphy reporting the BSS
diff --git a/include/net/flow.h b/include/net/flow.h
index 64fd2483665..8109a159d1b 100644
--- a/include/net/flow.h
+++ b/include/net/flow.h
@@ -11,6 +11,14 @@
 #include <linux/in6.h>
 #include <linux/atomic.h>
 
+/*
+ * ifindex generation is per-net namespace, and loopback is
+ * always the 1st device in ns (see net_dev_init), thus any
+ * loopback device should get ifindex 1
+ */
+
+#define LOOPBACK_IFINDEX	1
+
 struct flowi_common {
 	int	flowic_oif;
 	int	flowic_iif;
@@ -80,7 +88,7 @@ static inline void flowi4_init_output(struct flowi4 *fl4, int oif,
 				      __be16 dport, __be16 sport)
 {
 	fl4->flowi4_oif = oif;
-	fl4->flowi4_iif = 0;
+	fl4->flowi4_iif = LOOPBACK_IFINDEX;
 	fl4->flowi4_mark = mark;
 	fl4->flowi4_tos = tos;
 	fl4->flowi4_scope = scope;
diff --git a/include/net/ip6_route.h b/include/net/ip6_route.h
index 6c4f5eac98e..216cecce65e 100644
--- a/include/net/ip6_route.h
+++ b/include/net/ip6_route.h
@@ -127,6 +127,7 @@ int rt6_dump_route(struct rt6_info *rt, void *p_arg);
 void rt6_ifdown(struct net *net, struct net_device *dev);
 void rt6_mtu_change(struct net_device *dev, unsigned int mtu);
 void rt6_remove_prefsrc(struct inet6_ifaddr *ifp);
+void rt6_clean_tohost(struct net *net, struct in6_addr *gateway);
 
 
 /*
diff --git a/include/net/net_namespace.h b/include/net/net_namespace.h
index 79387f73f87..5f9eb260990 100644
--- a/include/net/net_namespace.h
+++ b/include/net/net_namespace.h
@@ -9,6 +9,7 @@
 #include <linux/list.h>
 #include <linux/sysctl.h>
 
+#include <net/flow.h>
 #include <net/netns/core.h>
 #include <net/netns/mib.h>
 #include <net/netns/unix.h>
@@ -131,14 +132,6 @@ struct net {
 	atomic_t		fnhe_genid;
 };
 
-/*
- * ifindex generation is per-net namespace, and loopback is
- * always the 1st device in ns (see net_dev_init), thus any
- * loopback device should get ifindex 1
- */
-
-#define LOOPBACK_IFINDEX	1
-
 #include <linux/seq_file_net.h>
 
 /* Init's network namespace */
diff --git a/include/net/netns/ipv4.h b/include/net/netns/ipv4.h
index 80f500a2949..b2704fd0ec8 100644
--- a/include/net/netns/ipv4.h
+++ b/include/net/netns/ipv4.h
@@ -20,6 +20,11 @@ struct local_ports {
 	int		range[2];
 };
 
+struct ping_group_range {
+	seqlock_t	lock;
+	kgid_t		range[2];
+};
+
 struct netns_ipv4 {
 #ifdef CONFIG_SYSCTL
 	struct ctl_table_header	*forw_hdr;
@@ -66,13 +71,13 @@ struct netns_ipv4 {
 	int sysctl_icmp_ratemask;
 	int sysctl_icmp_errors_use_inbound_ifaddr;
 
-	struct local_ports sysctl_local_ports;
+	struct local_ports ip_local_ports;
 
 	int sysctl_tcp_ecn;
 	int sysctl_ip_no_pmtu_disc;
 	int sysctl_ip_fwd_use_pmtu;
 
-	kgid_t sysctl_ping_group_range[2];
+	struct ping_group_range ping_group_range;
 
 	atomic_t dev_addr_genid;
 
diff --git a/include/net/sctp/structs.h b/include/net/sctp/structs.h
index d992ca3145f..0dfcc92600e 100644
--- a/include/net/sctp/structs.h
+++ b/include/net/sctp/structs.h
@@ -1241,6 +1241,7 @@ struct sctp_endpoint {
 	/* SCTP-AUTH: endpoint shared keys */
 	struct list_head endpoint_shared_keys;
 	__u16 active_key_id;
+	__u8  auth_enable;
 };
 
 /* Recover the outter endpoint structure. */
@@ -1269,7 +1270,8 @@ struct sctp_endpoint *sctp_endpoint_is_match(struct sctp_endpoint *,
 int sctp_has_association(struct net *net, const union sctp_addr *laddr,
 			 const union sctp_addr *paddr);
 
-int sctp_verify_init(struct net *net, const struct sctp_association *asoc,
+int sctp_verify_init(struct net *net, const struct sctp_endpoint *ep,
+		     const struct sctp_association *asoc,
 		     sctp_cid_t, sctp_init_chunk_t *peer_init,
 		     struct sctp_chunk *chunk, struct sctp_chunk **err_chunk);
 int sctp_process_init(struct sctp_association *, struct sctp_chunk *chunk,
diff --git a/include/net/sock.h b/include/net/sock.h
index 8338a14e480..21569cf456e 100644
--- a/include/net/sock.h
+++ b/include/net/sock.h
@@ -2255,6 +2255,11 @@ int sock_get_timestampns(struct sock *, struct timespec __user *);
 int sock_recv_errqueue(struct sock *sk, struct msghdr *msg, int len, int level,
 		       int type);
 
+bool sk_ns_capable(const struct sock *sk,
+		   struct user_namespace *user_ns, int cap);
+bool sk_capable(const struct sock *sk, int cap);
+bool sk_net_capable(const struct sock *sk, int cap);
+
 /*
  *	Enable debug/info messages
  */
diff --git a/include/trace/events/ext4.h b/include/trace/events/ext4.h
index 010ea89eeb0..6a1a0245474 100644
--- a/include/trace/events/ext4.h
+++ b/include/trace/events/ext4.h
@@ -16,15 +16,6 @@ struct mpage_da_data;
 struct ext4_map_blocks;
 struct extent_status;
 
-/* shim until we merge in the xfs_collapse_range branch */
-#ifndef FALLOC_FL_COLLAPSE_RANGE
-#define FALLOC_FL_COLLAPSE_RANGE	0x08
-#endif
-
-#ifndef FALLOC_FL_ZERO_RANGE
-#define FALLOC_FL_ZERO_RANGE           0x10
-#endif
-
 #define EXT4_I(inode) (container_of(inode, struct ext4_inode_info, vfs_inode))
 
 #define show_mballoc_flags(flags) __print_flags(flags, "|",	\
diff --git a/include/trace/events/module.h b/include/trace/events/module.h
index 11fd51b413d..7c5cbfe3fc4 100644
--- a/include/trace/events/module.h
+++ b/include/trace/events/module.h
@@ -25,7 +25,7 @@ struct module;
 	{ (1UL << TAINT_OOT_MODULE),		"O" },		\
 	{ (1UL << TAINT_FORCED_MODULE),		"F" },		\
 	{ (1UL << TAINT_CRAP),			"C" },		\
-	{ (1UL << TAINT_UNSIGNED_MODULE),	"X" })
+	{ (1UL << TAINT_UNSIGNED_MODULE),	"E" })
 
 TRACE_EVENT(module_load,
 
@@ -80,7 +80,7 @@ DECLARE_EVENT_CLASS(module_refcnt,
 
 	TP_fast_assign(
 		__entry->ip	= ip;
-		__entry->refcnt	= __this_cpu_read(mod->refptr->incs) + __this_cpu_read(mod->refptr->decs);
+		__entry->refcnt	= __this_cpu_read(mod->refptr->incs) - __this_cpu_read(mod->refptr->decs);
 		__assign_str(name, mod->name);
 	),
 
diff --git a/include/uapi/asm-generic/fcntl.h b/include/uapi/asm-generic/fcntl.h
index a9b13f8b359..7543b3e5133 100644
--- a/include/uapi/asm-generic/fcntl.h
+++ b/include/uapi/asm-generic/fcntl.h
@@ -133,20 +133,20 @@
 #endif
 
 /*
- * fd "private" POSIX locks.
+ * Open File Description Locks
  *
- * Usually POSIX locks held by a process are released on *any* close and are
+ * Usually record locks held by a process are released on *any* close and are
  * not inherited across a fork().
  *
- * These cmd values will set locks that conflict with normal POSIX locks, but
- * are "owned" by the opened file, not the process. This means that they are
- * inherited across fork() like BSD (flock) locks, and they are only released
- * automatically when the last reference to the the open file against which
- * they were acquired is put.
+ * These cmd values will set locks that conflict with process-associated
+ * record  locks, but are "owned" by the open file description, not the
+ * process. This means that they are inherited across fork() like BSD (flock)
+ * locks, and they are only released automatically when the last reference to
+ * the the open file against which they were acquired is put.
  */
-#define F_GETLKP	36
-#define F_SETLKP	37
-#define F_SETLKPW	38
+#define F_OFD_GETLK	36
+#define F_OFD_SETLK	37
+#define F_OFD_SETLKW	38
 
 #define F_OWNER_TID	0
 #define F_OWNER_PID	1
diff --git a/include/uapi/asm-generic/resource.h b/include/uapi/asm-generic/resource.h
index f863428796d..c6d10af5012 100644
--- a/include/uapi/asm-generic/resource.h
+++ b/include/uapi/asm-generic/resource.h
@@ -57,12 +57,5 @@
 # define RLIM_INFINITY		(~0UL)
 #endif
 
-/*
- * RLIMIT_STACK default maximum - some architectures override it:
- */
-#ifndef _STK_LIM_MAX
-# define _STK_LIM_MAX		RLIM_INFINITY
-#endif
-
 
 #endif /* _UAPI_ASM_GENERIC_RESOURCE_H */
diff --git a/include/uapi/asm-generic/unistd.h b/include/uapi/asm-generic/unistd.h
index 6db66783d26..33364060808 100644
--- a/include/uapi/asm-generic/unistd.h
+++ b/include/uapi/asm-generic/unistd.h
@@ -697,9 +697,11 @@ __SYSCALL(__NR_finit_module, sys_finit_module)
 __SYSCALL(__NR_sched_setattr, sys_sched_setattr)
 #define __NR_sched_getattr 275
 __SYSCALL(__NR_sched_getattr, sys_sched_getattr)
+#define __NR_renameat2 276
+__SYSCALL(__NR_renameat2, sys_renameat2)
 
 #undef __NR_syscalls
-#define __NR_syscalls 276
+#define __NR_syscalls 277
 
 /*
  * All syscalls below here should go away really,
diff --git a/include/uapi/drm/tegra_drm.h b/include/uapi/drm/tegra_drm.h
index b042b48495d..b7548211242 100644
--- a/include/uapi/drm/tegra_drm.h
+++ b/include/uapi/drm/tegra_drm.h
@@ -120,7 +120,6 @@ struct drm_tegra_submit {
 	__u32 num_waitchks;
 	__u32 waitchk_mask;
 	__u32 timeout;
-	__u32 pad;
 	__u64 syncpts;
 	__u64 cmdbufs;
 	__u64 relocs;
diff --git a/include/uapi/linux/audit.h b/include/uapi/linux/audit.h
index 11917f747cb..4c31a366be1 100644
--- a/include/uapi/linux/audit.h
+++ b/include/uapi/linux/audit.h
@@ -331,9 +331,17 @@ enum {
 #define AUDIT_FAIL_PRINTK	1
 #define AUDIT_FAIL_PANIC	2
 
+/*
+ * These bits disambiguate different calling conventions that share an
+ * ELF machine type, bitness, and endianness
+ */
+#define __AUDIT_ARCH_CONVENTION_MASK 0x30000000
+#define __AUDIT_ARCH_CONVENTION_MIPS64_N32 0x20000000
+
 /* distinguish syscall tables */
 #define __AUDIT_ARCH_64BIT 0x80000000
 #define __AUDIT_ARCH_LE	   0x40000000
+
 #define AUDIT_ARCH_ALPHA	(EM_ALPHA|__AUDIT_ARCH_64BIT|__AUDIT_ARCH_LE)
 #define AUDIT_ARCH_ARM		(EM_ARM|__AUDIT_ARCH_LE)
 #define AUDIT_ARCH_ARMEB	(EM_ARM)
@@ -346,7 +354,11 @@ enum {
 #define AUDIT_ARCH_MIPS		(EM_MIPS)
 #define AUDIT_ARCH_MIPSEL	(EM_MIPS|__AUDIT_ARCH_LE)
 #define AUDIT_ARCH_MIPS64	(EM_MIPS|__AUDIT_ARCH_64BIT)
+#define AUDIT_ARCH_MIPS64N32	(EM_MIPS|__AUDIT_ARCH_64BIT|\
+				 __AUDIT_ARCH_CONVENTION_MIPS64_N32)
 #define AUDIT_ARCH_MIPSEL64	(EM_MIPS|__AUDIT_ARCH_64BIT|__AUDIT_ARCH_LE)
+#define AUDIT_ARCH_MIPSEL64N32	(EM_MIPS|__AUDIT_ARCH_64BIT|__AUDIT_ARCH_LE|\
+				 __AUDIT_ARCH_CONVENTION_MIPS64_N32)
 #define AUDIT_ARCH_OPENRISC	(EM_OPENRISC)
 #define AUDIT_ARCH_PARISC	(EM_PARISC)
 #define AUDIT_ARCH_PARISC64	(EM_PARISC|__AUDIT_ARCH_64BIT)
diff --git a/include/uapi/linux/fuse.h b/include/uapi/linux/fuse.h
index cf4750e1bb4..40b5ca8a1b1 100644
--- a/include/uapi/linux/fuse.h
+++ b/include/uapi/linux/fuse.h
@@ -96,6 +96,11 @@
  *
  * 7.23
  *  - add FUSE_WRITEBACK_CACHE
+ *  - add time_gran to fuse_init_out
+ *  - add reserved space to fuse_init_out
+ *  - add FATTR_CTIME
+ *  - add ctime and ctimensec to fuse_setattr_in
+ *  - add FUSE_RENAME2 request
  */
 
 #ifndef _LINUX_FUSE_H
@@ -191,6 +196,7 @@ struct fuse_file_lock {
 #define FATTR_ATIME_NOW	(1 << 7)
 #define FATTR_MTIME_NOW	(1 << 8)
 #define FATTR_LOCKOWNER	(1 << 9)
+#define FATTR_CTIME	(1 << 10)
 
 /**
  * Flags returned by the OPEN request
@@ -348,6 +354,7 @@ enum fuse_opcode {
 	FUSE_BATCH_FORGET  = 42,
 	FUSE_FALLOCATE     = 43,
 	FUSE_READDIRPLUS   = 44,
+	FUSE_RENAME2       = 45,
 
 	/* CUSE specific operations */
 	CUSE_INIT          = 4096,
@@ -426,6 +433,12 @@ struct fuse_rename_in {
 	uint64_t	newdir;
 };
 
+struct fuse_rename2_in {
+	uint64_t	newdir;
+	uint32_t	flags;
+	uint32_t	padding;
+};
+
 struct fuse_link_in {
 	uint64_t	oldnodeid;
 };
@@ -438,10 +451,10 @@ struct fuse_setattr_in {
 	uint64_t	lock_owner;
 	uint64_t	atime;
 	uint64_t	mtime;
-	uint64_t	unused2;
+	uint64_t	ctime;
 	uint32_t	atimensec;
 	uint32_t	mtimensec;
-	uint32_t	unused3;
+	uint32_t	ctimensec;
 	uint32_t	mode;
 	uint32_t	unused4;
 	uint32_t	uid;
@@ -559,6 +572,9 @@ struct fuse_init_in {
 	uint32_t	flags;
 };
 
+#define FUSE_COMPAT_INIT_OUT_SIZE 8
+#define FUSE_COMPAT_22_INIT_OUT_SIZE 24
+
 struct fuse_init_out {
 	uint32_t	major;
 	uint32_t	minor;
@@ -567,6 +583,8 @@ struct fuse_init_out {
 	uint16_t	max_background;
 	uint16_t	congestion_threshold;
 	uint32_t	max_write;
+	uint32_t	time_gran;
+	uint32_t	unused[9];
 };
 
 #define CUSE_INIT_INFO_MAX 4096
diff --git a/include/uapi/linux/hyperv.h b/include/uapi/linux/hyperv.h
index 9beb7c99163..78e4a86030d 100644
--- a/include/uapi/linux/hyperv.h
+++ b/include/uapi/linux/hyperv.h
@@ -305,6 +305,7 @@ enum hv_kvp_exchg_pool {
 #define HV_ERROR_DEVICE_NOT_CONNECTED	0x8007048F
 #define HV_INVALIDARG			0x80070057
 #define HV_GUID_NOTFOUND		0x80041002
+#define HV_ERROR_ALREADY_EXISTS		0x80070050
 
 #define ADDR_FAMILY_NONE	0x00
 #define ADDR_FAMILY_IPV4	0x01
diff --git a/include/uapi/linux/input.h b/include/uapi/linux/input.h
index bd24470d24a..f4849525519 100644
--- a/include/uapi/linux/input.h
+++ b/include/uapi/linux/input.h
@@ -164,6 +164,7 @@ struct input_keymap_entry {
 #define INPUT_PROP_DIRECT		0x01	/* direct input devices */
 #define INPUT_PROP_BUTTONPAD		0x02	/* has button(s) under pad */
 #define INPUT_PROP_SEMI_MT		0x03	/* touch rectangle only */
+#define INPUT_PROP_TOPBUTTONPAD		0x04	/* softbuttons at top of pad */
 
 #define INPUT_PROP_MAX			0x1f
 #define INPUT_PROP_CNT			(INPUT_PROP_MAX + 1)
diff --git a/include/uapi/linux/nl80211.h b/include/uapi/linux/nl80211.h
index 1ba9d626aa8..194c1eab04d 100644
--- a/include/uapi/linux/nl80211.h
+++ b/include/uapi/linux/nl80211.h
@@ -3856,6 +3856,8 @@ enum nl80211_ap_sme_features {
  * @NL80211_FEATURE_CELL_BASE_REG_HINTS: This driver has been tested
  *	to work properly to suppport receiving regulatory hints from
  *	cellular base stations.
+ * @NL80211_FEATURE_P2P_DEVICE_NEEDS_CHANNEL: (no longer available, only
+ *	here to reserve the value for API/ABI compatibility)
  * @NL80211_FEATURE_SAE: This driver supports simultaneous authentication of
  *	equals (SAE) with user space SME (NL80211_CMD_AUTHENTICATE) in station
  *	mode
@@ -3897,7 +3899,7 @@ enum nl80211_feature_flags {
 	NL80211_FEATURE_HT_IBSS				= 1 << 1,
 	NL80211_FEATURE_INACTIVITY_TIMER		= 1 << 2,
 	NL80211_FEATURE_CELL_BASE_REG_HINTS		= 1 << 3,
-	/* bit 4 is reserved - don't use */
+	NL80211_FEATURE_P2P_DEVICE_NEEDS_CHANNEL	= 1 << 4,
 	NL80211_FEATURE_SAE				= 1 << 5,
 	NL80211_FEATURE_LOW_PRIORITY_SCAN		= 1 << 6,
 	NL80211_FEATURE_SCAN_FLUSH			= 1 << 7,
diff --git a/include/uapi/linux/serial_core.h b/include/uapi/linux/serial_core.h
index b47dba2c1e6..5820269aa13 100644
--- a/include/uapi/linux/serial_core.h
+++ b/include/uapi/linux/serial_core.h
@@ -211,7 +211,7 @@
 /* VIA VT8500 SoC */
 #define PORT_VT8500	97
 
-/* Xilinx PSS UART */
+/* Cadence (Xilinx Zynq) UART */
 #define PORT_XUARTPS	98
 
 /* Atheros AR933X SoC */
@@ -238,4 +238,10 @@
 /* Tilera TILE-Gx UART */
 #define PORT_TILEGX	106
 
+/* MEN 16z135 UART */
+#define PORT_MEN_Z135	107
+
+/* SC16IS74xx */
+#define PORT_SC16IS7XX   108
+
 #endif /* _UAPILINUX_SERIAL_CORE_H */
diff --git a/include/uapi/linux/serial_reg.h b/include/uapi/linux/serial_reg.h
index e6322605b13..99b47058816 100644
--- a/include/uapi/linux/serial_reg.h
+++ b/include/uapi/linux/serial_reg.h
@@ -32,7 +32,7 @@
 
 #define UART_IIR	2	/* In:  Interrupt ID Register */
 #define UART_IIR_NO_INT		0x01 /* No interrupts pending */
-#define UART_IIR_ID		0x06 /* Mask for the interrupt ID */
+#define UART_IIR_ID		0x0e /* Mask for the interrupt ID */
 #define UART_IIR_MSI		0x00 /* Modem status interrupt */
 #define UART_IIR_THRI		0x02 /* Transmitter holding register empty */
 #define UART_IIR_RDI		0x04 /* Receiver data interrupt */
diff --git a/include/xen/interface/io/blkif.h b/include/xen/interface/io/blkif.h
index 32ec05a6572..c33e1c489eb 100644
--- a/include/xen/interface/io/blkif.h
+++ b/include/xen/interface/io/blkif.h
@@ -86,7 +86,7 @@ typedef uint64_t blkif_sector_t;
  *     Interface%20manuals/100293068c.pdf
  * The backend can optionally provide three extra XenBus attributes to
  * further optimize the discard functionality:
- * 'discard-aligment' - Devices that support discard functionality may
+ * 'discard-alignment' - Devices that support discard functionality may
  * internally allocate space in units that are bigger than the exported
  * logical block size. The discard-alignment parameter indicates how many bytes
  * the beginning of the partition is offset from the internal allocation unit's
diff --git a/include/xen/interface/xen.h b/include/xen/interface/xen.h
index 0cd5ca333fa..de082130ba4 100644
--- a/include/xen/interface/xen.h
+++ b/include/xen/interface/xen.h
@@ -275,9 +275,9 @@ DEFINE_GUEST_HANDLE_STRUCT(mmu_update);
  * NB. The fields are natural register size for this architecture.
  */
 struct multicall_entry {
-    unsigned long op;
-    long result;
-    unsigned long args[6];
+    xen_ulong_t op;
+    xen_long_t result;
+    xen_ulong_t args[6];
 };
 DEFINE_GUEST_HANDLE_STRUCT(multicall_entry);
 
diff --git a/include/xen/xen-ops.h b/include/xen/xen-ops.h
index 2cf47175b12..0b3149ed7ea 100644
--- a/include/xen/xen-ops.h
+++ b/include/xen/xen-ops.h
@@ -9,10 +9,6 @@ DECLARE_PER_CPU(struct vcpu_info *, xen_vcpu);
 
 void xen_arch_pre_suspend(void);
 void xen_arch_post_suspend(int suspend_cancelled);
-void xen_arch_hvm_post_suspend(int suspend_cancelled);
-
-void xen_mm_pin_all(void);
-void xen_mm_unpin_all(void);
 
 void xen_timer_resume(void);
 void xen_arch_resume(void);
diff --git a/init/Kconfig b/init/Kconfig
index 765018c24cf..9d3585bb2a7 100644
--- a/init/Kconfig
+++ b/init/Kconfig
@@ -1646,6 +1646,18 @@ config MMAP_ALLOW_UNINITIALIZED
 
 	  See Documentation/nommu-mmap.txt for more information.
 
+config SYSTEM_TRUSTED_KEYRING
+	bool "Provide system-wide ring of trusted keys"
+	depends on KEYS
+	help
+	  Provide a system keyring to which trusted keys can be added.  Keys in
+	  the keyring are considered to be trusted.  Keys may be added at will
+	  by the kernel from compiled-in data and from hardware key stores, but
+	  userspace may only add extra keys if those keys can be verified by
+	  keys already in the keyring.
+
+	  Keys in this keyring are used by module signature checking.
+
 config PROFILING
 	bool "Profiling support"
 	help
@@ -1681,18 +1693,6 @@ config BASE_SMALL
 	default 0 if BASE_FULL
 	default 1 if !BASE_FULL
 
-config SYSTEM_TRUSTED_KEYRING
-	bool "Provide system-wide ring of trusted keys"
-	depends on KEYS
-	help
-	  Provide a system keyring to which trusted keys can be added.  Keys in
-	  the keyring are considered to be trusted.  Keys may be added at will
-	  by the kernel from compiled-in data and from hardware key stores, but
-	  userspace may only add extra keys if those keys can be verified by
-	  keys already in the keyring.
-
-	  Keys in this keyring are used by module signature checking.
-
 menuconfig MODULES
 	bool "Enable loadable module support"
 	option modules
diff --git a/init/main.c b/init/main.c
index 9c7fd4c9249..48655ceb66f 100644
--- a/init/main.c
+++ b/init/main.c
@@ -476,7 +476,7 @@ static void __init mm_init(void)
 	vmalloc_init();
 }
 
-asmlinkage void __init start_kernel(void)
+asmlinkage __visible void __init start_kernel(void)
 {
 	char * command_line;
 	extern const struct kernel_param __start___param[], __stop___param[];
diff --git a/kernel/audit.c b/kernel/audit.c
index 7c2893602d0..47845c57eb1 100644
--- a/kernel/audit.c
+++ b/kernel/audit.c
@@ -643,13 +643,13 @@ static int audit_netlink_ok(struct sk_buff *skb, u16 msg_type)
 		if ((task_active_pid_ns(current) != &init_pid_ns))
 			return -EPERM;
 
-		if (!capable(CAP_AUDIT_CONTROL))
+		if (!netlink_capable(skb, CAP_AUDIT_CONTROL))
 			err = -EPERM;
 		break;
 	case AUDIT_USER:
 	case AUDIT_FIRST_USER_MSG ... AUDIT_LAST_USER_MSG:
 	case AUDIT_FIRST_USER_MSG2 ... AUDIT_LAST_USER_MSG2:
-		if (!capable(CAP_AUDIT_WRITE))
+		if (!netlink_capable(skb, CAP_AUDIT_WRITE))
 			err = -EPERM;
 		break;
 	default:  /* bad msg */
diff --git a/kernel/cgroup.c b/kernel/cgroup.c
index 9fcdaa705b6..ceee0c54c6a 100644
--- a/kernel/cgroup.c
+++ b/kernel/cgroup.c
@@ -33,6 +33,7 @@
 #include <linux/init_task.h>
 #include <linux/kernel.h>
 #include <linux/list.h>
+#include <linux/magic.h>
 #include <linux/mm.h>
 #include <linux/mutex.h>
 #include <linux/mount.h>
@@ -348,7 +349,7 @@ struct cgrp_cset_link {
  * reference-counted, to improve performance when child cgroups
  * haven't been created.
  */
-static struct css_set init_css_set = {
+struct css_set init_css_set = {
 	.refcount		= ATOMIC_INIT(1),
 	.cgrp_links		= LIST_HEAD_INIT(init_css_set.cgrp_links),
 	.tasks			= LIST_HEAD_INIT(init_css_set.tasks),
@@ -1495,7 +1496,7 @@ static struct dentry *cgroup_mount(struct file_system_type *fs_type,
 	 */
 	if (!use_task_css_set_links)
 		cgroup_enable_task_cg_lists();
-retry:
+
 	mutex_lock(&cgroup_tree_mutex);
 	mutex_lock(&cgroup_mutex);
 
@@ -1503,7 +1504,7 @@ retry:
 	ret = parse_cgroupfs_options(data, &opts);
 	if (ret)
 		goto out_unlock;
-
+retry:
 	/* look for a matching existing root */
 	if (!opts.subsys_mask && !opts.none && !opts.name) {
 		cgrp_dfl_root_visible = true;
@@ -1562,9 +1563,9 @@ retry:
 		if (!atomic_inc_not_zero(&root->cgrp.refcnt)) {
 			mutex_unlock(&cgroup_mutex);
 			mutex_unlock(&cgroup_tree_mutex);
-			kfree(opts.release_agent);
-			kfree(opts.name);
 			msleep(10);
+			mutex_lock(&cgroup_tree_mutex);
+			mutex_lock(&cgroup_mutex);
 			goto retry;
 		}
 
@@ -1604,7 +1605,8 @@ out_unlock:
 	if (ret)
 		return ERR_PTR(ret);
 
-	dentry = kernfs_mount(fs_type, flags, root->kf_root, &new_sb);
+	dentry = kernfs_mount(fs_type, flags, root->kf_root,
+				CGROUP_SUPER_MAGIC, &new_sb);
 	if (IS_ERR(dentry) || !new_sb)
 		cgroup_put(&root->cgrp);
 	return dentry;
diff --git a/kernel/cgroup_freezer.c b/kernel/cgroup_freezer.c
index 2bc4a225644..345628c78b5 100644
--- a/kernel/cgroup_freezer.c
+++ b/kernel/cgroup_freezer.c
@@ -21,6 +21,7 @@
 #include <linux/uaccess.h>
 #include <linux/freezer.h>
 #include <linux/seq_file.h>
+#include <linux/mutex.h>
 
 /*
  * A cgroup is freezing if any FREEZING flags are set.  FREEZING_SELF is
@@ -42,9 +43,10 @@ enum freezer_state_flags {
 struct freezer {
 	struct cgroup_subsys_state	css;
 	unsigned int			state;
-	spinlock_t			lock;
 };
 
+static DEFINE_MUTEX(freezer_mutex);
+
 static inline struct freezer *css_freezer(struct cgroup_subsys_state *css)
 {
 	return css ? container_of(css, struct freezer, css) : NULL;
@@ -93,7 +95,6 @@ freezer_css_alloc(struct cgroup_subsys_state *parent_css)
 	if (!freezer)
 		return ERR_PTR(-ENOMEM);
 
-	spin_lock_init(&freezer->lock);
 	return &freezer->css;
 }
 
@@ -110,14 +111,7 @@ static int freezer_css_online(struct cgroup_subsys_state *css)
 	struct freezer *freezer = css_freezer(css);
 	struct freezer *parent = parent_freezer(freezer);
 
-	/*
-	 * The following double locking and freezing state inheritance
-	 * guarantee that @cgroup can never escape ancestors' freezing
-	 * states.  See css_for_each_descendant_pre() for details.
-	 */
-	if (parent)
-		spin_lock_irq(&parent->lock);
-	spin_lock_nested(&freezer->lock, SINGLE_DEPTH_NESTING);
+	mutex_lock(&freezer_mutex);
 
 	freezer->state |= CGROUP_FREEZER_ONLINE;
 
@@ -126,10 +120,7 @@ static int freezer_css_online(struct cgroup_subsys_state *css)
 		atomic_inc(&system_freezing_cnt);
 	}
 
-	spin_unlock(&freezer->lock);
-	if (parent)
-		spin_unlock_irq(&parent->lock);
-
+	mutex_unlock(&freezer_mutex);
 	return 0;
 }
 
@@ -144,14 +135,14 @@ static void freezer_css_offline(struct cgroup_subsys_state *css)
 {
 	struct freezer *freezer = css_freezer(css);
 
-	spin_lock_irq(&freezer->lock);
+	mutex_lock(&freezer_mutex);
 
 	if (freezer->state & CGROUP_FREEZING)
 		atomic_dec(&system_freezing_cnt);
 
 	freezer->state = 0;
 
-	spin_unlock_irq(&freezer->lock);
+	mutex_unlock(&freezer_mutex);
 }
 
 static void freezer_css_free(struct cgroup_subsys_state *css)
@@ -175,7 +166,7 @@ static void freezer_attach(struct cgroup_subsys_state *new_css,
 	struct task_struct *task;
 	bool clear_frozen = false;
 
-	spin_lock_irq(&freezer->lock);
+	mutex_lock(&freezer_mutex);
 
 	/*
 	 * Make the new tasks conform to the current state of @new_css.
@@ -197,21 +188,13 @@ static void freezer_attach(struct cgroup_subsys_state *new_css,
 		}
 	}
 
-	spin_unlock_irq(&freezer->lock);
-
-	/*
-	 * Propagate FROZEN clearing upwards.  We may race with
-	 * update_if_frozen(), but as long as both work bottom-up, either
-	 * update_if_frozen() sees child's FROZEN cleared or we clear the
-	 * parent's FROZEN later.  No parent w/ !FROZEN children can be
-	 * left FROZEN.
-	 */
+	/* propagate FROZEN clearing upwards */
 	while (clear_frozen && (freezer = parent_freezer(freezer))) {
-		spin_lock_irq(&freezer->lock);
 		freezer->state &= ~CGROUP_FROZEN;
 		clear_frozen = freezer->state & CGROUP_FREEZING;
-		spin_unlock_irq(&freezer->lock);
 	}
+
+	mutex_unlock(&freezer_mutex);
 }
 
 /**
@@ -228,9 +211,6 @@ static void freezer_fork(struct task_struct *task)
 {
 	struct freezer *freezer;
 
-	rcu_read_lock();
-	freezer = task_freezer(task);
-
 	/*
 	 * The root cgroup is non-freezable, so we can skip locking the
 	 * freezer.  This is safe regardless of race with task migration.
@@ -238,24 +218,18 @@ static void freezer_fork(struct task_struct *task)
 	 * to do.  If we lost and root is the new cgroup, noop is still the
 	 * right thing to do.
 	 */
-	if (!parent_freezer(freezer))
-		goto out;
+	if (task_css_is_root(task, freezer_cgrp_id))
+		return;
 
-	/*
-	 * Grab @freezer->lock and freeze @task after verifying @task still
-	 * belongs to @freezer and it's freezing.  The former is for the
-	 * case where we have raced against task migration and lost and
-	 * @task is already in a different cgroup which may not be frozen.
-	 * This isn't strictly necessary as freeze_task() is allowed to be
-	 * called spuriously but let's do it anyway for, if nothing else,
-	 * documentation.
-	 */
-	spin_lock_irq(&freezer->lock);
-	if (freezer == task_freezer(task) && (freezer->state & CGROUP_FREEZING))
+	mutex_lock(&freezer_mutex);
+	rcu_read_lock();
+
+	freezer = task_freezer(task);
+	if (freezer->state & CGROUP_FREEZING)
 		freeze_task(task);
-	spin_unlock_irq(&freezer->lock);
-out:
+
 	rcu_read_unlock();
+	mutex_unlock(&freezer_mutex);
 }
 
 /**
@@ -281,22 +255,24 @@ static void update_if_frozen(struct cgroup_subsys_state *css)
 	struct css_task_iter it;
 	struct task_struct *task;
 
-	WARN_ON_ONCE(!rcu_read_lock_held());
-
-	spin_lock_irq(&freezer->lock);
+	lockdep_assert_held(&freezer_mutex);
 
 	if (!(freezer->state & CGROUP_FREEZING) ||
 	    (freezer->state & CGROUP_FROZEN))
-		goto out_unlock;
+		return;
 
 	/* are all (live) children frozen? */
+	rcu_read_lock();
 	css_for_each_child(pos, css) {
 		struct freezer *child = css_freezer(pos);
 
 		if ((child->state & CGROUP_FREEZER_ONLINE) &&
-		    !(child->state & CGROUP_FROZEN))
-			goto out_unlock;
+		    !(child->state & CGROUP_FROZEN)) {
+			rcu_read_unlock();
+			return;
+		}
 	}
+	rcu_read_unlock();
 
 	/* are all tasks frozen? */
 	css_task_iter_start(css, &it);
@@ -317,21 +293,29 @@ static void update_if_frozen(struct cgroup_subsys_state *css)
 	freezer->state |= CGROUP_FROZEN;
 out_iter_end:
 	css_task_iter_end(&it);
-out_unlock:
-	spin_unlock_irq(&freezer->lock);
 }
 
 static int freezer_read(struct seq_file *m, void *v)
 {
 	struct cgroup_subsys_state *css = seq_css(m), *pos;
 
+	mutex_lock(&freezer_mutex);
 	rcu_read_lock();
 
 	/* update states bottom-up */
-	css_for_each_descendant_post(pos, css)
+	css_for_each_descendant_post(pos, css) {
+		if (!css_tryget(pos))
+			continue;
+		rcu_read_unlock();
+
 		update_if_frozen(pos);
 
+		rcu_read_lock();
+		css_put(pos);
+	}
+
 	rcu_read_unlock();
+	mutex_unlock(&freezer_mutex);
 
 	seq_puts(m, freezer_state_strs(css_freezer(css)->state));
 	seq_putc(m, '\n');
@@ -373,7 +357,7 @@ static void freezer_apply_state(struct freezer *freezer, bool freeze,
 				unsigned int state)
 {
 	/* also synchronizes against task migration, see freezer_attach() */
-	lockdep_assert_held(&freezer->lock);
+	lockdep_assert_held(&freezer_mutex);
 
 	if (!(freezer->state & CGROUP_FREEZER_ONLINE))
 		return;
@@ -414,31 +398,29 @@ static void freezer_change_state(struct freezer *freezer, bool freeze)
 	 * descendant will try to inherit its parent's FREEZING state as
 	 * CGROUP_FREEZING_PARENT.
 	 */
+	mutex_lock(&freezer_mutex);
 	rcu_read_lock();
 	css_for_each_descendant_pre(pos, &freezer->css) {
 		struct freezer *pos_f = css_freezer(pos);
 		struct freezer *parent = parent_freezer(pos_f);
 
-		spin_lock_irq(&pos_f->lock);
+		if (!css_tryget(pos))
+			continue;
+		rcu_read_unlock();
 
-		if (pos_f == freezer) {
+		if (pos_f == freezer)
 			freezer_apply_state(pos_f, freeze,
 					    CGROUP_FREEZING_SELF);
-		} else {
-			/*
-			 * Our update to @parent->state is already visible
-			 * which is all we need.  No need to lock @parent.
-			 * For more info on synchronization, see
-			 * freezer_post_create().
-			 */
+		else
 			freezer_apply_state(pos_f,
 					    parent->state & CGROUP_FREEZING,
 					    CGROUP_FREEZING_PARENT);
-		}
 
-		spin_unlock_irq(&pos_f->lock);
+		rcu_read_lock();
+		css_put(pos);
 	}
 	rcu_read_unlock();
+	mutex_unlock(&freezer_mutex);
 }
 
 static int freezer_write(struct cgroup_subsys_state *css, struct cftype *cft,
diff --git a/kernel/context_tracking.c b/kernel/context_tracking.c
index 6cb20d2e7ee..019d4500844 100644
--- a/kernel/context_tracking.c
+++ b/kernel/context_tracking.c
@@ -120,7 +120,7 @@ void context_tracking_user_enter(void)
  * instead of preempt_schedule() to exit user context if needed before
  * calling the scheduler.
  */
-asmlinkage void __sched notrace preempt_schedule_context(void)
+asmlinkage __visible void __sched notrace preempt_schedule_context(void)
 {
 	enum ctx_state prev_ctx;
 
diff --git a/kernel/cpu.c b/kernel/cpu.c
index a9e710eef0e..247979a1b81 100644
--- a/kernel/cpu.c
+++ b/kernel/cpu.c
@@ -726,10 +726,12 @@ void set_cpu_present(unsigned int cpu, bool present)
 
 void set_cpu_online(unsigned int cpu, bool online)
 {
-	if (online)
+	if (online) {
 		cpumask_set_cpu(cpu, to_cpumask(cpu_online_bits));
-	else
+		cpumask_set_cpu(cpu, to_cpumask(cpu_active_bits));
+	} else {
 		cpumask_clear_cpu(cpu, to_cpumask(cpu_online_bits));
+	}
 }
 
 void set_cpu_active(unsigned int cpu, bool active)
diff --git a/kernel/events/core.c b/kernel/events/core.c
index f83a71a3e46..440eefc6739 100644
--- a/kernel/events/core.c
+++ b/kernel/events/core.c
@@ -1443,6 +1443,11 @@ group_sched_out(struct perf_event *group_event,
 		cpuctx->exclusive = 0;
 }
 
+struct remove_event {
+	struct perf_event *event;
+	bool detach_group;
+};
+
 /*
  * Cross CPU call to remove a performance event
  *
@@ -1451,12 +1456,15 @@ group_sched_out(struct perf_event *group_event,
  */
 static int __perf_remove_from_context(void *info)
 {
-	struct perf_event *event = info;
+	struct remove_event *re = info;
+	struct perf_event *event = re->event;
 	struct perf_event_context *ctx = event->ctx;
 	struct perf_cpu_context *cpuctx = __get_cpu_context(ctx);
 
 	raw_spin_lock(&ctx->lock);
 	event_sched_out(event, cpuctx, ctx);
+	if (re->detach_group)
+		perf_group_detach(event);
 	list_del_event(event, ctx);
 	if (!ctx->nr_events && cpuctx->task_ctx == ctx) {
 		ctx->is_active = 0;
@@ -1481,10 +1489,14 @@ static int __perf_remove_from_context(void *info)
  * When called from perf_event_exit_task, it's OK because the
  * context has been detached from its task.
  */
-static void perf_remove_from_context(struct perf_event *event)
+static void perf_remove_from_context(struct perf_event *event, bool detach_group)
 {
 	struct perf_event_context *ctx = event->ctx;
 	struct task_struct *task = ctx->task;
+	struct remove_event re = {
+		.event = event,
+		.detach_group = detach_group,
+	};
 
 	lockdep_assert_held(&ctx->mutex);
 
@@ -1493,12 +1505,12 @@ static void perf_remove_from_context(struct perf_event *event)
 		 * Per cpu events are removed via an smp call and
 		 * the removal is always successful.
 		 */
-		cpu_function_call(event->cpu, __perf_remove_from_context, event);
+		cpu_function_call(event->cpu, __perf_remove_from_context, &re);
 		return;
 	}
 
 retry:
-	if (!task_function_call(task, __perf_remove_from_context, event))
+	if (!task_function_call(task, __perf_remove_from_context, &re))
 		return;
 
 	raw_spin_lock_irq(&ctx->lock);
@@ -1515,6 +1527,8 @@ retry:
 	 * Since the task isn't running, its safe to remove the event, us
 	 * holding the ctx->lock ensures the task won't get scheduled in.
 	 */
+	if (detach_group)
+		perf_group_detach(event);
 	list_del_event(event, ctx);
 	raw_spin_unlock_irq(&ctx->lock);
 }
@@ -3178,7 +3192,8 @@ static void free_event_rcu(struct rcu_head *head)
 }
 
 static void ring_buffer_put(struct ring_buffer *rb);
-static void ring_buffer_detach(struct perf_event *event, struct ring_buffer *rb);
+static void ring_buffer_attach(struct perf_event *event,
+			       struct ring_buffer *rb);
 
 static void unaccount_event_cpu(struct perf_event *event, int cpu)
 {
@@ -3238,8 +3253,6 @@ static void free_event(struct perf_event *event)
 	unaccount_event(event);
 
 	if (event->rb) {
-		struct ring_buffer *rb;
-
 		/*
 		 * Can happen when we close an event with re-directed output.
 		 *
@@ -3247,12 +3260,7 @@ static void free_event(struct perf_event *event)
 		 * over us; possibly making our ring_buffer_put() the last.
 		 */
 		mutex_lock(&event->mmap_mutex);
-		rb = event->rb;
-		if (rb) {
-			rcu_assign_pointer(event->rb, NULL);
-			ring_buffer_detach(event, rb);
-			ring_buffer_put(rb); /* could be last */
-		}
+		ring_buffer_attach(event, NULL);
 		mutex_unlock(&event->mmap_mutex);
 	}
 
@@ -3281,10 +3289,7 @@ int perf_event_release_kernel(struct perf_event *event)
 	 *     to trigger the AB-BA case.
 	 */
 	mutex_lock_nested(&ctx->mutex, SINGLE_DEPTH_NESTING);
-	raw_spin_lock_irq(&ctx->lock);
-	perf_group_detach(event);
-	raw_spin_unlock_irq(&ctx->lock);
-	perf_remove_from_context(event);
+	perf_remove_from_context(event, true);
 	mutex_unlock(&ctx->mutex);
 
 	free_event(event);
@@ -3839,28 +3844,47 @@ unlock:
 static void ring_buffer_attach(struct perf_event *event,
 			       struct ring_buffer *rb)
 {
+	struct ring_buffer *old_rb = NULL;
 	unsigned long flags;
 
-	if (!list_empty(&event->rb_entry))
-		return;
+	if (event->rb) {
+		/*
+		 * Should be impossible, we set this when removing
+		 * event->rb_entry and wait/clear when adding event->rb_entry.
+		 */
+		WARN_ON_ONCE(event->rcu_pending);
 
-	spin_lock_irqsave(&rb->event_lock, flags);
-	if (list_empty(&event->rb_entry))
-		list_add(&event->rb_entry, &rb->event_list);
-	spin_unlock_irqrestore(&rb->event_lock, flags);
-}
+		old_rb = event->rb;
+		event->rcu_batches = get_state_synchronize_rcu();
+		event->rcu_pending = 1;
 
-static void ring_buffer_detach(struct perf_event *event, struct ring_buffer *rb)
-{
-	unsigned long flags;
+		spin_lock_irqsave(&old_rb->event_lock, flags);
+		list_del_rcu(&event->rb_entry);
+		spin_unlock_irqrestore(&old_rb->event_lock, flags);
+	}
 
-	if (list_empty(&event->rb_entry))
-		return;
+	if (event->rcu_pending && rb) {
+		cond_synchronize_rcu(event->rcu_batches);
+		event->rcu_pending = 0;
+	}
+
+	if (rb) {
+		spin_lock_irqsave(&rb->event_lock, flags);
+		list_add_rcu(&event->rb_entry, &rb->event_list);
+		spin_unlock_irqrestore(&rb->event_lock, flags);
+	}
+
+	rcu_assign_pointer(event->rb, rb);
 
-	spin_lock_irqsave(&rb->event_lock, flags);
-	list_del_init(&event->rb_entry);
-	wake_up_all(&event->waitq);
-	spin_unlock_irqrestore(&rb->event_lock, flags);
+	if (old_rb) {
+		ring_buffer_put(old_rb);
+		/*
+		 * Since we detached before setting the new rb, so that we
+		 * could attach the new rb, we could have missed a wakeup.
+		 * Provide it now.
+		 */
+		wake_up_all(&event->waitq);
+	}
 }
 
 static void ring_buffer_wakeup(struct perf_event *event)
@@ -3929,7 +3953,7 @@ static void perf_mmap_close(struct vm_area_struct *vma)
 {
 	struct perf_event *event = vma->vm_file->private_data;
 
-	struct ring_buffer *rb = event->rb;
+	struct ring_buffer *rb = ring_buffer_get(event);
 	struct user_struct *mmap_user = rb->mmap_user;
 	int mmap_locked = rb->mmap_locked;
 	unsigned long size = perf_data_size(rb);
@@ -3937,18 +3961,14 @@ static void perf_mmap_close(struct vm_area_struct *vma)
 	atomic_dec(&rb->mmap_count);
 
 	if (!atomic_dec_and_mutex_lock(&event->mmap_count, &event->mmap_mutex))
-		return;
+		goto out_put;
 
-	/* Detach current event from the buffer. */
-	rcu_assign_pointer(event->rb, NULL);
-	ring_buffer_detach(event, rb);
+	ring_buffer_attach(event, NULL);
 	mutex_unlock(&event->mmap_mutex);
 
 	/* If there's still other mmap()s of this buffer, we're done. */
-	if (atomic_read(&rb->mmap_count)) {
-		ring_buffer_put(rb); /* can't be last */
-		return;
-	}
+	if (atomic_read(&rb->mmap_count))
+		goto out_put;
 
 	/*
 	 * No other mmap()s, detach from all other events that might redirect
@@ -3978,11 +3998,9 @@ again:
 		 * still restart the iteration to make sure we're not now
 		 * iterating the wrong list.
 		 */
-		if (event->rb == rb) {
-			rcu_assign_pointer(event->rb, NULL);
-			ring_buffer_detach(event, rb);
-			ring_buffer_put(rb); /* can't be last, we still have one */
-		}
+		if (event->rb == rb)
+			ring_buffer_attach(event, NULL);
+
 		mutex_unlock(&event->mmap_mutex);
 		put_event(event);
 
@@ -4007,6 +4025,7 @@ again:
 	vma->vm_mm->pinned_vm -= mmap_locked;
 	free_uid(mmap_user);
 
+out_put:
 	ring_buffer_put(rb); /* could be last */
 }
 
@@ -4124,7 +4143,6 @@ again:
 	vma->vm_mm->pinned_vm += extra;
 
 	ring_buffer_attach(event, rb);
-	rcu_assign_pointer(event->rb, rb);
 
 	perf_event_init_userpage(event);
 	perf_event_update_userpage(event);
@@ -5408,6 +5426,9 @@ struct swevent_htable {
 
 	/* Recursion avoidance in each contexts */
 	int				recursion[PERF_NR_CONTEXTS];
+
+	/* Keeps track of cpu being initialized/exited */
+	bool				online;
 };
 
 static DEFINE_PER_CPU(struct swevent_htable, swevent_htable);
@@ -5654,8 +5675,14 @@ static int perf_swevent_add(struct perf_event *event, int flags)
 	hwc->state = !(flags & PERF_EF_START);
 
 	head = find_swevent_head(swhash, event);
-	if (WARN_ON_ONCE(!head))
+	if (!head) {
+		/*
+		 * We can race with cpu hotplug code. Do not
+		 * WARN if the cpu just got unplugged.
+		 */
+		WARN_ON_ONCE(swhash->online);
 		return -EINVAL;
+	}
 
 	hlist_add_head_rcu(&event->hlist_entry, head);
 
@@ -6914,7 +6941,7 @@ err_size:
 static int
 perf_event_set_output(struct perf_event *event, struct perf_event *output_event)
 {
-	struct ring_buffer *rb = NULL, *old_rb = NULL;
+	struct ring_buffer *rb = NULL;
 	int ret = -EINVAL;
 
 	if (!output_event)
@@ -6942,8 +6969,6 @@ set:
 	if (atomic_read(&event->mmap_count))
 		goto unlock;
 
-	old_rb = event->rb;
-
 	if (output_event) {
 		/* get the rb we want to redirect to */
 		rb = ring_buffer_get(output_event);
@@ -6951,23 +6976,7 @@ set:
 			goto unlock;
 	}
 
-	if (old_rb)
-		ring_buffer_detach(event, old_rb);
-
-	if (rb)
-		ring_buffer_attach(event, rb);
-
-	rcu_assign_pointer(event->rb, rb);
-
-	if (old_rb) {
-		ring_buffer_put(old_rb);
-		/*
-		 * Since we detached before setting the new rb, so that we
-		 * could attach the new rb, we could have missed a wakeup.
-		 * Provide it now.
-		 */
-		wake_up_all(&event->waitq);
-	}
+	ring_buffer_attach(event, rb);
 
 	ret = 0;
 unlock:
@@ -7018,6 +7027,9 @@ SYSCALL_DEFINE5(perf_event_open,
 	if (attr.freq) {
 		if (attr.sample_freq > sysctl_perf_event_sample_rate)
 			return -EINVAL;
+	} else {
+		if (attr.sample_period & (1ULL << 63))
+			return -EINVAL;
 	}
 
 	/*
@@ -7165,7 +7177,7 @@ SYSCALL_DEFINE5(perf_event_open,
 		struct perf_event_context *gctx = group_leader->ctx;
 
 		mutex_lock(&gctx->mutex);
-		perf_remove_from_context(group_leader);
+		perf_remove_from_context(group_leader, false);
 
 		/*
 		 * Removing from the context ends up with disabled
@@ -7175,7 +7187,7 @@ SYSCALL_DEFINE5(perf_event_open,
 		perf_event__state_init(group_leader);
 		list_for_each_entry(sibling, &group_leader->sibling_list,
 				    group_entry) {
-			perf_remove_from_context(sibling);
+			perf_remove_from_context(sibling, false);
 			perf_event__state_init(sibling);
 			put_ctx(gctx);
 		}
@@ -7305,7 +7317,7 @@ void perf_pmu_migrate_context(struct pmu *pmu, int src_cpu, int dst_cpu)
 	mutex_lock(&src_ctx->mutex);
 	list_for_each_entry_safe(event, tmp, &src_ctx->event_list,
 				 event_entry) {
-		perf_remove_from_context(event);
+		perf_remove_from_context(event, false);
 		unaccount_event_cpu(event, src_cpu);
 		put_ctx(src_ctx);
 		list_add(&event->migrate_entry, &events);
@@ -7367,13 +7379,7 @@ __perf_event_exit_task(struct perf_event *child_event,
 			 struct perf_event_context *child_ctx,
 			 struct task_struct *child)
 {
-	if (child_event->parent) {
-		raw_spin_lock_irq(&child_ctx->lock);
-		perf_group_detach(child_event);
-		raw_spin_unlock_irq(&child_ctx->lock);
-	}
-
-	perf_remove_from_context(child_event);
+	perf_remove_from_context(child_event, !!child_event->parent);
 
 	/*
 	 * It can happen that the parent exits first, and has events
@@ -7724,6 +7730,8 @@ int perf_event_init_context(struct task_struct *child, int ctxn)
 	 * swapped under us.
 	 */
 	parent_ctx = perf_pin_task_context(parent, ctxn);
+	if (!parent_ctx)
+		return 0;
 
 	/*
 	 * No need to check if parent_ctx != NULL here; since we saw
@@ -7835,6 +7843,7 @@ static void perf_event_init_cpu(int cpu)
 	struct swevent_htable *swhash = &per_cpu(swevent_htable, cpu);
 
 	mutex_lock(&swhash->hlist_mutex);
+	swhash->online = true;
 	if (swhash->hlist_refcount > 0) {
 		struct swevent_hlist *hlist;
 
@@ -7857,14 +7866,14 @@ static void perf_pmu_rotate_stop(struct pmu *pmu)
 
 static void __perf_event_exit_context(void *__info)
 {
+	struct remove_event re = { .detach_group = false };
 	struct perf_event_context *ctx = __info;
-	struct perf_event *event;
 
 	perf_pmu_rotate_stop(ctx->pmu);
 
 	rcu_read_lock();
-	list_for_each_entry_rcu(event, &ctx->event_list, event_entry)
-		__perf_remove_from_context(event);
+	list_for_each_entry_rcu(re.event, &ctx->event_list, event_entry)
+		__perf_remove_from_context(&re);
 	rcu_read_unlock();
 }
 
@@ -7892,6 +7901,7 @@ static void perf_event_exit_cpu(int cpu)
 	perf_event_exit_cpu_context(cpu);
 
 	mutex_lock(&swhash->hlist_mutex);
+	swhash->online = false;
 	swevent_hlist_release(swhash);
 	mutex_unlock(&swhash->hlist_mutex);
 }
diff --git a/kernel/futex.c b/kernel/futex.c
index b991ec05b8f..89bc9d59ac6 100644
--- a/kernel/futex.c
+++ b/kernel/futex.c
@@ -745,7 +745,8 @@ void exit_pi_state_list(struct task_struct *curr)
 
 static int
 lookup_pi_state(u32 uval, struct futex_hash_bucket *hb,
-		union futex_key *key, struct futex_pi_state **ps)
+		union futex_key *key, struct futex_pi_state **ps,
+		struct task_struct *task)
 {
 	struct futex_pi_state *pi_state = NULL;
 	struct futex_q *this, *next;
@@ -786,6 +787,16 @@ lookup_pi_state(u32 uval, struct futex_hash_bucket *hb,
 					return -EINVAL;
 			}
 
+			/*
+			 * Protect against a corrupted uval. If uval
+			 * is 0x80000000 then pid is 0 and the waiter
+			 * bit is set. So the deadlock check in the
+			 * calling code has failed and we did not fall
+			 * into the check above due to !pid.
+			 */
+			if (task && pi_state->owner == task)
+				return -EDEADLK;
+
 			atomic_inc(&pi_state->refcount);
 			*ps = pi_state;
 
@@ -803,6 +814,11 @@ lookup_pi_state(u32 uval, struct futex_hash_bucket *hb,
 	if (!p)
 		return -ESRCH;
 
+	if (!p->mm) {
+		put_task_struct(p);
+		return -EPERM;
+	}
+
 	/*
 	 * We need to look at the task state flags to figure out,
 	 * whether the task is exiting. To protect against the do_exit
@@ -935,7 +951,7 @@ retry:
 	 * We dont have the lock. Look up the PI state (or create it if
 	 * we are the first waiter):
 	 */
-	ret = lookup_pi_state(uval, hb, key, ps);
+	ret = lookup_pi_state(uval, hb, key, ps, task);
 
 	if (unlikely(ret)) {
 		switch (ret) {
@@ -1347,7 +1363,7 @@ void requeue_pi_wake_futex(struct futex_q *q, union futex_key *key,
  *
  * Return:
  *  0 - failed to acquire the lock atomically;
- *  1 - acquired the lock;
+ * >0 - acquired the lock, return value is vpid of the top_waiter
  * <0 - error
  */
 static int futex_proxy_trylock_atomic(u32 __user *pifutex,
@@ -1358,7 +1374,7 @@ static int futex_proxy_trylock_atomic(u32 __user *pifutex,
 {
 	struct futex_q *top_waiter = NULL;
 	u32 curval;
-	int ret;
+	int ret, vpid;
 
 	if (get_futex_value_locked(&curval, pifutex))
 		return -EFAULT;
@@ -1386,11 +1402,13 @@ static int futex_proxy_trylock_atomic(u32 __user *pifutex,
 	 * the contended case or if set_waiters is 1.  The pi_state is returned
 	 * in ps in contended cases.
 	 */
+	vpid = task_pid_vnr(top_waiter->task);
 	ret = futex_lock_pi_atomic(pifutex, hb2, key2, ps, top_waiter->task,
 				   set_waiters);
-	if (ret == 1)
+	if (ret == 1) {
 		requeue_pi_wake_futex(top_waiter, key2, hb2);
-
+		return vpid;
+	}
 	return ret;
 }
 
@@ -1421,7 +1439,6 @@ static int futex_requeue(u32 __user *uaddr1, unsigned int flags,
 	struct futex_pi_state *pi_state = NULL;
 	struct futex_hash_bucket *hb1, *hb2;
 	struct futex_q *this, *next;
-	u32 curval2;
 
 	if (requeue_pi) {
 		/*
@@ -1509,16 +1526,25 @@ retry_private:
 		 * At this point the top_waiter has either taken uaddr2 or is
 		 * waiting on it.  If the former, then the pi_state will not
 		 * exist yet, look it up one more time to ensure we have a
-		 * reference to it.
+		 * reference to it. If the lock was taken, ret contains the
+		 * vpid of the top waiter task.
 		 */
-		if (ret == 1) {
+		if (ret > 0) {
 			WARN_ON(pi_state);
 			drop_count++;
 			task_count++;
-			ret = get_futex_value_locked(&curval2, uaddr2);
-			if (!ret)
-				ret = lookup_pi_state(curval2, hb2, &key2,
-						      &pi_state);
+			/*
+			 * If we acquired the lock, then the user
+			 * space value of uaddr2 should be vpid. It
+			 * cannot be changed by the top waiter as it
+			 * is blocked on hb2 lock if it tries to do
+			 * so. If something fiddled with it behind our
+			 * back the pi state lookup might unearth
+			 * it. So we rather use the known value than
+			 * rereading and handing potential crap to
+			 * lookup_pi_state.
+			 */
+			ret = lookup_pi_state(ret, hb2, &key2, &pi_state, NULL);
 		}
 
 		switch (ret) {
diff --git a/kernel/hrtimer.c b/kernel/hrtimer.c
index d55092ceee2..e0501fe7140 100644
--- a/kernel/hrtimer.c
+++ b/kernel/hrtimer.c
@@ -234,6 +234,11 @@ again:
 			goto again;
 		}
 		timer->base = new_base;
+	} else {
+		if (cpu != this_cpu && hrtimer_check_target(timer, new_base)) {
+			cpu = this_cpu;
+			goto again;
+		}
 	}
 	return new_base;
 }
@@ -569,6 +574,23 @@ hrtimer_force_reprogram(struct hrtimer_cpu_base *cpu_base, int skip_equal)
 
 	cpu_base->expires_next.tv64 = expires_next.tv64;
 
+	/*
+	 * If a hang was detected in the last timer interrupt then we
+	 * leave the hang delay active in the hardware. We want the
+	 * system to make progress. That also prevents the following
+	 * scenario:
+	 * T1 expires 50ms from now
+	 * T2 expires 5s from now
+	 *
+	 * T1 is removed, so this code is called and would reprogram
+	 * the hardware to 5s from now. Any hrtimer_start after that
+	 * will not reprogram the hardware due to hang_detected being
+	 * set. So we'd effectivly block all timers until the T2 event
+	 * fires.
+	 */
+	if (cpu_base->hang_detected)
+		return;
+
 	if (cpu_base->expires_next.tv64 != KTIME_MAX)
 		tick_program_event(cpu_base->expires_next, 1);
 }
@@ -968,11 +990,8 @@ int __hrtimer_start_range_ns(struct hrtimer *timer, ktime_t tim,
 	/* Remove an active timer from the queue: */
 	ret = remove_hrtimer(timer, base);
 
-	/* Switch the timer base, if necessary: */
-	new_base = switch_hrtimer_base(timer, base, mode & HRTIMER_MODE_PINNED);
-
 	if (mode & HRTIMER_MODE_REL) {
-		tim = ktime_add_safe(tim, new_base->get_time());
+		tim = ktime_add_safe(tim, base->get_time());
 		/*
 		 * CONFIG_TIME_LOW_RES is a temporary way for architectures
 		 * to signal that they simply return xtime in
@@ -987,6 +1006,9 @@ int __hrtimer_start_range_ns(struct hrtimer *timer, ktime_t tim,
 
 	hrtimer_set_expires_range_ns(timer, tim, delta_ns);
 
+	/* Switch the timer base, if necessary: */
+	new_base = switch_hrtimer_base(timer, base, mode & HRTIMER_MODE_PINNED);
+
 	timer_stats_hrtimer_set_start_info(timer);
 
 	leftmost = enqueue_hrtimer(timer, new_base);
diff --git a/kernel/irq/irqdesc.c b/kernel/irq/irqdesc.c
index a7174617616..bb07f2928f4 100644
--- a/kernel/irq/irqdesc.c
+++ b/kernel/irq/irqdesc.c
@@ -363,6 +363,13 @@ __irq_alloc_descs(int irq, unsigned int from, unsigned int cnt, int node,
 		if (from > irq)
 			return -EINVAL;
 		from = irq;
+	} else {
+		/*
+		 * For interrupts which are freely allocated the
+		 * architecture can force a lower bound to the @from
+		 * argument. x86 uses this to exclude the GSI space.
+		 */
+		from = arch_dynirq_lower_bound(from);
 	}
 
 	mutex_lock(&sparse_irq_lock);
diff --git a/kernel/irq/manage.c b/kernel/irq/manage.c
index 2486a4c1a71..d34131ca372 100644
--- a/kernel/irq/manage.c
+++ b/kernel/irq/manage.c
@@ -180,7 +180,7 @@ int irq_do_set_affinity(struct irq_data *data, const struct cpumask *mask,
 	struct irq_chip *chip = irq_data_get_irq_chip(data);
 	int ret;
 
-	ret = chip->irq_set_affinity(data, mask, false);
+	ret = chip->irq_set_affinity(data, mask, force);
 	switch (ret) {
 	case IRQ_SET_MASK_OK:
 		cpumask_copy(data->affinity, mask);
@@ -192,7 +192,8 @@ int irq_do_set_affinity(struct irq_data *data, const struct cpumask *mask,
 	return ret;
 }
 
-int __irq_set_affinity_locked(struct irq_data *data, const struct cpumask *mask)
+int irq_set_affinity_locked(struct irq_data *data, const struct cpumask *mask,
+			    bool force)
 {
 	struct irq_chip *chip = irq_data_get_irq_chip(data);
 	struct irq_desc *desc = irq_data_to_desc(data);
@@ -202,7 +203,7 @@ int __irq_set_affinity_locked(struct irq_data *data, const struct cpumask *mask)
 		return -EINVAL;
 
 	if (irq_can_move_pcntxt(data)) {
-		ret = irq_do_set_affinity(data, mask, false);
+		ret = irq_do_set_affinity(data, mask, force);
 	} else {
 		irqd_set_move_pending(data);
 		irq_copy_pending(desc, mask);
@@ -217,13 +218,7 @@ int __irq_set_affinity_locked(struct irq_data *data, const struct cpumask *mask)
 	return ret;
 }
 
-/**
- *	irq_set_affinity - Set the irq affinity of a given irq
- *	@irq:		Interrupt to set affinity
- *	@mask:		cpumask
- *
- */
-int irq_set_affinity(unsigned int irq, const struct cpumask *mask)
+int __irq_set_affinity(unsigned int irq, const struct cpumask *mask, bool force)
 {
 	struct irq_desc *desc = irq_to_desc(irq);
 	unsigned long flags;
@@ -233,7 +228,7 @@ int irq_set_affinity(unsigned int irq, const struct cpumask *mask)
 		return -EINVAL;
 
 	raw_spin_lock_irqsave(&desc->lock, flags);
-	ret =  __irq_set_affinity_locked(irq_desc_get_irq_data(desc), mask);
+	ret = irq_set_affinity_locked(irq_desc_get_irq_data(desc), mask, force);
 	raw_spin_unlock_irqrestore(&desc->lock, flags);
 	return ret;
 }
diff --git a/kernel/kexec.c b/kernel/kexec.c
index c8380ad203b..28c57069ef6 100644
--- a/kernel/kexec.c
+++ b/kernel/kexec.c
@@ -1683,6 +1683,14 @@ int kernel_kexec(void)
 		kexec_in_progress = true;
 		kernel_restart_prepare(NULL);
 		migrate_to_reboot_cpu();
+
+		/*
+		 * migrate_to_reboot_cpu() disables CPU hotplug assuming that
+		 * no further code needs to use CPU hotplug (which is true in
+		 * the reboot case). However, the kexec path depends on using
+		 * CPU hotplug again; so re-enable it here.
+		 */
+		cpu_hotplug_enable();
 		printk(KERN_EMERG "Starting new kernel\n");
 		machine_shutdown();
 	}
diff --git a/kernel/ksysfs.c b/kernel/ksysfs.c
index 2495a9b14ac..6683ccef9ff 100644
--- a/kernel/ksysfs.c
+++ b/kernel/ksysfs.c
@@ -37,6 +37,7 @@ static ssize_t uevent_seqnum_show(struct kobject *kobj,
 }
 KERNEL_ATTR_RO(uevent_seqnum);
 
+#ifdef CONFIG_UEVENT_HELPER
 /* uevent helper program, used during early boot */
 static ssize_t uevent_helper_show(struct kobject *kobj,
 				  struct kobj_attribute *attr, char *buf)
@@ -56,7 +57,7 @@ static ssize_t uevent_helper_store(struct kobject *kobj,
 	return count;
 }
 KERNEL_ATTR_RW(uevent_helper);
-
+#endif
 
 #ifdef CONFIG_PROFILING
 static ssize_t profiling_show(struct kobject *kobj,
@@ -189,7 +190,9 @@ EXPORT_SYMBOL_GPL(kernel_kobj);
 static struct attribute * kernel_attrs[] = {
 	&fscaps_attr.attr,
 	&uevent_seqnum_attr.attr,
+#ifdef CONFIG_UEVENT_HELPER
 	&uevent_helper_attr.attr,
+#endif
 #ifdef CONFIG_PROFILING
 	&profiling_attr.attr,
 #endif
diff --git a/kernel/locking/lockdep.c b/kernel/locking/lockdep.c
index b0e9467922e..d24e4339b46 100644
--- a/kernel/locking/lockdep.c
+++ b/kernel/locking/lockdep.c
@@ -4188,7 +4188,7 @@ void debug_show_held_locks(struct task_struct *task)
 }
 EXPORT_SYMBOL_GPL(debug_show_held_locks);
 
-asmlinkage void lockdep_sys_exit(void)
+asmlinkage __visible void lockdep_sys_exit(void)
 {
 	struct task_struct *curr = current;
 
diff --git a/kernel/locking/locktorture.c b/kernel/locking/locktorture.c
index f26b1a18e34..dbafeac18e4 100644
--- a/kernel/locking/locktorture.c
+++ b/kernel/locking/locktorture.c
@@ -82,14 +82,14 @@ struct lock_writer_stress_stats {
 };
 static struct lock_writer_stress_stats *lwsa;
 
-#if defined(MODULE) || defined(CONFIG_LOCK_TORTURE_TEST_RUNNABLE)
+#if defined(MODULE)
 #define LOCKTORTURE_RUNNABLE_INIT 1
 #else
 #define LOCKTORTURE_RUNNABLE_INIT 0
 #endif
 int locktorture_runnable = LOCKTORTURE_RUNNABLE_INIT;
 module_param(locktorture_runnable, int, 0444);
-MODULE_PARM_DESC(locktorture_runnable, "Start locktorture at boot");
+MODULE_PARM_DESC(locktorture_runnable, "Start locktorture at module init");
 
 /* Forward reference. */
 static void lock_torture_cleanup(void);
@@ -219,7 +219,8 @@ static int lock_torture_writer(void *arg)
 	set_user_nice(current, 19);
 
 	do {
-		schedule_timeout_uninterruptible(1);
+		if ((torture_random(&rand) & 0xfffff) == 0)
+			schedule_timeout_uninterruptible(1);
 		cur_ops->writelock();
 		if (WARN_ON_ONCE(lock_is_write_held))
 			lwsp->n_write_lock_fail++;
@@ -354,7 +355,8 @@ static int __init lock_torture_init(void)
 		&lock_busted_ops, &spin_lock_ops, &spin_lock_irq_ops,
 	};
 
-	torture_init_begin(torture_type, verbose, &locktorture_runnable);
+	if (!torture_init_begin(torture_type, verbose, &locktorture_runnable))
+		return -EBUSY;
 
 	/* Process args and tell the world that the torturer is on the job. */
 	for (i = 0; i < ARRAY_SIZE(torture_ops); i++) {
diff --git a/kernel/locking/rtmutex.c b/kernel/locking/rtmutex.c
index aa4dff04b59..a620d4d08ca 100644
--- a/kernel/locking/rtmutex.c
+++ b/kernel/locking/rtmutex.c
@@ -343,9 +343,16 @@ static int rt_mutex_adjust_prio_chain(struct task_struct *task,
 	 * top_waiter can be NULL, when we are in the deboosting
 	 * mode!
 	 */
-	if (top_waiter && (!task_has_pi_waiters(task) ||
-			   top_waiter != task_top_pi_waiter(task)))
-		goto out_unlock_pi;
+	if (top_waiter) {
+		if (!task_has_pi_waiters(task))
+			goto out_unlock_pi;
+		/*
+		 * If deadlock detection is off, we stop here if we
+		 * are not the top pi waiter of the task.
+		 */
+		if (!detect_deadlock && top_waiter != task_top_pi_waiter(task))
+			goto out_unlock_pi;
+	}
 
 	/*
 	 * When deadlock detection is off then we check, if further
@@ -361,7 +368,12 @@ static int rt_mutex_adjust_prio_chain(struct task_struct *task,
 		goto retry;
 	}
 
-	/* Deadlock detection */
+	/*
+	 * Deadlock detection. If the lock is the same as the original
+	 * lock which caused us to walk the lock chain or if the
+	 * current lock is owned by the task which initiated the chain
+	 * walk, we detected a deadlock.
+	 */
 	if (lock == orig_lock || rt_mutex_owner(lock) == top_task) {
 		debug_rt_mutex_deadlock(deadlock_detect, orig_waiter, lock);
 		raw_spin_unlock(&lock->wait_lock);
@@ -527,6 +539,18 @@ static int task_blocks_on_rt_mutex(struct rt_mutex *lock,
 	unsigned long flags;
 	int chain_walk = 0, res;
 
+	/*
+	 * Early deadlock detection. We really don't want the task to
+	 * enqueue on itself just to untangle the mess later. It's not
+	 * only an optimization. We drop the locks, so another waiter
+	 * can come in before the chain walk detects the deadlock. So
+	 * the other will detect the deadlock and return -EDEADLOCK,
+	 * which is wrong, as the other waiter is not in a deadlock
+	 * situation.
+	 */
+	if (detect_deadlock && owner == task)
+		return -EDEADLK;
+
 	raw_spin_lock_irqsave(&task->pi_lock, flags);
 	__rt_mutex_adjust_prio(task);
 	waiter->task = task;
diff --git a/kernel/module.c b/kernel/module.c
index 11869408f79..079c4615607 100644
--- a/kernel/module.c
+++ b/kernel/module.c
@@ -815,9 +815,6 @@ SYSCALL_DEFINE2(delete_module, const char __user *, name_user,
 		return -EFAULT;
 	name[MODULE_NAME_LEN-1] = '\0';
 
-	if (!(flags & O_NONBLOCK))
-		pr_warn("waiting module removal not supported: please upgrade\n");
-
 	if (mutex_lock_interruptible(&module_mutex) != 0)
 		return -EINTR;
 
@@ -3271,6 +3268,9 @@ static int load_module(struct load_info *info, const char __user *uargs,
 
 	dynamic_debug_setup(info->debug, info->num_debug);
 
+	/* Ftrace init must be called in the MODULE_STATE_UNFORMED state */
+	ftrace_module_init(mod);
+
 	/* Finally it's fully formed, ready to start executing. */
 	err = complete_formation(mod, info);
 	if (err)
diff --git a/kernel/power/snapshot.c b/kernel/power/snapshot.c
index 18fb7a2fb14..1ea328aafdc 100644
--- a/kernel/power/snapshot.c
+++ b/kernel/power/snapshot.c
@@ -1586,7 +1586,7 @@ swsusp_alloc(struct memory_bitmap *orig_bm, struct memory_bitmap *copy_bm,
 	return -ENOMEM;
 }
 
-asmlinkage int swsusp_save(void)
+asmlinkage __visible int swsusp_save(void)
 {
 	unsigned int nr_pages, nr_highmem;
 
diff --git a/kernel/power/suspend.c b/kernel/power/suspend.c
index c3ad9cafe93..8233cd4047d 100644
--- a/kernel/power/suspend.c
+++ b/kernel/power/suspend.c
@@ -14,6 +14,7 @@
 #include <linux/init.h>
 #include <linux/console.h>
 #include <linux/cpu.h>
+#include <linux/cpuidle.h>
 #include <linux/syscalls.h>
 #include <linux/gfp.h>
 #include <linux/io.h>
@@ -53,7 +54,9 @@ static void freeze_begin(void)
 
 static void freeze_enter(void)
 {
+	cpuidle_resume();
 	wait_event(suspend_freeze_wait_head, suspend_freeze_wake);
+	cpuidle_pause();
 }
 
 void freeze_wake(void)
diff --git a/kernel/printk/printk.c b/kernel/printk/printk.c
index a45b5096229..221229cf019 100644
--- a/kernel/printk/printk.c
+++ b/kernel/printk/printk.c
@@ -1674,7 +1674,7 @@ EXPORT_SYMBOL(printk_emit);
  *
  * See the vsnprintf() documentation for format string extensions over C99.
  */
-asmlinkage int printk(const char *fmt, ...)
+asmlinkage __visible int printk(const char *fmt, ...)
 {
 	va_list args;
 	int r;
@@ -1737,7 +1737,7 @@ void early_vprintk(const char *fmt, va_list ap)
 	}
 }
 
-asmlinkage void early_printk(const char *fmt, ...)
+asmlinkage __visible void early_printk(const char *fmt, ...)
 {
 	va_list ap;
 
@@ -2413,6 +2413,7 @@ int unregister_console(struct console *console)
 	if (console_drivers != NULL && console->flags & CON_CONSDEV)
 		console_drivers->flags |= CON_CONSDEV;
 
+	console->flags &= ~CON_ENABLED;
 	console_unlock();
 	console_sysfs_notify();
 	return res;
diff --git a/kernel/rcu/rcutorture.c b/kernel/rcu/rcutorture.c
index bd30bc61bc0..7fa34f86e5b 100644
--- a/kernel/rcu/rcutorture.c
+++ b/kernel/rcu/rcutorture.c
@@ -58,9 +58,11 @@ torture_param(int, fqs_duration, 0,
 	      "Duration of fqs bursts (us), 0 to disable");
 torture_param(int, fqs_holdoff, 0, "Holdoff time within fqs bursts (us)");
 torture_param(int, fqs_stutter, 3, "Wait time between fqs bursts (s)");
+torture_param(bool, gp_cond, false, "Use conditional/async GP wait primitives");
 torture_param(bool, gp_exp, false, "Use expedited GP wait primitives");
 torture_param(bool, gp_normal, false,
 	     "Use normal (non-expedited) GP wait primitives");
+torture_param(bool, gp_sync, false, "Use synchronous GP wait primitives");
 torture_param(int, irqreader, 1, "Allow RCU readers from irq handlers");
 torture_param(int, n_barrier_cbs, 0,
 	     "# of callbacks/kthreads for barrier testing");
@@ -138,6 +140,18 @@ static long n_barrier_attempts;
 static long n_barrier_successes;
 static struct list_head rcu_torture_removed;
 
+static int rcu_torture_writer_state;
+#define RTWS_FIXED_DELAY	0
+#define RTWS_DELAY		1
+#define RTWS_REPLACE		2
+#define RTWS_DEF_FREE		3
+#define RTWS_EXP_SYNC		4
+#define RTWS_COND_GET		5
+#define RTWS_COND_SYNC		6
+#define RTWS_SYNC		7
+#define RTWS_STUTTER		8
+#define RTWS_STOPPING		9
+
 #if defined(MODULE) || defined(CONFIG_RCU_TORTURE_TEST_RUNNABLE)
 #define RCUTORTURE_RUNNABLE_INIT 1
 #else
@@ -214,6 +228,7 @@ rcu_torture_free(struct rcu_torture *p)
  */
 
 struct rcu_torture_ops {
+	int ttype;
 	void (*init)(void);
 	int (*readlock)(void);
 	void (*read_delay)(struct torture_random_state *rrsp);
@@ -222,6 +237,8 @@ struct rcu_torture_ops {
 	void (*deferred_free)(struct rcu_torture *p);
 	void (*sync)(void);
 	void (*exp_sync)(void);
+	unsigned long (*get_state)(void);
+	void (*cond_sync)(unsigned long oldstate);
 	void (*call)(struct rcu_head *head, void (*func)(struct rcu_head *rcu));
 	void (*cb_barrier)(void);
 	void (*fqs)(void);
@@ -273,10 +290,48 @@ static int rcu_torture_completed(void)
 	return rcu_batches_completed();
 }
 
+/*
+ * Update callback in the pipe.  This should be invoked after a grace period.
+ */
+static bool
+rcu_torture_pipe_update_one(struct rcu_torture *rp)
+{
+	int i;
+
+	i = rp->rtort_pipe_count;
+	if (i > RCU_TORTURE_PIPE_LEN)
+		i = RCU_TORTURE_PIPE_LEN;
+	atomic_inc(&rcu_torture_wcount[i]);
+	if (++rp->rtort_pipe_count >= RCU_TORTURE_PIPE_LEN) {
+		rp->rtort_mbtest = 0;
+		return true;
+	}
+	return false;
+}
+
+/*
+ * Update all callbacks in the pipe.  Suitable for synchronous grace-period
+ * primitives.
+ */
+static void
+rcu_torture_pipe_update(struct rcu_torture *old_rp)
+{
+	struct rcu_torture *rp;
+	struct rcu_torture *rp1;
+
+	if (old_rp)
+		list_add(&old_rp->rtort_free, &rcu_torture_removed);
+	list_for_each_entry_safe(rp, rp1, &rcu_torture_removed, rtort_free) {
+		if (rcu_torture_pipe_update_one(rp)) {
+			list_del(&rp->rtort_free);
+			rcu_torture_free(rp);
+		}
+	}
+}
+
 static void
 rcu_torture_cb(struct rcu_head *p)
 {
-	int i;
 	struct rcu_torture *rp = container_of(p, struct rcu_torture, rtort_rcu);
 
 	if (torture_must_stop_irq()) {
@@ -284,16 +339,10 @@ rcu_torture_cb(struct rcu_head *p)
 		/* The next initialization will pick up the pieces. */
 		return;
 	}
-	i = rp->rtort_pipe_count;
-	if (i > RCU_TORTURE_PIPE_LEN)
-		i = RCU_TORTURE_PIPE_LEN;
-	atomic_inc(&rcu_torture_wcount[i]);
-	if (++rp->rtort_pipe_count >= RCU_TORTURE_PIPE_LEN) {
-		rp->rtort_mbtest = 0;
+	if (rcu_torture_pipe_update_one(rp))
 		rcu_torture_free(rp);
-	} else {
+	else
 		cur_ops->deferred_free(rp);
-	}
 }
 
 static int rcu_no_completed(void)
@@ -312,6 +361,7 @@ static void rcu_sync_torture_init(void)
 }
 
 static struct rcu_torture_ops rcu_ops = {
+	.ttype		= RCU_FLAVOR,
 	.init		= rcu_sync_torture_init,
 	.readlock	= rcu_torture_read_lock,
 	.read_delay	= rcu_read_delay,
@@ -320,6 +370,8 @@ static struct rcu_torture_ops rcu_ops = {
 	.deferred_free	= rcu_torture_deferred_free,
 	.sync		= synchronize_rcu,
 	.exp_sync	= synchronize_rcu_expedited,
+	.get_state	= get_state_synchronize_rcu,
+	.cond_sync	= cond_synchronize_rcu,
 	.call		= call_rcu,
 	.cb_barrier	= rcu_barrier,
 	.fqs		= rcu_force_quiescent_state,
@@ -355,6 +407,7 @@ static void rcu_bh_torture_deferred_free(struct rcu_torture *p)
 }
 
 static struct rcu_torture_ops rcu_bh_ops = {
+	.ttype		= RCU_BH_FLAVOR,
 	.init		= rcu_sync_torture_init,
 	.readlock	= rcu_bh_torture_read_lock,
 	.read_delay	= rcu_read_delay,  /* just reuse rcu's version. */
@@ -397,6 +450,7 @@ call_rcu_busted(struct rcu_head *head, void (*func)(struct rcu_head *rcu))
 }
 
 static struct rcu_torture_ops rcu_busted_ops = {
+	.ttype		= INVALID_RCU_FLAVOR,
 	.init		= rcu_sync_torture_init,
 	.readlock	= rcu_torture_read_lock,
 	.read_delay	= rcu_read_delay,  /* just reuse rcu's version. */
@@ -479,9 +533,11 @@ static void srcu_torture_stats(char *page)
 	page += sprintf(page, "%s%s per-CPU(idx=%d):",
 		       torture_type, TORTURE_FLAG, idx);
 	for_each_possible_cpu(cpu) {
-		page += sprintf(page, " %d(%lu,%lu)", cpu,
-			       per_cpu_ptr(srcu_ctl.per_cpu_ref, cpu)->c[!idx],
-			       per_cpu_ptr(srcu_ctl.per_cpu_ref, cpu)->c[idx]);
+		long c0, c1;
+
+		c0 = (long)per_cpu_ptr(srcu_ctl.per_cpu_ref, cpu)->c[!idx];
+		c1 = (long)per_cpu_ptr(srcu_ctl.per_cpu_ref, cpu)->c[idx];
+		page += sprintf(page, " %d(%ld,%ld)", cpu, c0, c1);
 	}
 	sprintf(page, "\n");
 }
@@ -492,6 +548,7 @@ static void srcu_torture_synchronize_expedited(void)
 }
 
 static struct rcu_torture_ops srcu_ops = {
+	.ttype		= SRCU_FLAVOR,
 	.init		= rcu_sync_torture_init,
 	.readlock	= srcu_torture_read_lock,
 	.read_delay	= srcu_read_delay,
@@ -527,6 +584,7 @@ static void rcu_sched_torture_deferred_free(struct rcu_torture *p)
 }
 
 static struct rcu_torture_ops sched_ops = {
+	.ttype		= RCU_SCHED_FLAVOR,
 	.init		= rcu_sync_torture_init,
 	.readlock	= sched_torture_read_lock,
 	.read_delay	= rcu_read_delay,  /* just reuse rcu's version. */
@@ -688,23 +746,59 @@ rcu_torture_fqs(void *arg)
 static int
 rcu_torture_writer(void *arg)
 {
-	bool exp;
+	unsigned long gp_snap;
+	bool gp_cond1 = gp_cond, gp_exp1 = gp_exp, gp_normal1 = gp_normal;
+	bool gp_sync1 = gp_sync;
 	int i;
 	struct rcu_torture *rp;
-	struct rcu_torture *rp1;
 	struct rcu_torture *old_rp;
 	static DEFINE_TORTURE_RANDOM(rand);
+	int synctype[] = { RTWS_DEF_FREE, RTWS_EXP_SYNC,
+			   RTWS_COND_GET, RTWS_SYNC };
+	int nsynctypes = 0;
 
 	VERBOSE_TOROUT_STRING("rcu_torture_writer task started");
-	set_user_nice(current, MAX_NICE);
+
+	/* Initialize synctype[] array.  If none set, take default. */
+	if (!gp_cond1 && !gp_exp1 && !gp_normal1 && !gp_sync)
+		gp_cond1 = gp_exp1 = gp_normal1 = gp_sync1 = true;
+	if (gp_cond1 && cur_ops->get_state && cur_ops->cond_sync)
+		synctype[nsynctypes++] = RTWS_COND_GET;
+	else if (gp_cond && (!cur_ops->get_state || !cur_ops->cond_sync))
+		pr_alert("rcu_torture_writer: gp_cond without primitives.\n");
+	if (gp_exp1 && cur_ops->exp_sync)
+		synctype[nsynctypes++] = RTWS_EXP_SYNC;
+	else if (gp_exp && !cur_ops->exp_sync)
+		pr_alert("rcu_torture_writer: gp_exp without primitives.\n");
+	if (gp_normal1 && cur_ops->deferred_free)
+		synctype[nsynctypes++] = RTWS_DEF_FREE;
+	else if (gp_normal && !cur_ops->deferred_free)
+		pr_alert("rcu_torture_writer: gp_normal without primitives.\n");
+	if (gp_sync1 && cur_ops->sync)
+		synctype[nsynctypes++] = RTWS_SYNC;
+	else if (gp_sync && !cur_ops->sync)
+		pr_alert("rcu_torture_writer: gp_sync without primitives.\n");
+	if (WARN_ONCE(nsynctypes == 0,
+		      "rcu_torture_writer: No update-side primitives.\n")) {
+		/*
+		 * No updates primitives, so don't try updating.
+		 * The resulting test won't be testing much, hence the
+		 * above WARN_ONCE().
+		 */
+		rcu_torture_writer_state = RTWS_STOPPING;
+		torture_kthread_stopping("rcu_torture_writer");
+	}
 
 	do {
+		rcu_torture_writer_state = RTWS_FIXED_DELAY;
 		schedule_timeout_uninterruptible(1);
 		rp = rcu_torture_alloc();
 		if (rp == NULL)
 			continue;
 		rp->rtort_pipe_count = 0;
+		rcu_torture_writer_state = RTWS_DELAY;
 		udelay(torture_random(&rand) & 0x3ff);
+		rcu_torture_writer_state = RTWS_REPLACE;
 		old_rp = rcu_dereference_check(rcu_torture_current,
 					       current == writer_task);
 		rp->rtort_mbtest = 1;
@@ -716,35 +810,42 @@ rcu_torture_writer(void *arg)
 				i = RCU_TORTURE_PIPE_LEN;
 			atomic_inc(&rcu_torture_wcount[i]);
 			old_rp->rtort_pipe_count++;
-			if (gp_normal == gp_exp)
-				exp = !!(torture_random(&rand) & 0x80);
-			else
-				exp = gp_exp;
-			if (!exp) {
+			switch (synctype[torture_random(&rand) % nsynctypes]) {
+			case RTWS_DEF_FREE:
+				rcu_torture_writer_state = RTWS_DEF_FREE;
 				cur_ops->deferred_free(old_rp);
-			} else {
+				break;
+			case RTWS_EXP_SYNC:
+				rcu_torture_writer_state = RTWS_EXP_SYNC;
 				cur_ops->exp_sync();
-				list_add(&old_rp->rtort_free,
-					 &rcu_torture_removed);
-				list_for_each_entry_safe(rp, rp1,
-							 &rcu_torture_removed,
-							 rtort_free) {
-					i = rp->rtort_pipe_count;
-					if (i > RCU_TORTURE_PIPE_LEN)
-						i = RCU_TORTURE_PIPE_LEN;
-					atomic_inc(&rcu_torture_wcount[i]);
-					if (++rp->rtort_pipe_count >=
-					    RCU_TORTURE_PIPE_LEN) {
-						rp->rtort_mbtest = 0;
-						list_del(&rp->rtort_free);
-						rcu_torture_free(rp);
-					}
-				 }
+				rcu_torture_pipe_update(old_rp);
+				break;
+			case RTWS_COND_GET:
+				rcu_torture_writer_state = RTWS_COND_GET;
+				gp_snap = cur_ops->get_state();
+				i = torture_random(&rand) % 16;
+				if (i != 0)
+					schedule_timeout_interruptible(i);
+				udelay(torture_random(&rand) % 1000);
+				rcu_torture_writer_state = RTWS_COND_SYNC;
+				cur_ops->cond_sync(gp_snap);
+				rcu_torture_pipe_update(old_rp);
+				break;
+			case RTWS_SYNC:
+				rcu_torture_writer_state = RTWS_SYNC;
+				cur_ops->sync();
+				rcu_torture_pipe_update(old_rp);
+				break;
+			default:
+				WARN_ON_ONCE(1);
+				break;
 			}
 		}
 		rcutorture_record_progress(++rcu_torture_current_version);
+		rcu_torture_writer_state = RTWS_STUTTER;
 		stutter_wait("rcu_torture_writer");
 	} while (!torture_must_stop());
+	rcu_torture_writer_state = RTWS_STOPPING;
 	torture_kthread_stopping("rcu_torture_writer");
 	return 0;
 }
@@ -784,7 +885,7 @@ rcu_torture_fakewriter(void *arg)
 	return 0;
 }
 
-void rcutorture_trace_dump(void)
+static void rcutorture_trace_dump(void)
 {
 	static atomic_t beenhere = ATOMIC_INIT(0);
 
@@ -918,11 +1019,13 @@ rcu_torture_reader(void *arg)
 		__this_cpu_inc(rcu_torture_batch[completed]);
 		preempt_enable();
 		cur_ops->readunlock(idx);
-		schedule();
+		cond_resched();
 		stutter_wait("rcu_torture_reader");
 	} while (!torture_must_stop());
-	if (irqreader && cur_ops->irq_capable)
+	if (irqreader && cur_ops->irq_capable) {
 		del_timer_sync(&t);
+		destroy_timer_on_stack(&t);
+	}
 	torture_kthread_stopping("rcu_torture_reader");
 	return 0;
 }
@@ -937,6 +1040,7 @@ rcu_torture_printk(char *page)
 	int i;
 	long pipesummary[RCU_TORTURE_PIPE_LEN + 1] = { 0 };
 	long batchsummary[RCU_TORTURE_PIPE_LEN + 1] = { 0 };
+	static unsigned long rtcv_snap = ULONG_MAX;
 
 	for_each_possible_cpu(cpu) {
 		for (i = 0; i < RCU_TORTURE_PIPE_LEN + 1; i++) {
@@ -997,6 +1101,22 @@ rcu_torture_printk(char *page)
 	page += sprintf(page, "\n");
 	if (cur_ops->stats)
 		cur_ops->stats(page);
+	if (rtcv_snap == rcu_torture_current_version &&
+	    rcu_torture_current != NULL) {
+		int __maybe_unused flags;
+		unsigned long __maybe_unused gpnum;
+		unsigned long __maybe_unused completed;
+
+		rcutorture_get_gp_data(cur_ops->ttype,
+				       &flags, &gpnum, &completed);
+		page += sprintf(page,
+				"??? Writer stall state %d g%lu c%lu f%#x\n",
+				rcu_torture_writer_state,
+				gpnum, completed, flags);
+		show_rcu_gp_kthreads();
+		rcutorture_trace_dump();
+	}
+	rtcv_snap = rcu_torture_current_version;
 }
 
 /*
@@ -1146,7 +1266,7 @@ static int __init rcu_torture_stall_init(void)
 }
 
 /* Callback function for RCU barrier testing. */
-void rcu_torture_barrier_cbf(struct rcu_head *rcu)
+static void rcu_torture_barrier_cbf(struct rcu_head *rcu)
 {
 	atomic_inc(&barrier_cbs_invoked);
 }
@@ -1416,7 +1536,8 @@ rcu_torture_init(void)
 		&rcu_ops, &rcu_bh_ops, &rcu_busted_ops, &srcu_ops, &sched_ops,
 	};
 
-	torture_init_begin(torture_type, verbose, &rcutorture_runnable);
+	if (!torture_init_begin(torture_type, verbose, &rcutorture_runnable))
+		return -EBUSY;
 
 	/* Process args and tell the world that the torturer is on the job. */
 	for (i = 0; i < ARRAY_SIZE(torture_ops); i++) {
@@ -1441,10 +1562,13 @@ rcu_torture_init(void)
 	if (cur_ops->init)
 		cur_ops->init(); /* no "goto unwind" prior to this point!!! */
 
-	if (nreaders >= 0)
+	if (nreaders >= 0) {
 		nrealreaders = nreaders;
-	else
-		nrealreaders = 2 * num_online_cpus();
+	} else {
+		nrealreaders = num_online_cpus() - 1;
+		if (nrealreaders <= 0)
+			nrealreaders = 1;
+	}
 	rcu_torture_print_module_parms(cur_ops, "Start of test");
 
 	/* Set up the freelist. */
@@ -1533,7 +1657,8 @@ rcu_torture_init(void)
 		fqs_duration = 0;
 	if (fqs_duration) {
 		/* Create the fqs thread */
-		torture_create_kthread(rcu_torture_fqs, NULL, fqs_task);
+		firsterr = torture_create_kthread(rcu_torture_fqs, NULL,
+						  fqs_task);
 		if (firsterr)
 			goto unwind;
 	}
diff --git a/kernel/rcu/tiny_plugin.h b/kernel/rcu/tiny_plugin.h
index 43152852056..858c5656912 100644
--- a/kernel/rcu/tiny_plugin.h
+++ b/kernel/rcu/tiny_plugin.h
@@ -144,7 +144,7 @@ static void check_cpu_stall(struct rcu_ctrlblk *rcp)
 		return;
 	rcp->ticks_this_gp++;
 	j = jiffies;
-	js = rcp->jiffies_stall;
+	js = ACCESS_ONCE(rcp->jiffies_stall);
 	if (*rcp->curtail && ULONG_CMP_GE(j, js)) {
 		pr_err("INFO: %s stall on CPU (%lu ticks this GP) idle=%llx (t=%lu jiffies q=%ld)\n",
 		       rcp->name, rcp->ticks_this_gp, rcu_dynticks_nesting,
@@ -152,17 +152,17 @@ static void check_cpu_stall(struct rcu_ctrlblk *rcp)
 		dump_stack();
 	}
 	if (*rcp->curtail && ULONG_CMP_GE(j, js))
-		rcp->jiffies_stall = jiffies +
+		ACCESS_ONCE(rcp->jiffies_stall) = jiffies +
 			3 * rcu_jiffies_till_stall_check() + 3;
 	else if (ULONG_CMP_GE(j, js))
-		rcp->jiffies_stall = jiffies + rcu_jiffies_till_stall_check();
+		ACCESS_ONCE(rcp->jiffies_stall) = jiffies + rcu_jiffies_till_stall_check();
 }
 
 static void reset_cpu_stall_ticks(struct rcu_ctrlblk *rcp)
 {
 	rcp->ticks_this_gp = 0;
 	rcp->gp_start = jiffies;
-	rcp->jiffies_stall = jiffies + rcu_jiffies_till_stall_check();
+	ACCESS_ONCE(rcp->jiffies_stall) = jiffies + rcu_jiffies_till_stall_check();
 }
 
 static void check_cpu_stalls(void)
diff --git a/kernel/rcu/tree.c b/kernel/rcu/tree.c
index 88b4a1dcb58..f1ba77363fb 100644
--- a/kernel/rcu/tree.c
+++ b/kernel/rcu/tree.c
@@ -101,7 +101,7 @@ DEFINE_PER_CPU(struct rcu_data, sname##_data)
 RCU_STATE_INITIALIZER(rcu_sched, 's', call_rcu_sched);
 RCU_STATE_INITIALIZER(rcu_bh, 'b', call_rcu_bh);
 
-static struct rcu_state *rcu_state;
+static struct rcu_state *rcu_state_p;
 LIST_HEAD(rcu_struct_flavors);
 
 /* Increase (but not decrease) the CONFIG_RCU_FANOUT_LEAF at boot time. */
@@ -243,7 +243,7 @@ static ulong jiffies_till_next_fqs = ULONG_MAX;
 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,
+static bool 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 *rsp, bool *isidle,
@@ -271,6 +271,15 @@ long rcu_batches_completed_bh(void)
 EXPORT_SYMBOL_GPL(rcu_batches_completed_bh);
 
 /*
+ * Force a quiescent state.
+ */
+void rcu_force_quiescent_state(void)
+{
+	force_quiescent_state(rcu_state_p);
+}
+EXPORT_SYMBOL_GPL(rcu_force_quiescent_state);
+
+/*
  * Force a quiescent state for RCU BH.
  */
 void rcu_bh_force_quiescent_state(void)
@@ -280,6 +289,21 @@ void rcu_bh_force_quiescent_state(void)
 EXPORT_SYMBOL_GPL(rcu_bh_force_quiescent_state);
 
 /*
+ * Show the state of the grace-period kthreads.
+ */
+void show_rcu_gp_kthreads(void)
+{
+	struct rcu_state *rsp;
+
+	for_each_rcu_flavor(rsp) {
+		pr_info("%s: wait state: %d ->state: %#lx\n",
+			rsp->name, rsp->gp_state, rsp->gp_kthread->state);
+		/* sched_show_task(rsp->gp_kthread); */
+	}
+}
+EXPORT_SYMBOL_GPL(show_rcu_gp_kthreads);
+
+/*
  * Record the number of times rcutorture tests have been initiated and
  * terminated.  This information allows the debugfs tracing stats to be
  * correlated to the rcutorture messages, even when the rcutorture module
@@ -294,6 +318,39 @@ void rcutorture_record_test_transition(void)
 EXPORT_SYMBOL_GPL(rcutorture_record_test_transition);
 
 /*
+ * Send along grace-period-related data for rcutorture diagnostics.
+ */
+void rcutorture_get_gp_data(enum rcutorture_type test_type, int *flags,
+			    unsigned long *gpnum, unsigned long *completed)
+{
+	struct rcu_state *rsp = NULL;
+
+	switch (test_type) {
+	case RCU_FLAVOR:
+		rsp = rcu_state_p;
+		break;
+	case RCU_BH_FLAVOR:
+		rsp = &rcu_bh_state;
+		break;
+	case RCU_SCHED_FLAVOR:
+		rsp = &rcu_sched_state;
+		break;
+	default:
+		break;
+	}
+	if (rsp != NULL) {
+		*flags = ACCESS_ONCE(rsp->gp_flags);
+		*gpnum = ACCESS_ONCE(rsp->gpnum);
+		*completed = ACCESS_ONCE(rsp->completed);
+		return;
+	}
+	*flags = 0;
+	*gpnum = 0;
+	*completed = 0;
+}
+EXPORT_SYMBOL_GPL(rcutorture_get_gp_data);
+
+/*
  * Record the number of writer passes through the current rcutorture test.
  * This is also used to correlate debugfs tracing stats with the rcutorture
  * messages.
@@ -324,6 +381,28 @@ cpu_has_callbacks_ready_to_invoke(struct rcu_data *rdp)
 }
 
 /*
+ * Return the root node of the specified rcu_state structure.
+ */
+static struct rcu_node *rcu_get_root(struct rcu_state *rsp)
+{
+	return &rsp->node[0];
+}
+
+/*
+ * Is there any need for future grace periods?
+ * 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_future_needs_gp(struct rcu_state *rsp)
+{
+	struct rcu_node *rnp = rcu_get_root(rsp);
+	int idx = (ACCESS_ONCE(rnp->completed) + 1) & 0x1;
+	int *fp = &rnp->need_future_gp[idx];
+
+	return ACCESS_ONCE(*fp);
+}
+
+/*
  * Does the current CPU require a not-yet-started grace period?
  * The caller must have disabled interrupts to prevent races with
  * normal callback registry.
@@ -335,7 +414,7 @@ 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))
+	if (rcu_future_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. */
@@ -350,14 +429,6 @@ cpu_needs_another_gp(struct rcu_state *rsp, struct rcu_data *rdp)
 }
 
 /*
- * Return the root node of the specified rcu_state structure.
- */
-static struct rcu_node *rcu_get_root(struct rcu_state *rsp)
-{
-	return &rsp->node[0];
-}
-
-/*
  * rcu_eqs_enter_common - current CPU is moving towards extended quiescent state
  *
  * If the new value of the ->dynticks_nesting counter now is zero,
@@ -758,7 +829,12 @@ static int dyntick_save_progress_counter(struct rcu_data *rdp,
 {
 	rdp->dynticks_snap = atomic_add_return(0, &rdp->dynticks->dynticks);
 	rcu_sysidle_check_cpu(rdp, isidle, maxj);
-	return (rdp->dynticks_snap & 0x1) == 0;
+	if ((rdp->dynticks_snap & 0x1) == 0) {
+		trace_rcu_fqs(rdp->rsp->name, rdp->gpnum, rdp->cpu, TPS("dti"));
+		return 1;
+	} else {
+		return 0;
+	}
 }
 
 /*
@@ -834,7 +910,7 @@ static int rcu_implicit_dynticks_qs(struct rcu_data *rdp,
 	 * we will beat on the first one until it gets unstuck, then move
 	 * to the next.  Only do this for the primary flavor of RCU.
 	 */
-	if (rdp->rsp == rcu_state &&
+	if (rdp->rsp == rcu_state_p &&
 	    ULONG_CMP_GE(jiffies, rdp->rsp->jiffies_resched)) {
 		rdp->rsp->jiffies_resched += 5;
 		resched_cpu(rdp->cpu);
@@ -851,7 +927,7 @@ static void record_gp_stall_check_time(struct rcu_state *rsp)
 	rsp->gp_start = j;
 	smp_wmb(); /* Record start time before stall time. */
 	j1 = rcu_jiffies_till_stall_check();
-	rsp->jiffies_stall = j + j1;
+	ACCESS_ONCE(rsp->jiffies_stall) = j + j1;
 	rsp->jiffies_resched = j + j1 / 2;
 }
 
@@ -890,12 +966,12 @@ static void print_other_cpu_stall(struct rcu_state *rsp)
 	/* Only let one CPU complain about others per time interval. */
 
 	raw_spin_lock_irqsave(&rnp->lock, flags);
-	delta = jiffies - rsp->jiffies_stall;
+	delta = jiffies - ACCESS_ONCE(rsp->jiffies_stall);
 	if (delta < RCU_STALL_RAT_DELAY || !rcu_gp_in_progress(rsp)) {
 		raw_spin_unlock_irqrestore(&rnp->lock, flags);
 		return;
 	}
-	rsp->jiffies_stall = jiffies + 3 * rcu_jiffies_till_stall_check() + 3;
+	ACCESS_ONCE(rsp->jiffies_stall) = jiffies + 3 * rcu_jiffies_till_stall_check() + 3;
 	raw_spin_unlock_irqrestore(&rnp->lock, flags);
 
 	/*
@@ -932,9 +1008,9 @@ static void print_other_cpu_stall(struct rcu_state *rsp)
 	print_cpu_stall_info_end();
 	for_each_possible_cpu(cpu)
 		totqlen += per_cpu_ptr(rsp->rda, cpu)->qlen;
-	pr_cont("(detected by %d, t=%ld jiffies, g=%lu, c=%lu, q=%lu)\n",
+	pr_cont("(detected by %d, t=%ld jiffies, g=%ld, c=%ld, q=%lu)\n",
 	       smp_processor_id(), (long)(jiffies - rsp->gp_start),
-	       rsp->gpnum, rsp->completed, totqlen);
+	       (long)rsp->gpnum, (long)rsp->completed, totqlen);
 	if (ndetected == 0)
 		pr_err("INFO: Stall ended before state dump start\n");
 	else if (!trigger_all_cpu_backtrace())
@@ -947,12 +1023,6 @@ static void print_other_cpu_stall(struct rcu_state *rsp)
 	force_quiescent_state(rsp);  /* Kick them all. */
 }
 
-/*
- * This function really isn't for public consumption, but RCU is special in
- * that context switches can allow the state machine to make progress.
- */
-extern void resched_cpu(int cpu);
-
 static void print_cpu_stall(struct rcu_state *rsp)
 {
 	int cpu;
@@ -971,14 +1041,15 @@ static void print_cpu_stall(struct rcu_state *rsp)
 	print_cpu_stall_info_end();
 	for_each_possible_cpu(cpu)
 		totqlen += per_cpu_ptr(rsp->rda, cpu)->qlen;
-	pr_cont(" (t=%lu jiffies g=%lu c=%lu q=%lu)\n",
-		jiffies - rsp->gp_start, rsp->gpnum, rsp->completed, totqlen);
+	pr_cont(" (t=%lu jiffies g=%ld c=%ld q=%lu)\n",
+		jiffies - rsp->gp_start,
+		(long)rsp->gpnum, (long)rsp->completed, totqlen);
 	if (!trigger_all_cpu_backtrace())
 		dump_stack();
 
 	raw_spin_lock_irqsave(&rnp->lock, flags);
-	if (ULONG_CMP_GE(jiffies, rsp->jiffies_stall))
-		rsp->jiffies_stall = jiffies +
+	if (ULONG_CMP_GE(jiffies, ACCESS_ONCE(rsp->jiffies_stall)))
+		ACCESS_ONCE(rsp->jiffies_stall) = jiffies +
 				     3 * rcu_jiffies_till_stall_check() + 3;
 	raw_spin_unlock_irqrestore(&rnp->lock, flags);
 
@@ -1062,7 +1133,7 @@ void rcu_cpu_stall_reset(void)
 	struct rcu_state *rsp;
 
 	for_each_rcu_flavor(rsp)
-		rsp->jiffies_stall = jiffies + ULONG_MAX / 2;
+		ACCESS_ONCE(rsp->jiffies_stall) = jiffies + ULONG_MAX / 2;
 }
 
 /*
@@ -1123,15 +1194,18 @@ static void trace_rcu_future_gp(struct rcu_node *rnp, struct rcu_data *rdp,
 /*
  * 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.
+ * rcu_node structure's ->need_future_gp field.  Returns true if there
+ * is reason to awaken the grace-period kthread.
  *
  * 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)
+static bool __maybe_unused
+rcu_start_future_gp(struct rcu_node *rnp, struct rcu_data *rdp,
+		    unsigned long *c_out)
 {
 	unsigned long c;
 	int i;
+	bool ret = false;
 	struct rcu_node *rnp_root = rcu_get_root(rdp->rsp);
 
 	/*
@@ -1142,7 +1216,7 @@ rcu_start_future_gp(struct rcu_node *rnp, struct rcu_data *rdp)
 	trace_rcu_future_gp(rnp, rdp, c, TPS("Startleaf"));
 	if (rnp->need_future_gp[c & 0x1]) {
 		trace_rcu_future_gp(rnp, rdp, c, TPS("Prestartleaf"));
-		return c;
+		goto out;
 	}
 
 	/*
@@ -1156,7 +1230,7 @@ rcu_start_future_gp(struct rcu_node *rnp, struct rcu_data *rdp)
 	    ACCESS_ONCE(rnp->gpnum) != ACCESS_ONCE(rnp->completed)) {
 		rnp->need_future_gp[c & 0x1]++;
 		trace_rcu_future_gp(rnp, rdp, c, TPS("Startedleaf"));
-		return c;
+		goto out;
 	}
 
 	/*
@@ -1197,12 +1271,15 @@ rcu_start_future_gp(struct rcu_node *rnp, struct rcu_data *rdp)
 		trace_rcu_future_gp(rnp, rdp, c, TPS("Startedleafroot"));
 	} else {
 		trace_rcu_future_gp(rnp, rdp, c, TPS("Startedroot"));
-		rcu_start_gp_advanced(rdp->rsp, rnp_root, rdp);
+		ret = rcu_start_gp_advanced(rdp->rsp, rnp_root, rdp);
 	}
 unlock_out:
 	if (rnp != rnp_root)
 		raw_spin_unlock(&rnp_root->lock);
-	return c;
+out:
+	if (c_out != NULL)
+		*c_out = c;
+	return ret;
 }
 
 /*
@@ -1226,25 +1303,43 @@ static int rcu_future_gp_cleanup(struct rcu_state *rsp, struct rcu_node *rnp)
 }
 
 /*
+ * Awaken the grace-period kthread for the specified flavor of RCU.
+ * Don't do a self-awaken, and don't bother awakening when there is
+ * nothing for the grace-period kthread to do (as in several CPUs
+ * raced to awaken, and we lost), and finally don't try to awaken
+ * a kthread that has not yet been created.
+ */
+static void rcu_gp_kthread_wake(struct rcu_state *rsp)
+{
+	if (current == rsp->gp_kthread ||
+	    !ACCESS_ONCE(rsp->gp_flags) ||
+	    !rsp->gp_kthread)
+		return;
+	wake_up(&rsp->gp_wq);
+}
+
+/*
  * 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
  * since proven to be too conservative, which can happen if callbacks get
  * assigned a ->completed number while RCU is idle, but with reference to
  * a non-root rcu_node structure.  This function is idempotent, so it does
- * not hurt to call it repeatedly.
+ * not hurt to call it repeatedly.  Returns an flag saying that we should
+ * awaken the RCU grace-period kthread.
  *
  * The caller must hold rnp->lock with interrupts disabled.
  */
-static void rcu_accelerate_cbs(struct rcu_state *rsp, struct rcu_node *rnp,
+static bool rcu_accelerate_cbs(struct rcu_state *rsp, struct rcu_node *rnp,
 			       struct rcu_data *rdp)
 {
 	unsigned long c;
 	int i;
+	bool ret;
 
 	/* If the CPU has no callbacks, nothing to do. */
 	if (!rdp->nxttail[RCU_NEXT_TAIL] || !*rdp->nxttail[RCU_DONE_TAIL])
-		return;
+		return false;
 
 	/*
 	 * Starting from the sublist containing the callbacks most
@@ -1273,7 +1368,7 @@ static void rcu_accelerate_cbs(struct rcu_state *rsp, struct rcu_node *rnp,
 	 * be grouped into.
 	 */
 	if (++i >= RCU_NEXT_TAIL)
-		return;
+		return false;
 
 	/*
 	 * Assign all subsequent callbacks' ->completed number to the next
@@ -1285,13 +1380,14 @@ static void rcu_accelerate_cbs(struct rcu_state *rsp, struct rcu_node *rnp,
 		rdp->nxtcompleted[i] = c;
 	}
 	/* Record any needed additional grace periods. */
-	rcu_start_future_gp(rnp, rdp);
+	ret = rcu_start_future_gp(rnp, rdp, NULL);
 
 	/* Trace depending on how much we were able to accelerate. */
 	if (!*rdp->nxttail[RCU_WAIT_TAIL])
 		trace_rcu_grace_period(rsp->name, rdp->gpnum, TPS("AccWaitCB"));
 	else
 		trace_rcu_grace_period(rsp->name, rdp->gpnum, TPS("AccReadyCB"));
+	return ret;
 }
 
 /*
@@ -1300,17 +1396,18 @@ static void rcu_accelerate_cbs(struct rcu_state *rsp, struct rcu_node *rnp,
  * assign ->completed numbers to any callbacks in the RCU_NEXT_TAIL
  * sublist.  This function is idempotent, so it does not hurt to
  * invoke it repeatedly.  As long as it is not invoked -too- often...
+ * Returns true if the RCU grace-period kthread needs to be awakened.
  *
  * The caller must hold rnp->lock with interrupts disabled.
  */
-static void rcu_advance_cbs(struct rcu_state *rsp, struct rcu_node *rnp,
+static bool rcu_advance_cbs(struct rcu_state *rsp, struct rcu_node *rnp,
 			    struct rcu_data *rdp)
 {
 	int i, j;
 
 	/* If the CPU has no callbacks, nothing to do. */
 	if (!rdp->nxttail[RCU_NEXT_TAIL] || !*rdp->nxttail[RCU_DONE_TAIL])
-		return;
+		return false;
 
 	/*
 	 * Find all callbacks whose ->completed numbers indicate that they
@@ -1334,26 +1431,30 @@ static void rcu_advance_cbs(struct rcu_state *rsp, struct rcu_node *rnp,
 	}
 
 	/* Classify any remaining callbacks. */
-	rcu_accelerate_cbs(rsp, rnp, rdp);
+	return rcu_accelerate_cbs(rsp, rnp, rdp);
 }
 
 /*
  * Update CPU-local rcu_data state to record the beginnings and ends of
  * grace periods.  The caller must hold the ->lock of the leaf rcu_node
  * structure corresponding to the current CPU, and must have irqs disabled.
+ * Returns true if the grace-period kthread needs to be awakened.
  */
-static void __note_gp_changes(struct rcu_state *rsp, struct rcu_node *rnp, struct rcu_data *rdp)
+static bool __note_gp_changes(struct rcu_state *rsp, struct rcu_node *rnp,
+			      struct rcu_data *rdp)
 {
+	bool ret;
+
 	/* Handle the ends of any preceding grace periods first. */
 	if (rdp->completed == rnp->completed) {
 
 		/* No grace period end, so just accelerate recent callbacks. */
-		rcu_accelerate_cbs(rsp, rnp, rdp);
+		ret = rcu_accelerate_cbs(rsp, rnp, rdp);
 
 	} else {
 
 		/* Advance callbacks. */
-		rcu_advance_cbs(rsp, rnp, rdp);
+		ret = rcu_advance_cbs(rsp, rnp, rdp);
 
 		/* Remember that we saw this grace-period completion. */
 		rdp->completed = rnp->completed;
@@ -1372,11 +1473,13 @@ static void __note_gp_changes(struct rcu_state *rsp, struct rcu_node *rnp, struc
 		rdp->qs_pending = !!(rnp->qsmask & rdp->grpmask);
 		zero_cpu_stall_ticks(rdp);
 	}
+	return ret;
 }
 
 static void note_gp_changes(struct rcu_state *rsp, struct rcu_data *rdp)
 {
 	unsigned long flags;
+	bool needwake;
 	struct rcu_node *rnp;
 
 	local_irq_save(flags);
@@ -1388,8 +1491,10 @@ static void note_gp_changes(struct rcu_state *rsp, struct rcu_data *rdp)
 		return;
 	}
 	smp_mb__after_unlock_lock();
-	__note_gp_changes(rsp, rnp, rdp);
+	needwake = __note_gp_changes(rsp, rnp, rdp);
 	raw_spin_unlock_irqrestore(&rnp->lock, flags);
+	if (needwake)
+		rcu_gp_kthread_wake(rsp);
 }
 
 /*
@@ -1403,12 +1508,12 @@ static int rcu_gp_init(struct rcu_state *rsp)
 	rcu_bind_gp_kthread();
 	raw_spin_lock_irq(&rnp->lock);
 	smp_mb__after_unlock_lock();
-	if (rsp->gp_flags == 0) {
+	if (!ACCESS_ONCE(rsp->gp_flags)) {
 		/* Spurious wakeup, tell caller to go back to sleep.  */
 		raw_spin_unlock_irq(&rnp->lock);
 		return 0;
 	}
-	rsp->gp_flags = 0; /* Clear all flags: New grace period. */
+	ACCESS_ONCE(rsp->gp_flags) = 0; /* Clear all flags: New grace period. */
 
 	if (WARN_ON_ONCE(rcu_gp_in_progress(rsp))) {
 		/*
@@ -1453,7 +1558,7 @@ static int rcu_gp_init(struct rcu_state *rsp)
 		WARN_ON_ONCE(rnp->completed != rsp->completed);
 		ACCESS_ONCE(rnp->completed) = rsp->completed;
 		if (rnp == rdp->mynode)
-			__note_gp_changes(rsp, rnp, rdp);
+			(void)__note_gp_changes(rsp, rnp, rdp);
 		rcu_preempt_boost_start_gp(rnp);
 		trace_rcu_grace_period_init(rsp->name, rnp->gpnum,
 					    rnp->level, rnp->grplo,
@@ -1501,7 +1606,7 @@ static int rcu_gp_fqs(struct rcu_state *rsp, int fqs_state_in)
 	if (ACCESS_ONCE(rsp->gp_flags) & RCU_GP_FLAG_FQS) {
 		raw_spin_lock_irq(&rnp->lock);
 		smp_mb__after_unlock_lock();
-		rsp->gp_flags &= ~RCU_GP_FLAG_FQS;
+		ACCESS_ONCE(rsp->gp_flags) &= ~RCU_GP_FLAG_FQS;
 		raw_spin_unlock_irq(&rnp->lock);
 	}
 	return fqs_state;
@@ -1513,6 +1618,7 @@ static 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;
+	bool needgp = false;
 	int nocb = 0;
 	struct rcu_data *rdp;
 	struct rcu_node *rnp = rcu_get_root(rsp);
@@ -1548,7 +1654,7 @@ static void rcu_gp_cleanup(struct rcu_state *rsp)
 		ACCESS_ONCE(rnp->completed) = rsp->gpnum;
 		rdp = this_cpu_ptr(rsp->rda);
 		if (rnp == rdp->mynode)
-			__note_gp_changes(rsp, rnp, rdp);
+			needgp = __note_gp_changes(rsp, rnp, rdp) || needgp;
 		/* smp_mb() provided by prior unlock-lock pair. */
 		nocb += rcu_future_gp_cleanup(rsp, rnp);
 		raw_spin_unlock_irq(&rnp->lock);
@@ -1564,9 +1670,10 @@ static void rcu_gp_cleanup(struct rcu_state *rsp)
 	trace_rcu_grace_period(rsp->name, rsp->completed, TPS("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 = RCU_GP_FLAG_INIT;
+	/* Advance CBs to reduce false positives below. */
+	needgp = rcu_advance_cbs(rsp, rnp, rdp) || needgp;
+	if (needgp || cpu_needs_another_gp(rsp, rdp)) {
+		ACCESS_ONCE(rsp->gp_flags) = RCU_GP_FLAG_INIT;
 		trace_rcu_grace_period(rsp->name,
 				       ACCESS_ONCE(rsp->gpnum),
 				       TPS("newreq"));
@@ -1593,6 +1700,7 @@ static int __noreturn rcu_gp_kthread(void *arg)
 			trace_rcu_grace_period(rsp->name,
 					       ACCESS_ONCE(rsp->gpnum),
 					       TPS("reqwait"));
+			rsp->gp_state = RCU_GP_WAIT_GPS;
 			wait_event_interruptible(rsp->gp_wq,
 						 ACCESS_ONCE(rsp->gp_flags) &
 						 RCU_GP_FLAG_INIT);
@@ -1620,6 +1728,7 @@ static int __noreturn rcu_gp_kthread(void *arg)
 			trace_rcu_grace_period(rsp->name,
 					       ACCESS_ONCE(rsp->gpnum),
 					       TPS("fqswait"));
+			rsp->gp_state = RCU_GP_WAIT_FQS;
 			ret = wait_event_interruptible_timeout(rsp->gp_wq,
 					((gf = ACCESS_ONCE(rsp->gp_flags)) &
 					 RCU_GP_FLAG_FQS) ||
@@ -1665,14 +1774,6 @@ static int __noreturn rcu_gp_kthread(void *arg)
 	}
 }
 
-static void rsp_wakeup(struct irq_work *work)
-{
-	struct rcu_state *rsp = container_of(work, struct rcu_state, wakeup_work);
-
-	/* Wake up rcu_gp_kthread() to start the grace period. */
-	wake_up(&rsp->gp_wq);
-}
-
 /*
  * Start a new RCU grace period if warranted, re-initializing the hierarchy
  * in preparation for detecting the next grace period.  The caller must hold
@@ -1681,8 +1782,10 @@ static void rsp_wakeup(struct irq_work *work)
  * 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.
+ *
+ * Returns true if the grace-period kthread must be awakened.
  */
-static void
+static bool
 rcu_start_gp_advanced(struct rcu_state *rsp, struct rcu_node *rnp,
 		      struct rcu_data *rdp)
 {
@@ -1693,20 +1796,18 @@ rcu_start_gp_advanced(struct rcu_state *rsp, struct rcu_node *rnp,
 		 * or a grace period is already in progress.
 		 * Either way, don't start a new grace period.
 		 */
-		return;
+		return false;
 	}
-	rsp->gp_flags = RCU_GP_FLAG_INIT;
+	ACCESS_ONCE(rsp->gp_flags) = RCU_GP_FLAG_INIT;
 	trace_rcu_grace_period(rsp->name, ACCESS_ONCE(rsp->gpnum),
 			       TPS("newreq"));
 
 	/*
 	 * We can't do wakeups while holding the rnp->lock, as that
 	 * could cause possible deadlocks with the rq->lock. Defer
-	 * the wakeup to interrupt context.  And don't bother waking
-	 * up the running kthread.
+	 * the wakeup to our caller.
 	 */
-	if (current != rsp->gp_kthread)
-		irq_work_queue(&rsp->wakeup_work);
+	return true;
 }
 
 /*
@@ -1715,12 +1816,14 @@ rcu_start_gp_advanced(struct rcu_state *rsp, struct rcu_node *rnp,
  * 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.
+ *
+ * Returns true if the grace-period kthread needs to be awakened.
  */
-static void
-rcu_start_gp(struct rcu_state *rsp)
+static bool rcu_start_gp(struct rcu_state *rsp)
 {
 	struct rcu_data *rdp = this_cpu_ptr(rsp->rda);
 	struct rcu_node *rnp = rcu_get_root(rsp);
+	bool ret = false;
 
 	/*
 	 * If there is no grace period in progress right now, any
@@ -1730,8 +1833,9 @@ rcu_start_gp(struct rcu_state *rsp)
 	 * 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);
+	ret = rcu_advance_cbs(rsp, rnp, rdp) || ret;
+	ret = rcu_start_gp_advanced(rsp, rnp, rdp) || ret;
+	return ret;
 }
 
 /*
@@ -1820,6 +1924,7 @@ rcu_report_qs_rdp(int cpu, struct rcu_state *rsp, struct rcu_data *rdp)
 {
 	unsigned long flags;
 	unsigned long mask;
+	bool needwake;
 	struct rcu_node *rnp;
 
 	rnp = rdp->mynode;
@@ -1848,9 +1953,11 @@ rcu_report_qs_rdp(int cpu, struct rcu_state *rsp, struct rcu_data *rdp)
 		 * This GP can't end until cpu checks in, so all of our
 		 * callbacks can be processed during the next GP.
 		 */
-		rcu_accelerate_cbs(rsp, rnp, rdp);
+		needwake = rcu_accelerate_cbs(rsp, rnp, rdp);
 
 		rcu_report_qs_rnp(mask, rsp, rnp, flags); /* rlses rnp->lock */
+		if (needwake)
+			rcu_gp_kthread_wake(rsp);
 	}
 }
 
@@ -1951,7 +2058,7 @@ rcu_send_cbs_to_orphanage(int cpu, struct rcu_state *rsp,
 static void rcu_adopt_orphan_cbs(struct rcu_state *rsp, unsigned long flags)
 {
 	int i;
-	struct rcu_data *rdp = __this_cpu_ptr(rsp->rda);
+	struct rcu_data *rdp = raw_cpu_ptr(rsp->rda);
 
 	/* No-CBs CPUs are handled specially. */
 	if (rcu_nocb_adopt_orphan_cbs(rsp, rdp, flags))
@@ -2320,7 +2427,7 @@ static void force_quiescent_state(struct rcu_state *rsp)
 		raw_spin_unlock_irqrestore(&rnp_old->lock, flags);
 		return;  /* Someone beat us to it. */
 	}
-	rsp->gp_flags |= RCU_GP_FLAG_FQS;
+	ACCESS_ONCE(rsp->gp_flags) |= RCU_GP_FLAG_FQS;
 	raw_spin_unlock_irqrestore(&rnp_old->lock, flags);
 	wake_up(&rsp->gp_wq);  /* Memory barrier implied by wake_up() path. */
 }
@@ -2334,7 +2441,8 @@ static void
 __rcu_process_callbacks(struct rcu_state *rsp)
 {
 	unsigned long flags;
-	struct rcu_data *rdp = __this_cpu_ptr(rsp->rda);
+	bool needwake;
+	struct rcu_data *rdp = raw_cpu_ptr(rsp->rda);
 
 	WARN_ON_ONCE(rdp->beenonline == 0);
 
@@ -2345,8 +2453,10 @@ __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);
+		needwake = rcu_start_gp(rsp);
 		raw_spin_unlock_irqrestore(&rcu_get_root(rsp)->lock, flags);
+		if (needwake)
+			rcu_gp_kthread_wake(rsp);
 	} else {
 		local_irq_restore(flags);
 	}
@@ -2404,6 +2514,8 @@ static void invoke_rcu_core(void)
 static void __call_rcu_core(struct rcu_state *rsp, struct rcu_data *rdp,
 			    struct rcu_head *head, unsigned long flags)
 {
+	bool needwake;
+
 	/*
 	 * If called from an extended quiescent state, invoke the RCU
 	 * core in order to force a re-evaluation of RCU's idleness.
@@ -2433,8 +2545,10 @@ static void __call_rcu_core(struct rcu_state *rsp, struct rcu_data *rdp,
 
 			raw_spin_lock(&rnp_root->lock);
 			smp_mb__after_unlock_lock();
-			rcu_start_gp(rsp);
+			needwake = rcu_start_gp(rsp);
 			raw_spin_unlock(&rnp_root->lock);
+			if (needwake)
+				rcu_gp_kthread_wake(rsp);
 		} else {
 			/* Give the grace period a kick. */
 			rdp->blimit = LONG_MAX;
@@ -2537,6 +2651,20 @@ void call_rcu_bh(struct rcu_head *head, void (*func)(struct rcu_head *rcu))
 EXPORT_SYMBOL_GPL(call_rcu_bh);
 
 /*
+ * Queue an RCU callback for lazy invocation after a grace period.
+ * This will likely be later named something like "call_rcu_lazy()",
+ * but this change will require some way of tagging the lazy RCU
+ * callbacks in the list of pending callbacks. Until then, this
+ * function may only be called from __kfree_rcu().
+ */
+void kfree_call_rcu(struct rcu_head *head,
+		    void (*func)(struct rcu_head *rcu))
+{
+	__call_rcu(head, func, rcu_state_p, -1, 1);
+}
+EXPORT_SYMBOL_GPL(kfree_call_rcu);
+
+/*
  * Because a context switch is a grace period for RCU-sched and RCU-bh,
  * any blocking grace-period wait automatically implies a grace period
  * if there is only one CPU online at any point time during execution
@@ -2659,7 +2787,7 @@ unsigned long get_state_synchronize_rcu(void)
 	 * time-consuming work between get_state_synchronize_rcu()
 	 * and cond_synchronize_rcu().
 	 */
-	return smp_load_acquire(&rcu_state->gpnum);
+	return smp_load_acquire(&rcu_state_p->gpnum);
 }
 EXPORT_SYMBOL_GPL(get_state_synchronize_rcu);
 
@@ -2685,7 +2813,7 @@ void cond_synchronize_rcu(unsigned long oldstate)
 	 * Ensure that this load happens before any RCU-destructive
 	 * actions the caller might carry out after we return.
 	 */
-	newstate = smp_load_acquire(&rcu_state->completed);
+	newstate = smp_load_acquire(&rcu_state_p->completed);
 	if (ULONG_CMP_GE(oldstate, newstate))
 		synchronize_rcu();
 }
@@ -2988,7 +3116,7 @@ static void rcu_barrier_callback(struct rcu_head *rhp)
 static void rcu_barrier_func(void *type)
 {
 	struct rcu_state *rsp = type;
-	struct rcu_data *rdp = __this_cpu_ptr(rsp->rda);
+	struct rcu_data *rdp = raw_cpu_ptr(rsp->rda);
 
 	_rcu_barrier_trace(rsp, "IRQ", -1, rsp->n_barrier_done);
 	atomic_inc(&rsp->barrier_cpu_count);
@@ -3160,7 +3288,7 @@ rcu_boot_init_percpu_data(int cpu, struct rcu_state *rsp)
  * that this CPU cannot possibly have any RCU callbacks in flight yet.
  */
 static void
-rcu_init_percpu_data(int cpu, struct rcu_state *rsp, int preemptible)
+rcu_init_percpu_data(int cpu, struct rcu_state *rsp)
 {
 	unsigned long flags;
 	unsigned long mask;
@@ -3173,7 +3301,6 @@ rcu_init_percpu_data(int cpu, struct rcu_state *rsp, int preemptible)
 	/* Set up local state, ensuring consistent view of global state. */
 	raw_spin_lock_irqsave(&rnp->lock, flags);
 	rdp->beenonline = 1;	 /* We have now been online. */
-	rdp->preemptible = preemptible;
 	rdp->qlen_last_fqs_check = 0;
 	rdp->n_force_qs_snap = rsp->n_force_qs;
 	rdp->blimit = blimit;
@@ -3217,8 +3344,7 @@ static void rcu_prepare_cpu(int cpu)
 	struct rcu_state *rsp;
 
 	for_each_rcu_flavor(rsp)
-		rcu_init_percpu_data(cpu, rsp,
-				     strcmp(rsp->name, "rcu_preempt") == 0);
+		rcu_init_percpu_data(cpu, rsp);
 }
 
 /*
@@ -3228,7 +3354,7 @@ static int rcu_cpu_notify(struct notifier_block *self,
 				    unsigned long action, void *hcpu)
 {
 	long cpu = (long)hcpu;
-	struct rcu_data *rdp = per_cpu_ptr(rcu_state->rda, cpu);
+	struct rcu_data *rdp = per_cpu_ptr(rcu_state_p->rda, cpu);
 	struct rcu_node *rnp = rdp->mynode;
 	struct rcu_state *rsp;
 
@@ -3402,8 +3528,8 @@ static void __init rcu_init_one(struct rcu_state *rsp,
 			rnp->qsmaskinit = 0;
 			rnp->grplo = j * cpustride;
 			rnp->grphi = (j + 1) * cpustride - 1;
-			if (rnp->grphi >= NR_CPUS)
-				rnp->grphi = NR_CPUS - 1;
+			if (rnp->grphi >= nr_cpu_ids)
+				rnp->grphi = nr_cpu_ids - 1;
 			if (i == 0) {
 				rnp->grpnum = 0;
 				rnp->grpmask = 0;
@@ -3422,7 +3548,6 @@ static void __init rcu_init_one(struct rcu_state *rsp,
 
 	rsp->rda = rda;
 	init_waitqueue_head(&rsp->gp_wq);
-	init_irq_work(&rsp->wakeup_work, rsp_wakeup);
 	rnp = rsp->level[rcu_num_lvls - 1];
 	for_each_possible_cpu(i) {
 		while (i > rnp->grphi)
diff --git a/kernel/rcu/tree.h b/kernel/rcu/tree.h
index 75dc3c39a02..bf2c1e66969 100644
--- a/kernel/rcu/tree.h
+++ b/kernel/rcu/tree.h
@@ -252,7 +252,6 @@ struct rcu_data {
 	bool		passed_quiesce;	/* User-mode/idle loop etc. */
 	bool		qs_pending;	/* Core waits for quiesc state. */
 	bool		beenonline;	/* CPU online at least once. */
-	bool		preemptible;	/* Preemptible RCU? */
 	struct rcu_node *mynode;	/* This CPU's leaf of hierarchy */
 	unsigned long grpmask;		/* Mask to apply to leaf qsmask. */
 #ifdef CONFIG_RCU_CPU_STALL_INFO
@@ -406,7 +405,8 @@ struct rcu_state {
 	unsigned long completed;		/* # of last completed gp. */
 	struct task_struct *gp_kthread;		/* Task for grace periods. */
 	wait_queue_head_t gp_wq;		/* Where GP task waits. */
-	int gp_flags;				/* Commands for GP task. */
+	short gp_flags;				/* Commands for GP task. */
+	short gp_state;				/* GP kthread sleep state. */
 
 	/* End of fields guarded by root rcu_node's lock. */
 
@@ -462,13 +462,17 @@ struct rcu_state {
 	const char *name;			/* Name of structure. */
 	char abbr;				/* Abbreviated name. */
 	struct list_head flavors;		/* List of RCU flavors. */
-	struct irq_work wakeup_work;		/* Postponed wakeups */
 };
 
 /* Values for rcu_state structure's gp_flags field. */
 #define RCU_GP_FLAG_INIT 0x1	/* Need grace-period initialization. */
 #define RCU_GP_FLAG_FQS  0x2	/* Need grace-period quiescent-state forcing. */
 
+/* Values for rcu_state structure's gp_flags field. */
+#define RCU_GP_WAIT_INIT 0	/* Initial state. */
+#define RCU_GP_WAIT_GPS  1	/* Wait for grace-period start. */
+#define RCU_GP_WAIT_FQS  2	/* Wait for force-quiescent-state time. */
+
 extern struct list_head rcu_struct_flavors;
 
 /* Sequence through rcu_state structures for each RCU flavor. */
@@ -547,7 +551,6 @@ 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);
diff --git a/kernel/rcu/tree_plugin.h b/kernel/rcu/tree_plugin.h
index 56db2f853e4..cbc2c45265e 100644
--- a/kernel/rcu/tree_plugin.h
+++ b/kernel/rcu/tree_plugin.h
@@ -116,7 +116,7 @@ static void __init rcu_bootup_announce_oddness(void)
 #ifdef CONFIG_TREE_PREEMPT_RCU
 
 RCU_STATE_INITIALIZER(rcu_preempt, 'p', call_rcu);
-static struct rcu_state *rcu_state = &rcu_preempt_state;
+static struct rcu_state *rcu_state_p = &rcu_preempt_state;
 
 static int rcu_preempted_readers_exp(struct rcu_node *rnp);
 
@@ -149,15 +149,6 @@ long rcu_batches_completed(void)
 EXPORT_SYMBOL_GPL(rcu_batches_completed);
 
 /*
- * Force a quiescent state for preemptible RCU.
- */
-void rcu_force_quiescent_state(void)
-{
-	force_quiescent_state(&rcu_preempt_state);
-}
-EXPORT_SYMBOL_GPL(rcu_force_quiescent_state);
-
-/*
  * Record a preemptible-RCU quiescent state for the specified CPU.  Note
  * that this just means that the task currently running on the CPU is
  * not in a quiescent state.  There might be any number of tasks blocked
@@ -688,20 +679,6 @@ void call_rcu(struct rcu_head *head, void (*func)(struct rcu_head *rcu))
 }
 EXPORT_SYMBOL_GPL(call_rcu);
 
-/*
- * Queue an RCU callback for lazy invocation after a grace period.
- * This will likely be later named something like "call_rcu_lazy()",
- * but this change will require some way of tagging the lazy RCU
- * callbacks in the list of pending callbacks.  Until then, this
- * function may only be called from __kfree_rcu().
- */
-void kfree_call_rcu(struct rcu_head *head,
-		    void (*func)(struct rcu_head *rcu))
-{
-	__call_rcu(head, func, &rcu_preempt_state, -1, 1);
-}
-EXPORT_SYMBOL_GPL(kfree_call_rcu);
-
 /**
  * synchronize_rcu - wait until a grace period has elapsed.
  *
@@ -970,7 +947,7 @@ void exit_rcu(void)
 
 #else /* #ifdef CONFIG_TREE_PREEMPT_RCU */
 
-static struct rcu_state *rcu_state = &rcu_sched_state;
+static struct rcu_state *rcu_state_p = &rcu_sched_state;
 
 /*
  * Tell them what RCU they are running.
@@ -991,16 +968,6 @@ long rcu_batches_completed(void)
 EXPORT_SYMBOL_GPL(rcu_batches_completed);
 
 /*
- * Force a quiescent state for RCU, which, because there is no preemptible
- * RCU, becomes the same as rcu-sched.
- */
-void rcu_force_quiescent_state(void)
-{
-	rcu_sched_force_quiescent_state();
-}
-EXPORT_SYMBOL_GPL(rcu_force_quiescent_state);
-
-/*
  * Because preemptible RCU does not exist, we never have to check for
  * CPUs being in quiescent states.
  */
@@ -1080,22 +1047,6 @@ static void rcu_preempt_check_callbacks(int cpu)
 }
 
 /*
- * Queue an RCU callback for lazy invocation after a grace period.
- * This will likely be later named something like "call_rcu_lazy()",
- * but this change will require some way of tagging the lazy RCU
- * callbacks in the list of pending callbacks.  Until then, this
- * function may only be called from __kfree_rcu().
- *
- * Because there is no preemptible RCU, we use RCU-sched instead.
- */
-void kfree_call_rcu(struct rcu_head *head,
-		    void (*func)(struct rcu_head *rcu))
-{
-	__call_rcu(head, func, &rcu_sched_state, -1, 1);
-}
-EXPORT_SYMBOL_GPL(kfree_call_rcu);
-
-/*
  * Wait for an rcu-preempt grace period, but make it happen quickly.
  * But because preemptible RCU does not exist, map to rcu-sched.
  */
@@ -1517,11 +1468,11 @@ static int __init rcu_spawn_kthreads(void)
 	for_each_possible_cpu(cpu)
 		per_cpu(rcu_cpu_has_work, cpu) = 0;
 	BUG_ON(smpboot_register_percpu_thread(&rcu_cpu_thread_spec));
-	rnp = rcu_get_root(rcu_state);
-	(void)rcu_spawn_one_boost_kthread(rcu_state, rnp);
+	rnp = rcu_get_root(rcu_state_p);
+	(void)rcu_spawn_one_boost_kthread(rcu_state_p, rnp);
 	if (NUM_RCU_NODES > 1) {
-		rcu_for_each_leaf_node(rcu_state, rnp)
-			(void)rcu_spawn_one_boost_kthread(rcu_state, rnp);
+		rcu_for_each_leaf_node(rcu_state_p, rnp)
+			(void)rcu_spawn_one_boost_kthread(rcu_state_p, rnp);
 	}
 	return 0;
 }
@@ -1529,12 +1480,12 @@ early_initcall(rcu_spawn_kthreads);
 
 static void rcu_prepare_kthreads(int cpu)
 {
-	struct rcu_data *rdp = per_cpu_ptr(rcu_state->rda, cpu);
+	struct rcu_data *rdp = per_cpu_ptr(rcu_state_p->rda, cpu);
 	struct rcu_node *rnp = rdp->mynode;
 
 	/* Fire up the incoming CPU's kthread and leaf rcu_node kthread. */
 	if (rcu_scheduler_fully_active)
-		(void)rcu_spawn_one_boost_kthread(rcu_state, rnp);
+		(void)rcu_spawn_one_boost_kthread(rcu_state_p, rnp);
 }
 
 #else /* #ifdef CONFIG_RCU_BOOST */
@@ -1744,6 +1695,7 @@ int rcu_needs_cpu(int cpu, unsigned long *dj)
 static void rcu_prepare_for_idle(int cpu)
 {
 #ifndef CONFIG_RCU_NOCB_CPU_ALL
+	bool needwake;
 	struct rcu_data *rdp;
 	struct rcu_dynticks *rdtp = &per_cpu(rcu_dynticks, cpu);
 	struct rcu_node *rnp;
@@ -1792,8 +1744,10 @@ static void rcu_prepare_for_idle(int cpu)
 		rnp = rdp->mynode;
 		raw_spin_lock(&rnp->lock); /* irqs already disabled. */
 		smp_mb__after_unlock_lock();
-		rcu_accelerate_cbs(rsp, rnp, rdp);
+		needwake = rcu_accelerate_cbs(rsp, rnp, rdp);
 		raw_spin_unlock(&rnp->lock); /* irqs remain disabled. */
+		if (needwake)
+			rcu_gp_kthread_wake(rsp);
 	}
 #endif /* #ifndef CONFIG_RCU_NOCB_CPU_ALL */
 }
@@ -1855,7 +1809,7 @@ static void rcu_oom_notify_cpu(void *unused)
 	struct rcu_data *rdp;
 
 	for_each_rcu_flavor(rsp) {
-		rdp = __this_cpu_ptr(rsp->rda);
+		rdp = raw_cpu_ptr(rsp->rda);
 		if (rdp->qlen_lazy != 0) {
 			atomic_inc(&oom_callback_count);
 			rsp->call(&rdp->oom_head, rcu_oom_callback);
@@ -1997,7 +1951,7 @@ static void increment_cpu_stall_ticks(void)
 	struct rcu_state *rsp;
 
 	for_each_rcu_flavor(rsp)
-		__this_cpu_ptr(rsp->rda)->ticks_this_gp++;
+		raw_cpu_inc(rsp->rda->ticks_this_gp);
 }
 
 #else /* #ifdef CONFIG_RCU_CPU_STALL_INFO */
@@ -2068,19 +2022,6 @@ 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.
  */
@@ -2109,7 +2050,7 @@ static void rcu_init_one_nocb(struct rcu_node *rnp)
 }
 
 #ifndef CONFIG_RCU_NOCB_CPU_ALL
-/* Is the specified CPU a no-CPUs CPU? */
+/* Is the specified CPU a no-CBs CPU? */
 bool rcu_is_nocb_cpu(int cpu)
 {
 	if (have_rcu_nocb_mask)
@@ -2243,12 +2184,15 @@ static void rcu_nocb_wait_gp(struct rcu_data *rdp)
 	unsigned long c;
 	bool d;
 	unsigned long flags;
+	bool needwake;
 	struct rcu_node *rnp = rdp->mynode;
 
 	raw_spin_lock_irqsave(&rnp->lock, flags);
 	smp_mb__after_unlock_lock();
-	c = rcu_start_future_gp(rnp, rdp);
+	needwake = rcu_start_future_gp(rnp, rdp, &c);
 	raw_spin_unlock_irqrestore(&rnp->lock, flags);
+	if (needwake)
+		rcu_gp_kthread_wake(rdp->rsp);
 
 	/*
 	 * Wait for the grace period.  Do so interruptibly to avoid messing
@@ -2402,11 +2346,6 @@ static bool init_nocb_callback_list(struct rcu_data *rdp)
 
 #else /* #ifdef CONFIG_RCU_NOCB_CPU */
 
-static int rcu_nocb_needs_gp(struct rcu_state *rsp)
-{
-	return 0;
-}
-
 static void rcu_nocb_gp_cleanup(struct rcu_state *rsp, struct rcu_node *rnp)
 {
 }
@@ -2657,20 +2596,6 @@ static bool is_sysidle_rcu_state(struct rcu_state *rsp)
 }
 
 /*
- * Bind the grace-period kthread for the sysidle flavor of RCU to the
- * timekeeping CPU.
- */
-static void rcu_bind_gp_kthread(void)
-{
-	int cpu = ACCESS_ONCE(tick_do_timer_cpu);
-
-	if (cpu < 0 || cpu >= nr_cpu_ids)
-		return;
-	if (raw_smp_processor_id() != cpu)
-		set_cpus_allowed_ptr(current, cpumask_of(cpu));
-}
-
-/*
  * Return a delay in jiffies based on the number of CPUs, rcu_node
  * leaf fanout, and jiffies tick rate.  The idea is to allow larger
  * systems more time to transition to full-idle state in order to
@@ -2734,7 +2659,8 @@ static void rcu_sysidle(unsigned long j)
 static void rcu_sysidle_cancel(void)
 {
 	smp_mb();
-	ACCESS_ONCE(full_sysidle_state) = RCU_SYSIDLE_NOT;
+	if (full_sysidle_state > RCU_SYSIDLE_SHORT)
+		ACCESS_ONCE(full_sysidle_state) = RCU_SYSIDLE_NOT;
 }
 
 /*
@@ -2880,10 +2806,6 @@ static bool is_sysidle_rcu_state(struct rcu_state *rsp)
 	return false;
 }
 
-static void rcu_bind_gp_kthread(void)
-{
-}
-
 static void rcu_sysidle_report_gp(struct rcu_state *rsp, int isidle,
 				  unsigned long maxj)
 {
@@ -2914,3 +2836,19 @@ static bool rcu_nohz_full_cpu(struct rcu_state *rsp)
 #endif /* #ifdef CONFIG_NO_HZ_FULL */
 	return 0;
 }
+
+/*
+ * Bind the grace-period kthread for the sysidle flavor of RCU to the
+ * timekeeping CPU.
+ */
+static void rcu_bind_gp_kthread(void)
+{
+#ifdef CONFIG_NO_HZ_FULL
+	int cpu = ACCESS_ONCE(tick_do_timer_cpu);
+
+	if (cpu < 0 || cpu >= nr_cpu_ids)
+		return;
+	if (raw_smp_processor_id() != cpu)
+		set_cpus_allowed_ptr(current, cpumask_of(cpu));
+#endif /* #ifdef CONFIG_NO_HZ_FULL */
+}
diff --git a/kernel/rcu/update.c b/kernel/rcu/update.c
index 4c0a9b0af46..a2aeb4df0f6 100644
--- a/kernel/rcu/update.c
+++ b/kernel/rcu/update.c
@@ -320,6 +320,18 @@ int rcu_jiffies_till_stall_check(void)
 	return till_stall_check * HZ + RCU_STALL_DELAY_DELTA;
 }
 
+void rcu_sysrq_start(void)
+{
+	if (!rcu_cpu_stall_suppress)
+		rcu_cpu_stall_suppress = 2;
+}
+
+void rcu_sysrq_end(void)
+{
+	if (rcu_cpu_stall_suppress == 2)
+		rcu_cpu_stall_suppress = 0;
+}
+
 static int rcu_panic(struct notifier_block *this, unsigned long ev, void *ptr)
 {
 	rcu_cpu_stall_suppress = 1;
@@ -338,3 +350,21 @@ static int __init check_cpu_stall_init(void)
 early_initcall(check_cpu_stall_init);
 
 #endif /* #ifdef CONFIG_RCU_STALL_COMMON */
+
+/*
+ * Hooks for cond_resched() and friends to avoid RCU CPU stall warnings.
+ */
+
+DEFINE_PER_CPU(int, rcu_cond_resched_count);
+
+/*
+ * Report a set of RCU quiescent states, for use by cond_resched()
+ * and friends.  Out of line due to being called infrequently.
+ */
+void rcu_resched(void)
+{
+	preempt_disable();
+	__this_cpu_write(rcu_cond_resched_count, 0);
+	rcu_note_context_switch(smp_processor_id());
+	preempt_enable();
+}
diff --git a/kernel/resource.c b/kernel/resource.c
index 8957d686e29..3c2237ac32d 100644
--- a/kernel/resource.c
+++ b/kernel/resource.c
@@ -1288,13 +1288,10 @@ int iomem_map_sanity_check(resource_size_t addr, unsigned long size)
 		if (p->flags & IORESOURCE_BUSY)
 			continue;
 
-		printk(KERN_WARNING "resource map sanity check conflict: "
-		       "0x%llx 0x%llx 0x%llx 0x%llx %s\n",
+		printk(KERN_WARNING "resource sanity check: requesting [mem %#010llx-%#010llx], which spans more than %s %pR\n",
 		       (unsigned long long)addr,
 		       (unsigned long long)(addr + size - 1),
-		       (unsigned long long)p->start,
-		       (unsigned long long)p->end,
-		       p->name);
+		       p->name, p);
 		err = -1;
 		break;
 	}
diff --git a/kernel/sched/core.c b/kernel/sched/core.c
index 8a70ec09176..a62a7dec398 100644
--- a/kernel/sched/core.c
+++ b/kernel/sched/core.c
@@ -2208,7 +2208,7 @@ static inline void post_schedule(struct rq *rq)
  * schedule_tail - first thing a freshly forked thread must call.
  * @prev: the thread we just switched away from.
  */
-asmlinkage void schedule_tail(struct task_struct *prev)
+asmlinkage __visible void schedule_tail(struct task_struct *prev)
 	__releases(rq->lock)
 {
 	struct rq *rq = this_rq();
@@ -2608,8 +2608,14 @@ pick_next_task(struct rq *rq, struct task_struct *prev)
 	if (likely(prev->sched_class == class &&
 		   rq->nr_running == rq->cfs.h_nr_running)) {
 		p = fair_sched_class.pick_next_task(rq, prev);
-		if (likely(p && p != RETRY_TASK))
-			return p;
+		if (unlikely(p == RETRY_TASK))
+			goto again;
+
+		/* assumes fair_sched_class->next == idle_sched_class */
+		if (unlikely(!p))
+			p = idle_sched_class.pick_next_task(rq, prev);
+
+		return p;
 	}
 
 again:
@@ -2757,7 +2763,7 @@ static inline void sched_submit_work(struct task_struct *tsk)
 		blk_schedule_flush_plug(tsk);
 }
 
-asmlinkage void __sched schedule(void)
+asmlinkage __visible void __sched schedule(void)
 {
 	struct task_struct *tsk = current;
 
@@ -2767,7 +2773,7 @@ asmlinkage void __sched schedule(void)
 EXPORT_SYMBOL(schedule);
 
 #ifdef CONFIG_CONTEXT_TRACKING
-asmlinkage void __sched schedule_user(void)
+asmlinkage __visible void __sched schedule_user(void)
 {
 	/*
 	 * If we come here after a random call to set_need_resched(),
@@ -2799,7 +2805,7 @@ void __sched schedule_preempt_disabled(void)
  * off of preempt_enable. Kernel preemptions off return from interrupt
  * occur there and call schedule directly.
  */
-asmlinkage void __sched notrace preempt_schedule(void)
+asmlinkage __visible void __sched notrace preempt_schedule(void)
 {
 	/*
 	 * If there is a non-zero preempt_count or interrupts are disabled,
@@ -2829,7 +2835,7 @@ EXPORT_SYMBOL(preempt_schedule);
  * Note, that this is called and return with irqs disabled. This will
  * protect us against recursive calling from irq.
  */
-asmlinkage void __sched preempt_schedule_irq(void)
+asmlinkage __visible void __sched preempt_schedule_irq(void)
 {
 	enum ctx_state prev_state;
 
@@ -3140,6 +3146,7 @@ __setparam_dl(struct task_struct *p, const struct sched_attr *attr)
 	dl_se->dl_bw = to_ratio(dl_se->dl_period, dl_se->dl_runtime);
 	dl_se->dl_throttled = 0;
 	dl_se->dl_new = 1;
+	dl_se->dl_yielded = 0;
 }
 
 static void __setscheduler_params(struct task_struct *p,
@@ -3204,17 +3211,40 @@ __getparam_dl(struct task_struct *p, struct sched_attr *attr)
  * We ask for the deadline not being zero, and greater or equal
  * than the runtime, as well as the period of being zero or
  * greater than deadline. Furthermore, we have to be sure that
- * user parameters are above the internal resolution (1us); we
- * check sched_runtime only since it is always the smaller one.
+ * user parameters are above the internal resolution of 1us (we
+ * check sched_runtime only since it is always the smaller one) and
+ * below 2^63 ns (we have to check both sched_deadline and
+ * sched_period, as the latter can be zero).
  */
 static bool
 __checkparam_dl(const struct sched_attr *attr)
 {
-	return attr && attr->sched_deadline != 0 &&
-		(attr->sched_period == 0 ||
-		(s64)(attr->sched_period   - attr->sched_deadline) >= 0) &&
-		(s64)(attr->sched_deadline - attr->sched_runtime ) >= 0  &&
-		attr->sched_runtime >= (2 << (DL_SCALE - 1));
+	/* deadline != 0 */
+	if (attr->sched_deadline == 0)
+		return false;
+
+	/*
+	 * Since we truncate DL_SCALE bits, make sure we're at least
+	 * that big.
+	 */
+	if (attr->sched_runtime < (1ULL << DL_SCALE))
+		return false;
+
+	/*
+	 * Since we use the MSB for wrap-around and sign issues, make
+	 * sure it's not set (mind that period can be equal to zero).
+	 */
+	if (attr->sched_deadline & (1ULL << 63) ||
+	    attr->sched_period & (1ULL << 63))
+		return false;
+
+	/* runtime <= deadline <= period (if period != 0) */
+	if ((attr->sched_period != 0 &&
+	     attr->sched_period < attr->sched_deadline) ||
+	    attr->sched_deadline < attr->sched_runtime)
+		return false;
+
+	return true;
 }
 
 /*
@@ -3655,6 +3685,7 @@ SYSCALL_DEFINE2(sched_setparam, pid_t, pid, struct sched_param __user *, param)
  * sys_sched_setattr - same as above, but with extended sched_attr
  * @pid: the pid in question.
  * @uattr: structure containing the extended parameters.
+ * @flags: for future extension.
  */
 SYSCALL_DEFINE3(sched_setattr, pid_t, pid, struct sched_attr __user *, uattr,
 			       unsigned int, flags)
@@ -3666,8 +3697,12 @@ SYSCALL_DEFINE3(sched_setattr, pid_t, pid, struct sched_attr __user *, uattr,
 	if (!uattr || pid < 0 || flags)
 		return -EINVAL;
 
-	if (sched_copy_attr(uattr, &attr))
-		return -EFAULT;
+	retval = sched_copy_attr(uattr, &attr);
+	if (retval)
+		return retval;
+
+	if (attr.sched_policy < 0)
+		return -EINVAL;
 
 	rcu_read_lock();
 	retval = -ESRCH;
@@ -3717,7 +3752,7 @@ SYSCALL_DEFINE1(sched_getscheduler, pid_t, pid)
  */
 SYSCALL_DEFINE2(sched_getparam, pid_t, pid, struct sched_param __user *, param)
 {
-	struct sched_param lp;
+	struct sched_param lp = { .sched_priority = 0 };
 	struct task_struct *p;
 	int retval;
 
@@ -3734,11 +3769,8 @@ SYSCALL_DEFINE2(sched_getparam, pid_t, pid, struct sched_param __user *, param)
 	if (retval)
 		goto out_unlock;
 
-	if (task_has_dl_policy(p)) {
-		retval = -EINVAL;
-		goto out_unlock;
-	}
-	lp.sched_priority = p->rt_priority;
+	if (task_has_rt_policy(p))
+		lp.sched_priority = p->rt_priority;
 	rcu_read_unlock();
 
 	/*
@@ -3799,6 +3831,7 @@ err_size:
  * @pid: the pid in question.
  * @uattr: structure containing the extended parameters.
  * @size: sizeof(attr) for fwd/bwd comp.
+ * @flags: for future extension.
  */
 SYSCALL_DEFINE4(sched_getattr, pid_t, pid, struct sched_attr __user *, uattr,
 		unsigned int, size, unsigned int, flags)
@@ -4067,6 +4100,7 @@ static void __cond_resched(void)
 
 int __sched _cond_resched(void)
 {
+	rcu_cond_resched();
 	if (should_resched()) {
 		__cond_resched();
 		return 1;
@@ -4085,15 +4119,18 @@ EXPORT_SYMBOL(_cond_resched);
  */
 int __cond_resched_lock(spinlock_t *lock)
 {
+	bool need_rcu_resched = rcu_should_resched();
 	int resched = should_resched();
 	int ret = 0;
 
 	lockdep_assert_held(lock);
 
-	if (spin_needbreak(lock) || resched) {
+	if (spin_needbreak(lock) || resched || need_rcu_resched) {
 		spin_unlock(lock);
 		if (resched)
 			__cond_resched();
+		else if (unlikely(need_rcu_resched))
+			rcu_resched();
 		else
 			cpu_relax();
 		ret = 1;
@@ -4107,6 +4144,7 @@ int __sched __cond_resched_softirq(void)
 {
 	BUG_ON(!in_softirq());
 
+	rcu_cond_resched();  /* BH disabled OK, just recording QSes. */
 	if (should_resched()) {
 		local_bh_enable();
 		__cond_resched();
@@ -5059,7 +5097,6 @@ static int sched_cpu_active(struct notifier_block *nfb,
 				      unsigned long action, void *hcpu)
 {
 	switch (action & ~CPU_TASKS_FROZEN) {
-	case CPU_STARTING:
 	case CPU_DOWN_FAILED:
 		set_cpu_active((long)hcpu, true);
 		return NOTIFY_OK;
@@ -6033,6 +6070,8 @@ sd_numa_init(struct sched_domain_topology_level *tl, int cpu)
 					,
 		.last_balance		= jiffies,
 		.balance_interval	= sd_weight,
+		.max_newidle_lb_cost	= 0,
+		.next_decay_max_lb_cost	= jiffies,
 	};
 	SD_INIT_NAME(sd, NUMA);
 	sd->private = &tl->data;
diff --git a/kernel/sched/cpudeadline.c b/kernel/sched/cpudeadline.c
index 5b9bb42b2d4..bd95963dae8 100644
--- a/kernel/sched/cpudeadline.c
+++ b/kernel/sched/cpudeadline.c
@@ -13,6 +13,7 @@
 
 #include <linux/gfp.h>
 #include <linux/kernel.h>
+#include <linux/slab.h>
 #include "cpudeadline.h"
 
 static inline int parent(int i)
@@ -39,8 +40,10 @@ static void cpudl_exchange(struct cpudl *cp, int a, int b)
 {
 	int cpu_a = cp->elements[a].cpu, cpu_b = cp->elements[b].cpu;
 
-	swap(cp->elements[a], cp->elements[b]);
-	swap(cp->cpu_to_idx[cpu_a], cp->cpu_to_idx[cpu_b]);
+	swap(cp->elements[a].cpu, cp->elements[b].cpu);
+	swap(cp->elements[a].dl , cp->elements[b].dl );
+
+	swap(cp->elements[cpu_a].idx, cp->elements[cpu_b].idx);
 }
 
 static void cpudl_heapify(struct cpudl *cp, int idx)
@@ -140,7 +143,7 @@ void cpudl_set(struct cpudl *cp, int cpu, u64 dl, int is_valid)
 	WARN_ON(!cpu_present(cpu));
 
 	raw_spin_lock_irqsave(&cp->lock, flags);
-	old_idx = cp->cpu_to_idx[cpu];
+	old_idx = cp->elements[cpu].idx;
 	if (!is_valid) {
 		/* remove item */
 		if (old_idx == IDX_INVALID) {
@@ -155,8 +158,8 @@ void cpudl_set(struct cpudl *cp, int cpu, u64 dl, int is_valid)
 		cp->elements[old_idx].dl = cp->elements[cp->size - 1].dl;
 		cp->elements[old_idx].cpu = new_cpu;
 		cp->size--;
-		cp->cpu_to_idx[new_cpu] = old_idx;
-		cp->cpu_to_idx[cpu] = IDX_INVALID;
+		cp->elements[new_cpu].idx = old_idx;
+		cp->elements[cpu].idx = IDX_INVALID;
 		while (old_idx > 0 && dl_time_before(
 				cp->elements[parent(old_idx)].dl,
 				cp->elements[old_idx].dl)) {
@@ -173,7 +176,7 @@ void cpudl_set(struct cpudl *cp, int cpu, u64 dl, int is_valid)
 		cp->size++;
 		cp->elements[cp->size - 1].dl = 0;
 		cp->elements[cp->size - 1].cpu = cpu;
-		cp->cpu_to_idx[cpu] = cp->size - 1;
+		cp->elements[cpu].idx = cp->size - 1;
 		cpudl_change_key(cp, cp->size - 1, dl);
 		cpumask_clear_cpu(cpu, cp->free_cpus);
 	} else {
@@ -195,10 +198,21 @@ int cpudl_init(struct cpudl *cp)
 	memset(cp, 0, sizeof(*cp));
 	raw_spin_lock_init(&cp->lock);
 	cp->size = 0;
-	for (i = 0; i < NR_CPUS; i++)
-		cp->cpu_to_idx[i] = IDX_INVALID;
-	if (!alloc_cpumask_var(&cp->free_cpus, GFP_KERNEL))
+
+	cp->elements = kcalloc(nr_cpu_ids,
+			       sizeof(struct cpudl_item),
+			       GFP_KERNEL);
+	if (!cp->elements)
+		return -ENOMEM;
+
+	if (!alloc_cpumask_var(&cp->free_cpus, GFP_KERNEL)) {
+		kfree(cp->elements);
 		return -ENOMEM;
+	}
+
+	for_each_possible_cpu(i)
+		cp->elements[i].idx = IDX_INVALID;
+
 	cpumask_setall(cp->free_cpus);
 
 	return 0;
@@ -210,7 +224,6 @@ int cpudl_init(struct cpudl *cp)
  */
 void cpudl_cleanup(struct cpudl *cp)
 {
-	/*
-	 * nothing to do for the moment
-	 */
+	free_cpumask_var(cp->free_cpus);
+	kfree(cp->elements);
 }
diff --git a/kernel/sched/cpudeadline.h b/kernel/sched/cpudeadline.h
index a202789a412..538c9796ad4 100644
--- a/kernel/sched/cpudeadline.h
+++ b/kernel/sched/cpudeadline.h
@@ -5,17 +5,17 @@
 
 #define IDX_INVALID     -1
 
-struct array_item {
+struct cpudl_item {
 	u64 dl;
 	int cpu;
+	int idx;
 };
 
 struct cpudl {
 	raw_spinlock_t lock;
 	int size;
-	int cpu_to_idx[NR_CPUS];
-	struct array_item elements[NR_CPUS];
 	cpumask_var_t free_cpus;
+	struct cpudl_item *elements;
 };
 
 
diff --git a/kernel/sched/cpupri.c b/kernel/sched/cpupri.c
index 746bc934496..981fcd7dc39 100644
--- a/kernel/sched/cpupri.c
+++ b/kernel/sched/cpupri.c
@@ -30,6 +30,7 @@
 #include <linux/gfp.h>
 #include <linux/sched.h>
 #include <linux/sched/rt.h>
+#include <linux/slab.h>
 #include "cpupri.h"
 
 /* Convert between a 140 based task->prio, and our 102 based cpupri */
@@ -70,8 +71,7 @@ int cpupri_find(struct cpupri *cp, struct task_struct *p,
 	int idx = 0;
 	int task_pri = convert_prio(p->prio);
 
-	if (task_pri >= MAX_RT_PRIO)
-		return 0;
+	BUG_ON(task_pri >= CPUPRI_NR_PRIORITIES);
 
 	for (idx = 0; idx < task_pri; idx++) {
 		struct cpupri_vec *vec  = &cp->pri_to_cpu[idx];
@@ -219,8 +219,13 @@ int cpupri_init(struct cpupri *cp)
 			goto cleanup;
 	}
 
+	cp->cpu_to_pri = kcalloc(nr_cpu_ids, sizeof(int), GFP_KERNEL);
+	if (!cp->cpu_to_pri)
+		goto cleanup;
+
 	for_each_possible_cpu(i)
 		cp->cpu_to_pri[i] = CPUPRI_INVALID;
+
 	return 0;
 
 cleanup:
@@ -237,6 +242,7 @@ void cpupri_cleanup(struct cpupri *cp)
 {
 	int i;
 
+	kfree(cp->cpu_to_pri);
 	for (i = 0; i < CPUPRI_NR_PRIORITIES; i++)
 		free_cpumask_var(cp->pri_to_cpu[i].mask);
 }
diff --git a/kernel/sched/cpupri.h b/kernel/sched/cpupri.h
index f6d75617349..6b033347fdf 100644
--- a/kernel/sched/cpupri.h
+++ b/kernel/sched/cpupri.h
@@ -17,7 +17,7 @@ struct cpupri_vec {
 
 struct cpupri {
 	struct cpupri_vec pri_to_cpu[CPUPRI_NR_PRIORITIES];
-	int               cpu_to_pri[NR_CPUS];
+	int *cpu_to_pri;
 };
 
 #ifdef CONFIG_SMP
diff --git a/kernel/sched/cputime.c b/kernel/sched/cputime.c
index a95097cb459..72fdf06ef86 100644
--- a/kernel/sched/cputime.c
+++ b/kernel/sched/cputime.c
@@ -332,50 +332,50 @@ out:
  * softirq as those do not count in task exec_runtime any more.
  */
 static void irqtime_account_process_tick(struct task_struct *p, int user_tick,
-						struct rq *rq)
+					 struct rq *rq, int ticks)
 {
-	cputime_t one_jiffy_scaled = cputime_to_scaled(cputime_one_jiffy);
+	cputime_t scaled = cputime_to_scaled(cputime_one_jiffy);
+	u64 cputime = (__force u64) cputime_one_jiffy;
 	u64 *cpustat = kcpustat_this_cpu->cpustat;
 
 	if (steal_account_process_tick())
 		return;
 
+	cputime *= ticks;
+	scaled *= ticks;
+
 	if (irqtime_account_hi_update()) {
-		cpustat[CPUTIME_IRQ] += (__force u64) cputime_one_jiffy;
+		cpustat[CPUTIME_IRQ] += cputime;
 	} else if (irqtime_account_si_update()) {
-		cpustat[CPUTIME_SOFTIRQ] += (__force u64) cputime_one_jiffy;
+		cpustat[CPUTIME_SOFTIRQ] += cputime;
 	} else if (this_cpu_ksoftirqd() == p) {
 		/*
 		 * ksoftirqd time do not get accounted in cpu_softirq_time.
 		 * So, we have to handle it separately here.
 		 * Also, p->stime needs to be updated for ksoftirqd.
 		 */
-		__account_system_time(p, cputime_one_jiffy, one_jiffy_scaled,
-					CPUTIME_SOFTIRQ);
+		__account_system_time(p, cputime, scaled, CPUTIME_SOFTIRQ);
 	} else if (user_tick) {
-		account_user_time(p, cputime_one_jiffy, one_jiffy_scaled);
+		account_user_time(p, cputime, scaled);
 	} else if (p == rq->idle) {
-		account_idle_time(cputime_one_jiffy);
+		account_idle_time(cputime);
 	} else if (p->flags & PF_VCPU) { /* System time or guest time */
-		account_guest_time(p, cputime_one_jiffy, one_jiffy_scaled);
+		account_guest_time(p, cputime, scaled);
 	} else {
-		__account_system_time(p, cputime_one_jiffy, one_jiffy_scaled,
-					CPUTIME_SYSTEM);
+		__account_system_time(p, cputime, scaled,	CPUTIME_SYSTEM);
 	}
 }
 
 static void irqtime_account_idle_ticks(int ticks)
 {
-	int i;
 	struct rq *rq = this_rq();
 
-	for (i = 0; i < ticks; i++)
-		irqtime_account_process_tick(current, 0, rq);
+	irqtime_account_process_tick(current, 0, rq, ticks);
 }
 #else /* CONFIG_IRQ_TIME_ACCOUNTING */
 static inline void irqtime_account_idle_ticks(int ticks) {}
 static inline void irqtime_account_process_tick(struct task_struct *p, int user_tick,
-						struct rq *rq) {}
+						struct rq *rq, int nr_ticks) {}
 #endif /* CONFIG_IRQ_TIME_ACCOUNTING */
 
 /*
@@ -464,7 +464,7 @@ void account_process_tick(struct task_struct *p, int user_tick)
 		return;
 
 	if (sched_clock_irqtime) {
-		irqtime_account_process_tick(p, user_tick, rq);
+		irqtime_account_process_tick(p, user_tick, rq, 1);
 		return;
 	}
 
diff --git a/kernel/sched/deadline.c b/kernel/sched/deadline.c
index 27ef4092552..800e99b9907 100644
--- a/kernel/sched/deadline.c
+++ b/kernel/sched/deadline.c
@@ -528,6 +528,7 @@ static enum hrtimer_restart dl_task_timer(struct hrtimer *timer)
 	sched_clock_tick();
 	update_rq_clock(rq);
 	dl_se->dl_throttled = 0;
+	dl_se->dl_yielded = 0;
 	if (p->on_rq) {
 		enqueue_task_dl(rq, p, ENQUEUE_REPLENISH);
 		if (task_has_dl_policy(rq->curr))
@@ -893,10 +894,10 @@ static void yield_task_dl(struct rq *rq)
 	 * We make the task go to sleep until its current deadline by
 	 * forcing its runtime to zero. This way, update_curr_dl() stops
 	 * it and the bandwidth timer will wake it up and will give it
-	 * new scheduling parameters (thanks to dl_new=1).
+	 * new scheduling parameters (thanks to dl_yielded=1).
 	 */
 	if (p->dl.runtime > 0) {
-		rq->curr->dl.dl_new = 1;
+		rq->curr->dl.dl_yielded = 1;
 		p->dl.runtime = 0;
 	}
 	update_curr_dl(rq);
@@ -1021,8 +1022,17 @@ struct task_struct *pick_next_task_dl(struct rq *rq, struct task_struct *prev)
 
 	dl_rq = &rq->dl;
 
-	if (need_pull_dl_task(rq, prev))
+	if (need_pull_dl_task(rq, prev)) {
 		pull_dl_task(rq);
+		/*
+		 * pull_rt_task() can drop (and re-acquire) rq->lock; this
+		 * means a stop task can slip in, in which case we need to
+		 * re-start task selection.
+		 */
+		if (rq->stop && rq->stop->on_rq)
+			return RETRY_TASK;
+	}
+
 	/*
 	 * When prev is DL, we may throttle it in put_prev_task().
 	 * So, we update time before we check for dl_nr_running.
diff --git a/kernel/sched/fair.c b/kernel/sched/fair.c
index 7e9bd0b1fa9..0fdb96de81a 100644
--- a/kernel/sched/fair.c
+++ b/kernel/sched/fair.c
@@ -1497,7 +1497,7 @@ static void task_numa_placement(struct task_struct *p)
 	/* If the task is part of a group prevent parallel updates to group stats */
 	if (p->numa_group) {
 		group_lock = &p->numa_group->lock;
-		spin_lock(group_lock);
+		spin_lock_irq(group_lock);
 	}
 
 	/* Find the node with the highest number of faults */
@@ -1572,7 +1572,7 @@ static void task_numa_placement(struct task_struct *p)
 			}
 		}
 
-		spin_unlock(group_lock);
+		spin_unlock_irq(group_lock);
 	}
 
 	/* Preferred node as the node with the most faults */
@@ -1677,7 +1677,8 @@ static void task_numa_group(struct task_struct *p, int cpupid, int flags,
 	if (!join)
 		return;
 
-	double_lock(&my_grp->lock, &grp->lock);
+	BUG_ON(irqs_disabled());
+	double_lock_irq(&my_grp->lock, &grp->lock);
 
 	for (i = 0; i < NR_NUMA_HINT_FAULT_STATS * nr_node_ids; i++) {
 		my_grp->faults[i] -= p->numa_faults_memory[i];
@@ -1691,7 +1692,7 @@ static void task_numa_group(struct task_struct *p, int cpupid, int flags,
 	grp->nr_tasks++;
 
 	spin_unlock(&my_grp->lock);
-	spin_unlock(&grp->lock);
+	spin_unlock_irq(&grp->lock);
 
 	rcu_assign_pointer(p->numa_group, grp);
 
@@ -1710,14 +1711,14 @@ void task_numa_free(struct task_struct *p)
 	void *numa_faults = p->numa_faults_memory;
 
 	if (grp) {
-		spin_lock(&grp->lock);
+		spin_lock_irq(&grp->lock);
 		for (i = 0; i < NR_NUMA_HINT_FAULT_STATS * nr_node_ids; i++)
 			grp->faults[i] -= p->numa_faults_memory[i];
 		grp->total_faults -= p->total_numa_faults;
 
 		list_del(&p->numa_entry);
 		grp->nr_tasks--;
-		spin_unlock(&grp->lock);
+		spin_unlock_irq(&grp->lock);
 		rcu_assign_pointer(p->numa_group, NULL);
 		put_numa_group(grp);
 	}
@@ -6652,6 +6653,7 @@ static int idle_balance(struct rq *this_rq)
 	int this_cpu = this_rq->cpu;
 
 	idle_enter_fair(this_rq);
+
 	/*
 	 * We must set idle_stamp _before_ calling idle_balance(), such that we
 	 * measure the duration of idle_balance() as idle time.
@@ -6704,14 +6706,16 @@ static int idle_balance(struct rq *this_rq)
 
 	raw_spin_lock(&this_rq->lock);
 
+	if (curr_cost > this_rq->max_idle_balance_cost)
+		this_rq->max_idle_balance_cost = curr_cost;
+
 	/*
-	 * While browsing the domains, we released the rq lock.
-	 * A task could have be enqueued in the meantime
+	 * While browsing the domains, we released the rq lock, a task could
+	 * have been enqueued in the meantime. Since we're not going idle,
+	 * pretend we pulled a task.
 	 */
-	if (this_rq->cfs.h_nr_running && !pulled_task) {
+	if (this_rq->cfs.h_nr_running && !pulled_task)
 		pulled_task = 1;
-		goto out;
-	}
 
 	if (pulled_task || time_after(jiffies, this_rq->next_balance)) {
 		/*
@@ -6721,13 +6725,11 @@ static int idle_balance(struct rq *this_rq)
 		this_rq->next_balance = next_balance;
 	}
 
-	if (curr_cost > this_rq->max_idle_balance_cost)
-		this_rq->max_idle_balance_cost = curr_cost;
-
 out:
 	/* Is there a task of a high priority class? */
 	if (this_rq->nr_running != this_rq->cfs.h_nr_running &&
-	    (this_rq->dl.dl_nr_running ||
+	    ((this_rq->stop && this_rq->stop->on_rq) ||
+	     this_rq->dl.dl_nr_running ||
 	     (this_rq->rt.rt_nr_running && !rt_rq_throttled(&this_rq->rt))))
 		pulled_task = -1;
 
diff --git a/kernel/sched/rt.c b/kernel/sched/rt.c
index d8cdf161855..bd2267ad404 100644
--- a/kernel/sched/rt.c
+++ b/kernel/sched/rt.c
@@ -1362,10 +1362,11 @@ pick_next_task_rt(struct rq *rq, struct task_struct *prev)
 		pull_rt_task(rq);
 		/*
 		 * pull_rt_task() can drop (and re-acquire) rq->lock; this
-		 * means a dl task can slip in, in which case we need to
-		 * re-start task selection.
+		 * means a dl or stop task can slip in, in which case we need
+		 * to re-start task selection.
 		 */
-		if (unlikely(rq->dl.dl_nr_running))
+		if (unlikely((rq->stop && rq->stop->on_rq) ||
+			     rq->dl.dl_nr_running))
 			return RETRY_TASK;
 	}
 
diff --git a/kernel/sched/sched.h b/kernel/sched/sched.h
index c9007f28d3a..456e492a3dc 100644
--- a/kernel/sched/sched.h
+++ b/kernel/sched/sched.h
@@ -1385,6 +1385,15 @@ static inline void double_lock(spinlock_t *l1, spinlock_t *l2)
 	spin_lock_nested(l2, SINGLE_DEPTH_NESTING);
 }
 
+static inline void double_lock_irq(spinlock_t *l1, spinlock_t *l2)
+{
+	if (l1 > l2)
+		swap(l1, l2);
+
+	spin_lock_irq(l1);
+	spin_lock_nested(l2, SINGLE_DEPTH_NESTING);
+}
+
 static inline void double_raw_lock(raw_spinlock_t *l1, raw_spinlock_t *l2)
 {
 	if (l1 > l2)
diff --git a/kernel/seccomp.c b/kernel/seccomp.c
index 590c3792508..b35c21503a3 100644
--- a/kernel/seccomp.c
+++ b/kernel/seccomp.c
@@ -255,6 +255,7 @@ static long seccomp_attach_filter(struct sock_fprog *fprog)
 		goto free_prog;
 
 	/* Allocate a new seccomp_filter */
+	ret = -ENOMEM;
 	filter = kzalloc(sizeof(struct seccomp_filter) +
 			 sizeof(struct sock_filter_int) * new_len,
 			 GFP_KERNEL|__GFP_NOWARN);
@@ -264,6 +265,7 @@ static long seccomp_attach_filter(struct sock_fprog *fprog)
 	ret = sk_convert_filter(fp, fprog->len, filter->insnsi, &new_len);
 	if (ret)
 		goto free_filter;
+	kfree(fp);
 
 	atomic_set(&filter->usage, 1);
 	filter->len = new_len;
diff --git a/kernel/softirq.c b/kernel/softirq.c
index b50990a5bea..5918d227730 100644
--- a/kernel/softirq.c
+++ b/kernel/softirq.c
@@ -223,7 +223,7 @@ static inline bool lockdep_softirq_start(void) { return false; }
 static inline void lockdep_softirq_end(bool in_hardirq) { }
 #endif
 
-asmlinkage void __do_softirq(void)
+asmlinkage __visible void __do_softirq(void)
 {
 	unsigned long end = jiffies + MAX_SOFTIRQ_TIME;
 	unsigned long old_flags = current->flags;
@@ -232,7 +232,6 @@ asmlinkage void __do_softirq(void)
 	bool in_hardirq;
 	__u32 pending;
 	int softirq_bit;
-	int cpu;
 
 	/*
 	 * Mask out PF_MEMALLOC s current task context is borrowed for the
@@ -247,7 +246,6 @@ asmlinkage void __do_softirq(void)
 	__local_bh_disable_ip(_RET_IP_, SOFTIRQ_OFFSET);
 	in_hardirq = lockdep_softirq_start();
 
-	cpu = smp_processor_id();
 restart:
 	/* Reset the pending bitmask before enabling irqs */
 	set_softirq_pending(0);
@@ -276,11 +274,11 @@ restart:
 			       prev_count, preempt_count());
 			preempt_count_set(prev_count);
 		}
-		rcu_bh_qs(cpu);
 		h++;
 		pending >>= softirq_bit;
 	}
 
+	rcu_bh_qs(smp_processor_id());
 	local_irq_disable();
 
 	pending = local_softirq_pending();
@@ -299,7 +297,7 @@ restart:
 	tsk_restore_flags(current, old_flags, PF_MEMALLOC);
 }
 
-asmlinkage void do_softirq(void)
+asmlinkage __visible void do_softirq(void)
 {
 	__u32 pending;
 	unsigned long flags;
@@ -779,3 +777,8 @@ int __init __weak arch_early_irq_init(void)
 {
 	return 0;
 }
+
+unsigned int __weak arch_dynirq_lower_bound(unsigned int from)
+{
+	return from;
+}
diff --git a/kernel/sysctl.c b/kernel/sysctl.c
index 74f5b580fe3..bc966a8ffc3 100644
--- a/kernel/sysctl.c
+++ b/kernel/sysctl.c
@@ -643,7 +643,7 @@ static struct ctl_table kern_table[] = {
 		.extra2		= &one,
 	},
 #endif
-
+#ifdef CONFIG_UEVENT_HELPER
 	{
 		.procname	= "hotplug",
 		.data		= &uevent_helper,
@@ -651,7 +651,7 @@ static struct ctl_table kern_table[] = {
 		.mode		= 0644,
 		.proc_handler	= proc_dostring,
 	},
-
+#endif
 #ifdef CONFIG_CHR_DEV_SG
 	{
 		.procname	= "sg-big-buff",
diff --git a/kernel/time/tick-common.c b/kernel/time/tick-common.c
index 015661279b6..0a0608edeb2 100644
--- a/kernel/time/tick-common.c
+++ b/kernel/time/tick-common.c
@@ -276,7 +276,7 @@ static bool tick_check_preferred(struct clock_event_device *curdev,
 bool tick_check_replacement(struct clock_event_device *curdev,
 			    struct clock_event_device *newdev)
 {
-	if (tick_check_percpu(curdev, newdev, smp_processor_id()))
+	if (!tick_check_percpu(curdev, newdev, smp_processor_id()))
 		return false;
 
 	return tick_check_preferred(curdev, newdev);
diff --git a/kernel/time/tick-sched.c b/kernel/time/tick-sched.c
index 9f8af69c67e..6558b7ac112 100644
--- a/kernel/time/tick-sched.c
+++ b/kernel/time/tick-sched.c
@@ -84,6 +84,9 @@ static void tick_do_update_jiffies64(ktime_t now)
 
 		/* Keep the tick_next_period variable up to date */
 		tick_next_period = ktime_add(last_jiffies_update, tick_period);
+	} else {
+		write_sequnlock(&jiffies_lock);
+		return;
 	}
 	write_sequnlock(&jiffies_lock);
 	update_wall_time();
@@ -967,7 +970,7 @@ static void tick_nohz_switch_to_nohz(void)
 	struct tick_sched *ts = &__get_cpu_var(tick_cpu_sched);
 	ktime_t next;
 
-	if (!tick_nohz_active)
+	if (!tick_nohz_enabled)
 		return;
 
 	local_irq_disable();
diff --git a/kernel/timer.c b/kernel/timer.c
index 87bd529879c..3bb01a323b2 100644
--- a/kernel/timer.c
+++ b/kernel/timer.c
@@ -838,7 +838,7 @@ unsigned long apply_slack(struct timer_list *timer, unsigned long expires)
 
 	bit = find_last_bit(&mask, BITS_PER_LONG);
 
-	mask = (1 << bit) - 1;
+	mask = (1UL << bit) - 1;
 
 	expires_limit = expires_limit & ~(mask);
 
diff --git a/kernel/torture.c b/kernel/torture.c
index acc9afc2f26..40bb511cca4 100644
--- a/kernel/torture.c
+++ b/kernel/torture.c
@@ -335,13 +335,8 @@ static void torture_shuffle_tasks(void)
 	shuffle_idle_cpu = cpumask_next(shuffle_idle_cpu, shuffle_tmp_mask);
 	if (shuffle_idle_cpu >= nr_cpu_ids)
 		shuffle_idle_cpu = -1;
-	if (shuffle_idle_cpu != -1) {
+	else
 		cpumask_clear_cpu(shuffle_idle_cpu, shuffle_tmp_mask);
-		if (cpumask_empty(shuffle_tmp_mask)) {
-			put_online_cpus();
-			return;
-		}
-	}
 
 	mutex_lock(&shuffle_task_mutex);
 	list_for_each_entry(stp, &shuffle_task_list, st_l)
@@ -533,7 +528,11 @@ void stutter_wait(const char *title)
 	while (ACCESS_ONCE(stutter_pause_test) ||
 	       (torture_runnable && !ACCESS_ONCE(*torture_runnable))) {
 		if (stutter_pause_test)
-			schedule_timeout_interruptible(1);
+			if (ACCESS_ONCE(stutter_pause_test) == 1)
+				schedule_timeout_interruptible(1);
+			else
+				while (ACCESS_ONCE(stutter_pause_test))
+					cond_resched();
 		else
 			schedule_timeout_interruptible(round_jiffies_relative(HZ));
 		torture_shutdown_absorb(title);
@@ -550,7 +549,11 @@ static int torture_stutter(void *arg)
 	VERBOSE_TOROUT_STRING("torture_stutter task started");
 	do {
 		if (!torture_must_stop()) {
-			schedule_timeout_interruptible(stutter);
+			if (stutter > 1) {
+				schedule_timeout_interruptible(stutter - 1);
+				ACCESS_ONCE(stutter_pause_test) = 2;
+			}
+			schedule_timeout_interruptible(1);
 			ACCESS_ONCE(stutter_pause_test) = 1;
 		}
 		if (!torture_must_stop())
@@ -596,21 +599,27 @@ static void torture_stutter_cleanup(void)
  * The runnable parameter points to a flag that controls whether or not
  * the test is currently runnable.  If there is no such flag, pass in NULL.
  */
-void __init torture_init_begin(char *ttype, bool v, int *runnable)
+bool torture_init_begin(char *ttype, bool v, int *runnable)
 {
 	mutex_lock(&fullstop_mutex);
+	if (torture_type != NULL) {
+		pr_alert("torture_init_begin: refusing %s init: %s running",
+			 ttype, torture_type);
+		mutex_unlock(&fullstop_mutex);
+		return false;
+	}
 	torture_type = ttype;
 	verbose = v;
 	torture_runnable = runnable;
 	fullstop = FULLSTOP_DONTSTOP;
-
+	return true;
 }
 EXPORT_SYMBOL_GPL(torture_init_begin);
 
 /*
  * Tell the torture module that initialization is complete.
  */
-void __init torture_init_end(void)
+void torture_init_end(void)
 {
 	mutex_unlock(&fullstop_mutex);
 	register_reboot_notifier(&torture_shutdown_nb);
@@ -642,6 +651,9 @@ bool torture_cleanup(void)
 	torture_shuffle_cleanup();
 	torture_stutter_cleanup();
 	torture_onoff_cleanup();
+	mutex_lock(&fullstop_mutex);
+	torture_type = NULL;
+	mutex_unlock(&fullstop_mutex);
 	return false;
 }
 EXPORT_SYMBOL_GPL(torture_cleanup);
@@ -674,8 +686,10 @@ EXPORT_SYMBOL_GPL(torture_must_stop_irq);
  */
 void torture_kthread_stopping(char *title)
 {
-	if (verbose)
-		VERBOSE_TOROUT_STRING(title);
+	char buf[128];
+
+	snprintf(buf, sizeof(buf), "Stopping %s", title);
+	VERBOSE_TOROUT_STRING(buf);
 	while (!kthread_should_stop()) {
 		torture_shutdown_absorb(title);
 		schedule_timeout_uninterruptible(1);
diff --git a/kernel/trace/ftrace.c b/kernel/trace/ftrace.c
index 1fd4b947921..4a54a25afa2 100644
--- a/kernel/trace/ftrace.c
+++ b/kernel/trace/ftrace.c
@@ -4330,16 +4330,11 @@ static void ftrace_init_module(struct module *mod,
 	ftrace_process_locs(mod, start, end);
 }
 
-static int ftrace_module_notify_enter(struct notifier_block *self,
-				      unsigned long val, void *data)
+void ftrace_module_init(struct module *mod)
 {
-	struct module *mod = data;
-
-	if (val == MODULE_STATE_COMING)
-		ftrace_init_module(mod, mod->ftrace_callsites,
-				   mod->ftrace_callsites +
-				   mod->num_ftrace_callsites);
-	return 0;
+	ftrace_init_module(mod, mod->ftrace_callsites,
+			   mod->ftrace_callsites +
+			   mod->num_ftrace_callsites);
 }
 
 static int ftrace_module_notify_exit(struct notifier_block *self,
@@ -4353,11 +4348,6 @@ static int ftrace_module_notify_exit(struct notifier_block *self,
 	return 0;
 }
 #else
-static int ftrace_module_notify_enter(struct notifier_block *self,
-				      unsigned long val, void *data)
-{
-	return 0;
-}
 static int ftrace_module_notify_exit(struct notifier_block *self,
 				     unsigned long val, void *data)
 {
@@ -4365,11 +4355,6 @@ static int ftrace_module_notify_exit(struct notifier_block *self,
 }
 #endif /* CONFIG_MODULES */
 
-struct notifier_block ftrace_module_enter_nb = {
-	.notifier_call = ftrace_module_notify_enter,
-	.priority = INT_MAX,	/* Run before anything that can use kprobes */
-};
-
 struct notifier_block ftrace_module_exit_nb = {
 	.notifier_call = ftrace_module_notify_exit,
 	.priority = INT_MIN,	/* Run after anything that can remove kprobes */
@@ -4403,10 +4388,6 @@ void __init ftrace_init(void)
 				  __start_mcount_loc,
 				  __stop_mcount_loc);
 
-	ret = register_module_notifier(&ftrace_module_enter_nb);
-	if (ret)
-		pr_warning("Failed to register trace ftrace module enter notifier\n");
-
 	ret = register_module_notifier(&ftrace_module_exit_nb);
 	if (ret)
 		pr_warning("Failed to register trace ftrace module exit notifier\n");
diff --git a/kernel/trace/trace_events_trigger.c b/kernel/trace/trace_events_trigger.c
index 925f537f07d..4747b476a03 100644
--- a/kernel/trace/trace_events_trigger.c
+++ b/kernel/trace/trace_events_trigger.c
@@ -77,7 +77,7 @@ event_triggers_call(struct ftrace_event_file *file, void *rec)
 			data->ops->func(data);
 			continue;
 		}
-		filter = rcu_dereference(data->filter);
+		filter = rcu_dereference_sched(data->filter);
 		if (filter && !filter_match_preds(filter, rec))
 			continue;
 		if (data->cmd_ops->post_trigger) {
diff --git a/kernel/trace/trace_functions.c b/kernel/trace/trace_functions.c
index 5b781d2be38..ffd56351b52 100644
--- a/kernel/trace/trace_functions.c
+++ b/kernel/trace/trace_functions.c
@@ -58,12 +58,16 @@ int ftrace_create_function_files(struct trace_array *tr,
 {
 	int ret;
 
-	/* The top level array uses the "global_ops". */
-	if (!(tr->flags & TRACE_ARRAY_FL_GLOBAL)) {
-		ret = allocate_ftrace_ops(tr);
-		if (ret)
-			return ret;
-	}
+	/*
+	 * The top level array uses the "global_ops", and the files are
+	 * created on boot up.
+	 */
+	if (tr->flags & TRACE_ARRAY_FL_GLOBAL)
+		return 0;
+
+	ret = allocate_ftrace_ops(tr);
+	if (ret)
+		return ret;
 
 	ftrace_create_filter_files(tr->ops, parent);
 
diff --git a/kernel/trace/trace_uprobe.c b/kernel/trace/trace_uprobe.c
index 930e51462dc..c082a744134 100644
--- a/kernel/trace/trace_uprobe.c
+++ b/kernel/trace/trace_uprobe.c
@@ -732,9 +732,15 @@ static int uprobe_buffer_enable(void)
 
 static void uprobe_buffer_disable(void)
 {
+	int cpu;
+
 	BUG_ON(!mutex_is_locked(&event_mutex));
 
 	if (--uprobe_buffer_refcnt == 0) {
+		for_each_possible_cpu(cpu)
+			free_page((unsigned long)per_cpu_ptr(uprobe_cpu_buffer,
+							     cpu)->buf);
+
 		free_percpu(uprobe_cpu_buffer);
 		uprobe_cpu_buffer = NULL;
 	}
diff --git a/kernel/tracepoint.c b/kernel/tracepoint.c
index ac5b23cf721..6620e5837ce 100644
--- a/kernel/tracepoint.c
+++ b/kernel/tracepoint.c
@@ -188,7 +188,6 @@ static int tracepoint_add_func(struct tracepoint *tp,
 		WARN_ON_ONCE(1);
 		return PTR_ERR(old);
 	}
-	release_probes(old);
 
 	/*
 	 * rcu_assign_pointer has a smp_wmb() which makes sure that the new
@@ -200,6 +199,7 @@ static int tracepoint_add_func(struct tracepoint *tp,
 	rcu_assign_pointer(tp->funcs, tp_funcs);
 	if (!static_key_enabled(&tp->key))
 		static_key_slow_inc(&tp->key);
+	release_probes(old);
 	return 0;
 }
 
@@ -221,7 +221,6 @@ static int tracepoint_remove_func(struct tracepoint *tp,
 		WARN_ON_ONCE(1);
 		return PTR_ERR(old);
 	}
-	release_probes(old);
 
 	if (!tp_funcs) {
 		/* Removed last function */
@@ -232,6 +231,7 @@ static int tracepoint_remove_func(struct tracepoint *tp,
 			static_key_slow_dec(&tp->key);
 	}
 	rcu_assign_pointer(tp->funcs, tp_funcs);
+	release_probes(old);
 	return 0;
 }
 
diff --git a/kernel/watchdog.c b/kernel/watchdog.c
index e90089fd78e..516203e665f 100644
--- a/kernel/watchdog.c
+++ b/kernel/watchdog.c
@@ -138,7 +138,11 @@ static void __touch_watchdog(void)
 
 void touch_softlockup_watchdog(void)
 {
-	__this_cpu_write(watchdog_touch_ts, 0);
+	/*
+	 * Preemption can be enabled.  It doesn't matter which CPU's timestamp
+	 * gets zeroed here, so use the raw_ operation.
+	 */
+	raw_cpu_write(watchdog_touch_ts, 0);
 }
 EXPORT_SYMBOL(touch_softlockup_watchdog);
 
diff --git a/kernel/workqueue.c b/kernel/workqueue.c
index 0ee63af30bd..8edc8718542 100644
--- a/kernel/workqueue.c
+++ b/kernel/workqueue.c
@@ -1916,6 +1916,12 @@ static void send_mayday(struct work_struct *work)
 
 	/* mayday mayday mayday */
 	if (list_empty(&pwq->mayday_node)) {
+		/*
+		 * If @pwq is for an unbound wq, its base ref may be put at
+		 * any time due to an attribute change.  Pin @pwq until the
+		 * rescuer is done with it.
+		 */
+		get_pwq(pwq);
 		list_add_tail(&pwq->mayday_node, &wq->maydays);
 		wake_up_process(wq->rescuer->task);
 	}
@@ -2398,6 +2404,7 @@ static int rescuer_thread(void *__rescuer)
 	struct worker *rescuer = __rescuer;
 	struct workqueue_struct *wq = rescuer->rescue_wq;
 	struct list_head *scheduled = &rescuer->scheduled;
+	bool should_stop;
 
 	set_user_nice(current, RESCUER_NICE_LEVEL);
 
@@ -2409,11 +2416,15 @@ static int rescuer_thread(void *__rescuer)
 repeat:
 	set_current_state(TASK_INTERRUPTIBLE);
 
-	if (kthread_should_stop()) {
-		__set_current_state(TASK_RUNNING);
-		rescuer->task->flags &= ~PF_WQ_WORKER;
-		return 0;
-	}
+	/*
+	 * By the time the rescuer is requested to stop, the workqueue
+	 * shouldn't have any work pending, but @wq->maydays may still have
+	 * pwq(s) queued.  This can happen by non-rescuer workers consuming
+	 * all the work items before the rescuer got to them.  Go through
+	 * @wq->maydays processing before acting on should_stop so that the
+	 * list is always empty on exit.
+	 */
+	should_stop = kthread_should_stop();
 
 	/* see whether any pwq is asking for help */
 	spin_lock_irq(&wq_mayday_lock);
@@ -2445,6 +2456,12 @@ repeat:
 		process_scheduled_works(rescuer);
 
 		/*
+		 * Put the reference grabbed by send_mayday().  @pool won't
+		 * go away while we're holding its lock.
+		 */
+		put_pwq(pwq);
+
+		/*
 		 * Leave this pool.  If keep_working() is %true, notify a
 		 * regular worker; otherwise, we end up with 0 concurrency
 		 * and stalling the execution.
@@ -2459,6 +2476,12 @@ repeat:
 
 	spin_unlock_irq(&wq_mayday_lock);
 
+	if (should_stop) {
+		__set_current_state(TASK_RUNNING);
+		rescuer->task->flags &= ~PF_WQ_WORKER;
+		return 0;
+	}
+
 	/* rescuers should never participate in concurrency management */
 	WARN_ON_ONCE(!(rescuer->flags & WORKER_NOT_RUNNING));
 	schedule();
@@ -4100,7 +4123,8 @@ static void wq_update_unbound_numa(struct workqueue_struct *wq, int cpu,
 	if (!pwq) {
 		pr_warning("workqueue: allocation failed while updating NUMA affinity of \"%s\"\n",
 			   wq->name);
-		goto out_unlock;
+		mutex_lock(&wq->mutex);
+		goto use_dfl_pwq;
 	}
 
 	/*
diff --git a/lib/Kconfig.debug b/lib/Kconfig.debug
index 140b66a874c..819ac51202c 100644
--- a/lib/Kconfig.debug
+++ b/lib/Kconfig.debug
@@ -505,8 +505,7 @@ config DEBUG_VM_RB
 	bool "Debug VM red-black trees"
 	depends on DEBUG_VM
 	help
-	  Enable this to turn on more extended checks in the virtual-memory
-	  system that may impact performance.
+	  Enable VM red-black tree debugging information and extra validations.
 
 	  If unsure, say N.
 
diff --git a/lib/devres.c b/lib/devres.c
index 2f16c133fd3..f562bf6ff71 100644
--- a/lib/devres.c
+++ b/lib/devres.c
@@ -157,12 +157,12 @@ EXPORT_SYMBOL(devm_ioremap_resource);
  *	if (!base)
  *		return -EADDRNOTAVAIL;
  */
-void __iomem *devm_request_and_ioremap(struct device *device,
+void __iomem *devm_request_and_ioremap(struct device *dev,
 				       struct resource *res)
 {
 	void __iomem *dest_ptr;
 
-	dest_ptr = devm_ioremap_resource(device, res);
+	dest_ptr = devm_ioremap_resource(dev, res);
 	if (IS_ERR(dest_ptr))
 		return NULL;
 
@@ -194,7 +194,7 @@ static int devm_ioport_map_match(struct device *dev, void *res,
  * Managed ioport_map().  Map is automatically unmapped on driver
  * detach.
  */
-void __iomem * devm_ioport_map(struct device *dev, unsigned long port,
+void __iomem *devm_ioport_map(struct device *dev, unsigned long port,
 			       unsigned int nr)
 {
 	void __iomem **ptr, *addr;
@@ -265,7 +265,7 @@ static void pcim_iomap_release(struct device *gendev, void *res)
  * be safely called without context and guaranteed to succed once
  * allocated.
  */
-void __iomem * const * pcim_iomap_table(struct pci_dev *pdev)
+void __iomem * const *pcim_iomap_table(struct pci_dev *pdev)
 {
 	struct pcim_iomap_devres *dr, *new_dr;
 
@@ -290,7 +290,7 @@ EXPORT_SYMBOL(pcim_iomap_table);
  * Managed pci_iomap().  Map is automatically unmapped on driver
  * detach.
  */
-void __iomem * pcim_iomap(struct pci_dev *pdev, int bar, unsigned long maxlen)
+void __iomem *pcim_iomap(struct pci_dev *pdev, int bar, unsigned long maxlen)
 {
 	void __iomem **tbl;
 
diff --git a/lib/dump_stack.c b/lib/dump_stack.c
index f23b63f0a1c..6745c6230db 100644
--- a/lib/dump_stack.c
+++ b/lib/dump_stack.c
@@ -23,7 +23,7 @@ static void __dump_stack(void)
 #ifdef CONFIG_SMP
 static atomic_t dump_lock = ATOMIC_INIT(-1);
 
-asmlinkage void dump_stack(void)
+asmlinkage __visible void dump_stack(void)
 {
 	int was_locked;
 	int old;
@@ -55,7 +55,7 @@ retry:
 	preempt_enable();
 }
 #else
-asmlinkage void dump_stack(void)
+asmlinkage __visible void dump_stack(void)
 {
 	__dump_stack();
 }
diff --git a/lib/kobject_uevent.c b/lib/kobject_uevent.c
index 4e3bd71bd94..9ebf9e20de5 100644
--- a/lib/kobject_uevent.c
+++ b/lib/kobject_uevent.c
@@ -29,7 +29,9 @@
 
 
 u64 uevent_seqnum;
+#ifdef CONFIG_UEVENT_HELPER
 char uevent_helper[UEVENT_HELPER_PATH_LEN] = CONFIG_UEVENT_HELPER_PATH;
+#endif
 #ifdef CONFIG_NET
 struct uevent_sock {
 	struct list_head list;
@@ -109,6 +111,7 @@ static int kobj_bcast_filter(struct sock *dsk, struct sk_buff *skb, void *data)
 }
 #endif
 
+#ifdef CONFIG_UEVENT_HELPER
 static int kobj_usermode_filter(struct kobject *kobj)
 {
 	const struct kobj_ns_type_operations *ops;
@@ -147,6 +150,7 @@ static void cleanup_uevent_env(struct subprocess_info *info)
 {
 	kfree(info->data);
 }
+#endif
 
 /**
  * kobject_uevent_env - send an uevent with environmental data
@@ -323,6 +327,7 @@ int kobject_uevent_env(struct kobject *kobj, enum kobject_action action,
 #endif
 	mutex_unlock(&uevent_sock_mutex);
 
+#ifdef CONFIG_UEVENT_HELPER
 	/* call uevent_helper, usually only enabled during early boot */
 	if (uevent_helper[0] && !kobj_usermode_filter(kobj)) {
 		struct subprocess_info *info;
@@ -347,6 +352,7 @@ int kobject_uevent_env(struct kobject *kobj, enum kobject_action action,
 			env = NULL;	/* freed by cleanup_uevent_env */
 		}
 	}
+#endif
 
 exit:
 	kfree(devpath);
diff --git a/mm/Kconfig b/mm/Kconfig
index ebe5880c29d..28cec518f4d 100644
--- a/mm/Kconfig
+++ b/mm/Kconfig
@@ -134,6 +134,9 @@ config HAVE_MEMBLOCK
 config HAVE_MEMBLOCK_NODE_MAP
 	boolean
 
+config HAVE_MEMBLOCK_PHYS_MAP
+	boolean
+
 config ARCH_DISCARD_MEMBLOCK
 	boolean
 
@@ -581,3 +584,18 @@ config PGTABLE_MAPPING
 
 config GENERIC_EARLY_IOREMAP
 	bool
+
+config MAX_STACK_SIZE_MB
+	int "Maximum user stack size for 32-bit processes (MB)"
+	default 80
+	range 8 256 if METAG
+	range 8 2048
+	depends on STACK_GROWSUP && (!64BIT || COMPAT)
+	help
+	  This is the maximum stack size in Megabytes in the VM layout of 32-bit
+	  user processes when the stack grows upwards (currently only on parisc
+	  and metag arch). The stack will be located at the highest memory
+	  address minus the given value, unless the RLIMIT_STACK hard limit is
+	  changed to a smaller value in which case that is used.
+
+	  A sane initial value is 80 MB.
diff --git a/mm/Makefile b/mm/Makefile
index b484452dac5..0173940407f 100644
--- a/mm/Makefile
+++ b/mm/Makefile
@@ -30,7 +30,6 @@ endif
 
 obj-$(CONFIG_HAVE_MEMBLOCK) += memblock.o
 
-obj-$(CONFIG_BOUNCE)	+= bounce.o
 obj-$(CONFIG_SWAP)	+= page_io.o swap_state.o swapfile.o
 obj-$(CONFIG_FRONTSWAP)	+= frontswap.o
 obj-$(CONFIG_ZSWAP)	+= zswap.o
diff --git a/mm/compaction.c b/mm/compaction.c
index 37f97628706..627dc2e4320 100644
--- a/mm/compaction.c
+++ b/mm/compaction.c
@@ -671,16 +671,20 @@ static void isolate_freepages(struct zone *zone,
 				struct compact_control *cc)
 {
 	struct page *page;
-	unsigned long high_pfn, low_pfn, pfn, z_end_pfn, end_pfn;
+	unsigned long high_pfn, low_pfn, pfn, z_end_pfn;
 	int nr_freepages = cc->nr_freepages;
 	struct list_head *freelist = &cc->freepages;
 
 	/*
 	 * Initialise the free scanner. The starting point is where we last
-	 * scanned from (or the end of the zone if starting). The low point
-	 * is the end of the pageblock the migration scanner is using.
+	 * successfully isolated from, zone-cached value, or the end of the
+	 * zone when isolating for the first time. We need this aligned to
+	 * the pageblock boundary, because we do pfn -= pageblock_nr_pages
+	 * in the for loop.
+	 * The low boundary is the end of the pageblock the migration scanner
+	 * is using.
 	 */
-	pfn = cc->free_pfn;
+	pfn = cc->free_pfn & ~(pageblock_nr_pages-1);
 	low_pfn = ALIGN(cc->migrate_pfn + 1, pageblock_nr_pages);
 
 	/*
@@ -700,6 +704,7 @@ static void isolate_freepages(struct zone *zone,
 	for (; pfn >= low_pfn && cc->nr_migratepages > nr_freepages;
 					pfn -= pageblock_nr_pages) {
 		unsigned long isolated;
+		unsigned long end_pfn;
 
 		/*
 		 * This can iterate a massively long zone without finding any
@@ -734,13 +739,10 @@ static void isolate_freepages(struct zone *zone,
 		isolated = 0;
 
 		/*
-		 * As pfn may not start aligned, pfn+pageblock_nr_page
-		 * may cross a MAX_ORDER_NR_PAGES boundary and miss
-		 * a pfn_valid check. Ensure isolate_freepages_block()
-		 * only scans within a pageblock
+		 * Take care when isolating in last pageblock of a zone which
+		 * ends in the middle of a pageblock.
 		 */
-		end_pfn = ALIGN(pfn + 1, pageblock_nr_pages);
-		end_pfn = min(end_pfn, z_end_pfn);
+		end_pfn = min(pfn + pageblock_nr_pages, z_end_pfn);
 		isolated = isolate_freepages_block(cc, pfn, end_pfn,
 						   freelist, false);
 		nr_freepages += isolated;
diff --git a/mm/filemap.c b/mm/filemap.c
index c73535c914c..021056c324e 100644
--- a/mm/filemap.c
+++ b/mm/filemap.c
@@ -257,9 +257,11 @@ 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))
+	if (test_bit(AS_ENOSPC, &mapping->flags) &&
+	    test_and_clear_bit(AS_ENOSPC, &mapping->flags))
 		ret = -ENOSPC;
-	if (test_and_clear_bit(AS_EIO, &mapping->flags))
+	if (test_bit(AS_EIO, &mapping->flags) &&
+	    test_and_clear_bit(AS_EIO, &mapping->flags))
 		ret = -EIO;
 	return ret;
 }
@@ -906,8 +908,8 @@ EXPORT_SYMBOL(page_cache_prev_hole);
  * Looks up the page cache slot at @mapping & @offset.  If there is a
  * page cache page, it is returned with an increased refcount.
  *
- * If the slot holds a shadow entry of a previously evicted page, it
- * is returned.
+ * If the slot holds a shadow entry of a previously evicted page, or a
+ * swap entry from shmem/tmpfs, it is returned.
  *
  * Otherwise, %NULL is returned.
  */
@@ -928,9 +930,9 @@ repeat:
 			if (radix_tree_deref_retry(page))
 				goto repeat;
 			/*
-			 * Otherwise, shmem/tmpfs must be storing a swap entry
-			 * here as an exceptional entry: so return it without
-			 * attempting to raise page count.
+			 * A shadow entry of a recently evicted page,
+			 * or a swap entry from shmem/tmpfs.  Return
+			 * it without attempting to raise page count.
 			 */
 			goto out;
 		}
@@ -983,8 +985,8 @@ EXPORT_SYMBOL(find_get_page);
  * page cache page, it is returned locked and with an increased
  * refcount.
  *
- * If the slot holds a shadow entry of a previously evicted page, it
- * is returned.
+ * If the slot holds a shadow entry of a previously evicted page, or a
+ * swap entry from shmem/tmpfs, it is returned.
  *
  * Otherwise, %NULL is returned.
  *
@@ -1099,8 +1101,8 @@ EXPORT_SYMBOL(find_or_create_page);
  * with ascending indexes.  There may be holes in the indices due to
  * not-present pages.
  *
- * Any shadow entries of evicted pages are included in the returned
- * array.
+ * Any shadow entries of evicted pages, or swap entries from
+ * shmem/tmpfs, are included in the returned array.
  *
  * find_get_entries() returns the number of pages and shadow entries
  * which were found.
@@ -1128,9 +1130,9 @@ repeat:
 			if (radix_tree_deref_retry(page))
 				goto restart;
 			/*
-			 * Otherwise, we must be storing a swap entry
-			 * here as an exceptional entry: so return it
-			 * without attempting to raise page count.
+			 * A shadow entry of a recently evicted page,
+			 * or a swap entry from shmem/tmpfs.  Return
+			 * it without attempting to raise page count.
 			 */
 			goto export;
 		}
@@ -1198,9 +1200,9 @@ repeat:
 				goto restart;
 			}
 			/*
-			 * Otherwise, shmem/tmpfs must be storing a swap entry
-			 * here as an exceptional entry: so skip over it -
-			 * we only reach this from invalidate_mapping_pages().
+			 * A shadow entry of a recently evicted page,
+			 * or a swap entry from shmem/tmpfs.  Skip
+			 * over it.
 			 */
 			continue;
 		}
@@ -1265,9 +1267,9 @@ repeat:
 				goto restart;
 			}
 			/*
-			 * Otherwise, shmem/tmpfs must be storing a swap entry
-			 * here as an exceptional entry: so stop looking for
-			 * contiguous pages.
+			 * A shadow entry of a recently evicted page,
+			 * or a swap entry from shmem/tmpfs.  Stop
+			 * looking for contiguous pages.
 			 */
 			break;
 		}
@@ -1341,10 +1343,17 @@ repeat:
 				goto restart;
 			}
 			/*
-			 * This function is never used on a shmem/tmpfs
-			 * mapping, so a swap entry won't be found here.
+			 * A shadow entry of a recently evicted page.
+			 *
+			 * Those entries should never be tagged, but
+			 * this tree walk is lockless and the tags are
+			 * looked up in bulk, one radix tree node at a
+			 * time, so there is a sizable window for page
+			 * reclaim to evict a page we saw tagged.
+			 *
+			 * Skip over it.
 			 */
-			BUG();
+			continue;
 		}
 
 		if (!page_cache_get_speculative(page))
@@ -2581,7 +2590,6 @@ EXPORT_SYMBOL(generic_perform_write);
  * @iocb:	IO state structure (file, offset, etc.)
  * @iov:	vector with data to write
  * @nr_segs:	number of segments in the vector
- * @ppos:	position where to write
  *
  * This function does all the work needed for actually writing data to a
  * file. It does all basic checks, removes SUID from the file, updates
diff --git a/mm/huge_memory.c b/mm/huge_memory.c
index 64635f5278f..b4b1feba647 100644
--- a/mm/huge_memory.c
+++ b/mm/huge_memory.c
@@ -1536,16 +1536,23 @@ pmd_t *page_check_address_pmd(struct page *page,
 			      enum page_check_address_pmd_flag flag,
 			      spinlock_t **ptl)
 {
+	pgd_t *pgd;
+	pud_t *pud;
 	pmd_t *pmd;
 
 	if (address & ~HPAGE_PMD_MASK)
 		return NULL;
 
-	pmd = mm_find_pmd(mm, address);
-	if (!pmd)
+	pgd = pgd_offset(mm, address);
+	if (!pgd_present(*pgd))
 		return NULL;
+	pud = pud_offset(pgd, address);
+	if (!pud_present(*pud))
+		return NULL;
+	pmd = pmd_offset(pud, address);
+
 	*ptl = pmd_lock(mm, pmd);
-	if (pmd_none(*pmd))
+	if (!pmd_present(*pmd))
 		goto unlock;
 	if (pmd_page(*pmd) != page)
 		goto unlock;
diff --git a/mm/hugetlb.c b/mm/hugetlb.c
index dd30f22b35e..c82290b9c1f 100644
--- a/mm/hugetlb.c
+++ b/mm/hugetlb.c
@@ -1172,6 +1172,7 @@ static void return_unused_surplus_pages(struct hstate *h,
 	while (nr_pages--) {
 		if (!free_pool_huge_page(h, &node_states[N_MEMORY], 1))
 			break;
+		cond_resched_lock(&hugetlb_lock);
 	}
 }
 
@@ -1980,11 +1981,7 @@ static int __init hugetlb_init(void)
 {
 	int i;
 
-	/* Some platform decide whether they support huge pages at boot
-	 * time. On these, such as powerpc, HPAGE_SHIFT is set to 0 when
-	 * there is no such support
-	 */
-	if (HPAGE_SHIFT == 0)
+	if (!hugepages_supported())
 		return 0;
 
 	if (!size_to_hstate(default_hstate_size)) {
@@ -2111,6 +2108,9 @@ static int hugetlb_sysctl_handler_common(bool obey_mempolicy,
 	unsigned long tmp;
 	int ret;
 
+	if (!hugepages_supported())
+		return -ENOTSUPP;
+
 	tmp = h->max_huge_pages;
 
 	if (write && h->order >= MAX_ORDER)
@@ -2164,6 +2164,9 @@ int hugetlb_overcommit_handler(struct ctl_table *table, int write,
 	unsigned long tmp;
 	int ret;
 
+	if (!hugepages_supported())
+		return -ENOTSUPP;
+
 	tmp = h->nr_overcommit_huge_pages;
 
 	if (write && h->order >= MAX_ORDER)
@@ -2189,6 +2192,8 @@ out:
 void hugetlb_report_meminfo(struct seq_file *m)
 {
 	struct hstate *h = &default_hstate;
+	if (!hugepages_supported())
+		return;
 	seq_printf(m,
 			"HugePages_Total:   %5lu\n"
 			"HugePages_Free:    %5lu\n"
@@ -2205,6 +2210,8 @@ void hugetlb_report_meminfo(struct seq_file *m)
 int hugetlb_report_node_meminfo(int nid, char *buf)
 {
 	struct hstate *h = &default_hstate;
+	if (!hugepages_supported())
+		return 0;
 	return sprintf(buf,
 		"Node %d HugePages_Total: %5u\n"
 		"Node %d HugePages_Free:  %5u\n"
@@ -2219,6 +2226,9 @@ void hugetlb_show_meminfo(void)
 	struct hstate *h;
 	int nid;
 
+	if (!hugepages_supported())
+		return;
+
 	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",
diff --git a/mm/kmemleak.c b/mm/kmemleak.c
index 91d67eaee05..8d2fcdfeff7 100644
--- a/mm/kmemleak.c
+++ b/mm/kmemleak.c
@@ -1775,10 +1775,9 @@ void __init kmemleak_init(void)
 	int i;
 	unsigned long flags;
 
-	kmemleak_early_log = 0;
-
 #ifdef CONFIG_DEBUG_KMEMLEAK_DEFAULT_OFF
 	if (!kmemleak_skip_disable) {
+		kmemleak_early_log = 0;
 		kmemleak_disable();
 		return;
 	}
@@ -1796,6 +1795,7 @@ void __init kmemleak_init(void)
 
 	/* the kernel is still in UP mode, so disabling the IRQs is enough */
 	local_irq_save(flags);
+	kmemleak_early_log = 0;
 	if (kmemleak_error) {
 		local_irq_restore(flags);
 		return;
diff --git a/mm/madvise.c b/mm/madvise.c
index 539eeb96b32..a402f8fdc68 100644
--- a/mm/madvise.c
+++ b/mm/madvise.c
@@ -195,7 +195,7 @@ static void force_shm_swapin_readahead(struct vm_area_struct *vma,
 	for (; start < end; start += PAGE_SIZE) {
 		index = ((start - vma->vm_start) >> PAGE_SHIFT) + vma->vm_pgoff;
 
-		page = find_get_page(mapping, index);
+		page = find_get_entry(mapping, index);
 		if (!radix_tree_exceptional_entry(page)) {
 			if (page)
 				page_cache_release(page);
diff --git a/mm/memblock.c b/mm/memblock.c
index e9d6ca9a01a..a810ba923cd 100644
--- a/mm/memblock.c
+++ b/mm/memblock.c
@@ -27,6 +27,9 @@
 
 static struct memblock_region memblock_memory_init_regions[INIT_MEMBLOCK_REGIONS] __initdata_memblock;
 static struct memblock_region memblock_reserved_init_regions[INIT_MEMBLOCK_REGIONS] __initdata_memblock;
+#ifdef CONFIG_HAVE_MEMBLOCK_PHYS_MAP
+static struct memblock_region memblock_physmem_init_regions[INIT_PHYSMEM_REGIONS] __initdata_memblock;
+#endif
 
 struct memblock memblock __initdata_memblock = {
 	.memory.regions		= memblock_memory_init_regions,
@@ -37,6 +40,12 @@ struct memblock memblock __initdata_memblock = {
 	.reserved.cnt		= 1,	/* empty dummy entry */
 	.reserved.max		= INIT_MEMBLOCK_REGIONS,
 
+#ifdef CONFIG_HAVE_MEMBLOCK_PHYS_MAP
+	.physmem.regions	= memblock_physmem_init_regions,
+	.physmem.cnt		= 1,	/* empty dummy entry */
+	.physmem.max		= INIT_PHYSMEM_REGIONS,
+#endif
+
 	.bottom_up		= false,
 	.current_limit		= MEMBLOCK_ALLOC_ANYWHERE,
 };
@@ -472,7 +481,7 @@ static void __init_memblock memblock_insert_region(struct memblock_type *type,
 }
 
 /**
- * memblock_add_region - add new memblock region
+ * memblock_add_range - add new memblock region
  * @type: memblock type to add new region into
  * @base: base address of the new region
  * @size: size of the new region
@@ -487,7 +496,7 @@ static void __init_memblock memblock_insert_region(struct memblock_type *type,
  * RETURNS:
  * 0 on success, -errno on failure.
  */
-static int __init_memblock memblock_add_region(struct memblock_type *type,
+int __init_memblock memblock_add_range(struct memblock_type *type,
 				phys_addr_t base, phys_addr_t size,
 				int nid, unsigned long flags)
 {
@@ -569,12 +578,12 @@ repeat:
 int __init_memblock memblock_add_node(phys_addr_t base, phys_addr_t size,
 				       int nid)
 {
-	return memblock_add_region(&memblock.memory, base, size, nid, 0);
+	return memblock_add_range(&memblock.memory, base, size, nid, 0);
 }
 
 int __init_memblock memblock_add(phys_addr_t base, phys_addr_t size)
 {
-	return memblock_add_region(&memblock.memory, base, size,
+	return memblock_add_range(&memblock.memory, base, size,
 				   MAX_NUMNODES, 0);
 }
 
@@ -654,8 +663,8 @@ static int __init_memblock memblock_isolate_range(struct memblock_type *type,
 	return 0;
 }
 
-static int __init_memblock __memblock_remove(struct memblock_type *type,
-					     phys_addr_t base, phys_addr_t size)
+int __init_memblock memblock_remove_range(struct memblock_type *type,
+					  phys_addr_t base, phys_addr_t size)
 {
 	int start_rgn, end_rgn;
 	int i, ret;
@@ -671,9 +680,10 @@ static int __init_memblock __memblock_remove(struct memblock_type *type,
 
 int __init_memblock memblock_remove(phys_addr_t base, phys_addr_t size)
 {
-	return __memblock_remove(&memblock.memory, base, size);
+	return memblock_remove_range(&memblock.memory, base, size);
 }
 
+
 int __init_memblock memblock_free(phys_addr_t base, phys_addr_t size)
 {
 	memblock_dbg("   memblock_free: [%#016llx-%#016llx] %pF\n",
@@ -681,7 +691,7 @@ int __init_memblock memblock_free(phys_addr_t base, phys_addr_t size)
 		     (unsigned long long)base + size - 1,
 		     (void *)_RET_IP_);
 
-	return __memblock_remove(&memblock.reserved, base, size);
+	return memblock_remove_range(&memblock.reserved, base, size);
 }
 
 static int __init_memblock memblock_reserve_region(phys_addr_t base,
@@ -696,7 +706,7 @@ static int __init_memblock memblock_reserve_region(phys_addr_t base,
 		     (unsigned long long)base + size - 1,
 		     flags, (void *)_RET_IP_);
 
-	return memblock_add_region(_rgn, base, size, nid, flags);
+	return memblock_add_range(_rgn, base, size, nid, flags);
 }
 
 int __init_memblock memblock_reserve(phys_addr_t base, phys_addr_t size)
@@ -758,17 +768,19 @@ int __init_memblock memblock_clear_hotplug(phys_addr_t base, phys_addr_t size)
 }
 
 /**
- * __next_free_mem_range - next function for for_each_free_mem_range()
+ * __next__mem_range - next function for for_each_free_mem_range() etc.
  * @idx: pointer to u64 loop variable
  * @nid: node selector, %NUMA_NO_NODE for all nodes
+ * @type_a: pointer to memblock_type from where the range is taken
+ * @type_b: pointer to memblock_type which excludes memory from being taken
  * @out_start: ptr to phys_addr_t for start address of the range, can be %NULL
  * @out_end: ptr to phys_addr_t for end address of the range, can be %NULL
  * @out_nid: ptr to int for nid of the range, can be %NULL
  *
- * Find the first free area from *@idx which matches @nid, fill the out
+ * Find the first area from *@idx which matches @nid, fill the out
  * parameters, and update *@idx for the next iteration.  The lower 32bit of
- * *@idx contains index into memory region and the upper 32bit indexes the
- * areas before each reserved region.  For example, if reserved regions
+ * *@idx contains index into type_a and the upper 32bit indexes the
+ * areas before each region in type_b.	For example, if type_b regions
  * look like the following,
  *
  *	0:[0-16), 1:[32-48), 2:[128-130)
@@ -780,53 +792,77 @@ int __init_memblock memblock_clear_hotplug(phys_addr_t base, phys_addr_t size)
  * As both region arrays are sorted, the function advances the two indices
  * in lockstep and returns each intersection.
  */
-void __init_memblock __next_free_mem_range(u64 *idx, int nid,
-					   phys_addr_t *out_start,
-					   phys_addr_t *out_end, int *out_nid)
+void __init_memblock __next_mem_range(u64 *idx, int nid,
+				      struct memblock_type *type_a,
+				      struct memblock_type *type_b,
+				      phys_addr_t *out_start,
+				      phys_addr_t *out_end, int *out_nid)
 {
-	struct memblock_type *mem = &memblock.memory;
-	struct memblock_type *rsv = &memblock.reserved;
-	int mi = *idx & 0xffffffff;
-	int ri = *idx >> 32;
+	int idx_a = *idx & 0xffffffff;
+	int idx_b = *idx >> 32;
 
-	if (WARN_ONCE(nid == MAX_NUMNODES, "Usage of MAX_NUMNODES is deprecated. Use NUMA_NO_NODE instead\n"))
+	if (WARN_ONCE(nid == MAX_NUMNODES,
+	"Usage of MAX_NUMNODES is deprecated. Use NUMA_NO_NODE instead\n"))
 		nid = NUMA_NO_NODE;
 
-	for ( ; mi < mem->cnt; mi++) {
-		struct memblock_region *m = &mem->regions[mi];
+	for (; idx_a < type_a->cnt; idx_a++) {
+		struct memblock_region *m = &type_a->regions[idx_a];
+
 		phys_addr_t m_start = m->base;
 		phys_addr_t m_end = m->base + m->size;
+		int	    m_nid = memblock_get_region_node(m);
 
 		/* only memory regions are associated with nodes, check it */
-		if (nid != NUMA_NO_NODE && nid != memblock_get_region_node(m))
+		if (nid != NUMA_NO_NODE && nid != m_nid)
 			continue;
 
-		/* scan areas before each reservation for intersection */
-		for ( ; ri < rsv->cnt + 1; ri++) {
-			struct memblock_region *r = &rsv->regions[ri];
-			phys_addr_t r_start = ri ? r[-1].base + r[-1].size : 0;
-			phys_addr_t r_end = ri < rsv->cnt ? r->base : ULLONG_MAX;
+		if (!type_b) {
+			if (out_start)
+				*out_start = m_start;
+			if (out_end)
+				*out_end = m_end;
+			if (out_nid)
+				*out_nid = m_nid;
+			idx_a++;
+			*idx = (u32)idx_a | (u64)idx_b << 32;
+			return;
+		}
 
-			/* if ri advanced past mi, break out to advance mi */
+		/* scan areas before each reservation */
+		for (; idx_b < type_b->cnt + 1; idx_b++) {
+			struct memblock_region *r;
+			phys_addr_t r_start;
+			phys_addr_t r_end;
+
+			r = &type_b->regions[idx_b];
+			r_start = idx_b ? r[-1].base + r[-1].size : 0;
+			r_end = idx_b < type_b->cnt ?
+				r->base : ULLONG_MAX;
+
+			/*
+			 * if idx_b advanced past idx_a,
+			 * break out to advance idx_a
+			 */
 			if (r_start >= m_end)
 				break;
 			/* if the two regions intersect, we're done */
 			if (m_start < r_end) {
 				if (out_start)
-					*out_start = max(m_start, r_start);
+					*out_start =
+						max(m_start, r_start);
 				if (out_end)
 					*out_end = min(m_end, r_end);
 				if (out_nid)
-					*out_nid = memblock_get_region_node(m);
+					*out_nid = m_nid;
 				/*
-				 * The region which ends first is advanced
-				 * for the next iteration.
+				 * The region which ends first is
+				 * advanced for the next iteration.
 				 */
 				if (m_end <= r_end)
-					mi++;
+					idx_a++;
 				else
-					ri++;
-				*idx = (u32)mi | (u64)ri << 32;
+					idx_b++;
+				*idx = (u32)idx_a | (u64)idx_b << 32;
 				return;
 			}
 		}
@@ -837,57 +873,80 @@ void __init_memblock __next_free_mem_range(u64 *idx, int nid,
 }
 
 /**
- * __next_free_mem_range_rev - next function for for_each_free_mem_range_reverse()
+ * __next_mem_range_rev - generic next function for for_each_*_range_rev()
+ *
+ * Finds the next range from type_a which is not marked as unsuitable
+ * in type_b.
+ *
  * @idx: pointer to u64 loop variable
  * @nid: nid: node selector, %NUMA_NO_NODE for all nodes
+ * @type_a: pointer to memblock_type from where the range is taken
+ * @type_b: pointer to memblock_type which excludes memory from being taken
  * @out_start: ptr to phys_addr_t for start address of the range, can be %NULL
  * @out_end: ptr to phys_addr_t for end address of the range, can be %NULL
  * @out_nid: ptr to int for nid of the range, can be %NULL
  *
- * Reverse of __next_free_mem_range().
- *
- * Linux kernel cannot migrate pages used by itself. Memory hotplug users won't
- * be able to hot-remove hotpluggable memory used by the kernel. So this
- * function skip hotpluggable regions if needed when allocating memory for the
- * kernel.
+ * Reverse of __next_mem_range().
  */
-void __init_memblock __next_free_mem_range_rev(u64 *idx, int nid,
-					   phys_addr_t *out_start,
-					   phys_addr_t *out_end, int *out_nid)
+void __init_memblock __next_mem_range_rev(u64 *idx, int nid,
+					  struct memblock_type *type_a,
+					  struct memblock_type *type_b,
+					  phys_addr_t *out_start,
+					  phys_addr_t *out_end, int *out_nid)
 {
-	struct memblock_type *mem = &memblock.memory;
-	struct memblock_type *rsv = &memblock.reserved;
-	int mi = *idx & 0xffffffff;
-	int ri = *idx >> 32;
+	int idx_a = *idx & 0xffffffff;
+	int idx_b = *idx >> 32;
 
 	if (WARN_ONCE(nid == MAX_NUMNODES, "Usage of MAX_NUMNODES is deprecated. Use NUMA_NO_NODE instead\n"))
 		nid = NUMA_NO_NODE;
 
 	if (*idx == (u64)ULLONG_MAX) {
-		mi = mem->cnt - 1;
-		ri = rsv->cnt;
+		idx_a = type_a->cnt - 1;
+		idx_b = type_b->cnt;
 	}
 
-	for ( ; mi >= 0; mi--) {
-		struct memblock_region *m = &mem->regions[mi];
+	for (; idx_a >= 0; idx_a--) {
+		struct memblock_region *m = &type_a->regions[idx_a];
+
 		phys_addr_t m_start = m->base;
 		phys_addr_t m_end = m->base + m->size;
+		int m_nid = memblock_get_region_node(m);
 
 		/* only memory regions are associated with nodes, check it */
-		if (nid != NUMA_NO_NODE && nid != memblock_get_region_node(m))
+		if (nid != NUMA_NO_NODE && nid != m_nid)
 			continue;
 
 		/* skip hotpluggable memory regions if needed */
 		if (movable_node_is_enabled() && memblock_is_hotpluggable(m))
 			continue;
 
-		/* scan areas before each reservation for intersection */
-		for ( ; ri >= 0; ri--) {
-			struct memblock_region *r = &rsv->regions[ri];
-			phys_addr_t r_start = ri ? r[-1].base + r[-1].size : 0;
-			phys_addr_t r_end = ri < rsv->cnt ? r->base : ULLONG_MAX;
+		if (!type_b) {
+			if (out_start)
+				*out_start = m_start;
+			if (out_end)
+				*out_end = m_end;
+			if (out_nid)
+				*out_nid = m_nid;
+			idx_a++;
+			*idx = (u32)idx_a | (u64)idx_b << 32;
+			return;
+		}
+
+		/* scan areas before each reservation */
+		for (; idx_b >= 0; idx_b--) {
+			struct memblock_region *r;
+			phys_addr_t r_start;
+			phys_addr_t r_end;
+
+			r = &type_b->regions[idx_b];
+			r_start = idx_b ? r[-1].base + r[-1].size : 0;
+			r_end = idx_b < type_b->cnt ?
+				r->base : ULLONG_MAX;
+			/*
+			 * if idx_b advanced past idx_a,
+			 * break out to advance idx_a
+			 */
 
-			/* if ri advanced past mi, break out to advance mi */
 			if (r_end <= m_start)
 				break;
 			/* if the two regions intersect, we're done */
@@ -897,18 +956,17 @@ void __init_memblock __next_free_mem_range_rev(u64 *idx, int nid,
 				if (out_end)
 					*out_end = min(m_end, r_end);
 				if (out_nid)
-					*out_nid = memblock_get_region_node(m);
-
+					*out_nid = m_nid;
 				if (m_start >= r_start)
-					mi--;
+					idx_a--;
 				else
-					ri--;
-				*idx = (u32)mi | (u64)ri << 32;
+					idx_b--;
+				*idx = (u32)idx_a | (u64)idx_b << 32;
 				return;
 			}
 		}
 	}
-
+	/* signal end of iteration */
 	*idx = ULLONG_MAX;
 }
 
@@ -1201,7 +1259,7 @@ void __init __memblock_free_early(phys_addr_t base, phys_addr_t size)
 		     __func__, (u64)base, (u64)base + size - 1,
 		     (void *)_RET_IP_);
 	kmemleak_free_part(__va(base), size);
-	__memblock_remove(&memblock.reserved, base, size);
+	memblock_remove_range(&memblock.reserved, base, size);
 }
 
 /*
@@ -1287,8 +1345,10 @@ void __init memblock_enforce_memory_limit(phys_addr_t limit)
 	}
 
 	/* truncate both memory and reserved regions */
-	__memblock_remove(&memblock.memory, max_addr, (phys_addr_t)ULLONG_MAX);
-	__memblock_remove(&memblock.reserved, max_addr, (phys_addr_t)ULLONG_MAX);
+	memblock_remove_range(&memblock.memory, max_addr,
+			      (phys_addr_t)ULLONG_MAX);
+	memblock_remove_range(&memblock.reserved, max_addr,
+			      (phys_addr_t)ULLONG_MAX);
 }
 
 static int __init_memblock memblock_search(struct memblock_type *type, phys_addr_t addr)
@@ -1502,6 +1562,9 @@ static int __init memblock_init_debugfs(void)
 		return -ENXIO;
 	debugfs_create_file("memory", S_IRUGO, root, &memblock.memory, &memblock_debug_fops);
 	debugfs_create_file("reserved", S_IRUGO, root, &memblock.reserved, &memblock_debug_fops);
+#ifdef CONFIG_HAVE_MEMBLOCK_PHYS_MAP
+	debugfs_create_file("physmem", S_IRUGO, root, &memblock.physmem, &memblock_debug_fops);
+#endif
 
 	return 0;
 }
diff --git a/mm/memcontrol.c b/mm/memcontrol.c
index 29501f04056..5177c6d4a2d 100644
--- a/mm/memcontrol.c
+++ b/mm/memcontrol.c
@@ -1077,9 +1077,18 @@ static struct mem_cgroup *get_mem_cgroup_from_mm(struct mm_struct *mm)
 
 	rcu_read_lock();
 	do {
-		memcg = mem_cgroup_from_task(rcu_dereference(mm->owner));
-		if (unlikely(!memcg))
+		/*
+		 * Page cache insertions can happen withou an
+		 * actual mm context, e.g. during disk probing
+		 * on boot, loopback IO, acct() writes etc.
+		 */
+		if (unlikely(!mm))
 			memcg = root_mem_cgroup;
+		else {
+			memcg = mem_cgroup_from_task(rcu_dereference(mm->owner));
+			if (unlikely(!memcg))
+				memcg = root_mem_cgroup;
+		}
 	} while (!css_tryget(&memcg->css));
 	rcu_read_unlock();
 	return memcg;
@@ -3958,17 +3967,9 @@ int mem_cgroup_charge_file(struct page *page, struct mm_struct *mm,
 		return 0;
 	}
 
-	/*
-	 * Page cache insertions can happen without an actual mm
-	 * context, e.g. during disk probing on boot.
-	 */
-	if (unlikely(!mm))
-		memcg = root_mem_cgroup;
-	else {
-		memcg = mem_cgroup_try_charge_mm(mm, gfp_mask, 1, true);
-		if (!memcg)
-			return -ENOMEM;
-	}
+	memcg = mem_cgroup_try_charge_mm(mm, gfp_mask, 1, true);
+	if (!memcg)
+		return -ENOMEM;
 	__mem_cgroup_commit_charge(memcg, page, 1, type, false);
 	return 0;
 }
@@ -6686,16 +6687,20 @@ static struct page *mc_handle_file_pte(struct vm_area_struct *vma,
 		pgoff = pte_to_pgoff(ptent);
 
 	/* page is moved even if it's not RSS of this task(page-faulted). */
-	page = find_get_page(mapping, pgoff);
-
 #ifdef CONFIG_SWAP
 	/* shmem/tmpfs may report page out on swap: account for that too. */
-	if (radix_tree_exceptional_entry(page)) {
-		swp_entry_t swap = radix_to_swp_entry(page);
-		if (do_swap_account)
-			*entry = swap;
-		page = find_get_page(swap_address_space(swap), swap.val);
-	}
+	if (shmem_mapping(mapping)) {
+		page = find_get_entry(mapping, pgoff);
+		if (radix_tree_exceptional_entry(page)) {
+			swp_entry_t swp = radix_to_swp_entry(page);
+			if (do_swap_account)
+				*entry = swp;
+			page = find_get_page(swap_address_space(swp), swp.val);
+		}
+	} else
+		page = find_get_page(mapping, pgoff);
+#else
+	page = find_get_page(mapping, pgoff);
 #endif
 	return page;
 }
diff --git a/mm/memory-failure.c b/mm/memory-failure.c
index 35ef28acf13..9ccef39a9de 100644
--- a/mm/memory-failure.c
+++ b/mm/memory-failure.c
@@ -1081,15 +1081,16 @@ int memory_failure(unsigned long pfn, int trapno, int flags)
 			return 0;
 		} else if (PageHuge(hpage)) {
 			/*
-			 * Check "just unpoisoned", "filter hit", and
-			 * "race with other subpage."
+			 * Check "filter hit" and "race with other subpage."
 			 */
 			lock_page(hpage);
-			if (!PageHWPoison(hpage)
-			    || (hwpoison_filter(p) && TestClearPageHWPoison(p))
-			    || (p != hpage && TestSetPageHWPoison(hpage))) {
-				atomic_long_sub(nr_pages, &num_poisoned_pages);
-				return 0;
+			if (PageHWPoison(hpage)) {
+				if ((hwpoison_filter(p) && TestClearPageHWPoison(p))
+				    || (p != hpage && TestSetPageHWPoison(hpage))) {
+					atomic_long_sub(nr_pages, &num_poisoned_pages);
+					unlock_page(hpage);
+					return 0;
+				}
 			}
 			set_page_hwpoison_huge_page(hpage);
 			res = dequeue_hwpoisoned_huge_page(hpage);
@@ -1152,6 +1153,8 @@ int memory_failure(unsigned long pfn, int trapno, int flags)
 	 */
 	if (!PageHWPoison(p)) {
 		printk(KERN_ERR "MCE %#lx: just unpoisoned\n", pfn);
+		atomic_long_sub(nr_pages, &num_poisoned_pages);
+		put_page(hpage);
 		res = 0;
 		goto out;
 	}
diff --git a/mm/memory.c b/mm/memory.c
index d0f0bef3be4..037b812a953 100644
--- a/mm/memory.c
+++ b/mm/memory.c
@@ -232,17 +232,18 @@ void tlb_gather_mmu(struct mmu_gather *tlb, struct mm_struct *mm, unsigned long
 #endif
 }
 
-void tlb_flush_mmu(struct mmu_gather *tlb)
+static void tlb_flush_mmu_tlbonly(struct mmu_gather *tlb)
 {
-	struct mmu_gather_batch *batch;
-
-	if (!tlb->need_flush)
-		return;
 	tlb->need_flush = 0;
 	tlb_flush(tlb);
 #ifdef CONFIG_HAVE_RCU_TABLE_FREE
 	tlb_table_flush(tlb);
 #endif
+}
+
+static void tlb_flush_mmu_free(struct mmu_gather *tlb)
+{
+	struct mmu_gather_batch *batch;
 
 	for (batch = &tlb->local; batch; batch = batch->next) {
 		free_pages_and_swap_cache(batch->pages, batch->nr);
@@ -251,6 +252,14 @@ void tlb_flush_mmu(struct mmu_gather *tlb)
 	tlb->active = &tlb->local;
 }
 
+void tlb_flush_mmu(struct mmu_gather *tlb)
+{
+	if (!tlb->need_flush)
+		return;
+	tlb_flush_mmu_tlbonly(tlb);
+	tlb_flush_mmu_free(tlb);
+}
+
 /* tlb_finish_mmu
  *	Called at the end of the shootdown operation to free up any resources
  *	that were required.
@@ -1127,8 +1136,10 @@ again:
 			if (PageAnon(page))
 				rss[MM_ANONPAGES]--;
 			else {
-				if (pte_dirty(ptent))
+				if (pte_dirty(ptent)) {
+					force_flush = 1;
 					set_page_dirty(page);
+				}
 				if (pte_young(ptent) &&
 				    likely(!(vma->vm_flags & VM_SEQ_READ)))
 					mark_page_accessed(page);
@@ -1137,9 +1148,10 @@ again:
 			page_remove_rmap(page);
 			if (unlikely(page_mapcount(page) < 0))
 				print_bad_pte(vma, addr, ptent, page);
-			force_flush = !__tlb_remove_page(tlb, page);
-			if (force_flush)
+			if (unlikely(!__tlb_remove_page(tlb, page))) {
+				force_flush = 1;
 				break;
+			}
 			continue;
 		}
 		/*
@@ -1174,18 +1186,11 @@ again:
 
 	add_mm_rss_vec(mm, rss);
 	arch_leave_lazy_mmu_mode();
-	pte_unmap_unlock(start_pte, ptl);
 
-	/*
-	 * mmu_gather ran out of room to batch pages, we break out of
-	 * the PTE lock to avoid doing the potential expensive TLB invalidate
-	 * and page-free while holding it.
-	 */
+	/* Do the actual TLB flush before dropping ptl */
 	if (force_flush) {
 		unsigned long old_end;
 
-		force_flush = 0;
-
 		/*
 		 * Flush the TLB just for the previous segment,
 		 * then update the range to be the remaining
@@ -1193,11 +1198,21 @@ again:
 		 */
 		old_end = tlb->end;
 		tlb->end = addr;
-
-		tlb_flush_mmu(tlb);
-
+		tlb_flush_mmu_tlbonly(tlb);
 		tlb->start = addr;
 		tlb->end = old_end;
+	}
+	pte_unmap_unlock(start_pte, ptl);
+
+	/*
+	 * If we forced a TLB flush (either due to running out of
+	 * batch buffers or because we needed to flush dirty TLB
+	 * entries before releasing the ptl), free the batched
+	 * memory too. Restart if we didn't do everything.
+	 */
+	if (force_flush) {
+		force_flush = 0;
+		tlb_flush_mmu_free(tlb);
 
 		if (addr != end)
 			goto again;
@@ -1955,12 +1970,17 @@ int fixup_user_fault(struct task_struct *tsk, struct mm_struct *mm,
 		     unsigned long address, unsigned int fault_flags)
 {
 	struct vm_area_struct *vma;
+	vm_flags_t vm_flags;
 	int ret;
 
 	vma = find_extend_vma(mm, address);
 	if (!vma || address < vma->vm_start)
 		return -EFAULT;
 
+	vm_flags = (fault_flags & FAULT_FLAG_WRITE) ? VM_WRITE : VM_READ;
+	if (!(vm_flags & vma->vm_flags))
+		return -EFAULT;
+
 	ret = handle_mm_fault(mm, vma, address, fault_flags);
 	if (ret & VM_FAULT_ERROR) {
 		if (ret & VM_FAULT_OOM)
diff --git a/mm/mremap.c b/mm/mremap.c
index 0843feb66f3..05f1180e9f2 100644
--- a/mm/mremap.c
+++ b/mm/mremap.c
@@ -194,10 +194,17 @@ unsigned long move_page_tables(struct vm_area_struct *vma,
 			break;
 		if (pmd_trans_huge(*old_pmd)) {
 			int err = 0;
-			if (extent == HPAGE_PMD_SIZE)
+			if (extent == HPAGE_PMD_SIZE) {
+				VM_BUG_ON(vma->vm_file || !vma->anon_vma);
+				/* See comment in move_ptes() */
+				if (need_rmap_locks)
+					anon_vma_lock_write(vma->anon_vma);
 				err = move_huge_pmd(vma, new_vma, old_addr,
 						    new_addr, old_end,
 						    old_pmd, new_pmd);
+				if (need_rmap_locks)
+					anon_vma_unlock_write(vma->anon_vma);
+			}
 			if (err > 0) {
 				need_flush = true;
 				continue;
diff --git a/mm/page-writeback.c b/mm/page-writeback.c
index ef413492a14..a4317da6053 100644
--- a/mm/page-writeback.c
+++ b/mm/page-writeback.c
@@ -593,14 +593,14 @@ unsigned long bdi_dirty_limit(struct backing_dev_info *bdi, unsigned long dirty)
  * (5) the closer to setpoint, the smaller |df/dx| (and the reverse)
  *     => fast response on large errors; small oscillation near setpoint
  */
-static inline long long pos_ratio_polynom(unsigned long setpoint,
+static long long pos_ratio_polynom(unsigned long setpoint,
 					  unsigned long dirty,
 					  unsigned long limit)
 {
 	long long pos_ratio;
 	long x;
 
-	x = div_s64(((s64)setpoint - (s64)dirty) << RATELIMIT_CALC_SHIFT,
+	x = div64_s64(((s64)setpoint - (s64)dirty) << RATELIMIT_CALC_SHIFT,
 		    limit - setpoint + 1);
 	pos_ratio = x;
 	pos_ratio = pos_ratio * x >> RATELIMIT_CALC_SHIFT;
@@ -842,7 +842,7 @@ static unsigned long bdi_position_ratio(struct backing_dev_info *bdi,
 	x_intercept = bdi_setpoint + span;
 
 	if (bdi_dirty < x_intercept - span / 4) {
-		pos_ratio = div_u64(pos_ratio * (x_intercept - bdi_dirty),
+		pos_ratio = div64_u64(pos_ratio * (x_intercept - bdi_dirty),
 				    x_intercept - bdi_setpoint + 1);
 	} else
 		pos_ratio /= 4;
diff --git a/mm/percpu.c b/mm/percpu.c
index 63e24fb4387..2ddf9a990db 100644
--- a/mm/percpu.c
+++ b/mm/percpu.c
@@ -610,7 +610,7 @@ static struct pcpu_chunk *pcpu_alloc_chunk(void)
 	chunk->map = pcpu_mem_zalloc(PCPU_DFL_MAP_ALLOC *
 						sizeof(chunk->map[0]));
 	if (!chunk->map) {
-		kfree(chunk);
+		pcpu_mem_free(chunk, pcpu_chunk_struct_size);
 		return NULL;
 	}
 
diff --git a/mm/slab.c b/mm/slab.c
index 388cb1ae6fb..19d92181ce2 100644
--- a/mm/slab.c
+++ b/mm/slab.c
@@ -166,7 +166,7 @@ typedef unsigned char freelist_idx_t;
 typedef unsigned short freelist_idx_t;
 #endif
 
-#define SLAB_OBJ_MAX_NUM (1 << sizeof(freelist_idx_t) * BITS_PER_BYTE)
+#define SLAB_OBJ_MAX_NUM ((1 << sizeof(freelist_idx_t) * BITS_PER_BYTE) - 1)
 
 /*
  * true if a page was allocated from pfmemalloc reserves for network-based
@@ -2572,13 +2572,13 @@ static void *alloc_slabmgmt(struct kmem_cache *cachep,
 	return freelist;
 }
 
-static inline freelist_idx_t get_free_obj(struct page *page, unsigned char idx)
+static inline freelist_idx_t get_free_obj(struct page *page, unsigned int idx)
 {
 	return ((freelist_idx_t *)page->freelist)[idx];
 }
 
 static inline void set_free_obj(struct page *page,
-					unsigned char idx, freelist_idx_t val)
+					unsigned int idx, freelist_idx_t val)
 {
 	((freelist_idx_t *)(page->freelist))[idx] = val;
 }
diff --git a/mm/slab.h b/mm/slab.h
index 3045316b7c9..6bd4c353704 100644
--- a/mm/slab.h
+++ b/mm/slab.h
@@ -91,6 +91,7 @@ __kmem_cache_alias(const char *name, size_t size, size_t align,
 #define CACHE_CREATE_MASK (SLAB_CORE_FLAGS | SLAB_DEBUG_FLAGS | SLAB_CACHE_FLAGS)
 
 int __kmem_cache_shutdown(struct kmem_cache *);
+void slab_kmem_cache_release(struct kmem_cache *);
 
 struct seq_file;
 struct file;
diff --git a/mm/slab_common.c b/mm/slab_common.c
index f3cfccf76dd..102cc6fca3d 100644
--- a/mm/slab_common.c
+++ b/mm/slab_common.c
@@ -323,6 +323,12 @@ static int kmem_cache_destroy_memcg_children(struct kmem_cache *s)
 }
 #endif /* CONFIG_MEMCG_KMEM */
 
+void slab_kmem_cache_release(struct kmem_cache *s)
+{
+	kfree(s->name);
+	kmem_cache_free(kmem_cache, s);
+}
+
 void kmem_cache_destroy(struct kmem_cache *s)
 {
 	get_online_cpus();
@@ -352,8 +358,11 @@ void kmem_cache_destroy(struct kmem_cache *s)
 		rcu_barrier();
 
 	memcg_free_cache_params(s);
-	kfree(s->name);
-	kmem_cache_free(kmem_cache, s);
+#ifdef SLAB_SUPPORTS_SYSFS
+	sysfs_slab_remove(s);
+#else
+	slab_kmem_cache_release(s);
+#endif
 	goto out_put_cpus;
 
 out_unlock:
diff --git a/mm/slub.c b/mm/slub.c
index 5e234f1f885..2b1ce697fc4 100644
--- a/mm/slub.c
+++ b/mm/slub.c
@@ -210,14 +210,11 @@ enum track_item { TRACK_ALLOC, TRACK_FREE };
 #ifdef CONFIG_SYSFS
 static int sysfs_slab_add(struct kmem_cache *);
 static int sysfs_slab_alias(struct kmem_cache *, const char *);
-static void sysfs_slab_remove(struct kmem_cache *);
 static void memcg_propagate_slab_attrs(struct kmem_cache *s);
 #else
 static inline int sysfs_slab_add(struct kmem_cache *s) { return 0; }
 static inline int sysfs_slab_alias(struct kmem_cache *s, const char *p)
 							{ return 0; }
-static inline void sysfs_slab_remove(struct kmem_cache *s) { }
-
 static inline void memcg_propagate_slab_attrs(struct kmem_cache *s) { }
 #endif
 
@@ -3238,24 +3235,7 @@ static inline int kmem_cache_close(struct kmem_cache *s)
 
 int __kmem_cache_shutdown(struct kmem_cache *s)
 {
-	int rc = kmem_cache_close(s);
-
-	if (!rc) {
-		/*
-		 * Since slab_attr_store may take the slab_mutex, we should
-		 * release the lock while removing the sysfs entry in order to
-		 * avoid a deadlock. Because this is pretty much the last
-		 * operation we do and the lock will be released shortly after
-		 * that in slab_common.c, we could just move sysfs_slab_remove
-		 * to a later point in common code. We should do that when we
-		 * have a common sysfs framework for all allocators.
-		 */
-		mutex_unlock(&slab_mutex);
-		sysfs_slab_remove(s);
-		mutex_lock(&slab_mutex);
-	}
-
-	return rc;
+	return kmem_cache_close(s);
 }
 
 /********************************************************************
@@ -5071,15 +5051,18 @@ static void memcg_propagate_slab_attrs(struct kmem_cache *s)
 #ifdef CONFIG_MEMCG_KMEM
 	int i;
 	char *buffer = NULL;
+	struct kmem_cache *root_cache;
 
-	if (!is_root_cache(s))
+	if (is_root_cache(s))
 		return;
 
+	root_cache = s->memcg_params->root_cache;
+
 	/*
 	 * This mean this cache had no attribute written. Therefore, no point
 	 * in copying default values around
 	 */
-	if (!s->max_attr_size)
+	if (!root_cache->max_attr_size)
 		return;
 
 	for (i = 0; i < ARRAY_SIZE(slab_attrs); i++) {
@@ -5101,7 +5084,7 @@ static void memcg_propagate_slab_attrs(struct kmem_cache *s)
 		 */
 		if (buffer)
 			buf = buffer;
-		else if (s->max_attr_size < ARRAY_SIZE(mbuf))
+		else if (root_cache->max_attr_size < ARRAY_SIZE(mbuf))
 			buf = mbuf;
 		else {
 			buffer = (char *) get_zeroed_page(GFP_KERNEL);
@@ -5110,7 +5093,7 @@ static void memcg_propagate_slab_attrs(struct kmem_cache *s)
 			buf = buffer;
 		}
 
-		attr->show(s->memcg_params->root_cache, buf);
+		attr->show(root_cache, buf);
 		attr->store(s, buf, strlen(buf));
 	}
 
@@ -5119,6 +5102,11 @@ static void memcg_propagate_slab_attrs(struct kmem_cache *s)
 #endif
 }
 
+static void kmem_cache_release(struct kobject *k)
+{
+	slab_kmem_cache_release(to_slab(k));
+}
+
 static const struct sysfs_ops slab_sysfs_ops = {
 	.show = slab_attr_show,
 	.store = slab_attr_store,
@@ -5126,6 +5114,7 @@ static const struct sysfs_ops slab_sysfs_ops = {
 
 static struct kobj_type slab_ktype = {
 	.sysfs_ops = &slab_sysfs_ops,
+	.release = kmem_cache_release,
 };
 
 static int uevent_filter(struct kset *kset, struct kobject *kobj)
@@ -5252,7 +5241,7 @@ out_put_kobj:
 	goto out;
 }
 
-static void sysfs_slab_remove(struct kmem_cache *s)
+void sysfs_slab_remove(struct kmem_cache *s)
 {
 	if (slab_state < FULL)
 		/*
diff --git a/mm/truncate.c b/mm/truncate.c
index e5cc39ab075..6a78c814beb 100644
--- a/mm/truncate.c
+++ b/mm/truncate.c
@@ -484,14 +484,6 @@ unsigned long invalidate_mapping_pages(struct address_space *mapping,
 	unsigned long count = 0;
 	int i;
 
-	/*
-	 * Note: this function may get called on a shmem/tmpfs mapping:
-	 * pagevec_lookup() might then return 0 prematurely (because it
-	 * got a gangful of swap entries); but it's hardly worth worrying
-	 * about - it can rarely have anything to free from such a mapping
-	 * (most pages are dirty), and already skips over any difficulties.
-	 */
-
 	pagevec_init(&pvec, 0);
 	while (index <= end && pagevec_lookup_entries(&pvec, mapping, index,
 			min(end - index, (pgoff_t)PAGEVEC_SIZE - 1) + 1,
diff --git a/mm/util.c b/mm/util.c
index f380af7ea77..d5ea733c508 100644
--- a/mm/util.c
+++ b/mm/util.c
@@ -10,6 +10,7 @@
 #include <linux/swapops.h>
 #include <linux/mman.h>
 #include <linux/hugetlb.h>
+#include <linux/vmalloc.h>
 
 #include <asm/uaccess.h>
 
@@ -387,6 +388,15 @@ unsigned long vm_mmap(struct file *file, unsigned long addr,
 }
 EXPORT_SYMBOL(vm_mmap);
 
+void kvfree(const void *addr)
+{
+	if (is_vmalloc_addr(addr))
+		vfree(addr);
+	else
+		kfree(addr);
+}
+EXPORT_SYMBOL(kvfree);
+
 struct address_space *page_mapping(struct page *page)
 {
 	struct address_space *mapping = page->mapping;
diff --git a/mm/vmacache.c b/mm/vmacache.c
index d4224b397c0..1037a3bab50 100644
--- a/mm/vmacache.c
+++ b/mm/vmacache.c
@@ -81,10 +81,12 @@ struct vm_area_struct *vmacache_find(struct mm_struct *mm, unsigned long addr)
 	for (i = 0; i < VMACACHE_SIZE; i++) {
 		struct vm_area_struct *vma = current->vmacache[i];
 
-		if (vma && vma->vm_start <= addr && vma->vm_end > addr) {
-			BUG_ON(vma->vm_mm != mm);
+		if (!vma)
+			continue;
+		if (WARN_ON_ONCE(vma->vm_mm != mm))
+			break;
+		if (vma->vm_start <= addr && vma->vm_end > addr)
 			return vma;
-		}
 	}
 
 	return NULL;
diff --git a/mm/vmscan.c b/mm/vmscan.c
index 9b6497eda80..32c661d66a4 100644
--- a/mm/vmscan.c
+++ b/mm/vmscan.c
@@ -1158,7 +1158,7 @@ unsigned long reclaim_clean_pages_from_list(struct zone *zone,
 			TTU_UNMAP|TTU_IGNORE_ACCESS,
 			&dummy1, &dummy2, &dummy3, &dummy4, &dummy5, true);
 	list_splice(&clean_pages, page_list);
-	__mod_zone_page_state(zone, NR_ISOLATED_FILE, -ret);
+	mod_zone_page_state(zone, NR_ISOLATED_FILE, -ret);
 	return ret;
 }
 
@@ -1916,6 +1916,24 @@ static void get_scan_count(struct lruvec *lruvec, struct scan_control *sc,
 		get_lru_size(lruvec, LRU_INACTIVE_FILE);
 
 	/*
+	 * Prevent the reclaimer from falling into the cache trap: as
+	 * cache pages start out inactive, every cache fault will tip
+	 * the scan balance towards the file LRU.  And as the file LRU
+	 * shrinks, so does the window for rotation from references.
+	 * This means we have a runaway feedback loop where a tiny
+	 * thrashing file LRU becomes infinitely more attractive than
+	 * anon pages.  Try to detect this based on file LRU size.
+	 */
+	if (global_reclaim(sc)) {
+		unsigned long free = zone_page_state(zone, NR_FREE_PAGES);
+
+		if (unlikely(file + free <= high_wmark_pages(zone))) {
+			scan_balance = SCAN_ANON;
+			goto out;
+		}
+	}
+
+	/*
 	 * There is enough inactive page cache, do not reclaim
 	 * anything from the anonymous working set right now.
 	 */
diff --git a/net/8021q/vlan.c b/net/8021q/vlan.c
index 175273f38cb..44ebd5c2cd4 100644
--- a/net/8021q/vlan.c
+++ b/net/8021q/vlan.c
@@ -169,6 +169,7 @@ int register_vlan_dev(struct net_device *dev)
 	if (err < 0)
 		goto out_uninit_mvrp;
 
+	vlan->nest_level = dev_get_nest_level(real_dev, is_vlan_dev) + 1;
 	err = register_netdevice(dev);
 	if (err < 0)
 		goto out_uninit_mvrp;
diff --git a/net/8021q/vlan_dev.c b/net/8021q/vlan_dev.c
index 6f142f03716..019efb79708 100644
--- a/net/8021q/vlan_dev.c
+++ b/net/8021q/vlan_dev.c
@@ -524,6 +524,11 @@ static void vlan_dev_set_lockdep_class(struct net_device *dev, int subclass)
 	netdev_for_each_tx_queue(dev, vlan_dev_set_lockdep_one, &subclass);
 }
 
+static int vlan_dev_get_lock_subclass(struct net_device *dev)
+{
+	return vlan_dev_priv(dev)->nest_level;
+}
+
 static const struct header_ops vlan_header_ops = {
 	.create	 = vlan_dev_hard_header,
 	.rebuild = vlan_dev_rebuild_header,
@@ -559,7 +564,6 @@ static const struct net_device_ops vlan_netdev_ops;
 static int vlan_dev_init(struct net_device *dev)
 {
 	struct net_device *real_dev = vlan_dev_priv(dev)->real_dev;
-	int subclass = 0;
 
 	netif_carrier_off(dev);
 
@@ -608,10 +612,7 @@ static int vlan_dev_init(struct net_device *dev)
 
 	SET_NETDEV_DEVTYPE(dev, &vlan_type);
 
-	if (is_vlan_dev(real_dev))
-		subclass = 1;
-
-	vlan_dev_set_lockdep_class(dev, subclass);
+	vlan_dev_set_lockdep_class(dev, vlan_dev_get_lock_subclass(dev));
 
 	vlan_dev_priv(dev)->vlan_pcpu_stats = netdev_alloc_pcpu_stats(struct vlan_pcpu_stats);
 	if (!vlan_dev_priv(dev)->vlan_pcpu_stats)
@@ -783,6 +784,7 @@ static const struct net_device_ops vlan_netdev_ops = {
 	.ndo_netpoll_cleanup	= vlan_dev_netpoll_cleanup,
 #endif
 	.ndo_fix_features	= vlan_dev_fix_features,
+	.ndo_get_lock_subclass  = vlan_dev_get_lock_subclass,
 };
 
 void vlan_setup(struct net_device *dev)
diff --git a/net/batman-adv/bat_iv_ogm.c b/net/batman-adv/bat_iv_ogm.c
index b3bd4ec3fd9..f04224c3200 100644
--- a/net/batman-adv/bat_iv_ogm.c
+++ b/net/batman-adv/bat_iv_ogm.c
@@ -1545,6 +1545,8 @@ out_neigh:
 	if ((orig_neigh_node) && (!is_single_hop_neigh))
 		batadv_orig_node_free_ref(orig_neigh_node);
 out:
+	if (router_ifinfo)
+		batadv_neigh_ifinfo_free_ref(router_ifinfo);
 	if (router)
 		batadv_neigh_node_free_ref(router);
 	if (router_router)
diff --git a/net/batman-adv/distributed-arp-table.c b/net/batman-adv/distributed-arp-table.c
index b25fd64d727..aa5d4946d0d 100644
--- a/net/batman-adv/distributed-arp-table.c
+++ b/net/batman-adv/distributed-arp-table.c
@@ -940,8 +940,7 @@ bool batadv_dat_snoop_outgoing_arp_request(struct batadv_priv *bat_priv,
 		 * additional DAT answer may trigger kernel warnings about
 		 * a packet coming from the wrong port.
 		 */
-		if (batadv_is_my_client(bat_priv, dat_entry->mac_addr,
-					BATADV_NO_FLAGS)) {
+		if (batadv_is_my_client(bat_priv, dat_entry->mac_addr, vid)) {
 			ret = true;
 			goto out;
 		}
diff --git a/net/batman-adv/fragmentation.c b/net/batman-adv/fragmentation.c
index bcc4bea632f..f14e54a0569 100644
--- a/net/batman-adv/fragmentation.c
+++ b/net/batman-adv/fragmentation.c
@@ -418,12 +418,13 @@ bool batadv_frag_send_packet(struct sk_buff *skb,
 			     struct batadv_neigh_node *neigh_node)
 {
 	struct batadv_priv *bat_priv;
-	struct batadv_hard_iface *primary_if;
+	struct batadv_hard_iface *primary_if = NULL;
 	struct batadv_frag_packet frag_header;
 	struct sk_buff *skb_fragment;
 	unsigned mtu = neigh_node->if_incoming->net_dev->mtu;
 	unsigned header_size = sizeof(frag_header);
 	unsigned max_fragment_size, max_packet_size;
+	bool ret = false;
 
 	/* To avoid merge and refragmentation at next-hops we never send
 	 * fragments larger than BATADV_FRAG_MAX_FRAG_SIZE
@@ -483,7 +484,11 @@ bool batadv_frag_send_packet(struct sk_buff *skb,
 			   skb->len + ETH_HLEN);
 	batadv_send_skb_packet(skb, neigh_node->if_incoming, neigh_node->addr);
 
-	return true;
+	ret = true;
+
 out_err:
-	return false;
+	if (primary_if)
+		batadv_hardif_free_ref(primary_if);
+
+	return ret;
 }
diff --git a/net/batman-adv/gateway_client.c b/net/batman-adv/gateway_client.c
index c835e137423..90cff585b37 100644
--- a/net/batman-adv/gateway_client.c
+++ b/net/batman-adv/gateway_client.c
@@ -42,8 +42,10 @@
 
 static void batadv_gw_node_free_ref(struct batadv_gw_node *gw_node)
 {
-	if (atomic_dec_and_test(&gw_node->refcount))
+	if (atomic_dec_and_test(&gw_node->refcount)) {
+		batadv_orig_node_free_ref(gw_node->orig_node);
 		kfree_rcu(gw_node, rcu);
+	}
 }
 
 static struct batadv_gw_node *
@@ -406,9 +408,14 @@ static void batadv_gw_node_add(struct batadv_priv *bat_priv,
 	if (gateway->bandwidth_down == 0)
 		return;
 
+	if (!atomic_inc_not_zero(&orig_node->refcount))
+		return;
+
 	gw_node = kzalloc(sizeof(*gw_node), GFP_ATOMIC);
-	if (!gw_node)
+	if (!gw_node) {
+		batadv_orig_node_free_ref(orig_node);
 		return;
+	}
 
 	INIT_HLIST_NODE(&gw_node->list);
 	gw_node->orig_node = orig_node;
diff --git a/net/batman-adv/hard-interface.c b/net/batman-adv/hard-interface.c
index b851cc58085..fbda6b54baf 100644
--- a/net/batman-adv/hard-interface.c
+++ b/net/batman-adv/hard-interface.c
@@ -83,7 +83,7 @@ static bool batadv_is_on_batman_iface(const struct net_device *net_dev)
 		return true;
 
 	/* no more parents..stop recursion */
-	if (net_dev->iflink == net_dev->ifindex)
+	if (net_dev->iflink == 0 || net_dev->iflink == net_dev->ifindex)
 		return false;
 
 	/* recurse over the parent device */
diff --git a/net/batman-adv/originator.c b/net/batman-adv/originator.c
index ffd9dfbd9b0..6a484514cd3 100644
--- a/net/batman-adv/originator.c
+++ b/net/batman-adv/originator.c
@@ -501,12 +501,17 @@ batadv_neigh_node_get(const struct batadv_orig_node *orig_node,
 static void batadv_orig_ifinfo_free_rcu(struct rcu_head *rcu)
 {
 	struct batadv_orig_ifinfo *orig_ifinfo;
+	struct batadv_neigh_node *router;
 
 	orig_ifinfo = container_of(rcu, struct batadv_orig_ifinfo, rcu);
 
 	if (orig_ifinfo->if_outgoing != BATADV_IF_DEFAULT)
 		batadv_hardif_free_ref_now(orig_ifinfo->if_outgoing);
 
+	/* this is the last reference to this object */
+	router = rcu_dereference_protected(orig_ifinfo->router, true);
+	if (router)
+		batadv_neigh_node_free_ref_now(router);
 	kfree(orig_ifinfo);
 }
 
@@ -702,6 +707,47 @@ free_orig_node:
 }
 
 /**
+ * batadv_purge_neigh_ifinfo - purge obsolete ifinfo entries from neighbor
+ * @bat_priv: the bat priv with all the soft interface information
+ * @neigh: orig node which is to be checked
+ */
+static void
+batadv_purge_neigh_ifinfo(struct batadv_priv *bat_priv,
+			  struct batadv_neigh_node *neigh)
+{
+	struct batadv_neigh_ifinfo *neigh_ifinfo;
+	struct batadv_hard_iface *if_outgoing;
+	struct hlist_node *node_tmp;
+
+	spin_lock_bh(&neigh->ifinfo_lock);
+
+	/* for all ifinfo objects for this neighinator */
+	hlist_for_each_entry_safe(neigh_ifinfo, node_tmp,
+				  &neigh->ifinfo_list, list) {
+		if_outgoing = neigh_ifinfo->if_outgoing;
+
+		/* always keep the default interface */
+		if (if_outgoing == BATADV_IF_DEFAULT)
+			continue;
+
+		/* don't purge if the interface is not (going) down */
+		if ((if_outgoing->if_status != BATADV_IF_INACTIVE) &&
+		    (if_outgoing->if_status != BATADV_IF_NOT_IN_USE) &&
+		    (if_outgoing->if_status != BATADV_IF_TO_BE_REMOVED))
+			continue;
+
+		batadv_dbg(BATADV_DBG_BATMAN, bat_priv,
+			   "neighbor/ifinfo purge: neighbor %pM, iface: %s\n",
+			   neigh->addr, if_outgoing->net_dev->name);
+
+		hlist_del_rcu(&neigh_ifinfo->list);
+		batadv_neigh_ifinfo_free_ref(neigh_ifinfo);
+	}
+
+	spin_unlock_bh(&neigh->ifinfo_lock);
+}
+
+/**
  * batadv_purge_orig_ifinfo - purge obsolete ifinfo entries from originator
  * @bat_priv: the bat priv with all the soft interface information
  * @orig_node: orig node which is to be checked
@@ -800,6 +846,11 @@ batadv_purge_orig_neighbors(struct batadv_priv *bat_priv,
 
 			hlist_del_rcu(&neigh_node->list);
 			batadv_neigh_node_free_ref(neigh_node);
+		} else {
+			/* only necessary if not the whole neighbor is to be
+			 * deleted, but some interface has been removed.
+			 */
+			batadv_purge_neigh_ifinfo(bat_priv, neigh_node);
 		}
 	}
 
@@ -857,7 +908,7 @@ static bool batadv_purge_orig_node(struct batadv_priv *bat_priv,
 {
 	struct batadv_neigh_node *best_neigh_node;
 	struct batadv_hard_iface *hard_iface;
-	bool changed;
+	bool changed_ifinfo, changed_neigh;
 
 	if (batadv_has_timed_out(orig_node->last_seen,
 				 2 * BATADV_PURGE_TIMEOUT)) {
@@ -867,10 +918,10 @@ static bool batadv_purge_orig_node(struct batadv_priv *bat_priv,
 			   jiffies_to_msecs(orig_node->last_seen));
 		return true;
 	}
-	changed = batadv_purge_orig_ifinfo(bat_priv, orig_node);
-	changed = changed || batadv_purge_orig_neighbors(bat_priv, orig_node);
+	changed_ifinfo = batadv_purge_orig_ifinfo(bat_priv, orig_node);
+	changed_neigh = batadv_purge_orig_neighbors(bat_priv, orig_node);
 
-	if (!changed)
+	if (!changed_ifinfo && !changed_neigh)
 		return false;
 
 	/* first for NULL ... */
@@ -1028,7 +1079,8 @@ int batadv_orig_hardif_seq_print_text(struct seq_file *seq, void *offset)
 	bat_priv->bat_algo_ops->bat_orig_print(bat_priv, seq, hard_iface);
 
 out:
-	batadv_hardif_free_ref(hard_iface);
+	if (hard_iface)
+		batadv_hardif_free_ref(hard_iface);
 	return 0;
 }
 
diff --git a/net/bluetooth/hci_conn.c b/net/bluetooth/hci_conn.c
index d958e2dca52..521fd4f3985 100644
--- a/net/bluetooth/hci_conn.c
+++ b/net/bluetooth/hci_conn.c
@@ -819,14 +819,17 @@ static int hci_conn_auth(struct hci_conn *conn, __u8 sec_level, __u8 auth_type)
 	if (!test_and_set_bit(HCI_CONN_AUTH_PEND, &conn->flags)) {
 		struct hci_cp_auth_requested cp;
 
-		/* encrypt must be pending if auth is also pending */
-		set_bit(HCI_CONN_ENCRYPT_PEND, &conn->flags);
-
 		cp.handle = cpu_to_le16(conn->handle);
 		hci_send_cmd(conn->hdev, HCI_OP_AUTH_REQUESTED,
 			     sizeof(cp), &cp);
+
+		/* If we're already encrypted set the REAUTH_PEND flag,
+		 * otherwise set the ENCRYPT_PEND.
+		 */
 		if (conn->key_type != 0xff)
 			set_bit(HCI_CONN_REAUTH_PEND, &conn->flags);
+		else
+			set_bit(HCI_CONN_ENCRYPT_PEND, &conn->flags);
 	}
 
 	return 0;
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 74014420b3c..682f33a3836 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -3330,6 +3330,12 @@ static void hci_key_refresh_complete_evt(struct hci_dev *hdev,
 	if (!conn)
 		goto unlock;
 
+	/* For BR/EDR the necessary steps are taken through the
+	 * auth_complete event.
+	 */
+	if (conn->type != LE_LINK)
+		goto unlock;
+
 	if (!ev->status)
 		conn->sec_level = conn->pending_sec_level;
 
diff --git a/net/bridge/br_netfilter.c b/net/bridge/br_netfilter.c
index 80e1b0f60a3..2acf7fa1fec 100644
--- a/net/bridge/br_netfilter.c
+++ b/net/bridge/br_netfilter.c
@@ -859,12 +859,12 @@ static unsigned int br_nf_forward_arp(const struct nf_hook_ops *ops,
 	return NF_STOLEN;
 }
 
-#if IS_ENABLED(CONFIG_NF_CONNTRACK_IPV4)
+#if IS_ENABLED(CONFIG_NF_DEFRAG_IPV4)
 static int br_nf_dev_queue_xmit(struct sk_buff *skb)
 {
 	int ret;
 
-	if (skb->nfct != NULL && skb->protocol == htons(ETH_P_IP) &&
+	if (skb->protocol == htons(ETH_P_IP) &&
 	    skb->len + nf_bridge_mtu_reduction(skb) > skb->dev->mtu &&
 	    !skb_is_gso(skb)) {
 		if (br_parse_ip_options(skb))
diff --git a/net/bridge/br_netlink.c b/net/bridge/br_netlink.c
index e74b6d530cb..e8844d975b3 100644
--- a/net/bridge/br_netlink.c
+++ b/net/bridge/br_netlink.c
@@ -445,6 +445,20 @@ static int br_validate(struct nlattr *tb[], struct nlattr *data[])
 	return 0;
 }
 
+static int br_dev_newlink(struct net *src_net, struct net_device *dev,
+			  struct nlattr *tb[], struct nlattr *data[])
+{
+	struct net_bridge *br = netdev_priv(dev);
+
+	if (tb[IFLA_ADDRESS]) {
+		spin_lock_bh(&br->lock);
+		br_stp_change_bridge_id(br, nla_data(tb[IFLA_ADDRESS]));
+		spin_unlock_bh(&br->lock);
+	}
+
+	return register_netdevice(dev);
+}
+
 static size_t br_get_link_af_size(const struct net_device *dev)
 {
 	struct net_port_vlans *pv;
@@ -473,6 +487,7 @@ struct rtnl_link_ops br_link_ops __read_mostly = {
 	.priv_size	= sizeof(struct net_bridge),
 	.setup		= br_dev_setup,
 	.validate	= br_validate,
+	.newlink	= br_dev_newlink,
 	.dellink	= br_dev_delete,
 };
 
diff --git a/net/can/gw.c b/net/can/gw.c
index ac31891967d..050a2110d43 100644
--- a/net/can/gw.c
+++ b/net/can/gw.c
@@ -804,7 +804,7 @@ static int cgw_create_job(struct sk_buff *skb,  struct nlmsghdr *nlh)
 	u8 limhops = 0;
 	int err = 0;
 
-	if (!capable(CAP_NET_ADMIN))
+	if (!netlink_capable(skb, CAP_NET_ADMIN))
 		return -EPERM;
 
 	if (nlmsg_len(nlh) < sizeof(*r))
@@ -893,7 +893,7 @@ static int cgw_remove_job(struct sk_buff *skb, struct nlmsghdr *nlh)
 	u8 limhops = 0;
 	int err = 0;
 
-	if (!capable(CAP_NET_ADMIN))
+	if (!netlink_capable(skb, CAP_NET_ADMIN))
 		return -EPERM;
 
 	if (nlmsg_len(nlh) < sizeof(*r))
diff --git a/net/ceph/messenger.c b/net/ceph/messenger.c
index dac7f9b9868..1948d592aa5 100644
--- a/net/ceph/messenger.c
+++ b/net/ceph/messenger.c
@@ -557,7 +557,7 @@ static int ceph_tcp_sendmsg(struct socket *sock, struct kvec *iov,
 	return r;
 }
 
-static int ceph_tcp_sendpage(struct socket *sock, struct page *page,
+static int __ceph_tcp_sendpage(struct socket *sock, struct page *page,
 		     int offset, size_t size, bool more)
 {
 	int flags = MSG_DONTWAIT | MSG_NOSIGNAL | (more ? MSG_MORE : MSG_EOR);
@@ -570,6 +570,24 @@ static int ceph_tcp_sendpage(struct socket *sock, struct page *page,
 	return ret;
 }
 
+static int ceph_tcp_sendpage(struct socket *sock, struct page *page,
+		     int offset, size_t size, bool more)
+{
+	int ret;
+	struct kvec iov;
+
+	/* sendpage cannot properly handle pages with page_count == 0,
+	 * we need to fallback to sendmsg if that's the case */
+	if (page_count(page) >= 1)
+		return __ceph_tcp_sendpage(sock, page, offset, size, more);
+
+	iov.iov_base = kmap(page) + offset;
+	iov.iov_len = size;
+	ret = ceph_tcp_sendmsg(sock, &iov, 1, size, more);
+	kunmap(page);
+
+	return ret;
+}
 
 /*
  * Shutdown/close the socket for the given connection.
diff --git a/net/ceph/osdmap.c b/net/ceph/osdmap.c
index e632b5a52f5..c547e46084d 100644
--- a/net/ceph/osdmap.c
+++ b/net/ceph/osdmap.c
@@ -329,6 +329,11 @@ static struct crush_map *crush_decode(void *pbyval, void *end)
 	dout("crush decode tunable chooseleaf_descend_once = %d",
 	     c->chooseleaf_descend_once);
 
+	ceph_decode_need(p, end, sizeof(u8), done);
+	c->chooseleaf_vary_r = ceph_decode_8(p);
+	dout("crush decode tunable chooseleaf_vary_r = %d",
+	     c->chooseleaf_vary_r);
+
 done:
 	dout("crush_decode success\n");
 	return c;
@@ -1548,8 +1553,10 @@ static void apply_primary_affinity(struct ceph_osdmap *osdmap, u32 pps,
 		return;
 
 	for (i = 0; i < len; i++) {
-		if (osds[i] != CRUSH_ITEM_NONE &&
-		    osdmap->osd_primary_affinity[i] !=
+		int osd = osds[i];
+
+		if (osd != CRUSH_ITEM_NONE &&
+		    osdmap->osd_primary_affinity[osd] !=
 					CEPH_OSD_DEFAULT_PRIMARY_AFFINITY) {
 			break;
 		}
@@ -1563,10 +1570,9 @@ static void apply_primary_affinity(struct ceph_osdmap *osdmap, u32 pps,
 	 * osd's pgs get rejected as primary.
 	 */
 	for (i = 0; i < len; i++) {
-		int osd;
+		int osd = osds[i];
 		u32 aff;
 
-		osd = osds[i];
 		if (osd == CRUSH_ITEM_NONE)
 			continue;
 
diff --git a/net/core/dev.c b/net/core/dev.c
index e14f1cba591..8b07db37dc1 100644
--- a/net/core/dev.c
+++ b/net/core/dev.c
@@ -2418,7 +2418,7 @@ EXPORT_SYMBOL(netdev_rx_csum_fault);
  * 2. No high memory really exists on this machine.
  */
 
-static int illegal_highdma(const struct net_device *dev, struct sk_buff *skb)
+static int illegal_highdma(struct net_device *dev, struct sk_buff *skb)
 {
 #ifdef CONFIG_HIGHMEM
 	int i;
@@ -2493,38 +2493,36 @@ static int dev_gso_segment(struct sk_buff *skb, netdev_features_t features)
 }
 
 static netdev_features_t harmonize_features(struct sk_buff *skb,
-					    const struct net_device *dev,
-					    netdev_features_t features)
+	netdev_features_t features)
 {
 	int tmp;
 
 	if (skb->ip_summed != CHECKSUM_NONE &&
 	    !can_checksum_protocol(features, skb_network_protocol(skb, &tmp))) {
 		features &= ~NETIF_F_ALL_CSUM;
-	} else if (illegal_highdma(dev, skb)) {
+	} else if (illegal_highdma(skb->dev, skb)) {
 		features &= ~NETIF_F_SG;
 	}
 
 	return features;
 }
 
-netdev_features_t netif_skb_dev_features(struct sk_buff *skb,
-					 const struct net_device *dev)
+netdev_features_t netif_skb_features(struct sk_buff *skb)
 {
 	__be16 protocol = skb->protocol;
-	netdev_features_t features = dev->features;
+	netdev_features_t features = skb->dev->features;
 
-	if (skb_shinfo(skb)->gso_segs > dev->gso_max_segs)
+	if (skb_shinfo(skb)->gso_segs > skb->dev->gso_max_segs)
 		features &= ~NETIF_F_GSO_MASK;
 
 	if (protocol == htons(ETH_P_8021Q) || protocol == htons(ETH_P_8021AD)) {
 		struct vlan_ethhdr *veh = (struct vlan_ethhdr *)skb->data;
 		protocol = veh->h_vlan_encapsulated_proto;
 	} else if (!vlan_tx_tag_present(skb)) {
-		return harmonize_features(skb, dev, features);
+		return harmonize_features(skb, features);
 	}
 
-	features &= (dev->vlan_features | NETIF_F_HW_VLAN_CTAG_TX |
+	features &= (skb->dev->vlan_features | NETIF_F_HW_VLAN_CTAG_TX |
 					       NETIF_F_HW_VLAN_STAG_TX);
 
 	if (protocol == htons(ETH_P_8021Q) || protocol == htons(ETH_P_8021AD))
@@ -2532,9 +2530,9 @@ netdev_features_t netif_skb_dev_features(struct sk_buff *skb,
 				NETIF_F_GEN_CSUM | NETIF_F_HW_VLAN_CTAG_TX |
 				NETIF_F_HW_VLAN_STAG_TX;
 
-	return harmonize_features(skb, dev, features);
+	return harmonize_features(skb, features);
 }
-EXPORT_SYMBOL(netif_skb_dev_features);
+EXPORT_SYMBOL(netif_skb_features);
 
 int dev_hard_start_xmit(struct sk_buff *skb, struct net_device *dev,
 			struct netdev_queue *txq)
@@ -3953,6 +3951,7 @@ static enum gro_result dev_gro_receive(struct napi_struct *napi, struct sk_buff
 	}
 	NAPI_GRO_CB(skb)->count = 1;
 	NAPI_GRO_CB(skb)->age = jiffies;
+	NAPI_GRO_CB(skb)->last = skb;
 	skb_shinfo(skb)->gso_size = skb_gro_len(skb);
 	skb->next = napi->gro_list;
 	napi->gro_list = skb;
@@ -4543,6 +4542,32 @@ void *netdev_adjacent_get_private(struct list_head *adj_list)
 EXPORT_SYMBOL(netdev_adjacent_get_private);
 
 /**
+ * netdev_upper_get_next_dev_rcu - Get the next dev from upper list
+ * @dev: device
+ * @iter: list_head ** of the current position
+ *
+ * Gets the next device from the dev's upper list, starting from iter
+ * position. The caller must hold RCU read lock.
+ */
+struct net_device *netdev_upper_get_next_dev_rcu(struct net_device *dev,
+						 struct list_head **iter)
+{
+	struct netdev_adjacent *upper;
+
+	WARN_ON_ONCE(!rcu_read_lock_held() && !lockdep_rtnl_is_held());
+
+	upper = list_entry_rcu((*iter)->next, struct netdev_adjacent, list);
+
+	if (&upper->list == &dev->adj_list.upper)
+		return NULL;
+
+	*iter = &upper->list;
+
+	return upper->dev;
+}
+EXPORT_SYMBOL(netdev_upper_get_next_dev_rcu);
+
+/**
  * netdev_all_upper_get_next_dev_rcu - Get the next dev from upper list
  * @dev: device
  * @iter: list_head ** of the current position
@@ -4624,6 +4649,32 @@ void *netdev_lower_get_next_private_rcu(struct net_device *dev,
 EXPORT_SYMBOL(netdev_lower_get_next_private_rcu);
 
 /**
+ * netdev_lower_get_next - Get the next device from the lower neighbour
+ *                         list
+ * @dev: device
+ * @iter: list_head ** of the current position
+ *
+ * Gets the next netdev_adjacent from the dev's lower neighbour
+ * list, starting from iter position. The caller must hold RTNL lock or
+ * its own locking that guarantees that the neighbour lower
+ * list will remain unchainged.
+ */
+void *netdev_lower_get_next(struct net_device *dev, struct list_head **iter)
+{
+	struct netdev_adjacent *lower;
+
+	lower = list_entry((*iter)->next, struct netdev_adjacent, list);
+
+	if (&lower->list == &dev->adj_list.lower)
+		return NULL;
+
+	*iter = &lower->list;
+
+	return lower->dev;
+}
+EXPORT_SYMBOL(netdev_lower_get_next);
+
+/**
  * netdev_lower_get_first_private_rcu - Get the first ->private from the
  *				       lower neighbour list, RCU
  *				       variant
@@ -5073,6 +5124,30 @@ void *netdev_lower_dev_get_private(struct net_device *dev,
 }
 EXPORT_SYMBOL(netdev_lower_dev_get_private);
 
+
+int dev_get_nest_level(struct net_device *dev,
+		       bool (*type_check)(struct net_device *dev))
+{
+	struct net_device *lower = NULL;
+	struct list_head *iter;
+	int max_nest = -1;
+	int nest;
+
+	ASSERT_RTNL();
+
+	netdev_for_each_lower_dev(dev, lower, iter) {
+		nest = dev_get_nest_level(lower, type_check);
+		if (max_nest < nest)
+			max_nest = nest;
+	}
+
+	if (type_check(dev))
+		max_nest++;
+
+	return max_nest;
+}
+EXPORT_SYMBOL(dev_get_nest_level);
+
 static void dev_change_rx_flags(struct net_device *dev, int flags)
 {
 	const struct net_device_ops *ops = dev->netdev_ops;
@@ -5542,7 +5617,7 @@ static int dev_new_index(struct net *net)
 
 /* Delayed registration/unregisteration */
 static LIST_HEAD(net_todo_list);
-static DECLARE_WAIT_QUEUE_HEAD(netdev_unregistering_wq);
+DECLARE_WAIT_QUEUE_HEAD(netdev_unregistering_wq);
 
 static void net_set_todo(struct net_device *dev)
 {
diff --git a/net/core/filter.c b/net/core/filter.c
index cd58614660c..9d79ca0a6e8 100644
--- a/net/core/filter.c
+++ b/net/core/filter.c
@@ -122,6 +122,13 @@ noinline u64 __bpf_call_base(u64 r1, u64 r2, u64 r3, u64 r4, u64 r5)
 	return 0;
 }
 
+/* Register mappings for user programs. */
+#define A_REG		0
+#define X_REG		7
+#define TMP_REG		8
+#define ARG2_REG	2
+#define ARG3_REG	3
+
 /**
  *	__sk_run_filter - run a filter on a given context
  *	@ctx: buffer to run the filter on
@@ -242,6 +249,8 @@ unsigned int __sk_run_filter(void *ctx, const struct sock_filter_int *insn)
 
 	regs[FP_REG]  = (u64) (unsigned long) &stack[ARRAY_SIZE(stack)];
 	regs[ARG1_REG] = (u64) (unsigned long) ctx;
+	regs[A_REG] = 0;
+	regs[X_REG] = 0;
 
 select_insn:
 	goto *jumptable[insn->code];
@@ -643,13 +652,6 @@ static u64 __get_raw_cpu_id(u64 ctx, u64 A, u64 X, u64 r4, u64 r5)
 	return raw_smp_processor_id();
 }
 
-/* Register mappings for user programs. */
-#define A_REG		0
-#define X_REG		7
-#define TMP_REG		8
-#define ARG2_REG	2
-#define ARG3_REG	3
-
 static bool convert_bpf_extensions(struct sock_filter *fp,
 				   struct sock_filter_int **insnp)
 {
diff --git a/net/core/neighbour.c b/net/core/neighbour.c
index 8f8a96ef9f3..32d872eec7f 100644
--- a/net/core/neighbour.c
+++ b/net/core/neighbour.c
@@ -1248,8 +1248,8 @@ void __neigh_set_probe_once(struct neighbour *neigh)
 	neigh->updated = jiffies;
 	if (!(neigh->nud_state & NUD_FAILED))
 		return;
-	neigh->nud_state = NUD_PROBE;
-	atomic_set(&neigh->probes, NEIGH_VAR(neigh->parms, UCAST_PROBES));
+	neigh->nud_state = NUD_INCOMPLETE;
+	atomic_set(&neigh->probes, neigh_max_probes(neigh));
 	neigh_add_timer(neigh,
 			jiffies + NEIGH_VAR(neigh->parms, RETRANS_TIME));
 }
diff --git a/net/core/net_namespace.c b/net/core/net_namespace.c
index 81d3a9a0845..7c8ffd97496 100644
--- a/net/core/net_namespace.c
+++ b/net/core/net_namespace.c
@@ -24,7 +24,7 @@
 
 static LIST_HEAD(pernet_list);
 static struct list_head *first_device = &pernet_list;
-static DEFINE_MUTEX(net_mutex);
+DEFINE_MUTEX(net_mutex);
 
 LIST_HEAD(net_namespace_list);
 EXPORT_SYMBOL_GPL(net_namespace_list);
diff --git a/net/core/rtnetlink.c b/net/core/rtnetlink.c
index d4ff41739b0..2d8d8fcfa06 100644
--- a/net/core/rtnetlink.c
+++ b/net/core/rtnetlink.c
@@ -353,15 +353,46 @@ void __rtnl_link_unregister(struct rtnl_link_ops *ops)
 }
 EXPORT_SYMBOL_GPL(__rtnl_link_unregister);
 
+/* Return with the rtnl_lock held when there are no network
+ * devices unregistering in any network namespace.
+ */
+static void rtnl_lock_unregistering_all(void)
+{
+	struct net *net;
+	bool unregistering;
+	DEFINE_WAIT(wait);
+
+	for (;;) {
+		prepare_to_wait(&netdev_unregistering_wq, &wait,
+				TASK_UNINTERRUPTIBLE);
+		unregistering = false;
+		rtnl_lock();
+		for_each_net(net) {
+			if (net->dev_unreg_count > 0) {
+				unregistering = true;
+				break;
+			}
+		}
+		if (!unregistering)
+			break;
+		__rtnl_unlock();
+		schedule();
+	}
+	finish_wait(&netdev_unregistering_wq, &wait);
+}
+
 /**
  * rtnl_link_unregister - Unregister rtnl_link_ops from rtnetlink.
  * @ops: struct rtnl_link_ops * to unregister
  */
 void rtnl_link_unregister(struct rtnl_link_ops *ops)
 {
-	rtnl_lock();
+	/* Close the race with cleanup_net() */
+	mutex_lock(&net_mutex);
+	rtnl_lock_unregistering_all();
 	__rtnl_link_unregister(ops);
 	rtnl_unlock();
+	mutex_unlock(&net_mutex);
 }
 EXPORT_SYMBOL_GPL(rtnl_link_unregister);
 
@@ -774,7 +805,8 @@ static inline int rtnl_vfinfo_size(const struct net_device *dev,
 		return 0;
 }
 
-static size_t rtnl_port_size(const struct net_device *dev)
+static size_t rtnl_port_size(const struct net_device *dev,
+			     u32 ext_filter_mask)
 {
 	size_t port_size = nla_total_size(4)		/* PORT_VF */
 		+ nla_total_size(PORT_PROFILE_MAX)	/* PORT_PROFILE */
@@ -790,7 +822,8 @@ static size_t rtnl_port_size(const struct net_device *dev)
 	size_t port_self_size = nla_total_size(sizeof(struct nlattr))
 		+ port_size;
 
-	if (!dev->netdev_ops->ndo_get_vf_port || !dev->dev.parent)
+	if (!dev->netdev_ops->ndo_get_vf_port || !dev->dev.parent ||
+	    !(ext_filter_mask & RTEXT_FILTER_VF))
 		return 0;
 	if (dev_num_vf(dev->dev.parent))
 		return port_self_size + vf_ports_size +
@@ -826,7 +859,7 @@ static noinline size_t if_nlmsg_size(const struct net_device *dev,
 	       + nla_total_size(ext_filter_mask
 			        & RTEXT_FILTER_VF ? 4 : 0) /* IFLA_NUM_VF */
 	       + rtnl_vfinfo_size(dev, ext_filter_mask) /* IFLA_VFINFO_LIST */
-	       + rtnl_port_size(dev) /* IFLA_VF_PORTS + IFLA_PORT_SELF */
+	       + rtnl_port_size(dev, ext_filter_mask) /* IFLA_VF_PORTS + IFLA_PORT_SELF */
 	       + rtnl_link_get_size(dev) /* IFLA_LINKINFO */
 	       + rtnl_link_get_af_size(dev) /* IFLA_AF_SPEC */
 	       + nla_total_size(MAX_PHYS_PORT_ID_LEN); /* IFLA_PHYS_PORT_ID */
@@ -888,11 +921,13 @@ static int rtnl_port_self_fill(struct sk_buff *skb, struct net_device *dev)
 	return 0;
 }
 
-static int rtnl_port_fill(struct sk_buff *skb, struct net_device *dev)
+static int rtnl_port_fill(struct sk_buff *skb, struct net_device *dev,
+			  u32 ext_filter_mask)
 {
 	int err;
 
-	if (!dev->netdev_ops->ndo_get_vf_port || !dev->dev.parent)
+	if (!dev->netdev_ops->ndo_get_vf_port || !dev->dev.parent ||
+	    !(ext_filter_mask & RTEXT_FILTER_VF))
 		return 0;
 
 	err = rtnl_port_self_fill(skb, dev);
@@ -1079,7 +1114,7 @@ static int rtnl_fill_ifinfo(struct sk_buff *skb, struct net_device *dev,
 		nla_nest_end(skb, vfinfo);
 	}
 
-	if (rtnl_port_fill(skb, dev))
+	if (rtnl_port_fill(skb, dev, ext_filter_mask))
 		goto nla_put_failure;
 
 	if (dev->rtnl_link_ops || rtnl_have_link_slave_info(dev)) {
@@ -1198,6 +1233,7 @@ static int rtnl_dump_ifinfo(struct sk_buff *skb, struct netlink_callback *cb)
 	struct hlist_head *head;
 	struct nlattr *tb[IFLA_MAX+1];
 	u32 ext_filter_mask = 0;
+	int err;
 
 	s_h = cb->args[0];
 	s_idx = cb->args[1];
@@ -1218,11 +1254,17 @@ static int rtnl_dump_ifinfo(struct sk_buff *skb, struct netlink_callback *cb)
 		hlist_for_each_entry_rcu(dev, head, index_hlist) {
 			if (idx < s_idx)
 				goto cont;
-			if (rtnl_fill_ifinfo(skb, dev, RTM_NEWLINK,
-					     NETLINK_CB(cb->skb).portid,
-					     cb->nlh->nlmsg_seq, 0,
-					     NLM_F_MULTI,
-					     ext_filter_mask) <= 0)
+			err = rtnl_fill_ifinfo(skb, dev, RTM_NEWLINK,
+					       NETLINK_CB(cb->skb).portid,
+					       cb->nlh->nlmsg_seq, 0,
+					       NLM_F_MULTI,
+					       ext_filter_mask);
+			/* If we ran out of room on the first message,
+			 * we're in trouble
+			 */
+			WARN_ON((err == -EMSGSIZE) && (skb->len == 0));
+
+			if (err <= 0)
 				goto out;
 
 			nl_dump_check_consistent(cb, nlmsg_hdr(skb));
@@ -1395,7 +1437,8 @@ static int do_set_master(struct net_device *dev, int ifindex)
 	return 0;
 }
 
-static int do_setlink(struct net_device *dev, struct ifinfomsg *ifm,
+static int do_setlink(const struct sk_buff *skb,
+		      struct net_device *dev, struct ifinfomsg *ifm,
 		      struct nlattr **tb, char *ifname, int modified)
 {
 	const struct net_device_ops *ops = dev->netdev_ops;
@@ -1407,7 +1450,7 @@ static int do_setlink(struct net_device *dev, struct ifinfomsg *ifm,
 			err = PTR_ERR(net);
 			goto errout;
 		}
-		if (!ns_capable(net->user_ns, CAP_NET_ADMIN)) {
+		if (!netlink_ns_capable(skb, net->user_ns, CAP_NET_ADMIN)) {
 			err = -EPERM;
 			goto errout;
 		}
@@ -1661,7 +1704,7 @@ static int rtnl_setlink(struct sk_buff *skb, struct nlmsghdr *nlh)
 	if (err < 0)
 		goto errout;
 
-	err = do_setlink(dev, ifm, tb, ifname, 0);
+	err = do_setlink(skb, dev, ifm, tb, ifname, 0);
 errout:
 	return err;
 }
@@ -1778,7 +1821,8 @@ err:
 }
 EXPORT_SYMBOL(rtnl_create_link);
 
-static int rtnl_group_changelink(struct net *net, int group,
+static int rtnl_group_changelink(const struct sk_buff *skb,
+		struct net *net, int group,
 		struct ifinfomsg *ifm,
 		struct nlattr **tb)
 {
@@ -1787,7 +1831,7 @@ static int rtnl_group_changelink(struct net *net, int group,
 
 	for_each_netdev(net, dev) {
 		if (dev->group == group) {
-			err = do_setlink(dev, ifm, tb, NULL, 0);
+			err = do_setlink(skb, dev, ifm, tb, NULL, 0);
 			if (err < 0)
 				return err;
 		}
@@ -1929,12 +1973,12 @@ replay:
 				modified = 1;
 			}
 
-			return do_setlink(dev, ifm, tb, ifname, modified);
+			return do_setlink(skb, dev, ifm, tb, ifname, modified);
 		}
 
 		if (!(nlh->nlmsg_flags & NLM_F_CREATE)) {
 			if (ifm->ifi_index == 0 && tb[IFLA_GROUP])
-				return rtnl_group_changelink(net,
+				return rtnl_group_changelink(skb, net,
 						nla_get_u32(tb[IFLA_GROUP]),
 						ifm, tb);
 			return -ENODEV;
@@ -2321,7 +2365,7 @@ static int rtnl_fdb_del(struct sk_buff *skb, struct nlmsghdr *nlh)
 	int err = -EINVAL;
 	__u8 *addr;
 
-	if (!capable(CAP_NET_ADMIN))
+	if (!netlink_capable(skb, CAP_NET_ADMIN))
 		return -EPERM;
 
 	err = nlmsg_parse(nlh, sizeof(*ndm), tb, NDA_MAX, NULL);
@@ -2773,7 +2817,7 @@ static int rtnetlink_rcv_msg(struct sk_buff *skb, struct nlmsghdr *nlh)
 	sz_idx = type>>2;
 	kind = type&3;
 
-	if (kind != 2 && !ns_capable(net->user_ns, CAP_NET_ADMIN))
+	if (kind != 2 && !netlink_net_capable(skb, CAP_NET_ADMIN))
 		return -EPERM;
 
 	if (kind == 2 && nlh->nlmsg_flags&NLM_F_DUMP) {
diff --git a/net/core/skbuff.c b/net/core/skbuff.c
index 1b62343f583..8383b2bddeb 100644
--- a/net/core/skbuff.c
+++ b/net/core/skbuff.c
@@ -3076,7 +3076,7 @@ int skb_gro_receive(struct sk_buff **head, struct sk_buff *skb)
 	if (unlikely(p->len + len >= 65536))
 		return -E2BIG;
 
-	lp = NAPI_GRO_CB(p)->last ?: p;
+	lp = NAPI_GRO_CB(p)->last;
 	pinfo = skb_shinfo(lp);
 
 	if (headlen <= offset) {
@@ -3192,7 +3192,7 @@ merge:
 
 	__skb_pull(skb, offset);
 
-	if (!NAPI_GRO_CB(p)->last)
+	if (NAPI_GRO_CB(p)->last == p)
 		skb_shinfo(p)->frag_list = skb;
 	else
 		NAPI_GRO_CB(p)->last->next = skb;
diff --git a/net/core/sock.c b/net/core/sock.c
index b4fff008136..664ee4295b6 100644
--- a/net/core/sock.c
+++ b/net/core/sock.c
@@ -145,6 +145,55 @@
 static DEFINE_MUTEX(proto_list_mutex);
 static LIST_HEAD(proto_list);
 
+/**
+ * sk_ns_capable - General socket capability test
+ * @sk: Socket to use a capability on or through
+ * @user_ns: The user namespace of the capability to use
+ * @cap: The capability to use
+ *
+ * Test to see if the opener of the socket had when the socket was
+ * created and the current process has the capability @cap in the user
+ * namespace @user_ns.
+ */
+bool sk_ns_capable(const struct sock *sk,
+		   struct user_namespace *user_ns, int cap)
+{
+	return file_ns_capable(sk->sk_socket->file, user_ns, cap) &&
+		ns_capable(user_ns, cap);
+}
+EXPORT_SYMBOL(sk_ns_capable);
+
+/**
+ * sk_capable - Socket global capability test
+ * @sk: Socket to use a capability on or through
+ * @cap: The global capbility to use
+ *
+ * Test to see if the opener of the socket had when the socket was
+ * created and the current process has the capability @cap in all user
+ * namespaces.
+ */
+bool sk_capable(const struct sock *sk, int cap)
+{
+	return sk_ns_capable(sk, &init_user_ns, cap);
+}
+EXPORT_SYMBOL(sk_capable);
+
+/**
+ * sk_net_capable - Network namespace socket capability test
+ * @sk: Socket to use a capability on or through
+ * @cap: The capability to use
+ *
+ * Test to see if the opener of the socket had when the socke was created
+ * and the current process has the capability @cap over the network namespace
+ * the socket is a member of.
+ */
+bool sk_net_capable(const struct sock *sk, int cap)
+{
+	return sk_ns_capable(sk, sock_net(sk)->user_ns, cap);
+}
+EXPORT_SYMBOL(sk_net_capable);
+
+
 #ifdef CONFIG_MEMCG_KMEM
 int mem_cgroup_sockets_init(struct mem_cgroup *memcg, struct cgroup_subsys *ss)
 {
diff --git a/net/core/sock_diag.c b/net/core/sock_diag.c
index d7af1885932..a4216a4c957 100644
--- a/net/core/sock_diag.c
+++ b/net/core/sock_diag.c
@@ -49,7 +49,7 @@ int sock_diag_put_meminfo(struct sock *sk, struct sk_buff *skb, int attrtype)
 }
 EXPORT_SYMBOL_GPL(sock_diag_put_meminfo);
 
-int sock_diag_put_filterinfo(struct user_namespace *user_ns, struct sock *sk,
+int sock_diag_put_filterinfo(bool may_report_filterinfo, struct sock *sk,
 			     struct sk_buff *skb, int attrtype)
 {
 	struct sock_fprog_kern *fprog;
@@ -58,7 +58,7 @@ int sock_diag_put_filterinfo(struct user_namespace *user_ns, struct sock *sk,
 	unsigned int flen;
 	int err = 0;
 
-	if (!ns_capable(user_ns, CAP_NET_ADMIN)) {
+	if (!may_report_filterinfo) {
 		nla_reserve(skb, attrtype, 0);
 		return 0;
 	}
diff --git a/net/core/utils.c b/net/core/utils.c
index 2f737bf90b3..eed34338736 100644
--- a/net/core/utils.c
+++ b/net/core/utils.c
@@ -348,8 +348,8 @@ static void __net_random_once_deferred(struct work_struct *w)
 {
 	struct __net_random_once_work *work =
 		container_of(w, struct __net_random_once_work, work);
-	if (!static_key_enabled(work->key))
-		static_key_slow_inc(work->key);
+	BUG_ON(!static_key_enabled(work->key));
+	static_key_slow_dec(work->key);
 	kfree(work);
 }
 
@@ -367,7 +367,7 @@ static void __net_random_once_disable_jump(struct static_key *key)
 }
 
 bool __net_get_random_once(void *buf, int nbytes, bool *done,
-			   struct static_key *done_key)
+			   struct static_key *once_key)
 {
 	static DEFINE_SPINLOCK(lock);
 	unsigned long flags;
@@ -382,7 +382,7 @@ bool __net_get_random_once(void *buf, int nbytes, bool *done,
 	*done = true;
 	spin_unlock_irqrestore(&lock, flags);
 
-	__net_random_once_disable_jump(done_key);
+	__net_random_once_disable_jump(once_key);
 
 	return true;
 }
diff --git a/net/dcb/dcbnl.c b/net/dcb/dcbnl.c
index 55364440267..f8b98d89c28 100644
--- a/net/dcb/dcbnl.c
+++ b/net/dcb/dcbnl.c
@@ -1669,7 +1669,7 @@ static int dcb_doit(struct sk_buff *skb, struct nlmsghdr *nlh)
 	struct nlmsghdr *reply_nlh = NULL;
 	const struct reply_func *fn;
 
-	if ((nlh->nlmsg_type == RTM_SETDCB) && !capable(CAP_NET_ADMIN))
+	if ((nlh->nlmsg_type == RTM_SETDCB) && !netlink_capable(skb, CAP_NET_ADMIN))
 		return -EPERM;
 
 	ret = nlmsg_parse(nlh, sizeof(*dcb), tb, DCB_ATTR_MAX,
diff --git a/net/decnet/dn_dev.c b/net/decnet/dn_dev.c
index a603823a3e2..3b726f31c64 100644
--- a/net/decnet/dn_dev.c
+++ b/net/decnet/dn_dev.c
@@ -574,7 +574,7 @@ static int dn_nl_deladdr(struct sk_buff *skb, struct nlmsghdr *nlh)
 	struct dn_ifaddr __rcu **ifap;
 	int err = -EINVAL;
 
-	if (!capable(CAP_NET_ADMIN))
+	if (!netlink_capable(skb, CAP_NET_ADMIN))
 		return -EPERM;
 
 	if (!net_eq(net, &init_net))
@@ -618,7 +618,7 @@ static int dn_nl_newaddr(struct sk_buff *skb, struct nlmsghdr *nlh)
 	struct dn_ifaddr *ifa;
 	int err;
 
-	if (!capable(CAP_NET_ADMIN))
+	if (!netlink_capable(skb, CAP_NET_ADMIN))
 		return -EPERM;
 
 	if (!net_eq(net, &init_net))
diff --git a/net/decnet/dn_fib.c b/net/decnet/dn_fib.c
index 57dc159245e..d332aefb084 100644
--- a/net/decnet/dn_fib.c
+++ b/net/decnet/dn_fib.c
@@ -505,7 +505,7 @@ static int dn_fib_rtm_delroute(struct sk_buff *skb, struct nlmsghdr *nlh)
 	struct nlattr *attrs[RTA_MAX+1];
 	int err;
 
-	if (!capable(CAP_NET_ADMIN))
+	if (!netlink_capable(skb, CAP_NET_ADMIN))
 		return -EPERM;
 
 	if (!net_eq(net, &init_net))
@@ -530,7 +530,7 @@ static int dn_fib_rtm_newroute(struct sk_buff *skb, struct nlmsghdr *nlh)
 	struct nlattr *attrs[RTA_MAX+1];
 	int err;
 
-	if (!capable(CAP_NET_ADMIN))
+	if (!netlink_capable(skb, CAP_NET_ADMIN))
 		return -EPERM;
 
 	if (!net_eq(net, &init_net))
diff --git a/net/decnet/netfilter/dn_rtmsg.c b/net/decnet/netfilter/dn_rtmsg.c
index e83015cecfa..e4d9560a910 100644
--- a/net/decnet/netfilter/dn_rtmsg.c
+++ b/net/decnet/netfilter/dn_rtmsg.c
@@ -107,7 +107,7 @@ static inline void dnrmg_receive_user_skb(struct sk_buff *skb)
 	if (nlh->nlmsg_len < sizeof(*nlh) || skb->len < nlh->nlmsg_len)
 		return;
 
-	if (!capable(CAP_NET_ADMIN))
+	if (!netlink_capable(skb, CAP_NET_ADMIN))
 		RCV_SKB_FAIL(-EPERM);
 
 	/* Eventually we might send routing messages too */
diff --git a/net/dsa/dsa.c b/net/dsa/dsa.c
index 0eb5d5e76df..5db37cef50a 100644
--- a/net/dsa/dsa.c
+++ b/net/dsa/dsa.c
@@ -406,8 +406,9 @@ static int dsa_of_probe(struct platform_device *pdev)
 		goto out_free;
 	}
 
-	chip_index = 0;
+	chip_index = -1;
 	for_each_available_child_of_node(np, child) {
+		chip_index++;
 		cd = &pd->chip[chip_index];
 
 		cd->mii_bus = &mdio_bus->dev;
diff --git a/net/ipv4/af_inet.c b/net/ipv4/af_inet.c
index 8c54870db79..6d6dd345bc4 100644
--- a/net/ipv4/af_inet.c
+++ b/net/ipv4/af_inet.c
@@ -1650,6 +1650,39 @@ static int __init init_ipv4_mibs(void)
 	return register_pernet_subsys(&ipv4_mib_ops);
 }
 
+static __net_init int inet_init_net(struct net *net)
+{
+	/*
+	 * Set defaults for local port range
+	 */
+	seqlock_init(&net->ipv4.ip_local_ports.lock);
+	net->ipv4.ip_local_ports.range[0] =  32768;
+	net->ipv4.ip_local_ports.range[1] =  61000;
+
+	seqlock_init(&net->ipv4.ping_group_range.lock);
+	/*
+	 * Sane defaults - nobody may create ping sockets.
+	 * Boot scripts should set this to distro-specific group.
+	 */
+	net->ipv4.ping_group_range.range[0] = make_kgid(&init_user_ns, 1);
+	net->ipv4.ping_group_range.range[1] = make_kgid(&init_user_ns, 0);
+	return 0;
+}
+
+static __net_exit void inet_exit_net(struct net *net)
+{
+}
+
+static __net_initdata struct pernet_operations af_inet_ops = {
+	.init = inet_init_net,
+	.exit = inet_exit_net,
+};
+
+static int __init init_inet_pernet_ops(void)
+{
+	return register_pernet_subsys(&af_inet_ops);
+}
+
 static int ipv4_proc_init(void);
 
 /*
@@ -1794,6 +1827,9 @@ static int __init inet_init(void)
 	if (ip_mr_init())
 		pr_crit("%s: Cannot init ipv4 mroute\n", __func__);
 #endif
+
+	if (init_inet_pernet_ops())
+		pr_crit("%s: Cannot init ipv4 inet pernet ops\n", __func__);
 	/*
 	 *	Initialise per-cpu ipv4 mibs
 	 */
diff --git a/net/ipv4/fib_frontend.c b/net/ipv4/fib_frontend.c
index 1a629f87027..255aa9946fe 100644
--- a/net/ipv4/fib_frontend.c
+++ b/net/ipv4/fib_frontend.c
@@ -250,7 +250,7 @@ static int __fib_validate_source(struct sk_buff *skb, __be32 src, __be32 dst,
 	bool dev_match;
 
 	fl4.flowi4_oif = 0;
-	fl4.flowi4_iif = oif;
+	fl4.flowi4_iif = oif ? : LOOPBACK_IFINDEX;
 	fl4.daddr = src;
 	fl4.saddr = dst;
 	fl4.flowi4_tos = tos;
diff --git a/net/ipv4/fib_semantics.c b/net/ipv4/fib_semantics.c
index b53f0bf84dc..b10cd43a472 100644
--- a/net/ipv4/fib_semantics.c
+++ b/net/ipv4/fib_semantics.c
@@ -631,6 +631,7 @@ static int fib_check_nh(struct fib_config *cfg, struct fib_info *fi,
 				.daddr = nh->nh_gw,
 				.flowi4_scope = cfg->fc_scope + 1,
 				.flowi4_oif = nh->nh_oif,
+				.flowi4_iif = LOOPBACK_IFINDEX,
 			};
 
 			/* It is not necessary, but requires a bit of thinking */
@@ -820,13 +821,13 @@ struct fib_info *fib_create_info(struct fib_config *cfg)
 	fi = kzalloc(sizeof(*fi)+nhs*sizeof(struct fib_nh), GFP_KERNEL);
 	if (fi == NULL)
 		goto failure;
+	fib_info_cnt++;
 	if (cfg->fc_mx) {
 		fi->fib_metrics = kzalloc(sizeof(u32) * RTAX_MAX, GFP_KERNEL);
 		if (!fi->fib_metrics)
 			goto failure;
 	} else
 		fi->fib_metrics = (u32 *) dst_default_metrics;
-	fib_info_cnt++;
 
 	fi->fib_net = hold_net(net);
 	fi->fib_protocol = cfg->fc_protocol;
diff --git a/net/ipv4/inet_connection_sock.c b/net/ipv4/inet_connection_sock.c
index 0d1e2cb877e..a56b8e6e866 100644
--- a/net/ipv4/inet_connection_sock.c
+++ b/net/ipv4/inet_connection_sock.c
@@ -37,11 +37,11 @@ void inet_get_local_port_range(struct net *net, int *low, int *high)
 	unsigned int seq;
 
 	do {
-		seq = read_seqbegin(&net->ipv4.sysctl_local_ports.lock);
+		seq = read_seqbegin(&net->ipv4.ip_local_ports.lock);
 
-		*low = net->ipv4.sysctl_local_ports.range[0];
-		*high = net->ipv4.sysctl_local_ports.range[1];
-	} while (read_seqretry(&net->ipv4.sysctl_local_ports.lock, seq));
+		*low = net->ipv4.ip_local_ports.range[0];
+		*high = net->ipv4.ip_local_ports.range[1];
+	} while (read_seqretry(&net->ipv4.ip_local_ports.lock, seq));
 }
 EXPORT_SYMBOL(inet_get_local_port_range);
 
diff --git a/net/ipv4/ip_forward.c b/net/ipv4/ip_forward.c
index be8abe73bb9..6f111e48e11 100644
--- a/net/ipv4/ip_forward.c
+++ b/net/ipv4/ip_forward.c
@@ -42,12 +42,12 @@
 static bool ip_may_fragment(const struct sk_buff *skb)
 {
 	return unlikely((ip_hdr(skb)->frag_off & htons(IP_DF)) == 0) ||
-	       !skb->local_df;
+		skb->local_df;
 }
 
 static bool ip_exceeds_mtu(const struct sk_buff *skb, unsigned int mtu)
 {
-	if (skb->len <= mtu || skb->local_df)
+	if (skb->len <= mtu)
 		return false;
 
 	if (skb_is_gso(skb) && skb_gso_network_seglen(skb) <= mtu)
@@ -56,53 +56,6 @@ static bool ip_exceeds_mtu(const struct sk_buff *skb, unsigned int mtu)
 	return true;
 }
 
-static bool ip_gso_exceeds_dst_mtu(const struct sk_buff *skb)
-{
-	unsigned int mtu;
-
-	if (skb->local_df || !skb_is_gso(skb))
-		return false;
-
-	mtu = ip_dst_mtu_maybe_forward(skb_dst(skb), true);
-
-	/* if seglen > mtu, do software segmentation for IP fragmentation on
-	 * output.  DF bit cannot be set since ip_forward would have sent
-	 * icmp error.
-	 */
-	return skb_gso_network_seglen(skb) > mtu;
-}
-
-/* called if GSO skb needs to be fragmented on forward */
-static int ip_forward_finish_gso(struct sk_buff *skb)
-{
-	struct dst_entry *dst = skb_dst(skb);
-	netdev_features_t features;
-	struct sk_buff *segs;
-	int ret = 0;
-
-	features = netif_skb_dev_features(skb, dst->dev);
-	segs = skb_gso_segment(skb, features & ~NETIF_F_GSO_MASK);
-	if (IS_ERR(segs)) {
-		kfree_skb(skb);
-		return -ENOMEM;
-	}
-
-	consume_skb(skb);
-
-	do {
-		struct sk_buff *nskb = segs->next;
-		int err;
-
-		segs->next = NULL;
-		err = dst_output(segs);
-
-		if (err && ret == 0)
-			ret = err;
-		segs = nskb;
-	} while (segs);
-
-	return ret;
-}
 
 static int ip_forward_finish(struct sk_buff *skb)
 {
@@ -114,9 +67,6 @@ static int ip_forward_finish(struct sk_buff *skb)
 	if (unlikely(opt->optlen))
 		ip_forward_options(skb);
 
-	if (ip_gso_exceeds_dst_mtu(skb))
-		return ip_forward_finish_gso(skb);
-
 	return dst_output(skb);
 }
 
diff --git a/net/ipv4/ip_fragment.c b/net/ipv4/ip_fragment.c
index c10a3ce5cbf..ed32313e307 100644
--- a/net/ipv4/ip_fragment.c
+++ b/net/ipv4/ip_fragment.c
@@ -232,8 +232,9 @@ static void ip_expire(unsigned long arg)
 		 * "Fragment Reassembly Timeout" message, per RFC792.
 		 */
 		if (qp->user == IP_DEFRAG_AF_PACKET ||
-		    (qp->user == IP_DEFRAG_CONNTRACK_IN &&
-		     skb_rtable(head)->rt_type != RTN_LOCAL))
+		    ((qp->user >= IP_DEFRAG_CONNTRACK_IN) &&
+		     (qp->user <= __IP_DEFRAG_CONNTRACK_IN_END) &&
+		     (skb_rtable(head)->rt_type != RTN_LOCAL)))
 			goto out_rcu_unlock;
 
 
diff --git a/net/ipv4/ip_output.c b/net/ipv4/ip_output.c
index 1cbeba5edff..a52f50187b5 100644
--- a/net/ipv4/ip_output.c
+++ b/net/ipv4/ip_output.c
@@ -211,6 +211,48 @@ static inline int ip_finish_output2(struct sk_buff *skb)
 	return -EINVAL;
 }
 
+static int ip_finish_output_gso(struct sk_buff *skb)
+{
+	netdev_features_t features;
+	struct sk_buff *segs;
+	int ret = 0;
+
+	/* common case: locally created skb or seglen is <= mtu */
+	if (((IPCB(skb)->flags & IPSKB_FORWARDED) == 0) ||
+	      skb_gso_network_seglen(skb) <= ip_skb_dst_mtu(skb))
+		return ip_finish_output2(skb);
+
+	/* Slowpath -  GSO segment length is exceeding the dst MTU.
+	 *
+	 * This can happen in two cases:
+	 * 1) TCP GRO packet, DF bit not set
+	 * 2) skb arrived via virtio-net, we thus get TSO/GSO skbs directly
+	 * from host network stack.
+	 */
+	features = netif_skb_features(skb);
+	segs = skb_gso_segment(skb, features & ~NETIF_F_GSO_MASK);
+	if (IS_ERR(segs)) {
+		kfree_skb(skb);
+		return -ENOMEM;
+	}
+
+	consume_skb(skb);
+
+	do {
+		struct sk_buff *nskb = segs->next;
+		int err;
+
+		segs->next = NULL;
+		err = ip_fragment(segs, ip_finish_output2);
+
+		if (err && ret == 0)
+			ret = err;
+		segs = nskb;
+	} while (segs);
+
+	return ret;
+}
+
 static int ip_finish_output(struct sk_buff *skb)
 {
 #if defined(CONFIG_NETFILTER) && defined(CONFIG_XFRM)
@@ -220,10 +262,13 @@ static int ip_finish_output(struct sk_buff *skb)
 		return dst_output(skb);
 	}
 #endif
-	if (skb->len > ip_skb_dst_mtu(skb) && !skb_is_gso(skb))
+	if (skb_is_gso(skb))
+		return ip_finish_output_gso(skb);
+
+	if (skb->len > ip_skb_dst_mtu(skb))
 		return ip_fragment(skb, ip_finish_output2);
-	else
-		return ip_finish_output2(skb);
+
+	return ip_finish_output2(skb);
 }
 
 int ip_mc_output(struct sock *sk, struct sk_buff *skb)
diff --git a/net/ipv4/ip_tunnel.c b/net/ipv4/ip_tunnel.c
index 484d0ce27ef..2acc2337d38 100644
--- a/net/ipv4/ip_tunnel.c
+++ b/net/ipv4/ip_tunnel.c
@@ -442,6 +442,8 @@ int ip_tunnel_rcv(struct ip_tunnel *tunnel, struct sk_buff *skb,
 		tunnel->i_seqno = ntohl(tpi->seq) + 1;
 	}
 
+	skb_reset_network_header(skb);
+
 	err = IP_ECN_decapsulate(iph, skb);
 	if (unlikely(err)) {
 		if (log_ecn_error)
@@ -538,9 +540,10 @@ void ip_tunnel_xmit(struct sk_buff *skb, struct net_device *dev,
 	unsigned int max_headroom;	/* The extra header space needed */
 	__be32 dst;
 	int err;
-	bool connected = true;
+	bool connected;
 
 	inner_iph = (const struct iphdr *)skb_inner_network_header(skb);
+	connected = (tunnel->parms.iph.daddr != 0);
 
 	dst = tnl_params->daddr;
 	if (dst == 0) {
@@ -722,19 +725,18 @@ static void ip_tunnel_update(struct ip_tunnel_net *itn,
 int ip_tunnel_ioctl(struct net_device *dev, struct ip_tunnel_parm *p, int cmd)
 {
 	int err = 0;
-	struct ip_tunnel *t;
-	struct net *net = dev_net(dev);
-	struct ip_tunnel *tunnel = netdev_priv(dev);
-	struct ip_tunnel_net *itn = net_generic(net, tunnel->ip_tnl_net_id);
+	struct ip_tunnel *t = netdev_priv(dev);
+	struct net *net = t->net;
+	struct ip_tunnel_net *itn = net_generic(net, t->ip_tnl_net_id);
 
 	BUG_ON(!itn->fb_tunnel_dev);
 	switch (cmd) {
 	case SIOCGETTUNNEL:
-		t = NULL;
-		if (dev == itn->fb_tunnel_dev)
+		if (dev == itn->fb_tunnel_dev) {
 			t = ip_tunnel_find(itn, p, itn->fb_tunnel_dev->type);
-		if (t == NULL)
-			t = netdev_priv(dev);
+			if (t == NULL)
+				t = netdev_priv(dev);
+		}
 		memcpy(p, &t->parms, sizeof(*p));
 		break;
 
@@ -881,6 +883,7 @@ int ip_tunnel_init_net(struct net *net, int ip_tnl_net_id,
 	 */
 	if (!IS_ERR(itn->fb_tunnel_dev)) {
 		itn->fb_tunnel_dev->features |= NETIF_F_NETNS_LOCAL;
+		itn->fb_tunnel_dev->mtu = ip_tunnel_bind_dev(itn->fb_tunnel_dev);
 		ip_tunnel_add(itn, netdev_priv(itn->fb_tunnel_dev));
 	}
 	rtnl_unlock();
diff --git a/net/ipv4/ip_vti.c b/net/ipv4/ip_vti.c
index afcee51b90e..13ef00f1e17 100644
--- a/net/ipv4/ip_vti.c
+++ b/net/ipv4/ip_vti.c
@@ -239,6 +239,7 @@ static netdev_tx_t vti_tunnel_xmit(struct sk_buff *skb, struct net_device *dev)
 static int vti4_err(struct sk_buff *skb, u32 info)
 {
 	__be32 spi;
+	__u32 mark;
 	struct xfrm_state *x;
 	struct ip_tunnel *tunnel;
 	struct ip_esp_hdr *esph;
@@ -254,6 +255,8 @@ static int vti4_err(struct sk_buff *skb, u32 info)
 	if (!tunnel)
 		return -1;
 
+	mark = be32_to_cpu(tunnel->parms.o_key);
+
 	switch (protocol) {
 	case IPPROTO_ESP:
 		esph = (struct ip_esp_hdr *)(skb->data+(iph->ihl<<2));
@@ -281,7 +284,7 @@ static int vti4_err(struct sk_buff *skb, u32 info)
 		return 0;
 	}
 
-	x = xfrm_state_lookup(net, skb->mark, (const xfrm_address_t *)&iph->daddr,
+	x = xfrm_state_lookup(net, mark, (const xfrm_address_t *)&iph->daddr,
 			      spi, protocol, AF_INET);
 	if (!x)
 		return 0;
diff --git a/net/ipv4/ipmr.c b/net/ipv4/ipmr.c
index 28863570dd6..d84dc8d4c91 100644
--- a/net/ipv4/ipmr.c
+++ b/net/ipv4/ipmr.c
@@ -455,7 +455,7 @@ static netdev_tx_t reg_vif_xmit(struct sk_buff *skb, struct net_device *dev)
 	struct mr_table *mrt;
 	struct flowi4 fl4 = {
 		.flowi4_oif	= dev->ifindex,
-		.flowi4_iif	= skb->skb_iif,
+		.flowi4_iif	= skb->skb_iif ? : LOOPBACK_IFINDEX,
 		.flowi4_mark	= skb->mark,
 	};
 	int err;
diff --git a/net/ipv4/netfilter/ipt_rpfilter.c b/net/ipv4/netfilter/ipt_rpfilter.c
index c49dcd0284a..4bfaedf9b34 100644
--- a/net/ipv4/netfilter/ipt_rpfilter.c
+++ b/net/ipv4/netfilter/ipt_rpfilter.c
@@ -89,11 +89,8 @@ static bool rpfilter_mt(const struct sk_buff *skb, struct xt_action_param *par)
 	if (ipv4_is_multicast(iph->daddr)) {
 		if (ipv4_is_zeronet(iph->saddr))
 			return ipv4_is_local_multicast(iph->daddr) ^ invert;
-		flow.flowi4_iif = 0;
-	} else {
-		flow.flowi4_iif = LOOPBACK_IFINDEX;
 	}
-
+	flow.flowi4_iif = LOOPBACK_IFINDEX;
 	flow.daddr = iph->saddr;
 	flow.saddr = rpfilter_get_saddr(iph->daddr);
 	flow.flowi4_oif = 0;
diff --git a/net/ipv4/netfilter/nf_defrag_ipv4.c b/net/ipv4/netfilter/nf_defrag_ipv4.c
index 12e13bd82b5..f40f321b41f 100644
--- a/net/ipv4/netfilter/nf_defrag_ipv4.c
+++ b/net/ipv4/netfilter/nf_defrag_ipv4.c
@@ -22,7 +22,6 @@
 #endif
 #include <net/netfilter/nf_conntrack_zones.h>
 
-/* Returns new sk_buff, or NULL */
 static int nf_ct_ipv4_gather_frags(struct sk_buff *skb, u_int32_t user)
 {
 	int err;
@@ -33,8 +32,10 @@ static int nf_ct_ipv4_gather_frags(struct sk_buff *skb, u_int32_t user)
 	err = ip_defrag(skb, user);
 	local_bh_enable();
 
-	if (!err)
+	if (!err) {
 		ip_send_check(ip_hdr(skb));
+		skb->local_df = 1;
+	}
 
 	return err;
 }
diff --git a/net/ipv4/ping.c b/net/ipv4/ping.c
index 8210964a9f1..044a0ddf6a7 100644
--- a/net/ipv4/ping.c
+++ b/net/ipv4/ping.c
@@ -236,15 +236,15 @@ exit:
 static void inet_get_ping_group_range_net(struct net *net, kgid_t *low,
 					  kgid_t *high)
 {
-	kgid_t *data = net->ipv4.sysctl_ping_group_range;
+	kgid_t *data = net->ipv4.ping_group_range.range;
 	unsigned int seq;
 
 	do {
-		seq = read_seqbegin(&net->ipv4.sysctl_local_ports.lock);
+		seq = read_seqbegin(&net->ipv4.ping_group_range.lock);
 
 		*low = data[0];
 		*high = data[1];
-	} while (read_seqretry(&net->ipv4.sysctl_local_ports.lock, seq));
+	} while (read_seqretry(&net->ipv4.ping_group_range.lock, seq));
 }
 
 
diff --git a/net/ipv4/route.c b/net/ipv4/route.c
index 1485aafcad5..5e676be3dae 100644
--- a/net/ipv4/route.c
+++ b/net/ipv4/route.c
@@ -1519,7 +1519,7 @@ static int __mkroute_input(struct sk_buff *skb,
 	struct in_device *out_dev;
 	unsigned int flags = 0;
 	bool do_cache;
-	u32 itag;
+	u32 itag = 0;
 
 	/* get a working reference to the output device */
 	out_dev = __in_dev_get_rcu(FIB_RES_DEV(*res));
@@ -1700,8 +1700,7 @@ static int ip_route_input_slow(struct sk_buff *skb, __be32 daddr, __be32 saddr,
 
 	if (res.type == RTN_LOCAL) {
 		err = fib_validate_source(skb, saddr, daddr, tos,
-					  LOOPBACK_IFINDEX,
-					  dev, in_dev, &itag);
+					  0, dev, in_dev, &itag);
 		if (err < 0)
 			goto martian_source_keep_err;
 		goto local_input;
diff --git a/net/ipv4/sysctl_net_ipv4.c b/net/ipv4/sysctl_net_ipv4.c
index 44eba052b43..5cde8f263d4 100644
--- a/net/ipv4/sysctl_net_ipv4.c
+++ b/net/ipv4/sysctl_net_ipv4.c
@@ -45,10 +45,10 @@ static int ip_ping_group_range_max[] = { GID_T_MAX, GID_T_MAX };
 /* Update system visible IP port range */
 static void set_local_port_range(struct net *net, int range[2])
 {
-	write_seqlock(&net->ipv4.sysctl_local_ports.lock);
-	net->ipv4.sysctl_local_ports.range[0] = range[0];
-	net->ipv4.sysctl_local_ports.range[1] = range[1];
-	write_sequnlock(&net->ipv4.sysctl_local_ports.lock);
+	write_seqlock(&net->ipv4.ip_local_ports.lock);
+	net->ipv4.ip_local_ports.range[0] = range[0];
+	net->ipv4.ip_local_ports.range[1] = range[1];
+	write_sequnlock(&net->ipv4.ip_local_ports.lock);
 }
 
 /* Validate changes from /proc interface. */
@@ -57,7 +57,7 @@ static int ipv4_local_port_range(struct ctl_table *table, int write,
 				 size_t *lenp, loff_t *ppos)
 {
 	struct net *net =
-		container_of(table->data, struct net, ipv4.sysctl_local_ports.range);
+		container_of(table->data, struct net, ipv4.ip_local_ports.range);
 	int ret;
 	int range[2];
 	struct ctl_table tmp = {
@@ -87,14 +87,14 @@ static void inet_get_ping_group_range_table(struct ctl_table *table, kgid_t *low
 {
 	kgid_t *data = table->data;
 	struct net *net =
-		container_of(table->data, struct net, ipv4.sysctl_ping_group_range);
+		container_of(table->data, struct net, ipv4.ping_group_range.range);
 	unsigned int seq;
 	do {
-		seq = read_seqbegin(&net->ipv4.sysctl_local_ports.lock);
+		seq = read_seqbegin(&net->ipv4.ip_local_ports.lock);
 
 		*low = data[0];
 		*high = data[1];
-	} while (read_seqretry(&net->ipv4.sysctl_local_ports.lock, seq));
+	} while (read_seqretry(&net->ipv4.ip_local_ports.lock, seq));
 }
 
 /* Update system visible IP port range */
@@ -102,11 +102,11 @@ static void set_ping_group_range(struct ctl_table *table, kgid_t low, kgid_t hig
 {
 	kgid_t *data = table->data;
 	struct net *net =
-		container_of(table->data, struct net, ipv4.sysctl_ping_group_range);
-	write_seqlock(&net->ipv4.sysctl_local_ports.lock);
+		container_of(table->data, struct net, ipv4.ping_group_range.range);
+	write_seqlock(&net->ipv4.ip_local_ports.lock);
 	data[0] = low;
 	data[1] = high;
-	write_sequnlock(&net->ipv4.sysctl_local_ports.lock);
+	write_sequnlock(&net->ipv4.ip_local_ports.lock);
 }
 
 /* Validate changes from /proc interface. */
@@ -805,7 +805,7 @@ static struct ctl_table ipv4_net_table[] = {
 	},
 	{
 		.procname	= "ping_group_range",
-		.data		= &init_net.ipv4.sysctl_ping_group_range,
+		.data		= &init_net.ipv4.ping_group_range.range,
 		.maxlen		= sizeof(gid_t)*2,
 		.mode		= 0644,
 		.proc_handler	= ipv4_ping_group_range,
@@ -819,8 +819,8 @@ static struct ctl_table ipv4_net_table[] = {
 	},
 	{
 		.procname	= "ip_local_port_range",
-		.maxlen		= sizeof(init_net.ipv4.sysctl_local_ports.range),
-		.data		= &init_net.ipv4.sysctl_local_ports.range,
+		.maxlen		= sizeof(init_net.ipv4.ip_local_ports.range),
+		.data		= &init_net.ipv4.ip_local_ports.range,
 		.mode		= 0644,
 		.proc_handler	= ipv4_local_port_range,
 	},
@@ -858,20 +858,6 @@ static __net_init int ipv4_sysctl_init_net(struct net *net)
 			table[i].data += (void *)net - (void *)&init_net;
 	}
 
-	/*
-	 * Sane defaults - nobody may create ping sockets.
-	 * Boot scripts should set this to distro-specific group.
-	 */
-	net->ipv4.sysctl_ping_group_range[0] = make_kgid(&init_user_ns, 1);
-	net->ipv4.sysctl_ping_group_range[1] = make_kgid(&init_user_ns, 0);
-
-	/*
-	 * Set defaults for local port range
-	 */
-	seqlock_init(&net->ipv4.sysctl_local_ports.lock);
-	net->ipv4.sysctl_local_ports.range[0] =  32768;
-	net->ipv4.sysctl_local_ports.range[1] =  61000;
-
 	net->ipv4.ipv4_hdr = register_net_sysctl(net, "net/ipv4", table);
 	if (net->ipv4.ipv4_hdr == NULL)
 		goto err_reg;
diff --git a/net/ipv4/tcp_cubic.c b/net/ipv4/tcp_cubic.c
index 8bf224516ba..b4f1b29b08b 100644
--- a/net/ipv4/tcp_cubic.c
+++ b/net/ipv4/tcp_cubic.c
@@ -409,7 +409,7 @@ static void bictcp_acked(struct sock *sk, u32 cnt, s32 rtt_us)
 		ratio -= ca->delayed_ack >> ACK_RATIO_SHIFT;
 		ratio += cnt;
 
-		ca->delayed_ack = min(ratio, ACK_RATIO_LIMIT);
+		ca->delayed_ack = clamp(ratio, 1U, ACK_RATIO_LIMIT);
 	}
 
 	/* Some calls are for duplicates without timetamps */
diff --git a/net/ipv4/tcp_output.c b/net/ipv4/tcp_output.c
index 366cf06587b..2d340bd2cd3 100644
--- a/net/ipv4/tcp_output.c
+++ b/net/ipv4/tcp_output.c
@@ -2439,8 +2439,14 @@ int __tcp_retransmit_skb(struct sock *sk, struct sk_buff *skb)
 		err = tcp_transmit_skb(sk, skb, 1, GFP_ATOMIC);
 	}
 
-	if (likely(!err))
+	if (likely(!err)) {
 		TCP_SKB_CB(skb)->sacked |= TCPCB_EVER_RETRANS;
+		/* Update global TCP statistics. */
+		TCP_INC_STATS(sock_net(sk), TCP_MIB_RETRANSSEGS);
+		if (TCP_SKB_CB(skb)->tcp_flags & TCPHDR_SYN)
+			NET_INC_STATS_BH(sock_net(sk), LINUX_MIB_TCPSYNRETRANS);
+		tp->total_retrans++;
+	}
 	return err;
 }
 
@@ -2450,12 +2456,6 @@ int tcp_retransmit_skb(struct sock *sk, struct sk_buff *skb)
 	int err = __tcp_retransmit_skb(sk, skb);
 
 	if (err == 0) {
-		/* Update global TCP statistics. */
-		TCP_INC_STATS(sock_net(sk), TCP_MIB_RETRANSSEGS);
-		if (TCP_SKB_CB(skb)->tcp_flags & TCPHDR_SYN)
-			NET_INC_STATS_BH(sock_net(sk), LINUX_MIB_TCPSYNRETRANS);
-		tp->total_retrans++;
-
 #if FASTRETRANS_DEBUG > 0
 		if (TCP_SKB_CB(skb)->sacked & TCPCB_SACKED_RETRANS) {
 			net_dbg_ratelimited("retrans_out leaked\n");
diff --git a/net/ipv4/xfrm4_output.c b/net/ipv4/xfrm4_output.c
index 40e701f2e1e..186a8ecf92f 100644
--- a/net/ipv4/xfrm4_output.c
+++ b/net/ipv4/xfrm4_output.c
@@ -62,10 +62,7 @@ int xfrm4_prepare_output(struct xfrm_state *x, struct sk_buff *skb)
 	if (err)
 		return err;
 
-	memset(IPCB(skb), 0, sizeof(*IPCB(skb)));
-	IPCB(skb)->flags |= IPSKB_XFRM_TUNNEL_SIZE | IPSKB_XFRM_TRANSFORMED;
-
-	skb->protocol = htons(ETH_P_IP);
+	IPCB(skb)->flags |= IPSKB_XFRM_TUNNEL_SIZE;
 
 	return x->outer_mode->output2(x, skb);
 }
@@ -73,27 +70,34 @@ EXPORT_SYMBOL(xfrm4_prepare_output);
 
 int xfrm4_output_finish(struct sk_buff *skb)
 {
+	memset(IPCB(skb), 0, sizeof(*IPCB(skb)));
+	skb->protocol = htons(ETH_P_IP);
+
+#ifdef CONFIG_NETFILTER
+	IPCB(skb)->flags |= IPSKB_XFRM_TRANSFORMED;
+#endif
+
+	return xfrm_output(skb);
+}
+
+static int __xfrm4_output(struct sk_buff *skb)
+{
+	struct xfrm_state *x = skb_dst(skb)->xfrm;
+
 #ifdef CONFIG_NETFILTER
-	if (!skb_dst(skb)->xfrm) {
+	if (!x) {
 		IPCB(skb)->flags |= IPSKB_REROUTED;
 		return dst_output(skb);
 	}
-
-	IPCB(skb)->flags |= IPSKB_XFRM_TRANSFORMED;
 #endif
 
-	skb->protocol = htons(ETH_P_IP);
-	return xfrm_output(skb);
+	return x->outer_mode->afinfo->output_finish(skb);
 }
 
 int xfrm4_output(struct sock *sk, struct sk_buff *skb)
 {
-	struct dst_entry *dst = skb_dst(skb);
-	struct xfrm_state *x = dst->xfrm;
-
 	return NF_HOOK_COND(NFPROTO_IPV4, NF_INET_POST_ROUTING, skb,
-			    NULL, dst->dev,
-			    x->outer_mode->afinfo->output_finish,
+			    NULL, skb_dst(skb)->dev, __xfrm4_output,
 			    !(IPCB(skb)->flags & IPSKB_REROUTED));
 }
 
diff --git a/net/ipv4/xfrm4_protocol.c b/net/ipv4/xfrm4_protocol.c
index 7f7b243e813..a2ce0101eaa 100644
--- a/net/ipv4/xfrm4_protocol.c
+++ b/net/ipv4/xfrm4_protocol.c
@@ -50,8 +50,12 @@ int xfrm4_rcv_cb(struct sk_buff *skb, u8 protocol, int err)
 {
 	int ret;
 	struct xfrm4_protocol *handler;
+	struct xfrm4_protocol __rcu **head = proto_handlers(protocol);
 
-	for_each_protocol_rcu(*proto_handlers(protocol), handler)
+	if (!head)
+		return 0;
+
+	for_each_protocol_rcu(*head, handler)
 		if ((ret = handler->cb_handler(skb, err)) <= 0)
 			return ret;
 
@@ -64,15 +68,20 @@ int xfrm4_rcv_encap(struct sk_buff *skb, int nexthdr, __be32 spi,
 {
 	int ret;
 	struct xfrm4_protocol *handler;
+	struct xfrm4_protocol __rcu **head = proto_handlers(nexthdr);
 
 	XFRM_TUNNEL_SKB_CB(skb)->tunnel.ip4 = NULL;
 	XFRM_SPI_SKB_CB(skb)->family = AF_INET;
 	XFRM_SPI_SKB_CB(skb)->daddroff = offsetof(struct iphdr, daddr);
 
-	for_each_protocol_rcu(*proto_handlers(nexthdr), handler)
+	if (!head)
+		goto out;
+
+	for_each_protocol_rcu(*head, handler)
 		if ((ret = handler->input_handler(skb, nexthdr, spi, encap_type)) != -EINVAL)
 			return ret;
 
+out:
 	icmp_send(skb, ICMP_DEST_UNREACH, ICMP_PORT_UNREACH, 0);
 
 	kfree_skb(skb);
@@ -208,6 +217,9 @@ int xfrm4_protocol_register(struct xfrm4_protocol *handler,
 	int ret = -EEXIST;
 	int priority = handler->priority;
 
+	if (!proto_handlers(protocol) || !netproto(protocol))
+		return -EINVAL;
+
 	mutex_lock(&xfrm4_protocol_mutex);
 
 	if (!rcu_dereference_protected(*proto_handlers(protocol),
@@ -250,6 +262,9 @@ int xfrm4_protocol_deregister(struct xfrm4_protocol *handler,
 	struct xfrm4_protocol *t;
 	int ret = -ENOENT;
 
+	if (!proto_handlers(protocol) || !netproto(protocol))
+		return -EINVAL;
+
 	mutex_lock(&xfrm4_protocol_mutex);
 
 	for (pprev = proto_handlers(protocol);
diff --git a/net/ipv6/ip6_fib.c b/net/ipv6/ip6_fib.c
index 34e0ded5c14..87891f5f57b 100644
--- a/net/ipv6/ip6_fib.c
+++ b/net/ipv6/ip6_fib.c
@@ -1459,7 +1459,7 @@ static int fib6_walk_continue(struct fib6_walker_t *w)
 
 				if (w->skip) {
 					w->skip--;
-					continue;
+					goto skip;
 				}
 
 				err = w->func(w);
@@ -1469,6 +1469,7 @@ static int fib6_walk_continue(struct fib6_walker_t *w)
 				w->count++;
 				continue;
 			}
+skip:
 			w->state = FWS_U;
 		case FWS_U:
 			if (fn == w->root)
diff --git a/net/ipv6/ip6_offload.c b/net/ipv6/ip6_offload.c
index 59f95affceb..b2f091566f8 100644
--- a/net/ipv6/ip6_offload.c
+++ b/net/ipv6/ip6_offload.c
@@ -196,7 +196,6 @@ static struct sk_buff **ipv6_gro_receive(struct sk_buff **head,
 	unsigned int off;
 	u16 flush = 1;
 	int proto;
-	__wsum csum;
 
 	off = skb_gro_offset(skb);
 	hlen = off + sizeof(*iph);
@@ -264,13 +263,10 @@ static struct sk_buff **ipv6_gro_receive(struct sk_buff **head,
 
 	NAPI_GRO_CB(skb)->flush |= flush;
 
-	csum = skb->csum;
-	skb_postpull_rcsum(skb, iph, skb_network_header_len(skb));
+	skb_gro_postpull_rcsum(skb, iph, nlen);
 
 	pp = ops->callbacks.gro_receive(head, skb);
 
-	skb->csum = csum;
-
 out_unlock:
 	rcu_read_unlock();
 
diff --git a/net/ipv6/ip6_output.c b/net/ipv6/ip6_output.c
index 40e7581374f..fbf11562b54 100644
--- a/net/ipv6/ip6_output.c
+++ b/net/ipv6/ip6_output.c
@@ -344,12 +344,16 @@ static unsigned int ip6_dst_mtu_forward(const struct dst_entry *dst)
 
 static bool ip6_pkt_too_big(const struct sk_buff *skb, unsigned int mtu)
 {
-	if (skb->len <= mtu || skb->local_df)
+	if (skb->len <= mtu)
 		return false;
 
+	/* ipv6 conntrack defrag sets max_frag_size + local_df */
 	if (IP6CB(skb)->frag_max_size && IP6CB(skb)->frag_max_size > mtu)
 		return true;
 
+	if (skb->local_df)
+		return false;
+
 	if (skb_is_gso(skb) && skb_gso_network_seglen(skb) <= mtu)
 		return false;
 
@@ -1225,7 +1229,7 @@ int ip6_append_data(struct sock *sk, int getfrag(void *from, char *to,
 		unsigned int maxnonfragsize, headersize;
 
 		headersize = sizeof(struct ipv6hdr) +
-			     (opt ? opt->tot_len : 0) +
+			     (opt ? opt->opt_flen + opt->opt_nflen : 0) +
 			     (dst_allfrag(&rt->dst) ?
 			      sizeof(struct frag_hdr) : 0) +
 			     rt->rt6i_nfheader_len;
diff --git a/net/ipv6/ip6_tunnel.c b/net/ipv6/ip6_tunnel.c
index e1df691d78b..f6a66bb4114 100644
--- a/net/ipv6/ip6_tunnel.c
+++ b/net/ipv6/ip6_tunnel.c
@@ -1340,8 +1340,8 @@ ip6_tnl_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 	int err = 0;
 	struct ip6_tnl_parm p;
 	struct __ip6_tnl_parm p1;
-	struct ip6_tnl *t = NULL;
-	struct net *net = dev_net(dev);
+	struct ip6_tnl *t = netdev_priv(dev);
+	struct net *net = t->net;
 	struct ip6_tnl_net *ip6n = net_generic(net, ip6_tnl_net_id);
 
 	switch (cmd) {
@@ -1353,11 +1353,11 @@ ip6_tnl_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 			}
 			ip6_tnl_parm_from_user(&p1, &p);
 			t = ip6_tnl_locate(net, &p1, 0);
+			if (t == NULL)
+				t = netdev_priv(dev);
 		} else {
 			memset(&p, 0, sizeof(p));
 		}
-		if (t == NULL)
-			t = netdev_priv(dev);
 		ip6_tnl_parm_to_user(&p, &t->parms);
 		if (copy_to_user(ifr->ifr_ifru.ifru_data, &p, sizeof (p))) {
 			err = -EFAULT;
@@ -1557,7 +1557,7 @@ static int ip6_tnl_validate(struct nlattr *tb[], struct nlattr *data[])
 {
 	u8 proto;
 
-	if (!data)
+	if (!data || !data[IFLA_IPTUN_PROTO])
 		return 0;
 
 	proto = nla_get_u8(data[IFLA_IPTUN_PROTO]);
diff --git a/net/ipv6/ip6_vti.c b/net/ipv6/ip6_vti.c
index b7c0f827140..6cc9f9371cc 100644
--- a/net/ipv6/ip6_vti.c
+++ b/net/ipv6/ip6_vti.c
@@ -511,6 +511,7 @@ static int vti6_err(struct sk_buff *skb, struct inet6_skb_parm *opt,
 		    u8 type, u8 code, int offset, __be32 info)
 {
 	__be32 spi;
+	__u32 mark;
 	struct xfrm_state *x;
 	struct ip6_tnl *t;
 	struct ip_esp_hdr *esph;
@@ -524,6 +525,8 @@ static int vti6_err(struct sk_buff *skb, struct inet6_skb_parm *opt,
 	if (!t)
 		return -1;
 
+	mark = be32_to_cpu(t->parms.o_key);
+
 	switch (protocol) {
 	case IPPROTO_ESP:
 		esph = (struct ip_esp_hdr *)(skb->data + offset);
@@ -545,7 +548,7 @@ static int vti6_err(struct sk_buff *skb, struct inet6_skb_parm *opt,
 	    type != NDISC_REDIRECT)
 		return 0;
 
-	x = xfrm_state_lookup(net, skb->mark, (const xfrm_address_t *)&iph->daddr,
+	x = xfrm_state_lookup(net, mark, (const xfrm_address_t *)&iph->daddr,
 			      spi, protocol, AF_INET6);
 	if (!x)
 		return 0;
@@ -1097,7 +1100,6 @@ static int __init vti6_tunnel_init(void)
 
 	err = xfrm6_protocol_register(&vti_esp6_protocol, IPPROTO_ESP);
 	if (err < 0) {
-		unregister_pernet_device(&vti6_net_ops);
 		pr_err("%s: can't register vti6 protocol\n", __func__);
 
 		goto out;
@@ -1106,7 +1108,6 @@ static int __init vti6_tunnel_init(void)
 	err = xfrm6_protocol_register(&vti_ah6_protocol, IPPROTO_AH);
 	if (err < 0) {
 		xfrm6_protocol_deregister(&vti_esp6_protocol, IPPROTO_ESP);
-		unregister_pernet_device(&vti6_net_ops);
 		pr_err("%s: can't register vti6 protocol\n", __func__);
 
 		goto out;
@@ -1116,7 +1117,6 @@ static int __init vti6_tunnel_init(void)
 	if (err < 0) {
 		xfrm6_protocol_deregister(&vti_ah6_protocol, IPPROTO_AH);
 		xfrm6_protocol_deregister(&vti_esp6_protocol, IPPROTO_ESP);
-		unregister_pernet_device(&vti6_net_ops);
 		pr_err("%s: can't register vti6 protocol\n", __func__);
 
 		goto out;
diff --git a/net/ipv6/ip6mr.c b/net/ipv6/ip6mr.c
index 8737400af0a..8250474ab7d 100644
--- a/net/ipv6/ip6mr.c
+++ b/net/ipv6/ip6mr.c
@@ -700,7 +700,7 @@ static netdev_tx_t reg_vif_xmit(struct sk_buff *skb,
 	struct mr6_table *mrt;
 	struct flowi6 fl6 = {
 		.flowi6_oif	= dev->ifindex,
-		.flowi6_iif	= skb->skb_iif,
+		.flowi6_iif	= skb->skb_iif ? : LOOPBACK_IFINDEX,
 		.flowi6_mark	= skb->mark,
 	};
 	int err;
@@ -1633,7 +1633,7 @@ struct sock *mroute6_socket(struct net *net, struct sk_buff *skb)
 {
 	struct mr6_table *mrt;
 	struct flowi6 fl6 = {
-		.flowi6_iif	= skb->skb_iif,
+		.flowi6_iif	= skb->skb_iif ? : LOOPBACK_IFINDEX,
 		.flowi6_oif	= skb->dev->ifindex,
 		.flowi6_mark	= skb->mark,
 	};
diff --git a/net/ipv6/ndisc.c b/net/ipv6/ndisc.c
index 09a22f4f36c..ca8d4ea48a5 100644
--- a/net/ipv6/ndisc.c
+++ b/net/ipv6/ndisc.c
@@ -851,7 +851,7 @@ out:
 static void ndisc_recv_na(struct sk_buff *skb)
 {
 	struct nd_msg *msg = (struct nd_msg *)skb_transport_header(skb);
-	const struct in6_addr *saddr = &ipv6_hdr(skb)->saddr;
+	struct in6_addr *saddr = &ipv6_hdr(skb)->saddr;
 	const struct in6_addr *daddr = &ipv6_hdr(skb)->daddr;
 	u8 *lladdr = NULL;
 	u32 ndoptlen = skb_tail_pointer(skb) - (skb_transport_header(skb) +
@@ -944,10 +944,7 @@ static void ndisc_recv_na(struct sk_buff *skb)
 			/*
 			 * Change: router to host
 			 */
-			struct rt6_info *rt;
-			rt = rt6_get_dflt_router(saddr, dev);
-			if (rt)
-				ip6_del_rt(rt);
+			rt6_clean_tohost(dev_net(dev),  saddr);
 		}
 
 out:
diff --git a/net/ipv6/netfilter.c b/net/ipv6/netfilter.c
index 95f3f1da0d7..d38e6a8d8b9 100644
--- a/net/ipv6/netfilter.c
+++ b/net/ipv6/netfilter.c
@@ -30,13 +30,15 @@ int ip6_route_me_harder(struct sk_buff *skb)
 		.daddr = iph->daddr,
 		.saddr = iph->saddr,
 	};
+	int err;
 
 	dst = ip6_route_output(net, skb->sk, &fl6);
-	if (dst->error) {
+	err = dst->error;
+	if (err) {
 		IP6_INC_STATS(net, ip6_dst_idev(dst), IPSTATS_MIB_OUTNOROUTES);
 		LIMIT_NETDEBUG(KERN_DEBUG "ip6_route_me_harder: No more route.\n");
 		dst_release(dst);
-		return dst->error;
+		return err;
 	}
 
 	/* Drop old route. */
diff --git a/net/ipv6/netfilter/ip6t_rpfilter.c b/net/ipv6/netfilter/ip6t_rpfilter.c
index e0983f3648a..790e0c6b19e 100644
--- a/net/ipv6/netfilter/ip6t_rpfilter.c
+++ b/net/ipv6/netfilter/ip6t_rpfilter.c
@@ -33,6 +33,7 @@ static bool rpfilter_lookup_reverse6(const struct sk_buff *skb,
 	struct ipv6hdr *iph = ipv6_hdr(skb);
 	bool ret = false;
 	struct flowi6 fl6 = {
+		.flowi6_iif = LOOPBACK_IFINDEX,
 		.flowlabel = (* (__be32 *) iph) & IPV6_FLOWINFO_MASK,
 		.flowi6_proto = iph->nexthdr,
 		.daddr = iph->saddr,
diff --git a/net/ipv6/route.c b/net/ipv6/route.c
index 4011617cca6..6ebdb7b6744 100644
--- a/net/ipv6/route.c
+++ b/net/ipv6/route.c
@@ -1273,6 +1273,7 @@ void ip6_redirect(struct sk_buff *skb, struct net *net, int oif, u32 mark)
 	struct flowi6 fl6;
 
 	memset(&fl6, 0, sizeof(fl6));
+	fl6.flowi6_iif = LOOPBACK_IFINDEX;
 	fl6.flowi6_oif = oif;
 	fl6.flowi6_mark = mark;
 	fl6.daddr = iph->daddr;
@@ -1294,6 +1295,7 @@ void ip6_redirect_no_header(struct sk_buff *skb, struct net *net, int oif,
 	struct flowi6 fl6;
 
 	memset(&fl6, 0, sizeof(fl6));
+	fl6.flowi6_iif = LOOPBACK_IFINDEX;
 	fl6.flowi6_oif = oif;
 	fl6.flowi6_mark = mark;
 	fl6.daddr = msg->dest;
@@ -2232,6 +2234,27 @@ void rt6_remove_prefsrc(struct inet6_ifaddr *ifp)
 	fib6_clean_all(net, fib6_remove_prefsrc, &adni);
 }
 
+#define RTF_RA_ROUTER		(RTF_ADDRCONF | RTF_DEFAULT | RTF_GATEWAY)
+#define RTF_CACHE_GATEWAY	(RTF_GATEWAY | RTF_CACHE)
+
+/* Remove routers and update dst entries when gateway turn into host. */
+static int fib6_clean_tohost(struct rt6_info *rt, void *arg)
+{
+	struct in6_addr *gateway = (struct in6_addr *)arg;
+
+	if ((((rt->rt6i_flags & RTF_RA_ROUTER) == RTF_RA_ROUTER) ||
+	     ((rt->rt6i_flags & RTF_CACHE_GATEWAY) == RTF_CACHE_GATEWAY)) &&
+	     ipv6_addr_equal(gateway, &rt->rt6i_gateway)) {
+		return -1;
+	}
+	return 0;
+}
+
+void rt6_clean_tohost(struct net *net, struct in6_addr *gateway)
+{
+	fib6_clean_all(net, fib6_clean_tohost, gateway);
+}
+
 struct arg_dev_net {
 	struct net_device *dev;
 	struct net *net;
@@ -2707,6 +2730,9 @@ static int inet6_rtm_getroute(struct sk_buff *in_skb, struct nlmsghdr* nlh)
 	if (tb[RTA_OIF])
 		oif = nla_get_u32(tb[RTA_OIF]);
 
+	if (tb[RTA_MARK])
+		fl6.flowi6_mark = nla_get_u32(tb[RTA_MARK]);
+
 	if (iif) {
 		struct net_device *dev;
 		int flags = 0;
diff --git a/net/ipv6/sit.c b/net/ipv6/sit.c
index 8da8268d65f..e5a453ca302 100644
--- a/net/ipv6/sit.c
+++ b/net/ipv6/sit.c
@@ -1127,8 +1127,8 @@ ipip6_tunnel_ioctl (struct net_device *dev, struct ifreq *ifr, int cmd)
 	int err = 0;
 	struct ip_tunnel_parm p;
 	struct ip_tunnel_prl prl;
-	struct ip_tunnel *t;
-	struct net *net = dev_net(dev);
+	struct ip_tunnel *t = netdev_priv(dev);
+	struct net *net = t->net;
 	struct sit_net *sitn = net_generic(net, sit_net_id);
 #ifdef CONFIG_IPV6_SIT_6RD
 	struct ip_tunnel_6rd ip6rd;
@@ -1139,16 +1139,15 @@ ipip6_tunnel_ioctl (struct net_device *dev, struct ifreq *ifr, int cmd)
 #ifdef CONFIG_IPV6_SIT_6RD
 	case SIOCGET6RD:
 #endif
-		t = NULL;
 		if (dev == sitn->fb_tunnel_dev) {
 			if (copy_from_user(&p, ifr->ifr_ifru.ifru_data, sizeof(p))) {
 				err = -EFAULT;
 				break;
 			}
 			t = ipip6_tunnel_locate(net, &p, 0);
+			if (t == NULL)
+				t = netdev_priv(dev);
 		}
-		if (t == NULL)
-			t = netdev_priv(dev);
 
 		err = -EFAULT;
 		if (cmd == SIOCGETTUNNEL) {
@@ -1244,9 +1243,6 @@ ipip6_tunnel_ioctl (struct net_device *dev, struct ifreq *ifr, int cmd)
 		err = -EINVAL;
 		if (dev == sitn->fb_tunnel_dev)
 			goto done;
-		err = -ENOENT;
-		if (!(t = netdev_priv(dev)))
-			goto done;
 		err = ipip6_tunnel_get_prl(t, ifr->ifr_ifru.ifru_data);
 		break;
 
@@ -1262,9 +1258,6 @@ ipip6_tunnel_ioctl (struct net_device *dev, struct ifreq *ifr, int cmd)
 		err = -EFAULT;
 		if (copy_from_user(&prl, ifr->ifr_ifru.ifru_data, sizeof(prl)))
 			goto done;
-		err = -ENOENT;
-		if (!(t = netdev_priv(dev)))
-			goto done;
 
 		switch (cmd) {
 		case SIOCDELPRL:
@@ -1292,8 +1285,6 @@ ipip6_tunnel_ioctl (struct net_device *dev, struct ifreq *ifr, int cmd)
 				   sizeof(ip6rd)))
 			goto done;
 
-		t = netdev_priv(dev);
-
 		if (cmd != SIOCDEL6RD) {
 			err = ipip6_tunnel_update_6rd(t, &ip6rd);
 			if (err < 0)
diff --git a/net/ipv6/tcpv6_offload.c b/net/ipv6/tcpv6_offload.c
index 0d78132ff18..8517d3cd1ae 100644
--- a/net/ipv6/tcpv6_offload.c
+++ b/net/ipv6/tcpv6_offload.c
@@ -42,7 +42,7 @@ static struct sk_buff **tcp6_gro_receive(struct sk_buff **head,
 	if (NAPI_GRO_CB(skb)->flush)
 		goto skip_csum;
 
-	wsum = skb->csum;
+	wsum = NAPI_GRO_CB(skb)->csum;
 
 	switch (skb->ip_summed) {
 	case CHECKSUM_NONE:
diff --git a/net/ipv6/xfrm6_output.c b/net/ipv6/xfrm6_output.c
index 19ef329bdbf..b930d080c66 100644
--- a/net/ipv6/xfrm6_output.c
+++ b/net/ipv6/xfrm6_output.c
@@ -114,12 +114,6 @@ int xfrm6_prepare_output(struct xfrm_state *x, struct sk_buff *skb)
 	if (err)
 		return err;
 
-	memset(IP6CB(skb), 0, sizeof(*IP6CB(skb)));
-#ifdef CONFIG_NETFILTER
-	IP6CB(skb)->flags |= IP6SKB_XFRM_TRANSFORMED;
-#endif
-
-	skb->protocol = htons(ETH_P_IPV6);
 	skb->local_df = 1;
 
 	return x->outer_mode->output2(x, skb);
@@ -128,11 +122,13 @@ EXPORT_SYMBOL(xfrm6_prepare_output);
 
 int xfrm6_output_finish(struct sk_buff *skb)
 {
+	memset(IP6CB(skb), 0, sizeof(*IP6CB(skb)));
+	skb->protocol = htons(ETH_P_IPV6);
+
 #ifdef CONFIG_NETFILTER
 	IP6CB(skb)->flags |= IP6SKB_XFRM_TRANSFORMED;
 #endif
 
-	skb->protocol = htons(ETH_P_IPV6);
 	return xfrm_output(skb);
 }
 
@@ -142,6 +138,13 @@ static int __xfrm6_output(struct sk_buff *skb)
 	struct xfrm_state *x = dst->xfrm;
 	int mtu;
 
+#ifdef CONFIG_NETFILTER
+	if (!x) {
+		IP6CB(skb)->flags |= IP6SKB_REROUTED;
+		return dst_output(skb);
+	}
+#endif
+
 	if (skb->protocol == htons(ETH_P_IPV6))
 		mtu = ip6_skb_dst_mtu(skb);
 	else
@@ -165,6 +168,7 @@ static int __xfrm6_output(struct sk_buff *skb)
 
 int xfrm6_output(struct sock *sk, struct sk_buff *skb)
 {
-	return NF_HOOK(NFPROTO_IPV6, NF_INET_POST_ROUTING, skb, NULL,
-		       skb_dst(skb)->dev, __xfrm6_output);
+	return NF_HOOK_COND(NFPROTO_IPV6, NF_INET_POST_ROUTING, skb,
+			    NULL, skb_dst(skb)->dev, __xfrm6_output,
+			    !(IP6CB(skb)->flags & IP6SKB_REROUTED));
 }
diff --git a/net/ipv6/xfrm6_protocol.c b/net/ipv6/xfrm6_protocol.c
index 6ab989c486f..54d13f8dbba 100644
--- a/net/ipv6/xfrm6_protocol.c
+++ b/net/ipv6/xfrm6_protocol.c
@@ -50,6 +50,10 @@ int xfrm6_rcv_cb(struct sk_buff *skb, u8 protocol, int err)
 {
 	int ret;
 	struct xfrm6_protocol *handler;
+	struct xfrm6_protocol __rcu **head = proto_handlers(protocol);
+
+	if (!head)
+		return 0;
 
 	for_each_protocol_rcu(*proto_handlers(protocol), handler)
 		if ((ret = handler->cb_handler(skb, err)) <= 0)
@@ -184,10 +188,12 @@ int xfrm6_protocol_register(struct xfrm6_protocol *handler,
 	struct xfrm6_protocol __rcu **pprev;
 	struct xfrm6_protocol *t;
 	bool add_netproto = false;
-
 	int ret = -EEXIST;
 	int priority = handler->priority;
 
+	if (!proto_handlers(protocol) || !netproto(protocol))
+		return -EINVAL;
+
 	mutex_lock(&xfrm6_protocol_mutex);
 
 	if (!rcu_dereference_protected(*proto_handlers(protocol),
@@ -230,6 +236,9 @@ int xfrm6_protocol_deregister(struct xfrm6_protocol *handler,
 	struct xfrm6_protocol *t;
 	int ret = -ENOENT;
 
+	if (!proto_handlers(protocol) || !netproto(protocol))
+		return -EINVAL;
+
 	mutex_lock(&xfrm6_protocol_mutex);
 
 	for (pprev = proto_handlers(protocol);
diff --git a/net/iucv/af_iucv.c b/net/iucv/af_iucv.c
index 01e77b0ae07..8c9d7302c84 100644
--- a/net/iucv/af_iucv.c
+++ b/net/iucv/af_iucv.c
@@ -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 (msg->tag != IUCV_SKB_CB(list_skb)->tag) {
+			if (msg->tag == IUCV_SKB_CB(list_skb)->tag) {
 				this = list_skb;
 				break;
 			}
diff --git a/net/mac80211/chan.c b/net/mac80211/chan.c
index bd1fd8ea510..75b5dd2c926 100644
--- a/net/mac80211/chan.c
+++ b/net/mac80211/chan.c
@@ -249,7 +249,7 @@ ieee80211_new_chanctx(struct ieee80211_local *local,
 
 	if (!local->use_chanctx) {
 		local->_oper_chandef = *chandef;
-		ieee80211_hw_config(local, 0);
+		ieee80211_hw_config(local, IEEE80211_CONF_CHANGE_CHANNEL);
 	} else {
 		err = drv_add_chanctx(local, ctx);
 		if (err) {
@@ -286,7 +286,7 @@ static void ieee80211_free_chanctx(struct ieee80211_local *local,
 			check_single_channel = true;
 		local->hw.conf.radar_enabled = false;
 
-		ieee80211_hw_config(local, 0);
+		ieee80211_hw_config(local, IEEE80211_CONF_CHANGE_CHANNEL);
 	} else {
 		drv_remove_chanctx(local, ctx);
 	}
@@ -492,6 +492,13 @@ void ieee80211_recalc_smps_chanctx(struct ieee80211_local *local,
 		rx_chains_static = max(rx_chains_static, needed_static);
 		rx_chains_dynamic = max(rx_chains_dynamic, needed_dynamic);
 	}
+
+	/* Disable SMPS for the monitor interface */
+	sdata = rcu_dereference(local->monitor_sdata);
+	if (sdata &&
+	    rcu_access_pointer(sdata->vif.chanctx_conf) == &chanctx->conf)
+		rx_chains_dynamic = rx_chains_static = local->rx_chains;
+
 	rcu_read_unlock();
 
 	if (!local->use_chanctx) {
diff --git a/net/mac80211/ieee80211_i.h b/net/mac80211/ieee80211_i.h
index 222c28b7531..f169b6ee94e 100644
--- a/net/mac80211/ieee80211_i.h
+++ b/net/mac80211/ieee80211_i.h
@@ -317,6 +317,7 @@ struct ieee80211_roc_work {
 
 	bool started, abort, hw_begun, notified;
 	bool to_be_freed;
+	bool on_channel;
 
 	unsigned long hw_start_time;
 
diff --git a/net/mac80211/main.c b/net/mac80211/main.c
index b055f6a55c6..4c1bf61bc77 100644
--- a/net/mac80211/main.c
+++ b/net/mac80211/main.c
@@ -148,6 +148,8 @@ static u32 ieee80211_hw_conf_chan(struct ieee80211_local *local)
 	list_for_each_entry_rcu(sdata, &local->interfaces, list) {
 		if (!rcu_access_pointer(sdata->vif.chanctx_conf))
 			continue;
+		if (sdata->vif.type == NL80211_IFTYPE_AP_VLAN)
+			continue;
 		power = min(power, sdata->vif.bss_conf.txpower);
 	}
 	rcu_read_unlock();
@@ -199,7 +201,7 @@ void ieee80211_bss_info_change_notify(struct ieee80211_sub_if_data *sdata,
 {
 	struct ieee80211_local *local = sdata->local;
 
-	if (!changed)
+	if (!changed || sdata->vif.type == NL80211_IFTYPE_AP_VLAN)
 		return;
 
 	drv_bss_info_changed(local, sdata, &sdata->vif.bss_conf, changed);
diff --git a/net/mac80211/mlme.c b/net/mac80211/mlme.c
index dee50aefd6e..27600a9808b 100644
--- a/net/mac80211/mlme.c
+++ b/net/mac80211/mlme.c
@@ -3598,18 +3598,24 @@ void ieee80211_mgd_quiesce(struct ieee80211_sub_if_data *sdata)
 
 	sdata_lock(sdata);
 
-	if (ifmgd->auth_data) {
+	if (ifmgd->auth_data || ifmgd->assoc_data) {
+		const u8 *bssid = ifmgd->auth_data ?
+				ifmgd->auth_data->bss->bssid :
+				ifmgd->assoc_data->bss->bssid;
+
 		/*
-		 * If we are trying to authenticate while suspending, cfg80211
-		 * won't know and won't actually abort those attempts, thus we
-		 * need to do that ourselves.
+		 * If we are trying to authenticate / associate while suspending,
+		 * cfg80211 won't know and won't actually abort those attempts,
+		 * thus we need to do that ourselves.
 		 */
-		ieee80211_send_deauth_disassoc(sdata,
-					       ifmgd->auth_data->bss->bssid,
+		ieee80211_send_deauth_disassoc(sdata, bssid,
 					       IEEE80211_STYPE_DEAUTH,
 					       WLAN_REASON_DEAUTH_LEAVING,
 					       false, frame_buf);
-		ieee80211_destroy_auth_data(sdata, false);
+		if (ifmgd->assoc_data)
+			ieee80211_destroy_assoc_data(sdata, false);
+		if (ifmgd->auth_data)
+			ieee80211_destroy_auth_data(sdata, false);
 		cfg80211_tx_mlme_mgmt(sdata->dev, frame_buf,
 				      IEEE80211_DEAUTH_FRAME_LEN);
 	}
diff --git a/net/mac80211/offchannel.c b/net/mac80211/offchannel.c
index 0c2a29484c0..7a17decd27f 100644
--- a/net/mac80211/offchannel.c
+++ b/net/mac80211/offchannel.c
@@ -333,7 +333,7 @@ void ieee80211_sw_roc_work(struct work_struct *work)
 		container_of(work, struct ieee80211_roc_work, work.work);
 	struct ieee80211_sub_if_data *sdata = roc->sdata;
 	struct ieee80211_local *local = sdata->local;
-	bool started;
+	bool started, on_channel;
 
 	mutex_lock(&local->mtx);
 
@@ -354,13 +354,26 @@ void ieee80211_sw_roc_work(struct work_struct *work)
 	if (!roc->started) {
 		struct ieee80211_roc_work *dep;
 
-		/* start this ROC */
+		WARN_ON(local->use_chanctx);
+
+		/* If actually operating on the desired channel (with at least
+		 * 20 MHz channel width) don't stop all the operations but still
+		 * treat it as though the ROC operation started properly, so
+		 * other ROC operations won't interfere with this one.
+		 */
+		roc->on_channel = roc->chan == local->_oper_chandef.chan &&
+				  local->_oper_chandef.width != NL80211_CHAN_WIDTH_5 &&
+				  local->_oper_chandef.width != NL80211_CHAN_WIDTH_10;
 
-		/* switch channel etc */
+		/* start this ROC */
 		ieee80211_recalc_idle(local);
 
-		local->tmp_channel = roc->chan;
-		ieee80211_hw_config(local, 0);
+		if (!roc->on_channel) {
+			ieee80211_offchannel_stop_vifs(local);
+
+			local->tmp_channel = roc->chan;
+			ieee80211_hw_config(local, 0);
+		}
 
 		/* tell userspace or send frame */
 		ieee80211_handle_roc_started(roc);
@@ -379,9 +392,10 @@ void ieee80211_sw_roc_work(struct work_struct *work)
  finish:
 		list_del(&roc->list);
 		started = roc->started;
+		on_channel = roc->on_channel;
 		ieee80211_roc_notify_destroy(roc, !roc->abort);
 
-		if (started) {
+		if (started && !on_channel) {
 			ieee80211_flush_queues(local, NULL);
 
 			local->tmp_channel = NULL;
diff --git a/net/mac80211/rx.c b/net/mac80211/rx.c
index 216c45b949e..2b608b2b70e 100644
--- a/net/mac80211/rx.c
+++ b/net/mac80211/rx.c
@@ -1231,7 +1231,8 @@ ieee80211_rx_h_sta_process(struct ieee80211_rx_data *rx)
 		if (ether_addr_equal(bssid, rx->sdata->u.ibss.bssid) &&
 		    test_sta_flag(sta, WLAN_STA_AUTHORIZED)) {
 			sta->last_rx = jiffies;
-			if (ieee80211_is_data(hdr->frame_control)) {
+			if (ieee80211_is_data(hdr->frame_control) &&
+			    !is_multicast_ether_addr(hdr->addr1)) {
 				sta->last_rx_rate_idx = status->rate_idx;
 				sta->last_rx_rate_flag = status->flag;
 				sta->last_rx_rate_vht_flag = status->vht_flag;
diff --git a/net/mac80211/sta_info.c b/net/mac80211/sta_info.c
index 137a192e64b..847d92f6bef 100644
--- a/net/mac80211/sta_info.c
+++ b/net/mac80211/sta_info.c
@@ -1148,7 +1148,8 @@ void ieee80211_sta_ps_deliver_wakeup(struct sta_info *sta)
 	atomic_dec(&ps->num_sta_ps);
 
 	/* This station just woke up and isn't aware of our SMPS state */
-	if (!ieee80211_smps_is_restrictive(sta->known_smps_mode,
+	if (!ieee80211_vif_is_mesh(&sdata->vif) &&
+	    !ieee80211_smps_is_restrictive(sta->known_smps_mode,
 					   sdata->smps_mode) &&
 	    sta->known_smps_mode != sdata->bss->req_smps &&
 	    sta_info_tx_streams(sta) != 1) {
diff --git a/net/mac80211/status.c b/net/mac80211/status.c
index e6e574a307c..60cb7a66597 100644
--- a/net/mac80211/status.c
+++ b/net/mac80211/status.c
@@ -314,10 +314,9 @@ ieee80211_add_tx_radiotap_header(struct ieee80211_local *local,
 	    !is_multicast_ether_addr(hdr->addr1))
 		txflags |= IEEE80211_RADIOTAP_F_TX_FAIL;
 
-	if ((info->status.rates[0].flags & IEEE80211_TX_RC_USE_RTS_CTS) ||
-	    (info->status.rates[0].flags & IEEE80211_TX_RC_USE_CTS_PROTECT))
+	if (info->status.rates[0].flags & IEEE80211_TX_RC_USE_CTS_PROTECT)
 		txflags |= IEEE80211_RADIOTAP_F_TX_CTS;
-	else if (info->status.rates[0].flags & IEEE80211_TX_RC_USE_RTS_CTS)
+	if (info->status.rates[0].flags & IEEE80211_TX_RC_USE_RTS_CTS)
 		txflags |= IEEE80211_RADIOTAP_F_TX_RTS;
 
 	put_unaligned_le16(txflags, pos);
@@ -618,6 +617,7 @@ void ieee80211_tx_status(struct ieee80211_hw *hw, struct sk_buff *skb)
 					sta, true, acked);
 
 		if ((local->hw.flags & IEEE80211_HW_HAS_RATE_CONTROL) &&
+		    (ieee80211_is_data(hdr->frame_control)) &&
 		    (rates_idx != -1))
 			sta->last_tx_rate = info->status.rates[rates_idx];
 
diff --git a/net/mac80211/trace.h b/net/mac80211/trace.h
index a0b0aea7652..cec5b60487a 100644
--- a/net/mac80211/trace.h
+++ b/net/mac80211/trace.h
@@ -21,10 +21,10 @@
 
 #define VIF_ENTRY	__field(enum nl80211_iftype, vif_type) __field(void *, sdata)	\
 			__field(bool, p2p)						\
-			__string(vif_name, sdata->dev ? sdata->dev->name : "<nodev>")
+			__string(vif_name, sdata->name)
 #define VIF_ASSIGN	__entry->vif_type = sdata->vif.type; __entry->sdata = sdata;	\
 			__entry->p2p = sdata->vif.p2p;					\
-			__assign_str(vif_name, sdata->dev ? sdata->dev->name : sdata->name)
+			__assign_str(vif_name, sdata->name)
 #define VIF_PR_FMT	" vif:%s(%d%s)"
 #define VIF_PR_ARG	__get_str(vif_name), __entry->vif_type, __entry->p2p ? "/p2p" : ""
 
diff --git a/net/mac80211/util.c b/net/mac80211/util.c
index 275c94f995f..3c365837e91 100644
--- a/net/mac80211/util.c
+++ b/net/mac80211/util.c
@@ -1780,7 +1780,7 @@ int ieee80211_reconfig(struct ieee80211_local *local)
 	mutex_unlock(&local->mtx);
 
 	if (sched_scan_stopped)
-		cfg80211_sched_scan_stopped(local->hw.wiphy);
+		cfg80211_sched_scan_stopped_rtnl(local->hw.wiphy);
 
 	/*
 	 * If this is for hw restart things are still running.
diff --git a/net/mac80211/vht.c b/net/mac80211/vht.c
index e9e36a25616..9265adfdabf 100644
--- a/net/mac80211/vht.c
+++ b/net/mac80211/vht.c
@@ -129,9 +129,12 @@ ieee80211_vht_cap_ie_to_sta_vht_cap(struct ieee80211_sub_if_data *sdata,
 	if (!vht_cap_ie || !sband->vht_cap.vht_supported)
 		return;
 
-	/* A VHT STA must support 40 MHz */
-	if (!(sta->sta.ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40))
-		return;
+	/*
+	 * A VHT STA must support 40 MHz, but if we verify that here
+	 * then we break a few things - some APs (e.g. Netgear R6300v2
+	 * and others based on the BCM4360 chipset) will unset this
+	 * capability bit when operating in 20 MHz.
+	 */
 
 	vht_cap->vht_supported = true;
 
diff --git a/net/netfilter/nf_conntrack_netlink.c b/net/netfilter/nf_conntrack_netlink.c
index ccc46fa5edb..58579634427 100644
--- a/net/netfilter/nf_conntrack_netlink.c
+++ b/net/netfilter/nf_conntrack_netlink.c
@@ -1336,6 +1336,9 @@ ctnetlink_setup_nat(struct nf_conn *ct, const struct nlattr * const cda[])
 #ifdef CONFIG_NF_NAT_NEEDED
 	int ret;
 
+	if (!cda[CTA_NAT_DST] && !cda[CTA_NAT_SRC])
+		return 0;
+
 	ret = ctnetlink_parse_nat_setup(ct, NF_NAT_MANIP_DST,
 					cda[CTA_NAT_DST]);
 	if (ret < 0)
diff --git a/net/netfilter/nf_tables_core.c b/net/netfilter/nf_tables_core.c
index 804105391b9..345acfb1720 100644
--- a/net/netfilter/nf_tables_core.c
+++ b/net/netfilter/nf_tables_core.c
@@ -66,20 +66,6 @@ struct nft_jumpstack {
 	int			rulenum;
 };
 
-static inline void
-nft_chain_stats(const struct nft_chain *this, const struct nft_pktinfo *pkt,
-		struct nft_jumpstack *jumpstack, unsigned int stackptr)
-{
-	struct nft_stats __percpu *stats;
-	const struct nft_chain *chain = stackptr ? jumpstack[0].chain : this;
-
-	rcu_read_lock_bh();
-	stats = rcu_dereference(nft_base_chain(chain)->stats);
-	__this_cpu_inc(stats->pkts);
-	__this_cpu_add(stats->bytes, pkt->skb->len);
-	rcu_read_unlock_bh();
-}
-
 enum nft_trace {
 	NFT_TRACE_RULE,
 	NFT_TRACE_RETURN,
@@ -117,13 +103,14 @@ static void nft_trace_packet(const struct nft_pktinfo *pkt,
 unsigned int
 nft_do_chain(struct nft_pktinfo *pkt, const struct nf_hook_ops *ops)
 {
-	const struct nft_chain *chain = ops->priv;
+	const struct nft_chain *chain = ops->priv, *basechain = chain;
 	const struct nft_rule *rule;
 	const struct nft_expr *expr, *last;
 	struct nft_data data[NFT_REG_MAX + 1];
 	unsigned int stackptr = 0;
 	struct nft_jumpstack jumpstack[NFT_JUMP_STACK_SIZE];
-	int rulenum = 0;
+	struct nft_stats __percpu *stats;
+	int rulenum;
 	/*
 	 * Cache cursor to avoid problems in case that the cursor is updated
 	 * while traversing the ruleset.
@@ -131,6 +118,7 @@ nft_do_chain(struct nft_pktinfo *pkt, const struct nf_hook_ops *ops)
 	unsigned int gencursor = ACCESS_ONCE(chain->net->nft.gencursor);
 
 do_chain:
+	rulenum = 0;
 	rule = list_entry(&chain->rules, struct nft_rule, list);
 next_rule:
 	data[NFT_REG_VERDICT].verdict = NFT_CONTINUE;
@@ -156,8 +144,10 @@ next_rule:
 		switch (data[NFT_REG_VERDICT].verdict) {
 		case NFT_BREAK:
 			data[NFT_REG_VERDICT].verdict = NFT_CONTINUE;
-			/* fall through */
+			continue;
 		case NFT_CONTINUE:
+			if (unlikely(pkt->skb->nf_trace))
+				nft_trace_packet(pkt, chain, rulenum, NFT_TRACE_RULE);
 			continue;
 		}
 		break;
@@ -183,37 +173,44 @@ next_rule:
 		jumpstack[stackptr].rule  = rule;
 		jumpstack[stackptr].rulenum = rulenum;
 		stackptr++;
-		/* fall through */
+		chain = data[NFT_REG_VERDICT].chain;
+		goto do_chain;
 	case NFT_GOTO:
+		if (unlikely(pkt->skb->nf_trace))
+			nft_trace_packet(pkt, chain, rulenum, NFT_TRACE_RULE);
+
 		chain = data[NFT_REG_VERDICT].chain;
 		goto do_chain;
 	case NFT_RETURN:
 		if (unlikely(pkt->skb->nf_trace))
 			nft_trace_packet(pkt, chain, rulenum, NFT_TRACE_RETURN);
-
-		/* fall through */
+		break;
 	case NFT_CONTINUE:
+		if (unlikely(pkt->skb->nf_trace && !(chain->flags & NFT_BASE_CHAIN)))
+			nft_trace_packet(pkt, chain, ++rulenum, NFT_TRACE_RETURN);
 		break;
 	default:
 		WARN_ON(1);
 	}
 
 	if (stackptr > 0) {
-		if (unlikely(pkt->skb->nf_trace))
-			nft_trace_packet(pkt, chain, ++rulenum, NFT_TRACE_RETURN);
-
 		stackptr--;
 		chain = jumpstack[stackptr].chain;
 		rule  = jumpstack[stackptr].rule;
 		rulenum = jumpstack[stackptr].rulenum;
 		goto next_rule;
 	}
-	nft_chain_stats(chain, pkt, jumpstack, stackptr);
 
 	if (unlikely(pkt->skb->nf_trace))
-		nft_trace_packet(pkt, chain, ++rulenum, NFT_TRACE_POLICY);
+		nft_trace_packet(pkt, basechain, -1, NFT_TRACE_POLICY);
+
+	rcu_read_lock_bh();
+	stats = rcu_dereference(nft_base_chain(basechain)->stats);
+	__this_cpu_inc(stats->pkts);
+	__this_cpu_add(stats->bytes, pkt->skb->len);
+	rcu_read_unlock_bh();
 
-	return nft_base_chain(chain)->policy;
+	return nft_base_chain(basechain)->policy;
 }
 EXPORT_SYMBOL_GPL(nft_do_chain);
 
diff --git a/net/netfilter/nfnetlink.c b/net/netfilter/nfnetlink.c
index e8138da4c14..23ef77c60ff 100644
--- a/net/netfilter/nfnetlink.c
+++ b/net/netfilter/nfnetlink.c
@@ -256,15 +256,15 @@ replay:
 #endif
 		{
 			nfnl_unlock(subsys_id);
-			kfree_skb(nskb);
-			return netlink_ack(skb, nlh, -EOPNOTSUPP);
+			netlink_ack(skb, nlh, -EOPNOTSUPP);
+			return kfree_skb(nskb);
 		}
 	}
 
 	if (!ss->commit || !ss->abort) {
 		nfnl_unlock(subsys_id);
-		kfree_skb(nskb);
-		return netlink_ack(skb, nlh, -EOPNOTSUPP);
+		netlink_ack(skb, nlh, -EOPNOTSUPP);
+		return kfree_skb(skb);
 	}
 
 	while (skb->len >= nlmsg_total_size(0)) {
@@ -368,14 +368,13 @@ done:
 static void nfnetlink_rcv(struct sk_buff *skb)
 {
 	struct nlmsghdr *nlh = nlmsg_hdr(skb);
-	struct net *net = sock_net(skb->sk);
 	int msglen;
 
 	if (nlh->nlmsg_len < NLMSG_HDRLEN ||
 	    skb->len < nlh->nlmsg_len)
 		return;
 
-	if (!ns_capable(net->user_ns, CAP_NET_ADMIN)) {
+	if (!netlink_net_capable(skb, CAP_NET_ADMIN)) {
 		netlink_ack(skb, nlh, -EPERM);
 		return;
 	}
diff --git a/net/netlink/af_netlink.c b/net/netlink/af_netlink.c
index 894cda0206b..81dca96d2be 100644
--- a/net/netlink/af_netlink.c
+++ b/net/netlink/af_netlink.c
@@ -1360,7 +1360,72 @@ retry:
 	return err;
 }
 
-static inline int netlink_capable(const struct socket *sock, unsigned int flag)
+/**
+ * __netlink_ns_capable - General netlink message capability test
+ * @nsp: NETLINK_CB of the socket buffer holding a netlink command from userspace.
+ * @user_ns: The user namespace of the capability to use
+ * @cap: The capability to use
+ *
+ * Test to see if the opener of the socket we received the message
+ * from had when the netlink socket was created and the sender of the
+ * message has has the capability @cap in the user namespace @user_ns.
+ */
+bool __netlink_ns_capable(const struct netlink_skb_parms *nsp,
+			struct user_namespace *user_ns, int cap)
+{
+	return sk_ns_capable(nsp->sk, user_ns, cap);
+}
+EXPORT_SYMBOL(__netlink_ns_capable);
+
+/**
+ * netlink_ns_capable - General netlink message capability test
+ * @skb: socket buffer holding a netlink command from userspace
+ * @user_ns: The user namespace of the capability to use
+ * @cap: The capability to use
+ *
+ * Test to see if the opener of the socket we received the message
+ * from had when the netlink socket was created and the sender of the
+ * message has has the capability @cap in the user namespace @user_ns.
+ */
+bool netlink_ns_capable(const struct sk_buff *skb,
+			struct user_namespace *user_ns, int cap)
+{
+	return __netlink_ns_capable(&NETLINK_CB(skb), user_ns, cap);
+}
+EXPORT_SYMBOL(netlink_ns_capable);
+
+/**
+ * netlink_capable - Netlink global message capability test
+ * @skb: socket buffer holding a netlink command from userspace
+ * @cap: The capability to use
+ *
+ * Test to see if the opener of the socket we received the message
+ * from had when the netlink socket was created and the sender of the
+ * message has has the capability @cap in all user namespaces.
+ */
+bool netlink_capable(const struct sk_buff *skb, int cap)
+{
+	return netlink_ns_capable(skb, &init_user_ns, cap);
+}
+EXPORT_SYMBOL(netlink_capable);
+
+/**
+ * netlink_net_capable - Netlink network namespace message capability test
+ * @skb: socket buffer holding a netlink command from userspace
+ * @cap: The capability to use
+ *
+ * Test to see if the opener of the socket we received the message
+ * from had when the netlink socket was created and the sender of the
+ * message has has the capability @cap over the network namespace of
+ * the socket we received the message from.
+ */
+bool netlink_net_capable(const struct sk_buff *skb, int cap)
+{
+	return netlink_ns_capable(skb, sock_net(skb->sk)->user_ns, cap);
+}
+EXPORT_SYMBOL(netlink_net_capable);
+
+static inline int netlink_allowed(const struct socket *sock, unsigned int flag)
 {
 	return (nl_table[sock->sk->sk_protocol].flags & flag) ||
 		ns_capable(sock_net(sock->sk)->user_ns, CAP_NET_ADMIN);
@@ -1428,7 +1493,7 @@ static int netlink_bind(struct socket *sock, struct sockaddr *addr,
 
 	/* Only superuser is allowed to listen multicasts */
 	if (nladdr->nl_groups) {
-		if (!netlink_capable(sock, NL_CFG_F_NONROOT_RECV))
+		if (!netlink_allowed(sock, NL_CFG_F_NONROOT_RECV))
 			return -EPERM;
 		err = netlink_realloc_groups(sk);
 		if (err)
@@ -1490,7 +1555,7 @@ static int netlink_connect(struct socket *sock, struct sockaddr *addr,
 		return -EINVAL;
 
 	if ((nladdr->nl_groups || nladdr->nl_pid) &&
-	    !netlink_capable(sock, NL_CFG_F_NONROOT_SEND))
+	    !netlink_allowed(sock, NL_CFG_F_NONROOT_SEND))
 		return -EPERM;
 
 	if (!nlk->portid)
@@ -2096,7 +2161,7 @@ static int netlink_setsockopt(struct socket *sock, int level, int optname,
 		break;
 	case NETLINK_ADD_MEMBERSHIP:
 	case NETLINK_DROP_MEMBERSHIP: {
-		if (!netlink_capable(sock, NL_CFG_F_NONROOT_RECV))
+		if (!netlink_allowed(sock, NL_CFG_F_NONROOT_RECV))
 			return -EPERM;
 		err = netlink_realloc_groups(sk);
 		if (err)
@@ -2247,7 +2312,7 @@ static int netlink_sendmsg(struct kiocb *kiocb, struct socket *sock,
 		dst_group = ffs(addr->nl_groups);
 		err =  -EPERM;
 		if ((dst_group || dst_portid) &&
-		    !netlink_capable(sock, NL_CFG_F_NONROOT_SEND))
+		    !netlink_allowed(sock, NL_CFG_F_NONROOT_SEND))
 			goto out;
 	} else {
 		dst_portid = nlk->dst_portid;
diff --git a/net/netlink/genetlink.c b/net/netlink/genetlink.c
index b1dcdb932a8..a3ba3ca0ff9 100644
--- a/net/netlink/genetlink.c
+++ b/net/netlink/genetlink.c
@@ -561,7 +561,7 @@ static int genl_family_rcv_msg(struct genl_family *family,
 		return -EOPNOTSUPP;
 
 	if ((ops->flags & GENL_ADMIN_PERM) &&
-	    !capable(CAP_NET_ADMIN))
+	    !netlink_capable(skb, CAP_NET_ADMIN))
 		return -EPERM;
 
 	if ((nlh->nlmsg_flags & NLM_F_DUMP) == NLM_F_DUMP) {
diff --git a/net/packet/diag.c b/net/packet/diag.c
index 533ce4ff108..92f2c7107ee 100644
--- a/net/packet/diag.c
+++ b/net/packet/diag.c
@@ -128,6 +128,7 @@ static int pdiag_put_fanout(struct packet_sock *po, struct sk_buff *nlskb)
 
 static int sk_diag_fill(struct sock *sk, struct sk_buff *skb,
 			struct packet_diag_req *req,
+			bool may_report_filterinfo,
 			struct user_namespace *user_ns,
 			u32 portid, u32 seq, u32 flags, int sk_ino)
 {
@@ -172,7 +173,8 @@ static int sk_diag_fill(struct sock *sk, struct sk_buff *skb,
 		goto out_nlmsg_trim;
 
 	if ((req->pdiag_show & PACKET_SHOW_FILTER) &&
-	    sock_diag_put_filterinfo(user_ns, sk, skb, PACKET_DIAG_FILTER))
+	    sock_diag_put_filterinfo(may_report_filterinfo, sk, skb,
+				     PACKET_DIAG_FILTER))
 		goto out_nlmsg_trim;
 
 	return nlmsg_end(skb, nlh);
@@ -188,9 +190,11 @@ static int packet_diag_dump(struct sk_buff *skb, struct netlink_callback *cb)
 	struct packet_diag_req *req;
 	struct net *net;
 	struct sock *sk;
+	bool may_report_filterinfo;
 
 	net = sock_net(skb->sk);
 	req = nlmsg_data(cb->nlh);
+	may_report_filterinfo = netlink_net_capable(cb->skb, CAP_NET_ADMIN);
 
 	mutex_lock(&net->packet.sklist_lock);
 	sk_for_each(sk, &net->packet.sklist) {
@@ -200,6 +204,7 @@ static int packet_diag_dump(struct sk_buff *skb, struct netlink_callback *cb)
 			goto next;
 
 		if (sk_diag_fill(sk, skb, req,
+				 may_report_filterinfo,
 				 sk_user_ns(NETLINK_CB(cb->skb).sk),
 				 NETLINK_CB(cb->skb).portid,
 				 cb->nlh->nlmsg_seq, NLM_F_MULTI,
diff --git a/net/phonet/pn_netlink.c b/net/phonet/pn_netlink.c
index dc15f430080..b64151ade6b 100644
--- a/net/phonet/pn_netlink.c
+++ b/net/phonet/pn_netlink.c
@@ -70,10 +70,10 @@ static int addr_doit(struct sk_buff *skb, struct nlmsghdr *nlh)
 	int err;
 	u8 pnaddr;
 
-	if (!capable(CAP_NET_ADMIN))
+	if (!netlink_capable(skb, CAP_NET_ADMIN))
 		return -EPERM;
 
-	if (!capable(CAP_SYS_ADMIN))
+	if (!netlink_capable(skb, CAP_SYS_ADMIN))
 		return -EPERM;
 
 	ASSERT_RTNL();
@@ -233,10 +233,10 @@ static int route_doit(struct sk_buff *skb, struct nlmsghdr *nlh)
 	int err;
 	u8 dst;
 
-	if (!capable(CAP_NET_ADMIN))
+	if (!netlink_capable(skb, CAP_NET_ADMIN))
 		return -EPERM;
 
-	if (!capable(CAP_SYS_ADMIN))
+	if (!netlink_capable(skb, CAP_SYS_ADMIN))
 		return -EPERM;
 
 	ASSERT_RTNL();
diff --git a/net/rxrpc/ar-key.c b/net/rxrpc/ar-key.c
index 7633a752c65..0ad080790a3 100644
--- a/net/rxrpc/ar-key.c
+++ b/net/rxrpc/ar-key.c
@@ -99,7 +99,7 @@ static int rxrpc_instantiate_xdr_rxkad(struct key *key, const __be32 *xdr,
 	_debug("tktlen: %x", tktlen);
 	if (tktlen > AFSTOKEN_RK_TIX_MAX)
 		return -EKEYREJECTED;
-	if (8 * 4 + tktlen != toklen)
+	if (toklen < 8 * 4 + tktlen)
 		return -EKEYREJECTED;
 
 	plen = sizeof(*token) + sizeof(*token->kad) + tktlen;
diff --git a/net/sched/act_api.c b/net/sched/act_api.c
index 8a5ba5add4b..648778aef1a 100644
--- a/net/sched/act_api.c
+++ b/net/sched/act_api.c
@@ -948,7 +948,7 @@ static int tc_ctl_action(struct sk_buff *skb, struct nlmsghdr *n)
 	u32 portid = skb ? NETLINK_CB(skb).portid : 0;
 	int ret = 0, ovr = 0;
 
-	if ((n->nlmsg_type != RTM_GETACTION) && !capable(CAP_NET_ADMIN))
+	if ((n->nlmsg_type != RTM_GETACTION) && !netlink_capable(skb, CAP_NET_ADMIN))
 		return -EPERM;
 
 	ret = nlmsg_parse(n, sizeof(struct tcamsg), tca, TCA_ACT_MAX, NULL);
diff --git a/net/sched/cls_api.c b/net/sched/cls_api.c
index 29a30a14c31..bdbdb1a7920 100644
--- a/net/sched/cls_api.c
+++ b/net/sched/cls_api.c
@@ -134,7 +134,7 @@ static int tc_ctl_tfilter(struct sk_buff *skb, struct nlmsghdr *n)
 	int err;
 	int tp_created = 0;
 
-	if ((n->nlmsg_type != RTM_GETTFILTER) && !capable(CAP_NET_ADMIN))
+	if ((n->nlmsg_type != RTM_GETTFILTER) && !netlink_capable(skb, CAP_NET_ADMIN))
 		return -EPERM;
 
 replay:
diff --git a/net/sched/cls_tcindex.c b/net/sched/cls_tcindex.c
index eed8404443d..f435a88d899 100644
--- a/net/sched/cls_tcindex.c
+++ b/net/sched/cls_tcindex.c
@@ -188,6 +188,12 @@ static const struct nla_policy tcindex_policy[TCA_TCINDEX_MAX + 1] = {
 	[TCA_TCINDEX_CLASSID]		= { .type = NLA_U32 },
 };
 
+static void tcindex_filter_result_init(struct tcindex_filter_result *r)
+{
+	memset(r, 0, sizeof(*r));
+	tcf_exts_init(&r->exts, TCA_TCINDEX_ACT, TCA_TCINDEX_POLICE);
+}
+
 static int
 tcindex_set_parms(struct net *net, struct tcf_proto *tp, unsigned long base,
 		  u32 handle, struct tcindex_data *p,
@@ -207,15 +213,11 @@ tcindex_set_parms(struct net *net, struct tcf_proto *tp, unsigned long base,
 		return err;
 
 	memcpy(&cp, p, sizeof(cp));
-	memset(&new_filter_result, 0, sizeof(new_filter_result));
-	tcf_exts_init(&new_filter_result.exts, TCA_TCINDEX_ACT, TCA_TCINDEX_POLICE);
+	tcindex_filter_result_init(&new_filter_result);
 
+	tcindex_filter_result_init(&cr);
 	if (old_r)
-		memcpy(&cr, r, sizeof(cr));
-	else {
-		memset(&cr, 0, sizeof(cr));
-		tcf_exts_init(&cr.exts, TCA_TCINDEX_ACT, TCA_TCINDEX_POLICE);
-	}
+		cr.res = r->res;
 
 	if (tb[TCA_TCINDEX_HASH])
 		cp.hash = nla_get_u32(tb[TCA_TCINDEX_HASH]);
@@ -267,9 +269,14 @@ tcindex_set_parms(struct net *net, struct tcf_proto *tp, unsigned long base,
 	err = -ENOMEM;
 	if (!cp.perfect && !cp.h) {
 		if (valid_perfect_hash(&cp)) {
+			int i;
+
 			cp.perfect = kcalloc(cp.hash, sizeof(*r), GFP_KERNEL);
 			if (!cp.perfect)
 				goto errout;
+			for (i = 0; i < cp.hash; i++)
+				tcf_exts_init(&cp.perfect[i].exts, TCA_TCINDEX_ACT,
+					      TCA_TCINDEX_POLICE);
 			balloc = 1;
 		} else {
 			cp.h = kcalloc(cp.hash, sizeof(f), GFP_KERNEL);
@@ -295,14 +302,17 @@ tcindex_set_parms(struct net *net, struct tcf_proto *tp, unsigned long base,
 		tcf_bind_filter(tp, &cr.res, base);
 	}
 
-	tcf_exts_change(tp, &cr.exts, &e);
+	if (old_r)
+		tcf_exts_change(tp, &r->exts, &e);
+	else
+		tcf_exts_change(tp, &cr.exts, &e);
 
 	tcf_tree_lock(tp);
 	if (old_r && old_r != r)
-		memset(old_r, 0, sizeof(*old_r));
+		tcindex_filter_result_init(old_r);
 
 	memcpy(p, &cp, sizeof(cp));
-	memcpy(r, &cr, sizeof(cr));
+	r->res = cr.res;
 
 	if (r == &new_filter_result) {
 		struct tcindex_filter **fp;
diff --git a/net/sched/sch_api.c b/net/sched/sch_api.c
index a0b84e0e22d..400769014bb 100644
--- a/net/sched/sch_api.c
+++ b/net/sched/sch_api.c
@@ -1084,7 +1084,7 @@ static int tc_get_qdisc(struct sk_buff *skb, struct nlmsghdr *n)
 	struct Qdisc *p = NULL;
 	int err;
 
-	if ((n->nlmsg_type != RTM_GETQDISC) && !capable(CAP_NET_ADMIN))
+	if ((n->nlmsg_type != RTM_GETQDISC) && !netlink_capable(skb, CAP_NET_ADMIN))
 		return -EPERM;
 
 	err = nlmsg_parse(n, sizeof(*tcm), tca, TCA_MAX, NULL);
@@ -1151,7 +1151,7 @@ static int tc_modify_qdisc(struct sk_buff *skb, struct nlmsghdr *n)
 	struct Qdisc *q, *p;
 	int err;
 
-	if (!capable(CAP_NET_ADMIN))
+	if (!netlink_capable(skb, CAP_NET_ADMIN))
 		return -EPERM;
 
 replay:
@@ -1490,7 +1490,7 @@ static int tc_ctl_tclass(struct sk_buff *skb, struct nlmsghdr *n)
 	u32 qid;
 	int err;
 
-	if ((n->nlmsg_type != RTM_GETTCLASS) && !capable(CAP_NET_ADMIN))
+	if ((n->nlmsg_type != RTM_GETTCLASS) && !netlink_capable(skb, CAP_NET_ADMIN))
 		return -EPERM;
 
 	err = nlmsg_parse(n, sizeof(*tcm), tca, TCA_MAX, NULL);
diff --git a/net/sched/sch_hhf.c b/net/sched/sch_hhf.c
index edee03d922e..6e957c3b985 100644
--- a/net/sched/sch_hhf.c
+++ b/net/sched/sch_hhf.c
@@ -553,11 +553,6 @@ static int hhf_change(struct Qdisc *sch, struct nlattr *opt)
 	if (err < 0)
 		return err;
 
-	sch_tree_lock(sch);
-
-	if (tb[TCA_HHF_BACKLOG_LIMIT])
-		sch->limit = nla_get_u32(tb[TCA_HHF_BACKLOG_LIMIT]);
-
 	if (tb[TCA_HHF_QUANTUM])
 		new_quantum = nla_get_u32(tb[TCA_HHF_QUANTUM]);
 
@@ -567,6 +562,12 @@ static int hhf_change(struct Qdisc *sch, struct nlattr *opt)
 	non_hh_quantum = (u64)new_quantum * new_hhf_non_hh_weight;
 	if (non_hh_quantum > INT_MAX)
 		return -EINVAL;
+
+	sch_tree_lock(sch);
+
+	if (tb[TCA_HHF_BACKLOG_LIMIT])
+		sch->limit = nla_get_u32(tb[TCA_HHF_BACKLOG_LIMIT]);
+
 	q->quantum = new_quantum;
 	q->hhf_non_hh_weight = new_hhf_non_hh_weight;
 
diff --git a/net/sctp/auth.c b/net/sctp/auth.c
index 683c7d1b130..0e8529113dc 100644
--- a/net/sctp/auth.c
+++ b/net/sctp/auth.c
@@ -386,14 +386,13 @@ nomem:
  */
 int sctp_auth_asoc_init_active_key(struct sctp_association *asoc, gfp_t gfp)
 {
-	struct net *net = sock_net(asoc->base.sk);
 	struct sctp_auth_bytes	*secret;
 	struct sctp_shared_key *ep_key;
 
 	/* If we don't support AUTH, or peer is not capable
 	 * we don't need to do anything.
 	 */
-	if (!net->sctp.auth_enable || !asoc->peer.auth_capable)
+	if (!asoc->ep->auth_enable || !asoc->peer.auth_capable)
 		return 0;
 
 	/* If the key_id is non-zero and we couldn't find an
@@ -440,16 +439,16 @@ struct sctp_shared_key *sctp_auth_get_shkey(
  */
 int sctp_auth_init_hmacs(struct sctp_endpoint *ep, gfp_t gfp)
 {
-	struct net *net = sock_net(ep->base.sk);
 	struct crypto_hash *tfm = NULL;
 	__u16   id;
 
-	/* if the transforms are already allocted, we are done */
-	if (!net->sctp.auth_enable) {
+	/* If AUTH extension is disabled, we are done */
+	if (!ep->auth_enable) {
 		ep->auth_hmacs = NULL;
 		return 0;
 	}
 
+	/* If the transforms are already allocated, we are done */
 	if (ep->auth_hmacs)
 		return 0;
 
@@ -665,12 +664,10 @@ static int __sctp_auth_cid(sctp_cid_t chunk, struct sctp_chunks_param *param)
 /* Check if peer requested that this chunk is authenticated */
 int sctp_auth_send_cid(sctp_cid_t chunk, const struct sctp_association *asoc)
 {
-	struct net  *net;
 	if (!asoc)
 		return 0;
 
-	net = sock_net(asoc->base.sk);
-	if (!net->sctp.auth_enable || !asoc->peer.auth_capable)
+	if (!asoc->ep->auth_enable || !asoc->peer.auth_capable)
 		return 0;
 
 	return __sctp_auth_cid(chunk, asoc->peer.peer_chunks);
@@ -679,12 +676,10 @@ int sctp_auth_send_cid(sctp_cid_t chunk, const struct sctp_association *asoc)
 /* Check if we requested that peer authenticate this chunk. */
 int sctp_auth_recv_cid(sctp_cid_t chunk, const struct sctp_association *asoc)
 {
-	struct net *net;
 	if (!asoc)
 		return 0;
 
-	net = sock_net(asoc->base.sk);
-	if (!net->sctp.auth_enable)
+	if (!asoc->ep->auth_enable)
 		return 0;
 
 	return __sctp_auth_cid(chunk,
diff --git a/net/sctp/endpointola.c b/net/sctp/endpointola.c
index 8e5fdea0521..3d9f429858d 100644
--- a/net/sctp/endpointola.c
+++ b/net/sctp/endpointola.c
@@ -68,7 +68,8 @@ static struct sctp_endpoint *sctp_endpoint_init(struct sctp_endpoint *ep,
 	if (!ep->digest)
 		return NULL;
 
-	if (net->sctp.auth_enable) {
+	ep->auth_enable = net->sctp.auth_enable;
+	if (ep->auth_enable) {
 		/* Allocate space for HMACS and CHUNKS authentication
 		 * variables.  There are arrays that we encode directly
 		 * into parameters to make the rest of the operations easier.
diff --git a/net/sctp/protocol.c b/net/sctp/protocol.c
index c09757fbf80..44cbb54c857 100644
--- a/net/sctp/protocol.c
+++ b/net/sctp/protocol.c
@@ -491,8 +491,13 @@ static void sctp_v4_get_dst(struct sctp_transport *t, union sctp_addr *saddr,
 			continue;
 		if ((laddr->state == SCTP_ADDR_SRC) &&
 		    (AF_INET == laddr->a.sa.sa_family)) {
-			fl4->saddr = laddr->a.v4.sin_addr.s_addr;
 			fl4->fl4_sport = laddr->a.v4.sin_port;
+			flowi4_update_output(fl4,
+					     asoc->base.sk->sk_bound_dev_if,
+					     RT_CONN_FLAGS(asoc->base.sk),
+					     daddr->v4.sin_addr.s_addr,
+					     laddr->a.v4.sin_addr.s_addr);
+
 			rt = ip_route_output_key(sock_net(sk), fl4);
 			if (!IS_ERR(rt)) {
 				dst = &rt->dst;
diff --git a/net/sctp/sm_make_chunk.c b/net/sctp/sm_make_chunk.c
index 3a1767ef320..fee5552ddf9 100644
--- a/net/sctp/sm_make_chunk.c
+++ b/net/sctp/sm_make_chunk.c
@@ -219,6 +219,7 @@ struct sctp_chunk *sctp_make_init(const struct sctp_association *asoc,
 			     gfp_t gfp, int vparam_len)
 {
 	struct net *net = sock_net(asoc->base.sk);
+	struct sctp_endpoint *ep = asoc->ep;
 	sctp_inithdr_t init;
 	union sctp_params addrs;
 	size_t chunksize;
@@ -278,7 +279,7 @@ struct sctp_chunk *sctp_make_init(const struct sctp_association *asoc,
 	chunksize += vparam_len;
 
 	/* Account for AUTH related parameters */
-	if (net->sctp.auth_enable) {
+	if (ep->auth_enable) {
 		/* Add random parameter length*/
 		chunksize += sizeof(asoc->c.auth_random);
 
@@ -363,7 +364,7 @@ struct sctp_chunk *sctp_make_init(const struct sctp_association *asoc,
 	}
 
 	/* Add SCTP-AUTH chunks to the parameter list */
-	if (net->sctp.auth_enable) {
+	if (ep->auth_enable) {
 		sctp_addto_chunk(retval, sizeof(asoc->c.auth_random),
 				 asoc->c.auth_random);
 		if (auth_hmacs)
@@ -2010,7 +2011,7 @@ static void sctp_process_ext_param(struct sctp_association *asoc,
 			/* if the peer reports AUTH, assume that he
 			 * supports AUTH.
 			 */
-			if (net->sctp.auth_enable)
+			if (asoc->ep->auth_enable)
 				asoc->peer.auth_capable = 1;
 			break;
 		case SCTP_CID_ASCONF:
@@ -2102,6 +2103,7 @@ static sctp_ierror_t sctp_process_unk_param(const struct sctp_association *asoc,
  * 	SCTP_IERROR_NO_ERROR - continue with the chunk
  */
 static sctp_ierror_t sctp_verify_param(struct net *net,
+					const struct sctp_endpoint *ep,
 					const struct sctp_association *asoc,
 					union sctp_params param,
 					sctp_cid_t cid,
@@ -2152,7 +2154,7 @@ static sctp_ierror_t sctp_verify_param(struct net *net,
 		goto fallthrough;
 
 	case SCTP_PARAM_RANDOM:
-		if (!net->sctp.auth_enable)
+		if (!ep->auth_enable)
 			goto fallthrough;
 
 		/* SCTP-AUTH: Secion 6.1
@@ -2169,7 +2171,7 @@ static sctp_ierror_t sctp_verify_param(struct net *net,
 		break;
 
 	case SCTP_PARAM_CHUNKS:
-		if (!net->sctp.auth_enable)
+		if (!ep->auth_enable)
 			goto fallthrough;
 
 		/* SCTP-AUTH: Section 3.2
@@ -2185,7 +2187,7 @@ static sctp_ierror_t sctp_verify_param(struct net *net,
 		break;
 
 	case SCTP_PARAM_HMAC_ALGO:
-		if (!net->sctp.auth_enable)
+		if (!ep->auth_enable)
 			goto fallthrough;
 
 		hmacs = (struct sctp_hmac_algo_param *)param.p;
@@ -2220,10 +2222,9 @@ fallthrough:
 }
 
 /* Verify the INIT packet before we process it.  */
-int sctp_verify_init(struct net *net, const struct sctp_association *asoc,
-		     sctp_cid_t cid,
-		     sctp_init_chunk_t *peer_init,
-		     struct sctp_chunk *chunk,
+int sctp_verify_init(struct net *net, const struct sctp_endpoint *ep,
+		     const struct sctp_association *asoc, sctp_cid_t cid,
+		     sctp_init_chunk_t *peer_init, struct sctp_chunk *chunk,
 		     struct sctp_chunk **errp)
 {
 	union sctp_params param;
@@ -2264,8 +2265,8 @@ int sctp_verify_init(struct net *net, const struct sctp_association *asoc,
 
 	/* Verify all the variable length parameters */
 	sctp_walk_params(param, peer_init, init_hdr.params) {
-
-		result = sctp_verify_param(net, asoc, param, cid, chunk, errp);
+		result = sctp_verify_param(net, ep, asoc, param, cid,
+					   chunk, errp);
 		switch (result) {
 		case SCTP_IERROR_ABORT:
 		case SCTP_IERROR_NOMEM:
@@ -2497,6 +2498,7 @@ static int sctp_process_param(struct sctp_association *asoc,
 	struct sctp_af *af;
 	union sctp_addr_param *addr_param;
 	struct sctp_transport *t;
+	struct sctp_endpoint *ep = asoc->ep;
 
 	/* We maintain all INIT parameters in network byte order all the
 	 * time.  This allows us to not worry about whether the parameters
@@ -2636,7 +2638,7 @@ do_addr_param:
 		goto fall_through;
 
 	case SCTP_PARAM_RANDOM:
-		if (!net->sctp.auth_enable)
+		if (!ep->auth_enable)
 			goto fall_through;
 
 		/* Save peer's random parameter */
@@ -2649,7 +2651,7 @@ do_addr_param:
 		break;
 
 	case SCTP_PARAM_HMAC_ALGO:
-		if (!net->sctp.auth_enable)
+		if (!ep->auth_enable)
 			goto fall_through;
 
 		/* Save peer's HMAC list */
@@ -2665,7 +2667,7 @@ do_addr_param:
 		break;
 
 	case SCTP_PARAM_CHUNKS:
-		if (!net->sctp.auth_enable)
+		if (!ep->auth_enable)
 			goto fall_through;
 
 		asoc->peer.peer_chunks = kmemdup(param.p,
diff --git a/net/sctp/sm_sideeffect.c b/net/sctp/sm_sideeffect.c
index 5d6883ff00c..fef2acdf4a2 100644
--- a/net/sctp/sm_sideeffect.c
+++ b/net/sctp/sm_sideeffect.c
@@ -496,11 +496,10 @@ static void sctp_do_8_2_transport_strike(sctp_cmd_seq_t *commands,
 
 	/* If the transport error count is greater than the pf_retrans
 	 * threshold, and less than pathmaxrtx, and if the current state
-	 * is not SCTP_UNCONFIRMED, then mark this transport as Partially
-	 * Failed, see SCTP Quick Failover Draft, section 5.1
+	 * is SCTP_ACTIVE, then mark this transport as Partially Failed,
+	 * see SCTP Quick Failover Draft, section 5.1
 	 */
-	if ((transport->state != SCTP_PF) &&
-	   (transport->state != SCTP_UNCONFIRMED) &&
+	if ((transport->state == SCTP_ACTIVE) &&
 	   (asoc->pf_retrans < transport->pathmaxrxt) &&
 	   (transport->error_count > asoc->pf_retrans)) {
 
diff --git a/net/sctp/sm_statefuns.c b/net/sctp/sm_statefuns.c
index ae9fbeba40b..5170a1ff95a 100644
--- a/net/sctp/sm_statefuns.c
+++ b/net/sctp/sm_statefuns.c
@@ -357,7 +357,7 @@ sctp_disposition_t sctp_sf_do_5_1B_init(struct net *net,
 
 	/* Verify the INIT chunk before processing it. */
 	err_chunk = NULL;
-	if (!sctp_verify_init(net, asoc, chunk->chunk_hdr->type,
+	if (!sctp_verify_init(net, ep, asoc, chunk->chunk_hdr->type,
 			      (sctp_init_chunk_t *)chunk->chunk_hdr, chunk,
 			      &err_chunk)) {
 		/* This chunk contains fatal error. It is to be discarded.
@@ -524,7 +524,7 @@ sctp_disposition_t sctp_sf_do_5_1C_ack(struct net *net,
 
 	/* Verify the INIT chunk before processing it. */
 	err_chunk = NULL;
-	if (!sctp_verify_init(net, asoc, chunk->chunk_hdr->type,
+	if (!sctp_verify_init(net, ep, asoc, chunk->chunk_hdr->type,
 			      (sctp_init_chunk_t *)chunk->chunk_hdr, chunk,
 			      &err_chunk)) {
 
@@ -1430,7 +1430,7 @@ static sctp_disposition_t sctp_sf_do_unexpected_init(
 
 	/* Verify the INIT chunk before processing it. */
 	err_chunk = NULL;
-	if (!sctp_verify_init(net, asoc, chunk->chunk_hdr->type,
+	if (!sctp_verify_init(net, ep, asoc, chunk->chunk_hdr->type,
 			      (sctp_init_chunk_t *)chunk->chunk_hdr, chunk,
 			      &err_chunk)) {
 		/* This chunk contains fatal error. It is to be discarded.
diff --git a/net/sctp/socket.c b/net/sctp/socket.c
index ff20e2dbbbc..fee06b99a4d 100644
--- a/net/sctp/socket.c
+++ b/net/sctp/socket.c
@@ -3321,10 +3321,10 @@ static int sctp_setsockopt_auth_chunk(struct sock *sk,
 				      char __user *optval,
 				      unsigned int optlen)
 {
-	struct net *net = sock_net(sk);
+	struct sctp_endpoint *ep = sctp_sk(sk)->ep;
 	struct sctp_authchunk val;
 
-	if (!net->sctp.auth_enable)
+	if (!ep->auth_enable)
 		return -EACCES;
 
 	if (optlen != sizeof(struct sctp_authchunk))
@@ -3341,7 +3341,7 @@ static int sctp_setsockopt_auth_chunk(struct sock *sk,
 	}
 
 	/* add this chunk id to the endpoint */
-	return sctp_auth_ep_add_chunkid(sctp_sk(sk)->ep, val.sauth_chunk);
+	return sctp_auth_ep_add_chunkid(ep, val.sauth_chunk);
 }
 
 /*
@@ -3354,12 +3354,12 @@ static int sctp_setsockopt_hmac_ident(struct sock *sk,
 				      char __user *optval,
 				      unsigned int optlen)
 {
-	struct net *net = sock_net(sk);
+	struct sctp_endpoint *ep = sctp_sk(sk)->ep;
 	struct sctp_hmacalgo *hmacs;
 	u32 idents;
 	int err;
 
-	if (!net->sctp.auth_enable)
+	if (!ep->auth_enable)
 		return -EACCES;
 
 	if (optlen < sizeof(struct sctp_hmacalgo))
@@ -3376,7 +3376,7 @@ static int sctp_setsockopt_hmac_ident(struct sock *sk,
 		goto out;
 	}
 
-	err = sctp_auth_ep_set_hmacs(sctp_sk(sk)->ep, hmacs);
+	err = sctp_auth_ep_set_hmacs(ep, hmacs);
 out:
 	kfree(hmacs);
 	return err;
@@ -3392,12 +3392,12 @@ static int sctp_setsockopt_auth_key(struct sock *sk,
 				    char __user *optval,
 				    unsigned int optlen)
 {
-	struct net *net = sock_net(sk);
+	struct sctp_endpoint *ep = sctp_sk(sk)->ep;
 	struct sctp_authkey *authkey;
 	struct sctp_association *asoc;
 	int ret;
 
-	if (!net->sctp.auth_enable)
+	if (!ep->auth_enable)
 		return -EACCES;
 
 	if (optlen <= sizeof(struct sctp_authkey))
@@ -3418,7 +3418,7 @@ static int sctp_setsockopt_auth_key(struct sock *sk,
 		goto out;
 	}
 
-	ret = sctp_auth_set_key(sctp_sk(sk)->ep, asoc, authkey);
+	ret = sctp_auth_set_key(ep, asoc, authkey);
 out:
 	kzfree(authkey);
 	return ret;
@@ -3434,11 +3434,11 @@ static int sctp_setsockopt_active_key(struct sock *sk,
 				      char __user *optval,
 				      unsigned int optlen)
 {
-	struct net *net = sock_net(sk);
+	struct sctp_endpoint *ep = sctp_sk(sk)->ep;
 	struct sctp_authkeyid val;
 	struct sctp_association *asoc;
 
-	if (!net->sctp.auth_enable)
+	if (!ep->auth_enable)
 		return -EACCES;
 
 	if (optlen != sizeof(struct sctp_authkeyid))
@@ -3450,8 +3450,7 @@ static int sctp_setsockopt_active_key(struct sock *sk,
 	if (!asoc && val.scact_assoc_id && sctp_style(sk, UDP))
 		return -EINVAL;
 
-	return sctp_auth_set_active_key(sctp_sk(sk)->ep, asoc,
-					val.scact_keynumber);
+	return sctp_auth_set_active_key(ep, asoc, val.scact_keynumber);
 }
 
 /*
@@ -3463,11 +3462,11 @@ static int sctp_setsockopt_del_key(struct sock *sk,
 				   char __user *optval,
 				   unsigned int optlen)
 {
-	struct net *net = sock_net(sk);
+	struct sctp_endpoint *ep = sctp_sk(sk)->ep;
 	struct sctp_authkeyid val;
 	struct sctp_association *asoc;
 
-	if (!net->sctp.auth_enable)
+	if (!ep->auth_enable)
 		return -EACCES;
 
 	if (optlen != sizeof(struct sctp_authkeyid))
@@ -3479,8 +3478,7 @@ static int sctp_setsockopt_del_key(struct sock *sk,
 	if (!asoc && val.scact_assoc_id && sctp_style(sk, UDP))
 		return -EINVAL;
 
-	return sctp_auth_del_key_id(sctp_sk(sk)->ep, asoc,
-				    val.scact_keynumber);
+	return sctp_auth_del_key_id(ep, asoc, val.scact_keynumber);
 
 }
 
@@ -5387,16 +5385,16 @@ static int sctp_getsockopt_maxburst(struct sock *sk, int len,
 static int sctp_getsockopt_hmac_ident(struct sock *sk, int len,
 				    char __user *optval, int __user *optlen)
 {
-	struct net *net = sock_net(sk);
+	struct sctp_endpoint *ep = sctp_sk(sk)->ep;
 	struct sctp_hmacalgo  __user *p = (void __user *)optval;
 	struct sctp_hmac_algo_param *hmacs;
 	__u16 data_len = 0;
 	u32 num_idents;
 
-	if (!net->sctp.auth_enable)
+	if (!ep->auth_enable)
 		return -EACCES;
 
-	hmacs = sctp_sk(sk)->ep->auth_hmacs_list;
+	hmacs = ep->auth_hmacs_list;
 	data_len = ntohs(hmacs->param_hdr.length) - sizeof(sctp_paramhdr_t);
 
 	if (len < sizeof(struct sctp_hmacalgo) + data_len)
@@ -5417,11 +5415,11 @@ static int sctp_getsockopt_hmac_ident(struct sock *sk, int len,
 static int sctp_getsockopt_active_key(struct sock *sk, int len,
 				    char __user *optval, int __user *optlen)
 {
-	struct net *net = sock_net(sk);
+	struct sctp_endpoint *ep = sctp_sk(sk)->ep;
 	struct sctp_authkeyid val;
 	struct sctp_association *asoc;
 
-	if (!net->sctp.auth_enable)
+	if (!ep->auth_enable)
 		return -EACCES;
 
 	if (len < sizeof(struct sctp_authkeyid))
@@ -5436,7 +5434,7 @@ static int sctp_getsockopt_active_key(struct sock *sk, int len,
 	if (asoc)
 		val.scact_keynumber = asoc->active_key_id;
 	else
-		val.scact_keynumber = sctp_sk(sk)->ep->active_key_id;
+		val.scact_keynumber = ep->active_key_id;
 
 	len = sizeof(struct sctp_authkeyid);
 	if (put_user(len, optlen))
@@ -5450,7 +5448,7 @@ static int sctp_getsockopt_active_key(struct sock *sk, int len,
 static int sctp_getsockopt_peer_auth_chunks(struct sock *sk, int len,
 				    char __user *optval, int __user *optlen)
 {
-	struct net *net = sock_net(sk);
+	struct sctp_endpoint *ep = sctp_sk(sk)->ep;
 	struct sctp_authchunks __user *p = (void __user *)optval;
 	struct sctp_authchunks val;
 	struct sctp_association *asoc;
@@ -5458,7 +5456,7 @@ static int sctp_getsockopt_peer_auth_chunks(struct sock *sk, int len,
 	u32    num_chunks = 0;
 	char __user *to;
 
-	if (!net->sctp.auth_enable)
+	if (!ep->auth_enable)
 		return -EACCES;
 
 	if (len < sizeof(struct sctp_authchunks))
@@ -5495,7 +5493,7 @@ num:
 static int sctp_getsockopt_local_auth_chunks(struct sock *sk, int len,
 				    char __user *optval, int __user *optlen)
 {
-	struct net *net = sock_net(sk);
+	struct sctp_endpoint *ep = sctp_sk(sk)->ep;
 	struct sctp_authchunks __user *p = (void __user *)optval;
 	struct sctp_authchunks val;
 	struct sctp_association *asoc;
@@ -5503,7 +5501,7 @@ static int sctp_getsockopt_local_auth_chunks(struct sock *sk, int len,
 	u32    num_chunks = 0;
 	char __user *to;
 
-	if (!net->sctp.auth_enable)
+	if (!ep->auth_enable)
 		return -EACCES;
 
 	if (len < sizeof(struct sctp_authchunks))
@@ -5520,7 +5518,7 @@ static int sctp_getsockopt_local_auth_chunks(struct sock *sk, int len,
 	if (asoc)
 		ch = (struct sctp_chunks_param *)asoc->c.auth_chunks;
 	else
-		ch = sctp_sk(sk)->ep->auth_chunk_list;
+		ch = ep->auth_chunk_list;
 
 	if (!ch)
 		goto num;
diff --git a/net/sctp/sysctl.c b/net/sctp/sysctl.c
index 35c8923b555..c82fdc1eab7 100644
--- a/net/sctp/sysctl.c
+++ b/net/sctp/sysctl.c
@@ -64,6 +64,9 @@ static int proc_sctp_do_rto_min(struct ctl_table *ctl, int write,
 static int proc_sctp_do_rto_max(struct ctl_table *ctl, int write,
 				void __user *buffer, size_t *lenp,
 				loff_t *ppos);
+static int proc_sctp_do_auth(struct ctl_table *ctl, int write,
+			     void __user *buffer, size_t *lenp,
+			     loff_t *ppos);
 
 static struct ctl_table sctp_table[] = {
 	{
@@ -266,7 +269,7 @@ static struct ctl_table sctp_net_table[] = {
 		.data		= &init_net.sctp.auth_enable,
 		.maxlen		= sizeof(int),
 		.mode		= 0644,
-		.proc_handler	= proc_dointvec,
+		.proc_handler	= proc_sctp_do_auth,
 	},
 	{
 		.procname	= "addr_scope_policy",
@@ -400,6 +403,37 @@ static int proc_sctp_do_rto_max(struct ctl_table *ctl, int write,
 	return ret;
 }
 
+static int proc_sctp_do_auth(struct ctl_table *ctl, int write,
+			     void __user *buffer, size_t *lenp,
+			     loff_t *ppos)
+{
+	struct net *net = current->nsproxy->net_ns;
+	struct ctl_table tbl;
+	int new_value, ret;
+
+	memset(&tbl, 0, sizeof(struct ctl_table));
+	tbl.maxlen = sizeof(unsigned int);
+
+	if (write)
+		tbl.data = &new_value;
+	else
+		tbl.data = &net->sctp.auth_enable;
+
+	ret = proc_dointvec(&tbl, write, buffer, lenp, ppos);
+
+	if (write) {
+		struct sock *sk = net->sctp.ctl_sock;
+
+		net->sctp.auth_enable = new_value;
+		/* Update the value in the control socket */
+		lock_sock(sk);
+		sctp_sk(sk)->ep->auth_enable = new_value;
+		release_sock(sk);
+	}
+
+	return ret;
+}
+
 int sctp_sysctl_net_register(struct net *net)
 {
 	struct ctl_table *table = sctp_net_table;
diff --git a/net/socket.c b/net/socket.c
index 1b1e7e6a960..abf56b2a14f 100644
--- a/net/socket.c
+++ b/net/socket.c
@@ -1880,8 +1880,8 @@ out:
  *	Receive a datagram from a socket.
  */
 
-asmlinkage long sys_recv(int fd, void __user *ubuf, size_t size,
-			 unsigned int flags)
+SYSCALL_DEFINE4(recv, int, fd, void __user *, ubuf, size_t, size,
+		unsigned int, flags)
 {
 	return sys_recvfrom(fd, ubuf, size, flags, NULL, NULL);
 }
diff --git a/net/tipc/netlink.c b/net/tipc/netlink.c
index 3aaf73de9e2..ad844d36534 100644
--- a/net/tipc/netlink.c
+++ b/net/tipc/netlink.c
@@ -47,7 +47,7 @@ static int handle_cmd(struct sk_buff *skb, struct genl_info *info)
 	int hdr_space = nlmsg_total_size(GENL_HDRLEN + TIPC_GENL_HDRLEN);
 	u16 cmd;
 
-	if ((req_userhdr->cmd & 0xC000) && (!capable(CAP_NET_ADMIN)))
+	if ((req_userhdr->cmd & 0xC000) && (!netlink_capable(skb, CAP_NET_ADMIN)))
 		cmd = TIPC_CMD_NOT_NET_ADMIN;
 	else
 		cmd = req_userhdr->cmd;
diff --git a/net/vmw_vsock/af_vsock.c b/net/vmw_vsock/af_vsock.c
index 5adfd94c5b8..85d232bed87 100644
--- a/net/vmw_vsock/af_vsock.c
+++ b/net/vmw_vsock/af_vsock.c
@@ -1925,9 +1925,23 @@ static struct miscdevice vsock_device = {
 	.fops		= &vsock_device_ops,
 };
 
-static int __vsock_core_init(void)
+int __vsock_core_init(const struct vsock_transport *t, struct module *owner)
 {
-	int err;
+	int err = mutex_lock_interruptible(&vsock_register_mutex);
+
+	if (err)
+		return err;
+
+	if (transport) {
+		err = -EBUSY;
+		goto err_busy;
+	}
+
+	/* Transport must be the owner of the protocol so that it can't
+	 * unload while there are open sockets.
+	 */
+	vsock_proto.owner = owner;
+	transport = t;
 
 	vsock_init_tables();
 
@@ -1951,36 +1965,19 @@ static int __vsock_core_init(void)
 		goto err_unregister_proto;
 	}
 
+	mutex_unlock(&vsock_register_mutex);
 	return 0;
 
 err_unregister_proto:
 	proto_unregister(&vsock_proto);
 err_misc_deregister:
 	misc_deregister(&vsock_device);
-	return err;
-}
-
-int vsock_core_init(const struct vsock_transport *t)
-{
-	int retval = mutex_lock_interruptible(&vsock_register_mutex);
-	if (retval)
-		return retval;
-
-	if (transport) {
-		retval = -EBUSY;
-		goto out;
-	}
-
-	transport = t;
-	retval = __vsock_core_init();
-	if (retval)
-		transport = NULL;
-
-out:
+	transport = NULL;
+err_busy:
 	mutex_unlock(&vsock_register_mutex);
-	return retval;
+	return err;
 }
-EXPORT_SYMBOL_GPL(vsock_core_init);
+EXPORT_SYMBOL_GPL(__vsock_core_init);
 
 void vsock_core_exit(void)
 {
@@ -2000,5 +1997,5 @@ EXPORT_SYMBOL_GPL(vsock_core_exit);
 
 MODULE_AUTHOR("VMware, Inc.");
 MODULE_DESCRIPTION("VMware Virtual Socket Family");
-MODULE_VERSION("1.0.0.0-k");
+MODULE_VERSION("1.0.1.0-k");
 MODULE_LICENSE("GPL v2");
diff --git a/net/wireless/scan.c b/net/wireless/scan.c
index 7d09a712cb1..88f108edfb5 100644
--- a/net/wireless/scan.c
+++ b/net/wireless/scan.c
@@ -284,14 +284,22 @@ void cfg80211_sched_scan_results(struct wiphy *wiphy)
 }
 EXPORT_SYMBOL(cfg80211_sched_scan_results);
 
-void cfg80211_sched_scan_stopped(struct wiphy *wiphy)
+void cfg80211_sched_scan_stopped_rtnl(struct wiphy *wiphy)
 {
 	struct cfg80211_registered_device *rdev = wiphy_to_dev(wiphy);
 
+	ASSERT_RTNL();
+
 	trace_cfg80211_sched_scan_stopped(wiphy);
 
-	rtnl_lock();
 	__cfg80211_stop_sched_scan(rdev, true);
+}
+EXPORT_SYMBOL(cfg80211_sched_scan_stopped_rtnl);
+
+void cfg80211_sched_scan_stopped(struct wiphy *wiphy)
+{
+	rtnl_lock();
+	cfg80211_sched_scan_stopped_rtnl(wiphy);
 	rtnl_unlock();
 }
 EXPORT_SYMBOL(cfg80211_sched_scan_stopped);
diff --git a/net/wireless/sme.c b/net/wireless/sme.c
index acdcb4a8181..3546a77033d 100644
--- a/net/wireless/sme.c
+++ b/net/wireless/sme.c
@@ -234,7 +234,6 @@ void cfg80211_conn_work(struct work_struct *work)
 					NULL, 0, NULL, 0,
 					WLAN_STATUS_UNSPECIFIED_FAILURE,
 					false, NULL);
-			cfg80211_sme_free(wdev);
 		}
 		wdev_unlock(wdev);
 	}
@@ -648,6 +647,7 @@ void __cfg80211_connect_result(struct net_device *dev, const u8 *bssid,
 			cfg80211_unhold_bss(bss_from_pub(bss));
 			cfg80211_put_bss(wdev->wiphy, bss);
 		}
+		cfg80211_sme_free(wdev);
 		return;
 	}
 
diff --git a/net/xfrm/xfrm_user.c b/net/xfrm/xfrm_user.c
index 8f131c10a6f..51398ae6cda 100644
--- a/net/xfrm/xfrm_user.c
+++ b/net/xfrm/xfrm_user.c
@@ -2377,7 +2377,7 @@ static int xfrm_user_rcv_msg(struct sk_buff *skb, struct nlmsghdr *nlh)
 	link = &xfrm_dispatch[type];
 
 	/* All operations require privileges, even GET */
-	if (!ns_capable(net->user_ns, CAP_NET_ADMIN))
+	if (!netlink_net_capable(skb, CAP_NET_ADMIN))
 		return -EPERM;
 
 	if ((type == (XFRM_MSG_GETSA - XFRM_MSG_BASE) ||
diff --git a/scripts/checksyscalls.sh b/scripts/checksyscalls.sh
index fd8fa9aa7c4..5b3add31f9f 100755
--- a/scripts/checksyscalls.sh
+++ b/scripts/checksyscalls.sh
@@ -25,7 +25,7 @@ cat << EOF
 #define __IGNORE_rmdir		/* unlinkat */
 #define __IGNORE_lchown		/* fchownat */
 #define __IGNORE_access		/* faccessat */
-#define __IGNORE_rename		/* renameat */
+#define __IGNORE_rename		/* renameat2 */
 #define __IGNORE_readlink	/* readlinkat */
 #define __IGNORE_symlink	/* symlinkat */
 #define __IGNORE_utimes		/* futimesat */
@@ -37,6 +37,9 @@ cat << EOF
 #define __IGNORE_lstat64	/* fstatat64 */
 #endif
 
+/* Missing flags argument */
+#define __IGNORE_renameat	/* renameat2 */
+
 /* CLOEXEC flag */
 #define __IGNORE_pipe		/* pipe2 */
 #define __IGNORE_dup2		/* dup3 */
diff --git a/scripts/sortextable.c b/scripts/sortextable.c
index cc49062acde..1052d4834a4 100644
--- a/scripts/sortextable.c
+++ b/scripts/sortextable.c
@@ -35,6 +35,10 @@
 #define EM_ARCOMPACT	93
 #endif
 
+#ifndef EM_XTENSA
+#define EM_XTENSA	94
+#endif
+
 #ifndef EM_AARCH64
 #define EM_AARCH64	183
 #endif
@@ -281,6 +285,7 @@ do_file(char const *const fname)
 	case EM_AARCH64:
 	case EM_MICROBLAZE:
 	case EM_MIPS:
+	case EM_XTENSA:
 		break;
 	}  /* end switch */
 
diff --git a/security/apparmor/include/apparmor.h b/security/apparmor/include/apparmor.h
index 8fb1488a3cd..97130f88838 100644
--- a/security/apparmor/include/apparmor.h
+++ b/security/apparmor/include/apparmor.h
@@ -66,7 +66,6 @@ extern int apparmor_initialized __initdata;
 char *aa_split_fqname(char *args, char **ns_name);
 void aa_info_message(const char *str);
 void *__aa_kvmalloc(size_t size, gfp_t flags);
-void kvfree(void *buffer);
 
 static inline void *kvmalloc(size_t size)
 {
diff --git a/security/apparmor/lib.c b/security/apparmor/lib.c
index 69689922c49..c1827e06845 100644
--- a/security/apparmor/lib.c
+++ b/security/apparmor/lib.c
@@ -104,17 +104,3 @@ void *__aa_kvmalloc(size_t size, gfp_t flags)
 	}
 	return buffer;
 }
-
-/**
- * kvfree - free an allocation do by kvmalloc
- * @buffer: buffer to free (MAYBE_NULL)
- *
- * Free a buffer allocated by kvmalloc
- */
-void kvfree(void *buffer)
-{
-	if (is_vmalloc_addr(buffer))
-		vfree(buffer);
-	else
-		kfree(buffer);
-}
diff --git a/security/device_cgroup.c b/security/device_cgroup.c
index 8365909f5f8..9134dbf70d3 100644
--- a/security/device_cgroup.c
+++ b/security/device_cgroup.c
@@ -306,57 +306,138 @@ static int devcgroup_seq_show(struct seq_file *m, void *v)
 }
 
 /**
- * may_access - verifies if a new exception is part of what is allowed
- *		by a dev cgroup based on the default policy +
- *		exceptions. This is used to make sure a child cgroup
- *		won't have more privileges than its parent or to
- *		verify if a certain access is allowed.
- * @dev_cgroup: dev cgroup to be tested against
- * @refex: new exception
- * @behavior: behavior of the exception
+ * match_exception	- iterates the exception list trying to find a complete match
+ * @exceptions: list of exceptions
+ * @type: device type (DEV_BLOCK or DEV_CHAR)
+ * @major: device file major number, ~0 to match all
+ * @minor: device file minor number, ~0 to match all
+ * @access: permission mask (ACC_READ, ACC_WRITE, ACC_MKNOD)
+ *
+ * It is considered a complete match if an exception is found that will
+ * contain the entire range of provided parameters.
+ *
+ * Return: true in case it matches an exception completely
  */
-static bool may_access(struct dev_cgroup *dev_cgroup,
-		       struct dev_exception_item *refex,
-		       enum devcg_behavior behavior)
+static bool match_exception(struct list_head *exceptions, short type,
+			    u32 major, u32 minor, short access)
 {
 	struct dev_exception_item *ex;
-	bool match = false;
 
-	rcu_lockdep_assert(rcu_read_lock_held() ||
-			   lockdep_is_held(&devcgroup_mutex),
-			   "device_cgroup::may_access() called without proper synchronization");
+	list_for_each_entry_rcu(ex, exceptions, list) {
+		if ((type & DEV_BLOCK) && !(ex->type & DEV_BLOCK))
+			continue;
+		if ((type & DEV_CHAR) && !(ex->type & DEV_CHAR))
+			continue;
+		if (ex->major != ~0 && ex->major != major)
+			continue;
+		if (ex->minor != ~0 && ex->minor != minor)
+			continue;
+		/* provided access cannot have more than the exception rule */
+		if (access & (~ex->access))
+			continue;
+		return true;
+	}
+	return false;
+}
+
+/**
+ * match_exception_partial - iterates the exception list trying to find a partial match
+ * @exceptions: list of exceptions
+ * @type: device type (DEV_BLOCK or DEV_CHAR)
+ * @major: device file major number, ~0 to match all
+ * @minor: device file minor number, ~0 to match all
+ * @access: permission mask (ACC_READ, ACC_WRITE, ACC_MKNOD)
+ *
+ * It is considered a partial match if an exception's range is found to
+ * contain *any* of the devices specified by provided parameters. This is
+ * used to make sure no extra access is being granted that is forbidden by
+ * any of the exception list.
+ *
+ * Return: true in case the provided range mat matches an exception completely
+ */
+static bool match_exception_partial(struct list_head *exceptions, short type,
+				    u32 major, u32 minor, short access)
+{
+	struct dev_exception_item *ex;
 
-	list_for_each_entry_rcu(ex, &dev_cgroup->exceptions, list) {
-		if ((refex->type & DEV_BLOCK) && !(ex->type & DEV_BLOCK))
+	list_for_each_entry_rcu(ex, exceptions, list) {
+		if ((type & DEV_BLOCK) && !(ex->type & DEV_BLOCK))
 			continue;
-		if ((refex->type & DEV_CHAR) && !(ex->type & DEV_CHAR))
+		if ((type & DEV_CHAR) && !(ex->type & DEV_CHAR))
 			continue;
-		if (ex->major != ~0 && ex->major != refex->major)
+		/*
+		 * We must be sure that both the exception and the provided
+		 * range aren't masking all devices
+		 */
+		if (ex->major != ~0 && major != ~0 && ex->major != major)
 			continue;
-		if (ex->minor != ~0 && ex->minor != refex->minor)
+		if (ex->minor != ~0 && minor != ~0 && ex->minor != minor)
 			continue;
-		if (refex->access & (~ex->access))
+		/*
+		 * In order to make sure the provided range isn't matching
+		 * an exception, all its access bits shouldn't match the
+		 * exception's access bits
+		 */
+		if (!(access & ex->access))
 			continue;
-		match = true;
-		break;
+		return true;
 	}
+	return false;
+}
+
+/**
+ * verify_new_ex - verifies if a new exception is allowed by parent cgroup's permissions
+ * @dev_cgroup: dev cgroup to be tested against
+ * @refex: new exception
+ * @behavior: behavior of the exception's dev_cgroup
+ *
+ * This is used to make sure a child cgroup won't have more privileges
+ * than its parent
+ */
+static bool verify_new_ex(struct dev_cgroup *dev_cgroup,
+		          struct dev_exception_item *refex,
+		          enum devcg_behavior behavior)
+{
+	bool match = false;
+
+	rcu_lockdep_assert(rcu_read_lock_held() ||
+			   lockdep_is_held(&devcgroup_mutex),
+			   "device_cgroup:verify_new_ex called without proper synchronization");
 
 	if (dev_cgroup->behavior == DEVCG_DEFAULT_ALLOW) {
 		if (behavior == DEVCG_DEFAULT_ALLOW) {
-			/* the exception will deny access to certain devices */
+			/*
+			 * new exception in the child doesn't matter, only
+			 * adding extra restrictions
+			 */ 
 			return true;
 		} else {
-			/* the exception will allow access to certain devices */
+			/*
+			 * new exception in the child will add more devices
+			 * that can be acessed, so it can't match any of
+			 * parent's exceptions, even slightly
+			 */ 
+			match = match_exception_partial(&dev_cgroup->exceptions,
+							refex->type,
+							refex->major,
+							refex->minor,
+							refex->access);
+
 			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 */
+		/*
+		 * Only behavior == DEVCG_DEFAULT_DENY allowed here, therefore
+		 * the new exception will add access to more devices and must
+		 * be contained completely in an parent's exception to be
+		 * allowed
+		 */
+		match = match_exception(&dev_cgroup->exceptions, refex->type,
+					refex->major, refex->minor,
+					refex->access);
+
 		if (match)
 			/* parent has an exception that matches the proposed */
 			return true;
@@ -378,7 +459,38 @@ static int parent_has_perm(struct dev_cgroup *childcg,
 
 	if (!parent)
 		return 1;
-	return may_access(parent, ex, childcg->behavior);
+	return verify_new_ex(parent, ex, childcg->behavior);
+}
+
+/**
+ * parent_allows_removal - verify if it's ok to remove an exception
+ * @childcg: child cgroup from where the exception will be removed
+ * @ex: exception being removed
+ *
+ * When removing an exception in cgroups with default ALLOW policy, it must
+ * be checked if removing it will give the child cgroup more access than the
+ * parent.
+ *
+ * Return: true if it's ok to remove exception, false otherwise
+ */
+static bool parent_allows_removal(struct dev_cgroup *childcg,
+				  struct dev_exception_item *ex)
+{
+	struct dev_cgroup *parent = css_to_devcgroup(css_parent(&childcg->css));
+
+	if (!parent)
+		return true;
+
+	/* It's always allowed to remove access to devices */
+	if (childcg->behavior == DEVCG_DEFAULT_DENY)
+		return true;
+
+	/*
+	 * Make sure you're not removing part or a whole exception existing in
+	 * the parent cgroup
+	 */
+	return !match_exception_partial(&parent->exceptions, ex->type,
+					ex->major, ex->minor, ex->access);
 }
 
 /**
@@ -616,17 +728,21 @@ static int devcgroup_update_access(struct dev_cgroup *devcgroup,
 
 	switch (filetype) {
 	case DEVCG_ALLOW:
-		if (!parent_has_perm(devcgroup, &ex))
-			return -EPERM;
 		/*
 		 * If the default policy is to allow 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_ALLOW) {
+			/* Check if the parent allows removing it first */
+			if (!parent_allows_removal(devcgroup, &ex))
+				return -EPERM;
 			dev_exception_rm(devcgroup, &ex);
-			return 0;
+			break;
 		}
+
+		if (!parent_has_perm(devcgroup, &ex))
+			return -EPERM;
 		rc = dev_exception_add(devcgroup, &ex);
 		break;
 	case DEVCG_DENY:
@@ -704,18 +820,18 @@ static int __devcgroup_check_permission(short type, u32 major, u32 minor,
 				        short access)
 {
 	struct dev_cgroup *dev_cgroup;
-	struct dev_exception_item ex;
-	int rc;
-
-	memset(&ex, 0, sizeof(ex));
-	ex.type = type;
-	ex.major = major;
-	ex.minor = minor;
-	ex.access = access;
+	bool rc;
 
 	rcu_read_lock();
 	dev_cgroup = task_devcgroup(current);
-	rc = may_access(dev_cgroup, &ex, dev_cgroup->behavior);
+	if (dev_cgroup->behavior == DEVCG_DEFAULT_ALLOW)
+		/* Can't match any of the exceptions, even partially */
+		rc = !match_exception_partial(&dev_cgroup->exceptions,
+					      type, major, minor, access);
+	else
+		/* Need to match completely one exception to be allowed */
+		rc = match_exception(&dev_cgroup->exceptions, type, major,
+				     minor, access);
 	rcu_read_unlock();
 
 	if (!rc)
diff --git a/security/selinux/hooks.c b/security/selinux/hooks.c
index b4beb77967b..2c7341dbc5d 100644
--- a/security/selinux/hooks.c
+++ b/security/selinux/hooks.c
@@ -3317,9 +3317,9 @@ static int selinux_file_fcntl(struct file *file, unsigned int cmd,
 	case F_GETLK:
 	case F_SETLK:
 	case F_SETLKW:
-	case F_GETLKP:
-	case F_SETLKP:
-	case F_SETLKPW:
+	case F_OFD_GETLK:
+	case F_OFD_SETLK:
+	case F_OFD_SETLKW:
 #if BITS_PER_LONG == 32
 	case F_GETLK64:
 	case F_SETLK64:
diff --git a/sound/core/pcm_dmaengine.c b/sound/core/pcm_dmaengine.c
index 94d08733cb3..76cbb9ec953 100644
--- a/sound/core/pcm_dmaengine.c
+++ b/sound/core/pcm_dmaengine.c
@@ -182,6 +182,7 @@ static int dmaengine_pcm_prepare_and_submit(struct snd_pcm_substream *substream)
 int snd_dmaengine_pcm_trigger(struct snd_pcm_substream *substream, int cmd)
 {
 	struct dmaengine_pcm_runtime_data *prtd = substream_to_prtd(substream);
+	struct snd_pcm_runtime *runtime = substream->runtime;
 	int ret;
 
 	switch (cmd) {
@@ -196,6 +197,11 @@ int snd_dmaengine_pcm_trigger(struct snd_pcm_substream *substream, int cmd)
 		dmaengine_resume(prtd->dma_chan);
 		break;
 	case SNDRV_PCM_TRIGGER_SUSPEND:
+		if (runtime->info & SNDRV_PCM_INFO_PAUSE)
+			dmaengine_pause(prtd->dma_chan);
+		else
+			dmaengine_terminate_all(prtd->dma_chan);
+		break;
 	case SNDRV_PCM_TRIGGER_PAUSE_PUSH:
 		dmaengine_pause(prtd->dma_chan);
 		break;
diff --git a/sound/isa/es18xx.c b/sound/isa/es18xx.c
index 1c16830af3d..6faaac60161 100644
--- a/sound/isa/es18xx.c
+++ b/sound/isa/es18xx.c
@@ -520,7 +520,7 @@ static int snd_es18xx_playback1_trigger(struct snd_es18xx *chip,
 			snd_es18xx_mixer_write(chip, 0x78, 0x93);
 #ifdef AVOID_POPS
 		/* Avoid pops */
-                udelay(100000);
+		mdelay(100);
 		if (chip->caps & ES18XX_PCM2)
 			/* Restore Audio 2 volume */
 			snd_es18xx_mixer_write(chip, 0x7C, chip->audio2_vol);
@@ -537,7 +537,7 @@ static int snd_es18xx_playback1_trigger(struct snd_es18xx *chip,
                 /* Stop DMA */
                 snd_es18xx_mixer_write(chip, 0x78, 0x00);
 #ifdef AVOID_POPS
-                udelay(25000);
+		mdelay(25);
 		if (chip->caps & ES18XX_PCM2)
 			/* Set Audio 2 volume to 0 */
 			snd_es18xx_mixer_write(chip, 0x7C, 0);
@@ -596,7 +596,7 @@ static int snd_es18xx_capture_prepare(struct snd_pcm_substream *substream)
 	snd_es18xx_write(chip, 0xA5, count >> 8);
 
 #ifdef AVOID_POPS
-	udelay(100000);
+	mdelay(100);
 #endif
 
         /* Set format */
@@ -691,7 +691,7 @@ static int snd_es18xx_playback2_trigger(struct snd_es18xx *chip,
                 snd_es18xx_write(chip, 0xB8, 0x05);
 #ifdef AVOID_POPS
 		/* Avoid pops */
-                udelay(100000);
+		mdelay(100);
                 /* Enable Audio 1 */
                 snd_es18xx_dsp_command(chip, 0xD1);
 #endif
@@ -705,7 +705,7 @@ static int snd_es18xx_playback2_trigger(struct snd_es18xx *chip,
                 snd_es18xx_write(chip, 0xB8, 0x00);
 #ifdef AVOID_POPS
 		/* Avoid pops */
-                udelay(25000);
+		mdelay(25);
                 /* Disable Audio 1 */
                 snd_es18xx_dsp_command(chip, 0xD3);
 #endif
diff --git a/sound/isa/sb/sb_mixer.c b/sound/isa/sb/sb_mixer.c
index 6496822c180..1ff78ec9f0a 100644
--- a/sound/isa/sb/sb_mixer.c
+++ b/sound/isa/sb/sb_mixer.c
@@ -818,12 +818,14 @@ int snd_sbmixer_new(struct snd_sb *chip)
 			return err;
 		break;
 	case SB_HW_DT019X:
-		if ((err = snd_sbmixer_init(chip,
-					    snd_dt019x_controls,
-					    ARRAY_SIZE(snd_dt019x_controls),
-					    snd_dt019x_init_values,
-					    ARRAY_SIZE(snd_dt019x_init_values),
-					    "DT019X")) < 0)
+		err = snd_sbmixer_init(chip,
+				       snd_dt019x_controls,
+				       ARRAY_SIZE(snd_dt019x_controls),
+				       snd_dt019x_init_values,
+				       ARRAY_SIZE(snd_dt019x_init_values),
+				       "DT019X");
+		if (err < 0)
+			return err;
 		break;
 	default:
 		strcpy(card->mixername, "???");
diff --git a/sound/pci/hda/hda_controller.c b/sound/pci/hda/hda_controller.c
index 248b90abb88..480bbddbd80 100644
--- a/sound/pci/hda/hda_controller.c
+++ b/sound/pci/hda/hda_controller.c
@@ -1059,24 +1059,26 @@ static void azx_init_cmd_io(struct azx *chip)
 
 	/* reset the corb hw read pointer */
 	azx_writew(chip, CORBRP, ICH6_CORBRP_RST);
-	for (timeout = 1000; timeout > 0; timeout--) {
-		if ((azx_readw(chip, CORBRP) & ICH6_CORBRP_RST) == ICH6_CORBRP_RST)
-			break;
-		udelay(1);
-	}
-	if (timeout <= 0)
-		dev_err(chip->card->dev, "CORB reset timeout#1, CORBRP = %d\n",
-			azx_readw(chip, CORBRP));
+	if (!(chip->driver_caps & AZX_DCAPS_CORBRP_SELF_CLEAR)) {
+		for (timeout = 1000; timeout > 0; timeout--) {
+			if ((azx_readw(chip, CORBRP) & ICH6_CORBRP_RST) == ICH6_CORBRP_RST)
+				break;
+			udelay(1);
+		}
+		if (timeout <= 0)
+			dev_err(chip->card->dev, "CORB reset timeout#1, CORBRP = %d\n",
+				azx_readw(chip, CORBRP));
 
-	azx_writew(chip, CORBRP, 0);
-	for (timeout = 1000; timeout > 0; timeout--) {
-		if (azx_readw(chip, CORBRP) == 0)
-			break;
-		udelay(1);
+		azx_writew(chip, CORBRP, 0);
+		for (timeout = 1000; timeout > 0; timeout--) {
+			if (azx_readw(chip, CORBRP) == 0)
+				break;
+			udelay(1);
+		}
+		if (timeout <= 0)
+			dev_err(chip->card->dev, "CORB reset timeout#2, CORBRP = %d\n",
+				azx_readw(chip, CORBRP));
 	}
-	if (timeout <= 0)
-		dev_err(chip->card->dev, "CORB reset timeout#2, CORBRP = %d\n",
-			azx_readw(chip, CORBRP));
 
 	/* enable corb dma */
 	azx_writeb(chip, CORBCTL, ICH6_CORBCTL_RUN);
diff --git a/sound/pci/hda/hda_intel.c b/sound/pci/hda/hda_intel.c
index d6bca62ef38..6cc3cf28555 100644
--- a/sound/pci/hda/hda_intel.c
+++ b/sound/pci/hda/hda_intel.c
@@ -249,7 +249,8 @@ enum {
 /* quirks for Nvidia */
 #define AZX_DCAPS_PRESET_NVIDIA \
 	(AZX_DCAPS_NVIDIA_SNOOP | AZX_DCAPS_RIRB_DELAY | AZX_DCAPS_NO_MSI |\
-	 AZX_DCAPS_ALIGN_BUFSIZE | AZX_DCAPS_NO_64BIT)
+	 AZX_DCAPS_ALIGN_BUFSIZE | AZX_DCAPS_NO_64BIT |\
+	 AZX_DCAPS_CORBRP_SELF_CLEAR)
 
 #define AZX_DCAPS_PRESET_CTHDA \
 	(AZX_DCAPS_NO_MSI | AZX_DCAPS_POSFIX_LPIB | AZX_DCAPS_4K_BDLE_BOUNDARY)
@@ -1366,6 +1367,12 @@ static int azx_first_init(struct azx *chip)
 	/* initialize streams */
 	azx_init_stream(chip);
 
+	/* workaround for Broadwell HDMI: the first stream is broken,
+	 * so mask it by keeping it as if opened
+	 */
+	if (pci->vendor == 0x8086 && pci->device == 0x160c)
+		chip->azx_dev[0].opened = 1;
+
 	/* initialize chip */
 	azx_init_pci(chip);
 	azx_init_chip(chip, (probe_only[dev] & 2) == 0);
@@ -1736,6 +1743,9 @@ static DEFINE_PCI_DEVICE_TABLE(azx_ids) = {
 	/* Lynx Point */
 	{ PCI_DEVICE(0x8086, 0x8c20),
 	  .driver_data = AZX_DRIVER_PCH | AZX_DCAPS_INTEL_PCH },
+	/* 9 Series */
+	{ PCI_DEVICE(0x8086, 0x8ca0),
+	  .driver_data = AZX_DRIVER_PCH | AZX_DCAPS_INTEL_PCH },
 	/* Wellsburg */
 	{ PCI_DEVICE(0x8086, 0x8d20),
 	  .driver_data = AZX_DRIVER_PCH | AZX_DCAPS_INTEL_PCH },
diff --git a/sound/pci/hda/hda_priv.h b/sound/pci/hda/hda_priv.h
index ba38b819f98..4a7cb01fa91 100644
--- a/sound/pci/hda/hda_priv.h
+++ b/sound/pci/hda/hda_priv.h
@@ -189,6 +189,7 @@ enum { SDI0, SDI1, SDI2, SDI3, SDO0, SDO1, SDO2, SDO3 };
 #define AZX_DCAPS_COUNT_LPIB_DELAY  (1 << 25)	/* Take LPIB as delay */
 #define AZX_DCAPS_PM_RUNTIME	(1 << 26)	/* runtime PM support */
 #define AZX_DCAPS_I915_POWERWELL (1 << 27)	/* HSW i915 powerwell support */
+#define AZX_DCAPS_CORBRP_SELF_CLEAR (1 << 28)	/* CORBRP clears itself after reset */
 
 /* position fix mode */
 enum {
diff --git a/sound/pci/hda/patch_hdmi.c b/sound/pci/hda/patch_hdmi.c
index 0cb5b89cd0c..b4218a19df2 100644
--- a/sound/pci/hda/patch_hdmi.c
+++ b/sound/pci/hda/patch_hdmi.c
@@ -1127,8 +1127,10 @@ static void hdmi_setup_audio_infoframe(struct hda_codec *codec,
 					    AMP_OUT_UNMUTE);
 
 	eld = &per_pin->sink_eld;
-	if (!eld->monitor_present)
+	if (!eld->monitor_present) {
+		hdmi_set_channel_count(codec, per_pin->cvt_nid, channels);
 		return;
+	}
 
 	if (!non_pcm && per_pin->chmap_set)
 		ca = hdmi_manual_channel_allocation(channels, per_pin->chmap);
@@ -3330,6 +3332,7 @@ static const struct hda_codec_preset snd_hda_preset_hdmi[] = {
 { .id = 0x10de0051, .name = "GPU 51 HDMI/DP",	.patch = patch_nvhdmi },
 { .id = 0x10de0060, .name = "GPU 60 HDMI/DP",	.patch = patch_nvhdmi },
 { .id = 0x10de0067, .name = "MCP67 HDMI",	.patch = patch_nvhdmi_2ch },
+{ .id = 0x10de0071, .name = "GPU 71 HDMI/DP",	.patch = patch_nvhdmi },
 { .id = 0x10de8001, .name = "MCP73 HDMI",	.patch = patch_nvhdmi_2ch },
 { .id = 0x11069f80, .name = "VX900 HDMI/DP",	.patch = patch_via_hdmi },
 { .id = 0x11069f81, .name = "VX900 HDMI/DP",	.patch = patch_via_hdmi },
@@ -3385,6 +3388,7 @@ MODULE_ALIAS("snd-hda-codec-id:10de0044");
 MODULE_ALIAS("snd-hda-codec-id:10de0051");
 MODULE_ALIAS("snd-hda-codec-id:10de0060");
 MODULE_ALIAS("snd-hda-codec-id:10de0067");
+MODULE_ALIAS("snd-hda-codec-id:10de0071");
 MODULE_ALIAS("snd-hda-codec-id:10de8001");
 MODULE_ALIAS("snd-hda-codec-id:11069f80");
 MODULE_ALIAS("snd-hda-codec-id:11069f81");
diff --git a/sound/pci/hda/patch_realtek.c b/sound/pci/hda/patch_realtek.c
index 14ae979a92e..49e884fb3e5 100644
--- a/sound/pci/hda/patch_realtek.c
+++ b/sound/pci/hda/patch_realtek.c
@@ -4616,11 +4616,17 @@ static const struct snd_pci_quirk alc269_fixup_tbl[] = {
 	SND_PCI_QUIRK(0x1028, 0x0653, "Dell", ALC255_FIXUP_DELL1_MIC_NO_PRESENCE),
 	SND_PCI_QUIRK(0x1028, 0x0657, "Dell", ALC255_FIXUP_DELL1_MIC_NO_PRESENCE),
 	SND_PCI_QUIRK(0x1028, 0x0658, "Dell", ALC269_FIXUP_DELL1_MIC_NO_PRESENCE),
+	SND_PCI_QUIRK(0x1028, 0x065c, "Dell", ALC255_FIXUP_DELL1_MIC_NO_PRESENCE),
 	SND_PCI_QUIRK(0x1028, 0x065f, "Dell", ALC255_FIXUP_DELL1_MIC_NO_PRESENCE),
 	SND_PCI_QUIRK(0x1028, 0x0662, "Dell", ALC255_FIXUP_DELL1_MIC_NO_PRESENCE),
 	SND_PCI_QUIRK(0x1028, 0x0667, "Dell", ALC269_FIXUP_DELL1_MIC_NO_PRESENCE),
 	SND_PCI_QUIRK(0x1028, 0x0668, "Dell", ALC255_FIXUP_DELL2_MIC_NO_PRESENCE),
 	SND_PCI_QUIRK(0x1028, 0x0669, "Dell", ALC255_FIXUP_DELL2_MIC_NO_PRESENCE),
+	SND_PCI_QUIRK(0x1028, 0x0674, "Dell", ALC255_FIXUP_DELL1_MIC_NO_PRESENCE),
+	SND_PCI_QUIRK(0x1028, 0x067e, "Dell", ALC255_FIXUP_DELL1_MIC_NO_PRESENCE),
+	SND_PCI_QUIRK(0x1028, 0x067f, "Dell", ALC255_FIXUP_DELL1_MIC_NO_PRESENCE),
+	SND_PCI_QUIRK(0x1028, 0x0680, "Dell", ALC255_FIXUP_DELL1_MIC_NO_PRESENCE),
+	SND_PCI_QUIRK(0x1028, 0x0684, "Dell", ALC269_FIXUP_DELL2_MIC_NO_PRESENCE),
 	SND_PCI_QUIRK(0x1028, 0x15cc, "Dell X5 Precision", ALC269_FIXUP_DELL2_MIC_NO_PRESENCE),
 	SND_PCI_QUIRK(0x1028, 0x15cd, "Dell X5 Precision", ALC269_FIXUP_DELL2_MIC_NO_PRESENCE),
 	SND_PCI_QUIRK(0x103c, 0x1586, "HP", ALC269_FIXUP_HP_MUTE_LED_MIC2),
@@ -4912,6 +4918,7 @@ static int patch_alc269(struct hda_codec *codec)
 		spec->codec_variant = ALC269_TYPE_ALC285;
 		break;
 	case 0x10ec0286:
+	case 0x10ec0288:
 		spec->codec_variant = ALC269_TYPE_ALC286;
 		break;
 	case 0x10ec0255:
@@ -5539,6 +5546,8 @@ static const struct snd_pci_quirk alc662_fixup_tbl[] = {
 	SND_PCI_QUIRK(0x1028, 0x0626, "Dell", ALC668_FIXUP_DELL_MIC_NO_PRESENCE),
 	SND_PCI_QUIRK(0x1028, 0x0628, "Dell", ALC668_FIXUP_AUTO_MUTE),
 	SND_PCI_QUIRK(0x1028, 0x064e, "Dell", ALC668_FIXUP_AUTO_MUTE),
+	SND_PCI_QUIRK(0x1028, 0x0696, "Dell", ALC668_FIXUP_DELL_MIC_NO_PRESENCE),
+	SND_PCI_QUIRK(0x1028, 0x0698, "Dell", ALC668_FIXUP_DELL_MIC_NO_PRESENCE),
 	SND_PCI_QUIRK(0x103c, 0x1632, "HP RP5800", ALC662_FIXUP_HP_RP5800),
 	SND_PCI_QUIRK(0x1043, 0x11cd, "Asus N550", ALC662_FIXUP_BASS_1A),
 	SND_PCI_QUIRK(0x1043, 0x1477, "ASUS N56VZ", ALC662_FIXUP_BASS_MODE4_CHMAP),
@@ -5781,6 +5790,7 @@ static const struct hda_codec_preset snd_hda_preset_realtek[] = {
 	{ .id = 0x10ec0284, .name = "ALC284", .patch = patch_alc269 },
 	{ .id = 0x10ec0285, .name = "ALC285", .patch = patch_alc269 },
 	{ .id = 0x10ec0286, .name = "ALC286", .patch = patch_alc269 },
+	{ .id = 0x10ec0288, .name = "ALC288", .patch = patch_alc269 },
 	{ .id = 0x10ec0290, .name = "ALC290", .patch = patch_alc269 },
 	{ .id = 0x10ec0292, .name = "ALC292", .patch = patch_alc269 },
 	{ .id = 0x10ec0293, .name = "ALC293", .patch = patch_alc269 },
diff --git a/sound/soc/atmel/sam9g20_wm8731.c b/sound/soc/atmel/sam9g20_wm8731.c
index 174bd546c08..bb1149126c5 100644
--- a/sound/soc/atmel/sam9g20_wm8731.c
+++ b/sound/soc/atmel/sam9g20_wm8731.c
@@ -48,7 +48,6 @@
 
 #include <asm/mach-types.h>
 #include <mach/hardware.h>
-#include <mach/gpio.h>
 
 #include "../codecs/wm8731.h"
 #include "atmel-pcm.h"
diff --git a/sound/soc/codecs/alc5623.c b/sound/soc/codecs/alc5623.c
index f500905e937..2acf82f4a08 100644
--- a/sound/soc/codecs/alc5623.c
+++ b/sound/soc/codecs/alc5623.c
@@ -1018,13 +1018,13 @@ static int alc5623_i2c_probe(struct i2c_client *client,
 		dev_err(&client->dev, "failed to read vendor ID1: %d\n", ret);
 		return ret;
 	}
-	vid1 = ((vid1 & 0xff) << 8) | (vid1 >> 8);
 
 	ret = regmap_read(alc5623->regmap, ALC5623_VENDOR_ID2, &vid2);
 	if (ret < 0) {
 		dev_err(&client->dev, "failed to read vendor ID2: %d\n", ret);
 		return ret;
 	}
+	vid2 >>= 8;
 
 	if ((vid1 != 0x10ec) || (vid2 != id->driver_data)) {
 		dev_err(&client->dev, "unknown or wrong codec\n");
diff --git a/sound/soc/codecs/cs42l52.c b/sound/soc/codecs/cs42l52.c
index 460d35547a6..2213a037c89 100644
--- a/sound/soc/codecs/cs42l52.c
+++ b/sound/soc/codecs/cs42l52.c
@@ -1229,8 +1229,10 @@ static int cs42l52_i2c_probe(struct i2c_client *i2c_client,
 	}
 
 	if (cs42l52->pdata.reset_gpio) {
-		ret = gpio_request_one(cs42l52->pdata.reset_gpio,
-				       GPIOF_OUT_INIT_HIGH, "CS42L52 /RST");
+		ret = devm_gpio_request_one(&i2c_client->dev,
+					    cs42l52->pdata.reset_gpio,
+					    GPIOF_OUT_INIT_HIGH,
+					    "CS42L52 /RST");
 		if (ret < 0) {
 			dev_err(&i2c_client->dev, "Failed to request /RST %d: %d\n",
 				cs42l52->pdata.reset_gpio, ret);
diff --git a/sound/soc/codecs/cs42l73.c b/sound/soc/codecs/cs42l73.c
index 0ee60a19a26..ae3717992d5 100644
--- a/sound/soc/codecs/cs42l73.c
+++ b/sound/soc/codecs/cs42l73.c
@@ -1443,8 +1443,10 @@ static int cs42l73_i2c_probe(struct i2c_client *i2c_client,
 	i2c_set_clientdata(i2c_client, cs42l73);
 
 	if (cs42l73->pdata.reset_gpio) {
-		ret = gpio_request_one(cs42l73->pdata.reset_gpio,
-				       GPIOF_OUT_INIT_HIGH, "CS42L73 /RST");
+		ret = devm_gpio_request_one(&i2c_client->dev,
+					    cs42l73->pdata.reset_gpio,
+					    GPIOF_OUT_INIT_HIGH,
+					    "CS42L73 /RST");
 		if (ret < 0) {
 			dev_err(&i2c_client->dev, "Failed to request /RST %d: %d\n",
 				cs42l73->pdata.reset_gpio, ret);
diff --git a/sound/soc/codecs/tlv320aic31xx.c b/sound/soc/codecs/tlv320aic31xx.c
index fa158cfe9b3..d1929de641e 100644
--- a/sound/soc/codecs/tlv320aic31xx.c
+++ b/sound/soc/codecs/tlv320aic31xx.c
@@ -376,7 +376,7 @@ static int aic31xx_dapm_power_event(struct snd_soc_dapm_widget *w,
 		reg = AIC31XX_ADCFLAG;
 		break;
 	default:
-		dev_err(w->codec->dev, "Unknown widget '%s' calling %s/n",
+		dev_err(w->codec->dev, "Unknown widget '%s' calling %s\n",
 			w->name, __func__);
 		return -EINVAL;
 	}
diff --git a/sound/soc/codecs/tlv320aic3x.c b/sound/soc/codecs/tlv320aic3x.c
index b1835103e9b..d7349bc89ad 100644
--- a/sound/soc/codecs/tlv320aic3x.c
+++ b/sound/soc/codecs/tlv320aic3x.c
@@ -1399,7 +1399,6 @@ static int aic3x_probe(struct snd_soc_codec *codec)
 	}
 
 	aic3x_add_widgets(codec);
-	list_add(&aic3x->list, &reset_list);
 
 	return 0;
 
@@ -1569,7 +1568,13 @@ static int aic3x_i2c_probe(struct i2c_client *i2c,
 
 	ret = snd_soc_register_codec(&i2c->dev,
 			&soc_codec_dev_aic3x, &aic3x_dai, 1);
-	return ret;
+
+	if (ret != 0)
+		goto err_gpio;
+
+	list_add(&aic3x->list, &reset_list);
+
+	return 0;
 
 err_gpio:
 	if (gpio_is_valid(aic3x->gpio_reset) &&
diff --git a/sound/soc/codecs/wm8962.c b/sound/soc/codecs/wm8962.c
index 5522d2566c6..ecd26dd2e44 100644
--- a/sound/soc/codecs/wm8962.c
+++ b/sound/soc/codecs/wm8962.c
@@ -154,6 +154,7 @@ static struct reg_default wm8962_reg[] = {
 	{ 40, 0x0000 },   /* R40    - SPKOUTL volume */
 	{ 41, 0x0000 },   /* R41    - SPKOUTR volume */
 
+	{ 49, 0x0010 },   /* R49    - Class D Control 1 */
 	{ 51, 0x0003 },   /* R51    - Class D Control 2 */
 
 	{ 56, 0x0506 },   /* R56    - Clocking 4 */
@@ -795,7 +796,6 @@ static bool wm8962_volatile_register(struct device *dev, unsigned int reg)
 	case WM8962_ALC2:
 	case WM8962_THERMAL_SHUTDOWN_STATUS:
 	case WM8962_ADDITIONAL_CONTROL_4:
-	case WM8962_CLASS_D_CONTROL_1:
 	case WM8962_DC_SERVO_6:
 	case WM8962_INTERRUPT_STATUS_1:
 	case WM8962_INTERRUPT_STATUS_2:
@@ -2929,13 +2929,22 @@ static int wm8962_set_fll(struct snd_soc_codec *codec, int fll_id, int source,
 static int wm8962_mute(struct snd_soc_dai *dai, int mute)
 {
 	struct snd_soc_codec *codec = dai->codec;
-	int val;
+	int val, ret;
 
 	if (mute)
-		val = WM8962_DAC_MUTE;
+		val = WM8962_DAC_MUTE | WM8962_DAC_MUTE_ALT;
 	else
 		val = 0;
 
+	/**
+	 * The DAC mute bit is mirrored in two registers, update both to keep
+	 * the register cache consistent.
+	 */
+	ret = snd_soc_update_bits(codec, WM8962_CLASS_D_CONTROL_1,
+				  WM8962_DAC_MUTE_ALT, val);
+	if (ret < 0)
+		return ret;
+
 	return snd_soc_update_bits(codec, WM8962_ADC_DAC_CONTROL_1,
 				   WM8962_DAC_MUTE, val);
 }
diff --git a/sound/soc/codecs/wm8962.h b/sound/soc/codecs/wm8962.h
index a1a5d5294c1..910aafd09d2 100644
--- a/sound/soc/codecs/wm8962.h
+++ b/sound/soc/codecs/wm8962.h
@@ -1954,6 +1954,10 @@
 #define WM8962_SPKOUTL_ENA_MASK                 0x0040  /* SPKOUTL_ENA */
 #define WM8962_SPKOUTL_ENA_SHIFT                     6  /* SPKOUTL_ENA */
 #define WM8962_SPKOUTL_ENA_WIDTH                     1  /* SPKOUTL_ENA */
+#define WM8962_DAC_MUTE_ALT                     0x0010  /* DAC_MUTE */
+#define WM8962_DAC_MUTE_ALT_MASK                0x0010  /* DAC_MUTE */
+#define WM8962_DAC_MUTE_ALT_SHIFT                    4  /* DAC_MUTE */
+#define WM8962_DAC_MUTE_ALT_WIDTH                    1  /* DAC_MUTE */
 #define WM8962_SPKOUTL_PGA_MUTE                 0x0002  /* SPKOUTL_PGA_MUTE */
 #define WM8962_SPKOUTL_PGA_MUTE_MASK            0x0002  /* SPKOUTL_PGA_MUTE */
 #define WM8962_SPKOUTL_PGA_MUTE_SHIFT                1  /* SPKOUTL_PGA_MUTE */
diff --git a/sound/soc/fsl/fsl_esai.c b/sound/soc/fsl/fsl_esai.c
index c8e5db1414d..496ce2eb2f1 100644
--- a/sound/soc/fsl/fsl_esai.c
+++ b/sound/soc/fsl/fsl_esai.c
@@ -258,10 +258,16 @@ static int fsl_esai_set_dai_sysclk(struct snd_soc_dai *dai, int clk_id,
 		return -EINVAL;
 	}
 
-	if (ratio == 1) {
+	/* Only EXTAL source can be output directly without using PSR and PM */
+	if (ratio == 1 && clksrc == esai_priv->extalclk) {
 		/* Bypass all the dividers if not being needed */
 		ecr |= tx ? ESAI_ECR_ETO : ESAI_ECR_ERO;
 		goto out;
+	} else if (ratio < 2) {
+		/* The ratio should be no less than 2 if using other sources */
+		dev_err(dai->dev, "failed to derive required HCK%c rate\n",
+				tx ? 'T' : 'R');
+		return -EINVAL;
 	}
 
 	ret = fsl_esai_divisor_cal(dai, tx, ratio, false, 0);
@@ -307,7 +313,8 @@ static int fsl_esai_set_bclk(struct snd_soc_dai *dai, bool tx, u32 freq)
 		return -EINVAL;
 	}
 
-	if (esai_priv->sck_div[tx] && (ratio > 16 || ratio == 0)) {
+	/* The ratio should be contented by FP alone if bypassing PM and PSR */
+	if (!esai_priv->sck_div[tx] && (ratio > 16 || ratio == 0)) {
 		dev_err(dai->dev, "the ratio is out of range (1 ~ 16)\n");
 		return -EINVAL;
 	}
@@ -454,12 +461,6 @@ static int fsl_esai_startup(struct snd_pcm_substream *substream,
 	}
 
 	if (!dai->active) {
-		/* Reset Port C */
-		regmap_update_bits(esai_priv->regmap, REG_ESAI_PRRC,
-				   ESAI_PRRC_PDC_MASK, ESAI_PRRC_PDC(ESAI_GPIO));
-		regmap_update_bits(esai_priv->regmap, REG_ESAI_PCRC,
-				   ESAI_PCRC_PC_MASK, ESAI_PCRC_PC(ESAI_GPIO));
-
 		/* Set synchronous mode */
 		regmap_update_bits(esai_priv->regmap, REG_ESAI_SAICR,
 				   ESAI_SAICR_SYNC, esai_priv->synchronous ?
@@ -519,6 +520,11 @@ static int fsl_esai_hw_params(struct snd_pcm_substream *substream,
 
 	regmap_update_bits(esai_priv->regmap, REG_ESAI_xCR(tx), mask, val);
 
+	/* Remove ESAI personal reset by configuring ESAI_PCRC and ESAI_PRRC */
+	regmap_update_bits(esai_priv->regmap, REG_ESAI_PRRC,
+			   ESAI_PRRC_PDC_MASK, ESAI_PRRC_PDC(ESAI_GPIO));
+	regmap_update_bits(esai_priv->regmap, REG_ESAI_PCRC,
+			   ESAI_PCRC_PC_MASK, ESAI_PCRC_PC(ESAI_GPIO));
 	return 0;
 }
 
diff --git a/sound/soc/fsl/fsl_spdif.h b/sound/soc/fsl/fsl_spdif.h
index b1266790d11..605a10b2112 100644
--- a/sound/soc/fsl/fsl_spdif.h
+++ b/sound/soc/fsl/fsl_spdif.h
@@ -144,8 +144,8 @@ enum spdif_gainsel {
 
 /* SPDIF Clock register */
 #define STC_SYSCLK_DIV_OFFSET		11
-#define STC_SYSCLK_DIV_MASK		(0x1ff << STC_TXCLK_SRC_OFFSET)
-#define STC_SYSCLK_DIV(x)		((((x) - 1) << STC_TXCLK_DIV_OFFSET) & STC_SYSCLK_DIV_MASK)
+#define STC_SYSCLK_DIV_MASK		(0x1ff << STC_SYSCLK_DIV_OFFSET)
+#define STC_SYSCLK_DIV(x)		((((x) - 1) << STC_SYSCLK_DIV_OFFSET) & STC_SYSCLK_DIV_MASK)
 #define STC_TXCLK_SRC_OFFSET		8
 #define STC_TXCLK_SRC_MASK		(0x7 << STC_TXCLK_SRC_OFFSET)
 #define STC_TXCLK_SRC_SET(x)		((x << STC_TXCLK_SRC_OFFSET) & STC_TXCLK_SRC_MASK)
diff --git a/sound/soc/fsl/imx-audmux.c b/sound/soc/fsl/imx-audmux.c
index ac869931d7f..267717aa96c 100644
--- a/sound/soc/fsl/imx-audmux.c
+++ b/sound/soc/fsl/imx-audmux.c
@@ -145,7 +145,7 @@ static const struct file_operations audmux_debugfs_fops = {
 	.llseek = default_llseek,
 };
 
-static void __init audmux_debugfs_init(void)
+static void audmux_debugfs_init(void)
 {
 	int i;
 	char buf[20];
diff --git a/sound/soc/intel/sst-acpi.c b/sound/soc/intel/sst-acpi.c
index 5d06eecb619..18aee77f8d4 100644
--- a/sound/soc/intel/sst-acpi.c
+++ b/sound/soc/intel/sst-acpi.c
@@ -138,6 +138,7 @@ static int sst_acpi_probe(struct platform_device *pdev)
 
 	sst_pdata = &sst_acpi->sst_pdata;
 	sst_pdata->id = desc->sst_id;
+	sst_pdata->dma_dev = dev;
 	sst_acpi->desc = desc;
 	sst_acpi->mach = mach;
 
diff --git a/sound/soc/intel/sst-baytrail-dsp.c b/sound/soc/intel/sst-baytrail-dsp.c
index a50bf7fc0e3..adf0aca5aca 100644
--- a/sound/soc/intel/sst-baytrail-dsp.c
+++ b/sound/soc/intel/sst-baytrail-dsp.c
@@ -324,7 +324,7 @@ static int sst_byt_init(struct sst_dsp *sst, struct sst_pdata *pdata)
 	memcpy_toio(sst->addr.lpe + SST_BYT_MAILBOX_OFFSET,
 	       &pdata->fw_base, sizeof(u32));
 
-	ret = dma_coerce_mask_and_coherent(dev, DMA_BIT_MASK(32));
+	ret = dma_coerce_mask_and_coherent(sst->dma_dev, DMA_BIT_MASK(32));
 	if (ret)
 		return ret;
 
diff --git a/sound/soc/intel/sst-baytrail-ipc.c b/sound/soc/intel/sst-baytrail-ipc.c
index d0eaeee21be..0d31dbbf480 100644
--- a/sound/soc/intel/sst-baytrail-ipc.c
+++ b/sound/soc/intel/sst-baytrail-ipc.c
@@ -542,16 +542,20 @@ struct sst_byt_stream *sst_byt_stream_new(struct sst_byt *byt, int id,
 	void *data)
 {
 	struct sst_byt_stream *stream;
+	struct sst_dsp *sst = byt->dsp;
+	unsigned long flags;
 
 	stream = kzalloc(sizeof(*stream), GFP_KERNEL);
 	if (stream == NULL)
 		return NULL;
 
+	spin_lock_irqsave(&sst->spinlock, flags);
 	list_add(&stream->node, &byt->stream_list);
 	stream->notify_position = notify_position;
 	stream->pdata = data;
 	stream->byt = byt;
 	stream->str_id = id;
+	spin_unlock_irqrestore(&sst->spinlock, flags);
 
 	return stream;
 }
@@ -630,6 +634,8 @@ int sst_byt_stream_free(struct sst_byt *byt, struct sst_byt_stream *stream)
 {
 	u64 header;
 	int ret = 0;
+	struct sst_dsp *sst = byt->dsp;
+	unsigned long flags;
 
 	if (!stream->commited)
 		goto out;
@@ -644,8 +650,10 @@ int sst_byt_stream_free(struct sst_byt *byt, struct sst_byt_stream *stream)
 
 	stream->commited = false;
 out:
+	spin_lock_irqsave(&sst->spinlock, flags);
 	list_del(&stream->node);
 	kfree(stream);
+	spin_unlock_irqrestore(&sst->spinlock, flags);
 
 	return ret;
 }
diff --git a/sound/soc/intel/sst-dsp-priv.h b/sound/soc/intel/sst-dsp-priv.h
index fe8e81aad64..40121345549 100644
--- a/sound/soc/intel/sst-dsp-priv.h
+++ b/sound/soc/intel/sst-dsp-priv.h
@@ -136,7 +136,7 @@ struct sst_module_data {
 	enum sst_data_type data_type;	/* type of module data */
 
 	u32 size;		/* size in bytes */
-	u32 offset;		/* offset in FW file */
+	int32_t offset;		/* offset in FW file */
 	u32 data_offset;	/* offset in ADSP memory space */
 	void *data;		/* module data */
 };
@@ -228,6 +228,7 @@ struct sst_dsp {
 	spinlock_t spinlock;	/* IPC locking */
 	struct mutex mutex;	/* DSP FW lock */
 	struct device *dev;
+	struct device *dma_dev;
 	void *thread_context;
 	int irq;
 	u32 id;
diff --git a/sound/soc/intel/sst-dsp.c b/sound/soc/intel/sst-dsp.c
index 0c129fd85ec..0b715b20a2d 100644
--- a/sound/soc/intel/sst-dsp.c
+++ b/sound/soc/intel/sst-dsp.c
@@ -337,6 +337,7 @@ struct sst_dsp *sst_dsp_new(struct device *dev,
 	spin_lock_init(&sst->spinlock);
 	mutex_init(&sst->mutex);
 	sst->dev = dev;
+	sst->dma_dev = pdata->dma_dev;
 	sst->thread_context = sst_dev->thread_context;
 	sst->sst_dev = sst_dev;
 	sst->id = pdata->id;
diff --git a/sound/soc/intel/sst-dsp.h b/sound/soc/intel/sst-dsp.h
index 74052b59485..e44423be66c 100644
--- a/sound/soc/intel/sst-dsp.h
+++ b/sound/soc/intel/sst-dsp.h
@@ -169,6 +169,7 @@ struct sst_pdata {
 	u32 dma_base;
 	u32 dma_size;
 	int dma_engine;
+	struct device *dma_dev;
 
 	/* DSP */
 	u32 id;
diff --git a/sound/soc/intel/sst-firmware.c b/sound/soc/intel/sst-firmware.c
index f7687107cf7..928f228c38e 100644
--- a/sound/soc/intel/sst-firmware.c
+++ b/sound/soc/intel/sst-firmware.c
@@ -57,14 +57,8 @@ struct sst_fw *sst_fw_new(struct sst_dsp *dsp,
 	sst_fw->private = private;
 	sst_fw->size = fw->size;
 
-	err = dma_coerce_mask_and_coherent(dsp->dev, DMA_BIT_MASK(32));
-	if (err < 0) {
-		kfree(sst_fw);
-		return NULL;
-	}
-
 	/* allocate DMA buffer to store FW data */
-	sst_fw->dma_buf = dma_alloc_coherent(dsp->dev, sst_fw->size,
+	sst_fw->dma_buf = dma_alloc_coherent(dsp->dma_dev, sst_fw->size,
 				&sst_fw->dmable_fw_paddr, GFP_DMA | GFP_KERNEL);
 	if (!sst_fw->dma_buf) {
 		dev_err(dsp->dev, "error: DMA alloc failed\n");
@@ -106,7 +100,7 @@ void sst_fw_free(struct sst_fw *sst_fw)
 	list_del(&sst_fw->list);
 	mutex_unlock(&dsp->mutex);
 
-	dma_free_coherent(dsp->dev, sst_fw->size, sst_fw->dma_buf,
+	dma_free_coherent(dsp->dma_dev, sst_fw->size, sst_fw->dma_buf,
 			sst_fw->dmable_fw_paddr);
 	kfree(sst_fw);
 }
@@ -202,6 +196,9 @@ static int block_alloc_contiguous(struct sst_module *module,
 		size -= block->size;
 	}
 
+	list_for_each_entry(block, &tmp, list)
+		list_add(&block->module_list, &module->block_list);
+
 	list_splice(&tmp, &dsp->used_block_list);
 	return 0;
 }
@@ -247,8 +244,7 @@ static int block_alloc(struct sst_module *module,
 		/* do we span > 1 blocks */
 		if (data->size > block->size) {
 			ret = block_alloc_contiguous(module, data,
-				block->offset + block->size,
-				data->size - block->size);
+				block->offset, data->size);
 			if (ret == 0)
 				return ret;
 		}
@@ -344,7 +340,7 @@ static int block_alloc_fixed(struct sst_module *module,
 
 			err = block_alloc_contiguous(module, data,
 				block->offset + block->size,
-				data->size - block->size + data->offset - block->offset);
+				data->size - block->size);
 			if (err < 0)
 				return -ENOMEM;
 
@@ -371,15 +367,10 @@ static int block_alloc_fixed(struct sst_module *module,
 		if (data->offset >= block->offset && data->offset < block_end) {
 
 			err = block_alloc_contiguous(module, data,
-				block->offset + block->size,
-				data->size - block->size);
+				block->offset, data->size);
 			if (err < 0)
 				return -ENOMEM;
 
-			/* add block */
-			block->data_type = data->data_type;
-			list_move(&block->list, &dsp->used_block_list);
-			list_add(&block->module_list, &module->block_list);
 			return 0;
 		}
 
diff --git a/sound/soc/intel/sst-haswell-dsp.c b/sound/soc/intel/sst-haswell-dsp.c
index f5ebf36af88..535f517629f 100644
--- a/sound/soc/intel/sst-haswell-dsp.c
+++ b/sound/soc/intel/sst-haswell-dsp.c
@@ -433,7 +433,7 @@ static int hsw_init(struct sst_dsp *sst, struct sst_pdata *pdata)
 	int ret = -ENODEV, i, j, region_count;
 	u32 offset, size;
 
-	dev = sst->dev;
+	dev = sst->dma_dev;
 
 	switch (sst->id) {
 	case SST_DEV_ID_LYNX_POINT:
@@ -466,7 +466,7 @@ static int hsw_init(struct sst_dsp *sst, struct sst_pdata *pdata)
 		return ret;
 	}
 
-	ret = dma_coerce_mask_and_coherent(dev, DMA_BIT_MASK(32));
+	ret = dma_coerce_mask_and_coherent(dev, DMA_BIT_MASK(31));
 	if (ret)
 		return ret;
 
diff --git a/sound/soc/intel/sst-haswell-ipc.c b/sound/soc/intel/sst-haswell-ipc.c
index f46bb4ddde6..e7996b39a48 100644
--- a/sound/soc/intel/sst-haswell-ipc.c
+++ b/sound/soc/intel/sst-haswell-ipc.c
@@ -617,7 +617,7 @@ static void hsw_notification_work(struct work_struct *work)
 	case IPC_POSITION_CHANGED:
 		trace_ipc_notification("DSP stream position changed for",
 			stream->reply.stream_hw_id);
-		sst_dsp_inbox_read(hsw->dsp, pos, sizeof(pos));
+		sst_dsp_inbox_read(hsw->dsp, pos, sizeof(*pos));
 
 		if (stream->notify_position)
 			stream->notify_position(stream, stream->pdata);
@@ -991,7 +991,8 @@ int sst_hsw_stream_get_volume(struct sst_hsw *hsw, struct sst_hsw_stream *stream
 		return -EINVAL;
 
 	sst_dsp_read(hsw->dsp, volume,
-		stream->reply.volume_register_address[channel], sizeof(volume));
+		stream->reply.volume_register_address[channel],
+		sizeof(*volume));
 
 	return 0;
 }
@@ -1158,11 +1159,14 @@ struct sst_hsw_stream *sst_hsw_stream_new(struct sst_hsw *hsw, int id,
 	void *data)
 {
 	struct sst_hsw_stream *stream;
+	struct sst_dsp *sst = hsw->dsp;
+	unsigned long flags;
 
 	stream = kzalloc(sizeof(*stream), GFP_KERNEL);
 	if (stream == NULL)
 		return NULL;
 
+	spin_lock_irqsave(&sst->spinlock, flags);
 	list_add(&stream->node, &hsw->stream_list);
 	stream->notify_position = notify_position;
 	stream->pdata = data;
@@ -1171,6 +1175,7 @@ struct sst_hsw_stream *sst_hsw_stream_new(struct sst_hsw *hsw, int id,
 
 	/* work to process notification messages */
 	INIT_WORK(&stream->notify_work, hsw_notification_work);
+	spin_unlock_irqrestore(&sst->spinlock, flags);
 
 	return stream;
 }
@@ -1179,6 +1184,8 @@ int sst_hsw_stream_free(struct sst_hsw *hsw, struct sst_hsw_stream *stream)
 {
 	u32 header;
 	int ret = 0;
+	struct sst_dsp *sst = hsw->dsp;
+	unsigned long flags;
 
 	/* dont free DSP streams that are not commited */
 	if (!stream->commited)
@@ -1200,8 +1207,11 @@ int sst_hsw_stream_free(struct sst_hsw *hsw, struct sst_hsw_stream *stream)
 	trace_hsw_stream_free_req(stream, &stream->free_req);
 
 out:
+	cancel_work_sync(&stream->notify_work);
+	spin_lock_irqsave(&sst->spinlock, flags);
 	list_del(&stream->node);
 	kfree(stream);
+	spin_unlock_irqrestore(&sst->spinlock, flags);
 
 	return ret;
 }
@@ -1537,10 +1547,28 @@ int sst_hsw_stream_reset(struct sst_hsw *hsw, struct sst_hsw_stream *stream)
 }
 
 /* Stream pointer positions */
-int sst_hsw_get_dsp_position(struct sst_hsw *hsw,
+u32 sst_hsw_get_dsp_position(struct sst_hsw *hsw,
 	struct sst_hsw_stream *stream)
 {
-	return stream->rpos.position;
+	u32 rpos;
+
+	sst_dsp_read(hsw->dsp, &rpos,
+		stream->reply.read_position_register_address, sizeof(rpos));
+
+	return rpos;
+}
+
+/* Stream presentation (monotonic) positions */
+u64 sst_hsw_get_dsp_presentation_position(struct sst_hsw *hsw,
+	struct sst_hsw_stream *stream)
+{
+	u64 ppos;
+
+	sst_dsp_read(hsw->dsp, &ppos,
+		stream->reply.presentation_position_register_address,
+		sizeof(ppos));
+
+	return ppos;
 }
 
 int sst_hsw_stream_set_write_position(struct sst_hsw *hsw,
@@ -1609,7 +1637,7 @@ int sst_hsw_dx_set_state(struct sst_hsw *hsw,
 	trace_ipc_request("PM enter Dx state", state);
 
 	ret = ipc_tx_message_wait(hsw, header, &state_, sizeof(state_),
-		dx, sizeof(dx));
+		dx, sizeof(*dx));
 	if (ret < 0) {
 		dev_err(hsw->dev, "ipc: error set dx state %d failed\n", state);
 		return ret;
diff --git a/sound/soc/intel/sst-haswell-ipc.h b/sound/soc/intel/sst-haswell-ipc.h
index d517929ccc3..2ac194a6d04 100644
--- a/sound/soc/intel/sst-haswell-ipc.h
+++ b/sound/soc/intel/sst-haswell-ipc.h
@@ -464,7 +464,9 @@ int sst_hsw_stream_get_write_pos(struct sst_hsw *hsw,
 	struct sst_hsw_stream *stream, u32 *position);
 int sst_hsw_stream_set_write_position(struct sst_hsw *hsw,
 	struct sst_hsw_stream *stream, u32 stage_id, u32 position);
-int sst_hsw_get_dsp_position(struct sst_hsw *hsw,
+u32 sst_hsw_get_dsp_position(struct sst_hsw *hsw,
+	struct sst_hsw_stream *stream);
+u64 sst_hsw_get_dsp_presentation_position(struct sst_hsw *hsw,
 	struct sst_hsw_stream *stream);
 
 /* HW port config */
diff --git a/sound/soc/intel/sst-haswell-pcm.c b/sound/soc/intel/sst-haswell-pcm.c
index 0a32dd13a23..9d5f64a583a 100644
--- a/sound/soc/intel/sst-haswell-pcm.c
+++ b/sound/soc/intel/sst-haswell-pcm.c
@@ -99,6 +99,7 @@ struct hsw_pcm_data {
 	struct snd_compr_stream *cstream;
 	unsigned int wpos;
 	struct mutex mutex;
+	bool allocated;
 };
 
 /* private data for the driver */
@@ -107,12 +108,14 @@ struct hsw_priv_data {
 	struct sst_hsw *hsw;
 
 	/* page tables */
-	unsigned char *pcm_pg[HSW_PCM_COUNT][2];
+	struct snd_dma_buffer dmab[HSW_PCM_COUNT][2];
 
 	/* DAI data */
 	struct hsw_pcm_data pcm[HSW_PCM_COUNT];
 };
 
+static u32 hsw_notify_pointer(struct sst_hsw_stream *stream, void *data);
+
 static inline u32 hsw_mixer_to_ipc(unsigned int value)
 {
 	if (value >= ARRAY_SIZE(volume_map))
@@ -273,28 +276,26 @@ static const struct snd_kcontrol_new hsw_volume_controls[] = {
 };
 
 /* Create DMA buffer page table for DSP */
-static int create_adsp_page_table(struct hsw_priv_data *pdata,
-	struct snd_soc_pcm_runtime *rtd,
-	unsigned char *dma_area, size_t size, int pcm, int stream)
+static int create_adsp_page_table(struct snd_pcm_substream *substream,
+	struct hsw_priv_data *pdata, struct snd_soc_pcm_runtime *rtd,
+	unsigned char *dma_area, size_t size, int pcm)
 {
-	int i, pages;
+	struct snd_dma_buffer *dmab = snd_pcm_get_dma_buf(substream);
+	int i, pages, stream = substream->stream;
 
-	if (size % PAGE_SIZE)
-		pages = (size / PAGE_SIZE) + 1;
-	else
-		pages = size / PAGE_SIZE;
+	pages = snd_sgbuf_aligned_pages(size);
 
 	dev_dbg(rtd->dev, "generating page table for %p size 0x%zu pages %d\n",
 		dma_area, size, pages);
 
 	for (i = 0; i < pages; i++) {
 		u32 idx = (((i << 2) + i)) >> 1;
-		u32 pfn = (virt_to_phys(dma_area + i * PAGE_SIZE)) >> PAGE_SHIFT;
+		u32 pfn = snd_sgbuf_get_addr(dmab, i * PAGE_SIZE) >> PAGE_SHIFT;
 		u32 *pg_table;
 
 		dev_dbg(rtd->dev, "pfn i %i idx %d pfn %x\n", i, idx, pfn);
 
-		pg_table = (u32*)(pdata->pcm_pg[pcm][stream] + idx);
+		pg_table = (u32 *)(pdata->dmab[pcm][stream].area + idx);
 
 		if (i & 1)
 			*pg_table |= (pfn << 4);
@@ -317,12 +318,36 @@ static int hsw_pcm_hw_params(struct snd_pcm_substream *substream,
 	struct sst_hsw *hsw = pdata->hsw;
 	struct sst_module *module_data;
 	struct sst_dsp *dsp;
+	struct snd_dma_buffer *dmab;
 	enum sst_hsw_stream_type stream_type;
 	enum sst_hsw_stream_path_id path_id;
 	u32 rate, bits, map, pages, module_id;
 	u8 channels;
 	int ret;
 
+	/* check if we are being called a subsequent time */
+	if (pcm_data->allocated) {
+		ret = sst_hsw_stream_reset(hsw, pcm_data->stream);
+		if (ret < 0)
+			dev_dbg(rtd->dev, "error: reset stream failed %d\n",
+				ret);
+
+		ret = sst_hsw_stream_free(hsw, pcm_data->stream);
+		if (ret < 0) {
+			dev_dbg(rtd->dev, "error: free stream failed %d\n",
+				ret);
+			return ret;
+		}
+		pcm_data->allocated = false;
+
+		pcm_data->stream = sst_hsw_stream_new(hsw, rtd->cpu_dai->id,
+			hsw_notify_pointer, pcm_data);
+		if (pcm_data->stream == NULL) {
+			dev_err(rtd->dev, "error: failed to create stream\n");
+			return -EINVAL;
+		}
+	}
+
 	/* stream direction */
 	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK)
 		path_id = SST_HSW_STREAM_PATH_SSP0_OUT;
@@ -416,8 +441,10 @@ static int hsw_pcm_hw_params(struct snd_pcm_substream *substream,
 		return ret;
 	}
 
-	ret = create_adsp_page_table(pdata, rtd, runtime->dma_area,
-		runtime->dma_bytes, rtd->cpu_dai->id, substream->stream);
+	dmab = snd_pcm_get_dma_buf(substream);
+
+	ret = create_adsp_page_table(substream, pdata, rtd, runtime->dma_area,
+		runtime->dma_bytes, rtd->cpu_dai->id);
 	if (ret < 0)
 		return ret;
 
@@ -430,9 +457,9 @@ static int hsw_pcm_hw_params(struct snd_pcm_substream *substream,
 		pages = runtime->dma_bytes / PAGE_SIZE;
 
 	ret = sst_hsw_stream_buffer(hsw, pcm_data->stream,
-		virt_to_phys(pdata->pcm_pg[rtd->cpu_dai->id][substream->stream]),
+		pdata->dmab[rtd->cpu_dai->id][substream->stream].addr,
 		pages, runtime->dma_bytes, 0,
-		(u32)(virt_to_phys(runtime->dma_area) >> PAGE_SHIFT));
+		snd_sgbuf_get_addr(dmab, 0) >> PAGE_SHIFT);
 	if (ret < 0) {
 		dev_err(rtd->dev, "error: failed to set DMA buffer %d\n", ret);
 		return ret;
@@ -474,6 +501,7 @@ static int hsw_pcm_hw_params(struct snd_pcm_substream *substream,
 		dev_err(rtd->dev, "error: failed to commit stream %d\n", ret);
 		return ret;
 	}
+	pcm_data->allocated = true;
 
 	ret = sst_hsw_stream_pause(hsw, pcm_data->stream, 1);
 	if (ret < 0)
@@ -541,12 +569,14 @@ static snd_pcm_uframes_t hsw_pcm_pointer(struct snd_pcm_substream *substream)
 	struct hsw_pcm_data *pcm_data = snd_soc_pcm_get_drvdata(rtd);
 	struct sst_hsw *hsw = pdata->hsw;
 	snd_pcm_uframes_t offset;
+	uint64_t ppos;
+	u32 position = sst_hsw_get_dsp_position(hsw, pcm_data->stream);
 
-	offset = bytes_to_frames(runtime,
-		sst_hsw_get_dsp_position(hsw, pcm_data->stream));
+	offset = bytes_to_frames(runtime, position);
+	ppos = sst_hsw_get_dsp_presentation_position(hsw, pcm_data->stream);
 
-	dev_dbg(rtd->dev, "PCM: DMA pointer %zu bytes\n",
-		frames_to_bytes(runtime, (u32)offset));
+	dev_dbg(rtd->dev, "PCM: DMA pointer %du bytes, pos %llu\n",
+		position, ppos);
 	return offset;
 }
 
@@ -606,6 +636,7 @@ static int hsw_pcm_close(struct snd_pcm_substream *substream)
 		dev_dbg(rtd->dev, "error: free stream failed %d\n", ret);
 		goto out;
 	}
+	pcm_data->allocated = 0;
 	pcm_data->stream = NULL;
 
 out:
@@ -621,7 +652,7 @@ static struct snd_pcm_ops hsw_pcm_ops = {
 	.hw_free	= hsw_pcm_hw_free,
 	.trigger	= hsw_pcm_trigger,
 	.pointer	= hsw_pcm_pointer,
-	.mmap		= snd_pcm_lib_default_mmap,
+	.page		= snd_pcm_sgbuf_ops_page,
 };
 
 static void hsw_pcm_free(struct snd_pcm *pcm)
@@ -632,17 +663,16 @@ static void hsw_pcm_free(struct snd_pcm *pcm)
 static int hsw_pcm_new(struct snd_soc_pcm_runtime *rtd)
 {
 	struct snd_pcm *pcm = rtd->pcm;
+	struct snd_soc_platform *platform = rtd->platform;
+	struct sst_pdata *pdata = dev_get_platdata(platform->dev);
+	struct device *dev = pdata->dma_dev;
 	int ret = 0;
 
-	ret = dma_coerce_mask_and_coherent(rtd->card->dev, DMA_BIT_MASK(32));
-	if (ret)
-		return ret;
-
 	if (pcm->streams[SNDRV_PCM_STREAM_PLAYBACK].substream ||
 			pcm->streams[SNDRV_PCM_STREAM_CAPTURE].substream) {
 		ret = snd_pcm_lib_preallocate_pages_for_all(pcm,
-			SNDRV_DMA_TYPE_DEV,
-			rtd->card->dev,
+			SNDRV_DMA_TYPE_DEV_SG,
+			dev,
 			hsw_pcm_hardware.buffer_bytes_max,
 			hsw_pcm_hardware.buffer_bytes_max);
 		if (ret) {
@@ -742,11 +772,14 @@ static int hsw_pcm_probe(struct snd_soc_platform *platform)
 {
 	struct sst_pdata *pdata = dev_get_platdata(platform->dev);
 	struct hsw_priv_data *priv_data;
-	int i;
+	struct device *dma_dev;
+	int i, ret = 0;
 
 	if (!pdata)
 		return -ENODEV;
 
+	dma_dev = pdata->dma_dev;
+
 	priv_data = devm_kzalloc(platform->dev, sizeof(*priv_data), GFP_KERNEL);
 	priv_data->hsw = pdata->dsp;
 	snd_soc_platform_set_drvdata(platform, priv_data);
@@ -758,15 +791,17 @@ static int hsw_pcm_probe(struct snd_soc_platform *platform)
 
 		/* playback */
 		if (hsw_dais[i].playback.channels_min) {
-			priv_data->pcm_pg[i][0] = kzalloc(PAGE_SIZE, GFP_DMA);
-			if (priv_data->pcm_pg[i][0] == NULL)
+			ret = snd_dma_alloc_pages(SNDRV_DMA_TYPE_DEV, dma_dev,
+				PAGE_SIZE, &priv_data->dmab[i][0]);
+			if (ret < 0)
 				goto err;
 		}
 
 		/* capture */
 		if (hsw_dais[i].capture.channels_min) {
-			priv_data->pcm_pg[i][1] = kzalloc(PAGE_SIZE, GFP_DMA);
-			if (priv_data->pcm_pg[i][1] == NULL)
+			ret = snd_dma_alloc_pages(SNDRV_DMA_TYPE_DEV, dma_dev,
+				PAGE_SIZE, &priv_data->dmab[i][1]);
+			if (ret < 0)
 				goto err;
 		}
 	}
@@ -776,11 +811,11 @@ static int hsw_pcm_probe(struct snd_soc_platform *platform)
 err:
 	for (;i >= 0; i--) {
 		if (hsw_dais[i].playback.channels_min)
-			kfree(priv_data->pcm_pg[i][0]);
+			snd_dma_free_pages(&priv_data->dmab[i][0]);
 		if (hsw_dais[i].capture.channels_min)
-			kfree(priv_data->pcm_pg[i][1]);
+			snd_dma_free_pages(&priv_data->dmab[i][1]);
 	}
-	return -ENOMEM;
+	return ret;
 }
 
 static int hsw_pcm_remove(struct snd_soc_platform *platform)
@@ -791,9 +826,9 @@ static int hsw_pcm_remove(struct snd_soc_platform *platform)
 
 	for (i = 0; i < ARRAY_SIZE(hsw_dais); i++) {
 		if (hsw_dais[i].playback.channels_min)
-			kfree(priv_data->pcm_pg[i][0]);
+			snd_dma_free_pages(&priv_data->dmab[i][0]);
 		if (hsw_dais[i].capture.channels_min)
-			kfree(priv_data->pcm_pg[i][1]);
+			snd_dma_free_pages(&priv_data->dmab[i][1]);
 	}
 
 	return 0;
diff --git a/sound/soc/jz4740/Makefile b/sound/soc/jz4740/Makefile
index be873c1b0c2..d32c540555c 100644
--- a/sound/soc/jz4740/Makefile
+++ b/sound/soc/jz4740/Makefile
@@ -1,10 +1,8 @@
 #
 # Jz4740 Platform Support
 #
-snd-soc-jz4740-objs := jz4740-pcm.o
 snd-soc-jz4740-i2s-objs := jz4740-i2s.o
 
-obj-$(CONFIG_SND_JZ4740_SOC) += snd-soc-jz4740.o
 obj-$(CONFIG_SND_JZ4740_SOC_I2S) += snd-soc-jz4740-i2s.o
 
 # Jz4740 Machine Support
diff --git a/sound/soc/sh/rcar/core.c b/sound/soc/sh/rcar/core.c
index 215b668166b..89424470a1f 100644
--- a/sound/soc/sh/rcar/core.c
+++ b/sound/soc/sh/rcar/core.c
@@ -197,13 +197,12 @@ static void rsnd_dma_complete(void *data)
 	 * rsnd_dai_pointer_update() will be called twice,
 	 * ant it will breaks io->byte_pos
 	 */
-
-	rsnd_dai_pointer_update(io, io->byte_per_period);
-
 	if (dma->submit_loop)
 		rsnd_dma_continue(dma);
 
 	rsnd_unlock(priv, flags);
+
+	rsnd_dai_pointer_update(io, io->byte_per_period);
 }
 
 static void __rsnd_dma_start(struct rsnd_dma *dma)
diff --git a/sound/soc/sh/rcar/src.c b/sound/soc/sh/rcar/src.c
index 6232b7d307a..4d0720ed5a9 100644
--- a/sound/soc/sh/rcar/src.c
+++ b/sound/soc/sh/rcar/src.c
@@ -258,7 +258,7 @@ static int rsnd_src_init(struct rsnd_mod *mod,
 {
 	struct rsnd_src *src = rsnd_mod_to_src(mod);
 
-	clk_enable(src->clk);
+	clk_prepare_enable(src->clk);
 
 	return 0;
 }
@@ -269,7 +269,7 @@ static int rsnd_src_quit(struct rsnd_mod *mod,
 {
 	struct rsnd_src *src = rsnd_mod_to_src(mod);
 
-	clk_disable(src->clk);
+	clk_disable_unprepare(src->clk);
 
 	return 0;
 }
diff --git a/sound/soc/sh/rcar/ssi.c b/sound/soc/sh/rcar/ssi.c
index 4b7e20603dd..1d8387c25bd 100644
--- a/sound/soc/sh/rcar/ssi.c
+++ b/sound/soc/sh/rcar/ssi.c
@@ -171,7 +171,7 @@ static void rsnd_ssi_hw_start(struct rsnd_ssi *ssi,
 	u32 cr;
 
 	if (0 == ssi->usrcnt) {
-		clk_enable(ssi->clk);
+		clk_prepare_enable(ssi->clk);
 
 		if (rsnd_dai_is_clk_master(rdai)) {
 			if (rsnd_ssi_clk_from_parent(ssi))
@@ -230,7 +230,7 @@ static void rsnd_ssi_hw_stop(struct rsnd_ssi *ssi,
 				rsnd_ssi_master_clk_stop(ssi);
 		}
 
-		clk_disable(ssi->clk);
+		clk_disable_unprepare(ssi->clk);
 	}
 
 	dev_dbg(dev, "ssi%d hw stopped\n", rsnd_mod_id(&ssi->mod));
diff --git a/sound/soc/soc-dapm.c b/sound/soc/soc-dapm.c
index c8a780d0d05..6d6ceee447d 100644
--- a/sound/soc/soc-dapm.c
+++ b/sound/soc/soc-dapm.c
@@ -254,7 +254,6 @@ static int dapm_kcontrol_data_alloc(struct snd_soc_dapm_widget *widget,
 static void dapm_kcontrol_free(struct snd_kcontrol *kctl)
 {
 	struct dapm_kcontrol_data *data = snd_kcontrol_chip(kctl);
-	kfree(data->widget);
 	kfree(data->wlist);
 	kfree(data);
 }
@@ -1613,8 +1612,11 @@ static void dapm_pre_sequence_async(void *data, async_cookie_t cookie)
 				"ASoC: Failed to turn on bias: %d\n", ret);
 	}
 
-	/* Prepare for a STADDBY->ON or ON->STANDBY transition */
-	if (d->bias_level != d->target_bias_level) {
+	/* Prepare for a transition to ON or away from ON */
+	if ((d->target_bias_level == SND_SOC_BIAS_ON &&
+	     d->bias_level != SND_SOC_BIAS_ON) ||
+	    (d->target_bias_level != SND_SOC_BIAS_ON &&
+	     d->bias_level == SND_SOC_BIAS_ON)) {
 		ret = snd_soc_dapm_set_bias_level(d, SND_SOC_BIAS_PREPARE);
 		if (ret != 0)
 			dev_err(d->dev,
@@ -3476,8 +3478,11 @@ void snd_soc_dapm_connect_dai_link_widgets(struct snd_soc_card *card)
 		cpu_dai = rtd->cpu_dai;
 		codec_dai = rtd->codec_dai;
 
-		/* dynamic FE links have no fixed DAI mapping */
-		if (rtd->dai_link->dynamic)
+		/*
+		 * dynamic FE links have no fixed DAI mapping.
+		 * CODEC<->CODEC links have no direct connection.
+		 */
+		if (rtd->dai_link->dynamic || rtd->dai_link->params)
 			continue;
 
 		/* there is no point in connecting BE DAI links with dummies */
diff --git a/sound/soc/soc-pcm.c b/sound/soc/soc-pcm.c
index 2cedf09f6d9..a391de05803 100644
--- a/sound/soc/soc-pcm.c
+++ b/sound/soc/soc-pcm.c
@@ -1675,7 +1675,7 @@ int dpcm_be_dai_trigger(struct snd_soc_pcm_runtime *fe, int stream,
 			be->dpcm[stream].state = SND_SOC_DPCM_STATE_STOP;
 			break;
 		case SNDRV_PCM_TRIGGER_SUSPEND:
-			if (be->dpcm[stream].state != SND_SOC_DPCM_STATE_STOP)
+			if (be->dpcm[stream].state != SND_SOC_DPCM_STATE_START)
 				continue;
 
 			if (!snd_soc_dpcm_can_be_free_stop(fe, be, stream))
diff --git a/sound/usb/card.c b/sound/usb/card.c
index 893d5a1afc3..c3b5b7dca1c 100644
--- a/sound/usb/card.c
+++ b/sound/usb/card.c
@@ -651,7 +651,7 @@ int snd_usb_autoresume(struct snd_usb_audio *chip)
 	int err = -ENODEV;
 
 	down_read(&chip->shutdown_rwsem);
-	if (chip->probing)
+	if (chip->probing && chip->in_pm)
 		err = 0;
 	else if (!chip->shutdown)
 		err = usb_autopm_get_interface(chip->pm_intf);
@@ -663,7 +663,7 @@ int snd_usb_autoresume(struct snd_usb_audio *chip)
 void snd_usb_autosuspend(struct snd_usb_audio *chip)
 {
 	down_read(&chip->shutdown_rwsem);
-	if (!chip->shutdown && !chip->probing)
+	if (!chip->shutdown && !chip->probing && !chip->in_pm)
 		usb_autopm_put_interface(chip->pm_intf);
 	up_read(&chip->shutdown_rwsem);
 }
@@ -695,8 +695,9 @@ static int usb_audio_suspend(struct usb_interface *intf, pm_message_t message)
 			chip->autosuspended = 1;
 	}
 
-	list_for_each_entry(mixer, &chip->mixer_list, list)
-		snd_usb_mixer_suspend(mixer);
+	if (chip->num_suspended_intf == 1)
+		list_for_each_entry(mixer, &chip->mixer_list, list)
+			snd_usb_mixer_suspend(mixer);
 
 	return 0;
 }
@@ -711,6 +712,8 @@ static int __usb_audio_resume(struct usb_interface *intf, bool reset_resume)
 		return 0;
 	if (--chip->num_suspended_intf)
 		return 0;
+
+	chip->in_pm = 1;
 	/*
 	 * ALSA leaves material resumption to user space
 	 * we just notify and restart the mixers
@@ -726,6 +729,7 @@ static int __usb_audio_resume(struct usb_interface *intf, bool reset_resume)
 	chip->autosuspended = 0;
 
 err_out:
+	chip->in_pm = 0;
 	return err;
 }
 
diff --git a/sound/usb/card.h b/sound/usb/card.h
index 9867ab86685..97acb906acc 100644
--- a/sound/usb/card.h
+++ b/sound/usb/card.h
@@ -92,6 +92,7 @@ struct snd_usb_endpoint {
 	unsigned int curframesize;      /* current packet size in frames (for capture) */
 	unsigned int syncmaxsize;	/* sync endpoint packet size */
 	unsigned int fill_max:1;	/* fill max packet size always */
+	unsigned int udh01_fb_quirk:1;	/* corrupted feedback data */
 	unsigned int datainterval;      /* log_2 of data packet interval */
 	unsigned int syncinterval;	/* P for adaptive mode, 0 otherwise */
 	unsigned char silence_value;
diff --git a/sound/usb/endpoint.c b/sound/usb/endpoint.c
index e70a87e0d9f..289f582c913 100644
--- a/sound/usb/endpoint.c
+++ b/sound/usb/endpoint.c
@@ -471,6 +471,10 @@ struct snd_usb_endpoint *snd_usb_add_endpoint(struct snd_usb_audio *chip,
 			ep->syncinterval = 3;
 
 		ep->syncmaxsize = le16_to_cpu(get_endpoint(alts, 1)->wMaxPacketSize);
+
+		if (chip->usb_id == USB_ID(0x0644, 0x8038) /* TEAC UD-H01 */ &&
+		    ep->syncmaxsize == 4)
+			ep->udh01_fb_quirk = 1;
 	}
 
 	list_add_tail(&ep->list, &chip->ep_list);
@@ -1105,7 +1109,16 @@ void snd_usb_handle_sync_urb(struct snd_usb_endpoint *ep,
 	if (f == 0)
 		return;
 
-	if (unlikely(ep->freqshift == INT_MIN)) {
+	if (unlikely(sender->udh01_fb_quirk)) {
+		/*
+		 * The TEAC UD-H01 firmware sometimes changes the feedback value
+		 * by +/- 0x1.0000.
+		 */
+		if (f < ep->freqn - 0x8000)
+			f += 0x10000;
+		else if (f > ep->freqn + 0x8000)
+			f -= 0x10000;
+	} else if (unlikely(ep->freqshift == INT_MIN)) {
 		/*
 		 * The first time we see a feedback value, determine its format
 		 * by shifting it left or right until it matches the nominal
diff --git a/sound/usb/pcm.c b/sound/usb/pcm.c
index 131336d4049..c62a1659106 100644
--- a/sound/usb/pcm.c
+++ b/sound/usb/pcm.c
@@ -1501,9 +1501,8 @@ static void retire_playback_urb(struct snd_usb_substream *subs,
 	 * The error should be lower than 2ms since the estimate relies
 	 * on two reads of a counter updated every ms.
 	 */
-	if (printk_ratelimit() &&
-	    abs(est_delay - subs->last_delay) * 1000 > runtime->rate * 2)
-		dev_dbg(&subs->dev->dev,
+	if (abs(est_delay - subs->last_delay) * 1000 > runtime->rate * 2)
+		dev_dbg_ratelimited(&subs->dev->dev,
 			"delay: estimated %d, actual %d\n",
 			est_delay, subs->last_delay);
 
diff --git a/sound/usb/usbaudio.h b/sound/usb/usbaudio.h
index 25c4c7e217d..91d0380431b 100644
--- a/sound/usb/usbaudio.h
+++ b/sound/usb/usbaudio.h
@@ -40,6 +40,7 @@ struct snd_usb_audio {
 	struct rw_semaphore shutdown_rwsem;
 	unsigned int shutdown:1;
 	unsigned int probing:1;
+	unsigned int in_pm:1;
 	unsigned int autosuspended:1;	
 	unsigned int txfr_quirk:1; /* Subframe boundaries on transfers */
 	
diff --git a/tools/Makefile b/tools/Makefile
index bcae806b0c3..9a617adc667 100644
--- a/tools/Makefile
+++ b/tools/Makefile
@@ -44,6 +44,9 @@ cpupower: FORCE
 cgroup firewire hv guest usb virtio vm net: FORCE
 	$(call descend,$@)
 
+liblockdep: FORCE
+	$(call descend,lib/lockdep)
+
 libapikfs: FORCE
 	$(call descend,lib/api)
 
@@ -91,6 +94,9 @@ cpupower_clean:
 cgroup_clean hv_clean firewire_clean lguest_clean usb_clean virtio_clean vm_clean net_clean:
 	$(call descend,$(@:_clean=),clean)
 
+liblockdep_clean:
+	$(call descend,lib/lockdep,clean)
+
 libapikfs_clean:
 	$(call descend,lib/api,clean)
 
diff --git a/tools/hv/hv_fcopy_daemon.c b/tools/hv/hv_fcopy_daemon.c
index 4ecc4fd0bc1..fba1c75aa48 100644
--- a/tools/hv/hv_fcopy_daemon.c
+++ b/tools/hv/hv_fcopy_daemon.c
@@ -82,8 +82,10 @@ static int hv_start_fcopy(struct hv_start_fcopy *smsg)
 
 	if (!access(target_fname, F_OK)) {
 		syslog(LOG_INFO, "File: %s exists", target_fname);
-		if (!smsg->copy_flags & OVER_WRITE)
+		if (!(smsg->copy_flags & OVER_WRITE)) {
+			error = HV_ERROR_ALREADY_EXISTS;
 			goto done;
+		}
 	}
 
 	target_fd = open(target_fname, O_RDWR | O_CREAT | O_CLOEXEC, 0744);
diff --git a/tools/lib/api/fs/debugfs.c b/tools/lib/api/fs/debugfs.c
index 7c434796235..a74fba6d774 100644
--- a/tools/lib/api/fs/debugfs.c
+++ b/tools/lib/api/fs/debugfs.c
@@ -12,8 +12,8 @@
 char debugfs_mountpoint[PATH_MAX + 1] = "/sys/kernel/debug";
 
 static const char * const debugfs_known_mountpoints[] = {
-	"/sys/kernel/debug/",
-	"/debug/",
+	"/sys/kernel/debug",
+	"/debug",
 	0,
 };
 
diff --git a/tools/lib/lockdep/Makefile b/tools/lib/lockdep/Makefile
index cb09d3ff8f5..bba2f5253b6 100644
--- a/tools/lib/lockdep/Makefile
+++ b/tools/lib/lockdep/Makefile
@@ -1,8 +1,7 @@
 # file format version
 FILE_VERSION = 1
 
-MAKEFLAGS += --no-print-directory
-LIBLOCKDEP_VERSION=$(shell make -sC ../../.. kernelversion)
+LIBLOCKDEP_VERSION=$(shell make --no-print-directory -sC ../../.. kernelversion)
 
 # Makefiles suck: This macro sets a default value of $(2) for the
 # variable named by $(1), unless the variable has been set by
@@ -231,7 +230,7 @@ install_lib: all_cmd
 install: install_lib
 
 clean:
-	$(RM) *.o *~ $(TARGETS) *.a *.so $(VERSION_FILES) .*.d
+	$(RM) *.o *~ $(TARGETS) *.a *liblockdep*.so* $(VERSION_FILES) .*.d
 	$(RM) tags TAGS
 
 endif # skip-makefile
diff --git a/tools/lib/traceevent/event-parse.c b/tools/lib/traceevent/event-parse.c
index baec7d887da..b83184f2d48 100644
--- a/tools/lib/traceevent/event-parse.c
+++ b/tools/lib/traceevent/event-parse.c
@@ -4344,6 +4344,7 @@ static void pretty_print(struct trace_seq *s, void *data, int size, struct event
 					      format, len_arg, arg);
 				trace_seq_terminate(&p);
 				trace_seq_puts(s, p.buffer);
+				trace_seq_destroy(&p);
 				arg = arg->next;
 				break;
 			default:
diff --git a/tools/lib/traceevent/event-parse.h b/tools/lib/traceevent/event-parse.h
index 791c539374c..feab9428163 100644
--- a/tools/lib/traceevent/event-parse.h
+++ b/tools/lib/traceevent/event-parse.h
@@ -876,8 +876,8 @@ struct event_filter {
 struct event_filter *pevent_filter_alloc(struct pevent *pevent);
 
 /* for backward compatibility */
-#define FILTER_NONE		PEVENT_ERRNO__FILTER_NOT_FOUND
-#define FILTER_NOEXIST		PEVENT_ERRNO__NO_FILTER
+#define FILTER_NONE		PEVENT_ERRNO__NO_FILTER
+#define FILTER_NOEXIST		PEVENT_ERRNO__FILTER_NOT_FOUND
 #define FILTER_MISS		PEVENT_ERRNO__FILTER_MISS
 #define FILTER_MATCH		PEVENT_ERRNO__FILTER_MATCH
 
diff --git a/tools/net/bpf_dbg.c b/tools/net/bpf_dbg.c
index bb31813e43d..9a287bec695 100644
--- a/tools/net/bpf_dbg.c
+++ b/tools/net/bpf_dbg.c
@@ -820,7 +820,7 @@ do_div:
 		r->A &= r->X;
 		break;
 	case BPF_ALU_AND | BPF_K:
-		r->A &= r->X;
+		r->A &= K;
 		break;
 	case BPF_ALU_OR | BPF_X:
 		r->A |= r->X;
diff --git a/tools/perf/Makefile.perf b/tools/perf/Makefile.perf
index e96923310d5..895edd32930 100644
--- a/tools/perf/Makefile.perf
+++ b/tools/perf/Makefile.perf
@@ -589,7 +589,7 @@ $(GTK_OBJS): $(OUTPUT)%.o: %.c $(LIB_H)
 	$(QUIET_CC)$(CC) -o $@ -c -fPIC $(CFLAGS) $(GTK_CFLAGS) $<
 
 $(OUTPUT)libperf-gtk.so: $(GTK_OBJS) $(PERFLIBS)
-	$(QUIET_LINK)$(CC) -o $@ -shared $(ALL_LDFLAGS) $(filter %.o,$^) $(GTK_LIBS)
+	$(QUIET_LINK)$(CC) -o $@ -shared $(LDFLAGS) $(filter %.o,$^) $(GTK_LIBS)
 
 $(OUTPUT)builtin-help.o: builtin-help.c $(OUTPUT)common-cmds.h $(OUTPUT)PERF-CFLAGS
 	$(QUIET_CC)$(CC) -o $@ -c $(CFLAGS) \
diff --git a/tools/perf/arch/x86/tests/dwarf-unwind.c b/tools/perf/arch/x86/tests/dwarf-unwind.c
index b602ad93ce6..83bc2385e6d 100644
--- a/tools/perf/arch/x86/tests/dwarf-unwind.c
+++ b/tools/perf/arch/x86/tests/dwarf-unwind.c
@@ -23,9 +23,10 @@ static int sample_ustack(struct perf_sample *sample,
 
 	sp = (unsigned long) regs[PERF_REG_X86_SP];
 
-	map = map_groups__find(&thread->mg, MAP__FUNCTION, (u64) sp);
+	map = map_groups__find(&thread->mg, MAP__VARIABLE, (u64) sp);
 	if (!map) {
 		pr_debug("failed to get stack map\n");
+		free(buf);
 		return -1;
 	}
 
diff --git a/tools/perf/arch/x86/tests/regs_load.S b/tools/perf/arch/x86/tests/regs_load.S
index 99167bf644e..60875d5c556 100644
--- a/tools/perf/arch/x86/tests/regs_load.S
+++ b/tools/perf/arch/x86/tests/regs_load.S
@@ -1,4 +1,3 @@
-
 #include <linux/linkage.h>
 
 #define AX	 0
@@ -90,3 +89,10 @@ ENTRY(perf_regs_load)
 	ret
 ENDPROC(perf_regs_load)
 #endif
+
+/*
+ * We need to provide note.GNU-stack section, saying that we want
+ * NOT executable stack. Otherwise the final linking will assume that
+ * the ELF stack should not be restricted at all and set it RWX.
+ */
+.section .note.GNU-stack,"",@progbits
diff --git a/tools/perf/builtin-kvm.c b/tools/perf/builtin-kvm.c
index 21c164b8f9d..0f1e5a2f6ad 100644
--- a/tools/perf/builtin-kvm.c
+++ b/tools/perf/builtin-kvm.c
@@ -404,6 +404,7 @@ static struct kvm_event *kvm_alloc_init_event(struct event_key *key)
 	}
 
 	event->key = *key;
+	init_stats(&event->total.stats);
 	return event;
 }
 
diff --git a/tools/perf/builtin-record.c b/tools/perf/builtin-record.c
index eb524f91bff..8ce62ef7f6c 100644
--- a/tools/perf/builtin-record.c
+++ b/tools/perf/builtin-record.c
@@ -374,7 +374,7 @@ static int __cmd_record(struct record *rec, int argc, const char **argv)
 
 	session = perf_session__new(file, false, NULL);
 	if (session == NULL) {
-		pr_err("Not enough memory for reading perf file header\n");
+		pr_err("Perf session creation failed.\n");
 		return -1;
 	}
 
diff --git a/tools/perf/config/Makefile b/tools/perf/config/Makefile
index ee21fa95ebc..802cf544202 100644
--- a/tools/perf/config/Makefile
+++ b/tools/perf/config/Makefile
@@ -34,6 +34,14 @@ ifeq ($(ARCH),arm)
   LIBUNWIND_LIBS = -lunwind -lunwind-arm
 endif
 
+# So far there's only x86 libdw unwind support merged in perf.
+# Disable it on all other architectures in case libdw unwind
+# support is detected in system. Add supported architectures
+# to the check.
+ifneq ($(ARCH),x86)
+  NO_LIBDW_DWARF_UNWIND := 1
+endif
+
 ifeq ($(LIBUNWIND_LIBS),)
   NO_LIBUNWIND := 1
 else
@@ -109,6 +117,10 @@ CFLAGS += -Wall
 CFLAGS += -Wextra
 CFLAGS += -std=gnu99
 
+# Enforce a non-executable stack, as we may regress (again) in the future by
+# adding assembler files missing the .GNU-stack linker note.
+LDFLAGS += -Wl,-z,noexecstack
+
 EXTLIBS = -lelf -lpthread -lrt -lm -ldl
 
 ifneq ($(OUTPUT),)
@@ -186,7 +198,10 @@ VF_FEATURE_TESTS =			\
 	stackprotector-all		\
 	timerfd				\
 	libunwind-debug-frame		\
-	bionic
+	bionic				\
+	liberty				\
+	liberty-z			\
+	cplus-demangle
 
 # Set FEATURE_CHECK_(C|LD)FLAGS-all for all CORE_FEATURE_TESTS features.
 # If in the future we need per-feature checks/flags for features not
@@ -504,7 +519,21 @@ else
 endif
 
 ifeq ($(feature-libbfd), 1)
-  EXTLIBS += -lbfd -lz -liberty
+  EXTLIBS += -lbfd
+
+  # call all detections now so we get correct
+  # status in VF output
+  $(call feature_check,liberty)
+  $(call feature_check,liberty-z)
+  $(call feature_check,cplus-demangle)
+
+  ifeq ($(feature-liberty), 1)
+    EXTLIBS += -liberty
+  else
+    ifeq ($(feature-liberty-z), 1)
+      EXTLIBS += -liberty -lz
+    endif
+  endif
 endif
 
 ifdef NO_DEMANGLE
@@ -515,15 +544,10 @@ else
     CFLAGS += -DHAVE_CPLUS_DEMANGLE_SUPPORT
   else
     ifneq ($(feature-libbfd), 1)
-      $(call feature_check,liberty)
-      ifeq ($(feature-liberty), 1)
-        EXTLIBS += -lbfd -liberty
-      else
-        $(call feature_check,liberty-z)
-        ifeq ($(feature-liberty-z), 1)
-          EXTLIBS += -lbfd -liberty -lz
-        else
-          $(call feature_check,cplus-demangle)
+      ifneq ($(feature-liberty), 1)
+        ifneq ($(feature-liberty-z), 1)
+          # we dont have neither HAVE_CPLUS_DEMANGLE_SUPPORT
+          # or any of 'bfd iberty z' trinity
           ifeq ($(feature-cplus-demangle), 1)
             EXTLIBS += -liberty
             CFLAGS += -DHAVE_CPLUS_DEMANGLE_SUPPORT
diff --git a/tools/perf/tests/make b/tools/perf/tests/make
index 5daeae1cb4c..2f92d6e7ee0 100644
--- a/tools/perf/tests/make
+++ b/tools/perf/tests/make
@@ -46,6 +46,7 @@ make_install_man    := install-man
 make_install_html   := install-html
 make_install_info   := install-info
 make_install_pdf    := install-pdf
+make_static         := LDFLAGS=-static
 
 # all the NO_* variable combined
 make_minimal        := NO_LIBPERL=1 NO_LIBPYTHON=1 NO_NEWT=1 NO_GTK2=1
@@ -87,6 +88,7 @@ run += make_install_bin
 # run += make_install_info
 # run += make_install_pdf
 run += make_minimal
+run += make_static
 
 ifneq ($(call has,ctags),)
 run += make_tags
diff --git a/tools/perf/util/data.c b/tools/perf/util/data.c
index 1fbcd8bdc11..55de44ecebe 100644
--- a/tools/perf/util/data.c
+++ b/tools/perf/util/data.c
@@ -86,10 +86,17 @@ static int open_file_read(struct perf_data_file *file)
 
 static int open_file_write(struct perf_data_file *file)
 {
+	int fd;
+
 	if (check_backup(file))
 		return -1;
 
-	return open(file->path, O_CREAT|O_RDWR|O_TRUNC, S_IRUSR|S_IWUSR);
+	fd = open(file->path, O_CREAT|O_RDWR|O_TRUNC, S_IRUSR|S_IWUSR);
+
+	if (fd < 0)
+		pr_err("failed to open %s : %s\n", file->path, strerror(errno));
+
+	return fd;
 }
 
 static int open_file(struct perf_data_file *file)
diff --git a/tools/perf/util/machine.c b/tools/perf/util/machine.c
index a53cd0b8c15..27c2a5efe45 100644
--- a/tools/perf/util/machine.c
+++ b/tools/perf/util/machine.c
@@ -717,7 +717,7 @@ static char *get_kernel_version(const char *root_dir)
 }
 
 static int map_groups__set_modules_path_dir(struct map_groups *mg,
-				const char *dir_name)
+				const char *dir_name, int depth)
 {
 	struct dirent *dent;
 	DIR *dir = opendir(dir_name);
@@ -742,7 +742,15 @@ static int map_groups__set_modules_path_dir(struct map_groups *mg,
 			    !strcmp(dent->d_name, ".."))
 				continue;
 
-			ret = map_groups__set_modules_path_dir(mg, path);
+			/* Do not follow top-level source and build symlinks */
+			if (depth == 0) {
+				if (!strcmp(dent->d_name, "source") ||
+				    !strcmp(dent->d_name, "build"))
+					continue;
+			}
+
+			ret = map_groups__set_modules_path_dir(mg, path,
+							       depth + 1);
 			if (ret < 0)
 				goto out;
 		} else {
@@ -786,11 +794,11 @@ static int machine__set_modules_path(struct machine *machine)
 	if (!version)
 		return -1;
 
-	snprintf(modules_path, sizeof(modules_path), "%s/lib/modules/%s/kernel",
+	snprintf(modules_path, sizeof(modules_path), "%s/lib/modules/%s",
 		 machine->root_dir, version);
 	free(version);
 
-	return map_groups__set_modules_path_dir(&machine->kmaps, modules_path);
+	return map_groups__set_modules_path_dir(&machine->kmaps, modules_path, 0);
 }
 
 static int machine__create_module(void *arg, const char *name, u64 start)
diff --git a/tools/perf/util/symbol-elf.c b/tools/perf/util/symbol-elf.c
index 3b7dbf51d4a..6864661a79d 100644
--- a/tools/perf/util/symbol-elf.c
+++ b/tools/perf/util/symbol-elf.c
@@ -6,6 +6,7 @@
 #include <inttypes.h>
 
 #include "symbol.h"
+#include "vdso.h"
 #include <symbol/kallsyms.h>
 #include "debug.h"
 
@@ -618,6 +619,7 @@ int symsrc__init(struct symsrc *ss, struct dso *dso, const char *name,
 		GElf_Shdr shdr;
 		ss->adjust_symbols = (ehdr.e_type == ET_EXEC ||
 				ehdr.e_type == ET_REL ||
+				is_vdso_map(dso->short_name) ||
 				elf_section_by_name(elf, &ehdr, &shdr,
 						     ".gnu.prelink_undo",
 						     NULL) != NULL);
diff --git a/tools/power/acpi/Makefile b/tools/power/acpi/Makefile
index d9186a2fdf0..c2c0f20067a 100644
--- a/tools/power/acpi/Makefile
+++ b/tools/power/acpi/Makefile
@@ -89,15 +89,6 @@ else
 	STRIPCMD = $(STRIP) -s --remove-section=.note --remove-section=.comment
 endif
 
-# if DEBUG is enabled, then we do not strip or optimize
-ifeq ($(strip $(DEBUG)),true)
-	CFLAGS += -O1 -g -DDEBUG
-	STRIPCMD = /bin/true -Since_we_are_debugging
-else
-	CFLAGS += $(OPTIMIZATION) -fomit-frame-pointer
-	STRIPCMD = $(STRIP) -s --remove-section=.note --remove-section=.comment
-endif
-
 # --- ACPIDUMP BEGIN ---
 
 vpath %.c \
@@ -128,7 +119,7 @@ clean:
 	-rm -f $(OUTPUT)acpidump
 
 install-tools:
-	$(INSTALL) -d $(DESTDIR)${bindir}
+	$(INSTALL) -d $(DESTDIR)${sbindir}
 	$(INSTALL_PROGRAM) $(OUTPUT)acpidump $(DESTDIR)${sbindir}
 
 install-man:
diff --git a/tools/testing/selftests/rcutorture/bin/configinit.sh b/tools/testing/selftests/rcutorture/bin/configinit.sh
index a1be6e62add..9c3f3d39b93 100755
--- a/tools/testing/selftests/rcutorture/bin/configinit.sh
+++ b/tools/testing/selftests/rcutorture/bin/configinit.sh
@@ -62,7 +62,7 @@ grep '^grep' < $T/u.sh > $T/upd.sh
 echo "cat - $c" >> $T/upd.sh
 make mrproper
 make $buildloc distclean > $builddir/Make.distclean 2>&1
-make $buildloc defconfig > $builddir/Make.defconfig.out 2>&1
+make $buildloc $TORTURE_DEFCONFIG > $builddir/Make.defconfig.out 2>&1
 mv $builddir/.config $builddir/.config.sav
 sh $T/upd.sh < $builddir/.config.sav > $builddir/.config
 cp $builddir/.config $builddir/.config.new
diff --git a/tools/testing/selftests/rcutorture/bin/functions.sh b/tools/testing/selftests/rcutorture/bin/functions.sh
index 9b17e810ddc..d01b865bb10 100644
--- a/tools/testing/selftests/rcutorture/bin/functions.sh
+++ b/tools/testing/selftests/rcutorture/bin/functions.sh
@@ -76,15 +76,39 @@ configfrag_hotplug_cpu () {
 	grep -q '^CONFIG_HOTPLUG_CPU=y$' "$1"
 }
 
+# identify_boot_image qemu-cmd
+#
+# Returns the relative path to the kernel build image.  This will be
+# arch/<arch>/boot/bzImage unless overridden with the TORTURE_BOOT_IMAGE
+# environment variable.
+identify_boot_image () {
+	if test -n "$TORTURE_BOOT_IMAGE"
+	then
+		echo $TORTURE_BOOT_IMAGE
+	else
+		case "$1" in
+		qemu-system-x86_64|qemu-system-i386)
+			echo arch/x86/boot/bzImage
+			;;
+		qemu-system-ppc64)
+			echo arch/powerpc/boot/bzImage
+			;;
+		*)
+			echo ""
+			;;
+		esac
+	fi
+}
+
 # identify_qemu builddir
 #
 # Returns our best guess as to which qemu command is appropriate for
-# the kernel at hand.  Override with the RCU_QEMU_CMD environment variable.
+# the kernel at hand.  Override with the TORTURE_QEMU_CMD environment variable.
 identify_qemu () {
 	local u="`file "$1"`"
-	if test -n "$RCU_QEMU_CMD"
+	if test -n "$TORTURE_QEMU_CMD"
 	then
-		echo $RCU_QEMU_CMD
+		echo $TORTURE_QEMU_CMD
 	elif echo $u | grep -q x86-64
 	then
 		echo qemu-system-x86_64
@@ -98,7 +122,7 @@ identify_qemu () {
 		echo Cannot figure out what qemu command to use! 1>&2
 		echo file $1 output: $u
 		# Usually this will be one of /usr/bin/qemu-system-*
-		# Use RCU_QEMU_CMD environment variable or appropriate
+		# Use TORTURE_QEMU_CMD environment variable or appropriate
 		# argument to top-level script.
 		exit 1
 	fi
@@ -107,14 +131,14 @@ identify_qemu () {
 # identify_qemu_append qemu-cmd
 #
 # Output arguments for the qemu "-append" string based on CPU type
-# and the RCU_QEMU_INTERACTIVE environment variable.
+# and the TORTURE_QEMU_INTERACTIVE environment variable.
 identify_qemu_append () {
 	case "$1" in
 	qemu-system-x86_64|qemu-system-i386)
 		echo noapic selinux=0 initcall_debug debug
 		;;
 	esac
-	if test -n "$RCU_QEMU_INTERACTIVE"
+	if test -n "$TORTURE_QEMU_INTERACTIVE"
 	then
 		echo root=/dev/sda
 	else
@@ -124,8 +148,8 @@ identify_qemu_append () {
 
 # identify_qemu_args qemu-cmd serial-file
 #
-# Output arguments for qemu arguments based on the RCU_QEMU_MAC
-# and RCU_QEMU_INTERACTIVE environment variables.
+# Output arguments for qemu arguments based on the TORTURE_QEMU_MAC
+# and TORTURE_QEMU_INTERACTIVE environment variables.
 identify_qemu_args () {
 	case "$1" in
 	qemu-system-x86_64|qemu-system-i386)
@@ -133,17 +157,17 @@ identify_qemu_args () {
 	qemu-system-ppc64)
 		echo -enable-kvm -M pseries -cpu POWER7 -nodefaults
 		echo -device spapr-vscsi
-		if test -n "$RCU_QEMU_INTERACTIVE" -a -n "$RCU_QEMU_MAC"
+		if test -n "$TORTURE_QEMU_INTERACTIVE" -a -n "$TORTURE_QEMU_MAC"
 		then
-			echo -device spapr-vlan,netdev=net0,mac=$RCU_QEMU_MAC
+			echo -device spapr-vlan,netdev=net0,mac=$TORTURE_QEMU_MAC
 			echo -netdev bridge,br=br0,id=net0
-		elif test -n "$RCU_QEMU_INTERACTIVE"
+		elif test -n "$TORTURE_QEMU_INTERACTIVE"
 		then
 			echo -net nic -net user
 		fi
 		;;
 	esac
-	if test -n "$RCU_QEMU_INTERACTIVE"
+	if test -n "$TORTURE_QEMU_INTERACTIVE"
 	then
 		echo -monitor stdio -serial pty -S
 	else
diff --git a/tools/testing/selftests/rcutorture/bin/kvm-build.sh b/tools/testing/selftests/rcutorture/bin/kvm-build.sh
index 197901ec10b..7c1e56b46de 100755
--- a/tools/testing/selftests/rcutorture/bin/kvm-build.sh
+++ b/tools/testing/selftests/rcutorture/bin/kvm-build.sh
@@ -45,9 +45,9 @@ T=/tmp/test-linux.sh.$$
 trap 'rm -rf $T' 0
 mkdir $T
 
-cat ${config_template} | grep -v CONFIG_RCU_TORTURE_TEST > $T/config
+grep -v 'CONFIG_[A-Z]*_TORTURE_TEST' < ${config_template} > $T/config
 cat << ___EOF___ >> $T/config
-CONFIG_INITRAMFS_SOURCE="$RCU_INITRD"
+CONFIG_INITRAMFS_SOURCE="$TORTURE_INITRD"
 CONFIG_VIRTIO_PCI=y
 CONFIG_VIRTIO_CONSOLE=y
 ___EOF___
@@ -60,7 +60,7 @@ then
 	exit 2
 fi
 ncpus=`cpus2use.sh`
-make O=$builddir -j$ncpus $RCU_KMAKE_ARG > $builddir/Make.out 2>&1
+make O=$builddir -j$ncpus $TORTURE_KMAKE_ARG > $builddir/Make.out 2>&1
 retval=$?
 if test $retval -ne 0 || grep "rcu[^/]*": < $builddir/Make.out | egrep -q "Stop|Error|error:|warning:" || egrep -q "Stop|Error|error:" < $builddir/Make.out
 then
diff --git a/tools/testing/selftests/rcutorture/bin/kvm-recheck-lock.sh b/tools/testing/selftests/rcutorture/bin/kvm-recheck-lock.sh
index 829186e19eb..7f1ff1a8fc4 100755
--- a/tools/testing/selftests/rcutorture/bin/kvm-recheck-lock.sh
+++ b/tools/testing/selftests/rcutorture/bin/kvm-recheck-lock.sh
@@ -35,7 +35,7 @@ configfile=`echo $i | sed -e 's/^.*\///'`
 ncs=`grep "Writes:  Total:" $i/console.log 2> /dev/null | tail -1 | sed -e 's/^.* Total: //' -e 's/ .*$//'`
 if test -z "$ncs"
 then
-	echo $configfile
+	echo "$configfile -------"
 else
 	title="$configfile ------- $ncs acquisitions/releases"
 	dur=`sed -e 's/^.* locktorture.shutdown_secs=//' -e 's/ .*$//' < $i/qemu-cmd 2> /dev/null`
diff --git a/tools/testing/selftests/rcutorture/bin/kvm-recheck-rcu.sh b/tools/testing/selftests/rcutorture/bin/kvm-recheck-rcu.sh
index d75b1dc5ae5..307c4b95f32 100755
--- a/tools/testing/selftests/rcutorture/bin/kvm-recheck-rcu.sh
+++ b/tools/testing/selftests/rcutorture/bin/kvm-recheck-rcu.sh
@@ -35,7 +35,7 @@ configfile=`echo $i | sed -e 's/^.*\///'`
 ngps=`grep ver: $i/console.log 2> /dev/null | tail -1 | sed -e 's/^.* ver: //' -e 's/ .*$//'`
 if test -z "$ngps"
 then
-	echo $configfile
+	echo "$configfile -------"
 else
 	title="$configfile ------- $ngps grace periods"
 	dur=`sed -e 's/^.* rcutorture.shutdown_secs=//' -e 's/ .*$//' < $i/qemu-cmd 2> /dev/null`
diff --git a/tools/testing/selftests/rcutorture/bin/kvm-recheck.sh b/tools/testing/selftests/rcutorture/bin/kvm-recheck.sh
index a44daaa259a..ee1f6cae3d7 100755
--- a/tools/testing/selftests/rcutorture/bin/kvm-recheck.sh
+++ b/tools/testing/selftests/rcutorture/bin/kvm-recheck.sh
@@ -25,6 +25,7 @@
 # Authors: Paul E. McKenney <paulmck@linux.vnet.ibm.com>
 
 PATH=`pwd`/tools/testing/selftests/rcutorture/bin:$PATH; export PATH
+. tools/testing/selftests/rcutorture/bin/functions.sh
 for rd in "$@"
 do
 	firsttime=1
@@ -39,13 +40,24 @@ do
 		fi
 		TORTURE_SUITE="`cat $i/../TORTURE_SUITE`"
 		kvm-recheck-${TORTURE_SUITE}.sh $i
-		configcheck.sh $i/.config $i/ConfigFragment
-		parse-build.sh $i/Make.out $configfile
-		parse-rcutorture.sh $i/console.log $configfile
-		parse-console.sh $i/console.log $configfile
-		if test -r $i/Warnings
+		if test -f "$i/console.log"
 		then
-			cat $i/Warnings
+			configcheck.sh $i/.config $i/ConfigFragment
+			parse-build.sh $i/Make.out $configfile
+			parse-torture.sh $i/console.log $configfile
+			parse-console.sh $i/console.log $configfile
+			if test -r $i/Warnings
+			then
+				cat $i/Warnings
+			fi
+		else
+			if test -f "$i/qemu-cmd"
+			then
+				print_bug qemu failed
+			else
+				print_bug Build failed
+			fi
+			echo "   $i"
 		fi
 	done
 done
diff --git a/tools/testing/selftests/rcutorture/bin/kvm-test-1-run.sh b/tools/testing/selftests/rcutorture/bin/kvm-test-1-run.sh
index 94b28bb37d3..27e544e2951 100755
--- a/tools/testing/selftests/rcutorture/bin/kvm-test-1-run.sh
+++ b/tools/testing/selftests/rcutorture/bin/kvm-test-1-run.sh
@@ -94,9 +94,17 @@ fi
 # CONFIG_YENTA=n
 if kvm-build.sh $config_template $builddir $T
 then
+	QEMU="`identify_qemu $builddir/vmlinux`"
+	BOOT_IMAGE="`identify_boot_image $QEMU`"
 	cp $builddir/Make*.out $resdir
 	cp $builddir/.config $resdir
-	cp $builddir/arch/x86/boot/bzImage $resdir
+	if test -n "$BOOT_IMAGE"
+	then
+		cp $builddir/$BOOT_IMAGE $resdir
+	else
+		echo No identifiable boot image, not running KVM, see $resdir.
+		echo Do the torture scripts know about your architecture?
+	fi
 	parse-build.sh $resdir/Make.out $title
 	if test -f $builddir.wait
 	then
@@ -104,6 +112,7 @@ then
 	fi
 else
 	cp $builddir/Make*.out $resdir
+	cp $builddir/.config $resdir || :
 	echo Build failed, not running KVM, see $resdir.
 	if test -f $builddir.wait
 	then
@@ -124,9 +133,6 @@ cd $KVM
 kstarttime=`awk 'BEGIN { print systime() }' < /dev/null`
 echo ' ---' `date`: Starting kernel
 
-# Determine the appropriate flavor of qemu command.
-QEMU="`identify_qemu $builddir/vmlinux`"
-
 # Generate -smp qemu argument.
 qemu_args="-nographic $qemu_args"
 cpu_count=`configNR_CPUS.sh $config_template`
@@ -151,27 +157,38 @@ boot_args="`configfrag_boot_params "$boot_args" "$config_template"`"
 # Generate kernel-version-specific boot parameters
 boot_args="`per_version_boot_params "$boot_args" $builddir/.config $seconds`"
 
-echo $QEMU $qemu_args -m 512 -kernel $builddir/arch/x86/boot/bzImage -append \"$qemu_append $boot_args\" > $resdir/qemu-cmd
-if test -n "$RCU_BUILDONLY"
+echo $QEMU $qemu_args -m 512 -kernel $builddir/$BOOT_IMAGE -append \"$qemu_append $boot_args\" > $resdir/qemu-cmd
+if test -n "$TORTURE_BUILDONLY"
 then
 	echo Build-only run specified, boot/test omitted.
 	exit 0
 fi
-$QEMU $qemu_args -m 512 -kernel $builddir/arch/x86/boot/bzImage -append "$qemu_append $boot_args" &
+( $QEMU $qemu_args -m 512 -kernel $builddir/$BOOT_IMAGE -append "$qemu_append $boot_args"; echo $? > $resdir/qemu-retval ) &
 qemu_pid=$!
 commandcompleted=0
 echo Monitoring qemu job at pid $qemu_pid
-for ((i=0;i<$seconds;i++))
+while :
 do
+	kruntime=`awk 'BEGIN { print systime() - '"$kstarttime"' }' < /dev/null`
 	if kill -0 $qemu_pid > /dev/null 2>&1
 	then
+		if test $kruntime -ge $seconds
+		then
+			break;
+		fi
 		sleep 1
 	else
 		commandcompleted=1
-		kruntime=`awk 'BEGIN { print systime() - '"$kstarttime"' }' < /dev/null`
 		if test $kruntime -lt $seconds
 		then
 			echo Completed in $kruntime vs. $seconds >> $resdir/Warnings 2>&1
+			grep "^(qemu) qemu:" $resdir/kvm-test-1-run.sh.out >> $resdir/Warnings 2>&1
+			killpid="`sed -n "s/^(qemu) qemu: terminating on signal [0-9]* from pid \([0-9]*\).*$/\1/p" $resdir/Warnings`"
+			if test -n "$killpid"
+			then
+				echo "ps -fp $killpid" >> $resdir/Warnings 2>&1
+				ps -fp $killpid >> $resdir/Warnings 2>&1
+			fi
 		else
 			echo ' ---' `date`: Kernel done
 		fi
@@ -181,23 +198,25 @@ done
 if test $commandcompleted -eq 0
 then
 	echo Grace period for qemu job at pid $qemu_pid
-	for ((i=0;i<=$grace;i++))
+	while :
 	do
+		kruntime=`awk 'BEGIN { print systime() - '"$kstarttime"' }' < /dev/null`
 		if kill -0 $qemu_pid > /dev/null 2>&1
 		then
-			sleep 1
+			:
 		else
 			break
 		fi
-		if test $i -eq $grace
+		if test $kruntime -ge $((seconds + grace))
 		then
-			kruntime=`awk 'BEGIN { print systime() - '"$kstarttime"' }'`
 			echo "!!! Hang at $kruntime vs. $seconds seconds" >> $resdir/Warnings 2>&1
 			kill -KILL $qemu_pid
+			break
 		fi
+		sleep 1
 	done
 fi
 
 cp $builddir/console.log $resdir
-parse-${TORTURE_SUITE}torture.sh $resdir/console.log $title
+parse-torture.sh $resdir/console.log $title
 parse-console.sh $resdir/console.log $title
diff --git a/tools/testing/selftests/rcutorture/bin/kvm.sh b/tools/testing/selftests/rcutorture/bin/kvm.sh
index 5a78cbf55f0..40285c58653 100644
--- a/tools/testing/selftests/rcutorture/bin/kvm.sh
+++ b/tools/testing/selftests/rcutorture/bin/kvm.sh
@@ -38,9 +38,10 @@ dur=30
 dryrun=""
 KVM="`pwd`/tools/testing/selftests/rcutorture"; export KVM
 PATH=${KVM}/bin:$PATH; export PATH
-builddir="${KVM}/b1"
-RCU_INITRD="$KVM/initrd"; export RCU_INITRD
-RCU_KMAKE_ARG=""; export RCU_KMAKE_ARG
+TORTURE_DEFCONFIG=defconfig
+TORTURE_BOOT_IMAGE=""
+TORTURE_INITRD="$KVM/initrd"; export TORTURE_INITRD
+TORTURE_KMAKE_ARG=""
 TORTURE_SUITE=rcu
 resdir=""
 configs=""
@@ -53,11 +54,12 @@ kversion=""
 usage () {
 	echo "Usage: $scriptname optional arguments:"
 	echo "       --bootargs kernel-boot-arguments"
-	echo "       --builddir absolute-pathname"
+	echo "       --bootimage relative-path-to-kernel-boot-image"
 	echo "       --buildonly"
 	echo "       --configs \"config-file list\""
 	echo "       --cpus N"
 	echo "       --datestamp string"
+	echo "       --defconfig string"
 	echo "       --dryrun sched|script"
 	echo "       --duration minutes"
 	echo "       --interactive"
@@ -67,7 +69,6 @@ usage () {
 	echo "       --no-initrd"
 	echo "       --qemu-args qemu-system-..."
 	echo "       --qemu-cmd qemu-system-..."
-	echo "       --relbuilddir relative-pathname"
 	echo "       --results absolute-pathname"
 	echo "       --torture rcu"
 	exit 1
@@ -78,17 +79,16 @@ do
 	case "$1" in
 	--bootargs)
 		checkarg --bootargs "(list of kernel boot arguments)" "$#" "$2" '.*' '^--'
-		RCU_BOOTARGS="$2"
+		TORTURE_BOOTARGS="$2"
 		shift
 		;;
-	--builddir)
-		checkarg --builddir "(absolute pathname)" "$#" "$2" '^/' '^error'
-		builddir=$2
-		gotbuilddir=1
+	--bootimage)
+		checkarg --bootimage "(relative path to kernel boot image)" "$#" "$2" '[a-zA-Z0-9][a-zA-Z0-9_]*' '^--'
+		TORTURE_BOOT_IMAGE="$2"
 		shift
 		;;
 	--buildonly)
-		RCU_BUILDONLY=1; export RCU_BUILDONLY
+		TORTURE_BUILDONLY=1
 		;;
 	--configs)
 		checkarg --configs "(list of config files)" "$#" "$2" '^[^/]*$' '^--'
@@ -105,6 +105,11 @@ do
 		ds=$2
 		shift
 		;;
+	--defconfig)
+		checkarg --defconfig "defconfigtype" "$#" "$2" '^[^/][^/]*$' '^--'
+		TORTURE_DEFCONFIG=$2
+		shift
+		;;
 	--dryrun)
 		checkarg --dryrun "sched|script" $# "$2" 'sched\|script' '^--'
 		dryrun=$2
@@ -116,11 +121,11 @@ do
 		shift
 		;;
 	--interactive)
-		RCU_QEMU_INTERACTIVE=1; export RCU_QEMU_INTERACTIVE
+		TORTURE_QEMU_INTERACTIVE=1; export TORTURE_QEMU_INTERACTIVE
 		;;
 	--kmake-arg)
 		checkarg --kmake-arg "(kernel make arguments)" $# "$2" '.*' '^error$'
-		RCU_KMAKE_ARG="$2"; export RCU_KMAKE_ARG
+		TORTURE_KMAKE_ARG="$2"
 		shift
 		;;
 	--kversion)
@@ -130,27 +135,20 @@ do
 		;;
 	--mac)
 		checkarg --mac "(MAC address)" $# "$2" '^\([0-9a-fA-F]\{2\}:\)\{5\}[0-9a-fA-F]\{2\}$' error
-		RCU_QEMU_MAC=$2; export RCU_QEMU_MAC
+		TORTURE_QEMU_MAC=$2
 		shift
 		;;
 	--no-initrd)
-		RCU_INITRD=""; export RCU_INITRD
+		TORTURE_INITRD=""; export TORTURE_INITRD
 		;;
 	--qemu-args)
 		checkarg --qemu-args "-qemu args" $# "$2" '^-' '^error'
-		RCU_QEMU_ARG="$2"
+		TORTURE_QEMU_ARG="$2"
 		shift
 		;;
 	--qemu-cmd)
 		checkarg --qemu-cmd "(qemu-system-...)" $# "$2" 'qemu-system-' '^--'
-		RCU_QEMU_CMD="$2"; export RCU_QEMU_CMD
-		shift
-		;;
-	--relbuilddir)
-		checkarg --relbuilddir "(relative pathname)" "$#" "$2" '^[^/]*$' '^--'
-		relbuilddir=$2
-		gotrelbuilddir=1
-		builddir=${KVM}/${relbuilddir}
+		TORTURE_QEMU_CMD="$2"
 		shift
 		;;
 	--results)
@@ -184,30 +182,6 @@ then
 	resdir=$KVM/res
 fi
 
-if test "$dryrun" = ""
-then
-	if ! test -e $resdir
-	then
-		mkdir -p "$resdir" || :
-	fi
-	mkdir $resdir/$ds
-
-	# Be noisy only if running the script.
-	echo Results directory: $resdir/$ds
-	echo $scriptname $args
-
-	touch $resdir/$ds/log
-	echo $scriptname $args >> $resdir/$ds/log
-	echo ${TORTURE_SUITE} > $resdir/$ds/TORTURE_SUITE
-
-	pwd > $resdir/$ds/testid.txt
-	if test -d .git
-	then
-		git status >> $resdir/$ds/testid.txt
-		git rev-parse HEAD >> $resdir/$ds/testid.txt
-	fi
-fi
-
 # Create a file of test-name/#cpus pairs, sorted by decreasing #cpus.
 touch $T/cfgcpu
 for CF in $configs
@@ -274,7 +248,39 @@ END {
 
 # Generate a script to execute the tests in appropriate batches.
 cat << ___EOF___ > $T/script
+CONFIGFRAG="$CONFIGFRAG"; export CONFIGFRAG
+KVM="$KVM"; export KVM
+KVPATH="$KVPATH"; export KVPATH
+PATH="$PATH"; export PATH
+TORTURE_BOOT_IMAGE="$TORTURE_BOOT_IMAGE"; export TORTURE_BOOT_IMAGE
+TORTURE_BUILDONLY="$TORTURE_BUILDONLY"; export TORTURE_BUILDONLY
+TORTURE_DEFCONFIG="$TORTURE_DEFCONFIG"; export TORTURE_DEFCONFIG
+TORTURE_INITRD="$TORTURE_INITRD"; export TORTURE_INITRD
+TORTURE_KMAKE_ARG="$TORTURE_KMAKE_ARG"; export TORTURE_KMAKE_ARG
+TORTURE_QEMU_CMD="$TORTURE_QEMU_CMD"; export TORTURE_QEMU_CMD
+TORTURE_QEMU_INTERACTIVE="$TORTURE_QEMU_INTERACTIVE"; export TORTURE_QEMU_INTERACTIVE
+TORTURE_QEMU_MAC="$TORTURE_QEMU_MAC"; export TORTURE_QEMU_MAC
 TORTURE_SUITE="$TORTURE_SUITE"; export TORTURE_SUITE
+if ! test -e $resdir
+then
+	mkdir -p "$resdir" || :
+fi
+mkdir $resdir/$ds
+echo Results directory: $resdir/$ds
+echo $scriptname $args
+touch $resdir/$ds/log
+echo $scriptname $args >> $resdir/$ds/log
+echo ${TORTURE_SUITE} > $resdir/$ds/TORTURE_SUITE
+pwd > $resdir/$ds/testid.txt
+if test -d .git
+then
+	git status >> $resdir/$ds/testid.txt
+	git rev-parse HEAD >> $resdir/$ds/testid.txt
+	if ! git diff HEAD > $T/git-diff 2>&1
+	then
+		cp $T/git-diff $resdir/$ds
+	fi
+fi
 ___EOF___
 awk < $T/cfgcpu.pack \
 	-v CONFIGDIR="$CONFIGFRAG/$kversion/" \
@@ -282,8 +288,8 @@ awk < $T/cfgcpu.pack \
 	-v ncpus=$cpus \
 	-v rd=$resdir/$ds/ \
 	-v dur=$dur \
-	-v RCU_QEMU_ARG=$RCU_QEMU_ARG \
-	-v RCU_BOOTARGS=$RCU_BOOTARGS \
+	-v TORTURE_QEMU_ARG="$TORTURE_QEMU_ARG" \
+	-v TORTURE_BOOTARGS="$TORTURE_BOOTARGS" \
 'BEGIN {
 	i = 0;
 }
@@ -320,7 +326,7 @@ function dump(first, pastlast)
 		print "touch " builddir ".wait";
 		print "mkdir " builddir " > /dev/null 2>&1 || :";
 		print "mkdir " rd cfr[jn] " || :";
-		print "kvm-test-1-run.sh " CONFIGDIR cf[j], builddir, rd cfr[jn], dur " \"" RCU_QEMU_ARG "\" \"" RCU_BOOTARGS "\" > " rd cfr[jn]  "/kvm-test-1-run.sh.out 2>&1 &"
+		print "kvm-test-1-run.sh " CONFIGDIR cf[j], builddir, rd cfr[jn], dur " \"" TORTURE_QEMU_ARG "\" \"" TORTURE_BOOTARGS "\" > " rd cfr[jn]  "/kvm-test-1-run.sh.out 2>&1 &"
 		print "echo ", cfr[jn], cpusr[jn] ovf ": Waiting for build to complete. `date`";
 		print "echo ", cfr[jn], cpusr[jn] ovf ": Waiting for build to complete. `date` >> " rd "/log";
 		print "while test -f " builddir ".wait"
@@ -374,28 +380,26 @@ END {
 		dump(first, i);
 }' >> $T/script
 
+cat << ___EOF___ >> $T/script
+echo
+echo
+echo " --- `date` Test summary:"
+echo Results directory: $resdir/$ds
+if test -z "$TORTURE_BUILDONLY"
+then
+	kvm-recheck.sh $resdir/$ds
+fi
+___EOF___
+
 if test "$dryrun" = script
 then
-	# Dump out the script, but define the environment variables that
-	# it needs to run standalone.
-	echo CONFIGFRAG="$CONFIGFRAG; export CONFIGFRAG"
-	echo KVM="$KVM; export KVM"
-	echo KVPATH="$KVPATH; export KVPATH"
-	echo PATH="$PATH; export PATH"
-	echo RCU_BUILDONLY="$RCU_BUILDONLY; export RCU_BUILDONLY"
-	echo RCU_INITRD="$RCU_INITRD; export RCU_INITRD"
-	echo RCU_KMAKE_ARG="$RCU_KMAKE_ARG; export RCU_KMAKE_ARG"
-	echo RCU_QEMU_CMD="$RCU_QEMU_CMD; export RCU_QEMU_CMD"
-	echo RCU_QEMU_INTERACTIVE="$RCU_QEMU_INTERACTIVE; export RCU_QEMU_INTERACTIVE"
-	echo RCU_QEMU_MAC="$RCU_QEMU_MAC; export RCU_QEMU_MAC"
-	echo "mkdir -p "$resdir" || :"
-	echo "mkdir $resdir/$ds"
 	cat $T/script
 	exit 0
 elif test "$dryrun" = sched
 then
 	# Extract the test run schedule from the script.
-	egrep 'start batch|Starting build\.' $T/script |
+	egrep 'Start batch|Starting build\.' $T/script |
+		grep -v ">>" |
 		sed -e 's/:.*$//' -e 's/^echo //'
 	exit 0
 else
@@ -404,9 +408,3 @@ else
 fi
 
 # Tracing: trace_event=rcu:rcu_grace_period,rcu:rcu_future_grace_period,rcu:rcu_grace_period_init,rcu:rcu_nocb_wake,rcu:rcu_preempt_task,rcu:rcu_unlock_preempted_task,rcu:rcu_quiescent_state_report,rcu:rcu_fqs,rcu:rcu_callback,rcu:rcu_kfree_callback,rcu:rcu_batch_start,rcu:rcu_invoke_callback,rcu:rcu_invoke_kfree_callback,rcu:rcu_batch_end,rcu:rcu_torture_read,rcu:rcu_barrier
-
-echo
-echo
-echo " --- `date` Test summary:"
-echo Results directory: $resdir/$ds
-kvm-recheck.sh $resdir/$ds
diff --git a/tools/testing/selftests/rcutorture/bin/parse-rcutorture.sh b/tools/testing/selftests/rcutorture/bin/parse-torture.sh
index dd0a275d979..3455560ab4e 100755
--- a/tools/testing/selftests/rcutorture/bin/parse-rcutorture.sh
+++ b/tools/testing/selftests/rcutorture/bin/parse-torture.sh
@@ -1,14 +1,14 @@
 #!/bin/sh
 #
-# Check the console output from an rcutorture run for goodness.
+# Check the console output from a torture run for goodness.
 # The "file" is a pathname on the local system, and "title" is
 # a text string for error-message purposes.
 #
-# The file must contain rcutorture output, but can be interspersed
-# with other dmesg text.
+# The file must contain torture output, but can be interspersed
+# with other dmesg text, as in console-log output.
 #
 # Usage:
-#	sh parse-rcutorture.sh file title
+#	sh parse-torture.sh file title
 #
 # 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
@@ -28,7 +28,7 @@
 #
 # Authors: Paul E. McKenney <paulmck@linux.vnet.ibm.com>
 
-T=/tmp/parse-rcutorture.sh.$$
+T=/tmp/parse-torture.sh.$$
 file="$1"
 title="$2"
 
@@ -36,13 +36,13 @@ trap 'rm -f $T.seq' 0
 
 . functions.sh
 
-# check for presence of rcutorture.txt file
+# check for presence of torture output file.
 
 if test -f "$file" -a -r "$file"
 then
 	:
 else
-	echo $title unreadable rcutorture.txt file: $file
+	echo $title unreadable torture output file: $file
 	exit 1
 fi
 
@@ -76,9 +76,9 @@ BEGIN	{
 END	{
 	if (badseq) {
 		if (badseqno1 == badseqno2 && badseqno2 == ver)
-			print "RCU GP HANG at " ver " rcutorture stat " badseqnr;
+			print "GP HANG at " ver " torture stat " badseqnr;
 		else
-			print "BAD SEQ " badseqno1 ":" badseqno2 " last:" ver " RCU version " badseqnr;
+			print "BAD SEQ " badseqno1 ":" badseqno2 " last:" ver " version " badseqnr;
 	}
 	}' > $T.seq
 
@@ -91,13 +91,13 @@ then
 		exit 2
 	fi
 else
-	if grep -q RCU_HOTPLUG $file
+	if grep -q "_HOTPLUG:" $file
 	then
 		print_warning HOTPLUG FAILURES $title `cat $T.seq`
 		echo "   " $file
 		exit 3
 	fi
-	echo $title no success message, `grep --binary-files=text 'ver:' $file | wc -l` successful RCU version messages
+	echo $title no success message, `grep --binary-files=text 'ver:' $file | wc -l` successful version messages
 	if test -s $T.seq
 	then
 		print_warning $title `cat $T.seq`
diff --git a/tools/testing/selftests/rcutorture/configs/rcu/TREE02-T b/tools/testing/selftests/rcutorture/configs/rcu/TREE02-T
new file mode 100644
index 00000000000..61c8d9ce5bb
--- /dev/null
+++ b/tools/testing/selftests/rcutorture/configs/rcu/TREE02-T
@@ -0,0 +1,25 @@
+CONFIG_SMP=y
+CONFIG_NR_CPUS=8
+CONFIG_PREEMPT_NONE=n
+CONFIG_PREEMPT_VOLUNTARY=n
+CONFIG_PREEMPT=y
+#CHECK#CONFIG_TREE_PREEMPT_RCU=y
+CONFIG_HZ_PERIODIC=n
+CONFIG_NO_HZ_IDLE=y
+CONFIG_NO_HZ_FULL=n
+CONFIG_RCU_FAST_NO_HZ=n
+CONFIG_RCU_TRACE=y
+CONFIG_HOTPLUG_CPU=n
+CONFIG_SUSPEND=n
+CONFIG_HIBERNATION=n
+CONFIG_RCU_FANOUT=3
+CONFIG_RCU_FANOUT_LEAF=3
+CONFIG_RCU_FANOUT_EXACT=n
+CONFIG_RCU_NOCB_CPU=n
+CONFIG_DEBUG_LOCK_ALLOC=y
+CONFIG_PROVE_LOCKING=n
+CONFIG_PROVE_RCU_DELAY=n
+CONFIG_RCU_CPU_STALL_INFO=n
+CONFIG_RCU_CPU_STALL_VERBOSE=y
+CONFIG_RCU_BOOST=n
+CONFIG_DEBUG_OBJECTS_RCU_HEAD=n
diff --git a/tools/testing/selftests/rcutorture/configs/rcu/TREE08.boot b/tools/testing/selftests/rcutorture/configs/rcu/TREE08.boot
new file mode 100644
index 00000000000..3b42b8b033c
--- /dev/null
+++ b/tools/testing/selftests/rcutorture/configs/rcu/TREE08.boot
@@ -0,0 +1 @@
+rcutorture.torture_type=sched
diff --git a/tools/usb/ffs-aio-example/multibuff/device_app/aio_multibuff.c b/tools/usb/ffs-aio-example/multibuff/device_app/aio_multibuff.c
new file mode 100644
index 00000000000..87216a0c4a8
--- /dev/null
+++ b/tools/usb/ffs-aio-example/multibuff/device_app/aio_multibuff.c
@@ -0,0 +1,349 @@
+#define _BSD_SOURCE /* for endian.h */
+
+#include <endian.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <stdarg.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <sys/poll.h>
+#include <unistd.h>
+#include <stdbool.h>
+#include <sys/eventfd.h>
+
+#include "libaio.h"
+#define IOCB_FLAG_RESFD         (1 << 0)
+
+#include <linux/usb/functionfs.h>
+
+#define BUF_LEN		8192
+#define BUFS_MAX	128
+#define AIO_MAX		(BUFS_MAX*2)
+
+/******************** Descriptors and Strings *******************************/
+
+static const struct {
+	struct usb_functionfs_descs_head header;
+	struct {
+		struct usb_interface_descriptor intf;
+		struct usb_endpoint_descriptor_no_audio bulk_sink;
+		struct usb_endpoint_descriptor_no_audio bulk_source;
+	} __attribute__ ((__packed__)) fs_descs, hs_descs;
+} __attribute__ ((__packed__)) descriptors = {
+	.header = {
+		.magic = htole32(FUNCTIONFS_DESCRIPTORS_MAGIC),
+		.length = htole32(sizeof(descriptors)),
+		.fs_count = 3,
+		.hs_count = 3,
+	},
+	.fs_descs = {
+		.intf = {
+			.bLength = sizeof(descriptors.fs_descs.intf),
+			.bDescriptorType = USB_DT_INTERFACE,
+			.bNumEndpoints = 2,
+			.bInterfaceClass = USB_CLASS_VENDOR_SPEC,
+			.iInterface = 1,
+		},
+		.bulk_sink = {
+			.bLength = sizeof(descriptors.fs_descs.bulk_sink),
+			.bDescriptorType = USB_DT_ENDPOINT,
+			.bEndpointAddress = 1 | USB_DIR_IN,
+			.bmAttributes = USB_ENDPOINT_XFER_BULK,
+		},
+		.bulk_source = {
+			.bLength = sizeof(descriptors.fs_descs.bulk_source),
+			.bDescriptorType = USB_DT_ENDPOINT,
+			.bEndpointAddress = 2 | USB_DIR_OUT,
+			.bmAttributes = USB_ENDPOINT_XFER_BULK,
+		},
+	},
+	.hs_descs = {
+		.intf = {
+			.bLength = sizeof(descriptors.hs_descs.intf),
+			.bDescriptorType = USB_DT_INTERFACE,
+			.bNumEndpoints = 2,
+			.bInterfaceClass = USB_CLASS_VENDOR_SPEC,
+			.iInterface = 1,
+		},
+		.bulk_sink = {
+			.bLength = sizeof(descriptors.hs_descs.bulk_sink),
+			.bDescriptorType = USB_DT_ENDPOINT,
+			.bEndpointAddress = 1 | USB_DIR_IN,
+			.bmAttributes = USB_ENDPOINT_XFER_BULK,
+			.wMaxPacketSize = htole16(512),
+		},
+		.bulk_source = {
+			.bLength = sizeof(descriptors.hs_descs.bulk_source),
+			.bDescriptorType = USB_DT_ENDPOINT,
+			.bEndpointAddress = 2 | USB_DIR_OUT,
+			.bmAttributes = USB_ENDPOINT_XFER_BULK,
+			.wMaxPacketSize = htole16(512),
+		},
+	},
+};
+
+#define STR_INTERFACE "AIO Test"
+
+static const struct {
+	struct usb_functionfs_strings_head header;
+	struct {
+		__le16 code;
+		const char str1[sizeof(STR_INTERFACE)];
+	} __attribute__ ((__packed__)) lang0;
+} __attribute__ ((__packed__)) strings = {
+	.header = {
+		.magic = htole32(FUNCTIONFS_STRINGS_MAGIC),
+		.length = htole32(sizeof(strings)),
+		.str_count = htole32(1),
+		.lang_count = htole32(1),
+	},
+	.lang0 = {
+		htole16(0x0409), /* en-us */
+		STR_INTERFACE,
+	},
+};
+
+/********************** Buffer structure *******************************/
+
+struct io_buffer {
+	struct iocb **iocb;
+	unsigned char **buf;
+	unsigned cnt;
+	unsigned len;
+	unsigned requested;
+};
+
+/******************** Endpoints handling *******************************/
+
+static void display_event(struct usb_functionfs_event *event)
+{
+	static const char *const names[] = {
+		[FUNCTIONFS_BIND] = "BIND",
+		[FUNCTIONFS_UNBIND] = "UNBIND",
+		[FUNCTIONFS_ENABLE] = "ENABLE",
+		[FUNCTIONFS_DISABLE] = "DISABLE",
+		[FUNCTIONFS_SETUP] = "SETUP",
+		[FUNCTIONFS_SUSPEND] = "SUSPEND",
+		[FUNCTIONFS_RESUME] = "RESUME",
+	};
+	switch (event->type) {
+	case FUNCTIONFS_BIND:
+	case FUNCTIONFS_UNBIND:
+	case FUNCTIONFS_ENABLE:
+	case FUNCTIONFS_DISABLE:
+	case FUNCTIONFS_SETUP:
+	case FUNCTIONFS_SUSPEND:
+	case FUNCTIONFS_RESUME:
+		printf("Event %s\n", names[event->type]);
+	}
+}
+
+static void handle_ep0(int ep0, bool *ready)
+{
+	int ret;
+	struct usb_functionfs_event event;
+
+	ret = read(ep0, &event, sizeof(event));
+	if (!ret) {
+		perror("unable to read event from ep0");
+		return;
+	}
+	display_event(&event);
+	switch (event.type) {
+	case FUNCTIONFS_SETUP:
+		if (event.u.setup.bRequestType & USB_DIR_IN)
+			write(ep0, NULL, 0);
+		else
+			read(ep0, NULL, 0);
+		break;
+
+	case FUNCTIONFS_ENABLE:
+		*ready = true;
+		break;
+
+	case FUNCTIONFS_DISABLE:
+		*ready = false;
+		break;
+
+	default:
+		break;
+	}
+}
+
+void init_bufs(struct io_buffer *iobuf, unsigned n, unsigned len)
+{
+	unsigned i;
+	iobuf->buf = malloc(n*sizeof(*iobuf->buf));
+	iobuf->iocb = malloc(n*sizeof(*iobuf->iocb));
+	iobuf->cnt = n;
+	iobuf->len = len;
+	iobuf->requested = 0;
+	for (i = 0; i < n; ++i) {
+		iobuf->buf[i] = malloc(len*sizeof(**iobuf->buf));
+		iobuf->iocb[i] = malloc(sizeof(**iobuf->iocb));
+	}
+	iobuf->cnt = n;
+}
+
+void delete_bufs(struct io_buffer *iobuf)
+{
+	unsigned i;
+	for (i = 0; i < iobuf->cnt; ++i) {
+		free(iobuf->buf[i]);
+		free(iobuf->iocb[i]);
+	}
+	free(iobuf->buf);
+	free(iobuf->iocb);
+}
+
+int main(int argc, char *argv[])
+{
+	int ret;
+	unsigned i, j;
+	char *ep_path;
+
+	int ep0, ep1;
+
+	io_context_t ctx;
+
+	int evfd;
+	fd_set rfds;
+
+	struct io_buffer iobuf[2];
+	int actual = 0;
+	bool ready;
+
+	if (argc != 2) {
+		printf("ffs directory not specified!\n");
+		return 1;
+	}
+
+	ep_path = malloc(strlen(argv[1]) + 4 /* "/ep#" */ + 1 /* '\0' */);
+	if (!ep_path) {
+		perror("malloc");
+		return 1;
+	}
+
+	/* open endpoint files */
+	sprintf(ep_path, "%s/ep0", argv[1]);
+	ep0 = open(ep_path, O_RDWR);
+	if (ep0 < 0) {
+		perror("unable to open ep0");
+		return 1;
+	}
+	if (write(ep0, &descriptors, sizeof(descriptors)) < 0) {
+		perror("unable do write descriptors");
+		return 1;
+	}
+	if (write(ep0, &strings, sizeof(strings)) < 0) {
+		perror("unable to write strings");
+		return 1;
+	}
+	sprintf(ep_path, "%s/ep1", argv[1]);
+	ep1 = open(ep_path, O_RDWR);
+	if (ep1 < 0) {
+		perror("unable to open ep1");
+		return 1;
+	}
+
+	free(ep_path);
+
+	memset(&ctx, 0, sizeof(ctx));
+	/* setup aio context to handle up to AIO_MAX requests */
+	if (io_setup(AIO_MAX, &ctx) < 0) {
+		perror("unable to setup aio");
+		return 1;
+	}
+
+	evfd = eventfd(0, 0);
+	if (evfd < 0) {
+		perror("unable to open eventfd");
+		return 1;
+	}
+
+	for (i = 0; i < sizeof(iobuf)/sizeof(*iobuf); ++i)
+		init_bufs(&iobuf[i], BUFS_MAX, BUF_LEN);
+
+	while (1) {
+		FD_ZERO(&rfds);
+		FD_SET(ep0, &rfds);
+		FD_SET(evfd, &rfds);
+
+		ret = select(((ep0 > evfd) ? ep0 : evfd)+1,
+			     &rfds, NULL, NULL, NULL);
+		if (ret < 0) {
+			if (errno == EINTR)
+				continue;
+			perror("select");
+			break;
+		}
+
+		if (FD_ISSET(ep0, &rfds))
+			handle_ep0(ep0, &ready);
+
+		/* we are waiting for function ENABLE */
+		if (!ready)
+			continue;
+
+		/*
+		 * when we're preparing new data to submit,
+		 * second buffer being transmitted
+		 */
+		for (i = 0; i < sizeof(iobuf)/sizeof(*iobuf); ++i) {
+			if (iobuf[i].requested)
+				continue;
+			/* prepare requests */
+			for (j = 0; j < iobuf[i].cnt; ++j) {
+				io_prep_pwrite(iobuf[i].iocb[j], ep1,
+					       iobuf[i].buf[j],
+					       iobuf[i].len, 0);
+				/* enable eventfd notification */
+				iobuf[i].iocb[j]->u.c.flags |= IOCB_FLAG_RESFD;
+				iobuf[i].iocb[j]->u.c.resfd = evfd;
+			}
+			/* submit table of requests */
+			ret = io_submit(ctx, iobuf[i].cnt, iobuf[i].iocb);
+			if (ret >= 0) {
+				iobuf[i].requested = ret;
+				printf("submit: %d requests buf: %d\n", ret, i);
+			} else
+				perror("unable to submit reqests");
+		}
+
+		/* if event is ready to read */
+		if (!FD_ISSET(evfd, &rfds))
+			continue;
+
+		uint64_t ev_cnt;
+		ret = read(evfd, &ev_cnt, sizeof(ev_cnt));
+		if (ret < 0) {
+			perror("unable to read eventfd");
+			break;
+		}
+
+		struct io_event e[BUFS_MAX];
+		/* we read aio events */
+		ret = io_getevents(ctx, 1, BUFS_MAX, e, NULL);
+		if (ret > 0) /* if we got events */
+			iobuf[actual].requested -= ret;
+
+		/* if all req's from iocb completed */
+		if (!iobuf[actual].requested)
+			actual = (actual + 1)%(sizeof(iobuf)/sizeof(*iobuf));
+	}
+
+	/* free resources */
+
+	for (i = 0; i < sizeof(iobuf)/sizeof(*iobuf); ++i)
+		delete_bufs(&iobuf[i]);
+	io_destroy(ctx);
+
+	close(ep1);
+	close(ep0);
+
+	return 0;
+}
diff --git a/tools/usb/ffs-aio-example/multibuff/host_app/Makefile b/tools/usb/ffs-aio-example/multibuff/host_app/Makefile
new file mode 100644
index 00000000000..8c4a6f0aa82
--- /dev/null
+++ b/tools/usb/ffs-aio-example/multibuff/host_app/Makefile
@@ -0,0 +1,13 @@
+CC = gcc
+LIBUSB_CFLAGS = $(shell pkg-config --cflags libusb-1.0)
+LIBUSB_LIBS = $(shell pkg-config --libs libusb-1.0)
+WARNINGS = -Wall -Wextra
+CFLAGS = $(LIBUSB_CFLAGS) $(WARNINGS)
+LDFLAGS = $(LIBUSB_LIBS)
+
+all: test
+%: %.c
+	$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
+
+clean:
+	$(RM) test
diff --git a/tools/usb/ffs-aio-example/multibuff/host_app/test.c b/tools/usb/ffs-aio-example/multibuff/host_app/test.c
new file mode 100644
index 00000000000..b0ad8747d03
--- /dev/null
+++ b/tools/usb/ffs-aio-example/multibuff/host_app/test.c
@@ -0,0 +1,146 @@
+#include <libusb.h>
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+
+#define VENDOR	0x1d6b
+#define PRODUCT	0x0105
+
+/* endpoints indexes */
+
+#define EP_BULK_IN	(1 | LIBUSB_ENDPOINT_IN)
+#define EP_BULK_OUT	(2 | LIBUSB_ENDPOINT_OUT)
+
+#define BUF_LEN		8192
+
+/*
+ * struct test_state - describes test program state
+ * @list: list of devices returned by libusb_get_device_list function
+ * @found: pointer to struct describing tested device
+ * @ctx: context, set to NULL
+ * @handle: handle of tested device
+ * @attached: indicates that device was attached to kernel, and has to be
+ *            reattached at the end of test program
+ */
+
+struct test_state {
+	libusb_device *found;
+	libusb_context *ctx;
+	libusb_device_handle *handle;
+	int attached;
+};
+
+/*
+ * test_init - initialize test program
+ */
+
+int test_init(struct test_state *state)
+{
+	int i, ret;
+	ssize_t cnt;
+	libusb_device **list;
+
+	state->found = NULL;
+	state->ctx = NULL;
+	state->handle = NULL;
+	state->attached = 0;
+
+	ret = libusb_init(&state->ctx);
+	if (ret) {
+		printf("cannot init libusb: %s\n", libusb_error_name(ret));
+		return 1;
+	}
+
+	cnt = libusb_get_device_list(state->ctx, &list);
+	if (cnt <= 0) {
+		printf("no devices found\n");
+		goto error1;
+	}
+
+	for (i = 0; i < cnt; ++i) {
+		libusb_device *dev = list[i];
+		struct libusb_device_descriptor desc;
+		ret = libusb_get_device_descriptor(dev, &desc);
+		if (ret) {
+			printf("unable to get device descriptor: %s\n",
+			       libusb_error_name(ret));
+			goto error2;
+		}
+		if (desc.idVendor == VENDOR && desc.idProduct == PRODUCT) {
+			state->found = dev;
+			break;
+		}
+	}
+
+	if (!state->found) {
+		printf("no devices found\n");
+		goto error2;
+	}
+
+	ret = libusb_open(state->found, &state->handle);
+	if (ret) {
+		printf("cannot open device: %s\n", libusb_error_name(ret));
+		goto error2;
+	}
+
+	if (libusb_claim_interface(state->handle, 0)) {
+		ret = libusb_detach_kernel_driver(state->handle, 0);
+		if (ret) {
+			printf("unable to detach kernel driver: %s\n",
+			       libusb_error_name(ret));
+			goto error3;
+		}
+		state->attached = 1;
+		ret = libusb_claim_interface(state->handle, 0);
+		if (ret) {
+			printf("cannot claim interface: %s\n",
+			       libusb_error_name(ret));
+			goto error4;
+		}
+	}
+
+	return 0;
+
+error4:
+	if (state->attached == 1)
+		libusb_attach_kernel_driver(state->handle, 0);
+
+error3:
+	libusb_close(state->handle);
+
+error2:
+	libusb_free_device_list(list, 1);
+
+error1:
+	libusb_exit(state->ctx);
+	return 1;
+}
+
+/*
+ * test_exit - cleanup test program
+ */
+
+void test_exit(struct test_state *state)
+{
+	libusb_release_interface(state->handle, 0);
+	if (state->attached == 1)
+		libusb_attach_kernel_driver(state->handle, 0);
+	libusb_close(state->handle);
+	libusb_exit(state->ctx);
+}
+
+int main(void)
+{
+	struct test_state state;
+
+	if (test_init(&state))
+		return 1;
+
+	while (1) {
+		static unsigned char buffer[BUF_LEN];
+		int bytes;
+		libusb_bulk_transfer(state.handle, EP_BULK_IN, buffer, BUF_LEN,
+				     &bytes, 500);
+	}
+	test_exit(&state);
+}
diff --git a/tools/usb/ffs-aio-example/simple/device_app/aio_simple.c b/tools/usb/ffs-aio-example/simple/device_app/aio_simple.c
new file mode 100644
index 00000000000..f558664a331
--- /dev/null
+++ b/tools/usb/ffs-aio-example/simple/device_app/aio_simple.c
@@ -0,0 +1,335 @@
+#define _BSD_SOURCE /* for endian.h */
+
+#include <endian.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <stdarg.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <sys/poll.h>
+#include <unistd.h>
+#include <stdbool.h>
+#include <sys/eventfd.h>
+
+#include "libaio.h"
+#define IOCB_FLAG_RESFD         (1 << 0)
+
+#include <linux/usb/functionfs.h>
+
+#define BUF_LEN		8192
+
+/******************** Descriptors and Strings *******************************/
+
+static const struct {
+	struct usb_functionfs_descs_head header;
+	struct {
+		struct usb_interface_descriptor intf;
+		struct usb_endpoint_descriptor_no_audio bulk_sink;
+		struct usb_endpoint_descriptor_no_audio bulk_source;
+	} __attribute__ ((__packed__)) fs_descs, hs_descs;
+} __attribute__ ((__packed__)) descriptors = {
+	.header = {
+		.magic = htole32(FUNCTIONFS_DESCRIPTORS_MAGIC),
+		.length = htole32(sizeof(descriptors)),
+		.fs_count = 3,
+		.hs_count = 3,
+	},
+	.fs_descs = {
+		.intf = {
+			.bLength = sizeof(descriptors.fs_descs.intf),
+			.bDescriptorType = USB_DT_INTERFACE,
+			.bNumEndpoints = 2,
+			.bInterfaceClass = USB_CLASS_VENDOR_SPEC,
+			.iInterface = 1,
+		},
+		.bulk_sink = {
+			.bLength = sizeof(descriptors.fs_descs.bulk_sink),
+			.bDescriptorType = USB_DT_ENDPOINT,
+			.bEndpointAddress = 1 | USB_DIR_IN,
+			.bmAttributes = USB_ENDPOINT_XFER_BULK,
+		},
+		.bulk_source = {
+			.bLength = sizeof(descriptors.fs_descs.bulk_source),
+			.bDescriptorType = USB_DT_ENDPOINT,
+			.bEndpointAddress = 2 | USB_DIR_OUT,
+			.bmAttributes = USB_ENDPOINT_XFER_BULK,
+		},
+	},
+	.hs_descs = {
+		.intf = {
+			.bLength = sizeof(descriptors.hs_descs.intf),
+			.bDescriptorType = USB_DT_INTERFACE,
+			.bNumEndpoints = 2,
+			.bInterfaceClass = USB_CLASS_VENDOR_SPEC,
+			.iInterface = 1,
+		},
+		.bulk_sink = {
+			.bLength = sizeof(descriptors.hs_descs.bulk_sink),
+			.bDescriptorType = USB_DT_ENDPOINT,
+			.bEndpointAddress = 1 | USB_DIR_IN,
+			.bmAttributes = USB_ENDPOINT_XFER_BULK,
+		},
+		.bulk_source = {
+			.bLength = sizeof(descriptors.hs_descs.bulk_source),
+			.bDescriptorType = USB_DT_ENDPOINT,
+			.bEndpointAddress = 2 | USB_DIR_OUT,
+			.bmAttributes = USB_ENDPOINT_XFER_BULK,
+		},
+	},
+};
+
+#define STR_INTERFACE "AIO Test"
+
+static const struct {
+	struct usb_functionfs_strings_head header;
+	struct {
+		__le16 code;
+		const char str1[sizeof(STR_INTERFACE)];
+	} __attribute__ ((__packed__)) lang0;
+} __attribute__ ((__packed__)) strings = {
+	.header = {
+		.magic = htole32(FUNCTIONFS_STRINGS_MAGIC),
+		.length = htole32(sizeof(strings)),
+		.str_count = htole32(1),
+		.lang_count = htole32(1),
+	},
+	.lang0 = {
+		htole16(0x0409), /* en-us */
+		STR_INTERFACE,
+	},
+};
+
+/******************** Endpoints handling *******************************/
+
+static void display_event(struct usb_functionfs_event *event)
+{
+	static const char *const names[] = {
+		[FUNCTIONFS_BIND] = "BIND",
+		[FUNCTIONFS_UNBIND] = "UNBIND",
+		[FUNCTIONFS_ENABLE] = "ENABLE",
+		[FUNCTIONFS_DISABLE] = "DISABLE",
+		[FUNCTIONFS_SETUP] = "SETUP",
+		[FUNCTIONFS_SUSPEND] = "SUSPEND",
+		[FUNCTIONFS_RESUME] = "RESUME",
+	};
+	switch (event->type) {
+	case FUNCTIONFS_BIND:
+	case FUNCTIONFS_UNBIND:
+	case FUNCTIONFS_ENABLE:
+	case FUNCTIONFS_DISABLE:
+	case FUNCTIONFS_SETUP:
+	case FUNCTIONFS_SUSPEND:
+	case FUNCTIONFS_RESUME:
+		printf("Event %s\n", names[event->type]);
+	}
+}
+
+static void handle_ep0(int ep0, bool *ready)
+{
+	struct usb_functionfs_event event;
+	int ret;
+
+	struct pollfd pfds[1];
+	pfds[0].fd = ep0;
+	pfds[0].events = POLLIN;
+
+	ret = poll(pfds, 1, 0);
+
+	if (ret && (pfds[0].revents & POLLIN)) {
+		ret = read(ep0, &event, sizeof(event));
+		if (!ret) {
+			perror("unable to read event from ep0");
+			return;
+		}
+		display_event(&event);
+		switch (event.type) {
+		case FUNCTIONFS_SETUP:
+			if (event.u.setup.bRequestType & USB_DIR_IN)
+				write(ep0, NULL, 0);
+			else
+				read(ep0, NULL, 0);
+			break;
+
+		case FUNCTIONFS_ENABLE:
+			*ready = true;
+			break;
+
+		case FUNCTIONFS_DISABLE:
+			*ready = false;
+			break;
+
+		default:
+			break;
+		}
+	}
+}
+
+int main(int argc, char *argv[])
+{
+	int i, ret;
+	char *ep_path;
+
+	int ep0;
+	int ep[2];
+
+	io_context_t ctx;
+
+	int evfd;
+	fd_set rfds;
+
+	char *buf_in, *buf_out;
+	struct iocb *iocb_in, *iocb_out;
+	int req_in = 0, req_out = 0;
+	bool ready;
+
+	if (argc != 2) {
+		printf("ffs directory not specified!\n");
+		return 1;
+	}
+
+	ep_path = malloc(strlen(argv[1]) + 4 /* "/ep#" */ + 1 /* '\0' */);
+	if (!ep_path) {
+		perror("malloc");
+		return 1;
+	}
+
+	/* open endpoint files */
+	sprintf(ep_path, "%s/ep0", argv[1]);
+	ep0 = open(ep_path, O_RDWR);
+	if (ep0 < 0) {
+		perror("unable to open ep0");
+		return 1;
+	}
+	if (write(ep0, &descriptors, sizeof(descriptors)) < 0) {
+		perror("unable do write descriptors");
+		return 1;
+	}
+	if (write(ep0, &strings, sizeof(strings)) < 0) {
+		perror("unable to write strings");
+		return 1;
+	}
+	for (i = 0; i < 2; ++i) {
+		sprintf(ep_path, "%s/ep%d", argv[1], i+1);
+		ep[i] = open(ep_path, O_RDWR);
+		if (ep[i] < 0) {
+			printf("unable to open ep%d: %s\n", i+1,
+			       strerror(errno));
+			return 1;
+		}
+	}
+
+	free(ep_path);
+
+	memset(&ctx, 0, sizeof(ctx));
+	/* setup aio context to handle up to 2 requests */
+	if (io_setup(2, &ctx) < 0) {
+		perror("unable to setup aio");
+		return 1;
+	}
+
+	evfd = eventfd(0, 0);
+	if (evfd < 0) {
+		perror("unable to open eventfd");
+		return 1;
+	}
+
+	/* alloc buffers and requests */
+	buf_in = malloc(BUF_LEN);
+	buf_out = malloc(BUF_LEN);
+	iocb_in = malloc(sizeof(*iocb_in));
+	iocb_out = malloc(sizeof(*iocb_out));
+
+	while (1) {
+		FD_ZERO(&rfds);
+		FD_SET(ep0, &rfds);
+		FD_SET(evfd, &rfds);
+
+		ret = select(((ep0 > evfd) ? ep0 : evfd)+1,
+			     &rfds, NULL, NULL, NULL);
+		if (ret < 0) {
+			if (errno == EINTR)
+				continue;
+			perror("select");
+			break;
+		}
+
+		if (FD_ISSET(ep0, &rfds))
+			handle_ep0(ep0, &ready);
+
+		/* we are waiting for function ENABLE */
+		if (!ready)
+			continue;
+
+		/* if something was submitted we wait for event */
+		if (FD_ISSET(evfd, &rfds)) {
+			uint64_t ev_cnt;
+			ret = read(evfd, &ev_cnt, sizeof(ev_cnt));
+			if (ret < 0) {
+				perror("unable to read eventfd");
+				break;
+			}
+
+			struct io_event e[2];
+			/* we wait for one event */
+			ret = io_getevents(ctx, 1, 2, e, NULL);
+			/* if we got event */
+			for (i = 0; i < ret; ++i) {
+				if (e[i].obj->aio_fildes == ep[0]) {
+					printf("ev=in; ret=%lu\n", e[i].res);
+					req_in = 0;
+				} else if (e[i].obj->aio_fildes == ep[1]) {
+					printf("ev=out; ret=%lu\n", e[i].res);
+					req_out = 0;
+				}
+			}
+		}
+
+		if (!req_in) { /* if IN transfer not requested*/
+			/* prepare write request */
+			io_prep_pwrite(iocb_in, ep[0], buf_in, BUF_LEN, 0);
+			/* enable eventfd notification */
+			iocb_in->u.c.flags |= IOCB_FLAG_RESFD;
+			iocb_in->u.c.resfd = evfd;
+			/* submit table of requests */
+			ret = io_submit(ctx, 1, &iocb_in);
+			if (ret >= 0) { /* if ret > 0 request is queued */
+				req_in = 1;
+				printf("submit: in\n");
+			} else
+				perror("unable to submit request");
+		}
+		if (!req_out) { /* if OUT transfer not requested */
+			/* prepare read request */
+			io_prep_pread(iocb_out, ep[1], buf_out, BUF_LEN, 0);
+			/* enable eventfs notification */
+			iocb_out->u.c.flags |= IOCB_FLAG_RESFD;
+			iocb_out->u.c.resfd = evfd;
+			/* submit table of requests */
+			ret = io_submit(ctx, 1, &iocb_out);
+			if (ret >= 0) { /* if ret > 0 request is queued */
+				req_out = 1;
+				printf("submit: out\n");
+			} else
+				perror("unable to submit request");
+		}
+	}
+
+	/* free resources */
+
+	io_destroy(ctx);
+
+	free(buf_in);
+	free(buf_out);
+	free(iocb_in);
+	free(iocb_out);
+
+	for (i = 0; i < 2; ++i)
+		close(ep[i]);
+	close(ep0);
+
+	return 0;
+}
diff --git a/tools/usb/ffs-aio-example/simple/host_app/Makefile b/tools/usb/ffs-aio-example/simple/host_app/Makefile
new file mode 100644
index 00000000000..8c4a6f0aa82
--- /dev/null
+++ b/tools/usb/ffs-aio-example/simple/host_app/Makefile
@@ -0,0 +1,13 @@
+CC = gcc
+LIBUSB_CFLAGS = $(shell pkg-config --cflags libusb-1.0)
+LIBUSB_LIBS = $(shell pkg-config --libs libusb-1.0)
+WARNINGS = -Wall -Wextra
+CFLAGS = $(LIBUSB_CFLAGS) $(WARNINGS)
+LDFLAGS = $(LIBUSB_LIBS)
+
+all: test
+%: %.c
+	$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
+
+clean:
+	$(RM) test
diff --git a/tools/usb/ffs-aio-example/simple/host_app/test.c b/tools/usb/ffs-aio-example/simple/host_app/test.c
new file mode 100644
index 00000000000..64b6a57d8ca
--- /dev/null
+++ b/tools/usb/ffs-aio-example/simple/host_app/test.c
@@ -0,0 +1,148 @@
+#include <libusb.h>
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+
+#define VENDOR	0x1d6b
+#define PRODUCT	0x0105
+
+/* endpoints indexes */
+
+#define EP_BULK_IN	(1 | LIBUSB_ENDPOINT_IN)
+#define EP_BULK_OUT	(2 | LIBUSB_ENDPOINT_OUT)
+
+#define BUF_LEN		8192
+
+/*
+ * struct test_state - describes test program state
+ * @list: list of devices returned by libusb_get_device_list function
+ * @found: pointer to struct describing tested device
+ * @ctx: context, set to NULL
+ * @handle: handle of tested device
+ * @attached: indicates that device was attached to kernel, and has to be
+ *            reattached at the end of test program
+ */
+
+struct test_state {
+	libusb_device *found;
+	libusb_context *ctx;
+	libusb_device_handle *handle;
+	int attached;
+};
+
+/*
+ * test_init - initialize test program
+ */
+
+int test_init(struct test_state *state)
+{
+	int i, ret;
+	ssize_t cnt;
+	libusb_device **list;
+
+	state->found = NULL;
+	state->ctx = NULL;
+	state->handle = NULL;
+	state->attached = 0;
+
+	ret = libusb_init(&state->ctx);
+	if (ret) {
+		printf("cannot init libusb: %s\n", libusb_error_name(ret));
+		return 1;
+	}
+
+	cnt = libusb_get_device_list(state->ctx, &list);
+	if (cnt <= 0) {
+		printf("no devices found\n");
+		goto error1;
+	}
+
+	for (i = 0; i < cnt; ++i) {
+		libusb_device *dev = list[i];
+		struct libusb_device_descriptor desc;
+		ret = libusb_get_device_descriptor(dev, &desc);
+		if (ret) {
+			printf("unable to get device descriptor: %s\n",
+			       libusb_error_name(ret));
+			goto error2;
+		}
+		if (desc.idVendor == VENDOR && desc.idProduct == PRODUCT) {
+			state->found = dev;
+			break;
+		}
+	}
+
+	if (!state->found) {
+		printf("no devices found\n");
+		goto error2;
+	}
+
+	ret = libusb_open(state->found, &state->handle);
+	if (ret) {
+		printf("cannot open device: %s\n", libusb_error_name(ret));
+		goto error2;
+	}
+
+	if (libusb_claim_interface(state->handle, 0)) {
+		ret = libusb_detach_kernel_driver(state->handle, 0);
+		if (ret) {
+			printf("unable to detach kernel driver: %s\n",
+			       libusb_error_name(ret));
+			goto error3;
+		}
+		state->attached = 1;
+		ret = libusb_claim_interface(state->handle, 0);
+		if (ret) {
+			printf("cannot claim interface: %s\n",
+			       libusb_error_name(ret));
+			goto error4;
+		}
+	}
+
+	return 0;
+
+error4:
+	if (state->attached == 1)
+		libusb_attach_kernel_driver(state->handle, 0);
+
+error3:
+	libusb_close(state->handle);
+
+error2:
+	libusb_free_device_list(list, 1);
+
+error1:
+	libusb_exit(state->ctx);
+	return 1;
+}
+
+/*
+ * test_exit - cleanup test program
+ */
+
+void test_exit(struct test_state *state)
+{
+	libusb_release_interface(state->handle, 0);
+	if (state->attached == 1)
+		libusb_attach_kernel_driver(state->handle, 0);
+	libusb_close(state->handle);
+	libusb_exit(state->ctx);
+}
+
+int main(void)
+{
+	struct test_state state;
+
+	if (test_init(&state))
+		return 1;
+
+	while (1) {
+		static unsigned char buffer[BUF_LEN];
+		int bytes;
+		libusb_bulk_transfer(state.handle, EP_BULK_IN, buffer, BUF_LEN,
+				     &bytes, 500);
+		libusb_bulk_transfer(state.handle, EP_BULK_OUT, buffer, BUF_LEN,
+				     &bytes, 500);
+	}
+	test_exit(&state);
+}
diff --git a/virt/kvm/arm/vgic.c b/virt/kvm/arm/vgic.c
index 47b29834a6b..56ff9bebb57 100644
--- a/virt/kvm/arm/vgic.c
+++ b/virt/kvm/arm/vgic.c
@@ -548,11 +548,10 @@ static bool handle_mmio_cfg_reg(struct kvm_vcpu *vcpu,
 	u32 val;
 	u32 *reg;
 
-	offset >>= 1;
 	reg = vgic_bitmap_get_reg(&vcpu->kvm->arch.vgic.irq_cfg,
-				  vcpu->vcpu_id, offset);
+				  vcpu->vcpu_id, offset >> 1);
 
-	if (offset & 2)
+	if (offset & 4)
 		val = *reg >> 16;
 	else
 		val = *reg & 0xffff;
@@ -561,13 +560,13 @@ static bool handle_mmio_cfg_reg(struct kvm_vcpu *vcpu,
 	vgic_reg_access(mmio, &val, offset,
 			ACCESS_READ_VALUE | ACCESS_WRITE_VALUE);
 	if (mmio->is_write) {
-		if (offset < 4) {
+		if (offset < 8) {
 			*reg = ~0U; /* Force PPIs/SGIs to 1 */
 			return false;
 		}
 
 		val = vgic_cfg_compress(val);
-		if (offset & 2) {
+		if (offset & 4) {
 			*reg &= 0xffff;
 			*reg |= val << 16;
 		} else {
@@ -916,6 +915,7 @@ static void vgic_dispatch_sgi(struct kvm_vcpu *vcpu, u32 reg)
 	case 0:
 		if (!target_cpus)
 			return;
+		break;
 
 	case 1:
 		target_cpus = ((1 << nrcpus) - 1) & ~(1 << vcpu_id) & 0xff;
@@ -1667,10 +1667,11 @@ static int vgic_ioaddr_assign(struct kvm *kvm, phys_addr_t *ioaddr,
 	if (addr + size < addr)
 		return -EINVAL;
 
+	*ioaddr = addr;
 	ret = vgic_ioaddr_overlap(kvm);
 	if (ret)
-		return ret;
-	*ioaddr = addr;
+		*ioaddr = VGIC_ADDR_UNDEF;
+
 	return ret;
 }
 
diff --git a/virt/kvm/assigned-dev.c b/virt/kvm/assigned-dev.c
index 8db43701016..bf06577fea5 100644
--- a/virt/kvm/assigned-dev.c
+++ b/virt/kvm/assigned-dev.c
@@ -395,7 +395,8 @@ static int assigned_device_enable_host_msix(struct kvm *kvm,
 	if (dev->entries_nr == 0)
 		return r;
 
-	r = pci_enable_msix(dev->dev, dev->host_msix_entries, dev->entries_nr);
+	r = pci_enable_msix_exact(dev->dev,
+				  dev->host_msix_entries, dev->entries_nr);
 	if (r)
 		return r;
 
diff --git a/virt/kvm/async_pf.c b/virt/kvm/async_pf.c
index 10df100c451..06e6401d6ef 100644
--- a/virt/kvm/async_pf.c
+++ b/virt/kvm/async_pf.c
@@ -101,7 +101,7 @@ static void async_pf_execute(struct work_struct *work)
 	if (waitqueue_active(&vcpu->wq))
 		wake_up_interruptible(&vcpu->wq);
 
-	mmdrop(mm);
+	mmput(mm);
 	kvm_put_kvm(vcpu->kvm);
 }
 
@@ -118,7 +118,7 @@ void kvm_clear_async_pf_completion_queue(struct kvm_vcpu *vcpu)
 		flush_work(&work->work);
 #else
 		if (cancel_work_sync(&work->work)) {
-			mmdrop(work->mm);
+			mmput(work->mm);
 			kvm_put_kvm(vcpu->kvm); /* == work->vcpu->kvm */
 			kmem_cache_free(async_pf_cache, work);
 		}
@@ -183,7 +183,7 @@ int kvm_setup_async_pf(struct kvm_vcpu *vcpu, gva_t gva, unsigned long hva,
 	work->addr = hva;
 	work->arch = *arch;
 	work->mm = current->mm;
-	atomic_inc(&work->mm->mm_count);
+	atomic_inc(&work->mm->mm_users);
 	kvm_get_kvm(work->vcpu->kvm);
 
 	/* this can't really happen otherwise gfn_to_pfn_async
@@ -201,7 +201,7 @@ int kvm_setup_async_pf(struct kvm_vcpu *vcpu, gva_t gva, unsigned long hva,
 	return 1;
 retry_sync:
 	kvm_put_kvm(work->vcpu->kvm);
-	mmdrop(work->mm);
+	mmput(work->mm);
 	kmem_cache_free(async_pf_cache, work);
 	return 0;
 }